4137 files changed, 205083 insertions, 150984 deletions

diff --git a/CREDITS b/CREDITS
index afaa7cec6ea..206d0fcf07a 100644
--- a/CREDITS
+++ b/CREDITS
@@ -761,6 +761,10 @@ S: Northampton
 S: NN1 3QT
 S: United Kingdom
 
+N: Massimo Dal Zotto
+E: dz@debian.org
+D: i8k Dell laptop SMM driver
+
 N: Uwe Dannowski
 E: Uwe.Dannowski@ira.uka.de
 W: http://i30www.ira.uka.de/~dannowsk/
diff --git a/Documentation/ABI/testing/sysfs-bus-mei b/Documentation/ABI/testing/sysfs-bus-mei
new file mode 100644
index 00000000000..2066f0bbd45
--- /dev/null
+++ b/Documentation/ABI/testing/sysfs-bus-mei
@@ -0,0 +1,7 @@
+What:		/sys/bus/mei/devices/.../modalias
+Date:		March 2013
+KernelVersion:	3.10
+Contact:	Samuel Ortiz <sameo@linux.intel.com>
+		linux-mei@linux.intel.com
+Description:	Stores the same MODALIAS value emitted by uevent
+		Format: mei:<mei device name>
diff --git a/Documentation/ABI/testing/sysfs-bus-usb b/Documentation/ABI/testing/sysfs-bus-usb
index c8baaf53594..f093e59cbe5 100644
--- a/Documentation/ABI/testing/sysfs-bus-usb
+++ b/Documentation/ABI/testing/sysfs-bus-usb
@@ -32,7 +32,7 @@ Date:		January 2008
 KernelVersion:	2.6.25
 Contact:	Sarah Sharp <sarah.a.sharp@intel.com>
 Description:
-		If CONFIG_PM and CONFIG_USB_SUSPEND are enabled, then this file
+		If CONFIG_PM_RUNTIME is enabled then this file
 		is present.  When read, it returns the total time (in msec)
 		that the USB device has been connected to the machine.  This
 		file is read-only.
@@ -45,7 +45,7 @@ Date:		January 2008
 KernelVersion:	2.6.25
 Contact:	Sarah Sharp <sarah.a.sharp@intel.com>
 Description:
-		If CONFIG_PM and CONFIG_USB_SUSPEND are enabled, then this file
+		If CONFIG_PM_RUNTIME is enabled then this file
 		is present.  When read, it returns the total time (in msec)
 		that the USB device has been active, i.e. not in a suspended
 		state.  This file is read-only.
@@ -187,7 +187,7 @@ What:		/sys/bus/usb/devices/.../power/usb2_hardware_lpm
 Date:		September 2011
 Contact:	Andiry Xu <andiry.xu@amd.com>
 Description:
-		If CONFIG_USB_SUSPEND is set and a USB 2.0 lpm-capable device
+		If CONFIG_PM_RUNTIME is set and a USB 2.0 lpm-capable device
 		is plugged in to a xHCI host which support link PM, it will
 		perform a LPM test; if the test is passed and host supports
 		USB2 hardware LPM (xHCI 1.0 feature), USB2 hardware LPM will
diff --git a/Documentation/ABI/testing/sysfs-devices-system-cpu b/Documentation/ABI/testing/sysfs-devices-system-cpu
index 9c978dcae07..2447698aed4 100644
--- a/Documentation/ABI/testing/sysfs-devices-system-cpu
+++ b/Documentation/ABI/testing/sysfs-devices-system-cpu
@@ -173,3 +173,15 @@ Description:	Processor frequency boosting control
 		Boosting allows the CPU and the firmware to run at a frequency
 		beyound it's nominal limit.
 		More details can be found in Documentation/cpu-freq/boost.txt
+
+
+What:		/sys/devices/system/cpu/cpu#/crash_notes
+		/sys/devices/system/cpu/cpu#/crash_notes_size
+Date:		April 2013
+Contact:	kexec@lists.infradead.org
+Description:	address and size of the percpu note.
+
+		crash_notes: the physical address of the memory that holds the
+		note of cpu#.
+
+		crash_notes_size: size of the note of cpu#.
diff --git a/Documentation/ABI/testing/sysfs-driver-hid-roccat-isku b/Documentation/ABI/testing/sysfs-driver-hid-roccat-isku
index 9eca5a182e6..c601d0f2ac4 100644
--- a/Documentation/ABI/testing/sysfs-driver-hid-roccat-isku
+++ b/Documentation/ABI/testing/sysfs-driver-hid-roccat-isku
@@ -101,7 +101,8 @@ Date:		June 2011
 Contact:	Stefan Achatz <erazor_de@users.sourceforge.net>
 Description:	When written, this file lets one set the backlight intensity for
 		a specific profile. Profile number is included in written data.
-		The data has to be 10 bytes long.
+		The data has to be 10 bytes long for Isku, IskuFX needs	16 bytes
+		of data.
 		Before reading this file, control has to be written to select
 		which profile to read.
 Users:		http://roccat.sourceforge.net
@@ -141,3 +142,12 @@ Description:	When written, this file lets one trigger easyshift functionality
 		The data has to be 16 bytes long.
 		This file is writeonly.
 Users:		http://roccat.sourceforge.net
+
+What:		/sys/bus/usb/devices/<busnum>-<devnum>:<config num>.<interface num>/<hid-bus>:<vendor-id>:<product-id>.<num>/isku/roccatisku<minor>/talkfx
+Date:		February 2013
+Contact:	Stefan Achatz <erazor_de@users.sourceforge.net>
+Description:	When written, this file lets one trigger temporary color schemes
+		from the host.
+		The data has to be 16 bytes long.
+		This file is writeonly.
+Users:		http://roccat.sourceforge.net
diff --git a/Documentation/ABI/testing/sysfs-driver-hid-roccat-konepure b/Documentation/ABI/testing/sysfs-driver-hid-roccat-konepure
new file mode 100644
index 00000000000..41a9b7fbfc7
--- /dev/null
+++ b/Documentation/ABI/testing/sysfs-driver-hid-roccat-konepure
@@ -0,0 +1,105 @@
+What:		/sys/bus/usb/devices/<busnum>-<devnum>:<config num>.<interface num>/<hid-bus>:<vendor-id>:<product-id>.<num>/konepure/roccatkonepure<minor>/actual_profile
+Date:		December 2012
+Contact:	Stefan Achatz <erazor_de@users.sourceforge.net>
+Description:	The mouse can store 5 profiles which can be switched by the
+		press of a button. actual_profile holds number of actual profile.
+		This value is persistent, so its value determines the profile
+		that's active when the mouse is powered on next time.
+		When written, the mouse activates the set profile immediately.
+		The data has to be 3 bytes long.
+		The mouse will reject invalid data.
+Users:		http://roccat.sourceforge.net
+
+What:		/sys/bus/usb/devices/<busnum>-<devnum>:<config num>.<interface num>/<hid-bus>:<vendor-id>:<product-id>.<num>/konepure/roccatkonepure<minor>/control
+Date:		December 2012
+Contact:	Stefan Achatz <erazor_de@users.sourceforge.net>
+Description:	When written, this file lets one select which data from which
+		profile will be	read next. The data has to be 3 bytes long.
+		This file is writeonly.
+Users:		http://roccat.sourceforge.net
+
+What:		/sys/bus/usb/devices/<busnum>-<devnum>:<config num>.<interface num>/<hid-bus>:<vendor-id>:<product-id>.<num>/konepure/roccatkonepure<minor>/info
+Date:		December 2012
+Contact:	Stefan Achatz <erazor_de@users.sourceforge.net>
+Description:	When read, this file returns general data like firmware version.
+		When written, the device can be reset.
+		The data is 6 bytes long.
+Users:		http://roccat.sourceforge.net
+
+What:		/sys/bus/usb/devices/<busnum>-<devnum>:<config num>.<interface num>/<hid-bus>:<vendor-id>:<product-id>.<num>/konepure/roccatkonepure<minor>/macro
+Date:		December 2012
+Contact:	Stefan Achatz <erazor_de@users.sourceforge.net>
+Description:	The mouse can store a macro with max 500 key/button strokes
+		internally.
+		When written, this file lets one set the sequence for a specific
+		button for a specific profile. Button and profile numbers are
+		included in written data. The data has to be 2082 bytes long.
+		This file is writeonly.
+Users:		http://roccat.sourceforge.net
+
+What:		/sys/bus/usb/devices/<busnum>-<devnum>:<config num>.<interface num>/<hid-bus>:<vendor-id>:<product-id>.<num>/konepure/roccatkonepure<minor>/profile_buttons
+Date:		December 2012
+Contact:	Stefan Achatz <erazor_de@users.sourceforge.net>
+Description:	The mouse can store 5 profiles which can be switched by the
+		press of a button. A profile is split in settings and buttons.
+		profile_buttons holds information about button layout.
+		When written, this file lets one write the respective profile
+		buttons back to the mouse. The data has to be 59 bytes long.
+		The mouse will reject invalid data.
+		Which profile to write is determined by the profile number
+		contained in the data.
+		Before reading this file, control has to be written to select
+		which profile to read.
+Users:		http://roccat.sourceforge.net
+
+What:		/sys/bus/usb/devices/<busnum>-<devnum>:<config num>.<interface num>/<hid-bus>:<vendor-id>:<product-id>.<num>/konepure/roccatkonepure<minor>/profile_settings
+Date:		December 2012
+Contact:	Stefan Achatz <erazor_de@users.sourceforge.net>
+Description:	The mouse can store 5 profiles which can be switched by the
+		press of a button. A profile is split in settings and buttons.
+		profile_settings holds information like resolution, sensitivity
+		and light effects.
+		When written, this file lets one write the respective profile
+		settings back to the mouse. The data has to be 31 bytes long.
+		The mouse will reject invalid data.
+		Which profile to write is determined by the profile number
+		contained in the data.
+		Before reading this file, control has to be written to select
+		which profile to read.
+Users:		http://roccat.sourceforge.net
+
+What:		/sys/bus/usb/devices/<busnum>-<devnum>:<config num>.<interface num>/<hid-bus>:<vendor-id>:<product-id>.<num>/konepure/roccatkonepure<minor>/sensor
+Date:		December 2012
+Contact:	Stefan Achatz <erazor_de@users.sourceforge.net>
+Description:	The mouse has a tracking- and a distance-control-unit. These
+		can be activated/deactivated and the lift-off distance can be
+		set. The data has to be 6 bytes long.
+		This file is writeonly.
+Users:		http://roccat.sourceforge.net
+
+What:		/sys/bus/usb/devices/<busnum>-<devnum>:<config num>.<interface num>/<hid-bus>:<vendor-id>:<product-id>.<num>/konepure/roccatkonepure<minor>/talk
+Date:		December 2012
+Contact:	Stefan Achatz <erazor_de@users.sourceforge.net>
+Description:	Used to active some easy* functions of the mouse from outside.
+		The data has to be 16 bytes long.
+		This file is writeonly.
+Users:		http://roccat.sourceforge.net
+
+What:		/sys/bus/usb/devices/<busnum>-<devnum>:<config num>.<interface num>/<hid-bus>:<vendor-id>:<product-id>.<num>/konepure/roccatkonepure<minor>/tcu
+Date:		December 2012
+Contact:	Stefan Achatz <erazor_de@users.sourceforge.net>
+Description:	When written a calibration process for the tracking control unit
+		can be initiated/cancelled. Also lets one read/write sensor
+		registers.
+		The data has to be 4 bytes long.
+Users:		http://roccat.sourceforge.net
+
+What:		/sys/bus/usb/devices/<busnum>-<devnum>:<config num>.<interface num>/<hid-bus>:<vendor-id>:<product-id>.<num>/konepure/roccatkonepure<minor>/tcu_image
+Date:		December 2012
+Contact:	Stefan Achatz <erazor_de@users.sourceforge.net>
+Description:	When read the mouse returns a 30x30 pixel image of the
+		sampled underground. This works only in the course of a
+		calibration process initiated with tcu.
+		The returned data is 1028 bytes in size.
+		This file is readonly.
+Users:		http://roccat.sourceforge.net
diff --git a/Documentation/DocBook/device-drivers.tmpl b/Documentation/DocBook/device-drivers.tmpl
index 7514dbf0a67..c36892c072d 100644
--- a/Documentation/DocBook/device-drivers.tmpl
+++ b/Documentation/DocBook/device-drivers.tmpl
@@ -227,7 +227,7 @@ X!Isound/sound_firmware.c
   <chapter id="uart16x50">
      <title>16x50 UART Driver</title>
 !Edrivers/tty/serial/serial_core.c
-!Edrivers/tty/serial/8250/8250.c
+!Edrivers/tty/serial/8250/8250_core.c
   </chapter>
 
   <chapter id="fbdev">
diff --git a/Documentation/DocBook/media/dvb/dvbproperty.xml b/Documentation/DocBook/media/dvb/dvbproperty.xml
index 4a5eaeed0b9..a9b15e34c5b 100644
--- a/Documentation/DocBook/media/dvb/dvbproperty.xml
+++ b/Documentation/DocBook/media/dvb/dvbproperty.xml
@@ -1,6 +1,6 @@
 <section id="FE_GET_SET_PROPERTY">
 <title><constant>FE_GET_PROPERTY/FE_SET_PROPERTY</constant></title>
-<para>This section describes the DVB version 5 extention of the DVB-API, also
+<para>This section describes the DVB version 5 extension of the DVB-API, also
 called "S2API", as this API were added to provide support for DVB-S2. It was
 designed to be able to replace the old frontend API. Yet, the DISEQC and
 the capability ioctls weren't implemented yet via the new way.</para>
@@ -903,14 +903,12 @@ enum fe_interleaving {
 			<constant>svalue</constant> is for signed values of the measure (dB measures)
 			and <constant>uvalue</constant> is for unsigned values (counters, relative scale)</para></listitem>
 		<listitem><para><constant>scale</constant> - Scale for the value. It can be:</para>
-			<section id = "fecap-scale-params">
-			<itemizedlist mark='bullet'>
+			<itemizedlist mark='bullet' id="fecap-scale-params">
 				<listitem><para><constant>FE_SCALE_NOT_AVAILABLE</constant> - The parameter is supported by the frontend, but it was not possible to collect it (could be a transitory or permanent condition)</para></listitem>
 				<listitem><para><constant>FE_SCALE_DECIBEL</constant> - parameter is a signed value, measured in 1/1000 dB</para></listitem>
 				<listitem><para><constant>FE_SCALE_RELATIVE</constant> - parameter is a unsigned value, where 0 means 0% and 65535 means 100%.</para></listitem>
 				<listitem><para><constant>FE_SCALE_COUNTER</constant> - parameter is a unsigned value that counts the occurrence of an event, like bit error, block error, or lapsed time.</para></listitem>
 			</itemizedlist>
-			</section>
 		</listitem>
 	</itemizedlist>
 	<section id="DTV-STAT-SIGNAL-STRENGTH">
@@ -918,9 +916,9 @@ enum fe_interleaving {
 		<para>Indicates the signal strength level at the analog part of the tuner or of the demod.</para>
 		<para>Possible scales for this metric are:</para>
 		<itemizedlist mark='bullet'>
-			<listitem><constant>FE_SCALE_NOT_AVAILABLE</constant> - it failed to measure it, or the measurement was not complete yet.</listitem>
-			<listitem><constant>FE_SCALE_DECIBEL</constant> - signal strength is in 0.0001 dBm units, power measured in miliwatts. This value is generally negative.</listitem>
-			<listitem><constant>FE_SCALE_RELATIVE</constant> - The frontend provides a 0% to 100% measurement for power (actually, 0 to 65535).</listitem>
+			<listitem><para><constant>FE_SCALE_NOT_AVAILABLE</constant> - it failed to measure it, or the measurement was not complete yet.</para></listitem>
+			<listitem><para><constant>FE_SCALE_DECIBEL</constant> - signal strength is in 0.0001 dBm units, power measured in miliwatts. This value is generally negative.</para></listitem>
+			<listitem><para><constant>FE_SCALE_RELATIVE</constant> - The frontend provides a 0% to 100% measurement for power (actually, 0 to 65535).</para></listitem>
 		</itemizedlist>
 	</section>
 	<section id="DTV-STAT-CNR">
@@ -928,9 +926,9 @@ enum fe_interleaving {
 		<para>Indicates the Signal to Noise ratio for the main carrier.</para>
 		<para>Possible scales for this metric are:</para>
 		<itemizedlist mark='bullet'>
-			<listitem><constant>FE_SCALE_NOT_AVAILABLE</constant> - it failed to measure it, or the measurement was not complete yet.</listitem>
-			<listitem><constant>FE_SCALE_DECIBEL</constant> - Signal/Noise ratio is in 0.0001 dB units.</listitem>
-			<listitem><constant>FE_SCALE_RELATIVE</constant> - The frontend provides a 0% to 100% measurement for Signal/Noise (actually, 0 to 65535).</listitem>
+			<listitem><para><constant>FE_SCALE_NOT_AVAILABLE</constant> - it failed to measure it, or the measurement was not complete yet.</para></listitem>
+			<listitem><para><constant>FE_SCALE_DECIBEL</constant> - Signal/Noise ratio is in 0.0001 dB units.</para></listitem>
+			<listitem><para><constant>FE_SCALE_RELATIVE</constant> - The frontend provides a 0% to 100% measurement for Signal/Noise (actually, 0 to 65535).</para></listitem>
 		</itemizedlist>
 	</section>
 	<section id="DTV-STAT-PRE-ERROR-BIT-COUNT">
@@ -943,8 +941,8 @@ enum fe_interleaving {
 		      The frontend may reset it when a channel/transponder is tuned.</para>
 		<para>Possible scales for this metric are:</para>
 		<itemizedlist mark='bullet'>
-			<listitem><constant>FE_SCALE_NOT_AVAILABLE</constant> - it failed to measure it, or the measurement was not complete yet.</listitem>
-			<listitem><constant>FE_SCALE_COUNTER</constant> - Number of error bits counted before the inner coding.</listitem>
+			<listitem><para><constant>FE_SCALE_NOT_AVAILABLE</constant> - it failed to measure it, or the measurement was not complete yet.</para></listitem>
+			<listitem><para><constant>FE_SCALE_COUNTER</constant> - Number of error bits counted before the inner coding.</para></listitem>
 		</itemizedlist>
 	</section>
 	<section id="DTV-STAT-PRE-TOTAL-BIT-COUNT">
@@ -952,14 +950,14 @@ enum fe_interleaving {
 		<para>Measures the amount of bits received before the inner code block, during the same period as
 		<link linkend="DTV-STAT-PRE-ERROR-BIT-COUNT"><constant>DTV_STAT_PRE_ERROR_BIT_COUNT</constant></link> measurement was taken.</para>
 		<para>It should be noticed that this measurement can be smaller than the total amount of bits on the transport stream,
-		      as the frontend may need to manually restart the measurement, loosing some data between each measurement interval.</para>
+		      as the frontend may need to manually restart the measurement, losing some data between each measurement interval.</para>
 		<para>This measurement is monotonically increased, as the frontend gets more bit count measurements.
 		      The frontend may reset it when a channel/transponder is tuned.</para>
 		<para>Possible scales for this metric are:</para>
 		<itemizedlist mark='bullet'>
-			<listitem><constant>FE_SCALE_NOT_AVAILABLE</constant> - it failed to measure it, or the measurement was not complete yet.</listitem>
-			<listitem><constant>FE_SCALE_COUNTER</constant> - Number of bits counted while measuring
-				 <link linkend="DTV-STAT-PRE-ERROR-BIT-COUNT"><constant>DTV_STAT_PRE_ERROR_BIT_COUNT</constant></link>.</listitem>
+			<listitem><para><constant>FE_SCALE_NOT_AVAILABLE</constant> - it failed to measure it, or the measurement was not complete yet.</para></listitem>
+			<listitem><para><constant>FE_SCALE_COUNTER</constant> - Number of bits counted while measuring
+				 <link linkend="DTV-STAT-PRE-ERROR-BIT-COUNT"><constant>DTV_STAT_PRE_ERROR_BIT_COUNT</constant></link>.</para></listitem>
 		</itemizedlist>
 	</section>
 	<section id="DTV-STAT-POST-ERROR-BIT-COUNT">
@@ -972,8 +970,8 @@ enum fe_interleaving {
 		      The frontend may reset it when a channel/transponder is tuned.</para>
 		<para>Possible scales for this metric are:</para>
 		<itemizedlist mark='bullet'>
-			<listitem><constant>FE_SCALE_NOT_AVAILABLE</constant> - it failed to measure it, or the measurement was not complete yet.</listitem>
-			<listitem><constant>FE_SCALE_COUNTER</constant> - Number of error bits counted after the inner coding.</listitem>
+			<listitem><para><constant>FE_SCALE_NOT_AVAILABLE</constant> - it failed to measure it, or the measurement was not complete yet.</para></listitem>
+			<listitem><para><constant>FE_SCALE_COUNTER</constant> - Number of error bits counted after the inner coding.</para></listitem>
 		</itemizedlist>
 	</section>
 	<section id="DTV-STAT-POST-TOTAL-BIT-COUNT">
@@ -981,14 +979,14 @@ enum fe_interleaving {
 		<para>Measures the amount of bits received after the inner coding, during the same period as
 		<link linkend="DTV-STAT-POST-ERROR-BIT-COUNT"><constant>DTV_STAT_POST_ERROR_BIT_COUNT</constant></link> measurement was taken.</para>
 		<para>It should be noticed that this measurement can be smaller than the total amount of bits on the transport stream,
-		      as the frontend may need to manually restart the measurement, loosing some data between each measurement interval.</para>
+		      as the frontend may need to manually restart the measurement, losing some data between each measurement interval.</para>
 		<para>This measurement is monotonically increased, as the frontend gets more bit count measurements.
 		      The frontend may reset it when a channel/transponder is tuned.</para>
 		<para>Possible scales for this metric are:</para>
 		<itemizedlist mark='bullet'>
-			<listitem><constant>FE_SCALE_NOT_AVAILABLE</constant> - it failed to measure it, or the measurement was not complete yet.</listitem>
-			<listitem><constant>FE_SCALE_COUNTER</constant> - Number of bits counted while measuring
-				 <link linkend="DTV-STAT-POST-ERROR-BIT-COUNT"><constant>DTV_STAT_POST_ERROR_BIT_COUNT</constant></link>.</listitem>
+			<listitem><para><constant>FE_SCALE_NOT_AVAILABLE</constant> - it failed to measure it, or the measurement was not complete yet.</para></listitem>
+			<listitem><para><constant>FE_SCALE_COUNTER</constant> - Number of bits counted while measuring
+				 <link linkend="DTV-STAT-POST-ERROR-BIT-COUNT"><constant>DTV_STAT_POST_ERROR_BIT_COUNT</constant></link>.</para></listitem>
 		</itemizedlist>
 	</section>
 	<section id="DTV-STAT-ERROR-BLOCK-COUNT">
@@ -998,8 +996,8 @@ enum fe_interleaving {
 		      The frontend may reset it when a channel/transponder is tuned.</para>
 		<para>Possible scales for this metric are:</para>
 		<itemizedlist mark='bullet'>
-			<listitem><constant>FE_SCALE_NOT_AVAILABLE</constant> - it failed to measure it, or the measurement was not complete yet.</listitem>
-			<listitem><constant>FE_SCALE_COUNTER</constant> - Number of error blocks counted after the outer coding.</listitem>
+			<listitem><para><constant>FE_SCALE_NOT_AVAILABLE</constant> - it failed to measure it, or the measurement was not complete yet.</para></listitem>
+			<listitem><para><constant>FE_SCALE_COUNTER</constant> - Number of error blocks counted after the outer coding.</para></listitem>
 		</itemizedlist>
 	</section>
 	<section id="DTV-STAT-TOTAL-BLOCK-COUNT">
@@ -1011,9 +1009,9 @@ enum fe_interleaving {
 		by <link linkend="DTV-STAT-TOTAL-BLOCK-COUNT"><constant>DTV-STAT-TOTAL-BLOCK-COUNT</constant></link>.</para>
 		<para>Possible scales for this metric are:</para>
 		<itemizedlist mark='bullet'>
-			<listitem><constant>FE_SCALE_NOT_AVAILABLE</constant> - it failed to measure it, or the measurement was not complete yet.</listitem>
-			<listitem><constant>FE_SCALE_COUNTER</constant> - Number of blocks counted while measuring
-			<link linkend="DTV-STAT-ERROR-BLOCK-COUNT"><constant>DTV_STAT_ERROR_BLOCK_COUNT</constant></link>.</listitem>
+			<listitem><para><constant>FE_SCALE_NOT_AVAILABLE</constant> - it failed to measure it, or the measurement was not complete yet.</para></listitem>
+			<listitem><para><constant>FE_SCALE_COUNTER</constant> - Number of blocks counted while measuring
+			<link linkend="DTV-STAT-ERROR-BLOCK-COUNT"><constant>DTV_STAT_ERROR_BLOCK_COUNT</constant></link>.</para></listitem>
 		</itemizedlist>
 	</section>
 	</section>
diff --git a/Documentation/DocBook/media/v4l/common.xml b/Documentation/DocBook/media/v4l/common.xml
index ae06afbbb3a..1ddf354aa99 100644
--- a/Documentation/DocBook/media/v4l/common.xml
+++ b/Documentation/DocBook/media/v4l/common.xml
@@ -750,15 +750,6 @@ header can be used to get the timings of the formats in the <xref linkend="cea86
 <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. 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.
@@ -766,11 +757,6 @@ API.</para>
 &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. 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>
diff --git a/Documentation/DocBook/media/v4l/compat.xml b/Documentation/DocBook/media/v4l/compat.xml
index 104a1a2b884..f43542ae298 100644
--- a/Documentation/DocBook/media/v4l/compat.xml
+++ b/Documentation/DocBook/media/v4l/compat.xml
@@ -2310,6 +2310,9 @@ more information.</para>
 	<listitem>
 	  <para>Added FM Modulator (FM TX) Extended Control Class: <constant>V4L2_CTRL_CLASS_FM_TX</constant> and their Control IDs.</para>
 	</listitem>
+<listitem>
+	  <para>Added FM Receiver (FM RX) Extended Control Class: <constant>V4L2_CTRL_CLASS_FM_RX</constant> and their Control IDs.</para>
+	</listitem>
 	<listitem>
 	  <para>Added Remote Controller chapter, describing the default Remote Controller mapping for media devices.</para>
 	</listitem>
@@ -2493,6 +2496,23 @@ that used it. It was originally scheduled for removal in 2.6.35.
       </orderedlist>
     </section>
 
+    <section>
+      <title>V4L2 in Linux 3.10</title>
+      <orderedlist>
+        <listitem>
+	  <para>Removed obsolete and unused DV_PRESET ioctls
+	  VIDIOC_G_DV_PRESET, VIDIOC_S_DV_PRESET, VIDIOC_QUERY_DV_PRESET and
+	  VIDIOC_ENUM_DV_PRESET. Remove the related v4l2_input/output capability
+	  flags V4L2_IN_CAP_PRESETS and V4L2_OUT_CAP_PRESETS.
+	  </para>
+        </listitem>
+        <listitem>
+	  <para>Added new debugging ioctl &VIDIOC-DBG-G-CHIP-INFO;.
+	  </para>
+        </listitem>
+      </orderedlist>
+    </section>
+
     <section id="other">
       <title>Relation of V4L2 to other Linux multimedia APIs</title>
 
@@ -2625,8 +2645,8 @@ interfaces and should not be implemented in new drivers.</para>
 <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>
+	  <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
diff --git a/Documentation/DocBook/media/v4l/controls.xml b/Documentation/DocBook/media/v4l/controls.xml
index 9e8f8549867..8d7a77928d4 100644
--- a/Documentation/DocBook/media/v4l/controls.xml
+++ b/Documentation/DocBook/media/v4l/controls.xml
@@ -2300,6 +2300,12 @@ Possible values are:</entry>
 	      </row>
 	      <row><entry></entry></row>
 	      <row>
+		<entry spanname="id"><constant>V4L2_CID_MPEG_VIDEO_REPEAT_SEQ_HEADER</constant>&nbsp;</entry>
+		<entry>boolean</entry>
+	      </row><row><entry spanname="descr">Repeat the video sequence headers. Repeating these
+headers makes random access to the video stream easier. Applicable to the MPEG1, 2 and 4 encoder.</entry>
+	      </row>
+	      <row>
 		<entry spanname="id"><constant>V4L2_CID_MPEG_VIDEO_DECODER_MPEG4_DEBLOCK_FILTER</constant>&nbsp;</entry>
 		<entry>boolean</entry>
 	      </row><row><entry spanname="descr">Enabled the deblocking post processing filter for MPEG4 decoder.
@@ -3136,6 +3142,13 @@ giving priority to the center of the metered area.</entry>
 		  <entry><constant>V4L2_EXPOSURE_METERING_SPOT</constant>&nbsp;</entry>
 		  <entry>Measure only very small area at the center of the frame.</entry>
 		</row>
+		<row>
+		  <entry><constant>V4L2_EXPOSURE_METERING_MATRIX</constant>&nbsp;</entry>
+		  <entry>A multi-zone metering. The light intensity is measured
+in several points of the frame and the the results are combined. The
+algorithm of the zones selection and their significance in calculating the
+final value is device dependant.</entry>
+		</row>
 	      </tbody>
 	    </entrytbl>
 	  </row>
@@ -3848,7 +3861,7 @@ in Hz. The range and step are driver-specific.</entry>
 	  </row>
 	  <row>
 	    <entry spanname="id"><constant>V4L2_CID_TUNE_PREEMPHASIS</constant>&nbsp;</entry>
-	    <entry>integer</entry>
+	    <entry>enum v4l2_preemphasis</entry>
 	  </row>
 	  <row id="v4l2-preemphasis"><entry spanname="descr">Configures the pre-emphasis value for broadcasting.
 A pre-emphasis filter is applied to the broadcast to accentuate the high audio frequencies.
@@ -4687,4 +4700,76 @@ interface and may change in the future.</para>
       </table>
 
     </section>
+
+    <section id="fm-rx-controls">
+      <title>FM Receiver Control Reference</title>
+
+      <para>The FM Receiver (FM_RX) class includes controls for common features of
+      FM Reception capable devices.</para>
+
+      <table pgwide="1" frame="none" id="fm-rx-control-id">
+      <title>FM_RX 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_FM_RX_CLASS</constant>&nbsp;</entry>
+            <entry>class</entry>
+          </row><row><entry spanname="descr">The FM_RX class
+descriptor. Calling &VIDIOC-QUERYCTRL; for this control will return a
+description of this control class.</entry>
+          </row>
+          <row>
+            <entry spanname="id"><constant>V4L2_CID_RDS_RECEPTION</constant>&nbsp;</entry>
+            <entry>boolean</entry>
+          </row><row><entry spanname="descr">Enables/disables RDS
+	  reception by the radio tuner</entry>
+          </row>
+          <row>
+	    <entry spanname="id"><constant>V4L2_CID_TUNE_DEEMPHASIS</constant>&nbsp;</entry>
+	    <entry>enum v4l2_deemphasis</entry>
+	  </row>
+	  <row id="v4l2-deemphasis"><entry spanname="descr">Configures the de-emphasis value for reception.
+A de-emphasis filter is applied to the broadcast to accentuate the high audio frequencies.
+Depending on the region, a time constant of either 50 or 75 useconds is used. The enum&nbsp;v4l2_deemphasis
+defines possible values for de-emphasis. Here they are:</entry>
+	</row><row>
+	<entrytbl spanname="descr" cols="2">
+		  <tbody valign="top">
+		    <row>
+		      <entry><constant>V4L2_DEEMPHASIS_DISABLED</constant>&nbsp;</entry>
+		      <entry>No de-emphasis is applied.</entry>
+		    </row>
+		    <row>
+		      <entry><constant>V4L2_DEEMPHASIS_50_uS</constant>&nbsp;</entry>
+		      <entry>A de-emphasis of 50 uS is used.</entry>
+		    </row>
+		    <row>
+		      <entry><constant>V4L2_DEEMPHASIS_75_uS</constant>&nbsp;</entry>
+		      <entry>A de-emphasis of 75 uS is used.</entry>
+		    </row>
+		  </tbody>
+		</entrytbl>
+
+	  </row>
+          <row><entry></entry></row>
+        </tbody>
+      </tgroup>
+      </table>
+
+      </section>
 </section>
diff --git a/Documentation/DocBook/media/v4l/io.xml b/Documentation/DocBook/media/v4l/io.xml
index e6c58559ca6..2c4c068dde8 100644
--- a/Documentation/DocBook/media/v4l/io.xml
+++ b/Documentation/DocBook/media/v4l/io.xml
@@ -1145,6 +1145,12 @@ in which case caches have not been used.</entry>
 	    same clock outside V4L2, use
 	    <function>clock_gettime(2)</function> .</entry>
 	  </row>
+	  <row>
+	    <entry><constant>V4L2_BUF_FLAG_TIMESTAMP_COPY</constant></entry>
+	    <entry>0x4000</entry>
+	    <entry>The CAPTURE buffer timestamp has been taken from the
+	    corresponding OUTPUT buffer. This flag applies only to mem2mem devices.</entry>
+	  </row>
 	</tbody>
       </tgroup>
     </table>
diff --git a/Documentation/DocBook/media/v4l/media-ioc-enum-entities.xml b/Documentation/DocBook/media/v4l/media-ioc-enum-entities.xml
index 576b68b33f2..116c301656e 100644
--- a/Documentation/DocBook/media/v4l/media-ioc-enum-entities.xml
+++ b/Documentation/DocBook/media/v4l/media-ioc-enum-entities.xml
@@ -272,6 +272,16 @@
 	    <entry><constant>MEDIA_ENT_T_V4L2_SUBDEV_LENS</constant></entry>
 	    <entry>Lens controller</entry>
 	  </row>
+	  <row>
+	    <entry><constant>MEDIA_ENT_T_V4L2_SUBDEV_DECODER</constant></entry>
+	    <entry>Video decoder, the basic function of the video decoder is to
+	    accept analogue video from a wide variety of sources such as
+	    broadcast, DVD players, cameras and video cassette recorders, in
+	    either NTSC, PAL or HD format and still occasionally SECAM, separate
+	    it into its component parts, luminance and chrominance, and output
+	    it in some digital video standard, with appropriate embedded timing
+	    signals.</entry>
+	  </row>
 	</tbody>
       </tgroup>
     </table>
diff --git a/Documentation/DocBook/media/v4l/subdev-formats.xml b/Documentation/DocBook/media/v4l/subdev-formats.xml
index cc51372ed5e..adc61982df7 100644
--- a/Documentation/DocBook/media/v4l/subdev-formats.xml
+++ b/Documentation/DocBook/media/v4l/subdev-formats.xml
@@ -93,19 +93,35 @@
 
       <table pgwide="0" frame="none" id="v4l2-mbus-pixelcode-rgb">
 	<title>RGB formats</title>
-	<tgroup cols="11">
+	<tgroup cols="27">
 	  <colspec colname="id" align="left" />
 	  <colspec colname="code" align="center"/>
 	  <colspec colname="bit" />
-	  <colspec colnum="4" colname="b07" align="center" />
-	  <colspec colnum="5" colname="b06" align="center" />
-	  <colspec colnum="6" colname="b05" align="center" />
-	  <colspec colnum="7" colname="b04" align="center" />
-	  <colspec colnum="8" colname="b03" align="center" />
-	  <colspec colnum="9" colname="b02" align="center" />
-	  <colspec colnum="10" colname="b01" align="center" />
-	  <colspec colnum="11" colname="b00" align="center" />
-	  <spanspec namest="b07" nameend="b00" spanname="b0" />
+	  <colspec colnum="4" colname="b23" align="center" />
+	  <colspec colnum="5" colname="b22" align="center" />
+	  <colspec colnum="6" colname="b21" align="center" />
+	  <colspec colnum="7" colname="b20" align="center" />
+	  <colspec colnum="8" colname="b19" align="center" />
+	  <colspec colnum="9" colname="b18" align="center" />
+	  <colspec colnum="10" colname="b17" align="center" />
+	  <colspec colnum="11" colname="b16" align="center" />
+	  <colspec colnum="12" colname="b15" align="center" />
+	  <colspec colnum="13" colname="b14" align="center" />
+	  <colspec colnum="14" colname="b13" align="center" />
+	  <colspec colnum="15" colname="b12" align="center" />
+	  <colspec colnum="16" colname="b11" align="center" />
+	  <colspec colnum="17" colname="b10" align="center" />
+	  <colspec colnum="18" colname="b09" align="center" />
+	  <colspec colnum="19" colname="b08" align="center" />
+	  <colspec colnum="20" colname="b07" align="center" />
+	  <colspec colnum="21" colname="b06" align="center" />
+	  <colspec colnum="22" colname="b05" align="center" />
+	  <colspec colnum="23" colname="b04" align="center" />
+	  <colspec colnum="24" colname="b03" align="center" />
+	  <colspec colnum="25" colname="b02" align="center" />
+	  <colspec colnum="26" colname="b01" align="center" />
+	  <colspec colnum="27" colname="b00" align="center" />
+	  <spanspec namest="b23" nameend="b00" spanname="b0" />
 	  <thead>
 	    <row>
 	      <entry>Identifier</entry>
@@ -117,6 +133,22 @@
 	      <entry></entry>
 	      <entry></entry>
 	      <entry>Bit</entry>
+	      <entry>23</entry>
+	      <entry>22</entry>
+	      <entry>21</entry>
+	      <entry>20</entry>
+	      <entry>19</entry>
+	      <entry>18</entry>
+	      <entry>17</entry>
+	      <entry>16</entry>
+	      <entry>15</entry>
+	      <entry>14</entry>
+	      <entry>13</entry>
+	      <entry>12</entry>
+	      <entry>11</entry>
+	      <entry>10</entry>
+	      <entry>9</entry>
+	      <entry>8</entry>
 	      <entry>7</entry>
 	      <entry>6</entry>
 	      <entry>5</entry>
@@ -132,6 +164,7 @@
 	      <entry>V4L2_MBUS_FMT_RGB444_2X8_PADHI_BE</entry>
 	      <entry>0x1001</entry>
 	      <entry></entry>
+	      &dash-ent-16;
 	      <entry>0</entry>
 	      <entry>0</entry>
 	      <entry>0</entry>
@@ -145,6 +178,7 @@
 	      <entry></entry>
 	      <entry></entry>
 	      <entry></entry>
+	      &dash-ent-16;
 	      <entry>g<subscript>3</subscript></entry>
 	      <entry>g<subscript>2</subscript></entry>
 	      <entry>g<subscript>1</subscript></entry>
@@ -158,6 +192,7 @@
 	      <entry>V4L2_MBUS_FMT_RGB444_2X8_PADHI_LE</entry>
 	      <entry>0x1002</entry>
 	      <entry></entry>
+	      &dash-ent-16;
 	      <entry>g<subscript>3</subscript></entry>
 	      <entry>g<subscript>2</subscript></entry>
 	      <entry>g<subscript>1</subscript></entry>
@@ -171,6 +206,7 @@
 	      <entry></entry>
 	      <entry></entry>
 	      <entry></entry>
+	      &dash-ent-16;
 	      <entry>0</entry>
 	      <entry>0</entry>
 	      <entry>0</entry>
@@ -184,6 +220,7 @@
 	      <entry>V4L2_MBUS_FMT_RGB555_2X8_PADHI_BE</entry>
 	      <entry>0x1003</entry>
 	      <entry></entry>
+	      &dash-ent-16;
 	      <entry>0</entry>
 	      <entry>r<subscript>4</subscript></entry>
 	      <entry>r<subscript>3</subscript></entry>
@@ -197,6 +234,7 @@
 	      <entry></entry>
 	      <entry></entry>
 	      <entry></entry>
+	      &dash-ent-16;
 	      <entry>g<subscript>2</subscript></entry>
 	      <entry>g<subscript>1</subscript></entry>
 	      <entry>g<subscript>0</subscript></entry>
@@ -210,6 +248,7 @@
 	      <entry>V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE</entry>
 	      <entry>0x1004</entry>
 	      <entry></entry>
+	      &dash-ent-16;
 	      <entry>g<subscript>2</subscript></entry>
 	      <entry>g<subscript>1</subscript></entry>
 	      <entry>g<subscript>0</subscript></entry>
@@ -223,6 +262,7 @@
 	      <entry></entry>
 	      <entry></entry>
 	      <entry></entry>
+	      &dash-ent-16;
 	      <entry>0</entry>
 	      <entry>r<subscript>4</subscript></entry>
 	      <entry>r<subscript>3</subscript></entry>
@@ -236,6 +276,7 @@
 	      <entry>V4L2_MBUS_FMT_BGR565_2X8_BE</entry>
 	      <entry>0x1005</entry>
 	      <entry></entry>
+	      &dash-ent-16;
 	      <entry>b<subscript>4</subscript></entry>
 	      <entry>b<subscript>3</subscript></entry>
 	      <entry>b<subscript>2</subscript></entry>
@@ -249,6 +290,7 @@
 	      <entry></entry>
 	      <entry></entry>
 	      <entry></entry>
+	      &dash-ent-16;
 	      <entry>g<subscript>2</subscript></entry>
 	      <entry>g<subscript>1</subscript></entry>
 	      <entry>g<subscript>0</subscript></entry>
@@ -262,6 +304,7 @@
 	      <entry>V4L2_MBUS_FMT_BGR565_2X8_LE</entry>
 	      <entry>0x1006</entry>
 	      <entry></entry>
+	      &dash-ent-16;
 	      <entry>g<subscript>2</subscript></entry>
 	      <entry>g<subscript>1</subscript></entry>
 	      <entry>g<subscript>0</subscript></entry>
@@ -275,6 +318,7 @@
 	      <entry></entry>
 	      <entry></entry>
 	      <entry></entry>
+	      &dash-ent-16;
 	      <entry>b<subscript>4</subscript></entry>
 	      <entry>b<subscript>3</subscript></entry>
 	      <entry>b<subscript>2</subscript></entry>
@@ -288,6 +332,7 @@
 	      <entry>V4L2_MBUS_FMT_RGB565_2X8_BE</entry>
 	      <entry>0x1007</entry>
 	      <entry></entry>
+	      &dash-ent-16;
 	      <entry>r<subscript>4</subscript></entry>
 	      <entry>r<subscript>3</subscript></entry>
 	      <entry>r<subscript>2</subscript></entry>
@@ -301,6 +346,7 @@
 	      <entry></entry>
 	      <entry></entry>
 	      <entry></entry>
+	      &dash-ent-16;
 	      <entry>g<subscript>2</subscript></entry>
 	      <entry>g<subscript>1</subscript></entry>
 	      <entry>g<subscript>0</subscript></entry>
@@ -314,6 +360,7 @@
 	      <entry>V4L2_MBUS_FMT_RGB565_2X8_LE</entry>
 	      <entry>0x1008</entry>
 	      <entry></entry>
+	      &dash-ent-16;
 	      <entry>g<subscript>2</subscript></entry>
 	      <entry>g<subscript>1</subscript></entry>
 	      <entry>g<subscript>0</subscript></entry>
@@ -327,6 +374,27 @@
 	      <entry></entry>
 	      <entry></entry>
 	      <entry></entry>
+	      &dash-ent-16;
+	      <entry>r<subscript>4</subscript></entry>
+	      <entry>r<subscript>3</subscript></entry>
+	      <entry>r<subscript>2</subscript></entry>
+	      <entry>r<subscript>1</subscript></entry>
+	      <entry>r<subscript>0</subscript></entry>
+	      <entry>g<subscript>5</subscript></entry>
+	      <entry>g<subscript>4</subscript></entry>
+	      <entry>g<subscript>3</subscript></entry>
+	    </row>
+	    <row id="V4L2-MBUS-FMT-RGB666-1X18">
+	      <entry>V4L2_MBUS_FMT_RGB666_1X18</entry>
+	      <entry>0x1009</entry>
+	      <entry></entry>
+	      <entry>-</entry>
+	      <entry>-</entry>
+	      <entry>-</entry>
+	      <entry>-</entry>
+	      <entry>-</entry>
+	      <entry>-</entry>
+	      <entry>r<subscript>5</subscript></entry>
 	      <entry>r<subscript>4</subscript></entry>
 	      <entry>r<subscript>3</subscript></entry>
 	      <entry>r<subscript>2</subscript></entry>
@@ -335,6 +403,124 @@
 	      <entry>g<subscript>5</subscript></entry>
 	      <entry>g<subscript>4</subscript></entry>
 	      <entry>g<subscript>3</subscript></entry>
+	      <entry>g<subscript>2</subscript></entry>
+	      <entry>g<subscript>1</subscript></entry>
+	      <entry>g<subscript>0</subscript></entry>
+	      <entry>b<subscript>5</subscript></entry>
+	      <entry>b<subscript>4</subscript></entry>
+	      <entry>b<subscript>3</subscript></entry>
+	      <entry>b<subscript>2</subscript></entry>
+	      <entry>b<subscript>1</subscript></entry>
+	      <entry>b<subscript>0</subscript></entry>
+	    </row>
+	    <row id="V4L2-MBUS-FMT-RGB888-1X24">
+	      <entry>V4L2_MBUS_FMT_RGB888_1X24</entry>
+	      <entry>0x100a</entry>
+	      <entry></entry>
+	      <entry>r<subscript>7</subscript></entry>
+	      <entry>r<subscript>6</subscript></entry>
+	      <entry>r<subscript>5</subscript></entry>
+	      <entry>r<subscript>4</subscript></entry>
+	      <entry>r<subscript>3</subscript></entry>
+	      <entry>r<subscript>2</subscript></entry>
+	      <entry>r<subscript>1</subscript></entry>
+	      <entry>r<subscript>0</subscript></entry>
+	      <entry>g<subscript>7</subscript></entry>
+	      <entry>g<subscript>6</subscript></entry>
+	      <entry>g<subscript>5</subscript></entry>
+	      <entry>g<subscript>4</subscript></entry>
+	      <entry>g<subscript>3</subscript></entry>
+	      <entry>g<subscript>2</subscript></entry>
+	      <entry>g<subscript>1</subscript></entry>
+	      <entry>g<subscript>0</subscript></entry>
+	      <entry>b<subscript>7</subscript></entry>
+	      <entry>b<subscript>6</subscript></entry>
+	      <entry>b<subscript>5</subscript></entry>
+	      <entry>b<subscript>4</subscript></entry>
+	      <entry>b<subscript>3</subscript></entry>
+	      <entry>b<subscript>2</subscript></entry>
+	      <entry>b<subscript>1</subscript></entry>
+	      <entry>b<subscript>0</subscript></entry>
+	    </row>
+	    <row id="V4L2-MBUS-FMT-RGB888-2X12-BE">
+	      <entry>V4L2_MBUS_FMT_RGB888_2X12_BE</entry>
+	      <entry>0x100b</entry>
+	      <entry></entry>
+	      &dash-ent-10;
+	      <entry>-</entry>
+	      <entry>-</entry>
+	      <entry>r<subscript>7</subscript></entry>
+	      <entry>r<subscript>6</subscript></entry>
+	      <entry>r<subscript>5</subscript></entry>
+	      <entry>r<subscript>4</subscript></entry>
+	      <entry>r<subscript>3</subscript></entry>
+	      <entry>r<subscript>2</subscript></entry>
+	      <entry>r<subscript>1</subscript></entry>
+	      <entry>r<subscript>0</subscript></entry>
+	      <entry>g<subscript>7</subscript></entry>
+	      <entry>g<subscript>6</subscript></entry>
+	      <entry>g<subscript>5</subscript></entry>
+	      <entry>g<subscript>4</subscript></entry>
+	    </row>
+	    <row>
+	      <entry></entry>
+	      <entry></entry>
+	      <entry></entry>
+	      &dash-ent-10;
+	      <entry>-</entry>
+	      <entry>-</entry>
+	      <entry>g<subscript>3</subscript></entry>
+	      <entry>g<subscript>2</subscript></entry>
+	      <entry>g<subscript>1</subscript></entry>
+	      <entry>g<subscript>0</subscript></entry>
+	      <entry>b<subscript>7</subscript></entry>
+	      <entry>b<subscript>6</subscript></entry>
+	      <entry>b<subscript>5</subscript></entry>
+	      <entry>b<subscript>4</subscript></entry>
+	      <entry>b<subscript>3</subscript></entry>
+	      <entry>b<subscript>2</subscript></entry>
+	      <entry>b<subscript>1</subscript></entry>
+	      <entry>b<subscript>0</subscript></entry>
+	    </row>
+	    <row id="V4L2-MBUS-FMT-RGB888-2X12-LE">
+	      <entry>V4L2_MBUS_FMT_RGB888_2X12_LE</entry>
+	      <entry>0x100c</entry>
+	      <entry></entry>
+	      &dash-ent-10;
+	      <entry>-</entry>
+	      <entry>-</entry>
+	      <entry>g<subscript>3</subscript></entry>
+	      <entry>g<subscript>2</subscript></entry>
+	      <entry>g<subscript>1</subscript></entry>
+	      <entry>g<subscript>0</subscript></entry>
+	      <entry>b<subscript>7</subscript></entry>
+	      <entry>b<subscript>6</subscript></entry>
+	      <entry>b<subscript>5</subscript></entry>
+	      <entry>b<subscript>4</subscript></entry>
+	      <entry>b<subscript>3</subscript></entry>
+	      <entry>b<subscript>2</subscript></entry>
+	      <entry>b<subscript>1</subscript></entry>
+	      <entry>b<subscript>0</subscript></entry>
+	    </row>
+	    <row>
+	      <entry></entry>
+	      <entry></entry>
+	      <entry></entry>
+	      &dash-ent-10;
+	      <entry>-</entry>
+	      <entry>-</entry>
+	      <entry>r<subscript>7</subscript></entry>
+	      <entry>r<subscript>6</subscript></entry>
+	      <entry>r<subscript>5</subscript></entry>
+	      <entry>r<subscript>4</subscript></entry>
+	      <entry>r<subscript>3</subscript></entry>
+	      <entry>r<subscript>2</subscript></entry>
+	      <entry>r<subscript>1</subscript></entry>
+	      <entry>r<subscript>0</subscript></entry>
+	      <entry>g<subscript>7</subscript></entry>
+	      <entry>g<subscript>6</subscript></entry>
+	      <entry>g<subscript>5</subscript></entry>
+	      <entry>g<subscript>4</subscript></entry>
 	    </row>
 	  </tbody>
 	</tgroup>
diff --git a/Documentation/DocBook/media/v4l/v4l2.xml b/Documentation/DocBook/media/v4l/v4l2.xml
index a3cce18384e..bfc93cdcf69 100644
--- a/Documentation/DocBook/media/v4l/v4l2.xml
+++ b/Documentation/DocBook/media/v4l/v4l2.xml
@@ -124,6 +124,7 @@ Remote Controller chapter.</contrib>
       <year>2010</year>
       <year>2011</year>
       <year>2012</year>
+      <year>2013</year>
       <holder>Bill Dirks, Michael H. Schimek, Hans Verkuil, Martin
 Rubli, Andy Walls, Muralidharan Karicheri, Mauro Carvalho Chehab,
 	Pawel Osciak</holder>
@@ -140,12 +141,22 @@ structs, ioctls) must be noted in more detail in the history chapter
 applications. -->
 
       <revision>
+	<revnumber>3.10</revnumber>
+	<date>2013-03-25</date>
+	<authorinitials>hv</authorinitials>
+	<revremark>Remove obsolete and unused DV_PRESET ioctls:
+	VIDIOC_G_DV_PRESET, VIDIOC_S_DV_PRESET, VIDIOC_QUERY_DV_PRESET and
+	VIDIOC_ENUM_DV_PRESET. Remove the related v4l2_input/output capability
+	flags V4L2_IN_CAP_PRESETS and V4L2_OUT_CAP_PRESETS. Added VIDIOC_DBG_G_CHIP_INFO.
+	</revremark>
+      </revision>
+
+      <revision>
 	<revnumber>3.9</revnumber>
 	<date>2012-12-03</date>
 	<authorinitials>sa, sn</authorinitials>
 	<revremark>Added timestamp types to v4l2_buffer.
-	Added <constant>V4L2_EVENT_CTRL_CH_RANGE</constant> control
-	event changes flag, see <xref linkend="changes-flags"/>.
+	Added V4L2_EVENT_CTRL_CH_RANGE control event changes flag.
 	</revremark>
       </revision>
 
@@ -537,6 +548,7 @@ and discussions on the V4L mailing list.</revremark>
     &sub-create-bufs;
     &sub-cropcap;
     &sub-dbg-g-chip-ident;
+    &sub-dbg-g-chip-info;
     &sub-dbg-g-register;
     &sub-decoder-cmd;
     &sub-dqevent;
@@ -544,7 +556,6 @@ and discussions on the V4L mailing list.</revremark>
     &sub-encoder-cmd;
     &sub-enumaudio;
     &sub-enumaudioout;
-    &sub-enum-dv-presets;
     &sub-enum-dv-timings;
     &sub-enum-fmt;
     &sub-enum-framesizes;
@@ -558,7 +569,6 @@ and discussions on the V4L mailing list.</revremark>
     &sub-g-audioout;
     &sub-g-crop;
     &sub-g-ctrl;
-    &sub-g-dv-preset;
     &sub-g-dv-timings;
     &sub-g-enc-index;
     &sub-g-ext-ctrls;
@@ -582,7 +592,6 @@ and discussions on the V4L mailing list.</revremark>
     &sub-querybuf;
     &sub-querycap;
     &sub-queryctrl;
-    &sub-query-dv-preset;
     &sub-query-dv-timings;
     &sub-querystd;
     &sub-reqbufs;
diff --git a/Documentation/DocBook/media/v4l/vidioc-dbg-g-chip-ident.xml b/Documentation/DocBook/media/v4l/vidioc-dbg-g-chip-ident.xml
index 4ecd966808d..921e18550d2 100644
--- a/Documentation/DocBook/media/v4l/vidioc-dbg-g-chip-ident.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-dbg-g-chip-ident.xml
@@ -200,10 +200,10 @@ the values from <xref linkend="chip-ids" />.</entry>
 	&cs-def;
 	<tbody valign="top">
 	  <row>
-	    <entry><constant>V4L2_CHIP_MATCH_HOST</constant></entry>
+	    <entry><constant>V4L2_CHIP_MATCH_BRIDGE</constant></entry>
 	    <entry>0</entry>
 	    <entry>Match the nth chip on the card, zero for the
-	    host chip. Does not match &i2c; chips.</entry>
+	    bridge chip. Does not match sub-devices.</entry>
 	  </row>
 	  <row>
 	    <entry><constant>V4L2_CHIP_MATCH_I2C_DRIVER</constant></entry>
@@ -220,6 +220,11 @@ the values from <xref linkend="chip-ids" />.</entry>
 	    <entry>3</entry>
 	    <entry>Match the nth anciliary AC97 chip.</entry>
 	  </row>
+	  <row>
+	    <entry><constant>V4L2_CHIP_MATCH_SUBDEV</constant></entry>
+	    <entry>4</entry>
+	    <entry>Match the nth sub-device. Can't be used with this ioctl.</entry>
+	  </row>
 	</tbody>
       </tgroup>
     </table>
diff --git a/Documentation/DocBook/media/v4l/vidioc-dbg-g-chip-info.xml b/Documentation/DocBook/media/v4l/vidioc-dbg-g-chip-info.xml
new file mode 100644
index 00000000000..e1cece6c5de
--- /dev/null
+++ b/Documentation/DocBook/media/v4l/vidioc-dbg-g-chip-info.xml
@@ -0,0 +1,223 @@
+<refentry id="vidioc-dbg-g-chip-info">
+  <refmeta>
+    <refentrytitle>ioctl VIDIOC_DBG_G_CHIP_INFO</refentrytitle>
+    &manvol;
+  </refmeta>
+
+  <refnamediv>
+    <refname>VIDIOC_DBG_G_CHIP_INFO</refname>
+    <refpurpose>Identify the chips on a TV card</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_dbg_chip_info
+*<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_DBG_G_CHIP_INFO</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>For driver debugging purposes this ioctl allows test
+applications to query the driver about the chips present on the TV
+card. Regular applications must not use it. When you found a chip
+specific bug, please contact the linux-media mailing list (&v4l-ml;)
+so it can be fixed.</para>
+
+    <para>Additionally the Linux kernel must be compiled with the
+<constant>CONFIG_VIDEO_ADV_DEBUG</constant> option to enable this ioctl.</para>
+
+    <para>To query the driver applications must initialize the
+<structfield>match.type</structfield> and
+<structfield>match.addr</structfield> or <structfield>match.name</structfield>
+fields of a &v4l2-dbg-chip-info;
+and call <constant>VIDIOC_DBG_G_CHIP_INFO</constant> with a pointer to
+this structure. On success the driver stores information about the
+selected chip in the <structfield>name</structfield> and
+<structfield>flags</structfield> fields. On failure the structure
+remains unchanged.</para>
+
+    <para>When <structfield>match.type</structfield> is
+<constant>V4L2_CHIP_MATCH_BRIDGE</constant>,
+<structfield>match.addr</structfield> selects the nth bridge 'chip'
+on the TV card. You can enumerate all chips by starting at zero and
+incrementing <structfield>match.addr</structfield> by one until
+<constant>VIDIOC_DBG_G_CHIP_INFO</constant> fails with an &EINVAL;.
+The number zero always selects the bridge chip itself, &eg; the chip
+connected to the PCI or USB bus. Non-zero numbers identify specific
+parts of the bridge chip such as an AC97 register block.</para>
+
+    <para>When <structfield>match.type</structfield> is
+<constant>V4L2_CHIP_MATCH_SUBDEV</constant>,
+<structfield>match.addr</structfield> selects the nth sub-device. This
+allows you to enumerate over all sub-devices.</para>
+
+    <para>On success, the <structfield>name</structfield> field will
+contain a chip name and the <structfield>flags</structfield> field will
+contain <constant>V4L2_CHIP_FL_READABLE</constant> if the driver supports
+reading registers from the device or <constant>V4L2_CHIP_FL_WRITABLE</constant>
+if the driver supports writing registers to the device.</para>
+
+    <para>We recommended the <application>v4l2-dbg</application>
+utility over calling this ioctl directly. It is available from the
+LinuxTV v4l-dvb repository; see <ulink
+url="http://linuxtv.org/repo/">http://linuxtv.org/repo/</ulink> for
+access instructions.</para>
+
+    <!-- Note for convenience vidioc-dbg-g-register.sgml
+	 contains a duplicate of this table. -->
+    <table pgwide="1" frame="none" id="name-v4l2-dbg-match">
+      <title>struct <structname>v4l2_dbg_match</structname></title>
+      <tgroup cols="4">
+	&cs-ustr;
+	<tbody valign="top">
+	  <row>
+	    <entry>__u32</entry>
+	    <entry><structfield>type</structfield></entry>
+	    <entry>See <xref linkend="name-chip-match-types" /> for a list of
+possible types.</entry>
+	  </row>
+	  <row>
+	    <entry>union</entry>
+	    <entry>(anonymous)</entry>
+	  </row>
+	  <row>
+	    <entry></entry>
+	    <entry>__u32</entry>
+	    <entry><structfield>addr</structfield></entry>
+	    <entry>Match a chip by this number, interpreted according
+to the <structfield>type</structfield> field.</entry>
+	  </row>
+	  <row>
+	    <entry></entry>
+	    <entry>char</entry>
+	    <entry><structfield>name[32]</structfield></entry>
+	    <entry>Match a chip by this name, interpreted according
+to the <structfield>type</structfield> field.</entry>
+	  </row>
+	</tbody>
+      </tgroup>
+    </table>
+
+    <table pgwide="1" frame="none" id="v4l2-dbg-chip-info">
+      <title>struct <structname>v4l2_dbg_chip_info</structname></title>
+      <tgroup cols="3">
+	&cs-str;
+	<tbody valign="top">
+	  <row>
+	    <entry>struct v4l2_dbg_match</entry>
+	    <entry><structfield>match</structfield></entry>
+	    <entry>How to match the chip, see <xref linkend="name-v4l2-dbg-match" />.</entry>
+	  </row>
+	  <row>
+	    <entry>char</entry>
+	    <entry><structfield>name[32]</structfield></entry>
+	    <entry>The name of the chip.</entry>
+	  </row>
+	  <row>
+	    <entry>__u32</entry>
+	    <entry><structfield>flags</structfield></entry>
+	    <entry>Set by the driver. If <constant>V4L2_CHIP_FL_READABLE</constant>
+is set, then the driver supports reading registers from the device. If
+<constant>V4L2_CHIP_FL_WRITABLE</constant> is set, then it supports writing registers.</entry>
+	  </row>
+	  <row>
+	    <entry>__u32</entry>
+	    <entry><structfield>reserved[8]</structfield></entry>
+	    <entry>Reserved fields, both application and driver must set these to 0.</entry>
+	  </row>
+	</tbody>
+      </tgroup>
+    </table>
+
+    <!-- Note for convenience vidioc-dbg-g-register.sgml
+	 contains a duplicate of this table. -->
+    <table pgwide="1" frame="none" id="name-chip-match-types">
+      <title>Chip Match Types</title>
+      <tgroup cols="3">
+	&cs-def;
+	<tbody valign="top">
+	  <row>
+	    <entry><constant>V4L2_CHIP_MATCH_BRIDGE</constant></entry>
+	    <entry>0</entry>
+	    <entry>Match the nth chip on the card, zero for the
+	    bridge chip. Does not match sub-devices.</entry>
+	  </row>
+	  <row>
+	    <entry><constant>V4L2_CHIP_MATCH_I2C_DRIVER</constant></entry>
+	    <entry>1</entry>
+	    <entry>Match an &i2c; chip by its driver name. Can't be used with this ioctl.</entry>
+	  </row>
+	  <row>
+	    <entry><constant>V4L2_CHIP_MATCH_I2C_ADDR</constant></entry>
+	    <entry>2</entry>
+	    <entry>Match a chip by its 7 bit &i2c; bus address. Can't be used with this ioctl.</entry>
+	  </row>
+	  <row>
+	    <entry><constant>V4L2_CHIP_MATCH_AC97</constant></entry>
+	    <entry>3</entry>
+	    <entry>Match the nth anciliary AC97 chip. Can't be used with this ioctl.</entry>
+	  </row>
+	  <row>
+	    <entry><constant>V4L2_CHIP_MATCH_SUBDEV</constant></entry>
+	    <entry>4</entry>
+	    <entry>Match the nth sub-device.</entry>
+	  </row>
+	</tbody>
+      </tgroup>
+    </table>
+  </refsect1>
+
+  <refsect1>
+    &return-value;
+
+    <variablelist>
+      <varlistentry>
+	<term><errorcode>EINVAL</errorcode></term>
+	<listitem>
+	  <para>The <structfield>match_type</structfield> is invalid or
+no device could be matched.</para>
+	</listitem>
+      </varlistentry>
+     </variablelist>
+  </refsect1>
+</refentry>
diff --git a/Documentation/DocBook/media/v4l/vidioc-dbg-g-register.xml b/Documentation/DocBook/media/v4l/vidioc-dbg-g-register.xml
index a44aebc7997..d13bac9e244 100644
--- a/Documentation/DocBook/media/v4l/vidioc-dbg-g-register.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-dbg-g-register.xml
@@ -87,7 +87,7 @@ written into the register.</para>
 
     <para>To read a register applications must initialize the
 <structfield>match.type</structfield>,
-<structfield>match.chip</structfield> or <structfield>match.name</structfield> and
+<structfield>match.addr</structfield> or <structfield>match.name</structfield> and
 <structfield>reg</structfield> fields, and call
 <constant>VIDIOC_DBG_G_REGISTER</constant> with a pointer to this
 structure. On success the driver stores the register value in the
@@ -95,11 +95,11 @@ structure. On success the driver stores the register value in the
 unchanged.</para>
 
     <para>When <structfield>match.type</structfield> is
-<constant>V4L2_CHIP_MATCH_HOST</constant>,
-<structfield>match.addr</structfield> selects the nth non-&i2c; chip
+<constant>V4L2_CHIP_MATCH_BRIDGE</constant>,
+<structfield>match.addr</structfield> selects the nth non-sub-device chip
 on the TV card.  The number zero always selects the host chip, &eg; the
 chip connected to the PCI or USB bus. You can find out which chips are
-present with the &VIDIOC-DBG-G-CHIP-IDENT; ioctl.</para>
+present with the &VIDIOC-DBG-G-CHIP-INFO; ioctl.</para>
 
     <para>When <structfield>match.type</structfield> is
 <constant>V4L2_CHIP_MATCH_I2C_DRIVER</constant>,
@@ -109,7 +109,7 @@ For instance
 supported by the saa7127 driver, regardless of its &i2c; bus address.
 When multiple chips supported by the same driver are present, the
 effect of these ioctls is undefined. Again with the
-&VIDIOC-DBG-G-CHIP-IDENT; ioctl you can find out which &i2c; chips are
+&VIDIOC-DBG-G-CHIP-INFO; ioctl you can find out which &i2c; chips are
 present.</para>
 
     <para>When <structfield>match.type</structfield> is
@@ -122,19 +122,23 @@ bus address.</para>
 <structfield>match.addr</structfield> selects the nth AC97 chip
 on the TV card.</para>
 
+    <para>When <structfield>match.type</structfield> is
+<constant>V4L2_CHIP_MATCH_SUBDEV</constant>,
+<structfield>match.addr</structfield> selects the nth sub-device.</para>
+
     <note>
       <title>Success not guaranteed</title>
 
       <para>Due to a flaw in the Linux &i2c; bus driver these ioctls may
 return successfully without actually reading or writing a register. To
-catch the most likely failure we recommend a &VIDIOC-DBG-G-CHIP-IDENT;
+catch the most likely failure we recommend a &VIDIOC-DBG-G-CHIP-INFO;
 call confirming the presence of the selected &i2c; chip.</para>
     </note>
 
     <para>These ioctls are optional, not all drivers may support them.
 However when a driver supports these ioctls it must also support
-&VIDIOC-DBG-G-CHIP-IDENT;. Conversely it may support
-<constant>VIDIOC_DBG_G_CHIP_IDENT</constant> but not these ioctls.</para>
+&VIDIOC-DBG-G-CHIP-INFO;. Conversely it may support
+<constant>VIDIOC_DBG_G_CHIP_INFO</constant> but not these ioctls.</para>
 
     <para><constant>VIDIOC_DBG_G_REGISTER</constant> and
 <constant>VIDIOC_DBG_S_REGISTER</constant> were introduced in Linux
@@ -217,10 +221,10 @@ register.</entry>
 	&cs-def;
 	<tbody valign="top">
 	  <row>
-	    <entry><constant>V4L2_CHIP_MATCH_HOST</constant></entry>
+	    <entry><constant>V4L2_CHIP_MATCH_BRIDGE</constant></entry>
 	    <entry>0</entry>
 	    <entry>Match the nth chip on the card, zero for the
-	    host chip. Does not match &i2c; chips.</entry>
+	    bridge chip. Does not match sub-devices.</entry>
 	  </row>
 	  <row>
 	    <entry><constant>V4L2_CHIP_MATCH_I2C_DRIVER</constant></entry>
@@ -237,6 +241,11 @@ register.</entry>
 	    <entry>3</entry>
 	    <entry>Match the nth anciliary AC97 chip.</entry>
 	  </row>
+	  <row>
+	    <entry><constant>V4L2_CHIP_MATCH_SUBDEV</constant></entry>
+	    <entry>4</entry>
+	    <entry>Match the nth sub-device.</entry>
+	  </row>
 	</tbody>
       </tgroup>
     </table>
diff --git a/Documentation/DocBook/media/v4l/vidioc-enum-dv-presets.xml b/Documentation/DocBook/media/v4l/vidioc-enum-dv-presets.xml
deleted file mode 100644
index fced5fb0dbf..00000000000
--- a/Documentation/DocBook/media/v4l/vidioc-enum-dv-presets.xml
+++ /dev/null
@@ -1,240 +0,0 @@
-<refentry id="vidioc-enum-dv-presets">
-  <refmeta>
-    <refentrytitle>ioctl VIDIOC_ENUM_DV_PRESETS</refentrytitle>
-    &manvol;
-  </refmeta>
-
-  <refnamediv>
-    <refname>VIDIOC_ENUM_DV_PRESETS</refname>
-    <refpurpose>Enumerate supported Digital Video presets</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_enum_preset *<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_PRESETS</para>
-	</listitem>
-      </varlistentry>
-      <varlistentry>
-	<term><parameter>argp</parameter></term>
-	<listitem>
-	  <para></para>
-	</listitem>
-      </varlistentry>
-    </variablelist>
-  </refsect1>
-
-  <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
-structure. Drivers fill the rest of the structure or return an
-&EINVAL; when the index is out of bounds. To enumerate all DV Presets supported,
-applications shall begin at index zero, incrementing by one until the
-driver returns <errorcode>EINVAL</errorcode>. Drivers may enumerate a
-different set of DV presets after switching the video input or
-output.</para>
-
-    <table pgwide="1" frame="none" id="v4l2-dv-enum-preset">
-      <title>struct <structname>v4l2_dv_enum_presets</structname></title>
-      <tgroup cols="3">
-	&cs-str;
-	<tbody valign="top">
-	  <row>
-	    <entry>__u32</entry>
-	    <entry><structfield>index</structfield></entry>
-	    <entry>Number of the DV preset, set by the
-application.</entry>
-	  </row>
-	  <row>
-	    <entry>__u32</entry>
-	    <entry><structfield>preset</structfield></entry>
-	    <entry>This field identifies one of the DV preset values listed in <xref linkend="v4l2-dv-presets-vals"/>.</entry>
-	  </row>
-	  <row>
-	    <entry>__u8</entry>
-	    <entry><structfield>name</structfield>[24]</entry>
-	    <entry>Name of the preset, a NUL-terminated ASCII string, for example: "720P-60", "1080I-60". This information is
-intended for the user.</entry>
-	  </row>
-	  <row>
-	    <entry>__u32</entry>
-	    <entry><structfield>width</structfield></entry>
-	    <entry>Width of the active video in pixels for the DV preset.</entry>
-	  </row>
-	  <row>
-	    <entry>__u32</entry>
-	    <entry><structfield>height</structfield></entry>
-	    <entry>Height of the active video in lines for the DV preset.</entry>
-	  </row>
-	  <row>
-	    <entry>__u32</entry>
-	    <entry><structfield>reserved</structfield>[4]</entry>
-	    <entry>Reserved for future extensions. Drivers must set the array to zero.</entry>
-	  </row>
-	</tbody>
-      </tgroup>
-    </table>
-
-    <table pgwide="1" frame="none" id="v4l2-dv-presets-vals">
-      <title>struct <structname>DV Presets</structname></title>
-      <tgroup cols="3">
-	&cs-str;
-	<tbody valign="top">
-	  <row>
-	    <entry>Preset</entry>
-	    <entry>Preset value</entry>
-	    <entry>Description</entry>
-	  </row>
-	  <row>
-	    <entry></entry>
-	    <entry></entry>
-	    <entry></entry>
-	  </row>
-	  <row>
-	    <entry>V4L2_DV_INVALID</entry>
-	    <entry>0</entry>
-	    <entry>Invalid preset value.</entry>
-	  </row>
-	  <row>
-	    <entry>V4L2_DV_480P59_94</entry>
-	    <entry>1</entry>
-	    <entry>720x480 progressive video at 59.94 fps as per BT.1362.</entry>
-	  </row>
-	  <row>
-	    <entry>V4L2_DV_576P50</entry>
-	    <entry>2</entry>
-	    <entry>720x576 progressive video at 50 fps as per BT.1362.</entry>
-	  </row>
-	  <row>
-	    <entry>V4L2_DV_720P24</entry>
-	    <entry>3</entry>
-	    <entry>1280x720 progressive video at 24 fps as per SMPTE 296M.</entry>
-	  </row>
-	  <row>
-	    <entry>V4L2_DV_720P25</entry>
-	    <entry>4</entry>
-	    <entry>1280x720 progressive video at 25 fps as per SMPTE 296M.</entry>
-	  </row>
-	  <row>
-	    <entry>V4L2_DV_720P30</entry>
-	    <entry>5</entry>
-	    <entry>1280x720 progressive video at 30 fps as per SMPTE 296M.</entry>
-	  </row>
-	  <row>
-	    <entry>V4L2_DV_720P50</entry>
-	    <entry>6</entry>
-	    <entry>1280x720 progressive video at 50 fps as per SMPTE 296M.</entry>
-	  </row>
-	  <row>
-	    <entry>V4L2_DV_720P59_94</entry>
-	    <entry>7</entry>
-	    <entry>1280x720 progressive video at 59.94 fps as per SMPTE 274M.</entry>
-	  </row>
-	  <row>
-	    <entry>V4L2_DV_720P60</entry>
-	    <entry>8</entry>
-	    <entry>1280x720 progressive video at 60 fps as per SMPTE 274M/296M.</entry>
-	  </row>
-	  <row>
-	    <entry>V4L2_DV_1080I29_97</entry>
-	    <entry>9</entry>
-	    <entry>1920x1080 interlaced video at 29.97 fps as per BT.1120/SMPTE 274M.</entry>
-	  </row>
-	  <row>
-	    <entry>V4L2_DV_1080I30</entry>
-	    <entry>10</entry>
-	    <entry>1920x1080 interlaced video at 30 fps as per BT.1120/SMPTE 274M.</entry>
-	  </row>
-	  <row>
-	    <entry>V4L2_DV_1080I25</entry>
-	    <entry>11</entry>
-	    <entry>1920x1080 interlaced video at 25 fps as per BT.1120.</entry>
-	  </row>
-	  <row>
-	    <entry>V4L2_DV_1080I50</entry>
-	    <entry>12</entry>
-	    <entry>1920x1080 interlaced video at 50 fps as per SMPTE 296M.</entry>
-	  </row>
-	  <row>
-	    <entry>V4L2_DV_1080I60</entry>
-	    <entry>13</entry>
-	    <entry>1920x1080 interlaced video at 60 fps as per SMPTE 296M.</entry>
-	  </row>
-	  <row>
-	    <entry>V4L2_DV_1080P24</entry>
-	    <entry>14</entry>
-	    <entry>1920x1080 progressive video at 24 fps as per SMPTE 296M.</entry>
-	  </row>
-	  <row>
-	    <entry>V4L2_DV_1080P25</entry>
-	    <entry>15</entry>
-	    <entry>1920x1080 progressive video at 25 fps as per SMPTE 296M.</entry>
-	  </row>
-	  <row>
-	    <entry>V4L2_DV_1080P30</entry>
-	    <entry>16</entry>
-	    <entry>1920x1080 progressive video at 30 fps as per SMPTE 296M.</entry>
-	  </row>
-	  <row>
-	    <entry>V4L2_DV_1080P50</entry>
-	    <entry>17</entry>
-	    <entry>1920x1080 progressive video at 50 fps as per BT.1120.</entry>
-	  </row>
-	  <row>
-	    <entry>V4L2_DV_1080P60</entry>
-	    <entry>18</entry>
-	    <entry>1920x1080 progressive video at 60 fps as per BT.1120.</entry>
-	  </row>
-	</tbody>
-      </tgroup>
-    </table>
-  </refsect1>
-
-  <refsect1>
-    &return-value;
-
-    <variablelist>
-      <varlistentry>
-	<term><errorcode>EINVAL</errorcode></term>
-	<listitem>
-	  <para>The &v4l2-dv-enum-preset; <structfield>index</structfield>
-is out of bounds.</para>
-	</listitem>
-      </varlistentry>
-      <varlistentry>
-	<term><errorcode>ENODATA</errorcode></term>
-	<listitem>
-	  <para>Digital video presets are not supported for this input or output.</para>
-	</listitem>
-      </varlistentry>
-    </variablelist>
-  </refsect1>
-</refentry>
diff --git a/Documentation/DocBook/media/v4l/vidioc-enuminput.xml b/Documentation/DocBook/media/v4l/vidioc-enuminput.xml
index 3c9a81305ad..493a39a8ef2 100644
--- a/Documentation/DocBook/media/v4l/vidioc-enuminput.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-enuminput.xml
@@ -278,11 +278,6 @@ input/output interface to linux-media@vger.kernel.org on 19 Oct 2009.
 	&cs-def;
 	<tbody valign="top">
 	  <row>
-	    <entry><constant>V4L2_IN_CAP_PRESETS</constant></entry>
-	    <entry>0x00000001</entry>
-	    <entry>This input supports setting DV presets by using VIDIOC_S_DV_PRESET.</entry>
-	  </row>
-	  <row>
 	    <entry><constant>V4L2_IN_CAP_DV_TIMINGS</constant></entry>
 	    <entry>0x00000002</entry>
 	    <entry>This input supports setting video timings by using VIDIOC_S_DV_TIMINGS.</entry>
diff --git a/Documentation/DocBook/media/v4l/vidioc-enumoutput.xml b/Documentation/DocBook/media/v4l/vidioc-enumoutput.xml
index f4ab0798545..2654e097df3 100644
--- a/Documentation/DocBook/media/v4l/vidioc-enumoutput.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-enumoutput.xml
@@ -163,11 +163,6 @@ input/output interface to linux-media@vger.kernel.org on 19 Oct 2009.
 	&cs-def;
 	<tbody valign="top">
 	  <row>
-	    <entry><constant>V4L2_OUT_CAP_PRESETS</constant></entry>
-	    <entry>0x00000001</entry>
-	    <entry>This output supports setting DV presets by using VIDIOC_S_DV_PRESET.</entry>
-	  </row>
-	  <row>
 	    <entry><constant>V4L2_OUT_CAP_DV_TIMINGS</constant></entry>
 	    <entry>0x00000002</entry>
 	    <entry>This output supports setting video timings by using VIDIOC_S_DV_TIMINGS.</entry>
diff --git a/Documentation/DocBook/media/v4l/vidioc-g-dv-preset.xml b/Documentation/DocBook/media/v4l/vidioc-g-dv-preset.xml
deleted file mode 100644
index b9ea37634f6..00000000000
--- a/Documentation/DocBook/media/v4l/vidioc-g-dv-preset.xml
+++ /dev/null
@@ -1,113 +0,0 @@
-<refentry id="vidioc-g-dv-preset">
-  <refmeta>
-    <refentrytitle>ioctl VIDIOC_G_DV_PRESET, VIDIOC_S_DV_PRESET</refentrytitle>
-    &manvol;
-  </refmeta>
-
-  <refnamediv>
-    <refname>VIDIOC_G_DV_PRESET</refname>
-    <refname>VIDIOC_S_DV_PRESET</refname>
-    <refpurpose>Query or select the DV preset of the current input or output</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_preset *<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_G_DV_PRESET, VIDIOC_S_DV_PRESET</para>
-	</listitem>
-      </varlistentry>
-      <varlistentry>
-	<term><parameter>argp</parameter></term>
-	<listitem>
-	  <para></para>
-	</listitem>
-      </varlistentry>
-    </variablelist>
-  </refsect1>
-
-  <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.
-Applications must zero the reserved array in &v4l2-dv-preset;.
-<constant>VIDIOC_G_DV_PRESET</constant> returns a dv preset in the field
-<structfield>preset</structfield> of &v4l2-dv-preset;.</para>
-
-    <para><constant>VIDIOC_S_DV_PRESET</constant> accepts a pointer to a &v4l2-dv-preset;
-that has the preset value to be set. Applications must zero the reserved array in &v4l2-dv-preset;.
-If the preset is not supported, it returns an &EINVAL; </para>
-  </refsect1>
-
-  <refsect1>
-    &return-value;
-
-    <variablelist>
-      <varlistentry>
-	<term><errorcode>EINVAL</errorcode></term>
-	<listitem>
-	  <para>This ioctl is not supported, or the
-<constant>VIDIOC_S_DV_PRESET</constant>,<constant>VIDIOC_S_DV_PRESET</constant> parameter was unsuitable.</para>
-	</listitem>
-      </varlistentry>
-      <varlistentry>
-	<term><errorcode>ENODATA</errorcode></term>
-	<listitem>
-	  <para>Digital video presets are not supported for this input or output.</para>
-	</listitem>
-      </varlistentry>
-      <varlistentry>
-	<term><errorcode>EBUSY</errorcode></term>
-	<listitem>
-	  <para>The device is busy and therefore can not change the preset.</para>
-	</listitem>
-      </varlistentry>
-    </variablelist>
-
-    <table pgwide="1" frame="none" id="v4l2-dv-preset">
-      <title>struct <structname>v4l2_dv_preset</structname></title>
-      <tgroup cols="3">
-	&cs-str;
-	<tbody valign="top">
-	  <row>
-	    <entry>__u32</entry>
-	    <entry><structfield>preset</structfield></entry>
-	    <entry>Preset value to represent the digital video timings</entry>
-	  </row>
-	  <row>
-	    <entry>__u32</entry>
-	    <entry><structfield>reserved[4]</structfield></entry>
-	    <entry>Reserved fields for future use</entry>
-	  </row>
-	</tbody>
-      </tgroup>
-    </table>
-  </refsect1>
-</refentry>
diff --git a/Documentation/DocBook/media/v4l/vidioc-g-ext-ctrls.xml b/Documentation/DocBook/media/v4l/vidioc-g-ext-ctrls.xml
index 4e16112df99..b3bb9575b2e 100644
--- a/Documentation/DocBook/media/v4l/vidioc-g-ext-ctrls.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-g-ext-ctrls.xml
@@ -319,6 +319,15 @@ These controls are described in <xref
 	    processing controls. These controls are described in <xref
 	    linkend="image-process-controls" />.</entry>
 	  </row>
+
+	  <row>
+	    <entry><constant>V4L2_CTRL_CLASS_FM_RX</constant></entry>
+	    <entry>0xa10000</entry>
+	    <entry>The class containing FM Receiver (FM RX) controls.
+These controls are described in <xref
+		linkend="fm-rx-controls" />.</entry>
+	  </row>
+
 	</tbody>
       </tgroup>
     </table>
diff --git a/Documentation/DocBook/media/v4l/vidioc-query-dv-preset.xml b/Documentation/DocBook/media/v4l/vidioc-query-dv-preset.xml
deleted file mode 100644
index 68b49d09e24..00000000000
--- a/Documentation/DocBook/media/v4l/vidioc-query-dv-preset.xml
+++ /dev/null
@@ -1,78 +0,0 @@
-<refentry id="vidioc-query-dv-preset">
-  <refmeta>
-    <refentrytitle>ioctl VIDIOC_QUERY_DV_PRESET</refentrytitle>
-    &manvol;
-  </refmeta>
-
-  <refnamediv>
-    <refname>VIDIOC_QUERY_DV_PRESET</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_preset *<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_PRESET</para>
-	</listitem>
-      </varlistentry>
-      <varlistentry>
-	<term><parameter>argp</parameter></term>
-	<listitem>
-	  <para></para>
-	</listitem>
-      </varlistentry>
-    </variablelist>
-  </refsect1>
-
-  <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
-&v4l2-dv-preset; type. Once the hardware detects a preset, that preset is
-returned in the preset field of &v4l2-dv-preset;. If the preset could not be
-detected because there was no signal, or the signal was unreliable, or the
-signal did not map to a supported preset, then the value V4L2_DV_INVALID is
-returned.</para>
-  </refsect1>
-
-  <refsect1>
-    &return-value;
-
-    <variablelist>
-      <varlistentry>
-	<term><errorcode>ENODATA</errorcode></term>
-	<listitem>
-	  <para>Digital video presets are not supported for this input or output.</para>
-	</listitem>
-      </varlistentry>
-    </variablelist>
-  </refsect1>
-</refentry>
diff --git a/Documentation/DocBook/media_api.tmpl b/Documentation/DocBook/media_api.tmpl
index 1f6593deb99..6a8b7158697 100644
--- a/Documentation/DocBook/media_api.tmpl
+++ b/Documentation/DocBook/media_api.tmpl
@@ -23,6 +23,7 @@
 <!-- LinuxTV v4l-dvb repository. -->
 <!ENTITY v4l-dvb		"<ulink url='http://linuxtv.org/repo/'>http://linuxtv.org/repo/</ulink>">
 <!ENTITY dash-ent-10            "<entry>-</entry><entry>-</entry><entry>-</entry><entry>-</entry><entry>-</entry><entry>-</entry><entry>-</entry><entry>-</entry><entry>-</entry><entry>-</entry>">
+<!ENTITY dash-ent-16            "<entry>-</entry><entry>-</entry><entry>-</entry><entry>-</entry><entry>-</entry><entry>-</entry><entry>-</entry><entry>-</entry><entry>-</entry><entry>-</entry><entry>-</entry><entry>-</entry><entry>-</entry><entry>-</entry><entry>-</entry><entry>-</entry>">
 ]>
 
 <book id="media_api">
diff --git a/Documentation/RCU/checklist.txt b/Documentation/RCU/checklist.txt
index 31ef8fe07f8..79e789b8b8e 100644
--- a/Documentation/RCU/checklist.txt
+++ b/Documentation/RCU/checklist.txt
@@ -217,9 +217,14 @@ over a rather long period of time, but improvements are always welcome!
 	whether the increased speed is worth it.
 
 8.	Although synchronize_rcu() is slower than is call_rcu(), it
-	usually results in simpler code.  So, unless update performance
-	is critically important or the updaters cannot block,
-	synchronize_rcu() should be used in preference to call_rcu().
+	usually results in simpler code.  So, unless update performance is
+	critically important, the updaters cannot block, or the latency of
+	synchronize_rcu() is visible from userspace, synchronize_rcu()
+	should be used in preference to call_rcu().  Furthermore,
+	kfree_rcu() usually results in even simpler code than does
+	synchronize_rcu() without synchronize_rcu()'s multi-millisecond
+	latency.  So please take advantage of kfree_rcu()'s "fire and
+	forget" memory-freeing capabilities where it applies.
 
 	An especially important property of the synchronize_rcu()
 	primitive is that it automatically self-limits: if grace periods
@@ -268,7 +273,8 @@ over a rather long period of time, but improvements are always welcome!
 	e.	Periodically invoke synchronize_rcu(), permitting a limited
 		number of updates per grace period.
 
-	The same cautions apply to call_rcu_bh() and call_rcu_sched().
+	The same cautions apply to call_rcu_bh(), call_rcu_sched(),
+	call_srcu(), and kfree_rcu().
 
 9.	All RCU list-traversal primitives, which include
 	rcu_dereference(), list_for_each_entry_rcu(), and
@@ -296,9 +302,9 @@ over a rather long period of time, but improvements are always welcome!
 	all currently executing rcu_read_lock()-protected RCU read-side
 	critical sections complete.  It does -not- necessarily guarantee
 	that all currently running interrupts, NMIs, preempt_disable()
-	code, or idle loops will complete.  Therefore, if you do not have
-	rcu_read_lock()-protected read-side critical sections, do -not-
-	use synchronize_rcu().
+	code, or idle loops will complete.  Therefore, if your
+	read-side critical sections are protected by something other
+	than rcu_read_lock(), do -not- use synchronize_rcu().
 
 	Similarly, disabling preemption is not an acceptable substitute
 	for rcu_read_lock().  Code that attempts to use preemption
@@ -401,9 +407,9 @@ over a rather long period of time, but improvements are always welcome!
 	read-side critical sections.  It is the responsibility of the
 	RCU update-side primitives to deal with this.
 
-17.	Use CONFIG_PROVE_RCU, CONFIG_DEBUG_OBJECTS_RCU_HEAD, and
-	the __rcu sparse checks to validate your RCU code.  These
-	can help find problems as follows:
+17.	Use CONFIG_PROVE_RCU, CONFIG_DEBUG_OBJECTS_RCU_HEAD, and the
+	__rcu sparse checks (enabled by CONFIG_SPARSE_RCU_POINTER) to
+	validate your RCU code.  These can help find problems as follows:
 
 	CONFIG_PROVE_RCU: check that accesses to RCU-protected data
 		structures are carried out under the proper RCU
diff --git a/Documentation/RCU/lockdep.txt b/Documentation/RCU/lockdep.txt
index a102d4b3724..cd83d2348fe 100644
--- a/Documentation/RCU/lockdep.txt
+++ b/Documentation/RCU/lockdep.txt
@@ -64,6 +64,11 @@ checking of rcu_dereference() primitives:
 		but retain the compiler constraints that prevent duplicating
 		or coalescsing.  This is useful when when testing the
 		value of the pointer itself, for example, against NULL.
+	rcu_access_index(idx):
+		Return the value of the index and omit all barriers, but
+		retain the compiler constraints that prevent duplicating
+		or coalescsing.  This is useful when when testing the
+		value of the index itself, for example, against -1.
 
 The rcu_dereference_check() check expression can be any boolean
 expression, but would normally include a lockdep expression.  However,
diff --git a/Documentation/RCU/rcubarrier.txt b/Documentation/RCU/rcubarrier.txt
index 38428c12513..2e319d1b9ef 100644
--- a/Documentation/RCU/rcubarrier.txt
+++ b/Documentation/RCU/rcubarrier.txt
@@ -79,7 +79,20 @@ complete. Pseudo-code using rcu_barrier() is as follows:
    2. Execute rcu_barrier().
    3. Allow the module to be unloaded.
 
-The rcutorture module makes use of rcu_barrier in its exit function
+There are also rcu_barrier_bh(), rcu_barrier_sched(), and srcu_barrier()
+functions for the other flavors of RCU, and you of course must match
+the flavor of rcu_barrier() with that of call_rcu().  If your module
+uses multiple flavors of call_rcu(), then it must also use multiple
+flavors of rcu_barrier() when unloading that module.  For example, if
+it uses call_rcu_bh(), call_srcu() on srcu_struct_1, and call_srcu() on
+srcu_struct_2(), then the following three lines of code will be required
+when unloading:
+
+ 1 rcu_barrier_bh();
+ 2 srcu_barrier(&srcu_struct_1);
+ 3 srcu_barrier(&srcu_struct_2);
+
+The rcutorture module makes use of rcu_barrier() in its exit function
 as follows:
 
  1 static void
diff --git a/Documentation/RCU/stallwarn.txt b/Documentation/RCU/stallwarn.txt
index 1927151b386..e38b8df3d72 100644
--- a/Documentation/RCU/stallwarn.txt
+++ b/Documentation/RCU/stallwarn.txt
@@ -92,14 +92,14 @@ If the CONFIG_RCU_CPU_STALL_INFO kernel configuration parameter is set,
 more information is printed with the stall-warning message, for example:
 
 	INFO: rcu_preempt detected stall on CPU
-	0: (63959 ticks this GP) idle=241/3fffffffffffffff/0
+	0: (63959 ticks this GP) idle=241/3fffffffffffffff/0 softirq=82/543
 	   (t=65000 jiffies)
 
 In kernels with CONFIG_RCU_FAST_NO_HZ, even more information is
 printed:
 
 	INFO: rcu_preempt detected stall on CPU
-	0: (64628 ticks this GP) idle=dd5/3fffffffffffffff/0 drain=0 . timer not pending
+	0: (64628 ticks this GP) idle=dd5/3fffffffffffffff/0 softirq=82/543 last_accelerate: a345/d342 nonlazy_posted: 25 .D
 	   (t=65000 jiffies)
 
 The "(64628 ticks this GP)" indicates that this CPU has taken more
@@ -116,13 +116,28 @@ number between the two "/"s is the value of the nesting, which will
 be a small positive number if in the idle loop and a very large positive
 number (as shown above) otherwise.
 
-For CONFIG_RCU_FAST_NO_HZ kernels, the "drain=0" indicates that the CPU is
-not in the process of trying to force itself into dyntick-idle state, the
-"." indicates that the CPU has not given up forcing RCU into dyntick-idle
-mode (it would be "H" otherwise), and the "timer not pending" indicates
-that the CPU has not recently forced RCU into dyntick-idle mode (it
-would otherwise indicate the number of microseconds remaining in this
-forced state).
+The "softirq=" portion of the message tracks the number of RCU softirq
+handlers that the stalled CPU has executed.  The number before the "/"
+is the number that had executed since boot at the time that this CPU
+last noted the beginning of a grace period, which might be the current
+(stalled) grace period, or it might be some earlier grace period (for
+example, if the CPU might have been in dyntick-idle mode for an extended
+time period.  The number after the "/" is the number that have executed
+since boot until the current time.  If this latter number stays constant
+across repeated stall-warning messages, it is possible that RCU's softirq
+handlers are no longer able to execute on this CPU.  This can happen if
+the stalled CPU is spinning with interrupts are disabled, or, in -rt
+kernels, if a high-priority process is starving RCU's softirq handler.
+
+For CONFIG_RCU_FAST_NO_HZ kernels, the "last_accelerate:" prints the
+low-order 16 bits (in hex) of the jiffies counter when this CPU last
+invoked rcu_try_advance_all_cbs() from rcu_needs_cpu() or last invoked
+rcu_accelerate_cbs() from rcu_prepare_for_idle().  The "nonlazy_posted:"
+prints the number of non-lazy callbacks posted since the last call to
+rcu_needs_cpu().  Finally, an "L" indicates that there are currently
+no non-lazy callbacks ("." is printed otherwise, as shown above) and
+"D" indicates that dyntick-idle processing is enabled ("." is printed
+otherwise, for example, if disabled via the "nohz=" kernel boot parameter).
 
 
 Multiple Warnings From One Stall
diff --git a/Documentation/RCU/whatisRCU.txt b/Documentation/RCU/whatisRCU.txt
index 0cc7820967f..10df0b82f45 100644
--- a/Documentation/RCU/whatisRCU.txt
+++ b/Documentation/RCU/whatisRCU.txt
@@ -265,9 +265,9 @@ rcu_dereference()
 		rcu_read_lock();
 		p = rcu_dereference(head.next);
 		rcu_read_unlock();
-		x = p->address;
+		x = p->address;	/* BUG!!! */
 		rcu_read_lock();
-		y = p->data;
+		y = p->data;	/* BUG!!! */
 		rcu_read_unlock();
 
 	Holding a reference from one RCU read-side critical section
diff --git a/Documentation/SubmittingPatches b/Documentation/SubmittingPatches
index aa0c1e63f05..6e97e73d87b 100644
--- a/Documentation/SubmittingPatches
+++ b/Documentation/SubmittingPatches
@@ -420,7 +420,7 @@ person it names.  This tag documents that potentially interested parties
 have been included in the discussion
 
 
-14) Using Reported-by:, Tested-by: and Reviewed-by:
+14) Using Reported-by:, Tested-by:, Reviewed-by: and Suggested-by:
 
 If this patch fixes a problem reported by somebody else, consider adding a
 Reported-by: tag to credit the reporter for their contribution.  Please
@@ -468,6 +468,13 @@ done on the patch.  Reviewed-by: tags, when supplied by reviewers known to
 understand the subject area and to perform thorough reviews, will normally
 increase the likelihood of your patch getting into the kernel.
 
+A Suggested-by: tag indicates that the patch idea is suggested by the person
+named and ensures credit to the person for the idea. Please note that this
+tag should not be added without the reporter's permission, especially if the
+idea was not posted in a public forum. That said, if we diligently credit our
+idea reporters, they will, hopefully, be inspired to help us again in the
+future.
+
 
 15) The canonical patch format
 
diff --git a/Documentation/arm/sunxi/clocks.txt b/Documentation/arm/sunxi/clocks.txt
new file mode 100644
index 00000000000..e09a88aa313
--- /dev/null
+++ b/Documentation/arm/sunxi/clocks.txt
@@ -0,0 +1,56 @@
+Frequently asked questions about the sunxi clock system
+=======================================================
+
+This document contains useful bits of information that people tend to ask
+about the sunxi clock system, as well as accompanying ASCII art when adequate.
+
+Q: Why is the main 24MHz oscillator gatable? Wouldn't that break the
+   system?
+
+A: The 24MHz oscillator allows gating to save power. Indeed, if gated
+   carelessly the system would stop functioning, but with the right
+   steps, one can gate it and keep the system running. Consider this
+   simplified suspend example:
+
+   While the system is operational, you would see something like
+
+      24MHz         32kHz
+       |
+      PLL1
+       \
+        \_ CPU Mux
+             |
+           [CPU]
+
+   When you are about to suspend, you switch the CPU Mux to the 32kHz
+   oscillator:
+
+      24Mhz         32kHz
+       |              |
+      PLL1            |
+                     /
+           CPU Mux _/
+             |
+           [CPU]
+
+    Finally you can gate the main oscillator
+
+                    32kHz
+                      |
+                      |
+                     /
+           CPU Mux _/
+             |
+           [CPU]
+
+Q: Were can I learn more about the sunxi clocks?
+
+A: The linux-sunxi wiki contains a page documenting the clock registers,
+   you can find it at
+
+        http://linux-sunxi.org/A10/CCM
+
+   The authoritative source for information at this time is the ccmu driver
+   released by Allwinner, you can find it at
+
+        https://github.com/linux-sunxi/linux-sunxi/tree/sunxi-3.0/arch/arm/mach-sun4i/clock/ccmu
diff --git a/Documentation/backlight/lp855x-driver.txt b/Documentation/backlight/lp855x-driver.txt
index 18b06ca038e..1c732f0c675 100644
--- a/Documentation/backlight/lp855x-driver.txt
+++ b/Documentation/backlight/lp855x-driver.txt
@@ -32,14 +32,10 @@ Platform data for lp855x
 For supporting platform specific data, the lp855x platform data can be used.
 
 * name : Backlight driver name. If it is not defined, default name is set.
-* mode : Brightness control mode. PWM or register based.
 * device_control : Value of DEVICE CONTROL register.
 * initial_brightness : Initial value of backlight brightness.
 * period_ns : Platform specific PWM period value. unit is nano.
 	     Only valid when brightness is pwm input mode.
-* load_new_rom_data :
-	0 : use default configuration data
-	1 : update values of eeprom or eprom registers on loading driver
 * size_program : Total size of lp855x_rom_data.
 * rom_data : List of new eeprom/eprom registers.
 
@@ -54,10 +50,8 @@ static struct lp855x_rom_data lp8552_eeprom_arr[] = {
 
 static struct lp855x_platform_data lp8552_pdata = {
 	.name = "lcd-bl",
-	.mode = REGISTER_BASED,
 	.device_control = I2C_CONFIG(LP8552),
 	.initial_brightness = INITIAL_BRT,
-	.load_new_rom_data = 1,
 	.size_program = ARRAY_SIZE(lp8552_eeprom_arr),
 	.rom_data = lp8552_eeprom_arr,
 };
@@ -65,7 +59,6 @@ static struct lp855x_platform_data lp8552_pdata = {
 example 2) lp8556 platform data : pwm input mode with default rom data
 
 static struct lp855x_platform_data lp8556_pdata = {
-	.mode = PWM_BASED,
 	.device_control = PWM_CONFIG(LP8556),
 	.initial_brightness = INITIAL_BRT,
 	.period_ns = 1000000,
diff --git a/Documentation/cgroups/cgroups.txt b/Documentation/cgroups/cgroups.txt
index bcf1a00b06a..638bf17ff86 100644
--- a/Documentation/cgroups/cgroups.txt
+++ b/Documentation/cgroups/cgroups.txt
@@ -442,7 +442,7 @@ You can attach the current shell task by echoing 0:
 You can use the cgroup.procs file instead of the tasks file to move all
 threads in a threadgroup at once. Echoing the PID of any task in a
 threadgroup to cgroup.procs causes all tasks in that threadgroup to be
-be attached to the cgroup. Writing 0 to cgroup.procs moves all tasks
+attached to the cgroup. Writing 0 to cgroup.procs moves all tasks
 in the writing task's threadgroup.
 
 Note: Since every task is always a member of exactly one cgroup in each
@@ -580,6 +580,7 @@ propagation along the hierarchy. See the comment on
 cgroup_for_each_descendant_pre() for details.
 
 void css_offline(struct cgroup *cgrp);
+(cgroup_mutex held by caller)
 
 This is the counterpart of css_online() and called iff css_online()
 has succeeded on @cgrp. This signifies the beginning of the end of
diff --git a/Documentation/cgroups/devices.txt b/Documentation/cgroups/devices.txt
index 16624a7f822..3c1095ca02e 100644
--- a/Documentation/cgroups/devices.txt
+++ b/Documentation/cgroups/devices.txt
@@ -13,9 +13,7 @@ either an integer or * for all.  Access is a composition of r
 The root device cgroup starts with rwm to 'all'.  A child device
 cgroup gets a copy of the parent.  Administrators can then remove
 devices from the whitelist or add new entries.  A child cgroup can
-never receive a device access which is denied by its parent.  However
-when a device access is removed from a parent it will not also be
-removed from the child(ren).
+never receive a device access which is denied by its parent.
 
 2. User Interface
 
@@ -50,3 +48,69 @@ task to a new cgroup.  (Again we'll probably want to change that).
 
 A cgroup may not be granted more permissions than the cgroup's
 parent has.
+
+4. Hierarchy
+
+device cgroups maintain hierarchy by making sure a cgroup never has more
+access permissions than its parent.  Every time an entry is written to
+a cgroup's devices.deny file, all its children will have that entry removed
+from their whitelist and all the locally set whitelist entries will be
+re-evaluated.  In case one of the locally set whitelist entries would provide
+more access than the cgroup's parent, it'll be removed from the whitelist.
+
+Example:
+      A
+     / \
+        B
+
+    group        behavior	exceptions
+    A            allow		"b 8:* rwm", "c 116:1 rw"
+    B            deny		"c 1:3 rwm", "c 116:2 rwm", "b 3:* rwm"
+
+If a device is denied in group A:
+	# echo "c 116:* r" > A/devices.deny
+it'll propagate down and after revalidating B's entries, the whitelist entry
+"c 116:2 rwm" will be removed:
+
+    group        whitelist entries                        denied devices
+    A            all                                      "b 8:* rwm", "c 116:* rw"
+    B            "c 1:3 rwm", "b 3:* rwm"                 all the rest
+
+In case parent's exceptions change and local exceptions are not allowed
+anymore, they'll be deleted.
+
+Notice that new whitelist entries will not be propagated:
+      A
+     / \
+        B
+
+    group        whitelist entries                        denied devices
+    A            "c 1:3 rwm", "c 1:5 r"                   all the rest
+    B            "c 1:3 rwm", "c 1:5 r"                   all the rest
+
+when adding "c *:3 rwm":
+	# echo "c *:3 rwm" >A/devices.allow
+
+the result:
+    group        whitelist entries                        denied devices
+    A            "c *:3 rwm", "c 1:5 r"                   all the rest
+    B            "c 1:3 rwm", "c 1:5 r"                   all the rest
+
+but now it'll be possible to add new entries to B:
+	# echo "c 2:3 rwm" >B/devices.allow
+	# echo "c 50:3 r" >B/devices.allow
+or even
+	# echo "c *:3 rwm" >B/devices.allow
+
+Allowing or denying all by writing 'a' to devices.allow or devices.deny will
+not be possible once the device cgroups has children.
+
+4.1 Hierarchy (internal implementation)
+
+device cgroups is implemented internally using a behavior (ALLOW, DENY) and a
+list of exceptions.  The internal state is controlled using the same user
+interface to preserve compatibility with the previous whitelist-only
+implementation.  Removal or addition of exceptions that will reduce the access
+to devices will be propagated down the hierarchy.
+For every propagated exception, the effective rules will be re-evaluated based
+on current parent's access rules.
diff --git a/Documentation/cgroups/memory.txt b/Documentation/cgroups/memory.txt
index 8b8c28b9864..09027a9fece 100644
--- a/Documentation/cgroups/memory.txt
+++ b/Documentation/cgroups/memory.txt
@@ -40,6 +40,7 @@ Features:
  - soft limit
  - moving (recharging) account at moving a task is selectable.
  - usage threshold notifier
+ - memory pressure notifier
  - oom-killer disable knob and oom-notifier
  - Root cgroup has no limit controls.
 
@@ -65,6 +66,7 @@ Brief summary of control files.
  memory.stat			 # show various statistics
  memory.use_hierarchy		 # set/show hierarchical account enabled
  memory.force_empty		 # trigger forced move charge to parent
+ memory.pressure_level		 # set memory pressure notifications
  memory.swappiness		 # set/show swappiness parameter of vmscan
 				 (See sysctl's vm.swappiness)
  memory.move_charge_at_immigrate # set/show controls of moving charges
@@ -194,7 +196,7 @@ the cgroup that brought it in -- this will happen on memory pressure).
 But see section 8.2: when moving a task to another cgroup, its pages may
 be recharged to the new cgroup, if move_charge_at_immigrate has been chosen.
 
-Exception: If CONFIG_CGROUP_CGROUP_MEMCG_SWAP is not used.
+Exception: If CONFIG_MEMCG_SWAP is not used.
 When you do swapoff and make swapped-out pages of shmem(tmpfs) to
 be backed into memory in force, charges for pages are accounted against the
 caller of swapoff rather than the users of shmem.
@@ -762,7 +764,73 @@ At reading, current status of OOM is shown.
 	under_oom	 0 or 1 (if 1, the memory cgroup is under OOM, tasks may
 				 be stopped.)
 
-11. TODO
+11. Memory Pressure
+
+The pressure level notifications can be used to monitor the memory
+allocation cost; based on the pressure, applications can implement
+different strategies of managing their memory resources. The pressure
+levels are defined as following:
+
+The "low" level means that the system is reclaiming memory for new
+allocations. Monitoring this reclaiming activity might be useful for
+maintaining cache level. Upon notification, the program (typically
+"Activity Manager") might analyze vmstat and act in advance (i.e.
+prematurely shutdown unimportant services).
+
+The "medium" level means that the system is experiencing medium memory
+pressure, the system might be making swap, paging out active file caches,
+etc. Upon this event applications may decide to further analyze
+vmstat/zoneinfo/memcg or internal memory usage statistics and free any
+resources that can be easily reconstructed or re-read from a disk.
+
+The "critical" level means that the system is actively thrashing, it is
+about to out of memory (OOM) or even the in-kernel OOM killer is on its
+way to trigger. Applications should do whatever they can to help the
+system. It might be too late to consult with vmstat or any other
+statistics, so it's advisable to take an immediate action.
+
+The events are propagated upward until the event is handled, i.e. the
+events are not pass-through. Here is what this means: for example you have
+three cgroups: A->B->C. Now you set up an event listener on cgroups A, B
+and C, and suppose group C experiences some pressure. In this situation,
+only group C will receive the notification, i.e. groups A and B will not
+receive it. This is done to avoid excessive "broadcasting" of messages,
+which disturbs the system and which is especially bad if we are low on
+memory or thrashing. So, organize the cgroups wisely, or propagate the
+events manually (or, ask us to implement the pass-through events,
+explaining why would you need them.)
+
+The file memory.pressure_level is only used to setup an eventfd. To
+register a notification, an application must:
+
+- create an eventfd using eventfd(2);
+- open memory.pressure_level;
+- write string like "<event_fd> <fd of memory.pressure_level> <level>"
+  to cgroup.event_control.
+
+Application will be notified through eventfd when memory pressure is at
+the specific level (or higher). Read/write operations to
+memory.pressure_level are no implemented.
+
+Test:
+
+   Here is a small script example that makes a new cgroup, sets up a
+   memory limit, sets up a notification in the cgroup and then makes child
+   cgroup experience a critical pressure:
+
+   # cd /sys/fs/cgroup/memory/
+   # mkdir foo
+   # cd foo
+   # cgroup_event_listener memory.pressure_level low &
+   # echo 8000000 > memory.limit_in_bytes
+   # echo 8000000 > memory.memsw.limit_in_bytes
+   # echo $$ > tasks
+   # dd if=/dev/zero | read x
+
+   (Expect a bunch of notifications, and eventually, the oom-killer will
+   trigger.)
+
+12. TODO
 
 1. Add support for accounting huge pages (as a separate controller)
 2. Make per-cgroup scanner reclaim not-shared pages first
diff --git a/Documentation/clk.txt b/Documentation/clk.txt
index 1943fae014f..b9911c27f49 100644
--- a/Documentation/clk.txt
+++ b/Documentation/clk.txt
@@ -174,9 +174,9 @@ int clk_foo_enable(struct clk_hw *hw)
 };
 
 Below is a matrix detailing which clk_ops are mandatory based upon the
-hardware capbilities of that clock.  A cell marked as "y" means
+hardware capabilities of that clock.  A cell marked as "y" means
 mandatory, a cell marked as "n" implies that either including that
-callback is invalid or otherwise uneccesary.  Empty cells are either
+callback is invalid or otherwise unnecessary.  Empty cells are either
 optional or must be evaluated on a case-by-case basis.
 
                            clock hardware characteristics
@@ -231,3 +231,14 @@ To better enforce this policy, always follow this simple rule: any
 statically initialized clock data MUST be defined in a separate file
 from the logic that implements its ops.  Basically separate the logic
 from the data and all is well.
+
+	Part 6 - Disabling clock gating of unused clocks
+
+Sometimes during development it can be useful to be able to bypass the
+default disabling of unused clocks. For example, if drivers aren't enabling
+clocks properly but rely on them being on from the bootloader, bypassing
+the disabling means that the driver will remain functional while the issues
+are sorted out.
+
+To bypass this disabling, include "clk_ignore_unused" in the bootargs to the
+kernel.
diff --git a/Documentation/devicetree/bindings/arm/atmel-adc.txt b/Documentation/devicetree/bindings/arm/atmel-adc.txt
index c63097d6afe..16769d9cedd 100644
--- a/Documentation/devicetree/bindings/arm/atmel-adc.txt
+++ b/Documentation/devicetree/bindings/arm/atmel-adc.txt
@@ -14,9 +14,19 @@ Required properties:
   - atmel,adc-status-register: Offset of the Interrupt Status Register
   - atmel,adc-trigger-register: Offset of the Trigger Register
   - atmel,adc-vref: Reference voltage in millivolts for the conversions
+  - atmel,adc-res: List of resolution in bits supported by the ADC. List size
+		   must be two at least.
+  - atmel,adc-res-names: Contains one identifier string for each resolution
+			 in atmel,adc-res property. "lowres" and "highres"
+			 identifiers are required.
 
 Optional properties:
   - atmel,adc-use-external: Boolean to enable of external triggers
+  - atmel,adc-use-res: String corresponding to an identifier from
+		       atmel,adc-res-names property. If not specified, the highest
+		       resolution will be used.
+  - atmel,adc-sleep-mode: Boolean to enable sleep mode when no conversion
+  - atmel,adc-sample-hold-time: Sample and Hold Time in microseconds
  
 Optional trigger Nodes:
   - Required properties:
@@ -40,6 +50,9 @@ adc0: adc@fffb0000 {
 	atmel,adc-trigger-register = <0x08>;
 	atmel,adc-use-external;
 	atmel,adc-vref = <3300>;
+	atmel,adc-res = <8 10>;
+	atmel,adc-res-names = "lowres", "highres";
+	atmel,adc-use-res = "lowres";
 
 	trigger@0 {
 		trigger-name = "external-rising";
diff --git a/Documentation/devicetree/bindings/arm/msm/ssbi.txt b/Documentation/devicetree/bindings/arm/msm/ssbi.txt
new file mode 100644
index 00000000000..54fd5ced340
--- /dev/null
+++ b/Documentation/devicetree/bindings/arm/msm/ssbi.txt
@@ -0,0 +1,18 @@
+* Qualcomm SSBI
+
+Some Qualcomm MSM devices contain a point-to-point serial bus used to
+communicate with a limited range of devices (mostly power management
+chips).
+
+These require the following properties:
+
+- compatible: "qcom,ssbi"
+
+- qcom,controller-type
+  indicates the SSBI bus variant the controller should use to talk
+  with the slave device.  This should be one of "ssbi", "ssbi2", or
+  "pmic-arbiter".  The type chosen is determined by the attached
+  slave.
+
+The slave device should be the single child node of the ssbi device
+with a compatible field.
diff --git a/Documentation/devicetree/bindings/arm/samsung/exynos-adc.txt b/Documentation/devicetree/bindings/arm/samsung/exynos-adc.txt
new file mode 100644
index 00000000000..47ada1dff21
--- /dev/null
+++ b/Documentation/devicetree/bindings/arm/samsung/exynos-adc.txt
@@ -0,0 +1,60 @@
+Samsung Exynos Analog to Digital Converter bindings
+
+The devicetree bindings are for the new ADC driver written for
+Exynos4 and upward SoCs from Samsung.
+
+New driver handles the following
+1. Supports ADC IF found on EXYNOS4412/EXYNOS5250
+   and future SoCs from Samsung
+2. Add ADC driver under iio/adc framework
+3. Also adds the Documentation for device tree bindings
+
+Required properties:
+- compatible:		Must be "samsung,exynos-adc-v1"
+				for exynos4412/5250 controllers.
+			Must be "samsung,exynos-adc-v2" for
+				future controllers.
+- reg:			Contains ADC register address range (base address and
+			length) and the address of the phy enable register.
+- interrupts: 		Contains the interrupt information for the timer. The
+			format is being dependent on which interrupt controller
+			the Samsung device uses.
+- #io-channel-cells = <1>; As ADC has multiple outputs
+- clocks		From common clock binding: handle to adc clock.
+- clock-names		From common clock binding: Shall be "adc".
+- vdd-supply		VDD input supply.
+
+Note: child nodes can be added for auto probing from device tree.
+
+Example: adding device info in dtsi file
+
+adc: adc@12D10000 {
+	compatible = "samsung,exynos-adc-v1";
+	reg = <0x12D10000 0x100>, <0x10040718 0x4>;
+	interrupts = <0 106 0>;
+	#io-channel-cells = <1>;
+	io-channel-ranges;
+
+	clocks = <&clock 303>;
+	clock-names = "adc";
+
+	vdd-supply = <&buck5_reg>;
+};
+
+
+Example: Adding child nodes in dts file
+
+adc@12D10000 {
+
+	/* NTC thermistor is a hwmon device */
+	ncp15wb473@0 {
+		compatible = "ntc,ncp15wb473";
+		pullup-uV = <1800000>;
+		pullup-ohm = <47000>;
+		pulldown-ohm = <0>;
+		io-channels = <&adc 4>;
+	};
+};
+
+Note: Does not apply to ADC driver under arch/arm/plat-samsung/
+Note: The child node can be added under the adc node or separately.
diff --git a/Documentation/devicetree/bindings/clock/axi-clkgen.txt b/Documentation/devicetree/bindings/clock/axi-clkgen.txt
new file mode 100644
index 00000000000..028b493e97f
--- /dev/null
+++ b/Documentation/devicetree/bindings/clock/axi-clkgen.txt
@@ -0,0 +1,22 @@
+Binding for the axi-clkgen clock generator
+
+This binding uses the common clock binding[1].
+
+[1] Documentation/devicetree/bindings/clock/clock-bindings.txt
+
+Required properties:
+- compatible : shall be "adi,axi-clkgen".
+- #clock-cells : from common clock binding; Should always be set to 0.
+- reg : Address and length of the axi-clkgen register set.
+- clocks : Phandle and clock specifier for the parent clock.
+
+Optional properties:
+- clock-output-names : From common clock binding.
+
+Example:
+	clock@0xff000000 {
+		compatible = "adi,axi-clkgen";
+		#clock-cells = <0>;
+		reg = <0xff000000 0x1000>;
+		clocks = <&osc 1>;
+	};
diff --git a/Documentation/devicetree/bindings/clock/fixed-factor-clock.txt b/Documentation/devicetree/bindings/clock/fixed-factor-clock.txt
new file mode 100644
index 00000000000..5757f9abfc2
--- /dev/null
+++ b/Documentation/devicetree/bindings/clock/fixed-factor-clock.txt
@@ -0,0 +1,24 @@
+Binding for simple fixed factor rate clock sources.
+
+This binding uses the common clock binding[1].
+
+[1] Documentation/devicetree/bindings/clock/clock-bindings.txt
+
+Required properties:
+- compatible : shall be "fixed-factor-clock".
+- #clock-cells : from common clock binding; shall be set to 0.
+- clock-div: fixed divider.
+- clock-mult: fixed multiplier.
+- clocks: parent clock.
+
+Optional properties:
+- clock-output-names : From common clock binding.
+
+Example:
+	clock {
+		compatible = "fixed-factor-clock";
+		clocks = <&parentclk>;
+		#clock-cells = <0>;
+		div = <2>;
+		mult = <1>;
+	};
diff --git a/Documentation/devicetree/bindings/clock/silabs,si5351.txt b/Documentation/devicetree/bindings/clock/silabs,si5351.txt
new file mode 100644
index 00000000000..cc374651662
--- /dev/null
+++ b/Documentation/devicetree/bindings/clock/silabs,si5351.txt
@@ -0,0 +1,114 @@
+Binding for Silicon Labs Si5351a/b/c programmable i2c clock generator.
+
+Reference
+[1] Si5351A/B/C Data Sheet
+    http://www.silabs.com/Support%20Documents/TechnicalDocs/Si5351.pdf
+
+The Si5351a/b/c are programmable i2c clock generators with upto 8 output
+clocks. Si5351a also has a reduced pin-count package (MSOP10) where only
+3 output clocks are accessible. The internal structure of the clock
+generators can be found in [1].
+
+==I2C device node==
+
+Required properties:
+- compatible: shall be one of "silabs,si5351{a,a-msop,b,c}".
+- reg: i2c device address, shall be 0x60 or 0x61.
+- #clock-cells: from common clock binding; shall be set to 1.
+- clocks: from common clock binding; list of parent clock
+  handles, shall be xtal reference clock or xtal and clkin for
+  si5351c only.
+- #address-cells: shall be set to 1.
+- #size-cells: shall be set to 0.
+
+Optional properties:
+- silabs,pll-source: pair of (number, source) for each pll. Allows
+  to overwrite clock source of pll A (number=0) or B (number=1).
+
+==Child nodes==
+
+Each of the clock outputs can be overwritten individually by
+using a child node to the I2C device node. If a child node for a clock
+output is not set, the eeprom configuration is not overwritten.
+
+Required child node properties:
+- reg: number of clock output.
+
+Optional child node properties:
+- silabs,clock-source: source clock of the output divider stage N, shall be
+  0 = multisynth N
+  1 = multisynth 0 for output clocks 0-3, else multisynth4
+  2 = xtal
+  3 = clkin (si5351c only)
+- silabs,drive-strength: output drive strength in mA, shall be one of {2,4,6,8}.
+- silabs,multisynth-source: source pll A(0) or B(1) of corresponding multisynth
+  divider.
+- silabs,pll-master: boolean, multisynth can change pll frequency.
+
+==Example==
+
+/* 25MHz reference crystal */
+ref25: ref25M {
+	compatible = "fixed-clock";
+	#clock-cells = <0>;
+	clock-frequency = <25000000>;
+};
+
+i2c-master-node {
+
+	/* Si5351a msop10 i2c clock generator */
+	si5351a: clock-generator@60 {
+		compatible = "silabs,si5351a-msop";
+		reg = <0x60>;
+		#address-cells = <1>;
+		#size-cells = <0>;
+		#clock-cells = <1>;
+
+		/* connect xtal input to 25MHz reference */
+		clocks = <&ref25>;
+
+		/* connect xtal input as source of pll0 and pll1 */
+		silabs,pll-source = <0 0>, <1 0>;
+
+		/*
+		 * overwrite clkout0 configuration with:
+		 * - 8mA output drive strength
+		 * - pll0 as clock source of multisynth0
+		 * - multisynth0 as clock source of output divider
+		 * - multisynth0 can change pll0
+		 * - set initial clock frequency of 74.25MHz
+		 */
+		clkout0 {
+			reg = <0>;
+			silabs,drive-strength = <8>;
+			silabs,multisynth-source = <0>;
+			silabs,clock-source = <0>;
+			silabs,pll-master;
+			clock-frequency = <74250000>;
+		};
+
+		/*
+		 * overwrite clkout1 configuration with:
+		 * - 4mA output drive strength
+		 * - pll1 as clock source of multisynth1
+		 * - multisynth1 as clock source of output divider
+		 * - multisynth1 can change pll1
+		 */
+		clkout1 {
+			reg = <1>;
+			silabs,drive-strength = <4>;
+			silabs,multisynth-source = <1>;
+			silabs,clock-source = <0>;
+			pll-master;
+		};
+
+		/*
+		 * overwrite clkout2 configuration with:
+		 * - xtal as clock source of output divider
+		 */
+		clkout2 {
+			reg = <2>;
+			silabs,clock-source = <2>;
+		};
+	};
+};
diff --git a/Documentation/devicetree/bindings/clock/sunxi.txt b/Documentation/devicetree/bindings/clock/sunxi.txt
new file mode 100644
index 00000000000..729f52426fe
--- /dev/null
+++ b/Documentation/devicetree/bindings/clock/sunxi.txt
@@ -0,0 +1,151 @@
+Device Tree Clock bindings for arch-sunxi
+
+This binding uses the common clock binding[1].
+
+[1] Documentation/devicetree/bindings/clock/clock-bindings.txt
+
+Required properties:
+- compatible : shall be one of the following:
+	"allwinner,sun4i-osc-clk" - for a gatable oscillator
+	"allwinner,sun4i-pll1-clk" - for the main PLL clock
+	"allwinner,sun4i-cpu-clk" - for the CPU multiplexer clock
+	"allwinner,sun4i-axi-clk" - for the AXI clock
+	"allwinner,sun4i-axi-gates-clk" - for the AXI gates
+	"allwinner,sun4i-ahb-clk" - for the AHB clock
+	"allwinner,sun4i-ahb-gates-clk" - for the AHB gates
+	"allwinner,sun4i-apb0-clk" - for the APB0 clock
+	"allwinner,sun4i-apb0-gates-clk" - for the APB0 gates
+	"allwinner,sun4i-apb1-clk" - for the APB1 clock
+	"allwinner,sun4i-apb1-mux-clk" - for the APB1 clock muxing
+	"allwinner,sun4i-apb1-gates-clk" - for the APB1 gates
+
+Required properties for all clocks:
+- reg : shall be the control register address for the clock.
+- clocks : shall be the input parent clock(s) phandle for the clock
+- #clock-cells : from common clock binding; shall be set to 0 except for
+	"allwinner,sun4i-*-gates-clk" where it shall be set to 1
+
+Additionally, "allwinner,sun4i-*-gates-clk" clocks require:
+- clock-output-names : the corresponding gate names that the clock controls
+
+For example:
+
+osc24M: osc24M@01c20050 {
+	#clock-cells = <0>;
+	compatible = "allwinner,sun4i-osc-clk";
+	reg = <0x01c20050 0x4>;
+	clocks = <&osc24M_fixed>;
+};
+
+pll1: pll1@01c20000 {
+	#clock-cells = <0>;
+	compatible = "allwinner,sun4i-pll1-clk";
+	reg = <0x01c20000 0x4>;
+	clocks = <&osc24M>;
+};
+
+cpu: cpu@01c20054 {
+	#clock-cells = <0>;
+	compatible = "allwinner,sun4i-cpu-clk";
+	reg = <0x01c20054 0x4>;
+	clocks = <&osc32k>, <&osc24M>, <&pll1>;
+};
+
+
+
+Gate clock outputs
+
+The "allwinner,sun4i-*-gates-clk" clocks provide several gatable outputs;
+their corresponding offsets as present on sun4i are listed below. Note that
+some of these gates are not present on sun5i.
+
+  * AXI gates ("allwinner,sun4i-axi-gates-clk")
+
+    DRAM                                                                0
+
+  * AHB gates ("allwinner,sun4i-ahb-gates-clk")
+
+    USB0                                                                0
+    EHCI0                                                               1
+    OHCI0                                                               2*
+    EHCI1                                                               3
+    OHCI1                                                               4*
+    SS                                                                  5
+    DMA                                                                 6
+    BIST                                                                7
+    MMC0                                                                8
+    MMC1                                                                9
+    MMC2                                                                10
+    MMC3                                                                11
+    MS                                                                  12**
+    NAND                                                                13
+    SDRAM                                                               14
+
+    ACE                                                                 16
+    EMAC                                                                17
+    TS                                                                  18
+
+    SPI0                                                                20
+    SPI1                                                                21
+    SPI2                                                                22
+    SPI3                                                                23
+    PATA                                                                24
+    SATA                                                                25**
+    GPS                                                                 26*
+
+    VE                                                                  32
+    TVD                                                                 33
+    TVE0                                                                34
+    TVE1                                                                35
+    LCD0                                                                36
+    LCD1                                                                37
+
+    CSI0                                                                40
+    CSI1                                                                41
+
+    HDMI                                                                43
+    DE_BE0                                                              44
+    DE_BE1                                                              45
+    DE_FE0                                                              46
+    DE_FE1                                                              47
+
+    MP                                                                  50
+
+    MALI400                                                             52
+
+  * APB0 gates ("allwinner,sun4i-apb0-gates-clk")
+
+    CODEC                                                               0
+    SPDIF                                                               1*
+    AC97                                                                2
+    IIS                                                                 3
+
+    PIO                                                                 5
+    IR0                                                                 6
+    IR1                                                                 7
+
+    KEYPAD                                                              10
+
+  * APB1 gates ("allwinner,sun4i-apb1-gates-clk")
+
+    I2C0                                                                0
+    I2C1                                                                1
+    I2C2                                                                2
+
+    CAN                                                                 4
+    SCR                                                                 5
+    PS20                                                                6
+    PS21                                                                7
+
+    UART0                                                               16
+    UART1                                                               17
+    UART2                                                               18
+    UART3                                                               19
+    UART4                                                               20
+    UART5                                                               21
+    UART6                                                               22
+    UART7                                                               23
+
+Notation:
+ [*]:  The datasheet didn't mention these, but they are present on AW code
+ [**]: The datasheet had this marked as "NC" but they are used on AW code
diff --git a/Documentation/devicetree/bindings/gpio/gpio.txt b/Documentation/devicetree/bindings/gpio/gpio.txt
index a33628759d3..d933af37069 100644
--- a/Documentation/devicetree/bindings/gpio/gpio.txt
+++ b/Documentation/devicetree/bindings/gpio/gpio.txt
@@ -98,7 +98,7 @@ announce the pinrange to the pin ctrl subsystem. For example,
 		compatible = "fsl,qe-pario-bank-e", "fsl,qe-pario-bank";
 		reg = <0x1460 0x18>;
 		gpio-controller;
-		gpio-ranges = <&pinctrl1 20 10>, <&pinctrl2 50 20>;
+		gpio-ranges = <&pinctrl1 0 20 10>, <&pinctrl2 10 50 20>;
 
     }
 
@@ -107,8 +107,8 @@ where,
 
    Next values specify the base pin and number of pins for the range
    handled by 'qe_pio_e' gpio. In the given example from base pin 20 to
-   pin 29 under pinctrl1 and pin 50 to pin 69 under pinctrl2 is handled
-   by this gpio controller.
+   pin 29 under pinctrl1 with gpio offset 0 and pin 50 to pin 69 under
+   pinctrl2 with gpio offset 10 is handled by this gpio controller.
 
 The pinctrl node must have "#gpio-range-cells" property to show number of
 arguments to pass with phandle from gpio controllers node.
diff --git a/Documentation/devicetree/bindings/hwmon/ntc_thermistor.txt b/Documentation/devicetree/bindings/hwmon/ntc_thermistor.txt
new file mode 100644
index 00000000000..c6f66674f19
--- /dev/null
+++ b/Documentation/devicetree/bindings/hwmon/ntc_thermistor.txt
@@ -0,0 +1,29 @@
+NTC Thermistor hwmon sensors
+-------------------------------
+
+Requires node properties:
+- "compatible" value : one of
+	"ntc,ncp15wb473"
+	"ntc,ncp18wb473"
+	"ntc,ncp21wb473"
+	"ntc,ncp03wb473"
+	"ntc,ncp15wl333"
+- "pullup-uv"	Pull up voltage in micro volts
+- "pullup-ohm"	Pull up resistor value in ohms
+- "pulldown-ohm" Pull down resistor value in ohms
+- "connected-positive" Always ON, If not specified.
+		Status change is possible.
+- "io-channels"	Channel node of ADC to be used for
+		conversion.
+
+Read more about iio bindings at
+	Documentation/devicetree/bindings/iio/iio-bindings.txt
+
+Example:
+	ncp15wb473@0 {
+		compatible = "ntc,ncp15wb473";
+		pullup-uv = <1800000>;
+		pullup-ohm = <47000>;
+		pulldown-ohm = <0>;
+		io-channels = <&adc 3>;
+	};
diff --git a/Documentation/devicetree/bindings/i2c/i2c-s3c2410.txt b/Documentation/devicetree/bindings/i2c/i2c-s3c2410.txt
index f98d4c5b5cc..296eb453612 100644
--- a/Documentation/devicetree/bindings/i2c/i2c-s3c2410.txt
+++ b/Documentation/devicetree/bindings/i2c/i2c-s3c2410.txt
@@ -26,7 +26,7 @@ Required for all cases except "samsung,s3c2440-hdmiphy-i2c":
     - pinctrl-names: Should contain only one value - "default".
 
 Optional properties:
-  - samsung,i2c-slave-addr: Slave address in multi-master enviroment. If not
+  - samsung,i2c-slave-addr: Slave address in multi-master environment. If not
     specified, default value is 0.
   - samsung,i2c-max-bus-freq: Desired frequency in Hz of the bus. If not
     specified, the default value in Hz is 100000.
diff --git a/Documentation/devicetree/bindings/iio/iio-bindings.txt b/Documentation/devicetree/bindings/iio/iio-bindings.txt
new file mode 100644
index 00000000000..0b447d9ad19
--- /dev/null
+++ b/Documentation/devicetree/bindings/iio/iio-bindings.txt
@@ -0,0 +1,97 @@
+This binding is derived from clock bindings, and based on suggestions
+from Lars-Peter Clausen [1].
+
+Sources of IIO channels can be represented by any node in the device
+tree. Those nodes are designated as IIO providers. IIO consumer
+nodes use a phandle and IIO specifier pair to connect IIO provider
+outputs to IIO inputs. Similar to the gpio specifiers, an IIO
+specifier is an array of one or more cells identifying the IIO
+output on a device. The length of an IIO specifier is defined by the
+value of a #io-channel-cells property in the IIO provider node.
+
+[1] http://marc.info/?l=linux-iio&m=135902119507483&w=2
+
+==IIO providers==
+
+Required properties:
+#io-channel-cells: Number of cells in an IIO specifier; Typically 0 for nodes
+		   with a single IIO output and 1 for nodes with multiple
+		   IIO outputs.
+
+Example for a simple configuration with no trigger:
+
+	adc: voltage-sensor@35 {
+		compatible = "maxim,max1139";
+		reg = <0x35>;
+		#io-channel-cells = <1>;
+	};
+
+Example for a configuration with trigger:
+
+	adc@35 {
+		compatible = "some-vendor,some-adc";
+		reg = <0x35>;
+
+		adc1: iio-device@0 {
+			#io-channel-cells = <1>;
+			/* other properties */
+		};
+		adc2: iio-device@1 {
+			#io-channel-cells = <1>;
+			/* other properties */
+		};
+	};
+
+==IIO consumers==
+
+Required properties:
+io-channels:	List of phandle and IIO specifier pairs, one pair
+		for each IIO input to the device. Note: if the
+		IIO provider specifies '0' for #io-channel-cells,
+		then only the phandle portion of the pair will appear.
+
+Optional properties:
+io-channel-names:
+		List of IIO input name strings sorted in the same
+		order as the io-channels property. Consumers drivers
+		will use io-channel-names to match IIO input names
+		with IIO specifiers.
+io-channel-ranges:
+		Empty property indicating that child nodes can inherit named
+		IIO channels from this node. Useful for bus nodes to provide
+		and IIO channel to their children.
+
+For example:
+
+	device {
+		io-channels = <&adc 1>, <&ref 0>;
+		io-channel-names = "vcc", "vdd";
+	};
+
+This represents a device with two IIO inputs, named "vcc" and "vdd".
+The vcc channel is connected to output 1 of the &adc device, and the
+vdd channel is connected to output 0 of the &ref device.
+
+==Example==
+
+	adc: max1139@35 {
+		compatible = "maxim,max1139";
+		reg = <0x35>;
+		#io-channel-cells = <1>;
+	};
+
+	...
+
+	iio_hwmon {
+		compatible = "iio-hwmon";
+		io-channels = <&adc 0>, <&adc 1>, <&adc 2>,
+			<&adc 3>, <&adc 4>, <&adc 5>,
+			<&adc 6>, <&adc 7>, <&adc 8>,
+			<&adc 9>;
+	};
+
+	some_consumer {
+		compatible = "some-consumer";
+		io-channels = <&adc 10>, <&adc 11>;
+		io-channel-names = "adc1", "adc2";
+	};
diff --git a/Documentation/devicetree/bindings/leds/tca6507.txt b/Documentation/devicetree/bindings/leds/tca6507.txt
index 2b6693b972f..80ff3dfb1f3 100644
--- a/Documentation/devicetree/bindings/leds/tca6507.txt
+++ b/Documentation/devicetree/bindings/leds/tca6507.txt
@@ -1,4 +1,4 @@
-LEDs conected to tca6507
+LEDs connected to tca6507
 
 Required properties:
 - compatible : should be : "ti,tca6507".
diff --git a/Documentation/devicetree/bindings/media/coda.txt b/Documentation/devicetree/bindings/media/coda.txt
new file mode 100644
index 00000000000..2865d04e403
--- /dev/null
+++ b/Documentation/devicetree/bindings/media/coda.txt
@@ -0,0 +1,30 @@
+Chips&Media Coda multi-standard codec IP
+========================================
+
+Coda codec IPs are present in i.MX SoCs in various versions,
+called VPU (Video Processing Unit).
+
+Required properties:
+- compatible : should be "fsl,<chip>-src" for i.MX SoCs:
+  (a) "fsl,imx27-vpu" for CodaDx6 present in i.MX27
+  (b) "fsl,imx53-vpu" for CODA7541 present in i.MX53
+  (c) "fsl,imx6q-vpu" for CODA960 present in i.MX6q
+- reg: should be register base and length as documented in the
+  SoC reference manual
+- interrupts : Should contain the VPU interrupt. For CODA960,
+  a second interrupt is needed for the MJPEG unit.
+- clocks : Should contain the ahb and per clocks, in the order
+  determined by the clock-names property.
+- clock-names : Should be "ahb", "per"
+- iram : phandle pointing to the SRAM device node
+
+Example:
+
+vpu: vpu@63ff4000 {
+	compatible = "fsl,imx53-vpu";
+	reg = <0x63ff4000 0x1000>;
+	interrupts = <9>;
+	clocks = <&clks 63>, <&clks 63>;
+	clock-names = "ahb", "per";
+	iram = <&ocram>;
+};
diff --git a/Documentation/devicetree/bindings/media/exynos-fimc-lite.txt b/Documentation/devicetree/bindings/media/exynos-fimc-lite.txt
new file mode 100644
index 00000000000..3f62adfb3e0
--- /dev/null
+++ b/Documentation/devicetree/bindings/media/exynos-fimc-lite.txt
@@ -0,0 +1,14 @@
+Exynos4x12/Exynos5 SoC series camera host interface (FIMC-LITE)
+
+Required properties:
+
+- compatible	: should be "samsung,exynos4212-fimc" for Exynos4212 and
+		  Exynos4412 SoCs;
+- reg		: physical base address and size of the device memory mapped
+		  registers;
+- interrupts	: should contain FIMC-LITE interrupt;
+- clocks	: FIMC LITE gate clock should be specified in this property.
+- clock-names	: should contain "flite" entry.
+
+Each FIMC device should have an alias in the aliases node, in the form of
+fimc-lite<n>, where <n> is an integer specifying the IP block instance.
diff --git a/Documentation/devicetree/bindings/media/exynos4-fimc-is.txt b/Documentation/devicetree/bindings/media/exynos4-fimc-is.txt
new file mode 100644
index 00000000000..55c9ad6f959
--- /dev/null
+++ b/Documentation/devicetree/bindings/media/exynos4-fimc-is.txt
@@ -0,0 +1,49 @@
+Exynos4x12 SoC series Imaging Subsystem (FIMC-IS)
+
+The FIMC-IS is a subsystem for processing image signal from an image sensor.
+The Exynos4x12 SoC series FIMC-IS V1.5 comprises of a dedicated ARM Cortex-A5
+processor, ISP, DRC and FD IP blocks and peripheral devices such as UART, I2C
+and SPI bus controllers, PWM and ADC.
+
+fimc-is node
+------------
+
+Required properties:
+- compatible	: should be "samsung,exynos4212-fimc-is" for Exynos4212 and
+		  Exynos4412 SoCs;
+- reg		: physical base address and length of the registers set;
+- interrupts	: must contain two FIMC-IS interrupts, in order: ISP0, ISP1;
+- clocks	: list of clock specifiers, corresponding to entries in
+		  clock-names property;
+- clock-names	: must contain "ppmuispx", "ppmuispx", "lite0", "lite1"
+		  "mpll", "sysreg", "isp", "drc", "fd", "mcuisp", "uart",
+		  "ispdiv0", "ispdiv1", "mcuispdiv0", "mcuispdiv1", "aclk200",
+		  "div_aclk200", "aclk400mcuisp", "div_aclk400mcuisp" entries,
+		  matching entries in the clocks property.
+pmu subnode
+-----------
+
+Required properties:
+ - reg : must contain PMU physical base address and size of the register set.
+
+The following are the FIMC-IS peripheral device nodes and can be specified
+either standalone or as the fimc-is node child nodes.
+
+i2c-isp (ISP I2C bus controller) nodes
+------------------------------------------
+
+Required properties:
+
+- compatible	: should be "samsung,exynos4212-i2c-isp" for Exynos4212 and
+		  Exynos4412 SoCs;
+- reg		: physical base address and length of the registers set;
+- clocks	: must contain gate clock specifier for this controller;
+- clock-names	: must contain "i2c_isp" entry.
+
+For the above nodes it is required to specify a pinctrl state named "default",
+according to the pinctrl bindings defined in ../pinctrl/pinctrl-bindings.txt.
+
+Device tree nodes of the image sensors' controlled directly by the FIMC-IS
+firmware must be child nodes of their corresponding ISP I2C bus controller node.
+The data link of these image sensors must be specified using the common video
+interfaces bindings, defined in video-interfaces.txt.
diff --git a/Documentation/devicetree/bindings/media/samsung-fimc.txt b/Documentation/devicetree/bindings/media/samsung-fimc.txt
new file mode 100644
index 00000000000..51c776b7f7a
--- /dev/null
+++ b/Documentation/devicetree/bindings/media/samsung-fimc.txt
@@ -0,0 +1,197 @@
+Samsung S5P/EXYNOS SoC Camera Subsystem (FIMC)
+----------------------------------------------
+
+The S5P/Exynos SoC Camera subsystem comprises of multiple sub-devices
+represented by separate device tree nodes. Currently this includes: FIMC (in
+the S5P SoCs series known as CAMIF), MIPI CSIS, FIMC-LITE and FIMC-IS (ISP).
+
+The sub-subdevices are defined as child nodes of the common 'camera' node which
+also includes common properties of the whole subsystem not really specific to
+any single sub-device, like common camera port pins or the CAMCLK clock outputs
+for external image sensors attached to an SoC.
+
+Common 'camera' node
+--------------------
+
+Required properties:
+
+- compatible	: must be "samsung,fimc", "simple-bus"
+- clocks	: list of clock specifiers, corresponding to entries in
+		  the clock-names property;
+- clock-names	: must contain "sclk_cam0", "sclk_cam1", "pxl_async0",
+		  "pxl_async1" entries, matching entries in the clocks property.
+
+The pinctrl bindings defined in ../pinctrl/pinctrl-bindings.txt must be used
+to define a required pinctrl state named "default" and optional pinctrl states:
+"idle", "active-a", active-b". These optional states can be used to switch the
+camera port pinmux at runtime. The "idle" state should configure both the camera
+ports A and B into high impedance state, especially the CAMCLK clock output
+should be inactive. For the "active-a" state the camera port A must be activated
+and the port B deactivated and for the state "active-b" it should be the other
+way around.
+
+The 'camera' node must include at least one 'fimc' child node.
+
+'fimc' device nodes
+-------------------
+
+Required properties:
+
+- compatible: "samsung,s5pv210-fimc" for S5PV210, "samsung,exynos4210-fimc"
+  for Exynos4210 and "samsung,exynos4212-fimc" for Exynos4x12 SoCs;
+- reg: physical base address and length of the registers set for the device;
+- interrupts: should contain FIMC interrupt;
+- clocks: list of clock specifiers, must contain an entry for each required
+  entry in clock-names;
+- clock-names: must contain "fimc", "sclk_fimc" entries.
+- samsung,pix-limits: an array of maximum supported image sizes in pixels, for
+  details refer to Table 2-1 in the S5PV210 SoC User Manual; The meaning of
+  each cell is as follows:
+  0 - scaler input horizontal size,
+  1 - input horizontal size for the scaler bypassed,
+  2 - REAL_WIDTH without input rotation,
+  3 - REAL_HEIGHT with input rotation,
+- samsung,sysreg: a phandle to the SYSREG node.
+
+Each FIMC device should have an alias in the aliases node, in the form of
+fimc<n>, where <n> is an integer specifying the IP block instance.
+
+Optional properties:
+
+- clock-frequency: maximum FIMC local clock (LCLK) frequency;
+- samsung,min-pix-sizes: an array specyfing minimum image size in pixels at
+  the FIMC input and output DMA, in the first and second cell respectively.
+  Default value when this property is not present is <16 16>;
+- samsung,min-pix-alignment: minimum supported image height alignment (first
+  cell) and the horizontal image offset (second cell). The values are in pixels
+  and default to <2 1> when this property is not present;
+- samsung,mainscaler-ext: a boolean property indicating whether the FIMC IP
+  supports extended image size and has CIEXTEN register;
+- samsung,rotators: a bitmask specifying whether this IP has the input and
+  the output rotator. Bits 4 and 0 correspond to input and output rotator
+  respectively. If a rotator is present its corresponding bit should be set.
+  Default value when this property is not specified is 0x11.
+- samsung,cam-if: a bolean property indicating whether the IP block includes
+  the camera input interface.
+- samsung,isp-wb: this property must be present if the IP block has the ISP
+  writeback input.
+- samsung,lcd-wb: this property must be present if the IP block has the LCD
+  writeback input.
+
+
+'parallel-ports' node
+---------------------
+
+This node should contain child 'port' nodes specifying active parallel video
+input ports. It includes camera A and camera B inputs. 'reg' property in the
+port nodes specifies data input - 0, 1 indicates input A, B respectively.
+
+Optional properties
+
+- samsung,camclk-out : specifies clock output for remote sensor,
+		       0 - CAM_A_CLKOUT, 1 - CAM_B_CLKOUT;
+
+Image sensor nodes
+------------------
+
+The sensor device nodes should be added to their control bus controller (e.g.
+I2C0) nodes and linked to a port node in the csis or the parallel-ports node,
+using the common video interfaces bindings, defined in video-interfaces.txt.
+The implementation of this bindings requires clock-frequency property to be
+present in the sensor device nodes.
+
+Example:
+
+	aliases {
+		fimc0 = &fimc_0;
+	};
+
+	/* Parallel bus IF sensor */
+	i2c_0: i2c@13860000 {
+		s5k6aa: sensor@3c {
+			compatible = "samsung,s5k6aafx";
+			reg = <0x3c>;
+			vddio-supply = <...>;
+
+			clock-frequency = <24000000>;
+			clocks = <...>;
+			clock-names = "mclk";
+
+			port {
+				s5k6aa_ep: endpoint {
+					remote-endpoint = <&fimc0_ep>;
+					bus-width = <8>;
+					hsync-active = <0>;
+					vsync-active = <1>;
+					pclk-sample = <1>;
+				};
+			};
+		};
+	};
+
+	/* MIPI CSI-2 bus IF sensor */
+	s5c73m3: sensor@0x1a {
+		compatible = "samsung,s5c73m3";
+		reg = <0x1a>;
+		vddio-supply = <...>;
+
+		clock-frequency = <24000000>;
+		clocks = <...>;
+		clock-names = "mclk";
+
+		port {
+			s5c73m3_1: endpoint {
+				data-lanes = <1 2 3 4>;
+				remote-endpoint = <&csis0_ep>;
+			};
+		};
+	};
+
+	camera {
+		compatible = "samsung,fimc", "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <1>;
+		status = "okay";
+
+		pinctrl-names = "default";
+		pinctrl-0 = <&cam_port_a_clk_active>;
+
+		/* parallel camera ports */
+		parallel-ports {
+			/* camera A input */
+			port@0 {
+				reg = <0>;
+				fimc0_ep: endpoint {
+					remote-endpoint = <&s5k6aa_ep>;
+					bus-width = <8>;
+					hsync-active = <0>;
+					vsync-active = <1>;
+					pclk-sample = <1>;
+				};
+			};
+		};
+
+		fimc_0: fimc@11800000 {
+			compatible = "samsung,exynos4210-fimc";
+			reg = <0x11800000 0x1000>;
+			interrupts = <0 85 0>;
+			status = "okay";
+		};
+
+		csis_0: csis@11880000 {
+			compatible = "samsung,exynos4210-csis";
+			reg = <0x11880000 0x1000>;
+			interrupts = <0 78 0>;
+			/* camera C input */
+			port@3 {
+				reg = <3>;
+				csis0_ep: endpoint {
+					remote-endpoint = <&s5c73m3_ep>;
+					data-lanes = <1 2 3 4>;
+					samsung,csis-hs-settle = <12>;
+				};
+			};
+		};
+	};
+
+The MIPI-CSIS device binding is defined in samsung-mipi-csis.txt.
diff --git a/Documentation/devicetree/bindings/media/samsung-mipi-csis.txt b/Documentation/devicetree/bindings/media/samsung-mipi-csis.txt
new file mode 100644
index 00000000000..5f8e28e2484
--- /dev/null
+++ b/Documentation/devicetree/bindings/media/samsung-mipi-csis.txt
@@ -0,0 +1,81 @@
+Samsung S5P/EXYNOS SoC series MIPI CSI-2 receiver (MIPI CSIS)
+-------------------------------------------------------------
+
+Required properties:
+
+- compatible	  : "samsung,s5pv210-csis" for S5PV210 (S5PC110),
+		    "samsung,exynos4210-csis" for Exynos4210 (S5PC210),
+		    "samsung,exynos4212-csis" for Exynos4212/Exynos4412
+		    SoC series;
+- reg		  : offset and length of the register set for the device;
+- interrupts      : should contain MIPI CSIS interrupt; the format of the
+		    interrupt specifier depends on the interrupt controller;
+- bus-width	  : maximum number of data lanes supported (SoC specific);
+- vddio-supply    : MIPI CSIS I/O and PLL voltage supply (e.g. 1.8V);
+- vddcore-supply  : MIPI CSIS Core voltage supply (e.g. 1.1V);
+- clocks	  : list of clock specifiers, corresponding to entries in
+		    clock-names property;
+- clock-names	  : must contain "csis", "sclk_csis" entries, matching entries
+		    in the clocks property.
+
+Optional properties:
+
+- clock-frequency : The IP's main (system bus) clock frequency in Hz, default
+		    value when this property is not specified is 166 MHz;
+- samsung,csis-wclk : CSI-2 wrapper clock selection. If this property is present
+		    external clock from CMU will be used, or the bus clock if
+		    if it's not specified.
+
+The device node should contain one 'port' child node with one child 'endpoint'
+node, according to the bindings defined in Documentation/devicetree/bindings/
+media/video-interfaces.txt. The following are properties specific to those nodes.
+
+port node
+---------
+
+- reg		  : (required) must be 3 for camera C input (CSIS0) or 4 for
+		    camera D input (CSIS1);
+
+endpoint node
+-------------
+
+- data-lanes	  : (required) an array specifying active physical MIPI-CSI2
+		    data input lanes and their mapping to logical lanes; the
+		    array's content is unused, only its length is meaningful;
+
+- samsung,csis-hs-settle : (optional) differential receiver (HS-RX) settle time;
+
+
+Example:
+
+	reg0: regulator@0 {
+	};
+
+	reg1: regulator@1 {
+	};
+
+/* SoC properties */
+
+	csis_0: csis@11880000 {
+		compatible = "samsung,exynos4210-csis";
+		reg = <0x11880000 0x1000>;
+		interrupts = <0 78 0>;
+		#address-cells = <1>;
+		#size-cells = <0>;
+	};
+
+/* Board properties */
+
+	csis_0: csis@11880000 {
+		clock-frequency = <166000000>;
+		vddio-supply = <&reg0>;
+		vddcore-supply = <&reg1>;
+		port {
+			reg = <3>; /* 3 - CSIS0, 4 - CSIS1 */
+			csis0_ep: endpoint {
+				remote-endpoint = <...>;
+				data-lanes = <1>, <2>;
+				samsung,csis-hs-settle = <12>;
+			};
+		};
+	};
diff --git a/Documentation/devicetree/bindings/media/video-interfaces.txt b/Documentation/devicetree/bindings/media/video-interfaces.txt
new file mode 100644
index 00000000000..e022d2dc496
--- /dev/null
+++ b/Documentation/devicetree/bindings/media/video-interfaces.txt
@@ -0,0 +1,228 @@
+Common bindings for video receiver and transmitter interfaces
+
+General concept
+---------------
+
+Video data pipelines usually consist of external devices, e.g. camera sensors,
+controlled over an I2C, SPI or UART bus, and SoC internal IP blocks, including
+video DMA engines and video data processors.
+
+SoC internal blocks are described by DT nodes, placed similarly to other SoC
+blocks.  External devices are represented as child nodes of their respective
+bus controller nodes, e.g. I2C.
+
+Data interfaces on all video devices are described by their child 'port' nodes.
+Configuration of a port depends on other devices participating in the data
+transfer and is described by 'endpoint' subnodes.
+
+device {
+	...
+	ports {
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		port@0 {
+			...
+			endpoint@0 { ... };
+			endpoint@1 { ... };
+		};
+		port@1 { ... };
+	};
+};
+
+If a port can be configured to work with more than one remote device on the same
+bus, an 'endpoint' child node must be provided for each of them.  If more than
+one port is present in a device node or there is more than one endpoint at a
+port, or port node needs to be associated with a selected hardware interface,
+a common scheme using '#address-cells', '#size-cells' and 'reg' properties is
+used.
+
+All 'port' nodes can be grouped under optional 'ports' node, which allows to
+specify #address-cells, #size-cells properties independently for the 'port'
+and 'endpoint' nodes and any child device nodes a device might have.
+
+Two 'endpoint' nodes are linked with each other through their 'remote-endpoint'
+phandles.  An endpoint subnode of a device contains all properties needed for
+configuration of this device for data exchange with other device.  In most
+cases properties at the peer 'endpoint' nodes will be identical, however they
+might need to be different when there is any signal modifications on the bus
+between two devices, e.g. there are logic signal inverters on the lines.
+
+It is allowed for multiple endpoints at a port to be active simultaneously,
+where supported by a device.  For example, in case where a data interface of
+a device is partitioned into multiple data busses, e.g. 16-bit input port
+divided into two separate ITU-R BT.656 8-bit busses.  In such case bus-width
+and data-shift properties can be used to assign physical data lines to each
+endpoint node (logical bus).
+
+
+Required properties
+-------------------
+
+If there is more than one 'port' or more than one 'endpoint' node or 'reg'
+property is present in port and/or endpoint nodes the following properties
+are required in a relevant parent node:
+
+ - #address-cells : number of cells required to define port/endpoint
+		    identifier, should be 1.
+ - #size-cells    : should be zero.
+
+Optional endpoint properties
+----------------------------
+
+- remote-endpoint: phandle to an 'endpoint' subnode of a remote device node.
+- slave-mode: a boolean property indicating that the link is run in slave mode.
+  The default when this property is not specified is master mode. In the slave
+  mode horizontal and vertical synchronization signals are provided to the
+  slave device (data source) by the master device (data sink). In the master
+  mode the data source device is also the source of the synchronization signals.
+- bus-width: number of data lines actively used, valid for the parallel busses.
+- data-shift: on the parallel data busses, if bus-width is used to specify the
+  number of data lines, data-shift can be used to specify which data lines are
+  used, e.g. "bus-width=<8>; data-shift=<2>;" means, that lines 9:2 are used.
+- hsync-active: active state of the HSYNC signal, 0/1 for LOW/HIGH respectively.
+- vsync-active: active state of the VSYNC signal, 0/1 for LOW/HIGH respectively.
+  Note, that if HSYNC and VSYNC polarities are not specified, embedded
+  synchronization may be required, where supported.
+- data-active: similar to HSYNC and VSYNC, specifies data line polarity.
+- field-even-active: field signal level during the even field data transmission.
+- pclk-sample: sample data on rising (1) or falling (0) edge of the pixel clock
+  signal.
+- data-lanes: an array of physical data lane indexes. Position of an entry
+  determines the logical lane number, while the value of an entry indicates
+  physical lane, e.g. for 2-lane MIPI CSI-2 bus we could have
+  "data-lanes = <1 2>;", assuming the clock lane is on hardware lane 0.
+  This property is valid for serial busses only (e.g. MIPI CSI-2).
+- clock-lanes: an array of physical clock lane indexes. Position of an entry
+  determines the logical lane number, while the value of an entry indicates
+  physical lane, e.g. for a MIPI CSI-2 bus we could have "clock-lanes = <0>;",
+  which places the clock lane on hardware lane 0. This property is valid for
+  serial busses only (e.g. MIPI CSI-2). Note that for the MIPI CSI-2 bus this
+  array contains only one entry.
+- clock-noncontinuous: a boolean property to allow MIPI CSI-2 non-continuous
+  clock mode.
+
+
+Example
+-------
+
+The example snippet below describes two data pipelines.  ov772x and imx074 are
+camera sensors with a parallel and serial (MIPI CSI-2) video bus respectively.
+Both sensors are on the I2C control bus corresponding to the i2c0 controller
+node.  ov772x sensor is linked directly to the ceu0 video host interface.
+imx074 is linked to ceu0 through the MIPI CSI-2 receiver (csi2). ceu0 has a
+(single) DMA engine writing captured data to memory.  ceu0 node has a single
+'port' node which may indicate that at any time only one of the following data
+pipelines can be active: ov772x -> ceu0 or imx074 -> csi2 -> ceu0.
+
+	ceu0: ceu@0xfe910000 {
+		compatible = "renesas,sh-mobile-ceu";
+		reg = <0xfe910000 0xa0>;
+		interrupts = <0x880>;
+
+		mclk: master_clock {
+			compatible = "renesas,ceu-clock";
+			#clock-cells = <1>;
+			clock-frequency = <50000000>;	/* Max clock frequency */
+			clock-output-names = "mclk";
+		};
+
+		port {
+			#address-cells = <1>;
+			#size-cells = <0>;
+
+			/* Parallel bus endpoint */
+			ceu0_1: endpoint@1 {
+				reg = <1>;		/* Local endpoint # */
+				remote = <&ov772x_1_1>;	/* Remote phandle */
+				bus-width = <8>;	/* Used data lines */
+				data-shift = <2>;	/* Lines 9:2 are used */
+
+				/* If hsync-active/vsync-active are missing,
+				   embedded BT.656 sync is used */
+				hsync-active = <0>;	/* Active low */
+				vsync-active = <0>;	/* Active low */
+				data-active = <1>;	/* Active high */
+				pclk-sample = <1>;	/* Rising */
+			};
+
+			/* MIPI CSI-2 bus endpoint */
+			ceu0_0: endpoint@0 {
+				reg = <0>;
+				remote = <&csi2_2>;
+			};
+		};
+	};
+
+	i2c0: i2c@0xfff20000 {
+		...
+		ov772x_1: camera@0x21 {
+			compatible = "omnivision,ov772x";
+			reg = <0x21>;
+			vddio-supply = <&regulator1>;
+			vddcore-supply = <&regulator2>;
+
+			clock-frequency = <20000000>;
+			clocks = <&mclk 0>;
+			clock-names = "xclk";
+
+			port {
+				/* With 1 endpoint per port no need for addresses. */
+				ov772x_1_1: endpoint {
+					bus-width = <8>;
+					remote-endpoint = <&ceu0_1>;
+					hsync-active = <1>;
+					vsync-active = <0>; /* Who came up with an
+							       inverter here ?... */
+					data-active = <1>;
+					pclk-sample = <1>;
+				};
+			};
+		};
+
+		imx074: camera@0x1a {
+			compatible = "sony,imx074";
+			reg = <0x1a>;
+			vddio-supply = <&regulator1>;
+			vddcore-supply = <&regulator2>;
+
+			clock-frequency = <30000000>;	/* Shared clock with ov772x_1 */
+			clocks = <&mclk 0>;
+			clock-names = "sysclk";		/* Assuming this is the
+							   name in the datasheet */
+			port {
+				imx074_1: endpoint {
+					clock-lanes = <0>;
+					data-lanes = <1 2>;
+					remote-endpoint = <&csi2_1>;
+				};
+			};
+		};
+	};
+
+	csi2: csi2@0xffc90000 {
+		compatible = "renesas,sh-mobile-csi2";
+		reg = <0xffc90000 0x1000>;
+		interrupts = <0x17a0>;
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		port@1 {
+			compatible = "renesas,csi2c";	/* One of CSI2I and CSI2C. */
+			reg = <1>;			/* CSI-2 PHY #1 of 2: PHY_S,
+							   PHY_M has port address 0,
+							   is unused. */
+			csi2_1: endpoint {
+				clock-lanes = <0>;
+				data-lanes = <2 1>;
+				remote-endpoint = <&imx074_1>;
+			};
+		};
+		port@2 {
+			reg = <2>;			/* port 2: link to the CEU */
+
+			csi2_2: endpoint {
+				remote-endpoint = <&ceu0_0>;
+			};
+		};
+	};
diff --git a/Documentation/devicetree/bindings/metag/meta-intc.txt b/Documentation/devicetree/bindings/metag/meta-intc.txt
index 8c47dcbfabc..80994adab39 100644
--- a/Documentation/devicetree/bindings/metag/meta-intc.txt
+++ b/Documentation/devicetree/bindings/metag/meta-intc.txt
@@ -12,7 +12,7 @@ Required properties:
       handle 32 interrupt sources).
 
     - interrupt-controller: The presence of this property identifies the node
-      as an interupt controller. No property value shall be defined.
+      as an interrupt controller. No property value shall be defined.
 
     - #interrupt-cells: Specifies the number of cells needed to encode an
       interrupt source. The type shall be a <u32> and the value shall be 2.
diff --git a/Documentation/devicetree/bindings/mfd/mc13xxx.txt b/Documentation/devicetree/bindings/mfd/mc13xxx.txt
index baf07987ae6..abd9e3cb2db 100644
--- a/Documentation/devicetree/bindings/mfd/mc13xxx.txt
+++ b/Documentation/devicetree/bindings/mfd/mc13xxx.txt
@@ -10,10 +10,40 @@ Optional properties:
 - fsl,mc13xxx-uses-touch : Indicate the touchscreen controller is being used
 
 Sub-nodes:
-- regulators : Contain the regulator nodes.  The MC13892 regulators are
-  bound using their names as listed below with their registers and bits
-  for enabling.
+- regulators : Contain the regulator nodes. The regulators are bound using
+  their names as listed below with their registers and bits for enabling.
 
+MC13783 regulators:
+    sw1a      : regulator SW1A      (register 24, bit 0)
+    sw1b      : regulator SW1B      (register 25, bit 0)
+    sw2a      : regulator SW2A      (register 26, bit 0)
+    sw2b      : regulator SW2B      (register 27, bit 0)
+    sw3       : regulator SW3       (register 29, bit 20)
+    vaudio    : regulator VAUDIO    (register 32, bit 0)
+    viohi     : regulator VIOHI     (register 32, bit 3)
+    violo     : regulator VIOLO     (register 32, bit 6)
+    vdig      : regulator VDIG      (register 32, bit 9)
+    vgen      : regulator VGEN      (register 32, bit 12)
+    vrfdig    : regulator VRFDIG    (register 32, bit 15)
+    vrfref    : regulator VRFREF    (register 32, bit 18)
+    vrfcp     : regulator VRFCP     (register 32, bit 21)
+    vsim      : regulator VSIM      (register 33, bit 0)
+    vesim     : regulator VESIM     (register 33, bit 3)
+    vcam      : regulator VCAM      (register 33, bit 6)
+    vrfbg     : regulator VRFBG     (register 33, bit 9)
+    vvib      : regulator VVIB      (register 33, bit 11)
+    vrf1      : regulator VRF1      (register 33, bit 12)
+    vrf2      : regulator VRF2      (register 33, bit 15)
+    vmmc1     : regulator VMMC1     (register 33, bit 18)
+    vmmc2     : regulator VMMC2     (register 33, bit 21)
+    gpo1      : regulator GPO1      (register 34, bit 6)
+    gpo2      : regulator GPO2      (register 34, bit 8)
+    gpo3      : regulator GPO3      (register 34, bit 10)
+    gpo4      : regulator GPO4      (register 34, bit 12)
+    pwgt1spi  : regulator PWGT1SPI  (register 34, bit 15)
+    pwgt2spi  : regulator PWGT2SPI  (register 34, bit 16)
+
+MC13892 regulators:
     vcoincell : regulator VCOINCELL (register 13, bit 23)
     sw1       : regulator SW1	    (register 24, bit 0)
     sw2       : regulator SW2	    (register 25, bit 0)
diff --git a/Documentation/devicetree/bindings/misc/sram.txt b/Documentation/devicetree/bindings/misc/sram.txt
new file mode 100644
index 00000000000..4d0a00e453a
--- /dev/null
+++ b/Documentation/devicetree/bindings/misc/sram.txt
@@ -0,0 +1,16 @@
+Generic on-chip SRAM
+
+Simple IO memory regions to be managed by the genalloc API.
+
+Required properties:
+
+- compatible : mmio-sram
+
+- reg : SRAM iomem address range
+
+Example:
+
+sram: sram@5c000000 {
+	compatible = "mmio-sram";
+	reg = <0x5c000000 0x40000>; /* 256 KiB SRAM at address 0x5c000000 */
+};
diff --git a/Documentation/devicetree/bindings/pinctrl/atmel,at91-pinctrl.txt b/Documentation/devicetree/bindings/pinctrl/atmel,at91-pinctrl.txt
index bc50899e0c8..648d60eb9fd 100644
--- a/Documentation/devicetree/bindings/pinctrl/atmel,at91-pinctrl.txt
+++ b/Documentation/devicetree/bindings/pinctrl/atmel,at91-pinctrl.txt
@@ -1,6 +1,6 @@
 * Atmel AT91 Pinmux Controller
 
-The AT91 Pinmux Controler, enables the IC
+The AT91 Pinmux Controller, enables the IC
 to share one PAD to several functional blocks. The sharing is done by
 multiplexing the PAD input/output signals. For each PAD there are up to
 8 muxing options (called periph modes). Since different modules require
diff --git a/Documentation/devicetree/bindings/pinctrl/brcm,bcm2835-gpio.txt b/Documentation/devicetree/bindings/pinctrl/brcm,bcm2835-gpio.txt
index 8edc20e1b09..2569866c692 100644
--- a/Documentation/devicetree/bindings/pinctrl/brcm,bcm2835-gpio.txt
+++ b/Documentation/devicetree/bindings/pinctrl/brcm,bcm2835-gpio.txt
@@ -5,7 +5,7 @@ controller, and pinmux/control device.
 
 Required properties:
 - compatible: "brcm,bcm2835-gpio"
-- reg: Should contain the physical address of the GPIO module's registes.
+- reg: Should contain the physical address of the GPIO module's registers.
 - gpio-controller: Marks the device node as a GPIO controller.
 - #gpio-cells : Should be two. The first cell is the pin number and the
   second cell is used to specify optional parameters:
diff --git a/Documentation/devicetree/bindings/pinctrl/pinctrl-single.txt b/Documentation/devicetree/bindings/pinctrl/pinctrl-single.txt
index 2c81e45f137..08f0c3d0157 100644
--- a/Documentation/devicetree/bindings/pinctrl/pinctrl-single.txt
+++ b/Documentation/devicetree/bindings/pinctrl/pinctrl-single.txt
@@ -1,7 +1,9 @@
 One-register-per-pin type device tree based pinctrl driver
 
 Required properties:
-- compatible : "pinctrl-single"
+- compatible : "pinctrl-single" or "pinconf-single".
+  "pinctrl-single" means that pinconf isn't supported.
+  "pinconf-single" means that generic pinconf is supported.
 
 - reg : offset and length of the register set for the mux registers
 
@@ -14,9 +16,61 @@ Optional properties:
 - pinctrl-single,function-off : function off mode for disabled state if
   available and same for all registers; if not specified, disabling of
   pin functions is ignored
+
 - pinctrl-single,bit-per-mux : boolean to indicate that one register controls
   more than one pin
 
+- pinctrl-single,drive-strength : array of value that are used to configure
+  drive strength in the pinmux register. They're value of drive strength
+  current and drive strength mask.
+
+		/* drive strength current, mask */
+		pinctrl-single,power-source = <0x30 0xf0>;
+
+- pinctrl-single,bias-pullup : array of value that are used to configure the
+  input bias pullup in the pinmux register.
+
+		/* input, enabled pullup bits, disabled pullup bits, mask */
+		pinctrl-single,bias-pullup = <0 1 0 1>;
+
+- pinctrl-single,bias-pulldown : array of value that are used to configure the
+  input bias pulldown in the pinmux register.
+
+		/* input, enabled pulldown bits, disabled pulldown bits, mask */
+		pinctrl-single,bias-pulldown = <2 2 0 2>;
+
+  * Two bits to control input bias pullup and pulldown: User should use
+    pinctrl-single,bias-pullup & pinctrl-single,bias-pulldown. One bit means
+    pullup, and the other one bit means pulldown.
+  * Three bits to control input bias enable, pullup and pulldown. User should
+    use pinctrl-single,bias-pullup & pinctrl-single,bias-pulldown. Input bias
+    enable bit should be included in pullup or pulldown bits.
+  * Although driver could set PIN_CONFIG_BIAS_DISABLE, there's no property as
+    pinctrl-single,bias-disable. Because pinctrl single driver could implement
+    it by calling pulldown, pullup disabled.
+
+- pinctrl-single,input-schmitt : array of value that are used to configure
+  input schmitt in the pinmux register. In some silicons, there're two input
+  schmitt value (rising-edge & falling-edge) in the pinmux register.
+
+		/* input schmitt value, mask */
+		pinctrl-single,input-schmitt = <0x30 0x70>;
+
+- pinctrl-single,input-schmitt-enable : array of value that are used to
+  configure input schmitt enable or disable in the pinmux register.
+
+		/* input, enable bits, disable bits, mask */
+		pinctrl-single,input-schmitt-enable = <0x30 0x40 0 0x70>;
+
+- pinctrl-single,gpio-range : list of value that are used to configure a GPIO
+  range. They're value of subnode phandle, pin base in pinctrl device, pin
+  number in this range, GPIO function value of this GPIO range.
+  The number of parameters is depend on #pinctrl-single,gpio-range-cells
+  property.
+
+		/* pin base, nr pins & gpio function */
+		pinctrl-single,gpio-range = <&range 0 3 0 &range 3 9 1>;
+
 This driver assumes that there is only one register for each pin (unless the
 pinctrl-single,bit-per-mux is set), and uses the common pinctrl bindings as
 specified in the pinctrl-bindings.txt document in this directory.
@@ -42,6 +96,20 @@ Where 0xdc is the offset from the pinctrl register base address for the
 device pinctrl register, 0x18 is the desired value, and 0xff is the sub mask to
 be used when applying this change to the register.
 
+
+Optional sub-node: In case some pins could be configured as GPIO in the pinmux
+register, those pins could be defined as a GPIO range. This sub-node is required
+by pinctrl-single,gpio-range property.
+
+Required properties in sub-node:
+- #pinctrl-single,gpio-range-cells : the number of parameters after phandle in
+  pinctrl-single,gpio-range property.
+
+	range: gpio-range {
+		#pinctrl-single,gpio-range-cells = <3>;
+	};
+
+
 Example:
 
 /* SoC common file */
@@ -58,7 +126,7 @@ pmx_core: pinmux@4a100040 {
 
 /* second controller instance for pins in wkup domain */
 pmx_wkup: pinmux@4a31e040 {
-	compatible = "pinctrl-single;
+	compatible = "pinctrl-single";
 	reg = <0x4a31e040 0x0038>;
 	#address-cells = <1>;
 	#size-cells = <0>;
@@ -76,6 +144,29 @@ control_devconf0: pinmux@48002274 {
 	pinctrl-single,function-mask = <0x5F>;
 };
 
+/* third controller instance for pins in gpio domain */
+pmx_gpio: pinmux@d401e000 {
+	compatible = "pinconf-single";
+	reg = <0xd401e000 0x0330>;
+	#address-cells = <1>;
+	#size-cells = <1>;
+	ranges;
+
+	pinctrl-single,register-width = <32>;
+	pinctrl-single,function-mask = <7>;
+
+	/* sparse GPIO range could be supported */
+	pinctrl-single,gpio-range = <&range 0 3 0 &range 3 9 1
+				&range 12 1 0 &range 13 29 1
+				&range 43 1 0 &range 44 49 1
+				&range 94 1 1 &range 96 2 1>;
+
+	range: gpio-range {
+		#pinctrl-single,gpio-range-cells = <3>;
+	};
+};
+
+
 /* board specific .dts file */
 
 &pmx_core {
@@ -96,6 +187,15 @@ control_devconf0: pinmux@48002274 {
 		>;
 	};
 
+	uart0_pins: pinmux_uart0_pins {
+		pinctrl-single,pins = <
+			0x208 0		/* UART0_RXD (IOCFG138) */
+			0x20c 0		/* UART0_TXD (IOCFG139) */
+		>;
+		pinctrl-single,bias-pulldown = <0 2 2>;
+		pinctrl-single,bias-pullup = <0 1 1>;
+	};
+
 	/* map uart2 pins */
 	uart2_pins: pinmux_uart2_pins {
 		pinctrl-single,pins = <
@@ -122,6 +222,11 @@ control_devconf0: pinmux@48002274 {
 
 };
 
+&uart1 {
+       pinctrl-names = "default";
+       pinctrl-0 = <&uart0_pins>;
+};
+
 &uart2 {
        pinctrl-names = "default";
        pinctrl-0 = <&uart2_pins>;
diff --git a/Documentation/devicetree/bindings/pinctrl/samsung-pinctrl.txt b/Documentation/devicetree/bindings/pinctrl/samsung-pinctrl.txt
index 4598a47aa0c..c70fca146e9 100644
--- a/Documentation/devicetree/bindings/pinctrl/samsung-pinctrl.txt
+++ b/Documentation/devicetree/bindings/pinctrl/samsung-pinctrl.txt
@@ -7,6 +7,7 @@ on-chip controllers onto these pads.
 
 Required Properties:
 - compatible: should be one of the following.
+  - "samsung,s3c64xx-pinctrl": for S3C64xx-compatible pin-controller,
   - "samsung,exynos4210-pinctrl": for Exynos4210 compatible pin-controller.
   - "samsung,exynos4x12-pinctrl": for Exynos4x12 compatible pin-controller.
   - "samsung,exynos5250-pinctrl": for Exynos5250 compatible pin-controller.
@@ -105,6 +106,8 @@ B. External Wakeup Interrupts: For supporting external wakeup interrupts, a
 
    - compatible: identifies the type of the external wakeup interrupt controller
      The possible values are:
+     - samsung,s3c64xx-wakeup-eint: represents wakeup interrupt controller
+       found on Samsung S3C64xx SoCs,
      - samsung,exynos4210-wakeup-eint: represents wakeup interrupt controller
        found on Samsung Exynos4210 SoC.
    - interrupt-parent: phandle of the interrupt parent to which the external
diff --git a/Documentation/devicetree/bindings/power_supply/qnap-poweroff.txt b/Documentation/devicetree/bindings/power_supply/qnap-poweroff.txt
index 9a599d27bd7..0347d8350d9 100644
--- a/Documentation/devicetree/bindings/power_supply/qnap-poweroff.txt
+++ b/Documentation/devicetree/bindings/power_supply/qnap-poweroff.txt
@@ -2,7 +2,7 @@
 
 QNAP NAS devices have a microcontroller controlling the main power
 supply. This microcontroller is connected to UART1 of the Kirkwood and
-Orion5x SoCs. Sending the charactor 'A', at 19200 baud, tells the
+Orion5x SoCs. Sending the character 'A', at 19200 baud, tells the
 microcontroller to turn the power off. This driver adds a handler to
 pm_power_off which is called to turn the power off.
 
diff --git a/Documentation/devicetree/bindings/regulator/max8952.txt b/Documentation/devicetree/bindings/regulator/max8952.txt
new file mode 100644
index 00000000000..866fcdd0f4e
--- /dev/null
+++ b/Documentation/devicetree/bindings/regulator/max8952.txt
@@ -0,0 +1,52 @@
+Maxim MAX8952 voltage regulator
+
+Required properties:
+- compatible: must be equal to "maxim,max8952"
+- reg: I2C slave address, usually 0x60
+- max8952,dvs-mode-microvolt: array of 4 integer values defining DVS voltages
+  in microvolts. All values must be from range <770000, 1400000>
+- any required generic properties defined in regulator.txt
+
+Optional properties:
+- max8952,vid-gpios: array of two GPIO pins used for DVS voltage selection
+- max8952,en-gpio: GPIO used to control enable status of regulator
+- max8952,default-mode: index of default DVS voltage, from <0, 3> range
+- max8952,sync-freq: sync frequency, must be one of following values:
+    - 0: 26 MHz
+    - 1: 13 MHz
+    - 2: 19.2 MHz
+  Defaults to 26 MHz if not specified.
+- max8952,ramp-speed: voltage ramp speed, must be one of following values:
+    - 0: 32mV/us
+    - 1: 16mV/us
+    - 2: 8mV/us
+    - 3: 4mV/us
+    - 4: 2mV/us
+    - 5: 1mV/us
+    - 6: 0.5mV/us
+    - 7: 0.25mV/us
+  Defaults to 32mV/us if not specified.
+- any available generic properties defined in regulator.txt
+
+Example:
+
+	vdd_arm_reg: pmic@60 {
+		compatible = "maxim,max8952";
+		reg = <0x60>;
+
+		/* max8952-specific properties */
+		max8952,vid-gpios = <&gpx0 3 0>, <&gpx0 4 0>;
+		max8952,en-gpio = <&gpx0 1 0>;
+		max8952,default-mode = <0>;
+		max8952,dvs-mode-microvolt = <1250000>, <1200000>,
+						<1050000>, <950000>;
+		max8952,sync-freq = <0>;
+		max8952,ramp-speed = <0>;
+
+		/* generic regulator properties */
+		regulator-name = "vdd_arm";
+		regulator-min-microvolt = <770000>;
+		regulator-max-microvolt = <1400000>;
+		regulator-always-on;
+		regulator-boot-on;
+	};
diff --git a/Documentation/devicetree/bindings/regulator/max8997-regulator.txt b/Documentation/devicetree/bindings/regulator/max8997-regulator.txt
index 9fd69a18b0b..9e5e51d7886 100644
--- a/Documentation/devicetree/bindings/regulator/max8997-regulator.txt
+++ b/Documentation/devicetree/bindings/regulator/max8997-regulator.txt
@@ -28,7 +28,7 @@ Required properties:
     safe operating voltage).
 
     If either of the 'max8997,pmic-buck[1/2/5]-uses-gpio-dvs' optional
-    property is specified, then all the eigth voltage values for the
+    property is specified, then all the eight voltage values for the
     'max8997,pmic-buck[1/2/5]-dvs-voltage' should be specified.
 
 Optional properties:
diff --git a/Documentation/devicetree/bindings/rtc/atmel,at91rm9200-rtc.txt b/Documentation/devicetree/bindings/rtc/atmel,at91rm9200-rtc.txt
new file mode 100644
index 00000000000..2a3feabd3b2
--- /dev/null
+++ b/Documentation/devicetree/bindings/rtc/atmel,at91rm9200-rtc.txt
@@ -0,0 +1,15 @@
+Atmel AT91RM9200 Real Time Clock
+
+Required properties:
+- compatible: should be: "atmel,at91rm9200-rtc"
+- reg: physical base address of the controller and length of memory mapped
+  region.
+- interrupts: rtc alarm/event interrupt
+
+Example:
+
+rtc@fffffe00 {
+	compatible = "atmel,at91rm9200-rtc";
+	reg = <0xfffffe00 0x100>;
+	interrupts = <1 4 7>;
+};
diff --git a/Documentation/devicetree/bindings/spi/brcm,bcm2835-spi.txt b/Documentation/devicetree/bindings/spi/brcm,bcm2835-spi.txt
new file mode 100644
index 00000000000..8bf89c64364
--- /dev/null
+++ b/Documentation/devicetree/bindings/spi/brcm,bcm2835-spi.txt
@@ -0,0 +1,22 @@
+Broadcom BCM2835 SPI0 controller
+
+The BCM2835 contains two forms of SPI master controller, one known simply as
+SPI0, and the other known as the "Universal SPI Master"; part of the
+auxilliary block. This binding applies to the SPI0 controller.
+
+Required properties:
+- compatible: Should be "brcm,bcm2835-spi".
+- reg: Should contain register location and length.
+- interrupts: Should contain interrupt.
+- clocks: The clock feeding the SPI controller.
+
+Example:
+
+spi@20204000 {
+	compatible = "brcm,bcm2835-spi";
+	reg = <0x7e204000 0x1000>;
+	interrupts = <2 22>;
+	clocks = <&clk_spi>;
+	#address-cells = <1>;
+	#size-cells = <0>;
+};
diff --git a/Documentation/devicetree/bindings/spi/fsl-spi.txt b/Documentation/devicetree/bindings/spi/fsl-spi.txt
index 777abd7399d..b032dd76e9d 100644
--- a/Documentation/devicetree/bindings/spi/fsl-spi.txt
+++ b/Documentation/devicetree/bindings/spi/fsl-spi.txt
@@ -4,7 +4,7 @@ Required properties:
 - cell-index : QE SPI subblock index.
 		0: QE subblock SPI1
 		1: QE subblock SPI2
-- compatible : should be "fsl,spi".
+- compatible : should be "fsl,spi" or "aeroflexgaisler,spictrl".
 - mode : the SPI operation mode, it can be "cpu" or "cpu-qe".
 - reg : Offset and length of the register set for the device
 - interrupts : <a b> where a is the interrupt number and b is a
@@ -14,6 +14,7 @@ Required properties:
   controller you have.
 - interrupt-parent : the phandle for the interrupt controller that
   services interrupts for this device.
+- clock-frequency : input clock frequency to non FSL_SOC cores
 
 Optional properties:
 - gpios : specifies the gpio pins to be used for chipselects.
diff --git a/Documentation/devicetree/bindings/spi/nvidia,tegra114-spi.txt b/Documentation/devicetree/bindings/spi/nvidia,tegra114-spi.txt
new file mode 100644
index 00000000000..91ff771c7e7
--- /dev/null
+++ b/Documentation/devicetree/bindings/spi/nvidia,tegra114-spi.txt
@@ -0,0 +1,26 @@
+NVIDIA Tegra114 SPI controller.
+
+Required properties:
+- compatible : should be "nvidia,tegra114-spi".
+- reg: Should contain SPI registers location and length.
+- interrupts: Should contain SPI interrupts.
+- nvidia,dma-request-selector : The Tegra DMA controller's phandle and
+  request selector for this SPI controller.
+- This is also require clock named "spi" as per binding document
+  Documentation/devicetree/bindings/clock/clock-bindings.txt
+
+Recommended properties:
+- spi-max-frequency: Definition as per
+                     Documentation/devicetree/bindings/spi/spi-bus.txt
+Example:
+
+spi@7000d600 {
+	compatible = "nvidia,tegra114-spi";
+	reg = <0x7000d600 0x200>;
+	interrupts = <0 82 0x04>;
+	nvidia,dma-request-selector = <&apbdma 16>;
+	spi-max-frequency = <25000000>;
+	#address-cells = <1>;
+	#size-cells = <0>;
+	status = "disabled";
+};
diff --git a/Documentation/devicetree/bindings/spi/spi-samsung.txt b/Documentation/devicetree/bindings/spi/spi-samsung.txt
index a15ffeddfba..86aa061f069 100644
--- a/Documentation/devicetree/bindings/spi/spi-samsung.txt
+++ b/Documentation/devicetree/bindings/spi/spi-samsung.txt
@@ -31,9 +31,6 @@ Required Board Specific Properties:
 
 - #address-cells: should be 1.
 - #size-cells: should be 0.
-- gpios: The gpio specifier for clock, mosi and miso interface lines (in the
-  order specified). The format of the gpio specifier depends on the gpio
-  controller.
 
 Optional Board Specific Properties:
 
@@ -86,9 +83,8 @@ Example:
 	spi_0: spi@12d20000 {
 		#address-cells = <1>;
 		#size-cells = <0>;
-		gpios = <&gpa2 4 2 3 0>,
-			<&gpa2 6 2 3 0>,
-			<&gpa2 7 2 3 0>;
+		pinctrl-names = "default";
+		pinctrl-0 = <&spi0_bus>;
 
 		w25q80bw@0 {
 			#address-cells = <1>;
diff --git a/Documentation/devicetree/bindings/staging/dwc2.txt b/Documentation/devicetree/bindings/staging/dwc2.txt
new file mode 100644
index 00000000000..1a1b7cfa484
--- /dev/null
+++ b/Documentation/devicetree/bindings/staging/dwc2.txt
@@ -0,0 +1,15 @@
+Platform DesignWare HS OTG USB 2.0 controller
+-----------------------------------------------------
+
+Required properties:
+- compatible : "snps,dwc2"
+- reg : Should contain 1 register range (address and length)
+- interrupts : Should contain 1 interrupt
+
+Example:
+
+        usb@101c0000 {
+                compatible = "ralink,rt3050-usb, snps,dwc2";
+                reg = <0x101c0000 40000>;
+                interrupts = <18>;
+        };
diff --git a/Documentation/devicetree/bindings/staging/imx-drm/fsl-imx-drm.txt b/Documentation/devicetree/bindings/staging/imx-drm/fsl-imx-drm.txt
index 07654f0338b..8071ac20d4b 100644
--- a/Documentation/devicetree/bindings/staging/imx-drm/fsl-imx-drm.txt
+++ b/Documentation/devicetree/bindings/staging/imx-drm/fsl-imx-drm.txt
@@ -26,7 +26,7 @@ Required properties:
 - crtc: the crtc this display is connected to, see below
 Optional properties:
 - interface_pix_fmt: How this display is connected to the
-  crtc. Currently supported types: "rgb24", "rgb565"
+  crtc. Currently supported types: "rgb24", "rgb565", "bgr666"
 - edid: verbatim EDID data block describing attached display.
 - ddc: phandle describing the i2c bus handling the display data
   channel
diff --git a/Documentation/devicetree/bindings/tty/serial/of-serial.txt b/Documentation/devicetree/bindings/tty/serial/of-serial.txt
index 8f01cb190f2..1928a3e83cd 100644
--- a/Documentation/devicetree/bindings/tty/serial/of-serial.txt
+++ b/Documentation/devicetree/bindings/tty/serial/of-serial.txt
@@ -33,6 +33,10 @@ Optional properties:
   RTAS and should not be registered.
 - no-loopback-test: set to indicate that the port does not implements loopback
   test mode
+- fifo-size: the fifo size of the UART.
+- auto-flow-control: one way to enable automatic flow control support. The
+  driver is allowed to detect support for the capability even without this
+  property.
 
 Example:
 
diff --git a/Documentation/devicetree/bindings/usb/ci13xxx-imx.txt b/Documentation/devicetree/bindings/usb/ci13xxx-imx.txt
index 5778b9c83bd..1c04a4c9515 100644
--- a/Documentation/devicetree/bindings/usb/ci13xxx-imx.txt
+++ b/Documentation/devicetree/bindings/usb/ci13xxx-imx.txt
@@ -11,6 +11,7 @@ Optional properties:
   that indicate usb controller index
 - vbus-supply: regulator for vbus
 - disable-over-current: disable over current detect
+- external-vbus-divider: enables off-chip resistor divider for Vbus
 
 Examples:
 usb@02184000 { /* USB OTG */
@@ -20,4 +21,5 @@ usb@02184000 { /* USB OTG */
 	fsl,usbphy = <&usbphy1>;
 	fsl,usbmisc = <&usbmisc 0>;
 	disable-over-current;
+	external-vbus-divider;
 };
diff --git a/Documentation/devicetree/bindings/usb/ehci-omap.txt b/Documentation/devicetree/bindings/usb/ehci-omap.txt
new file mode 100644
index 00000000000..485a9a1efa7
--- /dev/null
+++ b/Documentation/devicetree/bindings/usb/ehci-omap.txt
@@ -0,0 +1,32 @@
+OMAP HS USB EHCI controller
+
+This device is usually the child of the omap-usb-host
+Documentation/devicetree/bindings/mfd/omap-usb-host.txt
+
+Required properties:
+
+- compatible: should be "ti,ehci-omap"
+- reg: should contain one register range i.e. start and length
+- interrupts: description of the interrupt line
+
+Optional properties:
+
+- phys: list of phandles to PHY nodes.
+  This property is required if at least one of the ports are in
+  PHY mode i.e. OMAP_EHCI_PORT_MODE_PHY
+
+To specify the port mode, see
+Documentation/devicetree/bindings/mfd/omap-usb-host.txt
+
+Example for OMAP4:
+
+usbhsehci: ehci@4a064c00 {
+	compatible = "ti,ehci-omap", "usb-ehci";
+	reg = <0x4a064c00 0x400>;
+	interrupts = <0 77 0x4>;
+};
+
+&usbhsehci {
+	phys = <&hsusb1_phy 0 &hsusb3_phy>;
+};
+
diff --git a/Documentation/devicetree/bindings/usb/ohci-omap3.txt b/Documentation/devicetree/bindings/usb/ohci-omap3.txt
new file mode 100644
index 00000000000..14ab42812a8
--- /dev/null
+++ b/Documentation/devicetree/bindings/usb/ohci-omap3.txt
@@ -0,0 +1,15 @@
+OMAP HS USB OHCI controller (OMAP3 and later)
+
+Required properties:
+
+- compatible: should be "ti,ohci-omap3"
+- reg: should contain one register range i.e. start and length
+- interrupts: description of the interrupt line
+
+Example for OMAP4:
+
+usbhsohci: ohci@4a064800 {
+	compatible = "ti,ohci-omap3", "usb-ohci";
+	reg = <0x4a064800 0x400>;
+	interrupts = <0 76 0x4>;
+};
diff --git a/Documentation/devicetree/bindings/usb/omap-usb.txt b/Documentation/devicetree/bindings/usb/omap-usb.txt
index 1ef0ce71f8f..662f0f1d231 100644
--- a/Documentation/devicetree/bindings/usb/omap-usb.txt
+++ b/Documentation/devicetree/bindings/usb/omap-usb.txt
@@ -8,10 +8,10 @@ OMAP MUSB GLUE
    and disconnect.
  - multipoint : Should be "1" indicating the musb controller supports
    multipoint. This is a MUSB configuration-specific setting.
- - num_eps : Specifies the number of endpoints. This is also a
+ - num-eps : Specifies the number of endpoints. This is also a
    MUSB configuration-specific setting. Should be set to "16"
- - ram_bits : Specifies the ram address size. Should be set to "12"
- - interface_type : This is a board specific setting to describe the type of
+ - ram-bits : Specifies the ram address size. Should be set to "12"
+ - interface-type : This is a board specific setting to describe the type of
    interface between the controller and the phy. It should be "0" or "1"
    specifying ULPI and UTMI respectively.
  - mode : Should be "3" to represent OTG. "1" signifies HOST and "2"
@@ -29,18 +29,46 @@ usb_otg_hs: usb_otg_hs@4a0ab000 {
 	ti,hwmods = "usb_otg_hs";
 	ti,has-mailbox;
 	multipoint = <1>;
-	num_eps = <16>;
-	ram_bits = <12>;
+	num-eps = <16>;
+	ram-bits = <12>;
 	ctrl-module = <&omap_control_usb>;
 };
 
 Board specific device node entry
 &usb_otg_hs {
-	interface_type = <1>;
+	interface-type = <1>;
 	mode = <3>;
 	power = <50>;
 };
 
+OMAP DWC3 GLUE
+ - compatible : Should be "ti,dwc3"
+ - ti,hwmods : Should be "usb_otg_ss"
+ - reg : Address and length of the register set for the device.
+ - interrupts : The irq number of this device that is used to interrupt the
+   MPU
+ - #address-cells, #size-cells : Must be present if the device has sub-nodes
+ - utmi-mode : controls the source of UTMI/PIPE status for VBUS and OTG ID.
+   It should be set to "1" for HW mode and "2" for SW mode.
+ - ranges: the child address space are mapped 1:1 onto the parent address space
+
+Sub-nodes:
+The dwc3 core should be added as subnode to omap dwc3 glue.
+- dwc3 :
+   The binding details of dwc3 can be found in:
+   Documentation/devicetree/bindings/usb/dwc3.txt
+
+omap_dwc3 {
+	compatible = "ti,dwc3";
+	ti,hwmods = "usb_otg_ss";
+	reg = <0x4a020000 0x1ff>;
+	interrupts = <0 93 4>;
+	#address-cells = <1>;
+	#size-cells = <1>;
+	utmi-mode = <2>;
+	ranges;
+};
+
 OMAP CONTROL USB
 
 Required properties:
diff --git a/Documentation/devicetree/bindings/usb/samsung-usbphy.txt b/Documentation/devicetree/bindings/usb/samsung-usbphy.txt
index 033194934f6..33fd3543f3f 100644
--- a/Documentation/devicetree/bindings/usb/samsung-usbphy.txt
+++ b/Documentation/devicetree/bindings/usb/samsung-usbphy.txt
@@ -1,20 +1,25 @@
-* Samsung's usb phy transceiver
+SAMSUNG USB-PHY controllers
 
-The Samsung's phy transceiver is used for controlling usb phy for
-s3c-hsotg as well as ehci-s5p and ohci-exynos usb controllers
-across Samsung SOCs.
+** Samsung's usb 2.0 phy transceiver
+
+The Samsung's usb 2.0 phy transceiver is used for controlling
+usb 2.0 phy for s3c-hsotg as well as ehci-s5p and ohci-exynos
+usb controllers across Samsung SOCs.
 TODO: Adding the PHY binding with controller(s) according to the under
-developement generic PHY driver.
+development generic PHY driver.
 
 Required properties:
 
 Exynos4210:
-- compatible : should be "samsung,exynos4210-usbphy"
+- compatible : should be "samsung,exynos4210-usb2phy"
 - reg : base physical address of the phy registers and length of memory mapped
 	region.
+- clocks: Clock IDs array as required by the controller.
+- clock-names: names of clock correseponding IDs clock property as requested
+	       by the controller driver.
 
 Exynos5250:
-- compatible : should be "samsung,exynos5250-usbphy"
+- compatible : should be "samsung,exynos5250-usb2phy"
 - reg : base physical address of the phy registers and length of memory mapped
 	region.
 
@@ -44,12 +49,69 @@ Example:
 	usbphy@125B0000 {
 		#address-cells = <1>;
 		#size-cells = <1>;
-		compatible = "samsung,exynos4210-usbphy";
+		compatible = "samsung,exynos4210-usb2phy";
 		reg = <0x125B0000 0x100>;
 		ranges;
 
+		clocks = <&clock 2>, <&clock 305>;
+		clock-names = "xusbxti", "otg";
+
 		usbphy-sys {
 			/* USB device and host PHY_CONTROL registers */
 			reg = <0x10020704 0x8>;
 		};
 	};
+
+
+** Samsung's usb 3.0 phy transceiver
+
+Starting exynso5250, Samsung's SoC have usb 3.0 phy transceiver
+which is used for controlling usb 3.0 phy for dwc3-exynos usb 3.0
+controllers across Samsung SOCs.
+
+Required properties:
+
+Exynos5250:
+- compatible : should be "samsung,exynos5250-usb3phy"
+- reg : base physical address of the phy registers and length of memory mapped
+	region.
+- clocks: Clock IDs array as required by the controller.
+- clock-names: names of clocks correseponding to IDs in the clock property
+	       as requested by the controller driver.
+
+Optional properties:
+- #address-cells: should be '1' when usbphy node has a child node with 'reg'
+		  property.
+- #size-cells: should be '1' when usbphy node has a child node with 'reg'
+	       property.
+- ranges: allows valid translation between child's address space and parent's
+	  address space.
+
+- The child node 'usbphy-sys' to the node 'usbphy' is for the system controller
+  interface for usb-phy. It should provide the following information required by
+  usb-phy controller to control phy.
+  - reg : base physical address of PHY_CONTROL registers.
+	  The size of this register is the total sum of size of all PHY_CONTROL
+	  registers that the SoC has. For example, the size will be
+	  '0x4' in case we have only one PHY_CONTROL register (e.g.
+	  OTHERS register in S3C64XX or USB_PHY_CONTROL register in S5PV210)
+	  and, '0x8' in case we have two PHY_CONTROL registers (e.g.
+	  USBDEVICE_PHY_CONTROL and USBHOST_PHY_CONTROL registers in exynos4x).
+	  and so on.
+
+Example:
+	usbphy@12100000 {
+		compatible = "samsung,exynos5250-usb3phy";
+		reg = <0x12100000 0x100>;
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges;
+
+		clocks = <&clock 1>, <&clock 286>;
+		clock-names = "ext_xtal", "usbdrd30";
+
+		usbphy-sys {
+			/* USB device and host PHY_CONTROL registers */
+			reg = <0x10040704 0x8>;
+		};
+	};
diff --git a/Documentation/devicetree/bindings/usb/usb-nop-xceiv.txt b/Documentation/devicetree/bindings/usb/usb-nop-xceiv.txt
new file mode 100644
index 00000000000..d7e272671c7
--- /dev/null
+++ b/Documentation/devicetree/bindings/usb/usb-nop-xceiv.txt
@@ -0,0 +1,34 @@
+USB NOP PHY
+
+Required properties:
+- compatible: should be usb-nop-xceiv
+
+Optional properties:
+- clocks: phandle to the PHY clock. Use as per Documentation/devicetree
+  /bindings/clock/clock-bindings.txt
+  This property is required if clock-frequency is specified.
+
+- clock-names: Should be "main_clk"
+
+- clock-frequency: the clock frequency (in Hz) that the PHY clock must
+  be configured to.
+
+- vcc-supply: phandle to the regulator that provides RESET to the PHY.
+
+- reset-supply: phandle to the regulator that provides power to the PHY.
+
+Example:
+
+	hsusb1_phy {
+		compatible = "usb-nop-xceiv";
+		clock-frequency = <19200000>;
+		clocks = <&osc 0>;
+		clock-names = "main_clk";
+		vcc-supply = <&hsusb1_vcc_regulator>;
+		reset-supply = <&hsusb1_reset_regulator>;
+	};
+
+hsusb1_phy is a NOP USB PHY device that gets its clock from an oscillator
+and expects that clock to be configured to 19.2MHz by the NOP PHY driver.
+hsusb1_vcc_regulator provides power to the PHY and hsusb1_reset_regulator
+controls RESET.
diff --git a/Documentation/devicetree/bindings/vendor-prefixes.txt b/Documentation/devicetree/bindings/vendor-prefixes.txt
index 19e1ef73ab0..4d1919bf233 100644
--- a/Documentation/devicetree/bindings/vendor-prefixes.txt
+++ b/Documentation/devicetree/bindings/vendor-prefixes.txt
@@ -5,6 +5,7 @@ using them to avoid name-space collisions.
 
 ad	Avionic Design GmbH
 adi	Analog Devices, Inc.
+aeroflexgaisler	Aeroflex Gaisler AB
 ak	Asahi Kasei Corp.
 amcc	Applied Micro Circuits Corporation (APM, formally AMCC)
 apm	Applied Micro Circuits Corporation (APM)
@@ -48,6 +49,7 @@ samsung	Samsung Semiconductor
 sbs	Smart Battery System
 schindler	Schindler
 sil	Silicon Image
+silabs	Silicon Laboratories
 simtek
 sirf	SiRF Technology, Inc.
 snps 	Synopsys, Inc.
diff --git a/Documentation/devicetree/bindings/video/backlight/lp855x.txt b/Documentation/devicetree/bindings/video/backlight/lp855x.txt
new file mode 100644
index 00000000000..1482103d288
--- /dev/null
+++ b/Documentation/devicetree/bindings/video/backlight/lp855x.txt
@@ -0,0 +1,41 @@
+lp855x bindings
+
+Required properties:
+  - compatible: "ti,lp8550", "ti,lp8551", "ti,lp8552", "ti,lp8553",
+                "ti,lp8556", "ti,lp8557"
+  - reg: I2C slave address (u8)
+  - dev-ctrl: Value of DEVICE CONTROL register (u8). It depends on the device.
+
+Optional properties:
+  - bl-name: Backlight device name (string)
+  - init-brt: Initial value of backlight brightness (u8)
+  - pwm-period: PWM period value. Set only PWM input mode used (u32)
+  - rom-addr: Register address of ROM area to be updated (u8)
+  - rom-val: Register value to be updated (u8)
+
+Example:
+
+	/* LP8556 */
+	backlight@2c {
+		compatible = "ti,lp8556";
+		reg = <0x2c>;
+
+		bl-name = "lcd-bl";
+		dev-ctrl = /bits/ 8 <0x85>;
+		init-brt = /bits/ 8 <0x10>;
+	};
+
+	/* LP8557 */
+	backlight@2c {
+		compatible = "ti,lp8557";
+		reg = <0x2c>;
+
+		dev-ctrl = /bits/ 8 <0x41>;
+		init-brt = /bits/ 8 <0x0a>;
+
+		/* 4V OV, 4 output LED string enabled */
+		rom_14h {
+			rom-addr = /bits/ 8 <0x14>;
+			rom-val = /bits/ 8 <0xcf>;
+		};
+	};
diff --git a/Documentation/devicetree/bindings/video/backlight/tps65217-backlight.txt b/Documentation/devicetree/bindings/video/backlight/tps65217-backlight.txt
new file mode 100644
index 00000000000..5fb9279ac28
--- /dev/null
+++ b/Documentation/devicetree/bindings/video/backlight/tps65217-backlight.txt
@@ -0,0 +1,27 @@
+TPS65217 family of regulators
+
+The TPS65217 chip contains a boost converter and current sinks which can be
+used to drive LEDs for use as backlights.
+
+Required properties:
+- compatible: "ti,tps65217"
+- reg: I2C slave address
+- backlight: node for specifying WLED1 and WLED2 lines in TPS65217
+- isel: selection bit, valid values: 1 for ISEL1 (low-level) and 2 for ISEL2 (high-level)
+- fdim: PWM dimming frequency, valid values: 100, 200, 500, 1000
+- default-brightness: valid values: 0-100
+
+Each regulator is defined using the standard binding for regulators.
+
+Example:
+
+	tps: tps@24 {
+		reg = <0x24>;
+		compatible = "ti,tps65217";
+		backlight {
+			isel = <1>;  /* 1 - ISET1, 2 ISET2 */
+			fdim = <100>; /* TPS65217_BL_FDIM_100HZ */
+			default-brightness = <50>;
+		};
+	};
+
diff --git a/Documentation/devicetree/bindings/video/via,vt8500-fb.txt b/Documentation/devicetree/bindings/video/via,vt8500-fb.txt
index c870b6478ec..2871e218a0f 100644
--- a/Documentation/devicetree/bindings/video/via,vt8500-fb.txt
+++ b/Documentation/devicetree/bindings/video/via,vt8500-fb.txt
@@ -5,58 +5,32 @@ Required properties:
 - compatible : "via,vt8500-fb"
 - reg : Should contain 1 register ranges(address and length)
 - interrupts : framebuffer controller interrupt
-- display: a phandle pointing to the display node
+- bits-per-pixel : bit depth of framebuffer (16 or 32)
 
-Required nodes:
-- display: a display node is required to initialize the lcd panel
-	This should be in the board dts.
-- default-mode: a videomode within the display with timing parameters
-	as specified below.
+Required subnodes:
+- display-timings: see display-timing.txt for information
 
 Example:
 
-	fb@d800e400 {
+	fb@d8050800 {
 		compatible = "via,vt8500-fb";
 		reg = <0xd800e400 0x400>;
 		interrupts = <12>;
-		display = <&display>;
-		default-mode = <&mode0>;
-	};
-
-VIA VT8500 Display
------------------------------------------------------
-Required properties (as per of_videomode_helper):
-
- - hactive, vactive: Display resolution
- - hfront-porch, hback-porch, hsync-len: Horizontal Display timing parameters
-   in pixels
-   vfront-porch, vback-porch, vsync-len: Vertical display timing parameters in
-   lines
- - clock: displayclock in Hz
- - bpp: lcd panel bit-depth.
-	<16> for RGB565, <32> for RGB888
-
-Optional properties (as per of_videomode_helper):
- - width-mm, height-mm: Display dimensions in mm
- - hsync-active-high (bool): Hsync pulse is active high
- - vsync-active-high (bool): Vsync pulse is active high
- - interlaced (bool): This is an interlaced mode
- - doublescan (bool): This is a doublescan mode
+		bits-per-pixel = <16>;
 
-Example:
-	display: display@0 {
-		modes {
-			mode0: mode@0 {
+		display-timings {
+			native-mode = <&timing0>;
+			timing0: 800x480 {
+				clock-frequency = <0>; /* unused but required */
 				hactive = <800>;
 				vactive = <480>;
-				hback-porch = <88>;
 				hfront-porch = <40>;
+				hback-porch = <88>;
 				hsync-len = <0>;
 				vback-porch = <32>;
 				vfront-porch = <11>;
 				vsync-len = <1>;
-				clock = <0>;	/* unused but required */
-				bpp = <16>;	/* non-standard but required */
 			};
 		};
 	};
+
diff --git a/Documentation/devicetree/bindings/video/wm,wm8505-fb.txt b/Documentation/devicetree/bindings/video/wm,wm8505-fb.txt
index 3d325e1d11e..0bcadb2840a 100644
--- a/Documentation/devicetree/bindings/video/wm,wm8505-fb.txt
+++ b/Documentation/devicetree/bindings/video/wm,wm8505-fb.txt
@@ -4,20 +4,30 @@ Wondermedia WM8505 Framebuffer
 Required properties:
 - compatible : "wm,wm8505-fb"
 - reg : Should contain 1 register ranges(address and length)
-- via,display: a phandle pointing to the display node
+- bits-per-pixel : bit depth of framebuffer (16 or 32)
 
-Required nodes:
-- display: a display node is required to initialize the lcd panel
-	This should be in the board dts. See definition in
-	Documentation/devicetree/bindings/video/via,vt8500-fb.txt
-- default-mode: a videomode node as specified in
-	Documentation/devicetree/bindings/video/via,vt8500-fb.txt
+Required subnodes:
+- display-timings: see display-timing.txt for information
 
 Example:
 
-	fb@d8050800 {
+	fb@d8051700 {
 		compatible = "wm,wm8505-fb";
-		reg = <0xd8050800 0x200>;
-		display = <&display>;
-		default-mode = <&mode0>;
+		reg = <0xd8051700 0x200>;
+		bits-per-pixel = <16>;
+
+		display-timings {
+			native-mode = <&timing0>;
+			timing0: 800x480 {
+				clock-frequency = <0>; /* unused but required */
+				hactive = <800>;
+				vactive = <480>;
+				hfront-porch = <40>;
+				hback-porch = <88>;
+				hsync-len = <0>;
+				vback-porch = <32>;
+				vfront-porch = <11>;
+				vsync-len = <1>;
+			};
+		};
 	};
diff --git a/Documentation/filesystems/vfat.txt b/Documentation/filesystems/vfat.txt
index d230dd9c99b..4a93e98b290 100644
--- a/Documentation/filesystems/vfat.txt
+++ b/Documentation/filesystems/vfat.txt
@@ -150,12 +150,28 @@ discard       -- If set, issues discard/TRIM commands to the block
 		 device when blocks are freed. This is useful for SSD devices
 		 and sparse/thinly-provisoned LUNs.
 
-nfs           -- This option maintains an index (cache) of directory
-		 inodes by i_logstart which is used by the nfs-related code to
-		 improve look-ups.
+nfs=stale_rw|nostale_ro
+		Enable this only if you want to export the FAT filesystem
+		over NFS.
+
+		stale_rw: This option maintains an index (cache) of directory
+		inodes by i_logstart which is used by the nfs-related code to
+		improve look-ups. Full file operations (read/write) over NFS is
+		supported but with cache eviction at NFS server, this could
+		result in ESTALE issues.
+
+		nostale_ro: This option bases the inode number and filehandle
+		on the on-disk location of a file in the MS-DOS directory entry.
+		This ensures that ESTALE will not be returned after a file is
+		evicted from the inode cache. However, it means that operations
+		such as rename, create and unlink could cause filehandles that
+		previously pointed at one file to point at a different file,
+		potentially causing data corruption. For this reason, this
+		option also mounts the filesystem readonly.
+
+		To maintain backward compatibility, '-o nfs' is also accepted,
+		defaulting to stale_rw
 
-		 Enable this only if you want to export the FAT filesystem
-		 over NFS
 
 <bool>: 0,1,yes,no,true,false
 
diff --git a/Documentation/hwmon/adt7410 b/Documentation/hwmon/adt7410
index 58150c480e5..9817941e5f1 100644
--- a/Documentation/hwmon/adt7410
+++ b/Documentation/hwmon/adt7410
@@ -12,29 +12,42 @@ Supported chips:
     Addresses scanned: None
     Datasheet: Publicly available at the Analog Devices website
                http://www.analog.com/static/imported-files/data_sheets/ADT7420.pdf
+  * Analog Devices ADT7310
+    Prefix: 'adt7310'
+    Addresses scanned: None
+    Datasheet: Publicly available at the Analog Devices website
+               http://www.analog.com/static/imported-files/data_sheets/ADT7310.pdf
+  * Analog Devices ADT7320
+    Prefix: 'adt7320'
+    Addresses scanned: None
+    Datasheet: Publicly available at the Analog Devices website
+               http://www.analog.com/static/imported-files/data_sheets/ADT7320.pdf
 
 Author: Hartmut Knaack <knaack.h@gmx.de>
 
 Description
 -----------
 
-The ADT7410 is a temperature sensor with rated temperature range of -55°C to
-+150°C. It has a high accuracy of +/-0.5°C and can be operated at a resolution
-of 13 bits (0.0625°C) or 16 bits (0.0078°C). The sensor provides an INT pin to
-indicate that a minimum or maximum temperature set point has been exceeded, as
-well as a critical temperature (CT) pin to indicate that the critical
-temperature set point has been exceeded. Both pins can be set up with a common
-hysteresis of 0°C - 15°C and a fault queue, ranging from 1 to 4 events. Both
-pins can individually set to be active-low or active-high, while the whole
-device can either run in comparator mode or interrupt mode. The ADT7410
-supports continous temperature sampling, as well as sampling one temperature
-value per second or even justget one sample on demand for power saving.
-Besides, it can completely power down its ADC, if power management is
-required.
-
-The ADT7420 is register compatible, the only differences being the package,
-a slightly narrower operating temperature range (-40°C to +150°C), and a
-better accuracy (0.25°C instead of 0.50°C.)
+The ADT7310/ADT7410 is a temperature sensor with rated temperature range of
+-55°C to +150°C. It has a high accuracy of +/-0.5°C and can be operated at a
+resolution of 13 bits (0.0625°C) or 16 bits (0.0078°C). The sensor provides an
+INT pin to indicate that a minimum or maximum temperature set point has been
+exceeded, as well as a critical temperature (CT) pin to indicate that the
+critical temperature set point has been exceeded. Both pins can be set up with a
+common hysteresis of 0°C - 15°C and a fault queue, ranging from 1 to 4 events.
+Both pins can individually set to be active-low or active-high, while the whole
+device can either run in comparator mode or interrupt mode. The ADT7410 supports
+continuous temperature sampling, as well as sampling one temperature value per
+second or even just get one sample on demand for power saving. Besides, it can
+completely power down its ADC, if power management is required.
+
+The ADT7320/ADT7420 is register compatible, the only differences being the
+package, a slightly narrower operating temperature range (-40°C to +150°C), and
+a better accuracy (0.25°C instead of 0.50°C.)
+
+The difference between the ADT7310/ADT7320 and ADT7410/ADT7420 is the control
+interface, the ADT7310 and ADT7320 use SPI while the ADT7410 and ADT7420 use
+I2C.
 
 Configuration Notes
 -------------------
diff --git a/Documentation/hwmon/lm25066 b/Documentation/hwmon/lm25066
index 26025e419d3..c1b57d72efc 100644
--- a/Documentation/hwmon/lm25066
+++ b/Documentation/hwmon/lm25066
@@ -1,7 +1,13 @@
-Kernel driver max8688
+Kernel driver lm25066
 =====================
 
 Supported chips:
+  * TI LM25056
+    Prefix: 'lm25056'
+    Addresses scanned: -
+    Datasheets:
+	http://www.ti.com/lit/gpn/lm25056
+	http://www.ti.com/lit/gpn/lm25056a
   * National Semiconductor LM25066
     Prefix: 'lm25066'
     Addresses scanned: -
@@ -25,8 +31,9 @@ Author: Guenter Roeck <linux@roeck-us.net>
 Description
 -----------
 
-This driver supports hardware montoring for National Semiconductor LM25066,
-LM5064, and LM5064 Power Management, Monitoring, Control, and Protection ICs.
+This driver supports hardware montoring for National Semiconductor / TI LM25056,
+LM25066, LM5064, and LM5064 Power Management, Monitoring, Control, and
+Protection ICs.
 
 The driver is a client driver to the core PMBus driver. Please see
 Documentation/hwmon/pmbus for details on PMBus client drivers.
@@ -60,14 +67,19 @@ in1_max			Maximum input voltage.
 in1_min_alarm		Input voltage low alarm.
 in1_max_alarm		Input voltage high alarm.
 
-in2_label		"vout1"
-in2_input		Measured output voltage.
-in2_average		Average measured output voltage.
-in2_min			Minimum output voltage.
-in2_min_alarm		Output voltage low alarm.
-
-in3_label		"vout2"
-in3_input		Measured voltage on vaux pin
+in2_label		"vmon"
+in2_input		Measured voltage on VAUX pin
+in2_min			Minimum VAUX voltage (LM25056 only).
+in2_max			Maximum VAUX voltage (LM25056 only).
+in2_min_alarm		VAUX voltage low alarm (LM25056 only).
+in2_max_alarm		VAUX voltage high alarm (LM25056 only).
+
+in3_label		"vout1"
+			Not supported on LM25056.
+in3_input		Measured output voltage.
+in3_average		Average measured output voltage.
+in3_min			Minimum output voltage.
+in3_min_alarm		Output voltage low alarm.
 
 curr1_label		"iin"
 curr1_input		Measured input current.
diff --git a/Documentation/hwmon/lm95234 b/Documentation/hwmon/lm95234
new file mode 100644
index 00000000000..a0e95ddfd37
--- /dev/null
+++ b/Documentation/hwmon/lm95234
@@ -0,0 +1,36 @@
+Kernel driver lm95234
+=====================
+
+Supported chips:
+  * National Semiconductor / Texas Instruments LM95234
+    Addresses scanned: I2C 0x18, 0x4d, 0x4e
+    Datasheet: Publicly available at the Texas Instruments website
+               http://www.ti.com/product/lm95234
+
+
+Author: Guenter Roeck <linux@roeck-us.net>
+
+Description
+-----------
+
+LM95234 is an 11-bit digital temperature sensor with a 2-wire System Management
+Bus (SMBus) interface and TrueTherm technology that can very accurately monitor
+the temperature of four remote diodes as well as its own temperature.
+The four remote diodes can be external devices such as microprocessors,
+graphics processors or diode-connected 2N3904s. The LM95234's TruTherm
+beta compensation technology allows sensing of 90 nm or 65 nm process
+thermal diodes accurately.
+
+All temperature values are given in millidegrees Celsius. Temperature
+is provided within a range of -127 to +255 degrees (+127.875 degrees for
+the internal sensor). Resolution depends on temperature input and range.
+
+Each sensor has its own maximum limit, but the hysteresis is common to all
+channels. The hysteresis is configurable with the tem1_max_hyst attribute and
+affects the hysteresis on all channels. The first two external sensors also
+have a critical limit.
+
+The lm95234 driver can change its update interval to a fixed set of values.
+It will round up to the next selectable interval. See the datasheet for exact
+values. Reading sensor values more often will do no harm, but will return
+'old' values.
diff --git a/Documentation/hwmon/ltc2978 b/Documentation/hwmon/ltc2978
index e4d75c606c9..dc0d08c6130 100644
--- a/Documentation/hwmon/ltc2978
+++ b/Documentation/hwmon/ltc2978
@@ -2,6 +2,10 @@ Kernel driver ltc2978
 =====================
 
 Supported chips:
+  * Linear Technology LTC2974
+    Prefix: 'ltc2974'
+    Addresses scanned: -
+    Datasheet: http://www.linear.com/product/ltc2974
   * Linear Technology LTC2978
     Prefix: 'ltc2978'
     Addresses scanned: -
@@ -10,6 +14,10 @@ Supported chips:
     Prefix: 'ltc3880'
     Addresses scanned: -
     Datasheet: http://www.linear.com/product/ltc3880
+  * Linear Technology LTC3883
+    Prefix: 'ltc3883'
+    Addresses scanned: -
+    Datasheet: http://www.linear.com/product/ltc3883
 
 Author: Guenter Roeck <linux@roeck-us.net>
 
@@ -17,9 +25,9 @@ Author: Guenter Roeck <linux@roeck-us.net>
 Description
 -----------
 
-The LTC2978 is an octal power supply monitor, supervisor, sequencer and
-margin controller. The LTC3880 is a dual, PolyPhase DC/DC synchronous
-step-down switching regulator controller.
+LTC2974 is a quad digital power supply manager. LTC2978 is an octal power supply
+monitor. LTC3880 is a dual output poly-phase step-down DC/DC controller. LTC3883
+is a single phase step-down DC/DC controller.
 
 
 Usage Notes
@@ -41,63 +49,90 @@ Sysfs attributes
 in1_label		"vin"
 in1_input		Measured input voltage.
 in1_min			Minimum input voltage.
-in1_max			Maximum input voltage.
-in1_lcrit		Critical minimum input voltage.
+in1_max			Maximum input voltage. LTC2974 and LTC2978 only.
+in1_lcrit		Critical minimum input voltage. LTC2974 and LTC2978
+			only.
 in1_crit		Critical maximum input voltage.
 in1_min_alarm		Input voltage low alarm.
-in1_max_alarm		Input voltage high alarm.
-in1_lcrit_alarm		Input voltage critical low alarm.
+in1_max_alarm		Input voltage high alarm. LTC2974 and LTC2978 only.
+in1_lcrit_alarm		Input voltage critical low alarm. LTC2974 and LTC2978
+			only.
 in1_crit_alarm		Input voltage critical high alarm.
-in1_lowest		Lowest input voltage. LTC2978 only.
+in1_lowest		Lowest input voltage. LTC2974 and LTC2978 only.
 in1_highest		Highest input voltage.
-in1_reset_history	Reset history. Writing into this attribute will reset
-			history for all attributes.
-
-in[2-9]_label		"vout[1-8]". Channels 3 to 9 on LTC2978 only.
-in[2-9]_input		Measured output voltage.
-in[2-9]_min		Minimum output voltage.
-in[2-9]_max		Maximum output voltage.
-in[2-9]_lcrit		Critical minimum output voltage.
-in[2-9]_crit		Critical maximum output voltage.
-in[2-9]_min_alarm	Output voltage low alarm.
-in[2-9]_max_alarm	Output voltage high alarm.
-in[2-9]_lcrit_alarm	Output voltage critical low alarm.
-in[2-9]_crit_alarm	Output voltage critical high alarm.
-in[2-9]_lowest		Lowest output voltage. LTC2978 only.
-in[2-9]_highest		Lowest output voltage.
-in[2-9]_reset_history	Reset history. Writing into this attribute will reset
-			history for all attributes.
-
-temp[1-3]_input		Measured temperature.
+in1_reset_history	Reset input voltage history.
+
+in[N]_label		"vout[1-8]".
+			LTC2974: N=2-5
+			LTC2978: N=2-9
+			LTC3880: N=2-3
+			LTC3883: N=2
+in[N]_input		Measured output voltage.
+in[N]_min		Minimum output voltage.
+in[N]_max		Maximum output voltage.
+in[N]_lcrit		Critical minimum output voltage.
+in[N]_crit		Critical maximum output voltage.
+in[N]_min_alarm		Output voltage low alarm.
+in[N]_max_alarm		Output voltage high alarm.
+in[N]_lcrit_alarm	Output voltage critical low alarm.
+in[N]_crit_alarm	Output voltage critical high alarm.
+in[N]_lowest		Lowest output voltage. LTC2974 and LTC2978 only.
+in[N]_highest		Highest output voltage.
+in[N]_reset_history	Reset output voltage history.
+
+temp[N]_input		Measured temperature.
+			On LTC2974, temp[1-4] report external temperatures,
+			and temp5 reports the chip temperature.
 			On LTC2978, only one temperature measurement is
-			supported and reflects the internal temperature.
+			supported and reports the chip temperature.
 			On LTC3880, temp1 and temp2 report external
-			temperatures, and temp3 reports the internal
-			temperature.
-temp[1-3]_min		Mimimum temperature.
-temp[1-3]_max		Maximum temperature.
-temp[1-3]_lcrit		Critical low temperature.
-temp[1-3]_crit		Critical high temperature.
-temp[1-3]_min_alarm	Chip temperature low alarm.
-temp[1-3]_max_alarm	Chip temperature high alarm.
-temp[1-3]_lcrit_alarm	Chip temperature critical low alarm.
-temp[1-3]_crit_alarm	Chip temperature critical high alarm.
-temp[1-3]_lowest	Lowest measured temperature. LTC2978 only.
-temp[1-3]_highest	Highest measured temperature.
-temp[1-3]_reset_history	Reset history. Writing into this attribute will reset
-			history for all attributes.
-
-power[1-2]_label	"pout[1-2]". LTC3880 only.
-power[1-2]_input	Measured power.
-
-curr1_label		"iin". LTC3880 only.
+			temperatures, and temp3 reports the chip temperature.
+			On LTC3883, temp1 reports an external temperature,
+			and temp2 reports the chip temperature.
+temp[N]_min		Mimimum temperature. LTC2974 and LTC2978 only.
+temp[N]_max		Maximum temperature.
+temp[N]_lcrit		Critical low temperature.
+temp[N]_crit		Critical high temperature.
+temp[N]_min_alarm	Temperature low alarm. LTC2974 and LTC2978 only.
+temp[N]_max_alarm	Temperature high alarm.
+temp[N]_lcrit_alarm	Temperature critical low alarm.
+temp[N]_crit_alarm	Temperature critical high alarm.
+temp[N]_lowest		Lowest measured temperature. LTC2974 and LTC2978 only.
+			Not supported for chip temperature sensor on LTC2974.
+temp[N]_highest		Highest measured temperature. Not supported for chip
+			temperature sensor on LTC2974.
+temp[N]_reset_history	Reset temperature history. Not supported for chip
+			temperature sensor on LTC2974.
+
+power1_label		"pin". LTC3883 only.
+power1_input		Measured input power.
+
+power[N]_label		"pout[1-4]".
+			LTC2974: N=1-4
+			LTC2978: Not supported
+			LTC3880: N=1-2
+			LTC3883: N=2
+power[N]_input		Measured output power.
+
+curr1_label		"iin". LTC3880 and LTC3883 only.
 curr1_input		Measured input current.
 curr1_max		Maximum input current.
 curr1_max_alarm		Input current high alarm.
-
-curr[2-3]_label		"iout[1-2]". LTC3880 only.
-curr[2-3]_input		Measured input current.
-curr[2-3]_max		Maximum input current.
-curr[2-3]_crit		Critical input current.
-curr[2-3]_max_alarm	Input current high alarm.
-curr[2-3]_crit_alarm	Input current critical high alarm.
+curr1_highest		Highest input current. LTC3883 only.
+curr1_reset_history	Reset input current history. LTC3883 only.
+
+curr[N]_label		"iout[1-4]".
+			LTC2974: N=1-4
+			LTC2978: not supported
+			LTC3880: N=2-3
+			LTC3883: N=2
+curr[N]_input		Measured output current.
+curr[N]_max		Maximum output current.
+curr[N]_crit		Critical high output current.
+curr[N]_lcrit		Critical low output current. LTC2974 only.
+curr[N]_max_alarm	Output current high alarm.
+curr[N]_crit_alarm	Output current critical high alarm.
+curr[N]_lcrit_alarm	Output current critical low alarm. LTC2974 only.
+curr[N]_lowest		Lowest output current. LTC2974 only.
+curr[N]_highest		Highest output current.
+curr[N]_reset_history	Reset output current history.
diff --git a/Documentation/hwmon/nct6775 b/Documentation/hwmon/nct6775
new file mode 100644
index 00000000000..4e9ef60e8c6
--- /dev/null
+++ b/Documentation/hwmon/nct6775
@@ -0,0 +1,188 @@
+Note
+====
+
+This driver supersedes the NCT6775F and NCT6776F support in the W83627EHF
+driver.
+
+Kernel driver NCT6775
+=====================
+
+Supported chips:
+  * Nuvoton NCT5572D/NCT6771F/NCT6772F/NCT6775F/W83677HG-I
+    Prefix: 'nct6775'
+    Addresses scanned: ISA address retrieved from Super I/O registers
+    Datasheet: Available from Nuvoton upon request
+  * Nuvoton NCT5577D/NCT6776D/NCT6776F
+    Prefix: 'nct6776'
+    Addresses scanned: ISA address retrieved from Super I/O registers
+    Datasheet: Available from Nuvoton upon request
+  * Nuvoton NCT5532D/NCT6779D
+    Prefix: 'nct6779'
+    Addresses scanned: ISA address retrieved from Super I/O registers
+    Datasheet: Available from Nuvoton upon request
+
+Authors:
+        Guenter Roeck <linux@roeck-us.net>
+
+Description
+-----------
+
+This driver implements support for the Nuvoton NCT6775F, NCT6776F, and NCT6779D
+and compatible super I/O chips.
+
+The chips support up to 25 temperature monitoring sources. Up to 6 of those are
+direct temperature sensor inputs, the others are special sources such as PECI,
+PCH, and SMBUS. Depending on the chip type, 2 to 6 of the temperature sources
+can be monitored and compared against minimum, maximum, and critical
+temperatures. The driver reports up to 10 of the temperatures to the user.
+There are 4 to 5 fan rotation speed sensors, 8 to 15 analog voltage sensors,
+one VID, alarms with beep warnings (control unimplemented), and some automatic
+fan regulation strategies (plus manual fan control mode).
+
+The temperature sensor sources on all chips are configurable. The configured
+source for each of the temperature sensors is provided in tempX_label.
+
+Temperatures are measured in degrees Celsius and measurement resolution is
+either 1 degC or 0.5 degC, depending on the temperature source and
+configuration. An alarm is triggered when the temperature gets higher than
+the high limit; it stays on until the temperature falls below the hysteresis
+value. Alarms are only supported for temp1 to temp6, depending on the chip type.
+
+Fan rotation speeds are reported in RPM (rotations per minute). An alarm is
+triggered if the rotation speed has dropped below a programmable limit. On
+NCT6775F, fan readings can be divided by a programmable divider (1, 2, 4, 8,
+16, 32, 64 or 128) to give the readings more range or accuracy; the other chips
+do not have a fan speed divider. The driver sets the most suitable fan divisor
+itself; specifically, it increases the divider value each time a fan speed
+reading returns an invalid value, and it reduces it if the fan speed reading
+is lower than optimal. Some fans might not be present because they share pins
+with other functions.
+
+Voltage sensors (also known as IN sensors) report their values in millivolts.
+An alarm is triggered if the voltage has crossed a programmable minimum
+or maximum limit.
+
+The driver supports automatic fan control mode known as Thermal Cruise.
+In this mode, the chip attempts to keep the measured temperature in a
+predefined temperature range. If the temperature goes out of range, fan
+is driven slower/faster to reach the predefined range again.
+
+The mode works for fan1-fan5.
+
+sysfs attributes
+----------------
+
+pwm[1-5] - this file stores PWM duty cycle or DC value (fan speed) in range:
+	   0 (lowest speed) to 255 (full)
+
+pwm[1-5]_enable - this file controls mode of fan/temperature control:
+	* 0 Fan control disabled (fans set to maximum speed)
+	* 1 Manual mode, write to pwm[0-5] any value 0-255
+	* 2 "Thermal Cruise" mode
+	* 3 "Fan Speed Cruise" mode
+	* 4 "Smart Fan III" mode (NCT6775F only)
+	* 5 "Smart Fan IV" mode
+
+pwm[1-5]_mode - controls if output is PWM or DC level
+        * 0 DC output
+        * 1 PWM output
+
+Common fan control attributes
+-----------------------------
+
+pwm[1-5]_temp_sel	Temperature source. Value is temperature sensor index.
+			For example, select '1' for temp1_input.
+pwm[1-5]_weight_temp_sel
+			Secondary temperature source. Value is temperature
+			sensor index. For example, select '1' for temp1_input.
+			Set to 0 to disable secondary temperature control.
+
+If secondary temperature functionality is enabled, it is controlled with the
+following attributes.
+
+pwm[1-5]_weight_duty_step
+			Duty step size.
+pwm[1-5]_weight_temp_step
+			Temperature step size. With each step over
+			temp_step_base, the value of weight_duty_step is added
+			to the current pwm value.
+pwm[1-5]_weight_temp_step_base
+			Temperature at which secondary temperature control kicks
+			in.
+pwm[1-5]_weight_temp_step_tol
+			Temperature step tolerance.
+
+Thermal Cruise mode (2)
+-----------------------
+
+If the temperature is in the range defined by:
+
+pwm[1-5]_target_temp	Target temperature, unit millidegree Celsius
+			(range 0 - 127000)
+pwm[1-5]_temp_tolerance
+			Target temperature tolerance, unit millidegree Celsius
+
+there are no changes to fan speed. Once the temperature leaves the interval, fan
+speed increases (if temperature is higher that desired) or decreases (if
+temperature is lower than desired), using the following limits and time
+intervals.
+
+pwm[1-5]_start		fan pwm start value (range 1 - 255), to start fan
+			when the temperature is above defined range.
+pwm[1-5]_floor		lowest fan pwm (range 0 - 255) if temperature is below
+			the defined range. If set to 0, the fan is expected to
+			stop if the temperature is below the defined range.
+pwm[1-5]_step_up_time	milliseconds before fan speed is increased
+pwm[1-5]_step_down_time	milliseconds before fan speed is decreased
+pwm[1-5]_stop_time	how many milliseconds must elapse to switch
+			corresponding fan off (when the temperature was below
+			defined range).
+
+Speed Cruise mode (3)
+---------------------
+
+This modes tries to keep the fan speed constant.
+
+fan[1-5]_target		Target fan speed
+fan[1-5]_tolerance
+			Target speed tolerance
+
+
+Untested; use at your own risk.
+
+Smart Fan IV mode (5)
+---------------------
+
+This mode offers multiple slopes to control the fan speed. The slopes can be
+controlled by setting the pwm and temperature attributes. When the temperature
+rises, the chip will calculate the DC/PWM output based on the current slope.
+There are up to seven data points depending on the chip type. Subsequent data
+points should be set to higher temperatures and higher pwm values to achieve
+higher fan speeds with increasing temperature. The last data point reflects
+critical temperature mode, in which the fans should run at full speed.
+
+pwm[1-5]_auto_point[1-7]_pwm
+			pwm value to be set if temperature reaches matching
+			temperature range.
+pwm[1-5]_auto_point[1-7]_temp
+			Temperature over which the matching pwm is enabled.
+pwm[1-5]_temp_tolerance
+			Temperature tolerance, unit millidegree Celsius
+pwm[1-5]_crit_temp_tolerance
+			Temperature tolerance for critical temperature,
+			unit millidegree Celsius
+
+pwm[1-5]_step_up_time	milliseconds before fan speed is increased
+pwm[1-5]_step_down_time	milliseconds before fan speed is decreased
+
+Usage Notes
+-----------
+
+On various ASUS boards with NCT6776F, it appears that CPUTIN is not really
+connected to anything and floats, or that it is connected to some non-standard
+temperature measurement device. As a result, the temperature reported on CPUTIN
+will not reflect a usable value. It often reports unreasonably high
+temperatures, and in some cases the reported temperature declines if the actual
+temperature increases (similar to the raw PECI temperature value - see PECI
+specification for details). CPUTIN should therefore be be ignored on ASUS
+boards. The CPU temperature on ASUS boards is reported from PECI 0.
diff --git a/Documentation/hwmon/sht15 b/Documentation/hwmon/sht15
index 02850bdfac1..778987d1856 100644
--- a/Documentation/hwmon/sht15
+++ b/Documentation/hwmon/sht15
@@ -40,7 +40,7 @@ bits for humidity, or 12 bits for temperature and 8 bits for humidity.
 The humidity calibration coefficients are programmed into an OTP memory on the
 chip. These coefficients are used to internally calibrate the signals from the
 sensors. Disabling the reload of those coefficients allows saving 10ms for each
-measurement and decrease power consumption, while loosing on precision.
+measurement and decrease power consumption, while losing on precision.
 
 Some options may be set directly in the sht15_platform_data structure
 or via sysfs attributes.
diff --git a/Documentation/hwmon/tmp401 b/Documentation/hwmon/tmp401
index 9fc44724921..f91e3fa7e5e 100644
--- a/Documentation/hwmon/tmp401
+++ b/Documentation/hwmon/tmp401
@@ -8,8 +8,16 @@ Supported chips:
     Datasheet: http://focus.ti.com/docs/prod/folders/print/tmp401.html
   * Texas Instruments TMP411
     Prefix: 'tmp411'
-    Addresses scanned: I2C 0x4c
+    Addresses scanned: I2C 0x4c, 0x4d, 0x4e
     Datasheet: http://focus.ti.com/docs/prod/folders/print/tmp411.html
+  * Texas Instruments TMP431
+    Prefix: 'tmp431'
+    Addresses scanned: I2C 0x4c, 0x4d
+    Datasheet: http://focus.ti.com/docs/prod/folders/print/tmp431.html
+  * Texas Instruments TMP432
+    Prefix: 'tmp432'
+    Addresses scanned: I2C 0x4c, 0x4d
+    Datasheet: http://focus.ti.com/docs/prod/folders/print/tmp432.html
 
 Authors:
          Hans de Goede <hdegoede@redhat.com>
@@ -18,19 +26,19 @@ Authors:
 Description
 -----------
 
-This driver implements support for Texas Instruments TMP401 and
-TMP411 chips. These chips implements one remote and one local
-temperature sensor. Temperature is measured in degrees
+This driver implements support for Texas Instruments TMP401, TMP411,
+TMP431, and TMP432 chips. These chips implement one or two remote and
+one local temperature sensors. Temperature is measured in degrees
 Celsius. Resolution of the remote sensor is 0.0625 degree. Local
 sensor resolution can be set to 0.5, 0.25, 0.125 or 0.0625 degree (not
 supported by the driver so far, so using the default resolution of 0.5
 degree).
 
 The driver provides the common sysfs-interface for temperatures (see
-/Documentation/hwmon/sysfs-interface under Temperatures).
+Documentation/hwmon/sysfs-interface under Temperatures).
 
-The TMP411 chip is compatible with TMP401. It provides some additional
-features.
+The TMP411 and TMP431 chips are compatible with TMP401. TMP411 provides
+some additional features.
 
 * Minimum and Maximum temperature measured since power-on, chip-reset
 
@@ -40,3 +48,6 @@ features.
 
   Exported via sysfs attribute temp_reset_history. Writing 1 to this
   file triggers a reset.
+
+TMP432 is compatible with TMP401 and TMP431. It supports two external
+temperature sensors.
diff --git a/Documentation/hwmon/zl6100 b/Documentation/hwmon/zl6100
index 756b57c6b73..33908a4d68f 100644
--- a/Documentation/hwmon/zl6100
+++ b/Documentation/hwmon/zl6100
@@ -125,7 +125,7 @@ in2_label		"vmon"
 in2_input		Measured voltage on VMON (ZL2004) or VDRV (ZL9101M,
 			ZL9117M) pin. Reported voltage is 16x the voltage on the
 			pin (adjusted internally by the chip).
-in2_lcrit		Critical minumum VMON/VDRV Voltage.
+in2_lcrit		Critical minimum VMON/VDRV Voltage.
 in2_crit		Critical maximum VMON/VDRV voltage.
 in2_lcrit_alarm		VMON/VDRV voltage critical low alarm.
 in2_crit_alarm		VMON/VDRV voltage critical high alarm.
diff --git a/Documentation/ia64/err_inject.txt b/Documentation/ia64/err_inject.txt
index 223e4f0582d..9f651c18142 100644
--- a/Documentation/ia64/err_inject.txt
+++ b/Documentation/ia64/err_inject.txt
@@ -882,7 +882,7 @@ int err_inj()
 			cpu=parameters[i].cpu;
 			k = cpu%64;
 			j = cpu/64;
-			mask[j]=1<<k;
+			mask[j] = 1UL << k;
 
 			if (sched_setaffinity(0, MASK_SIZE*8, mask)==-1) {
 				perror("Error sched_setaffinity:");
diff --git a/Documentation/ioctl/ioctl-number.txt b/Documentation/ioctl/ioctl-number.txt
index 3210540f8bd..237acab169d 100644
--- a/Documentation/ioctl/ioctl-number.txt
+++ b/Documentation/ioctl/ioctl-number.txt
@@ -131,6 +131,7 @@ Code  Seq#(hex)	Include File		Comments
 'H'	40-4F	sound/hdspm.h		conflict!
 'H'	40-4F	sound/hdsp.h		conflict!
 'H'	90	sound/usb/usx2y/usb_stream.h
+'H'	A0	uapi/linux/usb/cdc-wdm.h
 'H'	C0-F0	net/bluetooth/hci.h	conflict!
 'H'	C0-DF	net/bluetooth/hidp/hidp.h	conflict!
 'H'	C0-DF	net/bluetooth/cmtp/cmtp.h	conflict!
diff --git a/Documentation/iostats.txt b/Documentation/iostats.txt
index c76c21d87e8..65f694f2d1c 100644
--- a/Documentation/iostats.txt
+++ b/Documentation/iostats.txt
@@ -71,6 +71,8 @@ Field  4 -- # of milliseconds spent reading
     measured from __make_request() to end_that_request_last()).
 Field  5 -- # of writes completed
     This is the total number of writes completed successfully.
+Field  6 -- # of writes merged
+    See the description of field 2.
 Field  7 -- # of sectors written
     This is the total number of sectors written successfully.
 Field  8 -- # of milliseconds spent writing
diff --git a/Documentation/kdump/kdump.txt b/Documentation/kdump/kdump.txt
index 13f1aa09b93..9c7fd988e29 100644
--- a/Documentation/kdump/kdump.txt
+++ b/Documentation/kdump/kdump.txt
@@ -297,6 +297,7 @@ Boot into System Kernel
    On ia64, 256M@256M is a generous value that typically works.
    The region may be automatically placed on ia64, see the
    dump-capture kernel config option notes above.
+   If use sparse memory, the size should be rounded to GRANULE boundaries.
 
    On s390x, typically use "crashkernel=xxM". The value of xx is dependent
    on the memory consumption of the kdump system. In general this is not
diff --git a/Documentation/kernel-parameters.txt b/Documentation/kernel-parameters.txt
index 4609e81dbc3..8c01a0218a1 100644
--- a/Documentation/kernel-parameters.txt
+++ b/Documentation/kernel-parameters.txt
@@ -44,6 +44,7 @@ parameter is applicable:
 	AVR32	AVR32 architecture is enabled.
 	AX25	Appropriate AX.25 support is enabled.
 	BLACKFIN Blackfin architecture is enabled.
+	CLK	Common clock infrastructure is enabled.
 	DRM	Direct Rendering Management support is enabled.
 	DYNAMIC_DEBUG Build in debug messages and enable them at runtime
 	EDD	BIOS Enhanced Disk Drive Services (EDD) is enabled
@@ -320,6 +321,13 @@ bytes respectively. Such letter suffixes can also be entirely omitted.
 			on: enable for both 32- and 64-bit processes
 			off: disable for both 32- and 64-bit processes
 
+	alloc_snapshot	[FTRACE]
+			Allocate the ftrace snapshot buffer on boot up when the
+			main buffer is allocated. This is handy if debugging
+			and you need to use tracing_snapshot() on boot up, and
+			do not want to use tracing_snapshot_alloc() as it needs
+			to be done where GFP_KERNEL allocations are allowed.
+
 	amd_iommu=	[HW,X86-64]
 			Pass parameters to the AMD IOMMU driver in the system.
 			Possible values are:
@@ -465,6 +473,13 @@ bytes respectively. Such letter suffixes can also be entirely omitted.
 
 	cio_ignore=	[S390]
 			See Documentation/s390/CommonIO for details.
+	clk_ignore_unused
+			[CLK]
+			Keep all clocks already enabled by bootloader on,
+			even if no driver has claimed them. This is useful
+			for debug and development, but should not be
+			needed on a platform with proper driver support.
+			For more information, see Documentation/clk.txt.
 
 	clock=		[BUGS=X86-32, HW] gettimeofday clocksource override.
 			[Deprecated]
@@ -596,9 +611,6 @@ bytes respectively. Such letter suffixes can also be entirely omitted.
 			is selected automatically. Check
 			Documentation/kdump/kdump.txt for further details.
 
-	crashkernel_low=size[KMG]
-			[KNL, x86] parts under 4G.
-
 	crashkernel=range1:size1[,range2:size2,...][@offset]
 			[KNL] Same as above, but depends on the memory
 			in the running system. The syntax of range is
@@ -606,6 +618,26 @@ bytes respectively. Such letter suffixes can also be entirely omitted.
 			a memory unit (amount[KMG]). See also
 			Documentation/kdump/kdump.txt for an example.
 
+	crashkernel=size[KMG],high
+			[KNL, x86_64] range could be above 4G. Allow kernel
+			to allocate physical memory region from top, so could
+			be above 4G if system have more than 4G ram installed.
+			Otherwise memory region will be allocated below 4G, if
+			available.
+			It will be ignored if crashkernel=X is specified.
+	crashkernel=size[KMG],low
+			[KNL, x86_64] range under 4G. When crashkernel=X,high
+			is passed, kernel could allocate physical memory region
+			above 4G, that cause second kernel crash on system
+			that require some amount of low memory, e.g. swiotlb
+			requires at least 64M+32K low memory.  Kernel would
+			try to allocate 72M below 4G automatically.
+			This one let user to specify own low range under 4G
+			for second kernel instead.
+			0: to disable low allocation.
+			It will be ignored when crashkernel=X,high is not used
+			or memory reserved is below 4G.
+
 	cs89x0_dma=	[HW,NET]
 			Format: <dma>
 
@@ -757,19 +789,31 @@ bytes respectively. Such letter suffixes can also be entirely omitted.
 			(mmio) or 32-bit (mmio32).
 			The options are the same as for ttyS, above.
 
-	earlyprintk=	[X86,SH,BLACKFIN]
+	earlyprintk=	[X86,SH,BLACKFIN,ARM]
 			earlyprintk=vga
 			earlyprintk=xen
 			earlyprintk=serial[,ttySn[,baudrate]]
+			earlyprintk=serial[,0x...[,baudrate]]
 			earlyprintk=ttySn[,baudrate]
 			earlyprintk=dbgp[debugController#]
 
+			earlyprintk is useful when the kernel crashes before
+			the normal console is initialized. It is not enabled by
+			default because it has some cosmetic problems.
+
 			Append ",keep" to not disable it when the real console
 			takes over.
 
 			Only vga or serial or usb debug port at a time.
 
-			Currently only ttyS0 and ttyS1 are supported.
+			Currently only ttyS0 and ttyS1 may be specified by
+			name.  Other I/O ports may be explicitly specified
+			on some architectures (x86 and arm at least) by
+			replacing ttySn with an I/O port address, like this:
+				earlyprintk=serial,0x1008,115200
+			You can find the port for a given device in
+			/proc/tty/driver/serial:
+				2: uart:ST16650V2 port:00001008 irq:18 ...
 
 			Interaction with the standard serial driver is not
 			very good.
@@ -788,6 +832,12 @@ bytes respectively. Such letter suffixes can also be entirely omitted.
 	edd=		[EDD]
 			Format: {"off" | "on" | "skip[mbr]"}
 
+	efi_no_storage_paranoia [EFI; X86]
+			Using this parameter you can use more than 50% of
+			your efi variable storage. Use this parameter only if
+			you are really sure that your UEFI does sane gc and
+			fulfills the spec otherwise your board may brick.
+
 	eisa_irq_edge=	[PARISC,HW]
 			See header of drivers/parisc/eisa.c.
 
@@ -1625,7 +1675,7 @@ bytes respectively. Such letter suffixes can also be entirely omitted.
 	module.sig_enforce
 			[KNL] When CONFIG_MODULE_SIG is set, this means that
 			modules without (valid) signatures will fail to load.
-			Note that if CONFIG_MODULE_SIG_ENFORCE is set, that
+			Note that if CONFIG_MODULE_SIG_FORCE is set, that
 			is always true, so this option does nothing.
 
 	mousedev.tap_time=
@@ -1974,8 +2024,6 @@ bytes respectively. Such letter suffixes can also be entirely omitted.
 	noreplace-smp	[X86-32,SMP] Don't replace SMP instructions
 			with UP alternatives
 
-	noresidual	[PPC] Don't use residual data on PReP machines.
-
 	nordrand	[X86] Disable the direct use of the RDRAND
 			instruction even if it is supported by the
 			processor.  RDRAND is still available to user
@@ -2461,9 +2509,12 @@ bytes respectively. Such letter suffixes can also be entirely omitted.
 			In kernels built with CONFIG_RCU_NOCB_CPU=y, set
 			the specified list of CPUs to be no-callback CPUs.
 			Invocation of these CPUs' RCU callbacks will
-			be offloaded to "rcuoN" kthreads created for
-			that purpose.  This reduces OS jitter on the
+			be offloaded to "rcuox/N" kthreads created for
+			that purpose, where "x" is "b" for RCU-bh, "p"
+			for RCU-preempt, and "s" for RCU-sched, and "N"
+			is the CPU number.  This reduces OS jitter on the
 			offloaded CPUs, which can be useful for HPC and
+
 			real-time workloads.  It can also improve energy
 			efficiency for asymmetric multiprocessors.
 
@@ -2487,6 +2538,17 @@ bytes respectively. Such letter suffixes can also be entirely omitted.
 			leaf rcu_node structure.  Useful for very large
 			systems.
 
+	rcutree.jiffies_till_first_fqs= [KNL,BOOT]
+			Set delay from grace-period initialization to
+			first attempt to force quiescent states.
+			Units are jiffies, minimum value is zero,
+			and maximum value is HZ.
+
+	rcutree.jiffies_till_next_fqs= [KNL,BOOT]
+			Set delay between subsequent attempts to force
+			quiescent states.  Units are jiffies, minimum
+			value is one, and maximum value is HZ.
+
 	rcutree.qhimark=	[KNL,BOOT]
 			Set threshold of queued
 			RCU callbacks over which batch limiting is disabled.
@@ -2501,16 +2563,15 @@ bytes respectively. Such letter suffixes can also be entirely omitted.
 	rcutree.rcu_cpu_stall_timeout= [KNL,BOOT]
 			Set timeout for RCU CPU stall warning messages.
 
-	rcutree.jiffies_till_first_fqs= [KNL,BOOT]
-			Set delay from grace-period initialization to
-			first attempt to force quiescent states.
-			Units are jiffies, minimum value is zero,
-			and maximum value is HZ.
+	rcutree.rcu_idle_gp_delay=	[KNL,BOOT]
+			Set wakeup interval for idle CPUs that have
+			RCU callbacks (RCU_FAST_NO_HZ=y).
 
-	rcutree.jiffies_till_next_fqs= [KNL,BOOT]
-			Set delay between subsequent attempts to force
-			quiescent states.  Units are jiffies, minimum
-			value is one, and maximum value is HZ.
+	rcutree.rcu_idle_lazy_gp_delay=	[KNL,BOOT]
+			Set wakeup interval for idle CPUs that have
+			only "lazy" RCU callbacks (RCU_FAST_NO_HZ=y).
+			Lazy RCU callbacks are those which RCU can
+			prove do nothing more than free memory.
 
 	rcutorture.fqs_duration= [KNL,BOOT]
 			Set duration of force_quiescent_state bursts.
@@ -3222,6 +3283,15 @@ bytes respectively. Such letter suffixes can also be entirely omitted.
 			or other driver-specific files in the
 			Documentation/watchdog/ directory.
 
+	workqueue.disable_numa
+			By default, all work items queued to unbound
+			workqueues are affine to the NUMA nodes they're
+			issued on, which results in better behavior in
+			general.  If NUMA affinity needs to be disabled for
+			whatever reason, this option can be used.  Note
+			that this also can be controlled per-workqueue for
+			workqueues visible under /sys/bus/workqueue/.
+
 	x2apic_phys	[X86-64,APIC] Use x2apic physical mode instead of
 			default x2apic cluster mode on platforms
 			supporting x2apic.
diff --git a/Documentation/misc-devices/mei/mei-client-bus.txt b/Documentation/misc-devices/mei/mei-client-bus.txt
new file mode 100644
index 00000000000..f83910a8ce7
--- /dev/null
+++ b/Documentation/misc-devices/mei/mei-client-bus.txt
@@ -0,0 +1,138 @@
+Intel(R) Management Engine (ME) Client bus API
+===============================================
+
+
+Rationale
+=========
+MEI misc character device is useful for dedicated applications to send and receive
+data to the many FW appliance found in Intel's ME from the user space.
+However for some of the ME functionalities it make sense to leverage existing software
+stack and expose them through existing kernel subsystems.
+
+In order to plug seamlessly into the kernel device driver model we add kernel virtual
+bus abstraction on top of the MEI driver. This allows implementing linux kernel drivers
+for the various MEI features as a stand alone entities found in their respective subsystem.
+Existing device drivers can even potentially be re-used by adding an MEI CL bus layer to
+the existing code.
+
+
+MEI CL bus API
+===========
+A driver implementation for an MEI Client is very similar to existing bus
+based device drivers. The driver registers itself as an MEI CL bus driver through
+the mei_cl_driver structure:
+
+struct mei_cl_driver {
+	struct device_driver driver;
+	const char *name;
+
+	const struct mei_cl_device_id *id_table;
+
+	int (*probe)(struct mei_cl_device *dev, const struct mei_cl_id *id);
+	int (*remove)(struct mei_cl_device *dev);
+};
+
+struct mei_cl_id {
+	char name[MEI_NAME_SIZE];
+	kernel_ulong_t driver_info;
+};
+
+The mei_cl_id structure allows the driver to bind itself against a device name.
+
+To actually register a driver on the ME Client bus one must call the mei_cl_add_driver()
+API. This is typically called at module init time.
+
+Once registered on the ME Client bus, a driver will typically try to do some I/O on
+this bus and this should be done through the mei_cl_send() and mei_cl_recv()
+routines. The latter is synchronous (blocks and sleeps until data shows up).
+In order for drivers to be notified of pending events waiting for them (e.g.
+an Rx event) they can register an event handler through the
+mei_cl_register_event_cb() routine. Currently only the MEI_EVENT_RX event
+will trigger an event handler call and the driver implementation is supposed
+to call mei_recv() from the event handler in order to fetch the pending
+received buffers.
+
+
+Example
+=======
+As a theoretical example let's pretend the ME comes with a "contact" NFC IP.
+The driver init and exit routines for this device would look like:
+
+#define CONTACT_DRIVER_NAME "contact"
+
+static struct mei_cl_device_id contact_mei_cl_tbl[] = {
+	{ CONTACT_DRIVER_NAME, },
+
+	/* required last entry */
+	{ }
+};
+MODULE_DEVICE_TABLE(mei_cl, contact_mei_cl_tbl);
+
+static struct mei_cl_driver contact_driver = {
+       .id_table = contact_mei_tbl,
+       .name = CONTACT_DRIVER_NAME,
+
+       .probe = contact_probe,
+       .remove = contact_remove,
+};
+
+static int contact_init(void)
+{
+	int r;
+
+	r = mei_cl_driver_register(&contact_driver);
+	if (r) {
+		pr_err(CONTACT_DRIVER_NAME ": driver registration failed\n");
+		return r;
+	}
+
+	return 0;
+}
+
+static void __exit contact_exit(void)
+{
+	mei_cl_driver_unregister(&contact_driver);
+}
+
+module_init(contact_init);
+module_exit(contact_exit);
+
+And the driver's simplified probe routine would look like that:
+
+int contact_probe(struct mei_cl_device *dev, struct mei_cl_device_id *id)
+{
+	struct contact_driver *contact;
+
+	[...]
+	mei_cl_enable_device(dev);
+
+	mei_cl_register_event_cb(dev, contact_event_cb, contact);
+
+	return 0;
+ }
+
+In the probe routine the driver first enable the MEI device and then registers
+an ME bus event handler which is as close as it can get to registering a
+threaded IRQ handler.
+The handler implementation will typically call some I/O routine depending on
+the pending events:
+
+#define MAX_NFC_PAYLOAD 128
+
+static void contact_event_cb(struct mei_cl_device *dev, u32 events,
+			     void *context)
+{
+	struct contact_driver *contact = context;
+
+	if (events & BIT(MEI_EVENT_RX)) {
+		u8 payload[MAX_NFC_PAYLOAD];
+		int payload_size;
+
+		payload_size = mei_recv(dev, payload, MAX_NFC_PAYLOAD);
+		if (payload_size <= 0)
+			return;
+
+		/* Hook to the NFC subsystem */
+		nfc_hci_recv_frame(contact->hdev, payload, payload_size);
+	}
+}
diff --git a/Documentation/networking/ipvs-sysctl.txt b/Documentation/networking/ipvs-sysctl.txt
index f2a2488f1bf..9573d0c48c6 100644
--- a/Documentation/networking/ipvs-sysctl.txt
+++ b/Documentation/networking/ipvs-sysctl.txt
@@ -15,6 +15,13 @@ amemthresh - INTEGER
         enabled and the variable is automatically set to 2, otherwise
         the strategy is disabled and the variable is  set  to 1.
 
+backup_only - BOOLEAN
+	0 - disabled (default)
+	not 0 - enabled
+
+	If set, disable the director function while the server is
+	in backup mode to avoid packet loops for DR/TUN methods.
+
 conntrack - BOOLEAN
 	0 - disabled (default)
 	not 0 - enabled
diff --git a/Documentation/pinctrl.txt b/Documentation/pinctrl.txt
index a2b57e0a1db..447fd4cd54e 100644
--- a/Documentation/pinctrl.txt
+++ b/Documentation/pinctrl.txt
@@ -736,6 +736,13 @@ All the above functions are mandatory to implement for a pinmux driver.
 Pin control interaction with the GPIO subsystem
 ===============================================
 
+Note that the following implies that the use case is to use a certain pin
+from the Linux kernel using the API in <linux/gpio.h> with gpio_request()
+and similar functions. There are cases where you may be using something
+that your datasheet calls "GPIO mode" but actually is just an electrical
+configuration for a certain device. See the section below named
+"GPIO mode pitfalls" for more details on this scenario.
+
 The public pinmux API contains two functions named pinctrl_request_gpio()
 and pinctrl_free_gpio(). These two functions shall *ONLY* be called from
 gpiolib-based drivers as part of their gpio_request() and
@@ -774,6 +781,111 @@ obtain the function "gpioN" where "N" is the global GPIO pin number if no
 special GPIO-handler is registered.
 
 
+GPIO mode pitfalls
+==================
+
+Sometime the developer may be confused by a datasheet talking about a pin
+being possible to set into "GPIO mode". It appears that what hardware
+engineers mean with "GPIO mode" is not necessarily the use case that is
+implied in the kernel interface <linux/gpio.h>: a pin that you grab from
+kernel code and then either listen for input or drive high/low to
+assert/deassert some external line.
+
+Rather hardware engineers think that "GPIO mode" means that you can
+software-control a few electrical properties of the pin that you would
+not be able to control if the pin was in some other mode, such as muxed in
+for a device.
+
+Example: a pin is usually muxed in to be used as a UART TX line. But during
+system sleep, we need to put this pin into "GPIO mode" and ground it.
+
+If you make a 1-to-1 map to the GPIO subsystem for this pin, you may start
+to think that you need to come up with something real complex, that the
+pin shall be used for UART TX and GPIO at the same time, that you will grab
+a pin control handle and set it to a certain state to enable UART TX to be
+muxed in, then twist it over to GPIO mode and use gpio_direction_output()
+to drive it low during sleep, then mux it over to UART TX again when you
+wake up and maybe even gpio_request/gpio_free as part of this cycle. This
+all gets very complicated.
+
+The solution is to not think that what the datasheet calls "GPIO mode"
+has to be handled by the <linux/gpio.h> interface. Instead view this as
+a certain pin config setting. Look in e.g. <linux/pinctrl/pinconf-generic.h>
+and you find this in the documentation:
+
+  PIN_CONFIG_OUTPUT: this will configure the pin in output, use argument
+     1 to indicate high level, argument 0 to indicate low level.
+
+So it is perfectly possible to push a pin into "GPIO mode" and drive the
+line low as part of the usual pin control map. So for example your UART
+driver may look like this:
+
+#include <linux/pinctrl/consumer.h>
+
+struct pinctrl          *pinctrl;
+struct pinctrl_state    *pins_default;
+struct pinctrl_state    *pins_sleep;
+
+pins_default = pinctrl_lookup_state(uap->pinctrl, PINCTRL_STATE_DEFAULT);
+pins_sleep = pinctrl_lookup_state(uap->pinctrl, PINCTRL_STATE_SLEEP);
+
+/* Normal mode */
+retval = pinctrl_select_state(pinctrl, pins_default);
+/* Sleep mode */
+retval = pinctrl_select_state(pinctrl, pins_sleep);
+
+And your machine configuration may look like this:
+--------------------------------------------------
+
+static unsigned long uart_default_mode[] = {
+    PIN_CONF_PACKED(PIN_CONFIG_DRIVE_PUSH_PULL, 0),
+};
+
+static unsigned long uart_sleep_mode[] = {
+    PIN_CONF_PACKED(PIN_CONFIG_OUTPUT, 0),
+};
+
+static struct pinctrl_map __initdata pinmap[] = {
+    PIN_MAP_MUX_GROUP("uart", PINCTRL_STATE_DEFAULT, "pinctrl-foo",
+                      "u0_group", "u0"),
+    PIN_MAP_CONFIGS_PIN("uart", PINCTRL_STATE_DEFAULT, "pinctrl-foo",
+                        "UART_TX_PIN", uart_default_mode),
+    PIN_MAP_MUX_GROUP("uart", PINCTRL_STATE_SLEEP, "pinctrl-foo",
+                      "u0_group", "gpio-mode"),
+    PIN_MAP_CONFIGS_PIN("uart", PINCTRL_STATE_SLEEP, "pinctrl-foo",
+                        "UART_TX_PIN", uart_sleep_mode),
+};
+
+foo_init(void) {
+    pinctrl_register_mappings(pinmap, ARRAY_SIZE(pinmap));
+}
+
+Here the pins we want to control are in the "u0_group" and there is some
+function called "u0" that can be enabled on this group of pins, and then
+everything is UART business as usual. But there is also some function
+named "gpio-mode" that can be mapped onto the same pins to move them into
+GPIO mode.
+
+This will give the desired effect without any bogus interaction with the
+GPIO subsystem. It is just an electrical configuration used by that device
+when going to sleep, it might imply that the pin is set into something the
+datasheet calls "GPIO mode" but that is not the point: it is still used
+by that UART device to control the pins that pertain to that very UART
+driver, putting them into modes needed by the UART. GPIO in the Linux
+kernel sense are just some 1-bit line, and is a different use case.
+
+How the registers are poked to attain the push/pull and output low
+configuration and the muxing of the "u0" or "gpio-mode" group onto these
+pins is a question for the driver.
+
+Some datasheets will be more helpful and refer to the "GPIO mode" as
+"low power mode" rather than anything to do with GPIO. This often means
+the same thing electrically speaking, but in this latter case the
+software engineers will usually quickly identify that this is some
+specific muxing/configuration rather than anything related to the GPIO
+API.
+
+
 Board/machine configuration
 ==================================
 
diff --git a/Documentation/printk-formats.txt b/Documentation/printk-formats.txt
index 6e953564de0..3af5ae6c9c1 100644
--- a/Documentation/printk-formats.txt
+++ b/Documentation/printk-formats.txt
@@ -17,6 +17,8 @@ Symbols/Function Pointers:
 	%pF	versatile_init+0x0/0x110
 	%pf	versatile_init
 	%pS	versatile_init+0x0/0x110
+	%pSR	versatile_init+0x9/0x110
+		(with __builtin_extract_return_addr() translation)
 	%ps	versatile_init
 	%pB	prev_fn_of_versatile_init+0x88/0x88
 
diff --git a/Documentation/s390/s390dbf.txt b/Documentation/s390/s390dbf.txt
index ae66f9b90a2..fcaf0b4efba 100644
--- a/Documentation/s390/s390dbf.txt
+++ b/Documentation/s390/s390dbf.txt
@@ -143,7 +143,8 @@ Parameter:     id:   handle for debug log
 
 Return Value:  none 
 
-Description:   frees memory for a debug log     
+Description:   frees memory for a debug log and removes all registered debug
+	       views.
                Must not be called within an interrupt handler 
 
 ---------------------------------------------------------------------------
diff --git a/Documentation/scsi/LICENSE.qla2xxx b/Documentation/scsi/LICENSE.qla2xxx
index 27a91cf43d6..5020b7b5a24 100644
--- a/Documentation/scsi/LICENSE.qla2xxx
+++ b/Documentation/scsi/LICENSE.qla2xxx
@@ -1,4 +1,4 @@
-Copyright (c) 2003-2012 QLogic Corporation
+Copyright (c) 2003-2013 QLogic Corporation
 QLogic Linux FC-FCoE Driver
 
 This program includes a device driver for Linux 3.x.
diff --git a/Documentation/sound/alsa/ALSA-Configuration.txt b/Documentation/sound/alsa/ALSA-Configuration.txt
index 4499bd94886..95731a08f25 100644
--- a/Documentation/sound/alsa/ALSA-Configuration.txt
+++ b/Documentation/sound/alsa/ALSA-Configuration.txt
@@ -890,9 +890,8 @@ Prior to version 0.9.0rc4 options had a 'snd_' prefix. This was removed.
     enable_msi	- Enable Message Signaled Interrupt (MSI) (default = off)
     power_save	- Automatic power-saving timeout (in second, 0 =
 		disable)
-    power_save_controller - Support runtime D3 of HD-audio controller
-		(-1 = on for supported chip (default), false = off,
-		 true = force to on even for unsupported hardware)
+    power_save_controller - Reset HD-audio controller in power-saving mode
+		(default = on)
     align_buffer_size - Force rounding of buffer/period sizes to multiples
     		      of 128 bytes. This is more efficient in terms of memory
 		      access but isn't required by the HDA spec and prevents
diff --git a/Documentation/sysctl/vm.txt b/Documentation/sysctl/vm.txt
index 078701fdbd4..dcc75a9ed91 100644
--- a/Documentation/sysctl/vm.txt
+++ b/Documentation/sysctl/vm.txt
@@ -18,6 +18,7 @@ files can be found in mm/swap.c.
 
 Currently, these files are in /proc/sys/vm:
 
+- admin_reserve_kbytes
 - block_dump
 - compact_memory
 - dirty_background_bytes
@@ -53,11 +54,41 @@ Currently, these files are in /proc/sys/vm:
 - percpu_pagelist_fraction
 - stat_interval
 - swappiness
+- user_reserve_kbytes
 - vfs_cache_pressure
 - zone_reclaim_mode
 
 ==============================================================
 
+admin_reserve_kbytes
+
+The amount of free memory in the system that should be reserved for users
+with the capability cap_sys_admin.
+
+admin_reserve_kbytes defaults to min(3% of free pages, 8MB)
+
+That should provide enough for the admin to log in and kill a process,
+if necessary, under the default overcommit 'guess' mode.
+
+Systems running under overcommit 'never' should increase this to account
+for the full Virtual Memory Size of programs used to recover. Otherwise,
+root may not be able to log in to recover the system.
+
+How do you calculate a minimum useful reserve?
+
+sshd or login + bash (or some other shell) + top (or ps, kill, etc.)
+
+For overcommit 'guess', we can sum resident set sizes (RSS).
+On x86_64 this is about 8MB.
+
+For overcommit 'never', we can take the max of their virtual sizes (VSZ)
+and add the sum of their RSS.
+On x86_64 this is about 128MB.
+
+Changing this takes effect whenever an application requests memory.
+
+==============================================================
+
 block_dump
 
 block_dump enables block I/O debugging when set to a nonzero value. More
@@ -542,6 +573,7 @@ memory until it actually runs out.
 
 When this flag is 2, the kernel uses a "never overcommit"
 policy that attempts to prevent any overcommit of memory.
+Note that user_reserve_kbytes affects this policy.
 
 This feature can be very useful because there are a lot of
 programs that malloc() huge amounts of memory "just-in-case"
@@ -645,6 +677,24 @@ The default value is 60.
 
 ==============================================================
 
+- user_reserve_kbytes
+
+When overcommit_memory is set to 2, "never overommit" mode, reserve
+min(3% of current process size, user_reserve_kbytes) of free memory.
+This is intended to prevent a user from starting a single memory hogging
+process, such that they cannot recover (kill the hog).
+
+user_reserve_kbytes defaults to min(3% of the current process size, 128MB).
+
+If this is reduced to zero, then the user will be allowed to allocate
+all free memory with a single process, minus admin_reserve_kbytes.
+Any subsequent attempts to execute a command will result in
+"fork: Cannot allocate memory".
+
+Changing this takes effect whenever an application requests memory.
+
+==============================================================
+
 vfs_cache_pressure
 ------------------
 
diff --git a/Documentation/this_cpu_ops.txt b/Documentation/this_cpu_ops.txt
new file mode 100644
index 00000000000..1a4ce7e3e05
--- /dev/null
+++ b/Documentation/this_cpu_ops.txt
@@ -0,0 +1,205 @@
+this_cpu operations
+-------------------
+
+this_cpu operations are a way of optimizing access to per cpu
+variables associated with the *currently* executing processor through
+the use of segment registers (or a dedicated register where the cpu
+permanently stored the beginning of the per cpu area for a specific
+processor).
+
+The this_cpu operations add a per cpu variable offset to the processor
+specific percpu base and encode that operation in the instruction
+operating on the per cpu variable.
+
+This means there are no atomicity issues between the calculation of
+the offset and the operation on the data. Therefore it is not
+necessary to disable preempt or interrupts to ensure that the
+processor is not changed between the calculation of the address and
+the operation on the data.
+
+Read-modify-write operations are of particular interest. Frequently
+processors have special lower latency instructions that can operate
+without the typical synchronization overhead but still provide some
+sort of relaxed atomicity guarantee. The x86 for example can execute
+RMV (Read Modify Write) instructions like inc/dec/cmpxchg without the
+lock prefix and the associated latency penalty.
+
+Access to the variable without the lock prefix is not synchronized but
+synchronization is not necessary since we are dealing with per cpu
+data specific to the currently executing processor. Only the current
+processor should be accessing that variable and therefore there are no
+concurrency issues with other processors in the system.
+
+On x86 the fs: or the gs: segment registers contain the base of the
+per cpu area. It is then possible to simply use the segment override
+to relocate a per cpu relative address to the proper per cpu area for
+the processor. So the relocation to the per cpu base is encoded in the
+instruction via a segment register prefix.
+
+For example:
+
+	DEFINE_PER_CPU(int, x);
+	int z;
+
+	z = this_cpu_read(x);
+
+results in a single instruction
+
+	mov ax, gs:[x]
+
+instead of a sequence of calculation of the address and then a fetch
+from that address which occurs with the percpu operations. Before
+this_cpu_ops such sequence also required preempt disable/enable to
+prevent the kernel from moving the thread to a different processor
+while the calculation is performed.
+
+The main use of the this_cpu operations has been to optimize counter
+operations.
+
+	this_cpu_inc(x)
+
+results in the following single instruction (no lock prefix!)
+
+	inc gs:[x]
+
+instead of the following operations required if there is no segment
+register.
+
+	int *y;
+	int cpu;
+
+	cpu = get_cpu();
+	y = per_cpu_ptr(&x, cpu);
+	(*y)++;
+	put_cpu();
+
+Note that these operations can only be used on percpu data that is
+reserved for a specific processor. Without disabling preemption in the
+surrounding code this_cpu_inc() will only guarantee that one of the
+percpu counters is correctly incremented. However, there is no
+guarantee that the OS will not move the process directly before or
+after the this_cpu instruction is executed. In general this means that
+the value of the individual counters for each processor are
+meaningless. The sum of all the per cpu counters is the only value
+that is of interest.
+
+Per cpu variables are used for performance reasons. Bouncing cache
+lines can be avoided if multiple processors concurrently go through
+the same code paths.  Since each processor has its own per cpu
+variables no concurrent cacheline updates take place. The price that
+has to be paid for this optimization is the need to add up the per cpu
+counters when the value of the counter is needed.
+
+
+Special operations:
+-------------------
+
+	y = this_cpu_ptr(&x)
+
+Takes the offset of a per cpu variable (&x !) and returns the address
+of the per cpu variable that belongs to the currently executing
+processor.  this_cpu_ptr avoids multiple steps that the common
+get_cpu/put_cpu sequence requires. No processor number is
+available. Instead the offset of the local per cpu area is simply
+added to the percpu offset.
+
+
+
+Per cpu variables and offsets
+-----------------------------
+
+Per cpu variables have *offsets* to the beginning of the percpu
+area. They do not have addresses although they look like that in the
+code. Offsets cannot be directly dereferenced. The offset must be
+added to a base pointer of a percpu area of a processor in order to
+form a valid address.
+
+Therefore the use of x or &x outside of the context of per cpu
+operations is invalid and will generally be treated like a NULL
+pointer dereference.
+
+In the context of per cpu operations
+
+	x is a per cpu variable. Most this_cpu operations take a cpu
+	variable.
+
+	&x is the *offset* a per cpu variable. this_cpu_ptr() takes
+	the offset of a per cpu variable which makes this look a bit
+	strange.
+
+
+
+Operations on a field of a per cpu structure
+--------------------------------------------
+
+Let's say we have a percpu structure
+
+	struct s {
+		int n,m;
+	};
+
+	DEFINE_PER_CPU(struct s, p);
+
+
+Operations on these fields are straightforward
+
+	this_cpu_inc(p.m)
+
+	z = this_cpu_cmpxchg(p.m, 0, 1);
+
+
+If we have an offset to struct s:
+
+	struct s __percpu *ps = &p;
+
+	z = this_cpu_dec(ps->m);
+
+	z = this_cpu_inc_return(ps->n);
+
+
+The calculation of the pointer may require the use of this_cpu_ptr()
+if we do not make use of this_cpu ops later to manipulate fields:
+
+	struct s *pp;
+
+	pp = this_cpu_ptr(&p);
+
+	pp->m--;
+
+	z = pp->n++;
+
+
+Variants of this_cpu ops
+-------------------------
+
+this_cpu ops are interrupt safe. Some architecture do not support
+these per cpu local operations. In that case the operation must be
+replaced by code that disables interrupts, then does the operations
+that are guaranteed to be atomic and then reenable interrupts. Doing
+so is expensive. If there are other reasons why the scheduler cannot
+change the processor we are executing on then there is no reason to
+disable interrupts. For that purpose the __this_cpu operations are
+provided. For example.
+
+	__this_cpu_inc(x);
+
+Will increment x and will not fallback to code that disables
+interrupts on platforms that cannot accomplish atomicity through
+address relocation and a Read-Modify-Write operation in the same
+instruction.
+
+
+
+&this_cpu_ptr(pp)->n vs this_cpu_ptr(&pp->n)
+--------------------------------------------
+
+The first operation takes the offset and forms an address and then
+adds the offset of the n field.
+
+The second one first adds the two offsets and then does the
+relocation.  IMHO the second form looks cleaner and has an easier time
+with (). The second form also is consistent with the way
+this_cpu_read() and friends are used.
+
+
+Christoph Lameter, April 3rd, 2013
diff --git a/Documentation/trace/ftrace.txt b/Documentation/trace/ftrace.txt
index a372304aef1..bfe8c29b1f1 100644
--- a/Documentation/trace/ftrace.txt
+++ b/Documentation/trace/ftrace.txt
@@ -8,6 +8,7 @@ Copyright 2008 Red Hat Inc.
 Reviewers:   Elias Oltmanns, Randy Dunlap, Andrew Morton,
 	     John Kacur, and David Teigland.
 Written for: 2.6.28-rc2
+Updated for: 3.10
 
 Introduction
 ------------
@@ -17,13 +18,16 @@ designers of systems to find what is going on inside the kernel.
 It can be used for debugging or analyzing latencies and
 performance issues that take place outside of user-space.
 
-Although ftrace is the function tracer, it also includes an
-infrastructure that allows for other types of tracing. Some of
-the tracers that are currently in ftrace include a tracer to
-trace context switches, the time it takes for a high priority
-task to run after it was woken up, the time interrupts are
-disabled, and more (ftrace allows for tracer plugins, which
-means that the list of tracers can always grow).
+Although ftrace is typically considered the function tracer, it
+is really a frame work of several assorted tracing utilities.
+There's latency tracing to examine what occurs between interrupts
+disabled and enabled, as well as for preemption and from a time
+a task is woken to the task is actually scheduled in.
+
+One of the most common uses of ftrace is the event tracing.
+Through out the kernel is hundreds of static event points that
+can be enabled via the debugfs file system to see what is
+going on in certain parts of the kernel.
 
 
 Implementation Details
@@ -61,7 +65,7 @@ the extended "/sys/kernel/debug/tracing" path name.
 
 That's it! (assuming that you have ftrace configured into your kernel)
 
-After mounting the debugfs, you can see a directory called
+After mounting debugfs, you can see a directory called
 "tracing".  This directory contains the control and output files
 of ftrace. Here is a list of some of the key files:
 
@@ -84,7 +88,9 @@ of ftrace. Here is a list of some of the key files:
 
 	This sets or displays whether writing to the trace
 	ring buffer is enabled. Echo 0 into this file to disable
-	the tracer or 1 to enable it.
+	the tracer or 1 to enable it. Note, this only disables
+	writing to the ring buffer, the tracing overhead may
+	still be occurring.
 
   trace:
 
@@ -109,7 +115,15 @@ of ftrace. Here is a list of some of the key files:
 
 	This file lets the user control the amount of data
 	that is displayed in one of the above output
-	files.
+	files. Options also exist to modify how a tracer
+	or events work (stack traces, timestamps, etc).
+
+  options:
+
+	This is a directory that has a file for every available
+	trace option (also in trace_options). Options may also be set
+	or cleared by writing a "1" or "0" respectively into the
+	corresponding file with the option name.
 
   tracing_max_latency:
 
@@ -121,10 +135,17 @@ of ftrace. Here is a list of some of the key files:
 	latency is greater than the value in this
 	file. (in microseconds)
 
+  tracing_thresh:
+
+	Some latency tracers will record a trace whenever the
+	latency is greater than the number in this file.
+	Only active when the file contains a number greater than 0.
+	(in microseconds)
+
   buffer_size_kb:
 
 	This sets or displays the number of kilobytes each CPU
-	buffer can hold. The tracer buffers are the same size
+	buffer holds. By default, the trace buffers are the same size
 	for each CPU. The displayed number is the size of the
 	CPU buffer and not total size of all buffers. The
 	trace buffers are allocated in pages (blocks of memory
@@ -133,16 +154,30 @@ of ftrace. Here is a list of some of the key files:
 	than requested, the rest of the page will be used,
 	making the actual allocation bigger than requested.
 	( Note, the size may not be a multiple of the page size
-	  due to buffer management overhead. )
+	  due to buffer management meta-data. )
 
-	This can only be updated when the current_tracer
-	is set to "nop".
+  buffer_total_size_kb:
+
+	This displays the total combined size of all the trace buffers.
+
+  free_buffer:
+
+	If a process is performing the tracing, and the ring buffer
+	should be shrunk "freed" when the process is finished, even
+	if it were to be killed by a signal, this file can be used
+	for that purpose. On close of this file, the ring buffer will
+	be resized to its minimum size. Having a process that is tracing
+	also open this file, when the process exits its file descriptor
+	for this file will be closed, and in doing so, the ring buffer
+	will be "freed".
+
+	It may also stop tracing if disable_on_free option is set.
 
   tracing_cpumask:
 
 	This is a mask that lets the user only trace
-	on specified CPUS. The format is a hex string
-	representing the CPUS.
+	on specified CPUs. The format is a hex string
+	representing the CPUs.
 
   set_ftrace_filter:
 
@@ -183,6 +218,261 @@ of ftrace. Here is a list of some of the key files:
 	"set_ftrace_notrace". (See the section "dynamic ftrace"
 	below for more details.)
 
+  enabled_functions:
+
+	This file is more for debugging ftrace, but can also be useful
+	in seeing if any function has a callback attached to it.
+	Not only does the trace infrastructure use ftrace function
+	trace utility, but other subsystems might too. This file
+	displays all functions that have a callback attached to them
+	as well as the number of callbacks that have been attached.
+	Note, a callback may also call multiple functions which will
+	not be listed in this count.
+
+	If the callback registered to be traced by a function with
+	the "save regs" attribute (thus even more overhead), a 'R'
+	will be displayed on the same line as the function that
+	is returning registers.
+
+  function_profile_enabled:
+
+	When set it will enable all functions with either the function
+	tracer, or if enabled, the function graph tracer. It will
+	keep a histogram of the number of functions that were called
+	and if run with the function graph tracer, it will also keep
+	track of the time spent in those functions. The histogram
+	content can be displayed in the files:
+
+	trace_stats/function<cpu> ( function0, function1, etc).
+
+  trace_stats:
+
+	A directory that holds different tracing stats.
+
+  kprobe_events:
+ 
+	Enable dynamic trace points. See kprobetrace.txt.
+
+  kprobe_profile:
+
+	Dynamic trace points stats. See kprobetrace.txt.
+
+  max_graph_depth:
+
+	Used with the function graph tracer. This is the max depth
+	it will trace into a function. Setting this to a value of
+	one will show only the first kernel function that is called
+	from user space.
+
+  printk_formats:
+
+	This is for tools that read the raw format files. If an event in
+	the ring buffer references a string (currently only trace_printk()
+	does this), only a pointer to the string is recorded into the buffer
+	and not the string itself. This prevents tools from knowing what
+	that string was. This file displays the string and address for
+	the string allowing tools to map the pointers to what the
+	strings were.
+
+  saved_cmdlines:
+
+	Only the pid of the task is recorded in a trace event unless
+	the event specifically saves the task comm as well. Ftrace
+	makes a cache of pid mappings to comms to try to display
+	comms for events. If a pid for a comm is not listed, then
+	"<...>" is displayed in the output.
+
+  snapshot:
+
+	This displays the "snapshot" buffer and also lets the user
+	take a snapshot of the current running trace.
+	See the "Snapshot" section below for more details.
+
+  stack_max_size:
+
+	When the stack tracer is activated, this will display the
+	maximum stack size it has encountered.
+	See the "Stack Trace" section below.
+
+  stack_trace:
+
+	This displays the stack back trace of the largest stack
+	that was encountered when the stack tracer is activated.
+	See the "Stack Trace" section below.
+
+  stack_trace_filter:
+
+	This is similar to "set_ftrace_filter" but it limits what
+	functions the stack tracer will check.
+
+  trace_clock:
+
+	Whenever an event is recorded into the ring buffer, a
+	"timestamp" is added. This stamp comes from a specified
+	clock. By default, ftrace uses the "local" clock. This
+	clock is very fast and strictly per cpu, but on some
+	systems it may not be monotonic with respect to other
+	CPUs. In other words, the local clocks may not be in sync
+	with local clocks on other CPUs.
+
+	Usual clocks for tracing:
+
+	  # cat trace_clock
+	  [local] global counter x86-tsc
+
+	  local: Default clock, but may not be in sync across CPUs
+
+	  global: This clock is in sync with all CPUs but may
+	  	  be a bit slower than the local clock.
+
+	  counter: This is not a clock at all, but literally an atomic
+	  	   counter. It counts up one by one, but is in sync
+		   with all CPUs. This is useful when you need to
+		   know exactly the order events occurred with respect to
+		   each other on different CPUs.
+
+	  uptime: This uses the jiffies counter and the time stamp
+	  	  is relative to the time since boot up.
+
+	  perf: This makes ftrace use the same clock that perf uses.
+	  	Eventually perf will be able to read ftrace buffers
+		and this will help out in interleaving the data.
+
+	  x86-tsc: Architectures may define their own clocks. For
+	  	   example, x86 uses its own TSC cycle clock here.
+
+	To set a clock, simply echo the clock name into this file.
+
+	  echo global > trace_clock
+
+  trace_marker:
+
+	This is a very useful file for synchronizing user space
+	with events happening in the kernel. Writing strings into
+	this file will be written into the ftrace buffer.
+
+	It is useful in applications to open this file at the start
+	of the application and just reference the file descriptor
+	for the file.
+
+	void trace_write(const char *fmt, ...)
+	{
+		va_list ap;
+		char buf[256];
+		int n;
+
+		if (trace_fd < 0)
+			return;
+
+		va_start(ap, fmt);
+		n = vsnprintf(buf, 256, fmt, ap);
+		va_end(ap);
+
+		write(trace_fd, buf, n);
+	}
+
+	start:
+
+		trace_fd = open("trace_marker", WR_ONLY);
+
+  uprobe_events:
+ 
+	Add dynamic tracepoints in programs.
+	See uprobetracer.txt
+
+  uprobe_profile:
+
+	Uprobe statistics. See uprobetrace.txt
+
+  instances:
+
+	This is a way to make multiple trace buffers where different
+	events can be recorded in different buffers.
+	See "Instances" section below.
+
+  events:
+
+	This is the trace event directory. It holds event tracepoints
+	(also known as static tracepoints) that have been compiled
+	into the kernel. It shows what event tracepoints exist
+	and how they are grouped by system. There are "enable"
+	files at various levels that can enable the tracepoints
+	when a "1" is written to them.
+
+	See events.txt for more information.
+
+  per_cpu:
+
+	This is a directory that contains the trace per_cpu information.
+
+  per_cpu/cpu0/buffer_size_kb:
+
+	The ftrace buffer is defined per_cpu. That is, there's a separate
+	buffer for each CPU to allow writes to be done atomically,
+	and free from cache bouncing. These buffers may have different
+	size buffers. This file is similar to the buffer_size_kb
+	file, but it only displays or sets the buffer size for the
+	specific CPU. (here cpu0).
+
+  per_cpu/cpu0/trace:
+
+	This is similar to the "trace" file, but it will only display
+	the data specific for the CPU. If written to, it only clears
+	the specific CPU buffer.
+
+  per_cpu/cpu0/trace_pipe
+
+	This is similar to the "trace_pipe" file, and is a consuming
+	read, but it will only display (and consume) the data specific
+	for the CPU.
+
+  per_cpu/cpu0/trace_pipe_raw
+
+	For tools that can parse the ftrace ring buffer binary format,
+	the trace_pipe_raw file can be used to extract the data
+	from the ring buffer directly. With the use of the splice()
+	system call, the buffer data can be quickly transferred to
+	a file or to the network where a server is collecting the
+	data.
+
+	Like trace_pipe, this is a consuming reader, where multiple
+	reads will always produce different data.
+
+  per_cpu/cpu0/snapshot:
+
+	This is similar to the main "snapshot" file, but will only
+	snapshot the current CPU (if supported). It only displays
+	the content of the snapshot for a given CPU, and if
+	written to, only clears this CPU buffer.
+
+  per_cpu/cpu0/snapshot_raw:
+
+	Similar to the trace_pipe_raw, but will read the binary format
+	from the snapshot buffer for the given CPU.
+
+  per_cpu/cpu0/stats:
+
+	This displays certain stats about the ring buffer:
+
+	 entries: The number of events that are still in the buffer.
+
+	 overrun: The number of lost events due to overwriting when
+	 	  the buffer was full.
+
+	 commit overrun: Should always be zero.
+	 	This gets set if so many events happened within a nested
+		event (ring buffer is re-entrant), that it fills the
+		buffer and starts dropping events.
+
+	 bytes: Bytes actually read (not overwritten).
+
+	 oldest event ts: The oldest timestamp in the buffer
+
+	 now ts: The current timestamp
+
+	 dropped events: Events lost due to overwrite option being off.
+
+	 read events: The number of events read.
 
 The Tracers
 -----------
@@ -234,11 +524,6 @@ Here is the list of current tracers that may be configured.
         RT tasks (as the current "wakeup" does). This is useful
         for those interested in wake up timings of RT tasks.
 
-  "hw-branch-tracer"
-
-	Uses the BTS CPU feature on x86 CPUs to traces all
-	branches executed.
-
   "nop"
 
 	This is the "trace nothing" tracer. To remove all
@@ -261,70 +546,100 @@ Here is an example of the output format of the file "trace"
                              --------
 # tracer: function
 #
-#           TASK-PID   CPU#    TIMESTAMP  FUNCTION
-#              | |      |          |         |
-            bash-4251  [01] 10152.583854: path_put <-path_walk
-            bash-4251  [01] 10152.583855: dput <-path_put
-            bash-4251  [01] 10152.583855: _atomic_dec_and_lock <-dput
+# entries-in-buffer/entries-written: 140080/250280   #P:4
+#
+#                              _-----=> irqs-off
+#                             / _----=> need-resched
+#                            | / _---=> hardirq/softirq
+#                            || / _--=> preempt-depth
+#                            ||| /     delay
+#           TASK-PID   CPU#  ||||    TIMESTAMP  FUNCTION
+#              | |       |   ||||       |         |
+            bash-1977  [000] .... 17284.993652: sys_close <-system_call_fastpath
+            bash-1977  [000] .... 17284.993653: __close_fd <-sys_close
+            bash-1977  [000] .... 17284.993653: _raw_spin_lock <-__close_fd
+            sshd-1974  [003] .... 17284.993653: __srcu_read_unlock <-fsnotify
+            bash-1977  [000] .... 17284.993654: add_preempt_count <-_raw_spin_lock
+            bash-1977  [000] ...1 17284.993655: _raw_spin_unlock <-__close_fd
+            bash-1977  [000] ...1 17284.993656: sub_preempt_count <-_raw_spin_unlock
+            bash-1977  [000] .... 17284.993657: filp_close <-__close_fd
+            bash-1977  [000] .... 17284.993657: dnotify_flush <-filp_close
+            sshd-1974  [003] .... 17284.993658: sys_select <-system_call_fastpath
                              --------
 
 A header is printed with the tracer name that is represented by
-the trace. In this case the tracer is "function". Then a header
-showing the format. Task name "bash", the task PID "4251", the
-CPU that it was running on "01", the timestamp in <secs>.<usecs>
-format, the function name that was traced "path_put" and the
-parent function that called this function "path_walk". The
-timestamp is the time at which the function was entered.
+the trace. In this case the tracer is "function". Then it shows the
+number of events in the buffer as well as the total number of entries
+that were written. The difference is the number of entries that were
+lost due to the buffer filling up (250280 - 140080 = 110200 events
+lost).
+
+The header explains the content of the events. Task name "bash", the task
+PID "1977", the CPU that it was running on "000", the latency format
+(explained below), the timestamp in <secs>.<usecs> format, the
+function name that was traced "sys_close" and the parent function that
+called this function "system_call_fastpath". The timestamp is the time
+at which the function was entered.
 
 Latency trace format
 --------------------
 
-When the latency-format option is enabled, the trace file gives
-somewhat more information to see why a latency happened.
-Here is a typical trace.
+When the latency-format option is enabled or when one of the latency
+tracers is set, the trace file gives somewhat more information to see
+why a latency happened. Here is a typical trace.
 
 # tracer: irqsoff
 #
-irqsoff latency trace v1.1.5 on 2.6.26-rc8
---------------------------------------------------------------------
- latency: 97 us, #3/3, CPU#0 | (M:preempt VP:0, KP:0, SP:0 HP:0 #P:2)
-    -----------------
-    | task: swapper-0 (uid:0 nice:0 policy:0 rt_prio:0)
-    -----------------
- => started at: apic_timer_interrupt
- => ended at:   do_softirq
-
-#                _------=> CPU#
-#               / _-----=> irqs-off
-#              | / _----=> need-resched
-#              || / _---=> hardirq/softirq
-#              ||| / _--=> preempt-depth
-#              |||| /
-#              |||||     delay
-#  cmd     pid ||||| time  |   caller
-#     \   /    |||||   \   |   /
-  <idle>-0     0d..1    0us+: trace_hardirqs_off_thunk (apic_timer_interrupt)
-  <idle>-0     0d.s.   97us : __do_softirq (do_softirq)
-  <idle>-0     0d.s1   98us : trace_hardirqs_on (do_softirq)
+# irqsoff latency trace v1.1.5 on 3.8.0-test+
+# --------------------------------------------------------------------
+# latency: 259 us, #4/4, CPU#2 | (M:preempt VP:0, KP:0, SP:0 HP:0 #P:4)
+#    -----------------
+#    | task: ps-6143 (uid:0 nice:0 policy:0 rt_prio:0)
+#    -----------------
+#  => started at: __lock_task_sighand
+#  => ended at:   _raw_spin_unlock_irqrestore
+#
+#
+#                  _------=> CPU#            
+#                 / _-----=> irqs-off        
+#                | / _----=> need-resched    
+#                || / _---=> hardirq/softirq 
+#                ||| / _--=> preempt-depth   
+#                |||| /     delay             
+#  cmd     pid   ||||| time  |   caller      
+#     \   /      |||||  \    |   /           
+      ps-6143    2d...    0us!: trace_hardirqs_off <-__lock_task_sighand
+      ps-6143    2d..1  259us+: trace_hardirqs_on <-_raw_spin_unlock_irqrestore
+      ps-6143    2d..1  263us+: time_hardirqs_on <-_raw_spin_unlock_irqrestore
+      ps-6143    2d..1  306us : <stack trace>
+ => trace_hardirqs_on_caller
+ => trace_hardirqs_on
+ => _raw_spin_unlock_irqrestore
+ => do_task_stat
+ => proc_tgid_stat
+ => proc_single_show
+ => seq_read
+ => vfs_read
+ => sys_read
+ => system_call_fastpath
 
 
 This shows that the current tracer is "irqsoff" tracing the time
-for which interrupts were disabled. It gives the trace version
-and the version of the kernel upon which this was executed on
-(2.6.26-rc8). Then it displays the max latency in microsecs (97
-us). The number of trace entries displayed and the total number
-recorded (both are three: #3/3). The type of preemption that was
-used (PREEMPT). VP, KP, SP, and HP are always zero and are
-reserved for later use. #P is the number of online CPUS (#P:2).
+for which interrupts were disabled. It gives the trace version (which
+never changes) and the version of the kernel upon which this was executed on
+(3.10). Then it displays the max latency in microseconds (259 us). The number
+of trace entries displayed and the total number (both are four: #4/4).
+VP, KP, SP, and HP are always zero and are reserved for later use.
+#P is the number of online CPUs (#P:4).
 
 The task is the process that was running when the latency
-occurred. (swapper pid: 0).
+occurred. (ps pid: 6143).
 
 The start and stop (the functions in which the interrupts were
 disabled and enabled respectively) that caused the latencies:
 
-  apic_timer_interrupt is where the interrupts were disabled.
-  do_softirq is where they were enabled again.
+ __lock_task_sighand is where the interrupts were disabled.
+ _raw_spin_unlock_irqrestore is where they were enabled again.
 
 The next lines after the header are the trace itself. The header
 explains which is which.
@@ -367,16 +682,43 @@ The above is mostly meaningful for kernel developers.
 
   The rest is the same as the 'trace' file.
 
+  Note, the latency tracers will usually end with a back trace
+  to easily find where the latency occurred.
 
 trace_options
 -------------
 
-The trace_options file is used to control what gets printed in
-the trace output. To see what is available, simply cat the file:
+The trace_options file (or the options directory) is used to control
+what gets printed in the trace output, or manipulate the tracers.
+To see what is available, simply cat the file:
 
   cat trace_options
-  print-parent nosym-offset nosym-addr noverbose noraw nohex nobin \
-  noblock nostacktrace nosched-tree nouserstacktrace nosym-userobj
+print-parent
+nosym-offset
+nosym-addr
+noverbose
+noraw
+nohex
+nobin
+noblock
+nostacktrace
+trace_printk
+noftrace_preempt
+nobranch
+annotate
+nouserstacktrace
+nosym-userobj
+noprintk-msg-only
+context-info
+latency-format
+sleep-time
+graph-time
+record-cmd
+overwrite
+nodisable_on_free
+irq-info
+markers
+function-trace
 
 To disable one of the options, echo in the option prepended with
 "no".
@@ -428,13 +770,34 @@ Here are the available options:
 
   bin - This will print out the formats in raw binary.
 
-  block - TBD (needs update)
+  block - When set, reading trace_pipe will not block when polled.
 
   stacktrace - This is one of the options that changes the trace
 	       itself. When a trace is recorded, so is the stack
 	       of functions. This allows for back traces of
 	       trace sites.
 
+  trace_printk - Can disable trace_printk() from writing into the buffer.
+
+  branch - Enable branch tracing with the tracer.
+
+  annotate - It is sometimes confusing when the CPU buffers are full
+  	     and one CPU buffer had a lot of events recently, thus
+	     a shorter time frame, were another CPU may have only had
+	     a few events, which lets it have older events. When
+	     the trace is reported, it shows the oldest events first,
+	     and it may look like only one CPU ran (the one with the
+	     oldest events). When the annotate option is set, it will
+	     display when a new CPU buffer started:
+
+          <idle>-0     [001] dNs4 21169.031481: wake_up_idle_cpu <-add_timer_on
+          <idle>-0     [001] dNs4 21169.031482: _raw_spin_unlock_irqrestore <-add_timer_on
+          <idle>-0     [001] .Ns4 21169.031484: sub_preempt_count <-_raw_spin_unlock_irqrestore
+##### CPU 2 buffer started ####
+          <idle>-0     [002] .N.1 21169.031484: rcu_idle_exit <-cpu_idle
+          <idle>-0     [001] .Ns3 21169.031484: _raw_spin_unlock <-clocksource_watchdog
+          <idle>-0     [001] .Ns3 21169.031485: sub_preempt_count <-_raw_spin_unlock
+
   userstacktrace - This option changes the trace. It records a
 		   stacktrace of the current userspace thread.
 
@@ -451,9 +814,13 @@ Here are the available options:
 		a.out-1623  [000] 40874.465068: /root/a.out[+0x480] <-/root/a.out[+0
 x494] <- /root/a.out[+0x4a8] <- /lib/libc-2.7.so[+0x1e1a6]
 
-  sched-tree - trace all tasks that are on the runqueue, at
-	       every scheduling event. Will add overhead if
-	       there's a lot of tasks running at once.
+
+  printk-msg-only - When set, trace_printk()s will only show the format
+  		    and not their parameters (if trace_bprintk() or
+		    trace_bputs() was used to save the trace_printk()).
+
+  context-info - Show only the event data. Hides the comm, PID,
+  	         timestamp, CPU, and other useful data.
 
   latency-format - This option changes the trace. When
                    it is enabled, the trace displays
@@ -461,31 +828,61 @@ x494] <- /root/a.out[+0x4a8] <- /lib/libc-2.7.so[+0x1e1a6]
                    latencies, as described in "Latency
                    trace format".
 
+  sleep-time - When running function graph tracer, to include
+  	       the time a task schedules out in its function.
+	       When enabled, it will account time the task has been
+	       scheduled out as part of the function call.
+
+  graph-time - When running function graph tracer, to include the
+  	       time to call nested functions. When this is not set,
+	       the time reported for the function will only include
+	       the time the function itself executed for, not the time
+	       for functions that it called.
+
+  record-cmd - When any event or tracer is enabled, a hook is enabled
+  	       in the sched_switch trace point to fill comm cache
+	       with mapped pids and comms. But this may cause some
+	       overhead, and if you only care about pids, and not the
+	       name of the task, disabling this option can lower the
+	       impact of tracing.
+
   overwrite - This controls what happens when the trace buffer is
               full. If "1" (default), the oldest events are
               discarded and overwritten. If "0", then the newest
               events are discarded.
+	        (see per_cpu/cpu0/stats for overrun and dropped)
 
-ftrace_enabled
---------------
+  disable_on_free - When the free_buffer is closed, tracing will
+  		    stop (tracing_on set to 0).
 
-The following tracers (listed below) give different output
-depending on whether or not the sysctl ftrace_enabled is set. To
-set ftrace_enabled, one can either use the sysctl function or
-set it via the proc file system interface.
+  irq-info - Shows the interrupt, preempt count, need resched data.
+  	     When disabled, the trace looks like:
 
-  sysctl kernel.ftrace_enabled=1
+# tracer: function
+#
+# entries-in-buffer/entries-written: 144405/9452052   #P:4
+#
+#           TASK-PID   CPU#      TIMESTAMP  FUNCTION
+#              | |       |          |         |
+          <idle>-0     [002]  23636.756054: ttwu_do_activate.constprop.89 <-try_to_wake_up
+          <idle>-0     [002]  23636.756054: activate_task <-ttwu_do_activate.constprop.89
+          <idle>-0     [002]  23636.756055: enqueue_task <-activate_task
 
- or
 
-  echo 1 > /proc/sys/kernel/ftrace_enabled
+  markers - When set, the trace_marker is writable (only by root).
+  	    When disabled, the trace_marker will error with EINVAL
+	    on write.
+
+
+  function-trace - The latency tracers will enable function tracing
+  	    if this option is enabled (default it is). When
+	    it is disabled, the latency tracers do not trace
+	    functions. This keeps the overhead of the tracer down
+	    when performing latency tests.
 
-To disable ftrace_enabled simply replace the '1' with '0' in the
-above commands.
+ Note: Some tracers have their own options. They only appear
+       when the tracer is active.
 
-When ftrace_enabled is set the tracers will also record the
-functions that are within the trace. The descriptions of the
-tracers will also show an example with ftrace enabled.
 
 
 irqsoff
@@ -506,95 +903,133 @@ new trace is saved.
 To reset the maximum, echo 0 into tracing_max_latency. Here is
 an example:
 
+ # echo 0 > options/function-trace
  # echo irqsoff > current_tracer
- # echo latency-format > trace_options
- # echo 0 > tracing_max_latency
  # echo 1 > tracing_on
+ # echo 0 > tracing_max_latency
  # ls -ltr
  [...]
  # echo 0 > tracing_on
  # cat trace
 # tracer: irqsoff
 #
-irqsoff latency trace v1.1.5 on 2.6.26
---------------------------------------------------------------------
- latency: 12 us, #3/3, CPU#1 | (M:preempt VP:0, KP:0, SP:0 HP:0 #P:2)
-    -----------------
-    | task: bash-3730 (uid:0 nice:0 policy:0 rt_prio:0)
-    -----------------
- => started at: sys_setpgid
- => ended at:   sys_setpgid
-
-#                _------=> CPU#
-#               / _-----=> irqs-off
-#              | / _----=> need-resched
-#              || / _---=> hardirq/softirq
-#              ||| / _--=> preempt-depth
-#              |||| /
-#              |||||     delay
-#  cmd     pid ||||| time  |   caller
-#     \   /    |||||   \   |   /
-    bash-3730  1d...    0us : _write_lock_irq (sys_setpgid)
-    bash-3730  1d..1    1us+: _write_unlock_irq (sys_setpgid)
-    bash-3730  1d..2   14us : trace_hardirqs_on (sys_setpgid)
-
-
-Here we see that that we had a latency of 12 microsecs (which is
-very good). The _write_lock_irq in sys_setpgid disabled
-interrupts. The difference between the 12 and the displayed
-timestamp 14us occurred because the clock was incremented
+# irqsoff latency trace v1.1.5 on 3.8.0-test+
+# --------------------------------------------------------------------
+# latency: 16 us, #4/4, CPU#0 | (M:preempt VP:0, KP:0, SP:0 HP:0 #P:4)
+#    -----------------
+#    | task: swapper/0-0 (uid:0 nice:0 policy:0 rt_prio:0)
+#    -----------------
+#  => started at: run_timer_softirq
+#  => ended at:   run_timer_softirq
+#
+#
+#                  _------=> CPU#            
+#                 / _-----=> irqs-off        
+#                | / _----=> need-resched    
+#                || / _---=> hardirq/softirq 
+#                ||| / _--=> preempt-depth   
+#                |||| /     delay             
+#  cmd     pid   ||||| time  |   caller      
+#     \   /      |||||  \    |   /           
+  <idle>-0       0d.s2    0us+: _raw_spin_lock_irq <-run_timer_softirq
+  <idle>-0       0dNs3   17us : _raw_spin_unlock_irq <-run_timer_softirq
+  <idle>-0       0dNs3   17us+: trace_hardirqs_on <-run_timer_softirq
+  <idle>-0       0dNs3   25us : <stack trace>
+ => _raw_spin_unlock_irq
+ => run_timer_softirq
+ => __do_softirq
+ => call_softirq
+ => do_softirq
+ => irq_exit
+ => smp_apic_timer_interrupt
+ => apic_timer_interrupt
+ => rcu_idle_exit
+ => cpu_idle
+ => rest_init
+ => start_kernel
+ => x86_64_start_reservations
+ => x86_64_start_kernel
+
+Here we see that that we had a latency of 16 microseconds (which is
+very good). The _raw_spin_lock_irq in run_timer_softirq disabled
+interrupts. The difference between the 16 and the displayed
+timestamp 25us occurred because the clock was incremented
 between the time of recording the max latency and the time of
 recording the function that had that latency.
 
-Note the above example had ftrace_enabled not set. If we set the
-ftrace_enabled, we get a much larger output:
+Note the above example had function-trace not set. If we set
+function-trace, we get a much larger output:
+
+ with echo 1 > options/function-trace
 
 # tracer: irqsoff
 #
-irqsoff latency trace v1.1.5 on 2.6.26-rc8
---------------------------------------------------------------------
- latency: 50 us, #101/101, CPU#0 | (M:preempt VP:0, KP:0, SP:0 HP:0 #P:2)
-    -----------------
-    | task: ls-4339 (uid:0 nice:0 policy:0 rt_prio:0)
-    -----------------
- => started at: __alloc_pages_internal
- => ended at:   __alloc_pages_internal
-
-#                _------=> CPU#
-#               / _-----=> irqs-off
-#              | / _----=> need-resched
-#              || / _---=> hardirq/softirq
-#              ||| / _--=> preempt-depth
-#              |||| /
-#              |||||     delay
-#  cmd     pid ||||| time  |   caller
-#     \   /    |||||   \   |   /
-      ls-4339  0...1    0us+: get_page_from_freelist (__alloc_pages_internal)
-      ls-4339  0d..1    3us : rmqueue_bulk (get_page_from_freelist)
-      ls-4339  0d..1    3us : _spin_lock (rmqueue_bulk)
-      ls-4339  0d..1    4us : add_preempt_count (_spin_lock)
-      ls-4339  0d..2    4us : __rmqueue (rmqueue_bulk)
-      ls-4339  0d..2    5us : __rmqueue_smallest (__rmqueue)
-      ls-4339  0d..2    5us : __mod_zone_page_state (__rmqueue_smallest)
-      ls-4339  0d..2    6us : __rmqueue (rmqueue_bulk)
-      ls-4339  0d..2    6us : __rmqueue_smallest (__rmqueue)
-      ls-4339  0d..2    7us : __mod_zone_page_state (__rmqueue_smallest)
-      ls-4339  0d..2    7us : __rmqueue (rmqueue_bulk)
-      ls-4339  0d..2    8us : __rmqueue_smallest (__rmqueue)
+# irqsoff latency trace v1.1.5 on 3.8.0-test+
+# --------------------------------------------------------------------
+# latency: 71 us, #168/168, CPU#3 | (M:preempt VP:0, KP:0, SP:0 HP:0 #P:4)
+#    -----------------
+#    | task: bash-2042 (uid:0 nice:0 policy:0 rt_prio:0)
+#    -----------------
+#  => started at: ata_scsi_queuecmd
+#  => ended at:   ata_scsi_queuecmd
+#
+#
+#                  _------=> CPU#            
+#                 / _-----=> irqs-off        
+#                | / _----=> need-resched    
+#                || / _---=> hardirq/softirq 
+#                ||| / _--=> preempt-depth   
+#                |||| /     delay             
+#  cmd     pid   ||||| time  |   caller      
+#     \   /      |||||  \    |   /           
+    bash-2042    3d...    0us : _raw_spin_lock_irqsave <-ata_scsi_queuecmd
+    bash-2042    3d...    0us : add_preempt_count <-_raw_spin_lock_irqsave
+    bash-2042    3d..1    1us : ata_scsi_find_dev <-ata_scsi_queuecmd
+    bash-2042    3d..1    1us : __ata_scsi_find_dev <-ata_scsi_find_dev
+    bash-2042    3d..1    2us : ata_find_dev.part.14 <-__ata_scsi_find_dev
+    bash-2042    3d..1    2us : ata_qc_new_init <-__ata_scsi_queuecmd
+    bash-2042    3d..1    3us : ata_sg_init <-__ata_scsi_queuecmd
+    bash-2042    3d..1    4us : ata_scsi_rw_xlat <-__ata_scsi_queuecmd
+    bash-2042    3d..1    4us : ata_build_rw_tf <-ata_scsi_rw_xlat
 [...]
-      ls-4339  0d..2   46us : __rmqueue_smallest (__rmqueue)
-      ls-4339  0d..2   47us : __mod_zone_page_state (__rmqueue_smallest)
-      ls-4339  0d..2   47us : __rmqueue (rmqueue_bulk)
-      ls-4339  0d..2   48us : __rmqueue_smallest (__rmqueue)
-      ls-4339  0d..2   48us : __mod_zone_page_state (__rmqueue_smallest)
-      ls-4339  0d..2   49us : _spin_unlock (rmqueue_bulk)
-      ls-4339  0d..2   49us : sub_preempt_count (_spin_unlock)
-      ls-4339  0d..1   50us : get_page_from_freelist (__alloc_pages_internal)
-      ls-4339  0d..2   51us : trace_hardirqs_on (__alloc_pages_internal)
-
-
-
-Here we traced a 50 microsecond latency. But we also see all the
+    bash-2042    3d..1   67us : delay_tsc <-__delay
+    bash-2042    3d..1   67us : add_preempt_count <-delay_tsc
+    bash-2042    3d..2   67us : sub_preempt_count <-delay_tsc
+    bash-2042    3d..1   67us : add_preempt_count <-delay_tsc
+    bash-2042    3d..2   68us : sub_preempt_count <-delay_tsc
+    bash-2042    3d..1   68us+: ata_bmdma_start <-ata_bmdma_qc_issue
+    bash-2042    3d..1   71us : _raw_spin_unlock_irqrestore <-ata_scsi_queuecmd
+    bash-2042    3d..1   71us : _raw_spin_unlock_irqrestore <-ata_scsi_queuecmd
+    bash-2042    3d..1   72us+: trace_hardirqs_on <-ata_scsi_queuecmd
+    bash-2042    3d..1  120us : <stack trace>
+ => _raw_spin_unlock_irqrestore
+ => ata_scsi_queuecmd
+ => scsi_dispatch_cmd
+ => scsi_request_fn
+ => __blk_run_queue_uncond
+ => __blk_run_queue
+ => blk_queue_bio
+ => generic_make_request
+ => submit_bio
+ => submit_bh
+ => __ext3_get_inode_loc
+ => ext3_iget
+ => ext3_lookup
+ => lookup_real
+ => __lookup_hash
+ => walk_component
+ => lookup_last
+ => path_lookupat
+ => filename_lookup
+ => user_path_at_empty
+ => user_path_at
+ => vfs_fstatat
+ => vfs_stat
+ => sys_newstat
+ => system_call_fastpath
+
+
+Here we traced a 71 microsecond latency. But we also see all the
 functions that were called during that time. Note that by
 enabling function tracing, we incur an added overhead. This
 overhead may extend the latency times. But nevertheless, this
@@ -614,120 +1049,122 @@ Like the irqsoff tracer, it records the maximum latency for
 which preemption was disabled. The control of preemptoff tracer
 is much like the irqsoff tracer.
 
+ # echo 0 > options/function-trace
  # echo preemptoff > current_tracer
- # echo latency-format > trace_options
- # echo 0 > tracing_max_latency
  # echo 1 > tracing_on
+ # echo 0 > tracing_max_latency
  # ls -ltr
  [...]
  # echo 0 > tracing_on
  # cat trace
 # tracer: preemptoff
 #
-preemptoff latency trace v1.1.5 on 2.6.26-rc8
---------------------------------------------------------------------
- latency: 29 us, #3/3, CPU#0 | (M:preempt VP:0, KP:0, SP:0 HP:0 #P:2)
-    -----------------
-    | task: sshd-4261 (uid:0 nice:0 policy:0 rt_prio:0)
-    -----------------
- => started at: do_IRQ
- => ended at:   __do_softirq
-
-#                _------=> CPU#
-#               / _-----=> irqs-off
-#              | / _----=> need-resched
-#              || / _---=> hardirq/softirq
-#              ||| / _--=> preempt-depth
-#              |||| /
-#              |||||     delay
-#  cmd     pid ||||| time  |   caller
-#     \   /    |||||   \   |   /
-    sshd-4261  0d.h.    0us+: irq_enter (do_IRQ)
-    sshd-4261  0d.s.   29us : _local_bh_enable (__do_softirq)
-    sshd-4261  0d.s1   30us : trace_preempt_on (__do_softirq)
+# preemptoff latency trace v1.1.5 on 3.8.0-test+
+# --------------------------------------------------------------------
+# latency: 46 us, #4/4, CPU#1 | (M:preempt VP:0, KP:0, SP:0 HP:0 #P:4)
+#    -----------------
+#    | task: sshd-1991 (uid:0 nice:0 policy:0 rt_prio:0)
+#    -----------------
+#  => started at: do_IRQ
+#  => ended at:   do_IRQ
+#
+#
+#                  _------=> CPU#            
+#                 / _-----=> irqs-off        
+#                | / _----=> need-resched    
+#                || / _---=> hardirq/softirq 
+#                ||| / _--=> preempt-depth   
+#                |||| /     delay             
+#  cmd     pid   ||||| time  |   caller      
+#     \   /      |||||  \    |   /           
+    sshd-1991    1d.h.    0us+: irq_enter <-do_IRQ
+    sshd-1991    1d..1   46us : irq_exit <-do_IRQ
+    sshd-1991    1d..1   47us+: trace_preempt_on <-do_IRQ
+    sshd-1991    1d..1   52us : <stack trace>
+ => sub_preempt_count
+ => irq_exit
+ => do_IRQ
+ => ret_from_intr
 
 
 This has some more changes. Preemption was disabled when an
-interrupt came in (notice the 'h'), and was enabled while doing
-a softirq. (notice the 's'). But we also see that interrupts
-have been disabled when entering the preempt off section and
-leaving it (the 'd'). We do not know if interrupts were enabled
-in the mean time.
+interrupt came in (notice the 'h'), and was enabled on exit.
+But we also see that interrupts have been disabled when entering
+the preempt off section and leaving it (the 'd'). We do not know if
+interrupts were enabled in the mean time or shortly after this
+was over.
 
 # tracer: preemptoff
 #
-preemptoff latency trace v1.1.5 on 2.6.26-rc8
---------------------------------------------------------------------
- latency: 63 us, #87/87, CPU#0 | (M:preempt VP:0, KP:0, SP:0 HP:0 #P:2)
-    -----------------
-    | task: sshd-4261 (uid:0 nice:0 policy:0 rt_prio:0)
-    -----------------
- => started at: remove_wait_queue
- => ended at:   __do_softirq
-
-#                _------=> CPU#
-#               / _-----=> irqs-off
-#              | / _----=> need-resched
-#              || / _---=> hardirq/softirq
-#              ||| / _--=> preempt-depth
-#              |||| /
-#              |||||     delay
-#  cmd     pid ||||| time  |   caller
-#     \   /    |||||   \   |   /
-    sshd-4261  0d..1    0us : _spin_lock_irqsave (remove_wait_queue)
-    sshd-4261  0d..1    1us : _spin_unlock_irqrestore (remove_wait_queue)
-    sshd-4261  0d..1    2us : do_IRQ (common_interrupt)
-    sshd-4261  0d..1    2us : irq_enter (do_IRQ)
-    sshd-4261  0d..1    2us : idle_cpu (irq_enter)
-    sshd-4261  0d..1    3us : add_preempt_count (irq_enter)
-    sshd-4261  0d.h1    3us : idle_cpu (irq_enter)
-    sshd-4261  0d.h.    4us : handle_fasteoi_irq (do_IRQ)
+# preemptoff latency trace v1.1.5 on 3.8.0-test+
+# --------------------------------------------------------------------
+# latency: 83 us, #241/241, CPU#1 | (M:preempt VP:0, KP:0, SP:0 HP:0 #P:4)
+#    -----------------
+#    | task: bash-1994 (uid:0 nice:0 policy:0 rt_prio:0)
+#    -----------------
+#  => started at: wake_up_new_task
+#  => ended at:   task_rq_unlock
+#
+#
+#                  _------=> CPU#            
+#                 / _-----=> irqs-off        
+#                | / _----=> need-resched    
+#                || / _---=> hardirq/softirq 
+#                ||| / _--=> preempt-depth   
+#                |||| /     delay             
+#  cmd     pid   ||||| time  |   caller      
+#     \   /      |||||  \    |   /           
+    bash-1994    1d..1    0us : _raw_spin_lock_irqsave <-wake_up_new_task
+    bash-1994    1d..1    0us : select_task_rq_fair <-select_task_rq
+    bash-1994    1d..1    1us : __rcu_read_lock <-select_task_rq_fair
+    bash-1994    1d..1    1us : source_load <-select_task_rq_fair
+    bash-1994    1d..1    1us : source_load <-select_task_rq_fair
 [...]
-    sshd-4261  0d.h.   12us : add_preempt_count (_spin_lock)
-    sshd-4261  0d.h1   12us : ack_ioapic_quirk_irq (handle_fasteoi_irq)
-    sshd-4261  0d.h1   13us : move_native_irq (ack_ioapic_quirk_irq)
-    sshd-4261  0d.h1   13us : _spin_unlock (handle_fasteoi_irq)
-    sshd-4261  0d.h1   14us : sub_preempt_count (_spin_unlock)
-    sshd-4261  0d.h1   14us : irq_exit (do_IRQ)
-    sshd-4261  0d.h1   15us : sub_preempt_count (irq_exit)
-    sshd-4261  0d..2   15us : do_softirq (irq_exit)
-    sshd-4261  0d...   15us : __do_softirq (do_softirq)
-    sshd-4261  0d...   16us : __local_bh_disable (__do_softirq)
-    sshd-4261  0d...   16us+: add_preempt_count (__local_bh_disable)
-    sshd-4261  0d.s4   20us : add_preempt_count (__local_bh_disable)
-    sshd-4261  0d.s4   21us : sub_preempt_count (local_bh_enable)
-    sshd-4261  0d.s5   21us : sub_preempt_count (local_bh_enable)
+    bash-1994    1d..1   12us : irq_enter <-smp_apic_timer_interrupt
+    bash-1994    1d..1   12us : rcu_irq_enter <-irq_enter
+    bash-1994    1d..1   13us : add_preempt_count <-irq_enter
+    bash-1994    1d.h1   13us : exit_idle <-smp_apic_timer_interrupt
+    bash-1994    1d.h1   13us : hrtimer_interrupt <-smp_apic_timer_interrupt
+    bash-1994    1d.h1   13us : _raw_spin_lock <-hrtimer_interrupt
+    bash-1994    1d.h1   14us : add_preempt_count <-_raw_spin_lock
+    bash-1994    1d.h2   14us : ktime_get_update_offsets <-hrtimer_interrupt
 [...]
-    sshd-4261  0d.s6   41us : add_preempt_count (__local_bh_disable)
-    sshd-4261  0d.s6   42us : sub_preempt_count (local_bh_enable)
-    sshd-4261  0d.s7   42us : sub_preempt_count (local_bh_enable)
-    sshd-4261  0d.s5   43us : add_preempt_count (__local_bh_disable)
-    sshd-4261  0d.s5   43us : sub_preempt_count (local_bh_enable_ip)
-    sshd-4261  0d.s6   44us : sub_preempt_count (local_bh_enable_ip)
-    sshd-4261  0d.s5   44us : add_preempt_count (__local_bh_disable)
-    sshd-4261  0d.s5   45us : sub_preempt_count (local_bh_enable)
+    bash-1994    1d.h1   35us : lapic_next_event <-clockevents_program_event
+    bash-1994    1d.h1   35us : irq_exit <-smp_apic_timer_interrupt
+    bash-1994    1d.h1   36us : sub_preempt_count <-irq_exit
+    bash-1994    1d..2   36us : do_softirq <-irq_exit
+    bash-1994    1d..2   36us : __do_softirq <-call_softirq
+    bash-1994    1d..2   36us : __local_bh_disable <-__do_softirq
+    bash-1994    1d.s2   37us : add_preempt_count <-_raw_spin_lock_irq
+    bash-1994    1d.s3   38us : _raw_spin_unlock <-run_timer_softirq
+    bash-1994    1d.s3   39us : sub_preempt_count <-_raw_spin_unlock
+    bash-1994    1d.s2   39us : call_timer_fn <-run_timer_softirq
 [...]
-    sshd-4261  0d.s.   63us : _local_bh_enable (__do_softirq)
-    sshd-4261  0d.s1   64us : trace_preempt_on (__do_softirq)
+    bash-1994    1dNs2   81us : cpu_needs_another_gp <-rcu_process_callbacks
+    bash-1994    1dNs2   82us : __local_bh_enable <-__do_softirq
+    bash-1994    1dNs2   82us : sub_preempt_count <-__local_bh_enable
+    bash-1994    1dN.2   82us : idle_cpu <-irq_exit
+    bash-1994    1dN.2   83us : rcu_irq_exit <-irq_exit
+    bash-1994    1dN.2   83us : sub_preempt_count <-irq_exit
+    bash-1994    1.N.1   84us : _raw_spin_unlock_irqrestore <-task_rq_unlock
+    bash-1994    1.N.1   84us+: trace_preempt_on <-task_rq_unlock
+    bash-1994    1.N.1  104us : <stack trace>
+ => sub_preempt_count
+ => _raw_spin_unlock_irqrestore
+ => task_rq_unlock
+ => wake_up_new_task
+ => do_fork
+ => sys_clone
+ => stub_clone
 
 
 The above is an example of the preemptoff trace with
-ftrace_enabled set. Here we see that interrupts were disabled
+function-trace set. Here we see that interrupts were not disabled
 the entire time. The irq_enter code lets us know that we entered
 an interrupt 'h'. Before that, the functions being traced still
 show that it is not in an interrupt, but we can see from the
 functions themselves that this is not the case.
 
-Notice that __do_softirq when called does not have a
-preempt_count. It may seem that we missed a preempt enabling.
-What really happened is that the preempt count is held on the
-thread's stack and we switched to the softirq stack (4K stacks
-in effect). The code does not copy the preempt count, but
-because interrupts are disabled, we do not need to worry about
-it. Having a tracer like this is good for letting people know
-what really happens inside the kernel.
-
-
 preemptirqsoff
 --------------
 
@@ -762,38 +1199,57 @@ tracer.
 Again, using this trace is much like the irqsoff and preemptoff
 tracers.
 
+ # echo 0 > options/function-trace
  # echo preemptirqsoff > current_tracer
- # echo latency-format > trace_options
- # echo 0 > tracing_max_latency
  # echo 1 > tracing_on
+ # echo 0 > tracing_max_latency
  # ls -ltr
  [...]
  # echo 0 > tracing_on
  # cat trace
 # tracer: preemptirqsoff
 #
-preemptirqsoff latency trace v1.1.5 on 2.6.26-rc8
---------------------------------------------------------------------
- latency: 293 us, #3/3, CPU#0 | (M:preempt VP:0, KP:0, SP:0 HP:0 #P:2)
-    -----------------
-    | task: ls-4860 (uid:0 nice:0 policy:0 rt_prio:0)
-    -----------------
- => started at: apic_timer_interrupt
- => ended at:   __do_softirq
-
-#                _------=> CPU#
-#               / _-----=> irqs-off
-#              | / _----=> need-resched
-#              || / _---=> hardirq/softirq
-#              ||| / _--=> preempt-depth
-#              |||| /
-#              |||||     delay
-#  cmd     pid ||||| time  |   caller
-#     \   /    |||||   \   |   /
-      ls-4860  0d...    0us!: trace_hardirqs_off_thunk (apic_timer_interrupt)
-      ls-4860  0d.s.  294us : _local_bh_enable (__do_softirq)
-      ls-4860  0d.s1  294us : trace_preempt_on (__do_softirq)
-
+# preemptirqsoff latency trace v1.1.5 on 3.8.0-test+
+# --------------------------------------------------------------------
+# latency: 100 us, #4/4, CPU#3 | (M:preempt VP:0, KP:0, SP:0 HP:0 #P:4)
+#    -----------------
+#    | task: ls-2230 (uid:0 nice:0 policy:0 rt_prio:0)
+#    -----------------
+#  => started at: ata_scsi_queuecmd
+#  => ended at:   ata_scsi_queuecmd
+#
+#
+#                  _------=> CPU#            
+#                 / _-----=> irqs-off        
+#                | / _----=> need-resched    
+#                || / _---=> hardirq/softirq 
+#                ||| / _--=> preempt-depth   
+#                |||| /     delay             
+#  cmd     pid   ||||| time  |   caller      
+#     \   /      |||||  \    |   /           
+      ls-2230    3d...    0us+: _raw_spin_lock_irqsave <-ata_scsi_queuecmd
+      ls-2230    3...1  100us : _raw_spin_unlock_irqrestore <-ata_scsi_queuecmd
+      ls-2230    3...1  101us+: trace_preempt_on <-ata_scsi_queuecmd
+      ls-2230    3...1  111us : <stack trace>
+ => sub_preempt_count
+ => _raw_spin_unlock_irqrestore
+ => ata_scsi_queuecmd
+ => scsi_dispatch_cmd
+ => scsi_request_fn
+ => __blk_run_queue_uncond
+ => __blk_run_queue
+ => blk_queue_bio
+ => generic_make_request
+ => submit_bio
+ => submit_bh
+ => ext3_bread
+ => ext3_dir_bread
+ => htree_dirblock_to_tree
+ => ext3_htree_fill_tree
+ => ext3_readdir
+ => vfs_readdir
+ => sys_getdents
+ => system_call_fastpath
 
 
 The trace_hardirqs_off_thunk is called from assembly on x86 when
@@ -802,105 +1258,158 @@ function tracing, we do not know if interrupts were enabled
 within the preemption points. We do see that it started with
 preemption enabled.
 
-Here is a trace with ftrace_enabled set:
-
+Here is a trace with function-trace set:
 
 # tracer: preemptirqsoff
 #
-preemptirqsoff latency trace v1.1.5 on 2.6.26-rc8
---------------------------------------------------------------------
- latency: 105 us, #183/183, CPU#0 | (M:preempt VP:0, KP:0, SP:0 HP:0 #P:2)
-    -----------------
-    | task: sshd-4261 (uid:0 nice:0 policy:0 rt_prio:0)
-    -----------------
- => started at: write_chan
- => ended at:   __do_softirq
-
-#                _------=> CPU#
-#               / _-----=> irqs-off
-#              | / _----=> need-resched
-#              || / _---=> hardirq/softirq
-#              ||| / _--=> preempt-depth
-#              |||| /
-#              |||||     delay
-#  cmd     pid ||||| time  |   caller
-#     \   /    |||||   \   |   /
-      ls-4473  0.N..    0us : preempt_schedule (write_chan)
-      ls-4473  0dN.1    1us : _spin_lock (schedule)
-      ls-4473  0dN.1    2us : add_preempt_count (_spin_lock)
-      ls-4473  0d..2    2us : put_prev_task_fair (schedule)
-[...]
-      ls-4473  0d..2   13us : set_normalized_timespec (ktime_get_ts)
-      ls-4473  0d..2   13us : __switch_to (schedule)
-    sshd-4261  0d..2   14us : finish_task_switch (schedule)
-    sshd-4261  0d..2   14us : _spin_unlock_irq (finish_task_switch)
-    sshd-4261  0d..1   15us : add_preempt_count (_spin_lock_irqsave)
-    sshd-4261  0d..2   16us : _spin_unlock_irqrestore (hrtick_set)
-    sshd-4261  0d..2   16us : do_IRQ (common_interrupt)
-    sshd-4261  0d..2   17us : irq_enter (do_IRQ)
-    sshd-4261  0d..2   17us : idle_cpu (irq_enter)
-    sshd-4261  0d..2   18us : add_preempt_count (irq_enter)
-    sshd-4261  0d.h2   18us : idle_cpu (irq_enter)
-    sshd-4261  0d.h.   18us : handle_fasteoi_irq (do_IRQ)
-    sshd-4261  0d.h.   19us : _spin_lock (handle_fasteoi_irq)
-    sshd-4261  0d.h.   19us : add_preempt_count (_spin_lock)
-    sshd-4261  0d.h1   20us : _spin_unlock (handle_fasteoi_irq)
-    sshd-4261  0d.h1   20us : sub_preempt_count (_spin_unlock)
-[...]
-    sshd-4261  0d.h1   28us : _spin_unlock (handle_fasteoi_irq)
-    sshd-4261  0d.h1   29us : sub_preempt_count (_spin_unlock)
-    sshd-4261  0d.h2   29us : irq_exit (do_IRQ)
-    sshd-4261  0d.h2   29us : sub_preempt_count (irq_exit)
-    sshd-4261  0d..3   30us : do_softirq (irq_exit)
-    sshd-4261  0d...   30us : __do_softirq (do_softirq)
-    sshd-4261  0d...   31us : __local_bh_disable (__do_softirq)
-    sshd-4261  0d...   31us+: add_preempt_count (__local_bh_disable)
-    sshd-4261  0d.s4   34us : add_preempt_count (__local_bh_disable)
+# preemptirqsoff latency trace v1.1.5 on 3.8.0-test+
+# --------------------------------------------------------------------
+# latency: 161 us, #339/339, CPU#3 | (M:preempt VP:0, KP:0, SP:0 HP:0 #P:4)
+#    -----------------
+#    | task: ls-2269 (uid:0 nice:0 policy:0 rt_prio:0)
+#    -----------------
+#  => started at: schedule
+#  => ended at:   mutex_unlock
+#
+#
+#                  _------=> CPU#            
+#                 / _-----=> irqs-off        
+#                | / _----=> need-resched    
+#                || / _---=> hardirq/softirq 
+#                ||| / _--=> preempt-depth   
+#                |||| /     delay             
+#  cmd     pid   ||||| time  |   caller      
+#     \   /      |||||  \    |   /           
+kworker/-59      3...1    0us : __schedule <-schedule
+kworker/-59      3d..1    0us : rcu_preempt_qs <-rcu_note_context_switch
+kworker/-59      3d..1    1us : add_preempt_count <-_raw_spin_lock_irq
+kworker/-59      3d..2    1us : deactivate_task <-__schedule
+kworker/-59      3d..2    1us : dequeue_task <-deactivate_task
+kworker/-59      3d..2    2us : update_rq_clock <-dequeue_task
+kworker/-59      3d..2    2us : dequeue_task_fair <-dequeue_task
+kworker/-59      3d..2    2us : update_curr <-dequeue_task_fair
+kworker/-59      3d..2    2us : update_min_vruntime <-update_curr
+kworker/-59      3d..2    3us : cpuacct_charge <-update_curr
+kworker/-59      3d..2    3us : __rcu_read_lock <-cpuacct_charge
+kworker/-59      3d..2    3us : __rcu_read_unlock <-cpuacct_charge
+kworker/-59      3d..2    3us : update_cfs_rq_blocked_load <-dequeue_task_fair
+kworker/-59      3d..2    4us : clear_buddies <-dequeue_task_fair
+kworker/-59      3d..2    4us : account_entity_dequeue <-dequeue_task_fair
+kworker/-59      3d..2    4us : update_min_vruntime <-dequeue_task_fair
+kworker/-59      3d..2    4us : update_cfs_shares <-dequeue_task_fair
+kworker/-59      3d..2    5us : hrtick_update <-dequeue_task_fair
+kworker/-59      3d..2    5us : wq_worker_sleeping <-__schedule
+kworker/-59      3d..2    5us : kthread_data <-wq_worker_sleeping
+kworker/-59      3d..2    5us : put_prev_task_fair <-__schedule
+kworker/-59      3d..2    6us : pick_next_task_fair <-pick_next_task
+kworker/-59      3d..2    6us : clear_buddies <-pick_next_task_fair
+kworker/-59      3d..2    6us : set_next_entity <-pick_next_task_fair
+kworker/-59      3d..2    6us : update_stats_wait_end <-set_next_entity
+      ls-2269    3d..2    7us : finish_task_switch <-__schedule
+      ls-2269    3d..2    7us : _raw_spin_unlock_irq <-finish_task_switch
+      ls-2269    3d..2    8us : do_IRQ <-ret_from_intr
+      ls-2269    3d..2    8us : irq_enter <-do_IRQ
+      ls-2269    3d..2    8us : rcu_irq_enter <-irq_enter
+      ls-2269    3d..2    9us : add_preempt_count <-irq_enter
+      ls-2269    3d.h2    9us : exit_idle <-do_IRQ
 [...]
-    sshd-4261  0d.s3   43us : sub_preempt_count (local_bh_enable_ip)
-    sshd-4261  0d.s4   44us : sub_preempt_count (local_bh_enable_ip)
-    sshd-4261  0d.s3   44us : smp_apic_timer_interrupt (apic_timer_interrupt)
-    sshd-4261  0d.s3   45us : irq_enter (smp_apic_timer_interrupt)
-    sshd-4261  0d.s3   45us : idle_cpu (irq_enter)
-    sshd-4261  0d.s3   46us : add_preempt_count (irq_enter)
-    sshd-4261  0d.H3   46us : idle_cpu (irq_enter)
-    sshd-4261  0d.H3   47us : hrtimer_interrupt (smp_apic_timer_interrupt)
-    sshd-4261  0d.H3   47us : ktime_get (hrtimer_interrupt)
+      ls-2269    3d.h3   20us : sub_preempt_count <-_raw_spin_unlock
+      ls-2269    3d.h2   20us : irq_exit <-do_IRQ
+      ls-2269    3d.h2   21us : sub_preempt_count <-irq_exit
+      ls-2269    3d..3   21us : do_softirq <-irq_exit
+      ls-2269    3d..3   21us : __do_softirq <-call_softirq
+      ls-2269    3d..3   21us+: __local_bh_disable <-__do_softirq
+      ls-2269    3d.s4   29us : sub_preempt_count <-_local_bh_enable_ip
+      ls-2269    3d.s5   29us : sub_preempt_count <-_local_bh_enable_ip
+      ls-2269    3d.s5   31us : do_IRQ <-ret_from_intr
+      ls-2269    3d.s5   31us : irq_enter <-do_IRQ
+      ls-2269    3d.s5   31us : rcu_irq_enter <-irq_enter
 [...]
-    sshd-4261  0d.H3   81us : tick_program_event (hrtimer_interrupt)
-    sshd-4261  0d.H3   82us : ktime_get (tick_program_event)
-    sshd-4261  0d.H3   82us : ktime_get_ts (ktime_get)
-    sshd-4261  0d.H3   83us : getnstimeofday (ktime_get_ts)
-    sshd-4261  0d.H3   83us : set_normalized_timespec (ktime_get_ts)
-    sshd-4261  0d.H3   84us : clockevents_program_event (tick_program_event)
-    sshd-4261  0d.H3   84us : lapic_next_event (clockevents_program_event)
-    sshd-4261  0d.H3   85us : irq_exit (smp_apic_timer_interrupt)
-    sshd-4261  0d.H3   85us : sub_preempt_count (irq_exit)
-    sshd-4261  0d.s4   86us : sub_preempt_count (irq_exit)
-    sshd-4261  0d.s3   86us : add_preempt_count (__local_bh_disable)
+      ls-2269    3d.s5   31us : rcu_irq_enter <-irq_enter
+      ls-2269    3d.s5   32us : add_preempt_count <-irq_enter
+      ls-2269    3d.H5   32us : exit_idle <-do_IRQ
+      ls-2269    3d.H5   32us : handle_irq <-do_IRQ
+      ls-2269    3d.H5   32us : irq_to_desc <-handle_irq
+      ls-2269    3d.H5   33us : handle_fasteoi_irq <-handle_irq
 [...]
-    sshd-4261  0d.s1   98us : sub_preempt_count (net_rx_action)
-    sshd-4261  0d.s.   99us : add_preempt_count (_spin_lock_irq)
-    sshd-4261  0d.s1   99us+: _spin_unlock_irq (run_timer_softirq)
-    sshd-4261  0d.s.  104us : _local_bh_enable (__do_softirq)
-    sshd-4261  0d.s.  104us : sub_preempt_count (_local_bh_enable)
-    sshd-4261  0d.s.  105us : _local_bh_enable (__do_softirq)
-    sshd-4261  0d.s1  105us : trace_preempt_on (__do_softirq)
-
-
-This is a very interesting trace. It started with the preemption
-of the ls task. We see that the task had the "need_resched" bit
-set via the 'N' in the trace.  Interrupts were disabled before
-the spin_lock at the beginning of the trace. We see that a
-schedule took place to run sshd.  When the interrupts were
-enabled, we took an interrupt. On return from the interrupt
-handler, the softirq ran. We took another interrupt while
-running the softirq as we see from the capital 'H'.
+      ls-2269    3d.s5  158us : _raw_spin_unlock_irqrestore <-rtl8139_poll
+      ls-2269    3d.s3  158us : net_rps_action_and_irq_enable.isra.65 <-net_rx_action
+      ls-2269    3d.s3  159us : __local_bh_enable <-__do_softirq
+      ls-2269    3d.s3  159us : sub_preempt_count <-__local_bh_enable
+      ls-2269    3d..3  159us : idle_cpu <-irq_exit
+      ls-2269    3d..3  159us : rcu_irq_exit <-irq_exit
+      ls-2269    3d..3  160us : sub_preempt_count <-irq_exit
+      ls-2269    3d...  161us : __mutex_unlock_slowpath <-mutex_unlock
+      ls-2269    3d...  162us+: trace_hardirqs_on <-mutex_unlock
+      ls-2269    3d...  186us : <stack trace>
+ => __mutex_unlock_slowpath
+ => mutex_unlock
+ => process_output
+ => n_tty_write
+ => tty_write
+ => vfs_write
+ => sys_write
+ => system_call_fastpath
+
+This is an interesting trace. It started with kworker running and
+scheduling out and ls taking over. But as soon as ls released the
+rq lock and enabled interrupts (but not preemption) an interrupt
+triggered. When the interrupt finished, it started running softirqs.
+But while the softirq was running, another interrupt triggered.
+When an interrupt is running inside a softirq, the annotation is 'H'.
 
 
 wakeup
 ------
 
+One common case that people are interested in tracing is the
+time it takes for a task that is woken to actually wake up.
+Now for non Real-Time tasks, this can be arbitrary. But tracing
+it none the less can be interesting. 
+
+Without function tracing:
+
+ # echo 0 > options/function-trace
+ # echo wakeup > current_tracer
+ # echo 1 > tracing_on
+ # echo 0 > tracing_max_latency
+ # chrt -f 5 sleep 1
+ # echo 0 > tracing_on
+ # cat trace
+# tracer: wakeup
+#
+# wakeup latency trace v1.1.5 on 3.8.0-test+
+# --------------------------------------------------------------------
+# latency: 15 us, #4/4, CPU#3 | (M:preempt VP:0, KP:0, SP:0 HP:0 #P:4)
+#    -----------------
+#    | task: kworker/3:1H-312 (uid:0 nice:-20 policy:0 rt_prio:0)
+#    -----------------
+#
+#                  _------=> CPU#            
+#                 / _-----=> irqs-off        
+#                | / _----=> need-resched    
+#                || / _---=> hardirq/softirq 
+#                ||| / _--=> preempt-depth   
+#                |||| /     delay             
+#  cmd     pid   ||||| time  |   caller      
+#     \   /      |||||  \    |   /           
+  <idle>-0       3dNs7    0us :      0:120:R   + [003]   312:100:R kworker/3:1H
+  <idle>-0       3dNs7    1us+: ttwu_do_activate.constprop.87 <-try_to_wake_up
+  <idle>-0       3d..3   15us : __schedule <-schedule
+  <idle>-0       3d..3   15us :      0:120:R ==> [003]   312:100:R kworker/3:1H
+
+The tracer only traces the highest priority task in the system
+to avoid tracing the normal circumstances. Here we see that
+the kworker with a nice priority of -20 (not very nice), took
+just 15 microseconds from the time it woke up, to the time it
+ran.
+
+Non Real-Time tasks are not that interesting. A more interesting
+trace is to concentrate only on Real-Time tasks.
+
+wakeup_rt
+---------
+
 In a Real-Time environment it is very important to know the
 wakeup time it takes for the highest priority task that is woken
 up to the time that it executes. This is also known as "schedule
@@ -914,124 +1423,229 @@ Real-Time environments are interested in the worst case latency.
 That is the longest latency it takes for something to happen,
 and not the average. We can have a very fast scheduler that may
 only have a large latency once in a while, but that would not
-work well with Real-Time tasks.  The wakeup tracer was designed
+work well with Real-Time tasks.  The wakeup_rt tracer was designed
 to record the worst case wakeups of RT tasks. Non-RT tasks are
 not recorded because the tracer only records one worst case and
 tracing non-RT tasks that are unpredictable will overwrite the
-worst case latency of RT tasks.
+worst case latency of RT tasks (just run the normal wakeup
+tracer for a while to see that effect).
 
 Since this tracer only deals with RT tasks, we will run this
 slightly differently than we did with the previous tracers.
 Instead of performing an 'ls', we will run 'sleep 1' under
 'chrt' which changes the priority of the task.
 
- # echo wakeup > current_tracer
- # echo latency-format > trace_options
- # echo 0 > tracing_max_latency
+ # echo 0 > options/function-trace
+ # echo wakeup_rt > current_tracer
  # echo 1 > tracing_on
+ # echo 0 > tracing_max_latency
  # chrt -f 5 sleep 1
  # echo 0 > tracing_on
  # cat trace
 # tracer: wakeup
 #
-wakeup latency trace v1.1.5 on 2.6.26-rc8
---------------------------------------------------------------------
- latency: 4 us, #2/2, CPU#1 | (M:preempt VP:0, KP:0, SP:0 HP:0 #P:2)
-    -----------------
-    | task: sleep-4901 (uid:0 nice:0 policy:1 rt_prio:5)
-    -----------------
-
-#                _------=> CPU#
-#               / _-----=> irqs-off
-#              | / _----=> need-resched
-#              || / _---=> hardirq/softirq
-#              ||| / _--=> preempt-depth
-#              |||| /
-#              |||||     delay
-#  cmd     pid ||||| time  |   caller
-#     \   /    |||||   \   |   /
-  <idle>-0     1d.h4    0us+: try_to_wake_up (wake_up_process)
-  <idle>-0     1d..4    4us : schedule (cpu_idle)
-
-
-Running this on an idle system, we see that it only took 4
-microseconds to perform the task switch.  Note, since the trace
-marker in the schedule is before the actual "switch", we stop
-the tracing when the recorded task is about to schedule in. This
-may change if we add a new marker at the end of the scheduler.
-
-Notice that the recorded task is 'sleep' with the PID of 4901
+# tracer: wakeup_rt
+#
+# wakeup_rt latency trace v1.1.5 on 3.8.0-test+
+# --------------------------------------------------------------------
+# latency: 5 us, #4/4, CPU#3 | (M:preempt VP:0, KP:0, SP:0 HP:0 #P:4)
+#    -----------------
+#    | task: sleep-2389 (uid:0 nice:0 policy:1 rt_prio:5)
+#    -----------------
+#
+#                  _------=> CPU#            
+#                 / _-----=> irqs-off        
+#                | / _----=> need-resched    
+#                || / _---=> hardirq/softirq 
+#                ||| / _--=> preempt-depth   
+#                |||| /     delay             
+#  cmd     pid   ||||| time  |   caller      
+#     \   /      |||||  \    |   /           
+  <idle>-0       3d.h4    0us :      0:120:R   + [003]  2389: 94:R sleep
+  <idle>-0       3d.h4    1us+: ttwu_do_activate.constprop.87 <-try_to_wake_up
+  <idle>-0       3d..3    5us : __schedule <-schedule
+  <idle>-0       3d..3    5us :      0:120:R ==> [003]  2389: 94:R sleep
+
+
+Running this on an idle system, we see that it only took 5 microseconds
+to perform the task switch.  Note, since the trace point in the schedule
+is before the actual "switch", we stop the tracing when the recorded task
+is about to schedule in. This may change if we add a new marker at the
+end of the scheduler.
+
+Notice that the recorded task is 'sleep' with the PID of 2389
 and it has an rt_prio of 5. This priority is user-space priority
 and not the internal kernel priority. The policy is 1 for
 SCHED_FIFO and 2 for SCHED_RR.
 
-Doing the same with chrt -r 5 and ftrace_enabled set.
+Note, that the trace data shows the internal priority (99 - rtprio).
 
-# tracer: wakeup
+  <idle>-0       3d..3    5us :      0:120:R ==> [003]  2389: 94:R sleep
+
+The 0:120:R means idle was running with a nice priority of 0 (120 - 20)
+and in the running state 'R'. The sleep task was scheduled in with
+2389: 94:R. That is the priority is the kernel rtprio (99 - 5 = 94)
+and it too is in the running state.
+
+Doing the same with chrt -r 5 and function-trace set.
+
+  echo 1 > options/function-trace
+
+# tracer: wakeup_rt
 #
-wakeup latency trace v1.1.5 on 2.6.26-rc8
---------------------------------------------------------------------
- latency: 50 us, #60/60, CPU#1 | (M:preempt VP:0, KP:0, SP:0 HP:0 #P:2)
-    -----------------
-    | task: sleep-4068 (uid:0 nice:0 policy:2 rt_prio:5)
-    -----------------
-
-#                _------=> CPU#
-#               / _-----=> irqs-off
-#              | / _----=> need-resched
-#              || / _---=> hardirq/softirq
-#              ||| / _--=> preempt-depth
-#              |||| /
-#              |||||     delay
-#  cmd     pid ||||| time  |   caller
-#     \   /    |||||   \   |   /
-ksoftirq-7     1d.H3    0us : try_to_wake_up (wake_up_process)
-ksoftirq-7     1d.H4    1us : sub_preempt_count (marker_probe_cb)
-ksoftirq-7     1d.H3    2us : check_preempt_wakeup (try_to_wake_up)
-ksoftirq-7     1d.H3    3us : update_curr (check_preempt_wakeup)
-ksoftirq-7     1d.H3    4us : calc_delta_mine (update_curr)
-ksoftirq-7     1d.H3    5us : __resched_task (check_preempt_wakeup)
-ksoftirq-7     1d.H3    6us : task_wake_up_rt (try_to_wake_up)
-ksoftirq-7     1d.H3    7us : _spin_unlock_irqrestore (try_to_wake_up)
-[...]
-ksoftirq-7     1d.H2   17us : irq_exit (smp_apic_timer_interrupt)
-ksoftirq-7     1d.H2   18us : sub_preempt_count (irq_exit)
-ksoftirq-7     1d.s3   19us : sub_preempt_count (irq_exit)
-ksoftirq-7     1..s2   20us : rcu_process_callbacks (__do_softirq)
-[...]
-ksoftirq-7     1..s2   26us : __rcu_process_callbacks (rcu_process_callbacks)
-ksoftirq-7     1d.s2   27us : _local_bh_enable (__do_softirq)
-ksoftirq-7     1d.s2   28us : sub_preempt_count (_local_bh_enable)
-ksoftirq-7     1.N.3   29us : sub_preempt_count (ksoftirqd)
-ksoftirq-7     1.N.2   30us : _cond_resched (ksoftirqd)
-ksoftirq-7     1.N.2   31us : __cond_resched (_cond_resched)
-ksoftirq-7     1.N.2   32us : add_preempt_count (__cond_resched)
-ksoftirq-7     1.N.2   33us : schedule (__cond_resched)
-ksoftirq-7     1.N.2   33us : add_preempt_count (schedule)
-ksoftirq-7     1.N.3   34us : hrtick_clear (schedule)
-ksoftirq-7     1dN.3   35us : _spin_lock (schedule)
-ksoftirq-7     1dN.3   36us : add_preempt_count (_spin_lock)
-ksoftirq-7     1d..4   37us : put_prev_task_fair (schedule)
-ksoftirq-7     1d..4   38us : update_curr (put_prev_task_fair)
-[...]
-ksoftirq-7     1d..5   47us : _spin_trylock (tracing_record_cmdline)
-ksoftirq-7     1d..5   48us : add_preempt_count (_spin_trylock)
-ksoftirq-7     1d..6   49us : _spin_unlock (tracing_record_cmdline)
-ksoftirq-7     1d..6   49us : sub_preempt_count (_spin_unlock)
-ksoftirq-7     1d..4   50us : schedule (__cond_resched)
-
-The interrupt went off while running ksoftirqd. This task runs
-at SCHED_OTHER. Why did not we see the 'N' set early? This may
-be a harmless bug with x86_32 and 4K stacks. On x86_32 with 4K
-stacks configured, the interrupt and softirq run with their own
-stack. Some information is held on the top of the task's stack
-(need_resched and preempt_count are both stored there). The
-setting of the NEED_RESCHED bit is done directly to the task's
-stack, but the reading of the NEED_RESCHED is done by looking at
-the current stack, which in this case is the stack for the hard
-interrupt. This hides the fact that NEED_RESCHED has been set.
-We do not see the 'N' until we switch back to the task's
-assigned stack.
+# wakeup_rt latency trace v1.1.5 on 3.8.0-test+
+# --------------------------------------------------------------------
+# latency: 29 us, #85/85, CPU#3 | (M:preempt VP:0, KP:0, SP:0 HP:0 #P:4)
+#    -----------------
+#    | task: sleep-2448 (uid:0 nice:0 policy:1 rt_prio:5)
+#    -----------------
+#
+#                  _------=> CPU#            
+#                 / _-----=> irqs-off        
+#                | / _----=> need-resched    
+#                || / _---=> hardirq/softirq 
+#                ||| / _--=> preempt-depth   
+#                |||| /     delay             
+#  cmd     pid   ||||| time  |   caller      
+#     \   /      |||||  \    |   /           
+  <idle>-0       3d.h4    1us+:      0:120:R   + [003]  2448: 94:R sleep
+  <idle>-0       3d.h4    2us : ttwu_do_activate.constprop.87 <-try_to_wake_up
+  <idle>-0       3d.h3    3us : check_preempt_curr <-ttwu_do_wakeup
+  <idle>-0       3d.h3    3us : resched_task <-check_preempt_curr
+  <idle>-0       3dNh3    4us : task_woken_rt <-ttwu_do_wakeup
+  <idle>-0       3dNh3    4us : _raw_spin_unlock <-try_to_wake_up
+  <idle>-0       3dNh3    4us : sub_preempt_count <-_raw_spin_unlock
+  <idle>-0       3dNh2    5us : ttwu_stat <-try_to_wake_up
+  <idle>-0       3dNh2    5us : _raw_spin_unlock_irqrestore <-try_to_wake_up
+  <idle>-0       3dNh2    6us : sub_preempt_count <-_raw_spin_unlock_irqrestore
+  <idle>-0       3dNh1    6us : _raw_spin_lock <-__run_hrtimer
+  <idle>-0       3dNh1    6us : add_preempt_count <-_raw_spin_lock
+  <idle>-0       3dNh2    7us : _raw_spin_unlock <-hrtimer_interrupt
+  <idle>-0       3dNh2    7us : sub_preempt_count <-_raw_spin_unlock
+  <idle>-0       3dNh1    7us : tick_program_event <-hrtimer_interrupt
+  <idle>-0       3dNh1    7us : clockevents_program_event <-tick_program_event
+  <idle>-0       3dNh1    8us : ktime_get <-clockevents_program_event
+  <idle>-0       3dNh1    8us : lapic_next_event <-clockevents_program_event
+  <idle>-0       3dNh1    8us : irq_exit <-smp_apic_timer_interrupt
+  <idle>-0       3dNh1    9us : sub_preempt_count <-irq_exit
+  <idle>-0       3dN.2    9us : idle_cpu <-irq_exit
+  <idle>-0       3dN.2    9us : rcu_irq_exit <-irq_exit
+  <idle>-0       3dN.2   10us : rcu_eqs_enter_common.isra.45 <-rcu_irq_exit
+  <idle>-0       3dN.2   10us : sub_preempt_count <-irq_exit
+  <idle>-0       3.N.1   11us : rcu_idle_exit <-cpu_idle
+  <idle>-0       3dN.1   11us : rcu_eqs_exit_common.isra.43 <-rcu_idle_exit
+  <idle>-0       3.N.1   11us : tick_nohz_idle_exit <-cpu_idle
+  <idle>-0       3dN.1   12us : menu_hrtimer_cancel <-tick_nohz_idle_exit
+  <idle>-0       3dN.1   12us : ktime_get <-tick_nohz_idle_exit
+  <idle>-0       3dN.1   12us : tick_do_update_jiffies64 <-tick_nohz_idle_exit
+  <idle>-0       3dN.1   13us : update_cpu_load_nohz <-tick_nohz_idle_exit
+  <idle>-0       3dN.1   13us : _raw_spin_lock <-update_cpu_load_nohz
+  <idle>-0       3dN.1   13us : add_preempt_count <-_raw_spin_lock
+  <idle>-0       3dN.2   13us : __update_cpu_load <-update_cpu_load_nohz
+  <idle>-0       3dN.2   14us : sched_avg_update <-__update_cpu_load
+  <idle>-0       3dN.2   14us : _raw_spin_unlock <-update_cpu_load_nohz
+  <idle>-0       3dN.2   14us : sub_preempt_count <-_raw_spin_unlock
+  <idle>-0       3dN.1   15us : calc_load_exit_idle <-tick_nohz_idle_exit
+  <idle>-0       3dN.1   15us : touch_softlockup_watchdog <-tick_nohz_idle_exit
+  <idle>-0       3dN.1   15us : hrtimer_cancel <-tick_nohz_idle_exit
+  <idle>-0       3dN.1   15us : hrtimer_try_to_cancel <-hrtimer_cancel
+  <idle>-0       3dN.1   16us : lock_hrtimer_base.isra.18 <-hrtimer_try_to_cancel
+  <idle>-0       3dN.1   16us : _raw_spin_lock_irqsave <-lock_hrtimer_base.isra.18
+  <idle>-0       3dN.1   16us : add_preempt_count <-_raw_spin_lock_irqsave
+  <idle>-0       3dN.2   17us : __remove_hrtimer <-remove_hrtimer.part.16
+  <idle>-0       3dN.2   17us : hrtimer_force_reprogram <-__remove_hrtimer
+  <idle>-0       3dN.2   17us : tick_program_event <-hrtimer_force_reprogram
+  <idle>-0       3dN.2   18us : clockevents_program_event <-tick_program_event
+  <idle>-0       3dN.2   18us : ktime_get <-clockevents_program_event
+  <idle>-0       3dN.2   18us : lapic_next_event <-clockevents_program_event
+  <idle>-0       3dN.2   19us : _raw_spin_unlock_irqrestore <-hrtimer_try_to_cancel
+  <idle>-0       3dN.2   19us : sub_preempt_count <-_raw_spin_unlock_irqrestore
+  <idle>-0       3dN.1   19us : hrtimer_forward <-tick_nohz_idle_exit
+  <idle>-0       3dN.1   20us : ktime_add_safe <-hrtimer_forward
+  <idle>-0       3dN.1   20us : ktime_add_safe <-hrtimer_forward
+  <idle>-0       3dN.1   20us : hrtimer_start_range_ns <-hrtimer_start_expires.constprop.11
+  <idle>-0       3dN.1   20us : __hrtimer_start_range_ns <-hrtimer_start_range_ns
+  <idle>-0       3dN.1   21us : lock_hrtimer_base.isra.18 <-__hrtimer_start_range_ns
+  <idle>-0       3dN.1   21us : _raw_spin_lock_irqsave <-lock_hrtimer_base.isra.18
+  <idle>-0       3dN.1   21us : add_preempt_count <-_raw_spin_lock_irqsave
+  <idle>-0       3dN.2   22us : ktime_add_safe <-__hrtimer_start_range_ns
+  <idle>-0       3dN.2   22us : enqueue_hrtimer <-__hrtimer_start_range_ns
+  <idle>-0       3dN.2   22us : tick_program_event <-__hrtimer_start_range_ns
+  <idle>-0       3dN.2   23us : clockevents_program_event <-tick_program_event
+  <idle>-0       3dN.2   23us : ktime_get <-clockevents_program_event
+  <idle>-0       3dN.2   23us : lapic_next_event <-clockevents_program_event
+  <idle>-0       3dN.2   24us : _raw_spin_unlock_irqrestore <-__hrtimer_start_range_ns
+  <idle>-0       3dN.2   24us : sub_preempt_count <-_raw_spin_unlock_irqrestore
+  <idle>-0       3dN.1   24us : account_idle_ticks <-tick_nohz_idle_exit
+  <idle>-0       3dN.1   24us : account_idle_time <-account_idle_ticks
+  <idle>-0       3.N.1   25us : sub_preempt_count <-cpu_idle
+  <idle>-0       3.N..   25us : schedule <-cpu_idle
+  <idle>-0       3.N..   25us : __schedule <-preempt_schedule
+  <idle>-0       3.N..   26us : add_preempt_count <-__schedule
+  <idle>-0       3.N.1   26us : rcu_note_context_switch <-__schedule
+  <idle>-0       3.N.1   26us : rcu_sched_qs <-rcu_note_context_switch
+  <idle>-0       3dN.1   27us : rcu_preempt_qs <-rcu_note_context_switch
+  <idle>-0       3.N.1   27us : _raw_spin_lock_irq <-__schedule
+  <idle>-0       3dN.1   27us : add_preempt_count <-_raw_spin_lock_irq
+  <idle>-0       3dN.2   28us : put_prev_task_idle <-__schedule
+  <idle>-0       3dN.2   28us : pick_next_task_stop <-pick_next_task
+  <idle>-0       3dN.2   28us : pick_next_task_rt <-pick_next_task
+  <idle>-0       3dN.2   29us : dequeue_pushable_task <-pick_next_task_rt
+  <idle>-0       3d..3   29us : __schedule <-preempt_schedule
+  <idle>-0       3d..3   30us :      0:120:R ==> [003]  2448: 94:R sleep
+
+This isn't that big of a trace, even with function tracing enabled,
+so I included the entire trace.
+
+The interrupt went off while when the system was idle. Somewhere
+before task_woken_rt() was called, the NEED_RESCHED flag was set,
+this is indicated by the first occurrence of the 'N' flag.
+
+Latency tracing and events
+--------------------------
+As function tracing can induce a much larger latency, but without
+seeing what happens within the latency it is hard to know what
+caused it. There is a middle ground, and that is with enabling
+events.
+
+ # echo 0 > options/function-trace
+ # echo wakeup_rt > current_tracer
+ # echo 1 > events/enable
+ # echo 1 > tracing_on
+ # echo 0 > tracing_max_latency
+ # chrt -f 5 sleep 1
+ # echo 0 > tracing_on
+ # cat trace
+# tracer: wakeup_rt
+#
+# wakeup_rt latency trace v1.1.5 on 3.8.0-test+
+# --------------------------------------------------------------------
+# latency: 6 us, #12/12, CPU#2 | (M:preempt VP:0, KP:0, SP:0 HP:0 #P:4)
+#    -----------------
+#    | task: sleep-5882 (uid:0 nice:0 policy:1 rt_prio:5)
+#    -----------------
+#
+#                  _------=> CPU#            
+#                 / _-----=> irqs-off        
+#                | / _----=> need-resched    
+#                || / _---=> hardirq/softirq 
+#                ||| / _--=> preempt-depth   
+#                |||| /     delay             
+#  cmd     pid   ||||| time  |   caller      
+#     \   /      |||||  \    |   /           
+  <idle>-0       2d.h4    0us :      0:120:R   + [002]  5882: 94:R sleep
+  <idle>-0       2d.h4    0us : ttwu_do_activate.constprop.87 <-try_to_wake_up
+  <idle>-0       2d.h4    1us : sched_wakeup: comm=sleep pid=5882 prio=94 success=1 target_cpu=002
+  <idle>-0       2dNh2    1us : hrtimer_expire_exit: hrtimer=ffff88007796feb8
+  <idle>-0       2.N.2    2us : power_end: cpu_id=2
+  <idle>-0       2.N.2    3us : cpu_idle: state=4294967295 cpu_id=2
+  <idle>-0       2dN.3    4us : hrtimer_cancel: hrtimer=ffff88007d50d5e0
+  <idle>-0       2dN.3    4us : hrtimer_start: hrtimer=ffff88007d50d5e0 function=tick_sched_timer expires=34311211000000 softexpires=34311211000000
+  <idle>-0       2.N.2    5us : rcu_utilization: Start context switch
+  <idle>-0       2.N.2    5us : rcu_utilization: End context switch
+  <idle>-0       2d..3    6us : __schedule <-schedule
+  <idle>-0       2d..3    6us :      0:120:R ==> [002]  5882: 94:R sleep
+
 
 function
 --------
@@ -1039,6 +1653,7 @@ function
 This tracer is the function tracer. Enabling the function tracer
 can be done from the debug file system. Make sure the
 ftrace_enabled is set; otherwise this tracer is a nop.
+See the "ftrace_enabled" section below.
 
  # sysctl kernel.ftrace_enabled=1
  # echo function > current_tracer
@@ -1048,23 +1663,23 @@ ftrace_enabled is set; otherwise this tracer is a nop.
  # cat trace
 # tracer: function
 #
-#           TASK-PID   CPU#    TIMESTAMP  FUNCTION
-#              | |      |          |         |
-            bash-4003  [00]   123.638713: finish_task_switch <-schedule
-            bash-4003  [00]   123.638714: _spin_unlock_irq <-finish_task_switch
-            bash-4003  [00]   123.638714: sub_preempt_count <-_spin_unlock_irq
-            bash-4003  [00]   123.638715: hrtick_set <-schedule
-            bash-4003  [00]   123.638715: _spin_lock_irqsave <-hrtick_set
-            bash-4003  [00]   123.638716: add_preempt_count <-_spin_lock_irqsave
-            bash-4003  [00]   123.638716: _spin_unlock_irqrestore <-hrtick_set
-            bash-4003  [00]   123.638717: sub_preempt_count <-_spin_unlock_irqrestore
-            bash-4003  [00]   123.638717: hrtick_clear <-hrtick_set
-            bash-4003  [00]   123.638718: sub_preempt_count <-schedule
-            bash-4003  [00]   123.638718: sub_preempt_count <-preempt_schedule
-            bash-4003  [00]   123.638719: wait_for_completion <-__stop_machine_run
-            bash-4003  [00]   123.638719: wait_for_common <-wait_for_completion
-            bash-4003  [00]   123.638720: _spin_lock_irq <-wait_for_common
-            bash-4003  [00]   123.638720: add_preempt_count <-_spin_lock_irq
+# entries-in-buffer/entries-written: 24799/24799   #P:4
+#
+#                              _-----=> irqs-off
+#                             / _----=> need-resched
+#                            | / _---=> hardirq/softirq
+#                            || / _--=> preempt-depth
+#                            ||| /     delay
+#           TASK-PID   CPU#  ||||    TIMESTAMP  FUNCTION
+#              | |       |   ||||       |         |
+            bash-1994  [002] ....  3082.063030: mutex_unlock <-rb_simple_write
+            bash-1994  [002] ....  3082.063031: __mutex_unlock_slowpath <-mutex_unlock
+            bash-1994  [002] ....  3082.063031: __fsnotify_parent <-fsnotify_modify
+            bash-1994  [002] ....  3082.063032: fsnotify <-fsnotify_modify
+            bash-1994  [002] ....  3082.063032: __srcu_read_lock <-fsnotify
+            bash-1994  [002] ....  3082.063032: add_preempt_count <-__srcu_read_lock
+            bash-1994  [002] ...1  3082.063032: sub_preempt_count <-__srcu_read_lock
+            bash-1994  [002] ....  3082.063033: __srcu_read_unlock <-fsnotify
 [...]
 
 
@@ -1214,79 +1829,19 @@ int main (int argc, char **argv)
         return 0;
 }
 
+Or this simple script!
 
-hw-branch-tracer (x86 only)
----------------------------
-
-This tracer uses the x86 last branch tracing hardware feature to
-collect a branch trace on all cpus with relatively low overhead.
-
-The tracer uses a fixed-size circular buffer per cpu and only
-traces ring 0 branches. The trace file dumps that buffer in the
-following format:
-
-# tracer: hw-branch-tracer
-#
-# CPU#        TO  <-  FROM
-   0  scheduler_tick+0xb5/0x1bf	  <-  task_tick_idle+0x5/0x6
-   2  run_posix_cpu_timers+0x2b/0x72a	  <-  run_posix_cpu_timers+0x25/0x72a
-   0  scheduler_tick+0x139/0x1bf	  <-  scheduler_tick+0xed/0x1bf
-   0  scheduler_tick+0x17c/0x1bf	  <-  scheduler_tick+0x148/0x1bf
-   2  run_posix_cpu_timers+0x9e/0x72a	  <-  run_posix_cpu_timers+0x5e/0x72a
-   0  scheduler_tick+0x1b6/0x1bf	  <-  scheduler_tick+0x1aa/0x1bf
-
-
-The tracer may be used to dump the trace for the oops'ing cpu on
-a kernel oops into the system log. To enable this,
-ftrace_dump_on_oops must be set. To set ftrace_dump_on_oops, one
-can either use the sysctl function or set it via the proc system
-interface.
-
-  sysctl kernel.ftrace_dump_on_oops=n
-
-or
-
-  echo n > /proc/sys/kernel/ftrace_dump_on_oops
-
-If n = 1, ftrace will dump buffers of all CPUs, if n = 2 ftrace will
-only dump the buffer of the CPU that triggered the oops.
-
-Here's an example of such a dump after a null pointer
-dereference in a kernel module:
-
-[57848.105921] BUG: unable to handle kernel NULL pointer dereference at 0000000000000000
-[57848.106019] IP: [<ffffffffa0000006>] open+0x6/0x14 [oops]
-[57848.106019] PGD 2354e9067 PUD 2375e7067 PMD 0
-[57848.106019] Oops: 0002 [#1] SMP
-[57848.106019] last sysfs file: /sys/devices/pci0000:00/0000:00:1e.0/0000:20:05.0/local_cpus
-[57848.106019] Dumping ftrace buffer:
-[57848.106019] ---------------------------------
-[...]
-[57848.106019]    0  chrdev_open+0xe6/0x165	  <-  cdev_put+0x23/0x24
-[57848.106019]    0  chrdev_open+0x117/0x165	  <-  chrdev_open+0xfa/0x165
-[57848.106019]    0  chrdev_open+0x120/0x165	  <-  chrdev_open+0x11c/0x165
-[57848.106019]    0  chrdev_open+0x134/0x165	  <-  chrdev_open+0x12b/0x165
-[57848.106019]    0  open+0x0/0x14 [oops]	  <-  chrdev_open+0x144/0x165
-[57848.106019]    0  page_fault+0x0/0x30	  <-  open+0x6/0x14 [oops]
-[57848.106019]    0  error_entry+0x0/0x5b	  <-  page_fault+0x4/0x30
-[57848.106019]    0  error_kernelspace+0x0/0x31	  <-  error_entry+0x59/0x5b
-[57848.106019]    0  error_sti+0x0/0x1	  <-  error_kernelspace+0x2d/0x31
-[57848.106019]    0  page_fault+0x9/0x30	  <-  error_sti+0x0/0x1
-[57848.106019]    0  do_page_fault+0x0/0x881	  <-  page_fault+0x1a/0x30
-[...]
-[57848.106019]    0  do_page_fault+0x66b/0x881	  <-  is_prefetch+0x1ee/0x1f2
-[57848.106019]    0  do_page_fault+0x6e0/0x881	  <-  do_page_fault+0x67a/0x881
-[57848.106019]    0  oops_begin+0x0/0x96	  <-  do_page_fault+0x6e0/0x881
-[57848.106019]    0  trace_hw_branch_oops+0x0/0x2d	  <-  oops_begin+0x9/0x96
-[...]
-[57848.106019]    0  ds_suspend_bts+0x2a/0xe3	  <-  ds_suspend_bts+0x1a/0xe3
-[57848.106019] ---------------------------------
-[57848.106019] CPU 0
-[57848.106019] Modules linked in: oops
-[57848.106019] Pid: 5542, comm: cat Tainted: G        W  2.6.28 #23
-[57848.106019] RIP: 0010:[<ffffffffa0000006>]  [<ffffffffa0000006>] open+0x6/0x14 [oops]
-[57848.106019] RSP: 0018:ffff880235457d48  EFLAGS: 00010246
-[...]
+------
+#!/bin/bash
+
+debugfs=`sed -ne 's/^debugfs \(.*\) debugfs.*/\1/p' /proc/mounts`
+echo nop > $debugfs/tracing/current_tracer
+echo 0 > $debugfs/tracing/tracing_on
+echo $$ > $debugfs/tracing/set_ftrace_pid
+echo function > $debugfs/tracing/current_tracer
+echo 1 > $debugfs/tracing/tracing_on
+exec "$@"
+------
 
 
 function graph tracer
@@ -1473,16 +2028,18 @@ starts of pointing to a simple return. (Enabling FTRACE will
 include the -pg switch in the compiling of the kernel.)
 
 At compile time every C file object is run through the
-recordmcount.pl script (located in the scripts directory). This
-script will process the C object using objdump to find all the
-locations in the .text section that call mcount. (Note, only the
-.text section is processed, since processing other sections like
-.init.text may cause races due to those sections being freed).
+recordmcount program (located in the scripts directory). This
+program will parse the ELF headers in the C object to find all
+the locations in the .text section that call mcount. (Note, only
+white listed .text sections are processed, since processing other
+sections like .init.text may cause races due to those sections
+being freed unexpectedly).
 
 A new section called "__mcount_loc" is created that holds
 references to all the mcount call sites in the .text section.
-This section is compiled back into the original object. The
-final linker will add all these references into a single table.
+The recordmcount program re-links this section back into the
+original object. The final linking stage of the kernel will add all these
+references into a single table.
 
 On boot up, before SMP is initialized, the dynamic ftrace code
 scans this table and updates all the locations into nops. It
@@ -1493,13 +2050,25 @@ unloaded, it also removes its functions from the ftrace function
 list. This is automatic in the module unload code, and the
 module author does not need to worry about it.
 
-When tracing is enabled, kstop_machine is called to prevent
-races with the CPUS executing code being modified (which can
-cause the CPU to do undesirable things), and the nops are
+When tracing is enabled, the process of modifying the function
+tracepoints is dependent on architecture. The old method is to use
+kstop_machine to prevent races with the CPUs executing code being
+modified (which can cause the CPU to do undesirable things, especially
+if the modified code crosses cache (or page) boundaries), and the nops are
 patched back to calls. But this time, they do not call mcount
 (which is just a function stub). They now call into the ftrace
 infrastructure.
 
+The new method of modifying the function tracepoints is to place
+a breakpoint at the location to be modified, sync all CPUs, modify
+the rest of the instruction not covered by the breakpoint. Sync
+all CPUs again, and then remove the breakpoint with the finished
+version to the ftrace call site.
+
+Some archs do not even need to monkey around with the synchronization,
+and can just slap the new code on top of the old without any
+problems with other CPUs executing it at the same time.
+
 One special side-effect to the recording of the functions being
 traced is that we can now selectively choose which functions we
 wish to trace and which ones we want the mcount calls to remain
@@ -1530,20 +2099,28 @@ mutex_lock
 
 If I am only interested in sys_nanosleep and hrtimer_interrupt:
 
- # echo sys_nanosleep hrtimer_interrupt \
-		> set_ftrace_filter
+ # echo sys_nanosleep hrtimer_interrupt > set_ftrace_filter
  # echo function > current_tracer
  # echo 1 > tracing_on
  # usleep 1
  # echo 0 > tracing_on
  # cat trace
-# tracer: ftrace
+# tracer: function
+#
+# entries-in-buffer/entries-written: 5/5   #P:4
 #
-#           TASK-PID   CPU#    TIMESTAMP  FUNCTION
-#              | |      |          |         |
-          usleep-4134  [00]  1317.070017: hrtimer_interrupt <-smp_apic_timer_interrupt
-          usleep-4134  [00]  1317.070111: sys_nanosleep <-syscall_call
-          <idle>-0     [00]  1317.070115: hrtimer_interrupt <-smp_apic_timer_interrupt
+#                              _-----=> irqs-off
+#                             / _----=> need-resched
+#                            | / _---=> hardirq/softirq
+#                            || / _--=> preempt-depth
+#                            ||| /     delay
+#           TASK-PID   CPU#  ||||    TIMESTAMP  FUNCTION
+#              | |       |   ||||       |         |
+          usleep-2665  [001] ....  4186.475355: sys_nanosleep <-system_call_fastpath
+          <idle>-0     [001] d.h1  4186.475409: hrtimer_interrupt <-smp_apic_timer_interrupt
+          usleep-2665  [001] d.h1  4186.475426: hrtimer_interrupt <-smp_apic_timer_interrupt
+          <idle>-0     [003] d.h1  4186.475426: hrtimer_interrupt <-smp_apic_timer_interrupt
+          <idle>-0     [002] d.h1  4186.475427: hrtimer_interrupt <-smp_apic_timer_interrupt
 
 To see which functions are being traced, you can cat the file:
 
@@ -1571,20 +2148,25 @@ Note: It is better to use quotes to enclose the wild cards,
 
 Produces:
 
-# tracer: ftrace
+# tracer: function
 #
-#           TASK-PID   CPU#    TIMESTAMP  FUNCTION
-#              | |      |          |         |
-            bash-4003  [00]  1480.611794: hrtimer_init <-copy_process
-            bash-4003  [00]  1480.611941: hrtimer_start <-hrtick_set
-            bash-4003  [00]  1480.611956: hrtimer_cancel <-hrtick_clear
-            bash-4003  [00]  1480.611956: hrtimer_try_to_cancel <-hrtimer_cancel
-          <idle>-0     [00]  1480.612019: hrtimer_get_next_event <-get_next_timer_interrupt
-          <idle>-0     [00]  1480.612025: hrtimer_get_next_event <-get_next_timer_interrupt
-          <idle>-0     [00]  1480.612032: hrtimer_get_next_event <-get_next_timer_interrupt
-          <idle>-0     [00]  1480.612037: hrtimer_get_next_event <-get_next_timer_interrupt
-          <idle>-0     [00]  1480.612382: hrtimer_get_next_event <-get_next_timer_interrupt
-
+# entries-in-buffer/entries-written: 897/897   #P:4
+#
+#                              _-----=> irqs-off
+#                             / _----=> need-resched
+#                            | / _---=> hardirq/softirq
+#                            || / _--=> preempt-depth
+#                            ||| /     delay
+#           TASK-PID   CPU#  ||||    TIMESTAMP  FUNCTION
+#              | |       |   ||||       |         |
+          <idle>-0     [003] dN.1  4228.547803: hrtimer_cancel <-tick_nohz_idle_exit
+          <idle>-0     [003] dN.1  4228.547804: hrtimer_try_to_cancel <-hrtimer_cancel
+          <idle>-0     [003] dN.2  4228.547805: hrtimer_force_reprogram <-__remove_hrtimer
+          <idle>-0     [003] dN.1  4228.547805: hrtimer_forward <-tick_nohz_idle_exit
+          <idle>-0     [003] dN.1  4228.547805: hrtimer_start_range_ns <-hrtimer_start_expires.constprop.11
+          <idle>-0     [003] d..1  4228.547858: hrtimer_get_next_event <-get_next_timer_interrupt
+          <idle>-0     [003] d..1  4228.547859: hrtimer_start <-__tick_nohz_idle_enter
+          <idle>-0     [003] d..2  4228.547860: hrtimer_force_reprogram <-__rem
 
 Notice that we lost the sys_nanosleep.
 
@@ -1651,19 +2233,29 @@ traced.
 
 Produces:
 
-# tracer: ftrace
+# tracer: function
+#
+# entries-in-buffer/entries-written: 39608/39608   #P:4
 #
-#           TASK-PID   CPU#    TIMESTAMP  FUNCTION
-#              | |      |          |         |
-            bash-4043  [01]   115.281644: finish_task_switch <-schedule
-            bash-4043  [01]   115.281645: hrtick_set <-schedule
-            bash-4043  [01]   115.281645: hrtick_clear <-hrtick_set
-            bash-4043  [01]   115.281646: wait_for_completion <-__stop_machine_run
-            bash-4043  [01]   115.281647: wait_for_common <-wait_for_completion
-            bash-4043  [01]   115.281647: kthread_stop <-stop_machine_run
-            bash-4043  [01]   115.281648: init_waitqueue_head <-kthread_stop
-            bash-4043  [01]   115.281648: wake_up_process <-kthread_stop
-            bash-4043  [01]   115.281649: try_to_wake_up <-wake_up_process
+#                              _-----=> irqs-off
+#                             / _----=> need-resched
+#                            | / _---=> hardirq/softirq
+#                            || / _--=> preempt-depth
+#                            ||| /     delay
+#           TASK-PID   CPU#  ||||    TIMESTAMP  FUNCTION
+#              | |       |   ||||       |         |
+            bash-1994  [000] ....  4342.324896: file_ra_state_init <-do_dentry_open
+            bash-1994  [000] ....  4342.324897: open_check_o_direct <-do_last
+            bash-1994  [000] ....  4342.324897: ima_file_check <-do_last
+            bash-1994  [000] ....  4342.324898: process_measurement <-ima_file_check
+            bash-1994  [000] ....  4342.324898: ima_get_action <-process_measurement
+            bash-1994  [000] ....  4342.324898: ima_match_policy <-ima_get_action
+            bash-1994  [000] ....  4342.324899: do_truncate <-do_last
+            bash-1994  [000] ....  4342.324899: should_remove_suid <-do_truncate
+            bash-1994  [000] ....  4342.324899: notify_change <-do_truncate
+            bash-1994  [000] ....  4342.324900: current_fs_time <-notify_change
+            bash-1994  [000] ....  4342.324900: current_kernel_time <-current_fs_time
+            bash-1994  [000] ....  4342.324900: timespec_trunc <-current_fs_time
 
 We can see that there's no more lock or preempt tracing.
 
@@ -1729,6 +2321,28 @@ this special filter via:
  echo > set_graph_function
 
 
+ftrace_enabled
+--------------
+
+Note, the proc sysctl ftrace_enable is a big on/off switch for the
+function tracer. By default it is enabled (when function tracing is
+enabled in the kernel). If it is disabled, all function tracing is
+disabled. This includes not only the function tracers for ftrace, but
+also for any other uses (perf, kprobes, stack tracing, profiling, etc).
+
+Please disable this with care.
+
+This can be disable (and enabled) with:
+
+  sysctl kernel.ftrace_enabled=0
+  sysctl kernel.ftrace_enabled=1
+
+ or
+
+  echo 0 > /proc/sys/kernel/ftrace_enabled
+  echo 1 > /proc/sys/kernel/ftrace_enabled
+
+
 Filter commands
 ---------------
 
@@ -1763,12 +2377,58 @@ The following commands are supported:
 
    echo '__schedule_bug:traceoff:5' > set_ftrace_filter
 
+  To always disable tracing when __schedule_bug is hit:
+
+   echo '__schedule_bug:traceoff' > set_ftrace_filter
+
   These commands are cumulative whether or not they are appended
   to set_ftrace_filter. To remove a command, prepend it by '!'
   and drop the parameter:
 
+   echo '!__schedule_bug:traceoff:0' > set_ftrace_filter
+
+    The above removes the traceoff command for __schedule_bug
+    that have a counter. To remove commands without counters:
+
    echo '!__schedule_bug:traceoff' > set_ftrace_filter
 
+- snapshot
+  Will cause a snapshot to be triggered when the function is hit.
+
+   echo 'native_flush_tlb_others:snapshot' > set_ftrace_filter
+
+  To only snapshot once:
+
+   echo 'native_flush_tlb_others:snapshot:1' > set_ftrace_filter
+
+  To remove the above commands:
+
+   echo '!native_flush_tlb_others:snapshot' > set_ftrace_filter
+   echo '!native_flush_tlb_others:snapshot:0' > set_ftrace_filter
+
+- enable_event/disable_event
+  These commands can enable or disable a trace event. Note, because
+  function tracing callbacks are very sensitive, when these commands
+  are registered, the trace point is activated, but disabled in
+  a "soft" mode. That is, the tracepoint will be called, but
+  just will not be traced. The event tracepoint stays in this mode
+  as long as there's a command that triggers it.
+
+   echo 'try_to_wake_up:enable_event:sched:sched_switch:2' > \
+   	 set_ftrace_filter
+
+  The format is:
+
+    <function>:enable_event:<system>:<event>[:count]
+    <function>:disable_event:<system>:<event>[:count]
+
+  To remove the events commands:
+
+
+   echo '!try_to_wake_up:enable_event:sched:sched_switch:0' > \
+   	 set_ftrace_filter
+   echo '!schedule:disable_event:sched:sched_switch' > \
+   	 set_ftrace_filter
 
 trace_pipe
 ----------
@@ -1787,28 +2447,31 @@ different. The trace is live.
  # cat trace
 # tracer: function
 #
-#           TASK-PID   CPU#    TIMESTAMP  FUNCTION
-#              | |      |          |         |
+# entries-in-buffer/entries-written: 0/0   #P:4
+#
+#                              _-----=> irqs-off
+#                             / _----=> need-resched
+#                            | / _---=> hardirq/softirq
+#                            || / _--=> preempt-depth
+#                            ||| /     delay
+#           TASK-PID   CPU#  ||||    TIMESTAMP  FUNCTION
+#              | |       |   ||||       |         |
 
  #
  # cat /tmp/trace.out
-            bash-4043  [00] 41.267106: finish_task_switch <-schedule
-            bash-4043  [00] 41.267106: hrtick_set <-schedule
-            bash-4043  [00] 41.267107: hrtick_clear <-hrtick_set
-            bash-4043  [00] 41.267108: wait_for_completion <-__stop_machine_run
-            bash-4043  [00] 41.267108: wait_for_common <-wait_for_completion
-            bash-4043  [00] 41.267109: kthread_stop <-stop_machine_run
-            bash-4043  [00] 41.267109: init_waitqueue_head <-kthread_stop
-            bash-4043  [00] 41.267110: wake_up_process <-kthread_stop
-            bash-4043  [00] 41.267110: try_to_wake_up <-wake_up_process
-            bash-4043  [00] 41.267111: select_task_rq_rt <-try_to_wake_up
+            bash-1994  [000] ....  5281.568961: mutex_unlock <-rb_simple_write
+            bash-1994  [000] ....  5281.568963: __mutex_unlock_slowpath <-mutex_unlock
+            bash-1994  [000] ....  5281.568963: __fsnotify_parent <-fsnotify_modify
+            bash-1994  [000] ....  5281.568964: fsnotify <-fsnotify_modify
+            bash-1994  [000] ....  5281.568964: __srcu_read_lock <-fsnotify
+            bash-1994  [000] ....  5281.568964: add_preempt_count <-__srcu_read_lock
+            bash-1994  [000] ...1  5281.568965: sub_preempt_count <-__srcu_read_lock
+            bash-1994  [000] ....  5281.568965: __srcu_read_unlock <-fsnotify
+            bash-1994  [000] ....  5281.568967: sys_dup2 <-system_call_fastpath
 
 
 Note, reading the trace_pipe file will block until more input is
-added. By changing the tracer, trace_pipe will issue an EOF. We
-needed to set the function tracer _before_ we "cat" the
-trace_pipe file.
-
+added.
 
 trace entries
 -------------
@@ -1817,31 +2480,50 @@ Having too much or not enough data can be troublesome in
 diagnosing an issue in the kernel. The file buffer_size_kb is
 used to modify the size of the internal trace buffers. The
 number listed is the number of entries that can be recorded per
-CPU. To know the full size, multiply the number of possible CPUS
+CPU. To know the full size, multiply the number of possible CPUs
 with the number of entries.
 
  # cat buffer_size_kb
 1408 (units kilobytes)
 
-Note, to modify this, you must have tracing completely disabled.
-To do that, echo "nop" into the current_tracer. If the
-current_tracer is not set to "nop", an EINVAL error will be
-returned.
+Or simply read buffer_total_size_kb
+
+ # cat buffer_total_size_kb 
+5632
+
+To modify the buffer, simple echo in a number (in 1024 byte segments).
 
- # echo nop > current_tracer
  # echo 10000 > buffer_size_kb
  # cat buffer_size_kb
 10000 (units kilobytes)
 
-The number of pages which will be allocated is limited to a
-percentage of available memory. Allocating too much will produce
-an error.
+It will try to allocate as much as possible. If you allocate too
+much, it can cause Out-Of-Memory to trigger.
 
  # echo 1000000000000 > buffer_size_kb
 -bash: echo: write error: Cannot allocate memory
  # cat buffer_size_kb
 85
 
+The per_cpu buffers can be changed individually as well:
+
+ # echo 10000 > per_cpu/cpu0/buffer_size_kb
+ # echo 100 > per_cpu/cpu1/buffer_size_kb
+
+When the per_cpu buffers are not the same, the buffer_size_kb
+at the top level will just show an X
+
+ # cat buffer_size_kb
+X
+
+This is where the buffer_total_size_kb is useful:
+
+ # cat buffer_total_size_kb 
+12916
+
+Writing to the top level buffer_size_kb will reset all the buffers
+to be the same again.
+
 Snapshot
 --------
 CONFIG_TRACER_SNAPSHOT makes a generic snapshot feature
@@ -1925,7 +2607,188 @@ bash: echo: write error: Device or resource busy
  # cat snapshot
 cat: snapshot: Device or resource busy
 
+
+Instances
+---------
+In the debugfs tracing directory is a directory called "instances".
+This directory can have new directories created inside of it using
+mkdir, and removing directories with rmdir. The directory created
+with mkdir in this directory will already contain files and other
+directories after it is created.
+
+ # mkdir instances/foo
+ # ls instances/foo
+buffer_size_kb  buffer_total_size_kb  events  free_buffer  per_cpu
+set_event  snapshot  trace  trace_clock  trace_marker  trace_options
+trace_pipe  tracing_on
+
+As you can see, the new directory looks similar to the tracing directory
+itself. In fact, it is very similar, except that the buffer and
+events are agnostic from the main director, or from any other
+instances that are created.
+
+The files in the new directory work just like the files with the
+same name in the tracing directory except the buffer that is used
+is a separate and new buffer. The files affect that buffer but do not
+affect the main buffer with the exception of trace_options. Currently,
+the trace_options affect all instances and the top level buffer
+the same, but this may change in future releases. That is, options
+may become specific to the instance they reside in.
+
+Notice that none of the function tracer files are there, nor is
+current_tracer and available_tracers. This is because the buffers
+can currently only have events enabled for them.
+
+ # mkdir instances/foo
+ # mkdir instances/bar
+ # mkdir instances/zoot
+ # echo 100000 > buffer_size_kb
+ # echo 1000 > instances/foo/buffer_size_kb
+ # echo 5000 > instances/bar/per_cpu/cpu1/buffer_size_kb
+ # echo function > current_trace
+ # echo 1 > instances/foo/events/sched/sched_wakeup/enable
+ # echo 1 > instances/foo/events/sched/sched_wakeup_new/enable
+ # echo 1 > instances/foo/events/sched/sched_switch/enable
+ # echo 1 > instances/bar/events/irq/enable
+ # echo 1 > instances/zoot/events/syscalls/enable
+ # cat trace_pipe
+CPU:2 [LOST 11745 EVENTS]
+            bash-2044  [002] .... 10594.481032: _raw_spin_lock_irqsave <-get_page_from_freelist
+            bash-2044  [002] d... 10594.481032: add_preempt_count <-_raw_spin_lock_irqsave
+            bash-2044  [002] d..1 10594.481032: __rmqueue <-get_page_from_freelist
+            bash-2044  [002] d..1 10594.481033: _raw_spin_unlock <-get_page_from_freelist
+            bash-2044  [002] d..1 10594.481033: sub_preempt_count <-_raw_spin_unlock
+            bash-2044  [002] d... 10594.481033: get_pageblock_flags_group <-get_pageblock_migratetype
+            bash-2044  [002] d... 10594.481034: __mod_zone_page_state <-get_page_from_freelist
+            bash-2044  [002] d... 10594.481034: zone_statistics <-get_page_from_freelist
+            bash-2044  [002] d... 10594.481034: __inc_zone_state <-zone_statistics
+            bash-2044  [002] d... 10594.481034: __inc_zone_state <-zone_statistics
+            bash-2044  [002] .... 10594.481035: arch_dup_task_struct <-copy_process
+[...]
+
+ # cat instances/foo/trace_pipe
+            bash-1998  [000] d..4   136.676759: sched_wakeup: comm=kworker/0:1 pid=59 prio=120 success=1 target_cpu=000
+            bash-1998  [000] dN.4   136.676760: sched_wakeup: comm=bash pid=1998 prio=120 success=1 target_cpu=000
+          <idle>-0     [003] d.h3   136.676906: sched_wakeup: comm=rcu_preempt pid=9 prio=120 success=1 target_cpu=003
+          <idle>-0     [003] d..3   136.676909: sched_switch: prev_comm=swapper/3 prev_pid=0 prev_prio=120 prev_state=R ==> next_comm=rcu_preempt next_pid=9 next_prio=120
+     rcu_preempt-9     [003] d..3   136.676916: sched_switch: prev_comm=rcu_preempt prev_pid=9 prev_prio=120 prev_state=S ==> next_comm=swapper/3 next_pid=0 next_prio=120
+            bash-1998  [000] d..4   136.677014: sched_wakeup: comm=kworker/0:1 pid=59 prio=120 success=1 target_cpu=000
+            bash-1998  [000] dN.4   136.677016: sched_wakeup: comm=bash pid=1998 prio=120 success=1 target_cpu=000
+            bash-1998  [000] d..3   136.677018: sched_switch: prev_comm=bash prev_pid=1998 prev_prio=120 prev_state=R+ ==> next_comm=kworker/0:1 next_pid=59 next_prio=120
+     kworker/0:1-59    [000] d..4   136.677022: sched_wakeup: comm=sshd pid=1995 prio=120 success=1 target_cpu=001
+     kworker/0:1-59    [000] d..3   136.677025: sched_switch: prev_comm=kworker/0:1 prev_pid=59 prev_prio=120 prev_state=S ==> next_comm=bash next_pid=1998 next_prio=120
+[...]
+
+ # cat instances/bar/trace_pipe
+     migration/1-14    [001] d.h3   138.732674: softirq_raise: vec=3 [action=NET_RX]
+          <idle>-0     [001] dNh3   138.732725: softirq_raise: vec=3 [action=NET_RX]
+            bash-1998  [000] d.h1   138.733101: softirq_raise: vec=1 [action=TIMER]
+            bash-1998  [000] d.h1   138.733102: softirq_raise: vec=9 [action=RCU]
+            bash-1998  [000] ..s2   138.733105: softirq_entry: vec=1 [action=TIMER]
+            bash-1998  [000] ..s2   138.733106: softirq_exit: vec=1 [action=TIMER]
+            bash-1998  [000] ..s2   138.733106: softirq_entry: vec=9 [action=RCU]
+            bash-1998  [000] ..s2   138.733109: softirq_exit: vec=9 [action=RCU]
+            sshd-1995  [001] d.h1   138.733278: irq_handler_entry: irq=21 name=uhci_hcd:usb4
+            sshd-1995  [001] d.h1   138.733280: irq_handler_exit: irq=21 ret=unhandled
+            sshd-1995  [001] d.h1   138.733281: irq_handler_entry: irq=21 name=eth0
+            sshd-1995  [001] d.h1   138.733283: irq_handler_exit: irq=21 ret=handled
+[...]
+
+ # cat instances/zoot/trace
+# tracer: nop
+#
+# entries-in-buffer/entries-written: 18996/18996   #P:4
+#
+#                              _-----=> irqs-off
+#                             / _----=> need-resched
+#                            | / _---=> hardirq/softirq
+#                            || / _--=> preempt-depth
+#                            ||| /     delay
+#           TASK-PID   CPU#  ||||    TIMESTAMP  FUNCTION
+#              | |       |   ||||       |         |
+            bash-1998  [000] d...   140.733501: sys_write -> 0x2
+            bash-1998  [000] d...   140.733504: sys_dup2(oldfd: a, newfd: 1)
+            bash-1998  [000] d...   140.733506: sys_dup2 -> 0x1
+            bash-1998  [000] d...   140.733508: sys_fcntl(fd: a, cmd: 1, arg: 0)
+            bash-1998  [000] d...   140.733509: sys_fcntl -> 0x1
+            bash-1998  [000] d...   140.733510: sys_close(fd: a)
+            bash-1998  [000] d...   140.733510: sys_close -> 0x0
+            bash-1998  [000] d...   140.733514: sys_rt_sigprocmask(how: 0, nset: 0, oset: 6e2768, sigsetsize: 8)
+            bash-1998  [000] d...   140.733515: sys_rt_sigprocmask -> 0x0
+            bash-1998  [000] d...   140.733516: sys_rt_sigaction(sig: 2, act: 7fff718846f0, oact: 7fff71884650, sigsetsize: 8)
+            bash-1998  [000] d...   140.733516: sys_rt_sigaction -> 0x0
+
+You can see that the trace of the top most trace buffer shows only
+the function tracing. The foo instance displays wakeups and task
+switches.
+
+To remove the instances, simply delete their directories:
+
+ # rmdir instances/foo
+ # rmdir instances/bar
+ # rmdir instances/zoot
+
+Note, if a process has a trace file open in one of the instance
+directories, the rmdir will fail with EBUSY.
+
+
+Stack trace
 -----------
+Since the kernel has a fixed sized stack, it is important not to
+waste it in functions. A kernel developer must be conscience of
+what they allocate on the stack. If they add too much, the system
+can be in danger of a stack overflow, and corruption will occur,
+usually leading to a system panic.
+
+There are some tools that check this, usually with interrupts
+periodically checking usage. But if you can perform a check
+at every function call that will become very useful. As ftrace provides
+a function tracer, it makes it convenient to check the stack size
+at every function call. This is enabled via the stack tracer.
+
+CONFIG_STACK_TRACER enables the ftrace stack tracing functionality.
+To enable it, write a '1' into /proc/sys/kernel/stack_tracer_enabled.
+
+ # echo 1 > /proc/sys/kernel/stack_tracer_enabled
+
+You can also enable it from the kernel command line to trace
+the stack size of the kernel during boot up, by adding "stacktrace"
+to the kernel command line parameter.
+
+After running it for a few minutes, the output looks like:
+
+ # cat stack_max_size
+2928
+
+ # cat stack_trace
+        Depth    Size   Location    (18 entries)
+        -----    ----   --------
+  0)     2928     224   update_sd_lb_stats+0xbc/0x4ac
+  1)     2704     160   find_busiest_group+0x31/0x1f1
+  2)     2544     256   load_balance+0xd9/0x662
+  3)     2288      80   idle_balance+0xbb/0x130
+  4)     2208     128   __schedule+0x26e/0x5b9
+  5)     2080      16   schedule+0x64/0x66
+  6)     2064     128   schedule_timeout+0x34/0xe0
+  7)     1936     112   wait_for_common+0x97/0xf1
+  8)     1824      16   wait_for_completion+0x1d/0x1f
+  9)     1808     128   flush_work+0xfe/0x119
+ 10)     1680      16   tty_flush_to_ldisc+0x1e/0x20
+ 11)     1664      48   input_available_p+0x1d/0x5c
+ 12)     1616      48   n_tty_poll+0x6d/0x134
+ 13)     1568      64   tty_poll+0x64/0x7f
+ 14)     1504     880   do_select+0x31e/0x511
+ 15)      624     400   core_sys_select+0x177/0x216
+ 16)      224      96   sys_select+0x91/0xb9
+ 17)      128     128   system_call_fastpath+0x16/0x1b
+
+Note, if -mfentry is being used by gcc, functions get traced before
+they set up the stack frame. This means that leaf level functions
+are not tested by the stack tracer when -mfentry is used.
+
+Currently, -mfentry is used by gcc 4.6.0 and above on x86 only.
+
+---------
 
 More details can be found in the source code, in the
 kernel/trace/*.c files.
diff --git a/Documentation/trace/tracepoints.txt b/Documentation/trace/tracepoints.txt
index c0e1ceed75a..da49437d5ae 100644
--- a/Documentation/trace/tracepoints.txt
+++ b/Documentation/trace/tracepoints.txt
@@ -81,7 +81,6 @@ tracepoint_synchronize_unregister() must be called before the end of
 the module exit function to make sure there is no caller left using
 the probe. This, and the fact that preemption is disabled around the
 probe call, make sure that probe removal and module unload are safe.
-See the "Probe example" section below for a sample probe module.
 
 The tracepoint mechanism supports inserting multiple instances of the
 same tracepoint, but a single definition must be made of a given
@@ -100,17 +99,3 @@ core kernel image or in modules.
 If the tracepoint has to be used in kernel modules, an
 EXPORT_TRACEPOINT_SYMBOL_GPL() or EXPORT_TRACEPOINT_SYMBOL() can be
 used to export the defined tracepoints.
-
-* Probe / tracepoint example
-
-See the example provided in samples/tracepoints
-
-Compile them with your kernel.  They are built during 'make' (not
-'make modules') when CONFIG_SAMPLE_TRACEPOINTS=m.
-
-Run, as root :
-modprobe tracepoint-sample (insmod order is not important)
-modprobe tracepoint-probe-sample
-cat /proc/tracepoint-sample (returns an expected error)
-rmmod tracepoint-sample tracepoint-probe-sample
-dmesg
diff --git a/Documentation/trace/uprobetracer.txt b/Documentation/trace/uprobetracer.txt
index 24ce6823a09..d9c3e682312 100644
--- a/Documentation/trace/uprobetracer.txt
+++ b/Documentation/trace/uprobetracer.txt
@@ -1,6 +1,8 @@
-		Uprobe-tracer: Uprobe-based Event Tracing
-		=========================================
-                 Documentation written by Srikar Dronamraju
+            Uprobe-tracer: Uprobe-based Event Tracing
+            =========================================
+
+           Documentation written by Srikar Dronamraju
+
 
 Overview
 --------
@@ -13,78 +15,94 @@ current_tracer. Instead of that, add probe points 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
+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
+  p[:[GRP/]EVENT] PATH:SYMBOL[+offs] [FETCHARGS] : Set a uprobe
+  r[:[GRP/]EVENT] PATH:SYMBOL[+offs] [FETCHARGS] : Set a return uprobe (uretprobe)
+  -:[GRP/]EVENT                                  : Clear uprobe or uretprobe event
 
- 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.
+  GRP           : Group name. If omitted, "uprobes" is the default value.
+  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
+  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
+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 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.
+ * Add a probe as a new uprobe event, write a new definition to uprobe_events
+as below: (sets a uprobe at an offset of 0x4245c0 in the executable /bin/bash)
+
+    echo 'p: /bin/bash:0x4245c0' > /sys/kernel/debug/tracing/uprobe_events
+
+ * Add a probe as a new uretprobe event:
+
+    echo 'r: /bin/bash:0x4245c0' > /sys/kernel/debug/tracing/uprobe_events
+
+ * Unset registered event:
 
-  echo 'p: /bin/bash:0x4245c0' > /sys/kernel/debug/tracing/uprobe_events
+    echo '-:bash_0x4245c0' >> /sys/kernel/debug/tracing/uprobe_events
 
- This sets a uprobe at an offset of 0x4245c0 in the executable /bin/bash
+ * Print out the events that are registered:
 
-  echo > /sys/kernel/debug/tracing/uprobe_events
+    cat /sys/kernel/debug/tracing/uprobe_events
 
- This clears all probe points.
+ * Clear all events:
 
-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
+    echo > /sys/kernel/debug/tracing/uprobe_events
+
+Following example shows how to dump the instruction pointer and %ax register
+at the probed text address. Probe zfree function in /bin/zsh:
 
     # cd /sys/kernel/debug/tracing/
-    # cat /proc/`pgrep  zsh`/maps | grep /bin/zsh | grep r-xp
+    # 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 :
+  0x46420 is the offset of zfree in object /bin/zsh that is loaded at
+  0x00400000. Hence the command to uprobe would be:
+
+    # echo 'p:zfree_entry /bin/zsh:0x46420 %ip %ax' > uprobe_events
+
+  And the same for the uretprobe would be:
 
-    # echo 'p /bin/zsh:0x46420 %ip %ax' > uprobe_events
+    # echo 'r:zfree_exit /bin/zsh:0x46420 %ip %ax' >> uprobe_events
 
-Please note: User has to explicitly calculate the offset of the probepoint
+Please note: User has to explicitly calculate the offset of the probe-point
 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
+    p:uprobes/zfree_entry /bin/zsh:0x00046420 arg1=%ip arg2=%ax
+    r:uprobes/zfree_exit /bin/zsh:0x00046420 arg1=%ip arg2=%ax
 
-The format of events can be seen by viewing the file events/uprobes/p_zsh_0x46420/format
+Format of events can be seen by viewing the file events/uprobes/zfree_entry/format
 
-    # cat events/uprobes/p_zsh_0x46420/format
-    name: p_zsh_0x46420
+    # cat events/uprobes/zfree_entry/format
+    name: zfree_entry
     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 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;
+         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
 
@@ -94,6 +112,7 @@ 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
 
@@ -104,10 +123,11 @@ And you can see the traced information via /sys/kernel/debug/tracing/trace.
     #
     #           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.
+                 zsh-24842 [006] 258544.995456: zfree_entry: (0x446420) arg1=446420 arg2=79
+                 zsh-24842 [007] 258545.000270: zfree_exit:  (0x446540 <- 0x446420) arg1=446540 arg2=0
+                 zsh-24842 [002] 258545.043929: zfree_entry: (0x446420) arg1=446420 arg2=79
+                 zsh-24842 [004] 258547.046129: zfree_exit:  (0x446540 <- 0x446420) arg1=446540 arg2=0
+
+Output shows us uprobe was triggered for a pid 24842 with ip being 0x446420
+and contents of ax register being 79. And uretprobe was triggered with ip at
+0x446540 with counterpart function entry at 0x446420.
diff --git a/Documentation/usb/power-management.txt b/Documentation/usb/power-management.txt
index 4204eb01fd3..1392b61d6eb 100644
--- a/Documentation/usb/power-management.txt
+++ b/Documentation/usb/power-management.txt
@@ -33,6 +33,10 @@ built with CONFIG_USB_SUSPEND enabled (which depends on
 CONFIG_PM_RUNTIME).  System PM support is present only if the kernel
 was built with CONFIG_SUSPEND or CONFIG_HIBERNATION enabled.
 
+(Starting with the 3.10 kernel release, dynamic PM support for USB is
+present whenever the kernel was built with CONFIG_PM_RUNTIME enabled.
+The CONFIG_USB_SUSPEND option has been eliminated.)
+
 
 	What is Remote Wakeup?
 	----------------------
@@ -206,10 +210,8 @@ initialized to 5.  (The idle-delay values for already existing devices
 will not be affected.)
 
 Setting the initial default idle-delay to -1 will prevent any
-autosuspend of any USB device.  This is a simple alternative to
-disabling CONFIG_USB_SUSPEND and rebuilding the kernel, and it has the
-added benefit of allowing you to enable autosuspend for selected
-devices.
+autosuspend of any USB device.  This has the benefit of allowing you
+then to enable autosuspend for selected devices.
 
 
 	Warnings
diff --git a/Documentation/video4linux/CARDLIST.em28xx b/Documentation/video4linux/CARDLIST.em28xx
index 3f12865b2a8..e8186440510 100644
--- a/Documentation/video4linux/CARDLIST.em28xx
+++ b/Documentation/video4linux/CARDLIST.em28xx
@@ -76,7 +76,7 @@
  76 -> KWorld PlusTV 340U or UB435-Q (ATSC)     (em2870)        [1b80:a340]
  77 -> EM2874 Leadership ISDBT                  (em2874)
  78 -> PCTV nanoStick T2 290e                   (em28174)
- 79 -> Terratec Cinergy H5                      (em2884)        [0ccd:10a2,0ccd:10ad]
+ 79 -> Terratec Cinergy H5                      (em2884)        [0ccd:10a2,0ccd:10ad,0ccd:10b6]
  80 -> PCTV DVB-S2 Stick (460e)                 (em28174)
  81 -> Hauppauge WinTV HVR 930C                 (em2884)        [2040:1605]
  82 -> Terratec Cinergy HTC Stick               (em2884)        [0ccd:00b2]
@@ -85,3 +85,4 @@
  85 -> PCTV QuatroStick (510e)                  (em2884)        [2304:0242]
  86 -> PCTV QuatroStick nano (520e)             (em2884)        [2013:0251]
  87 -> Terratec Cinergy HTC USB XS              (em2884)        [0ccd:008e,0ccd:00ac]
+ 88 -> C3 Tech Digital Duo HDTV/SDTV USB        (em2884)        [1b80:e755]
diff --git a/Documentation/video4linux/CARDLIST.tuner b/Documentation/video4linux/CARDLIST.tuner
index c83f6e41887..5b83a3ff15c 100644
--- a/Documentation/video4linux/CARDLIST.tuner
+++ b/Documentation/video4linux/CARDLIST.tuner
@@ -86,3 +86,6 @@ tuner=85 - Philips FQ1236 MK5
 tuner=86 - Tena TNF5337 MFD
 tuner=87 - Xceive 4000 tuner
 tuner=88 - Xceive 5000C tuner
+tuner=89 - Sony PAL+SECAM (BTF-PG472Z)
+tuner=90 - Sony NTSC-M-JP (BTF-PK467Z)
+tuner=91 - Sony NTSC-M (BTF-PB463Z)
diff --git a/Documentation/video4linux/si476x.txt b/Documentation/video4linux/si476x.txt
new file mode 100644
index 00000000000..d1a08db2cbd
--- /dev/null
+++ b/Documentation/video4linux/si476x.txt
@@ -0,0 +1,187 @@
+SI476x Driver Readme
+------------------------------------------------
+	Copyright (C) 2013 Andrey Smirnov <andrew.smirnov@gmail.com>
+
+TODO for the driver
+------------------------------
+
+- According to the SiLabs' datasheet it is possible to update the
+  firmware of the radio chip in the run-time, thus bringing it to the
+  most recent version. Unfortunately I couldn't find any mentioning of
+  the said firmware update for the old chips that I tested the driver
+  against, so for chips like that the driver only exposes the old
+  functionality.
+
+
+Parameters exposed over debugfs
+-------------------------------
+SI476x allow user to get multiple characteristics that can be very
+useful for EoL testing/RF performance estimation, parameters that have
+very little to do with V4L2 subsystem. Such parameters are exposed via
+debugfs and can be accessed via regular file I/O operations.
+
+The drivers exposes following files:
+
+* /sys/kernel/debug/<device-name>/acf
+  This file contains ACF(Automatically Controlled Features) status
+  information. The contents of the file is binary data of the
+  following layout:
+
+  Offset	| Name		| Description
+  ====================================================================
+  0x00		| blend_int	| Flag, set when stereo separation has
+  		|  		| crossed below the blend threshold
+  --------------------------------------------------------------------
+  0x01		| hblend_int	| Flag, set when HiBlend cutoff
+  		| 		| frequency is lower than threshold
+  --------------------------------------------------------------------
+  0x02		| hicut_int	| Flag, set when HiCut cutoff
+  		| 		| frequency is lower than threshold
+  --------------------------------------------------------------------
+  0x03		| chbw_int	| Flag, set when channel filter
+  		| 		| bandwidth is less than threshold
+  --------------------------------------------------------------------
+  0x04		| softmute_int	| Flag indicating that softmute
+  		| 		| attenuation has increased above
+		|		| softmute threshold
+  --------------------------------------------------------------------
+  0x05		| smute		| 0 - Audio is not soft muted
+  		| 		| 1 - Audio is soft muted
+  --------------------------------------------------------------------
+  0x06		| smattn	| Soft mute attenuation level in dB
+  --------------------------------------------------------------------
+  0x07		| chbw		| Channel filter bandwidth in kHz
+  --------------------------------------------------------------------
+  0x08		| hicut		| HiCut cutoff frequency in units of
+  		| 		| 100Hz
+  --------------------------------------------------------------------
+  0x09		| hiblend	| HiBlend cutoff frequency in units
+  		| 		| of 100 Hz
+  --------------------------------------------------------------------
+  0x10		| pilot		| 0 - Stereo pilot is not present
+  		| 		| 1 - Stereo pilot is present
+  --------------------------------------------------------------------
+  0x11		| stblend	| Stereo blend in %
+  --------------------------------------------------------------------
+
+
+* /sys/kernel/debug/<device-name>/rds_blckcnt
+  This file contains statistics about RDS receptions. It's binary data
+  has the following layout:
+
+  Offset	| Name		| Description
+  ====================================================================
+  0x00		| expected	| Number of expected RDS blocks
+  --------------------------------------------------------------------
+  0x02		| received	| Number of received RDS blocks
+  --------------------------------------------------------------------
+  0x04		| uncorrectable	| Number of uncorrectable RDS blocks
+  --------------------------------------------------------------------
+
+* /sys/kernel/debug/<device-name>/agc
+  This file contains information about parameters pertaining to
+  AGC(Automatic Gain Control)
+
+  The layout is:
+  Offset	| Name		| Description
+  ====================================================================
+  0x00		| mxhi		| 0 - FM Mixer PD high threshold is
+  		| 		| not tripped
+		|		| 1 - FM Mixer PD high threshold is
+		|		| tripped
+  --------------------------------------------------------------------
+  0x01		| mxlo		| ditto for FM Mixer PD low
+  --------------------------------------------------------------------
+  0x02		| lnahi		| ditto for FM LNA PD high
+  --------------------------------------------------------------------
+  0x03		| lnalo		| ditto for FM LNA PD low
+  --------------------------------------------------------------------
+  0x04		| fmagc1	| FMAGC1 attenuator resistance
+  		| 		| (see datasheet for more detail)
+  --------------------------------------------------------------------
+  0x05		| fmagc2	| ditto for FMAGC2
+  --------------------------------------------------------------------
+  0x06		| pgagain	| PGA gain in dB
+  --------------------------------------------------------------------
+  0x07		| fmwblang	| FM/WB LNA Gain in dB
+  --------------------------------------------------------------------
+
+* /sys/kernel/debug/<device-name>/rsq
+  This file contains information about parameters pertaining to
+  RSQ(Received Signal Quality)
+
+  The layout is:
+  Offset	| Name		| Description
+  ====================================================================
+  0x00		| multhint	| 0 - multipath value has not crossed
+  		| 		| the Multipath high threshold
+		|		| 1 - multipath value has crossed
+  		| 		| the Multipath high threshold
+  --------------------------------------------------------------------
+  0x01		| multlint	| ditto for Multipath low threshold
+  --------------------------------------------------------------------
+  0x02		| snrhint	| 0 - received signal's SNR has not
+  		| 		| crossed high threshold
+		|		| 1 - received signal's SNR has
+  		| 		| crossed high threshold
+  --------------------------------------------------------------------
+  0x03		| snrlint	| ditto for low threshold
+  --------------------------------------------------------------------
+  0x04		| rssihint	| ditto for RSSI high threshold
+  --------------------------------------------------------------------
+  0x05		| rssilint	| ditto for RSSI low threshold
+  --------------------------------------------------------------------
+  0x06		| bltf		| Flag indicating if seek command
+  		| 		| reached/wrapped seek band limit
+  --------------------------------------------------------------------
+  0x07		| snr_ready	| Indicates that SNR metrics is ready
+  --------------------------------------------------------------------
+  0x08		| rssiready	| ditto for RSSI metrics
+  --------------------------------------------------------------------
+  0x09		| injside	| 0 - Low-side injection is being used
+  		| 		| 1 - High-side injection is used
+  --------------------------------------------------------------------
+  0x10		| afcrl		| Flag indicating if AFC rails
+  --------------------------------------------------------------------
+  0x11		| valid		| Flag indicating if channel is valid
+  --------------------------------------------------------------------
+  0x12		| readfreq	| Current tuned frequency
+  --------------------------------------------------------------------
+  0x14		| freqoff	| Singed frequency offset in units of
+  		| 		| 2ppm
+  --------------------------------------------------------------------
+  0x15		| rssi		| Signed value of RSSI in dBuV
+  --------------------------------------------------------------------
+  0x16		| snr		| Signed RF SNR in dB
+  --------------------------------------------------------------------
+  0x17		| issi		| Signed Image Strength Signal
+  		| 		| indicator
+  --------------------------------------------------------------------
+  0x18		| lassi		| Signed Low side adjacent Channel
+  		| 		| Strength indicator
+  --------------------------------------------------------------------
+  0x19		| hassi		| ditto fpr High side
+  --------------------------------------------------------------------
+  0x20		| mult		| Multipath indicator
+  --------------------------------------------------------------------
+  0x21		| dev		| Frequency deviation
+  --------------------------------------------------------------------
+  0x24		| assi		| Adjascent channel SSI
+  --------------------------------------------------------------------
+  0x25		| usn		| Ultrasonic noise indicator
+  --------------------------------------------------------------------
+  0x26		| pilotdev	| Pilot deviation in units of 100 Hz
+  --------------------------------------------------------------------
+  0x27		| rdsdev	| ditto for RDS
+  --------------------------------------------------------------------
+  0x28		| assidev	| ditto for ASSI
+  --------------------------------------------------------------------
+  0x29		| strongdev	| Frequency deviation
+  --------------------------------------------------------------------
+  0x30		| rdspi		| RDS PI code
+  --------------------------------------------------------------------
+
+* /sys/kernel/debug/<device-name>/rsq_primary
+  This file contains information about parameters pertaining to
+  RSQ(Received Signal Quality) for primary tuner only. Layout is as
+  the one above.
diff --git a/Documentation/virtual/virtio-spec.txt b/Documentation/virtual/virtio-spec.txt
index 0d6ec85481c..eb094039b50 100644
--- a/Documentation/virtual/virtio-spec.txt
+++ b/Documentation/virtual/virtio-spec.txt
@@ -1389,7 +1389,7 @@ segmentation, if both guests are amenable.
 
 Packets are transmitted by placing them in the transmitq, and
 buffers for incoming packets are placed in the receiveq. In each
-case, the packet itself is preceeded by a header:
+case, the packet itself is preceded by a header:
 
 struct virtio_net_hdr {
 
@@ -1631,7 +1631,7 @@ struct virtio_net_ctrl_mac {
 
 The device can filter incoming packets by any number of
 destination MAC addresses.[footnote:
-Since there are no guarentees, it can use a hash filter
+Since there are no guarantees, it can use a hash filter
 orsilently switch to allmulti or promiscuous mode if it is given
 too many addresses.
 ] This table is set using the class VIRTIO_NET_CTRL_MAC and the
@@ -1822,7 +1822,7 @@ the FLUSH and FLUSH_OUT types are equivalent, the device does not
 distinguish between them
 ]). If the device has VIRTIO_BLK_F_BARRIER feature the high bit
 (VIRTIO_BLK_T_BARRIER) indicates that this request acts as a
-barrier and that all preceeding requests must be complete before
+barrier and that all preceding requests must be complete before
 this one, and all following requests must not be started until
 this is complete. Note that a barrier does not flush caches in
 the underlying backend device in host, and thus does not serve as
diff --git a/Documentation/vm/overcommit-accounting b/Documentation/vm/overcommit-accounting
index 706d7ed9d8d..8eaa2fc4b8f 100644
--- a/Documentation/vm/overcommit-accounting
+++ b/Documentation/vm/overcommit-accounting
@@ -8,7 +8,9 @@ The Linux kernel supports the following overcommit handling modes
 		default.
 
 1	-	Always overcommit. Appropriate for some scientific
-		applications.
+		applications. Classic example is code using sparse arrays
+		and just relying on the virtual memory consisting almost
+		entirely of zero pages.
 
 2	-	Don't overcommit. The total address space commit
 		for the system is not permitted to exceed swap + a
@@ -18,6 +20,10 @@ The Linux kernel supports the following overcommit handling modes
 		pages but will receive errors on memory allocation as
 		appropriate.
 
+		Useful for applications that want to guarantee their
+		memory allocations will be available in the future
+		without having to initialize every page.
+
 The overcommit policy is set via the sysctl `vm.overcommit_memory'.
 
 The overcommit percentage is set via `vm.overcommit_ratio'.
diff --git a/Documentation/x86/x86_64/boot-options.txt b/Documentation/x86/x86_64/boot-options.txt
index e015a83c399..e9e8ddbbf37 100644
--- a/Documentation/x86/x86_64/boot-options.txt
+++ b/Documentation/x86/x86_64/boot-options.txt
@@ -91,20 +91,6 @@ APICs
 		 apicmaintimer. Useful when your PIT timer is totally
 		 broken.
 
-Early Console
-
-   syntax: earlyprintk=vga
-           earlyprintk=serial[,ttySn[,baudrate]]
-
-   The early console is useful when the kernel crashes before the
-   normal console is initialized. It is not enabled by
-   default because it has some cosmetic problems.
-   Append ,keep to not disable it when the real console takes over.
-   Only vga or serial at a time, not both.
-   Currently only ttyS0 and ttyS1 are supported.
-   Interaction with the standard serial driver is not very good.
-   The VGA output is eventually overwritten by the real console.
-
 Timing
 
   notsc
diff --git a/Documentation/x86/x86_64/mm.txt b/Documentation/x86/x86_64/mm.txt
index d6498e3cd71..881582f75c9 100644
--- a/Documentation/x86/x86_64/mm.txt
+++ b/Documentation/x86/x86_64/mm.txt
@@ -13,7 +13,9 @@ ffffe90000000000 - ffffe9ffffffffff (=40 bits) hole
 ffffea0000000000 - ffffeaffffffffff (=40 bits) virtual memory map (1TB)
 ... unused hole ...
 ffffffff80000000 - ffffffffa0000000 (=512 MB)  kernel text mapping, from phys 0
-ffffffffa0000000 - fffffffffff00000 (=1536 MB) module mapping space
+ffffffffa0000000 - ffffffffff5fffff (=1525 MB) module mapping space
+ffffffffff600000 - ffffffffffdfffff (=8 MB) vsyscalls
+ffffffffffe00000 - ffffffffffffffff (=2 MB) unused hole
 
 The direct mapping covers all memory in the system up to the highest
 memory address (this means in some cases it can also include PCI memory
diff --git a/MAINTAINERS b/MAINTAINERS
index 4cf5fd334a0..ea8f65fc6fa 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -90,6 +90,9 @@ Descriptions of section entries:
 	   F:	drivers/net/*	all files in drivers/net, but not below
 	   F:	*/net/*		all files in "any top level directory"/net
 	   One pattern per line.  Multiple F: lines acceptable.
+	N: Files and directories with regex patterns.
+	   N:	[^a-z]tegra	all files whose path contains the word tegra
+	   One pattern per line.  Multiple N: lines acceptable.
 	X: Files and directories that are NOT maintained, same rules as F:
 	   Files exclusions are tested before file matches.
 	   Can be useful for excluding a specific subdirectory, for instance:
@@ -97,13 +100,12 @@ Descriptions of section entries:
 	   X:	net/ipv6/
 	   matches all files in and below net excluding net/ipv6/
 	K: Keyword perl extended regex pattern to match content in a
-	   patch or file, or an affected filename.  For instance:
+	   patch or file.  For instance:
 	   K: of_get_profile
-	      matches patch or file content, or filenames, that contain
-	      "of_get_profile"
+	      matches patches or files that contain "of_get_profile"
 	   K: \b(printk|pr_(info|err))\b
-	      matches patch or file content, or filenames, that contain one or
-	      more of the words printk, pr_info or pr_err
+	      matches patches or files that contain one or more of the words
+	      printk, pr_info or pr_err
 	   One regex pattern per line.  Multiple K: lines acceptable.
 
 Note: For the hard of thinking, this list is meant to remain in alphabetical
@@ -1031,6 +1033,7 @@ F:	drivers/mmc/host/msm_sdcc.h
 F:	drivers/tty/serial/msm_serial.h
 F:	drivers/tty/serial/msm_serial.c
 F:	drivers/*/pm8???-*
+F:	drivers/ssbi/
 F:	include/linux/mfd/pm8xxx/
 T:	git git://git.kernel.org/pub/scm/linux/kernel/git/davidb/linux-msm.git
 S:	Maintained
@@ -2284,7 +2287,7 @@ L:	linux-media@vger.kernel.org
 T:	git git://linuxtv.org/media_tree.git
 W:	http://linuxtv.org
 S:	Maintained
-F:	drivers/media/i2c/cx2341x*
+F:	drivers/media/common/cx2341x*
 F:	include/media/cx2341x*
 
 CX88 VIDEO4LINUX DRIVER
@@ -2367,6 +2370,16 @@ W:	http://www.cyclades.com/
 S:	Orphan
 F:	drivers/net/wan/pc300*
 
+CYPRESS_FIRMWARE MEDIA DRIVER
+M:	Antti Palosaari <crope@iki.fi>
+L:	linux-media@vger.kernel.org
+W:	http://linuxtv.org/
+W:	http://palosaari.fi/linux/
+Q:	http://patchwork.linuxtv.org/project/linux-media/list/
+T:	git git://linuxtv.org/anttip/media_tree.git
+S:	Maintained
+F:	drivers/media/common/cypress_firmware*
+
 CYTTSP TOUCHSCREEN DRIVER
 M:	Javier Martinez Canillas <javier@dowhile0.org>
 L:	linux-input@vger.kernel.org
@@ -2441,9 +2454,7 @@ S:	Maintained
 F:	drivers/platform/x86/dell-laptop.c
 
 DELL LAPTOP SMM DRIVER
-M:	Massimo Dal Zotto <dz@debian.org>
-W:	http://www.debian.org/~dz/i8k/
-S:	Maintained
+S:	Orphan
 F:	drivers/char/i8k.c
 F:	include/uapi/linux/i8k.h
 
@@ -2458,6 +2469,12 @@ M:	Matthew Garrett <mjg59@srcf.ucam.org>
 S:	Maintained
 F:	drivers/platform/x86/dell-wmi.c
 
+DESIGNWARE USB2 DRD IP DRIVER
+M:	Paul Zimmerman <paulz@synopsys.com>
+L:	linux-usb@vger.kernel.org
+S:	Maintained
+F:	drivers/staging/dwc2/
+
 DESIGNWARE USB3 DRD IP DRIVER
 M:	Felipe Balbi <balbi@ti.com>
 L:	linux-usb@vger.kernel.org
@@ -2731,7 +2748,7 @@ T:	git git://linuxtv.org/media_tree.git
 S:	Maintained
 F:	drivers/media/usb/dvb-usb/cxusb*
 
-DVB_USB_CYPRESS_FIRMWARE MEDIA DRIVER
+DVB_USB_EC168 MEDIA DRIVER
 M:	Antti Palosaari <crope@iki.fi>
 L:	linux-media@vger.kernel.org
 W:	http://linuxtv.org/
@@ -2739,17 +2756,16 @@ W:	http://palosaari.fi/linux/
 Q:	http://patchwork.linuxtv.org/project/linux-media/list/
 T:	git git://linuxtv.org/anttip/media_tree.git
 S:	Maintained
-F:	drivers/media/usb/dvb-usb-v2/cypress_firmware*
+F:	drivers/media/usb/dvb-usb-v2/ec168*
 
-DVB_USB_EC168 MEDIA DRIVER
+DVB_USB_GL861 MEDIA DRIVER
 M:	Antti Palosaari <crope@iki.fi>
 L:	linux-media@vger.kernel.org
 W:	http://linuxtv.org/
-W:	http://palosaari.fi/linux/
 Q:	http://patchwork.linuxtv.org/project/linux-media/list/
 T:	git git://linuxtv.org/anttip/media_tree.git
 S:	Maintained
-F:	drivers/media/usb/dvb-usb-v2/ec168*
+F:	drivers/media/usb/dvb-usb-v2/gl861*
 
 DVB_USB_MXL111SF MEDIA DRIVER
 M:	Michael Krufky <mkrufky@linuxtv.org>
@@ -3242,6 +3258,12 @@ F:	Documentation/firmware_class/
 F:	drivers/base/firmware*.c
 F:	include/linux/firmware.h
 
+FLASHSYSTEM DRIVER (IBM FlashSystem 70/80 PCI SSD Flash Card)
+M:	Joshua Morris <josh.h.morris@us.ibm.com>
+M:	Philip Kelleher <pjk1939@linux.vnet.ibm.com>
+S:	Maintained
+F:	drivers/block/rsxx/
+
 FLOPPY DRIVER
 M:	Jiri Kosina <jkosina@suse.cz>
 T:	git git://git.kernel.org/pub/scm/linux/kernel/git/jikos/floppy.git
@@ -3502,7 +3524,7 @@ F:	drivers/isdn/gigaset/
 F:	include/uapi/linux/gigaset_dev.h
 
 GPIO SUBSYSTEM
-M:	Grant Likely <grant.likely@secretlab.ca>
+M:	Grant Likely <grant.likely@linaro.org>
 M:	Linus Walleij <linus.walleij@linaro.org>
 S:	Maintained
 T:	git git://git.secretlab.ca/git/linux-2.6.git
@@ -3587,6 +3609,14 @@ W:	http://www.kernel.org/pub/linux/kernel/people/fseidel/hdaps/
 S:	Maintained
 F:	drivers/platform/x86/hdaps.c
 
+HDPVR USB VIDEO ENCODER DRIVER
+M:	Hans Verkuil <hverkuil@xs4all.nl>
+L:	linux-media@vger.kernel.org
+T:	git git://linuxtv.org/media_tree.git
+W:	http://linuxtv.org
+S:	Odd Fixes
+F:	drivers/media/usb/hdpvr
+
 HWPOISON MEMORY FAILURE HANDLING
 M:	Andi Kleen <andi@firstfloor.org>
 L:	linux-mm@kvack.org
@@ -4335,7 +4365,7 @@ F:	drivers/irqchip/
 
 IRQ DOMAINS (IRQ NUMBER MAPPING LIBRARY)
 M:	Benjamin Herrenschmidt <benh@kernel.crashing.org>
-M:	Grant Likely <grant.likely@secretlab.ca>
+M:	Grant Likely <grant.likely@linaro.org>
 T:	git git://git.secretlab.ca/git/linux-2.6.git irqdomain/next
 S:	Maintained
 F:	Documentation/IRQ-domain.txt
@@ -4416,6 +4446,16 @@ Q:	http://patchwork.linuxtv.org/project/linux-media/list/
 S:	Maintained
 F:	drivers/media/dvb-frontends/it913x-fe*
 
+IT913X MEDIA DRIVER
+M:	Antti Palosaari <crope@iki.fi>
+L:	linux-media@vger.kernel.org
+W:	http://linuxtv.org/
+W:	http://palosaari.fi/linux/
+Q:	http://patchwork.linuxtv.org/project/linux-media/list/
+T:	git git://linuxtv.org/anttip/media_tree.git
+S:	Maintained
+F:	drivers/media/tuners/it913x*
+
 IVTV VIDEO4LINUX DRIVER
 M:	Andy Walls <awalls@md.metrocast.net>
 L:	ivtv-devel@ivtvdriver.org (moderated for non-subscribers)
@@ -4822,11 +4862,8 @@ F:	arch/powerpc/platforms/40x/
 F:	arch/powerpc/platforms/44x/
 
 LINUX FOR POWERPC EMBEDDED XILINX VIRTEX
-M:	Grant Likely <grant.likely@secretlab.ca>
-W:	http://wiki.secretlab.ca/index.php/Linux_on_Xilinx_Virtex
 L:	linuxppc-dev@lists.ozlabs.org
-T:	git git://git.secretlab.ca/git/linux-2.6.git
-S:	Maintained
+S:	Unmaintained
 F:	arch/powerpc/*/*virtex*
 F:	arch/powerpc/*/*/*virtex*
 
@@ -4935,6 +4972,12 @@ W:	logfs.org
 S:	Maintained
 F:	fs/logfs/
 
+LPC32XX MACHINE SUPPORT
+M:	Roland Stigge <stigge@antcom.de>
+L:	linux-arm-kernel@lists.infradead.org (moderated for non-subscribers)
+S:	Maintained
+F:	arch/arm/mach-lpc32xx/
+
 LSILOGIC MPT FUSION DRIVERS (FC/SAS/SPI)
 M:	Nagalakshmi Nandigama <Nagalakshmi.Nandigama@lsi.com>
 M:	Sreekanth Reddy <Sreekanth.Reddy@lsi.com>
@@ -5059,9 +5102,8 @@ S:	Maintained
 F:	drivers/net/ethernet/marvell/sk*
 
 MARVELL LIBERTAS WIRELESS DRIVER
-M:	Dan Williams <dcbw@redhat.com>
 L:	libertas-dev@lists.infradead.org
-S:	Maintained
+S:	Orphan
 F:	drivers/net/wireless/libertas/
 
 MARVELL MV643XX ETHERNET DRIVER
@@ -5401,6 +5443,13 @@ L:	linux-scsi@vger.kernel.org
 S:	Maintained
 F:	drivers/scsi/NCR_D700.*
 
+NCT6775 HARDWARE MONITOR DRIVER
+M:	Guenter Roeck <linux@roeck-us.net>
+L:	lm-sensors@lm-sensors.org
+S:	Maintained
+F:	Documentation/hwmon/nct6775
+F:	drivers/hwmon/nct6775.c
+
 NETEFFECT IWARP RNIC DRIVER (IW_NES)
 M:	Faisal Latif <faisal.latif@intel.com>
 L:	linux-rdma@vger.kernel.org
@@ -5563,6 +5612,7 @@ F:	include/uapi/linux/if_*
 F:	include/uapi/linux/netdevice.h
 
 NETXEN (1/10) GbE SUPPORT
+M:	Manish Chopra <manish.chopra@qlogic.com>
 M:	Sony Chacko <sony.chacko@qlogic.com>
 M:	Rajesh Borundia <rajesh.borundia@qlogic.com>
 L:	netdev@vger.kernel.org
@@ -5683,7 +5733,7 @@ S:	Maintained
 F:	arch/arm/*omap*/*clock*
 
 OMAP POWER MANAGEMENT SUPPORT
-M:	Kevin Hilman <khilman@ti.com>
+M:	Kevin Hilman <khilman@deeprootsystems.com>
 L:	linux-omap@vger.kernel.org
 S:	Maintained
 F:	arch/arm/*omap*/*pm*
@@ -5777,7 +5827,7 @@ F:	arch/arm/*omap*/usb*
 
 OMAP GPIO DRIVER
 M:	Santosh Shilimkar <santosh.shilimkar@ti.com>
-M:	Kevin Hilman <khilman@ti.com>
+M:	Kevin Hilman <khilman@deeprootsystems.com>
 L:	linux-omap@vger.kernel.org
 S:	Maintained
 F:	drivers/gpio/gpio-omap.c
@@ -5831,7 +5881,7 @@ F:	Documentation/i2c/busses/i2c-ocores
 F:	drivers/i2c/busses/i2c-ocores.c
 
 OPEN FIRMWARE AND FLATTENED DEVICE TREE
-M:	Grant Likely <grant.likely@secretlab.ca>
+M:	Grant Likely <grant.likely@linaro.org>
 M:	Rob Herring <rob.herring@calxeda.com>
 L:	devicetree-discuss@lists.ozlabs.org (moderated for non-subscribers)
 W:	http://fdt.secretlab.ca
@@ -6186,7 +6236,7 @@ S:	Supported
 F:	drivers/scsi/pmcraid.*
 
 PMC SIERRA PM8001 DRIVER
-M:	jack_wang@usish.com
+M:	xjtuwjp@gmail.com
 M:	lindar_liu@usish.com
 L:	linux-scsi@vger.kernel.org
 S:	Supported
@@ -6209,7 +6259,7 @@ F:	include/linux/power_supply.h
 F:	drivers/power/
 
 PNP SUPPORT
-M:	Adam Belay <abelay@mit.edu>
+M:	Rafael J. Wysocki <rafael.j.wysocki@intel.com>
 M:	Bjorn Helgaas <bhelgaas@google.com>
 S:	Maintained
 F:	drivers/pnp/
@@ -6551,12 +6601,6 @@ S:	Maintained
 F:	Documentation/blockdev/ramdisk.txt
 F:	drivers/block/brd.c
 
-RAMSAM DRIVER (IBM RamSan 70/80 PCI SSD Flash Card)
-M:	Joshua Morris <josh.h.morris@us.ibm.com>
-M:	Philip Kelleher <pjk1939@linux.vnet.ibm.com>
-S:	Maintained
-F:	drivers/block/rsxx/
-
 RANDOM NUMBER DRIVER
 M:	Theodore Ts'o" <tytso@mit.edu>
 S:	Maintained
@@ -6625,7 +6669,7 @@ S:	Supported
 F:	fs/reiserfs/
 
 REGISTER MAP ABSTRACTION
-M:	Mark Brown <broonie@opensource.wolfsonmicro.com>
+M:	Mark Brown <broonie@kernel.org>
 T:	git git://git.kernel.org/pub/scm/linux/kernel/git/broonie/regmap.git
 S:	Supported
 F:	drivers/base/regmap/
@@ -6686,6 +6730,16 @@ T:	git git://linuxtv.org/anttip/media_tree.git
 S:	Maintained
 F:	drivers/media/dvb-frontends/rtl2830*
 
+RTL2832 MEDIA DRIVER
+M:	Antti Palosaari <crope@iki.fi>
+L:	linux-media@vger.kernel.org
+W:	http://linuxtv.org/
+W:	http://palosaari.fi/linux/
+Q:	http://patchwork.linuxtv.org/project/linux-media/list/
+T:	git git://linuxtv.org/anttip/media_tree.git
+S:	Maintained
+F:	drivers/media/dvb-frontends/rtl2832*
+
 RTL8180 WIRELESS DRIVER
 M:	"John W. Linville" <linville@tuxdriver.com>
 L:	linux-wireless@vger.kernel.org
@@ -6788,7 +6842,7 @@ L:	linux-media@vger.kernel.org
 W:	http://linuxtv.org
 T:	git git://linuxtv.org/media_tree.git
 S:	Odd fixes
-F:	Documentation/video4linux/saa7134/
+F:	Documentation/video4linux/*.saa7134
 F:	drivers/media/pci/saa7134/
 
 SAA7146 VIDEO4LINUX-2 DRIVER
@@ -6881,9 +6935,8 @@ F:	drivers/clocksource
 
 TLG2300 VIDEO4LINUX-2 DRIVER
 M:	Huang Shijie <shijie8@gmail.com>
-M:	Kang Yong <kangyong@telegent.com>
-M:	Zhang Xiaobing <xbzhang@telegent.com>
-S:	Supported
+M:	Hans Verkuil <hverkuil@xs4all.nl>
+S:	Odd Fixes
 F:	drivers/media/usb/tlg2300
 
 SC1200 WDT DRIVER
@@ -6951,7 +7004,6 @@ F:	drivers/scsi/st*
 
 SCTP PROTOCOL
 M:	Vlad Yasevich <vyasevich@gmail.com>
-M:	Sridhar Samudrala <sri@us.ibm.com>
 M:	Neil Horman <nhorman@tuxdriver.com>
 L:	linux-sctp@vger.kernel.org
 W:	http://lksctp.sourceforge.net
@@ -7130,17 +7182,43 @@ F:	drivers/media/radio/si470x/radio-si470x-common.c
 F:	drivers/media/radio/si470x/radio-si470x.h
 F:	drivers/media/radio/si470x/radio-si470x-usb.c
 
+SI4713 FM RADIO TRANSMITTER I2C DRIVER
+M:	Eduardo Valentin <edubezval@gmail.com>
+L:	linux-media@vger.kernel.org
+T:	git git://linuxtv.org/media_tree.git
+W:	http://linuxtv.org
+S:	Odd Fixes
+F:	drivers/media/radio/si4713-i2c.?
+
+SI4713 FM RADIO TRANSMITTER PLATFORM DRIVER
+M:	Eduardo Valentin <edubezval@gmail.com>
+L:	linux-media@vger.kernel.org
+T:	git git://linuxtv.org/media_tree.git
+W:	http://linuxtv.org
+S:	Odd Fixes
+F:	drivers/media/radio/radio-si4713.h
+
+SIANO DVB DRIVER
+M:	Mauro Carvalho Chehab <mchehab@redhat.com>
+L:	linux-media@vger.kernel.org
+W:	http://linuxtv.org
+T:	git git://linuxtv.org/media_tree.git
+S:	Odd fixes
+F:	drivers/media/common/siano/
+F:	drivers/media/dvb/siano/
+F:	drivers/media/usb/siano/
+F:	drivers/media/mmc/siano
+
 SH_VEU V4L2 MEM2MEM DRIVER
 M:	Guennadi Liakhovetski <g.liakhovetski@gmx.de>
 L:	linux-media@vger.kernel.org
 S:	Maintained
 F:	drivers/media/platform/sh_veu.c
-F:	include/media/sh_veu.h
 
 SH_VOU V4L2 OUTPUT DRIVER
 M:	Guennadi Liakhovetski <g.liakhovetski@gmx.de>
 L:	linux-media@vger.kernel.org
-S:	Maintained
+S:	Odd Fixes
 F:	drivers/media/platform/sh_vou.c
 F:	include/media/sh_vou.h
 
@@ -7173,7 +7251,7 @@ F:	arch/arm/mach-s3c2410/bast-irq.c
 
 TI DAVINCI MACHINE SUPPORT
 M:	Sekhar Nori <nsekhar@ti.com>
-M:	Kevin Hilman <khilman@ti.com>
+M:	Kevin Hilman <khilman@deeprootsystems.com>
 L:	davinci-linux-open-source@linux.davincidsp.com (moderated for non-subscribers)
 T:	git git://gitorious.org/linux-davinci/linux-davinci.git
 Q:	http://patchwork.kernel.org/project/linux-davinci/list/
@@ -7182,14 +7260,13 @@ F:	arch/arm/mach-davinci
 F:	drivers/i2c/busses/i2c-davinci.c
 
 TI DAVINCI SERIES MEDIA DRIVER
-M:	Manjunath Hadli <manjunath.hadli@ti.com>
-M:	Prabhakar Lad <prabhakar.lad@ti.com>
+M:	Lad, Prabhakar <prabhakar.csengg@gmail.com>
 L:	linux-media@vger.kernel.org
 L:	davinci-linux-open-source@linux.davincidsp.com (moderated for non-subscribers)
 W:	http://linuxtv.org/
 Q:	http://patchwork.linuxtv.org/project/linux-media/list/
 T:	git git://linuxtv.org/mhadli/v4l-dvb-davinci_devices.git
-S:	Supported
+S:	Maintained
 F:	drivers/media/platform/davinci/
 F:	include/media/davinci/
 
@@ -7374,7 +7451,7 @@ F:	sound/
 
 SOUND - SOC LAYER / DYNAMIC AUDIO POWER MANAGEMENT (ASoC)
 M:	Liam Girdwood <lgirdwood@gmail.com>
-M:	Mark Brown <broonie@opensource.wolfsonmicro.com>
+M:	Mark Brown <broonie@kernel.org>
 T:	git git://git.kernel.org/pub/scm/linux/kernel/git/broonie/sound.git
 L:	alsa-devel@alsa-project.org (moderated for non-subscribers)
 W:	http://alsa-project.org/main/index.php/ASoC
@@ -7462,11 +7539,11 @@ S:	Maintained
 F:	drivers/clk/spear/
 
 SPI SUBSYSTEM
-M:	Grant Likely <grant.likely@secretlab.ca>
-M:	Mark Brown <broonie@opensource.wolfsonmicro.com>
+M:	Mark Brown <broonie@kernel.org>
+M:	Grant Likely <grant.likely@linaro.org>
 L:	spi-devel-general@lists.sourceforge.net
+T:	git git://git.kernel.org/pub/scm/linux/kernel/git/broonie/spi.git
 Q:	http://patchwork.kernel.org/project/spi-devel-general/list/
-T:	git git://git.secretlab.ca/git/linux-2.6.git
 S:	Maintained
 F:	Documentation/spi/
 F:	drivers/spi/
@@ -7561,6 +7638,11 @@ M:	David Täht <d@teklibre.com>
 S:	Odd Fixes
 F:	drivers/staging/frontier/
 
+STAGING - GO7007 MPEG CODEC
+M:	Hans Verkuil <hans.verkuil@cisco.com>
+S:	Maintained
+F:	drivers/staging/media/go7007/
+
 STAGING - INDUSTRIAL IO
 M:	Jonathan Cameron <jic23@cam.ac.uk>
 L:	linux-iio@vger.kernel.org
@@ -7611,8 +7693,8 @@ S:	Odd Fixes
 F:	drivers/staging/sm7xxfb/
 
 STAGING - SOFTLOGIC 6x10 MPEG CODEC
-M:	Ben Collins <bcollins@bluecherry.net>
-S:	Odd Fixes
+M:	Ismael Luceno <ismael.luceno@corp.bluecherry.net>
+S:	Supported
 F:	drivers/staging/media/solo6x10/
 
 STAGING - SPEAKUP CONSOLE SPEECH DRIVER
@@ -7706,9 +7788,10 @@ F:	include/linux/swiotlb.h
 
 SYNOPSYS ARC ARCHITECTURE
 M:	Vineet Gupta <vgupta@synopsys.com>
-L:	linux-snps-arc@vger.kernel.org
 S:	Supported
 F:	arch/arc/
+F:	Documentation/devicetree/bindings/arc/
+F:	drivers/tty/serial/arc-uart.c
 
 SYSV FILESYSTEM
 M:	Christoph Hellwig <hch@infradead.org>
@@ -7876,7 +7959,7 @@ L:	linux-tegra@vger.kernel.org
 Q:	http://patchwork.ozlabs.org/project/linux-tegra/list/
 T:	git git://git.kernel.org/pub/scm/linux/kernel/git/swarren/linux-tegra.git
 S:	Supported
-K:	(?i)[^a-z]tegra
+N:	[^a-z]tegra
 
 TEHUTI ETHERNET DRIVER
 M:	Andy Gospodarek <andy@greyhouse.net>
@@ -7919,6 +8002,12 @@ T:	git git://repo.or.cz/linux-2.6/linux-acpi-2.6/ibm-acpi-2.6.git
 S:	Maintained
 F:	drivers/platform/x86/thinkpad_acpi.c
 
+TI BANDGAP AND THERMAL DRIVER
+M:	Eduardo Valentin <eduardo.valentin@ti.com>
+L:	linux-pm@vger.kernel.org
+S:	Maintained
+F:	drivers/staging/omap-thermal/
+
 TI FLASH MEDIA INTERFACE DRIVER
 M:	Alex Dubov <oakad@yahoo.com>
 S:	Maintained
@@ -8356,9 +8445,10 @@ S:	Maintained
 F:	drivers/usb/serial/option.c
 
 USB PEGASUS DRIVER
-M:	Petko Manolov <petkan@users.sourceforge.net>
+M:	Petko Manolov <petkan@nucleusys.com>
 L:	linux-usb@vger.kernel.org
 L:	netdev@vger.kernel.org
+T:	git git://git.code.sf.net/p/pegasus2/git
 W:	http://pegasus2.sourceforge.net/
 S:	Maintained
 F:	drivers/net/usb/pegasus.*
@@ -8378,9 +8468,10 @@ S:	Supported
 F:	drivers/usb/class/usblp.c
 
 USB RTL8150 DRIVER
-M:	Petko Manolov <petkan@users.sourceforge.net>
+M:	Petko Manolov <petkan@nucleusys.com>
 L:	linux-usb@vger.kernel.org
 L:	netdev@vger.kernel.org
+T:	git git://git.code.sf.net/p/pegasus2/git
 W:	http://pegasus2.sourceforge.net/
 S:	Maintained
 F:	drivers/net/usb/rtl8150.c
@@ -8706,8 +8797,8 @@ F:	drivers/scsi/vmw_pvscsi.c
 F:	drivers/scsi/vmw_pvscsi.h
 
 VOLTAGE AND CURRENT REGULATOR FRAMEWORK
-M:	Liam Girdwood <lrg@ti.com>
-M:	Mark Brown <broonie@opensource.wolfsonmicro.com>
+M:	Liam Girdwood <lgirdwood@gmail.com>
+M:	Mark Brown <broonie@kernel.org>
 W:	http://opensource.wolfsonmicro.com/node/15
 W:	http://www.slimlogic.co.uk/?p=48
 T:	git git://git.kernel.org/pub/scm/linux/kernel/git/lrg/regulator.git
@@ -8970,9 +9061,7 @@ S:	Maintained
 F:	drivers/net/ethernet/xilinx/xilinx_axienet*
 
 XILINX SYSTEMACE DRIVER
-M:	Grant Likely <grant.likely@secretlab.ca>
-W:	http://www.secretlab.ca/
-S:	Maintained
+S:	Unmaintained
 F:	drivers/block/xsysace.c
 
 XILINX UARTLITE SERIAL DRIVER
diff --git a/Makefile b/Makefile
index 54d2b2a0fef..d3528f6aa93 100644
--- a/Makefile
+++ b/Makefile
@@ -1,7 +1,7 @@
 VERSION = 3
 PATCHLEVEL = 9
 SUBLEVEL = 0
-EXTRAVERSION = -rc4
+EXTRAVERSION =
 NAME = Unicycling Gorilla
 
 # *DOCUMENTATION*
@@ -513,7 +513,8 @@ ifeq ($(KBUILD_EXTMOD),)
 # Carefully list dependencies so we do not try to build scripts twice
 # in parallel
 PHONY += scripts
-scripts: scripts_basic include/config/auto.conf include/config/tristate.conf
+scripts: scripts_basic include/config/auto.conf include/config/tristate.conf \
+	 asm-generic
 	$(Q)$(MAKE) $(build)=$(@)
 
 # Objects we will link into vmlinux / subdirs we need to visit
@@ -1331,11 +1332,11 @@ kernelversion:
 # Clear a bunch of variables before executing the submake
 tools/: FORCE
 	$(Q)mkdir -p $(objtree)/tools
-	$(Q)$(MAKE) LDFLAGS= MAKEFLAGS= O=$(objtree) subdir=tools -C $(src)/tools/
+	$(Q)$(MAKE) LDFLAGS= MAKEFLAGS="$(filter --j% -j,$(MAKEFLAGS))" O=$(objtree) subdir=tools -C $(src)/tools/
 
 tools/%: FORCE
 	$(Q)mkdir -p $(objtree)/tools
-	$(Q)$(MAKE) LDFLAGS= MAKEFLAGS= O=$(objtree) subdir=tools -C $(src)/tools/ $*
+	$(Q)$(MAKE) LDFLAGS= MAKEFLAGS="$(filter --j% -j,$(MAKEFLAGS))" O=$(objtree) subdir=tools -C $(src)/tools/ $*
 
 # Single targets
 # ---------------------------------------------------------------------------
diff --git a/arch/alpha/Makefile b/arch/alpha/Makefile
index 4759fe751aa..2cc3cc519c5 100644
--- a/arch/alpha/Makefile
+++ b/arch/alpha/Makefile
@@ -12,7 +12,7 @@ NM := $(NM) -B
 
 LDFLAGS_vmlinux	:= -static -N #-relax
 CHECKFLAGS	+= -D__alpha__ -m64
-cflags-y	:= -pipe -mno-fp-regs -ffixed-8 -msmall-data
+cflags-y	:= -pipe -mno-fp-regs -ffixed-8
 cflags-y	+= $(call cc-option, -fno-jump-tables)
 
 cpuflags-$(CONFIG_ALPHA_EV4)		:= -mcpu=ev4
diff --git a/arch/alpha/include/asm/floppy.h b/arch/alpha/include/asm/floppy.h
index 46cefbd50e7..bae97eb19d2 100644
--- a/arch/alpha/include/asm/floppy.h
+++ b/arch/alpha/include/asm/floppy.h
@@ -26,7 +26,7 @@
 #define fd_disable_irq()        disable_irq(FLOPPY_IRQ)
 #define fd_cacheflush(addr,size) /* nothing */
 #define fd_request_irq()        request_irq(FLOPPY_IRQ, floppy_interrupt,\
-					    IRQF_DISABLED, "floppy", NULL)
+					    0, "floppy", NULL)
 #define fd_free_irq()           free_irq(FLOPPY_IRQ, NULL)
 
 #ifdef CONFIG_PCI
diff --git a/arch/alpha/include/asm/thread_info.h b/arch/alpha/include/asm/thread_info.h
index 1f8c72959fb..52cd2a4a3ff 100644
--- a/arch/alpha/include/asm/thread_info.h
+++ b/arch/alpha/include/asm/thread_info.h
@@ -95,8 +95,6 @@ register struct thread_info *__current_thread_info __asm__("$8");
 #define TS_POLLING		0x0010	/* idle task polling need_resched,
 					   skip sending interrupt */
 
-#define tsk_is_polling(t) (task_thread_info(t)->status & TS_POLLING)
-
 #ifndef __ASSEMBLY__
 #define HAVE_SET_RESTORE_SIGMASK	1
 static inline void set_restore_sigmask(void)
diff --git a/arch/alpha/kernel/irq.c b/arch/alpha/kernel/irq.c
index 2872accd221..7b2be251c30 100644
--- a/arch/alpha/kernel/irq.c
+++ b/arch/alpha/kernel/irq.c
@@ -117,13 +117,6 @@ handle_irq(int irq)
 		return;
 	}
 
-	/*
-	 * From here we must proceed with IPL_MAX. Note that we do not
-	 * explicitly enable interrupts afterwards - some MILO PALcode
-	 * (namely LX164 one) seems to have severe problems with RTI
-	 * at IPL 0.
-	 */
-	local_irq_disable();
 	irq_enter();
 	generic_handle_irq_desc(irq, desc);
 	irq_exit();
diff --git a/arch/alpha/kernel/irq_alpha.c b/arch/alpha/kernel/irq_alpha.c
index 772ddfdb71a..f433fc11877 100644
--- a/arch/alpha/kernel/irq_alpha.c
+++ b/arch/alpha/kernel/irq_alpha.c
@@ -45,6 +45,14 @@ do_entInt(unsigned long type, unsigned long vector,
 	  unsigned long la_ptr, struct pt_regs *regs)
 {
 	struct pt_regs *old_regs;
+
+	/*
+	 * Disable interrupts during IRQ handling.
+	 * Note that there is no matching local_irq_enable() due to
+	 * severe problems with RTI at IPL0 and some MILO PALcode
+	 * (namely LX164).
+	 */
+	local_irq_disable();
 	switch (type) {
 	case 0:
 #ifdef CONFIG_SMP
@@ -62,7 +70,6 @@ do_entInt(unsigned long type, unsigned long vector,
 	  {
 		long cpu;
 
-		local_irq_disable();
 		smp_percpu_timer_interrupt(regs);
 		cpu = smp_processor_id();
 		if (cpu != boot_cpuid) {
@@ -222,7 +229,6 @@ process_mcheck_info(unsigned long vector, unsigned long la_ptr,
 
 struct irqaction timer_irqaction = {
 	.handler	= timer_interrupt,
-	.flags		= IRQF_DISABLED,
 	.name		= "timer",
 };
 
diff --git a/arch/alpha/kernel/process.c b/arch/alpha/kernel/process.c
index 63d27fb9b02..a3fd8a29cca 100644
--- a/arch/alpha/kernel/process.c
+++ b/arch/alpha/kernel/process.c
@@ -46,25 +46,6 @@
 void (*pm_power_off)(void) = machine_power_off;
 EXPORT_SYMBOL(pm_power_off);
 
-void
-cpu_idle(void)
-{
-	current_thread_info()->status |= TS_POLLING;
-
-	while (1) {
-		/* FIXME -- EV6 and LCA45 know how to power down
-		   the CPU.  */
-
-		rcu_idle_enter();
-		while (!need_resched())
-			cpu_relax();
-
-		rcu_idle_exit();
-		schedule_preempt_disabled();
-	}
-}
-
-
 struct halt_info {
 	int mode;
 	char *restart_cmd;
diff --git a/arch/alpha/kernel/smp.c b/arch/alpha/kernel/smp.c
index 9603bc234b4..7b60834fb4b 100644
--- a/arch/alpha/kernel/smp.c
+++ b/arch/alpha/kernel/smp.c
@@ -167,8 +167,7 @@ smp_callin(void)
 	      cpuid, current, current->active_mm));
 
 	preempt_disable();
-	/* Do nothing.  */
-	cpu_idle();
+	cpu_startup_entry(CPUHP_ONLINE);
 }
 
 /* Wait until hwrpb->txrdy is clear for cpu.  Return -1 on timeout.  */
diff --git a/arch/alpha/kernel/sys_nautilus.c b/arch/alpha/kernel/sys_nautilus.c
index 4d4c046f708..1d4aabfcf9a 100644
--- a/arch/alpha/kernel/sys_nautilus.c
+++ b/arch/alpha/kernel/sys_nautilus.c
@@ -185,9 +185,12 @@ nautilus_machine_check(unsigned long vector, unsigned long la_ptr)
 	mb();
 }
 
-extern void free_reserved_mem(void *, void *);
 extern void pcibios_claim_one_bus(struct pci_bus *);
 
+static struct resource irongate_io = {
+	.name	= "Irongate PCI IO",
+	.flags	= IORESOURCE_IO,
+};
 static struct resource irongate_mem = {
 	.name	= "Irongate PCI MEM",
 	.flags	= IORESOURCE_MEM,
@@ -209,6 +212,7 @@ nautilus_init_pci(void)
 
 	irongate = pci_get_bus_and_slot(0, 0);
 	bus->self = irongate;
+	bus->resource[0] = &irongate_io;
 	bus->resource[1] = &irongate_mem;
 
 	pci_bus_size_bridges(bus);
@@ -234,8 +238,8 @@ nautilus_init_pci(void)
 	if (pci_mem < memtop)
 		memtop = pci_mem;
 	if (memtop > alpha_mv.min_mem_address) {
-		free_reserved_mem(__va(alpha_mv.min_mem_address),
-				  __va(memtop));
+		free_reserved_area((unsigned long)__va(alpha_mv.min_mem_address),
+				   (unsigned long)__va(memtop), 0, NULL);
 		printk("nautilus_init_pci: %ldk freed\n",
 			(memtop - alpha_mv.min_mem_address) >> 10);
 	}
diff --git a/arch/alpha/kernel/sys_titan.c b/arch/alpha/kernel/sys_titan.c
index 5cf4a481b8c..a53cf03f49d 100644
--- a/arch/alpha/kernel/sys_titan.c
+++ b/arch/alpha/kernel/sys_titan.c
@@ -280,15 +280,15 @@ titan_late_init(void)
 	 * all reported to the kernel as machine checks, so the handler
 	 * is a nop so it can be called to count the individual events.
 	 */
-	titan_request_irq(63+16, titan_intr_nop, IRQF_DISABLED,
+	titan_request_irq(63+16, titan_intr_nop, 0,
 		    "CChip Error", NULL);
-	titan_request_irq(62+16, titan_intr_nop, IRQF_DISABLED,
+	titan_request_irq(62+16, titan_intr_nop, 0,
 		    "PChip 0 H_Error", NULL);
-	titan_request_irq(61+16, titan_intr_nop, IRQF_DISABLED,
+	titan_request_irq(61+16, titan_intr_nop, 0,
 		    "PChip 1 H_Error", NULL);
-	titan_request_irq(60+16, titan_intr_nop, IRQF_DISABLED,
+	titan_request_irq(60+16, titan_intr_nop, 0,
 		    "PChip 0 C_Error", NULL);
-	titan_request_irq(59+16, titan_intr_nop, IRQF_DISABLED,
+	titan_request_irq(59+16, titan_intr_nop, 0,
 		    "PChip 1 C_Error", NULL);
 
 	/* 
@@ -348,9 +348,9 @@ privateer_init_pci(void)
 	 * Hook a couple of extra err interrupts that the
 	 * common titan code won't.
 	 */
-	titan_request_irq(53+16, titan_intr_nop, IRQF_DISABLED,
+	titan_request_irq(53+16, titan_intr_nop, 0,
 		    "NMI", NULL);
-	titan_request_irq(50+16, titan_intr_nop, IRQF_DISABLED,
+	titan_request_irq(50+16, titan_intr_nop, 0,
 		    "Temperature Warning", NULL);
 
 	/*
diff --git a/arch/alpha/mm/init.c b/arch/alpha/mm/init.c
index 1ad6ca74bed..0ba85ee4a46 100644
--- a/arch/alpha/mm/init.c
+++ b/arch/alpha/mm/init.c
@@ -31,6 +31,7 @@
 #include <asm/console.h>
 #include <asm/tlb.h>
 #include <asm/setup.h>
+#include <asm/sections.h>
 
 extern void die_if_kernel(char *,struct pt_regs *,long);
 
@@ -281,8 +282,6 @@ printk_memory_info(void)
 {
 	unsigned long codesize, reservedpages, datasize, initsize, tmp;
 	extern int page_is_ram(unsigned long) __init;
-	extern char _text, _etext, _data, _edata;
-	extern char __init_begin, __init_end;
 
 	/* printk all informations */
 	reservedpages = 0;
@@ -318,32 +317,15 @@ mem_init(void)
 #endif /* CONFIG_DISCONTIGMEM */
 
 void
-free_reserved_mem(void *start, void *end)
-{
-	void *__start = start;
-	for (; __start < end; __start += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(__start));
-		init_page_count(virt_to_page(__start));
-		free_page((long)__start);
-		totalram_pages++;
-	}
-}
-
-void
 free_initmem(void)
 {
-	extern char __init_begin, __init_end;
-
-	free_reserved_mem(&__init_begin, &__init_end);
-	printk ("Freeing unused kernel memory: %ldk freed\n",
-		(&__init_end - &__init_begin) >> 10);
+	free_initmem_default(0);
 }
 
 #ifdef CONFIG_BLK_DEV_INITRD
 void
 free_initrd_mem(unsigned long start, unsigned long end)
 {
-	free_reserved_mem((void *)start, (void *)end);
-	printk ("Freeing initrd memory: %ldk freed\n", (end - start) >> 10);
+	free_reserved_area(start, end, 0, "initrd");
 }
 #endif
diff --git a/arch/alpha/mm/numa.c b/arch/alpha/mm/numa.c
index 3973ae39577..33885048fa3 100644
--- a/arch/alpha/mm/numa.c
+++ b/arch/alpha/mm/numa.c
@@ -17,6 +17,7 @@
 
 #include <asm/hwrpb.h>
 #include <asm/pgalloc.h>
+#include <asm/sections.h>
 
 pg_data_t node_data[MAX_NUMNODES];
 EXPORT_SYMBOL(node_data);
@@ -325,8 +326,6 @@ void __init mem_init(void)
 {
 	unsigned long codesize, reservedpages, datasize, initsize, pfn;
 	extern int page_is_ram(unsigned long) __init;
-	extern char _text, _etext, _data, _edata;
-	extern char __init_begin, __init_end;
 	unsigned long nid, i;
 	high_memory = (void *) __va(max_low_pfn << PAGE_SHIFT);
 
diff --git a/arch/arc/include/asm/dma-mapping.h b/arch/arc/include/asm/dma-mapping.h
index 31f77aec082..45b8e0cea17 100644
--- a/arch/arc/include/asm/dma-mapping.h
+++ b/arch/arc/include/asm/dma-mapping.h
@@ -126,7 +126,7 @@ dma_map_sg(struct device *dev, struct scatterlist *sg,
 	int i;
 
 	for_each_sg(sg, s, nents, i)
-		sg->dma_address = dma_map_page(dev, sg_page(s), s->offset,
+		s->dma_address = dma_map_page(dev, sg_page(s), s->offset,
 					       s->length, dir);
 
 	return nents;
diff --git a/arch/arc/include/asm/elf.h b/arch/arc/include/asm/elf.h
index f4c8d36ebec..a2628285768 100644
--- a/arch/arc/include/asm/elf.h
+++ b/arch/arc/include/asm/elf.h
@@ -72,7 +72,4 @@ extern int elf_check_arch(const struct elf32_hdr *);
  */
 #define ELF_PLATFORM	(NULL)
 
-#define SET_PERSONALITY(ex) \
-	set_personality(PER_LINUX | (current->personality & (~PER_MASK)))
-
 #endif
diff --git a/arch/arc/include/asm/entry.h b/arch/arc/include/asm/entry.h
index 23daa326fc9..eb2ae53187d 100644
--- a/arch/arc/include/asm/entry.h
+++ b/arch/arc/include/asm/entry.h
@@ -415,7 +415,7 @@
  *-------------------------------------------------------------*/
 .macro SAVE_ALL_EXCEPTION   marker
 
-	st      \marker, [sp, 8]
+	st      \marker, [sp, 8]	/* orig_r8 */
 	st      r0, [sp, 4]    /* orig_r0, needed only for sys calls */
 
 	/* Restore r9 used to code the early prologue */
diff --git a/arch/arc/include/asm/irqflags.h b/arch/arc/include/asm/irqflags.h
index ccd84806b62..eac07166820 100644
--- a/arch/arc/include/asm/irqflags.h
+++ b/arch/arc/include/asm/irqflags.h
@@ -39,7 +39,7 @@ static inline long arch_local_irq_save(void)
 	"	flag.nz %0		\n"
 	: "=r"(temp), "=r"(flags)
 	: "n"((STATUS_E1_MASK | STATUS_E2_MASK))
-	: "cc");
+	: "memory", "cc");
 
 	return flags;
 }
@@ -53,7 +53,8 @@ static inline void arch_local_irq_restore(unsigned long flags)
 	__asm__ __volatile__(
 	"	flag %0			\n"
 	:
-	: "r"(flags));
+	: "r"(flags)
+	: "memory");
 }
 
 /*
@@ -73,7 +74,8 @@ static inline void arch_local_irq_disable(void)
 	"	and %0, %0, %1		\n"
 	"	flag %0			\n"
 	: "=&r"(temp)
-	: "n"(~(STATUS_E1_MASK | STATUS_E2_MASK)));
+	: "n"(~(STATUS_E1_MASK | STATUS_E2_MASK))
+	: "memory");
 }
 
 /*
@@ -85,7 +87,9 @@ static inline long arch_local_save_flags(void)
 
 	__asm__ __volatile__(
 	"	lr  %0, [status32]	\n"
-	: "=&r"(temp));
+	: "=&r"(temp)
+	:
+	: "memory");
 
 	return temp;
 }
diff --git a/arch/arc/include/asm/kgdb.h b/arch/arc/include/asm/kgdb.h
index f3c4934f0ca..4930957ca3d 100644
--- a/arch/arc/include/asm/kgdb.h
+++ b/arch/arc/include/asm/kgdb.h
@@ -13,7 +13,7 @@
 
 #ifdef CONFIG_KGDB
 
-#include <asm/user.h>
+#include <asm/ptrace.h>
 
 /* to ensure compatibility with Linux 2.6.35, we don't implement the get/set
  * register API yet */
@@ -53,9 +53,7 @@ enum arc700_linux_regnums {
 };
 
 #else
-static inline void kgdb_trap(struct pt_regs *regs, int param)
-{
-}
+#define kgdb_trap(regs, param)
 #endif
 
 #endif	/* __ARC_KGDB_H__ */
diff --git a/arch/arc/include/asm/ptrace.h b/arch/arc/include/asm/ptrace.h
index 8ae783d20a8..6179de7e07c 100644
--- a/arch/arc/include/asm/ptrace.h
+++ b/arch/arc/include/asm/ptrace.h
@@ -123,7 +123,7 @@ static inline long regs_return_value(struct pt_regs *regs)
 #define orig_r8_IS_SCALL		0x0001
 #define orig_r8_IS_SCALL_RESTARTED	0x0002
 #define orig_r8_IS_BRKPT		0x0004
-#define orig_r8_IS_EXCPN		0x0004
+#define orig_r8_IS_EXCPN		0x0008
 #define orig_r8_IS_IRQ1			0x0010
 #define orig_r8_IS_IRQ2			0x0020
 
diff --git a/arch/arc/include/asm/syscalls.h b/arch/arc/include/asm/syscalls.h
index e53a5340ba4..dd785befe7f 100644
--- a/arch/arc/include/asm/syscalls.h
+++ b/arch/arc/include/asm/syscalls.h
@@ -16,8 +16,6 @@
 #include <linux/types.h>
 
 int sys_clone_wrapper(int, int, int, int, int);
-int sys_fork_wrapper(void);
-int sys_vfork_wrapper(void);
 int sys_cacheflush(uint32_t, uint32_t uint32_t);
 int sys_arc_settls(void *);
 int sys_arc_gettls(void);
diff --git a/arch/arc/include/uapi/asm/ptrace.h b/arch/arc/include/uapi/asm/ptrace.h
index 6afa4f70207..30333cec0fe 100644
--- a/arch/arc/include/uapi/asm/ptrace.h
+++ b/arch/arc/include/uapi/asm/ptrace.h
@@ -28,14 +28,14 @@
 */
 struct user_regs_struct {
 
-	struct scratch {
+	struct {
 		long pad;
 		long bta, lp_start, lp_end, lp_count;
 		long status32, ret, blink, fp, gp;
 		long r12, r11, r10, r9, r8, r7, r6, r5, r4, r3, r2, r1, r0;
 		long sp;
 	} scratch;
-	struct callee {
+	struct {
 		long pad;
 		long r25, r24, r23, r22, r21, r20;
 		long r19, r18, r17, r16, r15, r14, r13;
diff --git a/arch/arc/kernel/disasm.c b/arch/arc/kernel/disasm.c
index 2f390289a79..d14764ae2c6 100644
--- a/arch/arc/kernel/disasm.c
+++ b/arch/arc/kernel/disasm.c
@@ -535,4 +535,4 @@ int __kprobes disasm_next_pc(unsigned long pc, struct pt_regs *regs,
 	return instr.is_branch;
 }
 
-#endif /* CONFIG_KGDB || CONFIG_MISALIGN_ACCESS || CONFIG_KPROBES */
+#endif /* CONFIG_KGDB || CONFIG_ARC_MISALIGN_ACCESS || CONFIG_KPROBES */
diff --git a/arch/arc/kernel/entry.S b/arch/arc/kernel/entry.S
index ef6800ba2f0..91eeab81f52 100644
--- a/arch/arc/kernel/entry.S
+++ b/arch/arc/kernel/entry.S
@@ -452,7 +452,7 @@ tracesys:
 	; using ERET won't work since next-PC has already committed
 	lr  r12, [efa]
 	GET_CURR_TASK_FIELD_PTR   TASK_THREAD, r11
-	st  r12, [r11, THREAD_FAULT_ADDR]
+	st  r12, [r11, THREAD_FAULT_ADDR]	; thread.fault_address
 
 	; PRE Sys Call Ptrace hook
 	mov r0, sp			; pt_regs needed
@@ -792,31 +792,6 @@ ARC_EXIT ret_from_fork
 
 ;################### Special Sys Call Wrappers ##########################
 
-; TBD: call do_fork directly from here
-ARC_ENTRY sys_fork_wrapper
-	SAVE_CALLEE_SAVED_USER
-	bl  @sys_fork
-	DISCARD_CALLEE_SAVED_USER
-
-	GET_CURR_THR_INFO_FLAGS   r10
-	btst r10, TIF_SYSCALL_TRACE
-	bnz  tracesys_exit
-
-	b ret_from_system_call
-ARC_EXIT sys_fork_wrapper
-
-ARC_ENTRY sys_vfork_wrapper
-	SAVE_CALLEE_SAVED_USER
-	bl  @sys_vfork
-	DISCARD_CALLEE_SAVED_USER
-
-	GET_CURR_THR_INFO_FLAGS   r10
-	btst r10, TIF_SYSCALL_TRACE
-	bnz  tracesys_exit
-
-	b ret_from_system_call
-ARC_EXIT sys_vfork_wrapper
-
 ARC_ENTRY sys_clone_wrapper
 	SAVE_CALLEE_SAVED_USER
 	bl  @sys_clone
diff --git a/arch/arc/kernel/kgdb.c b/arch/arc/kernel/kgdb.c
index 2888ba5be47..52bdc83c149 100644
--- a/arch/arc/kernel/kgdb.c
+++ b/arch/arc/kernel/kgdb.c
@@ -9,6 +9,7 @@
  */
 
 #include <linux/kgdb.h>
+#include <linux/sched.h>
 #include <asm/disasm.h>
 #include <asm/cacheflush.h>
 
diff --git a/arch/arc/kernel/process.c b/arch/arc/kernel/process.c
index 0a7531d9929..cad66851e0c 100644
--- a/arch/arc/kernel/process.c
+++ b/arch/arc/kernel/process.c
@@ -41,37 +41,12 @@ SYSCALL_DEFINE0(arc_gettls)
 	return task_thread_info(current)->thr_ptr;
 }
 
-static inline void arch_idle(void)
+void arch_cpu_idle(void)
 {
 	/* sleep, but enable all interrupts before committing */
 	__asm__("sleep 0x3");
 }
 
-void cpu_idle(void)
-{
-	/* Since we SLEEP in idle loop, TIF_POLLING_NRFLAG can't be set */
-
-	/* endless idle loop with no priority at all */
-	while (1) {
-		tick_nohz_idle_enter();
-		rcu_idle_enter();
-
-doze:
-		local_irq_disable();
-		if (!need_resched()) {
-			arch_idle();
-			goto doze;
-		} else {
-			local_irq_enable();
-		}
-
-		rcu_idle_exit();
-		tick_nohz_idle_exit();
-
-		schedule_preempt_disabled();
-	}
-}
-
 asmlinkage void ret_from_fork(void);
 
 /* Layout of Child kernel mode stack as setup at the end of this function is
diff --git a/arch/arc/kernel/setup.c b/arch/arc/kernel/setup.c
index dc0f968dae0..2d95ac07df7 100644
--- a/arch/arc/kernel/setup.c
+++ b/arch/arc/kernel/setup.c
@@ -232,10 +232,8 @@ char *arc_extn_mumbojumbo(int cpu_id, char *buf, int len)
 
 	n += scnprintf(buf + n, len - n, "\n");
 
-#ifdef _ASM_GENERIC_UNISTD_H
 	n += scnprintf(buf + n, len - n,
-		       "OS ABI [v2]\t: asm-generic/{unistd,stat,fcntl}\n");
-#endif
+		       "OS ABI [v3]\t: no-legacy-syscalls\n");
 
 	return buf;
 }
diff --git a/arch/arc/kernel/smp.c b/arch/arc/kernel/smp.c
index 3af3e06dcf0..5c7fd603d21 100644
--- a/arch/arc/kernel/smp.c
+++ b/arch/arc/kernel/smp.c
@@ -141,7 +141,7 @@ void __cpuinit start_kernel_secondary(void)
 
 	local_irq_enable();
 	preempt_disable();
-	cpu_idle();
+	cpu_startup_entry(CPUHP_ONLINE);
 }
 
 /*
diff --git a/arch/arc/kernel/sys.c b/arch/arc/kernel/sys.c
index f6bdd07583f..9d6c1ca26af 100644
--- a/arch/arc/kernel/sys.c
+++ b/arch/arc/kernel/sys.c
@@ -6,8 +6,6 @@
 #include <asm/syscalls.h>
 
 #define sys_clone	sys_clone_wrapper
-#define sys_fork	sys_fork_wrapper
-#define sys_vfork	sys_vfork_wrapper
 
 #undef __SYSCALL
 #define __SYSCALL(nr, call) [nr] = (call),
diff --git a/arch/arc/mm/init.c b/arch/arc/mm/init.c
index caf797de23f..727d4794ea0 100644
--- a/arch/arc/mm/init.c
+++ b/arch/arc/mm/init.c
@@ -144,37 +144,18 @@ void __init mem_init(void)
 		PAGES_TO_KB(reserved_pages));
 }
 
-static void __init free_init_pages(const char *what, unsigned long begin,
-				   unsigned long end)
-{
-	unsigned long addr;
-
-	pr_info("Freeing %s: %ldk [%lx] to [%lx]\n",
-		what, TO_KB(end - begin), begin, end);
-
-	/* need to check that the page we free is not a partial page */
-	for (addr = begin; addr + PAGE_SIZE <= end; addr += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(addr));
-		init_page_count(virt_to_page(addr));
-		free_page(addr);
-		totalram_pages++;
-	}
-}
-
 /*
  * free_initmem: Free all the __init memory.
  */
 void __init_refok free_initmem(void)
 {
-	free_init_pages("unused kernel memory",
-			(unsigned long)__init_begin,
-			(unsigned long)__init_end);
+	free_initmem_default(0);
 }
 
 #ifdef CONFIG_BLK_DEV_INITRD
 void __init free_initrd_mem(unsigned long start, unsigned long end)
 {
-	free_init_pages("initrd memory", start, end);
+	free_reserved_area(start, end, 0, "initrd");
 }
 #endif
 
diff --git a/arch/arc/plat-arcfpga/Kconfig b/arch/arc/plat-arcfpga/Kconfig
index b41e786cdbc..295cefeb25d 100644
--- a/arch/arc/plat-arcfpga/Kconfig
+++ b/arch/arc/plat-arcfpga/Kconfig
@@ -53,7 +53,7 @@ menuconfig ARC_HAS_BVCI_LAT_UNIT
 	bool "BVCI Bus Latency Unit"
 	depends on ARC_BOARD_ML509 || ARC_BOARD_ANGEL4
 	help
-	  IP to add artifical latency to BVCI Bus Based FPGA builds.
+	  IP to add artificial latency to BVCI Bus Based FPGA builds.
 	  The default latency (even worst case) for FPGA is non-realistic
 	  (~10 SDRAM, ~5 SSRAM).
 
diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index 13b739469c5..a39e3214ea3 100644
--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -15,6 +15,7 @@ config ARM
 	select GENERIC_IRQ_SHOW
 	select GENERIC_PCI_IOMAP
 	select GENERIC_SMP_IDLE_THREAD
+	select GENERIC_IDLE_POLL_SETUP
 	select GENERIC_STRNCPY_FROM_USER
 	select GENERIC_STRNLEN_USER
 	select HARDIRQS_SW_RESEND
@@ -549,6 +550,8 @@ config ARCH_IXP4XX
 	select GENERIC_CLOCKEVENTS
 	select MIGHT_HAVE_PCI
 	select NEED_MACH_IO_H
+	select USB_EHCI_BIG_ENDIAN_MMIO
+	select USB_EHCI_BIG_ENDIAN_DESC
 	help
 	  Support for Intel's IXP4XX (XScale) family of processors.
 
@@ -1183,9 +1186,9 @@ config ARM_NR_BANKS
 	default 8
 
 config IWMMXT
-	bool "Enable iWMMXt support"
+	bool "Enable iWMMXt support" if !CPU_PJ4
 	depends on CPU_XSCALE || CPU_XSC3 || CPU_MOHAWK || CPU_PJ4
-	default y if PXA27x || PXA3xx || ARCH_MMP
+	default y if PXA27x || PXA3xx || ARCH_MMP || CPU_PJ4
 	help
 	  Enable support for iWMMXt context switching at run time if
 	  running on a CPU that supports it.
@@ -1439,6 +1442,16 @@ config ARM_ERRATA_775420
 	 to deadlock. This workaround puts DSB before executing ISB if
 	 an abort may occur on cache maintenance.
 
+config ARM_ERRATA_798181
+	bool "ARM errata: TLBI/DSB failure on Cortex-A15"
+	depends on CPU_V7 && SMP
+	help
+	  On Cortex-A15 (r0p0..r3p2) the TLBI*IS/DSB operations are not
+	  adequately shooting down all use of the old entries. This
+	  option enables the Linux kernel workaround for this erratum
+	  which sends an IPI to the CPUs that are running the same ASID
+	  as the one being invalidated.
+
 endmenu
 
 source "arch/arm/common/Kconfig"
diff --git a/arch/arm/Kconfig.debug b/arch/arm/Kconfig.debug
index ecfcdba2d17..9b31f4311ea 100644
--- a/arch/arm/Kconfig.debug
+++ b/arch/arm/Kconfig.debug
@@ -495,6 +495,7 @@ config DEBUG_IMX_UART_PORT
 						DEBUG_IMX53_UART || \
 						DEBUG_IMX6Q_UART
 	default 1
+	depends on ARCH_MXC
 	help
 	  Choose UART port on which kernel low-level debug messages
 	  should be output.
diff --git a/arch/arm/boot/dts/armada-370-mirabox.dts b/arch/arm/boot/dts/armada-370-mirabox.dts
index dd0c57dd9f3..3234875824d 100644
--- a/arch/arm/boot/dts/armada-370-mirabox.dts
+++ b/arch/arm/boot/dts/armada-370-mirabox.dts
@@ -54,7 +54,7 @@
 		};
 
 		mvsdio@d00d4000 {
-			pinctrl-0 = <&sdio_pins2>;
+			pinctrl-0 = <&sdio_pins3>;
 			pinctrl-names = "default";
 			status = "okay";
 			/*
diff --git a/arch/arm/boot/dts/armada-370.dtsi b/arch/arm/boot/dts/armada-370.dtsi
index 8188d138020..a195debb67d 100644
--- a/arch/arm/boot/dts/armada-370.dtsi
+++ b/arch/arm/boot/dts/armada-370.dtsi
@@ -59,6 +59,12 @@
 					     "mpp50", "mpp51", "mpp52";
 			      marvell,function = "sd0";
 			};
+
+			sdio_pins3: sdio-pins3 {
+			      marvell,pins = "mpp48", "mpp49", "mpp50",
+					     "mpp51", "mpp52", "mpp53";
+			      marvell,function = "sd0";
+			};
 	        };
 
 		gpio0: gpio@d0018100 {
diff --git a/arch/arm/boot/dts/at91sam9260.dtsi b/arch/arm/boot/dts/at91sam9260.dtsi
index cb7bcc51608..39253b9aedd 100644
--- a/arch/arm/boot/dts/at91sam9260.dtsi
+++ b/arch/arm/boot/dts/at91sam9260.dtsi
@@ -322,6 +322,24 @@
 					};
 				};
 
+				spi0 {
+					pinctrl_spi0: spi0-0 {
+						atmel,pins =
+							<0 0 0x1 0x0	/* PA0 periph A SPI0_MISO pin */
+							 0 1 0x1 0x0	/* PA1 periph A SPI0_MOSI pin */
+							 0 2 0x1 0x0>;	/* PA2 periph A SPI0_SPCK pin */
+					};
+				};
+
+				spi1 {
+					pinctrl_spi1: spi1-0 {
+						atmel,pins =
+							<1 0 0x1 0x0	/* PB0 periph A SPI1_MISO pin */
+							 1 1 0x1 0x0	/* PB1 periph A SPI1_MOSI pin */
+							 1 2 0x1 0x0>;	/* PB2 periph A SPI1_SPCK pin */
+					};
+				};
+
 				pioA: gpio@fffff400 {
 					compatible = "atmel,at91rm9200-gpio";
 					reg = <0xfffff400 0x200>;
@@ -471,6 +489,28 @@
 				status = "disabled";
 			};
 
+			spi0: spi@fffc8000 {
+				#address-cells = <1>;
+				#size-cells = <0>;
+				compatible = "atmel,at91rm9200-spi";
+				reg = <0xfffc8000 0x200>;
+				interrupts = <12 4 3>;
+				pinctrl-names = "default";
+				pinctrl-0 = <&pinctrl_spi0>;
+				status = "disabled";
+			};
+
+			spi1: spi@fffcc000 {
+				#address-cells = <1>;
+				#size-cells = <0>;
+				compatible = "atmel,at91rm9200-spi";
+				reg = <0xfffcc000 0x200>;
+				interrupts = <13 4 3>;
+				pinctrl-names = "default";
+				pinctrl-0 = <&pinctrl_spi1>;
+				status = "disabled";
+			};
+
 			adc0: adc@fffe0000 {
 				compatible = "atmel,at91sam9260-adc";
 				reg = <0xfffe0000 0x100>;
diff --git a/arch/arm/boot/dts/at91sam9263.dtsi b/arch/arm/boot/dts/at91sam9263.dtsi
index 271d4de026e..94b58ab2cc0 100644
--- a/arch/arm/boot/dts/at91sam9263.dtsi
+++ b/arch/arm/boot/dts/at91sam9263.dtsi
@@ -303,6 +303,24 @@
 					};
 				};
 
+				spi0 {
+					pinctrl_spi0: spi0-0 {
+						atmel,pins =
+							<0 0 0x2 0x0	/* PA0 periph B SPI0_MISO pin */
+							 0 1 0x2 0x0	/* PA1 periph B SPI0_MOSI pin */
+							 0 2 0x2 0x0>;	/* PA2 periph B SPI0_SPCK pin */
+					};
+				};
+
+				spi1 {
+					pinctrl_spi1: spi1-0 {
+						atmel,pins =
+							<1 12 0x1 0x0	/* PB12 periph A SPI1_MISO pin */
+							 1 13 0x1 0x0	/* PB13 periph A SPI1_MOSI pin */
+							 1 14 0x1 0x0>;	/* PB14 periph A SPI1_SPCK pin */
+					};
+				};
+
 				pioA: gpio@fffff200 {
 					compatible = "atmel,at91rm9200-gpio";
 					reg = <0xfffff200 0x200>;
@@ -462,6 +480,28 @@
 				reg = <0xfffffd40 0x10>;
 				status = "disabled";
 			};
+
+			spi0: spi@fffa4000 {
+				#address-cells = <1>;
+				#size-cells = <0>;
+				compatible = "atmel,at91rm9200-spi";
+				reg = <0xfffa4000 0x200>;
+				interrupts = <14 4 3>;
+				pinctrl-names = "default";
+				pinctrl-0 = <&pinctrl_spi0>;
+				status = "disabled";
+			};
+
+			spi1: spi@fffa8000 {
+				#address-cells = <1>;
+				#size-cells = <0>;
+				compatible = "atmel,at91rm9200-spi";
+				reg = <0xfffa8000 0x200>;
+				interrupts = <15 4 3>;
+				pinctrl-names = "default";
+				pinctrl-0 = <&pinctrl_spi1>;
+				status = "disabled";
+			};
 		};
 
 		nand0: nand@40000000 {
diff --git a/arch/arm/boot/dts/at91sam9263ek.dts b/arch/arm/boot/dts/at91sam9263ek.dts
index 1eb08728f52..a14e424b2e8 100644
--- a/arch/arm/boot/dts/at91sam9263ek.dts
+++ b/arch/arm/boot/dts/at91sam9263ek.dts
@@ -79,6 +79,16 @@
 					};
 				};
 			};
+
+			spi0: spi@fffa4000 {
+				status = "okay";
+				cs-gpios = <&pioA 5 0>, <0>, <0>, <0>;
+				mtd_dataflash@0 {
+					compatible = "atmel,at45", "atmel,dataflash";
+					spi-max-frequency = <50000000>;
+					reg = <0>;
+				};
+			};
 		};
 
 		nand0: nand@40000000 {
diff --git a/arch/arm/boot/dts/at91sam9g20ek_common.dtsi b/arch/arm/boot/dts/at91sam9g20ek_common.dtsi
index da15e83e7f1..23d1f468f27 100644
--- a/arch/arm/boot/dts/at91sam9g20ek_common.dtsi
+++ b/arch/arm/boot/dts/at91sam9g20ek_common.dtsi
@@ -96,6 +96,16 @@
 				status = "okay";
 				pinctrl-0 = <&pinctrl_ssc0_tx>;
 			};
+
+			spi0: spi@fffc8000 {
+				status = "okay";
+				cs-gpios = <0>, <&pioC 11 0>, <0>, <0>;
+				mtd_dataflash@0 {
+					compatible = "atmel,at45", "atmel,dataflash";
+					spi-max-frequency = <50000000>;
+					reg = <1>;
+				};
+			};
 		};
 
 		nand0: nand@40000000 {
diff --git a/arch/arm/boot/dts/at91sam9g45.dtsi b/arch/arm/boot/dts/at91sam9g45.dtsi
index 6b1d4cab24c..cfdf429578b 100644
--- a/arch/arm/boot/dts/at91sam9g45.dtsi
+++ b/arch/arm/boot/dts/at91sam9g45.dtsi
@@ -322,6 +322,24 @@
 					};
 				};
 
+				spi0 {
+					pinctrl_spi0: spi0-0 {
+						atmel,pins =
+							<1 0 0x1 0x0	/* PB0 periph A SPI0_MISO pin */
+							 1 1 0x1 0x0	/* PB1 periph A SPI0_MOSI pin */
+							 1 2 0x1 0x0>;	/* PB2 periph A SPI0_SPCK pin */
+					};
+				};
+
+				spi1 {
+					pinctrl_spi1: spi1-0 {
+						atmel,pins =
+							<1 14 0x1 0x0	/* PB14 periph A SPI1_MISO pin */
+							 1 15 0x1 0x0	/* PB15 periph A SPI1_MOSI pin */
+							 1 16 0x1 0x0>;	/* PB16 periph A SPI1_SPCK pin */
+					};
+				};
+
 				pioA: gpio@fffff200 {
 					compatible = "atmel,at91rm9200-gpio";
 					reg = <0xfffff200 0x200>;
@@ -531,6 +549,28 @@
 				reg = <0xfffffd40 0x10>;
 				status = "disabled";
 			};
+
+			spi0: spi@fffa4000 {
+				#address-cells = <1>;
+				#size-cells = <0>;
+				compatible = "atmel,at91rm9200-spi";
+				reg = <0xfffa4000 0x200>;
+				interrupts = <14 4 3>;
+				pinctrl-names = "default";
+				pinctrl-0 = <&pinctrl_spi0>;
+				status = "disabled";
+			};
+
+			spi1: spi@fffa8000 {
+				#address-cells = <1>;
+				#size-cells = <0>;
+				compatible = "atmel,at91rm9200-spi";
+				reg = <0xfffa8000 0x200>;
+				interrupts = <15 4 3>;
+				pinctrl-names = "default";
+				pinctrl-0 = <&pinctrl_spi1>;
+				status = "disabled";
+			};
 		};
 
 		nand0: nand@40000000 {
diff --git a/arch/arm/boot/dts/at91sam9m10g45ek.dts b/arch/arm/boot/dts/at91sam9m10g45ek.dts
index 20c31913c27..92c52a7d70b 100644
--- a/arch/arm/boot/dts/at91sam9m10g45ek.dts
+++ b/arch/arm/boot/dts/at91sam9m10g45ek.dts
@@ -102,6 +102,16 @@
 					};
 				};
 			};
+
+			spi0: spi@fffa4000{
+				status = "okay";
+				cs-gpios = <&pioB 3 0>, <0>, <0>, <0>;
+				mtd_dataflash@0 {
+					compatible = "atmel,at45", "atmel,dataflash";
+					spi-max-frequency = <13000000>;
+					reg = <0>;
+				};
+			};
 		};
 
 		nand0: nand@40000000 {
diff --git a/arch/arm/boot/dts/at91sam9n12.dtsi b/arch/arm/boot/dts/at91sam9n12.dtsi
index 7750f98dd76..b2961f1ea51 100644
--- a/arch/arm/boot/dts/at91sam9n12.dtsi
+++ b/arch/arm/boot/dts/at91sam9n12.dtsi
@@ -261,6 +261,24 @@
 					};
 				};
 
+				spi0 {
+					pinctrl_spi0: spi0-0 {
+						atmel,pins =
+							<0 11 0x1 0x0	/* PA11 periph A SPI0_MISO pin */
+							 0 12 0x1 0x0	/* PA12 periph A SPI0_MOSI pin */
+							 0 13 0x1 0x0>;	/* PA13 periph A SPI0_SPCK pin */
+					};
+				};
+
+				spi1 {
+					pinctrl_spi1: spi1-0 {
+						atmel,pins =
+							<0 21 0x2 0x0	/* PA21 periph B SPI1_MISO pin */
+							 0 22 0x2 0x0	/* PA22 periph B SPI1_MOSI pin */
+							 0 23 0x2 0x0>;	/* PA23 periph B SPI1_SPCK pin */
+					};
+				};
+
 				pioA: gpio@fffff400 {
 					compatible = "atmel,at91sam9x5-gpio", "atmel,at91rm9200-gpio";
 					reg = <0xfffff400 0x200>;
@@ -373,6 +391,28 @@
 				#size-cells = <0>;
 				status = "disabled";
 			};
+
+			spi0: spi@f0000000 {
+				#address-cells = <1>;
+				#size-cells = <0>;
+				compatible = "atmel,at91rm9200-spi";
+				reg = <0xf0000000 0x100>;
+				interrupts = <13 4 3>;
+				pinctrl-names = "default";
+				pinctrl-0 = <&pinctrl_spi0>;
+				status = "disabled";
+			};
+
+			spi1: spi@f0004000 {
+				#address-cells = <1>;
+				#size-cells = <0>;
+				compatible = "atmel,at91rm9200-spi";
+				reg = <0xf0004000 0x100>;
+				interrupts = <14 4 3>;
+				pinctrl-names = "default";
+				pinctrl-0 = <&pinctrl_spi1>;
+				status = "disabled";
+			};
 		};
 
 		nand0: nand@40000000 {
diff --git a/arch/arm/boot/dts/at91sam9n12ek.dts b/arch/arm/boot/dts/at91sam9n12ek.dts
index d400f8de438..34c842b1efb 100644
--- a/arch/arm/boot/dts/at91sam9n12ek.dts
+++ b/arch/arm/boot/dts/at91sam9n12ek.dts
@@ -67,6 +67,16 @@
 					};
 				};
 			};
+
+			spi0: spi@f0000000 {
+				status = "okay";
+				cs-gpios = <&pioA 14 0>, <0>, <0>, <0>;
+				m25p80@0 {
+					compatible = "atmel,at25df321a";
+					spi-max-frequency = <50000000>;
+					reg = <0>;
+				};
+			};
 		};
 
 		nand0: nand@40000000 {
diff --git a/arch/arm/boot/dts/at91sam9x5.dtsi b/arch/arm/boot/dts/at91sam9x5.dtsi
index a98c0d50fbb..347b438d47f 100644
--- a/arch/arm/boot/dts/at91sam9x5.dtsi
+++ b/arch/arm/boot/dts/at91sam9x5.dtsi
@@ -343,6 +343,24 @@
 					};
 				};
 
+				spi0 {
+					pinctrl_spi0: spi0-0 {
+						atmel,pins =
+							<0 11 0x1 0x0	/* PA11 periph A SPI0_MISO pin */
+							 0 12 0x1 0x0	/* PA12 periph A SPI0_MOSI pin */
+							 0 13 0x1 0x0>;	/* PA13 periph A SPI0_SPCK pin */
+					};
+				};
+
+				spi1 {
+					pinctrl_spi1: spi1-0 {
+						atmel,pins =
+							<0 21 0x2 0x0	/* PA21 periph B SPI1_MISO pin */
+							 0 22 0x2 0x0	/* PA22 periph B SPI1_MOSI pin */
+							 0 23 0x2 0x0>;	/* PA23 periph B SPI1_SPCK pin */
+					};
+				};
+
 				pioA: gpio@fffff400 {
 					compatible = "atmel,at91sam9x5-gpio", "atmel,at91rm9200-gpio";
 					reg = <0xfffff400 0x200>;
@@ -529,6 +547,28 @@
 					trigger-value = <0x6>;
 				};
 			};
+
+			spi0: spi@f0000000 {
+				#address-cells = <1>;
+				#size-cells = <0>;
+				compatible = "atmel,at91rm9200-spi";
+				reg = <0xf0000000 0x100>;
+				interrupts = <13 4 3>;
+				pinctrl-names = "default";
+				pinctrl-0 = <&pinctrl_spi0>;
+				status = "disabled";
+			};
+
+			spi1: spi@f0004000 {
+				#address-cells = <1>;
+				#size-cells = <0>;
+				compatible = "atmel,at91rm9200-spi";
+				reg = <0xf0004000 0x100>;
+				interrupts = <14 4 3>;
+				pinctrl-names = "default";
+				pinctrl-0 = <&pinctrl_spi1>;
+				status = "disabled";
+			};
 		};
 
 		nand0: nand@40000000 {
diff --git a/arch/arm/boot/dts/at91sam9x5ek.dtsi b/arch/arm/boot/dts/at91sam9x5ek.dtsi
index 8a7cf1d9cf5..09f5e667ca7 100644
--- a/arch/arm/boot/dts/at91sam9x5ek.dtsi
+++ b/arch/arm/boot/dts/at91sam9x5ek.dtsi
@@ -84,6 +84,16 @@
 					};
 				};
 			};
+
+			spi0: spi@f0000000 {
+				status = "okay";
+				cs-gpios = <&pioA 14 0>, <0>, <0>, <0>;
+				m25p80@0 {
+					compatible = "atmel,at25df321a";
+					spi-max-frequency = <50000000>;
+					reg = <0>;
+				};
+			};
 		};
 
 		usb0: ohci@00600000 {
diff --git a/arch/arm/boot/dts/dbx5x0.dtsi b/arch/arm/boot/dts/dbx5x0.dtsi
index 9de93096601..aaa63d0a809 100644
--- a/arch/arm/boot/dts/dbx5x0.dtsi
+++ b/arch/arm/boot/dts/dbx5x0.dtsi
@@ -191,8 +191,8 @@
 
 		prcmu: prcmu@80157000 {
 			compatible = "stericsson,db8500-prcmu";
-			reg = <0x80157000 0x1000>;
-			reg-names = "prcmu";
+			reg = <0x80157000 0x1000>, <0x801b0000 0x8000>, <0x801b8000 0x1000>;
+			reg-names = "prcmu", "prcmu-tcpm", "prcmu-tcdm";
 			interrupts = <0 47 0x4>;
 			#address-cells = <1>;
 			#size-cells = <1>;
diff --git a/arch/arm/boot/dts/imx28-m28evk.dts b/arch/arm/boot/dts/imx28-m28evk.dts
index 6ce3d17c3a2..fd36e1cca10 100644
--- a/arch/arm/boot/dts/imx28-m28evk.dts
+++ b/arch/arm/boot/dts/imx28-m28evk.dts
@@ -152,7 +152,6 @@
 			i2c0: i2c@80058000 {
 				pinctrl-names = "default";
 				pinctrl-0 = <&i2c0_pins_a>;
-				clock-frequency = <400000>;
 				status = "okay";
 
 				sgtl5000: codec@0a {
diff --git a/arch/arm/boot/dts/imx28-sps1.dts b/arch/arm/boot/dts/imx28-sps1.dts
index e6cde8aa7ff..6c6a5442800 100644
--- a/arch/arm/boot/dts/imx28-sps1.dts
+++ b/arch/arm/boot/dts/imx28-sps1.dts
@@ -70,7 +70,6 @@
 			i2c0: i2c@80058000 {
 				pinctrl-names = "default";
 				pinctrl-0 = <&i2c0_pins_a>;
-				clock-frequency = <400000>;
 				status = "okay";
 
 				rtc: rtc@51 {
diff --git a/arch/arm/boot/dts/imx6qdl.dtsi b/arch/arm/boot/dts/imx6qdl.dtsi
index 06ec460b458..281a223591f 100644
--- a/arch/arm/boot/dts/imx6qdl.dtsi
+++ b/arch/arm/boot/dts/imx6qdl.dtsi
@@ -91,6 +91,7 @@
 			compatible = "arm,cortex-a9-twd-timer";
 			reg = <0x00a00600 0x20>;
 			interrupts = <1 13 0xf01>;
+			clocks = <&clks 15>;
 		};
 
 		L2: l2-cache@00a02000 {
diff --git a/arch/arm/boot/dts/kirkwood-goflexnet.dts b/arch/arm/boot/dts/kirkwood-goflexnet.dts
index bd83b8fc7c8..c3573be7b92 100644
--- a/arch/arm/boot/dts/kirkwood-goflexnet.dts
+++ b/arch/arm/boot/dts/kirkwood-goflexnet.dts
@@ -77,6 +77,7 @@
 		};
 
 		nand@3000000 {
+			chip-delay = <40>;
 			status = "okay";
 
 			partition@0 {
diff --git a/arch/arm/boot/dts/kirkwood-iomega_ix2_200.dts b/arch/arm/boot/dts/kirkwood-iomega_ix2_200.dts
index 93c3afbef9e..3694e94f6e9 100644
--- a/arch/arm/boot/dts/kirkwood-iomega_ix2_200.dts
+++ b/arch/arm/boot/dts/kirkwood-iomega_ix2_200.dts
@@ -96,11 +96,11 @@
 				marvell,function = "gpio";
 			};
 			pmx_led_rebuild_brt_ctrl_1: pmx-led-rebuild-brt-ctrl-1 {
-				marvell,pins = "mpp44";
+				marvell,pins = "mpp46";
 				marvell,function = "gpio";
 			};
 			pmx_led_rebuild_brt_ctrl_2: pmx-led-rebuild-brt-ctrl-2 {
-				marvell,pins = "mpp45";
+				marvell,pins = "mpp47";
 				marvell,function = "gpio";
 			};
 
@@ -157,14 +157,14 @@
 			gpios = <&gpio0 16 0>;
 			linux,default-trigger = "default-on";
 		};
-		health_led1 {
+		rebuild_led {
+			label = "status:white:rebuild_led";
+			gpios = <&gpio1 4 0>;
+		};
+		health_led {
 			label = "status:red:health_led";
 			gpios = <&gpio1 5 0>;
 		};
-		health_led2 {
-			label = "status:white:health_led";
-			gpios = <&gpio1 4 0>;
-		};
 		backup_led {
 			label = "status:blue:backup_led";
 			gpios = <&gpio0 15 0>;
diff --git a/arch/arm/boot/dts/msm8660-surf.dts b/arch/arm/boot/dts/msm8660-surf.dts
index 31f2157cd7d..67f8670c4d6 100644
--- a/arch/arm/boot/dts/msm8660-surf.dts
+++ b/arch/arm/boot/dts/msm8660-surf.dts
@@ -38,4 +38,10 @@
 		      <0x19c00000 0x1000>;
 		interrupts = <0 195 0x0>;
 	};
+
+	qcom,ssbi@500000 {
+		compatible = "qcom,ssbi";
+		reg = <0x500000 0x1000>;
+		qcom,controller-type = "pmic-arbiter";
+	};
 };
diff --git a/arch/arm/boot/dts/msm8960-cdp.dts b/arch/arm/boot/dts/msm8960-cdp.dts
index 9e621b5ad3d..c9b09a813a4 100644
--- a/arch/arm/boot/dts/msm8960-cdp.dts
+++ b/arch/arm/boot/dts/msm8960-cdp.dts
@@ -38,4 +38,10 @@
 		      <0x16400000 0x1000>;
 		interrupts = <0 154 0x0>;
 	};
+
+	qcom,ssbi@500000 {
+		compatible = "qcom,ssbi";
+		reg = <0x500000 0x1000>;
+		qcom,controller-type = "pmic-arbiter";
+	};
 };
diff --git a/arch/arm/boot/dts/orion5x.dtsi b/arch/arm/boot/dts/orion5x.dtsi
index 8aad00f81ed..f7bec3b1ba3 100644
--- a/arch/arm/boot/dts/orion5x.dtsi
+++ b/arch/arm/boot/dts/orion5x.dtsi
@@ -13,6 +13,9 @@
 	compatible = "marvell,orion5x";
 	interrupt-parent = <&intc>;
 
+	aliases {
+		gpio0 = &gpio0;
+	};
 	intc: interrupt-controller {
 		compatible = "marvell,orion-intc", "marvell,intc";
 		interrupt-controller;
@@ -32,7 +35,9 @@
 			#gpio-cells = <2>;
 			gpio-controller;
 			reg = <0x10100 0x40>;
-			ngpio = <32>;
+			ngpios = <32>;
+			interrupt-controller;
+			#interrupt-cells = <2>;
 			interrupts = <6>, <7>, <8>, <9>;
 		};
 
@@ -91,7 +96,7 @@
 			reg = <0x90000 0x10000>,
 			      <0xf2200000 0x800>;
 			reg-names = "regs", "sram";
-			interrupts = <22>;
+			interrupts = <28>;
 			status = "okay";
 		};
 	};
diff --git a/arch/arm/boot/dts/spear1310.dtsi b/arch/arm/boot/dts/spear1310.dtsi
index 1513c1927cc..122ae94076c 100644
--- a/arch/arm/boot/dts/spear1310.dtsi
+++ b/arch/arm/boot/dts/spear1310.dtsi
@@ -89,7 +89,7 @@
 		pinmux: pinmux@e0700000 {
 			compatible = "st,spear1310-pinmux";
 			reg = <0xe0700000 0x1000>;
-			#gpio-range-cells = <2>;
+			#gpio-range-cells = <3>;
 		};
 
 		apb {
@@ -212,7 +212,7 @@
 				interrupt-controller;
 				gpio-controller;
 				#gpio-cells = <2>;
-				gpio-ranges = <&pinmux 0 246>;
+				gpio-ranges = <&pinmux 0 0 246>;
 				status = "disabled";
 
 				st-plgpio,ngpio = <246>;
diff --git a/arch/arm/boot/dts/spear1340.dtsi b/arch/arm/boot/dts/spear1340.dtsi
index 34da11aa679..c511c4772ef 100644
--- a/arch/arm/boot/dts/spear1340.dtsi
+++ b/arch/arm/boot/dts/spear1340.dtsi
@@ -63,7 +63,7 @@
 		pinmux: pinmux@e0700000 {
 			compatible = "st,spear1340-pinmux";
 			reg = <0xe0700000 0x1000>;
-			#gpio-range-cells = <2>;
+			#gpio-range-cells = <3>;
 		};
 
 		pwm: pwm@e0180000 {
@@ -127,7 +127,7 @@
 				interrupt-controller;
 				gpio-controller;
 				#gpio-cells = <2>;
-				gpio-ranges = <&pinmux 0 252>;
+				gpio-ranges = <&pinmux 0 0 252>;
 				status = "disabled";
 
 				st-plgpio,ngpio = <250>;
diff --git a/arch/arm/boot/dts/spear310.dtsi b/arch/arm/boot/dts/spear310.dtsi
index ab45b8c8198..95372080eea 100644
--- a/arch/arm/boot/dts/spear310.dtsi
+++ b/arch/arm/boot/dts/spear310.dtsi
@@ -25,7 +25,7 @@
 		pinmux: pinmux@b4000000 {
 			compatible = "st,spear310-pinmux";
 			reg = <0xb4000000 0x1000>;
-			#gpio-range-cells = <2>;
+			#gpio-range-cells = <3>;
 		};
 
 		fsmc: flash@44000000 {
@@ -102,7 +102,7 @@
 				interrupt-controller;
 				gpio-controller;
 				#gpio-cells = <2>;
-				gpio-ranges = <&pinmux 0 102>;
+				gpio-ranges = <&pinmux 0 0 102>;
 				status = "disabled";
 
 				st-plgpio,ngpio = <102>;
diff --git a/arch/arm/boot/dts/spear320.dtsi b/arch/arm/boot/dts/spear320.dtsi
index caa5520b1fd..ffea342aeec 100644
--- a/arch/arm/boot/dts/spear320.dtsi
+++ b/arch/arm/boot/dts/spear320.dtsi
@@ -24,7 +24,7 @@
 		pinmux: pinmux@b3000000 {
 			compatible = "st,spear320-pinmux";
 			reg = <0xb3000000 0x1000>;
-			#gpio-range-cells = <2>;
+			#gpio-range-cells = <3>;
 		};
 
 		clcd@90000000 {
@@ -130,7 +130,7 @@
 				interrupt-controller;
 				gpio-controller;
 				#gpio-cells = <2>;
-				gpio-ranges = <&pinmux 0 102>;
+				gpio-ranges = <&pinmux 0 0 102>;
 				status = "disabled";
 
 				st-plgpio,ngpio = <102>;
diff --git a/arch/arm/boot/dts/tegra20.dtsi b/arch/arm/boot/dts/tegra20.dtsi
index 48d00a099ce..3d3f64d2111 100644
--- a/arch/arm/boot/dts/tegra20.dtsi
+++ b/arch/arm/boot/dts/tegra20.dtsi
@@ -385,7 +385,7 @@
 
 	spi@7000d800 {
 		compatible = "nvidia,tegra20-slink";
-		reg = <0x7000d480 0x200>;
+		reg = <0x7000d800 0x200>;
 		interrupts = <0 83 0x04>;
 		nvidia,dma-request-selector = <&apbdma 17>;
 		#address-cells = <1>;
diff --git a/arch/arm/boot/dts/tegra30.dtsi b/arch/arm/boot/dts/tegra30.dtsi
index 9d87a3ffe99..dbf46c27256 100644
--- a/arch/arm/boot/dts/tegra30.dtsi
+++ b/arch/arm/boot/dts/tegra30.dtsi
@@ -372,7 +372,7 @@
 
 	spi@7000d800 {
 		compatible = "nvidia,tegra30-slink", "nvidia,tegra20-slink";
-		reg = <0x7000d480 0x200>;
+		reg = <0x7000d800 0x200>;
 		interrupts = <0 83 0x04>;
 		nvidia,dma-request-selector = <&apbdma 17>;
 		#address-cells = <1>;
diff --git a/arch/arm/boot/dts/vt8500-bv07.dts b/arch/arm/boot/dts/vt8500-bv07.dts
index 567cf4e8ab8..877b33afa7e 100644
--- a/arch/arm/boot/dts/vt8500-bv07.dts
+++ b/arch/arm/boot/dts/vt8500-bv07.dts
@@ -11,26 +11,22 @@
 
 / {
 	model = "Benign BV07 Netbook";
+};
 
-	/*
-	 * Display node is based on Sascha Hauer's patch on dri-devel.
-	 * Added a bpp property to calculate the size of the framebuffer
-	 * until the binding is formalized.
-	 */
-	display: display@0 {
-		modes {
-			mode0: mode@0 {
-				hactive = <800>;
-				vactive = <480>;
-				hback-porch = <88>;
-				hfront-porch = <40>;
-				hsync-len = <0>;
-				vback-porch = <32>;
-				vfront-porch = <11>;
-				vsync-len = <1>;
-				clock = <0>;	/* unused but required */
-				bpp = <16>;	/* non-standard but required */
-			};
+&fb {
+	bits-per-pixel = <16>;
+	display-timings {
+		native-mode = <&timing0>;
+		timing0: 800x480 {
+			clock-frequency = <0>; /* unused but required */
+			hactive = <800>;
+			vactive = <480>;
+			hfront-porch = <40>;
+			hback-porch = <88>;
+			hsync-len = <0>;
+			vback-porch = <32>;
+			vfront-porch = <11>;
+			vsync-len = <1>;
 		};
 	};
 };
diff --git a/arch/arm/boot/dts/vt8500.dtsi b/arch/arm/boot/dts/vt8500.dtsi
index cf31ced4660..68c8dc64438 100644
--- a/arch/arm/boot/dts/vt8500.dtsi
+++ b/arch/arm/boot/dts/vt8500.dtsi
@@ -98,12 +98,10 @@
 			interrupts = <43>;
 		};
 
-		fb@d800e400 {
+		fb: fb@d8050800 {
 			compatible = "via,vt8500-fb";
 			reg = <0xd800e400 0x400>;
 			interrupts = <12>;
-			display = <&display>;
-			default-mode = <&mode0>;
 		};
 
 		ge_rops@d8050400 {
diff --git a/arch/arm/boot/dts/wm8505-ref.dts b/arch/arm/boot/dts/wm8505-ref.dts
index fd4e248074c..edd2cec3d37 100644
--- a/arch/arm/boot/dts/wm8505-ref.dts
+++ b/arch/arm/boot/dts/wm8505-ref.dts
@@ -11,26 +11,22 @@
 
 / {
 	model = "Wondermedia WM8505 Netbook";
+};
 
-	/*
-	 * Display node is based on Sascha Hauer's patch on dri-devel.
-	 * Added a bpp property to calculate the size of the framebuffer
-	 * until the binding is formalized.
-	 */
-	display: display@0 {
-		modes {
-			mode0: mode@0 {
-				hactive = <800>;
-				vactive = <480>;
-				hback-porch = <88>;
-				hfront-porch = <40>;
-				hsync-len = <0>;
-				vback-porch = <32>;
-				vfront-porch = <11>;
-				vsync-len = <1>;
-				clock = <0>;	/* unused but required */
-				bpp = <32>;	/* non-standard but required */
-			};
+&fb {
+	bits-per-pixel = <32>;
+	display-timings {
+		native-mode = <&timing0>;
+		timing0: 800x480 {
+			clock-frequency = <0>; /* unused but required */
+			hactive = <800>;
+			vactive = <480>;
+			hfront-porch = <40>;
+			hback-porch = <88>;
+			hsync-len = <0>;
+			vback-porch = <32>;
+			vfront-porch = <11>;
+			vsync-len = <1>;
 		};
 	};
 };
diff --git a/arch/arm/boot/dts/wm8505.dtsi b/arch/arm/boot/dts/wm8505.dtsi
index e74a1c0fb9a..bcf668d31b2 100644
--- a/arch/arm/boot/dts/wm8505.dtsi
+++ b/arch/arm/boot/dts/wm8505.dtsi
@@ -128,11 +128,9 @@
 			interrupts = <0>;
 		};
 
-		fb@d8050800 {
+		fb: fb@d8050800 {
 			compatible = "wm,wm8505-fb";
 			reg = <0xd8050800 0x200>;
-			display = <&display>;
-			default-mode = <&mode0>;
 		};
 
 		ge_rops@d8050400 {
diff --git a/arch/arm/boot/dts/wm8650-mid.dts b/arch/arm/boot/dts/wm8650-mid.dts
index cefd938f842..61671a0d9ed 100644
--- a/arch/arm/boot/dts/wm8650-mid.dts
+++ b/arch/arm/boot/dts/wm8650-mid.dts
@@ -11,26 +11,24 @@
 
 / {
 	model = "Wondermedia WM8650-MID Tablet";
+};
+
+&fb {
+	bits-per-pixel = <16>;
 
-	/*
-	 * Display node is based on Sascha Hauer's patch on dri-devel.
-	 * Added a bpp property to calculate the size of the framebuffer
-	 * until the binding is formalized.
-	 */
-	display: display@0 {
-		modes {
-			mode0: mode@0 {
-				hactive = <800>;
-				vactive = <480>;
-				hback-porch = <88>;
-				hfront-porch = <40>;
-				hsync-len = <0>;
-				vback-porch = <32>;
-				vfront-porch = <11>;
-				vsync-len = <1>;
-				clock = <0>;	/* unused but required */
-				bpp = <16>;	/* non-standard but required */
-			};
+	display-timings {
+		native-mode = <&timing0>;
+		timing0: 800x480 {
+			clock-frequency = <0>; /* unused but required */
+			hactive = <800>;
+			vactive = <480>;
+			hfront-porch = <40>;
+			hback-porch = <88>;
+			hsync-len = <0>;
+			vback-porch = <32>;
+			vfront-porch = <11>;
+			vsync-len = <1>;
 		};
 	};
 };
+
diff --git a/arch/arm/boot/dts/wm8650.dtsi b/arch/arm/boot/dts/wm8650.dtsi
index db3c0a12e05..9313407bbc3 100644
--- a/arch/arm/boot/dts/wm8650.dtsi
+++ b/arch/arm/boot/dts/wm8650.dtsi
@@ -128,11 +128,9 @@
 			interrupts = <43>;
 		};
 
-		fb@d8050800 {
+		fb: fb@d8050800 {
 			compatible = "wm,wm8505-fb";
 			reg = <0xd8050800 0x200>;
-			display = <&display>;
-			default-mode = <&mode0>;
 		};
 
 		ge_rops@d8050400 {
diff --git a/arch/arm/boot/dts/wm8850-w70v2.dts b/arch/arm/boot/dts/wm8850-w70v2.dts
index fcc660c8954..32d22532cd6 100644
--- a/arch/arm/boot/dts/wm8850-w70v2.dts
+++ b/arch/arm/boot/dts/wm8850-w70v2.dts
@@ -15,28 +15,6 @@
 / {
 	model = "Wondermedia WM8850-W70v2 Tablet";
 
-	/*
-	 * Display node is based on Sascha Hauer's patch on dri-devel.
-	 * Added a bpp property to calculate the size of the framebuffer
-	 * until the binding is formalized.
-	 */
-	display: display@0 {
-		modes {
-			mode0: mode@0 {
-				hactive = <800>;
-				vactive = <480>;
-				hback-porch = <88>;
-				hfront-porch = <40>;
-				hsync-len = <0>;
-				vback-porch = <32>;
-				vfront-porch = <11>;
-				vsync-len = <1>;
-				clock = <0>;	/* unused but required */
-				bpp = <16>;	/* non-standard but required */
-			};
-		};
-	};
-
 	backlight {
 		compatible = "pwm-backlight";
 		pwms = <&pwm 0 50000 1>;	/* duty inverted */
@@ -45,3 +23,21 @@
 		default-brightness-level = <5>;
 	};
 };
+
+&fb {
+	bits-per-pixel = <16>;
+	display-timings {
+		native-mode = <&timing0>;
+		timing0: 800x480 {
+			clock-frequency = <0>; /* unused but required */
+			hactive = <800>;
+			vactive = <480>;
+			hfront-porch = <40>;
+			hback-porch = <88>;
+			hsync-len = <0>;
+			vback-porch = <32>;
+			vfront-porch = <11>;
+			vsync-len = <1>;
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/wm8850.dtsi b/arch/arm/boot/dts/wm8850.dtsi
index e8cbfdc87bb..7149cd13e3b 100644
--- a/arch/arm/boot/dts/wm8850.dtsi
+++ b/arch/arm/boot/dts/wm8850.dtsi
@@ -135,11 +135,9 @@
 			};
 		};
 
-		fb@d8051700 {
+		fb: fb@d8051700 {
 			compatible = "wm,wm8505-fb";
 			reg = <0xd8051700 0x200>;
-			display = <&display>;
-			default-mode = <&mode0>;
 		};
 
 		ge_rops@d8050400 {
diff --git a/arch/arm/configs/imx_v6_v7_defconfig b/arch/arm/configs/imx_v6_v7_defconfig
index e36b0102532..088d6c11a0f 100644
--- a/arch/arm/configs/imx_v6_v7_defconfig
+++ b/arch/arm/configs/imx_v6_v7_defconfig
@@ -188,6 +188,7 @@ CONFIG_USB_EHCI_HCD=y
 CONFIG_USB_EHCI_MXC=y
 CONFIG_USB_CHIPIDEA=y
 CONFIG_USB_CHIPIDEA_HOST=y
+CONFIG_USB_PHY=y
 CONFIG_USB_MXS_PHY=y
 CONFIG_USB_STORAGE=y
 CONFIG_MMC=y
diff --git a/arch/arm/configs/lpc32xx_defconfig b/arch/arm/configs/lpc32xx_defconfig
index 92386b20bd0..afa7249fac6 100644
--- a/arch/arm/configs/lpc32xx_defconfig
+++ b/arch/arm/configs/lpc32xx_defconfig
@@ -134,6 +134,7 @@ CONFIG_SND_DEBUG_VERBOSE=y
 # CONFIG_SND_SPI is not set
 CONFIG_SND_SOC=y
 CONFIG_USB=y
+CONFIG_USB_PHY=y
 CONFIG_USB_OHCI_HCD=y
 CONFIG_USB_STORAGE=y
 CONFIG_USB_GADGET=y
diff --git a/arch/arm/configs/mxs_defconfig b/arch/arm/configs/mxs_defconfig
index 6a99e30f81d..87924d67111 100644
--- a/arch/arm/configs/mxs_defconfig
+++ b/arch/arm/configs/mxs_defconfig
@@ -120,6 +120,7 @@ CONFIG_USB_EHCI_HCD=y
 CONFIG_USB_CHIPIDEA=y
 CONFIG_USB_CHIPIDEA_HOST=y
 CONFIG_USB_STORAGE=y
+CONFIG_USB_PHY=y
 CONFIG_USB_MXS_PHY=y
 CONFIG_MMC=y
 CONFIG_MMC_MXS=y
diff --git a/arch/arm/configs/omap1_defconfig b/arch/arm/configs/omap1_defconfig
index 42eab9a2a0f..7e0ebb64a7f 100644
--- a/arch/arm/configs/omap1_defconfig
+++ b/arch/arm/configs/omap1_defconfig
@@ -195,6 +195,7 @@ CONFIG_SND_SOC=y
 CONFIG_SND_OMAP_SOC=y
 # CONFIG_USB_HID is not set
 CONFIG_USB=y
+CONFIG_USB_PHY=y
 CONFIG_USB_DEBUG=y
 CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
diff --git a/arch/arm/include/asm/delay.h b/arch/arm/include/asm/delay.h
index 720799fd3a8..dff714d886d 100644
--- a/arch/arm/include/asm/delay.h
+++ b/arch/arm/include/asm/delay.h
@@ -24,7 +24,7 @@ extern struct arm_delay_ops {
 	void (*delay)(unsigned long);
 	void (*const_udelay)(unsigned long);
 	void (*udelay)(unsigned long);
-	bool const_clock;
+	unsigned long ticks_per_jiffy;
 } arm_delay_ops;
 
 #define __delay(n)		arm_delay_ops.delay(n)
diff --git a/arch/arm/include/asm/glue-cache.h b/arch/arm/include/asm/glue-cache.h
index cca9f15704e..ea289e1435e 100644
--- a/arch/arm/include/asm/glue-cache.h
+++ b/arch/arm/include/asm/glue-cache.h
@@ -19,14 +19,6 @@
 #undef _CACHE
 #undef MULTI_CACHE
 
-#if defined(CONFIG_CPU_CACHE_V3)
-# ifdef _CACHE
-#  define MULTI_CACHE 1
-# else
-#  define _CACHE v3
-# endif
-#endif
-
 #if defined(CONFIG_CPU_CACHE_V4)
 # ifdef _CACHE
 #  define MULTI_CACHE 1
diff --git a/arch/arm/include/asm/hardware/iop3xx.h b/arch/arm/include/asm/hardware/iop3xx.h
index 02fe2fbe247..ed94b1a366a 100644
--- a/arch/arm/include/asm/hardware/iop3xx.h
+++ b/arch/arm/include/asm/hardware/iop3xx.h
@@ -37,7 +37,7 @@ extern int iop3xx_get_init_atu(void);
  * IOP3XX processor registers
  */
 #define IOP3XX_PERIPHERAL_PHYS_BASE	0xffffe000
-#define IOP3XX_PERIPHERAL_VIRT_BASE	0xfeffe000
+#define IOP3XX_PERIPHERAL_VIRT_BASE	0xfedfe000
 #define IOP3XX_PERIPHERAL_SIZE		0x00002000
 #define IOP3XX_PERIPHERAL_UPPER_PA (IOP3XX_PERIPHERAL_PHYS_BASE +\
 					IOP3XX_PERIPHERAL_SIZE - 1)
diff --git a/arch/arm/include/asm/highmem.h b/arch/arm/include/asm/highmem.h
index 8c5e828f484..91b99abe7a9 100644
--- a/arch/arm/include/asm/highmem.h
+++ b/arch/arm/include/asm/highmem.h
@@ -41,6 +41,13 @@ extern void kunmap_high(struct page *page);
 #endif
 #endif
 
+/*
+ * Needed to be able to broadcast the TLB invalidation for kmap.
+ */
+#ifdef CONFIG_ARM_ERRATA_798181
+#undef ARCH_NEEDS_KMAP_HIGH_GET
+#endif
+
 #ifdef ARCH_NEEDS_KMAP_HIGH_GET
 extern void *kmap_high_get(struct page *page);
 #else
diff --git a/arch/arm/include/asm/mmu_context.h b/arch/arm/include/asm/mmu_context.h
index 863a6611323..a7b85e0d0cc 100644
--- a/arch/arm/include/asm/mmu_context.h
+++ b/arch/arm/include/asm/mmu_context.h
@@ -27,6 +27,8 @@ void __check_vmalloc_seq(struct mm_struct *mm);
 void check_and_switch_context(struct mm_struct *mm, struct task_struct *tsk);
 #define init_new_context(tsk,mm)	({ atomic64_set(&mm->context.id, 0); 0; })
 
+DECLARE_PER_CPU(atomic64_t, active_asids);
+
 #else	/* !CONFIG_CPU_HAS_ASID */
 
 #ifdef CONFIG_MMU
diff --git a/arch/arm/include/asm/pgtable-3level.h b/arch/arm/include/asm/pgtable-3level.h
index 6ef8afd1b64..86b8fe398b9 100644
--- a/arch/arm/include/asm/pgtable-3level.h
+++ b/arch/arm/include/asm/pgtable-3level.h
@@ -111,7 +111,7 @@
 #define L_PTE_S2_MT_WRITETHROUGH (_AT(pteval_t, 0xa) << 2) /* MemAttr[3:0] */
 #define L_PTE_S2_MT_WRITEBACK	 (_AT(pteval_t, 0xf) << 2) /* MemAttr[3:0] */
 #define L_PTE_S2_RDONLY		 (_AT(pteval_t, 1) << 6)   /* HAP[1]   */
-#define L_PTE_S2_RDWR		 (_AT(pteval_t, 2) << 6)   /* HAP[2:1] */
+#define L_PTE_S2_RDWR		 (_AT(pteval_t, 3) << 6)   /* HAP[2:1] */
 
 /*
  * Hyp-mode PL2 PTE definitions for LPAE.
diff --git a/arch/arm/include/asm/pgtable.h b/arch/arm/include/asm/pgtable.h
index 80d6fc4dbe4..9bcd262a900 100644
--- a/arch/arm/include/asm/pgtable.h
+++ b/arch/arm/include/asm/pgtable.h
@@ -61,6 +61,15 @@ extern void __pgd_error(const char *file, int line, pgd_t);
 #define FIRST_USER_ADDRESS	PAGE_SIZE
 
 /*
+ * Use TASK_SIZE as the ceiling argument for free_pgtables() and
+ * free_pgd_range() to avoid freeing the modules pmd when LPAE is enabled (pmd
+ * page shared between user and kernel).
+ */
+#ifdef CONFIG_ARM_LPAE
+#define USER_PGTABLES_CEILING	TASK_SIZE
+#endif
+
+/*
  * The pgprot_* and protection_map entries will be fixed up in runtime
  * to include the cachable and bufferable bits based on memory policy,
  * as well as any architecture dependent bits like global/ASID and SMP
diff --git a/arch/arm/include/asm/system_misc.h b/arch/arm/include/asm/system_misc.h
index 5a85f148b60..21a23e378bb 100644
--- a/arch/arm/include/asm/system_misc.h
+++ b/arch/arm/include/asm/system_misc.h
@@ -21,9 +21,6 @@ extern void (*arm_pm_idle)(void);
 
 extern unsigned int user_debug;
 
-extern void disable_hlt(void);
-extern void enable_hlt(void);
-
 #endif /* !__ASSEMBLY__ */
 
 #endif /* __ASM_ARM_SYSTEM_MISC_H */
diff --git a/arch/arm/include/asm/tlbflush.h b/arch/arm/include/asm/tlbflush.h
index 4db8c8820f0..ab865e65a84 100644
--- a/arch/arm/include/asm/tlbflush.h
+++ b/arch/arm/include/asm/tlbflush.h
@@ -14,7 +14,6 @@
 
 #include <asm/glue.h>
 
-#define TLB_V3_PAGE	(1 << 0)
 #define TLB_V4_U_PAGE	(1 << 1)
 #define TLB_V4_D_PAGE	(1 << 2)
 #define TLB_V4_I_PAGE	(1 << 3)
@@ -22,7 +21,6 @@
 #define TLB_V6_D_PAGE	(1 << 5)
 #define TLB_V6_I_PAGE	(1 << 6)
 
-#define TLB_V3_FULL	(1 << 8)
 #define TLB_V4_U_FULL	(1 << 9)
 #define TLB_V4_D_FULL	(1 << 10)
 #define TLB_V4_I_FULL	(1 << 11)
@@ -52,7 +50,6 @@
  *	=============
  *
  *	We have the following to choose from:
- *	  v3    - ARMv3
  *	  v4    - ARMv4 without write buffer
  *	  v4wb  - ARMv4 with write buffer without I TLB flush entry instruction
  *	  v4wbi - ARMv4 with write buffer with I TLB flush entry instruction
@@ -330,7 +327,6 @@ static inline void local_flush_tlb_all(void)
 	if (tlb_flag(TLB_WB))
 		dsb();
 
-	tlb_op(TLB_V3_FULL, "c6, c0, 0", zero);
 	tlb_op(TLB_V4_U_FULL | TLB_V6_U_FULL, "c8, c7, 0", zero);
 	tlb_op(TLB_V4_D_FULL | TLB_V6_D_FULL, "c8, c6, 0", zero);
 	tlb_op(TLB_V4_I_FULL | TLB_V6_I_FULL, "c8, c5, 0", zero);
@@ -351,9 +347,8 @@ static inline void local_flush_tlb_mm(struct mm_struct *mm)
 	if (tlb_flag(TLB_WB))
 		dsb();
 
-	if (possible_tlb_flags & (TLB_V3_FULL|TLB_V4_U_FULL|TLB_V4_D_FULL|TLB_V4_I_FULL)) {
+	if (possible_tlb_flags & (TLB_V4_U_FULL|TLB_V4_D_FULL|TLB_V4_I_FULL)) {
 		if (cpumask_test_cpu(get_cpu(), mm_cpumask(mm))) {
-			tlb_op(TLB_V3_FULL, "c6, c0, 0", zero);
 			tlb_op(TLB_V4_U_FULL, "c8, c7, 0", zero);
 			tlb_op(TLB_V4_D_FULL, "c8, c6, 0", zero);
 			tlb_op(TLB_V4_I_FULL, "c8, c5, 0", zero);
@@ -385,9 +380,8 @@ local_flush_tlb_page(struct vm_area_struct *vma, unsigned long uaddr)
 	if (tlb_flag(TLB_WB))
 		dsb();
 
-	if (possible_tlb_flags & (TLB_V3_PAGE|TLB_V4_U_PAGE|TLB_V4_D_PAGE|TLB_V4_I_PAGE|TLB_V4_I_FULL) &&
+	if (possible_tlb_flags & (TLB_V4_U_PAGE|TLB_V4_D_PAGE|TLB_V4_I_PAGE|TLB_V4_I_FULL) &&
 	    cpumask_test_cpu(smp_processor_id(), mm_cpumask(vma->vm_mm))) {
-		tlb_op(TLB_V3_PAGE, "c6, c0, 0", uaddr);
 		tlb_op(TLB_V4_U_PAGE, "c8, c7, 1", uaddr);
 		tlb_op(TLB_V4_D_PAGE, "c8, c6, 1", uaddr);
 		tlb_op(TLB_V4_I_PAGE, "c8, c5, 1", uaddr);
@@ -418,7 +412,6 @@ static inline void local_flush_tlb_kernel_page(unsigned long kaddr)
 	if (tlb_flag(TLB_WB))
 		dsb();
 
-	tlb_op(TLB_V3_PAGE, "c6, c0, 0", kaddr);
 	tlb_op(TLB_V4_U_PAGE, "c8, c7, 1", kaddr);
 	tlb_op(TLB_V4_D_PAGE, "c8, c6, 1", kaddr);
 	tlb_op(TLB_V4_I_PAGE, "c8, c5, 1", kaddr);
@@ -450,6 +443,21 @@ static inline void local_flush_bp_all(void)
 		isb();
 }
 
+#ifdef CONFIG_ARM_ERRATA_798181
+static inline void dummy_flush_tlb_a15_erratum(void)
+{
+	/*
+	 * Dummy TLBIMVAIS. Using the unmapped address 0 and ASID 0.
+	 */
+	asm("mcr p15, 0, %0, c8, c3, 1" : : "r" (0));
+	dsb();
+}
+#else
+static inline void dummy_flush_tlb_a15_erratum(void)
+{
+}
+#endif
+
 /*
  *	flush_pmd_entry
  *
diff --git a/arch/arm/kernel/early_printk.c b/arch/arm/kernel/early_printk.c
index 85aa2b29269..43076536965 100644
--- a/arch/arm/kernel/early_printk.c
+++ b/arch/arm/kernel/early_printk.c
@@ -29,28 +29,17 @@ static void early_console_write(struct console *con, const char *s, unsigned n)
 	early_write(s, n);
 }
 
-static struct console early_console = {
+static struct console early_console_dev = {
 	.name =		"earlycon",
 	.write =	early_console_write,
 	.flags =	CON_PRINTBUFFER | CON_BOOT,
 	.index =	-1,
 };
 
-asmlinkage void early_printk(const char *fmt, ...)
-{
-	char buf[512];
-	int n;
-	va_list ap;
-
-	va_start(ap, fmt);
-	n = vscnprintf(buf, sizeof(buf), fmt, ap);
-	early_write(buf, n);
-	va_end(ap);
-}
-
 static int __init setup_early_printk(char *buf)
 {
-	register_console(&early_console);
+	early_console = &early_console_dev;
+	register_console(&early_console_dev);
 	return 0;
 }
 
diff --git a/arch/arm/kernel/entry-common.S b/arch/arm/kernel/entry-common.S
index 3248cde504e..fefd7f97143 100644
--- a/arch/arm/kernel/entry-common.S
+++ b/arch/arm/kernel/entry-common.S
@@ -276,7 +276,13 @@ ENDPROC(ftrace_graph_caller_old)
  */
 
 .macro mcount_enter
+/*
+ * This pad compensates for the push {lr} at the call site.  Note that we are
+ * unable to unwind through a function which does not otherwise save its lr.
+ */
+ UNWIND(.pad	#4)
 	stmdb	sp!, {r0-r3, lr}
+ UNWIND(.save	{r0-r3, lr})
 .endm
 
 .macro mcount_get_lr reg
@@ -289,6 +295,7 @@ ENDPROC(ftrace_graph_caller_old)
 .endm
 
 ENTRY(__gnu_mcount_nc)
+UNWIND(.fnstart)
 #ifdef CONFIG_DYNAMIC_FTRACE
 	mov	ip, lr
 	ldmia	sp!, {lr}
@@ -296,17 +303,22 @@ ENTRY(__gnu_mcount_nc)
 #else
 	__mcount
 #endif
+UNWIND(.fnend)
 ENDPROC(__gnu_mcount_nc)
 
 #ifdef CONFIG_DYNAMIC_FTRACE
 ENTRY(ftrace_caller)
+UNWIND(.fnstart)
 	__ftrace_caller
+UNWIND(.fnend)
 ENDPROC(ftrace_caller)
 #endif
 
 #ifdef CONFIG_FUNCTION_GRAPH_TRACER
 ENTRY(ftrace_graph_caller)
+UNWIND(.fnstart)
 	__ftrace_graph_caller
+UNWIND(.fnend)
 ENDPROC(ftrace_graph_caller)
 #endif
 
diff --git a/arch/arm/kernel/head.S b/arch/arm/kernel/head.S
index e0eb9a1cae7..8bac553fe21 100644
--- a/arch/arm/kernel/head.S
+++ b/arch/arm/kernel/head.S
@@ -267,7 +267,7 @@ __create_page_tables:
 	addne	r6, r6, #1 << SECTION_SHIFT
 	strne	r6, [r3]
 
-#if defined(CONFIG_LPAE) && defined(CONFIG_CPU_ENDIAN_BE8)
+#if defined(CONFIG_ARM_LPAE) && defined(CONFIG_CPU_ENDIAN_BE8)
 	sub	r4, r4, #4			@ Fixup page table pointer
 						@ for 64-bit descriptors
 #endif
diff --git a/arch/arm/kernel/hw_breakpoint.c b/arch/arm/kernel/hw_breakpoint.c
index 96093b75ab9..1fd749ee4a1 100644
--- a/arch/arm/kernel/hw_breakpoint.c
+++ b/arch/arm/kernel/hw_breakpoint.c
@@ -966,7 +966,7 @@ static void reset_ctrl_regs(void *unused)
 	}
 
 	if (err) {
-		pr_warning("CPU %d debug is powered down!\n", cpu);
+		pr_warn_once("CPU %d debug is powered down!\n", cpu);
 		cpumask_or(&debug_err_mask, &debug_err_mask, cpumask_of(cpu));
 		return;
 	}
@@ -987,7 +987,7 @@ clear_vcr:
 	isb();
 
 	if (cpumask_intersects(&debug_err_mask, cpumask_of(cpu))) {
-		pr_warning("CPU %d failed to disable vector catch\n", cpu);
+		pr_warn_once("CPU %d failed to disable vector catch\n", cpu);
 		return;
 	}
 
@@ -1007,7 +1007,7 @@ clear_vcr:
 	}
 
 	if (cpumask_intersects(&debug_err_mask, cpumask_of(cpu))) {
-		pr_warning("CPU %d failed to clear debug register pairs\n", cpu);
+		pr_warn_once("CPU %d failed to clear debug register pairs\n", cpu);
 		return;
 	}
 
@@ -1043,7 +1043,7 @@ static int dbg_cpu_pm_notify(struct notifier_block *self, unsigned long action,
 	return NOTIFY_OK;
 }
 
-static struct notifier_block __cpuinitdata dbg_cpu_pm_nb = {
+static struct notifier_block dbg_cpu_pm_nb = {
 	.notifier_call = dbg_cpu_pm_notify,
 };
 
diff --git a/arch/arm/kernel/perf_event.c b/arch/arm/kernel/perf_event.c
index 146157dfe27..8c3094d0f7b 100644
--- a/arch/arm/kernel/perf_event.c
+++ b/arch/arm/kernel/perf_event.c
@@ -253,7 +253,10 @@ validate_event(struct pmu_hw_events *hw_events,
 	struct arm_pmu *armpmu = to_arm_pmu(event->pmu);
 	struct pmu *leader_pmu = event->group_leader->pmu;
 
-	if (event->pmu != leader_pmu || event->state <= PERF_EVENT_STATE_OFF)
+	if (event->pmu != leader_pmu || event->state < PERF_EVENT_STATE_OFF)
+		return 1;
+
+	if (event->state == PERF_EVENT_STATE_OFF && !event->attr.enable_on_exec)
 		return 1;
 
 	return armpmu->get_event_idx(hw_events, event) >= 0;
diff --git a/arch/arm/kernel/process.c b/arch/arm/kernel/process.c
index 047d3e40e47..c9a5e2ce8aa 100644
--- a/arch/arm/kernel/process.c
+++ b/arch/arm/kernel/process.c
@@ -57,38 +57,6 @@ static const char *isa_modes[] = {
   "ARM" , "Thumb" , "Jazelle", "ThumbEE"
 };
 
-static volatile int hlt_counter;
-
-void disable_hlt(void)
-{
-	hlt_counter++;
-}
-
-EXPORT_SYMBOL(disable_hlt);
-
-void enable_hlt(void)
-{
-	hlt_counter--;
-	BUG_ON(hlt_counter < 0);
-}
-
-EXPORT_SYMBOL(enable_hlt);
-
-static int __init nohlt_setup(char *__unused)
-{
-	hlt_counter = 1;
-	return 1;
-}
-
-static int __init hlt_setup(char *__unused)
-{
-	hlt_counter = 0;
-	return 1;
-}
-
-__setup("nohlt", nohlt_setup);
-__setup("hlt", hlt_setup);
-
 extern void call_with_stack(void (*fn)(void *), void *arg, void *sp);
 typedef void (*phys_reset_t)(unsigned long);
 
@@ -172,54 +140,38 @@ static void default_idle(void)
 	local_irq_enable();
 }
 
-/*
- * The idle thread.
- * We always respect 'hlt_counter' to prevent low power idle.
- */
-void cpu_idle(void)
+void arch_cpu_idle_prepare(void)
 {
 	local_fiq_enable();
+}
 
-	/* endless idle loop with no priority at all */
-	while (1) {
-		tick_nohz_idle_enter();
-		rcu_idle_enter();
-		ledtrig_cpu(CPU_LED_IDLE_START);
-		while (!need_resched()) {
-#ifdef CONFIG_HOTPLUG_CPU
-			if (cpu_is_offline(smp_processor_id()))
-				cpu_die();
+void arch_cpu_idle_enter(void)
+{
+	ledtrig_cpu(CPU_LED_IDLE_START);
+#ifdef CONFIG_PL310_ERRATA_769419
+	wmb();
 #endif
+}
 
-			/*
-			 * We need to disable interrupts here
-			 * to ensure we don't miss a wakeup call.
-			 */
-			local_irq_disable();
-#ifdef CONFIG_PL310_ERRATA_769419
-			wmb();
+void arch_cpu_idle_exit(void)
+{
+	ledtrig_cpu(CPU_LED_IDLE_END);
+}
+
+#ifdef CONFIG_HOTPLUG_CPU
+void arch_cpu_idle_dead(void)
+{
+	cpu_die();
+}
 #endif
-			if (hlt_counter) {
-				local_irq_enable();
-				cpu_relax();
-			} else if (!need_resched()) {
-				stop_critical_timings();
-				if (cpuidle_idle_call())
-					default_idle();
-				start_critical_timings();
-				/*
-				 * default_idle functions must always
-				 * return with IRQs enabled.
-				 */
-				WARN_ON(irqs_disabled());
-			} else
-				local_irq_enable();
-		}
-		ledtrig_cpu(CPU_LED_IDLE_END);
-		rcu_idle_exit();
-		tick_nohz_idle_exit();
-		schedule_preempt_disabled();
-	}
+
+/*
+ * Called from the core idle loop.
+ */
+void arch_cpu_idle(void)
+{
+	if (cpuidle_idle_call())
+		default_idle();
 }
 
 static char reboot_mode = 'h';
diff --git a/arch/arm/kernel/sched_clock.c b/arch/arm/kernel/sched_clock.c
index bd6f56b9ec2..59d2adb764a 100644
--- a/arch/arm/kernel/sched_clock.c
+++ b/arch/arm/kernel/sched_clock.c
@@ -45,12 +45,12 @@ static u32 notrace jiffy_sched_clock_read(void)
 
 static u32 __read_mostly (*read_sched_clock)(void) = jiffy_sched_clock_read;
 
-static inline u64 cyc_to_ns(u64 cyc, u32 mult, u32 shift)
+static inline u64 notrace cyc_to_ns(u64 cyc, u32 mult, u32 shift)
 {
 	return (cyc * mult) >> shift;
 }
 
-static unsigned long long cyc_to_sched_clock(u32 cyc, u32 mask)
+static unsigned long long notrace cyc_to_sched_clock(u32 cyc, u32 mask)
 {
 	u64 epoch_ns;
 	u32 epoch_cyc;
diff --git a/arch/arm/kernel/setup.c b/arch/arm/kernel/setup.c
index 3f6cbb2e3ed..234e339196c 100644
--- a/arch/arm/kernel/setup.c
+++ b/arch/arm/kernel/setup.c
@@ -56,7 +56,6 @@
 #include <asm/virt.h>
 
 #include "atags.h"
-#include "tcm.h"
 
 
 #if defined(CONFIG_FPE_NWFPE) || defined(CONFIG_FPE_FASTFPE)
@@ -353,6 +352,23 @@ void __init early_print(const char *str, ...)
 	printk("%s", buf);
 }
 
+static void __init cpuid_init_hwcaps(void)
+{
+	unsigned int divide_instrs;
+
+	if (cpu_architecture() < CPU_ARCH_ARMv7)
+		return;
+
+	divide_instrs = (read_cpuid_ext(CPUID_EXT_ISAR0) & 0x0f000000) >> 24;
+
+	switch (divide_instrs) {
+	case 2:
+		elf_hwcap |= HWCAP_IDIVA;
+	case 1:
+		elf_hwcap |= HWCAP_IDIVT;
+	}
+}
+
 static void __init feat_v6_fixup(void)
 {
 	int id = read_cpuid_id();
@@ -483,8 +499,11 @@ static void __init setup_processor(void)
 	snprintf(elf_platform, ELF_PLATFORM_SIZE, "%s%c",
 		 list->elf_name, ENDIANNESS);
 	elf_hwcap = list->elf_hwcap;
+
+	cpuid_init_hwcaps();
+
 #ifndef CONFIG_ARM_THUMB
-	elf_hwcap &= ~HWCAP_THUMB;
+	elf_hwcap &= ~(HWCAP_THUMB | HWCAP_IDIVT);
 #endif
 
 	feat_v6_fixup();
@@ -524,7 +543,7 @@ int __init arm_add_memory(phys_addr_t start, phys_addr_t size)
 	size -= start & ~PAGE_MASK;
 	bank->start = PAGE_ALIGN(start);
 
-#ifndef CONFIG_LPAE
+#ifndef CONFIG_ARM_LPAE
 	if (bank->start + size < bank->start) {
 		printk(KERN_CRIT "Truncating memory at 0x%08llx to fit in "
 			"32-bit physical address space\n", (long long)start);
@@ -778,8 +797,6 @@ void __init setup_arch(char **cmdline_p)
 
 	reserve_crashkernel();
 
-	tcm_init();
-
 #ifdef CONFIG_MULTI_IRQ_HANDLER
 	handle_arch_irq = mdesc->handle_irq;
 #endif
diff --git a/arch/arm/kernel/smp.c b/arch/arm/kernel/smp.c
index 79078edbb9b..4619177bcfe 100644
--- a/arch/arm/kernel/smp.c
+++ b/arch/arm/kernel/smp.c
@@ -336,7 +336,7 @@ asmlinkage void __cpuinit secondary_start_kernel(void)
 	/*
 	 * OK, it's off to the idle thread for us
 	 */
-	cpu_idle();
+	cpu_startup_entry(CPUHP_ONLINE);
 }
 
 void __init smp_cpus_done(unsigned int max_cpus)
@@ -673,9 +673,6 @@ static int cpufreq_callback(struct notifier_block *nb,
 	if (freq->flags & CPUFREQ_CONST_LOOPS)
 		return NOTIFY_OK;
 
-	if (arm_delay_ops.const_clock)
-		return NOTIFY_OK;
-
 	if (!per_cpu(l_p_j_ref, cpu)) {
 		per_cpu(l_p_j_ref, cpu) =
 			per_cpu(cpu_data, cpu).loops_per_jiffy;
diff --git a/arch/arm/kernel/smp_tlb.c b/arch/arm/kernel/smp_tlb.c
index bd030053139..e82e1d24877 100644
--- a/arch/arm/kernel/smp_tlb.c
+++ b/arch/arm/kernel/smp_tlb.c
@@ -12,6 +12,7 @@
 
 #include <asm/smp_plat.h>
 #include <asm/tlbflush.h>
+#include <asm/mmu_context.h>
 
 /**********************************************************************/
 
@@ -69,12 +70,72 @@ static inline void ipi_flush_bp_all(void *ignored)
 	local_flush_bp_all();
 }
 
+#ifdef CONFIG_ARM_ERRATA_798181
+static int erratum_a15_798181(void)
+{
+	unsigned int midr = read_cpuid_id();
+
+	/* Cortex-A15 r0p0..r3p2 affected */
+	if ((midr & 0xff0ffff0) != 0x410fc0f0 || midr > 0x413fc0f2)
+		return 0;
+	return 1;
+}
+#else
+static int erratum_a15_798181(void)
+{
+	return 0;
+}
+#endif
+
+static void ipi_flush_tlb_a15_erratum(void *arg)
+{
+	dmb();
+}
+
+static void broadcast_tlb_a15_erratum(void)
+{
+	if (!erratum_a15_798181())
+		return;
+
+	dummy_flush_tlb_a15_erratum();
+	smp_call_function_many(cpu_online_mask, ipi_flush_tlb_a15_erratum,
+			       NULL, 1);
+}
+
+static void broadcast_tlb_mm_a15_erratum(struct mm_struct *mm)
+{
+	int cpu;
+	cpumask_t mask = { CPU_BITS_NONE };
+
+	if (!erratum_a15_798181())
+		return;
+
+	dummy_flush_tlb_a15_erratum();
+	for_each_online_cpu(cpu) {
+		if (cpu == smp_processor_id())
+			continue;
+		/*
+		 * We only need to send an IPI if the other CPUs are running
+		 * the same ASID as the one being invalidated. There is no
+		 * need for locking around the active_asids check since the
+		 * switch_mm() function has at least one dmb() (as required by
+		 * this workaround) in case a context switch happens on
+		 * another CPU after the condition below.
+		 */
+		if (atomic64_read(&mm->context.id) ==
+		    atomic64_read(&per_cpu(active_asids, cpu)))
+			cpumask_set_cpu(cpu, &mask);
+	}
+	smp_call_function_many(&mask, ipi_flush_tlb_a15_erratum, NULL, 1);
+}
+
 void flush_tlb_all(void)
 {
 	if (tlb_ops_need_broadcast())
 		on_each_cpu(ipi_flush_tlb_all, NULL, 1);
 	else
 		local_flush_tlb_all();
+	broadcast_tlb_a15_erratum();
 }
 
 void flush_tlb_mm(struct mm_struct *mm)
@@ -83,6 +144,7 @@ void flush_tlb_mm(struct mm_struct *mm)
 		on_each_cpu_mask(mm_cpumask(mm), ipi_flush_tlb_mm, mm, 1);
 	else
 		local_flush_tlb_mm(mm);
+	broadcast_tlb_mm_a15_erratum(mm);
 }
 
 void flush_tlb_page(struct vm_area_struct *vma, unsigned long uaddr)
@@ -95,6 +157,7 @@ void flush_tlb_page(struct vm_area_struct *vma, unsigned long uaddr)
 					&ta, 1);
 	} else
 		local_flush_tlb_page(vma, uaddr);
+	broadcast_tlb_mm_a15_erratum(vma->vm_mm);
 }
 
 void flush_tlb_kernel_page(unsigned long kaddr)
@@ -105,6 +168,7 @@ void flush_tlb_kernel_page(unsigned long kaddr)
 		on_each_cpu(ipi_flush_tlb_kernel_page, &ta, 1);
 	} else
 		local_flush_tlb_kernel_page(kaddr);
+	broadcast_tlb_a15_erratum();
 }
 
 void flush_tlb_range(struct vm_area_struct *vma,
@@ -119,6 +183,7 @@ void flush_tlb_range(struct vm_area_struct *vma,
 					&ta, 1);
 	} else
 		local_flush_tlb_range(vma, start, end);
+	broadcast_tlb_mm_a15_erratum(vma->vm_mm);
 }
 
 void flush_tlb_kernel_range(unsigned long start, unsigned long end)
@@ -130,6 +195,7 @@ void flush_tlb_kernel_range(unsigned long start, unsigned long end)
 		on_each_cpu(ipi_flush_tlb_kernel_range, &ta, 1);
 	} else
 		local_flush_tlb_kernel_range(start, end);
+	broadcast_tlb_a15_erratum();
 }
 
 void flush_bp_all(void)
diff --git a/arch/arm/kernel/tcm.c b/arch/arm/kernel/tcm.c
index 30ae6bb4a31..f50f19e5c13 100644
--- a/arch/arm/kernel/tcm.c
+++ b/arch/arm/kernel/tcm.c
@@ -17,7 +17,6 @@
 #include <asm/mach/map.h>
 #include <asm/memory.h>
 #include <asm/system_info.h>
-#include "tcm.h"
 
 static struct gen_pool *tcm_pool;
 static bool dtcm_present;
diff --git a/arch/arm/kernel/topology.c b/arch/arm/kernel/topology.c
index 79282ebcd93..f10316b4ecd 100644
--- a/arch/arm/kernel/topology.c
+++ b/arch/arm/kernel/topology.c
@@ -100,7 +100,7 @@ static void __init parse_dt_topology(void)
 	int alloc_size, cpu = 0;
 
 	alloc_size = nr_cpu_ids * sizeof(struct cpu_capacity);
-	cpu_capacity = (struct cpu_capacity *)kzalloc(alloc_size, GFP_NOWAIT);
+	cpu_capacity = kzalloc(alloc_size, GFP_NOWAIT);
 
 	while ((cn = of_find_node_by_type(cn, "cpu"))) {
 		const u32 *rate, *reg;
diff --git a/arch/arm/kvm/arm.c b/arch/arm/kvm/arm.c
index 5a936988eb2..842098d78f5 100644
--- a/arch/arm/kvm/arm.c
+++ b/arch/arm/kvm/arm.c
@@ -201,6 +201,7 @@ int kvm_dev_ioctl_check_extension(long ext)
 		break;
 	case KVM_CAP_ARM_SET_DEVICE_ADDR:
 		r = 1;
+		break;
 	case KVM_CAP_NR_VCPUS:
 		r = num_online_cpus();
 		break;
@@ -613,7 +614,7 @@ static int handle_exit(struct kvm_vcpu *vcpu, struct kvm_run *run,
 
 		if (hsr_ec >= ARRAY_SIZE(arm_exit_handlers)
 		    || !arm_exit_handlers[hsr_ec]) {
-			kvm_err("Unkown exception class: %#08lx, "
+			kvm_err("Unknown exception class: %#08lx, "
 				"hsr: %#08x\n", hsr_ec,
 				(unsigned int)vcpu->arch.hsr);
 			BUG();
diff --git a/arch/arm/kvm/coproc.c b/arch/arm/kvm/coproc.c
index 4ea9a982269..7bed7556077 100644
--- a/arch/arm/kvm/coproc.c
+++ b/arch/arm/kvm/coproc.c
@@ -79,11 +79,11 @@ static bool access_dcsw(struct kvm_vcpu *vcpu,
 	u32 val;
 	int cpu;
 
-	cpu = get_cpu();
-
 	if (!p->is_write)
 		return read_from_write_only(vcpu, p);
 
+	cpu = get_cpu();
+
 	cpumask_setall(&vcpu->arch.require_dcache_flush);
 	cpumask_clear_cpu(cpu, &vcpu->arch.require_dcache_flush);
 
diff --git a/arch/arm/kvm/vgic.c b/arch/arm/kvm/vgic.c
index c9a17316e9f..0e4cfe123b3 100644
--- a/arch/arm/kvm/vgic.c
+++ b/arch/arm/kvm/vgic.c
@@ -883,8 +883,7 @@ static bool vgic_queue_irq(struct kvm_vcpu *vcpu, u8 sgi_source_id, int irq)
 			  lr, irq, vgic_cpu->vgic_lr[lr]);
 		BUG_ON(!test_bit(lr, vgic_cpu->lr_used));
 		vgic_cpu->vgic_lr[lr] |= GICH_LR_PENDING_BIT;
-
-		goto out;
+		return true;
 	}
 
 	/* Try to use another LR for this interrupt */
@@ -898,7 +897,6 @@ static bool vgic_queue_irq(struct kvm_vcpu *vcpu, u8 sgi_source_id, int irq)
 	vgic_cpu->vgic_irq_lr_map[irq] = lr;
 	set_bit(lr, vgic_cpu->lr_used);
 
-out:
 	if (!vgic_irq_is_edge(vcpu, irq))
 		vgic_cpu->vgic_lr[lr] |= GICH_LR_EOI;
 
@@ -1018,21 +1016,6 @@ static bool vgic_process_maintenance(struct kvm_vcpu *vcpu)
 
 	kvm_debug("MISR = %08x\n", vgic_cpu->vgic_misr);
 
-	/*
-	 * We do not need to take the distributor lock here, since the only
-	 * action we perform is clearing the irq_active_bit for an EOIed
-	 * level interrupt.  There is a potential race with
-	 * the queuing of an interrupt in __kvm_vgic_flush_hwstate(), where we
-	 * check if the interrupt is already active. Two possibilities:
-	 *
-	 * - The queuing is occurring on the same vcpu: cannot happen,
-	 *   as we're already in the context of this vcpu, and
-	 *   executing the handler
-	 * - The interrupt has been migrated to another vcpu, and we
-	 *   ignore this interrupt for this run. Big deal. It is still
-	 *   pending though, and will get considered when this vcpu
-	 *   exits.
-	 */
 	if (vgic_cpu->vgic_misr & GICH_MISR_EOI) {
 		/*
 		 * Some level interrupts have been EOIed. Clear their
@@ -1054,6 +1037,13 @@ static bool vgic_process_maintenance(struct kvm_vcpu *vcpu)
 			} else {
 				vgic_cpu_irq_clear(vcpu, irq);
 			}
+
+			/*
+			 * Despite being EOIed, the LR may not have
+			 * been marked as empty.
+			 */
+			set_bit(lr, (unsigned long *)vgic_cpu->vgic_elrsr);
+			vgic_cpu->vgic_lr[lr] &= ~GICH_LR_ACTIVE_BIT;
 		}
 	}
 
@@ -1064,9 +1054,8 @@ static bool vgic_process_maintenance(struct kvm_vcpu *vcpu)
 }
 
 /*
- * Sync back the VGIC state after a guest run. We do not really touch
- * the distributor here (the irq_pending_on_cpu bit is safe to set),
- * so there is no need for taking its lock.
+ * Sync back the VGIC state after a guest run. The distributor lock is
+ * needed so we don't get preempted in the middle of the state processing.
  */
 static void __kvm_vgic_sync_hwstate(struct kvm_vcpu *vcpu)
 {
@@ -1112,10 +1101,14 @@ void kvm_vgic_flush_hwstate(struct kvm_vcpu *vcpu)
 
 void kvm_vgic_sync_hwstate(struct kvm_vcpu *vcpu)
 {
+	struct vgic_dist *dist = &vcpu->kvm->arch.vgic;
+
 	if (!irqchip_in_kernel(vcpu->kvm))
 		return;
 
+	spin_lock(&dist->lock);
 	__kvm_vgic_sync_hwstate(vcpu);
+	spin_unlock(&dist->lock);
 }
 
 int kvm_vgic_vcpu_pending_irq(struct kvm_vcpu *vcpu)
diff --git a/arch/arm/lib/delay.c b/arch/arm/lib/delay.c
index 6b93f6a1a3c..64dbfa57204 100644
--- a/arch/arm/lib/delay.c
+++ b/arch/arm/lib/delay.c
@@ -58,7 +58,7 @@ static void __timer_delay(unsigned long cycles)
 static void __timer_const_udelay(unsigned long xloops)
 {
 	unsigned long long loops = xloops;
-	loops *= loops_per_jiffy;
+	loops *= arm_delay_ops.ticks_per_jiffy;
 	__timer_delay(loops >> UDELAY_SHIFT);
 }
 
@@ -73,11 +73,13 @@ void __init register_current_timer_delay(const struct delay_timer *timer)
 		pr_info("Switching to timer-based delay loop\n");
 		delay_timer			= timer;
 		lpj_fine			= timer->freq / HZ;
-		loops_per_jiffy			= lpj_fine;
+
+		/* cpufreq may scale loops_per_jiffy, so keep a private copy */
+		arm_delay_ops.ticks_per_jiffy	= lpj_fine;
 		arm_delay_ops.delay		= __timer_delay;
 		arm_delay_ops.const_udelay	= __timer_const_udelay;
 		arm_delay_ops.udelay		= __timer_udelay;
-		arm_delay_ops.const_clock	= true;
+
 		delay_calibrated		= true;
 	} else {
 		pr_info("Ignoring duplicate/late registration of read_current_timer delay\n");
diff --git a/arch/arm/mach-at91/at91sam9260.c b/arch/arm/mach-at91/at91sam9260.c
index b67cd537411..44199bc2c66 100644
--- a/arch/arm/mach-at91/at91sam9260.c
+++ b/arch/arm/mach-at91/at91sam9260.c
@@ -232,6 +232,8 @@ static struct clk_lookup periph_clocks_lookups[] = {
 	CLKDEV_CON_DEV_ID("t2_clk", "fffdc000.timer", &tc5_clk),
 	CLKDEV_CON_DEV_ID("hclk", "500000.ohci", &ohci_clk),
 	CLKDEV_CON_DEV_ID("mci_clk", "fffa8000.mmc", &mmc_clk),
+	CLKDEV_CON_DEV_ID("spi_clk", "fffc8000.spi", &spi0_clk),
+	CLKDEV_CON_DEV_ID("spi_clk", "fffcc000.spi", &spi1_clk),
 	/* fake hclk clock */
 	CLKDEV_CON_DEV_ID("hclk", "at91_ohci", &ohci_clk),
 	CLKDEV_CON_ID("pioA", &pioA_clk),
diff --git a/arch/arm/mach-at91/at91sam9g45.c b/arch/arm/mach-at91/at91sam9g45.c
index d3addee43d8..2ec5efea3f0 100644
--- a/arch/arm/mach-at91/at91sam9g45.c
+++ b/arch/arm/mach-at91/at91sam9g45.c
@@ -262,6 +262,8 @@ static struct clk_lookup periph_clocks_lookups[] = {
 	CLKDEV_CON_DEV_ID("mci_clk", "fffd0000.mmc", &mmc1_clk),
 	CLKDEV_CON_DEV_ID(NULL, "fff84000.i2c", &twi0_clk),
 	CLKDEV_CON_DEV_ID(NULL, "fff88000.i2c", &twi1_clk),
+	CLKDEV_CON_DEV_ID("spi_clk", "fffa4000.spi", &spi0_clk),
+	CLKDEV_CON_DEV_ID("spi_clk", "fffa8000.spi", &spi1_clk),
 	/* fake hclk clock */
 	CLKDEV_CON_DEV_ID("hclk", "at91_ohci", &uhphs_clk),
 	CLKDEV_CON_DEV_ID(NULL, "fffff200.gpio", &pioA_clk),
diff --git a/arch/arm/mach-at91/at91sam9n12.c b/arch/arm/mach-at91/at91sam9n12.c
index 5dfc8fd8710..ccd078355ee 100644
--- a/arch/arm/mach-at91/at91sam9n12.c
+++ b/arch/arm/mach-at91/at91sam9n12.c
@@ -172,6 +172,8 @@ static struct clk_lookup periph_clocks_lookups[] = {
 	CLKDEV_CON_DEV_ID("dma_clk", "ffffec00.dma-controller", &dma_clk),
 	CLKDEV_CON_DEV_ID(NULL, "f8010000.i2c", &twi0_clk),
 	CLKDEV_CON_DEV_ID(NULL, "f8014000.i2c", &twi1_clk),
+	CLKDEV_CON_DEV_ID("spi_clk", "f0000000.spi", &spi0_clk),
+	CLKDEV_CON_DEV_ID("spi_clk", "f0004000.spi", &spi1_clk),
 	CLKDEV_CON_DEV_ID(NULL, "fffff400.gpio", &pioAB_clk),
 	CLKDEV_CON_DEV_ID(NULL, "fffff600.gpio", &pioAB_clk),
 	CLKDEV_CON_DEV_ID(NULL, "fffff800.gpio", &pioCD_clk),
diff --git a/arch/arm/mach-at91/at91sam9x5.c b/arch/arm/mach-at91/at91sam9x5.c
index 44a9a62dcc1..a200d8a1712 100644
--- a/arch/arm/mach-at91/at91sam9x5.c
+++ b/arch/arm/mach-at91/at91sam9x5.c
@@ -237,6 +237,8 @@ static struct clk_lookup periph_clocks_lookups[] = {
 	CLKDEV_CON_DEV_ID(NULL, "f8010000.i2c", &twi0_clk),
 	CLKDEV_CON_DEV_ID(NULL, "f8014000.i2c", &twi1_clk),
 	CLKDEV_CON_DEV_ID(NULL, "f8018000.i2c", &twi2_clk),
+	CLKDEV_CON_DEV_ID("spi_clk", "f0000000.spi", &spi0_clk),
+	CLKDEV_CON_DEV_ID("spi_clk", "f0004000.spi", &spi1_clk),
 	CLKDEV_CON_DEV_ID(NULL, "fffff400.gpio", &pioAB_clk),
 	CLKDEV_CON_DEV_ID(NULL, "fffff600.gpio", &pioAB_clk),
 	CLKDEV_CON_DEV_ID(NULL, "fffff800.gpio", &pioCD_clk),
diff --git a/arch/arm/mach-bcm/Kconfig b/arch/arm/mach-bcm/Kconfig
index bf02471d7e7..f11289519c3 100644
--- a/arch/arm/mach-bcm/Kconfig
+++ b/arch/arm/mach-bcm/Kconfig
@@ -6,6 +6,7 @@ config ARCH_BCM
 	select ARM_ERRATA_764369 if SMP
 	select ARM_GIC
 	select CPU_V7
+	select CLKSRC_OF
 	select GENERIC_CLOCKEVENTS
 	select GENERIC_TIME
 	select GPIO_BCM
diff --git a/arch/arm/mach-bcm/board_bcm.c b/arch/arm/mach-bcm/board_bcm.c
index f0f9abafad2..25959354047 100644
--- a/arch/arm/mach-bcm/board_bcm.c
+++ b/arch/arm/mach-bcm/board_bcm.c
@@ -16,14 +16,11 @@
 #include <linux/device.h>
 #include <linux/platform_device.h>
 #include <linux/irqchip.h>
+#include <linux/clocksource.h>
 
 #include <asm/mach/arch.h>
 #include <asm/mach/time.h>
 
-static void timer_init(void)
-{
-}
-
 
 static void __init board_init(void)
 {
@@ -35,7 +32,7 @@ static const char * const bcm11351_dt_compat[] = { "bcm,bcm11351", NULL, };
 
 DT_MACHINE_START(BCM11351_DT, "Broadcom Application Processor")
 	.init_irq = irqchip_init,
-	.init_time = timer_init,
+	.init_time = clocksource_of_init,
 	.init_machine = board_init,
 	.dt_compat = bcm11351_dt_compat,
 MACHINE_END
diff --git a/arch/arm/mach-cns3xxx/core.c b/arch/arm/mach-cns3xxx/core.c
index e698f26cc0c..52e4bb5cf12 100644
--- a/arch/arm/mach-cns3xxx/core.c
+++ b/arch/arm/mach-cns3xxx/core.c
@@ -22,19 +22,9 @@
 
 static struct map_desc cns3xxx_io_desc[] __initdata = {
 	{
-		.virtual	= CNS3XXX_TC11MP_TWD_BASE_VIRT,
-		.pfn		= __phys_to_pfn(CNS3XXX_TC11MP_TWD_BASE),
-		.length		= SZ_4K,
-		.type		= MT_DEVICE,
-	}, {
-		.virtual	= CNS3XXX_TC11MP_GIC_CPU_BASE_VIRT,
-		.pfn		= __phys_to_pfn(CNS3XXX_TC11MP_GIC_CPU_BASE),
-		.length		= SZ_4K,
-		.type		= MT_DEVICE,
-	}, {
-		.virtual	= CNS3XXX_TC11MP_GIC_DIST_BASE_VIRT,
-		.pfn		= __phys_to_pfn(CNS3XXX_TC11MP_GIC_DIST_BASE),
-		.length		= SZ_4K,
+		.virtual	= CNS3XXX_TC11MP_SCU_BASE_VIRT,
+		.pfn		= __phys_to_pfn(CNS3XXX_TC11MP_SCU_BASE),
+		.length		= SZ_8K,
 		.type		= MT_DEVICE,
 	}, {
 		.virtual	= CNS3XXX_TIMER1_2_3_BASE_VIRT,
diff --git a/arch/arm/mach-cns3xxx/include/mach/cns3xxx.h b/arch/arm/mach-cns3xxx/include/mach/cns3xxx.h
index 191c8e57f28..b1021aafa48 100644
--- a/arch/arm/mach-cns3xxx/include/mach/cns3xxx.h
+++ b/arch/arm/mach-cns3xxx/include/mach/cns3xxx.h
@@ -94,10 +94,10 @@
 #define RTC_INTR_STS_OFFSET			0x34
 
 #define CNS3XXX_MISC_BASE			0x76000000	/* Misc Control */
-#define CNS3XXX_MISC_BASE_VIRT			0xFFF07000	/* Misc Control */
+#define CNS3XXX_MISC_BASE_VIRT			0xFB000000	/* Misc Control */
 
 #define CNS3XXX_PM_BASE				0x77000000	/* Power Management Control */
-#define CNS3XXX_PM_BASE_VIRT			0xFFF08000
+#define CNS3XXX_PM_BASE_VIRT			0xFB001000
 
 #define PM_CLK_GATE_OFFSET			0x00
 #define PM_SOFT_RST_OFFSET			0x04
@@ -109,7 +109,7 @@
 #define PM_PLL_HM_PD_OFFSET			0x1C
 
 #define CNS3XXX_UART0_BASE			0x78000000	/* UART 0 */
-#define CNS3XXX_UART0_BASE_VIRT			0xFFF09000
+#define CNS3XXX_UART0_BASE_VIRT			0xFB002000
 
 #define CNS3XXX_UART1_BASE			0x78400000	/* UART 1 */
 #define CNS3XXX_UART1_BASE_VIRT			0xFFF0A000
@@ -130,7 +130,7 @@
 #define CNS3XXX_I2S_BASE_VIRT			0xFFF10000
 
 #define CNS3XXX_TIMER1_2_3_BASE			0x7C800000	/* Timer */
-#define CNS3XXX_TIMER1_2_3_BASE_VIRT		0xFFF10800
+#define CNS3XXX_TIMER1_2_3_BASE_VIRT		0xFB003000
 
 #define TIMER1_COUNTER_OFFSET			0x00
 #define TIMER1_AUTO_RELOAD_OFFSET		0x04
@@ -227,16 +227,16 @@
  * Testchip peripheral and fpga gic regions
  */
 #define CNS3XXX_TC11MP_SCU_BASE			0x90000000	/* IRQ, Test chip */
-#define CNS3XXX_TC11MP_SCU_BASE_VIRT		0xFF000000
+#define CNS3XXX_TC11MP_SCU_BASE_VIRT		0xFB004000
 
 #define CNS3XXX_TC11MP_GIC_CPU_BASE		0x90000100	/* Test chip interrupt controller CPU interface */
-#define CNS3XXX_TC11MP_GIC_CPU_BASE_VIRT	0xFF000100
+#define CNS3XXX_TC11MP_GIC_CPU_BASE_VIRT	(CNS3XXX_TC11MP_SCU_BASE_VIRT + 0x100)
 
 #define CNS3XXX_TC11MP_TWD_BASE			0x90000600
-#define CNS3XXX_TC11MP_TWD_BASE_VIRT		0xFF000600
+#define CNS3XXX_TC11MP_TWD_BASE_VIRT		(CNS3XXX_TC11MP_SCU_BASE_VIRT + 0x600)
 
 #define CNS3XXX_TC11MP_GIC_DIST_BASE		0x90001000	/* Test chip interrupt controller distributor */
-#define CNS3XXX_TC11MP_GIC_DIST_BASE_VIRT	0xFF001000
+#define CNS3XXX_TC11MP_GIC_DIST_BASE_VIRT	(CNS3XXX_TC11MP_SCU_BASE_VIRT + 0x1000)
 
 #define CNS3XXX_TC11MP_L220_BASE		0x92002000	/* L220 registers */
 #define CNS3XXX_TC11MP_L220_BASE_VIRT		0xFF002000
diff --git a/arch/arm/mach-davinci/board-dm355-evm.c b/arch/arm/mach-davinci/board-dm355-evm.c
index 147b8e1a440..886481c1217 100644
--- a/arch/arm/mach-davinci/board-dm355-evm.c
+++ b/arch/arm/mach-davinci/board-dm355-evm.c
@@ -242,6 +242,73 @@ static struct vpfe_config vpfe_cfg = {
 	.ccdc = "DM355 CCDC",
 };
 
+/* venc standards timings */
+static struct vpbe_enc_mode_info dm355evm_enc_preset_timing[] = {
+	{
+		.name		= "ntsc",
+		.timings_type	= VPBE_ENC_STD,
+		.std_id		= V4L2_STD_NTSC,
+		.interlaced	= 1,
+		.xres		= 720,
+		.yres		= 480,
+		.aspect		= {11, 10},
+		.fps		= {30000, 1001},
+		.left_margin	= 0x79,
+		.upper_margin	= 0x10,
+	},
+	{
+		.name		= "pal",
+		.timings_type	= VPBE_ENC_STD,
+		.std_id		= V4L2_STD_PAL,
+		.interlaced	= 1,
+		.xres		= 720,
+		.yres		= 576,
+		.aspect		= {54, 59},
+		.fps		= {25, 1},
+		.left_margin	= 0x7E,
+		.upper_margin	= 0x16
+	},
+};
+
+#define VENC_STD_ALL	(V4L2_STD_NTSC | V4L2_STD_PAL)
+
+/*
+ * The outputs available from VPBE + ecnoders. Keep the
+ * the order same as that of encoders. First those from venc followed by that
+ * from encoders. Index in the output refers to index on a particular encoder.
+ * Driver uses this index to pass it to encoder when it supports more than
+ * one output. Application uses index of the array to set an output.
+ */
+static struct vpbe_output dm355evm_vpbe_outputs[] = {
+	{
+		.output		= {
+			.index		= 0,
+			.name		= "Composite",
+			.type		= V4L2_OUTPUT_TYPE_ANALOG,
+			.std		= VENC_STD_ALL,
+			.capabilities	= V4L2_OUT_CAP_STD,
+		},
+		.subdev_name	= DM355_VPBE_VENC_SUBDEV_NAME,
+		.default_mode	= "ntsc",
+		.num_modes	= ARRAY_SIZE(dm355evm_enc_preset_timing),
+		.modes		= dm355evm_enc_preset_timing,
+		.if_params	= V4L2_MBUS_FMT_FIXED,
+	},
+};
+
+static struct vpbe_config dm355evm_display_cfg = {
+	.module_name	= "dm355-vpbe-display",
+	.i2c_adapter_id	= 1,
+	.osd		= {
+		.module_name	= DM355_VPBE_OSD_SUBDEV_NAME,
+	},
+	.venc		= {
+		.module_name	= DM355_VPBE_VENC_SUBDEV_NAME,
+	},
+	.num_outputs	= ARRAY_SIZE(dm355evm_vpbe_outputs),
+	.outputs	= dm355evm_vpbe_outputs,
+};
+
 static struct platform_device *davinci_evm_devices[] __initdata = {
 	&dm355evm_dm9000,
 	&davinci_nand_device,
@@ -253,8 +320,6 @@ static struct davinci_uart_config uart_config __initdata = {
 
 static void __init dm355_evm_map_io(void)
 {
-	/* setup input configuration for VPFE input devices */
-	dm355_set_vpfe_config(&vpfe_cfg);
 	dm355_init();
 }
 
@@ -344,6 +409,8 @@ static __init void dm355_evm_init(void)
 	davinci_setup_mmc(0, &dm355evm_mmc_config);
 	davinci_setup_mmc(1, &dm355evm_mmc_config);
 
+	dm355_init_video(&vpfe_cfg, &dm355evm_display_cfg);
+
 	dm355_init_spi0(BIT(0), dm355_evm_spi_info,
 			ARRAY_SIZE(dm355_evm_spi_info));
 
diff --git a/arch/arm/mach-davinci/board-dm365-evm.c b/arch/arm/mach-davinci/board-dm365-evm.c
index c2d4958a0cb..2a667435658 100644
--- a/arch/arm/mach-davinci/board-dm365-evm.c
+++ b/arch/arm/mach-davinci/board-dm365-evm.c
@@ -27,6 +27,7 @@
 #include <linux/input.h>
 #include <linux/spi/spi.h>
 #include <linux/spi/eeprom.h>
+#include <linux/v4l2-dv-timings.h>
 
 #include <asm/mach-types.h>
 #include <asm/mach/arch.h>
@@ -39,6 +40,7 @@
 #include <linux/platform_data/mtd-davinci.h>
 #include <linux/platform_data/keyscan-davinci.h>
 
+#include <media/ths7303.h>
 #include <media/tvp514x.h>
 
 #include "davinci.h"
@@ -374,6 +376,166 @@ static struct vpfe_config vpfe_cfg = {
 	.ccdc = "ISIF",
 };
 
+/* venc standards timings */
+static struct vpbe_enc_mode_info dm365evm_enc_std_timing[] = {
+	{
+		.name		= "ntsc",
+		.timings_type	= VPBE_ENC_STD,
+		.std_id		= V4L2_STD_NTSC,
+		.interlaced	= 1,
+		.xres		= 720,
+		.yres		= 480,
+		.aspect		= {11, 10},
+		.fps		= {30000, 1001},
+		.left_margin	= 0x79,
+		.upper_margin	= 0x10,
+	},
+	{
+		.name		= "pal",
+		.timings_type	= VPBE_ENC_STD,
+		.std_id		= V4L2_STD_PAL,
+		.interlaced	= 1,
+		.xres		= 720,
+		.yres		= 576,
+		.aspect		= {54, 59},
+		.fps		= {25, 1},
+		.left_margin	= 0x7E,
+		.upper_margin	= 0x16,
+	},
+};
+
+/* venc dv timings */
+static struct vpbe_enc_mode_info dm365evm_enc_preset_timing[] = {
+	{
+		.name		= "480p59_94",
+		.timings_type	= VPBE_ENC_DV_TIMINGS,
+		.dv_timings	= V4L2_DV_BT_CEA_720X480P59_94,
+		.interlaced	= 0,
+		.xres		= 720,
+		.yres		= 480,
+		.aspect		= {1, 1},
+		.fps		= {5994, 100},
+		.left_margin	= 0x8F,
+		.upper_margin	= 0x2D,
+	},
+	{
+		.name		= "576p50",
+		.timings_type	= VPBE_ENC_DV_TIMINGS,
+		.dv_timings	= V4L2_DV_BT_CEA_720X576P50,
+		.interlaced	= 0,
+		.xres		= 720,
+		.yres		= 576,
+		.aspect		= {1, 1},
+		.fps		= {50, 1},
+		.left_margin	= 0x8C,
+		.upper_margin   = 0x36,
+	},
+	{
+		.name		= "720p60",
+		.timings_type	= VPBE_ENC_DV_TIMINGS,
+		.dv_timings	= V4L2_DV_BT_CEA_1280X720P60,
+		.interlaced	= 0,
+		.xres		= 1280,
+		.yres		= 720,
+		.aspect		= {1, 1},
+		.fps		= {60, 1},
+		.left_margin	= 0x117,
+		.right_margin	= 70,
+		.upper_margin	= 38,
+		.lower_margin	= 3,
+		.hsync_len	= 80,
+		.vsync_len	= 5,
+	},
+	{
+		.name		= "1080i60",
+		.timings_type	= VPBE_ENC_DV_TIMINGS,
+		.dv_timings	= V4L2_DV_BT_CEA_1920X1080I60,
+		.interlaced	= 1,
+		.xres		= 1920,
+		.yres		= 1080,
+		.aspect		= {1, 1},
+		.fps		= {30, 1},
+		.left_margin	= 0xc9,
+		.right_margin	= 80,
+		.upper_margin	= 30,
+		.lower_margin	= 3,
+		.hsync_len	= 88,
+		.vsync_len	= 5,
+	},
+};
+
+#define VENC_STD_ALL	(V4L2_STD_NTSC | V4L2_STD_PAL)
+
+/*
+ * The outputs available from VPBE + ecnoders. Keep the
+ * the order same as that of encoders. First those from venc followed by that
+ * from encoders. Index in the output refers to index on a particular
+ * encoder.Driver uses this index to pass it to encoder when it supports more
+ * than one output. Application uses index of the array to set an output.
+ */
+static struct vpbe_output dm365evm_vpbe_outputs[] = {
+	{
+		.output		= {
+			.index		= 0,
+			.name		= "Composite",
+			.type		= V4L2_OUTPUT_TYPE_ANALOG,
+			.std		= VENC_STD_ALL,
+			.capabilities	= V4L2_OUT_CAP_STD,
+		},
+		.subdev_name	= DM365_VPBE_VENC_SUBDEV_NAME,
+		.default_mode	= "ntsc",
+		.num_modes	= ARRAY_SIZE(dm365evm_enc_std_timing),
+		.modes		= dm365evm_enc_std_timing,
+		.if_params	= V4L2_MBUS_FMT_FIXED,
+	},
+	{
+		.output		= {
+			.index		= 1,
+			.name		= "Component",
+			.type		= V4L2_OUTPUT_TYPE_ANALOG,
+			.capabilities	= V4L2_OUT_CAP_DV_TIMINGS,
+		},
+		.subdev_name	= DM365_VPBE_VENC_SUBDEV_NAME,
+		.default_mode	= "480p59_94",
+		.num_modes	= ARRAY_SIZE(dm365evm_enc_preset_timing),
+		.modes		= dm365evm_enc_preset_timing,
+		.if_params	= V4L2_MBUS_FMT_FIXED,
+	},
+};
+
+/*
+ * Amplifiers on the board
+ */
+struct ths7303_platform_data ths7303_pdata = {
+	.ch_1 = 3,
+	.ch_2 = 3,
+	.ch_3 = 3,
+	.init_enable = 1,
+};
+
+static struct amp_config_info vpbe_amp = {
+	.module_name	= "ths7303",
+	.is_i2c		= 1,
+	.board_info	= {
+		I2C_BOARD_INFO("ths7303", 0x2c),
+		.platform_data = &ths7303_pdata,
+	}
+};
+
+static struct vpbe_config dm365evm_display_cfg = {
+	.module_name	= "dm365-vpbe-display",
+	.i2c_adapter_id	= 1,
+	.amp		= &vpbe_amp,
+	.osd		= {
+		.module_name	= DM365_VPBE_OSD_SUBDEV_NAME,
+	},
+	.venc		= {
+		.module_name	= DM365_VPBE_VENC_SUBDEV_NAME,
+	},
+	.num_outputs	= ARRAY_SIZE(dm365evm_vpbe_outputs),
+	.outputs	= dm365evm_vpbe_outputs,
+};
+
 static void __init evm_init_i2c(void)
 {
 	davinci_init_i2c(&i2c_pdata);
@@ -564,8 +726,6 @@ static struct davinci_uart_config uart_config __initdata = {
 
 static void __init dm365_evm_map_io(void)
 {
-	/* setup input configuration for VPFE input devices */
-	dm365_set_vpfe_config(&vpfe_cfg);
 	dm365_init();
 }
 
@@ -597,6 +757,8 @@ static __init void dm365_evm_init(void)
 
 	davinci_setup_mmc(0, &dm365evm_mmc_config);
 
+	dm365_init_video(&vpfe_cfg, &dm365evm_display_cfg);
+
 	/* maybe setup mmc1/etc ... _after_ mmc0 */
 	evm_init_cpld();
 
diff --git a/arch/arm/mach-davinci/board-dm644x-evm.c b/arch/arm/mach-davinci/board-dm644x-evm.c
index 71735e7797c..745280d4144 100644
--- a/arch/arm/mach-davinci/board-dm644x-evm.c
+++ b/arch/arm/mach-davinci/board-dm644x-evm.c
@@ -622,7 +622,7 @@ static struct vpbe_enc_mode_info dm644xevm_enc_std_timing[] = {
 	{
 		.name		= "ntsc",
 		.timings_type	= VPBE_ENC_STD,
-		.std_id		= V4L2_STD_525_60,
+		.std_id		= V4L2_STD_NTSC,
 		.interlaced	= 1,
 		.xres		= 720,
 		.yres		= 480,
@@ -634,7 +634,7 @@ static struct vpbe_enc_mode_info dm644xevm_enc_std_timing[] = {
 	{
 		.name		= "pal",
 		.timings_type	= VPBE_ENC_STD,
-		.std_id		= V4L2_STD_625_50,
+		.std_id		= V4L2_STD_PAL,
 		.interlaced	= 1,
 		.xres		= 720,
 		.yres		= 576,
@@ -649,7 +649,7 @@ static struct vpbe_enc_mode_info dm644xevm_enc_std_timing[] = {
 static struct vpbe_enc_mode_info dm644xevm_enc_preset_timing[] = {
 	{
 		.name		= "480p59_94",
-		.timings_type	= VPBE_ENC_CUSTOM_TIMINGS,
+		.timings_type	= VPBE_ENC_DV_TIMINGS,
 		.dv_timings	= V4L2_DV_BT_CEA_720X480P59_94,
 		.interlaced	= 0,
 		.xres		= 720,
@@ -661,7 +661,7 @@ static struct vpbe_enc_mode_info dm644xevm_enc_preset_timing[] = {
 	},
 	{
 		.name		= "576p50",
-		.timings_type	= VPBE_ENC_CUSTOM_TIMINGS,
+		.timings_type	= VPBE_ENC_DV_TIMINGS,
 		.dv_timings	= V4L2_DV_BT_CEA_720X576P50,
 		.interlaced	= 0,
 		.xres		= 720,
diff --git a/arch/arm/mach-davinci/board-dm646x-evm.c b/arch/arm/mach-davinci/board-dm646x-evm.c
index de7adff324d..fc4871ac1c2 100644
--- a/arch/arm/mach-davinci/board-dm646x-evm.c
+++ b/arch/arm/mach-davinci/board-dm646x-evm.c
@@ -514,7 +514,7 @@ static const struct vpif_output dm6467_ch0_outputs[] = {
 			.index = 1,
 			.name = "Component",
 			.type = V4L2_OUTPUT_TYPE_ANALOG,
-			.capabilities = V4L2_OUT_CAP_CUSTOM_TIMINGS,
+			.capabilities = V4L2_OUT_CAP_DV_TIMINGS,
 		},
 		.subdev_name = "adv7343",
 		.output_route = ADV7343_COMPONENT_ID,
diff --git a/arch/arm/mach-davinci/davinci.h b/arch/arm/mach-davinci/davinci.h
index 12d544befcf..1ab3df423da 100644
--- a/arch/arm/mach-davinci/davinci.h
+++ b/arch/arm/mach-davinci/davinci.h
@@ -36,12 +36,19 @@
 #include <media/davinci/vpbe_osd.h>
 
 #define DAVINCI_SYSTEM_MODULE_BASE	0x01c40000
+#define SYSMOD_VDAC_CONFIG		0x2c
 #define SYSMOD_VIDCLKCTL		0x38
 #define SYSMOD_VPSS_CLKCTL		0x44
 #define SYSMOD_VDD3P3VPWDN		0x48
 #define SYSMOD_VSCLKDIS			0x6c
 #define SYSMOD_PUPDCTL1			0x7c
 
+/* VPSS CLKCTL bit definitions */
+#define VPSS_MUXSEL_EXTCLK_ENABLE	BIT(1)
+#define VPSS_VENCCLKEN_ENABLE		BIT(3)
+#define VPSS_DACCLKEN_ENABLE		BIT(4)
+#define VPSS_PLLC2SYSCLK5_ENABLE	BIT(5)
+
 extern void __iomem *davinci_sysmod_base;
 #define DAVINCI_SYSMOD_VIRT(x)	(davinci_sysmod_base + (x))
 void davinci_map_sysmod(void);
@@ -74,7 +81,7 @@ void __init dm355_init(void);
 void dm355_init_spi0(unsigned chipselect_mask,
 		const struct spi_board_info *info, unsigned len);
 void __init dm355_init_asp1(u32 evt_enable, struct snd_platform_data *pdata);
-void dm355_set_vpfe_config(struct vpfe_config *cfg);
+int dm355_init_video(struct vpfe_config *, struct vpbe_config *);
 
 /* DM365 function declarations */
 void __init dm365_init(void);
@@ -84,7 +91,7 @@ void __init dm365_init_ks(struct davinci_ks_platform_data *pdata);
 void __init dm365_init_rtc(void);
 void dm365_init_spi0(unsigned chipselect_mask,
 			const struct spi_board_info *info, unsigned len);
-void dm365_set_vpfe_config(struct vpfe_config *cfg);
+int dm365_init_video(struct vpfe_config *, struct vpbe_config *);
 
 /* DM644x function declarations */
 void __init dm644x_init(void);
diff --git a/arch/arm/mach-davinci/dm355.c b/arch/arm/mach-davinci/dm355.c
index b49c3b77d55..bf9a9d4ad9f 100644
--- a/arch/arm/mach-davinci/dm355.c
+++ b/arch/arm/mach-davinci/dm355.c
@@ -35,6 +35,8 @@
 #include "asp.h"
 
 #define DM355_UART2_BASE	(IO_PHYS + 0x206000)
+#define DM355_OSD_BASE		(IO_PHYS + 0x70200)
+#define DM355_VENC_BASE		(IO_PHYS + 0x70400)
 
 /*
  * Device specific clocks
@@ -345,8 +347,8 @@ static struct clk_lookup dm355_clks[] = {
 	CLK(NULL, "pll1_aux", &pll1_aux_clk),
 	CLK(NULL, "pll1_sysclkbp", &pll1_sysclkbp),
 	CLK(NULL, "vpss_dac", &vpss_dac_clk),
-	CLK(NULL, "vpss_master", &vpss_master_clk),
-	CLK(NULL, "vpss_slave", &vpss_slave_clk),
+	CLK("vpss", "master", &vpss_master_clk),
+	CLK("vpss", "slave", &vpss_slave_clk),
 	CLK(NULL, "clkout1", &clkout1_clk),
 	CLK(NULL, "clkout2", &clkout2_clk),
 	CLK(NULL, "pll2", &pll2_clk),
@@ -744,11 +746,146 @@ static struct platform_device vpfe_capture_dev = {
 	},
 };
 
-void dm355_set_vpfe_config(struct vpfe_config *cfg)
+static struct resource dm355_osd_resources[] = {
+	{
+		.start	= DM355_OSD_BASE,
+		.end	= DM355_OSD_BASE + 0x17f,
+		.flags	= IORESOURCE_MEM,
+	},
+};
+
+static struct platform_device dm355_osd_dev = {
+	.name		= DM355_VPBE_OSD_SUBDEV_NAME,
+	.id		= -1,
+	.num_resources	= ARRAY_SIZE(dm355_osd_resources),
+	.resource	= dm355_osd_resources,
+	.dev		= {
+		.dma_mask		= &vpfe_capture_dma_mask,
+		.coherent_dma_mask	= DMA_BIT_MASK(32),
+	},
+};
+
+static struct resource dm355_venc_resources[] = {
+	{
+		.start	= IRQ_VENCINT,
+		.end	= IRQ_VENCINT,
+		.flags	= IORESOURCE_IRQ,
+	},
+	/* venc registers io space */
+	{
+		.start	= DM355_VENC_BASE,
+		.end	= DM355_VENC_BASE + 0x17f,
+		.flags	= IORESOURCE_MEM,
+	},
+	/* VDAC config register io space */
+	{
+		.start	= DAVINCI_SYSTEM_MODULE_BASE + SYSMOD_VDAC_CONFIG,
+		.end	= DAVINCI_SYSTEM_MODULE_BASE + SYSMOD_VDAC_CONFIG + 3,
+		.flags	= IORESOURCE_MEM,
+	},
+};
+
+static struct resource dm355_v4l2_disp_resources[] = {
+	{
+		.start	= IRQ_VENCINT,
+		.end	= IRQ_VENCINT,
+		.flags	= IORESOURCE_IRQ,
+	},
+	/* venc registers io space */
+	{
+		.start	= DM355_VENC_BASE,
+		.end	= DM355_VENC_BASE + 0x17f,
+		.flags	= IORESOURCE_MEM,
+	},
+};
+
+static int dm355_vpbe_setup_pinmux(enum v4l2_mbus_pixelcode if_type,
+			    int field)
+{
+	switch (if_type) {
+	case V4L2_MBUS_FMT_SGRBG8_1X8:
+		davinci_cfg_reg(DM355_VOUT_FIELD_G70);
+		break;
+	case V4L2_MBUS_FMT_YUYV10_1X20:
+		if (field)
+			davinci_cfg_reg(DM355_VOUT_FIELD);
+		else
+			davinci_cfg_reg(DM355_VOUT_FIELD_G70);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	davinci_cfg_reg(DM355_VOUT_COUTL_EN);
+	davinci_cfg_reg(DM355_VOUT_COUTH_EN);
+
+	return 0;
+}
+
+static int dm355_venc_setup_clock(enum vpbe_enc_timings_type type,
+				   unsigned int pclock)
 {
-	vpfe_capture_dev.dev.platform_data = cfg;
+	void __iomem *vpss_clk_ctrl_reg;
+
+	vpss_clk_ctrl_reg = DAVINCI_SYSMOD_VIRT(SYSMOD_VPSS_CLKCTL);
+
+	switch (type) {
+	case VPBE_ENC_STD:
+		writel(VPSS_DACCLKEN_ENABLE | VPSS_VENCCLKEN_ENABLE,
+		       vpss_clk_ctrl_reg);
+		break;
+	case VPBE_ENC_DV_TIMINGS:
+		if (pclock > 27000000)
+			/*
+			 * For HD, use external clock source since we cannot
+			 * support HD mode with internal clocks.
+			 */
+			writel(VPSS_MUXSEL_EXTCLK_ENABLE, vpss_clk_ctrl_reg);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
 }
 
+static struct platform_device dm355_vpbe_display = {
+	.name		= "vpbe-v4l2",
+	.id		= -1,
+	.num_resources	= ARRAY_SIZE(dm355_v4l2_disp_resources),
+	.resource	= dm355_v4l2_disp_resources,
+	.dev		= {
+		.dma_mask		= &vpfe_capture_dma_mask,
+		.coherent_dma_mask	= DMA_BIT_MASK(32),
+	},
+};
+
+struct venc_platform_data dm355_venc_pdata = {
+	.setup_pinmux	= dm355_vpbe_setup_pinmux,
+	.setup_clock	= dm355_venc_setup_clock,
+};
+
+static struct platform_device dm355_venc_dev = {
+	.name		= DM355_VPBE_VENC_SUBDEV_NAME,
+	.id		= -1,
+	.num_resources	= ARRAY_SIZE(dm355_venc_resources),
+	.resource	= dm355_venc_resources,
+	.dev		= {
+		.dma_mask		= &vpfe_capture_dma_mask,
+		.coherent_dma_mask	= DMA_BIT_MASK(32),
+		.platform_data		= (void *)&dm355_venc_pdata,
+	},
+};
+
+static struct platform_device dm355_vpbe_dev = {
+	.name		= "vpbe_controller",
+	.id		= -1,
+	.dev		= {
+		.dma_mask		= &vpfe_capture_dma_mask,
+		.coherent_dma_mask	= DMA_BIT_MASK(32),
+	},
+};
+
 /*----------------------------------------------------------------------*/
 
 static struct map_desc dm355_io_desc[] = {
@@ -868,19 +1005,36 @@ void __init dm355_init(void)
 	davinci_map_sysmod();
 }
 
+int __init dm355_init_video(struct vpfe_config *vpfe_cfg,
+				struct vpbe_config *vpbe_cfg)
+{
+	if (vpfe_cfg || vpbe_cfg)
+		platform_device_register(&dm355_vpss_device);
+
+	if (vpfe_cfg) {
+		vpfe_capture_dev.dev.platform_data = vpfe_cfg;
+		platform_device_register(&dm355_ccdc_dev);
+		platform_device_register(&vpfe_capture_dev);
+	}
+
+	if (vpbe_cfg) {
+		dm355_vpbe_dev.dev.platform_data = vpbe_cfg;
+		platform_device_register(&dm355_osd_dev);
+		platform_device_register(&dm355_venc_dev);
+		platform_device_register(&dm355_vpbe_dev);
+		platform_device_register(&dm355_vpbe_display);
+	}
+
+	return 0;
+}
+
 static int __init dm355_init_devices(void)
 {
 	if (!cpu_is_davinci_dm355())
 		return 0;
 
-	/* Add ccdc clock aliases */
-	clk_add_alias("master", dm355_ccdc_dev.name, "vpss_master", NULL);
-	clk_add_alias("slave", dm355_ccdc_dev.name, "vpss_master", NULL);
 	davinci_cfg_reg(DM355_INT_EDMA_CC);
 	platform_device_register(&dm355_edma_device);
-	platform_device_register(&dm355_vpss_device);
-	platform_device_register(&dm355_ccdc_dev);
-	platform_device_register(&vpfe_capture_dev);
 
 	return 0;
 }
diff --git a/arch/arm/mach-davinci/dm365.c b/arch/arm/mach-davinci/dm365.c
index 6c3980540be..ff771ceac3f 100644
--- a/arch/arm/mach-davinci/dm365.c
+++ b/arch/arm/mach-davinci/dm365.c
@@ -39,16 +39,13 @@
 #include "asp.h"
 
 #define DM365_REF_FREQ		24000000	/* 24 MHz on the DM365 EVM */
-
-/* Base of key scan register bank */
-#define DM365_KEYSCAN_BASE		0x01c69400
-
 #define DM365_RTC_BASE			0x01c69000
-
+#define DM365_KEYSCAN_BASE		0x01c69400
+#define DM365_OSD_BASE			0x01c71c00
+#define DM365_VENC_BASE			0x01c71e00
 #define DAVINCI_DM365_VC_BASE		0x01d0c000
 #define DAVINCI_DMA_VC_TX		2
 #define DAVINCI_DMA_VC_RX		3
-
 #define DM365_EMAC_BASE			0x01d07000
 #define DM365_EMAC_MDIO_BASE		(DM365_EMAC_BASE + 0x4000)
 #define DM365_EMAC_CNTRL_OFFSET		0x0000
@@ -257,6 +254,12 @@ static struct clk vpss_master_clk = {
 	.flags		= CLK_PSC,
 };
 
+static struct clk vpss_slave_clk = {
+	.name		= "vpss_slave",
+	.parent		= &pll1_sysclk5,
+	.lpsc		= DAVINCI_LPSC_VPSSSLV,
+};
+
 static struct clk arm_clk = {
 	.name		= "arm_clk",
 	.parent		= &pll2_sysclk2,
@@ -449,7 +452,8 @@ static struct clk_lookup dm365_clks[] = {
 	CLK(NULL, "pll2_sysclk8", &pll2_sysclk8),
 	CLK(NULL, "pll2_sysclk9", &pll2_sysclk9),
 	CLK(NULL, "vpss_dac", &vpss_dac_clk),
-	CLK(NULL, "vpss_master", &vpss_master_clk),
+	CLK("vpss", "master", &vpss_master_clk),
+	CLK("vpss", "slave", &vpss_slave_clk),
 	CLK(NULL, "arm", &arm_clk),
 	CLK(NULL, "uart0", &uart0_clk),
 	CLK(NULL, "uart1", &uart1_clk),
@@ -1226,6 +1230,173 @@ static struct platform_device dm365_isif_dev = {
 	},
 };
 
+static struct resource dm365_osd_resources[] = {
+	{
+		.start = DM365_OSD_BASE,
+		.end   = DM365_OSD_BASE + 0xff,
+		.flags = IORESOURCE_MEM,
+	},
+};
+
+static u64 dm365_video_dma_mask = DMA_BIT_MASK(32);
+
+static struct platform_device dm365_osd_dev = {
+	.name		= DM365_VPBE_OSD_SUBDEV_NAME,
+	.id		= -1,
+	.num_resources	= ARRAY_SIZE(dm365_osd_resources),
+	.resource	= dm365_osd_resources,
+	.dev		= {
+		.dma_mask		= &dm365_video_dma_mask,
+		.coherent_dma_mask	= DMA_BIT_MASK(32),
+	},
+};
+
+static struct resource dm365_venc_resources[] = {
+	{
+		.start = IRQ_VENCINT,
+		.end   = IRQ_VENCINT,
+		.flags = IORESOURCE_IRQ,
+	},
+	/* venc registers io space */
+	{
+		.start = DM365_VENC_BASE,
+		.end   = DM365_VENC_BASE + 0x177,
+		.flags = IORESOURCE_MEM,
+	},
+	/* vdaccfg registers io space */
+	{
+		.start = DAVINCI_SYSTEM_MODULE_BASE + SYSMOD_VDAC_CONFIG,
+		.end   = DAVINCI_SYSTEM_MODULE_BASE + SYSMOD_VDAC_CONFIG + 3,
+		.flags = IORESOURCE_MEM,
+	},
+};
+
+static struct resource dm365_v4l2_disp_resources[] = {
+	{
+		.start = IRQ_VENCINT,
+		.end   = IRQ_VENCINT,
+		.flags = IORESOURCE_IRQ,
+	},
+	/* venc registers io space */
+	{
+		.start = DM365_VENC_BASE,
+		.end   = DM365_VENC_BASE + 0x177,
+		.flags = IORESOURCE_MEM,
+	},
+};
+
+static int dm365_vpbe_setup_pinmux(enum v4l2_mbus_pixelcode if_type,
+			    int field)
+{
+	switch (if_type) {
+	case V4L2_MBUS_FMT_SGRBG8_1X8:
+		davinci_cfg_reg(DM365_VOUT_FIELD_G81);
+		davinci_cfg_reg(DM365_VOUT_COUTL_EN);
+		davinci_cfg_reg(DM365_VOUT_COUTH_EN);
+		break;
+	case V4L2_MBUS_FMT_YUYV10_1X20:
+		if (field)
+			davinci_cfg_reg(DM365_VOUT_FIELD);
+		else
+			davinci_cfg_reg(DM365_VOUT_FIELD_G81);
+		davinci_cfg_reg(DM365_VOUT_COUTL_EN);
+		davinci_cfg_reg(DM365_VOUT_COUTH_EN);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int dm365_venc_setup_clock(enum vpbe_enc_timings_type type,
+				  unsigned int pclock)
+{
+	void __iomem *vpss_clkctl_reg;
+	u32 val;
+
+	vpss_clkctl_reg = DAVINCI_SYSMOD_VIRT(SYSMOD_VPSS_CLKCTL);
+
+	switch (type) {
+	case VPBE_ENC_STD:
+		val = VPSS_VENCCLKEN_ENABLE | VPSS_DACCLKEN_ENABLE;
+		break;
+	case VPBE_ENC_DV_TIMINGS:
+		if (pclock <= 27000000) {
+			val = VPSS_VENCCLKEN_ENABLE | VPSS_DACCLKEN_ENABLE;
+		} else {
+			/* set sysclk4 to output 74.25 MHz from pll1 */
+			val = VPSS_PLLC2SYSCLK5_ENABLE | VPSS_DACCLKEN_ENABLE |
+			      VPSS_VENCCLKEN_ENABLE;
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+	writel(val, vpss_clkctl_reg);
+
+	return 0;
+}
+
+static struct platform_device dm365_vpbe_display = {
+	.name		= "vpbe-v4l2",
+	.id		= -1,
+	.num_resources  = ARRAY_SIZE(dm365_v4l2_disp_resources),
+	.resource	= dm365_v4l2_disp_resources,
+	.dev		= {
+		.dma_mask		= &dm365_video_dma_mask,
+		.coherent_dma_mask	= DMA_BIT_MASK(32),
+	},
+};
+
+struct venc_platform_data dm365_venc_pdata = {
+	.setup_pinmux	= dm365_vpbe_setup_pinmux,
+	.setup_clock	= dm365_venc_setup_clock,
+};
+
+static struct platform_device dm365_venc_dev = {
+	.name		= DM365_VPBE_VENC_SUBDEV_NAME,
+	.id		= -1,
+	.num_resources	= ARRAY_SIZE(dm365_venc_resources),
+	.resource	= dm365_venc_resources,
+	.dev		= {
+		.dma_mask		= &dm365_video_dma_mask,
+		.coherent_dma_mask	= DMA_BIT_MASK(32),
+		.platform_data		= (void *)&dm365_venc_pdata,
+	},
+};
+
+static struct platform_device dm365_vpbe_dev = {
+	.name		= "vpbe_controller",
+	.id		= -1,
+	.dev		= {
+		.dma_mask		= &dm365_video_dma_mask,
+		.coherent_dma_mask	= DMA_BIT_MASK(32),
+	},
+};
+
+int __init dm365_init_video(struct vpfe_config *vpfe_cfg,
+				struct vpbe_config *vpbe_cfg)
+{
+	if (vpfe_cfg || vpbe_cfg)
+		platform_device_register(&dm365_vpss_device);
+
+	if (vpfe_cfg) {
+		vpfe_capture_dev.dev.platform_data = vpfe_cfg;
+		platform_device_register(&dm365_isif_dev);
+		platform_device_register(&vpfe_capture_dev);
+	}
+	if (vpbe_cfg) {
+		dm365_vpbe_dev.dev.platform_data = vpbe_cfg;
+		platform_device_register(&dm365_osd_dev);
+		platform_device_register(&dm365_venc_dev);
+		platform_device_register(&dm365_vpbe_dev);
+		platform_device_register(&dm365_vpbe_display);
+	}
+
+	return 0;
+}
+
 static int __init dm365_init_devices(void)
 {
 	if (!cpu_is_davinci_dm365())
@@ -1239,16 +1410,6 @@ static int __init dm365_init_devices(void)
 	clk_add_alias(NULL, dev_name(&dm365_mdio_device.dev),
 		      NULL, &dm365_emac_device.dev);
 
-	/* Add isif clock alias */
-	clk_add_alias("master", dm365_isif_dev.name, "vpss_master", NULL);
-	platform_device_register(&dm365_vpss_device);
-	platform_device_register(&dm365_isif_dev);
-	platform_device_register(&vpfe_capture_dev);
 	return 0;
 }
 postcore_initcall(dm365_init_devices);
-
-void dm365_set_vpfe_config(struct vpfe_config *cfg)
-{
-       vpfe_capture_dev.dev.platform_data = cfg;
-}
diff --git a/arch/arm/mach-davinci/dm644x.c b/arch/arm/mach-davinci/dm644x.c
index db1dd92e00a..c2a9273330b 100644
--- a/arch/arm/mach-davinci/dm644x.c
+++ b/arch/arm/mach-davinci/dm644x.c
@@ -300,8 +300,8 @@ static struct clk_lookup dm644x_clks[] = {
 	CLK(NULL, "dsp", &dsp_clk),
 	CLK(NULL, "arm", &arm_clk),
 	CLK(NULL, "vicp", &vicp_clk),
-	CLK(NULL, "vpss_master", &vpss_master_clk),
-	CLK(NULL, "vpss_slave", &vpss_slave_clk),
+	CLK("vpss", "master", &vpss_master_clk),
+	CLK("vpss", "slave", &vpss_slave_clk),
 	CLK(NULL, "arm", &arm_clk),
 	CLK(NULL, "uart0", &uart0_clk),
 	CLK(NULL, "uart1", &uart1_clk),
@@ -706,7 +706,7 @@ static int dm644x_venc_setup_clock(enum vpbe_enc_timings_type type,
 		v |= DM644X_VPSS_DACCLKEN;
 		writel(v, DAVINCI_SYSMOD_VIRT(SYSMOD_VPSS_CLKCTL));
 		break;
-	case VPBE_ENC_CUSTOM_TIMINGS:
+	case VPBE_ENC_DV_TIMINGS:
 		if (pclock <= 27000000) {
 			v |= DM644X_VPSS_DACCLKEN;
 			writel(v, DAVINCI_SYSMOD_VIRT(SYSMOD_VPSS_CLKCTL));
@@ -901,11 +901,6 @@ int __init dm644x_init_video(struct vpfe_config *vpfe_cfg,
 		dm644x_vpfe_dev.dev.platform_data = vpfe_cfg;
 		platform_device_register(&dm644x_ccdc_dev);
 		platform_device_register(&dm644x_vpfe_dev);
-		/* Add ccdc clock aliases */
-		clk_add_alias("master", dm644x_ccdc_dev.name,
-			      "vpss_master", NULL);
-		clk_add_alias("slave", dm644x_ccdc_dev.name,
-			      "vpss_slave", NULL);
 	}
 
 	if (vpbe_cfg) {
diff --git a/arch/arm/mach-davinci/pm_domain.c b/arch/arm/mach-davinci/pm_domain.c
index c90250e3bef..6b98413cebd 100644
--- a/arch/arm/mach-davinci/pm_domain.c
+++ b/arch/arm/mach-davinci/pm_domain.c
@@ -53,7 +53,7 @@ static struct dev_pm_domain davinci_pm_domain = {
 
 static struct pm_clk_notifier_block platform_bus_notifier = {
 	.pm_domain = &davinci_pm_domain,
-	.con_ids = { "fck", NULL, },
+	.con_ids = { "fck", "master", "slave", NULL },
 };
 
 static int __init davinci_pm_runtime_init(void)
diff --git a/arch/arm/mach-ep93xx/include/mach/uncompress.h b/arch/arm/mach-ep93xx/include/mach/uncompress.h
index d2afb4dd82a..b5cc77d2380 100644
--- a/arch/arm/mach-ep93xx/include/mach/uncompress.h
+++ b/arch/arm/mach-ep93xx/include/mach/uncompress.h
@@ -47,9 +47,13 @@ static void __raw_writel(unsigned int value, unsigned int ptr)
 
 static inline void putc(int c)
 {
-	/* Transmit fifo not full?  */
-	while (__raw_readb(PHYS_UART_FLAG) & UART_FLAG_TXFF)
-		;
+	int i;
+
+	for (i = 0; i < 10000; i++) {
+		/* Transmit fifo not full? */
+		if (!(__raw_readb(PHYS_UART_FLAG) & UART_FLAG_TXFF))
+			break;
+	}
 
 	__raw_writeb(c, PHYS_UART_DATA);
 }
diff --git a/arch/arm/mach-exynos/mach-universal_c210.c b/arch/arm/mach-exynos/mach-universal_c210.c
index 497fcb793dc..d28c7fbaba2 100644
--- a/arch/arm/mach-exynos/mach-universal_c210.c
+++ b/arch/arm/mach-exynos/mach-universal_c210.c
@@ -97,6 +97,19 @@ static struct s3c2410_uartcfg universal_uartcfgs[] __initdata = {
 static struct regulator_consumer_supply max8952_consumer =
 	REGULATOR_SUPPLY("vdd_arm", NULL);
 
+static struct regulator_init_data universal_max8952_reg_data = {
+	.constraints	= {
+		.name		= "VARM_1.2V",
+		.min_uV		= 770000,
+		.max_uV		= 1400000,
+		.valid_ops_mask	= REGULATOR_CHANGE_VOLTAGE,
+		.always_on	= 1,
+		.boot_on	= 1,
+	},
+	.num_consumer_supplies	= 1,
+	.consumer_supplies	= &max8952_consumer,
+};
+
 static struct max8952_platform_data universal_max8952_pdata __initdata = {
 	.gpio_vid0	= EXYNOS4_GPX0(3),
 	.gpio_vid1	= EXYNOS4_GPX0(4),
@@ -105,19 +118,7 @@ static struct max8952_platform_data universal_max8952_pdata __initdata = {
 	.dvs_mode	= { 48, 32, 28, 18 }, /* 1.25, 1.20, 1.05, 0.95V */
 	.sync_freq	= 0, /* default: fastest */
 	.ramp_speed	= 0, /* default: fastest */
-
-	.reg_data	= {
-		.constraints	= {
-			.name		= "VARM_1.2V",
-			.min_uV		= 770000,
-			.max_uV		= 1400000,
-			.valid_ops_mask	= REGULATOR_CHANGE_VOLTAGE,
-			.always_on	= 1,
-			.boot_on	= 1,
-		},
-		.num_consumer_supplies	= 1,
-		.consumer_supplies	= &max8952_consumer,
-	},
+	.reg_data	= &universal_max8952_reg_data,
 };
 
 static struct regulator_consumer_supply lp3974_buck1_consumer =
diff --git a/arch/arm/mach-exynos/setup-usb-phy.c b/arch/arm/mach-exynos/setup-usb-phy.c
index b81cc569a8d..6af40662a44 100644
--- a/arch/arm/mach-exynos/setup-usb-phy.c
+++ b/arch/arm/mach-exynos/setup-usb-phy.c
@@ -204,9 +204,9 @@ static int exynos4210_usb_phy1_exit(struct platform_device *pdev)
 
 int s5p_usb_phy_init(struct platform_device *pdev, int type)
 {
-	if (type == S5P_USB_PHY_DEVICE)
+	if (type == USB_PHY_TYPE_DEVICE)
 		return exynos4210_usb_phy0_init(pdev);
-	else if (type == S5P_USB_PHY_HOST)
+	else if (type == USB_PHY_TYPE_HOST)
 		return exynos4210_usb_phy1_init(pdev);
 
 	return -EINVAL;
@@ -214,9 +214,9 @@ int s5p_usb_phy_init(struct platform_device *pdev, int type)
 
 int s5p_usb_phy_exit(struct platform_device *pdev, int type)
 {
-	if (type == S5P_USB_PHY_DEVICE)
+	if (type == USB_PHY_TYPE_DEVICE)
 		return exynos4210_usb_phy0_exit(pdev);
-	else if (type == S5P_USB_PHY_HOST)
+	else if (type == USB_PHY_TYPE_HOST)
 		return exynos4210_usb_phy1_exit(pdev);
 
 	return -EINVAL;
diff --git a/arch/arm/mach-gemini/idle.c b/arch/arm/mach-gemini/idle.c
index 92bbd6bb600..87dff4f5059 100644
--- a/arch/arm/mach-gemini/idle.c
+++ b/arch/arm/mach-gemini/idle.c
@@ -13,9 +13,11 @@ static void gemini_idle(void)
 	 * will never wakeup... Acctualy it is not very good to enable
 	 * interrupts first since scheduler can miss a tick, but there is
 	 * no other way around this. Platforms that needs it for power saving
-	 * should call enable_hlt() in init code, since by default it is
+	 * should enable it in init code, since by default it is
 	 * disabled.
 	 */
+
+	/* FIXME: Enabling interrupts here is racy! */
 	local_irq_enable();
 	cpu_do_idle();
 }
diff --git a/arch/arm/mach-gemini/irq.c b/arch/arm/mach-gemini/irq.c
index 020852d3bdd..6d8f6d1669f 100644
--- a/arch/arm/mach-gemini/irq.c
+++ b/arch/arm/mach-gemini/irq.c
@@ -15,6 +15,8 @@
 #include <linux/stddef.h>
 #include <linux/list.h>
 #include <linux/sched.h>
+#include <linux/cpu.h>
+
 #include <asm/irq.h>
 #include <asm/mach/irq.h>
 #include <asm/system_misc.h>
@@ -77,7 +79,7 @@ void __init gemini_init_irq(void)
 	 * Disable the idle handler by default since it is buggy
 	 * For more info see arch/arm/mach-gemini/idle.c
 	 */
-	disable_hlt();
+	cpu_idle_poll_ctrl(true);
 
 	request_resource(&iomem_resource, &irq_resource);
 
diff --git a/arch/arm/mach-highbank/hotplug.c b/arch/arm/mach-highbank/hotplug.c
index f30c5284339..890cae23c12 100644
--- a/arch/arm/mach-highbank/hotplug.c
+++ b/arch/arm/mach-highbank/hotplug.c
@@ -28,13 +28,11 @@ extern void secondary_startup(void);
  */
 void __ref highbank_cpu_die(unsigned int cpu)
 {
-	flush_cache_all();
-
 	highbank_set_cpu_jump(cpu, phys_to_virt(0));
-	highbank_set_core_pwr();
 
-	cpu_do_idle();
+	flush_cache_louis();
+	highbank_set_core_pwr();
 
-	/* We should never return from idle */
-	panic("highbank: cpu %d unexpectedly exit from shutdown\n", cpu);
+	while (1)
+		cpu_do_idle();
 }
diff --git a/arch/arm/mach-imx/clk-busy.c b/arch/arm/mach-imx/clk-busy.c
index 1ab91b5209e..85b728cc27a 100644
--- a/arch/arm/mach-imx/clk-busy.c
+++ b/arch/arm/mach-imx/clk-busy.c
@@ -169,7 +169,7 @@ struct clk *imx_clk_busy_mux(const char *name, void __iomem *reg, u8 shift,
 
 	busy->mux.reg = reg;
 	busy->mux.shift = shift;
-	busy->mux.width = width;
+	busy->mux.mask = BIT(width) - 1;
 	busy->mux.lock = &imx_ccm_lock;
 	busy->mux_ops = &clk_mux_ops;
 
diff --git a/arch/arm/mach-imx/clk-imx35.c b/arch/arm/mach-imx/clk-imx35.c
index e13a8fa5e62..2193c834f55 100644
--- a/arch/arm/mach-imx/clk-imx35.c
+++ b/arch/arm/mach-imx/clk-imx35.c
@@ -257,6 +257,7 @@ int __init mx35_clocks_init(void)
 	clk_register_clkdev(clk[wdog_gate], NULL, "imx2-wdt.0");
 	clk_register_clkdev(clk[nfc_div], NULL, "imx25-nand.0");
 	clk_register_clkdev(clk[csi_gate], NULL, "mx3-camera.0");
+	clk_register_clkdev(clk[admux_gate], "audmux", NULL);
 
 	clk_prepare_enable(clk[spba_gate]);
 	clk_prepare_enable(clk[gpio1_gate]);
@@ -265,6 +266,7 @@ int __init mx35_clocks_init(void)
 	clk_prepare_enable(clk[iim_gate]);
 	clk_prepare_enable(clk[emi_gate]);
 	clk_prepare_enable(clk[max_gate]);
+	clk_prepare_enable(clk[iomuxc_gate]);
 
 	/*
 	 * SCC is needed to boot via mmc after a watchdog reset. The clock code
diff --git a/arch/arm/mach-imx/clk-imx6q.c b/arch/arm/mach-imx/clk-imx6q.c
index 2f9ff93a4e6..d38e54f5b6d 100644
--- a/arch/arm/mach-imx/clk-imx6q.c
+++ b/arch/arm/mach-imx/clk-imx6q.c
@@ -115,7 +115,7 @@ static const char *gpu2d_core_sels[]	= { "axi", "pll3_usb_otg", "pll2_pfd0_352m"
 static const char *gpu3d_core_sels[]	= { "mmdc_ch0_axi", "pll3_usb_otg", "pll2_pfd1_594m", "pll2_pfd2_396m", };
 static const char *gpu3d_shader_sels[] = { "mmdc_ch0_axi", "pll3_usb_otg", "pll2_pfd1_594m", "pll2_pfd9_720m", };
 static const char *ipu_sels[]		= { "mmdc_ch0_axi", "pll2_pfd2_396m", "pll3_120m", "pll3_pfd1_540m", };
-static const char *ldb_di_sels[]	= { "pll5_video", "pll2_pfd0_352m", "pll2_pfd2_396m", "mmdc_ch1_axi", "pll3_pfd1_540m", };
+static const char *ldb_di_sels[]	= { "pll5_video", "pll2_pfd0_352m", "pll2_pfd2_396m", "mmdc_ch1_axi", "pll3_usb_otg", };
 static const char *ipu_di_pre_sels[]	= { "mmdc_ch0_axi", "pll3_usb_otg", "pll5_video", "pll2_pfd0_352m", "pll2_pfd2_396m", "pll3_pfd1_540m", };
 static const char *ipu1_di0_sels[]	= { "ipu1_di0_pre", "dummy", "dummy", "ldb_di0", "ldb_di1", };
 static const char *ipu1_di1_sels[]	= { "ipu1_di1_pre", "dummy", "dummy", "ldb_di0", "ldb_di1", };
@@ -443,7 +443,6 @@ int __init mx6q_clocks_init(void)
 
 	clk_register_clkdev(clk[gpt_ipg], "ipg", "imx-gpt.0");
 	clk_register_clkdev(clk[gpt_ipg_per], "per", "imx-gpt.0");
-	clk_register_clkdev(clk[twd], NULL, "smp_twd");
 	clk_register_clkdev(clk[cko1_sel], "cko1_sel", NULL);
 	clk_register_clkdev(clk[ahb], "ahb", NULL);
 	clk_register_clkdev(clk[cko1], "cko1", NULL);
diff --git a/arch/arm/mach-imx/common.h b/arch/arm/mach-imx/common.h
index 5a800bfcec5..5bf4a97ab24 100644
--- a/arch/arm/mach-imx/common.h
+++ b/arch/arm/mach-imx/common.h
@@ -110,6 +110,8 @@ void tzic_handle_irq(struct pt_regs *);
 
 extern void imx_enable_cpu(int cpu, bool enable);
 extern void imx_set_cpu_jump(int cpu, void *jump_addr);
+extern u32 imx_get_cpu_arg(int cpu);
+extern void imx_set_cpu_arg(int cpu, u32 arg);
 extern void v7_cpu_resume(void);
 extern u32 *pl310_get_save_ptr(void);
 #ifdef CONFIG_SMP
diff --git a/arch/arm/mach-imx/hotplug.c b/arch/arm/mach-imx/hotplug.c
index 7bc5fe15dda..361a253e2b6 100644
--- a/arch/arm/mach-imx/hotplug.c
+++ b/arch/arm/mach-imx/hotplug.c
@@ -46,11 +46,23 @@ static inline void cpu_enter_lowpower(void)
 void imx_cpu_die(unsigned int cpu)
 {
 	cpu_enter_lowpower();
+	/*
+	 * We use the cpu jumping argument register to sync with
+	 * imx_cpu_kill() which is running on cpu0 and waiting for
+	 * the register being cleared to kill the cpu.
+	 */
+	imx_set_cpu_arg(cpu, ~0);
 	cpu_do_idle();
 }
 
 int imx_cpu_kill(unsigned int cpu)
 {
+	unsigned long timeout = jiffies + msecs_to_jiffies(50);
+
+	while (imx_get_cpu_arg(cpu) == 0)
+		if (time_after(jiffies, timeout))
+			return 0;
 	imx_enable_cpu(cpu, false);
+	imx_set_cpu_arg(cpu, 0);
 	return 1;
 }
diff --git a/arch/arm/mach-imx/src.c b/arch/arm/mach-imx/src.c
index e15f1555c59..09a742f8c7a 100644
--- a/arch/arm/mach-imx/src.c
+++ b/arch/arm/mach-imx/src.c
@@ -43,6 +43,18 @@ void imx_set_cpu_jump(int cpu, void *jump_addr)
 		       src_base + SRC_GPR1 + cpu * 8);
 }
 
+u32 imx_get_cpu_arg(int cpu)
+{
+	cpu = cpu_logical_map(cpu);
+	return readl_relaxed(src_base + SRC_GPR1 + cpu * 8 + 4);
+}
+
+void imx_set_cpu_arg(int cpu, u32 arg)
+{
+	cpu = cpu_logical_map(cpu);
+	writel_relaxed(arg, src_base + SRC_GPR1 + cpu * 8 + 4);
+}
+
 void imx_src_prepare_restart(void)
 {
 	u32 val;
diff --git a/arch/arm/mach-ixp4xx/common.c b/arch/arm/mach-ixp4xx/common.c
index 1dbeb7c99d5..6600cff6bd9 100644
--- a/arch/arm/mach-ixp4xx/common.c
+++ b/arch/arm/mach-ixp4xx/common.c
@@ -29,6 +29,7 @@
 #include <linux/io.h>
 #include <linux/export.h>
 #include <linux/gpio.h>
+#include <linux/cpu.h>
 
 #include <mach/udc.h>
 #include <mach/hardware.h>
@@ -239,7 +240,7 @@ void __init ixp4xx_init_irq(void)
 	 * ixp4xx does not implement the XScale PWRMODE register
 	 * so it must not call cpu_do_idle().
 	 */
-	disable_hlt();
+	cpu_idle_poll_ctrl(true);
 
 	/* Route all sources to IRQ instead of FIQ */
 	*IXP4XX_ICLR = 0x0;
diff --git a/arch/arm/mach-kirkwood/board-iomega_ix2_200.c b/arch/arm/mach-kirkwood/board-iomega_ix2_200.c
index f655b2637b0..e5f70415905 100644
--- a/arch/arm/mach-kirkwood/board-iomega_ix2_200.c
+++ b/arch/arm/mach-kirkwood/board-iomega_ix2_200.c
@@ -20,10 +20,15 @@ static struct mv643xx_eth_platform_data iomega_ix2_200_ge00_data = {
 	.duplex         = DUPLEX_FULL,
 };
 
+static struct mv643xx_eth_platform_data iomega_ix2_200_ge01_data = {
+        .phy_addr       = MV643XX_ETH_PHY_ADDR(11),
+};
+
 void __init iomega_ix2_200_init(void)
 {
 	/*
 	 * Basic setup. Needs to be called early.
 	 */
-	kirkwood_ge01_init(&iomega_ix2_200_ge00_data);
+	kirkwood_ge00_init(&iomega_ix2_200_ge00_data);
+	kirkwood_ge01_init(&iomega_ix2_200_ge01_data);
 }
diff --git a/arch/arm/mach-kirkwood/guruplug-setup.c b/arch/arm/mach-kirkwood/guruplug-setup.c
index 1c6e736cbbf..08dd739aa70 100644
--- a/arch/arm/mach-kirkwood/guruplug-setup.c
+++ b/arch/arm/mach-kirkwood/guruplug-setup.c
@@ -53,6 +53,8 @@ static struct mv_sata_platform_data guruplug_sata_data = {
 
 static struct mvsdio_platform_data guruplug_mvsdio_data = {
 	/* unfortunately the CD signal has not been connected */
+	.gpio_card_detect = -1,
+	.gpio_write_protect = -1,
 };
 
 static struct gpio_led guruplug_led_pins[] = {
diff --git a/arch/arm/mach-kirkwood/openrd-setup.c b/arch/arm/mach-kirkwood/openrd-setup.c
index 8ddd69fdc93..6a6eb548307 100644
--- a/arch/arm/mach-kirkwood/openrd-setup.c
+++ b/arch/arm/mach-kirkwood/openrd-setup.c
@@ -55,6 +55,7 @@ static struct mv_sata_platform_data openrd_sata_data = {
 
 static struct mvsdio_platform_data openrd_mvsdio_data = {
 	.gpio_card_detect = 29,	/* MPP29 used as SD card detect */
+	.gpio_write_protect = -1,
 };
 
 static unsigned int openrd_mpp_config[] __initdata = {
diff --git a/arch/arm/mach-kirkwood/rd88f6281-setup.c b/arch/arm/mach-kirkwood/rd88f6281-setup.c
index c7d93b48926..d24223166e0 100644
--- a/arch/arm/mach-kirkwood/rd88f6281-setup.c
+++ b/arch/arm/mach-kirkwood/rd88f6281-setup.c
@@ -69,6 +69,7 @@ static struct mv_sata_platform_data rd88f6281_sata_data = {
 
 static struct mvsdio_platform_data rd88f6281_mvsdio_data = {
 	.gpio_card_detect = 28,
+	.gpio_write_protect = -1,
 };
 
 static unsigned int rd88f6281_mpp_config[] __initdata = {
diff --git a/arch/arm/mach-mmp/aspenite.c b/arch/arm/mach-mmp/aspenite.c
index 9f64d5632e0..76901f4ce61 100644
--- a/arch/arm/mach-mmp/aspenite.c
+++ b/arch/arm/mach-mmp/aspenite.c
@@ -223,13 +223,7 @@ static struct pxa27x_keypad_platform_data aspenite_keypad_info __initdata = {
 };
 
 #if defined(CONFIG_USB_EHCI_MV)
-static char *pxa168_sph_clock_name[] = {
-	[0] = "PXA168-USBCLK",
-};
-
 static struct mv_usb_platform_data pxa168_sph_pdata = {
-	.clknum         = 1,
-	.clkname        = pxa168_sph_clock_name,
 	.mode           = MV_USB_MODE_HOST,
 	.phy_init	= pxa_usb_phy_init,
 	.phy_deinit	= pxa_usb_phy_deinit,
diff --git a/arch/arm/mach-mmp/ttc_dkb.c b/arch/arm/mach-mmp/ttc_dkb.c
index 22a9058f9f4..6528a5fa6a2 100644
--- a/arch/arm/mach-mmp/ttc_dkb.c
+++ b/arch/arm/mach-mmp/ttc_dkb.c
@@ -162,13 +162,7 @@ static struct i2c_board_info ttc_dkb_i2c_info[] = {
 #ifdef CONFIG_USB_SUPPORT
 #if defined(CONFIG_USB_MV_UDC) || defined(CONFIG_USB_EHCI_MV_U2O)
 
-static char *pxa910_usb_clock_name[] = {
-	[0] = "U2OCLK",
-};
-
 static struct mv_usb_platform_data ttc_usb_pdata = {
-	.clknum		= 1,
-	.clkname	= pxa910_usb_clock_name,
 	.vbus		= NULL,
 	.mode		= MV_USB_MODE_OTG,
 	.otg_force_a_bus_req = 1,
diff --git a/arch/arm/mach-msm/timer.c b/arch/arm/mach-msm/timer.c
index 2969027f02f..f9fd77e8f1f 100644
--- a/arch/arm/mach-msm/timer.c
+++ b/arch/arm/mach-msm/timer.c
@@ -62,7 +62,10 @@ static int msm_timer_set_next_event(unsigned long cycles,
 {
 	u32 ctrl = readl_relaxed(event_base + TIMER_ENABLE);
 
-	writel_relaxed(0, event_base + TIMER_CLEAR);
+	ctrl &= ~TIMER_ENABLE_EN;
+	writel_relaxed(ctrl, event_base + TIMER_ENABLE);
+
+	writel_relaxed(ctrl, event_base + TIMER_CLEAR);
 	writel_relaxed(cycles, event_base + TIMER_MATCH_VAL);
 	writel_relaxed(ctrl | TIMER_ENABLE_EN, event_base + TIMER_ENABLE);
 	return 0;
diff --git a/arch/arm/mach-mvebu/irq-armada-370-xp.c b/arch/arm/mach-mvebu/irq-armada-370-xp.c
index 274ff58271d..830139a3e2b 100644
--- a/arch/arm/mach-mvebu/irq-armada-370-xp.c
+++ b/arch/arm/mach-mvebu/irq-armada-370-xp.c
@@ -44,6 +44,8 @@
 
 #define ARMADA_370_XP_MAX_PER_CPU_IRQS		(28)
 
+#define ARMADA_370_XP_TIMER0_PER_CPU_IRQ	(5)
+
 #define ACTIVE_DOORBELLS			(8)
 
 static DEFINE_RAW_SPINLOCK(irq_controller_lock);
@@ -55,40 +57,30 @@ static struct irq_domain *armada_370_xp_mpic_domain;
 /*
  * In SMP mode:
  * For shared global interrupts, mask/unmask global enable bit
- * For CPU interrtups, mask/unmask the calling CPU's bit
+ * For CPU interrupts, mask/unmask the calling CPU's bit
  */
 static void armada_370_xp_irq_mask(struct irq_data *d)
 {
-#ifdef CONFIG_SMP
 	irq_hw_number_t hwirq = irqd_to_hwirq(d);
 
-	if (hwirq > ARMADA_370_XP_MAX_PER_CPU_IRQS)
+	if (hwirq != ARMADA_370_XP_TIMER0_PER_CPU_IRQ)
 		writel(hwirq, main_int_base +
 				ARMADA_370_XP_INT_CLEAR_ENABLE_OFFS);
 	else
 		writel(hwirq, per_cpu_int_base +
 				ARMADA_370_XP_INT_SET_MASK_OFFS);
-#else
-	writel(irqd_to_hwirq(d),
-	       per_cpu_int_base + ARMADA_370_XP_INT_SET_MASK_OFFS);
-#endif
 }
 
 static void armada_370_xp_irq_unmask(struct irq_data *d)
 {
-#ifdef CONFIG_SMP
 	irq_hw_number_t hwirq = irqd_to_hwirq(d);
 
-	if (hwirq > ARMADA_370_XP_MAX_PER_CPU_IRQS)
+	if (hwirq != ARMADA_370_XP_TIMER0_PER_CPU_IRQ)
 		writel(hwirq, main_int_base +
 				ARMADA_370_XP_INT_SET_ENABLE_OFFS);
 	else
 		writel(hwirq, per_cpu_int_base +
 				ARMADA_370_XP_INT_CLEAR_MASK_OFFS);
-#else
-	writel(irqd_to_hwirq(d),
-	       per_cpu_int_base + ARMADA_370_XP_INT_CLEAR_MASK_OFFS);
-#endif
 }
 
 #ifdef CONFIG_SMP
@@ -144,10 +136,14 @@ static int armada_370_xp_mpic_irq_map(struct irq_domain *h,
 				      unsigned int virq, irq_hw_number_t hw)
 {
 	armada_370_xp_irq_mask(irq_get_irq_data(virq));
-	writel(hw, main_int_base + ARMADA_370_XP_INT_SET_ENABLE_OFFS);
+	if (hw != ARMADA_370_XP_TIMER0_PER_CPU_IRQ)
+		writel(hw, per_cpu_int_base +
+			ARMADA_370_XP_INT_CLEAR_MASK_OFFS);
+	else
+		writel(hw, main_int_base + ARMADA_370_XP_INT_SET_ENABLE_OFFS);
 	irq_set_status_flags(virq, IRQ_LEVEL);
 
-	if (hw < ARMADA_370_XP_MAX_PER_CPU_IRQS) {
+	if (hw == ARMADA_370_XP_TIMER0_PER_CPU_IRQ) {
 		irq_set_percpu_devid(virq);
 		irq_set_chip_and_handler(virq, &armada_370_xp_irq_chip,
 					handle_percpu_devid_irq);
diff --git a/arch/arm/mach-mxs/mach-mxs.c b/arch/arm/mach-mxs/mach-mxs.c
index 3218f1f2c0e..e7b781d3788 100644
--- a/arch/arm/mach-mxs/mach-mxs.c
+++ b/arch/arm/mach-mxs/mach-mxs.c
@@ -41,8 +41,6 @@ static struct fb_videomode mx23evk_video_modes[] = {
 		.lower_margin	= 4,
 		.hsync_len	= 1,
 		.vsync_len	= 1,
-		.sync		= FB_SYNC_DATA_ENABLE_HIGH_ACT |
-				  FB_SYNC_DOTCLK_FAILING_ACT,
 	},
 };
 
@@ -59,8 +57,6 @@ static struct fb_videomode mx28evk_video_modes[] = {
 		.lower_margin	= 10,
 		.hsync_len	= 10,
 		.vsync_len	= 10,
-		.sync		= FB_SYNC_DATA_ENABLE_HIGH_ACT |
-				  FB_SYNC_DOTCLK_FAILING_ACT,
 	},
 };
 
@@ -77,7 +73,6 @@ static struct fb_videomode m28evk_video_modes[] = {
 		.lower_margin	= 45,
 		.hsync_len	= 1,
 		.vsync_len	= 1,
-		.sync		= FB_SYNC_DATA_ENABLE_HIGH_ACT,
 	},
 };
 
@@ -94,9 +89,7 @@ static struct fb_videomode apx4devkit_video_modes[] = {
 		.lower_margin	= 13,
 		.hsync_len	= 48,
 		.vsync_len	= 3,
-		.sync		= FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT |
-				  FB_SYNC_DATA_ENABLE_HIGH_ACT |
-				  FB_SYNC_DOTCLK_FAILING_ACT,
+		.sync		= FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
 	},
 };
 
@@ -113,9 +106,7 @@ static struct fb_videomode apf28dev_video_modes[] = {
 		.lower_margin = 0x15,
 		.hsync_len = 64,
 		.vsync_len = 4,
-		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT |
-				FB_SYNC_DATA_ENABLE_HIGH_ACT |
-				FB_SYNC_DOTCLK_FAILING_ACT,
+		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
 	},
 };
 
@@ -132,7 +123,6 @@ static struct fb_videomode cfa10049_video_modes[] = {
 		.lower_margin	= 2,
 		.hsync_len	= 15,
 		.vsync_len	= 15,
-		.sync		= FB_SYNC_DATA_ENABLE_HIGH_ACT
 	},
 };
 
@@ -259,6 +249,8 @@ static void __init imx23_evk_init(void)
 	mxsfb_pdata.mode_count = ARRAY_SIZE(mx23evk_video_modes);
 	mxsfb_pdata.default_bpp = 32;
 	mxsfb_pdata.ld_intf_width = STMLCDIF_24BIT;
+	mxsfb_pdata.sync = MXSFB_SYNC_DATA_ENABLE_HIGH_ACT |
+				MXSFB_SYNC_DOTCLK_FAILING_ACT;
 }
 
 static inline void enable_clk_enet_out(void)
@@ -278,6 +270,8 @@ static void __init imx28_evk_init(void)
 	mxsfb_pdata.mode_count = ARRAY_SIZE(mx28evk_video_modes);
 	mxsfb_pdata.default_bpp = 32;
 	mxsfb_pdata.ld_intf_width = STMLCDIF_24BIT;
+	mxsfb_pdata.sync = MXSFB_SYNC_DATA_ENABLE_HIGH_ACT |
+				MXSFB_SYNC_DOTCLK_FAILING_ACT;
 
 	mxs_saif_clkmux_select(MXS_DIGCTL_SAIF_CLKMUX_EXTMSTR0);
 }
@@ -297,6 +291,7 @@ static void __init m28evk_init(void)
 	mxsfb_pdata.mode_count = ARRAY_SIZE(m28evk_video_modes);
 	mxsfb_pdata.default_bpp = 16;
 	mxsfb_pdata.ld_intf_width = STMLCDIF_18BIT;
+	mxsfb_pdata.sync = MXSFB_SYNC_DATA_ENABLE_HIGH_ACT;
 }
 
 static void __init sc_sps1_init(void)
@@ -322,6 +317,8 @@ static void __init apx4devkit_init(void)
 	mxsfb_pdata.mode_count = ARRAY_SIZE(apx4devkit_video_modes);
 	mxsfb_pdata.default_bpp = 32;
 	mxsfb_pdata.ld_intf_width = STMLCDIF_24BIT;
+	mxsfb_pdata.sync = MXSFB_SYNC_DATA_ENABLE_HIGH_ACT |
+				MXSFB_SYNC_DOTCLK_FAILING_ACT;
 }
 
 #define ENET0_MDC__GPIO_4_0	MXS_GPIO_NR(4, 0)
@@ -407,6 +404,7 @@ static void __init cfa10049_init(void)
 	mxsfb_pdata.mode_count = ARRAY_SIZE(cfa10049_video_modes);
 	mxsfb_pdata.default_bpp = 32;
 	mxsfb_pdata.ld_intf_width = STMLCDIF_18BIT;
+	mxsfb_pdata.sync = MXSFB_SYNC_DATA_ENABLE_HIGH_ACT;
 }
 
 static void __init cfa10037_init(void)
@@ -423,6 +421,8 @@ static void __init apf28_init(void)
 	mxsfb_pdata.mode_count = ARRAY_SIZE(apf28dev_video_modes);
 	mxsfb_pdata.default_bpp = 16;
 	mxsfb_pdata.ld_intf_width = STMLCDIF_16BIT;
+	mxsfb_pdata.sync = MXSFB_SYNC_DATA_ENABLE_HIGH_ACT |
+				MXSFB_SYNC_DOTCLK_FAILING_ACT;
 }
 
 static void __init mxs_machine_init(void)
diff --git a/arch/arm/mach-omap1/clock_data.c b/arch/arm/mach-omap1/clock_data.c
index cb7c6ae2e3f..6c4f766365a 100644
--- a/arch/arm/mach-omap1/clock_data.c
+++ b/arch/arm/mach-omap1/clock_data.c
@@ -543,15 +543,6 @@ static struct clk usb_dc_ck = {
 	/* Direct from ULPD, no parent */
 	.rate		= 48000000,
 	.enable_reg	= OMAP1_IO_ADDRESS(SOFT_REQ_REG),
-	.enable_bit	= USB_REQ_EN_SHIFT,
-};
-
-static struct clk usb_dc_ck7xx = {
-	.name		= "usb_dc_ck",
-	.ops		= &clkops_generic,
-	/* Direct from ULPD, no parent */
-	.rate		= 48000000,
-	.enable_reg	= OMAP1_IO_ADDRESS(SOFT_REQ_REG),
 	.enable_bit	= SOFT_USB_OTG_DPLL_REQ_SHIFT,
 };
 
@@ -727,8 +718,7 @@ static struct omap_clk omap_clks[] = {
 	CLK(NULL,	"usb_clko",	&usb_clko,	CK_16XX | CK_1510 | CK_310),
 	CLK(NULL,	"usb_hhc_ck",	&usb_hhc_ck1510, CK_1510 | CK_310),
 	CLK(NULL,	"usb_hhc_ck",	&usb_hhc_ck16xx, CK_16XX),
-	CLK(NULL,	"usb_dc_ck",	&usb_dc_ck,	CK_16XX),
-	CLK(NULL,	"usb_dc_ck",	&usb_dc_ck7xx,	CK_7XX),
+	CLK(NULL,	"usb_dc_ck",	&usb_dc_ck,	CK_16XX | CK_7XX),
 	CLK(NULL,	"mclk",		&mclk_1510,	CK_1510 | CK_310),
 	CLK(NULL,	"mclk",		&mclk_16xx,	CK_16XX),
 	CLK(NULL,	"bclk",		&bclk_1510,	CK_1510 | CK_310),
diff --git a/arch/arm/mach-omap1/pm.c b/arch/arm/mach-omap1/pm.c
index 7a7690ab6cb..db37f49da5a 100644
--- a/arch/arm/mach-omap1/pm.c
+++ b/arch/arm/mach-omap1/pm.c
@@ -43,6 +43,7 @@
 #include <linux/module.h>
 #include <linux/io.h>
 #include <linux/atomic.h>
+#include <linux/cpu.h>
 
 #include <asm/fncpy.h>
 #include <asm/system_misc.h>
@@ -584,8 +585,7 @@ static void omap_pm_init_proc(void)
 static int omap_pm_prepare(void)
 {
 	/* We cannot sleep in idle until we have resumed */
-	disable_hlt();
-
+	cpu_idle_poll_ctrl(true);
 	return 0;
 }
 
@@ -621,7 +621,7 @@ static int omap_pm_enter(suspend_state_t state)
 
 static void omap_pm_finish(void)
 {
-	enable_hlt();
+	cpu_idle_poll_ctrl(false);
 }
 
 
diff --git a/arch/arm/mach-omap2/cclock44xx_data.c b/arch/arm/mach-omap2/cclock44xx_data.c
index 3d58f335f17..0c6834ae1fc 100644
--- a/arch/arm/mach-omap2/cclock44xx_data.c
+++ b/arch/arm/mach-omap2/cclock44xx_data.c
@@ -52,6 +52,13 @@
  */
 #define OMAP4_DPLL_ABE_DEFFREQ				98304000
 
+/*
+ * OMAP4 USB DPLL default frequency. In OMAP4430 TRM version V, section
+ * "3.6.3.9.5 DPLL_USB Preferred Settings" shows that the preferred
+ * locked frequency for the USB DPLL is 960MHz.
+ */
+#define OMAP4_DPLL_USB_DEFFREQ				960000000
+
 /* Root clocks */
 
 DEFINE_CLK_FIXED_RATE(extalt_clkin_ck, CLK_IS_ROOT, 59000000, 0x0);
@@ -1011,6 +1018,10 @@ DEFINE_CLK_OMAP_MUX(hsmmc2_fclk, "l3_init_clkdm", hsmmc1_fclk_sel,
 		    OMAP4430_CM_L3INIT_MMC2_CLKCTRL, OMAP4430_CLKSEL_MASK,
 		    hsmmc1_fclk_parents, func_dmic_abe_gfclk_ops);
 
+DEFINE_CLK_GATE(ocp2scp_usb_phy_phy_48m, "func_48m_fclk", &func_48m_fclk, 0x0,
+		OMAP4430_CM_L3INIT_USBPHYOCP2SCP_CLKCTRL,
+		OMAP4430_OPTFCLKEN_PHY_48M_SHIFT, 0x0, NULL);
+
 DEFINE_CLK_GATE(sha2md5_fck, "l3_div_ck", &l3_div_ck, 0x0,
 		OMAP4430_CM_L4SEC_SHA2MD51_CLKCTRL,
 		OMAP4430_MODULEMODE_SWCTRL_SHIFT, 0x0, NULL);
@@ -1538,6 +1549,7 @@ static struct omap_clk omap44xx_clks[] = {
 	CLK(NULL,	"per_mcbsp4_gfclk",			&per_mcbsp4_gfclk,	CK_443X),
 	CLK(NULL,	"hsmmc1_fclk",			&hsmmc1_fclk,	CK_443X),
 	CLK(NULL,	"hsmmc2_fclk",			&hsmmc2_fclk,	CK_443X),
+	CLK(NULL,	"ocp2scp_usb_phy_phy_48m",	&ocp2scp_usb_phy_phy_48m,	CK_443X),
 	CLK(NULL,	"sha2md5_fck",			&sha2md5_fck,	CK_443X),
 	CLK(NULL,	"slimbus1_fclk_1",		&slimbus1_fclk_1,	CK_443X),
 	CLK(NULL,	"slimbus1_fclk_0",		&slimbus1_fclk_0,	CK_443X),
@@ -1705,5 +1717,13 @@ int __init omap4xxx_clk_init(void)
 	if (rc)
 		pr_err("%s: failed to configure ABE DPLL!\n", __func__);
 
+	/*
+	 * Lock USB DPLL on OMAP4 devices so that the L3INIT power
+	 * domain can transition to retention state when not in use.
+	 */
+	rc = clk_set_rate(&dpll_usb_ck, OMAP4_DPLL_USB_DEFFREQ);
+	if (rc)
+		pr_err("%s: failed to configure USB DPLL!\n", __func__);
+
 	return 0;
 }
diff --git a/arch/arm/mach-omap2/common.h b/arch/arm/mach-omap2/common.h
index 40f4a03d728..d6ba13e1c54 100644
--- a/arch/arm/mach-omap2/common.h
+++ b/arch/arm/mach-omap2/common.h
@@ -293,5 +293,8 @@ extern void omap_reserve(void);
 struct omap_hwmod;
 extern int omap_dss_reset(struct omap_hwmod *);
 
+/* SoC specific clock initializer */
+extern int (*omap_clk_init)(void);
+
 #endif /* __ASSEMBLER__ */
 #endif /* __ARCH_ARM_MACH_OMAP2PLUS_COMMON_H */
diff --git a/arch/arm/mach-omap2/io.c b/arch/arm/mach-omap2/io.c
index 2c3fdd65387..5c445ca1e27 100644
--- a/arch/arm/mach-omap2/io.c
+++ b/arch/arm/mach-omap2/io.c
@@ -55,6 +55,12 @@
 #include "prm44xx.h"
 
 /*
+ * omap_clk_init: points to a function that does the SoC-specific
+ * clock initializations
+ */
+int (*omap_clk_init)(void);
+
+/*
  * The machine specific code may provide the extra mapping besides the
  * default mapping provided here.
  */
@@ -397,7 +403,7 @@ void __init omap2420_init_early(void)
 	omap242x_clockdomains_init();
 	omap2420_hwmod_init();
 	omap_hwmod_init_postsetup();
-	omap2420_clk_init();
+	omap_clk_init = omap2420_clk_init;
 }
 
 void __init omap2420_init_late(void)
@@ -427,7 +433,7 @@ void __init omap2430_init_early(void)
 	omap243x_clockdomains_init();
 	omap2430_hwmod_init();
 	omap_hwmod_init_postsetup();
-	omap2430_clk_init();
+	omap_clk_init = omap2430_clk_init;
 }
 
 void __init omap2430_init_late(void)
@@ -462,7 +468,7 @@ void __init omap3_init_early(void)
 	omap3xxx_clockdomains_init();
 	omap3xxx_hwmod_init();
 	omap_hwmod_init_postsetup();
-	omap3xxx_clk_init();
+	omap_clk_init = omap3xxx_clk_init;
 }
 
 void __init omap3430_init_early(void)
@@ -500,7 +506,7 @@ void __init ti81xx_init_early(void)
 	omap3xxx_clockdomains_init();
 	omap3xxx_hwmod_init();
 	omap_hwmod_init_postsetup();
-	omap3xxx_clk_init();
+	omap_clk_init = omap3xxx_clk_init;
 }
 
 void __init omap3_init_late(void)
@@ -568,7 +574,7 @@ void __init am33xx_init_early(void)
 	am33xx_clockdomains_init();
 	am33xx_hwmod_init();
 	omap_hwmod_init_postsetup();
-	am33xx_clk_init();
+	omap_clk_init = am33xx_clk_init;
 }
 #endif
 
@@ -593,7 +599,7 @@ void __init omap4430_init_early(void)
 	omap44xx_clockdomains_init();
 	omap44xx_hwmod_init();
 	omap_hwmod_init_postsetup();
-	omap4xxx_clk_init();
+	omap_clk_init = omap4xxx_clk_init;
 }
 
 void __init omap4430_init_late(void)
diff --git a/arch/arm/mach-omap2/omap_hwmod.c b/arch/arm/mach-omap2/omap_hwmod.c
index c2c798c08c2..e512253601c 100644
--- a/arch/arm/mach-omap2/omap_hwmod.c
+++ b/arch/arm/mach-omap2/omap_hwmod.c
@@ -138,6 +138,7 @@
 #include <linux/spinlock.h>
 #include <linux/slab.h>
 #include <linux/bootmem.h>
+#include <linux/cpu.h>
 
 #include <asm/system_misc.h>
 
@@ -1368,7 +1369,9 @@ static void _enable_sysc(struct omap_hwmod *oh)
 	}
 
 	if (sf & SYSC_HAS_MIDLEMODE) {
-		if (oh->flags & HWMOD_SWSUP_MSTANDBY) {
+		if (oh->flags & HWMOD_FORCE_MSTANDBY) {
+			idlemode = HWMOD_IDLEMODE_FORCE;
+		} else if (oh->flags & HWMOD_SWSUP_MSTANDBY) {
 			idlemode = HWMOD_IDLEMODE_NO;
 		} else {
 			if (sf & SYSC_HAS_ENAWAKEUP)
@@ -1440,7 +1443,8 @@ static void _idle_sysc(struct omap_hwmod *oh)
 	}
 
 	if (sf & SYSC_HAS_MIDLEMODE) {
-		if (oh->flags & HWMOD_SWSUP_MSTANDBY) {
+		if ((oh->flags & HWMOD_SWSUP_MSTANDBY) ||
+		    (oh->flags & HWMOD_FORCE_MSTANDBY)) {
 			idlemode = HWMOD_IDLEMODE_FORCE;
 		} else {
 			if (sf & SYSC_HAS_ENAWAKEUP)
@@ -2154,7 +2158,7 @@ static int _enable(struct omap_hwmod *oh)
 	if (soc_ops.enable_module)
 		soc_ops.enable_module(oh);
 	if (oh->flags & HWMOD_BLOCK_WFI)
-		disable_hlt();
+		cpu_idle_poll_ctrl(true);
 
 	if (soc_ops.update_context_lost)
 		soc_ops.update_context_lost(oh);
@@ -2218,7 +2222,7 @@ static int _idle(struct omap_hwmod *oh)
 	_del_initiator_dep(oh, mpu_oh);
 
 	if (oh->flags & HWMOD_BLOCK_WFI)
-		enable_hlt();
+		cpu_idle_poll_ctrl(false);
 	if (soc_ops.disable_module)
 		soc_ops.disable_module(oh);
 
@@ -2328,7 +2332,7 @@ static int _shutdown(struct omap_hwmod *oh)
 		_del_initiator_dep(oh, mpu_oh);
 		/* XXX what about the other system initiators here? dma, dsp */
 		if (oh->flags & HWMOD_BLOCK_WFI)
-			enable_hlt();
+			cpu_idle_poll_ctrl(false);
 		if (soc_ops.disable_module)
 			soc_ops.disable_module(oh);
 		_disable_clocks(oh);
diff --git a/arch/arm/mach-omap2/omap_hwmod.h b/arch/arm/mach-omap2/omap_hwmod.h
index d43d9b608ed..d5dc935f606 100644
--- a/arch/arm/mach-omap2/omap_hwmod.h
+++ b/arch/arm/mach-omap2/omap_hwmod.h
@@ -427,8 +427,8 @@ struct omap_hwmod_omap4_prcm {
  *
  * HWMOD_SWSUP_SIDLE: omap_hwmod code should manually bring module in and out
  *     of idle, rather than relying on module smart-idle
- * HWMOD_SWSUP_MSTDBY: omap_hwmod code should manually bring module in and out
- *     of standby, rather than relying on module smart-standby
+ * HWMOD_SWSUP_MSTANDBY: omap_hwmod code should manually bring module in and
+ *     out of standby, rather than relying on module smart-standby
  * HWMOD_INIT_NO_RESET: don't reset this module at boot - important for
  *     SDRAM controller, etc. XXX probably belongs outside the main hwmod file
  *     XXX Should be HWMOD_SETUP_NO_RESET
@@ -459,6 +459,10 @@ struct omap_hwmod_omap4_prcm {
  *     correctly, or this is being abused to deal with some PM latency
  *     issues -- but we're currently suffering from a shortage of
  *     folks who are able to track these issues down properly.
+ * HWMOD_FORCE_MSTANDBY: Always keep MIDLEMODE bits cleared so that device
+ *     is kept in force-standby mode. Failing to do so causes PM problems
+ *     with musb on OMAP3630 at least. Note that musb has a dedicated register
+ *     to control MSTANDBY signal when MIDLEMODE is set to force-standby.
  */
 #define HWMOD_SWSUP_SIDLE			(1 << 0)
 #define HWMOD_SWSUP_MSTANDBY			(1 << 1)
@@ -471,6 +475,7 @@ struct omap_hwmod_omap4_prcm {
 #define HWMOD_16BIT_REG				(1 << 8)
 #define HWMOD_EXT_OPT_MAIN_CLK			(1 << 9)
 #define HWMOD_BLOCK_WFI				(1 << 10)
+#define HWMOD_FORCE_MSTANDBY			(1 << 11)
 
 /*
  * omap_hwmod._int_flags definitions
diff --git a/arch/arm/mach-omap2/omap_hwmod_3xxx_data.c b/arch/arm/mach-omap2/omap_hwmod_3xxx_data.c
index ac7e03ec952..5112d04e7b7 100644
--- a/arch/arm/mach-omap2/omap_hwmod_3xxx_data.c
+++ b/arch/arm/mach-omap2/omap_hwmod_3xxx_data.c
@@ -1707,9 +1707,14 @@ static struct omap_hwmod omap3xxx_usbhsotg_hwmod = {
 	 * Erratum ID: i479  idle_req / idle_ack mechanism potentially
 	 * broken when autoidle is enabled
 	 * workaround is to disable the autoidle bit at module level.
+	 *
+	 * Enabling the device in any other MIDLEMODE setting but force-idle
+	 * causes core_pwrdm not enter idle states at least on OMAP3630.
+	 * Note that musb has OTG_FORCESTDBY register that controls MSTANDBY
+	 * signal when MIDLEMODE is set to force-idle.
 	 */
 	.flags		= HWMOD_NO_OCP_AUTOIDLE | HWMOD_SWSUP_SIDLE
-				| HWMOD_SWSUP_MSTANDBY,
+				| HWMOD_FORCE_MSTANDBY,
 };
 
 /* usb_otg_hs */
diff --git a/arch/arm/mach-omap2/omap_hwmod_44xx_data.c b/arch/arm/mach-omap2/omap_hwmod_44xx_data.c
index 0e47d2e1687..eaba9dc91a0 100644
--- a/arch/arm/mach-omap2/omap_hwmod_44xx_data.c
+++ b/arch/arm/mach-omap2/omap_hwmod_44xx_data.c
@@ -2719,7 +2719,17 @@ static struct omap_hwmod omap44xx_ocp2scp_usb_phy_hwmod = {
 	.name		= "ocp2scp_usb_phy",
 	.class		= &omap44xx_ocp2scp_hwmod_class,
 	.clkdm_name	= "l3_init_clkdm",
-	.main_clk	= "func_48m_fclk",
+	/*
+	 * ocp2scp_usb_phy_phy_48m is provided by the OMAP4 PRCM IP
+	 * block as an "optional clock," and normally should never be
+	 * specified as the main_clk for an OMAP IP block.  However it
+	 * turns out that this clock is actually the main clock for
+	 * the ocp2scp_usb_phy IP block:
+	 * http://lists.infradead.org/pipermail/linux-arm-kernel/2012-September/119943.html
+	 * So listing ocp2scp_usb_phy_phy_48m as a main_clk here seems
+	 * to be the best workaround.
+	 */
+	.main_clk	= "ocp2scp_usb_phy_phy_48m",
 	.prcm = {
 		.omap4 = {
 			.clkctrl_offs = OMAP4_CM_L3INIT_USBPHYOCP2SCP_CLKCTRL_OFFSET,
diff --git a/arch/arm/mach-omap2/pm.c b/arch/arm/mach-omap2/pm.c
index 673a4c1d1d7..dec553349ae 100644
--- a/arch/arm/mach-omap2/pm.c
+++ b/arch/arm/mach-omap2/pm.c
@@ -218,7 +218,7 @@ static int omap_pm_enter(suspend_state_t suspend_state)
 
 static int omap_pm_begin(suspend_state_t state)
 {
-	disable_hlt();
+	cpu_idle_poll_ctrl(true);
 	if (cpu_is_omap34xx())
 		omap_prcm_irq_prepare();
 	return 0;
@@ -226,8 +226,7 @@ static int omap_pm_begin(suspend_state_t state)
 
 static void omap_pm_end(void)
 {
-	enable_hlt();
-	return;
+	cpu_idle_poll_ctrl(false);
 }
 
 static void omap_pm_finish(void)
diff --git a/arch/arm/mach-omap2/pm44xx.c b/arch/arm/mach-omap2/pm44xx.c
index ea62e75ef21..152a10cf4f1 100644
--- a/arch/arm/mach-omap2/pm44xx.c
+++ b/arch/arm/mach-omap2/pm44xx.c
@@ -126,8 +126,8 @@ static int __init pwrdms_setup(struct powerdomain *pwrdm, void *unused)
  * omap_default_idle - OMAP4 default ilde routine.'
  *
  * Implements OMAP4 memory, IO ordering requirements which can't be addressed
- * with default cpu_do_idle() hook. Used by all CPUs with !CONFIG_CPUIDLE and
- * by secondary CPU with CONFIG_CPUIDLE.
+ * with default cpu_do_idle() hook. Used by all CPUs with !CONFIG_CPU_IDLE and
+ * by secondary CPU with CONFIG_CPU_IDLE.
  */
 static void omap_default_idle(void)
 {
diff --git a/arch/arm/mach-omap2/timer.c b/arch/arm/mach-omap2/timer.c
index 2bdd4cf17a8..f62b509ed08 100644
--- a/arch/arm/mach-omap2/timer.c
+++ b/arch/arm/mach-omap2/timer.c
@@ -547,6 +547,8 @@ static inline void __init realtime_counter_init(void)
 			       clksrc_nr, clksrc_src)			\
 void __init omap##name##_gptimer_timer_init(void)			\
 {									\
+	if (omap_clk_init)						\
+		omap_clk_init();					\
 	omap_dmtimer_init();						\
 	omap2_gp_clockevent_init((clkev_nr), clkev_src, clkev_prop);	\
 	omap2_gptimer_clocksource_init((clksrc_nr), clksrc_src);	\
@@ -556,6 +558,8 @@ void __init omap##name##_gptimer_timer_init(void)			\
 				clksrc_nr, clksrc_src)			\
 void __init omap##name##_sync32k_timer_init(void)		\
 {									\
+	if (omap_clk_init)						\
+		omap_clk_init();					\
 	omap_dmtimer_init();						\
 	omap2_gp_clockevent_init((clkev_nr), clkev_src, clkev_prop);	\
 	/* Enable the use of clocksource="gp_timer" kernel parameter */	\
diff --git a/arch/arm/mach-orion5x/board-dt.c b/arch/arm/mach-orion5x/board-dt.c
index 35a8014529c..94fbb815680 100644
--- a/arch/arm/mach-orion5x/board-dt.c
+++ b/arch/arm/mach-orion5x/board-dt.c
@@ -14,6 +14,7 @@
 #include <linux/init.h>
 #include <linux/of.h>
 #include <linux/of_platform.h>
+#include <linux/cpu.h>
 #include <asm/system_misc.h>
 #include <asm/mach/arch.h>
 #include <mach/orion5x.h>
@@ -52,7 +53,7 @@ static void __init orion5x_dt_init(void)
 	 */
 	if (dev == MV88F5281_DEV_ID && rev == MV88F5281_REV_D0) {
 		printk(KERN_INFO "Orion: Applying 5281 D0 WFI workaround.\n");
-		disable_hlt();
+		cpu_idle_poll_ctrl(true);
 	}
 
 	if (of_machine_is_compatible("lacie,ethernet-disk-mini-v2"))
diff --git a/arch/arm/mach-orion5x/common.c b/arch/arm/mach-orion5x/common.c
index d068f1431c4..ad71c8a03ff 100644
--- a/arch/arm/mach-orion5x/common.c
+++ b/arch/arm/mach-orion5x/common.c
@@ -293,7 +293,7 @@ void __init orion5x_init(void)
 	 */
 	if (dev == MV88F5281_DEV_ID && rev == MV88F5281_REV_D0) {
 		printk(KERN_INFO "Orion: Applying 5281 D0 WFI workaround.\n");
-		disable_hlt();
+		cpu_idle_poll_ctrl(true);
 	}
 
 	/*
diff --git a/arch/arm/mach-s3c24xx/clock-s3c2440.c b/arch/arm/mach-s3c24xx/clock-s3c2440.c
index 04b87ec9253..1069b568082 100644
--- a/arch/arm/mach-s3c24xx/clock-s3c2440.c
+++ b/arch/arm/mach-s3c24xx/clock-s3c2440.c
@@ -123,6 +123,11 @@ static struct clk s3c2440_clk_ac97 = {
 	.ctrlbit	= S3C2440_CLKCON_AC97,
 };
 
+#define S3C24XX_VA_UART0      (S3C_VA_UART)
+#define S3C24XX_VA_UART1      (S3C_VA_UART + 0x4000 )
+#define S3C24XX_VA_UART2      (S3C_VA_UART + 0x8000 )
+#define S3C24XX_VA_UART3      (S3C_VA_UART + 0xC000 )
+
 static unsigned long  s3c2440_fclk_n_getrate(struct clk *clk)
 {
 	unsigned long ucon0, ucon1, ucon2, divisor;
diff --git a/arch/arm/mach-s3c24xx/common.c b/arch/arm/mach-s3c24xx/common.c
index 6bcf87f65f9..92e609440c5 100644
--- a/arch/arm/mach-s3c24xx/common.c
+++ b/arch/arm/mach-s3c24xx/common.c
@@ -239,6 +239,11 @@ void __init s3c24xx_init_io(struct map_desc *mach_desc, int size)
 
 /* Serial port registrations */
 
+#define S3C2410_PA_UART0      (S3C24XX_PA_UART)
+#define S3C2410_PA_UART1      (S3C24XX_PA_UART + 0x4000 )
+#define S3C2410_PA_UART2      (S3C24XX_PA_UART + 0x8000 )
+#define S3C2443_PA_UART3      (S3C24XX_PA_UART + 0xC000 )
+
 static struct resource s3c2410_uart0_resource[] = {
 	[0] = DEFINE_RES_MEM(S3C2410_PA_UART0, SZ_16K),
 	[1] = DEFINE_RES_NAMED(IRQ_S3CUART_RX0, \
diff --git a/arch/arm/mach-s3c24xx/include/mach/irqs.h b/arch/arm/mach-s3c24xx/include/mach/irqs.h
index b7a9f4d469e..1e73f5fa865 100644
--- a/arch/arm/mach-s3c24xx/include/mach/irqs.h
+++ b/arch/arm/mach-s3c24xx/include/mach/irqs.h
@@ -188,10 +188,8 @@
 
 #if defined(CONFIG_CPU_S3C2416)
 #define NR_IRQS (IRQ_S3C2416_I2S1 + 1)
-#elif defined(CONFIG_CPU_S3C2443)
-#define NR_IRQS (IRQ_S3C2443_AC97+1)
 #else
-#define NR_IRQS (IRQ_S3C2440_AC97+1)
+#define NR_IRQS (IRQ_S3C2443_AC97 + 1)
 #endif
 
 /* compatibility define. */
diff --git a/arch/arm/mach-s3c24xx/irq.c b/arch/arm/mach-s3c24xx/irq.c
index cb9f5e011e7..d8ba9bee4c7 100644
--- a/arch/arm/mach-s3c24xx/irq.c
+++ b/arch/arm/mach-s3c24xx/irq.c
@@ -500,7 +500,7 @@ struct s3c_irq_intc *s3c24xx_init_intc(struct device_node *np,
 		base = (void *)0xfd000000;
 
 		intc->reg_mask = base + 0xa4;
-		intc->reg_pending = base + 0x08;
+		intc->reg_pending = base + 0xa8;
 		irq_num = 20;
 		irq_start = S3C2410_IRQ(32);
 		irq_offset = 4;
diff --git a/arch/arm/mach-s3c64xx/setup-usb-phy.c b/arch/arm/mach-s3c64xx/setup-usb-phy.c
index c8174d95339..ca960bda02f 100644
--- a/arch/arm/mach-s3c64xx/setup-usb-phy.c
+++ b/arch/arm/mach-s3c64xx/setup-usb-phy.c
@@ -76,7 +76,7 @@ static int s3c_usb_otgphy_exit(struct platform_device *pdev)
 
 int s5p_usb_phy_init(struct platform_device *pdev, int type)
 {
-	if (type == S5P_USB_PHY_DEVICE)
+	if (type == USB_PHY_TYPE_DEVICE)
 		return s3c_usb_otgphy_init(pdev);
 
 	return -EINVAL;
@@ -84,7 +84,7 @@ int s5p_usb_phy_init(struct platform_device *pdev, int type)
 
 int s5p_usb_phy_exit(struct platform_device *pdev, int type)
 {
-	if (type == S5P_USB_PHY_DEVICE)
+	if (type == USB_PHY_TYPE_DEVICE)
 		return s3c_usb_otgphy_exit(pdev);
 
 	return -EINVAL;
diff --git a/arch/arm/mach-s5pv210/setup-usb-phy.c b/arch/arm/mach-s5pv210/setup-usb-phy.c
index 356a0900af0..b2ee5333f89 100644
--- a/arch/arm/mach-s5pv210/setup-usb-phy.c
+++ b/arch/arm/mach-s5pv210/setup-usb-phy.c
@@ -80,7 +80,7 @@ static int s5pv210_usb_otgphy_exit(struct platform_device *pdev)
 
 int s5p_usb_phy_init(struct platform_device *pdev, int type)
 {
-	if (type == S5P_USB_PHY_DEVICE)
+	if (type == USB_PHY_TYPE_DEVICE)
 		return s5pv210_usb_otgphy_init(pdev);
 
 	return -EINVAL;
@@ -88,7 +88,7 @@ int s5p_usb_phy_init(struct platform_device *pdev, int type)
 
 int s5p_usb_phy_exit(struct platform_device *pdev, int type)
 {
-	if (type == S5P_USB_PHY_DEVICE)
+	if (type == USB_PHY_TYPE_DEVICE)
 		return s5pv210_usb_otgphy_exit(pdev);
 
 	return -EINVAL;
diff --git a/arch/arm/mach-shark/core.c b/arch/arm/mach-shark/core.c
index b63dec84819..15355572498 100644
--- a/arch/arm/mach-shark/core.c
+++ b/arch/arm/mach-shark/core.c
@@ -10,6 +10,7 @@
 #include <linux/sched.h>
 #include <linux/serial_8250.h>
 #include <linux/io.h>
+#include <linux/cpu.h>
 
 #include <asm/setup.h>
 #include <asm/mach-types.h>
@@ -130,7 +131,7 @@ static void __init shark_timer_init(void)
 
 static void shark_init_early(void)
 {
-	disable_hlt();
+	cpu_idle_poll_ctrl(true);
 }
 
 MACHINE_START(SHARK, "Shark")
diff --git a/arch/arm/mach-shmobile/board-armadillo800eva.c b/arch/arm/mach-shmobile/board-armadillo800eva.c
index f2ec0777cfb..ff8b7ba9b93 100644
--- a/arch/arm/mach-shmobile/board-armadillo800eva.c
+++ b/arch/arm/mach-shmobile/board-armadillo800eva.c
@@ -169,7 +169,7 @@ static int usbhsf_get_id(struct platform_device *pdev)
 	return USBHS_GADGET;
 }
 
-static void usbhsf_power_ctrl(struct platform_device *pdev,
+static int usbhsf_power_ctrl(struct platform_device *pdev,
 			      void __iomem *base, int enable)
 {
 	struct usbhsf_private *priv = usbhsf_get_priv(pdev);
@@ -223,6 +223,8 @@ static void usbhsf_power_ctrl(struct platform_device *pdev,
 		clk_disable(priv->pci);		/* usb work around */
 		clk_disable(priv->usb24);	/* usb work around */
 	}
+
+	return 0;
 }
 
 static int usbhsf_get_vbus(struct platform_device *pdev)
@@ -239,7 +241,7 @@ static irqreturn_t usbhsf_interrupt(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
-static void usbhsf_hardware_exit(struct platform_device *pdev)
+static int usbhsf_hardware_exit(struct platform_device *pdev)
 {
 	struct usbhsf_private *priv = usbhsf_get_priv(pdev);
 
@@ -264,6 +266,8 @@ static void usbhsf_hardware_exit(struct platform_device *pdev)
 	priv->usbh_base	= NULL;
 
 	free_irq(IRQ7, pdev);
+
+	return 0;
 }
 
 static int usbhsf_hardware_init(struct platform_device *pdev)
diff --git a/arch/arm/mach-shmobile/board-kzm9g.c b/arch/arm/mach-shmobile/board-kzm9g.c
index 7f3a6b7e7b7..a385f570bbf 100644
--- a/arch/arm/mach-shmobile/board-kzm9g.c
+++ b/arch/arm/mach-shmobile/board-kzm9g.c
@@ -155,12 +155,14 @@ static int usbhs_get_vbus(struct platform_device *pdev)
 	return !((1 << 7) & __raw_readw(priv->cr2));
 }
 
-static void usbhs_phy_reset(struct platform_device *pdev)
+static int usbhs_phy_reset(struct platform_device *pdev)
 {
 	struct usbhs_private *priv = usbhs_get_priv(pdev);
 
 	/* init phy */
 	__raw_writew(0x8a0a, priv->cr2);
+
+	return 0;
 }
 
 static int usbhs_get_id(struct platform_device *pdev)
@@ -202,7 +204,7 @@ static int usbhs_hardware_init(struct platform_device *pdev)
 	return 0;
 }
 
-static void usbhs_hardware_exit(struct platform_device *pdev)
+static int usbhs_hardware_exit(struct platform_device *pdev)
 {
 	struct usbhs_private *priv = usbhs_get_priv(pdev);
 
@@ -210,6 +212,8 @@ static void usbhs_hardware_exit(struct platform_device *pdev)
 	__raw_writew(USB_PHY_MODE | USB_PHY_INT_CLR, priv->phy);
 
 	free_irq(IRQ15, pdev);
+
+	return 0;
 }
 
 static u32 usbhs_pipe_cfg[] = {
diff --git a/arch/arm/mach-shmobile/board-mackerel.c b/arch/arm/mach-shmobile/board-mackerel.c
index db968a585ff..979237c18da 100644
--- a/arch/arm/mach-shmobile/board-mackerel.c
+++ b/arch/arm/mach-shmobile/board-mackerel.c
@@ -596,12 +596,14 @@ static int usbhs_get_vbus(struct platform_device *pdev)
 	return usbhs_is_connected(usbhs_get_priv(pdev));
 }
 
-static void usbhs_phy_reset(struct platform_device *pdev)
+static int usbhs_phy_reset(struct platform_device *pdev)
 {
 	struct usbhs_private *priv = usbhs_get_priv(pdev);
 
 	/* init phy */
 	__raw_writew(0x8a0a, priv->usbcrcaddr);
+
+	return 0;
 }
 
 static int usbhs0_get_id(struct platform_device *pdev)
@@ -628,11 +630,13 @@ static int usbhs0_hardware_init(struct platform_device *pdev)
 	return 0;
 }
 
-static void usbhs0_hardware_exit(struct platform_device *pdev)
+static int usbhs0_hardware_exit(struct platform_device *pdev)
 {
 	struct usbhs_private *priv = usbhs_get_priv(pdev);
 
 	cancel_delayed_work_sync(&priv->work);
+
+	return 0;
 }
 
 static struct usbhs_private usbhs0_private = {
@@ -735,7 +739,7 @@ static int usbhs1_hardware_init(struct platform_device *pdev)
 	return 0;
 }
 
-static void usbhs1_hardware_exit(struct platform_device *pdev)
+static int usbhs1_hardware_exit(struct platform_device *pdev)
 {
 	struct usbhs_private *priv = usbhs_get_priv(pdev);
 
@@ -743,6 +747,8 @@ static void usbhs1_hardware_exit(struct platform_device *pdev)
 	__raw_writew(USB_PHY_MODE | USB_PHY_INT_CLR, priv->usbphyaddr);
 
 	free_irq(IRQ8, pdev);
+
+	return 0;
 }
 
 static int usbhs1_get_id(struct platform_device *pdev)
diff --git a/arch/arm/mach-shmobile/suspend.c b/arch/arm/mach-shmobile/suspend.c
index 47d83f7a70b..5d92b5dd486 100644
--- a/arch/arm/mach-shmobile/suspend.c
+++ b/arch/arm/mach-shmobile/suspend.c
@@ -12,6 +12,8 @@
 #include <linux/suspend.h>
 #include <linux/module.h>
 #include <linux/err.h>
+#include <linux/cpu.h>
+
 #include <asm/io.h>
 #include <asm/system_misc.h>
 
@@ -23,13 +25,13 @@ static int shmobile_suspend_default_enter(suspend_state_t suspend_state)
 
 static int shmobile_suspend_begin(suspend_state_t state)
 {
-	disable_hlt();
+	cpu_idle_poll_ctrl(true);
 	return 0;
 }
 
 static void shmobile_suspend_end(void)
 {
-	enable_hlt();
+	cpu_idle_poll_ctrl(false);
 }
 
 struct platform_suspend_ops shmobile_suspend_ops = {
diff --git a/arch/arm/mach-tegra/Kconfig b/arch/arm/mach-tegra/Kconfig
index d1c4893894c..dbc653ea851 100644
--- a/arch/arm/mach-tegra/Kconfig
+++ b/arch/arm/mach-tegra/Kconfig
@@ -18,8 +18,8 @@ config ARCH_TEGRA_2x_SOC
 	select PL310_ERRATA_727915 if CACHE_L2X0
 	select PL310_ERRATA_769419 if CACHE_L2X0
 	select USB_ARCH_HAS_EHCI if USB_SUPPORT
-	select USB_ULPI if USB
-	select USB_ULPI_VIEWPORT if USB_SUPPORT
+	select USB_ULPI if USB_PHY
+	select USB_ULPI_VIEWPORT if USB_PHY
 	help
 	  Support for NVIDIA Tegra AP20 and T20 processors, based on the
 	  ARM CortexA9MP CPU and the ARM PL310 L2 cache controller
@@ -37,8 +37,8 @@ config ARCH_TEGRA_3x_SOC
 	select PINCTRL_TEGRA30
 	select PL310_ERRATA_769419 if CACHE_L2X0
 	select USB_ARCH_HAS_EHCI if USB_SUPPORT
-	select USB_ULPI if USB
-	select USB_ULPI_VIEWPORT if USB_SUPPORT
+	select USB_ULPI if USB_PHY
+	select USB_ULPI_VIEWPORT if USB_PHY
 	help
 	  Support for NVIDIA Tegra T30 processor family, based on the
 	  ARM CortexA9MP CPU and the ARM PL310 L2 cache controller
diff --git a/arch/arm/mach-ux500/board-mop500-regulators.c b/arch/arm/mach-ux500/board-mop500-regulators.c
index 2a17bc506cf..ff3c9f01659 100644
--- a/arch/arm/mach-ux500/board-mop500-regulators.c
+++ b/arch/arm/mach-ux500/board-mop500-regulators.c
@@ -5,6 +5,7 @@
  *
  * Authors: Sundar Iyer <sundar.iyer@stericsson.com>
  *          Bengt Jonsson <bengt.g.jonsson@stericsson.com>
+ *          Daniel Willerud <daniel.willerud@stericsson.com>
  *
  * MOP500 board specific initialization for regulators
  */
@@ -12,6 +13,7 @@
 #include <linux/regulator/machine.h>
 #include <linux/regulator/ab8500.h>
 #include "board-mop500-regulators.h"
+#include "id.h"
 
 static struct regulator_consumer_supply gpio_en_3v3_consumers[] = {
        REGULATOR_SUPPLY("vdd33a", "smsc911x.0"),
@@ -53,21 +55,37 @@ struct regulator_init_data tps61052_regulator = {
 };
 
 static struct regulator_consumer_supply ab8500_vaux1_consumers[] = {
-	/* External displays, connector on board 2v5 power supply */
-	REGULATOR_SUPPLY("vaux12v5", "mcde.0"),
+	/* Main display, u8500 R3 uib */
+	REGULATOR_SUPPLY("vddi", "mcde_disp_sony_acx424akp.0"),
+	/* Main display, u8500 uib and ST uib */
+	REGULATOR_SUPPLY("vdd1", "samsung_s6d16d0.0"),
+	/* Secondary display, ST uib */
+	REGULATOR_SUPPLY("vdd1", "samsung_s6d16d0.1"),
 	/* SFH7741 proximity sensor */
 	REGULATOR_SUPPLY("vcc", "gpio-keys.0"),
 	/* BH1780GLS ambient light sensor */
 	REGULATOR_SUPPLY("vcc", "2-0029"),
 	/* lsm303dlh accelerometer */
-	REGULATOR_SUPPLY("vdd", "3-0018"),
+	REGULATOR_SUPPLY("vdd", "2-0018"),
+	/* lsm303dlhc accelerometer */
+	REGULATOR_SUPPLY("vdd", "2-0019"),
 	/* lsm303dlh magnetometer */
-	REGULATOR_SUPPLY("vdd", "3-001e"),
+	REGULATOR_SUPPLY("vdd", "2-001e"),
 	/* Rohm BU21013 Touchscreen devices */
 	REGULATOR_SUPPLY("avdd", "3-005c"),
 	REGULATOR_SUPPLY("avdd", "3-005d"),
 	/* Synaptics RMI4 Touchscreen device */
 	REGULATOR_SUPPLY("vdd", "3-004b"),
+	/* L3G4200D Gyroscope device */
+	REGULATOR_SUPPLY("vdd", "2-0068"),
+	/* Ambient light sensor device */
+	REGULATOR_SUPPLY("vdd", "3-0029"),
+	/* Pressure sensor device */
+	REGULATOR_SUPPLY("vdd", "2-005c"),
+	/* Cypress TrueTouch Touchscreen device */
+	REGULATOR_SUPPLY("vcpin", "spi8.0"),
+	/* Camera device */
+	REGULATOR_SUPPLY("vaux12v5", "mmio_camera"),
 };
 
 static struct regulator_consumer_supply ab8500_vaux2_consumers[] = {
@@ -75,18 +93,50 @@ static struct regulator_consumer_supply ab8500_vaux2_consumers[] = {
 	REGULATOR_SUPPLY("vmmc", "sdi4"),
 	/* AB8500 audio codec */
 	REGULATOR_SUPPLY("vcc-N2158", "ab8500-codec.0"),
+	/* AB8500 accessory detect 1 */
+	REGULATOR_SUPPLY("vcc-N2158", "ab8500-acc-det.0"),
+	/* AB8500 Tv-out device */
+	REGULATOR_SUPPLY("vcc-N2158", "mcde_tv_ab8500.4"),
+	/* AV8100 HDMI device */
+	REGULATOR_SUPPLY("vcc-N2158", "av8100_hdmi.3"),
 };
 
 static struct regulator_consumer_supply ab8500_vaux3_consumers[] = {
+	REGULATOR_SUPPLY("v-SD-STM", "stm"),
 	/* External MMC slot power */
 	REGULATOR_SUPPLY("vmmc", "sdi0"),
 };
 
+static struct regulator_consumer_supply ab8505_vaux4_consumers[] = {
+};
+
+static struct regulator_consumer_supply ab8505_vaux5_consumers[] = {
+};
+
+static struct regulator_consumer_supply ab8505_vaux6_consumers[] = {
+};
+
+static struct regulator_consumer_supply ab8505_vaux8_consumers[] = {
+	/* AB8500 audio codec device */
+	REGULATOR_SUPPLY("v-aux8", NULL),
+};
+
+static struct regulator_consumer_supply ab8505_vadc_consumers[] = {
+	/* Internal general-purpose ADC */
+	REGULATOR_SUPPLY("vddadc", "ab8500-gpadc.0"),
+	/* ADC for charger */
+	REGULATOR_SUPPLY("vddadc", "ab8500-charger.0"),
+};
+
 static struct regulator_consumer_supply ab8500_vtvout_consumers[] = {
 	/* TV-out DENC supply */
 	REGULATOR_SUPPLY("vtvout", "ab8500-denc.0"),
 	/* Internal general-purpose ADC */
 	REGULATOR_SUPPLY("vddadc", "ab8500-gpadc.0"),
+	/* ADC for charger */
+	REGULATOR_SUPPLY("vddadc", "ab8500-charger.0"),
+	/* AB8500 Tv-out device */
+	REGULATOR_SUPPLY("vtvout", "mcde_tv_ab8500.4"),
 };
 
 static struct regulator_consumer_supply ab8500_vaud_consumers[] = {
@@ -114,77 +164,90 @@ static struct regulator_consumer_supply ab8500_vintcore_consumers[] = {
 	REGULATOR_SUPPLY("v-intcore", NULL),
 	/* USB Transceiver */
 	REGULATOR_SUPPLY("vddulpivio18", "ab8500-usb.0"),
+	/* Handled by abx500 clk driver */
+	REGULATOR_SUPPLY("v-intcore", "abx500-clk.0"),
+};
+
+static struct regulator_consumer_supply ab8505_usb_consumers[] = {
+	/* HS USB OTG physical interface */
+	REGULATOR_SUPPLY("v-ape", NULL),
 };
 
 static struct regulator_consumer_supply ab8500_vana_consumers[] = {
-	/* External displays, connector on board, 1v8 power supply */
-	REGULATOR_SUPPLY("vsmps2", "mcde.0"),
+	/* DB8500 DSI */
+	REGULATOR_SUPPLY("vdddsi1v2", "mcde"),
+	REGULATOR_SUPPLY("vdddsi1v2", "b2r2_core"),
+	REGULATOR_SUPPLY("vdddsi1v2", "b2r2_1_core"),
+	REGULATOR_SUPPLY("vdddsi1v2", "dsilink.0"),
+	REGULATOR_SUPPLY("vdddsi1v2", "dsilink.1"),
+	REGULATOR_SUPPLY("vdddsi1v2", "dsilink.2"),
+	/* DB8500 CSI */
+	REGULATOR_SUPPLY("vddcsi1v2", "mmio_camera"),
 };
 
 /* ab8500 regulator register initialization */
-struct ab8500_regulator_reg_init
-ab8500_regulator_reg_init[AB8500_NUM_REGULATOR_REGISTERS] = {
+static struct ab8500_regulator_reg_init ab8500_reg_init[] = {
 	/*
 	 * VanaRequestCtrl          = HP/LP depending on VxRequest
 	 * VextSupply1RequestCtrl   = HP/LP depending on VxRequest
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_REGUREQUESTCTRL2, 0x00),
+	INIT_REGULATOR_REGISTER(AB8500_REGUREQUESTCTRL2,       0xf0, 0x00),
 	/*
 	 * VextSupply2RequestCtrl   = HP/LP depending on VxRequest
 	 * VextSupply3RequestCtrl   = HP/LP depending on VxRequest
 	 * Vaux1RequestCtrl         = HP/LP depending on VxRequest
 	 * Vaux2RequestCtrl         = HP/LP depending on VxRequest
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_REGUREQUESTCTRL3, 0x00),
+	INIT_REGULATOR_REGISTER(AB8500_REGUREQUESTCTRL3,       0xff, 0x00),
 	/*
 	 * Vaux3RequestCtrl         = HP/LP depending on VxRequest
 	 * SwHPReq                  = Control through SWValid disabled
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_REGUREQUESTCTRL4, 0x00),
+	INIT_REGULATOR_REGISTER(AB8500_REGUREQUESTCTRL4,       0x07, 0x00),
 	/*
 	 * VanaSysClkReq1HPValid    = disabled
 	 * Vaux1SysClkReq1HPValid   = disabled
 	 * Vaux2SysClkReq1HPValid   = disabled
 	 * Vaux3SysClkReq1HPValid   = disabled
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_REGUSYSCLKREQ1HPVALID1, 0x00),
+	INIT_REGULATOR_REGISTER(AB8500_REGUSYSCLKREQ1HPVALID1, 0xe8, 0x00),
 	/*
 	 * VextSupply1SysClkReq1HPValid = disabled
 	 * VextSupply2SysClkReq1HPValid = disabled
 	 * VextSupply3SysClkReq1HPValid = SysClkReq1 controlled
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_REGUSYSCLKREQ1HPVALID2, 0x40),
+	INIT_REGULATOR_REGISTER(AB8500_REGUSYSCLKREQ1HPVALID2, 0x70, 0x40),
 	/*
 	 * VanaHwHPReq1Valid        = disabled
 	 * Vaux1HwHPreq1Valid       = disabled
 	 * Vaux2HwHPReq1Valid       = disabled
 	 * Vaux3HwHPReqValid        = disabled
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_REGUHWHPREQ1VALID1, 0x00),
+	INIT_REGULATOR_REGISTER(AB8500_REGUHWHPREQ1VALID1,     0xe8, 0x00),
 	/*
 	 * VextSupply1HwHPReq1Valid = disabled
 	 * VextSupply2HwHPReq1Valid = disabled
 	 * VextSupply3HwHPReq1Valid = disabled
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_REGUHWHPREQ1VALID2, 0x00),
+	INIT_REGULATOR_REGISTER(AB8500_REGUHWHPREQ1VALID2,     0x07, 0x00),
 	/*
 	 * VanaHwHPReq2Valid        = disabled
 	 * Vaux1HwHPReq2Valid       = disabled
 	 * Vaux2HwHPReq2Valid       = disabled
 	 * Vaux3HwHPReq2Valid       = disabled
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_REGUHWHPREQ2VALID1, 0x00),
+	INIT_REGULATOR_REGISTER(AB8500_REGUHWHPREQ2VALID1,     0xe8, 0x00),
 	/*
 	 * VextSupply1HwHPReq2Valid = disabled
 	 * VextSupply2HwHPReq2Valid = disabled
 	 * VextSupply3HwHPReq2Valid = HWReq2 controlled
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_REGUHWHPREQ2VALID2, 0x04),
+	INIT_REGULATOR_REGISTER(AB8500_REGUHWHPREQ2VALID2,     0x07, 0x04),
 	/*
 	 * VanaSwHPReqValid         = disabled
 	 * Vaux1SwHPReqValid        = disabled
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_REGUSWHPREQVALID1, 0x00),
+	INIT_REGULATOR_REGISTER(AB8500_REGUSWHPREQVALID1,      0xa0, 0x00),
 	/*
 	 * Vaux2SwHPReqValid        = disabled
 	 * Vaux3SwHPReqValid        = disabled
@@ -192,7 +255,7 @@ ab8500_regulator_reg_init[AB8500_NUM_REGULATOR_REGISTERS] = {
 	 * VextSupply2SwHPReqValid  = disabled
 	 * VextSupply3SwHPReqValid  = disabled
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_REGUSWHPREQVALID2, 0x00),
+	INIT_REGULATOR_REGISTER(AB8500_REGUSWHPREQVALID2,      0x1f, 0x00),
 	/*
 	 * SysClkReq2Valid1         = SysClkReq2 controlled
 	 * SysClkReq3Valid1         = disabled
@@ -202,7 +265,7 @@ ab8500_regulator_reg_init[AB8500_NUM_REGULATOR_REGISTERS] = {
 	 * SysClkReq7Valid1         = disabled
 	 * SysClkReq8Valid1         = disabled
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_REGUSYSCLKREQVALID1, 0x2a),
+	INIT_REGULATOR_REGISTER(AB8500_REGUSYSCLKREQVALID1,    0xfe, 0x2a),
 	/*
 	 * SysClkReq2Valid2         = disabled
 	 * SysClkReq3Valid2         = disabled
@@ -212,7 +275,7 @@ ab8500_regulator_reg_init[AB8500_NUM_REGULATOR_REGISTERS] = {
 	 * SysClkReq7Valid2         = disabled
 	 * SysClkReq8Valid2         = disabled
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_REGUSYSCLKREQVALID2, 0x20),
+	INIT_REGULATOR_REGISTER(AB8500_REGUSYSCLKREQVALID2,    0xfe, 0x20),
 	/*
 	 * VTVoutEna                = disabled
 	 * Vintcore12Ena            = disabled
@@ -220,66 +283,62 @@ ab8500_regulator_reg_init[AB8500_NUM_REGULATOR_REGISTERS] = {
 	 * Vintcore12LP             = inactive (HP)
 	 * VTVoutLP                 = inactive (HP)
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_REGUMISC1, 0x10),
+	INIT_REGULATOR_REGISTER(AB8500_REGUMISC1,              0xfe, 0x10),
 	/*
 	 * VaudioEna                = disabled
 	 * VdmicEna                 = disabled
 	 * Vamic1Ena                = disabled
 	 * Vamic2Ena                = disabled
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_VAUDIOSUPPLY, 0x00),
+	INIT_REGULATOR_REGISTER(AB8500_VAUDIOSUPPLY,           0x1e, 0x00),
 	/*
 	 * Vamic1_dzout             = high-Z when Vamic1 is disabled
 	 * Vamic2_dzout             = high-Z when Vamic2 is disabled
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_REGUCTRL1VAMIC, 0x00),
+	INIT_REGULATOR_REGISTER(AB8500_REGUCTRL1VAMIC,         0x03, 0x00),
 	/*
-	 * VPll                     = Hw controlled
+	 * VPll                     = Hw controlled (NOTE! PRCMU bits)
 	 * VanaRegu                 = force off
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_VPLLVANAREGU, 0x02),
+	INIT_REGULATOR_REGISTER(AB8500_VPLLVANAREGU,           0x0f, 0x02),
 	/*
 	 * VrefDDREna               = disabled
 	 * VrefDDRSleepMode         = inactive (no pulldown)
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_VREFDDR, 0x00),
+	INIT_REGULATOR_REGISTER(AB8500_VREFDDR,                0x03, 0x00),
 	/*
-	 * VextSupply1Regu          = HW control
-	 * VextSupply2Regu          = HW control
-	 * VextSupply3Regu          = HW control
+	 * VextSupply1Regu          = force LP
+	 * VextSupply2Regu          = force OFF
+	 * VextSupply3Regu          = force HP (-> STBB2=LP and TPS=LP)
 	 * ExtSupply2Bypass         = ExtSupply12LPn ball is 0 when Ena is 0
 	 * ExtSupply3Bypass         = ExtSupply3LPn ball is 0 when Ena is 0
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_EXTSUPPLYREGU, 0x2a),
+	INIT_REGULATOR_REGISTER(AB8500_EXTSUPPLYREGU,          0xff, 0x13),
 	/*
 	 * Vaux1Regu                = force HP
 	 * Vaux2Regu                = force off
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_VAUX12REGU, 0x01),
+	INIT_REGULATOR_REGISTER(AB8500_VAUX12REGU,             0x0f, 0x01),
 	/*
-	 * Vaux3regu                = force off
+	 * Vaux3Regu                = force off
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_VRF1VAUX3REGU, 0x00),
+	INIT_REGULATOR_REGISTER(AB8500_VRF1VAUX3REGU,          0x03, 0x00),
 	/*
-	 * Vsmps1                   = 1.15V
+	 * Vaux1Sel                 = 2.8 V
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_VSMPS1SEL1, 0x24),
-	/*
-	 * Vaux1Sel                 = 2.5 V
-	 */
-	INIT_REGULATOR_REGISTER(AB8500_VAUX1SEL, 0x08),
+	INIT_REGULATOR_REGISTER(AB8500_VAUX1SEL,               0x0f, 0x0C),
 	/*
 	 * Vaux2Sel                 = 2.9 V
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_VAUX2SEL, 0x0d),
+	INIT_REGULATOR_REGISTER(AB8500_VAUX2SEL,               0x0f, 0x0d),
 	/*
 	 * Vaux3Sel                 = 2.91 V
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_VRF1VAUX3SEL, 0x07),
+	INIT_REGULATOR_REGISTER(AB8500_VRF1VAUX3SEL,           0x07, 0x07),
 	/*
 	 * VextSupply12LP           = disabled (no LP)
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_REGUCTRL2SPARE, 0x00),
+	INIT_REGULATOR_REGISTER(AB8500_REGUCTRL2SPARE,         0x01, 0x00),
 	/*
 	 * Vaux1Disch               = short discharge time
 	 * Vaux2Disch               = short discharge time
@@ -288,33 +347,26 @@ ab8500_regulator_reg_init[AB8500_NUM_REGULATOR_REGISTERS] = {
 	 * VTVoutDisch              = short discharge time
 	 * VaudioDisch              = short discharge time
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_REGUCTRLDISCH, 0x00),
+	INIT_REGULATOR_REGISTER(AB8500_REGUCTRLDISCH,          0xfc, 0x00),
 	/*
 	 * VanaDisch                = short discharge time
 	 * VdmicPullDownEna         = pulldown disabled when Vdmic is disabled
 	 * VdmicDisch               = short discharge time
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_REGUCTRLDISCH2, 0x00),
+	INIT_REGULATOR_REGISTER(AB8500_REGUCTRLDISCH2,         0x16, 0x00),
 };
 
 /* AB8500 regulators */
-struct regulator_init_data ab8500_regulators[AB8500_NUM_REGULATORS] = {
+static struct regulator_init_data ab8500_regulators[AB8500_NUM_REGULATORS] = {
 	/* supplies to the display/camera */
 	[AB8500_LDO_AUX1] = {
 		.constraints = {
 			.name = "V-DISPLAY",
-			.min_uV = 2500000,
-			.max_uV = 2900000,
+			.min_uV = 2800000,
+			.max_uV = 3300000,
 			.valid_ops_mask = REGULATOR_CHANGE_VOLTAGE |
 					  REGULATOR_CHANGE_STATUS,
 			.boot_on = 1, /* display is on at boot */
-			/*
-			 * This voltage cannot be disabled right now because
-			 * it is somehow affecting the external MMC
-			 * functionality, though that typically will use
-			 * AUX3.
-			 */
-			.always_on = 1,
 		},
 		.num_consumer_supplies = ARRAY_SIZE(ab8500_vaux1_consumers),
 		.consumer_supplies = ab8500_vaux1_consumers,
@@ -326,7 +378,10 @@ struct regulator_init_data ab8500_regulators[AB8500_NUM_REGULATORS] = {
 			.min_uV = 1100000,
 			.max_uV = 3300000,
 			.valid_ops_mask = REGULATOR_CHANGE_VOLTAGE |
-					  REGULATOR_CHANGE_STATUS,
+					  REGULATOR_CHANGE_STATUS |
+					  REGULATOR_CHANGE_MODE,
+			.valid_modes_mask = REGULATOR_MODE_NORMAL |
+					    REGULATOR_MODE_IDLE,
 		},
 		.num_consumer_supplies = ARRAY_SIZE(ab8500_vaux2_consumers),
 		.consumer_supplies = ab8500_vaux2_consumers,
@@ -338,7 +393,10 @@ struct regulator_init_data ab8500_regulators[AB8500_NUM_REGULATORS] = {
 			.min_uV = 1100000,
 			.max_uV = 3300000,
 			.valid_ops_mask = REGULATOR_CHANGE_VOLTAGE |
-					  REGULATOR_CHANGE_STATUS,
+					  REGULATOR_CHANGE_STATUS |
+					  REGULATOR_CHANGE_MODE,
+			.valid_modes_mask = REGULATOR_MODE_NORMAL |
+					    REGULATOR_MODE_IDLE,
 		},
 		.num_consumer_supplies = ARRAY_SIZE(ab8500_vaux3_consumers),
 		.consumer_supplies = ab8500_vaux3_consumers,
@@ -392,18 +450,614 @@ struct regulator_init_data ab8500_regulators[AB8500_NUM_REGULATORS] = {
 	[AB8500_LDO_INTCORE] = {
 		.constraints = {
 			.name = "V-INTCORE",
-			.valid_ops_mask = REGULATOR_CHANGE_STATUS,
+			.min_uV = 1250000,
+			.max_uV = 1350000,
+			.input_uV = 1800000,
+			.valid_ops_mask = REGULATOR_CHANGE_VOLTAGE |
+					  REGULATOR_CHANGE_STATUS |
+					  REGULATOR_CHANGE_MODE |
+					  REGULATOR_CHANGE_DRMS,
+			.valid_modes_mask = REGULATOR_MODE_NORMAL |
+					    REGULATOR_MODE_IDLE,
 		},
 		.num_consumer_supplies = ARRAY_SIZE(ab8500_vintcore_consumers),
 		.consumer_supplies = ab8500_vintcore_consumers,
 	},
-	/* supply for U8500 CSI/DSI, VANA LDO */
+	/* supply for U8500 CSI-DSI, VANA LDO */
 	[AB8500_LDO_ANA] = {
 		.constraints = {
-			.name = "V-CSI/DSI",
+			.name = "V-CSI-DSI",
 			.valid_ops_mask = REGULATOR_CHANGE_STATUS,
 		},
 		.num_consumer_supplies = ARRAY_SIZE(ab8500_vana_consumers),
 		.consumer_supplies = ab8500_vana_consumers,
 	},
 };
+
+/* supply for VextSupply3 */
+static struct regulator_consumer_supply ab8500_ext_supply3_consumers[] = {
+	/* SIM supply for 3 V SIM cards */
+	REGULATOR_SUPPLY("vinvsim", "sim-detect.0"),
+};
+
+/* extended configuration for VextSupply2, only used for HREFP_V20 boards */
+static struct ab8500_ext_regulator_cfg ab8500_ext_supply2 = {
+	.hwreq = true,
+};
+
+/*
+ * AB8500 external regulators
+ */
+static struct regulator_init_data ab8500_ext_regulators[] = {
+	/* fixed Vbat supplies VSMPS1_EXT_1V8 */
+	[AB8500_EXT_SUPPLY1] = {
+		.constraints = {
+			.name = "ab8500-ext-supply1",
+			.min_uV = 1800000,
+			.max_uV = 1800000,
+			.initial_mode = REGULATOR_MODE_IDLE,
+			.boot_on = 1,
+			.always_on = 1,
+		},
+	},
+	/* fixed Vbat supplies VSMPS2_EXT_1V36 and VSMPS5_EXT_1V15 */
+	[AB8500_EXT_SUPPLY2] = {
+		.constraints = {
+			.name = "ab8500-ext-supply2",
+			.min_uV = 1360000,
+			.max_uV = 1360000,
+		},
+	},
+	/* fixed Vbat supplies VSMPS3_EXT_3V4 and VSMPS4_EXT_3V4 */
+	[AB8500_EXT_SUPPLY3] = {
+		.constraints = {
+			.name = "ab8500-ext-supply3",
+			.min_uV = 3400000,
+			.max_uV = 3400000,
+			.valid_ops_mask = REGULATOR_CHANGE_STATUS,
+			.boot_on = 1,
+		},
+		.num_consumer_supplies =
+			ARRAY_SIZE(ab8500_ext_supply3_consumers),
+		.consumer_supplies = ab8500_ext_supply3_consumers,
+	},
+};
+
+/* ab8505 regulator register initialization */
+static struct ab8500_regulator_reg_init ab8505_reg_init[] = {
+	/*
+	 * VarmRequestCtrl
+	 * VsmpsCRequestCtrl
+	 * VsmpsARequestCtrl
+	 * VsmpsBRequestCtrl
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_REGUREQUESTCTRL1,       0x00, 0x00),
+	/*
+	 * VsafeRequestCtrl
+	 * VpllRequestCtrl
+	 * VanaRequestCtrl          = HP/LP depending on VxRequest
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_REGUREQUESTCTRL2,       0x30, 0x00),
+	/*
+	 * Vaux1RequestCtrl         = HP/LP depending on VxRequest
+	 * Vaux2RequestCtrl         = HP/LP depending on VxRequest
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_REGUREQUESTCTRL3,       0xf0, 0x00),
+	/*
+	 * Vaux3RequestCtrl         = HP/LP depending on VxRequest
+	 * SwHPReq                  = Control through SWValid disabled
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_REGUREQUESTCTRL4,       0x07, 0x00),
+	/*
+	 * VsmpsASysClkReq1HPValid
+	 * VsmpsBSysClkReq1HPValid
+	 * VsafeSysClkReq1HPValid
+	 * VanaSysClkReq1HPValid    = disabled
+	 * VpllSysClkReq1HPValid
+	 * Vaux1SysClkReq1HPValid   = disabled
+	 * Vaux2SysClkReq1HPValid   = disabled
+	 * Vaux3SysClkReq1HPValid   = disabled
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_REGUSYSCLKREQ1HPVALID1, 0xe8, 0x00),
+	/*
+	 * VsmpsCSysClkReq1HPValid
+	 * VarmSysClkReq1HPValid
+	 * VbbSysClkReq1HPValid
+	 * VsmpsMSysClkReq1HPValid
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_REGUSYSCLKREQ1HPVALID2, 0x00, 0x00),
+	/*
+	 * VsmpsAHwHPReq1Valid
+	 * VsmpsBHwHPReq1Valid
+	 * VsafeHwHPReq1Valid
+	 * VanaHwHPReq1Valid        = disabled
+	 * VpllHwHPReq1Valid
+	 * Vaux1HwHPreq1Valid       = disabled
+	 * Vaux2HwHPReq1Valid       = disabled
+	 * Vaux3HwHPReqValid        = disabled
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_REGUHWHPREQ1VALID1,     0xe8, 0x00),
+	/*
+	 * VsmpsMHwHPReq1Valid
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_REGUHWHPREQ1VALID2,     0x00, 0x00),
+	/*
+	 * VsmpsAHwHPReq2Valid
+	 * VsmpsBHwHPReq2Valid
+	 * VsafeHwHPReq2Valid
+	 * VanaHwHPReq2Valid        = disabled
+	 * VpllHwHPReq2Valid
+	 * Vaux1HwHPReq2Valid       = disabled
+	 * Vaux2HwHPReq2Valid       = disabled
+	 * Vaux3HwHPReq2Valid       = disabled
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_REGUHWHPREQ2VALID1,     0xe8, 0x00),
+	/*
+	 * VsmpsMHwHPReq2Valid
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_REGUHWHPREQ2VALID2,     0x00, 0x00),
+	/**
+	 * VsmpsCSwHPReqValid
+	 * VarmSwHPReqValid
+	 * VsmpsASwHPReqValid
+	 * VsmpsBSwHPReqValid
+	 * VsafeSwHPReqValid
+	 * VanaSwHPReqValid
+	 * VanaSwHPReqValid         = disabled
+	 * VpllSwHPReqValid
+	 * Vaux1SwHPReqValid        = disabled
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_REGUSWHPREQVALID1,      0xa0, 0x00),
+	/*
+	 * Vaux2SwHPReqValid        = disabled
+	 * Vaux3SwHPReqValid        = disabled
+	 * VsmpsMSwHPReqValid
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_REGUSWHPREQVALID2,      0x03, 0x00),
+	/*
+	 * SysClkReq2Valid1         = SysClkReq2 controlled
+	 * SysClkReq3Valid1         = disabled
+	 * SysClkReq4Valid1         = SysClkReq4 controlled
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_REGUSYSCLKREQVALID1,    0x0e, 0x0a),
+	/*
+	 * SysClkReq2Valid2         = disabled
+	 * SysClkReq3Valid2         = disabled
+	 * SysClkReq4Valid2         = disabled
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_REGUSYSCLKREQVALID2,    0x0e, 0x00),
+	/*
+	 * Vaux4SwHPReqValid
+	 * Vaux4HwHPReq2Valid
+	 * Vaux4HwHPReq1Valid
+	 * Vaux4SysClkReq1HPValid
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_REGUVAUX4REQVALID,    0x00, 0x00),
+	/*
+	 * VadcEna                  = disabled
+	 * VintCore12Ena            = disabled
+	 * VintCore12Sel            = 1.25 V
+	 * VintCore12LP             = inactive (HP)
+	 * VadcLP                   = inactive (HP)
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_REGUMISC1,              0xfe, 0x10),
+	/*
+	 * VaudioEna                = disabled
+	 * Vaux8Ena                 = disabled
+	 * Vamic1Ena                = disabled
+	 * Vamic2Ena                = disabled
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_VAUDIOSUPPLY,           0x1e, 0x00),
+	/*
+	 * Vamic1_dzout             = high-Z when Vamic1 is disabled
+	 * Vamic2_dzout             = high-Z when Vamic2 is disabled
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_REGUCTRL1VAMIC,         0x03, 0x00),
+	/*
+	 * VsmpsARegu
+	 * VsmpsASelCtrl
+	 * VsmpsAAutoMode
+	 * VsmpsAPWMMode
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_VSMPSAREGU,    0x00, 0x00),
+	/*
+	 * VsmpsBRegu
+	 * VsmpsBSelCtrl
+	 * VsmpsBAutoMode
+	 * VsmpsBPWMMode
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_VSMPSBREGU,    0x00, 0x00),
+	/*
+	 * VsafeRegu
+	 * VsafeSelCtrl
+	 * VsafeAutoMode
+	 * VsafePWMMode
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_VSAFEREGU,    0x00, 0x00),
+	/*
+	 * VPll                     = Hw controlled (NOTE! PRCMU bits)
+	 * VanaRegu                 = force off
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_VPLLVANAREGU,           0x0f, 0x02),
+	/*
+	 * VextSupply1Regu          = force OFF (OTP_ExtSupply12LPnPolarity 1)
+	 * VextSupply2Regu          = force OFF (OTP_ExtSupply12LPnPolarity 1)
+	 * VextSupply3Regu          = force OFF (OTP_ExtSupply3LPnPolarity 0)
+	 * ExtSupply2Bypass         = ExtSupply12LPn ball is 0 when Ena is 0
+	 * ExtSupply3Bypass         = ExtSupply3LPn ball is 0 when Ena is 0
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_EXTSUPPLYREGU,          0xff, 0x30),
+	/*
+	 * Vaux1Regu                = force HP
+	 * Vaux2Regu                = force off
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_VAUX12REGU,             0x0f, 0x01),
+	/*
+	 * Vaux3Regu                = force off
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_VRF1VAUX3REGU,          0x03, 0x00),
+	/*
+	 * VsmpsASel1
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_VSMPSASEL1,    0x00, 0x00),
+	/*
+	 * VsmpsASel2
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_VSMPSASEL2,    0x00, 0x00),
+	/*
+	 * VsmpsASel3
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_VSMPSASEL3,    0x00, 0x00),
+	/*
+	 * VsmpsBSel1
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_VSMPSBSEL1,    0x00, 0x00),
+	/*
+	 * VsmpsBSel2
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_VSMPSBSEL2,    0x00, 0x00),
+	/*
+	 * VsmpsBSel3
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_VSMPSBSEL3,    0x00, 0x00),
+	/*
+	 * VsafeSel1
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_VSAFESEL1,    0x00, 0x00),
+	/*
+	 * VsafeSel2
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_VSAFESEL2,    0x00, 0x00),
+	/*
+	 * VsafeSel3
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_VSAFESEL3,    0x00, 0x00),
+	/*
+	 * Vaux1Sel                 = 2.8 V
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_VAUX1SEL,               0x0f, 0x0C),
+	/*
+	 * Vaux2Sel                 = 2.9 V
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_VAUX2SEL,               0x0f, 0x0d),
+	/*
+	 * Vaux3Sel                 = 2.91 V
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_VRF1VAUX3SEL,           0x07, 0x07),
+	/*
+	 * Vaux4RequestCtrl
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_VAUX4REQCTRL,    0x00, 0x00),
+	/*
+	 * Vaux4Regu
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_VAUX4REGU,    0x00, 0x00),
+	/*
+	 * Vaux4Sel
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_VAUX4SEL,    0x00, 0x00),
+	/*
+	 * Vaux1Disch               = short discharge time
+	 * Vaux2Disch               = short discharge time
+	 * Vaux3Disch               = short discharge time
+	 * Vintcore12Disch          = short discharge time
+	 * VTVoutDisch              = short discharge time
+	 * VaudioDisch              = short discharge time
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_REGUCTRLDISCH,          0xfc, 0x00),
+	/*
+	 * VanaDisch                = short discharge time
+	 * Vaux8PullDownEna         = pulldown disabled when Vaux8 is disabled
+	 * Vaux8Disch               = short discharge time
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_REGUCTRLDISCH2,         0x16, 0x00),
+	/*
+	 * Vaux4Disch               = short discharge time
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_REGUCTRLDISCH3,         0x01, 0x00),
+	/*
+	 * Vaux5Sel
+	 * Vaux5LP
+	 * Vaux5Ena
+	 * Vaux5Disch
+	 * Vaux5DisSfst
+	 * Vaux5DisPulld
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_CTRLVAUX5,              0x00, 0x00),
+	/*
+	 * Vaux6Sel
+	 * Vaux6LP
+	 * Vaux6Ena
+	 * Vaux6DisPulld
+	 */
+	INIT_REGULATOR_REGISTER(AB8505_CTRLVAUX6,              0x00, 0x00),
+};
+
+struct regulator_init_data ab8505_regulators[AB8505_NUM_REGULATORS] = {
+	/* supplies to the display/camera */
+	[AB8505_LDO_AUX1] = {
+		.constraints = {
+			.name = "V-DISPLAY",
+			.min_uV = 2800000,
+			.max_uV = 3300000,
+			.valid_ops_mask = REGULATOR_CHANGE_VOLTAGE |
+					  REGULATOR_CHANGE_STATUS,
+			.boot_on = 1, /* display is on at boot */
+		},
+		.num_consumer_supplies = ARRAY_SIZE(ab8500_vaux1_consumers),
+		.consumer_supplies = ab8500_vaux1_consumers,
+	},
+	/* supplies to the on-board eMMC */
+	[AB8505_LDO_AUX2] = {
+		.constraints = {
+			.name = "V-eMMC1",
+			.min_uV = 1100000,
+			.max_uV = 3300000,
+			.valid_ops_mask = REGULATOR_CHANGE_VOLTAGE |
+					  REGULATOR_CHANGE_STATUS |
+					  REGULATOR_CHANGE_MODE,
+			.valid_modes_mask = REGULATOR_MODE_NORMAL |
+					    REGULATOR_MODE_IDLE,
+		},
+		.num_consumer_supplies = ARRAY_SIZE(ab8500_vaux2_consumers),
+		.consumer_supplies = ab8500_vaux2_consumers,
+	},
+	/* supply for VAUX3, supplies to SDcard slots */
+	[AB8505_LDO_AUX3] = {
+		.constraints = {
+			.name = "V-MMC-SD",
+			.min_uV = 1100000,
+			.max_uV = 3300000,
+			.valid_ops_mask = REGULATOR_CHANGE_VOLTAGE |
+					  REGULATOR_CHANGE_STATUS |
+					  REGULATOR_CHANGE_MODE,
+			.valid_modes_mask = REGULATOR_MODE_NORMAL |
+					    REGULATOR_MODE_IDLE,
+		},
+		.num_consumer_supplies = ARRAY_SIZE(ab8500_vaux3_consumers),
+		.consumer_supplies = ab8500_vaux3_consumers,
+	},
+	/* supply for VAUX4, supplies to NFC and standalone secure element */
+	[AB8505_LDO_AUX4] = {
+		.constraints = {
+			.name = "V-NFC-SE",
+			.min_uV = 1100000,
+			.max_uV = 3300000,
+			.valid_ops_mask = REGULATOR_CHANGE_VOLTAGE |
+					  REGULATOR_CHANGE_STATUS |
+					  REGULATOR_CHANGE_MODE,
+			.valid_modes_mask = REGULATOR_MODE_NORMAL |
+					    REGULATOR_MODE_IDLE,
+		},
+		.num_consumer_supplies = ARRAY_SIZE(ab8505_vaux4_consumers),
+		.consumer_supplies = ab8505_vaux4_consumers,
+	},
+	/* supply for VAUX5, supplies to TBD */
+	[AB8505_LDO_AUX5] = {
+		.constraints = {
+			.name = "V-AUX5",
+			.min_uV = 1050000,
+			.max_uV = 2790000,
+			.valid_ops_mask = REGULATOR_CHANGE_VOLTAGE |
+					  REGULATOR_CHANGE_STATUS |
+					  REGULATOR_CHANGE_MODE,
+			.valid_modes_mask = REGULATOR_MODE_NORMAL |
+					    REGULATOR_MODE_IDLE,
+		},
+		.num_consumer_supplies = ARRAY_SIZE(ab8505_vaux5_consumers),
+		.consumer_supplies = ab8505_vaux5_consumers,
+	},
+	/* supply for VAUX6, supplies to TBD */
+	[AB8505_LDO_AUX6] = {
+		.constraints = {
+			.name = "V-AUX6",
+			.min_uV = 1050000,
+			.max_uV = 2790000,
+			.valid_ops_mask = REGULATOR_CHANGE_VOLTAGE |
+					  REGULATOR_CHANGE_STATUS |
+					  REGULATOR_CHANGE_MODE,
+			.valid_modes_mask = REGULATOR_MODE_NORMAL |
+					    REGULATOR_MODE_IDLE,
+		},
+		.num_consumer_supplies = ARRAY_SIZE(ab8505_vaux6_consumers),
+		.consumer_supplies = ab8505_vaux6_consumers,
+	},
+	/* supply for gpadc, ADC LDO */
+	[AB8505_LDO_ADC] = {
+		.constraints = {
+			.name = "V-ADC",
+			.valid_ops_mask = REGULATOR_CHANGE_STATUS,
+		},
+		.num_consumer_supplies = ARRAY_SIZE(ab8505_vadc_consumers),
+		.consumer_supplies = ab8505_vadc_consumers,
+	},
+	/* supply for ab8500-vaudio, VAUDIO LDO */
+	[AB8505_LDO_AUDIO] = {
+		.constraints = {
+			.name = "V-AUD",
+			.valid_ops_mask = REGULATOR_CHANGE_STATUS,
+		},
+		.num_consumer_supplies = ARRAY_SIZE(ab8500_vaud_consumers),
+		.consumer_supplies = ab8500_vaud_consumers,
+	},
+	/* supply for v-anamic1 VAMic1-LDO */
+	[AB8505_LDO_ANAMIC1] = {
+		.constraints = {
+			.name = "V-AMIC1",
+			.valid_ops_mask = REGULATOR_CHANGE_STATUS |
+					  REGULATOR_CHANGE_MODE,
+			.valid_modes_mask = REGULATOR_MODE_NORMAL |
+					    REGULATOR_MODE_IDLE,
+		},
+		.num_consumer_supplies = ARRAY_SIZE(ab8500_vamic1_consumers),
+		.consumer_supplies = ab8500_vamic1_consumers,
+	},
+	/* supply for v-amic2, VAMIC2 LDO, reuse constants for AMIC1 */
+	[AB8505_LDO_ANAMIC2] = {
+		.constraints = {
+			.name = "V-AMIC2",
+			.valid_ops_mask = REGULATOR_CHANGE_STATUS |
+					  REGULATOR_CHANGE_MODE,
+			.valid_modes_mask = REGULATOR_MODE_NORMAL |
+					    REGULATOR_MODE_IDLE,
+		},
+		.num_consumer_supplies = ARRAY_SIZE(ab8500_vamic2_consumers),
+		.consumer_supplies = ab8500_vamic2_consumers,
+	},
+	/* supply for v-aux8, VAUX8 LDO */
+	[AB8505_LDO_AUX8] = {
+		.constraints = {
+			.name = "V-AUX8",
+			.valid_ops_mask = REGULATOR_CHANGE_STATUS,
+		},
+		.num_consumer_supplies = ARRAY_SIZE(ab8505_vaux8_consumers),
+		.consumer_supplies = ab8505_vaux8_consumers,
+	},
+	/* supply for v-intcore12, VINTCORE12 LDO */
+	[AB8505_LDO_INTCORE] = {
+		.constraints = {
+			.name = "V-INTCORE",
+			.min_uV = 1250000,
+			.max_uV = 1350000,
+			.input_uV = 1800000,
+			.valid_ops_mask = REGULATOR_CHANGE_VOLTAGE |
+					  REGULATOR_CHANGE_STATUS |
+					  REGULATOR_CHANGE_MODE |
+					  REGULATOR_CHANGE_DRMS,
+			.valid_modes_mask = REGULATOR_MODE_NORMAL |
+					    REGULATOR_MODE_IDLE,
+		},
+		.num_consumer_supplies = ARRAY_SIZE(ab8500_vintcore_consumers),
+		.consumer_supplies = ab8500_vintcore_consumers,
+	},
+	/* supply for LDO USB */
+	[AB8505_LDO_USB] = {
+		.constraints = {
+			.name = "V-USB",
+			.valid_ops_mask = REGULATOR_CHANGE_STATUS |
+					  REGULATOR_CHANGE_MODE,
+			.valid_modes_mask = REGULATOR_MODE_NORMAL |
+					    REGULATOR_MODE_IDLE,
+		},
+		.num_consumer_supplies = ARRAY_SIZE(ab8505_usb_consumers),
+		.consumer_supplies = ab8505_usb_consumers,
+	},
+	/* supply for U8500 CSI-DSI, VANA LDO */
+	[AB8505_LDO_ANA] = {
+		.constraints = {
+			.name = "V-CSI-DSI",
+			.valid_ops_mask = REGULATOR_CHANGE_STATUS,
+		},
+		.num_consumer_supplies = ARRAY_SIZE(ab8500_vana_consumers),
+		.consumer_supplies = ab8500_vana_consumers,
+	},
+};
+
+struct ab8500_regulator_platform_data ab8500_regulator_plat_data = {
+	.reg_init               = ab8500_reg_init,
+	.num_reg_init           = ARRAY_SIZE(ab8500_reg_init),
+	.regulator              = ab8500_regulators,
+	.num_regulator          = ARRAY_SIZE(ab8500_regulators),
+	.ext_regulator          = ab8500_ext_regulators,
+	.num_ext_regulator      = ARRAY_SIZE(ab8500_ext_regulators),
+};
+
+/* Use the AB8500 init settings for AB8505 as they are the same right now */
+struct ab8500_regulator_platform_data ab8505_regulator_plat_data = {
+	.reg_init               = ab8505_reg_init,
+	.num_reg_init           = ARRAY_SIZE(ab8505_reg_init),
+	.regulator              = ab8505_regulators,
+	.num_regulator          = ARRAY_SIZE(ab8505_regulators),
+};
+
+static void ab8500_modify_reg_init(int id, u8 mask, u8 value)
+{
+	int i;
+
+	if (cpu_is_u8520()) {
+		for (i = ARRAY_SIZE(ab8505_reg_init) - 1; i >= 0; i--) {
+			if (ab8505_reg_init[i].id == id) {
+				u8 initval = ab8505_reg_init[i].value;
+				initval = (initval & ~mask) | (value & mask);
+				ab8505_reg_init[i].value = initval;
+
+				BUG_ON(mask & ~ab8505_reg_init[i].mask);
+				return;
+			}
+		}
+	} else {
+		for (i = ARRAY_SIZE(ab8500_reg_init) - 1; i >= 0; i--) {
+			if (ab8500_reg_init[i].id == id) {
+				u8 initval = ab8500_reg_init[i].value;
+				initval = (initval & ~mask) | (value & mask);
+				ab8500_reg_init[i].value = initval;
+
+				BUG_ON(mask & ~ab8500_reg_init[i].mask);
+				return;
+			}
+		}
+	}
+
+	BUG_ON(1);
+}
+
+void mop500_regulator_init(void)
+{
+	struct regulator_init_data *regulator;
+
+	/*
+	 * Temporarily turn on Vaux2 on 8520 machine
+	 */
+	if (cpu_is_u8520()) {
+		/* Vaux2 initialized to be on */
+		ab8500_modify_reg_init(AB8505_VAUX12REGU, 0x0f, 0x05);
+	}
+
+	/*
+	 * Handle AB8500_EXT_SUPPLY2 on HREFP_V20_V50 boards (do it for
+	 * all HREFP_V20 boards)
+	 */
+	if (cpu_is_u8500v20()) {
+		/* VextSupply2RequestCtrl =  HP/OFF depending on VxRequest */
+		ab8500_modify_reg_init(AB8500_REGUREQUESTCTRL3, 0x01, 0x01);
+
+		/* VextSupply2SysClkReq1HPValid = SysClkReq1 controlled */
+		ab8500_modify_reg_init(AB8500_REGUSYSCLKREQ1HPVALID2,
+			0x20, 0x20);
+
+		/* VextSupply2 = force HP at initialization */
+		ab8500_modify_reg_init(AB8500_EXTSUPPLYREGU, 0x0c, 0x04);
+
+		/* enable VextSupply2 during platform active */
+		regulator = &ab8500_ext_regulators[AB8500_EXT_SUPPLY2];
+		regulator->constraints.always_on = 1;
+
+		/* disable VextSupply2 in suspend */
+		regulator = &ab8500_ext_regulators[AB8500_EXT_SUPPLY2];
+		regulator->constraints.state_mem.disabled = 1;
+		regulator->constraints.state_standby.disabled = 1;
+
+		/* enable VextSupply2 HW control (used in suspend) */
+		regulator->driver_data = (void *)&ab8500_ext_supply2;
+	}
+}
diff --git a/arch/arm/mach-ux500/board-mop500-regulators.h b/arch/arm/mach-ux500/board-mop500-regulators.h
index 78a0642a220..9bece38fe93 100644
--- a/arch/arm/mach-ux500/board-mop500-regulators.h
+++ b/arch/arm/mach-ux500/board-mop500-regulators.h
@@ -14,10 +14,11 @@
 #include <linux/regulator/machine.h>
 #include <linux/regulator/ab8500.h>
 
-extern struct ab8500_regulator_reg_init
-ab8500_regulator_reg_init[AB8500_NUM_REGULATOR_REGISTERS];
-extern struct regulator_init_data ab8500_regulators[AB8500_NUM_REGULATORS];
+extern struct ab8500_regulator_platform_data ab8500_regulator_plat_data;
+extern struct ab8500_regulator_platform_data ab8505_regulator_plat_data;
 extern struct regulator_init_data tps61052_regulator;
 extern struct regulator_init_data gpio_en_3v3_regulator;
 
+void mop500_regulator_init(void);
+
 #endif
diff --git a/arch/arm/mach-ux500/board-mop500-sdi.c b/arch/arm/mach-ux500/board-mop500-sdi.c
index 051b62c2710..7f2cb6c5e2c 100644
--- a/arch/arm/mach-ux500/board-mop500-sdi.c
+++ b/arch/arm/mach-ux500/board-mop500-sdi.c
@@ -81,7 +81,6 @@ static struct stedma40_chan_cfg mop500_sdi0_dma_cfg_tx = {
 #endif
 
 struct mmci_platform_data mop500_sdi0_data = {
-	.ios_handler	= mop500_sdi0_ios_handler,
 	.ocr_mask	= MMC_VDD_29_30,
 	.f_max		= 50000000,
 	.capabilities	= MMC_CAP_4_BIT_DATA |
diff --git a/arch/arm/mach-ux500/board-mop500.c b/arch/arm/mach-ux500/board-mop500.c
index b03457881c4..ce672378a83 100644
--- a/arch/arm/mach-ux500/board-mop500.c
+++ b/arch/arm/mach-ux500/board-mop500.c
@@ -12,6 +12,7 @@
 #include <linux/init.h>
 #include <linux/interrupt.h>
 #include <linux/platform_device.h>
+#include <linux/clk.h>
 #include <linux/io.h>
 #include <linux/i2c.h>
 #include <linux/platform_data/i2c-nomadik.h>
@@ -198,10 +199,7 @@ static struct platform_device snowball_sbnet_dev = {
 
 struct ab8500_platform_data ab8500_platdata = {
 	.irq_base	= MOP500_AB8500_IRQ_BASE,
-	.regulator_reg_init = ab8500_regulator_reg_init,
-	.num_regulator_reg_init	= ARRAY_SIZE(ab8500_regulator_reg_init),
-	.regulator	= ab8500_regulators,
-	.num_regulator	= ARRAY_SIZE(ab8500_regulators),
+	.regulator	= &ab8500_regulator_plat_data,
 	.gpio		= &ab8500_gpio_pdata,
 	.codec		= &ab8500_codec_pdata,
 };
@@ -439,6 +437,15 @@ static void mop500_prox_deactivate(struct device *dev)
 	regulator_put(prox_regulator);
 }
 
+void mop500_snowball_ethernet_clock_enable(void)
+{
+	struct clk *clk;
+
+	clk = clk_get_sys("fsmc", NULL);
+	if (!IS_ERR(clk))
+		clk_prepare_enable(clk);
+}
+
 static struct cryp_platform_data u8500_cryp1_platform_data = {
 		.mem_to_engine = {
 				.dir = STEDMA40_MEM_TO_PERIPH,
@@ -683,6 +690,8 @@ static void __init snowball_init_machine(void)
 	mop500_audio_init(parent);
 	mop500_uart_init(parent);
 
+	mop500_snowball_ethernet_clock_enable();
+
 	/* This board has full regulator constraints */
 	regulator_has_full_constraints();
 }
diff --git a/arch/arm/mach-ux500/board-mop500.h b/arch/arm/mach-ux500/board-mop500.h
index eaa605f5d90..d38951be70d 100644
--- a/arch/arm/mach-ux500/board-mop500.h
+++ b/arch/arm/mach-ux500/board-mop500.h
@@ -104,6 +104,7 @@ void __init mop500_pinmaps_init(void);
 void __init snowball_pinmaps_init(void);
 void __init hrefv60_pinmaps_init(void);
 void mop500_audio_init(struct device *parent);
+void mop500_snowball_ethernet_clock_enable(void);
 
 int __init mop500_uib_init(void);
 void mop500_uib_i2c_add(int busnum, struct i2c_board_info *info,
diff --git a/arch/arm/mach-ux500/cpu-db8500.c b/arch/arm/mach-ux500/cpu-db8500.c
index 19235cf7bbe..f1a58184437 100644
--- a/arch/arm/mach-ux500/cpu-db8500.c
+++ b/arch/arm/mach-ux500/cpu-db8500.c
@@ -312,9 +312,10 @@ static void __init u8500_init_machine(void)
 	/* Pinmaps must be in place before devices register */
 	if (of_machine_is_compatible("st-ericsson,mop500"))
 		mop500_pinmaps_init();
-	else if (of_machine_is_compatible("calaosystems,snowball-a9500"))
+	else if (of_machine_is_compatible("calaosystems,snowball-a9500")) {
 		snowball_pinmaps_init();
-	else if (of_machine_is_compatible("st-ericsson,hrefv60+"))
+		mop500_snowball_ethernet_clock_enable();
+	} else if (of_machine_is_compatible("st-ericsson,hrefv60+"))
 		hrefv60_pinmaps_init();
 	else if (of_machine_is_compatible("st-ericsson,ccu9540")) {}
 		/* TODO: Add pinmaps for ccu9540 board. */
diff --git a/arch/arm/mach-vexpress/v2m.c b/arch/arm/mach-vexpress/v2m.c
index c970762e838..eb2b3a627f0 100644
--- a/arch/arm/mach-vexpress/v2m.c
+++ b/arch/arm/mach-vexpress/v2m.c
@@ -21,6 +21,8 @@
 #include <linux/regulator/fixed.h>
 #include <linux/regulator/machine.h>
 #include <linux/vexpress.h>
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
 
 #include <asm/arch_timer.h>
 #include <asm/mach-types.h>
@@ -430,7 +432,7 @@ static void __init v2m_dt_timer_init(void)
 {
 	struct device_node *node = NULL;
 
-	vexpress_clk_of_init();
+	of_clk_init(NULL);
 
 	do {
 		node = of_find_compatible_node(node, NULL, "arm,sp804");
@@ -438,6 +440,10 @@ static void __init v2m_dt_timer_init(void)
 	if (node) {
 		pr_info("Using SP804 '%s' as a clock & events source\n",
 				node->full_name);
+		WARN_ON(clk_register_clkdev(of_clk_get_by_name(node,
+				"timclken1"), "v2m-timer0", "sp804"));
+		WARN_ON(clk_register_clkdev(of_clk_get_by_name(node,
+				"timclken2"), "v2m-timer1", "sp804"));
 		v2m_sp804_init(of_iomap(node, 0),
 				irq_of_parse_and_map(node, 0));
 	}
diff --git a/arch/arm/mach-w90x900/dev.c b/arch/arm/mach-w90x900/dev.c
index 7abdb9645c5..e65a80a1ac7 100644
--- a/arch/arm/mach-w90x900/dev.c
+++ b/arch/arm/mach-w90x900/dev.c
@@ -19,6 +19,7 @@
 #include <linux/init.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
+#include <linux/cpu.h>
 
 #include <linux/mtd/physmap.h>
 #include <linux/mtd/mtd.h>
@@ -531,7 +532,7 @@ static struct platform_device *nuc900_public_dev[] __initdata = {
 
 void __init nuc900_board_init(struct platform_device **device, int size)
 {
-	disable_hlt();
+	cpu_idle_poll_ctrl(true);
 	platform_add_devices(device, size);
 	platform_add_devices(nuc900_public_dev, ARRAY_SIZE(nuc900_public_dev));
 	spi_register_board_info(nuc900_spi_board_info,
diff --git a/arch/arm/mm/Kconfig b/arch/arm/mm/Kconfig
index 025d1732873..4045c4931a3 100644
--- a/arch/arm/mm/Kconfig
+++ b/arch/arm/mm/Kconfig
@@ -43,7 +43,7 @@ config CPU_ARM740T
 	depends on !MMU
 	select CPU_32v4T
 	select CPU_ABRT_LV4T
-	select CPU_CACHE_V3	# although the core is v4t
+	select CPU_CACHE_V4
 	select CPU_CP15_MPU
 	select CPU_PABRT_LEGACY
 	help
@@ -469,9 +469,6 @@ config CPU_PABRT_V7
 	bool
 
 # The cache model
-config CPU_CACHE_V3
-	bool
-
 config CPU_CACHE_V4
 	bool
 
diff --git a/arch/arm/mm/Makefile b/arch/arm/mm/Makefile
index 4e333fa2756..9e51be96f63 100644
--- a/arch/arm/mm/Makefile
+++ b/arch/arm/mm/Makefile
@@ -33,7 +33,6 @@ obj-$(CONFIG_CPU_PABRT_LEGACY)	+= pabort-legacy.o
 obj-$(CONFIG_CPU_PABRT_V6)	+= pabort-v6.o
 obj-$(CONFIG_CPU_PABRT_V7)	+= pabort-v7.o
 
-obj-$(CONFIG_CPU_CACHE_V3)	+= cache-v3.o
 obj-$(CONFIG_CPU_CACHE_V4)	+= cache-v4.o
 obj-$(CONFIG_CPU_CACHE_V4WT)	+= cache-v4wt.o
 obj-$(CONFIG_CPU_CACHE_V4WB)	+= cache-v4wb.o
diff --git a/arch/arm/mm/cache-feroceon-l2.c b/arch/arm/mm/cache-feroceon-l2.c
index dd3d59122cc..48bc3c0a87c 100644
--- a/arch/arm/mm/cache-feroceon-l2.c
+++ b/arch/arm/mm/cache-feroceon-l2.c
@@ -343,6 +343,7 @@ void __init feroceon_l2_init(int __l2_wt_override)
 	outer_cache.inv_range = feroceon_l2_inv_range;
 	outer_cache.clean_range = feroceon_l2_clean_range;
 	outer_cache.flush_range = feroceon_l2_flush_range;
+	outer_cache.inv_all = l2_inv_all;
 
 	enable_l2();
 
diff --git a/arch/arm/mm/cache-l2x0.c b/arch/arm/mm/cache-l2x0.c
index c2f37390308..c465faca51b 100644
--- a/arch/arm/mm/cache-l2x0.c
+++ b/arch/arm/mm/cache-l2x0.c
@@ -299,7 +299,7 @@ static void l2x0_unlock(u32 cache_id)
 	int lockregs;
 	int i;
 
-	switch (cache_id) {
+	switch (cache_id & L2X0_CACHE_ID_PART_MASK) {
 	case L2X0_CACHE_ID_PART_L310:
 		lockregs = 8;
 		break;
@@ -333,15 +333,14 @@ void __init l2x0_init(void __iomem *base, u32 aux_val, u32 aux_mask)
 	if (cache_id_part_number_from_dt)
 		cache_id = cache_id_part_number_from_dt;
 	else
-		cache_id = readl_relaxed(l2x0_base + L2X0_CACHE_ID)
-			& L2X0_CACHE_ID_PART_MASK;
+		cache_id = readl_relaxed(l2x0_base + L2X0_CACHE_ID);
 	aux = readl_relaxed(l2x0_base + L2X0_AUX_CTRL);
 
 	aux &= aux_mask;
 	aux |= aux_val;
 
 	/* Determine the number of ways */
-	switch (cache_id) {
+	switch (cache_id & L2X0_CACHE_ID_PART_MASK) {
 	case L2X0_CACHE_ID_PART_L310:
 		if (aux & (1 << 16))
 			ways = 16;
@@ -725,7 +724,6 @@ static const struct l2x0_of_data pl310_data = {
 		.flush_all   = l2x0_flush_all,
 		.inv_all     = l2x0_inv_all,
 		.disable     = l2x0_disable,
-		.set_debug   = pl310_set_debug,
 	},
 };
 
@@ -814,9 +812,8 @@ int __init l2x0_of_init(u32 aux_val, u32 aux_mask)
 		data->save();
 
 	of_init = true;
-	l2x0_init(l2x0_base, aux_val, aux_mask);
-
 	memcpy(&outer_cache, &data->outer_cache, sizeof(outer_cache));
+	l2x0_init(l2x0_base, aux_val, aux_mask);
 
 	return 0;
 }
diff --git a/arch/arm/mm/cache-v3.S b/arch/arm/mm/cache-v3.S
deleted file mode 100644
index 8a3fadece8d..00000000000
--- a/arch/arm/mm/cache-v3.S
+++ /dev/null
@@ -1,137 +0,0 @@
-/*
- *  linux/arch/arm/mm/cache-v3.S
- *
- *  Copyright (C) 1997-2002 Russell king
- *
- * 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/linkage.h>
-#include <linux/init.h>
-#include <asm/page.h>
-#include "proc-macros.S"
-
-/*
- *	flush_icache_all()
- *
- *	Unconditionally clean and invalidate the entire icache.
- */
-ENTRY(v3_flush_icache_all)
-	mov	pc, lr
-ENDPROC(v3_flush_icache_all)
-
-/*
- *	flush_user_cache_all()
- *
- *	Invalidate all cache entries in a particular address
- *	space.
- *
- *	- mm	- mm_struct describing address space
- */
-ENTRY(v3_flush_user_cache_all)
-	/* FALLTHROUGH */
-/*
- *	flush_kern_cache_all()
- *
- *	Clean and invalidate the entire cache.
- */
-ENTRY(v3_flush_kern_cache_all)
-	/* FALLTHROUGH */
-
-/*
- *	flush_user_cache_range(start, end, flags)
- *
- *	Invalidate a range of cache entries in the specified
- *	address space.
- *
- *	- start - start address (may not be aligned)
- *	- end	- end address (exclusive, may not be aligned)
- *	- flags	- vma_area_struct flags describing address space
- */
-ENTRY(v3_flush_user_cache_range)
-	mov	ip, #0
-	mcreq	p15, 0, ip, c7, c0, 0		@ flush ID cache
-	mov	pc, lr
-
-/*
- *	coherent_kern_range(start, end)
- *
- *	Ensure coherency between the Icache and the Dcache in the
- *	region described by start.  If you have non-snooping
- *	Harvard caches, you need to implement this function.
- *
- *	- start  - virtual start address
- *	- end	 - virtual end address
- */
-ENTRY(v3_coherent_kern_range)
-	/* FALLTHROUGH */
-
-/*
- *	coherent_user_range(start, end)
- *
- *	Ensure coherency between the Icache and the Dcache in the
- *	region described by start.  If you have non-snooping
- *	Harvard caches, you need to implement this function.
- *
- *	- start  - virtual start address
- *	- end	 - virtual end address
- */
-ENTRY(v3_coherent_user_range)
-	mov	r0, #0
-	mov	pc, lr
-
-/*
- *	flush_kern_dcache_area(void *page, size_t size)
- *
- *	Ensure no D cache aliasing occurs, either with itself or
- *	the I cache
- *
- *	- addr	- kernel address
- *	- size	- region size
- */
-ENTRY(v3_flush_kern_dcache_area)
-	/* FALLTHROUGH */
-
-/*
- *	dma_flush_range(start, end)
- *
- *	Clean and invalidate the specified virtual address range.
- *
- *	- start  - virtual start address
- *	- end	 - virtual end address
- */
-ENTRY(v3_dma_flush_range)
-	mov	r0, #0
-	mcr	p15, 0, r0, c7, c0, 0		@ flush ID cache
-	mov	pc, lr
-
-/*
- *	dma_unmap_area(start, size, dir)
- *	- start	- kernel virtual start address
- *	- size	- size of region
- *	- dir	- DMA direction
- */
-ENTRY(v3_dma_unmap_area)
-	teq	r2, #DMA_TO_DEVICE
-	bne	v3_dma_flush_range
-	/* FALLTHROUGH */
-
-/*
- *	dma_map_area(start, size, dir)
- *	- start	- kernel virtual start address
- *	- size	- size of region
- *	- dir	- DMA direction
- */
-ENTRY(v3_dma_map_area)
-	mov	pc, lr
-ENDPROC(v3_dma_unmap_area)
-ENDPROC(v3_dma_map_area)
-
-	.globl	v3_flush_kern_cache_louis
-	.equ	v3_flush_kern_cache_louis, v3_flush_kern_cache_all
-
-	__INITDATA
-
-	@ define struct cpu_cache_fns (see <asm/cacheflush.h> and proc-macros.S)
-	define_cache_functions v3
diff --git a/arch/arm/mm/cache-v4.S b/arch/arm/mm/cache-v4.S
index 43e5d77be67..a7ba68f59f0 100644
--- a/arch/arm/mm/cache-v4.S
+++ b/arch/arm/mm/cache-v4.S
@@ -58,7 +58,7 @@ ENTRY(v4_flush_kern_cache_all)
 ENTRY(v4_flush_user_cache_range)
 #ifdef CONFIG_CPU_CP15
 	mov	ip, #0
-	mcreq	p15, 0, ip, c7, c7, 0		@ flush ID cache
+	mcr	p15, 0, ip, c7, c7, 0		@ flush ID cache
 	mov	pc, lr
 #else
 	/* FALLTHROUGH */
diff --git a/arch/arm/mm/context.c b/arch/arm/mm/context.c
index a5a4b2bc42b..2ac37372ef5 100644
--- a/arch/arm/mm/context.c
+++ b/arch/arm/mm/context.c
@@ -48,7 +48,7 @@ static DEFINE_RAW_SPINLOCK(cpu_asid_lock);
 static atomic64_t asid_generation = ATOMIC64_INIT(ASID_FIRST_VERSION);
 static DECLARE_BITMAP(asid_map, NUM_USER_ASIDS);
 
-static DEFINE_PER_CPU(atomic64_t, active_asids);
+DEFINE_PER_CPU(atomic64_t, active_asids);
 static DEFINE_PER_CPU(u64, reserved_asids);
 static cpumask_t tlb_flush_pending;
 
@@ -215,6 +215,7 @@ void check_and_switch_context(struct mm_struct *mm, struct task_struct *tsk)
 	if (cpumask_test_and_clear_cpu(cpu, &tlb_flush_pending)) {
 		local_flush_bp_all();
 		local_flush_tlb_all();
+		dummy_flush_tlb_a15_erratum();
 	}
 
 	atomic64_set(&per_cpu(active_asids, cpu), asid);
diff --git a/arch/arm/mm/init.c b/arch/arm/mm/init.c
index ad722f1208a..9a5cdc01fcd 100644
--- a/arch/arm/mm/init.c
+++ b/arch/arm/mm/init.c
@@ -99,6 +99,9 @@ void show_mem(unsigned int filter)
 	printk("Mem-info:\n");
 	show_free_areas(filter);
 
+	if (filter & SHOW_MEM_FILTER_PAGE_COUNT)
+		return;
+
 	for_each_bank (i, mi) {
 		struct membank *bank = &mi->bank[i];
 		unsigned int pfn1, pfn2;
@@ -424,24 +427,6 @@ void __init bootmem_init(void)
 	max_pfn = max_high - PHYS_PFN_OFFSET;
 }
 
-static inline int free_area(unsigned long pfn, unsigned long end, char *s)
-{
-	unsigned int pages = 0, size = (end - pfn) << (PAGE_SHIFT - 10);
-
-	for (; pfn < end; pfn++) {
-		struct page *page = pfn_to_page(pfn);
-		ClearPageReserved(page);
-		init_page_count(page);
-		__free_page(page);
-		pages++;
-	}
-
-	if (size && s)
-		printk(KERN_INFO "Freeing %s memory: %dK\n", s, size);
-
-	return pages;
-}
-
 /*
  * Poison init memory with an undefined instruction (ARM) or a branch to an
  * undefined instruction (Thumb).
@@ -534,6 +519,14 @@ static void __init free_unused_memmap(struct meminfo *mi)
 #endif
 }
 
+#ifdef CONFIG_HIGHMEM
+static inline void free_area_high(unsigned long pfn, unsigned long end)
+{
+	for (; pfn < end; pfn++)
+		free_highmem_page(pfn_to_page(pfn));
+}
+#endif
+
 static void __init free_highpages(void)
 {
 #ifdef CONFIG_HIGHMEM
@@ -569,8 +562,7 @@ static void __init free_highpages(void)
 			if (res_end > end)
 				res_end = end;
 			if (res_start != start)
-				totalhigh_pages += free_area(start, res_start,
-							     NULL);
+				free_area_high(start, res_start);
 			start = res_end;
 			if (start == end)
 				break;
@@ -578,9 +570,8 @@ static void __init free_highpages(void)
 
 		/* And now free anything which remains */
 		if (start < end)
-			totalhigh_pages += free_area(start, end, NULL);
+			free_area_high(start, end);
 	}
-	totalram_pages += totalhigh_pages;
 #endif
 }
 
@@ -609,8 +600,7 @@ void __init mem_init(void)
 
 #ifdef CONFIG_SA1111
 	/* now that our DMA memory is actually so designated, we can free it */
-	totalram_pages += free_area(PHYS_PFN_OFFSET,
-				    __phys_to_pfn(__pa(swapper_pg_dir)), NULL);
+	free_reserved_area(__va(PHYS_PFN_OFFSET), swapper_pg_dir, 0, NULL);
 #endif
 
 	free_highpages();
@@ -738,16 +728,12 @@ void free_initmem(void)
 	extern char __tcm_start, __tcm_end;
 
 	poison_init_mem(&__tcm_start, &__tcm_end - &__tcm_start);
-	totalram_pages += free_area(__phys_to_pfn(__pa(&__tcm_start)),
-				    __phys_to_pfn(__pa(&__tcm_end)),
-				    "TCM link");
+	free_reserved_area(&__tcm_start, &__tcm_end, 0, "TCM link");
 #endif
 
 	poison_init_mem(__init_begin, __init_end - __init_begin);
 	if (!machine_is_integrator() && !machine_is_cintegrator())
-		totalram_pages += free_area(__phys_to_pfn(__pa(__init_begin)),
-					    __phys_to_pfn(__pa(__init_end)),
-					    "init");
+		free_initmem_default(0);
 }
 
 #ifdef CONFIG_BLK_DEV_INITRD
@@ -758,9 +744,7 @@ void free_initrd_mem(unsigned long start, unsigned long end)
 {
 	if (!keep_initrd) {
 		poison_init_mem((void *)start, PAGE_ALIGN(end) - start);
-		totalram_pages += free_area(__phys_to_pfn(__pa(start)),
-					    __phys_to_pfn(__pa(end)),
-					    "initrd");
+		free_reserved_area(start, end, 0, "initrd");
 	}
 }
 
diff --git a/arch/arm/mm/mmu.c b/arch/arm/mm/mmu.c
index e95a996ab78..a84ff763ac3 100644
--- a/arch/arm/mm/mmu.c
+++ b/arch/arm/mm/mmu.c
@@ -34,6 +34,7 @@
 #include <asm/mach/pci.h>
 
 #include "mm.h"
+#include "tcm.h"
 
 /*
  * empty_zero_page is a special page that is used for
@@ -598,39 +599,60 @@ static void __init alloc_init_pte(pmd_t *pmd, unsigned long addr,
 	} while (pte++, addr += PAGE_SIZE, addr != end);
 }
 
-static void __init alloc_init_section(pud_t *pud, unsigned long addr,
-				      unsigned long end, phys_addr_t phys,
-				      const struct mem_type *type)
+static void __init map_init_section(pmd_t *pmd, unsigned long addr,
+			unsigned long end, phys_addr_t phys,
+			const struct mem_type *type)
 {
-	pmd_t *pmd = pmd_offset(pud, addr);
-
+#ifndef CONFIG_ARM_LPAE
 	/*
-	 * Try a section mapping - end, addr and phys must all be aligned
-	 * to a section boundary.  Note that PMDs refer to the individual
-	 * L1 entries, whereas PGDs refer to a group of L1 entries making
-	 * up one logical pointer to an L2 table.
+	 * In classic MMU format, puds and pmds are folded in to
+	 * the pgds. pmd_offset gives the PGD entry. PGDs refer to a
+	 * group of L1 entries making up one logical pointer to
+	 * an L2 table (2MB), where as PMDs refer to the individual
+	 * L1 entries (1MB). Hence increment to get the correct
+	 * offset for odd 1MB sections.
+	 * (See arch/arm/include/asm/pgtable-2level.h)
 	 */
-	if (type->prot_sect && ((addr | end | phys) & ~SECTION_MASK) == 0) {
-		pmd_t *p = pmd;
-
-#ifndef CONFIG_ARM_LPAE
-		if (addr & SECTION_SIZE)
-			pmd++;
+	if (addr & SECTION_SIZE)
+		pmd++;
 #endif
+	do {
+		*pmd = __pmd(phys | type->prot_sect);
+		phys += SECTION_SIZE;
+	} while (pmd++, addr += SECTION_SIZE, addr != end);
 
-		do {
-			*pmd = __pmd(phys | type->prot_sect);
-			phys += SECTION_SIZE;
-		} while (pmd++, addr += SECTION_SIZE, addr != end);
+	flush_pmd_entry(pmd);
+}
 
-		flush_pmd_entry(p);
-	} else {
+static void __init alloc_init_pmd(pud_t *pud, unsigned long addr,
+				      unsigned long end, phys_addr_t phys,
+				      const struct mem_type *type)
+{
+	pmd_t *pmd = pmd_offset(pud, addr);
+	unsigned long next;
+
+	do {
 		/*
-		 * No need to loop; pte's aren't interested in the
-		 * individual L1 entries.
+		 * With LPAE, we must loop over to map
+		 * all the pmds for the given range.
 		 */
-		alloc_init_pte(pmd, addr, end, __phys_to_pfn(phys), type);
-	}
+		next = pmd_addr_end(addr, end);
+
+		/*
+		 * Try a section mapping - addr, next and phys must all be
+		 * aligned to a section boundary.
+		 */
+		if (type->prot_sect &&
+				((addr | next | phys) & ~SECTION_MASK) == 0) {
+			map_init_section(pmd, addr, next, phys, type);
+		} else {
+			alloc_init_pte(pmd, addr, next,
+						__phys_to_pfn(phys), type);
+		}
+
+		phys += next - addr;
+
+	} while (pmd++, addr = next, addr != end);
 }
 
 static void __init alloc_init_pud(pgd_t *pgd, unsigned long addr,
@@ -641,7 +663,7 @@ static void __init alloc_init_pud(pgd_t *pgd, unsigned long addr,
 
 	do {
 		next = pud_addr_end(addr, end);
-		alloc_init_section(pud, addr, next, phys, type);
+		alloc_init_pmd(pud, addr, next, phys, type);
 		phys += next - addr;
 	} while (pud++, addr = next, addr != end);
 }
@@ -1256,6 +1278,7 @@ void __init paging_init(struct machine_desc *mdesc)
 	dma_contiguous_remap();
 	devicemaps_init(mdesc);
 	kmap_init();
+	tcm_init();
 
 	top_pmd = pmd_off_k(0xffff0000);
 
diff --git a/arch/arm/mm/proc-arm740.S b/arch/arm/mm/proc-arm740.S
index dc5de5d53f2..fde2d2a794c 100644
--- a/arch/arm/mm/proc-arm740.S
+++ b/arch/arm/mm/proc-arm740.S
@@ -77,24 +77,27 @@ __arm740_setup:
 	mcr	p15, 0, r0, c6,	c0		@ set area 0, default
 
 	ldr	r0, =(CONFIG_DRAM_BASE & 0xFFFFF000) @ base[31:12] of RAM
-	ldr	r1, =(CONFIG_DRAM_SIZE >> 12)	@ size of RAM (must be >= 4KB)
-	mov	r2, #10				@ 11 is the minimum (4KB)
-1:	add	r2, r2, #1			@ area size *= 2
-	mov	r1, r1, lsr #1
+	ldr	r3, =(CONFIG_DRAM_SIZE >> 12)	@ size of RAM (must be >= 4KB)
+	mov	r4, #10				@ 11 is the minimum (4KB)
+1:	add	r4, r4, #1			@ area size *= 2
+	movs	r3, r3, lsr #1
 	bne	1b				@ count not zero r-shift
-	orr	r0, r0, r2, lsl #1		@ the area register value
+	orr	r0, r0, r4, lsl #1		@ the area register value
 	orr	r0, r0, #1			@ set enable bit
 	mcr	p15, 0, r0, c6,	c1		@ set area 1, RAM
 
 	ldr	r0, =(CONFIG_FLASH_MEM_BASE & 0xFFFFF000) @ base[31:12] of FLASH
-	ldr	r1, =(CONFIG_FLASH_SIZE >> 12)	@ size of FLASH (must be >= 4KB)
-	mov	r2, #10				@ 11 is the minimum (4KB)
-1:	add	r2, r2, #1			@ area size *= 2
-	mov	r1, r1, lsr #1
+	ldr	r3, =(CONFIG_FLASH_SIZE >> 12)	@ size of FLASH (must be >= 4KB)
+	cmp	r3, #0
+	moveq	r0, #0
+	beq	2f
+	mov	r4, #10				@ 11 is the minimum (4KB)
+1:	add	r4, r4, #1			@ area size *= 2
+	movs	r3, r3, lsr #1
 	bne	1b				@ count not zero r-shift
-	orr	r0, r0, r2, lsl #1		@ the area register value
+	orr	r0, r0, r4, lsl #1		@ the area register value
 	orr	r0, r0, #1			@ set enable bit
-	mcr	p15, 0, r0, c6,	c2		@ set area 2, ROM/FLASH
+2:	mcr	p15, 0, r0, c6,	c2		@ set area 2, ROM/FLASH
 
 	mov	r0, #0x06
 	mcr	p15, 0, r0, c2, c0		@ Region 1&2 cacheable
@@ -137,13 +140,14 @@ __arm740_proc_info:
 	.long	0x41807400
 	.long	0xfffffff0
 	.long	0
+	.long	0
 	b	__arm740_setup
 	.long	cpu_arch_name
 	.long	cpu_elf_name
-	.long	HWCAP_SWP | HWCAP_HALF | HWCAP_26BIT
+	.long	HWCAP_SWP | HWCAP_HALF | HWCAP_THUMB | HWCAP_26BIT
 	.long	cpu_arm740_name
 	.long	arm740_processor_functions
 	.long	0
 	.long	0
-	.long	v3_cache_fns			@ cache model
+	.long	v4_cache_fns			@ cache model
 	.size	__arm740_proc_info, . - __arm740_proc_info
diff --git a/arch/arm/mm/proc-arm920.S b/arch/arm/mm/proc-arm920.S
index 2c3b9421ab5..2556cf1c2da 100644
--- a/arch/arm/mm/proc-arm920.S
+++ b/arch/arm/mm/proc-arm920.S
@@ -387,7 +387,7 @@ ENTRY(cpu_arm920_set_pte_ext)
 /* Suspend/resume support: taken from arch/arm/plat-s3c24xx/sleep.S */
 .globl	cpu_arm920_suspend_size
 .equ	cpu_arm920_suspend_size, 4 * 3
-#ifdef CONFIG_PM_SLEEP
+#ifdef CONFIG_ARM_CPU_SUSPEND
 ENTRY(cpu_arm920_do_suspend)
 	stmfd	sp!, {r4 - r6, lr}
 	mrc	p15, 0, r4, c13, c0, 0	@ PID
diff --git a/arch/arm/mm/proc-arm926.S b/arch/arm/mm/proc-arm926.S
index f1803f7e297..344c8a548cc 100644
--- a/arch/arm/mm/proc-arm926.S
+++ b/arch/arm/mm/proc-arm926.S
@@ -402,7 +402,7 @@ ENTRY(cpu_arm926_set_pte_ext)
 /* Suspend/resume support: taken from arch/arm/plat-s3c24xx/sleep.S */
 .globl	cpu_arm926_suspend_size
 .equ	cpu_arm926_suspend_size, 4 * 3
-#ifdef CONFIG_PM_SLEEP
+#ifdef CONFIG_ARM_CPU_SUSPEND
 ENTRY(cpu_arm926_do_suspend)
 	stmfd	sp!, {r4 - r6, lr}
 	mrc	p15, 0, r4, c13, c0, 0	@ PID
diff --git a/arch/arm/mm/proc-mohawk.S b/arch/arm/mm/proc-mohawk.S
index 82f9cdc751d..0b60dd3d742 100644
--- a/arch/arm/mm/proc-mohawk.S
+++ b/arch/arm/mm/proc-mohawk.S
@@ -350,7 +350,7 @@ ENTRY(cpu_mohawk_set_pte_ext)
 
 .globl	cpu_mohawk_suspend_size
 .equ	cpu_mohawk_suspend_size, 4 * 6
-#ifdef CONFIG_PM_SLEEP
+#ifdef CONFIG_ARM_CPU_SUSPEND
 ENTRY(cpu_mohawk_do_suspend)
 	stmfd	sp!, {r4 - r9, lr}
 	mrc	p14, 0, r4, c6, c0, 0	@ clock configuration, for turbo mode
diff --git a/arch/arm/mm/proc-sa1100.S b/arch/arm/mm/proc-sa1100.S
index 3aa0da11fd8..d92dfd08142 100644
--- a/arch/arm/mm/proc-sa1100.S
+++ b/arch/arm/mm/proc-sa1100.S
@@ -172,7 +172,7 @@ ENTRY(cpu_sa1100_set_pte_ext)
 
 .globl	cpu_sa1100_suspend_size
 .equ	cpu_sa1100_suspend_size, 4 * 3
-#ifdef CONFIG_PM_SLEEP
+#ifdef CONFIG_ARM_CPU_SUSPEND
 ENTRY(cpu_sa1100_do_suspend)
 	stmfd	sp!, {r4 - r6, lr}
 	mrc	p15, 0, r4, c3, c0, 0		@ domain ID
diff --git a/arch/arm/mm/proc-syms.c b/arch/arm/mm/proc-syms.c
index 3e6210b4d6d..054b491ff76 100644
--- a/arch/arm/mm/proc-syms.c
+++ b/arch/arm/mm/proc-syms.c
@@ -17,7 +17,9 @@
 
 #ifndef MULTI_CPU
 EXPORT_SYMBOL(cpu_dcache_clean_area);
+#ifdef CONFIG_MMU
 EXPORT_SYMBOL(cpu_set_pte_ext);
+#endif
 #else
 EXPORT_SYMBOL(processor);
 #endif
diff --git a/arch/arm/mm/proc-v6.S b/arch/arm/mm/proc-v6.S
index bcaaa8de932..5c07ee4fe3e 100644
--- a/arch/arm/mm/proc-v6.S
+++ b/arch/arm/mm/proc-v6.S
@@ -138,7 +138,7 @@ ENTRY(cpu_v6_set_pte_ext)
 /* Suspend/resume support: taken from arch/arm/mach-s3c64xx/sleep.S */
 .globl	cpu_v6_suspend_size
 .equ	cpu_v6_suspend_size, 4 * 6
-#ifdef CONFIG_PM_SLEEP
+#ifdef CONFIG_ARM_CPU_SUSPEND
 ENTRY(cpu_v6_do_suspend)
 	stmfd	sp!, {r4 - r9, lr}
 	mrc	p15, 0, r4, c13, c0, 0	@ FCSE/PID
diff --git a/arch/arm/mm/proc-v7.S b/arch/arm/mm/proc-v7.S
index 3a3c015f8d5..f584d3f5b37 100644
--- a/arch/arm/mm/proc-v7.S
+++ b/arch/arm/mm/proc-v7.S
@@ -420,7 +420,7 @@ __v7_pj4b_proc_info:
 __v7_ca7mp_proc_info:
 	.long	0x410fc070
 	.long	0xff0ffff0
-	__v7_proc __v7_ca7mp_setup, hwcaps = HWCAP_IDIV
+	__v7_proc __v7_ca7mp_setup
 	.size	__v7_ca7mp_proc_info, . - __v7_ca7mp_proc_info
 
 	/*
@@ -430,10 +430,25 @@ __v7_ca7mp_proc_info:
 __v7_ca15mp_proc_info:
 	.long	0x410fc0f0
 	.long	0xff0ffff0
-	__v7_proc __v7_ca15mp_setup, hwcaps = HWCAP_IDIV
+	__v7_proc __v7_ca15mp_setup
 	.size	__v7_ca15mp_proc_info, . - __v7_ca15mp_proc_info
 
 	/*
+	 * Qualcomm Inc. Krait processors.
+	 */
+	.type	__krait_proc_info, #object
+__krait_proc_info:
+	.long	0x510f0400		@ Required ID value
+	.long	0xff0ffc00		@ Mask for ID
+	/*
+	 * Some Krait processors don't indicate support for SDIV and UDIV
+	 * instructions in the ARM instruction set, even though they actually
+	 * do support them.
+	 */
+	__v7_proc __v7_setup, hwcaps = HWCAP_IDIV
+	.size	__krait_proc_info, . - __krait_proc_info
+
+	/*
 	 * Match any ARMv7 processor core.
 	 */
 	.type	__v7_proc_info, #object
diff --git a/arch/arm/mm/proc-xsc3.S b/arch/arm/mm/proc-xsc3.S
index eb93d6487f3..e8efd83b6f2 100644
--- a/arch/arm/mm/proc-xsc3.S
+++ b/arch/arm/mm/proc-xsc3.S
@@ -413,7 +413,7 @@ ENTRY(cpu_xsc3_set_pte_ext)
 
 .globl	cpu_xsc3_suspend_size
 .equ	cpu_xsc3_suspend_size, 4 * 6
-#ifdef CONFIG_PM_SLEEP
+#ifdef CONFIG_ARM_CPU_SUSPEND
 ENTRY(cpu_xsc3_do_suspend)
 	stmfd	sp!, {r4 - r9, lr}
 	mrc	p14, 0, r4, c6, c0, 0	@ clock configuration, for turbo mode
diff --git a/arch/arm/mm/proc-xscale.S b/arch/arm/mm/proc-xscale.S
index 25510361aa1..e766f889bfd 100644
--- a/arch/arm/mm/proc-xscale.S
+++ b/arch/arm/mm/proc-xscale.S
@@ -528,7 +528,7 @@ ENTRY(cpu_xscale_set_pte_ext)
 
 .globl	cpu_xscale_suspend_size
 .equ	cpu_xscale_suspend_size, 4 * 6
-#ifdef CONFIG_PM_SLEEP
+#ifdef CONFIG_ARM_CPU_SUSPEND
 ENTRY(cpu_xscale_do_suspend)
 	stmfd	sp!, {r4 - r9, lr}
 	mrc	p14, 0, r4, c6, c0, 0	@ clock configuration, for turbo mode
diff --git a/arch/arm/kernel/tcm.h b/arch/arm/mm/tcm.h
index 8015ad434a4..8015ad434a4 100644
--- a/arch/arm/kernel/tcm.h
+++ b/arch/arm/mm/tcm.h
diff --git a/arch/arm/plat-samsung/devs.c b/arch/arm/plat-samsung/devs.c
index 51afedda9ab..03db14d8ace 100644
--- a/arch/arm/plat-samsung/devs.c
+++ b/arch/arm/plat-samsung/devs.c
@@ -10,6 +10,7 @@
  * published by the Free Software Foundation.
 */
 
+#include <linux/amba/pl330.h>
 #include <linux/kernel.h>
 #include <linux/types.h>
 #include <linux/interrupt.h>
@@ -1552,6 +1553,9 @@ void __init s3c64xx_spi0_set_platdata(int (*cfg_gpio)(void), int src_clk_nr,
 	pd.num_cs = num_cs;
 	pd.src_clk_nr = src_clk_nr;
 	pd.cfg_gpio = (cfg_gpio) ? cfg_gpio : s3c64xx_spi0_cfg_gpio;
+#ifdef CONFIG_PL330_DMA
+	pd.filter = pl330_filter;
+#endif
 
 	s3c_set_platdata(&pd, sizeof(pd), &s3c64xx_device_spi0);
 }
@@ -1590,6 +1594,9 @@ void __init s3c64xx_spi1_set_platdata(int (*cfg_gpio)(void), int src_clk_nr,
 	pd.num_cs = num_cs;
 	pd.src_clk_nr = src_clk_nr;
 	pd.cfg_gpio = (cfg_gpio) ? cfg_gpio : s3c64xx_spi1_cfg_gpio;
+#ifdef CONFIG_PL330_DMA
+	pd.filter = pl330_filter;
+#endif
 
 	s3c_set_platdata(&pd, sizeof(pd), &s3c64xx_device_spi1);
 }
@@ -1628,6 +1635,9 @@ void __init s3c64xx_spi2_set_platdata(int (*cfg_gpio)(void), int src_clk_nr,
 	pd.num_cs = num_cs;
 	pd.src_clk_nr = src_clk_nr;
 	pd.cfg_gpio = (cfg_gpio) ? cfg_gpio : s3c64xx_spi2_cfg_gpio;
+#ifdef CONFIG_PL330_DMA
+	pd.filter = pl330_filter;
+#endif
 
 	s3c_set_platdata(&pd, sizeof(pd), &s3c64xx_device_spi2);
 }
diff --git a/arch/arm/plat-samsung/include/plat/fb.h b/arch/arm/plat-samsung/include/plat/fb.h
index b885322717a..9ae50727078 100644
--- a/arch/arm/plat-samsung/include/plat/fb.h
+++ b/arch/arm/plat-samsung/include/plat/fb.h
@@ -15,55 +15,7 @@
 #ifndef __PLAT_S3C_FB_H
 #define __PLAT_S3C_FB_H __FILE__
 
-/* S3C_FB_MAX_WIN
- * Set to the maximum number of windows that any of the supported hardware
- * can use. Since the platform data uses this for an array size, having it
- * set to the maximum of any version of the hardware can do is safe.
- */
-#define S3C_FB_MAX_WIN	(5)
-
-/**
- * struct s3c_fb_pd_win - per window setup data
- * @xres     : The window X size.
- * @yres     : The window Y size.
- * @virtual_x: The virtual X size.
- * @virtual_y: The virtual Y size.
- */
-struct s3c_fb_pd_win {
-	unsigned short		default_bpp;
-	unsigned short		max_bpp;
-	unsigned short		xres;
-	unsigned short		yres;
-	unsigned short		virtual_x;
-	unsigned short		virtual_y;
-};
-
-/**
- * struct s3c_fb_platdata -  S3C driver platform specific information
- * @setup_gpio: Setup the external GPIO pins to the right state to transfer
- *		the data from the display system to the connected display
- *		device.
- * @vidcon0: The base vidcon0 values to control the panel data format.
- * @vidcon1: The base vidcon1 values to control the panel data output.
- * @vtiming: Video timing when connected to a RGB type panel.
- * @win: The setup data for each hardware window, or NULL for unused.
- * @display_mode: The LCD output display mode.
- *
- * The platform data supplies the video driver with all the information
- * it requires to work with the display(s) attached to the machine. It
- * controls the initial mode, the number of display windows (0 is always
- * the base framebuffer) that are initialised etc.
- *
- */
-struct s3c_fb_platdata {
-	void	(*setup_gpio)(void);
-
-	struct s3c_fb_pd_win	*win[S3C_FB_MAX_WIN];
-	struct fb_videomode     *vtiming;
-
-	u32			 vidcon0;
-	u32			 vidcon1;
-};
+#include <linux/platform_data/video_s3c.h>
 
 /**
  * s3c_fb_set_platdata() - Setup the FB device with platform data.
diff --git a/arch/arm/plat-samsung/include/plat/regs-serial.h b/arch/arm/plat-samsung/include/plat/regs-serial.h
index 29c26a81884..f05f2afa440 100644
--- a/arch/arm/plat-samsung/include/plat/regs-serial.h
+++ b/arch/arm/plat-samsung/include/plat/regs-serial.h
@@ -1,281 +1 @@
-/* arch/arm/plat-samsung/include/plat/regs-serial.h
- *
- *  From linux/include/asm-arm/hardware/serial_s3c2410.h
- *
- *  Internal header file for Samsung S3C2410 serial ports (UART0-2)
- *
- *  Copyright (C) 2002 Shane Nay (shane@minirl.com)
- *
- *  Additional defines, Copyright 2003 Simtec Electronics (linux@simtec.co.uk)
- *
- *  Adapted from:
- *
- *  Internal header file for MX1ADS serial ports (UART1 & 2)
- *
- *  Copyright (C) 2002 Shane Nay (shane@minirl.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
-*/
-
-#ifndef __ASM_ARM_REGS_SERIAL_H
-#define __ASM_ARM_REGS_SERIAL_H
-
-#define S3C24XX_VA_UART0      (S3C_VA_UART)
-#define S3C24XX_VA_UART1      (S3C_VA_UART + 0x4000 )
-#define S3C24XX_VA_UART2      (S3C_VA_UART + 0x8000 )
-#define S3C24XX_VA_UART3      (S3C_VA_UART + 0xC000 )
-
-#define S3C2410_PA_UART0      (S3C24XX_PA_UART)
-#define S3C2410_PA_UART1      (S3C24XX_PA_UART + 0x4000 )
-#define S3C2410_PA_UART2      (S3C24XX_PA_UART + 0x8000 )
-#define S3C2443_PA_UART3      (S3C24XX_PA_UART + 0xC000 )
-
-#define S3C2410_URXH	  (0x24)
-#define S3C2410_UTXH	  (0x20)
-#define S3C2410_ULCON	  (0x00)
-#define S3C2410_UCON	  (0x04)
-#define S3C2410_UFCON	  (0x08)
-#define S3C2410_UMCON	  (0x0C)
-#define S3C2410_UBRDIV	  (0x28)
-#define S3C2410_UTRSTAT	  (0x10)
-#define S3C2410_UERSTAT	  (0x14)
-#define S3C2410_UFSTAT	  (0x18)
-#define S3C2410_UMSTAT	  (0x1C)
-
-#define S3C2410_LCON_CFGMASK	  ((0xF<<3)|(0x3))
-
-#define S3C2410_LCON_CS5	  (0x0)
-#define S3C2410_LCON_CS6	  (0x1)
-#define S3C2410_LCON_CS7	  (0x2)
-#define S3C2410_LCON_CS8	  (0x3)
-#define S3C2410_LCON_CSMASK	  (0x3)
-
-#define S3C2410_LCON_PNONE	  (0x0)
-#define S3C2410_LCON_PEVEN	  (0x5 << 3)
-#define S3C2410_LCON_PODD	  (0x4 << 3)
-#define S3C2410_LCON_PMASK	  (0x7 << 3)
-
-#define S3C2410_LCON_STOPB	  (1<<2)
-#define S3C2410_LCON_IRM          (1<<6)
-
-#define S3C2440_UCON_CLKMASK	  (3<<10)
-#define S3C2440_UCON_CLKSHIFT	  (10)
-#define S3C2440_UCON_PCLK	  (0<<10)
-#define S3C2440_UCON_UCLK	  (1<<10)
-#define S3C2440_UCON_PCLK2	  (2<<10)
-#define S3C2440_UCON_FCLK	  (3<<10)
-#define S3C2443_UCON_EPLL	  (3<<10)
-
-#define S3C6400_UCON_CLKMASK	(3<<10)
-#define S3C6400_UCON_CLKSHIFT	(10)
-#define S3C6400_UCON_PCLK	(0<<10)
-#define S3C6400_UCON_PCLK2	(2<<10)
-#define S3C6400_UCON_UCLK0	(1<<10)
-#define S3C6400_UCON_UCLK1	(3<<10)
-
-#define S3C2440_UCON2_FCLK_EN	  (1<<15)
-#define S3C2440_UCON0_DIVMASK	  (15 << 12)
-#define S3C2440_UCON1_DIVMASK	  (15 << 12)
-#define S3C2440_UCON2_DIVMASK	  (7 << 12)
-#define S3C2440_UCON_DIVSHIFT	  (12)
-
-#define S3C2412_UCON_CLKMASK	(3<<10)
-#define S3C2412_UCON_CLKSHIFT	(10)
-#define S3C2412_UCON_UCLK	(1<<10)
-#define S3C2412_UCON_USYSCLK	(3<<10)
-#define S3C2412_UCON_PCLK	(0<<10)
-#define S3C2412_UCON_PCLK2	(2<<10)
-
-#define S3C2410_UCON_CLKMASK	(1 << 10)
-#define S3C2410_UCON_CLKSHIFT	(10)
-#define S3C2410_UCON_UCLK	  (1<<10)
-#define S3C2410_UCON_SBREAK	  (1<<4)
-
-#define S3C2410_UCON_TXILEVEL	  (1<<9)
-#define S3C2410_UCON_RXILEVEL	  (1<<8)
-#define S3C2410_UCON_TXIRQMODE	  (1<<2)
-#define S3C2410_UCON_RXIRQMODE	  (1<<0)
-#define S3C2410_UCON_RXFIFO_TOI	  (1<<7)
-#define S3C2443_UCON_RXERR_IRQEN  (1<<6)
-#define S3C2443_UCON_LOOPBACK	  (1<<5)
-
-#define S3C2410_UCON_DEFAULT	  (S3C2410_UCON_TXILEVEL  | \
-				   S3C2410_UCON_RXILEVEL  | \
-				   S3C2410_UCON_TXIRQMODE | \
-				   S3C2410_UCON_RXIRQMODE | \
-				   S3C2410_UCON_RXFIFO_TOI)
-
-#define S3C2410_UFCON_FIFOMODE	  (1<<0)
-#define S3C2410_UFCON_TXTRIG0	  (0<<6)
-#define S3C2410_UFCON_RXTRIG8	  (1<<4)
-#define S3C2410_UFCON_RXTRIG12	  (2<<4)
-
-/* S3C2440 FIFO trigger levels */
-#define S3C2440_UFCON_RXTRIG1	  (0<<4)
-#define S3C2440_UFCON_RXTRIG8	  (1<<4)
-#define S3C2440_UFCON_RXTRIG16	  (2<<4)
-#define S3C2440_UFCON_RXTRIG32	  (3<<4)
-
-#define S3C2440_UFCON_TXTRIG0	  (0<<6)
-#define S3C2440_UFCON_TXTRIG16	  (1<<6)
-#define S3C2440_UFCON_TXTRIG32	  (2<<6)
-#define S3C2440_UFCON_TXTRIG48	  (3<<6)
-
-#define S3C2410_UFCON_RESETBOTH	  (3<<1)
-#define S3C2410_UFCON_RESETTX	  (1<<2)
-#define S3C2410_UFCON_RESETRX	  (1<<1)
-
-#define S3C2410_UFCON_DEFAULT	  (S3C2410_UFCON_FIFOMODE | \
-				   S3C2410_UFCON_TXTRIG0  | \
-				   S3C2410_UFCON_RXTRIG8 )
-
-#define	S3C2410_UMCOM_AFC	  (1<<4)
-#define	S3C2410_UMCOM_RTS_LOW	  (1<<0)
-
-#define S3C2412_UMCON_AFC_63	(0<<5)		/* same as s3c2443 */
-#define S3C2412_UMCON_AFC_56	(1<<5)
-#define S3C2412_UMCON_AFC_48	(2<<5)
-#define S3C2412_UMCON_AFC_40	(3<<5)
-#define S3C2412_UMCON_AFC_32	(4<<5)
-#define S3C2412_UMCON_AFC_24	(5<<5)
-#define S3C2412_UMCON_AFC_16	(6<<5)
-#define S3C2412_UMCON_AFC_8	(7<<5)
-
-#define S3C2410_UFSTAT_TXFULL	  (1<<9)
-#define S3C2410_UFSTAT_RXFULL	  (1<<8)
-#define S3C2410_UFSTAT_TXMASK	  (15<<4)
-#define S3C2410_UFSTAT_TXSHIFT	  (4)
-#define S3C2410_UFSTAT_RXMASK	  (15<<0)
-#define S3C2410_UFSTAT_RXSHIFT	  (0)
-
-/* UFSTAT S3C2443 same as S3C2440 */
-#define S3C2440_UFSTAT_TXFULL	  (1<<14)
-#define S3C2440_UFSTAT_RXFULL	  (1<<6)
-#define S3C2440_UFSTAT_TXSHIFT	  (8)
-#define S3C2440_UFSTAT_RXSHIFT	  (0)
-#define S3C2440_UFSTAT_TXMASK	  (63<<8)
-#define S3C2440_UFSTAT_RXMASK	  (63)
-
-#define S3C2410_UTRSTAT_TXE	  (1<<2)
-#define S3C2410_UTRSTAT_TXFE	  (1<<1)
-#define S3C2410_UTRSTAT_RXDR	  (1<<0)
-
-#define S3C2410_UERSTAT_OVERRUN	  (1<<0)
-#define S3C2410_UERSTAT_FRAME	  (1<<2)
-#define S3C2410_UERSTAT_BREAK	  (1<<3)
-#define S3C2443_UERSTAT_PARITY	  (1<<1)
-
-#define S3C2410_UERSTAT_ANY	  (S3C2410_UERSTAT_OVERRUN | \
-				   S3C2410_UERSTAT_FRAME | \
-				   S3C2410_UERSTAT_BREAK)
-
-#define S3C2410_UMSTAT_CTS	  (1<<0)
-#define S3C2410_UMSTAT_DeltaCTS	  (1<<2)
-
-#define S3C2443_DIVSLOT		  (0x2C)
-
-/* S3C64XX interrupt registers. */
-#define S3C64XX_UINTP		0x30
-#define S3C64XX_UINTSP		0x34
-#define S3C64XX_UINTM		0x38
-
-#define S3C64XX_UINTM_RXD	(0)
-#define S3C64XX_UINTM_TXD	(2)
-#define S3C64XX_UINTM_RXD_MSK	(1 << S3C64XX_UINTM_RXD)
-#define S3C64XX_UINTM_TXD_MSK	(1 << S3C64XX_UINTM_TXD)
-
-/* Following are specific to S5PV210 */
-#define S5PV210_UCON_CLKMASK	(1<<10)
-#define S5PV210_UCON_CLKSHIFT	(10)
-#define S5PV210_UCON_PCLK	(0<<10)
-#define S5PV210_UCON_UCLK	(1<<10)
-
-#define S5PV210_UFCON_TXTRIG0	(0<<8)
-#define S5PV210_UFCON_TXTRIG4	(1<<8)
-#define S5PV210_UFCON_TXTRIG8	(2<<8)
-#define S5PV210_UFCON_TXTRIG16	(3<<8)
-#define S5PV210_UFCON_TXTRIG32	(4<<8)
-#define S5PV210_UFCON_TXTRIG64	(5<<8)
-#define S5PV210_UFCON_TXTRIG128 (6<<8)
-#define S5PV210_UFCON_TXTRIG256 (7<<8)
-
-#define S5PV210_UFCON_RXTRIG1	(0<<4)
-#define S5PV210_UFCON_RXTRIG4	(1<<4)
-#define S5PV210_UFCON_RXTRIG8	(2<<4)
-#define S5PV210_UFCON_RXTRIG16	(3<<4)
-#define S5PV210_UFCON_RXTRIG32	(4<<4)
-#define S5PV210_UFCON_RXTRIG64	(5<<4)
-#define S5PV210_UFCON_RXTRIG128	(6<<4)
-#define S5PV210_UFCON_RXTRIG256	(7<<4)
-
-#define S5PV210_UFSTAT_TXFULL	(1<<24)
-#define S5PV210_UFSTAT_RXFULL	(1<<8)
-#define S5PV210_UFSTAT_TXMASK	(255<<16)
-#define S5PV210_UFSTAT_TXSHIFT	(16)
-#define S5PV210_UFSTAT_RXMASK	(255<<0)
-#define S5PV210_UFSTAT_RXSHIFT	(0)
-
-#define S3C2410_UCON_CLKSEL0	(1 << 0)
-#define S3C2410_UCON_CLKSEL1	(1 << 1)
-#define S3C2410_UCON_CLKSEL2	(1 << 2)
-#define S3C2410_UCON_CLKSEL3	(1 << 3)
-
-/* Default values for s5pv210 UCON and UFCON uart registers */
-#define S5PV210_UCON_DEFAULT	(S3C2410_UCON_TXILEVEL |	\
-				 S3C2410_UCON_RXILEVEL |	\
-				 S3C2410_UCON_TXIRQMODE |	\
-				 S3C2410_UCON_RXIRQMODE |	\
-				 S3C2410_UCON_RXFIFO_TOI |	\
-				 S3C2443_UCON_RXERR_IRQEN)
-
-#define S5PV210_UFCON_DEFAULT	(S3C2410_UFCON_FIFOMODE |	\
-				 S5PV210_UFCON_TXTRIG4 |	\
-				 S5PV210_UFCON_RXTRIG4)
-
-#ifndef __ASSEMBLY__
-
-/* configuration structure for per-machine configurations for the
- * serial port
- *
- * the pointer is setup by the machine specific initialisation from the
- * arch/arm/mach-s3c2410/ directory.
-*/
-
-struct s3c2410_uartcfg {
-	unsigned char	   hwport;	 /* hardware port number */
-	unsigned char	   unused;
-	unsigned short	   flags;
-	upf_t		   uart_flags;	 /* default uart flags */
-	unsigned int	   clk_sel;
-
-	unsigned int	   has_fracval;
-
-	unsigned long	   ucon;	 /* value of ucon for port */
-	unsigned long	   ulcon;	 /* value of ulcon for port */
-	unsigned long	   ufcon;	 /* value of ufcon for port */
-};
-
-/* s3c24xx_uart_devs
- *
- * this is exported from the core as we cannot use driver_register(),
- * or platform_add_device() before the console_initcall()
-*/
-
-extern struct platform_device *s3c24xx_uart_devs[4];
-
-#endif /* __ASSEMBLY__ */
-
-#endif /* __ASM_ARM_REGS_SERIAL_H */
-
+#include <linux/serial_s3c.h>
diff --git a/arch/arm/plat-samsung/include/plat/usb-phy.h b/arch/arm/plat-samsung/include/plat/usb-phy.h
index 959bcdb03a2..ab34dfadb7f 100644
--- a/arch/arm/plat-samsung/include/plat/usb-phy.h
+++ b/arch/arm/plat-samsung/include/plat/usb-phy.h
@@ -11,10 +11,7 @@
 #ifndef __PLAT_SAMSUNG_USB_PHY_H
 #define __PLAT_SAMSUNG_USB_PHY_H __FILE__
 
-enum s5p_usb_phy_type {
-	S5P_USB_PHY_DEVICE,
-	S5P_USB_PHY_HOST,
-};
+#include <linux/usb/samsung_usb_phy.h>
 
 extern int s5p_usb_phy_init(struct platform_device *pdev, int type);
 extern int s5p_usb_phy_exit(struct platform_device *pdev, int type);
diff --git a/arch/arm64/Kconfig b/arch/arm64/Kconfig
index 9b6d19f7407..73b6e764034 100644
--- a/arch/arm64/Kconfig
+++ b/arch/arm64/Kconfig
@@ -99,7 +99,16 @@ source "init/Kconfig"
 
 source "kernel/Kconfig.freezer"
 
-menu "System Type"
+menu "Platform selection"
+
+config ARCH_VEXPRESS
+	bool "ARMv8 software model (Versatile Express)"
+	select ARCH_REQUIRE_GPIOLIB
+	select COMMON_CLK_VERSATILE
+	select VEXPRESS_CONFIG
+	help
+	  This enables support for the ARMv8 software model (Versatile
+	  Express).
 
 endmenu
 
diff --git a/arch/arm64/boot/dts/Makefile b/arch/arm64/boot/dts/Makefile
index 32ac0aef006..68457e9e097 100644
--- a/arch/arm64/boot/dts/Makefile
+++ b/arch/arm64/boot/dts/Makefile
@@ -1,3 +1,5 @@
+dtb-$(CONFIG_ARCH_VEXPRESS) += rtsm_ve-aemv8a.dtb foundation-v8.dtb
+
 targets += dtbs
 targets += $(dtb-y)
 
diff --git a/arch/arm64/boot/dts/foundation-v8.dts b/arch/arm64/boot/dts/foundation-v8.dts
new file mode 100644
index 00000000000..198682b6de3
--- /dev/null
+++ b/arch/arm64/boot/dts/foundation-v8.dts
@@ -0,0 +1,230 @@
+/*
+ * ARM Ltd.
+ *
+ * ARMv8 Foundation model DTS
+ */
+
+/dts-v1/;
+
+/ {
+	model = "Foundation-v8A";
+	compatible = "arm,foundation-aarch64", "arm,vexpress";
+	interrupt-parent = <&gic>;
+	#address-cells = <2>;
+	#size-cells = <2>;
+
+	chosen { };
+
+	aliases {
+		serial0 = &v2m_serial0;
+		serial1 = &v2m_serial1;
+		serial2 = &v2m_serial2;
+		serial3 = &v2m_serial3;
+	};
+
+	cpus {
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		cpu@0 {
+			device_type = "cpu";
+			compatible = "arm,armv8";
+			reg = <0x0 0x0>;
+			enable-method = "spin-table";
+			cpu-release-addr = <0x0 0x8000fff8>;
+		};
+		cpu@1 {
+			device_type = "cpu";
+			compatible = "arm,armv8";
+			reg = <0x0 0x1>;
+			enable-method = "spin-table";
+			cpu-release-addr = <0x0 0x8000fff8>;
+		};
+		cpu@2 {
+			device_type = "cpu";
+			compatible = "arm,armv8";
+			reg = <0x0 0x2>;
+			enable-method = "spin-table";
+			cpu-release-addr = <0x0 0x8000fff8>;
+		};
+		cpu@3 {
+			device_type = "cpu";
+			compatible = "arm,armv8";
+			reg = <0x0 0x3>;
+			enable-method = "spin-table";
+			cpu-release-addr = <0x0 0x8000fff8>;
+		};
+	};
+
+	memory@80000000 {
+		device_type = "memory";
+		reg = <0x00000000 0x80000000 0 0x80000000>,
+		      <0x00000008 0x80000000 0 0x80000000>;
+	};
+
+	gic: interrupt-controller@2c001000 {
+		compatible = "arm,cortex-a15-gic", "arm,cortex-a9-gic";
+		#interrupt-cells = <3>;
+		#address-cells = <0>;
+		interrupt-controller;
+		reg = <0x0 0x2c001000 0 0x1000>,
+		      <0x0 0x2c002000 0 0x1000>,
+		      <0x0 0x2c004000 0 0x2000>,
+		      <0x0 0x2c006000 0 0x2000>;
+		interrupts = <1 9 0xf04>;
+	};
+
+	timer {
+		compatible = "arm,armv8-timer";
+		interrupts = <1 13 0xff01>,
+			     <1 14 0xff01>,
+			     <1 11 0xff01>,
+			     <1 10 0xff01>;
+		clock-frequency = <100000000>;
+	};
+
+	pmu {
+		compatible = "arm,armv8-pmuv3";
+		interrupts = <0 60 4>,
+			     <0 61 4>,
+			     <0 62 4>,
+			     <0 63 4>;
+	};
+
+	smb {
+		compatible = "arm,vexpress,v2m-p1", "simple-bus";
+		arm,v2m-memory-map = "rs1";
+		#address-cells = <2>; /* SMB chipselect number and offset */
+		#size-cells = <1>;
+
+		ranges = <0 0 0 0x08000000 0x04000000>,
+			 <1 0 0 0x14000000 0x04000000>,
+			 <2 0 0 0x18000000 0x04000000>,
+			 <3 0 0 0x1c000000 0x04000000>,
+			 <4 0 0 0x0c000000 0x04000000>,
+			 <5 0 0 0x10000000 0x04000000>;
+
+		#interrupt-cells = <1>;
+		interrupt-map-mask = <0 0 63>;
+		interrupt-map = <0 0  0 &gic 0  0 4>,
+				<0 0  1 &gic 0  1 4>,
+				<0 0  2 &gic 0  2 4>,
+				<0 0  3 &gic 0  3 4>,
+				<0 0  4 &gic 0  4 4>,
+				<0 0  5 &gic 0  5 4>,
+				<0 0  6 &gic 0  6 4>,
+				<0 0  7 &gic 0  7 4>,
+				<0 0  8 &gic 0  8 4>,
+				<0 0  9 &gic 0  9 4>,
+				<0 0 10 &gic 0 10 4>,
+				<0 0 11 &gic 0 11 4>,
+				<0 0 12 &gic 0 12 4>,
+				<0 0 13 &gic 0 13 4>,
+				<0 0 14 &gic 0 14 4>,
+				<0 0 15 &gic 0 15 4>,
+				<0 0 16 &gic 0 16 4>,
+				<0 0 17 &gic 0 17 4>,
+				<0 0 18 &gic 0 18 4>,
+				<0 0 19 &gic 0 19 4>,
+				<0 0 20 &gic 0 20 4>,
+				<0 0 21 &gic 0 21 4>,
+				<0 0 22 &gic 0 22 4>,
+				<0 0 23 &gic 0 23 4>,
+				<0 0 24 &gic 0 24 4>,
+				<0 0 25 &gic 0 25 4>,
+				<0 0 26 &gic 0 26 4>,
+				<0 0 27 &gic 0 27 4>,
+				<0 0 28 &gic 0 28 4>,
+				<0 0 29 &gic 0 29 4>,
+				<0 0 30 &gic 0 30 4>,
+				<0 0 31 &gic 0 31 4>,
+				<0 0 32 &gic 0 32 4>,
+				<0 0 33 &gic 0 33 4>,
+				<0 0 34 &gic 0 34 4>,
+				<0 0 35 &gic 0 35 4>,
+				<0 0 36 &gic 0 36 4>,
+				<0 0 37 &gic 0 37 4>,
+				<0 0 38 &gic 0 38 4>,
+				<0 0 39 &gic 0 39 4>,
+				<0 0 40 &gic 0 40 4>,
+				<0 0 41 &gic 0 41 4>,
+				<0 0 42 &gic 0 42 4>;
+
+		ethernet@2,02000000 {
+			compatible = "smsc,lan91c111";
+			reg = <2 0x02000000 0x10000>;
+			interrupts = <15>;
+		};
+
+		v2m_clk24mhz: clk24mhz {
+			compatible = "fixed-clock";
+			#clock-cells = <0>;
+			clock-frequency = <24000000>;
+			clock-output-names = "v2m:clk24mhz";
+		};
+
+		v2m_refclk1mhz: refclk1mhz {
+			compatible = "fixed-clock";
+			#clock-cells = <0>;
+			clock-frequency = <1000000>;
+			clock-output-names = "v2m:refclk1mhz";
+		};
+
+		v2m_refclk32khz: refclk32khz {
+			compatible = "fixed-clock";
+			#clock-cells = <0>;
+			clock-frequency = <32768>;
+			clock-output-names = "v2m:refclk32khz";
+		};
+
+		iofpga@3,00000000 {
+			compatible = "arm,amba-bus", "simple-bus";
+			#address-cells = <1>;
+			#size-cells = <1>;
+			ranges = <0 3 0 0x200000>;
+
+			v2m_sysreg: sysreg@010000 {
+				compatible = "arm,vexpress-sysreg";
+				reg = <0x010000 0x1000>;
+			};
+
+			v2m_serial0: uart@090000 {
+				compatible = "arm,pl011", "arm,primecell";
+				reg = <0x090000 0x1000>;
+				interrupts = <5>;
+				clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+				clock-names = "uartclk", "apb_pclk";
+			};
+
+			v2m_serial1: uart@0a0000 {
+				compatible = "arm,pl011", "arm,primecell";
+				reg = <0x0a0000 0x1000>;
+				interrupts = <6>;
+				clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+				clock-names = "uartclk", "apb_pclk";
+			};
+
+			v2m_serial2: uart@0b0000 {
+				compatible = "arm,pl011", "arm,primecell";
+				reg = <0x0b0000 0x1000>;
+				interrupts = <7>;
+				clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+				clock-names = "uartclk", "apb_pclk";
+			};
+
+			v2m_serial3: uart@0c0000 {
+				compatible = "arm,pl011", "arm,primecell";
+				reg = <0x0c0000 0x1000>;
+				interrupts = <8>;
+				clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+				clock-names = "uartclk", "apb_pclk";
+			};
+
+			virtio_block@0130000 {
+				compatible = "virtio,mmio";
+				reg = <0x130000 0x1000>;
+				interrupts = <42>;
+			};
+		};
+	};
+};
diff --git a/arch/arm64/boot/dts/rtsm_ve-aemv8a.dts b/arch/arm64/boot/dts/rtsm_ve-aemv8a.dts
new file mode 100644
index 00000000000..572005ea221
--- /dev/null
+++ b/arch/arm64/boot/dts/rtsm_ve-aemv8a.dts
@@ -0,0 +1,159 @@
+/*
+ * ARM Ltd. Fast Models
+ *
+ * Architecture Envelope Model (AEM) ARMv8-A
+ * ARMAEMv8AMPCT
+ *
+ * RTSM_VE_AEMv8A.lisa
+ */
+
+/dts-v1/;
+
+/memreserve/ 0x80000000 0x00010000;
+
+/ {
+	model = "RTSM_VE_AEMv8A";
+	compatible = "arm,rtsm_ve,aemv8a", "arm,vexpress";
+	interrupt-parent = <&gic>;
+	#address-cells = <2>;
+	#size-cells = <2>;
+
+	chosen { };
+
+	aliases {
+		serial0 = &v2m_serial0;
+		serial1 = &v2m_serial1;
+		serial2 = &v2m_serial2;
+		serial3 = &v2m_serial3;
+	};
+
+	cpus {
+		#address-cells = <2>;
+		#size-cells = <0>;
+
+		cpu@0 {
+			device_type = "cpu";
+			compatible = "arm,armv8";
+			reg = <0x0 0x0>;
+			enable-method = "spin-table";
+			cpu-release-addr = <0x0 0x8000fff8>;
+		};
+		cpu@1 {
+			device_type = "cpu";
+			compatible = "arm,armv8";
+			reg = <0x0 0x1>;
+			enable-method = "spin-table";
+			cpu-release-addr = <0x0 0x8000fff8>;
+		};
+		cpu@2 {
+			device_type = "cpu";
+			compatible = "arm,armv8";
+			reg = <0x0 0x2>;
+			enable-method = "spin-table";
+			cpu-release-addr = <0x0 0x8000fff8>;
+		};
+		cpu@3 {
+			device_type = "cpu";
+			compatible = "arm,armv8";
+			reg = <0x0 0x3>;
+			enable-method = "spin-table";
+			cpu-release-addr = <0x0 0x8000fff8>;
+		};
+	};
+
+	memory@80000000 {
+		device_type = "memory";
+		reg = <0x00000000 0x80000000 0 0x80000000>,
+		      <0x00000008 0x80000000 0 0x80000000>;
+	};
+
+	gic: interrupt-controller@2c001000 {
+		compatible = "arm,cortex-a15-gic", "arm,cortex-a9-gic";
+		#interrupt-cells = <3>;
+		#address-cells = <0>;
+		interrupt-controller;
+		reg = <0x0 0x2c001000 0 0x1000>,
+		      <0x0 0x2c002000 0 0x1000>,
+		      <0x0 0x2c004000 0 0x2000>,
+		      <0x0 0x2c006000 0 0x2000>;
+		interrupts = <1 9 0xf04>;
+	};
+
+	timer {
+		compatible = "arm,armv8-timer";
+		interrupts = <1 13 0xff01>,
+			     <1 14 0xff01>,
+			     <1 11 0xff01>,
+			     <1 10 0xff01>;
+		clock-frequency = <100000000>;
+	};
+
+	pmu {
+		compatible = "arm,armv8-pmuv3";
+		interrupts = <0 60 4>,
+			     <0 61 4>,
+			     <0 62 4>,
+			     <0 63 4>;
+	};
+
+	smb {
+		compatible = "simple-bus";
+
+		#address-cells = <2>;
+		#size-cells = <1>;
+		ranges = <0 0 0 0x08000000 0x04000000>,
+			 <1 0 0 0x14000000 0x04000000>,
+			 <2 0 0 0x18000000 0x04000000>,
+			 <3 0 0 0x1c000000 0x04000000>,
+			 <4 0 0 0x0c000000 0x04000000>,
+			 <5 0 0 0x10000000 0x04000000>;
+
+		#interrupt-cells = <1>;
+		interrupt-map-mask = <0 0 63>;
+		interrupt-map = <0 0  0 &gic 0  0 4>,
+				<0 0  1 &gic 0  1 4>,
+				<0 0  2 &gic 0  2 4>,
+				<0 0  3 &gic 0  3 4>,
+				<0 0  4 &gic 0  4 4>,
+				<0 0  5 &gic 0  5 4>,
+				<0 0  6 &gic 0  6 4>,
+				<0 0  7 &gic 0  7 4>,
+				<0 0  8 &gic 0  8 4>,
+				<0 0  9 &gic 0  9 4>,
+				<0 0 10 &gic 0 10 4>,
+				<0 0 11 &gic 0 11 4>,
+				<0 0 12 &gic 0 12 4>,
+				<0 0 13 &gic 0 13 4>,
+				<0 0 14 &gic 0 14 4>,
+				<0 0 15 &gic 0 15 4>,
+				<0 0 16 &gic 0 16 4>,
+				<0 0 17 &gic 0 17 4>,
+				<0 0 18 &gic 0 18 4>,
+				<0 0 19 &gic 0 19 4>,
+				<0 0 20 &gic 0 20 4>,
+				<0 0 21 &gic 0 21 4>,
+				<0 0 22 &gic 0 22 4>,
+				<0 0 23 &gic 0 23 4>,
+				<0 0 24 &gic 0 24 4>,
+				<0 0 25 &gic 0 25 4>,
+				<0 0 26 &gic 0 26 4>,
+				<0 0 27 &gic 0 27 4>,
+				<0 0 28 &gic 0 28 4>,
+				<0 0 29 &gic 0 29 4>,
+				<0 0 30 &gic 0 30 4>,
+				<0 0 31 &gic 0 31 4>,
+				<0 0 32 &gic 0 32 4>,
+				<0 0 33 &gic 0 33 4>,
+				<0 0 34 &gic 0 34 4>,
+				<0 0 35 &gic 0 35 4>,
+				<0 0 36 &gic 0 36 4>,
+				<0 0 37 &gic 0 37 4>,
+				<0 0 38 &gic 0 38 4>,
+				<0 0 39 &gic 0 39 4>,
+				<0 0 40 &gic 0 40 4>,
+				<0 0 41 &gic 0 41 4>,
+				<0 0 42 &gic 0 42 4>;
+
+		/include/ "rtsm_ve-motherboard.dtsi"
+	};
+};
diff --git a/arch/arm64/boot/dts/rtsm_ve-motherboard.dtsi b/arch/arm64/boot/dts/rtsm_ve-motherboard.dtsi
new file mode 100644
index 00000000000..b45e5f39f57
--- /dev/null
+++ b/arch/arm64/boot/dts/rtsm_ve-motherboard.dtsi
@@ -0,0 +1,234 @@
+/*
+ * ARM Ltd. Fast Models
+ *
+ * Versatile Express (VE) system model
+ * Motherboard component
+ *
+ * VEMotherBoard.lisa
+ */
+
+	motherboard {
+		arm,v2m-memory-map = "rs1";
+		compatible = "arm,vexpress,v2m-p1", "simple-bus";
+		#address-cells = <2>; /* SMB chipselect number and offset */
+		#size-cells = <1>;
+		#interrupt-cells = <1>;
+		ranges;
+
+		flash@0,00000000 {
+			compatible = "arm,vexpress-flash", "cfi-flash";
+			reg = <0 0x00000000 0x04000000>,
+			      <4 0x00000000 0x04000000>;
+			bank-width = <4>;
+		};
+
+		vram@2,00000000 {
+			compatible = "arm,vexpress-vram";
+			reg = <2 0x00000000 0x00800000>;
+		};
+
+		ethernet@2,02000000 {
+			compatible = "smsc,lan91c111";
+			reg = <2 0x02000000 0x10000>;
+			interrupts = <15>;
+		};
+
+		v2m_clk24mhz: clk24mhz {
+			compatible = "fixed-clock";
+			#clock-cells = <0>;
+			clock-frequency = <24000000>;
+			clock-output-names = "v2m:clk24mhz";
+		};
+
+		v2m_refclk1mhz: refclk1mhz {
+			compatible = "fixed-clock";
+			#clock-cells = <0>;
+			clock-frequency = <1000000>;
+			clock-output-names = "v2m:refclk1mhz";
+		};
+
+		v2m_refclk32khz: refclk32khz {
+			compatible = "fixed-clock";
+			#clock-cells = <0>;
+			clock-frequency = <32768>;
+			clock-output-names = "v2m:refclk32khz";
+		};
+
+		iofpga@3,00000000 {
+			compatible = "arm,amba-bus", "simple-bus";
+			#address-cells = <1>;
+			#size-cells = <1>;
+			ranges = <0 3 0 0x200000>;
+
+			v2m_sysreg: sysreg@010000 {
+				compatible = "arm,vexpress-sysreg";
+				reg = <0x010000 0x1000>;
+				gpio-controller;
+				#gpio-cells = <2>;
+			};
+
+			v2m_sysctl: sysctl@020000 {
+				compatible = "arm,sp810", "arm,primecell";
+				reg = <0x020000 0x1000>;
+				clocks = <&v2m_refclk32khz>, <&v2m_refclk1mhz>, <&v2m_clk24mhz>;
+				clock-names = "refclk", "timclk", "apb_pclk";
+				#clock-cells = <1>;
+				clock-output-names = "timerclken0", "timerclken1", "timerclken2", "timerclken3";
+			};
+
+			aaci@040000 {
+				compatible = "arm,pl041", "arm,primecell";
+				reg = <0x040000 0x1000>;
+				interrupts = <11>;
+				clocks = <&v2m_clk24mhz>;
+				clock-names = "apb_pclk";
+			};
+
+			mmci@050000 {
+				compatible = "arm,pl180", "arm,primecell";
+				reg = <0x050000 0x1000>;
+				interrupts = <9 10>;
+				cd-gpios = <&v2m_sysreg 0 0>;
+				wp-gpios = <&v2m_sysreg 1 0>;
+				max-frequency = <12000000>;
+				vmmc-supply = <&v2m_fixed_3v3>;
+				clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+				clock-names = "mclk", "apb_pclk";
+			};
+
+			kmi@060000 {
+				compatible = "arm,pl050", "arm,primecell";
+				reg = <0x060000 0x1000>;
+				interrupts = <12>;
+				clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+				clock-names = "KMIREFCLK", "apb_pclk";
+			};
+
+			kmi@070000 {
+				compatible = "arm,pl050", "arm,primecell";
+				reg = <0x070000 0x1000>;
+				interrupts = <13>;
+				clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+				clock-names = "KMIREFCLK", "apb_pclk";
+			};
+
+			v2m_serial0: uart@090000 {
+				compatible = "arm,pl011", "arm,primecell";
+				reg = <0x090000 0x1000>;
+				interrupts = <5>;
+				clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+				clock-names = "uartclk", "apb_pclk";
+			};
+
+			v2m_serial1: uart@0a0000 {
+				compatible = "arm,pl011", "arm,primecell";
+				reg = <0x0a0000 0x1000>;
+				interrupts = <6>;
+				clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+				clock-names = "uartclk", "apb_pclk";
+			};
+
+			v2m_serial2: uart@0b0000 {
+				compatible = "arm,pl011", "arm,primecell";
+				reg = <0x0b0000 0x1000>;
+				interrupts = <7>;
+				clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+				clock-names = "uartclk", "apb_pclk";
+			};
+
+			v2m_serial3: uart@0c0000 {
+				compatible = "arm,pl011", "arm,primecell";
+				reg = <0x0c0000 0x1000>;
+				interrupts = <8>;
+				clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+				clock-names = "uartclk", "apb_pclk";
+			};
+
+			wdt@0f0000 {
+				compatible = "arm,sp805", "arm,primecell";
+				reg = <0x0f0000 0x1000>;
+				interrupts = <0>;
+				clocks = <&v2m_refclk32khz>, <&v2m_clk24mhz>;
+				clock-names = "wdogclk", "apb_pclk";
+			};
+
+			v2m_timer01: timer@110000 {
+				compatible = "arm,sp804", "arm,primecell";
+				reg = <0x110000 0x1000>;
+				interrupts = <2>;
+				clocks = <&v2m_sysctl 0>, <&v2m_sysctl 1>, <&v2m_clk24mhz>;
+				clock-names = "timclken1", "timclken2", "apb_pclk";
+			};
+
+			v2m_timer23: timer@120000 {
+				compatible = "arm,sp804", "arm,primecell";
+				reg = <0x120000 0x1000>;
+				interrupts = <3>;
+				clocks = <&v2m_sysctl 2>, <&v2m_sysctl 3>, <&v2m_clk24mhz>;
+				clock-names = "timclken1", "timclken2", "apb_pclk";
+			};
+
+			rtc@170000 {
+				compatible = "arm,pl031", "arm,primecell";
+				reg = <0x170000 0x1000>;
+				interrupts = <4>;
+				clocks = <&v2m_clk24mhz>;
+				clock-names = "apb_pclk";
+			};
+
+			clcd@1f0000 {
+				compatible = "arm,pl111", "arm,primecell";
+				reg = <0x1f0000 0x1000>;
+				interrupts = <14>;
+				clocks = <&v2m_oscclk1>, <&v2m_clk24mhz>;
+				clock-names = "clcdclk", "apb_pclk";
+			};
+		};
+
+		v2m_fixed_3v3: fixedregulator@0 {
+			compatible = "regulator-fixed";
+			regulator-name = "3V3";
+			regulator-min-microvolt = <3300000>;
+			regulator-max-microvolt = <3300000>;
+			regulator-always-on;
+		};
+
+		mcc {
+			compatible = "arm,vexpress,config-bus", "simple-bus";
+			arm,vexpress,config-bridge = <&v2m_sysreg>;
+
+			v2m_oscclk1: osc@1 {
+				/* CLCD clock */
+				compatible = "arm,vexpress-osc";
+				arm,vexpress-sysreg,func = <1 1>;
+				freq-range = <23750000 63500000>;
+				#clock-cells = <0>;
+				clock-output-names = "v2m:oscclk1";
+			};
+
+			reset@0 {
+				compatible = "arm,vexpress-reset";
+				arm,vexpress-sysreg,func = <5 0>;
+			};
+
+			muxfpga@0 {
+				compatible = "arm,vexpress-muxfpga";
+				arm,vexpress-sysreg,func = <7 0>;
+			};
+
+			shutdown@0 {
+				compatible = "arm,vexpress-shutdown";
+				arm,vexpress-sysreg,func = <8 0>;
+			};
+
+			reboot@0 {
+				compatible = "arm,vexpress-reboot";
+				arm,vexpress-sysreg,func = <9 0>;
+			};
+
+			dvimode@0 {
+				compatible = "arm,vexpress-dvimode";
+				arm,vexpress-sysreg,func = <11 0>;
+			};
+		};
+	};
diff --git a/arch/arm64/boot/dts/skeleton.dtsi b/arch/arm64/boot/dts/skeleton.dtsi
new file mode 100644
index 00000000000..38ead821bb4
--- /dev/null
+++ b/arch/arm64/boot/dts/skeleton.dtsi
@@ -0,0 +1,13 @@
+/*
+ * Skeleton device tree; the bare minimum needed to boot; just include and
+ * add a compatible value.  The bootloader will typically populate the memory
+ * node.
+ */
+
+/ {
+	#address-cells = <2>;
+	#size-cells = <1>;
+	chosen { };
+	aliases { };
+	memory { device_type = "memory"; reg = <0 0 0>; };
+};
diff --git a/arch/arm64/configs/defconfig b/arch/arm64/configs/defconfig
index 09bef29f3a0..8d9696adb44 100644
--- a/arch/arm64/configs/defconfig
+++ b/arch/arm64/configs/defconfig
@@ -23,6 +23,7 @@ CONFIG_MODULES=y
 CONFIG_MODULE_UNLOAD=y
 # CONFIG_BLK_DEV_BSG is not set
 # CONFIG_IOSCHED_DEADLINE is not set
+CONFIG_ARCH_VEXPRESS=y
 CONFIG_SMP=y
 CONFIG_PREEMPT_VOLUNTARY=y
 CONFIG_CMDLINE="console=ttyAMA0"
@@ -47,11 +48,14 @@ CONFIG_BLK_DEV_SD=y
 # CONFIG_SCSI_LOWLEVEL is not set
 CONFIG_NETDEVICES=y
 CONFIG_MII=y
+CONFIG_SMC91X=y
 # CONFIG_WLAN is not set
 CONFIG_INPUT_EVDEV=y
 # CONFIG_SERIO_I8042 is not set
 # CONFIG_SERIO_SERPORT is not set
 CONFIG_LEGACY_PTY_COUNT=16
+CONFIG_SERIAL_AMBA_PL011=y
+CONFIG_SERIAL_AMBA_PL011_CONSOLE=y
 # CONFIG_HW_RANDOM is not set
 # CONFIG_HWMON is not set
 CONFIG_FB=y
diff --git a/arch/arm64/include/asm/Kbuild b/arch/arm64/include/asm/Kbuild
index e5fe4f99fe1..79a642d199f 100644
--- a/arch/arm64/include/asm/Kbuild
+++ b/arch/arm64/include/asm/Kbuild
@@ -39,7 +39,6 @@ generic-y += shmbuf.h
 generic-y += sizes.h
 generic-y += socket.h
 generic-y += sockios.h
-generic-y += string.h
 generic-y += switch_to.h
 generic-y += swab.h
 generic-y += termbits.h
@@ -49,4 +48,5 @@ generic-y += trace_clock.h
 generic-y += types.h
 generic-y += unaligned.h
 generic-y += user.h
+generic-y += vga.h
 generic-y += xor.h
diff --git a/arch/arm64/include/asm/bitops.h b/arch/arm64/include/asm/bitops.h
index 5e693073b03..aa5b59d6ba4 100644
--- a/arch/arm64/include/asm/bitops.h
+++ b/arch/arm64/include/asm/bitops.h
@@ -32,6 +32,16 @@
 #error only <linux/bitops.h> can be included directly
 #endif
 
+/*
+ * Little endian assembly atomic bitops.
+ */
+extern void set_bit(int nr, volatile unsigned long *p);
+extern void clear_bit(int nr, volatile unsigned long *p);
+extern void change_bit(int nr, volatile unsigned long *p);
+extern int test_and_set_bit(int nr, volatile unsigned long *p);
+extern int test_and_clear_bit(int nr, volatile unsigned long *p);
+extern int test_and_change_bit(int nr, volatile unsigned long *p);
+
 #include <asm-generic/bitops/builtin-__ffs.h>
 #include <asm-generic/bitops/builtin-ffs.h>
 #include <asm-generic/bitops/builtin-__fls.h>
@@ -45,9 +55,13 @@
 #include <asm-generic/bitops/hweight.h>
 #include <asm-generic/bitops/lock.h>
 
-#include <asm-generic/bitops/atomic.h>
 #include <asm-generic/bitops/non-atomic.h>
 #include <asm-generic/bitops/le.h>
-#include <asm-generic/bitops/ext2-atomic.h>
+
+/*
+ * Ext2 is defined to use little-endian byte ordering.
+ */
+#define ext2_set_bit_atomic(lock, nr, p)	test_and_set_bit_le(nr, p)
+#define ext2_clear_bit_atomic(lock, nr, p)	test_and_clear_bit_le(nr, p)
 
 #endif /* __ASM_BITOPS_H */
diff --git a/arch/arm64/include/asm/cmpxchg.h b/arch/arm64/include/asm/cmpxchg.h
index 968b5cbfc26..8a8ce0e73a3 100644
--- a/arch/arm64/include/asm/cmpxchg.h
+++ b/arch/arm64/include/asm/cmpxchg.h
@@ -170,4 +170,7 @@ static inline unsigned long __cmpxchg_mb(volatile void *ptr, unsigned long old,
 				       (unsigned long)(n),		\
 				       sizeof(*(ptr))))
 
+#define cmpxchg64(ptr,o,n)		cmpxchg((ptr),(o),(n))
+#define cmpxchg64_local(ptr,o,n)	cmpxchg_local((ptr),(o),(n))
+
 #endif	/* __ASM_CMPXCHG_H */
diff --git a/arch/arm64/include/asm/compat.h b/arch/arm64/include/asm/compat.h
index 618b450e5a1..899af807ef0 100644
--- a/arch/arm64/include/asm/compat.h
+++ b/arch/arm64/include/asm/compat.h
@@ -35,14 +35,16 @@ typedef s32		compat_clock_t;
 typedef s32		compat_pid_t;
 typedef u32		__compat_uid_t;
 typedef u32		__compat_gid_t;
+typedef u16		__compat_uid16_t;
+typedef u16		__compat_gid16_t;
 typedef u32		__compat_uid32_t;
 typedef u32		__compat_gid32_t;
-typedef u32		compat_mode_t;
+typedef u16		compat_mode_t;
 typedef u32		compat_ino_t;
 typedef u32		compat_dev_t;
 typedef s32		compat_off_t;
 typedef s64		compat_loff_t;
-typedef s16		compat_nlink_t;
+typedef s32		compat_nlink_t;
 typedef u16		compat_ipc_pid_t;
 typedef s32		compat_daddr_t;
 typedef u32		compat_caddr_t;
@@ -50,9 +52,11 @@ typedef __kernel_fsid_t	compat_fsid_t;
 typedef s32		compat_key_t;
 typedef s32		compat_timer_t;
 
+typedef s16		compat_short_t;
 typedef s32		compat_int_t;
 typedef s32		compat_long_t;
 typedef s64		compat_s64;
+typedef u16		compat_ushort_t;
 typedef u32		compat_uint_t;
 typedef u32		compat_ulong_t;
 typedef u64		compat_u64;
@@ -72,20 +76,20 @@ struct compat_stat {
 	compat_dev_t	st_dev;
 	compat_ino_t	st_ino;
 	compat_mode_t	st_mode;
-	compat_nlink_t	st_nlink;
-	__compat_uid32_t	st_uid;
-	__compat_gid32_t	st_gid;
+	compat_ushort_t	st_nlink;
+	__compat_uid16_t	st_uid;
+	__compat_gid16_t	st_gid;
 	compat_dev_t	st_rdev;
 	compat_off_t	st_size;
 	compat_off_t	st_blksize;
 	compat_off_t	st_blocks;
 	compat_time_t	st_atime;
-	u32		st_atime_nsec;
+	compat_ulong_t	st_atime_nsec;
 	compat_time_t	st_mtime;
-	u32		st_mtime_nsec;
+	compat_ulong_t	st_mtime_nsec;
 	compat_time_t	st_ctime;
-	u32		st_ctime_nsec;
-	u32		__unused4[2];
+	compat_ulong_t	st_ctime_nsec;
+	compat_ulong_t	__unused4[2];
 };
 
 struct compat_flock {
diff --git a/arch/arm64/include/asm/cputype.h b/arch/arm64/include/asm/cputype.h
index ef54125e6c1..cf2749488cd 100644
--- a/arch/arm64/include/asm/cputype.h
+++ b/arch/arm64/include/asm/cputype.h
@@ -17,6 +17,7 @@
 #define __ASM_CPUTYPE_H
 
 #define ID_MIDR_EL1		"midr_el1"
+#define ID_MPIDR_EL1		"mpidr_el1"
 #define ID_CTR_EL0		"ctr_el0"
 
 #define ID_AA64PFR0_EL1		"id_aa64pfr0_el1"
@@ -25,12 +26,24 @@
 #define ID_AA64ISAR0_EL1	"id_aa64isar0_el1"
 #define ID_AA64MMFR0_EL1	"id_aa64mmfr0_el1"
 
+#define INVALID_HWID		ULONG_MAX
+
+#define MPIDR_HWID_BITMASK	0xff00ffffff
+
 #define read_cpuid(reg) ({						\
 	u64 __val;							\
 	asm("mrs	%0, " reg : "=r" (__val));			\
 	__val;								\
 })
 
+#define ARM_CPU_IMP_ARM		0x41
+
+#define ARM_CPU_PART_AEM_V8	0xD0F0
+#define ARM_CPU_PART_FOUNDATION	0xD000
+#define ARM_CPU_PART_CORTEX_A57	0xD070
+
+#ifndef __ASSEMBLY__
+
 /*
  * The CPU ID never changes at run time, so we might as well tell the
  * compiler that it's constant.  Use this function to read the CPU ID
@@ -41,9 +54,26 @@ static inline u32 __attribute_const__ read_cpuid_id(void)
 	return read_cpuid(ID_MIDR_EL1);
 }
 
+static inline u64 __attribute_const__ read_cpuid_mpidr(void)
+{
+	return read_cpuid(ID_MPIDR_EL1);
+}
+
+static inline unsigned int __attribute_const__ read_cpuid_implementor(void)
+{
+	return (read_cpuid_id() & 0xFF000000) >> 24;
+}
+
+static inline unsigned int __attribute_const__ read_cpuid_part_number(void)
+{
+	return (read_cpuid_id() & 0xFFF0);
+}
+
 static inline u32 __attribute_const__ read_cpuid_cachetype(void)
 {
 	return read_cpuid(ID_CTR_EL0);
 }
 
+#endif /* __ASSEMBLY__ */
+
 #endif
diff --git a/arch/arm64/include/asm/esr.h b/arch/arm64/include/asm/esr.h
new file mode 100644
index 00000000000..78834123a32
--- /dev/null
+++ b/arch/arm64/include/asm/esr.h
@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) 2013 - ARM Ltd
+ * Author: Marc Zyngier <marc.zyngier@arm.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, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __ASM_ESR_H
+#define __ASM_ESR_H
+
+#define ESR_EL1_EC_SHIFT	(26)
+#define ESR_EL1_IL		(1U << 25)
+
+#define ESR_EL1_EC_UNKNOWN	(0x00)
+#define ESR_EL1_EC_WFI		(0x01)
+#define ESR_EL1_EC_CP15_32	(0x03)
+#define ESR_EL1_EC_CP15_64	(0x04)
+#define ESR_EL1_EC_CP14_MR	(0x05)
+#define ESR_EL1_EC_CP14_LS	(0x06)
+#define ESR_EL1_EC_FP_ASIMD	(0x07)
+#define ESR_EL1_EC_CP10_ID	(0x08)
+#define ESR_EL1_EC_CP14_64	(0x0C)
+#define ESR_EL1_EC_ILL_ISS	(0x0E)
+#define ESR_EL1_EC_SVC32	(0x11)
+#define ESR_EL1_EC_SVC64	(0x15)
+#define ESR_EL1_EC_SYS64	(0x18)
+#define ESR_EL1_EC_IABT_EL0	(0x20)
+#define ESR_EL1_EC_IABT_EL1	(0x21)
+#define ESR_EL1_EC_PC_ALIGN	(0x22)
+#define ESR_EL1_EC_DABT_EL0	(0x24)
+#define ESR_EL1_EC_DABT_EL1	(0x25)
+#define ESR_EL1_EC_SP_ALIGN	(0x26)
+#define ESR_EL1_EC_FP_EXC32	(0x28)
+#define ESR_EL1_EC_FP_EXC64	(0x2C)
+#define ESR_EL1_EC_SERRROR	(0x2F)
+#define ESR_EL1_EC_BREAKPT_EL0	(0x30)
+#define ESR_EL1_EC_BREAKPT_EL1	(0x31)
+#define ESR_EL1_EC_SOFTSTP_EL0	(0x32)
+#define ESR_EL1_EC_SOFTSTP_EL1	(0x33)
+#define ESR_EL1_EC_WATCHPT_EL0	(0x34)
+#define ESR_EL1_EC_WATCHPT_EL1	(0x35)
+#define ESR_EL1_EC_BKPT32	(0x38)
+#define ESR_EL1_EC_BRK64	(0x3C)
+
+#endif /* __ASM_ESR_H */
diff --git a/arch/arm64/include/asm/exception.h b/arch/arm64/include/asm/exception.h
index ac63519b7b9..0303705fcad 100644
--- a/arch/arm64/include/asm/exception.h
+++ b/arch/arm64/include/asm/exception.h
@@ -19,5 +19,6 @@
 #define __ASM_EXCEPTION_H
 
 #define __exception	__attribute__((section(".exception.text")))
+#define __exception_irq_entry	__exception
 
 #endif	/* __ASM_EXCEPTION_H */
diff --git a/arch/arm64/include/asm/hardirq.h b/arch/arm64/include/asm/hardirq.h
index 507546353d6..990c051e782 100644
--- a/arch/arm64/include/asm/hardirq.h
+++ b/arch/arm64/include/asm/hardirq.h
@@ -49,4 +49,9 @@ static inline void ack_bad_irq(unsigned int irq)
 
 extern void handle_IRQ(unsigned int, struct pt_regs *);
 
+/*
+ * No arch-specific IRQ flags.
+ */
+#define set_irq_flags(irq, flags)
+
 #endif /* __ASM_HARDIRQ_H */
diff --git a/arch/arm64/include/asm/io.h b/arch/arm64/include/asm/io.h
index 57f12c991de..2e12258aa7e 100644
--- a/arch/arm64/include/asm/io.h
+++ b/arch/arm64/include/asm/io.h
@@ -92,10 +92,12 @@ static inline u64 __raw_readq(const volatile void __iomem *addr)
 #define readb_relaxed(c)	({ u8  __v = __raw_readb(c); __v; })
 #define readw_relaxed(c)	({ u16 __v = le16_to_cpu((__force __le16)__raw_readw(c)); __v; })
 #define readl_relaxed(c)	({ u32 __v = le32_to_cpu((__force __le32)__raw_readl(c)); __v; })
+#define readq_relaxed(c)	({ u64 __v = le64_to_cpu((__force __le64)__raw_readq(c)); __v; })
 
 #define writeb_relaxed(v,c)	((void)__raw_writeb((v),(c)))
 #define writew_relaxed(v,c)	((void)__raw_writew((__force u16)cpu_to_le16(v),(c)))
 #define writel_relaxed(v,c)	((void)__raw_writel((__force u32)cpu_to_le32(v),(c)))
+#define writeq_relaxed(v,c)	((void)__raw_writeq((__force u64)cpu_to_le64(v),(c)))
 
 /*
  * I/O memory access primitives. Reads are ordered relative to any
@@ -105,10 +107,12 @@ static inline u64 __raw_readq(const volatile void __iomem *addr)
 #define readb(c)		({ u8  __v = readb_relaxed(c); __iormb(); __v; })
 #define readw(c)		({ u16 __v = readw_relaxed(c); __iormb(); __v; })
 #define readl(c)		({ u32 __v = readl_relaxed(c); __iormb(); __v; })
+#define readq(c)		({ u64 __v = readq_relaxed(c); __iormb(); __v; })
 
 #define writeb(v,c)		({ __iowmb(); writeb_relaxed((v),(c)); })
 #define writew(v,c)		({ __iowmb(); writew_relaxed((v),(c)); })
 #define writel(v,c)		({ __iowmb(); writel_relaxed((v),(c)); })
+#define writeq(v,c)		({ __iowmb(); writeq_relaxed((v),(c)); })
 
 /*
  *  I/O port access primitives.
diff --git a/arch/arm64/include/asm/irq.h b/arch/arm64/include/asm/irq.h
index a4e1cad3202..0332fc077f6 100644
--- a/arch/arm64/include/asm/irq.h
+++ b/arch/arm64/include/asm/irq.h
@@ -4,5 +4,6 @@
 #include <asm-generic/irq.h>
 
 extern void (*handle_arch_irq)(struct pt_regs *);
+extern void set_handle_irq(void (*handle_irq)(struct pt_regs *));
 
 #endif
diff --git a/arch/arm64/lib/bitops.c b/arch/arm64/include/asm/smp_plat.h
index aa4965e60ac..ed43a0d2b1b 100644
--- a/arch/arm64/lib/bitops.c
+++ b/arch/arm64/include/asm/smp_plat.h
@@ -1,7 +1,9 @@
 /*
- * Copyright (C) 2012 ARM Limited
+ * Definitions specific to SMP platforms.
  *
- * This program is free software; you can redistribute it and/or modify
+ * Copyright (C) 2013 ARM 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.
  *
@@ -14,12 +16,15 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include <linux/kernel.h>
-#include <linux/spinlock.h>
-#include <linux/atomic.h>
+#ifndef __ASM_SMP_PLAT_H
+#define __ASM_SMP_PLAT_H
+
+#include <asm/types.h>
+
+/*
+ * Logical CPU mapping.
+ */
+extern u64 __cpu_logical_map[NR_CPUS];
+#define cpu_logical_map(cpu)    __cpu_logical_map[cpu]
 
-#ifdef CONFIG_SMP
-arch_spinlock_t __atomic_hash[ATOMIC_HASH_SIZE] __lock_aligned = {
-       [0 ... (ATOMIC_HASH_SIZE-1)]  = __ARCH_SPIN_LOCK_UNLOCKED
-};
-#endif
+#endif /* __ASM_SMP_PLAT_H */
diff --git a/arch/arm64/include/asm/string.h b/arch/arm64/include/asm/string.h
new file mode 100644
index 00000000000..3ee8b303d9a
--- /dev/null
+++ b/arch/arm64/include/asm/string.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright (C) 2013 ARM 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#ifndef __ASM_STRING_H
+#define __ASM_STRING_H
+
+#define __HAVE_ARCH_STRRCHR
+extern char *strrchr(const char *, int c);
+
+#define __HAVE_ARCH_STRCHR
+extern char *strchr(const char *, int c);
+
+#define __HAVE_ARCH_MEMCPY
+extern void *memcpy(void *, const void *, __kernel_size_t);
+
+#define __HAVE_ARCH_MEMMOVE
+extern void *memmove(void *, const void *, __kernel_size_t);
+
+#define __HAVE_ARCH_MEMCHR
+extern void *memchr(const void *, int, __kernel_size_t);
+
+#define __HAVE_ARCH_MEMSET
+extern void *memset(void *, int, __kernel_size_t);
+
+#endif
diff --git a/arch/arm64/kernel/arm64ksyms.c b/arch/arm64/kernel/arm64ksyms.c
index aa3e948f788..7df1aad29b6 100644
--- a/arch/arm64/kernel/arm64ksyms.c
+++ b/arch/arm64/kernel/arm64ksyms.c
@@ -39,10 +39,21 @@ EXPORT_SYMBOL(__copy_from_user);
 EXPORT_SYMBOL(__copy_to_user);
 EXPORT_SYMBOL(__clear_user);
 
-	/* bitops */
-#ifdef CONFIG_SMP
-EXPORT_SYMBOL(__atomic_hash);
-#endif
-
 	/* physical memory */
 EXPORT_SYMBOL(memstart_addr);
+
+	/* string / mem functions */
+EXPORT_SYMBOL(strchr);
+EXPORT_SYMBOL(strrchr);
+EXPORT_SYMBOL(memset);
+EXPORT_SYMBOL(memcpy);
+EXPORT_SYMBOL(memmove);
+EXPORT_SYMBOL(memchr);
+
+	/* atomic bitops */
+EXPORT_SYMBOL(set_bit);
+EXPORT_SYMBOL(test_and_set_bit);
+EXPORT_SYMBOL(clear_bit);
+EXPORT_SYMBOL(test_and_clear_bit);
+EXPORT_SYMBOL(change_bit);
+EXPORT_SYMBOL(test_and_change_bit);
diff --git a/arch/arm64/kernel/early_printk.c b/arch/arm64/kernel/early_printk.c
index 7e320a2edb9..ac974f48a7a 100644
--- a/arch/arm64/kernel/early_printk.c
+++ b/arch/arm64/kernel/early_printk.c
@@ -24,6 +24,7 @@
 #include <linux/io.h>
 
 #include <linux/amba/serial.h>
+#include <linux/serial_reg.h>
 
 static void __iomem *early_base;
 static void (*printch)(char ch);
@@ -40,6 +41,37 @@ static void pl011_printch(char ch)
 		;
 }
 
+/*
+ * Semihosting-based debug console
+ */
+static void smh_printch(char ch)
+{
+	asm volatile("mov  x1, %0\n"
+		     "mov  x0, #3\n"
+		     "hlt  0xf000\n"
+		     : : "r" (&ch) : "x0", "x1", "memory");
+}
+
+/*
+ * 8250/16550 (8-bit aligned registers) single character TX.
+ */
+static void uart8250_8bit_printch(char ch)
+{
+	while (!(readb_relaxed(early_base + UART_LSR) & UART_LSR_THRE))
+		;
+	writeb_relaxed(ch, early_base + UART_TX);
+}
+
+/*
+ * 8250/16550 (32-bit aligned registers) single character TX.
+ */
+static void uart8250_32bit_printch(char ch)
+{
+	while (!(readl_relaxed(early_base + (UART_LSR << 2)) & UART_LSR_THRE))
+		;
+	writel_relaxed(ch, early_base + (UART_TX << 2));
+}
+
 struct earlycon_match {
 	const char *name;
 	void (*printch)(char ch);
@@ -47,6 +79,9 @@ struct earlycon_match {
 
 static const struct earlycon_match earlycon_match[] __initconst = {
 	{ .name = "pl011", .printch = pl011_printch, },
+	{ .name = "smh", .printch = smh_printch, },
+	{ .name = "uart8250-8bit", .printch = uart8250_8bit_printch, },
+	{ .name = "uart8250-32bit", .printch = uart8250_32bit_printch, },
 	{}
 };
 
diff --git a/arch/arm64/kernel/entry.S b/arch/arm64/kernel/entry.S
index 514d6098dbe..c7e047049f2 100644
--- a/arch/arm64/kernel/entry.S
+++ b/arch/arm64/kernel/entry.S
@@ -24,6 +24,7 @@
 #include <asm/assembler.h>
 #include <asm/asm-offsets.h>
 #include <asm/errno.h>
+#include <asm/esr.h>
 #include <asm/thread_info.h>
 #include <asm/unistd.h>
 #include <asm/unistd32.h>
@@ -239,18 +240,18 @@ ENDPROC(el1_error_invalid)
 el1_sync:
 	kernel_entry 1
 	mrs	x1, esr_el1			// read the syndrome register
-	lsr	x24, x1, #26			// exception class
-	cmp	x24, #0x25			// data abort in EL1
+	lsr	x24, x1, #ESR_EL1_EC_SHIFT	// exception class
+	cmp	x24, #ESR_EL1_EC_DABT_EL1	// data abort in EL1
 	b.eq	el1_da
-	cmp	x24, #0x18			// configurable trap
+	cmp	x24, #ESR_EL1_EC_SYS64		// configurable trap
 	b.eq	el1_undef
-	cmp	x24, #0x26			// stack alignment exception
+	cmp	x24, #ESR_EL1_EC_SP_ALIGN	// stack alignment exception
 	b.eq	el1_sp_pc
-	cmp	x24, #0x22			// pc alignment exception
+	cmp	x24, #ESR_EL1_EC_PC_ALIGN	// pc alignment exception
 	b.eq	el1_sp_pc
-	cmp	x24, #0x00			// unknown exception in EL1
+	cmp	x24, #ESR_EL1_EC_UNKNOWN	// unknown exception in EL1
 	b.eq	el1_undef
-	cmp	x24, #0x30			// debug exception in EL1
+	cmp	x24, #ESR_EL1_EC_BREAKPT_EL1	// debug exception in EL1
 	b.ge	el1_dbg
 	b	el1_inv
 el1_da:
@@ -346,27 +347,27 @@ el1_preempt:
 el0_sync:
 	kernel_entry 0
 	mrs	x25, esr_el1			// read the syndrome register
-	lsr	x24, x25, #26			// exception class
-	cmp	x24, #0x15			// SVC in 64-bit state
+	lsr	x24, x25, #ESR_EL1_EC_SHIFT	// exception class
+	cmp	x24, #ESR_EL1_EC_SVC64		// SVC in 64-bit state
 	b.eq	el0_svc
 	adr	lr, ret_from_exception
-	cmp	x24, #0x24			// data abort in EL0
+	cmp	x24, #ESR_EL1_EC_DABT_EL0	// data abort in EL0
 	b.eq	el0_da
-	cmp	x24, #0x20			// instruction abort in EL0
+	cmp	x24, #ESR_EL1_EC_IABT_EL0	// instruction abort in EL0
 	b.eq	el0_ia
-	cmp	x24, #0x07			// FP/ASIMD access
+	cmp	x24, #ESR_EL1_EC_FP_ASIMD	// FP/ASIMD access
 	b.eq	el0_fpsimd_acc
-	cmp	x24, #0x2c			// FP/ASIMD exception
+	cmp	x24, #ESR_EL1_EC_FP_EXC64	// FP/ASIMD exception
 	b.eq	el0_fpsimd_exc
-	cmp	x24, #0x18			// configurable trap
+	cmp	x24, #ESR_EL1_EC_SYS64		// configurable trap
 	b.eq	el0_undef
-	cmp	x24, #0x26			// stack alignment exception
+	cmp	x24, #ESR_EL1_EC_SP_ALIGN	// stack alignment exception
 	b.eq	el0_sp_pc
-	cmp	x24, #0x22			// pc alignment exception
+	cmp	x24, #ESR_EL1_EC_PC_ALIGN	// pc alignment exception
 	b.eq	el0_sp_pc
-	cmp	x24, #0x00			// unknown exception in EL0
+	cmp	x24, #ESR_EL1_EC_UNKNOWN	// unknown exception in EL0
 	b.eq	el0_undef
-	cmp	x24, #0x30			// debug exception in EL0
+	cmp	x24, #ESR_EL1_EC_BREAKPT_EL0	// debug exception in EL0
 	b.ge	el0_dbg
 	b	el0_inv
 
@@ -375,21 +376,21 @@ el0_sync:
 el0_sync_compat:
 	kernel_entry 0, 32
 	mrs	x25, esr_el1			// read the syndrome register
-	lsr	x24, x25, #26			// exception class
-	cmp	x24, #0x11			// SVC in 32-bit state
+	lsr	x24, x25, #ESR_EL1_EC_SHIFT	// exception class
+	cmp	x24, #ESR_EL1_EC_SVC32		// SVC in 32-bit state
 	b.eq	el0_svc_compat
 	adr	lr, ret_from_exception
-	cmp	x24, #0x24			// data abort in EL0
+	cmp	x24, #ESR_EL1_EC_DABT_EL0	// data abort in EL0
 	b.eq	el0_da
-	cmp	x24, #0x20			// instruction abort in EL0
+	cmp	x24, #ESR_EL1_EC_IABT_EL0	// instruction abort in EL0
 	b.eq	el0_ia
-	cmp	x24, #0x07			// FP/ASIMD access
+	cmp	x24, #ESR_EL1_EC_FP_ASIMD	// FP/ASIMD access
 	b.eq	el0_fpsimd_acc
-	cmp	x24, #0x28			// FP/ASIMD exception
+	cmp	x24, #ESR_EL1_EC_FP_EXC32	// FP/ASIMD exception
 	b.eq	el0_fpsimd_exc
-	cmp	x24, #0x00			// unknown exception in EL0
+	cmp	x24, #ESR_EL1_EC_UNKNOWN	// unknown exception in EL0
 	b.eq	el0_undef
-	cmp	x24, #0x30			// debug exception in EL0
+	cmp	x24, #ESR_EL1_EC_BREAKPT_EL0	// debug exception in EL0
 	b.ge	el0_dbg
 	b	el0_inv
 el0_svc_compat:
diff --git a/arch/arm64/kernel/head.S b/arch/arm64/kernel/head.S
index 0a0a4975682..53dcae49e72 100644
--- a/arch/arm64/kernel/head.S
+++ b/arch/arm64/kernel/head.S
@@ -26,6 +26,7 @@
 #include <asm/assembler.h>
 #include <asm/ptrace.h>
 #include <asm/asm-offsets.h>
+#include <asm/cputype.h>
 #include <asm/memory.h>
 #include <asm/thread_info.h>
 #include <asm/pgtable-hwdef.h>
@@ -229,7 +230,8 @@ ENTRY(secondary_holding_pen)
 	bl	__calc_phys_offset		// x24=phys offset
 	bl	el2_setup			// Drop to EL1
 	mrs	x0, mpidr_el1
-	and	x0, x0, #15			// CPU number
+	ldr     x1, =MPIDR_HWID_BITMASK
+	and	x0, x0, x1
 	adr	x1, 1b
 	ldp	x2, x3, [x1]
 	sub	x1, x1, x2
diff --git a/arch/arm64/kernel/irq.c b/arch/arm64/kernel/irq.c
index 0373c6609ea..ecb3354292e 100644
--- a/arch/arm64/kernel/irq.c
+++ b/arch/arm64/kernel/irq.c
@@ -25,7 +25,7 @@
 #include <linux/irq.h>
 #include <linux/smp.h>
 #include <linux/init.h>
-#include <linux/of_irq.h>
+#include <linux/irqchip.h>
 #include <linux/seq_file.h>
 #include <linux/ratelimit.h>
 
@@ -67,18 +67,17 @@ void handle_IRQ(unsigned int irq, struct pt_regs *regs)
 	set_irq_regs(old_regs);
 }
 
-/*
- * Interrupt controllers supported by the kernel.
- */
-static const struct of_device_id intctrl_of_match[] __initconst = {
-	/* IRQ controllers { .compatible, .data } info to go here */
-	{}
-};
+void __init set_handle_irq(void (*handle_irq)(struct pt_regs *))
+{
+	if (handle_arch_irq)
+		return;
+
+	handle_arch_irq = handle_irq;
+}
 
 void __init init_IRQ(void)
 {
-	of_irq_init(intctrl_of_match);
-
+	irqchip_init();
 	if (!handle_arch_irq)
 		panic("No interrupt controller found.");
 }
diff --git a/arch/arm64/kernel/process.c b/arch/arm64/kernel/process.c
index 0337cdb0667..6f3822f98dc 100644
--- a/arch/arm64/kernel/process.c
+++ b/arch/arm64/kernel/process.c
@@ -84,11 +84,15 @@ EXPORT_SYMBOL_GPL(pm_power_off);
 void (*pm_restart)(const char *cmd);
 EXPORT_SYMBOL_GPL(pm_restart);
 
+void arch_cpu_idle_prepare(void)
+{
+	local_fiq_enable();
+}
 
 /*
  * This is our default idle handler.
  */
-static void default_idle(void)
+void arch_cpu_idle(void)
 {
 	/*
 	 * This should do all the clock switching and wait for interrupt
@@ -98,43 +102,6 @@ static void default_idle(void)
 	local_irq_enable();
 }
 
-/*
- * The idle thread.
- * We always respect 'hlt_counter' to prevent low power idle.
- */
-void cpu_idle(void)
-{
-	local_fiq_enable();
-
-	/* endless idle loop with no priority at all */
-	while (1) {
-		tick_nohz_idle_enter();
-		rcu_idle_enter();
-		while (!need_resched()) {
-			/*
-			 * We need to disable interrupts here to ensure
-			 * we don't miss a wakeup call.
-			 */
-			local_irq_disable();
-			if (!need_resched()) {
-				stop_critical_timings();
-				default_idle();
-				start_critical_timings();
-				/*
-				 * default_idle functions should always return
-				 * with IRQs enabled.
-				 */
-				WARN_ON(irqs_disabled());
-			} else {
-				local_irq_enable();
-			}
-		}
-		rcu_idle_exit();
-		tick_nohz_idle_exit();
-		schedule_preempt_disabled();
-	}
-}
-
 void machine_shutdown(void)
 {
 #ifdef CONFIG_SMP
@@ -311,11 +278,17 @@ struct task_struct *__switch_to(struct task_struct *prev,
 	fpsimd_thread_switch(next);
 	tls_thread_switch(next);
 	hw_breakpoint_thread_switch(next);
+	contextidr_thread_switch(next);
+
+	/*
+	 * Complete any pending TLB or cache maintenance on this CPU in case
+	 * the thread migrates to a different CPU.
+	 */
+	dsb();
 
 	/* the actual thread switch */
 	last = cpu_switch_to(prev, next);
 
-	contextidr_thread_switch(next);
 	return last;
 }
 
diff --git a/arch/arm64/kernel/setup.c b/arch/arm64/kernel/setup.c
index 113db863f83..6a9a5329259 100644
--- a/arch/arm64/kernel/setup.c
+++ b/arch/arm64/kernel/setup.c
@@ -32,6 +32,7 @@
 #include <linux/kexec.h>
 #include <linux/crash_dump.h>
 #include <linux/root_dev.h>
+#include <linux/clk-provider.h>
 #include <linux/cpu.h>
 #include <linux/interrupt.h>
 #include <linux/smp.h>
@@ -46,6 +47,7 @@
 #include <asm/cputable.h>
 #include <asm/sections.h>
 #include <asm/setup.h>
+#include <asm/smp_plat.h>
 #include <asm/cacheflush.h>
 #include <asm/tlbflush.h>
 #include <asm/traps.h>
@@ -240,6 +242,8 @@ static void __init request_standard_resources(void)
 	}
 }
 
+u64 __cpu_logical_map[NR_CPUS] = { [0 ... NR_CPUS-1] = INVALID_HWID };
+
 void __init setup_arch(char **cmdline_p)
 {
 	setup_processor();
@@ -264,6 +268,7 @@ void __init setup_arch(char **cmdline_p)
 
 	psci_init();
 
+	cpu_logical_map(0) = read_cpuid_mpidr() & MPIDR_HWID_BITMASK;
 #ifdef CONFIG_SMP
 	smp_init_cpus();
 #endif
@@ -277,6 +282,13 @@ void __init setup_arch(char **cmdline_p)
 #endif
 }
 
+static int __init arm64_of_clk_init(void)
+{
+	of_clk_init(NULL);
+	return 0;
+}
+arch_initcall(arm64_of_clk_init);
+
 static DEFINE_PER_CPU(struct cpu, cpu_data);
 
 static int __init topology_init(void)
diff --git a/arch/arm64/kernel/smp.c b/arch/arm64/kernel/smp.c
index bdd34597254..5d54e3717bf 100644
--- a/arch/arm64/kernel/smp.c
+++ b/arch/arm64/kernel/smp.c
@@ -43,6 +43,7 @@
 #include <asm/pgtable.h>
 #include <asm/pgalloc.h>
 #include <asm/processor.h>
+#include <asm/smp_plat.h>
 #include <asm/sections.h>
 #include <asm/tlbflush.h>
 #include <asm/ptrace.h>
@@ -53,7 +54,7 @@
  * where to place its SVC stack
  */
 struct secondary_data secondary_data;
-volatile unsigned long secondary_holding_pen_release = -1;
+volatile unsigned long secondary_holding_pen_release = INVALID_HWID;
 
 enum ipi_msg_type {
 	IPI_RESCHEDULE,
@@ -70,7 +71,7 @@ static DEFINE_RAW_SPINLOCK(boot_lock);
  * in coherency or not.  This is necessary for the hotplug code to work
  * reliably.
  */
-static void __cpuinit write_pen_release(int val)
+static void __cpuinit write_pen_release(u64 val)
 {
 	void *start = (void *)&secondary_holding_pen_release;
 	unsigned long size = sizeof(secondary_holding_pen_release);
@@ -96,7 +97,7 @@ static int __cpuinit boot_secondary(unsigned int cpu, struct task_struct *idle)
 	/*
 	 * Update the pen release flag.
 	 */
-	write_pen_release(cpu);
+	write_pen_release(cpu_logical_map(cpu));
 
 	/*
 	 * Send an event, causing the secondaries to read pen_release.
@@ -105,7 +106,7 @@ static int __cpuinit boot_secondary(unsigned int cpu, struct task_struct *idle)
 
 	timeout = jiffies + (1 * HZ);
 	while (time_before(jiffies, timeout)) {
-		if (secondary_holding_pen_release == -1UL)
+		if (secondary_holding_pen_release == INVALID_HWID)
 			break;
 		udelay(10);
 	}
@@ -116,7 +117,7 @@ static int __cpuinit boot_secondary(unsigned int cpu, struct task_struct *idle)
 	 */
 	raw_spin_unlock(&boot_lock);
 
-	return secondary_holding_pen_release != -1 ? -ENOSYS : 0;
+	return secondary_holding_pen_release != INVALID_HWID ? -ENOSYS : 0;
 }
 
 static DECLARE_COMPLETION(cpu_running);
@@ -190,7 +191,7 @@ asmlinkage void __cpuinit secondary_start_kernel(void)
 	 * Let the primary processor know we're out of the
 	 * pen, then head off into the C entry point
 	 */
-	write_pen_release(-1);
+	write_pen_release(INVALID_HWID);
 
 	/*
 	 * Synchronise with the boot thread.
@@ -216,7 +217,7 @@ asmlinkage void __cpuinit secondary_start_kernel(void)
 	/*
 	 * OK, it's off to the idle thread for us
 	 */
-	cpu_idle();
+	cpu_startup_entry(CPUHP_ONLINE);
 }
 
 void __init smp_cpus_done(unsigned int max_cpus)
@@ -244,11 +245,11 @@ static const struct smp_enable_ops *smp_enable_ops[NR_CPUS];
 
 static const struct smp_enable_ops * __init smp_get_enable_ops(const char *name)
 {
-	const struct smp_enable_ops *ops = enable_ops[0];
+	const struct smp_enable_ops **ops = enable_ops;
 
-	while (ops) {
-		if (!strcmp(name, ops->name))
-			return ops;
+	while (*ops) {
+		if (!strcmp(name, (*ops)->name))
+			return *ops;
 
 		ops++;
 	}
@@ -257,15 +258,80 @@ static const struct smp_enable_ops * __init smp_get_enable_ops(const char *name)
 }
 
 /*
- * Enumerate the possible CPU set from the device tree.
+ * Enumerate the possible CPU set from the device tree and build the
+ * cpu logical map array containing MPIDR values related to logical
+ * cpus. Assumes that cpu_logical_map(0) has already been initialized.
  */
 void __init smp_init_cpus(void)
 {
 	const char *enable_method;
 	struct device_node *dn = NULL;
-	int cpu = 0;
+	int i, cpu = 1;
+	bool bootcpu_valid = false;
 
 	while ((dn = of_find_node_by_type(dn, "cpu"))) {
+		const u32 *cell;
+		u64 hwid;
+
+		/*
+		 * A cpu node with missing "reg" property is
+		 * considered invalid to build a cpu_logical_map
+		 * entry.
+		 */
+		cell = of_get_property(dn, "reg", NULL);
+		if (!cell) {
+			pr_err("%s: missing reg property\n", dn->full_name);
+			goto next;
+		}
+		hwid = of_read_number(cell, of_n_addr_cells(dn));
+
+		/*
+		 * Non affinity bits must be set to 0 in the DT
+		 */
+		if (hwid & ~MPIDR_HWID_BITMASK) {
+			pr_err("%s: invalid reg property\n", dn->full_name);
+			goto next;
+		}
+
+		/*
+		 * Duplicate MPIDRs are a recipe for disaster. Scan
+		 * all initialized entries and check for
+		 * duplicates. If any is found just ignore the cpu.
+		 * cpu_logical_map was initialized to INVALID_HWID to
+		 * avoid matching valid MPIDR values.
+		 */
+		for (i = 1; (i < cpu) && (i < NR_CPUS); i++) {
+			if (cpu_logical_map(i) == hwid) {
+				pr_err("%s: duplicate cpu reg properties in the DT\n",
+					dn->full_name);
+				goto next;
+			}
+		}
+
+		/*
+		 * The numbering scheme requires that the boot CPU
+		 * must be assigned logical id 0. Record it so that
+		 * the logical map built from DT is validated and can
+		 * be used.
+		 */
+		if (hwid == cpu_logical_map(0)) {
+			if (bootcpu_valid) {
+				pr_err("%s: duplicate boot cpu reg property in DT\n",
+					dn->full_name);
+				goto next;
+			}
+
+			bootcpu_valid = true;
+
+			/*
+			 * cpu_logical_map has already been
+			 * initialized and the boot cpu doesn't need
+			 * the enable-method so continue without
+			 * incrementing cpu.
+			 */
+			continue;
+		}
+
 		if (cpu >= NR_CPUS)
 			goto next;
 
@@ -274,22 +340,24 @@ void __init smp_init_cpus(void)
 		 */
 		enable_method = of_get_property(dn, "enable-method", NULL);
 		if (!enable_method) {
-			pr_err("CPU %d: missing enable-method property\n", cpu);
+			pr_err("%s: missing enable-method property\n",
+				dn->full_name);
 			goto next;
 		}
 
 		smp_enable_ops[cpu] = smp_get_enable_ops(enable_method);
 
 		if (!smp_enable_ops[cpu]) {
-			pr_err("CPU %d: invalid enable-method property: %s\n",
-			       cpu, enable_method);
+			pr_err("%s: invalid enable-method property: %s\n",
+			       dn->full_name, enable_method);
 			goto next;
 		}
 
 		if (smp_enable_ops[cpu]->init_cpu(dn, cpu))
 			goto next;
 
-		set_cpu_possible(cpu, true);
+		pr_debug("cpu logical map 0x%llx\n", hwid);
+		cpu_logical_map(cpu) = hwid;
 next:
 		cpu++;
 	}
@@ -298,6 +366,19 @@ next:
 	if (cpu > NR_CPUS)
 		pr_warning("no. of cores (%d) greater than configured maximum of %d - clipping\n",
 			   cpu, NR_CPUS);
+
+	if (!bootcpu_valid) {
+		pr_err("DT missing boot CPU MPIDR, not enabling secondaries\n");
+		return;
+	}
+
+	/*
+	 * All the cpus that made it to the cpu_logical_map have been
+	 * validated so set them as possible cpus.
+	 */
+	for (i = 0; i < NR_CPUS; i++)
+		if (cpu_logical_map(i) != INVALID_HWID)
+			set_cpu_possible(i, true);
 }
 
 void __init smp_prepare_cpus(unsigned int max_cpus)
diff --git a/arch/arm64/kernel/smp_psci.c b/arch/arm64/kernel/smp_psci.c
index 112091684c2..0c533301be7 100644
--- a/arch/arm64/kernel/smp_psci.c
+++ b/arch/arm64/kernel/smp_psci.c
@@ -21,6 +21,7 @@
 #include <linux/smp.h>
 
 #include <asm/psci.h>
+#include <asm/smp_plat.h>
 
 static int __init smp_psci_init_cpu(struct device_node *dn, int cpu)
 {
@@ -36,7 +37,7 @@ static int __init smp_psci_prepare_cpu(int cpu)
 		return -ENODEV;
 	}
 
-	err = psci_ops.cpu_on(cpu, __pa(secondary_holding_pen));
+	err = psci_ops.cpu_on(cpu_logical_map(cpu), __pa(secondary_holding_pen));
 	if (err) {
 		pr_err("psci: failed to boot CPU%d (%d)\n", cpu, err);
 		return err;
@@ -47,6 +48,6 @@ static int __init smp_psci_prepare_cpu(int cpu)
 
 const struct smp_enable_ops smp_psci_ops __initconst = {
 	.name		= "psci",
-	.init_cpu 	= smp_psci_init_cpu,
+	.init_cpu	= smp_psci_init_cpu,
 	.prepare_cpu	= smp_psci_prepare_cpu,
 };
diff --git a/arch/arm64/lib/Makefile b/arch/arm64/lib/Makefile
index 2fb7f6092aa..59acc0ef046 100644
--- a/arch/arm64/lib/Makefile
+++ b/arch/arm64/lib/Makefile
@@ -1,4 +1,6 @@
 lib-y		:= bitops.o delay.o					\
 		   strncpy_from_user.o strnlen_user.o clear_user.o	\
 		   copy_from_user.o copy_to_user.o copy_in_user.o	\
-		   copy_page.o clear_page.o
+		   copy_page.o clear_page.o				\
+		   memchr.o memcpy.o memmove.o memset.o			\
+		   strchr.o strrchr.o
diff --git a/arch/arm64/lib/bitops.S b/arch/arm64/lib/bitops.S
new file mode 100644
index 00000000000..36216d30cb9
--- /dev/null
+++ b/arch/arm64/lib/bitops.S
@@ -0,0 +1,68 @@
+/*
+ * Based on arch/arm/lib/bitops.h
+ *
+ * Copyright (C) 2013 ARM 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/linkage.h>
+#include <asm/assembler.h>
+
+/*
+ * x0: bits 5:0  bit offset
+ *     bits 63:6 word offset
+ * x1: address
+ */
+	.macro	bitop, name, instr
+ENTRY(	\name	)
+	and	x3, x0, #63		// Get bit offset
+	eor	x0, x0, x3		// Clear low bits
+	mov	x2, #1
+	add	x1, x1, x0, lsr #3	// Get word offset
+	lsl	x3, x2, x3		// Create mask
+1:	ldxr	x2, [x1]
+	\instr	x2, x2, x3
+	stxr	w0, x2, [x1]
+	cbnz	w0, 1b
+	ret
+ENDPROC(\name	)
+	.endm
+
+	.macro	testop, name, instr
+ENTRY(	\name	)
+	and	x3, x0, #63		// Get bit offset
+	eor	x0, x0, x3		// Clear low bits
+	mov	x2, #1
+	add	x1, x1, x0, lsr #3	// Get word offset
+	lsl	x4, x2, x3		// Create mask
+1:	ldaxr	x2, [x1]
+	lsr	x0, x2, x3		// Save old value of bit
+	\instr	x2, x2, x4		// toggle bit
+	stlxr	w5, x2, [x1]
+	cbnz	w5, 1b
+	and	x0, x0, #1
+3:	ret
+ENDPROC(\name	)
+	.endm
+
+/*
+ * Atomic bit operations.
+ */
+	bitop	change_bit, eor
+	bitop	clear_bit, bic
+	bitop	set_bit, orr
+
+	testop	test_and_change_bit, eor
+	testop	test_and_clear_bit, bic
+	testop	test_and_set_bit, orr
diff --git a/arch/arm64/lib/memchr.S b/arch/arm64/lib/memchr.S
new file mode 100644
index 00000000000..8636b754916
--- /dev/null
+++ b/arch/arm64/lib/memchr.S
@@ -0,0 +1,44 @@
+/*
+ * Based on arch/arm/lib/memchr.S
+ *
+ * Copyright (C) 1995-2000 Russell King
+ * Copyright (C) 2013 ARM 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/linkage.h>
+#include <asm/assembler.h>
+
+/*
+ * Find a character in an area of memory.
+ *
+ * Parameters:
+ *	x0 - buf
+ *	x1 - c
+ *	x2 - n
+ * Returns:
+ *	x0 - address of first occurrence of 'c' or 0
+ */
+ENTRY(memchr)
+	and	w1, w1, #0xff
+1:	subs	x2, x2, #1
+	b.mi	2f
+	ldrb	w3, [x0], #1
+	cmp	w3, w1
+	b.ne	1b
+	sub	x0, x0, #1
+	ret
+2:	mov	x0, #0
+	ret
+ENDPROC(memchr)
diff --git a/arch/arm64/lib/memcpy.S b/arch/arm64/lib/memcpy.S
new file mode 100644
index 00000000000..27b5003609b
--- /dev/null
+++ b/arch/arm64/lib/memcpy.S
@@ -0,0 +1,53 @@
+/*
+ * Copyright (C) 2013 ARM 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/linkage.h>
+#include <asm/assembler.h>
+
+/*
+ * Copy a buffer from src to dest (alignment handled by the hardware)
+ *
+ * Parameters:
+ *	x0 - dest
+ *	x1 - src
+ *	x2 - n
+ * Returns:
+ *	x0 - dest
+ */
+ENTRY(memcpy)
+	mov	x4, x0
+	subs	x2, x2, #8
+	b.mi	2f
+1:	ldr	x3, [x1], #8
+	subs	x2, x2, #8
+	str	x3, [x4], #8
+	b.pl	1b
+2:	adds	x2, x2, #4
+	b.mi	3f
+	ldr	w3, [x1], #4
+	sub	x2, x2, #4
+	str	w3, [x4], #4
+3:	adds	x2, x2, #2
+	b.mi	4f
+	ldrh	w3, [x1], #2
+	sub	x2, x2, #2
+	strh	w3, [x4], #2
+4:	adds	x2, x2, #1
+	b.mi	5f
+	ldrb	w3, [x1]
+	strb	w3, [x4]
+5:	ret
+ENDPROC(memcpy)
diff --git a/arch/arm64/lib/memmove.S b/arch/arm64/lib/memmove.S
new file mode 100644
index 00000000000..b79fdfa42d3
--- /dev/null
+++ b/arch/arm64/lib/memmove.S
@@ -0,0 +1,57 @@
+/*
+ * Copyright (C) 2013 ARM 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/linkage.h>
+#include <asm/assembler.h>
+
+/*
+ * Move a buffer from src to test (alignment handled by the hardware).
+ * If dest <= src, call memcpy, otherwise copy in reverse order.
+ *
+ * Parameters:
+ *	x0 - dest
+ *	x1 - src
+ *	x2 - n
+ * Returns:
+ *	x0 - dest
+ */
+ENTRY(memmove)
+	cmp	x0, x1
+	b.ls	memcpy
+	add	x4, x0, x2
+	add	x1, x1, x2
+	subs	x2, x2, #8
+	b.mi	2f
+1:	ldr	x3, [x1, #-8]!
+	subs	x2, x2, #8
+	str	x3, [x4, #-8]!
+	b.pl	1b
+2:	adds	x2, x2, #4
+	b.mi	3f
+	ldr	w3, [x1, #-4]!
+	sub	x2, x2, #4
+	str	w3, [x4, #-4]!
+3:	adds	x2, x2, #2
+	b.mi	4f
+	ldrh	w3, [x1, #-2]!
+	sub	x2, x2, #2
+	strh	w3, [x4, #-2]!
+4:	adds	x2, x2, #1
+	b.mi	5f
+	ldrb	w3, [x1, #-1]
+	strb	w3, [x4, #-1]
+5:	ret
+ENDPROC(memmove)
diff --git a/arch/arm64/lib/memset.S b/arch/arm64/lib/memset.S
new file mode 100644
index 00000000000..87e4a68fbbb
--- /dev/null
+++ b/arch/arm64/lib/memset.S
@@ -0,0 +1,53 @@
+/*
+ * Copyright (C) 2013 ARM 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/linkage.h>
+#include <asm/assembler.h>
+
+/*
+ * Fill in the buffer with character c (alignment handled by the hardware)
+ *
+ * Parameters:
+ *	x0 - buf
+ *	x1 - c
+ *	x2 - n
+ * Returns:
+ *	x0 - buf
+ */
+ENTRY(memset)
+	mov	x4, x0
+	and	w1, w1, #0xff
+	orr	w1, w1, w1, lsl #8
+	orr	w1, w1, w1, lsl #16
+	orr	x1, x1, x1, lsl #32
+	subs	x2, x2, #8
+	b.mi	2f
+1:	str	x1, [x4], #8
+	subs	x2, x2, #8
+	b.pl	1b
+2:	adds	x2, x2, #4
+	b.mi	3f
+	sub	x2, x2, #4
+	str	w1, [x4], #4
+3:	adds	x2, x2, #2
+	b.mi	4f
+	sub	x2, x2, #2
+	strh	w1, [x4], #2
+4:	adds	x2, x2, #1
+	b.mi	5f
+	strb	w1, [x4]
+5:	ret
+ENDPROC(memset)
diff --git a/arch/arm64/lib/strchr.S b/arch/arm64/lib/strchr.S
new file mode 100644
index 00000000000..dae0cf5591f
--- /dev/null
+++ b/arch/arm64/lib/strchr.S
@@ -0,0 +1,42 @@
+/*
+ * Based on arch/arm/lib/strchr.S
+ *
+ * Copyright (C) 1995-2000 Russell King
+ * Copyright (C) 2013 ARM 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/linkage.h>
+#include <asm/assembler.h>
+
+/*
+ * Find the first occurrence of a character in a string.
+ *
+ * Parameters:
+ *	x0 - str
+ *	x1 - c
+ * Returns:
+ *	x0 - address of first occurrence of 'c' or 0
+ */
+ENTRY(strchr)
+	and	w1, w1, #0xff
+1:	ldrb	w2, [x0], #1
+	cmp	w2, w1
+	ccmp	w2, wzr, #4, ne
+	b.ne	1b
+	sub	x0, x0, #1
+	cmp	w2, w1
+	csel	x0, x0, xzr, eq
+	ret
+ENDPROC(strchr)
diff --git a/arch/arm64/lib/strrchr.S b/arch/arm64/lib/strrchr.S
new file mode 100644
index 00000000000..61eabd9a289
--- /dev/null
+++ b/arch/arm64/lib/strrchr.S
@@ -0,0 +1,43 @@
+/*
+ * Based on arch/arm/lib/strrchr.S
+ *
+ * Copyright (C) 1995-2000 Russell King
+ * Copyright (C) 2013 ARM 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/linkage.h>
+#include <asm/assembler.h>
+
+/*
+ * Find the last occurrence of a character in a string.
+ *
+ * Parameters:
+ *	x0 - str
+ *	x1 - c
+ * Returns:
+ *	x0 - address of last occurrence of 'c' or 0
+ */
+ENTRY(strrchr)
+	mov	x3, #0
+	and	w1, w1, #0xff
+1:	ldrb	w2, [x0], #1
+	cbz	w2, 2f
+	cmp	w2, w1
+	b.ne	1b
+	sub	x3, x0, #1
+	b	1b
+2:	mov	x0, x3
+	ret
+ENDPROC(strrchr)
diff --git a/arch/arm64/mm/fault.c b/arch/arm64/mm/fault.c
index afadae6682e..52638171d6f 100644
--- a/arch/arm64/mm/fault.c
+++ b/arch/arm64/mm/fault.c
@@ -57,16 +57,16 @@ void show_pte(struct mm_struct *mm, unsigned long addr)
 		pmd_t *pmd;
 		pte_t *pte;
 
-		if (pgd_none_or_clear_bad(pgd))
+		if (pgd_none(*pgd) || pgd_bad(*pgd))
 			break;
 
 		pud = pud_offset(pgd, addr);
-		if (pud_none_or_clear_bad(pud))
+		if (pud_none(*pud) || pud_bad(*pud))
 			break;
 
 		pmd = pmd_offset(pud, addr);
 		printk(", *pmd=%016llx", pmd_val(*pmd));
-		if (pmd_none_or_clear_bad(pmd))
+		if (pmd_none(*pmd) || pmd_bad(*pmd))
 			break;
 
 		pte = pte_offset_map(pmd, addr);
diff --git a/arch/arm64/mm/init.c b/arch/arm64/mm/init.c
index 800aac306a0..f497ca77925 100644
--- a/arch/arm64/mm/init.c
+++ b/arch/arm64/mm/init.c
@@ -197,24 +197,6 @@ void __init bootmem_init(void)
 	max_pfn = max_low_pfn = max;
 }
 
-static inline int free_area(unsigned long pfn, unsigned long end, char *s)
-{
-	unsigned int pages = 0, size = (end - pfn) << (PAGE_SHIFT - 10);
-
-	for (; pfn < end; pfn++) {
-		struct page *page = pfn_to_page(pfn);
-		ClearPageReserved(page);
-		init_page_count(page);
-		__free_page(page);
-		pages++;
-	}
-
-	if (size && s)
-		pr_info("Freeing %s memory: %dK\n", s, size);
-
-	return pages;
-}
-
 /*
  * Poison init memory with an undefined instruction (0x0).
  */
@@ -405,9 +387,7 @@ void __init mem_init(void)
 void free_initmem(void)
 {
 	poison_init_mem(__init_begin, __init_end - __init_begin);
-	totalram_pages += free_area(__phys_to_pfn(__pa(__init_begin)),
-				    __phys_to_pfn(__pa(__init_end)),
-				    "init");
+	free_initmem_default(0);
 }
 
 #ifdef CONFIG_BLK_DEV_INITRD
@@ -418,9 +398,7 @@ void free_initrd_mem(unsigned long start, unsigned long end)
 {
 	if (!keep_initrd) {
 		poison_init_mem((void *)start, PAGE_ALIGN(end) - start);
-		totalram_pages += free_area(__phys_to_pfn(__pa(start)),
-					    __phys_to_pfn(__pa(end)),
-					    "initrd");
+		free_reserved_area(start, end, 0, "initrd");
 	}
 }
 
diff --git a/arch/arm64/mm/mmu.c b/arch/arm64/mm/mmu.c
index 224b44ab534..eeecc9c8ed6 100644
--- a/arch/arm64/mm/mmu.c
+++ b/arch/arm64/mm/mmu.c
@@ -261,7 +261,7 @@ static void __init create_mapping(phys_addr_t phys, unsigned long virt,
 void __iomem * __init early_io_map(phys_addr_t phys, unsigned long virt)
 {
 	unsigned long size, mask;
-	bool page64k = IS_ENABLED(ARM64_64K_PAGES);
+	bool page64k = IS_ENABLED(CONFIG_ARM64_64K_PAGES);
 	pgd_t *pgd;
 	pud_t *pud;
 	pmd_t *pmd;
@@ -391,17 +391,14 @@ int kern_addr_valid(unsigned long addr)
 }
 #ifdef CONFIG_SPARSEMEM_VMEMMAP
 #ifdef CONFIG_ARM64_64K_PAGES
-int __meminit vmemmap_populate(struct page *start_page,
-			       unsigned long size, int node)
+int __meminit vmemmap_populate(unsigned long start, unsigned long end, int node)
 {
-	return vmemmap_populate_basepages(start_page, size, node);
+	return vmemmap_populate_basepages(start, end, node);
 }
 #else	/* !CONFIG_ARM64_64K_PAGES */
-int __meminit vmemmap_populate(struct page *start_page,
-			       unsigned long size, int node)
+int __meminit vmemmap_populate(unsigned long start, unsigned long end, int node)
 {
-	unsigned long addr = (unsigned long)start_page;
-	unsigned long end = (unsigned long)(start_page + size);
+	unsigned long addr = start;
 	unsigned long next;
 	pgd_t *pgd;
 	pud_t *pud;
@@ -434,7 +431,7 @@ int __meminit vmemmap_populate(struct page *start_page,
 	return 0;
 }
 #endif	/* CONFIG_ARM64_64K_PAGES */
-void vmemmap_free(struct page *memmap, unsigned long nr_pages)
+void vmemmap_free(unsigned long start, unsigned long end)
 {
 }
 #endif	/* CONFIG_SPARSEMEM_VMEMMAP */
diff --git a/arch/avr32/include/asm/io.h b/arch/avr32/include/asm/io.h
index cf60d0a9f17..fc6483f83cc 100644
--- a/arch/avr32/include/asm/io.h
+++ b/arch/avr32/include/asm/io.h
@@ -165,6 +165,10 @@ BUILDIO_IOPORT(l, u32)
 #define readw_be			__raw_readw
 #define readl_be			__raw_readl
 
+#define writeb_relaxed			writeb
+#define writew_relaxed			writew
+#define writel_relaxed			writel
+
 #define writeb_be			__raw_writeb
 #define writew_be			__raw_writew
 #define writel_be			__raw_writel
diff --git a/arch/avr32/kernel/process.c b/arch/avr32/kernel/process.c
index fd78f58ea79..073c3c2fa52 100644
--- a/arch/avr32/kernel/process.c
+++ b/arch/avr32/kernel/process.c
@@ -30,18 +30,9 @@ EXPORT_SYMBOL(pm_power_off);
  * This file handles the architecture-dependent parts of process handling..
  */
 
-void cpu_idle(void)
+void arch_cpu_idle(void)
 {
-	/* endless idle loop with no priority at all */
-	while (1) {
-		tick_nohz_idle_enter();
-		rcu_idle_enter();
-		while (!need_resched())
-			cpu_idle_sleep();
-		rcu_idle_exit();
-		tick_nohz_idle_exit();
-		schedule_preempt_disabled();
-	}
+	cpu_enter_idle();
 }
 
 void machine_halt(void)
diff --git a/arch/avr32/kernel/time.c b/arch/avr32/kernel/time.c
index 05ad29112ff..869a1c6ffee 100644
--- a/arch/avr32/kernel/time.c
+++ b/arch/avr32/kernel/time.c
@@ -12,6 +12,7 @@
 #include <linux/irq.h>
 #include <linux/kernel.h>
 #include <linux/time.h>
+#include <linux/cpu.h>
 
 #include <asm/sysreg.h>
 
@@ -87,13 +88,17 @@ static void comparator_mode(enum clock_event_mode mode,
 		pr_debug("%s: start\n", evdev->name);
 		/* FALLTHROUGH */
 	case CLOCK_EVT_MODE_RESUME:
-		cpu_disable_idle_sleep();
+		/*
+		 * If we're using the COUNT and COMPARE registers we
+		 * need to force idle poll.
+		 */
+		cpu_idle_poll_ctrl(true);
 		break;
 	case CLOCK_EVT_MODE_UNUSED:
 	case CLOCK_EVT_MODE_SHUTDOWN:
 		sysreg_write(COMPARE, 0);
 		pr_debug("%s: stop\n", evdev->name);
-		cpu_enable_idle_sleep();
+		cpu_idle_poll_ctrl(false);
 		break;
 	default:
 		BUG();
diff --git a/arch/avr32/mach-at32ap/include/mach/pm.h b/arch/avr32/mach-at32ap/include/mach/pm.h
index 979b355b77b..f29ff2cd23d 100644
--- a/arch/avr32/mach-at32ap/include/mach/pm.h
+++ b/arch/avr32/mach-at32ap/include/mach/pm.h
@@ -21,30 +21,6 @@
 extern void cpu_enter_idle(void);
 extern void cpu_enter_standby(unsigned long sdramc_base);
 
-extern bool disable_idle_sleep;
-
-static inline void cpu_disable_idle_sleep(void)
-{
-	disable_idle_sleep = true;
-}
-
-static inline void cpu_enable_idle_sleep(void)
-{
-	disable_idle_sleep = false;
-}
-
-static inline void cpu_idle_sleep(void)
-{
-	/*
-	 * If we're using the COUNT and COMPARE registers for
-	 * timekeeping, we can't use the IDLE state.
-	 */
-	if (disable_idle_sleep)
-		cpu_relax();
-	else
-		cpu_enter_idle();
-}
-
 void intc_set_suspend_handler(unsigned long offset);
 #endif
 
diff --git a/arch/avr32/mach-at32ap/pm-at32ap700x.S b/arch/avr32/mach-at32ap/pm-at32ap700x.S
index f868f4ce761..1c8e4e6bff0 100644
--- a/arch/avr32/mach-at32ap/pm-at32ap700x.S
+++ b/arch/avr32/mach-at32ap/pm-at32ap700x.S
@@ -18,13 +18,6 @@
 /* Same as 0xfff00000 but fits in a 21 bit signed immediate */
 #define PM_BASE	-0x100000
 
-	.section .bss, "wa", @nobits
-	.global	disable_idle_sleep
-	.type	disable_idle_sleep, @object
-disable_idle_sleep:
-	.int	4
-	.size	disable_idle_sleep, . - disable_idle_sleep
-
 	/* Keep this close to the irq handlers */
 	.section .irq.text, "ax", @progbits
 
diff --git a/arch/avr32/mm/init.c b/arch/avr32/mm/init.c
index 2798c2d4a1c..e66e8406f99 100644
--- a/arch/avr32/mm/init.c
+++ b/arch/avr32/mm/init.c
@@ -146,34 +146,14 @@ void __init mem_init(void)
 		initsize >> 10);
 }
 
-static inline void free_area(unsigned long addr, unsigned long end, char *s)
-{
-	unsigned int size = (end - addr) >> 10;
-
-	for (; addr < end; addr += PAGE_SIZE) {
-		struct page *page = virt_to_page(addr);
-		ClearPageReserved(page);
-		init_page_count(page);
-		free_page(addr);
-		totalram_pages++;
-	}
-
-	if (size && s)
-		printk(KERN_INFO "Freeing %s memory: %dK (%lx - %lx)\n",
-		       s, size, end - (size << 10), end);
-}
-
 void free_initmem(void)
 {
-	free_area((unsigned long)__init_begin, (unsigned long)__init_end,
-		  "init");
+	free_initmem_default(0);
 }
 
 #ifdef CONFIG_BLK_DEV_INITRD
-
 void free_initrd_mem(unsigned long start, unsigned long end)
 {
-	free_area(start, end, "initrd");
+	free_reserved_area(start, end, 0, "initrd");
 }
-
 #endif
diff --git a/arch/blackfin/include/asm/bfin_sport3.h b/arch/blackfin/include/asm/bfin_sport3.h
index 03c00220d69..d82f5fa0ad9 100644
--- a/arch/blackfin/include/asm/bfin_sport3.h
+++ b/arch/blackfin/include/asm/bfin_sport3.h
@@ -41,7 +41,7 @@
 #define SPORT_CTL_LAFS                0x00020000    /* Late Transmit frame select */
 #define SPORT_CTL_RJUST               0x00040000    /* Right Justified mode select */
 #define SPORT_CTL_FSED                0x00080000    /* External frame sync edge select */
-#define SPORT_CTL_TFIEN               0x00100000    /* Transmit finish interrrupt enable select */
+#define SPORT_CTL_TFIEN               0x00100000    /* Transmit finish interrupt enable select */
 #define SPORT_CTL_GCLKEN              0x00200000    /* Gated clock mode select */
 #define SPORT_CTL_SPENSEC             0x01000000    /* Enable secondary channel */
 #define SPORT_CTL_SPTRAN              0x02000000    /* Data direction control */
diff --git a/arch/blackfin/kernel/early_printk.c b/arch/blackfin/kernel/early_printk.c
index 84ed8375113..61fbd2de993 100644
--- a/arch/blackfin/kernel/early_printk.c
+++ b/arch/blackfin/kernel/early_printk.c
@@ -25,8 +25,6 @@ extern struct console *bfin_earlyserial_init(unsigned int port,
 extern struct console *bfin_jc_early_init(void);
 #endif
 
-static struct console *early_console;
-
 /* Default console */
 #define DEFAULT_PORT 0
 #define DEFAULT_CFLAG CS8|B57600
diff --git a/arch/blackfin/kernel/process.c b/arch/blackfin/kernel/process.c
index 9782c0329c1..4aa5545c4fd 100644
--- a/arch/blackfin/kernel/process.c
+++ b/arch/blackfin/kernel/process.c
@@ -46,15 +46,14 @@ EXPORT_SYMBOL(pm_power_off);
  * The idle loop on BFIN
  */
 #ifdef CONFIG_IDLE_L1
-static void default_idle(void)__attribute__((l1_text));
-void cpu_idle(void)__attribute__((l1_text));
+void arch_cpu_idle(void)__attribute__((l1_text));
 #endif
 
 /*
  * This is our default idle handler.  We need to disable
  * interrupts here to ensure we don't miss a wakeup call.
  */
-static void default_idle(void)
+void arch_cpu_idle(void)
 {
 #ifdef CONFIG_IPIPE
 	ipipe_suspend_domain();
@@ -66,31 +65,12 @@ static void default_idle(void)
 	hard_local_irq_enable();
 }
 
-/*
- * The idle thread.  We try to conserve power, while trying to keep
- * overall latency low.  The architecture specific idle is passed
- * a value to indicate the level of "idleness" of the system.
- */
-void cpu_idle(void)
-{
-	/* endless idle loop with no priority at all */
-	while (1) {
-
 #ifdef CONFIG_HOTPLUG_CPU
-		if (cpu_is_offline(smp_processor_id()))
-			cpu_die();
-#endif
-		tick_nohz_idle_enter();
-		rcu_idle_enter();
-		while (!need_resched())
-			default_idle();
-		rcu_idle_exit();
-		tick_nohz_idle_exit();
-		preempt_enable_no_resched();
-		schedule();
-		preempt_disable();
-	}
+void arch_cpu_idle_dead(void)
+{
+	cpu_die();
 }
+#endif
 
 /*
  * Do necessary setup to start up a newly executed thread.
diff --git a/arch/blackfin/mach-bf609/boards/ezkit.c b/arch/blackfin/mach-bf609/boards/ezkit.c
index 61c1f47a4bf..97d70163958 100644
--- a/arch/blackfin/mach-bf609/boards/ezkit.c
+++ b/arch/blackfin/mach-bf609/boards/ezkit.c
@@ -936,19 +936,19 @@ static struct v4l2_input adv7842_inputs[] = {
 		.index = 2,
 		.name = "Component",
 		.type = V4L2_INPUT_TYPE_CAMERA,
-		.capabilities = V4L2_IN_CAP_CUSTOM_TIMINGS,
+		.capabilities = V4L2_IN_CAP_DV_TIMINGS,
 	},
 	{
 		.index = 3,
 		.name = "VGA",
 		.type = V4L2_INPUT_TYPE_CAMERA,
-		.capabilities = V4L2_IN_CAP_CUSTOM_TIMINGS,
+		.capabilities = V4L2_IN_CAP_DV_TIMINGS,
 	},
 	{
 		.index = 4,
 		.name = "HDMI",
 		.type = V4L2_INPUT_TYPE_CAMERA,
-		.capabilities = V4L2_IN_CAP_CUSTOM_TIMINGS,
+		.capabilities = V4L2_IN_CAP_DV_TIMINGS,
 	},
 };
 
@@ -1074,7 +1074,7 @@ static struct v4l2_output adv7511_outputs[] = {
 		.index = 0,
 		.name = "HDMI",
 		.type = V4L2_INPUT_TYPE_CAMERA,
-		.capabilities = V4L2_OUT_CAP_CUSTOM_TIMINGS,
+		.capabilities = V4L2_OUT_CAP_DV_TIMINGS,
 	},
 };
 
diff --git a/arch/blackfin/mach-common/smp.c b/arch/blackfin/mach-common/smp.c
index bb61ae4986e..1bc2ce6f3c9 100644
--- a/arch/blackfin/mach-common/smp.c
+++ b/arch/blackfin/mach-common/smp.c
@@ -335,7 +335,7 @@ void __cpuinit secondary_start_kernel(void)
 	 */
 	calibrate_delay();
 
-	cpu_idle();
+	cpu_startup_entry(CPUHP_ONLINE);
 }
 
 void __init smp_prepare_boot_cpu(void)
diff --git a/arch/blackfin/mm/init.c b/arch/blackfin/mm/init.c
index 9cb85537bd2..82d01a71207 100644
--- a/arch/blackfin/mm/init.c
+++ b/arch/blackfin/mm/init.c
@@ -103,7 +103,7 @@ void __init mem_init(void)
 	max_mapnr = num_physpages = MAP_NR(high_memory);
 	printk(KERN_DEBUG "Kernel managed physical pages: %lu\n", num_physpages);
 
-	/* This will put all memory onto the freelists. */
+	/* This will put all low memory onto the freelists. */
 	totalram_pages = free_all_bootmem();
 
 	reservedpages = 0;
@@ -129,24 +129,11 @@ void __init mem_init(void)
 		initk, codek, datak, DMA_UNCACHED_REGION >> 10, (reservedpages << (PAGE_SHIFT-10)));
 }
 
-static void __init free_init_pages(const char *what, unsigned long begin, unsigned long end)
-{
-	unsigned long addr;
-	/* next to check that the page we free is not a partial page */
-	for (addr = begin; addr + PAGE_SIZE <= end; addr += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(addr));
-		init_page_count(virt_to_page(addr));
-		free_page(addr);
-		totalram_pages++;
-	}
-	printk(KERN_INFO "Freeing %s: %ldk freed\n", what, (end - begin) >> 10);
-}
-
 #ifdef CONFIG_BLK_DEV_INITRD
 void __init free_initrd_mem(unsigned long start, unsigned long end)
 {
 #ifndef CONFIG_MPU
-	free_init_pages("initrd memory", start, end);
+	free_reserved_area(start, end, 0, "initrd");
 #endif
 }
 #endif
@@ -154,10 +141,7 @@ void __init free_initrd_mem(unsigned long start, unsigned long end)
 void __init_refok free_initmem(void)
 {
 #if defined CONFIG_RAMKERNEL && !defined CONFIG_MPU
-	free_init_pages("unused kernel memory",
-			(unsigned long)(&__init_begin),
-			(unsigned long)(&__init_end));
-
+	free_initmem_default(0);
 	if (memory_start == (unsigned long)(&__init_end))
 		memory_start = (unsigned long)(&__init_begin);
 #endif
diff --git a/arch/c6x/include/asm/irqflags.h b/arch/c6x/include/asm/irqflags.h
index cf78e09e18c..2c71d5634ec 100644
--- a/arch/c6x/include/asm/irqflags.h
+++ b/arch/c6x/include/asm/irqflags.h
@@ -27,7 +27,7 @@ static inline unsigned long arch_local_save_flags(void)
 /* set interrupt enabled status */
 static inline void arch_local_irq_restore(unsigned long flags)
 {
-	asm volatile (" mvc .s2 %0,CSR\n" : : "b"(flags));
+	asm volatile (" mvc .s2 %0,CSR\n" : : "b"(flags) : "memory");
 }
 
 /* unconditionally enable interrupts */
diff --git a/arch/c6x/kernel/process.c b/arch/c6x/kernel/process.c
index 6434df476f7..57d2ea8d197 100644
--- a/arch/c6x/kernel/process.c
+++ b/arch/c6x/kernel/process.c
@@ -33,7 +33,7 @@ extern asmlinkage void ret_from_kernel_thread(void);
 void (*pm_power_off)(void);
 EXPORT_SYMBOL(pm_power_off);
 
-static void c6x_idle(void)
+void arch_cpu_idle(void)
 {
 	unsigned long tmp;
 
@@ -49,32 +49,6 @@ static void c6x_idle(void)
 		      : "=b"(tmp));
 }
 
-/*
- * The idle loop for C64x
- */
-void cpu_idle(void)
-{
-	/* endless idle loop with no priority at all */
-	while (1) {
-		tick_nohz_idle_enter();
-		rcu_idle_enter();
-		while (1) {
-			local_irq_disable();
-			if (need_resched()) {
-				local_irq_enable();
-				break;
-			}
-			c6x_idle(); /* enables local irqs */
-		}
-		rcu_idle_exit();
-		tick_nohz_idle_exit();
-
-		preempt_enable_no_resched();
-		schedule();
-		preempt_disable();
-	}
-}
-
 static void halt_loop(void)
 {
 	printk(KERN_EMERG "System Halted, OK to turn off power\n");
diff --git a/arch/c6x/mm/init.c b/arch/c6x/mm/init.c
index 89395f09648..a9fcd89b251 100644
--- a/arch/c6x/mm/init.c
+++ b/arch/c6x/mm/init.c
@@ -77,37 +77,11 @@ void __init mem_init(void)
 #ifdef CONFIG_BLK_DEV_INITRD
 void __init free_initrd_mem(unsigned long start, unsigned long end)
 {
-	int pages = 0;
-	for (; start < end; start += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(start));
-		init_page_count(virt_to_page(start));
-		free_page(start);
-		totalram_pages++;
-		pages++;
-	}
-	printk(KERN_INFO "Freeing initrd memory: %luk freed\n",
-	       (pages * PAGE_SIZE) >> 10);
+	free_reserved_area(start, end, 0, "initrd");
 }
 #endif
 
 void __init free_initmem(void)
 {
-	unsigned long addr;
-
-	/*
-	 * The following code should be cool even if these sections
-	 * are not page aligned.
-	 */
-	addr = PAGE_ALIGN((unsigned long)(__init_begin));
-
-	/* next to check that the page we free is not a partial page */
-	for (; addr + PAGE_SIZE < (unsigned long)(__init_end);
-	     addr += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(addr));
-		init_page_count(virt_to_page(addr));
-		free_page(addr);
-		totalram_pages++;
-	}
-	printk(KERN_INFO "Freeing unused kernel memory: %dK freed\n",
-	       (int) ((addr - PAGE_ALIGN((long) &__init_begin)) >> 10));
+	free_initmem_default(0);
 }
diff --git a/arch/cris/arch-v10/kernel/process.c b/arch/cris/arch-v10/kernel/process.c
index b1018750cff..2ba23c13df6 100644
--- a/arch/cris/arch-v10/kernel/process.c
+++ b/arch/cris/arch-v10/kernel/process.c
@@ -30,8 +30,9 @@ void etrax_gpio_wake_up_check(void); /* drivers/gpio.c */
 void default_idle(void)
 {
 #ifdef CONFIG_ETRAX_GPIO
-  etrax_gpio_wake_up_check();
+	etrax_gpio_wake_up_check();
 #endif
+	local_irq_enable();
 }
 
 /*
diff --git a/arch/cris/arch-v32/kernel/process.c b/arch/cris/arch-v32/kernel/process.c
index 2b23ef0e445..57451faa9b2 100644
--- a/arch/cris/arch-v32/kernel/process.c
+++ b/arch/cris/arch-v32/kernel/process.c
@@ -20,18 +20,12 @@
 
 extern void stop_watchdog(void);
 
-extern int cris_hlt_counter;
-
 /* We use this if we don't have any better idle routine. */
 void default_idle(void)
 {
-	local_irq_disable();
-	if (!need_resched() && !cris_hlt_counter) {
-	        /* Halt until exception. */
-		__asm__ volatile("ei    \n\t"
-                                 "halt      ");
-	}
-	local_irq_enable();
+	/* Halt until exception. */
+	__asm__ volatile("ei    \n\t"
+			 "halt      ");
 }
 
 /*
diff --git a/arch/cris/arch-v32/kernel/smp.c b/arch/cris/arch-v32/kernel/smp.c
index 04a16edd540..cdd12028de0 100644
--- a/arch/cris/arch-v32/kernel/smp.c
+++ b/arch/cris/arch-v32/kernel/smp.c
@@ -145,8 +145,6 @@ smp_boot_one_cpu(int cpuid, struct task_struct idle)
  * specific stuff such as the local timer and the MMU. */
 void __init smp_callin(void)
 {
-	extern void cpu_idle(void);
-
 	int cpu = cpu_now_booting;
 	reg_intr_vect_rw_mask vect_mask = {0};
 
@@ -170,7 +168,7 @@ void __init smp_callin(void)
 	local_irq_enable();
 
 	set_cpu_online(cpu, true);
-	cpu_idle();
+	cpu_startup_entry(CPUHP_ONLINE);
 }
 
 /* Stop execution on this CPU.*/
diff --git a/arch/cris/include/asm/processor.h b/arch/cris/include/asm/processor.h
index 675823f70c0..c0a29b96b92 100644
--- a/arch/cris/include/asm/processor.h
+++ b/arch/cris/include/asm/processor.h
@@ -65,13 +65,6 @@ static inline void release_thread(struct task_struct *dead_task)
 
 #define cpu_relax()     barrier()
 
-/*
- * disable hlt during certain critical i/o operations
- */
-#define HAVE_DISABLE_HLT
-void disable_hlt(void);
-void enable_hlt(void);
-
 void default_idle(void);
 
 #endif /* __ASM_CRIS_PROCESSOR_H */
diff --git a/arch/cris/kernel/process.c b/arch/cris/kernel/process.c
index 104ff4dd9b9..b78498eb079 100644
--- a/arch/cris/kernel/process.c
+++ b/arch/cris/kernel/process.c
@@ -29,59 +29,14 @@
 
 //#define DEBUG
 
-/*
- * The hlt_counter, disable_hlt and enable_hlt is just here as a hook if
- * there would ever be a halt sequence (for power save when idle) with
- * some largish delay when halting or resuming *and* a driver that can't
- * afford that delay.  The hlt_counter would then be checked before
- * executing the halt sequence, and the driver marks the unhaltable
- * region by enable_hlt/disable_hlt.
- */
-
-int cris_hlt_counter=0;
-
-void disable_hlt(void)
-{
-	cris_hlt_counter++;
-}
-
-EXPORT_SYMBOL(disable_hlt);
-
-void enable_hlt(void)
-{
-	cris_hlt_counter--;
-}
-
-EXPORT_SYMBOL(enable_hlt);
- 
 extern void default_idle(void);
 
 void (*pm_power_off)(void);
 EXPORT_SYMBOL(pm_power_off);
 
-/*
- * The idle thread. There's no useful work to be
- * done, so just try to conserve power and have a
- * low exit latency (ie sit in a loop waiting for
- * somebody to say that they'd like to reschedule)
- */
-
-void cpu_idle (void)
+void arch_cpu_idle(void)
 {
-	/* endless idle loop with no priority at all */
-	while (1) {
-		rcu_idle_enter();
-		while (!need_resched()) {
-			/*
-			 * Mark this as an RCU critical section so that
-			 * synchronize_kernel() in the unload path waits
-			 * for our completion.
-			 */
-			default_idle();
-		}
-		rcu_idle_exit();
-		schedule_preempt_disabled();
-	}
+	default_idle();
 }
 
 void hard_reset_now (void);
diff --git a/arch/cris/mm/init.c b/arch/cris/mm/init.c
index d72ab58fd83..9ac80946dad 100644
--- a/arch/cris/mm/init.c
+++ b/arch/cris/mm/init.c
@@ -12,12 +12,10 @@
 #include <linux/init.h>
 #include <linux/bootmem.h>
 #include <asm/tlb.h>
+#include <asm/sections.h>
 
 unsigned long empty_zero_page;
 
-extern char _stext, _edata, _etext; /* From linkerscript */
-extern char __init_begin, __init_end;
-
 void __init
 mem_init(void)
 {
@@ -67,15 +65,5 @@ mem_init(void)
 void 
 free_initmem(void)
 {
-        unsigned long addr;
-
-        addr = (unsigned long)(&__init_begin);
-        for (; addr < (unsigned long)(&__init_end); addr += PAGE_SIZE) {
-                ClearPageReserved(virt_to_page(addr));
-                init_page_count(virt_to_page(addr));
-                free_page(addr);
-                totalram_pages++;
-        }
-        printk (KERN_INFO "Freeing unused kernel memory: %luk freed\n",
-		(unsigned long)((&__init_end - &__init_begin) >> 10));
+	free_initmem_default(0);
 }
diff --git a/arch/frv/kernel/process.c b/arch/frv/kernel/process.c
index 23916b2a12a..5d40aeb7712 100644
--- a/arch/frv/kernel/process.c
+++ b/arch/frv/kernel/process.c
@@ -59,29 +59,12 @@ static void core_sleep_idle(void)
 	mb();
 }
 
-void (*idle)(void) = core_sleep_idle;
-
-/*
- * The idle thread. There's no useful work to be
- * done, so just try to conserve power and have a
- * low exit latency (ie sit in a loop waiting for
- * somebody to say that they'd like to reschedule)
- */
-void cpu_idle(void)
+void arch_cpu_idle(void)
 {
-	/* endless idle loop with no priority at all */
-	while (1) {
-		rcu_idle_enter();
-		while (!need_resched()) {
-			check_pgt_cache();
-
-			if (!frv_dma_inprogress && idle)
-				idle();
-		}
-		rcu_idle_exit();
-
-		schedule_preempt_disabled();
-	}
+	if (!frv_dma_inprogress)
+		core_sleep_idle();
+	else
+		local_irq_enable();
 }
 
 void machine_restart(char * __unused)
diff --git a/arch/frv/mm/init.c b/arch/frv/mm/init.c
index 92e97b0894a..dee354fa6b6 100644
--- a/arch/frv/mm/init.c
+++ b/arch/frv/mm/init.c
@@ -122,7 +122,7 @@ void __init mem_init(void)
 #endif
 	int codek = 0, datak = 0;
 
-	/* this will put all memory onto the freelists */
+	/* this will put all low memory onto the freelists */
 	totalram_pages = free_all_bootmem();
 
 #ifdef CONFIG_MMU
@@ -131,14 +131,8 @@ void __init mem_init(void)
 			datapages++;
 
 #ifdef CONFIG_HIGHMEM
-	for (pfn = num_physpages - 1; pfn >= num_mappedpages; pfn--) {
-		struct page *page = &mem_map[pfn];
-
-		ClearPageReserved(page);
-		init_page_count(page);
-		__free_page(page);
-		totalram_pages++;
-	}
+	for (pfn = num_physpages - 1; pfn >= num_mappedpages; pfn--)
+		free_highmem_page(&mem_map[pfn]);
 #endif
 
 	codek = ((unsigned long) &_etext - (unsigned long) &_stext) >> 10;
@@ -168,21 +162,7 @@ void __init mem_init(void)
 void free_initmem(void)
 {
 #if defined(CONFIG_RAMKERNEL) && !defined(CONFIG_PROTECT_KERNEL)
-	unsigned long start, end, addr;
-
-	start = PAGE_ALIGN((unsigned long) &__init_begin);	/* round up */
-	end   = ((unsigned long) &__init_end) & PAGE_MASK;	/* round down */
-
-	/* next to check that the page we free is not a partial page */
-	for (addr = start; addr < end; addr += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(addr));
-		init_page_count(virt_to_page(addr));
-		free_page(addr);
-		totalram_pages++;
-	}
-
-	printk("Freeing unused kernel memory: %ldKiB freed (0x%lx - 0x%lx)\n",
-	       (end - start) >> 10, start, end);
+	free_initmem_default(0);
 #endif
 } /* end free_initmem() */
 
@@ -193,14 +173,6 @@ void free_initmem(void)
 #ifdef CONFIG_BLK_DEV_INITRD
 void __init free_initrd_mem(unsigned long start, unsigned long end)
 {
-	int pages = 0;
-	for (; start < end; start += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(start));
-		init_page_count(virt_to_page(start));
-		free_page(start);
-		totalram_pages++;
-		pages++;
-	}
-	printk("Freeing initrd memory: %dKiB freed\n", (pages * PAGE_SIZE) >> 10);
+	free_reserved_area(start, end, 0, "initrd");
 } /* end free_initrd_mem() */
 #endif
diff --git a/arch/h8300/kernel/process.c b/arch/h8300/kernel/process.c
index b609f63f159..a17d2cd463d 100644
--- a/arch/h8300/kernel/process.c
+++ b/arch/h8300/kernel/process.c
@@ -53,40 +53,13 @@ asmlinkage void ret_from_kernel_thread(void);
  * The idle loop on an H8/300..
  */
 #if !defined(CONFIG_H8300H_SIM) && !defined(CONFIG_H8S_SIM)
-static void default_idle(void)
+void arch_cpu_idle(void)
 {
-	local_irq_disable();
-	if (!need_resched()) {
-		local_irq_enable();
-		/* XXX: race here! What if need_resched() gets set now? */
-		__asm__("sleep");
-	} else
-		local_irq_enable();
-}
-#else
-static void default_idle(void)
-{
-	cpu_relax();
+	local_irq_enable();
+	/* XXX: race here! What if need_resched() gets set now? */
+	__asm__("sleep");
 }
 #endif
-void (*idle)(void) = default_idle;
-
-/*
- * The idle thread. There's no useful work to be
- * done, so just try to conserve power and have a
- * low exit latency (ie sit in a loop waiting for
- * somebody to say that they'd like to reschedule)
- */
-void cpu_idle(void)
-{
-	while (1) {
-		rcu_idle_enter();
-		while (!need_resched())
-			idle();
-		rcu_idle_exit();
-		schedule_preempt_disabled();
-	}
-}
 
 void machine_restart(char * __unused)
 {
diff --git a/arch/h8300/mm/init.c b/arch/h8300/mm/init.c
index 981e25094b1..ff349d70a29 100644
--- a/arch/h8300/mm/init.c
+++ b/arch/h8300/mm/init.c
@@ -139,7 +139,7 @@ void __init mem_init(void)
 	start_mem = PAGE_ALIGN(start_mem);
 	max_mapnr = num_physpages = MAP_NR(high_memory);
 
-	/* this will put all memory onto the freelists */
+	/* this will put all low memory onto the freelists */
 	totalram_pages = free_all_bootmem();
 
 	codek = (_etext - _stext) >> 10;
@@ -161,15 +161,7 @@ void __init mem_init(void)
 #ifdef CONFIG_BLK_DEV_INITRD
 void free_initrd_mem(unsigned long start, unsigned long end)
 {
-	int pages = 0;
-	for (; start < end; start += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(start));
-		init_page_count(virt_to_page(start));
-		free_page(start);
-		totalram_pages++;
-		pages++;
-	}
-	printk ("Freeing initrd memory: %dk freed\n", pages);
+	free_reserved_area(start, end, 0, "initrd");
 }
 #endif
 
@@ -177,23 +169,7 @@ void
 free_initmem(void)
 {
 #ifdef CONFIG_RAMKERNEL
-	unsigned long addr;
-/*
- *	the following code should be cool even if these sections
- *	are not page aligned.
- */
-	addr = PAGE_ALIGN((unsigned long)(__init_begin));
-	/* next to check that the page we free is not a partial page */
-	for (; addr + PAGE_SIZE < (unsigned long)__init_end; addr +=PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(addr));
-		init_page_count(virt_to_page(addr));
-		free_page(addr);
-		totalram_pages++;
-	}
-	printk(KERN_INFO "Freeing unused kernel memory: %ldk freed (0x%x - 0x%x)\n",
-			(addr - PAGE_ALIGN((long) __init_begin)) >> 10,
-			(int)(PAGE_ALIGN((unsigned long)__init_begin)),
-			(int)(addr - PAGE_SIZE));
+	free_initmem_default(0);
 #endif
 }
 
diff --git a/arch/hexagon/kernel/process.c b/arch/hexagon/kernel/process.c
index 06ae9ffcabd..9b948c619a0 100644
--- a/arch/hexagon/kernel/process.c
+++ b/arch/hexagon/kernel/process.c
@@ -51,28 +51,11 @@ void start_thread(struct pt_regs *regs, unsigned long pc, unsigned long sp)
  *  If hardware or VM offer wait termination even though interrupts
  *  are disabled.
  */
-static void default_idle(void)
+void arch_cpu_idle(void)
 {
 	__vmwait();
-}
-
-void (*idle_sleep)(void) = default_idle;
-
-void cpu_idle(void)
-{
-	while (1) {
-		tick_nohz_idle_enter();
-		local_irq_disable();
-		while (!need_resched()) {
-			idle_sleep();
-			/*  interrupts wake us up, but aren't serviced  */
-			local_irq_enable();	/* service interrupt   */
-			local_irq_disable();
-		}
-		local_irq_enable();
-		tick_nohz_idle_exit();
-		schedule();
-	}
+	/*  interrupts wake us up, but irqs are still disabled */
+	local_irq_enable();
 }
 
 /*
diff --git a/arch/hexagon/kernel/smp.c b/arch/hexagon/kernel/smp.c
index 8e095dffd07..0e364ca4319 100644
--- a/arch/hexagon/kernel/smp.c
+++ b/arch/hexagon/kernel/smp.c
@@ -184,7 +184,7 @@ void __cpuinit start_secondary(void)
 
 	local_irq_enable();
 
-	cpu_idle();
+	cpu_startup_entry(CPUHP_ONLINE);
 }
 
 
diff --git a/arch/ia64/Kconfig b/arch/ia64/Kconfig
index 9a02f71c6b1..e7e55a00f94 100644
--- a/arch/ia64/Kconfig
+++ b/arch/ia64/Kconfig
@@ -187,7 +187,7 @@ config IA64_DIG
 
 config IA64_DIG_VTD
 	bool "DIG+Intel+IOMMU"
-	select DMAR
+	select INTEL_IOMMU
 	select PCI_MSI
 
 config IA64_HP_ZX1
diff --git a/arch/ia64/hp/sim/simserial.c b/arch/ia64/hp/sim/simserial.c
index da2f319fb71..e70cadec7ce 100644
--- a/arch/ia64/hp/sim/simserial.c
+++ b/arch/ia64/hp/sim/simserial.c
@@ -142,8 +142,7 @@ static void transmit_chars(struct tty_struct *tty, struct serial_state *info,
 		goto out;
 	}
 
-	if (info->xmit.head == info->xmit.tail || tty->stopped ||
-			tty->hw_stopped) {
+	if (info->xmit.head == info->xmit.tail || tty->stopped) {
 #ifdef SIMSERIAL_DEBUG
 		printk("transmit_chars: head=%d, tail=%d, stopped=%d\n",
 		       info->xmit.head, info->xmit.tail, tty->stopped);
@@ -181,7 +180,7 @@ static void rs_flush_chars(struct tty_struct *tty)
 	struct serial_state *info = tty->driver_data;
 
 	if (info->xmit.head == info->xmit.tail || tty->stopped ||
-			tty->hw_stopped || !info->xmit.buf)
+			!info->xmit.buf)
 		return;
 
 	transmit_chars(tty, info, NULL);
@@ -217,7 +216,7 @@ static int rs_write(struct tty_struct * tty,
 	 * Hey, we transmit directly from here in our case
 	 */
 	if (CIRC_CNT(info->xmit.head, info->xmit.tail, SERIAL_XMIT_SIZE) &&
-			!tty->stopped && !tty->hw_stopped)
+			!tty->stopped)
 		transmit_chars(tty, info, NULL);
 
 	return ret;
@@ -325,14 +324,6 @@ static int rs_ioctl(struct tty_struct *tty, unsigned int cmd, unsigned long arg)
 
 #define RELEVANT_IFLAG(iflag) (iflag & (IGNBRK|BRKINT|IGNPAR|PARMRK|INPCK))
 
-static void rs_set_termios(struct tty_struct *tty, struct ktermios *old_termios)
-{
-	/* Handle turning off CRTSCTS */
-	if ((old_termios->c_cflag & CRTSCTS) &&
-	    !(tty->termios.c_cflag & CRTSCTS)) {
-		tty->hw_stopped = 0;
-	}
-}
 /*
  * This routine will shutdown a serial port; interrupts are disabled, and
  * DTR is dropped if the hangup on close termio flag is on.
@@ -481,7 +472,6 @@ static const struct tty_operations hp_ops = {
 	.throttle = rs_throttle,
 	.unthrottle = rs_unthrottle,
 	.send_xchar = rs_send_xchar,
-	.set_termios = rs_set_termios,
 	.hangup = rs_hangup,
 	.proc_fops = &rs_proc_fops,
 };
diff --git a/arch/ia64/include/asm/futex.h b/arch/ia64/include/asm/futex.h
index d2bf1fd5e44..76acbcd5c06 100644
--- a/arch/ia64/include/asm/futex.h
+++ b/arch/ia64/include/asm/futex.h
@@ -106,16 +106,15 @@ futex_atomic_cmpxchg_inatomic(u32 *uval, u32 __user *uaddr,
 		return -EFAULT;
 
 	{
-		register unsigned long r8 __asm ("r8");
+		register unsigned long r8 __asm ("r8") = 0;
 		unsigned long prev;
 		__asm__ __volatile__(
 			"	mf;;					\n"
-			"	mov %0=r0				\n"
 			"	mov ar.ccv=%4;;				\n"
 			"[1:]	cmpxchg4.acq %1=[%2],%3,ar.ccv		\n"
 			"	.xdata4 \"__ex_table\", 1b-., 2f-.	\n"
 			"[2:]"
-			: "=r" (r8), "=r" (prev)
+			: "+r" (r8), "=&r" (prev)
 			: "r" (uaddr), "r" (newval),
 			  "rO" ((long) (unsigned) oldval)
 			: "memory");
diff --git a/arch/ia64/include/asm/hugetlb.h b/arch/ia64/include/asm/hugetlb.h
index 94eaa5bd5d0..aa910054b8e 100644
--- a/arch/ia64/include/asm/hugetlb.h
+++ b/arch/ia64/include/asm/hugetlb.h
@@ -2,6 +2,7 @@
 #define _ASM_IA64_HUGETLB_H
 
 #include <asm/page.h>
+#include <asm-generic/hugetlb.h>
 
 
 void hugetlb_free_pgd_range(struct mmu_gather *tlb, unsigned long addr,
diff --git a/arch/ia64/include/asm/irqflags.h b/arch/ia64/include/asm/irqflags.h
index 2b68d856dc7..1bf2cf2f4ab 100644
--- a/arch/ia64/include/asm/irqflags.h
+++ b/arch/ia64/include/asm/irqflags.h
@@ -89,6 +89,7 @@ static inline bool arch_irqs_disabled(void)
 
 static inline void arch_safe_halt(void)
 {
+	arch_local_irq_enable();
 	ia64_pal_halt_light();	/* PAL_HALT_LIGHT */
 }
 
diff --git a/arch/ia64/include/asm/mca.h b/arch/ia64/include/asm/mca.h
index 43f96ab18fa..8c709616871 100644
--- a/arch/ia64/include/asm/mca.h
+++ b/arch/ia64/include/asm/mca.h
@@ -143,6 +143,7 @@ extern unsigned long __per_cpu_mca[NR_CPUS];
 extern int cpe_vector;
 extern int ia64_cpe_irq;
 extern void ia64_mca_init(void);
+extern void ia64_mca_irq_init(void);
 extern void ia64_mca_cpu_init(void *);
 extern void ia64_os_mca_dispatch(void);
 extern void ia64_os_mca_dispatch_end(void);
diff --git a/arch/ia64/include/asm/numa.h b/arch/ia64/include/asm/numa.h
index 2e27ef17565..2db0a6c6daa 100644
--- a/arch/ia64/include/asm/numa.h
+++ b/arch/ia64/include/asm/numa.h
@@ -67,14 +67,13 @@ extern int paddr_to_nid(unsigned long paddr);
 
 extern void map_cpu_to_node(int cpu, int nid);
 extern void unmap_cpu_from_node(int cpu, int nid);
-
+extern void numa_clear_node(int cpu);
 
 #else /* !CONFIG_NUMA */
 #define map_cpu_to_node(cpu, nid)	do{}while(0)
 #define unmap_cpu_from_node(cpu, nid)	do{}while(0)
-
 #define paddr_to_nid(addr)	0
-
+#define numa_clear_node(cpu)	do { } while (0)
 #endif /* CONFIG_NUMA */
 
 #endif /* _ASM_IA64_NUMA_H */
diff --git a/arch/ia64/include/asm/thread_info.h b/arch/ia64/include/asm/thread_info.h
index 020d655ed08..cade13dd029 100644
--- a/arch/ia64/include/asm/thread_info.h
+++ b/arch/ia64/include/asm/thread_info.h
@@ -131,8 +131,6 @@ struct thread_info {
 #define TS_POLLING		1 	/* true if in idle loop and not sleeping */
 #define TS_RESTORE_SIGMASK	2	/* restore signal mask in do_signal() */
 
-#define tsk_is_polling(t) (task_thread_info(t)->status & TS_POLLING)
-
 #ifndef __ASSEMBLY__
 #define HAVE_SET_RESTORE_SIGMASK	1
 static inline void set_restore_sigmask(void)
diff --git a/arch/ia64/kernel/fsys.S b/arch/ia64/kernel/fsys.S
index c4cd45d9774..abc6dee3799 100644
--- a/arch/ia64/kernel/fsys.S
+++ b/arch/ia64/kernel/fsys.S
@@ -90,53 +90,6 @@ ENTRY(fsys_getpid)
 	FSYS_RETURN
 END(fsys_getpid)
 
-ENTRY(fsys_getppid)
-	.prologue
-	.altrp b6
-	.body
-	add r17=IA64_TASK_GROUP_LEADER_OFFSET,r16
-	;;
-	ld8 r17=[r17]				// r17 = current->group_leader
-	add r9=TI_FLAGS+IA64_TASK_SIZE,r16
-	;;
-
-	ld4 r9=[r9]
-	add r17=IA64_TASK_REAL_PARENT_OFFSET,r17 // r17 = &current->group_leader->real_parent
-	;;
-	and r9=TIF_ALLWORK_MASK,r9
-
-1:	ld8 r18=[r17]				// r18 = current->group_leader->real_parent
-	;;
-	cmp.ne p8,p0=0,r9
-	add r8=IA64_TASK_TGID_OFFSET,r18	// r8 = &current->group_leader->real_parent->tgid
-	;;
-
-	/*
-	 * The .acq is needed to ensure that the read of tgid has returned its data before
-	 * we re-check "real_parent".
-	 */
-	ld4.acq r8=[r8]				// r8 = current->group_leader->real_parent->tgid
-#ifdef CONFIG_SMP
-	/*
-	 * Re-read current->group_leader->real_parent.
-	 */
-	ld8 r19=[r17]				// r19 = current->group_leader->real_parent
-(p8)	br.spnt.many fsys_fallback_syscall
-	;;
-	cmp.ne p6,p0=r18,r19			// did real_parent change?
-	mov r19=0			// i must not leak kernel bits...
-(p6)	br.cond.spnt.few 1b			// yes -> redo the read of tgid and the check
-	;;
-	mov r17=0			// i must not leak kernel bits...
-	mov r18=0			// i must not leak kernel bits...
-#else
-	mov r17=0			// i must not leak kernel bits...
-	mov r18=0			// i must not leak kernel bits...
-	mov r19=0			// i must not leak kernel bits...
-#endif
-	FSYS_RETURN
-END(fsys_getppid)
-
 ENTRY(fsys_set_tid_address)
 	.prologue
 	.altrp b6
@@ -614,7 +567,7 @@ paravirt_fsyscall_table:
 	data8 0				// chown
 	data8 0				// lseek		// 1040
 	data8 fsys_getpid		// getpid
-	data8 fsys_getppid		// getppid
+	data8 0				// getppid
 	data8 0				// mount
 	data8 0				// umount
 	data8 0				// setuid		// 1045
diff --git a/arch/ia64/kernel/iosapic.c b/arch/ia64/kernel/iosapic.c
index ee33c3aaa2f..19f107be734 100644
--- a/arch/ia64/kernel/iosapic.c
+++ b/arch/ia64/kernel/iosapic.c
@@ -76,7 +76,7 @@
  *	PCI pin -> global system interrupt (GSI) -> IA-64 vector <-> IRQ
  *
  * Note: The term "IRQ" is loosely used everywhere in Linux kernel to
- * describeinterrupts.  Now we use "IRQ" only for Linux IRQ's.  ISA IRQ
+ * describe interrupts.  Now we use "IRQ" only for Linux IRQ's.  ISA IRQ
  * (isa_irq) is the only exception in this source code.
  */
 
@@ -1010,6 +1010,26 @@ iosapic_check_gsi_range (unsigned int gsi_base, unsigned int ver)
 	return 0;
 }
 
+static int
+iosapic_delete_rte(unsigned int irq, unsigned int gsi)
+{
+	struct iosapic_rte_info *rte, *temp;
+
+	list_for_each_entry_safe(rte, temp, &iosapic_intr_info[irq].rtes,
+								rte_list) {
+		if (rte->iosapic->gsi_base + rte->rte_index == gsi) {
+			if (rte->refcnt)
+				return -EBUSY;
+
+			list_del(&rte->rte_list);
+			kfree(rte);
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
 int iosapic_init(unsigned long phys_addr, unsigned int gsi_base)
 {
 	int num_rte, err, index;
@@ -1069,7 +1089,7 @@ int iosapic_init(unsigned long phys_addr, unsigned int gsi_base)
 
 int iosapic_remove(unsigned int gsi_base)
 {
-	int index, err = 0;
+	int i, irq, index, err = 0;
 	unsigned long flags;
 
 	spin_lock_irqsave(&iosapic_lock, flags);
@@ -1087,6 +1107,16 @@ int iosapic_remove(unsigned int gsi_base)
 		goto out;
 	}
 
+	for (i = gsi_base; i < gsi_base + iosapic_lists[index].num_rte; i++) {
+		irq = __gsi_to_irq(i);
+		if (irq < 0)
+			continue;
+
+		err = iosapic_delete_rte(irq, i);
+		if (err)
+			goto out;
+	}
+
 	iounmap(iosapic_lists[index].addr);
 	iosapic_free(index);
  out:
diff --git a/arch/ia64/kernel/irq.c b/arch/ia64/kernel/irq.c
index ad69606613e..f2c41828113 100644
--- a/arch/ia64/kernel/irq.c
+++ b/arch/ia64/kernel/irq.c
@@ -23,6 +23,8 @@
 #include <linux/interrupt.h>
 #include <linux/kernel_stat.h>
 
+#include <asm/mca.h>
+
 /*
  * 'what should we do if we get a hw irq event on an illegal vector'.
  * each architecture has to answer this themselves.
@@ -83,6 +85,12 @@ bool is_affinity_mask_valid(const struct cpumask *cpumask)
 
 #endif /* CONFIG_SMP */
 
+int __init arch_early_irq_init(void)
+{
+	ia64_mca_irq_init();
+	return 0;
+}
+
 #ifdef CONFIG_HOTPLUG_CPU
 unsigned int vectors_in_migration[NR_IRQS];
 
diff --git a/arch/ia64/kernel/mca.c b/arch/ia64/kernel/mca.c
index 65bf9cd3904..d7396dbb07b 100644
--- a/arch/ia64/kernel/mca.c
+++ b/arch/ia64/kernel/mca.c
@@ -2074,22 +2074,16 @@ ia64_mca_init(void)
 	printk(KERN_INFO "MCA related initialization done\n");
 }
 
+
 /*
- * ia64_mca_late_init
- *
- *	Opportunity to setup things that require initialization later
- *	than ia64_mca_init.  Setup a timer to poll for CPEs if the
- *	platform doesn't support an interrupt driven mechanism.
- *
- *  Inputs  :   None
- *  Outputs :   Status
+ * These pieces cannot be done in ia64_mca_init() because it is called before
+ * early_irq_init() which would wipe out our percpu irq registrations. But we
+ * cannot leave them until ia64_mca_late_init() because by then all the other
+ * processors have been brought online and have set their own CMC vectors to
+ * point at a non-existant action. Called from arch_early_irq_init().
  */
-static int __init
-ia64_mca_late_init(void)
+void __init ia64_mca_irq_init(void)
 {
-	if (!mca_init)
-		return 0;
-
 	/*
 	 *  Configure the CMCI/P vector and handler. Interrupts for CMC are
 	 *  per-processor, so AP CMC interrupts are setup in smp_callin() (smpboot.c).
@@ -2108,6 +2102,23 @@ ia64_mca_late_init(void)
 	/* Setup the CPEI/P handler */
 	register_percpu_irq(IA64_CPEP_VECTOR, &mca_cpep_irqaction);
 #endif
+}
+
+/*
+ * ia64_mca_late_init
+ *
+ *	Opportunity to setup things that require initialization later
+ *	than ia64_mca_init.  Setup a timer to poll for CPEs if the
+ *	platform doesn't support an interrupt driven mechanism.
+ *
+ *  Inputs  :   None
+ *  Outputs :   Status
+ */
+static int __init
+ia64_mca_late_init(void)
+{
+	if (!mca_init)
+		return 0;
 
 	register_hotcpu_notifier(&mca_cpu_notifier);
 
diff --git a/arch/ia64/kernel/mca_drv.c b/arch/ia64/kernel/mca_drv.c
index 9392e021c93..94f8bf777af 100644
--- a/arch/ia64/kernel/mca_drv.c
+++ b/arch/ia64/kernel/mca_drv.c
@@ -349,7 +349,7 @@ init_record_index_pools(void)
 
 	/* - 3 - */
 	slidx_pool.max_idx = (rec_max_size/sect_min_size) * 2 + 1;
-	slidx_pool.buffer = (slidx_list_t *)
+	slidx_pool.buffer =
 		kmalloc(slidx_pool.max_idx * sizeof(slidx_list_t), GFP_KERNEL);
 
 	return slidx_pool.buffer ? 0 : -ENOMEM;
diff --git a/arch/ia64/kernel/palinfo.c b/arch/ia64/kernel/palinfo.c
index 77597e5ea60..79521d5499f 100644
--- a/arch/ia64/kernel/palinfo.c
+++ b/arch/ia64/kernel/palinfo.c
@@ -849,17 +849,6 @@ static palinfo_entry_t palinfo_entries[]={
 
 #define NR_PALINFO_ENTRIES	(int) ARRAY_SIZE(palinfo_entries)
 
-/*
- * this array is used to keep track of the proc entries we create. This is
- * required in the module mode when we need to remove all entries. The procfs code
- * does not do recursion of deletion
- *
- * Notes:
- *	- +1 accounts for the cpuN directory entry in /proc/pal
- */
-#define NR_PALINFO_PROC_ENTRIES	(NR_CPUS*(NR_PALINFO_ENTRIES+1))
-
-static struct proc_dir_entry *palinfo_proc_entries[NR_PALINFO_PROC_ENTRIES];
 static struct proc_dir_entry *palinfo_dir;
 
 /*
@@ -971,60 +960,32 @@ palinfo_read_entry(char *page, char **start, off_t off, int count, int *eof, voi
 static void __cpuinit
 create_palinfo_proc_entries(unsigned int cpu)
 {
-#	define CPUSTR	"cpu%d"
-
 	pal_func_cpu_u_t f;
-	struct proc_dir_entry **pdir;
 	struct proc_dir_entry *cpu_dir;
 	int j;
-	char cpustr[sizeof(CPUSTR)];
-
-
-	/*
-	 * we keep track of created entries in a depth-first order for
-	 * cleanup purposes. Each entry is stored into palinfo_proc_entries
-	 */
-	sprintf(cpustr,CPUSTR, cpu);
+	char cpustr[3+4+1];	/* cpu numbers are up to 4095 on itanic */
+	sprintf(cpustr, "cpu%d", cpu);
 
 	cpu_dir = proc_mkdir(cpustr, palinfo_dir);
+	if (!cpu_dir)
+		return;
 
 	f.req_cpu = cpu;
 
-	/*
-	 * Compute the location to store per cpu entries
-	 * We dont store the top level entry in this list, but
-	 * remove it finally after removing all cpu entries.
-	 */
-	pdir = &palinfo_proc_entries[cpu*(NR_PALINFO_ENTRIES+1)];
-	*pdir++ = cpu_dir;
 	for (j=0; j < NR_PALINFO_ENTRIES; j++) {
 		f.func_id = j;
-		*pdir = create_proc_read_entry(
-				palinfo_entries[j].name, 0, cpu_dir,
-				palinfo_read_entry, (void *)f.value);
-		pdir++;
+		create_proc_read_entry(
+			palinfo_entries[j].name, 0, cpu_dir,
+			palinfo_read_entry, (void *)f.value);
 	}
 }
 
 static void
 remove_palinfo_proc_entries(unsigned int hcpu)
 {
-	int j;
-	struct proc_dir_entry *cpu_dir, **pdir;
-
-	pdir = &palinfo_proc_entries[hcpu*(NR_PALINFO_ENTRIES+1)];
-	cpu_dir = *pdir;
-	*pdir++=NULL;
-	for (j=0; j < (NR_PALINFO_ENTRIES); j++) {
-		if ((*pdir)) {
-			remove_proc_entry ((*pdir)->name, cpu_dir);
-			*pdir ++= NULL;
-		}
-	}
-
-	if (cpu_dir) {
-		remove_proc_entry(cpu_dir->name, palinfo_dir);
-	}
+	char cpustr[3+4+1];	/* cpu numbers are up to 4095 on itanic */
+	sprintf(cpustr, "cpu%d", hcpu);
+	remove_proc_subtree(cpustr, palinfo_dir);
 }
 
 static int __cpuinit palinfo_cpu_callback(struct notifier_block *nfb,
@@ -1058,6 +1019,8 @@ palinfo_init(void)
 
 	printk(KERN_INFO "PAL Information Facility v%s\n", PALINFO_VERSION);
 	palinfo_dir = proc_mkdir("pal", NULL);
+	if (!palinfo_dir)
+		return -ENOMEM;
 
 	/* Create palinfo dirs in /proc for all online cpus */
 	for_each_online_cpu(i) {
@@ -1073,22 +1036,8 @@ palinfo_init(void)
 static void __exit
 palinfo_exit(void)
 {
-	int i = 0;
-
-	/* remove all nodes: depth first pass. Could optimize this  */
-	for_each_online_cpu(i) {
-		remove_palinfo_proc_entries(i);
-	}
-
-	/*
-	 * Remove the top level entry finally
-	 */
-	remove_proc_entry(palinfo_dir->name, NULL);
-
-	/*
-	 * Unregister from cpu notifier callbacks
-	 */
 	unregister_hotcpu_notifier(&palinfo_cpu_notifier);
+	remove_proc_subtree("pal", NULL);
 }
 
 module_init(palinfo_init);
diff --git a/arch/ia64/kernel/perfmon.c b/arch/ia64/kernel/perfmon.c
index 2eda28414ab..9ea25fce06d 100644
--- a/arch/ia64/kernel/perfmon.c
+++ b/arch/ia64/kernel/perfmon.c
@@ -42,6 +42,7 @@
 #include <linux/completion.h>
 #include <linux/tracehook.h>
 #include <linux/slab.h>
+#include <linux/cpu.h>
 
 #include <asm/errno.h>
 #include <asm/intrinsics.h>
@@ -1322,8 +1323,6 @@ out:
 }
 EXPORT_SYMBOL(pfm_unregister_buffer_fmt);
 
-extern void update_pal_halt_status(int);
-
 static int
 pfm_reserve_session(struct task_struct *task, int is_syswide, unsigned int cpu)
 {
@@ -1371,9 +1370,9 @@ pfm_reserve_session(struct task_struct *task, int is_syswide, unsigned int cpu)
 		cpu));
 
 	/*
-	 * disable default_idle() to go to PAL_HALT
+	 * Force idle() into poll mode
 	 */
-	update_pal_halt_status(0);
+	cpu_idle_poll_ctrl(true);
 
 	UNLOCK_PFS(flags);
 
@@ -1430,11 +1429,8 @@ pfm_unreserve_session(pfm_context_t *ctx, int is_syswide, unsigned int cpu)
 		is_syswide,
 		cpu));
 
-	/*
-	 * if possible, enable default_idle() to go into PAL_HALT
-	 */
-	if (pfm_sessions.pfs_task_sessions == 0 && pfm_sessions.pfs_sys_sessions == 0)
-		update_pal_halt_status(1);
+	/* Undo forced polling. Last session reenables pal_halt */
+	cpu_idle_poll_ctrl(false);
 
 	UNLOCK_PFS(flags);
 
diff --git a/arch/ia64/kernel/process.c b/arch/ia64/kernel/process.c
index e34f565f595..a26fc640e4c 100644
--- a/arch/ia64/kernel/process.c
+++ b/arch/ia64/kernel/process.c
@@ -209,41 +209,13 @@ do_notify_resume_user(sigset_t *unused, struct sigscratch *scr, long in_syscall)
 	local_irq_disable();	/* force interrupt disable */
 }
 
-static int pal_halt        = 1;
-static int can_do_pal_halt = 1;
-
 static int __init nohalt_setup(char * str)
 {
-	pal_halt = can_do_pal_halt = 0;
+	cpu_idle_poll_ctrl(true);
 	return 1;
 }
 __setup("nohalt", nohalt_setup);
 
-void
-update_pal_halt_status(int status)
-{
-	can_do_pal_halt = pal_halt && status;
-}
-
-/*
- * We use this if we don't have any better idle routine..
- */
-void
-default_idle (void)
-{
-	local_irq_enable();
-	while (!need_resched()) {
-		if (can_do_pal_halt) {
-			local_irq_disable();
-			if (!need_resched()) {
-				safe_halt();
-			}
-			local_irq_enable();
-		} else
-			cpu_relax();
-	}
-}
-
 #ifdef CONFIG_HOTPLUG_CPU
 /* We don't actually take CPU down, just spin without interrupts. */
 static inline void play_dead(void)
@@ -270,50 +242,29 @@ static inline void play_dead(void)
 }
 #endif /* CONFIG_HOTPLUG_CPU */
 
-void __attribute__((noreturn))
-cpu_idle (void)
+void arch_cpu_idle_dead(void)
+{
+	play_dead();
+}
+
+void arch_cpu_idle(void)
 {
 	void (*mark_idle)(int) = ia64_mark_idle;
-  	int cpu = smp_processor_id();
-
-	/* endless idle loop with no priority at all */
-	while (1) {
-		rcu_idle_enter();
-		if (can_do_pal_halt) {
-			current_thread_info()->status &= ~TS_POLLING;
-			/*
-			 * TS_POLLING-cleared state must be visible before we
-			 * test NEED_RESCHED:
-			 */
-			smp_mb();
-		} else {
-			current_thread_info()->status |= TS_POLLING;
-		}
 
-		if (!need_resched()) {
-			void (*idle)(void);
 #ifdef CONFIG_SMP
-			min_xtp();
+	min_xtp();
 #endif
-			rmb();
-			if (mark_idle)
-				(*mark_idle)(1);
-
-			if (!idle)
-				idle = default_idle;
-			(*idle)();
-			if (mark_idle)
-				(*mark_idle)(0);
+	rmb();
+	if (mark_idle)
+		(*mark_idle)(1);
+
+	safe_halt();
+
+	if (mark_idle)
+		(*mark_idle)(0);
 #ifdef CONFIG_SMP
-			normal_xtp();
+	normal_xtp();
 #endif
-		}
-		rcu_idle_exit();
-		schedule_preempt_disabled();
-		check_pgt_cache();
-		if (cpu_is_offline(cpu))
-			play_dead();
-	}
 }
 
 void
diff --git a/arch/ia64/kernel/smpboot.c b/arch/ia64/kernel/smpboot.c
index 500f1e4d9f9..8d87168d218 100644
--- a/arch/ia64/kernel/smpboot.c
+++ b/arch/ia64/kernel/smpboot.c
@@ -455,7 +455,7 @@ start_secondary (void *unused)
 	preempt_disable();
 	smp_callin();
 
-	cpu_idle();
+	cpu_startup_entry(CPUHP_ONLINE);
 	return 0;
 }
 
diff --git a/arch/ia64/kvm/vtlb.c b/arch/ia64/kvm/vtlb.c
index 4332f7ee520..a7869f8f49a 100644
--- a/arch/ia64/kvm/vtlb.c
+++ b/arch/ia64/kvm/vtlb.c
@@ -256,7 +256,7 @@ u64 guest_vhpt_lookup(u64 iha, u64 *pte)
 			"srlz.d;;"
 			"ssm psr.i;;"
 			"srlz.d;;"
-			: "=r"(ret) : "r"(iha), "r"(pte):"memory");
+			: "=&r"(ret) : "r"(iha), "r"(pte) : "memory");
 
 	return ret;
 }
diff --git a/arch/ia64/mm/contig.c b/arch/ia64/mm/contig.c
index 80dab509dfb..67c59ebec89 100644
--- a/arch/ia64/mm/contig.c
+++ b/arch/ia64/mm/contig.c
@@ -47,6 +47,8 @@ void show_mem(unsigned int filter)
 	printk(KERN_INFO "Mem-info:\n");
 	show_free_areas(filter);
 	printk(KERN_INFO "Node memory in pages:\n");
+	if (filter & SHOW_MEM_FILTER_PAGE_COUNT)
+		return;
 	for_each_online_pgdat(pgdat) {
 		unsigned long present;
 		unsigned long flags;
diff --git a/arch/ia64/mm/discontig.c b/arch/ia64/mm/discontig.c
index c2e955ee79a..ae4db4bd6d9 100644
--- a/arch/ia64/mm/discontig.c
+++ b/arch/ia64/mm/discontig.c
@@ -623,6 +623,8 @@ void show_mem(unsigned int filter)
 
 	printk(KERN_INFO "Mem-info:\n");
 	show_free_areas(filter);
+	if (filter & SHOW_MEM_FILTER_PAGE_COUNT)
+		return;
 	printk(KERN_INFO "Node memory in pages:\n");
 	for_each_online_pgdat(pgdat) {
 		unsigned long present;
@@ -817,13 +819,12 @@ void arch_refresh_nodedata(int update_node, pg_data_t *update_pgdat)
 #endif
 
 #ifdef CONFIG_SPARSEMEM_VMEMMAP
-int __meminit vmemmap_populate(struct page *start_page,
-						unsigned long size, int node)
+int __meminit vmemmap_populate(unsigned long start, unsigned long end, int node)
 {
-	return vmemmap_populate_basepages(start_page, size, node);
+	return vmemmap_populate_basepages(start, end, node);
 }
 
-void vmemmap_free(struct page *memmap, unsigned long nr_pages)
+void vmemmap_free(unsigned long start, unsigned long end)
 {
 }
 #endif
diff --git a/arch/ia64/mm/init.c b/arch/ia64/mm/init.c
index 20bc967c720..d1fe4b40260 100644
--- a/arch/ia64/mm/init.c
+++ b/arch/ia64/mm/init.c
@@ -154,25 +154,14 @@ ia64_init_addr_space (void)
 void
 free_initmem (void)
 {
-	unsigned long addr, eaddr;
-
-	addr = (unsigned long) ia64_imva(__init_begin);
-	eaddr = (unsigned long) ia64_imva(__init_end);
-	while (addr < eaddr) {
-		ClearPageReserved(virt_to_page(addr));
-		init_page_count(virt_to_page(addr));
-		free_page(addr);
-		++totalram_pages;
-		addr += PAGE_SIZE;
-	}
-	printk(KERN_INFO "Freeing unused kernel memory: %ldkB freed\n",
-	       (__init_end - __init_begin) >> 10);
+	free_reserved_area((unsigned long)ia64_imva(__init_begin),
+			   (unsigned long)ia64_imva(__init_end),
+			   0, "unused kernel");
 }
 
 void __init
 free_initrd_mem (unsigned long start, unsigned long end)
 {
-	struct page *page;
 	/*
 	 * EFI uses 4KB pages while the kernel can use 4KB or bigger.
 	 * Thus EFI and the kernel may have different page sizes. It is
@@ -213,11 +202,7 @@ free_initrd_mem (unsigned long start, unsigned long end)
 	for (; start < end; start += PAGE_SIZE) {
 		if (!virt_addr_valid(start))
 			continue;
-		page = virt_to_page(start);
-		ClearPageReserved(page);
-		init_page_count(page);
-		free_page(start);
-		++totalram_pages;
+		free_reserved_page(virt_to_page(start));
 	}
 }
 
diff --git a/arch/ia64/mm/ioremap.c b/arch/ia64/mm/ioremap.c
index 3dccdd8eb27..43964cde621 100644
--- a/arch/ia64/mm/ioremap.c
+++ b/arch/ia64/mm/ioremap.c
@@ -16,7 +16,7 @@
 #include <asm/meminit.h>
 
 static inline void __iomem *
-__ioremap (unsigned long phys_addr)
+__ioremap_uc(unsigned long phys_addr)
 {
 	return (void __iomem *) (__IA64_UNCACHED_OFFSET | phys_addr);
 }
@@ -24,7 +24,11 @@ __ioremap (unsigned long phys_addr)
 void __iomem *
 early_ioremap (unsigned long phys_addr, unsigned long size)
 {
-	return __ioremap(phys_addr);
+	u64 attr;
+	attr = kern_mem_attribute(phys_addr, size);
+	if (attr & EFI_MEMORY_WB)
+		return (void __iomem *) phys_to_virt(phys_addr);
+	return __ioremap_uc(phys_addr);
 }
 
 void __iomem *
@@ -47,7 +51,7 @@ ioremap (unsigned long phys_addr, unsigned long size)
 	if (attr & EFI_MEMORY_WB)
 		return (void __iomem *) phys_to_virt(phys_addr);
 	else if (attr & EFI_MEMORY_UC)
-		return __ioremap(phys_addr);
+		return __ioremap_uc(phys_addr);
 
 	/*
 	 * Some chipsets don't support UC access to memory.  If
@@ -93,7 +97,7 @@ ioremap (unsigned long phys_addr, unsigned long size)
 		return (void __iomem *) (offset + (char __iomem *)addr);
 	}
 
-	return __ioremap(phys_addr);
+	return __ioremap_uc(phys_addr);
 }
 EXPORT_SYMBOL(ioremap);
 
@@ -103,7 +107,7 @@ ioremap_nocache (unsigned long phys_addr, unsigned long size)
 	if (kern_mem_attribute(phys_addr, size) & EFI_MEMORY_WB)
 		return NULL;
 
-	return __ioremap(phys_addr);
+	return __ioremap_uc(phys_addr);
 }
 EXPORT_SYMBOL(ioremap_nocache);
 
diff --git a/arch/ia64/mm/numa.c b/arch/ia64/mm/numa.c
index 3efea7d0a35..4248492b932 100644
--- a/arch/ia64/mm/numa.c
+++ b/arch/ia64/mm/numa.c
@@ -61,18 +61,36 @@ paddr_to_nid(unsigned long paddr)
 int __meminit __early_pfn_to_nid(unsigned long pfn)
 {
 	int i, section = pfn >> PFN_SECTION_SHIFT, ssec, esec;
+	/*
+	 * NOTE: The following SMP-unsafe globals are only used early in boot
+	 * when the kernel is running single-threaded.
+	 */
+	static int __meminitdata last_ssec, last_esec;
+	static int __meminitdata last_nid;
+
+	if (section >= last_ssec && section < last_esec)
+		return last_nid;
 
 	for (i = 0; i < num_node_memblks; i++) {
 		ssec = node_memblk[i].start_paddr >> PA_SECTION_SHIFT;
 		esec = (node_memblk[i].start_paddr + node_memblk[i].size +
 			((1L << PA_SECTION_SHIFT) - 1)) >> PA_SECTION_SHIFT;
-		if (section >= ssec && section < esec)
+		if (section >= ssec && section < esec) {
+			last_ssec = ssec;
+			last_esec = esec;
+			last_nid = node_memblk[i].nid;
 			return node_memblk[i].nid;
+		}
 	}
 
 	return -1;
 }
 
+void __cpuinit numa_clear_node(int cpu)
+{
+	unmap_cpu_from_node(cpu, NUMA_NO_NODE);
+}
+
 #ifdef CONFIG_MEMORY_HOTPLUG
 /*
  *  SRAT information is stored in node_memblk[], then we can use SRAT
diff --git a/arch/ia64/pci/pci.c b/arch/ia64/pci/pci.c
index 60532ab2734..de1474ff0bc 100644
--- a/arch/ia64/pci/pci.c
+++ b/arch/ia64/pci/pci.c
@@ -15,6 +15,7 @@
 #include <linux/types.h>
 #include <linux/kernel.h>
 #include <linux/pci.h>
+#include <linux/pci-acpi.h>
 #include <linux/init.h>
 #include <linux/ioport.h>
 #include <linux/slab.h>
@@ -458,6 +459,16 @@ void pcibios_fixup_bus(struct pci_bus *b)
 	platform_pci_fixup_bus(b);
 }
 
+void pcibios_add_bus(struct pci_bus *bus)
+{
+	acpi_pci_add_bus(bus);
+}
+
+void pcibios_remove_bus(struct pci_bus *bus)
+{
+	acpi_pci_remove_bus(bus);
+}
+
 void pcibios_set_master (struct pci_dev *dev)
 {
 	/* No special bus mastering setup handling */
diff --git a/arch/ia64/sn/kernel/tiocx.c b/arch/ia64/sn/kernel/tiocx.c
index 14c1711238c..e35f6485c1f 100644
--- a/arch/ia64/sn/kernel/tiocx.c
+++ b/arch/ia64/sn/kernel/tiocx.c
@@ -490,11 +490,14 @@ static int __init tiocx_init(void)
 {
 	cnodeid_t cnodeid;
 	int found_tiocx_device = 0;
+	int err;
 
 	if (!ia64_platform_is("sn2"))
 		return 0;
 
-	bus_register(&tiocx_bus_type);
+	err = bus_register(&tiocx_bus_type);
+	if (err)
+		return err;
 
 	for (cnodeid = 0; cnodeid < num_cnodes; cnodeid++) {
 		nasid_t nasid;
diff --git a/arch/m32r/kernel/process.c b/arch/m32r/kernel/process.c
index bde899e155d..e2d049018c3 100644
--- a/arch/m32r/kernel/process.c
+++ b/arch/m32r/kernel/process.c
@@ -47,24 +47,6 @@ unsigned long thread_saved_pc(struct task_struct *tsk)
 void (*pm_power_off)(void) = NULL;
 EXPORT_SYMBOL(pm_power_off);
 
-/*
- * The idle thread. There's no useful work to be
- * done, so just try to conserve power and have a
- * low exit latency (ie sit in a loop waiting for
- * somebody to say that they'd like to reschedule)
- */
-void cpu_idle (void)
-{
-	/* endless idle loop with no priority at all */
-	while (1) {
-		rcu_idle_enter();
-		while (!need_resched())
-			cpu_relax();
-		rcu_idle_exit();
-		schedule_preempt_disabled();
-	}
-}
-
 void machine_restart(char *__unused)
 {
 #if defined(CONFIG_PLAT_MAPPI3)
diff --git a/arch/m32r/kernel/smpboot.c b/arch/m32r/kernel/smpboot.c
index 13168a769f8..0ac558adc60 100644
--- a/arch/m32r/kernel/smpboot.c
+++ b/arch/m32r/kernel/smpboot.c
@@ -432,7 +432,7 @@ int __init start_secondary(void *unused)
 	 */
 	local_flush_tlb_all();
 
-	cpu_idle();
+	cpu_startup_entry(CPUHP_ONLINE);
 	return 0;
 }
 
diff --git a/arch/m32r/kernel/traps.c b/arch/m32r/kernel/traps.c
index 3bcb207e5b6..9fe3467a513 100644
--- a/arch/m32r/kernel/traps.c
+++ b/arch/m32r/kernel/traps.c
@@ -132,10 +132,8 @@ static void show_trace(struct task_struct *task, unsigned long *stack)
 	printk("Call Trace: ");
 	while (!kstack_end(stack)) {
 		addr = *stack++;
-		if (__kernel_text_address(addr)) {
-			printk("[<%08lx>] ", addr);
-			print_symbol("%s\n", addr);
-		}
+		if (__kernel_text_address(addr))
+			printk("[<%08lx>] %pSR\n", addr, (void *)addr);
 	}
 	printk("\n");
 }
diff --git a/arch/m32r/mm/init.c b/arch/m32r/mm/init.c
index 78b660e903d..ab4cbce91a9 100644
--- a/arch/m32r/mm/init.c
+++ b/arch/m32r/mm/init.c
@@ -28,10 +28,7 @@
 #include <asm/mmu_context.h>
 #include <asm/setup.h>
 #include <asm/tlb.h>
-
-/* References to section boundaries */
-extern char _text, _etext, _edata;
-extern char __init_begin, __init_end;
+#include <asm/sections.h>
 
 pgd_t swapper_pg_dir[1024];
 
@@ -184,17 +181,7 @@ void __init mem_init(void)
  *======================================================================*/
 void free_initmem(void)
 {
-	unsigned long addr;
-
-	addr = (unsigned long)(&__init_begin);
-	for (; addr < (unsigned long)(&__init_end); addr += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(addr));
-		init_page_count(virt_to_page(addr));
-		free_page(addr);
-		totalram_pages++;
-	}
-	printk (KERN_INFO "Freeing unused kernel memory: %dk freed\n", \
-	  (int)(&__init_end - &__init_begin) >> 10);
+	free_initmem_default(0);
 }
 
 #ifdef CONFIG_BLK_DEV_INITRD
@@ -204,13 +191,6 @@ void free_initmem(void)
  *======================================================================*/
 void free_initrd_mem(unsigned long start, unsigned long end)
 {
-	unsigned long p;
-	for (p = start; p < end; p += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(p));
-		init_page_count(virt_to_page(p));
-		free_page(p);
-		totalram_pages++;
-	}
-	printk (KERN_INFO "Freeing initrd memory: %ldk freed\n", (end - start) >> 10);
+	free_reserved_area(start, end, 0, "initrd");
 }
 #endif
diff --git a/arch/m68k/Kconfig.bus b/arch/m68k/Kconfig.bus
index 93ef0346b20..675b087198f 100644
--- a/arch/m68k/Kconfig.bus
+++ b/arch/m68k/Kconfig.bus
@@ -45,6 +45,16 @@ config ISA
 	  (MCA) or VESA.  ISA is an older system, now being displaced by PCI;
 	  newer boards don't support it.  If you have ISA, say Y, otherwise N.
 
+config ATARI_ROM_ISA
+	bool "Atari ROM port ISA adapter support"
+	depends on ATARI
+	help
+	  This option enables support for the ROM port ISA adapter used to
+	  operate ISA cards on Atari. Only 8  bit cards are supported, and
+	  no interrupt lines are connected.
+	  The only driver currently using this adapter is the EtherNEC
+	  driver for RTL8019AS based NE2000 compatible network cards.
+
 config GENERIC_ISA_DMA
 	def_bool ISA
 
diff --git a/arch/m68k/Kconfig.devices b/arch/m68k/Kconfig.devices
index 4bc945dfe46..d163991c571 100644
--- a/arch/m68k/Kconfig.devices
+++ b/arch/m68k/Kconfig.devices
@@ -55,6 +55,30 @@ config NFETH
 	  which will emulate a regular ethernet device while presenting an
 	  ethertap device to the host system.
 
+config ATARI_ETHERNAT
+	bool "Atari EtherNAT Ethernet support"
+	depends on ATARI
+	---help---
+	  Say Y to include support for the EtherNAT network adapter for the
+	  CT/60 extension port.
+
+	  To compile the actual ethernet driver, choose Y or M for the SMC91X
+	  option in the network device section; the module will be called smc91x.
+
+config ATARI_ETHERNEC
+	bool "Atari EtherNEC Ethernet support"
+	depends on ATARI_ROM_ISA
+	---help---
+	  Say Y to include support for the EtherNEC network adapter for the
+	  ROM port. The driver works by polling instead of interrupts, so it
+	  is quite slow.
+
+	  This driver also suppports the ethernet part of the NetUSBee ROM
+	  port combined Ethernet/USB adapter.
+
+	  To compile the actual ethernet driver, choose Y or M in for the NE2000
+	  option in the network device section; the module will be called ne.
+
 endmenu
 
 menu "Character devices"
diff --git a/arch/m68k/atari/ataints.c b/arch/m68k/atari/ataints.c
index 3f41092d1b7..20cde4e9fc7 100644
--- a/arch/m68k/atari/ataints.c
+++ b/arch/m68k/atari/ataints.c
@@ -49,6 +49,7 @@
 #include <asm/atari_stdma.h>
 #include <asm/irq.h>
 #include <asm/entry.h>
+#include <asm/io.h>
 
 
 /*
@@ -122,6 +123,136 @@ static struct irq_chip atari_irq_chip = {
 };
 
 /*
+ * ST-MFP timer D chained interrupts - each driver gets its own timer
+ * interrupt instance.
+ */
+
+struct mfptimerbase {
+	volatile struct MFP *mfp;
+	unsigned char mfp_mask, mfp_data;
+	unsigned short int_mask;
+	int handler_irq, mfptimer_irq, server_irq;
+	char *name;
+} stmfp_base = {
+	.mfp		= &st_mfp,
+	.int_mask	= 0x0,
+	.handler_irq	= IRQ_MFP_TIMD,
+	.mfptimer_irq	= IRQ_MFP_TIMER1,
+	.name		= "MFP Timer D"
+};
+
+static irqreturn_t mfptimer_handler(int irq, void *dev_id)
+{
+	struct mfptimerbase *base = dev_id;
+	int mach_irq;
+	unsigned char ints;
+
+	mach_irq = base->mfptimer_irq;
+	ints = base->int_mask;
+	for (; ints; mach_irq++, ints >>= 1) {
+		if (ints & 1)
+			generic_handle_irq(mach_irq);
+	}
+	return IRQ_HANDLED;
+}
+
+
+static void atari_mfptimer_enable(struct irq_data *data)
+{
+	int mfp_num = data->irq - IRQ_MFP_TIMER1;
+	stmfp_base.int_mask |= 1 << mfp_num;
+	atari_enable_irq(IRQ_MFP_TIMD);
+}
+
+static void atari_mfptimer_disable(struct irq_data *data)
+{
+	int mfp_num = data->irq - IRQ_MFP_TIMER1;
+	stmfp_base.int_mask &= ~(1 << mfp_num);
+	if (!stmfp_base.int_mask)
+		atari_disable_irq(IRQ_MFP_TIMD);
+}
+
+static struct irq_chip atari_mfptimer_chip = {
+	.name		= "timer_d",
+	.irq_enable	= atari_mfptimer_enable,
+	.irq_disable	= atari_mfptimer_disable,
+};
+
+
+/*
+ * EtherNAT CPLD interrupt handling
+ * CPLD interrupt register is at phys. 0x80000023
+ * Need this mapped in at interrupt startup time
+ * Possibly need this mapped on demand anyway -
+ * EtherNAT USB driver needs to disable IRQ before
+ * startup!
+ */
+
+static unsigned char *enat_cpld;
+
+static unsigned int atari_ethernat_startup(struct irq_data *data)
+{
+	int enat_num = 140 - data->irq + 1;
+
+	m68k_irq_startup(data);
+	/*
+	* map CPLD interrupt register
+	*/
+	if (!enat_cpld)
+		enat_cpld = (unsigned char *)ioremap((ATARI_ETHERNAT_PHYS_ADDR+0x23), 0x2);
+	/*
+	 * do _not_ enable the USB chip interrupt here - causes interrupt storm
+	 * and triggers dead interrupt watchdog
+	 * Need to reset the USB chip to a sane state in early startup before
+	 * removing this hack
+	 */
+	if (enat_num == 1)
+		*enat_cpld |= 1 << enat_num;
+
+	return 0;
+}
+
+static void atari_ethernat_enable(struct irq_data *data)
+{
+	int enat_num = 140 - data->irq + 1;
+	/*
+	* map CPLD interrupt register
+	*/
+	if (!enat_cpld)
+		enat_cpld = (unsigned char *)ioremap((ATARI_ETHERNAT_PHYS_ADDR+0x23), 0x2);
+	*enat_cpld |= 1 << enat_num;
+}
+
+static void atari_ethernat_disable(struct irq_data *data)
+{
+	int enat_num = 140 - data->irq + 1;
+	/*
+	* map CPLD interrupt register
+	*/
+	if (!enat_cpld)
+		enat_cpld = (unsigned char *)ioremap((ATARI_ETHERNAT_PHYS_ADDR+0x23), 0x2);
+	*enat_cpld &= ~(1 << enat_num);
+}
+
+static void atari_ethernat_shutdown(struct irq_data *data)
+{
+	int enat_num = 140 - data->irq + 1;
+	if (enat_cpld) {
+		*enat_cpld &= ~(1 << enat_num);
+		iounmap(enat_cpld);
+		enat_cpld = NULL;
+	}
+}
+
+static struct irq_chip atari_ethernat_chip = {
+	.name		= "ethernat",
+	.irq_startup	= atari_ethernat_startup,
+	.irq_shutdown	= atari_ethernat_shutdown,
+	.irq_enable	= atari_ethernat_enable,
+	.irq_disable	= atari_ethernat_disable,
+};
+
+/*
  * void atari_init_IRQ (void)
  *
  * Parameters:	None
@@ -198,6 +329,27 @@ void __init atari_init_IRQ(void)
 	/* Initialize the PSG: all sounds off, both ports output */
 	sound_ym.rd_data_reg_sel = 7;
 	sound_ym.wd_data = 0xff;
+
+	m68k_setup_irq_controller(&atari_mfptimer_chip, handle_simple_irq,
+				  IRQ_MFP_TIMER1, 8);
+
+	/* prepare timer D data for use as poll interrupt */
+	/* set Timer D data Register - needs to be > 0 */
+	st_mfp.tim_dt_d = 254;	/* < 100 Hz */
+	/* start timer D, div = 1:100 */
+	st_mfp.tim_ct_cd = (st_mfp.tim_ct_cd & 0xf0) | 0x6;
+
+	/* request timer D dispatch handler */
+	if (request_irq(IRQ_MFP_TIMD, mfptimer_handler, IRQF_SHARED,
+			stmfp_base.name, &stmfp_base))
+		pr_err("Couldn't register %s interrupt\n", stmfp_base.name);
+
+	/*
+	 * EtherNAT ethernet / USB interrupt handlers
+	 */
+
+	m68k_setup_irq_controller(&atari_ethernat_chip, handle_simple_irq,
+				  139, 2);
 }
 
 
diff --git a/arch/m68k/atari/config.c b/arch/m68k/atari/config.c
index 037c11c9933..fb2d0bd9b3a 100644
--- a/arch/m68k/atari/config.c
+++ b/arch/m68k/atari/config.c
@@ -31,6 +31,8 @@
 #include <linux/init.h>
 #include <linux/delay.h>
 #include <linux/ioport.h>
+#include <linux/platform_device.h>
+#include <linux/usb/isp116x.h>
 #include <linux/vt_kern.h>
 #include <linux/module.h>
 
@@ -655,3 +657,240 @@ static void atari_get_hardware_list(struct seq_file *m)
 	ATARIHW_ANNOUNCE(VME, "VME Bus");
 	ATARIHW_ANNOUNCE(DSP56K, "DSP56001 processor");
 }
+
+/*
+ * MSch: initial platform device support for Atari,
+ * required for EtherNAT/EtherNEC/NetUSBee drivers
+ */
+
+#if defined(CONFIG_ATARI_ETHERNAT) || defined(CONFIG_ATARI_ETHERNEC)
+static void isp1160_delay(struct device *dev, int delay)
+{
+	ndelay(delay);
+}
+#endif
+
+#ifdef CONFIG_ATARI_ETHERNAT
+/*
+ * EtherNAT: SMC91C111 Ethernet chipset, handled by smc91x driver
+ */
+
+#define ATARI_ETHERNAT_IRQ		140
+
+static struct resource smc91x_resources[] = {
+	[0] = {
+		.name	= "smc91x-regs",
+		.start	= ATARI_ETHERNAT_PHYS_ADDR,
+		.end	= ATARI_ETHERNAT_PHYS_ADDR + 0xfffff,
+		.flags	= IORESOURCE_MEM,
+	},
+	[1] = {
+		.name	= "smc91x-irq",
+		.start	= ATARI_ETHERNAT_IRQ,
+		.end	= ATARI_ETHERNAT_IRQ,
+		.flags	= IORESOURCE_IRQ,
+	},
+};
+
+static struct platform_device smc91x_device = {
+	.name		= "smc91x",
+	.id		= -1,
+	.num_resources	= ARRAY_SIZE(smc91x_resources),
+	.resource	= smc91x_resources,
+};
+
+/*
+ * ISP 1160 - using the isp116x-hcd module
+ */
+
+#define ATARI_USB_PHYS_ADDR	0x80000012
+#define ATARI_USB_IRQ		139
+
+static struct resource isp1160_resources[] = {
+	[0] = {
+		.name	= "isp1160-data",
+		.start	= ATARI_USB_PHYS_ADDR,
+		.end	= ATARI_USB_PHYS_ADDR + 0x1,
+		.flags	= IORESOURCE_MEM,
+	},
+	[1] = {
+		.name	= "isp1160-regs",
+		.start	= ATARI_USB_PHYS_ADDR + 0x4,
+		.end	= ATARI_USB_PHYS_ADDR + 0x5,
+		.flags	= IORESOURCE_MEM,
+	},
+	[2] = {
+		.name	= "isp1160-irq",
+		.start	= ATARI_USB_IRQ,
+		.end	= ATARI_USB_IRQ,
+		.flags	= IORESOURCE_IRQ,
+	},
+};
+
+/* (DataBusWidth16|AnalogOCEnable|DREQOutputPolarity|DownstreamPort15KRSel ) */
+static struct isp116x_platform_data isp1160_platform_data = {
+	/* Enable internal resistors on downstream ports */
+	.sel15Kres		= 1,
+	/* On-chip overcurrent protection */
+	.oc_enable		= 1,
+	/* INT output polarity */
+	.int_act_high		= 1,
+	/* INT edge or level triggered */
+	.int_edge_triggered	= 0,
+
+	/* WAKEUP pin connected - NOT SUPPORTED  */
+	/* .remote_wakeup_connected = 0, */
+	/* Wakeup by devices on usb bus enabled */
+	.remote_wakeup_enable	= 0,
+	.delay			= isp1160_delay,
+};
+
+static struct platform_device isp1160_device = {
+	.name		= "isp116x-hcd",
+	.id		= 0,
+	.num_resources	= ARRAY_SIZE(isp1160_resources),
+	.resource	= isp1160_resources,
+	.dev			= {
+		.platform_data	= &isp1160_platform_data,
+	},
+};
+
+static struct platform_device *atari_ethernat_devices[] __initdata = {
+	&smc91x_device,
+	&isp1160_device
+};
+#endif /* CONFIG_ATARI_ETHERNAT */
+
+#ifdef CONFIG_ATARI_ETHERNEC
+/*
+ * EtherNEC: RTL8019 (NE2000 compatible) Ethernet chipset,
+ * handled by ne.c driver
+ */
+
+#define ATARI_ETHERNEC_PHYS_ADDR	0xfffa0000
+#define ATARI_ETHERNEC_BASE		0x300
+#define ATARI_ETHERNEC_IRQ		IRQ_MFP_TIMER1
+
+static struct resource rtl8019_resources[] = {
+	[0] = {
+		.name	= "rtl8019-regs",
+		.start	= ATARI_ETHERNEC_BASE,
+		.end	= ATARI_ETHERNEC_BASE + 0x20 - 1,
+		.flags	= IORESOURCE_IO,
+	},
+	[1] = {
+		.name	= "rtl8019-irq",
+		.start	= ATARI_ETHERNEC_IRQ,
+		.end	= ATARI_ETHERNEC_IRQ,
+		.flags	= IORESOURCE_IRQ,
+	},
+};
+
+static struct platform_device rtl8019_device = {
+	.name		= "ne",
+	.id		= -1,
+	.num_resources	= ARRAY_SIZE(rtl8019_resources),
+	.resource	= rtl8019_resources,
+};
+
+/*
+ * NetUSBee: ISP1160 USB host adapter via ROM-port adapter
+ */
+
+#define ATARI_NETUSBEE_PHYS_ADDR	0xfffa8000
+#define ATARI_NETUSBEE_BASE		0x340
+#define ATARI_NETUSBEE_IRQ		IRQ_MFP_TIMER2
+
+static struct resource netusbee_resources[] = {
+	[0] = {
+		.name	= "isp1160-data",
+		.start	= ATARI_NETUSBEE_BASE,
+		.end	= ATARI_NETUSBEE_BASE + 0x1,
+		.flags	= IORESOURCE_MEM,
+	},
+	[1] = {
+		.name	= "isp1160-regs",
+		.start	= ATARI_NETUSBEE_BASE + 0x20,
+		.end	= ATARI_NETUSBEE_BASE + 0x21,
+		.flags	= IORESOURCE_MEM,
+	},
+	[2] = {
+		.name	= "isp1160-irq",
+		.start	= ATARI_NETUSBEE_IRQ,
+		.end	= ATARI_NETUSBEE_IRQ,
+		.flags	= IORESOURCE_IRQ,
+	},
+};
+
+/* (DataBusWidth16|AnalogOCEnable|DREQOutputPolarity|DownstreamPort15KRSel ) */
+static struct isp116x_platform_data netusbee_platform_data = {
+	/* Enable internal resistors on downstream ports */
+	.sel15Kres		= 1,
+	/* On-chip overcurrent protection */
+	.oc_enable		= 1,
+	/* INT output polarity */
+	.int_act_high		= 1,
+	/* INT edge or level triggered */
+	.int_edge_triggered	= 0,
+
+	/* WAKEUP pin connected - NOT SUPPORTED  */
+	/* .remote_wakeup_connected = 0, */
+	/* Wakeup by devices on usb bus enabled */
+	.remote_wakeup_enable	= 0,
+	.delay			= isp1160_delay,
+};
+
+static struct platform_device netusbee_device = {
+	.name		= "isp116x-hcd",
+	.id		= 1,
+	.num_resources	= ARRAY_SIZE(netusbee_resources),
+	.resource	= netusbee_resources,
+	.dev			= {
+		.platform_data	= &netusbee_platform_data,
+	},
+};
+
+static struct platform_device *atari_netusbee_devices[] __initdata = {
+	&rtl8019_device,
+	&netusbee_device
+};
+#endif /* CONFIG_ATARI_ETHERNEC */
+
+int __init atari_platform_init(void)
+{
+	int rv = 0;
+
+	if (!MACH_IS_ATARI)
+		return -ENODEV;
+
+#ifdef CONFIG_ATARI_ETHERNAT
+	{
+		unsigned char *enatc_virt;
+		enatc_virt = (unsigned char *)ioremap((ATARI_ETHERNAT_PHYS_ADDR+0x23), 0xf);
+		if (hwreg_present(enatc_virt)) {
+			rv = platform_add_devices(atari_ethernat_devices,
+						ARRAY_SIZE(atari_ethernat_devices));
+		}
+		iounmap(enatc_virt);
+	}
+#endif
+
+#ifdef CONFIG_ATARI_ETHERNEC
+	{
+		int error;
+		unsigned char *enec_virt;
+		enec_virt = (unsigned char *)ioremap((ATARI_ETHERNEC_PHYS_ADDR), 0xf);
+		if (hwreg_present(enec_virt)) {
+			error = platform_add_devices(atari_netusbee_devices,
+						ARRAY_SIZE(atari_netusbee_devices));
+			if (error && !rv)
+				rv = error;
+		}
+		iounmap(enec_virt);
+	}
+#endif
+
+	return rv;
+}
+
+arch_initcall(atari_platform_init);
diff --git a/arch/m68k/include/asm/atarihw.h b/arch/m68k/include/asm/atarihw.h
index c0cb3635077..d887050e6da 100644
--- a/arch/m68k/include/asm/atarihw.h
+++ b/arch/m68k/include/asm/atarihw.h
@@ -805,5 +805,11 @@ struct MSTE_RTC {
 
 #define mste_rtc ((*(volatile struct MSTE_RTC *)MSTE_RTC_BAS))
 
+/*
+** EtherNAT add-on card for Falcon - combined ethernet and USB adapter
+*/
+
+#define ATARI_ETHERNAT_PHYS_ADDR	0x80000000
+
 #endif /* linux/atarihw.h */
 
diff --git a/arch/m68k/include/asm/atariints.h b/arch/m68k/include/asm/atariints.h
index 5fc13bdf904..953e0ac6855 100644
--- a/arch/m68k/include/asm/atariints.h
+++ b/arch/m68k/include/asm/atariints.h
@@ -32,7 +32,7 @@
 #define VME_SOURCE_BASE    56
 #define VME_MAX_SOURCES    16
 
-#define NUM_ATARI_SOURCES   (VME_SOURCE_BASE+VME_MAX_SOURCES-STMFP_SOURCE_BASE)
+#define NUM_ATARI_SOURCES  141
 
 /* convert vector number to int source number */
 #define IRQ_VECTOR_TO_SOURCE(v)	((v) - ((v) < 0x20 ? 0x18 : (0x40-8)))
@@ -94,6 +94,15 @@
 #define IRQ_SCCA_RX	     (52)
 #define IRQ_SCCA_SPCOND	     (54)
 
+/* shared MFP timer D interrupts - hires timer for EtherNEC et al. */
+#define IRQ_MFP_TIMER1       (64)
+#define IRQ_MFP_TIMER2       (65)
+#define IRQ_MFP_TIMER3       (66)
+#define IRQ_MFP_TIMER4       (67)
+#define IRQ_MFP_TIMER5       (68)
+#define IRQ_MFP_TIMER6       (69)
+#define IRQ_MFP_TIMER7       (70)
+#define IRQ_MFP_TIMER8       (71)
 
 #define INT_CLK   24576	    /* CLK while int_clk =2.456MHz and divide = 100 */
 #define INT_TICKS 246	    /* to make sched_time = 99.902... HZ */
diff --git a/arch/m68k/include/asm/cmpxchg.h b/arch/m68k/include/asm/cmpxchg.h
index 5c81d0eae5c..bc755bc620a 100644
--- a/arch/m68k/include/asm/cmpxchg.h
+++ b/arch/m68k/include/asm/cmpxchg.h
@@ -124,6 +124,9 @@ static inline unsigned long __cmpxchg(volatile void *p, unsigned long old,
 #define cmpxchg_local(ptr, o, n)					    \
 	((__typeof__(*(ptr)))__cmpxchg((ptr), (unsigned long)(o),	    \
 			(unsigned long)(n), sizeof(*(ptr))))
+
+#define cmpxchg64(ptr, o, n)	cmpxchg64_local((ptr), (o), (n))
+
 #else
 
 /*
diff --git a/arch/m68k/include/asm/delay.h b/arch/m68k/include/asm/delay.h
index 12d8fe4f1d3..d28fa8fe26f 100644
--- a/arch/m68k/include/asm/delay.h
+++ b/arch/m68k/include/asm/delay.h
@@ -92,5 +92,28 @@ static inline void __udelay(unsigned long usecs)
 #define udelay(n) (__builtin_constant_p(n) ? \
 	((n) > 20000 ? __bad_udelay() : __const_udelay(n)) : __udelay(n))
 
+/*
+ * nanosecond delay:
+ *
+ * ((((HZSCALE) >> 11) * (loops_per_jiffy >> 11)) >> 6) is the number of loops
+ * per microsecond
+ *
+ * 1000 / ((((HZSCALE) >> 11) * (loops_per_jiffy >> 11)) >> 6) is the number of
+ * nanoseconds per loop
+ *
+ * So n / ( 1000 / ((((HZSCALE) >> 11) * (loops_per_jiffy >> 11)) >> 6) ) would
+ * be the number of loops for n nanoseconds
+ */
+
+/*
+ * The simpler m68k and ColdFire processors do not have a 32*32->64
+ * multiply instruction. So we need to handle them a little differently.
+ * We use a bit of shifting and a single 32*32->32 multiply to get close.
+ * This is a macro so that the const version can factor out the first
+ * multiply and shift.
+ */
+#define	HZSCALE		(268435456 / (1000000 / HZ))
+
+#define ndelay(n) __delay(DIV_ROUND_UP((n) * ((((HZSCALE) >> 11) * (loops_per_jiffy >> 11)) >> 6), 1000));
 
 #endif /* defined(_M68K_DELAY_H) */
diff --git a/arch/m68k/include/asm/gpio.h b/arch/m68k/include/asm/gpio.h
index 4395ffc51fd..8cc83431805 100644
--- a/arch/m68k/include/asm/gpio.h
+++ b/arch/m68k/include/asm/gpio.h
@@ -86,4 +86,24 @@ static inline int gpio_cansleep(unsigned gpio)
 	return gpio < MCFGPIO_PIN_MAX ? 0 : __gpio_cansleep(gpio);
 }
 
+static inline int gpio_request_one(unsigned gpio, unsigned long flags, const char *label)
+{
+	int err;
+
+	err = gpio_request(gpio, label);
+	if (err)
+		return err;
+
+	if (flags & GPIOF_DIR_IN)
+		err = gpio_direction_input(gpio);
+	else
+		err = gpio_direction_output(gpio,
+			(flags & GPIOF_INIT_HIGH) ? 1 : 0);
+
+	if (err)
+		gpio_free(gpio);
+
+	return err;
+}
+
 #endif
diff --git a/arch/m68k/include/asm/io_mm.h b/arch/m68k/include/asm/io_mm.h
index a6686d26fe1..ffdf54f44bc 100644
--- a/arch/m68k/include/asm/io_mm.h
+++ b/arch/m68k/include/asm/io_mm.h
@@ -63,6 +63,23 @@
 #endif
 #endif /* AMIGA_PCMCIA */
 
+#ifdef CONFIG_ATARI_ROM_ISA
+
+#define enec_isa_read_base  0xfffa0000
+#define enec_isa_write_base 0xfffb0000
+
+#define ENEC_ISA_IO_B(ioaddr)	(enec_isa_read_base+((((unsigned long)(ioaddr))&0x7F)<<9))
+#define ENEC_ISA_IO_W(ioaddr)	(enec_isa_read_base+((((unsigned long)(ioaddr))&0x7F)<<9))
+#define ENEC_ISA_MEM_B(madr)	(enec_isa_read_base+((((unsigned long)(madr))&0x7F)<<9))
+#define ENEC_ISA_MEM_W(madr)	(enec_isa_read_base+((((unsigned long)(madr))&0x7F)<<9))
+
+#ifndef MULTI_ISA
+#define MULTI_ISA 0
+#else
+#undef MULTI_ISA
+#define MULTI_ISA 1
+#endif
+#endif /* ATARI_ROM_ISA */
 
 
 #if defined(CONFIG_PCI) && defined(CONFIG_COLDFIRE)
@@ -111,14 +128,15 @@ void mcf_pci_outsl(u32 addr, const u32 *buf, u32 len);
 #define readw(addr)	in_le16(addr)
 #define writew(v, addr)	out_le16((addr), (v))
 
-#elif defined(CONFIG_ISA)
+#elif defined(CONFIG_ISA) || defined(CONFIG_ATARI_ROM_ISA)
 
 #if MULTI_ISA == 0
 #undef MULTI_ISA
 #endif
 
-#define ISA_TYPE_Q40 (1)
-#define ISA_TYPE_AG  (2)
+#define ISA_TYPE_Q40  (1)
+#define ISA_TYPE_AG   (2)
+#define ISA_TYPE_ENEC (3)
 
 #if defined(CONFIG_Q40) && !defined(MULTI_ISA)
 #define ISA_TYPE ISA_TYPE_Q40
@@ -128,6 +146,10 @@ void mcf_pci_outsl(u32 addr, const u32 *buf, u32 len);
 #define ISA_TYPE ISA_TYPE_AG
 #define ISA_SEX  1
 #endif
+#if defined(CONFIG_ATARI_ROM_ISA) && !defined(MULTI_ISA)
+#define ISA_TYPE ISA_TYPE_ENEC
+#define ISA_SEX  0
+#endif
 
 #ifdef MULTI_ISA
 extern int isa_type;
@@ -152,6 +174,9 @@ static inline u8 __iomem *isa_itb(unsigned long addr)
 #ifdef CONFIG_AMIGA_PCMCIA
     case ISA_TYPE_AG: return (u8 __iomem *)AG_ISA_IO_B(addr);
 #endif
+#ifdef CONFIG_ATARI_ROM_ISA
+    case ISA_TYPE_ENEC: return (u8 __iomem *)ENEC_ISA_IO_B(addr);
+#endif
     default: return NULL; /* avoid warnings, just in case */
     }
 }
@@ -165,6 +190,9 @@ static inline u16 __iomem *isa_itw(unsigned long addr)
 #ifdef CONFIG_AMIGA_PCMCIA
     case ISA_TYPE_AG: return (u16 __iomem *)AG_ISA_IO_W(addr);
 #endif
+#ifdef CONFIG_ATARI_ROM_ISA
+    case ISA_TYPE_ENEC: return (u16 __iomem *)ENEC_ISA_IO_W(addr);
+#endif
     default: return NULL; /* avoid warnings, just in case */
     }
 }
@@ -188,6 +216,9 @@ static inline u8 __iomem *isa_mtb(unsigned long addr)
 #ifdef CONFIG_AMIGA_PCMCIA
     case ISA_TYPE_AG: return (u8 __iomem *)addr;
 #endif
+#ifdef CONFIG_ATARI_ROM_ISA
+    case ISA_TYPE_ENEC: return (u8 __iomem *)ENEC_ISA_MEM_B(addr);
+#endif
     default: return NULL; /* avoid warnings, just in case */
     }
 }
@@ -201,6 +232,9 @@ static inline u16 __iomem *isa_mtw(unsigned long addr)
 #ifdef CONFIG_AMIGA_PCMCIA
     case ISA_TYPE_AG: return (u16 __iomem *)addr;
 #endif
+#ifdef CONFIG_ATARI_ROM_ISA
+    case ISA_TYPE_ENEC: return (u16 __iomem *)ENEC_ISA_MEM_W(addr);
+#endif
     default: return NULL; /* avoid warnings, just in case */
     }
 }
@@ -222,6 +256,36 @@ static inline u16 __iomem *isa_mtw(unsigned long addr)
 	(ISA_SEX ? out_be16(isa_mtw((unsigned long)(p)),(val))	\
 		 : out_le16(isa_mtw((unsigned long)(p)),(val)))
 
+#ifdef CONFIG_ATARI_ROM_ISA
+#define isa_rom_inb(port)      rom_in_8(isa_itb(port))
+#define isa_rom_inw(port)	\
+	(ISA_SEX ? rom_in_be16(isa_itw(port))	\
+		 : rom_in_le16(isa_itw(port)))
+
+#define isa_rom_outb(val, port) rom_out_8(isa_itb(port), (val))
+#define isa_rom_outw(val, port)	\
+	(ISA_SEX ? rom_out_be16(isa_itw(port), (val))	\
+		 : rom_out_le16(isa_itw(port), (val)))
+
+#define isa_rom_readb(p)       rom_in_8(isa_mtb((unsigned long)(p)))
+#define isa_rom_readw(p)       \
+	(ISA_SEX ? rom_in_be16(isa_mtw((unsigned long)(p)))	\
+		 : rom_in_le16(isa_mtw((unsigned long)(p))))
+#define isa_rom_readw_swap(p)       \
+	(ISA_SEX ? rom_in_le16(isa_mtw((unsigned long)(p)))	\
+		 : rom_in_be16(isa_mtw((unsigned long)(p))))
+#define isa_rom_readw_raw(p)   rom_in_be16(isa_mtw((unsigned long)(p)))
+
+#define isa_rom_writeb(val, p)  rom_out_8(isa_mtb((unsigned long)(p)), (val))
+#define isa_rom_writew(val, p)  \
+	(ISA_SEX ? rom_out_be16(isa_mtw((unsigned long)(p)), (val))	\
+		 : rom_out_le16(isa_mtw((unsigned long)(p)), (val)))
+#define isa_rom_writew_swap(val, p)  \
+	(ISA_SEX ? rom_out_le16(isa_mtw((unsigned long)(p)), (val))	\
+		 : rom_out_be16(isa_mtw((unsigned long)(p)), (val)))
+#define isa_rom_writew_raw(val, p)  rom_out_be16(isa_mtw((unsigned long)(p)), (val))
+#endif /* CONFIG_ATARI_ROM_ISA */
+
 static inline void isa_delay(void)
 {
   switch(ISA_TYPE)
@@ -232,6 +296,9 @@ static inline void isa_delay(void)
 #ifdef CONFIG_AMIGA_PCMCIA
     case ISA_TYPE_AG: break;
 #endif
+#ifdef CONFIG_ATARI_ROM_ISA
+    case ISA_TYPE_ENEC: break;
+#endif
     default: break; /* avoid warnings */
     }
 }
@@ -263,6 +330,29 @@ static inline void isa_delay(void)
                   raw_outsw_swapw(isa_itw(port), (u16 *)(buf), (nr)<<1))
 
 
+#ifdef CONFIG_ATARI_ROM_ISA
+#define isa_rom_inb_p(p)	({ u8 _v = isa_rom_inb(p); isa_delay(); _v; })
+#define isa_rom_inw_p(p)	({ u16 _v = isa_rom_inw(p); isa_delay(); _v; })
+#define isa_rom_outb_p(v, p)	({ isa_rom_outb((v), (p)); isa_delay(); })
+#define isa_rom_outw_p(v, p)	({ isa_rom_outw((v), (p)); isa_delay(); })
+
+#define isa_rom_insb(port, buf, nr) raw_rom_insb(isa_itb(port), (u8 *)(buf), (nr))
+
+#define isa_rom_insw(port, buf, nr)     \
+       (ISA_SEX ? raw_rom_insw(isa_itw(port), (u16 *)(buf), (nr)) :    \
+		  raw_rom_insw_swapw(isa_itw(port), (u16 *)(buf), (nr)))
+
+#define isa_rom_outsb(port, buf, nr) raw_rom_outsb(isa_itb(port), (u8 *)(buf), (nr))
+
+#define isa_rom_outsw(port, buf, nr)    \
+       (ISA_SEX ? raw_rom_outsw(isa_itw(port), (u16 *)(buf), (nr)) :  \
+		  raw_rom_outsw_swapw(isa_itw(port), (u16 *)(buf), (nr)))
+#endif /* CONFIG_ATARI_ROM_ISA */
+
+#endif  /* CONFIG_ISA || CONFIG_ATARI_ROM_ISA */
+
+
+#if defined(CONFIG_ISA) && !defined(CONFIG_ATARI_ROM_ISA)
 #define inb     isa_inb
 #define inb_p   isa_inb_p
 #define outb    isa_outb
@@ -285,9 +375,43 @@ static inline void isa_delay(void)
 #define readw   isa_readw
 #define writeb  isa_writeb
 #define writew  isa_writew
+#endif  /* CONFIG_ISA && !CONFIG_ATARI_ROM_ISA */
 
-#else  /* CONFIG_ISA */
-
+#ifdef CONFIG_ATARI_ROM_ISA
+/*
+ * kernel with both ROM port ISA and IDE compiled in, those have
+ * conflicting defs for in/out. Simply consider port < 1024
+ * ROM port ISA and everything else regular ISA for IDE. read,write defined
+ * below.
+ */
+#define inb(port)	((port) < 1024 ? isa_rom_inb(port) : in_8(port))
+#define inb_p(port)	((port) < 1024 ? isa_rom_inb_p(port) : in_8(port))
+#define inw(port)	((port) < 1024 ? isa_rom_inw(port) : in_le16(port))
+#define inw_p(port)	((port) < 1024 ? isa_rom_inw_p(port) : in_le16(port))
+#define inl		isa_inl
+#define inl_p		isa_inl_p
+
+#define outb(val, port)	((port) < 1024 ? isa_rom_outb((val), (port)) : out_8((port), (val)))
+#define outb_p(val, port) ((port) < 1024 ? isa_rom_outb_p((val), (port)) : out_8((port), (val)))
+#define outw(val, port)	((port) < 1024 ? isa_rom_outw((val), (port)) : out_le16((port), (val)))
+#define outw_p(val, port) ((port) < 1024 ? isa_rom_outw_p((val), (port)) : out_le16((port), (val)))
+#define outl		isa_outl
+#define outl_p		isa_outl_p
+
+#define insb(port, buf, nr)	((port) < 1024 ? isa_rom_insb((port), (buf), (nr)) : isa_insb((port), (buf), (nr)))
+#define insw(port, buf, nr)	((port) < 1024 ? isa_rom_insw((port), (buf), (nr)) : isa_insw((port), (buf), (nr)))
+#define insl			isa_insl
+#define outsb(port, buf, nr)	((port) < 1024 ? isa_rom_outsb((port), (buf), (nr)) : isa_outsb((port), (buf), (nr)))
+#define outsw(port, buf, nr)	((port) < 1024 ? isa_rom_outsw((port), (buf), (nr)) : isa_outsw((port), (buf), (nr)))
+#define outsl			isa_outsl
+
+#define readb(addr)		in_8(addr)
+#define writeb(val, addr)	out_8((addr), (val))
+#define readw(addr)		in_le16(addr)
+#define writew(val, addr)	out_le16((addr), (val))
+#endif /* CONFIG_ATARI_ROM_ISA */
+
+#if !defined(CONFIG_ISA) && !defined(CONFIG_ATARI_ROM_ISA)
 /*
  * We need to define dummy functions for GENERIC_IOMAP support.
  */
@@ -319,7 +443,7 @@ static inline void isa_delay(void)
 #define readw(addr)      in_le16(addr)
 #define writew(val,addr) out_le16((addr),(val))
 
-#endif /* CONFIG_ISA */
+#endif /* !CONFIG_ISA && !CONFIG_ATARI_ROM_ISA */
 
 #define readl(addr)      in_le32(addr)
 #define writel(val,addr) out_le32((addr),(val))
diff --git a/arch/m68k/include/asm/irq.h b/arch/m68k/include/asm/irq.h
index c1155f0e22c..81ca118d58a 100644
--- a/arch/m68k/include/asm/irq.h
+++ b/arch/m68k/include/asm/irq.h
@@ -6,12 +6,16 @@
  * different m68k hosts compiled into the kernel.
  * Currently the Atari has 72 and the Amiga 24, but if both are
  * supported in the kernel it is better to make room for 72.
+ * With EtherNAT add-on card on Atari, the highest interrupt
+ * number is 140 so NR_IRQS needs to be 141.
  */
 #if defined(CONFIG_COLDFIRE)
 #define NR_IRQS 256
 #elif defined(CONFIG_VME) || defined(CONFIG_SUN3) || defined(CONFIG_SUN3X)
 #define NR_IRQS 200
-#elif defined(CONFIG_ATARI) || defined(CONFIG_MAC)
+#elif defined(CONFIG_ATARI)
+#define NR_IRQS 141
+#elif defined(CONFIG_MAC)
 #define NR_IRQS 72
 #elif defined(CONFIG_Q40)
 #define NR_IRQS	43
diff --git a/arch/m68k/include/asm/raw_io.h b/arch/m68k/include/asm/raw_io.h
index d9eb9834ccc..932faa35655 100644
--- a/arch/m68k/include/asm/raw_io.h
+++ b/arch/m68k/include/asm/raw_io.h
@@ -10,7 +10,7 @@
 
 #ifdef __KERNEL__
 
-#include <asm/types.h>
+#include <asm/byteorder.h>
 
 
 /* Values for nocacheflag and cmode */
@@ -60,6 +60,57 @@ extern void __iounmap(void *addr, unsigned long size);
 #define __raw_writew(val,addr) out_be16((addr),(val))
 #define __raw_writel(val,addr) out_be32((addr),(val))
 
+/*
+ * Atari ROM port (cartridge port) ISA adapter, used for the EtherNEC NE2000
+ * network card driver.
+ * The ISA adapter connects address lines A9-A13 to ISA address lines A0-A4,
+ * and hardwires the rest of the ISA addresses for a base address of 0x300.
+ *
+ * Data lines D8-D15 are connected to ISA data lines D0-D7 for reading.
+ * For writes, address lines A1-A8 are latched to ISA data lines D0-D7
+ * (meaning the bit pattern on A1-A8 can be read back as byte).
+ *
+ * Read and write operations are distinguished by the base address used:
+ * reads are from the ROM A side range, writes are through the B side range
+ * addresses (A side base + 0x10000).
+ *
+ * Reads and writes are byte only.
+ *
+ * 16 bit reads and writes are necessary for the NetUSBee adapter's USB
+ * chipset - 16 bit words are read straight off the ROM port while 16 bit
+ * reads are split into two byte writes. The low byte is latched to the
+ * NetUSBee buffer by a read from the _read_ window (with the data pattern
+ * asserted as A1-A8 address pattern). The high byte is then written to the
+ * write range as usual, completing the write cycle.
+ */
+
+#if defined(CONFIG_ATARI_ROM_ISA)
+#define rom_in_8(addr) \
+	({ u16 __v = (*(__force volatile u16 *) (addr)); __v >>= 8; __v; })
+#define rom_in_be16(addr) \
+	({ u16 __v = (*(__force volatile u16 *) (addr)); __v; })
+#define rom_in_le16(addr) \
+	({ u16 __v = le16_to_cpu(*(__force volatile u16 *) (addr)); __v; })
+
+#define rom_out_8(addr, b)	\
+	({u8 __w, __v = (b);  u32 _addr = ((u32) (addr)); \
+	__w = ((*(__force volatile u8 *)  ((_addr | 0x10000) + (__v<<1)))); })
+#define rom_out_be16(addr, w)	\
+	({u16 __w, __v = (w); u32 _addr = ((u32) (addr)); \
+	__w = ((*(__force volatile u16 *) ((_addr & 0xFFFF0000UL) + ((__v & 0xFF)<<1)))); \
+	__w = ((*(__force volatile u16 *) ((_addr | 0x10000) + ((__v >> 8)<<1)))); })
+#define rom_out_le16(addr, w)	\
+	({u16 __w, __v = (w); u32 _addr = ((u32) (addr)); \
+	__w = ((*(__force volatile u16 *) ((_addr & 0xFFFF0000UL) + ((__v >> 8)<<1)))); \
+	__w = ((*(__force volatile u16 *) ((_addr | 0x10000) + ((__v & 0xFF)<<1)))); })
+
+#define raw_rom_inb rom_in_8
+#define raw_rom_inw rom_in_be16
+
+#define raw_rom_outb(val, port) rom_out_8((port), (val))
+#define raw_rom_outw(val, port) rom_out_be16((port), (val))
+#endif /* CONFIG_ATARI_ROM_ISA */
+
 static inline void raw_insb(volatile u8 __iomem *port, u8 *buf, unsigned int len)
 {
 	unsigned int i;
@@ -342,6 +393,62 @@ static inline void raw_outsw_swapw(volatile u16 __iomem *port, const u16 *buf,
 		: "d0", "a0", "a1", "d6");
 }
 
+
+#if defined(CONFIG_ATARI_ROM_ISA)
+static inline void raw_rom_insb(volatile u8 __iomem *port, u8 *buf, unsigned int len)
+{
+	unsigned int i;
+
+	for (i = 0; i < len; i++)
+		*buf++ = rom_in_8(port);
+}
+
+static inline void raw_rom_outsb(volatile u8 __iomem *port, const u8 *buf,
+			     unsigned int len)
+{
+	unsigned int i;
+
+	for (i = 0; i < len; i++)
+		rom_out_8(port, *buf++);
+}
+
+static inline void raw_rom_insw(volatile u16 __iomem *port, u16 *buf,
+				   unsigned int nr)
+{
+	unsigned int i;
+
+	for (i = 0; i < nr; i++)
+		*buf++ = rom_in_be16(port);
+}
+
+static inline void raw_rom_outsw(volatile u16 __iomem *port, const u16 *buf,
+				   unsigned int nr)
+{
+	unsigned int i;
+
+	for (i = 0; i < nr; i++)
+		rom_out_be16(port, *buf++);
+}
+
+static inline void raw_rom_insw_swapw(volatile u16 __iomem *port, u16 *buf,
+				   unsigned int nr)
+{
+	unsigned int i;
+
+	for (i = 0; i < nr; i++)
+		*buf++ = rom_in_le16(port);
+}
+
+static inline void raw_rom_outsw_swapw(volatile u16 __iomem *port, const u16 *buf,
+				   unsigned int nr)
+{
+	unsigned int i;
+
+	for (i = 0; i < nr; i++)
+		rom_out_le16(port, *buf++);
+}
+#endif /* CONFIG_ATARI_ROM_ISA */
+
 #endif /* __KERNEL__ */
 
 #endif /* _RAW_IO_H */
diff --git a/arch/m68k/include/asm/string.h b/arch/m68k/include/asm/string.h
index 32198454da7..9aea9f11fa2 100644
--- a/arch/m68k/include/asm/string.h
+++ b/arch/m68k/include/asm/string.h
@@ -4,15 +4,6 @@
 #include <linux/types.h>
 #include <linux/compiler.h>
 
-static inline size_t __kernel_strlen(const char *s)
-{
-	const char *sc;
-
-	for (sc = s; *sc++; )
-		;
-	return sc - s - 1;
-}
-
 static inline char *__kernel_strcpy(char *dest, const char *src)
 {
 	char *xdest = dest;
@@ -27,11 +18,6 @@ static inline char *__kernel_strcpy(char *dest, const char *src)
 
 #ifndef __IN_STRING_C
 
-#define __HAVE_ARCH_STRLEN
-#define strlen(s)	(__builtin_constant_p(s) ?	\
-			 __builtin_strlen(s) :		\
-			 __kernel_strlen(s))
-
 #define __HAVE_ARCH_STRNLEN
 static inline size_t strnlen(const char *s, size_t count)
 {
diff --git a/arch/m68k/kernel/process.c b/arch/m68k/kernel/process.c
index d538694ad20..c55ff719fa7 100644
--- a/arch/m68k/kernel/process.c
+++ b/arch/m68k/kernel/process.c
@@ -51,40 +51,16 @@ unsigned long thread_saved_pc(struct task_struct *tsk)
 		return sw->retpc;
 }
 
-/*
- * The idle loop on an m68k..
- */
-static void default_idle(void)
+void arch_cpu_idle(void)
 {
-	if (!need_resched())
 #if defined(MACH_ATARI_ONLY)
-		/* block out HSYNC on the atari (falcon) */
-		__asm__("stop #0x2200" : : : "cc");
+	/* block out HSYNC on the atari (falcon) */
+	__asm__("stop #0x2200" : : : "cc");
 #else
-		__asm__("stop #0x2000" : : : "cc");
+	__asm__("stop #0x2000" : : : "cc");
 #endif
 }
 
-void (*idle)(void) = default_idle;
-
-/*
- * The idle thread. There's no useful work to be
- * done, so just try to conserve power and have a
- * low exit latency (ie sit in a loop waiting for
- * somebody to say that they'd like to reschedule)
- */
-void cpu_idle(void)
-{
-	/* endless idle loop with no priority at all */
-	while (1) {
-		rcu_idle_enter();
-		while (!need_resched())
-			idle();
-		rcu_idle_exit();
-		schedule_preempt_disabled();
-	}
-}
-
 void machine_restart(char * __unused)
 {
 	if (mach_reset)
diff --git a/arch/m68k/kernel/setup_mm.c b/arch/m68k/kernel/setup_mm.c
index 80cfbe56ea3..e67e5315957 100644
--- a/arch/m68k/kernel/setup_mm.c
+++ b/arch/m68k/kernel/setup_mm.c
@@ -381,6 +381,12 @@ void __init setup_arch(char **cmdline_p)
 		isa_sex = 1;
 	}
 #endif
+#ifdef CONFIG_ATARI_ROM_ISA
+	if (MACH_IS_ATARI) {
+		isa_type = ISA_TYPE_ENEC;
+		isa_sex = 0;
+	}
+#endif
 #endif
 }
 
diff --git a/arch/m68k/lib/string.c b/arch/m68k/lib/string.c
index b9a57abfad0..4d61fa8a112 100644
--- a/arch/m68k/lib/string.c
+++ b/arch/m68k/lib/string.c
@@ -17,6 +17,6 @@ EXPORT_SYMBOL(strcpy);
 
 char *strcat(char *dest, const char *src)
 {
-	return __kernel_strcpy(dest + __kernel_strlen(dest), src);
+	return __kernel_strcpy(dest + strlen(dest), src);
 }
 EXPORT_SYMBOL(strcat);
diff --git a/arch/m68k/mm/init.c b/arch/m68k/mm/init.c
index 519aad8fa81..1af2ca3411f 100644
--- a/arch/m68k/mm/init.c
+++ b/arch/m68k/mm/init.c
@@ -110,18 +110,7 @@ void __init paging_init(void)
 void free_initmem(void)
 {
 #ifndef CONFIG_MMU_SUN3
-	unsigned long addr;
-
-	addr = (unsigned long) __init_begin;
-	for (; addr < ((unsigned long) __init_end); addr += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(addr));
-		init_page_count(virt_to_page(addr));
-		free_page(addr);
-		totalram_pages++;
-	}
-	pr_notice("Freeing unused kernel memory: %luk freed (0x%x - 0x%x)\n",
-		(addr - (unsigned long) __init_begin) >> 10,
-		(unsigned int) __init_begin, (unsigned int) __init_end);
+	free_initmem_default(0);
 #endif /* CONFIG_MMU_SUN3 */
 }
 
@@ -213,15 +202,6 @@ void __init mem_init(void)
 #ifdef CONFIG_BLK_DEV_INITRD
 void free_initrd_mem(unsigned long start, unsigned long end)
 {
-	int pages = 0;
-	for (; start < end; start += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(start));
-		init_page_count(virt_to_page(start));
-		free_page(start);
-		totalram_pages++;
-		pages++;
-	}
-	pr_notice("Freeing initrd memory: %dk freed\n",
-		pages << (PAGE_SHIFT - 10));
+	free_reserved_area(start, end, 0, "initrd");
 }
 #endif
diff --git a/arch/metag/Kconfig b/arch/metag/Kconfig
index afc8973d148..b06b41861aa 100644
--- a/arch/metag/Kconfig
+++ b/arch/metag/Kconfig
@@ -25,6 +25,7 @@ config METAG
 	select HAVE_MEMBLOCK
 	select HAVE_MEMBLOCK_NODE_MAP
 	select HAVE_MOD_ARCH_SPECIFIC
+	select HAVE_OPROFILE
 	select HAVE_PERF_EVENTS
 	select HAVE_SYSCALL_TRACEPOINTS
 	select IRQ_DOMAIN
@@ -209,6 +210,9 @@ config METAG_PERFCOUNTER_IRQS
 	  When disabled, Performance Counters information will be collected
 	  based on Timer Interrupt.
 
+config HW_PERF_EVENTS
+	def_bool METAG_PERFCOUNTER_IRQS && PERF_EVENTS
+
 config METAG_DA
 	bool "DA support"
 	help
diff --git a/arch/metag/Makefile b/arch/metag/Makefile
index 81bd6a1c748..b566116b171 100644
--- a/arch/metag/Makefile
+++ b/arch/metag/Makefile
@@ -49,6 +49,8 @@ core-y					+= arch/metag/mm/
 libs-y					+= arch/metag/lib/
 libs-y					+= arch/metag/tbx/
 
+drivers-$(CONFIG_OPROFILE)		+= arch/metag/oprofile/
+
 boot					:= arch/metag/boot
 
 boot_targets				+= uImage
diff --git a/arch/metag/boot/dts/Makefile b/arch/metag/boot/dts/Makefile
index e0b5afd8bde..dbd95217733 100644
--- a/arch/metag/boot/dts/Makefile
+++ b/arch/metag/boot/dts/Makefile
@@ -4,13 +4,17 @@ dtb-y	+= skeleton.dtb
 builtindtb-y				:= skeleton
 
 ifneq ($(CONFIG_METAG_BUILTIN_DTB_NAME),"")
-	builtindtb-y			:= $(CONFIG_METAG_BUILTIN_DTB_NAME)
+	builtindtb-y			:= $(patsubst "%",%,$(CONFIG_METAG_BUILTIN_DTB_NAME))
 endif
-obj-$(CONFIG_METAG_BUILTIN_DTB)	+= $(patsubst "%",%,$(builtindtb-y)).dtb.o
+
+dtb-$(CONFIG_METAG_BUILTIN_DTB)	+= $(builtindtb-y).dtb
+obj-$(CONFIG_METAG_BUILTIN_DTB)	+= $(builtindtb-y).dtb.o
 
 targets	+= dtbs
 targets	+= $(dtb-y)
 
+.SECONDARY: $(obj)/$(builtindtb-y).dtb.S
+
 dtbs: $(addprefix $(obj)/, $(dtb-y))
 
-clean-files += *.dtb
+clean-files += *.dtb *.dtb.S
diff --git a/arch/metag/configs/meta1_defconfig b/arch/metag/configs/meta1_defconfig
index c35a75e8ecf..01cd67e4403 100644
--- a/arch/metag/configs/meta1_defconfig
+++ b/arch/metag/configs/meta1_defconfig
@@ -1,6 +1,5 @@
 # CONFIG_LOCALVERSION_AUTO is not set
 # CONFIG_SWAP is not set
-CONFIG_LOG_BUF_SHIFT=13
 CONFIG_SYSFS_DEPRECATED=y
 CONFIG_SYSFS_DEPRECATED_V2=y
 CONFIG_KALLSYMS_ALL=y
diff --git a/arch/metag/configs/meta2_defconfig b/arch/metag/configs/meta2_defconfig
index fb314841018..643392ba7ed 100644
--- a/arch/metag/configs/meta2_defconfig
+++ b/arch/metag/configs/meta2_defconfig
@@ -1,7 +1,6 @@
 # CONFIG_LOCALVERSION_AUTO is not set
 # CONFIG_SWAP is not set
 CONFIG_SYSVIPC=y
-CONFIG_LOG_BUF_SHIFT=13
 CONFIG_SYSFS_DEPRECATED=y
 CONFIG_SYSFS_DEPRECATED_V2=y
 CONFIG_KALLSYMS_ALL=y
diff --git a/arch/metag/configs/meta2_smp_defconfig b/arch/metag/configs/meta2_smp_defconfig
index 6c7b777ac27..f3306737da2 100644
--- a/arch/metag/configs/meta2_smp_defconfig
+++ b/arch/metag/configs/meta2_smp_defconfig
@@ -1,7 +1,6 @@
 # CONFIG_LOCALVERSION_AUTO is not set
 # CONFIG_SWAP is not set
 CONFIG_SYSVIPC=y
-CONFIG_LOG_BUF_SHIFT=13
 CONFIG_SYSFS_DEPRECATED=y
 CONFIG_SYSFS_DEPRECATED_V2=y
 CONFIG_KALLSYMS_ALL=y
diff --git a/arch/metag/include/asm/metag_mem.h b/arch/metag/include/asm/metag_mem.h
index 3f7b54d8cca..aa5a076df43 100644
--- a/arch/metag/include/asm/metag_mem.h
+++ b/arch/metag/include/asm/metag_mem.h
@@ -700,6 +700,9 @@
 #define     SYSC_xCPARTG_AND_S    8
 #define     SYSC_xCPARTL_OR_BITS  0x000F0000 /* Ors into top 4 bits */
 #define     SYSC_xCPARTL_OR_S     16
+#ifdef METAC_2_1
+#define     SYSC_DCPART_GCON_BIT  0x00100000 /* Coherent shared local */
+#endif /* METAC_2_1 */
 #define     SYSC_xCPARTG_OR_BITS  0x0F000000 /* Ors into top 4 bits */
 #define     SYSC_xCPARTG_OR_S     24
 #define     SYSC_CWRMODE_BIT      0x80000000 /* Write cache mode bit */
diff --git a/arch/metag/include/asm/thread_info.h b/arch/metag/include/asm/thread_info.h
index 0ecd34d8b5f..7c4a3300614 100644
--- a/arch/metag/include/asm/thread_info.h
+++ b/arch/metag/include/asm/thread_info.h
@@ -150,6 +150,4 @@ static inline int kstack_end(void *addr)
 #define _TIF_WORK_MASK		(_TIF_ALLWORK_MASK & ~(_TIF_SYSCALL_TRACE | \
 				 _TIF_SYSCALL_AUDIT | _TIF_SINGLESTEP))
 
-#define tsk_is_polling(t) test_tsk_thread_flag(t, TIF_POLLING_NRFLAG)
-
 #endif /* _ASM_THREAD_INFO_H */
diff --git a/arch/metag/include/uapi/asm/Kbuild b/arch/metag/include/uapi/asm/Kbuild
index 876c71f866d..84e09feb4d5 100644
--- a/arch/metag/include/uapi/asm/Kbuild
+++ b/arch/metag/include/uapi/asm/Kbuild
@@ -2,6 +2,7 @@
 include include/uapi/asm-generic/Kbuild.asm
 
 header-y += byteorder.h
+header-y += ech.h
 header-y += ptrace.h
 header-y += resource.h
 header-y += sigcontext.h
diff --git a/arch/metag/include/uapi/asm/ech.h b/arch/metag/include/uapi/asm/ech.h
new file mode 100644
index 00000000000..ac94d1cf9be
--- /dev/null
+++ b/arch/metag/include/uapi/asm/ech.h
@@ -0,0 +1,15 @@
+#ifndef _UAPI_METAG_ECH_H
+#define _UAPI_METAG_ECH_H
+
+/*
+ * These bits can be set in the top half of the D0.8 register when DSP context
+ * switching is enabled, in order to support partial DSP context save/restore.
+ */
+
+#define TBICTX_XEXT_BIT	0x1000	/* Enable extended context save */
+#define TBICTX_XTDP_BIT	0x0800	/* DSP accumulators/RAM/templates */
+#define TBICTX_XHL2_BIT	0x0400	/* Hardware loops */
+#define TBICTX_XAXX_BIT	0x0200	/* Extended AX registers (A*.4-7) */
+#define TBICTX_XDX8_BIT	0x0100	/* Extended DX registers (D*.8-15) */
+
+#endif /* _UAPI_METAG_ECH_H */
diff --git a/arch/metag/kernel/cachepart.c b/arch/metag/kernel/cachepart.c
index 3a589dfb966..954548b1bea 100644
--- a/arch/metag/kernel/cachepart.c
+++ b/arch/metag/kernel/cachepart.c
@@ -24,15 +24,21 @@
 unsigned int get_dcache_size(void)
 {
 	unsigned int config2 = metag_in32(METAC_CORE_CONFIG2);
-	return 0x1000 << ((config2 & METAC_CORECFG2_DCSZ_BITS)
-				>> METAC_CORECFG2_DCSZ_S);
+	unsigned int sz = 0x1000 << ((config2 & METAC_CORECFG2_DCSZ_BITS)
+				     >> METAC_CORECFG2_DCSZ_S);
+	if (config2 & METAC_CORECFG2_DCSMALL_BIT)
+		sz >>= 6;
+	return sz;
 }
 
 unsigned int get_icache_size(void)
 {
 	unsigned int config2 = metag_in32(METAC_CORE_CONFIG2);
-	return 0x1000 << ((config2 & METAC_CORE_C2ICSZ_BITS)
-				>> METAC_CORE_C2ICSZ_S);
+	unsigned int sz = 0x1000 << ((config2 & METAC_CORE_C2ICSZ_BITS)
+				     >> METAC_CORE_C2ICSZ_S);
+	if (config2 & METAC_CORECFG2_ICSMALL_BIT)
+		sz >>= 6;
+	return sz;
 }
 
 unsigned int get_global_dcache_size(void)
@@ -61,7 +67,7 @@ static unsigned int get_thread_cache_size(unsigned int cache, int thread_id)
 		return 0;
 #if PAGE_OFFSET >= LINGLOBAL_BASE
 	/* Checking for global cache */
-	cache_size = (cache == DCACHE ? get_global_dache_size() :
+	cache_size = (cache == DCACHE ? get_global_dcache_size() :
 		get_global_icache_size());
 	offset = 8;
 #else
diff --git a/arch/metag/kernel/da.c b/arch/metag/kernel/da.c
index 52aabb658fd..a35dbed6fff 100644
--- a/arch/metag/kernel/da.c
+++ b/arch/metag/kernel/da.c
@@ -5,12 +5,14 @@
  */
 
 
+#include <linux/export.h>
 #include <linux/io.h>
 #include <linux/kernel.h>
 #include <asm/da.h>
 #include <asm/metag_mem.h>
 
 bool _metag_da_present;
+EXPORT_SYMBOL_GPL(_metag_da_present);
 
 int __init metag_da_probe(void)
 {
diff --git a/arch/metag/kernel/head.S b/arch/metag/kernel/head.S
index 969dffabc03..713f71d1bdf 100644
--- a/arch/metag/kernel/head.S
+++ b/arch/metag/kernel/head.S
@@ -1,6 +1,7 @@
 	! Copyright 2005,2006,2007,2009 Imagination Technologies
 
 #include <linux/init.h>
+#include <asm/metag_mem.h>
 #include <generated/asm-offsets.h>
 #undef __exit
 
@@ -48,6 +49,13 @@ __exit:
 	.global _secondary_startup
 	.type _secondary_startup,function
 _secondary_startup:
+#if CONFIG_PAGE_OFFSET < LINGLOBAL_BASE
+	! In case GCOn has just been turned on we need to fence any writes that
+	! the boot thread might have performed prior to coherency taking effect.
+	MOVT	D0Re0,#HI(LINSYSEVENT_WR_ATOMIC_UNLOCK)
+	MOV	D1Re0,#0
+	SETD	[D0Re0], D1Re0
+#endif
 	MOVT	A0StP,#HI(_secondary_data_stack)
 	ADD	A0StP,A0StP,#LO(_secondary_data_stack)
 	GETD	A0StP,[A0StP]
diff --git a/arch/metag/kernel/perf/perf_event.c b/arch/metag/kernel/perf/perf_event.c
index a876d5ff389..366569425c5 100644
--- a/arch/metag/kernel/perf/perf_event.c
+++ b/arch/metag/kernel/perf/perf_event.c
@@ -22,9 +22,9 @@
 #include <linux/slab.h>
 
 #include <asm/core_reg.h>
-#include <asm/hwthread.h>
 #include <asm/io.h>
 #include <asm/irq.h>
+#include <asm/processor.h>
 
 #include "perf_event.h"
 
@@ -40,10 +40,10 @@ static DEFINE_PER_CPU(struct cpu_hw_events, cpu_hw_events);
 /* PMU admin */
 const char *perf_pmu_name(void)
 {
-	if (metag_pmu)
-		return metag_pmu->pmu.name;
+	if (!metag_pmu)
+		return NULL;
 
-	return NULL;
+	return metag_pmu->name;
 }
 EXPORT_SYMBOL_GPL(perf_pmu_name);
 
@@ -171,6 +171,7 @@ static int metag_pmu_event_init(struct perf_event *event)
 	switch (event->attr.type) {
 	case PERF_TYPE_HARDWARE:
 	case PERF_TYPE_HW_CACHE:
+	case PERF_TYPE_RAW:
 		err = _hw_perf_event_init(event);
 		break;
 
@@ -211,9 +212,10 @@ again:
 	/*
 	 * Calculate the delta and add it to the counter.
 	 */
-	delta = new_raw_count - prev_raw_count;
+	delta = (new_raw_count - prev_raw_count) & MAX_PERIOD;
 
 	local64_add(delta, &event->count);
+	local64_sub(delta, &hwc->period_left);
 }
 
 int metag_pmu_event_set_period(struct perf_event *event,
@@ -223,6 +225,10 @@ int metag_pmu_event_set_period(struct perf_event *event,
 	s64 period = hwc->sample_period;
 	int ret = 0;
 
+	/* The period may have been changed */
+	if (unlikely(period != hwc->last_period))
+		left += period - hwc->last_period;
+
 	if (unlikely(left <= -period)) {
 		left = period;
 		local64_set(&hwc->period_left, left);
@@ -240,8 +246,10 @@ int metag_pmu_event_set_period(struct perf_event *event,
 	if (left > (s64)metag_pmu->max_period)
 		left = metag_pmu->max_period;
 
-	if (metag_pmu->write)
-		metag_pmu->write(idx, (u64)(-left) & MAX_PERIOD);
+	if (metag_pmu->write) {
+		local64_set(&hwc->prev_count, -(s32)left);
+		metag_pmu->write(idx, -left & MAX_PERIOD);
+	}
 
 	perf_event_update_userpage(event);
 
@@ -549,6 +557,10 @@ static int _hw_perf_event_init(struct perf_event *event)
 		if (err)
 			return err;
 		break;
+
+	case PERF_TYPE_RAW:
+		mapping = attr->config;
+		break;
 	}
 
 	/* Return early if the event is unsupported */
@@ -610,15 +622,13 @@ static void metag_pmu_enable_counter(struct hw_perf_event *event, int idx)
 		WARN_ONCE((config != 0x100),
 			"invalid configuration (%d) for counter (%d)\n",
 			config, idx);
-
-		/* Reset the cycle count */
-		__core_reg_set(TXTACTCYC, 0);
+		local64_set(&event->prev_count, __core_reg_get(TXTACTCYC));
 		goto unlock;
 	}
 
 	/* Check for a core internal or performance channel event. */
 	if (tmp) {
-		void *perf_addr = (void *)PERF_COUNT(idx);
+		void *perf_addr;
 
 		/*
 		 * Anything other than a cycle count will write the low-
@@ -632,9 +642,14 @@ static void metag_pmu_enable_counter(struct hw_perf_event *event, int idx)
 		case 0xf0:
 			perf_addr = (void *)PERF_CHAN(idx);
 			break;
+
+		default:
+			perf_addr = NULL;
+			break;
 		}
 
-		metag_out32((tmp & 0x0f), perf_addr);
+		if (perf_addr)
+			metag_out32((config & 0x0f), perf_addr);
 
 		/*
 		 * Now we use the high nibble as the performance event to
@@ -643,13 +658,21 @@ static void metag_pmu_enable_counter(struct hw_perf_event *event, int idx)
 		config = tmp >> 4;
 	}
 
-	/*
-	 * Enabled counters start from 0. Early cores clear the count on
-	 * write but newer cores don't, so we make sure that the count is
-	 * set to 0.
-	 */
 	tmp = ((config & 0xf) << 28) |
-			((1 << 24) << cpu_2_hwthread_id[get_cpu()]);
+			((1 << 24) << hard_processor_id());
+	if (metag_pmu->max_period)
+		/*
+		 * Cores supporting overflow interrupts may have had the counter
+		 * set to a specific value that needs preserving.
+		 */
+		tmp |= metag_in32(PERF_COUNT(idx)) & 0x00ffffff;
+	else
+		/*
+		 * Older cores reset the counter on write, so prev_count needs
+		 * resetting too so we can calculate a correct delta.
+		 */
+		local64_set(&event->prev_count, 0);
+
 	metag_out32(tmp, PERF_COUNT(idx));
 unlock:
 	raw_spin_unlock_irqrestore(&events->pmu_lock, flags);
@@ -693,9 +716,8 @@ static u64 metag_pmu_read_counter(int idx)
 {
 	u32 tmp = 0;
 
-	/* The act of reading the cycle counter also clears it */
 	if (METAG_INST_COUNTER == idx) {
-		__core_reg_swap(TXTACTCYC, tmp);
+		tmp = __core_reg_get(TXTACTCYC);
 		goto out;
 	}
 
@@ -764,10 +786,16 @@ static irqreturn_t metag_pmu_counter_overflow(int irq, void *dev)
 
 	/*
 	 * Enable the counter again once core overflow processing has
-	 * completed.
+	 * completed. Note the counter value may have been modified while it was
+	 * inactive to set it up ready for the next interrupt.
 	 */
-	if (!perf_event_overflow(event, &sampledata, regs))
+	if (!perf_event_overflow(event, &sampledata, regs)) {
+		__global_lock2(flags);
+		counter = (counter & 0xff000000) |
+			  (metag_in32(PERF_COUNT(idx)) & 0x00ffffff);
 		metag_out32(counter, PERF_COUNT(idx));
+		__global_unlock2(flags);
+	}
 
 	return IRQ_HANDLED;
 }
@@ -830,7 +858,7 @@ static int __init init_hw_perf_events(void)
 			metag_pmu->max_period = 0;
 		}
 
-		metag_pmu->name = "Meta 2";
+		metag_pmu->name = "meta2";
 		metag_pmu->version = version;
 		metag_pmu->pmu = pmu;
 	}
diff --git a/arch/metag/kernel/process.c b/arch/metag/kernel/process.c
index c6efe62e5b7..dc592354456 100644
--- a/arch/metag/kernel/process.c
+++ b/arch/metag/kernel/process.c
@@ -22,6 +22,7 @@
 #include <linux/pm.h>
 #include <linux/syscalls.h>
 #include <linux/uaccess.h>
+#include <linux/smp.h>
 #include <asm/core_reg.h>
 #include <asm/user_gateway.h>
 #include <asm/tcm.h>
@@ -31,7 +32,7 @@
 /*
  * Wait for the next interrupt and enable local interrupts
  */
-static inline void arch_idle(void)
+void arch_cpu_idle(void)
 {
 	int tmp;
 
@@ -59,36 +60,12 @@ static inline void arch_idle(void)
 		      : "r" (get_trigger_mask()));
 }
 
-void cpu_idle(void)
-{
-	set_thread_flag(TIF_POLLING_NRFLAG);
-
-	while (1) {
-		tick_nohz_idle_enter();
-		rcu_idle_enter();
-
-		while (!need_resched()) {
-			/*
-			 * We need to disable interrupts here to ensure we don't
-			 * miss a wakeup call.
-			 */
-			local_irq_disable();
-			if (!need_resched()) {
 #ifdef CONFIG_HOTPLUG_CPU
-				if (cpu_is_offline(smp_processor_id()))
-					cpu_die();
-#endif
-				arch_idle();
-			} else {
-				local_irq_enable();
-			}
-		}
-
-		rcu_idle_exit();
-		tick_nohz_idle_exit();
-		schedule_preempt_disabled();
-	 }
+void arch_cpu_idle_dead(void)
+{
+	cpu_die();
 }
+#endif
 
 void (*pm_power_off)(void);
 EXPORT_SYMBOL(pm_power_off);
diff --git a/arch/metag/kernel/ptrace.c b/arch/metag/kernel/ptrace.c
index 47a8828615a..7563628822b 100644
--- a/arch/metag/kernel/ptrace.c
+++ b/arch/metag/kernel/ptrace.c
@@ -288,10 +288,36 @@ static int metag_rp_state_set(struct task_struct *target,
 	return metag_rp_state_copyin(regs, pos, count, kbuf, ubuf);
 }
 
+static int metag_tls_get(struct task_struct *target,
+			const struct user_regset *regset,
+			unsigned int pos, unsigned int count,
+			void *kbuf, void __user *ubuf)
+{
+	void __user *tls = target->thread.tls_ptr;
+	return user_regset_copyout(&pos, &count, &kbuf, &ubuf, &tls, 0, -1);
+}
+
+static int metag_tls_set(struct task_struct *target,
+			const struct user_regset *regset,
+			unsigned int pos, unsigned int count,
+			const void *kbuf, const void __user *ubuf)
+{
+	int ret;
+	void __user *tls;
+
+	ret = user_regset_copyin(&pos, &count, &kbuf, &ubuf, &tls, 0, -1);
+	if (ret)
+		return ret;
+
+	target->thread.tls_ptr = tls;
+	return ret;
+}
+
 enum metag_regset {
 	REGSET_GENERAL,
 	REGSET_CBUF,
 	REGSET_READPIPE,
+	REGSET_TLS,
 };
 
 static const struct user_regset metag_regsets[] = {
@@ -319,6 +345,14 @@ static const struct user_regset metag_regsets[] = {
 		.get = metag_rp_state_get,
 		.set = metag_rp_state_set,
 	},
+	[REGSET_TLS] = {
+		.core_note_type = NT_METAG_TLS,
+		.n = 1,
+		.size = sizeof(void *),
+		.align = sizeof(void *),
+		.get = metag_tls_get,
+		.set = metag_tls_set,
+	},
 };
 
 static const struct user_regset_view user_metag_view = {
diff --git a/arch/metag/kernel/setup.c b/arch/metag/kernel/setup.c
index 879246170ae..4f5726f1a55 100644
--- a/arch/metag/kernel/setup.c
+++ b/arch/metag/kernel/setup.c
@@ -124,6 +124,7 @@ struct machine_desc *machine_desc __initdata;
 u8 cpu_2_hwthread_id[NR_CPUS] __read_mostly = {
 	[0 ... NR_CPUS-1] = BAD_HWTHREAD_ID
 };
+EXPORT_SYMBOL_GPL(cpu_2_hwthread_id);
 
 /*
  * Map a hardware thread ID to a Linux CPU number
diff --git a/arch/metag/kernel/smp.c b/arch/metag/kernel/smp.c
index 4b6d1f14df3..f443ec9a7cb 100644
--- a/arch/metag/kernel/smp.c
+++ b/arch/metag/kernel/smp.c
@@ -28,6 +28,8 @@
 #include <asm/cachepart.h>
 #include <asm/core_reg.h>
 #include <asm/cpu.h>
+#include <asm/global_lock.h>
+#include <asm/metag_mem.h>
 #include <asm/mmu_context.h>
 #include <asm/pgtable.h>
 #include <asm/pgalloc.h>
@@ -37,6 +39,9 @@
 #include <asm/hwthread.h>
 #include <asm/traps.h>
 
+#define SYSC_DCPART(n)	(SYSC_DCPART0 + SYSC_xCPARTn_STRIDE * (n))
+#define SYSC_ICPART(n)	(SYSC_ICPART0 + SYSC_xCPARTn_STRIDE * (n))
+
 DECLARE_PER_CPU(PTBI, pTBI);
 
 void *secondary_data_stack;
@@ -99,6 +104,114 @@ int __cpuinit boot_secondary(unsigned int thread, struct task_struct *idle)
 	return 0;
 }
 
+/**
+ * describe_cachepart_change: describe a change to cache partitions.
+ * @thread:	Hardware thread number.
+ * @label:	Label of cache type, e.g. "dcache" or "icache".
+ * @sz:		Total size of the cache.
+ * @old:	Old cache partition configuration (*CPART* register).
+ * @new:	New cache partition configuration (*CPART* register).
+ *
+ * If the cache partition has changed, prints a message to the log describing
+ * those changes.
+ */
+static __cpuinit void describe_cachepart_change(unsigned int thread,
+						const char *label,
+						unsigned int sz,
+						unsigned int old,
+						unsigned int new)
+{
+	unsigned int lor1, land1, gor1, gand1;
+	unsigned int lor2, land2, gor2, gand2;
+	unsigned int diff = old ^ new;
+
+	if (!diff)
+		return;
+
+	pr_info("Thread %d: %s partition changed:", thread, label);
+	if (diff & (SYSC_xCPARTL_OR_BITS | SYSC_xCPARTL_AND_BITS)) {
+		lor1   = (old & SYSC_xCPARTL_OR_BITS)  >> SYSC_xCPARTL_OR_S;
+		lor2   = (new & SYSC_xCPARTL_OR_BITS)  >> SYSC_xCPARTL_OR_S;
+		land1  = (old & SYSC_xCPARTL_AND_BITS) >> SYSC_xCPARTL_AND_S;
+		land2  = (new & SYSC_xCPARTL_AND_BITS) >> SYSC_xCPARTL_AND_S;
+		pr_cont(" L:%#x+%#x->%#x+%#x",
+			(lor1 * sz) >> 4,
+			((land1 + 1) * sz) >> 4,
+			(lor2 * sz) >> 4,
+			((land2 + 1) * sz) >> 4);
+	}
+	if (diff & (SYSC_xCPARTG_OR_BITS | SYSC_xCPARTG_AND_BITS)) {
+		gor1   = (old & SYSC_xCPARTG_OR_BITS)  >> SYSC_xCPARTG_OR_S;
+		gor2   = (new & SYSC_xCPARTG_OR_BITS)  >> SYSC_xCPARTG_OR_S;
+		gand1  = (old & SYSC_xCPARTG_AND_BITS) >> SYSC_xCPARTG_AND_S;
+		gand2  = (new & SYSC_xCPARTG_AND_BITS) >> SYSC_xCPARTG_AND_S;
+		pr_cont(" G:%#x+%#x->%#x+%#x",
+			(gor1 * sz) >> 4,
+			((gand1 + 1) * sz) >> 4,
+			(gor2 * sz) >> 4,
+			((gand2 + 1) * sz) >> 4);
+	}
+	if (diff & SYSC_CWRMODE_BIT)
+		pr_cont(" %sWR",
+			(new & SYSC_CWRMODE_BIT) ? "+" : "-");
+	if (diff & SYSC_DCPART_GCON_BIT)
+		pr_cont(" %sGCOn",
+			(new & SYSC_DCPART_GCON_BIT) ? "+" : "-");
+	pr_cont("\n");
+}
+
+/**
+ * setup_smp_cache: ensure cache coherency for new SMP thread.
+ * @thread:	New hardware thread number.
+ *
+ * Ensures that coherency is enabled and that the threads share the same cache
+ * partitions.
+ */
+static __cpuinit void setup_smp_cache(unsigned int thread)
+{
+	unsigned int this_thread, lflags;
+	unsigned int dcsz, dcpart_this, dcpart_old, dcpart_new;
+	unsigned int icsz, icpart_old, icpart_new;
+
+	/*
+	 * Copy over the current thread's cache partition configuration to the
+	 * new thread so that they share cache partitions.
+	 */
+	__global_lock2(lflags);
+	this_thread = hard_processor_id();
+	/* Share dcache partition */
+	dcpart_this = metag_in32(SYSC_DCPART(this_thread));
+	dcpart_old = metag_in32(SYSC_DCPART(thread));
+	dcpart_new = dcpart_this;
+#if PAGE_OFFSET < LINGLOBAL_BASE
+	/*
+	 * For the local data cache to be coherent the threads must also have
+	 * GCOn enabled.
+	 */
+	dcpart_new |= SYSC_DCPART_GCON_BIT;
+	metag_out32(dcpart_new, SYSC_DCPART(this_thread));
+#endif
+	metag_out32(dcpart_new, SYSC_DCPART(thread));
+	/* Share icache partition too */
+	icpart_new = metag_in32(SYSC_ICPART(this_thread));
+	icpart_old = metag_in32(SYSC_ICPART(thread));
+	metag_out32(icpart_new, SYSC_ICPART(thread));
+	__global_unlock2(lflags);
+
+	/*
+	 * Log if the cache partitions were altered so the user is aware of any
+	 * potential unintentional cache wastage.
+	 */
+	dcsz = get_dcache_size();
+	icsz = get_dcache_size();
+	describe_cachepart_change(this_thread, "dcache", dcsz,
+				  dcpart_this, dcpart_new);
+	describe_cachepart_change(thread, "dcache", dcsz,
+				  dcpart_old, dcpart_new);
+	describe_cachepart_change(thread, "icache", icsz,
+				  icpart_old, icpart_new);
+}
+
 int __cpuinit __cpu_up(unsigned int cpu, struct task_struct *idle)
 {
 	unsigned int thread = cpu_2_hwthread_id[cpu];
@@ -108,6 +221,8 @@ int __cpuinit __cpu_up(unsigned int cpu, struct task_struct *idle)
 
 	flush_tlb_all();
 
+	setup_smp_cache(thread);
+
 	/*
 	 * Tell the secondary CPU where to find its idle thread's stack.
 	 */
@@ -297,7 +412,7 @@ asmlinkage void secondary_start_kernel(void)
 	/*
 	 * OK, it's off to the idle thread for us
 	 */
-	cpu_idle();
+	cpu_startup_entry(CPUHP_ONLINE);
 }
 
 void __init smp_cpus_done(unsigned int max_cpus)
diff --git a/arch/metag/mm/Kconfig b/arch/metag/mm/Kconfig
index 975f2f4e3ec..794f26a187f 100644
--- a/arch/metag/mm/Kconfig
+++ b/arch/metag/mm/Kconfig
@@ -98,9 +98,6 @@ config MAX_ACTIVE_REGIONS
 	default "2" if SPARSEMEM
 	default "1"
 
-config ARCH_POPULATES_NODE_MAP
-	def_bool y
-
 config ARCH_SELECT_MEMORY_MODEL
 	def_bool y
 
diff --git a/arch/metag/mm/init.c b/arch/metag/mm/init.c
index 504a398d5f8..d05b8455c44 100644
--- a/arch/metag/mm/init.c
+++ b/arch/metag/mm/init.c
@@ -380,14 +380,8 @@ void __init mem_init(void)
 
 #ifdef CONFIG_HIGHMEM
 	unsigned long tmp;
-	for (tmp = highstart_pfn; tmp < highend_pfn; tmp++) {
-		struct page *page = pfn_to_page(tmp);
-		ClearPageReserved(page);
-		init_page_count(page);
-		__free_page(page);
-		totalhigh_pages++;
-	}
-	totalram_pages += totalhigh_pages;
+	for (tmp = highstart_pfn; tmp < highend_pfn; tmp++)
+		free_highmem_page(pfn_to_page(tmp));
 	num_physpages += totalhigh_pages;
 #endif /* CONFIG_HIGHMEM */
 
@@ -412,32 +406,15 @@ void __init mem_init(void)
 	return;
 }
 
-static void free_init_pages(char *what, unsigned long begin, unsigned long end)
-{
-	unsigned long addr;
-
-	for (addr = begin; addr < end; addr += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(addr));
-		init_page_count(virt_to_page(addr));
-		memset((void *)addr, POISON_FREE_INITMEM, PAGE_SIZE);
-		free_page(addr);
-		totalram_pages++;
-	}
-	pr_info("Freeing %s: %luk freed\n", what, (end - begin) >> 10);
-}
-
 void free_initmem(void)
 {
-	free_init_pages("unused kernel memory",
-			(unsigned long)(&__init_begin),
-			(unsigned long)(&__init_end));
+	free_initmem_default(POISON_FREE_INITMEM);
 }
 
 #ifdef CONFIG_BLK_DEV_INITRD
 void free_initrd_mem(unsigned long start, unsigned long end)
 {
-	end = end & PAGE_MASK;
-	free_init_pages("initrd memory", start, end);
+	free_reserved_area(start, end, POISON_FREE_INITMEM, "initrd");
 }
 #endif
 
diff --git a/arch/metag/oprofile/Makefile b/arch/metag/oprofile/Makefile
new file mode 100644
index 00000000000..c9639d4734d
--- /dev/null
+++ b/arch/metag/oprofile/Makefile
@@ -0,0 +1,17 @@
+obj-$(CONFIG_OPROFILE)	+= oprofile.o
+
+oprofile-core-y	+= buffer_sync.o
+oprofile-core-y	+= cpu_buffer.o
+oprofile-core-y	+= event_buffer.o
+oprofile-core-y	+= oprof.o
+oprofile-core-y	+= oprofile_files.o
+oprofile-core-y	+= oprofile_stats.o
+oprofile-core-y	+= oprofilefs.o
+oprofile-core-y	+= timer_int.o
+oprofile-core-$(CONFIG_HW_PERF_EVENTS)	+= oprofile_perf.o
+
+oprofile-y	+= backtrace.o
+oprofile-y	+= common.o
+oprofile-y	+= $(addprefix ../../../drivers/oprofile/,$(oprofile-core-y))
+
+ccflags-y	+= -Werror
diff --git a/arch/metag/oprofile/backtrace.c b/arch/metag/oprofile/backtrace.c
new file mode 100644
index 00000000000..7cc3f37cb40
--- /dev/null
+++ b/arch/metag/oprofile/backtrace.c
@@ -0,0 +1,63 @@
+/*
+ * Copyright (C) 2010-2013 Imagination Technologies Ltd.
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ */
+
+#include <linux/oprofile.h>
+#include <linux/uaccess.h>
+#include <asm/processor.h>
+#include <asm/stacktrace.h>
+
+#include "backtrace.h"
+
+static void user_backtrace_fp(unsigned long __user *fp, unsigned int depth)
+{
+	while (depth-- && access_ok(VERIFY_READ, fp, 8)) {
+		unsigned long addr;
+		unsigned long __user *fpnew;
+		if (__copy_from_user_inatomic(&addr, fp + 1, sizeof(addr)))
+			break;
+		addr -= 4;
+
+		oprofile_add_trace(addr);
+
+		/* stack grows up, so frame pointers must decrease */
+		if (__copy_from_user_inatomic(&fpnew, fp + 0, sizeof(fpnew)))
+			break;
+		if (fpnew >= fp)
+			break;
+		fp = fpnew;
+	}
+}
+
+static int kernel_backtrace_frame(struct stackframe *frame, void *data)
+{
+	unsigned int *depth = data;
+
+	oprofile_add_trace(frame->pc);
+
+	/* decrement depth and stop if we reach 0 */
+	if ((*depth)-- == 0)
+		return 1;
+
+	/* otherwise onto the next frame */
+	return 0;
+}
+
+void metag_backtrace(struct pt_regs * const regs, unsigned int depth)
+{
+	if (user_mode(regs)) {
+		unsigned long *fp = (unsigned long *)regs->ctx.AX[1].U0;
+		user_backtrace_fp((unsigned long __user __force *)fp, depth);
+	} else {
+		struct stackframe frame;
+		frame.fp = regs->ctx.AX[1].U0;		/* A0FrP */
+		frame.sp = user_stack_pointer(regs);	/* A0StP */
+		frame.lr = 0;				/* from stack */
+		frame.pc = regs->ctx.CurrPC;		/* PC */
+		walk_stackframe(&frame, &kernel_backtrace_frame, &depth);
+	}
+}
diff --git a/arch/metag/oprofile/backtrace.h b/arch/metag/oprofile/backtrace.h
new file mode 100644
index 00000000000..c0fcc4265ab
--- /dev/null
+++ b/arch/metag/oprofile/backtrace.h
@@ -0,0 +1,6 @@
+#ifndef _METAG_OPROFILE_BACKTRACE_H
+#define _METAG_OPROFILE_BACKTRACE_H
+
+void metag_backtrace(struct pt_regs * const regs, unsigned int depth);
+
+#endif
diff --git a/arch/metag/oprofile/common.c b/arch/metag/oprofile/common.c
new file mode 100644
index 00000000000..ba26152b3c0
--- /dev/null
+++ b/arch/metag/oprofile/common.c
@@ -0,0 +1,66 @@
+/*
+ * arch/metag/oprofile/common.c
+ *
+ * Copyright (C) 2013 Imagination Technologies Ltd.
+ *
+ * Based on arch/sh/oprofile/common.c:
+ *
+ * Copyright (C) 2003 - 2010  Paul Mundt
+ *
+ * Based on arch/mips/oprofile/common.c:
+ *
+ *	Copyright (C) 2004, 2005 Ralf Baechle
+ *	Copyright (C) 2005 MIPS Technologies, Inc.
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ */
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/oprofile.h>
+#include <linux/perf_event.h>
+#include <linux/slab.h>
+
+#include "backtrace.h"
+
+#ifdef CONFIG_HW_PERF_EVENTS
+/*
+ * This will need to be reworked when multiple PMUs are supported.
+ */
+static char *metag_pmu_op_name;
+
+char *op_name_from_perf_id(void)
+{
+	return metag_pmu_op_name;
+}
+
+int __init oprofile_arch_init(struct oprofile_operations *ops)
+{
+	ops->backtrace = metag_backtrace;
+
+	if (perf_num_counters() == 0)
+		return -ENODEV;
+
+	metag_pmu_op_name = kasprintf(GFP_KERNEL, "metag/%s",
+				      perf_pmu_name());
+	if (unlikely(!metag_pmu_op_name))
+		return -ENOMEM;
+
+	return oprofile_perf_init(ops);
+}
+
+void oprofile_arch_exit(void)
+{
+	oprofile_perf_exit();
+	kfree(metag_pmu_op_name);
+}
+#else
+int __init oprofile_arch_init(struct oprofile_operations *ops)
+{
+	ops->backtrace = metag_backtrace;
+	/* fall back to timer interrupt PC sampling */
+	return -ENODEV;
+}
+void oprofile_arch_exit(void) {}
+#endif /* CONFIG_HW_PERF_EVENTS */
diff --git a/arch/microblaze/Kconfig b/arch/microblaze/Kconfig
index 1323fa2530e..54237af0b07 100644
--- a/arch/microblaze/Kconfig
+++ b/arch/microblaze/Kconfig
@@ -26,6 +26,7 @@ config MICROBLAZE
 	select GENERIC_CPU_DEVICES
 	select GENERIC_ATOMIC64
 	select GENERIC_CLOCKEVENTS
+	select GENERIC_IDLE_POLL_SETUP
 	select MODULES_USE_ELF_RELA
 	select CLONE_BACKWARDS
 
@@ -38,9 +39,6 @@ config RWSEM_GENERIC_SPINLOCK
 config ZONE_DMA
 	def_bool y
 
-config ARCH_POPULATES_NODE_MAP
-	def_bool y
-
 config RWSEM_XCHGADD_ALGORITHM
 	bool
 
diff --git a/arch/microblaze/include/asm/processor.h b/arch/microblaze/include/asm/processor.h
index 0759153e811..d6e0ffea28b 100644
--- a/arch/microblaze/include/asm/processor.h
+++ b/arch/microblaze/include/asm/processor.h
@@ -22,7 +22,6 @@
 extern const struct seq_operations cpuinfo_op;
 
 # define cpu_relax()		barrier()
-# define cpu_sleep()		do {} while (0)
 
 #define task_pt_regs(tsk) \
 		(((struct pt_regs *)(THREAD_SIZE + task_stack_page(tsk))) - 1)
@@ -160,10 +159,6 @@ unsigned long get_wchan(struct task_struct *p);
 #  define STACK_TOP	TASK_SIZE
 #  define STACK_TOP_MAX	STACK_TOP
 
-void disable_hlt(void);
-void enable_hlt(void);
-void default_idle(void);
-
 #ifdef CONFIG_DEBUG_FS
 extern struct dentry *of_debugfs_root;
 #endif
diff --git a/arch/microblaze/include/asm/setup.h b/arch/microblaze/include/asm/setup.h
index 0e0b0a5ec75..f05df5630c8 100644
--- a/arch/microblaze/include/asm/setup.h
+++ b/arch/microblaze/include/asm/setup.h
@@ -46,7 +46,6 @@ void machine_shutdown(void);
 void machine_halt(void);
 void machine_power_off(void);
 
-void free_init_pages(char *what, unsigned long begin, unsigned long end);
 extern void *alloc_maybe_bootmem(size_t size, gfp_t mask);
 extern void *zalloc_maybe_bootmem(size_t size, gfp_t mask);
 
diff --git a/arch/microblaze/include/asm/thread_info.h b/arch/microblaze/include/asm/thread_info.h
index 008f30433d2..de26ea6373d 100644
--- a/arch/microblaze/include/asm/thread_info.h
+++ b/arch/microblaze/include/asm/thread_info.h
@@ -182,7 +182,6 @@ static inline bool test_and_clear_restore_sigmask(void)
 	ti->status &= ~TS_RESTORE_SIGMASK;
 	return true;
 }
-#define tsk_is_polling(t) test_tsk_thread_flag(t, TIF_POLLING_NRFLAG)
 #endif
 
 #endif /* __KERNEL__ */
diff --git a/arch/microblaze/kernel/early_printk.c b/arch/microblaze/kernel/early_printk.c
index 60dcacc6803..365f2d53f1b 100644
--- a/arch/microblaze/kernel/early_printk.c
+++ b/arch/microblaze/kernel/early_printk.c
@@ -21,7 +21,6 @@
 #include <asm/setup.h>
 #include <asm/prom.h>
 
-static u32 early_console_initialized;
 static u32 base_addr;
 
 #ifdef CONFIG_SERIAL_UARTLITE_CONSOLE
@@ -109,27 +108,11 @@ static struct console early_serial_uart16550_console = {
 };
 #endif /* CONFIG_SERIAL_8250_CONSOLE */
 
-static struct console *early_console;
-
-void early_printk(const char *fmt, ...)
-{
-	char buf[512];
-	int n;
-	va_list ap;
-
-	if (early_console_initialized) {
-		va_start(ap, fmt);
-		n = vscnprintf(buf, 512, fmt, ap);
-		early_console->write(early_console, buf, n);
-		va_end(ap);
-	}
-}
-
 int __init setup_early_printk(char *opt)
 {
 	int version = 0;
 
-	if (early_console_initialized)
+	if (early_console)
 		return 1;
 
 	base_addr = of_early_console(&version);
@@ -159,7 +142,6 @@ int __init setup_early_printk(char *opt)
 		}
 
 		register_console(early_console);
-		early_console_initialized = 1;
 		return 0;
 	}
 	return 1;
@@ -169,7 +151,7 @@ int __init setup_early_printk(char *opt)
  * only for early console because of performance degression */
 void __init remap_early_printk(void)
 {
-	if (!early_console_initialized || !early_console)
+	if (!early_console)
 		return;
 	pr_info("early_printk_console remapping from 0x%x to ", base_addr);
 	base_addr = (u32) ioremap(base_addr, PAGE_SIZE);
@@ -194,9 +176,9 @@ void __init remap_early_printk(void)
 
 void __init disable_early_printk(void)
 {
-	if (!early_console_initialized || !early_console)
+	if (!early_console)
 		return;
 	pr_warn("disabling early console\n");
 	unregister_console(early_console);
-	early_console_initialized = 0;
+	early_console = NULL;
 }
diff --git a/arch/microblaze/kernel/process.c b/arch/microblaze/kernel/process.c
index fa0ea609137..7cce2e9c171 100644
--- a/arch/microblaze/kernel/process.c
+++ b/arch/microblaze/kernel/process.c
@@ -44,71 +44,6 @@ void show_regs(struct pt_regs *regs)
 void (*pm_power_off)(void) = NULL;
 EXPORT_SYMBOL(pm_power_off);
 
-static int hlt_counter = 1;
-
-void disable_hlt(void)
-{
-	hlt_counter++;
-}
-EXPORT_SYMBOL(disable_hlt);
-
-void enable_hlt(void)
-{
-	hlt_counter--;
-}
-EXPORT_SYMBOL(enable_hlt);
-
-static int __init nohlt_setup(char *__unused)
-{
-	hlt_counter = 1;
-	return 1;
-}
-__setup("nohlt", nohlt_setup);
-
-static int __init hlt_setup(char *__unused)
-{
-	hlt_counter = 0;
-	return 1;
-}
-__setup("hlt", hlt_setup);
-
-void default_idle(void)
-{
-	if (likely(hlt_counter)) {
-		local_irq_disable();
-		stop_critical_timings();
-		cpu_relax();
-		start_critical_timings();
-		local_irq_enable();
-	} else {
-		clear_thread_flag(TIF_POLLING_NRFLAG);
-		smp_mb__after_clear_bit();
-		local_irq_disable();
-		while (!need_resched())
-			cpu_sleep();
-		local_irq_enable();
-		set_thread_flag(TIF_POLLING_NRFLAG);
-	}
-}
-
-void cpu_idle(void)
-{
-	set_thread_flag(TIF_POLLING_NRFLAG);
-
-	/* endless idle loop with no priority at all */
-	while (1) {
-		tick_nohz_idle_enter();
-		rcu_idle_enter();
-		while (!need_resched())
-			default_idle();
-		rcu_idle_exit();
-		tick_nohz_idle_exit();
-
-		schedule_preempt_disabled();
-		check_pgt_cache();
-	}
-}
-
 void flush_thread(void)
 {
 }
diff --git a/arch/microblaze/mm/init.c b/arch/microblaze/mm/init.c
index 8f8b367c079..4ec137d13ad 100644
--- a/arch/microblaze/mm/init.c
+++ b/arch/microblaze/mm/init.c
@@ -82,13 +82,9 @@ static unsigned long highmem_setup(void)
 		/* FIXME not sure about */
 		if (memblock_is_reserved(pfn << PAGE_SHIFT))
 			continue;
-		ClearPageReserved(page);
-		init_page_count(page);
-		__free_page(page);
-		totalhigh_pages++;
+		free_highmem_page(page);
 		reservedpages++;
 	}
-	totalram_pages += totalhigh_pages;
 	pr_info("High memory: %luk\n",
 					totalhigh_pages << (PAGE_SHIFT-10));
 
@@ -236,40 +232,16 @@ void __init setup_memory(void)
 	paging_init();
 }
 
-void free_init_pages(char *what, unsigned long begin, unsigned long end)
-{
-	unsigned long addr;
-
-	for (addr = begin; addr < end; addr += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(addr));
-		init_page_count(virt_to_page(addr));
-		free_page(addr);
-		totalram_pages++;
-	}
-	pr_info("Freeing %s: %ldk freed\n", what, (end - begin) >> 10);
-}
-
 #ifdef CONFIG_BLK_DEV_INITRD
 void free_initrd_mem(unsigned long start, unsigned long end)
 {
-	int pages = 0;
-	for (; start < end; start += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(start));
-		init_page_count(virt_to_page(start));
-		free_page(start);
-		totalram_pages++;
-		pages++;
-	}
-	pr_notice("Freeing initrd memory: %dk freed\n",
-					(int)(pages * (PAGE_SIZE / 1024)));
+	free_reserved_area(start, end, 0, "initrd");
 }
 #endif
 
 void free_initmem(void)
 {
-	free_init_pages("unused kernel memory",
-			(unsigned long)(&__init_begin),
-			(unsigned long)(&__init_end));
+	free_initmem_default(0);
 }
 
 void __init mem_init(void)
diff --git a/arch/mips/Kconfig b/arch/mips/Kconfig
index cd2e21ff562..3a7b3954ce1 100644
--- a/arch/mips/Kconfig
+++ b/arch/mips/Kconfig
@@ -18,7 +18,7 @@ config MIPS
 	select HAVE_KRETPROBES
 	select HAVE_DEBUG_KMEMLEAK
 	select ARCH_BINFMT_ELF_RANDOMIZE_PIE
-	select HAVE_ARCH_TRANSPARENT_HUGEPAGE
+	select HAVE_ARCH_TRANSPARENT_HUGEPAGE if CPU_SUPPORTS_HUGEPAGES && 64BIT
 	select RTC_LIB if !MACH_LOONGSON
 	select GENERIC_ATOMIC64 if !64BIT
 	select ARCH_HAS_ATOMIC64_DEC_IF_POSITIVE
@@ -404,6 +404,8 @@ config PMC_MSP
 	select IRQ_CPU
 	select SERIAL_8250
 	select SERIAL_8250_CONSOLE
+	select USB_EHCI_BIG_ENDIAN_MMIO
+	select USB_EHCI_BIG_ENDIAN_DESC
 	help
 	  This adds support for the PMC-Sierra family of Multi-Service
 	  Processor System-On-A-Chips.  These parts include a number
@@ -657,7 +659,7 @@ config SNI_RM
 	bool "SNI RM200/300/400"
 	select FW_ARC if CPU_LITTLE_ENDIAN
 	select FW_ARC32 if CPU_LITTLE_ENDIAN
-	select SNIPROM if CPU_BIG_ENDIAN
+	select FW_SNIPROM if CPU_BIG_ENDIAN
 	select ARCH_MAY_HAVE_PC_FDC
 	select BOOT_ELF32
 	select CEVT_R4K
@@ -1144,7 +1146,7 @@ config DEFAULT_SGI_PARTITION
 config FW_ARC32
 	bool
 
-config SNIPROM
+config FW_SNIPROM
 	bool
 
 config BOOT_ELF32
@@ -1433,6 +1435,7 @@ config CPU_CAVIUM_OCTEON
 	select CPU_SUPPORTS_HUGEPAGES
 	select LIBFDT
 	select USE_OF
+	select USB_EHCI_BIG_ENDIAN_MMIO
 	help
 	  The Cavium Octeon processor is a highly integrated chip containing
 	  many ethernet hardware widgets for networking tasks. The processor
@@ -1493,7 +1496,6 @@ config CPU_XLP
 	select CPU_SUPPORTS_32BIT_KERNEL
 	select CPU_SUPPORTS_64BIT_KERNEL
 	select CPU_SUPPORTS_HIGHMEM
-	select CPU_HAS_LLSC
 	select WEAK_ORDERING
 	select WEAK_REORDERING_BEYOND_LLSC
 	select CPU_HAS_PREFETCH
diff --git a/arch/mips/bcm63xx/boards/board_bcm963xx.c b/arch/mips/bcm63xx/boards/board_bcm963xx.c
index ed1949c2950..9aa7d44898e 100644
--- a/arch/mips/bcm63xx/boards/board_bcm963xx.c
+++ b/arch/mips/bcm63xx/boards/board_bcm963xx.c
@@ -745,10 +745,7 @@ void __init board_prom_init(void)
 		strcpy(cfe_version, "unknown");
 	printk(KERN_INFO PFX "CFE version: %s\n", cfe_version);
 
-	if (bcm63xx_nvram_init(boot_addr + BCM963XX_NVRAM_OFFSET)) {
-		printk(KERN_ERR PFX "invalid nvram checksum\n");
-		return;
-	}
+	bcm63xx_nvram_init(boot_addr + BCM963XX_NVRAM_OFFSET);
 
 	board_name = bcm63xx_nvram_get_name();
 	/* find board by name */
diff --git a/arch/mips/bcm63xx/dev-spi.c b/arch/mips/bcm63xx/dev-spi.c
index f1c9c3e2f67..e97fd60e92e 100644
--- a/arch/mips/bcm63xx/dev-spi.c
+++ b/arch/mips/bcm63xx/dev-spi.c
@@ -85,20 +85,9 @@ static struct platform_device bcm63xx_spi_device = {
 
 int __init bcm63xx_spi_register(void)
 {
-	struct clk *periph_clk;
-
 	if (BCMCPU_IS_6328() || BCMCPU_IS_6345())
 		return -ENODEV;
 
-	periph_clk = clk_get(NULL, "periph");
-	if (IS_ERR(periph_clk)) {
-		pr_err("unable to get periph clock\n");
-		return -ENODEV;
-	}
-
-	/* Set bus frequency */
-	spi_pdata.speed_hz = clk_get_rate(periph_clk);
-
 	spi_resources[0].start = bcm63xx_regset_address(RSET_SPI);
 	spi_resources[0].end = spi_resources[0].start;
 	spi_resources[1].start = bcm63xx_get_irq_number(IRQ_SPI);
diff --git a/arch/mips/bcm63xx/nvram.c b/arch/mips/bcm63xx/nvram.c
index 62061168083..a4b8864f930 100644
--- a/arch/mips/bcm63xx/nvram.c
+++ b/arch/mips/bcm63xx/nvram.c
@@ -38,7 +38,7 @@ struct bcm963xx_nvram {
 static struct bcm963xx_nvram nvram;
 static int mac_addr_used;
 
-int __init bcm63xx_nvram_init(void *addr)
+void __init bcm63xx_nvram_init(void *addr)
 {
 	unsigned int check_len;
 	u32 crc, expected_crc;
@@ -60,9 +60,8 @@ int __init bcm63xx_nvram_init(void *addr)
 	crc = crc32_le(~0, (u8 *)&nvram, check_len);
 
 	if (crc != expected_crc)
-		return -EINVAL;
-
-	return 0;
+		pr_warn("nvram checksum failed, contents may be invalid (expected %08x, got %08x)\n",
+			expected_crc, crc);
 }
 
 u8 *bcm63xx_nvram_get_name(void)
diff --git a/arch/mips/bcm63xx/setup.c b/arch/mips/bcm63xx/setup.c
index 314231be788..35e18e98beb 100644
--- a/arch/mips/bcm63xx/setup.c
+++ b/arch/mips/bcm63xx/setup.c
@@ -157,4 +157,4 @@ int __init bcm63xx_register_devices(void)
 	return board_register_devices();
 }
 
-device_initcall(bcm63xx_register_devices);
+arch_initcall(bcm63xx_register_devices);
diff --git a/arch/mips/cavium-octeon/setup.c b/arch/mips/cavium-octeon/setup.c
index c594a3d4f74..b0baa299f89 100644
--- a/arch/mips/cavium-octeon/setup.c
+++ b/arch/mips/cavium-octeon/setup.c
@@ -174,7 +174,10 @@ static int octeon_kexec_prepare(struct kimage *image)
 
 static void octeon_generic_shutdown(void)
 {
-	int cpu, i;
+	int i;
+#ifdef CONFIG_SMP
+	int cpu;
+#endif
 	struct cvmx_bootmem_desc *bootmem_desc;
 	void *named_block_array_ptr;
 
diff --git a/arch/mips/include/asm/hugetlb.h b/arch/mips/include/asm/hugetlb.h
index ef99db994c2..fe0d15d3266 100644
--- a/arch/mips/include/asm/hugetlb.h
+++ b/arch/mips/include/asm/hugetlb.h
@@ -10,6 +10,7 @@
 #define __ASM_HUGETLB_H
 
 #include <asm/page.h>
+#include <asm-generic/hugetlb.h>
 
 
 static inline int is_hugepage_only_range(struct mm_struct *mm,
diff --git a/arch/mips/include/asm/mach-bcm63xx/bcm63xx_dev_spi.h b/arch/mips/include/asm/mach-bcm63xx/bcm63xx_dev_spi.h
index c9bae136260..b0184cf0257 100644
--- a/arch/mips/include/asm/mach-bcm63xx/bcm63xx_dev_spi.h
+++ b/arch/mips/include/asm/mach-bcm63xx/bcm63xx_dev_spi.h
@@ -13,7 +13,6 @@ struct bcm63xx_spi_pdata {
 	unsigned int	msg_ctl_width;
 	int		bus_num;
 	int		num_chipselect;
-	u32		speed_hz;
 };
 
 enum bcm63xx_regs_spi {
diff --git a/arch/mips/include/asm/mach-bcm63xx/bcm63xx_nvram.h b/arch/mips/include/asm/mach-bcm63xx/bcm63xx_nvram.h
index 62d6a3b4d3b..4e0b6bc1165 100644
--- a/arch/mips/include/asm/mach-bcm63xx/bcm63xx_nvram.h
+++ b/arch/mips/include/asm/mach-bcm63xx/bcm63xx_nvram.h
@@ -9,10 +9,8 @@
  *
  * Initialized the local nvram copy from the target address and checks
  * its checksum.
- *
- * Returns 0 on success.
  */
-int __init bcm63xx_nvram_init(void *nvram);
+void bcm63xx_nvram_init(void *nvram);
 
 /**
  * bcm63xx_nvram_get_name() - returns the board name according to nvram
diff --git a/arch/mips/include/asm/mach-sead3/cpu-feature-overrides.h b/arch/mips/include/asm/mach-sead3/cpu-feature-overrides.h
index d9c82841903..193c0912d38 100644
--- a/arch/mips/include/asm/mach-sead3/cpu-feature-overrides.h
+++ b/arch/mips/include/asm/mach-sead3/cpu-feature-overrides.h
@@ -28,11 +28,7 @@
 /* #define cpu_has_prefetch	? */
 #define cpu_has_mcheck		1
 /* #define cpu_has_ejtag	? */
-#ifdef CONFIG_CPU_HAS_LLSC
 #define cpu_has_llsc		1
-#else
-#define cpu_has_llsc		0
-#endif
 /* #define cpu_has_vtag_icache	? */
 /* #define cpu_has_dc_aliases	? */
 /* #define cpu_has_ic_fills_f_dc ? */
diff --git a/arch/mips/include/asm/mipsregs.h b/arch/mips/include/asm/mipsregs.h
index 12b70c25906..0da44d422f5 100644
--- a/arch/mips/include/asm/mipsregs.h
+++ b/arch/mips/include/asm/mipsregs.h
@@ -1166,7 +1166,10 @@ do {									\
 	unsigned int __dspctl;						\
 									\
 	__asm__ __volatile__(						\
+	"	.set push					\n"	\
+	"	.set dsp					\n"	\
 	"	rddsp	%0, %x1					\n"	\
+	"	.set pop					\n"	\
 	: "=r" (__dspctl)						\
 	: "i" (mask));							\
 	__dspctl;							\
@@ -1175,30 +1178,198 @@ do {									\
 #define wrdsp(val, mask)						\
 do {									\
 	__asm__ __volatile__(						\
+	"	.set push					\n"	\
+	"	.set dsp					\n"	\
 	"	wrdsp	%0, %x1					\n"	\
+	"	.set pop					\n"	\
 	:								\
 	: "r" (val), "i" (mask));					\
 } while (0)
 
-#define mflo0() ({ long mflo0; __asm__("mflo %0, $ac0" : "=r" (mflo0)); mflo0;})
-#define mflo1() ({ long mflo1; __asm__("mflo %0, $ac1" : "=r" (mflo1)); mflo1;})
-#define mflo2() ({ long mflo2; __asm__("mflo %0, $ac2" : "=r" (mflo2)); mflo2;})
-#define mflo3() ({ long mflo3; __asm__("mflo %0, $ac3" : "=r" (mflo3)); mflo3;})
-
-#define mfhi0() ({ long mfhi0; __asm__("mfhi %0, $ac0" : "=r" (mfhi0)); mfhi0;})
-#define mfhi1() ({ long mfhi1; __asm__("mfhi %0, $ac1" : "=r" (mfhi1)); mfhi1;})
-#define mfhi2() ({ long mfhi2; __asm__("mfhi %0, $ac2" : "=r" (mfhi2)); mfhi2;})
-#define mfhi3() ({ long mfhi3; __asm__("mfhi %0, $ac3" : "=r" (mfhi3)); mfhi3;})
-
-#define mtlo0(x) __asm__("mtlo %0, $ac0" ::"r" (x))
-#define mtlo1(x) __asm__("mtlo %0, $ac1" ::"r" (x))
-#define mtlo2(x) __asm__("mtlo %0, $ac2" ::"r" (x))
-#define mtlo3(x) __asm__("mtlo %0, $ac3" ::"r" (x))
-
-#define mthi0(x) __asm__("mthi %0, $ac0" ::"r" (x))
-#define mthi1(x) __asm__("mthi %0, $ac1" ::"r" (x))
-#define mthi2(x) __asm__("mthi %0, $ac2" ::"r" (x))
-#define mthi3(x) __asm__("mthi %0, $ac3" ::"r" (x))
+#define mflo0()								\
+({									\
+	long mflo0;							\
+	__asm__(							\
+	"	.set push					\n"	\
+	"	.set dsp					\n"	\
+	"	mflo %0, $ac0					\n"	\
+	"	.set pop					\n" 	\
+	: "=r" (mflo0)); 						\
+	mflo0;								\
+})
+
+#define mflo1()								\
+({									\
+	long mflo1;							\
+	__asm__(							\
+	"	.set push					\n"	\
+	"	.set dsp					\n"	\
+	"	mflo %0, $ac1					\n"	\
+	"	.set pop					\n" 	\
+	: "=r" (mflo1)); 						\
+	mflo1;								\
+})
+
+#define mflo2()								\
+({									\
+	long mflo2;							\
+	__asm__(							\
+	"	.set push					\n"	\
+	"	.set dsp					\n"	\
+	"	mflo %0, $ac2					\n"	\
+	"	.set pop					\n" 	\
+	: "=r" (mflo2)); 						\
+	mflo2;								\
+})
+
+#define mflo3()								\
+({									\
+	long mflo3;							\
+	__asm__(							\
+	"	.set push					\n"	\
+	"	.set dsp					\n"	\
+	"	mflo %0, $ac3					\n"	\
+	"	.set pop					\n" 	\
+	: "=r" (mflo3)); 						\
+	mflo3;								\
+})
+
+#define mfhi0()								\
+({									\
+	long mfhi0;							\
+	__asm__(							\
+	"	.set push					\n"	\
+	"	.set dsp					\n"	\
+	"	mfhi %0, $ac0					\n"	\
+	"	.set pop					\n" 	\
+	: "=r" (mfhi0)); 						\
+	mfhi0;								\
+})
+
+#define mfhi1()								\
+({									\
+	long mfhi1;							\
+	__asm__(							\
+	"	.set push					\n"	\
+	"	.set dsp					\n"	\
+	"	mfhi %0, $ac1					\n"	\
+	"	.set pop					\n" 	\
+	: "=r" (mfhi1)); 						\
+	mfhi1;								\
+})
+
+#define mfhi2()								\
+({									\
+	long mfhi2;							\
+	__asm__(							\
+	"	.set push					\n"	\
+	"	.set dsp					\n"	\
+	"	mfhi %0, $ac2					\n"	\
+	"	.set pop					\n" 	\
+	: "=r" (mfhi2)); 						\
+	mfhi2;								\
+})
+
+#define mfhi3()								\
+({									\
+	long mfhi3;							\
+	__asm__(							\
+	"	.set push					\n"	\
+	"	.set dsp					\n"	\
+	"	mfhi %0, $ac3					\n"	\
+	"	.set pop					\n" 	\
+	: "=r" (mfhi3)); 						\
+	mfhi3;								\
+})
+
+
+#define mtlo0(x)							\
+({									\
+	__asm__(							\
+	"	.set push					\n"	\
+	"	.set dsp					\n"	\
+	"	mtlo %0, $ac0					\n"	\
+	"	.set pop					\n"	\
+	:								\
+	: "r" (x));							\
+})
+
+#define mtlo1(x)							\
+({									\
+	__asm__(							\
+	"	.set push					\n"	\
+	"	.set dsp					\n"	\
+	"	mtlo %0, $ac1					\n"	\
+	"	.set pop					\n"	\
+	:								\
+	: "r" (x));							\
+})
+
+#define mtlo2(x)							\
+({									\
+	__asm__(							\
+	"	.set push					\n"	\
+	"	.set dsp					\n"	\
+	"	mtlo %0, $ac2					\n"	\
+	"	.set pop					\n"	\
+	:								\
+	: "r" (x));							\
+})
+
+#define mtlo3(x)							\
+({									\
+	__asm__(							\
+	"	.set push					\n"	\
+	"	.set dsp					\n"	\
+	"	mtlo %0, $ac3					\n"	\
+	"	.set pop					\n"	\
+	:								\
+	: "r" (x));							\
+})
+
+#define mthi0(x)							\
+({									\
+	__asm__(							\
+	"	.set push					\n"	\
+	"	.set dsp					\n"	\
+	"	mthi %0, $ac0					\n"	\
+	"	.set pop					\n"	\
+	:								\
+	: "r" (x));							\
+})
+
+#define mthi1(x)							\
+({									\
+	__asm__(							\
+	"	.set push					\n"	\
+	"	.set dsp					\n"	\
+	"	mthi %0, $ac1					\n"	\
+	"	.set pop					\n"	\
+	:								\
+	: "r" (x));							\
+})
+
+#define mthi2(x)							\
+({									\
+	__asm__(							\
+	"	.set push					\n"	\
+	"	.set dsp					\n"	\
+	"	mthi %0, $ac2					\n"	\
+	"	.set pop					\n"	\
+	:								\
+	: "r" (x));							\
+})
+
+#define mthi3(x)							\
+({									\
+	__asm__(							\
+	"	.set push					\n"	\
+	"	.set dsp					\n"	\
+	"	mthi %0, $ac3					\n"	\
+	"	.set pop					\n"	\
+	:								\
+	: "r" (x));							\
+})
 
 #else
 
diff --git a/arch/mips/include/asm/page.h b/arch/mips/include/asm/page.h
index 99fc547af9d..eab99e536b5 100644
--- a/arch/mips/include/asm/page.h
+++ b/arch/mips/include/asm/page.h
@@ -31,7 +31,7 @@
 #define PAGE_SHIFT	16
 #endif
 #define PAGE_SIZE	(_AC(1,UL) << PAGE_SHIFT)
-#define PAGE_MASK	(~(PAGE_SIZE - 1))
+#define PAGE_MASK	(~((1 << PAGE_SHIFT) - 1))
 
 #ifdef CONFIG_MIPS_HUGE_TLB_SUPPORT
 #define HPAGE_SHIFT	(PAGE_SHIFT + PAGE_SHIFT - 3)
diff --git a/arch/mips/include/asm/signal.h b/arch/mips/include/asm/signal.h
index 197f6367c20..8efe5a9e2c3 100644
--- a/arch/mips/include/asm/signal.h
+++ b/arch/mips/include/asm/signal.h
@@ -21,6 +21,6 @@
 #include <asm/sigcontext.h>
 #include <asm/siginfo.h>
 
-#define __ARCH_HAS_ODD_SIGACTION
+#define __ARCH_HAS_IRIX_SIGACTION
 
 #endif /* _ASM_SIGNAL_H */
diff --git a/arch/mips/include/uapi/asm/signal.h b/arch/mips/include/uapi/asm/signal.h
index d6b18b4d0f3..addb9f556b7 100644
--- a/arch/mips/include/uapi/asm/signal.h
+++ b/arch/mips/include/uapi/asm/signal.h
@@ -72,6 +72,12 @@ typedef unsigned long old_sigset_t;		/* at least 32 bits */
  *
  * SA_ONESHOT and SA_NOMASK are the historical Linux names for the Single
  * Unix names RESETHAND and NODEFER respectively.
+ *
+ * SA_RESTORER used to be defined as 0x04000000 but only the O32 ABI ever
+ * supported its use and no libc was using it, so the entire sa-restorer
+ * functionality was removed with lmo commit 39bffc12c3580ab for 2.5.48
+ * retaining only the SA_RESTORER definition as a reminder to avoid
+ * accidental reuse of the mask bit.
  */
 #define SA_ONSTACK	0x08000000
 #define SA_RESETHAND	0x80000000
@@ -84,8 +90,6 @@ typedef unsigned long old_sigset_t;		/* at least 32 bits */
 #define SA_NOMASK	SA_NODEFER
 #define SA_ONESHOT	SA_RESETHAND
 
-#define SA_RESTORER	0x04000000	/* Only for o32 */
-
 #define MINSIGSTKSZ    2048
 #define SIGSTKSZ       8192
 
diff --git a/arch/mips/kernel/Makefile b/arch/mips/kernel/Makefile
index f81d98f6184..de75fb50562 100644
--- a/arch/mips/kernel/Makefile
+++ b/arch/mips/kernel/Makefile
@@ -100,29 +100,16 @@ obj-$(CONFIG_HW_PERF_EVENTS)	+= perf_event_mipsxx.o
 obj-$(CONFIG_JUMP_LABEL)	+= jump_label.o
 
 #
-# DSP ASE supported for MIPS32 or MIPS64 Release 2 cores only. It is safe
-# to enable DSP assembler support here even if the MIPS Release 2 CPU we
-# are targetting does not support DSP because all code-paths making use of
-# it properly check that the running CPU *actually does* support these
-# instructions.
+# DSP ASE supported for MIPS32 or MIPS64 Release 2 cores only. It is not
+# safe to unconditionnaly use the assembler -mdsp / -mdspr2 switches
+# here because the compiler may use DSP ASE instructions (such as lwx) in
+# code paths where we cannot check that the CPU we are running on supports it.
+# Proper abstraction using HAVE_AS_DSP and macros is done in
+# arch/mips/include/asm/mipsregs.h.
 #
 ifeq ($(CONFIG_CPU_MIPSR2), y)
 CFLAGS_DSP 			= -DHAVE_AS_DSP
 
-#
-# Check if assembler supports DSP ASE
-#
-ifeq ($(call cc-option-yn,-mdsp), y)
-CFLAGS_DSP			+= -mdsp
-endif
-
-#
-# Check if assembler supports DSP ASE Rev2
-#
-ifeq ($(call cc-option-yn,-mdspr2), y)
-CFLAGS_DSP			+= -mdspr2
-endif
-
 CFLAGS_signal.o			= $(CFLAGS_DSP)
 CFLAGS_signal32.o		= $(CFLAGS_DSP)
 CFLAGS_process.o		= $(CFLAGS_DSP)
diff --git a/arch/mips/kernel/cpu-probe.c b/arch/mips/kernel/cpu-probe.c
index 6bfccc227a9..5fe66a0c322 100644
--- a/arch/mips/kernel/cpu-probe.c
+++ b/arch/mips/kernel/cpu-probe.c
@@ -580,6 +580,9 @@ static inline void cpu_probe_legacy(struct cpuinfo_mips *c, unsigned int cpu)
 		c->tlbsize = 48;
 		break;
 	case PRID_IMP_VR41XX:
+		set_isa(c, MIPS_CPU_ISA_III);
+		c->options = R4K_OPTS;
+		c->tlbsize = 32;
 		switch (c->processor_id & 0xf0) {
 		case PRID_REV_VR4111:
 			c->cputype = CPU_VR4111;
@@ -604,6 +607,7 @@ static inline void cpu_probe_legacy(struct cpuinfo_mips *c, unsigned int cpu)
 				__cpu_name[cpu] = "NEC VR4131";
 			} else {
 				c->cputype = CPU_VR4133;
+				c->options |= MIPS_CPU_LLSC;
 				__cpu_name[cpu] = "NEC VR4133";
 			}
 			break;
@@ -613,9 +617,6 @@ static inline void cpu_probe_legacy(struct cpuinfo_mips *c, unsigned int cpu)
 			__cpu_name[cpu] = "NEC Vr41xx";
 			break;
 		}
-		set_isa(c, MIPS_CPU_ISA_III);
-		c->options = R4K_OPTS;
-		c->tlbsize = 32;
 		break;
 	case PRID_IMP_R4300:
 		c->cputype = CPU_R4300;
@@ -1226,10 +1227,8 @@ __cpuinit void cpu_probe(void)
 	if (c->options & MIPS_CPU_FPU) {
 		c->fpu_id = cpu_get_fpu_id();
 
-		if (c->isa_level == MIPS_CPU_ISA_M32R1 ||
-		    c->isa_level == MIPS_CPU_ISA_M32R2 ||
-		    c->isa_level == MIPS_CPU_ISA_M64R1 ||
-		    c->isa_level == MIPS_CPU_ISA_M64R2) {
+		if (c->isa_level & (MIPS_CPU_ISA_M32R1 | MIPS_CPU_ISA_M32R2 |
+				    MIPS_CPU_ISA_M64R1 | MIPS_CPU_ISA_M64R2)) {
 			if (c->fpu_id & MIPS_FPIR_3D)
 				c->ases |= MIPS_ASE_MIPS3D;
 		}
diff --git a/arch/mips/kernel/early_printk.c b/arch/mips/kernel/early_printk.c
index 9e6440eaa45..505cb77d128 100644
--- a/arch/mips/kernel/early_printk.c
+++ b/arch/mips/kernel/early_printk.c
@@ -7,7 +7,9 @@
  * Copyright (C) 2007 MIPS Technologies, Inc.
  *   written by Ralf Baechle (ralf@linux-mips.org)
  */
+#include <linux/kernel.h>
 #include <linux/console.h>
+#include <linux/printk.h>
 #include <linux/init.h>
 
 #include <asm/setup.h>
@@ -24,20 +26,18 @@ static void early_console_write(struct console *con, const char *s, unsigned n)
 	}
 }
 
-static struct console early_console = {
+static struct console early_console_prom = {
 	.name	= "early",
 	.write	= early_console_write,
 	.flags	= CON_PRINTBUFFER | CON_BOOT,
 	.index	= -1
 };
 
-static int early_console_initialized __initdata;
-
 void __init setup_early_printk(void)
 {
-	if (early_console_initialized)
+	if (early_console)
 		return;
-	early_console_initialized = 1;
+	early_console = &early_console_prom;
 
-	register_console(&early_console);
+	register_console(&early_console_prom);
 }
diff --git a/arch/mips/kernel/linux32.c b/arch/mips/kernel/linux32.c
index 8eeee1c860c..db9655f0889 100644
--- a/arch/mips/kernel/linux32.c
+++ b/arch/mips/kernel/linux32.c
@@ -171,7 +171,7 @@ SYSCALL_DEFINE6(32_ipc, u32, call, long, first, long, second, long, third,
 		err = compat_sys_shmctl(first, second, compat_ptr(ptr));
 		break;
 	default:
-		err = -EINVAL;
+		err = -ENOSYS;
 		break;
 	}
 
diff --git a/arch/mips/kernel/mcount.S b/arch/mips/kernel/mcount.S
index 16586767335..33d067148e6 100644
--- a/arch/mips/kernel/mcount.S
+++ b/arch/mips/kernel/mcount.S
@@ -46,10 +46,9 @@
 	PTR_L	a5, PT_R9(sp)
 	PTR_L	a6, PT_R10(sp)
 	PTR_L	a7, PT_R11(sp)
-#else
-	PTR_ADDIU	sp, PT_SIZE
 #endif
-.endm
+	PTR_ADDIU	sp, PT_SIZE
+	.endm
 
 	.macro RETURN_BACK
 	jr ra
@@ -68,7 +67,11 @@ NESTED(ftrace_caller, PT_SIZE, ra)
 	.globl _mcount
 _mcount:
 	b	ftrace_stub
-	addiu sp,sp,8
+#ifdef CONFIG_32BIT
+	 addiu sp,sp,8
+#else
+	 nop
+#endif
 
 	/* When tracing is activated, it calls ftrace_caller+8 (aka here) */
 	lw	t1, function_trace_stop
diff --git a/arch/mips/kernel/proc.c b/arch/mips/kernel/proc.c
index 135c4aadccb..7a54f74b781 100644
--- a/arch/mips/kernel/proc.c
+++ b/arch/mips/kernel/proc.c
@@ -67,7 +67,7 @@ static int show_cpuinfo(struct seq_file *m, void *v)
 	if (cpu_has_mips_r) {
 		seq_printf(m, "isa\t\t\t:");
 		if (cpu_has_mips_1)
-			seq_printf(m, "%s", "mips1");
+			seq_printf(m, "%s", " mips1");
 		if (cpu_has_mips_2)
 			seq_printf(m, "%s", " mips2");
 		if (cpu_has_mips_3)
diff --git a/arch/mips/kernel/process.c b/arch/mips/kernel/process.c
index 3be4405c2d1..cfc742d75b7 100644
--- a/arch/mips/kernel/process.c
+++ b/arch/mips/kernel/process.c
@@ -41,44 +41,26 @@
 #include <asm/inst.h>
 #include <asm/stacktrace.h>
 
-/*
- * The idle thread. There's no useful work to be done, so just try to conserve
- * power and have a low exit latency (ie sit in a loop waiting for somebody to
- * say that they'd like to reschedule)
- */
-void __noreturn cpu_idle(void)
+#ifdef CONFIG_HOTPLUG_CPU
+void arch_cpu_idle_dead(void)
 {
-	int cpu;
-
-	/* CPU is going idle. */
-	cpu = smp_processor_id();
+	/* What the heck is this check doing ? */
+	if (!cpu_isset(smp_processor_id(), cpu_callin_map))
+		play_dead();
+}
+#endif
 
-	/* endless idle loop with no priority at all */
-	while (1) {
-		tick_nohz_idle_enter();
-		rcu_idle_enter();
-		while (!need_resched() && cpu_online(cpu)) {
+void arch_cpu_idle(void)
+{
 #ifdef CONFIG_MIPS_MT_SMTC
-			extern void smtc_idle_loop_hook(void);
+	extern void smtc_idle_loop_hook(void);
 
-			smtc_idle_loop_hook();
+	smtc_idle_loop_hook();
 #endif
-
-			if (cpu_wait) {
-				/* Don't trace irqs off for idle */
-				stop_critical_timings();
-				(*cpu_wait)();
-				start_critical_timings();
-			}
-		}
-#ifdef CONFIG_HOTPLUG_CPU
-		if (!cpu_online(cpu) && !cpu_isset(cpu, cpu_callin_map))
-			play_dead();
-#endif
-		rcu_idle_exit();
-		tick_nohz_idle_exit();
-		schedule_preempt_disabled();
-	}
+	if (cpu_wait)
+		(*cpu_wait)();
+	else
+		local_irq_enable();
 }
 
 asmlinkage void ret_from_fork(void);
diff --git a/arch/mips/kernel/smp.c b/arch/mips/kernel/smp.c
index 66bf4e22d9b..aee04af213c 100644
--- a/arch/mips/kernel/smp.c
+++ b/arch/mips/kernel/smp.c
@@ -139,7 +139,7 @@ asmlinkage __cpuinit void start_secondary(void)
 	WARN_ON_ONCE(!irqs_disabled());
 	mp_ops->smp_finish();
 
-	cpu_idle();
+	cpu_startup_entry(CPUHP_ONLINE);
 }
 
 /*
diff --git a/arch/mips/kernel/traps.c b/arch/mips/kernel/traps.c
index a200b5bdbb8..c3abb88170f 100644
--- a/arch/mips/kernel/traps.c
+++ b/arch/mips/kernel/traps.c
@@ -1571,7 +1571,7 @@ void __cpuinit per_cpu_trap_init(bool is_boot_cpu)
 #ifdef CONFIG_64BIT
 	status_set |= ST0_FR|ST0_KX|ST0_SX|ST0_UX;
 #endif
-	if (current_cpu_data.isa_level == MIPS_CPU_ISA_IV)
+	if (current_cpu_data.isa_level & MIPS_CPU_ISA_IV)
 		status_set |= ST0_XX;
 	if (cpu_has_dsp)
 		status_set |= ST0_MX;
diff --git a/arch/mips/lib/bitops.c b/arch/mips/lib/bitops.c
index 81f1dcfdcab..a64daee740e 100644
--- a/arch/mips/lib/bitops.c
+++ b/arch/mips/lib/bitops.c
@@ -90,12 +90,12 @@ int __mips_test_and_set_bit(unsigned long nr,
 	unsigned bit = nr & SZLONG_MASK;
 	unsigned long mask;
 	unsigned long flags;
-	unsigned long res;
+	int res;
 
 	a += nr >> SZLONG_LOG;
 	mask = 1UL << bit;
 	raw_local_irq_save(flags);
-	res = (mask & *a);
+	res = (mask & *a) != 0;
 	*a |= mask;
 	raw_local_irq_restore(flags);
 	return res;
@@ -116,12 +116,12 @@ int __mips_test_and_set_bit_lock(unsigned long nr,
 	unsigned bit = nr & SZLONG_MASK;
 	unsigned long mask;
 	unsigned long flags;
-	unsigned long res;
+	int res;
 
 	a += nr >> SZLONG_LOG;
 	mask = 1UL << bit;
 	raw_local_irq_save(flags);
-	res = (mask & *a);
+	res = (mask & *a) != 0;
 	*a |= mask;
 	raw_local_irq_restore(flags);
 	return res;
@@ -141,12 +141,12 @@ int __mips_test_and_clear_bit(unsigned long nr, volatile unsigned long *addr)
 	unsigned bit = nr & SZLONG_MASK;
 	unsigned long mask;
 	unsigned long flags;
-	unsigned long res;
+	int res;
 
 	a += nr >> SZLONG_LOG;
 	mask = 1UL << bit;
 	raw_local_irq_save(flags);
-	res = (mask & *a);
+	res = (mask & *a) != 0;
 	*a &= ~mask;
 	raw_local_irq_restore(flags);
 	return res;
@@ -166,12 +166,12 @@ int __mips_test_and_change_bit(unsigned long nr, volatile unsigned long *addr)
 	unsigned bit = nr & SZLONG_MASK;
 	unsigned long mask;
 	unsigned long flags;
-	unsigned long res;
+	int res;
 
 	a += nr >> SZLONG_LOG;
 	mask = 1UL << bit;
 	raw_local_irq_save(flags);
-	res = (mask & *a);
+	res = (mask & *a) != 0;
 	*a ^= mask;
 	raw_local_irq_restore(flags);
 	return res;
diff --git a/arch/mips/lib/csum_partial.S b/arch/mips/lib/csum_partial.S
index 507147aebd4..a6adffbb4e5 100644
--- a/arch/mips/lib/csum_partial.S
+++ b/arch/mips/lib/csum_partial.S
@@ -270,7 +270,7 @@ LEAF(csum_partial)
 #endif
 
 	/* odd buffer alignment? */
-#ifdef CPU_MIPSR2
+#ifdef CONFIG_CPU_MIPSR2
 	wsbh	v1, sum
 	movn	sum, v1, t7
 #else
@@ -670,7 +670,7 @@ EXC(	sb	t0, NBYTES-2(dst), .Ls_exc)
 	addu	sum, v1
 #endif
 
-#ifdef CPU_MIPSR2
+#ifdef CONFIG_CPU_MIPSR2
 	wsbh	v1, sum
 	movn	sum, v1, odd
 #else
diff --git a/arch/mips/mm/c-r4k.c b/arch/mips/mm/c-r4k.c
index ecca559b8d7..2078915eacb 100644
--- a/arch/mips/mm/c-r4k.c
+++ b/arch/mips/mm/c-r4k.c
@@ -1247,10 +1247,8 @@ static void __cpuinit setup_scache(void)
 		return;
 
 	default:
-		if (c->isa_level == MIPS_CPU_ISA_M32R1 ||
-		    c->isa_level == MIPS_CPU_ISA_M32R2 ||
-		    c->isa_level == MIPS_CPU_ISA_M64R1 ||
-		    c->isa_level == MIPS_CPU_ISA_M64R2) {
+		if (c->isa_level & (MIPS_CPU_ISA_M32R1 | MIPS_CPU_ISA_M32R2 |
+				    MIPS_CPU_ISA_M64R1 | MIPS_CPU_ISA_M64R2)) {
 #ifdef CONFIG_MIPS_CPU_SCACHE
 			if (mips_sc_init ()) {
 				scache_size = c->scache.ways * c->scache.sets * c->scache.linesz;
diff --git a/arch/mips/mm/init.c b/arch/mips/mm/init.c
index 67929251286..3d0346dbccf 100644
--- a/arch/mips/mm/init.c
+++ b/arch/mips/mm/init.c
@@ -77,10 +77,9 @@ EXPORT_SYMBOL_GPL(empty_zero_page);
 /*
  * Not static inline because used by IP27 special magic initialization code
  */
-unsigned long setup_zero_pages(void)
+void setup_zero_pages(void)
 {
-	unsigned int order;
-	unsigned long size;
+	unsigned int order, i;
 	struct page *page;
 
 	if (cpu_has_vce)
@@ -94,15 +93,10 @@ unsigned long setup_zero_pages(void)
 
 	page = virt_to_page((void *)empty_zero_page);
 	split_page(page, order);
-	while (page < virt_to_page((void *)(empty_zero_page + (PAGE_SIZE << order)))) {
-		SetPageReserved(page);
-		page++;
-	}
-
-	size = PAGE_SIZE << order;
-	zero_page_mask = (size - 1) & PAGE_MASK;
+	for (i = 0; i < (1 << order); i++, page++)
+		mark_page_reserved(page);
 
-	return 1UL << order;
+	zero_page_mask = ((PAGE_SIZE << order) - 1) & PAGE_MASK;
 }
 
 #ifdef CONFIG_MIPS_MT_SMTC
@@ -380,7 +374,7 @@ void __init mem_init(void)
 	high_memory = (void *) __va(max_low_pfn << PAGE_SHIFT);
 
 	totalram_pages += free_all_bootmem();
-	totalram_pages -= setup_zero_pages();	/* Setup zeroed pages.	*/
+	setup_zero_pages();	/* Setup zeroed pages.  */
 
 	reservedpages = ram = 0;
 	for (tmp = 0; tmp < max_low_pfn; tmp++)
@@ -399,12 +393,8 @@ void __init mem_init(void)
 			SetPageReserved(page);
 			continue;
 		}
-		ClearPageReserved(page);
-		init_page_count(page);
-		__free_page(page);
-		totalhigh_pages++;
+		free_highmem_page(page);
 	}
-	totalram_pages += totalhigh_pages;
 	num_physpages += totalhigh_pages;
 #endif
 
@@ -440,11 +430,8 @@ void free_init_pages(const char *what, unsigned long begin, unsigned long end)
 		struct page *page = pfn_to_page(pfn);
 		void *addr = phys_to_virt(PFN_PHYS(pfn));
 
-		ClearPageReserved(page);
-		init_page_count(page);
 		memset(addr, POISON_FREE_INITMEM, PAGE_SIZE);
-		__free_page(page);
-		totalram_pages++;
+		free_reserved_page(page);
 	}
 	printk(KERN_INFO "Freeing %s: %ldk freed\n", what, (end - begin) >> 10);
 }
@@ -452,18 +439,14 @@ void free_init_pages(const char *what, unsigned long begin, unsigned long end)
 #ifdef CONFIG_BLK_DEV_INITRD
 void free_initrd_mem(unsigned long start, unsigned long end)
 {
-	free_init_pages("initrd memory",
-			virt_to_phys((void *)start),
-			virt_to_phys((void *)end));
+	free_reserved_area(start, end, POISON_FREE_INITMEM, "initrd");
 }
 #endif
 
 void __init_refok free_initmem(void)
 {
 	prom_free_prom_memory();
-	free_init_pages("unused kernel memory",
-			__pa_symbol(&__init_begin),
-			__pa_symbol(&__init_end));
+	free_initmem_default(POISON_FREE_INITMEM);
 }
 
 #ifndef CONFIG_MIPS_PGD_C0_CONTEXT
diff --git a/arch/mips/mm/sc-mips.c b/arch/mips/mm/sc-mips.c
index 93d937b4b1b..df96da7e939 100644
--- a/arch/mips/mm/sc-mips.c
+++ b/arch/mips/mm/sc-mips.c
@@ -98,10 +98,8 @@ static inline int __init mips_sc_probe(void)
 	c->scache.flags |= MIPS_CACHE_NOT_PRESENT;
 
 	/* Ignore anything but MIPSxx processors */
-	if (c->isa_level != MIPS_CPU_ISA_M32R1 &&
-	    c->isa_level != MIPS_CPU_ISA_M32R2 &&
-	    c->isa_level != MIPS_CPU_ISA_M64R1 &&
-	    c->isa_level != MIPS_CPU_ISA_M64R2)
+	if (!(c->isa_level & (MIPS_CPU_ISA_M32R1 | MIPS_CPU_ISA_M32R2 |
+			      MIPS_CPU_ISA_M64R1 | MIPS_CPU_ISA_M64R2)))
 		return 0;
 
 	/* Does this MIPS32/MIPS64 CPU have a config2 register? */
diff --git a/arch/mips/pci/pci-alchemy.c b/arch/mips/pci/pci-alchemy.c
index 38a80c83fd6..d1faece21b6 100644
--- a/arch/mips/pci/pci-alchemy.c
+++ b/arch/mips/pci/pci-alchemy.c
@@ -19,7 +19,7 @@
 #include <asm/mach-au1x00/au1000.h>
 #include <asm/tlbmisc.h>
 
-#ifdef CONFIG_DEBUG_PCI
+#ifdef CONFIG_PCI_DEBUG
 #define DBG(x...) printk(KERN_DEBUG x)
 #else
 #define DBG(x...) do {} while (0)
@@ -162,7 +162,7 @@ static int config_access(unsigned char access_type, struct pci_bus *bus,
 	if (status & (1 << 29)) {
 		*data = 0xffffffff;
 		error = -1;
-		DBG("alchemy-pci: master abort on cfg access %d bus %d dev %d",
+		DBG("alchemy-pci: master abort on cfg access %d bus %d dev %d\n",
 		    access_type, bus->number, device);
 	} else if ((status >> 28) & 0xf) {
 		DBG("alchemy-pci: PCI ERR detected: dev %d, status %lx\n",
diff --git a/arch/mips/pci/pci.c b/arch/mips/pci/pci.c
index 0872f12f268..594e60d6a43 100644
--- a/arch/mips/pci/pci.c
+++ b/arch/mips/pci/pci.c
@@ -115,7 +115,6 @@ static void pcibios_scanbus(struct pci_controller *hose)
 			pci_bus_assign_resources(bus);
 			pci_enable_bridges(bus);
 		}
-		bus->dev.of_node = hose->of_node;
 	}
 }
 
@@ -169,6 +168,13 @@ void pci_load_of_ranges(struct pci_controller *hose, struct device_node *node)
 		}
 	}
 }
+
+struct device_node *pcibios_get_phb_of_node(struct pci_bus *bus)
+{
+	struct pci_controller *hose = bus->sysdata;
+
+	return of_node_get(hose->of_node);
+}
 #endif
 
 static DEFINE_MUTEX(pci_scan_mutex);
diff --git a/arch/mips/sgi-ip27/ip27-memory.c b/arch/mips/sgi-ip27/ip27-memory.c
index 3505d08ff2f..5f2bddb1860 100644
--- a/arch/mips/sgi-ip27/ip27-memory.c
+++ b/arch/mips/sgi-ip27/ip27-memory.c
@@ -457,7 +457,7 @@ void __init prom_free_prom_memory(void)
 	/* We got nothing to free here ...  */
 }
 
-extern unsigned long setup_zero_pages(void);
+extern void setup_zero_pages(void);
 
 void __init paging_init(void)
 {
@@ -492,7 +492,7 @@ void __init mem_init(void)
 		totalram_pages += free_all_bootmem_node(NODE_DATA(node));
 	}
 
-	totalram_pages -= setup_zero_pages();	/* This comes from node 0 */
+	setup_zero_pages();	/* This comes from node 0 */
 
 	codesize =  (unsigned long) &_etext - (unsigned long) &_text;
 	datasize =  (unsigned long) &_edata - (unsigned long) &_etext;
diff --git a/arch/mn10300/include/asm/thread_info.h b/arch/mn10300/include/asm/thread_info.h
index f90062b0622..224b4262486 100644
--- a/arch/mn10300/include/asm/thread_info.h
+++ b/arch/mn10300/include/asm/thread_info.h
@@ -165,8 +165,6 @@ void arch_release_thread_info(struct thread_info *ti);
 #define _TIF_WORK_MASK		0x0000FFFE	/* work to do on interrupt/exception return */
 #define _TIF_ALLWORK_MASK	0x0000FFFF	/* work to do on any return to u-space */
 
-#define tsk_is_polling(t) test_tsk_thread_flag(t, TIF_POLLING_NRFLAG)
-
 #endif /* __KERNEL__ */
 
 #endif /* _ASM_THREAD_INFO_H */
diff --git a/arch/mn10300/kernel/process.c b/arch/mn10300/kernel/process.c
index 84f4e97e307..2da39fb8b3b 100644
--- a/arch/mn10300/kernel/process.c
+++ b/arch/mn10300/kernel/process.c
@@ -50,77 +50,19 @@ unsigned long thread_saved_pc(struct task_struct *tsk)
 void (*pm_power_off)(void);
 EXPORT_SYMBOL(pm_power_off);
 
-#if !defined(CONFIG_SMP) || defined(CONFIG_HOTPLUG_CPU)
-/*
- * we use this if we don't have any better idle routine
- */
-static void default_idle(void)
-{
-	local_irq_disable();
-	if (!need_resched())
-		safe_halt();
-	else
-		local_irq_enable();
-}
-
-#else /* !CONFIG_SMP || CONFIG_HOTPLUG_CPU  */
 /*
  * On SMP it's slightly faster (but much more power-consuming!)
  * to poll the ->work.need_resched flag instead of waiting for the
  * cross-CPU IPI to arrive. Use this option with caution.
+ *
+ * tglx: No idea why this depends on HOTPLUG_CPU !?!
  */
-static inline void poll_idle(void)
-{
-	int oldval;
-
-	local_irq_enable();
-
-	/*
-	 * Deal with another CPU just having chosen a thread to
-	 * run here:
-	 */
-	oldval = test_and_clear_thread_flag(TIF_NEED_RESCHED);
-
-	if (!oldval) {
-		set_thread_flag(TIF_POLLING_NRFLAG);
-		while (!need_resched())
-			cpu_relax();
-		clear_thread_flag(TIF_POLLING_NRFLAG);
-	} else {
-		set_need_resched();
-	}
-}
-#endif /* !CONFIG_SMP || CONFIG_HOTPLUG_CPU */
-
-/*
- * the idle thread
- * - there's no useful work to be done, so just try to conserve power and have
- *   a low exit latency (ie sit in a loop waiting for somebody to say that
- *   they'd like to reschedule)
- */
-void cpu_idle(void)
+#if !defined(CONFIG_SMP) || defined(CONFIG_HOTPLUG_CPU)
+void arch_cpu_idle(void)
 {
-	/* endless idle loop with no priority at all */
-	for (;;) {
-		rcu_idle_enter();
-		while (!need_resched()) {
-			void (*idle)(void);
-
-			smp_rmb();
-			if (!idle) {
-#if defined(CONFIG_SMP) && !defined(CONFIG_HOTPLUG_CPU)
-				idle = poll_idle;
-#else  /* CONFIG_SMP && !CONFIG_HOTPLUG_CPU */
-				idle = default_idle;
-#endif /* CONFIG_SMP && !CONFIG_HOTPLUG_CPU */
-			}
-			idle();
-		}
-		rcu_idle_exit();
-
-		schedule_preempt_disabled();
-	}
+	safe_halt();
 }
+#endif
 
 void release_segments(struct mm_struct *mm)
 {
diff --git a/arch/mn10300/kernel/smp.c b/arch/mn10300/kernel/smp.c
index 5d7e152a23b..a17f9c9c14c 100644
--- a/arch/mn10300/kernel/smp.c
+++ b/arch/mn10300/kernel/smp.c
@@ -675,7 +675,7 @@ int __init start_secondary(void *unused)
 #ifdef CONFIG_GENERIC_CLOCKEVENTS
 	init_clockevents();
 #endif
-	cpu_idle();
+	cpu_startup_entry(CPUHP_ONLINE);
 	return 0;
 }
 
@@ -935,8 +935,6 @@ int __cpu_up(unsigned int cpu, struct task_struct *tidle)
 	int timeout;
 
 #ifdef CONFIG_HOTPLUG_CPU
-	if (num_online_cpus() == 1)
-		disable_hlt();
 	if (sleep_mode[cpu])
 		run_wakeup_cpu(cpu);
 #endif /* CONFIG_HOTPLUG_CPU */
@@ -1003,9 +1001,6 @@ int __cpu_disable(void)
 void __cpu_die(unsigned int cpu)
 {
 	run_sleep_cpu(cpu);
-
-	if (num_online_cpus() == 1)
-		enable_hlt();
 }
 
 #ifdef CONFIG_MN10300_CACHE_ENABLED
diff --git a/arch/mn10300/mm/init.c b/arch/mn10300/mm/init.c
index e57e5bc2356..5a8ace63a6b 100644
--- a/arch/mn10300/mm/init.c
+++ b/arch/mn10300/mm/init.c
@@ -139,30 +139,11 @@ void __init mem_init(void)
 }
 
 /*
- *
- */
-void free_init_pages(char *what, unsigned long begin, unsigned long end)
-{
-	unsigned long addr;
-
-	for (addr = begin; addr < end; addr += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(addr));
-		init_page_count(virt_to_page(addr));
-		memset((void *) addr, 0xcc, PAGE_SIZE);
-		free_page(addr);
-		totalram_pages++;
-	}
-	printk(KERN_INFO "Freeing %s: %ldk freed\n", what, (end - begin) >> 10);
-}
-
-/*
  * recycle memory containing stuff only required for initialisation
  */
 void free_initmem(void)
 {
-	free_init_pages("unused kernel memory",
-			(unsigned long) &__init_begin,
-			(unsigned long) &__init_end);
+	free_initmem_default(POISON_FREE_INITMEM);
 }
 
 /*
@@ -171,6 +152,6 @@ void free_initmem(void)
 #ifdef CONFIG_BLK_DEV_INITRD
 void free_initrd_mem(unsigned long start, unsigned long end)
 {
-	free_init_pages("initrd memory", start, end);
+	free_reserved_area(start, end, POISON_FREE_INITMEM, "initrd");
 }
 #endif
diff --git a/arch/openrisc/include/asm/thread_info.h b/arch/openrisc/include/asm/thread_info.h
index 07f3212422a..d797acc901e 100644
--- a/arch/openrisc/include/asm/thread_info.h
+++ b/arch/openrisc/include/asm/thread_info.h
@@ -128,8 +128,6 @@ register struct thread_info *current_thread_info_reg asm("r10");
 /* For OpenRISC, this is anything in the LSW other than syscall trace */
 #define _TIF_WORK_MASK (0xff & ~(_TIF_SYSCALL_TRACE|_TIF_SINGLESTEP))
 
-#define tsk_is_polling(t) test_tsk_thread_flag(t, TIF_POLLING_NRFLAG)
-
 #endif /* __KERNEL__ */
 
 #endif /* _ASM_THREAD_INFO_H */
diff --git a/arch/openrisc/kernel/Makefile b/arch/openrisc/kernel/Makefile
index 35f92ce51c2..ec6d9d37cef 100644
--- a/arch/openrisc/kernel/Makefile
+++ b/arch/openrisc/kernel/Makefile
@@ -4,7 +4,7 @@
 
 extra-y	:= head.o vmlinux.lds
 
-obj-y	:= setup.o idle.o or32_ksyms.o process.o dma.o \
+obj-y	:= setup.o or32_ksyms.o process.o dma.o \
 	   traps.o time.o irq.o entry.o ptrace.o signal.o \
 	   sys_call_table.o
 
diff --git a/arch/openrisc/kernel/idle.c b/arch/openrisc/kernel/idle.c
deleted file mode 100644
index 5e8a3b6d6bc..00000000000
--- a/arch/openrisc/kernel/idle.c
+++ /dev/null
@@ -1,73 +0,0 @@
-/*
- * OpenRISC idle.c
- *
- * Linux architectural port borrowing liberally from similar works of
- * others.  All original copyrights apply as per the original source
- * declaration.
- *
- * Modifications for the OpenRISC architecture:
- * Copyright (C) 2003 Matjaz Breskvar <phoenix@bsemi.com>
- * Copyright (C) 2010-2011 Jonas Bonn <jonas@southpole.se>
- *
- *      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.
- *
- * Idle daemon for or32.  Idle daemon will handle any action
- * that needs to be taken when the system becomes idle.
- */
-
-#include <linux/errno.h>
-#include <linux/sched.h>
-#include <linux/kernel.h>
-#include <linux/mm.h>
-#include <linux/smp.h>
-#include <linux/stddef.h>
-#include <linux/unistd.h>
-#include <linux/ptrace.h>
-#include <linux/slab.h>
-#include <linux/tick.h>
-
-#include <asm/pgtable.h>
-#include <asm/uaccess.h>
-#include <asm/io.h>
-#include <asm/processor.h>
-#include <asm/mmu.h>
-#include <asm/cache.h>
-#include <asm/pgalloc.h>
-
-void (*powersave) (void) = NULL;
-
-void cpu_idle(void)
-{
-	set_thread_flag(TIF_POLLING_NRFLAG);
-
-	/* endless idle loop with no priority at all */
-	while (1) {
-		tick_nohz_idle_enter();
-		rcu_idle_enter();
-
-		while (!need_resched()) {
-			check_pgt_cache();
-			rmb();
-
-			clear_thread_flag(TIF_POLLING_NRFLAG);
-
-			local_irq_disable();
-			/* Don't trace irqs off for idle */
-			stop_critical_timings();
-			if (!need_resched() && powersave != NULL)
-				powersave();
-			start_critical_timings();
-			local_irq_enable();
-			set_thread_flag(TIF_POLLING_NRFLAG);
-		}
-
-		rcu_idle_exit();
-		tick_nohz_idle_exit();
-		preempt_enable_no_resched();
-		schedule();
-		preempt_disable();
-	}
-}
diff --git a/arch/openrisc/mm/init.c b/arch/openrisc/mm/init.c
index e7fdc50c4bf..b3cbc670383 100644
--- a/arch/openrisc/mm/init.c
+++ b/arch/openrisc/mm/init.c
@@ -43,6 +43,7 @@
 #include <asm/kmap_types.h>
 #include <asm/fixmap.h>
 #include <asm/tlbflush.h>
+#include <asm/sections.h>
 
 int mem_init_done;
 
@@ -201,9 +202,6 @@ void __init paging_init(void)
 
 /* References to section boundaries */
 
-extern char _stext, _etext, _edata, __bss_start, _end;
-extern char __init_begin, __init_end;
-
 static int __init free_pages_init(void)
 {
 	int reservedpages, pfn;
@@ -263,30 +261,11 @@ void __init mem_init(void)
 #ifdef CONFIG_BLK_DEV_INITRD
 void free_initrd_mem(unsigned long start, unsigned long end)
 {
-	printk(KERN_INFO "Freeing initrd memory: %ldk freed\n",
-	       (end - start) >> 10);
-
-	for (; start < end; start += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(start));
-		init_page_count(virt_to_page(start));
-		free_page(start);
-		totalram_pages++;
-	}
+	free_reserved_area(start, end, 0, "initrd");
 }
 #endif
 
 void free_initmem(void)
 {
-	unsigned long addr;
-
-	addr = (unsigned long)(&__init_begin);
-	for (; addr < (unsigned long)(&__init_end); addr += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(addr));
-		init_page_count(virt_to_page(addr));
-		free_page(addr);
-		totalram_pages++;
-	}
-	printk(KERN_INFO "Freeing unused kernel memory: %luk freed\n",
-	       ((unsigned long)&__init_end -
-		(unsigned long)&__init_begin) >> 10);
+	free_initmem_default(0);
 }
diff --git a/arch/parisc/Makefile b/arch/parisc/Makefile
index 01d95e2f058..113e2820650 100644
--- a/arch/parisc/Makefile
+++ b/arch/parisc/Makefile
@@ -65,8 +65,10 @@ ifndef CONFIG_FUNCTION_TRACER
 endif
 
 # Use long jumps instead of long branches (needed if your linker fails to
-# link a too big vmlinux executable)
-cflags-$(CONFIG_MLONGCALLS)	+= -mlong-calls
+# link a too big vmlinux executable). Not enabled for building modules.
+ifdef CONFIG_MLONGCALLS
+KBUILD_CFLAGS_KERNEL += -mlong-calls
+endif
 
 # select which processor to optimise for
 cflags-$(CONFIG_PA7100)		+= -march=1.1 -mschedule=7100
diff --git a/arch/parisc/include/asm/cacheflush.h b/arch/parisc/include/asm/cacheflush.h
index 79f694f3ad9..f0e2784e7cc 100644
--- a/arch/parisc/include/asm/cacheflush.h
+++ b/arch/parisc/include/asm/cacheflush.h
@@ -140,7 +140,10 @@ static inline void *kmap(struct page *page)
 	return page_address(page);
 }
 
-#define kunmap(page)			kunmap_parisc(page_address(page))
+static inline void kunmap(struct page *page)
+{
+	kunmap_parisc(page_address(page));
+}
 
 static inline void *kmap_atomic(struct page *page)
 {
diff --git a/arch/parisc/include/asm/pgtable.h b/arch/parisc/include/asm/pgtable.h
index 7df49fad29f..1e40d7f86be 100644
--- a/arch/parisc/include/asm/pgtable.h
+++ b/arch/parisc/include/asm/pgtable.h
@@ -16,6 +16,8 @@
 #include <asm/processor.h>
 #include <asm/cache.h>
 
+extern spinlock_t pa_dbit_lock;
+
 /*
  * kern_addr_valid(ADDR) tests if ADDR is pointing to valid kernel
  * memory.  For the return value to be meaningful, ADDR must be >=
@@ -44,8 +46,11 @@ extern void purge_tlb_entries(struct mm_struct *, unsigned long);
 
 #define set_pte_at(mm, addr, ptep, pteval)                      \
 	do {                                                    \
+		unsigned long flags;				\
+		spin_lock_irqsave(&pa_dbit_lock, flags);	\
 		set_pte(ptep, pteval);                          \
 		purge_tlb_entries(mm, addr);                    \
+		spin_unlock_irqrestore(&pa_dbit_lock, flags);	\
 	} while (0)
 
 #endif /* !__ASSEMBLY__ */
@@ -435,48 +440,46 @@ extern void update_mmu_cache(struct vm_area_struct *, unsigned long, pte_t *);
 
 static inline int ptep_test_and_clear_young(struct vm_area_struct *vma, unsigned long addr, pte_t *ptep)
 {
-#ifdef CONFIG_SMP
+	pte_t pte;
+	unsigned long flags;
+
 	if (!pte_young(*ptep))
 		return 0;
-	return test_and_clear_bit(xlate_pabit(_PAGE_ACCESSED_BIT), &pte_val(*ptep));
-#else
-	pte_t pte = *ptep;
-	if (!pte_young(pte))
+
+	spin_lock_irqsave(&pa_dbit_lock, flags);
+	pte = *ptep;
+	if (!pte_young(pte)) {
+		spin_unlock_irqrestore(&pa_dbit_lock, flags);
 		return 0;
-	set_pte_at(vma->vm_mm, addr, ptep, pte_mkold(pte));
+	}
+	set_pte(ptep, pte_mkold(pte));
+	purge_tlb_entries(vma->vm_mm, addr);
+	spin_unlock_irqrestore(&pa_dbit_lock, flags);
 	return 1;
-#endif
 }
 
-extern spinlock_t pa_dbit_lock;
-
 struct mm_struct;
 static inline pte_t ptep_get_and_clear(struct mm_struct *mm, unsigned long addr, pte_t *ptep)
 {
 	pte_t old_pte;
+	unsigned long flags;
 
-	spin_lock(&pa_dbit_lock);
+	spin_lock_irqsave(&pa_dbit_lock, flags);
 	old_pte = *ptep;
 	pte_clear(mm,addr,ptep);
-	spin_unlock(&pa_dbit_lock);
+	purge_tlb_entries(mm, addr);
+	spin_unlock_irqrestore(&pa_dbit_lock, flags);
 
 	return old_pte;
 }
 
 static inline void ptep_set_wrprotect(struct mm_struct *mm, unsigned long addr, pte_t *ptep)
 {
-#ifdef CONFIG_SMP
-	unsigned long new, old;
-
-	do {
-		old = pte_val(*ptep);
-		new = pte_val(pte_wrprotect(__pte (old)));
-	} while (cmpxchg((unsigned long *) ptep, old, new) != old);
+	unsigned long flags;
+	spin_lock_irqsave(&pa_dbit_lock, flags);
+	set_pte(ptep, pte_wrprotect(*ptep));
 	purge_tlb_entries(mm, addr);
-#else
-	pte_t old_pte = *ptep;
-	set_pte_at(mm, addr, ptep, pte_wrprotect(old_pte));
-#endif
+	spin_unlock_irqrestore(&pa_dbit_lock, flags);
 }
 
 #define pte_same(A,B)	(pte_val(A) == pte_val(B))
diff --git a/arch/parisc/include/asm/thread_info.h b/arch/parisc/include/asm/thread_info.h
index d1fb79a36f3..6182832e5b6 100644
--- a/arch/parisc/include/asm/thread_info.h
+++ b/arch/parisc/include/asm/thread_info.h
@@ -77,8 +77,6 @@ struct thread_info {
 #define _TIF_SYSCALL_TRACE_MASK (_TIF_SYSCALL_TRACE | _TIF_SINGLESTEP |	\
 				 _TIF_BLOCKSTEP)
 
-#define tsk_is_polling(t) test_tsk_thread_flag(t, TIF_POLLING_NRFLAG)
-
 #endif /* __KERNEL__ */
 
 #endif /* _ASM_PARISC_THREAD_INFO_H */
diff --git a/arch/parisc/include/asm/uaccess.h b/arch/parisc/include/asm/uaccess.h
index 4ba2c93770f..e0a82358517 100644
--- a/arch/parisc/include/asm/uaccess.h
+++ b/arch/parisc/include/asm/uaccess.h
@@ -181,30 +181,24 @@ struct exception_data {
 #if !defined(CONFIG_64BIT)
 
 #define __put_kernel_asm64(__val,ptr) do {		    \
-	u64 __val64 = (u64)(__val);			    \
-	u32 hi = (__val64) >> 32;			    \
-	u32 lo = (__val64) & 0xffffffff;		    \
 	__asm__ __volatile__ (				    \
 		"\n1:\tstw %2,0(%1)"			    \
-		"\n2:\tstw %3,4(%1)\n\t"		    \
+		"\n2:\tstw %R2,4(%1)\n\t"		    \
 		ASM_EXCEPTIONTABLE_ENTRY(1b,fixup_put_user_skip_2)\
 		ASM_EXCEPTIONTABLE_ENTRY(2b,fixup_put_user_skip_1)\
 		: "=r"(__pu_err)                            \
-		: "r"(ptr), "r"(hi), "r"(lo), "0"(__pu_err) \
+		: "r"(ptr), "r"(__val), "0"(__pu_err) \
 		: "r1");				    \
 } while (0)
 
 #define __put_user_asm64(__val,ptr) do {	    	    \
-	u64 __val64 = (u64)(__val);			    \
-	u32 hi = (__val64) >> 32;			    \
-	u32 lo = (__val64) & 0xffffffff;		    \
 	__asm__ __volatile__ (				    \
 		"\n1:\tstw %2,0(%%sr3,%1)"		    \
-		"\n2:\tstw %3,4(%%sr3,%1)\n\t"		    \
+		"\n2:\tstw %R2,4(%%sr3,%1)\n\t"		    \
 		ASM_EXCEPTIONTABLE_ENTRY(1b,fixup_put_user_skip_2)\
 		ASM_EXCEPTIONTABLE_ENTRY(2b,fixup_put_user_skip_1)\
 		: "=r"(__pu_err)                            \
-		: "r"(ptr), "r"(hi), "r"(lo), "0"(__pu_err) \
+		: "r"(ptr), "r"(__val), "0"(__pu_err) \
 		: "r1");				    \
 } while (0)
 
diff --git a/arch/parisc/kernel/cache.c b/arch/parisc/kernel/cache.c
index 4b12890642e..83ded26cad0 100644
--- a/arch/parisc/kernel/cache.c
+++ b/arch/parisc/kernel/cache.c
@@ -421,14 +421,11 @@ void purge_tlb_entries(struct mm_struct *mm, unsigned long addr)
 	/* Note: purge_tlb_entries can be called at startup with
 	   no context.  */
 
-	/* Disable preemption while we play with %sr1.  */
-	preempt_disable();
-	mtsp(mm->context, 1);
 	purge_tlb_start(flags);
+	mtsp(mm->context, 1);
 	pdtlb(addr);
 	pitlb(addr);
 	purge_tlb_end(flags);
-	preempt_enable();
 }
 EXPORT_SYMBOL(purge_tlb_entries);
 
diff --git a/arch/parisc/kernel/parisc_ksyms.c b/arch/parisc/kernel/parisc_ksyms.c
index 6795dc6c995..568b2c61ea0 100644
--- a/arch/parisc/kernel/parisc_ksyms.c
+++ b/arch/parisc/kernel/parisc_ksyms.c
@@ -120,11 +120,13 @@ extern void __ashrdi3(void);
 extern void __ashldi3(void);
 extern void __lshrdi3(void);
 extern void __muldi3(void);
+extern void __ucmpdi2(void);
 
 EXPORT_SYMBOL(__ashrdi3);
 EXPORT_SYMBOL(__ashldi3);
 EXPORT_SYMBOL(__lshrdi3);
 EXPORT_SYMBOL(__muldi3);
+EXPORT_SYMBOL(__ucmpdi2);
 
 asmlinkage void * __canonicalize_funcptr_for_compare(void *);
 EXPORT_SYMBOL(__canonicalize_funcptr_for_compare);
diff --git a/arch/parisc/kernel/process.c b/arch/parisc/kernel/process.c
index d13507246c5..55f92b61418 100644
--- a/arch/parisc/kernel/process.c
+++ b/arch/parisc/kernel/process.c
@@ -59,28 +59,6 @@
 #include <asm/unwind.h>
 #include <asm/sections.h>
 
-/*
- * The idle thread. There's no useful work to be
- * done, so just try to conserve power and have a
- * low exit latency (ie sit in a loop waiting for
- * somebody to say that they'd like to reschedule)
- */
-void cpu_idle(void)
-{
-	set_thread_flag(TIF_POLLING_NRFLAG);
-
-	/* endless idle loop with no priority at all */
-	while (1) {
-		rcu_idle_enter();
-		while (!need_resched())
-			barrier();
-		rcu_idle_exit();
-		schedule_preempt_disabled();
-		check_pgt_cache();
-	}
-}
-
-
 #define COMMAND_GLOBAL  F_EXTEND(0xfffe0030)
 #define CMD_RESET       5       /* reset any module */
 
diff --git a/arch/parisc/kernel/smp.c b/arch/parisc/kernel/smp.c
index 6266730efd6..fd1bb1519c2 100644
--- a/arch/parisc/kernel/smp.c
+++ b/arch/parisc/kernel/smp.c
@@ -329,7 +329,7 @@ void __init smp_callin(void)
 
 	local_irq_enable();  /* Interrupts have been off until now */
 
-	cpu_idle();      /* Wait for timer to schedule some work */
+	cpu_startup_entry(CPUHP_ONLINE);
 
 	/* NOTREACHED */
 	panic("smp_callin() AAAAaaaaahhhh....\n");
diff --git a/arch/parisc/lib/Makefile b/arch/parisc/lib/Makefile
index 5f2e6904d14..5651536ac73 100644
--- a/arch/parisc/lib/Makefile
+++ b/arch/parisc/lib/Makefile
@@ -2,6 +2,7 @@
 # Makefile for parisc-specific library files
 #
 
-lib-y	:= lusercopy.o bitops.o checksum.o io.o memset.o fixup.o memcpy.o
+lib-y	:= lusercopy.o bitops.o checksum.o io.o memset.o fixup.o memcpy.o \
+	   ucmpdi2.o
 
 obj-y	:= iomap.o
diff --git a/arch/parisc/lib/ucmpdi2.c b/arch/parisc/lib/ucmpdi2.c
new file mode 100644
index 00000000000..149c016f32c
--- /dev/null
+++ b/arch/parisc/lib/ucmpdi2.c
@@ -0,0 +1,25 @@
+#include <linux/module.h>
+
+union ull_union {
+	unsigned long long ull;
+	struct {
+		unsigned int high;
+		unsigned int low;
+	} ui;
+};
+
+int __ucmpdi2(unsigned long long a, unsigned long long b)
+{
+	union ull_union au = {.ull = a};
+	union ull_union bu = {.ull = b};
+
+	if (au.ui.high < bu.ui.high)
+		return 0;
+	else if (au.ui.high > bu.ui.high)
+		return 2;
+	if (au.ui.low < bu.ui.low)
+		return 0;
+	else if (au.ui.low > bu.ui.low)
+		return 2;
+	return 1;
+}
diff --git a/arch/parisc/mm/init.c b/arch/parisc/mm/init.c
index 3ac462de53a..157b931e7b0 100644
--- a/arch/parisc/mm/init.c
+++ b/arch/parisc/mm/init.c
@@ -505,7 +505,6 @@ static void __init map_pages(unsigned long start_vaddr,
 
 void free_initmem(void)
 {
-	unsigned long addr;
 	unsigned long init_begin = (unsigned long)__init_begin;
 	unsigned long init_end = (unsigned long)__init_end;
 
@@ -533,19 +532,10 @@ void free_initmem(void)
 	 * pages are no-longer executable */
 	flush_icache_range(init_begin, init_end);
 	
-	for (addr = init_begin; addr < init_end; addr += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(addr));
-		init_page_count(virt_to_page(addr));
-		free_page(addr);
-		num_physpages++;
-		totalram_pages++;
-	}
+	num_physpages += free_initmem_default(0);
 
 	/* set up a new led state on systems shipped LED State panel */
 	pdc_chassis_send_status(PDC_CHASSIS_DIRECT_BCOMPLETE);
-	
-	printk(KERN_INFO "Freeing unused kernel memory: %luk freed\n",
-		(init_end - init_begin) >> 10);
 }
 
 
@@ -697,6 +687,8 @@ void show_mem(unsigned int filter)
 
 	printk(KERN_INFO "Mem-info:\n");
 	show_free_areas(filter);
+	if (filter & SHOW_MEM_FILTER_PAGE_COUNT)
+		return;
 #ifndef CONFIG_DISCONTIGMEM
 	i = max_mapnr;
 	while (i-- > 0) {
@@ -1107,15 +1099,6 @@ void flush_tlb_all(void)
 #ifdef CONFIG_BLK_DEV_INITRD
 void free_initrd_mem(unsigned long start, unsigned long end)
 {
-	if (start >= end)
-		return;
-	printk(KERN_INFO "Freeing initrd memory: %ldk freed\n", (end - start) >> 10);
-	for (; start < end; start += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(start));
-		init_page_count(virt_to_page(start));
-		free_page(start);
-		num_physpages++;
-		totalram_pages++;
-	}
+	num_physpages += free_reserved_area(start, end, 0, "initrd");
 }
 #endif
diff --git a/arch/powerpc/include/asm/hugetlb.h b/arch/powerpc/include/asm/hugetlb.h
index 62e11a32c4c..4fcbd6b14a3 100644
--- a/arch/powerpc/include/asm/hugetlb.h
+++ b/arch/powerpc/include/asm/hugetlb.h
@@ -3,6 +3,7 @@
 
 #ifdef CONFIG_HUGETLB_PAGE
 #include <asm/page.h>
+#include <asm-generic/hugetlb.h>
 
 extern struct kmem_cache *hugepte_cache;
 
diff --git a/arch/powerpc/include/asm/thread_info.h b/arch/powerpc/include/asm/thread_info.h
index 406b7b9a134..8ceea14d6fe 100644
--- a/arch/powerpc/include/asm/thread_info.h
+++ b/arch/powerpc/include/asm/thread_info.h
@@ -182,8 +182,6 @@ static inline bool test_thread_local_flags(unsigned int flags)
 #define is_32bit_task()	(1)
 #endif
 
-#define tsk_is_polling(t) test_tsk_thread_flag(t, TIF_POLLING_NRFLAG)
-
 #endif	/* !__ASSEMBLY__ */
 
 #endif /* __KERNEL__ */
diff --git a/arch/powerpc/include/asm/uprobes.h b/arch/powerpc/include/asm/uprobes.h
index b532060d091..23016020915 100644
--- a/arch/powerpc/include/asm/uprobes.h
+++ b/arch/powerpc/include/asm/uprobes.h
@@ -51,4 +51,5 @@ 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);
+extern unsigned long arch_uretprobe_hijack_return_addr(unsigned long trampoline_vaddr, struct pt_regs *regs);
 #endif	/* _ASM_UPROBES_H */
diff --git a/arch/powerpc/kernel/crash_dump.c b/arch/powerpc/kernel/crash_dump.c
index b3ba5163eae..9ec3fe174cb 100644
--- a/arch/powerpc/kernel/crash_dump.c
+++ b/arch/powerpc/kernel/crash_dump.c
@@ -150,10 +150,7 @@ void crash_free_reserved_phys_range(unsigned long begin, unsigned long end)
 		if (addr <= rtas_end && ((addr + PAGE_SIZE) > rtas_start))
 			continue;
 
-		ClearPageReserved(pfn_to_page(addr >> PAGE_SHIFT));
-		init_page_count(pfn_to_page(addr >> PAGE_SHIFT));
-		free_page((unsigned long)__va(addr));
-		totalram_pages++;
+		free_reserved_page(pfn_to_page(addr >> PAGE_SHIFT));
 	}
 }
 #endif
diff --git a/arch/powerpc/kernel/entry_64.S b/arch/powerpc/kernel/entry_64.S
index 256c5bf0adb..04d69c4a5ac 100644
--- a/arch/powerpc/kernel/entry_64.S
+++ b/arch/powerpc/kernel/entry_64.S
@@ -304,7 +304,7 @@ syscall_exit_work:
 	subi	r12,r12,TI_FLAGS
 
 4:	/* Anything else left to do? */
-	SET_DEFAULT_THREAD_PPR(r3, r9)		/* Set thread.ppr = 3 */
+	SET_DEFAULT_THREAD_PPR(r3, r10)		/* Set thread.ppr = 3 */
 	andi.	r0,r9,(_TIF_SYSCALL_T_OR_A|_TIF_SINGLESTEP)
 	beq	.ret_from_except_lite
 
@@ -657,7 +657,7 @@ resume_kernel:
 	/* Clear _TIF_EMULATE_STACK_STORE flag */
 	lis	r11,_TIF_EMULATE_STACK_STORE@h
 	addi	r5,r9,TI_FLAGS
-	ldarx	r4,0,r5
+0:	ldarx	r4,0,r5
 	andc	r4,r4,r11
 	stdcx.	r4,0,r5
 	bne-	0b
diff --git a/arch/powerpc/kernel/epapr_paravirt.c b/arch/powerpc/kernel/epapr_paravirt.c
index f3eab8594d9..d44a571e45a 100644
--- a/arch/powerpc/kernel/epapr_paravirt.c
+++ b/arch/powerpc/kernel/epapr_paravirt.c
@@ -23,8 +23,10 @@
 #include <asm/code-patching.h>
 #include <asm/machdep.h>
 
+#if !defined(CONFIG_64BIT) || defined(CONFIG_PPC_BOOK3E_64)
 extern void epapr_ev_idle(void);
 extern u32 epapr_ev_idle_start[];
+#endif
 
 bool epapr_paravirt_enabled;
 
@@ -47,11 +49,15 @@ static int __init epapr_paravirt_init(void)
 
 	for (i = 0; i < (len / 4); i++) {
 		patch_instruction(epapr_hypercall_start + i, insts[i]);
+#if !defined(CONFIG_64BIT) || defined(CONFIG_PPC_BOOK3E_64)
 		patch_instruction(epapr_ev_idle_start + i, insts[i]);
+#endif
 	}
 
+#if !defined(CONFIG_64BIT) || defined(CONFIG_PPC_BOOK3E_64)
 	if (of_get_property(hyper_node, "has-idle", NULL))
 		ppc_md.power_save = epapr_ev_idle;
+#endif
 
 	epapr_paravirt_enabled = true;
 
diff --git a/arch/powerpc/kernel/exceptions-64s.S b/arch/powerpc/kernel/exceptions-64s.S
index 200afa5bcfb..56bd92362ce 100644
--- a/arch/powerpc/kernel/exceptions-64s.S
+++ b/arch/powerpc/kernel/exceptions-64s.S
@@ -1066,78 +1066,6 @@ unrecov_user_slb:
 #endif /* __DISABLED__ */
 
 
-/*
- * r13 points to the PACA, r9 contains the saved CR,
- * r12 contain the saved SRR1, SRR0 is still ready for return
- * r3 has the faulting address
- * r9 - r13 are saved in paca->exslb.
- * r3 is saved in paca->slb_r3
- * We assume we aren't going to take any exceptions during this procedure.
- */
-_GLOBAL(slb_miss_realmode)
-	mflr	r10
-#ifdef CONFIG_RELOCATABLE
-	mtctr	r11
-#endif
-
-	stw	r9,PACA_EXSLB+EX_CCR(r13)	/* save CR in exc. frame */
-	std	r10,PACA_EXSLB+EX_LR(r13)	/* save LR */
-
-	bl	.slb_allocate_realmode
-
-	/* All done -- return from exception. */
-
-	ld	r10,PACA_EXSLB+EX_LR(r13)
-	ld	r3,PACA_EXSLB+EX_R3(r13)
-	lwz	r9,PACA_EXSLB+EX_CCR(r13)	/* get saved CR */
-
-	mtlr	r10
-
-	andi.	r10,r12,MSR_RI	/* check for unrecoverable exception */
-	beq-	2f
-
-.machine	push
-.machine	"power4"
-	mtcrf	0x80,r9
-	mtcrf	0x01,r9		/* slb_allocate uses cr0 and cr7 */
-.machine	pop
-
-	RESTORE_PPR_PACA(PACA_EXSLB, r9)
-	ld	r9,PACA_EXSLB+EX_R9(r13)
-	ld	r10,PACA_EXSLB+EX_R10(r13)
-	ld	r11,PACA_EXSLB+EX_R11(r13)
-	ld	r12,PACA_EXSLB+EX_R12(r13)
-	ld	r13,PACA_EXSLB+EX_R13(r13)
-	rfid
-	b	.	/* prevent speculative execution */
-
-2:	mfspr	r11,SPRN_SRR0
-	ld	r10,PACAKBASE(r13)
-	LOAD_HANDLER(r10,unrecov_slb)
-	mtspr	SPRN_SRR0,r10
-	ld	r10,PACAKMSR(r13)
-	mtspr	SPRN_SRR1,r10
-	rfid
-	b	.
-
-unrecov_slb:
-	EXCEPTION_PROLOG_COMMON(0x4100, PACA_EXSLB)
-	DISABLE_INTS
-	bl	.save_nvgprs
-1:	addi	r3,r1,STACK_FRAME_OVERHEAD
-	bl	.unrecoverable_exception
-	b	1b
-
-
-#ifdef CONFIG_PPC_970_NAP
-power4_fixup_nap:
-	andc	r9,r9,r10
-	std	r9,TI_LOCAL_FLAGS(r11)
-	ld	r10,_LINK(r1)		/* make idle task do the */
-	std	r10,_NIP(r1)		/* equivalent of a blr */
-	blr
-#endif
-
 	.align	7
 	.globl alignment_common
 alignment_common:
@@ -1336,6 +1264,78 @@ _GLOBAL(opal_mc_secondary_handler)
 
 
 /*
+ * r13 points to the PACA, r9 contains the saved CR,
+ * r12 contain the saved SRR1, SRR0 is still ready for return
+ * r3 has the faulting address
+ * r9 - r13 are saved in paca->exslb.
+ * r3 is saved in paca->slb_r3
+ * We assume we aren't going to take any exceptions during this procedure.
+ */
+_GLOBAL(slb_miss_realmode)
+	mflr	r10
+#ifdef CONFIG_RELOCATABLE
+	mtctr	r11
+#endif
+
+	stw	r9,PACA_EXSLB+EX_CCR(r13)	/* save CR in exc. frame */
+	std	r10,PACA_EXSLB+EX_LR(r13)	/* save LR */
+
+	bl	.slb_allocate_realmode
+
+	/* All done -- return from exception. */
+
+	ld	r10,PACA_EXSLB+EX_LR(r13)
+	ld	r3,PACA_EXSLB+EX_R3(r13)
+	lwz	r9,PACA_EXSLB+EX_CCR(r13)	/* get saved CR */
+
+	mtlr	r10
+
+	andi.	r10,r12,MSR_RI	/* check for unrecoverable exception */
+	beq-	2f
+
+.machine	push
+.machine	"power4"
+	mtcrf	0x80,r9
+	mtcrf	0x01,r9		/* slb_allocate uses cr0 and cr7 */
+.machine	pop
+
+	RESTORE_PPR_PACA(PACA_EXSLB, r9)
+	ld	r9,PACA_EXSLB+EX_R9(r13)
+	ld	r10,PACA_EXSLB+EX_R10(r13)
+	ld	r11,PACA_EXSLB+EX_R11(r13)
+	ld	r12,PACA_EXSLB+EX_R12(r13)
+	ld	r13,PACA_EXSLB+EX_R13(r13)
+	rfid
+	b	.	/* prevent speculative execution */
+
+2:	mfspr	r11,SPRN_SRR0
+	ld	r10,PACAKBASE(r13)
+	LOAD_HANDLER(r10,unrecov_slb)
+	mtspr	SPRN_SRR0,r10
+	ld	r10,PACAKMSR(r13)
+	mtspr	SPRN_SRR1,r10
+	rfid
+	b	.
+
+unrecov_slb:
+	EXCEPTION_PROLOG_COMMON(0x4100, PACA_EXSLB)
+	DISABLE_INTS
+	bl	.save_nvgprs
+1:	addi	r3,r1,STACK_FRAME_OVERHEAD
+	bl	.unrecoverable_exception
+	b	1b
+
+
+#ifdef CONFIG_PPC_970_NAP
+power4_fixup_nap:
+	andc	r9,r9,r10
+	std	r9,TI_LOCAL_FLAGS(r11)
+	ld	r10,_LINK(r1)		/* make idle task do the */
+	std	r10,_NIP(r1)		/* equivalent of a blr */
+	blr
+#endif
+
+/*
  * Hash table stuff
  */
 	.align	7
diff --git a/arch/powerpc/kernel/fadump.c b/arch/powerpc/kernel/fadump.c
index 06c8202a69c..2230fd0ca3e 100644
--- a/arch/powerpc/kernel/fadump.c
+++ b/arch/powerpc/kernel/fadump.c
@@ -1045,10 +1045,7 @@ static void fadump_release_memory(unsigned long begin, unsigned long end)
 		if (addr <= ra_end && ((addr + PAGE_SIZE) > ra_start))
 			continue;
 
-		ClearPageReserved(pfn_to_page(addr >> PAGE_SHIFT));
-		init_page_count(pfn_to_page(addr >> PAGE_SHIFT));
-		free_page((unsigned long)__va(addr));
-		totalram_pages++;
+		free_reserved_page(pfn_to_page(addr >> PAGE_SHIFT));
 	}
 }
 
diff --git a/arch/powerpc/kernel/idle.c b/arch/powerpc/kernel/idle.c
index ea78761aa16..939ea7ef0dc 100644
--- a/arch/powerpc/kernel/idle.c
+++ b/arch/powerpc/kernel/idle.c
@@ -33,11 +33,6 @@
 #include <asm/runlatch.h>
 #include <asm/smp.h>
 
-#ifdef CONFIG_HOTPLUG_CPU
-#define cpu_should_die()	cpu_is_offline(smp_processor_id())
-#else
-#define cpu_should_die()	0
-#endif
 
 unsigned long cpuidle_disable = IDLE_NO_OVERRIDE;
 EXPORT_SYMBOL(cpuidle_disable);
@@ -50,64 +45,38 @@ static int __init powersave_off(char *arg)
 }
 __setup("powersave=off", powersave_off);
 
-/*
- * The body of the idle task.
- */
-void cpu_idle(void)
+#ifdef CONFIG_HOTPLUG_CPU
+void arch_cpu_idle_dead(void)
 {
-	set_thread_flag(TIF_POLLING_NRFLAG);
-	while (1) {
-		tick_nohz_idle_enter();
-		rcu_idle_enter();
-
-		while (!need_resched() && !cpu_should_die()) {
-			ppc64_runlatch_off();
-
-			if (ppc_md.power_save) {
-				clear_thread_flag(TIF_POLLING_NRFLAG);
-				/*
-				 * smp_mb is so clearing of TIF_POLLING_NRFLAG
-				 * is ordered w.r.t. need_resched() test.
-				 */
-				smp_mb();
-				local_irq_disable();
-
-				/* Don't trace irqs off for idle */
-				stop_critical_timings();
-
-				/* check again after disabling irqs */
-				if (!need_resched() && !cpu_should_die())
-					ppc_md.power_save();
-
-				start_critical_timings();
-
-				/* Some power_save functions return with
-				 * interrupts enabled, some don't.
-				 */
-				if (irqs_disabled())
-					local_irq_enable();
-				set_thread_flag(TIF_POLLING_NRFLAG);
-
-			} else {
-				/*
-				 * Go into low thread priority and possibly
-				 * low power mode.
-				 */
-				HMT_low();
-				HMT_very_low();
-			}
-		}
+	sched_preempt_enable_no_resched();
+	cpu_die();
+}
+#endif
 
-		HMT_medium();
-		ppc64_runlatch_on();
-		rcu_idle_exit();
-		tick_nohz_idle_exit();
-		if (cpu_should_die()) {
-			sched_preempt_enable_no_resched();
-			cpu_die();
-		}
-		schedule_preempt_disabled();
+void arch_cpu_idle(void)
+{
+	ppc64_runlatch_off();
+
+	if (ppc_md.power_save) {
+		ppc_md.power_save();
+		/*
+		 * Some power_save functions return with
+		 * interrupts enabled, some don't.
+		 */
+		if (irqs_disabled())
+			local_irq_enable();
+	} else {
+		local_irq_enable();
+		/*
+		 * Go into low thread priority and possibly
+		 * low power mode.
+		 */
+		HMT_low();
+		HMT_very_low();
 	}
+
+	HMT_medium();
+	ppc64_runlatch_on();
 }
 
 int powersave_nap;
diff --git a/arch/powerpc/kernel/kvm.c b/arch/powerpc/kernel/kvm.c
index a61b133c4f9..6782221d49b 100644
--- a/arch/powerpc/kernel/kvm.c
+++ b/arch/powerpc/kernel/kvm.c
@@ -756,12 +756,7 @@ static __init void kvm_free_tmp(void)
 	end = (ulong)&kvm_tmp[ARRAY_SIZE(kvm_tmp)] & PAGE_MASK;
 
 	/* Free the tmp space we don't need */
-	for (; start < end; start += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(start));
-		init_page_count(virt_to_page(start));
-		free_page(start);
-		totalram_pages++;
-	}
+	free_reserved_area(start, end, 0, NULL);
 }
 
 static int __init kvm_guest_init(void)
diff --git a/arch/powerpc/kernel/nvram_64.c b/arch/powerpc/kernel/nvram_64.c
index bec1e930ed7..48fbc2b97e9 100644
--- a/arch/powerpc/kernel/nvram_64.c
+++ b/arch/powerpc/kernel/nvram_64.c
@@ -511,8 +511,7 @@ int __init nvram_scan_partitions(void)
 			       "detected: 0-length partition\n");
 			goto out;
 		}
-		tmp_part = (struct nvram_partition *)
-			kmalloc(sizeof(struct nvram_partition), GFP_KERNEL);
+		tmp_part = kmalloc(sizeof(struct nvram_partition), GFP_KERNEL);
 		err = -ENOMEM;
 		if (!tmp_part) {
 			printk(KERN_ERR "nvram_scan_partitions: kmalloc failed\n");
diff --git a/arch/powerpc/kernel/process.c b/arch/powerpc/kernel/process.c
index 59dd545fdde..16e77a81ab4 100644
--- a/arch/powerpc/kernel/process.c
+++ b/arch/powerpc/kernel/process.c
@@ -555,10 +555,12 @@ static inline void tm_recheckpoint_new_task(struct task_struct *new)
 		new->thread.regs->msr |=
 			(MSR_FP | new->thread.fpexc_mode);
 	}
+#ifdef CONFIG_ALTIVEC
 	if (msr & MSR_VEC) {
 		do_load_up_transact_altivec(&new->thread);
 		new->thread.regs->msr |= MSR_VEC;
 	}
+#endif
 	/* We may as well turn on VSX too since all the state is restored now */
 	if (msr & MSR_VSX)
 		new->thread.regs->msr |= MSR_VSX;
diff --git a/arch/powerpc/kernel/signal_32.c b/arch/powerpc/kernel/signal_32.c
index 3acb28e245b..95068bf569a 100644
--- a/arch/powerpc/kernel/signal_32.c
+++ b/arch/powerpc/kernel/signal_32.c
@@ -866,10 +866,12 @@ static long restore_tm_user_regs(struct pt_regs *regs,
 		do_load_up_transact_fpu(&current->thread);
 		regs->msr |= (MSR_FP | current->thread.fpexc_mode);
 	}
+#ifdef CONFIG_ALTIVEC
 	if (msr & MSR_VEC) {
 		do_load_up_transact_altivec(&current->thread);
 		regs->msr |= MSR_VEC;
 	}
+#endif
 
 	return 0;
 }
diff --git a/arch/powerpc/kernel/signal_64.c b/arch/powerpc/kernel/signal_64.c
index 995f8543cb5..c1794286098 100644
--- a/arch/powerpc/kernel/signal_64.c
+++ b/arch/powerpc/kernel/signal_64.c
@@ -522,10 +522,12 @@ static long restore_tm_sigcontexts(struct pt_regs *regs,
 		do_load_up_transact_fpu(&current->thread);
 		regs->msr |= (MSR_FP | current->thread.fpexc_mode);
 	}
+#ifdef CONFIG_ALTIVEC
 	if (msr & MSR_VEC) {
 		do_load_up_transact_altivec(&current->thread);
 		regs->msr |= MSR_VEC;
 	}
+#endif
 
 	return err;
 }
diff --git a/arch/powerpc/kernel/smp.c b/arch/powerpc/kernel/smp.c
index 76bd9da8cb7..ee7ac5e6e28 100644
--- a/arch/powerpc/kernel/smp.c
+++ b/arch/powerpc/kernel/smp.c
@@ -669,7 +669,7 @@ __cpuinit void start_secondary(void *unused)
 
 	local_irq_enable();
 
-	cpu_idle();
+	cpu_startup_entry(CPUHP_ONLINE);
 
 	BUG();
 }
diff --git a/arch/powerpc/kernel/tm.S b/arch/powerpc/kernel/tm.S
index 84dbace657c..2da67e7a16d 100644
--- a/arch/powerpc/kernel/tm.S
+++ b/arch/powerpc/kernel/tm.S
@@ -309,6 +309,7 @@ _GLOBAL(tm_recheckpoint)
 	or	r5, r6, r5			/* Set MSR.FP+.VSX/.VEC */
 	mtmsr	r5
 
+#ifdef CONFIG_ALTIVEC
 	/* FP and VEC registers:  These are recheckpointed from thread.fpr[]
 	 * and thread.vr[] respectively.  The thread.transact_fpr[] version
 	 * is more modern, and will be loaded subsequently by any FPUnavailable
@@ -323,6 +324,7 @@ _GLOBAL(tm_recheckpoint)
 	REST_32VRS(0, r5, r3)			/* r5 scratch, r3 THREAD ptr */
 	ld	r5, THREAD_VRSAVE(r3)
 	mtspr	SPRN_VRSAVE, r5
+#endif
 
 dont_restore_vec:
 	andi.	r0, r4, MSR_FP
diff --git a/arch/powerpc/kernel/udbg.c b/arch/powerpc/kernel/udbg.c
index f9748498fe5..13b86709349 100644
--- a/arch/powerpc/kernel/udbg.c
+++ b/arch/powerpc/kernel/udbg.c
@@ -156,15 +156,13 @@ static struct console udbg_console = {
 	.index	= 0,
 };
 
-static int early_console_initialized;
-
 /*
  * Called by setup_system after ppc_md->probe and ppc_md->early_init.
  * Call it again after setting udbg_putc in ppc_md->setup_arch.
  */
 void __init register_early_udbg_console(void)
 {
-	if (early_console_initialized)
+	if (early_console)
 		return;
 
 	if (!udbg_putc)
@@ -174,7 +172,7 @@ void __init register_early_udbg_console(void)
 		printk(KERN_INFO "early console immortal !\n");
 		udbg_console.flags &= ~CON_BOOT;
 	}
-	early_console_initialized = 1;
+	early_console = &udbg_console;
 	register_console(&udbg_console);
 }
 
diff --git a/arch/powerpc/kernel/uprobes.c b/arch/powerpc/kernel/uprobes.c
index bc77834dbf4..59f419b935f 100644
--- a/arch/powerpc/kernel/uprobes.c
+++ b/arch/powerpc/kernel/uprobes.c
@@ -31,6 +31,16 @@
 #define UPROBE_TRAP_NR	UINT_MAX
 
 /**
+ * is_trap_insn - check if the instruction is a trap variant
+ * @insn: instruction to be checked.
+ * Returns true if @insn is a trap variant.
+ */
+bool is_trap_insn(uprobe_opcode_t *insn)
+{
+	return (is_trap(*insn));
+}
+
+/**
  * arch_uprobe_analyze_insn
  * @mm: the probed address space.
  * @arch_uprobe: the probepoint information.
@@ -43,12 +53,6 @@ int arch_uprobe_analyze_insn(struct arch_uprobe *auprobe,
 	if (addr & 0x03)
 		return -EINVAL;
 
-	/*
-	 * We currently don't support a uprobe on an already
-	 * existing breakpoint instruction underneath
-	 */
-	if (is_trap(auprobe->ainsn))
-		return -ENOTSUPP;
 	return 0;
 }
 
@@ -188,3 +192,16 @@ bool arch_uprobe_skip_sstep(struct arch_uprobe *auprobe, struct pt_regs *regs)
 
 	return false;
 }
+
+unsigned long
+arch_uretprobe_hijack_return_addr(unsigned long trampoline_vaddr, struct pt_regs *regs)
+{
+	unsigned long orig_ret_vaddr;
+
+	orig_ret_vaddr = regs->link;
+
+	/* Replace the return addr with trampoline addr */
+	regs->link = trampoline_vaddr;
+
+	return orig_ret_vaddr;
+}
diff --git a/arch/powerpc/kvm/book3s_pr.c b/arch/powerpc/kvm/book3s_pr.c
index 5e93438afb0..dbdc15aa812 100644
--- a/arch/powerpc/kvm/book3s_pr.c
+++ b/arch/powerpc/kvm/book3s_pr.c
@@ -1039,7 +1039,7 @@ struct kvm_vcpu *kvmppc_core_vcpu_create(struct kvm *kvm, unsigned int id)
 	if (!vcpu_book3s)
 		goto out;
 
-	vcpu_book3s->shadow_vcpu = (struct kvmppc_book3s_shadow_vcpu *)
+	vcpu_book3s->shadow_vcpu =
 		kzalloc(sizeof(*vcpu_book3s->shadow_vcpu), GFP_KERNEL);
 	if (!vcpu_book3s->shadow_vcpu)
 		goto free_vcpu;
diff --git a/arch/powerpc/kvm/e500.h b/arch/powerpc/kvm/e500.h
index 41cefd43655..33db48a8ce2 100644
--- a/arch/powerpc/kvm/e500.h
+++ b/arch/powerpc/kvm/e500.h
@@ -26,17 +26,20 @@
 #define E500_PID_NUM   3
 #define E500_TLB_NUM   2
 
-#define E500_TLB_VALID 1
-#define E500_TLB_BITMAP 2
+/* entry is mapped somewhere in host TLB */
+#define E500_TLB_VALID		(1 << 0)
+/* TLB1 entry is mapped by host TLB1, tracked by bitmaps */
+#define E500_TLB_BITMAP		(1 << 1)
+/* TLB1 entry is mapped by host TLB0 */
 #define E500_TLB_TLB0		(1 << 2)
 
 struct tlbe_ref {
-	pfn_t pfn;
-	unsigned int flags; /* E500_TLB_* */
+	pfn_t pfn;		/* valid only for TLB0, except briefly */
+	unsigned int flags;	/* E500_TLB_* */
 };
 
 struct tlbe_priv {
-	struct tlbe_ref ref; /* TLB0 only -- TLB1 uses tlb_refs */
+	struct tlbe_ref ref;
 };
 
 #ifdef CONFIG_KVM_E500V2
@@ -63,17 +66,6 @@ struct kvmppc_vcpu_e500 {
 
 	unsigned int gtlb_nv[E500_TLB_NUM];
 
-	/*
-	 * information associated with each host TLB entry --
-	 * TLB1 only for now.  If/when guest TLB1 entries can be
-	 * mapped with host TLB0, this will be used for that too.
-	 *
-	 * We don't want to use this for guest TLB0 because then we'd
-	 * have the overhead of doing the translation again even if
-	 * the entry is still in the guest TLB (e.g. we swapped out
-	 * and back, and our host TLB entries got evicted).
-	 */
-	struct tlbe_ref *tlb_refs[E500_TLB_NUM];
 	unsigned int host_tlb1_nv;
 
 	u32 svr;
diff --git a/arch/powerpc/kvm/e500_mmu_host.c b/arch/powerpc/kvm/e500_mmu_host.c
index a222edfb9a9..1c6a9d729df 100644
--- a/arch/powerpc/kvm/e500_mmu_host.c
+++ b/arch/powerpc/kvm/e500_mmu_host.c
@@ -193,8 +193,11 @@ void inval_gtlbe_on_host(struct kvmppc_vcpu_e500 *vcpu_e500, int tlbsel,
 	struct tlbe_ref *ref = &vcpu_e500->gtlb_priv[tlbsel][esel].ref;
 
 	/* Don't bother with unmapped entries */
-	if (!(ref->flags & E500_TLB_VALID))
-		return;
+	if (!(ref->flags & E500_TLB_VALID)) {
+		WARN(ref->flags & (E500_TLB_BITMAP | E500_TLB_TLB0),
+		     "%s: flags %x\n", __func__, ref->flags);
+		WARN_ON(tlbsel == 1 && vcpu_e500->g2h_tlb1_map[esel]);
+	}
 
 	if (tlbsel == 1 && ref->flags & E500_TLB_BITMAP) {
 		u64 tmp = vcpu_e500->g2h_tlb1_map[esel];
@@ -248,7 +251,7 @@ static inline void kvmppc_e500_ref_setup(struct tlbe_ref *ref,
 					 pfn_t pfn)
 {
 	ref->pfn = pfn;
-	ref->flags = E500_TLB_VALID;
+	ref->flags |= E500_TLB_VALID;
 
 	if (tlbe_is_writable(gtlbe))
 		kvm_set_pfn_dirty(pfn);
@@ -257,6 +260,7 @@ static inline void kvmppc_e500_ref_setup(struct tlbe_ref *ref,
 static inline void kvmppc_e500_ref_release(struct tlbe_ref *ref)
 {
 	if (ref->flags & E500_TLB_VALID) {
+		/* FIXME: don't log bogus pfn for TLB1 */
 		trace_kvm_booke206_ref_release(ref->pfn, ref->flags);
 		ref->flags = 0;
 	}
@@ -274,36 +278,23 @@ static void clear_tlb1_bitmap(struct kvmppc_vcpu_e500 *vcpu_e500)
 
 static void clear_tlb_privs(struct kvmppc_vcpu_e500 *vcpu_e500)
 {
-	int tlbsel = 0;
-	int i;
-
-	for (i = 0; i < vcpu_e500->gtlb_params[tlbsel].entries; i++) {
-		struct tlbe_ref *ref =
-			&vcpu_e500->gtlb_priv[tlbsel][i].ref;
-		kvmppc_e500_ref_release(ref);
-	}
-}
-
-static void clear_tlb_refs(struct kvmppc_vcpu_e500 *vcpu_e500)
-{
-	int stlbsel = 1;
+	int tlbsel;
 	int i;
 
-	kvmppc_e500_tlbil_all(vcpu_e500);
-
-	for (i = 0; i < host_tlb_params[stlbsel].entries; i++) {
-		struct tlbe_ref *ref =
-			&vcpu_e500->tlb_refs[stlbsel][i];
-		kvmppc_e500_ref_release(ref);
+	for (tlbsel = 0; tlbsel <= 1; tlbsel++) {
+		for (i = 0; i < vcpu_e500->gtlb_params[tlbsel].entries; i++) {
+			struct tlbe_ref *ref =
+				&vcpu_e500->gtlb_priv[tlbsel][i].ref;
+			kvmppc_e500_ref_release(ref);
+		}
 	}
-
-	clear_tlb_privs(vcpu_e500);
 }
 
 void kvmppc_core_flush_tlb(struct kvm_vcpu *vcpu)
 {
 	struct kvmppc_vcpu_e500 *vcpu_e500 = to_e500(vcpu);
-	clear_tlb_refs(vcpu_e500);
+	kvmppc_e500_tlbil_all(vcpu_e500);
+	clear_tlb_privs(vcpu_e500);
 	clear_tlb1_bitmap(vcpu_e500);
 }
 
@@ -458,8 +449,6 @@ static inline int kvmppc_e500_shadow_map(struct kvmppc_vcpu_e500 *vcpu_e500,
 		gvaddr &= ~((tsize_pages << PAGE_SHIFT) - 1);
 	}
 
-	/* Drop old ref and setup new one. */
-	kvmppc_e500_ref_release(ref);
 	kvmppc_e500_ref_setup(ref, gtlbe, pfn);
 
 	kvmppc_e500_setup_stlbe(&vcpu_e500->vcpu, gtlbe, tsize,
@@ -507,14 +496,15 @@ static int kvmppc_e500_tlb1_map_tlb1(struct kvmppc_vcpu_e500 *vcpu_e500,
 	if (unlikely(vcpu_e500->host_tlb1_nv >= tlb1_max_shadow_size()))
 		vcpu_e500->host_tlb1_nv = 0;
 
-	vcpu_e500->tlb_refs[1][sesel] = *ref;
-	vcpu_e500->g2h_tlb1_map[esel] |= (u64)1 << sesel;
-	vcpu_e500->gtlb_priv[1][esel].ref.flags |= E500_TLB_BITMAP;
 	if (vcpu_e500->h2g_tlb1_rmap[sesel]) {
-		unsigned int idx = vcpu_e500->h2g_tlb1_rmap[sesel];
+		unsigned int idx = vcpu_e500->h2g_tlb1_rmap[sesel] - 1;
 		vcpu_e500->g2h_tlb1_map[idx] &= ~(1ULL << sesel);
 	}
-	vcpu_e500->h2g_tlb1_rmap[sesel] = esel;
+
+	vcpu_e500->gtlb_priv[1][esel].ref.flags |= E500_TLB_BITMAP;
+	vcpu_e500->g2h_tlb1_map[esel] |= (u64)1 << sesel;
+	vcpu_e500->h2g_tlb1_rmap[sesel] = esel + 1;
+	WARN_ON(!(ref->flags & E500_TLB_VALID));
 
 	return sesel;
 }
@@ -526,13 +516,12 @@ static int kvmppc_e500_tlb1_map(struct kvmppc_vcpu_e500 *vcpu_e500,
 		u64 gvaddr, gfn_t gfn, struct kvm_book3e_206_tlb_entry *gtlbe,
 		struct kvm_book3e_206_tlb_entry *stlbe, int esel)
 {
-	struct tlbe_ref ref;
+	struct tlbe_ref *ref = &vcpu_e500->gtlb_priv[1][esel].ref;
 	int sesel;
 	int r;
 
-	ref.flags = 0;
 	r = kvmppc_e500_shadow_map(vcpu_e500, gvaddr, gfn, gtlbe, 1, stlbe,
-				   &ref);
+				   ref);
 	if (r)
 		return r;
 
@@ -544,7 +533,7 @@ static int kvmppc_e500_tlb1_map(struct kvmppc_vcpu_e500 *vcpu_e500,
 	}
 
 	/* Otherwise map into TLB1 */
-	sesel = kvmppc_e500_tlb1_map_tlb1(vcpu_e500, &ref, esel);
+	sesel = kvmppc_e500_tlb1_map_tlb1(vcpu_e500, ref, esel);
 	write_stlbe(vcpu_e500, gtlbe, stlbe, 1, sesel);
 
 	return 0;
@@ -565,7 +554,7 @@ void kvmppc_mmu_map(struct kvm_vcpu *vcpu, u64 eaddr, gpa_t gpaddr,
 	case 0:
 		priv = &vcpu_e500->gtlb_priv[tlbsel][esel];
 
-		/* Triggers after clear_tlb_refs or on initial mapping */
+		/* Triggers after clear_tlb_privs or on initial mapping */
 		if (!(priv->ref.flags & E500_TLB_VALID)) {
 			kvmppc_e500_tlb0_map(vcpu_e500, esel, &stlbe);
 		} else {
@@ -665,35 +654,16 @@ int e500_mmu_host_init(struct kvmppc_vcpu_e500 *vcpu_e500)
 		host_tlb_params[0].entries / host_tlb_params[0].ways;
 	host_tlb_params[1].sets = 1;
 
-	vcpu_e500->tlb_refs[0] =
-		kzalloc(sizeof(struct tlbe_ref) * host_tlb_params[0].entries,
-			GFP_KERNEL);
-	if (!vcpu_e500->tlb_refs[0])
-		goto err;
-
-	vcpu_e500->tlb_refs[1] =
-		kzalloc(sizeof(struct tlbe_ref) * host_tlb_params[1].entries,
-			GFP_KERNEL);
-	if (!vcpu_e500->tlb_refs[1])
-		goto err;
-
 	vcpu_e500->h2g_tlb1_rmap = kzalloc(sizeof(unsigned int) *
 					   host_tlb_params[1].entries,
 					   GFP_KERNEL);
 	if (!vcpu_e500->h2g_tlb1_rmap)
-		goto err;
+		return -EINVAL;
 
 	return 0;
-
-err:
-	kfree(vcpu_e500->tlb_refs[0]);
-	kfree(vcpu_e500->tlb_refs[1]);
-	return -EINVAL;
 }
 
 void e500_mmu_host_uninit(struct kvmppc_vcpu_e500 *vcpu_e500)
 {
 	kfree(vcpu_e500->h2g_tlb1_rmap);
-	kfree(vcpu_e500->tlb_refs[0]);
-	kfree(vcpu_e500->tlb_refs[1]);
 }
diff --git a/arch/powerpc/kvm/e500mc.c b/arch/powerpc/kvm/e500mc.c
index 1f89d26e65f..2f4baa074b2 100644
--- a/arch/powerpc/kvm/e500mc.c
+++ b/arch/powerpc/kvm/e500mc.c
@@ -108,6 +108,8 @@ void kvmppc_mmu_msr_notify(struct kvm_vcpu *vcpu, u32 old_msr)
 {
 }
 
+static DEFINE_PER_CPU(struct kvm_vcpu *, last_vcpu_on_cpu);
+
 void kvmppc_core_vcpu_load(struct kvm_vcpu *vcpu, int cpu)
 {
 	struct kvmppc_vcpu_e500 *vcpu_e500 = to_e500(vcpu);
@@ -136,8 +138,11 @@ void kvmppc_core_vcpu_load(struct kvm_vcpu *vcpu, int cpu)
 	mtspr(SPRN_GDEAR, vcpu->arch.shared->dar);
 	mtspr(SPRN_GESR, vcpu->arch.shared->esr);
 
-	if (vcpu->arch.oldpir != mfspr(SPRN_PIR))
+	if (vcpu->arch.oldpir != mfspr(SPRN_PIR) ||
+	    __get_cpu_var(last_vcpu_on_cpu) != vcpu) {
 		kvmppc_e500_tlbil_all(vcpu_e500);
+		__get_cpu_var(last_vcpu_on_cpu) = vcpu;
+	}
 
 	kvmppc_load_guest_fp(vcpu);
 }
diff --git a/arch/powerpc/mm/init_64.c b/arch/powerpc/mm/init_64.c
index 7e2246fb2f3..5a535b73ea1 100644
--- a/arch/powerpc/mm/init_64.c
+++ b/arch/powerpc/mm/init_64.c
@@ -263,19 +263,14 @@ static __meminit void vmemmap_list_populate(unsigned long phys,
 	vmemmap_list = vmem_back;
 }
 
-int __meminit vmemmap_populate(struct page *start_page,
-			       unsigned long nr_pages, int node)
+int __meminit vmemmap_populate(unsigned long start, unsigned long end, int node)
 {
-	unsigned long start = (unsigned long)start_page;
-	unsigned long end = (unsigned long)(start_page + nr_pages);
 	unsigned long page_size = 1 << mmu_psize_defs[mmu_vmemmap_psize].shift;
 
 	/* Align to the page size of the linear mapping. */
 	start = _ALIGN_DOWN(start, page_size);
 
-	pr_debug("vmemmap_populate page %p, %ld pages, node %d\n",
-		 start_page, nr_pages, node);
-	pr_debug(" -> map %lx..%lx\n", start, end);
+	pr_debug("vmemmap_populate %lx..%lx, node %d\n", start, end, node);
 
 	for (; start < end; start += page_size) {
 		void *p;
@@ -298,7 +293,7 @@ int __meminit vmemmap_populate(struct page *start_page,
 	return 0;
 }
 
-void vmemmap_free(struct page *memmap, unsigned long nr_pages)
+void vmemmap_free(unsigned long start, unsigned long end)
 {
 }
 
diff --git a/arch/powerpc/mm/mem.c b/arch/powerpc/mm/mem.c
index f1f7409a418..cd76c454942 100644
--- a/arch/powerpc/mm/mem.c
+++ b/arch/powerpc/mm/mem.c
@@ -352,13 +352,9 @@ void __init mem_init(void)
 			struct page *page = pfn_to_page(pfn);
 			if (memblock_is_reserved(paddr))
 				continue;
-			ClearPageReserved(page);
-			init_page_count(page);
-			__free_page(page);
-			totalhigh_pages++;
+			free_highmem_page(page);
 			reservedpages--;
 		}
-		totalram_pages += totalhigh_pages;
 		printk(KERN_DEBUG "High memory: %luk\n",
 		       totalhigh_pages << (PAGE_SHIFT-10));
 	}
@@ -405,39 +401,14 @@ void __init mem_init(void)
 
 void free_initmem(void)
 {
-	unsigned long addr;
-
 	ppc_md.progress = ppc_printk_progress;
-
-	addr = (unsigned long)__init_begin;
-	for (; addr < (unsigned long)__init_end; addr += PAGE_SIZE) {
-		memset((void *)addr, POISON_FREE_INITMEM, PAGE_SIZE);
-		ClearPageReserved(virt_to_page(addr));
-		init_page_count(virt_to_page(addr));
-		free_page(addr);
-		totalram_pages++;
-	}
-	pr_info("Freeing unused kernel memory: %luk freed\n",
-		((unsigned long)__init_end -
-		(unsigned long)__init_begin) >> 10);
+	free_initmem_default(POISON_FREE_INITMEM);
 }
 
 #ifdef CONFIG_BLK_DEV_INITRD
 void __init free_initrd_mem(unsigned long start, unsigned long end)
 {
-	if (start >= end)
-		return;
-
-	start = _ALIGN_DOWN(start, PAGE_SIZE);
-	end = _ALIGN_UP(end, PAGE_SIZE);
-	pr_info("Freeing initrd memory: %ldk freed\n", (end - start) >> 10);
-
-	for (; start < end; start += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(start));
-		init_page_count(virt_to_page(start));
-		free_page(start);
-		totalram_pages++;
-	}
+	free_reserved_area(start, end, 0, "initrd");
 }
 #endif
 
diff --git a/arch/powerpc/mm/numa.c b/arch/powerpc/mm/numa.c
index bba87ca2b4d..fa33c546e77 100644
--- a/arch/powerpc/mm/numa.c
+++ b/arch/powerpc/mm/numa.c
@@ -22,6 +22,7 @@
 #include <linux/pfn.h>
 #include <linux/cpuset.h>
 #include <linux/node.h>
+#include <linux/slab.h>
 #include <asm/sparsemem.h>
 #include <asm/prom.h>
 #include <asm/smp.h>
@@ -62,14 +63,11 @@ static int distance_lookup_table[MAX_NUMNODES][MAX_DISTANCE_REF_POINTS];
  */
 static void __init setup_node_to_cpumask_map(void)
 {
-	unsigned int node, num = 0;
+	unsigned int node;
 
 	/* setup nr_node_ids if not done yet */
-	if (nr_node_ids == MAX_NUMNODES) {
-		for_each_node_mask(node, node_possible_map)
-			num = node;
-		nr_node_ids = num + 1;
-	}
+	if (nr_node_ids == MAX_NUMNODES)
+		setup_nr_node_ids();
 
 	/* allocate the map */
 	for (node = 0; node < nr_node_ids; node++)
diff --git a/arch/powerpc/platforms/44x/Kconfig b/arch/powerpc/platforms/44x/Kconfig
index 0effe9f5a1e..7be93367d92 100644
--- a/arch/powerpc/platforms/44x/Kconfig
+++ b/arch/powerpc/platforms/44x/Kconfig
@@ -274,6 +274,8 @@ config 440EPX
 	select IBM_EMAC_EMAC4
 	select IBM_EMAC_RGMII
 	select IBM_EMAC_ZMII
+	select USB_EHCI_BIG_ENDIAN_MMIO
+	select USB_EHCI_BIG_ENDIAN_DESC
 
 config 440GRX
 	bool
diff --git a/arch/powerpc/platforms/512x/Kconfig b/arch/powerpc/platforms/512x/Kconfig
index c16999802ec..381a592826a 100644
--- a/arch/powerpc/platforms/512x/Kconfig
+++ b/arch/powerpc/platforms/512x/Kconfig
@@ -7,6 +7,8 @@ config PPC_MPC512x
 	select PPC_PCI_CHOICE
 	select FSL_PCI if PCI
 	select ARCH_WANT_OPTIONAL_GPIOLIB
+	select USB_EHCI_BIG_ENDIAN_MMIO
+	select USB_EHCI_BIG_ENDIAN_DESC
 
 config MPC5121_ADS
 	bool "Freescale MPC5121E ADS"
diff --git a/arch/powerpc/platforms/512x/mpc512x_shared.c b/arch/powerpc/platforms/512x/mpc512x_shared.c
index d30235b7e3f..db6ac389ef8 100644
--- a/arch/powerpc/platforms/512x/mpc512x_shared.c
+++ b/arch/powerpc/platforms/512x/mpc512x_shared.c
@@ -172,12 +172,9 @@ static struct fsl_diu_shared_fb __attribute__ ((__aligned__(8))) diu_shared_fb;
 
 static inline void mpc512x_free_bootmem(struct page *page)
 {
-	__ClearPageReserved(page);
 	BUG_ON(PageTail(page));
 	BUG_ON(atomic_read(&page->_count) > 1);
-	atomic_set(&page->_count, 1);
-	__free_page(page);
-	totalram_pages++;
+	free_reserved_page(page);
 }
 
 void mpc512x_release_bootmem(void)
diff --git a/arch/powerpc/platforms/pseries/hotplug-memory.c b/arch/powerpc/platforms/pseries/hotplug-memory.c
index 2372c609fa2..9a432de363b 100644
--- a/arch/powerpc/platforms/pseries/hotplug-memory.c
+++ b/arch/powerpc/platforms/pseries/hotplug-memory.c
@@ -72,6 +72,7 @@ unsigned long memory_block_size_bytes(void)
 	return get_memblock_size();
 }
 
+#ifdef CONFIG_MEMORY_HOTREMOVE
 static int pseries_remove_memblock(unsigned long base, unsigned int memblock_size)
 {
 	unsigned long start, start_pfn;
@@ -153,6 +154,17 @@ static int pseries_remove_memory(struct device_node *np)
 	ret = pseries_remove_memblock(base, lmb_size);
 	return ret;
 }
+#else
+static inline int pseries_remove_memblock(unsigned long base,
+					  unsigned int memblock_size)
+{
+	return -EOPNOTSUPP;
+}
+static inline int pseries_remove_memory(struct device_node *np)
+{
+	return -EOPNOTSUPP;
+}
+#endif /* CONFIG_MEMORY_HOTREMOVE */
 
 static int pseries_add_memory(struct device_node *np)
 {
diff --git a/arch/powerpc/platforms/pseries/lpar.c b/arch/powerpc/platforms/pseries/lpar.c
index 0da39fed355..299731e9036 100644
--- a/arch/powerpc/platforms/pseries/lpar.c
+++ b/arch/powerpc/platforms/pseries/lpar.c
@@ -186,7 +186,13 @@ static long pSeries_lpar_hpte_remove(unsigned long hpte_group)
 					   (0x1UL << 4), &dummy1, &dummy2);
 		if (lpar_rc == H_SUCCESS)
 			return i;
-		BUG_ON(lpar_rc != H_NOT_FOUND);
+
+		/*
+		 * The test for adjunct partition is performed before the
+		 * ANDCOND test.  H_RESOURCE may be returned, so we need to
+		 * check for that as well.
+		 */
+		BUG_ON(lpar_rc != H_NOT_FOUND && lpar_rc != H_RESOURCE);
 
 		slot_offset++;
 		slot_offset &= 0x7;
diff --git a/arch/s390/Kconfig b/arch/s390/Kconfig
index eb8fb629f00..bda6ba6f3cf 100644
--- a/arch/s390/Kconfig
+++ b/arch/s390/Kconfig
@@ -375,19 +375,6 @@ config PACK_STACK
 
 	  Say Y if you are unsure.
 
-config SMALL_STACK
-	def_bool n
-	prompt "Use 8kb for kernel stack instead of 16kb"
-	depends on PACK_STACK && 64BIT && !LOCKDEP
-	help
-	  If you say Y here and the compiler supports the -mkernel-backchain
-	  option the kernel will use a smaller kernel stack size. The reduced
-	  size is 8kb instead of 16kb. This allows to run more threads on a
-	  system and reduces the pressure on the memory management for higher
-	  order page allocations.
-
-	  Say N if you are unsure.
-
 config CHECK_STACK
 	def_bool y
 	prompt "Detect kernel stack overflow"
diff --git a/arch/s390/Makefile b/arch/s390/Makefile
index 7e3ce78d429..a7d68a467ce 100644
--- a/arch/s390/Makefile
+++ b/arch/s390/Makefile
@@ -55,22 +55,12 @@ cflags-$(CONFIG_FRAME_POINTER) += -fno-optimize-sibling-calls
 ifeq ($(call cc-option-yn,-mkernel-backchain),y)
 cflags-$(CONFIG_PACK_STACK)  += -mkernel-backchain -D__PACK_STACK
 aflags-$(CONFIG_PACK_STACK)  += -D__PACK_STACK
-cflags-$(CONFIG_SMALL_STACK) += -D__SMALL_STACK
-aflags-$(CONFIG_SMALL_STACK) += -D__SMALL_STACK
-ifdef CONFIG_SMALL_STACK
-STACK_SIZE := $(shell echo $$(($(STACK_SIZE)/2)) )
-endif
 endif
 
 # new style option for packed stacks
 ifeq ($(call cc-option-yn,-mpacked-stack),y)
 cflags-$(CONFIG_PACK_STACK)  += -mpacked-stack -D__PACK_STACK
 aflags-$(CONFIG_PACK_STACK)  += -D__PACK_STACK
-cflags-$(CONFIG_SMALL_STACK) += -D__SMALL_STACK
-aflags-$(CONFIG_SMALL_STACK) += -D__SMALL_STACK
-ifdef CONFIG_SMALL_STACK
-STACK_SIZE := $(shell echo $$(($(STACK_SIZE)/2)) )
-endif
 endif
 
 ifeq ($(call cc-option-yn,-mstack-size=8192 -mstack-guard=128),y)
diff --git a/arch/s390/hypfs/hypfs_dbfs.c b/arch/s390/hypfs/hypfs_dbfs.c
index 9fd4a40c675..bb5dd496614 100644
--- a/arch/s390/hypfs/hypfs_dbfs.c
+++ b/arch/s390/hypfs/hypfs_dbfs.c
@@ -105,9 +105,7 @@ void hypfs_dbfs_remove_file(struct hypfs_dbfs_file *df)
 int hypfs_dbfs_init(void)
 {
 	dbfs_dir = debugfs_create_dir("s390_hypfs", NULL);
-	if (IS_ERR(dbfs_dir))
-		return PTR_ERR(dbfs_dir);
-	return 0;
+	return PTR_RET(dbfs_dir);
 }
 
 void hypfs_dbfs_exit(void)
diff --git a/arch/s390/include/asm/bitops.h b/arch/s390/include/asm/bitops.h
index 15422933c60..4d8604e311f 100644
--- a/arch/s390/include/asm/bitops.h
+++ b/arch/s390/include/asm/bitops.h
@@ -61,8 +61,6 @@ extern const char _sb_findmap[];
 
 #ifndef CONFIG_64BIT
 
-#define __BITOPS_ALIGN		3
-#define __BITOPS_WORDSIZE	32
 #define __BITOPS_OR		"or"
 #define __BITOPS_AND		"nr"
 #define __BITOPS_XOR		"xr"
@@ -81,8 +79,6 @@ extern const char _sb_findmap[];
 
 #else /* CONFIG_64BIT */
 
-#define __BITOPS_ALIGN		7
-#define __BITOPS_WORDSIZE	64
 #define __BITOPS_OR		"ogr"
 #define __BITOPS_AND		"ngr"
 #define __BITOPS_XOR		"xgr"
@@ -101,8 +97,7 @@ extern const char _sb_findmap[];
 
 #endif /* CONFIG_64BIT */
 
-#define __BITOPS_WORDS(bits) (((bits)+__BITOPS_WORDSIZE-1)/__BITOPS_WORDSIZE)
-#define __BITOPS_BARRIER() asm volatile("" : : : "memory")
+#define __BITOPS_WORDS(bits) (((bits) + BITS_PER_LONG - 1) / BITS_PER_LONG)
 
 #ifdef CONFIG_SMP
 /*
@@ -114,9 +109,9 @@ static inline void set_bit_cs(unsigned long nr, volatile unsigned long *ptr)
 
 	addr = (unsigned long) ptr;
 	/* calculate address for CS */
-	addr += (nr ^ (nr & (__BITOPS_WORDSIZE - 1))) >> 3;
+	addr += (nr ^ (nr & (BITS_PER_LONG - 1))) >> 3;
 	/* make OR mask */
-	mask = 1UL << (nr & (__BITOPS_WORDSIZE - 1));
+	mask = 1UL << (nr & (BITS_PER_LONG - 1));
 	/* Do the atomic update. */
 	__BITOPS_LOOP(old, new, addr, mask, __BITOPS_OR);
 }
@@ -130,9 +125,9 @@ static inline void clear_bit_cs(unsigned long nr, volatile unsigned long *ptr)
 
 	addr = (unsigned long) ptr;
 	/* calculate address for CS */
-	addr += (nr ^ (nr & (__BITOPS_WORDSIZE - 1))) >> 3;
+	addr += (nr ^ (nr & (BITS_PER_LONG - 1))) >> 3;
 	/* make AND mask */
-	mask = ~(1UL << (nr & (__BITOPS_WORDSIZE - 1)));
+	mask = ~(1UL << (nr & (BITS_PER_LONG - 1)));
 	/* Do the atomic update. */
 	__BITOPS_LOOP(old, new, addr, mask, __BITOPS_AND);
 }
@@ -146,9 +141,9 @@ static inline void change_bit_cs(unsigned long nr, volatile unsigned long *ptr)
 
 	addr = (unsigned long) ptr;
 	/* calculate address for CS */
-	addr += (nr ^ (nr & (__BITOPS_WORDSIZE - 1))) >> 3;
+	addr += (nr ^ (nr & (BITS_PER_LONG - 1))) >> 3;
 	/* make XOR mask */
-	mask = 1UL << (nr & (__BITOPS_WORDSIZE - 1));
+	mask = 1UL << (nr & (BITS_PER_LONG - 1));
 	/* Do the atomic update. */
 	__BITOPS_LOOP(old, new, addr, mask, __BITOPS_XOR);
 }
@@ -163,12 +158,12 @@ test_and_set_bit_cs(unsigned long nr, volatile unsigned long *ptr)
 
 	addr = (unsigned long) ptr;
 	/* calculate address for CS */
-	addr += (nr ^ (nr & (__BITOPS_WORDSIZE - 1))) >> 3;
+	addr += (nr ^ (nr & (BITS_PER_LONG - 1))) >> 3;
 	/* make OR/test mask */
-	mask = 1UL << (nr & (__BITOPS_WORDSIZE - 1));
+	mask = 1UL << (nr & (BITS_PER_LONG - 1));
 	/* Do the atomic update. */
 	__BITOPS_LOOP(old, new, addr, mask, __BITOPS_OR);
-	__BITOPS_BARRIER();
+	barrier();
 	return (old & mask) != 0;
 }
 
@@ -182,12 +177,12 @@ test_and_clear_bit_cs(unsigned long nr, volatile unsigned long *ptr)
 
 	addr = (unsigned long) ptr;
 	/* calculate address for CS */
-	addr += (nr ^ (nr & (__BITOPS_WORDSIZE - 1))) >> 3;
+	addr += (nr ^ (nr & (BITS_PER_LONG - 1))) >> 3;
 	/* make AND/test mask */
-	mask = ~(1UL << (nr & (__BITOPS_WORDSIZE - 1)));
+	mask = ~(1UL << (nr & (BITS_PER_LONG - 1)));
 	/* Do the atomic update. */
 	__BITOPS_LOOP(old, new, addr, mask, __BITOPS_AND);
-	__BITOPS_BARRIER();
+	barrier();
 	return (old ^ new) != 0;
 }
 
@@ -201,12 +196,12 @@ test_and_change_bit_cs(unsigned long nr, volatile unsigned long *ptr)
 
 	addr = (unsigned long) ptr;
 	/* calculate address for CS */
-	addr += (nr ^ (nr & (__BITOPS_WORDSIZE - 1))) >> 3;
+	addr += (nr ^ (nr & (BITS_PER_LONG - 1))) >> 3;
 	/* make XOR/test mask */
-	mask = 1UL << (nr & (__BITOPS_WORDSIZE - 1));
+	mask = 1UL << (nr & (BITS_PER_LONG - 1));
 	/* Do the atomic update. */
 	__BITOPS_LOOP(old, new, addr, mask, __BITOPS_XOR);
-	__BITOPS_BARRIER();
+	barrier();
 	return (old & mask) != 0;
 }
 #endif /* CONFIG_SMP */
@@ -218,7 +213,7 @@ static inline void __set_bit(unsigned long nr, volatile unsigned long *ptr)
 {
 	unsigned long addr;
 
-	addr = (unsigned long) ptr + ((nr ^ (__BITOPS_WORDSIZE - 8)) >> 3);
+	addr = (unsigned long) ptr + ((nr ^ (BITS_PER_LONG - 8)) >> 3);
 	asm volatile(
 		"	oc	%O0(1,%R0),%1"
 		: "=Q" (*(char *) addr) : "Q" (_oi_bitmap[nr & 7]) : "cc" );
@@ -229,7 +224,7 @@ __constant_set_bit(const unsigned long nr, volatile unsigned long *ptr)
 {
 	unsigned long addr;
 
-	addr = ((unsigned long) ptr) + ((nr ^ (__BITOPS_WORDSIZE - 8)) >> 3);
+	addr = ((unsigned long) ptr) + ((nr ^ (BITS_PER_LONG - 8)) >> 3);
 	*(unsigned char *) addr |= 1 << (nr & 7);
 }
 
@@ -246,7 +241,7 @@ __clear_bit(unsigned long nr, volatile unsigned long *ptr)
 {
 	unsigned long addr;
 
-	addr = (unsigned long) ptr + ((nr ^ (__BITOPS_WORDSIZE - 8)) >> 3);
+	addr = (unsigned long) ptr + ((nr ^ (BITS_PER_LONG - 8)) >> 3);
 	asm volatile(
 		"	nc	%O0(1,%R0),%1"
 		: "=Q" (*(char *) addr) : "Q" (_ni_bitmap[nr & 7]) : "cc" );
@@ -257,7 +252,7 @@ __constant_clear_bit(const unsigned long nr, volatile unsigned long *ptr)
 {
 	unsigned long addr;
 
-	addr = ((unsigned long) ptr) + ((nr ^ (__BITOPS_WORDSIZE - 8)) >> 3);
+	addr = ((unsigned long) ptr) + ((nr ^ (BITS_PER_LONG - 8)) >> 3);
 	*(unsigned char *) addr &= ~(1 << (nr & 7));
 }
 
@@ -273,7 +268,7 @@ static inline void __change_bit(unsigned long nr, volatile unsigned long *ptr)
 {
 	unsigned long addr;
 
-	addr = (unsigned long) ptr + ((nr ^ (__BITOPS_WORDSIZE - 8)) >> 3);
+	addr = (unsigned long) ptr + ((nr ^ (BITS_PER_LONG - 8)) >> 3);
 	asm volatile(
 		"	xc	%O0(1,%R0),%1"
 		: "=Q" (*(char *) addr) : "Q" (_oi_bitmap[nr & 7]) : "cc" );
@@ -284,7 +279,7 @@ __constant_change_bit(const unsigned long nr, volatile unsigned long *ptr)
 {
 	unsigned long addr;
 
-	addr = ((unsigned long) ptr) + ((nr ^ (__BITOPS_WORDSIZE - 8)) >> 3);
+	addr = ((unsigned long) ptr) + ((nr ^ (BITS_PER_LONG - 8)) >> 3);
 	*(unsigned char *) addr ^= 1 << (nr & 7);
 }
 
@@ -302,7 +297,7 @@ test_and_set_bit_simple(unsigned long nr, volatile unsigned long *ptr)
 	unsigned long addr;
 	unsigned char ch;
 
-	addr = (unsigned long) ptr + ((nr ^ (__BITOPS_WORDSIZE - 8)) >> 3);
+	addr = (unsigned long) ptr + ((nr ^ (BITS_PER_LONG - 8)) >> 3);
 	ch = *(unsigned char *) addr;
 	asm volatile(
 		"	oc	%O0(1,%R0),%1"
@@ -321,7 +316,7 @@ test_and_clear_bit_simple(unsigned long nr, volatile unsigned long *ptr)
 	unsigned long addr;
 	unsigned char ch;
 
-	addr = (unsigned long) ptr + ((nr ^ (__BITOPS_WORDSIZE - 8)) >> 3);
+	addr = (unsigned long) ptr + ((nr ^ (BITS_PER_LONG - 8)) >> 3);
 	ch = *(unsigned char *) addr;
 	asm volatile(
 		"	nc	%O0(1,%R0),%1"
@@ -340,7 +335,7 @@ test_and_change_bit_simple(unsigned long nr, volatile unsigned long *ptr)
 	unsigned long addr;
 	unsigned char ch;
 
-	addr = (unsigned long) ptr + ((nr ^ (__BITOPS_WORDSIZE - 8)) >> 3);
+	addr = (unsigned long) ptr + ((nr ^ (BITS_PER_LONG - 8)) >> 3);
 	ch = *(unsigned char *) addr;
 	asm volatile(
 		"	xc	%O0(1,%R0),%1"
@@ -376,7 +371,7 @@ static inline int __test_bit(unsigned long nr, const volatile unsigned long *ptr
 	unsigned long addr;
 	unsigned char ch;
 
-	addr = (unsigned long) ptr + ((nr ^ (__BITOPS_WORDSIZE - 8)) >> 3);
+	addr = (unsigned long) ptr + ((nr ^ (BITS_PER_LONG - 8)) >> 3);
 	ch = *(volatile unsigned char *) addr;
 	return (ch >> (nr & 7)) & 1;
 }
@@ -384,7 +379,7 @@ static inline int __test_bit(unsigned long nr, const volatile unsigned long *ptr
 static inline int 
 __constant_test_bit(unsigned long nr, const volatile unsigned long *addr) {
     return (((volatile char *) addr)
-	    [(nr^(__BITOPS_WORDSIZE-8))>>3] & (1<<(nr&7))) != 0;
+	    [(nr^(BITS_PER_LONG-8))>>3] & (1<<(nr&7))) != 0;
 }
 
 #define test_bit(nr,addr) \
@@ -693,18 +688,18 @@ static inline int find_next_bit_left(const unsigned long *addr,
 
 	if (offset >= size)
 		return size;
-	bit = offset & (__BITOPS_WORDSIZE - 1);
+	bit = offset & (BITS_PER_LONG - 1);
 	offset -= bit;
 	size -= offset;
-	p = addr + offset / __BITOPS_WORDSIZE;
+	p = addr + offset / BITS_PER_LONG;
 	if (bit) {
 		set = __flo_word(0, *p & (~0UL << bit));
 		if (set >= size)
 			return size + offset;
-		if (set < __BITOPS_WORDSIZE)
+		if (set < BITS_PER_LONG)
 			return set + offset;
-		offset += __BITOPS_WORDSIZE;
-		size -= __BITOPS_WORDSIZE;
+		offset += BITS_PER_LONG;
+		size -= BITS_PER_LONG;
 		p++;
 	}
 	return offset + find_first_bit_left(p, size);
@@ -736,22 +731,22 @@ static inline int find_next_zero_bit (const unsigned long * addr,
 
 	if (offset >= size)
 		return size;
-	bit = offset & (__BITOPS_WORDSIZE - 1);
+	bit = offset & (BITS_PER_LONG - 1);
 	offset -= bit;
 	size -= offset;
-	p = addr + offset / __BITOPS_WORDSIZE;
+	p = addr + offset / BITS_PER_LONG;
 	if (bit) {
 		/*
-		 * __ffz_word returns __BITOPS_WORDSIZE
+		 * __ffz_word returns BITS_PER_LONG
 		 * if no zero bit is present in the word.
 		 */
 		set = __ffz_word(bit, *p >> bit);
 		if (set >= size)
 			return size + offset;
-		if (set < __BITOPS_WORDSIZE)
+		if (set < BITS_PER_LONG)
 			return set + offset;
-		offset += __BITOPS_WORDSIZE;
-		size -= __BITOPS_WORDSIZE;
+		offset += BITS_PER_LONG;
+		size -= BITS_PER_LONG;
 		p++;
 	}
 	return offset + find_first_zero_bit(p, size);
@@ -773,22 +768,22 @@ static inline int find_next_bit (const unsigned long * addr,
 
 	if (offset >= size)
 		return size;
-	bit = offset & (__BITOPS_WORDSIZE - 1);
+	bit = offset & (BITS_PER_LONG - 1);
 	offset -= bit;
 	size -= offset;
-	p = addr + offset / __BITOPS_WORDSIZE;
+	p = addr + offset / BITS_PER_LONG;
 	if (bit) {
 		/*
-		 * __ffs_word returns __BITOPS_WORDSIZE
+		 * __ffs_word returns BITS_PER_LONG
 		 * if no one bit is present in the word.
 		 */
 		set = __ffs_word(0, *p & (~0UL << bit));
 		if (set >= size)
 			return size + offset;
-		if (set < __BITOPS_WORDSIZE)
+		if (set < BITS_PER_LONG)
 			return set + offset;
-		offset += __BITOPS_WORDSIZE;
-		size -= __BITOPS_WORDSIZE;
+		offset += BITS_PER_LONG;
+		size -= BITS_PER_LONG;
 		p++;
 	}
 	return offset + find_first_bit(p, size);
@@ -843,22 +838,22 @@ static inline int find_next_zero_bit_le(void *vaddr, unsigned long size,
 
         if (offset >= size)
                 return size;
-	bit = offset & (__BITOPS_WORDSIZE - 1);
+	bit = offset & (BITS_PER_LONG - 1);
 	offset -= bit;
 	size -= offset;
-	p = addr + offset / __BITOPS_WORDSIZE;
+	p = addr + offset / BITS_PER_LONG;
         if (bit) {
 		/*
-		 * s390 version of ffz returns __BITOPS_WORDSIZE
+		 * s390 version of ffz returns BITS_PER_LONG
 		 * if no zero bit is present in the word.
 		 */
 		set = __ffz_word(bit, __load_ulong_le(p, 0) >> bit);
 		if (set >= size)
 			return size + offset;
-		if (set < __BITOPS_WORDSIZE)
+		if (set < BITS_PER_LONG)
 			return set + offset;
-		offset += __BITOPS_WORDSIZE;
-		size -= __BITOPS_WORDSIZE;
+		offset += BITS_PER_LONG;
+		size -= BITS_PER_LONG;
 		p++;
         }
 	return offset + find_first_zero_bit_le(p, size);
@@ -885,22 +880,22 @@ static inline int find_next_bit_le(void *vaddr, unsigned long size,
 
 	if (offset >= size)
 		return size;
-	bit = offset & (__BITOPS_WORDSIZE - 1);
+	bit = offset & (BITS_PER_LONG - 1);
 	offset -= bit;
 	size -= offset;
-	p = addr + offset / __BITOPS_WORDSIZE;
+	p = addr + offset / BITS_PER_LONG;
 	if (bit) {
 		/*
-		 * s390 version of ffz returns __BITOPS_WORDSIZE
+		 * s390 version of ffz returns BITS_PER_LONG
 		 * if no zero bit is present in the word.
 		 */
 		set = __ffs_word(0, __load_ulong_le(p, 0) & (~0UL << bit));
 		if (set >= size)
 			return size + offset;
-		if (set < __BITOPS_WORDSIZE)
+		if (set < BITS_PER_LONG)
 			return set + offset;
-		offset += __BITOPS_WORDSIZE;
-		size -= __BITOPS_WORDSIZE;
+		offset += BITS_PER_LONG;
+		size -= BITS_PER_LONG;
 		p++;
 	}
 	return offset + find_first_bit_le(p, size);
diff --git a/arch/s390/include/asm/ccwdev.h b/arch/s390/include/asm/ccwdev.h
index e6061617a50..f201af8be58 100644
--- a/arch/s390/include/asm/ccwdev.h
+++ b/arch/s390/include/asm/ccwdev.h
@@ -220,7 +220,8 @@ extern void ccw_device_get_id(struct ccw_device *, struct ccw_dev_id *);
 #define to_ccwdrv(n) container_of(n, struct ccw_driver, driver)
 
 extern struct ccw_device *ccw_device_probe_console(void);
-extern int ccw_device_force_console(void);
+extern void ccw_device_wait_idle(struct ccw_device *);
+extern int ccw_device_force_console(struct ccw_device *);
 
 int ccw_device_siosl(struct ccw_device *);
 
diff --git a/arch/s390/include/asm/cio.h b/arch/s390/include/asm/cio.h
index ad2b924167d..ffb898961c8 100644
--- a/arch/s390/include/asm/cio.h
+++ b/arch/s390/include/asm/cio.h
@@ -296,8 +296,6 @@ static inline int ccw_dev_id_is_equal(struct ccw_dev_id *dev_id1,
 	return 0;
 }
 
-extern void wait_cons_dev(void);
-
 extern void css_schedule_reprobe(void);
 
 extern void reipl_ccw_dev(struct ccw_dev_id *id);
diff --git a/arch/s390/include/asm/compat.h b/arch/s390/include/asm/compat.h
index f8c6df6cd1f..c1e7c646727 100644
--- a/arch/s390/include/asm/compat.h
+++ b/arch/s390/include/asm/compat.h
@@ -70,6 +70,22 @@ typedef u32		compat_ulong_t;
 typedef u64		compat_u64;
 typedef u32		compat_uptr_t;
 
+typedef struct {
+	u32 mask;
+	u32 addr;
+} __aligned(8) psw_compat_t;
+
+typedef struct {
+	psw_compat_t psw;
+	u32 gprs[NUM_GPRS];
+	u32 acrs[NUM_ACRS];
+	u32 orig_gpr2;
+} s390_compat_regs;
+
+typedef struct {
+	u32 gprs_high[NUM_GPRS];
+} s390_compat_regs_high;
+
 struct compat_timespec {
 	compat_time_t	tv_sec;
 	s32		tv_nsec;
@@ -124,18 +140,33 @@ struct compat_flock64 {
 };
 
 struct compat_statfs {
-	s32		f_type;
-	s32		f_bsize;
-	s32		f_blocks;
-	s32		f_bfree;
-	s32		f_bavail;
-	s32		f_files;
-	s32		f_ffree;
+	u32		f_type;
+	u32		f_bsize;
+	u32		f_blocks;
+	u32		f_bfree;
+	u32		f_bavail;
+	u32		f_files;
+	u32		f_ffree;
+	compat_fsid_t	f_fsid;
+	u32		f_namelen;
+	u32		f_frsize;
+	u32		f_flags;
+	u32		f_spare[4];
+};
+
+struct compat_statfs64 {
+	u32		f_type;
+	u32		f_bsize;
+	u64		f_blocks;
+	u64		f_bfree;
+	u64		f_bavail;
+	u64		f_files;
+	u64		f_ffree;
 	compat_fsid_t	f_fsid;
-	s32		f_namelen;
-	s32		f_frsize;
-	s32		f_flags;
-	s32		f_spare[5];
+	u32		f_namelen;
+	u32		f_frsize;
+	u32		f_flags;
+	u32		f_spare[4];
 };
 
 #define COMPAT_RLIM_OLD_INFINITY	0x7fffffff
@@ -248,8 +279,6 @@ static inline int is_compat_task(void)
 	return is_32bit_task();
 }
 
-#endif
-
 static inline void __user *arch_compat_alloc_user_space(long len)
 {
 	unsigned long stack;
@@ -260,6 +289,8 @@ static inline void __user *arch_compat_alloc_user_space(long len)
 	return (void __user *) (stack - len);
 }
 
+#endif
+
 struct compat_ipc64_perm {
 	compat_key_t key;
 	__compat_uid32_t uid;
diff --git a/arch/s390/include/asm/elf.h b/arch/s390/include/asm/elf.h
index 1bfdf24b85a..78f4f8711d5 100644
--- a/arch/s390/include/asm/elf.h
+++ b/arch/s390/include/asm/elf.h
@@ -119,6 +119,8 @@
  */
 
 #include <asm/ptrace.h>
+#include <asm/compat.h>
+#include <asm/syscall.h>
 #include <asm/user.h>
 
 typedef s390_fp_regs elf_fpregset_t;
@@ -180,18 +182,31 @@ extern unsigned long elf_hwcap;
 extern char elf_platform[];
 #define ELF_PLATFORM (elf_platform)
 
-#ifdef CONFIG_64BIT
+#ifndef CONFIG_COMPAT
+#define SET_PERSONALITY(ex) \
+do {								\
+	set_personality(PER_LINUX |				\
+		(current->personality & (~PER_MASK)));		\
+	current_thread_info()->sys_call_table = 		\
+		(unsigned long) &sys_call_table;		\
+} while (0)
+#else /* CONFIG_COMPAT */
 #define SET_PERSONALITY(ex)					\
 do {								\
 	if (personality(current->personality) != PER_LINUX32)	\
 		set_personality(PER_LINUX |			\
 			(current->personality & ~PER_MASK));	\
-	if ((ex).e_ident[EI_CLASS] == ELFCLASS32)		\
+	if ((ex).e_ident[EI_CLASS] == ELFCLASS32) {		\
 		set_thread_flag(TIF_31BIT);			\
-	else							\
+		current_thread_info()->sys_call_table =		\
+			(unsigned long)	&sys_call_table_emu;	\
+	} else {						\
 		clear_thread_flag(TIF_31BIT);			\
+		current_thread_info()->sys_call_table =		\
+			(unsigned long) &sys_call_table;	\
+	}							\
 } while (0)
-#endif /* CONFIG_64BIT */
+#endif /* CONFIG_COMPAT */
 
 #define STACK_RND_MASK	0x7ffUL
 
diff --git a/arch/s390/include/asm/hugetlb.h b/arch/s390/include/asm/hugetlb.h
index 593753ee07f..bd90359d6d2 100644
--- a/arch/s390/include/asm/hugetlb.h
+++ b/arch/s390/include/asm/hugetlb.h
@@ -114,7 +114,7 @@ static inline pte_t huge_ptep_get_and_clear(struct mm_struct *mm,
 #define huge_ptep_set_wrprotect(__mm, __addr, __ptep)			\
 ({									\
 	pte_t __pte = huge_ptep_get(__ptep);				\
-	if (pte_write(__pte)) {						\
+	if (huge_pte_write(__pte)) {					\
 		huge_ptep_invalidate(__mm, __addr, __ptep);		\
 		set_huge_pte_at(__mm, __addr, __ptep,			\
 				huge_pte_wrprotect(__pte));		\
@@ -127,4 +127,58 @@ static inline void huge_ptep_clear_flush(struct vm_area_struct *vma,
 	huge_ptep_invalidate(vma->vm_mm, address, ptep);
 }
 
+static inline pte_t mk_huge_pte(struct page *page, pgprot_t pgprot)
+{
+	pte_t pte;
+	pmd_t pmd;
+
+	pmd = mk_pmd_phys(page_to_phys(page), pgprot);
+	pte_val(pte) = pmd_val(pmd);
+	return pte;
+}
+
+static inline int huge_pte_write(pte_t pte)
+{
+	pmd_t pmd;
+
+	pmd_val(pmd) = pte_val(pte);
+	return pmd_write(pmd);
+}
+
+static inline int huge_pte_dirty(pte_t pte)
+{
+	/* No dirty bit in the segment table entry. */
+	return 0;
+}
+
+static inline pte_t huge_pte_mkwrite(pte_t pte)
+{
+	pmd_t pmd;
+
+	pmd_val(pmd) = pte_val(pte);
+	pte_val(pte) = pmd_val(pmd_mkwrite(pmd));
+	return pte;
+}
+
+static inline pte_t huge_pte_mkdirty(pte_t pte)
+{
+	/* No dirty bit in the segment table entry. */
+	return pte;
+}
+
+static inline pte_t huge_pte_modify(pte_t pte, pgprot_t newprot)
+{
+	pmd_t pmd;
+
+	pmd_val(pmd) = pte_val(pte);
+	pte_val(pte) = pmd_val(pmd_modify(pmd, newprot));
+	return pte;
+}
+
+static inline void huge_pte_clear(struct mm_struct *mm, unsigned long addr,
+				  pte_t *ptep)
+{
+	pmd_clear((pmd_t *) ptep);
+}
+
 #endif /* _ASM_S390_HUGETLB_H */
diff --git a/arch/s390/include/asm/io.h b/arch/s390/include/asm/io.h
index 27cb32185ce..379d96e2105 100644
--- a/arch/s390/include/asm/io.h
+++ b/arch/s390/include/asm/io.h
@@ -50,10 +50,6 @@ void unxlate_dev_mem_ptr(unsigned long phys, void *addr);
 #define ioremap_nocache(addr, size)	ioremap(addr, size)
 #define ioremap_wc			ioremap_nocache
 
-/* TODO: s390 cannot support io_remap_pfn_range... */
-#define io_remap_pfn_range(vma, vaddr, pfn, size, prot) 	       \
-	remap_pfn_range(vma, vaddr, pfn, size, prot)
-
 static inline void __iomem *ioremap(unsigned long offset, unsigned long size)
 {
 	return (void __iomem *) offset;
diff --git a/arch/s390/include/asm/pci.h b/arch/s390/include/asm/pci.h
index 05333b7f046..6c1801235db 100644
--- a/arch/s390/include/asm/pci.h
+++ b/arch/s390/include/asm/pci.h
@@ -140,6 +140,7 @@ static inline bool zdev_enabled(struct zpci_dev *zdev)
 struct zpci_dev *zpci_alloc_device(void);
 int zpci_create_device(struct zpci_dev *);
 int zpci_enable_device(struct zpci_dev *);
+int zpci_disable_device(struct zpci_dev *);
 void zpci_stop_device(struct zpci_dev *);
 void zpci_free_device(struct zpci_dev *);
 int zpci_scan_device(struct zpci_dev *);
diff --git a/arch/s390/include/asm/pci_debug.h b/arch/s390/include/asm/pci_debug.h
index 6bbec4265b6..1ca5d1047c7 100644
--- a/arch/s390/include/asm/pci_debug.h
+++ b/arch/s390/include/asm/pci_debug.h
@@ -7,14 +7,11 @@ extern debug_info_t *pci_debug_msg_id;
 extern debug_info_t *pci_debug_err_id;
 
 #ifdef CONFIG_PCI_DEBUG
-#define zpci_dbg(fmt, args...)							\
-	do {									\
-		if (pci_debug_msg_id->level >= 2)				\
-			debug_sprintf_event(pci_debug_msg_id, 2, fmt , ## args);\
-	} while (0)
+#define zpci_dbg(imp, fmt, args...)				\
+	debug_sprintf_event(pci_debug_msg_id, imp, fmt, ##args)
 
 #else /* !CONFIG_PCI_DEBUG */
-#define zpci_dbg(fmt, args...) do { } while (0)
+#define zpci_dbg(imp, fmt, args...) do { } while (0)
 #endif
 
 #define zpci_err(text...)							\
diff --git a/arch/s390/include/asm/pci_insn.h b/arch/s390/include/asm/pci_insn.h
index 1486a98d5da..e6a2bdd4d70 100644
--- a/arch/s390/include/asm/pci_insn.h
+++ b/arch/s390/include/asm/pci_insn.h
@@ -1,10 +1,6 @@
 #ifndef _ASM_S390_PCI_INSN_H
 #define _ASM_S390_PCI_INSN_H
 
-#include <linux/delay.h>
-
-#define ZPCI_INSN_BUSY_DELAY	1	/* 1 microsecond */
-
 /* Load/Store status codes */
 #define ZPCI_PCI_ST_FUNC_NOT_ENABLED		4
 #define ZPCI_PCI_ST_FUNC_IN_ERR			8
@@ -82,199 +78,12 @@ struct zpci_fib {
 	u64 reserved7;
 } __packed;
 
-/* Modify PCI Function Controls */
-static inline u8 __mpcifc(u64 req, struct zpci_fib *fib, u8 *status)
-{
-	u8 cc;
-
-	asm volatile (
-		"	.insn	rxy,0xe300000000d0,%[req],%[fib]\n"
-		"	ipm	%[cc]\n"
-		"	srl	%[cc],28\n"
-		: [cc] "=d" (cc), [req] "+d" (req), [fib] "+Q" (*fib)
-		: : "cc");
-	*status = req >> 24 & 0xff;
-	return cc;
-}
-
-static inline int mpcifc_instr(u64 req, struct zpci_fib *fib)
-{
-	u8 cc, status;
-
-	do {
-		cc = __mpcifc(req, fib, &status);
-		if (cc == 2)
-			msleep(ZPCI_INSN_BUSY_DELAY);
-	} while (cc == 2);
-
-	if (cc)
-		printk_once(KERN_ERR "%s: error cc: %d  status: %d\n",
-			     __func__, cc, status);
-	return (cc) ? -EIO : 0;
-}
-
-/* Refresh PCI Translations */
-static inline u8 __rpcit(u64 fn, u64 addr, u64 range, u8 *status)
-{
-	register u64 __addr asm("2") = addr;
-	register u64 __range asm("3") = range;
-	u8 cc;
-
-	asm volatile (
-		"	.insn	rre,0xb9d30000,%[fn],%[addr]\n"
-		"	ipm	%[cc]\n"
-		"	srl	%[cc],28\n"
-		: [cc] "=d" (cc), [fn] "+d" (fn)
-		: [addr] "d" (__addr), "d" (__range)
-		: "cc");
-	*status = fn >> 24 & 0xff;
-	return cc;
-}
-
-static inline int rpcit_instr(u64 fn, u64 addr, u64 range)
-{
-	u8 cc, status;
-
-	do {
-		cc = __rpcit(fn, addr, range, &status);
-		if (cc == 2)
-			udelay(ZPCI_INSN_BUSY_DELAY);
-	} while (cc == 2);
-
-	if (cc)
-		printk_once(KERN_ERR "%s: error cc: %d  status: %d  dma_addr: %Lx  size: %Lx\n",
-			    __func__, cc, status, addr, range);
-	return (cc) ? -EIO : 0;
-}
-
-/* Store PCI function controls */
-static inline u8 __stpcifc(u32 handle, u8 space, struct zpci_fib *fib, u8 *status)
-{
-	u64 fn = (u64) handle << 32 | space << 16;
-	u8 cc;
-
-	asm volatile (
-		"	.insn	rxy,0xe300000000d4,%[fn],%[fib]\n"
-		"	ipm	%[cc]\n"
-		"	srl	%[cc],28\n"
-		: [cc] "=d" (cc), [fn] "+d" (fn), [fib] "=m" (*fib)
-		: : "cc");
-	*status = fn >> 24 & 0xff;
-	return cc;
-}
-
-/* Set Interruption Controls */
-static inline void sic_instr(u16 ctl, char *unused, u8 isc)
-{
-	asm volatile (
-		"	.insn	rsy,0xeb00000000d1,%[ctl],%[isc],%[u]\n"
-		: : [ctl] "d" (ctl), [isc] "d" (isc << 27), [u] "Q" (*unused));
-}
-
-/* PCI Load */
-static inline u8 __pcilg(u64 *data, u64 req, u64 offset, u8 *status)
-{
-	register u64 __req asm("2") = req;
-	register u64 __offset asm("3") = offset;
-	u64 __data;
-	u8 cc;
-
-	asm volatile (
-		"	.insn	rre,0xb9d20000,%[data],%[req]\n"
-		"	ipm	%[cc]\n"
-		"	srl	%[cc],28\n"
-		: [cc] "=d" (cc), [data] "=d" (__data), [req] "+d" (__req)
-		:  "d" (__offset)
-		: "cc");
-	*status = __req >> 24 & 0xff;
-	*data = __data;
-	return cc;
-}
-
-static inline int pcilg_instr(u64 *data, u64 req, u64 offset)
-{
-	u8 cc, status;
-
-	do {
-		cc = __pcilg(data, req, offset, &status);
-		if (cc == 2)
-			udelay(ZPCI_INSN_BUSY_DELAY);
-	} while (cc == 2);
-
-	if (cc) {
-		printk_once(KERN_ERR "%s: error cc: %d  status: %d  req: %Lx  offset: %Lx\n",
-			    __func__, cc, status, req, offset);
-		/* TODO: on IO errors set data to 0xff...
-		 * here or in users of pcilg (le conversion)?
-		 */
-	}
-	return (cc) ? -EIO : 0;
-}
-
-/* PCI Store */
-static inline u8 __pcistg(u64 data, u64 req, u64 offset, u8 *status)
-{
-	register u64 __req asm("2") = req;
-	register u64 __offset asm("3") = offset;
-	u8 cc;
-
-	asm volatile (
-		"	.insn	rre,0xb9d00000,%[data],%[req]\n"
-		"	ipm	%[cc]\n"
-		"	srl	%[cc],28\n"
-		: [cc] "=d" (cc), [req] "+d" (__req)
-		: "d" (__offset), [data] "d" (data)
-		: "cc");
-	*status = __req >> 24 & 0xff;
-	return cc;
-}
-
-static inline int pcistg_instr(u64 data, u64 req, u64 offset)
-{
-	u8 cc, status;
-
-	do {
-		cc = __pcistg(data, req, offset, &status);
-		if (cc == 2)
-			udelay(ZPCI_INSN_BUSY_DELAY);
-	} while (cc == 2);
-
-	if (cc)
-		printk_once(KERN_ERR "%s: error cc: %d  status: %d  req: %Lx  offset: %Lx\n",
-			__func__, cc, status, req, offset);
-	return (cc) ? -EIO : 0;
-}
-
-/* PCI Store Block */
-static inline u8 __pcistb(const u64 *data, u64 req, u64 offset, u8 *status)
-{
-	u8 cc;
-
-	asm volatile (
-		"	.insn	rsy,0xeb00000000d0,%[req],%[offset],%[data]\n"
-		"	ipm	%[cc]\n"
-		"	srl	%[cc],28\n"
-		: [cc] "=d" (cc), [req] "+d" (req)
-		: [offset] "d" (offset), [data] "Q" (*data)
-		: "cc");
-	*status = req >> 24 & 0xff;
-	return cc;
-}
-
-static inline int pcistb_instr(const u64 *data, u64 req, u64 offset)
-{
-	u8 cc, status;
-
-	do {
-		cc = __pcistb(data, req, offset, &status);
-		if (cc == 2)
-			udelay(ZPCI_INSN_BUSY_DELAY);
-	} while (cc == 2);
 
-	if (cc)
-		printk_once(KERN_ERR "%s: error cc: %d  status: %d  req: %Lx  offset: %Lx\n",
-			    __func__, cc, status, req, offset);
-	return (cc) ? -EIO : 0;
-}
+int s390pci_mod_fc(u64 req, struct zpci_fib *fib);
+int s390pci_refresh_trans(u64 fn, u64 addr, u64 range);
+int s390pci_load(u64 *data, u64 req, u64 offset);
+int s390pci_store(u64 data, u64 req, u64 offset);
+int s390pci_store_block(const u64 *data, u64 req, u64 offset);
+void set_irq_ctrl(u16 ctl, char *unused, u8 isc);
 
 #endif
diff --git a/arch/s390/include/asm/pci_io.h b/arch/s390/include/asm/pci_io.h
index 5fd81f31d6c..83a9caa6ae5 100644
--- a/arch/s390/include/asm/pci_io.h
+++ b/arch/s390/include/asm/pci_io.h
@@ -36,7 +36,7 @@ static inline RETTYPE zpci_read_##RETTYPE(const volatile void __iomem *addr)	\
 	u64 data;								\
 	int rc;									\
 										\
-	rc = pcilg_instr(&data, req, ZPCI_OFFSET(addr));			\
+	rc = s390pci_load(&data, req, ZPCI_OFFSET(addr));			\
 	if (rc)									\
 		data = -1ULL;							\
 	return (RETTYPE) data;							\
@@ -50,7 +50,7 @@ static inline void zpci_write_##VALTYPE(VALTYPE val,				\
 	u64 req = ZPCI_CREATE_REQ(entry->fh, entry->bar, LENGTH);		\
 	u64 data = (VALTYPE) val;						\
 										\
-	pcistg_instr(data, req, ZPCI_OFFSET(addr));				\
+	s390pci_store(data, req, ZPCI_OFFSET(addr));				\
 }
 
 zpci_read(8, u64)
@@ -83,15 +83,18 @@ static inline int zpci_write_single(u64 req, const u64 *data, u64 offset, u8 len
 		val = 0;		/* let FW report error */
 		break;
 	}
-	return pcistg_instr(val, req, offset);
+	return s390pci_store(val, req, offset);
 }
 
 static inline int zpci_read_single(u64 req, u64 *dst, u64 offset, u8 len)
 {
 	u64 data;
-	u8 cc;
+	int cc;
+
+	cc = s390pci_load(&data, req, offset);
+	if (cc)
+		goto out;
 
-	cc = pcilg_instr(&data,	 req, offset);
 	switch (len) {
 	case 1:
 		*((u8 *) dst) = (u8) data;
@@ -106,12 +109,13 @@ static inline int zpci_read_single(u64 req, u64 *dst, u64 offset, u8 len)
 		*((u64 *) dst) = (u64) data;
 		break;
 	}
+out:
 	return cc;
 }
 
 static inline int zpci_write_block(u64 req, const u64 *data, u64 offset)
 {
-	return pcistb_instr(data, req, offset);
+	return s390pci_store_block(data, req, offset);
 }
 
 static inline u8 zpci_get_max_write_size(u64 src, u64 dst, int len, int max)
diff --git a/arch/s390/include/asm/pgtable.h b/arch/s390/include/asm/pgtable.h
index 4a2930844d4..b4622915bd1 100644
--- a/arch/s390/include/asm/pgtable.h
+++ b/arch/s390/include/asm/pgtable.h
@@ -57,6 +57,10 @@ extern unsigned long zero_page_mask;
 	 (((unsigned long)(vaddr)) &zero_page_mask))))
 #define __HAVE_COLOR_ZERO_PAGE
 
+/* TODO: s390 cannot support io_remap_pfn_range... */
+#define io_remap_pfn_range(vma, vaddr, pfn, size, prot) 	       \
+	remap_pfn_range(vma, vaddr, pfn, size, prot)
+
 #endif /* !__ASSEMBLY__ */
 
 /*
@@ -344,6 +348,7 @@ extern unsigned long MODULES_END;
 #define _REGION3_ENTRY_CO	0x100	/* change-recording override	    */
 
 /* Bits in the segment table entry */
+#define _SEGMENT_ENTRY_ORIGIN_LARGE ~0xfffffUL /* large page address	    */
 #define _SEGMENT_ENTRY_ORIGIN	~0x7ffUL/* segment table origin		    */
 #define _SEGMENT_ENTRY_RO	0x200	/* page protection bit		    */
 #define _SEGMENT_ENTRY_INV	0x20	/* invalid segment table entry	    */
@@ -419,6 +424,13 @@ extern unsigned long MODULES_END;
 #define __S110	PAGE_RW
 #define __S111	PAGE_RW
 
+/*
+ * Segment entry (large page) protection definitions.
+ */
+#define SEGMENT_NONE	__pgprot(_HPAGE_TYPE_NONE)
+#define SEGMENT_RO	__pgprot(_HPAGE_TYPE_RO)
+#define SEGMENT_RW	__pgprot(_HPAGE_TYPE_RW)
+
 static inline int mm_exclusive(struct mm_struct *mm)
 {
 	return likely(mm == current->active_mm &&
@@ -759,6 +771,8 @@ void gmap_disable(struct gmap *gmap);
 int gmap_map_segment(struct gmap *gmap, unsigned long from,
 		     unsigned long to, unsigned long length);
 int gmap_unmap_segment(struct gmap *gmap, unsigned long to, unsigned long len);
+unsigned long __gmap_translate(unsigned long address, struct gmap *);
+unsigned long gmap_translate(unsigned long address, struct gmap *);
 unsigned long __gmap_fault(unsigned long address, struct gmap *);
 unsigned long gmap_fault(unsigned long address, struct gmap *);
 void gmap_discard(unsigned long from, unsigned long to, struct gmap *);
@@ -907,26 +921,6 @@ static inline pte_t pte_mkspecial(pte_t pte)
 #ifdef CONFIG_HUGETLB_PAGE
 static inline pte_t pte_mkhuge(pte_t pte)
 {
-	/*
-	 * PROT_NONE needs to be remapped from the pte type to the ste type.
-	 * The HW invalid bit is also different for pte and ste. The pte
-	 * invalid bit happens to be the same as the ste _SEGMENT_ENTRY_LARGE
-	 * bit, so we don't have to clear it.
-	 */
-	if (pte_val(pte) & _PAGE_INVALID) {
-		if (pte_val(pte) & _PAGE_SWT)
-			pte_val(pte) |= _HPAGE_TYPE_NONE;
-		pte_val(pte) |= _SEGMENT_ENTRY_INV;
-	}
-	/*
-	 * Clear SW pte bits, there are no SW bits in a segment table entry.
-	 */
-	pte_val(pte) &= ~(_PAGE_SWT | _PAGE_SWX | _PAGE_SWC |
-			  _PAGE_SWR | _PAGE_SWW);
-	/*
-	 * Also set the change-override bit because we don't need dirty bit
-	 * tracking for hugetlbfs pages.
-	 */
 	pte_val(pte) |= (_SEGMENT_ENTRY_LARGE | _SEGMENT_ENTRY_CO);
 	return pte;
 }
@@ -1271,31 +1265,7 @@ static inline void __pmd_idte(unsigned long address, pmd_t *pmdp)
 	}
 }
 
-#ifdef CONFIG_TRANSPARENT_HUGEPAGE
-
-#define SEGMENT_NONE	__pgprot(_HPAGE_TYPE_NONE)
-#define SEGMENT_RO	__pgprot(_HPAGE_TYPE_RO)
-#define SEGMENT_RW	__pgprot(_HPAGE_TYPE_RW)
-
-#define __HAVE_ARCH_PGTABLE_DEPOSIT
-extern void pgtable_trans_huge_deposit(struct mm_struct *mm, pgtable_t pgtable);
-
-#define __HAVE_ARCH_PGTABLE_WITHDRAW
-extern pgtable_t pgtable_trans_huge_withdraw(struct mm_struct *mm);
-
-static inline int pmd_trans_splitting(pmd_t pmd)
-{
-	return pmd_val(pmd) & _SEGMENT_ENTRY_SPLIT;
-}
-
-static inline void set_pmd_at(struct mm_struct *mm, unsigned long addr,
-			      pmd_t *pmdp, pmd_t entry)
-{
-	if (!(pmd_val(entry) & _SEGMENT_ENTRY_INV) && MACHINE_HAS_EDAT1)
-		pmd_val(entry) |= _SEGMENT_ENTRY_CO;
-	*pmdp = entry;
-}
-
+#if defined(CONFIG_TRANSPARENT_HUGEPAGE) || defined(CONFIG_HUGETLB_PAGE)
 static inline unsigned long massage_pgprot_pmd(pgprot_t pgprot)
 {
 	/*
@@ -1316,10 +1286,11 @@ static inline pmd_t pmd_modify(pmd_t pmd, pgprot_t newprot)
 	return pmd;
 }
 
-static inline pmd_t pmd_mkhuge(pmd_t pmd)
+static inline pmd_t mk_pmd_phys(unsigned long physpage, pgprot_t pgprot)
 {
-	pmd_val(pmd) |= _SEGMENT_ENTRY_LARGE;
-	return pmd;
+	pmd_t __pmd;
+	pmd_val(__pmd) = physpage + massage_pgprot_pmd(pgprot);
+	return __pmd;
 }
 
 static inline pmd_t pmd_mkwrite(pmd_t pmd)
@@ -1329,6 +1300,34 @@ static inline pmd_t pmd_mkwrite(pmd_t pmd)
 		pmd_val(pmd) &= ~_SEGMENT_ENTRY_RO;
 	return pmd;
 }
+#endif /* CONFIG_TRANSPARENT_HUGEPAGE || CONFIG_HUGETLB_PAGE */
+
+#ifdef CONFIG_TRANSPARENT_HUGEPAGE
+
+#define __HAVE_ARCH_PGTABLE_DEPOSIT
+extern void pgtable_trans_huge_deposit(struct mm_struct *mm, pgtable_t pgtable);
+
+#define __HAVE_ARCH_PGTABLE_WITHDRAW
+extern pgtable_t pgtable_trans_huge_withdraw(struct mm_struct *mm);
+
+static inline int pmd_trans_splitting(pmd_t pmd)
+{
+	return pmd_val(pmd) & _SEGMENT_ENTRY_SPLIT;
+}
+
+static inline void set_pmd_at(struct mm_struct *mm, unsigned long addr,
+			      pmd_t *pmdp, pmd_t entry)
+{
+	if (!(pmd_val(entry) & _SEGMENT_ENTRY_INV) && MACHINE_HAS_EDAT1)
+		pmd_val(entry) |= _SEGMENT_ENTRY_CO;
+	*pmdp = entry;
+}
+
+static inline pmd_t pmd_mkhuge(pmd_t pmd)
+{
+	pmd_val(pmd) |= _SEGMENT_ENTRY_LARGE;
+	return pmd;
+}
 
 static inline pmd_t pmd_wrprotect(pmd_t pmd)
 {
@@ -1425,13 +1424,6 @@ static inline void pmdp_set_wrprotect(struct mm_struct *mm,
 	}
 }
 
-static inline pmd_t mk_pmd_phys(unsigned long physpage, pgprot_t pgprot)
-{
-	pmd_t __pmd;
-	pmd_val(__pmd) = physpage + massage_pgprot_pmd(pgprot);
-	return __pmd;
-}
-
 #define pfn_pmd(pfn, pgprot)	mk_pmd_phys(__pa((pfn) << PAGE_SHIFT), (pgprot))
 #define mk_pmd(page, pgprot)	pfn_pmd(page_to_pfn(page), (pgprot))
 
@@ -1531,7 +1523,8 @@ extern int s390_enable_sie(void);
 /*
  * No page table caches to initialise
  */
-#define pgtable_cache_init()	do { } while (0)
+static inline void pgtable_cache_init(void) { }
+static inline void check_pgt_cache(void) { }
 
 #include <asm-generic/pgtable.h>
 
diff --git a/arch/s390/include/asm/processor.h b/arch/s390/include/asm/processor.h
index 94e749c9023..6b499870662 100644
--- a/arch/s390/include/asm/processor.h
+++ b/arch/s390/include/asm/processor.h
@@ -161,7 +161,8 @@ extern unsigned long thread_saved_pc(struct task_struct *t);
 
 extern void show_code(struct pt_regs *regs);
 extern void print_fn_code(unsigned char *code, unsigned long len);
-extern int insn_to_mnemonic(unsigned char *instruction, char buf[8]);
+extern int insn_to_mnemonic(unsigned char *instruction, char *buf,
+			    unsigned int len);
 
 unsigned long get_wchan(struct task_struct *p);
 #define task_pt_regs(tsk) ((struct pt_regs *) \
diff --git a/arch/s390/include/asm/ptrace.h b/arch/s390/include/asm/ptrace.h
index 3ee5da3bc10..559512a455d 100644
--- a/arch/s390/include/asm/ptrace.h
+++ b/arch/s390/include/asm/ptrace.h
@@ -9,9 +9,7 @@
 #include <uapi/asm/ptrace.h>
 
 #ifndef __ASSEMBLY__
-#ifndef __s390x__
-#else /* __s390x__ */
-#endif /* __s390x__ */
+
 extern long psw_kernel_bits;
 extern long psw_user_bits;
 
@@ -77,8 +75,6 @@ struct per_struct_kernel {
 #define PER_CONTROL_SUSPENSION		0x00400000UL
 #define PER_CONTROL_ALTERATION		0x00200000UL
 
-#ifdef __s390x__
-#endif /* __s390x__ */
 /*
  * These are defined as per linux/ptrace.h, which see.
  */
diff --git a/arch/s390/include/asm/syscall.h b/arch/s390/include/asm/syscall.h
index fe7b99759e1..cd29d2f4e4f 100644
--- a/arch/s390/include/asm/syscall.h
+++ b/arch/s390/include/asm/syscall.h
@@ -23,6 +23,7 @@
  * type here is what we want [need] for both 32 bit and 64 bit systems.
  */
 extern const unsigned int sys_call_table[];
+extern const unsigned int sys_call_table_emu[];
 
 static inline long syscall_get_nr(struct task_struct *task,
 				  struct pt_regs *regs)
diff --git a/arch/s390/include/asm/thread_info.h b/arch/s390/include/asm/thread_info.h
index 9e2cfe0349c..eb5f64d26d0 100644
--- a/arch/s390/include/asm/thread_info.h
+++ b/arch/s390/include/asm/thread_info.h
@@ -14,13 +14,8 @@
 #define THREAD_ORDER 1
 #define ASYNC_ORDER  1
 #else /* CONFIG_64BIT */
-#ifndef __SMALL_STACK
 #define THREAD_ORDER 2
 #define ASYNC_ORDER  2
-#else
-#define THREAD_ORDER 1
-#define ASYNC_ORDER  1
-#endif
 #endif /* CONFIG_64BIT */
 
 #define THREAD_SIZE (PAGE_SIZE << THREAD_ORDER)
@@ -41,6 +36,7 @@ struct thread_info {
 	struct task_struct	*task;		/* main task structure */
 	struct exec_domain	*exec_domain;	/* execution domain */
 	unsigned long		flags;		/* low level flags */
+	unsigned long		sys_call_table;	/* System call table address */
 	unsigned int		cpu;		/* current CPU */
 	int			preempt_count;	/* 0 => preemptable, <0 => BUG */
 	struct restart_block	restart_block;
diff --git a/arch/s390/include/uapi/asm/ptrace.h b/arch/s390/include/uapi/asm/ptrace.h
index a5ca214b34f..3aa9f1ec5b2 100644
--- a/arch/s390/include/uapi/asm/ptrace.h
+++ b/arch/s390/include/uapi/asm/ptrace.h
@@ -215,12 +215,6 @@ typedef struct
         unsigned long addr;
 } __attribute__ ((aligned(8))) psw_t;
 
-typedef struct
-{
-	__u32	mask;
-	__u32	addr;
-} __attribute__ ((aligned(8))) psw_compat_t;
-
 #ifndef __s390x__
 
 #define PSW_MASK_PER		0x40000000UL
@@ -295,20 +289,6 @@ typedef struct
 	unsigned long orig_gpr2;
 } s390_regs;
 
-typedef struct
-{
-	psw_compat_t	psw;
-	__u32		gprs[NUM_GPRS];
-	__u32		acrs[NUM_ACRS];
-	__u32		orig_gpr2;
-} s390_compat_regs;
-
-typedef struct
-{
-	__u32		gprs_high[NUM_GPRS];
-} s390_compat_regs_high;
-
-
 /*
  * Now for the user space program event recording (trace) definitions.
  * The following structures are used only for the ptrace interface, don't
diff --git a/arch/s390/include/uapi/asm/statfs.h b/arch/s390/include/uapi/asm/statfs.h
index 5acca0a34c2..a61d538756f 100644
--- a/arch/s390/include/uapi/asm/statfs.h
+++ b/arch/s390/include/uapi/asm/statfs.h
@@ -7,9 +7,6 @@
 #ifndef _S390_STATFS_H
 #define _S390_STATFS_H
 
-#ifndef __s390x__
-#include <asm-generic/statfs.h>
-#else
 /*
  * We can't use <asm-generic/statfs.h> because in 64-bit mode
  * we mix ints of different sizes in our struct statfs.
@@ -21,49 +18,33 @@ typedef __kernel_fsid_t	fsid_t;
 #endif
 
 struct statfs {
-	int  f_type;
-	int  f_bsize;
-	long f_blocks;
-	long f_bfree;
-	long f_bavail;
-	long f_files;
-	long f_ffree;
+	unsigned int	f_type;
+	unsigned int	f_bsize;
+	unsigned long	f_blocks;
+	unsigned long	f_bfree;
+	unsigned long	f_bavail;
+	unsigned long	f_files;
+	unsigned long	f_ffree;
 	__kernel_fsid_t f_fsid;
-	int  f_namelen;
-	int  f_frsize;
-	int  f_flags;
-	int  f_spare[4];
+	unsigned int	f_namelen;
+	unsigned int	f_frsize;
+	unsigned int	f_flags;
+	unsigned int	f_spare[4];
 };
 
 struct statfs64 {
-	int  f_type;
-	int  f_bsize;
-	long f_blocks;
-	long f_bfree;
-	long f_bavail;
-	long f_files;
-	long f_ffree;
+	unsigned int	f_type;
+	unsigned int	f_bsize;
+	unsigned long	f_blocks;
+	unsigned long	f_bfree;
+	unsigned long	f_bavail;
+	unsigned long	f_files;
+	unsigned long	f_ffree;
 	__kernel_fsid_t f_fsid;
-	int  f_namelen;
-	int  f_frsize;
-	int  f_flags;
-	int  f_spare[4];
+	unsigned int	f_namelen;
+	unsigned int	f_frsize;
+	unsigned int	f_flags;
+	unsigned int	f_spare[4];
 };
 
-struct compat_statfs64 {
-	__u32 f_type;
-	__u32 f_bsize;
-	__u64 f_blocks;
-	__u64 f_bfree;
-	__u64 f_bavail;
-	__u64 f_files;
-	__u64 f_ffree;
-	__kernel_fsid_t f_fsid;
-	__u32 f_namelen;
-	__u32 f_frsize;
-	__u32 f_flags;
-	__u32 f_spare[4];
-};
-
-#endif /* __s390x__ */
 #endif
diff --git a/arch/s390/kernel/Makefile b/arch/s390/kernel/Makefile
index 2ac311ef5c9..1386fcaf4ef 100644
--- a/arch/s390/kernel/Makefile
+++ b/arch/s390/kernel/Makefile
@@ -14,16 +14,25 @@ endif
 CFLAGS_smp.o	:= -Wno-nonnull
 
 #
+# Disable tailcall optimizations for stack / callchain walking functions
+# since this might generate broken code when accessing register 15 and
+# passing its content to other functions.
+#
+CFLAGS_stacktrace.o	+= -fno-optimize-sibling-calls
+CFLAGS_dumpstack.o	+= -fno-optimize-sibling-calls
+
+#
 # Pass UTS_MACHINE for user_regset definition
 #
 CFLAGS_ptrace.o		+= -DUTS_MACHINE='"$(UTS_MACHINE)"'
 
 CFLAGS_sysinfo.o += -Iinclude/math-emu -Iarch/s390/math-emu -w
 
-obj-y	:=  bitmap.o traps.o time.o process.o base.o early.o setup.o vtime.o \
-	    processor.o sys_s390.o ptrace.o signal.o cpcmd.o ebcdic.o nmi.o \
-	    debug.o irq.o ipl.o dis.o diag.o mem_detect.o sclp.o vdso.o \
-	    sysinfo.o jump_label.o lgr.o os_info.o machine_kexec.o pgm_check.o
+obj-y	:= bitmap.o traps.o time.o process.o base.o early.o setup.o vtime.o
+obj-y	+= processor.o sys_s390.o ptrace.o signal.o cpcmd.o ebcdic.o nmi.o
+obj-y	+= debug.o irq.o ipl.o dis.o diag.o mem_detect.o sclp.o vdso.o
+obj-y	+= sysinfo.o jump_label.o lgr.o os_info.o machine_kexec.o pgm_check.o
+obj-y	+= dumpstack.o
 
 obj-y	+= $(if $(CONFIG_64BIT),entry64.o,entry.o)
 obj-y	+= $(if $(CONFIG_64BIT),reipl64.o,reipl.o)
diff --git a/arch/s390/kernel/asm-offsets.c b/arch/s390/kernel/asm-offsets.c
index fface87056e..7a82f9f7010 100644
--- a/arch/s390/kernel/asm-offsets.c
+++ b/arch/s390/kernel/asm-offsets.c
@@ -35,6 +35,7 @@ int main(void)
 	DEFINE(__TI_task, offsetof(struct thread_info, task));
 	DEFINE(__TI_domain, offsetof(struct thread_info, exec_domain));
 	DEFINE(__TI_flags, offsetof(struct thread_info, flags));
+	DEFINE(__TI_sysc_table, offsetof(struct thread_info, sys_call_table));
 	DEFINE(__TI_cpu, offsetof(struct thread_info, cpu));
 	DEFINE(__TI_precount, offsetof(struct thread_info, preempt_count));
 	DEFINE(__TI_user_timer, offsetof(struct thread_info, user_timer));
diff --git a/arch/s390/kernel/compat_signal.c b/arch/s390/kernel/compat_signal.c
index 6de049fbe62..c439ac9ced0 100644
--- a/arch/s390/kernel/compat_signal.c
+++ b/arch/s390/kernel/compat_signal.c
@@ -362,6 +362,7 @@ static int setup_frame32(int sig, struct k_sigaction *ka,
 		/* set extra registers only for synchronous signals */
 		regs->gprs[4] = regs->int_code & 127;
 		regs->gprs[5] = regs->int_parm_long;
+		regs->gprs[6] = task_thread_info(current)->last_break;
 	}
 
 	/* Place signal number on stack to allow backtrace from handler.  */
@@ -421,6 +422,7 @@ static int setup_rt_frame32(int sig, struct k_sigaction *ka, siginfo_t *info,
 	regs->gprs[2] = map_signal(sig);
 	regs->gprs[3] = (__force __u64) &frame->info;
 	regs->gprs[4] = (__force __u64) &frame->uc;
+	regs->gprs[5] = task_thread_info(current)->last_break;
 	return 0;
 
 give_sigsegv:
diff --git a/arch/s390/kernel/dis.c b/arch/s390/kernel/dis.c
index 3ad5e954016..7f4a4a8c847 100644
--- a/arch/s390/kernel/dis.c
+++ b/arch/s390/kernel/dis.c
@@ -1696,14 +1696,15 @@ static struct insn *find_insn(unsigned char *code)
  * insn_to_mnemonic - decode an s390 instruction
  * @instruction: instruction to decode
  * @buf: buffer to fill with mnemonic
+ * @len: length of buffer
  *
  * Decode the instruction at @instruction and store the corresponding
- * mnemonic into @buf.
+ * mnemonic into @buf of length @len.
  * @buf is left unchanged if the instruction could not be decoded.
  * Returns:
  *  %0 on success, %-ENOENT if the instruction was not found.
  */
-int insn_to_mnemonic(unsigned char *instruction, char buf[8])
+int insn_to_mnemonic(unsigned char *instruction, char *buf, unsigned int len)
 {
 	struct insn *insn;
 
@@ -1711,10 +1712,10 @@ int insn_to_mnemonic(unsigned char *instruction, char buf[8])
 	if (!insn)
 		return -ENOENT;
 	if (insn->name[0] == '\0')
-		snprintf(buf, 8, "%s",
+		snprintf(buf, len, "%s",
 			 long_insn_name[(int) insn->name[1]]);
 	else
-		snprintf(buf, 8, "%.5s", insn->name);
+		snprintf(buf, len, "%.5s", insn->name);
 	return 0;
 }
 EXPORT_SYMBOL_GPL(insn_to_mnemonic);
diff --git a/arch/s390/kernel/dumpstack.c b/arch/s390/kernel/dumpstack.c
new file mode 100644
index 00000000000..03dce39d01e
--- /dev/null
+++ b/arch/s390/kernel/dumpstack.c
@@ -0,0 +1,236 @@
+/*
+ * Stack dumping functions
+ *
+ *  Copyright IBM Corp. 1999, 2013
+ */
+
+#include <linux/kallsyms.h>
+#include <linux/hardirq.h>
+#include <linux/kprobes.h>
+#include <linux/utsname.h>
+#include <linux/export.h>
+#include <linux/kdebug.h>
+#include <linux/ptrace.h>
+#include <linux/module.h>
+#include <linux/sched.h>
+#include <asm/processor.h>
+#include <asm/debug.h>
+#include <asm/ipl.h>
+
+#ifndef CONFIG_64BIT
+#define LONG "%08lx "
+#define FOURLONG "%08lx %08lx %08lx %08lx\n"
+static int kstack_depth_to_print = 12;
+#else /* CONFIG_64BIT */
+#define LONG "%016lx "
+#define FOURLONG "%016lx %016lx %016lx %016lx\n"
+static int kstack_depth_to_print = 20;
+#endif /* CONFIG_64BIT */
+
+/*
+ * For show_trace we have tree different stack to consider:
+ *   - the panic stack which is used if the kernel stack has overflown
+ *   - the asynchronous interrupt stack (cpu related)
+ *   - the synchronous kernel stack (process related)
+ * The stack trace can start at any of the three stack and can potentially
+ * touch all of them. The order is: panic stack, async stack, sync stack.
+ */
+static unsigned long
+__show_trace(unsigned long sp, unsigned long low, unsigned long high)
+{
+	struct stack_frame *sf;
+	struct pt_regs *regs;
+
+	while (1) {
+		sp = sp & PSW_ADDR_INSN;
+		if (sp < low || sp > high - sizeof(*sf))
+			return sp;
+		sf = (struct stack_frame *) sp;
+		printk("([<%016lx>] ", sf->gprs[8] & PSW_ADDR_INSN);
+		print_symbol("%s)\n", sf->gprs[8] & PSW_ADDR_INSN);
+		/* Follow the backchain. */
+		while (1) {
+			low = sp;
+			sp = sf->back_chain & PSW_ADDR_INSN;
+			if (!sp)
+				break;
+			if (sp <= low || sp > high - sizeof(*sf))
+				return sp;
+			sf = (struct stack_frame *) sp;
+			printk(" [<%016lx>] ", sf->gprs[8] & PSW_ADDR_INSN);
+			print_symbol("%s\n", sf->gprs[8] & PSW_ADDR_INSN);
+		}
+		/* Zero backchain detected, check for interrupt frame. */
+		sp = (unsigned long) (sf + 1);
+		if (sp <= low || sp > high - sizeof(*regs))
+			return sp;
+		regs = (struct pt_regs *) sp;
+		printk(" [<%016lx>] ", regs->psw.addr & PSW_ADDR_INSN);
+		print_symbol("%s\n", regs->psw.addr & PSW_ADDR_INSN);
+		low = sp;
+		sp = regs->gprs[15];
+	}
+}
+
+static void show_trace(struct task_struct *task, unsigned long *stack)
+{
+	register unsigned long __r15 asm ("15");
+	unsigned long sp;
+
+	sp = (unsigned long) stack;
+	if (!sp)
+		sp = task ? task->thread.ksp : __r15;
+	printk("Call Trace:\n");
+#ifdef CONFIG_CHECK_STACK
+	sp = __show_trace(sp, S390_lowcore.panic_stack - 4096,
+			  S390_lowcore.panic_stack);
+#endif
+	sp = __show_trace(sp, S390_lowcore.async_stack - ASYNC_SIZE,
+			  S390_lowcore.async_stack);
+	if (task)
+		__show_trace(sp, (unsigned long) task_stack_page(task),
+			     (unsigned long) task_stack_page(task) + THREAD_SIZE);
+	else
+		__show_trace(sp, S390_lowcore.thread_info,
+			     S390_lowcore.thread_info + THREAD_SIZE);
+	if (!task)
+		task = current;
+	debug_show_held_locks(task);
+}
+
+void show_stack(struct task_struct *task, unsigned long *sp)
+{
+	register unsigned long *__r15 asm ("15");
+	unsigned long *stack;
+	int i;
+
+	if (!sp)
+		stack = task ? (unsigned long *) task->thread.ksp : __r15;
+	else
+		stack = sp;
+
+	for (i = 0; i < kstack_depth_to_print; i++) {
+		if (((addr_t) stack & (THREAD_SIZE-1)) == 0)
+			break;
+		if ((i * sizeof(long) % 32) == 0)
+			printk("%s       ", i == 0 ? "" : "\n");
+		printk(LONG, *stack++);
+	}
+	printk("\n");
+	show_trace(task, sp);
+}
+
+static void show_last_breaking_event(struct pt_regs *regs)
+{
+#ifdef CONFIG_64BIT
+	printk("Last Breaking-Event-Address:\n");
+	printk(" [<%016lx>] ", regs->args[0] & PSW_ADDR_INSN);
+	print_symbol("%s\n", regs->args[0] & PSW_ADDR_INSN);
+#endif
+}
+
+/*
+ * The architecture-independent dump_stack generator
+ */
+void dump_stack(void)
+{
+	printk("CPU: %d %s %s %.*s\n",
+	       task_thread_info(current)->cpu, print_tainted(),
+	       init_utsname()->release,
+	       (int)strcspn(init_utsname()->version, " "),
+	       init_utsname()->version);
+	printk("Process %s (pid: %d, task: %p, ksp: %p)\n",
+	       current->comm, current->pid, current,
+	       (void *) current->thread.ksp);
+	show_stack(NULL, NULL);
+}
+EXPORT_SYMBOL(dump_stack);
+
+static inline int mask_bits(struct pt_regs *regs, unsigned long bits)
+{
+	return (regs->psw.mask & bits) / ((~bits + 1) & bits);
+}
+
+void show_registers(struct pt_regs *regs)
+{
+	char *mode;
+
+	mode = user_mode(regs) ? "User" : "Krnl";
+	printk("%s PSW : %p %p",
+	       mode, (void *) regs->psw.mask,
+	       (void *) regs->psw.addr);
+	print_symbol(" (%s)\n", regs->psw.addr & PSW_ADDR_INSN);
+	printk("           R:%x T:%x IO:%x EX:%x Key:%x M:%x W:%x "
+	       "P:%x AS:%x CC:%x PM:%x", mask_bits(regs, PSW_MASK_PER),
+	       mask_bits(regs, PSW_MASK_DAT), mask_bits(regs, PSW_MASK_IO),
+	       mask_bits(regs, PSW_MASK_EXT), mask_bits(regs, PSW_MASK_KEY),
+	       mask_bits(regs, PSW_MASK_MCHECK), mask_bits(regs, PSW_MASK_WAIT),
+	       mask_bits(regs, PSW_MASK_PSTATE), mask_bits(regs, PSW_MASK_ASC),
+	       mask_bits(regs, PSW_MASK_CC), mask_bits(regs, PSW_MASK_PM));
+#ifdef CONFIG_64BIT
+	printk(" EA:%x", mask_bits(regs, PSW_MASK_EA | PSW_MASK_BA));
+#endif
+	printk("\n%s GPRS: " FOURLONG, mode,
+	       regs->gprs[0], regs->gprs[1], regs->gprs[2], regs->gprs[3]);
+	printk("           " FOURLONG,
+	       regs->gprs[4], regs->gprs[5], regs->gprs[6], regs->gprs[7]);
+	printk("           " FOURLONG,
+	       regs->gprs[8], regs->gprs[9], regs->gprs[10], regs->gprs[11]);
+	printk("           " FOURLONG,
+	       regs->gprs[12], regs->gprs[13], regs->gprs[14], regs->gprs[15]);
+	show_code(regs);
+}
+
+void show_regs(struct pt_regs *regs)
+{
+	printk("CPU: %d %s %s %.*s\n",
+	       task_thread_info(current)->cpu, print_tainted(),
+	       init_utsname()->release,
+	       (int)strcspn(init_utsname()->version, " "),
+	       init_utsname()->version);
+	printk("Process %s (pid: %d, task: %p, ksp: %p)\n",
+	       current->comm, current->pid, current,
+	       (void *) current->thread.ksp);
+	show_registers(regs);
+	/* Show stack backtrace if pt_regs is from kernel mode */
+	if (!user_mode(regs))
+		show_trace(NULL, (unsigned long *) regs->gprs[15]);
+	show_last_breaking_event(regs);
+}
+
+static DEFINE_SPINLOCK(die_lock);
+
+void die(struct pt_regs *regs, const char *str)
+{
+	static int die_counter;
+
+	oops_enter();
+	lgr_info_log();
+	debug_stop_all();
+	console_verbose();
+	spin_lock_irq(&die_lock);
+	bust_spinlocks(1);
+	printk("%s: %04x [#%d] ", str, regs->int_code & 0xffff, ++die_counter);
+#ifdef CONFIG_PREEMPT
+	printk("PREEMPT ");
+#endif
+#ifdef CONFIG_SMP
+	printk("SMP ");
+#endif
+#ifdef CONFIG_DEBUG_PAGEALLOC
+	printk("DEBUG_PAGEALLOC");
+#endif
+	printk("\n");
+	notify_die(DIE_OOPS, str, regs, 0, regs->int_code & 0xffff, SIGSEGV);
+	print_modules();
+	show_regs(regs);
+	bust_spinlocks(0);
+	add_taint(TAINT_DIE, LOCKDEP_NOW_UNRELIABLE);
+	spin_unlock_irq(&die_lock);
+	if (in_interrupt())
+		panic("Fatal exception in interrupt");
+	if (panic_on_oops)
+		panic("Fatal exception: panic_on_oops");
+	oops_exit();
+	do_exit(SIGSEGV);
+}
diff --git a/arch/s390/kernel/entry.S b/arch/s390/kernel/entry.S
index 94feff7d613..4d5e6f8a797 100644
--- a/arch/s390/kernel/entry.S
+++ b/arch/s390/kernel/entry.S
@@ -45,6 +45,7 @@ _TIF_TRACE    = (_TIF_SYSCALL_TRACE | _TIF_SYSCALL_AUDIT | _TIF_SECCOMP | \
 
 STACK_SHIFT = PAGE_SHIFT + THREAD_ORDER
 STACK_SIZE  = 1 << STACK_SHIFT
+STACK_INIT  = STACK_SIZE - STACK_FRAME_OVERHEAD - __PT_SIZE
 
 #define BASED(name) name-system_call(%r13)
 
@@ -97,10 +98,10 @@ STACK_SIZE  = 1 << STACK_SHIFT
 	sra	%r14,\shift
 	jnz	1f
 	CHECK_STACK 1<<\shift,\savearea
+	ahi	%r15,-(STACK_FRAME_OVERHEAD + __PT_SIZE)
 	j	2f
 1:	l	%r15,\stack		# load target stack
-2:	ahi	%r15,-(STACK_FRAME_OVERHEAD + __PT_SIZE)
-	la	%r11,STACK_FRAME_OVERHEAD(%r15)
+2:	la	%r11,STACK_FRAME_OVERHEAD(%r15)
 	.endm
 
 	.macro	ADD64 high,low,timer
@@ -150,7 +151,7 @@ ENTRY(__switch_to)
 	l	%r4,__THREAD_info(%r2)		# get thread_info of prev
 	l	%r5,__THREAD_info(%r3)		# get thread_info of next
 	lr	%r15,%r5
-	ahi	%r15,STACK_SIZE			# end of kernel stack of next
+	ahi	%r15,STACK_INIT			# end of kernel stack of next
 	st	%r3,__LC_CURRENT		# store task struct of next
 	st	%r5,__LC_THREAD_INFO		# store thread info of next
 	st	%r15,__LC_KERNEL_STACK		# store end of kernel stack
@@ -178,7 +179,6 @@ sysc_stm:
 	l	%r13,__LC_SVC_NEW_PSW+4
 sysc_per:
 	l	%r15,__LC_KERNEL_STACK
-	ahi	%r15,-(STACK_FRAME_OVERHEAD + __PT_SIZE)
 	la	%r11,STACK_FRAME_OVERHEAD(%r15)	# pointer to pt_regs
 sysc_vtime:
 	UPDATE_VTIME %r8,%r9,__LC_SYNC_ENTER_TIMER
@@ -188,6 +188,7 @@ sysc_vtime:
 	mvc	__PT_INT_CODE(4,%r11),__LC_SVC_ILC
 sysc_do_svc:
 	oi	__TI_flags+3(%r12),_TIF_SYSCALL
+	l	%r10,__TI_sysc_table(%r12)	# 31 bit system call table
 	lh	%r8,__PT_INT_CODE+2(%r11)
 	sla	%r8,2				# shift and test for svc0
 	jnz	sysc_nr_ok
@@ -198,7 +199,6 @@ sysc_do_svc:
 	lr	%r8,%r1
 	sla	%r8,2
 sysc_nr_ok:
-	l	%r10,BASED(.Lsys_call_table)	# 31 bit system call table
 	xc	__SF_BACKCHAIN(4,%r15),__SF_BACKCHAIN(%r15)
 	st	%r2,__PT_ORIG_GPR2(%r11)
 	st	%r7,STACK_FRAME_OVERHEAD(%r15)
@@ -359,11 +359,11 @@ ENTRY(pgm_check_handler)
 	tm	__LC_PGM_ILC+3,0x80	# check for per exception
 	jnz	pgm_svcper		# -> single stepped svc
 0:	CHECK_STACK STACK_SIZE,__LC_SAVE_AREA_SYNC
+	ahi	%r15,-(STACK_FRAME_OVERHEAD + __PT_SIZE)
 	j	2f
 1:	UPDATE_VTIME %r14,%r15,__LC_SYNC_ENTER_TIMER
 	l	%r15,__LC_KERNEL_STACK
-2:	ahi	%r15,-(STACK_FRAME_OVERHEAD + __PT_SIZE)
-	la	%r11,STACK_FRAME_OVERHEAD(%r15)
+2:	la	%r11,STACK_FRAME_OVERHEAD(%r15)
 	stm	%r0,%r7,__PT_R0(%r11)
 	mvc	__PT_R8(32,%r11),__LC_SAVE_AREA_SYNC
 	stm	%r8,%r9,__PT_PSW(%r11)
@@ -485,7 +485,6 @@ io_work:
 #
 io_work_user:
 	l	%r1,__LC_KERNEL_STACK
-	ahi	%r1,-(STACK_FRAME_OVERHEAD + __PT_SIZE)
 	mvc	STACK_FRAME_OVERHEAD(__PT_SIZE,%r1),0(%r11)
 	xc	__SF_BACKCHAIN(4,%r1),__SF_BACKCHAIN(%r1)
 	la	%r11,STACK_FRAME_OVERHEAD(%r1)
@@ -646,7 +645,6 @@ mcck_skip:
 	tm	__PT_PSW+1(%r11),0x01	# returning to user ?
 	jno	mcck_return
 	l	%r1,__LC_KERNEL_STACK	# switch to kernel stack
-	ahi	%r1,-(STACK_FRAME_OVERHEAD + __PT_SIZE)
 	mvc	STACK_FRAME_OVERHEAD(__PT_SIZE,%r1),0(%r11)
 	xc	__SF_BACKCHAIN(4,%r1),__SF_BACKCHAIN(%r1)
 	la	%r11,STACK_FRAME_OVERHEAD(%r15)
@@ -674,6 +672,7 @@ mcck_panic:
 	sra	%r14,PAGE_SHIFT
 	jz	0f
 	l	%r15,__LC_PANIC_STACK
+	j	mcck_skip
 0:	ahi	%r15,-(STACK_FRAME_OVERHEAD + __PT_SIZE)
 	j	mcck_skip
 
@@ -714,12 +713,10 @@ ENTRY(restart_int_handler)
  */
 stack_overflow:
 	l	%r15,__LC_PANIC_STACK	# change to panic stack
-	ahi	%r15,-__PT_SIZE		# create pt_regs
-	stm	%r0,%r7,__PT_R0(%r15)
-	stm	%r8,%r9,__PT_PSW(%r15)
+	la	%r11,STACK_FRAME_OVERHEAD(%r15)
+	stm	%r0,%r7,__PT_R0(%r11)
+	stm	%r8,%r9,__PT_PSW(%r11)
 	mvc	__PT_R8(32,%r11),0(%r14)
-	lr	%r15,%r11
-	ahi	%r15,-STACK_FRAME_OVERHEAD
 	l	%r1,BASED(1f)
 	xc	__SF_BACKCHAIN(4,%r15),__SF_BACKCHAIN(%r15)
 	lr	%r2,%r11		# pass pointer to pt_regs
@@ -799,15 +796,14 @@ cleanup_system_call:
 	mvc	__LC_LAST_UPDATE_TIMER(8),__LC_SYNC_ENTER_TIMER
 	# set up saved register 11
 	l	%r15,__LC_KERNEL_STACK
-	ahi	%r15,-__PT_SIZE
-	st	%r15,12(%r11)		# r11 pt_regs pointer
+	la	%r9,STACK_FRAME_OVERHEAD(%r15)
+	st	%r9,12(%r11)		# r11 pt_regs pointer
 	# fill pt_regs
-	mvc	__PT_R8(32,%r15),__LC_SAVE_AREA_SYNC
-	stm	%r0,%r7,__PT_R0(%r15)
-	mvc	__PT_PSW(8,%r15),__LC_SVC_OLD_PSW
-	mvc	__PT_INT_CODE(4,%r15),__LC_SVC_ILC
+	mvc	__PT_R8(32,%r9),__LC_SAVE_AREA_SYNC
+	stm	%r0,%r7,__PT_R0(%r9)
+	mvc	__PT_PSW(8,%r9),__LC_SVC_OLD_PSW
+	mvc	__PT_INT_CODE(4,%r9),__LC_SVC_ILC
 	# setup saved register 15
-	ahi	%r15,-STACK_FRAME_OVERHEAD
 	st	%r15,28(%r11)		# r15 stack pointer
 	# set new psw address and exit
 	l	%r9,BASED(cleanup_table+4)	# sysc_do_svc + 0x80000000
@@ -910,7 +906,6 @@ cleanup_idle_wait:
 .Ltrace_enter:		.long	do_syscall_trace_enter
 .Ltrace_exit:		.long	do_syscall_trace_exit
 .Lschedule_tail:	.long	schedule_tail
-.Lsys_call_table:	.long	sys_call_table
 .Lsysc_per:		.long	sysc_per + 0x80000000
 #ifdef CONFIG_TRACE_IRQFLAGS
 .Lhardirqs_on:		.long	trace_hardirqs_on_caller
diff --git a/arch/s390/kernel/entry.h b/arch/s390/kernel/entry.h
index c3a736a3ed4..aa0ab02e959 100644
--- a/arch/s390/kernel/entry.h
+++ b/arch/s390/kernel/entry.h
@@ -7,6 +7,7 @@
 #include <asm/cputime.h>
 
 extern void *restart_stack;
+extern unsigned long suspend_zero_pages;
 
 void system_call(void);
 void pgm_check_handler(void);
diff --git a/arch/s390/kernel/entry64.S b/arch/s390/kernel/entry64.S
index 2e6d60c55f9..4c17eece707 100644
--- a/arch/s390/kernel/entry64.S
+++ b/arch/s390/kernel/entry64.S
@@ -39,6 +39,7 @@ __PT_R15     =	__PT_GPRS + 120
 
 STACK_SHIFT = PAGE_SHIFT + THREAD_ORDER
 STACK_SIZE  = 1 << STACK_SHIFT
+STACK_INIT = STACK_SIZE - STACK_FRAME_OVERHEAD - __PT_SIZE
 
 _TIF_WORK_SVC = (_TIF_SIGPENDING | _TIF_NOTIFY_RESUME | _TIF_NEED_RESCHED | \
 		 _TIF_MCCK_PENDING | _TIF_PER_TRAP )
@@ -124,10 +125,10 @@ _TIF_EXIT_SIE = (_TIF_SIGPENDING | _TIF_NEED_RESCHED | _TIF_MCCK_PENDING)
 	srag	%r14,%r14,\shift
 	jnz	1f
 	CHECK_STACK 1<<\shift,\savearea
+	aghi	%r15,-(STACK_FRAME_OVERHEAD + __PT_SIZE)
 	j	2f
 1:	lg	%r15,\stack		# load target stack
-2:	aghi	%r15,-(STACK_FRAME_OVERHEAD + __PT_SIZE)
-	la	%r11,STACK_FRAME_OVERHEAD(%r15)
+2:	la	%r11,STACK_FRAME_OVERHEAD(%r15)
 	.endm
 
 	.macro UPDATE_VTIME scratch,enter_timer
@@ -177,7 +178,7 @@ ENTRY(__switch_to)
 	lg	%r4,__THREAD_info(%r2)		# get thread_info of prev
 	lg	%r5,__THREAD_info(%r3)		# get thread_info of next
 	lgr	%r15,%r5
-	aghi	%r15,STACK_SIZE			# end of kernel stack of next
+	aghi	%r15,STACK_INIT			# end of kernel stack of next
 	stg	%r3,__LC_CURRENT		# store task struct of next
 	stg	%r5,__LC_THREAD_INFO		# store thread info of next
 	stg	%r15,__LC_KERNEL_STACK		# store end of kernel stack
@@ -203,10 +204,8 @@ sysc_stmg:
 	stmg	%r8,%r15,__LC_SAVE_AREA_SYNC
 	lg	%r10,__LC_LAST_BREAK
 	lg	%r12,__LC_THREAD_INFO
-	larl	%r13,system_call
 sysc_per:
 	lg	%r15,__LC_KERNEL_STACK
-	aghi	%r15,-(STACK_FRAME_OVERHEAD + __PT_SIZE)
 	la	%r11,STACK_FRAME_OVERHEAD(%r15)	# pointer to pt_regs
 sysc_vtime:
 	UPDATE_VTIME %r13,__LC_SYNC_ENTER_TIMER
@@ -217,6 +216,7 @@ sysc_vtime:
 	mvc	__PT_INT_CODE(4,%r11),__LC_SVC_ILC
 sysc_do_svc:
 	oi	__TI_flags+7(%r12),_TIF_SYSCALL
+	lg	%r10,__TI_sysc_table(%r12)	# address of system call table
 	llgh	%r8,__PT_INT_CODE+2(%r11)
 	slag	%r8,%r8,2			# shift and test for svc 0
 	jnz	sysc_nr_ok
@@ -227,13 +227,6 @@ sysc_do_svc:
 	sth	%r1,__PT_INT_CODE+2(%r11)
 	slag	%r8,%r1,2
 sysc_nr_ok:
-	larl	%r10,sys_call_table		# 64 bit system call table
-#ifdef CONFIG_COMPAT
-	tm	__TI_flags+5(%r12),(_TIF_31BIT>>16)
-	jno	sysc_noemu
-	larl	%r10,sys_call_table_emu		# 31 bit system call table
-sysc_noemu:
-#endif
 	xc	__SF_BACKCHAIN(8,%r15),__SF_BACKCHAIN(%r15)
 	stg	%r2,__PT_ORIG_GPR2(%r11)
 	stg	%r7,STACK_FRAME_OVERHEAD(%r15)
@@ -389,6 +382,7 @@ ENTRY(pgm_check_handler)
 	tm	__LC_PGM_ILC+3,0x80	# check for per exception
 	jnz	pgm_svcper		# -> single stepped svc
 0:	CHECK_STACK STACK_SIZE,__LC_SAVE_AREA_SYNC
+	aghi	%r15,-(STACK_FRAME_OVERHEAD + __PT_SIZE)
 	j	2f
 1:	UPDATE_VTIME %r14,__LC_SYNC_ENTER_TIMER
 	LAST_BREAK %r14
@@ -398,8 +392,7 @@ ENTRY(pgm_check_handler)
 	tm	__LC_PGM_ILC+2,0x02	# check for transaction abort
 	jz	2f
 	mvc	__THREAD_trap_tdb(256,%r14),0(%r13)
-2:	aghi	%r15,-(STACK_FRAME_OVERHEAD + __PT_SIZE)
-	la	%r11,STACK_FRAME_OVERHEAD(%r15)
+2:	la	%r11,STACK_FRAME_OVERHEAD(%r15)
 	stmg	%r0,%r7,__PT_R0(%r11)
 	mvc	__PT_R8(64,%r11),__LC_SAVE_AREA_SYNC
 	stmg	%r8,%r9,__PT_PSW(%r11)
@@ -526,7 +519,6 @@ io_work:
 #
 io_work_user:
 	lg	%r1,__LC_KERNEL_STACK
-	aghi	%r1,-(STACK_FRAME_OVERHEAD + __PT_SIZE)
 	mvc	STACK_FRAME_OVERHEAD(__PT_SIZE,%r1),0(%r11)
 	xc	__SF_BACKCHAIN(8,%r1),__SF_BACKCHAIN(%r1)
 	la	%r11,STACK_FRAME_OVERHEAD(%r1)
@@ -688,7 +680,6 @@ mcck_skip:
 	tm	__PT_PSW+1(%r11),0x01	# returning to user ?
 	jno	mcck_return
 	lg	%r1,__LC_KERNEL_STACK	# switch to kernel stack
-	aghi	%r1,-(STACK_FRAME_OVERHEAD + __PT_SIZE)
 	mvc	STACK_FRAME_OVERHEAD(__PT_SIZE,%r1),0(%r11)
 	xc	__SF_BACKCHAIN(8,%r1),__SF_BACKCHAIN(%r1)
 	la	%r11,STACK_FRAME_OVERHEAD(%r1)
@@ -755,14 +746,12 @@ ENTRY(restart_int_handler)
  * Setup a pt_regs so that show_trace can provide a good call trace.
  */
 stack_overflow:
-	lg	%r11,__LC_PANIC_STACK	# change to panic stack
-	aghi	%r11,-__PT_SIZE		# create pt_regs
+	lg	%r15,__LC_PANIC_STACK	# change to panic stack
+	la	%r11,STACK_FRAME_OVERHEAD(%r15)
 	stmg	%r0,%r7,__PT_R0(%r11)
 	stmg	%r8,%r9,__PT_PSW(%r11)
 	mvc	__PT_R8(64,%r11),0(%r14)
 	stg	%r10,__PT_ORIG_GPR2(%r11) # store last break to orig_gpr2
-	lgr	%r15,%r11
-	aghi	%r15,-STACK_FRAME_OVERHEAD
 	xc	__SF_BACKCHAIN(8,%r15),__SF_BACKCHAIN(%r15)
 	lgr	%r2,%r11		# pass pointer to pt_regs
 	jg	kernel_stack_overflow
@@ -846,15 +835,14 @@ cleanup_system_call:
 	mvc	__TI_last_break(8,%r12),16(%r11)
 0:	# set up saved register r11
 	lg	%r15,__LC_KERNEL_STACK
-	aghi	%r15,-__PT_SIZE
-	stg	%r15,24(%r11)		# r11 pt_regs pointer
+	la	%r9,STACK_FRAME_OVERHEAD(%r15)
+	stg	%r9,24(%r11)		# r11 pt_regs pointer
 	# fill pt_regs
-	mvc	__PT_R8(64,%r15),__LC_SAVE_AREA_SYNC
-	stmg	%r0,%r7,__PT_R0(%r15)
-	mvc	__PT_PSW(16,%r15),__LC_SVC_OLD_PSW
-	mvc	__PT_INT_CODE(4,%r15),__LC_SVC_ILC
+	mvc	__PT_R8(64,%r9),__LC_SAVE_AREA_SYNC
+	stmg	%r0,%r7,__PT_R0(%r9)
+	mvc	__PT_PSW(16,%r9),__LC_SVC_OLD_PSW
+	mvc	__PT_INT_CODE(4,%r9),__LC_SVC_ILC
 	# setup saved register r15
-	aghi	%r15,-STACK_FRAME_OVERHEAD
 	stg	%r15,56(%r11)		# r15 stack pointer
 	# set new psw address and exit
 	larl	%r9,sysc_do_svc
@@ -1011,6 +999,7 @@ sys_call_table:
 #ifdef CONFIG_COMPAT
 
 #define SYSCALL(esa,esame,emu)	.long emu
+	.globl	sys_call_table_emu
 sys_call_table_emu:
 #include "syscalls.S"
 #undef SYSCALL
diff --git a/arch/s390/kernel/irq.c b/arch/s390/kernel/irq.c
index 1630f439cd2..4f5ef62934a 100644
--- a/arch/s390/kernel/irq.c
+++ b/arch/s390/kernel/irq.c
@@ -33,7 +33,7 @@ struct irq_class {
 };
 
 /*
- * The list of "main" irq classes on s390. This is the list of interrrupts
+ * The list of "main" irq classes on s390. This is the list of interrupts
  * that appear both in /proc/stat ("intr" line) and /proc/interrupts.
  * Historically only external and I/O interrupts have been part of /proc/stat.
  * We can't add the split external and I/O sub classes since the first field
diff --git a/arch/s390/kernel/machine_kexec.c b/arch/s390/kernel/machine_kexec.c
index b3de2770001..ac2178161ec 100644
--- a/arch/s390/kernel/machine_kexec.c
+++ b/arch/s390/kernel/machine_kexec.c
@@ -13,6 +13,7 @@
 #include <linux/reboot.h>
 #include <linux/ftrace.h>
 #include <linux/debug_locks.h>
+#include <linux/suspend.h>
 #include <asm/cio.h>
 #include <asm/setup.h>
 #include <asm/pgtable.h>
@@ -67,6 +68,35 @@ void setup_regs(void)
 	memcpy((void *) SAVE_AREA_BASE, (void *) sa, sizeof(struct save_area));
 }
 
+/*
+ * PM notifier callback for kdump
+ */
+static int machine_kdump_pm_cb(struct notifier_block *nb, unsigned long action,
+			       void *ptr)
+{
+	switch (action) {
+	case PM_SUSPEND_PREPARE:
+	case PM_HIBERNATION_PREPARE:
+		if (crashk_res.start)
+			crash_map_reserved_pages();
+		break;
+	case PM_POST_SUSPEND:
+	case PM_POST_HIBERNATION:
+		if (crashk_res.start)
+			crash_unmap_reserved_pages();
+		break;
+	default:
+		return NOTIFY_DONE;
+	}
+	return NOTIFY_OK;
+}
+
+static int __init machine_kdump_pm_init(void)
+{
+	pm_notifier(machine_kdump_pm_cb, 0);
+	return 0;
+}
+arch_initcall(machine_kdump_pm_init);
 #endif
 
 /*
diff --git a/arch/s390/kernel/process.c b/arch/s390/kernel/process.c
index 536d64579d9..2bc3eddae34 100644
--- a/arch/s390/kernel/process.c
+++ b/arch/s390/kernel/process.c
@@ -61,18 +61,8 @@ unsigned long thread_saved_pc(struct task_struct *tsk)
 	return sf->gprs[8];
 }
 
-/*
- * The idle loop on a S390...
- */
-static void default_idle(void)
+void arch_cpu_idle(void)
 {
-	if (cpu_is_offline(smp_processor_id()))
-		cpu_die();
-	local_irq_disable();
-	if (need_resched()) {
-		local_irq_enable();
-		return;
-	}
 	local_mcck_disable();
 	if (test_thread_flag(TIF_MCCK_PENDING)) {
 		local_mcck_enable();
@@ -83,19 +73,15 @@ static void default_idle(void)
 	vtime_stop_cpu();
 }
 
-void cpu_idle(void)
+void arch_cpu_idle_exit(void)
 {
-	for (;;) {
-		tick_nohz_idle_enter();
-		rcu_idle_enter();
-		while (!need_resched() && !test_thread_flag(TIF_MCCK_PENDING))
-			default_idle();
-		rcu_idle_exit();
-		tick_nohz_idle_exit();
-		if (test_thread_flag(TIF_MCCK_PENDING))
-			s390_handle_mcck();
-		schedule_preempt_disabled();
-	}
+	if (test_thread_flag(TIF_MCCK_PENDING))
+		s390_handle_mcck();
+}
+
+void arch_cpu_idle_dead(void)
+{
+	cpu_die();
 }
 
 extern void __kprobes kernel_thread_starter(void);
diff --git a/arch/s390/kernel/setup.c b/arch/s390/kernel/setup.c
index 29268859d8e..0f419c5765c 100644
--- a/arch/s390/kernel/setup.c
+++ b/arch/s390/kernel/setup.c
@@ -377,11 +377,14 @@ static void __init setup_lowcore(void)
 		PSW_MASK_DAT | PSW_MASK_MCHECK;
 	lc->io_new_psw.addr = PSW_ADDR_AMODE | (unsigned long) io_int_handler;
 	lc->clock_comparator = -1ULL;
-	lc->kernel_stack = ((unsigned long) &init_thread_union) + THREAD_SIZE;
+	lc->kernel_stack = ((unsigned long) &init_thread_union)
+		+ THREAD_SIZE - STACK_FRAME_OVERHEAD - sizeof(struct pt_regs);
 	lc->async_stack = (unsigned long)
-		__alloc_bootmem(ASYNC_SIZE, ASYNC_SIZE, 0) + ASYNC_SIZE;
+		__alloc_bootmem(ASYNC_SIZE, ASYNC_SIZE, 0)
+		+ ASYNC_SIZE - STACK_FRAME_OVERHEAD - sizeof(struct pt_regs);
 	lc->panic_stack = (unsigned long)
-		__alloc_bootmem(PAGE_SIZE, PAGE_SIZE, 0) + PAGE_SIZE;
+		__alloc_bootmem(PAGE_SIZE, PAGE_SIZE, 0)
+		+ PAGE_SIZE - STACK_FRAME_OVERHEAD - sizeof(struct pt_regs);
 	lc->current_task = (unsigned long) init_thread_union.thread_info.task;
 	lc->thread_info = (unsigned long) &init_thread_union;
 	lc->machine_flags = S390_lowcore.machine_flags;
diff --git a/arch/s390/kernel/smp.c b/arch/s390/kernel/smp.c
index 549c9d173c0..8074cb4b7cb 100644
--- a/arch/s390/kernel/smp.c
+++ b/arch/s390/kernel/smp.c
@@ -181,8 +181,10 @@ static int __cpuinit pcpu_alloc_lowcore(struct pcpu *pcpu, int cpu)
 	lc = pcpu->lowcore;
 	memcpy(lc, &S390_lowcore, 512);
 	memset((char *) lc + 512, 0, sizeof(*lc) - 512);
-	lc->async_stack = pcpu->async_stack + ASYNC_SIZE;
-	lc->panic_stack = pcpu->panic_stack + PAGE_SIZE;
+	lc->async_stack = pcpu->async_stack + ASYNC_SIZE
+		- STACK_FRAME_OVERHEAD - sizeof(struct pt_regs);
+	lc->panic_stack = pcpu->panic_stack + PAGE_SIZE
+		- STACK_FRAME_OVERHEAD - sizeof(struct pt_regs);
 	lc->cpu_nr = cpu;
 #ifndef CONFIG_64BIT
 	if (MACHINE_HAS_IEEE) {
@@ -253,7 +255,8 @@ static void pcpu_attach_task(struct pcpu *pcpu, struct task_struct *tsk)
 	struct _lowcore *lc = pcpu->lowcore;
 	struct thread_info *ti = task_thread_info(tsk);
 
-	lc->kernel_stack = (unsigned long) task_stack_page(tsk) + THREAD_SIZE;
+	lc->kernel_stack = (unsigned long) task_stack_page(tsk)
+		+ THREAD_SIZE - STACK_FRAME_OVERHEAD - sizeof(struct pt_regs);
 	lc->thread_info = (unsigned long) task_thread_info(tsk);
 	lc->current_task = (unsigned long) tsk;
 	lc->user_timer = ti->user_timer;
@@ -711,8 +714,7 @@ static void __cpuinit smp_start_secondary(void *cpuvoid)
 	set_cpu_online(smp_processor_id(), true);
 	inc_irq_stat(CPU_RST);
 	local_irq_enable();
-	/* cpu_idle will call schedule for us */
-	cpu_idle();
+	cpu_startup_entry(CPUHP_ONLINE);
 }
 
 /* Upping and downing of CPUs */
@@ -810,8 +812,10 @@ void __init smp_prepare_boot_cpu(void)
 	pcpu->state = CPU_STATE_CONFIGURED;
 	pcpu->address = boot_cpu_address;
 	pcpu->lowcore = (struct _lowcore *)(unsigned long) store_prefix();
-	pcpu->async_stack = S390_lowcore.async_stack - ASYNC_SIZE;
-	pcpu->panic_stack = S390_lowcore.panic_stack - PAGE_SIZE;
+	pcpu->async_stack = S390_lowcore.async_stack - ASYNC_SIZE
+		+ STACK_FRAME_OVERHEAD + sizeof(struct pt_regs);
+	pcpu->panic_stack = S390_lowcore.panic_stack - PAGE_SIZE
+		+ STACK_FRAME_OVERHEAD + sizeof(struct pt_regs);
 	S390_lowcore.percpu_offset = __per_cpu_offset[0];
 	smp_cpu_set_polarization(0, POLARIZATION_UNKNOWN);
 	set_cpu_present(0, true);
diff --git a/arch/s390/kernel/suspend.c b/arch/s390/kernel/suspend.c
index aa1494d0e38..c479d2f9605 100644
--- a/arch/s390/kernel/suspend.c
+++ b/arch/s390/kernel/suspend.c
@@ -41,6 +41,7 @@ struct page_key_data {
 static struct page_key_data *page_key_data;
 static struct page_key_data *page_key_rp, *page_key_wp;
 static unsigned long page_key_rx, page_key_wx;
+unsigned long suspend_zero_pages;
 
 /*
  * For each page in the hibernation image one additional byte is
@@ -149,6 +150,36 @@ int pfn_is_nosave(unsigned long pfn)
 	return 0;
 }
 
+/*
+ * PM notifier callback for suspend
+ */
+static int suspend_pm_cb(struct notifier_block *nb, unsigned long action,
+			 void *ptr)
+{
+	switch (action) {
+	case PM_SUSPEND_PREPARE:
+	case PM_HIBERNATION_PREPARE:
+		suspend_zero_pages = __get_free_pages(GFP_KERNEL, LC_ORDER);
+		if (!suspend_zero_pages)
+			return NOTIFY_BAD;
+		break;
+	case PM_POST_SUSPEND:
+	case PM_POST_HIBERNATION:
+		free_pages(suspend_zero_pages, LC_ORDER);
+		break;
+	default:
+		return NOTIFY_DONE;
+	}
+	return NOTIFY_OK;
+}
+
+static int __init suspend_pm_init(void)
+{
+	pm_notifier(suspend_pm_cb, 0);
+	return 0;
+}
+arch_initcall(suspend_pm_init);
+
 void save_processor_state(void)
 {
 	/* swsusp_arch_suspend() actually saves all cpu register contents.
diff --git a/arch/s390/kernel/swsusp_asm64.S b/arch/s390/kernel/swsusp_asm64.S
index d4ca4e0617b..c487be4cfc8 100644
--- a/arch/s390/kernel/swsusp_asm64.S
+++ b/arch/s390/kernel/swsusp_asm64.S
@@ -36,8 +36,8 @@ ENTRY(swsusp_arch_suspend)
 	/* Store prefix register on stack */
 	stpx	__SF_EMPTY(%r15)
 
-	/* Save prefix register contents for lowcore */
-	llgf	%r4,__SF_EMPTY(%r15)
+	/* Save prefix register contents for lowcore copy */
+	llgf	%r10,__SF_EMPTY(%r15)
 
 	/* Get pointer to save area */
 	lghi	%r1,0x1000
@@ -91,7 +91,18 @@ ENTRY(swsusp_arch_suspend)
 	xc	__SF_EMPTY(4,%r15),__SF_EMPTY(%r15)
 	spx	__SF_EMPTY(%r15)
 
+	/* Save absolute zero pages */
+	larl	%r2,suspend_zero_pages
+	lg	%r2,0(%r2)
+	lghi	%r4,0
+	lghi	%r3,2*PAGE_SIZE
+	lghi	%r5,2*PAGE_SIZE
+1:	mvcle	%r2,%r4,0
+	jo	1b
+
+	/* Copy lowcore to absolute zero lowcore */
 	lghi	%r2,0
+	lgr	%r4,%r10
 	lghi	%r3,2*PAGE_SIZE
 	lghi	%r5,2*PAGE_SIZE
 1:	mvcle	%r2,%r4,0
@@ -248,8 +259,20 @@ restore_registers:
 	/* Load old stack */
 	lg	%r15,0x2f8(%r13)
 
+	/* Save prefix register */
+	mvc __SF_EMPTY(4,%r15),0x318(%r13)
+
+	/* Restore absolute zero pages */
+	lghi	%r2,0
+	larl	%r4,suspend_zero_pages
+	lg	%r4,0(%r4)
+	lghi	%r3,2*PAGE_SIZE
+	lghi	%r5,2*PAGE_SIZE
+1:	mvcle	%r2,%r4,0
+	jo	1b
+
 	/* Restore prefix register */
-	spx	0x318(%r13)
+	spx	__SF_EMPTY(%r15)
 
 	/* Activate DAT */
 	stosm	__SF_EMPTY(%r15),0x04
diff --git a/arch/s390/kernel/traps.c b/arch/s390/kernel/traps.c
index 13dd63fba36..c5762324d9e 100644
--- a/arch/s390/kernel/traps.c
+++ b/arch/s390/kernel/traps.c
@@ -12,49 +12,16 @@
  * 'Traps.c' handles hardware traps and faults after we have saved some
  * state in 'asm.s'.
  */
-#include <linux/sched.h>
-#include <linux/kernel.h>
-#include <linux/string.h>
-#include <linux/errno.h>
+#include <linux/kprobes.h>
+#include <linux/kdebug.h>
+#include <linux/module.h>
 #include <linux/ptrace.h>
-#include <linux/timer.h>
+#include <linux/sched.h>
 #include <linux/mm.h>
-#include <linux/smp.h>
-#include <linux/init.h>
-#include <linux/interrupt.h>
-#include <linux/seq_file.h>
-#include <linux/delay.h>
-#include <linux/module.h>
-#include <linux/kdebug.h>
-#include <linux/kallsyms.h>
-#include <linux/reboot.h>
-#include <linux/kprobes.h>
-#include <linux/bug.h>
-#include <linux/utsname.h>
-#include <asm/uaccess.h>
-#include <asm/io.h>
-#include <linux/atomic.h>
-#include <asm/mathemu.h>
-#include <asm/cpcmd.h>
-#include <asm/lowcore.h>
-#include <asm/debug.h>
-#include <asm/ipl.h>
 #include "entry.h"
 
 int show_unhandled_signals = 1;
 
-#define stack_pointer ({ void **sp; asm("la %0,0(15)" : "=&d" (sp)); sp; })
-
-#ifndef CONFIG_64BIT
-#define LONG "%08lx "
-#define FOURLONG "%08lx %08lx %08lx %08lx\n"
-static int kstack_depth_to_print = 12;
-#else /* CONFIG_64BIT */
-#define LONG "%016lx "
-#define FOURLONG "%016lx %016lx %016lx %016lx\n"
-static int kstack_depth_to_print = 20;
-#endif /* CONFIG_64BIT */
-
 static inline void __user *get_trap_ip(struct pt_regs *regs)
 {
 #ifdef CONFIG_64BIT
@@ -72,215 +39,6 @@ static inline void __user *get_trap_ip(struct pt_regs *regs)
 #endif
 }
 
-/*
- * For show_trace we have tree different stack to consider:
- *   - the panic stack which is used if the kernel stack has overflown
- *   - the asynchronous interrupt stack (cpu related)
- *   - the synchronous kernel stack (process related)
- * The stack trace can start at any of the three stack and can potentially
- * touch all of them. The order is: panic stack, async stack, sync stack.
- */
-static unsigned long
-__show_trace(unsigned long sp, unsigned long low, unsigned long high)
-{
-	struct stack_frame *sf;
-	struct pt_regs *regs;
-
-	while (1) {
-		sp = sp & PSW_ADDR_INSN;
-		if (sp < low || sp > high - sizeof(*sf))
-			return sp;
-		sf = (struct stack_frame *) sp;
-		printk("([<%016lx>] ", sf->gprs[8] & PSW_ADDR_INSN);
-		print_symbol("%s)\n", sf->gprs[8] & PSW_ADDR_INSN);
-		/* Follow the backchain. */
-		while (1) {
-			low = sp;
-			sp = sf->back_chain & PSW_ADDR_INSN;
-			if (!sp)
-				break;
-			if (sp <= low || sp > high - sizeof(*sf))
-				return sp;
-			sf = (struct stack_frame *) sp;
-			printk(" [<%016lx>] ", sf->gprs[8] & PSW_ADDR_INSN);
-			print_symbol("%s\n", sf->gprs[8] & PSW_ADDR_INSN);
-		}
-		/* Zero backchain detected, check for interrupt frame. */
-		sp = (unsigned long) (sf + 1);
-		if (sp <= low || sp > high - sizeof(*regs))
-			return sp;
-		regs = (struct pt_regs *) sp;
-		printk(" [<%016lx>] ", regs->psw.addr & PSW_ADDR_INSN);
-		print_symbol("%s\n", regs->psw.addr & PSW_ADDR_INSN);
-		low = sp;
-		sp = regs->gprs[15];
-	}
-}
-
-static void show_trace(struct task_struct *task, unsigned long *stack)
-{
-	register unsigned long __r15 asm ("15");
-	unsigned long sp;
-
-	sp = (unsigned long) stack;
-	if (!sp)
-		sp = task ? task->thread.ksp : __r15;
-	printk("Call Trace:\n");
-#ifdef CONFIG_CHECK_STACK
-	sp = __show_trace(sp, S390_lowcore.panic_stack - 4096,
-			  S390_lowcore.panic_stack);
-#endif
-	sp = __show_trace(sp, S390_lowcore.async_stack - ASYNC_SIZE,
-			  S390_lowcore.async_stack);
-	if (task)
-		__show_trace(sp, (unsigned long) task_stack_page(task),
-			     (unsigned long) task_stack_page(task) + THREAD_SIZE);
-	else
-		__show_trace(sp, S390_lowcore.thread_info,
-			     S390_lowcore.thread_info + THREAD_SIZE);
-	if (!task)
-		task = current;
-	debug_show_held_locks(task);
-}
-
-void show_stack(struct task_struct *task, unsigned long *sp)
-{
-	register unsigned long * __r15 asm ("15");
-	unsigned long *stack;
-	int i;
-
-	if (!sp)
-		stack = task ? (unsigned long *) task->thread.ksp : __r15;
-	else
-		stack = sp;
-
-	for (i = 0; i < kstack_depth_to_print; i++) {
-		if (((addr_t) stack & (THREAD_SIZE-1)) == 0)
-			break;
-		if ((i * sizeof(long) % 32) == 0)
-			printk("%s       ", i == 0 ? "" : "\n");
-		printk(LONG, *stack++);
-	}
-	printk("\n");
-	show_trace(task, sp);
-}
-
-static void show_last_breaking_event(struct pt_regs *regs)
-{
-#ifdef CONFIG_64BIT
-	printk("Last Breaking-Event-Address:\n");
-	printk(" [<%016lx>] ", regs->args[0] & PSW_ADDR_INSN);
-	print_symbol("%s\n", regs->args[0] & PSW_ADDR_INSN);
-#endif
-}
-
-/*
- * The architecture-independent dump_stack generator
- */
-void dump_stack(void)
-{
-	printk("CPU: %d %s %s %.*s\n",
-	       task_thread_info(current)->cpu, print_tainted(),
-	       init_utsname()->release,
-	       (int)strcspn(init_utsname()->version, " "),
-	       init_utsname()->version);
-	printk("Process %s (pid: %d, task: %p, ksp: %p)\n",
-	       current->comm, current->pid, current,
-	       (void *) current->thread.ksp);
-	show_stack(NULL, NULL);
-}
-EXPORT_SYMBOL(dump_stack);
-
-static inline int mask_bits(struct pt_regs *regs, unsigned long bits)
-{
-	return (regs->psw.mask & bits) / ((~bits + 1) & bits);
-}
-
-void show_registers(struct pt_regs *regs)
-{
-	char *mode;
-
-	mode = user_mode(regs) ? "User" : "Krnl";
-	printk("%s PSW : %p %p",
-	       mode, (void *) regs->psw.mask,
-	       (void *) regs->psw.addr);
-	print_symbol(" (%s)\n", regs->psw.addr & PSW_ADDR_INSN);
-	printk("           R:%x T:%x IO:%x EX:%x Key:%x M:%x W:%x "
-	       "P:%x AS:%x CC:%x PM:%x", mask_bits(regs, PSW_MASK_PER),
-	       mask_bits(regs, PSW_MASK_DAT), mask_bits(regs, PSW_MASK_IO),
-	       mask_bits(regs, PSW_MASK_EXT), mask_bits(regs, PSW_MASK_KEY),
-	       mask_bits(regs, PSW_MASK_MCHECK), mask_bits(regs, PSW_MASK_WAIT),
-	       mask_bits(regs, PSW_MASK_PSTATE), mask_bits(regs, PSW_MASK_ASC),
-	       mask_bits(regs, PSW_MASK_CC), mask_bits(regs, PSW_MASK_PM));
-#ifdef CONFIG_64BIT
-	printk(" EA:%x", mask_bits(regs, PSW_MASK_EA | PSW_MASK_BA));
-#endif
-	printk("\n%s GPRS: " FOURLONG, mode,
-	       regs->gprs[0], regs->gprs[1], regs->gprs[2], regs->gprs[3]);
-	printk("           " FOURLONG,
-	       regs->gprs[4], regs->gprs[5], regs->gprs[6], regs->gprs[7]);
-	printk("           " FOURLONG,
-	       regs->gprs[8], regs->gprs[9], regs->gprs[10], regs->gprs[11]);
-	printk("           " FOURLONG,
-	       regs->gprs[12], regs->gprs[13], regs->gprs[14], regs->gprs[15]);
-
-	show_code(regs);
-}	
-
-void show_regs(struct pt_regs *regs)
-{
-	printk("CPU: %d %s %s %.*s\n",
-	       task_thread_info(current)->cpu, print_tainted(),
-	       init_utsname()->release,
-	       (int)strcspn(init_utsname()->version, " "),
-	       init_utsname()->version);
-	printk("Process %s (pid: %d, task: %p, ksp: %p)\n",
-	       current->comm, current->pid, current,
-	       (void *) current->thread.ksp);
-	show_registers(regs);
-	/* Show stack backtrace if pt_regs is from kernel mode */
-	if (!user_mode(regs))
-		show_trace(NULL, (unsigned long *) regs->gprs[15]);
-	show_last_breaking_event(regs);
-}
-
-static DEFINE_SPINLOCK(die_lock);
-
-void die(struct pt_regs *regs, const char *str)
-{
-	static int die_counter;
-
-	oops_enter();
-	lgr_info_log();
-	debug_stop_all();
-	console_verbose();
-	spin_lock_irq(&die_lock);
-	bust_spinlocks(1);
-	printk("%s: %04x [#%d] ", str, regs->int_code & 0xffff, ++die_counter);
-#ifdef CONFIG_PREEMPT
-	printk("PREEMPT ");
-#endif
-#ifdef CONFIG_SMP
-	printk("SMP ");
-#endif
-#ifdef CONFIG_DEBUG_PAGEALLOC
-	printk("DEBUG_PAGEALLOC");
-#endif
-	printk("\n");
-	notify_die(DIE_OOPS, str, regs, 0, regs->int_code & 0xffff, SIGSEGV);
-	print_modules();
-	show_regs(regs);
-	bust_spinlocks(0);
-	add_taint(TAINT_DIE, LOCKDEP_NOW_UNRELIABLE);
-	spin_unlock_irq(&die_lock);
-	if (in_interrupt())
-		panic("Fatal exception in interrupt");
-	if (panic_on_oops)
-		panic("Fatal exception: panic_on_oops");
-	oops_exit();
-	do_exit(SIGSEGV);
-}
-
 static inline void report_user_fault(struct pt_regs *regs, int signr)
 {
 	if ((task_pid_nr(current) > 1) && !show_unhandled_signals)
diff --git a/arch/s390/kernel/vtime.c b/arch/s390/kernel/vtime.c
index a0042acbd98..3fb09359eda 100644
--- a/arch/s390/kernel/vtime.c
+++ b/arch/s390/kernel/vtime.c
@@ -158,8 +158,6 @@ void __kprobes vtime_stop_cpu(void)
 	unsigned long psw_mask;
 
 	trace_hardirqs_on();
-	/* Don't trace preempt off for idle. */
-	stop_critical_timings();
 
 	/* Wait for external, I/O or machine check interrupt. */
 	psw_mask = psw_kernel_bits | PSW_MASK_WAIT | PSW_MASK_DAT |
@@ -169,9 +167,6 @@ void __kprobes vtime_stop_cpu(void)
 	/* Call the assembler magic in entry.S */
 	psw_idle(idle, psw_mask);
 
-	/* Reenable preemption tracer. */
-	start_critical_timings();
-
 	/* Account time spent with enabled wait psw loaded as idle time. */
 	idle->sequence++;
 	smp_wmb();
diff --git a/arch/s390/kvm/trace.h b/arch/s390/kvm/trace.h
index 2b29e62351d..c2f582bb1cb 100644
--- a/arch/s390/kvm/trace.h
+++ b/arch/s390/kvm/trace.h
@@ -67,7 +67,7 @@ TRACE_EVENT(kvm_s390_sie_fault,
 #define sie_intercept_code				\
 	{0x04, "Instruction"},				\
 	{0x08, "Program interruption"},			\
-	{0x0C, "Instruction and program interuption"},	\
+	{0x0C, "Instruction and program interruption"},	\
 	{0x10, "External request"},			\
 	{0x14, "External interruption"},		\
 	{0x18, "I/O request"},				\
@@ -117,7 +117,7 @@ TRACE_EVENT(kvm_s390_intercept_instruction,
 			   __entry->instruction,
 			   insn_to_mnemonic((unsigned char *)
 					    &__entry->instruction,
-					 __entry->insn) ?
+					 __entry->insn, sizeof(__entry->insn)) ?
 			   "unknown" : __entry->insn)
 	);
 
diff --git a/arch/s390/lib/uaccess_pt.c b/arch/s390/lib/uaccess_pt.c
index dff631d34b4..466fb338396 100644
--- a/arch/s390/lib/uaccess_pt.c
+++ b/arch/s390/lib/uaccess_pt.c
@@ -77,42 +77,69 @@ static size_t copy_in_kernel(size_t count, void __user *to,
  * >= -4095 (IS_ERR_VALUE(x) returns true), a fault has occured and the address
  * contains the (negative) exception code.
  */
-static __always_inline unsigned long follow_table(struct mm_struct *mm,
-						  unsigned long addr, int write)
+#ifdef CONFIG_64BIT
+static unsigned long follow_table(struct mm_struct *mm,
+				  unsigned long address, int write)
 {
-	pgd_t *pgd;
-	pud_t *pud;
-	pmd_t *pmd;
-	pte_t *ptep;
+	unsigned long *table = (unsigned long *)__pa(mm->pgd);
+
+	switch (mm->context.asce_bits & _ASCE_TYPE_MASK) {
+	case _ASCE_TYPE_REGION1:
+		table = table + ((address >> 53) & 0x7ff);
+		if (unlikely(*table & _REGION_ENTRY_INV))
+			return -0x39UL;
+		table = (unsigned long *)(*table & _REGION_ENTRY_ORIGIN);
+	case _ASCE_TYPE_REGION2:
+		table = table + ((address >> 42) & 0x7ff);
+		if (unlikely(*table & _REGION_ENTRY_INV))
+			return -0x3aUL;
+		table = (unsigned long *)(*table & _REGION_ENTRY_ORIGIN);
+	case _ASCE_TYPE_REGION3:
+		table = table + ((address >> 31) & 0x7ff);
+		if (unlikely(*table & _REGION_ENTRY_INV))
+			return -0x3bUL;
+		table = (unsigned long *)(*table & _REGION_ENTRY_ORIGIN);
+	case _ASCE_TYPE_SEGMENT:
+		table = table + ((address >> 20) & 0x7ff);
+		if (unlikely(*table & _SEGMENT_ENTRY_INV))
+			return -0x10UL;
+		if (unlikely(*table & _SEGMENT_ENTRY_LARGE)) {
+			if (write && (*table & _SEGMENT_ENTRY_RO))
+				return -0x04UL;
+			return (*table & _SEGMENT_ENTRY_ORIGIN_LARGE) +
+				(address & ~_SEGMENT_ENTRY_ORIGIN_LARGE);
+		}
+		table = (unsigned long *)(*table & _SEGMENT_ENTRY_ORIGIN);
+	}
+	table = table + ((address >> 12) & 0xff);
+	if (unlikely(*table & _PAGE_INVALID))
+		return -0x11UL;
+	if (write && (*table & _PAGE_RO))
+		return -0x04UL;
+	return (*table & PAGE_MASK) + (address & ~PAGE_MASK);
+}
 
-	pgd = pgd_offset(mm, addr);
-	if (pgd_none(*pgd) || unlikely(pgd_bad(*pgd)))
-		return -0x3aUL;
+#else /* CONFIG_64BIT */
 
-	pud = pud_offset(pgd, addr);
-	if (pud_none(*pud) || unlikely(pud_bad(*pud)))
-		return -0x3bUL;
+static unsigned long follow_table(struct mm_struct *mm,
+				  unsigned long address, int write)
+{
+	unsigned long *table = (unsigned long *)__pa(mm->pgd);
 
-	pmd = pmd_offset(pud, addr);
-	if (pmd_none(*pmd))
+	table = table + ((address >> 20) & 0x7ff);
+	if (unlikely(*table & _SEGMENT_ENTRY_INV))
 		return -0x10UL;
-	if (pmd_large(*pmd)) {
-		if (write && (pmd_val(*pmd) & _SEGMENT_ENTRY_RO))
-			return -0x04UL;
-		return (pmd_val(*pmd) & HPAGE_MASK) + (addr & ~HPAGE_MASK);
-	}
-	if (unlikely(pmd_bad(*pmd)))
-		return -0x10UL;
-
-	ptep = pte_offset_map(pmd, addr);
-	if (!pte_present(*ptep))
+	table = (unsigned long *)(*table & _SEGMENT_ENTRY_ORIGIN);
+	table = table + ((address >> 12) & 0xff);
+	if (unlikely(*table & _PAGE_INVALID))
 		return -0x11UL;
-	if (write && (!pte_write(*ptep) || !pte_dirty(*ptep)))
+	if (write && (*table & _PAGE_RO))
 		return -0x04UL;
-
-	return (pte_val(*ptep) & PAGE_MASK) + (addr & ~PAGE_MASK);
+	return (*table & PAGE_MASK) + (address & ~PAGE_MASK);
 }
 
+#endif /* CONFIG_64BIT */
+
 static __always_inline size_t __user_copy_pt(unsigned long uaddr, void *kptr,
 					     size_t n, int write_user)
 {
@@ -197,7 +224,7 @@ size_t copy_to_user_pt(size_t n, void __user *to, const void *from)
 
 static size_t clear_user_pt(size_t n, void __user *to)
 {
-	void *zpage = &empty_zero_page;
+	void *zpage = (void *) empty_zero_page;
 	long done, size, ret;
 
 	done = 0;
diff --git a/arch/s390/mm/cmm.c b/arch/s390/mm/cmm.c
index 479e9428291..9d84a1feefe 100644
--- a/arch/s390/mm/cmm.c
+++ b/arch/s390/mm/cmm.c
@@ -458,12 +458,10 @@ static int __init cmm_init(void)
 	if (rc)
 		goto out_pm;
 	cmm_thread_ptr = kthread_run(cmm_thread, NULL, "cmmthread");
-	rc = IS_ERR(cmm_thread_ptr) ? PTR_ERR(cmm_thread_ptr) : 0;
-	if (rc)
-		goto out_kthread;
-	return 0;
+	if (!IS_ERR(cmm_thread_ptr))
+		return 0;
 
-out_kthread:
+	rc = PTR_ERR(cmm_thread_ptr);
 	unregister_pm_notifier(&cmm_power_notifier);
 out_pm:
 	unregister_oom_notifier(&cmm_oom_nb);
diff --git a/arch/s390/mm/fault.c b/arch/s390/mm/fault.c
index 2fb9e63b8fc..047c3e4c59a 100644
--- a/arch/s390/mm/fault.c
+++ b/arch/s390/mm/fault.c
@@ -395,8 +395,13 @@ void __kprobes do_protection_exception(struct pt_regs *regs)
 	int fault;
 
 	trans_exc_code = regs->int_parm_long;
-	/* Protection exception is suppressing, decrement psw address. */
-	regs->psw.addr = __rewind_psw(regs->psw, regs->int_code >> 16);
+	/*
+	 * Protection exceptions are suppressing, decrement psw address.
+	 * The exception to this rule are aborted transactions, for these
+	 * the PSW already points to the correct location.
+	 */
+	if (!(regs->int_code & 0x200))
+		regs->psw.addr = __rewind_psw(regs->psw, regs->int_code >> 16);
 	/*
 	 * Check for low-address protection.  This needs to be treated
 	 * as a special case because the translation exception code
diff --git a/arch/s390/mm/hugetlbpage.c b/arch/s390/mm/hugetlbpage.c
index 532525ec88c..121089d5780 100644
--- a/arch/s390/mm/hugetlbpage.c
+++ b/arch/s390/mm/hugetlbpage.c
@@ -39,7 +39,7 @@ int arch_prepare_hugepage(struct page *page)
 	if (!ptep)
 		return -ENOMEM;
 
-	pte = mk_pte(page, PAGE_RW);
+	pte_val(pte) = addr;
 	for (i = 0; i < PTRS_PER_PTE; i++) {
 		set_pte_at(&init_mm, addr + i * PAGE_SIZE, ptep + i, pte);
 		pte_val(pte) += PAGE_SIZE;
diff --git a/arch/s390/mm/init.c b/arch/s390/mm/init.c
index 49ce6bb2c64..0b09b234230 100644
--- a/arch/s390/mm/init.c
+++ b/arch/s390/mm/init.c
@@ -42,11 +42,10 @@ pgd_t swapper_pg_dir[PTRS_PER_PGD] __attribute__((__aligned__(PAGE_SIZE)));
 unsigned long empty_zero_page, zero_page_mask;
 EXPORT_SYMBOL(empty_zero_page);
 
-static unsigned long __init setup_zero_pages(void)
+static void __init setup_zero_pages(void)
 {
 	struct cpuid cpu_id;
 	unsigned int order;
-	unsigned long size;
 	struct page *page;
 	int i;
 
@@ -63,10 +62,18 @@ static unsigned long __init setup_zero_pages(void)
 		break;
 	case 0x2097:	/* z10 */
 	case 0x2098:	/* z10 */
-	default:
+	case 0x2817:	/* z196 */
+	case 0x2818:	/* z196 */
 		order = 2;
 		break;
+	case 0x2827:	/* zEC12 */
+	default:
+		order = 5;
+		break;
 	}
+	/* Limit number of empty zero pages for small memory sizes */
+	if (order > 2 && totalram_pages <= 16384)
+		order = 2;
 
 	empty_zero_page = __get_free_pages(GFP_KERNEL | __GFP_ZERO, order);
 	if (!empty_zero_page)
@@ -75,14 +82,11 @@ static unsigned long __init setup_zero_pages(void)
 	page = virt_to_page((void *) empty_zero_page);
 	split_page(page, order);
 	for (i = 1 << order; i > 0; i--) {
-		SetPageReserved(page);
+		mark_page_reserved(page);
 		page++;
 	}
 
-	size = PAGE_SIZE << order;
-	zero_page_mask = (size - 1) & PAGE_MASK;
-
-	return 1UL << order;
+	zero_page_mask = ((PAGE_SIZE << order) - 1) & PAGE_MASK;
 }
 
 /*
@@ -139,7 +143,7 @@ void __init mem_init(void)
 
 	/* this will put all low memory onto the freelists */
 	totalram_pages += free_all_bootmem();
-	totalram_pages -= setup_zero_pages();	/* Setup zeroed pages. */
+	setup_zero_pages();	/* Setup zeroed pages. */
 
 	reservedpages = 0;
 
@@ -158,34 +162,15 @@ void __init mem_init(void)
 	       PFN_ALIGN((unsigned long)&_eshared) - 1);
 }
 
-void free_init_pages(char *what, unsigned long begin, unsigned long end)
-{
-	unsigned long addr = begin;
-
-	if (begin >= end)
-		return;
-	for (; addr < end; addr += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(addr));
-		init_page_count(virt_to_page(addr));
-		memset((void *)(addr & PAGE_MASK), POISON_FREE_INITMEM,
-		       PAGE_SIZE);
-		free_page(addr);
-		totalram_pages++;
-	}
-	printk(KERN_INFO "Freeing %s: %luk freed\n", what, (end - begin) >> 10);
-}
-
 void free_initmem(void)
 {
-	free_init_pages("unused kernel memory",
-			(unsigned long)&__init_begin,
-			(unsigned long)&__init_end);
+	free_initmem_default(0);
 }
 
 #ifdef CONFIG_BLK_DEV_INITRD
 void __init free_initrd_mem(unsigned long start, unsigned long end)
 {
-	free_init_pages("initrd memory", start, end);
+	free_reserved_area(start, end, POISON_FREE_INITMEM, "initrd");
 }
 #endif
 
diff --git a/arch/s390/mm/pageattr.c b/arch/s390/mm/pageattr.c
index d21040ed5e5..80adfbf7506 100644
--- a/arch/s390/mm/pageattr.c
+++ b/arch/s390/mm/pageattr.c
@@ -9,31 +9,25 @@
 #include <asm/pgtable.h>
 #include <asm/page.h>
 
+static inline unsigned long sske_frame(unsigned long addr, unsigned char skey)
+{
+	asm volatile(".insn rrf,0xb22b0000,%[skey],%[addr],9,0"
+		     : [addr] "+a" (addr) : [skey] "d" (skey));
+	return addr;
+}
+
 void storage_key_init_range(unsigned long start, unsigned long end)
 {
-	unsigned long boundary, function, size;
+	unsigned long boundary, size;
 
 	while (start < end) {
-		if (MACHINE_HAS_EDAT2) {
-			/* set storage keys for a 2GB frame */
-			function = 0x22000 | PAGE_DEFAULT_KEY;
-			size = 1UL << 31;
-			boundary = (start + size) & ~(size - 1);
-			if (boundary <= end) {
-				do {
-					start = pfmf(function, start);
-				} while (start < boundary);
-				continue;
-			}
-		}
 		if (MACHINE_HAS_EDAT1) {
 			/* set storage keys for a 1MB frame */
-			function = 0x21000 | PAGE_DEFAULT_KEY;
 			size = 1UL << 20;
 			boundary = (start + size) & ~(size - 1);
 			if (boundary <= end) {
 				do {
-					start = pfmf(function, start);
+					start = sske_frame(start, PAGE_DEFAULT_KEY);
 				} while (start < boundary);
 				continue;
 			}
diff --git a/arch/s390/mm/pgtable.c b/arch/s390/mm/pgtable.c
index ae44d2a3431..bd954e96f51 100644
--- a/arch/s390/mm/pgtable.c
+++ b/arch/s390/mm/pgtable.c
@@ -379,75 +379,183 @@ out_unmap:
 }
 EXPORT_SYMBOL_GPL(gmap_map_segment);
 
-/*
- * this function is assumed to be called with mmap_sem held
- */
-unsigned long __gmap_fault(unsigned long address, struct gmap *gmap)
+static unsigned long *gmap_table_walk(unsigned long address, struct gmap *gmap)
 {
-	unsigned long *table, vmaddr, segment;
-	struct mm_struct *mm;
-	struct gmap_pgtable *mp;
-	struct gmap_rmap *rmap;
-	struct vm_area_struct *vma;
-	struct page *page;
-	pgd_t *pgd;
-	pud_t *pud;
-	pmd_t *pmd;
+	unsigned long *table;
 
-	current->thread.gmap_addr = address;
-	mm = gmap->mm;
-	/* Walk the gmap address space page table */
 	table = gmap->table + ((address >> 53) & 0x7ff);
 	if (unlikely(*table & _REGION_ENTRY_INV))
-		return -EFAULT;
+		return ERR_PTR(-EFAULT);
 	table = (unsigned long *)(*table & _REGION_ENTRY_ORIGIN);
 	table = table + ((address >> 42) & 0x7ff);
 	if (unlikely(*table & _REGION_ENTRY_INV))
-		return -EFAULT;
+		return ERR_PTR(-EFAULT);
 	table = (unsigned long *)(*table & _REGION_ENTRY_ORIGIN);
 	table = table + ((address >> 31) & 0x7ff);
 	if (unlikely(*table & _REGION_ENTRY_INV))
-		return -EFAULT;
+		return ERR_PTR(-EFAULT);
 	table = (unsigned long *)(*table & _REGION_ENTRY_ORIGIN);
 	table = table + ((address >> 20) & 0x7ff);
+	return table;
+}
+
+/**
+ * __gmap_translate - translate a guest address to a user space address
+ * @address: guest address
+ * @gmap: pointer to guest mapping meta data structure
+ *
+ * Returns user space address which corresponds to the guest address or
+ * -EFAULT if no such mapping exists.
+ * This function does not establish potentially missing page table entries.
+ * The mmap_sem of the mm that belongs to the address space must be held
+ * when this function gets called.
+ */
+unsigned long __gmap_translate(unsigned long address, struct gmap *gmap)
+{
+	unsigned long *segment_ptr, vmaddr, segment;
+	struct gmap_pgtable *mp;
+	struct page *page;
 
+	current->thread.gmap_addr = address;
+	segment_ptr = gmap_table_walk(address, gmap);
+	if (IS_ERR(segment_ptr))
+		return PTR_ERR(segment_ptr);
 	/* Convert the gmap address to an mm address. */
-	segment = *table;
-	if (likely(!(segment & _SEGMENT_ENTRY_INV))) {
+	segment = *segment_ptr;
+	if (!(segment & _SEGMENT_ENTRY_INV)) {
 		page = pfn_to_page(segment >> PAGE_SHIFT);
 		mp = (struct gmap_pgtable *) page->index;
 		return mp->vmaddr | (address & ~PMD_MASK);
 	} else if (segment & _SEGMENT_ENTRY_RO) {
 		vmaddr = segment & _SEGMENT_ENTRY_ORIGIN;
-		vma = find_vma(mm, vmaddr);
-		if (!vma || vma->vm_start > vmaddr)
-			return -EFAULT;
-
-		/* Walk the parent mm page table */
-		pgd = pgd_offset(mm, vmaddr);
-		pud = pud_alloc(mm, pgd, vmaddr);
-		if (!pud)
-			return -ENOMEM;
-		pmd = pmd_alloc(mm, pud, vmaddr);
-		if (!pmd)
-			return -ENOMEM;
-		if (!pmd_present(*pmd) &&
-		    __pte_alloc(mm, vma, pmd, vmaddr))
-			return -ENOMEM;
-		/* pmd now points to a valid segment table entry. */
-		rmap = kmalloc(sizeof(*rmap), GFP_KERNEL|__GFP_REPEAT);
-		if (!rmap)
-			return -ENOMEM;
-		/* Link gmap segment table entry location to page table. */
-		page = pmd_page(*pmd);
-		mp = (struct gmap_pgtable *) page->index;
-		rmap->entry = table;
-		spin_lock(&mm->page_table_lock);
+		return vmaddr | (address & ~PMD_MASK);
+	}
+	return -EFAULT;
+}
+EXPORT_SYMBOL_GPL(__gmap_translate);
+
+/**
+ * gmap_translate - translate a guest address to a user space address
+ * @address: guest address
+ * @gmap: pointer to guest mapping meta data structure
+ *
+ * Returns user space address which corresponds to the guest address or
+ * -EFAULT if no such mapping exists.
+ * This function does not establish potentially missing page table entries.
+ */
+unsigned long gmap_translate(unsigned long address, struct gmap *gmap)
+{
+	unsigned long rc;
+
+	down_read(&gmap->mm->mmap_sem);
+	rc = __gmap_translate(address, gmap);
+	up_read(&gmap->mm->mmap_sem);
+	return rc;
+}
+EXPORT_SYMBOL_GPL(gmap_translate);
+
+static int gmap_connect_pgtable(unsigned long segment,
+				unsigned long *segment_ptr,
+				struct gmap *gmap)
+{
+	unsigned long vmaddr;
+	struct vm_area_struct *vma;
+	struct gmap_pgtable *mp;
+	struct gmap_rmap *rmap;
+	struct mm_struct *mm;
+	struct page *page;
+	pgd_t *pgd;
+	pud_t *pud;
+	pmd_t *pmd;
+
+	mm = gmap->mm;
+	vmaddr = segment & _SEGMENT_ENTRY_ORIGIN;
+	vma = find_vma(mm, vmaddr);
+	if (!vma || vma->vm_start > vmaddr)
+		return -EFAULT;
+	/* Walk the parent mm page table */
+	pgd = pgd_offset(mm, vmaddr);
+	pud = pud_alloc(mm, pgd, vmaddr);
+	if (!pud)
+		return -ENOMEM;
+	pmd = pmd_alloc(mm, pud, vmaddr);
+	if (!pmd)
+		return -ENOMEM;
+	if (!pmd_present(*pmd) &&
+	    __pte_alloc(mm, vma, pmd, vmaddr))
+		return -ENOMEM;
+	/* pmd now points to a valid segment table entry. */
+	rmap = kmalloc(sizeof(*rmap), GFP_KERNEL|__GFP_REPEAT);
+	if (!rmap)
+		return -ENOMEM;
+	/* Link gmap segment table entry location to page table. */
+	page = pmd_page(*pmd);
+	mp = (struct gmap_pgtable *) page->index;
+	rmap->entry = segment_ptr;
+	spin_lock(&mm->page_table_lock);
+	if (*segment_ptr == segment) {
 		list_add(&rmap->list, &mp->mapper);
-		spin_unlock(&mm->page_table_lock);
 		/* Set gmap segment table entry to page table. */
-		*table = pmd_val(*pmd) & PAGE_MASK;
-		return vmaddr | (address & ~PMD_MASK);
+		*segment_ptr = pmd_val(*pmd) & PAGE_MASK;
+		rmap = NULL;
+	}
+	spin_unlock(&mm->page_table_lock);
+	kfree(rmap);
+	return 0;
+}
+
+static void gmap_disconnect_pgtable(struct mm_struct *mm, unsigned long *table)
+{
+	struct gmap_rmap *rmap, *next;
+	struct gmap_pgtable *mp;
+	struct page *page;
+	int flush;
+
+	flush = 0;
+	spin_lock(&mm->page_table_lock);
+	page = pfn_to_page(__pa(table) >> PAGE_SHIFT);
+	mp = (struct gmap_pgtable *) page->index;
+	list_for_each_entry_safe(rmap, next, &mp->mapper, list) {
+		*rmap->entry =
+			_SEGMENT_ENTRY_INV | _SEGMENT_ENTRY_RO | mp->vmaddr;
+		list_del(&rmap->list);
+		kfree(rmap);
+		flush = 1;
+	}
+	spin_unlock(&mm->page_table_lock);
+	if (flush)
+		__tlb_flush_global();
+}
+
+/*
+ * this function is assumed to be called with mmap_sem held
+ */
+unsigned long __gmap_fault(unsigned long address, struct gmap *gmap)
+{
+	unsigned long *segment_ptr, segment;
+	struct gmap_pgtable *mp;
+	struct page *page;
+	int rc;
+
+	current->thread.gmap_addr = address;
+	segment_ptr = gmap_table_walk(address, gmap);
+	if (IS_ERR(segment_ptr))
+		return -EFAULT;
+	/* Convert the gmap address to an mm address. */
+	while (1) {
+		segment = *segment_ptr;
+		if (!(segment & _SEGMENT_ENTRY_INV)) {
+			/* Page table is present */
+			page = pfn_to_page(segment >> PAGE_SHIFT);
+			mp = (struct gmap_pgtable *) page->index;
+			return mp->vmaddr | (address & ~PMD_MASK);
+		}
+		if (!(segment & _SEGMENT_ENTRY_RO))
+			/* Nothing mapped in the gmap address space. */
+			break;
+		rc = gmap_connect_pgtable(segment, segment_ptr, gmap);
+		if (rc)
+			return rc;
 	}
 	return -EFAULT;
 }
@@ -511,29 +619,6 @@ void gmap_discard(unsigned long from, unsigned long to, struct gmap *gmap)
 }
 EXPORT_SYMBOL_GPL(gmap_discard);
 
-void gmap_unmap_notifier(struct mm_struct *mm, unsigned long *table)
-{
-	struct gmap_rmap *rmap, *next;
-	struct gmap_pgtable *mp;
-	struct page *page;
-	int flush;
-
-	flush = 0;
-	spin_lock(&mm->page_table_lock);
-	page = pfn_to_page(__pa(table) >> PAGE_SHIFT);
-	mp = (struct gmap_pgtable *) page->index;
-	list_for_each_entry_safe(rmap, next, &mp->mapper, list) {
-		*rmap->entry =
-			_SEGMENT_ENTRY_INV | _SEGMENT_ENTRY_RO | mp->vmaddr;
-		list_del(&rmap->list);
-		kfree(rmap);
-		flush = 1;
-	}
-	spin_unlock(&mm->page_table_lock);
-	if (flush)
-		__tlb_flush_global();
-}
-
 static inline unsigned long *page_table_alloc_pgste(struct mm_struct *mm,
 						    unsigned long vmaddr)
 {
@@ -586,8 +671,8 @@ static inline void page_table_free_pgste(unsigned long *table)
 {
 }
 
-static inline void gmap_unmap_notifier(struct mm_struct *mm,
-					  unsigned long *table)
+static inline void gmap_disconnect_pgtable(struct mm_struct *mm,
+					   unsigned long *table)
 {
 }
 
@@ -653,7 +738,7 @@ void page_table_free(struct mm_struct *mm, unsigned long *table)
 	unsigned int bit, mask;
 
 	if (mm_has_pgste(mm)) {
-		gmap_unmap_notifier(mm, table);
+		gmap_disconnect_pgtable(mm, table);
 		return page_table_free_pgste(table);
 	}
 	/* Free 1K/2K page table fragment of a 4K page */
@@ -696,7 +781,7 @@ void page_table_free_rcu(struct mmu_gather *tlb, unsigned long *table)
 
 	mm = tlb->mm;
 	if (mm_has_pgste(mm)) {
-		gmap_unmap_notifier(mm, table);
+		gmap_disconnect_pgtable(mm, table);
 		table = (unsigned long *) (__pa(table) | FRAG_MASK);
 		tlb_remove_table(tlb, table);
 		return;
diff --git a/arch/s390/mm/vmem.c b/arch/s390/mm/vmem.c
index ffab84db690..35837054f73 100644
--- a/arch/s390/mm/vmem.c
+++ b/arch/s390/mm/vmem.c
@@ -191,19 +191,16 @@ static void vmem_remove_range(unsigned long start, unsigned long size)
 /*
  * Add a backed mem_map array to the virtual mem_map array.
  */
-int __meminit vmemmap_populate(struct page *start, unsigned long nr, int node)
+int __meminit vmemmap_populate(unsigned long start, unsigned long end, int node)
 {
-	unsigned long address, start_addr, end_addr;
+	unsigned long address = start;
 	pgd_t *pg_dir;
 	pud_t *pu_dir;
 	pmd_t *pm_dir;
 	pte_t *pt_dir;
 	int ret = -ENOMEM;
 
-	start_addr = (unsigned long) start;
-	end_addr = (unsigned long) (start + nr);
-
-	for (address = start_addr; address < end_addr;) {
+	for (address = start; address < end;) {
 		pg_dir = pgd_offset_k(address);
 		if (pgd_none(*pg_dir)) {
 			pu_dir = vmem_pud_alloc();
@@ -262,14 +259,14 @@ int __meminit vmemmap_populate(struct page *start, unsigned long nr, int node)
 		}
 		address += PAGE_SIZE;
 	}
-	memset(start, 0, nr * sizeof(struct page));
+	memset((void *)start, 0, end - start);
 	ret = 0;
 out:
-	flush_tlb_kernel_range(start_addr, end_addr);
+	flush_tlb_kernel_range(start, end);
 	return ret;
 }
 
-void vmemmap_free(struct page *memmap, unsigned long nr_pages)
+void vmemmap_free(unsigned long start, unsigned long end)
 {
 }
 
diff --git a/arch/s390/net/bpf_jit_comp.c b/arch/s390/net/bpf_jit_comp.c
index 0972e91cced..82f165f8078 100644
--- a/arch/s390/net/bpf_jit_comp.c
+++ b/arch/s390/net/bpf_jit_comp.c
@@ -747,10 +747,9 @@ void bpf_jit_compile(struct sk_filter *fp)
 
 	if (!bpf_jit_enable)
 		return;
-	addrs = kmalloc(fp->len * sizeof(*addrs), GFP_KERNEL);
+	addrs = kcalloc(fp->len, sizeof(*addrs), GFP_KERNEL);
 	if (addrs == NULL)
 		return;
-	memset(addrs, 0, fp->len * sizeof(*addrs));
 	memset(&jit, 0, sizeof(cjit));
 	memset(&cjit, 0, sizeof(cjit));
 
diff --git a/arch/s390/oprofile/init.c b/arch/s390/oprofile/init.c
index 584b93674ea..ffeb17ce7f3 100644
--- a/arch/s390/oprofile/init.c
+++ b/arch/s390/oprofile/init.c
@@ -440,6 +440,7 @@ static int oprofile_hwsampler_init(struct oprofile_operations *ops)
 		switch (id.machine) {
 		case 0x2097: case 0x2098: ops->cpu_type = "s390/z10"; break;
 		case 0x2817: case 0x2818: ops->cpu_type = "s390/z196"; break;
+		case 0x2827:              ops->cpu_type = "s390/zEC12"; break;
 		default: return -ENODEV;
 		}
 	}
diff --git a/arch/s390/pci/Makefile b/arch/s390/pci/Makefile
index f0f426a113c..086a2e37935 100644
--- a/arch/s390/pci/Makefile
+++ b/arch/s390/pci/Makefile
@@ -2,5 +2,5 @@
 # Makefile for the s390 PCI subsystem.
 #
 
-obj-$(CONFIG_PCI)	+= pci.o pci_dma.o pci_clp.o pci_msi.o \
-			   pci_sysfs.o pci_event.o pci_debug.o
+obj-$(CONFIG_PCI)	+= pci.o pci_dma.o pci_clp.o pci_msi.o pci_sysfs.o \
+			   pci_event.o pci_debug.o pci_insn.o
diff --git a/arch/s390/pci/pci.c b/arch/s390/pci/pci.c
index 27b4c17855b..e6f15b5d8b7 100644
--- a/arch/s390/pci/pci.c
+++ b/arch/s390/pci/pci.c
@@ -99,9 +99,6 @@ static int __read_mostly aisb_max;
 static struct kmem_cache *zdev_irq_cache;
 static struct kmem_cache *zdev_fmb_cache;
 
-debug_info_t *pci_debug_msg_id;
-debug_info_t *pci_debug_err_id;
-
 static inline int irq_to_msi_nr(unsigned int irq)
 {
 	return irq & ZPCI_MSI_MASK;
@@ -179,7 +176,7 @@ static int zpci_register_airq(struct zpci_dev *zdev, unsigned int aisb,
 	fib->aisb = (u64) bucket->aisb + aisb / 8;
 	fib->aisbo = aisb & ZPCI_MSI_MASK;
 
-	rc = mpcifc_instr(req, fib);
+	rc = s390pci_mod_fc(req, fib);
 	pr_debug("%s mpcifc returned noi: %d\n", __func__, fib->noi);
 
 	free_page((unsigned long) fib);
@@ -209,7 +206,7 @@ static int mod_pci(struct zpci_dev *zdev, int fn, u8 dmaas, struct mod_pci_args
 	fib->iota = args->iota;
 	fib->fmb_addr = args->fmb_addr;
 
-	rc = mpcifc_instr(req, fib);
+	rc = s390pci_mod_fc(req, fib);
 	free_page((unsigned long) fib);
 	return rc;
 }
@@ -249,10 +246,9 @@ int zpci_fmb_enable_device(struct zpci_dev *zdev)
 	if (zdev->fmb)
 		return -EINVAL;
 
-	zdev->fmb = kmem_cache_alloc(zdev_fmb_cache, GFP_KERNEL);
+	zdev->fmb = kmem_cache_zalloc(zdev_fmb_cache, GFP_KERNEL);
 	if (!zdev->fmb)
 		return -ENOMEM;
-	memset(zdev->fmb, 0, sizeof(*zdev->fmb));
 	WARN_ON((u64) zdev->fmb & 0xf);
 
 	args.fmb_addr = virt_to_phys(zdev->fmb);
@@ -284,12 +280,12 @@ static int zpci_cfg_load(struct zpci_dev *zdev, int offset, u32 *val, u8 len)
 	u64 data;
 	int rc;
 
-	rc = pcilg_instr(&data, req, offset);
-	data = data << ((8 - len) * 8);
-	data = le64_to_cpu(data);
-	if (!rc)
+	rc = s390pci_load(&data, req, offset);
+	if (!rc) {
+		data = data << ((8 - len) * 8);
+		data = le64_to_cpu(data);
 		*val = (u32) data;
-	else
+	} else
 		*val = 0xffffffff;
 	return rc;
 }
@@ -302,7 +298,7 @@ static int zpci_cfg_store(struct zpci_dev *zdev, int offset, u32 val, u8 len)
 
 	data = cpu_to_le64(data);
 	data = data >> ((8 - len) * 8);
-	rc = pcistg_instr(data, req, offset);
+	rc = s390pci_store(data, req, offset);
 	return rc;
 }
 
@@ -409,20 +405,28 @@ static int pci_read(struct pci_bus *bus, unsigned int devfn, int where,
 		    int size, u32 *val)
 {
 	struct zpci_dev *zdev = get_zdev_by_bus(bus);
+	int ret;
 
 	if (!zdev || devfn != ZPCI_DEVFN)
-		return 0;
-	return zpci_cfg_load(zdev, where, val, size);
+		ret = -ENODEV;
+	else
+		ret = zpci_cfg_load(zdev, where, val, size);
+
+	return ret;
 }
 
 static int pci_write(struct pci_bus *bus, unsigned int devfn, int where,
 		     int size, u32 val)
 {
 	struct zpci_dev *zdev = get_zdev_by_bus(bus);
+	int ret;
 
 	if (!zdev || devfn != ZPCI_DEVFN)
-		return 0;
-	return zpci_cfg_store(zdev, where, val, size);
+		ret = -ENODEV;
+	else
+		ret = zpci_cfg_store(zdev, where, val, size);
+
+	return ret;
 }
 
 static struct pci_ops pci_root_ops = {
@@ -474,7 +478,7 @@ scan:
 	}
 
 	/* enable interrupts again */
-	sic_instr(SIC_IRQ_MODE_SINGLE, NULL, PCI_ISC);
+	set_irq_ctrl(SIC_IRQ_MODE_SINGLE, NULL, PCI_ISC);
 
 	/* check again to not lose initiative */
 	rmb();
@@ -596,19 +600,6 @@ static void zpci_map_resources(struct zpci_dev *zdev)
 	}
 };
 
-static void zpci_unmap_resources(struct pci_dev *pdev)
-{
-	resource_size_t len;
-	int i;
-
-	for (i = 0; i < PCI_BAR_COUNT; i++) {
-		len = pci_resource_len(pdev, i);
-		if (!len)
-			continue;
-		pci_iounmap(pdev, (void *) pdev->resource[i].start);
-	}
-};
-
 struct zpci_dev *zpci_alloc_device(void)
 {
 	struct zpci_dev *zdev;
@@ -636,32 +627,6 @@ void zpci_free_device(struct zpci_dev *zdev)
 	kfree(zdev);
 }
 
-/* Called on removal of pci_dev, leaves zpci and bus device */
-static void zpci_remove_device(struct pci_dev *pdev)
-{
-	struct zpci_dev *zdev = get_zdev(pdev);
-
-	dev_info(&pdev->dev, "Removing device %u\n", zdev->domain);
-	zdev->state = ZPCI_FN_STATE_CONFIGURED;
-	zpci_dma_exit_device(zdev);
-	zpci_fmb_disable_device(zdev);
-	zpci_sysfs_remove_device(&pdev->dev);
-	zpci_unmap_resources(pdev);
-	list_del(&zdev->entry);		/* can be called from init */
-	zdev->pdev = NULL;
-}
-
-static void zpci_scan_devices(void)
-{
-	struct zpci_dev *zdev;
-
-	mutex_lock(&zpci_list_lock);
-	list_for_each_entry(zdev, &zpci_list, entry)
-		if (zdev->state == ZPCI_FN_STATE_CONFIGURED)
-			zpci_scan_device(zdev);
-	mutex_unlock(&zpci_list_lock);
-}
-
 /*
  * Too late for any s390 specific setup, since interrupts must be set up
  * already which requires DMA setup too and the pci scan will access the
@@ -688,12 +653,6 @@ int pcibios_enable_device(struct pci_dev *pdev, int mask)
 	return 0;
 }
 
-void pcibios_disable_device(struct pci_dev *pdev)
-{
-	zpci_remove_device(pdev);
-	pdev->sysdata = NULL;
-}
-
 int pcibios_add_platform_entries(struct pci_dev *pdev)
 {
 	return zpci_sysfs_add_device(&pdev->dev);
@@ -789,7 +748,7 @@ static int __init zpci_irq_init(void)
 	spin_lock_init(&bucket->lock);
 	/* set summary to 1 to be called every time for the ISC */
 	*zpci_irq_si = 1;
-	sic_instr(SIC_IRQ_MODE_SINGLE, NULL, PCI_ISC);
+	set_irq_ctrl(SIC_IRQ_MODE_SINGLE, NULL, PCI_ISC);
 	return 0;
 
 out_ai:
@@ -872,7 +831,19 @@ static void zpci_free_iomap(struct zpci_dev *zdev, int entry)
 	spin_unlock(&zpci_iomap_lock);
 }
 
-static int zpci_create_device_bus(struct zpci_dev *zdev)
+int pcibios_add_device(struct pci_dev *pdev)
+{
+	struct zpci_dev *zdev = get_zdev(pdev);
+
+	zdev->pdev = pdev;
+	zpci_debug_init_device(zdev);
+	zpci_fmb_enable_device(zdev);
+	zpci_map_resources(zdev);
+
+	return 0;
+}
+
+static int zpci_scan_bus(struct zpci_dev *zdev)
 {
 	struct resource *res;
 	LIST_HEAD(resources);
@@ -909,8 +880,8 @@ static int zpci_create_device_bus(struct zpci_dev *zdev)
 		pci_add_resource(&resources, res);
 	}
 
-	zdev->bus = pci_create_root_bus(NULL, ZPCI_BUS_NR, &pci_root_ops,
-					zdev, &resources);
+	zdev->bus = pci_scan_root_bus(NULL, ZPCI_BUS_NR, &pci_root_ops,
+				      zdev, &resources);
 	if (!zdev->bus)
 		return -EIO;
 
@@ -959,6 +930,13 @@ out:
 }
 EXPORT_SYMBOL_GPL(zpci_enable_device);
 
+int zpci_disable_device(struct zpci_dev *zdev)
+{
+	zpci_dma_exit_device(zdev);
+	return clp_disable_fh(zdev);
+}
+EXPORT_SYMBOL_GPL(zpci_disable_device);
+
 int zpci_create_device(struct zpci_dev *zdev)
 {
 	int rc;
@@ -967,9 +945,16 @@ int zpci_create_device(struct zpci_dev *zdev)
 	if (rc)
 		goto out;
 
-	rc = zpci_create_device_bus(zdev);
+	if (zdev->state == ZPCI_FN_STATE_CONFIGURED) {
+		rc = zpci_enable_device(zdev);
+		if (rc)
+			goto out_free;
+
+		zdev->state = ZPCI_FN_STATE_ONLINE;
+	}
+	rc = zpci_scan_bus(zdev);
 	if (rc)
-		goto out_bus;
+		goto out_disable;
 
 	mutex_lock(&zpci_list_lock);
 	list_add_tail(&zdev->entry, &zpci_list);
@@ -977,21 +962,12 @@ int zpci_create_device(struct zpci_dev *zdev)
 		hotplug_ops->create_slot(zdev);
 	mutex_unlock(&zpci_list_lock);
 
-	if (zdev->state == ZPCI_FN_STATE_STANDBY)
-		return 0;
-
-	rc = zpci_enable_device(zdev);
-	if (rc)
-		goto out_start;
 	return 0;
 
-out_start:
-	mutex_lock(&zpci_list_lock);
-	list_del(&zdev->entry);
-	if (hotplug_ops)
-		hotplug_ops->remove_slot(zdev);
-	mutex_unlock(&zpci_list_lock);
-out_bus:
+out_disable:
+	if (zdev->state == ZPCI_FN_STATE_ONLINE)
+		zpci_disable_device(zdev);
+out_free:
 	zpci_free_domain(zdev);
 out:
 	return rc;
@@ -1016,15 +992,9 @@ int zpci_scan_device(struct zpci_dev *zdev)
 		goto out;
 	}
 
-	zpci_debug_init_device(zdev);
-	zpci_fmb_enable_device(zdev);
-	zpci_map_resources(zdev);
 	pci_bus_add_devices(zdev->bus);
 
-	/* now that pdev was added to the bus mark it as used */
-	zdev->state = ZPCI_FN_STATE_ONLINE;
 	return 0;
-
 out:
 	zpci_dma_exit_device(zdev);
 	clp_disable_fh(zdev);
@@ -1087,13 +1057,13 @@ void zpci_deregister_hp_ops(void)
 }
 EXPORT_SYMBOL_GPL(zpci_deregister_hp_ops);
 
-unsigned int s390_pci_probe = 1;
+unsigned int s390_pci_probe;
 EXPORT_SYMBOL_GPL(s390_pci_probe);
 
 char * __init pcibios_setup(char *str)
 {
-	if (!strcmp(str, "off")) {
-		s390_pci_probe = 0;
+	if (!strcmp(str, "on")) {
+		s390_pci_probe = 1;
 		return NULL;
 	}
 	return str;
@@ -1138,7 +1108,6 @@ static int __init pci_base_init(void)
 	if (rc)
 		goto out_find;
 
-	zpci_scan_devices();
 	return 0;
 
 out_find:
diff --git a/arch/s390/pci/pci_clp.c b/arch/s390/pci/pci_clp.c
index f339fe2feb1..bd34359d154 100644
--- a/arch/s390/pci/pci_clp.c
+++ b/arch/s390/pci/pci_clp.c
@@ -13,6 +13,7 @@
 #include <linux/err.h>
 #include <linux/delay.h>
 #include <linux/pci.h>
+#include <asm/pci_debug.h>
 #include <asm/pci_clp.h>
 
 /*
@@ -144,6 +145,7 @@ int clp_add_pci_device(u32 fid, u32 fh, int configured)
 	struct zpci_dev *zdev;
 	int rc;
 
+	zpci_dbg(3, "add fid:%x, fh:%x, c:%d\n", fid, fh, configured);
 	zdev = zpci_alloc_device();
 	if (IS_ERR(zdev))
 		return PTR_ERR(zdev);
@@ -204,8 +206,8 @@ static int clp_set_pci_fn(u32 *fh, u8 nr_dma_as, u8 command)
 	if (!rc && rrb->response.hdr.rsp == CLP_RC_OK)
 		*fh = rrb->response.fh;
 	else {
-		pr_err("Set PCI FN failed with response: %x  cc: %d\n",
-			rrb->response.hdr.rsp, rc);
+		zpci_dbg(0, "SPF fh:%x, cc:%d, resp:%x\n", *fh, rc,
+			 rrb->response.hdr.rsp);
 		rc = -EIO;
 	}
 	clp_free_block(rrb);
@@ -221,6 +223,8 @@ int clp_enable_fh(struct zpci_dev *zdev, u8 nr_dma_as)
 	if (!rc)
 		/* Success -> store enabled handle in zdev */
 		zdev->fh = fh;
+
+	zpci_dbg(3, "ena fid:%x, fh:%x, rc:%d\n", zdev->fid, zdev->fh, rc);
 	return rc;
 }
 
@@ -237,9 +241,8 @@ int clp_disable_fh(struct zpci_dev *zdev)
 	if (!rc)
 		/* Success -> store disabled handle in zdev */
 		zdev->fh = fh;
-	else
-		dev_err(&zdev->pdev->dev,
-			"Failed to disable fn handle: 0x%x\n", fh);
+
+	zpci_dbg(3, "dis fid:%x, fh:%x, rc:%d\n", zdev->fid, zdev->fh, rc);
 	return rc;
 }
 
diff --git a/arch/s390/pci/pci_debug.c b/arch/s390/pci/pci_debug.c
index a5d07bc2a54..771b82359af 100644
--- a/arch/s390/pci/pci_debug.c
+++ b/arch/s390/pci/pci_debug.c
@@ -11,12 +11,17 @@
 #include <linux/kernel.h>
 #include <linux/seq_file.h>
 #include <linux/debugfs.h>
+#include <linux/export.h>
 #include <linux/pci.h>
 #include <asm/debug.h>
 
 #include <asm/pci_dma.h>
 
 static struct dentry *debugfs_root;
+debug_info_t *pci_debug_msg_id;
+EXPORT_SYMBOL_GPL(pci_debug_msg_id);
+debug_info_t *pci_debug_err_id;
+EXPORT_SYMBOL_GPL(pci_debug_err_id);
 
 static char *pci_perf_names[] = {
 	/* hardware counters */
@@ -168,7 +173,6 @@ int __init zpci_debug_init(void)
 		return -EINVAL;
 	debug_register_view(pci_debug_msg_id, &debug_sprintf_view);
 	debug_set_level(pci_debug_msg_id, 3);
-	zpci_dbg("Debug view initialized\n");
 
 	/* error log */
 	pci_debug_err_id = debug_register("pci_error", 2, 1, 16);
@@ -176,7 +180,6 @@ int __init zpci_debug_init(void)
 		return -EINVAL;
 	debug_register_view(pci_debug_err_id, &debug_hex_ascii_view);
 	debug_set_level(pci_debug_err_id, 6);
-	zpci_err("Debug view initialized\n");
 
 	debugfs_root = debugfs_create_dir("pci", NULL);
 	return 0;
diff --git a/arch/s390/pci/pci_dma.c b/arch/s390/pci/pci_dma.c
index a547419907c..f8e69d5bc0a 100644
--- a/arch/s390/pci/pci_dma.c
+++ b/arch/s390/pci/pci_dma.c
@@ -169,8 +169,9 @@ static int dma_update_trans(struct zpci_dev *zdev, unsigned long pa,
 		 * needs to be redone!
 		 */
 		goto no_refresh;
-	rc = rpcit_instr((u64) zdev->fh << 32, start_dma_addr,
-			  nr_pages * PAGE_SIZE);
+
+	rc = s390pci_refresh_trans((u64) zdev->fh << 32, start_dma_addr,
+				   nr_pages * PAGE_SIZE);
 
 no_refresh:
 	spin_unlock_irqrestore(&zdev->dma_table_lock, irq_flags);
@@ -268,8 +269,6 @@ static dma_addr_t s390_dma_map_pages(struct device *dev, struct page *page,
 	int flags = ZPCI_PTE_VALID;
 	dma_addr_t dma_addr;
 
-	WARN_ON_ONCE(offset > PAGE_SIZE);
-
 	/* This rounds up number of pages based on size and offset */
 	nr_pages = iommu_num_pages(pa, size, PAGE_SIZE);
 	iommu_page_index = dma_alloc_iommu(zdev, nr_pages);
@@ -291,7 +290,7 @@ static dma_addr_t s390_dma_map_pages(struct device *dev, struct page *page,
 
 	if (!dma_update_trans(zdev, pa, dma_addr, size, flags)) {
 		atomic64_add(nr_pages, (atomic64_t *) &zdev->fmb->mapped_pages);
-		return dma_addr + offset;
+		return dma_addr + (offset & ~PAGE_MASK);
 	}
 
 out_free:
diff --git a/arch/s390/pci/pci_insn.c b/arch/s390/pci/pci_insn.c
new file mode 100644
index 00000000000..22eeb9d7ffe
--- /dev/null
+++ b/arch/s390/pci/pci_insn.c
@@ -0,0 +1,202 @@
+/*
+ * s390 specific pci instructions
+ *
+ * Copyright IBM Corp. 2013
+ */
+
+#include <linux/export.h>
+#include <linux/errno.h>
+#include <linux/delay.h>
+#include <asm/pci_insn.h>
+#include <asm/processor.h>
+
+#define ZPCI_INSN_BUSY_DELAY	1	/* 1 microsecond */
+
+/* Modify PCI Function Controls */
+static inline u8 __mpcifc(u64 req, struct zpci_fib *fib, u8 *status)
+{
+	u8 cc;
+
+	asm volatile (
+		"	.insn	rxy,0xe300000000d0,%[req],%[fib]\n"
+		"	ipm	%[cc]\n"
+		"	srl	%[cc],28\n"
+		: [cc] "=d" (cc), [req] "+d" (req), [fib] "+Q" (*fib)
+		: : "cc");
+	*status = req >> 24 & 0xff;
+	return cc;
+}
+
+int s390pci_mod_fc(u64 req, struct zpci_fib *fib)
+{
+	u8 cc, status;
+
+	do {
+		cc = __mpcifc(req, fib, &status);
+		if (cc == 2)
+			msleep(ZPCI_INSN_BUSY_DELAY);
+	} while (cc == 2);
+
+	if (cc)
+		printk_once(KERN_ERR "%s: error cc: %d  status: %d\n",
+			     __func__, cc, status);
+	return (cc) ? -EIO : 0;
+}
+
+/* Refresh PCI Translations */
+static inline u8 __rpcit(u64 fn, u64 addr, u64 range, u8 *status)
+{
+	register u64 __addr asm("2") = addr;
+	register u64 __range asm("3") = range;
+	u8 cc;
+
+	asm volatile (
+		"	.insn	rre,0xb9d30000,%[fn],%[addr]\n"
+		"	ipm	%[cc]\n"
+		"	srl	%[cc],28\n"
+		: [cc] "=d" (cc), [fn] "+d" (fn)
+		: [addr] "d" (__addr), "d" (__range)
+		: "cc");
+	*status = fn >> 24 & 0xff;
+	return cc;
+}
+
+int s390pci_refresh_trans(u64 fn, u64 addr, u64 range)
+{
+	u8 cc, status;
+
+	do {
+		cc = __rpcit(fn, addr, range, &status);
+		if (cc == 2)
+			udelay(ZPCI_INSN_BUSY_DELAY);
+	} while (cc == 2);
+
+	if (cc)
+		printk_once(KERN_ERR "%s: error cc: %d  status: %d  dma_addr: %Lx  size: %Lx\n",
+			    __func__, cc, status, addr, range);
+	return (cc) ? -EIO : 0;
+}
+
+/* Set Interruption Controls */
+void set_irq_ctrl(u16 ctl, char *unused, u8 isc)
+{
+	asm volatile (
+		"	.insn	rsy,0xeb00000000d1,%[ctl],%[isc],%[u]\n"
+		: : [ctl] "d" (ctl), [isc] "d" (isc << 27), [u] "Q" (*unused));
+}
+
+/* PCI Load */
+static inline int __pcilg(u64 *data, u64 req, u64 offset, u8 *status)
+{
+	register u64 __req asm("2") = req;
+	register u64 __offset asm("3") = offset;
+	int cc = -ENXIO;
+	u64 __data;
+
+	asm volatile (
+		"	.insn	rre,0xb9d20000,%[data],%[req]\n"
+		"0:	ipm	%[cc]\n"
+		"	srl	%[cc],28\n"
+		"1:\n"
+		EX_TABLE(0b, 1b)
+		: [cc] "+d" (cc), [data] "=d" (__data), [req] "+d" (__req)
+		:  "d" (__offset)
+		: "cc");
+	*status = __req >> 24 & 0xff;
+	if (!cc)
+		*data = __data;
+
+	return cc;
+}
+
+int s390pci_load(u64 *data, u64 req, u64 offset)
+{
+	u8 status;
+	int cc;
+
+	do {
+		cc = __pcilg(data, req, offset, &status);
+		if (cc == 2)
+			udelay(ZPCI_INSN_BUSY_DELAY);
+	} while (cc == 2);
+
+	if (cc)
+		printk_once(KERN_ERR "%s: error cc: %d  status: %d  req: %Lx  offset: %Lx\n",
+			    __func__, cc, status, req, offset);
+	return (cc > 0) ? -EIO : cc;
+}
+EXPORT_SYMBOL_GPL(s390pci_load);
+
+/* PCI Store */
+static inline int __pcistg(u64 data, u64 req, u64 offset, u8 *status)
+{
+	register u64 __req asm("2") = req;
+	register u64 __offset asm("3") = offset;
+	int cc = -ENXIO;
+
+	asm volatile (
+		"	.insn	rre,0xb9d00000,%[data],%[req]\n"
+		"0:	ipm	%[cc]\n"
+		"	srl	%[cc],28\n"
+		"1:\n"
+		EX_TABLE(0b, 1b)
+		: [cc] "+d" (cc), [req] "+d" (__req)
+		: "d" (__offset), [data] "d" (data)
+		: "cc");
+	*status = __req >> 24 & 0xff;
+	return cc;
+}
+
+int s390pci_store(u64 data, u64 req, u64 offset)
+{
+	u8 status;
+	int cc;
+
+	do {
+		cc = __pcistg(data, req, offset, &status);
+		if (cc == 2)
+			udelay(ZPCI_INSN_BUSY_DELAY);
+	} while (cc == 2);
+
+	if (cc)
+		printk_once(KERN_ERR "%s: error cc: %d  status: %d  req: %Lx  offset: %Lx\n",
+			__func__, cc, status, req, offset);
+	return (cc > 0) ? -EIO : cc;
+}
+EXPORT_SYMBOL_GPL(s390pci_store);
+
+/* PCI Store Block */
+static inline int __pcistb(const u64 *data, u64 req, u64 offset, u8 *status)
+{
+	int cc = -ENXIO;
+
+	asm volatile (
+		"	.insn	rsy,0xeb00000000d0,%[req],%[offset],%[data]\n"
+		"0:	ipm	%[cc]\n"
+		"	srl	%[cc],28\n"
+		"1:\n"
+		EX_TABLE(0b, 1b)
+		: [cc] "+d" (cc), [req] "+d" (req)
+		: [offset] "d" (offset), [data] "Q" (*data)
+		: "cc");
+	*status = req >> 24 & 0xff;
+	return cc;
+}
+
+int s390pci_store_block(const u64 *data, u64 req, u64 offset)
+{
+	u8 status;
+	int cc;
+
+	do {
+		cc = __pcistb(data, req, offset, &status);
+		if (cc == 2)
+			udelay(ZPCI_INSN_BUSY_DELAY);
+	} while (cc == 2);
+
+	if (cc)
+		printk_once(KERN_ERR "%s: error cc: %d  status: %d  req: %Lx  offset: %Lx\n",
+			    __func__, cc, status, req, offset);
+	return (cc > 0) ? -EIO : cc;
+}
+EXPORT_SYMBOL_GPL(s390pci_store_block);
diff --git a/arch/s390/pci/pci_msi.c b/arch/s390/pci/pci_msi.c
index 0297931335e..b097aed05a9 100644
--- a/arch/s390/pci/pci_msi.c
+++ b/arch/s390/pci/pci_msi.c
@@ -18,8 +18,9 @@
 
 /* mapping of irq numbers to msi_desc */
 static struct hlist_head *msi_hash;
-static unsigned int msihash_shift = 6;
-#define msi_hashfn(nr)	hash_long(nr, msihash_shift)
+static const unsigned int msi_hash_bits = 8;
+#define MSI_HASH_BUCKETS (1U << msi_hash_bits)
+#define msi_hashfn(nr)	hash_long(nr, msi_hash_bits)
 
 static DEFINE_SPINLOCK(msi_map_lock);
 
@@ -74,6 +75,7 @@ int zpci_setup_msi_irq(struct zpci_dev *zdev, struct msi_desc *msi,
 	map->irq = nr;
 	map->msi = msi;
 	zdev->msi_map[nr & ZPCI_MSI_MASK] = map;
+	INIT_HLIST_NODE(&map->msi_chain);
 
 	pr_debug("%s hashing irq: %u  to bucket nr: %llu\n",
 		__func__, nr, msi_hashfn(nr));
@@ -125,11 +127,11 @@ int __init zpci_msihash_init(void)
 {
 	unsigned int i;
 
-	msi_hash = kmalloc(256 * sizeof(*msi_hash), GFP_KERNEL);
+	msi_hash = kmalloc(MSI_HASH_BUCKETS * sizeof(*msi_hash), GFP_KERNEL);
 	if (!msi_hash)
 		return -ENOMEM;
 
-	for (i = 0; i < (1U << msihash_shift); i++)
+	for (i = 0; i < MSI_HASH_BUCKETS; i++)
 		INIT_HLIST_HEAD(&msi_hash[i]);
 	return 0;
 }
diff --git a/arch/score/kernel/process.c b/arch/score/kernel/process.c
index 79568466b57..f4c6d02421d 100644
--- a/arch/score/kernel/process.c
+++ b/arch/score/kernel/process.c
@@ -41,24 +41,6 @@ void machine_halt(void) {}
 /* If or when software machine-power-off is implemented, add code here. */
 void machine_power_off(void) {}
 
-/*
- * The idle thread. There's no useful work to be
- * done, so just try to conserve power and have a
- * low exit latency (ie sit in a loop waiting for
- * somebody to say that they'd like to reschedule)
- */
-void __noreturn cpu_idle(void)
-{
-	/* endless idle loop with no priority at all */
-	while (1) {
-		rcu_idle_enter();
-		while (!need_resched())
-			barrier();
-		rcu_idle_exit();
-		schedule_preempt_disabled();
-	}
-}
-
 void ret_from_fork(void);
 void ret_from_kernel_thread(void);
 
diff --git a/arch/score/mm/init.c b/arch/score/mm/init.c
index cee6bce1e30..1592aad7dbc 100644
--- a/arch/score/mm/init.c
+++ b/arch/score/mm/init.c
@@ -43,7 +43,7 @@ EXPORT_SYMBOL_GPL(empty_zero_page);
 
 static struct kcore_list kcore_mem, kcore_vmalloc;
 
-static unsigned long setup_zero_page(void)
+static void setup_zero_page(void)
 {
 	struct page *page;
 
@@ -52,9 +52,7 @@ static unsigned long setup_zero_page(void)
 		panic("Oh boy, that early out of memory?");
 
 	page = virt_to_page((void *) empty_zero_page);
-	SetPageReserved(page);
-
-	return 1UL;
+	mark_page_reserved(page);
 }
 
 #ifndef CONFIG_NEED_MULTIPLE_NODES
@@ -84,7 +82,7 @@ void __init mem_init(void)
 
 	high_memory = (void *) __va(max_low_pfn << PAGE_SHIFT);
 	totalram_pages += free_all_bootmem();
-	totalram_pages -= setup_zero_page();	/* Setup zeroed pages. */
+	setup_zero_page();	/* Setup zeroed pages. */
 	reservedpages = 0;
 
 	for (tmp = 0; tmp < max_low_pfn; tmp++)
@@ -109,37 +107,16 @@ void __init mem_init(void)
 }
 #endif /* !CONFIG_NEED_MULTIPLE_NODES */
 
-static void free_init_pages(const char *what, unsigned long begin, unsigned long end)
-{
-	unsigned long pfn;
-
-	for (pfn = PFN_UP(begin); pfn < PFN_DOWN(end); pfn++) {
-		struct page *page = pfn_to_page(pfn);
-		void *addr = phys_to_virt(PFN_PHYS(pfn));
-
-		ClearPageReserved(page);
-		init_page_count(page);
-		memset(addr, POISON_FREE_INITMEM, PAGE_SIZE);
-		__free_page(page);
-		totalram_pages++;
-	}
-	printk(KERN_INFO "Freeing %s: %ldk freed\n", what, (end - begin) >> 10);
-}
-
 #ifdef CONFIG_BLK_DEV_INITRD
 void free_initrd_mem(unsigned long start, unsigned long end)
 {
-	free_init_pages("initrd memory",
-		virt_to_phys((void *) start),
-		virt_to_phys((void *) end));
+	free_reserved_area(start, end, POISON_FREE_INITMEM, "initrd");
 }
 #endif
 
 void __init_refok free_initmem(void)
 {
-	free_init_pages("unused kernel memory",
-	__pa(&__init_begin),
-	__pa(&__init_end));
+	free_initmem_default(POISON_FREE_INITMEM);
 }
 
 unsigned long pgd_current;
diff --git a/arch/sh/Kconfig b/arch/sh/Kconfig
index 5e859633ce6..1ea597c6497 100644
--- a/arch/sh/Kconfig
+++ b/arch/sh/Kconfig
@@ -33,6 +33,7 @@ config SUPERH
 	select GENERIC_ATOMIC64
 	select GENERIC_IRQ_SHOW
 	select GENERIC_SMP_IDLE_THREAD
+	select GENERIC_IDLE_POLL_SETUP
 	select GENERIC_CLOCKEVENTS
 	select GENERIC_CMOS_UPDATE if SH_SH03 || SH_DREAMCAST
 	select GENERIC_STRNCPY_FROM_USER
@@ -148,9 +149,6 @@ config ARCH_HAS_ILOG2_U32
 config ARCH_HAS_ILOG2_U64
 	def_bool n
 
-config ARCH_HAS_DEFAULT_IDLE
-	def_bool y
-
 config NO_IOPORT
 	def_bool !PCI
 	depends on !SH_CAYMAN && !SH_SH4202_MICRODEV && !SH_SHMIN && \
diff --git a/arch/sh/boards/mach-ecovec24/setup.c b/arch/sh/boards/mach-ecovec24/setup.c
index aaff7671101..764530c85aa 100644
--- a/arch/sh/boards/mach-ecovec24/setup.c
+++ b/arch/sh/boards/mach-ecovec24/setup.c
@@ -254,11 +254,13 @@ static int usbhs_get_id(struct platform_device *pdev)
 	return gpio_get_value(GPIO_PTB3);
 }
 
-static void usbhs_phy_reset(struct platform_device *pdev)
+static int usbhs_phy_reset(struct platform_device *pdev)
 {
 	/* enable vbus if HOST */
 	if (!gpio_get_value(GPIO_PTB3))
 		gpio_set_value(GPIO_PTB5, 1);
+
+	return 0;
 }
 
 static struct renesas_usbhs_platform_info usbhs_info = {
diff --git a/arch/sh/drivers/pci/pcie-sh7786.c b/arch/sh/drivers/pci/pcie-sh7786.c
index c2c85f6cd73..a162a7f86b2 100644
--- a/arch/sh/drivers/pci/pcie-sh7786.c
+++ b/arch/sh/drivers/pci/pcie-sh7786.c
@@ -35,7 +35,7 @@ static unsigned int nr_ports;
 
 static struct sh7786_pcie_hwops {
 	int (*core_init)(void);
-	async_func_ptr *port_init_hw;
+	async_func_t port_init_hw;
 } *sh7786_pcie_hwops;
 
 static struct resource sh7786_pci0_resources[] = {
diff --git a/arch/sh/include/asm/hugetlb.h b/arch/sh/include/asm/hugetlb.h
index b3808c7d67b..699255d6d1c 100644
--- a/arch/sh/include/asm/hugetlb.h
+++ b/arch/sh/include/asm/hugetlb.h
@@ -3,6 +3,7 @@
 
 #include <asm/cacheflush.h>
 #include <asm/page.h>
+#include <asm-generic/hugetlb.h>
 
 
 static inline int is_hugepage_only_range(struct mm_struct *mm,
diff --git a/arch/sh/include/asm/thread_info.h b/arch/sh/include/asm/thread_info.h
index 7d5ac4e4848..45a93669289 100644
--- a/arch/sh/include/asm/thread_info.h
+++ b/arch/sh/include/asm/thread_info.h
@@ -207,8 +207,6 @@ static inline bool test_and_clear_restore_sigmask(void)
 	return true;
 }
 
-#define tsk_is_polling(t) test_tsk_thread_flag(t, TIF_POLLING_NRFLAG)
-
 #endif	/* !__ASSEMBLY__ */
 
 #endif /* __KERNEL__ */
diff --git a/arch/sh/kernel/idle.c b/arch/sh/kernel/idle.c
index 3d5a1b387cc..2ea4483fd72 100644
--- a/arch/sh/kernel/idle.c
+++ b/arch/sh/kernel/idle.c
@@ -24,98 +24,24 @@
 
 static void (*sh_idle)(void);
 
-static int hlt_counter;
-
-static int __init nohlt_setup(char *__unused)
-{
-	hlt_counter = 1;
-	return 1;
-}
-__setup("nohlt", nohlt_setup);
-
-static int __init hlt_setup(char *__unused)
-{
-	hlt_counter = 0;
-	return 1;
-}
-__setup("hlt", hlt_setup);
-
-static inline int hlt_works(void)
-{
-	return !hlt_counter;
-}
-
-/*
- * On SMP it's slightly faster (but much more power-consuming!)
- * to poll the ->work.need_resched flag instead of waiting for the
- * cross-CPU IPI to arrive. Use this option with caution.
- */
-static void poll_idle(void)
+void default_idle(void)
 {
+	set_bl_bit();
 	local_irq_enable();
-	while (!need_resched())
-		cpu_relax();
+	/* Isn't this racy ? */
+	cpu_sleep();
+	clear_bl_bit();
 }
 
-void default_idle(void)
+void arch_cpu_idle_dead(void)
 {
-	if (hlt_works()) {
-		clear_thread_flag(TIF_POLLING_NRFLAG);
-		smp_mb__after_clear_bit();
-
-		set_bl_bit();
-		if (!need_resched()) {
-			local_irq_enable();
-			cpu_sleep();
-		} else
-			local_irq_enable();
-
-		set_thread_flag(TIF_POLLING_NRFLAG);
-		clear_bl_bit();
-	} else
-		poll_idle();
+	play_dead();
 }
 
-/*
- * The idle thread. There's no useful work to be done, so just try to conserve
- * power and have a low exit latency (ie sit in a loop waiting for somebody to
- * say that they'd like to reschedule)
- */
-void cpu_idle(void)
+void arch_cpu_idle(void)
 {
-	unsigned int cpu = smp_processor_id();
-
-	set_thread_flag(TIF_POLLING_NRFLAG);
-
-	/* endless idle loop with no priority at all */
-	while (1) {
-		tick_nohz_idle_enter();
-		rcu_idle_enter();
-
-		while (!need_resched()) {
-			check_pgt_cache();
-			rmb();
-
-			if (cpu_is_offline(cpu))
-				play_dead();
-
-			local_irq_disable();
-			/* Don't trace irqs off for idle */
-			stop_critical_timings();
-			if (cpuidle_idle_call())
-				sh_idle();
-			/*
-			 * Sanity check to ensure that sh_idle() returns
-			 * with IRQs enabled
-			 */
-			WARN_ON(irqs_disabled());
-			start_critical_timings();
-		}
-
-		rcu_idle_exit();
-		tick_nohz_idle_exit();
-		schedule_preempt_disabled();
-	}
+	if (cpuidle_idle_call())
+		sh_idle();
 }
 
 void __init select_idle_routine(void)
@@ -123,13 +49,8 @@ void __init select_idle_routine(void)
 	/*
 	 * If a platform has set its own idle routine, leave it alone.
 	 */
-	if (sh_idle)
-		return;
-
-	if (hlt_works())
+	if (!sh_idle)
 		sh_idle = default_idle;
-	else
-		sh_idle = poll_idle;
 }
 
 void stop_this_cpu(void *unused)
diff --git a/arch/sh/kernel/sh_bios.c b/arch/sh/kernel/sh_bios.c
index 47475cca068..fe584e51696 100644
--- a/arch/sh/kernel/sh_bios.c
+++ b/arch/sh/kernel/sh_bios.c
@@ -104,6 +104,7 @@ void sh_bios_vbr_reload(void)
 		);
 }
 
+#ifdef CONFIG_EARLY_PRINTK
 /*
  *	Print a string through the BIOS
  */
@@ -144,8 +145,6 @@ static struct console bios_console = {
 	.index		= -1,
 };
 
-static struct console *early_console;
-
 static int __init setup_early_printk(char *buf)
 {
 	int keep_early = 0;
@@ -170,3 +169,4 @@ static int __init setup_early_printk(char *buf)
 	return 0;
 }
 early_param("earlyprintk", setup_early_printk);
+#endif
diff --git a/arch/sh/kernel/smp.c b/arch/sh/kernel/smp.c
index 2062aa88af4..45696451f0e 100644
--- a/arch/sh/kernel/smp.c
+++ b/arch/sh/kernel/smp.c
@@ -203,7 +203,7 @@ asmlinkage void __cpuinit start_secondary(void)
 	set_cpu_online(cpu, true);
 	per_cpu(cpu_state, cpu) = CPU_ONLINE;
 
-	cpu_idle();
+	cpu_startup_entry(CPUHP_ONLINE);
 }
 
 extern struct {
diff --git a/arch/sh/mm/init.c b/arch/sh/mm/init.c
index 10579403714..20f9ead650d 100644
--- a/arch/sh/mm/init.c
+++ b/arch/sh/mm/init.c
@@ -417,15 +417,13 @@ void __init mem_init(void)
 
 	for_each_online_node(nid) {
 		pg_data_t *pgdat = NODE_DATA(nid);
-		unsigned long node_pages = 0;
 		void *node_high_memory;
 
 		num_physpages += pgdat->node_present_pages;
 
 		if (pgdat->node_spanned_pages)
-			node_pages = free_all_bootmem_node(pgdat);
+			totalram_pages += free_all_bootmem_node(pgdat);
 
-		totalram_pages += node_pages;
 
 		node_high_memory = (void *)__va((pgdat->node_start_pfn +
 						 pgdat->node_spanned_pages) <<
@@ -501,31 +499,13 @@ void __init mem_init(void)
 
 void free_initmem(void)
 {
-	unsigned long addr;
-
-	addr = (unsigned long)(&__init_begin);
-	for (; addr < (unsigned long)(&__init_end); addr += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(addr));
-		init_page_count(virt_to_page(addr));
-		free_page(addr);
-		totalram_pages++;
-	}
-	printk("Freeing unused kernel memory: %ldk freed\n",
-	       ((unsigned long)&__init_end -
-	        (unsigned long)&__init_begin) >> 10);
+	free_initmem_default(0);
 }
 
 #ifdef CONFIG_BLK_DEV_INITRD
 void free_initrd_mem(unsigned long start, unsigned long end)
 {
-	unsigned long p;
-	for (p = start; p < end; p += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(p));
-		init_page_count(virt_to_page(p));
-		free_page(p);
-		totalram_pages++;
-	}
-	printk("Freeing initrd memory: %ldk freed\n", (end - start) >> 10);
+	free_reserved_area(start, end, 0, "initrd");
 }
 #endif
 
diff --git a/arch/sparc/Kconfig b/arch/sparc/Kconfig
index 3d361f23630..66dc562950a 100644
--- a/arch/sparc/Kconfig
+++ b/arch/sparc/Kconfig
@@ -407,6 +407,8 @@ config SERIAL_CONSOLE
 config SPARC_LEON
 	bool "Sparc Leon processor family"
 	depends on SPARC32
+	select USB_EHCI_BIG_ENDIAN_MMIO
+	select USB_EHCI_BIG_ENDIAN_DESC
 	---help---
 	  If you say Y here if you are running on a SPARC-LEON processor.
 	  The LEON processor is a synthesizable VHDL model of the
diff --git a/arch/sparc/include/asm/Kbuild b/arch/sparc/include/asm/Kbuild
index e26d430ce2f..ff18e3cfb6b 100644
--- a/arch/sparc/include/asm/Kbuild
+++ b/arch/sparc/include/asm/Kbuild
@@ -2,11 +2,16 @@
 
 
 generic-y += clkdev.h
+generic-y += cputime.h
 generic-y += div64.h
+generic-y += emergency-restart.h
 generic-y += exec.h
 generic-y += local64.h
+generic-y += mutex.h
 generic-y += irq_regs.h
 generic-y += local.h
 generic-y += module.h
+generic-y += serial.h
 generic-y += trace_clock.h
+generic-y += types.h
 generic-y += word-at-a-time.h
diff --git a/arch/sparc/include/asm/cputime.h b/arch/sparc/include/asm/cputime.h
deleted file mode 100644
index 1a642b81e01..00000000000
--- a/arch/sparc/include/asm/cputime.h
+++ /dev/null
@@ -1,6 +0,0 @@
-#ifndef __SPARC_CPUTIME_H
-#define __SPARC_CPUTIME_H
-
-#include <asm-generic/cputime.h>
-
-#endif /* __SPARC_CPUTIME_H */
diff --git a/arch/sparc/include/asm/emergency-restart.h b/arch/sparc/include/asm/emergency-restart.h
deleted file mode 100644
index 108d8c48e42..00000000000
--- a/arch/sparc/include/asm/emergency-restart.h
+++ /dev/null
@@ -1,6 +0,0 @@
-#ifndef _ASM_EMERGENCY_RESTART_H
-#define _ASM_EMERGENCY_RESTART_H
-
-#include <asm-generic/emergency-restart.h>
-
-#endif /* _ASM_EMERGENCY_RESTART_H */
diff --git a/arch/sparc/include/asm/hugetlb.h b/arch/sparc/include/asm/hugetlb.h
index 7eb57d24504..e4cab465b81 100644
--- a/arch/sparc/include/asm/hugetlb.h
+++ b/arch/sparc/include/asm/hugetlb.h
@@ -2,6 +2,7 @@
 #define _ASM_SPARC64_HUGETLB_H
 
 #include <asm/page.h>
+#include <asm-generic/hugetlb.h>
 
 
 void set_huge_pte_at(struct mm_struct *mm, unsigned long addr,
diff --git a/arch/sparc/include/asm/mutex.h b/arch/sparc/include/asm/mutex.h
deleted file mode 100644
index 458c1f7fbc1..00000000000
--- a/arch/sparc/include/asm/mutex.h
+++ /dev/null
@@ -1,9 +0,0 @@
-/*
- * Pull in the generic implementation for the mutex fastpath.
- *
- * TODO: implement optimized primitives instead, or leave the generic
- * implementation in place, or pick the atomic_xchg() based generic
- * implementation. (see asm-generic/mutex-xchg.h for details)
- */
-
-#include <asm-generic/mutex-dec.h>
diff --git a/arch/sparc/include/asm/pgtable_64.h b/arch/sparc/include/asm/pgtable_64.h
index 08fcce90316..7619f2f792a 100644
--- a/arch/sparc/include/asm/pgtable_64.h
+++ b/arch/sparc/include/asm/pgtable_64.h
@@ -915,6 +915,7 @@ static inline int io_remap_pfn_range(struct vm_area_struct *vma,
 	return remap_pfn_range(vma, from, phys_base >> PAGE_SHIFT, size, prot);
 }
 
+#include <asm/tlbflush.h>
 #include <asm-generic/pgtable.h>
 
 /* We provide our own get_unmapped_area to cope with VA holes and
diff --git a/arch/sparc/include/asm/serial.h b/arch/sparc/include/asm/serial.h
deleted file mode 100644
index f90d61c2805..00000000000
--- a/arch/sparc/include/asm/serial.h
+++ /dev/null
@@ -1,6 +0,0 @@
-#ifndef __SPARC_SERIAL_H
-#define __SPARC_SERIAL_H
-
-#define BASE_BAUD ( 1843200 / 16 )
-
-#endif /* __SPARC_SERIAL_H */
diff --git a/arch/sparc/include/asm/smp_32.h b/arch/sparc/include/asm/smp_32.h
index b73da3c5f10..3c8917f054d 100644
--- a/arch/sparc/include/asm/smp_32.h
+++ b/arch/sparc/include/asm/smp_32.h
@@ -36,7 +36,6 @@ typedef void (*smpfunc_t)(unsigned long, unsigned long, unsigned long,
 		       unsigned long, unsigned long);
 
 void cpu_panic(void);
-extern void smp4m_irq_rotate(int cpu);
 
 /*
  *	General functions that each host system must provide.
@@ -46,7 +45,6 @@ void sun4m_init_smp(void);
 void sun4d_init_smp(void);
 
 void smp_callin(void);
-void smp_boot_cpus(void);
 void smp_store_cpu_info(int);
 
 void smp_resched_interrupt(void);
@@ -107,9 +105,6 @@ extern int hard_smp_processor_id(void);
 
 #define raw_smp_processor_id()		(current_thread_info()->cpu)
 
-#define prof_multiplier(__cpu)		cpu_data(__cpu).multiplier
-#define prof_counter(__cpu)		cpu_data(__cpu).counter
-
 void smp_setup_cpu_possible_map(void);
 
 #endif /* !(__ASSEMBLY__) */
diff --git a/arch/sparc/include/asm/switch_to_64.h b/arch/sparc/include/asm/switch_to_64.h
index cad36f56fa0..c7de3323819 100644
--- a/arch/sparc/include/asm/switch_to_64.h
+++ b/arch/sparc/include/asm/switch_to_64.h
@@ -18,8 +18,7 @@ do {						\
 	 * and 2 stores in this critical code path.  -DaveM
 	 */
 #define switch_to(prev, next, last)					\
-do {	flush_tlb_pending();						\
-	save_and_clear_fpu();						\
+do {	save_and_clear_fpu();						\
 	/* If you are tempted to conditionalize the following */	\
 	/* so that ASI is only written if it changes, think again. */	\
 	__asm__ __volatile__("wr %%g0, %0, %%asi"			\
diff --git a/arch/sparc/include/asm/thread_info_32.h b/arch/sparc/include/asm/thread_info_32.h
index 25849ae3e90..dd3807599bb 100644
--- a/arch/sparc/include/asm/thread_info_32.h
+++ b/arch/sparc/include/asm/thread_info_32.h
@@ -132,8 +132,6 @@ register struct thread_info *current_thread_info_reg asm("g6");
 #define _TIF_DO_NOTIFY_RESUME_MASK	(_TIF_NOTIFY_RESUME | \
 					 _TIF_SIGPENDING)
 
-#define tsk_is_polling(t) test_tsk_thread_flag(t, TIF_POLLING_NRFLAG)
-
 #endif /* __KERNEL__ */
 
 #endif /* _ASM_THREAD_INFO_H */
diff --git a/arch/sparc/include/asm/thread_info_64.h b/arch/sparc/include/asm/thread_info_64.h
index 269bd92313d..d5e50425107 100644
--- a/arch/sparc/include/asm/thread_info_64.h
+++ b/arch/sparc/include/asm/thread_info_64.h
@@ -256,8 +256,6 @@ static inline bool test_and_clear_restore_sigmask(void)
 	return true;
 }
 
-#define tsk_is_polling(t) test_tsk_thread_flag(t, TIF_POLLING_NRFLAG)
-
 #define thread32_stack_is_64bit(__SP) (((__SP) & 0x1) != 0)
 #define test_thread_64bit_stack(__SP) \
 	((test_thread_flag(TIF_32BIT) && !thread32_stack_is_64bit(__SP)) ? \
diff --git a/arch/sparc/include/asm/tlbflush_64.h b/arch/sparc/include/asm/tlbflush_64.h
index 2ef46349415..f0d6a9700f4 100644
--- a/arch/sparc/include/asm/tlbflush_64.h
+++ b/arch/sparc/include/asm/tlbflush_64.h
@@ -11,24 +11,40 @@
 struct tlb_batch {
 	struct mm_struct *mm;
 	unsigned long tlb_nr;
+	unsigned long active;
 	unsigned long vaddrs[TLB_BATCH_NR];
 };
 
 extern void flush_tsb_kernel_range(unsigned long start, unsigned long end);
 extern void flush_tsb_user(struct tlb_batch *tb);
+extern void flush_tsb_user_page(struct mm_struct *mm, unsigned long vaddr);
 
 /* TLB flush operations. */
 
-extern void flush_tlb_pending(void);
+static inline void flush_tlb_mm(struct mm_struct *mm)
+{
+}
+
+static inline void flush_tlb_page(struct vm_area_struct *vma,
+				  unsigned long vmaddr)
+{
+}
+
+static inline void flush_tlb_range(struct vm_area_struct *vma,
+				   unsigned long start, unsigned long end)
+{
+}
+
+#define __HAVE_ARCH_ENTER_LAZY_MMU_MODE
 
-#define flush_tlb_range(vma,start,end)	\
-	do { (void)(start); flush_tlb_pending(); } while (0)
-#define flush_tlb_page(vma,addr)	flush_tlb_pending()
-#define flush_tlb_mm(mm)		flush_tlb_pending()
+extern void flush_tlb_pending(void);
+extern void arch_enter_lazy_mmu_mode(void);
+extern void arch_leave_lazy_mmu_mode(void);
+#define arch_flush_lazy_mmu_mode()      do {} while (0)
 
 /* Local cpu only.  */
 extern void __flush_tlb_all(void);
-
+extern void __flush_tlb_page(unsigned long context, unsigned long vaddr);
 extern void __flush_tlb_kernel_range(unsigned long start, unsigned long end);
 
 #ifndef CONFIG_SMP
@@ -38,15 +54,24 @@ do {	flush_tsb_kernel_range(start,end); \
 	__flush_tlb_kernel_range(start,end); \
 } while (0)
 
+static inline void global_flush_tlb_page(struct mm_struct *mm, unsigned long vaddr)
+{
+	__flush_tlb_page(CTX_HWBITS(mm->context), vaddr);
+}
+
 #else /* CONFIG_SMP */
 
 extern void smp_flush_tlb_kernel_range(unsigned long start, unsigned long end);
+extern void smp_flush_tlb_page(struct mm_struct *mm, unsigned long vaddr);
 
 #define flush_tlb_kernel_range(start, end) \
 do {	flush_tsb_kernel_range(start,end); \
 	smp_flush_tlb_kernel_range(start, end); \
 } while (0)
 
+#define global_flush_tlb_page(mm, vaddr) \
+	smp_flush_tlb_page(mm, vaddr)
+
 #endif /* ! CONFIG_SMP */
 
 #endif /* _SPARC64_TLBFLUSH_H */
diff --git a/arch/sparc/include/uapi/asm/Kbuild b/arch/sparc/include/uapi/asm/Kbuild
index ce175aff71b..b5843ee09fb 100644
--- a/arch/sparc/include/uapi/asm/Kbuild
+++ b/arch/sparc/include/uapi/asm/Kbuild
@@ -44,7 +44,6 @@ header-y += swab.h
 header-y += termbits.h
 header-y += termios.h
 header-y += traps.h
-header-y += types.h
 header-y += uctx.h
 header-y += unistd.h
 header-y += utrap.h
diff --git a/arch/sparc/include/uapi/asm/types.h b/arch/sparc/include/uapi/asm/types.h
deleted file mode 100644
index 383d156cde9..00000000000
--- a/arch/sparc/include/uapi/asm/types.h
+++ /dev/null
@@ -1,17 +0,0 @@
-#ifndef _SPARC_TYPES_H
-#define _SPARC_TYPES_H
-/*
- * This file is never included by application software unless
- * explicitly requested (e.g., via linux/types.h) in which case the
- * application is Linux specific so (user-) name space pollution is
- * not a major issue.  However, for interoperability, libraries still
- * need to be careful to avoid a name clashes.
- */
-
-#if defined(__sparc__)
-
-#include <asm-generic/int-ll64.h>
-
-#endif /* defined(__sparc__) */
-
-#endif /* defined(_SPARC_TYPES_H) */
diff --git a/arch/sparc/kernel/hvtramp.S b/arch/sparc/kernel/hvtramp.S
index 9365432904d..605c960b2fa 100644
--- a/arch/sparc/kernel/hvtramp.S
+++ b/arch/sparc/kernel/hvtramp.S
@@ -128,8 +128,7 @@ hv_cpu_startup:
 
 	call		smp_callin
 	 nop
-	call		cpu_idle
-	 mov		0, %o0
+
 	call		cpu_panic
 	 nop
 
diff --git a/arch/sparc/kernel/process_32.c b/arch/sparc/kernel/process_32.c
index 62eede13831..c85241006e3 100644
--- a/arch/sparc/kernel/process_32.c
+++ b/arch/sparc/kernel/process_32.c
@@ -64,23 +64,12 @@ extern void fpsave(unsigned long *, unsigned long *, void *, unsigned long *);
 struct task_struct *last_task_used_math = NULL;
 struct thread_info *current_set[NR_CPUS];
 
-/*
- * the idle loop on a Sparc... ;)
- */
-void cpu_idle(void)
+/* Idle loop support. */
+void arch_cpu_idle(void)
 {
-	set_thread_flag(TIF_POLLING_NRFLAG);
-
-	/* endless idle loop with no priority at all */
-	for (;;) {
-		while (!need_resched()) {
-			if (sparc_idle)
-				(*sparc_idle)();
-			else
-				cpu_relax();
-		}
-		schedule_preempt_disabled();
-	}
+	if (sparc_idle)
+		(*sparc_idle)();
+	local_irq_enable();
 }
 
 /* XXX cli/sti -> local_irq_xxx here, check this works once SMP is fixed. */
diff --git a/arch/sparc/kernel/process_64.c b/arch/sparc/kernel/process_64.c
index cdb80b2adbe..9fbf0d14a36 100644
--- a/arch/sparc/kernel/process_64.c
+++ b/arch/sparc/kernel/process_64.c
@@ -52,20 +52,17 @@
 
 #include "kstack.h"
 
-static void sparc64_yield(int cpu)
+/* Idle loop support on sparc64. */
+void arch_cpu_idle(void)
 {
 	if (tlb_type != hypervisor) {
 		touch_nmi_watchdog();
-		return;
-	}
-
-	clear_thread_flag(TIF_POLLING_NRFLAG);
-	smp_mb__after_clear_bit();
-
-	while (!need_resched() && !cpu_is_offline(cpu)) {
+	} else {
 		unsigned long pstate;
 
-		/* Disable interrupts. */
+                /* The sun4v sleeping code requires that we have PSTATE.IE cleared over
+                 * the cpu sleep hypervisor call.
+                 */
 		__asm__ __volatile__(
 			"rdpr %%pstate, %0\n\t"
 			"andn %0, %1, %0\n\t"
@@ -73,7 +70,7 @@ static void sparc64_yield(int cpu)
 			: "=&r" (pstate)
 			: "i" (PSTATE_IE));
 
-		if (!need_resched() && !cpu_is_offline(cpu))
+		if (!need_resched() && !cpu_is_offline(smp_processor_id()))
 			sun4v_cpu_yield();
 
 		/* Re-enable interrupts. */
@@ -84,36 +81,16 @@ static void sparc64_yield(int cpu)
 			: "=&r" (pstate)
 			: "i" (PSTATE_IE));
 	}
-
-	set_thread_flag(TIF_POLLING_NRFLAG);
+	local_irq_enable();
 }
 
-/* The idle loop on sparc64. */
-void cpu_idle(void)
-{
-	int cpu = smp_processor_id();
-
-	set_thread_flag(TIF_POLLING_NRFLAG);
-
-	while(1) {
-		tick_nohz_idle_enter();
-		rcu_idle_enter();
-
-		while (!need_resched() && !cpu_is_offline(cpu))
-			sparc64_yield(cpu);
-
-		rcu_idle_exit();
-		tick_nohz_idle_exit();
-
 #ifdef CONFIG_HOTPLUG_CPU
-		if (cpu_is_offline(cpu)) {
-			sched_preempt_enable_no_resched();
-			cpu_play_dead();
-		}
-#endif
-		schedule_preempt_disabled();
-	}
+void arch_cpu_idle_dead()
+{
+	sched_preempt_enable_no_resched();
+	cpu_play_dead();
 }
+#endif
 
 #ifdef CONFIG_COMPAT
 static void show_regwindow32(struct pt_regs *regs)
diff --git a/arch/sparc/kernel/smp_32.c b/arch/sparc/kernel/smp_32.c
index 9e7e6d71836..e3f2b81c23f 100644
--- a/arch/sparc/kernel/smp_32.c
+++ b/arch/sparc/kernel/smp_32.c
@@ -369,7 +369,7 @@ void __cpuinit sparc_start_secondary(void *arg)
 	local_irq_enable();
 
 	wmb();
-	cpu_idle();
+	cpu_startup_entry(CPUHP_ONLINE);
 
 	/* We should never reach here! */
 	BUG();
diff --git a/arch/sparc/kernel/smp_64.c b/arch/sparc/kernel/smp_64.c
index 537eb66abd0..77539eda928 100644
--- a/arch/sparc/kernel/smp_64.c
+++ b/arch/sparc/kernel/smp_64.c
@@ -127,6 +127,8 @@ void __cpuinit smp_callin(void)
 
 	/* idle thread is expected to have preempt disabled */
 	preempt_disable();
+
+	cpu_startup_entry(CPUHP_ONLINE);
 }
 
 void cpu_panic(void)
@@ -849,7 +851,7 @@ void smp_tsb_sync(struct mm_struct *mm)
 }
 
 extern unsigned long xcall_flush_tlb_mm;
-extern unsigned long xcall_flush_tlb_pending;
+extern unsigned long xcall_flush_tlb_page;
 extern unsigned long xcall_flush_tlb_kernel_range;
 extern unsigned long xcall_fetch_glob_regs;
 extern unsigned long xcall_fetch_glob_pmu;
@@ -1074,23 +1076,56 @@ local_flush_and_out:
 	put_cpu();
 }
 
+struct tlb_pending_info {
+	unsigned long ctx;
+	unsigned long nr;
+	unsigned long *vaddrs;
+};
+
+static void tlb_pending_func(void *info)
+{
+	struct tlb_pending_info *t = info;
+
+	__flush_tlb_pending(t->ctx, t->nr, t->vaddrs);
+}
+
 void smp_flush_tlb_pending(struct mm_struct *mm, unsigned long nr, unsigned long *vaddrs)
 {
 	u32 ctx = CTX_HWBITS(mm->context);
+	struct tlb_pending_info info;
 	int cpu = get_cpu();
 
+	info.ctx = ctx;
+	info.nr = nr;
+	info.vaddrs = vaddrs;
+
 	if (mm == current->mm && atomic_read(&mm->mm_users) == 1)
 		cpumask_copy(mm_cpumask(mm), cpumask_of(cpu));
 	else
-		smp_cross_call_masked(&xcall_flush_tlb_pending,
-				      ctx, nr, (unsigned long) vaddrs,
-				      mm_cpumask(mm));
+		smp_call_function_many(mm_cpumask(mm), tlb_pending_func,
+				       &info, 1);
 
 	__flush_tlb_pending(ctx, nr, vaddrs);
 
 	put_cpu();
 }
 
+void smp_flush_tlb_page(struct mm_struct *mm, unsigned long vaddr)
+{
+	unsigned long context = CTX_HWBITS(mm->context);
+	int cpu = get_cpu();
+
+	if (mm == current->mm && atomic_read(&mm->mm_users) == 1)
+		cpumask_copy(mm_cpumask(mm), cpumask_of(cpu));
+	else
+		smp_cross_call_masked(&xcall_flush_tlb_page,
+				      context, vaddr, 0,
+				      mm_cpumask(mm));
+	__flush_tlb_page(context, vaddr);
+
+	put_cpu();
+}
+
 void smp_flush_tlb_kernel_range(unsigned long start, unsigned long end)
 {
 	start &= PAGE_MASK;
diff --git a/arch/sparc/kernel/trampoline_64.S b/arch/sparc/kernel/trampoline_64.S
index da1b781b5e6..2e973a26fbd 100644
--- a/arch/sparc/kernel/trampoline_64.S
+++ b/arch/sparc/kernel/trampoline_64.S
@@ -407,8 +407,7 @@ after_lock_tlb:
 
 	call		smp_callin
 	 nop
-	call		cpu_idle
-	 mov		0, %o0
+
 	call		cpu_panic
 	 nop
 1:	b,a,pt		%xcc, 1b
diff --git a/arch/sparc/lib/bitext.c b/arch/sparc/lib/bitext.c
index 48d00e72ce1..8ec4e9c0251 100644
--- a/arch/sparc/lib/bitext.c
+++ b/arch/sparc/lib/bitext.c
@@ -119,11 +119,7 @@ void bit_map_clear(struct bit_map *t, int offset, int len)
 
 void bit_map_init(struct bit_map *t, unsigned long *map, int size)
 {
-
-	if ((size & 07) != 0)
-		BUG();
-	memset(map, 0, size>>3);
-
+	bitmap_zero(map, size);
 	memset(t, 0, sizeof *t);
 	spin_lock_init(&t->lock);
 	t->map = map;
diff --git a/arch/sparc/mm/init_32.c b/arch/sparc/mm/init_32.c
index 48e0c030e8f..4490c397bb5 100644
--- a/arch/sparc/mm/init_32.c
+++ b/arch/sparc/mm/init_32.c
@@ -282,14 +282,8 @@ static void map_high_region(unsigned long start_pfn, unsigned long end_pfn)
 	printk("mapping high region %08lx - %08lx\n", start_pfn, end_pfn);
 #endif
 
-	for (tmp = start_pfn; tmp < end_pfn; tmp++) {
-		struct page *page = pfn_to_page(tmp);
-
-		ClearPageReserved(page);
-		init_page_count(page);
-		__free_page(page);
-		totalhigh_pages++;
-	}
+	for (tmp = start_pfn; tmp < end_pfn; tmp++)
+		free_highmem_page(pfn_to_page(tmp));
 }
 
 void __init mem_init(void)
@@ -347,8 +341,6 @@ void __init mem_init(void)
 		map_high_region(start_pfn, end_pfn);
 	}
 	
-	totalram_pages += totalhigh_pages;
-
 	codepages = (((unsigned long) &_etext) - ((unsigned long)&_start));
 	codepages = PAGE_ALIGN(codepages) >> PAGE_SHIFT;
 	datapages = (((unsigned long) &_edata) - ((unsigned long)&_etext));
diff --git a/arch/sparc/mm/init_64.c b/arch/sparc/mm/init_64.c
index 1588d33d549..6ac99d64a13 100644
--- a/arch/sparc/mm/init_64.c
+++ b/arch/sparc/mm/init_64.c
@@ -2181,10 +2181,9 @@ unsigned long vmemmap_table[VMEMMAP_SIZE];
 static long __meminitdata addr_start, addr_end;
 static int __meminitdata node_start;
 
-int __meminit vmemmap_populate(struct page *start, unsigned long nr, int node)
+int __meminit vmemmap_populate(unsigned long vstart, unsigned long vend,
+			       int node)
 {
-	unsigned long vstart = (unsigned long) start;
-	unsigned long vend = (unsigned long) (start + nr);
 	unsigned long phys_start = (vstart - VMEMMAP_BASE);
 	unsigned long phys_end = (vend - VMEMMAP_BASE);
 	unsigned long addr = phys_start & VMEMMAP_CHUNK_MASK;
@@ -2236,7 +2235,7 @@ void __meminit vmemmap_populate_print_last(void)
 	}
 }
 
-void vmemmap_free(struct page *memmap, unsigned long nr_pages)
+void vmemmap_free(unsigned long start, unsigned long end)
 {
 }
 
diff --git a/arch/sparc/mm/iommu.c b/arch/sparc/mm/iommu.c
index 0f4f7191fbb..28f96f27c76 100644
--- a/arch/sparc/mm/iommu.c
+++ b/arch/sparc/mm/iommu.c
@@ -34,7 +34,7 @@
 #define IOMMU_RNGE	IOMMU_RNGE_256MB
 #define IOMMU_START	0xF0000000
 #define IOMMU_WINSIZE	(256*1024*1024U)
-#define IOMMU_NPTES	(IOMMU_WINSIZE/PAGE_SIZE)	/* 64K PTEs, 265KB */
+#define IOMMU_NPTES	(IOMMU_WINSIZE/PAGE_SIZE)	/* 64K PTEs, 256KB */
 #define IOMMU_ORDER	6				/* 4096 * (1<<6) */
 
 /* srmmu.c */
diff --git a/arch/sparc/mm/srmmu.c b/arch/sparc/mm/srmmu.c
index c38bb72e3e8..036c2797dec 100644
--- a/arch/sparc/mm/srmmu.c
+++ b/arch/sparc/mm/srmmu.c
@@ -280,7 +280,9 @@ static void __init srmmu_nocache_init(void)
 		SRMMU_NOCACHE_ALIGN_MAX, 0UL);
 	memset(srmmu_nocache_pool, 0, srmmu_nocache_size);
 
-	srmmu_nocache_bitmap = __alloc_bootmem(bitmap_bits >> 3, SMP_CACHE_BYTES, 0UL);
+	srmmu_nocache_bitmap =
+		__alloc_bootmem(BITS_TO_LONGS(bitmap_bits) * sizeof(long),
+				SMP_CACHE_BYTES, 0UL);
 	bit_map_init(&srmmu_nocache_map, srmmu_nocache_bitmap, bitmap_bits);
 
 	srmmu_swapper_pg_dir = __srmmu_get_nocache(SRMMU_PGD_TABLE_SIZE, SRMMU_PGD_TABLE_SIZE);
diff --git a/arch/sparc/mm/tlb.c b/arch/sparc/mm/tlb.c
index ba6ae7ffdc2..83d89bcb44a 100644
--- a/arch/sparc/mm/tlb.c
+++ b/arch/sparc/mm/tlb.c
@@ -24,11 +24,17 @@ static DEFINE_PER_CPU(struct tlb_batch, tlb_batch);
 void flush_tlb_pending(void)
 {
 	struct tlb_batch *tb = &get_cpu_var(tlb_batch);
+	struct mm_struct *mm = tb->mm;
 
-	if (tb->tlb_nr) {
-		flush_tsb_user(tb);
+	if (!tb->tlb_nr)
+		goto out;
 
-		if (CTX_VALID(tb->mm->context)) {
+	flush_tsb_user(tb);
+
+	if (CTX_VALID(mm->context)) {
+		if (tb->tlb_nr == 1) {
+			global_flush_tlb_page(mm, tb->vaddrs[0]);
+		} else {
 #ifdef CONFIG_SMP
 			smp_flush_tlb_pending(tb->mm, tb->tlb_nr,
 					      &tb->vaddrs[0]);
@@ -37,12 +43,30 @@ void flush_tlb_pending(void)
 					    tb->tlb_nr, &tb->vaddrs[0]);
 #endif
 		}
-		tb->tlb_nr = 0;
 	}
 
+	tb->tlb_nr = 0;
+
+out:
 	put_cpu_var(tlb_batch);
 }
 
+void arch_enter_lazy_mmu_mode(void)
+{
+	struct tlb_batch *tb = &__get_cpu_var(tlb_batch);
+
+	tb->active = 1;
+}
+
+void arch_leave_lazy_mmu_mode(void)
+{
+	struct tlb_batch *tb = &__get_cpu_var(tlb_batch);
+
+	if (tb->tlb_nr)
+		flush_tlb_pending();
+	tb->active = 0;
+}
+
 static void tlb_batch_add_one(struct mm_struct *mm, unsigned long vaddr,
 			      bool exec)
 {
@@ -60,6 +84,12 @@ static void tlb_batch_add_one(struct mm_struct *mm, unsigned long vaddr,
 		nr = 0;
 	}
 
+	if (!tb->active) {
+		global_flush_tlb_page(mm, vaddr);
+		flush_tsb_user_page(mm, vaddr);
+		goto out;
+	}
+
 	if (nr == 0)
 		tb->mm = mm;
 
@@ -68,6 +98,7 @@ static void tlb_batch_add_one(struct mm_struct *mm, unsigned long vaddr,
 	if (nr >= TLB_BATCH_NR)
 		flush_tlb_pending();
 
+out:
 	put_cpu_var(tlb_batch);
 }
 
diff --git a/arch/sparc/mm/tsb.c b/arch/sparc/mm/tsb.c
index 428982b9bec..2cc3bce5ee9 100644
--- a/arch/sparc/mm/tsb.c
+++ b/arch/sparc/mm/tsb.c
@@ -7,11 +7,10 @@
 #include <linux/preempt.h>
 #include <linux/slab.h>
 #include <asm/page.h>
-#include <asm/tlbflush.h>
-#include <asm/tlb.h>
-#include <asm/mmu_context.h>
 #include <asm/pgtable.h>
+#include <asm/mmu_context.h>
 #include <asm/tsb.h>
+#include <asm/tlb.h>
 #include <asm/oplib.h>
 
 extern struct tsb swapper_tsb[KERNEL_TSB_NENTRIES];
@@ -46,23 +45,27 @@ void flush_tsb_kernel_range(unsigned long start, unsigned long end)
 	}
 }
 
-static void __flush_tsb_one(struct tlb_batch *tb, unsigned long hash_shift,
-			    unsigned long tsb, unsigned long nentries)
+static void __flush_tsb_one_entry(unsigned long tsb, unsigned long v,
+				  unsigned long hash_shift,
+				  unsigned long nentries)
 {
-	unsigned long i;
+	unsigned long tag, ent, hash;
 
-	for (i = 0; i < tb->tlb_nr; i++) {
-		unsigned long v = tb->vaddrs[i];
-		unsigned long tag, ent, hash;
+	v &= ~0x1UL;
+	hash = tsb_hash(v, hash_shift, nentries);
+	ent = tsb + (hash * sizeof(struct tsb));
+	tag = (v >> 22UL);
 
-		v &= ~0x1UL;
+	tsb_flush(ent, tag);
+}
 
-		hash = tsb_hash(v, hash_shift, nentries);
-		ent = tsb + (hash * sizeof(struct tsb));
-		tag = (v >> 22UL);
+static void __flush_tsb_one(struct tlb_batch *tb, unsigned long hash_shift,
+			    unsigned long tsb, unsigned long nentries)
+{
+	unsigned long i;
 
-		tsb_flush(ent, tag);
-	}
+	for (i = 0; i < tb->tlb_nr; i++)
+		__flush_tsb_one_entry(tsb, tb->vaddrs[i], hash_shift, nentries);
 }
 
 void flush_tsb_user(struct tlb_batch *tb)
@@ -90,6 +93,30 @@ void flush_tsb_user(struct tlb_batch *tb)
 	spin_unlock_irqrestore(&mm->context.lock, flags);
 }
 
+void flush_tsb_user_page(struct mm_struct *mm, unsigned long vaddr)
+{
+	unsigned long nentries, base, flags;
+
+	spin_lock_irqsave(&mm->context.lock, flags);
+
+	base = (unsigned long) mm->context.tsb_block[MM_TSB_BASE].tsb;
+	nentries = mm->context.tsb_block[MM_TSB_BASE].tsb_nentries;
+	if (tlb_type == cheetah_plus || tlb_type == hypervisor)
+		base = __pa(base);
+	__flush_tsb_one_entry(base, vaddr, PAGE_SHIFT, nentries);
+
+#if defined(CONFIG_HUGETLB_PAGE) || defined(CONFIG_TRANSPARENT_HUGEPAGE)
+	if (mm->context.tsb_block[MM_TSB_HUGE].tsb) {
+		base = (unsigned long) mm->context.tsb_block[MM_TSB_HUGE].tsb;
+		nentries = mm->context.tsb_block[MM_TSB_HUGE].tsb_nentries;
+		if (tlb_type == cheetah_plus || tlb_type == hypervisor)
+			base = __pa(base);
+		__flush_tsb_one_entry(base, vaddr, HPAGE_SHIFT, nentries);
+	}
+#endif
+	spin_unlock_irqrestore(&mm->context.lock, flags);
+}
+
 #define HV_PGSZ_IDX_BASE	HV_PGSZ_IDX_8K
 #define HV_PGSZ_MASK_BASE	HV_PGSZ_MASK_8K
 
diff --git a/arch/sparc/mm/ultra.S b/arch/sparc/mm/ultra.S
index f8e13d421fc..432aa0cb1b3 100644
--- a/arch/sparc/mm/ultra.S
+++ b/arch/sparc/mm/ultra.S
@@ -53,6 +53,33 @@ __flush_tlb_mm:		/* 18 insns */
 	nop
 
 	.align		32
+	.globl		__flush_tlb_page
+__flush_tlb_page:	/* 22 insns */
+	/* %o0 = context, %o1 = vaddr */
+	rdpr		%pstate, %g7
+	andn		%g7, PSTATE_IE, %g2
+	wrpr		%g2, %pstate
+	mov		SECONDARY_CONTEXT, %o4
+	ldxa		[%o4] ASI_DMMU, %g2
+	stxa		%o0, [%o4] ASI_DMMU
+	andcc		%o1, 1, %g0
+	andn		%o1, 1, %o3
+	be,pn		%icc, 1f
+	 or		%o3, 0x10, %o3
+	stxa		%g0, [%o3] ASI_IMMU_DEMAP
+1:	stxa		%g0, [%o3] ASI_DMMU_DEMAP
+	membar		#Sync
+	stxa		%g2, [%o4] ASI_DMMU
+	sethi		%hi(KERNBASE), %o4
+	flush		%o4
+	retl
+	 wrpr		%g7, 0x0, %pstate
+	nop
+	nop
+	nop
+	nop
+
+	.align		32
 	.globl		__flush_tlb_pending
 __flush_tlb_pending:	/* 26 insns */
 	/* %o0 = context, %o1 = nr, %o2 = vaddrs[] */
@@ -203,6 +230,31 @@ __cheetah_flush_tlb_mm: /* 19 insns */
 	retl
 	 wrpr		%g7, 0x0, %pstate
 
+__cheetah_flush_tlb_page:	/* 22 insns */
+	/* %o0 = context, %o1 = vaddr */
+	rdpr		%pstate, %g7
+	andn		%g7, PSTATE_IE, %g2
+	wrpr		%g2, 0x0, %pstate
+	wrpr		%g0, 1, %tl
+	mov		PRIMARY_CONTEXT, %o4
+	ldxa		[%o4] ASI_DMMU, %g2
+	srlx		%g2, CTX_PGSZ1_NUC_SHIFT, %o3
+	sllx		%o3, CTX_PGSZ1_NUC_SHIFT, %o3
+	or		%o0, %o3, %o0	/* Preserve nucleus page size fields */
+	stxa		%o0, [%o4] ASI_DMMU
+	andcc		%o1, 1, %g0
+	be,pn		%icc, 1f
+	 andn		%o1, 1, %o3
+	stxa		%g0, [%o3] ASI_IMMU_DEMAP
+1:	stxa		%g0, [%o3] ASI_DMMU_DEMAP	
+	membar		#Sync
+	stxa		%g2, [%o4] ASI_DMMU
+	sethi		%hi(KERNBASE), %o4
+	flush		%o4
+	wrpr		%g0, 0, %tl
+	retl
+	 wrpr		%g7, 0x0, %pstate
+
 __cheetah_flush_tlb_pending:	/* 27 insns */
 	/* %o0 = context, %o1 = nr, %o2 = vaddrs[] */
 	rdpr		%pstate, %g7
@@ -269,6 +321,20 @@ __hypervisor_flush_tlb_mm: /* 10 insns */
 	retl
 	 nop
 
+__hypervisor_flush_tlb_page: /* 11 insns */
+	/* %o0 = context, %o1 = vaddr */
+	mov		%o0, %g2
+	mov		%o1, %o0              /* ARG0: vaddr + IMMU-bit */
+	mov		%g2, %o1	      /* ARG1: mmu context */
+	mov		HV_MMU_ALL, %o2	      /* ARG2: flags */
+	srlx		%o0, PAGE_SHIFT, %o0
+	sllx		%o0, PAGE_SHIFT, %o0
+	ta		HV_MMU_UNMAP_ADDR_TRAP
+	brnz,pn		%o0, __hypervisor_tlb_tl0_error
+	 mov		HV_MMU_UNMAP_ADDR_TRAP, %o1
+	retl
+	 nop
+
 __hypervisor_flush_tlb_pending: /* 16 insns */
 	/* %o0 = context, %o1 = nr, %o2 = vaddrs[] */
 	sllx		%o1, 3, %g1
@@ -339,6 +405,13 @@ cheetah_patch_cachetlbops:
 	call		tlb_patch_one
 	 mov		19, %o2
 
+	sethi		%hi(__flush_tlb_page), %o0
+	or		%o0, %lo(__flush_tlb_page), %o0
+	sethi		%hi(__cheetah_flush_tlb_page), %o1
+	or		%o1, %lo(__cheetah_flush_tlb_page), %o1
+	call		tlb_patch_one
+	 mov		22, %o2
+
 	sethi		%hi(__flush_tlb_pending), %o0
 	or		%o0, %lo(__flush_tlb_pending), %o0
 	sethi		%hi(__cheetah_flush_tlb_pending), %o1
@@ -397,10 +470,9 @@ xcall_flush_tlb_mm:	/* 21 insns */
 	nop
 	nop
 
-	.globl		xcall_flush_tlb_pending
-xcall_flush_tlb_pending:	/* 21 insns */
-	/* %g5=context, %g1=nr, %g7=vaddrs[] */
-	sllx		%g1, 3, %g1
+	.globl		xcall_flush_tlb_page
+xcall_flush_tlb_page:	/* 17 insns */
+	/* %g5=context, %g1=vaddr */
 	mov		PRIMARY_CONTEXT, %g4
 	ldxa		[%g4] ASI_DMMU, %g2
 	srlx		%g2, CTX_PGSZ1_NUC_SHIFT, %g4
@@ -408,20 +480,16 @@ xcall_flush_tlb_pending:	/* 21 insns */
 	or		%g5, %g4, %g5
 	mov		PRIMARY_CONTEXT, %g4
 	stxa		%g5, [%g4] ASI_DMMU
-1:	sub		%g1, (1 << 3), %g1
-	ldx		[%g7 + %g1], %g5
-	andcc		%g5, 0x1, %g0
+	andcc		%g1, 0x1, %g0
 	be,pn		%icc, 2f
-
-	 andn		%g5, 0x1, %g5
+	 andn		%g1, 0x1, %g5
 	stxa		%g0, [%g5] ASI_IMMU_DEMAP
 2:	stxa		%g0, [%g5] ASI_DMMU_DEMAP
 	membar		#Sync
-	brnz,pt		%g1, 1b
-	 nop
 	stxa		%g2, [%g4] ASI_DMMU
 	retry
 	nop
+	nop
 
 	.globl		xcall_flush_tlb_kernel_range
 xcall_flush_tlb_kernel_range:	/* 25 insns */
@@ -656,15 +724,13 @@ __hypervisor_xcall_flush_tlb_mm: /* 21 insns */
 	membar		#Sync
 	retry
 
-	.globl		__hypervisor_xcall_flush_tlb_pending
-__hypervisor_xcall_flush_tlb_pending: /* 21 insns */
-	/* %g5=ctx, %g1=nr, %g7=vaddrs[], %g2,%g3,%g4,g6=scratch */
-	sllx		%g1, 3, %g1
+	.globl		__hypervisor_xcall_flush_tlb_page
+__hypervisor_xcall_flush_tlb_page: /* 17 insns */
+	/* %g5=ctx, %g1=vaddr */
 	mov		%o0, %g2
 	mov		%o1, %g3
 	mov		%o2, %g4
-1:	sub		%g1, (1 << 3), %g1
-	ldx		[%g7 + %g1], %o0	/* ARG0: virtual address */
+	mov		%g1, %o0	        /* ARG0: virtual address */
 	mov		%g5, %o1		/* ARG1: mmu context */
 	mov		HV_MMU_ALL, %o2		/* ARG2: flags */
 	srlx		%o0, PAGE_SHIFT, %o0
@@ -673,8 +739,6 @@ __hypervisor_xcall_flush_tlb_pending: /* 21 insns */
 	mov		HV_MMU_UNMAP_ADDR_TRAP, %g6
 	brnz,a,pn	%o0, __hypervisor_tlb_xcall_error
 	 mov		%o0, %g5
-	brnz,pt		%g1, 1b
-	 nop
 	mov		%g2, %o0
 	mov		%g3, %o1
 	mov		%g4, %o2
@@ -757,6 +821,13 @@ hypervisor_patch_cachetlbops:
 	call		tlb_patch_one
 	 mov		10, %o2
 
+	sethi		%hi(__flush_tlb_page), %o0
+	or		%o0, %lo(__flush_tlb_page), %o0
+	sethi		%hi(__hypervisor_flush_tlb_page), %o1
+	or		%o1, %lo(__hypervisor_flush_tlb_page), %o1
+	call		tlb_patch_one
+	 mov		11, %o2
+
 	sethi		%hi(__flush_tlb_pending), %o0
 	or		%o0, %lo(__flush_tlb_pending), %o0
 	sethi		%hi(__hypervisor_flush_tlb_pending), %o1
@@ -788,12 +859,12 @@ hypervisor_patch_cachetlbops:
 	call		tlb_patch_one
 	 mov		21, %o2
 
-	sethi		%hi(xcall_flush_tlb_pending), %o0
-	or		%o0, %lo(xcall_flush_tlb_pending), %o0
-	sethi		%hi(__hypervisor_xcall_flush_tlb_pending), %o1
-	or		%o1, %lo(__hypervisor_xcall_flush_tlb_pending), %o1
+	sethi		%hi(xcall_flush_tlb_page), %o0
+	or		%o0, %lo(xcall_flush_tlb_page), %o0
+	sethi		%hi(__hypervisor_xcall_flush_tlb_page), %o1
+	or		%o1, %lo(__hypervisor_xcall_flush_tlb_page), %o1
 	call		tlb_patch_one
-	 mov		21, %o2
+	 mov		17, %o2
 
 	sethi		%hi(xcall_flush_tlb_kernel_range), %o0
 	or		%o0, %lo(xcall_flush_tlb_kernel_range), %o0
diff --git a/arch/tile/include/asm/hugetlb.h b/arch/tile/include/asm/hugetlb.h
index 0f885af2b62..3257733003f 100644
--- a/arch/tile/include/asm/hugetlb.h
+++ b/arch/tile/include/asm/hugetlb.h
@@ -16,6 +16,7 @@
 #define _ASM_TILE_HUGETLB_H
 
 #include <asm/page.h>
+#include <asm-generic/hugetlb.h>
 
 
 static inline int is_hugepage_only_range(struct mm_struct *mm,
diff --git a/arch/tile/include/asm/irqflags.h b/arch/tile/include/asm/irqflags.h
index 241c0bb60b1..c96f9bbb760 100644
--- a/arch/tile/include/asm/irqflags.h
+++ b/arch/tile/include/asm/irqflags.h
@@ -40,7 +40,15 @@
 #include <asm/percpu.h>
 #include <arch/spr_def.h>
 
-/* Set and clear kernel interrupt masks. */
+/*
+ * Set and clear kernel interrupt masks.
+ *
+ * NOTE: __insn_mtspr() is a compiler builtin marked as a memory
+ * clobber.  We rely on it being equivalent to a compiler barrier in
+ * this code since arch_local_irq_save() and friends must act as
+ * compiler barriers.  This compiler semantic is baked into enough
+ * places that the compiler will maintain it going forward.
+ */
 #if CHIP_HAS_SPLIT_INTR_MASK()
 #if INT_PERF_COUNT < 32 || INT_AUX_PERF_COUNT < 32 || INT_MEM_ERROR >= 32
 # error Fix assumptions about which word various interrupts are in
diff --git a/arch/tile/include/asm/thread_info.h b/arch/tile/include/asm/thread_info.h
index e9c670d7a7f..ccc8ef37235 100644
--- a/arch/tile/include/asm/thread_info.h
+++ b/arch/tile/include/asm/thread_info.h
@@ -153,8 +153,6 @@ extern void _cpu_idle(void);
 #define TS_POLLING		0x0004	/* in idle loop but not sleeping */
 #define TS_RESTORE_SIGMASK	0x0008	/* restore signal mask in do_signal */
 
-#define tsk_is_polling(t) (task_thread_info(t)->status & TS_POLLING)
-
 #ifndef __ASSEMBLY__
 #define HAVE_SET_RESTORE_SIGMASK	1
 static inline void set_restore_sigmask(void)
diff --git a/arch/tile/kernel/early_printk.c b/arch/tile/kernel/early_printk.c
index afb9c9a0d88..34d72a151bf 100644
--- a/arch/tile/kernel/early_printk.c
+++ b/arch/tile/kernel/early_printk.c
@@ -17,6 +17,7 @@
 #include <linux/init.h>
 #include <linux/string.h>
 #include <linux/irqflags.h>
+#include <linux/printk.h>
 #include <asm/setup.h>
 #include <hv/hypervisor.h>
 
@@ -33,25 +34,8 @@ static struct console early_hv_console = {
 };
 
 /* Direct interface for emergencies */
-static struct console *early_console = &early_hv_console;
-static int early_console_initialized;
 static int early_console_complete;
 
-static void early_vprintk(const char *fmt, va_list ap)
-{
-	char buf[512];
-	int n = vscnprintf(buf, sizeof(buf), fmt, ap);
-	early_console->write(early_console, buf, n);
-}
-
-void early_printk(const char *fmt, ...)
-{
-	va_list ap;
-	va_start(ap, fmt);
-	early_vprintk(fmt, ap);
-	va_end(ap);
-}
-
 void early_panic(const char *fmt, ...)
 {
 	va_list ap;
@@ -69,14 +53,13 @@ static int __initdata keep_early;
 
 static int __init setup_early_printk(char *str)
 {
-	if (early_console_initialized)
+	if (early_console)
 		return 1;
 
 	if (str != NULL && strncmp(str, "keep", 4) == 0)
 		keep_early = 1;
 
 	early_console = &early_hv_console;
-	early_console_initialized = 1;
 	register_console(early_console);
 
 	return 0;
@@ -85,12 +68,12 @@ static int __init setup_early_printk(char *str)
 void __init disable_early_printk(void)
 {
 	early_console_complete = 1;
-	if (!early_console_initialized || !early_console)
+	if (!early_console)
 		return;
 	if (!keep_early) {
 		early_printk("disabling early console\n");
 		unregister_console(early_console);
-		early_console_initialized = 0;
+		early_console = NULL;
 	} else {
 		early_printk("keeping early console\n");
 	}
@@ -98,7 +81,7 @@ void __init disable_early_printk(void)
 
 void warn_early_printk(void)
 {
-	if (early_console_complete || early_console_initialized)
+	if (early_console_complete || early_console)
 		return;
 	early_printk("\
 Machine shutting down before console output is fully initialized.\n\
diff --git a/arch/tile/kernel/process.c b/arch/tile/kernel/process.c
index caf93ae1179..80b2a18deb8 100644
--- a/arch/tile/kernel/process.c
+++ b/arch/tile/kernel/process.c
@@ -40,13 +40,11 @@
 #include <arch/abi.h>
 #include <arch/sim_def.h>
 
-
 /*
  * Use the (x86) "idle=poll" option to prefer low latency when leaving the
  * idle loop over low power while in the idle loop, e.g. if we have
  * one thread per core and we want to get threads out of futex waits fast.
  */
-static int no_idle_nap;
 static int __init idle_setup(char *str)
 {
 	if (!str)
@@ -54,64 +52,19 @@ static int __init idle_setup(char *str)
 
 	if (!strcmp(str, "poll")) {
 		pr_info("using polling idle threads.\n");
-		no_idle_nap = 1;
-	} else if (!strcmp(str, "halt"))
-		no_idle_nap = 0;
-	else
-		return -1;
-
-	return 0;
+		cpu_idle_poll_ctrl(true);
+		return 0;
+	} else if (!strcmp(str, "halt")) {
+		return 0;
+	}
+	return -1;
 }
 early_param("idle", idle_setup);
 
-/*
- * The idle thread. There's no useful work to be
- * done, so just try to conserve power and have a
- * low exit latency (ie sit in a loop waiting for
- * somebody to say that they'd like to reschedule)
- */
-void cpu_idle(void)
+void arch_cpu_idle(void)
 {
-	int cpu = smp_processor_id();
-
-
-	current_thread_info()->status |= TS_POLLING;
-
-	if (no_idle_nap) {
-		while (1) {
-			while (!need_resched())
-				cpu_relax();
-			schedule();
-		}
-	}
-
-	/* endless idle loop with no priority at all */
-	while (1) {
-		tick_nohz_idle_enter();
-		rcu_idle_enter();
-		while (!need_resched()) {
-			if (cpu_is_offline(cpu))
-				BUG();  /* no HOTPLUG_CPU */
-
-			local_irq_disable();
-			__get_cpu_var(irq_stat).idle_timestamp = jiffies;
-			current_thread_info()->status &= ~TS_POLLING;
-			/*
-			 * TS_POLLING-cleared state must be visible before we
-			 * test NEED_RESCHED:
-			 */
-			smp_mb();
-
-			if (!need_resched())
-				_cpu_idle();
-			else
-				local_irq_enable();
-			current_thread_info()->status |= TS_POLLING;
-		}
-		rcu_idle_exit();
-		tick_nohz_idle_exit();
-		schedule_preempt_disabled();
-	}
+	__get_cpu_var(irq_stat).idle_timestamp = jiffies;
+	_cpu_idle();
 }
 
 /*
diff --git a/arch/tile/kernel/setup.c b/arch/tile/kernel/setup.c
index d1e15f7b59c..7a5aa1a7864 100644
--- a/arch/tile/kernel/setup.c
+++ b/arch/tile/kernel/setup.c
@@ -1004,15 +1004,8 @@ void __cpuinit setup_cpu(int boot)
 
 #ifdef CONFIG_BLK_DEV_INITRD
 
-/*
- * Note that the kernel can potentially support other compression
- * techniques than gz, though we don't do so by default.  If we ever
- * decide to do so we can either look for other filename extensions,
- * or just allow a file with this name to be compressed with an
- * arbitrary compressor (somewhat counterintuitively).
- */
 static int __initdata set_initramfs_file;
-static char __initdata initramfs_file[128] = "initramfs.cpio.gz";
+static char __initdata initramfs_file[128] = "initramfs";
 
 static int __init setup_initramfs_file(char *str)
 {
@@ -1026,9 +1019,9 @@ static int __init setup_initramfs_file(char *str)
 early_param("initramfs_file", setup_initramfs_file);
 
 /*
- * We look for an "initramfs.cpio.gz" file in the hvfs.
- * If there is one, we allocate some memory for it and it will be
- * unpacked to the initramfs.
+ * We look for a file called "initramfs" in the hvfs.  If there is one, we
+ * allocate some memory for it and it will be unpacked to the initramfs.
+ * If it's compressed, the initd code will uncompress it first.
  */
 static void __init load_hv_initrd(void)
 {
@@ -1038,10 +1031,16 @@ static void __init load_hv_initrd(void)
 
 	fd = hv_fs_findfile((HV_VirtAddr) initramfs_file);
 	if (fd == HV_ENOENT) {
-		if (set_initramfs_file)
+		if (set_initramfs_file) {
 			pr_warning("No such hvfs initramfs file '%s'\n",
 				   initramfs_file);
-		return;
+			return;
+		} else {
+			/* Try old backwards-compatible name. */
+			fd = hv_fs_findfile((HV_VirtAddr)"initramfs.cpio.gz");
+			if (fd == HV_ENOENT)
+				return;
+		}
 	}
 	BUG_ON(fd < 0);
 	stat = hv_fs_fstat(fd);
diff --git a/arch/tile/kernel/smpboot.c b/arch/tile/kernel/smpboot.c
index e686c5ac90b..44bab29bf2f 100644
--- a/arch/tile/kernel/smpboot.c
+++ b/arch/tile/kernel/smpboot.c
@@ -207,9 +207,7 @@ void __cpuinit online_secondary(void)
 	/* Set up tile-timer clock-event device on this cpu */
 	setup_tile_timer();
 
-	preempt_enable();
-
-	cpu_idle();
+	cpu_startup_entry(CPUHP_ONLINE);
 }
 
 int __cpuinit __cpu_up(unsigned int cpu, struct task_struct *tidle)
diff --git a/arch/tile/mm/pgtable.c b/arch/tile/mm/pgtable.c
index b3b4972c245..dfd63ce8732 100644
--- a/arch/tile/mm/pgtable.c
+++ b/arch/tile/mm/pgtable.c
@@ -592,12 +592,7 @@ void iounmap(volatile void __iomem *addr_in)
 	   in parallel. Reuse of the virtual address is prevented by
 	   leaving it in the global lists until we're done with it.
 	   cpa takes care of the direct mappings. */
-	read_lock(&vmlist_lock);
-	for (p = vmlist; p; p = p->next) {
-		if (p->addr == addr)
-			break;
-	}
-	read_unlock(&vmlist_lock);
+	p = find_vm_area((void *)addr);
 
 	if (!p) {
 		pr_err("iounmap: bad address %p\n", addr);
diff --git a/arch/um/drivers/chan_kern.c b/arch/um/drivers/chan_kern.c
index 80b47cb71e0..acbe6c67afb 100644
--- a/arch/um/drivers/chan_kern.c
+++ b/arch/um/drivers/chan_kern.c
@@ -568,11 +568,7 @@ void chan_interrupt(struct line *line, int irq)
 		reactivate_fd(chan->fd, irq);
 	if (err == -EIO) {
 		if (chan->primary) {
-			struct tty_struct *tty = tty_port_tty_get(&line->port);
-			if (tty != NULL) {
-				tty_hangup(tty);
-				tty_kref_put(tty);
-			}
+			tty_port_tty_hangup(&line->port, false);
 			if (line->chan_out != chan)
 				close_one_chan(line->chan_out, 1);
 		}
diff --git a/arch/um/drivers/line.c b/arch/um/drivers/line.c
index be541cf69fd..8035145f043 100644
--- a/arch/um/drivers/line.c
+++ b/arch/um/drivers/line.c
@@ -248,7 +248,6 @@ static irqreturn_t line_write_interrupt(int irq, void *data)
 {
 	struct chan *chan = data;
 	struct line *line = chan->line;
-	struct tty_struct *tty;
 	int err;
 
 	/*
@@ -267,12 +266,7 @@ static irqreturn_t line_write_interrupt(int irq, void *data)
 	}
 	spin_unlock(&line->lock);
 
-	tty = tty_port_tty_get(&line->port);
-	if (tty == NULL)
-		return IRQ_NONE;
-
-	tty_wakeup(tty);
-	tty_kref_put(tty);
+	tty_port_tty_wakeup(&line->port);
 
 	return IRQ_HANDLED;
 }
diff --git a/arch/um/kernel/early_printk.c b/arch/um/kernel/early_printk.c
index 49480f09245..4a0800bc37b 100644
--- a/arch/um/kernel/early_printk.c
+++ b/arch/um/kernel/early_printk.c
@@ -16,7 +16,7 @@ static void early_console_write(struct console *con, const char *s, unsigned int
 	um_early_printk(s, n);
 }
 
-static struct console early_console = {
+static struct console early_console_dev = {
 	.name = "earlycon",
 	.write = early_console_write,
 	.flags = CON_BOOT,
@@ -25,8 +25,10 @@ static struct console early_console = {
 
 static int __init setup_early_printk(char *buf)
 {
-	register_console(&early_console);
-
+	if (!early_console) {
+		early_console = &early_console_dev;
+		register_console(&early_console_dev);
+	}
 	return 0;
 }
 
diff --git a/arch/um/kernel/mem.c b/arch/um/kernel/mem.c
index 5abcbfbe7e2..9df292b270a 100644
--- a/arch/um/kernel/mem.c
+++ b/arch/um/kernel/mem.c
@@ -42,17 +42,12 @@ static unsigned long brk_end;
 static void setup_highmem(unsigned long highmem_start,
 			  unsigned long highmem_len)
 {
-	struct page *page;
 	unsigned long highmem_pfn;
 	int i;
 
 	highmem_pfn = __pa(highmem_start) >> PAGE_SHIFT;
-	for (i = 0; i < highmem_len >> PAGE_SHIFT; i++) {
-		page = &mem_map[highmem_pfn + i];
-		ClearPageReserved(page);
-		init_page_count(page);
-		__free_page(page);
-	}
+	for (i = 0; i < highmem_len >> PAGE_SHIFT; i++)
+		free_highmem_page(&mem_map[highmem_pfn + i]);
 }
 #endif
 
@@ -73,18 +68,13 @@ void __init mem_init(void)
 	totalram_pages = free_all_bootmem();
 	max_low_pfn = totalram_pages;
 #ifdef CONFIG_HIGHMEM
-	totalhigh_pages = highmem >> PAGE_SHIFT;
-	totalram_pages += totalhigh_pages;
+	setup_highmem(end_iomem, highmem);
 #endif
 	num_physpages = totalram_pages;
 	max_pfn = totalram_pages;
 	printk(KERN_INFO "Memory: %luk available\n",
 	       nr_free_pages() << (PAGE_SHIFT-10));
 	kmalloc_ok = 1;
-
-#ifdef CONFIG_HIGHMEM
-	setup_highmem(end_iomem, highmem);
-#endif
 }
 
 /*
@@ -254,15 +244,7 @@ void free_initmem(void)
 #ifdef CONFIG_BLK_DEV_INITRD
 void free_initrd_mem(unsigned long start, unsigned long end)
 {
-	if (start < end)
-		printk(KERN_INFO "Freeing initrd memory: %ldk freed\n",
-		       (end - start) >> 10);
-	for (; start < end; start += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(start));
-		init_page_count(virt_to_page(start));
-		free_page(start);
-		totalram_pages++;
-	}
+	free_reserved_area(start, end, 0, "initrd");
 }
 #endif
 
diff --git a/arch/um/kernel/process.c b/arch/um/kernel/process.c
index b462b13c5ba..bbcef522bcb 100644
--- a/arch/um/kernel/process.c
+++ b/arch/um/kernel/process.c
@@ -210,33 +210,14 @@ void initial_thread_cb(void (*proc)(void *), void *arg)
 	kmalloc_ok = save_kmalloc_ok;
 }
 
-void default_idle(void)
+void arch_cpu_idle(void)
 {
 	unsigned long long nsecs;
 
-	while (1) {
-		/* endless idle loop with no priority at all */
-
-		/*
-		 * although we are an idle CPU, we do not want to
-		 * get into the scheduler unnecessarily.
-		 */
-		if (need_resched())
-			schedule();
-
-		tick_nohz_idle_enter();
-		rcu_idle_enter();
-		nsecs = disable_timer();
-		idle_sleep(nsecs);
-		rcu_idle_exit();
-		tick_nohz_idle_exit();
-	}
-}
-
-void cpu_idle(void)
-{
 	cpu_tasks[current_thread_info()->cpu].pid = os_getpid();
-	default_idle();
+	nsecs = disable_timer();
+	idle_sleep(nsecs);
+	local_irq_enable();
 }
 
 int __cant_sleep(void) {
diff --git a/arch/unicore32/kernel/early_printk.c b/arch/unicore32/kernel/early_printk.c
index 3922255f1fa..9be0d5d02a9 100644
--- a/arch/unicore32/kernel/early_printk.c
+++ b/arch/unicore32/kernel/early_printk.c
@@ -33,21 +33,17 @@ static struct console early_ocd_console = {
 	.index =	-1,
 };
 
-/* Direct interface for emergencies */
-static struct console *early_console = &early_ocd_console;
-
-static int __initdata keep_early;
-
 static int __init setup_early_printk(char *buf)
 {
-	if (!buf)
+	int keep_early;
+
+	if (!buf || early_console)
 		return 0;
 
 	if (strstr(buf, "keep"))
 		keep_early = 1;
 
-	if (!strncmp(buf, "ocd", 3))
-		early_console = &early_ocd_console;
+	early_console = &early_ocd_console;
 
 	if (keep_early)
 		early_console->flags &= ~CON_BOOT;
diff --git a/arch/unicore32/kernel/process.c b/arch/unicore32/kernel/process.c
index 872d7e22d84..7fab86d7c5d 100644
--- a/arch/unicore32/kernel/process.c
+++ b/arch/unicore32/kernel/process.c
@@ -45,25 +45,10 @@ static const char * const processor_modes[] = {
 	"UK18", "UK19", "UK1A", "EXTN", "UK1C", "UK1D", "UK1E", "SUSR"
 };
 
-void cpu_idle(void)
+void arch_cpu_idle(void)
 {
-	/* endless idle loop with no priority at all */
-	while (1) {
-		tick_nohz_idle_enter();
-		rcu_idle_enter();
-		while (!need_resched()) {
-			local_irq_disable();
-			stop_critical_timings();
-			cpu_do_idle();
-			local_irq_enable();
-			start_critical_timings();
-		}
-		rcu_idle_exit();
-		tick_nohz_idle_exit();
-		preempt_enable_no_resched();
-		schedule();
-		preempt_disable();
-	}
+	cpu_do_idle();
+	local_irq_enable();
 }
 
 static char reboot_mode = 'h';
diff --git a/arch/unicore32/mm/init.c b/arch/unicore32/mm/init.c
index de186bde897..63df12d71ce 100644
--- a/arch/unicore32/mm/init.c
+++ b/arch/unicore32/mm/init.c
@@ -66,6 +66,9 @@ void show_mem(unsigned int filter)
 	printk(KERN_DEFAULT "Mem-info:\n");
 	show_free_areas(filter);
 
+	if (filter & SHOW_MEM_FILTER_PAGE_COUNT)
+		return;
+
 	for_each_bank(i, mi) {
 		struct membank *bank = &mi->bank[i];
 		unsigned int pfn1, pfn2;
@@ -313,24 +316,6 @@ void __init bootmem_init(void)
 	max_pfn = max_high - PHYS_PFN_OFFSET;
 }
 
-static inline int free_area(unsigned long pfn, unsigned long end, char *s)
-{
-	unsigned int pages = 0, size = (end - pfn) << (PAGE_SHIFT - 10);
-
-	for (; pfn < end; pfn++) {
-		struct page *page = pfn_to_page(pfn);
-		ClearPageReserved(page);
-		init_page_count(page);
-		__free_page(page);
-		pages++;
-	}
-
-	if (size && s)
-		printk(KERN_INFO "Freeing %s memory: %dK\n", s, size);
-
-	return pages;
-}
-
 static inline void
 free_memmap(unsigned long start_pfn, unsigned long end_pfn)
 {
@@ -404,9 +389,9 @@ void __init mem_init(void)
 
 	max_mapnr   = pfn_to_page(max_pfn + PHYS_PFN_OFFSET) - mem_map;
 
-	/* this will put all unused low memory onto the freelists */
 	free_unused_memmap(&meminfo);
 
+	/* this will put all unused low memory onto the freelists */
 	totalram_pages += free_all_bootmem();
 
 	reserved_pages = free_pages = 0;
@@ -491,9 +476,7 @@ void __init mem_init(void)
 
 void free_initmem(void)
 {
-	totalram_pages += free_area(__phys_to_pfn(__pa(__init_begin)),
-				    __phys_to_pfn(__pa(__init_end)),
-				    "init");
+	free_initmem_default(0);
 }
 
 #ifdef CONFIG_BLK_DEV_INITRD
@@ -503,9 +486,7 @@ static int keep_initrd;
 void free_initrd_mem(unsigned long start, unsigned long end)
 {
 	if (!keep_initrd)
-		totalram_pages += free_area(__phys_to_pfn(__pa(start)),
-					    __phys_to_pfn(__pa(end)),
-					    "initrd");
+		free_reserved_area(start, end, 0, "initrd");
 }
 
 static int __init keepinitrd_setup(char *__unused)
diff --git a/arch/unicore32/mm/ioremap.c b/arch/unicore32/mm/ioremap.c
index b7a605597b0..13068ee22f3 100644
--- a/arch/unicore32/mm/ioremap.c
+++ b/arch/unicore32/mm/ioremap.c
@@ -235,7 +235,7 @@ EXPORT_SYMBOL(__uc32_ioremap_cached);
 void __uc32_iounmap(volatile void __iomem *io_addr)
 {
 	void *addr = (void *)(PAGE_MASK & (unsigned long)io_addr);
-	struct vm_struct **p, *tmp;
+	struct vm_struct *vm;
 
 	/*
 	 * If this is a section based mapping we need to handle it
@@ -244,17 +244,10 @@ void __uc32_iounmap(volatile void __iomem *io_addr)
 	 * all the mappings before the area can be reclaimed
 	 * by someone else.
 	 */
-	write_lock(&vmlist_lock);
-	for (p = &vmlist ; (tmp = *p) ; p = &tmp->next) {
-		if ((tmp->flags & VM_IOREMAP) && (tmp->addr == addr)) {
-			if (tmp->flags & VM_UNICORE_SECTION_MAPPING) {
-				unmap_area_sections((unsigned long)tmp->addr,
-						    tmp->size);
-			}
-			break;
-		}
-	}
-	write_unlock(&vmlist_lock);
+	vm = find_vm_area(addr);
+	if (vm && (vm->flags & VM_IOREMAP) &&
+		(vm->flags & VM_UNICORE_SECTION_MAPPING))
+		unmap_area_sections((unsigned long)vm->addr, vm->size);
 
 	vunmap(addr);
 }
diff --git a/arch/x86/Kconfig b/arch/x86/Kconfig
index 70c0f3da047..05b057dca4a 100644
--- a/arch/x86/Kconfig
+++ b/arch/x86/Kconfig
@@ -120,6 +120,7 @@ config X86
 	select OLD_SIGSUSPEND3 if X86_32 || IA32_EMULATION
 	select OLD_SIGACTION if X86_32
 	select COMPAT_OLD_SIGACTION if IA32_EMULATION
+	select RTC_LIB
 
 config INSTRUCTION_DECODER
 	def_bool y
@@ -188,9 +189,6 @@ config GENERIC_CALIBRATE_DELAY
 config ARCH_HAS_CPU_RELAX
 	def_bool y
 
-config ARCH_HAS_DEFAULT_IDLE
-	def_bool y
-
 config ARCH_HAS_CACHE_LINE_SIZE
 	def_bool y
 
@@ -389,7 +387,7 @@ config X86_NUMACHIP
 
 config X86_VSMP
 	bool "ScaleMP vSMP"
-	select PARAVIRT_GUEST
+	select HYPERVISOR_GUEST
 	select PARAVIRT
 	depends on X86_64 && PCI
 	depends on X86_EXTENDED_PLATFORM
@@ -596,44 +594,17 @@ config SCHED_OMIT_FRAME_POINTER
 
 	  If in doubt, say "Y".
 
-menuconfig PARAVIRT_GUEST
-	bool "Paravirtualized guest support"
-	---help---
-	  Say Y here to get to see options related to running Linux under
-	  various hypervisors.  This option alone does not add any kernel code.
-
-	  If you say N, all options in this submenu will be skipped and disabled.
-
-if PARAVIRT_GUEST
-
-config PARAVIRT_TIME_ACCOUNTING
-	bool "Paravirtual steal time accounting"
-	select PARAVIRT
-	default n
+menuconfig HYPERVISOR_GUEST
+	bool "Linux guest support"
 	---help---
-	  Select this option to enable fine granularity task steal time
-	  accounting. Time spent executing other tasks in parallel with
-	  the current vCPU is discounted from the vCPU power. To account for
-	  that, there can be a small performance impact.
-
-	  If in doubt, say N here.
-
-source "arch/x86/xen/Kconfig"
+	  Say Y here to enable options for running Linux under various hyper-
+	  visors. This option enables basic hypervisor detection and platform
+	  setup.
 
-config KVM_GUEST
-	bool "KVM Guest support (including kvmclock)"
-	select PARAVIRT
-	select PARAVIRT
-	select PARAVIRT_CLOCK
-	default y if PARAVIRT_GUEST
-	---help---
-	  This option enables various optimizations for running under the KVM
-	  hypervisor. It includes a paravirtualized clock, so that instead
-	  of relying on a PIT (or probably other) emulation by the
-	  underlying device model, the host provides the guest with
-	  timing infrastructure such as time of day, and system time
+	  If you say N, all options in this submenu will be skipped and
+	  disabled, and Linux guest support won't be built in.
 
-source "arch/x86/lguest/Kconfig"
+if HYPERVISOR_GUEST
 
 config PARAVIRT
 	bool "Enable paravirtualization code"
@@ -643,6 +614,13 @@ config PARAVIRT
 	  over full virtualization.  However, when run without a hypervisor
 	  the kernel is theoretically slower and slightly larger.
 
+config PARAVIRT_DEBUG
+	bool "paravirt-ops debugging"
+	depends on PARAVIRT && DEBUG_KERNEL
+	---help---
+	  Enable to debug paravirt_ops internals.  Specifically, BUG if
+	  a paravirt_op is missing when it is called.
+
 config PARAVIRT_SPINLOCKS
 	bool "Paravirtualization layer for spinlocks"
 	depends on PARAVIRT && SMP
@@ -656,17 +634,38 @@ config PARAVIRT_SPINLOCKS
 
 	  If you are unsure how to answer this question, answer N.
 
-config PARAVIRT_CLOCK
-	bool
+source "arch/x86/xen/Kconfig"
 
-endif
+config KVM_GUEST
+	bool "KVM Guest support (including kvmclock)"
+	depends on PARAVIRT
+	select PARAVIRT_CLOCK
+	default y
+	---help---
+	  This option enables various optimizations for running under the KVM
+	  hypervisor. It includes a paravirtualized clock, so that instead
+	  of relying on a PIT (or probably other) emulation by the
+	  underlying device model, the host provides the guest with
+	  timing infrastructure such as time of day, and system time
 
-config PARAVIRT_DEBUG
-	bool "paravirt-ops debugging"
-	depends on PARAVIRT && DEBUG_KERNEL
+source "arch/x86/lguest/Kconfig"
+
+config PARAVIRT_TIME_ACCOUNTING
+	bool "Paravirtual steal time accounting"
+	depends on PARAVIRT
+	default n
 	---help---
-	  Enable to debug paravirt_ops internals.  Specifically, BUG if
-	  a paravirt_op is missing when it is called.
+	  Select this option to enable fine granularity task steal time
+	  accounting. Time spent executing other tasks in parallel with
+	  the current vCPU is discounted from the vCPU power. To account for
+	  that, there can be a small performance impact.
+
+	  If in doubt, say N here.
+
+config PARAVIRT_CLOCK
+	bool
+
+endif #HYPERVISOR_GUEST
 
 config NO_BOOTMEM
 	def_bool y
@@ -1549,6 +1548,7 @@ config X86_SMAP
 config EFI
 	bool "EFI runtime service support"
 	depends on ACPI
+	select UCS2_STRING
 	---help---
 	  This enables the kernel to use EFI runtime services that are
 	  available (such as the EFI variable services).
diff --git a/arch/x86/Kconfig.debug b/arch/x86/Kconfig.debug
index b322f124ee3..16f738385dc 100644
--- a/arch/x86/Kconfig.debug
+++ b/arch/x86/Kconfig.debug
@@ -131,7 +131,7 @@ config DOUBLEFAULT
 
 config DEBUG_TLBFLUSH
 	bool "Set upper limit of TLB entries to flush one-by-one"
-	depends on DEBUG_KERNEL && (X86_64 || X86_INVLPG)
+	depends on DEBUG_KERNEL
 	---help---
 
 	X86-only for now.
diff --git a/arch/x86/boot/compressed/Makefile b/arch/x86/boot/compressed/Makefile
index 8a84501acb1..5ef205c5f37 100644
--- a/arch/x86/boot/compressed/Makefile
+++ b/arch/x86/boot/compressed/Makefile
@@ -4,7 +4,7 @@
 # create a compressed vmlinux image from the original vmlinux
 #
 
-targets := vmlinux.lds vmlinux vmlinux.bin vmlinux.bin.gz vmlinux.bin.bz2 vmlinux.bin.lzma vmlinux.bin.xz vmlinux.bin.lzo head_$(BITS).o misc.o string.o cmdline.o early_serial_console.o piggy.o
+targets := vmlinux vmlinux.bin vmlinux.bin.gz vmlinux.bin.bz2 vmlinux.bin.lzma vmlinux.bin.xz vmlinux.bin.lzo
 
 KBUILD_CFLAGS := -m$(BITS) -D__KERNEL__ $(LINUX_INCLUDE) -O2
 KBUILD_CFLAGS += -fno-strict-aliasing -fPIC
@@ -29,7 +29,6 @@ VMLINUX_OBJS = $(obj)/vmlinux.lds $(obj)/head_$(BITS).o $(obj)/misc.o \
 	$(obj)/piggy.o
 
 $(obj)/eboot.o: KBUILD_CFLAGS += -fshort-wchar -mno-red-zone
-$(obj)/efi_stub_$(BITS).o: KBUILD_CLFAGS += -fshort-wchar -mno-red-zone
 
 ifeq ($(CONFIG_EFI_STUB), y)
 	VMLINUX_OBJS += $(obj)/eboot.o $(obj)/efi_stub_$(BITS).o
@@ -43,7 +42,7 @@ OBJCOPYFLAGS_vmlinux.bin :=  -R .comment -S
 $(obj)/vmlinux.bin: vmlinux FORCE
 	$(call if_changed,objcopy)
 
-targets += vmlinux.bin.all vmlinux.relocs
+targets += $(patsubst $(obj)/%,%,$(VMLINUX_OBJS)) vmlinux.bin.all vmlinux.relocs
 
 CMD_RELOCS = arch/x86/tools/relocs
 quiet_cmd_relocs = RELOCS  $@
diff --git a/arch/x86/boot/compressed/eboot.c b/arch/x86/boot/compressed/eboot.c
index c205035a6b9..35ee62fccf9 100644
--- a/arch/x86/boot/compressed/eboot.c
+++ b/arch/x86/boot/compressed/eboot.c
@@ -251,6 +251,51 @@ static void find_bits(unsigned long mask, u8 *pos, u8 *size)
 	*size = len;
 }
 
+static efi_status_t setup_efi_vars(struct boot_params *params)
+{
+	struct setup_data *data;
+	struct efi_var_bootdata *efidata;
+	u64 store_size, remaining_size, var_size;
+	efi_status_t status;
+
+	if (sys_table->runtime->hdr.revision < EFI_2_00_SYSTEM_TABLE_REVISION)
+		return EFI_UNSUPPORTED;
+
+	data = (struct setup_data *)(unsigned long)params->hdr.setup_data;
+
+	while (data && data->next)
+		data = (struct setup_data *)(unsigned long)data->next;
+
+	status = efi_call_phys4((void *)sys_table->runtime->query_variable_info,
+				EFI_VARIABLE_NON_VOLATILE |
+				EFI_VARIABLE_BOOTSERVICE_ACCESS |
+				EFI_VARIABLE_RUNTIME_ACCESS, &store_size,
+				&remaining_size, &var_size);
+
+	if (status != EFI_SUCCESS)
+		return status;
+
+	status = efi_call_phys3(sys_table->boottime->allocate_pool,
+				EFI_LOADER_DATA, sizeof(*efidata), &efidata);
+
+	if (status != EFI_SUCCESS)
+		return status;
+
+	efidata->data.type = SETUP_EFI_VARS;
+	efidata->data.len = sizeof(struct efi_var_bootdata) -
+		sizeof(struct setup_data);
+	efidata->data.next = 0;
+	efidata->store_size = store_size;
+	efidata->remaining_size = remaining_size;
+	efidata->max_var_size = var_size;
+
+	if (data)
+		data->next = (unsigned long)efidata;
+	else
+		params->hdr.setup_data = (unsigned long)efidata;
+
+}
+
 static efi_status_t setup_efi_pci(struct boot_params *params)
 {
 	efi_pci_io_protocol *pci;
@@ -1157,6 +1202,8 @@ struct boot_params *efi_main(void *handle, efi_system_table_t *_table,
 
 	setup_graphics(boot_params);
 
+	setup_efi_vars(boot_params);
+
 	setup_efi_pci(boot_params);
 
 	status = efi_call_phys3(sys_table->boottime->allocate_pool,
diff --git a/arch/x86/boot/compressed/head_64.S b/arch/x86/boot/compressed/head_64.S
index c1d383d1fb7..16f24e6dad7 100644
--- a/arch/x86/boot/compressed/head_64.S
+++ b/arch/x86/boot/compressed/head_64.S
@@ -52,7 +52,7 @@ ENTRY(startup_32)
 	jnz 1f
 
 	cli
-	movl	$(__KERNEL_DS), %eax
+	movl	$(__BOOT_DS), %eax
 	movl	%eax, %ds
 	movl	%eax, %es
 	movl	%eax, %ss
diff --git a/arch/x86/include/asm/cmpxchg.h b/arch/x86/include/asm/cmpxchg.h
index 8d871eaddb6..d47786acb01 100644
--- a/arch/x86/include/asm/cmpxchg.h
+++ b/arch/x86/include/asm/cmpxchg.h
@@ -35,7 +35,7 @@ extern void __add_wrong_size(void)
 
 /* 
  * An exchange-type operation, which takes a value and a pointer, and
- * returns a the old value.
+ * returns the old value.
  */
 #define __xchg_op(ptr, arg, op, lock)					\
 	({								\
diff --git a/arch/x86/include/asm/context_tracking.h b/arch/x86/include/asm/context_tracking.h
index 1616562683e..1fe49704b14 100644
--- a/arch/x86/include/asm/context_tracking.h
+++ b/arch/x86/include/asm/context_tracking.h
@@ -1,31 +1,10 @@
 #ifndef _ASM_X86_CONTEXT_TRACKING_H
 #define _ASM_X86_CONTEXT_TRACKING_H
 
-#ifndef __ASSEMBLY__
-#include <linux/context_tracking.h>
-#include <asm/ptrace.h>
-
-static inline void exception_enter(struct pt_regs *regs)
-{
-	user_exit();
-}
-
-static inline void exception_exit(struct pt_regs *regs)
-{
-#ifdef CONFIG_CONTEXT_TRACKING
-	if (user_mode(regs))
-		user_enter();
-#endif
-}
-
-#else /* __ASSEMBLY__ */
-
 #ifdef CONFIG_CONTEXT_TRACKING
 # define SCHEDULE_USER call schedule_user
 #else
 # define SCHEDULE_USER call schedule
 #endif
 
-#endif /* !__ASSEMBLY__ */
-
 #endif
diff --git a/arch/x86/include/asm/cpufeature.h b/arch/x86/include/asm/cpufeature.h
index 93fe929d1ce..398f7cb1353 100644
--- a/arch/x86/include/asm/cpufeature.h
+++ b/arch/x86/include/asm/cpufeature.h
@@ -9,6 +9,7 @@
 #endif
 
 #define NCAPINTS	10	/* N 32-bit words worth of info */
+#define NBUGINTS	1	/* N 32-bit bug flags */
 
 /*
  * Note: If the comment begins with a quoted string, that string is used
@@ -100,6 +101,7 @@
 #define X86_FEATURE_AMD_DCM     (3*32+27) /* multi-node processor */
 #define X86_FEATURE_APERFMPERF	(3*32+28) /* APERFMPERF */
 #define X86_FEATURE_EAGER_FPU	(3*32+29) /* "eagerfpu" Non lazy FPU restore */
+#define X86_FEATURE_NONSTOP_TSC_S3 (3*32+30) /* TSC doesn't stop in S3 state */
 
 /* Intel-defined CPU features, CPUID level 0x00000001 (ecx), word 4 */
 #define X86_FEATURE_XMM3	(4*32+ 0) /* "pni" SSE-3 */
@@ -168,6 +170,7 @@
 #define X86_FEATURE_TOPOEXT	(6*32+22) /* topology extensions CPUID leafs */
 #define X86_FEATURE_PERFCTR_CORE (6*32+23) /* core performance counter extensions */
 #define X86_FEATURE_PERFCTR_NB  (6*32+24) /* NB performance counter extensions */
+#define X86_FEATURE_PERFCTR_L2	(6*32+28) /* L2 performance counter extensions */
 
 /*
  * Auxiliary flags: Linux defined - For features scattered in various
@@ -216,6 +219,17 @@
 #define X86_FEATURE_ADX		(9*32+19) /* The ADCX and ADOX instructions */
 #define X86_FEATURE_SMAP	(9*32+20) /* Supervisor Mode Access Prevention */
 
+/*
+ * BUG word(s)
+ */
+#define X86_BUG(x)		(NCAPINTS*32 + (x))
+
+#define X86_BUG_F00F		X86_BUG(0) /* Intel F00F */
+#define X86_BUG_FDIV		X86_BUG(1) /* FPU FDIV */
+#define X86_BUG_COMA		X86_BUG(2) /* Cyrix 6x86 coma */
+#define X86_BUG_AMD_TLB_MMATCH	X86_BUG(3) /* AMD Erratum 383 */
+#define X86_BUG_AMD_APIC_C1E	X86_BUG(4) /* AMD Erratum 400 */
+
 #if defined(__KERNEL__) && !defined(__ASSEMBLY__)
 
 #include <asm/asm.h>
@@ -311,6 +325,7 @@ extern const char * const x86_power_flags[32];
 #define cpu_has_pclmulqdq	boot_cpu_has(X86_FEATURE_PCLMULQDQ)
 #define cpu_has_perfctr_core	boot_cpu_has(X86_FEATURE_PERFCTR_CORE)
 #define cpu_has_perfctr_nb	boot_cpu_has(X86_FEATURE_PERFCTR_NB)
+#define cpu_has_perfctr_l2	boot_cpu_has(X86_FEATURE_PERFCTR_L2)
 #define cpu_has_cx8		boot_cpu_has(X86_FEATURE_CX8)
 #define cpu_has_cx16		boot_cpu_has(X86_FEATURE_CX16)
 #define cpu_has_eager_fpu	boot_cpu_has(X86_FEATURE_EAGER_FPU)
@@ -401,6 +416,13 @@ static __always_inline __pure bool __static_cpu_has(u16 bit)
 #define static_cpu_has(bit) boot_cpu_has(bit)
 #endif
 
+#define cpu_has_bug(c, bit)	cpu_has(c, (bit))
+#define set_cpu_bug(c, bit)	set_cpu_cap(c, (bit))
+#define clear_cpu_bug(c, bit)	clear_cpu_cap(c, (bit));
+
+#define static_cpu_has_bug(bit)	static_cpu_has((bit))
+#define boot_cpu_has_bug(bit)	cpu_has_bug(&boot_cpu_data, (bit))
+
 #endif /* defined(__KERNEL__) && !defined(__ASSEMBLY__) */
 
 #endif /* _ASM_X86_CPUFEATURE_H */
diff --git a/arch/x86/include/asm/efi.h b/arch/x86/include/asm/efi.h
index 60c89f30c72..2fb5d5884e2 100644
--- a/arch/x86/include/asm/efi.h
+++ b/arch/x86/include/asm/efi.h
@@ -102,6 +102,13 @@ extern void efi_call_phys_epilog(void);
 extern void efi_unmap_memmap(void);
 extern void efi_memory_uc(u64 addr, unsigned long size);
 
+struct efi_var_bootdata {
+	struct setup_data data;
+	u64 store_size;
+	u64 remaining_size;
+	u64 max_var_size;
+};
+
 #ifdef CONFIG_EFI
 
 static inline bool efi_is_native(void)
diff --git a/arch/x86/include/asm/fixmap.h b/arch/x86/include/asm/fixmap.h
index a09c2857106..0dc7d9e21c3 100644
--- a/arch/x86/include/asm/fixmap.h
+++ b/arch/x86/include/asm/fixmap.h
@@ -104,12 +104,7 @@ enum fixed_addresses {
 	FIX_LI_PCIA,	/* Lithium PCI Bridge A */
 	FIX_LI_PCIB,	/* Lithium PCI Bridge B */
 #endif
-#ifdef CONFIG_X86_F00F_BUG
-	FIX_F00F_IDT,	/* Virtual mapping for IDT */
-#endif
-#ifdef CONFIG_X86_CYCLONE_TIMER
-	FIX_CYCLONE_TIMER, /*cyclone timer register*/
-#endif
+	FIX_RO_IDT,	/* Virtual mapping for read-only IDT */
 #ifdef CONFIG_X86_32
 	FIX_KMAP_BEGIN,	/* reserved pte's for temporary kernel mappings */
 	FIX_KMAP_END = FIX_KMAP_BEGIN+(KM_TYPE_NR*NR_CPUS)-1,
diff --git a/arch/x86/include/asm/hugetlb.h b/arch/x86/include/asm/hugetlb.h
index bdd35dbd060..a8091216963 100644
--- a/arch/x86/include/asm/hugetlb.h
+++ b/arch/x86/include/asm/hugetlb.h
@@ -2,6 +2,7 @@
 #define _ASM_X86_HUGETLB_H
 
 #include <asm/page.h>
+#include <asm-generic/hugetlb.h>
 
 
 static inline int is_hugepage_only_range(struct mm_struct *mm,
diff --git a/arch/x86/include/asm/hypervisor.h b/arch/x86/include/asm/hypervisor.h
index 86095ed1413..2d4b5e6107c 100644
--- a/arch/x86/include/asm/hypervisor.h
+++ b/arch/x86/include/asm/hypervisor.h
@@ -20,13 +20,11 @@
 #ifndef _ASM_X86_HYPERVISOR_H
 #define _ASM_X86_HYPERVISOR_H
 
+#ifdef CONFIG_HYPERVISOR_GUEST
+
 #include <asm/kvm_para.h>
 #include <asm/xen/hypervisor.h>
 
-extern void init_hypervisor(struct cpuinfo_x86 *c);
-extern void init_hypervisor_platform(void);
-extern bool hypervisor_x2apic_available(void);
-
 /*
  * x86 hypervisor information
  */
@@ -55,4 +53,12 @@ extern const struct hypervisor_x86 x86_hyper_ms_hyperv;
 extern const struct hypervisor_x86 x86_hyper_xen_hvm;
 extern const struct hypervisor_x86 x86_hyper_kvm;
 
-#endif
+extern void init_hypervisor(struct cpuinfo_x86 *c);
+extern void init_hypervisor_platform(void);
+extern bool hypervisor_x2apic_available(void);
+#else
+static inline void init_hypervisor(struct cpuinfo_x86 *c) { }
+static inline void init_hypervisor_platform(void) { }
+static inline bool hypervisor_x2apic_available(void) { return false; }
+#endif /* CONFIG_HYPERVISOR_GUEST */
+#endif /* _ASM_X86_HYPERVISOR_H */
diff --git a/arch/x86/include/asm/mce.h b/arch/x86/include/asm/mce.h
index f4076af1f4e..fa5f71e021d 100644
--- a/arch/x86/include/asm/mce.h
+++ b/arch/x86/include/asm/mce.h
@@ -146,13 +146,13 @@ DECLARE_PER_CPU(struct device *, mce_device);
 void mce_intel_feature_init(struct cpuinfo_x86 *c);
 void cmci_clear(void);
 void cmci_reenable(void);
-void cmci_rediscover(int dying);
+void cmci_rediscover(void);
 void cmci_recheck(void);
 #else
 static inline void mce_intel_feature_init(struct cpuinfo_x86 *c) { }
 static inline void cmci_clear(void) {}
 static inline void cmci_reenable(void) {}
-static inline void cmci_rediscover(int dying) {}
+static inline void cmci_rediscover(void) {}
 static inline void cmci_recheck(void) {}
 #endif
 
diff --git a/arch/x86/include/asm/msr.h b/arch/x86/include/asm/msr.h
index 9264802e282..cb7502852ac 100644
--- a/arch/x86/include/asm/msr.h
+++ b/arch/x86/include/asm/msr.h
@@ -137,11 +137,11 @@ static inline unsigned long long native_read_pmc(int counter)
  * pointer indirection), this allows gcc to optimize better
  */
 
-#define rdmsr(msr, val1, val2)					\
+#define rdmsr(msr, low, high)					\
 do {								\
 	u64 __val = native_read_msr((msr));			\
-	(void)((val1) = (u32)__val);				\
-	(void)((val2) = (u32)(__val >> 32));			\
+	(void)((low) = (u32)__val);				\
+	(void)((high) = (u32)(__val >> 32));			\
 } while (0)
 
 static inline void wrmsr(unsigned msr, unsigned low, unsigned high)
@@ -162,12 +162,12 @@ static inline int wrmsr_safe(unsigned msr, unsigned low, unsigned high)
 }
 
 /* rdmsr with exception handling */
-#define rdmsr_safe(msr, p1, p2)					\
+#define rdmsr_safe(msr, low, high)				\
 ({								\
 	int __err;						\
 	u64 __val = native_read_msr_safe((msr), &__err);	\
-	(*p1) = (u32)__val;					\
-	(*p2) = (u32)(__val >> 32);				\
+	(*low) = (u32)__val;					\
+	(*high) = (u32)(__val >> 32);				\
 	__err;							\
 })
 
@@ -208,7 +208,7 @@ do {                                                            \
 #define wrmsrl_safe(msr, val) wrmsr_safe((msr), (u32)(val),		\
 					     (u32)((val) >> 32))
 
-#define write_tsc(val1, val2) wrmsr(MSR_IA32_TSC, (val1), (val2))
+#define write_tsc(low, high) wrmsr(MSR_IA32_TSC, (low), (high))
 
 #define write_rdtscp_aux(val) wrmsr(MSR_TSC_AUX, (val), 0)
 
diff --git a/arch/x86/include/asm/page_64_types.h b/arch/x86/include/asm/page_64_types.h
index 8b491e66eaa..6c896fbe21d 100644
--- a/arch/x86/include/asm/page_64_types.h
+++ b/arch/x86/include/asm/page_64_types.h
@@ -48,6 +48,5 @@
  * arch/x86/kernel/head_64.S), and it is mapped here:
  */
 #define KERNEL_IMAGE_SIZE	(512 * 1024 * 1024)
-#define KERNEL_IMAGE_START	_AC(0xffffffff80000000, UL)
 
 #endif /* _ASM_X86_PAGE_64_DEFS_H */
diff --git a/arch/x86/include/asm/paravirt.h b/arch/x86/include/asm/paravirt.h
index 5edd1742cfd..cfdc9ee4c90 100644
--- a/arch/x86/include/asm/paravirt.h
+++ b/arch/x86/include/asm/paravirt.h
@@ -262,10 +262,6 @@ static inline void set_ldt(const void *addr, unsigned entries)
 {
 	PVOP_VCALL2(pv_cpu_ops.set_ldt, addr, entries);
 }
-static inline void store_gdt(struct desc_ptr *dtr)
-{
-	PVOP_VCALL1(pv_cpu_ops.store_gdt, dtr);
-}
 static inline void store_idt(struct desc_ptr *dtr)
 {
 	PVOP_VCALL1(pv_cpu_ops.store_idt, dtr);
@@ -703,7 +699,10 @@ static inline void arch_leave_lazy_mmu_mode(void)
 	PVOP_VCALL0(pv_mmu_ops.lazy_mode.leave);
 }
 
-void arch_flush_lazy_mmu_mode(void);
+static inline void arch_flush_lazy_mmu_mode(void)
+{
+	PVOP_VCALL0(pv_mmu_ops.lazy_mode.flush);
+}
 
 static inline void __set_fixmap(unsigned /* enum fixed_addresses */ idx,
 				phys_addr_t phys, pgprot_t flags)
diff --git a/arch/x86/include/asm/paravirt_types.h b/arch/x86/include/asm/paravirt_types.h
index 142236ed83a..0db1fcac668 100644
--- a/arch/x86/include/asm/paravirt_types.h
+++ b/arch/x86/include/asm/paravirt_types.h
@@ -91,6 +91,7 @@ struct pv_lazy_ops {
 	/* Set deferred update mode, used for batching operations. */
 	void (*enter)(void);
 	void (*leave)(void);
+	void (*flush)(void);
 };
 
 struct pv_time_ops {
@@ -122,7 +123,7 @@ struct pv_cpu_ops {
 	void (*load_tr_desc)(void);
 	void (*load_gdt)(const struct desc_ptr *);
 	void (*load_idt)(const struct desc_ptr *);
-	void (*store_gdt)(struct desc_ptr *);
+	/* store_gdt has been removed. */
 	void (*store_idt)(struct desc_ptr *);
 	void (*set_ldt)(const void *desc, unsigned entries);
 	unsigned long (*store_tr)(void);
@@ -679,6 +680,7 @@ void paravirt_end_context_switch(struct task_struct *next);
 
 void paravirt_enter_lazy_mmu(void);
 void paravirt_leave_lazy_mmu(void);
+void paravirt_flush_lazy_mmu(void);
 
 void _paravirt_nop(void);
 u32 _paravirt_ident_32(u32);
diff --git a/arch/x86/include/asm/perf_event_p4.h b/arch/x86/include/asm/perf_event_p4.h
index 4f7e67e2345..85e13ccf15c 100644
--- a/arch/x86/include/asm/perf_event_p4.h
+++ b/arch/x86/include/asm/perf_event_p4.h
@@ -24,45 +24,45 @@
 #define ARCH_P4_CNTRVAL_MASK	((1ULL << ARCH_P4_CNTRVAL_BITS) - 1)
 #define ARCH_P4_UNFLAGGED_BIT	((1ULL) << (ARCH_P4_CNTRVAL_BITS - 1))
 
-#define P4_ESCR_EVENT_MASK	0x7e000000U
+#define P4_ESCR_EVENT_MASK	0x7e000000ULL
 #define P4_ESCR_EVENT_SHIFT	25
-#define P4_ESCR_EVENTMASK_MASK	0x01fffe00U
+#define P4_ESCR_EVENTMASK_MASK	0x01fffe00ULL
 #define P4_ESCR_EVENTMASK_SHIFT	9
-#define P4_ESCR_TAG_MASK	0x000001e0U
+#define P4_ESCR_TAG_MASK	0x000001e0ULL
 #define P4_ESCR_TAG_SHIFT	5
-#define P4_ESCR_TAG_ENABLE	0x00000010U
-#define P4_ESCR_T0_OS		0x00000008U
-#define P4_ESCR_T0_USR		0x00000004U
-#define P4_ESCR_T1_OS		0x00000002U
-#define P4_ESCR_T1_USR		0x00000001U
+#define P4_ESCR_TAG_ENABLE	0x00000010ULL
+#define P4_ESCR_T0_OS		0x00000008ULL
+#define P4_ESCR_T0_USR		0x00000004ULL
+#define P4_ESCR_T1_OS		0x00000002ULL
+#define P4_ESCR_T1_USR		0x00000001ULL
 
 #define P4_ESCR_EVENT(v)	((v) << P4_ESCR_EVENT_SHIFT)
 #define P4_ESCR_EMASK(v)	((v) << P4_ESCR_EVENTMASK_SHIFT)
 #define P4_ESCR_TAG(v)		((v) << P4_ESCR_TAG_SHIFT)
 
-#define P4_CCCR_OVF			0x80000000U
-#define P4_CCCR_CASCADE			0x40000000U
-#define P4_CCCR_OVF_PMI_T0		0x04000000U
-#define P4_CCCR_OVF_PMI_T1		0x08000000U
-#define P4_CCCR_FORCE_OVF		0x02000000U
-#define P4_CCCR_EDGE			0x01000000U
-#define P4_CCCR_THRESHOLD_MASK		0x00f00000U
+#define P4_CCCR_OVF			0x80000000ULL
+#define P4_CCCR_CASCADE			0x40000000ULL
+#define P4_CCCR_OVF_PMI_T0		0x04000000ULL
+#define P4_CCCR_OVF_PMI_T1		0x08000000ULL
+#define P4_CCCR_FORCE_OVF		0x02000000ULL
+#define P4_CCCR_EDGE			0x01000000ULL
+#define P4_CCCR_THRESHOLD_MASK		0x00f00000ULL
 #define P4_CCCR_THRESHOLD_SHIFT		20
-#define P4_CCCR_COMPLEMENT		0x00080000U
-#define P4_CCCR_COMPARE			0x00040000U
-#define P4_CCCR_ESCR_SELECT_MASK	0x0000e000U
+#define P4_CCCR_COMPLEMENT		0x00080000ULL
+#define P4_CCCR_COMPARE			0x00040000ULL
+#define P4_CCCR_ESCR_SELECT_MASK	0x0000e000ULL
 #define P4_CCCR_ESCR_SELECT_SHIFT	13
-#define P4_CCCR_ENABLE			0x00001000U
-#define P4_CCCR_THREAD_SINGLE		0x00010000U
-#define P4_CCCR_THREAD_BOTH		0x00020000U
-#define P4_CCCR_THREAD_ANY		0x00030000U
-#define P4_CCCR_RESERVED		0x00000fffU
+#define P4_CCCR_ENABLE			0x00001000ULL
+#define P4_CCCR_THREAD_SINGLE		0x00010000ULL
+#define P4_CCCR_THREAD_BOTH		0x00020000ULL
+#define P4_CCCR_THREAD_ANY		0x00030000ULL
+#define P4_CCCR_RESERVED		0x00000fffULL
 
 #define P4_CCCR_THRESHOLD(v)		((v) << P4_CCCR_THRESHOLD_SHIFT)
 #define P4_CCCR_ESEL(v)			((v) << P4_CCCR_ESCR_SELECT_SHIFT)
 
 #define P4_GEN_ESCR_EMASK(class, name, bit)	\
-	class##__##name = ((1 << bit) << P4_ESCR_EVENTMASK_SHIFT)
+	class##__##name = ((1ULL << bit) << P4_ESCR_EVENTMASK_SHIFT)
 #define P4_ESCR_EMASK_BIT(class, name)		class##__##name
 
 /*
@@ -107,7 +107,7 @@
  * P4_PEBS_CONFIG_MASK and related bits on
  * modification.)
  */
-#define P4_CONFIG_ALIASABLE		(1 << 9)
+#define P4_CONFIG_ALIASABLE		(1ULL << 9)
 
 /*
  * The bits we allow to pass for RAW events
@@ -784,17 +784,17 @@ enum P4_ESCR_EMASKS {
  * Note we have UOP and PEBS bits reserved for now
  * just in case if we will need them once
  */
-#define P4_PEBS_CONFIG_ENABLE		(1 << 7)
-#define P4_PEBS_CONFIG_UOP_TAG		(1 << 8)
-#define P4_PEBS_CONFIG_METRIC_MASK	0x3f
-#define P4_PEBS_CONFIG_MASK		0xff
+#define P4_PEBS_CONFIG_ENABLE		(1ULL << 7)
+#define P4_PEBS_CONFIG_UOP_TAG		(1ULL << 8)
+#define P4_PEBS_CONFIG_METRIC_MASK	0x3FLL
+#define P4_PEBS_CONFIG_MASK		0xFFLL
 
 /*
  * mem: Only counters MSR_IQ_COUNTER4 (16) and
  * MSR_IQ_COUNTER5 (17) are allowed for PEBS sampling
  */
-#define P4_PEBS_ENABLE			0x02000000U
-#define P4_PEBS_ENABLE_UOP_TAG		0x01000000U
+#define P4_PEBS_ENABLE			0x02000000ULL
+#define P4_PEBS_ENABLE_UOP_TAG		0x01000000ULL
 
 #define p4_config_unpack_metric(v)	(((u64)(v)) & P4_PEBS_CONFIG_METRIC_MASK)
 #define p4_config_unpack_pebs(v)	(((u64)(v)) & P4_PEBS_CONFIG_MASK)
diff --git a/arch/x86/include/asm/pgtable_types.h b/arch/x86/include/asm/pgtable_types.h
index 567b5d0632b..e6423002c10 100644
--- a/arch/x86/include/asm/pgtable_types.h
+++ b/arch/x86/include/asm/pgtable_types.h
@@ -351,7 +351,6 @@ static inline void update_page_count(int level, unsigned long pages) { }
  * as a pte too.
  */
 extern pte_t *lookup_address(unsigned long address, unsigned int *level);
-extern int __split_large_page(pte_t *kpte, unsigned long address, pte_t *pbase);
 extern phys_addr_t slow_virt_to_phys(void *__address);
 
 #endif	/* !__ASSEMBLY__ */
diff --git a/arch/x86/include/asm/processor.h b/arch/x86/include/asm/processor.h
index 3270116b148..22224b3b43b 100644
--- a/arch/x86/include/asm/processor.h
+++ b/arch/x86/include/asm/processor.h
@@ -91,9 +91,6 @@ struct cpuinfo_x86 {
 	/* Problems on some 486Dx4's and old 386's: */
 	char			hard_math;
 	char			rfu;
-	char			fdiv_bug;
-	char			f00f_bug;
-	char			coma_bug;
 	char			pad0;
 #else
 	/* Number of 4K pages in DTLB/ITLB combined(in pages): */
@@ -107,7 +104,7 @@ struct cpuinfo_x86 {
 	__u32			extended_cpuid_level;
 	/* Maximum supported CPUID level, -1=no CPUID: */
 	int			cpuid_level;
-	__u32			x86_capability[NCAPINTS];
+	__u32			x86_capability[NCAPINTS + NBUGINTS];
 	char			x86_vendor_id[16];
 	char			x86_model_id[64];
 	/* in KB - valid for CPUS which support this call: */
@@ -973,26 +970,6 @@ unsigned long calc_aperfmperf_ratio(struct aperfmperf *old,
 	return ratio;
 }
 
-/*
- * AMD errata checking
- */
-#ifdef CONFIG_CPU_SUP_AMD
-extern const int amd_erratum_383[];
-extern const int amd_erratum_400[];
-extern bool cpu_has_amd_erratum(const int *);
-
-#define AMD_LEGACY_ERRATUM(...)		{ -1, __VA_ARGS__, 0 }
-#define AMD_OSVW_ERRATUM(osvw_id, ...)	{ osvw_id, __VA_ARGS__, 0 }
-#define AMD_MODEL_RANGE(f, m_start, s_start, m_end, s_end) \
-	((f << 24) | (m_start << 16) | (s_start << 12) | (m_end << 4) | (s_end))
-#define AMD_MODEL_RANGE_FAMILY(range)	(((range) >> 24) & 0xff)
-#define AMD_MODEL_RANGE_START(range)	(((range) >> 12) & 0xfff)
-#define AMD_MODEL_RANGE_END(range)	((range) & 0xfff)
-
-#else
-#define cpu_has_amd_erratum(x)	(false)
-#endif /* CONFIG_CPU_SUP_AMD */
-
 extern unsigned long arch_align_stack(unsigned long sp);
 extern void free_init_pages(char *what, unsigned long begin, unsigned long end);
 
diff --git a/arch/x86/include/asm/suspend_32.h b/arch/x86/include/asm/suspend_32.h
index 487055c8c1a..f6064b7385b 100644
--- a/arch/x86/include/asm/suspend_32.h
+++ b/arch/x86/include/asm/suspend_32.h
@@ -15,7 +15,6 @@ struct saved_context {
 	unsigned long cr0, cr2, cr3, cr4;
 	u64 misc_enable;
 	bool misc_enable_saved;
-	struct desc_ptr gdt;
 	struct desc_ptr idt;
 	u16 ldt;
 	u16 tss;
diff --git a/arch/x86/include/asm/suspend_64.h b/arch/x86/include/asm/suspend_64.h
index 09b0bf10415..97b84e08a21 100644
--- a/arch/x86/include/asm/suspend_64.h
+++ b/arch/x86/include/asm/suspend_64.h
@@ -25,9 +25,6 @@ struct saved_context {
 	u64 misc_enable;
 	bool misc_enable_saved;
 	unsigned long efer;
-	u16 gdt_pad;
-	u16 gdt_limit;
-	unsigned long gdt_base;
 	u16 idt_pad;
 	u16 idt_limit;
 	unsigned long idt_base;
diff --git a/arch/x86/include/asm/syscall.h b/arch/x86/include/asm/syscall.h
index 1ace47b6259..2e188d68397 100644
--- a/arch/x86/include/asm/syscall.h
+++ b/arch/x86/include/asm/syscall.h
@@ -29,13 +29,13 @@ extern const unsigned long sys_call_table[];
  */
 static inline int syscall_get_nr(struct task_struct *task, struct pt_regs *regs)
 {
-	return regs->orig_ax & __SYSCALL_MASK;
+	return regs->orig_ax;
 }
 
 static inline void syscall_rollback(struct task_struct *task,
 				    struct pt_regs *regs)
 {
-	regs->ax = regs->orig_ax & __SYSCALL_MASK;
+	regs->ax = regs->orig_ax;
 }
 
 static inline long syscall_get_error(struct task_struct *task,
diff --git a/arch/x86/include/asm/thread_info.h b/arch/x86/include/asm/thread_info.h
index 2cd056e3ada..a1df6e84691 100644
--- a/arch/x86/include/asm/thread_info.h
+++ b/arch/x86/include/asm/thread_info.h
@@ -241,8 +241,6 @@ static inline struct thread_info *current_thread_info(void)
 					   skip sending interrupt */
 #define TS_RESTORE_SIGMASK	0x0008	/* restore signal mask in do_signal() */
 
-#define tsk_is_polling(t) (task_thread_info(t)->status & TS_POLLING)
-
 #ifndef __ASSEMBLY__
 #define HAVE_SET_RESTORE_SIGMASK	1
 static inline void set_restore_sigmask(void)
diff --git a/arch/x86/include/asm/tlb.h b/arch/x86/include/asm/tlb.h
index 4fef20773b8..c7797307fc2 100644
--- a/arch/x86/include/asm/tlb.h
+++ b/arch/x86/include/asm/tlb.h
@@ -7,7 +7,7 @@
 
 #define tlb_flush(tlb)							\
 {									\
-	if (tlb->fullmm == 0)						\
+	if (!tlb->fullmm && !tlb->need_flush_all) 			\
 		flush_tlb_mm_range(tlb->mm, tlb->start, tlb->end, 0UL);	\
 	else								\
 		flush_tlb_mm_range(tlb->mm, 0UL, TLB_FLUSH_ALL, 0UL);	\
diff --git a/arch/x86/include/asm/uprobes.h b/arch/x86/include/asm/uprobes.h
index 8ff8be7835a..6e5197910fd 100644
--- a/arch/x86/include/asm/uprobes.h
+++ b/arch/x86/include/asm/uprobes.h
@@ -55,4 +55,5 @@ 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);
+extern unsigned long arch_uretprobe_hijack_return_addr(unsigned long trampoline_vaddr, struct pt_regs *regs);
 #endif	/* _ASM_UPROBES_H */
diff --git a/arch/x86/include/asm/xen/hypercall.h b/arch/x86/include/asm/xen/hypercall.h
index c20d1ce62dc..e709884d0ef 100644
--- a/arch/x86/include/asm/xen/hypercall.h
+++ b/arch/x86/include/asm/xen/hypercall.h
@@ -382,14 +382,14 @@ HYPERVISOR_console_io(int cmd, int count, char *str)
 	return _hypercall3(int, console_io, cmd, count, str);
 }
 
-extern int __must_check HYPERVISOR_physdev_op_compat(int, void *);
+extern int __must_check xen_physdev_op_compat(int, void *);
 
 static inline int
 HYPERVISOR_physdev_op(int cmd, void *arg)
 {
 	int rc = _hypercall2(int, physdev_op, cmd, arg);
 	if (unlikely(rc == -ENOSYS))
-		rc = HYPERVISOR_physdev_op_compat(cmd, arg);
+		rc = xen_physdev_op_compat(cmd, arg);
 	return rc;
 }
 
diff --git a/arch/x86/include/uapi/asm/bootparam.h b/arch/x86/include/uapi/asm/bootparam.h
index c15ddaf9071..08744242b8d 100644
--- a/arch/x86/include/uapi/asm/bootparam.h
+++ b/arch/x86/include/uapi/asm/bootparam.h
@@ -6,6 +6,7 @@
 #define SETUP_E820_EXT			1
 #define SETUP_DTB			2
 #define SETUP_PCI			3
+#define SETUP_EFI_VARS			4
 
 /* ram_size flags */
 #define RAMDISK_IMAGE_START_MASK	0x07FF
diff --git a/arch/x86/include/uapi/asm/msr-index.h b/arch/x86/include/uapi/asm/msr-index.h
index 892ce40a747..b5757885d7a 100644
--- a/arch/x86/include/uapi/asm/msr-index.h
+++ b/arch/x86/include/uapi/asm/msr-index.h
@@ -44,6 +44,7 @@
 #define SNB_C1_AUTO_UNDEMOTE		(1UL << 27)
 #define SNB_C3_AUTO_UNDEMOTE		(1UL << 28)
 
+#define MSR_PLATFORM_INFO		0x000000ce
 #define MSR_MTRRcap			0x000000fe
 #define MSR_IA32_BBL_CR_CTL		0x00000119
 #define MSR_IA32_BBL_CR_CTL3		0x0000011e
@@ -71,6 +72,7 @@
 #define MSR_IA32_PEBS_ENABLE		0x000003f1
 #define MSR_IA32_DS_AREA		0x00000600
 #define MSR_IA32_PERF_CAPABILITIES	0x00000345
+#define MSR_PEBS_LD_LAT_THRESHOLD	0x000003f6
 
 #define MSR_MTRRfix64K_00000		0x00000250
 #define MSR_MTRRfix16K_80000		0x00000258
@@ -194,6 +196,10 @@
 #define MSR_AMD64_IBSBRTARGET		0xc001103b
 #define MSR_AMD64_IBS_REG_COUNT_MAX	8 /* includes MSR_AMD64_IBSBRTARGET */
 
+/* Fam 16h MSRs */
+#define MSR_F16H_L2I_PERF_CTL		0xc0010230
+#define MSR_F16H_L2I_PERF_CTR		0xc0010231
+
 /* Fam 15h MSRs */
 #define MSR_F15H_PERF_CTL		0xc0010200
 #define MSR_F15H_PERF_CTR		0xc0010201
diff --git a/arch/x86/kernel/acpi/sleep.c b/arch/x86/kernel/acpi/sleep.c
index 0532f5d6e4e..b44577bc974 100644
--- a/arch/x86/kernel/acpi/sleep.c
+++ b/arch/x86/kernel/acpi/sleep.c
@@ -46,7 +46,7 @@ int acpi_suspend_lowlevel(void)
 	header->pmode_behavior = 0;
 
 #ifndef CONFIG_64BIT
-	store_gdt((struct desc_ptr *)&header->pmode_gdt);
+	native_store_gdt((struct desc_ptr *)&header->pmode_gdt);
 
 	if (!rdmsr_safe(MSR_EFER,
 			&header->pmode_efer_low,
diff --git a/arch/x86/kernel/acpi/wakeup_32.S b/arch/x86/kernel/acpi/wakeup_32.S
index 13ab720573e..d1daa66ab16 100644
--- a/arch/x86/kernel/acpi/wakeup_32.S
+++ b/arch/x86/kernel/acpi/wakeup_32.S
@@ -1,4 +1,4 @@
-	.section .text..page_aligned
+	.text
 #include <linux/linkage.h>
 #include <asm/segment.h>
 #include <asm/page_types.h>
@@ -18,7 +18,6 @@ wakeup_pmode_return:
 	movw	%ax, %gs
 
 	# reload the gdt, as we need the full 32 bit address
-	lgdt	saved_gdt
 	lidt	saved_idt
 	lldt	saved_ldt
 	ljmp	$(__KERNEL_CS), $1f
@@ -44,7 +43,6 @@ bogus_magic:
 
 
 save_registers:
-	sgdt	saved_gdt
 	sidt	saved_idt
 	sldt	saved_ldt
 	str	saved_tss
@@ -93,7 +91,6 @@ ENTRY(saved_magic)	.long	0
 ENTRY(saved_eip)	.long	0
 
 # saved registers
-saved_gdt:	.long	0,0
 saved_idt:	.long	0,0
 saved_ldt:	.long	0
 saved_tss:	.long	0
diff --git a/arch/x86/kernel/alternative.c b/arch/x86/kernel/alternative.c
index ef5ccca79a6..c15cf9a25e2 100644
--- a/arch/x86/kernel/alternative.c
+++ b/arch/x86/kernel/alternative.c
@@ -271,7 +271,7 @@ void __init_or_module apply_alternatives(struct alt_instr *start,
 		replacement = (u8 *)&a->repl_offset + a->repl_offset;
 		BUG_ON(a->replacementlen > a->instrlen);
 		BUG_ON(a->instrlen > sizeof(insnbuf));
-		BUG_ON(a->cpuid >= NCAPINTS*32);
+		BUG_ON(a->cpuid >= (NCAPINTS + NBUGINTS) * 32);
 		if (!boot_cpu_has(a->cpuid))
 			continue;
 
diff --git a/arch/x86/kernel/amd_nb.c b/arch/x86/kernel/amd_nb.c
index aadf3359e2a..3048ded1b59 100644
--- a/arch/x86/kernel/amd_nb.c
+++ b/arch/x86/kernel/amd_nb.c
@@ -20,12 +20,14 @@ const struct pci_device_id amd_nb_misc_ids[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_AMD, PCI_DEVICE_ID_AMD_10H_NB_MISC) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_AMD, PCI_DEVICE_ID_AMD_15H_NB_F3) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_AMD, PCI_DEVICE_ID_AMD_15H_M10H_F3) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_AMD, PCI_DEVICE_ID_AMD_16H_NB_F3) },
 	{}
 };
 EXPORT_SYMBOL(amd_nb_misc_ids);
 
-static struct pci_device_id amd_nb_link_ids[] = {
+static const struct pci_device_id amd_nb_link_ids[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_AMD, PCI_DEVICE_ID_AMD_15H_NB_F4) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_AMD, PCI_DEVICE_ID_AMD_16H_NB_F4) },
 	{}
 };
 
@@ -81,7 +83,6 @@ int amd_cache_northbridges(void)
 			next_northbridge(link, amd_nb_link_ids);
         }
 
-	/* some CPU families (e.g. family 0x11) do not support GART */
 	if (boot_cpu_data.x86 == 0xf || boot_cpu_data.x86 == 0x10 ||
 	    boot_cpu_data.x86 == 0x15)
 		amd_northbridges.flags |= AMD_NB_GART;
diff --git a/arch/x86/kernel/aperture_64.c b/arch/x86/kernel/aperture_64.c
index d5fd66f0d4c..fd972a3e4cb 100644
--- a/arch/x86/kernel/aperture_64.c
+++ b/arch/x86/kernel/aperture_64.c
@@ -87,7 +87,7 @@ static u32 __init allocate_aperture(void)
 	 */
 	addr = memblock_find_in_range(GART_MIN_ADDR, GART_MAX_ADDR,
 				      aper_size, aper_size);
-	if (!addr || addr + aper_size > GART_MAX_ADDR) {
+	if (!addr) {
 		printk(KERN_ERR
 			"Cannot allocate aperture memory hole (%lx,%uK)\n",
 				addr, aper_size>>10);
diff --git a/arch/x86/kernel/cpu/Makefile b/arch/x86/kernel/cpu/Makefile
index a0e067d3d96..b0684e4a73a 100644
--- a/arch/x86/kernel/cpu/Makefile
+++ b/arch/x86/kernel/cpu/Makefile
@@ -14,7 +14,6 @@ CFLAGS_common.o		:= $(nostackp)
 
 obj-y			:= intel_cacheinfo.o scattered.o topology.o
 obj-y			+= proc.o capflags.o powerflags.o common.o
-obj-y			+= vmware.o hypervisor.o mshyperv.o
 obj-y			+= rdrand.o
 obj-y			+= match.o
 
@@ -31,7 +30,7 @@ obj-$(CONFIG_CPU_SUP_UMC_32)		+= umc.o
 obj-$(CONFIG_PERF_EVENTS)		+= perf_event.o
 
 ifdef CONFIG_PERF_EVENTS
-obj-$(CONFIG_CPU_SUP_AMD)		+= perf_event_amd.o
+obj-$(CONFIG_CPU_SUP_AMD)		+= perf_event_amd.o perf_event_amd_uncore.o
 obj-$(CONFIG_CPU_SUP_INTEL)		+= perf_event_p6.o perf_event_knc.o perf_event_p4.o
 obj-$(CONFIG_CPU_SUP_INTEL)		+= perf_event_intel_lbr.o perf_event_intel_ds.o perf_event_intel.o
 obj-$(CONFIG_CPU_SUP_INTEL)		+= perf_event_intel_uncore.o
@@ -42,11 +41,13 @@ obj-$(CONFIG_MTRR)			+= mtrr/
 
 obj-$(CONFIG_X86_LOCAL_APIC)		+= perfctr-watchdog.o perf_event_amd_ibs.o
 
+obj-$(CONFIG_HYPERVISOR_GUEST)		+= vmware.o hypervisor.o mshyperv.o
+
 quiet_cmd_mkcapflags = MKCAP   $@
-      cmd_mkcapflags = $(PERL) $(srctree)/$(src)/mkcapflags.pl $< $@
+      cmd_mkcapflags = $(CONFIG_SHELL) $(srctree)/$(src)/mkcapflags.sh $< $@
 
 cpufeature = $(src)/../../include/asm/cpufeature.h
 
 targets += capflags.c
-$(obj)/capflags.c: $(cpufeature) $(src)/mkcapflags.pl FORCE
+$(obj)/capflags.c: $(cpufeature) $(src)/mkcapflags.sh FORCE
 	$(call if_changed,mkcapflags)
diff --git a/arch/x86/kernel/cpu/amd.c b/arch/x86/kernel/cpu/amd.c
index fa96eb0d02f..5013a48d1af 100644
--- a/arch/x86/kernel/cpu/amd.c
+++ b/arch/x86/kernel/cpu/amd.c
@@ -20,11 +20,11 @@
 
 static inline int rdmsrl_amd_safe(unsigned msr, unsigned long long *p)
 {
-	struct cpuinfo_x86 *c = &cpu_data(smp_processor_id());
 	u32 gprs[8] = { 0 };
 	int err;
 
-	WARN_ONCE((c->x86 != 0xf), "%s should only be used on K8!\n", __func__);
+	WARN_ONCE((boot_cpu_data.x86 != 0xf),
+		  "%s should only be used on K8!\n", __func__);
 
 	gprs[1] = msr;
 	gprs[7] = 0x9c5a203a;
@@ -38,10 +38,10 @@ static inline int rdmsrl_amd_safe(unsigned msr, unsigned long long *p)
 
 static inline int wrmsrl_amd_safe(unsigned msr, unsigned long long val)
 {
-	struct cpuinfo_x86 *c = &cpu_data(smp_processor_id());
 	u32 gprs[8] = { 0 };
 
-	WARN_ONCE((c->x86 != 0xf), "%s should only be used on K8!\n", __func__);
+	WARN_ONCE((boot_cpu_data.x86 != 0xf),
+		  "%s should only be used on K8!\n", __func__);
 
 	gprs[0] = (u32)val;
 	gprs[1] = msr;
@@ -192,11 +192,11 @@ static void __cpuinit amd_k7_smp_check(struct cpuinfo_x86 *c)
 	/* Athlon 660/661 is valid. */
 	if ((c->x86_model == 6) && ((c->x86_mask == 0) ||
 	    (c->x86_mask == 1)))
-		goto valid_k7;
+		return;
 
 	/* Duron 670 is valid */
 	if ((c->x86_model == 7) && (c->x86_mask == 0))
-		goto valid_k7;
+		return;
 
 	/*
 	 * Athlon 662, Duron 671, and Athlon >model 7 have capability
@@ -209,7 +209,7 @@ static void __cpuinit amd_k7_smp_check(struct cpuinfo_x86 *c)
 	    ((c->x86_model == 7) && (c->x86_mask >= 1)) ||
 	     (c->x86_model > 7))
 		if (cpu_has_mp)
-			goto valid_k7;
+			return;
 
 	/* If we get here, not a certified SMP capable AMD system. */
 
@@ -220,9 +220,6 @@ static void __cpuinit amd_k7_smp_check(struct cpuinfo_x86 *c)
 	WARN_ONCE(1, "WARNING: This combination of AMD"
 		" processors is not suitable for SMP.\n");
 	add_taint(TAINT_UNSAFE_SMP, LOCKDEP_NOW_UNRELIABLE);
-
-valid_k7:
-	;
 }
 
 static void __cpuinit init_amd_k7(struct cpuinfo_x86 *c)
@@ -513,6 +510,10 @@ static void __cpuinit early_init_amd(struct cpuinfo_x86 *c)
 #endif
 }
 
+static const int amd_erratum_383[];
+static const int amd_erratum_400[];
+static bool cpu_has_amd_erratum(const int *erratum);
+
 static void __cpuinit init_amd(struct cpuinfo_x86 *c)
 {
 	u32 dummy;
@@ -727,8 +728,14 @@ static void __cpuinit init_amd(struct cpuinfo_x86 *c)
 		rdmsrl_safe(MSR_AMD64_BU_CFG2, &value);
 		value &= ~(1ULL << 24);
 		wrmsrl_safe(MSR_AMD64_BU_CFG2, value);
+
+		if (cpu_has_amd_erratum(amd_erratum_383))
+			set_cpu_bug(c, X86_BUG_AMD_TLB_MMATCH);
 	}
 
+	if (cpu_has_amd_erratum(amd_erratum_400))
+		set_cpu_bug(c, X86_BUG_AMD_APIC_C1E);
+
 	rdmsr_safe(MSR_AMD64_PATCH_LEVEL, &c->microcode, &dummy);
 }
 
@@ -847,8 +854,7 @@ cpu_dev_register(amd_cpu_dev);
  * AMD_OSVW_ERRATUM() macros. The latter is intended for newer errata that
  * have an OSVW id assigned, which it takes as first argument. Both take a
  * variable number of family-specific model-stepping ranges created by
- * AMD_MODEL_RANGE(). Each erratum also has to be declared as extern const
- * int[] in arch/x86/include/asm/processor.h.
+ * AMD_MODEL_RANGE().
  *
  * Example:
  *
@@ -858,16 +864,22 @@ cpu_dev_register(amd_cpu_dev);
  *			   AMD_MODEL_RANGE(0x10, 0x9, 0x0, 0x9, 0x0));
  */
 
-const int amd_erratum_400[] =
+#define AMD_LEGACY_ERRATUM(...)		{ -1, __VA_ARGS__, 0 }
+#define AMD_OSVW_ERRATUM(osvw_id, ...)	{ osvw_id, __VA_ARGS__, 0 }
+#define AMD_MODEL_RANGE(f, m_start, s_start, m_end, s_end) \
+	((f << 24) | (m_start << 16) | (s_start << 12) | (m_end << 4) | (s_end))
+#define AMD_MODEL_RANGE_FAMILY(range)	(((range) >> 24) & 0xff)
+#define AMD_MODEL_RANGE_START(range)	(((range) >> 12) & 0xfff)
+#define AMD_MODEL_RANGE_END(range)	((range) & 0xfff)
+
+static const int amd_erratum_400[] =
 	AMD_OSVW_ERRATUM(1, AMD_MODEL_RANGE(0xf, 0x41, 0x2, 0xff, 0xf),
 			    AMD_MODEL_RANGE(0x10, 0x2, 0x1, 0xff, 0xf));
-EXPORT_SYMBOL_GPL(amd_erratum_400);
 
-const int amd_erratum_383[] =
+static const int amd_erratum_383[] =
 	AMD_OSVW_ERRATUM(3, AMD_MODEL_RANGE(0x10, 0, 0, 0xff, 0xf));
-EXPORT_SYMBOL_GPL(amd_erratum_383);
 
-bool cpu_has_amd_erratum(const int *erratum)
+static bool cpu_has_amd_erratum(const int *erratum)
 {
 	struct cpuinfo_x86 *cpu = __this_cpu_ptr(&cpu_info);
 	int osvw_id = *erratum++;
@@ -908,5 +920,3 @@ bool cpu_has_amd_erratum(const int *erratum)
 
 	return false;
 }
-
-EXPORT_SYMBOL_GPL(cpu_has_amd_erratum);
diff --git a/arch/x86/kernel/cpu/bugs.c b/arch/x86/kernel/cpu/bugs.c
index af6455e3fcc..4112be9a465 100644
--- a/arch/x86/kernel/cpu/bugs.c
+++ b/arch/x86/kernel/cpu/bugs.c
@@ -59,7 +59,7 @@ static void __init check_fpu(void)
 	 * trap_init() enabled FXSR and company _before_ testing for FP
 	 * problems here.
 	 *
-	 * Test for the divl bug..
+	 * Test for the divl bug: http://en.wikipedia.org/wiki/Fdiv_bug
 	 */
 	__asm__("fninit\n\t"
 		"fldl %1\n\t"
@@ -75,26 +75,12 @@ static void __init check_fpu(void)
 
 	kernel_fpu_end();
 
-	boot_cpu_data.fdiv_bug = fdiv_bug;
-	if (boot_cpu_data.fdiv_bug)
+	if (fdiv_bug) {
+		set_cpu_bug(&boot_cpu_data, X86_BUG_FDIV);
 		pr_warn("Hmm, FPU with FDIV bug\n");
+	}
 }
 
-/*
- * Check whether we are able to run this kernel safely on SMP.
- *
- * - i386 is no longer supported.
- * - In order to run on anything without a TSC, we need to be
- *   compiled for a i486.
- */
-
-static void __init check_config(void)
-{
-	if (boot_cpu_data.x86 < 4)
-		panic("Kernel requires i486+ for 'invlpg' and other features");
-}
-
-
 void __init check_bugs(void)
 {
 	identify_boot_cpu();
@@ -102,7 +88,17 @@ void __init check_bugs(void)
 	pr_info("CPU: ");
 	print_cpu_info(&boot_cpu_data);
 #endif
-	check_config();
+
+	/*
+	 * Check whether we are able to run this kernel safely on SMP.
+	 *
+	 * - i386 is no longer supported.
+	 * - In order to run on anything without a TSC, we need to be
+	 *   compiled for a i486.
+	 */
+	if (boot_cpu_data.x86 < 4)
+		panic("Kernel requires i486+ for 'invlpg' and other features");
+
 	init_utsname()->machine[1] =
 		'0' + (boot_cpu_data.x86 > 6 ? 6 : boot_cpu_data.x86);
 	alternative_instructions();
diff --git a/arch/x86/kernel/cpu/common.c b/arch/x86/kernel/cpu/common.c
index d814772c5be..22018f70a67 100644
--- a/arch/x86/kernel/cpu/common.c
+++ b/arch/x86/kernel/cpu/common.c
@@ -920,6 +920,10 @@ static void __cpuinit identify_cpu(struct cpuinfo_x86 *c)
 		/* AND the already accumulated flags with these */
 		for (i = 0; i < NCAPINTS; i++)
 			boot_cpu_data.x86_capability[i] &= c->x86_capability[i];
+
+		/* OR, i.e. replicate the bug flags */
+		for (i = NCAPINTS; i < NCAPINTS + NBUGINTS; i++)
+			c->x86_capability[i] |= boot_cpu_data.x86_capability[i];
 	}
 
 	/* Init Machine Check Exception if available. */
diff --git a/arch/x86/kernel/cpu/cyrix.c b/arch/x86/kernel/cpu/cyrix.c
index 4fbd384fb64..d048d5ca43c 100644
--- a/arch/x86/kernel/cpu/cyrix.c
+++ b/arch/x86/kernel/cpu/cyrix.c
@@ -249,7 +249,7 @@ static void __cpuinit init_cyrix(struct cpuinfo_x86 *c)
 		/* Emulate MTRRs using Cyrix's ARRs. */
 		set_cpu_cap(c, X86_FEATURE_CYRIX_ARR);
 		/* 6x86's contain this bug */
-		c->coma_bug = 1;
+		set_cpu_bug(c, X86_BUG_COMA);
 		break;
 
 	case 4: /* MediaGX/GXm or Geode GXM/GXLV/GX1 */
@@ -317,7 +317,8 @@ static void __cpuinit init_cyrix(struct cpuinfo_x86 *c)
 			/* Enable MMX extensions (App note 108) */
 			setCx86_old(CX86_CCR7, getCx86_old(CX86_CCR7)|1);
 		} else {
-			c->coma_bug = 1;      /* 6x86MX, it has the bug. */
+			/* A 6x86MX - it has the bug. */
+			set_cpu_bug(c, X86_BUG_COMA);
 		}
 		tmp = (!(dir0_lsn & 7) || dir0_lsn & 1) ? 2 : 0;
 		Cx86_cb[tmp] = cyrix_model_mult2[dir0_lsn & 7];
diff --git a/arch/x86/kernel/cpu/intel.c b/arch/x86/kernel/cpu/intel.c
index 1905ce98bee..9b0c441c03f 100644
--- a/arch/x86/kernel/cpu/intel.c
+++ b/arch/x86/kernel/cpu/intel.c
@@ -96,6 +96,18 @@ static void __cpuinit early_init_intel(struct cpuinfo_x86 *c)
 			sched_clock_stable = 1;
 	}
 
+	/* Penwell and Cloverview have the TSC which doesn't sleep on S3 */
+	if (c->x86 == 6) {
+		switch (c->x86_model) {
+		case 0x27:	/* Penwell */
+		case 0x35:	/* Cloverview */
+			set_cpu_cap(c, X86_FEATURE_NONSTOP_TSC_S3);
+			break;
+		default:
+			break;
+		}
+	}
+
 	/*
 	 * There is a known erratum on Pentium III and Core Solo
 	 * and Core Duo CPUs.
@@ -164,20 +176,6 @@ int __cpuinit ppro_with_ram_bug(void)
 	return 0;
 }
 
-#ifdef CONFIG_X86_F00F_BUG
-static void __cpuinit trap_init_f00f_bug(void)
-{
-	__set_fixmap(FIX_F00F_IDT, __pa_symbol(idt_table), PAGE_KERNEL_RO);
-
-	/*
-	 * Update the IDT descriptor and reload the IDT so that
-	 * it uses the read-only mapped virtual address.
-	 */
-	idt_descr.address = fix_to_virt(FIX_F00F_IDT);
-	load_idt(&idt_descr);
-}
-#endif
-
 static void __cpuinit intel_smp_check(struct cpuinfo_x86 *c)
 {
 	/* calling is from identify_secondary_cpu() ? */
@@ -206,16 +204,14 @@ static void __cpuinit intel_workarounds(struct cpuinfo_x86 *c)
 	/*
 	 * All current models of Pentium and Pentium with MMX technology CPUs
 	 * have the F0 0F bug, which lets nonprivileged users lock up the
-	 * system.
-	 * Note that the workaround only should be initialized once...
+	 * system. Announce that the fault handler will be checking for it.
 	 */
-	c->f00f_bug = 0;
+	clear_cpu_bug(c, X86_BUG_F00F);
 	if (!paravirt_enabled() && c->x86 == 5) {
 		static int f00f_workaround_enabled;
 
-		c->f00f_bug = 1;
+		set_cpu_bug(c, X86_BUG_F00F);
 		if (!f00f_workaround_enabled) {
-			trap_init_f00f_bug();
 			printk(KERN_NOTICE "Intel Pentium with F0 0F bug - workaround enabled.\n");
 			f00f_workaround_enabled = 1;
 		}
diff --git a/arch/x86/kernel/cpu/mcheck/mce.c b/arch/x86/kernel/cpu/mcheck/mce.c
index 7bc126346ac..9239504b41c 100644
--- a/arch/x86/kernel/cpu/mcheck/mce.c
+++ b/arch/x86/kernel/cpu/mcheck/mce.c
@@ -2358,7 +2358,7 @@ mce_cpu_callback(struct notifier_block *nfb, unsigned long action, void *hcpu)
 
 	if (action == CPU_POST_DEAD) {
 		/* intentionally ignoring frozen here */
-		cmci_rediscover(cpu);
+		cmci_rediscover();
 	}
 
 	return NOTIFY_OK;
diff --git a/arch/x86/kernel/cpu/mcheck/mce_amd.c b/arch/x86/kernel/cpu/mcheck/mce_amd.c
index 1ac581f38df..9cb52767999 100644
--- a/arch/x86/kernel/cpu/mcheck/mce_amd.c
+++ b/arch/x86/kernel/cpu/mcheck/mce_amd.c
@@ -33,7 +33,6 @@
 #include <asm/mce.h>
 #include <asm/msr.h>
 
-#define NR_BANKS          6
 #define NR_BLOCKS         9
 #define THRESHOLD_MAX     0xFFF
 #define INT_TYPE_APIC     0x00020000
@@ -57,12 +56,7 @@ static const char * const th_names[] = {
 	"execution_unit",
 };
 
-static DEFINE_PER_CPU(struct threshold_bank * [NR_BANKS], threshold_banks);
-
-static unsigned char shared_bank[NR_BANKS] = {
-	0, 0, 0, 0, 1
-};
-
+static DEFINE_PER_CPU(struct threshold_bank **, threshold_banks);
 static DEFINE_PER_CPU(unsigned char, bank_map);	/* see which banks are on */
 
 static void amd_threshold_interrupt(void);
@@ -79,6 +73,12 @@ struct thresh_restart {
 	u16			old_limit;
 };
 
+static inline bool is_shared_bank(int bank)
+{
+	/* Bank 4 is for northbridge reporting and is thus shared */
+	return (bank == 4);
+}
+
 static const char * const bank4_names(struct threshold_block *b)
 {
 	switch (b->address) {
@@ -214,7 +214,7 @@ void mce_amd_feature_init(struct cpuinfo_x86 *c)
 	unsigned int bank, block;
 	int offset = -1;
 
-	for (bank = 0; bank < NR_BANKS; ++bank) {
+	for (bank = 0; bank < mca_cfg.banks; ++bank) {
 		for (block = 0; block < NR_BLOCKS; ++block) {
 			if (block == 0)
 				address = MSR_IA32_MC0_MISC + bank * 4;
@@ -276,7 +276,7 @@ static void amd_threshold_interrupt(void)
 	mce_setup(&m);
 
 	/* assume first bank caused it */
-	for (bank = 0; bank < NR_BANKS; ++bank) {
+	for (bank = 0; bank < mca_cfg.banks; ++bank) {
 		if (!(per_cpu(bank_map, m.cpu) & (1 << bank)))
 			continue;
 		for (block = 0; block < NR_BLOCKS; ++block) {
@@ -467,7 +467,7 @@ static __cpuinit int allocate_threshold_blocks(unsigned int cpu,
 	u32 low, high;
 	int err;
 
-	if ((bank >= NR_BANKS) || (block >= NR_BLOCKS))
+	if ((bank >= mca_cfg.banks) || (block >= NR_BLOCKS))
 		return 0;
 
 	if (rdmsr_safe_on_cpu(cpu, address, &low, &high))
@@ -575,7 +575,7 @@ static __cpuinit int threshold_create_bank(unsigned int cpu, unsigned int bank)
 	const char *name = th_names[bank];
 	int err = 0;
 
-	if (shared_bank[bank]) {
+	if (is_shared_bank(bank)) {
 		nb = node_to_amd_nb(amd_get_nb_id(cpu));
 
 		/* threshold descriptor already initialized on this node? */
@@ -609,7 +609,7 @@ static __cpuinit int threshold_create_bank(unsigned int cpu, unsigned int bank)
 
 	per_cpu(threshold_banks, cpu)[bank] = b;
 
-	if (shared_bank[bank]) {
+	if (is_shared_bank(bank)) {
 		atomic_set(&b->cpus, 1);
 
 		/* nb is already initialized, see above */
@@ -635,9 +635,17 @@ static __cpuinit int threshold_create_bank(unsigned int cpu, unsigned int bank)
 static __cpuinit int threshold_create_device(unsigned int cpu)
 {
 	unsigned int bank;
+	struct threshold_bank **bp;
 	int err = 0;
 
-	for (bank = 0; bank < NR_BANKS; ++bank) {
+	bp = kzalloc(sizeof(struct threshold_bank *) * mca_cfg.banks,
+		     GFP_KERNEL);
+	if (!bp)
+		return -ENOMEM;
+
+	per_cpu(threshold_banks, cpu) = bp;
+
+	for (bank = 0; bank < mca_cfg.banks; ++bank) {
 		if (!(per_cpu(bank_map, cpu) & (1 << bank)))
 			continue;
 		err = threshold_create_bank(cpu, bank);
@@ -691,7 +699,7 @@ static void threshold_remove_bank(unsigned int cpu, int bank)
 	if (!b->blocks)
 		goto free_out;
 
-	if (shared_bank[bank]) {
+	if (is_shared_bank(bank)) {
 		if (!atomic_dec_and_test(&b->cpus)) {
 			__threshold_remove_blocks(b);
 			per_cpu(threshold_banks, cpu)[bank] = NULL;
@@ -719,11 +727,12 @@ static void threshold_remove_device(unsigned int cpu)
 {
 	unsigned int bank;
 
-	for (bank = 0; bank < NR_BANKS; ++bank) {
+	for (bank = 0; bank < mca_cfg.banks; ++bank) {
 		if (!(per_cpu(bank_map, cpu) & (1 << bank)))
 			continue;
 		threshold_remove_bank(cpu, bank);
 	}
+	kfree(per_cpu(threshold_banks, cpu));
 }
 
 /* get notified when a cpu comes on/off */
diff --git a/arch/x86/kernel/cpu/mcheck/mce_intel.c b/arch/x86/kernel/cpu/mcheck/mce_intel.c
index 402c454fbff..ae1697c2afe 100644
--- a/arch/x86/kernel/cpu/mcheck/mce_intel.c
+++ b/arch/x86/kernel/cpu/mcheck/mce_intel.c
@@ -285,39 +285,24 @@ void cmci_clear(void)
 	raw_spin_unlock_irqrestore(&cmci_discover_lock, flags);
 }
 
-static long cmci_rediscover_work_func(void *arg)
+static void cmci_rediscover_work_func(void *arg)
 {
 	int banks;
 
 	/* Recheck banks in case CPUs don't all have the same */
 	if (cmci_supported(&banks))
 		cmci_discover(banks);
-
-	return 0;
 }
 
-/*
- * After a CPU went down cycle through all the others and rediscover
- * Must run in process context.
- */
-void cmci_rediscover(int dying)
+/* After a CPU went down cycle through all the others and rediscover */
+void cmci_rediscover(void)
 {
-	int cpu, banks;
+	int banks;
 
 	if (!cmci_supported(&banks))
 		return;
 
-	for_each_online_cpu(cpu) {
-		if (cpu == dying)
-			continue;
-
-		if (cpu == smp_processor_id()) {
-			cmci_rediscover_work_func(NULL);
-			continue;
-		}
-
-		work_on_cpu(cpu, cmci_rediscover_work_func, NULL);
-	}
+	on_each_cpu(cmci_rediscover_work_func, NULL, 1);
 }
 
 /*
diff --git a/arch/x86/kernel/cpu/mkcapflags.pl b/arch/x86/kernel/cpu/mkcapflags.pl
deleted file mode 100644
index 091972ef49d..00000000000
--- a/arch/x86/kernel/cpu/mkcapflags.pl
+++ /dev/null
@@ -1,48 +0,0 @@
-#!/usr/bin/perl -w
-#
-# Generate the x86_cap_flags[] array from include/asm-x86/cpufeature.h
-#
-
-($in, $out) = @ARGV;
-
-open(IN, "< $in\0")   or die "$0: cannot open: $in: $!\n";
-open(OUT, "> $out\0") or die "$0: cannot create: $out: $!\n";
-
-print OUT "#ifndef _ASM_X86_CPUFEATURE_H\n";
-print OUT "#include <asm/cpufeature.h>\n";
-print OUT "#endif\n";
-print OUT "\n";
-print OUT "const char * const x86_cap_flags[NCAPINTS*32] = {\n";
-
-%features = ();
-$err = 0;
-
-while (defined($line = <IN>)) {
-	if ($line =~ /^\s*\#\s*define\s+(X86_FEATURE_(\S+))\s+(.*)$/) {
-		$macro = $1;
-		$feature = "\L$2";
-		$tail = $3;
-		if ($tail =~ /\/\*\s*\"([^"]*)\".*\*\//) {
-			$feature = "\L$1";
-		}
-
-		next if ($feature eq '');
-
-		if ($features{$feature}++) {
-			print STDERR "$in: duplicate feature name: $feature\n";
-			$err++;
-		}
-		printf OUT "\t%-32s = \"%s\",\n", "[$macro]", $feature;
-	}
-}
-print OUT "};\n";
-
-close(IN);
-close(OUT);
-
-if ($err) {
-	unlink($out);
-	exit(1);
-}
-
-exit(0);
diff --git a/arch/x86/kernel/cpu/mkcapflags.sh b/arch/x86/kernel/cpu/mkcapflags.sh
new file mode 100644
index 00000000000..2bf61650549
--- /dev/null
+++ b/arch/x86/kernel/cpu/mkcapflags.sh
@@ -0,0 +1,41 @@
+#!/bin/sh
+#
+# Generate the x86_cap_flags[] array from include/asm/cpufeature.h
+#
+
+IN=$1
+OUT=$2
+
+TABS="$(printf '\t\t\t\t\t')"
+trap 'rm "$OUT"' EXIT
+
+(
+	echo "#ifndef _ASM_X86_CPUFEATURE_H"
+	echo "#include <asm/cpufeature.h>"
+	echo "#endif"
+	echo ""
+	echo "const char * const x86_cap_flags[NCAPINTS*32] = {"
+
+	# Iterate through any input lines starting with #define X86_FEATURE_
+	sed -n -e 's/\t/ /g' -e 's/^ *# *define *X86_FEATURE_//p' $IN |
+	while read i
+	do
+		# Name is everything up to the first whitespace
+		NAME="$(echo "$i" | sed 's/ .*//')"
+
+		# If the /* comment */ starts with a quote string, grab that.
+		VALUE="$(echo "$i" | sed -n 's@.*/\* *\("[^"]*"\).*\*/@\1@p')"
+		[ -z "$VALUE" ] && VALUE="\"$NAME\""
+		[ "$VALUE" == '""' ] && continue
+
+		# Name is uppercase, VALUE is all lowercase
+		VALUE="$(echo "$VALUE" | tr A-Z a-z)"
+
+		TABCOUNT=$(( ( 5*8 - 14 - $(echo "$NAME" | wc -c) ) / 8 ))
+		printf "\t[%s]%.*s = %s,\n" \
+			"X86_FEATURE_$NAME" "$TABCOUNT" "$TABS" "$VALUE"
+	done
+	echo "};"
+) > $OUT
+
+trap - EXIT
diff --git a/arch/x86/kernel/cpu/mshyperv.c b/arch/x86/kernel/cpu/mshyperv.c
index a7d26d83fb7..8f4be53ea04 100644
--- a/arch/x86/kernel/cpu/mshyperv.c
+++ b/arch/x86/kernel/cpu/mshyperv.c
@@ -35,13 +35,6 @@ static bool __init ms_hyperv_platform(void)
 	if (!boot_cpu_has(X86_FEATURE_HYPERVISOR))
 		return false;
 
-	/*
-	 * Xen emulates Hyper-V to support enlightened Windows.
-	 * Check to see first if we are on a Xen Hypervisor.
-	 */
-	if (xen_cpuid_base())
-		return false;
-
 	cpuid(HYPERV_CPUID_VENDOR_AND_MAX_FUNCTIONS,
 	      &eax, &hyp_signature[0], &hyp_signature[1], &hyp_signature[2]);
 
@@ -82,12 +75,6 @@ static void __init ms_hyperv_init_platform(void)
 
 	if (ms_hyperv.features & HV_X64_MSR_TIME_REF_COUNT_AVAILABLE)
 		clocksource_register_hz(&hyperv_cs, NSEC_PER_SEC/100);
-#if IS_ENABLED(CONFIG_HYPERV)
-	/*
-	 * Setup the IDT for hypervisor callback.
-	 */
-	alloc_intr_gate(HYPERVISOR_CALLBACK_VECTOR, hyperv_callback_vector);
-#endif
 }
 
 const __refconst struct hypervisor_x86 x86_hyper_ms_hyperv = {
@@ -103,6 +90,11 @@ static irq_handler_t vmbus_isr;
 
 void hv_register_vmbus_handler(int irq, irq_handler_t handler)
 {
+	/*
+	 * Setup the IDT for hypervisor callback.
+	 */
+	alloc_intr_gate(HYPERVISOR_CALLBACK_VECTOR, hyperv_callback_vector);
+
 	vmbus_irq = irq;
 	vmbus_isr = handler;
 }
diff --git a/arch/x86/kernel/cpu/perf_event.c b/arch/x86/kernel/cpu/perf_event.c
index bf0f01aea99..1025f3c99d2 100644
--- a/arch/x86/kernel/cpu/perf_event.c
+++ b/arch/x86/kernel/cpu/perf_event.c
@@ -180,8 +180,9 @@ static void release_pmc_hardware(void) {}
 
 static bool check_hw_exists(void)
 {
-	u64 val, val_new = ~0;
-	int i, reg, ret = 0;
+	u64 val, val_fail, val_new= ~0;
+	int i, reg, reg_fail, ret = 0;
+	int bios_fail = 0;
 
 	/*
 	 * Check to see if the BIOS enabled any of the counters, if so
@@ -192,8 +193,11 @@ static bool check_hw_exists(void)
 		ret = rdmsrl_safe(reg, &val);
 		if (ret)
 			goto msr_fail;
-		if (val & ARCH_PERFMON_EVENTSEL_ENABLE)
-			goto bios_fail;
+		if (val & ARCH_PERFMON_EVENTSEL_ENABLE) {
+			bios_fail = 1;
+			val_fail = val;
+			reg_fail = reg;
+		}
 	}
 
 	if (x86_pmu.num_counters_fixed) {
@@ -202,8 +206,11 @@ static bool check_hw_exists(void)
 		if (ret)
 			goto msr_fail;
 		for (i = 0; i < x86_pmu.num_counters_fixed; i++) {
-			if (val & (0x03 << i*4))
-				goto bios_fail;
+			if (val & (0x03 << i*4)) {
+				bios_fail = 1;
+				val_fail = val;
+				reg_fail = reg;
+			}
 		}
 	}
 
@@ -221,14 +228,13 @@ static bool check_hw_exists(void)
 	if (ret || val != val_new)
 		goto msr_fail;
 
-	return true;
-
-bios_fail:
 	/*
 	 * We still allow the PMU driver to operate:
 	 */
-	printk(KERN_CONT "Broken BIOS detected, complain to your hardware vendor.\n");
-	printk(KERN_ERR FW_BUG "the BIOS has corrupted hw-PMU resources (MSR %x is %Lx)\n", reg, val);
+	if (bios_fail) {
+		printk(KERN_CONT "Broken BIOS detected, complain to your hardware vendor.\n");
+		printk(KERN_ERR FW_BUG "the BIOS has corrupted hw-PMU resources (MSR %x is %Lx)\n", reg_fail, val_fail);
+	}
 
 	return true;
 
@@ -1316,9 +1322,16 @@ static struct attribute_group x86_pmu_format_group = {
  */
 static void __init filter_events(struct attribute **attrs)
 {
+	struct device_attribute *d;
+	struct perf_pmu_events_attr *pmu_attr;
 	int i, j;
 
 	for (i = 0; attrs[i]; i++) {
+		d = (struct device_attribute *)attrs[i];
+		pmu_attr = container_of(d, struct perf_pmu_events_attr, attr);
+		/* str trumps id */
+		if (pmu_attr->event_str)
+			continue;
 		if (x86_pmu.event_map(i))
 			continue;
 
@@ -1330,22 +1343,45 @@ static void __init filter_events(struct attribute **attrs)
 	}
 }
 
-static ssize_t events_sysfs_show(struct device *dev, struct device_attribute *attr,
+/* Merge two pointer arrays */
+static __init struct attribute **merge_attr(struct attribute **a, struct attribute **b)
+{
+	struct attribute **new;
+	int j, i;
+
+	for (j = 0; a[j]; j++)
+		;
+	for (i = 0; b[i]; i++)
+		j++;
+	j++;
+
+	new = kmalloc(sizeof(struct attribute *) * j, GFP_KERNEL);
+	if (!new)
+		return NULL;
+
+	j = 0;
+	for (i = 0; a[i]; i++)
+		new[j++] = a[i];
+	for (i = 0; b[i]; i++)
+		new[j++] = b[i];
+	new[j] = NULL;
+
+	return new;
+}
+
+ssize_t events_sysfs_show(struct device *dev, struct device_attribute *attr,
 			  char *page)
 {
 	struct perf_pmu_events_attr *pmu_attr = \
 		container_of(attr, struct perf_pmu_events_attr, attr);
-
 	u64 config = x86_pmu.event_map(pmu_attr->id);
-	return x86_pmu.events_sysfs_show(page, config);
-}
 
-#define EVENT_VAR(_id)  event_attr_##_id
-#define EVENT_PTR(_id) &event_attr_##_id.attr.attr
+	/* string trumps id */
+	if (pmu_attr->event_str)
+		return sprintf(page, "%s", pmu_attr->event_str);
 
-#define EVENT_ATTR(_name, _id)						\
-	PMU_EVENT_ATTR(_name, EVENT_VAR(_id), PERF_COUNT_HW_##_id,	\
-			events_sysfs_show)
+	return x86_pmu.events_sysfs_show(page, config);
+}
 
 EVENT_ATTR(cpu-cycles,			CPU_CYCLES		);
 EVENT_ATTR(instructions,		INSTRUCTIONS		);
@@ -1459,16 +1495,27 @@ static int __init init_hw_perf_events(void)
 
 	unconstrained = (struct event_constraint)
 		__EVENT_CONSTRAINT(0, (1ULL << x86_pmu.num_counters) - 1,
-				   0, x86_pmu.num_counters, 0);
+				   0, x86_pmu.num_counters, 0, 0);
 
 	x86_pmu.attr_rdpmc = 1; /* enable userspace RDPMC usage by default */
 	x86_pmu_format_group.attrs = x86_pmu.format_attrs;
 
+	if (x86_pmu.event_attrs)
+		x86_pmu_events_group.attrs = x86_pmu.event_attrs;
+
 	if (!x86_pmu.events_sysfs_show)
 		x86_pmu_events_group.attrs = &empty_attrs;
 	else
 		filter_events(x86_pmu_events_group.attrs);
 
+	if (x86_pmu.cpu_events) {
+		struct attribute **tmp;
+
+		tmp = merge_attr(x86_pmu_events_group.attrs, x86_pmu.cpu_events);
+		if (!WARN_ON(!tmp))
+			x86_pmu_events_group.attrs = tmp;
+	}
+
 	pr_info("... version:                %d\n",     x86_pmu.version);
 	pr_info("... bit width:              %d\n",     x86_pmu.cntval_bits);
 	pr_info("... generic registers:      %d\n",     x86_pmu.num_counters);
diff --git a/arch/x86/kernel/cpu/perf_event.h b/arch/x86/kernel/cpu/perf_event.h
index 7f5c75c2afd..ba9aadfa683 100644
--- a/arch/x86/kernel/cpu/perf_event.h
+++ b/arch/x86/kernel/cpu/perf_event.h
@@ -46,6 +46,7 @@ enum extra_reg_type {
 	EXTRA_REG_RSP_0 = 0,	/* offcore_response_0 */
 	EXTRA_REG_RSP_1 = 1,	/* offcore_response_1 */
 	EXTRA_REG_LBR   = 2,	/* lbr_select */
+	EXTRA_REG_LDLAT = 3,	/* ld_lat_threshold */
 
 	EXTRA_REG_MAX		/* number of entries needed */
 };
@@ -59,7 +60,13 @@ struct event_constraint {
 	u64	cmask;
 	int	weight;
 	int	overlap;
+	int	flags;
 };
+/*
+ * struct event_constraint flags
+ */
+#define PERF_X86_EVENT_PEBS_LDLAT	0x1 /* ld+ldlat data address sampling */
+#define PERF_X86_EVENT_PEBS_ST		0x2 /* st data address sampling */
 
 struct amd_nb {
 	int nb_id;  /* NorthBridge id */
@@ -170,16 +177,17 @@ struct cpu_hw_events {
 	void				*kfree_on_online;
 };
 
-#define __EVENT_CONSTRAINT(c, n, m, w, o) {\
+#define __EVENT_CONSTRAINT(c, n, m, w, o, f) {\
 	{ .idxmsk64 = (n) },		\
 	.code = (c),			\
 	.cmask = (m),			\
 	.weight = (w),			\
 	.overlap = (o),			\
+	.flags = f,			\
 }
 
 #define EVENT_CONSTRAINT(c, n, m)	\
-	__EVENT_CONSTRAINT(c, n, m, HWEIGHT(n), 0)
+	__EVENT_CONSTRAINT(c, n, m, HWEIGHT(n), 0, 0)
 
 /*
  * The overlap flag marks event constraints with overlapping counter
@@ -203,7 +211,7 @@ struct cpu_hw_events {
  * and its counter masks must be kept at a minimum.
  */
 #define EVENT_CONSTRAINT_OVERLAP(c, n, m)	\
-	__EVENT_CONSTRAINT(c, n, m, HWEIGHT(n), 1)
+	__EVENT_CONSTRAINT(c, n, m, HWEIGHT(n), 1, 0)
 
 /*
  * Constraint on the Event code.
@@ -231,6 +239,14 @@ struct cpu_hw_events {
 #define INTEL_UEVENT_CONSTRAINT(c, n)	\
 	EVENT_CONSTRAINT(c, n, INTEL_ARCH_EVENT_MASK)
 
+#define INTEL_PLD_CONSTRAINT(c, n)	\
+	__EVENT_CONSTRAINT(c, n, INTEL_ARCH_EVENT_MASK, \
+			   HWEIGHT(n), 0, PERF_X86_EVENT_PEBS_LDLAT)
+
+#define INTEL_PST_CONSTRAINT(c, n)	\
+	__EVENT_CONSTRAINT(c, n, INTEL_ARCH_EVENT_MASK, \
+			  HWEIGHT(n), 0, PERF_X86_EVENT_PEBS_ST)
+
 #define EVENT_CONSTRAINT_END		\
 	EVENT_CONSTRAINT(0, 0, 0)
 
@@ -260,12 +276,22 @@ struct extra_reg {
 	.msr = (ms),		\
 	.config_mask = (m),	\
 	.valid_mask = (vm),	\
-	.idx = EXTRA_REG_##i	\
+	.idx = EXTRA_REG_##i,	\
 	}
 
 #define INTEL_EVENT_EXTRA_REG(event, msr, vm, idx)	\
 	EVENT_EXTRA_REG(event, msr, ARCH_PERFMON_EVENTSEL_EVENT, vm, idx)
 
+#define INTEL_UEVENT_EXTRA_REG(event, msr, vm, idx) \
+	EVENT_EXTRA_REG(event, msr, ARCH_PERFMON_EVENTSEL_EVENT | \
+			ARCH_PERFMON_EVENTSEL_UMASK, vm, idx)
+
+#define INTEL_UEVENT_PEBS_LDLAT_EXTRA_REG(c) \
+	INTEL_UEVENT_EXTRA_REG(c, \
+			       MSR_PEBS_LD_LAT_THRESHOLD, \
+			       0xffff, \
+			       LDLAT)
+
 #define EVENT_EXTRA_END EVENT_EXTRA_REG(0, 0, 0, 0, RSP_0)
 
 union perf_capabilities {
@@ -355,8 +381,10 @@ struct x86_pmu {
 	 */
 	int		attr_rdpmc;
 	struct attribute **format_attrs;
+	struct attribute **event_attrs;
 
 	ssize_t		(*events_sysfs_show)(char *page, u64 config);
+	struct attribute **cpu_events;
 
 	/*
 	 * CPU Hotplug hooks
@@ -421,6 +449,23 @@ do {									\
 #define ERF_NO_HT_SHARING	1
 #define ERF_HAS_RSP_1		2
 
+#define EVENT_VAR(_id)  event_attr_##_id
+#define EVENT_PTR(_id) &event_attr_##_id.attr.attr
+
+#define EVENT_ATTR(_name, _id)						\
+static struct perf_pmu_events_attr EVENT_VAR(_id) = {			\
+	.attr		= __ATTR(_name, 0444, events_sysfs_show, NULL),	\
+	.id		= PERF_COUNT_HW_##_id,				\
+	.event_str	= NULL,						\
+};
+
+#define EVENT_ATTR_STR(_name, v, str)					\
+static struct perf_pmu_events_attr event_attr_##v = {			\
+	.attr		= __ATTR(_name, 0444, events_sysfs_show, NULL),	\
+	.id		= 0,						\
+	.event_str	= str,						\
+};
+
 extern struct x86_pmu x86_pmu __read_mostly;
 
 DECLARE_PER_CPU(struct cpu_hw_events, cpu_hw_events);
@@ -628,6 +673,9 @@ int p6_pmu_init(void);
 
 int knc_pmu_init(void);
 
+ssize_t events_sysfs_show(struct device *dev, struct device_attribute *attr,
+			  char *page);
+
 #else /* CONFIG_CPU_SUP_INTEL */
 
 static inline void reserve_ds_buffers(void)
diff --git a/arch/x86/kernel/cpu/perf_event_amd.c b/arch/x86/kernel/cpu/perf_event_amd.c
index dfdab42aed2..7e28d9467bb 100644
--- a/arch/x86/kernel/cpu/perf_event_amd.c
+++ b/arch/x86/kernel/cpu/perf_event_amd.c
@@ -132,14 +132,11 @@ static u64 amd_pmu_event_map(int hw_event)
 	return amd_perfmon_event_map[hw_event];
 }
 
-static struct event_constraint *amd_nb_event_constraint;
-
 /*
  * Previously calculated offsets
  */
 static unsigned int event_offsets[X86_PMC_IDX_MAX] __read_mostly;
 static unsigned int count_offsets[X86_PMC_IDX_MAX] __read_mostly;
-static unsigned int rdpmc_indexes[X86_PMC_IDX_MAX] __read_mostly;
 
 /*
  * Legacy CPUs:
@@ -147,14 +144,10 @@ static unsigned int rdpmc_indexes[X86_PMC_IDX_MAX] __read_mostly;
  *
  * CPUs with core performance counter extensions:
  *   6 counters starting at 0xc0010200 each offset by 2
- *
- * CPUs with north bridge performance counter extensions:
- *   4 additional counters starting at 0xc0010240 each offset by 2
- *   (indexed right above either one of the above core counters)
  */
 static inline int amd_pmu_addr_offset(int index, bool eventsel)
 {
-	int offset, first, base;
+	int offset;
 
 	if (!index)
 		return index;
@@ -167,23 +160,7 @@ static inline int amd_pmu_addr_offset(int index, bool eventsel)
 	if (offset)
 		return offset;
 
-	if (amd_nb_event_constraint &&
-	    test_bit(index, amd_nb_event_constraint->idxmsk)) {
-		/*
-		 * calculate the offset of NB counters with respect to
-		 * base eventsel or perfctr
-		 */
-
-		first = find_first_bit(amd_nb_event_constraint->idxmsk,
-				       X86_PMC_IDX_MAX);
-
-		if (eventsel)
-			base = MSR_F15H_NB_PERF_CTL - x86_pmu.eventsel;
-		else
-			base = MSR_F15H_NB_PERF_CTR - x86_pmu.perfctr;
-
-		offset = base + ((index - first) << 1);
-	} else if (!cpu_has_perfctr_core)
+	if (!cpu_has_perfctr_core)
 		offset = index;
 	else
 		offset = index << 1;
@@ -196,36 +173,6 @@ static inline int amd_pmu_addr_offset(int index, bool eventsel)
 	return offset;
 }
 
-static inline int amd_pmu_rdpmc_index(int index)
-{
-	int ret, first;
-
-	if (!index)
-		return index;
-
-	ret = rdpmc_indexes[index];
-
-	if (ret)
-		return ret;
-
-	if (amd_nb_event_constraint &&
-	    test_bit(index, amd_nb_event_constraint->idxmsk)) {
-		/*
-		 * according to the mnual, ECX value of the NB counters is
-		 * the index of the NB counter (0, 1, 2 or 3) plus 6
-		 */
-
-		first = find_first_bit(amd_nb_event_constraint->idxmsk,
-				       X86_PMC_IDX_MAX);
-		ret = index - first + 6;
-	} else
-		ret = index;
-
-	rdpmc_indexes[index] = ret;
-
-	return ret;
-}
-
 static int amd_core_hw_config(struct perf_event *event)
 {
 	if (event->attr.exclude_host && event->attr.exclude_guest)
@@ -245,34 +192,6 @@ static int amd_core_hw_config(struct perf_event *event)
 }
 
 /*
- * NB counters do not support the following event select bits:
- *   Host/Guest only
- *   Counter mask
- *   Invert counter mask
- *   Edge detect
- *   OS/User mode
- */
-static int amd_nb_hw_config(struct perf_event *event)
-{
-	/* for NB, we only allow system wide counting mode */
-	if (is_sampling_event(event) || event->attach_state & PERF_ATTACH_TASK)
-		return -EINVAL;
-
-	if (event->attr.exclude_user || event->attr.exclude_kernel ||
-	    event->attr.exclude_host || event->attr.exclude_guest)
-		return -EINVAL;
-
-	event->hw.config &= ~(ARCH_PERFMON_EVENTSEL_USR |
-			      ARCH_PERFMON_EVENTSEL_OS);
-
-	if (event->hw.config & ~(AMD64_RAW_EVENT_MASK_NB |
-				 ARCH_PERFMON_EVENTSEL_INT))
-		return -EINVAL;
-
-	return 0;
-}
-
-/*
  * AMD64 events are detected based on their event codes.
  */
 static inline unsigned int amd_get_event_code(struct hw_perf_event *hwc)
@@ -285,11 +204,6 @@ static inline int amd_is_nb_event(struct hw_perf_event *hwc)
 	return (hwc->config & 0xe0) == 0xe0;
 }
 
-static inline int amd_is_perfctr_nb_event(struct hw_perf_event *hwc)
-{
-	return amd_nb_event_constraint && amd_is_nb_event(hwc);
-}
-
 static inline int amd_has_nb(struct cpu_hw_events *cpuc)
 {
 	struct amd_nb *nb = cpuc->amd_nb;
@@ -315,9 +229,6 @@ static int amd_pmu_hw_config(struct perf_event *event)
 	if (event->attr.type == PERF_TYPE_RAW)
 		event->hw.config |= event->attr.config & AMD64_RAW_EVENT_MASK;
 
-	if (amd_is_perfctr_nb_event(&event->hw))
-		return amd_nb_hw_config(event);
-
 	return amd_core_hw_config(event);
 }
 
@@ -341,19 +252,6 @@ static void __amd_put_nb_event_constraints(struct cpu_hw_events *cpuc,
 	}
 }
 
-static void amd_nb_interrupt_hw_config(struct hw_perf_event *hwc)
-{
-	int core_id = cpu_data(smp_processor_id()).cpu_core_id;
-
-	/* deliver interrupts only to this core */
-	if (hwc->config & ARCH_PERFMON_EVENTSEL_INT) {
-		hwc->config |= AMD64_EVENTSEL_INT_CORE_ENABLE;
-		hwc->config &= ~AMD64_EVENTSEL_INT_CORE_SEL_MASK;
-		hwc->config |= (u64)(core_id) <<
-			AMD64_EVENTSEL_INT_CORE_SEL_SHIFT;
-	}
-}
-
  /*
   * AMD64 NorthBridge events need special treatment because
   * counter access needs to be synchronized across all cores
@@ -441,9 +339,6 @@ __amd_get_nb_event_constraints(struct cpu_hw_events *cpuc, struct perf_event *ev
 	if (new == -1)
 		return &emptyconstraint;
 
-	if (amd_is_perfctr_nb_event(hwc))
-		amd_nb_interrupt_hw_config(hwc);
-
 	return &nb->event_constraints[new];
 }
 
@@ -543,8 +438,7 @@ amd_get_event_constraints(struct cpu_hw_events *cpuc, struct perf_event *event)
 	if (!(amd_has_nb(cpuc) && amd_is_nb_event(&event->hw)))
 		return &unconstrained;
 
-	return __amd_get_nb_event_constraints(cpuc, event,
-					      amd_nb_event_constraint);
+	return __amd_get_nb_event_constraints(cpuc, event, NULL);
 }
 
 static void amd_put_event_constraints(struct cpu_hw_events *cpuc,
@@ -643,9 +537,6 @@ static struct event_constraint amd_f15_PMC30 = EVENT_CONSTRAINT_OVERLAP(0, 0x09,
 static struct event_constraint amd_f15_PMC50 = EVENT_CONSTRAINT(0, 0x3F, 0);
 static struct event_constraint amd_f15_PMC53 = EVENT_CONSTRAINT(0, 0x38, 0);
 
-static struct event_constraint amd_NBPMC96 = EVENT_CONSTRAINT(0, 0x3C0, 0);
-static struct event_constraint amd_NBPMC74 = EVENT_CONSTRAINT(0, 0xF0, 0);
-
 static struct event_constraint *
 amd_get_event_constraints_f15h(struct cpu_hw_events *cpuc, struct perf_event *event)
 {
@@ -711,8 +602,8 @@ amd_get_event_constraints_f15h(struct cpu_hw_events *cpuc, struct perf_event *ev
 			return &amd_f15_PMC20;
 		}
 	case AMD_EVENT_NB:
-		return __amd_get_nb_event_constraints(cpuc, event,
-						      amd_nb_event_constraint);
+		/* moved to perf_event_amd_uncore.c */
+		return &emptyconstraint;
 	default:
 		return &emptyconstraint;
 	}
@@ -738,7 +629,6 @@ static __initconst const struct x86_pmu amd_pmu = {
 	.eventsel		= MSR_K7_EVNTSEL0,
 	.perfctr		= MSR_K7_PERFCTR0,
 	.addr_offset            = amd_pmu_addr_offset,
-	.rdpmc_index		= amd_pmu_rdpmc_index,
 	.event_map		= amd_pmu_event_map,
 	.max_events		= ARRAY_SIZE(amd_perfmon_event_map),
 	.num_counters		= AMD64_NUM_COUNTERS,
@@ -790,23 +680,6 @@ static int setup_perfctr_core(void)
 	return 0;
 }
 
-static int setup_perfctr_nb(void)
-{
-	if (!cpu_has_perfctr_nb)
-		return -ENODEV;
-
-	x86_pmu.num_counters += AMD64_NUM_COUNTERS_NB;
-
-	if (cpu_has_perfctr_core)
-		amd_nb_event_constraint = &amd_NBPMC96;
-	else
-		amd_nb_event_constraint = &amd_NBPMC74;
-
-	printk(KERN_INFO "perf: AMD northbridge performance counters detected\n");
-
-	return 0;
-}
-
 __init int amd_pmu_init(void)
 {
 	/* Performance-monitoring supported from K7 and later: */
@@ -817,7 +690,6 @@ __init int amd_pmu_init(void)
 
 	setup_event_constraints();
 	setup_perfctr_core();
-	setup_perfctr_nb();
 
 	/* Events are common for all AMDs */
 	memcpy(hw_cache_event_ids, amd_hw_cache_event_ids,
diff --git a/arch/x86/kernel/cpu/perf_event_amd_uncore.c b/arch/x86/kernel/cpu/perf_event_amd_uncore.c
new file mode 100644
index 00000000000..c0c661adf03
--- /dev/null
+++ b/arch/x86/kernel/cpu/perf_event_amd_uncore.c
@@ -0,0 +1,547 @@
+/*
+ * Copyright (C) 2013 Advanced Micro Devices, Inc.
+ *
+ * Author: Jacob Shin <jacob.shin@amd.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/perf_event.h>
+#include <linux/percpu.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/cpu.h>
+#include <linux/cpumask.h>
+
+#include <asm/cpufeature.h>
+#include <asm/perf_event.h>
+#include <asm/msr.h>
+
+#define NUM_COUNTERS_NB		4
+#define NUM_COUNTERS_L2		4
+#define MAX_COUNTERS		NUM_COUNTERS_NB
+
+#define RDPMC_BASE_NB		6
+#define RDPMC_BASE_L2		10
+
+#define COUNTER_SHIFT		16
+
+struct amd_uncore {
+	int id;
+	int refcnt;
+	int cpu;
+	int num_counters;
+	int rdpmc_base;
+	u32 msr_base;
+	cpumask_t *active_mask;
+	struct pmu *pmu;
+	struct perf_event *events[MAX_COUNTERS];
+	struct amd_uncore *free_when_cpu_online;
+};
+
+static struct amd_uncore * __percpu *amd_uncore_nb;
+static struct amd_uncore * __percpu *amd_uncore_l2;
+
+static struct pmu amd_nb_pmu;
+static struct pmu amd_l2_pmu;
+
+static cpumask_t amd_nb_active_mask;
+static cpumask_t amd_l2_active_mask;
+
+static bool is_nb_event(struct perf_event *event)
+{
+	return event->pmu->type == amd_nb_pmu.type;
+}
+
+static bool is_l2_event(struct perf_event *event)
+{
+	return event->pmu->type == amd_l2_pmu.type;
+}
+
+static struct amd_uncore *event_to_amd_uncore(struct perf_event *event)
+{
+	if (is_nb_event(event) && amd_uncore_nb)
+		return *per_cpu_ptr(amd_uncore_nb, event->cpu);
+	else if (is_l2_event(event) && amd_uncore_l2)
+		return *per_cpu_ptr(amd_uncore_l2, event->cpu);
+
+	return NULL;
+}
+
+static void amd_uncore_read(struct perf_event *event)
+{
+	struct hw_perf_event *hwc = &event->hw;
+	u64 prev, new;
+	s64 delta;
+
+	/*
+	 * since we do not enable counter overflow interrupts,
+	 * we do not have to worry about prev_count changing on us
+	 */
+
+	prev = local64_read(&hwc->prev_count);
+	rdpmcl(hwc->event_base_rdpmc, new);
+	local64_set(&hwc->prev_count, new);
+	delta = (new << COUNTER_SHIFT) - (prev << COUNTER_SHIFT);
+	delta >>= COUNTER_SHIFT;
+	local64_add(delta, &event->count);
+}
+
+static void amd_uncore_start(struct perf_event *event, int flags)
+{
+	struct hw_perf_event *hwc = &event->hw;
+
+	if (flags & PERF_EF_RELOAD)
+		wrmsrl(hwc->event_base, (u64)local64_read(&hwc->prev_count));
+
+	hwc->state = 0;
+	wrmsrl(hwc->config_base, (hwc->config | ARCH_PERFMON_EVENTSEL_ENABLE));
+	perf_event_update_userpage(event);
+}
+
+static void amd_uncore_stop(struct perf_event *event, int flags)
+{
+	struct hw_perf_event *hwc = &event->hw;
+
+	wrmsrl(hwc->config_base, hwc->config);
+	hwc->state |= PERF_HES_STOPPED;
+
+	if ((flags & PERF_EF_UPDATE) && !(hwc->state & PERF_HES_UPTODATE)) {
+		amd_uncore_read(event);
+		hwc->state |= PERF_HES_UPTODATE;
+	}
+}
+
+static int amd_uncore_add(struct perf_event *event, int flags)
+{
+	int i;
+	struct amd_uncore *uncore = event_to_amd_uncore(event);
+	struct hw_perf_event *hwc = &event->hw;
+
+	/* are we already assigned? */
+	if (hwc->idx != -1 && uncore->events[hwc->idx] == event)
+		goto out;
+
+	for (i = 0; i < uncore->num_counters; i++) {
+		if (uncore->events[i] == event) {
+			hwc->idx = i;
+			goto out;
+		}
+	}
+
+	/* if not, take the first available counter */
+	hwc->idx = -1;
+	for (i = 0; i < uncore->num_counters; i++) {
+		if (cmpxchg(&uncore->events[i], NULL, event) == NULL) {
+			hwc->idx = i;
+			break;
+		}
+	}
+
+out:
+	if (hwc->idx == -1)
+		return -EBUSY;
+
+	hwc->config_base = uncore->msr_base + (2 * hwc->idx);
+	hwc->event_base = uncore->msr_base + 1 + (2 * hwc->idx);
+	hwc->event_base_rdpmc = uncore->rdpmc_base + hwc->idx;
+	hwc->state = PERF_HES_UPTODATE | PERF_HES_STOPPED;
+
+	if (flags & PERF_EF_START)
+		amd_uncore_start(event, PERF_EF_RELOAD);
+
+	return 0;
+}
+
+static void amd_uncore_del(struct perf_event *event, int flags)
+{
+	int i;
+	struct amd_uncore *uncore = event_to_amd_uncore(event);
+	struct hw_perf_event *hwc = &event->hw;
+
+	amd_uncore_stop(event, PERF_EF_UPDATE);
+
+	for (i = 0; i < uncore->num_counters; i++) {
+		if (cmpxchg(&uncore->events[i], event, NULL) == event)
+			break;
+	}
+
+	hwc->idx = -1;
+}
+
+static int amd_uncore_event_init(struct perf_event *event)
+{
+	struct amd_uncore *uncore;
+	struct hw_perf_event *hwc = &event->hw;
+
+	if (event->attr.type != event->pmu->type)
+		return -ENOENT;
+
+	/*
+	 * NB and L2 counters (MSRs) are shared across all cores that share the
+	 * same NB / L2 cache. Interrupts can be directed to a single target
+	 * core, however, event counts generated by processes running on other
+	 * cores cannot be masked out. So we do not support sampling and
+	 * per-thread events.
+	 */
+	if (is_sampling_event(event) || event->attach_state & PERF_ATTACH_TASK)
+		return -EINVAL;
+
+	/* NB and L2 counters do not have usr/os/guest/host bits */
+	if (event->attr.exclude_user || event->attr.exclude_kernel ||
+	    event->attr.exclude_host || event->attr.exclude_guest)
+		return -EINVAL;
+
+	/* and we do not enable counter overflow interrupts */
+	hwc->config = event->attr.config & AMD64_RAW_EVENT_MASK_NB;
+	hwc->idx = -1;
+
+	if (event->cpu < 0)
+		return -EINVAL;
+
+	uncore = event_to_amd_uncore(event);
+	if (!uncore)
+		return -ENODEV;
+
+	/*
+	 * since request can come in to any of the shared cores, we will remap
+	 * to a single common cpu.
+	 */
+	event->cpu = uncore->cpu;
+
+	return 0;
+}
+
+static ssize_t amd_uncore_attr_show_cpumask(struct device *dev,
+					    struct device_attribute *attr,
+					    char *buf)
+{
+	int n;
+	cpumask_t *active_mask;
+	struct pmu *pmu = dev_get_drvdata(dev);
+
+	if (pmu->type == amd_nb_pmu.type)
+		active_mask = &amd_nb_active_mask;
+	else if (pmu->type == amd_l2_pmu.type)
+		active_mask = &amd_l2_active_mask;
+	else
+		return 0;
+
+	n = cpulist_scnprintf(buf, PAGE_SIZE - 2, active_mask);
+	buf[n++] = '\n';
+	buf[n] = '\0';
+	return n;
+}
+static DEVICE_ATTR(cpumask, S_IRUGO, amd_uncore_attr_show_cpumask, NULL);
+
+static struct attribute *amd_uncore_attrs[] = {
+	&dev_attr_cpumask.attr,
+	NULL,
+};
+
+static struct attribute_group amd_uncore_attr_group = {
+	.attrs = amd_uncore_attrs,
+};
+
+PMU_FORMAT_ATTR(event, "config:0-7,32-35");
+PMU_FORMAT_ATTR(umask, "config:8-15");
+
+static struct attribute *amd_uncore_format_attr[] = {
+	&format_attr_event.attr,
+	&format_attr_umask.attr,
+	NULL,
+};
+
+static struct attribute_group amd_uncore_format_group = {
+	.name = "format",
+	.attrs = amd_uncore_format_attr,
+};
+
+static const struct attribute_group *amd_uncore_attr_groups[] = {
+	&amd_uncore_attr_group,
+	&amd_uncore_format_group,
+	NULL,
+};
+
+static struct pmu amd_nb_pmu = {
+	.attr_groups	= amd_uncore_attr_groups,
+	.name		= "amd_nb",
+	.event_init	= amd_uncore_event_init,
+	.add		= amd_uncore_add,
+	.del		= amd_uncore_del,
+	.start		= amd_uncore_start,
+	.stop		= amd_uncore_stop,
+	.read		= amd_uncore_read,
+};
+
+static struct pmu amd_l2_pmu = {
+	.attr_groups	= amd_uncore_attr_groups,
+	.name		= "amd_l2",
+	.event_init	= amd_uncore_event_init,
+	.add		= amd_uncore_add,
+	.del		= amd_uncore_del,
+	.start		= amd_uncore_start,
+	.stop		= amd_uncore_stop,
+	.read		= amd_uncore_read,
+};
+
+static struct amd_uncore * __cpuinit amd_uncore_alloc(unsigned int cpu)
+{
+	return kzalloc_node(sizeof(struct amd_uncore), GFP_KERNEL,
+			cpu_to_node(cpu));
+}
+
+static void __cpuinit amd_uncore_cpu_up_prepare(unsigned int cpu)
+{
+	struct amd_uncore *uncore;
+
+	if (amd_uncore_nb) {
+		uncore = amd_uncore_alloc(cpu);
+		uncore->cpu = cpu;
+		uncore->num_counters = NUM_COUNTERS_NB;
+		uncore->rdpmc_base = RDPMC_BASE_NB;
+		uncore->msr_base = MSR_F15H_NB_PERF_CTL;
+		uncore->active_mask = &amd_nb_active_mask;
+		uncore->pmu = &amd_nb_pmu;
+		*per_cpu_ptr(amd_uncore_nb, cpu) = uncore;
+	}
+
+	if (amd_uncore_l2) {
+		uncore = amd_uncore_alloc(cpu);
+		uncore->cpu = cpu;
+		uncore->num_counters = NUM_COUNTERS_L2;
+		uncore->rdpmc_base = RDPMC_BASE_L2;
+		uncore->msr_base = MSR_F16H_L2I_PERF_CTL;
+		uncore->active_mask = &amd_l2_active_mask;
+		uncore->pmu = &amd_l2_pmu;
+		*per_cpu_ptr(amd_uncore_l2, cpu) = uncore;
+	}
+}
+
+static struct amd_uncore *
+__cpuinit amd_uncore_find_online_sibling(struct amd_uncore *this,
+					 struct amd_uncore * __percpu *uncores)
+{
+	unsigned int cpu;
+	struct amd_uncore *that;
+
+	for_each_online_cpu(cpu) {
+		that = *per_cpu_ptr(uncores, cpu);
+
+		if (!that)
+			continue;
+
+		if (this == that)
+			continue;
+
+		if (this->id == that->id) {
+			that->free_when_cpu_online = this;
+			this = that;
+			break;
+		}
+	}
+
+	this->refcnt++;
+	return this;
+}
+
+static void __cpuinit amd_uncore_cpu_starting(unsigned int cpu)
+{
+	unsigned int eax, ebx, ecx, edx;
+	struct amd_uncore *uncore;
+
+	if (amd_uncore_nb) {
+		uncore = *per_cpu_ptr(amd_uncore_nb, cpu);
+		cpuid(0x8000001e, &eax, &ebx, &ecx, &edx);
+		uncore->id = ecx & 0xff;
+
+		uncore = amd_uncore_find_online_sibling(uncore, amd_uncore_nb);
+		*per_cpu_ptr(amd_uncore_nb, cpu) = uncore;
+	}
+
+	if (amd_uncore_l2) {
+		unsigned int apicid = cpu_data(cpu).apicid;
+		unsigned int nshared;
+
+		uncore = *per_cpu_ptr(amd_uncore_l2, cpu);
+		cpuid_count(0x8000001d, 2, &eax, &ebx, &ecx, &edx);
+		nshared = ((eax >> 14) & 0xfff) + 1;
+		uncore->id = apicid - (apicid % nshared);
+
+		uncore = amd_uncore_find_online_sibling(uncore, amd_uncore_l2);
+		*per_cpu_ptr(amd_uncore_l2, cpu) = uncore;
+	}
+}
+
+static void __cpuinit uncore_online(unsigned int cpu,
+				    struct amd_uncore * __percpu *uncores)
+{
+	struct amd_uncore *uncore = *per_cpu_ptr(uncores, cpu);
+
+	kfree(uncore->free_when_cpu_online);
+	uncore->free_when_cpu_online = NULL;
+
+	if (cpu == uncore->cpu)
+		cpumask_set_cpu(cpu, uncore->active_mask);
+}
+
+static void __cpuinit amd_uncore_cpu_online(unsigned int cpu)
+{
+	if (amd_uncore_nb)
+		uncore_online(cpu, amd_uncore_nb);
+
+	if (amd_uncore_l2)
+		uncore_online(cpu, amd_uncore_l2);
+}
+
+static void __cpuinit uncore_down_prepare(unsigned int cpu,
+					  struct amd_uncore * __percpu *uncores)
+{
+	unsigned int i;
+	struct amd_uncore *this = *per_cpu_ptr(uncores, cpu);
+
+	if (this->cpu != cpu)
+		return;
+
+	/* this cpu is going down, migrate to a shared sibling if possible */
+	for_each_online_cpu(i) {
+		struct amd_uncore *that = *per_cpu_ptr(uncores, i);
+
+		if (cpu == i)
+			continue;
+
+		if (this == that) {
+			perf_pmu_migrate_context(this->pmu, cpu, i);
+			cpumask_clear_cpu(cpu, that->active_mask);
+			cpumask_set_cpu(i, that->active_mask);
+			that->cpu = i;
+			break;
+		}
+	}
+}
+
+static void __cpuinit amd_uncore_cpu_down_prepare(unsigned int cpu)
+{
+	if (amd_uncore_nb)
+		uncore_down_prepare(cpu, amd_uncore_nb);
+
+	if (amd_uncore_l2)
+		uncore_down_prepare(cpu, amd_uncore_l2);
+}
+
+static void __cpuinit uncore_dead(unsigned int cpu,
+				  struct amd_uncore * __percpu *uncores)
+{
+	struct amd_uncore *uncore = *per_cpu_ptr(uncores, cpu);
+
+	if (cpu == uncore->cpu)
+		cpumask_clear_cpu(cpu, uncore->active_mask);
+
+	if (!--uncore->refcnt)
+		kfree(uncore);
+	*per_cpu_ptr(amd_uncore_nb, cpu) = NULL;
+}
+
+static void __cpuinit amd_uncore_cpu_dead(unsigned int cpu)
+{
+	if (amd_uncore_nb)
+		uncore_dead(cpu, amd_uncore_nb);
+
+	if (amd_uncore_l2)
+		uncore_dead(cpu, amd_uncore_l2);
+}
+
+static int __cpuinit
+amd_uncore_cpu_notifier(struct notifier_block *self, unsigned long action,
+			void *hcpu)
+{
+	unsigned int cpu = (long)hcpu;
+
+	switch (action & ~CPU_TASKS_FROZEN) {
+	case CPU_UP_PREPARE:
+		amd_uncore_cpu_up_prepare(cpu);
+		break;
+
+	case CPU_STARTING:
+		amd_uncore_cpu_starting(cpu);
+		break;
+
+	case CPU_ONLINE:
+		amd_uncore_cpu_online(cpu);
+		break;
+
+	case CPU_DOWN_PREPARE:
+		amd_uncore_cpu_down_prepare(cpu);
+		break;
+
+	case CPU_UP_CANCELED:
+	case CPU_DEAD:
+		amd_uncore_cpu_dead(cpu);
+		break;
+
+	default:
+		break;
+	}
+
+	return NOTIFY_OK;
+}
+
+static struct notifier_block amd_uncore_cpu_notifier_block __cpuinitdata = {
+	.notifier_call	= amd_uncore_cpu_notifier,
+	.priority	= CPU_PRI_PERF + 1,
+};
+
+static void __init init_cpu_already_online(void *dummy)
+{
+	unsigned int cpu = smp_processor_id();
+
+	amd_uncore_cpu_starting(cpu);
+	amd_uncore_cpu_online(cpu);
+}
+
+static int __init amd_uncore_init(void)
+{
+	unsigned int cpu;
+	int ret = -ENODEV;
+
+	if (boot_cpu_data.x86_vendor != X86_VENDOR_AMD)
+		return -ENODEV;
+
+	if (!cpu_has_topoext)
+		return -ENODEV;
+
+	if (cpu_has_perfctr_nb) {
+		amd_uncore_nb = alloc_percpu(struct amd_uncore *);
+		perf_pmu_register(&amd_nb_pmu, amd_nb_pmu.name, -1);
+
+		printk(KERN_INFO "perf: AMD NB counters detected\n");
+		ret = 0;
+	}
+
+	if (cpu_has_perfctr_l2) {
+		amd_uncore_l2 = alloc_percpu(struct amd_uncore *);
+		perf_pmu_register(&amd_l2_pmu, amd_l2_pmu.name, -1);
+
+		printk(KERN_INFO "perf: AMD L2I counters detected\n");
+		ret = 0;
+	}
+
+	if (ret)
+		return -ENODEV;
+
+	get_online_cpus();
+	/* init cpus already online before registering for hotplug notifier */
+	for_each_online_cpu(cpu) {
+		amd_uncore_cpu_up_prepare(cpu);
+		smp_call_function_single(cpu, init_cpu_already_online, NULL, 1);
+	}
+
+	register_cpu_notifier(&amd_uncore_cpu_notifier_block);
+	put_online_cpus();
+
+	return 0;
+}
+device_initcall(amd_uncore_init);
diff --git a/arch/x86/kernel/cpu/perf_event_intel.c b/arch/x86/kernel/cpu/perf_event_intel.c
index dab7580c47a..ffd6050a1de 100644
--- a/arch/x86/kernel/cpu/perf_event_intel.c
+++ b/arch/x86/kernel/cpu/perf_event_intel.c
@@ -81,6 +81,7 @@ static struct event_constraint intel_nehalem_event_constraints[] __read_mostly =
 static struct extra_reg intel_nehalem_extra_regs[] __read_mostly =
 {
 	INTEL_EVENT_EXTRA_REG(0xb7, MSR_OFFCORE_RSP_0, 0xffff, RSP_0),
+	INTEL_UEVENT_PEBS_LDLAT_EXTRA_REG(0x100b),
 	EVENT_EXTRA_END
 };
 
@@ -108,6 +109,8 @@ static struct event_constraint intel_snb_event_constraints[] __read_mostly =
 	INTEL_EVENT_CONSTRAINT(0x48, 0x4), /* L1D_PEND_MISS.PENDING */
 	INTEL_UEVENT_CONSTRAINT(0x01c0, 0x2), /* INST_RETIRED.PREC_DIST */
 	INTEL_EVENT_CONSTRAINT(0xcd, 0x8), /* MEM_TRANS_RETIRED.LOAD_LATENCY */
+	INTEL_UEVENT_CONSTRAINT(0x04a3, 0xf), /* CYCLE_ACTIVITY.CYCLES_NO_DISPATCH */
+	INTEL_UEVENT_CONSTRAINT(0x02a3, 0x4), /* CYCLE_ACTIVITY.CYCLES_L1D_PENDING */
 	EVENT_CONSTRAINT_END
 };
 
@@ -136,6 +139,7 @@ static struct extra_reg intel_westmere_extra_regs[] __read_mostly =
 {
 	INTEL_EVENT_EXTRA_REG(0xb7, MSR_OFFCORE_RSP_0, 0xffff, RSP_0),
 	INTEL_EVENT_EXTRA_REG(0xbb, MSR_OFFCORE_RSP_1, 0xffff, RSP_1),
+	INTEL_UEVENT_PEBS_LDLAT_EXTRA_REG(0x100b),
 	EVENT_EXTRA_END
 };
 
@@ -153,11 +157,34 @@ static struct event_constraint intel_gen_event_constraints[] __read_mostly =
 };
 
 static struct extra_reg intel_snb_extra_regs[] __read_mostly = {
-	INTEL_EVENT_EXTRA_REG(0xb7, MSR_OFFCORE_RSP_0, 0x3fffffffffull, RSP_0),
-	INTEL_EVENT_EXTRA_REG(0xbb, MSR_OFFCORE_RSP_1, 0x3fffffffffull, RSP_1),
+	INTEL_EVENT_EXTRA_REG(0xb7, MSR_OFFCORE_RSP_0, 0x3f807f8fffull, RSP_0),
+	INTEL_EVENT_EXTRA_REG(0xbb, MSR_OFFCORE_RSP_1, 0x3f807f8fffull, RSP_1),
+	INTEL_UEVENT_PEBS_LDLAT_EXTRA_REG(0x01cd),
+	INTEL_UEVENT_PEBS_LDLAT_EXTRA_REG(0x01cd),
+	EVENT_EXTRA_END
+};
+
+static struct extra_reg intel_snbep_extra_regs[] __read_mostly = {
+	INTEL_EVENT_EXTRA_REG(0xb7, MSR_OFFCORE_RSP_0, 0x3fffff8fffull, RSP_0),
+	INTEL_EVENT_EXTRA_REG(0xbb, MSR_OFFCORE_RSP_1, 0x3fffff8fffull, RSP_1),
 	EVENT_EXTRA_END
 };
 
+EVENT_ATTR_STR(mem-loads, mem_ld_nhm, "event=0x0b,umask=0x10,ldlat=3");
+EVENT_ATTR_STR(mem-loads, mem_ld_snb, "event=0xcd,umask=0x1,ldlat=3");
+EVENT_ATTR_STR(mem-stores, mem_st_snb, "event=0xcd,umask=0x2");
+
+struct attribute *nhm_events_attrs[] = {
+	EVENT_PTR(mem_ld_nhm),
+	NULL,
+};
+
+struct attribute *snb_events_attrs[] = {
+	EVENT_PTR(mem_ld_snb),
+	EVENT_PTR(mem_st_snb),
+	NULL,
+};
+
 static u64 intel_pmu_event_map(int hw_event)
 {
 	return intel_perfmon_event_map[hw_event];
@@ -1392,8 +1419,11 @@ x86_get_event_constraints(struct cpu_hw_events *cpuc, struct perf_event *event)
 
 	if (x86_pmu.event_constraints) {
 		for_each_event_constraint(c, x86_pmu.event_constraints) {
-			if ((event->hw.config & c->cmask) == c->code)
+			if ((event->hw.config & c->cmask) == c->code) {
+				/* hw.flags zeroed at initialization */
+				event->hw.flags |= c->flags;
 				return c;
+			}
 		}
 	}
 
@@ -1438,6 +1468,7 @@ intel_put_shared_regs_event_constraints(struct cpu_hw_events *cpuc,
 static void intel_put_event_constraints(struct cpu_hw_events *cpuc,
 					struct perf_event *event)
 {
+	event->hw.flags = 0;
 	intel_put_shared_regs_event_constraints(cpuc, event);
 }
 
@@ -1761,6 +1792,8 @@ static void intel_pmu_flush_branch_stack(void)
 
 PMU_FORMAT_ATTR(offcore_rsp, "config1:0-63");
 
+PMU_FORMAT_ATTR(ldlat, "config1:0-15");
+
 static struct attribute *intel_arch3_formats_attr[] = {
 	&format_attr_event.attr,
 	&format_attr_umask.attr,
@@ -1771,6 +1804,7 @@ static struct attribute *intel_arch3_formats_attr[] = {
 	&format_attr_cmask.attr,
 
 	&format_attr_offcore_rsp.attr, /* XXX do NHM/WSM + SNB breakout */
+	&format_attr_ldlat.attr, /* PEBS load latency */
 	NULL,
 };
 
@@ -2031,6 +2065,8 @@ __init int intel_pmu_init(void)
 		x86_pmu.enable_all = intel_pmu_nhm_enable_all;
 		x86_pmu.extra_regs = intel_nehalem_extra_regs;
 
+		x86_pmu.cpu_events = nhm_events_attrs;
+
 		/* UOPS_ISSUED.STALLED_CYCLES */
 		intel_perfmon_event_map[PERF_COUNT_HW_STALLED_CYCLES_FRONTEND] =
 			X86_CONFIG(.event=0x0e, .umask=0x01, .inv=1, .cmask=1);
@@ -2074,6 +2110,8 @@ __init int intel_pmu_init(void)
 		x86_pmu.extra_regs = intel_westmere_extra_regs;
 		x86_pmu.er_flags |= ERF_HAS_RSP_1;
 
+		x86_pmu.cpu_events = nhm_events_attrs;
+
 		/* UOPS_ISSUED.STALLED_CYCLES */
 		intel_perfmon_event_map[PERF_COUNT_HW_STALLED_CYCLES_FRONTEND] =
 			X86_CONFIG(.event=0x0e, .umask=0x01, .inv=1, .cmask=1);
@@ -2097,11 +2135,16 @@ __init int intel_pmu_init(void)
 		x86_pmu.event_constraints = intel_snb_event_constraints;
 		x86_pmu.pebs_constraints = intel_snb_pebs_event_constraints;
 		x86_pmu.pebs_aliases = intel_pebs_aliases_snb;
-		x86_pmu.extra_regs = intel_snb_extra_regs;
+		if (boot_cpu_data.x86_model == 45)
+			x86_pmu.extra_regs = intel_snbep_extra_regs;
+		else
+			x86_pmu.extra_regs = intel_snb_extra_regs;
 		/* all extra regs are per-cpu when HT is on */
 		x86_pmu.er_flags |= ERF_HAS_RSP_1;
 		x86_pmu.er_flags |= ERF_NO_HT_SHARING;
 
+		x86_pmu.cpu_events = snb_events_attrs;
+
 		/* UOPS_ISSUED.ANY,c=1,i=1 to count stall cycles */
 		intel_perfmon_event_map[PERF_COUNT_HW_STALLED_CYCLES_FRONTEND] =
 			X86_CONFIG(.event=0x0e, .umask=0x01, .inv=1, .cmask=1);
@@ -2123,11 +2166,16 @@ __init int intel_pmu_init(void)
 		x86_pmu.event_constraints = intel_ivb_event_constraints;
 		x86_pmu.pebs_constraints = intel_ivb_pebs_event_constraints;
 		x86_pmu.pebs_aliases = intel_pebs_aliases_snb;
-		x86_pmu.extra_regs = intel_snb_extra_regs;
+		if (boot_cpu_data.x86_model == 62)
+			x86_pmu.extra_regs = intel_snbep_extra_regs;
+		else
+			x86_pmu.extra_regs = intel_snb_extra_regs;
 		/* all extra regs are per-cpu when HT is on */
 		x86_pmu.er_flags |= ERF_HAS_RSP_1;
 		x86_pmu.er_flags |= ERF_NO_HT_SHARING;
 
+		x86_pmu.cpu_events = snb_events_attrs;
+
 		/* UOPS_ISSUED.ANY,c=1,i=1 to count stall cycles */
 		intel_perfmon_event_map[PERF_COUNT_HW_STALLED_CYCLES_FRONTEND] =
 			X86_CONFIG(.event=0x0e, .umask=0x01, .inv=1, .cmask=1);
diff --git a/arch/x86/kernel/cpu/perf_event_intel_ds.c b/arch/x86/kernel/cpu/perf_event_intel_ds.c
index b05a575d56f..60250f68705 100644
--- a/arch/x86/kernel/cpu/perf_event_intel_ds.c
+++ b/arch/x86/kernel/cpu/perf_event_intel_ds.c
@@ -24,6 +24,130 @@ struct pebs_record_32 {
 
  */
 
+union intel_x86_pebs_dse {
+	u64 val;
+	struct {
+		unsigned int ld_dse:4;
+		unsigned int ld_stlb_miss:1;
+		unsigned int ld_locked:1;
+		unsigned int ld_reserved:26;
+	};
+	struct {
+		unsigned int st_l1d_hit:1;
+		unsigned int st_reserved1:3;
+		unsigned int st_stlb_miss:1;
+		unsigned int st_locked:1;
+		unsigned int st_reserved2:26;
+	};
+};
+
+
+/*
+ * Map PEBS Load Latency Data Source encodings to generic
+ * memory data source information
+ */
+#define P(a, b) PERF_MEM_S(a, b)
+#define OP_LH (P(OP, LOAD) | P(LVL, HIT))
+#define SNOOP_NONE_MISS (P(SNOOP, NONE) | P(SNOOP, MISS))
+
+static const u64 pebs_data_source[] = {
+	P(OP, LOAD) | P(LVL, MISS) | P(LVL, L3) | P(SNOOP, NA),/* 0x00:ukn L3 */
+	OP_LH | P(LVL, L1)  | P(SNOOP, NONE),	/* 0x01: L1 local */
+	OP_LH | P(LVL, LFB) | P(SNOOP, NONE),	/* 0x02: LFB hit */
+	OP_LH | P(LVL, L2)  | P(SNOOP, NONE),	/* 0x03: L2 hit */
+	OP_LH | P(LVL, L3)  | P(SNOOP, NONE),	/* 0x04: L3 hit */
+	OP_LH | P(LVL, L3)  | P(SNOOP, MISS),	/* 0x05: L3 hit, snoop miss */
+	OP_LH | P(LVL, L3)  | P(SNOOP, HIT),	/* 0x06: L3 hit, snoop hit */
+	OP_LH | P(LVL, L3)  | P(SNOOP, HITM),	/* 0x07: L3 hit, snoop hitm */
+	OP_LH | P(LVL, REM_CCE1) | P(SNOOP, HIT),  /* 0x08: L3 miss snoop hit */
+	OP_LH | P(LVL, REM_CCE1) | P(SNOOP, HITM), /* 0x09: L3 miss snoop hitm*/
+	OP_LH | P(LVL, LOC_RAM)  | P(SNOOP, HIT),  /* 0x0a: L3 miss, shared */
+	OP_LH | P(LVL, REM_RAM1) | P(SNOOP, HIT),  /* 0x0b: L3 miss, shared */
+	OP_LH | P(LVL, LOC_RAM)  | SNOOP_NONE_MISS,/* 0x0c: L3 miss, excl */
+	OP_LH | P(LVL, REM_RAM1) | SNOOP_NONE_MISS,/* 0x0d: L3 miss, excl */
+	OP_LH | P(LVL, IO)  | P(SNOOP, NONE), /* 0x0e: I/O */
+	OP_LH | P(LVL, UNC) | P(SNOOP, NONE), /* 0x0f: uncached */
+};
+
+static u64 precise_store_data(u64 status)
+{
+	union intel_x86_pebs_dse dse;
+	u64 val = P(OP, STORE) | P(SNOOP, NA) | P(LVL, L1) | P(TLB, L2);
+
+	dse.val = status;
+
+	/*
+	 * bit 4: TLB access
+	 * 1 = stored missed 2nd level TLB
+	 *
+	 * so it either hit the walker or the OS
+	 * otherwise hit 2nd level TLB
+	 */
+	if (dse.st_stlb_miss)
+		val |= P(TLB, MISS);
+	else
+		val |= P(TLB, HIT);
+
+	/*
+	 * bit 0: hit L1 data cache
+	 * if not set, then all we know is that
+	 * it missed L1D
+	 */
+	if (dse.st_l1d_hit)
+		val |= P(LVL, HIT);
+	else
+		val |= P(LVL, MISS);
+
+	/*
+	 * bit 5: Locked prefix
+	 */
+	if (dse.st_locked)
+		val |= P(LOCK, LOCKED);
+
+	return val;
+}
+
+static u64 load_latency_data(u64 status)
+{
+	union intel_x86_pebs_dse dse;
+	u64 val;
+	int model = boot_cpu_data.x86_model;
+	int fam = boot_cpu_data.x86;
+
+	dse.val = status;
+
+	/*
+	 * use the mapping table for bit 0-3
+	 */
+	val = pebs_data_source[dse.ld_dse];
+
+	/*
+	 * Nehalem models do not support TLB, Lock infos
+	 */
+	if (fam == 0x6 && (model == 26 || model == 30
+	    || model == 31 || model == 46)) {
+		val |= P(TLB, NA) | P(LOCK, NA);
+		return val;
+	}
+	/*
+	 * bit 4: TLB access
+	 * 0 = did not miss 2nd level TLB
+	 * 1 = missed 2nd level TLB
+	 */
+	if (dse.ld_stlb_miss)
+		val |= P(TLB, MISS) | P(TLB, L2);
+	else
+		val |= P(TLB, HIT) | P(TLB, L1) | P(TLB, L2);
+
+	/*
+	 * bit 5: locked prefix
+	 */
+	if (dse.ld_locked)
+		val |= P(LOCK, LOCKED);
+
+	return val;
+}
+
 struct pebs_record_core {
 	u64 flags, ip;
 	u64 ax, bx, cx, dx;
@@ -314,10 +438,11 @@ int intel_pmu_drain_bts_buffer(void)
 	if (top <= at)
 		return 0;
 
+	memset(&regs, 0, sizeof(regs));
+
 	ds->bts_index = ds->bts_buffer_base;
 
 	perf_sample_data_init(&data, 0, event->hw.last_period);
-	regs.ip     = 0;
 
 	/*
 	 * Prepare a generic sample, i.e. fill in the invariant fields.
@@ -364,7 +489,7 @@ struct event_constraint intel_atom_pebs_event_constraints[] = {
 };
 
 struct event_constraint intel_nehalem_pebs_event_constraints[] = {
-	INTEL_EVENT_CONSTRAINT(0x0b, 0xf),    /* MEM_INST_RETIRED.* */
+	INTEL_PLD_CONSTRAINT(0x100b, 0xf),      /* MEM_INST_RETIRED.* */
 	INTEL_EVENT_CONSTRAINT(0x0f, 0xf),    /* MEM_UNCORE_RETIRED.* */
 	INTEL_UEVENT_CONSTRAINT(0x010c, 0xf), /* MEM_STORE_RETIRED.DTLB_MISS */
 	INTEL_EVENT_CONSTRAINT(0xc0, 0xf),    /* INST_RETIRED.ANY */
@@ -379,7 +504,7 @@ struct event_constraint intel_nehalem_pebs_event_constraints[] = {
 };
 
 struct event_constraint intel_westmere_pebs_event_constraints[] = {
-	INTEL_EVENT_CONSTRAINT(0x0b, 0xf),    /* MEM_INST_RETIRED.* */
+	INTEL_PLD_CONSTRAINT(0x100b, 0xf),      /* MEM_INST_RETIRED.* */
 	INTEL_EVENT_CONSTRAINT(0x0f, 0xf),    /* MEM_UNCORE_RETIRED.* */
 	INTEL_UEVENT_CONSTRAINT(0x010c, 0xf), /* MEM_STORE_RETIRED.DTLB_MISS */
 	INTEL_EVENT_CONSTRAINT(0xc0, 0xf),    /* INSTR_RETIRED.* */
@@ -399,7 +524,8 @@ struct event_constraint intel_snb_pebs_event_constraints[] = {
 	INTEL_UEVENT_CONSTRAINT(0x02c2, 0xf), /* UOPS_RETIRED.RETIRE_SLOTS */
 	INTEL_EVENT_CONSTRAINT(0xc4, 0xf),    /* BR_INST_RETIRED.* */
 	INTEL_EVENT_CONSTRAINT(0xc5, 0xf),    /* BR_MISP_RETIRED.* */
-	INTEL_EVENT_CONSTRAINT(0xcd, 0x8),    /* MEM_TRANS_RETIRED.* */
+	INTEL_PLD_CONSTRAINT(0x01cd, 0x8),    /* MEM_TRANS_RETIRED.LAT_ABOVE_THR */
+	INTEL_PST_CONSTRAINT(0x02cd, 0x8),    /* MEM_TRANS_RETIRED.PRECISE_STORES */
 	INTEL_EVENT_CONSTRAINT(0xd0, 0xf),    /* MEM_UOP_RETIRED.* */
 	INTEL_EVENT_CONSTRAINT(0xd1, 0xf),    /* MEM_LOAD_UOPS_RETIRED.* */
 	INTEL_EVENT_CONSTRAINT(0xd2, 0xf),    /* MEM_LOAD_UOPS_LLC_HIT_RETIRED.* */
@@ -413,7 +539,8 @@ struct event_constraint intel_ivb_pebs_event_constraints[] = {
         INTEL_UEVENT_CONSTRAINT(0x02c2, 0xf), /* UOPS_RETIRED.RETIRE_SLOTS */
         INTEL_EVENT_CONSTRAINT(0xc4, 0xf),    /* BR_INST_RETIRED.* */
         INTEL_EVENT_CONSTRAINT(0xc5, 0xf),    /* BR_MISP_RETIRED.* */
-        INTEL_EVENT_CONSTRAINT(0xcd, 0x8),    /* MEM_TRANS_RETIRED.* */
+        INTEL_PLD_CONSTRAINT(0x01cd, 0x8),    /* MEM_TRANS_RETIRED.LAT_ABOVE_THR */
+	INTEL_PST_CONSTRAINT(0x02cd, 0x8),    /* MEM_TRANS_RETIRED.PRECISE_STORES */
         INTEL_EVENT_CONSTRAINT(0xd0, 0xf),    /* MEM_UOP_RETIRED.* */
         INTEL_EVENT_CONSTRAINT(0xd1, 0xf),    /* MEM_LOAD_UOPS_RETIRED.* */
         INTEL_EVENT_CONSTRAINT(0xd2, 0xf),    /* MEM_LOAD_UOPS_LLC_HIT_RETIRED.* */
@@ -430,8 +557,10 @@ struct event_constraint *intel_pebs_constraints(struct perf_event *event)
 
 	if (x86_pmu.pebs_constraints) {
 		for_each_event_constraint(c, x86_pmu.pebs_constraints) {
-			if ((event->hw.config & c->cmask) == c->code)
+			if ((event->hw.config & c->cmask) == c->code) {
+				event->hw.flags |= c->flags;
 				return c;
+			}
 		}
 	}
 
@@ -446,6 +575,11 @@ void intel_pmu_pebs_enable(struct perf_event *event)
 	hwc->config &= ~ARCH_PERFMON_EVENTSEL_INT;
 
 	cpuc->pebs_enabled |= 1ULL << hwc->idx;
+
+	if (event->hw.flags & PERF_X86_EVENT_PEBS_LDLAT)
+		cpuc->pebs_enabled |= 1ULL << (hwc->idx + 32);
+	else if (event->hw.flags & PERF_X86_EVENT_PEBS_ST)
+		cpuc->pebs_enabled |= 1ULL << 63;
 }
 
 void intel_pmu_pebs_disable(struct perf_event *event)
@@ -558,20 +692,51 @@ static void __intel_pmu_pebs_event(struct perf_event *event,
 				   struct pt_regs *iregs, void *__pebs)
 {
 	/*
-	 * We cast to pebs_record_core since that is a subset of
-	 * both formats and we don't use the other fields in this
-	 * routine.
+	 * We cast to pebs_record_nhm to get the load latency data
+	 * if extra_reg MSR_PEBS_LD_LAT_THRESHOLD used
 	 */
 	struct cpu_hw_events *cpuc = &__get_cpu_var(cpu_hw_events);
-	struct pebs_record_core *pebs = __pebs;
+	struct pebs_record_nhm *pebs = __pebs;
 	struct perf_sample_data data;
 	struct pt_regs regs;
+	u64 sample_type;
+	int fll, fst;
 
 	if (!intel_pmu_save_and_restart(event))
 		return;
 
+	fll = event->hw.flags & PERF_X86_EVENT_PEBS_LDLAT;
+	fst = event->hw.flags & PERF_X86_EVENT_PEBS_ST;
+
 	perf_sample_data_init(&data, 0, event->hw.last_period);
 
+	data.period = event->hw.last_period;
+	sample_type = event->attr.sample_type;
+
+	/*
+	 * if PEBS-LL or PreciseStore
+	 */
+	if (fll || fst) {
+		if (sample_type & PERF_SAMPLE_ADDR)
+			data.addr = pebs->dla;
+
+		/*
+		 * Use latency for weight (only avail with PEBS-LL)
+		 */
+		if (fll && (sample_type & PERF_SAMPLE_WEIGHT))
+			data.weight = pebs->lat;
+
+		/*
+		 * data.data_src encodes the data source
+		 */
+		if (sample_type & PERF_SAMPLE_DATA_SRC) {
+			if (fll)
+				data.data_src.val = load_latency_data(pebs->dse);
+			else
+				data.data_src.val = precise_store_data(pebs->dse);
+		}
+	}
+
 	/*
 	 * We use the interrupt regs as a base because the PEBS record
 	 * does not contain a full regs set, specifically it seems to
diff --git a/arch/x86/kernel/cpu/perf_event_intel_uncore.c b/arch/x86/kernel/cpu/perf_event_intel_uncore.c
index b43200dbfe7..d0f9e5aa215 100644
--- a/arch/x86/kernel/cpu/perf_event_intel_uncore.c
+++ b/arch/x86/kernel/cpu/perf_event_intel_uncore.c
@@ -17,6 +17,9 @@ static struct event_constraint constraint_fixed =
 static struct event_constraint constraint_empty =
 	EVENT_CONSTRAINT(0, 0, 0);
 
+#define __BITS_VALUE(x, i, n)  ((typeof(x))(((x) >> ((i) * (n))) & \
+				((1ULL << (n)) - 1)))
+
 DEFINE_UNCORE_FORMAT_ATTR(event, event, "config:0-7");
 DEFINE_UNCORE_FORMAT_ATTR(event_ext, event, "config:0-7,21");
 DEFINE_UNCORE_FORMAT_ATTR(umask, umask, "config:8-15");
@@ -31,9 +34,13 @@ DEFINE_UNCORE_FORMAT_ATTR(occ_sel, occ_sel, "config:14-15");
 DEFINE_UNCORE_FORMAT_ATTR(occ_invert, occ_invert, "config:30");
 DEFINE_UNCORE_FORMAT_ATTR(occ_edge, occ_edge, "config:14-51");
 DEFINE_UNCORE_FORMAT_ATTR(filter_tid, filter_tid, "config1:0-4");
+DEFINE_UNCORE_FORMAT_ATTR(filter_link, filter_link, "config1:5-8");
 DEFINE_UNCORE_FORMAT_ATTR(filter_nid, filter_nid, "config1:10-17");
+DEFINE_UNCORE_FORMAT_ATTR(filter_nid2, filter_nid, "config1:32-47");
 DEFINE_UNCORE_FORMAT_ATTR(filter_state, filter_state, "config1:18-22");
+DEFINE_UNCORE_FORMAT_ATTR(filter_state2, filter_state, "config1:17-22");
 DEFINE_UNCORE_FORMAT_ATTR(filter_opc, filter_opc, "config1:23-31");
+DEFINE_UNCORE_FORMAT_ATTR(filter_opc2, filter_opc, "config1:52-60");
 DEFINE_UNCORE_FORMAT_ATTR(filter_band0, filter_band0, "config1:0-7");
 DEFINE_UNCORE_FORMAT_ATTR(filter_band1, filter_band1, "config1:8-15");
 DEFINE_UNCORE_FORMAT_ATTR(filter_band2, filter_band2, "config1:16-23");
@@ -110,6 +117,21 @@ static void uncore_put_constraint(struct intel_uncore_box *box, struct perf_even
 	reg1->alloc = 0;
 }
 
+static u64 uncore_shared_reg_config(struct intel_uncore_box *box, int idx)
+{
+	struct intel_uncore_extra_reg *er;
+	unsigned long flags;
+	u64 config;
+
+	er = &box->shared_regs[idx];
+
+	raw_spin_lock_irqsave(&er->lock, flags);
+	config = er->config;
+	raw_spin_unlock_irqrestore(&er->lock, flags);
+
+	return config;
+}
+
 /* Sandy Bridge-EP uncore support */
 static struct intel_uncore_type snbep_uncore_cbox;
 static struct intel_uncore_type snbep_uncore_pcu;
@@ -205,7 +227,7 @@ static void snbep_uncore_msr_enable_event(struct intel_uncore_box *box, struct p
 	struct hw_perf_event_extra *reg1 = &hwc->extra_reg;
 
 	if (reg1->idx != EXTRA_REG_NONE)
-		wrmsrl(reg1->reg, reg1->config);
+		wrmsrl(reg1->reg, uncore_shared_reg_config(box, 0));
 
 	wrmsrl(hwc->config_base, hwc->config | SNBEP_PMON_CTL_EN);
 }
@@ -226,29 +248,6 @@ static void snbep_uncore_msr_init_box(struct intel_uncore_box *box)
 		wrmsrl(msr, SNBEP_PMON_BOX_CTL_INT);
 }
 
-static int snbep_uncore_hw_config(struct intel_uncore_box *box, struct perf_event *event)
-{
-	struct hw_perf_event *hwc = &event->hw;
-	struct hw_perf_event_extra *reg1 = &hwc->extra_reg;
-
-	if (box->pmu->type == &snbep_uncore_cbox) {
-		reg1->reg = SNBEP_C0_MSR_PMON_BOX_FILTER +
-			SNBEP_CBO_MSR_OFFSET * box->pmu->pmu_idx;
-		reg1->config = event->attr.config1 &
-			SNBEP_CB0_MSR_PMON_BOX_FILTER_MASK;
-	} else {
-		if (box->pmu->type == &snbep_uncore_pcu) {
-			reg1->reg = SNBEP_PCU_MSR_PMON_BOX_FILTER;
-			reg1->config = event->attr.config1 & SNBEP_PCU_MSR_PMON_BOX_FILTER_MASK;
-		} else {
-			return 0;
-		}
-	}
-	reg1->idx = 0;
-
-	return 0;
-}
-
 static struct attribute *snbep_uncore_formats_attr[] = {
 	&format_attr_event.attr,
 	&format_attr_umask.attr,
@@ -345,16 +344,16 @@ static struct attribute_group snbep_uncore_qpi_format_group = {
 	.attrs = snbep_uncore_qpi_formats_attr,
 };
 
+#define SNBEP_UNCORE_MSR_OPS_COMMON_INIT()			\
+	.init_box	= snbep_uncore_msr_init_box,		\
+	.disable_box	= snbep_uncore_msr_disable_box,		\
+	.enable_box	= snbep_uncore_msr_enable_box,		\
+	.disable_event	= snbep_uncore_msr_disable_event,	\
+	.enable_event	= snbep_uncore_msr_enable_event,	\
+	.read_counter	= uncore_msr_read_counter
+
 static struct intel_uncore_ops snbep_uncore_msr_ops = {
-	.init_box	= snbep_uncore_msr_init_box,
-	.disable_box	= snbep_uncore_msr_disable_box,
-	.enable_box	= snbep_uncore_msr_enable_box,
-	.disable_event	= snbep_uncore_msr_disable_event,
-	.enable_event	= snbep_uncore_msr_enable_event,
-	.read_counter	= uncore_msr_read_counter,
-	.get_constraint = uncore_get_constraint,
-	.put_constraint = uncore_put_constraint,
-	.hw_config	= snbep_uncore_hw_config,
+	SNBEP_UNCORE_MSR_OPS_COMMON_INIT(),
 };
 
 static struct intel_uncore_ops snbep_uncore_pci_ops = {
@@ -372,6 +371,7 @@ static struct event_constraint snbep_uncore_cbox_constraints[] = {
 	UNCORE_EVENT_CONSTRAINT(0x04, 0x3),
 	UNCORE_EVENT_CONSTRAINT(0x05, 0x3),
 	UNCORE_EVENT_CONSTRAINT(0x07, 0x3),
+	UNCORE_EVENT_CONSTRAINT(0x09, 0x3),
 	UNCORE_EVENT_CONSTRAINT(0x11, 0x1),
 	UNCORE_EVENT_CONSTRAINT(0x12, 0x3),
 	UNCORE_EVENT_CONSTRAINT(0x13, 0x3),
@@ -421,6 +421,14 @@ static struct event_constraint snbep_uncore_r3qpi_constraints[] = {
 	UNCORE_EVENT_CONSTRAINT(0x24, 0x3),
 	UNCORE_EVENT_CONSTRAINT(0x25, 0x3),
 	UNCORE_EVENT_CONSTRAINT(0x26, 0x3),
+	UNCORE_EVENT_CONSTRAINT(0x28, 0x3),
+	UNCORE_EVENT_CONSTRAINT(0x29, 0x3),
+	UNCORE_EVENT_CONSTRAINT(0x2a, 0x3),
+	UNCORE_EVENT_CONSTRAINT(0x2b, 0x3),
+	UNCORE_EVENT_CONSTRAINT(0x2c, 0x3),
+	UNCORE_EVENT_CONSTRAINT(0x2d, 0x3),
+	UNCORE_EVENT_CONSTRAINT(0x2e, 0x3),
+	UNCORE_EVENT_CONSTRAINT(0x2f, 0x3),
 	UNCORE_EVENT_CONSTRAINT(0x30, 0x3),
 	UNCORE_EVENT_CONSTRAINT(0x31, 0x3),
 	UNCORE_EVENT_CONSTRAINT(0x32, 0x3),
@@ -428,6 +436,8 @@ static struct event_constraint snbep_uncore_r3qpi_constraints[] = {
 	UNCORE_EVENT_CONSTRAINT(0x34, 0x3),
 	UNCORE_EVENT_CONSTRAINT(0x36, 0x3),
 	UNCORE_EVENT_CONSTRAINT(0x37, 0x3),
+	UNCORE_EVENT_CONSTRAINT(0x38, 0x3),
+	UNCORE_EVENT_CONSTRAINT(0x39, 0x3),
 	EVENT_CONSTRAINT_END
 };
 
@@ -446,6 +456,145 @@ static struct intel_uncore_type snbep_uncore_ubox = {
 	.format_group	= &snbep_uncore_ubox_format_group,
 };
 
+static struct extra_reg snbep_uncore_cbox_extra_regs[] = {
+	SNBEP_CBO_EVENT_EXTRA_REG(SNBEP_CBO_PMON_CTL_TID_EN,
+				  SNBEP_CBO_PMON_CTL_TID_EN, 0x1),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x0334, 0xffff, 0x4),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x0534, 0xffff, 0x4),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x0934, 0xffff, 0x4),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x4134, 0xffff, 0x6),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x0135, 0xffff, 0x8),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x0335, 0xffff, 0x8),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x4135, 0xffff, 0xc),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x4335, 0xffff, 0xc),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x4435, 0xffff, 0x2),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x4835, 0xffff, 0x2),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x4a35, 0xffff, 0x2),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x5035, 0xffff, 0x2),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x0136, 0xffff, 0x8),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x0336, 0xffff, 0x8),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x4136, 0xffff, 0xc),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x4336, 0xffff, 0xc),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x4436, 0xffff, 0x2),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x4836, 0xffff, 0x2),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x4a36, 0xffff, 0x2),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x4037, 0x40ff, 0x2),
+	EVENT_EXTRA_END
+};
+
+static void snbep_cbox_put_constraint(struct intel_uncore_box *box, struct perf_event *event)
+{
+	struct hw_perf_event_extra *reg1 = &event->hw.extra_reg;
+	struct intel_uncore_extra_reg *er = &box->shared_regs[0];
+	int i;
+
+	if (uncore_box_is_fake(box))
+		return;
+
+	for (i = 0; i < 5; i++) {
+		if (reg1->alloc & (0x1 << i))
+			atomic_sub(1 << (i * 6), &er->ref);
+	}
+	reg1->alloc = 0;
+}
+
+static struct event_constraint *
+__snbep_cbox_get_constraint(struct intel_uncore_box *box, struct perf_event *event,
+			    u64 (*cbox_filter_mask)(int fields))
+{
+	struct hw_perf_event_extra *reg1 = &event->hw.extra_reg;
+	struct intel_uncore_extra_reg *er = &box->shared_regs[0];
+	int i, alloc = 0;
+	unsigned long flags;
+	u64 mask;
+
+	if (reg1->idx == EXTRA_REG_NONE)
+		return NULL;
+
+	raw_spin_lock_irqsave(&er->lock, flags);
+	for (i = 0; i < 5; i++) {
+		if (!(reg1->idx & (0x1 << i)))
+			continue;
+		if (!uncore_box_is_fake(box) && (reg1->alloc & (0x1 << i)))
+			continue;
+
+		mask = cbox_filter_mask(0x1 << i);
+		if (!__BITS_VALUE(atomic_read(&er->ref), i, 6) ||
+		    !((reg1->config ^ er->config) & mask)) {
+			atomic_add(1 << (i * 6), &er->ref);
+			er->config &= ~mask;
+			er->config |= reg1->config & mask;
+			alloc |= (0x1 << i);
+		} else {
+			break;
+		}
+	}
+	raw_spin_unlock_irqrestore(&er->lock, flags);
+	if (i < 5)
+		goto fail;
+
+	if (!uncore_box_is_fake(box))
+		reg1->alloc |= alloc;
+
+	return 0;
+fail:
+	for (; i >= 0; i--) {
+		if (alloc & (0x1 << i))
+			atomic_sub(1 << (i * 6), &er->ref);
+	}
+	return &constraint_empty;
+}
+
+static u64 snbep_cbox_filter_mask(int fields)
+{
+	u64 mask = 0;
+
+	if (fields & 0x1)
+		mask |= SNBEP_CB0_MSR_PMON_BOX_FILTER_TID;
+	if (fields & 0x2)
+		mask |= SNBEP_CB0_MSR_PMON_BOX_FILTER_NID;
+	if (fields & 0x4)
+		mask |= SNBEP_CB0_MSR_PMON_BOX_FILTER_STATE;
+	if (fields & 0x8)
+		mask |= SNBEP_CB0_MSR_PMON_BOX_FILTER_OPC;
+
+	return mask;
+}
+
+static struct event_constraint *
+snbep_cbox_get_constraint(struct intel_uncore_box *box, struct perf_event *event)
+{
+	return __snbep_cbox_get_constraint(box, event, snbep_cbox_filter_mask);
+}
+
+static int snbep_cbox_hw_config(struct intel_uncore_box *box, struct perf_event *event)
+{
+	struct hw_perf_event_extra *reg1 = &event->hw.extra_reg;
+	struct extra_reg *er;
+	int idx = 0;
+
+	for (er = snbep_uncore_cbox_extra_regs; er->msr; er++) {
+		if (er->event != (event->hw.config & er->config_mask))
+			continue;
+		idx |= er->idx;
+	}
+
+	if (idx) {
+		reg1->reg = SNBEP_C0_MSR_PMON_BOX_FILTER +
+			SNBEP_CBO_MSR_OFFSET * box->pmu->pmu_idx;
+		reg1->config = event->attr.config1 & snbep_cbox_filter_mask(idx);
+		reg1->idx = idx;
+	}
+	return 0;
+}
+
+static struct intel_uncore_ops snbep_uncore_cbox_ops = {
+	SNBEP_UNCORE_MSR_OPS_COMMON_INIT(),
+	.hw_config		= snbep_cbox_hw_config,
+	.get_constraint		= snbep_cbox_get_constraint,
+	.put_constraint		= snbep_cbox_put_constraint,
+};
+
 static struct intel_uncore_type snbep_uncore_cbox = {
 	.name			= "cbox",
 	.num_counters		= 4,
@@ -458,10 +607,104 @@ static struct intel_uncore_type snbep_uncore_cbox = {
 	.msr_offset		= SNBEP_CBO_MSR_OFFSET,
 	.num_shared_regs	= 1,
 	.constraints		= snbep_uncore_cbox_constraints,
-	.ops			= &snbep_uncore_msr_ops,
+	.ops			= &snbep_uncore_cbox_ops,
 	.format_group		= &snbep_uncore_cbox_format_group,
 };
 
+static u64 snbep_pcu_alter_er(struct perf_event *event, int new_idx, bool modify)
+{
+	struct hw_perf_event *hwc = &event->hw;
+	struct hw_perf_event_extra *reg1 = &hwc->extra_reg;
+	u64 config = reg1->config;
+
+	if (new_idx > reg1->idx)
+		config <<= 8 * (new_idx - reg1->idx);
+	else
+		config >>= 8 * (reg1->idx - new_idx);
+
+	if (modify) {
+		hwc->config += new_idx - reg1->idx;
+		reg1->config = config;
+		reg1->idx = new_idx;
+	}
+	return config;
+}
+
+static struct event_constraint *
+snbep_pcu_get_constraint(struct intel_uncore_box *box, struct perf_event *event)
+{
+	struct hw_perf_event_extra *reg1 = &event->hw.extra_reg;
+	struct intel_uncore_extra_reg *er = &box->shared_regs[0];
+	unsigned long flags;
+	int idx = reg1->idx;
+	u64 mask, config1 = reg1->config;
+	bool ok = false;
+
+	if (reg1->idx == EXTRA_REG_NONE ||
+	    (!uncore_box_is_fake(box) && reg1->alloc))
+		return NULL;
+again:
+	mask = 0xff << (idx * 8);
+	raw_spin_lock_irqsave(&er->lock, flags);
+	if (!__BITS_VALUE(atomic_read(&er->ref), idx, 8) ||
+	    !((config1 ^ er->config) & mask)) {
+		atomic_add(1 << (idx * 8), &er->ref);
+		er->config &= ~mask;
+		er->config |= config1 & mask;
+		ok = true;
+	}
+	raw_spin_unlock_irqrestore(&er->lock, flags);
+
+	if (!ok) {
+		idx = (idx + 1) % 4;
+		if (idx != reg1->idx) {
+			config1 = snbep_pcu_alter_er(event, idx, false);
+			goto again;
+		}
+		return &constraint_empty;
+	}
+
+	if (!uncore_box_is_fake(box)) {
+		if (idx != reg1->idx)
+			snbep_pcu_alter_er(event, idx, true);
+		reg1->alloc = 1;
+	}
+	return NULL;
+}
+
+static void snbep_pcu_put_constraint(struct intel_uncore_box *box, struct perf_event *event)
+{
+	struct hw_perf_event_extra *reg1 = &event->hw.extra_reg;
+	struct intel_uncore_extra_reg *er = &box->shared_regs[0];
+
+	if (uncore_box_is_fake(box) || !reg1->alloc)
+		return;
+
+	atomic_sub(1 << (reg1->idx * 8), &er->ref);
+	reg1->alloc = 0;
+}
+
+static int snbep_pcu_hw_config(struct intel_uncore_box *box, struct perf_event *event)
+{
+	struct hw_perf_event *hwc = &event->hw;
+	struct hw_perf_event_extra *reg1 = &hwc->extra_reg;
+	int ev_sel = hwc->config & SNBEP_PMON_CTL_EV_SEL_MASK;
+
+	if (ev_sel >= 0xb && ev_sel <= 0xe) {
+		reg1->reg = SNBEP_PCU_MSR_PMON_BOX_FILTER;
+		reg1->idx = ev_sel - 0xb;
+		reg1->config = event->attr.config1 & (0xff << reg1->idx);
+	}
+	return 0;
+}
+
+static struct intel_uncore_ops snbep_uncore_pcu_ops = {
+	SNBEP_UNCORE_MSR_OPS_COMMON_INIT(),
+	.hw_config		= snbep_pcu_hw_config,
+	.get_constraint		= snbep_pcu_get_constraint,
+	.put_constraint		= snbep_pcu_put_constraint,
+};
+
 static struct intel_uncore_type snbep_uncore_pcu = {
 	.name			= "pcu",
 	.num_counters		= 4,
@@ -472,7 +715,7 @@ static struct intel_uncore_type snbep_uncore_pcu = {
 	.event_mask		= SNBEP_PCU_MSR_PMON_RAW_EVENT_MASK,
 	.box_ctl		= SNBEP_PCU_MSR_PMON_BOX_CTL,
 	.num_shared_regs	= 1,
-	.ops			= &snbep_uncore_msr_ops,
+	.ops			= &snbep_uncore_pcu_ops,
 	.format_group		= &snbep_uncore_pcu_format_group,
 };
 
@@ -544,55 +787,63 @@ static struct intel_uncore_type snbep_uncore_r3qpi = {
 	SNBEP_UNCORE_PCI_COMMON_INIT(),
 };
 
+enum {
+	SNBEP_PCI_UNCORE_HA,
+	SNBEP_PCI_UNCORE_IMC,
+	SNBEP_PCI_UNCORE_QPI,
+	SNBEP_PCI_UNCORE_R2PCIE,
+	SNBEP_PCI_UNCORE_R3QPI,
+};
+
 static struct intel_uncore_type *snbep_pci_uncores[] = {
-	&snbep_uncore_ha,
-	&snbep_uncore_imc,
-	&snbep_uncore_qpi,
-	&snbep_uncore_r2pcie,
-	&snbep_uncore_r3qpi,
+	[SNBEP_PCI_UNCORE_HA]		= &snbep_uncore_ha,
+	[SNBEP_PCI_UNCORE_IMC]		= &snbep_uncore_imc,
+	[SNBEP_PCI_UNCORE_QPI]		= &snbep_uncore_qpi,
+	[SNBEP_PCI_UNCORE_R2PCIE]	= &snbep_uncore_r2pcie,
+	[SNBEP_PCI_UNCORE_R3QPI]	= &snbep_uncore_r3qpi,
 	NULL,
 };
 
 static DEFINE_PCI_DEVICE_TABLE(snbep_uncore_pci_ids) = {
 	{ /* Home Agent */
 		PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_UNC_HA),
-		.driver_data = (unsigned long)&snbep_uncore_ha,
+		.driver_data = SNBEP_PCI_UNCORE_HA,
 	},
 	{ /* MC Channel 0 */
 		PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_UNC_IMC0),
-		.driver_data = (unsigned long)&snbep_uncore_imc,
+		.driver_data = SNBEP_PCI_UNCORE_IMC,
 	},
 	{ /* MC Channel 1 */
 		PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_UNC_IMC1),
-		.driver_data = (unsigned long)&snbep_uncore_imc,
+		.driver_data = SNBEP_PCI_UNCORE_IMC,
 	},
 	{ /* MC Channel 2 */
 		PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_UNC_IMC2),
-		.driver_data = (unsigned long)&snbep_uncore_imc,
+		.driver_data = SNBEP_PCI_UNCORE_IMC,
 	},
 	{ /* MC Channel 3 */
 		PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_UNC_IMC3),
-		.driver_data = (unsigned long)&snbep_uncore_imc,
+		.driver_data = SNBEP_PCI_UNCORE_IMC,
 	},
 	{ /* QPI Port 0 */
 		PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_UNC_QPI0),
-		.driver_data = (unsigned long)&snbep_uncore_qpi,
+		.driver_data = SNBEP_PCI_UNCORE_QPI,
 	},
 	{ /* QPI Port 1 */
 		PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_UNC_QPI1),
-		.driver_data = (unsigned long)&snbep_uncore_qpi,
+		.driver_data = SNBEP_PCI_UNCORE_QPI,
 	},
-	{ /* P2PCIe */
+	{ /* R2PCIe */
 		PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_UNC_R2PCIE),
-		.driver_data = (unsigned long)&snbep_uncore_r2pcie,
+		.driver_data = SNBEP_PCI_UNCORE_R2PCIE,
 	},
 	{ /* R3QPI Link 0 */
 		PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_UNC_R3QPI0),
-		.driver_data = (unsigned long)&snbep_uncore_r3qpi,
+		.driver_data = SNBEP_PCI_UNCORE_R3QPI,
 	},
 	{ /* R3QPI Link 1 */
 		PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_UNC_R3QPI1),
-		.driver_data = (unsigned long)&snbep_uncore_r3qpi,
+		.driver_data = SNBEP_PCI_UNCORE_R3QPI,
 	},
 	{ /* end: all zeroes */ }
 };
@@ -605,7 +856,7 @@ static struct pci_driver snbep_uncore_pci_driver = {
 /*
  * build pci bus to socket mapping
  */
-static int snbep_pci2phy_map_init(void)
+static int snbep_pci2phy_map_init(int devid)
 {
 	struct pci_dev *ubox_dev = NULL;
 	int i, bus, nodeid;
@@ -614,9 +865,7 @@ static int snbep_pci2phy_map_init(void)
 
 	while (1) {
 		/* find the UBOX device */
-		ubox_dev = pci_get_device(PCI_VENDOR_ID_INTEL,
-					PCI_DEVICE_ID_INTEL_JAKETOWN_UBOX,
-					ubox_dev);
+		ubox_dev = pci_get_device(PCI_VENDOR_ID_INTEL, devid, ubox_dev);
 		if (!ubox_dev)
 			break;
 		bus = ubox_dev->bus->number;
@@ -639,7 +888,7 @@ static int snbep_pci2phy_map_init(void)
 				break;
 			}
 		}
-	};
+	}
 
 	if (ubox_dev)
 		pci_dev_put(ubox_dev);
@@ -648,6 +897,440 @@ static int snbep_pci2phy_map_init(void)
 }
 /* end of Sandy Bridge-EP uncore support */
 
+/* IvyTown uncore support */
+static void ivt_uncore_msr_init_box(struct intel_uncore_box *box)
+{
+	unsigned msr = uncore_msr_box_ctl(box);
+	if (msr)
+		wrmsrl(msr, IVT_PMON_BOX_CTL_INT);
+}
+
+static void ivt_uncore_pci_init_box(struct intel_uncore_box *box)
+{
+	struct pci_dev *pdev = box->pci_dev;
+
+	pci_write_config_dword(pdev, SNBEP_PCI_PMON_BOX_CTL, IVT_PMON_BOX_CTL_INT);
+}
+
+#define IVT_UNCORE_MSR_OPS_COMMON_INIT()			\
+	.init_box	= ivt_uncore_msr_init_box,		\
+	.disable_box	= snbep_uncore_msr_disable_box,		\
+	.enable_box	= snbep_uncore_msr_enable_box,		\
+	.disable_event	= snbep_uncore_msr_disable_event,	\
+	.enable_event	= snbep_uncore_msr_enable_event,	\
+	.read_counter	= uncore_msr_read_counter
+
+static struct intel_uncore_ops ivt_uncore_msr_ops = {
+	IVT_UNCORE_MSR_OPS_COMMON_INIT(),
+};
+
+static struct intel_uncore_ops ivt_uncore_pci_ops = {
+	.init_box	= ivt_uncore_pci_init_box,
+	.disable_box	= snbep_uncore_pci_disable_box,
+	.enable_box	= snbep_uncore_pci_enable_box,
+	.disable_event	= snbep_uncore_pci_disable_event,
+	.enable_event	= snbep_uncore_pci_enable_event,
+	.read_counter	= snbep_uncore_pci_read_counter,
+};
+
+#define IVT_UNCORE_PCI_COMMON_INIT()				\
+	.perf_ctr	= SNBEP_PCI_PMON_CTR0,			\
+	.event_ctl	= SNBEP_PCI_PMON_CTL0,			\
+	.event_mask	= IVT_PMON_RAW_EVENT_MASK,		\
+	.box_ctl	= SNBEP_PCI_PMON_BOX_CTL,		\
+	.ops		= &ivt_uncore_pci_ops,			\
+	.format_group	= &ivt_uncore_format_group
+
+static struct attribute *ivt_uncore_formats_attr[] = {
+	&format_attr_event.attr,
+	&format_attr_umask.attr,
+	&format_attr_edge.attr,
+	&format_attr_inv.attr,
+	&format_attr_thresh8.attr,
+	NULL,
+};
+
+static struct attribute *ivt_uncore_ubox_formats_attr[] = {
+	&format_attr_event.attr,
+	&format_attr_umask.attr,
+	&format_attr_edge.attr,
+	&format_attr_inv.attr,
+	&format_attr_thresh5.attr,
+	NULL,
+};
+
+static struct attribute *ivt_uncore_cbox_formats_attr[] = {
+	&format_attr_event.attr,
+	&format_attr_umask.attr,
+	&format_attr_edge.attr,
+	&format_attr_tid_en.attr,
+	&format_attr_thresh8.attr,
+	&format_attr_filter_tid.attr,
+	&format_attr_filter_link.attr,
+	&format_attr_filter_state2.attr,
+	&format_attr_filter_nid2.attr,
+	&format_attr_filter_opc2.attr,
+	NULL,
+};
+
+static struct attribute *ivt_uncore_pcu_formats_attr[] = {
+	&format_attr_event_ext.attr,
+	&format_attr_occ_sel.attr,
+	&format_attr_edge.attr,
+	&format_attr_thresh5.attr,
+	&format_attr_occ_invert.attr,
+	&format_attr_occ_edge.attr,
+	&format_attr_filter_band0.attr,
+	&format_attr_filter_band1.attr,
+	&format_attr_filter_band2.attr,
+	&format_attr_filter_band3.attr,
+	NULL,
+};
+
+static struct attribute *ivt_uncore_qpi_formats_attr[] = {
+	&format_attr_event_ext.attr,
+	&format_attr_umask.attr,
+	&format_attr_edge.attr,
+	&format_attr_thresh8.attr,
+	NULL,
+};
+
+static struct attribute_group ivt_uncore_format_group = {
+	.name = "format",
+	.attrs = ivt_uncore_formats_attr,
+};
+
+static struct attribute_group ivt_uncore_ubox_format_group = {
+	.name = "format",
+	.attrs = ivt_uncore_ubox_formats_attr,
+};
+
+static struct attribute_group ivt_uncore_cbox_format_group = {
+	.name = "format",
+	.attrs = ivt_uncore_cbox_formats_attr,
+};
+
+static struct attribute_group ivt_uncore_pcu_format_group = {
+	.name = "format",
+	.attrs = ivt_uncore_pcu_formats_attr,
+};
+
+static struct attribute_group ivt_uncore_qpi_format_group = {
+	.name = "format",
+	.attrs = ivt_uncore_qpi_formats_attr,
+};
+
+static struct intel_uncore_type ivt_uncore_ubox = {
+	.name		= "ubox",
+	.num_counters   = 2,
+	.num_boxes	= 1,
+	.perf_ctr_bits	= 44,
+	.fixed_ctr_bits	= 48,
+	.perf_ctr	= SNBEP_U_MSR_PMON_CTR0,
+	.event_ctl	= SNBEP_U_MSR_PMON_CTL0,
+	.event_mask	= IVT_U_MSR_PMON_RAW_EVENT_MASK,
+	.fixed_ctr	= SNBEP_U_MSR_PMON_UCLK_FIXED_CTR,
+	.fixed_ctl	= SNBEP_U_MSR_PMON_UCLK_FIXED_CTL,
+	.ops		= &ivt_uncore_msr_ops,
+	.format_group	= &ivt_uncore_ubox_format_group,
+};
+
+static struct extra_reg ivt_uncore_cbox_extra_regs[] = {
+	SNBEP_CBO_EVENT_EXTRA_REG(SNBEP_CBO_PMON_CTL_TID_EN,
+				  SNBEP_CBO_PMON_CTL_TID_EN, 0x1),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x1031, 0x10ff, 0x2),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x0334, 0xffff, 0x4),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x0534, 0xffff, 0x4),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x0934, 0xffff, 0x4),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x4134, 0xffff, 0xc),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x0135, 0xffff, 0x10),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x0335, 0xffff, 0x10),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x2135, 0xffff, 0x10),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x2335, 0xffff, 0x10),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x4135, 0xffff, 0x18),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x4335, 0xffff, 0x18),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x4435, 0xffff, 0x8),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x4835, 0xffff, 0x8),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x4a35, 0xffff, 0x8),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x5035, 0xffff, 0x8),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x8135, 0xffff, 0x10),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x8335, 0xffff, 0x10),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x0136, 0xffff, 0x10),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x0336, 0xffff, 0x10),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x2336, 0xffff, 0x10),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x2336, 0xffff, 0x10),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x4136, 0xffff, 0x18),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x4336, 0xffff, 0x18),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x4436, 0xffff, 0x8),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x4836, 0xffff, 0x8),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x4a36, 0xffff, 0x8),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x5036, 0xffff, 0x8),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x8136, 0xffff, 0x10),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x8336, 0xffff, 0x10),
+	SNBEP_CBO_EVENT_EXTRA_REG(0x4037, 0x40ff, 0x8),
+	EVENT_EXTRA_END
+};
+
+static u64 ivt_cbox_filter_mask(int fields)
+{
+	u64 mask = 0;
+
+	if (fields & 0x1)
+		mask |= IVT_CB0_MSR_PMON_BOX_FILTER_TID;
+	if (fields & 0x2)
+		mask |= IVT_CB0_MSR_PMON_BOX_FILTER_LINK;
+	if (fields & 0x4)
+		mask |= IVT_CB0_MSR_PMON_BOX_FILTER_STATE;
+	if (fields & 0x8)
+		mask |= IVT_CB0_MSR_PMON_BOX_FILTER_NID;
+	if (fields & 0x10)
+		mask |= IVT_CB0_MSR_PMON_BOX_FILTER_OPC;
+
+	return mask;
+}
+
+static struct event_constraint *
+ivt_cbox_get_constraint(struct intel_uncore_box *box, struct perf_event *event)
+{
+	return __snbep_cbox_get_constraint(box, event, ivt_cbox_filter_mask);
+}
+
+static int ivt_cbox_hw_config(struct intel_uncore_box *box, struct perf_event *event)
+{
+	struct hw_perf_event_extra *reg1 = &event->hw.extra_reg;
+	struct extra_reg *er;
+	int idx = 0;
+
+	for (er = ivt_uncore_cbox_extra_regs; er->msr; er++) {
+		if (er->event != (event->hw.config & er->config_mask))
+			continue;
+		idx |= er->idx;
+	}
+
+	if (idx) {
+		reg1->reg = SNBEP_C0_MSR_PMON_BOX_FILTER +
+			SNBEP_CBO_MSR_OFFSET * box->pmu->pmu_idx;
+		reg1->config = event->attr.config1 & ivt_cbox_filter_mask(idx);
+		reg1->idx = idx;
+	}
+	return 0;
+}
+
+static void ivt_cbox_enable_event(struct intel_uncore_box *box, struct perf_event *event)
+{
+	struct hw_perf_event *hwc = &event->hw;
+	struct hw_perf_event_extra *reg1 = &hwc->extra_reg;
+
+	if (reg1->idx != EXTRA_REG_NONE) {
+		u64 filter = uncore_shared_reg_config(box, 0);
+		wrmsrl(reg1->reg, filter & 0xffffffff);
+		wrmsrl(reg1->reg + 6, filter >> 32);
+	}
+
+	wrmsrl(hwc->config_base, hwc->config | SNBEP_PMON_CTL_EN);
+}
+
+static struct intel_uncore_ops ivt_uncore_cbox_ops = {
+	.init_box		= ivt_uncore_msr_init_box,
+	.disable_box		= snbep_uncore_msr_disable_box,
+	.enable_box		= snbep_uncore_msr_enable_box,
+	.disable_event		= snbep_uncore_msr_disable_event,
+	.enable_event		= ivt_cbox_enable_event,
+	.read_counter		= uncore_msr_read_counter,
+	.hw_config		= ivt_cbox_hw_config,
+	.get_constraint		= ivt_cbox_get_constraint,
+	.put_constraint		= snbep_cbox_put_constraint,
+};
+
+static struct intel_uncore_type ivt_uncore_cbox = {
+	.name			= "cbox",
+	.num_counters		= 4,
+	.num_boxes		= 15,
+	.perf_ctr_bits		= 44,
+	.event_ctl		= SNBEP_C0_MSR_PMON_CTL0,
+	.perf_ctr		= SNBEP_C0_MSR_PMON_CTR0,
+	.event_mask		= IVT_CBO_MSR_PMON_RAW_EVENT_MASK,
+	.box_ctl		= SNBEP_C0_MSR_PMON_BOX_CTL,
+	.msr_offset		= SNBEP_CBO_MSR_OFFSET,
+	.num_shared_regs	= 1,
+	.constraints		= snbep_uncore_cbox_constraints,
+	.ops			= &ivt_uncore_cbox_ops,
+	.format_group		= &ivt_uncore_cbox_format_group,
+};
+
+static struct intel_uncore_ops ivt_uncore_pcu_ops = {
+	IVT_UNCORE_MSR_OPS_COMMON_INIT(),
+	.hw_config		= snbep_pcu_hw_config,
+	.get_constraint		= snbep_pcu_get_constraint,
+	.put_constraint		= snbep_pcu_put_constraint,
+};
+
+static struct intel_uncore_type ivt_uncore_pcu = {
+	.name			= "pcu",
+	.num_counters		= 4,
+	.num_boxes		= 1,
+	.perf_ctr_bits		= 48,
+	.perf_ctr		= SNBEP_PCU_MSR_PMON_CTR0,
+	.event_ctl		= SNBEP_PCU_MSR_PMON_CTL0,
+	.event_mask		= IVT_PCU_MSR_PMON_RAW_EVENT_MASK,
+	.box_ctl		= SNBEP_PCU_MSR_PMON_BOX_CTL,
+	.num_shared_regs	= 1,
+	.ops			= &ivt_uncore_pcu_ops,
+	.format_group		= &ivt_uncore_pcu_format_group,
+};
+
+static struct intel_uncore_type *ivt_msr_uncores[] = {
+	&ivt_uncore_ubox,
+	&ivt_uncore_cbox,
+	&ivt_uncore_pcu,
+	NULL,
+};
+
+static struct intel_uncore_type ivt_uncore_ha = {
+	.name		= "ha",
+	.num_counters   = 4,
+	.num_boxes	= 2,
+	.perf_ctr_bits	= 48,
+	IVT_UNCORE_PCI_COMMON_INIT(),
+};
+
+static struct intel_uncore_type ivt_uncore_imc = {
+	.name		= "imc",
+	.num_counters   = 4,
+	.num_boxes	= 8,
+	.perf_ctr_bits	= 48,
+	.fixed_ctr_bits	= 48,
+	.fixed_ctr	= SNBEP_MC_CHy_PCI_PMON_FIXED_CTR,
+	.fixed_ctl	= SNBEP_MC_CHy_PCI_PMON_FIXED_CTL,
+	IVT_UNCORE_PCI_COMMON_INIT(),
+};
+
+static struct intel_uncore_type ivt_uncore_qpi = {
+	.name		= "qpi",
+	.num_counters   = 4,
+	.num_boxes	= 3,
+	.perf_ctr_bits	= 48,
+	.perf_ctr	= SNBEP_PCI_PMON_CTR0,
+	.event_ctl	= SNBEP_PCI_PMON_CTL0,
+	.event_mask	= IVT_QPI_PCI_PMON_RAW_EVENT_MASK,
+	.box_ctl	= SNBEP_PCI_PMON_BOX_CTL,
+	.ops		= &ivt_uncore_pci_ops,
+	.format_group	= &ivt_uncore_qpi_format_group,
+};
+
+static struct intel_uncore_type ivt_uncore_r2pcie = {
+	.name		= "r2pcie",
+	.num_counters   = 4,
+	.num_boxes	= 1,
+	.perf_ctr_bits	= 44,
+	.constraints	= snbep_uncore_r2pcie_constraints,
+	IVT_UNCORE_PCI_COMMON_INIT(),
+};
+
+static struct intel_uncore_type ivt_uncore_r3qpi = {
+	.name		= "r3qpi",
+	.num_counters   = 3,
+	.num_boxes	= 2,
+	.perf_ctr_bits	= 44,
+	.constraints	= snbep_uncore_r3qpi_constraints,
+	IVT_UNCORE_PCI_COMMON_INIT(),
+};
+
+enum {
+	IVT_PCI_UNCORE_HA,
+	IVT_PCI_UNCORE_IMC,
+	IVT_PCI_UNCORE_QPI,
+	IVT_PCI_UNCORE_R2PCIE,
+	IVT_PCI_UNCORE_R3QPI,
+};
+
+static struct intel_uncore_type *ivt_pci_uncores[] = {
+	[IVT_PCI_UNCORE_HA]	= &ivt_uncore_ha,
+	[IVT_PCI_UNCORE_IMC]	= &ivt_uncore_imc,
+	[IVT_PCI_UNCORE_QPI]	= &ivt_uncore_qpi,
+	[IVT_PCI_UNCORE_R2PCIE]	= &ivt_uncore_r2pcie,
+	[IVT_PCI_UNCORE_R3QPI]	= &ivt_uncore_r3qpi,
+	NULL,
+};
+
+static DEFINE_PCI_DEVICE_TABLE(ivt_uncore_pci_ids) = {
+	{ /* Home Agent 0 */
+		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0xe30),
+		.driver_data = IVT_PCI_UNCORE_HA,
+	},
+	{ /* Home Agent 1 */
+		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0xe38),
+		.driver_data = IVT_PCI_UNCORE_HA,
+	},
+	{ /* MC0 Channel 0 */
+		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0xeb4),
+		.driver_data = IVT_PCI_UNCORE_IMC,
+	},
+	{ /* MC0 Channel 1 */
+		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0xeb5),
+		.driver_data = IVT_PCI_UNCORE_IMC,
+	},
+	{ /* MC0 Channel 3 */
+		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0xeb0),
+		.driver_data = IVT_PCI_UNCORE_IMC,
+	},
+	{ /* MC0 Channel 4 */
+		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0xeb1),
+		.driver_data = IVT_PCI_UNCORE_IMC,
+	},
+	{ /* MC1 Channel 0 */
+		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0xef4),
+		.driver_data = IVT_PCI_UNCORE_IMC,
+	},
+	{ /* MC1 Channel 1 */
+		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0xef5),
+		.driver_data = IVT_PCI_UNCORE_IMC,
+	},
+	{ /* MC1 Channel 3 */
+		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0xef0),
+		.driver_data = IVT_PCI_UNCORE_IMC,
+	},
+	{ /* MC1 Channel 4 */
+		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0xef1),
+		.driver_data = IVT_PCI_UNCORE_IMC,
+	},
+	{ /* QPI0 Port 0 */
+		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0xe32),
+		.driver_data = IVT_PCI_UNCORE_QPI,
+	},
+	{ /* QPI0 Port 1 */
+		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0xe33),
+		.driver_data = IVT_PCI_UNCORE_QPI,
+	},
+	{ /* QPI1 Port 2 */
+		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0xe3a),
+		.driver_data = IVT_PCI_UNCORE_QPI,
+	},
+	{ /* R2PCIe */
+		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0xe34),
+		.driver_data = IVT_PCI_UNCORE_R2PCIE,
+	},
+	{ /* R3QPI0 Link 0 */
+		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0xe36),
+		.driver_data = IVT_PCI_UNCORE_R3QPI,
+	},
+	{ /* R3QPI0 Link 1 */
+		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0xe37),
+		.driver_data = IVT_PCI_UNCORE_R3QPI,
+	},
+	{ /* R3QPI1 Link 2 */
+		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0xe3e),
+		.driver_data = IVT_PCI_UNCORE_R3QPI,
+	},
+	{ /* end: all zeroes */ }
+};
+
+static struct pci_driver ivt_uncore_pci_driver = {
+	.name		= "ivt_uncore",
+	.id_table	= ivt_uncore_pci_ids,
+};
+/* end of IvyTown uncore support */
+
 /* Sandy Bridge uncore support */
 static void snb_uncore_msr_enable_event(struct intel_uncore_box *box, struct perf_event *event)
 {
@@ -808,9 +1491,6 @@ static struct intel_uncore_type *nhm_msr_uncores[] = {
 /* end of Nehalem uncore support */
 
 /* Nehalem-EX uncore support */
-#define __BITS_VALUE(x, i, n)  ((typeof(x))(((x) >> ((i) * (n))) & \
-				((1ULL << (n)) - 1)))
-
 DEFINE_UNCORE_FORMAT_ATTR(event5, event, "config:1-5");
 DEFINE_UNCORE_FORMAT_ATTR(counter, counter, "config:6-7");
 DEFINE_UNCORE_FORMAT_ATTR(match, match, "config1:0-63");
@@ -1161,7 +1841,7 @@ static struct extra_reg nhmex_uncore_mbox_extra_regs[] = {
 };
 
 /* Nehalem-EX or Westmere-EX ? */
-bool uncore_nhmex;
+static bool uncore_nhmex;
 
 static bool nhmex_mbox_get_shared_reg(struct intel_uncore_box *box, int idx, u64 config)
 {
@@ -1239,7 +1919,7 @@ static void nhmex_mbox_put_shared_reg(struct intel_uncore_box *box, int idx)
 	atomic_sub(1 << (idx * 8), &er->ref);
 }
 
-u64 nhmex_mbox_alter_er(struct perf_event *event, int new_idx, bool modify)
+static u64 nhmex_mbox_alter_er(struct perf_event *event, int new_idx, bool modify)
 {
 	struct hw_perf_event *hwc = &event->hw;
 	struct hw_perf_event_extra *reg1 = &hwc->extra_reg;
@@ -1554,7 +2234,7 @@ static struct intel_uncore_type nhmex_uncore_mbox = {
 	.format_group		= &nhmex_uncore_mbox_format_group,
 };
 
-void nhmex_rbox_alter_er(struct intel_uncore_box *box, struct perf_event *event)
+static void nhmex_rbox_alter_er(struct intel_uncore_box *box, struct perf_event *event)
 {
 	struct hw_perf_event *hwc = &event->hw;
 	struct hw_perf_event_extra *reg1 = &hwc->extra_reg;
@@ -1724,21 +2404,6 @@ static int nhmex_rbox_hw_config(struct intel_uncore_box *box, struct perf_event
 	return 0;
 }
 
-static u64 nhmex_rbox_shared_reg_config(struct intel_uncore_box *box, int idx)
-{
-	struct intel_uncore_extra_reg *er;
-	unsigned long flags;
-	u64 config;
-
-	er = &box->shared_regs[idx];
-
-	raw_spin_lock_irqsave(&er->lock, flags);
-	config = er->config;
-	raw_spin_unlock_irqrestore(&er->lock, flags);
-
-	return config;
-}
-
 static void nhmex_rbox_msr_enable_event(struct intel_uncore_box *box, struct perf_event *event)
 {
 	struct hw_perf_event *hwc = &event->hw;
@@ -1759,7 +2424,7 @@ static void nhmex_rbox_msr_enable_event(struct intel_uncore_box *box, struct per
 	case 2:
 	case 3:
 		wrmsrl(NHMEX_R_MSR_PORTN_QLX_CFG(port),
-			nhmex_rbox_shared_reg_config(box, 2 + (idx / 6) * 5));
+			uncore_shared_reg_config(box, 2 + (idx / 6) * 5));
 		break;
 	case 4:
 		wrmsrl(NHMEX_R_MSR_PORTN_XBR_SET1_MM_CFG(port),
@@ -2285,7 +2950,7 @@ out:
 	return ret;
 }
 
-int uncore_pmu_event_init(struct perf_event *event)
+static int uncore_pmu_event_init(struct perf_event *event)
 {
 	struct intel_uncore_pmu *pmu;
 	struct intel_uncore_box *box;
@@ -2438,7 +3103,7 @@ static int __init uncore_type_init(struct intel_uncore_type *type)
 
 	type->unconstrainted = (struct event_constraint)
 		__EVENT_CONSTRAINT(0, (1ULL << type->num_counters) - 1,
-				0, type->num_counters, 0);
+				0, type->num_counters, 0, 0);
 
 	for (i = 0; i < type->num_boxes; i++) {
 		pmus[i].func_id = -1;
@@ -2556,6 +3221,8 @@ static void uncore_pci_remove(struct pci_dev *pdev)
 	if (WARN_ON_ONCE(phys_id != box->phys_id))
 		return;
 
+	pci_set_drvdata(pdev, NULL);
+
 	raw_spin_lock(&uncore_box_lock);
 	list_del(&box->list);
 	raw_spin_unlock(&uncore_box_lock);
@@ -2574,11 +3241,7 @@ static void uncore_pci_remove(struct pci_dev *pdev)
 static int uncore_pci_probe(struct pci_dev *pdev,
 			    const struct pci_device_id *id)
 {
-	struct intel_uncore_type *type;
-
-	type = (struct intel_uncore_type *)id->driver_data;
-
-	return uncore_pci_add(type, pdev);
+	return uncore_pci_add(pci_uncores[id->driver_data], pdev);
 }
 
 static int __init uncore_pci_init(void)
@@ -2587,12 +3250,19 @@ static int __init uncore_pci_init(void)
 
 	switch (boot_cpu_data.x86_model) {
 	case 45: /* Sandy Bridge-EP */
-		ret = snbep_pci2phy_map_init();
+		ret = snbep_pci2phy_map_init(0x3ce0);
 		if (ret)
 			return ret;
 		pci_uncores = snbep_pci_uncores;
 		uncore_pci_driver = &snbep_uncore_pci_driver;
 		break;
+	case 62: /* IvyTown */
+		ret = snbep_pci2phy_map_init(0x0e1e);
+		if (ret)
+			return ret;
+		pci_uncores = ivt_pci_uncores;
+		uncore_pci_driver = &ivt_uncore_pci_driver;
+		break;
 	default:
 		return 0;
 	}
@@ -2622,6 +3292,21 @@ static void __init uncore_pci_exit(void)
 	}
 }
 
+/* CPU hot plug/unplug are serialized by cpu_add_remove_lock mutex */
+static LIST_HEAD(boxes_to_free);
+
+static void __cpuinit uncore_kfree_boxes(void)
+{
+	struct intel_uncore_box *box;
+
+	while (!list_empty(&boxes_to_free)) {
+		box = list_entry(boxes_to_free.next,
+				 struct intel_uncore_box, list);
+		list_del(&box->list);
+		kfree(box);
+	}
+}
+
 static void __cpuinit uncore_cpu_dying(int cpu)
 {
 	struct intel_uncore_type *type;
@@ -2636,7 +3321,7 @@ static void __cpuinit uncore_cpu_dying(int cpu)
 			box = *per_cpu_ptr(pmu->box, cpu);
 			*per_cpu_ptr(pmu->box, cpu) = NULL;
 			if (box && atomic_dec_and_test(&box->refcnt))
-				kfree(box);
+				list_add(&box->list, &boxes_to_free);
 		}
 	}
 }
@@ -2666,8 +3351,11 @@ static int __cpuinit uncore_cpu_starting(int cpu)
 				if (exist && exist->phys_id == phys_id) {
 					atomic_inc(&exist->refcnt);
 					*per_cpu_ptr(pmu->box, cpu) = exist;
-					kfree(box);
-					box = NULL;
+					if (box) {
+						list_add(&box->list,
+							 &boxes_to_free);
+						box = NULL;
+					}
 					break;
 				}
 			}
@@ -2806,6 +3494,10 @@ static int
 	case CPU_DYING:
 		uncore_cpu_dying(cpu);
 		break;
+	case CPU_ONLINE:
+	case CPU_DEAD:
+		uncore_kfree_boxes();
+		break;
 	default:
 		break;
 	}
@@ -2871,6 +3563,12 @@ static int __init uncore_cpu_init(void)
 			nhmex_uncore_cbox.num_boxes = max_cores;
 		msr_uncores = nhmex_msr_uncores;
 		break;
+	case 62: /* IvyTown */
+		if (ivt_uncore_cbox.num_boxes > max_cores)
+			ivt_uncore_cbox.num_boxes = max_cores;
+		msr_uncores = ivt_msr_uncores;
+		break;
+
 	default:
 		return 0;
 	}
diff --git a/arch/x86/kernel/cpu/perf_event_intel_uncore.h b/arch/x86/kernel/cpu/perf_event_intel_uncore.h
index e68a4550e95..f9528917f6e 100644
--- a/arch/x86/kernel/cpu/perf_event_intel_uncore.h
+++ b/arch/x86/kernel/cpu/perf_event_intel_uncore.h
@@ -76,7 +76,7 @@
 #define SNBEP_PMON_CTL_UMASK_MASK	0x0000ff00
 #define SNBEP_PMON_CTL_RST		(1 << 17)
 #define SNBEP_PMON_CTL_EDGE_DET		(1 << 18)
-#define SNBEP_PMON_CTL_EV_SEL_EXT	(1 << 21)	/* only for QPI */
+#define SNBEP_PMON_CTL_EV_SEL_EXT	(1 << 21)
 #define SNBEP_PMON_CTL_EN		(1 << 22)
 #define SNBEP_PMON_CTL_INVERT		(1 << 23)
 #define SNBEP_PMON_CTL_TRESH_MASK	0xff000000
@@ -148,9 +148,20 @@
 #define SNBEP_C0_MSR_PMON_CTL0			0xd10
 #define SNBEP_C0_MSR_PMON_BOX_CTL		0xd04
 #define SNBEP_C0_MSR_PMON_BOX_FILTER		0xd14
-#define SNBEP_CB0_MSR_PMON_BOX_FILTER_MASK	0xfffffc1f
 #define SNBEP_CBO_MSR_OFFSET			0x20
 
+#define SNBEP_CB0_MSR_PMON_BOX_FILTER_TID	0x1f
+#define SNBEP_CB0_MSR_PMON_BOX_FILTER_NID	0x3fc00
+#define SNBEP_CB0_MSR_PMON_BOX_FILTER_STATE	0x7c0000
+#define SNBEP_CB0_MSR_PMON_BOX_FILTER_OPC	0xff800000
+
+#define SNBEP_CBO_EVENT_EXTRA_REG(e, m, i) {	\
+	.event = (e),				\
+	.msr = SNBEP_C0_MSR_PMON_BOX_FILTER,	\
+	.config_mask = (m),			\
+	.idx = (i)				\
+}
+
 /* SNB-EP PCU register */
 #define SNBEP_PCU_MSR_PMON_CTR0			0xc36
 #define SNBEP_PCU_MSR_PMON_CTL0			0xc30
@@ -160,6 +171,55 @@
 #define SNBEP_PCU_MSR_CORE_C3_CTR		0x3fc
 #define SNBEP_PCU_MSR_CORE_C6_CTR		0x3fd
 
+/* IVT event control */
+#define IVT_PMON_BOX_CTL_INT		(SNBEP_PMON_BOX_CTL_RST_CTRL | \
+					 SNBEP_PMON_BOX_CTL_RST_CTRS)
+#define IVT_PMON_RAW_EVENT_MASK		(SNBEP_PMON_CTL_EV_SEL_MASK | \
+					 SNBEP_PMON_CTL_UMASK_MASK | \
+					 SNBEP_PMON_CTL_EDGE_DET | \
+					 SNBEP_PMON_CTL_TRESH_MASK)
+/* IVT Ubox */
+#define IVT_U_MSR_PMON_GLOBAL_CTL		0xc00
+#define IVT_U_PMON_GLOBAL_FRZ_ALL		(1 << 31)
+#define IVT_U_PMON_GLOBAL_UNFRZ_ALL		(1 << 29)
+
+#define IVT_U_MSR_PMON_RAW_EVENT_MASK	\
+				(SNBEP_PMON_CTL_EV_SEL_MASK | \
+				 SNBEP_PMON_CTL_UMASK_MASK | \
+				 SNBEP_PMON_CTL_EDGE_DET | \
+				 SNBEP_U_MSR_PMON_CTL_TRESH_MASK)
+/* IVT Cbo */
+#define IVT_CBO_MSR_PMON_RAW_EVENT_MASK		(IVT_PMON_RAW_EVENT_MASK | \
+						 SNBEP_CBO_PMON_CTL_TID_EN)
+
+#define IVT_CB0_MSR_PMON_BOX_FILTER_TID		(0x1fULL << 0)
+#define IVT_CB0_MSR_PMON_BOX_FILTER_LINK	(0xfULL << 5)
+#define IVT_CB0_MSR_PMON_BOX_FILTER_STATE	(0x3fULL << 17)
+#define IVT_CB0_MSR_PMON_BOX_FILTER_NID		(0xffffULL << 32)
+#define IVT_CB0_MSR_PMON_BOX_FILTER_OPC		(0x1ffULL << 52)
+#define IVT_CB0_MSR_PMON_BOX_FILTER_C6		(0x1ULL << 61)
+#define IVT_CB0_MSR_PMON_BOX_FILTER_NC		(0x1ULL << 62)
+#define IVT_CB0_MSR_PMON_BOX_FILTER_IOSC	(0x1ULL << 63)
+
+/* IVT home agent */
+#define IVT_HA_PCI_PMON_CTL_Q_OCC_RST		(1 << 16)
+#define IVT_HA_PCI_PMON_RAW_EVENT_MASK		\
+				(IVT_PMON_RAW_EVENT_MASK | \
+				 IVT_HA_PCI_PMON_CTL_Q_OCC_RST)
+/* IVT PCU */
+#define IVT_PCU_MSR_PMON_RAW_EVENT_MASK	\
+				(SNBEP_PMON_CTL_EV_SEL_MASK | \
+				 SNBEP_PMON_CTL_EV_SEL_EXT | \
+				 SNBEP_PCU_MSR_PMON_CTL_OCC_SEL_MASK | \
+				 SNBEP_PMON_CTL_EDGE_DET | \
+				 SNBEP_PCU_MSR_PMON_CTL_TRESH_MASK | \
+				 SNBEP_PCU_MSR_PMON_CTL_OCC_INVERT | \
+				 SNBEP_PCU_MSR_PMON_CTL_OCC_EDGE_DET)
+/* IVT QPI */
+#define IVT_QPI_PCI_PMON_RAW_EVENT_MASK	\
+				(IVT_PMON_RAW_EVENT_MASK | \
+				 SNBEP_PMON_CTL_EV_SEL_EXT)
+
 /* NHM-EX event control */
 #define NHMEX_PMON_CTL_EV_SEL_MASK	0x000000ff
 #define NHMEX_PMON_CTL_UMASK_MASK	0x0000ff00
diff --git a/arch/x86/kernel/cpu/perf_event_knc.c b/arch/x86/kernel/cpu/perf_event_knc.c
index 4b7731bf23a..838fa8772c6 100644
--- a/arch/x86/kernel/cpu/perf_event_knc.c
+++ b/arch/x86/kernel/cpu/perf_event_knc.c
@@ -17,7 +17,7 @@ static const u64 knc_perfmon_event_map[] =
   [PERF_COUNT_HW_BRANCH_MISSES]		= 0x002b,
 };
 
-static __initconst u64 knc_hw_cache_event_ids
+static const u64 __initconst knc_hw_cache_event_ids
 				[PERF_COUNT_HW_CACHE_MAX]
 				[PERF_COUNT_HW_CACHE_OP_MAX]
 				[PERF_COUNT_HW_CACHE_RESULT_MAX] =
@@ -284,7 +284,7 @@ static struct attribute *intel_knc_formats_attr[] = {
 	NULL,
 };
 
-static __initconst struct x86_pmu knc_pmu = {
+static const struct x86_pmu knc_pmu __initconst = {
 	.name			= "knc",
 	.handle_irq		= knc_pmu_handle_irq,
 	.disable_all		= knc_pmu_disable_all,
diff --git a/arch/x86/kernel/cpu/perf_event_p4.c b/arch/x86/kernel/cpu/perf_event_p4.c
index 92c7e39a079..3486e666035 100644
--- a/arch/x86/kernel/cpu/perf_event_p4.c
+++ b/arch/x86/kernel/cpu/perf_event_p4.c
@@ -895,8 +895,8 @@ static void p4_pmu_disable_pebs(void)
 	 * So at moment let leave metrics turned on forever -- it's
 	 * ok for now but need to be revisited!
 	 *
-	 * (void)wrmsrl_safe(MSR_IA32_PEBS_ENABLE, (u64)0);
-	 * (void)wrmsrl_safe(MSR_P4_PEBS_MATRIX_VERT, (u64)0);
+	 * (void)wrmsrl_safe(MSR_IA32_PEBS_ENABLE, 0);
+	 * (void)wrmsrl_safe(MSR_P4_PEBS_MATRIX_VERT, 0);
 	 */
 }
 
@@ -910,8 +910,7 @@ static inline void p4_pmu_disable_event(struct perf_event *event)
 	 * asserted again and again
 	 */
 	(void)wrmsrl_safe(hwc->config_base,
-		(u64)(p4_config_unpack_cccr(hwc->config)) &
-			~P4_CCCR_ENABLE & ~P4_CCCR_OVF & ~P4_CCCR_RESERVED);
+		p4_config_unpack_cccr(hwc->config) & ~P4_CCCR_ENABLE & ~P4_CCCR_OVF & ~P4_CCCR_RESERVED);
 }
 
 static void p4_pmu_disable_all(void)
@@ -957,7 +956,7 @@ static void p4_pmu_enable_event(struct perf_event *event)
 	u64 escr_addr, cccr;
 
 	bind = &p4_event_bind_map[idx];
-	escr_addr = (u64)bind->escr_msr[thread];
+	escr_addr = bind->escr_msr[thread];
 
 	/*
 	 * - we dont support cascaded counters yet
diff --git a/arch/x86/kernel/cpu/perf_event_p6.c b/arch/x86/kernel/cpu/perf_event_p6.c
index 4820c232a0b..b1e2fe11532 100644
--- a/arch/x86/kernel/cpu/perf_event_p6.c
+++ b/arch/x86/kernel/cpu/perf_event_p6.c
@@ -19,7 +19,7 @@ static const u64 p6_perfmon_event_map[] =
 
 };
 
-static u64 p6_hw_cache_event_ids
+static const u64 __initconst p6_hw_cache_event_ids
 				[PERF_COUNT_HW_CACHE_MAX]
 				[PERF_COUNT_HW_CACHE_OP_MAX]
 				[PERF_COUNT_HW_CACHE_RESULT_MAX] =
diff --git a/arch/x86/kernel/cpu/proc.c b/arch/x86/kernel/cpu/proc.c
index e280253f6f9..37a198bd48c 100644
--- a/arch/x86/kernel/cpu/proc.c
+++ b/arch/x86/kernel/cpu/proc.c
@@ -34,9 +34,9 @@ static void show_cpuinfo_misc(struct seq_file *m, struct cpuinfo_x86 *c)
 		   "fpu_exception\t: %s\n"
 		   "cpuid level\t: %d\n"
 		   "wp\t\t: %s\n",
-		   c->fdiv_bug ? "yes" : "no",
-		   c->f00f_bug ? "yes" : "no",
-		   c->coma_bug ? "yes" : "no",
+		   static_cpu_has_bug(X86_BUG_FDIV) ? "yes" : "no",
+		   static_cpu_has_bug(X86_BUG_F00F) ? "yes" : "no",
+		   static_cpu_has_bug(X86_BUG_COMA) ? "yes" : "no",
 		   c->hard_math ? "yes" : "no",
 		   c->hard_math ? "yes" : "no",
 		   c->cpuid_level,
diff --git a/arch/x86/kernel/doublefault_32.c b/arch/x86/kernel/doublefault_32.c
index 37250fe490b..155a13f33ed 100644
--- a/arch/x86/kernel/doublefault_32.c
+++ b/arch/x86/kernel/doublefault_32.c
@@ -20,7 +20,7 @@ static void doublefault_fn(void)
 	struct desc_ptr gdt_desc = {0, 0};
 	unsigned long gdt, tss;
 
-	store_gdt(&gdt_desc);
+	native_store_gdt(&gdt_desc);
 	gdt = gdt_desc.address;
 
 	printk(KERN_EMERG "PANIC: double fault, gdt at %08lx [%d bytes]\n", gdt, gdt_desc.size);
diff --git a/arch/x86/kernel/early_printk.c b/arch/x86/kernel/early_printk.c
index 9b9f18b4991..d15f575a861 100644
--- a/arch/x86/kernel/early_printk.c
+++ b/arch/x86/kernel/early_printk.c
@@ -169,25 +169,9 @@ static struct console early_serial_console = {
 	.index =	-1,
 };
 
-/* Direct interface for emergencies */
-static struct console *early_console = &early_vga_console;
-static int __initdata early_console_initialized;
-
-asmlinkage void early_printk(const char *fmt, ...)
-{
-	char buf[512];
-	int n;
-	va_list ap;
-
-	va_start(ap, fmt);
-	n = vscnprintf(buf, sizeof(buf), fmt, ap);
-	early_console->write(early_console, buf, n);
-	va_end(ap);
-}
-
 static inline void early_console_register(struct console *con, int keep_early)
 {
-	if (early_console->index != -1) {
+	if (con->index != -1) {
 		printk(KERN_CRIT "ERROR: earlyprintk= %s already used\n",
 		       con->name);
 		return;
@@ -207,9 +191,8 @@ static int __init setup_early_printk(char *buf)
 	if (!buf)
 		return 0;
 
-	if (early_console_initialized)
+	if (early_console)
 		return 0;
-	early_console_initialized = 1;
 
 	keep = (strstr(buf, "keep") != NULL);
 
diff --git a/arch/x86/kernel/head64.c b/arch/x86/kernel/head64.c
index c5e403f6d86..101ac1a9263 100644
--- a/arch/x86/kernel/head64.c
+++ b/arch/x86/kernel/head64.c
@@ -144,10 +144,10 @@ void __init x86_64_start_kernel(char * real_mode_data)
 	 * Build-time sanity checks on the kernel image and module
 	 * area mappings. (these are purely build-time and produce no code)
 	 */
-	BUILD_BUG_ON(MODULES_VADDR < KERNEL_IMAGE_START);
-	BUILD_BUG_ON(MODULES_VADDR-KERNEL_IMAGE_START < KERNEL_IMAGE_SIZE);
+	BUILD_BUG_ON(MODULES_VADDR < __START_KERNEL_map);
+	BUILD_BUG_ON(MODULES_VADDR - __START_KERNEL_map < KERNEL_IMAGE_SIZE);
 	BUILD_BUG_ON(MODULES_LEN + KERNEL_IMAGE_SIZE > 2*PUD_SIZE);
-	BUILD_BUG_ON((KERNEL_IMAGE_START & ~PMD_MASK) != 0);
+	BUILD_BUG_ON((__START_KERNEL_map & ~PMD_MASK) != 0);
 	BUILD_BUG_ON((MODULES_VADDR & ~PMD_MASK) != 0);
 	BUILD_BUG_ON(!(MODULES_VADDR > __START_KERNEL));
 	BUILD_BUG_ON(!(((MODULES_END - 1) & PGDIR_MASK) ==
diff --git a/arch/x86/kernel/kprobes/core.c b/arch/x86/kernel/kprobes/core.c
index 7bfe318d3d8..9895a9a4138 100644
--- a/arch/x86/kernel/kprobes/core.c
+++ b/arch/x86/kernel/kprobes/core.c
@@ -353,7 +353,11 @@ int __kprobes __copy_instruction(u8 *dest, u8 *src)
 		 * have given.
 		 */
 		newdisp = (u8 *) src + (s64) insn.displacement.value - (u8 *) dest;
-		BUG_ON((s64) (s32) newdisp != newdisp); /* Sanity check.  */
+		if ((s64) (s32) newdisp != newdisp) {
+			pr_err("Kprobes error: new displacement does not fit into s32 (%llx)\n", newdisp);
+			pr_err("\tSrc: %p, Dest: %p, old disp: %x\n", src, dest, insn.displacement.value);
+			return 0;
+		}
 		disp = (u8 *) dest + insn_offset_displacement(&insn);
 		*(s32 *) disp = (s32) newdisp;
 	}
diff --git a/arch/x86/kernel/kvm.c b/arch/x86/kernel/kvm.c
index b686a904d7c..cd6d9a5a42f 100644
--- a/arch/x86/kernel/kvm.c
+++ b/arch/x86/kernel/kvm.c
@@ -20,6 +20,7 @@
  *   Authors: Anthony Liguori <aliguori@us.ibm.com>
  */
 
+#include <linux/context_tracking.h>
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/kvm_para.h>
@@ -43,7 +44,6 @@
 #include <asm/apicdef.h>
 #include <asm/hypervisor.h>
 #include <asm/kvm_guest.h>
-#include <asm/context_tracking.h>
 
 static int kvmapf = 1;
 
@@ -254,16 +254,18 @@ EXPORT_SYMBOL_GPL(kvm_read_and_reset_pf_reason);
 dotraplinkage void __kprobes
 do_async_page_fault(struct pt_regs *regs, unsigned long error_code)
 {
+	enum ctx_state prev_state;
+
 	switch (kvm_read_and_reset_pf_reason()) {
 	default:
 		do_page_fault(regs, error_code);
 		break;
 	case KVM_PV_REASON_PAGE_NOT_PRESENT:
 		/* page is swapped out by the host. */
-		exception_enter(regs);
+		prev_state = exception_enter();
 		exit_idle();
 		kvm_async_pf_task_wait((u32)read_cr2());
-		exception_exit(regs);
+		exception_exit(prev_state);
 		break;
 	case KVM_PV_REASON_PAGE_READY:
 		rcu_irq_enter();
diff --git a/arch/x86/kernel/microcode_core_early.c b/arch/x86/kernel/microcode_core_early.c
index 577db8417d1..833d51d6ee0 100644
--- a/arch/x86/kernel/microcode_core_early.c
+++ b/arch/x86/kernel/microcode_core_early.c
@@ -45,9 +45,6 @@ static int __cpuinit x86_vendor(void)
 	u32 eax = 0x00000000;
 	u32 ebx, ecx = 0, edx;
 
-	if (!have_cpuid_p())
-		return X86_VENDOR_UNKNOWN;
-
 	native_cpuid(&eax, &ebx, &ecx, &edx);
 
 	if (CPUID_IS(CPUID_INTEL1, CPUID_INTEL2, CPUID_INTEL3, ebx, ecx, edx))
@@ -59,18 +56,45 @@ static int __cpuinit x86_vendor(void)
 	return X86_VENDOR_UNKNOWN;
 }
 
+static int __cpuinit x86_family(void)
+{
+	u32 eax = 0x00000001;
+	u32 ebx, ecx = 0, edx;
+	int x86;
+
+	native_cpuid(&eax, &ebx, &ecx, &edx);
+
+	x86 = (eax >> 8) & 0xf;
+	if (x86 == 15)
+		x86 += (eax >> 20) & 0xff;
+
+	return x86;
+}
+
 void __init load_ucode_bsp(void)
 {
-	int vendor = x86_vendor();
+	int vendor, x86;
+
+	if (!have_cpuid_p())
+		return;
 
-	if (vendor == X86_VENDOR_INTEL)
+	vendor = x86_vendor();
+	x86 = x86_family();
+
+	if (vendor == X86_VENDOR_INTEL && x86 >= 6)
 		load_ucode_intel_bsp();
 }
 
 void __cpuinit load_ucode_ap(void)
 {
-	int vendor = x86_vendor();
+	int vendor, x86;
+
+	if (!have_cpuid_p())
+		return;
+
+	vendor = x86_vendor();
+	x86 = x86_family();
 
-	if (vendor == X86_VENDOR_INTEL)
+	if (vendor == X86_VENDOR_INTEL && x86 >= 6)
 		load_ucode_intel_ap();
 }
diff --git a/arch/x86/kernel/microcode_intel_early.c b/arch/x86/kernel/microcode_intel_early.c
index 7890bc83895..d893e8ed8ac 100644
--- a/arch/x86/kernel/microcode_intel_early.c
+++ b/arch/x86/kernel/microcode_intel_early.c
@@ -90,13 +90,13 @@ microcode_phys(struct microcode_intel **mc_saved_tmp,
 	struct microcode_intel ***mc_saved;
 
 	mc_saved = (struct microcode_intel ***)
-		   __pa_symbol(&mc_saved_data->mc_saved);
+		   __pa_nodebug(&mc_saved_data->mc_saved);
 	for (i = 0; i < mc_saved_data->mc_saved_count; i++) {
 		struct microcode_intel *p;
 
 		p = *(struct microcode_intel **)
-			__pa(mc_saved_data->mc_saved + i);
-		mc_saved_tmp[i] = (struct microcode_intel *)__pa(p);
+			__pa_nodebug(mc_saved_data->mc_saved + i);
+		mc_saved_tmp[i] = (struct microcode_intel *)__pa_nodebug(p);
 	}
 }
 #endif
@@ -562,7 +562,7 @@ scan_microcode(unsigned long start, unsigned long end,
 	struct cpio_data cd;
 	long offset = 0;
 #ifdef CONFIG_X86_32
-	char *p = (char *)__pa_symbol(ucode_name);
+	char *p = (char *)__pa_nodebug(ucode_name);
 #else
 	char *p = ucode_name;
 #endif
@@ -630,8 +630,8 @@ static void __cpuinit print_ucode(struct ucode_cpu_info *uci)
 	if (mc_intel == NULL)
 		return;
 
-	delay_ucode_info_p = (int *)__pa_symbol(&delay_ucode_info);
-	current_mc_date_p = (int *)__pa_symbol(&current_mc_date);
+	delay_ucode_info_p = (int *)__pa_nodebug(&delay_ucode_info);
+	current_mc_date_p = (int *)__pa_nodebug(&current_mc_date);
 
 	*delay_ucode_info_p = 1;
 	*current_mc_date_p = mc_intel->hdr.date;
@@ -659,8 +659,8 @@ static inline void __cpuinit print_ucode(struct ucode_cpu_info *uci)
 }
 #endif
 
-static int apply_microcode_early(struct mc_saved_data *mc_saved_data,
-				 struct ucode_cpu_info *uci)
+static int __cpuinit apply_microcode_early(struct mc_saved_data *mc_saved_data,
+					   struct ucode_cpu_info *uci)
 {
 	struct microcode_intel *mc_intel;
 	unsigned int val[2];
@@ -741,15 +741,15 @@ load_ucode_intel_bsp(void)
 #ifdef CONFIG_X86_32
 	struct boot_params *boot_params_p;
 
-	boot_params_p = (struct boot_params *)__pa_symbol(&boot_params);
+	boot_params_p = (struct boot_params *)__pa_nodebug(&boot_params);
 	ramdisk_image = boot_params_p->hdr.ramdisk_image;
 	ramdisk_size  = boot_params_p->hdr.ramdisk_size;
 	initrd_start_early = ramdisk_image;
 	initrd_end_early = initrd_start_early + ramdisk_size;
 
 	_load_ucode_intel_bsp(
-		(struct mc_saved_data *)__pa_symbol(&mc_saved_data),
-		(unsigned long *)__pa_symbol(&mc_saved_in_initrd),
+		(struct mc_saved_data *)__pa_nodebug(&mc_saved_data),
+		(unsigned long *)__pa_nodebug(&mc_saved_in_initrd),
 		initrd_start_early, initrd_end_early, &uci);
 #else
 	ramdisk_image = boot_params.hdr.ramdisk_image;
@@ -772,10 +772,10 @@ void __cpuinit load_ucode_intel_ap(void)
 	unsigned long *initrd_start_p;
 
 	mc_saved_in_initrd_p =
-		(unsigned long *)__pa_symbol(mc_saved_in_initrd);
-	mc_saved_data_p = (struct mc_saved_data *)__pa_symbol(&mc_saved_data);
-	initrd_start_p = (unsigned long *)__pa_symbol(&initrd_start);
-	initrd_start_addr = (unsigned long)__pa_symbol(*initrd_start_p);
+		(unsigned long *)__pa_nodebug(mc_saved_in_initrd);
+	mc_saved_data_p = (struct mc_saved_data *)__pa_nodebug(&mc_saved_data);
+	initrd_start_p = (unsigned long *)__pa_nodebug(&initrd_start);
+	initrd_start_addr = (unsigned long)__pa_nodebug(*initrd_start_p);
 #else
 	mc_saved_data_p = &mc_saved_data;
 	mc_saved_in_initrd_p = mc_saved_in_initrd;
diff --git a/arch/x86/kernel/paravirt.c b/arch/x86/kernel/paravirt.c
index 17fff18a103..cd6de64cc48 100644
--- a/arch/x86/kernel/paravirt.c
+++ b/arch/x86/kernel/paravirt.c
@@ -263,6 +263,18 @@ void paravirt_leave_lazy_mmu(void)
 	leave_lazy(PARAVIRT_LAZY_MMU);
 }
 
+void paravirt_flush_lazy_mmu(void)
+{
+	preempt_disable();
+
+	if (paravirt_get_lazy_mode() == PARAVIRT_LAZY_MMU) {
+		arch_leave_lazy_mmu_mode();
+		arch_enter_lazy_mmu_mode();
+	}
+
+	preempt_enable();
+}
+
 void paravirt_start_context_switch(struct task_struct *prev)
 {
 	BUG_ON(preemptible());
@@ -292,18 +304,6 @@ enum paravirt_lazy_mode paravirt_get_lazy_mode(void)
 	return this_cpu_read(paravirt_lazy_mode);
 }
 
-void arch_flush_lazy_mmu_mode(void)
-{
-	preempt_disable();
-
-	if (paravirt_get_lazy_mode() == PARAVIRT_LAZY_MMU) {
-		arch_leave_lazy_mmu_mode();
-		arch_enter_lazy_mmu_mode();
-	}
-
-	preempt_enable();
-}
-
 struct pv_info pv_info = {
 	.name = "bare hardware",
 	.paravirt_enabled = 0,
@@ -360,7 +360,6 @@ struct pv_cpu_ops pv_cpu_ops = {
 	.set_ldt = native_set_ldt,
 	.load_gdt = native_load_gdt,
 	.load_idt = native_load_idt,
-	.store_gdt = native_store_gdt,
 	.store_idt = native_store_idt,
 	.store_tr = native_store_tr,
 	.load_tls = native_load_tls,
@@ -475,6 +474,7 @@ struct pv_mmu_ops pv_mmu_ops = {
 	.lazy_mode = {
 		.enter = paravirt_nop,
 		.leave = paravirt_nop,
+		.flush = paravirt_nop,
 	},
 
 	.set_fixmap = native_set_fixmap,
diff --git a/arch/x86/kernel/process.c b/arch/x86/kernel/process.c
index 14ae10031ff..14fcf55a5c5 100644
--- a/arch/x86/kernel/process.c
+++ b/arch/x86/kernel/process.c
@@ -301,13 +301,7 @@ void exit_idle(void)
 }
 #endif
 
-/*
- * The idle thread. There's no useful work to be
- * done, so just try to conserve power and have a
- * low exit latency (ie sit in a loop waiting for
- * somebody to say that they'd like to reschedule)
- */
-void cpu_idle(void)
+void arch_cpu_idle_prepare(void)
 {
 	/*
 	 * If we're the non-boot CPU, nothing set the stack canary up
@@ -317,71 +311,40 @@ void cpu_idle(void)
 	 * canaries already on the stack wont ever trigger).
 	 */
 	boot_init_stack_canary();
-	current_thread_info()->status |= TS_POLLING;
-
-	while (1) {
-		tick_nohz_idle_enter();
-
-		while (!need_resched()) {
-			rmb();
-
-			if (cpu_is_offline(smp_processor_id()))
-				play_dead();
-
-			/*
-			 * Idle routines should keep interrupts disabled
-			 * from here on, until they go to idle.
-			 * Otherwise, idle callbacks can misfire.
-			 */
-			local_touch_nmi();
-			local_irq_disable();
-
-			enter_idle();
-
-			/* Don't trace irqs off for idle */
-			stop_critical_timings();
-
-			/* enter_idle() needs rcu for notifiers */
-			rcu_idle_enter();
+}
 
-			if (cpuidle_idle_call())
-				x86_idle();
+void arch_cpu_idle_enter(void)
+{
+	local_touch_nmi();
+	enter_idle();
+}
 
-			rcu_idle_exit();
-			start_critical_timings();
+void arch_cpu_idle_exit(void)
+{
+	__exit_idle();
+}
 
-			/* In many cases the interrupt that ended idle
-			   has already called exit_idle. But some idle
-			   loops can be woken up without interrupt. */
-			__exit_idle();
-		}
+void arch_cpu_idle_dead(void)
+{
+	play_dead();
+}
 
-		tick_nohz_idle_exit();
-		preempt_enable_no_resched();
-		schedule();
-		preempt_disable();
-	}
+/*
+ * Called from the generic idle code.
+ */
+void arch_cpu_idle(void)
+{
+	if (cpuidle_idle_call())
+		x86_idle();
 }
 
 /*
- * We use this if we don't have any better
- * idle routine..
+ * We use this if we don't have any better idle routine..
  */
 void default_idle(void)
 {
 	trace_cpu_idle_rcuidle(1, smp_processor_id());
-	current_thread_info()->status &= ~TS_POLLING;
-	/*
-	 * TS_POLLING-cleared state must be visible before we
-	 * test NEED_RESCHED:
-	 */
-	smp_mb();
-
-	if (!need_resched())
-		safe_halt();	/* enables interrupts racelessly */
-	else
-		local_irq_enable();
-	current_thread_info()->status |= TS_POLLING;
+	safe_halt();
 	trace_cpu_idle_rcuidle(PWR_EVENT_EXIT, smp_processor_id());
 }
 #ifdef CONFIG_APM_MODULE
@@ -411,20 +374,6 @@ void stop_this_cpu(void *dummy)
 		halt();
 }
 
-/*
- * On SMP it's slightly faster (but much more power-consuming!)
- * to poll the ->work.need_resched flag instead of waiting for the
- * cross-CPU IPI to arrive. Use this option with caution.
- */
-static void poll_idle(void)
-{
-	trace_cpu_idle_rcuidle(0, smp_processor_id());
-	local_irq_enable();
-	while (!need_resched())
-		cpu_relax();
-	trace_cpu_idle_rcuidle(PWR_EVENT_EXIT, smp_processor_id());
-}
-
 bool amd_e400_c1e_detected;
 EXPORT_SYMBOL(amd_e400_c1e_detected);
 
@@ -489,13 +438,13 @@ static void amd_e400_idle(void)
 void __cpuinit select_idle_routine(const struct cpuinfo_x86 *c)
 {
 #ifdef CONFIG_SMP
-	if (x86_idle == poll_idle && smp_num_siblings > 1)
+	if (boot_option_idle_override == IDLE_POLL && smp_num_siblings > 1)
 		pr_warn_once("WARNING: polling idle and HT enabled, performance may degrade\n");
 #endif
-	if (x86_idle)
+	if (x86_idle || boot_option_idle_override == IDLE_POLL)
 		return;
 
-	if (cpu_has_amd_erratum(amd_erratum_400)) {
+	if (cpu_has_bug(c, X86_BUG_AMD_APIC_C1E)) {
 		/* E400: APIC timer interrupt does not wake up CPU from C1e */
 		pr_info("using AMD E400 aware idle routine\n");
 		x86_idle = amd_e400_idle;
@@ -517,8 +466,8 @@ static int __init idle_setup(char *str)
 
 	if (!strcmp(str, "poll")) {
 		pr_info("using polling idle threads\n");
-		x86_idle = poll_idle;
 		boot_option_idle_override = IDLE_POLL;
+		cpu_idle_poll_ctrl(true);
 	} else if (!strcmp(str, "halt")) {
 		/*
 		 * When the boot option of idle=halt is added, halt is
diff --git a/arch/x86/kernel/quirks.c b/arch/x86/kernel/quirks.c
index 26ee48a33dc..04ee1e2e4c0 100644
--- a/arch/x86/kernel/quirks.c
+++ b/arch/x86/kernel/quirks.c
@@ -354,18 +354,22 @@ static void ati_force_hpet_resume(void)
 
 static u32 ati_ixp4x0_rev(struct pci_dev *dev)
 {
-	u32 d;
-	u8  b;
+	int err = 0;
+	u32 d = 0;
+	u8  b = 0;
 
-	pci_read_config_byte(dev, 0xac, &b);
+	err = pci_read_config_byte(dev, 0xac, &b);
 	b &= ~(1<<5);
-	pci_write_config_byte(dev, 0xac, b);
-	pci_read_config_dword(dev, 0x70, &d);
+	err |= pci_write_config_byte(dev, 0xac, b);
+	err |= pci_read_config_dword(dev, 0x70, &d);
 	d |= 1<<8;
-	pci_write_config_dword(dev, 0x70, d);
-	pci_read_config_dword(dev, 0x8, &d);
+	err |= pci_write_config_dword(dev, 0x70, d);
+	err |= pci_read_config_dword(dev, 0x8, &d);
 	d &= 0xff;
 	dev_printk(KERN_DEBUG, &dev->dev, "SB4X0 revision 0x%x\n", d);
+
+	WARN_ON_ONCE(err);
+
 	return d;
 }
 
diff --git a/arch/x86/kernel/rtc.c b/arch/x86/kernel/rtc.c
index 2e8f3d3b564..198eb201ed3 100644
--- a/arch/x86/kernel/rtc.c
+++ b/arch/x86/kernel/rtc.c
@@ -13,6 +13,7 @@
 #include <asm/x86_init.h>
 #include <asm/time.h>
 #include <asm/mrst.h>
+#include <asm/rtc.h>
 
 #ifdef CONFIG_X86_32
 /*
@@ -36,70 +37,24 @@ EXPORT_SYMBOL(rtc_lock);
  * nowtime is written into the registers of the CMOS clock, it will
  * jump to the next second precisely 500 ms later. Check the Motorola
  * MC146818A or Dallas DS12887 data sheet for details.
- *
- * BUG: This routine does not handle hour overflow properly; it just
- *      sets the minutes. Usually you'll only notice that after reboot!
  */
 int mach_set_rtc_mmss(unsigned long nowtime)
 {
-	int real_seconds, real_minutes, cmos_minutes;
-	unsigned char save_control, save_freq_select;
-	unsigned long flags;
+	struct rtc_time tm;
 	int retval = 0;
 
-	spin_lock_irqsave(&rtc_lock, flags);
-
-	 /* tell the clock it's being set */
-	save_control = CMOS_READ(RTC_CONTROL);
-	CMOS_WRITE((save_control|RTC_SET), RTC_CONTROL);
-
-	/* stop and reset prescaler */
-	save_freq_select = CMOS_READ(RTC_FREQ_SELECT);
-	CMOS_WRITE((save_freq_select|RTC_DIV_RESET2), RTC_FREQ_SELECT);
-
-	cmos_minutes = CMOS_READ(RTC_MINUTES);
-	if (!(save_control & RTC_DM_BINARY) || RTC_ALWAYS_BCD)
-		cmos_minutes = bcd2bin(cmos_minutes);
-
-	/*
-	 * since we're only adjusting minutes and seconds,
-	 * don't interfere with hour overflow. This avoids
-	 * messing with unknown time zones but requires your
-	 * RTC not to be off by more than 15 minutes
-	 */
-	real_seconds = nowtime % 60;
-	real_minutes = nowtime / 60;
-	/* correct for half hour time zone */
-	if (((abs(real_minutes - cmos_minutes) + 15)/30) & 1)
-		real_minutes += 30;
-	real_minutes %= 60;
-
-	if (abs(real_minutes - cmos_minutes) < 30) {
-		if (!(save_control & RTC_DM_BINARY) || RTC_ALWAYS_BCD) {
-			real_seconds = bin2bcd(real_seconds);
-			real_minutes = bin2bcd(real_minutes);
-		}
-		CMOS_WRITE(real_seconds, RTC_SECONDS);
-		CMOS_WRITE(real_minutes, RTC_MINUTES);
+	rtc_time_to_tm(nowtime, &tm);
+	if (!rtc_valid_tm(&tm)) {
+		retval = set_rtc_time(&tm);
+		if (retval)
+			printk(KERN_ERR "%s: RTC write failed with error %d\n",
+			       __FUNCTION__, retval);
 	} else {
-		printk_once(KERN_NOTICE
-		       "set_rtc_mmss: can't update from %d to %d\n",
-		       cmos_minutes, real_minutes);
-		retval = -1;
+		printk(KERN_ERR
+		       "%s: Invalid RTC value: write of %lx to RTC failed\n",
+			__FUNCTION__, nowtime);
+		retval = -EINVAL;
 	}
-
-	/* The following flags have to be released exactly in this order,
-	 * otherwise the DS12887 (popular MC146818A clone with integrated
-	 * battery and quartz) will not reset the oscillator and will not
-	 * update precisely 500 ms later. You won't find this mentioned in
-	 * the Dallas Semiconductor data sheets, but who believes data
-	 * sheets anyway ...                           -- Markus Kuhn
-	 */
-	CMOS_WRITE(save_control, RTC_CONTROL);
-	CMOS_WRITE(save_freq_select, RTC_FREQ_SELECT);
-
-	spin_unlock_irqrestore(&rtc_lock, flags);
-
 	return retval;
 }
 
diff --git a/arch/x86/kernel/setup.c b/arch/x86/kernel/setup.c
index 90d8cc930f5..4689855c2f8 100644
--- a/arch/x86/kernel/setup.c
+++ b/arch/x86/kernel/setup.c
@@ -82,7 +82,6 @@
 #include <asm/timer.h>
 #include <asm/i8259.h>
 #include <asm/sections.h>
-#include <asm/dmi.h>
 #include <asm/io_apic.h>
 #include <asm/ist.h>
 #include <asm/setup_arch.h>
@@ -173,12 +172,10 @@ static struct resource bss_resource = {
 /* cpu data as detected by the assembly code in head.S */
 struct cpuinfo_x86 new_cpu_data __cpuinitdata = {
 	.wp_works_ok = -1,
-	.fdiv_bug = -1,
 };
 /* common cpu data for all cpus */
 struct cpuinfo_x86 boot_cpu_data __read_mostly = {
 	.wp_works_ok = -1,
-	.fdiv_bug = -1,
 };
 EXPORT_SYMBOL(boot_cpu_data);
 
@@ -507,11 +504,14 @@ static void __init memblock_x86_reserve_range_setup_data(void)
 /*
  * Keep the crash kernel below this limit.  On 32 bits earlier kernels
  * would limit the kernel to the low 512 MiB due to mapping restrictions.
+ * On 64bit, old kexec-tools need to under 896MiB.
  */
 #ifdef CONFIG_X86_32
-# define CRASH_KERNEL_ADDR_MAX	(512 << 20)
+# define CRASH_KERNEL_ADDR_LOW_MAX	(512 << 20)
+# define CRASH_KERNEL_ADDR_HIGH_MAX	(512 << 20)
 #else
-# define CRASH_KERNEL_ADDR_MAX	MAXMEM
+# define CRASH_KERNEL_ADDR_LOW_MAX	(896UL<<20)
+# define CRASH_KERNEL_ADDR_HIGH_MAX	MAXMEM
 #endif
 
 static void __init reserve_crashkernel_low(void)
@@ -521,19 +521,35 @@ static void __init reserve_crashkernel_low(void)
 	unsigned long long low_base = 0, low_size = 0;
 	unsigned long total_low_mem;
 	unsigned long long base;
+	bool auto_set = false;
 	int ret;
 
 	total_low_mem = memblock_mem_size(1UL<<(32-PAGE_SHIFT));
+	/* crashkernel=Y,low */
 	ret = parse_crashkernel_low(boot_command_line, total_low_mem,
 						&low_size, &base);
-	if (ret != 0 || low_size <= 0)
-		return;
+	if (ret != 0) {
+		/*
+		 * two parts from lib/swiotlb.c:
+		 *	swiotlb size: user specified with swiotlb= or default.
+		 *	swiotlb overflow buffer: now is hardcoded to 32k.
+		 *		We round it to 8M for other buffers that
+		 *		may need to stay low too.
+		 */
+		low_size = swiotlb_size_or_default() + (8UL<<20);
+		auto_set = true;
+	} else {
+		/* passed with crashkernel=0,low ? */
+		if (!low_size)
+			return;
+	}
 
 	low_base = memblock_find_in_range(low_size, (1ULL<<32),
 					low_size, alignment);
 
 	if (!low_base) {
-		pr_info("crashkernel low reservation failed - No suitable area found.\n");
+		if (!auto_set)
+			pr_info("crashkernel low reservation failed - No suitable area found.\n");
 
 		return;
 	}
@@ -554,14 +570,22 @@ static void __init reserve_crashkernel(void)
 	const unsigned long long alignment = 16<<20;	/* 16M */
 	unsigned long long total_mem;
 	unsigned long long crash_size, crash_base;
+	bool high = false;
 	int ret;
 
 	total_mem = memblock_phys_mem_size();
 
+	/* crashkernel=XM */
 	ret = parse_crashkernel(boot_command_line, total_mem,
 			&crash_size, &crash_base);
-	if (ret != 0 || crash_size <= 0)
-		return;
+	if (ret != 0 || crash_size <= 0) {
+		/* crashkernel=X,high */
+		ret = parse_crashkernel_high(boot_command_line, total_mem,
+				&crash_size, &crash_base);
+		if (ret != 0 || crash_size <= 0)
+			return;
+		high = true;
+	}
 
 	/* 0 means: find the address automatically */
 	if (crash_base <= 0) {
@@ -569,7 +593,9 @@ static void __init reserve_crashkernel(void)
 		 *  kexec want bzImage is below CRASH_KERNEL_ADDR_MAX
 		 */
 		crash_base = memblock_find_in_range(alignment,
-			       CRASH_KERNEL_ADDR_MAX, crash_size, alignment);
+					high ? CRASH_KERNEL_ADDR_HIGH_MAX :
+					       CRASH_KERNEL_ADDR_LOW_MAX,
+					crash_size, alignment);
 
 		if (!crash_base) {
 			pr_info("crashkernel reservation failed - No suitable area found.\n");
diff --git a/arch/x86/kernel/smpboot.c b/arch/x86/kernel/smpboot.c
index 9f190a2a00e..9c73b51817e 100644
--- a/arch/x86/kernel/smpboot.c
+++ b/arch/x86/kernel/smpboot.c
@@ -284,7 +284,7 @@ notrace static void __cpuinit start_secondary(void *unused)
 	x86_cpuinit.setup_percpu_clockev();
 
 	wmb();
-	cpu_idle();
+	cpu_startup_entry(CPUHP_ONLINE);
 }
 
 void __init smp_store_boot_cpu_info(void)
diff --git a/arch/x86/kernel/traps.c b/arch/x86/kernel/traps.c
index 68bda7a8415..772e2a846de 100644
--- a/arch/x86/kernel/traps.c
+++ b/arch/x86/kernel/traps.c
@@ -12,6 +12,7 @@
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
+#include <linux/context_tracking.h>
 #include <linux/interrupt.h>
 #include <linux/kallsyms.h>
 #include <linux/spinlock.h>
@@ -55,8 +56,7 @@
 #include <asm/i387.h>
 #include <asm/fpu-internal.h>
 #include <asm/mce.h>
-#include <asm/context_tracking.h>
-
+#include <asm/fixmap.h>
 #include <asm/mach_traps.h>
 
 #ifdef CONFIG_X86_64
@@ -176,34 +176,38 @@ do_trap(int trapnr, int signr, char *str, struct pt_regs *regs,
 #define DO_ERROR(trapnr, signr, str, name)				\
 dotraplinkage void do_##name(struct pt_regs *regs, long error_code)	\
 {									\
-	exception_enter(regs);						\
+	enum ctx_state prev_state;					\
+									\
+	prev_state = exception_enter();					\
 	if (notify_die(DIE_TRAP, str, regs, error_code,			\
 			trapnr, signr) == NOTIFY_STOP) {		\
-		exception_exit(regs);					\
+		exception_exit(prev_state);				\
 		return;							\
 	}								\
 	conditional_sti(regs);						\
 	do_trap(trapnr, signr, str, regs, error_code, NULL);		\
-	exception_exit(regs);						\
+	exception_exit(prev_state);					\
 }
 
 #define DO_ERROR_INFO(trapnr, signr, str, name, sicode, siaddr)		\
 dotraplinkage void do_##name(struct pt_regs *regs, long error_code)	\
 {									\
 	siginfo_t info;							\
+	enum ctx_state prev_state;					\
+									\
 	info.si_signo = signr;						\
 	info.si_errno = 0;						\
 	info.si_code = sicode;						\
 	info.si_addr = (void __user *)siaddr;				\
-	exception_enter(regs);						\
+	prev_state = exception_enter();					\
 	if (notify_die(DIE_TRAP, str, regs, error_code,			\
 			trapnr, signr) == NOTIFY_STOP) {		\
-		exception_exit(regs);					\
+		exception_exit(prev_state);				\
 		return;							\
 	}								\
 	conditional_sti(regs);						\
 	do_trap(trapnr, signr, str, regs, error_code, &info);		\
-	exception_exit(regs);						\
+	exception_exit(prev_state);					\
 }
 
 DO_ERROR_INFO(X86_TRAP_DE, SIGFPE, "divide error", divide_error, FPE_INTDIV,
@@ -226,14 +230,16 @@ DO_ERROR_INFO(X86_TRAP_AC, SIGBUS, "alignment check", alignment_check,
 /* Runs on IST stack */
 dotraplinkage void do_stack_segment(struct pt_regs *regs, long error_code)
 {
-	exception_enter(regs);
+	enum ctx_state prev_state;
+
+	prev_state = exception_enter();
 	if (notify_die(DIE_TRAP, "stack segment", regs, error_code,
 		       X86_TRAP_SS, SIGBUS) != NOTIFY_STOP) {
 		preempt_conditional_sti(regs);
 		do_trap(X86_TRAP_SS, SIGBUS, "stack segment", regs, error_code, NULL);
 		preempt_conditional_cli(regs);
 	}
-	exception_exit(regs);
+	exception_exit(prev_state);
 }
 
 dotraplinkage void do_double_fault(struct pt_regs *regs, long error_code)
@@ -241,7 +247,7 @@ dotraplinkage void do_double_fault(struct pt_regs *regs, long error_code)
 	static const char str[] = "double fault";
 	struct task_struct *tsk = current;
 
-	exception_enter(regs);
+	exception_enter();
 	/* Return not checked because double check cannot be ignored */
 	notify_die(DIE_TRAP, str, regs, error_code, X86_TRAP_DF, SIGSEGV);
 
@@ -261,8 +267,9 @@ dotraplinkage void __kprobes
 do_general_protection(struct pt_regs *regs, long error_code)
 {
 	struct task_struct *tsk;
+	enum ctx_state prev_state;
 
-	exception_enter(regs);
+	prev_state = exception_enter();
 	conditional_sti(regs);
 
 #ifdef CONFIG_X86_32
@@ -300,12 +307,14 @@ do_general_protection(struct pt_regs *regs, long error_code)
 
 	force_sig(SIGSEGV, tsk);
 exit:
-	exception_exit(regs);
+	exception_exit(prev_state);
 }
 
 /* May run on IST stack. */
 dotraplinkage void __kprobes notrace do_int3(struct pt_regs *regs, long error_code)
 {
+	enum ctx_state prev_state;
+
 #ifdef CONFIG_DYNAMIC_FTRACE
 	/*
 	 * ftrace must be first, everything else may cause a recursive crash.
@@ -315,7 +324,7 @@ dotraplinkage void __kprobes notrace do_int3(struct pt_regs *regs, long error_co
 	    ftrace_int3_handler(regs))
 		return;
 #endif
-	exception_enter(regs);
+	prev_state = exception_enter();
 #ifdef CONFIG_KGDB_LOW_LEVEL_TRAP
 	if (kgdb_ll_trap(DIE_INT3, "int3", regs, error_code, X86_TRAP_BP,
 				SIGTRAP) == NOTIFY_STOP)
@@ -336,7 +345,7 @@ dotraplinkage void __kprobes notrace do_int3(struct pt_regs *regs, long error_co
 	preempt_conditional_cli(regs);
 	debug_stack_usage_dec();
 exit:
-	exception_exit(regs);
+	exception_exit(prev_state);
 }
 
 #ifdef CONFIG_X86_64
@@ -393,11 +402,12 @@ asmlinkage __kprobes struct pt_regs *sync_regs(struct pt_regs *eregs)
 dotraplinkage void __kprobes do_debug(struct pt_regs *regs, long error_code)
 {
 	struct task_struct *tsk = current;
+	enum ctx_state prev_state;
 	int user_icebp = 0;
 	unsigned long dr6;
 	int si_code;
 
-	exception_enter(regs);
+	prev_state = exception_enter();
 
 	get_debugreg(dr6, 6);
 
@@ -467,7 +477,7 @@ dotraplinkage void __kprobes do_debug(struct pt_regs *regs, long error_code)
 	debug_stack_usage_dec();
 
 exit:
-	exception_exit(regs);
+	exception_exit(prev_state);
 }
 
 /*
@@ -561,17 +571,21 @@ void math_error(struct pt_regs *regs, int error_code, int trapnr)
 
 dotraplinkage void do_coprocessor_error(struct pt_regs *regs, long error_code)
 {
-	exception_enter(regs);
+	enum ctx_state prev_state;
+
+	prev_state = exception_enter();
 	math_error(regs, error_code, X86_TRAP_MF);
-	exception_exit(regs);
+	exception_exit(prev_state);
 }
 
 dotraplinkage void
 do_simd_coprocessor_error(struct pt_regs *regs, long error_code)
 {
-	exception_enter(regs);
+	enum ctx_state prev_state;
+
+	prev_state = exception_enter();
 	math_error(regs, error_code, X86_TRAP_XF);
-	exception_exit(regs);
+	exception_exit(prev_state);
 }
 
 dotraplinkage void
@@ -639,7 +653,9 @@ EXPORT_SYMBOL_GPL(math_state_restore);
 dotraplinkage void __kprobes
 do_device_not_available(struct pt_regs *regs, long error_code)
 {
-	exception_enter(regs);
+	enum ctx_state prev_state;
+
+	prev_state = exception_enter();
 	BUG_ON(use_eager_fpu());
 
 #ifdef CONFIG_MATH_EMULATION
@@ -650,7 +666,7 @@ do_device_not_available(struct pt_regs *regs, long error_code)
 
 		info.regs = regs;
 		math_emulate(&info);
-		exception_exit(regs);
+		exception_exit(prev_state);
 		return;
 	}
 #endif
@@ -658,15 +674,16 @@ do_device_not_available(struct pt_regs *regs, long error_code)
 #ifdef CONFIG_X86_32
 	conditional_sti(regs);
 #endif
-	exception_exit(regs);
+	exception_exit(prev_state);
 }
 
 #ifdef CONFIG_X86_32
 dotraplinkage void do_iret_error(struct pt_regs *regs, long error_code)
 {
 	siginfo_t info;
+	enum ctx_state prev_state;
 
-	exception_enter(regs);
+	prev_state = exception_enter();
 	local_irq_enable();
 
 	info.si_signo = SIGILL;
@@ -678,7 +695,7 @@ dotraplinkage void do_iret_error(struct pt_regs *regs, long error_code)
 		do_trap(X86_TRAP_IRET, SIGILL, "iret exception", regs, error_code,
 			&info);
 	}
-	exception_exit(regs);
+	exception_exit(prev_state);
 }
 #endif
 
@@ -753,6 +770,14 @@ void __init trap_init(void)
 #endif
 
 	/*
+	 * Set the IDT descriptor to a fixed read-only location, so that the
+	 * "sidt" instruction will not leak the location of the kernel, and
+	 * to defend the IDT against arbitrary memory write vulnerabilities.
+	 * It will be reloaded in cpu_init() */
+	__set_fixmap(FIX_RO_IDT, __pa_symbol(idt_table), PAGE_KERNEL_RO);
+	idt_descr.address = fix_to_virt(FIX_RO_IDT);
+
+	/*
 	 * Should be a barrier for any external CPU state:
 	 */
 	cpu_init();
diff --git a/arch/x86/kernel/tsc.c b/arch/x86/kernel/tsc.c
index 4b9ea101fe3..098b3cfda72 100644
--- a/arch/x86/kernel/tsc.c
+++ b/arch/x86/kernel/tsc.c
@@ -768,7 +768,8 @@ static cycle_t read_tsc(struct clocksource *cs)
 
 static void resume_tsc(struct clocksource *cs)
 {
-	clocksource_tsc.cycle_last = 0;
+	if (!boot_cpu_has(X86_FEATURE_NONSTOP_TSC_S3))
+		clocksource_tsc.cycle_last = 0;
 }
 
 static struct clocksource clocksource_tsc = {
@@ -939,6 +940,9 @@ static int __init init_tsc_clocksource(void)
 		clocksource_tsc.flags &= ~CLOCK_SOURCE_IS_CONTINUOUS;
 	}
 
+	if (boot_cpu_has(X86_FEATURE_NONSTOP_TSC_S3))
+		clocksource_tsc.flags |= CLOCK_SOURCE_SUSPEND_NONSTOP;
+
 	/*
 	 * Trust the results of the earlier calibration on systems
 	 * exporting a reliable TSC.
diff --git a/arch/x86/kernel/uprobes.c b/arch/x86/kernel/uprobes.c
index 0ba4cfb4f41..2ed845928b5 100644
--- a/arch/x86/kernel/uprobes.c
+++ b/arch/x86/kernel/uprobes.c
@@ -697,3 +697,32 @@ bool arch_uprobe_skip_sstep(struct arch_uprobe *auprobe, struct pt_regs *regs)
 		send_sig(SIGTRAP, current, 0);
 	return ret;
 }
+
+unsigned long
+arch_uretprobe_hijack_return_addr(unsigned long trampoline_vaddr, struct pt_regs *regs)
+{
+	int rasize, ncopied;
+	unsigned long orig_ret_vaddr = 0; /* clear high bits for 32-bit apps */
+
+	rasize = is_ia32_task() ? 4 : 8;
+	ncopied = copy_from_user(&orig_ret_vaddr, (void __user *)regs->sp, rasize);
+	if (unlikely(ncopied))
+		return -1;
+
+	/* check whether address has been already hijacked */
+	if (orig_ret_vaddr == trampoline_vaddr)
+		return orig_ret_vaddr;
+
+	ncopied = copy_to_user((void __user *)regs->sp, &trampoline_vaddr, rasize);
+	if (likely(!ncopied))
+		return orig_ret_vaddr;
+
+	if (ncopied != rasize) {
+		pr_err("uprobe: return address clobbered: pid=%d, %%sp=%#lx, "
+			"%%ip=%#lx\n", current->pid, regs->sp, regs->ip);
+
+		force_sig_info(SIGSEGV, SEND_SIG_FORCED, current);
+	}
+
+	return -1;
+}
diff --git a/arch/x86/kernel/vmlinux.lds.S b/arch/x86/kernel/vmlinux.lds.S
index 22a1530146a..10c4f3006af 100644
--- a/arch/x86/kernel/vmlinux.lds.S
+++ b/arch/x86/kernel/vmlinux.lds.S
@@ -94,10 +94,6 @@ SECTIONS
 		_text = .;
 		/* bootstrapping code */
 		HEAD_TEXT
-#ifdef CONFIG_X86_32
-		. = ALIGN(PAGE_SIZE);
-		*(.text..page_aligned)
-#endif
 		. = ALIGN(8);
 		_stext = .;
 		TEXT_TEXT
diff --git a/arch/x86/kvm/lapic.c b/arch/x86/kvm/lapic.c
index 02b51dd4e4a..f77df1c5de6 100644
--- a/arch/x86/kvm/lapic.c
+++ b/arch/x86/kvm/lapic.c
@@ -1857,7 +1857,7 @@ int kvm_lapic_enable_pv_eoi(struct kvm_vcpu *vcpu, u64 data)
 	if (!pv_eoi_enabled(vcpu))
 		return 0;
 	return kvm_gfn_to_hva_cache_init(vcpu->kvm, &vcpu->arch.pv_eoi.data,
-					 addr);
+					 addr, sizeof(u8));
 }
 
 void kvm_lapic_init(void)
diff --git a/arch/x86/kvm/svm.c b/arch/x86/kvm/svm.c
index e1b1ce21bc0..7d39d70647e 100644
--- a/arch/x86/kvm/svm.c
+++ b/arch/x86/kvm/svm.c
@@ -555,7 +555,7 @@ static void svm_init_erratum_383(void)
 	int err;
 	u64 val;
 
-	if (!cpu_has_amd_erratum(amd_erratum_383))
+	if (!static_cpu_has_bug(X86_BUG_AMD_TLB_MMATCH))
 		return;
 
 	/* Use _safe variants to not break nested virtualization */
diff --git a/arch/x86/kvm/vmx.c b/arch/x86/kvm/vmx.c
index 6667042714c..867b81037f9 100644
--- a/arch/x86/kvm/vmx.c
+++ b/arch/x86/kvm/vmx.c
@@ -2459,7 +2459,7 @@ static int hardware_enable(void *garbage)
 		ept_sync_global();
 	}
 
-	store_gdt(&__get_cpu_var(host_gdt));
+	native_store_gdt(&__get_cpu_var(host_gdt));
 
 	return 0;
 }
diff --git a/arch/x86/kvm/x86.c b/arch/x86/kvm/x86.c
index f19ac0aca60..e1721324c27 100644
--- a/arch/x86/kvm/x86.c
+++ b/arch/x86/kvm/x86.c
@@ -1823,7 +1823,8 @@ static int kvm_pv_enable_async_pf(struct kvm_vcpu *vcpu, u64 data)
 		return 0;
 	}
 
-	if (kvm_gfn_to_hva_cache_init(vcpu->kvm, &vcpu->arch.apf.data, gpa))
+	if (kvm_gfn_to_hva_cache_init(vcpu->kvm, &vcpu->arch.apf.data, gpa,
+					sizeof(u32)))
 		return 1;
 
 	vcpu->arch.apf.send_user_only = !(data & KVM_ASYNC_PF_SEND_ALWAYS);
@@ -1952,12 +1953,9 @@ int kvm_set_msr_common(struct kvm_vcpu *vcpu, struct msr_data *msr_info)
 
 		gpa_offset = data & ~(PAGE_MASK | 1);
 
-		/* Check that the address is 32-byte aligned. */
-		if (gpa_offset & (sizeof(struct pvclock_vcpu_time_info) - 1))
-			break;
-
 		if (kvm_gfn_to_hva_cache_init(vcpu->kvm,
-		     &vcpu->arch.pv_time, data & ~1ULL))
+		     &vcpu->arch.pv_time, data & ~1ULL,
+		     sizeof(struct pvclock_vcpu_time_info)))
 			vcpu->arch.pv_time_enabled = false;
 		else
 			vcpu->arch.pv_time_enabled = true;
@@ -1977,7 +1975,8 @@ int kvm_set_msr_common(struct kvm_vcpu *vcpu, struct msr_data *msr_info)
 			return 1;
 
 		if (kvm_gfn_to_hva_cache_init(vcpu->kvm, &vcpu->arch.st.stime,
-							data & KVM_STEAL_VALID_BITS))
+						data & KVM_STEAL_VALID_BITS,
+						sizeof(struct kvm_steal_time)))
 			return 1;
 
 		vcpu->arch.st.msr_val = data;
diff --git a/arch/x86/lguest/Kconfig b/arch/x86/lguest/Kconfig
index 29043d2048a..4a0890f815c 100644
--- a/arch/x86/lguest/Kconfig
+++ b/arch/x86/lguest/Kconfig
@@ -1,7 +1,6 @@
 config LGUEST_GUEST
 	bool "Lguest guest support"
-	select PARAVIRT
-	depends on X86_32
+	depends on X86_32 && PARAVIRT
 	select TTY
 	select VIRTUALIZATION
 	select VIRTIO
diff --git a/arch/x86/lguest/boot.c b/arch/x86/lguest/boot.c
index 1cbd89ca556..7114c63f047 100644
--- a/arch/x86/lguest/boot.c
+++ b/arch/x86/lguest/boot.c
@@ -1334,6 +1334,7 @@ __init void lguest_init(void)
 	pv_mmu_ops.read_cr3 = lguest_read_cr3;
 	pv_mmu_ops.lazy_mode.enter = paravirt_enter_lazy_mmu;
 	pv_mmu_ops.lazy_mode.leave = lguest_leave_lazy_mmu_mode;
+	pv_mmu_ops.lazy_mode.flush = paravirt_flush_lazy_mmu;
 	pv_mmu_ops.pte_update = lguest_pte_update;
 	pv_mmu_ops.pte_update_defer = lguest_pte_update;
 
diff --git a/arch/x86/lib/checksum_32.S b/arch/x86/lib/checksum_32.S
index 2af5df3ade7..e78b8eee661 100644
--- a/arch/x86/lib/checksum_32.S
+++ b/arch/x86/lib/checksum_32.S
@@ -61,7 +61,7 @@ ENTRY(csum_partial)
 	testl $3, %esi		# Check alignment.
 	jz 2f			# Jump if alignment is ok.
 	testl $1, %esi		# Check alignment.
-	jz 10f			# Jump if alignment is boundary of 2bytes.
+	jz 10f			# Jump if alignment is boundary of 2 bytes.
 
 	# buf is odd
 	dec %ecx
diff --git a/arch/x86/lib/memcpy_32.c b/arch/x86/lib/memcpy_32.c
index b908a59eccf..e78761d6b7f 100644
--- a/arch/x86/lib/memcpy_32.c
+++ b/arch/x86/lib/memcpy_32.c
@@ -26,7 +26,7 @@ void *memmove(void *dest, const void *src, size_t n)
 	char *ret = dest;
 
 	__asm__ __volatile__(
-		/* Handle more 16bytes in loop */
+		/* Handle more 16 bytes in loop */
 		"cmp $0x10, %0\n\t"
 		"jb	1f\n\t"
 
@@ -51,7 +51,7 @@ void *memmove(void *dest, const void *src, size_t n)
 		"sub $0x10, %0\n\t"
 
 		/*
-		 * We gobble 16byts forward in each loop.
+		 * We gobble 16 bytes forward in each loop.
 		 */
 		"3:\n\t"
 		"sub $0x10, %0\n\t"
@@ -117,7 +117,7 @@ void *memmove(void *dest, const void *src, size_t n)
 		"sub $0x10, %0\n\t"
 
 		/*
-		 * We gobble 16byts backward in each loop.
+		 * We gobble 16 bytes backward in each loop.
 		 */
 		"7:\n\t"
 		"sub $0x10, %0\n\t"
diff --git a/arch/x86/lib/memcpy_64.S b/arch/x86/lib/memcpy_64.S
index 1c273be7c97..56313a32618 100644
--- a/arch/x86/lib/memcpy_64.S
+++ b/arch/x86/lib/memcpy_64.S
@@ -98,7 +98,7 @@ ENTRY(memcpy)
 	subq $0x20,	%rdx
 	/*
 	 * At most 3 ALU operations in one cycle,
-	 * so append NOPS in the same 16bytes trunk.
+	 * so append NOPS in the same 16 bytes trunk.
 	 */
 	.p2align 4
 .Lcopy_backward_loop:
diff --git a/arch/x86/lib/memmove_64.S b/arch/x86/lib/memmove_64.S
index ee164610ec4..65268a6104f 100644
--- a/arch/x86/lib/memmove_64.S
+++ b/arch/x86/lib/memmove_64.S
@@ -27,7 +27,7 @@
 ENTRY(memmove)
 	CFI_STARTPROC
 
-	/* Handle more 32bytes in loop */
+	/* Handle more 32 bytes in loop */
 	mov %rdi, %rax
 	cmp $0x20, %rdx
 	jb	1f
@@ -56,7 +56,7 @@ ENTRY(memmove)
 3:
 	sub $0x20, %rdx
 	/*
-	 * We gobble 32byts forward in each loop.
+	 * We gobble 32 bytes forward in each loop.
 	 */
 5:
 	sub $0x20, %rdx
@@ -122,7 +122,7 @@ ENTRY(memmove)
 	addq %rdx, %rdi
 	subq $0x20, %rdx
 	/*
-	 * We gobble 32byts backward in each loop.
+	 * We gobble 32 bytes backward in each loop.
 	 */
 8:
 	subq $0x20, %rdx
diff --git a/arch/x86/lib/usercopy_64.c b/arch/x86/lib/usercopy_64.c
index 05928aae911..906fea31579 100644
--- a/arch/x86/lib/usercopy_64.c
+++ b/arch/x86/lib/usercopy_64.c
@@ -74,10 +74,10 @@ copy_user_handle_tail(char *to, char *from, unsigned len, unsigned zerorest)
 	char c;
 	unsigned zero_len;
 
-	for (; len; --len) {
+	for (; len; --len, to++) {
 		if (__get_user_nocheck(c, from++, sizeof(char)))
 			break;
-		if (__put_user_nocheck(c, to++, sizeof(char)))
+		if (__put_user_nocheck(c, to, sizeof(char)))
 			break;
 	}
 
diff --git a/arch/x86/mm/amdtopology.c b/arch/x86/mm/amdtopology.c
index 5247d01329c..2ca15b59fb3 100644
--- a/arch/x86/mm/amdtopology.c
+++ b/arch/x86/mm/amdtopology.c
@@ -130,9 +130,8 @@ int __init amd_numa_init(void)
 		}
 
 		limit >>= 16;
-		limit <<= 24;
-		limit |= (1<<24)-1;
 		limit++;
+		limit <<= 24;
 
 		if (limit > end)
 			limit = end;
diff --git a/arch/x86/mm/fault.c b/arch/x86/mm/fault.c
index 2b97525246d..654be4ae304 100644
--- a/arch/x86/mm/fault.c
+++ b/arch/x86/mm/fault.c
@@ -13,12 +13,12 @@
 #include <linux/perf_event.h>		/* perf_sw_event		*/
 #include <linux/hugetlb.h>		/* hstate_index_to_shift	*/
 #include <linux/prefetch.h>		/* prefetchw			*/
+#include <linux/context_tracking.h>	/* exception_enter(), ...	*/
 
 #include <asm/traps.h>			/* dotraplinkage, ...		*/
 #include <asm/pgalloc.h>		/* pgd_*(), ...			*/
 #include <asm/kmemcheck.h>		/* kmemcheck_*(), ...		*/
 #include <asm/fixmap.h>			/* VSYSCALL_START		*/
-#include <asm/context_tracking.h>	/* exception_enter(), ...	*/
 
 /*
  * Page fault error code bits:
@@ -378,10 +378,12 @@ static noinline __kprobes int vmalloc_fault(unsigned long address)
 	if (pgd_none(*pgd_ref))
 		return -1;
 
-	if (pgd_none(*pgd))
+	if (pgd_none(*pgd)) {
 		set_pgd(pgd, *pgd_ref);
-	else
+		arch_flush_lazy_mmu_mode();
+	} else {
 		BUG_ON(pgd_page_vaddr(*pgd) != pgd_page_vaddr(*pgd_ref));
+	}
 
 	/*
 	 * Below here mismatches are bugs because these lower tables
@@ -555,7 +557,7 @@ static int is_f00f_bug(struct pt_regs *regs, unsigned long address)
 	/*
 	 * Pentium F0 0F C7 C8 bug workaround:
 	 */
-	if (boot_cpu_data.f00f_bug) {
+	if (boot_cpu_has_bug(X86_BUG_F00F)) {
 		nr = (address - idt_descr.address) >> 3;
 
 		if (nr == 6) {
@@ -1222,7 +1224,9 @@ good_area:
 dotraplinkage void __kprobes
 do_page_fault(struct pt_regs *regs, unsigned long error_code)
 {
-	exception_enter(regs);
+	enum ctx_state prev_state;
+
+	prev_state = exception_enter();
 	__do_page_fault(regs, error_code);
-	exception_exit(regs);
+	exception_exit(prev_state);
 }
diff --git a/arch/x86/mm/highmem_32.c b/arch/x86/mm/highmem_32.c
index 6f31ee56c00..252b8f5489b 100644
--- a/arch/x86/mm/highmem_32.c
+++ b/arch/x86/mm/highmem_32.c
@@ -137,5 +137,4 @@ void __init set_highmem_pages_init(void)
 		add_highpages_with_active_regions(nid, zone_start_pfn,
 				 zone_end_pfn);
 	}
-	totalram_pages += totalhigh_pages;
 }
diff --git a/arch/x86/mm/init.c b/arch/x86/mm/init.c
index 59b7fc45327..fdc5dca14fb 100644
--- a/arch/x86/mm/init.c
+++ b/arch/x86/mm/init.c
@@ -515,11 +515,8 @@ void free_init_pages(char *what, unsigned long begin, unsigned long end)
 	printk(KERN_INFO "Freeing %s: %luk freed\n", what, (end - begin) >> 10);
 
 	for (; addr < end; addr += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(addr));
-		init_page_count(virt_to_page(addr));
 		memset((void *)addr, POISON_FREE_INITMEM, PAGE_SIZE);
-		free_page(addr);
-		totalram_pages++;
+		free_reserved_page(virt_to_page(addr));
 	}
 #endif
 }
diff --git a/arch/x86/mm/init_32.c b/arch/x86/mm/init_32.c
index 2d19001151d..3ac7e319918 100644
--- a/arch/x86/mm/init_32.c
+++ b/arch/x86/mm/init_32.c
@@ -427,14 +427,6 @@ static void __init permanent_kmaps_init(pgd_t *pgd_base)
 	pkmap_page_table = pte;
 }
 
-static void __init add_one_highpage_init(struct page *page)
-{
-	ClearPageReserved(page);
-	init_page_count(page);
-	__free_page(page);
-	totalhigh_pages++;
-}
-
 void __init add_highpages_with_active_regions(int nid,
 			 unsigned long start_pfn, unsigned long end_pfn)
 {
@@ -448,7 +440,7 @@ void __init add_highpages_with_active_regions(int nid,
 					      start_pfn, end_pfn);
 		for ( ; pfn < e_pfn; pfn++)
 			if (pfn_valid(pfn))
-				add_one_highpage_init(pfn_to_page(pfn));
+				free_highmem_page(pfn_to_page(pfn));
 	}
 }
 #else
diff --git a/arch/x86/mm/init_64.c b/arch/x86/mm/init_64.c
index 474e28f1081..caad9a0ee19 100644
--- a/arch/x86/mm/init_64.c
+++ b/arch/x86/mm/init_64.c
@@ -1011,14 +1011,12 @@ remove_pagetable(unsigned long start, unsigned long end, bool direct)
 	flush_tlb_all();
 }
 
-void __ref vmemmap_free(struct page *memmap, unsigned long nr_pages)
+void __ref vmemmap_free(unsigned long start, unsigned long end)
 {
-	unsigned long start = (unsigned long)memmap;
-	unsigned long end = (unsigned long)(memmap + nr_pages);
-
 	remove_pagetable(start, end, false);
 }
 
+#ifdef CONFIG_MEMORY_HOTREMOVE
 static void __meminit
 kernel_physical_mapping_remove(unsigned long start, unsigned long end)
 {
@@ -1028,7 +1026,6 @@ kernel_physical_mapping_remove(unsigned long start, unsigned long end)
 	remove_pagetable(start, end, true);
 }
 
-#ifdef CONFIG_MEMORY_HOTREMOVE
 int __ref arch_remove_memory(u64 start, u64 size)
 {
 	unsigned long start_pfn = start >> PAGE_SHIFT;
@@ -1067,10 +1064,9 @@ void __init mem_init(void)
 
 	/* clear_bss() already clear the empty_zero_page */
 
-	reservedpages = 0;
-
-	/* this will put all low memory onto the freelists */
 	register_page_bootmem_info();
+
+	/* this will put all memory onto the freelists */
 	totalram_pages = free_all_bootmem();
 
 	absent_pages = absent_pages_in_range(0, max_pfn);
@@ -1285,18 +1281,17 @@ static long __meminitdata addr_start, addr_end;
 static void __meminitdata *p_start, *p_end;
 static int __meminitdata node_start;
 
-int __meminit
-vmemmap_populate(struct page *start_page, unsigned long size, int node)
+static int __meminit vmemmap_populate_hugepages(unsigned long start,
+						unsigned long end, int node)
 {
-	unsigned long addr = (unsigned long)start_page;
-	unsigned long end = (unsigned long)(start_page + size);
+	unsigned long addr;
 	unsigned long next;
 	pgd_t *pgd;
 	pud_t *pud;
 	pmd_t *pmd;
 
-	for (; addr < end; addr = next) {
-		void *p = NULL;
+	for (addr = start; addr < end; addr = next) {
+		next = pmd_addr_end(addr, end);
 
 		pgd = vmemmap_pgd_populate(addr, node);
 		if (!pgd)
@@ -1306,31 +1301,14 @@ vmemmap_populate(struct page *start_page, unsigned long size, int node)
 		if (!pud)
 			return -ENOMEM;
 
-		if (!cpu_has_pse) {
-			next = (addr + PAGE_SIZE) & PAGE_MASK;
-			pmd = vmemmap_pmd_populate(pud, addr, node);
-
-			if (!pmd)
-				return -ENOMEM;
-
-			p = vmemmap_pte_populate(pmd, addr, node);
+		pmd = pmd_offset(pud, addr);
+		if (pmd_none(*pmd)) {
+			void *p;
 
-			if (!p)
-				return -ENOMEM;
-
-			addr_end = addr + PAGE_SIZE;
-			p_end = p + PAGE_SIZE;
-		} else {
-			next = pmd_addr_end(addr, end);
-
-			pmd = pmd_offset(pud, addr);
-			if (pmd_none(*pmd)) {
+			p = vmemmap_alloc_block_buf(PMD_SIZE, node);
+			if (p) {
 				pte_t entry;
 
-				p = vmemmap_alloc_block_buf(PMD_SIZE, node);
-				if (!p)
-					return -ENOMEM;
-
 				entry = pfn_pte(__pa(p) >> PAGE_SHIFT,
 						PAGE_KERNEL_LARGE);
 				set_pmd(pmd, __pmd(pte_val(entry)));
@@ -1347,15 +1325,32 @@ vmemmap_populate(struct page *start_page, unsigned long size, int node)
 
 				addr_end = addr + PMD_SIZE;
 				p_end = p + PMD_SIZE;
-			} else
-				vmemmap_verify((pte_t *)pmd, node, addr, next);
+				continue;
+			}
+		} else if (pmd_large(*pmd)) {
+			vmemmap_verify((pte_t *)pmd, node, addr, next);
+			continue;
 		}
-
+		pr_warn_once("vmemmap: falling back to regular page backing\n");
+		if (vmemmap_populate_basepages(addr, next, node))
+			return -ENOMEM;
 	}
-	sync_global_pgds((unsigned long)start_page, end - 1);
 	return 0;
 }
 
+int __meminit vmemmap_populate(unsigned long start, unsigned long end, int node)
+{
+	int err;
+
+	if (cpu_has_pse)
+		err = vmemmap_populate_hugepages(start, end, node);
+	else
+		err = vmemmap_populate_basepages(start, end, node);
+	if (!err)
+		sync_global_pgds(start, end - 1);
+	return err;
+}
+
 #if defined(CONFIG_MEMORY_HOTPLUG_SPARSE) && defined(CONFIG_HAVE_BOOTMEM_INFO_NODE)
 void register_page_bootmem_memmap(unsigned long section_nr,
 				  struct page *start_page, unsigned long size)
diff --git a/arch/x86/mm/ioremap.c b/arch/x86/mm/ioremap.c
index 78fe3f1ac49..9a1e6583910 100644
--- a/arch/x86/mm/ioremap.c
+++ b/arch/x86/mm/ioremap.c
@@ -282,12 +282,7 @@ void iounmap(volatile void __iomem *addr)
 	   in parallel. Reuse of the virtual address is prevented by
 	   leaving it in the global lists until we're done with it.
 	   cpa takes care of the direct mappings. */
-	read_lock(&vmlist_lock);
-	for (p = vmlist; p; p = p->next) {
-		if (p->addr == (void __force *)addr)
-			break;
-	}
-	read_unlock(&vmlist_lock);
+	p = find_vm_area((void __force *)addr);
 
 	if (!p) {
 		printk(KERN_ERR "iounmap: bad address %p\n", addr);
diff --git a/arch/x86/mm/numa.c b/arch/x86/mm/numa.c
index 72fe01e9e41..a71c4e20767 100644
--- a/arch/x86/mm/numa.c
+++ b/arch/x86/mm/numa.c
@@ -114,14 +114,11 @@ void numa_clear_node(int cpu)
  */
 void __init setup_node_to_cpumask_map(void)
 {
-	unsigned int node, num = 0;
+	unsigned int node;
 
 	/* setup nr_node_ids if not done yet */
-	if (nr_node_ids == MAX_NUMNODES) {
-		for_each_node_mask(node, node_possible_map)
-			num = node;
-		nr_node_ids = num + 1;
-	}
+	if (nr_node_ids == MAX_NUMNODES)
+		setup_nr_node_ids();
 
 	/* allocate the map */
 	for (node = 0; node < nr_node_ids; node++)
diff --git a/arch/x86/mm/pageattr-test.c b/arch/x86/mm/pageattr-test.c
index b0086567271..d0b1773d9d2 100644
--- a/arch/x86/mm/pageattr-test.c
+++ b/arch/x86/mm/pageattr-test.c
@@ -68,7 +68,7 @@ static int print_split(struct split_state *s)
 			s->gpg++;
 			i += GPS/PAGE_SIZE;
 		} else if (level == PG_LEVEL_2M) {
-			if (!(pte_val(*pte) & _PAGE_PSE)) {
+			if ((pte_val(*pte) & _PAGE_PRESENT) && !(pte_val(*pte) & _PAGE_PSE)) {
 				printk(KERN_ERR
 					"%lx level %d but not PSE %Lx\n",
 					addr, level, (u64)pte_val(*pte));
@@ -130,13 +130,12 @@ static int pageattr_test(void)
 	}
 
 	failed += print_split(&sa);
-	srandom32(100);
 
 	for (i = 0; i < NTEST; i++) {
-		unsigned long pfn = random32() % max_pfn_mapped;
+		unsigned long pfn = prandom_u32() % max_pfn_mapped;
 
 		addr[i] = (unsigned long)__va(pfn << PAGE_SHIFT);
-		len[i] = random32() % 100;
+		len[i] = prandom_u32() % 100;
 		len[i] = min_t(unsigned long, len[i], max_pfn_mapped - pfn - 1);
 
 		if (len[i] == 0)
diff --git a/arch/x86/mm/pageattr.c b/arch/x86/mm/pageattr.c
index 091934e1d0d..bb32480c2d7 100644
--- a/arch/x86/mm/pageattr.c
+++ b/arch/x86/mm/pageattr.c
@@ -467,7 +467,7 @@ try_preserve_large_page(pte_t *kpte, unsigned long address,
 	 * We are safe now. Check whether the new pgprot is the same:
 	 */
 	old_pte = *kpte;
-	old_prot = new_prot = req_prot = pte_pgprot(old_pte);
+	old_prot = req_prot = pte_pgprot(old_pte);
 
 	pgprot_val(req_prot) &= ~pgprot_val(cpa->mask_clr);
 	pgprot_val(req_prot) |= pgprot_val(cpa->mask_set);
@@ -478,12 +478,12 @@ try_preserve_large_page(pte_t *kpte, unsigned long address,
 	 * a non present pmd. The canon_pgprot will clear _PAGE_GLOBAL
 	 * for the ancient hardware that doesn't support it.
 	 */
-	if (pgprot_val(new_prot) & _PAGE_PRESENT)
-		pgprot_val(new_prot) |= _PAGE_PSE | _PAGE_GLOBAL;
+	if (pgprot_val(req_prot) & _PAGE_PRESENT)
+		pgprot_val(req_prot) |= _PAGE_PSE | _PAGE_GLOBAL;
 	else
-		pgprot_val(new_prot) &= ~(_PAGE_PSE | _PAGE_GLOBAL);
+		pgprot_val(req_prot) &= ~(_PAGE_PSE | _PAGE_GLOBAL);
 
-	new_prot = canon_pgprot(new_prot);
+	req_prot = canon_pgprot(req_prot);
 
 	/*
 	 * old_pte points to the large page base address. So we need
@@ -542,13 +542,14 @@ out_unlock:
 	return do_split;
 }
 
-int __split_large_page(pte_t *kpte, unsigned long address, pte_t *pbase)
+static int
+__split_large_page(pte_t *kpte, unsigned long address, struct page *base)
 {
+	pte_t *pbase = (pte_t *)page_address(base);
 	unsigned long pfn, pfninc = 1;
 	unsigned int i, level;
 	pte_t *tmp;
 	pgprot_t ref_prot;
-	struct page *base = virt_to_page(pbase);
 
 	spin_lock(&pgd_lock);
 	/*
@@ -633,7 +634,6 @@ int __split_large_page(pte_t *kpte, unsigned long address, pte_t *pbase)
 
 static int split_large_page(pte_t *kpte, unsigned long address)
 {
-	pte_t *pbase;
 	struct page *base;
 
 	if (!debug_pagealloc)
@@ -644,8 +644,7 @@ static int split_large_page(pte_t *kpte, unsigned long address)
 	if (!base)
 		return -ENOMEM;
 
-	pbase = (pte_t *)page_address(base);
-	if (__split_large_page(kpte, address, pbase))
+	if (__split_large_page(kpte, address, base))
 		__free_page(base);
 
 	return 0;
@@ -1413,6 +1412,8 @@ void kernel_map_pages(struct page *page, int numpages, int enable)
 	 * but that can deadlock->flush only current cpu:
 	 */
 	__flush_tlb_all();
+
+	arch_flush_lazy_mmu_mode();
 }
 
 #ifdef CONFIG_HIBERNATION
diff --git a/arch/x86/mm/pgtable.c b/arch/x86/mm/pgtable.c
index 193350b51f9..17fda6a8b3c 100644
--- a/arch/x86/mm/pgtable.c
+++ b/arch/x86/mm/pgtable.c
@@ -58,6 +58,13 @@ void ___pte_free_tlb(struct mmu_gather *tlb, struct page *pte)
 void ___pmd_free_tlb(struct mmu_gather *tlb, pmd_t *pmd)
 {
 	paravirt_release_pmd(__pa(pmd) >> PAGE_SHIFT);
+	/*
+	 * NOTE! For PAE, any changes to the top page-directory-pointer-table
+	 * entries need a full cr3 reload to flush.
+	 */
+#ifdef CONFIG_X86_PAE
+	tlb->need_flush_all = 1;
+#endif
 	tlb_remove_page(tlb, virt_to_page(pmd));
 }
 
diff --git a/arch/x86/pci/common.c b/arch/x86/pci/common.c
index 901177d75ff..305c68b8d53 100644
--- a/arch/x86/pci/common.c
+++ b/arch/x86/pci/common.c
@@ -6,6 +6,7 @@
 
 #include <linux/sched.h>
 #include <linux/pci.h>
+#include <linux/pci-acpi.h>
 #include <linux/ioport.h>
 #include <linux/init.h>
 #include <linux/dmi.h>
@@ -170,6 +171,16 @@ void pcibios_fixup_bus(struct pci_bus *b)
 		pcibios_fixup_device_resources(dev);
 }
 
+void pcibios_add_bus(struct pci_bus *bus)
+{
+	acpi_pci_add_bus(bus);
+}
+
+void pcibios_remove_bus(struct pci_bus *bus)
+{
+	acpi_pci_remove_bus(bus);
+}
+
 /*
  * Only use DMI information to set this if nothing was passed
  * on the kernel command line (which was parsed earlier).
diff --git a/arch/x86/pci/xen.c b/arch/x86/pci/xen.c
index 94e76620460..4a9be6ddf05 100644
--- a/arch/x86/pci/xen.c
+++ b/arch/x86/pci/xen.c
@@ -177,7 +177,7 @@ static int xen_setup_msi_irqs(struct pci_dev *dev, int nvec, int type)
 		goto error;
 	i = 0;
 	list_for_each_entry(msidesc, &dev->msi_list, list) {
-		irq = xen_bind_pirq_msi_to_irq(dev, msidesc, v[i], 0,
+		irq = xen_bind_pirq_msi_to_irq(dev, msidesc, v[i],
 					       (type == PCI_CAP_ID_MSIX) ?
 					       "pcifront-msi-x" :
 					       "pcifront-msi",
@@ -244,7 +244,7 @@ static int xen_hvm_setup_msi_irqs(struct pci_dev *dev, int nvec, int type)
 			dev_dbg(&dev->dev,
 				"xen: msi already bound to pirq=%d\n", pirq);
 		}
-		irq = xen_bind_pirq_msi_to_irq(dev, msidesc, pirq, 0,
+		irq = xen_bind_pirq_msi_to_irq(dev, msidesc, pirq,
 					       (type == PCI_CAP_ID_MSIX) ?
 					       "msi-x" : "msi",
 					       DOMID_SELF);
@@ -326,7 +326,7 @@ static int xen_initdom_setup_msi_irqs(struct pci_dev *dev, int nvec, int type)
 		}
 
 		ret = xen_bind_pirq_msi_to_irq(dev, msidesc,
-					       map_irq.pirq, map_irq.index,
+					       map_irq.pirq,
 					       (type == PCI_CAP_ID_MSIX) ?
 					       "msi-x" : "msi",
 						domid);
diff --git a/arch/x86/platform/efi/efi.c b/arch/x86/platform/efi/efi.c
index 5f2ecaf3f9d..b55d174e503 100644
--- a/arch/x86/platform/efi/efi.c
+++ b/arch/x86/platform/efi/efi.c
@@ -41,6 +41,7 @@
 #include <linux/io.h>
 #include <linux/reboot.h>
 #include <linux/bcd.h>
+#include <linux/ucs2_string.h>
 
 #include <asm/setup.h>
 #include <asm/efi.h>
@@ -48,9 +49,17 @@
 #include <asm/cacheflush.h>
 #include <asm/tlbflush.h>
 #include <asm/x86_init.h>
+#include <asm/rtc.h>
 
 #define EFI_DEBUG	1
 
+/*
+ * There's some additional metadata associated with each
+ * variable. Intel's reference implementation is 60 bytes - bump that
+ * to account for potential alignment constraints
+ */
+#define VAR_METADATA_SIZE 64
+
 struct efi __read_mostly efi = {
 	.mps        = EFI_INVALID_TABLE_ADDR,
 	.acpi       = EFI_INVALID_TABLE_ADDR,
@@ -69,6 +78,13 @@ struct efi_memory_map memmap;
 static struct efi efi_phys __initdata;
 static efi_system_table_t efi_systab __initdata;
 
+static u64 efi_var_store_size;
+static u64 efi_var_remaining_size;
+static u64 efi_var_max_var_size;
+static u64 boot_used_size;
+static u64 boot_var_size;
+static u64 active_size;
+
 unsigned long x86_efi_facility;
 
 /*
@@ -98,6 +114,15 @@ static int __init setup_add_efi_memmap(char *arg)
 }
 early_param("add_efi_memmap", setup_add_efi_memmap);
 
+static bool efi_no_storage_paranoia;
+
+static int __init setup_storage_paranoia(char *arg)
+{
+	efi_no_storage_paranoia = true;
+	return 0;
+}
+early_param("efi_no_storage_paranoia", setup_storage_paranoia);
+
 
 static efi_status_t virt_efi_get_time(efi_time_t *tm, efi_time_cap_t *tc)
 {
@@ -162,8 +187,53 @@ static efi_status_t virt_efi_get_next_variable(unsigned long *name_size,
 					       efi_char16_t *name,
 					       efi_guid_t *vendor)
 {
-	return efi_call_virt3(get_next_variable,
-			      name_size, name, vendor);
+	efi_status_t status;
+	static bool finished = false;
+	static u64 var_size;
+
+	status = efi_call_virt3(get_next_variable,
+				name_size, name, vendor);
+
+	if (status == EFI_NOT_FOUND) {
+		finished = true;
+		if (var_size < boot_used_size) {
+			boot_var_size = boot_used_size - var_size;
+			active_size += boot_var_size;
+		} else {
+			printk(KERN_WARNING FW_BUG  "efi: Inconsistent initial sizes\n");
+		}
+	}
+
+	if (boot_used_size && !finished) {
+		unsigned long size;
+		u32 attr;
+		efi_status_t s;
+		void *tmp;
+
+		s = virt_efi_get_variable(name, vendor, &attr, &size, NULL);
+
+		if (s != EFI_BUFFER_TOO_SMALL || !size)
+			return status;
+
+		tmp = kmalloc(size, GFP_ATOMIC);
+
+		if (!tmp)
+			return status;
+
+		s = virt_efi_get_variable(name, vendor, &attr, &size, tmp);
+
+		if (s == EFI_SUCCESS && (attr & EFI_VARIABLE_NON_VOLATILE)) {
+			var_size += size;
+			var_size += ucs2_strsize(name, 1024);
+			active_size += size;
+			active_size += VAR_METADATA_SIZE;
+			active_size += ucs2_strsize(name, 1024);
+		}
+
+		kfree(tmp);
+	}
+
+	return status;
 }
 
 static efi_status_t virt_efi_set_variable(efi_char16_t *name,
@@ -172,9 +242,34 @@ static efi_status_t virt_efi_set_variable(efi_char16_t *name,
 					  unsigned long data_size,
 					  void *data)
 {
-	return efi_call_virt5(set_variable,
-			      name, vendor, attr,
-			      data_size, data);
+	efi_status_t status;
+	u32 orig_attr = 0;
+	unsigned long orig_size = 0;
+
+	status = virt_efi_get_variable(name, vendor, &orig_attr, &orig_size,
+				       NULL);
+
+	if (status != EFI_BUFFER_TOO_SMALL)
+		orig_size = 0;
+
+	status = efi_call_virt5(set_variable,
+				name, vendor, attr,
+				data_size, data);
+
+	if (status == EFI_SUCCESS) {
+		if (orig_size) {
+			active_size -= orig_size;
+			active_size -= ucs2_strsize(name, 1024);
+			active_size -= VAR_METADATA_SIZE;
+		}
+		if (data_size) {
+			active_size += data_size;
+			active_size += ucs2_strsize(name, 1024);
+			active_size += VAR_METADATA_SIZE;
+		}
+	}
+
+	return status;
 }
 
 static efi_status_t virt_efi_query_variable_info(u32 attr,
@@ -258,10 +353,10 @@ static efi_status_t __init phys_efi_get_time(efi_time_t *tm,
 
 int efi_set_rtc_mmss(unsigned long nowtime)
 {
-	int real_seconds, real_minutes;
 	efi_status_t 	status;
 	efi_time_t 	eft;
 	efi_time_cap_t 	cap;
+	struct rtc_time	tm;
 
 	status = efi.get_time(&eft, &cap);
 	if (status != EFI_SUCCESS) {
@@ -269,13 +364,20 @@ int efi_set_rtc_mmss(unsigned long nowtime)
 		return -1;
 	}
 
-	real_seconds = nowtime % 60;
-	real_minutes = nowtime / 60;
-	if (((abs(real_minutes - eft.minute) + 15)/30) & 1)
-		real_minutes += 30;
-	real_minutes %= 60;
-	eft.minute = real_minutes;
-	eft.second = real_seconds;
+	rtc_time_to_tm(nowtime, &tm);
+	if (!rtc_valid_tm(&tm)) {
+		eft.year = tm.tm_year + 1900;
+		eft.month = tm.tm_mon + 1;
+		eft.day = tm.tm_mday;
+		eft.minute = tm.tm_min;
+		eft.second = tm.tm_sec;
+		eft.nanosecond = 0;
+	} else {
+		printk(KERN_ERR
+		       "%s: Invalid EFI RTC value: write of %lx to EFI RTC failed\n",
+		       __FUNCTION__, nowtime);
+		return -1;
+	}
 
 	status = efi.set_time(&eft);
 	if (status != EFI_SUCCESS) {
@@ -682,6 +784,9 @@ void __init efi_init(void)
 	char vendor[100] = "unknown";
 	int i = 0;
 	void *tmp;
+	struct setup_data *data;
+	struct efi_var_bootdata *efi_var_data;
+	u64 pa_data;
 
 #ifdef CONFIG_X86_32
 	if (boot_params.efi_info.efi_systab_hi ||
@@ -699,6 +804,22 @@ void __init efi_init(void)
 	if (efi_systab_init(efi_phys.systab))
 		return;
 
+	pa_data = boot_params.hdr.setup_data;
+	while (pa_data) {
+		data = early_ioremap(pa_data, sizeof(*efi_var_data));
+		if (data->type == SETUP_EFI_VARS) {
+			efi_var_data = (struct efi_var_bootdata *)data;
+
+			efi_var_store_size = efi_var_data->store_size;
+			efi_var_remaining_size = efi_var_data->remaining_size;
+			efi_var_max_var_size = efi_var_data->max_var_size;
+		}
+		pa_data = data->next;
+		early_iounmap(data, sizeof(*efi_var_data));
+	}
+
+	boot_used_size = efi_var_store_size - efi_var_remaining_size;
+
 	set_bit(EFI_SYSTEM_TABLES, &x86_efi_facility);
 
 	/*
@@ -999,3 +1120,48 @@ u64 efi_mem_attributes(unsigned long phys_addr)
 	}
 	return 0;
 }
+
+/*
+ * Some firmware has serious problems when using more than 50% of the EFI
+ * variable store, i.e. it triggers bugs that can brick machines. Ensure that
+ * we never use more than this safe limit.
+ *
+ * Return EFI_SUCCESS if it is safe to write 'size' bytes to the variable
+ * store.
+ */
+efi_status_t efi_query_variable_store(u32 attributes, unsigned long size)
+{
+	efi_status_t status;
+	u64 storage_size, remaining_size, max_size;
+
+	status = efi.query_variable_info(attributes, &storage_size,
+					 &remaining_size, &max_size);
+	if (status != EFI_SUCCESS)
+		return status;
+
+	if (!max_size && remaining_size > size)
+		printk_once(KERN_ERR FW_BUG "Broken EFI implementation"
+			    " is returning MaxVariableSize=0\n");
+	/*
+	 * Some firmware implementations refuse to boot if there's insufficient
+	 * space in the variable store. We account for that by refusing the
+	 * write if permitting it would reduce the available space to under
+	 * 50%. However, some firmware won't reclaim variable space until
+	 * after the used (not merely the actively used) space drops below
+	 * a threshold. We can approximate that case with the value calculated
+	 * above. If both the firmware and our calculations indicate that the
+	 * available space would drop below 50%, refuse the write.
+	 */
+
+	if (!storage_size || size > remaining_size ||
+	    (max_size && size > max_size))
+		return EFI_OUT_OF_RESOURCES;
+
+	if (!efi_no_storage_paranoia &&
+	    ((active_size + size + VAR_METADATA_SIZE > storage_size / 2) &&
+	     (remaining_size - size < storage_size / 2)))
+		return EFI_OUT_OF_RESOURCES;
+
+	return EFI_SUCCESS;
+}
+EXPORT_SYMBOL_GPL(efi_query_variable_store);
diff --git a/arch/x86/platform/mrst/mrst.c b/arch/x86/platform/mrst/mrst.c
index e31bcd8f2ee..a0a0a4389bb 100644
--- a/arch/x86/platform/mrst/mrst.c
+++ b/arch/x86/platform/mrst/mrst.c
@@ -356,8 +356,7 @@ static int __init sfi_parse_gpio(struct sfi_table_header *table)
 	num = SFI_GET_NUM_ENTRIES(sb, struct sfi_gpio_table_entry);
 	pentry = (struct sfi_gpio_table_entry *)sb->pentry;
 
-	gpio_table = (struct sfi_gpio_table_entry *)
-				kmalloc(num * sizeof(*pentry), GFP_KERNEL);
+	gpio_table = kmalloc(num * sizeof(*pentry), GFP_KERNEL);
 	if (!gpio_table)
 		return -1;
 	memcpy(gpio_table, pentry, num * sizeof(*pentry));
diff --git a/arch/x86/platform/mrst/vrtc.c b/arch/x86/platform/mrst/vrtc.c
index 225bd0f0f67..d62b0a3b5c1 100644
--- a/arch/x86/platform/mrst/vrtc.c
+++ b/arch/x86/platform/mrst/vrtc.c
@@ -85,27 +85,35 @@ unsigned long vrtc_get_time(void)
 	return mktime(year, mon, mday, hour, min, sec);
 }
 
-/* Only care about the minutes and seconds */
 int vrtc_set_mmss(unsigned long nowtime)
 {
-	int real_sec, real_min;
 	unsigned long flags;
-	int vrtc_min;
-
-	spin_lock_irqsave(&rtc_lock, flags);
-	vrtc_min = vrtc_cmos_read(RTC_MINUTES);
-
-	real_sec = nowtime % 60;
-	real_min = nowtime / 60;
-	if (((abs(real_min - vrtc_min) + 15)/30) & 1)
-		real_min += 30;
-	real_min %= 60;
-
-	vrtc_cmos_write(real_sec, RTC_SECONDS);
-	vrtc_cmos_write(real_min, RTC_MINUTES);
-	spin_unlock_irqrestore(&rtc_lock, flags);
-
-	return 0;
+	struct rtc_time tm;
+	int year;
+	int retval = 0;
+
+	rtc_time_to_tm(nowtime, &tm);
+	if (!rtc_valid_tm(&tm) && tm.tm_year >= 72) {
+		/*
+		 * tm.year is the number of years since 1900, and the
+		 * vrtc need the years since 1972.
+		 */
+		year = tm.tm_year - 72;
+		spin_lock_irqsave(&rtc_lock, flags);
+		vrtc_cmos_write(year, RTC_YEAR);
+		vrtc_cmos_write(tm.tm_mon, RTC_MONTH);
+		vrtc_cmos_write(tm.tm_mday, RTC_DAY_OF_MONTH);
+		vrtc_cmos_write(tm.tm_hour, RTC_HOURS);
+		vrtc_cmos_write(tm.tm_min, RTC_MINUTES);
+		vrtc_cmos_write(tm.tm_sec, RTC_SECONDS);
+		spin_unlock_irqrestore(&rtc_lock, flags);
+	} else {
+		printk(KERN_ERR
+		       "%s: Invalid vRTC value: write of %lx to vRTC failed\n",
+			__FUNCTION__, nowtime);
+		retval = -EINVAL;
+	}
+	return retval;
 }
 
 void __init mrst_rtc_init(void)
diff --git a/arch/x86/platform/olpc/olpc-xo1-sci.c b/arch/x86/platform/olpc/olpc-xo1-sci.c
index 74704be7b1f..9a2e590dd20 100644
--- a/arch/x86/platform/olpc/olpc-xo1-sci.c
+++ b/arch/x86/platform/olpc/olpc-xo1-sci.c
@@ -460,7 +460,6 @@ static int setup_power_button(struct platform_device *pdev)
 static void free_power_button(void)
 {
 	input_unregister_device(power_button_idev);
-	input_free_device(power_button_idev);
 }
 
 static int setup_ebook_switch(struct platform_device *pdev)
@@ -491,7 +490,6 @@ static int setup_ebook_switch(struct platform_device *pdev)
 static void free_ebook_switch(void)
 {
 	input_unregister_device(ebook_switch_idev);
-	input_free_device(ebook_switch_idev);
 }
 
 static int setup_lid_switch(struct platform_device *pdev)
@@ -526,6 +524,7 @@ static int setup_lid_switch(struct platform_device *pdev)
 
 err_create_attr:
 	input_unregister_device(lid_switch_idev);
+	lid_switch_idev = NULL;
 err_register:
 	input_free_device(lid_switch_idev);
 	return r;
@@ -535,7 +534,6 @@ static void free_lid_switch(void)
 {
 	device_remove_file(&lid_switch_idev->dev, &dev_attr_lid_wake_mode);
 	input_unregister_device(lid_switch_idev);
-	input_free_device(lid_switch_idev);
 }
 
 static int xo1_sci_probe(struct platform_device *pdev)
diff --git a/arch/x86/platform/uv/uv_time.c b/arch/x86/platform/uv/uv_time.c
index 98718f604eb..5c86786bbfd 100644
--- a/arch/x86/platform/uv/uv_time.c
+++ b/arch/x86/platform/uv/uv_time.c
@@ -159,10 +159,9 @@ static __init int uv_rtc_allocate_timers(void)
 {
 	int cpu;
 
-	blade_info = kmalloc(uv_possible_blades * sizeof(void *), GFP_KERNEL);
+	blade_info = kzalloc(uv_possible_blades * sizeof(void *), GFP_KERNEL);
 	if (!blade_info)
 		return -ENOMEM;
-	memset(blade_info, 0, uv_possible_blades * sizeof(void *));
 
 	for_each_present_cpu(cpu) {
 		int nid = cpu_to_node(cpu);
diff --git a/arch/x86/power/cpu.c b/arch/x86/power/cpu.c
index 3c68768d7a7..6d6e907cee4 100644
--- a/arch/x86/power/cpu.c
+++ b/arch/x86/power/cpu.c
@@ -62,11 +62,9 @@ static void __save_processor_state(struct saved_context *ctxt)
 	 * descriptor tables
 	 */
 #ifdef CONFIG_X86_32
-	store_gdt(&ctxt->gdt);
 	store_idt(&ctxt->idt);
 #else
 /* CONFIG_X86_64 */
-	store_gdt((struct desc_ptr *)&ctxt->gdt_limit);
 	store_idt((struct desc_ptr *)&ctxt->idt_limit);
 #endif
 	store_tr(ctxt->tr);
@@ -135,7 +133,10 @@ static void fix_processor_context(void)
 {
 	int cpu = smp_processor_id();
 	struct tss_struct *t = &per_cpu(init_tss, cpu);
-
+#ifdef CONFIG_X86_64
+	struct desc_struct *desc = get_cpu_gdt_table(cpu);
+	tss_desc tss;
+#endif
 	set_tss_desc(cpu, t);	/*
 				 * This just modifies memory; should not be
 				 * necessary. But... This is necessary, because
@@ -144,7 +145,9 @@ static void fix_processor_context(void)
 				 */
 
 #ifdef CONFIG_X86_64
-	get_cpu_gdt_table(cpu)[GDT_ENTRY_TSS].type = 9;
+	memcpy(&tss, &desc[GDT_ENTRY_TSS], sizeof(tss_desc));
+	tss.type = 0x9; /* The available 64-bit TSS (see AMD vol 2, pg 91 */
+	write_gdt_entry(desc, GDT_ENTRY_TSS, &tss, DESC_TSS);
 
 	syscall_init();				/* This sets MSR_*STAR and related */
 #endif
@@ -183,11 +186,9 @@ static void __restore_processor_state(struct saved_context *ctxt)
 	 * ltr is done i fix_processor_context().
 	 */
 #ifdef CONFIG_X86_32
-	load_gdt(&ctxt->gdt);
 	load_idt(&ctxt->idt);
 #else
 /* CONFIG_X86_64 */
-	load_gdt((const struct desc_ptr *)&ctxt->gdt_limit);
 	load_idt((const struct desc_ptr *)&ctxt->idt_limit);
 #endif
 
diff --git a/arch/x86/tools/Makefile b/arch/x86/tools/Makefile
index bae601f900e..e8120346903 100644
--- a/arch/x86/tools/Makefile
+++ b/arch/x86/tools/Makefile
@@ -39,4 +39,5 @@ $(obj)/insn_sanity.o: $(srctree)/arch/x86/lib/insn.c $(srctree)/arch/x86/lib/ina
 
 HOST_EXTRACFLAGS += -I$(srctree)/tools/include
 hostprogs-y	+= relocs
+relocs-objs     := relocs_32.o relocs_64.o relocs_common.o
 relocs: $(obj)/relocs
diff --git a/arch/x86/tools/relocs.c b/arch/x86/tools/relocs.c
index 79d67bd507f..590be109089 100644
--- a/arch/x86/tools/relocs.c
+++ b/arch/x86/tools/relocs.c
@@ -1,43 +1,36 @@
-#include <stdio.h>
-#include <stdarg.h>
-#include <stdlib.h>
-#include <stdint.h>
-#include <string.h>
-#include <errno.h>
-#include <unistd.h>
-#include <elf.h>
-#include <byteswap.h>
-#define USE_BSD
-#include <endian.h>
-#include <regex.h>
-#include <tools/le_byteshift.h>
-
-static void die(char *fmt, ...);
-
-#define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0]))
-static Elf32_Ehdr ehdr;
-static unsigned long reloc_count, reloc_idx;
-static unsigned long *relocs;
-static unsigned long reloc16_count, reloc16_idx;
-static unsigned long *relocs16;
+/* This is included from relocs_32/64.c */
+
+#define ElfW(type)		_ElfW(ELF_BITS, type)
+#define _ElfW(bits, type)	__ElfW(bits, type)
+#define __ElfW(bits, type)	Elf##bits##_##type
+
+#define Elf_Addr		ElfW(Addr)
+#define Elf_Ehdr		ElfW(Ehdr)
+#define Elf_Phdr		ElfW(Phdr)
+#define Elf_Shdr		ElfW(Shdr)
+#define Elf_Sym			ElfW(Sym)
+
+static Elf_Ehdr ehdr;
+
+struct relocs {
+	uint32_t	*offset;
+	unsigned long	count;
+	unsigned long	size;
+};
+
+static struct relocs relocs16;
+static struct relocs relocs32;
+static struct relocs relocs64;
 
 struct section {
-	Elf32_Shdr     shdr;
+	Elf_Shdr       shdr;
 	struct section *link;
-	Elf32_Sym      *symtab;
-	Elf32_Rel      *reltab;
+	Elf_Sym        *symtab;
+	Elf_Rel        *reltab;
 	char           *strtab;
 };
 static struct section *secs;
 
-enum symtype {
-	S_ABS,
-	S_REL,
-	S_SEG,
-	S_LIN,
-	S_NSYMTYPES
-};
-
 static const char * const sym_regex_kernel[S_NSYMTYPES] = {
 /*
  * Following symbols have been audited. There values are constant and do
@@ -49,6 +42,9 @@ static const char * const sym_regex_kernel[S_NSYMTYPES] = {
 	"^(xen_irq_disable_direct_reloc$|"
 	"xen_save_fl_direct_reloc$|"
 	"VDSO|"
+#if ELF_BITS == 64
+	"__vvar_page|"
+#endif
 	"__crc_)",
 
 /*
@@ -72,6 +68,11 @@ static const char * const sym_regex_kernel[S_NSYMTYPES] = {
 	"__end_rodata|"
 	"__initramfs_start|"
 	"(jiffies|jiffies_64)|"
+#if ELF_BITS == 64
+	"__per_cpu_load|"
+	"init_per_cpu__.*|"
+	"__end_rodata_hpage_align|"
+#endif
 	"_end)$"
 };
 
@@ -132,15 +133,6 @@ static void regex_init(int use_real_mode)
         }
 }
 
-static void die(char *fmt, ...)
-{
-	va_list ap;
-	va_start(ap, fmt);
-	vfprintf(stderr, fmt, ap);
-	va_end(ap);
-	exit(1);
-}
-
 static const char *sym_type(unsigned type)
 {
 	static const char *type_name[] = {
@@ -198,6 +190,24 @@ static const char *rel_type(unsigned type)
 {
 	static const char *type_name[] = {
 #define REL_TYPE(X) [X] = #X
+#if ELF_BITS == 64
+		REL_TYPE(R_X86_64_NONE),
+		REL_TYPE(R_X86_64_64),
+		REL_TYPE(R_X86_64_PC32),
+		REL_TYPE(R_X86_64_GOT32),
+		REL_TYPE(R_X86_64_PLT32),
+		REL_TYPE(R_X86_64_COPY),
+		REL_TYPE(R_X86_64_GLOB_DAT),
+		REL_TYPE(R_X86_64_JUMP_SLOT),
+		REL_TYPE(R_X86_64_RELATIVE),
+		REL_TYPE(R_X86_64_GOTPCREL),
+		REL_TYPE(R_X86_64_32),
+		REL_TYPE(R_X86_64_32S),
+		REL_TYPE(R_X86_64_16),
+		REL_TYPE(R_X86_64_PC16),
+		REL_TYPE(R_X86_64_8),
+		REL_TYPE(R_X86_64_PC8),
+#else
 		REL_TYPE(R_386_NONE),
 		REL_TYPE(R_386_32),
 		REL_TYPE(R_386_PC32),
@@ -213,6 +223,7 @@ static const char *rel_type(unsigned type)
 		REL_TYPE(R_386_PC8),
 		REL_TYPE(R_386_16),
 		REL_TYPE(R_386_PC16),
+#endif
 #undef REL_TYPE
 	};
 	const char *name = "unknown type rel type name";
@@ -240,7 +251,7 @@ static const char *sec_name(unsigned shndx)
 	return name;
 }
 
-static const char *sym_name(const char *sym_strtab, Elf32_Sym *sym)
+static const char *sym_name(const char *sym_strtab, Elf_Sym *sym)
 {
 	const char *name;
 	name = "<noname>";
@@ -253,15 +264,42 @@ static const char *sym_name(const char *sym_strtab, Elf32_Sym *sym)
 	return name;
 }
 
+static Elf_Sym *sym_lookup(const char *symname)
+{
+	int i;
+	for (i = 0; i < ehdr.e_shnum; i++) {
+		struct section *sec = &secs[i];
+		long nsyms;
+		char *strtab;
+		Elf_Sym *symtab;
+		Elf_Sym *sym;
+
+		if (sec->shdr.sh_type != SHT_SYMTAB)
+			continue;
 
+		nsyms = sec->shdr.sh_size/sizeof(Elf_Sym);
+		symtab = sec->symtab;
+		strtab = sec->link->strtab;
+
+		for (sym = symtab; --nsyms >= 0; sym++) {
+			if (!sym->st_name)
+				continue;
+			if (strcmp(symname, strtab + sym->st_name) == 0)
+				return sym;
+		}
+	}
+	return 0;
+}
 
 #if BYTE_ORDER == LITTLE_ENDIAN
 #define le16_to_cpu(val) (val)
 #define le32_to_cpu(val) (val)
+#define le64_to_cpu(val) (val)
 #endif
 #if BYTE_ORDER == BIG_ENDIAN
 #define le16_to_cpu(val) bswap_16(val)
 #define le32_to_cpu(val) bswap_32(val)
+#define le64_to_cpu(val) bswap_64(val)
 #endif
 
 static uint16_t elf16_to_cpu(uint16_t val)
@@ -274,6 +312,23 @@ static uint32_t elf32_to_cpu(uint32_t val)
 	return le32_to_cpu(val);
 }
 
+#define elf_half_to_cpu(x)	elf16_to_cpu(x)
+#define elf_word_to_cpu(x)	elf32_to_cpu(x)
+
+#if ELF_BITS == 64
+static uint64_t elf64_to_cpu(uint64_t val)
+{
+        return le64_to_cpu(val);
+}
+#define elf_addr_to_cpu(x)	elf64_to_cpu(x)
+#define elf_off_to_cpu(x)	elf64_to_cpu(x)
+#define elf_xword_to_cpu(x)	elf64_to_cpu(x)
+#else
+#define elf_addr_to_cpu(x)	elf32_to_cpu(x)
+#define elf_off_to_cpu(x)	elf32_to_cpu(x)
+#define elf_xword_to_cpu(x)	elf32_to_cpu(x)
+#endif
+
 static void read_ehdr(FILE *fp)
 {
 	if (fread(&ehdr, sizeof(ehdr), 1, fp) != 1) {
@@ -283,8 +338,8 @@ static void read_ehdr(FILE *fp)
 	if (memcmp(ehdr.e_ident, ELFMAG, SELFMAG) != 0) {
 		die("No ELF magic\n");
 	}
-	if (ehdr.e_ident[EI_CLASS] != ELFCLASS32) {
-		die("Not a 32 bit executable\n");
+	if (ehdr.e_ident[EI_CLASS] != ELF_CLASS) {
+		die("Not a %d bit executable\n", ELF_BITS);
 	}
 	if (ehdr.e_ident[EI_DATA] != ELFDATA2LSB) {
 		die("Not a LSB ELF executable\n");
@@ -293,36 +348,36 @@ static void read_ehdr(FILE *fp)
 		die("Unknown ELF version\n");
 	}
 	/* Convert the fields to native endian */
-	ehdr.e_type      = elf16_to_cpu(ehdr.e_type);
-	ehdr.e_machine   = elf16_to_cpu(ehdr.e_machine);
-	ehdr.e_version   = elf32_to_cpu(ehdr.e_version);
-	ehdr.e_entry     = elf32_to_cpu(ehdr.e_entry);
-	ehdr.e_phoff     = elf32_to_cpu(ehdr.e_phoff);
-	ehdr.e_shoff     = elf32_to_cpu(ehdr.e_shoff);
-	ehdr.e_flags     = elf32_to_cpu(ehdr.e_flags);
-	ehdr.e_ehsize    = elf16_to_cpu(ehdr.e_ehsize);
-	ehdr.e_phentsize = elf16_to_cpu(ehdr.e_phentsize);
-	ehdr.e_phnum     = elf16_to_cpu(ehdr.e_phnum);
-	ehdr.e_shentsize = elf16_to_cpu(ehdr.e_shentsize);
-	ehdr.e_shnum     = elf16_to_cpu(ehdr.e_shnum);
-	ehdr.e_shstrndx  = elf16_to_cpu(ehdr.e_shstrndx);
+	ehdr.e_type      = elf_half_to_cpu(ehdr.e_type);
+	ehdr.e_machine   = elf_half_to_cpu(ehdr.e_machine);
+	ehdr.e_version   = elf_word_to_cpu(ehdr.e_version);
+	ehdr.e_entry     = elf_addr_to_cpu(ehdr.e_entry);
+	ehdr.e_phoff     = elf_off_to_cpu(ehdr.e_phoff);
+	ehdr.e_shoff     = elf_off_to_cpu(ehdr.e_shoff);
+	ehdr.e_flags     = elf_word_to_cpu(ehdr.e_flags);
+	ehdr.e_ehsize    = elf_half_to_cpu(ehdr.e_ehsize);
+	ehdr.e_phentsize = elf_half_to_cpu(ehdr.e_phentsize);
+	ehdr.e_phnum     = elf_half_to_cpu(ehdr.e_phnum);
+	ehdr.e_shentsize = elf_half_to_cpu(ehdr.e_shentsize);
+	ehdr.e_shnum     = elf_half_to_cpu(ehdr.e_shnum);
+	ehdr.e_shstrndx  = elf_half_to_cpu(ehdr.e_shstrndx);
 
 	if ((ehdr.e_type != ET_EXEC) && (ehdr.e_type != ET_DYN)) {
 		die("Unsupported ELF header type\n");
 	}
-	if (ehdr.e_machine != EM_386) {
-		die("Not for x86\n");
+	if (ehdr.e_machine != ELF_MACHINE) {
+		die("Not for %s\n", ELF_MACHINE_NAME);
 	}
 	if (ehdr.e_version != EV_CURRENT) {
 		die("Unknown ELF version\n");
 	}
-	if (ehdr.e_ehsize != sizeof(Elf32_Ehdr)) {
+	if (ehdr.e_ehsize != sizeof(Elf_Ehdr)) {
 		die("Bad Elf header size\n");
 	}
-	if (ehdr.e_phentsize != sizeof(Elf32_Phdr)) {
+	if (ehdr.e_phentsize != sizeof(Elf_Phdr)) {
 		die("Bad program header entry\n");
 	}
-	if (ehdr.e_shentsize != sizeof(Elf32_Shdr)) {
+	if (ehdr.e_shentsize != sizeof(Elf_Shdr)) {
 		die("Bad section header entry\n");
 	}
 	if (ehdr.e_shstrndx >= ehdr.e_shnum) {
@@ -333,7 +388,7 @@ static void read_ehdr(FILE *fp)
 static void read_shdrs(FILE *fp)
 {
 	int i;
-	Elf32_Shdr shdr;
+	Elf_Shdr shdr;
 
 	secs = calloc(ehdr.e_shnum, sizeof(struct section));
 	if (!secs) {
@@ -349,16 +404,16 @@ static void read_shdrs(FILE *fp)
 		if (fread(&shdr, sizeof shdr, 1, fp) != 1)
 			die("Cannot read ELF section headers %d/%d: %s\n",
 			    i, ehdr.e_shnum, strerror(errno));
-		sec->shdr.sh_name      = elf32_to_cpu(shdr.sh_name);
-		sec->shdr.sh_type      = elf32_to_cpu(shdr.sh_type);
-		sec->shdr.sh_flags     = elf32_to_cpu(shdr.sh_flags);
-		sec->shdr.sh_addr      = elf32_to_cpu(shdr.sh_addr);
-		sec->shdr.sh_offset    = elf32_to_cpu(shdr.sh_offset);
-		sec->shdr.sh_size      = elf32_to_cpu(shdr.sh_size);
-		sec->shdr.sh_link      = elf32_to_cpu(shdr.sh_link);
-		sec->shdr.sh_info      = elf32_to_cpu(shdr.sh_info);
-		sec->shdr.sh_addralign = elf32_to_cpu(shdr.sh_addralign);
-		sec->shdr.sh_entsize   = elf32_to_cpu(shdr.sh_entsize);
+		sec->shdr.sh_name      = elf_word_to_cpu(shdr.sh_name);
+		sec->shdr.sh_type      = elf_word_to_cpu(shdr.sh_type);
+		sec->shdr.sh_flags     = elf_xword_to_cpu(shdr.sh_flags);
+		sec->shdr.sh_addr      = elf_addr_to_cpu(shdr.sh_addr);
+		sec->shdr.sh_offset    = elf_off_to_cpu(shdr.sh_offset);
+		sec->shdr.sh_size      = elf_xword_to_cpu(shdr.sh_size);
+		sec->shdr.sh_link      = elf_word_to_cpu(shdr.sh_link);
+		sec->shdr.sh_info      = elf_word_to_cpu(shdr.sh_info);
+		sec->shdr.sh_addralign = elf_xword_to_cpu(shdr.sh_addralign);
+		sec->shdr.sh_entsize   = elf_xword_to_cpu(shdr.sh_entsize);
 		if (sec->shdr.sh_link < ehdr.e_shnum)
 			sec->link = &secs[sec->shdr.sh_link];
 	}
@@ -412,12 +467,12 @@ static void read_symtabs(FILE *fp)
 			die("Cannot read symbol table: %s\n",
 				strerror(errno));
 		}
-		for (j = 0; j < sec->shdr.sh_size/sizeof(Elf32_Sym); j++) {
-			Elf32_Sym *sym = &sec->symtab[j];
-			sym->st_name  = elf32_to_cpu(sym->st_name);
-			sym->st_value = elf32_to_cpu(sym->st_value);
-			sym->st_size  = elf32_to_cpu(sym->st_size);
-			sym->st_shndx = elf16_to_cpu(sym->st_shndx);
+		for (j = 0; j < sec->shdr.sh_size/sizeof(Elf_Sym); j++) {
+			Elf_Sym *sym = &sec->symtab[j];
+			sym->st_name  = elf_word_to_cpu(sym->st_name);
+			sym->st_value = elf_addr_to_cpu(sym->st_value);
+			sym->st_size  = elf_xword_to_cpu(sym->st_size);
+			sym->st_shndx = elf_half_to_cpu(sym->st_shndx);
 		}
 	}
 }
@@ -428,7 +483,7 @@ static void read_relocs(FILE *fp)
 	int i,j;
 	for (i = 0; i < ehdr.e_shnum; i++) {
 		struct section *sec = &secs[i];
-		if (sec->shdr.sh_type != SHT_REL) {
+		if (sec->shdr.sh_type != SHT_REL_TYPE) {
 			continue;
 		}
 		sec->reltab = malloc(sec->shdr.sh_size);
@@ -445,10 +500,13 @@ static void read_relocs(FILE *fp)
 			die("Cannot read symbol table: %s\n",
 				strerror(errno));
 		}
-		for (j = 0; j < sec->shdr.sh_size/sizeof(Elf32_Rel); j++) {
-			Elf32_Rel *rel = &sec->reltab[j];
-			rel->r_offset = elf32_to_cpu(rel->r_offset);
-			rel->r_info   = elf32_to_cpu(rel->r_info);
+		for (j = 0; j < sec->shdr.sh_size/sizeof(Elf_Rel); j++) {
+			Elf_Rel *rel = &sec->reltab[j];
+			rel->r_offset = elf_addr_to_cpu(rel->r_offset);
+			rel->r_info   = elf_xword_to_cpu(rel->r_info);
+#if (SHT_REL_TYPE == SHT_RELA)
+			rel->r_addend = elf_xword_to_cpu(rel->r_addend);
+#endif
 		}
 	}
 }
@@ -457,6 +515,13 @@ static void read_relocs(FILE *fp)
 static void print_absolute_symbols(void)
 {
 	int i;
+	const char *format;
+
+	if (ELF_BITS == 64)
+		format = "%5d %016"PRIx64" %5"PRId64" %10s %10s %12s %s\n";
+	else
+		format = "%5d %08"PRIx32"  %5"PRId32" %10s %10s %12s %s\n";
+
 	printf("Absolute symbols\n");
 	printf(" Num:    Value Size  Type       Bind        Visibility  Name\n");
 	for (i = 0; i < ehdr.e_shnum; i++) {
@@ -468,19 +533,19 @@ static void print_absolute_symbols(void)
 			continue;
 		}
 		sym_strtab = sec->link->strtab;
-		for (j = 0; j < sec->shdr.sh_size/sizeof(Elf32_Sym); j++) {
-			Elf32_Sym *sym;
+		for (j = 0; j < sec->shdr.sh_size/sizeof(Elf_Sym); j++) {
+			Elf_Sym *sym;
 			const char *name;
 			sym = &sec->symtab[j];
 			name = sym_name(sym_strtab, sym);
 			if (sym->st_shndx != SHN_ABS) {
 				continue;
 			}
-			printf("%5d %08x %5d %10s %10s %12s %s\n",
+			printf(format,
 				j, sym->st_value, sym->st_size,
-				sym_type(ELF32_ST_TYPE(sym->st_info)),
-				sym_bind(ELF32_ST_BIND(sym->st_info)),
-				sym_visibility(ELF32_ST_VISIBILITY(sym->st_other)),
+				sym_type(ELF_ST_TYPE(sym->st_info)),
+				sym_bind(ELF_ST_BIND(sym->st_info)),
+				sym_visibility(ELF_ST_VISIBILITY(sym->st_other)),
 				name);
 		}
 	}
@@ -490,14 +555,20 @@ static void print_absolute_symbols(void)
 static void print_absolute_relocs(void)
 {
 	int i, printed = 0;
+	const char *format;
+
+	if (ELF_BITS == 64)
+		format = "%016"PRIx64" %016"PRIx64" %10s %016"PRIx64"  %s\n";
+	else
+		format = "%08"PRIx32" %08"PRIx32" %10s %08"PRIx32"  %s\n";
 
 	for (i = 0; i < ehdr.e_shnum; i++) {
 		struct section *sec = &secs[i];
 		struct section *sec_applies, *sec_symtab;
 		char *sym_strtab;
-		Elf32_Sym *sh_symtab;
+		Elf_Sym *sh_symtab;
 		int j;
-		if (sec->shdr.sh_type != SHT_REL) {
+		if (sec->shdr.sh_type != SHT_REL_TYPE) {
 			continue;
 		}
 		sec_symtab  = sec->link;
@@ -507,12 +578,12 @@ static void print_absolute_relocs(void)
 		}
 		sh_symtab  = sec_symtab->symtab;
 		sym_strtab = sec_symtab->link->strtab;
-		for (j = 0; j < sec->shdr.sh_size/sizeof(Elf32_Rel); j++) {
-			Elf32_Rel *rel;
-			Elf32_Sym *sym;
+		for (j = 0; j < sec->shdr.sh_size/sizeof(Elf_Rel); j++) {
+			Elf_Rel *rel;
+			Elf_Sym *sym;
 			const char *name;
 			rel = &sec->reltab[j];
-			sym = &sh_symtab[ELF32_R_SYM(rel->r_info)];
+			sym = &sh_symtab[ELF_R_SYM(rel->r_info)];
 			name = sym_name(sym_strtab, sym);
 			if (sym->st_shndx != SHN_ABS) {
 				continue;
@@ -542,10 +613,10 @@ static void print_absolute_relocs(void)
 				printed = 1;
 			}
 
-			printf("%08x %08x %10s %08x  %s\n",
+			printf(format,
 				rel->r_offset,
 				rel->r_info,
-				rel_type(ELF32_R_TYPE(rel->r_info)),
+				rel_type(ELF_R_TYPE(rel->r_info)),
 				sym->st_value,
 				name);
 		}
@@ -555,19 +626,34 @@ static void print_absolute_relocs(void)
 		printf("\n");
 }
 
-static void walk_relocs(void (*visit)(Elf32_Rel *rel, Elf32_Sym *sym),
-			int use_real_mode)
+static void add_reloc(struct relocs *r, uint32_t offset)
+{
+	if (r->count == r->size) {
+		unsigned long newsize = r->size + 50000;
+		void *mem = realloc(r->offset, newsize * sizeof(r->offset[0]));
+
+		if (!mem)
+			die("realloc of %ld entries for relocs failed\n",
+                                newsize);
+		r->offset = mem;
+		r->size = newsize;
+	}
+	r->offset[r->count++] = offset;
+}
+
+static void walk_relocs(int (*process)(struct section *sec, Elf_Rel *rel,
+			Elf_Sym *sym, const char *symname))
 {
 	int i;
 	/* Walk through the relocations */
 	for (i = 0; i < ehdr.e_shnum; i++) {
 		char *sym_strtab;
-		Elf32_Sym *sh_symtab;
+		Elf_Sym *sh_symtab;
 		struct section *sec_applies, *sec_symtab;
 		int j;
 		struct section *sec = &secs[i];
 
-		if (sec->shdr.sh_type != SHT_REL) {
+		if (sec->shdr.sh_type != SHT_REL_TYPE) {
 			continue;
 		}
 		sec_symtab  = sec->link;
@@ -577,101 +663,281 @@ static void walk_relocs(void (*visit)(Elf32_Rel *rel, Elf32_Sym *sym),
 		}
 		sh_symtab = sec_symtab->symtab;
 		sym_strtab = sec_symtab->link->strtab;
-		for (j = 0; j < sec->shdr.sh_size/sizeof(Elf32_Rel); j++) {
-			Elf32_Rel *rel;
-			Elf32_Sym *sym;
-			unsigned r_type;
-			const char *symname;
-			int shn_abs;
+		for (j = 0; j < sec->shdr.sh_size/sizeof(Elf_Rel); j++) {
+			Elf_Rel *rel = &sec->reltab[j];
+			Elf_Sym *sym = &sh_symtab[ELF_R_SYM(rel->r_info)];
+			const char *symname = sym_name(sym_strtab, sym);
 
-			rel = &sec->reltab[j];
-			sym = &sh_symtab[ELF32_R_SYM(rel->r_info)];
-			r_type = ELF32_R_TYPE(rel->r_info);
-
-			shn_abs = sym->st_shndx == SHN_ABS;
-
-			switch (r_type) {
-			case R_386_NONE:
-			case R_386_PC32:
-			case R_386_PC16:
-			case R_386_PC8:
-				/*
-				 * NONE can be ignored and and PC relative
-				 * relocations don't need to be adjusted.
-				 */
-				break;
+			process(sec, rel, sym, symname);
+		}
+	}
+}
 
-			case R_386_16:
-				symname = sym_name(sym_strtab, sym);
-				if (!use_real_mode)
-					goto bad;
-				if (shn_abs) {
-					if (is_reloc(S_ABS, symname))
-						break;
-					else if (!is_reloc(S_SEG, symname))
-						goto bad;
-				} else {
-					if (is_reloc(S_LIN, symname))
-						goto bad;
-					else
-						break;
-				}
-				visit(rel, sym);
-				break;
+/*
+ * The .data..percpu section is a special case for x86_64 SMP kernels.
+ * It is used to initialize the actual per_cpu areas and to provide
+ * definitions for the per_cpu variables that correspond to their offsets
+ * within the percpu area. Since the values of all of the symbols need
+ * to be offsets from the start of the per_cpu area the virtual address
+ * (sh_addr) of .data..percpu is 0 in SMP kernels.
+ *
+ * This means that:
+ *
+ *	Relocations that reference symbols in the per_cpu area do not
+ *	need further relocation (since the value is an offset relative
+ *	to the start of the per_cpu area that does not change).
+ *
+ *	Relocations that apply to the per_cpu area need to have their
+ *	offset adjusted by by the value of __per_cpu_load to make them
+ *	point to the correct place in the loaded image (because the
+ *	virtual address of .data..percpu is 0).
+ *
+ * For non SMP kernels .data..percpu is linked as part of the normal
+ * kernel data and does not require special treatment.
+ *
+ */
+static int per_cpu_shndx	= -1;
+Elf_Addr per_cpu_load_addr;
 
-			case R_386_32:
-				symname = sym_name(sym_strtab, sym);
-				if (shn_abs) {
-					if (is_reloc(S_ABS, symname))
-						break;
-					else if (!is_reloc(S_REL, symname))
-						goto bad;
-				} else {
-					if (use_real_mode &&
-					    !is_reloc(S_LIN, symname))
-						break;
-				}
-				visit(rel, sym);
-				break;
-			default:
-				die("Unsupported relocation type: %s (%d)\n",
-				    rel_type(r_type), r_type);
+static void percpu_init(void)
+{
+	int i;
+	for (i = 0; i < ehdr.e_shnum; i++) {
+		ElfW(Sym) *sym;
+		if (strcmp(sec_name(i), ".data..percpu"))
+			continue;
+
+		if (secs[i].shdr.sh_addr != 0)	/* non SMP kernel */
+			return;
+
+		sym = sym_lookup("__per_cpu_load");
+		if (!sym)
+			die("can't find __per_cpu_load\n");
+
+		per_cpu_shndx = i;
+		per_cpu_load_addr = sym->st_value;
+		return;
+	}
+}
+
+#if ELF_BITS == 64
+
+/*
+ * Check to see if a symbol lies in the .data..percpu section.
+ * For some as yet not understood reason the "__init_begin"
+ * symbol which immediately preceeds the .data..percpu section
+ * also shows up as it it were part of it so we do an explict
+ * check for that symbol name and ignore it.
+ */
+static int is_percpu_sym(ElfW(Sym) *sym, const char *symname)
+{
+	return (sym->st_shndx == per_cpu_shndx) &&
+		strcmp(symname, "__init_begin");
+}
+
+
+static int do_reloc64(struct section *sec, Elf_Rel *rel, ElfW(Sym) *sym,
+		      const char *symname)
+{
+	unsigned r_type = ELF64_R_TYPE(rel->r_info);
+	ElfW(Addr) offset = rel->r_offset;
+	int shn_abs = (sym->st_shndx == SHN_ABS) && !is_reloc(S_REL, symname);
+
+	if (sym->st_shndx == SHN_UNDEF)
+		return 0;
+
+	/*
+	 * Adjust the offset if this reloc applies to the percpu section.
+	 */
+	if (sec->shdr.sh_info == per_cpu_shndx)
+		offset += per_cpu_load_addr;
+
+	switch (r_type) {
+	case R_X86_64_NONE:
+	case R_X86_64_PC32:
+		/*
+		 * NONE can be ignored and PC relative relocations don't
+		 * need to be adjusted.
+		 */
+		break;
+
+	case R_X86_64_32:
+	case R_X86_64_32S:
+	case R_X86_64_64:
+		/*
+		 * References to the percpu area don't need to be adjusted.
+		 */
+		if (is_percpu_sym(sym, symname))
+			break;
+
+		if (shn_abs) {
+			/*
+			 * Whitelisted absolute symbols do not require
+			 * relocation.
+			 */
+			if (is_reloc(S_ABS, symname))
 				break;
-			bad:
-				symname = sym_name(sym_strtab, sym);
-				die("Invalid %s %s relocation: %s\n",
-				    shn_abs ? "absolute" : "relative",
-				    rel_type(r_type), symname);
-			}
+
+			die("Invalid absolute %s relocation: %s\n",
+			    rel_type(r_type), symname);
+			break;
 		}
+
+		/*
+		 * Relocation offsets for 64 bit kernels are output
+		 * as 32 bits and sign extended back to 64 bits when
+		 * the relocations are processed.
+		 * Make sure that the offset will fit.
+		 */
+		if ((int32_t)offset != (int64_t)offset)
+			die("Relocation offset doesn't fit in 32 bits\n");
+
+		if (r_type == R_X86_64_64)
+			add_reloc(&relocs64, offset);
+		else
+			add_reloc(&relocs32, offset);
+		break;
+
+	default:
+		die("Unsupported relocation type: %s (%d)\n",
+		    rel_type(r_type), r_type);
+		break;
 	}
+
+	return 0;
 }
 
-static void count_reloc(Elf32_Rel *rel, Elf32_Sym *sym)
+#else
+
+static int do_reloc32(struct section *sec, Elf_Rel *rel, Elf_Sym *sym,
+		      const char *symname)
 {
-	if (ELF32_R_TYPE(rel->r_info) == R_386_16)
-		reloc16_count++;
-	else
-		reloc_count++;
+	unsigned r_type = ELF32_R_TYPE(rel->r_info);
+	int shn_abs = (sym->st_shndx == SHN_ABS) && !is_reloc(S_REL, symname);
+
+	switch (r_type) {
+	case R_386_NONE:
+	case R_386_PC32:
+	case R_386_PC16:
+	case R_386_PC8:
+		/*
+		 * NONE can be ignored and PC relative relocations don't
+		 * need to be adjusted.
+		 */
+		break;
+
+	case R_386_32:
+		if (shn_abs) {
+			/*
+			 * Whitelisted absolute symbols do not require
+			 * relocation.
+			 */
+			if (is_reloc(S_ABS, symname))
+				break;
+
+			die("Invalid absolute %s relocation: %s\n",
+			    rel_type(r_type), symname);
+			break;
+		}
+
+		add_reloc(&relocs32, rel->r_offset);
+		break;
+
+	default:
+		die("Unsupported relocation type: %s (%d)\n",
+		    rel_type(r_type), r_type);
+		break;
+	}
+
+	return 0;
 }
 
-static void collect_reloc(Elf32_Rel *rel, Elf32_Sym *sym)
+static int do_reloc_real(struct section *sec, Elf_Rel *rel, Elf_Sym *sym,
+			 const char *symname)
 {
-	/* Remember the address that needs to be adjusted. */
-	if (ELF32_R_TYPE(rel->r_info) == R_386_16)
-		relocs16[reloc16_idx++] = rel->r_offset;
-	else
-		relocs[reloc_idx++] = rel->r_offset;
+	unsigned r_type = ELF32_R_TYPE(rel->r_info);
+	int shn_abs = (sym->st_shndx == SHN_ABS) && !is_reloc(S_REL, symname);
+
+	switch (r_type) {
+	case R_386_NONE:
+	case R_386_PC32:
+	case R_386_PC16:
+	case R_386_PC8:
+		/*
+		 * NONE can be ignored and PC relative relocations don't
+		 * need to be adjusted.
+		 */
+		break;
+
+	case R_386_16:
+		if (shn_abs) {
+			/*
+			 * Whitelisted absolute symbols do not require
+			 * relocation.
+			 */
+			if (is_reloc(S_ABS, symname))
+				break;
+
+			if (is_reloc(S_SEG, symname)) {
+				add_reloc(&relocs16, rel->r_offset);
+				break;
+			}
+		} else {
+			if (!is_reloc(S_LIN, symname))
+				break;
+		}
+		die("Invalid %s %s relocation: %s\n",
+		    shn_abs ? "absolute" : "relative",
+		    rel_type(r_type), symname);
+		break;
+
+	case R_386_32:
+		if (shn_abs) {
+			/*
+			 * Whitelisted absolute symbols do not require
+			 * relocation.
+			 */
+			if (is_reloc(S_ABS, symname))
+				break;
+
+			if (is_reloc(S_REL, symname)) {
+				add_reloc(&relocs32, rel->r_offset);
+				break;
+			}
+		} else {
+			if (is_reloc(S_LIN, symname))
+				add_reloc(&relocs32, rel->r_offset);
+			break;
+		}
+		die("Invalid %s %s relocation: %s\n",
+		    shn_abs ? "absolute" : "relative",
+		    rel_type(r_type), symname);
+		break;
+
+	default:
+		die("Unsupported relocation type: %s (%d)\n",
+		    rel_type(r_type), r_type);
+		break;
+	}
+
+	return 0;
 }
 
+#endif
+
 static int cmp_relocs(const void *va, const void *vb)
 {
-	const unsigned long *a, *b;
+	const uint32_t *a, *b;
 	a = va; b = vb;
 	return (*a == *b)? 0 : (*a > *b)? 1 : -1;
 }
 
-static int write32(unsigned int v, FILE *f)
+static void sort_relocs(struct relocs *r)
+{
+	qsort(r->offset, r->count, sizeof(r->offset[0]), cmp_relocs);
+}
+
+static int write32(uint32_t v, FILE *f)
 {
 	unsigned char buf[4];
 
@@ -679,33 +945,40 @@ static int write32(unsigned int v, FILE *f)
 	return fwrite(buf, 1, 4, f) == 4 ? 0 : -1;
 }
 
+static int write32_as_text(uint32_t v, FILE *f)
+{
+	return fprintf(f, "\t.long 0x%08"PRIx32"\n", v) > 0 ? 0 : -1;
+}
+
 static void emit_relocs(int as_text, int use_real_mode)
 {
 	int i;
-	/* Count how many relocations I have and allocate space for them. */
-	reloc_count = 0;
-	walk_relocs(count_reloc, use_real_mode);
-	relocs = malloc(reloc_count * sizeof(relocs[0]));
-	if (!relocs) {
-		die("malloc of %d entries for relocs failed\n",
-			reloc_count);
-	}
+	int (*write_reloc)(uint32_t, FILE *) = write32;
+	int (*do_reloc)(struct section *sec, Elf_Rel *rel, Elf_Sym *sym,
+			const char *symname);
+
+#if ELF_BITS == 64
+	if (!use_real_mode)
+		do_reloc = do_reloc64;
+	else
+		die("--realmode not valid for a 64-bit ELF file");
+#else
+	if (!use_real_mode)
+		do_reloc = do_reloc32;
+	else
+		do_reloc = do_reloc_real;
+#endif
 
-	relocs16 = malloc(reloc16_count * sizeof(relocs[0]));
-	if (!relocs16) {
-		die("malloc of %d entries for relocs16 failed\n",
-			reloc16_count);
-	}
 	/* Collect up the relocations */
-	reloc_idx = 0;
-	walk_relocs(collect_reloc, use_real_mode);
+	walk_relocs(do_reloc);
 
-	if (reloc16_count && !use_real_mode)
+	if (relocs16.count && !use_real_mode)
 		die("Segment relocations found but --realmode not specified\n");
 
 	/* Order the relocations for more efficient processing */
-	qsort(relocs, reloc_count, sizeof(relocs[0]), cmp_relocs);
-	qsort(relocs16, reloc16_count, sizeof(relocs16[0]), cmp_relocs);
+	sort_relocs(&relocs16);
+	sort_relocs(&relocs32);
+	sort_relocs(&relocs64);
 
 	/* Print the relocations */
 	if (as_text) {
@@ -714,114 +987,60 @@ static void emit_relocs(int as_text, int use_real_mode)
 		 */
 		printf(".section \".data.reloc\",\"a\"\n");
 		printf(".balign 4\n");
-		if (use_real_mode) {
-			printf("\t.long %lu\n", reloc16_count);
-			for (i = 0; i < reloc16_count; i++)
-				printf("\t.long 0x%08lx\n", relocs16[i]);
-			printf("\t.long %lu\n", reloc_count);
-			for (i = 0; i < reloc_count; i++) {
-				printf("\t.long 0x%08lx\n", relocs[i]);
-			}
-		} else {
-			/* Print a stop */
-			printf("\t.long 0x%08lx\n", (unsigned long)0);
-			for (i = 0; i < reloc_count; i++) {
-				printf("\t.long 0x%08lx\n", relocs[i]);
-			}
-		}
-
-		printf("\n");
+		write_reloc = write32_as_text;
 	}
-	else {
-		if (use_real_mode) {
-			write32(reloc16_count, stdout);
-			for (i = 0; i < reloc16_count; i++)
-				write32(relocs16[i], stdout);
-			write32(reloc_count, stdout);
 
-			/* Now print each relocation */
-			for (i = 0; i < reloc_count; i++)
-				write32(relocs[i], stdout);
-		} else {
+	if (use_real_mode) {
+		write_reloc(relocs16.count, stdout);
+		for (i = 0; i < relocs16.count; i++)
+			write_reloc(relocs16.offset[i], stdout);
+
+		write_reloc(relocs32.count, stdout);
+		for (i = 0; i < relocs32.count; i++)
+			write_reloc(relocs32.offset[i], stdout);
+	} else {
+		if (ELF_BITS == 64) {
 			/* Print a stop */
-			write32(0, stdout);
+			write_reloc(0, stdout);
 
 			/* Now print each relocation */
-			for (i = 0; i < reloc_count; i++) {
-				write32(relocs[i], stdout);
-			}
+			for (i = 0; i < relocs64.count; i++)
+				write_reloc(relocs64.offset[i], stdout);
 		}
+
+		/* Print a stop */
+		write_reloc(0, stdout);
+
+		/* Now print each relocation */
+		for (i = 0; i < relocs32.count; i++)
+			write_reloc(relocs32.offset[i], stdout);
 	}
 }
 
-static void usage(void)
-{
-	die("relocs [--abs-syms|--abs-relocs|--text|--realmode] vmlinux\n");
-}
+#if ELF_BITS == 64
+# define process process_64
+#else
+# define process process_32
+#endif
 
-int main(int argc, char **argv)
+void process(FILE *fp, int use_real_mode, int as_text,
+	     int show_absolute_syms, int show_absolute_relocs)
 {
-	int show_absolute_syms, show_absolute_relocs;
-	int as_text, use_real_mode;
-	const char *fname;
-	FILE *fp;
-	int i;
-
-	show_absolute_syms = 0;
-	show_absolute_relocs = 0;
-	as_text = 0;
-	use_real_mode = 0;
-	fname = NULL;
-	for (i = 1; i < argc; i++) {
-		char *arg = argv[i];
-		if (*arg == '-') {
-			if (strcmp(arg, "--abs-syms") == 0) {
-				show_absolute_syms = 1;
-				continue;
-			}
-			if (strcmp(arg, "--abs-relocs") == 0) {
-				show_absolute_relocs = 1;
-				continue;
-			}
-			if (strcmp(arg, "--text") == 0) {
-				as_text = 1;
-				continue;
-			}
-			if (strcmp(arg, "--realmode") == 0) {
-				use_real_mode = 1;
-				continue;
-			}
-		}
-		else if (!fname) {
-			fname = arg;
-			continue;
-		}
-		usage();
-	}
-	if (!fname) {
-		usage();
-	}
 	regex_init(use_real_mode);
-	fp = fopen(fname, "r");
-	if (!fp) {
-		die("Cannot open %s: %s\n",
-			fname, strerror(errno));
-	}
 	read_ehdr(fp);
 	read_shdrs(fp);
 	read_strtabs(fp);
 	read_symtabs(fp);
 	read_relocs(fp);
+	if (ELF_BITS == 64)
+		percpu_init();
 	if (show_absolute_syms) {
 		print_absolute_symbols();
-		goto out;
+		return;
 	}
 	if (show_absolute_relocs) {
 		print_absolute_relocs();
-		goto out;
+		return;
 	}
 	emit_relocs(as_text, use_real_mode);
-out:
-	fclose(fp);
-	return 0;
 }
diff --git a/arch/x86/tools/relocs.h b/arch/x86/tools/relocs.h
new file mode 100644
index 00000000000..07cdb1eca4f
--- /dev/null
+++ b/arch/x86/tools/relocs.h
@@ -0,0 +1,36 @@
+#ifndef RELOCS_H
+#define RELOCS_H
+
+#include <stdio.h>
+#include <stdarg.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <inttypes.h>
+#include <string.h>
+#include <errno.h>
+#include <unistd.h>
+#include <elf.h>
+#include <byteswap.h>
+#define USE_BSD
+#include <endian.h>
+#include <regex.h>
+#include <tools/le_byteshift.h>
+
+void die(char *fmt, ...);
+
+#define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0]))
+
+enum symtype {
+	S_ABS,
+	S_REL,
+	S_SEG,
+	S_LIN,
+	S_NSYMTYPES
+};
+
+void process_32(FILE *fp, int use_real_mode, int as_text,
+		int show_absolute_syms, int show_absolute_relocs);
+void process_64(FILE *fp, int use_real_mode, int as_text,
+		int show_absolute_syms, int show_absolute_relocs);
+
+#endif /* RELOCS_H */
diff --git a/arch/x86/tools/relocs_32.c b/arch/x86/tools/relocs_32.c
new file mode 100644
index 00000000000..b2ade2bb416
--- /dev/null
+++ b/arch/x86/tools/relocs_32.c
@@ -0,0 +1,17 @@
+#include "relocs.h"
+
+#define ELF_BITS 32
+
+#define ELF_MACHINE		EM_386
+#define ELF_MACHINE_NAME	"i386"
+#define SHT_REL_TYPE		SHT_REL
+#define Elf_Rel			ElfW(Rel)
+
+#define ELF_CLASS		ELFCLASS32
+#define ELF_R_SYM(val)		ELF32_R_SYM(val)
+#define ELF_R_TYPE(val)		ELF32_R_TYPE(val)
+#define ELF_ST_TYPE(o)		ELF32_ST_TYPE(o)
+#define ELF_ST_BIND(o)		ELF32_ST_BIND(o)
+#define ELF_ST_VISIBILITY(o)	ELF32_ST_VISIBILITY(o)
+
+#include "relocs.c"
diff --git a/arch/x86/tools/relocs_64.c b/arch/x86/tools/relocs_64.c
new file mode 100644
index 00000000000..56b61b743c4
--- /dev/null
+++ b/arch/x86/tools/relocs_64.c
@@ -0,0 +1,17 @@
+#include "relocs.h"
+
+#define ELF_BITS 64
+
+#define ELF_MACHINE             EM_X86_64
+#define ELF_MACHINE_NAME        "x86_64"
+#define SHT_REL_TYPE            SHT_RELA
+#define Elf_Rel                 Elf64_Rela
+
+#define ELF_CLASS               ELFCLASS64
+#define ELF_R_SYM(val)          ELF64_R_SYM(val)
+#define ELF_R_TYPE(val)         ELF64_R_TYPE(val)
+#define ELF_ST_TYPE(o)          ELF64_ST_TYPE(o)
+#define ELF_ST_BIND(o)          ELF64_ST_BIND(o)
+#define ELF_ST_VISIBILITY(o)    ELF64_ST_VISIBILITY(o)
+
+#include "relocs.c"
diff --git a/arch/x86/tools/relocs_common.c b/arch/x86/tools/relocs_common.c
new file mode 100644
index 00000000000..44d396823a5
--- /dev/null
+++ b/arch/x86/tools/relocs_common.c
@@ -0,0 +1,76 @@
+#include "relocs.h"
+
+void die(char *fmt, ...)
+{
+	va_list ap;
+	va_start(ap, fmt);
+	vfprintf(stderr, fmt, ap);
+	va_end(ap);
+	exit(1);
+}
+
+static void usage(void)
+{
+	die("relocs [--abs-syms|--abs-relocs|--text|--realmode] vmlinux\n");
+}
+
+int main(int argc, char **argv)
+{
+	int show_absolute_syms, show_absolute_relocs;
+	int as_text, use_real_mode;
+	const char *fname;
+	FILE *fp;
+	int i;
+	unsigned char e_ident[EI_NIDENT];
+
+	show_absolute_syms = 0;
+	show_absolute_relocs = 0;
+	as_text = 0;
+	use_real_mode = 0;
+	fname = NULL;
+	for (i = 1; i < argc; i++) {
+		char *arg = argv[i];
+		if (*arg == '-') {
+			if (strcmp(arg, "--abs-syms") == 0) {
+				show_absolute_syms = 1;
+				continue;
+			}
+			if (strcmp(arg, "--abs-relocs") == 0) {
+				show_absolute_relocs = 1;
+				continue;
+			}
+			if (strcmp(arg, "--text") == 0) {
+				as_text = 1;
+				continue;
+			}
+			if (strcmp(arg, "--realmode") == 0) {
+				use_real_mode = 1;
+				continue;
+			}
+		}
+		else if (!fname) {
+			fname = arg;
+			continue;
+		}
+		usage();
+	}
+	if (!fname) {
+		usage();
+	}
+	fp = fopen(fname, "r");
+	if (!fp) {
+		die("Cannot open %s: %s\n", fname, strerror(errno));
+	}
+	if (fread(&e_ident, 1, EI_NIDENT, fp) != EI_NIDENT) {
+		die("Cannot read %s: %s", fname, strerror(errno));
+	}
+	rewind(fp);
+	if (e_ident[EI_CLASS] == ELFCLASS64)
+		process_64(fp, use_real_mode, as_text,
+			   show_absolute_syms, show_absolute_relocs);
+	else
+		process_32(fp, use_real_mode, as_text,
+			   show_absolute_syms, show_absolute_relocs);
+	fclose(fp);
+	return 0;
+}
diff --git a/arch/x86/xen/Kconfig b/arch/x86/xen/Kconfig
index 131dacd2748..1a3c7650564 100644
--- a/arch/x86/xen/Kconfig
+++ b/arch/x86/xen/Kconfig
@@ -4,7 +4,7 @@
 
 config XEN
 	bool "Xen guest support"
-	select PARAVIRT
+	depends on PARAVIRT
 	select PARAVIRT_CLOCK
 	select XEN_HAVE_PVMMU
 	depends on X86_64 || (X86_32 && X86_PAE && !X86_VISWS)
diff --git a/arch/x86/xen/enlighten.c b/arch/x86/xen/enlighten.c
index c8e1c7b95c3..53d4f680c9b 100644
--- a/arch/x86/xen/enlighten.c
+++ b/arch/x86/xen/enlighten.c
@@ -31,6 +31,7 @@
 #include <linux/pci.h>
 #include <linux/gfp.h>
 #include <linux/memblock.h>
+#include <linux/edd.h>
 
 #include <xen/xen.h>
 #include <xen/events.h>
@@ -1220,7 +1221,6 @@ static const struct pv_cpu_ops xen_cpu_ops __initconst = {
 	.alloc_ldt = xen_alloc_ldt,
 	.free_ldt = xen_free_ldt,
 
-	.store_gdt = native_store_gdt,
 	.store_idt = native_store_idt,
 	.store_tr = xen_store_tr,
 
@@ -1306,6 +1306,55 @@ static const struct machine_ops xen_machine_ops __initconst = {
 	.emergency_restart = xen_emergency_restart,
 };
 
+static void __init xen_boot_params_init_edd(void)
+{
+#if IS_ENABLED(CONFIG_EDD)
+	struct xen_platform_op op;
+	struct edd_info *edd_info;
+	u32 *mbr_signature;
+	unsigned nr;
+	int ret;
+
+	edd_info = boot_params.eddbuf;
+	mbr_signature = boot_params.edd_mbr_sig_buffer;
+
+	op.cmd = XENPF_firmware_info;
+
+	op.u.firmware_info.type = XEN_FW_DISK_INFO;
+	for (nr = 0; nr < EDDMAXNR; nr++) {
+		struct edd_info *info = edd_info + nr;
+
+		op.u.firmware_info.index = nr;
+		info->params.length = sizeof(info->params);
+		set_xen_guest_handle(op.u.firmware_info.u.disk_info.edd_params,
+				     &info->params);
+		ret = HYPERVISOR_dom0_op(&op);
+		if (ret)
+			break;
+
+#define C(x) info->x = op.u.firmware_info.u.disk_info.x
+		C(device);
+		C(version);
+		C(interface_support);
+		C(legacy_max_cylinder);
+		C(legacy_max_head);
+		C(legacy_sectors_per_track);
+#undef C
+	}
+	boot_params.eddbuf_entries = nr;
+
+	op.u.firmware_info.type = XEN_FW_DISK_MBR_SIGNATURE;
+	for (nr = 0; nr < EDD_MBR_SIG_MAX; nr++) {
+		op.u.firmware_info.index = nr;
+		ret = HYPERVISOR_dom0_op(&op);
+		if (ret)
+			break;
+		mbr_signature[nr] = op.u.firmware_info.u.disk_mbr_signature.mbr_signature;
+	}
+	boot_params.edd_mbr_sig_buf_entries = nr;
+#endif
+}
+
 /*
  * Set up the GDT and segment registers for -fstack-protector.  Until
  * we do this, we have to be careful not to call any stack-protected
@@ -1508,6 +1557,8 @@ asmlinkage void __init xen_start_kernel(void)
 		/* Avoid searching for BIOS MP tables */
 		x86_init.mpparse.find_smp_config = x86_init_noop;
 		x86_init.mpparse.get_smp_config = x86_init_uint_noop;
+
+		xen_boot_params_init_edd();
 	}
 #ifdef CONFIG_PCI
 	/* PCI BIOS service won't work from a PV guest. */
@@ -1589,8 +1640,11 @@ static int __cpuinit xen_hvm_cpu_notify(struct notifier_block *self,
 	switch (action) {
 	case CPU_UP_PREPARE:
 		xen_vcpu_setup(cpu);
-		if (xen_have_vector_callback)
+		if (xen_have_vector_callback) {
 			xen_init_lock_cpu(cpu);
+			if (xen_feature(XENFEAT_hvm_safe_pvclock))
+				xen_setup_timer(cpu);
+		}
 		break;
 	default:
 		break;
diff --git a/arch/x86/xen/mmu.c b/arch/x86/xen/mmu.c
index e8e34938c57..fdc3ba28ca3 100644
--- a/arch/x86/xen/mmu.c
+++ b/arch/x86/xen/mmu.c
@@ -1467,8 +1467,6 @@ static void __init xen_write_cr3_init(unsigned long cr3)
 	__xen_write_cr3(true, cr3);
 
 	xen_mc_issue(PARAVIRT_LAZY_CPU);  /* interrupts restored */
-
-	pv_mmu_ops.write_cr3 = &xen_write_cr3;
 }
 #endif
 
@@ -1750,14 +1748,18 @@ static void *m2v(phys_addr_t maddr)
 }
 
 /* Set the page permissions on an identity-mapped pages */
-static void set_page_prot(void *addr, pgprot_t prot)
+static void set_page_prot_flags(void *addr, pgprot_t prot, unsigned long flags)
 {
 	unsigned long pfn = __pa(addr) >> PAGE_SHIFT;
 	pte_t pte = pfn_pte(pfn, prot);
 
-	if (HYPERVISOR_update_va_mapping((unsigned long)addr, pte, 0))
+	if (HYPERVISOR_update_va_mapping((unsigned long)addr, pte, flags))
 		BUG();
 }
+static void set_page_prot(void *addr, pgprot_t prot)
+{
+	return set_page_prot_flags(addr, prot, UVMF_NONE);
+}
 #ifdef CONFIG_X86_32
 static void __init xen_map_identity_early(pmd_t *pmd, unsigned long max_pfn)
 {
@@ -1841,12 +1843,12 @@ static void __init check_pt_base(unsigned long *pt_base, unsigned long *pt_end,
 				 unsigned long addr)
 {
 	if (*pt_base == PFN_DOWN(__pa(addr))) {
-		set_page_prot((void *)addr, PAGE_KERNEL);
+		set_page_prot_flags((void *)addr, PAGE_KERNEL, UVMF_INVLPG);
 		clear_page((void *)addr);
 		(*pt_base)++;
 	}
 	if (*pt_end == PFN_DOWN(__pa(addr))) {
-		set_page_prot((void *)addr, PAGE_KERNEL);
+		set_page_prot_flags((void *)addr, PAGE_KERNEL, UVMF_INVLPG);
 		clear_page((void *)addr);
 		(*pt_end)--;
 	}
@@ -2041,9 +2043,7 @@ static void xen_set_fixmap(unsigned idx, phys_addr_t phys, pgprot_t prot)
 
 	switch (idx) {
 	case FIX_BTMAP_END ... FIX_BTMAP_BEGIN:
-#ifdef CONFIG_X86_F00F_BUG
-	case FIX_F00F_IDT:
-#endif
+	case FIX_RO_IDT:
 #ifdef CONFIG_X86_32
 	case FIX_WP_TEST:
 	case FIX_VDSO:
@@ -2122,6 +2122,7 @@ static void __init xen_post_allocator_init(void)
 #endif
 
 #ifdef CONFIG_X86_64
+	pv_mmu_ops.write_cr3 = &xen_write_cr3;
 	SetPagePinned(virt_to_page(level3_user_vsyscall));
 #endif
 	xen_mark_init_mm_pinned();
@@ -2197,6 +2198,7 @@ static const struct pv_mmu_ops xen_mmu_ops __initconst = {
 	.lazy_mode = {
 		.enter = paravirt_enter_lazy_mmu,
 		.leave = xen_leave_lazy_mmu,
+		.flush = paravirt_flush_lazy_mmu,
 	},
 
 	.set_fixmap = xen_set_fixmap,
diff --git a/arch/x86/xen/smp.c b/arch/x86/xen/smp.c
index 09ea61d2e02..8ff37995d54 100644
--- a/arch/x86/xen/smp.c
+++ b/arch/x86/xen/smp.c
@@ -95,7 +95,7 @@ static void __cpuinit cpu_bringup(void)
 static void __cpuinit cpu_bringup_and_idle(void)
 {
 	cpu_bringup();
-	cpu_idle();
+	cpu_startup_entry(CPUHP_ONLINE);
 }
 
 static int xen_smp_intr_init(unsigned int cpu)
@@ -144,6 +144,13 @@ static int xen_smp_intr_init(unsigned int cpu)
 		goto fail;
 	per_cpu(xen_callfuncsingle_irq, cpu) = rc;
 
+	/*
+	 * The IRQ worker on PVHVM goes through the native path and uses the
+	 * IPI mechanism.
+	 */
+	if (xen_hvm_domain())
+		return 0;
+
 	callfunc_name = kasprintf(GFP_KERNEL, "irqwork%d", cpu);
 	rc = bind_ipi_to_irqhandler(XEN_IRQ_WORK_VECTOR,
 				    cpu,
@@ -167,6 +174,9 @@ static int xen_smp_intr_init(unsigned int cpu)
 	if (per_cpu(xen_callfuncsingle_irq, cpu) >= 0)
 		unbind_from_irqhandler(per_cpu(xen_callfuncsingle_irq, cpu),
 				       NULL);
+	if (xen_hvm_domain())
+		return rc;
+
 	if (per_cpu(xen_irq_work, cpu) >= 0)
 		unbind_from_irqhandler(per_cpu(xen_irq_work, cpu), NULL);
 
@@ -418,7 +428,7 @@ static int xen_cpu_disable(void)
 
 static void xen_cpu_die(unsigned int cpu)
 {
-	while (HYPERVISOR_vcpu_op(VCPUOP_is_up, cpu, NULL)) {
+	while (xen_pv_domain() && HYPERVISOR_vcpu_op(VCPUOP_is_up, cpu, NULL)) {
 		current->state = TASK_UNINTERRUPTIBLE;
 		schedule_timeout(HZ/10);
 	}
@@ -426,7 +436,8 @@ static void xen_cpu_die(unsigned int cpu)
 	unbind_from_irqhandler(per_cpu(xen_callfunc_irq, cpu), NULL);
 	unbind_from_irqhandler(per_cpu(xen_debug_irq, cpu), NULL);
 	unbind_from_irqhandler(per_cpu(xen_callfuncsingle_irq, cpu), NULL);
-	unbind_from_irqhandler(per_cpu(xen_irq_work, cpu), NULL);
+	if (!xen_hvm_domain())
+		unbind_from_irqhandler(per_cpu(xen_irq_work, cpu), NULL);
 	xen_uninit_lock_cpu(cpu);
 	xen_teardown_timer(cpu);
 }
@@ -657,11 +668,7 @@ static int __cpuinit xen_hvm_cpu_up(unsigned int cpu, struct task_struct *tidle)
 
 static void xen_hvm_cpu_die(unsigned int cpu)
 {
-	unbind_from_irqhandler(per_cpu(xen_resched_irq, cpu), NULL);
-	unbind_from_irqhandler(per_cpu(xen_callfunc_irq, cpu), NULL);
-	unbind_from_irqhandler(per_cpu(xen_debug_irq, cpu), NULL);
-	unbind_from_irqhandler(per_cpu(xen_callfuncsingle_irq, cpu), NULL);
-	unbind_from_irqhandler(per_cpu(xen_irq_work, cpu), NULL);
+	xen_cpu_die(cpu);
 	native_cpu_die(cpu);
 }
 
diff --git a/arch/x86/xen/spinlock.c b/arch/x86/xen/spinlock.c
index f7a080ef035..8b54603ce81 100644
--- a/arch/x86/xen/spinlock.c
+++ b/arch/x86/xen/spinlock.c
@@ -364,6 +364,16 @@ void __cpuinit xen_init_lock_cpu(int cpu)
 	int irq;
 	const char *name;
 
+	WARN(per_cpu(lock_kicker_irq, cpu) > 0, "spinlock on CPU%d exists on IRQ%d!\n",
+	     cpu, per_cpu(lock_kicker_irq, cpu));
+
+	/*
+	 * See git commit f10cd522c5fbfec9ae3cc01967868c9c2401ed23
+	 * (xen: disable PV spinlocks on HVM)
+	 */
+	if (xen_hvm_domain())
+		return;
+
 	name = kasprintf(GFP_KERNEL, "spinlock%d", cpu);
 	irq = bind_ipi_to_irqhandler(XEN_SPIN_UNLOCK_VECTOR,
 				     cpu,
@@ -382,11 +392,26 @@ void __cpuinit xen_init_lock_cpu(int cpu)
 
 void xen_uninit_lock_cpu(int cpu)
 {
+	/*
+	 * See git commit f10cd522c5fbfec9ae3cc01967868c9c2401ed23
+	 * (xen: disable PV spinlocks on HVM)
+	 */
+	if (xen_hvm_domain())
+		return;
+
 	unbind_from_irqhandler(per_cpu(lock_kicker_irq, cpu), NULL);
+	per_cpu(lock_kicker_irq, cpu) = -1;
 }
 
 void __init xen_init_spinlocks(void)
 {
+	/*
+	 * See git commit f10cd522c5fbfec9ae3cc01967868c9c2401ed23
+	 * (xen: disable PV spinlocks on HVM)
+	 */
+	if (xen_hvm_domain())
+		return;
+
 	BUILD_BUG_ON(sizeof(struct xen_spinlock) > sizeof(arch_spinlock_t));
 
 	pv_lock_ops.spin_is_locked = xen_spin_is_locked;
diff --git a/arch/x86/xen/time.c b/arch/x86/xen/time.c
index 0296a952250..3d88bfdf9e1 100644
--- a/arch/x86/xen/time.c
+++ b/arch/x86/xen/time.c
@@ -377,7 +377,7 @@ static const struct clock_event_device xen_vcpuop_clockevent = {
 
 static const struct clock_event_device *xen_clockevent =
 	&xen_timerop_clockevent;
-static DEFINE_PER_CPU(struct clock_event_device, xen_clock_events);
+static DEFINE_PER_CPU(struct clock_event_device, xen_clock_events) = { .irq = -1 };
 
 static irqreturn_t xen_timer_interrupt(int irq, void *dev_id)
 {
@@ -401,6 +401,9 @@ void xen_setup_timer(int cpu)
 	struct clock_event_device *evt;
 	int irq;
 
+	evt = &per_cpu(xen_clock_events, cpu);
+	WARN(evt->irq >= 0, "IRQ%d for CPU%d is already allocated\n", evt->irq, cpu);
+
 	printk(KERN_INFO "installing Xen timer for CPU %d\n", cpu);
 
 	name = kasprintf(GFP_KERNEL, "timer%d", cpu);
@@ -413,7 +416,6 @@ void xen_setup_timer(int cpu)
 				      IRQF_FORCE_RESUME,
 				      name, NULL);
 
-	evt = &per_cpu(xen_clock_events, cpu);
 	memcpy(evt, xen_clockevent, sizeof(*evt));
 
 	evt->cpumask = cpumask_of(cpu);
@@ -426,6 +428,7 @@ void xen_teardown_timer(int cpu)
 	BUG_ON(cpu == 0);
 	evt = &per_cpu(xen_clock_events, cpu);
 	unbind_from_irqhandler(evt->irq, NULL);
+	evt->irq = -1;
 }
 
 void xen_setup_cpu_clockevents(void)
@@ -497,7 +500,11 @@ static void xen_hvm_setup_cpu_clockevents(void)
 {
 	int cpu = smp_processor_id();
 	xen_setup_runstate_info(cpu);
-	xen_setup_timer(cpu);
+	/*
+	 * xen_setup_timer(cpu) - snprintf is bad in atomic context. Hence
+	 * doing it xen_hvm_cpu_notify (which gets called by smp_init during
+	 * early bootup and also during CPU hotplug events).
+	 */
 	xen_setup_cpu_clockevents();
 }
 
diff --git a/arch/xtensa/kernel/process.c b/arch/xtensa/kernel/process.c
index 5cd82e9f601..1c85323f01d 100644
--- a/arch/xtensa/kernel/process.c
+++ b/arch/xtensa/kernel/process.c
@@ -105,19 +105,9 @@ void coprocessor_flush_all(struct thread_info *ti)
 /*
  * Powermanagement idle function, if any is provided by the platform.
  */
-
-void cpu_idle(void)
+void arch_cpu_idle(void)
 {
-	local_irq_enable();
-
-	/* endless idle loop with no priority at all */
-	while (1) {
-		rcu_idle_enter();
-		while (!need_resched())
-			platform_idle();
-		rcu_idle_exit();
-		schedule_preempt_disabled();
-	}
+	platform_idle();
 }
 
 /*
diff --git a/arch/xtensa/mm/init.c b/arch/xtensa/mm/init.c
index 7a5156ffebb..bba125b4bb0 100644
--- a/arch/xtensa/mm/init.c
+++ b/arch/xtensa/mm/init.c
@@ -208,32 +208,17 @@ void __init mem_init(void)
 	       highmemsize >> 10);
 }
 
-void
-free_reserved_mem(void *start, void *end)
-{
-	for (; start < end; start += PAGE_SIZE) {
-		ClearPageReserved(virt_to_page(start));
-		init_page_count(virt_to_page(start));
-		free_page((unsigned long)start);
-		totalram_pages++;
-	}
-}
-
 #ifdef CONFIG_BLK_DEV_INITRD
 extern int initrd_is_mapped;
 
 void free_initrd_mem(unsigned long start, unsigned long end)
 {
-	if (initrd_is_mapped) {
-		free_reserved_mem((void*)start, (void*)end);
-		printk ("Freeing initrd memory: %ldk freed\n",(end-start)>>10);
-	}
+	if (initrd_is_mapped)
+		free_reserved_area(start, end, 0, "initrd");
 }
 #endif
 
 void free_initmem(void)
 {
-	free_reserved_mem(__init_begin, __init_end);
-	printk("Freeing unused kernel memory: %zuk freed\n",
-	       (__init_end - __init_begin) >> 10);
+	free_initmem_default(0);
 }
diff --git a/block/blk-cgroup.h b/block/blk-cgroup.h
index f2b292925cc..4e595ee8c91 100644
--- a/block/blk-cgroup.h
+++ b/block/blk-cgroup.h
@@ -247,9 +247,7 @@ static inline int blkg_path(struct blkcg_gq *blkg, char *buf, int buflen)
 {
 	int ret;
 
-	rcu_read_lock();
 	ret = cgroup_path(blkg->blkcg->css.cgroup, buf, buflen);
-	rcu_read_unlock();
 	if (ret)
 		strncpy(buf, "<unavailable>", buflen);
 	return ret;
diff --git a/block/blk-core.c b/block/blk-core.c
index 074b758efc4..7c288358a74 100644
--- a/block/blk-core.c
+++ b/block/blk-core.c
@@ -39,6 +39,7 @@
 
 EXPORT_TRACEPOINT_SYMBOL_GPL(block_bio_remap);
 EXPORT_TRACEPOINT_SYMBOL_GPL(block_rq_remap);
+EXPORT_TRACEPOINT_SYMBOL_GPL(block_bio_complete);
 EXPORT_TRACEPOINT_SYMBOL_GPL(block_unplug);
 
 DEFINE_IDA(blk_queue_ida);
diff --git a/block/blk-flush.c b/block/blk-flush.c
index db8f1b50785..cc2b827a853 100644
--- a/block/blk-flush.c
+++ b/block/blk-flush.c
@@ -444,7 +444,7 @@ int blkdev_issue_flush(struct block_device *bdev, gfp_t gfp_mask,
 	 * copied from blk_rq_pos(rq).
 	 */
 	if (error_sector)
-               *error_sector = bio->bi_sector;
+		*error_sector = bio->bi_sector;
 
 	if (!bio_flagged(bio, BIO_UPTODATE))
 		ret = -EIO;
diff --git a/block/blk-sysfs.c b/block/blk-sysfs.c
index 6206a934eb8..5efc5a64718 100644
--- a/block/blk-sysfs.c
+++ b/block/blk-sysfs.c
@@ -229,6 +229,8 @@ queue_store_##name(struct request_queue *q, const char *page, size_t count) \
 	unsigned long val;						\
 	ssize_t ret;							\
 	ret = queue_var_store(&val, page, count);			\
+	if (ret < 0)							\
+		 return ret;						\
 	if (neg)							\
 		val = !val;						\
 									\
diff --git a/block/genhd.c b/block/genhd.c
index 3c001fba80c..20625eed551 100644
--- a/block/genhd.c
+++ b/block/genhd.c
@@ -1111,7 +1111,8 @@ struct class block_class = {
 	.name		= "block",
 };
 
-static char *block_devnode(struct device *dev, umode_t *mode)
+static char *block_devnode(struct device *dev, umode_t *mode,
+			   kuid_t *uid, kgid_t *gid)
 {
 	struct gendisk *disk = dev_to_disk(dev);
 
diff --git a/crypto/algif_hash.c b/crypto/algif_hash.c
index ef5356cd280..0262210cad3 100644
--- a/crypto/algif_hash.c
+++ b/crypto/algif_hash.c
@@ -161,6 +161,8 @@ static int hash_recvmsg(struct kiocb *unused, struct socket *sock,
 	else if (len < ds)
 		msg->msg_flags |= MSG_TRUNC;
 
+	msg->msg_namelen = 0;
+
 	lock_sock(sk);
 	if (ctx->more) {
 		ctx->more = 0;
diff --git a/crypto/algif_skcipher.c b/crypto/algif_skcipher.c
index 6a6dfc062d2..a1c4f0a5558 100644
--- a/crypto/algif_skcipher.c
+++ b/crypto/algif_skcipher.c
@@ -432,6 +432,7 @@ static int skcipher_recvmsg(struct kiocb *unused, struct socket *sock,
 	long copied = 0;
 
 	lock_sock(sk);
+	msg->msg_namelen = 0;
 	for (iov = msg->msg_iov, iovlen = msg->msg_iovlen; iovlen > 0;
 	     iovlen--, iov++) {
 		unsigned long seglen = iov->iov_len;
diff --git a/crypto/async_tx/raid6test.c b/crypto/async_tx/raid6test.c
index aa2b0270ed1..4a92bac744d 100644
--- a/crypto/async_tx/raid6test.c
+++ b/crypto/async_tx/raid6test.c
@@ -46,15 +46,10 @@ static void callback(void *param)
 
 static void makedata(int disks)
 {
-	int i, j;
+	int i;
 
 	for (i = 0; i < disks; i++) {
-		for (j = 0; j < PAGE_SIZE/sizeof(u32); j += sizeof(u32)) {
-			u32 *p = page_address(data[i]) + j;
-
-			*p = random32();
-		}
-
+		prandom_bytes(page_address(data[i]), PAGE_SIZE);
 		dataptrs[i] = data[i];
 	}
 }
diff --git a/crypto/gcm.c b/crypto/gcm.c
index 137ad1ec543..13ccbda34ff 100644
--- a/crypto/gcm.c
+++ b/crypto/gcm.c
@@ -44,6 +44,7 @@ struct crypto_rfc4543_ctx {
 
 struct crypto_rfc4543_req_ctx {
 	u8 auth_tag[16];
+	u8 assocbuf[32];
 	struct scatterlist cipher[1];
 	struct scatterlist payload[2];
 	struct scatterlist assoc[2];
@@ -1133,9 +1134,19 @@ static struct aead_request *crypto_rfc4543_crypt(struct aead_request *req,
 	scatterwalk_crypto_chain(payload, dst, vdst == req->iv + 8, 2);
 	assoclen += 8 + req->cryptlen - (enc ? 0 : authsize);
 
-	sg_init_table(assoc, 2);
-	sg_set_page(assoc, sg_page(req->assoc), req->assoc->length,
-		    req->assoc->offset);
+	if (req->assoc->length == req->assoclen) {
+		sg_init_table(assoc, 2);
+		sg_set_page(assoc, sg_page(req->assoc), req->assoc->length,
+			    req->assoc->offset);
+	} else {
+		BUG_ON(req->assoclen > sizeof(rctx->assocbuf));
+
+		scatterwalk_map_and_copy(rctx->assocbuf, req->assoc, 0,
+					 req->assoclen, 0);
+
+		sg_init_table(assoc, 2);
+		sg_set_buf(assoc, rctx->assocbuf, req->assoclen);
+	}
 	scatterwalk_crypto_chain(assoc, payload, 0, 2);
 
 	aead_request_set_tfm(subreq, ctx->child);
diff --git a/drivers/Kconfig b/drivers/Kconfig
index 202fa6d051b..78a956e286e 100644
--- a/drivers/Kconfig
+++ b/drivers/Kconfig
@@ -52,6 +52,8 @@ source "drivers/i2c/Kconfig"
 
 source "drivers/spi/Kconfig"
 
+source "drivers/ssbi/Kconfig"
+
 source "drivers/hsi/Kconfig"
 
 source "drivers/pps/Kconfig"
diff --git a/drivers/Makefile b/drivers/Makefile
index dce39a95fa7..33360de6365 100644
--- a/drivers/Makefile
+++ b/drivers/Makefile
@@ -79,7 +79,7 @@ obj-$(CONFIG_ATA_OVER_ETH)	+= block/aoe/
 obj-$(CONFIG_PARIDE) 		+= block/paride/
 obj-$(CONFIG_TC)		+= tc/
 obj-$(CONFIG_UWB)		+= uwb/
-obj-$(CONFIG_USB_OTG_UTILS)	+= usb/
+obj-$(CONFIG_USB_PHY)		+= usb/
 obj-$(CONFIG_USB)		+= usb/
 obj-$(CONFIG_PCI)		+= usb/
 obj-$(CONFIG_USB_GADGET)	+= usb/
@@ -114,6 +114,7 @@ obj-y				+= firmware/
 obj-$(CONFIG_CRYPTO)		+= crypto/
 obj-$(CONFIG_SUPERH)		+= sh/
 obj-$(CONFIG_ARCH_SHMOBILE)	+= sh/
+obj-$(CONFIG_SSBI)		+= ssbi/
 ifndef CONFIG_ARCH_USES_GETTIMEOFFSET
 obj-y				+= clocksource/
 endif
diff --git a/drivers/acpi/Kconfig b/drivers/acpi/Kconfig
index 92ed9692c47..4bf68c8d479 100644
--- a/drivers/acpi/Kconfig
+++ b/drivers/acpi/Kconfig
@@ -396,7 +396,7 @@ config ACPI_CUSTOM_METHOD
 
 config ACPI_BGRT
 	bool "Boottime Graphics Resource Table support"
-	depends on EFI
+	depends on EFI && X86
         help
 	  This driver adds support for exposing the ACPI Boottime Graphics
 	  Resource Table, which allows the operating system to obtain
diff --git a/drivers/acpi/acpi_i2c.c b/drivers/acpi/acpi_i2c.c
index 82045e3f5ca..a82c7626aa9 100644
--- a/drivers/acpi/acpi_i2c.c
+++ b/drivers/acpi/acpi_i2c.c
@@ -90,7 +90,7 @@ void acpi_i2c_register_devices(struct i2c_adapter *adapter)
 	acpi_handle handle;
 	acpi_status status;
 
-	handle = ACPI_HANDLE(&adapter->dev);
+	handle = ACPI_HANDLE(adapter->dev.parent);
 	if (!handle)
 		return;
 
diff --git a/drivers/acpi/apei/cper.c b/drivers/acpi/apei/cper.c
index 1e5d8a40101..fefc2ca7cc3 100644
--- a/drivers/acpi/apei/cper.c
+++ b/drivers/acpi/apei/cper.c
@@ -405,7 +405,7 @@ int apei_estatus_check(const struct acpi_hest_generic_status *estatus)
 		return rc;
 	data_len = estatus->data_length;
 	gdata = (struct acpi_hest_generic_data *)(estatus + 1);
-	while (data_len > sizeof(*gdata)) {
+	while (data_len >= sizeof(*gdata)) {
 		gedata_len = gdata->error_data_length;
 		if (gedata_len > data_len - sizeof(*gdata))
 			return -EINVAL;
diff --git a/drivers/acpi/pci_root.c b/drivers/acpi/pci_root.c
index 0ac546d5e53..ac8688b8970 100644
--- a/drivers/acpi/pci_root.c
+++ b/drivers/acpi/pci_root.c
@@ -65,44 +65,12 @@ static struct acpi_scan_handler pci_root_handler = {
 	.detach = acpi_pci_root_remove,
 };
 
-/* Lock to protect both acpi_pci_roots and acpi_pci_drivers lists */
+/* Lock to protect both acpi_pci_roots lists */
 static DEFINE_MUTEX(acpi_pci_root_lock);
 static LIST_HEAD(acpi_pci_roots);
-static LIST_HEAD(acpi_pci_drivers);
 
 static DEFINE_MUTEX(osc_lock);
 
-int acpi_pci_register_driver(struct acpi_pci_driver *driver)
-{
-	int n = 0;
-	struct acpi_pci_root *root;
-
-	mutex_lock(&acpi_pci_root_lock);
-	list_add_tail(&driver->node, &acpi_pci_drivers);
-	if (driver->add)
-		list_for_each_entry(root, &acpi_pci_roots, node) {
-			driver->add(root);
-			n++;
-		}
-	mutex_unlock(&acpi_pci_root_lock);
-
-	return n;
-}
-EXPORT_SYMBOL(acpi_pci_register_driver);
-
-void acpi_pci_unregister_driver(struct acpi_pci_driver *driver)
-{
-	struct acpi_pci_root *root;
-
-	mutex_lock(&acpi_pci_root_lock);
-	list_del(&driver->node);
-	if (driver->remove)
-		list_for_each_entry(root, &acpi_pci_roots, node)
-			driver->remove(root);
-	mutex_unlock(&acpi_pci_root_lock);
-}
-EXPORT_SYMBOL(acpi_pci_unregister_driver);
-
 /**
  * acpi_is_root_bridge - determine whether an ACPI CA node is a PCI root bridge
  * @handle - the ACPI CA node in question.
@@ -413,9 +381,7 @@ static int acpi_pci_root_add(struct acpi_device *device,
 	acpi_status status;
 	int result;
 	struct acpi_pci_root *root;
-	struct acpi_pci_driver *driver;
 	u32 flags, base_flags;
-	bool is_osc_granted = false;
 
 	root = kzalloc(sizeof(struct acpi_pci_root), GFP_KERNEL);
 	if (!root)
@@ -476,6 +442,30 @@ static int acpi_pci_root_add(struct acpi_device *device,
 	flags = base_flags = OSC_PCI_SEGMENT_GROUPS_SUPPORT;
 	acpi_pci_osc_support(root, flags);
 
+	/*
+	 * TBD: Need PCI interface for enumeration/configuration of roots.
+	 */
+
+	mutex_lock(&acpi_pci_root_lock);
+	list_add_tail(&root->node, &acpi_pci_roots);
+	mutex_unlock(&acpi_pci_root_lock);
+
+	/*
+	 * Scan the Root Bridge
+	 * --------------------
+	 * Must do this prior to any attempt to bind the root device, as the
+	 * PCI namespace does not get created until this call is made (and
+	 * thus the root bridge's pci_dev does not exist).
+	 */
+	root->bus = pci_acpi_scan_root(root);
+	if (!root->bus) {
+		printk(KERN_ERR PREFIX
+			    "Bus %04x:%02x not present in PCI namespace\n",
+			    root->segment, (unsigned int)root->secondary.start);
+		result = -ENODEV;
+		goto out_del_root;
+	}
+
 	/* Indicate support for various _OSC capabilities. */
 	if (pci_ext_cfg_avail())
 		flags |= OSC_EXT_PCI_CONFIG_SUPPORT;
@@ -494,6 +484,7 @@ static int acpi_pci_root_add(struct acpi_device *device,
 			flags = base_flags;
 		}
 	}
+
 	if (!pcie_ports_disabled
 	    && (flags & ACPI_PCIE_REQ_SUPPORT) == ACPI_PCIE_REQ_SUPPORT) {
 		flags = OSC_PCI_EXPRESS_CAP_STRUCTURE_CONTROL
@@ -514,54 +505,28 @@ static int acpi_pci_root_add(struct acpi_device *device,
 		status = acpi_pci_osc_control_set(device->handle, &flags,
 				       OSC_PCI_EXPRESS_CAP_STRUCTURE_CONTROL);
 		if (ACPI_SUCCESS(status)) {
-			is_osc_granted = true;
 			dev_info(&device->dev,
 				"ACPI _OSC control (0x%02x) granted\n", flags);
+			if (acpi_gbl_FADT.boot_flags & ACPI_FADT_NO_ASPM) {
+				/*
+				 * We have ASPM control, but the FADT indicates
+				 * that it's unsupported. Clear it.
+				 */
+				pcie_clear_aspm(root->bus);
+			}
 		} else {
-			is_osc_granted = false;
 			dev_info(&device->dev,
 				"ACPI _OSC request failed (%s), "
 				"returned control mask: 0x%02x\n",
 				acpi_format_exception(status), flags);
+			pr_info("ACPI _OSC control for PCIe not granted, "
+				"disabling ASPM\n");
+			pcie_no_aspm();
 		}
 	} else {
 		dev_info(&device->dev,
-			"Unable to request _OSC control "
-			"(_OSC support mask: 0x%02x)\n", flags);
-	}
-
-	/*
-	 * TBD: Need PCI interface for enumeration/configuration of roots.
-	 */
-
-	mutex_lock(&acpi_pci_root_lock);
-	list_add_tail(&root->node, &acpi_pci_roots);
-	mutex_unlock(&acpi_pci_root_lock);
-
-	/*
-	 * Scan the Root Bridge
-	 * --------------------
-	 * Must do this prior to any attempt to bind the root device, as the
-	 * PCI namespace does not get created until this call is made (and 
-	 * thus the root bridge's pci_dev does not exist).
-	 */
-	root->bus = pci_acpi_scan_root(root);
-	if (!root->bus) {
-		printk(KERN_ERR PREFIX
-			    "Bus %04x:%02x not present in PCI namespace\n",
-			    root->segment, (unsigned int)root->secondary.start);
-		result = -ENODEV;
-		goto out_del_root;
-	}
-
-	/* ASPM setting */
-	if (is_osc_granted) {
-		if (acpi_gbl_FADT.boot_flags & ACPI_FADT_NO_ASPM)
-			pcie_clear_aspm(root->bus);
-	} else {
-		pr_info("ACPI _OSC control for PCIe not granted, "
-			"disabling ASPM\n");
-		pcie_no_aspm();
+			 "Unable to request _OSC control "
+			 "(_OSC support mask: 0x%02x)\n", flags);
 	}
 
 	pci_acpi_add_bus_pm_notifier(device, root->bus);
@@ -573,12 +538,6 @@ static int acpi_pci_root_add(struct acpi_device *device,
 		pci_assign_unassigned_bus_resources(root->bus);
 	}
 
-	mutex_lock(&acpi_pci_root_lock);
-	list_for_each_entry(driver, &acpi_pci_drivers, node)
-		if (driver->add)
-			driver->add(root);
-	mutex_unlock(&acpi_pci_root_lock);
-
 	/* need to after hot-added ioapic is registered */
 	if (system_state != SYSTEM_BOOTING)
 		pci_enable_bridges(root->bus);
@@ -599,16 +558,9 @@ end:
 static void acpi_pci_root_remove(struct acpi_device *device)
 {
 	struct acpi_pci_root *root = acpi_driver_data(device);
-	struct acpi_pci_driver *driver;
 
 	pci_stop_root_bus(root->bus);
 
-	mutex_lock(&acpi_pci_root_lock);
-	list_for_each_entry_reverse(driver, &acpi_pci_drivers, node)
-		if (driver->remove)
-			driver->remove(root);
-	mutex_unlock(&acpi_pci_root_lock);
-
 	device_set_run_wake(root->bus->bridge, false);
 	pci_acpi_remove_bus_pm_notifier(device);
 
@@ -646,6 +598,7 @@ static void handle_root_bridge_insertion(acpi_handle handle)
 
 static void handle_root_bridge_removal(struct acpi_device *device)
 {
+	acpi_status status;
 	struct acpi_eject_event *ej_event;
 
 	ej_event = kmalloc(sizeof(*ej_event), GFP_KERNEL);
@@ -661,7 +614,9 @@ static void handle_root_bridge_removal(struct acpi_device *device)
 	ej_event->device = device;
 	ej_event->event = ACPI_NOTIFY_EJECT_REQUEST;
 
-	acpi_bus_hot_remove_device(ej_event);
+	status = acpi_os_hotplug_execute(acpi_bus_hot_remove_device, ej_event);
+	if (ACPI_FAILURE(status))
+		kfree(ej_event);
 }
 
 static void _handle_hotplug_event_root(struct work_struct *work)
@@ -676,8 +631,9 @@ static void _handle_hotplug_event_root(struct work_struct *work)
 	handle = hp_work->handle;
 	type = hp_work->type;
 
-	root = acpi_pci_find_root(handle);
+	acpi_scan_lock_acquire();
 
+	root = acpi_pci_find_root(handle);
 	acpi_get_name(handle, ACPI_FULL_PATHNAME, &buffer);
 
 	switch (type) {
@@ -711,6 +667,7 @@ static void _handle_hotplug_event_root(struct work_struct *work)
 		break;
 	}
 
+	acpi_scan_lock_release();
 	kfree(hp_work); /* allocated in handle_hotplug_event_bridge */
 	kfree(buffer.pointer);
 }
diff --git a/drivers/acpi/pci_slot.c b/drivers/acpi/pci_slot.c
index cd1434eb1de..033d1179bdb 100644
--- a/drivers/acpi/pci_slot.c
+++ b/drivers/acpi/pci_slot.c
@@ -9,6 +9,9 @@
  *  Copyright (C) 2007-2008 Hewlett-Packard Development Company, L.P.
  *  	Alex Chiang <achiang@hp.com>
  *
+ *  Copyright (C) 2013 Huawei Tech. Co., Ltd.
+ *	Jiang Liu <jiang.liu@huawei.com>
+ *
  *  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.
@@ -28,10 +31,9 @@
 #include <linux/init.h>
 #include <linux/slab.h>
 #include <linux/types.h>
+#include <linux/list.h>
 #include <linux/pci.h>
 #include <linux/acpi.h>
-#include <acpi/acpi_bus.h>
-#include <acpi/acpi_drivers.h>
 #include <linux/dmi.h>
 
 static bool debug;
@@ -61,20 +63,12 @@ ACPI_MODULE_NAME("pci_slot");
 #define SLOT_NAME_SIZE 21		/* Inspired by #define in acpiphp.h */
 
 struct acpi_pci_slot {
-	acpi_handle root_handle;	/* handle of the root bridge */
 	struct pci_slot *pci_slot;	/* corresponding pci_slot */
 	struct list_head list;		/* node in the list of slots */
 };
 
-static int acpi_pci_slot_add(struct acpi_pci_root *root);
-static void acpi_pci_slot_remove(struct acpi_pci_root *root);
-
 static LIST_HEAD(slot_list);
 static DEFINE_MUTEX(slot_list_lock);
-static struct acpi_pci_driver acpi_pci_slot_driver = {
-	.add = acpi_pci_slot_add,
-	.remove = acpi_pci_slot_remove,
-};
 
 static int
 check_slot(acpi_handle handle, unsigned long long *sun)
@@ -113,21 +107,8 @@ out:
 	return device;
 }
 
-struct callback_args {
-	acpi_walk_callback	user_function;	/* only for walk_p2p_bridge */
-	struct pci_bus		*pci_bus;
-	acpi_handle		root_handle;
-};
-
 /*
- * register_slot
- *
- * Called once for each SxFy object in the namespace. Don't worry about
- * calling pci_create_slot multiple times for the same pci_bus:device,
- * since each subsequent call simply bumps the refcount on the pci_slot.
- *
- * The number of calls to pci_destroy_slot from unregister_slot is
- * symmetrical.
+ * Check whether handle has an associated slot and create PCI slot if it has.
  */
 static acpi_status
 register_slot(acpi_handle handle, u32 lvl, void *context, void **rv)
@@ -137,13 +118,22 @@ register_slot(acpi_handle handle, u32 lvl, void *context, void **rv)
 	char name[SLOT_NAME_SIZE];
 	struct acpi_pci_slot *slot;
 	struct pci_slot *pci_slot;
-	struct callback_args *parent_context = context;
-	struct pci_bus *pci_bus = parent_context->pci_bus;
+	struct pci_bus *pci_bus = context;
 
 	device = check_slot(handle, &sun);
 	if (device < 0)
 		return AE_OK;
 
+	/*
+	 * There may be multiple PCI functions associated with the same slot.
+	 * Check whether PCI slot has already been created for this PCI device.
+	 */
+	list_for_each_entry(slot, &slot_list, list) {
+		pci_slot = slot->pci_slot;
+		if (pci_slot->bus == pci_bus && pci_slot->number == device)
+			return AE_OK;
+	}
+
 	slot = kmalloc(sizeof(*slot), GFP_KERNEL);
 	if (!slot) {
 		err("%s: cannot allocate memory\n", __func__);
@@ -158,12 +148,8 @@ register_slot(acpi_handle handle, u32 lvl, void *context, void **rv)
 		return AE_OK;
 	}
 
-	slot->root_handle = parent_context->root_handle;
 	slot->pci_slot = pci_slot;
-	INIT_LIST_HEAD(&slot->list);
-	mutex_lock(&slot_list_lock);
 	list_add(&slot->list, &slot_list);
-	mutex_unlock(&slot_list_lock);
 
 	get_device(&pci_bus->dev);
 
@@ -173,131 +159,24 @@ register_slot(acpi_handle handle, u32 lvl, void *context, void **rv)
 	return AE_OK;
 }
 
-/*
- * walk_p2p_bridge - discover and walk p2p bridges
- * @handle: points to an acpi_pci_root
- * @context: p2p_bridge_context pointer
- *
- * Note that when we call ourselves recursively, we pass a different
- * value of pci_bus in the child_context.
- */
-static acpi_status
-walk_p2p_bridge(acpi_handle handle, u32 lvl, void *context, void **rv)
-{
-	int device, function;
-	unsigned long long adr;
-	acpi_status status;
-	acpi_handle dummy_handle;
-	acpi_walk_callback user_function;
-
-	struct pci_dev *dev;
-	struct pci_bus *pci_bus;
-	struct callback_args child_context;
-	struct callback_args *parent_context = context;
-
-	pci_bus = parent_context->pci_bus;
-	user_function = parent_context->user_function;
-
-	status = acpi_get_handle(handle, "_ADR", &dummy_handle);
-	if (ACPI_FAILURE(status))
-		return AE_OK;
-
-	status = acpi_evaluate_integer(handle, "_ADR", NULL, &adr);
-	if (ACPI_FAILURE(status))
-		return AE_OK;
-
-	device = (adr >> 16) & 0xffff;
-	function = adr & 0xffff;
-
-	dev = pci_get_slot(pci_bus, PCI_DEVFN(device, function));
-	if (!dev || !dev->subordinate)
-		goto out;
-
-	child_context.pci_bus = dev->subordinate;
-	child_context.user_function = user_function;
-	child_context.root_handle = parent_context->root_handle;
-
-	dbg("p2p bridge walk, pci_bus = %x\n", dev->subordinate->number);
-	status = acpi_walk_namespace(ACPI_TYPE_DEVICE, handle, (u32)1,
-				     user_function, NULL, &child_context, NULL);
-	if (ACPI_FAILURE(status))
-		goto out;
-
-	status = acpi_walk_namespace(ACPI_TYPE_DEVICE, handle, (u32)1,
-				     walk_p2p_bridge, NULL, &child_context, NULL);
-out:
-	pci_dev_put(dev);
-	return AE_OK;
-}
-
-/*
- * walk_root_bridge - generic root bridge walker
- * @root: poiner of an acpi_pci_root
- * @user_function: user callback for slot objects
- *
- * Call user_function for all objects underneath this root bridge.
- * Walk p2p bridges underneath us and call user_function on those too.
- */
-static int
-walk_root_bridge(struct acpi_pci_root *root, acpi_walk_callback user_function)
-{
-	acpi_status status;
-	acpi_handle handle = root->device->handle;
-	struct pci_bus *pci_bus = root->bus;
-	struct callback_args context;
-
-	context.pci_bus = pci_bus;
-	context.user_function = user_function;
-	context.root_handle = handle;
-
-	dbg("root bridge walk, pci_bus = %x\n", pci_bus->number);
-	status = acpi_walk_namespace(ACPI_TYPE_DEVICE, handle, (u32)1,
-				     user_function, NULL, &context, NULL);
-	if (ACPI_FAILURE(status))
-		return status;
-
-	status = acpi_walk_namespace(ACPI_TYPE_DEVICE, handle, (u32)1,
-				     walk_p2p_bridge, NULL, &context, NULL);
-	if (ACPI_FAILURE(status))
-		err("%s: walk_p2p_bridge failure - %d\n", __func__, status);
-
-	return status;
-}
-
-/*
- * acpi_pci_slot_add
- * @handle: points to an acpi_pci_root
- */
-static int
-acpi_pci_slot_add(struct acpi_pci_root *root)
+void acpi_pci_slot_enumerate(struct pci_bus *bus, acpi_handle handle)
 {
-	acpi_status status;
-
-	status = walk_root_bridge(root, register_slot);
-	if (ACPI_FAILURE(status))
-		err("%s: register_slot failure - %d\n", __func__, status);
-
-	return status;
+	mutex_lock(&slot_list_lock);
+	acpi_walk_namespace(ACPI_TYPE_DEVICE, handle, 1,
+			    register_slot, NULL, bus, NULL);
+	mutex_unlock(&slot_list_lock);
 }
 
-/*
- * acpi_pci_slot_remove
- * @handle: points to an acpi_pci_root
- */
-static void
-acpi_pci_slot_remove(struct acpi_pci_root *root)
+void acpi_pci_slot_remove(struct pci_bus *bus)
 {
 	struct acpi_pci_slot *slot, *tmp;
-	struct pci_bus *pbus;
-	acpi_handle handle = root->device->handle;
 
 	mutex_lock(&slot_list_lock);
 	list_for_each_entry_safe(slot, tmp, &slot_list, list) {
-		if (slot->root_handle == handle) {
+		if (slot->pci_slot->bus == bus) {
 			list_del(&slot->list);
-			pbus = slot->pci_slot->bus;
 			pci_destroy_slot(slot->pci_slot);
-			put_device(&pbus->dev);
+			put_device(&bus->dev);
 			kfree(slot);
 		}
 	}
@@ -332,5 +211,4 @@ static struct dmi_system_id acpi_pci_slot_dmi_table[] __initdata = {
 void __init acpi_pci_slot_init(void)
 {
 	dmi_check_system(acpi_pci_slot_dmi_table);
-	acpi_pci_register_driver(&acpi_pci_slot_driver);
 }
diff --git a/drivers/acpi/processor_idle.c b/drivers/acpi/processor_idle.c
index fc95308e9a1..ee255c60bda 100644
--- a/drivers/acpi/processor_idle.c
+++ b/drivers/acpi/processor_idle.c
@@ -66,7 +66,8 @@ module_param(latency_factor, uint, 0644);
 
 static DEFINE_PER_CPU(struct cpuidle_device *, acpi_cpuidle_device);
 
-static struct acpi_processor_cx *acpi_cstate[CPUIDLE_STATE_MAX];
+static DEFINE_PER_CPU(struct acpi_processor_cx * [CPUIDLE_STATE_MAX],
+								acpi_cstate);
 
 static int disabled_by_idle_boot_param(void)
 {
@@ -722,7 +723,7 @@ static int acpi_idle_enter_c1(struct cpuidle_device *dev,
 		struct cpuidle_driver *drv, int index)
 {
 	struct acpi_processor *pr;
-	struct acpi_processor_cx *cx = acpi_cstate[index];
+	struct acpi_processor_cx *cx = per_cpu(acpi_cstate[index], dev->cpu);
 
 	pr = __this_cpu_read(processors);
 
@@ -745,7 +746,7 @@ static int acpi_idle_enter_c1(struct cpuidle_device *dev,
  */
 static int acpi_idle_play_dead(struct cpuidle_device *dev, int index)
 {
-	struct acpi_processor_cx *cx = acpi_cstate[index];
+	struct acpi_processor_cx *cx = per_cpu(acpi_cstate[index], dev->cpu);
 
 	ACPI_FLUSH_CPU_CACHE();
 
@@ -775,7 +776,7 @@ static int acpi_idle_enter_simple(struct cpuidle_device *dev,
 		struct cpuidle_driver *drv, int index)
 {
 	struct acpi_processor *pr;
-	struct acpi_processor_cx *cx = acpi_cstate[index];
+	struct acpi_processor_cx *cx = per_cpu(acpi_cstate[index], dev->cpu);
 
 	pr = __this_cpu_read(processors);
 
@@ -833,7 +834,7 @@ static int acpi_idle_enter_bm(struct cpuidle_device *dev,
 		struct cpuidle_driver *drv, int index)
 {
 	struct acpi_processor *pr;
-	struct acpi_processor_cx *cx = acpi_cstate[index];
+	struct acpi_processor_cx *cx = per_cpu(acpi_cstate[index], dev->cpu);
 
 	pr = __this_cpu_read(processors);
 
@@ -960,7 +961,7 @@ static int acpi_processor_setup_cpuidle_cx(struct acpi_processor *pr,
 		    !(acpi_gbl_FADT.flags & ACPI_FADT_C2_MP_SUPPORTED))
 			continue;
 #endif
-		acpi_cstate[count] = cx;
+		per_cpu(acpi_cstate[count], dev->cpu) = cx;
 
 		count++;
 		if (count == CPUIDLE_STATE_MAX)
diff --git a/drivers/acpi/scan.c b/drivers/acpi/scan.c
index 5e7e991717d..f54d1985e59 100644
--- a/drivers/acpi/scan.c
+++ b/drivers/acpi/scan.c
@@ -1790,7 +1790,6 @@ int __init acpi_scan_init(void)
 	acpi_platform_init();
 	acpi_csrt_init();
 	acpi_container_init();
-	acpi_pci_slot_init();
 
 	mutex_lock(&acpi_scan_lock);
 	/*
diff --git a/drivers/acpi/sleep.c b/drivers/acpi/sleep.c
index 24213033fba..9c1a435d10e 100644
--- a/drivers/acpi/sleep.c
+++ b/drivers/acpi/sleep.c
@@ -193,6 +193,14 @@ static struct dmi_system_id __initdata acpisleep_dmi_table[] = {
 	},
 	{
 	.callback = init_nvs_nosave,
+	.ident = "Sony Vaio VGN-FW21M",
+	.matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "Sony Corporation"),
+		DMI_MATCH(DMI_PRODUCT_NAME, "VGN-FW21M"),
+		},
+	},
+	{
+	.callback = init_nvs_nosave,
 	.ident = "Sony Vaio VPCEB17FX",
 	.matches = {
 		DMI_MATCH(DMI_SYS_VENDOR, "Sony Corporation"),
diff --git a/drivers/ata/ahci.c b/drivers/ata/ahci.c
index 6a67b07de49..251e57d3894 100644
--- a/drivers/ata/ahci.c
+++ b/drivers/ata/ahci.c
@@ -415,17 +415,17 @@ static const struct pci_device_id ahci_pci_tbl[] = {
 	/* Marvell */
 	{ PCI_VDEVICE(MARVELL, 0x6145), board_ahci_mv },	/* 6145 */
 	{ PCI_VDEVICE(MARVELL, 0x6121), board_ahci_mv },	/* 6121 */
-	{ PCI_DEVICE(0x1b4b, 0x9123),
+	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL_EXT, 0x9123),
 	  .class = PCI_CLASS_STORAGE_SATA_AHCI,
 	  .class_mask = 0xffffff,
 	  .driver_data = board_ahci_yes_fbs },			/* 88se9128 */
-	{ PCI_DEVICE(0x1b4b, 0x9125),
+	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL_EXT, 0x9125),
 	  .driver_data = board_ahci_yes_fbs },			/* 88se9125 */
-	{ PCI_DEVICE(0x1b4b, 0x917a),
+	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL_EXT, 0x917a),
 	  .driver_data = board_ahci_yes_fbs },			/* 88se9172 */
-	{ PCI_DEVICE(0x1b4b, 0x9192),
+	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL_EXT, 0x9192),
 	  .driver_data = board_ahci_yes_fbs },			/* 88se9172 on some Gigabyte */
-	{ PCI_DEVICE(0x1b4b, 0x91a3),
+	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL_EXT, 0x91a3),
 	  .driver_data = board_ahci_yes_fbs },
 
 	/* Promise */
diff --git a/drivers/ata/ata_piix.c b/drivers/ata/ata_piix.c
index ffdd32d2260..2f48123d74c 100644
--- a/drivers/ata/ata_piix.c
+++ b/drivers/ata/ata_piix.c
@@ -150,6 +150,7 @@ enum piix_controller_ids {
 	tolapai_sata,
 	piix_pata_vmw,			/* PIIX4 for VMware, spurious DMA_ERR */
 	ich8_sata_snb,
+	ich8_2port_sata_snb,
 };
 
 struct piix_map_db {
@@ -304,7 +305,7 @@ static const struct pci_device_id piix_pci_tbl[] = {
 	/* SATA Controller IDE (Lynx Point) */
 	{ 0x8086, 0x8c01, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich8_sata_snb },
 	/* SATA Controller IDE (Lynx Point) */
-	{ 0x8086, 0x8c08, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich8_2port_sata },
+	{ 0x8086, 0x8c08, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich8_2port_sata_snb },
 	/* SATA Controller IDE (Lynx Point) */
 	{ 0x8086, 0x8c09, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich8_2port_sata },
 	/* SATA Controller IDE (Lynx Point-LP) */
@@ -439,6 +440,7 @@ static const struct piix_map_db *piix_map_db_table[] = {
 	[ich8m_apple_sata]	= &ich8m_apple_map_db,
 	[tolapai_sata]		= &tolapai_map_db,
 	[ich8_sata_snb]		= &ich8_map_db,
+	[ich8_2port_sata_snb]	= &ich8_2port_map_db,
 };
 
 static struct pci_bits piix_enable_bits[] = {
@@ -1242,6 +1244,16 @@ static struct ata_port_info piix_port_info[] = {
 		.udma_mask	= ATA_UDMA6,
 		.port_ops	= &piix_sata_ops,
 	},
+
+	[ich8_2port_sata_snb] =
+	{
+		.flags		= PIIX_SATA_FLAGS | PIIX_FLAG_SIDPR
+					| PIIX_FLAG_PIO16,
+		.pio_mask	= ATA_PIO4,
+		.mwdma_mask	= ATA_MWDMA2,
+		.udma_mask	= ATA_UDMA6,
+		.port_ops	= &piix_sata_ops,
+	},
 };
 
 #define AHCI_PCI_BAR 5
diff --git a/drivers/ata/libata-core.c b/drivers/ata/libata-core.c
index 497adea1f0d..63c743baf92 100644
--- a/drivers/ata/libata-core.c
+++ b/drivers/ata/libata-core.c
@@ -2329,7 +2329,7 @@ int ata_dev_configure(struct ata_device *dev)
 		 * from SATA Settings page of Identify Device Data Log.
 		 */
 		if (ata_id_has_devslp(dev->id)) {
-			u8 sata_setting[ATA_SECT_SIZE];
+			u8 *sata_setting = ap->sector_buf;
 			int i, j;
 
 			dev->flags |= ATA_DFLAG_DEVSLP;
@@ -2439,6 +2439,9 @@ int ata_dev_configure(struct ata_device *dev)
 		dev->max_sectors = min_t(unsigned int, ATA_MAX_SECTORS_128,
 					 dev->max_sectors);
 
+	if (dev->horkage & ATA_HORKAGE_MAX_SEC_LBA48)
+		dev->max_sectors = ATA_MAX_SECTORS_LBA48;
+
 	if (ap->ops->dev_config)
 		ap->ops->dev_config(dev);
 
@@ -4100,6 +4103,7 @@ static const struct ata_blacklist_entry ata_device_blacklist [] = {
 	/* Weird ATAPI devices */
 	{ "TORiSAN DVD-ROM DRD-N216", NULL,	ATA_HORKAGE_MAX_SEC_128 },
 	{ "QUANTUM DAT    DAT72-000", NULL,	ATA_HORKAGE_ATAPI_MOD16_DMA },
+	{ "Slimtype DVD A  DS8A8SH", NULL,	ATA_HORKAGE_MAX_SEC_LBA48 },
 
 	/* Devices we expect to fail diagnostics */
 
diff --git a/drivers/ata/libata-scsi.c b/drivers/ata/libata-scsi.c
index 318b4135818..ff44787e5a4 100644
--- a/drivers/ata/libata-scsi.c
+++ b/drivers/ata/libata-scsi.c
@@ -532,8 +532,8 @@ int ata_cmd_ioctl(struct scsi_device *scsidev, void __user *arg)
 			struct scsi_sense_hdr sshdr;
 			scsi_normalize_sense(sensebuf, SCSI_SENSE_BUFFERSIZE,
 					     &sshdr);
-			if (sshdr.sense_key == 0 &&
-			    sshdr.asc == 0 && sshdr.ascq == 0)
+			if (sshdr.sense_key == RECOVERED_ERROR &&
+			    sshdr.asc == 0 && sshdr.ascq == 0x1d)
 				cmd_result &= ~SAM_STAT_CHECK_CONDITION;
 		}
 
@@ -618,8 +618,8 @@ int ata_task_ioctl(struct scsi_device *scsidev, void __user *arg)
 			struct scsi_sense_hdr sshdr;
 			scsi_normalize_sense(sensebuf, SCSI_SENSE_BUFFERSIZE,
 						&sshdr);
-			if (sshdr.sense_key == 0 &&
-				sshdr.asc == 0 && sshdr.ascq == 0)
+			if (sshdr.sense_key == RECOVERED_ERROR &&
+			    sshdr.asc == 0 && sshdr.ascq == 0x1d)
 				cmd_result &= ~SAM_STAT_CHECK_CONDITION;
 		}
 
diff --git a/drivers/ata/pata_pcmcia.c b/drivers/ata/pata_pcmcia.c
index 958238dda8f..40254f4df58 100644
--- a/drivers/ata/pata_pcmcia.c
+++ b/drivers/ata/pata_pcmcia.c
@@ -387,21 +387,9 @@ static struct pcmcia_driver pcmcia_driver = {
 	.probe		= pcmcia_init_one,
 	.remove		= pcmcia_remove_one,
 };
-
-static int __init pcmcia_init(void)
-{
-	return pcmcia_register_driver(&pcmcia_driver);
-}
-
-static void __exit pcmcia_exit(void)
-{
-	pcmcia_unregister_driver(&pcmcia_driver);
-}
+module_pcmcia_driver(pcmcia_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for PCMCIA ATA");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(DRV_VERSION);
-
-module_init(pcmcia_init);
-module_exit(pcmcia_exit);
diff --git a/drivers/ata/pata_pdc2027x.c b/drivers/ata/pata_pdc2027x.c
index 3f94a886bb3..c76e65927b0 100644
--- a/drivers/ata/pata_pdc2027x.c
+++ b/drivers/ata/pata_pdc2027x.c
@@ -63,7 +63,9 @@ enum {
 };
 
 static int pdc2027x_init_one(struct pci_dev *pdev, const struct pci_device_id *ent);
+#ifdef CONFIG_PM
 static int pdc2027x_reinit_one(struct pci_dev *pdev);
+#endif
 static int pdc2027x_prereset(struct ata_link *link, unsigned long deadline);
 static void pdc2027x_set_piomode(struct ata_port *ap, struct ata_device *adev);
 static void pdc2027x_set_dmamode(struct ata_port *ap, struct ata_device *adev);
diff --git a/drivers/ata/sata_fsl.c b/drivers/ata/sata_fsl.c
index 608f82fed63..dfc6fd0ac11 100644
--- a/drivers/ata/sata_fsl.c
+++ b/drivers/ata/sata_fsl.c
@@ -311,7 +311,7 @@ static void fsl_sata_set_irq_coalescing(struct ata_host *host,
 	intr_coalescing_ticks = ticks;
 	spin_unlock(&host->lock);
 
-	DPRINTK("intrrupt coalescing, count = 0x%x, ticks = %x\n",
+	DPRINTK("interrupt coalescing, count = 0x%x, ticks = %x\n",
 			intr_coalescing_count, intr_coalescing_ticks);
 	DPRINTK("ICC register status: (hcr base: 0x%x) = 0x%x\n",
 			hcr_base, ioread32(hcr_base + ICC));
diff --git a/drivers/base/base.h b/drivers/base/base.h
index 6ee17bb391a..b8bdfe61daa 100644
--- a/drivers/base/base.h
+++ b/drivers/base/base.h
@@ -101,6 +101,8 @@ static inline int hypervisor_init(void) { return 0; }
 extern int platform_bus_init(void);
 extern void cpu_dev_init(void);
 
+struct kobject *virtual_device_parent(struct device *dev);
+
 extern int bus_add_device(struct device *dev);
 extern void bus_probe_device(struct device *dev);
 extern void bus_remove_device(struct device *dev);
diff --git a/drivers/base/bus.c b/drivers/base/bus.c
index 519865b53f7..1a68f947ded 100644
--- a/drivers/base/bus.c
+++ b/drivers/base/bus.c
@@ -898,18 +898,18 @@ static ssize_t bus_uevent_store(struct bus_type *bus,
 static BUS_ATTR(uevent, S_IWUSR, NULL, bus_uevent_store);
 
 /**
- * __bus_register - register a driver-core subsystem
+ * bus_register - register a driver-core subsystem
  * @bus: bus to register
- * @key: lockdep class key
  *
  * Once we have that, we register the bus with the kobject
  * infrastructure, then register the children subsystems it has:
  * the devices and drivers that belong to the subsystem.
  */
-int __bus_register(struct bus_type *bus, struct lock_class_key *key)
+int bus_register(struct bus_type *bus)
 {
 	int retval;
 	struct subsys_private *priv;
+	struct lock_class_key *key = &bus->lock_key;
 
 	priv = kzalloc(sizeof(struct subsys_private), GFP_KERNEL);
 	if (!priv)
@@ -981,7 +981,7 @@ out:
 	bus->p = NULL;
 	return retval;
 }
-EXPORT_SYMBOL_GPL(__bus_register);
+EXPORT_SYMBOL_GPL(bus_register);
 
 /**
  * bus_unregister - remove a bus from the system
@@ -1205,26 +1205,10 @@ static void system_root_device_release(struct device *dev)
 {
 	kfree(dev);
 }
-/**
- * subsys_system_register - register a subsystem at /sys/devices/system/
- * @subsys: system subsystem
- * @groups: default attributes for the root device
- *
- * All 'system' subsystems have a /sys/devices/system/<name> root device
- * with the name of the subsystem. The root device can carry subsystem-
- * wide attributes. All registered devices are below this single root
- * device and are named after the subsystem with a simple enumeration
- * number appended. The registered devices are not explicitely named;
- * only 'id' in the device needs to be set.
- *
- * Do not use this interface for anything new, it exists for compatibility
- * with bad ideas only. New subsystems should use plain subsystems; and
- * add the subsystem-wide attributes should be added to the subsystem
- * directory itself and not some create fake root-device placed in
- * /sys/devices/system/<name>.
- */
-int subsys_system_register(struct bus_type *subsys,
-			   const struct attribute_group **groups)
+
+static int subsys_register(struct bus_type *subsys,
+			   const struct attribute_group **groups,
+			   struct kobject *parent_of_root)
 {
 	struct device *dev;
 	int err;
@@ -1243,7 +1227,7 @@ int subsys_system_register(struct bus_type *subsys,
 	if (err < 0)
 		goto err_name;
 
-	dev->kobj.parent = &system_kset->kobj;
+	dev->kobj.parent = parent_of_root;
 	dev->groups = groups;
 	dev->release = system_root_device_release;
 
@@ -1263,8 +1247,55 @@ err_dev:
 	bus_unregister(subsys);
 	return err;
 }
+
+/**
+ * subsys_system_register - register a subsystem at /sys/devices/system/
+ * @subsys: system subsystem
+ * @groups: default attributes for the root device
+ *
+ * All 'system' subsystems have a /sys/devices/system/<name> root device
+ * with the name of the subsystem. The root device can carry subsystem-
+ * wide attributes. All registered devices are below this single root
+ * device and are named after the subsystem with a simple enumeration
+ * number appended. The registered devices are not explicitely named;
+ * only 'id' in the device needs to be set.
+ *
+ * Do not use this interface for anything new, it exists for compatibility
+ * with bad ideas only. New subsystems should use plain subsystems; and
+ * add the subsystem-wide attributes should be added to the subsystem
+ * directory itself and not some create fake root-device placed in
+ * /sys/devices/system/<name>.
+ */
+int subsys_system_register(struct bus_type *subsys,
+			   const struct attribute_group **groups)
+{
+	return subsys_register(subsys, groups, &system_kset->kobj);
+}
 EXPORT_SYMBOL_GPL(subsys_system_register);
 
+/**
+ * subsys_virtual_register - register a subsystem at /sys/devices/virtual/
+ * @subsys: virtual subsystem
+ * @groups: default attributes for the root device
+ *
+ * All 'virtual' subsystems have a /sys/devices/system/<name> root device
+ * with the name of the subystem.  The root device can carry subsystem-wide
+ * attributes.  All registered devices are below this single root device.
+ * There's no restriction on device naming.  This is for kernel software
+ * constructs which need sysfs interface.
+ */
+int subsys_virtual_register(struct bus_type *subsys,
+			    const struct attribute_group **groups)
+{
+	struct kobject *virtual_dir;
+
+	virtual_dir = virtual_device_parent(NULL);
+	if (!virtual_dir)
+		return -ENOMEM;
+
+	return subsys_register(subsys, groups, virtual_dir);
+}
+
 int __init buses_init(void)
 {
 	bus_kset = kset_create_and_add("bus", &bus_uevent_ops, NULL);
diff --git a/drivers/base/core.c b/drivers/base/core.c
index 56536f4b0f6..01631243757 100644
--- a/drivers/base/core.c
+++ b/drivers/base/core.c
@@ -283,15 +283,21 @@ static int dev_uevent(struct kset *kset, struct kobject *kobj,
 		const char *tmp;
 		const char *name;
 		umode_t mode = 0;
+		kuid_t uid = GLOBAL_ROOT_UID;
+		kgid_t gid = GLOBAL_ROOT_GID;
 
 		add_uevent_var(env, "MAJOR=%u", MAJOR(dev->devt));
 		add_uevent_var(env, "MINOR=%u", MINOR(dev->devt));
-		name = device_get_devnode(dev, &mode, &tmp);
+		name = device_get_devnode(dev, &mode, &uid, &gid, &tmp);
 		if (name) {
 			add_uevent_var(env, "DEVNAME=%s", name);
-			kfree(tmp);
 			if (mode)
 				add_uevent_var(env, "DEVMODE=%#o", mode & 0777);
+			if (!uid_eq(uid, GLOBAL_ROOT_UID))
+				add_uevent_var(env, "DEVUID=%u", from_kuid(&init_user_ns, uid));
+			if (!gid_eq(gid, GLOBAL_ROOT_GID))
+				add_uevent_var(env, "DEVGID=%u", from_kgid(&init_user_ns, gid));
+			kfree(tmp);
 		}
 	}
 
@@ -563,8 +569,15 @@ int device_create_file(struct device *dev,
 		       const struct device_attribute *attr)
 {
 	int error = 0;
-	if (dev)
+
+	if (dev) {
+		WARN(((attr->attr.mode & S_IWUGO) && !attr->store),
+				"Write permission without 'store'\n");
+		WARN(((attr->attr.mode & S_IRUGO) && !attr->show),
+				"Read permission without 'show'\n");
 		error = sysfs_create_file(&dev->kobj, &attr->attr);
+	}
+
 	return error;
 }
 
@@ -690,7 +703,7 @@ void device_initialize(struct device *dev)
 	set_dev_node(dev, -1);
 }
 
-static struct kobject *virtual_device_parent(struct device *dev)
+struct kobject *virtual_device_parent(struct device *dev)
 {
 	static struct kobject *virtual_dir = NULL;
 
@@ -1274,6 +1287,8 @@ static struct device *next_device(struct klist_iter *i)
  * device_get_devnode - path of device node file
  * @dev: device
  * @mode: returned file access mode
+ * @uid: returned file owner
+ * @gid: returned file group
  * @tmp: possibly allocated string
  *
  * Return the relative path of a possible device node.
@@ -1282,7 +1297,8 @@ static struct device *next_device(struct klist_iter *i)
  * freed by the caller.
  */
 const char *device_get_devnode(struct device *dev,
-			       umode_t *mode, const char **tmp)
+			       umode_t *mode, kuid_t *uid, kgid_t *gid,
+			       const char **tmp)
 {
 	char *s;
 
@@ -1290,7 +1306,7 @@ const char *device_get_devnode(struct device *dev,
 
 	/* the device type may provide a specific name */
 	if (dev->type && dev->type->devnode)
-		*tmp = dev->type->devnode(dev, mode);
+		*tmp = dev->type->devnode(dev, mode, uid, gid);
 	if (*tmp)
 		return *tmp;
 
diff --git a/drivers/base/cpu.c b/drivers/base/cpu.c
index fb10728f637..3d48fc887ef 100644
--- a/drivers/base/cpu.c
+++ b/drivers/base/cpu.c
@@ -25,6 +25,15 @@ EXPORT_SYMBOL_GPL(cpu_subsys);
 static DEFINE_PER_CPU(struct device *, cpu_sys_devices);
 
 #ifdef CONFIG_HOTPLUG_CPU
+static void change_cpu_under_node(struct cpu *cpu,
+			unsigned int from_nid, unsigned int to_nid)
+{
+	int cpuid = cpu->dev.id;
+	unregister_cpu_under_node(cpuid, from_nid);
+	register_cpu_under_node(cpuid, to_nid);
+	cpu->node_id = to_nid;
+}
+
 static ssize_t show_online(struct device *dev,
 			   struct device_attribute *attr,
 			   char *buf)
@@ -39,17 +48,29 @@ static ssize_t __ref store_online(struct device *dev,
 				  const char *buf, size_t count)
 {
 	struct cpu *cpu = container_of(dev, struct cpu, dev);
+	int cpuid = cpu->dev.id;
+	int from_nid, to_nid;
 	ssize_t ret;
 
 	cpu_hotplug_driver_lock();
 	switch (buf[0]) {
 	case '0':
-		ret = cpu_down(cpu->dev.id);
+		ret = cpu_down(cpuid);
 		if (!ret)
 			kobject_uevent(&dev->kobj, KOBJ_OFFLINE);
 		break;
 	case '1':
-		ret = cpu_up(cpu->dev.id);
+		from_nid = cpu_to_node(cpuid);
+		ret = cpu_up(cpuid);
+
+		/*
+		 * When hot adding memory to memoryless node and enabling a cpu
+		 * on the node, node number of the cpu may internally change.
+		 */
+		to_nid = cpu_to_node(cpuid);
+		if (from_nid != to_nid)
+			change_cpu_under_node(cpu, from_nid, to_nid);
+
 		if (!ret)
 			kobject_uevent(&dev->kobj, KOBJ_ONLINE);
 		break;
@@ -132,6 +153,17 @@ static ssize_t show_crash_notes(struct device *dev, struct device_attribute *att
 	return rc;
 }
 static DEVICE_ATTR(crash_notes, 0400, show_crash_notes, NULL);
+
+static ssize_t show_crash_notes_size(struct device *dev,
+				     struct device_attribute *attr,
+				     char *buf)
+{
+	ssize_t rc;
+
+	rc = sprintf(buf, "%zu\n", sizeof(note_buf_t));
+	return rc;
+}
+static DEVICE_ATTR(crash_notes_size, 0400, show_crash_notes_size, NULL);
 #endif
 
 /*
@@ -259,6 +291,9 @@ int __cpuinit register_cpu(struct cpu *cpu, int num)
 #ifdef CONFIG_KEXEC
 	if (!error)
 		error = device_create_file(&cpu->dev, &dev_attr_crash_notes);
+	if (!error)
+		error = device_create_file(&cpu->dev,
+					   &dev_attr_crash_notes_size);
 #endif
 	return error;
 }
diff --git a/drivers/base/dd.c b/drivers/base/dd.c
index bb5645ea028..35fa3689891 100644
--- a/drivers/base/dd.c
+++ b/drivers/base/dd.c
@@ -380,7 +380,7 @@ int driver_probe_device(struct device_driver *drv, struct device *dev)
 
 	pm_runtime_barrier(dev);
 	ret = really_probe(dev, drv);
-	pm_runtime_idle(dev);
+	pm_request_idle(dev);
 
 	return ret;
 }
@@ -428,7 +428,7 @@ int device_attach(struct device *dev)
 		}
 	} else {
 		ret = bus_for_each_drv(dev->bus, NULL, dev, __device_attach);
-		pm_runtime_idle(dev);
+		pm_request_idle(dev);
 	}
 out_unlock:
 	device_unlock(dev);
@@ -499,7 +499,7 @@ static void __device_release_driver(struct device *dev)
 						     BUS_NOTIFY_UNBIND_DRIVER,
 						     dev);
 
-		pm_runtime_put_sync(dev);
+		pm_runtime_put(dev);
 
 		if (dev->bus && dev->bus->remove)
 			dev->bus->remove(dev);
diff --git a/drivers/base/devtmpfs.c b/drivers/base/devtmpfs.c
index 01fc5b07f95..7413d065906 100644
--- a/drivers/base/devtmpfs.c
+++ b/drivers/base/devtmpfs.c
@@ -24,6 +24,7 @@
 #include <linux/sched.h>
 #include <linux/slab.h>
 #include <linux/kthread.h>
+#include "base.h"
 
 static struct task_struct *thread;
 
@@ -41,6 +42,8 @@ static struct req {
 	int err;
 	const char *name;
 	umode_t mode;	/* 0 => delete */
+	kuid_t uid;
+	kgid_t gid;
 	struct device *dev;
 } *requests;
 
@@ -85,7 +88,9 @@ int devtmpfs_create_node(struct device *dev)
 		return 0;
 
 	req.mode = 0;
-	req.name = device_get_devnode(dev, &req.mode, &tmp);
+	req.uid = GLOBAL_ROOT_UID;
+	req.gid = GLOBAL_ROOT_GID;
+	req.name = device_get_devnode(dev, &req.mode, &req.uid, &req.gid, &tmp);
 	if (!req.name)
 		return -ENOMEM;
 
@@ -121,7 +126,7 @@ int devtmpfs_delete_node(struct device *dev)
 	if (!thread)
 		return 0;
 
-	req.name = device_get_devnode(dev, NULL, &tmp);
+	req.name = device_get_devnode(dev, NULL, NULL, NULL, &tmp);
 	if (!req.name)
 		return -ENOMEM;
 
@@ -187,7 +192,8 @@ static int create_path(const char *nodepath)
 	return err;
 }
 
-static int handle_create(const char *nodename, umode_t mode, struct device *dev)
+static int handle_create(const char *nodename, umode_t mode, kuid_t uid,
+			 kgid_t gid, struct device *dev)
 {
 	struct dentry *dentry;
 	struct path path;
@@ -201,14 +207,14 @@ static int handle_create(const char *nodename, umode_t mode, struct device *dev)
 	if (IS_ERR(dentry))
 		return PTR_ERR(dentry);
 
-	err = vfs_mknod(path.dentry->d_inode,
-			dentry, mode, dev->devt);
+	err = vfs_mknod(path.dentry->d_inode, dentry, mode, dev->devt);
 	if (!err) {
 		struct iattr newattrs;
 
-		/* fixup possibly umasked mode */
 		newattrs.ia_mode = mode;
-		newattrs.ia_valid = ATTR_MODE;
+		newattrs.ia_uid = uid;
+		newattrs.ia_gid = gid;
+		newattrs.ia_valid = ATTR_MODE|ATTR_UID|ATTR_GID;
 		mutex_lock(&dentry->d_inode->i_mutex);
 		notify_change(dentry, &newattrs);
 		mutex_unlock(&dentry->d_inode->i_mutex);
@@ -358,10 +364,11 @@ int devtmpfs_mount(const char *mntdir)
 
 static DECLARE_COMPLETION(setup_done);
 
-static int handle(const char *name, umode_t mode, struct device *dev)
+static int handle(const char *name, umode_t mode, kuid_t uid, kgid_t gid,
+		  struct device *dev)
 {
 	if (mode)
-		return handle_create(name, mode, dev);
+		return handle_create(name, mode, uid, gid, dev);
 	else
 		return handle_remove(name, dev);
 }
@@ -387,7 +394,8 @@ static int devtmpfsd(void *p)
 			spin_unlock(&req_lock);
 			while (req) {
 				struct req *next = req->next;
-				req->err = handle(req->name, req->mode, req->dev);
+				req->err = handle(req->name, req->mode,
+						  req->uid, req->gid, req->dev);
 				complete(&req->done);
 				req = next;
 			}
diff --git a/drivers/base/memory.c b/drivers/base/memory.c
index a51007b7903..14f8a6954da 100644
--- a/drivers/base/memory.c
+++ b/drivers/base/memory.c
@@ -93,16 +93,6 @@ int register_memory(struct memory_block *memory)
 	return error;
 }
 
-static void
-unregister_memory(struct memory_block *memory)
-{
-	BUG_ON(memory->dev.bus != &memory_subsys);
-
-	/* drop the ref. we got in remove_memory_block() */
-	kobject_put(&memory->dev.kobj);
-	device_unregister(&memory->dev);
-}
-
 unsigned long __weak memory_block_size_bytes(void)
 {
 	return MIN_MEMORY_BLOCK_SIZE;
@@ -217,8 +207,7 @@ int memory_isolate_notify(unsigned long val, void *v)
  * The probe routines leave the pages reserved, just as the bootmem code does.
  * Make sure they're still that way.
  */
-static bool pages_correctly_reserved(unsigned long start_pfn,
-					unsigned long nr_pages)
+static bool pages_correctly_reserved(unsigned long start_pfn)
 {
 	int i, j;
 	struct page *page;
@@ -266,7 +255,7 @@ memory_block_action(unsigned long phys_index, unsigned long action, int online_t
 
 	switch (action) {
 		case MEM_ONLINE:
-			if (!pages_correctly_reserved(start_pfn, nr_pages))
+			if (!pages_correctly_reserved(start_pfn))
 				return -EBUSY;
 
 			ret = online_pages(start_pfn, nr_pages, online_type);
@@ -637,8 +626,28 @@ static int add_memory_section(int nid, struct mem_section *section,
 	return ret;
 }
 
-int remove_memory_block(unsigned long node_id, struct mem_section *section,
-		int phys_device)
+/*
+ * need an interface for the VM to add new memory regions,
+ * but without onlining it.
+ */
+int register_new_memory(int nid, struct mem_section *section)
+{
+	return add_memory_section(nid, section, NULL, MEM_OFFLINE, HOTPLUG);
+}
+
+#ifdef CONFIG_MEMORY_HOTREMOVE
+static void
+unregister_memory(struct memory_block *memory)
+{
+	BUG_ON(memory->dev.bus != &memory_subsys);
+
+	/* drop the ref. we got in remove_memory_block() */
+	kobject_put(&memory->dev.kobj);
+	device_unregister(&memory->dev);
+}
+
+static int remove_memory_block(unsigned long node_id,
+			       struct mem_section *section, int phys_device)
 {
 	struct memory_block *mem;
 
@@ -661,15 +670,6 @@ int remove_memory_block(unsigned long node_id, struct mem_section *section,
 	return 0;
 }
 
-/*
- * need an interface for the VM to add new memory regions,
- * but without onlining it.
- */
-int register_new_memory(int nid, struct mem_section *section)
-{
-	return add_memory_section(nid, section, NULL, MEM_OFFLINE, HOTPLUG);
-}
-
 int unregister_memory_section(struct mem_section *section)
 {
 	if (!present_section(section))
@@ -677,6 +677,7 @@ int unregister_memory_section(struct mem_section *section)
 
 	return remove_memory_block(0, section, 0);
 }
+#endif /* CONFIG_MEMORY_HOTREMOVE */
 
 /*
  * offline one memory block. If the memory block has been offlined, do nothing.
diff --git a/drivers/base/node.c b/drivers/base/node.c
index fac124a7e1c..7616a77ca32 100644
--- a/drivers/base/node.c
+++ b/drivers/base/node.c
@@ -7,6 +7,7 @@
 #include <linux/mm.h>
 #include <linux/memory.h>
 #include <linux/vmstat.h>
+#include <linux/notifier.h>
 #include <linux/node.h>
 #include <linux/hugetlb.h>
 #include <linux/compaction.h>
@@ -683,8 +684,11 @@ static int __init register_node_type(void)
 
 	ret = subsys_system_register(&node_subsys, cpu_root_attr_groups);
 	if (!ret) {
-		hotplug_memory_notifier(node_memory_callback,
-					NODE_CALLBACK_PRI);
+		static struct notifier_block node_memory_callback_nb = {
+			.notifier_call = node_memory_callback,
+			.priority = NODE_CALLBACK_PRI,
+		};
+		register_hotmemory_notifier(&node_memory_callback_nb);
 	}
 
 	/*
diff --git a/drivers/base/platform.c b/drivers/base/platform.c
index c0b8df38402..9eda84246ff 100644
--- a/drivers/base/platform.c
+++ b/drivers/base/platform.c
@@ -46,8 +46,8 @@ EXPORT_SYMBOL_GPL(platform_bus);
  * manipulate any relevant information in the pdev_archdata they can do:
  *
  *	platform_device_alloc()
- * 	... manipulate ...
- * 	platform_device_add()
+ *	... manipulate ...
+ *	platform_device_add()
  *
  * And if they don't care they can just call platform_device_register() and
  * everything will just work out.
@@ -326,9 +326,7 @@ int platform_device_add(struct platform_device *pdev)
 		}
 
 		if (p && insert_resource(p, r)) {
-			printk(KERN_ERR
-			       "%s: failed to claim resource %d\n",
-			       dev_name(&pdev->dev), i);
+			dev_err(&pdev->dev, "failed to claim resource %d\n", i);
 			ret = -EBUSY;
 			goto failed;
 		}
@@ -555,7 +553,8 @@ EXPORT_SYMBOL_GPL(platform_driver_unregister);
 /**
  * platform_driver_probe - register driver for non-hotpluggable device
  * @drv: platform driver structure
- * @probe: the driver probe routine, probably from an __init section
+ * @probe: the driver probe routine, probably from an __init section,
+ *         must not return -EPROBE_DEFER.
  *
  * Use this instead of platform_driver_register() when you know the device
  * is not hotpluggable and has already been registered, and you want to
@@ -566,6 +565,9 @@ EXPORT_SYMBOL_GPL(platform_driver_unregister);
  * into system-on-chip processors, where the controller devices have been
  * configured as part of board setup.
  *
+ * This is incompatible with deferred probing so probe() must not
+ * return -EPROBE_DEFER.
+ *
  * Returns zero if the driver registered and bound to a device, else returns
  * a negative error code and with the driver not registered.
  */
@@ -682,7 +684,7 @@ static int platform_uevent(struct device *dev, struct kobj_uevent_env *env)
 	int rc;
 
 	/* Some devices have extra OF data and an OF-style MODALIAS */
-	rc = of_device_uevent_modalias(dev,env);
+	rc = of_device_uevent_modalias(dev, env);
 	if (rc != -ENODEV)
 		return rc;
 
@@ -1126,8 +1128,8 @@ static int __init early_platform_driver_probe_id(char *class_str,
 
 		switch (match_id) {
 		case EARLY_PLATFORM_ID_ERROR:
-			pr_warning("%s: unable to parse %s parameter\n",
-				   class_str, epdrv->pdrv->driver.name);
+			pr_warn("%s: unable to parse %s parameter\n",
+				class_str, epdrv->pdrv->driver.name);
 			/* fall-through */
 		case EARLY_PLATFORM_ID_UNSET:
 			match = NULL;
@@ -1158,8 +1160,8 @@ static int __init early_platform_driver_probe_id(char *class_str,
 			}
 
 			if (epdrv->pdrv->probe(match))
-				pr_warning("%s: unable to probe %s early.\n",
-					   class_str, match->name);
+				pr_warn("%s: unable to probe %s early.\n",
+					class_str, match->name);
 			else
 				n++;
 		}
diff --git a/drivers/base/power/qos.c b/drivers/base/power/qos.c
index 5f74587ef25..71671c42ef4 100644
--- a/drivers/base/power/qos.c
+++ b/drivers/base/power/qos.c
@@ -46,6 +46,7 @@
 #include "power.h"
 
 static DEFINE_MUTEX(dev_pm_qos_mtx);
+static DEFINE_MUTEX(dev_pm_qos_sysfs_mtx);
 
 static BLOCKING_NOTIFIER_HEAD(dev_pm_notifiers);
 
@@ -216,12 +217,17 @@ void dev_pm_qos_constraints_destroy(struct device *dev)
 	struct pm_qos_constraints *c;
 	struct pm_qos_flags *f;
 
-	mutex_lock(&dev_pm_qos_mtx);
+	mutex_lock(&dev_pm_qos_sysfs_mtx);
 
 	/*
 	 * If the device's PM QoS resume latency limit or PM QoS flags have been
 	 * exposed to user space, they have to be hidden at this point.
 	 */
+	pm_qos_sysfs_remove_latency(dev);
+	pm_qos_sysfs_remove_flags(dev);
+
+	mutex_lock(&dev_pm_qos_mtx);
+
 	__dev_pm_qos_hide_latency_limit(dev);
 	__dev_pm_qos_hide_flags(dev);
 
@@ -254,6 +260,8 @@ void dev_pm_qos_constraints_destroy(struct device *dev)
 
  out:
 	mutex_unlock(&dev_pm_qos_mtx);
+
+	mutex_unlock(&dev_pm_qos_sysfs_mtx);
 }
 
 /**
@@ -558,6 +566,14 @@ static void __dev_pm_qos_drop_user_request(struct device *dev,
 	kfree(req);
 }
 
+static void dev_pm_qos_drop_user_request(struct device *dev,
+					 enum dev_pm_qos_req_type type)
+{
+	mutex_lock(&dev_pm_qos_mtx);
+	__dev_pm_qos_drop_user_request(dev, type);
+	mutex_unlock(&dev_pm_qos_mtx);
+}
+
 /**
  * dev_pm_qos_expose_latency_limit - Expose PM QoS latency limit to user space.
  * @dev: Device whose PM QoS latency limit is to be exposed to user space.
@@ -581,6 +597,8 @@ int dev_pm_qos_expose_latency_limit(struct device *dev, s32 value)
 		return ret;
 	}
 
+	mutex_lock(&dev_pm_qos_sysfs_mtx);
+
 	mutex_lock(&dev_pm_qos_mtx);
 
 	if (IS_ERR_OR_NULL(dev->power.qos))
@@ -591,26 +609,27 @@ int dev_pm_qos_expose_latency_limit(struct device *dev, s32 value)
 	if (ret < 0) {
 		__dev_pm_qos_remove_request(req);
 		kfree(req);
+		mutex_unlock(&dev_pm_qos_mtx);
 		goto out;
 	}
-
 	dev->power.qos->latency_req = req;
+
+	mutex_unlock(&dev_pm_qos_mtx);
+
 	ret = pm_qos_sysfs_add_latency(dev);
 	if (ret)
-		__dev_pm_qos_drop_user_request(dev, DEV_PM_QOS_LATENCY);
+		dev_pm_qos_drop_user_request(dev, DEV_PM_QOS_LATENCY);
 
  out:
-	mutex_unlock(&dev_pm_qos_mtx);
+	mutex_unlock(&dev_pm_qos_sysfs_mtx);
 	return ret;
 }
 EXPORT_SYMBOL_GPL(dev_pm_qos_expose_latency_limit);
 
 static void __dev_pm_qos_hide_latency_limit(struct device *dev)
 {
-	if (!IS_ERR_OR_NULL(dev->power.qos) && dev->power.qos->latency_req) {
-		pm_qos_sysfs_remove_latency(dev);
+	if (!IS_ERR_OR_NULL(dev->power.qos) && dev->power.qos->latency_req)
 		__dev_pm_qos_drop_user_request(dev, DEV_PM_QOS_LATENCY);
-	}
 }
 
 /**
@@ -619,9 +638,15 @@ static void __dev_pm_qos_hide_latency_limit(struct device *dev)
  */
 void dev_pm_qos_hide_latency_limit(struct device *dev)
 {
+	mutex_lock(&dev_pm_qos_sysfs_mtx);
+
+	pm_qos_sysfs_remove_latency(dev);
+
 	mutex_lock(&dev_pm_qos_mtx);
 	__dev_pm_qos_hide_latency_limit(dev);
 	mutex_unlock(&dev_pm_qos_mtx);
+
+	mutex_unlock(&dev_pm_qos_sysfs_mtx);
 }
 EXPORT_SYMBOL_GPL(dev_pm_qos_hide_latency_limit);
 
@@ -649,6 +674,8 @@ int dev_pm_qos_expose_flags(struct device *dev, s32 val)
 	}
 
 	pm_runtime_get_sync(dev);
+	mutex_lock(&dev_pm_qos_sysfs_mtx);
+
 	mutex_lock(&dev_pm_qos_mtx);
 
 	if (IS_ERR_OR_NULL(dev->power.qos))
@@ -659,16 +686,19 @@ int dev_pm_qos_expose_flags(struct device *dev, s32 val)
 	if (ret < 0) {
 		__dev_pm_qos_remove_request(req);
 		kfree(req);
+		mutex_unlock(&dev_pm_qos_mtx);
 		goto out;
 	}
-
 	dev->power.qos->flags_req = req;
+
+	mutex_unlock(&dev_pm_qos_mtx);
+
 	ret = pm_qos_sysfs_add_flags(dev);
 	if (ret)
-		__dev_pm_qos_drop_user_request(dev, DEV_PM_QOS_FLAGS);
+		dev_pm_qos_drop_user_request(dev, DEV_PM_QOS_FLAGS);
 
  out:
-	mutex_unlock(&dev_pm_qos_mtx);
+	mutex_unlock(&dev_pm_qos_sysfs_mtx);
 	pm_runtime_put(dev);
 	return ret;
 }
@@ -676,10 +706,8 @@ EXPORT_SYMBOL_GPL(dev_pm_qos_expose_flags);
 
 static void __dev_pm_qos_hide_flags(struct device *dev)
 {
-	if (!IS_ERR_OR_NULL(dev->power.qos) && dev->power.qos->flags_req) {
-		pm_qos_sysfs_remove_flags(dev);
+	if (!IS_ERR_OR_NULL(dev->power.qos) && dev->power.qos->flags_req)
 		__dev_pm_qos_drop_user_request(dev, DEV_PM_QOS_FLAGS);
-	}
 }
 
 /**
@@ -689,9 +717,15 @@ static void __dev_pm_qos_hide_flags(struct device *dev)
 void dev_pm_qos_hide_flags(struct device *dev)
 {
 	pm_runtime_get_sync(dev);
+	mutex_lock(&dev_pm_qos_sysfs_mtx);
+
+	pm_qos_sysfs_remove_flags(dev);
+
 	mutex_lock(&dev_pm_qos_mtx);
 	__dev_pm_qos_hide_flags(dev);
 	mutex_unlock(&dev_pm_qos_mtx);
+
+	mutex_unlock(&dev_pm_qos_sysfs_mtx);
 	pm_runtime_put(dev);
 }
 EXPORT_SYMBOL_GPL(dev_pm_qos_hide_flags);
diff --git a/drivers/base/regmap/internal.h b/drivers/base/regmap/internal.h
index 5a22bd33ce3..c130536e0ab 100644
--- a/drivers/base/regmap/internal.h
+++ b/drivers/base/regmap/internal.h
@@ -38,7 +38,8 @@ struct regmap_format {
 			     unsigned int reg, unsigned int val);
 	void (*format_reg)(void *buf, unsigned int reg, unsigned int shift);
 	void (*format_val)(void *buf, unsigned int val, unsigned int shift);
-	unsigned int (*parse_val)(void *buf);
+	unsigned int (*parse_val)(const void *buf);
+	void (*parse_inplace)(void *buf);
 };
 
 struct regmap_async {
@@ -76,6 +77,7 @@ struct regmap {
 	unsigned int debugfs_tot_len;
 
 	struct list_head debugfs_off_cache;
+	struct mutex cache_lock;
 #endif
 
 	unsigned int max_register;
@@ -125,6 +127,9 @@ struct regmap {
 	void *cache;
 	u32 cache_dirty;
 
+	unsigned long *cache_present;
+	unsigned int cache_present_nbits;
+
 	struct reg_default *patch;
 	int patch_regs;
 
@@ -187,12 +192,35 @@ int regcache_read(struct regmap *map,
 int regcache_write(struct regmap *map,
 			unsigned int reg, unsigned int value);
 int regcache_sync(struct regmap *map);
-
-unsigned int regcache_get_val(const void *base, unsigned int idx,
-			      unsigned int word_size);
-bool regcache_set_val(void *base, unsigned int idx,
-		      unsigned int val, unsigned int word_size);
+int regcache_sync_block(struct regmap *map, void *block,
+			unsigned int block_base, unsigned int start,
+			unsigned int end);
+
+static inline const void *regcache_get_val_addr(struct regmap *map,
+						const void *base,
+						unsigned int idx)
+{
+	return base + (map->cache_word_size * idx);
+}
+
+unsigned int regcache_get_val(struct regmap *map, const void *base,
+			      unsigned int idx);
+bool regcache_set_val(struct regmap *map, void *base, unsigned int idx,
+		      unsigned int val);
 int regcache_lookup_reg(struct regmap *map, unsigned int reg);
+int regcache_set_reg_present(struct regmap *map, unsigned int reg);
+
+static inline bool regcache_reg_present(struct regmap *map, unsigned int reg)
+{
+	if (!map->cache_present)
+		return true;
+	if (reg > map->cache_present_nbits)
+		return false;
+	return map->cache_present[BIT_WORD(reg)] & BIT_MASK(reg);
+}
+
+int _regmap_raw_write(struct regmap *map, unsigned int reg,
+		      const void *val, size_t val_len, bool async);
 
 void regmap_async_complete_cb(struct regmap_async *async, int ret);
 
diff --git a/drivers/base/regmap/regcache-lzo.c b/drivers/base/regmap/regcache-lzo.c
index afd6aa91a0d..e210a6d1406 100644
--- a/drivers/base/regmap/regcache-lzo.c
+++ b/drivers/base/regmap/regcache-lzo.c
@@ -260,8 +260,7 @@ static int regcache_lzo_read(struct regmap *map,
 	ret = regcache_lzo_decompress_cache_block(map, lzo_block);
 	if (ret >= 0)
 		/* fetch the value from the cache */
-		*value = regcache_get_val(lzo_block->dst, blkpos,
-					  map->cache_word_size);
+		*value = regcache_get_val(map, lzo_block->dst, blkpos);
 
 	kfree(lzo_block->dst);
 	/* restore the pointer and length of the compressed block */
@@ -304,8 +303,7 @@ static int regcache_lzo_write(struct regmap *map,
 	}
 
 	/* write the new value to the cache */
-	if (regcache_set_val(lzo_block->dst, blkpos, value,
-			     map->cache_word_size)) {
+	if (regcache_set_val(map, lzo_block->dst, blkpos, value)) {
 		kfree(lzo_block->dst);
 		goto out;
 	}
diff --git a/drivers/base/regmap/regcache-rbtree.c b/drivers/base/regmap/regcache-rbtree.c
index e6732cf7c06..aa0875f6f1b 100644
--- a/drivers/base/regmap/regcache-rbtree.c
+++ b/drivers/base/regmap/regcache-rbtree.c
@@ -47,22 +47,21 @@ static inline void regcache_rbtree_get_base_top_reg(
 	*top = rbnode->base_reg + ((rbnode->blklen - 1) * map->reg_stride);
 }
 
-static unsigned int regcache_rbtree_get_register(
-	struct regcache_rbtree_node *rbnode, unsigned int idx,
-	unsigned int word_size)
+static unsigned int regcache_rbtree_get_register(struct regmap *map,
+	struct regcache_rbtree_node *rbnode, unsigned int idx)
 {
-	return regcache_get_val(rbnode->block, idx, word_size);
+	return regcache_get_val(map, rbnode->block, idx);
 }
 
-static void regcache_rbtree_set_register(struct regcache_rbtree_node *rbnode,
-					 unsigned int idx, unsigned int val,
-					 unsigned int word_size)
+static void regcache_rbtree_set_register(struct regmap *map,
+					 struct regcache_rbtree_node *rbnode,
+					 unsigned int idx, unsigned int val)
 {
-	regcache_set_val(rbnode->block, idx, val, word_size);
+	regcache_set_val(map, rbnode->block, idx, val);
 }
 
 static struct regcache_rbtree_node *regcache_rbtree_lookup(struct regmap *map,
-	unsigned int reg)
+							   unsigned int reg)
 {
 	struct regcache_rbtree_ctx *rbtree_ctx = map->cache;
 	struct rb_node *node;
@@ -139,15 +138,21 @@ static int rbtree_show(struct seq_file *s, void *ignored)
 	struct regcache_rbtree_node *n;
 	struct rb_node *node;
 	unsigned int base, top;
+	size_t mem_size;
 	int nodes = 0;
 	int registers = 0;
 	int this_registers, average;
 
 	map->lock(map);
 
+	mem_size = sizeof(*rbtree_ctx);
+	mem_size += BITS_TO_LONGS(map->cache_present_nbits) * sizeof(long);
+
 	for (node = rb_first(&rbtree_ctx->root); node != NULL;
 	     node = rb_next(node)) {
 		n = container_of(node, struct regcache_rbtree_node, node);
+		mem_size += sizeof(*n);
+		mem_size += (n->blklen * map->cache_word_size);
 
 		regcache_rbtree_get_base_top_reg(map, n, &base, &top);
 		this_registers = ((top - base) / map->reg_stride) + 1;
@@ -162,8 +167,8 @@ static int rbtree_show(struct seq_file *s, void *ignored)
 	else
 		average = 0;
 
-	seq_printf(s, "%d nodes, %d registers, average %d registers\n",
-		   nodes, registers, average);
+	seq_printf(s, "%d nodes, %d registers, average %d registers, used %zu bytes\n",
+		   nodes, registers, average, mem_size);
 
 	map->unlock(map);
 
@@ -260,8 +265,9 @@ static int regcache_rbtree_read(struct regmap *map,
 	rbnode = regcache_rbtree_lookup(map, reg);
 	if (rbnode) {
 		reg_tmp = (reg - rbnode->base_reg) / map->reg_stride;
-		*value = regcache_rbtree_get_register(rbnode, reg_tmp,
-						      map->cache_word_size);
+		if (!regcache_reg_present(map, reg))
+			return -ENOENT;
+		*value = regcache_rbtree_get_register(map, rbnode, reg_tmp);
 	} else {
 		return -ENOENT;
 	}
@@ -270,21 +276,23 @@ static int regcache_rbtree_read(struct regmap *map,
 }
 
 
-static int regcache_rbtree_insert_to_block(struct regcache_rbtree_node *rbnode,
+static int regcache_rbtree_insert_to_block(struct regmap *map,
+					   struct regcache_rbtree_node *rbnode,
 					   unsigned int pos, unsigned int reg,
-					   unsigned int value, unsigned int word_size)
+					   unsigned int value)
 {
 	u8 *blk;
 
 	blk = krealloc(rbnode->block,
-		       (rbnode->blklen + 1) * word_size, GFP_KERNEL);
+		       (rbnode->blklen + 1) * map->cache_word_size,
+		       GFP_KERNEL);
 	if (!blk)
 		return -ENOMEM;
 
 	/* insert the register value in the correct place in the rbnode block */
-	memmove(blk + (pos + 1) * word_size,
-		blk + pos * word_size,
-		(rbnode->blklen - pos) * word_size);
+	memmove(blk + (pos + 1) * map->cache_word_size,
+		blk + pos * map->cache_word_size,
+		(rbnode->blklen - pos) * map->cache_word_size);
 
 	/* update the rbnode block, its size and the base register */
 	rbnode->block = blk;
@@ -292,7 +300,7 @@ static int regcache_rbtree_insert_to_block(struct regcache_rbtree_node *rbnode,
 	if (!pos)
 		rbnode->base_reg = reg;
 
-	regcache_rbtree_set_register(rbnode, pos, value, word_size);
+	regcache_rbtree_set_register(map, rbnode, pos, value);
 	return 0;
 }
 
@@ -302,25 +310,24 @@ static int regcache_rbtree_write(struct regmap *map, unsigned int reg,
 	struct regcache_rbtree_ctx *rbtree_ctx;
 	struct regcache_rbtree_node *rbnode, *rbnode_tmp;
 	struct rb_node *node;
-	unsigned int val;
 	unsigned int reg_tmp;
 	unsigned int pos;
 	int i;
 	int ret;
 
 	rbtree_ctx = map->cache;
+	/* update the reg_present bitmap, make space if necessary */
+	ret = regcache_set_reg_present(map, reg);
+	if (ret < 0)
+		return ret;
+
 	/* if we can't locate it in the cached rbnode we'll have
 	 * to traverse the rbtree looking for it.
 	 */
 	rbnode = regcache_rbtree_lookup(map, reg);
 	if (rbnode) {
 		reg_tmp = (reg - rbnode->base_reg) / map->reg_stride;
-		val = regcache_rbtree_get_register(rbnode, reg_tmp,
-						   map->cache_word_size);
-		if (val == value)
-			return 0;
-		regcache_rbtree_set_register(rbnode, reg_tmp, value,
-					     map->cache_word_size);
+		regcache_rbtree_set_register(map, rbnode, reg_tmp, value);
 	} else {
 		/* look for an adjacent register to the one we are about to add */
 		for (node = rb_first(&rbtree_ctx->root); node;
@@ -337,9 +344,10 @@ static int regcache_rbtree_write(struct regmap *map, unsigned int reg,
 					pos = i + 1;
 				else
 					pos = i;
-				ret = regcache_rbtree_insert_to_block(rbnode_tmp, pos,
-								      reg, value,
-								      map->cache_word_size);
+				ret = regcache_rbtree_insert_to_block(map,
+								      rbnode_tmp,
+								      pos, reg,
+								      value);
 				if (ret)
 					return ret;
 				rbtree_ctx->cached_rbnode = rbnode_tmp;
@@ -354,7 +362,7 @@ static int regcache_rbtree_write(struct regmap *map, unsigned int reg,
 		rbnode = kzalloc(sizeof *rbnode, GFP_KERNEL);
 		if (!rbnode)
 			return -ENOMEM;
-		rbnode->blklen = 1;
+		rbnode->blklen = sizeof(*rbnode);
 		rbnode->base_reg = reg;
 		rbnode->block = kmalloc(rbnode->blklen * map->cache_word_size,
 					GFP_KERNEL);
@@ -362,7 +370,7 @@ static int regcache_rbtree_write(struct regmap *map, unsigned int reg,
 			kfree(rbnode);
 			return -ENOMEM;
 		}
-		regcache_rbtree_set_register(rbnode, 0, value, map->cache_word_size);
+		regcache_rbtree_set_register(map, rbnode, 0, value);
 		regcache_rbtree_insert(map, &rbtree_ctx->root, rbnode);
 		rbtree_ctx->cached_rbnode = rbnode;
 	}
@@ -376,10 +384,8 @@ static int regcache_rbtree_sync(struct regmap *map, unsigned int min,
 	struct regcache_rbtree_ctx *rbtree_ctx;
 	struct rb_node *node;
 	struct regcache_rbtree_node *rbnode;
-	unsigned int regtmp;
-	unsigned int val;
 	int ret;
-	int i, base, end;
+	int base, end;
 
 	rbtree_ctx = map->cache;
 	for (node = rb_first(&rbtree_ctx->root); node; node = rb_next(node)) {
@@ -398,31 +404,17 @@ static int regcache_rbtree_sync(struct regmap *map, unsigned int min,
 			base = 0;
 
 		if (max < rbnode->base_reg + rbnode->blklen)
-			end = rbnode->base_reg + rbnode->blklen - max;
+			end = max - rbnode->base_reg + 1;
 		else
 			end = rbnode->blklen;
 
-		for (i = base; i < end; i++) {
-			regtmp = rbnode->base_reg + (i * map->reg_stride);
-			val = regcache_rbtree_get_register(rbnode, i,
-							   map->cache_word_size);
-
-			/* Is this the hardware default?  If so skip. */
-			ret = regcache_lookup_reg(map, regtmp);
-			if (ret >= 0 && val == map->reg_defaults[ret].def)
-				continue;
-
-			map->cache_bypass = 1;
-			ret = _regmap_write(map, regtmp, val);
-			map->cache_bypass = 0;
-			if (ret)
-				return ret;
-			dev_dbg(map->dev, "Synced register %#x, value %#x\n",
-				regtmp, val);
-		}
+		ret = regcache_sync_block(map, rbnode->block, rbnode->base_reg,
+					  base, end);
+		if (ret != 0)
+			return ret;
 	}
 
-	return 0;
+	return regmap_async_complete(map);
 }
 
 struct regcache_ops regcache_rbtree_ops = {
diff --git a/drivers/base/regmap/regcache.c b/drivers/base/regmap/regcache.c
index e69ff3e4742..75923f2396b 100644
--- a/drivers/base/regmap/regcache.c
+++ b/drivers/base/regmap/regcache.c
@@ -45,8 +45,8 @@ static int regcache_hw_init(struct regmap *map)
 		tmp_buf = kmalloc(map->cache_size_raw, GFP_KERNEL);
 		if (!tmp_buf)
 			return -EINVAL;
-		ret = regmap_bulk_read(map, 0, tmp_buf,
-				       map->num_reg_defaults_raw);
+		ret = regmap_raw_read(map, 0, tmp_buf,
+				      map->num_reg_defaults_raw);
 		map->cache_bypass = cache_bypass;
 		if (ret < 0) {
 			kfree(tmp_buf);
@@ -58,8 +58,7 @@ static int regcache_hw_init(struct regmap *map)
 
 	/* calculate the size of reg_defaults */
 	for (count = 0, i = 0; i < map->num_reg_defaults_raw; i++) {
-		val = regcache_get_val(map->reg_defaults_raw,
-				       i, map->cache_word_size);
+		val = regcache_get_val(map, map->reg_defaults_raw, i);
 		if (regmap_volatile(map, i * map->reg_stride))
 			continue;
 		count++;
@@ -75,8 +74,7 @@ static int regcache_hw_init(struct regmap *map)
 	/* fill the reg_defaults */
 	map->num_reg_defaults = count;
 	for (i = 0, j = 0; i < map->num_reg_defaults_raw; i++) {
-		val = regcache_get_val(map->reg_defaults_raw,
-				       i, map->cache_word_size);
+		val = regcache_get_val(map, map->reg_defaults_raw, i);
 		if (regmap_volatile(map, i * map->reg_stride))
 			continue;
 		map->reg_defaults[j].reg = i * map->reg_stride;
@@ -123,6 +121,8 @@ int regcache_init(struct regmap *map, const struct regmap_config *config)
 	map->reg_defaults_raw = config->reg_defaults_raw;
 	map->cache_word_size = DIV_ROUND_UP(config->val_bits, 8);
 	map->cache_size_raw = map->cache_word_size * config->num_reg_defaults_raw;
+	map->cache_present = NULL;
+	map->cache_present_nbits = 0;
 
 	map->cache = NULL;
 	map->cache_ops = cache_types[i];
@@ -181,6 +181,7 @@ void regcache_exit(struct regmap *map)
 
 	BUG_ON(!map->cache_ops);
 
+	kfree(map->cache_present);
 	kfree(map->reg_defaults);
 	if (map->cache_free)
 		kfree(map->reg_defaults_raw);
@@ -417,28 +418,68 @@ void regcache_cache_bypass(struct regmap *map, bool enable)
 }
 EXPORT_SYMBOL_GPL(regcache_cache_bypass);
 
-bool regcache_set_val(void *base, unsigned int idx,
-		      unsigned int val, unsigned int word_size)
+int regcache_set_reg_present(struct regmap *map, unsigned int reg)
 {
-	switch (word_size) {
+	unsigned long *cache_present;
+	unsigned int cache_present_size;
+	unsigned int nregs;
+	int i;
+
+	nregs = reg + 1;
+	cache_present_size = BITS_TO_LONGS(nregs);
+	cache_present_size *= sizeof(long);
+
+	if (!map->cache_present) {
+		cache_present = kmalloc(cache_present_size, GFP_KERNEL);
+		if (!cache_present)
+			return -ENOMEM;
+		bitmap_zero(cache_present, nregs);
+		map->cache_present = cache_present;
+		map->cache_present_nbits = nregs;
+	}
+
+	if (nregs > map->cache_present_nbits) {
+		cache_present = krealloc(map->cache_present,
+					 cache_present_size, GFP_KERNEL);
+		if (!cache_present)
+			return -ENOMEM;
+		for (i = 0; i < nregs; i++)
+			if (i >= map->cache_present_nbits)
+				clear_bit(i, cache_present);
+		map->cache_present = cache_present;
+		map->cache_present_nbits = nregs;
+	}
+
+	set_bit(reg, map->cache_present);
+	return 0;
+}
+
+bool regcache_set_val(struct regmap *map, void *base, unsigned int idx,
+		      unsigned int val)
+{
+	if (regcache_get_val(map, base, idx) == val)
+		return true;
+
+	/* Use device native format if possible */
+	if (map->format.format_val) {
+		map->format.format_val(base + (map->cache_word_size * idx),
+				       val, 0);
+		return false;
+	}
+
+	switch (map->cache_word_size) {
 	case 1: {
 		u8 *cache = base;
-		if (cache[idx] == val)
-			return true;
 		cache[idx] = val;
 		break;
 	}
 	case 2: {
 		u16 *cache = base;
-		if (cache[idx] == val)
-			return true;
 		cache[idx] = val;
 		break;
 	}
 	case 4: {
 		u32 *cache = base;
-		if (cache[idx] == val)
-			return true;
 		cache[idx] = val;
 		break;
 	}
@@ -448,13 +489,18 @@ bool regcache_set_val(void *base, unsigned int idx,
 	return false;
 }
 
-unsigned int regcache_get_val(const void *base, unsigned int idx,
-			      unsigned int word_size)
+unsigned int regcache_get_val(struct regmap *map, const void *base,
+			      unsigned int idx)
 {
 	if (!base)
 		return -EINVAL;
 
-	switch (word_size) {
+	/* Use device native format if possible */
+	if (map->format.parse_val)
+		return map->format.parse_val(regcache_get_val_addr(map, base,
+								   idx));
+
+	switch (map->cache_word_size) {
 	case 1: {
 		const u8 *cache = base;
 		return cache[idx];
@@ -498,3 +544,117 @@ int regcache_lookup_reg(struct regmap *map, unsigned int reg)
 	else
 		return -ENOENT;
 }
+
+static int regcache_sync_block_single(struct regmap *map, void *block,
+				      unsigned int block_base,
+				      unsigned int start, unsigned int end)
+{
+	unsigned int i, regtmp, val;
+	int ret;
+
+	for (i = start; i < end; i++) {
+		regtmp = block_base + (i * map->reg_stride);
+
+		if (!regcache_reg_present(map, regtmp))
+			continue;
+
+		val = regcache_get_val(map, block, i);
+
+		/* Is this the hardware default?  If so skip. */
+		ret = regcache_lookup_reg(map, regtmp);
+		if (ret >= 0 && val == map->reg_defaults[ret].def)
+			continue;
+
+		map->cache_bypass = 1;
+
+		ret = _regmap_write(map, regtmp, val);
+
+		map->cache_bypass = 0;
+		if (ret != 0)
+			return ret;
+		dev_dbg(map->dev, "Synced register %#x, value %#x\n",
+			regtmp, val);
+	}
+
+	return 0;
+}
+
+static int regcache_sync_block_raw_flush(struct regmap *map, const void **data,
+					 unsigned int base, unsigned int cur)
+{
+	size_t val_bytes = map->format.val_bytes;
+	int ret, count;
+
+	if (*data == NULL)
+		return 0;
+
+	count = cur - base;
+
+	dev_dbg(map->dev, "Writing %zu bytes for %d registers from 0x%x-0x%x\n",
+		count * val_bytes, count, base, cur - 1);
+
+	map->cache_bypass = 1;
+
+	ret = _regmap_raw_write(map, base, *data, count * val_bytes,
+				false);
+
+	map->cache_bypass = 0;
+
+	*data = NULL;
+
+	return ret;
+}
+
+static int regcache_sync_block_raw(struct regmap *map, void *block,
+			    unsigned int block_base, unsigned int start,
+			    unsigned int end)
+{
+	unsigned int i, val;
+	unsigned int regtmp = 0;
+	unsigned int base = 0;
+	const void *data = NULL;
+	int ret;
+
+	for (i = start; i < end; i++) {
+		regtmp = block_base + (i * map->reg_stride);
+
+		if (!regcache_reg_present(map, regtmp)) {
+			ret = regcache_sync_block_raw_flush(map, &data,
+							    base, regtmp);
+			if (ret != 0)
+				return ret;
+			continue;
+		}
+
+		val = regcache_get_val(map, block, i);
+
+		/* Is this the hardware default?  If so skip. */
+		ret = regcache_lookup_reg(map, regtmp);
+		if (ret >= 0 && val == map->reg_defaults[ret].def) {
+			ret = regcache_sync_block_raw_flush(map, &data,
+							    base, regtmp);
+			if (ret != 0)
+				return ret;
+			continue;
+		}
+
+		if (!data) {
+			data = regcache_get_val_addr(map, block, i);
+			base = regtmp;
+		}
+	}
+
+	return regcache_sync_block_raw_flush(map, &data, base, regtmp);
+}
+
+int regcache_sync_block(struct regmap *map, void *block,
+			unsigned int block_base, unsigned int start,
+			unsigned int end)
+{
+	if (regmap_can_raw_write(map))
+		return regcache_sync_block_raw(map, block, block_base,
+					       start, end);
+	else
+		return regcache_sync_block_single(map, block, block_base,
+						  start, end);
+}
diff --git a/drivers/base/regmap/regmap-debugfs.c b/drivers/base/regmap/regmap-debugfs.c
index 81d6f605c92..23b701f5fd2 100644
--- a/drivers/base/regmap/regmap-debugfs.c
+++ b/drivers/base/regmap/regmap-debugfs.c
@@ -88,16 +88,16 @@ static unsigned int regmap_debugfs_get_dump_start(struct regmap *map,
 	 * If we don't have a cache build one so we don't have to do a
 	 * linear scan each time.
 	 */
+	mutex_lock(&map->cache_lock);
+	i = base;
 	if (list_empty(&map->debugfs_off_cache)) {
-		for (i = base; i <= map->max_register; i += map->reg_stride) {
+		for (; i <= map->max_register; i += map->reg_stride) {
 			/* Skip unprinted registers, closing off cache entry */
 			if (!regmap_readable(map, i) ||
 			    regmap_precious(map, i)) {
 				if (c) {
 					c->max = p - 1;
-					fpos_offset = c->max - c->min;
-					reg_offset = fpos_offset / map->debugfs_tot_len;
-					c->max_reg = c->base_reg + reg_offset;
+					c->max_reg = i - map->reg_stride;
 					list_add_tail(&c->list,
 						      &map->debugfs_off_cache);
 					c = NULL;
@@ -111,6 +111,7 @@ static unsigned int regmap_debugfs_get_dump_start(struct regmap *map,
 				c = kzalloc(sizeof(*c), GFP_KERNEL);
 				if (!c) {
 					regmap_debugfs_free_dump_cache(map);
+					mutex_unlock(&map->cache_lock);
 					return base;
 				}
 				c->min = p;
@@ -124,9 +125,7 @@ static unsigned int regmap_debugfs_get_dump_start(struct regmap *map,
 	/* Close the last entry off if we didn't scan beyond it */
 	if (c) {
 		c->max = p - 1;
-		fpos_offset = c->max - c->min;
-		reg_offset = fpos_offset / map->debugfs_tot_len;
-		c->max_reg = c->base_reg + reg_offset;
+		c->max_reg = i - map->reg_stride;
 		list_add_tail(&c->list,
 			      &map->debugfs_off_cache);
 	}
@@ -145,12 +144,14 @@ static unsigned int regmap_debugfs_get_dump_start(struct regmap *map,
 			fpos_offset = from - c->min;
 			reg_offset = fpos_offset / map->debugfs_tot_len;
 			*pos = c->min + (reg_offset * map->debugfs_tot_len);
+			mutex_unlock(&map->cache_lock);
 			return c->base_reg + reg_offset;
 		}
 
 		*pos = c->max;
 		ret = c->max_reg;
 	}
+	mutex_unlock(&map->cache_lock);
 
 	return ret;
 }
@@ -311,6 +312,79 @@ static const struct file_operations regmap_range_fops = {
 	.llseek = default_llseek,
 };
 
+static ssize_t regmap_reg_ranges_read_file(struct file *file,
+					   char __user *user_buf, size_t count,
+					   loff_t *ppos)
+{
+	struct regmap *map = file->private_data;
+	struct regmap_debugfs_off_cache *c;
+	loff_t p = 0;
+	size_t buf_pos = 0;
+	char *buf;
+	char *entry;
+	int ret;
+
+	if (*ppos < 0 || !count)
+		return -EINVAL;
+
+	buf = kmalloc(count, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	entry = kmalloc(PAGE_SIZE, GFP_KERNEL);
+	if (!entry) {
+		kfree(buf);
+		return -ENOMEM;
+	}
+
+	/* While we are at it, build the register dump cache
+	 * now so the read() operation on the `registers' file
+	 * can benefit from using the cache.  We do not care
+	 * about the file position information that is contained
+	 * in the cache, just about the actual register blocks */
+	regmap_calc_tot_len(map, buf, count);
+	regmap_debugfs_get_dump_start(map, 0, *ppos, &p);
+
+	/* Reset file pointer as the fixed-format of the `registers'
+	 * file is not compatible with the `range' file */
+	p = 0;
+	mutex_lock(&map->cache_lock);
+	list_for_each_entry(c, &map->debugfs_off_cache, list) {
+		snprintf(entry, PAGE_SIZE, "%x-%x",
+			 c->base_reg, c->max_reg);
+		if (p >= *ppos) {
+			if (buf_pos + 1 + strlen(entry) > count)
+				break;
+			snprintf(buf + buf_pos, count - buf_pos,
+				 "%s", entry);
+			buf_pos += strlen(entry);
+			buf[buf_pos] = '\n';
+			buf_pos++;
+		}
+		p += strlen(entry) + 1;
+	}
+	mutex_unlock(&map->cache_lock);
+
+	kfree(entry);
+	ret = buf_pos;
+
+	if (copy_to_user(user_buf, buf, buf_pos)) {
+		ret = -EFAULT;
+		goto out_buf;
+	}
+
+	*ppos += buf_pos;
+out_buf:
+	kfree(buf);
+	return ret;
+}
+
+static const struct file_operations regmap_reg_ranges_fops = {
+	.open = simple_open,
+	.read = regmap_reg_ranges_read_file,
+	.llseek = default_llseek,
+};
+
 static ssize_t regmap_access_read_file(struct file *file,
 				       char __user *user_buf, size_t count,
 				       loff_t *ppos)
@@ -385,6 +459,7 @@ void regmap_debugfs_init(struct regmap *map, const char *name)
 	struct regmap_range_node *range_node;
 
 	INIT_LIST_HEAD(&map->debugfs_off_cache);
+	mutex_init(&map->cache_lock);
 
 	if (name) {
 		map->debugfs_name = kasprintf(GFP_KERNEL, "%s-%s",
@@ -403,6 +478,9 @@ void regmap_debugfs_init(struct regmap *map, const char *name)
 	debugfs_create_file("name", 0400, map->debugfs,
 			    map, &regmap_name_fops);
 
+	debugfs_create_file("range", 0400, map->debugfs,
+			    map, &regmap_reg_ranges_fops);
+
 	if (map->max_register) {
 		debugfs_create_file("registers", 0400, map->debugfs,
 				    map, &regmap_map_fops);
@@ -435,7 +513,9 @@ void regmap_debugfs_init(struct regmap *map, const char *name)
 void regmap_debugfs_exit(struct regmap *map)
 {
 	debugfs_remove_recursive(map->debugfs);
+	mutex_lock(&map->cache_lock);
 	regmap_debugfs_free_dump_cache(map);
+	mutex_unlock(&map->cache_lock);
 	kfree(map->debugfs_name);
 }
 
diff --git a/drivers/base/regmap/regmap-irq.c b/drivers/base/regmap/regmap-irq.c
index 020ea2b9fd2..1643e889baf 100644
--- a/drivers/base/regmap/regmap-irq.c
+++ b/drivers/base/regmap/regmap-irq.c
@@ -460,7 +460,8 @@ int regmap_add_irq_chip(struct regmap *map, int irq, int irq_flags,
 	ret = request_threaded_irq(irq, NULL, regmap_irq_thread, irq_flags,
 				   chip->name, d);
 	if (ret != 0) {
-		dev_err(map->dev, "Failed to request IRQ %d: %d\n", irq, ret);
+		dev_err(map->dev, "Failed to request IRQ %d for %s: %d\n",
+			irq, chip->name, ret);
 		goto err_domain;
 	}
 
diff --git a/drivers/base/regmap/regmap.c b/drivers/base/regmap/regmap.c
index 3d2367501fd..a941dcfe759 100644
--- a/drivers/base/regmap/regmap.c
+++ b/drivers/base/regmap/regmap.c
@@ -228,30 +228,39 @@ static void regmap_format_32_native(void *buf, unsigned int val,
 	*(u32 *)buf = val << shift;
 }
 
-static unsigned int regmap_parse_8(void *buf)
+static void regmap_parse_inplace_noop(void *buf)
 {
-	u8 *b = buf;
+}
+
+static unsigned int regmap_parse_8(const void *buf)
+{
+	const u8 *b = buf;
 
 	return b[0];
 }
 
-static unsigned int regmap_parse_16_be(void *buf)
+static unsigned int regmap_parse_16_be(const void *buf)
+{
+	const __be16 *b = buf;
+
+	return be16_to_cpu(b[0]);
+}
+
+static void regmap_parse_16_be_inplace(void *buf)
 {
 	__be16 *b = buf;
 
 	b[0] = be16_to_cpu(b[0]);
-
-	return b[0];
 }
 
-static unsigned int regmap_parse_16_native(void *buf)
+static unsigned int regmap_parse_16_native(const void *buf)
 {
 	return *(u16 *)buf;
 }
 
-static unsigned int regmap_parse_24(void *buf)
+static unsigned int regmap_parse_24(const void *buf)
 {
-	u8 *b = buf;
+	const u8 *b = buf;
 	unsigned int ret = b[2];
 	ret |= ((unsigned int)b[1]) << 8;
 	ret |= ((unsigned int)b[0]) << 16;
@@ -259,16 +268,21 @@ static unsigned int regmap_parse_24(void *buf)
 	return ret;
 }
 
-static unsigned int regmap_parse_32_be(void *buf)
+static unsigned int regmap_parse_32_be(const void *buf)
+{
+	const __be32 *b = buf;
+
+	return be32_to_cpu(b[0]);
+}
+
+static void regmap_parse_32_be_inplace(void *buf)
 {
 	__be32 *b = buf;
 
 	b[0] = be32_to_cpu(b[0]);
-
-	return b[0];
 }
 
-static unsigned int regmap_parse_32_native(void *buf)
+static unsigned int regmap_parse_32_native(const void *buf)
 {
 	return *(u32 *)buf;
 }
@@ -555,16 +569,21 @@ struct regmap *regmap_init(struct device *dev,
 		goto err_map;
 	}
 
+	if (val_endian == REGMAP_ENDIAN_NATIVE)
+		map->format.parse_inplace = regmap_parse_inplace_noop;
+
 	switch (config->val_bits) {
 	case 8:
 		map->format.format_val = regmap_format_8;
 		map->format.parse_val = regmap_parse_8;
+		map->format.parse_inplace = regmap_parse_inplace_noop;
 		break;
 	case 16:
 		switch (val_endian) {
 		case REGMAP_ENDIAN_BIG:
 			map->format.format_val = regmap_format_16_be;
 			map->format.parse_val = regmap_parse_16_be;
+			map->format.parse_inplace = regmap_parse_16_be_inplace;
 			break;
 		case REGMAP_ENDIAN_NATIVE:
 			map->format.format_val = regmap_format_16_native;
@@ -585,6 +604,7 @@ struct regmap *regmap_init(struct device *dev,
 		case REGMAP_ENDIAN_BIG:
 			map->format.format_val = regmap_format_32_be;
 			map->format.parse_val = regmap_parse_32_be;
+			map->format.parse_inplace = regmap_parse_32_be_inplace;
 			break;
 		case REGMAP_ENDIAN_NATIVE:
 			map->format.format_val = regmap_format_32_native;
@@ -710,12 +730,12 @@ skip_format_initialization:
 		}
 	}
 
+	regmap_debugfs_init(map, config->name);
+
 	ret = regcache_init(map, config);
 	if (ret != 0)
 		goto err_range;
 
-	regmap_debugfs_init(map, config->name);
-
 	/* Add a devres resource for dev_get_regmap() */
 	m = devres_alloc(dev_get_regmap_release, sizeof(*m), GFP_KERNEL);
 	if (!m) {
@@ -917,8 +937,8 @@ static int _regmap_select_page(struct regmap *map, unsigned int *reg,
 	return 0;
 }
 
-static int _regmap_raw_write(struct regmap *map, unsigned int reg,
-			     const void *val, size_t val_len, bool async)
+int _regmap_raw_write(struct regmap *map, unsigned int reg,
+		      const void *val, size_t val_len, bool async)
 {
 	struct regmap_range_node *range;
 	unsigned long flags;
@@ -930,7 +950,7 @@ static int _regmap_raw_write(struct regmap *map, unsigned int reg,
 	size_t len;
 	int i;
 
-	BUG_ON(!map->bus);
+	WARN_ON(!map->bus);
 
 	/* Check for unwritable registers before we start */
 	if (map->writeable_reg)
@@ -943,8 +963,7 @@ static int _regmap_raw_write(struct regmap *map, unsigned int reg,
 		unsigned int ival;
 		int val_bytes = map->format.val_bytes;
 		for (i = 0; i < val_len / val_bytes; i++) {
-			memcpy(map->work_buf, val + (i * val_bytes), val_bytes);
-			ival = map->format.parse_val(map->work_buf);
+			ival = map->format.parse_val(val + (i * val_bytes));
 			ret = regcache_write(map, reg + (i * map->reg_stride),
 					     ival);
 			if (ret) {
@@ -999,6 +1018,8 @@ static int _regmap_raw_write(struct regmap *map, unsigned int reg,
 		if (!async)
 			return -ENOMEM;
 
+		trace_regmap_async_write_start(map->dev, reg, val_len);
+
 		async->work_buf = kzalloc(map->format.buf_size,
 					  GFP_KERNEL | GFP_DMA);
 		if (!async->work_buf) {
@@ -1036,6 +1057,8 @@ static int _regmap_raw_write(struct regmap *map, unsigned int reg,
 			kfree(async->work_buf);
 			kfree(async);
 		}
+
+		return ret;
 	}
 
 	trace_regmap_hw_write_start(map->dev, reg,
@@ -1077,6 +1100,17 @@ static int _regmap_raw_write(struct regmap *map, unsigned int reg,
 	return ret;
 }
 
+/**
+ * regmap_can_raw_write - Test if regmap_raw_write() is supported
+ *
+ * @map: Map to check.
+ */
+bool regmap_can_raw_write(struct regmap *map)
+{
+	return map->bus && map->format.format_val && map->format.format_reg;
+}
+EXPORT_SYMBOL_GPL(regmap_can_raw_write);
+
 static int _regmap_bus_formatted_write(void *context, unsigned int reg,
 				       unsigned int val)
 {
@@ -1084,7 +1118,7 @@ static int _regmap_bus_formatted_write(void *context, unsigned int reg,
 	struct regmap_range_node *range;
 	struct regmap *map = context;
 
-	BUG_ON(!map->bus || !map->format.format_write);
+	WARN_ON(!map->bus || !map->format.format_write);
 
 	range = _regmap_range_lookup(map, reg);
 	if (range) {
@@ -1110,7 +1144,7 @@ static int _regmap_bus_raw_write(void *context, unsigned int reg,
 {
 	struct regmap *map = context;
 
-	BUG_ON(!map->bus || !map->format.format_val);
+	WARN_ON(!map->bus || !map->format.format_val);
 
 	map->format.format_val(map->work_buf + map->format.reg_bytes
 			       + map->format.pad_bytes, val, 0);
@@ -1200,12 +1234,10 @@ int regmap_raw_write(struct regmap *map, unsigned int reg,
 {
 	int ret;
 
-	if (!map->bus)
+	if (!regmap_can_raw_write(map))
 		return -EINVAL;
 	if (val_len % map->format.val_bytes)
 		return -EINVAL;
-	if (reg % map->reg_stride)
-		return -EINVAL;
 
 	map->lock(map->lock_arg);
 
@@ -1240,7 +1272,7 @@ int regmap_bulk_write(struct regmap *map, unsigned int reg, const void *val,
 
 	if (!map->bus)
 		return -EINVAL;
-	if (!map->format.parse_val)
+	if (!map->format.parse_inplace)
 		return -EINVAL;
 	if (reg % map->reg_stride)
 		return -EINVAL;
@@ -1258,7 +1290,7 @@ int regmap_bulk_write(struct regmap *map, unsigned int reg, const void *val,
 			goto out;
 		}
 		for (i = 0; i < val_count * val_bytes; i += val_bytes)
-			map->format.parse_val(wval + i);
+			map->format.parse_inplace(wval + i);
 	}
 	/*
 	 * Some devices does not support bulk write, for
@@ -1336,7 +1368,7 @@ static int _regmap_raw_read(struct regmap *map, unsigned int reg, void *val,
 	u8 *u8 = map->work_buf;
 	int ret;
 
-	BUG_ON(!map->bus);
+	WARN_ON(!map->bus);
 
 	range = _regmap_range_lookup(map, reg);
 	if (range) {
@@ -1391,7 +1423,7 @@ static int _regmap_read(struct regmap *map, unsigned int reg,
 	int ret;
 	void *context = _regmap_map_get_context(map);
 
-	BUG_ON(!map->reg_read);
+	WARN_ON(!map->reg_read);
 
 	if (!map->cache_bypass) {
 		ret = regcache_read(map, reg, val);
@@ -1519,7 +1551,7 @@ int regmap_bulk_read(struct regmap *map, unsigned int reg, void *val,
 
 	if (!map->bus)
 		return -EINVAL;
-	if (!map->format.parse_val)
+	if (!map->format.parse_inplace)
 		return -EINVAL;
 	if (reg % map->reg_stride)
 		return -EINVAL;
@@ -1546,7 +1578,7 @@ int regmap_bulk_read(struct regmap *map, unsigned int reg, void *val,
 		}
 
 		for (i = 0; i < val_count * val_bytes; i += val_bytes)
-			map->format.parse_val(val + i);
+			map->format.parse_inplace(val + i);
 	} else {
 		for (i = 0; i < val_count; i++) {
 			unsigned int ival;
@@ -1640,6 +1672,8 @@ void regmap_async_complete_cb(struct regmap_async *async, int ret)
 	struct regmap *map = async->map;
 	bool wake;
 
+	trace_regmap_async_io_complete(map->dev);
+
 	spin_lock(&map->async_lock);
 
 	list_del(&async->list);
@@ -1686,6 +1720,8 @@ int regmap_async_complete(struct regmap *map)
 	if (!map->bus->async_write)
 		return 0;
 
+	trace_regmap_async_complete_start(map->dev);
+
 	wait_event(map->async_waitq, regmap_async_is_done(map));
 
 	spin_lock_irqsave(&map->async_lock, flags);
@@ -1693,6 +1729,8 @@ int regmap_async_complete(struct regmap *map)
 	map->async_ret = 0;
 	spin_unlock_irqrestore(&map->async_lock, flags);
 
+	trace_regmap_async_complete_done(map->dev);
+
 	return ret;
 }
 EXPORT_SYMBOL_GPL(regmap_async_complete);
diff --git a/drivers/block/Kconfig b/drivers/block/Kconfig
index 5dc0daed8fa..b81ddfea1da 100644
--- a/drivers/block/Kconfig
+++ b/drivers/block/Kconfig
@@ -532,11 +532,11 @@ config BLK_DEV_RBD
 	  If unsure, say N.
 
 config BLK_DEV_RSXX
-	tristate "RamSam PCIe Flash SSD Device Driver"
+	tristate "IBM FlashSystem 70/80 PCIe SSD Device Driver"
 	depends on PCI
 	help
 	  Device driver for IBM's high speed PCIe SSD
-	  storage devices: RamSan-70 and RamSan-80.
+	  storage devices: FlashSystem-70 and FlashSystem-80.
 
 	  To compile this driver as a module, choose M here: the
 	  module will be called rsxx.
diff --git a/drivers/block/aoe/aoecmd.c b/drivers/block/aoe/aoecmd.c
index 25ef5c014fc..92b6d7c51e3 100644
--- a/drivers/block/aoe/aoecmd.c
+++ b/drivers/block/aoe/aoecmd.c
@@ -51,8 +51,9 @@ new_skb(ulong len)
 {
 	struct sk_buff *skb;
 
-	skb = alloc_skb(len, GFP_ATOMIC);
+	skb = alloc_skb(len + MAX_HEADER, GFP_ATOMIC);
 	if (skb) {
+		skb_reserve(skb, MAX_HEADER);
 		skb_reset_mac_header(skb);
 		skb_reset_network_header(skb);
 		skb->protocol = __constant_htons(ETH_P_AOE);
diff --git a/drivers/block/cciss.c b/drivers/block/cciss.c
index ade58bc8f3c..1c1b8e544aa 100644
--- a/drivers/block/cciss.c
+++ b/drivers/block/cciss.c
@@ -4206,7 +4206,7 @@ static int cciss_find_cfgtables(ctlr_info_t *h)
 	if (rc)
 		return rc;
 	h->cfgtable = remap_pci_mem(pci_resource_start(h->pdev,
-		cfg_base_addr_index) + cfg_offset, sizeof(h->cfgtable));
+		cfg_base_addr_index) + cfg_offset, sizeof(*h->cfgtable));
 	if (!h->cfgtable)
 		return -ENOMEM;
 	rc = write_driver_ver_to_cfgtable(h->cfgtable);
diff --git a/drivers/block/drbd/drbd_receiver.c b/drivers/block/drbd/drbd_receiver.c
index a9eccfc6079..83c5ae0ed56 100644
--- a/drivers/block/drbd/drbd_receiver.c
+++ b/drivers/block/drbd/drbd_receiver.c
@@ -757,7 +757,8 @@ static struct socket *drbd_wait_for_connect(struct drbd_tconn *tconn, struct acc
 	rcu_read_unlock();
 
 	timeo = connect_int * HZ;
-	timeo += (random32() & 1) ? timeo / 7 : -timeo / 7; /* 28.5% random jitter */
+	/* 28.5% random jitter */
+	timeo += (prandom_u32() & 1) ? timeo / 7 : -timeo / 7;
 
 	err = wait_for_completion_interruptible_timeout(&ad->door_bell, timeo);
 	if (err <= 0)
@@ -953,7 +954,7 @@ retry:
 				conn_warn(tconn, "Error receiving initial packet\n");
 				sock_release(s);
 randomize:
-				if (random32() & 1)
+				if (prandom_u32() & 1)
 					goto retry;
 			}
 		}
diff --git a/drivers/block/loop.c b/drivers/block/loop.c
index 747bb2af69d..dfe758382ea 100644
--- a/drivers/block/loop.c
+++ b/drivers/block/loop.c
@@ -922,6 +922,11 @@ static int loop_set_fd(struct loop_device *lo, fmode_t mode,
 		lo->lo_flags |= LO_FLAGS_PARTSCAN;
 	if (lo->lo_flags & LO_FLAGS_PARTSCAN)
 		ioctl_by_bdev(bdev, BLKRRPART, 0);
+
+	/* Grab the block_device to prevent its destruction after we
+	 * put /dev/loopXX inode. Later in loop_clr_fd() we bdput(bdev).
+	 */
+	bdgrab(bdev);
 	return 0;
 
 out_clr:
@@ -1031,8 +1036,10 @@ static int loop_clr_fd(struct loop_device *lo)
 	memset(lo->lo_encrypt_key, 0, LO_KEY_SIZE);
 	memset(lo->lo_crypt_name, 0, LO_NAME_SIZE);
 	memset(lo->lo_file_name, 0, LO_NAME_SIZE);
-	if (bdev)
+	if (bdev) {
+		bdput(bdev);
 		invalidate_bdev(bdev);
+	}
 	set_capacity(lo->lo_disk, 0);
 	loop_sysfs_exit(lo);
 	if (bdev) {
@@ -1623,6 +1630,7 @@ static int loop_add(struct loop_device **l, int i)
 		goto out_free_dev;
 	i = err;
 
+	err = -ENOMEM;
 	lo->lo_queue = blk_alloc_queue(GFP_KERNEL);
 	if (!lo->lo_queue)
 		goto out_free_dev;
diff --git a/drivers/block/mg_disk.c b/drivers/block/mg_disk.c
index 1788f491e0f..076ae7f1b78 100644
--- a/drivers/block/mg_disk.c
+++ b/drivers/block/mg_disk.c
@@ -890,8 +890,10 @@ static int mg_probe(struct platform_device *plat_dev)
 	gpio_direction_output(host->rst, 1);
 
 	/* reset out pin */
-	if (!(prv_data->dev_attr & MG_DEV_MASK))
+	if (!(prv_data->dev_attr & MG_DEV_MASK)) {
+		err = -EINVAL;
 		goto probe_err_3a;
+	}
 
 	if (prv_data->dev_attr != MG_BOOT_DEV) {
 		rsc = platform_get_resource_byname(plat_dev, IORESOURCE_IO,
diff --git a/drivers/block/mtip32xx/mtip32xx.c b/drivers/block/mtip32xx/mtip32xx.c
index 11cc9522cdd..32c678028e5 100644
--- a/drivers/block/mtip32xx/mtip32xx.c
+++ b/drivers/block/mtip32xx/mtip32xx.c
@@ -81,12 +81,17 @@
 /* Device instance number, incremented each time a device is probed. */
 static int instance;
 
+struct list_head online_list;
+struct list_head removing_list;
+spinlock_t dev_lock;
+
 /*
  * Global variable used to hold the major block device number
  * allocated in mtip_init().
  */
 static int mtip_major;
 static struct dentry *dfs_parent;
+static struct dentry *dfs_device_status;
 
 static u32 cpu_use[NR_CPUS];
 
@@ -243,40 +248,31 @@ static inline void release_slot(struct mtip_port *port, int tag)
 /*
  * Reset the HBA (without sleeping)
  *
- * Just like hba_reset, except does not call sleep, so can be
- * run from interrupt/tasklet context.
- *
  * @dd Pointer to the driver data structure.
  *
  * return value
  *	0	The reset was successful.
  *	-1	The HBA Reset bit did not clear.
  */
-static int hba_reset_nosleep(struct driver_data *dd)
+static int mtip_hba_reset(struct driver_data *dd)
 {
 	unsigned long timeout;
 
-	/* Chip quirk: quiesce any chip function */
-	mdelay(10);
-
 	/* Set the reset bit */
 	writel(HOST_RESET, dd->mmio + HOST_CTL);
 
 	/* Flush */
 	readl(dd->mmio + HOST_CTL);
 
-	/*
-	 * Wait 10ms then spin for up to 1 second
-	 * waiting for reset acknowledgement
-	 */
-	timeout = jiffies + msecs_to_jiffies(1000);
-	mdelay(10);
-	while ((readl(dd->mmio + HOST_CTL) & HOST_RESET)
-		 && time_before(jiffies, timeout))
-		mdelay(1);
+	/* Spin for up to 2 seconds, waiting for reset acknowledgement */
+	timeout = jiffies + msecs_to_jiffies(2000);
+	do {
+		mdelay(10);
+		if (test_bit(MTIP_DDF_REMOVE_PENDING_BIT, &dd->dd_flag))
+			return -1;
 
-	if (test_bit(MTIP_DDF_REMOVE_PENDING_BIT, &dd->dd_flag))
-		return -1;
+	} while ((readl(dd->mmio + HOST_CTL) & HOST_RESET)
+		 && time_before(jiffies, timeout));
 
 	if (readl(dd->mmio + HOST_CTL) & HOST_RESET)
 		return -1;
@@ -481,7 +477,7 @@ static void mtip_restart_port(struct mtip_port *port)
 		dev_warn(&port->dd->pdev->dev,
 			"PxCMD.CR not clear, escalating reset\n");
 
-		if (hba_reset_nosleep(port->dd))
+		if (mtip_hba_reset(port->dd))
 			dev_err(&port->dd->pdev->dev,
 				"HBA reset escalation failed.\n");
 
@@ -527,6 +523,26 @@ static void mtip_restart_port(struct mtip_port *port)
 
 }
 
+static int mtip_device_reset(struct driver_data *dd)
+{
+	int rv = 0;
+
+	if (mtip_check_surprise_removal(dd->pdev))
+		return 0;
+
+	if (mtip_hba_reset(dd) < 0)
+		rv = -EFAULT;
+
+	mdelay(1);
+	mtip_init_port(dd->port);
+	mtip_start_port(dd->port);
+
+	/* Enable interrupts on the HBA. */
+	writel(readl(dd->mmio + HOST_CTL) | HOST_IRQ_EN,
+					dd->mmio + HOST_CTL);
+	return rv;
+}
+
 /*
  * Helper function for tag logging
  */
@@ -632,7 +648,7 @@ static void mtip_timeout_function(unsigned long int data)
 	if (cmdto_cnt) {
 		print_tags(port->dd, "timed out", tagaccum, cmdto_cnt);
 		if (!test_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags)) {
-			mtip_restart_port(port);
+			mtip_device_reset(port->dd);
 			wake_up_interruptible(&port->svc_wait);
 		}
 		clear_bit(MTIP_PF_EH_ACTIVE_BIT, &port->flags);
@@ -1283,11 +1299,11 @@ static int mtip_exec_internal_command(struct mtip_port *port,
 	int rv = 0, ready2go = 1;
 	struct mtip_cmd *int_cmd = &port->commands[MTIP_TAG_INTERNAL];
 	unsigned long to;
+	struct driver_data *dd = port->dd;
 
 	/* Make sure the buffer is 8 byte aligned. This is asic specific. */
 	if (buffer & 0x00000007) {
-		dev_err(&port->dd->pdev->dev,
-			"SG buffer is not 8 byte aligned\n");
+		dev_err(&dd->pdev->dev, "SG buffer is not 8 byte aligned\n");
 		return -EFAULT;
 	}
 
@@ -1300,23 +1316,21 @@ static int mtip_exec_internal_command(struct mtip_port *port,
 		mdelay(100);
 	} while (time_before(jiffies, to));
 	if (!ready2go) {
-		dev_warn(&port->dd->pdev->dev,
+		dev_warn(&dd->pdev->dev,
 			"Internal cmd active. new cmd [%02X]\n", fis->command);
 		return -EBUSY;
 	}
 	set_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags);
 	port->ic_pause_timer = 0;
 
-	if (fis->command == ATA_CMD_SEC_ERASE_UNIT)
-		clear_bit(MTIP_PF_SE_ACTIVE_BIT, &port->flags);
-	else if (fis->command == ATA_CMD_DOWNLOAD_MICRO)
-		clear_bit(MTIP_PF_DM_ACTIVE_BIT, &port->flags);
+	clear_bit(MTIP_PF_SE_ACTIVE_BIT, &port->flags);
+	clear_bit(MTIP_PF_DM_ACTIVE_BIT, &port->flags);
 
 	if (atomic == GFP_KERNEL) {
 		if (fis->command != ATA_CMD_STANDBYNOW1) {
 			/* wait for io to complete if non atomic */
 			if (mtip_quiesce_io(port, 5000) < 0) {
-				dev_warn(&port->dd->pdev->dev,
+				dev_warn(&dd->pdev->dev,
 					"Failed to quiesce IO\n");
 				release_slot(port, MTIP_TAG_INTERNAL);
 				clear_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags);
@@ -1361,58 +1375,84 @@ static int mtip_exec_internal_command(struct mtip_port *port,
 	/* Issue the command to the hardware */
 	mtip_issue_non_ncq_command(port, MTIP_TAG_INTERNAL);
 
-	/* Poll if atomic, wait_for_completion otherwise */
 	if (atomic == GFP_KERNEL) {
 		/* Wait for the command to complete or timeout. */
-		if (wait_for_completion_timeout(
+		if (wait_for_completion_interruptible_timeout(
 				&wait,
-				msecs_to_jiffies(timeout)) == 0) {
-			dev_err(&port->dd->pdev->dev,
-				"Internal command did not complete [%d] "
-				"within timeout of  %lu ms\n",
-				atomic, timeout);
-			if (mtip_check_surprise_removal(port->dd->pdev) ||
+				msecs_to_jiffies(timeout)) <= 0) {
+			if (rv == -ERESTARTSYS) { /* interrupted */
+				dev_err(&dd->pdev->dev,
+					"Internal command [%02X] was interrupted after %lu ms\n",
+					fis->command, timeout);
+				rv = -EINTR;
+				goto exec_ic_exit;
+			} else if (rv == 0) /* timeout */
+				dev_err(&dd->pdev->dev,
+					"Internal command did not complete [%02X] within timeout of  %lu ms\n",
+					fis->command, timeout);
+			else
+				dev_err(&dd->pdev->dev,
+					"Internal command [%02X] wait returned code [%d] after %lu ms - unhandled\n",
+					fis->command, rv, timeout);
+
+			if (mtip_check_surprise_removal(dd->pdev) ||
 				test_bit(MTIP_DDF_REMOVE_PENDING_BIT,
-						&port->dd->dd_flag)) {
+						&dd->dd_flag)) {
+				dev_err(&dd->pdev->dev,
+					"Internal command [%02X] wait returned due to SR\n",
+					fis->command);
 				rv = -ENXIO;
 				goto exec_ic_exit;
 			}
+			mtip_device_reset(dd); /* recover from timeout issue */
 			rv = -EAGAIN;
+			goto exec_ic_exit;
 		}
 	} else {
+		u32 hba_stat, port_stat;
+
 		/* Spin for <timeout> checking if command still outstanding */
 		timeout = jiffies + msecs_to_jiffies(timeout);
 		while ((readl(port->cmd_issue[MTIP_TAG_INTERNAL])
 				& (1 << MTIP_TAG_INTERNAL))
 				&& time_before(jiffies, timeout)) {
-			if (mtip_check_surprise_removal(port->dd->pdev)) {
+			if (mtip_check_surprise_removal(dd->pdev)) {
 				rv = -ENXIO;
 				goto exec_ic_exit;
 			}
 			if ((fis->command != ATA_CMD_STANDBYNOW1) &&
 				test_bit(MTIP_DDF_REMOVE_PENDING_BIT,
-						&port->dd->dd_flag)) {
+						&dd->dd_flag)) {
 				rv = -ENXIO;
 				goto exec_ic_exit;
 			}
-			if (readl(port->mmio + PORT_IRQ_STAT) & PORT_IRQ_ERR) {
-				atomic_inc(&int_cmd->active); /* error */
-				break;
+			port_stat = readl(port->mmio + PORT_IRQ_STAT);
+			if (!port_stat)
+				continue;
+
+			if (port_stat & PORT_IRQ_ERR) {
+				dev_err(&dd->pdev->dev,
+					"Internal command [%02X] failed\n",
+					fis->command);
+				mtip_device_reset(dd);
+				rv = -EIO;
+				goto exec_ic_exit;
+			} else {
+				writel(port_stat, port->mmio + PORT_IRQ_STAT);
+				hba_stat = readl(dd->mmio + HOST_IRQ_STAT);
+				if (hba_stat)
+					writel(hba_stat,
+						dd->mmio + HOST_IRQ_STAT);
 			}
+			break;
 		}
 	}
 
-	if (atomic_read(&int_cmd->active) > 1) {
-		dev_err(&port->dd->pdev->dev,
-			"Internal command [%02X] failed\n", fis->command);
-		rv = -EIO;
-	}
 	if (readl(port->cmd_issue[MTIP_TAG_INTERNAL])
 			& (1 << MTIP_TAG_INTERNAL)) {
 		rv = -ENXIO;
-		if (!test_bit(MTIP_DDF_REMOVE_PENDING_BIT,
-					&port->dd->dd_flag)) {
-			mtip_restart_port(port);
+		if (!test_bit(MTIP_DDF_REMOVE_PENDING_BIT, &dd->dd_flag)) {
+			mtip_device_reset(dd);
 			rv = -EAGAIN;
 		}
 	}
@@ -1724,7 +1764,8 @@ static int mtip_get_smart_attr(struct mtip_port *port, unsigned int id,
  *      -EINVAL		Invalid parameters passed in, trim not supported
  *      -EIO		Error submitting trim request to hw
  */
-static int mtip_send_trim(struct driver_data *dd, unsigned int lba, unsigned int len)
+static int mtip_send_trim(struct driver_data *dd, unsigned int lba,
+				unsigned int len)
 {
 	int i, rv = 0;
 	u64 tlba, tlen, sect_left;
@@ -1811,45 +1852,6 @@ static bool mtip_hw_get_capacity(struct driver_data *dd, sector_t *sectors)
 }
 
 /*
- * Reset the HBA.
- *
- * Resets the HBA by setting the HBA Reset bit in the Global
- * HBA Control register. After setting the HBA Reset bit the
- * function waits for 1 second before reading the HBA Reset
- * bit to make sure it has cleared. If HBA Reset is not clear
- * an error is returned. Cannot be used in non-blockable
- * context.
- *
- * @dd Pointer to the driver data structure.
- *
- * return value
- *	0  The reset was successful.
- *	-1 The HBA Reset bit did not clear.
- */
-static int mtip_hba_reset(struct driver_data *dd)
-{
-	mtip_deinit_port(dd->port);
-
-	/* Set the reset bit */
-	writel(HOST_RESET, dd->mmio + HOST_CTL);
-
-	/* Flush */
-	readl(dd->mmio + HOST_CTL);
-
-	/* Wait for reset to clear */
-	ssleep(1);
-
-	/* Check the bit has cleared */
-	if (readl(dd->mmio + HOST_CTL) & HOST_RESET) {
-		dev_err(&dd->pdev->dev,
-			"Reset bit did not clear.\n");
-		return -1;
-	}
-
-	return 0;
-}
-
-/*
  * Display the identify command data.
  *
  * @port Pointer to the port data structure.
@@ -2710,6 +2712,100 @@ static ssize_t mtip_hw_show_status(struct device *dev,
 
 static DEVICE_ATTR(status, S_IRUGO, mtip_hw_show_status, NULL);
 
+/* debugsfs entries */
+
+static ssize_t show_device_status(struct device_driver *drv, char *buf)
+{
+	int size = 0;
+	struct driver_data *dd, *tmp;
+	unsigned long flags;
+	char id_buf[42];
+	u16 status = 0;
+
+	spin_lock_irqsave(&dev_lock, flags);
+	size += sprintf(&buf[size], "Devices Present:\n");
+	list_for_each_entry_safe(dd, tmp, &online_list, online_list) {
+		if (dd->pdev) {
+			if (dd->port &&
+			    dd->port->identify &&
+			    dd->port->identify_valid) {
+				strlcpy(id_buf,
+					(char *) (dd->port->identify + 10), 21);
+				status = *(dd->port->identify + 141);
+			} else {
+				memset(id_buf, 0, 42);
+				status = 0;
+			}
+
+			if (dd->port &&
+			    test_bit(MTIP_PF_REBUILD_BIT, &dd->port->flags)) {
+				size += sprintf(&buf[size],
+					" device %s %s (ftl rebuild %d %%)\n",
+					dev_name(&dd->pdev->dev),
+					id_buf,
+					status);
+			} else {
+				size += sprintf(&buf[size],
+					" device %s %s\n",
+					dev_name(&dd->pdev->dev),
+					id_buf);
+			}
+		}
+	}
+
+	size += sprintf(&buf[size], "Devices Being Removed:\n");
+	list_for_each_entry_safe(dd, tmp, &removing_list, remove_list) {
+		if (dd->pdev) {
+			if (dd->port &&
+			    dd->port->identify &&
+			    dd->port->identify_valid) {
+				strlcpy(id_buf,
+					(char *) (dd->port->identify+10), 21);
+				status = *(dd->port->identify + 141);
+			} else {
+				memset(id_buf, 0, 42);
+				status = 0;
+			}
+
+			if (dd->port &&
+			    test_bit(MTIP_PF_REBUILD_BIT, &dd->port->flags)) {
+				size += sprintf(&buf[size],
+					" device %s %s (ftl rebuild %d %%)\n",
+					dev_name(&dd->pdev->dev),
+					id_buf,
+					status);
+			} else {
+				size += sprintf(&buf[size],
+					" device %s %s\n",
+					dev_name(&dd->pdev->dev),
+					id_buf);
+			}
+		}
+	}
+	spin_unlock_irqrestore(&dev_lock, flags);
+
+	return size;
+}
+
+static ssize_t mtip_hw_read_device_status(struct file *f, char __user *ubuf,
+						size_t len, loff_t *offset)
+{
+	int size = *offset;
+	char buf[MTIP_DFS_MAX_BUF_SIZE];
+
+	if (!len || *offset)
+		return 0;
+
+	size += show_device_status(NULL, buf);
+
+	*offset = size <= len ? size : len;
+	size = copy_to_user(ubuf, buf, *offset);
+	if (size)
+		return -EFAULT;
+
+	return *offset;
+}
+
 static ssize_t mtip_hw_read_registers(struct file *f, char __user *ubuf,
 				  size_t len, loff_t *offset)
 {
@@ -2804,6 +2900,13 @@ static ssize_t mtip_hw_read_flags(struct file *f, char __user *ubuf,
 	return *offset;
 }
 
+static const struct file_operations mtip_device_status_fops = {
+	.owner  = THIS_MODULE,
+	.open   = simple_open,
+	.read   = mtip_hw_read_device_status,
+	.llseek = no_llseek,
+};
+
 static const struct file_operations mtip_regs_fops = {
 	.owner  = THIS_MODULE,
 	.open   = simple_open,
@@ -4161,6 +4264,7 @@ static int mtip_pci_probe(struct pci_dev *pdev,
 	const struct cpumask *node_mask;
 	int cpu, i = 0, j = 0;
 	int my_node = NUMA_NO_NODE;
+	unsigned long flags;
 
 	/* Allocate memory for this devices private data. */
 	my_node = pcibus_to_node(pdev->bus);
@@ -4218,12 +4322,16 @@ static int mtip_pci_probe(struct pci_dev *pdev,
 	dd->pdev	= pdev;
 	dd->numa_node	= my_node;
 
+	INIT_LIST_HEAD(&dd->online_list);
+	INIT_LIST_HEAD(&dd->remove_list);
+
 	memset(dd->workq_name, 0, 32);
 	snprintf(dd->workq_name, 31, "mtipq%d", dd->instance);
 
 	dd->isr_workq = create_workqueue(dd->workq_name);
 	if (!dd->isr_workq) {
 		dev_warn(&pdev->dev, "Can't create wq %d\n", dd->instance);
+		rv = -ENOMEM;
 		goto block_initialize_err;
 	}
 
@@ -4282,7 +4390,8 @@ static int mtip_pci_probe(struct pci_dev *pdev,
 	INIT_WORK(&dd->work[7].work, mtip_workq_sdbf7);
 
 	pci_set_master(pdev);
-	if (pci_enable_msi(pdev)) {
+	rv = pci_enable_msi(pdev);
+	if (rv) {
 		dev_warn(&pdev->dev,
 			"Unable to enable MSI interrupt.\n");
 		goto block_initialize_err;
@@ -4303,6 +4412,14 @@ static int mtip_pci_probe(struct pci_dev *pdev,
 	instance++;
 	if (rv != MTIP_FTL_REBUILD_MAGIC)
 		set_bit(MTIP_DDF_INIT_DONE_BIT, &dd->dd_flag);
+	else
+		rv = 0; /* device in rebuild state, return 0 from probe */
+
+	/* Add to online list even if in ftl rebuild */
+	spin_lock_irqsave(&dev_lock, flags);
+	list_add(&dd->online_list, &online_list);
+	spin_unlock_irqrestore(&dev_lock, flags);
+
 	goto done;
 
 block_initialize_err:
@@ -4336,9 +4453,15 @@ static void mtip_pci_remove(struct pci_dev *pdev)
 {
 	struct driver_data *dd = pci_get_drvdata(pdev);
 	int counter = 0;
+	unsigned long flags;
 
 	set_bit(MTIP_DDF_REMOVE_PENDING_BIT, &dd->dd_flag);
 
+	spin_lock_irqsave(&dev_lock, flags);
+	list_del_init(&dd->online_list);
+	list_add(&dd->remove_list, &removing_list);
+	spin_unlock_irqrestore(&dev_lock, flags);
+
 	if (mtip_check_surprise_removal(pdev)) {
 		while (!test_bit(MTIP_DDF_CLEANUP_BIT, &dd->dd_flag)) {
 			counter++;
@@ -4364,6 +4487,10 @@ static void mtip_pci_remove(struct pci_dev *pdev)
 
 	pci_disable_msi(pdev);
 
+	spin_lock_irqsave(&dev_lock, flags);
+	list_del_init(&dd->remove_list);
+	spin_unlock_irqrestore(&dev_lock, flags);
+
 	kfree(dd);
 	pcim_iounmap_regions(pdev, 1 << MTIP_ABAR);
 }
@@ -4511,6 +4638,11 @@ static int __init mtip_init(void)
 
 	pr_info(MTIP_DRV_NAME " Version " MTIP_DRV_VERSION "\n");
 
+	spin_lock_init(&dev_lock);
+
+	INIT_LIST_HEAD(&online_list);
+	INIT_LIST_HEAD(&removing_list);
+
 	/* Allocate a major block device number to use with this driver. */
 	error = register_blkdev(0, MTIP_DRV_NAME);
 	if (error <= 0) {
@@ -4520,11 +4652,18 @@ static int __init mtip_init(void)
 	}
 	mtip_major = error;
 
-	if (!dfs_parent) {
-		dfs_parent = debugfs_create_dir("rssd", NULL);
-		if (IS_ERR_OR_NULL(dfs_parent)) {
-			pr_warn("Error creating debugfs parent\n");
-			dfs_parent = NULL;
+	dfs_parent = debugfs_create_dir("rssd", NULL);
+	if (IS_ERR_OR_NULL(dfs_parent)) {
+		pr_warn("Error creating debugfs parent\n");
+		dfs_parent = NULL;
+	}
+	if (dfs_parent) {
+		dfs_device_status = debugfs_create_file("device_status",
+					S_IRUGO, dfs_parent, NULL,
+					&mtip_device_status_fops);
+		if (IS_ERR_OR_NULL(dfs_device_status)) {
+			pr_err("Error creating device_status node\n");
+			dfs_device_status = NULL;
 		}
 	}
 
diff --git a/drivers/block/mtip32xx/mtip32xx.h b/drivers/block/mtip32xx/mtip32xx.h
index 3bffff5f670..8e8334c9dd0 100644
--- a/drivers/block/mtip32xx/mtip32xx.h
+++ b/drivers/block/mtip32xx/mtip32xx.h
@@ -129,9 +129,9 @@ enum {
 	MTIP_PF_EH_ACTIVE_BIT       = 1, /* error handling */
 	MTIP_PF_SE_ACTIVE_BIT       = 2, /* secure erase */
 	MTIP_PF_DM_ACTIVE_BIT       = 3, /* download microcde */
-	MTIP_PF_PAUSE_IO      =	((1 << MTIP_PF_IC_ACTIVE_BIT) | \
-				(1 << MTIP_PF_EH_ACTIVE_BIT) | \
-				(1 << MTIP_PF_SE_ACTIVE_BIT) | \
+	MTIP_PF_PAUSE_IO      =	((1 << MTIP_PF_IC_ACTIVE_BIT) |
+				(1 << MTIP_PF_EH_ACTIVE_BIT) |
+				(1 << MTIP_PF_SE_ACTIVE_BIT) |
 				(1 << MTIP_PF_DM_ACTIVE_BIT)),
 
 	MTIP_PF_SVC_THD_ACTIVE_BIT  = 4,
@@ -144,9 +144,9 @@ enum {
 	MTIP_DDF_REMOVE_PENDING_BIT = 1,
 	MTIP_DDF_OVER_TEMP_BIT      = 2,
 	MTIP_DDF_WRITE_PROTECT_BIT  = 3,
-	MTIP_DDF_STOP_IO      = ((1 << MTIP_DDF_REMOVE_PENDING_BIT) | \
-				(1 << MTIP_DDF_SEC_LOCK_BIT) | \
-				(1 << MTIP_DDF_OVER_TEMP_BIT) | \
+	MTIP_DDF_STOP_IO      = ((1 << MTIP_DDF_REMOVE_PENDING_BIT) |
+				(1 << MTIP_DDF_SEC_LOCK_BIT) |
+				(1 << MTIP_DDF_OVER_TEMP_BIT) |
 				(1 << MTIP_DDF_WRITE_PROTECT_BIT)),
 
 	MTIP_DDF_CLEANUP_BIT        = 5,
@@ -180,7 +180,7 @@ struct mtip_work {
 
 #define MTIP_TRIM_TIMEOUT_MS		240000
 #define MTIP_MAX_TRIM_ENTRIES		8
-#define MTIP_MAX_TRIM_ENTRY_LEN 	0xfff8
+#define MTIP_MAX_TRIM_ENTRY_LEN		0xfff8
 
 struct mtip_trim_entry {
 	u32 lba;   /* starting lba of region */
@@ -501,6 +501,10 @@ struct driver_data {
 	atomic_t irq_workers_active;
 
 	int isr_binding;
+
+	struct list_head online_list; /* linkage for online list */
+
+	struct list_head remove_list; /* linkage for removing list */
 };
 
 #endif
diff --git a/drivers/block/rbd.c b/drivers/block/rbd.c
index 6c81a4c040b..b7b7a88d9f6 100644
--- a/drivers/block/rbd.c
+++ b/drivers/block/rbd.c
@@ -1264,6 +1264,32 @@ static bool obj_request_done_test(struct rbd_obj_request *obj_request)
 	return atomic_read(&obj_request->done) != 0;
 }
 
+static void
+rbd_img_obj_request_read_callback(struct rbd_obj_request *obj_request)
+{
+	dout("%s: obj %p img %p result %d %llu/%llu\n", __func__,
+		obj_request, obj_request->img_request, obj_request->result,
+		obj_request->xferred, obj_request->length);
+	/*
+	 * ENOENT means a hole in the image.  We zero-fill the
+	 * entire length of the request.  A short read also implies
+	 * zero-fill to the end of the request.  Either way we
+	 * update the xferred count to indicate the whole request
+	 * was satisfied.
+	 */
+	BUG_ON(obj_request->type != OBJ_REQUEST_BIO);
+	if (obj_request->result == -ENOENT) {
+		zero_bio_chain(obj_request->bio_list, 0);
+		obj_request->result = 0;
+		obj_request->xferred = obj_request->length;
+	} else if (obj_request->xferred < obj_request->length &&
+			!obj_request->result) {
+		zero_bio_chain(obj_request->bio_list, obj_request->xferred);
+		obj_request->xferred = obj_request->length;
+	}
+	obj_request_done_set(obj_request);
+}
+
 static void rbd_obj_request_complete(struct rbd_obj_request *obj_request)
 {
 	dout("%s: obj %p cb %p\n", __func__, obj_request,
@@ -1284,23 +1310,10 @@ static void rbd_osd_read_callback(struct rbd_obj_request *obj_request)
 {
 	dout("%s: obj %p result %d %llu/%llu\n", __func__, obj_request,
 		obj_request->result, obj_request->xferred, obj_request->length);
-	/*
-	 * ENOENT means a hole in the object.  We zero-fill the
-	 * entire length of the request.  A short read also implies
-	 * zero-fill to the end of the request.  Either way we
-	 * update the xferred count to indicate the whole request
-	 * was satisfied.
-	 */
-	if (obj_request->result == -ENOENT) {
-		zero_bio_chain(obj_request->bio_list, 0);
-		obj_request->result = 0;
-		obj_request->xferred = obj_request->length;
-	} else if (obj_request->xferred < obj_request->length &&
-			!obj_request->result) {
-		zero_bio_chain(obj_request->bio_list, obj_request->xferred);
-		obj_request->xferred = obj_request->length;
-	}
-	obj_request_done_set(obj_request);
+	if (obj_request->img_request)
+		rbd_img_obj_request_read_callback(obj_request);
+	else
+		obj_request_done_set(obj_request);
 }
 
 static void rbd_osd_write_callback(struct rbd_obj_request *obj_request)
@@ -1729,9 +1742,10 @@ static int rbd_img_request_submit(struct rbd_img_request *img_request)
 	struct rbd_device *rbd_dev = img_request->rbd_dev;
 	struct ceph_osd_client *osdc = &rbd_dev->rbd_client->client->osdc;
 	struct rbd_obj_request *obj_request;
+	struct rbd_obj_request *next_obj_request;
 
 	dout("%s: img %p\n", __func__, img_request);
-	for_each_obj_request(img_request, obj_request) {
+	for_each_obj_request_safe(img_request, obj_request, next_obj_request) {
 		int ret;
 
 		obj_request->callback = rbd_img_obj_callback;
diff --git a/drivers/block/rsxx/Makefile b/drivers/block/rsxx/Makefile
index f35cd0b71f7..b1c53c0aa45 100644
--- a/drivers/block/rsxx/Makefile
+++ b/drivers/block/rsxx/Makefile
@@ -1,2 +1,2 @@
 obj-$(CONFIG_BLK_DEV_RSXX) += rsxx.o
-rsxx-y := config.o core.o cregs.o dev.o dma.o
+rsxx-objs := config.o core.o cregs.o dev.o dma.o
diff --git a/drivers/block/rsxx/config.c b/drivers/block/rsxx/config.c
index a295e7e9ee4..10cd530d3e1 100644
--- a/drivers/block/rsxx/config.c
+++ b/drivers/block/rsxx/config.c
@@ -29,15 +29,13 @@
 #include "rsxx_priv.h"
 #include "rsxx_cfg.h"
 
-static void initialize_config(void *config)
+static void initialize_config(struct rsxx_card_cfg *cfg)
 {
-	struct rsxx_card_cfg *cfg = config;
-
 	cfg->hdr.version = RSXX_CFG_VERSION;
 
 	cfg->data.block_size        = RSXX_HW_BLK_SIZE;
 	cfg->data.stripe_size       = RSXX_HW_BLK_SIZE;
-	cfg->data.vendor_id         = RSXX_VENDOR_ID_TMS_IBM;
+	cfg->data.vendor_id         = RSXX_VENDOR_ID_IBM;
 	cfg->data.cache_order       = (-1);
 	cfg->data.intr_coal.mode    = RSXX_INTR_COAL_DISABLED;
 	cfg->data.intr_coal.count   = 0;
@@ -181,7 +179,7 @@ int rsxx_load_config(struct rsxx_cardinfo *card)
 	} else {
 		dev_info(CARD_TO_DEV(card),
 			"Initializing card configuration.\n");
-		initialize_config(card);
+		initialize_config(&card->config);
 		st = rsxx_save_config(card);
 		if (st)
 			return st;
diff --git a/drivers/block/rsxx/core.c b/drivers/block/rsxx/core.c
index e5162487686..5af21f2db29 100644
--- a/drivers/block/rsxx/core.c
+++ b/drivers/block/rsxx/core.c
@@ -30,6 +30,7 @@
 #include <linux/reboot.h>
 #include <linux/slab.h>
 #include <linux/bitops.h>
+#include <linux/delay.h>
 
 #include <linux/genhd.h>
 #include <linux/idr.h>
@@ -39,8 +40,8 @@
 
 #define NO_LEGACY 0
 
-MODULE_DESCRIPTION("IBM RamSan PCIe Flash SSD Device Driver");
-MODULE_AUTHOR("IBM <support@ramsan.com>");
+MODULE_DESCRIPTION("IBM FlashSystem 70/80 PCIe SSD Device Driver");
+MODULE_AUTHOR("Joshua Morris/Philip Kelleher, IBM");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(DRIVER_VERSION);
 
@@ -52,6 +53,13 @@ static DEFINE_IDA(rsxx_disk_ida);
 static DEFINE_SPINLOCK(rsxx_ida_lock);
 
 /*----------------- Interrupt Control & Handling -------------------*/
+
+static void rsxx_mask_interrupts(struct rsxx_cardinfo *card)
+{
+	card->isr_mask = 0;
+	card->ier_mask = 0;
+}
+
 static void __enable_intr(unsigned int *mask, unsigned int intr)
 {
 	*mask |= intr;
@@ -71,7 +79,8 @@ static void __disable_intr(unsigned int *mask, unsigned int intr)
  */
 void rsxx_enable_ier(struct rsxx_cardinfo *card, unsigned int intr)
 {
-	if (unlikely(card->halt))
+	if (unlikely(card->halt) ||
+	    unlikely(card->eeh_state))
 		return;
 
 	__enable_intr(&card->ier_mask, intr);
@@ -80,6 +89,9 @@ void rsxx_enable_ier(struct rsxx_cardinfo *card, unsigned int intr)
 
 void rsxx_disable_ier(struct rsxx_cardinfo *card, unsigned int intr)
 {
+	if (unlikely(card->eeh_state))
+		return;
+
 	__disable_intr(&card->ier_mask, intr);
 	iowrite32(card->ier_mask, card->regmap + IER);
 }
@@ -87,7 +99,8 @@ void rsxx_disable_ier(struct rsxx_cardinfo *card, unsigned int intr)
 void rsxx_enable_ier_and_isr(struct rsxx_cardinfo *card,
 				 unsigned int intr)
 {
-	if (unlikely(card->halt))
+	if (unlikely(card->halt) ||
+	    unlikely(card->eeh_state))
 		return;
 
 	__enable_intr(&card->isr_mask, intr);
@@ -97,6 +110,9 @@ void rsxx_enable_ier_and_isr(struct rsxx_cardinfo *card,
 void rsxx_disable_ier_and_isr(struct rsxx_cardinfo *card,
 				  unsigned int intr)
 {
+	if (unlikely(card->eeh_state))
+		return;
+
 	__disable_intr(&card->isr_mask, intr);
 	__disable_intr(&card->ier_mask, intr);
 	iowrite32(card->ier_mask, card->regmap + IER);
@@ -115,6 +131,9 @@ static irqreturn_t rsxx_isr(int irq, void *pdata)
 	do {
 		reread_isr = 0;
 
+		if (unlikely(card->eeh_state))
+			break;
+
 		isr = ioread32(card->regmap + ISR);
 		if (isr == 0xffffffff) {
 			/*
@@ -161,9 +180,9 @@ static irqreturn_t rsxx_isr(int irq, void *pdata)
 }
 
 /*----------------- Card Event Handler -------------------*/
-static char *rsxx_card_state_to_str(unsigned int state)
+static const char * const rsxx_card_state_to_str(unsigned int state)
 {
-	static char *state_strings[] = {
+	static const char * const state_strings[] = {
 		"Unknown", "Shutdown", "Starting", "Formatting",
 		"Uninitialized", "Good", "Shutting Down",
 		"Fault", "Read Only Fault", "dStroying"
@@ -304,6 +323,192 @@ static int card_shutdown(struct rsxx_cardinfo *card)
 	return 0;
 }
 
+static int rsxx_eeh_frozen(struct pci_dev *dev)
+{
+	struct rsxx_cardinfo *card = pci_get_drvdata(dev);
+	int i;
+	int st;
+
+	dev_warn(&dev->dev, "IBM FlashSystem PCI: preparing for slot reset.\n");
+
+	card->eeh_state = 1;
+	rsxx_mask_interrupts(card);
+
+	/*
+	 * We need to guarantee that the write for eeh_state and masking
+	 * interrupts does not become reordered. This will prevent a possible
+	 * race condition with the EEH code.
+	 */
+	wmb();
+
+	pci_disable_device(dev);
+
+	st = rsxx_eeh_save_issued_dmas(card);
+	if (st)
+		return st;
+
+	rsxx_eeh_save_issued_creg(card);
+
+	for (i = 0; i < card->n_targets; i++) {
+		if (card->ctrl[i].status.buf)
+			pci_free_consistent(card->dev, STATUS_BUFFER_SIZE8,
+					    card->ctrl[i].status.buf,
+					    card->ctrl[i].status.dma_addr);
+		if (card->ctrl[i].cmd.buf)
+			pci_free_consistent(card->dev, COMMAND_BUFFER_SIZE8,
+					    card->ctrl[i].cmd.buf,
+					    card->ctrl[i].cmd.dma_addr);
+	}
+
+	return 0;
+}
+
+static void rsxx_eeh_failure(struct pci_dev *dev)
+{
+	struct rsxx_cardinfo *card = pci_get_drvdata(dev);
+	int i;
+
+	dev_err(&dev->dev, "IBM FlashSystem PCI: disabling failed card.\n");
+
+	card->eeh_state = 1;
+
+	for (i = 0; i < card->n_targets; i++)
+		del_timer_sync(&card->ctrl[i].activity_timer);
+
+	rsxx_eeh_cancel_dmas(card);
+}
+
+static int rsxx_eeh_fifo_flush_poll(struct rsxx_cardinfo *card)
+{
+	unsigned int status;
+	int iter = 0;
+
+	/* We need to wait for the hardware to reset */
+	while (iter++ < 10) {
+		status = ioread32(card->regmap + PCI_RECONFIG);
+
+		if (status & RSXX_FLUSH_BUSY) {
+			ssleep(1);
+			continue;
+		}
+
+		if (status & RSXX_FLUSH_TIMEOUT)
+			dev_warn(CARD_TO_DEV(card), "HW: flash controller timeout\n");
+		return 0;
+	}
+
+	/* Hardware failed resetting itself. */
+	return -1;
+}
+
+static pci_ers_result_t rsxx_error_detected(struct pci_dev *dev,
+					    enum pci_channel_state error)
+{
+	int st;
+
+	if (dev->revision < RSXX_EEH_SUPPORT)
+		return PCI_ERS_RESULT_NONE;
+
+	if (error == pci_channel_io_perm_failure) {
+		rsxx_eeh_failure(dev);
+		return PCI_ERS_RESULT_DISCONNECT;
+	}
+
+	st = rsxx_eeh_frozen(dev);
+	if (st) {
+		dev_err(&dev->dev, "Slot reset setup failed\n");
+		rsxx_eeh_failure(dev);
+		return PCI_ERS_RESULT_DISCONNECT;
+	}
+
+	return PCI_ERS_RESULT_NEED_RESET;
+}
+
+static pci_ers_result_t rsxx_slot_reset(struct pci_dev *dev)
+{
+	struct rsxx_cardinfo *card = pci_get_drvdata(dev);
+	unsigned long flags;
+	int i;
+	int st;
+
+	dev_warn(&dev->dev,
+		"IBM FlashSystem PCI: recovering from slot reset.\n");
+
+	st = pci_enable_device(dev);
+	if (st)
+		goto failed_hw_setup;
+
+	pci_set_master(dev);
+
+	st = rsxx_eeh_fifo_flush_poll(card);
+	if (st)
+		goto failed_hw_setup;
+
+	rsxx_dma_queue_reset(card);
+
+	for (i = 0; i < card->n_targets; i++) {
+		st = rsxx_hw_buffers_init(dev, &card->ctrl[i]);
+		if (st)
+			goto failed_hw_buffers_init;
+	}
+
+	if (card->config_valid)
+		rsxx_dma_configure(card);
+
+	/* Clears the ISR register from spurious interrupts */
+	st = ioread32(card->regmap + ISR);
+
+	card->eeh_state = 0;
+
+	st = rsxx_eeh_remap_dmas(card);
+	if (st)
+		goto failed_remap_dmas;
+
+	spin_lock_irqsave(&card->irq_lock, flags);
+	if (card->n_targets & RSXX_MAX_TARGETS)
+		rsxx_enable_ier_and_isr(card, CR_INTR_ALL_G);
+	else
+		rsxx_enable_ier_and_isr(card, CR_INTR_ALL_C);
+	spin_unlock_irqrestore(&card->irq_lock, flags);
+
+	rsxx_kick_creg_queue(card);
+
+	for (i = 0; i < card->n_targets; i++) {
+		spin_lock(&card->ctrl[i].queue_lock);
+		if (list_empty(&card->ctrl[i].queue)) {
+			spin_unlock(&card->ctrl[i].queue_lock);
+			continue;
+		}
+		spin_unlock(&card->ctrl[i].queue_lock);
+
+		queue_work(card->ctrl[i].issue_wq,
+				&card->ctrl[i].issue_dma_work);
+	}
+
+	dev_info(&dev->dev, "IBM FlashSystem PCI: recovery complete.\n");
+
+	return PCI_ERS_RESULT_RECOVERED;
+
+failed_hw_buffers_init:
+failed_remap_dmas:
+	for (i = 0; i < card->n_targets; i++) {
+		if (card->ctrl[i].status.buf)
+			pci_free_consistent(card->dev,
+					STATUS_BUFFER_SIZE8,
+					card->ctrl[i].status.buf,
+					card->ctrl[i].status.dma_addr);
+		if (card->ctrl[i].cmd.buf)
+			pci_free_consistent(card->dev,
+					COMMAND_BUFFER_SIZE8,
+					card->ctrl[i].cmd.buf,
+					card->ctrl[i].cmd.dma_addr);
+	}
+failed_hw_setup:
+	rsxx_eeh_failure(dev);
+	return PCI_ERS_RESULT_DISCONNECT;
+
+}
+
 /*----------------- Driver Initialization & Setup -------------------*/
 /* Returns:   0 if the driver is compatible with the device
 	     -1 if the driver is NOT compatible with the device */
@@ -383,6 +588,7 @@ static int rsxx_pci_probe(struct pci_dev *dev,
 
 	spin_lock_init(&card->irq_lock);
 	card->halt = 0;
+	card->eeh_state = 0;
 
 	spin_lock_irq(&card->irq_lock);
 	rsxx_disable_ier_and_isr(card, CR_INTR_ALL);
@@ -538,9 +744,6 @@ static void rsxx_pci_remove(struct pci_dev *dev)
 	rsxx_disable_ier_and_isr(card, CR_INTR_EVENT);
 	spin_unlock_irqrestore(&card->irq_lock, flags);
 
-	/* Prevent work_structs from re-queuing themselves. */
-	card->halt = 1;
-
 	cancel_work_sync(&card->event_work);
 
 	rsxx_destroy_dev(card);
@@ -549,6 +752,10 @@ static void rsxx_pci_remove(struct pci_dev *dev)
 	spin_lock_irqsave(&card->irq_lock, flags);
 	rsxx_disable_ier_and_isr(card, CR_INTR_ALL);
 	spin_unlock_irqrestore(&card->irq_lock, flags);
+
+	/* Prevent work_structs from re-queuing themselves. */
+	card->halt = 1;
+
 	free_irq(dev->irq, card);
 
 	if (!force_legacy)
@@ -592,11 +799,14 @@ static void rsxx_pci_shutdown(struct pci_dev *dev)
 	card_shutdown(card);
 }
 
+static const struct pci_error_handlers rsxx_err_handler = {
+	.error_detected = rsxx_error_detected,
+	.slot_reset     = rsxx_slot_reset,
+};
+
 static DEFINE_PCI_DEVICE_TABLE(rsxx_pci_ids) = {
-	{PCI_DEVICE(PCI_VENDOR_ID_TMS_IBM, PCI_DEVICE_ID_RS70_FLASH)},
-	{PCI_DEVICE(PCI_VENDOR_ID_TMS_IBM, PCI_DEVICE_ID_RS70D_FLASH)},
-	{PCI_DEVICE(PCI_VENDOR_ID_TMS_IBM, PCI_DEVICE_ID_RS80_FLASH)},
-	{PCI_DEVICE(PCI_VENDOR_ID_TMS_IBM, PCI_DEVICE_ID_RS81_FLASH)},
+	{PCI_DEVICE(PCI_VENDOR_ID_IBM, PCI_DEVICE_ID_FS70_FLASH)},
+	{PCI_DEVICE(PCI_VENDOR_ID_IBM, PCI_DEVICE_ID_FS80_FLASH)},
 	{0,},
 };
 
@@ -609,6 +819,7 @@ static struct pci_driver rsxx_pci_driver = {
 	.remove		= rsxx_pci_remove,
 	.suspend	= rsxx_pci_suspend,
 	.shutdown	= rsxx_pci_shutdown,
+	.err_handler    = &rsxx_err_handler,
 };
 
 static int __init rsxx_core_init(void)
diff --git a/drivers/block/rsxx/cregs.c b/drivers/block/rsxx/cregs.c
index 80bbe639fcc..4b5c020a0a6 100644
--- a/drivers/block/rsxx/cregs.c
+++ b/drivers/block/rsxx/cregs.c
@@ -58,7 +58,7 @@ static struct kmem_cache *creg_cmd_pool;
 #error Unknown endianess!!! Aborting...
 #endif
 
-static void copy_to_creg_data(struct rsxx_cardinfo *card,
+static int copy_to_creg_data(struct rsxx_cardinfo *card,
 			      int cnt8,
 			      void *buf,
 			      unsigned int stream)
@@ -66,6 +66,9 @@ static void copy_to_creg_data(struct rsxx_cardinfo *card,
 	int i = 0;
 	u32 *data = buf;
 
+	if (unlikely(card->eeh_state))
+		return -EIO;
+
 	for (i = 0; cnt8 > 0; i++, cnt8 -= 4) {
 		/*
 		 * Firmware implementation makes it necessary to byte swap on
@@ -76,10 +79,12 @@ static void copy_to_creg_data(struct rsxx_cardinfo *card,
 		else
 			iowrite32(data[i], card->regmap + CREG_DATA(i));
 	}
+
+	return 0;
 }
 
 
-static void copy_from_creg_data(struct rsxx_cardinfo *card,
+static int copy_from_creg_data(struct rsxx_cardinfo *card,
 				int cnt8,
 				void *buf,
 				unsigned int stream)
@@ -87,6 +92,9 @@ static void copy_from_creg_data(struct rsxx_cardinfo *card,
 	int i = 0;
 	u32 *data = buf;
 
+	if (unlikely(card->eeh_state))
+		return -EIO;
+
 	for (i = 0; cnt8 > 0; i++, cnt8 -= 4) {
 		/*
 		 * Firmware implementation makes it necessary to byte swap on
@@ -97,41 +105,31 @@ static void copy_from_creg_data(struct rsxx_cardinfo *card,
 		else
 			data[i] = ioread32(card->regmap + CREG_DATA(i));
 	}
-}
-
-static struct creg_cmd *pop_active_cmd(struct rsxx_cardinfo *card)
-{
-	struct creg_cmd *cmd;
 
-	/*
-	 * Spin lock is needed because this can be called in atomic/interrupt
-	 * context.
-	 */
-	spin_lock_bh(&card->creg_ctrl.lock);
-	cmd = card->creg_ctrl.active_cmd;
-	card->creg_ctrl.active_cmd = NULL;
-	spin_unlock_bh(&card->creg_ctrl.lock);
-
-	return cmd;
+	return 0;
 }
 
 static void creg_issue_cmd(struct rsxx_cardinfo *card, struct creg_cmd *cmd)
 {
+	int st;
+
+	if (unlikely(card->eeh_state))
+		return;
+
 	iowrite32(cmd->addr, card->regmap + CREG_ADD);
 	iowrite32(cmd->cnt8, card->regmap + CREG_CNT);
 
 	if (cmd->op == CREG_OP_WRITE) {
-		if (cmd->buf)
-			copy_to_creg_data(card, cmd->cnt8,
-					  cmd->buf, cmd->stream);
+		if (cmd->buf) {
+			st = copy_to_creg_data(card, cmd->cnt8,
+					       cmd->buf, cmd->stream);
+			if (st)
+				return;
+		}
 	}
 
-	/*
-	 * Data copy must complete before initiating the command. This is
-	 * needed for weakly ordered processors (i.e. PowerPC), so that all
-	 * neccessary registers are written before we kick the hardware.
-	 */
-	wmb();
+	if (unlikely(card->eeh_state))
+		return;
 
 	/* Setting the valid bit will kick off the command. */
 	iowrite32(cmd->op, card->regmap + CREG_CMD);
@@ -196,11 +194,11 @@ static int creg_queue_cmd(struct rsxx_cardinfo *card,
 	cmd->cb_private = cb_private;
 	cmd->status	= 0;
 
-	spin_lock(&card->creg_ctrl.lock);
+	spin_lock_bh(&card->creg_ctrl.lock);
 	list_add_tail(&cmd->list, &card->creg_ctrl.queue);
 	card->creg_ctrl.q_depth++;
 	creg_kick_queue(card);
-	spin_unlock(&card->creg_ctrl.lock);
+	spin_unlock_bh(&card->creg_ctrl.lock);
 
 	return 0;
 }
@@ -210,7 +208,11 @@ static void creg_cmd_timed_out(unsigned long data)
 	struct rsxx_cardinfo *card = (struct rsxx_cardinfo *) data;
 	struct creg_cmd *cmd;
 
-	cmd = pop_active_cmd(card);
+	spin_lock(&card->creg_ctrl.lock);
+	cmd = card->creg_ctrl.active_cmd;
+	card->creg_ctrl.active_cmd = NULL;
+	spin_unlock(&card->creg_ctrl.lock);
+
 	if (cmd == NULL) {
 		card->creg_ctrl.creg_stats.creg_timeout++;
 		dev_warn(CARD_TO_DEV(card),
@@ -247,7 +249,11 @@ static void creg_cmd_done(struct work_struct *work)
 	if (del_timer_sync(&card->creg_ctrl.cmd_timer) == 0)
 		card->creg_ctrl.creg_stats.failed_cancel_timer++;
 
-	cmd = pop_active_cmd(card);
+	spin_lock_bh(&card->creg_ctrl.lock);
+	cmd = card->creg_ctrl.active_cmd;
+	card->creg_ctrl.active_cmd = NULL;
+	spin_unlock_bh(&card->creg_ctrl.lock);
+
 	if (cmd == NULL) {
 		dev_err(CARD_TO_DEV(card),
 			"Spurious creg interrupt!\n");
@@ -287,7 +293,7 @@ static void creg_cmd_done(struct work_struct *work)
 			goto creg_done;
 		}
 
-		copy_from_creg_data(card, cnt8, cmd->buf, cmd->stream);
+		st = copy_from_creg_data(card, cnt8, cmd->buf, cmd->stream);
 	}
 
 creg_done:
@@ -296,10 +302,10 @@ creg_done:
 
 	kmem_cache_free(creg_cmd_pool, cmd);
 
-	spin_lock(&card->creg_ctrl.lock);
+	spin_lock_bh(&card->creg_ctrl.lock);
 	card->creg_ctrl.active = 0;
 	creg_kick_queue(card);
-	spin_unlock(&card->creg_ctrl.lock);
+	spin_unlock_bh(&card->creg_ctrl.lock);
 }
 
 static void creg_reset(struct rsxx_cardinfo *card)
@@ -324,7 +330,7 @@ static void creg_reset(struct rsxx_cardinfo *card)
 		"Resetting creg interface for recovery\n");
 
 	/* Cancel outstanding commands */
-	spin_lock(&card->creg_ctrl.lock);
+	spin_lock_bh(&card->creg_ctrl.lock);
 	list_for_each_entry_safe(cmd, tmp, &card->creg_ctrl.queue, list) {
 		list_del(&cmd->list);
 		card->creg_ctrl.q_depth--;
@@ -345,7 +351,7 @@ static void creg_reset(struct rsxx_cardinfo *card)
 
 		card->creg_ctrl.active = 0;
 	}
-	spin_unlock(&card->creg_ctrl.lock);
+	spin_unlock_bh(&card->creg_ctrl.lock);
 
 	card->creg_ctrl.reset = 0;
 	spin_lock_irqsave(&card->irq_lock, flags);
@@ -399,12 +405,12 @@ static int __issue_creg_rw(struct rsxx_cardinfo *card,
 		return st;
 
 	/*
-	 * This timeout is neccessary for unresponsive hardware. The additional
+	 * This timeout is necessary for unresponsive hardware. The additional
 	 * 20 seconds to used to guarantee that each cregs requests has time to
 	 * complete.
 	 */
-	timeout = msecs_to_jiffies((CREG_TIMEOUT_MSEC *
-				card->creg_ctrl.q_depth) + 20000);
+	timeout = msecs_to_jiffies(CREG_TIMEOUT_MSEC *
+				   card->creg_ctrl.q_depth + 20000);
 
 	/*
 	 * The creg interface is guaranteed to complete. It has a timeout
@@ -690,6 +696,32 @@ int rsxx_reg_access(struct rsxx_cardinfo *card,
 	return 0;
 }
 
+void rsxx_eeh_save_issued_creg(struct rsxx_cardinfo *card)
+{
+	struct creg_cmd *cmd = NULL;
+
+	cmd = card->creg_ctrl.active_cmd;
+	card->creg_ctrl.active_cmd = NULL;
+
+	if (cmd) {
+		del_timer_sync(&card->creg_ctrl.cmd_timer);
+
+		spin_lock_bh(&card->creg_ctrl.lock);
+		list_add(&cmd->list, &card->creg_ctrl.queue);
+		card->creg_ctrl.q_depth++;
+		card->creg_ctrl.active = 0;
+		spin_unlock_bh(&card->creg_ctrl.lock);
+	}
+}
+
+void rsxx_kick_creg_queue(struct rsxx_cardinfo *card)
+{
+	spin_lock_bh(&card->creg_ctrl.lock);
+	if (!list_empty(&card->creg_ctrl.queue))
+		creg_kick_queue(card);
+	spin_unlock_bh(&card->creg_ctrl.lock);
+}
+
 /*------------ Initialization & Setup --------------*/
 int rsxx_creg_setup(struct rsxx_cardinfo *card)
 {
@@ -712,7 +744,7 @@ void rsxx_creg_destroy(struct rsxx_cardinfo *card)
 	int cnt = 0;
 
 	/* Cancel outstanding commands */
-	spin_lock(&card->creg_ctrl.lock);
+	spin_lock_bh(&card->creg_ctrl.lock);
 	list_for_each_entry_safe(cmd, tmp, &card->creg_ctrl.queue, list) {
 		list_del(&cmd->list);
 		if (cmd->cb)
@@ -737,7 +769,7 @@ void rsxx_creg_destroy(struct rsxx_cardinfo *card)
 			"Canceled active creg command\n");
 		kmem_cache_free(creg_cmd_pool, cmd);
 	}
-	spin_unlock(&card->creg_ctrl.lock);
+	spin_unlock_bh(&card->creg_ctrl.lock);
 
 	cancel_work_sync(&card->creg_ctrl.done_work);
 }
diff --git a/drivers/block/rsxx/dma.c b/drivers/block/rsxx/dma.c
index 63176e67662..0607513cfb4 100644
--- a/drivers/block/rsxx/dma.c
+++ b/drivers/block/rsxx/dma.c
@@ -28,7 +28,7 @@
 struct rsxx_dma {
 	struct list_head	 list;
 	u8			 cmd;
-	unsigned int		 laddr;     /* Logical address on the ramsan */
+	unsigned int		 laddr;     /* Logical address */
 	struct {
 		u32		 off;
 		u32		 cnt;
@@ -81,9 +81,6 @@ enum rsxx_hw_status {
 	HW_STATUS_FAULT		= 0x08,
 };
 
-#define STATUS_BUFFER_SIZE8     4096
-#define COMMAND_BUFFER_SIZE8    4096
-
 static struct kmem_cache *rsxx_dma_pool;
 
 struct dma_tracker {
@@ -122,7 +119,7 @@ static unsigned int rsxx_get_dma_tgt(struct rsxx_cardinfo *card, u64 addr8)
 	return tgt;
 }
 
-static void rsxx_dma_queue_reset(struct rsxx_cardinfo *card)
+void rsxx_dma_queue_reset(struct rsxx_cardinfo *card)
 {
 	/* Reset all DMA Command/Status Queues */
 	iowrite32(DMA_QUEUE_RESET, card->regmap + RESET);
@@ -210,7 +207,8 @@ static void dma_intr_coal_auto_tune(struct rsxx_cardinfo *card)
 	u32 q_depth = 0;
 	u32 intr_coal;
 
-	if (card->config.data.intr_coal.mode != RSXX_INTR_COAL_AUTO_TUNE)
+	if (card->config.data.intr_coal.mode != RSXX_INTR_COAL_AUTO_TUNE ||
+	    unlikely(card->eeh_state))
 		return;
 
 	for (i = 0; i < card->n_targets; i++)
@@ -223,31 +221,26 @@ static void dma_intr_coal_auto_tune(struct rsxx_cardinfo *card)
 }
 
 /*----------------- RSXX DMA Handling -------------------*/
-static void rsxx_complete_dma(struct rsxx_cardinfo *card,
+static void rsxx_complete_dma(struct rsxx_dma_ctrl *ctrl,
 				  struct rsxx_dma *dma,
 				  unsigned int status)
 {
 	if (status & DMA_SW_ERR)
-		printk_ratelimited(KERN_ERR
-				   "SW Error in DMA(cmd x%02x, laddr x%08x)\n",
-				   dma->cmd, dma->laddr);
+		ctrl->stats.dma_sw_err++;
 	if (status & DMA_HW_FAULT)
-		printk_ratelimited(KERN_ERR
-				   "HW Fault in DMA(cmd x%02x, laddr x%08x)\n",
-				   dma->cmd, dma->laddr);
+		ctrl->stats.dma_hw_fault++;
 	if (status & DMA_CANCELLED)
-		printk_ratelimited(KERN_ERR
-				   "DMA Cancelled(cmd x%02x, laddr x%08x)\n",
-				   dma->cmd, dma->laddr);
+		ctrl->stats.dma_cancelled++;
 
 	if (dma->dma_addr)
-		pci_unmap_page(card->dev, dma->dma_addr, get_dma_size(dma),
+		pci_unmap_page(ctrl->card->dev, dma->dma_addr,
+			       get_dma_size(dma),
 			       dma->cmd == HW_CMD_BLK_WRITE ?
 					   PCI_DMA_TODEVICE :
 					   PCI_DMA_FROMDEVICE);
 
 	if (dma->cb)
-		dma->cb(card, dma->cb_data, status ? 1 : 0);
+		dma->cb(ctrl->card, dma->cb_data, status ? 1 : 0);
 
 	kmem_cache_free(rsxx_dma_pool, dma);
 }
@@ -330,14 +323,15 @@ static void rsxx_handle_dma_error(struct rsxx_dma_ctrl *ctrl,
 	if (requeue_cmd)
 		rsxx_requeue_dma(ctrl, dma);
 	else
-		rsxx_complete_dma(ctrl->card, dma, status);
+		rsxx_complete_dma(ctrl, dma, status);
 }
 
 static void dma_engine_stalled(unsigned long data)
 {
 	struct rsxx_dma_ctrl *ctrl = (struct rsxx_dma_ctrl *)data;
 
-	if (atomic_read(&ctrl->stats.hw_q_depth) == 0)
+	if (atomic_read(&ctrl->stats.hw_q_depth) == 0 ||
+	    unlikely(ctrl->card->eeh_state))
 		return;
 
 	if (ctrl->cmd.idx != ioread32(ctrl->regmap + SW_CMD_IDX)) {
@@ -369,7 +363,8 @@ static void rsxx_issue_dmas(struct work_struct *work)
 	ctrl = container_of(work, struct rsxx_dma_ctrl, issue_dma_work);
 	hw_cmd_buf = ctrl->cmd.buf;
 
-	if (unlikely(ctrl->card->halt))
+	if (unlikely(ctrl->card->halt) ||
+	    unlikely(ctrl->card->eeh_state))
 		return;
 
 	while (1) {
@@ -397,7 +392,7 @@ static void rsxx_issue_dmas(struct work_struct *work)
 		 */
 		if (unlikely(ctrl->card->dma_fault)) {
 			push_tracker(ctrl->trackers, tag);
-			rsxx_complete_dma(ctrl->card, dma, DMA_CANCELLED);
+			rsxx_complete_dma(ctrl, dma, DMA_CANCELLED);
 			continue;
 		}
 
@@ -432,19 +427,15 @@ static void rsxx_issue_dmas(struct work_struct *work)
 
 	/* Let HW know we've queued commands. */
 	if (cmds_pending) {
-		/*
-		 * We must guarantee that the CPU writes to 'ctrl->cmd.buf'
-		 * (which is in PCI-consistent system-memory) from the loop
-		 * above make it into the coherency domain before the
-		 * following PIO "trigger" updating the cmd.idx.  A WMB is
-		 * sufficient. We need not explicitly CPU cache-flush since
-		 * the memory is a PCI-consistent (ie; coherent) mapping.
-		 */
-		wmb();
-
 		atomic_add(cmds_pending, &ctrl->stats.hw_q_depth);
 		mod_timer(&ctrl->activity_timer,
 			  jiffies + DMA_ACTIVITY_TIMEOUT);
+
+		if (unlikely(ctrl->card->eeh_state)) {
+			del_timer_sync(&ctrl->activity_timer);
+			return;
+		}
+
 		iowrite32(ctrl->cmd.idx, ctrl->regmap + SW_CMD_IDX);
 	}
 }
@@ -463,7 +454,8 @@ static void rsxx_dma_done(struct work_struct *work)
 	hw_st_buf = ctrl->status.buf;
 
 	if (unlikely(ctrl->card->halt) ||
-	    unlikely(ctrl->card->dma_fault))
+	    unlikely(ctrl->card->dma_fault) ||
+	    unlikely(ctrl->card->eeh_state))
 		return;
 
 	count = le16_to_cpu(hw_st_buf[ctrl->status.idx].count);
@@ -508,7 +500,7 @@ static void rsxx_dma_done(struct work_struct *work)
 		if (status)
 			rsxx_handle_dma_error(ctrl, dma, status);
 		else
-			rsxx_complete_dma(ctrl->card, dma, 0);
+			rsxx_complete_dma(ctrl, dma, 0);
 
 		push_tracker(ctrl->trackers, tag);
 
@@ -727,20 +719,54 @@ bvec_err:
 
 
 /*----------------- DMA Engine Initialization & Setup -------------------*/
+int rsxx_hw_buffers_init(struct pci_dev *dev, struct rsxx_dma_ctrl *ctrl)
+{
+	ctrl->status.buf = pci_alloc_consistent(dev, STATUS_BUFFER_SIZE8,
+				&ctrl->status.dma_addr);
+	ctrl->cmd.buf = pci_alloc_consistent(dev, COMMAND_BUFFER_SIZE8,
+				&ctrl->cmd.dma_addr);
+	if (ctrl->status.buf == NULL || ctrl->cmd.buf == NULL)
+		return -ENOMEM;
+
+	memset(ctrl->status.buf, 0xac, STATUS_BUFFER_SIZE8);
+	iowrite32(lower_32_bits(ctrl->status.dma_addr),
+		ctrl->regmap + SB_ADD_LO);
+	iowrite32(upper_32_bits(ctrl->status.dma_addr),
+		ctrl->regmap + SB_ADD_HI);
+
+	memset(ctrl->cmd.buf, 0x83, COMMAND_BUFFER_SIZE8);
+	iowrite32(lower_32_bits(ctrl->cmd.dma_addr), ctrl->regmap + CB_ADD_LO);
+	iowrite32(upper_32_bits(ctrl->cmd.dma_addr), ctrl->regmap + CB_ADD_HI);
+
+	ctrl->status.idx = ioread32(ctrl->regmap + HW_STATUS_CNT);
+	if (ctrl->status.idx > RSXX_MAX_OUTSTANDING_CMDS) {
+		dev_crit(&dev->dev, "Failed reading status cnt x%x\n",
+			ctrl->status.idx);
+		return -EINVAL;
+	}
+	iowrite32(ctrl->status.idx, ctrl->regmap + HW_STATUS_CNT);
+	iowrite32(ctrl->status.idx, ctrl->regmap + SW_STATUS_CNT);
+
+	ctrl->cmd.idx = ioread32(ctrl->regmap + HW_CMD_IDX);
+	if (ctrl->cmd.idx > RSXX_MAX_OUTSTANDING_CMDS) {
+		dev_crit(&dev->dev, "Failed reading cmd cnt x%x\n",
+			ctrl->status.idx);
+		return -EINVAL;
+	}
+	iowrite32(ctrl->cmd.idx, ctrl->regmap + HW_CMD_IDX);
+	iowrite32(ctrl->cmd.idx, ctrl->regmap + SW_CMD_IDX);
+
+	return 0;
+}
+
 static int rsxx_dma_ctrl_init(struct pci_dev *dev,
 				  struct rsxx_dma_ctrl *ctrl)
 {
 	int i;
+	int st;
 
 	memset(&ctrl->stats, 0, sizeof(ctrl->stats));
 
-	ctrl->status.buf = pci_alloc_consistent(dev, STATUS_BUFFER_SIZE8,
-						&ctrl->status.dma_addr);
-	ctrl->cmd.buf = pci_alloc_consistent(dev, COMMAND_BUFFER_SIZE8,
-					     &ctrl->cmd.dma_addr);
-	if (ctrl->status.buf == NULL || ctrl->cmd.buf == NULL)
-		return -ENOMEM;
-
 	ctrl->trackers = vmalloc(DMA_TRACKER_LIST_SIZE8);
 	if (!ctrl->trackers)
 		return -ENOMEM;
@@ -770,35 +796,9 @@ static int rsxx_dma_ctrl_init(struct pci_dev *dev,
 	INIT_WORK(&ctrl->issue_dma_work, rsxx_issue_dmas);
 	INIT_WORK(&ctrl->dma_done_work, rsxx_dma_done);
 
-	memset(ctrl->status.buf, 0xac, STATUS_BUFFER_SIZE8);
-	iowrite32(lower_32_bits(ctrl->status.dma_addr),
-		  ctrl->regmap + SB_ADD_LO);
-	iowrite32(upper_32_bits(ctrl->status.dma_addr),
-		  ctrl->regmap + SB_ADD_HI);
-
-	memset(ctrl->cmd.buf, 0x83, COMMAND_BUFFER_SIZE8);
-	iowrite32(lower_32_bits(ctrl->cmd.dma_addr), ctrl->regmap + CB_ADD_LO);
-	iowrite32(upper_32_bits(ctrl->cmd.dma_addr), ctrl->regmap + CB_ADD_HI);
-
-	ctrl->status.idx = ioread32(ctrl->regmap + HW_STATUS_CNT);
-	if (ctrl->status.idx > RSXX_MAX_OUTSTANDING_CMDS) {
-		dev_crit(&dev->dev, "Failed reading status cnt x%x\n",
-			 ctrl->status.idx);
-		return -EINVAL;
-	}
-	iowrite32(ctrl->status.idx, ctrl->regmap + HW_STATUS_CNT);
-	iowrite32(ctrl->status.idx, ctrl->regmap + SW_STATUS_CNT);
-
-	ctrl->cmd.idx = ioread32(ctrl->regmap + HW_CMD_IDX);
-	if (ctrl->cmd.idx > RSXX_MAX_OUTSTANDING_CMDS) {
-		dev_crit(&dev->dev, "Failed reading cmd cnt x%x\n",
-			 ctrl->status.idx);
-		return -EINVAL;
-	}
-	iowrite32(ctrl->cmd.idx, ctrl->regmap + HW_CMD_IDX);
-	iowrite32(ctrl->cmd.idx, ctrl->regmap + SW_CMD_IDX);
-
-	wmb();
+	st = rsxx_hw_buffers_init(dev, ctrl);
+	if (st)
+		return st;
 
 	return 0;
 }
@@ -834,7 +834,7 @@ static int rsxx_dma_stripe_setup(struct rsxx_cardinfo *card,
 	return 0;
 }
 
-static int rsxx_dma_configure(struct rsxx_cardinfo *card)
+int rsxx_dma_configure(struct rsxx_cardinfo *card)
 {
 	u32 intr_coal;
 
@@ -980,6 +980,103 @@ void rsxx_dma_destroy(struct rsxx_cardinfo *card)
 	}
 }
 
+int rsxx_eeh_save_issued_dmas(struct rsxx_cardinfo *card)
+{
+	int i;
+	int j;
+	int cnt;
+	struct rsxx_dma *dma;
+	struct list_head *issued_dmas;
+
+	issued_dmas = kzalloc(sizeof(*issued_dmas) * card->n_targets,
+			      GFP_KERNEL);
+	if (!issued_dmas)
+		return -ENOMEM;
+
+	for (i = 0; i < card->n_targets; i++) {
+		INIT_LIST_HEAD(&issued_dmas[i]);
+		cnt = 0;
+		for (j = 0; j < RSXX_MAX_OUTSTANDING_CMDS; j++) {
+			dma = get_tracker_dma(card->ctrl[i].trackers, j);
+			if (dma == NULL)
+				continue;
+
+			if (dma->cmd == HW_CMD_BLK_WRITE)
+				card->ctrl[i].stats.writes_issued--;
+			else if (dma->cmd == HW_CMD_BLK_DISCARD)
+				card->ctrl[i].stats.discards_issued--;
+			else
+				card->ctrl[i].stats.reads_issued--;
+
+			list_add_tail(&dma->list, &issued_dmas[i]);
+			push_tracker(card->ctrl[i].trackers, j);
+			cnt++;
+		}
+
+		spin_lock(&card->ctrl[i].queue_lock);
+		list_splice(&issued_dmas[i], &card->ctrl[i].queue);
+
+		atomic_sub(cnt, &card->ctrl[i].stats.hw_q_depth);
+		card->ctrl[i].stats.sw_q_depth += cnt;
+		card->ctrl[i].e_cnt = 0;
+
+		list_for_each_entry(dma, &card->ctrl[i].queue, list) {
+			if (dma->dma_addr)
+				pci_unmap_page(card->dev, dma->dma_addr,
+					       get_dma_size(dma),
+					       dma->cmd == HW_CMD_BLK_WRITE ?
+					       PCI_DMA_TODEVICE :
+					       PCI_DMA_FROMDEVICE);
+		}
+		spin_unlock(&card->ctrl[i].queue_lock);
+	}
+
+	kfree(issued_dmas);
+
+	return 0;
+}
+
+void rsxx_eeh_cancel_dmas(struct rsxx_cardinfo *card)
+{
+	struct rsxx_dma *dma;
+	struct rsxx_dma *tmp;
+	int i;
+
+	for (i = 0; i < card->n_targets; i++) {
+		spin_lock(&card->ctrl[i].queue_lock);
+		list_for_each_entry_safe(dma, tmp, &card->ctrl[i].queue, list) {
+			list_del(&dma->list);
+
+			rsxx_complete_dma(&card->ctrl[i], dma, DMA_CANCELLED);
+		}
+		spin_unlock(&card->ctrl[i].queue_lock);
+	}
+}
+
+int rsxx_eeh_remap_dmas(struct rsxx_cardinfo *card)
+{
+	struct rsxx_dma *dma;
+	int i;
+
+	for (i = 0; i < card->n_targets; i++) {
+		spin_lock(&card->ctrl[i].queue_lock);
+		list_for_each_entry(dma, &card->ctrl[i].queue, list) {
+			dma->dma_addr = pci_map_page(card->dev, dma->page,
+					dma->pg_off, get_dma_size(dma),
+					dma->cmd == HW_CMD_BLK_WRITE ?
+					PCI_DMA_TODEVICE :
+					PCI_DMA_FROMDEVICE);
+			if (!dma->dma_addr) {
+				spin_unlock(&card->ctrl[i].queue_lock);
+				kmem_cache_free(rsxx_dma_pool, dma);
+				return -ENOMEM;
+			}
+		}
+		spin_unlock(&card->ctrl[i].queue_lock);
+	}
+
+	return 0;
+}
 
 int rsxx_dma_init(void)
 {
diff --git a/drivers/block/rsxx/rsxx.h b/drivers/block/rsxx/rsxx.h
index 2e50b65902b..24ba3642bd8 100644
--- a/drivers/block/rsxx/rsxx.h
+++ b/drivers/block/rsxx/rsxx.h
@@ -27,15 +27,17 @@
 
 /*----------------- IOCTL Definitions -------------------*/
 
+#define RSXX_MAX_DATA 8
+
 struct rsxx_reg_access {
 	__u32 addr;
 	__u32 cnt;
 	__u32 stat;
 	__u32 stream;
-	__u32 data[8];
+	__u32 data[RSXX_MAX_DATA];
 };
 
-#define RSXX_MAX_REG_CNT	(8 * (sizeof(__u32)))
+#define RSXX_MAX_REG_CNT	(RSXX_MAX_DATA * (sizeof(__u32)))
 
 #define RSXX_IOC_MAGIC 'r'
 
diff --git a/drivers/block/rsxx/rsxx_cfg.h b/drivers/block/rsxx/rsxx_cfg.h
index c025fe5fdb7..f384c943846 100644
--- a/drivers/block/rsxx/rsxx_cfg.h
+++ b/drivers/block/rsxx/rsxx_cfg.h
@@ -58,7 +58,7 @@ struct rsxx_card_cfg {
 };
 
 /* Vendor ID Values */
-#define RSXX_VENDOR_ID_TMS_IBM		0
+#define RSXX_VENDOR_ID_IBM		0
 #define RSXX_VENDOR_ID_DSI		1
 #define RSXX_VENDOR_COUNT		2
 
diff --git a/drivers/block/rsxx/rsxx_priv.h b/drivers/block/rsxx/rsxx_priv.h
index a1ac907d8f4..382e8bf5c03 100644
--- a/drivers/block/rsxx/rsxx_priv.h
+++ b/drivers/block/rsxx/rsxx_priv.h
@@ -45,16 +45,13 @@
 
 struct proc_cmd;
 
-#define PCI_VENDOR_ID_TMS_IBM		0x15B6
-#define PCI_DEVICE_ID_RS70_FLASH	0x0019
-#define PCI_DEVICE_ID_RS70D_FLASH	0x001A
-#define PCI_DEVICE_ID_RS80_FLASH	0x001C
-#define PCI_DEVICE_ID_RS81_FLASH	0x001E
+#define PCI_DEVICE_ID_FS70_FLASH	0x04A9
+#define PCI_DEVICE_ID_FS80_FLASH	0x04AA
 
 #define RS70_PCI_REV_SUPPORTED	4
 
 #define DRIVER_NAME "rsxx"
-#define DRIVER_VERSION "3.7"
+#define DRIVER_VERSION "4.0"
 
 /* Block size is 4096 */
 #define RSXX_HW_BLK_SHIFT		12
@@ -67,6 +64,9 @@ struct proc_cmd;
 #define RSXX_MAX_OUTSTANDING_CMDS	255
 #define RSXX_CS_IDX_MASK		0xff
 
+#define STATUS_BUFFER_SIZE8     4096
+#define COMMAND_BUFFER_SIZE8    4096
+
 #define RSXX_MAX_TARGETS	8
 
 struct dma_tracker_list;
@@ -91,6 +91,9 @@ struct rsxx_dma_stats {
 	u32 discards_failed;
 	u32 done_rescheduled;
 	u32 issue_rescheduled;
+	u32 dma_sw_err;
+	u32 dma_hw_fault;
+	u32 dma_cancelled;
 	u32 sw_q_depth;		/* Number of DMAs on the SW queue. */
 	atomic_t hw_q_depth;	/* Number of DMAs queued to HW. */
 };
@@ -116,6 +119,7 @@ struct rsxx_dma_ctrl {
 struct rsxx_cardinfo {
 	struct pci_dev		*dev;
 	unsigned int		halt;
+	unsigned int		eeh_state;
 
 	void			__iomem *regmap;
 	spinlock_t		irq_lock;
@@ -224,6 +228,7 @@ enum rsxx_pci_regmap {
 	PERF_RD512_HI	= 0xac,
 	PERF_WR512_LO	= 0xb0,
 	PERF_WR512_HI	= 0xb4,
+	PCI_RECONFIG	= 0xb8,
 };
 
 enum rsxx_intr {
@@ -237,6 +242,8 @@ enum rsxx_intr {
 	CR_INTR_DMA5	= 0x00000080,
 	CR_INTR_DMA6	= 0x00000100,
 	CR_INTR_DMA7	= 0x00000200,
+	CR_INTR_ALL_C	= 0x0000003f,
+	CR_INTR_ALL_G	= 0x000003ff,
 	CR_INTR_DMA_ALL = 0x000003f5,
 	CR_INTR_ALL	= 0xffffffff,
 };
@@ -253,8 +260,14 @@ enum rsxx_pci_reset {
 	DMA_QUEUE_RESET		= 0x00000001,
 };
 
+enum rsxx_hw_fifo_flush {
+	RSXX_FLUSH_BUSY		= 0x00000002,
+	RSXX_FLUSH_TIMEOUT	= 0x00000004,
+};
+
 enum rsxx_pci_revision {
 	RSXX_DISCARD_SUPPORT = 2,
+	RSXX_EEH_SUPPORT     = 3,
 };
 
 enum rsxx_creg_cmd {
@@ -360,11 +373,17 @@ int rsxx_dma_setup(struct rsxx_cardinfo *card);
 void rsxx_dma_destroy(struct rsxx_cardinfo *card);
 int rsxx_dma_init(void);
 void rsxx_dma_cleanup(void);
+void rsxx_dma_queue_reset(struct rsxx_cardinfo *card);
+int rsxx_dma_configure(struct rsxx_cardinfo *card);
 int rsxx_dma_queue_bio(struct rsxx_cardinfo *card,
 			   struct bio *bio,
 			   atomic_t *n_dmas,
 			   rsxx_dma_cb cb,
 			   void *cb_data);
+int rsxx_hw_buffers_init(struct pci_dev *dev, struct rsxx_dma_ctrl *ctrl);
+int rsxx_eeh_save_issued_dmas(struct rsxx_cardinfo *card);
+void rsxx_eeh_cancel_dmas(struct rsxx_cardinfo *card);
+int rsxx_eeh_remap_dmas(struct rsxx_cardinfo *card);
 
 /***** cregs.c *****/
 int rsxx_creg_write(struct rsxx_cardinfo *card, u32 addr,
@@ -389,10 +408,11 @@ int rsxx_creg_setup(struct rsxx_cardinfo *card);
 void rsxx_creg_destroy(struct rsxx_cardinfo *card);
 int rsxx_creg_init(void);
 void rsxx_creg_cleanup(void);
-
 int rsxx_reg_access(struct rsxx_cardinfo *card,
 			struct rsxx_reg_access __user *ucmd,
 			int read);
+void rsxx_eeh_save_issued_creg(struct rsxx_cardinfo *card);
+void rsxx_kick_creg_queue(struct rsxx_cardinfo *card);
 
 
 
diff --git a/drivers/block/xen-blkback/blkback.c b/drivers/block/xen-blkback/blkback.c
index de1f319f7bd..dd5b2fed97e 100644
--- a/drivers/block/xen-blkback/blkback.c
+++ b/drivers/block/xen-blkback/blkback.c
@@ -164,7 +164,7 @@ static void make_response(struct xen_blkif *blkif, u64 id,
 
 #define foreach_grant_safe(pos, n, rbtree, node) \
 	for ((pos) = container_of(rb_first((rbtree)), typeof(*(pos)), node), \
-	     (n) = rb_next(&(pos)->node); \
+	     (n) = (&(pos)->node != NULL) ? rb_next(&(pos)->node) : NULL; \
 	     &(pos)->node != NULL; \
 	     (pos) = container_of(n, typeof(*(pos)), node), \
 	     (n) = (&(pos)->node != NULL) ? rb_next(&(pos)->node) : NULL)
@@ -381,8 +381,8 @@ irqreturn_t xen_blkif_be_int(int irq, void *dev_id)
 
 static void print_stats(struct xen_blkif *blkif)
 {
-	pr_info("xen-blkback (%s): oo %3d  |  rd %4d  |  wr %4d  |  f %4d"
-		 "  |  ds %4d\n",
+	pr_info("xen-blkback (%s): oo %3llu  |  rd %4llu  |  wr %4llu  |  f %4llu"
+		 "  |  ds %4llu\n",
 		 current->comm, blkif->st_oo_req,
 		 blkif->st_rd_req, blkif->st_wr_req,
 		 blkif->st_f_req, blkif->st_ds_req);
@@ -442,7 +442,7 @@ int xen_blkif_schedule(void *arg)
 }
 
 struct seg_buf {
-	unsigned long buf;
+	unsigned int offset;
 	unsigned int nsec;
 };
 /*
@@ -621,30 +621,21 @@ static int xen_blkbk_map(struct blkif_request *req,
 				 * If this is a new persistent grant
 				 * save the handler
 				 */
-				persistent_gnts[i]->handle = map[j].handle;
-				persistent_gnts[i]->dev_bus_addr =
-					map[j++].dev_bus_addr;
+				persistent_gnts[i]->handle = map[j++].handle;
 			}
 			pending_handle(pending_req, i) =
 				persistent_gnts[i]->handle;
 
 			if (ret)
 				continue;
-
-			seg[i].buf = persistent_gnts[i]->dev_bus_addr |
-				(req->u.rw.seg[i].first_sect << 9);
 		} else {
-			pending_handle(pending_req, i) = map[j].handle;
+			pending_handle(pending_req, i) = map[j++].handle;
 			bitmap_set(pending_req->unmap_seg, i, 1);
 
-			if (ret) {
-				j++;
+			if (ret)
 				continue;
-			}
-
-			seg[i].buf = map[j++].dev_bus_addr |
-				(req->u.rw.seg[i].first_sect << 9);
 		}
+		seg[i].offset = (req->u.rw.seg[i].first_sect << 9);
 	}
 	return ret;
 }
@@ -679,6 +670,16 @@ static int dispatch_discard_io(struct xen_blkif *blkif,
 	return err;
 }
 
+static int dispatch_other_io(struct xen_blkif *blkif,
+			     struct blkif_request *req,
+			     struct pending_req *pending_req)
+{
+	free_req(pending_req);
+	make_response(blkif, req->u.other.id, req->operation,
+		      BLKIF_RSP_EOPNOTSUPP);
+	return -EIO;
+}
+
 static void xen_blk_drain_io(struct xen_blkif *blkif)
 {
 	atomic_set(&blkif->drain, 1);
@@ -800,17 +801,30 @@ __do_block_io_op(struct xen_blkif *blkif)
 
 		/* Apply all sanity checks to /private copy/ of request. */
 		barrier();
-		if (unlikely(req.operation == BLKIF_OP_DISCARD)) {
+
+		switch (req.operation) {
+		case BLKIF_OP_READ:
+		case BLKIF_OP_WRITE:
+		case BLKIF_OP_WRITE_BARRIER:
+		case BLKIF_OP_FLUSH_DISKCACHE:
+			if (dispatch_rw_block_io(blkif, &req, pending_req))
+				goto done;
+			break;
+		case BLKIF_OP_DISCARD:
 			free_req(pending_req);
 			if (dispatch_discard_io(blkif, &req))
-				break;
-		} else if (dispatch_rw_block_io(blkif, &req, pending_req))
+				goto done;
 			break;
+		default:
+			if (dispatch_other_io(blkif, &req, pending_req))
+				goto done;
+			break;
+		}
 
 		/* Yield point for this unbounded loop. */
 		cond_resched();
 	}
-
+done:
 	return more_to_do;
 }
 
@@ -904,7 +918,8 @@ static int dispatch_rw_block_io(struct xen_blkif *blkif,
 		pr_debug(DRV_PFX "access denied: %s of [%llu,%llu] on dev=%04x\n",
 			 operation == READ ? "read" : "write",
 			 preq.sector_number,
-			 preq.sector_number + preq.nr_sects, preq.dev);
+			 preq.sector_number + preq.nr_sects,
+			 blkif->vbd.pdevice);
 		goto fail_response;
 	}
 
@@ -947,7 +962,7 @@ static int dispatch_rw_block_io(struct xen_blkif *blkif,
 		       (bio_add_page(bio,
 				     pages[i],
 				     seg[i].nsec << 9,
-				     seg[i].buf & ~PAGE_MASK) == 0)) {
+				     seg[i].offset) == 0)) {
 
 			bio = bio_alloc(GFP_KERNEL, nseg-i);
 			if (unlikely(bio == NULL))
@@ -977,13 +992,7 @@ static int dispatch_rw_block_io(struct xen_blkif *blkif,
 		bio->bi_end_io  = end_block_io_op;
 	}
 
-	/*
-	 * We set it one so that the last submit_bio does not have to call
-	 * atomic_inc.
-	 */
 	atomic_set(&pending_req->pendcnt, nbio);
-
-	/* Get a reference count for the disk queue and start sending I/O */
 	blk_start_plug(&plug);
 
 	for (i = 0; i < nbio; i++)
@@ -1011,6 +1020,7 @@ static int dispatch_rw_block_io(struct xen_blkif *blkif,
  fail_put_bio:
 	for (i = 0; i < nbio; i++)
 		bio_put(biolist[i]);
+	atomic_set(&pending_req->pendcnt, 1);
 	__end_block_io_op(pending_req, -EINVAL);
 	msleep(1); /* back off a bit */
 	return -EIO;
diff --git a/drivers/block/xen-blkback/common.h b/drivers/block/xen-blkback/common.h
index 6072390c7f5..60103e2517b 100644
--- a/drivers/block/xen-blkback/common.h
+++ b/drivers/block/xen-blkback/common.h
@@ -77,11 +77,18 @@ struct blkif_x86_32_request_discard {
 	uint64_t       nr_sectors;
 } __attribute__((__packed__));
 
+struct blkif_x86_32_request_other {
+	uint8_t        _pad1;
+	blkif_vdev_t   _pad2;
+	uint64_t       id;           /* private guest value, echoed in resp  */
+} __attribute__((__packed__));
+
 struct blkif_x86_32_request {
 	uint8_t        operation;    /* BLKIF_OP_???                         */
 	union {
 		struct blkif_x86_32_request_rw rw;
 		struct blkif_x86_32_request_discard discard;
+		struct blkif_x86_32_request_other other;
 	} u;
 } __attribute__((__packed__));
 
@@ -113,11 +120,19 @@ struct blkif_x86_64_request_discard {
 	uint64_t       nr_sectors;
 } __attribute__((__packed__));
 
+struct blkif_x86_64_request_other {
+	uint8_t        _pad1;
+	blkif_vdev_t   _pad2;
+	uint32_t       _pad3;        /* offsetof(blkif_..,u.discard.id)==8   */
+	uint64_t       id;           /* private guest value, echoed in resp  */
+} __attribute__((__packed__));
+
 struct blkif_x86_64_request {
 	uint8_t        operation;    /* BLKIF_OP_???                         */
 	union {
 		struct blkif_x86_64_request_rw rw;
 		struct blkif_x86_64_request_discard discard;
+		struct blkif_x86_64_request_other other;
 	} u;
 } __attribute__((__packed__));
 
@@ -172,7 +187,6 @@ struct persistent_gnt {
 	struct page *page;
 	grant_ref_t gnt;
 	grant_handle_t handle;
-	uint64_t dev_bus_addr;
 	struct rb_node node;
 };
 
@@ -208,13 +222,13 @@ struct xen_blkif {
 
 	/* statistics */
 	unsigned long		st_print;
-	int			st_rd_req;
-	int			st_wr_req;
-	int			st_oo_req;
-	int			st_f_req;
-	int			st_ds_req;
-	int			st_rd_sect;
-	int			st_wr_sect;
+	unsigned long long			st_rd_req;
+	unsigned long long			st_wr_req;
+	unsigned long long			st_oo_req;
+	unsigned long long			st_f_req;
+	unsigned long long			st_ds_req;
+	unsigned long long			st_rd_sect;
+	unsigned long long			st_wr_sect;
 
 	wait_queue_head_t	waiting_to_free;
 };
@@ -278,6 +292,11 @@ static inline void blkif_get_x86_32_req(struct blkif_request *dst,
 		dst->u.discard.nr_sectors = src->u.discard.nr_sectors;
 		break;
 	default:
+		/*
+		 * Don't know how to translate this op. Only get the
+		 * ID so failure can be reported to the frontend.
+		 */
+		dst->u.other.id = src->u.other.id;
 		break;
 	}
 }
@@ -309,6 +328,11 @@ static inline void blkif_get_x86_64_req(struct blkif_request *dst,
 		dst->u.discard.nr_sectors = src->u.discard.nr_sectors;
 		break;
 	default:
+		/*
+		 * Don't know how to translate this op. Only get the
+		 * ID so failure can be reported to the frontend.
+		 */
+		dst->u.other.id = src->u.other.id;
 		break;
 	}
 }
diff --git a/drivers/block/xen-blkback/xenbus.c b/drivers/block/xen-blkback/xenbus.c
index 5e237f630c4..8bfd1bcf95e 100644
--- a/drivers/block/xen-blkback/xenbus.c
+++ b/drivers/block/xen-blkback/xenbus.c
@@ -230,13 +230,13 @@ int __init xen_blkif_interface_init(void)
 	}								\
 	static DEVICE_ATTR(name, S_IRUGO, show_##name, NULL)
 
-VBD_SHOW(oo_req,  "%d\n", be->blkif->st_oo_req);
-VBD_SHOW(rd_req,  "%d\n", be->blkif->st_rd_req);
-VBD_SHOW(wr_req,  "%d\n", be->blkif->st_wr_req);
-VBD_SHOW(f_req,  "%d\n", be->blkif->st_f_req);
-VBD_SHOW(ds_req,  "%d\n", be->blkif->st_ds_req);
-VBD_SHOW(rd_sect, "%d\n", be->blkif->st_rd_sect);
-VBD_SHOW(wr_sect, "%d\n", be->blkif->st_wr_sect);
+VBD_SHOW(oo_req,  "%llu\n", be->blkif->st_oo_req);
+VBD_SHOW(rd_req,  "%llu\n", be->blkif->st_rd_req);
+VBD_SHOW(wr_req,  "%llu\n", be->blkif->st_wr_req);
+VBD_SHOW(f_req,  "%llu\n", be->blkif->st_f_req);
+VBD_SHOW(ds_req,  "%llu\n", be->blkif->st_ds_req);
+VBD_SHOW(rd_sect, "%llu\n", be->blkif->st_rd_sect);
+VBD_SHOW(wr_sect, "%llu\n", be->blkif->st_wr_sect);
 
 static struct attribute *xen_vbdstat_attrs[] = {
 	&dev_attr_oo_req.attr,
diff --git a/drivers/block/xen-blkfront.c b/drivers/block/xen-blkfront.c
index c3dae2e0f29..a894f88762d 100644
--- a/drivers/block/xen-blkfront.c
+++ b/drivers/block/xen-blkfront.c
@@ -44,7 +44,7 @@
 #include <linux/mutex.h>
 #include <linux/scatterlist.h>
 #include <linux/bitmap.h>
-#include <linux/llist.h>
+#include <linux/list.h>
 
 #include <xen/xen.h>
 #include <xen/xenbus.h>
@@ -68,13 +68,12 @@ enum blkif_state {
 struct grant {
 	grant_ref_t gref;
 	unsigned long pfn;
-	struct llist_node node;
+	struct list_head node;
 };
 
 struct blk_shadow {
 	struct blkif_request req;
 	struct request *request;
-	unsigned long frame[BLKIF_MAX_SEGMENTS_PER_REQUEST];
 	struct grant *grants_used[BLKIF_MAX_SEGMENTS_PER_REQUEST];
 };
 
@@ -105,7 +104,7 @@ struct blkfront_info
 	struct work_struct work;
 	struct gnttab_free_callback callback;
 	struct blk_shadow shadow[BLK_RING_SIZE];
-	struct llist_head persistent_gnts;
+	struct list_head persistent_gnts;
 	unsigned int persistent_gnts_c;
 	unsigned long shadow_free;
 	unsigned int feature_flush;
@@ -165,6 +164,69 @@ static int add_id_to_freelist(struct blkfront_info *info,
 	return 0;
 }
 
+static int fill_grant_buffer(struct blkfront_info *info, int num)
+{
+	struct page *granted_page;
+	struct grant *gnt_list_entry, *n;
+	int i = 0;
+
+	while(i < num) {
+		gnt_list_entry = kzalloc(sizeof(struct grant), GFP_NOIO);
+		if (!gnt_list_entry)
+			goto out_of_memory;
+
+		granted_page = alloc_page(GFP_NOIO);
+		if (!granted_page) {
+			kfree(gnt_list_entry);
+			goto out_of_memory;
+		}
+
+		gnt_list_entry->pfn = page_to_pfn(granted_page);
+		gnt_list_entry->gref = GRANT_INVALID_REF;
+		list_add(&gnt_list_entry->node, &info->persistent_gnts);
+		i++;
+	}
+
+	return 0;
+
+out_of_memory:
+	list_for_each_entry_safe(gnt_list_entry, n,
+	                         &info->persistent_gnts, node) {
+		list_del(&gnt_list_entry->node);
+		__free_page(pfn_to_page(gnt_list_entry->pfn));
+		kfree(gnt_list_entry);
+		i--;
+	}
+	BUG_ON(i != 0);
+	return -ENOMEM;
+}
+
+static struct grant *get_grant(grant_ref_t *gref_head,
+                               struct blkfront_info *info)
+{
+	struct grant *gnt_list_entry;
+	unsigned long buffer_mfn;
+
+	BUG_ON(list_empty(&info->persistent_gnts));
+	gnt_list_entry = list_first_entry(&info->persistent_gnts, struct grant,
+	                                  node);
+	list_del(&gnt_list_entry->node);
+
+	if (gnt_list_entry->gref != GRANT_INVALID_REF) {
+		info->persistent_gnts_c--;
+		return gnt_list_entry;
+	}
+
+	/* Assign a gref to this page */
+	gnt_list_entry->gref = gnttab_claim_grant_reference(gref_head);
+	BUG_ON(gnt_list_entry->gref == -ENOSPC);
+	buffer_mfn = pfn_to_mfn(gnt_list_entry->pfn);
+	gnttab_grant_foreign_access_ref(gnt_list_entry->gref,
+	                                info->xbdev->otherend_id,
+	                                buffer_mfn, 0);
+	return gnt_list_entry;
+}
+
 static const char *op_name(int op)
 {
 	static const char *const names[] = {
@@ -293,7 +355,6 @@ static int blkif_ioctl(struct block_device *bdev, fmode_t mode,
 static int blkif_queue_request(struct request *req)
 {
 	struct blkfront_info *info = req->rq_disk->private_data;
-	unsigned long buffer_mfn;
 	struct blkif_request *ring_req;
 	unsigned long id;
 	unsigned int fsect, lsect;
@@ -306,7 +367,6 @@ static int blkif_queue_request(struct request *req)
 	 */
 	bool new_persistent_gnts;
 	grant_ref_t gref_head;
-	struct page *granted_page;
 	struct grant *gnt_list_entry = NULL;
 	struct scatterlist *sg;
 
@@ -370,41 +430,8 @@ static int blkif_queue_request(struct request *req)
 			fsect = sg->offset >> 9;
 			lsect = fsect + (sg->length >> 9) - 1;
 
-			if (info->persistent_gnts_c) {
-				BUG_ON(llist_empty(&info->persistent_gnts));
-				gnt_list_entry = llist_entry(
-					llist_del_first(&info->persistent_gnts),
-					struct grant, node);
-
-				ref = gnt_list_entry->gref;
-				buffer_mfn = pfn_to_mfn(gnt_list_entry->pfn);
-				info->persistent_gnts_c--;
-			} else {
-				ref = gnttab_claim_grant_reference(&gref_head);
-				BUG_ON(ref == -ENOSPC);
-
-				gnt_list_entry =
-					kmalloc(sizeof(struct grant),
-							 GFP_ATOMIC);
-				if (!gnt_list_entry)
-					return -ENOMEM;
-
-				granted_page = alloc_page(GFP_ATOMIC);
-				if (!granted_page) {
-					kfree(gnt_list_entry);
-					return -ENOMEM;
-				}
-
-				gnt_list_entry->pfn =
-					page_to_pfn(granted_page);
-				gnt_list_entry->gref = ref;
-
-				buffer_mfn = pfn_to_mfn(page_to_pfn(
-								granted_page));
-				gnttab_grant_foreign_access_ref(ref,
-					info->xbdev->otherend_id,
-					buffer_mfn, 0);
-			}
+			gnt_list_entry = get_grant(&gref_head, info);
+			ref = gnt_list_entry->gref;
 
 			info->shadow[id].grants_used[i] = gnt_list_entry;
 
@@ -435,7 +462,6 @@ static int blkif_queue_request(struct request *req)
 				kunmap_atomic(shared_data);
 			}
 
-			info->shadow[id].frame[i] = mfn_to_pfn(buffer_mfn);
 			ring_req->u.rw.seg[i] =
 					(struct blkif_request_segment) {
 						.gref       = ref,
@@ -790,9 +816,8 @@ static void blkif_restart_queue(struct work_struct *work)
 
 static void blkif_free(struct blkfront_info *info, int suspend)
 {
-	struct llist_node *all_gnts;
-	struct grant *persistent_gnt, *tmp;
-	struct llist_node *n;
+	struct grant *persistent_gnt;
+	struct grant *n;
 
 	/* Prevent new requests being issued until we fix things up. */
 	spin_lock_irq(&info->io_lock);
@@ -803,22 +828,20 @@ static void blkif_free(struct blkfront_info *info, int suspend)
 		blk_stop_queue(info->rq);
 
 	/* Remove all persistent grants */
-	if (info->persistent_gnts_c) {
-		all_gnts = llist_del_all(&info->persistent_gnts);
-		persistent_gnt = llist_entry(all_gnts, typeof(*(persistent_gnt)), node);
-		while (persistent_gnt) {
-			gnttab_end_foreign_access(persistent_gnt->gref, 0, 0UL);
+	if (!list_empty(&info->persistent_gnts)) {
+		list_for_each_entry_safe(persistent_gnt, n,
+		                         &info->persistent_gnts, node) {
+			list_del(&persistent_gnt->node);
+			if (persistent_gnt->gref != GRANT_INVALID_REF) {
+				gnttab_end_foreign_access(persistent_gnt->gref,
+				                          0, 0UL);
+				info->persistent_gnts_c--;
+			}
 			__free_page(pfn_to_page(persistent_gnt->pfn));
-			tmp = persistent_gnt;
-			n = persistent_gnt->node.next;
-			if (n)
-				persistent_gnt = llist_entry(n, typeof(*(persistent_gnt)), node);
-			else
-				persistent_gnt = NULL;
-			kfree(tmp);
+			kfree(persistent_gnt);
 		}
-		info->persistent_gnts_c = 0;
 	}
+	BUG_ON(info->persistent_gnts_c != 0);
 
 	/* No more gnttab callback work. */
 	gnttab_cancel_free_callback(&info->callback);
@@ -875,7 +898,7 @@ static void blkif_completion(struct blk_shadow *s, struct blkfront_info *info,
 	}
 	/* Add the persistent grant into the list of free grants */
 	for (i = 0; i < s->req.u.rw.nr_segments; i++) {
-		llist_add(&s->grants_used[i]->node, &info->persistent_gnts);
+		list_add(&s->grants_used[i]->node, &info->persistent_gnts);
 		info->persistent_gnts_c++;
 	}
 }
@@ -1013,6 +1036,12 @@ static int setup_blkring(struct xenbus_device *dev,
 
 	sg_init_table(info->sg, BLKIF_MAX_SEGMENTS_PER_REQUEST);
 
+	/* Allocate memory for grants */
+	err = fill_grant_buffer(info, BLK_RING_SIZE *
+	                              BLKIF_MAX_SEGMENTS_PER_REQUEST);
+	if (err)
+		goto fail;
+
 	err = xenbus_grant_ring(dev, virt_to_mfn(info->ring.sring));
 	if (err < 0) {
 		free_page((unsigned long)sring);
@@ -1171,7 +1200,7 @@ static int blkfront_probe(struct xenbus_device *dev,
 	spin_lock_init(&info->io_lock);
 	info->xbdev = dev;
 	info->vdevice = vdevice;
-	init_llist_head(&info->persistent_gnts);
+	INIT_LIST_HEAD(&info->persistent_gnts);
 	info->persistent_gnts_c = 0;
 	info->connected = BLKIF_STATE_DISCONNECTED;
 	INIT_WORK(&info->work, blkif_restart_queue);
@@ -1203,11 +1232,10 @@ static int blkif_recover(struct blkfront_info *info)
 	int j;
 
 	/* Stage 1: Make a safe copy of the shadow state. */
-	copy = kmalloc(sizeof(info->shadow),
+	copy = kmemdup(info->shadow, sizeof(info->shadow),
 		       GFP_NOIO | __GFP_REPEAT | __GFP_HIGH);
 	if (!copy)
 		return -ENOMEM;
-	memcpy(copy, info->shadow, sizeof(info->shadow));
 
 	/* Stage 2: Set up free list. */
 	memset(&info->shadow, 0, sizeof(info->shadow));
@@ -1236,7 +1264,7 @@ static int blkif_recover(struct blkfront_info *info)
 				gnttab_grant_foreign_access_ref(
 					req->u.rw.seg[j].gref,
 					info->xbdev->otherend_id,
-					pfn_to_mfn(info->shadow[req->u.rw.id].frame[j]),
+					pfn_to_mfn(copy[i].grants_used[j]->pfn),
 					0);
 		}
 		info->shadow[req->u.rw.id].req = *req;
diff --git a/drivers/bluetooth/ath3k.c b/drivers/bluetooth/ath3k.c
index b282af181b4..6aab00ef437 100644
--- a/drivers/bluetooth/ath3k.c
+++ b/drivers/bluetooth/ath3k.c
@@ -73,9 +73,11 @@ static struct usb_device_id ath3k_table[] = {
 	{ USB_DEVICE(0x03F0, 0x311D) },
 
 	/* Atheros AR3012 with sflash firmware*/
+	{ USB_DEVICE(0x0CF3, 0x0036) },
 	{ USB_DEVICE(0x0CF3, 0x3004) },
 	{ USB_DEVICE(0x0CF3, 0x3008) },
 	{ USB_DEVICE(0x0CF3, 0x311D) },
+	{ USB_DEVICE(0x0CF3, 0x817a) },
 	{ USB_DEVICE(0x13d3, 0x3375) },
 	{ USB_DEVICE(0x04CA, 0x3004) },
 	{ USB_DEVICE(0x04CA, 0x3005) },
@@ -107,9 +109,11 @@ MODULE_DEVICE_TABLE(usb, ath3k_table);
 static struct usb_device_id ath3k_blist_tbl[] = {
 
 	/* Atheros AR3012 with sflash firmware*/
+	{ USB_DEVICE(0x0CF3, 0x0036), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0cf3, 0x3004), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0cf3, 0x3008), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0cf3, 0x311D), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0CF3, 0x817a), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x13d3, 0x3375), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x3004), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x3005), .driver_info = BTUSB_ATH3012 },
diff --git a/drivers/bluetooth/bluecard_cs.c b/drivers/bluetooth/bluecard_cs.c
index 0d26851d6e4..6c3e3d43c71 100644
--- a/drivers/bluetooth/bluecard_cs.c
+++ b/drivers/bluetooth/bluecard_cs.c
@@ -934,17 +934,4 @@ static struct pcmcia_driver bluecard_driver = {
 	.remove		= bluecard_detach,
 	.id_table	= bluecard_ids,
 };
-
-static int __init init_bluecard_cs(void)
-{
-	return pcmcia_register_driver(&bluecard_driver);
-}
-
-
-static void __exit exit_bluecard_cs(void)
-{
-	pcmcia_unregister_driver(&bluecard_driver);
-}
-
-module_init(init_bluecard_cs);
-module_exit(exit_bluecard_cs);
+module_pcmcia_driver(bluecard_driver);
diff --git a/drivers/bluetooth/bt3c_cs.c b/drivers/bluetooth/bt3c_cs.c
index 7ffd3f40714..a1aaa3ba2a4 100644
--- a/drivers/bluetooth/bt3c_cs.c
+++ b/drivers/bluetooth/bt3c_cs.c
@@ -760,17 +760,4 @@ static struct pcmcia_driver bt3c_driver = {
 	.remove		= bt3c_detach,
 	.id_table	= bt3c_ids,
 };
-
-static int __init init_bt3c_cs(void)
-{
-	return pcmcia_register_driver(&bt3c_driver);
-}
-
-
-static void __exit exit_bt3c_cs(void)
-{
-	pcmcia_unregister_driver(&bt3c_driver);
-}
-
-module_init(init_bt3c_cs);
-module_exit(exit_bt3c_cs);
+module_pcmcia_driver(bt3c_driver);
diff --git a/drivers/bluetooth/btuart_cs.c b/drivers/bluetooth/btuart_cs.c
index 35a553a9061..beb262f2dc4 100644
--- a/drivers/bluetooth/btuart_cs.c
+++ b/drivers/bluetooth/btuart_cs.c
@@ -688,17 +688,4 @@ static struct pcmcia_driver btuart_driver = {
 	.remove		= btuart_detach,
 	.id_table	= btuart_ids,
 };
-
-static int __init init_btuart_cs(void)
-{
-	return pcmcia_register_driver(&btuart_driver);
-}
-
-
-static void __exit exit_btuart_cs(void)
-{
-	pcmcia_unregister_driver(&btuart_driver);
-}
-
-module_init(init_btuart_cs);
-module_exit(exit_btuart_cs);
+module_pcmcia_driver(btuart_driver);
diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c
index e547851870e..2cc5f774a29 100644
--- a/drivers/bluetooth/btusb.c
+++ b/drivers/bluetooth/btusb.c
@@ -131,9 +131,11 @@ static struct usb_device_id blacklist_table[] = {
 	{ USB_DEVICE(0x03f0, 0x311d), .driver_info = BTUSB_IGNORE },
 
 	/* Atheros 3012 with sflash firmware */
+	{ USB_DEVICE(0x0cf3, 0x0036), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0cf3, 0x3004), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0cf3, 0x3008), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0cf3, 0x311d), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0cf3, 0x817a), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x13d3, 0x3375), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x3004), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x3005), .driver_info = BTUSB_ATH3012 },
diff --git a/drivers/bluetooth/dtl1_cs.c b/drivers/bluetooth/dtl1_cs.c
index 036cb366fe6..33f3a6950c0 100644
--- a/drivers/bluetooth/dtl1_cs.c
+++ b/drivers/bluetooth/dtl1_cs.c
@@ -628,17 +628,4 @@ static struct pcmcia_driver dtl1_driver = {
 	.remove		= dtl1_detach,
 	.id_table	= dtl1_ids,
 };
-
-static int __init init_dtl1_cs(void)
-{
-	return pcmcia_register_driver(&dtl1_driver);
-}
-
-
-static void __exit exit_dtl1_cs(void)
-{
-	pcmcia_unregister_driver(&dtl1_driver);
-}
-
-module_init(init_dtl1_cs);
-module_exit(exit_dtl1_cs);
+module_pcmcia_driver(dtl1_driver);
diff --git a/drivers/char/applicom.c b/drivers/char/applicom.c
index 25373df1dcf..974321a2508 100644
--- a/drivers/char/applicom.c
+++ b/drivers/char/applicom.c
@@ -804,8 +804,8 @@ static long ac_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 
 			printk(KERN_INFO "Prom version board %d ....... V%d.%d %s",
 			       i+1,
-			       (int)(readb(apbs[IndexCard].RamIO + VERS) >> 4),
-			       (int)(readb(apbs[IndexCard].RamIO + VERS) & 0xF),
+			       (int)(readb(apbs[i].RamIO + VERS) >> 4),
+			       (int)(readb(apbs[i].RamIO + VERS) & 0xF),
 			       boardname);
 
 
diff --git a/drivers/char/hpet.c b/drivers/char/hpet.c
index e3f9a99b852..d784650d14f 100644
--- a/drivers/char/hpet.c
+++ b/drivers/char/hpet.c
@@ -373,26 +373,14 @@ static int hpet_mmap(struct file *file, struct vm_area_struct *vma)
 	struct hpet_dev *devp;
 	unsigned long addr;
 
-	if (((vma->vm_end - vma->vm_start) != PAGE_SIZE) || vma->vm_pgoff)
-		return -EINVAL;
-
 	devp = file->private_data;
 	addr = devp->hd_hpets->hp_hpet_phys;
 
 	if (addr & (PAGE_SIZE - 1))
 		return -ENOSYS;
 
-	vma->vm_flags |= VM_IO;
 	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
-
-	if (io_remap_pfn_range(vma, vma->vm_start, addr >> PAGE_SHIFT,
-					PAGE_SIZE, vma->vm_page_prot)) {
-		printk(KERN_ERR "%s: io_remap_pfn_range failed\n",
-			__func__);
-		return -EAGAIN;
-	}
-
-	return 0;
+	return vm_iomap_memory(vma, addr, PAGE_SIZE);
 #else
 	return -ENOSYS;
 #endif
diff --git a/drivers/char/hw_random/core.c b/drivers/char/hw_random/core.c
index 69ae5972713..a0f7724852e 100644
--- a/drivers/char/hw_random/core.c
+++ b/drivers/char/hw_random/core.c
@@ -380,6 +380,15 @@ void hwrng_unregister(struct hwrng *rng)
 }
 EXPORT_SYMBOL_GPL(hwrng_unregister);
 
+static void __exit hwrng_exit(void)
+{
+	mutex_lock(&rng_mutex);
+	BUG_ON(current_rng);
+	kfree(rng_buffer);
+	mutex_unlock(&rng_mutex);
+}
+
+module_exit(hwrng_exit);
 
 MODULE_DESCRIPTION("H/W Random Number Generator (RNG) driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/char/hw_random/mxc-rnga.c b/drivers/char/hw_random/mxc-rnga.c
index f05d85713fd..895d0b8fb9a 100644
--- a/drivers/char/hw_random/mxc-rnga.c
+++ b/drivers/char/hw_random/mxc-rnga.c
@@ -228,18 +228,7 @@ static struct platform_driver mxc_rnga_driver = {
 	.remove = __exit_p(mxc_rnga_remove),
 };
 
-static int __init mod_init(void)
-{
-	return platform_driver_probe(&mxc_rnga_driver, mxc_rnga_probe);
-}
-
-static void __exit mod_exit(void)
-{
-	platform_driver_unregister(&mxc_rnga_driver);
-}
-
-module_init(mod_init);
-module_exit(mod_exit);
+module_platform_driver_probe(mxc_rnga_driver, mxc_rnga_probe);
 
 MODULE_AUTHOR("Freescale Semiconductor, Inc.");
 MODULE_DESCRIPTION("H/W RNGA driver for i.MX");
diff --git a/drivers/char/hw_random/tx4939-rng.c b/drivers/char/hw_random/tx4939-rng.c
index 30991989d65..d34a24a0d48 100644
--- a/drivers/char/hw_random/tx4939-rng.c
+++ b/drivers/char/hw_random/tx4939-rng.c
@@ -166,18 +166,7 @@ static struct platform_driver tx4939_rng_driver = {
 	.remove = tx4939_rng_remove,
 };
 
-static int __init tx4939rng_init(void)
-{
-	return platform_driver_probe(&tx4939_rng_driver, tx4939_rng_probe);
-}
-
-static void __exit tx4939rng_exit(void)
-{
-	platform_driver_unregister(&tx4939_rng_driver);
-}
-
-module_init(tx4939rng_init);
-module_exit(tx4939rng_exit);
+module_platform_driver_probe(tx4939_rng_driver, tx4939_rng_probe);
 
 MODULE_DESCRIPTION("H/W Random Number Generator (RNG) driver for TX4939");
 MODULE_LICENSE("GPL");
diff --git a/drivers/char/tile-srom.c b/drivers/char/tile-srom.c
index 3b22a606f79..2e2036e940f 100644
--- a/drivers/char/tile-srom.c
+++ b/drivers/char/tile-srom.c
@@ -371,7 +371,7 @@ static int srom_setup_minor(struct srom_dev *srom, int index)
 
 	dev = device_create(srom_class, &platform_bus,
 			    MKDEV(srom_major, index), srom, "%d", index);
-	return IS_ERR(dev) ? PTR_ERR(dev) : 0;
+	return PTR_RET(dev);
 }
 
 /** srom_init() - Initialize the driver's module. */
diff --git a/drivers/char/virtio_console.c b/drivers/char/virtio_console.c
index e905d5f5305..ce5f3fc25d6 100644
--- a/drivers/char/virtio_console.c
+++ b/drivers/char/virtio_console.c
@@ -149,7 +149,8 @@ struct ports_device {
 	spinlock_t ports_lock;
 
 	/* To protect the vq operations for the control channel */
-	spinlock_t cvq_lock;
+	spinlock_t c_ivq_lock;
+	spinlock_t c_ovq_lock;
 
 	/* The current config space is stored here */
 	struct virtio_console_config config;
@@ -569,11 +570,14 @@ static ssize_t __send_control_msg(struct ports_device *portdev, u32 port_id,
 	vq = portdev->c_ovq;
 
 	sg_init_one(sg, &cpkt, sizeof(cpkt));
+
+	spin_lock(&portdev->c_ovq_lock);
 	if (virtqueue_add_buf(vq, sg, 1, 0, &cpkt, GFP_ATOMIC) == 0) {
 		virtqueue_kick(vq);
 		while (!virtqueue_get_buf(vq, &len))
 			cpu_relax();
 	}
+	spin_unlock(&portdev->c_ovq_lock);
 	return 0;
 }
 
@@ -1436,7 +1440,7 @@ static int add_port(struct ports_device *portdev, u32 id)
 		 * rproc_serial does not want the console port, only
 		 * the generic port implementation.
 		 */
-		port->host_connected = port->guest_connected = true;
+		port->host_connected = true;
 	else if (!use_multiport(port->portdev)) {
 		/*
 		 * If we're not using multiport support,
@@ -1709,23 +1713,23 @@ static void control_work_handler(struct work_struct *work)
 	portdev = container_of(work, struct ports_device, control_work);
 	vq = portdev->c_ivq;
 
-	spin_lock(&portdev->cvq_lock);
+	spin_lock(&portdev->c_ivq_lock);
 	while ((buf = virtqueue_get_buf(vq, &len))) {
-		spin_unlock(&portdev->cvq_lock);
+		spin_unlock(&portdev->c_ivq_lock);
 
 		buf->len = len;
 		buf->offset = 0;
 
 		handle_control_message(portdev, buf);
 
-		spin_lock(&portdev->cvq_lock);
+		spin_lock(&portdev->c_ivq_lock);
 		if (add_inbuf(portdev->c_ivq, buf) < 0) {
 			dev_warn(&portdev->vdev->dev,
 				 "Error adding buffer to queue\n");
 			free_buf(buf, false);
 		}
 	}
-	spin_unlock(&portdev->cvq_lock);
+	spin_unlock(&portdev->c_ivq_lock);
 }
 
 static void out_intr(struct virtqueue *vq)
@@ -1752,13 +1756,23 @@ static void in_intr(struct virtqueue *vq)
 	port->inbuf = get_inbuf(port);
 
 	/*
-	 * Don't queue up data when port is closed.  This condition
+	 * Normally the port should not accept data when the port is
+	 * closed. For generic serial ports, the host won't (shouldn't)
+	 * send data till the guest is connected. But this condition
 	 * can be reached when a console port is not yet connected (no
-	 * tty is spawned) and the host sends out data to console
-	 * ports.  For generic serial ports, the host won't
-	 * (shouldn't) send data till the guest is connected.
+	 * tty is spawned) and the other side sends out data over the
+	 * vring, or when a remote devices start sending data before
+	 * the ports are opened.
+	 *
+	 * A generic serial port will discard data if not connected,
+	 * while console ports and rproc-serial ports accepts data at
+	 * any time. rproc-serial is initiated with guest_connected to
+	 * false because port_fops_open expects this. Console ports are
+	 * hooked up with an HVC console and is initialized with
+	 * guest_connected to true.
 	 */
-	if (!port->guest_connected)
+
+	if (!port->guest_connected && !is_rproc_serial(port->portdev->vdev))
 		discard_port_data(port);
 
 	spin_unlock_irqrestore(&port->inbuf_lock, flags);
@@ -1986,10 +2000,12 @@ static int virtcons_probe(struct virtio_device *vdev)
 	if (multiport) {
 		unsigned int nr_added_bufs;
 
-		spin_lock_init(&portdev->cvq_lock);
+		spin_lock_init(&portdev->c_ivq_lock);
+		spin_lock_init(&portdev->c_ovq_lock);
 		INIT_WORK(&portdev->control_work, &control_work_handler);
 
-		nr_added_bufs = fill_queue(portdev->c_ivq, &portdev->cvq_lock);
+		nr_added_bufs = fill_queue(portdev->c_ivq,
+					   &portdev->c_ivq_lock);
 		if (!nr_added_bufs) {
 			dev_err(&vdev->dev,
 				"Error allocating buffers for control queue\n");
@@ -2140,7 +2156,7 @@ static int virtcons_restore(struct virtio_device *vdev)
 		return ret;
 
 	if (use_multiport(portdev))
-		fill_queue(portdev->c_ivq, &portdev->cvq_lock);
+		fill_queue(portdev->c_ivq, &portdev->c_ivq_lock);
 
 	list_for_each_entry(port, &portdev->ports, list) {
 		port->in_vq = portdev->in_vqs[port->id];
diff --git a/drivers/clk/Kconfig b/drivers/clk/Kconfig
index a47e6ee98b8..0357ac44638 100644
--- a/drivers/clk/Kconfig
+++ b/drivers/clk/Kconfig
@@ -55,6 +55,16 @@ config COMMON_CLK_MAX77686
 	---help---
 	  This driver supports Maxim 77686 crystal oscillator clock. 
 
+config COMMON_CLK_SI5351
+	tristate "Clock driver for SiLabs 5351A/B/C"
+	depends on I2C
+	depends on OF
+	select REGMAP_I2C
+	select RATIONAL
+	---help---
+	  This driver supports Silicon Labs 5351A/B/C programmable clock
+	  generators.
+
 config CLK_TWL6040
 	tristate "External McPDM functional clock from twl6040"
 	depends on TWL6040_CORE
@@ -63,6 +73,14 @@ config CLK_TWL6040
 	  McPDM. McPDM module is using the external bit clock on the McPDM bus
 	  as functional clock.
 
+config COMMON_CLK_AXI_CLKGEN
+	tristate "AXI clkgen driver"
+	depends on ARCH_ZYNQ || MICROBLAZE
+	help
+	---help---
+	  Support for the Analog Devices axi-clkgen pcore clock generator for Xilinx
+	  FPGAs. It is commonly used in Analog Devices' reference designs.
+
 endmenu
 
 source "drivers/clk/mvebu/Kconfig"
diff --git a/drivers/clk/Makefile b/drivers/clk/Makefile
index 300d4775d92..e7f7fe9b2f0 100644
--- a/drivers/clk/Makefile
+++ b/drivers/clk/Makefile
@@ -7,6 +7,7 @@ obj-$(CONFIG_COMMON_CLK)	+= clk-fixed-factor.o
 obj-$(CONFIG_COMMON_CLK)	+= clk-fixed-rate.o
 obj-$(CONFIG_COMMON_CLK)	+= clk-gate.o
 obj-$(CONFIG_COMMON_CLK)	+= clk-mux.o
+obj-$(CONFIG_COMMON_CLK)	+= clk-composite.o
 
 # SoCs specific
 obj-$(CONFIG_ARCH_BCM2835)	+= clk-bcm2835.o
@@ -23,6 +24,7 @@ ifeq ($(CONFIG_COMMON_CLK), y)
 obj-$(CONFIG_ARCH_MMP)		+= mmp/
 endif
 obj-$(CONFIG_MACH_LOONGSON1)	+= clk-ls1x.o
+obj-$(CONFIG_ARCH_SUNXI)	+= sunxi/
 obj-$(CONFIG_ARCH_U8500)	+= ux500/
 obj-$(CONFIG_ARCH_VT8500)	+= clk-vt8500.o
 obj-$(CONFIG_ARCH_ZYNQ)		+= clk-zynq.o
@@ -31,6 +33,8 @@ obj-$(CONFIG_ARCH_TEGRA)	+= tegra/
 obj-$(CONFIG_X86)		+= x86/
 
 # Chip specific
+obj-$(CONFIG_COMMON_CLK_AXI_CLKGEN) += clk-axi-clkgen.o
 obj-$(CONFIG_COMMON_CLK_WM831X) += clk-wm831x.o
 obj-$(CONFIG_COMMON_CLK_MAX77686) += clk-max77686.o
+obj-$(CONFIG_COMMON_CLK_SI5351) += clk-si5351.o
 obj-$(CONFIG_CLK_TWL6040)	+= clk-twl6040.o
diff --git a/drivers/clk/clk-axi-clkgen.c b/drivers/clk/clk-axi-clkgen.c
new file mode 100644
index 00000000000..8137327847c
--- /dev/null
+++ b/drivers/clk/clk-axi-clkgen.c
@@ -0,0 +1,331 @@
+/*
+ * AXI clkgen driver
+ *
+ * Copyright 2012-2013 Analog Devices Inc.
+ *  Author: Lars-Peter Clausen <lars@metafoo.de>
+ *
+ * Licensed under the GPL-2.
+ *
+ */
+
+#include <linux/platform_device.h>
+#include <linux/clk-provider.h>
+#include <linux/clk.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/module.h>
+#include <linux/err.h>
+
+#define AXI_CLKGEN_REG_UPDATE_ENABLE	0x04
+#define AXI_CLKGEN_REG_CLK_OUT1		0x08
+#define AXI_CLKGEN_REG_CLK_OUT2		0x0c
+#define AXI_CLKGEN_REG_CLK_DIV		0x10
+#define AXI_CLKGEN_REG_CLK_FB1		0x14
+#define AXI_CLKGEN_REG_CLK_FB2		0x18
+#define AXI_CLKGEN_REG_LOCK1		0x1c
+#define AXI_CLKGEN_REG_LOCK2		0x20
+#define AXI_CLKGEN_REG_LOCK3		0x24
+#define AXI_CLKGEN_REG_FILTER1		0x28
+#define AXI_CLKGEN_REG_FILTER2		0x2c
+
+struct axi_clkgen {
+	void __iomem *base;
+	struct clk_hw clk_hw;
+};
+
+static uint32_t axi_clkgen_lookup_filter(unsigned int m)
+{
+	switch (m) {
+	case 0:
+		return 0x01001990;
+	case 1:
+		return 0x01001190;
+	case 2:
+		return 0x01009890;
+	case 3:
+		return 0x01001890;
+	case 4:
+		return 0x01008890;
+	case 5 ... 8:
+		return 0x01009090;
+	case 9 ... 11:
+		return 0x01000890;
+	case 12:
+		return 0x08009090;
+	case 13 ... 22:
+		return 0x01001090;
+	case 23 ... 36:
+		return 0x01008090;
+	case 37 ... 46:
+		return 0x08001090;
+	default:
+		return 0x08008090;
+	}
+}
+
+static const uint32_t axi_clkgen_lock_table[] = {
+	0x060603e8, 0x060603e8, 0x080803e8, 0x0b0b03e8,
+	0x0e0e03e8, 0x111103e8, 0x131303e8, 0x161603e8,
+	0x191903e8, 0x1c1c03e8, 0x1f1f0384, 0x1f1f0339,
+	0x1f1f02ee, 0x1f1f02bc, 0x1f1f028a, 0x1f1f0271,
+	0x1f1f023f, 0x1f1f0226, 0x1f1f020d, 0x1f1f01f4,
+	0x1f1f01db, 0x1f1f01c2, 0x1f1f01a9, 0x1f1f0190,
+	0x1f1f0190, 0x1f1f0177, 0x1f1f015e, 0x1f1f015e,
+	0x1f1f0145, 0x1f1f0145, 0x1f1f012c, 0x1f1f012c,
+	0x1f1f012c, 0x1f1f0113, 0x1f1f0113, 0x1f1f0113,
+};
+
+static uint32_t axi_clkgen_lookup_lock(unsigned int m)
+{
+	if (m < ARRAY_SIZE(axi_clkgen_lock_table))
+		return axi_clkgen_lock_table[m];
+	return 0x1f1f00fa;
+}
+
+static const unsigned int fpfd_min = 10000;
+static const unsigned int fpfd_max = 300000;
+static const unsigned int fvco_min = 600000;
+static const unsigned int fvco_max = 1200000;
+
+static void axi_clkgen_calc_params(unsigned long fin, unsigned long fout,
+	unsigned int *best_d, unsigned int *best_m, unsigned int *best_dout)
+{
+	unsigned long d, d_min, d_max, _d_min, _d_max;
+	unsigned long m, m_min, m_max;
+	unsigned long f, dout, best_f, fvco;
+
+	fin /= 1000;
+	fout /= 1000;
+
+	best_f = ULONG_MAX;
+	*best_d = 0;
+	*best_m = 0;
+	*best_dout = 0;
+
+	d_min = max_t(unsigned long, DIV_ROUND_UP(fin, fpfd_max), 1);
+	d_max = min_t(unsigned long, fin / fpfd_min, 80);
+
+	m_min = max_t(unsigned long, DIV_ROUND_UP(fvco_min, fin) * d_min, 1);
+	m_max = min_t(unsigned long, fvco_max * d_max / fin, 64);
+
+	for (m = m_min; m <= m_max; m++) {
+		_d_min = max(d_min, DIV_ROUND_UP(fin * m, fvco_max));
+		_d_max = min(d_max, fin * m / fvco_min);
+
+		for (d = _d_min; d <= _d_max; d++) {
+			fvco = fin * m / d;
+
+			dout = DIV_ROUND_CLOSEST(fvco, fout);
+			dout = clamp_t(unsigned long, dout, 1, 128);
+			f = fvco / dout;
+			if (abs(f - fout) < abs(best_f - fout)) {
+				best_f = f;
+				*best_d = d;
+				*best_m = m;
+				*best_dout = dout;
+				if (best_f == fout)
+					return;
+			}
+		}
+	}
+}
+
+static void axi_clkgen_calc_clk_params(unsigned int divider, unsigned int *low,
+	unsigned int *high, unsigned int *edge, unsigned int *nocount)
+{
+	if (divider == 1)
+		*nocount = 1;
+	else
+		*nocount = 0;
+
+	*high = divider / 2;
+	*edge = divider % 2;
+	*low = divider - *high;
+}
+
+static void axi_clkgen_write(struct axi_clkgen *axi_clkgen,
+	unsigned int reg, unsigned int val)
+{
+	writel(val, axi_clkgen->base + reg);
+}
+
+static void axi_clkgen_read(struct axi_clkgen *axi_clkgen,
+	unsigned int reg, unsigned int *val)
+{
+	*val = readl(axi_clkgen->base + reg);
+}
+
+static struct axi_clkgen *clk_hw_to_axi_clkgen(struct clk_hw *clk_hw)
+{
+	return container_of(clk_hw, struct axi_clkgen, clk_hw);
+}
+
+static int axi_clkgen_set_rate(struct clk_hw *clk_hw,
+	unsigned long rate, unsigned long parent_rate)
+{
+	struct axi_clkgen *axi_clkgen = clk_hw_to_axi_clkgen(clk_hw);
+	unsigned int d, m, dout;
+	unsigned int nocount;
+	unsigned int high;
+	unsigned int edge;
+	unsigned int low;
+	uint32_t filter;
+	uint32_t lock;
+
+	if (parent_rate == 0 || rate == 0)
+		return -EINVAL;
+
+	axi_clkgen_calc_params(parent_rate, rate, &d, &m, &dout);
+
+	if (d == 0 || dout == 0 || m == 0)
+		return -EINVAL;
+
+	filter = axi_clkgen_lookup_filter(m - 1);
+	lock = axi_clkgen_lookup_lock(m - 1);
+
+	axi_clkgen_write(axi_clkgen, AXI_CLKGEN_REG_UPDATE_ENABLE, 0);
+
+	axi_clkgen_calc_clk_params(dout, &low, &high, &edge, &nocount);
+	axi_clkgen_write(axi_clkgen, AXI_CLKGEN_REG_CLK_OUT1,
+		(high << 6) | low);
+	axi_clkgen_write(axi_clkgen, AXI_CLKGEN_REG_CLK_OUT2,
+		(edge << 7) | (nocount << 6));
+
+	axi_clkgen_calc_clk_params(d, &low, &high, &edge, &nocount);
+	axi_clkgen_write(axi_clkgen, AXI_CLKGEN_REG_CLK_DIV,
+		(edge << 13) | (nocount << 12) | (high << 6) | low);
+
+	axi_clkgen_calc_clk_params(m, &low, &high, &edge, &nocount);
+	axi_clkgen_write(axi_clkgen, AXI_CLKGEN_REG_CLK_FB1,
+		(high << 6) | low);
+	axi_clkgen_write(axi_clkgen, AXI_CLKGEN_REG_CLK_FB2,
+		(edge << 7) | (nocount << 6));
+
+	axi_clkgen_write(axi_clkgen, AXI_CLKGEN_REG_LOCK1, lock & 0x3ff);
+	axi_clkgen_write(axi_clkgen, AXI_CLKGEN_REG_LOCK2,
+		(((lock >> 16) & 0x1f) << 10) | 0x1);
+	axi_clkgen_write(axi_clkgen, AXI_CLKGEN_REG_LOCK3,
+		(((lock >> 24) & 0x1f) << 10) | 0x3e9);
+	axi_clkgen_write(axi_clkgen, AXI_CLKGEN_REG_FILTER1, filter >> 16);
+	axi_clkgen_write(axi_clkgen, AXI_CLKGEN_REG_FILTER2, filter);
+
+	axi_clkgen_write(axi_clkgen, AXI_CLKGEN_REG_UPDATE_ENABLE, 1);
+
+	return 0;
+}
+
+static long axi_clkgen_round_rate(struct clk_hw *hw, unsigned long rate,
+	unsigned long *parent_rate)
+{
+	unsigned int d, m, dout;
+
+	axi_clkgen_calc_params(*parent_rate, rate, &d, &m, &dout);
+
+	if (d == 0 || dout == 0 || m == 0)
+		return -EINVAL;
+
+	return *parent_rate / d * m / dout;
+}
+
+static unsigned long axi_clkgen_recalc_rate(struct clk_hw *clk_hw,
+	unsigned long parent_rate)
+{
+	struct axi_clkgen *axi_clkgen = clk_hw_to_axi_clkgen(clk_hw);
+	unsigned int d, m, dout;
+	unsigned int reg;
+	unsigned long long tmp;
+
+	axi_clkgen_read(axi_clkgen, AXI_CLKGEN_REG_CLK_OUT1, &reg);
+	dout = (reg & 0x3f) + ((reg >> 6) & 0x3f);
+	axi_clkgen_read(axi_clkgen, AXI_CLKGEN_REG_CLK_DIV, &reg);
+	d = (reg & 0x3f) + ((reg >> 6) & 0x3f);
+	axi_clkgen_read(axi_clkgen, AXI_CLKGEN_REG_CLK_FB1, &reg);
+	m = (reg & 0x3f) + ((reg >> 6) & 0x3f);
+
+	if (d == 0 || dout == 0)
+		return 0;
+
+	tmp = (unsigned long long)(parent_rate / d) * m;
+	do_div(tmp, dout);
+
+	if (tmp > ULONG_MAX)
+		return ULONG_MAX;
+
+	return tmp;
+}
+
+static const struct clk_ops axi_clkgen_ops = {
+	.recalc_rate = axi_clkgen_recalc_rate,
+	.round_rate = axi_clkgen_round_rate,
+	.set_rate = axi_clkgen_set_rate,
+};
+
+static int axi_clkgen_probe(struct platform_device *pdev)
+{
+	struct axi_clkgen *axi_clkgen;
+	struct clk_init_data init;
+	const char *parent_name;
+	const char *clk_name;
+	struct resource *mem;
+	struct clk *clk;
+
+	axi_clkgen = devm_kzalloc(&pdev->dev, sizeof(*axi_clkgen), GFP_KERNEL);
+	if (!axi_clkgen)
+		return -ENOMEM;
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	axi_clkgen->base = devm_ioremap_resource(&pdev->dev, mem);
+	if (IS_ERR(axi_clkgen->base))
+		return PTR_ERR(axi_clkgen->base);
+
+	parent_name = of_clk_get_parent_name(pdev->dev.of_node, 0);
+	if (!parent_name)
+		return -EINVAL;
+
+	clk_name = pdev->dev.of_node->name;
+	of_property_read_string(pdev->dev.of_node, "clock-output-names",
+		&clk_name);
+
+	init.name = clk_name;
+	init.ops = &axi_clkgen_ops;
+	init.flags = 0;
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+
+	axi_clkgen->clk_hw.init = &init;
+	clk = devm_clk_register(&pdev->dev, &axi_clkgen->clk_hw);
+	if (IS_ERR(clk))
+		return PTR_ERR(clk);
+
+	return of_clk_add_provider(pdev->dev.of_node, of_clk_src_simple_get,
+				    clk);
+}
+
+static int axi_clkgen_remove(struct platform_device *pdev)
+{
+	of_clk_del_provider(pdev->dev.of_node);
+
+	return 0;
+}
+
+static const struct of_device_id axi_clkgen_ids[] = {
+	{ .compatible = "adi,axi-clkgen-1.00.a" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, axi_clkgen_ids);
+
+static struct platform_driver axi_clkgen_driver = {
+	.driver = {
+		.name = "adi-axi-clkgen",
+		.owner = THIS_MODULE,
+		.of_match_table = axi_clkgen_ids,
+	},
+	.probe = axi_clkgen_probe,
+	.remove = axi_clkgen_remove,
+};
+module_platform_driver(axi_clkgen_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
+MODULE_DESCRIPTION("Driver for the Analog Devices' AXI clkgen pcore clock generator");
diff --git a/drivers/clk/clk-composite.c b/drivers/clk/clk-composite.c
new file mode 100644
index 00000000000..a33f46f20a4
--- /dev/null
+++ b/drivers/clk/clk-composite.c
@@ -0,0 +1,210 @@
+/*
+ * Copyright (c) 2013 NVIDIA 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 it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+
+#define to_clk_composite(_hw) container_of(_hw, struct clk_composite, hw)
+
+static u8 clk_composite_get_parent(struct clk_hw *hw)
+{
+	struct clk_composite *composite = to_clk_composite(hw);
+	const struct clk_ops *mux_ops = composite->mux_ops;
+	struct clk_hw *mux_hw = composite->mux_hw;
+
+	mux_hw->clk = hw->clk;
+
+	return mux_ops->get_parent(mux_hw);
+}
+
+static int clk_composite_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct clk_composite *composite = to_clk_composite(hw);
+	const struct clk_ops *mux_ops = composite->mux_ops;
+	struct clk_hw *mux_hw = composite->mux_hw;
+
+	mux_hw->clk = hw->clk;
+
+	return mux_ops->set_parent(mux_hw, index);
+}
+
+static unsigned long clk_composite_recalc_rate(struct clk_hw *hw,
+					    unsigned long parent_rate)
+{
+	struct clk_composite *composite = to_clk_composite(hw);
+	const struct clk_ops *rate_ops = composite->rate_ops;
+	struct clk_hw *rate_hw = composite->rate_hw;
+
+	rate_hw->clk = hw->clk;
+
+	return rate_ops->recalc_rate(rate_hw, parent_rate);
+}
+
+static long clk_composite_round_rate(struct clk_hw *hw, unsigned long rate,
+				  unsigned long *prate)
+{
+	struct clk_composite *composite = to_clk_composite(hw);
+	const struct clk_ops *rate_ops = composite->rate_ops;
+	struct clk_hw *rate_hw = composite->rate_hw;
+
+	rate_hw->clk = hw->clk;
+
+	return rate_ops->round_rate(rate_hw, rate, prate);
+}
+
+static int clk_composite_set_rate(struct clk_hw *hw, unsigned long rate,
+			       unsigned long parent_rate)
+{
+	struct clk_composite *composite = to_clk_composite(hw);
+	const struct clk_ops *rate_ops = composite->rate_ops;
+	struct clk_hw *rate_hw = composite->rate_hw;
+
+	rate_hw->clk = hw->clk;
+
+	return rate_ops->set_rate(rate_hw, rate, parent_rate);
+}
+
+static int clk_composite_is_enabled(struct clk_hw *hw)
+{
+	struct clk_composite *composite = to_clk_composite(hw);
+	const struct clk_ops *gate_ops = composite->gate_ops;
+	struct clk_hw *gate_hw = composite->gate_hw;
+
+	gate_hw->clk = hw->clk;
+
+	return gate_ops->is_enabled(gate_hw);
+}
+
+static int clk_composite_enable(struct clk_hw *hw)
+{
+	struct clk_composite *composite = to_clk_composite(hw);
+	const struct clk_ops *gate_ops = composite->gate_ops;
+	struct clk_hw *gate_hw = composite->gate_hw;
+
+	gate_hw->clk = hw->clk;
+
+	return gate_ops->enable(gate_hw);
+}
+
+static void clk_composite_disable(struct clk_hw *hw)
+{
+	struct clk_composite *composite = to_clk_composite(hw);
+	const struct clk_ops *gate_ops = composite->gate_ops;
+	struct clk_hw *gate_hw = composite->gate_hw;
+
+	gate_hw->clk = hw->clk;
+
+	gate_ops->disable(gate_hw);
+}
+
+struct clk *clk_register_composite(struct device *dev, const char *name,
+			const char **parent_names, int num_parents,
+			struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
+			struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
+			struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
+			unsigned long flags)
+{
+	struct clk *clk;
+	struct clk_init_data init;
+	struct clk_composite *composite;
+	struct clk_ops *clk_composite_ops;
+
+	composite = kzalloc(sizeof(*composite), GFP_KERNEL);
+	if (!composite) {
+		pr_err("%s: could not allocate composite clk\n", __func__);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	init.name = name;
+	init.flags = flags | CLK_IS_BASIC;
+	init.parent_names = parent_names;
+	init.num_parents = num_parents;
+
+	clk_composite_ops = &composite->ops;
+
+	if (mux_hw && mux_ops) {
+		if (!mux_ops->get_parent || !mux_ops->set_parent) {
+			clk = ERR_PTR(-EINVAL);
+			goto err;
+		}
+
+		composite->mux_hw = mux_hw;
+		composite->mux_ops = mux_ops;
+		clk_composite_ops->get_parent = clk_composite_get_parent;
+		clk_composite_ops->set_parent = clk_composite_set_parent;
+	}
+
+	if (rate_hw && rate_ops) {
+		if (!rate_ops->recalc_rate) {
+			clk = ERR_PTR(-EINVAL);
+			goto err;
+		}
+
+		/* .round_rate is a prerequisite for .set_rate */
+		if (rate_ops->round_rate) {
+			clk_composite_ops->round_rate = clk_composite_round_rate;
+			if (rate_ops->set_rate) {
+				clk_composite_ops->set_rate = clk_composite_set_rate;
+			}
+		} else {
+			WARN(rate_ops->set_rate,
+				"%s: missing round_rate op is required\n",
+				__func__);
+		}
+
+		composite->rate_hw = rate_hw;
+		composite->rate_ops = rate_ops;
+		clk_composite_ops->recalc_rate = clk_composite_recalc_rate;
+	}
+
+	if (gate_hw && gate_ops) {
+		if (!gate_ops->is_enabled || !gate_ops->enable ||
+		    !gate_ops->disable) {
+			clk = ERR_PTR(-EINVAL);
+			goto err;
+		}
+
+		composite->gate_hw = gate_hw;
+		composite->gate_ops = gate_ops;
+		clk_composite_ops->is_enabled = clk_composite_is_enabled;
+		clk_composite_ops->enable = clk_composite_enable;
+		clk_composite_ops->disable = clk_composite_disable;
+	}
+
+	init.ops = clk_composite_ops;
+	composite->hw.init = &init;
+
+	clk = clk_register(dev, &composite->hw);
+	if (IS_ERR(clk))
+		goto err;
+
+	if (composite->mux_hw)
+		composite->mux_hw->clk = clk;
+
+	if (composite->rate_hw)
+		composite->rate_hw->clk = clk;
+
+	if (composite->gate_hw)
+		composite->gate_hw->clk = clk;
+
+	return clk;
+
+err:
+	kfree(composite);
+	return clk;
+}
diff --git a/drivers/clk/clk-divider.c b/drivers/clk/clk-divider.c
index 68b40210117..6d967416043 100644
--- a/drivers/clk/clk-divider.c
+++ b/drivers/clk/clk-divider.c
@@ -109,8 +109,9 @@ static unsigned long clk_divider_recalc_rate(struct clk_hw *hw,
 
 	div = _get_div(divider, val);
 	if (!div) {
-		WARN(1, "%s: Invalid divisor for clock %s\n", __func__,
-						__clk_get_name(hw->clk));
+		WARN(!(divider->flags & CLK_DIVIDER_ALLOW_ZERO),
+			"%s: Zero divisor and CLK_DIVIDER_ALLOW_ZERO not set\n",
+			__clk_get_name(hw->clk));
 		return parent_rate;
 	}
 
diff --git a/drivers/clk/clk-fixed-factor.c b/drivers/clk/clk-fixed-factor.c
index 1ef271e4759..9ff7d510faa 100644
--- a/drivers/clk/clk-fixed-factor.c
+++ b/drivers/clk/clk-fixed-factor.c
@@ -11,6 +11,7 @@
 #include <linux/clk-provider.h>
 #include <linux/slab.h>
 #include <linux/err.h>
+#include <linux/of.h>
 
 /*
  * DOC: basic fixed multiplier and divider clock that cannot gate
@@ -96,3 +97,38 @@ struct clk *clk_register_fixed_factor(struct device *dev, const char *name,
 
 	return clk;
 }
+#ifdef CONFIG_OF
+/**
+ * of_fixed_factor_clk_setup() - Setup function for simple fixed factor clock
+ */
+void __init of_fixed_factor_clk_setup(struct device_node *node)
+{
+	struct clk *clk;
+	const char *clk_name = node->name;
+	const char *parent_name;
+	u32 div, mult;
+
+	if (of_property_read_u32(node, "clock-div", &div)) {
+		pr_err("%s Fixed factor clock <%s> must have a clock-div property\n",
+			__func__, node->name);
+		return;
+	}
+
+	if (of_property_read_u32(node, "clock-mult", &mult)) {
+		pr_err("%s Fixed factor clock <%s> must have a clokc-mult property\n",
+			__func__, node->name);
+		return;
+	}
+
+	of_property_read_string(node, "clock-output-names", &clk_name);
+	parent_name = of_clk_get_parent_name(node, 0);
+
+	clk = clk_register_fixed_factor(NULL, clk_name, parent_name, 0,
+					mult, div);
+	if (!IS_ERR(clk))
+		of_clk_add_provider(node, of_clk_src_simple_get, clk);
+}
+EXPORT_SYMBOL_GPL(of_fixed_factor_clk_setup);
+CLK_OF_DECLARE(fixed_factor_clk, "fixed-factor-clock",
+		of_fixed_factor_clk_setup);
+#endif
diff --git a/drivers/clk/clk-mux.c b/drivers/clk/clk-mux.c
index 508c032edce..25b1734560d 100644
--- a/drivers/clk/clk-mux.c
+++ b/drivers/clk/clk-mux.c
@@ -32,6 +32,7 @@
 static u8 clk_mux_get_parent(struct clk_hw *hw)
 {
 	struct clk_mux *mux = to_clk_mux(hw);
+	int num_parents = __clk_get_num_parents(hw->clk);
 	u32 val;
 
 	/*
@@ -42,7 +43,16 @@ static u8 clk_mux_get_parent(struct clk_hw *hw)
 	 * val = 0x4 really means "bit 2, index starts at bit 0"
 	 */
 	val = readl(mux->reg) >> mux->shift;
-	val &= (1 << mux->width) - 1;
+	val &= mux->mask;
+
+	if (mux->table) {
+		int i;
+
+		for (i = 0; i < num_parents; i++)
+			if (mux->table[i] == val)
+				return i;
+		return -EINVAL;
+	}
 
 	if (val && (mux->flags & CLK_MUX_INDEX_BIT))
 		val = ffs(val) - 1;
@@ -50,7 +60,7 @@ static u8 clk_mux_get_parent(struct clk_hw *hw)
 	if (val && (mux->flags & CLK_MUX_INDEX_ONE))
 		val--;
 
-	if (val >= __clk_get_num_parents(hw->clk))
+	if (val >= num_parents)
 		return -EINVAL;
 
 	return val;
@@ -62,17 +72,22 @@ static int clk_mux_set_parent(struct clk_hw *hw, u8 index)
 	u32 val;
 	unsigned long flags = 0;
 
-	if (mux->flags & CLK_MUX_INDEX_BIT)
-		index = (1 << ffs(index));
+	if (mux->table)
+		index = mux->table[index];
 
-	if (mux->flags & CLK_MUX_INDEX_ONE)
-		index++;
+	else {
+		if (mux->flags & CLK_MUX_INDEX_BIT)
+			index = (1 << ffs(index));
+
+		if (mux->flags & CLK_MUX_INDEX_ONE)
+			index++;
+	}
 
 	if (mux->lock)
 		spin_lock_irqsave(mux->lock, flags);
 
 	val = readl(mux->reg);
-	val &= ~(((1 << mux->width) - 1) << mux->shift);
+	val &= ~(mux->mask << mux->shift);
 	val |= index << mux->shift;
 	writel(val, mux->reg);
 
@@ -88,10 +103,10 @@ const struct clk_ops clk_mux_ops = {
 };
 EXPORT_SYMBOL_GPL(clk_mux_ops);
 
-struct clk *clk_register_mux(struct device *dev, const char *name,
+struct clk *clk_register_mux_table(struct device *dev, const char *name,
 		const char **parent_names, u8 num_parents, unsigned long flags,
-		void __iomem *reg, u8 shift, u8 width,
-		u8 clk_mux_flags, spinlock_t *lock)
+		void __iomem *reg, u8 shift, u32 mask,
+		u8 clk_mux_flags, u32 *table, spinlock_t *lock)
 {
 	struct clk_mux *mux;
 	struct clk *clk;
@@ -113,9 +128,10 @@ struct clk *clk_register_mux(struct device *dev, const char *name,
 	/* struct clk_mux assignments */
 	mux->reg = reg;
 	mux->shift = shift;
-	mux->width = width;
+	mux->mask = mask;
 	mux->flags = clk_mux_flags;
 	mux->lock = lock;
+	mux->table = table;
 	mux->hw.init = &init;
 
 	clk = clk_register(dev, &mux->hw);
@@ -125,3 +141,15 @@ struct clk *clk_register_mux(struct device *dev, const char *name,
 
 	return clk;
 }
+
+struct clk *clk_register_mux(struct device *dev, const char *name,
+		const char **parent_names, u8 num_parents, unsigned long flags,
+		void __iomem *reg, u8 shift, u8 width,
+		u8 clk_mux_flags, spinlock_t *lock)
+{
+	u32 mask = BIT(width) - 1;
+
+	return clk_register_mux_table(dev, name, parent_names, num_parents,
+				      flags, reg, shift, mask, clk_mux_flags,
+				      NULL, lock);
+}
diff --git a/drivers/clk/clk-prima2.c b/drivers/clk/clk-prima2.c
index f8e9d0c27be..643ca653fef 100644
--- a/drivers/clk/clk-prima2.c
+++ b/drivers/clk/clk-prima2.c
@@ -1113,7 +1113,7 @@ void __init sirfsoc_of_clk_init(void)
 
 	for (i = pll1; i < maxclk; i++) {
 		prima2_clks[i] = clk_register(NULL, prima2_clk_hw_array[i]);
-		BUG_ON(!prima2_clks[i]);
+		BUG_ON(IS_ERR(prima2_clks[i]));
 	}
 	clk_register_clkdev(prima2_clks[cpu], NULL, "cpu");
 	clk_register_clkdev(prima2_clks[io],  NULL, "io");
diff --git a/drivers/clk/clk-si5351.c b/drivers/clk/clk-si5351.c
new file mode 100644
index 00000000000..892728412e9
--- /dev/null
+++ b/drivers/clk/clk-si5351.c
@@ -0,0 +1,1510 @@
+/*
+ * clk-si5351.c: Silicon Laboratories Si5351A/B/C I2C Clock Generator
+ *
+ * Sebastian Hesselbarth <sebastian.hesselbarth@gmail.com>
+ * Rabeeh Khoury <rabeeh@solid-run.com>
+ *
+ * References:
+ * [1] "Si5351A/B/C Data Sheet"
+ *     http://www.silabs.com/Support%20Documents/TechnicalDocs/Si5351.pdf
+ * [2] "Manually Generating an Si5351 Register Map"
+ *     http://www.silabs.com/Support%20Documents/TechnicalDocs/AN619.pdf
+ *
+ * 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/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/errno.h>
+#include <linux/rational.h>
+#include <linux/i2c.h>
+#include <linux/of_platform.h>
+#include <linux/platform_data/si5351.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <asm/div64.h>
+
+#include "clk-si5351.h"
+
+struct si5351_driver_data;
+
+struct si5351_parameters {
+	unsigned long	p1;
+	unsigned long	p2;
+	unsigned long	p3;
+	int		valid;
+};
+
+struct si5351_hw_data {
+	struct clk_hw			hw;
+	struct si5351_driver_data	*drvdata;
+	struct si5351_parameters	params;
+	unsigned char			num;
+};
+
+struct si5351_driver_data {
+	enum si5351_variant	variant;
+	struct i2c_client	*client;
+	struct regmap		*regmap;
+	struct clk_onecell_data onecell;
+
+	struct clk		*pxtal;
+	const char		*pxtal_name;
+	struct clk_hw		xtal;
+	struct clk		*pclkin;
+	const char		*pclkin_name;
+	struct clk_hw		clkin;
+
+	struct si5351_hw_data	pll[2];
+	struct si5351_hw_data	*msynth;
+	struct si5351_hw_data	*clkout;
+};
+
+static const char const *si5351_input_names[] = {
+	"xtal", "clkin"
+};
+static const char const *si5351_pll_names[] = {
+	"plla", "pllb", "vxco"
+};
+static const char const *si5351_msynth_names[] = {
+	"ms0", "ms1", "ms2", "ms3", "ms4", "ms5", "ms6", "ms7"
+};
+static const char const *si5351_clkout_names[] = {
+	"clk0", "clk1", "clk2", "clk3", "clk4", "clk5", "clk6", "clk7"
+};
+
+/*
+ * Si5351 i2c regmap
+ */
+static inline u8 si5351_reg_read(struct si5351_driver_data *drvdata, u8 reg)
+{
+	u32 val;
+	int ret;
+
+	ret = regmap_read(drvdata->regmap, reg, &val);
+	if (ret) {
+		dev_err(&drvdata->client->dev,
+			"unable to read from reg%02x\n", reg);
+		return 0;
+	}
+
+	return (u8)val;
+}
+
+static inline int si5351_bulk_read(struct si5351_driver_data *drvdata,
+				   u8 reg, u8 count, u8 *buf)
+{
+	return regmap_bulk_read(drvdata->regmap, reg, buf, count);
+}
+
+static inline int si5351_reg_write(struct si5351_driver_data *drvdata,
+				   u8 reg, u8 val)
+{
+	return regmap_write(drvdata->regmap, reg, val);
+}
+
+static inline int si5351_bulk_write(struct si5351_driver_data *drvdata,
+				    u8 reg, u8 count, const u8 *buf)
+{
+	return regmap_raw_write(drvdata->regmap, reg, buf, count);
+}
+
+static inline int si5351_set_bits(struct si5351_driver_data *drvdata,
+				  u8 reg, u8 mask, u8 val)
+{
+	return regmap_update_bits(drvdata->regmap, reg, mask, val);
+}
+
+static inline u8 si5351_msynth_params_address(int num)
+{
+	if (num > 5)
+		return SI5351_CLK6_PARAMETERS + (num - 6);
+	return SI5351_CLK0_PARAMETERS + (SI5351_PARAMETERS_LENGTH * num);
+}
+
+static void si5351_read_parameters(struct si5351_driver_data *drvdata,
+				   u8 reg, struct si5351_parameters *params)
+{
+	u8 buf[SI5351_PARAMETERS_LENGTH];
+
+	switch (reg) {
+	case SI5351_CLK6_PARAMETERS:
+	case SI5351_CLK7_PARAMETERS:
+		buf[0] = si5351_reg_read(drvdata, reg);
+		params->p1 = buf[0];
+		params->p2 = 0;
+		params->p3 = 1;
+		break;
+	default:
+		si5351_bulk_read(drvdata, reg, SI5351_PARAMETERS_LENGTH, buf);
+		params->p1 = ((buf[2] & 0x03) << 16) | (buf[3] << 8) | buf[4];
+		params->p2 = ((buf[5] & 0x0f) << 16) | (buf[6] << 8) | buf[7];
+		params->p3 = ((buf[5] & 0xf0) << 12) | (buf[0] << 8) | buf[1];
+	}
+	params->valid = 1;
+}
+
+static void si5351_write_parameters(struct si5351_driver_data *drvdata,
+				    u8 reg, struct si5351_parameters *params)
+{
+	u8 buf[SI5351_PARAMETERS_LENGTH];
+
+	switch (reg) {
+	case SI5351_CLK6_PARAMETERS:
+	case SI5351_CLK7_PARAMETERS:
+		buf[0] = params->p1 & 0xff;
+		si5351_reg_write(drvdata, reg, buf[0]);
+		break;
+	default:
+		buf[0] = ((params->p3 & 0x0ff00) >> 8) & 0xff;
+		buf[1] = params->p3 & 0xff;
+		/* save rdiv and divby4 */
+		buf[2] = si5351_reg_read(drvdata, reg + 2) & ~0x03;
+		buf[2] |= ((params->p1 & 0x30000) >> 16) & 0x03;
+		buf[3] = ((params->p1 & 0x0ff00) >> 8) & 0xff;
+		buf[4] = params->p1 & 0xff;
+		buf[5] = ((params->p3 & 0xf0000) >> 12) |
+			((params->p2 & 0xf0000) >> 16);
+		buf[6] = ((params->p2 & 0x0ff00) >> 8) & 0xff;
+		buf[7] = params->p2 & 0xff;
+		si5351_bulk_write(drvdata, reg, SI5351_PARAMETERS_LENGTH, buf);
+	}
+}
+
+static bool si5351_regmap_is_volatile(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case SI5351_DEVICE_STATUS:
+	case SI5351_INTERRUPT_STATUS:
+	case SI5351_PLL_RESET:
+		return true;
+	}
+	return false;
+}
+
+static bool si5351_regmap_is_writeable(struct device *dev, unsigned int reg)
+{
+	/* reserved registers */
+	if (reg >= 4 && reg <= 8)
+		return false;
+	if (reg >= 10 && reg <= 14)
+		return false;
+	if (reg >= 173 && reg <= 176)
+		return false;
+	if (reg >= 178 && reg <= 182)
+		return false;
+	/* read-only */
+	if (reg == SI5351_DEVICE_STATUS)
+		return false;
+	return true;
+}
+
+static struct regmap_config si5351_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.cache_type = REGCACHE_RBTREE,
+	.max_register = 187,
+	.writeable_reg = si5351_regmap_is_writeable,
+	.volatile_reg = si5351_regmap_is_volatile,
+};
+
+/*
+ * Si5351 xtal clock input
+ */
+static int si5351_xtal_prepare(struct clk_hw *hw)
+{
+	struct si5351_driver_data *drvdata =
+		container_of(hw, struct si5351_driver_data, xtal);
+	si5351_set_bits(drvdata, SI5351_FANOUT_ENABLE,
+			SI5351_XTAL_ENABLE, SI5351_XTAL_ENABLE);
+	return 0;
+}
+
+static void si5351_xtal_unprepare(struct clk_hw *hw)
+{
+	struct si5351_driver_data *drvdata =
+		container_of(hw, struct si5351_driver_data, xtal);
+	si5351_set_bits(drvdata, SI5351_FANOUT_ENABLE,
+			SI5351_XTAL_ENABLE, 0);
+}
+
+static const struct clk_ops si5351_xtal_ops = {
+	.prepare = si5351_xtal_prepare,
+	.unprepare = si5351_xtal_unprepare,
+};
+
+/*
+ * Si5351 clkin clock input (Si5351C only)
+ */
+static int si5351_clkin_prepare(struct clk_hw *hw)
+{
+	struct si5351_driver_data *drvdata =
+		container_of(hw, struct si5351_driver_data, clkin);
+	si5351_set_bits(drvdata, SI5351_FANOUT_ENABLE,
+			SI5351_CLKIN_ENABLE, SI5351_CLKIN_ENABLE);
+	return 0;
+}
+
+static void si5351_clkin_unprepare(struct clk_hw *hw)
+{
+	struct si5351_driver_data *drvdata =
+		container_of(hw, struct si5351_driver_data, clkin);
+	si5351_set_bits(drvdata, SI5351_FANOUT_ENABLE,
+			SI5351_CLKIN_ENABLE, 0);
+}
+
+/*
+ * CMOS clock source constraints:
+ * The input frequency range of the PLL is 10Mhz to 40MHz.
+ * If CLKIN is >40MHz, the input divider must be used.
+ */
+static unsigned long si5351_clkin_recalc_rate(struct clk_hw *hw,
+					      unsigned long parent_rate)
+{
+	struct si5351_driver_data *drvdata =
+		container_of(hw, struct si5351_driver_data, clkin);
+	unsigned long rate;
+	unsigned char idiv;
+
+	rate = parent_rate;
+	if (parent_rate > 160000000) {
+		idiv = SI5351_CLKIN_DIV_8;
+		rate /= 8;
+	} else if (parent_rate > 80000000) {
+		idiv = SI5351_CLKIN_DIV_4;
+		rate /= 4;
+	} else if (parent_rate > 40000000) {
+		idiv = SI5351_CLKIN_DIV_2;
+		rate /= 2;
+	} else {
+		idiv = SI5351_CLKIN_DIV_1;
+	}
+
+	si5351_set_bits(drvdata, SI5351_PLL_INPUT_SOURCE,
+			SI5351_CLKIN_DIV_MASK, idiv);
+
+	dev_dbg(&drvdata->client->dev, "%s - clkin div = %d, rate = %lu\n",
+		__func__, (1 << (idiv >> 6)), rate);
+
+	return rate;
+}
+
+static const struct clk_ops si5351_clkin_ops = {
+	.prepare = si5351_clkin_prepare,
+	.unprepare = si5351_clkin_unprepare,
+	.recalc_rate = si5351_clkin_recalc_rate,
+};
+
+/*
+ * Si5351 vxco clock input (Si5351B only)
+ */
+
+static int si5351_vxco_prepare(struct clk_hw *hw)
+{
+	struct si5351_hw_data *hwdata =
+		container_of(hw, struct si5351_hw_data, hw);
+
+	dev_warn(&hwdata->drvdata->client->dev, "VXCO currently unsupported\n");
+
+	return 0;
+}
+
+static void si5351_vxco_unprepare(struct clk_hw *hw)
+{
+}
+
+static unsigned long si5351_vxco_recalc_rate(struct clk_hw *hw,
+					     unsigned long parent_rate)
+{
+	return 0;
+}
+
+static int si5351_vxco_set_rate(struct clk_hw *hw, unsigned long rate,
+				unsigned long parent)
+{
+	return 0;
+}
+
+static const struct clk_ops si5351_vxco_ops = {
+	.prepare = si5351_vxco_prepare,
+	.unprepare = si5351_vxco_unprepare,
+	.recalc_rate = si5351_vxco_recalc_rate,
+	.set_rate = si5351_vxco_set_rate,
+};
+
+/*
+ * Si5351 pll a/b
+ *
+ * Feedback Multisynth Divider Equations [2]
+ *
+ * fVCO = fIN * (a + b/c)
+ *
+ * with 15 + 0/1048575 <= (a + b/c) <= 90 + 0/1048575 and
+ * fIN = fXTAL or fIN = fCLKIN/CLKIN_DIV
+ *
+ * Feedback Multisynth Register Equations
+ *
+ * (1) MSNx_P1[17:0] = 128 * a + floor(128 * b/c) - 512
+ * (2) MSNx_P2[19:0] = 128 * b - c * floor(128 * b/c) = (128*b) mod c
+ * (3) MSNx_P3[19:0] = c
+ *
+ * Transposing (2) yields: (4) floor(128 * b/c) = (128 * b / MSNx_P2)/c
+ *
+ * Using (4) on (1) yields:
+ * MSNx_P1 = 128 * a + (128 * b/MSNx_P2)/c - 512
+ * MSNx_P1 + 512 + MSNx_P2/c = 128 * a + 128 * b/c
+ *
+ * a + b/c = (MSNx_P1 + MSNx_P2/MSNx_P3 + 512)/128
+ *         = (MSNx_P1*MSNx_P3 + MSNx_P2 + 512*MSNx_P3)/(128*MSNx_P3)
+ *
+ */
+static int _si5351_pll_reparent(struct si5351_driver_data *drvdata,
+				int num, enum si5351_pll_src parent)
+{
+	u8 mask = (num == 0) ? SI5351_PLLA_SOURCE : SI5351_PLLB_SOURCE;
+
+	if (parent == SI5351_PLL_SRC_DEFAULT)
+		return 0;
+
+	if (num > 2)
+		return -EINVAL;
+
+	if (drvdata->variant != SI5351_VARIANT_C &&
+	    parent != SI5351_PLL_SRC_XTAL)
+		return -EINVAL;
+
+	si5351_set_bits(drvdata, SI5351_PLL_INPUT_SOURCE, mask,
+			(parent == SI5351_PLL_SRC_XTAL) ? 0 : mask);
+	return 0;
+}
+
+static unsigned char si5351_pll_get_parent(struct clk_hw *hw)
+{
+	struct si5351_hw_data *hwdata =
+		container_of(hw, struct si5351_hw_data, hw);
+	u8 mask = (hwdata->num == 0) ? SI5351_PLLA_SOURCE : SI5351_PLLB_SOURCE;
+	u8 val;
+
+	val = si5351_reg_read(hwdata->drvdata, SI5351_PLL_INPUT_SOURCE);
+
+	return (val & mask) ? 1 : 0;
+}
+
+static int si5351_pll_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct si5351_hw_data *hwdata =
+		container_of(hw, struct si5351_hw_data, hw);
+
+	if (hwdata->drvdata->variant != SI5351_VARIANT_C &&
+	    index > 0)
+		return -EPERM;
+
+	if (index > 1)
+		return -EINVAL;
+
+	return _si5351_pll_reparent(hwdata->drvdata, hwdata->num,
+			     (index == 0) ? SI5351_PLL_SRC_XTAL :
+			     SI5351_PLL_SRC_CLKIN);
+}
+
+static unsigned long si5351_pll_recalc_rate(struct clk_hw *hw,
+					    unsigned long parent_rate)
+{
+	struct si5351_hw_data *hwdata =
+		container_of(hw, struct si5351_hw_data, hw);
+	u8 reg = (hwdata->num == 0) ? SI5351_PLLA_PARAMETERS :
+		SI5351_PLLB_PARAMETERS;
+	unsigned long long rate;
+
+	if (!hwdata->params.valid)
+		si5351_read_parameters(hwdata->drvdata, reg, &hwdata->params);
+
+	if (hwdata->params.p3 == 0)
+		return parent_rate;
+
+	/* fVCO = fIN * (P1*P3 + 512*P3 + P2)/(128*P3) */
+	rate  = hwdata->params.p1 * hwdata->params.p3;
+	rate += 512 * hwdata->params.p3;
+	rate += hwdata->params.p2;
+	rate *= parent_rate;
+	do_div(rate, 128 * hwdata->params.p3);
+
+	dev_dbg(&hwdata->drvdata->client->dev,
+		"%s - %s: p1 = %lu, p2 = %lu, p3 = %lu, parent_rate = %lu, rate = %lu\n",
+		__func__, __clk_get_name(hwdata->hw.clk),
+		hwdata->params.p1, hwdata->params.p2, hwdata->params.p3,
+		parent_rate, (unsigned long)rate);
+
+	return (unsigned long)rate;
+}
+
+static long si5351_pll_round_rate(struct clk_hw *hw, unsigned long rate,
+				  unsigned long *parent_rate)
+{
+	struct si5351_hw_data *hwdata =
+		container_of(hw, struct si5351_hw_data, hw);
+	unsigned long rfrac, denom, a, b, c;
+	unsigned long long lltmp;
+
+	if (rate < SI5351_PLL_VCO_MIN)
+		rate = SI5351_PLL_VCO_MIN;
+	if (rate > SI5351_PLL_VCO_MAX)
+		rate = SI5351_PLL_VCO_MAX;
+
+	/* determine integer part of feedback equation */
+	a = rate / *parent_rate;
+
+	if (a < SI5351_PLL_A_MIN)
+		rate = *parent_rate * SI5351_PLL_A_MIN;
+	if (a > SI5351_PLL_A_MAX)
+		rate = *parent_rate * SI5351_PLL_A_MAX;
+
+	/* find best approximation for b/c = fVCO mod fIN */
+	denom = 1000 * 1000;
+	lltmp = rate % (*parent_rate);
+	lltmp *= denom;
+	do_div(lltmp, *parent_rate);
+	rfrac = (unsigned long)lltmp;
+
+	b = 0;
+	c = 1;
+	if (rfrac)
+		rational_best_approximation(rfrac, denom,
+				    SI5351_PLL_B_MAX, SI5351_PLL_C_MAX, &b, &c);
+
+	/* calculate parameters */
+	hwdata->params.p3  = c;
+	hwdata->params.p2  = (128 * b) % c;
+	hwdata->params.p1  = 128 * a;
+	hwdata->params.p1 += (128 * b / c);
+	hwdata->params.p1 -= 512;
+
+	/* recalculate rate by fIN * (a + b/c) */
+	lltmp  = *parent_rate;
+	lltmp *= b;
+	do_div(lltmp, c);
+
+	rate  = (unsigned long)lltmp;
+	rate += *parent_rate * a;
+
+	dev_dbg(&hwdata->drvdata->client->dev,
+		"%s - %s: a = %lu, b = %lu, c = %lu, parent_rate = %lu, rate = %lu\n",
+		__func__, __clk_get_name(hwdata->hw.clk), a, b, c,
+		*parent_rate, rate);
+
+	return rate;
+}
+
+static int si5351_pll_set_rate(struct clk_hw *hw, unsigned long rate,
+			       unsigned long parent_rate)
+{
+	struct si5351_hw_data *hwdata =
+		container_of(hw, struct si5351_hw_data, hw);
+	u8 reg = (hwdata->num == 0) ? SI5351_PLLA_PARAMETERS :
+		SI5351_PLLB_PARAMETERS;
+
+	/* write multisynth parameters */
+	si5351_write_parameters(hwdata->drvdata, reg, &hwdata->params);
+
+	/* plla/pllb ctrl is in clk6/clk7 ctrl registers */
+	si5351_set_bits(hwdata->drvdata, SI5351_CLK6_CTRL + hwdata->num,
+		SI5351_CLK_INTEGER_MODE,
+		(hwdata->params.p2 == 0) ? SI5351_CLK_INTEGER_MODE : 0);
+
+	dev_dbg(&hwdata->drvdata->client->dev,
+		"%s - %s: p1 = %lu, p2 = %lu, p3 = %lu, parent_rate = %lu, rate = %lu\n",
+		__func__, __clk_get_name(hwdata->hw.clk),
+		hwdata->params.p1, hwdata->params.p2, hwdata->params.p3,
+		parent_rate, rate);
+
+	return 0;
+}
+
+static const struct clk_ops si5351_pll_ops = {
+	.set_parent = si5351_pll_set_parent,
+	.get_parent = si5351_pll_get_parent,
+	.recalc_rate = si5351_pll_recalc_rate,
+	.round_rate = si5351_pll_round_rate,
+	.set_rate = si5351_pll_set_rate,
+};
+
+/*
+ * Si5351 multisync divider
+ *
+ * for fOUT <= 150 MHz:
+ *
+ * fOUT = (fIN * (a + b/c)) / CLKOUTDIV
+ *
+ * with 6 + 0/1048575 <= (a + b/c) <= 1800 + 0/1048575 and
+ * fIN = fVCO0, fVCO1
+ *
+ * Output Clock Multisynth Register Equations
+ *
+ * MSx_P1[17:0] = 128 * a + floor(128 * b/c) - 512
+ * MSx_P2[19:0] = 128 * b - c * floor(128 * b/c) = (128*b) mod c
+ * MSx_P3[19:0] = c
+ *
+ * MS[6,7] are integer (P1) divide only, P2 = 0, P3 = 0
+ *
+ * for 150MHz < fOUT <= 160MHz:
+ *
+ * MSx_P1 = 0, MSx_P2 = 0, MSx_P3 = 1, MSx_INT = 1, MSx_DIVBY4 = 11b
+ */
+static int _si5351_msynth_reparent(struct si5351_driver_data *drvdata,
+				   int num, enum si5351_multisynth_src parent)
+{
+	if (parent == SI5351_MULTISYNTH_SRC_DEFAULT)
+		return 0;
+
+	if (num > 8)
+		return -EINVAL;
+
+	si5351_set_bits(drvdata, SI5351_CLK0_CTRL + num, SI5351_CLK_PLL_SELECT,
+			(parent == SI5351_MULTISYNTH_SRC_VCO0) ? 0 :
+			SI5351_CLK_PLL_SELECT);
+	return 0;
+}
+
+static unsigned char si5351_msynth_get_parent(struct clk_hw *hw)
+{
+	struct si5351_hw_data *hwdata =
+		container_of(hw, struct si5351_hw_data, hw);
+	u8 val;
+
+	val = si5351_reg_read(hwdata->drvdata, SI5351_CLK0_CTRL + hwdata->num);
+
+	return (val & SI5351_CLK_PLL_SELECT) ? 1 : 0;
+}
+
+static int si5351_msynth_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct si5351_hw_data *hwdata =
+		container_of(hw, struct si5351_hw_data, hw);
+
+	return _si5351_msynth_reparent(hwdata->drvdata, hwdata->num,
+			       (index == 0) ? SI5351_MULTISYNTH_SRC_VCO0 :
+			       SI5351_MULTISYNTH_SRC_VCO1);
+}
+
+static unsigned long si5351_msynth_recalc_rate(struct clk_hw *hw,
+					       unsigned long parent_rate)
+{
+	struct si5351_hw_data *hwdata =
+		container_of(hw, struct si5351_hw_data, hw);
+	u8 reg = si5351_msynth_params_address(hwdata->num);
+	unsigned long long rate;
+	unsigned long m;
+
+	if (!hwdata->params.valid)
+		si5351_read_parameters(hwdata->drvdata, reg, &hwdata->params);
+
+	if (hwdata->params.p3 == 0)
+		return parent_rate;
+
+	/*
+	 * multisync0-5: fOUT = (128 * P3 * fIN) / (P1*P3 + P2 + 512*P3)
+	 * multisync6-7: fOUT = fIN / P1
+	 */
+	rate = parent_rate;
+	if (hwdata->num > 5) {
+		m = hwdata->params.p1;
+	} else if ((si5351_reg_read(hwdata->drvdata, reg + 2) &
+		    SI5351_OUTPUT_CLK_DIVBY4) == SI5351_OUTPUT_CLK_DIVBY4) {
+		m = 4;
+	} else {
+		rate *= 128 * hwdata->params.p3;
+		m = hwdata->params.p1 * hwdata->params.p3;
+		m += hwdata->params.p2;
+		m += 512 * hwdata->params.p3;
+	}
+
+	if (m == 0)
+		return 0;
+	do_div(rate, m);
+
+	dev_dbg(&hwdata->drvdata->client->dev,
+		"%s - %s: p1 = %lu, p2 = %lu, p3 = %lu, m = %lu, parent_rate = %lu, rate = %lu\n",
+		__func__, __clk_get_name(hwdata->hw.clk),
+		hwdata->params.p1, hwdata->params.p2, hwdata->params.p3,
+		m, parent_rate, (unsigned long)rate);
+
+	return (unsigned long)rate;
+}
+
+static long si5351_msynth_round_rate(struct clk_hw *hw, unsigned long rate,
+				     unsigned long *parent_rate)
+{
+	struct si5351_hw_data *hwdata =
+		container_of(hw, struct si5351_hw_data, hw);
+	unsigned long long lltmp;
+	unsigned long a, b, c;
+	int divby4;
+
+	/* multisync6-7 can only handle freqencies < 150MHz */
+	if (hwdata->num >= 6 && rate > SI5351_MULTISYNTH67_MAX_FREQ)
+		rate = SI5351_MULTISYNTH67_MAX_FREQ;
+
+	/* multisync frequency is 1MHz .. 160MHz */
+	if (rate > SI5351_MULTISYNTH_MAX_FREQ)
+		rate = SI5351_MULTISYNTH_MAX_FREQ;
+	if (rate < SI5351_MULTISYNTH_MIN_FREQ)
+		rate = SI5351_MULTISYNTH_MIN_FREQ;
+
+	divby4 = 0;
+	if (rate > SI5351_MULTISYNTH_DIVBY4_FREQ)
+		divby4 = 1;
+
+	/* multisync can set pll */
+	if (__clk_get_flags(hwdata->hw.clk) & CLK_SET_RATE_PARENT) {
+		/*
+		 * find largest integer divider for max
+		 * vco frequency and given target rate
+		 */
+		if (divby4 == 0) {
+			lltmp = SI5351_PLL_VCO_MAX;
+			do_div(lltmp, rate);
+			a = (unsigned long)lltmp;
+		} else
+			a = 4;
+
+		b = 0;
+		c = 1;
+
+		*parent_rate = a * rate;
+	} else {
+		unsigned long rfrac, denom;
+
+		/* disable divby4 */
+		if (divby4) {
+			rate = SI5351_MULTISYNTH_DIVBY4_FREQ;
+			divby4 = 0;
+		}
+
+		/* determine integer part of divider equation */
+		a = *parent_rate / rate;
+		if (a < SI5351_MULTISYNTH_A_MIN)
+			a = SI5351_MULTISYNTH_A_MIN;
+		if (hwdata->num >= 6 && a > SI5351_MULTISYNTH67_A_MAX)
+			a = SI5351_MULTISYNTH67_A_MAX;
+		else if (a > SI5351_MULTISYNTH_A_MAX)
+			a = SI5351_MULTISYNTH_A_MAX;
+
+		/* find best approximation for b/c = fVCO mod fOUT */
+		denom = 1000 * 1000;
+		lltmp = (*parent_rate) % rate;
+		lltmp *= denom;
+		do_div(lltmp, rate);
+		rfrac = (unsigned long)lltmp;
+
+		b = 0;
+		c = 1;
+		if (rfrac)
+			rational_best_approximation(rfrac, denom,
+			    SI5351_MULTISYNTH_B_MAX, SI5351_MULTISYNTH_C_MAX,
+			    &b, &c);
+	}
+
+	/* recalculate rate by fOUT = fIN / (a + b/c) */
+	lltmp  = *parent_rate;
+	lltmp *= c;
+	do_div(lltmp, a * c + b);
+	rate  = (unsigned long)lltmp;
+
+	/* calculate parameters */
+	if (divby4) {
+		hwdata->params.p3 = 1;
+		hwdata->params.p2 = 0;
+		hwdata->params.p1 = 0;
+	} else {
+		hwdata->params.p3  = c;
+		hwdata->params.p2  = (128 * b) % c;
+		hwdata->params.p1  = 128 * a;
+		hwdata->params.p1 += (128 * b / c);
+		hwdata->params.p1 -= 512;
+	}
+
+	dev_dbg(&hwdata->drvdata->client->dev,
+		"%s - %s: a = %lu, b = %lu, c = %lu, divby4 = %d, parent_rate = %lu, rate = %lu\n",
+		__func__, __clk_get_name(hwdata->hw.clk), a, b, c, divby4,
+		*parent_rate, rate);
+
+	return rate;
+}
+
+static int si5351_msynth_set_rate(struct clk_hw *hw, unsigned long rate,
+				  unsigned long parent_rate)
+{
+	struct si5351_hw_data *hwdata =
+		container_of(hw, struct si5351_hw_data, hw);
+	u8 reg = si5351_msynth_params_address(hwdata->num);
+	int divby4 = 0;
+
+	/* write multisynth parameters */
+	si5351_write_parameters(hwdata->drvdata, reg, &hwdata->params);
+
+	if (rate > SI5351_MULTISYNTH_DIVBY4_FREQ)
+		divby4 = 1;
+
+	/* enable/disable integer mode and divby4 on multisynth0-5 */
+	if (hwdata->num < 6) {
+		si5351_set_bits(hwdata->drvdata, reg + 2,
+				SI5351_OUTPUT_CLK_DIVBY4,
+				(divby4) ? SI5351_OUTPUT_CLK_DIVBY4 : 0);
+		si5351_set_bits(hwdata->drvdata, SI5351_CLK0_CTRL + hwdata->num,
+			SI5351_CLK_INTEGER_MODE,
+			(hwdata->params.p2 == 0) ? SI5351_CLK_INTEGER_MODE : 0);
+	}
+
+	dev_dbg(&hwdata->drvdata->client->dev,
+		"%s - %s: p1 = %lu, p2 = %lu, p3 = %lu, divby4 = %d, parent_rate = %lu, rate = %lu\n",
+		__func__, __clk_get_name(hwdata->hw.clk),
+		hwdata->params.p1, hwdata->params.p2, hwdata->params.p3,
+		divby4, parent_rate, rate);
+
+	return 0;
+}
+
+static const struct clk_ops si5351_msynth_ops = {
+	.set_parent = si5351_msynth_set_parent,
+	.get_parent = si5351_msynth_get_parent,
+	.recalc_rate = si5351_msynth_recalc_rate,
+	.round_rate = si5351_msynth_round_rate,
+	.set_rate = si5351_msynth_set_rate,
+};
+
+/*
+ * Si5351 clkout divider
+ */
+static int _si5351_clkout_reparent(struct si5351_driver_data *drvdata,
+				   int num, enum si5351_clkout_src parent)
+{
+	u8 val;
+
+	if (num > 8)
+		return -EINVAL;
+
+	switch (parent) {
+	case SI5351_CLKOUT_SRC_MSYNTH_N:
+		val = SI5351_CLK_INPUT_MULTISYNTH_N;
+		break;
+	case SI5351_CLKOUT_SRC_MSYNTH_0_4:
+		/* clk0/clk4 can only connect to its own multisync */
+		if (num == 0 || num == 4)
+			val = SI5351_CLK_INPUT_MULTISYNTH_N;
+		else
+			val = SI5351_CLK_INPUT_MULTISYNTH_0_4;
+		break;
+	case SI5351_CLKOUT_SRC_XTAL:
+		val = SI5351_CLK_INPUT_XTAL;
+		break;
+	case SI5351_CLKOUT_SRC_CLKIN:
+		if (drvdata->variant != SI5351_VARIANT_C)
+			return -EINVAL;
+
+		val = SI5351_CLK_INPUT_CLKIN;
+		break;
+	default:
+		return 0;
+	}
+
+	si5351_set_bits(drvdata, SI5351_CLK0_CTRL + num,
+			SI5351_CLK_INPUT_MASK, val);
+	return 0;
+}
+
+static int _si5351_clkout_set_drive_strength(
+	struct si5351_driver_data *drvdata, int num,
+	enum si5351_drive_strength drive)
+{
+	u8 mask;
+
+	if (num > 8)
+		return -EINVAL;
+
+	switch (drive) {
+	case SI5351_DRIVE_2MA:
+		mask = SI5351_CLK_DRIVE_STRENGTH_2MA;
+		break;
+	case SI5351_DRIVE_4MA:
+		mask = SI5351_CLK_DRIVE_STRENGTH_4MA;
+		break;
+	case SI5351_DRIVE_6MA:
+		mask = SI5351_CLK_DRIVE_STRENGTH_6MA;
+		break;
+	case SI5351_DRIVE_8MA:
+		mask = SI5351_CLK_DRIVE_STRENGTH_8MA;
+		break;
+	default:
+		return 0;
+	}
+
+	si5351_set_bits(drvdata, SI5351_CLK0_CTRL + num,
+			SI5351_CLK_DRIVE_STRENGTH_MASK, mask);
+	return 0;
+}
+
+static int si5351_clkout_prepare(struct clk_hw *hw)
+{
+	struct si5351_hw_data *hwdata =
+		container_of(hw, struct si5351_hw_data, hw);
+
+	si5351_set_bits(hwdata->drvdata, SI5351_CLK0_CTRL + hwdata->num,
+			SI5351_CLK_POWERDOWN, 0);
+	si5351_set_bits(hwdata->drvdata, SI5351_OUTPUT_ENABLE_CTRL,
+			(1 << hwdata->num), 0);
+	return 0;
+}
+
+static void si5351_clkout_unprepare(struct clk_hw *hw)
+{
+	struct si5351_hw_data *hwdata =
+		container_of(hw, struct si5351_hw_data, hw);
+
+	si5351_set_bits(hwdata->drvdata, SI5351_CLK0_CTRL + hwdata->num,
+			SI5351_CLK_POWERDOWN, SI5351_CLK_POWERDOWN);
+	si5351_set_bits(hwdata->drvdata, SI5351_OUTPUT_ENABLE_CTRL,
+			(1 << hwdata->num), (1 << hwdata->num));
+}
+
+static u8 si5351_clkout_get_parent(struct clk_hw *hw)
+{
+	struct si5351_hw_data *hwdata =
+		container_of(hw, struct si5351_hw_data, hw);
+	int index = 0;
+	unsigned char val;
+
+	val = si5351_reg_read(hwdata->drvdata, SI5351_CLK0_CTRL + hwdata->num);
+	switch (val & SI5351_CLK_INPUT_MASK) {
+	case SI5351_CLK_INPUT_MULTISYNTH_N:
+		index = 0;
+		break;
+	case SI5351_CLK_INPUT_MULTISYNTH_0_4:
+		index = 1;
+		break;
+	case SI5351_CLK_INPUT_XTAL:
+		index = 2;
+		break;
+	case SI5351_CLK_INPUT_CLKIN:
+		index = 3;
+		break;
+	}
+
+	return index;
+}
+
+static int si5351_clkout_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct si5351_hw_data *hwdata =
+		container_of(hw, struct si5351_hw_data, hw);
+	enum si5351_clkout_src parent = SI5351_CLKOUT_SRC_DEFAULT;
+
+	switch (index) {
+	case 0:
+		parent = SI5351_CLKOUT_SRC_MSYNTH_N;
+		break;
+	case 1:
+		parent = SI5351_CLKOUT_SRC_MSYNTH_0_4;
+		break;
+	case 2:
+		parent = SI5351_CLKOUT_SRC_XTAL;
+		break;
+	case 3:
+		parent = SI5351_CLKOUT_SRC_CLKIN;
+		break;
+	}
+
+	return _si5351_clkout_reparent(hwdata->drvdata, hwdata->num, parent);
+}
+
+static unsigned long si5351_clkout_recalc_rate(struct clk_hw *hw,
+					       unsigned long parent_rate)
+{
+	struct si5351_hw_data *hwdata =
+		container_of(hw, struct si5351_hw_data, hw);
+	unsigned char reg;
+	unsigned char rdiv;
+
+	if (hwdata->num > 5)
+		reg = si5351_msynth_params_address(hwdata->num) + 2;
+	else
+		reg = SI5351_CLK6_7_OUTPUT_DIVIDER;
+
+	rdiv = si5351_reg_read(hwdata->drvdata, reg);
+	if (hwdata->num == 6) {
+		rdiv &= SI5351_OUTPUT_CLK6_DIV_MASK;
+	} else {
+		rdiv &= SI5351_OUTPUT_CLK_DIV_MASK;
+		rdiv >>= SI5351_OUTPUT_CLK_DIV_SHIFT;
+	}
+
+	return parent_rate >> rdiv;
+}
+
+static long si5351_clkout_round_rate(struct clk_hw *hw, unsigned long rate,
+				     unsigned long *parent_rate)
+{
+	struct si5351_hw_data *hwdata =
+		container_of(hw, struct si5351_hw_data, hw);
+	unsigned char rdiv;
+
+	/* clkout6/7 can only handle output freqencies < 150MHz */
+	if (hwdata->num >= 6 && rate > SI5351_CLKOUT67_MAX_FREQ)
+		rate = SI5351_CLKOUT67_MAX_FREQ;
+
+	/* clkout freqency is 8kHz - 160MHz */
+	if (rate > SI5351_CLKOUT_MAX_FREQ)
+		rate = SI5351_CLKOUT_MAX_FREQ;
+	if (rate < SI5351_CLKOUT_MIN_FREQ)
+		rate = SI5351_CLKOUT_MIN_FREQ;
+
+	/* request frequency if multisync master */
+	if (__clk_get_flags(hwdata->hw.clk) & CLK_SET_RATE_PARENT) {
+		/* use r divider for frequencies below 1MHz */
+		rdiv = SI5351_OUTPUT_CLK_DIV_1;
+		while (rate < SI5351_MULTISYNTH_MIN_FREQ &&
+		       rdiv < SI5351_OUTPUT_CLK_DIV_128) {
+			rdiv += 1;
+			rate *= 2;
+		}
+		*parent_rate = rate;
+	} else {
+		unsigned long new_rate, new_err, err;
+
+		/* round to closed rdiv */
+		rdiv = SI5351_OUTPUT_CLK_DIV_1;
+		new_rate = *parent_rate;
+		err = abs(new_rate - rate);
+		do {
+			new_rate >>= 1;
+			new_err = abs(new_rate - rate);
+			if (new_err > err || rdiv == SI5351_OUTPUT_CLK_DIV_128)
+				break;
+			rdiv++;
+			err = new_err;
+		} while (1);
+	}
+	rate = *parent_rate >> rdiv;
+
+	dev_dbg(&hwdata->drvdata->client->dev,
+		"%s - %s: rdiv = %u, parent_rate = %lu, rate = %lu\n",
+		__func__, __clk_get_name(hwdata->hw.clk), (1 << rdiv),
+		*parent_rate, rate);
+
+	return rate;
+}
+
+static int si5351_clkout_set_rate(struct clk_hw *hw, unsigned long rate,
+				  unsigned long parent_rate)
+{
+	struct si5351_hw_data *hwdata =
+		container_of(hw, struct si5351_hw_data, hw);
+	unsigned long new_rate, new_err, err;
+	unsigned char rdiv;
+
+	/* round to closed rdiv */
+	rdiv = SI5351_OUTPUT_CLK_DIV_1;
+	new_rate = parent_rate;
+	err = abs(new_rate - rate);
+	do {
+		new_rate >>= 1;
+		new_err = abs(new_rate - rate);
+		if (new_err > err || rdiv == SI5351_OUTPUT_CLK_DIV_128)
+			break;
+		rdiv++;
+		err = new_err;
+	} while (1);
+
+	/* write output divider */
+	switch (hwdata->num) {
+	case 6:
+		si5351_set_bits(hwdata->drvdata, SI5351_CLK6_7_OUTPUT_DIVIDER,
+				SI5351_OUTPUT_CLK6_DIV_MASK, rdiv);
+		break;
+	case 7:
+		si5351_set_bits(hwdata->drvdata, SI5351_CLK6_7_OUTPUT_DIVIDER,
+				SI5351_OUTPUT_CLK_DIV_MASK,
+				rdiv << SI5351_OUTPUT_CLK_DIV_SHIFT);
+		break;
+	default:
+		si5351_set_bits(hwdata->drvdata,
+				si5351_msynth_params_address(hwdata->num) + 2,
+				SI5351_OUTPUT_CLK_DIV_MASK,
+				rdiv << SI5351_OUTPUT_CLK_DIV_SHIFT);
+	}
+
+	/* powerup clkout */
+	si5351_set_bits(hwdata->drvdata, SI5351_CLK0_CTRL + hwdata->num,
+			SI5351_CLK_POWERDOWN, 0);
+
+	dev_dbg(&hwdata->drvdata->client->dev,
+		"%s - %s: rdiv = %u, parent_rate = %lu, rate = %lu\n",
+		__func__, __clk_get_name(hwdata->hw.clk), (1 << rdiv),
+		parent_rate, rate);
+
+	return 0;
+}
+
+static const struct clk_ops si5351_clkout_ops = {
+	.prepare = si5351_clkout_prepare,
+	.unprepare = si5351_clkout_unprepare,
+	.set_parent = si5351_clkout_set_parent,
+	.get_parent = si5351_clkout_get_parent,
+	.recalc_rate = si5351_clkout_recalc_rate,
+	.round_rate = si5351_clkout_round_rate,
+	.set_rate = si5351_clkout_set_rate,
+};
+
+/*
+ * Si5351 i2c probe and DT
+ */
+#ifdef CONFIG_OF
+static const struct of_device_id si5351_dt_ids[] = {
+	{ .compatible = "silabs,si5351a", .data = (void *)SI5351_VARIANT_A, },
+	{ .compatible = "silabs,si5351a-msop",
+					 .data = (void *)SI5351_VARIANT_A3, },
+	{ .compatible = "silabs,si5351b", .data = (void *)SI5351_VARIANT_B, },
+	{ .compatible = "silabs,si5351c", .data = (void *)SI5351_VARIANT_C, },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, si5351_dt_ids);
+
+static int si5351_dt_parse(struct i2c_client *client)
+{
+	struct device_node *child, *np = client->dev.of_node;
+	struct si5351_platform_data *pdata;
+	const struct of_device_id *match;
+	struct property *prop;
+	const __be32 *p;
+	int num = 0;
+	u32 val;
+
+	if (np == NULL)
+		return 0;
+
+	match = of_match_node(si5351_dt_ids, np);
+	if (match == NULL)
+		return -EINVAL;
+
+	pdata = devm_kzalloc(&client->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return -ENOMEM;
+
+	pdata->variant = (enum si5351_variant)match->data;
+	pdata->clk_xtal = of_clk_get(np, 0);
+	if (!IS_ERR(pdata->clk_xtal))
+		clk_put(pdata->clk_xtal);
+	pdata->clk_clkin = of_clk_get(np, 1);
+	if (!IS_ERR(pdata->clk_clkin))
+		clk_put(pdata->clk_clkin);
+
+	/*
+	 * property silabs,pll-source : <num src>, [<..>]
+	 * allow to selectively set pll source
+	 */
+	of_property_for_each_u32(np, "silabs,pll-source", prop, p, num) {
+		if (num >= 2) {
+			dev_err(&client->dev,
+				"invalid pll %d on pll-source prop\n", num);
+			return -EINVAL;
+		}
+
+		p = of_prop_next_u32(prop, p, &val);
+		if (!p) {
+			dev_err(&client->dev,
+				"missing pll-source for pll %d\n", num);
+			return -EINVAL;
+		}
+
+		switch (val) {
+		case 0:
+			pdata->pll_src[num] = SI5351_PLL_SRC_XTAL;
+			break;
+		case 1:
+			if (pdata->variant != SI5351_VARIANT_C) {
+				dev_err(&client->dev,
+					"invalid parent %d for pll %d\n",
+					val, num);
+				return -EINVAL;
+			}
+			pdata->pll_src[num] = SI5351_PLL_SRC_CLKIN;
+			break;
+		default:
+			dev_err(&client->dev,
+				 "invalid parent %d for pll %d\n", val, num);
+			return -EINVAL;
+		}
+	}
+
+	/* per clkout properties */
+	for_each_child_of_node(np, child) {
+		if (of_property_read_u32(child, "reg", &num)) {
+			dev_err(&client->dev, "missing reg property of %s\n",
+				child->name);
+			return -EINVAL;
+		}
+
+		if (num >= 8 ||
+		    (pdata->variant == SI5351_VARIANT_A3 && num >= 3)) {
+			dev_err(&client->dev, "invalid clkout %d\n", num);
+			return -EINVAL;
+		}
+
+		if (!of_property_read_u32(child, "silabs,multisynth-source",
+					  &val)) {
+			switch (val) {
+			case 0:
+				pdata->clkout[num].multisynth_src =
+					SI5351_MULTISYNTH_SRC_VCO0;
+				break;
+			case 1:
+				pdata->clkout[num].multisynth_src =
+					SI5351_MULTISYNTH_SRC_VCO1;
+				break;
+			default:
+				dev_err(&client->dev,
+					"invalid parent %d for multisynth %d\n",
+					val, num);
+				return -EINVAL;
+			}
+		}
+
+		if (!of_property_read_u32(child, "silabs,clock-source", &val)) {
+			switch (val) {
+			case 0:
+				pdata->clkout[num].clkout_src =
+					SI5351_CLKOUT_SRC_MSYNTH_N;
+				break;
+			case 1:
+				pdata->clkout[num].clkout_src =
+					SI5351_CLKOUT_SRC_MSYNTH_0_4;
+				break;
+			case 2:
+				pdata->clkout[num].clkout_src =
+					SI5351_CLKOUT_SRC_XTAL;
+				break;
+			case 3:
+				if (pdata->variant != SI5351_VARIANT_C) {
+					dev_err(&client->dev,
+						"invalid parent %d for clkout %d\n",
+						val, num);
+					return -EINVAL;
+				}
+				pdata->clkout[num].clkout_src =
+					SI5351_CLKOUT_SRC_CLKIN;
+				break;
+			default:
+				dev_err(&client->dev,
+					"invalid parent %d for clkout %d\n",
+					val, num);
+				return -EINVAL;
+			}
+		}
+
+		if (!of_property_read_u32(child, "silabs,drive-strength",
+					  &val)) {
+			switch (val) {
+			case SI5351_DRIVE_2MA:
+			case SI5351_DRIVE_4MA:
+			case SI5351_DRIVE_6MA:
+			case SI5351_DRIVE_8MA:
+				pdata->clkout[num].drive = val;
+				break;
+			default:
+				dev_err(&client->dev,
+					"invalid drive strength %d for clkout %d\n",
+					val, num);
+				return -EINVAL;
+			}
+		}
+
+		if (!of_property_read_u32(child, "clock-frequency", &val))
+			pdata->clkout[num].rate = val;
+
+		pdata->clkout[num].pll_master =
+			of_property_read_bool(child, "silabs,pll-master");
+	}
+	client->dev.platform_data = pdata;
+
+	return 0;
+}
+#else
+static int si5351_dt_parse(struct i2c_client *client)
+{
+	return 0;
+}
+#endif /* CONFIG_OF */
+
+static int si5351_i2c_probe(struct i2c_client *client,
+			    const struct i2c_device_id *id)
+{
+	struct si5351_platform_data *pdata;
+	struct si5351_driver_data *drvdata;
+	struct clk_init_data init;
+	struct clk *clk;
+	const char *parent_names[4];
+	u8 num_parents, num_clocks;
+	int ret, n;
+
+	ret = si5351_dt_parse(client);
+	if (ret)
+		return ret;
+
+	pdata = client->dev.platform_data;
+	if (!pdata)
+		return -EINVAL;
+
+	drvdata = devm_kzalloc(&client->dev, sizeof(*drvdata), GFP_KERNEL);
+	if (drvdata == NULL) {
+		dev_err(&client->dev, "unable to allocate driver data\n");
+		return -ENOMEM;
+	}
+
+	i2c_set_clientdata(client, drvdata);
+	drvdata->client = client;
+	drvdata->variant = pdata->variant;
+	drvdata->pxtal = pdata->clk_xtal;
+	drvdata->pclkin = pdata->clk_clkin;
+
+	drvdata->regmap = devm_regmap_init_i2c(client, &si5351_regmap_config);
+	if (IS_ERR(drvdata->regmap)) {
+		dev_err(&client->dev, "failed to allocate register map\n");
+		return PTR_ERR(drvdata->regmap);
+	}
+
+	/* Disable interrupts */
+	si5351_reg_write(drvdata, SI5351_INTERRUPT_MASK, 0xf0);
+	/* Set disabled output drivers to drive low */
+	si5351_reg_write(drvdata, SI5351_CLK3_0_DISABLE_STATE, 0x00);
+	si5351_reg_write(drvdata, SI5351_CLK7_4_DISABLE_STATE, 0x00);
+	/* Ensure pll select is on XTAL for Si5351A/B */
+	if (drvdata->variant != SI5351_VARIANT_C)
+		si5351_set_bits(drvdata, SI5351_PLL_INPUT_SOURCE,
+				SI5351_PLLA_SOURCE | SI5351_PLLB_SOURCE, 0);
+
+	/* setup clock configuration */
+	for (n = 0; n < 2; n++) {
+		ret = _si5351_pll_reparent(drvdata, n, pdata->pll_src[n]);
+		if (ret) {
+			dev_err(&client->dev,
+				"failed to reparent pll %d to %d\n",
+				n, pdata->pll_src[n]);
+			return ret;
+		}
+	}
+
+	for (n = 0; n < 8; n++) {
+		ret = _si5351_msynth_reparent(drvdata, n,
+					      pdata->clkout[n].multisynth_src);
+		if (ret) {
+			dev_err(&client->dev,
+				"failed to reparent multisynth %d to %d\n",
+				n, pdata->clkout[n].multisynth_src);
+			return ret;
+		}
+
+		ret = _si5351_clkout_reparent(drvdata, n,
+					      pdata->clkout[n].clkout_src);
+		if (ret) {
+			dev_err(&client->dev,
+				"failed to reparent clkout %d to %d\n",
+				n, pdata->clkout[n].clkout_src);
+			return ret;
+		}
+
+		ret = _si5351_clkout_set_drive_strength(drvdata, n,
+							pdata->clkout[n].drive);
+		if (ret) {
+			dev_err(&client->dev,
+				"failed set drive strength of clkout%d to %d\n",
+				n, pdata->clkout[n].drive);
+			return ret;
+		}
+	}
+
+	/* register xtal input clock gate */
+	memset(&init, 0, sizeof(init));
+	init.name = si5351_input_names[0];
+	init.ops = &si5351_xtal_ops;
+	init.flags = 0;
+	if (!IS_ERR(drvdata->pxtal)) {
+		drvdata->pxtal_name = __clk_get_name(drvdata->pxtal);
+		init.parent_names = &drvdata->pxtal_name;
+		init.num_parents = 1;
+	}
+	drvdata->xtal.init = &init;
+	clk = devm_clk_register(&client->dev, &drvdata->xtal);
+	if (IS_ERR(clk)) {
+		dev_err(&client->dev, "unable to register %s\n", init.name);
+		return PTR_ERR(clk);
+	}
+
+	/* register clkin input clock gate */
+	if (drvdata->variant == SI5351_VARIANT_C) {
+		memset(&init, 0, sizeof(init));
+		init.name = si5351_input_names[1];
+		init.ops = &si5351_clkin_ops;
+		if (!IS_ERR(drvdata->pclkin)) {
+			drvdata->pclkin_name = __clk_get_name(drvdata->pclkin);
+			init.parent_names = &drvdata->pclkin_name;
+			init.num_parents = 1;
+		}
+		drvdata->clkin.init = &init;
+		clk = devm_clk_register(&client->dev, &drvdata->clkin);
+		if (IS_ERR(clk)) {
+			dev_err(&client->dev, "unable to register %s\n",
+				init.name);
+			return PTR_ERR(clk);
+		}
+	}
+
+	/* Si5351C allows to mux either xtal or clkin to PLL input */
+	num_parents = (drvdata->variant == SI5351_VARIANT_C) ? 2 : 1;
+	parent_names[0] = si5351_input_names[0];
+	parent_names[1] = si5351_input_names[1];
+
+	/* register PLLA */
+	drvdata->pll[0].num = 0;
+	drvdata->pll[0].drvdata = drvdata;
+	drvdata->pll[0].hw.init = &init;
+	memset(&init, 0, sizeof(init));
+	init.name = si5351_pll_names[0];
+	init.ops = &si5351_pll_ops;
+	init.flags = 0;
+	init.parent_names = parent_names;
+	init.num_parents = num_parents;
+	clk = devm_clk_register(&client->dev, &drvdata->pll[0].hw);
+	if (IS_ERR(clk)) {
+		dev_err(&client->dev, "unable to register %s\n", init.name);
+		return -EINVAL;
+	}
+
+	/* register PLLB or VXCO (Si5351B) */
+	drvdata->pll[1].num = 1;
+	drvdata->pll[1].drvdata = drvdata;
+	drvdata->pll[1].hw.init = &init;
+	memset(&init, 0, sizeof(init));
+	if (drvdata->variant == SI5351_VARIANT_B) {
+		init.name = si5351_pll_names[2];
+		init.ops = &si5351_vxco_ops;
+		init.flags = CLK_IS_ROOT;
+		init.parent_names = NULL;
+		init.num_parents = 0;
+	} else {
+		init.name = si5351_pll_names[1];
+		init.ops = &si5351_pll_ops;
+		init.flags = 0;
+		init.parent_names = parent_names;
+		init.num_parents = num_parents;
+	}
+	clk = devm_clk_register(&client->dev, &drvdata->pll[1].hw);
+	if (IS_ERR(clk)) {
+		dev_err(&client->dev, "unable to register %s\n", init.name);
+		return -EINVAL;
+	}
+
+	/* register clk multisync and clk out divider */
+	num_clocks = (drvdata->variant == SI5351_VARIANT_A3) ? 3 : 8;
+	parent_names[0] = si5351_pll_names[0];
+	if (drvdata->variant == SI5351_VARIANT_B)
+		parent_names[1] = si5351_pll_names[2];
+	else
+		parent_names[1] = si5351_pll_names[1];
+
+	drvdata->msynth = devm_kzalloc(&client->dev, num_clocks *
+				       sizeof(*drvdata->msynth), GFP_KERNEL);
+	drvdata->clkout = devm_kzalloc(&client->dev, num_clocks *
+				       sizeof(*drvdata->clkout), GFP_KERNEL);
+
+	drvdata->onecell.clk_num = num_clocks;
+	drvdata->onecell.clks = devm_kzalloc(&client->dev,
+		num_clocks * sizeof(*drvdata->onecell.clks), GFP_KERNEL);
+
+	if (WARN_ON(!drvdata->msynth || !drvdata->clkout ||
+		    !drvdata->onecell.clks))
+		return -ENOMEM;
+
+	for (n = 0; n < num_clocks; n++) {
+		drvdata->msynth[n].num = n;
+		drvdata->msynth[n].drvdata = drvdata;
+		drvdata->msynth[n].hw.init = &init;
+		memset(&init, 0, sizeof(init));
+		init.name = si5351_msynth_names[n];
+		init.ops = &si5351_msynth_ops;
+		init.flags = 0;
+		if (pdata->clkout[n].pll_master)
+			init.flags |= CLK_SET_RATE_PARENT;
+		init.parent_names = parent_names;
+		init.num_parents = 2;
+		clk = devm_clk_register(&client->dev, &drvdata->msynth[n].hw);
+		if (IS_ERR(clk)) {
+			dev_err(&client->dev, "unable to register %s\n",
+				init.name);
+			return -EINVAL;
+		}
+	}
+
+	num_parents = (drvdata->variant == SI5351_VARIANT_C) ? 4 : 3;
+	parent_names[2] = si5351_input_names[0];
+	parent_names[3] = si5351_input_names[1];
+	for (n = 0; n < num_clocks; n++) {
+		parent_names[0] = si5351_msynth_names[n];
+		parent_names[1] = (n < 4) ? si5351_msynth_names[0] :
+			si5351_msynth_names[4];
+
+		drvdata->clkout[n].num = n;
+		drvdata->clkout[n].drvdata = drvdata;
+		drvdata->clkout[n].hw.init = &init;
+		memset(&init, 0, sizeof(init));
+		init.name = si5351_clkout_names[n];
+		init.ops = &si5351_clkout_ops;
+		init.flags = 0;
+		if (pdata->clkout[n].clkout_src == SI5351_CLKOUT_SRC_MSYNTH_N)
+			init.flags |= CLK_SET_RATE_PARENT;
+		init.parent_names = parent_names;
+		init.num_parents = num_parents;
+		clk = devm_clk_register(&client->dev, &drvdata->clkout[n].hw);
+		if (IS_ERR(clk)) {
+			dev_err(&client->dev, "unable to register %s\n",
+				init.name);
+			return -EINVAL;
+		}
+		drvdata->onecell.clks[n] = clk;
+	}
+
+	ret = of_clk_add_provider(client->dev.of_node, of_clk_src_onecell_get,
+				  &drvdata->onecell);
+	if (ret) {
+		dev_err(&client->dev, "unable to add clk provider\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static const struct i2c_device_id si5351_i2c_ids[] = {
+	{ "silabs,si5351", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, si5351_i2c_ids);
+
+static struct i2c_driver si5351_driver = {
+	.driver = {
+		.name = "si5351",
+		.of_match_table = of_match_ptr(si5351_dt_ids),
+	},
+	.probe = si5351_i2c_probe,
+	.id_table = si5351_i2c_ids,
+};
+module_i2c_driver(si5351_driver);
+
+MODULE_AUTHOR("Sebastian Hesselbarth <sebastian.hesselbarth@gmail.com");
+MODULE_DESCRIPTION("Silicon Labs Si5351A/B/C clock generator driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/clk/clk-si5351.h b/drivers/clk/clk-si5351.h
new file mode 100644
index 00000000000..af41b5080f4
--- /dev/null
+++ b/drivers/clk/clk-si5351.h
@@ -0,0 +1,155 @@
+/*
+ * clk-si5351.h: Silicon Laboratories Si5351A/B/C I2C Clock Generator
+ *
+ * Sebastian Hesselbarth <sebastian.hesselbarth@gmail.com>
+ * Rabeeh Khoury <rabeeh@solid-run.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#ifndef _CLK_SI5351_H_
+#define _CLK_SI5351_H_
+
+#define SI5351_BUS_BASE_ADDR			0x60
+
+#define SI5351_PLL_VCO_MIN			600000000
+#define SI5351_PLL_VCO_MAX			900000000
+#define SI5351_MULTISYNTH_MIN_FREQ		1000000
+#define SI5351_MULTISYNTH_DIVBY4_FREQ		150000000
+#define SI5351_MULTISYNTH_MAX_FREQ		160000000
+#define SI5351_MULTISYNTH67_MAX_FREQ		SI5351_MULTISYNTH_DIVBY4_FREQ
+#define SI5351_CLKOUT_MIN_FREQ			8000
+#define SI5351_CLKOUT_MAX_FREQ			SI5351_MULTISYNTH_MAX_FREQ
+#define SI5351_CLKOUT67_MAX_FREQ		SI5351_MULTISYNTH67_MAX_FREQ
+
+#define SI5351_PLL_A_MIN			15
+#define SI5351_PLL_A_MAX			90
+#define SI5351_PLL_B_MAX			(SI5351_PLL_C_MAX-1)
+#define SI5351_PLL_C_MAX			1048575
+#define SI5351_MULTISYNTH_A_MIN			6
+#define SI5351_MULTISYNTH_A_MAX			1800
+#define SI5351_MULTISYNTH67_A_MAX		254
+#define SI5351_MULTISYNTH_B_MAX			(SI5351_MULTISYNTH_C_MAX-1)
+#define SI5351_MULTISYNTH_C_MAX			1048575
+#define SI5351_MULTISYNTH_P1_MAX		((1<<18)-1)
+#define SI5351_MULTISYNTH_P2_MAX		((1<<20)-1)
+#define SI5351_MULTISYNTH_P3_MAX		((1<<20)-1)
+
+#define SI5351_DEVICE_STATUS			0
+#define SI5351_INTERRUPT_STATUS			1
+#define SI5351_INTERRUPT_MASK			2
+#define  SI5351_STATUS_SYS_INIT			(1<<7)
+#define  SI5351_STATUS_LOL_B			(1<<6)
+#define  SI5351_STATUS_LOL_A			(1<<5)
+#define  SI5351_STATUS_LOS			(1<<4)
+#define SI5351_OUTPUT_ENABLE_CTRL		3
+#define SI5351_OEB_PIN_ENABLE_CTRL		9
+#define SI5351_PLL_INPUT_SOURCE			15
+#define  SI5351_CLKIN_DIV_MASK			(3<<6)
+#define  SI5351_CLKIN_DIV_1			(0<<6)
+#define  SI5351_CLKIN_DIV_2			(1<<6)
+#define  SI5351_CLKIN_DIV_4			(2<<6)
+#define  SI5351_CLKIN_DIV_8			(3<<6)
+#define  SI5351_PLLB_SOURCE			(1<<3)
+#define  SI5351_PLLA_SOURCE			(1<<2)
+
+#define SI5351_CLK0_CTRL			16
+#define SI5351_CLK1_CTRL			17
+#define SI5351_CLK2_CTRL			18
+#define SI5351_CLK3_CTRL			19
+#define SI5351_CLK4_CTRL			20
+#define SI5351_CLK5_CTRL			21
+#define SI5351_CLK6_CTRL			22
+#define SI5351_CLK7_CTRL			23
+#define  SI5351_CLK_POWERDOWN			(1<<7)
+#define  SI5351_CLK_INTEGER_MODE		(1<<6)
+#define  SI5351_CLK_PLL_SELECT			(1<<5)
+#define  SI5351_CLK_INVERT			(1<<4)
+#define  SI5351_CLK_INPUT_MASK			(3<<2)
+#define  SI5351_CLK_INPUT_XTAL			(0<<2)
+#define  SI5351_CLK_INPUT_CLKIN			(1<<2)
+#define  SI5351_CLK_INPUT_MULTISYNTH_0_4	(2<<2)
+#define  SI5351_CLK_INPUT_MULTISYNTH_N		(3<<2)
+#define  SI5351_CLK_DRIVE_STRENGTH_MASK		(3<<0)
+#define  SI5351_CLK_DRIVE_STRENGTH_2MA		(0<<0)
+#define  SI5351_CLK_DRIVE_STRENGTH_4MA		(1<<0)
+#define  SI5351_CLK_DRIVE_STRENGTH_6MA		(2<<0)
+#define  SI5351_CLK_DRIVE_STRENGTH_8MA		(3<<0)
+
+#define SI5351_CLK3_0_DISABLE_STATE		24
+#define SI5351_CLK7_4_DISABLE_STATE		25
+#define  SI5351_CLK_DISABLE_STATE_LOW		0
+#define  SI5351_CLK_DISABLE_STATE_HIGH		1
+#define  SI5351_CLK_DISABLE_STATE_FLOAT		2
+#define  SI5351_CLK_DISABLE_STATE_NEVER		3
+
+#define SI5351_PARAMETERS_LENGTH		8
+#define SI5351_PLLA_PARAMETERS			26
+#define SI5351_PLLB_PARAMETERS			34
+#define SI5351_CLK0_PARAMETERS			42
+#define SI5351_CLK1_PARAMETERS			50
+#define SI5351_CLK2_PARAMETERS			58
+#define SI5351_CLK3_PARAMETERS			66
+#define SI5351_CLK4_PARAMETERS			74
+#define SI5351_CLK5_PARAMETERS			82
+#define SI5351_CLK6_PARAMETERS			90
+#define SI5351_CLK7_PARAMETERS			91
+#define SI5351_CLK6_7_OUTPUT_DIVIDER		92
+#define  SI5351_OUTPUT_CLK_DIV_MASK		(7 << 4)
+#define  SI5351_OUTPUT_CLK6_DIV_MASK		(7 << 0)
+#define  SI5351_OUTPUT_CLK_DIV_SHIFT		4
+#define  SI5351_OUTPUT_CLK_DIV6_SHIFT		0
+#define  SI5351_OUTPUT_CLK_DIV_1		0
+#define  SI5351_OUTPUT_CLK_DIV_2		1
+#define  SI5351_OUTPUT_CLK_DIV_4		2
+#define  SI5351_OUTPUT_CLK_DIV_8		3
+#define  SI5351_OUTPUT_CLK_DIV_16		4
+#define  SI5351_OUTPUT_CLK_DIV_32		5
+#define  SI5351_OUTPUT_CLK_DIV_64		6
+#define  SI5351_OUTPUT_CLK_DIV_128		7
+#define  SI5351_OUTPUT_CLK_DIVBY4		(3<<2)
+
+#define SI5351_SSC_PARAM0			149
+#define SI5351_SSC_PARAM1			150
+#define SI5351_SSC_PARAM2			151
+#define SI5351_SSC_PARAM3			152
+#define SI5351_SSC_PARAM4			153
+#define SI5351_SSC_PARAM5			154
+#define SI5351_SSC_PARAM6			155
+#define SI5351_SSC_PARAM7			156
+#define SI5351_SSC_PARAM8			157
+#define SI5351_SSC_PARAM9			158
+#define SI5351_SSC_PARAM10			159
+#define SI5351_SSC_PARAM11			160
+#define SI5351_SSC_PARAM12			161
+
+#define SI5351_VXCO_PARAMETERS_LOW		162
+#define SI5351_VXCO_PARAMETERS_MID		163
+#define SI5351_VXCO_PARAMETERS_HIGH		164
+
+#define SI5351_CLK0_PHASE_OFFSET		165
+#define SI5351_CLK1_PHASE_OFFSET		166
+#define SI5351_CLK2_PHASE_OFFSET		167
+#define SI5351_CLK3_PHASE_OFFSET		168
+#define SI5351_CLK4_PHASE_OFFSET		169
+#define SI5351_CLK5_PHASE_OFFSET		170
+
+#define SI5351_PLL_RESET			177
+#define  SI5351_PLL_RESET_B			(1<<7)
+#define  SI5351_PLL_RESET_A			(1<<5)
+
+#define SI5351_CRYSTAL_LOAD			183
+#define  SI5351_CRYSTAL_LOAD_MASK		(3<<6)
+#define  SI5351_CRYSTAL_LOAD_6PF		(1<<6)
+#define  SI5351_CRYSTAL_LOAD_8PF		(2<<6)
+#define  SI5351_CRYSTAL_LOAD_10PF		(3<<6)
+
+#define SI5351_FANOUT_ENABLE			187
+#define  SI5351_CLKIN_ENABLE			(1<<7)
+#define  SI5351_XTAL_ENABLE			(1<<6)
+#define  SI5351_MULTISYNTH_ENABLE		(1<<4)
+
+#endif
diff --git a/drivers/clk/clk-vt8500.c b/drivers/clk/clk-vt8500.c
index 09c63315e57..debf688afa8 100644
--- a/drivers/clk/clk-vt8500.c
+++ b/drivers/clk/clk-vt8500.c
@@ -488,6 +488,7 @@ static int vtwm_pll_set_rate(struct clk_hw *hw, unsigned long rate,
 	case PLL_TYPE_WM8750:
 		wm8750_find_pll_bits(rate, parent_rate, &filter, &mul, &div1, &div2);
 		pll_val = WM8750_BITS_TO_VAL(filter, mul, div1, div2);
+		break;
 	default:
 		pr_err("%s: invalid pll type\n", __func__);
 		return 0;
@@ -523,6 +524,7 @@ static long vtwm_pll_round_rate(struct clk_hw *hw, unsigned long rate,
 	case PLL_TYPE_WM8750:
 		wm8750_find_pll_bits(rate, *prate, &filter, &mul, &div1, &div2);
 		round_rate = WM8750_BITS_TO_FREQ(*prate, mul, div1, div2);
+		break;
 	default:
 		round_rate = 0;
 	}
diff --git a/drivers/clk/clk-zynq.c b/drivers/clk/clk-zynq.c
index b14a25f3925..32062977f45 100644
--- a/drivers/clk/clk-zynq.c
+++ b/drivers/clk/clk-zynq.c
@@ -20,6 +20,7 @@
 #include <linux/slab.h>
 #include <linux/kernel.h>
 #include <linux/clk-provider.h>
+#include <linux/clk/zynq.h>
 
 static void __iomem *slcr_base;
 
diff --git a/drivers/clk/clk.c b/drivers/clk/clk.c
index ed87b240580..934cfd18f72 100644
--- a/drivers/clk/clk.c
+++ b/drivers/clk/clk.c
@@ -19,14 +19,77 @@
 #include <linux/of.h>
 #include <linux/device.h>
 #include <linux/init.h>
+#include <linux/sched.h>
 
 static DEFINE_SPINLOCK(enable_lock);
 static DEFINE_MUTEX(prepare_lock);
 
+static struct task_struct *prepare_owner;
+static struct task_struct *enable_owner;
+
+static int prepare_refcnt;
+static int enable_refcnt;
+
 static HLIST_HEAD(clk_root_list);
 static HLIST_HEAD(clk_orphan_list);
 static LIST_HEAD(clk_notifier_list);
 
+/***           locking             ***/
+static void clk_prepare_lock(void)
+{
+	if (!mutex_trylock(&prepare_lock)) {
+		if (prepare_owner == current) {
+			prepare_refcnt++;
+			return;
+		}
+		mutex_lock(&prepare_lock);
+	}
+	WARN_ON_ONCE(prepare_owner != NULL);
+	WARN_ON_ONCE(prepare_refcnt != 0);
+	prepare_owner = current;
+	prepare_refcnt = 1;
+}
+
+static void clk_prepare_unlock(void)
+{
+	WARN_ON_ONCE(prepare_owner != current);
+	WARN_ON_ONCE(prepare_refcnt == 0);
+
+	if (--prepare_refcnt)
+		return;
+	prepare_owner = NULL;
+	mutex_unlock(&prepare_lock);
+}
+
+static unsigned long clk_enable_lock(void)
+{
+	unsigned long flags;
+
+	if (!spin_trylock_irqsave(&enable_lock, flags)) {
+		if (enable_owner == current) {
+			enable_refcnt++;
+			return flags;
+		}
+		spin_lock_irqsave(&enable_lock, flags);
+	}
+	WARN_ON_ONCE(enable_owner != NULL);
+	WARN_ON_ONCE(enable_refcnt != 0);
+	enable_owner = current;
+	enable_refcnt = 1;
+	return flags;
+}
+
+static void clk_enable_unlock(unsigned long flags)
+{
+	WARN_ON_ONCE(enable_owner != current);
+	WARN_ON_ONCE(enable_refcnt == 0);
+
+	if (--enable_refcnt)
+		return;
+	enable_owner = NULL;
+	spin_unlock_irqrestore(&enable_lock, flags);
+}
+
 /***        debugfs support        ***/
 
 #ifdef CONFIG_COMMON_CLK_DEBUG
@@ -69,7 +132,7 @@ static int clk_summary_show(struct seq_file *s, void *data)
 	seq_printf(s, "   clock                        enable_cnt  prepare_cnt  rate\n");
 	seq_printf(s, "---------------------------------------------------------------------\n");
 
-	mutex_lock(&prepare_lock);
+	clk_prepare_lock();
 
 	hlist_for_each_entry(c, &clk_root_list, child_node)
 		clk_summary_show_subtree(s, c, 0);
@@ -77,7 +140,7 @@ static int clk_summary_show(struct seq_file *s, void *data)
 	hlist_for_each_entry(c, &clk_orphan_list, child_node)
 		clk_summary_show_subtree(s, c, 0);
 
-	mutex_unlock(&prepare_lock);
+	clk_prepare_unlock();
 
 	return 0;
 }
@@ -130,7 +193,7 @@ static int clk_dump(struct seq_file *s, void *data)
 
 	seq_printf(s, "{");
 
-	mutex_lock(&prepare_lock);
+	clk_prepare_lock();
 
 	hlist_for_each_entry(c, &clk_root_list, child_node) {
 		if (!first_node)
@@ -144,7 +207,7 @@ static int clk_dump(struct seq_file *s, void *data)
 		clk_dump_subtree(s, c, 0);
 	}
 
-	mutex_unlock(&prepare_lock);
+	clk_prepare_unlock();
 
 	seq_printf(s, "}");
 	return 0;
@@ -280,6 +343,39 @@ out:
 }
 
 /**
+ * clk_debug_reparent - reparent clk node in the debugfs clk tree
+ * @clk: the clk being reparented
+ * @new_parent: the new clk parent, may be NULL
+ *
+ * Rename clk entry in the debugfs clk tree if debugfs has been
+ * initialized.  Otherwise it bails out early since the debugfs clk tree
+ * will be created lazily by clk_debug_init as part of a late_initcall.
+ *
+ * Caller must hold prepare_lock.
+ */
+static void clk_debug_reparent(struct clk *clk, struct clk *new_parent)
+{
+	struct dentry *d;
+	struct dentry *new_parent_d;
+
+	if (!inited)
+		return;
+
+	if (new_parent)
+		new_parent_d = new_parent->dentry;
+	else
+		new_parent_d = orphandir;
+
+	d = debugfs_rename(clk->dentry->d_parent, clk->dentry,
+			new_parent_d, clk->name);
+	if (d)
+		clk->dentry = d;
+	else
+		pr_debug("%s: failed to rename debugfs entry for %s\n",
+				__func__, clk->name);
+}
+
+/**
  * clk_debug_init - lazily create the debugfs clk tree visualization
  *
  * clks are often initialized very early during boot before memory can
@@ -316,7 +412,7 @@ static int __init clk_debug_init(void)
 	if (!orphandir)
 		return -ENOMEM;
 
-	mutex_lock(&prepare_lock);
+	clk_prepare_lock();
 
 	hlist_for_each_entry(clk, &clk_root_list, child_node)
 		clk_debug_create_subtree(clk, rootdir);
@@ -326,16 +422,45 @@ static int __init clk_debug_init(void)
 
 	inited = 1;
 
-	mutex_unlock(&prepare_lock);
+	clk_prepare_unlock();
 
 	return 0;
 }
 late_initcall(clk_debug_init);
 #else
 static inline int clk_debug_register(struct clk *clk) { return 0; }
+static inline void clk_debug_reparent(struct clk *clk, struct clk *new_parent)
+{
+}
 #endif
 
 /* caller must hold prepare_lock */
+static void clk_unprepare_unused_subtree(struct clk *clk)
+{
+	struct clk *child;
+
+	if (!clk)
+		return;
+
+	hlist_for_each_entry(child, &clk->children, child_node)
+		clk_unprepare_unused_subtree(child);
+
+	if (clk->prepare_count)
+		return;
+
+	if (clk->flags & CLK_IGNORE_UNUSED)
+		return;
+
+	if (__clk_is_prepared(clk)) {
+		if (clk->ops->unprepare_unused)
+			clk->ops->unprepare_unused(clk->hw);
+		else if (clk->ops->unprepare)
+			clk->ops->unprepare(clk->hw);
+	}
+}
+EXPORT_SYMBOL_GPL(__clk_get_flags);
+
+/* caller must hold prepare_lock */
 static void clk_disable_unused_subtree(struct clk *clk)
 {
 	struct clk *child;
@@ -347,7 +472,7 @@ static void clk_disable_unused_subtree(struct clk *clk)
 	hlist_for_each_entry(child, &clk->children, child_node)
 		clk_disable_unused_subtree(child);
 
-	spin_lock_irqsave(&enable_lock, flags);
+	flags = clk_enable_lock();
 
 	if (clk->enable_count)
 		goto unlock_out;
@@ -368,17 +493,30 @@ static void clk_disable_unused_subtree(struct clk *clk)
 	}
 
 unlock_out:
-	spin_unlock_irqrestore(&enable_lock, flags);
+	clk_enable_unlock(flags);
 
 out:
 	return;
 }
 
+static bool clk_ignore_unused;
+static int __init clk_ignore_unused_setup(char *__unused)
+{
+	clk_ignore_unused = true;
+	return 1;
+}
+__setup("clk_ignore_unused", clk_ignore_unused_setup);
+
 static int clk_disable_unused(void)
 {
 	struct clk *clk;
 
-	mutex_lock(&prepare_lock);
+	if (clk_ignore_unused) {
+		pr_warn("clk: Not disabling unused clocks\n");
+		return 0;
+	}
+
+	clk_prepare_lock();
 
 	hlist_for_each_entry(clk, &clk_root_list, child_node)
 		clk_disable_unused_subtree(clk);
@@ -386,7 +524,13 @@ static int clk_disable_unused(void)
 	hlist_for_each_entry(clk, &clk_orphan_list, child_node)
 		clk_disable_unused_subtree(clk);
 
-	mutex_unlock(&prepare_lock);
+	hlist_for_each_entry(clk, &clk_root_list, child_node)
+		clk_unprepare_unused_subtree(clk);
+
+	hlist_for_each_entry(clk, &clk_orphan_list, child_node)
+		clk_unprepare_unused_subtree(clk);
+
+	clk_prepare_unlock();
 
 	return 0;
 }
@@ -451,6 +595,27 @@ unsigned long __clk_get_flags(struct clk *clk)
 	return !clk ? 0 : clk->flags;
 }
 
+bool __clk_is_prepared(struct clk *clk)
+{
+	int ret;
+
+	if (!clk)
+		return false;
+
+	/*
+	 * .is_prepared is optional for clocks that can prepare
+	 * fall back to software usage counter if it is missing
+	 */
+	if (!clk->ops->is_prepared) {
+		ret = clk->prepare_count ? 1 : 0;
+		goto out;
+	}
+
+	ret = clk->ops->is_prepared(clk->hw);
+out:
+	return !!ret;
+}
+
 bool __clk_is_enabled(struct clk *clk)
 {
 	int ret;
@@ -548,9 +713,9 @@ void __clk_unprepare(struct clk *clk)
  */
 void clk_unprepare(struct clk *clk)
 {
-	mutex_lock(&prepare_lock);
+	clk_prepare_lock();
 	__clk_unprepare(clk);
-	mutex_unlock(&prepare_lock);
+	clk_prepare_unlock();
 }
 EXPORT_SYMBOL_GPL(clk_unprepare);
 
@@ -596,9 +761,9 @@ int clk_prepare(struct clk *clk)
 {
 	int ret;
 
-	mutex_lock(&prepare_lock);
+	clk_prepare_lock();
 	ret = __clk_prepare(clk);
-	mutex_unlock(&prepare_lock);
+	clk_prepare_unlock();
 
 	return ret;
 }
@@ -640,9 +805,9 @@ void clk_disable(struct clk *clk)
 {
 	unsigned long flags;
 
-	spin_lock_irqsave(&enable_lock, flags);
+	flags = clk_enable_lock();
 	__clk_disable(clk);
-	spin_unlock_irqrestore(&enable_lock, flags);
+	clk_enable_unlock(flags);
 }
 EXPORT_SYMBOL_GPL(clk_disable);
 
@@ -693,9 +858,9 @@ int clk_enable(struct clk *clk)
 	unsigned long flags;
 	int ret;
 
-	spin_lock_irqsave(&enable_lock, flags);
+	flags = clk_enable_lock();
 	ret = __clk_enable(clk);
-	spin_unlock_irqrestore(&enable_lock, flags);
+	clk_enable_unlock(flags);
 
 	return ret;
 }
@@ -740,9 +905,9 @@ long clk_round_rate(struct clk *clk, unsigned long rate)
 {
 	unsigned long ret;
 
-	mutex_lock(&prepare_lock);
+	clk_prepare_lock();
 	ret = __clk_round_rate(clk, rate);
-	mutex_unlock(&prepare_lock);
+	clk_prepare_unlock();
 
 	return ret;
 }
@@ -837,13 +1002,13 @@ unsigned long clk_get_rate(struct clk *clk)
 {
 	unsigned long rate;
 
-	mutex_lock(&prepare_lock);
+	clk_prepare_lock();
 
 	if (clk && (clk->flags & CLK_GET_RATE_NOCACHE))
 		__clk_recalc_rates(clk, 0);
 
 	rate = __clk_get_rate(clk);
-	mutex_unlock(&prepare_lock);
+	clk_prepare_unlock();
 
 	return rate;
 }
@@ -876,16 +1041,16 @@ static int __clk_speculate_rates(struct clk *clk, unsigned long parent_rate)
 	else
 		new_rate = parent_rate;
 
-	/* abort the rate change if a driver returns NOTIFY_BAD */
+	/* abort rate change if a driver returns NOTIFY_BAD or NOTIFY_STOP */
 	if (clk->notifier_count)
 		ret = __clk_notify(clk, PRE_RATE_CHANGE, clk->rate, new_rate);
 
-	if (ret == NOTIFY_BAD)
+	if (ret & NOTIFY_STOP_MASK)
 		goto out;
 
 	hlist_for_each_entry(child, &clk->children, child_node) {
 		ret = __clk_speculate_rates(child, new_rate);
-		if (ret == NOTIFY_BAD)
+		if (ret & NOTIFY_STOP_MASK)
 			break;
 	}
 
@@ -974,11 +1139,11 @@ static struct clk *clk_propagate_rate_change(struct clk *clk, unsigned long even
 	int ret = NOTIFY_DONE;
 
 	if (clk->rate == clk->new_rate)
-		return 0;
+		return NULL;
 
 	if (clk->notifier_count) {
 		ret = __clk_notify(clk, event, clk->rate, clk->new_rate);
-		if (ret == NOTIFY_BAD)
+		if (ret & NOTIFY_STOP_MASK)
 			fail_clk = clk;
 	}
 
@@ -1048,7 +1213,7 @@ int clk_set_rate(struct clk *clk, unsigned long rate)
 	int ret = 0;
 
 	/* prevent racing with updates to the clock topology */
-	mutex_lock(&prepare_lock);
+	clk_prepare_lock();
 
 	/* bail early if nothing to do */
 	if (rate == clk->rate)
@@ -1080,7 +1245,7 @@ int clk_set_rate(struct clk *clk, unsigned long rate)
 	clk_change_rate(top);
 
 out:
-	mutex_unlock(&prepare_lock);
+	clk_prepare_unlock();
 
 	return ret;
 }
@@ -1096,9 +1261,9 @@ struct clk *clk_get_parent(struct clk *clk)
 {
 	struct clk *parent;
 
-	mutex_lock(&prepare_lock);
+	clk_prepare_lock();
 	parent = __clk_get_parent(clk);
-	mutex_unlock(&prepare_lock);
+	clk_prepare_unlock();
 
 	return parent;
 }
@@ -1162,16 +1327,8 @@ out:
 	return ret;
 }
 
-void __clk_reparent(struct clk *clk, struct clk *new_parent)
+static void clk_reparent(struct clk *clk, struct clk *new_parent)
 {
-#ifdef CONFIG_COMMON_CLK_DEBUG
-	struct dentry *d;
-	struct dentry *new_parent_d;
-#endif
-
-	if (!clk || !new_parent)
-		return;
-
 	hlist_del(&clk->child_node);
 
 	if (new_parent)
@@ -1179,39 +1336,20 @@ void __clk_reparent(struct clk *clk, struct clk *new_parent)
 	else
 		hlist_add_head(&clk->child_node, &clk_orphan_list);
 
-#ifdef CONFIG_COMMON_CLK_DEBUG
-	if (!inited)
-		goto out;
-
-	if (new_parent)
-		new_parent_d = new_parent->dentry;
-	else
-		new_parent_d = orphandir;
-
-	d = debugfs_rename(clk->dentry->d_parent, clk->dentry,
-			new_parent_d, clk->name);
-	if (d)
-		clk->dentry = d;
-	else
-		pr_debug("%s: failed to rename debugfs entry for %s\n",
-				__func__, clk->name);
-out:
-#endif
-
 	clk->parent = new_parent;
+}
 
+void __clk_reparent(struct clk *clk, struct clk *new_parent)
+{
+	clk_reparent(clk, new_parent);
+	clk_debug_reparent(clk, new_parent);
 	__clk_recalc_rates(clk, POST_RATE_CHANGE);
 }
 
-static int __clk_set_parent(struct clk *clk, struct clk *parent)
+static u8 clk_fetch_parent_index(struct clk *clk, struct clk *parent)
 {
-	struct clk *old_parent;
-	unsigned long flags;
-	int ret = -EINVAL;
 	u8 i;
 
-	old_parent = clk->parent;
-
 	if (!clk->parents)
 		clk->parents = kzalloc((sizeof(struct clk*) * clk->num_parents),
 								GFP_KERNEL);
@@ -1231,36 +1369,79 @@ static int __clk_set_parent(struct clk *clk, struct clk *parent)
 		}
 	}
 
-	if (i == clk->num_parents) {
-		pr_debug("%s: clock %s is not a possible parent of clock %s\n",
-				__func__, parent->name, clk->name);
-		goto out;
-	}
+	return i;
+}
 
-	/* migrate prepare and enable */
+static int __clk_set_parent(struct clk *clk, struct clk *parent, u8 p_index)
+{
+	unsigned long flags;
+	int ret = 0;
+	struct clk *old_parent = clk->parent;
+	bool migrated_enable = false;
+
+	/* migrate prepare */
 	if (clk->prepare_count)
 		__clk_prepare(parent);
 
-	/* FIXME replace with clk_is_enabled(clk) someday */
-	spin_lock_irqsave(&enable_lock, flags);
-	if (clk->enable_count)
+	flags = clk_enable_lock();
+
+	/* migrate enable */
+	if (clk->enable_count) {
 		__clk_enable(parent);
-	spin_unlock_irqrestore(&enable_lock, flags);
+		migrated_enable = true;
+	}
+
+	/* update the clk tree topology */
+	clk_reparent(clk, parent);
+
+	clk_enable_unlock(flags);
 
 	/* change clock input source */
-	ret = clk->ops->set_parent(clk->hw, i);
+	if (parent && clk->ops->set_parent)
+		ret = clk->ops->set_parent(clk->hw, p_index);
 
-	/* clean up old prepare and enable */
-	spin_lock_irqsave(&enable_lock, flags);
-	if (clk->enable_count)
+	if (ret) {
+		/*
+		 * The error handling is tricky due to that we need to release
+		 * the spinlock while issuing the .set_parent callback. This
+		 * means the new parent might have been enabled/disabled in
+		 * between, which must be considered when doing rollback.
+		 */
+		flags = clk_enable_lock();
+
+		clk_reparent(clk, old_parent);
+
+		if (migrated_enable && clk->enable_count) {
+			__clk_disable(parent);
+		} else if (migrated_enable && (clk->enable_count == 0)) {
+			__clk_disable(old_parent);
+		} else if (!migrated_enable && clk->enable_count) {
+			__clk_disable(parent);
+			__clk_enable(old_parent);
+		}
+
+		clk_enable_unlock(flags);
+
+		if (clk->prepare_count)
+			__clk_unprepare(parent);
+
+		return ret;
+	}
+
+	/* clean up enable for old parent if migration was done */
+	if (migrated_enable) {
+		flags = clk_enable_lock();
 		__clk_disable(old_parent);
-	spin_unlock_irqrestore(&enable_lock, flags);
+		clk_enable_unlock(flags);
+	}
 
+	/* clean up prepare for old parent if migration was done */
 	if (clk->prepare_count)
 		__clk_unprepare(old_parent);
 
-out:
-	return ret;
+	/* update debugfs with new clk tree topology */
+	clk_debug_reparent(clk, parent);
+	return 0;
 }
 
 /**
@@ -1278,44 +1459,59 @@ out:
 int clk_set_parent(struct clk *clk, struct clk *parent)
 {
 	int ret = 0;
+	u8 p_index = 0;
+	unsigned long p_rate = 0;
 
 	if (!clk || !clk->ops)
 		return -EINVAL;
 
-	if (!clk->ops->set_parent)
+	/* verify ops for for multi-parent clks */
+	if ((clk->num_parents > 1) && (!clk->ops->set_parent))
 		return -ENOSYS;
 
 	/* prevent racing with updates to the clock topology */
-	mutex_lock(&prepare_lock);
+	clk_prepare_lock();
 
 	if (clk->parent == parent)
 		goto out;
 
+	/* check that we are allowed to re-parent if the clock is in use */
+	if ((clk->flags & CLK_SET_PARENT_GATE) && clk->prepare_count) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	/* try finding the new parent index */
+	if (parent) {
+		p_index = clk_fetch_parent_index(clk, parent);
+		p_rate = parent->rate;
+		if (p_index == clk->num_parents) {
+			pr_debug("%s: clk %s can not be parent of clk %s\n",
+					__func__, parent->name, clk->name);
+			ret = -EINVAL;
+			goto out;
+		}
+	}
+
 	/* propagate PRE_RATE_CHANGE notifications */
 	if (clk->notifier_count)
-		ret = __clk_speculate_rates(clk, parent->rate);
+		ret = __clk_speculate_rates(clk, p_rate);
 
 	/* abort if a driver objects */
-	if (ret == NOTIFY_STOP)
+	if (ret & NOTIFY_STOP_MASK)
 		goto out;
 
-	/* only re-parent if the clock is not in use */
-	if ((clk->flags & CLK_SET_PARENT_GATE) && clk->prepare_count)
-		ret = -EBUSY;
-	else
-		ret = __clk_set_parent(clk, parent);
+	/* do the re-parent */
+	ret = __clk_set_parent(clk, parent, p_index);
 
-	/* propagate ABORT_RATE_CHANGE if .set_parent failed */
-	if (ret) {
+	/* propagate rate recalculation accordingly */
+	if (ret)
 		__clk_recalc_rates(clk, ABORT_RATE_CHANGE);
-		goto out;
-	}
-
-	/* propagate rate recalculation downstream */
-	__clk_reparent(clk, parent);
+	else
+		__clk_recalc_rates(clk, POST_RATE_CHANGE);
 
 out:
-	mutex_unlock(&prepare_lock);
+	clk_prepare_unlock();
 
 	return ret;
 }
@@ -1338,7 +1534,7 @@ int __clk_init(struct device *dev, struct clk *clk)
 	if (!clk)
 		return -EINVAL;
 
-	mutex_lock(&prepare_lock);
+	clk_prepare_lock();
 
 	/* check to see if a clock with this name is already registered */
 	if (__clk_lookup(clk->name)) {
@@ -1462,7 +1658,7 @@ int __clk_init(struct device *dev, struct clk *clk)
 	clk_debug_register(clk);
 
 out:
-	mutex_unlock(&prepare_lock);
+	clk_prepare_unlock();
 
 	return ret;
 }
@@ -1696,7 +1892,7 @@ int clk_notifier_register(struct clk *clk, struct notifier_block *nb)
 	if (!clk || !nb)
 		return -EINVAL;
 
-	mutex_lock(&prepare_lock);
+	clk_prepare_lock();
 
 	/* search the list of notifiers for this clk */
 	list_for_each_entry(cn, &clk_notifier_list, node)
@@ -1720,7 +1916,7 @@ int clk_notifier_register(struct clk *clk, struct notifier_block *nb)
 	clk->notifier_count++;
 
 out:
-	mutex_unlock(&prepare_lock);
+	clk_prepare_unlock();
 
 	return ret;
 }
@@ -1745,7 +1941,7 @@ int clk_notifier_unregister(struct clk *clk, struct notifier_block *nb)
 	if (!clk || !nb)
 		return -EINVAL;
 
-	mutex_lock(&prepare_lock);
+	clk_prepare_lock();
 
 	list_for_each_entry(cn, &clk_notifier_list, node)
 		if (cn->clk == clk)
@@ -1766,7 +1962,7 @@ int clk_notifier_unregister(struct clk *clk, struct notifier_block *nb)
 		ret = -ENOENT;
 	}
 
-	mutex_unlock(&prepare_lock);
+	clk_prepare_unlock();
 
 	return ret;
 }
diff --git a/drivers/clk/mvebu/clk-core.c b/drivers/clk/mvebu/clk-core.c
index 69056a7479e..0a53edbae8b 100644
--- a/drivers/clk/mvebu/clk-core.c
+++ b/drivers/clk/mvebu/clk-core.c
@@ -156,8 +156,8 @@ static u32 __init armada_370_get_cpu_freq(void __iomem *sar)
 
 	cpu_freq_select = ((readl(sar) >> SARL_A370_PCLK_FREQ_OPT) &
 			   SARL_A370_PCLK_FREQ_OPT_MASK);
-	if (cpu_freq_select > ARRAY_SIZE(armada_370_cpu_frequencies)) {
-		pr_err("CPU freq select unsuported %d\n", cpu_freq_select);
+	if (cpu_freq_select >= ARRAY_SIZE(armada_370_cpu_frequencies)) {
+		pr_err("CPU freq select unsupported %d\n", cpu_freq_select);
 		cpu_freq = 0;
 	} else
 		cpu_freq = armada_370_cpu_frequencies[cpu_freq_select];
@@ -278,8 +278,8 @@ static u32 __init armada_xp_get_cpu_freq(void __iomem *sar)
 	cpu_freq_select |= (((readl(sar+4) >> SARH_AXP_PCLK_FREQ_OPT) &
 			     SARH_AXP_PCLK_FREQ_OPT_MASK)
 			    << SARH_AXP_PCLK_FREQ_OPT_SHIFT);
-	if (cpu_freq_select > ARRAY_SIZE(armada_xp_cpu_frequencies)) {
-		pr_err("CPU freq select unsuported: %d\n", cpu_freq_select);
+	if (cpu_freq_select >= ARRAY_SIZE(armada_xp_cpu_frequencies)) {
+		pr_err("CPU freq select unsupported: %d\n", cpu_freq_select);
 		cpu_freq = 0;
 	} else
 		cpu_freq = armada_xp_cpu_frequencies[cpu_freq_select];
diff --git a/drivers/clk/mvebu/clk-cpu.c b/drivers/clk/mvebu/clk-cpu.c
index 9dd2551a0a4..b0fbc071549 100644
--- a/drivers/clk/mvebu/clk-cpu.c
+++ b/drivers/clk/mvebu/clk-cpu.c
@@ -16,7 +16,6 @@
 #include <linux/io.h>
 #include <linux/of.h>
 #include <linux/delay.h>
-#include "clk-cpu.h"
 
 #define SYS_CTRL_CLK_DIVIDER_CTRL_OFFSET    0x0
 #define SYS_CTRL_CLK_DIVIDER_VALUE_OFFSET   0xC
@@ -173,17 +172,5 @@ clks_out:
 	kfree(cpuclk);
 }
 
-static const __initconst struct of_device_id clk_cpu_match[] = {
-	{
-		.compatible = "marvell,armada-xp-cpu-clock",
-		.data = of_cpu_clk_setup,
-	},
-	{
-		/* sentinel */
-	},
-};
-
-void __init mvebu_cpu_clk_init(void)
-{
-	of_clk_init(clk_cpu_match);
-}
+CLK_OF_DECLARE(armada_xp_cpu_clock, "marvell,armada-xp-cpu-clock",
+					 of_cpu_clk_setup);
diff --git a/drivers/clk/mvebu/clk-cpu.h b/drivers/clk/mvebu/clk-cpu.h
deleted file mode 100644
index 08e2affba4e..00000000000
--- a/drivers/clk/mvebu/clk-cpu.h
+++ /dev/null
@@ -1,22 +0,0 @@
-/*
- * Marvell MVEBU CPU clock handling.
- *
- * Copyright (C) 2012 Marvell
- *
- * Gregory CLEMENT <gregory.clement@free-electrons.com>
- *
- * This file is licensed under the terms of the GNU General Public
- * License version 2.  This program is licensed "as is" without any
- * warranty of any kind, whether express or implied.
- */
-
-#ifndef __MVEBU_CLK_CPU_H
-#define __MVEBU_CLK_CPU_H
-
-#ifdef CONFIG_MVEBU_CLK_CPU
-void __init mvebu_cpu_clk_init(void);
-#else
-static inline void mvebu_cpu_clk_init(void) {}
-#endif
-
-#endif
diff --git a/drivers/clk/mvebu/clk.c b/drivers/clk/mvebu/clk.c
index 855681b8a9d..29f10fb3006 100644
--- a/drivers/clk/mvebu/clk.c
+++ b/drivers/clk/mvebu/clk.c
@@ -10,18 +10,14 @@
  * warranty of any kind, whether express or implied.
  */
 #include <linux/kernel.h>
-#include <linux/clk.h>
 #include <linux/clk-provider.h>
-#include <linux/of_address.h>
-#include <linux/clk/mvebu.h>
 #include <linux/of.h>
 #include "clk-core.h"
-#include "clk-cpu.h"
 #include "clk-gating-ctrl.h"
 
 void __init mvebu_clocks_init(void)
 {
 	mvebu_core_clk_init();
 	mvebu_gating_clk_init();
-	mvebu_cpu_clk_init();
+	of_clk_init(NULL);
 }
diff --git a/drivers/clk/mxs/clk.c b/drivers/clk/mxs/clk.c
index b24d56067c8..5301bce8957 100644
--- a/drivers/clk/mxs/clk.c
+++ b/drivers/clk/mxs/clk.c
@@ -13,6 +13,7 @@
 #include <linux/io.h>
 #include <linux/jiffies.h>
 #include <linux/spinlock.h>
+#include "clk.h"
 
 DEFINE_SPINLOCK(mxs_lock);
 
diff --git a/drivers/clk/spear/spear1340_clock.c b/drivers/clk/spear/spear1340_clock.c
index 82abea366b7..35e7e2698e1 100644
--- a/drivers/clk/spear/spear1340_clock.c
+++ b/drivers/clk/spear/spear1340_clock.c
@@ -960,47 +960,47 @@ void __init spear1340_clk_init(void)
 			SPEAR1340_SPDIF_IN_CLK_ENB, 0, &_lock);
 	clk_register_clkdev(clk, NULL, "d0100000.spdif-in");
 
-	clk = clk_register_gate(NULL, "acp_clk", "acp_mclk", 0,
+	clk = clk_register_gate(NULL, "acp_clk", "ahb_clk", 0,
 			SPEAR1340_PERIP2_CLK_ENB, SPEAR1340_ACP_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "acp_clk");
 
-	clk = clk_register_gate(NULL, "plgpio_clk", "plgpio_mclk", 0,
+	clk = clk_register_gate(NULL, "plgpio_clk", "ahb_clk", 0,
 			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_PLGPIO_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "e2800000.gpio");
 
-	clk = clk_register_gate(NULL, "video_dec_clk", "video_dec_mclk", 0,
+	clk = clk_register_gate(NULL, "video_dec_clk", "ahb_clk", 0,
 			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_VIDEO_DEC_CLK_ENB,
 			0, &_lock);
 	clk_register_clkdev(clk, NULL, "video_dec");
 
-	clk = clk_register_gate(NULL, "video_enc_clk", "video_enc_mclk", 0,
+	clk = clk_register_gate(NULL, "video_enc_clk", "ahb_clk", 0,
 			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_VIDEO_ENC_CLK_ENB,
 			0, &_lock);
 	clk_register_clkdev(clk, NULL, "video_enc");
 
-	clk = clk_register_gate(NULL, "video_in_clk", "video_in_mclk", 0,
+	clk = clk_register_gate(NULL, "video_in_clk", "ahb_clk", 0,
 			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_VIDEO_IN_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "spear_vip");
 
-	clk = clk_register_gate(NULL, "cam0_clk", "cam0_mclk", 0,
+	clk = clk_register_gate(NULL, "cam0_clk", "ahb_clk", 0,
 			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_CAM0_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "d0200000.cam0");
 
-	clk = clk_register_gate(NULL, "cam1_clk", "cam1_mclk", 0,
+	clk = clk_register_gate(NULL, "cam1_clk", "ahb_clk", 0,
 			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_CAM1_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "d0300000.cam1");
 
-	clk = clk_register_gate(NULL, "cam2_clk", "cam2_mclk", 0,
+	clk = clk_register_gate(NULL, "cam2_clk", "ahb_clk", 0,
 			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_CAM2_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "d0400000.cam2");
 
-	clk = clk_register_gate(NULL, "cam3_clk", "cam3_mclk", 0,
+	clk = clk_register_gate(NULL, "cam3_clk", "ahb_clk", 0,
 			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_CAM3_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "d0500000.cam3");
diff --git a/drivers/clk/sunxi/Makefile b/drivers/clk/sunxi/Makefile
new file mode 100644
index 00000000000..b5bac917612
--- /dev/null
+++ b/drivers/clk/sunxi/Makefile
@@ -0,0 +1,5 @@
+#
+# Makefile for sunxi specific clk
+#
+
+obj-y += clk-sunxi.o clk-factors.o
diff --git a/drivers/clk/sunxi/clk-factors.c b/drivers/clk/sunxi/clk-factors.c
new file mode 100644
index 00000000000..88523f91d9b
--- /dev/null
+++ b/drivers/clk/sunxi/clk-factors.c
@@ -0,0 +1,180 @@
+/*
+ * Copyright (C) 2013 Emilio López <emilio@elopez.com.ar>
+ *
+ * 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.
+ *
+ * Adjustable factor-based clock implementation
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include <linux/string.h>
+
+#include <linux/delay.h>
+
+#include "clk-factors.h"
+
+/*
+ * DOC: basic adjustable factor-based clock that cannot gate
+ *
+ * Traits of this clock:
+ * prepare - clk_prepare only ensures that parents are prepared
+ * enable - clk_enable only ensures that parents are enabled
+ * rate - rate is adjustable.
+ *        clk->rate = (parent->rate * N * (K + 1) >> P) / (M + 1)
+ * parent - fixed parent.  No clk_set_parent support
+ */
+
+struct clk_factors {
+	struct clk_hw hw;
+	void __iomem *reg;
+	struct clk_factors_config *config;
+	void (*get_factors) (u32 *rate, u32 parent, u8 *n, u8 *k, u8 *m, u8 *p);
+	spinlock_t *lock;
+};
+
+#define to_clk_factors(_hw) container_of(_hw, struct clk_factors, hw)
+
+#define SETMASK(len, pos)		(((-1U) >> (31-len))  << (pos))
+#define CLRMASK(len, pos)		(~(SETMASK(len, pos)))
+#define FACTOR_GET(bit, len, reg)	(((reg) & SETMASK(len, bit)) >> (bit))
+
+#define FACTOR_SET(bit, len, reg, val) \
+	(((reg) & CLRMASK(len, bit)) | (val << (bit)))
+
+static unsigned long clk_factors_recalc_rate(struct clk_hw *hw,
+					     unsigned long parent_rate)
+{
+	u8 n = 1, k = 0, p = 0, m = 0;
+	u32 reg;
+	unsigned long rate;
+	struct clk_factors *factors = to_clk_factors(hw);
+	struct clk_factors_config *config = factors->config;
+
+	/* Fetch the register value */
+	reg = readl(factors->reg);
+
+	/* Get each individual factor if applicable */
+	if (config->nwidth != SUNXI_FACTORS_NOT_APPLICABLE)
+		n = FACTOR_GET(config->nshift, config->nwidth, reg);
+	if (config->kwidth != SUNXI_FACTORS_NOT_APPLICABLE)
+		k = FACTOR_GET(config->kshift, config->kwidth, reg);
+	if (config->mwidth != SUNXI_FACTORS_NOT_APPLICABLE)
+		m = FACTOR_GET(config->mshift, config->mwidth, reg);
+	if (config->pwidth != SUNXI_FACTORS_NOT_APPLICABLE)
+		p = FACTOR_GET(config->pshift, config->pwidth, reg);
+
+	/* Calculate the rate */
+	rate = (parent_rate * n * (k + 1) >> p) / (m + 1);
+
+	return rate;
+}
+
+static long clk_factors_round_rate(struct clk_hw *hw, unsigned long rate,
+				   unsigned long *parent_rate)
+{
+	struct clk_factors *factors = to_clk_factors(hw);
+	factors->get_factors((u32 *)&rate, (u32)*parent_rate,
+			     NULL, NULL, NULL, NULL);
+
+	return rate;
+}
+
+static int clk_factors_set_rate(struct clk_hw *hw, unsigned long rate,
+				unsigned long parent_rate)
+{
+	u8 n, k, m, p;
+	u32 reg;
+	struct clk_factors *factors = to_clk_factors(hw);
+	struct clk_factors_config *config = factors->config;
+	unsigned long flags = 0;
+
+	factors->get_factors((u32 *)&rate, (u32)parent_rate, &n, &k, &m, &p);
+
+	if (factors->lock)
+		spin_lock_irqsave(factors->lock, flags);
+
+	/* Fetch the register value */
+	reg = readl(factors->reg);
+
+	/* Set up the new factors - macros do not do anything if width is 0 */
+	reg = FACTOR_SET(config->nshift, config->nwidth, reg, n);
+	reg = FACTOR_SET(config->kshift, config->kwidth, reg, k);
+	reg = FACTOR_SET(config->mshift, config->mwidth, reg, m);
+	reg = FACTOR_SET(config->pshift, config->pwidth, reg, p);
+
+	/* Apply them now */
+	writel(reg, factors->reg);
+
+	/* delay 500us so pll stabilizes */
+	__delay((rate >> 20) * 500 / 2);
+
+	if (factors->lock)
+		spin_unlock_irqrestore(factors->lock, flags);
+
+	return 0;
+}
+
+static const struct clk_ops clk_factors_ops = {
+	.recalc_rate = clk_factors_recalc_rate,
+	.round_rate = clk_factors_round_rate,
+	.set_rate = clk_factors_set_rate,
+};
+
+/**
+ * clk_register_factors - register a factors clock with
+ * the clock framework
+ * @dev: device registering this clock
+ * @name: name of this clock
+ * @parent_name: name of clock's parent
+ * @flags: framework-specific flags
+ * @reg: register address to adjust factors
+ * @config: shift and width of factors n, k, m and p
+ * @get_factors: function to calculate the factors for a given frequency
+ * @lock: shared register lock for this clock
+ */
+struct clk *clk_register_factors(struct device *dev, const char *name,
+				 const char *parent_name,
+				 unsigned long flags, void __iomem *reg,
+				 struct clk_factors_config *config,
+				 void (*get_factors)(u32 *rate, u32 parent,
+						     u8 *n, u8 *k, u8 *m, u8 *p),
+				 spinlock_t *lock)
+{
+	struct clk_factors *factors;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	/* allocate the factors */
+	factors = kzalloc(sizeof(struct clk_factors), GFP_KERNEL);
+	if (!factors) {
+		pr_err("%s: could not allocate factors clk\n", __func__);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	init.name = name;
+	init.ops = &clk_factors_ops;
+	init.flags = flags;
+	init.parent_names = (parent_name ? &parent_name : NULL);
+	init.num_parents = (parent_name ? 1 : 0);
+
+	/* struct clk_factors assignments */
+	factors->reg = reg;
+	factors->config = config;
+	factors->lock = lock;
+	factors->hw.init = &init;
+	factors->get_factors = get_factors;
+
+	/* register the clock */
+	clk = clk_register(dev, &factors->hw);
+
+	if (IS_ERR(clk))
+		kfree(factors);
+
+	return clk;
+}
diff --git a/drivers/clk/sunxi/clk-factors.h b/drivers/clk/sunxi/clk-factors.h
new file mode 100644
index 00000000000..f49851cc438
--- /dev/null
+++ b/drivers/clk/sunxi/clk-factors.h
@@ -0,0 +1,27 @@
+#ifndef __MACH_SUNXI_CLK_FACTORS_H
+#define __MACH_SUNXI_CLK_FACTORS_H
+
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
+
+#define SUNXI_FACTORS_NOT_APPLICABLE	(0)
+
+struct clk_factors_config {
+	u8 nshift;
+	u8 nwidth;
+	u8 kshift;
+	u8 kwidth;
+	u8 mshift;
+	u8 mwidth;
+	u8 pshift;
+	u8 pwidth;
+};
+
+struct clk *clk_register_factors(struct device *dev, const char *name,
+				 const char *parent_name,
+				 unsigned long flags, void __iomem *reg,
+				 struct clk_factors_config *config,
+				 void (*get_factors) (u32 *rate, u32 parent_rate,
+						      u8 *n, u8 *k, u8 *m, u8 *p),
+				 spinlock_t *lock);
+#endif
diff --git a/drivers/clk/sunxi/clk-sunxi.c b/drivers/clk/sunxi/clk-sunxi.c
new file mode 100644
index 00000000000..8492ad1d536
--- /dev/null
+++ b/drivers/clk/sunxi/clk-sunxi.c
@@ -0,0 +1,469 @@
+/*
+ * Copyright 2013 Emilio López
+ *
+ * Emilio López <emilio@elopez.com.ar>
+ *
+ * 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.
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
+#include <linux/clk/sunxi.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+
+#include "clk-factors.h"
+
+static DEFINE_SPINLOCK(clk_lock);
+
+/**
+ * sunxi_osc_clk_setup() - Setup function for gatable oscillator
+ */
+
+#define SUNXI_OSC24M_GATE	0
+
+static void __init sunxi_osc_clk_setup(struct device_node *node)
+{
+	struct clk *clk;
+	struct clk_fixed_rate *fixed;
+	struct clk_gate *gate;
+	const char *clk_name = node->name;
+	u32 rate;
+
+	/* allocate fixed-rate and gate clock structs */
+	fixed = kzalloc(sizeof(struct clk_fixed_rate), GFP_KERNEL);
+	if (!fixed)
+		return;
+	gate = kzalloc(sizeof(struct clk_gate), GFP_KERNEL);
+	if (!gate) {
+		kfree(fixed);
+		return;
+	}
+
+	if (of_property_read_u32(node, "clock-frequency", &rate))
+		return;
+
+	/* set up gate and fixed rate properties */
+	gate->reg = of_iomap(node, 0);
+	gate->bit_idx = SUNXI_OSC24M_GATE;
+	gate->lock = &clk_lock;
+	fixed->fixed_rate = rate;
+
+	clk = clk_register_composite(NULL, clk_name,
+			NULL, 0,
+			NULL, NULL,
+			&fixed->hw, &clk_fixed_rate_ops,
+			&gate->hw, &clk_gate_ops,
+			CLK_IS_ROOT);
+
+	if (clk) {
+		of_clk_add_provider(node, of_clk_src_simple_get, clk);
+		clk_register_clkdev(clk, clk_name, NULL);
+	}
+}
+
+
+
+/**
+ * sunxi_get_pll1_factors() - calculates n, k, m, p factors for PLL1
+ * PLL1 rate is calculated as follows
+ * rate = (parent_rate * n * (k + 1) >> p) / (m + 1);
+ * parent_rate is always 24Mhz
+ */
+
+static void sunxi_get_pll1_factors(u32 *freq, u32 parent_rate,
+				   u8 *n, u8 *k, u8 *m, u8 *p)
+{
+	u8 div;
+
+	/* Normalize value to a 6M multiple */
+	div = *freq / 6000000;
+	*freq = 6000000 * div;
+
+	/* we were called to round the frequency, we can now return */
+	if (n == NULL)
+		return;
+
+	/* m is always zero for pll1 */
+	*m = 0;
+
+	/* k is 1 only on these cases */
+	if (*freq >= 768000000 || *freq == 42000000 || *freq == 54000000)
+		*k = 1;
+	else
+		*k = 0;
+
+	/* p will be 3 for divs under 10 */
+	if (div < 10)
+		*p = 3;
+
+	/* p will be 2 for divs between 10 - 20 and odd divs under 32 */
+	else if (div < 20 || (div < 32 && (div & 1)))
+		*p = 2;
+
+	/* p will be 1 for even divs under 32, divs under 40 and odd pairs
+	 * of divs between 40-62 */
+	else if (div < 40 || (div < 64 && (div & 2)))
+		*p = 1;
+
+	/* any other entries have p = 0 */
+	else
+		*p = 0;
+
+	/* calculate a suitable n based on k and p */
+	div <<= *p;
+	div /= (*k + 1);
+	*n = div / 4;
+}
+
+
+
+/**
+ * sunxi_get_apb1_factors() - calculates m, p factors for APB1
+ * APB1 rate is calculated as follows
+ * rate = (parent_rate >> p) / (m + 1);
+ */
+
+static void sunxi_get_apb1_factors(u32 *freq, u32 parent_rate,
+				   u8 *n, u8 *k, u8 *m, u8 *p)
+{
+	u8 calcm, calcp;
+
+	if (parent_rate < *freq)
+		*freq = parent_rate;
+
+	parent_rate = (parent_rate + (*freq - 1)) / *freq;
+
+	/* Invalid rate! */
+	if (parent_rate > 32)
+		return;
+
+	if (parent_rate <= 4)
+		calcp = 0;
+	else if (parent_rate <= 8)
+		calcp = 1;
+	else if (parent_rate <= 16)
+		calcp = 2;
+	else
+		calcp = 3;
+
+	calcm = (parent_rate >> calcp) - 1;
+
+	*freq = (parent_rate >> calcp) / (calcm + 1);
+
+	/* we were called to round the frequency, we can now return */
+	if (n == NULL)
+		return;
+
+	*m = calcm;
+	*p = calcp;
+}
+
+
+
+/**
+ * sunxi_factors_clk_setup() - Setup function for factor clocks
+ */
+
+struct factors_data {
+	struct clk_factors_config *table;
+	void (*getter) (u32 *rate, u32 parent_rate, u8 *n, u8 *k, u8 *m, u8 *p);
+};
+
+static struct clk_factors_config pll1_config = {
+	.nshift = 8,
+	.nwidth = 5,
+	.kshift = 4,
+	.kwidth = 2,
+	.mshift = 0,
+	.mwidth = 2,
+	.pshift = 16,
+	.pwidth = 2,
+};
+
+static struct clk_factors_config apb1_config = {
+	.mshift = 0,
+	.mwidth = 5,
+	.pshift = 16,
+	.pwidth = 2,
+};
+
+static const __initconst struct factors_data pll1_data = {
+	.table = &pll1_config,
+	.getter = sunxi_get_pll1_factors,
+};
+
+static const __initconst struct factors_data apb1_data = {
+	.table = &apb1_config,
+	.getter = sunxi_get_apb1_factors,
+};
+
+static void __init sunxi_factors_clk_setup(struct device_node *node,
+					   struct factors_data *data)
+{
+	struct clk *clk;
+	const char *clk_name = node->name;
+	const char *parent;
+	void *reg;
+
+	reg = of_iomap(node, 0);
+
+	parent = of_clk_get_parent_name(node, 0);
+
+	clk = clk_register_factors(NULL, clk_name, parent, 0, reg,
+				   data->table, data->getter, &clk_lock);
+
+	if (clk) {
+		of_clk_add_provider(node, of_clk_src_simple_get, clk);
+		clk_register_clkdev(clk, clk_name, NULL);
+	}
+}
+
+
+
+/**
+ * sunxi_mux_clk_setup() - Setup function for muxes
+ */
+
+#define SUNXI_MUX_GATE_WIDTH	2
+
+struct mux_data {
+	u8 shift;
+};
+
+static const __initconst struct mux_data cpu_data = {
+	.shift = 16,
+};
+
+static const __initconst struct mux_data apb1_mux_data = {
+	.shift = 24,
+};
+
+static void __init sunxi_mux_clk_setup(struct device_node *node,
+				       struct mux_data *data)
+{
+	struct clk *clk;
+	const char *clk_name = node->name;
+	const char *parents[5];
+	void *reg;
+	int i = 0;
+
+	reg = of_iomap(node, 0);
+
+	while (i < 5 && (parents[i] = of_clk_get_parent_name(node, i)) != NULL)
+		i++;
+
+	clk = clk_register_mux(NULL, clk_name, parents, i, 0, reg,
+			       data->shift, SUNXI_MUX_GATE_WIDTH,
+			       0, &clk_lock);
+
+	if (clk) {
+		of_clk_add_provider(node, of_clk_src_simple_get, clk);
+		clk_register_clkdev(clk, clk_name, NULL);
+	}
+}
+
+
+
+/**
+ * sunxi_divider_clk_setup() - Setup function for simple divider clocks
+ */
+
+#define SUNXI_DIVISOR_WIDTH	2
+
+struct div_data {
+	u8 shift;
+	u8 pow;
+};
+
+static const __initconst struct div_data axi_data = {
+	.shift = 0,
+	.pow = 0,
+};
+
+static const __initconst struct div_data ahb_data = {
+	.shift = 4,
+	.pow = 1,
+};
+
+static const __initconst struct div_data apb0_data = {
+	.shift = 8,
+	.pow = 1,
+};
+
+static void __init sunxi_divider_clk_setup(struct device_node *node,
+					   struct div_data *data)
+{
+	struct clk *clk;
+	const char *clk_name = node->name;
+	const char *clk_parent;
+	void *reg;
+
+	reg = of_iomap(node, 0);
+
+	clk_parent = of_clk_get_parent_name(node, 0);
+
+	clk = clk_register_divider(NULL, clk_name, clk_parent, 0,
+				   reg, data->shift, SUNXI_DIVISOR_WIDTH,
+				   data->pow ? CLK_DIVIDER_POWER_OF_TWO : 0,
+				   &clk_lock);
+	if (clk) {
+		of_clk_add_provider(node, of_clk_src_simple_get, clk);
+		clk_register_clkdev(clk, clk_name, NULL);
+	}
+}
+
+
+
+/**
+ * sunxi_gates_clk_setup() - Setup function for leaf gates on clocks
+ */
+
+#define SUNXI_GATES_MAX_SIZE	64
+
+struct gates_data {
+	DECLARE_BITMAP(mask, SUNXI_GATES_MAX_SIZE);
+};
+
+static const __initconst struct gates_data axi_gates_data = {
+	.mask = {1},
+};
+
+static const __initconst struct gates_data ahb_gates_data = {
+	.mask = {0x7F77FFF, 0x14FB3F},
+};
+
+static const __initconst struct gates_data apb0_gates_data = {
+	.mask = {0x4EF},
+};
+
+static const __initconst struct gates_data apb1_gates_data = {
+	.mask = {0xFF00F7},
+};
+
+static void __init sunxi_gates_clk_setup(struct device_node *node,
+					 struct gates_data *data)
+{
+	struct clk_onecell_data *clk_data;
+	const char *clk_parent;
+	const char *clk_name;
+	void *reg;
+	int qty;
+	int i = 0;
+	int j = 0;
+	int ignore;
+
+	reg = of_iomap(node, 0);
+
+	clk_parent = of_clk_get_parent_name(node, 0);
+
+	/* Worst-case size approximation and memory allocation */
+	qty = find_last_bit(data->mask, SUNXI_GATES_MAX_SIZE);
+	clk_data = kmalloc(sizeof(struct clk_onecell_data), GFP_KERNEL);
+	if (!clk_data)
+		return;
+	clk_data->clks = kzalloc((qty+1) * sizeof(struct clk *), GFP_KERNEL);
+	if (!clk_data->clks) {
+		kfree(clk_data);
+		return;
+	}
+
+	for_each_set_bit(i, data->mask, SUNXI_GATES_MAX_SIZE) {
+		of_property_read_string_index(node, "clock-output-names",
+					      j, &clk_name);
+
+		/* No driver claims this clock, but it should remain gated */
+		ignore = !strcmp("ahb_sdram", clk_name) ? CLK_IGNORE_UNUSED : 0;
+
+		clk_data->clks[i] = clk_register_gate(NULL, clk_name,
+						      clk_parent, ignore,
+						      reg + 4 * (i/32), i % 32,
+						      0, &clk_lock);
+		WARN_ON(IS_ERR(clk_data->clks[i]));
+
+		j++;
+	}
+
+	/* Adjust to the real max */
+	clk_data->clk_num = i;
+
+	of_clk_add_provider(node, of_clk_src_onecell_get, clk_data);
+}
+
+/* Matches for of_clk_init */
+static const __initconst struct of_device_id clk_match[] = {
+	{.compatible = "allwinner,sun4i-osc-clk", .data = sunxi_osc_clk_setup,},
+	{}
+};
+
+/* Matches for factors clocks */
+static const __initconst struct of_device_id clk_factors_match[] = {
+	{.compatible = "allwinner,sun4i-pll1-clk", .data = &pll1_data,},
+	{.compatible = "allwinner,sun4i-apb1-clk", .data = &apb1_data,},
+	{}
+};
+
+/* Matches for divider clocks */
+static const __initconst struct of_device_id clk_div_match[] = {
+	{.compatible = "allwinner,sun4i-axi-clk", .data = &axi_data,},
+	{.compatible = "allwinner,sun4i-ahb-clk", .data = &ahb_data,},
+	{.compatible = "allwinner,sun4i-apb0-clk", .data = &apb0_data,},
+	{}
+};
+
+/* Matches for mux clocks */
+static const __initconst struct of_device_id clk_mux_match[] = {
+	{.compatible = "allwinner,sun4i-cpu-clk", .data = &cpu_data,},
+	{.compatible = "allwinner,sun4i-apb1-mux-clk", .data = &apb1_mux_data,},
+	{}
+};
+
+/* Matches for gate clocks */
+static const __initconst struct of_device_id clk_gates_match[] = {
+	{.compatible = "allwinner,sun4i-axi-gates-clk", .data = &axi_gates_data,},
+	{.compatible = "allwinner,sun4i-ahb-gates-clk", .data = &ahb_gates_data,},
+	{.compatible = "allwinner,sun4i-apb0-gates-clk", .data = &apb0_gates_data,},
+	{.compatible = "allwinner,sun4i-apb1-gates-clk", .data = &apb1_gates_data,},
+	{}
+};
+
+static void __init of_sunxi_table_clock_setup(const struct of_device_id *clk_match,
+					      void *function)
+{
+	struct device_node *np;
+	const struct div_data *data;
+	const struct of_device_id *match;
+	void (*setup_function)(struct device_node *, const void *) = function;
+
+	for_each_matching_node(np, clk_match) {
+		match = of_match_node(clk_match, np);
+		data = match->data;
+		setup_function(np, data);
+	}
+}
+
+void __init sunxi_init_clocks(void)
+{
+	/* Register all the simple sunxi clocks on DT */
+	of_clk_init(clk_match);
+
+	/* Register factor clocks */
+	of_sunxi_table_clock_setup(clk_factors_match, sunxi_factors_clk_setup);
+
+	/* Register divider clocks */
+	of_sunxi_table_clock_setup(clk_div_match, sunxi_divider_clk_setup);
+
+	/* Register mux clocks */
+	of_sunxi_table_clock_setup(clk_mux_match, sunxi_mux_clk_setup);
+
+	/* Register gate clocks */
+	of_sunxi_table_clock_setup(clk_gates_match, sunxi_gates_clk_setup);
+}
diff --git a/drivers/clk/tegra/clk-tegra20.c b/drivers/clk/tegra/clk-tegra20.c
index 1e2de730536..f873dcefe0d 100644
--- a/drivers/clk/tegra/clk-tegra20.c
+++ b/drivers/clk/tegra/clk-tegra20.c
@@ -703,7 +703,7 @@ static void tegra20_pll_init(void)
 	clks[pll_a_out0] = clk;
 
 	/* PLLE */
-	clk = tegra_clk_register_plle("pll_e", "pll_ref", clk_base, NULL,
+	clk = tegra_clk_register_plle("pll_e", "pll_ref", clk_base, pmc_base,
 			     0, 100000000, &pll_e_params,
 			     0, pll_e_freq_table, NULL);
 	clk_register_clkdev(clk, "pll_e", NULL);
diff --git a/drivers/clk/tegra/clk.h b/drivers/clk/tegra/clk.h
index 0744731c622..a09d7dcaf18 100644
--- a/drivers/clk/tegra/clk.h
+++ b/drivers/clk/tegra/clk.h
@@ -355,15 +355,16 @@ struct clk *tegra_clk_register_periph_nodiv(const char *name,
 		struct tegra_clk_periph *periph, void __iomem *clk_base,
 		u32 offset);
 
-#define TEGRA_CLK_PERIPH(_mux_shift, _mux_width, _mux_flags,		\
+#define TEGRA_CLK_PERIPH(_mux_shift, _mux_mask, _mux_flags,		\
 			 _div_shift, _div_width, _div_frac_width,	\
 			 _div_flags, _clk_num, _enb_refcnt, _regs,	\
-			 _gate_flags)					\
+			 _gate_flags, _table)				\
 	{								\
 		.mux = {						\
 			.flags = _mux_flags,				\
 			.shift = _mux_shift,				\
-			.width = _mux_width,				\
+			.mask = _mux_mask,				\
+			.table = _table,				\
 		},							\
 		.divider = {						\
 			.flags = _div_flags,				\
@@ -393,26 +394,36 @@ struct tegra_periph_init_data {
 	const char *dev_id;
 };
 
-#define TEGRA_INIT_DATA(_name, _con_id, _dev_id, _parent_names, _offset, \
-			_mux_shift, _mux_width, _mux_flags, _div_shift,	\
+#define TEGRA_INIT_DATA_TABLE(_name, _con_id, _dev_id, _parent_names, _offset,\
+			_mux_shift, _mux_mask, _mux_flags, _div_shift,	\
 			_div_width, _div_frac_width, _div_flags, _regs,	\
-			_clk_num, _enb_refcnt, _gate_flags, _clk_id)	\
+			_clk_num, _enb_refcnt, _gate_flags, _clk_id, _table) \
 	{								\
 		.name = _name,						\
 		.clk_id = _clk_id,					\
 		.parent_names = _parent_names,				\
 		.num_parents = ARRAY_SIZE(_parent_names),		\
-		.periph = TEGRA_CLK_PERIPH(_mux_shift, _mux_width,	\
+		.periph = TEGRA_CLK_PERIPH(_mux_shift, _mux_mask,	\
 					   _mux_flags, _div_shift,	\
 					   _div_width, _div_frac_width,	\
 					   _div_flags, _clk_num,	\
 					   _enb_refcnt, _regs,		\
-					   _gate_flags),		\
+					   _gate_flags, _table),	\
 		.offset = _offset,					\
 		.con_id = _con_id,					\
 		.dev_id = _dev_id,					\
 	}
 
+#define TEGRA_INIT_DATA(_name, _con_id, _dev_id, _parent_names, _offset,\
+			_mux_shift, _mux_width, _mux_flags, _div_shift,	\
+			_div_width, _div_frac_width, _div_flags, _regs,	\
+			_clk_num, _enb_refcnt, _gate_flags, _clk_id)	\
+	TEGRA_INIT_DATA_TABLE(_name, _con_id, _dev_id, _parent_names, _offset,\
+			_mux_shift, BIT(_mux_width) - 1, _mux_flags,	\
+			_div_shift, _div_width, _div_frac_width, _div_flags, \
+			_regs, _clk_num, _enb_refcnt, _gate_flags, _clk_id,\
+			NULL)
+
 /**
  * struct clk_super_mux - super clock
  *
diff --git a/drivers/clk/ux500/Makefile b/drivers/clk/ux500/Makefile
index bcc0c11a507..c6a806ed0e8 100644
--- a/drivers/clk/ux500/Makefile
+++ b/drivers/clk/ux500/Makefile
@@ -5,6 +5,7 @@
 # Clock types
 obj-y += clk-prcc.o
 obj-y += clk-prcmu.o
+obj-y += clk-sysctrl.o
 
 # Clock definitions
 obj-y += u8500_clk.o
diff --git a/drivers/clk/ux500/abx500-clk.c b/drivers/clk/ux500/abx500-clk.c
index 9f7400d74fa..a0fca004abc 100644
--- a/drivers/clk/ux500/abx500-clk.c
+++ b/drivers/clk/ux500/abx500-clk.c
@@ -12,13 +12,78 @@
 #include <linux/device.h>
 #include <linux/platform_device.h>
 #include <linux/mfd/abx500/ab8500.h>
-
-/* TODO: Add clock implementations here */
-
+#include <linux/mfd/abx500/ab8500-sysctrl.h>
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/mfd/dbx500-prcmu.h>
+#include "clk.h"
 
 /* Clock definitions for ab8500 */
 static int ab8500_reg_clks(struct device *dev)
 {
+	int ret;
+	struct clk *clk;
+
+	const char *intclk_parents[] = {"ab8500_sysclk", "ulpclk"};
+	u16 intclk_reg_sel[] = {0 , AB8500_SYSULPCLKCTRL1};
+	u8 intclk_reg_mask[] = {0 , AB8500_SYSULPCLKCTRL1_SYSULPCLKINTSEL_MASK};
+	u8 intclk_reg_bits[] = {
+		0 ,
+		(1 << AB8500_SYSULPCLKCTRL1_SYSULPCLKINTSEL_SHIFT)
+	};
+
+	dev_info(dev, "register clocks for ab850x\n");
+
+	/* Enable SWAT */
+	ret = ab8500_sysctrl_set(AB8500_SWATCTRL, AB8500_SWATCTRL_SWATENABLE);
+	if (ret)
+		return ret;
+
+	/* ab8500_sysclk */
+	clk = clk_reg_prcmu_gate("ab8500_sysclk", NULL, PRCMU_SYSCLK,
+				CLK_IS_ROOT);
+	clk_register_clkdev(clk, "sysclk", "ab8500-usb.0");
+	clk_register_clkdev(clk, "sysclk", "ab-iddet.0");
+	clk_register_clkdev(clk, "sysclk", "ab85xx-codec.0");
+	clk_register_clkdev(clk, "sysclk", "shrm_bus");
+
+	/* ab8500_sysclk2 */
+	clk = clk_reg_sysctrl_gate(dev , "ab8500_sysclk2", "ab8500_sysclk",
+		AB8500_SYSULPCLKCTRL1, AB8500_SYSULPCLKCTRL1_SYSCLKBUF2REQ,
+		AB8500_SYSULPCLKCTRL1_SYSCLKBUF2REQ, 0, 0);
+	clk_register_clkdev(clk, "sysclk", "0-0070");
+
+	/* ab8500_sysclk3 */
+	clk = clk_reg_sysctrl_gate(dev , "ab8500_sysclk3", "ab8500_sysclk",
+		AB8500_SYSULPCLKCTRL1, AB8500_SYSULPCLKCTRL1_SYSCLKBUF3REQ,
+		AB8500_SYSULPCLKCTRL1_SYSCLKBUF3REQ, 0, 0);
+	clk_register_clkdev(clk, "sysclk", "cg1960_core.0");
+
+	/* ab8500_sysclk4 */
+	clk = clk_reg_sysctrl_gate(dev , "ab8500_sysclk4", "ab8500_sysclk",
+		AB8500_SYSULPCLKCTRL1, AB8500_SYSULPCLKCTRL1_SYSCLKBUF4REQ,
+		AB8500_SYSULPCLKCTRL1_SYSCLKBUF4REQ, 0, 0);
+
+	/* ab_ulpclk */
+	clk = clk_reg_sysctrl_gate_fixed_rate(dev, "ulpclk", NULL,
+		AB8500_SYSULPCLKCTRL1, AB8500_SYSULPCLKCTRL1_ULPCLKREQ,
+		AB8500_SYSULPCLKCTRL1_ULPCLKREQ,
+		38400000, 9000, CLK_IS_ROOT);
+	clk_register_clkdev(clk, "ulpclk", "ab85xx-codec.0");
+
+	/* ab8500_intclk */
+	clk = clk_reg_sysctrl_set_parent(dev , "intclk", intclk_parents, 2,
+		intclk_reg_sel, intclk_reg_mask, intclk_reg_bits, 0);
+	clk_register_clkdev(clk, "intclk", "ab85xx-codec.0");
+	clk_register_clkdev(clk, NULL, "ab8500-pwm.1");
+
+	/* ab8500_audioclk */
+	clk = clk_reg_sysctrl_gate(dev , "audioclk", "intclk",
+		AB8500_SYSULPCLKCTRL1, AB8500_SYSULPCLKCTRL1_AUDIOCLKENA,
+		AB8500_SYSULPCLKCTRL1_AUDIOCLKENA, 0, 0);
+	clk_register_clkdev(clk, "audioclk", "ab85xx-codec.0");
+
 	return 0;
 }
 
diff --git a/drivers/clk/ux500/clk-prcmu.c b/drivers/clk/ux500/clk-prcmu.c
index 74faa7e3cf5..293a2885441 100644
--- a/drivers/clk/ux500/clk-prcmu.c
+++ b/drivers/clk/ux500/clk-prcmu.c
@@ -20,15 +20,23 @@
 struct clk_prcmu {
 	struct clk_hw hw;
 	u8 cg_sel;
+	int is_prepared;
 	int is_enabled;
+	int opp_requested;
 };
 
 /* PRCMU clock operations. */
 
 static int clk_prcmu_prepare(struct clk_hw *hw)
 {
+	int ret;
 	struct clk_prcmu *clk = to_clk_prcmu(hw);
-	return prcmu_request_clock(clk->cg_sel, true);
+
+	ret = prcmu_request_clock(clk->cg_sel, true);
+	if (!ret)
+		clk->is_prepared = 1;
+
+	return ret;;
 }
 
 static void clk_prcmu_unprepare(struct clk_hw *hw)
@@ -36,7 +44,15 @@ static void clk_prcmu_unprepare(struct clk_hw *hw)
 	struct clk_prcmu *clk = to_clk_prcmu(hw);
 	if (prcmu_request_clock(clk->cg_sel, false))
 		pr_err("clk_prcmu: %s failed to disable %s.\n", __func__,
-			hw->init->name);
+			__clk_get_name(hw->clk));
+	else
+		clk->is_prepared = 0;
+}
+
+static int clk_prcmu_is_prepared(struct clk_hw *hw)
+{
+	struct clk_prcmu *clk = to_clk_prcmu(hw);
+	return clk->is_prepared;
 }
 
 static int clk_prcmu_enable(struct clk_hw *hw)
@@ -79,58 +95,52 @@ static int clk_prcmu_set_rate(struct clk_hw *hw, unsigned long rate,
 	return prcmu_set_clock_rate(clk->cg_sel, rate);
 }
 
-static int request_ape_opp100(bool enable)
-{
-	static int reqs;
-	int err = 0;
-
-	if (enable) {
-		if (!reqs)
-			err = prcmu_qos_add_requirement(PRCMU_QOS_APE_OPP,
-							"clock", 100);
-		if (!err)
-			reqs++;
-	} else {
-		reqs--;
-		if (!reqs)
-			prcmu_qos_remove_requirement(PRCMU_QOS_APE_OPP,
-						"clock");
-	}
-	return err;
-}
-
 static int clk_prcmu_opp_prepare(struct clk_hw *hw)
 {
 	int err;
 	struct clk_prcmu *clk = to_clk_prcmu(hw);
 
-	err = request_ape_opp100(true);
-	if (err) {
-		pr_err("clk_prcmu: %s failed to request APE OPP100 for %s.\n",
-			__func__, hw->init->name);
-		return err;
+	if (!clk->opp_requested) {
+		err = prcmu_qos_add_requirement(PRCMU_QOS_APE_OPP,
+						(char *)__clk_get_name(hw->clk),
+						100);
+		if (err) {
+			pr_err("clk_prcmu: %s fail req APE OPP for %s.\n",
+				__func__, __clk_get_name(hw->clk));
+			return err;
+		}
+		clk->opp_requested = 1;
 	}
 
 	err = prcmu_request_clock(clk->cg_sel, true);
-	if (err)
-		request_ape_opp100(false);
+	if (err) {
+		prcmu_qos_remove_requirement(PRCMU_QOS_APE_OPP,
+					(char *)__clk_get_name(hw->clk));
+		clk->opp_requested = 0;
+		return err;
+	}
 
-	return err;
+	clk->is_prepared = 1;
+	return 0;
 }
 
 static void clk_prcmu_opp_unprepare(struct clk_hw *hw)
 {
 	struct clk_prcmu *clk = to_clk_prcmu(hw);
 
-	if (prcmu_request_clock(clk->cg_sel, false))
-		goto out_error;
-	if (request_ape_opp100(false))
-		goto out_error;
-	return;
-
-out_error:
-	pr_err("clk_prcmu: %s failed to disable %s.\n", __func__,
-		hw->init->name);
+	if (prcmu_request_clock(clk->cg_sel, false)) {
+		pr_err("clk_prcmu: %s failed to disable %s.\n", __func__,
+			__clk_get_name(hw->clk));
+		return;
+	}
+
+	if (clk->opp_requested) {
+		prcmu_qos_remove_requirement(PRCMU_QOS_APE_OPP,
+					(char *)__clk_get_name(hw->clk));
+		clk->opp_requested = 0;
+	}
+
+	clk->is_prepared = 0;
 }
 
 static int clk_prcmu_opp_volt_prepare(struct clk_hw *hw)
@@ -138,38 +148,49 @@ static int clk_prcmu_opp_volt_prepare(struct clk_hw *hw)
 	int err;
 	struct clk_prcmu *clk = to_clk_prcmu(hw);
 
-	err = prcmu_request_ape_opp_100_voltage(true);
-	if (err) {
-		pr_err("clk_prcmu: %s failed to request APE OPP VOLT for %s.\n",
-			__func__, hw->init->name);
-		return err;
+	if (!clk->opp_requested) {
+		err = prcmu_request_ape_opp_100_voltage(true);
+		if (err) {
+			pr_err("clk_prcmu: %s fail req APE OPP VOLT for %s.\n",
+				__func__, __clk_get_name(hw->clk));
+			return err;
+		}
+		clk->opp_requested = 1;
 	}
 
 	err = prcmu_request_clock(clk->cg_sel, true);
-	if (err)
+	if (err) {
 		prcmu_request_ape_opp_100_voltage(false);
+		clk->opp_requested = 0;
+		return err;
+	}
 
-	return err;
+	clk->is_prepared = 1;
+	return 0;
 }
 
 static void clk_prcmu_opp_volt_unprepare(struct clk_hw *hw)
 {
 	struct clk_prcmu *clk = to_clk_prcmu(hw);
 
-	if (prcmu_request_clock(clk->cg_sel, false))
-		goto out_error;
-	if (prcmu_request_ape_opp_100_voltage(false))
-		goto out_error;
-	return;
-
-out_error:
-	pr_err("clk_prcmu: %s failed to disable %s.\n", __func__,
-		hw->init->name);
+	if (prcmu_request_clock(clk->cg_sel, false)) {
+		pr_err("clk_prcmu: %s failed to disable %s.\n", __func__,
+			__clk_get_name(hw->clk));
+		return;
+	}
+
+	if (clk->opp_requested) {
+		prcmu_request_ape_opp_100_voltage(false);
+		clk->opp_requested = 0;
+	}
+
+	clk->is_prepared = 0;
 }
 
 static struct clk_ops clk_prcmu_scalable_ops = {
 	.prepare = clk_prcmu_prepare,
 	.unprepare = clk_prcmu_unprepare,
+	.is_prepared = clk_prcmu_is_prepared,
 	.enable = clk_prcmu_enable,
 	.disable = clk_prcmu_disable,
 	.is_enabled = clk_prcmu_is_enabled,
@@ -181,6 +202,7 @@ static struct clk_ops clk_prcmu_scalable_ops = {
 static struct clk_ops clk_prcmu_gate_ops = {
 	.prepare = clk_prcmu_prepare,
 	.unprepare = clk_prcmu_unprepare,
+	.is_prepared = clk_prcmu_is_prepared,
 	.enable = clk_prcmu_enable,
 	.disable = clk_prcmu_disable,
 	.is_enabled = clk_prcmu_is_enabled,
@@ -202,6 +224,7 @@ static struct clk_ops clk_prcmu_rate_ops = {
 static struct clk_ops clk_prcmu_opp_gate_ops = {
 	.prepare = clk_prcmu_opp_prepare,
 	.unprepare = clk_prcmu_opp_unprepare,
+	.is_prepared = clk_prcmu_is_prepared,
 	.enable = clk_prcmu_enable,
 	.disable = clk_prcmu_disable,
 	.is_enabled = clk_prcmu_is_enabled,
@@ -211,6 +234,7 @@ static struct clk_ops clk_prcmu_opp_gate_ops = {
 static struct clk_ops clk_prcmu_opp_volt_scalable_ops = {
 	.prepare = clk_prcmu_opp_volt_prepare,
 	.unprepare = clk_prcmu_opp_volt_unprepare,
+	.is_prepared = clk_prcmu_is_prepared,
 	.enable = clk_prcmu_enable,
 	.disable = clk_prcmu_disable,
 	.is_enabled = clk_prcmu_is_enabled,
@@ -242,7 +266,9 @@ static struct clk *clk_reg_prcmu(const char *name,
 	}
 
 	clk->cg_sel = cg_sel;
+	clk->is_prepared = 1;
 	clk->is_enabled = 1;
+	clk->opp_requested = 0;
 	/* "rate" can be used for changing the initial frequency */
 	if (rate)
 		prcmu_set_clock_rate(cg_sel, rate);
diff --git a/drivers/clk/ux500/clk-sysctrl.c b/drivers/clk/ux500/clk-sysctrl.c
new file mode 100644
index 00000000000..bc7e9bde792
--- /dev/null
+++ b/drivers/clk/ux500/clk-sysctrl.c
@@ -0,0 +1,221 @@
+/*
+ * Sysctrl clock implementation for ux500 platform.
+ *
+ * Copyright (C) 2013 ST-Ericsson SA
+ * Author: Ulf Hansson <ulf.hansson@linaro.org>
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/mfd/abx500/ab8500-sysctrl.h>
+#include <linux/device.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include "clk.h"
+
+#define SYSCTRL_MAX_NUM_PARENTS 4
+
+#define to_clk_sysctrl(_hw) container_of(_hw, struct clk_sysctrl, hw)
+
+struct clk_sysctrl {
+	struct clk_hw hw;
+	struct device *dev;
+	u8 parent_index;
+	u16 reg_sel[SYSCTRL_MAX_NUM_PARENTS];
+	u8 reg_mask[SYSCTRL_MAX_NUM_PARENTS];
+	u8 reg_bits[SYSCTRL_MAX_NUM_PARENTS];
+	unsigned long rate;
+	unsigned long enable_delay_us;
+};
+
+/* Sysctrl clock operations. */
+
+static int clk_sysctrl_prepare(struct clk_hw *hw)
+{
+	int ret;
+	struct clk_sysctrl *clk = to_clk_sysctrl(hw);
+
+	ret = ab8500_sysctrl_write(clk->reg_sel[0], clk->reg_mask[0],
+				clk->reg_bits[0]);
+
+	if (!ret && clk->enable_delay_us)
+		usleep_range(clk->enable_delay_us, clk->enable_delay_us);
+
+	return ret;
+}
+
+static void clk_sysctrl_unprepare(struct clk_hw *hw)
+{
+	struct clk_sysctrl *clk = to_clk_sysctrl(hw);
+	if (ab8500_sysctrl_clear(clk->reg_sel[0], clk->reg_mask[0]))
+		dev_err(clk->dev, "clk_sysctrl: %s fail to clear %s.\n",
+			__func__, __clk_get_name(hw->clk));
+}
+
+static unsigned long clk_sysctrl_recalc_rate(struct clk_hw *hw,
+					unsigned long parent_rate)
+{
+	struct clk_sysctrl *clk = to_clk_sysctrl(hw);
+	return clk->rate;
+}
+
+static int clk_sysctrl_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct clk_sysctrl *clk = to_clk_sysctrl(hw);
+	u8 old_index = clk->parent_index;
+	int ret = 0;
+
+	if (clk->reg_sel[old_index]) {
+		ret = ab8500_sysctrl_clear(clk->reg_sel[old_index],
+					clk->reg_mask[old_index]);
+		if (ret)
+			return ret;
+	}
+
+	if (clk->reg_sel[index]) {
+		ret = ab8500_sysctrl_write(clk->reg_sel[index],
+					clk->reg_mask[index],
+					clk->reg_bits[index]);
+		if (ret) {
+			if (clk->reg_sel[old_index])
+				ab8500_sysctrl_write(clk->reg_sel[old_index],
+						clk->reg_mask[old_index],
+						clk->reg_bits[old_index]);
+			return ret;
+		}
+	}
+	clk->parent_index = index;
+
+	return ret;
+}
+
+static u8 clk_sysctrl_get_parent(struct clk_hw *hw)
+{
+	struct clk_sysctrl *clk = to_clk_sysctrl(hw);
+	return clk->parent_index;
+}
+
+static struct clk_ops clk_sysctrl_gate_ops = {
+	.prepare = clk_sysctrl_prepare,
+	.unprepare = clk_sysctrl_unprepare,
+};
+
+static struct clk_ops clk_sysctrl_gate_fixed_rate_ops = {
+	.prepare = clk_sysctrl_prepare,
+	.unprepare = clk_sysctrl_unprepare,
+	.recalc_rate = clk_sysctrl_recalc_rate,
+};
+
+static struct clk_ops clk_sysctrl_set_parent_ops = {
+	.set_parent = clk_sysctrl_set_parent,
+	.get_parent = clk_sysctrl_get_parent,
+};
+
+static struct clk *clk_reg_sysctrl(struct device *dev,
+				const char *name,
+				const char **parent_names,
+				u8 num_parents,
+				u16 *reg_sel,
+				u8 *reg_mask,
+				u8 *reg_bits,
+				unsigned long rate,
+				unsigned long enable_delay_us,
+				unsigned long flags,
+				struct clk_ops *clk_sysctrl_ops)
+{
+	struct clk_sysctrl *clk;
+	struct clk_init_data clk_sysctrl_init;
+	struct clk *clk_reg;
+	int i;
+
+	if (!dev)
+		return ERR_PTR(-EINVAL);
+
+	if (!name || (num_parents > SYSCTRL_MAX_NUM_PARENTS)) {
+		dev_err(dev, "clk_sysctrl: invalid arguments passed\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	clk = devm_kzalloc(dev, sizeof(struct clk_sysctrl), GFP_KERNEL);
+	if (!clk) {
+		dev_err(dev, "clk_sysctrl: could not allocate clk\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	for (i = 0; i < num_parents; i++) {
+		clk->reg_sel[i] = reg_sel[i];
+		clk->reg_bits[i] = reg_bits[i];
+		clk->reg_mask[i] = reg_mask[i];
+	}
+
+	clk->parent_index = 0;
+	clk->rate = rate;
+	clk->enable_delay_us = enable_delay_us;
+	clk->dev = dev;
+
+	clk_sysctrl_init.name = name;
+	clk_sysctrl_init.ops = clk_sysctrl_ops;
+	clk_sysctrl_init.flags = flags;
+	clk_sysctrl_init.parent_names = parent_names;
+	clk_sysctrl_init.num_parents = num_parents;
+	clk->hw.init = &clk_sysctrl_init;
+
+	clk_reg = devm_clk_register(clk->dev, &clk->hw);
+	if (IS_ERR(clk_reg))
+		dev_err(dev, "clk_sysctrl: clk_register failed\n");
+
+	return clk_reg;
+}
+
+struct clk *clk_reg_sysctrl_gate(struct device *dev,
+				const char *name,
+				const char *parent_name,
+				u16 reg_sel,
+				u8 reg_mask,
+				u8 reg_bits,
+				unsigned long enable_delay_us,
+				unsigned long flags)
+{
+	const char **parent_names = (parent_name ? &parent_name : NULL);
+	u8 num_parents = (parent_name ? 1 : 0);
+
+	return clk_reg_sysctrl(dev, name, parent_names, num_parents,
+			&reg_sel, &reg_mask, &reg_bits, 0, enable_delay_us,
+			flags, &clk_sysctrl_gate_ops);
+}
+
+struct clk *clk_reg_sysctrl_gate_fixed_rate(struct device *dev,
+					const char *name,
+					const char *parent_name,
+					u16 reg_sel,
+					u8 reg_mask,
+					u8 reg_bits,
+					unsigned long rate,
+					unsigned long enable_delay_us,
+					unsigned long flags)
+{
+	const char **parent_names = (parent_name ? &parent_name : NULL);
+	u8 num_parents = (parent_name ? 1 : 0);
+
+	return clk_reg_sysctrl(dev, name, parent_names, num_parents,
+			&reg_sel, &reg_mask, &reg_bits,
+			rate, enable_delay_us, flags,
+			&clk_sysctrl_gate_fixed_rate_ops);
+}
+
+struct clk *clk_reg_sysctrl_set_parent(struct device *dev,
+				const char *name,
+				const char **parent_names,
+				u8 num_parents,
+				u16 *reg_sel,
+				u8 *reg_mask,
+				u8 *reg_bits,
+				unsigned long flags)
+{
+	return clk_reg_sysctrl(dev, name, parent_names, num_parents,
+			reg_sel, reg_mask, reg_bits, 0, 0, flags,
+			&clk_sysctrl_set_parent_ops);
+}
diff --git a/drivers/clk/ux500/clk.h b/drivers/clk/ux500/clk.h
index c3e449169a8..a2bb92d85ee 100644
--- a/drivers/clk/ux500/clk.h
+++ b/drivers/clk/ux500/clk.h
@@ -11,16 +11,18 @@
 #define __UX500_CLK_H
 
 #include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/types.h>
 
 struct clk *clk_reg_prcc_pclk(const char *name,
 			      const char *parent_name,
-			      unsigned int phy_base,
+			      resource_size_t phy_base,
 			      u32 cg_sel,
 			      unsigned long flags);
 
 struct clk *clk_reg_prcc_kclk(const char *name,
 			      const char *parent_name,
-			      unsigned int phy_base,
+			      resource_size_t phy_base,
 			      u32 cg_sel,
 			      unsigned long flags);
 
@@ -57,4 +59,32 @@ struct clk *clk_reg_prcmu_opp_volt_scalable(const char *name,
 					    unsigned long rate,
 					    unsigned long flags);
 
+struct clk *clk_reg_sysctrl_gate(struct device *dev,
+				 const char *name,
+				 const char *parent_name,
+				 u16 reg_sel,
+				 u8 reg_mask,
+				 u8 reg_bits,
+				 unsigned long enable_delay_us,
+				 unsigned long flags);
+
+struct clk *clk_reg_sysctrl_gate_fixed_rate(struct device *dev,
+					    const char *name,
+					    const char *parent_name,
+					    u16 reg_sel,
+					    u8 reg_mask,
+					    u8 reg_bits,
+					    unsigned long rate,
+					    unsigned long enable_delay_us,
+					    unsigned long flags);
+
+struct clk *clk_reg_sysctrl_set_parent(struct device *dev,
+				       const char *name,
+				       const char **parent_names,
+				       u8 num_parents,
+				       u16 *reg_sel,
+				       u8 *reg_mask,
+				       u8 *reg_bits,
+				       unsigned long flags);
+
 #endif /* __UX500_CLK_H */
diff --git a/drivers/clk/versatile/Makefile b/drivers/clk/versatile/Makefile
index ec3b88fe3e6..c16ca787170 100644
--- a/drivers/clk/versatile/Makefile
+++ b/drivers/clk/versatile/Makefile
@@ -3,5 +3,5 @@ obj-$(CONFIG_ICST)		+= clk-icst.o
 obj-$(CONFIG_ARCH_INTEGRATOR)	+= clk-integrator.o
 obj-$(CONFIG_INTEGRATOR_IMPD1)	+= clk-impd1.o
 obj-$(CONFIG_ARCH_REALVIEW)	+= clk-realview.o
-obj-$(CONFIG_ARCH_VEXPRESS)	+= clk-vexpress.o
+obj-$(CONFIG_ARCH_VEXPRESS)	+= clk-vexpress.o clk-sp810.o
 obj-$(CONFIG_VEXPRESS_CONFIG)	+= clk-vexpress-osc.o
diff --git a/drivers/clk/versatile/clk-sp810.c b/drivers/clk/versatile/clk-sp810.c
new file mode 100644
index 00000000000..bf9b15a585e
--- /dev/null
+++ b/drivers/clk/versatile/clk-sp810.c
@@ -0,0 +1,188 @@
+/*
+ * 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.
+ *
+ * Copyright (C) 2013 ARM Limited
+ */
+
+#include <linux/amba/sp810.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/err.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+
+#define to_clk_sp810_timerclken(_hw) \
+		container_of(_hw, struct clk_sp810_timerclken, hw)
+
+struct clk_sp810;
+
+struct clk_sp810_timerclken {
+	struct clk_hw hw;
+	struct clk *clk;
+	struct clk_sp810 *sp810;
+	int channel;
+};
+
+struct clk_sp810 {
+	struct device_node *node;
+	int refclk_index, timclk_index;
+	void __iomem *base;
+	spinlock_t lock;
+	struct clk_sp810_timerclken timerclken[4];
+	struct clk *refclk;
+	struct clk *timclk;
+};
+
+static u8 clk_sp810_timerclken_get_parent(struct clk_hw *hw)
+{
+	struct clk_sp810_timerclken *timerclken = to_clk_sp810_timerclken(hw);
+	u32 val = readl(timerclken->sp810->base + SCCTRL);
+
+	return !!(val & (1 << SCCTRL_TIMERENnSEL_SHIFT(timerclken->channel)));
+}
+
+static int clk_sp810_timerclken_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct clk_sp810_timerclken *timerclken = to_clk_sp810_timerclken(hw);
+	struct clk_sp810 *sp810 = timerclken->sp810;
+	u32 val, shift = SCCTRL_TIMERENnSEL_SHIFT(timerclken->channel);
+	unsigned long flags = 0;
+
+	if (WARN_ON(index > 1))
+		return -EINVAL;
+
+	spin_lock_irqsave(&sp810->lock, flags);
+
+	val = readl(sp810->base + SCCTRL);
+	val &= ~(1 << shift);
+	val |= index << shift;
+	writel(val, sp810->base + SCCTRL);
+
+	spin_unlock_irqrestore(&sp810->lock, flags);
+
+	return 0;
+}
+
+/*
+ * FIXME - setting the parent every time .prepare is invoked is inefficient.
+ * This is better handled by a dedicated clock tree configuration mechanism at
+ * init-time.  Revisit this later when such a mechanism exists
+ */
+static int clk_sp810_timerclken_prepare(struct clk_hw *hw)
+{
+	struct clk_sp810_timerclken *timerclken = to_clk_sp810_timerclken(hw);
+	struct clk_sp810 *sp810 = timerclken->sp810;
+	struct clk *old_parent = __clk_get_parent(hw->clk);
+	struct clk *new_parent;
+
+	if (!sp810->refclk)
+		sp810->refclk = of_clk_get(sp810->node, sp810->refclk_index);
+
+	if (!sp810->timclk)
+		sp810->timclk = of_clk_get(sp810->node, sp810->timclk_index);
+
+	if (WARN_ON(IS_ERR(sp810->refclk) || IS_ERR(sp810->timclk)))
+		return -ENOENT;
+
+	/* Select fastest parent */
+	if (clk_get_rate(sp810->refclk) > clk_get_rate(sp810->timclk))
+		new_parent = sp810->refclk;
+	else
+		new_parent = sp810->timclk;
+
+	/* Switch the parent if necessary */
+	if (old_parent != new_parent) {
+		clk_prepare(new_parent);
+		clk_set_parent(hw->clk, new_parent);
+		clk_unprepare(old_parent);
+	}
+
+	return 0;
+}
+
+static void clk_sp810_timerclken_unprepare(struct clk_hw *hw)
+{
+	struct clk_sp810_timerclken *timerclken = to_clk_sp810_timerclken(hw);
+	struct clk_sp810 *sp810 = timerclken->sp810;
+
+	clk_put(sp810->timclk);
+	clk_put(sp810->refclk);
+}
+
+static const struct clk_ops clk_sp810_timerclken_ops = {
+	.prepare = clk_sp810_timerclken_prepare,
+	.unprepare = clk_sp810_timerclken_unprepare,
+	.get_parent = clk_sp810_timerclken_get_parent,
+	.set_parent = clk_sp810_timerclken_set_parent,
+};
+
+struct clk *clk_sp810_timerclken_of_get(struct of_phandle_args *clkspec,
+		void *data)
+{
+	struct clk_sp810 *sp810 = data;
+
+	if (WARN_ON(clkspec->args_count != 1 || clkspec->args[0] >
+			ARRAY_SIZE(sp810->timerclken)))
+		return NULL;
+
+	return sp810->timerclken[clkspec->args[0]].clk;
+}
+
+void __init clk_sp810_of_setup(struct device_node *node)
+{
+	struct clk_sp810 *sp810 = kzalloc(sizeof(*sp810), GFP_KERNEL);
+	const char *parent_names[2];
+	char name[12];
+	struct clk_init_data init;
+	int i;
+
+	if (!sp810) {
+		pr_err("Failed to allocate memory for SP810!\n");
+		return;
+	}
+
+	sp810->refclk_index = of_property_match_string(node, "clock-names",
+			"refclk");
+	parent_names[0] = of_clk_get_parent_name(node, sp810->refclk_index);
+
+	sp810->timclk_index = of_property_match_string(node, "clock-names",
+			"timclk");
+	parent_names[1] = of_clk_get_parent_name(node, sp810->timclk_index);
+
+	if (parent_names[0] <= 0 || parent_names[1] <= 0) {
+		pr_warn("Failed to obtain parent clocks for SP810!\n");
+		return;
+	}
+
+	sp810->node = node;
+	sp810->base = of_iomap(node, 0);
+	spin_lock_init(&sp810->lock);
+
+	init.name = name;
+	init.ops = &clk_sp810_timerclken_ops;
+	init.flags = CLK_IS_BASIC;
+	init.parent_names = parent_names;
+	init.num_parents = ARRAY_SIZE(parent_names);
+
+	for (i = 0; i < ARRAY_SIZE(sp810->timerclken); i++) {
+		snprintf(name, ARRAY_SIZE(name), "timerclken%d", i);
+
+		sp810->timerclken[i].sp810 = sp810;
+		sp810->timerclken[i].channel = i;
+		sp810->timerclken[i].hw.init = &init;
+
+		sp810->timerclken[i].clk = clk_register(NULL,
+				&sp810->timerclken[i].hw);
+		WARN_ON(IS_ERR(sp810->timerclken[i].clk));
+	}
+
+	of_clk_add_provider(node, clk_sp810_timerclken_of_get, sp810);
+}
+CLK_OF_DECLARE(sp810, "arm,sp810", clk_sp810_of_setup);
diff --git a/drivers/clk/versatile/clk-vexpress.c b/drivers/clk/versatile/clk-vexpress.c
index 82b45aad8cc..a4a728d0509 100644
--- a/drivers/clk/versatile/clk-vexpress.c
+++ b/drivers/clk/versatile/clk-vexpress.c
@@ -15,8 +15,6 @@
 #include <linux/clkdev.h>
 #include <linux/clk-provider.h>
 #include <linux/err.h>
-#include <linux/of.h>
-#include <linux/of_address.h>
 #include <linux/vexpress.h>
 
 static struct clk *vexpress_sp810_timerclken[4];
@@ -86,50 +84,3 @@ void __init vexpress_clk_init(void __iomem *sp810_base)
 	WARN_ON(clk_register_clkdev(vexpress_sp810_timerclken[1],
 				"v2m-timer1", "sp804"));
 }
-
-#if defined(CONFIG_OF)
-
-struct clk *vexpress_sp810_of_get(struct of_phandle_args *clkspec, void *data)
-{
-	if (WARN_ON(clkspec->args_count != 1 || clkspec->args[0] >
-			ARRAY_SIZE(vexpress_sp810_timerclken)))
-		return NULL;
-
-	return vexpress_sp810_timerclken[clkspec->args[0]];
-}
-
-void __init vexpress_clk_of_init(void)
-{
-	struct device_node *node;
-	struct clk *clk;
-	struct clk *refclk, *timclk;
-
-	of_clk_init(NULL);
-
-	node = of_find_compatible_node(NULL, NULL, "arm,sp810");
-	vexpress_sp810_init(of_iomap(node, 0));
-	of_clk_add_provider(node, vexpress_sp810_of_get, NULL);
-
-	/* Select "better" (faster) parent for SP804 timers */
-	refclk = of_clk_get_by_name(node, "refclk");
-	timclk = of_clk_get_by_name(node, "timclk");
-	if (!WARN_ON(IS_ERR(refclk) || IS_ERR(timclk))) {
-		int i = 0;
-
-		if (clk_get_rate(refclk) > clk_get_rate(timclk))
-			clk = refclk;
-		else
-			clk = timclk;
-
-		for (i = 0; i < ARRAY_SIZE(vexpress_sp810_timerclken); i++)
-			WARN_ON(clk_set_parent(vexpress_sp810_timerclken[i],
-					clk));
-	}
-
-	WARN_ON(clk_register_clkdev(vexpress_sp810_timerclken[0],
-				"v2m-timer0", "sp804"));
-	WARN_ON(clk_register_clkdev(vexpress_sp810_timerclken[1],
-				"v2m-timer1", "sp804"));
-}
-
-#endif
diff --git a/drivers/clocksource/Makefile b/drivers/clocksource/Makefile
index 4d8283aec5b..96e25319659 100644
--- a/drivers/clocksource/Makefile
+++ b/drivers/clocksource/Makefile
@@ -19,6 +19,7 @@ obj-$(CONFIG_ARCH_BCM2835)	+= bcm2835_timer.o
 obj-$(CONFIG_SUNXI_TIMER)	+= sunxi_timer.o
 obj-$(CONFIG_ARCH_TEGRA)	+= tegra20_timer.o
 obj-$(CONFIG_VT8500_TIMER)	+= vt8500_timer.o
+obj-$(CONFIG_ARCH_BCM)		+= bcm_kona_timer.o
 
 obj-$(CONFIG_ARM_ARCH_TIMER)		+= arm_arch_timer.o
 obj-$(CONFIG_CLKSRC_METAG_GENERIC)	+= metag_generic.o
diff --git a/drivers/clocksource/bcm_kona_timer.c b/drivers/clocksource/bcm_kona_timer.c
new file mode 100644
index 00000000000..350f4935645
--- /dev/null
+++ b/drivers/clocksource/bcm_kona_timer.c
@@ -0,0 +1,211 @@
+/*
+ * Copyright (C) 2012 Broadcom Corporation
+ *
+ * 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 version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/init.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/jiffies.h>
+#include <linux/clockchips.h>
+#include <linux/types.h>
+
+#include <linux/io.h>
+#include <asm/mach/time.h>
+
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+
+
+#define KONA_GPTIMER_STCS_OFFSET			0x00000000
+#define KONA_GPTIMER_STCLO_OFFSET			0x00000004
+#define KONA_GPTIMER_STCHI_OFFSET			0x00000008
+#define KONA_GPTIMER_STCM0_OFFSET			0x0000000C
+
+#define KONA_GPTIMER_STCS_TIMER_MATCH_SHIFT		0
+#define KONA_GPTIMER_STCS_COMPARE_ENABLE_SHIFT		4
+
+struct kona_bcm_timers {
+	int tmr_irq;
+	void __iomem *tmr_regs;
+};
+
+static struct kona_bcm_timers timers;
+
+static u32 arch_timer_rate;
+
+/*
+ * We use the peripheral timers for system tick, the cpu global timer for
+ * profile tick
+ */
+static void kona_timer_disable_and_clear(void __iomem *base)
+{
+	uint32_t reg;
+
+	/*
+	 * clear and disable interrupts
+	 * We are using compare/match register 0 for our system interrupts
+	 */
+	reg = readl(base + KONA_GPTIMER_STCS_OFFSET);
+
+	/* Clear compare (0) interrupt */
+	reg |= 1 << KONA_GPTIMER_STCS_TIMER_MATCH_SHIFT;
+	/* disable compare */
+	reg &= ~(1 << KONA_GPTIMER_STCS_COMPARE_ENABLE_SHIFT);
+
+	writel(reg, base + KONA_GPTIMER_STCS_OFFSET);
+
+}
+
+static void
+kona_timer_get_counter(void *timer_base, uint32_t *msw, uint32_t *lsw)
+{
+	void __iomem *base = IOMEM(timer_base);
+	int loop_limit = 4;
+
+	/*
+	 * Read 64-bit free running counter
+	 * 1. Read hi-word
+	 * 2. Read low-word
+	 * 3. Read hi-word again
+	 * 4.1
+	 *      if new hi-word is not equal to previously read hi-word, then
+	 *      start from #1
+	 * 4.2
+	 *      if new hi-word is equal to previously read hi-word then stop.
+	 */
+
+	while (--loop_limit) {
+		*msw = readl(base + KONA_GPTIMER_STCHI_OFFSET);
+		*lsw = readl(base + KONA_GPTIMER_STCLO_OFFSET);
+		if (*msw == readl(base + KONA_GPTIMER_STCHI_OFFSET))
+			break;
+	}
+	if (!loop_limit) {
+		pr_err("bcm_kona_timer: getting counter failed.\n");
+		pr_err(" Timer will be impacted\n");
+	}
+
+	return;
+}
+
+static const struct of_device_id bcm_timer_ids[] __initconst = {
+	{.compatible = "bcm,kona-timer"},
+	{},
+};
+
+static void __init kona_timers_init(void)
+{
+	struct device_node *node;
+	u32 freq;
+
+	node = of_find_matching_node(NULL, bcm_timer_ids);
+
+	if (!node)
+		panic("No timer");
+
+	if (!of_property_read_u32(node, "clock-frequency", &freq))
+		arch_timer_rate = freq;
+	else
+		panic("clock-frequency not set in the .dts file");
+
+	/* Setup IRQ numbers */
+	timers.tmr_irq = irq_of_parse_and_map(node, 0);
+
+	/* Setup IO addresses */
+	timers.tmr_regs = of_iomap(node, 0);
+
+	kona_timer_disable_and_clear(timers.tmr_regs);
+}
+
+static int kona_timer_set_next_event(unsigned long clc,
+				  struct clock_event_device *unused)
+{
+	/*
+	 * timer (0) is disabled by the timer interrupt already
+	 * so, here we reload the next event value and re-enable
+	 * the timer.
+	 *
+	 * This way, we are potentially losing the time between
+	 * timer-interrupt->set_next_event. CPU local timers, when
+	 * they come in should get rid of skew.
+	 */
+
+	uint32_t lsw, msw;
+	uint32_t reg;
+
+	kona_timer_get_counter(timers.tmr_regs, &msw, &lsw);
+
+	/* Load the "next" event tick value */
+	writel(lsw + clc, timers.tmr_regs + KONA_GPTIMER_STCM0_OFFSET);
+
+	/* Enable compare */
+	reg = readl(timers.tmr_regs + KONA_GPTIMER_STCS_OFFSET);
+	reg |= (1 << KONA_GPTIMER_STCS_COMPARE_ENABLE_SHIFT);
+	writel(reg, timers.tmr_regs + KONA_GPTIMER_STCS_OFFSET);
+
+	return 0;
+}
+
+static void kona_timer_set_mode(enum clock_event_mode mode,
+			     struct clock_event_device *unused)
+{
+	switch (mode) {
+	case CLOCK_EVT_MODE_ONESHOT:
+		/* by default mode is one shot don't do any thing */
+		break;
+	case CLOCK_EVT_MODE_UNUSED:
+	case CLOCK_EVT_MODE_SHUTDOWN:
+	default:
+		kona_timer_disable_and_clear(timers.tmr_regs);
+	}
+}
+
+static struct clock_event_device kona_clockevent_timer = {
+	.name = "timer 1",
+	.features = CLOCK_EVT_FEAT_ONESHOT,
+	.set_next_event = kona_timer_set_next_event,
+	.set_mode = kona_timer_set_mode
+};
+
+static void __init kona_timer_clockevents_init(void)
+{
+	kona_clockevent_timer.cpumask = cpumask_of(0);
+	clockevents_config_and_register(&kona_clockevent_timer,
+		arch_timer_rate, 6, 0xffffffff);
+}
+
+static irqreturn_t kona_timer_interrupt(int irq, void *dev_id)
+{
+	struct clock_event_device *evt = &kona_clockevent_timer;
+
+	kona_timer_disable_and_clear(timers.tmr_regs);
+	evt->event_handler(evt);
+	return IRQ_HANDLED;
+}
+
+static struct irqaction kona_timer_irq = {
+	.name = "Kona Timer Tick",
+	.flags = IRQF_TIMER,
+	.handler = kona_timer_interrupt,
+};
+
+static void __init kona_timer_init(void)
+{
+	kona_timers_init();
+	kona_timer_clockevents_init();
+	setup_irq(timers.tmr_irq, &kona_timer_irq);
+	kona_timer_set_next_event((arch_timer_rate / HZ), NULL);
+}
+
+CLOCKSOURCE_OF_DECLARE(bcm_kona, "bcm,kona-timer",
+	kona_timer_init);
diff --git a/drivers/clocksource/sunxi_timer.c b/drivers/clocksource/sunxi_timer.c
index 4086b916715..0ce85e29769 100644
--- a/drivers/clocksource/sunxi_timer.c
+++ b/drivers/clocksource/sunxi_timer.c
@@ -23,7 +23,7 @@
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
 #include <linux/sunxi_timer.h>
-#include <linux/clk-provider.h>
+#include <linux/clk/sunxi.h>
 
 #define TIMER_CTL_REG		0x00
 #define TIMER_CTL_ENABLE		(1 << 0)
@@ -123,7 +123,7 @@ void __init sunxi_timer_init(void)
 	if (irq <= 0)
 		panic("Can't parse IRQ");
 
-	of_clk_init(NULL);
+	sunxi_init_clocks();
 
 	clk = of_clk_get(node, 0);
 	if (IS_ERR(clk))
diff --git a/drivers/cpufreq/acpi-cpufreq.c b/drivers/cpufreq/acpi-cpufreq.c
index 937bc286591..57a8774f0b4 100644
--- a/drivers/cpufreq/acpi-cpufreq.c
+++ b/drivers/cpufreq/acpi-cpufreq.c
@@ -730,7 +730,6 @@ static int acpi_cpufreq_cpu_init(struct cpufreq_policy *policy)
 	    policy->shared_type == CPUFREQ_SHARED_TYPE_ANY) {
 		cpumask_copy(policy->cpus, perf->shared_cpu_map);
 	}
-	cpumask_copy(policy->related_cpus, perf->shared_cpu_map);
 
 #ifdef CONFIG_SMP
 	dmi_check_system(sw_any_bug_dmi_table);
@@ -742,7 +741,6 @@ static int acpi_cpufreq_cpu_init(struct cpufreq_policy *policy)
 	if (check_amd_hwpstate_cpu(cpu) && !acpi_pstate_strict) {
 		cpumask_clear(policy->cpus);
 		cpumask_set_cpu(cpu, policy->cpus);
-		cpumask_copy(policy->related_cpus, cpu_sibling_mask(cpu));
 		policy->shared_type = CPUFREQ_SHARED_TYPE_HW;
 		pr_info_once(PFX "overriding BIOS provided _PSD data\n");
 	}
diff --git a/drivers/cpufreq/cpufreq-cpu0.c b/drivers/cpufreq/cpufreq-cpu0.c
index 4e5b7fb8927..37d23a0f8c5 100644
--- a/drivers/cpufreq/cpufreq-cpu0.c
+++ b/drivers/cpufreq/cpufreq-cpu0.c
@@ -178,10 +178,16 @@ static struct cpufreq_driver cpu0_cpufreq_driver = {
 
 static int cpu0_cpufreq_probe(struct platform_device *pdev)
 {
-	struct device_node *np;
+	struct device_node *np, *parent;
 	int ret;
 
-	for_each_child_of_node(of_find_node_by_path("/cpus"), np) {
+	parent = of_find_node_by_path("/cpus");
+	if (!parent) {
+		pr_err("failed to find OF /cpus\n");
+		return -ENOENT;
+	}
+
+	for_each_child_of_node(parent, np) {
 		if (of_get_property(np, "operating-points", NULL))
 			break;
 	}
diff --git a/drivers/cpufreq/cpufreq_governor.h b/drivers/cpufreq/cpufreq_governor.h
index 46bde01eee6..cc4bd2f6838 100644
--- a/drivers/cpufreq/cpufreq_governor.h
+++ b/drivers/cpufreq/cpufreq_governor.h
@@ -14,8 +14,8 @@
  * published by the Free Software Foundation.
  */
 
-#ifndef _CPUFREQ_GOVERNER_H
-#define _CPUFREQ_GOVERNER_H
+#ifndef _CPUFREQ_GOVERNOR_H
+#define _CPUFREQ_GOVERNOR_H
 
 #include <linux/cpufreq.h>
 #include <linux/kobject.h>
@@ -175,4 +175,4 @@ bool need_load_eval(struct cpu_dbs_common_info *cdbs,
 		unsigned int sampling_rate);
 int cpufreq_governor_dbs(struct dbs_data *dbs_data,
 		struct cpufreq_policy *policy, unsigned int event);
-#endif /* _CPUFREQ_GOVERNER_H */
+#endif /* _CPUFREQ_GOVERNOR_H */
diff --git a/drivers/cpufreq/cpufreq_stats.c b/drivers/cpufreq/cpufreq_stats.c
index 2fd779eb1ed..bfd6273fd87 100644
--- a/drivers/cpufreq/cpufreq_stats.c
+++ b/drivers/cpufreq/cpufreq_stats.c
@@ -180,15 +180,19 @@ static void cpufreq_stats_free_sysfs(unsigned int cpu)
 {
 	struct cpufreq_policy *policy = cpufreq_cpu_get(cpu);
 
-	if (!cpufreq_frequency_get_table(cpu))
+	if (!policy)
 		return;
 
-	if (policy && !policy_is_shared(policy)) {
+	if (!cpufreq_frequency_get_table(cpu))
+		goto put_ref;
+
+	if (!policy_is_shared(policy)) {
 		pr_debug("%s: Free sysfs stat\n", __func__);
 		sysfs_remove_group(&policy->kobj, &stats_attr_group);
 	}
-	if (policy)
-		cpufreq_cpu_put(policy);
+
+put_ref:
+	cpufreq_cpu_put(policy);
 }
 
 static int cpufreq_stats_create_table(struct cpufreq_policy *policy,
diff --git a/drivers/cpufreq/intel_pstate.c b/drivers/cpufreq/intel_pstate.c
index f6dd1e76112..6133ef5cf67 100644
--- a/drivers/cpufreq/intel_pstate.c
+++ b/drivers/cpufreq/intel_pstate.c
@@ -358,14 +358,14 @@ static void intel_pstate_sysfs_expose_params(void)
 static int intel_pstate_min_pstate(void)
 {
 	u64 value;
-	rdmsrl(0xCE, value);
+	rdmsrl(MSR_PLATFORM_INFO, value);
 	return (value >> 40) & 0xFF;
 }
 
 static int intel_pstate_max_pstate(void)
 {
 	u64 value;
-	rdmsrl(0xCE, value);
+	rdmsrl(MSR_PLATFORM_INFO, value);
 	return (value >> 8) & 0xFF;
 }
 
@@ -373,7 +373,7 @@ static int intel_pstate_turbo_pstate(void)
 {
 	u64 value;
 	int nont, ret;
-	rdmsrl(0x1AD, value);
+	rdmsrl(MSR_NHM_TURBO_RATIO_LIMIT, value);
 	nont = intel_pstate_max_pstate();
 	ret = ((value) & 255);
 	if (ret <= nont)
@@ -454,7 +454,7 @@ static inline void intel_pstate_calc_busy(struct cpudata *cpu,
 					sample->idletime_us * 100,
 					sample->duration_us);
 	core_pct = div64_u64(sample->aperf * 100, sample->mperf);
-	sample->freq = cpu->pstate.turbo_pstate * core_pct * 1000;
+	sample->freq = cpu->pstate.max_pstate * core_pct * 1000;
 
 	sample->core_pct_busy = div_s64((sample->pstate_pct_busy * core_pct),
 					100);
@@ -502,7 +502,6 @@ static inline void intel_pstate_set_sample_time(struct cpudata *cpu)
 
 	sample_time = cpu->pstate_policy->sample_rate_ms;
 	delay = msecs_to_jiffies(sample_time);
-	delay -= jiffies % delay;
 	mod_timer_pinned(&cpu->timer, jiffies + delay);
 }
 
@@ -752,6 +751,29 @@ static struct cpufreq_driver intel_pstate_driver = {
 
 static int __initdata no_load;
 
+static int intel_pstate_msrs_not_valid(void)
+{
+	/* Check that all the msr's we are using are valid. */
+	u64 aperf, mperf, tmp;
+
+	rdmsrl(MSR_IA32_APERF, aperf);
+	rdmsrl(MSR_IA32_MPERF, mperf);
+
+	if (!intel_pstate_min_pstate() ||
+		!intel_pstate_max_pstate() ||
+		!intel_pstate_turbo_pstate())
+		return -ENODEV;
+
+	rdmsrl(MSR_IA32_APERF, tmp);
+	if (!(tmp - aperf))
+		return -ENODEV;
+
+	rdmsrl(MSR_IA32_MPERF, tmp);
+	if (!(tmp - mperf))
+		return -ENODEV;
+
+	return 0;
+}
 static int __init intel_pstate_init(void)
 {
 	int cpu, rc = 0;
@@ -764,6 +786,9 @@ static int __init intel_pstate_init(void)
 	if (!id)
 		return -ENODEV;
 
+	if (intel_pstate_msrs_not_valid())
+		return -ENODEV;
+
 	pr_info("Intel P-state driver initializing.\n");
 
 	all_cpu_data = vmalloc(sizeof(void *) * num_possible_cpus());
diff --git a/drivers/crypto/caam/caamalg.c b/drivers/crypto/caam/caamalg.c
index b2a0a0726a5..cf268b14ae9 100644
--- a/drivers/crypto/caam/caamalg.c
+++ b/drivers/crypto/caam/caamalg.c
@@ -1650,11 +1650,7 @@ struct caam_alg_template {
 };
 
 static struct caam_alg_template driver_algs[] = {
-	/*
-	 * single-pass ipsec_esp descriptor
-	 * authencesn(*,*) is also registered, although not present
-	 * explicitly here.
-	 */
+	/* single-pass ipsec_esp descriptor */
 	{
 		.name = "authenc(hmac(md5),cbc(aes))",
 		.driver_name = "authenc-hmac-md5-cbc-aes-caam",
@@ -2217,9 +2213,7 @@ static int __init caam_algapi_init(void)
 	for (i = 0; i < ARRAY_SIZE(driver_algs); i++) {
 		/* TODO: check if h/w supports alg */
 		struct caam_crypto_alg *t_alg;
-		bool done = false;
 
-authencesn:
 		t_alg = caam_alg_alloc(ctrldev, &driver_algs[i]);
 		if (IS_ERR(t_alg)) {
 			err = PTR_ERR(t_alg);
@@ -2233,25 +2227,8 @@ authencesn:
 			dev_warn(ctrldev, "%s alg registration failed\n",
 				t_alg->crypto_alg.cra_driver_name);
 			kfree(t_alg);
-		} else {
+		} else
 			list_add_tail(&t_alg->entry, &priv->alg_list);
-			if (driver_algs[i].type == CRYPTO_ALG_TYPE_AEAD &&
-			    !memcmp(driver_algs[i].name, "authenc", 7) &&
-			    !done) {
-				char *name;
-
-				name = driver_algs[i].name;
-				memmove(name + 10, name + 7, strlen(name) - 7);
-				memcpy(name + 7, "esn", 3);
-
-				name = driver_algs[i].driver_name;
-				memmove(name + 10, name + 7, strlen(name) - 7);
-				memcpy(name + 7, "esn", 3);
-
-				done = true;
-				goto authencesn;
-			}
-		}
 	}
 	if (!list_empty(&priv->alg_list))
 		dev_info(ctrldev, "%s algorithms registered in /proc/crypto\n",
diff --git a/drivers/crypto/caam/compat.h b/drivers/crypto/caam/compat.h
index cf15e781380..762aeff626a 100644
--- a/drivers/crypto/caam/compat.h
+++ b/drivers/crypto/caam/compat.h
@@ -23,7 +23,6 @@
 #include <linux/types.h>
 #include <linux/debugfs.h>
 #include <linux/circ_buf.h>
-#include <linux/string.h>
 #include <net/xfrm.h>
 
 #include <crypto/algapi.h>
diff --git a/drivers/crypto/caam/ctrl.c b/drivers/crypto/caam/ctrl.c
index 1c56f63524f..8acf00490fd 100644
--- a/drivers/crypto/caam/ctrl.c
+++ b/drivers/crypto/caam/ctrl.c
@@ -66,7 +66,7 @@ static void build_instantiation_desc(u32 *desc)
 
 	/*
 	 * load 1 to clear written reg:
-	 * resets the done interrrupt and returns the RNG to idle.
+	 * resets the done interrupt and returns the RNG to idle.
 	 */
 	append_load_imm_u32(desc, 1, LDST_SRCDST_WORD_CLRW);
 
diff --git a/drivers/crypto/talitos.c b/drivers/crypto/talitos.c
index 09b184adf31..5b2b5e61e4f 100644
--- a/drivers/crypto/talitos.c
+++ b/drivers/crypto/talitos.c
@@ -38,7 +38,6 @@
 #include <linux/spinlock.h>
 #include <linux/rtnetlink.h>
 #include <linux/slab.h>
-#include <linux/string.h>
 
 #include <crypto/algapi.h>
 #include <crypto/aes.h>
@@ -1974,11 +1973,7 @@ struct talitos_alg_template {
 };
 
 static struct talitos_alg_template driver_algs[] = {
-	/*
-	 * AEAD algorithms. These use a single-pass ipsec_esp descriptor.
-	 * authencesn(*,*) is also registered, although not present
-	 * explicitly here.
-	 */
+	/* AEAD algorithms.  These use a single-pass ipsec_esp descriptor */
 	{	.type = CRYPTO_ALG_TYPE_AEAD,
 		.alg.crypto = {
 			.cra_name = "authenc(hmac(sha1),cbc(aes))",
@@ -2820,9 +2815,7 @@ static int talitos_probe(struct platform_device *ofdev)
 		if (hw_supports(dev, driver_algs[i].desc_hdr_template)) {
 			struct talitos_crypto_alg *t_alg;
 			char *name = NULL;
-			bool authenc = false;
 
-authencesn:
 			t_alg = talitos_alg_alloc(dev, &driver_algs[i]);
 			if (IS_ERR(t_alg)) {
 				err = PTR_ERR(t_alg);
@@ -2837,8 +2830,6 @@ authencesn:
 				err = crypto_register_alg(
 						&t_alg->algt.alg.crypto);
 				name = t_alg->algt.alg.crypto.cra_driver_name;
-				authenc = authenc ? !authenc :
-					  !(bool)memcmp(name, "authenc", 7);
 				break;
 			case CRYPTO_ALG_TYPE_AHASH:
 				err = crypto_register_ahash(
@@ -2851,25 +2842,8 @@ authencesn:
 				dev_err(dev, "%s alg registration failed\n",
 					name);
 				kfree(t_alg);
-			} else {
+			} else
 				list_add_tail(&t_alg->entry, &priv->alg_list);
-				if (authenc) {
-					struct crypto_alg *alg =
-						&driver_algs[i].alg.crypto;
-
-					name = alg->cra_name;
-					memmove(name + 10, name + 7,
-						strlen(name) - 7);
-					memcpy(name + 7, "esn", 3);
-
-					name = alg->cra_driver_name;
-					memmove(name + 10, name + 7,
-						strlen(name) - 7);
-					memcpy(name + 7, "esn", 3);
-
-					goto authencesn;
-				}
-			}
 		}
 	}
 	if (!list_empty(&priv->alg_list))
diff --git a/drivers/crypto/ux500/cryp/cryp_core.c b/drivers/crypto/ux500/cryp/cryp_core.c
index 8bc5fef07e7..22c9063e012 100644
--- a/drivers/crypto/ux500/cryp/cryp_core.c
+++ b/drivers/crypto/ux500/cryp/cryp_core.c
@@ -1750,7 +1750,7 @@ static struct platform_driver cryp_driver = {
 	.shutdown = ux500_cryp_shutdown,
 	.driver = {
 		.owner = THIS_MODULE,
-		.name  = "cryp1"
+		.name  = "cryp1",
 		.pm    = &ux500_cryp_pm,
 	}
 };
diff --git a/drivers/dma/Kconfig b/drivers/dma/Kconfig
index 80b69971cf2..aeaea32bcfd 100644
--- a/drivers/dma/Kconfig
+++ b/drivers/dma/Kconfig
@@ -83,6 +83,7 @@ config INTEL_IOP_ADMA
 
 config DW_DMAC
 	tristate "Synopsys DesignWare AHB DMA support"
+	depends on GENERIC_HARDIRQS
 	select DMA_ENGINE
 	default y if CPU_AT32AP7000
 	help
diff --git a/drivers/dma/at_hdmac.c b/drivers/dma/at_hdmac.c
index 6e13f262139..88cfc61329d 100644
--- a/drivers/dma/at_hdmac.c
+++ b/drivers/dma/at_hdmac.c
@@ -310,8 +310,6 @@ static void atc_complete_all(struct at_dma_chan *atchan)
 
 	dev_vdbg(chan2dev(&atchan->chan_common), "complete all\n");
 
-	BUG_ON(atc_chan_is_enabled(atchan));
-
 	/*
 	 * Submit queued descriptors ASAP, i.e. before we go through
 	 * the completed ones.
@@ -368,6 +366,9 @@ static void atc_advance_work(struct at_dma_chan *atchan)
 {
 	dev_vdbg(chan2dev(&atchan->chan_common), "advance_work\n");
 
+	if (atc_chan_is_enabled(atchan))
+		return;
+
 	if (list_empty(&atchan->active_list) ||
 	    list_is_singular(&atchan->active_list)) {
 		atc_complete_all(atchan);
@@ -1078,9 +1079,7 @@ static void atc_issue_pending(struct dma_chan *chan)
 		return;
 
 	spin_lock_irqsave(&atchan->lock, flags);
-	if (!atc_chan_is_enabled(atchan)) {
-		atc_advance_work(atchan);
-	}
+	atc_advance_work(atchan);
 	spin_unlock_irqrestore(&atchan->lock, flags);
 }
 
diff --git a/drivers/dma/dw_dmac.c b/drivers/dma/dw_dmac.c
index c599558faed..43a5329d448 100644
--- a/drivers/dma/dw_dmac.c
+++ b/drivers/dma/dw_dmac.c
@@ -1001,6 +1001,13 @@ static inline void convert_burst(u32 *maxburst)
 		*maxburst = 0;
 }
 
+static inline void convert_slave_id(struct dw_dma_chan *dwc)
+{
+	struct dw_dma *dw = to_dw_dma(dwc->chan.device);
+
+	dwc->dma_sconfig.slave_id -= dw->request_line_base;
+}
+
 static int
 set_runtime_config(struct dma_chan *chan, struct dma_slave_config *sconfig)
 {
@@ -1015,6 +1022,7 @@ set_runtime_config(struct dma_chan *chan, struct dma_slave_config *sconfig)
 
 	convert_burst(&dwc->dma_sconfig.src_maxburst);
 	convert_burst(&dwc->dma_sconfig.dst_maxburst);
+	convert_slave_id(dwc);
 
 	return 0;
 }
@@ -1276,9 +1284,9 @@ static struct dma_chan *dw_dma_xlate(struct of_phandle_args *dma_spec,
 	if (dma_spec->args_count != 3)
 		return NULL;
 
-	fargs.req = be32_to_cpup(dma_spec->args+0);
-	fargs.src = be32_to_cpup(dma_spec->args+1);
-	fargs.dst = be32_to_cpup(dma_spec->args+2);
+	fargs.req = dma_spec->args[0];
+	fargs.src = dma_spec->args[1];
+	fargs.dst = dma_spec->args[2];
 
 	if (WARN_ON(fargs.req >= DW_DMA_MAX_NR_REQUESTS ||
 		    fargs.src >= dw->nr_masters ||
@@ -1628,6 +1636,7 @@ dw_dma_parse_dt(struct platform_device *pdev)
 
 static int dw_probe(struct platform_device *pdev)
 {
+	const struct platform_device_id *match;
 	struct dw_dma_platform_data *pdata;
 	struct resource		*io;
 	struct dw_dma		*dw;
@@ -1711,6 +1720,11 @@ static int dw_probe(struct platform_device *pdev)
 		memcpy(dw->data_width, pdata->data_width, 4);
 	}
 
+	/* Get the base request line if set */
+	match = platform_get_device_id(pdev);
+	if (match)
+		dw->request_line_base = (unsigned int)match->driver_data;
+
 	/* Calculate all channel mask before DMA setup */
 	dw->all_chan_mask = (1 << nr_channels) - 1;
 
@@ -1906,7 +1920,8 @@ MODULE_DEVICE_TABLE(of, dw_dma_id_table);
 #endif
 
 static const struct platform_device_id dw_dma_ids[] = {
-	{ "INTL9C60", 0 },
+	/* Name,	Request Line Base */
+	{ "INTL9C60",	(kernel_ulong_t)16 },
 	{ }
 };
 
diff --git a/drivers/dma/dw_dmac_regs.h b/drivers/dma/dw_dmac_regs.h
index cf0ce5c77d6..4d02c3669b7 100644
--- a/drivers/dma/dw_dmac_regs.h
+++ b/drivers/dma/dw_dmac_regs.h
@@ -247,6 +247,7 @@ struct dw_dma {
 	/* hardware configuration */
 	unsigned char		nr_masters;
 	unsigned char		data_width[4];
+	unsigned int		request_line_base;
 
 	struct dw_dma_chan	chan[0];
 };
diff --git a/drivers/dma/omap-dma.c b/drivers/dma/omap-dma.c
index c4b4fd2acc4..08b43bf3715 100644
--- a/drivers/dma/omap-dma.c
+++ b/drivers/dma/omap-dma.c
@@ -276,12 +276,20 @@ static void omap_dma_issue_pending(struct dma_chan *chan)
 
 	spin_lock_irqsave(&c->vc.lock, flags);
 	if (vchan_issue_pending(&c->vc) && !c->desc) {
-		struct omap_dmadev *d = to_omap_dma_dev(chan->device);
-		spin_lock(&d->lock);
-		if (list_empty(&c->node))
-			list_add_tail(&c->node, &d->pending);
-		spin_unlock(&d->lock);
-		tasklet_schedule(&d->task);
+		/*
+		 * c->cyclic is used only by audio and in this case the DMA need
+		 * to be started without delay.
+		 */
+		if (!c->cyclic) {
+			struct omap_dmadev *d = to_omap_dma_dev(chan->device);
+			spin_lock(&d->lock);
+			if (list_empty(&c->node))
+				list_add_tail(&c->node, &d->pending);
+			spin_unlock(&d->lock);
+			tasklet_schedule(&d->task);
+		} else {
+			omap_dma_start_desc(c);
+		}
 	}
 	spin_unlock_irqrestore(&c->vc.lock, flags);
 }
diff --git a/drivers/dma/pl330.c b/drivers/dma/pl330.c
index 71815312275..5dbc5946c4c 100644
--- a/drivers/dma/pl330.c
+++ b/drivers/dma/pl330.c
@@ -2882,7 +2882,7 @@ pl330_probe(struct amba_device *adev, const struct amba_id *id)
 {
 	struct dma_pl330_platdata *pdat;
 	struct dma_pl330_dmac *pdmac;
-	struct dma_pl330_chan *pch;
+	struct dma_pl330_chan *pch, *_p;
 	struct pl330_info *pi;
 	struct dma_device *pd;
 	struct resource *res;
@@ -2984,7 +2984,16 @@ pl330_probe(struct amba_device *adev, const struct amba_id *id)
 	ret = dma_async_device_register(pd);
 	if (ret) {
 		dev_err(&adev->dev, "unable to register DMAC\n");
-		goto probe_err2;
+		goto probe_err3;
+	}
+
+	if (adev->dev.of_node) {
+		ret = of_dma_controller_register(adev->dev.of_node,
+					 of_dma_pl330_xlate, pdmac);
+		if (ret) {
+			dev_err(&adev->dev,
+			"unable to register DMA to the generic DT DMA helpers\n");
+		}
 	}
 
 	dev_info(&adev->dev,
@@ -2995,16 +3004,21 @@ pl330_probe(struct amba_device *adev, const struct amba_id *id)
 		pi->pcfg.data_bus_width / 8, pi->pcfg.num_chan,
 		pi->pcfg.num_peri, pi->pcfg.num_events);
 
-	ret = of_dma_controller_register(adev->dev.of_node,
-					 of_dma_pl330_xlate, pdmac);
-	if (ret) {
-		dev_err(&adev->dev,
-		"unable to register DMA to the generic DT DMA helpers\n");
-		goto probe_err2;
-	}
-
 	return 0;
+probe_err3:
+	amba_set_drvdata(adev, NULL);
 
+	/* Idle the DMAC */
+	list_for_each_entry_safe(pch, _p, &pdmac->ddma.channels,
+			chan.device_node) {
+
+		/* Remove the channel */
+		list_del(&pch->chan.device_node);
+
+		/* Flush the channel */
+		pl330_control(&pch->chan, DMA_TERMINATE_ALL, 0);
+		pl330_free_chan_resources(&pch->chan);
+	}
 probe_err2:
 	pl330_del(pi);
 probe_err1:
@@ -3023,8 +3037,10 @@ static int pl330_remove(struct amba_device *adev)
 	if (!pdmac)
 		return 0;
 
-	of_dma_controller_free(adev->dev.of_node);
+	if (adev->dev.of_node)
+		of_dma_controller_free(adev->dev.of_node);
 
+	dma_async_device_unregister(&pdmac->ddma);
 	amba_set_drvdata(adev, NULL);
 
 	/* Idle the DMAC */
diff --git a/drivers/edac/amd64_edac.c b/drivers/edac/amd64_edac.c
index e1d13c463c9..8b6a0343c22 100644
--- a/drivers/edac/amd64_edac.c
+++ b/drivers/edac/amd64_edac.c
@@ -98,6 +98,7 @@ int __amd64_write_pci_cfg_dword(struct pci_dev *pdev, int offset,
  *
  * F15h: we select which DCT we access using F1x10C[DctCfgSel]
  *
+ * F16h: has only 1 DCT
  */
 static int k8_read_dct_pci_cfg(struct amd64_pvt *pvt, int addr, u32 *val,
 			       const char *func)
@@ -340,6 +341,27 @@ static void get_cs_base_and_mask(struct amd64_pvt *pvt, int csrow, u8 dct,
 		base_bits	= GENMASK(21, 31) | GENMASK(9, 15);
 		mask_bits	= GENMASK(21, 29) | GENMASK(9, 15);
 		addr_shift	= 4;
+
+	/*
+	* F16h needs two addr_shift values: 8 for high and 6 for low
+	* (cf. F16h BKDG).
+	*/
+	} else if (boot_cpu_data.x86 == 0x16) {
+		csbase          = pvt->csels[dct].csbases[csrow];
+		csmask          = pvt->csels[dct].csmasks[csrow >> 1];
+
+		*base  = (csbase & GENMASK(5,  15)) << 6;
+		*base |= (csbase & GENMASK(19, 30)) << 8;
+
+		*mask = ~0ULL;
+		/* poke holes for the csmask */
+		*mask &= ~((GENMASK(5, 15)  << 6) |
+			   (GENMASK(19, 30) << 8));
+
+		*mask |= (csmask & GENMASK(5, 15))  << 6;
+		*mask |= (csmask & GENMASK(19, 30)) << 8;
+
+		return;
 	} else {
 		csbase		= pvt->csels[dct].csbases[csrow];
 		csmask		= pvt->csels[dct].csmasks[csrow >> 1];
@@ -1150,6 +1172,21 @@ static int f15_dbam_to_chip_select(struct amd64_pvt *pvt, u8 dct,
 	return ddr3_cs_size(cs_mode, false);
 }
 
+/*
+ * F16h has only limited cs_modes
+ */
+static int f16_dbam_to_chip_select(struct amd64_pvt *pvt, u8 dct,
+				unsigned cs_mode)
+{
+	WARN_ON(cs_mode > 12);
+
+	if (cs_mode == 6 || cs_mode == 8 ||
+	    cs_mode == 9 || cs_mode == 12)
+		return -1;
+	else
+		return ddr3_cs_size(cs_mode, false);
+}
+
 static void read_dram_ctl_register(struct amd64_pvt *pvt)
 {
 
@@ -1587,6 +1624,17 @@ static struct amd64_family_type amd64_family_types[] = {
 			.read_dct_pci_cfg	= f15_read_dct_pci_cfg,
 		}
 	},
+	[F16_CPUS] = {
+		.ctl_name = "F16h",
+		.f1_id = PCI_DEVICE_ID_AMD_16H_NB_F1,
+		.f3_id = PCI_DEVICE_ID_AMD_16H_NB_F3,
+		.ops = {
+			.early_channel_count	= f1x_early_channel_count,
+			.map_sysaddr_to_csrow	= f1x_map_sysaddr_to_csrow,
+			.dbam_to_cs		= f16_dbam_to_chip_select,
+			.read_dct_pci_cfg	= f10_read_dct_pci_cfg,
+		}
+	},
 };
 
 /*
@@ -1939,7 +1987,9 @@ static void read_mc_regs(struct amd64_pvt *pvt)
 
 	if (c->x86 >= 0x10) {
 		amd64_read_pci_cfg(pvt->F3, EXT_NB_MCA_CFG, &tmp);
-		amd64_read_dct_pci_cfg(pvt, DBAM1, &pvt->dbam1);
+		if (c->x86 != 0x16)
+			/* F16h has only DCT0 */
+			amd64_read_dct_pci_cfg(pvt, DBAM1, &pvt->dbam1);
 
 		/* F10h, revD and later can do x8 ECC too */
 		if ((c->x86 > 0x10 || c->x86_model > 7) && tmp & BIT(25))
@@ -2356,6 +2406,11 @@ static struct amd64_family_type *amd64_per_family_init(struct amd64_pvt *pvt)
 		pvt->ops		= &amd64_family_types[F15_CPUS].ops;
 		break;
 
+	case 0x16:
+		fam_type		= &amd64_family_types[F16_CPUS];
+		pvt->ops		= &amd64_family_types[F16_CPUS].ops;
+		break;
+
 	default:
 		amd64_err("Unsupported family!\n");
 		return NULL;
@@ -2581,6 +2636,14 @@ static DEFINE_PCI_DEVICE_TABLE(amd64_pci_table) = {
 		.class		= 0,
 		.class_mask	= 0,
 	},
+	{
+		.vendor		= PCI_VENDOR_ID_AMD,
+		.device		= PCI_DEVICE_ID_AMD_16H_NB_F2,
+		.subvendor	= PCI_ANY_ID,
+		.subdevice	= PCI_ANY_ID,
+		.class		= 0,
+		.class_mask	= 0,
+	},
 
 	{0, }
 };
diff --git a/drivers/edac/amd64_edac.h b/drivers/edac/amd64_edac.h
index 35637d83f23..2c6f113bae2 100644
--- a/drivers/edac/amd64_edac.h
+++ b/drivers/edac/amd64_edac.h
@@ -172,7 +172,8 @@
  */
 #define PCI_DEVICE_ID_AMD_15H_NB_F1	0x1601
 #define PCI_DEVICE_ID_AMD_15H_NB_F2	0x1602
-
+#define PCI_DEVICE_ID_AMD_16H_NB_F1	0x1531
+#define PCI_DEVICE_ID_AMD_16H_NB_F2	0x1532
 
 /*
  * Function 1 - Address Map
@@ -296,6 +297,7 @@ enum amd_families {
 	K8_CPUS = 0,
 	F10_CPUS,
 	F15_CPUS,
+	F16_CPUS,
 	NUM_FAMILIES,
 };
 
diff --git a/drivers/edac/i7300_edac.c b/drivers/edac/i7300_edac.c
index 087c27bc5d4..9004c64b169 100644
--- a/drivers/edac/i7300_edac.c
+++ b/drivers/edac/i7300_edac.c
@@ -750,15 +750,23 @@ static int i7300_init_csrows(struct mem_ctl_info *mci)
 	struct i7300_dimm_info *dinfo;
 	int rc = -ENODEV;
 	int mtr;
-	int ch, branch, slot, channel;
+	int ch, branch, slot, channel, max_channel, max_branch;
 	struct dimm_info *dimm;
 
 	pvt = mci->pvt_info;
 
 	edac_dbg(2, "Memory Technology Registers:\n");
 
+	if (IS_SINGLE_MODE(pvt->mc_settings_a)) {
+		max_branch = 1;
+		max_channel = 1;
+	} else {
+		max_branch = MAX_BRANCHES;
+		max_channel = MAX_CH_PER_BRANCH;
+	}
+
 	/* Get the AMB present registers for the four channels */
-	for (branch = 0; branch < MAX_BRANCHES; branch++) {
+	for (branch = 0; branch < max_branch; branch++) {
 		/* Read and dump branch 0's MTRs */
 		channel = to_channel(0, branch);
 		pci_read_config_word(pvt->pci_dev_2x_0_fbd_branch[branch],
@@ -767,6 +775,9 @@ static int i7300_init_csrows(struct mem_ctl_info *mci)
 		edac_dbg(2, "\t\tAMB-present CH%d = 0x%x:\n",
 			 channel, pvt->ambpresent[channel]);
 
+		if (max_channel == 1)
+			continue;
+
 		channel = to_channel(1, branch);
 		pci_read_config_word(pvt->pci_dev_2x_0_fbd_branch[branch],
 				     AMBPRESENT_1,
@@ -778,11 +789,11 @@ static int i7300_init_csrows(struct mem_ctl_info *mci)
 	/* Get the set of MTR[0-7] regs by each branch */
 	for (slot = 0; slot < MAX_SLOTS; slot++) {
 		int where = mtr_regs[slot];
-		for (branch = 0; branch < MAX_BRANCHES; branch++) {
+		for (branch = 0; branch < max_branch; branch++) {
 			pci_read_config_word(pvt->pci_dev_2x_0_fbd_branch[branch],
 					where,
 					&pvt->mtr[slot][branch]);
-			for (ch = 0; ch < MAX_CH_PER_BRANCH; ch++) {
+			for (ch = 0; ch < max_channel; ch++) {
 				int channel = to_channel(ch, branch);
 
 				dimm = EDAC_DIMM_PTR(mci->layers, mci->dimms,
diff --git a/drivers/edac/sb_edac.c b/drivers/edac/sb_edac.c
index 57244f99561..e04462b6075 100644
--- a/drivers/edac/sb_edac.c
+++ b/drivers/edac/sb_edac.c
@@ -331,30 +331,31 @@ struct sbridge_pvt {
 	u64			tolm, tohm;
 };
 
-#define PCI_DESCR(device, function, device_id)	\
-	.dev = (device),			\
-	.func = (function),			\
-	.dev_id = (device_id)
+#define PCI_DESCR(device, function, device_id, opt)	\
+	.dev = (device),				\
+	.func = (function),				\
+	.dev_id = (device_id),				\
+	.optional = opt
 
 static const struct pci_id_descr pci_dev_descr_sbridge[] = {
 		/* Processor Home Agent */
-	{ PCI_DESCR(14, 0, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_HA0)		},
+	{ PCI_DESCR(14, 0, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_HA0, 0)	},
 
 		/* Memory controller */
-	{ PCI_DESCR(15, 0, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TA)		},
-	{ PCI_DESCR(15, 1, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_RAS)		},
-	{ PCI_DESCR(15, 2, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD0)	},
-	{ PCI_DESCR(15, 3, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD1)	},
-	{ PCI_DESCR(15, 4, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD2)	},
-	{ PCI_DESCR(15, 5, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD3)	},
-	{ PCI_DESCR(17, 0, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_DDRIO)	},
+	{ PCI_DESCR(15, 0, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TA, 0)	},
+	{ PCI_DESCR(15, 1, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_RAS, 0)	},
+	{ PCI_DESCR(15, 2, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD0, 0)	},
+	{ PCI_DESCR(15, 3, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD1, 0)	},
+	{ PCI_DESCR(15, 4, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD2, 0)	},
+	{ PCI_DESCR(15, 5, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD3, 0)	},
+	{ PCI_DESCR(17, 0, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_DDRIO, 1)	},
 
 		/* System Address Decoder */
-	{ PCI_DESCR(12, 6, PCI_DEVICE_ID_INTEL_SBRIDGE_SAD0)		},
-	{ PCI_DESCR(12, 7, PCI_DEVICE_ID_INTEL_SBRIDGE_SAD1)		},
+	{ PCI_DESCR(12, 6, PCI_DEVICE_ID_INTEL_SBRIDGE_SAD0, 0)		},
+	{ PCI_DESCR(12, 7, PCI_DEVICE_ID_INTEL_SBRIDGE_SAD1, 0)		},
 
 		/* Broadcast Registers */
-	{ PCI_DESCR(13, 6, PCI_DEVICE_ID_INTEL_SBRIDGE_BR)		},
+	{ PCI_DESCR(13, 6, PCI_DEVICE_ID_INTEL_SBRIDGE_BR, 0)		},
 };
 
 #define PCI_ID_TABLE_ENTRY(A) { .descr=A, .n_devs = ARRAY_SIZE(A) }
@@ -556,14 +557,19 @@ static int get_dimm_config(struct mem_ctl_info *mci)
 		pvt->is_close_pg = false;
 	}
 
-	pci_read_config_dword(pvt->pci_ddrio, RANK_CFG_A, &reg);
-	if (IS_RDIMM_ENABLED(reg)) {
-		/* FIXME: Can also be LRDIMM */
-		edac_dbg(0, "Memory is registered\n");
-		mtype = MEM_RDDR3;
+	if (pvt->pci_ddrio) {
+		pci_read_config_dword(pvt->pci_ddrio, RANK_CFG_A, &reg);
+		if (IS_RDIMM_ENABLED(reg)) {
+			/* FIXME: Can also be LRDIMM */
+			edac_dbg(0, "Memory is registered\n");
+			mtype = MEM_RDDR3;
+		} else {
+			edac_dbg(0, "Memory is unregistered\n");
+			mtype = MEM_DDR3;
+		}
 	} else {
-		edac_dbg(0, "Memory is unregistered\n");
-		mtype = MEM_DDR3;
+		edac_dbg(0, "Cannot determine memory type\n");
+		mtype = MEM_UNKNOWN;
 	}
 
 	/* On all supported DDR3 DIMM types, there are 8 banks available */
@@ -1303,8 +1309,7 @@ static int mci_bind_devs(struct mem_ctl_info *mci,
 
 	/* Check if everything were registered */
 	if (!pvt->pci_sad0 || !pvt->pci_sad1 || !pvt->pci_ha0 ||
-	    !pvt-> pci_tad || !pvt->pci_ras  || !pvt->pci_ta ||
-	    !pvt->pci_ddrio)
+	    !pvt-> pci_tad || !pvt->pci_ras  || !pvt->pci_ta)
 		goto enodev;
 
 	for (i = 0; i < NUM_CHANNELS; i++) {
diff --git a/drivers/eisa/eisa-bus.c b/drivers/eisa/eisa-bus.c
index 806c77bfd43..272a3ec3595 100644
--- a/drivers/eisa/eisa-bus.c
+++ b/drivers/eisa/eisa-bus.c
@@ -275,19 +275,18 @@ static int __init eisa_request_resources(struct eisa_root_device *root,
 		}
 		
 		if (slot) {
-			edev->res[i].name  = NULL;
 			edev->res[i].start = SLOT_ADDRESS(root, slot)
 					     + (i * 0x400);
 			edev->res[i].end   = edev->res[i].start + 0xff;
 			edev->res[i].flags = IORESOURCE_IO;
 		} else {
-			edev->res[i].name  = NULL;
 			edev->res[i].start = SLOT_ADDRESS(root, slot)
 					     + EISA_VENDOR_ID_OFFSET;
 			edev->res[i].end   = edev->res[i].start + 3;
-			edev->res[i].flags = IORESOURCE_BUSY;
+			edev->res[i].flags = IORESOURCE_IO | IORESOURCE_BUSY;
 		}
 
+		dev_printk(KERN_DEBUG, &edev->dev, "%pR\n", &edev->res[i]);
 		if (request_resource(root->res, &edev->res[i]))
 			goto failed;
 	}
@@ -314,41 +313,40 @@ static int __init eisa_probe(struct eisa_root_device *root)
 {
         int i, c;
 	struct eisa_device *edev;
+	char *enabled_str;
 
-	printk(KERN_INFO "EISA: Probing bus %d at %s\n",
-	       root->bus_nr, dev_name(root->dev));
+	dev_info(root->dev, "Probing EISA bus %d\n", root->bus_nr);
 
 	/* First try to get hold of slot 0. If there is no device
 	 * here, simply fail, unless root->force_probe is set. */
 	
 	edev = kzalloc(sizeof(*edev), GFP_KERNEL);
 	if (!edev) {
-		printk(KERN_ERR "EISA: Couldn't allocate mainboard slot\n");
+		dev_err(root->dev, "EISA: Couldn't allocate mainboard slot\n");
 		return -ENOMEM;
 	}
 		
-	if (eisa_request_resources(root, edev, 0)) {
-		printk(KERN_WARNING \
-		       "EISA: Cannot allocate resource for mainboard\n");
+	if (eisa_init_device(root, edev, 0)) {
 		kfree(edev);
 		if (!root->force_probe)
-			return -EBUSY;
+			return -ENODEV;
 		goto force_probe;
 	}
 
-	if (eisa_init_device(root, edev, 0)) {
-		eisa_release_resources(edev);
+	if (eisa_request_resources(root, edev, 0)) {
+		dev_warn(root->dev,
+		         "EISA: Cannot allocate resource for mainboard\n");
 		kfree(edev);
 		if (!root->force_probe)
-			return -ENODEV;
+			return -EBUSY;
 		goto force_probe;
 	}
 
-	printk(KERN_INFO "EISA: Mainboard %s detected.\n", edev->id.sig);
+	dev_info(&edev->dev, "EISA: Mainboard %s detected\n", edev->id.sig);
 
 	if (eisa_register_device(edev)) {
-		printk(KERN_ERR "EISA: Failed to register %s\n",
-		       edev->id.sig);
+		dev_err(&edev->dev, "EISA: Failed to register %s\n",
+		        edev->id.sig);
 		eisa_release_resources(edev);
 		kfree(edev);
 	}
@@ -358,55 +356,47 @@ static int __init eisa_probe(struct eisa_root_device *root)
         for (c = 0, i = 1; i <= root->slots; i++) {
 		edev = kzalloc(sizeof(*edev), GFP_KERNEL);
 		if (!edev) {
-			printk(KERN_ERR "EISA: Out of memory for slot %d\n", i);
+			dev_err(root->dev, "EISA: Out of memory for slot %d\n",
+				i);
 			continue;
 		}
 
-		if (eisa_request_resources(root, edev, i)) {
-			printk(KERN_WARNING \
-			       "Cannot allocate resource for EISA slot %d\n",
-			       i);
+		if (eisa_init_device(root, edev, i)) {
 			kfree(edev);
 			continue;
 		}
 
-		if (eisa_init_device(root, edev, i)) {
-			eisa_release_resources(edev);
+		if (eisa_request_resources(root, edev, i)) {
+			dev_warn(root->dev,
+			         "Cannot allocate resource for EISA slot %d\n",
+			         i);
 			kfree(edev);
 			continue;
 		}
-		
-		printk(KERN_INFO "EISA: slot %d : %s detected",
-		       i, edev->id.sig);
-			
-		switch (edev->state) {
-		case EISA_CONFIG_ENABLED | EISA_CONFIG_FORCED:
-			printk(" (forced enabled)");
-			break;
-
-		case EISA_CONFIG_FORCED:
-			printk(" (forced disabled)");
-			break;
-
-		case 0:
-			printk(" (disabled)");
-			break;
-		}
-			
-		printk (".\n");
+
+		if (edev->state == (EISA_CONFIG_ENABLED | EISA_CONFIG_FORCED))
+			enabled_str = " (forced enabled)";
+		else if (edev->state == EISA_CONFIG_FORCED)
+			enabled_str = " (forced disabled)";
+		else if (edev->state == 0)
+			enabled_str = " (disabled)";
+		else
+			enabled_str = "";
+
+		dev_info(&edev->dev, "EISA: slot %d: %s detected%s\n", i,
+			 edev->id.sig, enabled_str);
 
 		c++;
 
 		if (eisa_register_device(edev)) {
-			printk(KERN_ERR "EISA: Failed to register %s\n",
-			       edev->id.sig);
+			dev_err(&edev->dev, "EISA: Failed to register %s\n",
+			        edev->id.sig);
 			eisa_release_resources(edev);
 			kfree(edev);
 		}
         }
 
-	printk(KERN_INFO "EISA: Detected %d card%s.\n", c, c == 1 ? "" : "s");
-
+	dev_info(root->dev, "EISA: Detected %d card%s\n", c, c == 1 ? "" : "s");
 	return 0;
 }
 
diff --git a/drivers/eisa/pci_eisa.c b/drivers/eisa/pci_eisa.c
index cdae207028a..a333bf3517d 100644
--- a/drivers/eisa/pci_eisa.c
+++ b/drivers/eisa/pci_eisa.c
@@ -19,48 +19,72 @@
 /* There is only *one* pci_eisa device per machine, right ? */
 static struct eisa_root_device pci_eisa_root;
 
-static int __init pci_eisa_init(struct pci_dev *pdev,
-				const struct pci_device_id *ent)
+static int __init pci_eisa_init(struct pci_dev *pdev)
 {
-	int rc;
+	int rc, i;
+	struct resource *res, *bus_res = NULL;
 
 	if ((rc = pci_enable_device (pdev))) {
-		printk (KERN_ERR "pci_eisa : Could not enable device %s\n",
-			pci_name(pdev));
+		dev_err(&pdev->dev, "Could not enable device\n");
 		return rc;
 	}
 
+	/*
+	 * The Intel 82375 PCI-EISA bridge is a subtractive-decode PCI
+	 * device, so the resources available on EISA are the same as those
+	 * available on the 82375 bus.  This works the same as a PCI-PCI
+	 * bridge in subtractive-decode mode (see pci_read_bridge_bases()).
+	 * We assume other PCI-EISA bridges are similar.
+	 *
+	 * eisa_root_register() can only deal with a single io port resource,
+	*  so we use the first valid io port resource.
+	 */
+	pci_bus_for_each_resource(pdev->bus, res, i)
+		if (res && (res->flags & IORESOURCE_IO)) {
+			bus_res = res;
+			break;
+		}
+
+	if (!bus_res) {
+		dev_err(&pdev->dev, "No resources available\n");
+		return -1;
+	}
+
 	pci_eisa_root.dev              = &pdev->dev;
-	pci_eisa_root.res	       = pdev->bus->resource[0];
-	pci_eisa_root.bus_base_addr    = pdev->bus->resource[0]->start;
+	pci_eisa_root.res	       = bus_res;
+	pci_eisa_root.bus_base_addr    = bus_res->start;
 	pci_eisa_root.slots	       = EISA_MAX_SLOTS;
 	pci_eisa_root.dma_mask         = pdev->dma_mask;
 	dev_set_drvdata(pci_eisa_root.dev, &pci_eisa_root);
 
 	if (eisa_root_register (&pci_eisa_root)) {
-		printk (KERN_ERR "pci_eisa : Could not register EISA root\n");
+		dev_err(&pdev->dev, "Could not register EISA root\n");
 		return -1;
 	}
 
 	return 0;
 }
 
-static struct pci_device_id pci_eisa_pci_tbl[] = {
-	{ PCI_ANY_ID, PCI_ANY_ID, PCI_ANY_ID, PCI_ANY_ID,
-	  PCI_CLASS_BRIDGE_EISA << 8, 0xffff00, 0 },
-	{ 0, }
-};
+/*
+ * We have to call pci_eisa_init_early() before pnpacpi_init()/isapnp_init().
+ *   Otherwise pnp resource will get enabled early and could prevent eisa
+ *   to be initialized.
+ * Also need to make sure pci_eisa_init_early() is called after
+ * x86/pci_subsys_init().
+ * So need to use subsys_initcall_sync with it.
+ */
+static int __init pci_eisa_init_early(void)
+{
+	struct pci_dev *dev = NULL;
+	int ret;
 
-static struct pci_driver __refdata pci_eisa_driver = {
-	.name		= "pci_eisa",
-	.id_table	= pci_eisa_pci_tbl,
-	.probe		= pci_eisa_init,
-};
+	for_each_pci_dev(dev)
+		if ((dev->class >> 8) == PCI_CLASS_BRIDGE_EISA) {
+			ret = pci_eisa_init(dev);
+			if (ret)
+				return ret;
+		}
 
-static int __init pci_eisa_init_module (void)
-{
-	return pci_register_driver (&pci_eisa_driver);
+	return 0;
 }
-
-device_initcall(pci_eisa_init_module);
-MODULE_DEVICE_TABLE(pci, pci_eisa_pci_tbl);
+subsys_initcall_sync(pci_eisa_init_early);
diff --git a/drivers/extcon/extcon-arizona.c b/drivers/extcon/extcon-arizona.c
index dc357a4051f..7a1b4a7791b 100644
--- a/drivers/extcon/extcon-arizona.c
+++ b/drivers/extcon/extcon-arizona.c
@@ -33,12 +33,17 @@
 #include <linux/mfd/arizona/pdata.h>
 #include <linux/mfd/arizona/registers.h>
 
-#define ARIZONA_NUM_BUTTONS 6
+#define ARIZONA_MAX_MICD_RANGE 8
 
 #define ARIZONA_ACCDET_MODE_MIC 0
 #define ARIZONA_ACCDET_MODE_HPL 1
 #define ARIZONA_ACCDET_MODE_HPR 2
 
+#define ARIZONA_HPDET_MAX 10000
+
+#define HPDET_DEBOUNCE 500
+#define DEFAULT_MICD_TIMEOUT 2000
+
 struct arizona_extcon_info {
 	struct device *dev;
 	struct arizona *arizona;
@@ -46,17 +51,27 @@ struct arizona_extcon_info {
 	struct regulator *micvdd;
 	struct input_dev *input;
 
+	u16 last_jackdet;
+
 	int micd_mode;
 	const struct arizona_micd_config *micd_modes;
 	int micd_num_modes;
 
+	const struct arizona_micd_range *micd_ranges;
+	int num_micd_ranges;
+
+	int micd_timeout;
+
 	bool micd_reva;
 	bool micd_clamp;
 
 	struct delayed_work hpdet_work;
+	struct delayed_work micd_detect_work;
+	struct delayed_work micd_timeout_work;
 
 	bool hpdet_active;
 	bool hpdet_done;
+	bool hpdet_retried;
 
 	int num_hpdet_res;
 	unsigned int hpdet_res[3];
@@ -71,20 +86,25 @@ struct arizona_extcon_info {
 };
 
 static const struct arizona_micd_config micd_default_modes[] = {
-	{ 0,                  2 << ARIZONA_MICD_BIAS_SRC_SHIFT, 1 },
 	{ ARIZONA_ACCDET_SRC, 1 << ARIZONA_MICD_BIAS_SRC_SHIFT, 0 },
+	{ 0,                  2 << ARIZONA_MICD_BIAS_SRC_SHIFT, 1 },
 };
 
-static struct {
-	u16 status;
-	int report;
-} arizona_lvl_to_key[ARIZONA_NUM_BUTTONS] = {
-	{  0x1, BTN_0 },
-	{  0x2, BTN_1 },
-	{  0x4, BTN_2 },
-	{  0x8, BTN_3 },
-	{ 0x10, BTN_4 },
-	{ 0x20, BTN_5 },
+static const struct arizona_micd_range micd_default_ranges[] = {
+	{ .max =  11, .key = BTN_0 },
+	{ .max =  28, .key = BTN_1 },
+	{ .max =  54, .key = BTN_2 },
+	{ .max = 100, .key = BTN_3 },
+	{ .max = 186, .key = BTN_4 },
+	{ .max = 430, .key = BTN_5 },
+};
+
+static const int arizona_micd_levels[] = {
+	3, 6, 8, 11, 13, 16, 18, 21, 23, 26, 28, 31, 34, 36, 39, 41, 44, 46,
+	49, 52, 54, 57, 60, 62, 65, 67, 70, 73, 75, 78, 81, 83, 89, 94, 100,
+	105, 111, 116, 122, 127, 139, 150, 161, 173, 186, 196, 209, 220, 245,
+	270, 295, 321, 348, 375, 402, 430, 489, 550, 614, 681, 752, 903, 1071,
+	1257,
 };
 
 #define ARIZONA_CABLE_MECHANICAL 0
@@ -100,10 +120,63 @@ static const char *arizona_cable[] = {
 	NULL,
 };
 
+static void arizona_start_hpdet_acc_id(struct arizona_extcon_info *info);
+
+static void arizona_extcon_do_magic(struct arizona_extcon_info *info,
+				    unsigned int magic)
+{
+	struct arizona *arizona = info->arizona;
+	int ret;
+
+	mutex_lock(&arizona->dapm->card->dapm_mutex);
+
+	arizona->hpdet_magic = magic;
+
+	/* Keep the HP output stages disabled while doing the magic */
+	if (magic) {
+		ret = regmap_update_bits(arizona->regmap,
+					 ARIZONA_OUTPUT_ENABLES_1,
+					 ARIZONA_OUT1L_ENA |
+					 ARIZONA_OUT1R_ENA, 0);
+		if (ret != 0)
+			dev_warn(arizona->dev,
+				"Failed to disable headphone outputs: %d\n",
+				 ret);
+	}
+
+	ret = regmap_update_bits(arizona->regmap, 0x225, 0x4000,
+				 magic);
+	if (ret != 0)
+		dev_warn(arizona->dev, "Failed to do magic: %d\n",
+				 ret);
+
+	ret = regmap_update_bits(arizona->regmap, 0x226, 0x4000,
+				 magic);
+	if (ret != 0)
+		dev_warn(arizona->dev, "Failed to do magic: %d\n",
+			 ret);
+
+	/* Restore the desired state while not doing the magic */
+	if (!magic) {
+		ret = regmap_update_bits(arizona->regmap,
+					 ARIZONA_OUTPUT_ENABLES_1,
+					 ARIZONA_OUT1L_ENA |
+					 ARIZONA_OUT1R_ENA, arizona->hp_ena);
+		if (ret != 0)
+			dev_warn(arizona->dev,
+				 "Failed to restore headphone outputs: %d\n",
+				 ret);
+	}
+
+	mutex_unlock(&arizona->dapm->card->dapm_mutex);
+}
+
 static void arizona_extcon_set_mode(struct arizona_extcon_info *info, int mode)
 {
 	struct arizona *arizona = info->arizona;
 
+	mode %= info->micd_num_modes;
+
 	if (arizona->pdata.micd_pol_gpio > 0)
 		gpio_set_value_cansleep(arizona->pdata.micd_pol_gpio,
 					info->micd_modes[mode].gpio);
@@ -330,7 +403,7 @@ static int arizona_hpdet_read(struct arizona_extcon_info *info)
 		/* If we go out of range report top of range */
 		if (val < 100 || val > 0x3fb) {
 			dev_dbg(arizona->dev, "Measurement out of range\n");
-			return 10000;
+			return ARIZONA_HPDET_MAX;
 		}
 
 		dev_dbg(arizona->dev, "HPDET read %d in range %d\n",
@@ -391,7 +464,8 @@ static int arizona_hpdet_read(struct arizona_extcon_info *info)
 	return val;
 }
 
-static int arizona_hpdet_do_id(struct arizona_extcon_info *info, int *reading)
+static int arizona_hpdet_do_id(struct arizona_extcon_info *info, int *reading,
+			       bool *mic)
 {
 	struct arizona *arizona = info->arizona;
 	int id_gpio = arizona->pdata.hpdet_id_gpio;
@@ -403,32 +477,8 @@ static int arizona_hpdet_do_id(struct arizona_extcon_info *info, int *reading)
 	if (arizona->pdata.hpdet_acc_id) {
 		info->hpdet_res[info->num_hpdet_res++] = *reading;
 
-		/*
-		 * If the impedence is too high don't measure the
-		 * second ground.
-		 */
-		if (info->num_hpdet_res == 1 && *reading >= 45) {
-			dev_dbg(arizona->dev, "Skipping ground flip\n");
-			info->hpdet_res[info->num_hpdet_res++] = *reading;
-		}
-
-		if (info->num_hpdet_res == 1) {
-			dev_dbg(arizona->dev, "Flipping ground\n");
-
-			regmap_update_bits(arizona->regmap,
-					   ARIZONA_ACCESSORY_DETECT_MODE_1,
-					   ARIZONA_ACCDET_SRC,
-					   ~info->micd_modes[0].src);
-
-			regmap_update_bits(arizona->regmap,
-					   ARIZONA_HEADPHONE_DETECT_1,
-					   ARIZONA_HP_POLL, ARIZONA_HP_POLL);
-			return -EAGAIN;
-		}
-
 		/* Only check the mic directly if we didn't already ID it */
-		if (id_gpio && info->num_hpdet_res == 2 &&
-		    !((info->hpdet_res[0] > info->hpdet_res[1] * 2))) {
+		if (id_gpio && info->num_hpdet_res == 1) {
 			dev_dbg(arizona->dev, "Measuring mic\n");
 
 			regmap_update_bits(arizona->regmap,
@@ -447,22 +497,28 @@ static int arizona_hpdet_do_id(struct arizona_extcon_info *info, int *reading)
 		}
 
 		/* OK, got both.  Now, compare... */
-		dev_dbg(arizona->dev, "HPDET measured %d %d %d\n",
-			info->hpdet_res[0], info->hpdet_res[1],
-			info->hpdet_res[2]);
-
+		dev_dbg(arizona->dev, "HPDET measured %d %d\n",
+			info->hpdet_res[0], info->hpdet_res[1]);
 
 		/* Take the headphone impedance for the main report */
 		*reading = info->hpdet_res[0];
 
+		/* Sometimes we get false readings due to slow insert */
+		if (*reading >= ARIZONA_HPDET_MAX && !info->hpdet_retried) {
+			dev_dbg(arizona->dev, "Retrying high impedance\n");
+			info->num_hpdet_res = 0;
+			info->hpdet_retried = true;
+			arizona_start_hpdet_acc_id(info);
+			pm_runtime_put(info->dev);
+			return -EAGAIN;
+		}
+
 		/*
-		 * Either the two grounds measure differently or we
-		 * measure the mic as high impedance.
+		 * If we measure the mic as 
 		 */
-		if ((info->hpdet_res[0] > info->hpdet_res[1] * 2) ||
-		    (id_gpio && info->hpdet_res[2] > 10)) {
+		if (!id_gpio || info->hpdet_res[1] > 50) {
 			dev_dbg(arizona->dev, "Detected mic\n");
-			info->mic = true;
+			*mic = true;
 			info->detecting = true;
 		} else {
 			dev_dbg(arizona->dev, "Detected headphone\n");
@@ -484,8 +540,8 @@ static irqreturn_t arizona_hpdet_irq(int irq, void *data)
 	struct arizona *arizona = info->arizona;
 	int id_gpio = arizona->pdata.hpdet_id_gpio;
 	int report = ARIZONA_CABLE_HEADPHONE;
-	unsigned int val;
 	int ret, reading;
+	bool mic = false;
 
 	mutex_lock(&info->lock);
 
@@ -521,7 +577,7 @@ static irqreturn_t arizona_hpdet_irq(int irq, void *data)
 			   ARIZONA_HP_IMPEDANCE_RANGE_MASK | ARIZONA_HP_POLL,
 			   0);
 
-	ret = arizona_hpdet_do_id(info, &reading);
+	ret = arizona_hpdet_do_id(info, &reading, &mic);
 	if (ret == -EAGAIN) {
 		goto out;
 	} else if (ret < 0) {
@@ -539,28 +595,7 @@ static irqreturn_t arizona_hpdet_irq(int irq, void *data)
 		dev_err(arizona->dev, "Failed to report HP/line: %d\n",
 			ret);
 
-	mutex_lock(&arizona->dapm->card->dapm_mutex);
-
-	ret = regmap_read(arizona->regmap, ARIZONA_OUTPUT_ENABLES_1, &val);
-	if (ret != 0) {
-		dev_err(arizona->dev, "Failed to read output enables: %d\n",
-			ret);
-		val = 0;
-	}
-
-	if (!(val & (ARIZONA_OUT1L_ENA | ARIZONA_OUT1R_ENA))) {
-		ret = regmap_update_bits(arizona->regmap, 0x225, 0x4000, 0);
-		if (ret != 0)
-			dev_warn(arizona->dev, "Failed to undo magic: %d\n",
-				 ret);
-
-		ret = regmap_update_bits(arizona->regmap, 0x226, 0x4000, 0);
-		if (ret != 0)
-			dev_warn(arizona->dev, "Failed to undo magic: %d\n",
-				 ret);
-	}
-
-	mutex_unlock(&arizona->dapm->card->dapm_mutex);
+	arizona_extcon_do_magic(info, 0);
 
 done:
 	if (id_gpio)
@@ -572,7 +607,7 @@ done:
 			   ARIZONA_ACCDET_MODE_MASK, ARIZONA_ACCDET_MODE_MIC);
 
 	/* If we have a mic then reenable MICDET */
-	if (info->mic)
+	if (mic || info->mic)
 		arizona_start_mic(info);
 
 	if (info->hpdet_active) {
@@ -606,13 +641,7 @@ static void arizona_identify_headphone(struct arizona_extcon_info *info)
 	if (info->mic)
 		arizona_stop_mic(info);
 
-	ret = regmap_update_bits(arizona->regmap, 0x225, 0x4000, 0x4000);
-	if (ret != 0)
-		dev_warn(arizona->dev, "Failed to do magic: %d\n", ret);
-
-	ret = regmap_update_bits(arizona->regmap, 0x226, 0x4000, 0x4000);
-	if (ret != 0)
-		dev_warn(arizona->dev, "Failed to do magic: %d\n", ret);
+	arizona_extcon_do_magic(info, 0x4000);
 
 	ret = regmap_update_bits(arizona->regmap,
 				 ARIZONA_ACCESSORY_DETECT_MODE_1,
@@ -653,7 +682,8 @@ err:
 static void arizona_start_hpdet_acc_id(struct arizona_extcon_info *info)
 {
 	struct arizona *arizona = info->arizona;
-	unsigned int val;
+	int hp_reading = 32;
+	bool mic;
 	int ret;
 
 	dev_dbg(arizona->dev, "Starting identification via HPDET\n");
@@ -663,32 +693,7 @@ static void arizona_start_hpdet_acc_id(struct arizona_extcon_info *info)
 
 	info->hpdet_active = true;
 
-	arizona_extcon_pulse_micbias(info);
-
-	mutex_lock(&arizona->dapm->card->dapm_mutex);
-
-	ret = regmap_read(arizona->regmap, ARIZONA_OUTPUT_ENABLES_1, &val);
-	if (ret != 0) {
-		dev_err(arizona->dev, "Failed to read output enables: %d\n",
-			ret);
-		val = 0;
-	}
-
-	if (!(val & (ARIZONA_OUT1L_ENA | ARIZONA_OUT1R_ENA))) {
-		ret = regmap_update_bits(arizona->regmap, 0x225, 0x4000,
-					 0x4000);
-		if (ret != 0)
-			dev_warn(arizona->dev, "Failed to do magic: %d\n",
-				 ret);
-
-		ret = regmap_update_bits(arizona->regmap, 0x226, 0x4000,
-					 0x4000);
-		if (ret != 0)
-			dev_warn(arizona->dev, "Failed to do magic: %d\n",
-				 ret);
-	}
-
-	mutex_unlock(&arizona->dapm->card->dapm_mutex);
+	arizona_extcon_do_magic(info, 0x4000);
 
 	ret = regmap_update_bits(arizona->regmap,
 				 ARIZONA_ACCESSORY_DETECT_MODE_1,
@@ -700,12 +705,18 @@ static void arizona_start_hpdet_acc_id(struct arizona_extcon_info *info)
 		goto err;
 	}
 
-	ret = regmap_update_bits(arizona->regmap, ARIZONA_HEADPHONE_DETECT_1,
-				 ARIZONA_HP_POLL, ARIZONA_HP_POLL);
-	if (ret != 0) {
-		dev_err(arizona->dev, "Can't start HPDETL measurement: %d\n",
-			ret);
-		goto err;
+	if (arizona->pdata.hpdet_acc_id_line) {
+		ret = regmap_update_bits(arizona->regmap,
+					 ARIZONA_HEADPHONE_DETECT_1,
+					 ARIZONA_HP_POLL, ARIZONA_HP_POLL);
+		if (ret != 0) {
+			dev_err(arizona->dev,
+				"Can't start HPDETL measurement: %d\n",
+				ret);
+			goto err;
+		}
+	} else {
+		arizona_hpdet_do_id(info, &hp_reading, &mic);
 	}
 
 	return;
@@ -724,28 +735,58 @@ err:
 	info->hpdet_active = false;
 }
 
-static irqreturn_t arizona_micdet(int irq, void *data)
+static void arizona_micd_timeout_work(struct work_struct *work)
 {
-	struct arizona_extcon_info *info = data;
+	struct arizona_extcon_info *info = container_of(work,
+							struct arizona_extcon_info,
+							micd_timeout_work.work);
+
+	mutex_lock(&info->lock);
+
+	dev_dbg(info->arizona->dev, "MICD timed out, reporting HP\n");
+	arizona_identify_headphone(info);
+
+	info->detecting = false;
+
+	arizona_stop_mic(info);
+
+	mutex_unlock(&info->lock);
+}
+
+static void arizona_micd_detect(struct work_struct *work)
+{
+	struct arizona_extcon_info *info = container_of(work,
+							struct arizona_extcon_info,
+							micd_detect_work.work);
 	struct arizona *arizona = info->arizona;
-	unsigned int val, lvl;
-	int ret, i;
+	unsigned int val = 0, lvl;
+	int ret, i, key;
+
+	cancel_delayed_work_sync(&info->micd_timeout_work);
 
 	mutex_lock(&info->lock);
 
-	ret = regmap_read(arizona->regmap, ARIZONA_MIC_DETECT_3, &val);
-	if (ret != 0) {
-		dev_err(arizona->dev, "Failed to read MICDET: %d\n", ret);
-		mutex_unlock(&info->lock);
-		return IRQ_NONE;
-	}
+	for (i = 0; i < 10 && !(val & 0x7fc); i++) {
+		ret = regmap_read(arizona->regmap, ARIZONA_MIC_DETECT_3, &val);
+		if (ret != 0) {
+			dev_err(arizona->dev, "Failed to read MICDET: %d\n", ret);
+			mutex_unlock(&info->lock);
+			return;
+		}
 
-	dev_dbg(arizona->dev, "MICDET: %x\n", val);
+		dev_dbg(arizona->dev, "MICDET: %x\n", val);
+
+		if (!(val & ARIZONA_MICD_VALID)) {
+			dev_warn(arizona->dev, "Microphone detection state invalid\n");
+			mutex_unlock(&info->lock);
+			return;
+		}
+	}
 
-	if (!(val & ARIZONA_MICD_VALID)) {
-		dev_warn(arizona->dev, "Microphone detection state invalid\n");
+	if (i == 10 && !(val & 0x7fc)) {
+		dev_err(arizona->dev, "Failed to get valid MICDET value\n");
 		mutex_unlock(&info->lock);
-		return IRQ_NONE;
+		return;
 	}
 
 	/* Due to jack detect this should never happen */
@@ -786,7 +827,7 @@ static irqreturn_t arizona_micdet(int irq, void *data)
 	 * impedence then give up and report headphones.
 	 */
 	if (info->detecting && (val & 0x3f8)) {
-		if (info->jack_flips >= info->micd_num_modes) {
+		if (info->jack_flips >= info->micd_num_modes * 10) {
 			dev_dbg(arizona->dev, "Detected HP/line\n");
 			arizona_identify_headphone(info);
 
@@ -816,12 +857,17 @@ static irqreturn_t arizona_micdet(int irq, void *data)
 			lvl = val & ARIZONA_MICD_LVL_MASK;
 			lvl >>= ARIZONA_MICD_LVL_SHIFT;
 
-			for (i = 0; i < ARIZONA_NUM_BUTTONS; i++)
-				if (lvl & arizona_lvl_to_key[i].status)
-					input_report_key(info->input,
-							 arizona_lvl_to_key[i].report,
-							 1);
-			input_sync(info->input);
+			for (i = 0; i < info->num_micd_ranges; i++)
+				input_report_key(info->input,
+						 info->micd_ranges[i].key, 0);
+
+			WARN_ON(!lvl);
+			WARN_ON(ffs(lvl) - 1 >= info->num_micd_ranges);
+			if (lvl && ffs(lvl) - 1 < info->num_micd_ranges) {
+				key = info->micd_ranges[ffs(lvl) - 1].key;
+				input_report_key(info->input, key, 1);
+				input_sync(info->input);
+			}
 
 		} else if (info->detecting) {
 			dev_dbg(arizona->dev, "Headphone detected\n");
@@ -835,16 +881,41 @@ static irqreturn_t arizona_micdet(int irq, void *data)
 		}
 	} else {
 		dev_dbg(arizona->dev, "Mic button released\n");
-		for (i = 0; i < ARIZONA_NUM_BUTTONS; i++)
+		for (i = 0; i < info->num_micd_ranges; i++)
 			input_report_key(info->input,
-					 arizona_lvl_to_key[i].report, 0);
+					 info->micd_ranges[i].key, 0);
 		input_sync(info->input);
 		arizona_extcon_pulse_micbias(info);
 	}
 
 handled:
+	if (info->detecting)
+		schedule_delayed_work(&info->micd_timeout_work,
+				      msecs_to_jiffies(info->micd_timeout));
+
 	pm_runtime_mark_last_busy(info->dev);
 	mutex_unlock(&info->lock);
+}
+
+static irqreturn_t arizona_micdet(int irq, void *data)
+{
+	struct arizona_extcon_info *info = data;
+	struct arizona *arizona = info->arizona;
+	int debounce = arizona->pdata.micd_detect_debounce;
+
+	cancel_delayed_work_sync(&info->micd_detect_work);
+	cancel_delayed_work_sync(&info->micd_timeout_work);
+
+	mutex_lock(&info->lock);
+	if (!info->detecting)
+		debounce = 0;
+	mutex_unlock(&info->lock);
+
+	if (debounce)
+		schedule_delayed_work(&info->micd_detect_work,
+				      msecs_to_jiffies(debounce));
+	else
+		arizona_micd_detect(&info->micd_detect_work.work);
 
 	return IRQ_HANDLED;
 }
@@ -865,11 +936,13 @@ static irqreturn_t arizona_jackdet(int irq, void *data)
 	struct arizona_extcon_info *info = data;
 	struct arizona *arizona = info->arizona;
 	unsigned int val, present, mask;
+	bool cancelled_hp, cancelled_mic;
 	int ret, i;
 
-	pm_runtime_get_sync(info->dev);
+	cancelled_hp = cancel_delayed_work_sync(&info->hpdet_work);
+	cancelled_mic = cancel_delayed_work_sync(&info->micd_timeout_work);
 
-	cancel_delayed_work_sync(&info->hpdet_work);
+	pm_runtime_get_sync(info->dev);
 
 	mutex_lock(&info->lock);
 
@@ -890,7 +963,22 @@ static irqreturn_t arizona_jackdet(int irq, void *data)
 		return IRQ_NONE;
 	}
 
-	if ((val & mask) == present) {
+	val &= mask;
+	if (val == info->last_jackdet) {
+		dev_dbg(arizona->dev, "Suppressing duplicate JACKDET\n");
+		if (cancelled_hp)
+			schedule_delayed_work(&info->hpdet_work,
+					      msecs_to_jiffies(HPDET_DEBOUNCE));
+
+		if (cancelled_mic)
+			schedule_delayed_work(&info->micd_timeout_work,
+					      msecs_to_jiffies(info->micd_timeout));
+
+		goto out;
+	}
+	info->last_jackdet = val;
+
+	if (info->last_jackdet == present) {
 		dev_dbg(arizona->dev, "Detected jack\n");
 		ret = extcon_set_cable_state_(&info->edev,
 					      ARIZONA_CABLE_MECHANICAL, true);
@@ -907,7 +995,7 @@ static irqreturn_t arizona_jackdet(int irq, void *data)
 			arizona_start_mic(info);
 		} else {
 			schedule_delayed_work(&info->hpdet_work,
-					      msecs_to_jiffies(250));
+					      msecs_to_jiffies(HPDET_DEBOUNCE));
 		}
 
 		regmap_update_bits(arizona->regmap,
@@ -923,10 +1011,11 @@ static irqreturn_t arizona_jackdet(int irq, void *data)
 			info->hpdet_res[i] = 0;
 		info->mic = false;
 		info->hpdet_done = false;
+		info->hpdet_retried = false;
 
-		for (i = 0; i < ARIZONA_NUM_BUTTONS; i++)
+		for (i = 0; i < info->num_micd_ranges; i++)
 			input_report_key(info->input,
-					 arizona_lvl_to_key[i].report, 0);
+					 info->micd_ranges[i].key, 0);
 		input_sync(info->input);
 
 		ret = extcon_update_state(&info->edev, 0xffffffff, 0);
@@ -940,6 +1029,11 @@ static irqreturn_t arizona_jackdet(int irq, void *data)
 				   ARIZONA_MICD_CLAMP_DB | ARIZONA_JD1_DB);
 	}
 
+	if (arizona->pdata.micd_timeout)
+		info->micd_timeout = arizona->pdata.micd_timeout;
+	else
+		info->micd_timeout = DEFAULT_MICD_TIMEOUT;
+
 	/* Clear trig_sts to make sure DCVDD is not forced up */
 	regmap_write(arizona->regmap, ARIZONA_AOD_WKUP_AND_TRIG,
 		     ARIZONA_MICD_CLAMP_FALL_TRIG_STS |
@@ -947,6 +1041,7 @@ static irqreturn_t arizona_jackdet(int irq, void *data)
 		     ARIZONA_JD1_FALL_TRIG_STS |
 		     ARIZONA_JD1_RISE_TRIG_STS);
 
+out:
 	mutex_unlock(&info->lock);
 
 	pm_runtime_mark_last_busy(info->dev);
@@ -955,13 +1050,34 @@ static irqreturn_t arizona_jackdet(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
+/* Map a level onto a slot in the register bank */
+static void arizona_micd_set_level(struct arizona *arizona, int index,
+				   unsigned int level)
+{
+	int reg;
+	unsigned int mask;
+
+	reg = ARIZONA_MIC_DETECT_LEVEL_4 - (index / 2);
+
+	if (!(index % 2)) {
+		mask = 0x3f00;
+		level <<= 8;
+	} else {
+		mask = 0x3f;
+	}
+
+	/* Program the level itself */
+	regmap_update_bits(arizona->regmap, reg, mask, level);
+}
+
 static int arizona_extcon_probe(struct platform_device *pdev)
 {
 	struct arizona *arizona = dev_get_drvdata(pdev->dev.parent);
 	struct arizona_pdata *pdata;
 	struct arizona_extcon_info *info;
+	unsigned int val;
 	int jack_irq_fall, jack_irq_rise;
-	int ret, mode, i;
+	int ret, mode, i, j;
 
 	if (!arizona->dapm || !arizona->dapm->card)
 		return -EPROBE_DEFER;
@@ -985,7 +1101,10 @@ static int arizona_extcon_probe(struct platform_device *pdev)
 	mutex_init(&info->lock);
 	info->arizona = arizona;
 	info->dev = &pdev->dev;
+	info->last_jackdet = ~(ARIZONA_MICD_CLAMP_STS | ARIZONA_JD1_STS);
 	INIT_DELAYED_WORK(&info->hpdet_work, arizona_hpdet_work);
+	INIT_DELAYED_WORK(&info->micd_detect_work, arizona_micd_detect);
+	INIT_DELAYED_WORK(&info->micd_timeout_work, arizona_micd_timeout_work);
 	platform_set_drvdata(pdev, info);
 
 	switch (arizona->type) {
@@ -1014,6 +1133,17 @@ static int arizona_extcon_probe(struct platform_device *pdev)
 		goto err;
 	}
 
+	info->input = devm_input_allocate_device(&pdev->dev);
+	if (!info->input) {
+		dev_err(arizona->dev, "Can't allocate input dev\n");
+		ret = -ENOMEM;
+		goto err_register;
+	}
+
+	info->input->name = "Headset";
+	info->input->phys = "arizona/extcon";
+	info->input->dev.parent = &pdev->dev;
+
 	if (pdata->num_micd_configs) {
 		info->micd_modes = pdata->micd_configs;
 		info->micd_num_modes = pdata->num_micd_configs;
@@ -1069,15 +1199,79 @@ static int arizona_extcon_probe(struct platform_device *pdev)
 				   arizona->pdata.micd_dbtime
 				   << ARIZONA_MICD_DBTIME_SHIFT);
 
+	BUILD_BUG_ON(ARRAY_SIZE(arizona_micd_levels) != 0x40);
+
+	if (arizona->pdata.num_micd_ranges) {
+		info->micd_ranges = pdata->micd_ranges;
+		info->num_micd_ranges = pdata->num_micd_ranges;
+	} else {
+		info->micd_ranges = micd_default_ranges;
+		info->num_micd_ranges = ARRAY_SIZE(micd_default_ranges);
+	}
+
+	if (arizona->pdata.num_micd_ranges > ARIZONA_MAX_MICD_RANGE) {
+		dev_err(arizona->dev, "Too many MICD ranges: %d\n",
+			arizona->pdata.num_micd_ranges);
+	}
+
+	if (info->num_micd_ranges > 1) {
+		for (i = 1; i < info->num_micd_ranges; i++) {
+			if (info->micd_ranges[i - 1].max >
+			    info->micd_ranges[i].max) {
+				dev_err(arizona->dev,
+					"MICD ranges must be sorted\n");
+				ret = -EINVAL;
+				goto err_input;
+			}
+		}
+	}
+
+	/* Disable all buttons by default */
+	regmap_update_bits(arizona->regmap, ARIZONA_MIC_DETECT_2,
+			   ARIZONA_MICD_LVL_SEL_MASK, 0x81);
+
+	/* Set up all the buttons the user specified */
+	for (i = 0; i < info->num_micd_ranges; i++) {
+		for (j = 0; j < ARRAY_SIZE(arizona_micd_levels); j++)
+			if (arizona_micd_levels[j] >= info->micd_ranges[i].max)
+				break;
+
+		if (j == ARRAY_SIZE(arizona_micd_levels)) {
+			dev_err(arizona->dev, "Unsupported MICD level %d\n",
+				info->micd_ranges[i].max);
+			ret = -EINVAL;
+			goto err_input;
+		}
+
+		dev_dbg(arizona->dev, "%d ohms for MICD threshold %d\n",
+			arizona_micd_levels[j], i);
+
+		arizona_micd_set_level(arizona, i, j);
+		input_set_capability(info->input, EV_KEY,
+				     info->micd_ranges[i].key);
+
+		/* Enable reporting of that range */
+		regmap_update_bits(arizona->regmap, ARIZONA_MIC_DETECT_2,
+				   1 << i, 1 << i);
+	}
+
+	/* Set all the remaining keys to a maximum */
+	for (; i < ARIZONA_MAX_MICD_RANGE; i++)
+		arizona_micd_set_level(arizona, i, 0x3f);
+
 	/*
 	 * If we have a clamp use it, activating in conjunction with
 	 * GPIO5 if that is connected for jack detect operation.
 	 */
 	if (info->micd_clamp) {
 		if (arizona->pdata.jd_gpio5) {
-			/* Put the GPIO into input mode */
+			/* Put the GPIO into input mode with optional pull */
+			val = 0xc101;
+			if (arizona->pdata.jd_gpio5_nopull)
+				val &= ~ARIZONA_GPN_PU;
+
 			regmap_write(arizona->regmap, ARIZONA_GPIO5_CTRL,
-				     0xc101);
+				     val);
 
 			regmap_update_bits(arizona->regmap,
 					   ARIZONA_MICD_CLAMP_CONTROL,
@@ -1096,20 +1290,6 @@ static int arizona_extcon_probe(struct platform_device *pdev)
 
 	arizona_extcon_set_mode(info, 0);
 
-	info->input = devm_input_allocate_device(&pdev->dev);
-	if (!info->input) {
-		dev_err(arizona->dev, "Can't allocate input dev\n");
-		ret = -ENOMEM;
-		goto err_register;
-	}
-
-	for (i = 0; i < ARIZONA_NUM_BUTTONS; i++)
-		input_set_capability(info->input, EV_KEY,
-				     arizona_lvl_to_key[i].report);
-	info->input->name = "Headset";
-	info->input->phys = "arizona/extcon";
-	info->input->dev.parent = &pdev->dev;
-
 	pm_runtime_enable(&pdev->dev);
 	pm_runtime_idle(&pdev->dev);
 	pm_runtime_get_sync(&pdev->dev);
diff --git a/drivers/extcon/extcon-max77693.c b/drivers/extcon/extcon-max77693.c
index b70e3815c45..b56bdaa27d4 100644
--- a/drivers/extcon/extcon-max77693.c
+++ b/drivers/extcon/extcon-max77693.c
@@ -32,6 +32,38 @@
 #define	DEV_NAME			"max77693-muic"
 #define	DELAY_MS_DEFAULT		20000		/* unit: millisecond */
 
+/*
+ * Default value of MAX77693 register to bring up MUIC device.
+ * If user don't set some initial value for MUIC device through platform data,
+ * extcon-max77693 driver use 'default_init_data' to bring up base operation
+ * of MAX77693 MUIC device.
+ */
+static struct max77693_reg_data default_init_data[] = {
+	{
+		/* STATUS2 - [3]ChgDetRun */
+		.addr = MAX77693_MUIC_REG_STATUS2,
+		.data = STATUS2_CHGDETRUN_MASK,
+	}, {
+		/* INTMASK1 - Unmask [3]ADC1KM,[0]ADCM */
+		.addr = MAX77693_MUIC_REG_INTMASK1,
+		.data = INTMASK1_ADC1K_MASK
+			| INTMASK1_ADC_MASK,
+	}, {
+		/* INTMASK2 - Unmask [0]ChgTypM */
+		.addr = MAX77693_MUIC_REG_INTMASK2,
+		.data = INTMASK2_CHGTYP_MASK,
+	}, {
+		/* INTMASK3 - Mask all of interrupts */
+		.addr = MAX77693_MUIC_REG_INTMASK3,
+		.data = 0x0,
+	}, {
+		/* CDETCTRL2 */
+		.addr = MAX77693_MUIC_REG_CDETCTRL2,
+		.data = CDETCTRL2_VIDRMEN_MASK
+			| CDETCTRL2_DXOVPEN_MASK,
+	},
+};
+
 enum max77693_muic_adc_debounce_time {
 	ADC_DEBOUNCE_TIME_5MS = 0,
 	ADC_DEBOUNCE_TIME_10MS,
@@ -226,7 +258,7 @@ static int max77693_muic_set_debounce_time(struct max77693_muic_info *info,
 					  CONTROL3_ADCDBSET_MASK);
 		if (ret) {
 			dev_err(info->dev, "failed to set ADC debounce time\n");
-			return -EAGAIN;
+			return ret;
 		}
 		break;
 	default:
@@ -262,7 +294,7 @@ static int max77693_muic_set_path(struct max77693_muic_info *info,
 			MAX77693_MUIC_REG_CTRL1, ctrl1, COMP_SW_MASK);
 	if (ret < 0) {
 		dev_err(info->dev, "failed to update MUIC register\n");
-		return -EAGAIN;
+		return ret;
 	}
 
 	if (attached)
@@ -275,7 +307,7 @@ static int max77693_muic_set_path(struct max77693_muic_info *info,
 			CONTROL2_LOWPWR_MASK | CONTROL2_CPEN_MASK);
 	if (ret < 0) {
 		dev_err(info->dev, "failed to update MUIC register\n");
-		return -EAGAIN;
+		return ret;
 	}
 
 	dev_info(info->dev,
@@ -1003,7 +1035,7 @@ static int max77693_muic_detect_accessory(struct max77693_muic_info *info)
 	if (ret) {
 		dev_err(info->dev, "failed to read MUIC register\n");
 		mutex_unlock(&info->mutex);
-		return -EINVAL;
+		return ret;
 	}
 
 	adc = max77693_muic_get_cable_type(info, MAX77693_CABLE_GROUP_ADC,
@@ -1045,8 +1077,9 @@ static int max77693_muic_probe(struct platform_device *pdev)
 {
 	struct max77693_dev *max77693 = dev_get_drvdata(pdev->dev.parent);
 	struct max77693_platform_data *pdata = dev_get_platdata(max77693->dev);
-	struct max77693_muic_platform_data *muic_pdata = pdata->muic_data;
 	struct max77693_muic_info *info;
+	struct max77693_reg_data *init_data;
+	int num_init_data;
 	int delay_jiffies;
 	int ret;
 	int i;
@@ -1145,15 +1178,25 @@ static int max77693_muic_probe(struct platform_device *pdev)
 		goto err_irq;
 	}
 
-	/* Initialize MUIC register by using platform data */
-	for (i = 0 ; i < muic_pdata->num_init_data ; i++) {
-		enum max77693_irq_source irq_src = MAX77693_IRQ_GROUP_NR;
+
+	/* Initialize MUIC register by using platform data or default data */
+	if (pdata->muic_data) {
+		init_data = pdata->muic_data->init_data;
+		num_init_data = pdata->muic_data->num_init_data;
+	} else {
+		init_data = default_init_data;
+		num_init_data = ARRAY_SIZE(default_init_data);
+	}
+
+	for (i = 0 ; i < num_init_data ; i++) {
+		enum max77693_irq_source irq_src
+				= MAX77693_IRQ_GROUP_NR;
 
 		max77693_write_reg(info->max77693->regmap_muic,
-				muic_pdata->init_data[i].addr,
-				muic_pdata->init_data[i].data);
+				init_data[i].addr,
+				init_data[i].data);
 
-		switch (muic_pdata->init_data[i].addr) {
+		switch (init_data[i].addr) {
 		case MAX77693_MUIC_REG_INTMASK1:
 			irq_src = MUIC_INT1;
 			break;
@@ -1167,22 +1210,40 @@ static int max77693_muic_probe(struct platform_device *pdev)
 
 		if (irq_src < MAX77693_IRQ_GROUP_NR)
 			info->max77693->irq_masks_cur[irq_src]
-				= muic_pdata->init_data[i].data;
+				= init_data[i].data;
 	}
 
-	/*
-	 * Default usb/uart path whether UART/USB or AUX_UART/AUX_USB
-	 * h/w path of COMP2/COMN1 on CONTROL1 register.
-	 */
-	if (muic_pdata->path_uart)
-		info->path_uart = muic_pdata->path_uart;
-	else
-		info->path_uart = CONTROL1_SW_UART;
+	if (pdata->muic_data) {
+		struct max77693_muic_platform_data *muic_pdata = pdata->muic_data;
 
-	if (muic_pdata->path_usb)
-		info->path_usb = muic_pdata->path_usb;
-	else
+		/*
+		 * Default usb/uart path whether UART/USB or AUX_UART/AUX_USB
+		 * h/w path of COMP2/COMN1 on CONTROL1 register.
+		 */
+		if (muic_pdata->path_uart)
+			info->path_uart = muic_pdata->path_uart;
+		else
+			info->path_uart = CONTROL1_SW_UART;
+
+		if (muic_pdata->path_usb)
+			info->path_usb = muic_pdata->path_usb;
+		else
+			info->path_usb = CONTROL1_SW_USB;
+
+		/*
+		 * Default delay time for detecting cable state
+		 * after certain time.
+		 */
+		if (muic_pdata->detcable_delay_ms)
+			delay_jiffies =
+				msecs_to_jiffies(muic_pdata->detcable_delay_ms);
+		else
+			delay_jiffies = msecs_to_jiffies(DELAY_MS_DEFAULT);
+	} else {
 		info->path_usb = CONTROL1_SW_USB;
+		info->path_uart = CONTROL1_SW_UART;
+		delay_jiffies = msecs_to_jiffies(DELAY_MS_DEFAULT);
+	}
 
 	/* Set initial path for UART */
 	 max77693_muic_set_path(info, info->path_uart, true);
@@ -1208,10 +1269,6 @@ static int max77693_muic_probe(struct platform_device *pdev)
 	 * driver should notify cable state to upper layer.
 	 */
 	INIT_DELAYED_WORK(&info->wq_detcable, max77693_muic_detect_cable_wq);
-	if (muic_pdata->detcable_delay_ms)
-		delay_jiffies = msecs_to_jiffies(muic_pdata->detcable_delay_ms);
-	else
-		delay_jiffies = msecs_to_jiffies(DELAY_MS_DEFAULT);
 	schedule_delayed_work(&info->wq_detcable, delay_jiffies);
 
 	return ret;
diff --git a/drivers/extcon/extcon-max8997.c b/drivers/extcon/extcon-max8997.c
index e636d950ad6..67d6738d85a 100644
--- a/drivers/extcon/extcon-max8997.c
+++ b/drivers/extcon/extcon-max8997.c
@@ -196,7 +196,7 @@ static int max8997_muic_set_debounce_time(struct max8997_muic_info *info,
 					  CONTROL3_ADCDBSET_MASK);
 		if (ret) {
 			dev_err(info->dev, "failed to set ADC debounce time\n");
-			return -EAGAIN;
+			return ret;
 		}
 		break;
 	default:
@@ -232,7 +232,7 @@ static int max8997_muic_set_path(struct max8997_muic_info *info,
 			MAX8997_MUIC_REG_CONTROL1, ctrl1, COMP_SW_MASK);
 	if (ret < 0) {
 		dev_err(info->dev, "failed to update MUIC register\n");
-		return -EAGAIN;
+		return ret;
 	}
 
 	if (attached)
@@ -245,7 +245,7 @@ static int max8997_muic_set_path(struct max8997_muic_info *info,
 			CONTROL2_LOWPWR_MASK | CONTROL2_CPEN_MASK);
 	if (ret < 0) {
 		dev_err(info->dev, "failed to update MUIC register\n");
-		return -EAGAIN;
+		return ret;
 	}
 
 	dev_info(info->dev,
@@ -397,7 +397,7 @@ static int max8997_muic_handle_jig_uart(struct max8997_muic_info *info,
 	ret = max8997_muic_set_path(info, info->path_uart, attached);
 	if (ret) {
 		dev_err(info->dev, "failed to update muic register\n");
-		return -EINVAL;
+		return ret;
 	}
 
 	extcon_set_cable_state(info->edev, "JIG", attached);
@@ -608,7 +608,7 @@ static int max8997_muic_detect_dev(struct max8997_muic_info *info)
 	if (ret) {
 		dev_err(info->dev, "failed to read MUIC register\n");
 		mutex_unlock(&info->mutex);
-		return -EINVAL;
+		return ret;
 	}
 
 	adc = max8997_muic_get_cable_type(info, MAX8997_CABLE_GROUP_ADC,
@@ -646,7 +646,7 @@ static void max8997_muic_detect_cable_wq(struct work_struct *work)
 
 	ret = max8997_muic_detect_dev(info);
 	if (ret < 0)
-		pr_err("failed to detect cable type\n");
+		dev_err(info->dev, "failed to detect cable type\n");
 }
 
 static int max8997_muic_probe(struct platform_device *pdev)
@@ -712,29 +712,45 @@ static int max8997_muic_probe(struct platform_device *pdev)
 		goto err_irq;
 	}
 
-	/* Initialize registers according to platform data */
 	if (pdata->muic_pdata) {
-		struct max8997_muic_platform_data *mdata = info->muic_pdata;
-
-		for (i = 0; i < mdata->num_init_data; i++) {
-			max8997_write_reg(info->muic, mdata->init_data[i].addr,
-					mdata->init_data[i].data);
+		struct max8997_muic_platform_data *muic_pdata
+			= pdata->muic_pdata;
+
+		/* Initialize registers according to platform data */
+		for (i = 0; i < muic_pdata->num_init_data; i++) {
+			max8997_write_reg(info->muic,
+					muic_pdata->init_data[i].addr,
+					muic_pdata->init_data[i].data);
 		}
-	}
 
-	/*
-	 * Default usb/uart path whether UART/USB or AUX_UART/AUX_USB
-	 * h/w path of COMP2/COMN1 on CONTROL1 register.
-	 */
-	if (pdata->muic_pdata->path_uart)
-		info->path_uart = pdata->muic_pdata->path_uart;
-	else
-		info->path_uart = CONTROL1_SW_UART;
+		/*
+		 * Default usb/uart path whether UART/USB or AUX_UART/AUX_USB
+		 * h/w path of COMP2/COMN1 on CONTROL1 register.
+		 */
+		if (muic_pdata->path_uart)
+			info->path_uart = muic_pdata->path_uart;
+		else
+			info->path_uart = CONTROL1_SW_UART;
 
-	if (pdata->muic_pdata->path_usb)
-		info->path_usb = pdata->muic_pdata->path_usb;
-	else
+		if (muic_pdata->path_usb)
+			info->path_usb = muic_pdata->path_usb;
+		else
+			info->path_usb = CONTROL1_SW_USB;
+
+		/*
+		 * Default delay time for detecting cable state
+		 * after certain time.
+		 */
+		if (muic_pdata->detcable_delay_ms)
+			delay_jiffies =
+				msecs_to_jiffies(muic_pdata->detcable_delay_ms);
+		else
+			delay_jiffies = msecs_to_jiffies(DELAY_MS_DEFAULT);
+	} else {
+		info->path_uart = CONTROL1_SW_UART;
 		info->path_usb = CONTROL1_SW_USB;
+		delay_jiffies = msecs_to_jiffies(DELAY_MS_DEFAULT);
+	}
 
 	/* Set initial path for UART */
 	 max8997_muic_set_path(info, info->path_uart, true);
@@ -751,10 +767,6 @@ static int max8997_muic_probe(struct platform_device *pdev)
 	 * driver should notify cable state to upper layer.
 	 */
 	INIT_DELAYED_WORK(&info->wq_detcable, max8997_muic_detect_cable_wq);
-	if (pdata->muic_pdata->detcable_delay_ms)
-		delay_jiffies = msecs_to_jiffies(pdata->muic_pdata->detcable_delay_ms);
-	else
-		delay_jiffies = msecs_to_jiffies(DELAY_MS_DEFAULT);
 	schedule_delayed_work(&info->wq_detcable, delay_jiffies);
 
 	return 0;
diff --git a/drivers/firmware/Kconfig b/drivers/firmware/Kconfig
index 9b00072a020..3e532002e4d 100644
--- a/drivers/firmware/Kconfig
+++ b/drivers/firmware/Kconfig
@@ -39,6 +39,7 @@ config FIRMWARE_MEMMAP
 config EFI_VARS
 	tristate "EFI Variable Support via sysfs"
 	depends on EFI
+	select UCS2_STRING
 	default n
 	help
 	  If you say Y here, you are able to get EFI (Extensible Firmware
@@ -53,6 +54,24 @@ config EFI_VARS
 	  Subsequent efibootmgr releases may be found at:
 	  <http://linux.dell.com/efibootmgr>
 
+config EFI_VARS_PSTORE
+	bool "Register efivars backend for pstore"
+	depends on EFI_VARS && PSTORE
+	default y
+	help
+	  Say Y here to enable use efivars as a backend to pstore. This
+	  will allow writing console messages, crash dumps, or anything
+	  else supported by pstore to EFI variables.
+
+config EFI_VARS_PSTORE_DEFAULT_DISABLE
+	bool "Disable using efivars as a pstore backend by default"
+	depends on EFI_VARS_PSTORE
+	default n
+	help
+	  Saying Y here will disable the use of efivars as a storage
+	  backend for pstore by default. This setting can be overridden
+	  using the efivars module's pstore_disable parameter.
+
 config EFI_PCDP
 	bool "Console device selection via EFI PCDP or HCDP table"
 	depends on ACPI && EFI && IA64
diff --git a/drivers/firmware/efivars.c b/drivers/firmware/efivars.c
index fe62aa39223..f4baa11d364 100644
--- a/drivers/firmware/efivars.c
+++ b/drivers/firmware/efivars.c
@@ -80,6 +80,7 @@
 #include <linux/slab.h>
 #include <linux/pstore.h>
 #include <linux/ctype.h>
+#include <linux/ucs2_string.h>
 
 #include <linux/fs.h>
 #include <linux/ramfs.h>
@@ -103,6 +104,11 @@ MODULE_VERSION(EFIVARS_VERSION);
  */
 #define GUID_LEN 36
 
+static bool efivars_pstore_disable =
+	IS_ENABLED(CONFIG_EFI_VARS_PSTORE_DEFAULT_DISABLE);
+
+module_param_named(pstore_disable, efivars_pstore_disable, bool, 0644);
+
 /*
  * The maximum size of VariableName + Data = 1024
  * Therefore, it's reasonable to save that much
@@ -165,51 +171,7 @@ efivar_create_sysfs_entry(struct efivars *efivars,
 
 static void efivar_update_sysfs_entries(struct work_struct *);
 static DECLARE_WORK(efivar_work, efivar_update_sysfs_entries);
-
-/* Return the number of unicode characters in data */
-static unsigned long
-utf16_strnlen(efi_char16_t *s, size_t maxlength)
-{
-	unsigned long length = 0;
-
-	while (*s++ != 0 && length < maxlength)
-		length++;
-	return length;
-}
-
-static inline unsigned long
-utf16_strlen(efi_char16_t *s)
-{
-	return utf16_strnlen(s, ~0UL);
-}
-
-/*
- * Return the number of bytes is the length of this string
- * Note: this is NOT the same as the number of unicode characters
- */
-static inline unsigned long
-utf16_strsize(efi_char16_t *data, unsigned long maxlength)
-{
-	return utf16_strnlen(data, maxlength/sizeof(efi_char16_t)) * sizeof(efi_char16_t);
-}
-
-static inline int
-utf16_strncmp(const efi_char16_t *a, const efi_char16_t *b, size_t len)
-{
-	while (1) {
-		if (len == 0)
-			return 0;
-		if (*a < *b)
-			return -1;
-		if (*a > *b)
-			return 1;
-		if (*a == 0) /* implies *b == 0 */
-			return 0;
-		a++;
-		b++;
-		len--;
-	}
-}
+static bool efivar_wq_enabled = true;
 
 static bool
 validate_device_path(struct efi_variable *var, int match, u8 *buffer,
@@ -262,7 +224,7 @@ validate_load_option(struct efi_variable *var, int match, u8 *buffer,
 	u16 filepathlength;
 	int i, desclength = 0, namelen;
 
-	namelen = utf16_strnlen(var->VariableName, sizeof(var->VariableName));
+	namelen = ucs2_strnlen(var->VariableName, sizeof(var->VariableName));
 
 	/* Either "Boot" or "Driver" followed by four digits of hex */
 	for (i = match; i < match+4; i++) {
@@ -285,7 +247,7 @@ validate_load_option(struct efi_variable *var, int match, u8 *buffer,
 	 * There's no stored length for the description, so it has to be
 	 * found by hand
 	 */
-	desclength = utf16_strsize((efi_char16_t *)(buffer + 6), len - 6) + 2;
+	desclength = ucs2_strsize((efi_char16_t *)(buffer + 6), len - 6) + 2;
 
 	/* Each boot entry must have a descriptor */
 	if (!desclength)
@@ -430,24 +392,12 @@ static efi_status_t
 check_var_size_locked(struct efivars *efivars, u32 attributes,
 			unsigned long size)
 {
-	u64 storage_size, remaining_size, max_size;
-	efi_status_t status;
 	const struct efivar_operations *fops = efivars->ops;
 
-	if (!efivars->ops->query_variable_info)
+	if (!efivars->ops->query_variable_store)
 		return EFI_UNSUPPORTED;
 
-	status = fops->query_variable_info(attributes, &storage_size,
-					   &remaining_size, &max_size);
-
-	if (status != EFI_SUCCESS)
-		return status;
-
-	if (!storage_size || size > remaining_size || size > max_size ||
-	    (remaining_size - size) < (storage_size / 2))
-		return EFI_OUT_OF_RESOURCES;
-
-	return status;
+	return fops->query_variable_store(attributes, size);
 }
 
 
@@ -587,7 +537,7 @@ efivar_store_raw(struct efivar_entry *entry, const char *buf, size_t count)
 	spin_lock_irq(&efivars->lock);
 
 	status = check_var_size_locked(efivars, new_var->Attributes,
-	       new_var->DataSize + utf16_strsize(new_var->VariableName, 1024));
+	       new_var->DataSize + ucs2_strsize(new_var->VariableName, 1024));
 
 	if (status == EFI_SUCCESS || status == EFI_UNSUPPORTED)
 		status = efivars->ops->set_variable(new_var->VariableName,
@@ -765,7 +715,7 @@ static ssize_t efivarfs_file_write(struct file *file,
 	 * QueryVariableInfo() isn't supported by the firmware.
 	 */
 
-	varsize = datasize + utf16_strsize(var->var.VariableName, 1024);
+	varsize = datasize + ucs2_strsize(var->var.VariableName, 1024);
 	status = check_var_size(efivars, attributes, varsize);
 
 	if (status != EFI_SUCCESS) {
@@ -1217,7 +1167,7 @@ static int efivarfs_fill_super(struct super_block *sb, void *data, int silent)
 
 		inode = NULL;
 
-		len = utf16_strlen(entry->var.VariableName);
+		len = ucs2_strlen(entry->var.VariableName);
 
 		/* name, plus '-', plus GUID, plus NUL*/
 		name = kmalloc(len + 1 + GUID_LEN + 1, GFP_ATOMIC);
@@ -1309,9 +1259,7 @@ static const struct inode_operations efivarfs_dir_inode_operations = {
 	.create = efivarfs_create,
 };
 
-static struct pstore_info efi_pstore_info;
-
-#ifdef CONFIG_PSTORE
+#ifdef CONFIG_EFI_VARS_PSTORE
 
 static int efi_pstore_open(struct pstore_info *psi)
 {
@@ -1441,7 +1389,7 @@ static int efi_pstore_write(enum pstore_type_id type,
 
 	spin_unlock_irqrestore(&efivars->lock, flags);
 
-	if (reason == KMSG_DUMP_OOPS)
+	if (reason == KMSG_DUMP_OOPS && efivar_wq_enabled)
 		schedule_work(&efivar_work);
 
 	*id = part;
@@ -1477,8 +1425,8 @@ static int efi_pstore_erase(enum pstore_type_id type, u64 id, int count,
 
 		if (efi_guidcmp(entry->var.VendorGuid, vendor))
 			continue;
-		if (utf16_strncmp(entry->var.VariableName, efi_name,
-				  utf16_strlen(efi_name))) {
+		if (ucs2_strncmp(entry->var.VariableName, efi_name,
+				  ucs2_strlen(efi_name))) {
 			/*
 			 * Check if an old format,
 			 * which doesn't support holding
@@ -1490,8 +1438,8 @@ static int efi_pstore_erase(enum pstore_type_id type, u64 id, int count,
 			for (i = 0; i < DUMP_NAME_LEN; i++)
 				efi_name_old[i] = name_old[i];
 
-			if (utf16_strncmp(entry->var.VariableName, efi_name_old,
-					  utf16_strlen(efi_name_old)))
+			if (ucs2_strncmp(entry->var.VariableName, efi_name_old,
+					  ucs2_strlen(efi_name_old)))
 				continue;
 		}
 
@@ -1514,38 +1462,6 @@ static int efi_pstore_erase(enum pstore_type_id type, u64 id, int count,
 
 	return 0;
 }
-#else
-static int efi_pstore_open(struct pstore_info *psi)
-{
-	return 0;
-}
-
-static int efi_pstore_close(struct pstore_info *psi)
-{
-	return 0;
-}
-
-static ssize_t efi_pstore_read(u64 *id, enum pstore_type_id *type, int *count,
-			       struct timespec *timespec,
-			       char **buf, struct pstore_info *psi)
-{
-	return -1;
-}
-
-static int efi_pstore_write(enum pstore_type_id type,
-		enum kmsg_dump_reason reason, u64 *id,
-		unsigned int part, int count, size_t size,
-		struct pstore_info *psi)
-{
-	return 0;
-}
-
-static int efi_pstore_erase(enum pstore_type_id type, u64 id, int count,
-			    struct timespec time, struct pstore_info *psi)
-{
-	return 0;
-}
-#endif
 
 static struct pstore_info efi_pstore_info = {
 	.owner		= THIS_MODULE,
@@ -1557,6 +1473,24 @@ static struct pstore_info efi_pstore_info = {
 	.erase		= efi_pstore_erase,
 };
 
+static void efivar_pstore_register(struct efivars *efivars)
+{
+	efivars->efi_pstore_info = efi_pstore_info;
+	efivars->efi_pstore_info.buf = kmalloc(4096, GFP_KERNEL);
+	if (efivars->efi_pstore_info.buf) {
+		efivars->efi_pstore_info.bufsize = 1024;
+		efivars->efi_pstore_info.data = efivars;
+		spin_lock_init(&efivars->efi_pstore_info.buf_lock);
+		pstore_register(&efivars->efi_pstore_info);
+	}
+}
+#else
+static void efivar_pstore_register(struct efivars *efivars)
+{
+	return;
+}
+#endif
+
 static ssize_t efivar_create(struct file *filp, struct kobject *kobj,
 			     struct bin_attribute *bin_attr,
 			     char *buf, loff_t pos, size_t count)
@@ -1583,8 +1517,8 @@ static ssize_t efivar_create(struct file *filp, struct kobject *kobj,
 	 * Does this variable already exist?
 	 */
 	list_for_each_entry_safe(search_efivar, n, &efivars->list, list) {
-		strsize1 = utf16_strsize(search_efivar->var.VariableName, 1024);
-		strsize2 = utf16_strsize(new_var->VariableName, 1024);
+		strsize1 = ucs2_strsize(search_efivar->var.VariableName, 1024);
+		strsize2 = ucs2_strsize(new_var->VariableName, 1024);
 		if (strsize1 == strsize2 &&
 			!memcmp(&(search_efivar->var.VariableName),
 				new_var->VariableName, strsize1) &&
@@ -1600,7 +1534,7 @@ static ssize_t efivar_create(struct file *filp, struct kobject *kobj,
 	}
 
 	status = check_var_size_locked(efivars, new_var->Attributes,
-	       new_var->DataSize + utf16_strsize(new_var->VariableName, 1024));
+	       new_var->DataSize + ucs2_strsize(new_var->VariableName, 1024));
 
 	if (status && status != EFI_UNSUPPORTED) {
 		spin_unlock_irq(&efivars->lock);
@@ -1624,7 +1558,7 @@ static ssize_t efivar_create(struct file *filp, struct kobject *kobj,
 
 	/* Create the entry in sysfs.  Locking is not required here */
 	status = efivar_create_sysfs_entry(efivars,
-					   utf16_strsize(new_var->VariableName,
+					   ucs2_strsize(new_var->VariableName,
 							 1024),
 					   new_var->VariableName,
 					   &new_var->VendorGuid);
@@ -1654,8 +1588,8 @@ static ssize_t efivar_delete(struct file *filp, struct kobject *kobj,
 	 * Does this variable already exist?
 	 */
 	list_for_each_entry_safe(search_efivar, n, &efivars->list, list) {
-		strsize1 = utf16_strsize(search_efivar->var.VariableName, 1024);
-		strsize2 = utf16_strsize(del_var->VariableName, 1024);
+		strsize1 = ucs2_strsize(search_efivar->var.VariableName, 1024);
+		strsize2 = ucs2_strsize(del_var->VariableName, 1024);
 		if (strsize1 == strsize2 &&
 			!memcmp(&(search_efivar->var.VariableName),
 				del_var->VariableName, strsize1) &&
@@ -1694,16 +1628,17 @@ static ssize_t efivar_delete(struct file *filp, struct kobject *kobj,
 	return count;
 }
 
-static bool variable_is_present(efi_char16_t *variable_name, efi_guid_t *vendor)
+static bool variable_is_present(struct efivars *efivars,
+				efi_char16_t *variable_name,
+				efi_guid_t *vendor)
 {
 	struct efivar_entry *entry, *n;
-	struct efivars *efivars = &__efivars;
 	unsigned long strsize1, strsize2;
 	bool found = false;
 
-	strsize1 = utf16_strsize(variable_name, 1024);
+	strsize1 = ucs2_strsize(variable_name, 1024);
 	list_for_each_entry_safe(entry, n, &efivars->list, list) {
-		strsize2 = utf16_strsize(entry->var.VariableName, 1024);
+		strsize2 = ucs2_strsize(entry->var.VariableName, 1024);
 		if (strsize1 == strsize2 &&
 			!memcmp(variable_name, &(entry->var.VariableName),
 				strsize2) &&
@@ -1716,6 +1651,31 @@ static bool variable_is_present(efi_char16_t *variable_name, efi_guid_t *vendor)
 	return found;
 }
 
+/*
+ * Returns the size of variable_name, in bytes, including the
+ * terminating NULL character, or variable_name_size if no NULL
+ * character is found among the first variable_name_size bytes.
+ */
+static unsigned long var_name_strnsize(efi_char16_t *variable_name,
+				       unsigned long variable_name_size)
+{
+	unsigned long len;
+	efi_char16_t c;
+
+	/*
+	 * The variable name is, by definition, a NULL-terminated
+	 * string, so make absolutely sure that variable_name_size is
+	 * the value we expect it to be. If not, return the real size.
+	 */
+	for (len = 2; len <= variable_name_size; len += sizeof(c)) {
+		c = variable_name[(len / sizeof(c)) - 1];
+		if (!c)
+			break;
+	}
+
+	return min(len, variable_name_size);
+}
+
 static void efivar_update_sysfs_entries(struct work_struct *work)
 {
 	struct efivars *efivars = &__efivars;
@@ -1744,8 +1704,8 @@ static void efivar_update_sysfs_entries(struct work_struct *work)
 			if (status != EFI_SUCCESS) {
 				break;
 			} else {
-				if (!variable_is_present(variable_name,
-				    &vendor)) {
+				if (!variable_is_present(efivars,
+				    variable_name, &vendor)) {
 					found = true;
 					break;
 				}
@@ -1756,10 +1716,13 @@ static void efivar_update_sysfs_entries(struct work_struct *work)
 		if (!found) {
 			kfree(variable_name);
 			break;
-		} else
+		} else {
+			variable_name_size = var_name_strnsize(variable_name,
+							       variable_name_size);
 			efivar_create_sysfs_entry(efivars,
 						  variable_name_size,
 						  variable_name, &vendor);
+		}
 	}
 }
 
@@ -1958,6 +1921,35 @@ void unregister_efivars(struct efivars *efivars)
 }
 EXPORT_SYMBOL_GPL(unregister_efivars);
 
+/*
+ * Print a warning when duplicate EFI variables are encountered and
+ * disable the sysfs workqueue since the firmware is buggy.
+ */
+static void dup_variable_bug(efi_char16_t *s16, efi_guid_t *vendor_guid,
+			     unsigned long len16)
+{
+	size_t i, len8 = len16 / sizeof(efi_char16_t);
+	char *s8;
+
+	/*
+	 * Disable the workqueue since the algorithm it uses for
+	 * detecting new variables won't work with this buggy
+	 * implementation of GetNextVariableName().
+	 */
+	efivar_wq_enabled = false;
+
+	s8 = kzalloc(len8, GFP_KERNEL);
+	if (!s8)
+		return;
+
+	for (i = 0; i < len8; i++)
+		s8[i] = s16[i];
+
+	printk(KERN_WARNING "efivars: duplicate variable: %s-%pUl\n",
+	       s8, vendor_guid);
+	kfree(s8);
+}
+
 int register_efivars(struct efivars *efivars,
 		     const struct efivar_operations *ops,
 		     struct kobject *parent_kobj)
@@ -2006,6 +1998,25 @@ int register_efivars(struct efivars *efivars,
 						&vendor_guid);
 		switch (status) {
 		case EFI_SUCCESS:
+			variable_name_size = var_name_strnsize(variable_name,
+							       variable_name_size);
+
+			/*
+			 * Some firmware implementations return the
+			 * same variable name on multiple calls to
+			 * get_next_variable(). Terminate the loop
+			 * immediately as there is no guarantee that
+			 * we'll ever see a different variable name,
+			 * and may end up looping here forever.
+			 */
+			if (variable_is_present(efivars, variable_name,
+						&vendor_guid)) {
+				dup_variable_bug(variable_name, &vendor_guid,
+						 variable_name_size);
+				status = EFI_NOT_FOUND;
+				break;
+			}
+
 			efivar_create_sysfs_entry(efivars,
 						  variable_name_size,
 						  variable_name,
@@ -2025,15 +2036,8 @@ int register_efivars(struct efivars *efivars,
 	if (error)
 		unregister_efivars(efivars);
 
-	efivars->efi_pstore_info = efi_pstore_info;
-
-	efivars->efi_pstore_info.buf = kmalloc(4096, GFP_KERNEL);
-	if (efivars->efi_pstore_info.buf) {
-		efivars->efi_pstore_info.bufsize = 1024;
-		efivars->efi_pstore_info.data = efivars;
-		spin_lock_init(&efivars->efi_pstore_info.buf_lock);
-		pstore_register(&efivars->efi_pstore_info);
-	}
+	if (!efivars_pstore_disable)
+		efivar_pstore_register(efivars);
 
 	register_filesystem(&efivarfs_type);
 
@@ -2073,7 +2077,7 @@ efivars_init(void)
 	ops.get_variable = efi.get_variable;
 	ops.set_variable = efi.set_variable;
 	ops.get_next_variable = efi.get_next_variable;
-	ops.query_variable_info = efi.query_variable_info;
+	ops.query_variable_store = efi_query_variable_store;
 
 	error = register_efivars(&__efivars, &ops, efi_kobj);
 	if (error)
diff --git a/drivers/firmware/memmap.c b/drivers/firmware/memmap.c
index 0b5b5f619c7..e2e04b007e1 100644
--- a/drivers/firmware/memmap.c
+++ b/drivers/firmware/memmap.c
@@ -114,12 +114,9 @@ static void __meminit release_firmware_map_entry(struct kobject *kobj)
 		 * map_entries_bootmem here, and deleted from &map_entries in
 		 * firmware_map_remove_entry().
 		 */
-		if (firmware_map_find_entry(entry->start, entry->end,
-		    entry->type)) {
-			spin_lock(&map_entries_bootmem_lock);
-			list_add(&entry->list, &map_entries_bootmem);
-			spin_unlock(&map_entries_bootmem_lock);
-		}
+		spin_lock(&map_entries_bootmem_lock);
+		list_add(&entry->list, &map_entries_bootmem);
+		spin_unlock(&map_entries_bootmem_lock);
 
 		return;
 	}
diff --git a/drivers/gpio/gpio-ich.c b/drivers/gpio/gpio-ich.c
index f9dbd503fc4..de3c317bd3e 100644
--- a/drivers/gpio/gpio-ich.c
+++ b/drivers/gpio/gpio-ich.c
@@ -214,7 +214,7 @@ static int ichx_gpio_request(struct gpio_chip *chip, unsigned nr)
 	 * If it can't be trusted, assume that the pin can be used as a GPIO.
 	 */
 	if (ichx_priv.desc->use_sel_ignore[nr / 32] & (1 << (nr & 0x1f)))
-		return 1;
+		return 0;
 
 	return ichx_read_bit(GPIO_USE_SEL, nr) ? 0 : -ENODEV;
 }
diff --git a/drivers/gpio/gpio-pca953x.c b/drivers/gpio/gpio-pca953x.c
index 24059462c87..9391cf16e99 100644
--- a/drivers/gpio/gpio-pca953x.c
+++ b/drivers/gpio/gpio-pca953x.c
@@ -575,7 +575,7 @@ static int pca953x_irq_setup(struct pca953x_chip *chip,
 						chip->gpio_chip.ngpio,
 						irq_base,
 						&pca953x_irq_simple_ops,
-						NULL);
+						chip);
 		if (!chip->domain)
 			return -ENODEV;
 
diff --git a/drivers/gpio/gpio-pl061.c b/drivers/gpio/gpio-pl061.c
index b820869ca93..d7008dfdd6f 100644
--- a/drivers/gpio/gpio-pl061.c
+++ b/drivers/gpio/gpio-pl061.c
@@ -15,6 +15,7 @@
 #include <linux/io.h>
 #include <linux/ioport.h>
 #include <linux/irq.h>
+#include <linux/irqdomain.h>
 #include <linux/bitops.h>
 #include <linux/workqueue.h>
 #include <linux/gpio.h>
@@ -22,6 +23,7 @@
 #include <linux/amba/bus.h>
 #include <linux/amba/pl061.h>
 #include <linux/slab.h>
+#include <linux/pinctrl/consumer.h>
 #include <linux/pm.h>
 #include <asm/mach/irq.h>
 
@@ -51,8 +53,7 @@ struct pl061_gpio {
 	spinlock_t		lock;
 
 	void __iomem		*base;
-	int			irq_base;
-	struct irq_chip_generic	*irq_gc;
+	struct irq_domain	*domain;
 	struct gpio_chip	gc;
 
 #ifdef CONFIG_PM
@@ -60,6 +61,24 @@ struct pl061_gpio {
 #endif
 };
 
+static int pl061_gpio_request(struct gpio_chip *chip, unsigned offset)
+{
+	/*
+	 * Map back to global GPIO space and request muxing, the direction
+	 * parameter does not matter for this controller.
+	 */
+	int gpio = chip->base + offset;
+
+	return pinctrl_request_gpio(gpio);
+}
+
+static void pl061_gpio_free(struct gpio_chip *chip, unsigned offset)
+{
+	int gpio = chip->base + offset;
+
+	pinctrl_free_gpio(gpio);
+}
+
 static int pl061_direction_input(struct gpio_chip *gc, unsigned offset)
 {
 	struct pl061_gpio *chip = container_of(gc, struct pl061_gpio, gc);
@@ -122,24 +141,20 @@ static int pl061_to_irq(struct gpio_chip *gc, unsigned offset)
 {
 	struct pl061_gpio *chip = container_of(gc, struct pl061_gpio, gc);
 
-	if (chip->irq_base <= 0)
-		return -EINVAL;
-
-	return chip->irq_base + offset;
+	return irq_create_mapping(chip->domain, offset);
 }
 
 static int pl061_irq_type(struct irq_data *d, unsigned trigger)
 {
-	struct irq_chip_generic *gc = irq_data_get_irq_chip_data(d);
-	struct pl061_gpio *chip = gc->private;
-	int offset = d->irq - chip->irq_base;
+	struct pl061_gpio *chip = irq_data_get_irq_chip_data(d);
+	int offset = irqd_to_hwirq(d);
 	unsigned long flags;
 	u8 gpiois, gpioibe, gpioiev;
 
 	if (offset < 0 || offset >= PL061_GPIO_NR)
 		return -EINVAL;
 
-	raw_spin_lock_irqsave(&gc->lock, flags);
+	spin_lock_irqsave(&chip->lock, flags);
 
 	gpioiev = readb(chip->base + GPIOIEV);
 
@@ -168,7 +183,7 @@ static int pl061_irq_type(struct irq_data *d, unsigned trigger)
 
 	writeb(gpioiev, chip->base + GPIOIEV);
 
-	raw_spin_unlock_irqrestore(&gc->lock, flags);
+	spin_unlock_irqrestore(&chip->lock, flags);
 
 	return 0;
 }
@@ -192,31 +207,61 @@ static void pl061_irq_handler(unsigned irq, struct irq_desc *desc)
 	chained_irq_exit(irqchip, desc);
 }
 
-static void __init pl061_init_gc(struct pl061_gpio *chip, int irq_base)
+static void pl061_irq_mask(struct irq_data *d)
+{
+	struct pl061_gpio *chip = irq_data_get_irq_chip_data(d);
+	u8 mask = 1 << (irqd_to_hwirq(d) % PL061_GPIO_NR);
+	u8 gpioie;
+
+	spin_lock(&chip->lock);
+	gpioie = readb(chip->base + GPIOIE) & ~mask;
+	writeb(gpioie, chip->base + GPIOIE);
+	spin_unlock(&chip->lock);
+}
+
+static void pl061_irq_unmask(struct irq_data *d)
 {
-	struct irq_chip_type *ct;
+	struct pl061_gpio *chip = irq_data_get_irq_chip_data(d);
+	u8 mask = 1 << (irqd_to_hwirq(d) % PL061_GPIO_NR);
+	u8 gpioie;
+
+	spin_lock(&chip->lock);
+	gpioie = readb(chip->base + GPIOIE) | mask;
+	writeb(gpioie, chip->base + GPIOIE);
+	spin_unlock(&chip->lock);
+}
+
+static struct irq_chip pl061_irqchip = {
+	.name		= "pl061 gpio",
+	.irq_mask	= pl061_irq_mask,
+	.irq_unmask	= pl061_irq_unmask,
+	.irq_set_type	= pl061_irq_type,
+};
 
-	chip->irq_gc = irq_alloc_generic_chip("gpio-pl061", 1, irq_base,
-					      chip->base, handle_simple_irq);
-	chip->irq_gc->private = chip;
+static int pl061_irq_map(struct irq_domain *d, unsigned int virq,
+			 irq_hw_number_t hw)
+{
+	struct pl061_gpio *chip = d->host_data;
 
-	ct = chip->irq_gc->chip_types;
-	ct->chip.irq_mask = irq_gc_mask_clr_bit;
-	ct->chip.irq_unmask = irq_gc_mask_set_bit;
-	ct->chip.irq_set_type = pl061_irq_type;
-	ct->chip.irq_set_wake = irq_gc_set_wake;
-	ct->regs.mask = GPIOIE;
+	irq_set_chip_and_handler_name(virq, &pl061_irqchip, handle_simple_irq,
+				      "pl061");
+	irq_set_chip_data(virq, chip);
+	irq_set_irq_type(virq, IRQ_TYPE_NONE);
 
-	irq_setup_generic_chip(chip->irq_gc, IRQ_MSK(PL061_GPIO_NR),
-			       IRQ_GC_INIT_NESTED_LOCK, IRQ_NOREQUEST, 0);
+	return 0;
 }
 
+static const struct irq_domain_ops pl061_domain_ops = {
+	.map	= pl061_irq_map,
+	.xlate	= irq_domain_xlate_twocell,
+};
+
 static int pl061_probe(struct amba_device *adev, const struct amba_id *id)
 {
 	struct device *dev = &adev->dev;
 	struct pl061_platform_data *pdata = dev->platform_data;
 	struct pl061_gpio *chip;
-	int ret, irq, i;
+	int ret, irq, i, irq_base;
 
 	chip = devm_kzalloc(dev, sizeof(*chip), GFP_KERNEL);
 	if (chip == NULL)
@@ -224,24 +269,32 @@ static int pl061_probe(struct amba_device *adev, const struct amba_id *id)
 
 	if (pdata) {
 		chip->gc.base = pdata->gpio_base;
-		chip->irq_base = pdata->irq_base;
-	} else if (adev->dev.of_node) {
+		irq_base = pdata->irq_base;
+		if (irq_base <= 0)
+			return -ENODEV;
+	} else {
 		chip->gc.base = -1;
-		chip->irq_base = 0;
-	} else
-		return -ENODEV;
+		irq_base = 0;
+	}
 
 	if (!devm_request_mem_region(dev, adev->res.start,
-				resource_size(&adev->res), "pl061"))
+				     resource_size(&adev->res), "pl061"))
 		return -EBUSY;
 
 	chip->base = devm_ioremap(dev, adev->res.start,
-				resource_size(&adev->res));
-	if (chip->base == NULL)
+				  resource_size(&adev->res));
+	if (!chip->base)
 		return -ENOMEM;
 
+	chip->domain = irq_domain_add_simple(adev->dev.of_node, PL061_GPIO_NR,
+					     irq_base, &pl061_domain_ops, chip);
+	if (!chip->domain)
+		return -ENODEV;
+
 	spin_lock_init(&chip->lock);
 
+	chip->gc.request = pl061_gpio_request;
+	chip->gc.free = pl061_gpio_free;
 	chip->gc.direction_input = pl061_direction_input;
 	chip->gc.direction_output = pl061_direction_output;
 	chip->gc.get = pl061_get_value;
@@ -259,12 +312,6 @@ static int pl061_probe(struct amba_device *adev, const struct amba_id *id)
 	/*
 	 * irq_chip support
 	 */
-
-	if (chip->irq_base <= 0)
-		return 0;
-
-	pl061_init_gc(chip, chip->irq_base);
-
 	writeb(0, chip->base + GPIOIE); /* disable irqs */
 	irq = adev->irq[0];
 	if (irq < 0)
diff --git a/drivers/gpio/gpio-pxa.c b/drivers/gpio/gpio-pxa.c
index 9cc108d2b77..8325f580c0f 100644
--- a/drivers/gpio/gpio-pxa.c
+++ b/drivers/gpio/gpio-pxa.c
@@ -642,7 +642,12 @@ static struct platform_driver pxa_gpio_driver = {
 		.of_match_table = of_match_ptr(pxa_gpio_dt_ids),
 	},
 };
-module_platform_driver(pxa_gpio_driver);
+
+static int __init pxa_gpio_init(void)
+{
+	return platform_driver_register(&pxa_gpio_driver);
+}
+postcore_initcall(pxa_gpio_init);
 
 #ifdef CONFIG_PM
 static int pxa_gpio_suspend(void)
diff --git a/drivers/gpio/gpio-stmpe.c b/drivers/gpio/gpio-stmpe.c
index 770476a9da8..3ce5bc38ac3 100644
--- a/drivers/gpio/gpio-stmpe.c
+++ b/drivers/gpio/gpio-stmpe.c
@@ -307,11 +307,15 @@ static const struct irq_domain_ops stmpe_gpio_irq_simple_ops = {
 	.xlate = irq_domain_xlate_twocell,
 };
 
-static int stmpe_gpio_irq_init(struct stmpe_gpio *stmpe_gpio)
+static int stmpe_gpio_irq_init(struct stmpe_gpio *stmpe_gpio,
+		struct device_node *np)
 {
-	int base = stmpe_gpio->irq_base;
+	int base = 0;
 
-	stmpe_gpio->domain = irq_domain_add_simple(NULL,
+	if (!np)
+		base = stmpe_gpio->irq_base;
+
+	stmpe_gpio->domain = irq_domain_add_simple(np,
 				stmpe_gpio->chip.ngpio, base,
 				&stmpe_gpio_irq_simple_ops, stmpe_gpio);
 	if (!stmpe_gpio->domain) {
@@ -346,6 +350,9 @@ static int stmpe_gpio_probe(struct platform_device *pdev)
 	stmpe_gpio->chip = template_chip;
 	stmpe_gpio->chip.ngpio = stmpe->num_gpios;
 	stmpe_gpio->chip.dev = &pdev->dev;
+#ifdef CONFIG_OF
+	stmpe_gpio->chip.of_node = np;
+#endif
 	stmpe_gpio->chip.base = pdata ? pdata->gpio_base : -1;
 
 	if (pdata)
@@ -366,7 +373,7 @@ static int stmpe_gpio_probe(struct platform_device *pdev)
 		goto out_free;
 
 	if (irq >= 0) {
-		ret = stmpe_gpio_irq_init(stmpe_gpio);
+		ret = stmpe_gpio_irq_init(stmpe_gpio, np);
 		if (ret)
 			goto out_disable;
 
diff --git a/drivers/gpio/gpiolib-of.c b/drivers/gpio/gpiolib-of.c
index a71a54a3e3f..465f4ca57e8 100644
--- a/drivers/gpio/gpiolib-of.c
+++ b/drivers/gpio/gpiolib-of.c
@@ -193,7 +193,7 @@ static void of_gpiochip_add_pin_range(struct gpio_chip *chip)
 	if (!np)
 		return;
 
-	do {
+	for (;; index++) {
 		ret = of_parse_phandle_with_args(np, "gpio-ranges",
 				"#gpio-range-cells", index, &pinspec);
 		if (ret)
@@ -203,27 +203,15 @@ static void of_gpiochip_add_pin_range(struct gpio_chip *chip)
 		if (!pctldev)
 			break;
 
-		/*
-		 * This assumes that the n GPIO pins are consecutive in the
-		 * GPIO number space, and that the pins are also consecutive
-		 * in their local number space. Currently it is not possible
-		 * to add different ranges for one and the same GPIO chip,
-		 * as the code assumes that we have one consecutive range
-		 * on both, mapping 1-to-1.
-		 *
-		 * TODO: make the OF bindings handle multiple sparse ranges
-		 * on the same GPIO chip.
-		 */
 		ret = gpiochip_add_pin_range(chip,
 					     pinctrl_dev_get_devname(pctldev),
-					     0, /* offset in gpiochip */
 					     pinspec.args[0],
-					     pinspec.args[1]);
+					     pinspec.args[1],
+					     pinspec.args[2]);
 
 		if (ret)
 			break;
-
-	} while (index++);
+	}
 }
 
 #else
diff --git a/drivers/gpu/drm/drm_crtc.c b/drivers/gpu/drm/drm_crtc.c
index 792c3e3795c..dd64a06dc5b 100644
--- a/drivers/gpu/drm/drm_crtc.c
+++ b/drivers/gpu/drm/drm_crtc.c
@@ -2326,7 +2326,6 @@ int drm_mode_addfb(struct drm_device *dev,
 	fb = dev->mode_config.funcs->fb_create(dev, file_priv, &r);
 	if (IS_ERR(fb)) {
 		DRM_DEBUG_KMS("could not create framebuffer\n");
-		drm_modeset_unlock_all(dev);
 		return PTR_ERR(fb);
 	}
 
@@ -2506,7 +2505,6 @@ int drm_mode_addfb2(struct drm_device *dev,
 	fb = dev->mode_config.funcs->fb_create(dev, file_priv, r);
 	if (IS_ERR(fb)) {
 		DRM_DEBUG_KMS("could not create framebuffer\n");
-		drm_modeset_unlock_all(dev);
 		return PTR_ERR(fb);
 	}
 
diff --git a/drivers/gpu/drm/drm_fb_helper.c b/drivers/gpu/drm/drm_fb_helper.c
index 59d6b9bf204..892ff9f9597 100644
--- a/drivers/gpu/drm/drm_fb_helper.c
+++ b/drivers/gpu/drm/drm_fb_helper.c
@@ -1544,10 +1544,10 @@ int drm_fb_helper_hotplug_event(struct drm_fb_helper *fb_helper)
 	if (!fb_helper->fb)
 		return 0;
 
-	drm_modeset_lock_all(dev);
+	mutex_lock(&fb_helper->dev->mode_config.mutex);
 	if (!drm_fb_helper_is_bound(fb_helper)) {
 		fb_helper->delayed_hotplug = true;
-		drm_modeset_unlock_all(dev);
+		mutex_unlock(&fb_helper->dev->mode_config.mutex);
 		return 0;
 	}
 	DRM_DEBUG_KMS("\n");
@@ -1558,9 +1558,11 @@ int drm_fb_helper_hotplug_event(struct drm_fb_helper *fb_helper)
 
 	count = drm_fb_helper_probe_connector_modes(fb_helper, max_width,
 						    max_height);
+	mutex_unlock(&fb_helper->dev->mode_config.mutex);
+
+	drm_modeset_lock_all(dev);
 	drm_setup_crtcs(fb_helper);
 	drm_modeset_unlock_all(dev);
-
 	drm_fb_helper_set_par(fb_helper->fbdev);
 
 	return 0;
diff --git a/drivers/gpu/drm/drm_fops.c b/drivers/gpu/drm/drm_fops.c
index 13fdcd10a60..429e07d0b0f 100644
--- a/drivers/gpu/drm/drm_fops.c
+++ b/drivers/gpu/drm/drm_fops.c
@@ -123,6 +123,7 @@ int drm_open(struct inode *inode, struct file *filp)
 	int retcode = 0;
 	int need_setup = 0;
 	struct address_space *old_mapping;
+	struct address_space *old_imapping;
 
 	minor = idr_find(&drm_minors_idr, minor_id);
 	if (!minor)
@@ -137,6 +138,7 @@ int drm_open(struct inode *inode, struct file *filp)
 	if (!dev->open_count++)
 		need_setup = 1;
 	mutex_lock(&dev->struct_mutex);
+	old_imapping = inode->i_mapping;
 	old_mapping = dev->dev_mapping;
 	if (old_mapping == NULL)
 		dev->dev_mapping = &inode->i_data;
@@ -159,8 +161,8 @@ int drm_open(struct inode *inode, struct file *filp)
 
 err_undo:
 	mutex_lock(&dev->struct_mutex);
-	filp->f_mapping = old_mapping;
-	inode->i_mapping = old_mapping;
+	filp->f_mapping = old_imapping;
+	inode->i_mapping = old_imapping;
 	iput(container_of(dev->dev_mapping, struct inode, i_data));
 	dev->dev_mapping = old_mapping;
 	mutex_unlock(&dev->struct_mutex);
diff --git a/drivers/gpu/drm/drm_modes.c b/drivers/gpu/drm/drm_modes.c
index 04fa6f1808d..f83f0719922 100644
--- a/drivers/gpu/drm/drm_modes.c
+++ b/drivers/gpu/drm/drm_modes.c
@@ -506,7 +506,7 @@ drm_gtf_mode(struct drm_device *dev, int hdisplay, int vdisplay, int vrefresh,
 }
 EXPORT_SYMBOL(drm_gtf_mode);
 
-#if IS_ENABLED(CONFIG_VIDEOMODE)
+#ifdef CONFIG_VIDEOMODE_HELPERS
 int drm_display_mode_from_videomode(const struct videomode *vm,
 				    struct drm_display_mode *dmode)
 {
@@ -523,26 +523,25 @@ int drm_display_mode_from_videomode(const struct videomode *vm,
 	dmode->clock = vm->pixelclock / 1000;
 
 	dmode->flags = 0;
-	if (vm->dmt_flags & VESA_DMT_HSYNC_HIGH)
+	if (vm->flags & DISPLAY_FLAGS_HSYNC_HIGH)
 		dmode->flags |= DRM_MODE_FLAG_PHSYNC;
-	else if (vm->dmt_flags & VESA_DMT_HSYNC_LOW)
+	else if (vm->flags & DISPLAY_FLAGS_HSYNC_LOW)
 		dmode->flags |= DRM_MODE_FLAG_NHSYNC;
-	if (vm->dmt_flags & VESA_DMT_VSYNC_HIGH)
+	if (vm->flags & DISPLAY_FLAGS_VSYNC_HIGH)
 		dmode->flags |= DRM_MODE_FLAG_PVSYNC;
-	else if (vm->dmt_flags & VESA_DMT_VSYNC_LOW)
+	else if (vm->flags & DISPLAY_FLAGS_VSYNC_LOW)
 		dmode->flags |= DRM_MODE_FLAG_NVSYNC;
-	if (vm->data_flags & DISPLAY_FLAGS_INTERLACED)
+	if (vm->flags & DISPLAY_FLAGS_INTERLACED)
 		dmode->flags |= DRM_MODE_FLAG_INTERLACE;
-	if (vm->data_flags & DISPLAY_FLAGS_DOUBLESCAN)
+	if (vm->flags & DISPLAY_FLAGS_DOUBLESCAN)
 		dmode->flags |= DRM_MODE_FLAG_DBLSCAN;
 	drm_mode_set_name(dmode);
 
 	return 0;
 }
 EXPORT_SYMBOL_GPL(drm_display_mode_from_videomode);
-#endif
 
-#if IS_ENABLED(CONFIG_OF_VIDEOMODE)
+#ifdef CONFIG_OF
 /**
  * of_get_drm_display_mode - get a drm_display_mode from devicetree
  * @np: device_node with the timing specification
@@ -572,7 +571,8 @@ int of_get_drm_display_mode(struct device_node *np,
 	return 0;
 }
 EXPORT_SYMBOL_GPL(of_get_drm_display_mode);
-#endif
+#endif /* CONFIG_OF */
+#endif /* CONFIG_VIDEOMODE_HELPERS */
 
 /**
  * drm_mode_set_name - set the name on a mode
diff --git a/drivers/gpu/drm/exynos/exynos_drm_fimd.c b/drivers/gpu/drm/exynos/exynos_drm_fimd.c
index 36493ce71f9..98cc14725ba 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_fimd.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_fimd.c
@@ -38,11 +38,12 @@
 /* position control register for hardware window 0, 2 ~ 4.*/
 #define VIDOSD_A(win)		(VIDOSD_BASE + 0x00 + (win) * 16)
 #define VIDOSD_B(win)		(VIDOSD_BASE + 0x04 + (win) * 16)
-/* size control register for hardware window 0. */
-#define VIDOSD_C_SIZE_W0	(VIDOSD_BASE + 0x08)
-/* alpha control register for hardware window 1 ~ 4. */
-#define VIDOSD_C(win)		(VIDOSD_BASE + 0x18 + (win) * 16)
-/* size control register for hardware window 1 ~ 4. */
+/*
+ * size control register for hardware windows 0 and alpha control register
+ * for hardware windows 1 ~ 4
+ */
+#define VIDOSD_C(win)		(VIDOSD_BASE + 0x08 + (win) * 16)
+/* size control register for hardware windows 1 ~ 2. */
 #define VIDOSD_D(win)		(VIDOSD_BASE + 0x0C + (win) * 16)
 
 #define VIDWx_BUF_START(win, buf)	(VIDW_BUF_START(buf) + (win) * 8)
@@ -50,9 +51,9 @@
 #define VIDWx_BUF_SIZE(win, buf)	(VIDW_BUF_SIZE(buf) + (win) * 4)
 
 /* color key control register for hardware window 1 ~ 4. */
-#define WKEYCON0_BASE(x)		((WKEYCON0 + 0x140) + (x * 8))
+#define WKEYCON0_BASE(x)		((WKEYCON0 + 0x140) + ((x - 1) * 8))
 /* color key value register for hardware window 1 ~ 4. */
-#define WKEYCON1_BASE(x)		((WKEYCON1 + 0x140) + (x * 8))
+#define WKEYCON1_BASE(x)		((WKEYCON1 + 0x140) + ((x - 1) * 8))
 
 /* FIMD has totally five hardware windows. */
 #define WINDOWS_NR	5
@@ -109,9 +110,9 @@ struct fimd_context {
 
 #ifdef CONFIG_OF
 static const struct of_device_id fimd_driver_dt_match[] = {
-	{ .compatible = "samsung,exynos4-fimd",
+	{ .compatible = "samsung,exynos4210-fimd",
 	  .data = &exynos4_fimd_driver_data },
-	{ .compatible = "samsung,exynos5-fimd",
+	{ .compatible = "samsung,exynos5250-fimd",
 	  .data = &exynos5_fimd_driver_data },
 	{},
 };
@@ -581,7 +582,7 @@ static void fimd_win_commit(struct device *dev, int zpos)
 	if (win != 3 && win != 4) {
 		u32 offset = VIDOSD_D(win);
 		if (win == 0)
-			offset = VIDOSD_C_SIZE_W0;
+			offset = VIDOSD_C(win);
 		val = win_data->ovl_width * win_data->ovl_height;
 		writel(val, ctx->regs + offset);
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_g2d.c b/drivers/gpu/drm/exynos/exynos_drm_g2d.c
index 3b0da0378ac..47a493c8a71 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_g2d.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_g2d.c
@@ -48,8 +48,14 @@
 
 /* registers for base address */
 #define G2D_SRC_BASE_ADDR		0x0304
+#define G2D_SRC_COLOR_MODE		0x030C
+#define G2D_SRC_LEFT_TOP		0x0310
+#define G2D_SRC_RIGHT_BOTTOM		0x0314
 #define G2D_SRC_PLANE2_BASE_ADDR	0x0318
 #define G2D_DST_BASE_ADDR		0x0404
+#define G2D_DST_COLOR_MODE		0x040C
+#define G2D_DST_LEFT_TOP		0x0410
+#define G2D_DST_RIGHT_BOTTOM		0x0414
 #define G2D_DST_PLANE2_BASE_ADDR	0x0418
 #define G2D_PAT_BASE_ADDR		0x0500
 #define G2D_MSK_BASE_ADDR		0x0520
@@ -82,7 +88,7 @@
 #define G2D_DMA_LIST_DONE_COUNT_OFFSET	17
 
 /* G2D_DMA_HOLD_CMD */
-#define G2D_USET_HOLD			(1 << 2)
+#define G2D_USER_HOLD			(1 << 2)
 #define G2D_LIST_HOLD			(1 << 1)
 #define G2D_BITBLT_HOLD			(1 << 0)
 
@@ -91,13 +97,27 @@
 #define G2D_START_NHOLT			(1 << 1)
 #define G2D_START_BITBLT		(1 << 0)
 
+/* buffer color format */
+#define G2D_FMT_XRGB8888		0
+#define G2D_FMT_ARGB8888		1
+#define G2D_FMT_RGB565			2
+#define G2D_FMT_XRGB1555		3
+#define G2D_FMT_ARGB1555		4
+#define G2D_FMT_XRGB4444		5
+#define G2D_FMT_ARGB4444		6
+#define G2D_FMT_PACKED_RGB888		7
+#define G2D_FMT_A8			11
+#define G2D_FMT_L8			12
+
+/* buffer valid length */
+#define G2D_LEN_MIN			1
+#define G2D_LEN_MAX			8000
+
 #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)
 
-#define MAX_BUF_ADDR_NR			6
-
 /* maximum buffer pool size of userptr is 64MB as default */
 #define MAX_POOL		(64 * 1024 * 1024)
 
@@ -106,6 +126,17 @@ enum {
 	BUF_TYPE_USERPTR,
 };
 
+enum g2d_reg_type {
+	REG_TYPE_NONE = -1,
+	REG_TYPE_SRC,
+	REG_TYPE_SRC_PLANE2,
+	REG_TYPE_DST,
+	REG_TYPE_DST_PLANE2,
+	REG_TYPE_PAT,
+	REG_TYPE_MSK,
+	MAX_REG_TYPE_NR
+};
+
 /* cmdlist data structure */
 struct g2d_cmdlist {
 	u32		head;
@@ -113,6 +144,42 @@ struct g2d_cmdlist {
 	u32		last;	/* last data offset */
 };
 
+/*
+ * A structure of buffer description
+ *
+ * @format: color format
+ * @left_x: the x coordinates of left top corner
+ * @top_y: the y coordinates of left top corner
+ * @right_x: the x coordinates of right bottom corner
+ * @bottom_y: the y coordinates of right bottom corner
+ *
+ */
+struct g2d_buf_desc {
+	unsigned int	format;
+	unsigned int	left_x;
+	unsigned int	top_y;
+	unsigned int	right_x;
+	unsigned int	bottom_y;
+};
+
+/*
+ * A structure of buffer information
+ *
+ * @map_nr: manages the number of mapped buffers
+ * @reg_types: stores regitster type in the order of requested command
+ * @handles: stores buffer handle in its reg_type position
+ * @types: stores buffer type in its reg_type position
+ * @descs: stores buffer description in its reg_type position
+ *
+ */
+struct g2d_buf_info {
+	unsigned int		map_nr;
+	enum g2d_reg_type	reg_types[MAX_REG_TYPE_NR];
+	unsigned long		handles[MAX_REG_TYPE_NR];
+	unsigned int		types[MAX_REG_TYPE_NR];
+	struct g2d_buf_desc	descs[MAX_REG_TYPE_NR];
+};
+
 struct drm_exynos_pending_g2d_event {
 	struct drm_pending_event	base;
 	struct drm_exynos_g2d_event	event;
@@ -131,14 +198,11 @@ struct g2d_cmdlist_userptr {
 	bool			in_pool;
 	bool			out_of_list;
 };
-
 struct g2d_cmdlist_node {
 	struct list_head	list;
 	struct g2d_cmdlist	*cmdlist;
-	unsigned int		map_nr;
-	unsigned long		handles[MAX_BUF_ADDR_NR];
-	unsigned int		obj_type[MAX_BUF_ADDR_NR];
 	dma_addr_t		dma_addr;
+	struct g2d_buf_info	buf_info;
 
 	struct drm_exynos_pending_g2d_event	*event;
 };
@@ -188,6 +252,7 @@ static int g2d_init_cmdlist(struct g2d_data *g2d)
 	struct exynos_drm_subdrv *subdrv = &g2d->subdrv;
 	int nr;
 	int ret;
+	struct g2d_buf_info *buf_info;
 
 	init_dma_attrs(&g2d->cmdlist_dma_attrs);
 	dma_set_attr(DMA_ATTR_WRITE_COMBINE, &g2d->cmdlist_dma_attrs);
@@ -209,11 +274,17 @@ static int g2d_init_cmdlist(struct g2d_data *g2d)
 	}
 
 	for (nr = 0; nr < G2D_CMDLIST_NUM; nr++) {
+		unsigned int i;
+
 		node[nr].cmdlist =
 			g2d->cmdlist_pool_virt + nr * G2D_CMDLIST_SIZE;
 		node[nr].dma_addr =
 			g2d->cmdlist_pool + nr * G2D_CMDLIST_SIZE;
 
+		buf_info = &node[nr].buf_info;
+		for (i = 0; i < MAX_REG_TYPE_NR; i++)
+			buf_info->reg_types[i] = REG_TYPE_NONE;
+
 		list_add_tail(&node[nr].list, &g2d->free_cmdlist);
 	}
 
@@ -450,7 +521,7 @@ static dma_addr_t *g2d_userptr_get_dma_addr(struct drm_device *drm_dev,
 						DMA_BIDIRECTIONAL);
 	if (ret < 0) {
 		DRM_ERROR("failed to map sgt with dma region.\n");
-		goto err_free_sgt;
+		goto err_sg_free_table;
 	}
 
 	g2d_userptr->dma_addr = sgt->sgl[0].dma_address;
@@ -467,8 +538,10 @@ static dma_addr_t *g2d_userptr_get_dma_addr(struct drm_device *drm_dev,
 
 	return &g2d_userptr->dma_addr;
 
-err_free_sgt:
+err_sg_free_table:
 	sg_free_table(sgt);
+
+err_free_sgt:
 	kfree(sgt);
 	sgt = NULL;
 
@@ -506,36 +579,172 @@ static void g2d_userptr_free_all(struct drm_device *drm_dev,
 	g2d->current_pool = 0;
 }
 
+static enum g2d_reg_type g2d_get_reg_type(int reg_offset)
+{
+	enum g2d_reg_type reg_type;
+
+	switch (reg_offset) {
+	case G2D_SRC_BASE_ADDR:
+	case G2D_SRC_COLOR_MODE:
+	case G2D_SRC_LEFT_TOP:
+	case G2D_SRC_RIGHT_BOTTOM:
+		reg_type = REG_TYPE_SRC;
+		break;
+	case G2D_SRC_PLANE2_BASE_ADDR:
+		reg_type = REG_TYPE_SRC_PLANE2;
+		break;
+	case G2D_DST_BASE_ADDR:
+	case G2D_DST_COLOR_MODE:
+	case G2D_DST_LEFT_TOP:
+	case G2D_DST_RIGHT_BOTTOM:
+		reg_type = REG_TYPE_DST;
+		break;
+	case G2D_DST_PLANE2_BASE_ADDR:
+		reg_type = REG_TYPE_DST_PLANE2;
+		break;
+	case G2D_PAT_BASE_ADDR:
+		reg_type = REG_TYPE_PAT;
+		break;
+	case G2D_MSK_BASE_ADDR:
+		reg_type = REG_TYPE_MSK;
+		break;
+	default:
+		reg_type = REG_TYPE_NONE;
+		DRM_ERROR("Unknown register offset![%d]\n", reg_offset);
+		break;
+	};
+
+	return reg_type;
+}
+
+static unsigned long g2d_get_buf_bpp(unsigned int format)
+{
+	unsigned long bpp;
+
+	switch (format) {
+	case G2D_FMT_XRGB8888:
+	case G2D_FMT_ARGB8888:
+		bpp = 4;
+		break;
+	case G2D_FMT_RGB565:
+	case G2D_FMT_XRGB1555:
+	case G2D_FMT_ARGB1555:
+	case G2D_FMT_XRGB4444:
+	case G2D_FMT_ARGB4444:
+		bpp = 2;
+		break;
+	case G2D_FMT_PACKED_RGB888:
+		bpp = 3;
+		break;
+	default:
+		bpp = 1;
+		break;
+	}
+
+	return bpp;
+}
+
+static bool g2d_check_buf_desc_is_valid(struct g2d_buf_desc *buf_desc,
+						enum g2d_reg_type reg_type,
+						unsigned long size)
+{
+	unsigned int width, height;
+	unsigned long area;
+
+	/*
+	 * check source and destination buffers only.
+	 * so the others are always valid.
+	 */
+	if (reg_type != REG_TYPE_SRC && reg_type != REG_TYPE_DST)
+		return true;
+
+	width = buf_desc->right_x - buf_desc->left_x;
+	if (width < G2D_LEN_MIN || width > G2D_LEN_MAX) {
+		DRM_ERROR("width[%u] is out of range!\n", width);
+		return false;
+	}
+
+	height = buf_desc->bottom_y - buf_desc->top_y;
+	if (height < G2D_LEN_MIN || height > G2D_LEN_MAX) {
+		DRM_ERROR("height[%u] is out of range!\n", height);
+		return false;
+	}
+
+	area = (unsigned long)width * (unsigned long)height *
+					g2d_get_buf_bpp(buf_desc->format);
+	if (area > size) {
+		DRM_ERROR("area[%lu] is out of range[%lu]!\n", area, size);
+		return false;
+	}
+
+	return true;
+}
+
 static int g2d_map_cmdlist_gem(struct g2d_data *g2d,
 				struct g2d_cmdlist_node *node,
 				struct drm_device *drm_dev,
 				struct drm_file *file)
 {
 	struct g2d_cmdlist *cmdlist = node->cmdlist;
+	struct g2d_buf_info *buf_info = &node->buf_info;
 	int offset;
+	int ret;
 	int i;
 
-	for (i = 0; i < node->map_nr; i++) {
+	for (i = 0; i < buf_info->map_nr; i++) {
+		struct g2d_buf_desc *buf_desc;
+		enum g2d_reg_type reg_type;
+		int reg_pos;
 		unsigned long handle;
 		dma_addr_t *addr;
 
-		offset = cmdlist->last - (i * 2 + 1);
-		handle = cmdlist->data[offset];
+		reg_pos = cmdlist->last - 2 * (i + 1);
+
+		offset = cmdlist->data[reg_pos];
+		handle = cmdlist->data[reg_pos + 1];
+
+		reg_type = g2d_get_reg_type(offset);
+		if (reg_type == REG_TYPE_NONE) {
+			ret = -EFAULT;
+			goto err;
+		}
+
+		buf_desc = &buf_info->descs[reg_type];
+
+		if (buf_info->types[reg_type] == BUF_TYPE_GEM) {
+			unsigned long size;
+
+			size = exynos_drm_gem_get_size(drm_dev, handle, file);
+			if (!size) {
+				ret = -EFAULT;
+				goto err;
+			}
+
+			if (!g2d_check_buf_desc_is_valid(buf_desc, reg_type,
+									size)) {
+				ret = -EFAULT;
+				goto err;
+			}
 
-		if (node->obj_type[i] == BUF_TYPE_GEM) {
 			addr = exynos_drm_gem_get_dma_addr(drm_dev, handle,
 								file);
 			if (IS_ERR(addr)) {
-				node->map_nr = i;
-				return -EFAULT;
+				ret = -EFAULT;
+				goto err;
 			}
 		} else {
 			struct drm_exynos_g2d_userptr g2d_userptr;
 
 			if (copy_from_user(&g2d_userptr, (void __user *)handle,
 				sizeof(struct drm_exynos_g2d_userptr))) {
-				node->map_nr = i;
-				return -EFAULT;
+				ret = -EFAULT;
+				goto err;
+			}
+
+			if (!g2d_check_buf_desc_is_valid(buf_desc, reg_type,
+							g2d_userptr.size)) {
+				ret = -EFAULT;
+				goto err;
 			}
 
 			addr = g2d_userptr_get_dma_addr(drm_dev,
@@ -544,16 +753,21 @@ static int g2d_map_cmdlist_gem(struct g2d_data *g2d,
 							file,
 							&handle);
 			if (IS_ERR(addr)) {
-				node->map_nr = i;
-				return -EFAULT;
+				ret = -EFAULT;
+				goto err;
 			}
 		}
 
-		cmdlist->data[offset] = *addr;
-		node->handles[i] = handle;
+		cmdlist->data[reg_pos + 1] = *addr;
+		buf_info->reg_types[i] = reg_type;
+		buf_info->handles[reg_type] = handle;
 	}
 
 	return 0;
+
+err:
+	buf_info->map_nr = i;
+	return ret;
 }
 
 static void g2d_unmap_cmdlist_gem(struct g2d_data *g2d,
@@ -561,22 +775,33 @@ static void g2d_unmap_cmdlist_gem(struct g2d_data *g2d,
 				  struct drm_file *filp)
 {
 	struct exynos_drm_subdrv *subdrv = &g2d->subdrv;
+	struct g2d_buf_info *buf_info = &node->buf_info;
 	int i;
 
-	for (i = 0; i < node->map_nr; i++) {
-		unsigned long handle = node->handles[i];
+	for (i = 0; i < buf_info->map_nr; i++) {
+		struct g2d_buf_desc *buf_desc;
+		enum g2d_reg_type reg_type;
+		unsigned long handle;
+
+		reg_type = buf_info->reg_types[i];
+
+		buf_desc = &buf_info->descs[reg_type];
+		handle = buf_info->handles[reg_type];
 
-		if (node->obj_type[i] == BUF_TYPE_GEM)
+		if (buf_info->types[reg_type] == BUF_TYPE_GEM)
 			exynos_drm_gem_put_dma_addr(subdrv->drm_dev, handle,
 							filp);
 		else
 			g2d_userptr_put_dma_addr(subdrv->drm_dev, handle,
 							false);
 
-		node->handles[i] = 0;
+		buf_info->reg_types[i] = REG_TYPE_NONE;
+		buf_info->handles[reg_type] = 0;
+		buf_info->types[reg_type] = 0;
+		memset(buf_desc, 0x00, sizeof(*buf_desc));
 	}
 
-	node->map_nr = 0;
+	buf_info->map_nr = 0;
 }
 
 static void g2d_dma_start(struct g2d_data *g2d,
@@ -589,10 +814,6 @@ static void g2d_dma_start(struct g2d_data *g2d,
 	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);
 }
@@ -643,7 +864,6 @@ 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);
@@ -724,20 +944,14 @@ static int g2d_check_reg_offset(struct device *dev,
 	int i;
 
 	for (i = 0; i < nr; i++) {
-		index = cmdlist->last - 2 * (i + 1);
+		struct g2d_buf_info *buf_info = &node->buf_info;
+		struct g2d_buf_desc *buf_desc;
+		enum g2d_reg_type reg_type;
+		unsigned long value;
 
-		if (for_addr) {
-			/* check userptr buffer type. */
-			reg_offset = (cmdlist->data[index] &
-					~0x7fffffff) >> 31;
-			if (reg_offset) {
-				node->obj_type[i] = BUF_TYPE_USERPTR;
-				cmdlist->data[index] &= ~G2D_BUF_USERPTR;
-			}
-		}
+		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)
@@ -753,8 +967,60 @@ static int g2d_check_reg_offset(struct device *dev,
 			if (!for_addr)
 				goto err;
 
-			if (node->obj_type[i] != BUF_TYPE_USERPTR)
-				node->obj_type[i] = BUF_TYPE_GEM;
+			reg_type = g2d_get_reg_type(reg_offset);
+			if (reg_type == REG_TYPE_NONE)
+				goto err;
+
+			/* check userptr buffer type. */
+			if ((cmdlist->data[index] & ~0x7fffffff) >> 31) {
+				buf_info->types[reg_type] = BUF_TYPE_USERPTR;
+				cmdlist->data[index] &= ~G2D_BUF_USERPTR;
+			} else
+				buf_info->types[reg_type] = BUF_TYPE_GEM;
+			break;
+		case G2D_SRC_COLOR_MODE:
+		case G2D_DST_COLOR_MODE:
+			if (for_addr)
+				goto err;
+
+			reg_type = g2d_get_reg_type(reg_offset);
+			if (reg_type == REG_TYPE_NONE)
+				goto err;
+
+			buf_desc = &buf_info->descs[reg_type];
+			value = cmdlist->data[index + 1];
+
+			buf_desc->format = value & 0xf;
+			break;
+		case G2D_SRC_LEFT_TOP:
+		case G2D_DST_LEFT_TOP:
+			if (for_addr)
+				goto err;
+
+			reg_type = g2d_get_reg_type(reg_offset);
+			if (reg_type == REG_TYPE_NONE)
+				goto err;
+
+			buf_desc = &buf_info->descs[reg_type];
+			value = cmdlist->data[index + 1];
+
+			buf_desc->left_x = value & 0x1fff;
+			buf_desc->top_y = (value & 0x1fff0000) >> 16;
+			break;
+		case G2D_SRC_RIGHT_BOTTOM:
+		case G2D_DST_RIGHT_BOTTOM:
+			if (for_addr)
+				goto err;
+
+			reg_type = g2d_get_reg_type(reg_offset);
+			if (reg_type == REG_TYPE_NONE)
+				goto err;
+
+			buf_desc = &buf_info->descs[reg_type];
+			value = cmdlist->data[index + 1];
+
+			buf_desc->right_x = value & 0x1fff;
+			buf_desc->bottom_y = (value & 0x1fff0000) >> 16;
 			break;
 		default:
 			if (for_addr)
@@ -860,9 +1126,23 @@ int exynos_g2d_set_cmdlist_ioctl(struct drm_device *drm_dev, void *data,
 	cmdlist->data[cmdlist->last++] = G2D_SRC_BASE_ADDR;
 	cmdlist->data[cmdlist->last++] = 0;
 
+	/*
+	 * 'LIST_HOLD' command should be set to the DMA_HOLD_CMD_REG
+	 * and GCF bit should be set to INTEN register if user wants
+	 * G2D interrupt event once current command list execution is
+	 * finished.
+	 * Otherwise only ACF bit should be set to INTEN register so
+	 * that one interrupt is occured after all command lists
+	 * have been completed.
+	 */
 	if (node->event) {
+		cmdlist->data[cmdlist->last++] = G2D_INTEN;
+		cmdlist->data[cmdlist->last++] = G2D_INTEN_ACF | G2D_INTEN_GCF;
 		cmdlist->data[cmdlist->last++] = G2D_DMA_HOLD_CMD;
 		cmdlist->data[cmdlist->last++] = G2D_LIST_HOLD;
+	} else {
+		cmdlist->data[cmdlist->last++] = G2D_INTEN;
+		cmdlist->data[cmdlist->last++] = G2D_INTEN_ACF;
 	}
 
 	/* Check size of cmdlist: last 2 is about G2D_BITBLT_START */
@@ -887,7 +1167,7 @@ int exynos_g2d_set_cmdlist_ioctl(struct drm_device *drm_dev, void *data,
 	if (ret < 0)
 		goto err_free_event;
 
-	node->map_nr = req->cmd_buf_nr;
+	node->buf_info.map_nr = req->cmd_buf_nr;
 	if (req->cmd_buf_nr) {
 		struct drm_exynos_g2d_cmd *cmd_buf;
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_gem.c b/drivers/gpu/drm/exynos/exynos_drm_gem.c
index 67e17ce112b..0e6fe000578 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_gem.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_gem.c
@@ -164,6 +164,27 @@ out:
 	exynos_gem_obj = NULL;
 }
 
+unsigned long exynos_drm_gem_get_size(struct drm_device *dev,
+						unsigned int gem_handle,
+						struct drm_file *file_priv)
+{
+	struct exynos_drm_gem_obj *exynos_gem_obj;
+	struct drm_gem_object *obj;
+
+	obj = drm_gem_object_lookup(dev, file_priv, gem_handle);
+	if (!obj) {
+		DRM_ERROR("failed to lookup gem object.\n");
+		return 0;
+	}
+
+	exynos_gem_obj = to_exynos_gem_obj(obj);
+
+	drm_gem_object_unreference_unlocked(obj);
+
+	return exynos_gem_obj->buffer->size;
+}
+
+
 struct exynos_drm_gem_obj *exynos_drm_gem_init(struct drm_device *dev,
 						      unsigned long size)
 {
diff --git a/drivers/gpu/drm/exynos/exynos_drm_gem.h b/drivers/gpu/drm/exynos/exynos_drm_gem.h
index 35ebac47dc2..468766bee45 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_gem.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_gem.h
@@ -130,6 +130,11 @@ int exynos_drm_gem_userptr_ioctl(struct drm_device *dev, void *data,
 int exynos_drm_gem_get_ioctl(struct drm_device *dev, void *data,
 				      struct drm_file *file_priv);
 
+/* get buffer size to gem handle. */
+unsigned long exynos_drm_gem_get_size(struct drm_device *dev,
+						unsigned int gem_handle,
+						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_vidi.c b/drivers/gpu/drm/exynos/exynos_drm_vidi.c
index 13ccbd4bcfa..9504b0cd825 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_vidi.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_vidi.c
@@ -117,13 +117,12 @@ static struct edid *vidi_get_edid(struct device *dev,
 	}
 
 	edid_len = (1 + ctx->raw_edid->extensions) * EDID_LENGTH;
-	edid = kzalloc(edid_len, GFP_KERNEL);
+	edid = kmemdup(ctx->raw_edid, edid_len, GFP_KERNEL);
 	if (!edid) {
 		DRM_DEBUG_KMS("failed to allocate edid\n");
 		return ERR_PTR(-ENOMEM);
 	}
 
-	memcpy(edid, ctx->raw_edid, edid_len);
 	return edid;
 }
 
@@ -563,12 +562,11 @@ int vidi_connection_ioctl(struct drm_device *drm_dev, void *data,
 			return -EINVAL;
 		}
 		edid_len = (1 + raw_edid->extensions) * EDID_LENGTH;
-		ctx->raw_edid = kzalloc(edid_len, GFP_KERNEL);
+		ctx->raw_edid = kmemdup(raw_edid, edid_len, GFP_KERNEL);
 		if (!ctx->raw_edid) {
 			DRM_DEBUG_KMS("failed to allocate raw_edid.\n");
 			return -ENOMEM;
 		}
-		memcpy(ctx->raw_edid, raw_edid, edid_len);
 	} else {
 		/*
 		 * with connection = 0, free raw_edid
diff --git a/drivers/gpu/drm/exynos/exynos_mixer.c b/drivers/gpu/drm/exynos/exynos_mixer.c
index e919aba29b3..2f4f72f0704 100644
--- a/drivers/gpu/drm/exynos/exynos_mixer.c
+++ b/drivers/gpu/drm/exynos/exynos_mixer.c
@@ -818,7 +818,7 @@ static void mixer_win_disable(void *ctx, int win)
 	mixer_ctx->win_data[win].enabled = false;
 }
 
-int mixer_check_timing(void *ctx, struct fb_videomode *timing)
+static int mixer_check_timing(void *ctx, struct fb_videomode *timing)
 {
 	struct mixer_context *mixer_ctx = ctx;
 	u32 w, h;
diff --git a/drivers/gpu/drm/i915/i915_drv.c b/drivers/gpu/drm/i915/i915_drv.c
index 0a8eceb7590..e9b57893db2 100644
--- a/drivers/gpu/drm/i915/i915_drv.c
+++ b/drivers/gpu/drm/i915/i915_drv.c
@@ -125,6 +125,11 @@ MODULE_PARM_DESC(preliminary_hw_support,
 		"Enable Haswell and ValleyView Support. "
 		"(default: false)");
 
+int i915_disable_power_well __read_mostly = 0;
+module_param_named(disable_power_well, i915_disable_power_well, int, 0600);
+MODULE_PARM_DESC(disable_power_well,
+		 "Disable the power well when possible (default: false)");
+
 static struct drm_driver driver;
 extern int intel_agp_enabled;
 
diff --git a/drivers/gpu/drm/i915/i915_drv.h b/drivers/gpu/drm/i915/i915_drv.h
index e95337c9745..01769e2a995 100644
--- a/drivers/gpu/drm/i915/i915_drv.h
+++ b/drivers/gpu/drm/i915/i915_drv.h
@@ -1398,6 +1398,7 @@ extern int i915_enable_fbc __read_mostly;
 extern bool i915_enable_hangcheck __read_mostly;
 extern int i915_enable_ppgtt __read_mostly;
 extern unsigned int i915_preliminary_hw_support __read_mostly;
+extern int i915_disable_power_well __read_mostly;
 
 extern int i915_suspend(struct drm_device *dev, pm_message_t state);
 extern int i915_resume(struct drm_device *dev);
diff --git a/drivers/gpu/drm/i915/i915_gem_execbuffer.c b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
index 3b11ab0fbc9..9a48e1a2d41 100644
--- a/drivers/gpu/drm/i915/i915_gem_execbuffer.c
+++ b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
@@ -57,7 +57,7 @@ eb_create(struct drm_i915_gem_execbuffer2 *args)
 	if (eb == NULL) {
 		int size = args->buffer_count;
 		int count = PAGE_SIZE / sizeof(struct hlist_head) / 2;
-		BUILD_BUG_ON(!is_power_of_2(PAGE_SIZE / sizeof(struct hlist_head)));
+		BUILD_BUG_ON_NOT_POWER_OF_2(PAGE_SIZE / sizeof(struct hlist_head));
 		while (count > 2*size)
 			count >>= 1;
 		eb = kzalloc(count*sizeof(struct hlist_head) +
diff --git a/drivers/gpu/drm/i915/intel_crt.c b/drivers/gpu/drm/i915/intel_crt.c
index 32a3693905e..1ce45a0a2d3 100644
--- a/drivers/gpu/drm/i915/intel_crt.c
+++ b/drivers/gpu/drm/i915/intel_crt.c
@@ -45,6 +45,9 @@
 
 struct intel_crt {
 	struct intel_encoder base;
+	/* DPMS state is stored in the connector, which we need in the
+	 * encoder's enable/disable callbacks */
+	struct intel_connector *connector;
 	bool force_hotplug_required;
 	u32 adpa_reg;
 };
@@ -81,29 +84,6 @@ static bool intel_crt_get_hw_state(struct intel_encoder *encoder,
 	return true;
 }
 
-static void intel_disable_crt(struct intel_encoder *encoder)
-{
-	struct drm_i915_private *dev_priv = encoder->base.dev->dev_private;
-	struct intel_crt *crt = intel_encoder_to_crt(encoder);
-	u32 temp;
-
-	temp = I915_READ(crt->adpa_reg);
-	temp |= ADPA_HSYNC_CNTL_DISABLE | ADPA_VSYNC_CNTL_DISABLE;
-	temp &= ~ADPA_DAC_ENABLE;
-	I915_WRITE(crt->adpa_reg, temp);
-}
-
-static void intel_enable_crt(struct intel_encoder *encoder)
-{
-	struct drm_i915_private *dev_priv = encoder->base.dev->dev_private;
-	struct intel_crt *crt = intel_encoder_to_crt(encoder);
-	u32 temp;
-
-	temp = I915_READ(crt->adpa_reg);
-	temp |= ADPA_DAC_ENABLE;
-	I915_WRITE(crt->adpa_reg, temp);
-}
-
 /* Note: The caller is required to filter out dpms modes not supported by the
  * platform. */
 static void intel_crt_set_dpms(struct intel_encoder *encoder, int mode)
@@ -135,6 +115,19 @@ static void intel_crt_set_dpms(struct intel_encoder *encoder, int mode)
 	I915_WRITE(crt->adpa_reg, temp);
 }
 
+static void intel_disable_crt(struct intel_encoder *encoder)
+{
+	intel_crt_set_dpms(encoder, DRM_MODE_DPMS_OFF);
+}
+
+static void intel_enable_crt(struct intel_encoder *encoder)
+{
+	struct intel_crt *crt = intel_encoder_to_crt(encoder);
+
+	intel_crt_set_dpms(encoder, crt->connector->base.dpms);
+}
+
+
 static void intel_crt_dpms(struct drm_connector *connector, int mode)
 {
 	struct drm_device *dev = connector->dev;
@@ -746,6 +739,7 @@ void intel_crt_init(struct drm_device *dev)
 	}
 
 	connector = &intel_connector->base;
+	crt->connector = intel_connector;
 	drm_connector_init(dev, &intel_connector->base,
 			   &intel_crt_connector_funcs, DRM_MODE_CONNECTOR_VGA);
 
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index 287b42c9d1a..b20d50192fc 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -5771,6 +5771,11 @@ static int haswell_crtc_mode_set(struct drm_crtc *crtc,
 		num_connectors++;
 	}
 
+	if (is_cpu_edp)
+		intel_crtc->cpu_transcoder = TRANSCODER_EDP;
+	else
+		intel_crtc->cpu_transcoder = pipe;
+
 	/* We are not sure yet this won't happen. */
 	WARN(!HAS_PCH_LPT(dev), "Unexpected PCH type %d\n",
 	     INTEL_PCH_TYPE(dev));
@@ -5837,11 +5842,6 @@ static int intel_crtc_mode_set(struct drm_crtc *crtc,
 	int pipe = intel_crtc->pipe;
 	int ret;
 
-	if (IS_HASWELL(dev) && intel_pipe_has_type(crtc, INTEL_OUTPUT_EDP))
-		intel_crtc->cpu_transcoder = TRANSCODER_EDP;
-	else
-		intel_crtc->cpu_transcoder = pipe;
-
 	drm_vblank_pre_modeset(dev, pipe);
 
 	ret = dev_priv->display.crtc_mode_set(crtc, mode, adjusted_mode,
diff --git a/drivers/gpu/drm/i915/intel_dp.c b/drivers/gpu/drm/i915/intel_dp.c
index d7d4afe0134..c3f5bd8a507 100644
--- a/drivers/gpu/drm/i915/intel_dp.c
+++ b/drivers/gpu/drm/i915/intel_dp.c
@@ -2028,7 +2028,7 @@ intel_dp_complete_link_train(struct intel_dp *intel_dp)
 	}
 
 	if (channel_eq)
-		DRM_DEBUG_KMS("Channel EQ done. DP Training successfull\n");
+		DRM_DEBUG_KMS("Channel EQ done. DP Training successful\n");
 
 	intel_dp_set_link_train(intel_dp, DP, DP_TRAINING_PATTERN_DISABLE);
 }
@@ -2559,12 +2559,15 @@ void intel_dp_encoder_destroy(struct drm_encoder *encoder)
 {
 	struct intel_digital_port *intel_dig_port = enc_to_dig_port(encoder);
 	struct intel_dp *intel_dp = &intel_dig_port->dp;
+	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 
 	i2c_del_adapter(&intel_dp->adapter);
 	drm_encoder_cleanup(encoder);
 	if (is_edp(intel_dp)) {
 		cancel_delayed_work_sync(&intel_dp->panel_vdd_work);
+		mutex_lock(&dev->mode_config.mutex);
 		ironlake_panel_vdd_off_sync(intel_dp);
+		mutex_unlock(&dev->mode_config.mutex);
 	}
 	kfree(intel_dig_port);
 }
diff --git a/drivers/gpu/drm/i915/intel_panel.c b/drivers/gpu/drm/i915/intel_panel.c
index a3730e0289e..bee8cb6108a 100644
--- a/drivers/gpu/drm/i915/intel_panel.c
+++ b/drivers/gpu/drm/i915/intel_panel.c
@@ -321,9 +321,6 @@ void intel_panel_enable_backlight(struct drm_device *dev,
 	if (dev_priv->backlight_level == 0)
 		dev_priv->backlight_level = intel_panel_get_max_backlight(dev);
 
-	dev_priv->backlight_enabled = true;
-	intel_panel_actually_set_backlight(dev, dev_priv->backlight_level);
-
 	if (INTEL_INFO(dev)->gen >= 4) {
 		uint32_t reg, tmp;
 
@@ -359,12 +356,12 @@ void intel_panel_enable_backlight(struct drm_device *dev,
 	}
 
 set_level:
-	/* Check the current backlight level and try to set again if it's zero.
-	 * On some machines, BLC_PWM_CPU_CTL is cleared to zero automatically
-	 * when BLC_PWM_CPU_CTL2 and BLC_PWM_PCH_CTL1 are written.
+	/* Call below after setting BLC_PWM_CPU_CTL2 and BLC_PWM_PCH_CTL1.
+	 * BLC_PWM_CPU_CTL may be cleared to zero automatically when these
+	 * registers are set.
 	 */
-	if (!intel_panel_get_backlight(dev))
-		intel_panel_actually_set_backlight(dev, dev_priv->backlight_level);
+	dev_priv->backlight_enabled = true;
+	intel_panel_actually_set_backlight(dev, dev_priv->backlight_level);
 }
 
 static void intel_panel_init_backlight(struct drm_device *dev)
diff --git a/drivers/gpu/drm/i915/intel_pm.c b/drivers/gpu/drm/i915/intel_pm.c
index a1794c6df1b..adca00783e6 100644
--- a/drivers/gpu/drm/i915/intel_pm.c
+++ b/drivers/gpu/drm/i915/intel_pm.c
@@ -4079,6 +4079,9 @@ void intel_set_power_well(struct drm_device *dev, bool enable)
 	if (!IS_HASWELL(dev))
 		return;
 
+	if (!i915_disable_power_well && !enable)
+		return;
+
 	tmp = I915_READ(HSW_PWR_WELL_DRIVER);
 	is_enabled = tmp & HSW_PWR_WELL_STATE;
 	enable_requested = tmp & HSW_PWR_WELL_ENABLE;
diff --git a/drivers/gpu/drm/i915/intel_sdvo.c b/drivers/gpu/drm/i915/intel_sdvo.c
index d07a8cdf998..78413ec623c 100644
--- a/drivers/gpu/drm/i915/intel_sdvo.c
+++ b/drivers/gpu/drm/i915/intel_sdvo.c
@@ -451,7 +451,7 @@ static bool intel_sdvo_write_cmd(struct intel_sdvo *intel_sdvo, u8 cmd,
 	int i, ret = true;
 
         /* Would be simpler to allocate both in one go ? */        
-	buf = (u8 *)kzalloc(args_len * 2 + 2, GFP_KERNEL);
+	buf = kzalloc(args_len * 2 + 2, GFP_KERNEL);
 	if (!buf)
 		return false;
 
diff --git a/drivers/gpu/drm/mgag200/mgag200_mode.c b/drivers/gpu/drm/mgag200/mgag200_mode.c
index fe22bb780e1..78d8e919509 100644
--- a/drivers/gpu/drm/mgag200/mgag200_mode.c
+++ b/drivers/gpu/drm/mgag200/mgag200_mode.c
@@ -751,8 +751,6 @@ static int mga_crtc_mode_set(struct drm_crtc *crtc,
 	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[] = {
@@ -803,7 +801,6 @@ static int mga_crtc_mode_set(struct drm_crtc *crtc,
 		option2 = 0x0000b000;
 		break;
 	case G200_ER:
-		dac_index90 = 0;
 		break;
 	}
 
@@ -852,10 +849,8 @@ static int mga_crtc_mode_set(struct drm_crtc *crtc,
 		WREG_DAC(i, dacvalue[i]);
 	}
 
-	if (mdev->type == G200_ER) {
-		WREG_DAC(0x90, dac_index90);
-	}
-
+	if (mdev->type == G200_ER)
+		WREG_DAC(0x90, 0);
 
 	if (option)
 		pci_write_config_dword(dev->pdev, PCI_MGA_OPTION, option);
@@ -952,8 +947,6 @@ static int mga_crtc_mode_set(struct drm_crtc *crtc,
 	if (mdev->type == G200_WB)
 		ext_vga[1] |= 0x88;
 
-	ext_vga_index24 = 0x05;
-
 	/* Set pixel clocks */
 	misc = 0x2d;
 	WREG8(MGA_MISC_OUT, misc);
@@ -965,7 +958,7 @@ static int mga_crtc_mode_set(struct drm_crtc *crtc,
 	}
 
 	if (mdev->type == G200_ER)
-		WREG_ECRT(24, ext_vga_index24);
+		WREG_ECRT(0x24, 0x5);
 
 	if (mdev->type == G200_EV) {
 		WREG_ECRT(6, 0);
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/base.c b/drivers/gpu/drm/nouveau/core/subdev/bios/base.c
index e816f06637a..0e2c1a4f165 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/bios/base.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/base.c
@@ -248,6 +248,22 @@ nouveau_bios_shadow_pci(struct nouveau_bios *bios)
 	}
 }
 
+static void
+nouveau_bios_shadow_platform(struct nouveau_bios *bios)
+{
+	struct pci_dev *pdev = nv_device(bios)->pdev;
+	size_t size;
+
+	void __iomem *rom = pci_platform_rom(pdev, &size);
+	if (rom && size) {
+		bios->data = kmalloc(size, GFP_KERNEL);
+		if (bios->data) {
+			memcpy_fromio(bios->data, rom, size);
+			bios->size = size;
+		}
+	}
+}
+
 static int
 nouveau_bios_score(struct nouveau_bios *bios, const bool writeable)
 {
@@ -288,6 +304,7 @@ nouveau_bios_shadow(struct nouveau_bios *bios)
 		{ "PROM", nouveau_bios_shadow_prom, false, 0, 0, NULL },
 		{ "ACPI", nouveau_bios_shadow_acpi, true, 0, 0, NULL },
 		{ "PCIROM", nouveau_bios_shadow_pci, true, 0, 0, NULL },
+		{ "PLATFORM", nouveau_bios_shadow_platform, true, 0, 0, NULL },
 		{}
 	};
 	struct methods *mthd, *best;
diff --git a/drivers/gpu/drm/nouveau/nouveau_abi16.c b/drivers/gpu/drm/nouveau/nouveau_abi16.c
index 3b6dc883e15..5eb3e0da7c6 100644
--- a/drivers/gpu/drm/nouveau/nouveau_abi16.c
+++ b/drivers/gpu/drm/nouveau/nouveau_abi16.c
@@ -391,7 +391,7 @@ nouveau_abi16_ioctl_notifierobj_alloc(ABI16_IOCTL_ARGS)
 	struct nouveau_drm *drm = nouveau_drm(dev);
 	struct nouveau_device *device = nv_device(drm->device);
 	struct nouveau_abi16 *abi16 = nouveau_abi16_get(file_priv, dev);
-	struct nouveau_abi16_chan *chan, *temp;
+	struct nouveau_abi16_chan *chan = NULL, *temp;
 	struct nouveau_abi16_ntfy *ntfy;
 	struct nouveau_object *object;
 	struct nv_dma_class args = {};
@@ -404,10 +404,11 @@ nouveau_abi16_ioctl_notifierobj_alloc(ABI16_IOCTL_ARGS)
 	if (unlikely(nv_device(abi16->device)->card_type >= NV_C0))
 		return nouveau_abi16_put(abi16, -EINVAL);
 
-	list_for_each_entry_safe(chan, temp, &abi16->channels, head) {
-		if (chan->chan->handle == (NVDRM_CHAN | info->channel))
+	list_for_each_entry(temp, &abi16->channels, head) {
+		if (temp->chan->handle == (NVDRM_CHAN | info->channel)) {
+			chan = temp;
 			break;
-		chan = NULL;
+		}
 	}
 
 	if (!chan)
@@ -459,17 +460,18 @@ nouveau_abi16_ioctl_gpuobj_free(ABI16_IOCTL_ARGS)
 {
 	struct drm_nouveau_gpuobj_free *fini = data;
 	struct nouveau_abi16 *abi16 = nouveau_abi16_get(file_priv, dev);
-	struct nouveau_abi16_chan *chan, *temp;
+	struct nouveau_abi16_chan *chan = NULL, *temp;
 	struct nouveau_abi16_ntfy *ntfy;
 	int ret;
 
 	if (unlikely(!abi16))
 		return -ENOMEM;
 
-	list_for_each_entry_safe(chan, temp, &abi16->channels, head) {
-		if (chan->chan->handle == (NVDRM_CHAN | fini->channel))
+	list_for_each_entry(temp, &abi16->channels, head) {
+		if (temp->chan->handle == (NVDRM_CHAN | fini->channel)) {
+			chan = temp;
 			break;
-		chan = NULL;
+		}
 	}
 
 	if (!chan)
diff --git a/drivers/gpu/drm/nouveau/nouveau_drm.c b/drivers/gpu/drm/nouveau/nouveau_drm.c
index d1099365bfc..c95decf543e 100644
--- a/drivers/gpu/drm/nouveau/nouveau_drm.c
+++ b/drivers/gpu/drm/nouveau/nouveau_drm.c
@@ -72,11 +72,25 @@ module_param_named(modeset, nouveau_modeset, int, 0400);
 static struct drm_driver driver;
 
 static int
+nouveau_drm_vblank_handler(struct nouveau_eventh *event, int head)
+{
+	struct nouveau_drm *drm =
+		container_of(event, struct nouveau_drm, vblank[head]);
+	drm_handle_vblank(drm->dev, head);
+	return NVKM_EVENT_KEEP;
+}
+
+static int
 nouveau_drm_vblank_enable(struct drm_device *dev, int head)
 {
 	struct nouveau_drm *drm = nouveau_drm(dev);
 	struct nouveau_disp *pdisp = nouveau_disp(drm->device);
-	nouveau_event_get(pdisp->vblank, head, &drm->vblank);
+
+	if (WARN_ON_ONCE(head > ARRAY_SIZE(drm->vblank)))
+		return -EIO;
+	WARN_ON_ONCE(drm->vblank[head].func);
+	drm->vblank[head].func = nouveau_drm_vblank_handler;
+	nouveau_event_get(pdisp->vblank, head, &drm->vblank[head]);
 	return 0;
 }
 
@@ -85,16 +99,11 @@ nouveau_drm_vblank_disable(struct drm_device *dev, int head)
 {
 	struct nouveau_drm *drm = nouveau_drm(dev);
 	struct nouveau_disp *pdisp = nouveau_disp(drm->device);
-	nouveau_event_put(pdisp->vblank, head, &drm->vblank);
-}
-
-static int
-nouveau_drm_vblank_handler(struct nouveau_eventh *event, int head)
-{
-	struct nouveau_drm *drm =
-		container_of(event, struct nouveau_drm, vblank);
-	drm_handle_vblank(drm->dev, head);
-	return NVKM_EVENT_KEEP;
+	if (drm->vblank[head].func)
+		nouveau_event_put(pdisp->vblank, head, &drm->vblank[head]);
+	else
+		WARN_ON_ONCE(1);
+	drm->vblank[head].func = NULL;
 }
 
 static u64
@@ -292,7 +301,6 @@ nouveau_drm_load(struct drm_device *dev, unsigned long flags)
 
 	dev->dev_private = drm;
 	drm->dev = dev;
-	drm->vblank.func = nouveau_drm_vblank_handler;
 
 	INIT_LIST_HEAD(&drm->clients);
 	spin_lock_init(&drm->tile.lock);
diff --git a/drivers/gpu/drm/nouveau/nouveau_drm.h b/drivers/gpu/drm/nouveau/nouveau_drm.h
index b25df374c90..9c39bafbef2 100644
--- a/drivers/gpu/drm/nouveau/nouveau_drm.h
+++ b/drivers/gpu/drm/nouveau/nouveau_drm.h
@@ -113,7 +113,7 @@ struct nouveau_drm {
 	struct nvbios vbios;
 	struct nouveau_display *display;
 	struct backlight_device *backlight;
-	struct nouveau_eventh vblank;
+	struct nouveau_eventh vblank[4];
 
 	/* power management */
 	struct nouveau_pm *pm;
diff --git a/drivers/gpu/drm/nouveau/nv50_display.c b/drivers/gpu/drm/nouveau/nv50_display.c
index 7f0e6c3f37d..1ddc03e51bf 100644
--- a/drivers/gpu/drm/nouveau/nv50_display.c
+++ b/drivers/gpu/drm/nouveau/nv50_display.c
@@ -479,7 +479,7 @@ nv50_display_flip_wait(void *data)
 {
 	struct nv50_display_flip *flip = data;
 	if (nouveau_bo_rd32(flip->disp->sync, flip->chan->addr / 4) ==
-					      flip->chan->data);
+					      flip->chan->data)
 		return true;
 	usleep_range(1, 2);
 	return false;
diff --git a/drivers/gpu/drm/radeon/radeon_bios.c b/drivers/gpu/drm/radeon/radeon_bios.c
index b8015913d38..fa3c56fba29 100644
--- a/drivers/gpu/drm/radeon/radeon_bios.c
+++ b/drivers/gpu/drm/radeon/radeon_bios.c
@@ -99,6 +99,29 @@ static bool radeon_read_bios(struct radeon_device *rdev)
 	return true;
 }
 
+static bool radeon_read_platform_bios(struct radeon_device *rdev)
+{
+	uint8_t __iomem *bios;
+	size_t size;
+
+	rdev->bios = NULL;
+
+	bios = pci_platform_rom(rdev->pdev, &size);
+	if (!bios) {
+		return false;
+	}
+
+	if (size == 0 || bios[0] != 0x55 || bios[1] != 0xaa) {
+		return false;
+	}
+	rdev->bios = kmemdup(bios, size, GFP_KERNEL);
+	if (rdev->bios == NULL) {
+		return false;
+	}
+
+	return true;
+}
+
 #ifdef CONFIG_ACPI
 /* ATRM is used to get the BIOS on the discrete cards in
  * dual-gpu systems.
@@ -620,6 +643,9 @@ bool radeon_get_bios(struct radeon_device *rdev)
 	if (r == false) {
 		r = radeon_read_disabled_bios(rdev);
 	}
+	if (r == false) {
+		r = radeon_read_platform_bios(rdev);
+	}
 	if (r == false || rdev->bios == NULL) {
 		DRM_ERROR("Unable to locate a BIOS ROM\n");
 		rdev->bios = NULL;
diff --git a/drivers/gpu/drm/radeon/radeon_irq_kms.c b/drivers/gpu/drm/radeon/radeon_irq_kms.c
index 48f80cd42d8..5a99d433fc3 100644
--- a/drivers/gpu/drm/radeon/radeon_irq_kms.c
+++ b/drivers/gpu/drm/radeon/radeon_irq_kms.c
@@ -270,7 +270,7 @@ int radeon_irq_kms_init(struct radeon_device *rdev)
 }
 
 /**
- * radeon_irq_kms_fini - tear down driver interrrupt info
+ * radeon_irq_kms_fini - tear down driver interrupt info
  *
  * @rdev: radeon device pointer
  *
diff --git a/drivers/gpu/drm/tilcdc/Kconfig b/drivers/gpu/drm/tilcdc/Kconfig
index d24d0401347..e461e997245 100644
--- a/drivers/gpu/drm/tilcdc/Kconfig
+++ b/drivers/gpu/drm/tilcdc/Kconfig
@@ -4,8 +4,7 @@ config DRM_TILCDC
 	select DRM_KMS_HELPER
 	select DRM_KMS_CMA_HELPER
 	select DRM_GEM_CMA_HELPER
-	select OF_VIDEOMODE
-	select OF_DISPLAY_TIMING
+	select VIDEOMODE_HELPERS
 	select BACKLIGHT_CLASS_DEVICE
 	help
 	  Choose this option if you have an TI SoC with LCDC display
diff --git a/drivers/gpu/drm/tilcdc/tilcdc_panel.c b/drivers/gpu/drm/tilcdc/tilcdc_panel.c
index 580b74e2022..90ee4978637 100644
--- a/drivers/gpu/drm/tilcdc/tilcdc_panel.c
+++ b/drivers/gpu/drm/tilcdc/tilcdc_panel.c
@@ -173,7 +173,7 @@ static int panel_connector_get_modes(struct drm_connector *connector)
 		struct drm_display_mode *mode = drm_mode_create(dev);
 		struct videomode vm;
 
-		if (videomode_from_timing(timings, &vm, i))
+		if (videomode_from_timings(timings, &vm, i))
 			break;
 
 		drm_display_mode_from_videomode(&vm, mode);
diff --git a/drivers/gpu/drm/udl/udl_connector.c b/drivers/gpu/drm/udl/udl_connector.c
index fe5cdbcf263..b44d548c56f 100644
--- a/drivers/gpu/drm/udl/udl_connector.c
+++ b/drivers/gpu/drm/udl/udl_connector.c
@@ -61,6 +61,10 @@ static int udl_get_modes(struct drm_connector *connector)
 	int ret;
 
 	edid = (struct edid *)udl_get_edid(udl);
+	if (!edid) {
+		drm_mode_connector_update_edid_property(connector, NULL);
+		return 0;
+	}
 
 	/*
 	 * We only read the main block, but if the monitor reports extension
diff --git a/drivers/hid/Kconfig b/drivers/hid/Kconfig
index 5f07d85c418..fb52f3f6de8 100644
--- a/drivers/hid/Kconfig
+++ b/drivers/hid/Kconfig
@@ -92,14 +92,14 @@ menu "Special HID drivers"
 
 config HID_A4TECH
 	tristate "A4 tech mice" if EXPERT
-	depends on USB_HID
+	depends on HID
 	default !EXPERT
 	---help---
 	Support for A4 tech X5 and WOP-35 / Trust 450L mice.
 
 config HID_ACRUX
 	tristate "ACRUX game controller support"
-	depends on USB_HID
+	depends on HID
 	---help---
 	Say Y here if you want to enable support for ACRUX game controllers.
 
@@ -113,7 +113,7 @@ config HID_ACRUX_FF
 
 config HID_APPLE
 	tristate "Apple {i,Power,Mac}Books" if EXPERT
-	depends on (USB_HID || BT_HIDP)
+	depends on HID
 	default !EXPERT
 	---help---
 	Support for some Apple devices which less or more break
@@ -122,36 +122,47 @@ config HID_APPLE
 	Say Y here if you want support for keyboards of	Apple iBooks, PowerBooks,
 	MacBooks, MacBook Pros and Apple Aluminum.
 
+config HID_APPLEIR
+	tristate "Apple infrared receiver"
+	depends on (USB_HID)
+	---help---
+	Support for Apple infrared remote control. All the Apple computers from
+	  2005 onwards include such a port, except the unibody Macbook (2009),
+	  and Mac Pros. This receiver is also used in the Apple TV set-top box
+	  prior to the 2010 model.
+
+	Say Y here if you want support for Apple infrared remote control.
+
 config HID_AUREAL
 	tristate "Aureal"
-	depends on USB_HID
+	depends on HID
 	---help---
 	Support for Aureal Cy se W-01RN Remote Controller and other Aureal derived remotes.
 
 config HID_BELKIN
 	tristate "Belkin Flip KVM and Wireless keyboard" if EXPERT
-	depends on USB_HID
+	depends on HID
 	default !EXPERT
 	---help---
 	Support for Belkin Flip KVM and Wireless keyboard.
 
 config HID_CHERRY
 	tristate "Cherry Cymotion keyboard" if EXPERT
-	depends on USB_HID
+	depends on HID
 	default !EXPERT
 	---help---
 	Support for Cherry Cymotion keyboard.
 
 config HID_CHICONY
 	tristate "Chicony Tactical pad" if EXPERT
-	depends on USB_HID
+	depends on HID
 	default !EXPERT
 	---help---
 	Support for Chicony Tactical pad.
 
 config HID_PRODIKEYS
 	tristate "Prodikeys PC-MIDI Keyboard support"
-	depends on USB_HID && SND
+	depends on HID && SND
 	select SND_RAWMIDI
 	---help---
 	Support for Prodikeys PC-MIDI Keyboard device support.
@@ -166,14 +177,14 @@ config HID_PRODIKEYS
 
 config HID_CYPRESS
 	tristate "Cypress mouse and barcode readers" if EXPERT
-	depends on USB_HID
+	depends on HID
 	default !EXPERT
 	---help---
 	Support for cypress mouse and barcode readers.
 
 config HID_DRAGONRISE
 	tristate "DragonRise Inc. game controller"
-	depends on USB_HID
+	depends on HID
 	---help---
 	Say Y here if you have DragonRise Inc. game controllers.
 	These might be branded as:
@@ -192,7 +203,7 @@ config DRAGONRISE_FF
 
 config HID_EMS_FF
 	tristate "EMS Production Inc. force feedback support"
-	depends on USB_HID
+	depends on HID
 	select INPUT_FF_MEMLESS
 	---help---
 	Say Y here if you want to enable force feedback support for devices by
@@ -202,13 +213,13 @@ config HID_EMS_FF
 
 config HID_ELECOM
 	tristate "ELECOM BM084 bluetooth mouse"
-	depends on BT_HIDP
+	depends on HID
 	---help---
 	Support for the ELECOM BM084 (bluetooth mouse).
 
 config HID_EZKEY
 	tristate "Ezkey BTC 8193 keyboard" if EXPERT
-	depends on USB_HID
+	depends on HID
 	default !EXPERT
 	---help---
 	Support for Ezkey BTC 8193 keyboard.
@@ -231,7 +242,7 @@ config HOLTEK_FF
 
 config HID_KEYTOUCH
 	tristate "Keytouch HID devices"
-	depends on USB_HID
+	depends on HID
 	---help---
 	Support for Keytouch HID devices not fully compliant with
 	the specification. Currently supported:
@@ -239,7 +250,7 @@ config HID_KEYTOUCH
 
 config HID_KYE
 	tristate "KYE/Genius devices"
-	depends on USB_HID
+	depends on HID
 	---help---
 	Support for KYE/Genius devices not fully compliant with HID standard:
 	- Ergo Mouse
@@ -249,25 +260,25 @@ config HID_KYE
 
 config HID_UCLOGIC
 	tristate "UC-Logic"
-	depends on USB_HID
+	depends on HID
 	---help---
 	Support for UC-Logic tablets.
 
 config HID_WALTOP
 	tristate "Waltop"
-	depends on USB_HID
+	depends on HID
 	---help---
 	Support for Waltop tablets.
 
 config HID_GYRATION
 	tristate "Gyration remote control"
-	depends on USB_HID
+	depends on HID
 	---help---
 	Support for Gyration remote control.
 
 config HID_ICADE
 	tristate "ION iCade arcade controller"
-	depends on BT_HIDP
+	depends on HID
 	---help---
 	Support for the ION iCade arcade controller to work as a joystick.
 
@@ -276,20 +287,20 @@ config HID_ICADE
 
 config HID_TWINHAN
 	tristate "Twinhan IR remote control"
-	depends on USB_HID
+	depends on HID
 	---help---
 	Support for Twinhan IR remote control.
 
 config HID_KENSINGTON
 	tristate "Kensington Slimblade Trackball" if EXPERT
-	depends on USB_HID
+	depends on HID
 	default !EXPERT
 	---help---
 	Support for Kensington Slimblade Trackball.
 
 config HID_LCPOWER
 	tristate "LC-Power"
-	depends on USB_HID
+	depends on HID
 	---help---
 	Support for LC-Power RC1000MCE RF remote control.
 
@@ -308,7 +319,7 @@ config HID_LENOVO_TPKBD
 
 config HID_LOGITECH
 	tristate "Logitech devices" if EXPERT
-	depends on USB_HID
+	depends on HID
 	default !EXPERT
 	---help---
 	Support for Logitech devices that are not fully compliant with HID standard.
@@ -373,31 +384,31 @@ config LOGIWHEELS_FF
 	  - Logitech Formula Force EX
 
 config HID_MAGICMOUSE
-	tristate "Apple MagicMouse multi-touch support"
-	depends on BT_HIDP
+	tristate "Apple Magic Mouse/Trackpad multi-touch support"
+	depends on HID
 	---help---
-	Support for the Apple Magic Mouse multi-touch.
+	Support for the Apple Magic Mouse/Trackpad multi-touch.
 
 	Say Y here if you want support for the multi-touch features of the
-	Apple Wireless "Magic" Mouse.
+	Apple Wireless "Magic" Mouse and the Apple Wireless "Magic" Trackpad.
 
 config HID_MICROSOFT
 	tristate "Microsoft non-fully HID-compliant devices" if EXPERT
-	depends on USB_HID
+	depends on HID
 	default !EXPERT
 	---help---
 	Support for Microsoft devices that are not fully compliant with HID standard.
 
 config HID_MONTEREY
 	tristate "Monterey Genius KB29E keyboard" if EXPERT
-	depends on USB_HID
+	depends on HID
 	default !EXPERT
 	---help---
 	Support for Monterey Genius KB29E.
 
 config HID_MULTITOUCH
 	tristate "HID Multitouch panels"
-	depends on USB_HID
+	depends on HID
 	---help---
 	  Generic support for HID multitouch panels.
 
@@ -445,7 +456,7 @@ config HID_NTRIG
 
 config HID_ORTEK
 	tristate "Ortek PKB-1700/WKB-2000/Skycable wireless keyboard and mouse trackpad"
-	depends on USB_HID
+	depends on HID
 	---help---
 	There are certain devices which have LogicalMaximum wrong in the keyboard
 	usage page of their report descriptor. The most prevailing ones so far
@@ -458,7 +469,7 @@ config HID_ORTEK
 
 config HID_PANTHERLORD
 	tristate "Pantherlord/GreenAsia game controller"
-	depends on USB_HID
+	depends on HID
 	---help---
 	  Say Y here if you have a PantherLord/GreenAsia based game controller
 	  or adapter.
@@ -473,13 +484,13 @@ config PANTHERLORD_FF
 
 config HID_PETALYNX
 	tristate "Petalynx Maxter remote control"
-	depends on USB_HID
+	depends on HID
 	---help---
 	Support for Petalynx Maxter remote control.
 
 config HID_PICOLCD
 	tristate "PicoLCD (graphic version)"
-	depends on USB_HID
+	depends on HID
 	---help---
 	  This provides support for Minibox PicoLCD devices, currently
 	  only the graphical ones are supported.
@@ -545,14 +556,14 @@ config HID_PICOLCD_CIR
 
 config HID_PRIMAX
 	tristate "Primax non-fully HID-compliant devices"
-	depends on USB_HID
+	depends on HID
 	---help---
 	Support for Primax devices that are not fully compliant with the
 	HID standard.
 
 config HID_PS3REMOTE
 	tristate "Sony PS3 BD Remote Control"
-	depends on BT_HIDP
+	depends on HID
 	---help---
 	Support for the Sony PS3 Blue-ray Disk Remote Control and Logitech
 	Harmony Adapter for PS3, which connect over Bluetooth.
@@ -569,7 +580,7 @@ config HID_ROCCAT
 
 config HID_SAITEK
 	tristate "Saitek non-fully HID-compliant devices"
-	depends on USB_HID
+	depends on HID
 	---help---
 	Support for Saitek devices that are not fully compliant with the
 	HID standard.
@@ -578,7 +589,7 @@ config HID_SAITEK
 
 config HID_SAMSUNG
 	tristate "Samsung InfraRed remote control or keyboards"
-	depends on USB_HID
+	depends on HID
 	---help---
 	Support for Samsung InfraRed remote control or keyboards.
 
@@ -592,25 +603,25 @@ config HID_SONY
 
 config HID_SPEEDLINK
 	tristate "Speedlink VAD Cezanne mouse support"
-	depends on USB_HID
+	depends on HID
 	---help---
 	Support for Speedlink Vicious and Divine Cezanne mouse.
 
 config HID_STEELSERIES
 	tristate "Steelseries SRW-S1 steering wheel support"
-	depends on USB_HID
+	depends on HID
 	---help---
 	Support for Steelseries SRW-S1 steering wheel
 
 config HID_SUNPLUS
 	tristate "Sunplus wireless desktop"
-	depends on USB_HID
+	depends on HID
 	---help---
 	Support for Sunplus wireless desktop.
 
 config HID_GREENASIA
 	tristate "GreenAsia (Product ID 0x12) game controller support"
-	depends on USB_HID
+	depends on HID
 	---help---
 	  Say Y here if you have a GreenAsia (Product ID 0x12) based game
 	  controller or adapter.
@@ -632,7 +643,7 @@ config HID_HYPERV_MOUSE
 
 config HID_SMARTJOYPLUS
 	tristate "SmartJoy PLUS PS2/USB adapter support"
-	depends on USB_HID
+	depends on HID
 	---help---
 	Support for SmartJoy PLUS PS2/USB adapter, Super Dual Box,
 	Super Joy Box 3 Pro, Super Dual Box Pro, and Super Joy Box 5 Pro.
@@ -650,20 +661,20 @@ config SMARTJOYPLUS_FF
 
 config HID_TIVO
 	tristate "TiVo Slide Bluetooth remote control support"
-	depends on (USB_HID || BT_HIDP)
+	depends on HID
 	---help---
 	Say Y if you have a TiVo Slide Bluetooth remote control.
 
 config HID_TOPSEED
 	tristate "TopSeed Cyberlink, BTC Emprex, Conceptronic remote control support"
-	depends on USB_HID
+	depends on HID
 	---help---
 	Say Y if you have a TopSeed Cyberlink or BTC Emprex or Conceptronic
 	CLLRCMCE remote control.
 
 config HID_THINGM
 	tristate "ThingM blink(1) USB RGB LED"
-	depends on USB_HID
+	depends on HID
 	depends on LEDS_CLASS
 	---help---
 	Support for the ThingM blink(1) USB RGB LED. This driver registers a
@@ -673,7 +684,7 @@ config HID_THINGM
 
 config HID_THRUSTMASTER
 	tristate "ThrustMaster devices support"
-	depends on USB_HID
+	depends on HID
 	---help---
 	  Say Y here if you have a THRUSTMASTER FireStore Dual Power 2 or
 	  a THRUSTMASTER Ferrari GT Rumble Wheel.
@@ -689,7 +700,7 @@ config THRUSTMASTER_FF
 
 config HID_WACOM
 	tristate "Wacom Bluetooth devices support"
-	depends on BT_HIDP
+	depends on HID
 	depends on LEDS_CLASS
 	select POWER_SUPPLY
 	---help---
@@ -697,7 +708,7 @@ config HID_WACOM
 
 config HID_WIIMOTE
 	tristate "Nintendo Wii Remote support"
-	depends on BT_HIDP
+	depends on HID
 	depends on LEDS_CLASS
 	select POWER_SUPPLY
 	select INPUT_FF_MEMLESS
@@ -715,7 +726,7 @@ config HID_WIIMOTE_EXT
 
 config HID_ZEROPLUS
 	tristate "Zeroplus based game controller support"
-	depends on USB_HID
+	depends on HID
 	---help---
 	  Say Y here if you have a Zeroplus based game controller.
 
@@ -729,16 +740,16 @@ config ZEROPLUS_FF
 
 config HID_ZYDACRON
 	tristate "Zydacron remote control support"
-	depends on USB_HID
+	depends on HID
 	---help---
 	Support for Zydacron remote control.
 
 config HID_SENSOR_HUB
 	tristate "HID Sensors framework support"
-	depends on USB_HID && GENERIC_HARDIRQS
+	depends on HID && GENERIC_HARDIRQS
 	select MFD_CORE
 	default n
-	-- help---
+	---help---
 	  Support for HID Sensor framework. This creates a MFD instance
 	  for a sensor hub and identifies all the sensors connected to it.
 	  Each sensor is registered as a MFD cell, so that sensor specific
diff --git a/drivers/hid/Makefile b/drivers/hid/Makefile
index 72d1b0bc0a9..2065694f57a 100644
--- a/drivers/hid/Makefile
+++ b/drivers/hid/Makefile
@@ -39,6 +39,7 @@ endif
 obj-$(CONFIG_HID_A4TECH)	+= hid-a4tech.o
 obj-$(CONFIG_HID_ACRUX)		+= hid-axff.o
 obj-$(CONFIG_HID_APPLE)		+= hid-apple.o
+obj-$(CONFIG_HID_APPLEIR)	+= hid-appleir.o
 obj-$(CONFIG_HID_AUREAL)        += hid-aureal.o
 obj-$(CONFIG_HID_BELKIN)	+= hid-belkin.o
 obj-$(CONFIG_HID_CHERRY)	+= hid-cherry.o
@@ -94,8 +95,8 @@ obj-$(CONFIG_HID_PRIMAX)	+= hid-primax.o
 obj-$(CONFIG_HID_PS3REMOTE)	+= hid-ps3remote.o
 obj-$(CONFIG_HID_ROCCAT)	+= hid-roccat.o hid-roccat-common.o \
 	hid-roccat-arvo.o hid-roccat-isku.o hid-roccat-kone.o \
-	hid-roccat-koneplus.o hid-roccat-kovaplus.o hid-roccat-lua.o \
-	hid-roccat-pyra.o hid-roccat-savu.o
+	hid-roccat-koneplus.o hid-roccat-konepure.o hid-roccat-kovaplus.o \
+	hid-roccat-lua.o hid-roccat-pyra.o hid-roccat-savu.o
 obj-$(CONFIG_HID_SAITEK)	+= hid-saitek.o
 obj-$(CONFIG_HID_SAMSUNG)	+= hid-samsung.o
 obj-$(CONFIG_HID_SMARTJOYPLUS)	+= hid-sjoy.o
diff --git a/drivers/hid/hid-apple.c b/drivers/hid/hid-apple.c
index 320a958d413..feae88b53fc 100644
--- a/drivers/hid/hid-apple.c
+++ b/drivers/hid/hid-apple.c
@@ -21,7 +21,6 @@
 #include <linux/hid.h>
 #include <linux/module.h>
 #include <linux/slab.h>
-#include <linux/usb.h>
 
 #include "hid-ids.h"
 
@@ -390,10 +389,6 @@ static void apple_remove(struct hid_device *hdev)
 }
 
 static const struct hid_device_id apple_devices[] = {
-	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ATV_IRCONTROL),
-		.driver_data = APPLE_HIDDEV | APPLE_IGNORE_HIDINPUT },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_IRCONTROL4),
-		.driver_data = APPLE_HIDDEV | APPLE_IGNORE_HIDINPUT },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_MIGHTYMOUSE),
 		.driver_data = APPLE_MIGHTYMOUSE | APPLE_INVERT_HWHEEL },
 
diff --git a/drivers/hid/hid-appleir.c b/drivers/hid/hid-appleir.c
new file mode 100644
index 00000000000..a42e6a394c5
--- /dev/null
+++ b/drivers/hid/hid-appleir.c
@@ -0,0 +1,352 @@
+/*
+ * HID driver for the apple ir device
+ *
+ * Original driver written by James McKenzie
+ * Ported to recent 2.6 kernel versions by Greg Kroah-Hartman <gregkh@suse.de>
+ * Updated to support newer remotes by Bastien Nocera <hadess@hadess.net>
+ * Ported to HID subsystem by Benjamin Tissoires <benjamin.tissoires@gmail.com>
+ *
+ * Copyright (C) 2006 James McKenzie
+ * Copyright (C) 2008 Greg Kroah-Hartman <greg@kroah.com>
+ * Copyright (C) 2008 Novell Inc.
+ * Copyright (C) 2010, 2012 Bastien Nocera <hadess@hadess.net>
+ * Copyright (C) 2013 Benjamin Tissoires <benjamin.tissoires@gmail.com>
+ * Copyright (C) 2013 Red Hat Inc. All Rights Reserved
+ *
+ * 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/hid.h>
+#include <linux/module.h>
+#include "hid-ids.h"
+
+MODULE_AUTHOR("James McKenzie");
+MODULE_AUTHOR("Benjamin Tissoires <benjamin.tissoires@redhat.com>");
+MODULE_DESCRIPTION("HID Apple IR remote controls");
+MODULE_LICENSE("GPL");
+
+#define KEY_MASK		0x0F
+#define TWO_PACKETS_MASK	0x40
+
+/*
+ * James McKenzie has two devices both of which report the following
+ * 25 87 ee 83 0a	+
+ * 25 87 ee 83 0c	-
+ * 25 87 ee 83 09	<<
+ * 25 87 ee 83 06	>>
+ * 25 87 ee 83 05	>"
+ * 25 87 ee 83 03	menu
+ * 26 00 00 00 00	for key repeat
+ */
+
+/*
+ * Thomas Glanzmann reports the following responses
+ * 25 87 ee ca 0b	+
+ * 25 87 ee ca 0d	-
+ * 25 87 ee ca 08	<<
+ * 25 87 ee ca 07	>>
+ * 25 87 ee ca 04	>"
+ * 25 87 ee ca 02	menu
+ * 26 00 00 00 00       for key repeat
+ *
+ * He also observes the following event sometimes
+ * sent after a key is release, which I interpret
+ * as a flat battery message
+ * 25 87 e0 ca 06	flat battery
+ */
+
+/*
+ * Alexandre Karpenko reports the following responses for Device ID 0x8242
+ * 25 87 ee 47 0b	+
+ * 25 87 ee 47 0d	-
+ * 25 87 ee 47 08	<<
+ * 25 87 ee 47 07	>>
+ * 25 87 ee 47 04	>"
+ * 25 87 ee 47 02	menu
+ * 26 87 ee 47 **	for key repeat (** is the code of the key being held)
+ */
+
+/*
+ * Bastien Nocera's remote
+ * 25 87 ee 91 5f	followed by
+ * 25 87 ee 91 05	gives you >"
+ *
+ * 25 87 ee 91 5c	followed by
+ * 25 87 ee 91 05	gives you the middle button
+ */
+
+/*
+ * Fabien Andre's remote
+ * 25 87 ee a3 5e	followed by
+ * 25 87 ee a3 04	gives you >"
+ *
+ * 25 87 ee a3 5d	followed by
+ * 25 87 ee a3 04	gives you the middle button
+ */
+
+static const unsigned short appleir_key_table[] = {
+	KEY_RESERVED,
+	KEY_MENU,
+	KEY_PLAYPAUSE,
+	KEY_FORWARD,
+	KEY_BACK,
+	KEY_VOLUMEUP,
+	KEY_VOLUMEDOWN,
+	KEY_RESERVED,
+	KEY_RESERVED,
+	KEY_RESERVED,
+	KEY_RESERVED,
+	KEY_RESERVED,
+	KEY_RESERVED,
+	KEY_RESERVED,
+	KEY_ENTER,
+	KEY_PLAYPAUSE,
+	KEY_RESERVED,
+};
+
+struct appleir {
+	struct input_dev *input_dev;
+	struct hid_device *hid;
+	unsigned short keymap[ARRAY_SIZE(appleir_key_table)];
+	struct timer_list key_up_timer;	/* timer for key up */
+	spinlock_t lock;		/* protects .current_key */
+	int current_key;		/* the currently pressed key */
+	int prev_key_idx;		/* key index in a 2 packets message */
+};
+
+static int get_key(int data)
+{
+	/*
+	 * The key is coded accross bits 2..9:
+	 *
+	 * 0x00 or 0x01 (        )	key:  0		-> KEY_RESERVED
+	 * 0x02 or 0x03 (  menu  )	key:  1		-> KEY_MENU
+	 * 0x04 or 0x05 (   >"   )	key:  2		-> KEY_PLAYPAUSE
+	 * 0x06 or 0x07 (   >>   )	key:  3		-> KEY_FORWARD
+	 * 0x08 or 0x09 (   <<   )	key:  4		-> KEY_BACK
+	 * 0x0a or 0x0b (    +   )	key:  5		-> KEY_VOLUMEUP
+	 * 0x0c or 0x0d (    -   )	key:  6		-> KEY_VOLUMEDOWN
+	 * 0x0e or 0x0f (        )	key:  7		-> KEY_RESERVED
+	 * 0x50 or 0x51 (        )	key:  8		-> KEY_RESERVED
+	 * 0x52 or 0x53 (        )	key:  9		-> KEY_RESERVED
+	 * 0x54 or 0x55 (        )	key: 10		-> KEY_RESERVED
+	 * 0x56 or 0x57 (        )	key: 11		-> KEY_RESERVED
+	 * 0x58 or 0x59 (        )	key: 12		-> KEY_RESERVED
+	 * 0x5a or 0x5b (        )	key: 13		-> KEY_RESERVED
+	 * 0x5c or 0x5d ( middle )	key: 14		-> KEY_ENTER
+	 * 0x5e or 0x5f (   >"   )	key: 15		-> KEY_PLAYPAUSE
+	 *
+	 * Packets starting with 0x5 are part of a two-packets message,
+	 * we notify the caller by sending a negative value.
+	 */
+	int key = (data >> 1) & KEY_MASK;
+
+	if ((data & TWO_PACKETS_MASK))
+		/* Part of a 2 packets-command */
+		key = -key;
+
+	return key;
+}
+
+static void key_up(struct hid_device *hid, struct appleir *appleir, int key)
+{
+	input_report_key(appleir->input_dev, key, 0);
+	input_sync(appleir->input_dev);
+}
+
+static void key_down(struct hid_device *hid, struct appleir *appleir, int key)
+{
+	input_report_key(appleir->input_dev, key, 1);
+	input_sync(appleir->input_dev);
+}
+
+static void battery_flat(struct appleir *appleir)
+{
+	dev_err(&appleir->input_dev->dev, "possible flat battery?\n");
+}
+
+static void key_up_tick(unsigned long data)
+{
+	struct appleir *appleir = (struct appleir *)data;
+	struct hid_device *hid = appleir->hid;
+	unsigned long flags;
+
+	spin_lock_irqsave(&appleir->lock, flags);
+	if (appleir->current_key) {
+		key_up(hid, appleir, appleir->current_key);
+		appleir->current_key = 0;
+	}
+	spin_unlock_irqrestore(&appleir->lock, flags);
+}
+
+static int appleir_raw_event(struct hid_device *hid, struct hid_report *report,
+	 u8 *data, int len)
+{
+	struct appleir *appleir = hid_get_drvdata(hid);
+	static const u8 keydown[] = { 0x25, 0x87, 0xee };
+	static const u8 keyrepeat[] = { 0x26, };
+	static const u8 flatbattery[] = { 0x25, 0x87, 0xe0 };
+	unsigned long flags;
+
+	if (len != 5)
+		goto out;
+
+	if (!memcmp(data, keydown, sizeof(keydown))) {
+		int index;
+
+		spin_lock_irqsave(&appleir->lock, flags);
+		/*
+		 * If we already have a key down, take it up before marking
+		 * this one down
+		 */
+		if (appleir->current_key)
+			key_up(hid, appleir, appleir->current_key);
+
+		/* Handle dual packet commands */
+		if (appleir->prev_key_idx > 0)
+			index = appleir->prev_key_idx;
+		else
+			index = get_key(data[4]);
+
+		if (index >= 0) {
+			appleir->current_key = appleir->keymap[index];
+
+			key_down(hid, appleir, appleir->current_key);
+			/*
+			 * Remote doesn't do key up, either pull them up, in
+			 * the test above, or here set a timer which pulls
+			 * them up after 1/8 s
+			 */
+			mod_timer(&appleir->key_up_timer, jiffies + HZ / 8);
+			appleir->prev_key_idx = 0;
+		} else
+			/* Remember key for next packet */
+			appleir->prev_key_idx = -index;
+		spin_unlock_irqrestore(&appleir->lock, flags);
+		goto out;
+	}
+
+	appleir->prev_key_idx = 0;
+
+	if (!memcmp(data, keyrepeat, sizeof(keyrepeat))) {
+		key_down(hid, appleir, appleir->current_key);
+		/*
+		 * Remote doesn't do key up, either pull them up, in the test
+		 * above, or here set a timer which pulls them up after 1/8 s
+		 */
+		mod_timer(&appleir->key_up_timer, jiffies + HZ / 8);
+		goto out;
+	}
+
+	if (!memcmp(data, flatbattery, sizeof(flatbattery))) {
+		battery_flat(appleir);
+		/* Fall through */
+	}
+
+out:
+	/* let hidraw and hiddev handle the report */
+	return 0;
+}
+
+static void appleir_input_configured(struct hid_device *hid,
+		struct hid_input *hidinput)
+{
+	struct input_dev *input_dev = hidinput->input;
+	struct appleir *appleir = hid_get_drvdata(hid);
+	int i;
+
+	appleir->input_dev = input_dev;
+
+	input_dev->keycode = appleir->keymap;
+	input_dev->keycodesize = sizeof(unsigned short);
+	input_dev->keycodemax = ARRAY_SIZE(appleir->keymap);
+
+	input_dev->evbit[0] = BIT(EV_KEY) | BIT(EV_REP);
+
+	memcpy(appleir->keymap, appleir_key_table, sizeof(appleir->keymap));
+	for (i = 0; i < ARRAY_SIZE(appleir_key_table); i++)
+		set_bit(appleir->keymap[i], input_dev->keybit);
+	clear_bit(KEY_RESERVED, input_dev->keybit);
+}
+
+static int appleir_input_mapping(struct hid_device *hid,
+		struct hid_input *hi, struct hid_field *field,
+		struct hid_usage *usage, unsigned long **bit, int *max)
+{
+	return -1;
+}
+
+static int appleir_probe(struct hid_device *hid, const struct hid_device_id *id)
+{
+	int ret;
+	struct appleir *appleir;
+
+	appleir = kzalloc(sizeof(struct appleir), GFP_KERNEL);
+	if (!appleir) {
+		ret = -ENOMEM;
+		goto allocfail;
+	}
+
+	appleir->hid = hid;
+
+	spin_lock_init(&appleir->lock);
+	setup_timer(&appleir->key_up_timer,
+		    key_up_tick, (unsigned long) appleir);
+
+	hid_set_drvdata(hid, appleir);
+
+	ret = hid_parse(hid);
+	if (ret) {
+		hid_err(hid, "parse failed\n");
+		goto fail;
+	}
+
+	ret = hid_hw_start(hid, HID_CONNECT_DEFAULT | HID_CONNECT_HIDDEV_FORCE);
+	if (ret) {
+		hid_err(hid, "hw start failed\n");
+		goto fail;
+	}
+
+	return 0;
+fail:
+	kfree(appleir);
+allocfail:
+	return ret;
+}
+
+static void appleir_remove(struct hid_device *hid)
+{
+	struct appleir *appleir = hid_get_drvdata(hid);
+	hid_hw_stop(hid);
+	del_timer_sync(&appleir->key_up_timer);
+	kfree(appleir);
+}
+
+static const struct hid_device_id appleir_devices[] = {
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_IRCONTROL) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_IRCONTROL2) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_IRCONTROL3) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_IRCONTROL4) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_IRCONTROL5) },
+	{ }
+};
+MODULE_DEVICE_TABLE(hid, appleir_devices);
+
+static struct hid_driver appleir_driver = {
+	.name = "appleir",
+	.id_table = appleir_devices,
+	.raw_event = appleir_raw_event,
+	.input_configured = appleir_input_configured,
+	.probe = appleir_probe,
+	.remove = appleir_remove,
+	.input_mapping = appleir_input_mapping,
+};
+module_hid_driver(appleir_driver);
diff --git a/drivers/hid/hid-axff.c b/drivers/hid/hid-axff.c
index 62f0cee032b..64ab94a55aa 100644
--- a/drivers/hid/hid-axff.c
+++ b/drivers/hid/hid-axff.c
@@ -29,14 +29,12 @@
 
 #include <linux/input.h>
 #include <linux/slab.h>
-#include <linux/usb.h>
 #include <linux/hid.h>
 #include <linux/module.h>
 
 #include "hid-ids.h"
 
 #ifdef CONFIG_HID_ACRUX_FF
-#include "usbhid/usbhid.h"
 
 struct axff_device {
 	struct hid_report *report;
@@ -68,7 +66,7 @@ static int axff_play(struct input_dev *dev, void *data, struct ff_effect *effect
 	}
 
 	dbg_hid("running with 0x%02x 0x%02x", left, right);
-	usbhid_submit_report(hid, axff->report, USB_DIR_OUT);
+	hid_hw_request(hid, axff->report, HID_REQ_SET_REPORT);
 
 	return 0;
 }
@@ -114,7 +112,7 @@ static int axff_init(struct hid_device *hid)
 		goto err_free_mem;
 
 	axff->report = report;
-	usbhid_submit_report(hid, axff->report, USB_DIR_OUT);
+	hid_hw_request(hid, axff->report, HID_REQ_SET_REPORT);
 
 	hid_info(hid, "Force Feedback for ACRUX game controllers by Sergei Kolzun <x0r@dv-life.ru>\n");
 
diff --git a/drivers/hid/hid-core.c b/drivers/hid/hid-core.c
index 512b01c04ea..6961bbeab3e 100644
--- a/drivers/hid/hid-core.c
+++ b/drivers/hid/hid-core.c
@@ -728,8 +728,7 @@ static int hid_scan_report(struct hid_device *hid)
 		} else if (page == HID_UP_SENSOR &&
 			item.type == HID_ITEM_TYPE_MAIN &&
 			item.tag == HID_MAIN_ITEM_TAG_BEGIN_COLLECTION &&
-			(item_udata(&item) & 0xff) == HID_COLLECTION_PHYSICAL &&
-			(hid->bus == BUS_USB || hid->bus == BUS_I2C))
+			(item_udata(&item) & 0xff) == HID_COLLECTION_PHYSICAL)
 			hid->group = HID_GROUP_SENSOR_HUB;
 	}
 
@@ -1260,14 +1259,12 @@ int hid_input_report(struct hid_device *hid, int type, u8 *data, int size, int i
 	struct hid_report_enum *report_enum;
 	struct hid_driver *hdrv;
 	struct hid_report *report;
-	char *buf;
-	unsigned int i;
 	int ret = 0;
 
 	if (!hid)
 		return -ENODEV;
 
-	if (down_trylock(&hid->driver_lock))
+	if (down_trylock(&hid->driver_input_lock))
 		return -EBUSY;
 
 	if (!hid->driver) {
@@ -1284,28 +1281,9 @@ int hid_input_report(struct hid_device *hid, int type, u8 *data, int size, int i
 	}
 
 	/* Avoid unnecessary overhead if debugfs is disabled */
-	if (list_empty(&hid->debug_list))
-		goto nomem;
-
-	buf = kmalloc(sizeof(char) * HID_DEBUG_BUFSIZE, GFP_ATOMIC);
-
-	if (!buf)
-		goto nomem;
-
-	/* dump the report */
-	snprintf(buf, HID_DEBUG_BUFSIZE - 1,
-			"\nreport (size %u) (%snumbered) = ", size, report_enum->numbered ? "" : "un");
-	hid_debug_event(hid, buf);
-
-	for (i = 0; i < size; i++) {
-		snprintf(buf, HID_DEBUG_BUFSIZE - 1,
-				" %02x", data[i]);
-		hid_debug_event(hid, buf);
-	}
-	hid_debug_event(hid, "\n");
-	kfree(buf);
+	if (!list_empty(&hid->debug_list))
+		hid_dump_report(hid, type, data, size);
 
-nomem:
 	report = hid_get_report(report_enum, data);
 
 	if (!report) {
@@ -1324,7 +1302,7 @@ nomem:
 	ret = hid_report_raw_event(hid, type, data, size, interrupt);
 
 unlock:
-	up(&hid->driver_lock);
+	up(&hid->driver_input_lock);
 	return ret;
 }
 EXPORT_SYMBOL_GPL(hid_input_report);
@@ -1502,8 +1480,6 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_A4TECH, USB_DEVICE_ID_A4TECH_X5_005D) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_A4TECH, USB_DEVICE_ID_A4TECH_RP_649) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ACRUX, 0x0802) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ATV_IRCONTROL) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_IRCONTROL4) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_MIGHTYMOUSE) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_MAGICMOUSE) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_MAGICTRACKPAD) },
@@ -1527,6 +1503,11 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_GEYSER4_HF_ANSI) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_GEYSER4_HF_ISO) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_GEYSER4_HF_JIS) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_IRCONTROL) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_IRCONTROL2) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_IRCONTROL3) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_IRCONTROL4) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_IRCONTROL5) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ALU_WIRELESS_ANSI) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ALU_WIRELESS_ISO) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ALU_WIRELESS_JIS) },
@@ -1654,6 +1635,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_COMFORT_MOUSE_4500) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_SIDEWINDER_GV) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_NE4K) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_NE4K_JP) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_LK6K) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_PRESENTER_8K_USB) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_DIGITAL_MEDIA_3K) },
@@ -1687,6 +1669,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_ARVO) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_ISKU) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_KONEPLUS) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_KONEPURE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_KOVAPLUS) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_LUA) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_PYRA_WIRED) },
@@ -1747,6 +1730,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_PRESENTER_8K_BT) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_NINTENDO, USB_DEVICE_ID_NINTENDO_WIIMOTE) },
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_NINTENDO, USB_DEVICE_ID_NINTENDO_WIIMOTE2) },
 	{ }
 };
 
@@ -1845,6 +1829,11 @@ static int hid_device_probe(struct device *dev)
 
 	if (down_interruptible(&hdev->driver_lock))
 		return -EINTR;
+	if (down_interruptible(&hdev->driver_input_lock)) {
+		ret = -EINTR;
+		goto unlock_driver_lock;
+	}
+	hdev->io_started = false;
 
 	if (!hdev->driver) {
 		id = hid_match_device(hdev, hdrv);
@@ -1867,6 +1856,9 @@ static int hid_device_probe(struct device *dev)
 		}
 	}
 unlock:
+	if (!hdev->io_started)
+		up(&hdev->driver_input_lock);
+unlock_driver_lock:
 	up(&hdev->driver_lock);
 	return ret;
 }
@@ -1875,9 +1867,15 @@ static int hid_device_remove(struct device *dev)
 {
 	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
 	struct hid_driver *hdrv;
+	int ret = 0;
 
 	if (down_interruptible(&hdev->driver_lock))
 		return -EINTR;
+	if (down_interruptible(&hdev->driver_input_lock)) {
+		ret = -EINTR;
+		goto unlock_driver_lock;
+	}
+	hdev->io_started = false;
 
 	hdrv = hdev->driver;
 	if (hdrv) {
@@ -1889,8 +1887,11 @@ static int hid_device_remove(struct device *dev)
 		hdev->driver = NULL;
 	}
 
+	if (!hdev->io_started)
+		up(&hdev->driver_input_lock);
+unlock_driver_lock:
 	up(&hdev->driver_lock);
-	return 0;
+	return ret;
 }
 
 static ssize_t modalias_show(struct device *dev, struct device_attribute *a,
@@ -2077,7 +2078,6 @@ static const struct hid_device_id hid_ignore_list[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LD, USB_DEVICE_ID_LD_HYBRID) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LD, USB_DEVICE_ID_LD_HEATCONTROL) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MADCATZ, USB_DEVICE_ID_MADCATZ_BEATPAD) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_MASTERKIT, USB_DEVICE_ID_MASTERKIT_MA901RADIO) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MCC, USB_DEVICE_ID_MCC_PMD1024LS) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MCC, USB_DEVICE_ID_MCC_PMD1208LS) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROCHIP, USB_DEVICE_ID_PICKIT1) },
@@ -2244,6 +2244,18 @@ bool hid_ignore(struct hid_device *hdev)
 		     hdev->product <= USB_DEVICE_ID_VELLEMAN_K8061_LAST))
 			return true;
 		break;
+	case USB_VENDOR_ID_ATMEL_V_USB:
+		/* Masterkit MA901 usb radio based on Atmel tiny85 chip and
+		 * it has the same USB ID as many Atmel V-USB devices. This
+		 * usb radio is handled by radio-ma901.c driver so we want
+		 * ignore the hid. Check the name, bus, product and ignore
+		 * if we have MA901 usb radio.
+		 */
+		if (hdev->product == USB_DEVICE_ID_ATMEL_V_USB &&
+			hdev->bus == BUS_USB &&
+			strncmp(hdev->name, "www.masterkit.ru MA901", 22) == 0)
+			return true;
+		break;
 	}
 
 	if (hdev->type == HID_TYPE_USBMOUSE &&
@@ -2329,7 +2341,9 @@ struct hid_device *hid_allocate_device(void)
 
 	init_waitqueue_head(&hdev->debug_wait);
 	INIT_LIST_HEAD(&hdev->debug_list);
+	mutex_init(&hdev->debug_list_lock);
 	sema_init(&hdev->driver_lock, 1);
+	sema_init(&hdev->driver_input_lock, 1);
 
 	return hdev;
 }
diff --git a/drivers/hid/hid-debug.c b/drivers/hid/hid-debug.c
index 933fff0fff1..7e56cb3855e 100644
--- a/drivers/hid/hid-debug.c
+++ b/drivers/hid/hid-debug.c
@@ -580,17 +580,49 @@ void hid_debug_event(struct hid_device *hdev, char *buf)
 	int i;
 	struct hid_debug_list *list;
 
+	mutex_lock(&hdev->debug_list_lock);
 	list_for_each_entry(list, &hdev->debug_list, node) {
 		for (i = 0; i < strlen(buf); i++)
 			list->hid_debug_buf[(list->tail + i) % HID_DEBUG_BUFSIZE] =
 				buf[i];
 		list->tail = (list->tail + i) % HID_DEBUG_BUFSIZE;
         }
+	mutex_unlock(&hdev->debug_list_lock);
 
 	wake_up_interruptible(&hdev->debug_wait);
 }
 EXPORT_SYMBOL_GPL(hid_debug_event);
 
+void hid_dump_report(struct hid_device *hid, int type, u8 *data,
+		int size)
+{
+	struct hid_report_enum *report_enum;
+	char *buf;
+	unsigned int i;
+
+	buf = kmalloc(sizeof(char) * HID_DEBUG_BUFSIZE, GFP_ATOMIC);
+
+	if (!buf)
+		return;
+
+	report_enum = hid->report_enum + type;
+
+	/* dump the report */
+	snprintf(buf, HID_DEBUG_BUFSIZE - 1,
+			"\nreport (size %u) (%snumbered) = ", size,
+			report_enum->numbered ? "" : "un");
+	hid_debug_event(hid, buf);
+
+	for (i = 0; i < size; i++) {
+		snprintf(buf, HID_DEBUG_BUFSIZE - 1,
+				" %02x", data[i]);
+		hid_debug_event(hid, buf);
+	}
+	hid_debug_event(hid, "\n");
+	kfree(buf);
+}
+EXPORT_SYMBOL_GPL(hid_dump_report);
+
 void hid_dump_input(struct hid_device *hdev, struct hid_usage *usage, __s32 value)
 {
 	char *buf;
@@ -960,7 +992,9 @@ static int hid_debug_events_open(struct inode *inode, struct file *file)
 	file->private_data = list;
 	mutex_init(&list->read_mutex);
 
+	mutex_lock(&list->hdev->debug_list_lock);
 	list_add_tail(&list->node, &list->hdev->debug_list);
+	mutex_unlock(&list->hdev->debug_list_lock);
 
 out:
 	return err;
@@ -1055,7 +1089,9 @@ static int hid_debug_events_release(struct inode *inode, struct file *file)
 {
 	struct hid_debug_list *list = file->private_data;
 
+	mutex_lock(&list->hdev->debug_list_lock);
 	list_del(&list->node);
+	mutex_unlock(&list->hdev->debug_list_lock);
 	kfree(list->hid_debug_buf);
 	kfree(list);
 
diff --git a/drivers/hid/hid-dr.c b/drivers/hid/hid-dr.c
index 0fe8f65ef01..ce0644424f5 100644
--- a/drivers/hid/hid-dr.c
+++ b/drivers/hid/hid-dr.c
@@ -29,14 +29,12 @@
 
 #include <linux/input.h>
 #include <linux/slab.h>
-#include <linux/usb.h>
 #include <linux/hid.h>
 #include <linux/module.h>
 
 #include "hid-ids.h"
 
 #ifdef CONFIG_DRAGONRISE_FF
-#include "usbhid/usbhid.h"
 
 struct drff_device {
 	struct hid_report *report;
@@ -68,7 +66,7 @@ static int drff_play(struct input_dev *dev, void *data,
 		drff->report->field[0]->value[1] = 0x00;
 		drff->report->field[0]->value[2] = weak;
 		drff->report->field[0]->value[4] = strong;
-		usbhid_submit_report(hid, drff->report, USB_DIR_OUT);
+		hid_hw_request(hid, drff->report, HID_REQ_SET_REPORT);
 
 		drff->report->field[0]->value[0] = 0xfa;
 		drff->report->field[0]->value[1] = 0xfe;
@@ -80,7 +78,7 @@ static int drff_play(struct input_dev *dev, void *data,
 	drff->report->field[0]->value[2] = 0x00;
 	drff->report->field[0]->value[4] = 0x00;
 	dbg_hid("running with 0x%02x 0x%02x", strong, weak);
-	usbhid_submit_report(hid, drff->report, USB_DIR_OUT);
+	hid_hw_request(hid, drff->report, HID_REQ_SET_REPORT);
 
 	return 0;
 }
@@ -132,7 +130,7 @@ static int drff_init(struct hid_device *hid)
 	drff->report->field[0]->value[4] = 0x00;
 	drff->report->field[0]->value[5] = 0x00;
 	drff->report->field[0]->value[6] = 0x00;
-	usbhid_submit_report(hid, drff->report, USB_DIR_OUT);
+	hid_hw_request(hid, drff->report, HID_REQ_SET_REPORT);
 
 	hid_info(hid, "Force Feedback for DragonRise Inc. "
 		 "game controllers by Richard Walmsley <richwalm@gmail.com>\n");
diff --git a/drivers/hid/hid-emsff.c b/drivers/hid/hid-emsff.c
index 2e093ab99b4..d82d75bb11f 100644
--- a/drivers/hid/hid-emsff.c
+++ b/drivers/hid/hid-emsff.c
@@ -23,11 +23,9 @@
 
 #include <linux/hid.h>
 #include <linux/input.h>
-#include <linux/usb.h>
 #include <linux/module.h>
 
 #include "hid-ids.h"
-#include "usbhid/usbhid.h"
 
 struct emsff_device {
 	struct hid_report *report;
@@ -52,7 +50,7 @@ static int emsff_play(struct input_dev *dev, void *data,
 	emsff->report->field[0]->value[2] = strong;
 
 	dbg_hid("running with 0x%02x 0x%02x\n", strong, weak);
-	usbhid_submit_report(hid, emsff->report, USB_DIR_OUT);
+	hid_hw_request(hid, emsff->report, HID_REQ_SET_REPORT);
 
 	return 0;
 }
@@ -104,7 +102,7 @@ static int emsff_init(struct hid_device *hid)
 	emsff->report->field[0]->value[4] = 0x00;
 	emsff->report->field[0]->value[5] = 0x00;
 	emsff->report->field[0]->value[6] = 0x00;
-	usbhid_submit_report(hid, emsff->report, USB_DIR_OUT);
+	hid_hw_request(hid, emsff->report, HID_REQ_SET_REPORT);
 
 	hid_info(hid, "force feedback for EMS based devices by Ignaz Forster <ignaz.forster@gmx.de>\n");
 
diff --git a/drivers/hid/hid-gaff.c b/drivers/hid/hid-gaff.c
index 04d2e6aca77..2d8cead3adc 100644
--- a/drivers/hid/hid-gaff.c
+++ b/drivers/hid/hid-gaff.c
@@ -29,13 +29,11 @@
 
 #include <linux/input.h>
 #include <linux/slab.h>
-#include <linux/usb.h>
 #include <linux/hid.h>
 #include <linux/module.h>
 #include "hid-ids.h"
 
 #ifdef CONFIG_GREENASIA_FF
-#include "usbhid/usbhid.h"
 
 struct gaff_device {
 	struct hid_report *report;
@@ -63,14 +61,14 @@ static int hid_gaff_play(struct input_dev *dev, void *data,
 	gaff->report->field[0]->value[4] = left;
 	gaff->report->field[0]->value[5] = 0;
 	dbg_hid("running with 0x%02x 0x%02x", left, right);
-	usbhid_submit_report(hid, gaff->report, USB_DIR_OUT);
+	hid_hw_request(hid, gaff->report, HID_REQ_SET_REPORT);
 
 	gaff->report->field[0]->value[0] = 0xfa;
 	gaff->report->field[0]->value[1] = 0xfe;
 	gaff->report->field[0]->value[2] = 0x0;
 	gaff->report->field[0]->value[4] = 0x0;
 
-	usbhid_submit_report(hid, gaff->report, USB_DIR_OUT);
+	hid_hw_request(hid, gaff->report, HID_REQ_SET_REPORT);
 
 	return 0;
 }
@@ -122,12 +120,12 @@ static int gaff_init(struct hid_device *hid)
 	gaff->report->field[0]->value[1] = 0x00;
 	gaff->report->field[0]->value[2] = 0x00;
 	gaff->report->field[0]->value[3] = 0x00;
-	usbhid_submit_report(hid, gaff->report, USB_DIR_OUT);
+	hid_hw_request(hid, gaff->report, HID_REQ_SET_REPORT);
 
 	gaff->report->field[0]->value[0] = 0xfa;
 	gaff->report->field[0]->value[1] = 0xfe;
 
-	usbhid_submit_report(hid, gaff->report, USB_DIR_OUT);
+	hid_hw_request(hid, gaff->report, HID_REQ_SET_REPORT);
 
 	hid_info(hid, "Force Feedback for GreenAsia 0x12 devices by Lukasz Lubojanski <lukasz@lubojanski.info>\n");
 
diff --git a/drivers/hid/hid-holtekff.c b/drivers/hid/hid-holtekff.c
index f34d1186a3e..9a8f0512452 100644
--- a/drivers/hid/hid-holtekff.c
+++ b/drivers/hid/hid-holtekff.c
@@ -27,12 +27,10 @@
 #include <linux/input.h>
 #include <linux/module.h>
 #include <linux/slab.h>
-#include <linux/usb.h>
 
 #include "hid-ids.h"
 
 #ifdef CONFIG_HOLTEK_FF
-#include "usbhid/usbhid.h"
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Anssi Hannula <anssi.hannula@iki.fi>");
@@ -102,7 +100,7 @@ static void holtekff_send(struct holtekff_device *holtekff,
 
 	dbg_hid("sending %*ph\n", 7, data);
 
-	usbhid_submit_report(hid, holtekff->field->report, USB_DIR_OUT);
+	hid_hw_request(hid, holtekff->field->report, HID_REQ_SET_REPORT);
 }
 
 static int holtekff_play(struct input_dev *dev, void *data,
diff --git a/drivers/hid/hid-icade.c b/drivers/hid/hid-icade.c
index 09dcc04595f..76b5a757078 100644
--- a/drivers/hid/hid-icade.c
+++ b/drivers/hid/hid-icade.c
@@ -159,7 +159,7 @@ static const struct icade_key icade_usage_table[30] = {
 
 static const struct icade_key *icade_find_translation(u16 from)
 {
-	if (from < 0 || from > ICADE_MAX_USAGE)
+	if (from > ICADE_MAX_USAGE)
 		return NULL;
 	return &icade_usage_table[from];
 }
diff --git a/drivers/hid/hid-ids.h b/drivers/hid/hid-ids.h
index 92e47e5c956..38535c9243d 100644
--- a/drivers/hid/hid-ids.h
+++ b/drivers/hid/hid-ids.h
@@ -137,8 +137,11 @@
 #define USB_DEVICE_ID_APPLE_ALU_WIRELESS_2011_ISO   0x0256
 #define USB_DEVICE_ID_APPLE_FOUNTAIN_TP_ONLY	0x030a
 #define USB_DEVICE_ID_APPLE_GEYSER1_TP_ONLY	0x030b
-#define USB_DEVICE_ID_APPLE_ATV_IRCONTROL	0x8241
+#define USB_DEVICE_ID_APPLE_IRCONTROL	0x8240
+#define USB_DEVICE_ID_APPLE_IRCONTROL2	0x1440
+#define USB_DEVICE_ID_APPLE_IRCONTROL3	0x8241
 #define USB_DEVICE_ID_APPLE_IRCONTROL4	0x8242
+#define USB_DEVICE_ID_APPLE_IRCONTROL5	0x8243
 
 #define USB_VENDOR_ID_ASUS		0x0486
 #define USB_DEVICE_ID_ASUS_T91MT	0x0185
@@ -158,6 +161,8 @@
 #define USB_VENDOR_ID_ATMEL		0x03eb
 #define USB_DEVICE_ID_ATMEL_MULTITOUCH	0x211c
 #define USB_DEVICE_ID_ATMEL_MXT_DIGITIZER	0x2118
+#define USB_VENDOR_ID_ATMEL_V_USB	0x16c0
+#define USB_DEVICE_ID_ATMEL_V_USB	0x05df
 
 #define USB_VENDOR_ID_AUREAL		0x0755
 #define USB_DEVICE_ID_AUREAL_W01RN	0x2626
@@ -557,9 +562,6 @@
 #define USB_VENDOR_ID_MADCATZ		0x0738
 #define USB_DEVICE_ID_MADCATZ_BEATPAD	0x4540
 
-#define USB_VENDOR_ID_MASTERKIT			0x16c0
-#define USB_DEVICE_ID_MASTERKIT_MA901RADIO	0x05df
-
 #define USB_VENDOR_ID_MCC		0x09db
 #define USB_DEVICE_ID_MCC_PMD1024LS	0x0076
 #define USB_DEVICE_ID_MCC_PMD1208LS	0x007a
@@ -578,6 +580,7 @@
 #define USB_DEVICE_ID_SIDEWINDER_GV	0x003b
 #define USB_DEVICE_ID_WIRELESS_OPTICAL_DESKTOP_3_0 0x009d
 #define USB_DEVICE_ID_MS_NE4K		0x00db
+#define USB_DEVICE_ID_MS_NE4K_JP	0x00dc
 #define USB_DEVICE_ID_MS_LK6K		0x00f9
 #define USB_DEVICE_ID_MS_PRESENTER_8K_BT	0x0701
 #define USB_DEVICE_ID_MS_PRESENTER_8K_USB	0x0713
@@ -590,6 +593,9 @@
 #define USB_VENDOR_ID_MONTEREY		0x0566
 #define USB_DEVICE_ID_GENIUS_KB29E	0x3004
 
+#define USB_VENDOR_ID_MSI		0x1770
+#define USB_DEVICE_ID_MSI_GX680R_LED_PANEL	0xff00
+
 #define USB_VENDOR_ID_NATIONAL_SEMICONDUCTOR 0x0400
 #define USB_DEVICE_ID_N_S_HARMONY	0xc359
 
@@ -611,6 +617,7 @@
 
 #define USB_VENDOR_ID_NINTENDO		0x057e
 #define USB_DEVICE_ID_NINTENDO_WIIMOTE	0x0306
+#define USB_DEVICE_ID_NINTENDO_WIIMOTE2	0x0330
 
 #define USB_VENDOR_ID_NOVATEK		0x0603
 #define USB_DEVICE_ID_NOVATEK_PCT	0x0600
@@ -684,11 +691,16 @@
 #define USB_DEVICE_ID_QUANTA_OPTICAL_TOUCH_3001		0x3001
 #define USB_DEVICE_ID_QUANTA_OPTICAL_TOUCH_3008		0x3008
 
+#define USB_VENDOR_ID_REALTEK		0x0bda
+#define USB_DEVICE_ID_REALTEK_READER	0x0152
+
 #define USB_VENDOR_ID_ROCCAT		0x1e7d
 #define USB_DEVICE_ID_ROCCAT_ARVO	0x30d4
 #define USB_DEVICE_ID_ROCCAT_ISKU	0x319c
+#define USB_DEVICE_ID_ROCCAT_ISKUFX	0x3264
 #define USB_DEVICE_ID_ROCCAT_KONE	0x2ced
 #define USB_DEVICE_ID_ROCCAT_KONEPLUS	0x2d51
+#define USB_DEVICE_ID_ROCCAT_KONEPURE	0x2dbe
 #define USB_DEVICE_ID_ROCCAT_KONEXTD	0x2e22
 #define USB_DEVICE_ID_ROCCAT_KOVAPLUS	0x2d50
 #define USB_DEVICE_ID_ROCCAT_LUA	0x2c2e
diff --git a/drivers/hid/hid-input.c b/drivers/hid/hid-input.c
index 21b196c394b..945b8158ec4 100644
--- a/drivers/hid/hid-input.c
+++ b/drivers/hid/hid-input.c
@@ -1198,6 +1198,67 @@ static struct hid_input *hidinput_allocate(struct hid_device *hid)
 	return hidinput;
 }
 
+static bool hidinput_has_been_populated(struct hid_input *hidinput)
+{
+	int i;
+	unsigned long r = 0;
+
+	for (i = 0; i < BITS_TO_LONGS(EV_CNT); i++)
+		r |= hidinput->input->evbit[i];
+
+	for (i = 0; i < BITS_TO_LONGS(KEY_CNT); i++)
+		r |= hidinput->input->keybit[i];
+
+	for (i = 0; i < BITS_TO_LONGS(REL_CNT); i++)
+		r |= hidinput->input->relbit[i];
+
+	for (i = 0; i < BITS_TO_LONGS(ABS_CNT); i++)
+		r |= hidinput->input->absbit[i];
+
+	for (i = 0; i < BITS_TO_LONGS(MSC_CNT); i++)
+		r |= hidinput->input->mscbit[i];
+
+	for (i = 0; i < BITS_TO_LONGS(LED_CNT); i++)
+		r |= hidinput->input->ledbit[i];
+
+	for (i = 0; i < BITS_TO_LONGS(SND_CNT); i++)
+		r |= hidinput->input->sndbit[i];
+
+	for (i = 0; i < BITS_TO_LONGS(FF_CNT); i++)
+		r |= hidinput->input->ffbit[i];
+
+	for (i = 0; i < BITS_TO_LONGS(SW_CNT); i++)
+		r |= hidinput->input->swbit[i];
+
+	return !!r;
+}
+
+static void hidinput_cleanup_hidinput(struct hid_device *hid,
+		struct hid_input *hidinput)
+{
+	struct hid_report *report;
+	int i, k;
+
+	list_del(&hidinput->list);
+	input_free_device(hidinput->input);
+
+	for (k = HID_INPUT_REPORT; k <= HID_OUTPUT_REPORT; k++) {
+		if (k == HID_OUTPUT_REPORT &&
+			hid->quirks & HID_QUIRK_SKIP_OUTPUT_REPORTS)
+			continue;
+
+		list_for_each_entry(report, &hid->report_enum[k].report_list,
+				    list) {
+
+			for (i = 0; i < report->maxfield; i++)
+				if (report->field[i]->hidinput == hidinput)
+					report->field[i]->hidinput = NULL;
+		}
+	}
+
+	kfree(hidinput);
+}
+
 /*
  * Register the input device; print a message.
  * Configure the input layer interface
@@ -1249,6 +1310,10 @@ int hidinput_connect(struct hid_device *hid, unsigned int force)
 					hidinput_configure_usage(hidinput, report->field[i],
 								 report->field[i]->usage + j);
 
+			if ((hid->quirks & HID_QUIRK_NO_EMPTY_INPUT) &&
+			    !hidinput_has_been_populated(hidinput))
+				continue;
+
 			if (hid->quirks & HID_QUIRK_MULTI_INPUT) {
 				/* This will leave hidinput NULL, so that it
 				 * allocates another one if we have more inputs on
@@ -1265,6 +1330,18 @@ int hidinput_connect(struct hid_device *hid, unsigned int force)
 		}
 	}
 
+	if (hidinput && (hid->quirks & HID_QUIRK_NO_EMPTY_INPUT) &&
+	    !hidinput_has_been_populated(hidinput)) {
+		/* no need to register an input device not populated */
+		hidinput_cleanup_hidinput(hid, hidinput);
+		hidinput = NULL;
+	}
+
+	if (list_empty(&hid->inputs)) {
+		hid_err(hid, "No inputs registered, leaving\n");
+		goto out_unwind;
+	}
+
 	if (hidinput) {
 		if (drv->input_configured)
 			drv->input_configured(hid, hidinput);
diff --git a/drivers/hid/hid-kye.c b/drivers/hid/hid-kye.c
index ef72daecfa1..6af90dbdc3d 100644
--- a/drivers/hid/hid-kye.c
+++ b/drivers/hid/hid-kye.c
@@ -16,8 +16,6 @@
 #include <linux/device.h>
 #include <linux/hid.h>
 #include <linux/module.h>
-#include <linux/usb.h>
-#include "usbhid/usbhid.h"
 
 #include "hid-ids.h"
 
@@ -361,7 +359,7 @@ static int kye_tablet_enable(struct hid_device *hdev)
 	value[4] = 0x00;
 	value[5] = 0x00;
 	value[6] = 0x00;
-	usbhid_submit_report(hdev, report, USB_DIR_OUT);
+	hid_hw_request(hdev, report, HID_REQ_SET_REPORT);
 
 	return 0;
 }
diff --git a/drivers/hid/hid-lenovo-tpkbd.c b/drivers/hid/hid-lenovo-tpkbd.c
index 956c3b135f6..07837f5a4eb 100644
--- a/drivers/hid/hid-lenovo-tpkbd.c
+++ b/drivers/hid/hid-lenovo-tpkbd.c
@@ -68,7 +68,7 @@ static int tpkbd_features_set(struct hid_device *hdev)
 	report->field[2]->value[0] = data_pointer->sensitivity;
 	report->field[3]->value[0] = data_pointer->press_speed;
 
-	usbhid_submit_report(hdev, report, USB_DIR_OUT);
+	hid_hw_request(hdev, report, HID_REQ_SET_REPORT);
 	return 0;
 }
 
@@ -228,8 +228,6 @@ static ssize_t pointer_press_speed_show(struct device *dev,
 	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
 	struct tpkbd_data_pointer *data_pointer = hid_get_drvdata(hdev);
 
-	data_pointer = hid_get_drvdata(hdev);
-
 	return snprintf(buf, PAGE_SIZE, "%u\n",
 		data_pointer->press_speed);
 }
@@ -332,7 +330,7 @@ static void tpkbd_led_brightness_set(struct led_classdev *led_cdev,
 	report = hdev->report_enum[HID_OUTPUT_REPORT].report_id_hash[3];
 	report->field[0]->value[0] = (data_pointer->led_state >> 0) & 1;
 	report->field[0]->value[1] = (data_pointer->led_state >> 1) & 1;
-	usbhid_submit_report(hdev, report, USB_DIR_OUT);
+	hid_hw_request(hdev, report, HID_REQ_SET_REPORT);
 }
 
 static int tpkbd_probe_tp(struct hid_device *hdev)
diff --git a/drivers/hid/hid-lg2ff.c b/drivers/hid/hid-lg2ff.c
index 3c31bc650e5..b3cd1507dda 100644
--- a/drivers/hid/hid-lg2ff.c
+++ b/drivers/hid/hid-lg2ff.c
@@ -23,10 +23,8 @@
 
 #include <linux/input.h>
 #include <linux/slab.h>
-#include <linux/usb.h>
 #include <linux/hid.h>
 
-#include "usbhid/usbhid.h"
 #include "hid-lg.h"
 
 struct lg2ff_device {
@@ -56,7 +54,7 @@ static int play_effect(struct input_dev *dev, void *data,
 		lg2ff->report->field[0]->value[4] = 0x00;
 	}
 
-	usbhid_submit_report(hid, lg2ff->report, USB_DIR_OUT);
+	hid_hw_request(hid, lg2ff->report, HID_REQ_SET_REPORT);
 	return 0;
 }
 
@@ -108,7 +106,7 @@ int lg2ff_init(struct hid_device *hid)
 	report->field[0]->value[5] = 0x00;
 	report->field[0]->value[6] = 0x00;
 
-	usbhid_submit_report(hid, report, USB_DIR_OUT);
+	hid_hw_request(hid, report, HID_REQ_SET_REPORT);
 
 	hid_info(hid, "Force feedback for Logitech RumblePad/Rumblepad 2 by Anssi Hannula <anssi.hannula@gmail.com>\n");
 
diff --git a/drivers/hid/hid-lg3ff.c b/drivers/hid/hid-lg3ff.c
index f98644c26c1..e52f181f6aa 100644
--- a/drivers/hid/hid-lg3ff.c
+++ b/drivers/hid/hid-lg3ff.c
@@ -22,10 +22,8 @@
 
 
 #include <linux/input.h>
-#include <linux/usb.h>
 #include <linux/hid.h>
 
-#include "usbhid/usbhid.h"
 #include "hid-lg.h"
 
 /*
@@ -92,7 +90,7 @@ static int hid_lg3ff_play(struct input_dev *dev, void *data,
 		report->field[0]->value[1] = (unsigned char)(-x);
 		report->field[0]->value[31] = (unsigned char)(-y);
 
-		usbhid_submit_report(hid, report, USB_DIR_OUT);
+		hid_hw_request(hid, report, HID_REQ_SET_REPORT);
 		break;
 	}
 	return 0;
@@ -118,7 +116,7 @@ static void hid_lg3ff_set_autocenter(struct input_dev *dev, u16 magnitude)
 	report->field[0]->value[33] = 0x7F;
 	report->field[0]->value[34] = 0x7F;
 
-	usbhid_submit_report(hid, report, USB_DIR_OUT);
+	hid_hw_request(hid, report, HID_REQ_SET_REPORT);
 }
 
 
diff --git a/drivers/hid/hid-lg4ff.c b/drivers/hid/hid-lg4ff.c
index 65a6ec8d374..0ddae2a00d5 100644
--- a/drivers/hid/hid-lg4ff.c
+++ b/drivers/hid/hid-lg4ff.c
@@ -34,6 +34,7 @@
 
 #define DFGT_REV_MAJ 0x13
 #define DFGT_REV_MIN 0x22
+#define DFGT2_REV_MIN 0x26
 #define DFP_REV_MAJ 0x11
 #define DFP_REV_MIN 0x06
 #define FFEX_REV_MAJ 0x21
@@ -125,6 +126,7 @@ static const struct lg4ff_native_cmd native_g27 = {
 
 static const struct lg4ff_usb_revision lg4ff_revs[] = {
 	{DFGT_REV_MAJ, DFGT_REV_MIN, &native_dfgt},	/* Driving Force GT */
+	{DFGT_REV_MAJ, DFGT2_REV_MIN, &native_dfgt},	/* Driving Force GT v2 */
 	{DFP_REV_MAJ,  DFP_REV_MIN,  &native_dfp},	/* Driving Force Pro */
 	{G25_REV_MAJ,  G25_REV_MIN,  &native_g25},	/* G25 */
 	{G27_REV_MAJ,  G27_REV_MIN,  &native_g27},	/* G27 */
@@ -202,7 +204,7 @@ static int hid_lg4ff_play(struct input_dev *dev, void *data, struct ff_effect *e
 		value[5] = 0x00;
 		value[6] = 0x00;
 
-		usbhid_submit_report(hid, report, USB_DIR_OUT);
+		hid_hw_request(hid, report, HID_REQ_SET_REPORT);
 		break;
 	}
 	return 0;
@@ -225,7 +227,7 @@ static void hid_lg4ff_set_autocenter_default(struct input_dev *dev, u16 magnitud
 	value[5] = 0x00;
 	value[6] = 0x00;
 
-	usbhid_submit_report(hid, report, USB_DIR_OUT);
+	hid_hw_request(hid, report, HID_REQ_SET_REPORT);
 }
 
 /* Sends autocentering command compatible with Formula Force EX */
@@ -245,7 +247,7 @@ static void hid_lg4ff_set_autocenter_ffex(struct input_dev *dev, u16 magnitude)
 	value[5] = 0x00;
 	value[6] = 0x00;
 
-	usbhid_submit_report(hid, report, USB_DIR_OUT);
+	hid_hw_request(hid, report, HID_REQ_SET_REPORT);
 }
 
 /* Sends command to set range compatible with G25/G27/Driving Force GT */
@@ -265,7 +267,7 @@ static void hid_lg4ff_set_range_g25(struct hid_device *hid, u16 range)
 	value[5] = 0x00;
 	value[6] = 0x00;
 
-	usbhid_submit_report(hid, report, USB_DIR_OUT);
+	hid_hw_request(hid, report, HID_REQ_SET_REPORT);
 }
 
 /* Sends commands to set range compatible with Driving Force Pro wheel */
@@ -294,7 +296,7 @@ static void hid_lg4ff_set_range_dfp(struct hid_device *hid, __u16 range)
 		report->field[0]->value[1] = 0x02;
 		full_range = 200;
 	}
-	usbhid_submit_report(hid, report, USB_DIR_OUT);
+	hid_hw_request(hid, report, HID_REQ_SET_REPORT);
 
 	/* Prepare "fine" limit command */
 	value[0] = 0x81;
@@ -306,7 +308,7 @@ static void hid_lg4ff_set_range_dfp(struct hid_device *hid, __u16 range)
 	value[6] = 0x00;
 
 	if (range == 200 || range == 900) {	/* Do not apply any fine limit */
-		usbhid_submit_report(hid, report, USB_DIR_OUT);
+		hid_hw_request(hid, report, HID_REQ_SET_REPORT);
 		return;
 	}
 
@@ -320,7 +322,7 @@ static void hid_lg4ff_set_range_dfp(struct hid_device *hid, __u16 range)
 	value[5] = (start_right & 0xe) << 4 | (start_left & 0xe);
 	value[6] = 0xff;
 
-	usbhid_submit_report(hid, report, USB_DIR_OUT);
+	hid_hw_request(hid, report, HID_REQ_SET_REPORT);
 }
 
 static void hid_lg4ff_switch_native(struct hid_device *hid, const struct lg4ff_native_cmd *cmd)
@@ -334,7 +336,7 @@ static void hid_lg4ff_switch_native(struct hid_device *hid, const struct lg4ff_n
 		for (i = 0; i < 7; i++)
 			report->field[0]->value[i] = cmd->cmd[j++];
 
-		usbhid_submit_report(hid, report, USB_DIR_OUT);
+		hid_hw_request(hid, report, HID_REQ_SET_REPORT);
 	}
 }
 
@@ -410,7 +412,7 @@ static void lg4ff_set_leds(struct hid_device *hid, __u8 leds)
 	value[4] = 0x00;
 	value[5] = 0x00;
 	value[6] = 0x00;
-	usbhid_submit_report(hid, report, USB_DIR_OUT);
+	hid_hw_request(hid, report, HID_REQ_SET_REPORT);
 }
 
 static void lg4ff_led_set_brightness(struct led_classdev *led_cdev,
diff --git a/drivers/hid/hid-lgff.c b/drivers/hid/hid-lgff.c
index 27bc54f92f4..d7ea8c845b4 100644
--- a/drivers/hid/hid-lgff.c
+++ b/drivers/hid/hid-lgff.c
@@ -30,10 +30,8 @@
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <linux/input.h>
-#include <linux/usb.h>
 #include <linux/hid.h>
 
-#include "usbhid/usbhid.h"
 #include "hid-lg.h"
 
 struct dev_type {
@@ -89,7 +87,7 @@ static int hid_lgff_play(struct input_dev *dev, void *data, struct ff_effect *ef
 		report->field[0]->value[2] = x;
 		report->field[0]->value[3] = y;
 		dbg_hid("(x, y)=(%04x, %04x)\n", x, y);
-		usbhid_submit_report(hid, report, USB_DIR_OUT);
+		hid_hw_request(hid, report, HID_REQ_SET_REPORT);
 		break;
 
 	case FF_RUMBLE:
@@ -104,7 +102,7 @@ static int hid_lgff_play(struct input_dev *dev, void *data, struct ff_effect *ef
 		report->field[0]->value[2] = left;
 		report->field[0]->value[3] = right;
 		dbg_hid("(left, right)=(%04x, %04x)\n", left, right);
-		usbhid_submit_report(hid, report, USB_DIR_OUT);
+		hid_hw_request(hid, report, HID_REQ_SET_REPORT);
 		break;
 	}
 	return 0;
@@ -124,7 +122,7 @@ static void hid_lgff_set_autocenter(struct input_dev *dev, u16 magnitude)
 	*value++ = 0x80;
 	*value++ = 0x00;
 	*value = 0x00;
-	usbhid_submit_report(hid, report, USB_DIR_OUT);
+	hid_hw_request(hid, report, HID_REQ_SET_REPORT);
 }
 
 int lgff_init(struct hid_device* hid)
diff --git a/drivers/hid/hid-logitech-dj.c b/drivers/hid/hid-logitech-dj.c
index 8758f38c948..5207591a598 100644
--- a/drivers/hid/hid-logitech-dj.c
+++ b/drivers/hid/hid-logitech-dj.c
@@ -27,7 +27,6 @@
 #include <linux/module.h>
 #include <linux/usb.h>
 #include <asm/unaligned.h>
-#include "usbhid/usbhid.h"
 #include "hid-ids.h"
 #include "hid-logitech-dj.h"
 
@@ -193,7 +192,6 @@ static struct hid_ll_driver logi_dj_ll_driver;
 static int logi_dj_output_hidraw_report(struct hid_device *hid, u8 * buf,
 					size_t count,
 					unsigned char report_type);
-static int logi_dj_recv_query_paired_devices(struct dj_receiver_dev *djrcv_dev);
 
 static void logi_dj_recv_destroy_djhid_device(struct dj_receiver_dev *djrcv_dev,
 						struct dj_report *dj_report)
@@ -234,7 +232,6 @@ static void logi_dj_recv_add_djhid_device(struct dj_receiver_dev *djrcv_dev,
 	if (dj_report->report_params[DEVICE_PAIRED_PARAM_SPFUNCTION] &
 	    SPFUNCTION_DEVICE_LIST_EMPTY) {
 		dbg_hid("%s: device list is empty\n", __func__);
-		djrcv_dev->querying_devices = false;
 		return;
 	}
 
@@ -245,12 +242,6 @@ static void logi_dj_recv_add_djhid_device(struct dj_receiver_dev *djrcv_dev,
 		return;
 	}
 
-	if (djrcv_dev->paired_dj_devices[dj_report->device_index]) {
-		/* The device is already known. No need to reallocate it. */
-		dbg_hid("%s: device is already known\n", __func__);
-		return;
-	}
-
 	dj_hiddev = hid_allocate_device();
 	if (IS_ERR(dj_hiddev)) {
 		dev_err(&djrcv_hdev->dev, "%s: hid_allocate_device failed\n",
@@ -314,7 +305,6 @@ static void delayedwork_callback(struct work_struct *work)
 	struct dj_report dj_report;
 	unsigned long flags;
 	int count;
-	int retval;
 
 	dbg_hid("%s\n", __func__);
 
@@ -347,25 +337,6 @@ static void delayedwork_callback(struct work_struct *work)
 		logi_dj_recv_destroy_djhid_device(djrcv_dev, &dj_report);
 		break;
 	default:
-	/* A normal report (i. e. not belonging to a pair/unpair notification)
-	 * arriving here, means that the report arrived but we did not have a
-	 * paired dj_device associated to the report's device_index, this
-	 * means that the original "device paired" notification corresponding
-	 * to this dj_device never arrived to this driver. The reason is that
-	 * hid-core discards all packets coming from a device while probe() is
-	 * executing. */
-	if (!djrcv_dev->paired_dj_devices[dj_report.device_index]) {
-		/* ok, we don't know the device, just re-ask the
-		 * receiver for the list of connected devices. */
-		retval = logi_dj_recv_query_paired_devices(djrcv_dev);
-		if (!retval) {
-			/* everything went fine, so just leave */
-			break;
-		}
-		dev_err(&djrcv_dev->hdev->dev,
-			"%s:logi_dj_recv_query_paired_devices "
-			"error:%d\n", __func__, retval);
-		}
 		dbg_hid("%s: unexpected report type\n", __func__);
 	}
 }
@@ -396,12 +367,6 @@ static void logi_dj_recv_forward_null_report(struct dj_receiver_dev *djrcv_dev,
 	if (!djdev) {
 		dbg_hid("djrcv_dev->paired_dj_devices[dj_report->device_index]"
 			" is NULL, index %d\n", dj_report->device_index);
-		kfifo_in(&djrcv_dev->notif_fifo, dj_report, sizeof(struct dj_report));
-
-		if (schedule_work(&djrcv_dev->work) == 0) {
-			dbg_hid("%s: did not schedule the work item, was already "
-			"queued\n", __func__);
-		}
 		return;
 	}
 
@@ -432,12 +397,6 @@ static void logi_dj_recv_forward_report(struct dj_receiver_dev *djrcv_dev,
 	if (dj_device == NULL) {
 		dbg_hid("djrcv_dev->paired_dj_devices[dj_report->device_index]"
 			" is NULL, index %d\n", dj_report->device_index);
-		kfifo_in(&djrcv_dev->notif_fifo, dj_report, sizeof(struct dj_report));
-
-		if (schedule_work(&djrcv_dev->work) == 0) {
-			dbg_hid("%s: did not schedule the work item, was already "
-			"queued\n", __func__);
-		}
 		return;
 	}
 
@@ -475,7 +434,7 @@ static int logi_dj_recv_send_report(struct dj_receiver_dev *djrcv_dev,
 	for (i = 0; i < report->field[0]->report_count; i++)
 		report->field[0]->value[i] = data[i];
 
-	usbhid_submit_report(hdev, report, USB_DIR_OUT);
+	hid_hw_request(hdev, report, HID_REQ_SET_REPORT);
 
 	return 0;
 }
@@ -485,10 +444,6 @@ static int logi_dj_recv_query_paired_devices(struct dj_receiver_dev *djrcv_dev)
 	struct dj_report *dj_report;
 	int retval;
 
-	/* no need to protect djrcv_dev->querying_devices */
-	if (djrcv_dev->querying_devices)
-		return 0;
-
 	dj_report = kzalloc(sizeof(struct dj_report), GFP_KERNEL);
 	if (!dj_report)
 		return -ENOMEM;
@@ -500,7 +455,6 @@ static int logi_dj_recv_query_paired_devices(struct dj_receiver_dev *djrcv_dev)
 	return retval;
 }
 
-
 static int logi_dj_recv_switch_to_dj_mode(struct dj_receiver_dev *djrcv_dev,
 					  unsigned timeout)
 {
@@ -644,7 +598,7 @@ static int logi_dj_ll_input_event(struct input_dev *dev, unsigned int type,
 	hid_set_field(report->field[0], 1, REPORT_TYPE_LEDS);
 	hid_set_field(report->field[0], 2, data[1]);
 
-	usbhid_submit_report(dj_rcv_hiddev, report, USB_DIR_OUT);
+	hid_hw_request(dj_rcv_hiddev, report, HID_REQ_SET_REPORT);
 
 	return 0;
 
@@ -809,6 +763,9 @@ static int logi_dj_probe(struct hid_device *hdev,
 		goto llopen_failed;
 	}
 
+	/* Allow incoming packets to arrive: */
+	hid_device_io_start(hdev);
+
 	retval = logi_dj_recv_query_paired_devices(djrcv_dev);
 	if (retval < 0) {
 		dev_err(&hdev->dev, "%s:logi_dj_recv_query_paired_devices "
diff --git a/drivers/hid/hid-logitech-dj.h b/drivers/hid/hid-logitech-dj.h
index 4a4000340ce..fd28a5e0ca3 100644
--- a/drivers/hid/hid-logitech-dj.h
+++ b/drivers/hid/hid-logitech-dj.h
@@ -101,7 +101,6 @@ struct dj_receiver_dev {
 	struct work_struct work;
 	struct kfifo notif_fifo;
 	spinlock_t lock;
-	bool querying_devices;
 };
 
 struct dj_device {
diff --git a/drivers/hid/hid-magicmouse.c b/drivers/hid/hid-magicmouse.c
index f7f113ba083..5bc37343eb2 100644
--- a/drivers/hid/hid-magicmouse.c
+++ b/drivers/hid/hid-magicmouse.c
@@ -19,7 +19,6 @@
 #include <linux/input/mt.h>
 #include <linux/module.h>
 #include <linux/slab.h>
-#include <linux/usb.h>
 
 #include "hid-ids.h"
 
@@ -462,6 +461,21 @@ static int magicmouse_input_mapping(struct hid_device *hdev,
 	return 0;
 }
 
+static void magicmouse_input_configured(struct hid_device *hdev,
+		struct hid_input *hi)
+
+{
+	struct magicmouse_sc *msc = hid_get_drvdata(hdev);
+
+	int ret = magicmouse_setup_input(msc->input, hdev);
+	if (ret) {
+		hid_err(hdev, "magicmouse setup input failed (%d)\n", ret);
+		/* clean msc->input to notify probe() of the failure */
+		msc->input = NULL;
+	}
+}
+
+
 static int magicmouse_probe(struct hid_device *hdev,
 	const struct hid_device_id *id)
 {
@@ -493,15 +507,10 @@ static int magicmouse_probe(struct hid_device *hdev,
 		goto err_free;
 	}
 
-	/* We do this after hid-input is done parsing reports so that
-	 * hid-input uses the most natural button and axis IDs.
-	 */
-	if (msc->input) {
-		ret = magicmouse_setup_input(msc->input, hdev);
-		if (ret) {
-			hid_err(hdev, "magicmouse setup input failed (%d)\n", ret);
-			goto err_stop_hw;
-		}
+	if (!msc->input) {
+		hid_err(hdev, "magicmouse input not registered\n");
+		ret = -ENOMEM;
+		goto err_stop_hw;
 	}
 
 	if (id->product == USB_DEVICE_ID_APPLE_MAGICMOUSE)
@@ -568,6 +577,7 @@ static struct hid_driver magicmouse_driver = {
 	.remove = magicmouse_remove,
 	.raw_event = magicmouse_raw_event,
 	.input_mapping = magicmouse_input_mapping,
+	.input_configured = magicmouse_input_configured,
 };
 module_hid_driver(magicmouse_driver);
 
diff --git a/drivers/hid/hid-microsoft.c b/drivers/hid/hid-microsoft.c
index 29d27f65a11..551795b7da1 100644
--- a/drivers/hid/hid-microsoft.c
+++ b/drivers/hid/hid-microsoft.c
@@ -195,6 +195,8 @@ static const struct hid_device_id ms_devices[] = {
 		.driver_data = MS_HIDINPUT },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_NE4K),
 		.driver_data = MS_ERGONOMY },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_NE4K_JP),
+		.driver_data = MS_ERGONOMY },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_LK6K),
 		.driver_data = MS_ERGONOMY | MS_RDESC },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_PRESENTER_8K_USB),
diff --git a/drivers/hid/hid-multitouch.c b/drivers/hid/hid-multitouch.c
index 7a1ebb867cf..dc3ae5c56f5 100644
--- a/drivers/hid/hid-multitouch.c
+++ b/drivers/hid/hid-multitouch.c
@@ -2,8 +2,9 @@
  *  HID driver for multitouch panels
  *
  *  Copyright (c) 2010-2012 Stephane Chatty <chatty@enac.fr>
- *  Copyright (c) 2010-2012 Benjamin Tissoires <benjamin.tissoires@gmail.com>
+ *  Copyright (c) 2010-2013 Benjamin Tissoires <benjamin.tissoires@gmail.com>
  *  Copyright (c) 2010-2012 Ecole Nationale de l'Aviation Civile, France
+ *  Copyright (c) 2012-2013 Red Hat, Inc
  *
  *  This code is partly based on hid-egalax.c:
  *
@@ -26,13 +27,24 @@
  * any later version.
  */
 
+/*
+ * This driver is regularly tested thanks to the tool hid-test[1].
+ * This tool relies on hid-replay[2] and a database of hid devices[3].
+ * Please run these regression tests before patching this module so that
+ * your patch won't break existing known devices.
+ *
+ * [1] https://github.com/bentiss/hid-test
+ * [2] https://github.com/bentiss/hid-replay
+ * [3] https://github.com/bentiss/hid-devices
+ */
+
 #include <linux/device.h>
 #include <linux/hid.h>
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/usb.h>
 #include <linux/input/mt.h>
-#include "usbhid/usbhid.h"
+#include <linux/string.h>
 
 
 MODULE_AUTHOR("Stephane Chatty <chatty@enac.fr>");
@@ -86,9 +98,9 @@ struct mt_device {
 					   multitouch fields */
 	int cc_index;	/* contact count field index in the report */
 	int cc_value_index;	/* contact count value index in the field */
-	unsigned last_field_index;	/* last field index of the report */
 	unsigned last_slot_field;	/* the last field of a slot */
 	unsigned mt_report_id;	/* the report ID of the multitouch device */
+	unsigned pen_report_id;	/* the report ID of the pen device */
 	__s8 inputmode;		/* InputMode HID feature, -1 if non-existent */
 	__s8 inputmode_index;	/* InputMode HID feature index in the report */
 	__s8 maxcontact_report_id;	/* Maximum Contact Number HID feature,
@@ -104,6 +116,9 @@ struct mt_device {
 	unsigned mt_flags;	/* flags to pass to input-mt */
 };
 
+static void mt_post_parse_default_settings(struct mt_device *td);
+static void mt_post_parse(struct mt_device *td);
+
 /* classes of device behavior */
 #define MT_CLS_DEFAULT				0x0001
 
@@ -246,6 +261,14 @@ static struct mt_class mt_classes[] = {
 	{ }
 };
 
+static void mt_free_input_name(struct hid_input *hi)
+{
+	struct hid_device *hdev = hi->report->device;
+
+	if (hi->input->name != hdev->name)
+		kfree(hi->input->name);
+}
+
 static ssize_t mt_show_quirks(struct device *dev,
 			   struct device_attribute *attr,
 			   char *buf)
@@ -354,7 +377,53 @@ static void mt_store_field(struct hid_usage *usage, struct mt_device *td,
 	f->usages[f->length++] = usage->hid;
 }
 
-static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
+static int mt_pen_input_mapping(struct hid_device *hdev, struct hid_input *hi,
+		struct hid_field *field, struct hid_usage *usage,
+		unsigned long **bit, int *max)
+{
+	struct mt_device *td = hid_get_drvdata(hdev);
+
+	td->pen_report_id = field->report->id;
+
+	return 0;
+}
+
+static int mt_pen_input_mapped(struct hid_device *hdev, struct hid_input *hi,
+		struct hid_field *field, struct hid_usage *usage,
+		unsigned long **bit, int *max)
+{
+	return 0;
+}
+
+static int mt_pen_event(struct hid_device *hid, struct hid_field *field,
+				struct hid_usage *usage, __s32 value)
+{
+	/* let hid-input handle it */
+	return 0;
+}
+
+static void mt_pen_report(struct hid_device *hid, struct hid_report *report)
+{
+	struct hid_field *field = report->field[0];
+
+	input_sync(field->hidinput->input);
+}
+
+static void mt_pen_input_configured(struct hid_device *hdev,
+					struct hid_input *hi)
+{
+	char *name = kzalloc(strlen(hi->input->name) + 5, GFP_KERNEL);
+	if (name) {
+		sprintf(name, "%s Pen", hi->input->name);
+		mt_free_input_name(hi);
+		hi->input->name = name;
+	}
+
+	/* force BTN_STYLUS to allow tablet matching in udev */
+	__set_bit(BTN_STYLUS, hi->input->keybit);
+}
+
+static int mt_touch_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 		struct hid_field *field, struct hid_usage *usage,
 		unsigned long **bit, int *max)
 {
@@ -363,13 +432,8 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 	int code;
 	struct hid_usage *prev_usage = NULL;
 
-	/* Only map fields from TouchScreen or TouchPad collections.
-	* We need to ignore fields that belong to other collections
-	* such as Mouse that might have the same GenericDesktop usages. */
 	if (field->application == HID_DG_TOUCHSCREEN)
 		td->mt_flags |= INPUT_MT_DIRECT;
-	else if (field->application != HID_DG_TOUCHPAD)
-		return 0;
 
 	/*
 	 * Model touchscreens providing buttons as touchpads.
@@ -378,12 +442,6 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 	    (usage->hid & HID_USAGE_PAGE) == HID_UP_BUTTON)
 		td->mt_flags |= INPUT_MT_POINTER;
 
-	/* eGalax devices provide a Digitizer.Stylus input which overrides
-	 * the correct Digitizers.Finger X/Y ranges.
-	 * Let's just ignore this input. */
-	if (field->physical == HID_DG_STYLUS)
-		return -1;
-
 	if (usage->usage_index)
 		prev_usage = &field->usage[usage->usage_index - 1];
 
@@ -405,7 +463,6 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 			}
 
 			mt_store_field(usage, td, hi);
-			td->last_field_index = field->index;
 			return 1;
 		case HID_GD_Y:
 			if (prev_usage && (prev_usage->hid == usage->hid)) {
@@ -421,7 +478,6 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 			}
 
 			mt_store_field(usage, td, hi);
-			td->last_field_index = field->index;
 			return 1;
 		}
 		return 0;
@@ -436,21 +492,17 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 					ABS_MT_DISTANCE, 0, 1, 0, 0);
 			}
 			mt_store_field(usage, td, hi);
-			td->last_field_index = field->index;
 			return 1;
 		case HID_DG_CONFIDENCE:
 			mt_store_field(usage, td, hi);
-			td->last_field_index = field->index;
 			return 1;
 		case HID_DG_TIPSWITCH:
 			hid_map_usage(hi, usage, bit, max, EV_KEY, BTN_TOUCH);
 			input_set_capability(hi->input, EV_KEY, BTN_TOUCH);
 			mt_store_field(usage, td, hi);
-			td->last_field_index = field->index;
 			return 1;
 		case HID_DG_CONTACTID:
 			mt_store_field(usage, td, hi);
-			td->last_field_index = field->index;
 			td->touches_by_report++;
 			td->mt_report_id = field->report->id;
 			return 1;
@@ -461,7 +513,6 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 				set_abs(hi->input, ABS_MT_TOUCH_MAJOR, field,
 					cls->sn_width);
 			mt_store_field(usage, td, hi);
-			td->last_field_index = field->index;
 			return 1;
 		case HID_DG_HEIGHT:
 			hid_map_usage(hi, usage, bit, max,
@@ -473,7 +524,6 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 					ABS_MT_ORIENTATION, 0, 1, 0, 0);
 			}
 			mt_store_field(usage, td, hi);
-			td->last_field_index = field->index;
 			return 1;
 		case HID_DG_TIPPRESSURE:
 			hid_map_usage(hi, usage, bit, max,
@@ -481,17 +531,14 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 			set_abs(hi->input, ABS_MT_PRESSURE, field,
 				cls->sn_pressure);
 			mt_store_field(usage, td, hi);
-			td->last_field_index = field->index;
 			return 1;
 		case HID_DG_CONTACTCOUNT:
 			td->cc_index = field->index;
 			td->cc_value_index = usage->usage_index;
-			td->last_field_index = field->index;
 			return 1;
 		case HID_DG_CONTACTMAX:
 			/* we don't set td->last_slot_field as contactcount and
 			 * contact max are global to the report */
-			td->last_field_index = field->index;
 			return -1;
 		case HID_DG_TOUCH:
 			/* Legacy devices use TIPSWITCH and not TOUCH.
@@ -515,7 +562,7 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 	return 0;
 }
 
-static int mt_input_mapped(struct hid_device *hdev, struct hid_input *hi,
+static int mt_touch_input_mapped(struct hid_device *hdev, struct hid_input *hi,
 		struct hid_field *field, struct hid_usage *usage,
 		unsigned long **bit, int *max)
 {
@@ -606,7 +653,7 @@ static void mt_sync_frame(struct mt_device *td, struct input_dev *input)
 	td->num_received = 0;
 }
 
-static int mt_event(struct hid_device *hid, struct hid_field *field,
+static int mt_touch_event(struct hid_device *hid, struct hid_field *field,
 				struct hid_usage *usage, __s32 value)
 {
 	/* we will handle the hidinput part later, now remains hiddev */
@@ -621,6 +668,7 @@ static void mt_process_mt_event(struct hid_device *hid, struct hid_field *field,
 {
 	struct mt_device *td = hid_get_drvdata(hid);
 	__s32 quirks = td->mtclass.quirks;
+	struct input_dev *input = field->hidinput->input;
 
 	if (hid->claimed & HID_CLAIMED_INPUT) {
 		switch (usage->hid) {
@@ -670,6 +718,9 @@ static void mt_process_mt_event(struct hid_device *hid, struct hid_field *field,
 			break;
 
 		default:
+			if (usage->type)
+				input_event(input, usage->type, usage->code,
+						value);
 			return;
 		}
 
@@ -677,28 +728,18 @@ static void mt_process_mt_event(struct hid_device *hid, struct hid_field *field,
 			/* we only take into account the last report. */
 			if (usage->hid == td->last_slot_field)
 				mt_complete_slot(td, field->hidinput->input);
-
-			if (field->index == td->last_field_index
-				&& td->num_received >= td->num_expected)
-				mt_sync_frame(td, field->hidinput->input);
 		}
 
 	}
 }
 
-static void mt_report(struct hid_device *hid, struct hid_report *report)
+static void mt_touch_report(struct hid_device *hid, struct hid_report *report)
 {
 	struct mt_device *td = hid_get_drvdata(hid);
 	struct hid_field *field;
 	unsigned count;
 	int r, n;
 
-	if (report->id != td->mt_report_id)
-		return;
-
-	if (!(hid->claimed & HID_CLAIMED_INPUT))
-		return;
-
 	/*
 	 * Includes multi-packet support where subsequent
 	 * packets are sent with zero contactcount.
@@ -721,6 +762,91 @@ static void mt_report(struct hid_device *hid, struct hid_report *report)
 			mt_process_mt_event(hid, field, &field->usage[n],
 					field->value[n]);
 	}
+
+	if (td->num_received >= td->num_expected)
+		mt_sync_frame(td, report->field[0]->hidinput->input);
+}
+
+static void mt_touch_input_configured(struct hid_device *hdev,
+					struct hid_input *hi)
+{
+	struct mt_device *td = hid_get_drvdata(hdev);
+	struct mt_class *cls = &td->mtclass;
+	struct input_dev *input = hi->input;
+
+	if (!td->maxcontacts)
+		td->maxcontacts = MT_DEFAULT_MAXCONTACT;
+
+	mt_post_parse(td);
+	if (td->serial_maybe)
+		mt_post_parse_default_settings(td);
+
+	if (cls->is_indirect)
+		td->mt_flags |= INPUT_MT_POINTER;
+
+	if (cls->quirks & MT_QUIRK_NOT_SEEN_MEANS_UP)
+		td->mt_flags |= INPUT_MT_DROP_UNUSED;
+
+	input_mt_init_slots(input, td->maxcontacts, td->mt_flags);
+
+	td->mt_flags = 0;
+}
+
+static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
+		struct hid_field *field, struct hid_usage *usage,
+		unsigned long **bit, int *max)
+{
+	/* Only map fields from TouchScreen or TouchPad collections.
+	* We need to ignore fields that belong to other collections
+	* such as Mouse that might have the same GenericDesktop usages. */
+	if (field->application != HID_DG_TOUCHSCREEN &&
+	    field->application != HID_DG_PEN &&
+	    field->application != HID_DG_TOUCHPAD)
+		return -1;
+
+	if (field->physical == HID_DG_STYLUS)
+		return mt_pen_input_mapping(hdev, hi, field, usage, bit, max);
+
+	return mt_touch_input_mapping(hdev, hi, field, usage, bit, max);
+}
+
+static int mt_input_mapped(struct hid_device *hdev, struct hid_input *hi,
+		struct hid_field *field, struct hid_usage *usage,
+		unsigned long **bit, int *max)
+{
+	if (field->physical == HID_DG_STYLUS)
+		return mt_pen_input_mapped(hdev, hi, field, usage, bit, max);
+
+	return mt_touch_input_mapped(hdev, hi, field, usage, bit, max);
+}
+
+static int mt_event(struct hid_device *hid, struct hid_field *field,
+				struct hid_usage *usage, __s32 value)
+{
+	struct mt_device *td = hid_get_drvdata(hid);
+
+	if (field->report->id == td->mt_report_id)
+		return mt_touch_event(hid, field, usage, value);
+
+	if (field->report->id == td->pen_report_id)
+		return mt_pen_event(hid, field, usage, value);
+
+	/* ignore other reports */
+	return 1;
+}
+
+static void mt_report(struct hid_device *hid, struct hid_report *report)
+{
+	struct mt_device *td = hid_get_drvdata(hid);
+
+	if (!(hid->claimed & HID_CLAIMED_INPUT))
+		return;
+
+	if (report->id == td->mt_report_id)
+		mt_touch_report(hid, report);
+
+	if (report->id == td->pen_report_id)
+		mt_pen_report(hid, report);
 }
 
 static void mt_set_input_mode(struct hid_device *hdev)
@@ -736,7 +862,7 @@ static void mt_set_input_mode(struct hid_device *hdev)
 	r = re->report_id_hash[td->inputmode];
 	if (r) {
 		r->field[0]->value[td->inputmode_index] = 0x02;
-		usbhid_submit_report(hdev, r, USB_DIR_OUT);
+		hid_hw_request(hdev, r, HID_REQ_SET_REPORT);
 	}
 }
 
@@ -761,7 +887,7 @@ static void mt_set_maxcontacts(struct hid_device *hdev)
 		max = min(fieldmax, max);
 		if (r->field[0]->value[0] != max) {
 			r->field[0]->value[0] = max;
-			usbhid_submit_report(hdev, r, USB_DIR_OUT);
+			hid_hw_request(hdev, r, HID_REQ_SET_REPORT);
 		}
 	}
 }
@@ -797,32 +923,18 @@ static void mt_post_parse(struct mt_device *td)
 }
 
 static void mt_input_configured(struct hid_device *hdev, struct hid_input *hi)
-
 {
 	struct mt_device *td = hid_get_drvdata(hdev);
-	struct mt_class *cls = &td->mtclass;
-	struct input_dev *input = hi->input;
+	char *name = kstrdup(hdev->name, GFP_KERNEL);
 
-	/* Only initialize slots for MT input devices */
-	if (!test_bit(ABS_MT_POSITION_X, input->absbit))
-		return;
+	if (name)
+		hi->input->name = name;
 
-	if (!td->maxcontacts)
-		td->maxcontacts = MT_DEFAULT_MAXCONTACT;
+	if (hi->report->id == td->mt_report_id)
+		mt_touch_input_configured(hdev, hi);
 
-	mt_post_parse(td);
-	if (td->serial_maybe)
-		mt_post_parse_default_settings(td);
-
-	if (cls->is_indirect)
-		td->mt_flags |= INPUT_MT_POINTER;
-
-	if (cls->quirks & MT_QUIRK_NOT_SEEN_MEANS_UP)
-		td->mt_flags |= INPUT_MT_DROP_UNUSED;
-
-	input_mt_init_slots(input, td->maxcontacts, td->mt_flags);
-
-	td->mt_flags = 0;
+	if (hi->report->id == td->pen_report_id)
+		mt_pen_input_configured(hdev, hi);
 }
 
 static int mt_probe(struct hid_device *hdev, const struct hid_device_id *id)
@@ -830,6 +942,7 @@ static int mt_probe(struct hid_device *hdev, const struct hid_device_id *id)
 	int ret, i;
 	struct mt_device *td;
 	struct mt_class *mtclass = mt_classes; /* MT_CLS_DEFAULT */
+	struct hid_input *hi;
 
 	for (i = 0; mt_classes[i].name ; i++) {
 		if (id->driver_data == mt_classes[i].name) {
@@ -843,6 +956,14 @@ static int mt_probe(struct hid_device *hdev, const struct hid_device_id *id)
 	 */
 	hdev->quirks |= HID_QUIRK_NO_INPUT_SYNC;
 
+	/*
+	 * This allows the driver to handle different input sensors
+	 * that emits events through different reports on the same HID
+	 * device.
+	 */
+	hdev->quirks |= HID_QUIRK_MULTI_INPUT;
+	hdev->quirks |= HID_QUIRK_NO_EMPTY_INPUT;
+
 	td = kzalloc(sizeof(struct mt_device), GFP_KERNEL);
 	if (!td) {
 		dev_err(&hdev->dev, "cannot allocate multitouch data\n");
@@ -852,6 +973,8 @@ static int mt_probe(struct hid_device *hdev, const struct hid_device_id *id)
 	td->inputmode = -1;
 	td->maxcontact_report_id = -1;
 	td->cc_index = -1;
+	td->mt_report_id = -1;
+	td->pen_report_id = -1;
 	hid_set_drvdata(hdev, td);
 
 	td->fields = kzalloc(sizeof(struct mt_fields), GFP_KERNEL);
@@ -870,7 +993,7 @@ static int mt_probe(struct hid_device *hdev, const struct hid_device_id *id)
 
 	ret = hid_hw_start(hdev, HID_CONNECT_DEFAULT);
 	if (ret)
-		goto fail;
+		goto hid_fail;
 
 	ret = sysfs_create_group(&hdev->dev.kobj, &mt_attribute_group);
 
@@ -882,6 +1005,9 @@ static int mt_probe(struct hid_device *hdev, const struct hid_device_id *id)
 
 	return 0;
 
+hid_fail:
+	list_for_each_entry(hi, &hdev->inputs, list)
+		mt_free_input_name(hi);
 fail:
 	kfree(td->fields);
 	kfree(td);
@@ -898,26 +1024,11 @@ static int mt_reset_resume(struct hid_device *hdev)
 
 static int mt_resume(struct hid_device *hdev)
 {
-	struct usb_interface *intf;
-	struct usb_host_interface *interface;
-	struct usb_device *dev;
-
-	if (hdev->bus != BUS_USB)
-		return 0;
-
-	intf = to_usb_interface(hdev->dev.parent);
-	interface = intf->cur_altsetting;
-	dev = hid_to_usb_dev(hdev);
-
 	/* Some Elan legacy devices require SET_IDLE to be set on resume.
 	 * It should be safe to send it to other devices too.
 	 * Tested on 3M, Stantum, Cypress, Zytronic, eGalax, and Elan panels. */
 
-	usb_control_msg(dev, usb_sndctrlpipe(dev, 0),
-			HID_REQ_SET_IDLE,
-			USB_TYPE_CLASS | USB_RECIP_INTERFACE,
-			0, interface->desc.bInterfaceNumber,
-			NULL, 0, USB_CTRL_SET_TIMEOUT);
+	hid_hw_idle(hdev, 0, 0, HID_REQ_SET_IDLE);
 
 	return 0;
 }
@@ -926,8 +1037,14 @@ static int mt_resume(struct hid_device *hdev)
 static void mt_remove(struct hid_device *hdev)
 {
 	struct mt_device *td = hid_get_drvdata(hdev);
+	struct hid_input *hi;
+
 	sysfs_remove_group(&hdev->dev.kobj, &mt_attribute_group);
 	hid_hw_stop(hdev);
+
+	list_for_each_entry(hi, &hdev->inputs, list)
+		mt_free_input_name(hi);
+
 	kfree(td);
 	hid_set_drvdata(hdev, NULL);
 }
diff --git a/drivers/hid/hid-ntrig.c b/drivers/hid/hid-ntrig.c
index 7757e82416e..ef95102515e 100644
--- a/drivers/hid/hid-ntrig.c
+++ b/drivers/hid/hid-ntrig.c
@@ -118,8 +118,8 @@ static inline int ntrig_get_mode(struct hid_device *hdev)
 	if (!report)
 		return -EINVAL;
 
-	usbhid_submit_report(hdev, report, USB_DIR_IN);
-	usbhid_wait_io(hdev);
+	hid_hw_request(hdev, report, HID_REQ_GET_REPORT);
+	hid_hw_wait(hdev);
 	return (int)report->field[0]->value[0];
 }
 
@@ -137,7 +137,7 @@ static inline void ntrig_set_mode(struct hid_device *hdev, const int mode)
 	if (!report)
 		return;
 
-	usbhid_submit_report(hdev, report, USB_DIR_IN);
+	hid_hw_request(hdev, report, HID_REQ_GET_REPORT);
 }
 
 static void ntrig_report_version(struct hid_device *hdev)
@@ -937,8 +937,8 @@ static int ntrig_probe(struct hid_device *hdev, const struct hid_device_id *id)
 	if (report) {
 		/* Let the device settle to ensure the wakeup message gets
 		 * through */
-		usbhid_wait_io(hdev);
-		usbhid_submit_report(hdev, report, USB_DIR_IN);
+		hid_hw_wait(hdev);
+		hid_hw_request(hdev, report, HID_REQ_GET_REPORT);
 
 		/*
 		 * Sanity check: if the current mode is invalid reset it to
diff --git a/drivers/hid/hid-picolcd.h b/drivers/hid/hid-picolcd.h
index 020cef69f6a..e56d847b2ef 100644
--- a/drivers/hid/hid-picolcd.h
+++ b/drivers/hid/hid-picolcd.h
@@ -142,10 +142,10 @@ struct hid_report *picolcd_report(int id, struct hid_device *hdev, int dir);
 #ifdef CONFIG_DEBUG_FS
 void picolcd_debug_out_report(struct picolcd_data *data,
 		struct hid_device *hdev, struct hid_report *report);
-#define usbhid_submit_report(a, b, c) \
+#define hid_hw_request(a, b, c) \
 	do { \
 		picolcd_debug_out_report(hid_get_drvdata(a), a, b); \
-		usbhid_submit_report(a, b, c); \
+		hid_hw_request(a, b, c); \
 	} while (0)
 
 void picolcd_debug_raw_event(struct picolcd_data *data,
@@ -302,7 +302,7 @@ static inline int picolcd_init_cir(struct picolcd_data *data, struct hid_report
 static inline void picolcd_exit_cir(struct picolcd_data *data)
 {
 }
-#endif /* CONFIG_HID_PICOLCD_LIRC */
+#endif /* CONFIG_HID_PICOLCD_CIR */
 
 int picolcd_reset(struct hid_device *hdev);
 struct picolcd_pending *picolcd_send_and_wait(struct hid_device *hdev,
diff --git a/drivers/hid/hid-picolcd_backlight.c b/drivers/hid/hid-picolcd_backlight.c
index b91f30945f9..a32c5f86b0b 100644
--- a/drivers/hid/hid-picolcd_backlight.c
+++ b/drivers/hid/hid-picolcd_backlight.c
@@ -18,8 +18,6 @@
  ***************************************************************************/
 
 #include <linux/hid.h>
-#include "usbhid/usbhid.h"
-#include <linux/usb.h>
 
 #include <linux/fb.h>
 #include <linux/backlight.h>
@@ -46,7 +44,7 @@ static int picolcd_set_brightness(struct backlight_device *bdev)
 	spin_lock_irqsave(&data->lock, flags);
 	hid_set_field(report->field[0], 0, data->lcd_power == FB_BLANK_UNBLANK ? data->lcd_brightness : 0);
 	if (!(data->status & PICOLCD_FAILED))
-		usbhid_submit_report(data->hdev, report, USB_DIR_OUT);
+		hid_hw_request(data->hdev, report, HID_REQ_SET_REPORT);
 	spin_unlock_irqrestore(&data->lock, flags);
 	return 0;
 }
diff --git a/drivers/hid/hid-picolcd_cir.c b/drivers/hid/hid-picolcd_cir.c
index a79e95bb9fb..e346038f0f1 100644
--- a/drivers/hid/hid-picolcd_cir.c
+++ b/drivers/hid/hid-picolcd_cir.c
@@ -21,8 +21,6 @@
 #include <linux/hid-debug.h>
 #include <linux/input.h>
 #include "hid-ids.h"
-#include "usbhid/usbhid.h"
-#include <linux/usb.h>
 
 #include <linux/fb.h>
 #include <linux/vmalloc.h>
diff --git a/drivers/hid/hid-picolcd_core.c b/drivers/hid/hid-picolcd_core.c
index 31cd93fc3d4..b48092d0e13 100644
--- a/drivers/hid/hid-picolcd_core.c
+++ b/drivers/hid/hid-picolcd_core.c
@@ -21,8 +21,6 @@
 #include <linux/hid-debug.h>
 #include <linux/input.h>
 #include "hid-ids.h"
-#include "usbhid/usbhid.h"
-#include <linux/usb.h>
 
 #include <linux/fb.h>
 #include <linux/vmalloc.h>
@@ -110,7 +108,7 @@ struct picolcd_pending *picolcd_send_and_wait(struct hid_device *hdev,
 		work = NULL;
 	} else {
 		data->pending = work;
-		usbhid_submit_report(data->hdev, report, USB_DIR_OUT);
+		hid_hw_request(data->hdev, report, HID_REQ_SET_REPORT);
 		spin_unlock_irqrestore(&data->lock, flags);
 		wait_for_completion_interruptible_timeout(&work->ready, HZ*2);
 		spin_lock_irqsave(&data->lock, flags);
@@ -244,7 +242,7 @@ int picolcd_reset(struct hid_device *hdev)
 		spin_unlock_irqrestore(&data->lock, flags);
 		return -ENODEV;
 	}
-	usbhid_submit_report(hdev, report, USB_DIR_OUT);
+	hid_hw_request(hdev, report, HID_REQ_SET_REPORT);
 	spin_unlock_irqrestore(&data->lock, flags);
 
 	error = picolcd_check_version(hdev);
@@ -303,7 +301,7 @@ static ssize_t picolcd_operation_mode_store(struct device *dev,
 	spin_lock_irqsave(&data->lock, flags);
 	hid_set_field(report->field[0], 0, timeout & 0xff);
 	hid_set_field(report->field[0], 1, (timeout >> 8) & 0xff);
-	usbhid_submit_report(data->hdev, report, USB_DIR_OUT);
+	hid_hw_request(data->hdev, report, HID_REQ_SET_REPORT);
 	spin_unlock_irqrestore(&data->lock, flags);
 	return count;
 }
diff --git a/drivers/hid/hid-picolcd_debugfs.c b/drivers/hid/hid-picolcd_debugfs.c
index 4809aa1bdb9..59ab8e157e6 100644
--- a/drivers/hid/hid-picolcd_debugfs.c
+++ b/drivers/hid/hid-picolcd_debugfs.c
@@ -19,8 +19,6 @@
 
 #include <linux/hid.h>
 #include <linux/hid-debug.h>
-#include "usbhid/usbhid.h"
-#include <linux/usb.h>
 
 #include <linux/fb.h>
 #include <linux/seq_file.h>
diff --git a/drivers/hid/hid-picolcd_fb.c b/drivers/hid/hid-picolcd_fb.c
index eb003574b63..591f6b22aa9 100644
--- a/drivers/hid/hid-picolcd_fb.c
+++ b/drivers/hid/hid-picolcd_fb.c
@@ -19,8 +19,6 @@
 
 #include <linux/hid.h>
 #include <linux/vmalloc.h>
-#include "usbhid/usbhid.h"
-#include <linux/usb.h>
 
 #include <linux/fb.h>
 #include <linux/module.h>
@@ -143,8 +141,8 @@ static int picolcd_fb_send_tile(struct picolcd_data *data, u8 *vbitmap,
 		else
 			hid_set_field(report2->field[0], 4 + i - 32, tdata[i]);
 
-	usbhid_submit_report(data->hdev, report1, USB_DIR_OUT);
-	usbhid_submit_report(data->hdev, report2, USB_DIR_OUT);
+	hid_hw_request(data->hdev, report1, HID_REQ_SET_REPORT);
+	hid_hw_request(data->hdev, report2, HID_REQ_SET_REPORT);
 	spin_unlock_irqrestore(&data->lock, flags);
 	return 0;
 }
@@ -214,7 +212,7 @@ int picolcd_fb_reset(struct picolcd_data *data, int clear)
 				hid_set_field(report->field[0], j, mapcmd[j]);
 			else
 				hid_set_field(report->field[0], j, 0);
-		usbhid_submit_report(data->hdev, report, USB_DIR_OUT);
+		hid_hw_request(data->hdev, report, HID_REQ_SET_REPORT);
 	}
 	spin_unlock_irqrestore(&data->lock, flags);
 
@@ -270,7 +268,7 @@ static void picolcd_fb_update(struct fb_info *info)
 				mutex_unlock(&info->lock);
 				if (!data)
 					return;
-				usbhid_wait_io(data->hdev);
+				hid_hw_wait(data->hdev);
 				mutex_lock(&info->lock);
 				n = 0;
 			}
@@ -288,7 +286,7 @@ static void picolcd_fb_update(struct fb_info *info)
 		spin_unlock_irqrestore(&fbdata->lock, flags);
 		mutex_unlock(&info->lock);
 		if (data)
-			usbhid_wait_io(data->hdev);
+			hid_hw_wait(data->hdev);
 		return;
 	}
 out:
diff --git a/drivers/hid/hid-picolcd_lcd.c b/drivers/hid/hid-picolcd_lcd.c
index 2d0ddc5ac65..89821c2da6d 100644
--- a/drivers/hid/hid-picolcd_lcd.c
+++ b/drivers/hid/hid-picolcd_lcd.c
@@ -18,8 +18,6 @@
  ***************************************************************************/
 
 #include <linux/hid.h>
-#include "usbhid/usbhid.h"
-#include <linux/usb.h>
 
 #include <linux/fb.h>
 #include <linux/lcd.h>
@@ -48,7 +46,7 @@ static int picolcd_set_contrast(struct lcd_device *ldev, int contrast)
 	spin_lock_irqsave(&data->lock, flags);
 	hid_set_field(report->field[0], 0, data->lcd_contrast);
 	if (!(data->status & PICOLCD_FAILED))
-		usbhid_submit_report(data->hdev, report, USB_DIR_OUT);
+		hid_hw_request(data->hdev, report, HID_REQ_SET_REPORT);
 	spin_unlock_irqrestore(&data->lock, flags);
 	return 0;
 }
diff --git a/drivers/hid/hid-picolcd_leds.c b/drivers/hid/hid-picolcd_leds.c
index 28cb6a4f963..e994f9c2901 100644
--- a/drivers/hid/hid-picolcd_leds.c
+++ b/drivers/hid/hid-picolcd_leds.c
@@ -21,8 +21,6 @@
 #include <linux/hid-debug.h>
 #include <linux/input.h>
 #include "hid-ids.h"
-#include "usbhid/usbhid.h"
-#include <linux/usb.h>
 
 #include <linux/fb.h>
 #include <linux/vmalloc.h>
@@ -55,7 +53,7 @@ void picolcd_leds_set(struct picolcd_data *data)
 	spin_lock_irqsave(&data->lock, flags);
 	hid_set_field(report->field[0], 0, data->led_state);
 	if (!(data->status & PICOLCD_FAILED))
-		usbhid_submit_report(data->hdev, report, USB_DIR_OUT);
+		hid_hw_request(data->hdev, report, HID_REQ_SET_REPORT);
 	spin_unlock_irqrestore(&data->lock, flags);
 }
 
diff --git a/drivers/hid/hid-pl.c b/drivers/hid/hid-pl.c
index b0199d27787..d29112fa5cd 100644
--- a/drivers/hid/hid-pl.c
+++ b/drivers/hid/hid-pl.c
@@ -43,13 +43,11 @@
 #include <linux/input.h>
 #include <linux/slab.h>
 #include <linux/module.h>
-#include <linux/usb.h>
 #include <linux/hid.h>
 
 #include "hid-ids.h"
 
 #ifdef CONFIG_PANTHERLORD_FF
-#include "usbhid/usbhid.h"
 
 struct plff_device {
 	struct hid_report *report;
@@ -75,7 +73,7 @@ static int hid_plff_play(struct input_dev *dev, void *data,
 	*plff->strong = left;
 	*plff->weak = right;
 	debug("running with 0x%02x 0x%02x", left, right);
-	usbhid_submit_report(hid, plff->report, USB_DIR_OUT);
+	hid_hw_request(hid, plff->report, HID_REQ_SET_REPORT);
 
 	return 0;
 }
@@ -169,7 +167,7 @@ static int plff_init(struct hid_device *hid)
 
 		*strong = 0x00;
 		*weak = 0x00;
-		usbhid_submit_report(hid, plff->report, USB_DIR_OUT);
+		hid_hw_request(hid, plff->report, HID_REQ_SET_REPORT);
 	}
 
 	hid_info(hid, "Force feedback for PantherLord/GreenAsia devices by Anssi Hannula <anssi.hannula@gmail.com>\n");
diff --git a/drivers/hid/hid-prodikeys.c b/drivers/hid/hid-prodikeys.c
index 4e1c4bcbdc0..7ed82805641 100644
--- a/drivers/hid/hid-prodikeys.c
+++ b/drivers/hid/hid-prodikeys.c
@@ -26,7 +26,6 @@
 #include <sound/core.h>
 #include <sound/initval.h>
 #include <sound/rawmidi.h>
-#include "usbhid/usbhid.h"
 #include "hid-ids.h"
 
 
@@ -306,7 +305,7 @@ static void pcmidi_submit_output_report(struct pcmidi_snd *pm, int state)
 	report->field[0]->value[0] = 0x01;
 	report->field[0]->value[1] = state;
 
-	usbhid_submit_report(hdev, report, USB_DIR_OUT);
+	hid_hw_request(hdev, report, HID_REQ_SET_REPORT);
 }
 
 static int pcmidi_handle_report1(struct pcmidi_snd *pm, u8 *data)
diff --git a/drivers/hid/hid-roccat-isku.c b/drivers/hid/hid-roccat-isku.c
index 1219998a02d..8023751d525 100644
--- a/drivers/hid/hid-roccat-isku.c
+++ b/drivers/hid/hid-roccat-isku.c
@@ -130,14 +130,14 @@ static ssize_t isku_sysfs_read(struct file *fp, struct kobject *kobj,
 	if (off >= real_size)
 		return 0;
 
-	if (off != 0 || count != real_size)
+	if (off != 0 || count > real_size)
 		return -EINVAL;
 
 	mutex_lock(&isku->isku_lock);
-	retval = isku_receive(usb_dev, command, buf, real_size);
+	retval = isku_receive(usb_dev, command, buf, count);
 	mutex_unlock(&isku->isku_lock);
 
-	return retval ? retval : real_size;
+	return retval ? retval : count;
 }
 
 static ssize_t isku_sysfs_write(struct file *fp, struct kobject *kobj,
@@ -150,15 +150,15 @@ static ssize_t isku_sysfs_write(struct file *fp, struct kobject *kobj,
 	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
 	int retval;
 
-	if (off != 0 || count != real_size)
+	if (off != 0 || count > real_size)
 		return -EINVAL;
 
 	mutex_lock(&isku->isku_lock);
 	retval = roccat_common2_send_with_status(usb_dev, command,
-			(void *)buf, real_size);
+			(void *)buf, count);
 	mutex_unlock(&isku->isku_lock);
 
-	return retval ? retval : real_size;
+	return retval ? retval : count;
 }
 
 #define ISKU_SYSFS_W(thingy, THINGY) \
@@ -216,6 +216,7 @@ ISKU_SYSFS_RW(light, LIGHT)
 ISKU_SYSFS_RW(key_mask, KEY_MASK)
 ISKU_SYSFS_RW(last_set, LAST_SET)
 ISKU_SYSFS_W(talk, TALK)
+ISKU_SYSFS_W(talkfx, TALKFX)
 ISKU_SYSFS_R(info, INFO)
 ISKU_SYSFS_W(control, CONTROL)
 ISKU_SYSFS_W(reset, RESET)
@@ -232,6 +233,7 @@ static struct bin_attribute isku_bin_attributes[] = {
 	ISKU_BIN_ATTR_RW(key_mask, KEY_MASK),
 	ISKU_BIN_ATTR_RW(last_set, LAST_SET),
 	ISKU_BIN_ATTR_W(talk, TALK),
+	ISKU_BIN_ATTR_W(talkfx, TALKFX),
 	ISKU_BIN_ATTR_R(info, INFO),
 	ISKU_BIN_ATTR_W(control, CONTROL),
 	ISKU_BIN_ATTR_W(reset, RESET),
@@ -405,6 +407,7 @@ static int isku_raw_event(struct hid_device *hdev,
 
 static const struct hid_device_id isku_devices[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_ISKU) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_ISKUFX) },
 	{ }
 };
 
@@ -443,5 +446,5 @@ module_init(isku_init);
 module_exit(isku_exit);
 
 MODULE_AUTHOR("Stefan Achatz");
-MODULE_DESCRIPTION("USB Roccat Isku driver");
+MODULE_DESCRIPTION("USB Roccat Isku/FX driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/hid/hid-roccat-isku.h b/drivers/hid/hid-roccat-isku.h
index cf6896c8386..53056860d4d 100644
--- a/drivers/hid/hid-roccat-isku.h
+++ b/drivers/hid/hid-roccat-isku.h
@@ -25,10 +25,11 @@ enum {
 	ISKU_SIZE_KEYS_MACRO = 0x23,
 	ISKU_SIZE_KEYS_CAPSLOCK = 0x06,
 	ISKU_SIZE_LAST_SET = 0x14,
-	ISKU_SIZE_LIGHT = 0x0a,
+	ISKU_SIZE_LIGHT = 0x10,
 	ISKU_SIZE_MACRO = 0x823,
 	ISKU_SIZE_RESET = 0x03,
 	ISKU_SIZE_TALK = 0x10,
+	ISKU_SIZE_TALKFX = 0x10,
 };
 
 enum {
@@ -59,6 +60,7 @@ enum isku_commands {
 	ISKU_COMMAND_LAST_SET = 0x14,
 	ISKU_COMMAND_15 = 0x15,
 	ISKU_COMMAND_TALK = 0x16,
+	ISKU_COMMAND_TALKFX = 0x17,
 	ISKU_COMMAND_FIRMWARE_WRITE = 0x1b,
 	ISKU_COMMAND_FIRMWARE_WRITE_CONTROL = 0x1c,
 };
diff --git a/drivers/hid/hid-roccat-kone.c b/drivers/hid/hid-roccat-kone.c
index 9ce2d0b615a..7fae070788f 100644
--- a/drivers/hid/hid-roccat-kone.c
+++ b/drivers/hid/hid-roccat-kone.c
@@ -818,8 +818,9 @@ static void kone_report_to_chrdev(struct kone_device const *kone,
 				(uint8_t *)&roccat_report);
 		break;
 	case kone_mouse_event_call_overlong_macro:
+	case kone_mouse_event_multimedia:
 		if (event->value == kone_keystroke_action_press) {
-			roccat_report.event = kone_mouse_event_call_overlong_macro;
+			roccat_report.event = event->event;
 			roccat_report.value = kone->actual_profile;
 			roccat_report.key = event->macro_key;
 			roccat_report_event(kone->chrdev_minor,
diff --git a/drivers/hid/hid-roccat-kone.h b/drivers/hid/hid-roccat-kone.h
index 64abb5b8a59..52c6167d023 100644
--- a/drivers/hid/hid-roccat-kone.h
+++ b/drivers/hid/hid-roccat-kone.h
@@ -169,6 +169,7 @@ enum kone_mouse_events {
 	/* TODO clarify meaning and occurence of kone_mouse_event_calibration */
 	kone_mouse_event_calibration = 0xc0,
 	kone_mouse_event_call_overlong_macro = 0xe0,
+	kone_mouse_event_multimedia = 0xe1,
 	/* switch events notify if user changed values with mousebutton click */
 	kone_mouse_event_switch_dpi = 0xf0,
 	kone_mouse_event_switch_profile = 0xf1
diff --git a/drivers/hid/hid-roccat-konepure.c b/drivers/hid/hid-roccat-konepure.c
new file mode 100644
index 00000000000..c79d0b06c14
--- /dev/null
+++ b/drivers/hid/hid-roccat-konepure.c
@@ -0,0 +1,304 @@
+/*
+ * Roccat KonePure driver for Linux
+ *
+ * Copyright (c) 2012 Stefan Achatz <erazor_de@users.sourceforge.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.
+ */
+
+/*
+ * Roccat KonePure is a smaller version of KoneXTD with less buttons and lights.
+ */
+
+#include <linux/device.h>
+#include <linux/input.h>
+#include <linux/hid.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/hid-roccat.h>
+#include "hid-ids.h"
+#include "hid-roccat-common.h"
+#include "hid-roccat-konepure.h"
+
+static struct class *konepure_class;
+
+static ssize_t konepure_sysfs_read(struct file *fp, struct kobject *kobj,
+		char *buf, loff_t off, size_t count,
+		size_t real_size, uint command)
+{
+	struct device *dev =
+			container_of(kobj, struct device, kobj)->parent->parent;
+	struct konepure_device *konepure = hid_get_drvdata(dev_get_drvdata(dev));
+	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
+	int retval;
+
+	if (off >= real_size)
+		return 0;
+
+	if (off != 0 || count != real_size)
+		return -EINVAL;
+
+	mutex_lock(&konepure->konepure_lock);
+	retval = roccat_common2_receive(usb_dev, command, buf, real_size);
+	mutex_unlock(&konepure->konepure_lock);
+
+	return retval ? retval : real_size;
+}
+
+static ssize_t konepure_sysfs_write(struct file *fp, struct kobject *kobj,
+		void const *buf, loff_t off, size_t count,
+		size_t real_size, uint command)
+{
+	struct device *dev =
+			container_of(kobj, struct device, kobj)->parent->parent;
+	struct konepure_device *konepure = hid_get_drvdata(dev_get_drvdata(dev));
+	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
+	int retval;
+
+	if (off != 0 || count != real_size)
+		return -EINVAL;
+
+	mutex_lock(&konepure->konepure_lock);
+	retval = roccat_common2_send_with_status(usb_dev, command,
+			(void *)buf, real_size);
+	mutex_unlock(&konepure->konepure_lock);
+
+	return retval ? retval : real_size;
+}
+
+#define KONEPURE_SYSFS_W(thingy, THINGY) \
+static ssize_t konepure_sysfs_write_ ## thingy(struct file *fp, \
+		struct kobject *kobj, struct bin_attribute *attr, char *buf, \
+		loff_t off, size_t count) \
+{ \
+	return konepure_sysfs_write(fp, kobj, buf, off, count, \
+			KONEPURE_SIZE_ ## THINGY, KONEPURE_COMMAND_ ## THINGY); \
+}
+
+#define KONEPURE_SYSFS_R(thingy, THINGY) \
+static ssize_t konepure_sysfs_read_ ## thingy(struct file *fp, \
+		struct kobject *kobj, struct bin_attribute *attr, char *buf, \
+		loff_t off, size_t count) \
+{ \
+	return konepure_sysfs_read(fp, kobj, buf, off, count, \
+			KONEPURE_SIZE_ ## THINGY, KONEPURE_COMMAND_ ## THINGY); \
+}
+
+#define KONEPURE_SYSFS_RW(thingy, THINGY) \
+KONEPURE_SYSFS_W(thingy, THINGY) \
+KONEPURE_SYSFS_R(thingy, THINGY)
+
+#define KONEPURE_BIN_ATTRIBUTE_RW(thingy, THINGY) \
+{ \
+	.attr = { .name = #thingy, .mode = 0660 }, \
+	.size = KONEPURE_SIZE_ ## THINGY, \
+	.read = konepure_sysfs_read_ ## thingy, \
+	.write = konepure_sysfs_write_ ## thingy \
+}
+
+#define KONEPURE_BIN_ATTRIBUTE_R(thingy, THINGY) \
+{ \
+	.attr = { .name = #thingy, .mode = 0440 }, \
+	.size = KONEPURE_SIZE_ ## THINGY, \
+	.read = konepure_sysfs_read_ ## thingy, \
+}
+
+#define KONEPURE_BIN_ATTRIBUTE_W(thingy, THINGY) \
+{ \
+	.attr = { .name = #thingy, .mode = 0220 }, \
+	.size = KONEPURE_SIZE_ ## THINGY, \
+	.write = konepure_sysfs_write_ ## thingy \
+}
+
+KONEPURE_SYSFS_RW(actual_profile, ACTUAL_PROFILE)
+KONEPURE_SYSFS_W(control, CONTROL)
+KONEPURE_SYSFS_RW(info, INFO)
+KONEPURE_SYSFS_W(talk, TALK)
+KONEPURE_SYSFS_W(macro, MACRO)
+KONEPURE_SYSFS_RW(sensor, SENSOR)
+KONEPURE_SYSFS_RW(tcu, TCU)
+KONEPURE_SYSFS_R(tcu_image, TCU_IMAGE)
+KONEPURE_SYSFS_RW(profile_settings, PROFILE_SETTINGS)
+KONEPURE_SYSFS_RW(profile_buttons, PROFILE_BUTTONS)
+
+static struct bin_attribute konepure_bin_attributes[] = {
+	KONEPURE_BIN_ATTRIBUTE_RW(actual_profile, ACTUAL_PROFILE),
+	KONEPURE_BIN_ATTRIBUTE_W(control, CONTROL),
+	KONEPURE_BIN_ATTRIBUTE_RW(info, INFO),
+	KONEPURE_BIN_ATTRIBUTE_W(talk, TALK),
+	KONEPURE_BIN_ATTRIBUTE_W(macro, MACRO),
+	KONEPURE_BIN_ATTRIBUTE_RW(sensor, SENSOR),
+	KONEPURE_BIN_ATTRIBUTE_RW(tcu, TCU),
+	KONEPURE_BIN_ATTRIBUTE_R(tcu_image, TCU_IMAGE),
+	KONEPURE_BIN_ATTRIBUTE_RW(profile_settings, PROFILE_SETTINGS),
+	KONEPURE_BIN_ATTRIBUTE_RW(profile_buttons, PROFILE_BUTTONS),
+	__ATTR_NULL
+};
+
+static int konepure_init_konepure_device_struct(struct usb_device *usb_dev,
+		struct konepure_device *konepure)
+{
+	mutex_init(&konepure->konepure_lock);
+
+	return 0;
+}
+
+static int konepure_init_specials(struct hid_device *hdev)
+{
+	struct usb_interface *intf = to_usb_interface(hdev->dev.parent);
+	struct usb_device *usb_dev = interface_to_usbdev(intf);
+	struct konepure_device *konepure;
+	int retval;
+
+	if (intf->cur_altsetting->desc.bInterfaceProtocol
+			!= USB_INTERFACE_PROTOCOL_MOUSE) {
+		hid_set_drvdata(hdev, NULL);
+		return 0;
+	}
+
+	konepure = kzalloc(sizeof(*konepure), GFP_KERNEL);
+	if (!konepure) {
+		hid_err(hdev, "can't alloc device descriptor\n");
+		return -ENOMEM;
+	}
+	hid_set_drvdata(hdev, konepure);
+
+	retval = konepure_init_konepure_device_struct(usb_dev, konepure);
+	if (retval) {
+		hid_err(hdev, "couldn't init struct konepure_device\n");
+		goto exit_free;
+	}
+
+	retval = roccat_connect(konepure_class, hdev,
+			sizeof(struct konepure_mouse_report_button));
+	if (retval < 0) {
+		hid_err(hdev, "couldn't init char dev\n");
+	} else {
+		konepure->chrdev_minor = retval;
+		konepure->roccat_claimed = 1;
+	}
+
+	return 0;
+exit_free:
+	kfree(konepure);
+	return retval;
+}
+
+static void konepure_remove_specials(struct hid_device *hdev)
+{
+	struct usb_interface *intf = to_usb_interface(hdev->dev.parent);
+	struct konepure_device *konepure;
+
+	if (intf->cur_altsetting->desc.bInterfaceProtocol
+			!= USB_INTERFACE_PROTOCOL_MOUSE)
+		return;
+
+	konepure = hid_get_drvdata(hdev);
+	if (konepure->roccat_claimed)
+		roccat_disconnect(konepure->chrdev_minor);
+	kfree(konepure);
+}
+
+static int konepure_probe(struct hid_device *hdev,
+		const struct hid_device_id *id)
+{
+	int retval;
+
+	retval = hid_parse(hdev);
+	if (retval) {
+		hid_err(hdev, "parse failed\n");
+		goto exit;
+	}
+
+	retval = hid_hw_start(hdev, HID_CONNECT_DEFAULT);
+	if (retval) {
+		hid_err(hdev, "hw start failed\n");
+		goto exit;
+	}
+
+	retval = konepure_init_specials(hdev);
+	if (retval) {
+		hid_err(hdev, "couldn't install mouse\n");
+		goto exit_stop;
+	}
+
+	return 0;
+
+exit_stop:
+	hid_hw_stop(hdev);
+exit:
+	return retval;
+}
+
+static void konepure_remove(struct hid_device *hdev)
+{
+	konepure_remove_specials(hdev);
+	hid_hw_stop(hdev);
+}
+
+static int konepure_raw_event(struct hid_device *hdev,
+		struct hid_report *report, u8 *data, int size)
+{
+	struct usb_interface *intf = to_usb_interface(hdev->dev.parent);
+	struct konepure_device *konepure = hid_get_drvdata(hdev);
+
+	if (intf->cur_altsetting->desc.bInterfaceProtocol
+			!= USB_INTERFACE_PROTOCOL_MOUSE)
+		return 0;
+
+	if (data[0] != KONEPURE_MOUSE_REPORT_NUMBER_BUTTON)
+		return 0;
+
+	if (konepure != NULL && konepure->roccat_claimed)
+		roccat_report_event(konepure->chrdev_minor, data);
+
+	return 0;
+}
+
+static const struct hid_device_id konepure_devices[] = {
+	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_KONEPURE) },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(hid, konepure_devices);
+
+static struct hid_driver konepure_driver = {
+		.name = "konepure",
+		.id_table = konepure_devices,
+		.probe = konepure_probe,
+		.remove = konepure_remove,
+		.raw_event = konepure_raw_event
+};
+
+static int __init konepure_init(void)
+{
+	int retval;
+
+	konepure_class = class_create(THIS_MODULE, "konepure");
+	if (IS_ERR(konepure_class))
+		return PTR_ERR(konepure_class);
+	konepure_class->dev_bin_attrs = konepure_bin_attributes;
+
+	retval = hid_register_driver(&konepure_driver);
+	if (retval)
+		class_destroy(konepure_class);
+	return retval;
+}
+
+static void __exit konepure_exit(void)
+{
+	hid_unregister_driver(&konepure_driver);
+	class_destroy(konepure_class);
+}
+
+module_init(konepure_init);
+module_exit(konepure_exit);
+
+MODULE_AUTHOR("Stefan Achatz");
+MODULE_DESCRIPTION("USB Roccat KonePure driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/hid/hid-roccat-konepure.h b/drivers/hid/hid-roccat-konepure.h
new file mode 100644
index 00000000000..2cd24e93dfd
--- /dev/null
+++ b/drivers/hid/hid-roccat-konepure.h
@@ -0,0 +1,72 @@
+#ifndef __HID_ROCCAT_KONEPURE_H
+#define __HID_ROCCAT_KONEPURE_H
+
+/*
+ * Copyright (c) 2012 Stefan Achatz <erazor_de@users.sourceforge.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.
+ */
+
+#include <linux/types.h>
+
+enum {
+	KONEPURE_SIZE_ACTUAL_PROFILE = 0x03,
+	KONEPURE_SIZE_CONTROL = 0x03,
+	KONEPURE_SIZE_FIRMWARE_WRITE = 0x0402,
+	KONEPURE_SIZE_INFO = 0x06,
+	KONEPURE_SIZE_MACRO = 0x0822,
+	KONEPURE_SIZE_PROFILE_SETTINGS = 0x1f,
+	KONEPURE_SIZE_PROFILE_BUTTONS = 0x3b,
+	KONEPURE_SIZE_SENSOR = 0x06,
+	KONEPURE_SIZE_TALK = 0x10,
+	KONEPURE_SIZE_TCU = 0x04,
+	KONEPURE_SIZE_TCU_IMAGE = 0x0404,
+};
+
+enum konepure_control_requests {
+	KONEPURE_CONTROL_REQUEST_GENERAL = 0x80,
+	KONEPURE_CONTROL_REQUEST_BUTTONS = 0x90,
+};
+
+enum konepure_commands {
+	KONEPURE_COMMAND_CONTROL = 0x04,
+	KONEPURE_COMMAND_ACTUAL_PROFILE = 0x05,
+	KONEPURE_COMMAND_PROFILE_SETTINGS = 0x06,
+	KONEPURE_COMMAND_PROFILE_BUTTONS = 0x07,
+	KONEPURE_COMMAND_MACRO = 0x08,
+	KONEPURE_COMMAND_INFO = 0x09,
+	KONEPURE_COMMAND_TCU = 0x0c,
+	KONEPURE_COMMAND_TCU_IMAGE = 0x0c,
+	KONEPURE_COMMAND_E = 0x0e,
+	KONEPURE_COMMAND_SENSOR = 0x0f,
+	KONEPURE_COMMAND_TALK = 0x10,
+	KONEPURE_COMMAND_FIRMWARE_WRITE = 0x1b,
+	KONEPURE_COMMAND_FIRMWARE_WRITE_CONTROL = 0x1c,
+};
+
+enum {
+	KONEPURE_MOUSE_REPORT_NUMBER_BUTTON = 3,
+};
+
+struct konepure_mouse_report_button {
+	uint8_t report_number; /* always KONEPURE_MOUSE_REPORT_NUMBER_BUTTON */
+	uint8_t zero;
+	uint8_t type;
+	uint8_t data1;
+	uint8_t data2;
+	uint8_t zero2;
+	uint8_t unknown[2];
+} __packed;
+
+struct konepure_device {
+	int roccat_claimed;
+	int chrdev_minor;
+	struct mutex konepure_lock;
+};
+
+#endif
diff --git a/drivers/hid/hid-roccat.c b/drivers/hid/hid-roccat.c
index d7437ef5c69..b59b3df9ca9 100644
--- a/drivers/hid/hid-roccat.c
+++ b/drivers/hid/hid-roccat.c
@@ -242,7 +242,6 @@ static int roccat_release(struct inode *inode, struct file *file)
  * roccat_report_event() - output data to readers
  * @minor: minor device number returned by roccat_connect()
  * @data: pointer to data
- * @len: size of data
  *
  * Return value is zero on success, a negative error code on failure.
  *
@@ -290,6 +289,7 @@ EXPORT_SYMBOL_GPL(roccat_report_event);
  * @class: the class thats used to create the device. Meant to hold device
  * specific sysfs attributes.
  * @hid: the hid device the char device should be connected to.
+ * @report_size: size of reports
  *
  * Return value is minor device number in Range [0, ROCCAT_MAX_DEVICES] on
  * success, a negative error code on failure.
diff --git a/drivers/hid/hid-sensor-hub.c b/drivers/hid/hid-sensor-hub.c
index 6679788bf75..ca749810732 100644
--- a/drivers/hid/hid-sensor-hub.c
+++ b/drivers/hid/hid-sensor-hub.c
@@ -18,8 +18,6 @@
  */
 #include <linux/device.h>
 #include <linux/hid.h>
-#include <linux/usb.h>
-#include "usbhid/usbhid.h"
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/mfd/core.h>
@@ -204,8 +202,8 @@ int sensor_hub_set_feature(struct hid_sensor_hub_device *hsdev, u32 report_id,
 		goto done_proc;
 	}
 	hid_set_field(report->field[field_index], 0, value);
-	usbhid_submit_report(hsdev->hdev, report, USB_DIR_OUT);
-	usbhid_wait_io(hsdev->hdev);
+	hid_hw_request(hsdev->hdev, report, HID_REQ_SET_REPORT);
+	hid_hw_wait(hsdev->hdev);
 
 done_proc:
 	mutex_unlock(&data->mutex);
@@ -227,8 +225,8 @@ int sensor_hub_get_feature(struct hid_sensor_hub_device *hsdev, u32 report_id,
 		ret = -EINVAL;
 		goto done_proc;
 	}
-	usbhid_submit_report(hsdev->hdev, report, USB_DIR_IN);
-	usbhid_wait_io(hsdev->hdev);
+	hid_hw_request(hsdev->hdev, report, HID_REQ_GET_REPORT);
+	hid_hw_wait(hsdev->hdev);
 	*value = report->field[field_index]->value[0];
 
 done_proc:
@@ -262,7 +260,7 @@ int sensor_hub_input_attr_get_raw_value(struct hid_sensor_hub_device *hsdev,
 		spin_unlock_irqrestore(&data->lock, flags);
 		goto err_free;
 	}
-	usbhid_submit_report(hsdev->hdev, report, USB_DIR_IN);
+	hid_hw_request(hsdev->hdev, report, HID_REQ_GET_REPORT);
 	spin_unlock_irqrestore(&data->lock, flags);
 	wait_for_completion_interruptible_timeout(&data->pending.ready, HZ*5);
 	switch (data->pending.raw_size) {
diff --git a/drivers/hid/hid-sjoy.c b/drivers/hid/hid-sjoy.c
index 28f774003f0..37845eccddb 100644
--- a/drivers/hid/hid-sjoy.c
+++ b/drivers/hid/hid-sjoy.c
@@ -28,13 +28,11 @@
 
 #include <linux/input.h>
 #include <linux/slab.h>
-#include <linux/usb.h>
 #include <linux/hid.h>
 #include <linux/module.h>
 #include "hid-ids.h"
 
 #ifdef CONFIG_SMARTJOYPLUS_FF
-#include "usbhid/usbhid.h"
 
 struct sjoyff_device {
 	struct hid_report *report;
@@ -57,7 +55,7 @@ static int hid_sjoyff_play(struct input_dev *dev, void *data,
 	sjoyff->report->field[0]->value[1] = right;
 	sjoyff->report->field[0]->value[2] = left;
 	dev_dbg(&dev->dev, "running with 0x%02x 0x%02x\n", left, right);
-	usbhid_submit_report(hid, sjoyff->report, USB_DIR_OUT);
+	hid_hw_request(hid, sjoyff->report, HID_REQ_SET_REPORT);
 
 	return 0;
 }
@@ -115,7 +113,7 @@ static int sjoyff_init(struct hid_device *hid)
 		sjoyff->report->field[0]->value[0] = 0x01;
 		sjoyff->report->field[0]->value[1] = 0x00;
 		sjoyff->report->field[0]->value[2] = 0x00;
-		usbhid_submit_report(hid, sjoyff->report, USB_DIR_OUT);
+		hid_hw_request(hid, sjoyff->report, HID_REQ_SET_REPORT);
 	}
 
 	hid_info(hid, "Force feedback for SmartJoy PLUS PS2/USB adapter\n");
diff --git a/drivers/hid/hid-speedlink.c b/drivers/hid/hid-speedlink.c
index e94371a059c..a2f587d004e 100644
--- a/drivers/hid/hid-speedlink.c
+++ b/drivers/hid/hid-speedlink.c
@@ -16,10 +16,8 @@
 #include <linux/device.h>
 #include <linux/hid.h>
 #include <linux/module.h>
-#include <linux/usb.h>
 
 #include "hid-ids.h"
-#include "usbhid/usbhid.h"
 
 static const struct hid_device_id speedlink_devices[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_X_TENSIONS, USB_DEVICE_ID_SPEEDLINK_VAD_CEZANNE)},
diff --git a/drivers/hid/hid-steelseries.c b/drivers/hid/hid-steelseries.c
index 2ed995cda44..9b0efb0083f 100644
--- a/drivers/hid/hid-steelseries.c
+++ b/drivers/hid/hid-steelseries.c
@@ -16,7 +16,6 @@
 #include <linux/hid.h>
 #include <linux/module.h>
 
-#include "usbhid/usbhid.h"
 #include "hid-ids.h"
 
 #if defined(CONFIG_LEDS_CLASS) || defined(CONFIG_LEDS_CLASS_MODULE)
@@ -132,7 +131,7 @@ static void steelseries_srws1_set_leds(struct hid_device *hdev, __u16 leds)
 	value[14] = 0x00;
 	value[15] = 0x00;
 
-	usbhid_submit_report(hdev, report, USB_DIR_OUT);
+	hid_hw_request(hdev, report, HID_REQ_SET_REPORT);
 
 	/* Note: LED change does not show on device until the device is read/polled */
 }
@@ -378,16 +377,5 @@ static struct hid_driver steelseries_srws1_driver = {
 	.report_fixup = steelseries_srws1_report_fixup
 };
 
-static int __init steelseries_srws1_init(void)
-{
-	return hid_register_driver(&steelseries_srws1_driver);
-}
-
-static void __exit steelseries_srws1_exit(void)
-{
-	hid_unregister_driver(&steelseries_srws1_driver);
-}
-
-module_init(steelseries_srws1_init);
-module_exit(steelseries_srws1_exit);
+module_hid_driver(steelseries_srws1_driver);
 MODULE_LICENSE("GPL");
diff --git a/drivers/hid/hid-thingm.c b/drivers/hid/hid-thingm.c
index 2055a52e9a2..99342cfa0ea 100644
--- a/drivers/hid/hid-thingm.c
+++ b/drivers/hid/hid-thingm.c
@@ -12,7 +12,6 @@
 #include <linux/hid.h>
 #include <linux/leds.h>
 #include <linux/module.h>
-#include <linux/usb.h>
 
 #include "hid-ids.h"
 
diff --git a/drivers/hid/hid-tmff.c b/drivers/hid/hid-tmff.c
index e4fcf3f702a..b83376077d7 100644
--- a/drivers/hid/hid-tmff.c
+++ b/drivers/hid/hid-tmff.c
@@ -30,7 +30,6 @@
 #include <linux/hid.h>
 #include <linux/input.h>
 #include <linux/slab.h>
-#include <linux/usb.h>
 #include <linux/module.h>
 
 #include "hid-ids.h"
@@ -46,7 +45,6 @@ static const signed short ff_joystick[] = {
 };
 
 #ifdef CONFIG_THRUSTMASTER_FF
-#include "usbhid/usbhid.h"
 
 /* Usages for thrustmaster devices I know about */
 #define THRUSTMASTER_USAGE_FF	(HID_UP_GENDESK | 0xbb)
@@ -103,7 +101,7 @@ static int tmff_play(struct input_dev *dev, void *data,
 		dbg_hid("(x, y)=(%04x, %04x)\n", x, y);
 		ff_field->value[0] = x;
 		ff_field->value[1] = y;
-		usbhid_submit_report(hid, tmff->report, USB_DIR_OUT);
+		hid_hw_request(hid, tmff->report, HID_REQ_SET_REPORT);
 		break;
 
 	case FF_RUMBLE:
@@ -117,7 +115,7 @@ static int tmff_play(struct input_dev *dev, void *data,
 		dbg_hid("(left,right)=(%08x, %08x)\n", left, right);
 		ff_field->value[0] = left;
 		ff_field->value[1] = right;
-		usbhid_submit_report(hid, tmff->report, USB_DIR_OUT);
+		hid_hw_request(hid, tmff->report, HID_REQ_SET_REPORT);
 		break;
 	}
 	return 0;
diff --git a/drivers/hid/hid-wiimote-core.c b/drivers/hid/hid-wiimote-core.c
index 0fb8ab93db6..e5ee1f20bbd 100644
--- a/drivers/hid/hid-wiimote-core.c
+++ b/drivers/hid/hid-wiimote-core.c
@@ -789,12 +789,20 @@ static void __ir_to_input(struct wiimote_data *wdata, const __u8 *ir,
 	input_report_abs(wdata->ir, yid, y);
 }
 
-static void handler_status(struct wiimote_data *wdata, const __u8 *payload)
+/* reduced status report with "BB BB" key data only */
+static void handler_status_K(struct wiimote_data *wdata,
+			     const __u8 *payload)
 {
 	handler_keys(wdata, payload);
 
 	/* on status reports the drm is reset so we need to resend the drm */
 	wiiproto_req_drm(wdata, WIIPROTO_REQ_NULL);
+}
+
+/* extended status report with "BB BB LF 00 00 VV" data */
+static void handler_status(struct wiimote_data *wdata, const __u8 *payload)
+{
+	handler_status_K(wdata, payload);
 
 	wiiext_event(wdata, payload[2] & 0x02);
 
@@ -804,6 +812,12 @@ static void handler_status(struct wiimote_data *wdata, const __u8 *payload)
 	}
 }
 
+/* reduced generic report with "BB BB" key data only */
+static void handler_generic_K(struct wiimote_data *wdata, const __u8 *payload)
+{
+	handler_keys(wdata, payload);
+}
+
 static void handler_data(struct wiimote_data *wdata, const __u8 *payload)
 {
 	__u16 offset = payload[3] << 8 | payload[4];
@@ -947,16 +961,26 @@ struct wiiproto_handler {
 
 static struct wiiproto_handler handlers[] = {
 	{ .id = WIIPROTO_REQ_STATUS, .size = 6, .func = handler_status },
+	{ .id = WIIPROTO_REQ_STATUS, .size = 2, .func = handler_status_K },
 	{ .id = WIIPROTO_REQ_DATA, .size = 21, .func = handler_data },
+	{ .id = WIIPROTO_REQ_DATA, .size = 2, .func = handler_generic_K },
 	{ .id = WIIPROTO_REQ_RETURN, .size = 4, .func = handler_return },
+	{ .id = WIIPROTO_REQ_RETURN, .size = 2, .func = handler_generic_K },
 	{ .id = WIIPROTO_REQ_DRM_K, .size = 2, .func = handler_keys },
 	{ .id = WIIPROTO_REQ_DRM_KA, .size = 5, .func = handler_drm_KA },
+	{ .id = WIIPROTO_REQ_DRM_KA, .size = 2, .func = handler_generic_K },
 	{ .id = WIIPROTO_REQ_DRM_KE, .size = 10, .func = handler_drm_KE },
+	{ .id = WIIPROTO_REQ_DRM_KE, .size = 2, .func = handler_generic_K },
 	{ .id = WIIPROTO_REQ_DRM_KAI, .size = 17, .func = handler_drm_KAI },
+	{ .id = WIIPROTO_REQ_DRM_KAI, .size = 2, .func = handler_generic_K },
 	{ .id = WIIPROTO_REQ_DRM_KEE, .size = 21, .func = handler_drm_KEE },
+	{ .id = WIIPROTO_REQ_DRM_KEE, .size = 2, .func = handler_generic_K },
 	{ .id = WIIPROTO_REQ_DRM_KAE, .size = 21, .func = handler_drm_KAE },
+	{ .id = WIIPROTO_REQ_DRM_KAE, .size = 2, .func = handler_generic_K },
 	{ .id = WIIPROTO_REQ_DRM_KIE, .size = 21, .func = handler_drm_KIE },
+	{ .id = WIIPROTO_REQ_DRM_KIE, .size = 2, .func = handler_generic_K },
 	{ .id = WIIPROTO_REQ_DRM_KAIE, .size = 21, .func = handler_drm_KAIE },
+	{ .id = WIIPROTO_REQ_DRM_KAIE, .size = 2, .func = handler_generic_K },
 	{ .id = WIIPROTO_REQ_DRM_E, .size = 21, .func = handler_drm_E },
 	{ .id = WIIPROTO_REQ_DRM_SKAI1, .size = 21, .func = handler_drm_SKAI1 },
 	{ .id = WIIPROTO_REQ_DRM_SKAI2, .size = 21, .func = handler_drm_SKAI2 },
@@ -970,7 +994,6 @@ static int wiimote_hid_event(struct hid_device *hdev, struct hid_report *report,
 	struct wiiproto_handler *h;
 	int i;
 	unsigned long flags;
-	bool handled = false;
 
 	if (size < 1)
 		return -EINVAL;
@@ -981,11 +1004,11 @@ static int wiimote_hid_event(struct hid_device *hdev, struct hid_report *report,
 		h = &handlers[i];
 		if (h->id == raw_data[0] && h->size < size) {
 			h->func(wdata, &raw_data[1]);
-			handled = true;
+			break;
 		}
 	}
 
-	if (!handled)
+	if (!handlers[i].id)
 		hid_warn(hdev, "Unhandled report %hhu size %d\n", raw_data[0],
 									size);
 
@@ -1160,6 +1183,7 @@ static void wiimote_destroy(struct wiimote_data *wdata)
 	wiimote_leds_destroy(wdata);
 
 	power_supply_unregister(&wdata->battery);
+	kfree(wdata->battery.name);
 	input_unregister_device(wdata->accel);
 	input_unregister_device(wdata->ir);
 	input_unregister_device(wdata->input);
@@ -1216,9 +1240,14 @@ static int wiimote_hid_probe(struct hid_device *hdev,
 	wdata->battery.properties = wiimote_battery_props;
 	wdata->battery.num_properties = ARRAY_SIZE(wiimote_battery_props);
 	wdata->battery.get_property = wiimote_battery_get_property;
-	wdata->battery.name = "wiimote_battery";
 	wdata->battery.type = POWER_SUPPLY_TYPE_BATTERY;
 	wdata->battery.use_for_apm = 0;
+	wdata->battery.name = kasprintf(GFP_KERNEL, "wiimote_battery_%s",
+					wdata->hdev->uniq);
+	if (!wdata->battery.name) {
+		ret = -ENOMEM;
+		goto err_battery_name;
+	}
 
 	ret = power_supply_register(&wdata->hdev->dev, &wdata->battery);
 	if (ret) {
@@ -1254,6 +1283,8 @@ err_free:
 	return ret;
 
 err_battery:
+	kfree(wdata->battery.name);
+err_battery_name:
 	input_unregister_device(wdata->input);
 	wdata->input = NULL;
 err_input:
@@ -1283,6 +1314,8 @@ static void wiimote_hid_remove(struct hid_device *hdev)
 static const struct hid_device_id wiimote_hid_devices[] = {
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_NINTENDO,
 				USB_DEVICE_ID_NINTENDO_WIIMOTE) },
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_NINTENDO,
+				USB_DEVICE_ID_NINTENDO_WIIMOTE2) },
 	{ }
 };
 MODULE_DEVICE_TABLE(hid, wiimote_hid_devices);
diff --git a/drivers/hid/hid-zpff.c b/drivers/hid/hid-zpff.c
index af66452592e..6ec28a37c14 100644
--- a/drivers/hid/hid-zpff.c
+++ b/drivers/hid/hid-zpff.c
@@ -24,13 +24,11 @@
 #include <linux/hid.h>
 #include <linux/input.h>
 #include <linux/slab.h>
-#include <linux/usb.h>
 #include <linux/module.h>
 
 #include "hid-ids.h"
 
 #ifdef CONFIG_ZEROPLUS_FF
-#include "usbhid/usbhid.h"
 
 struct zpff_device {
 	struct hid_report *report;
@@ -59,7 +57,7 @@ static int zpff_play(struct input_dev *dev, void *data,
 	zpff->report->field[2]->value[0] = left;
 	zpff->report->field[3]->value[0] = right;
 	dbg_hid("running with 0x%02x 0x%02x\n", left, right);
-	usbhid_submit_report(hid, zpff->report, USB_DIR_OUT);
+	hid_hw_request(hid, zpff->report, HID_REQ_SET_REPORT);
 
 	return 0;
 }
@@ -104,7 +102,7 @@ static int zpff_init(struct hid_device *hid)
 	zpff->report->field[1]->value[0] = 0x02;
 	zpff->report->field[2]->value[0] = 0x00;
 	zpff->report->field[3]->value[0] = 0x00;
-	usbhid_submit_report(hid, zpff->report, USB_DIR_OUT);
+	hid_hw_request(hid, zpff->report, HID_REQ_SET_REPORT);
 
 	hid_info(hid, "force feedback for Zeroplus based devices by Anssi Hannula <anssi.hannula@gmail.com>\n");
 
diff --git a/drivers/hid/i2c-hid/i2c-hid.c b/drivers/hid/i2c-hid/i2c-hid.c
index ec7930217a6..2b1799a3b21 100644
--- a/drivers/hid/i2c-hid/i2c-hid.c
+++ b/drivers/hid/i2c-hid/i2c-hid.c
@@ -563,6 +563,36 @@ static int i2c_hid_output_raw_report(struct hid_device *hid, __u8 *buf,
 	return ret;
 }
 
+static void i2c_hid_request(struct hid_device *hid, struct hid_report *rep,
+		int reqtype)
+{
+	struct i2c_client *client = hid->driver_data;
+	char *buf;
+	int ret;
+	int len = i2c_hid_get_report_length(rep) - 2;
+
+	buf = kzalloc(len, GFP_KERNEL);
+	if (!buf)
+		return;
+
+	switch (reqtype) {
+	case HID_REQ_GET_REPORT:
+		ret = i2c_hid_get_raw_report(hid, rep->id, buf, len, rep->type);
+		if (ret < 0)
+			dev_err(&client->dev, "%s: unable to get report: %d\n",
+				__func__, ret);
+		else
+			hid_input_report(hid, rep->type, buf, ret, 0);
+		break;
+	case HID_REQ_SET_REPORT:
+		hid_output_report(rep, buf);
+		i2c_hid_output_raw_report(hid, buf, len, rep->type);
+		break;
+	}
+
+	kfree(buf);
+}
+
 static int i2c_hid_parse(struct hid_device *hid)
 {
 	struct i2c_client *client = hid->driver_data;
@@ -742,6 +772,7 @@ static struct hid_ll_driver i2c_hid_ll_driver = {
 	.open = i2c_hid_open,
 	.close = i2c_hid_close,
 	.power = i2c_hid_power,
+	.request = i2c_hid_request,
 	.hidinput_input_event = i2c_hid_hidinput_input_event,
 };
 
diff --git a/drivers/hid/usbhid/hid-core.c b/drivers/hid/usbhid/hid-core.c
index 8e0c4bf94eb..99418285222 100644
--- a/drivers/hid/usbhid/hid-core.c
+++ b/drivers/hid/usbhid/hid-core.c
@@ -639,7 +639,7 @@ static void __usbhid_submit_report(struct hid_device *hid, struct hid_report *re
 	}
 }
 
-void usbhid_submit_report(struct hid_device *hid, struct hid_report *report, unsigned char dir)
+static void usbhid_submit_report(struct hid_device *hid, struct hid_report *report, unsigned char dir)
 {
 	struct usbhid_device *usbhid = hid->driver_data;
 	unsigned long flags;
@@ -648,7 +648,6 @@ void usbhid_submit_report(struct hid_device *hid, struct hid_report *report, uns
 	__usbhid_submit_report(hid, report, dir);
 	spin_unlock_irqrestore(&usbhid->lock, flags);
 }
-EXPORT_SYMBOL_GPL(usbhid_submit_report);
 
 /* Workqueue routine to send requests to change LEDs */
 static void hid_led(struct work_struct *work)
@@ -706,7 +705,7 @@ static int usb_hidinput_input_event(struct input_dev *dev, unsigned int type, un
 	return 0;
 }
 
-int usbhid_wait_io(struct hid_device *hid)
+static int usbhid_wait_io(struct hid_device *hid)
 {
 	struct usbhid_device *usbhid = hid->driver_data;
 
@@ -720,7 +719,6 @@ int usbhid_wait_io(struct hid_device *hid)
 
 	return 0;
 }
-EXPORT_SYMBOL_GPL(usbhid_wait_io);
 
 static int hid_set_idle(struct usb_device *dev, int ifnum, int report, int idle)
 {
@@ -1243,6 +1241,32 @@ static int usbhid_power(struct hid_device *hid, int lvl)
 	return r;
 }
 
+static void usbhid_request(struct hid_device *hid, struct hid_report *rep, int reqtype)
+{
+	switch (reqtype) {
+	case HID_REQ_GET_REPORT:
+		usbhid_submit_report(hid, rep, USB_DIR_IN);
+		break;
+	case HID_REQ_SET_REPORT:
+		usbhid_submit_report(hid, rep, USB_DIR_OUT);
+		break;
+	}
+}
+
+static int usbhid_idle(struct hid_device *hid, int report, int idle,
+		int reqtype)
+{
+	struct usb_device *dev = hid_to_usb_dev(hid);
+	struct usb_interface *intf = to_usb_interface(hid->dev.parent);
+	struct usb_host_interface *interface = intf->cur_altsetting;
+	int ifnum = interface->desc.bInterfaceNumber;
+
+	if (reqtype != HID_REQ_SET_IDLE)
+		return -EINVAL;
+
+	return hid_set_idle(dev, ifnum, report, idle);
+}
+
 static struct hid_ll_driver usb_hid_driver = {
 	.parse = usbhid_parse,
 	.start = usbhid_start,
@@ -1251,6 +1275,9 @@ static struct hid_ll_driver usb_hid_driver = {
 	.close = usbhid_close,
 	.power = usbhid_power,
 	.hidinput_input_event = usb_hidinput_input_event,
+	.request = usbhid_request,
+	.wait = usbhid_wait_io,
+	.idle = usbhid_idle,
 };
 
 static int usbhid_probe(struct usb_interface *intf, const struct usb_device_id *id)
@@ -1493,7 +1520,7 @@ static int hid_suspend(struct usb_interface *intf, pm_message_t message)
 {
 	struct hid_device *hid = usb_get_intfdata(intf);
 	struct usbhid_device *usbhid = hid->driver_data;
-	int status;
+	int status = 0;
 	bool driver_suspended = false;
 
 	if (PMSG_IS_AUTO(message)) {
@@ -1520,19 +1547,15 @@ static int hid_suspend(struct usb_interface *intf, pm_message_t message)
 		}
 
 	} else {
-		if (hid->driver && hid->driver->suspend) {
+		/* TODO: resume() might need to handle suspend failure */
+		if (hid->driver && hid->driver->suspend)
 			status = hid->driver->suspend(hid, message);
-			if (status < 0)
-				return status;
-		}
 		driver_suspended = true;
 		spin_lock_irq(&usbhid->lock);
 		set_bit(HID_SUSPENDED, &usbhid->iofl);
 		spin_unlock_irq(&usbhid->lock);
-		if (usbhid_wait_io(hid) < 0) {
+		if (usbhid_wait_io(hid) < 0)
 			status = -EIO;
-			goto failed;
-		}
 	}
 
 	hid_cancel_delayed_stuff(usbhid);
@@ -1544,7 +1567,7 @@ static int hid_suspend(struct usb_interface *intf, pm_message_t message)
 		goto failed;
 	}
 	dev_dbg(&intf->dev, "suspend\n");
-	return 0;
+	return status;
 
  failed:
 	hid_resume_common(hid, driver_suspended);
diff --git a/drivers/hid/usbhid/hid-pidff.c b/drivers/hid/usbhid/hid-pidff.c
index f91c136821f..10b61670278 100644
--- a/drivers/hid/usbhid/hid-pidff.c
+++ b/drivers/hid/usbhid/hid-pidff.c
@@ -263,8 +263,8 @@ static void pidff_set_envelope_report(struct pidff_device *pidff,
 		envelope->attack_level,
 		pidff->set_envelope[PID_ATTACK_LEVEL].value[0]);
 
-	usbhid_submit_report(pidff->hid, pidff->reports[PID_SET_ENVELOPE],
-			  USB_DIR_OUT);
+	hid_hw_request(pidff->hid, pidff->reports[PID_SET_ENVELOPE],
+			HID_REQ_SET_REPORT);
 }
 
 /*
@@ -290,8 +290,8 @@ static void pidff_set_constant_force_report(struct pidff_device *pidff,
 	pidff_set_signed(&pidff->set_constant[PID_MAGNITUDE],
 			 effect->u.constant.level);
 
-	usbhid_submit_report(pidff->hid, pidff->reports[PID_SET_CONSTANT],
-			  USB_DIR_OUT);
+	hid_hw_request(pidff->hid, pidff->reports[PID_SET_CONSTANT],
+			HID_REQ_SET_REPORT);
 }
 
 /*
@@ -325,8 +325,8 @@ static void pidff_set_effect_report(struct pidff_device *pidff,
 				pidff->effect_direction);
 	pidff->set_effect[PID_START_DELAY].value[0] = effect->replay.delay;
 
-	usbhid_submit_report(pidff->hid, pidff->reports[PID_SET_EFFECT],
-			  USB_DIR_OUT);
+	hid_hw_request(pidff->hid, pidff->reports[PID_SET_EFFECT],
+			HID_REQ_SET_REPORT);
 }
 
 /*
@@ -357,8 +357,8 @@ static void pidff_set_periodic_report(struct pidff_device *pidff,
 	pidff_set(&pidff->set_periodic[PID_PHASE], effect->u.periodic.phase);
 	pidff->set_periodic[PID_PERIOD].value[0] = effect->u.periodic.period;
 
-	usbhid_submit_report(pidff->hid, pidff->reports[PID_SET_PERIODIC],
-			  USB_DIR_OUT);
+	hid_hw_request(pidff->hid, pidff->reports[PID_SET_PERIODIC],
+			HID_REQ_SET_REPORT);
 
 }
 
@@ -399,8 +399,8 @@ static void pidff_set_condition_report(struct pidff_device *pidff,
 			  effect->u.condition[i].left_saturation);
 		pidff_set(&pidff->set_condition[PID_DEAD_BAND],
 			  effect->u.condition[i].deadband);
-		usbhid_submit_report(pidff->hid, pidff->reports[PID_SET_CONDITION],
-				  USB_DIR_OUT);
+		hid_hw_request(pidff->hid, pidff->reports[PID_SET_CONDITION],
+				HID_REQ_SET_REPORT);
 	}
 }
 
@@ -440,8 +440,8 @@ static void pidff_set_ramp_force_report(struct pidff_device *pidff,
 			 effect->u.ramp.start_level);
 	pidff_set_signed(&pidff->set_ramp[PID_RAMP_END],
 			 effect->u.ramp.end_level);
-	usbhid_submit_report(pidff->hid, pidff->reports[PID_SET_RAMP],
-			  USB_DIR_OUT);
+	hid_hw_request(pidff->hid, pidff->reports[PID_SET_RAMP],
+			HID_REQ_SET_REPORT);
 }
 
 /*
@@ -465,19 +465,19 @@ static int pidff_request_effect_upload(struct pidff_device *pidff, int efnum)
 	int j;
 
 	pidff->create_new_effect_type->value[0] = efnum;
-	usbhid_submit_report(pidff->hid, pidff->reports[PID_CREATE_NEW_EFFECT],
-			  USB_DIR_OUT);
+	hid_hw_request(pidff->hid, pidff->reports[PID_CREATE_NEW_EFFECT],
+			HID_REQ_SET_REPORT);
 	hid_dbg(pidff->hid, "create_new_effect sent, type: %d\n", efnum);
 
 	pidff->block_load[PID_EFFECT_BLOCK_INDEX].value[0] = 0;
 	pidff->block_load_status->value[0] = 0;
-	usbhid_wait_io(pidff->hid);
+	hid_hw_wait(pidff->hid);
 
 	for (j = 0; j < 60; j++) {
 		hid_dbg(pidff->hid, "pid_block_load requested\n");
-		usbhid_submit_report(pidff->hid, pidff->reports[PID_BLOCK_LOAD],
-				  USB_DIR_IN);
-		usbhid_wait_io(pidff->hid);
+		hid_hw_request(pidff->hid, pidff->reports[PID_BLOCK_LOAD],
+				HID_REQ_GET_REPORT);
+		hid_hw_wait(pidff->hid);
 		if (pidff->block_load_status->value[0] ==
 		    pidff->status_id[PID_BLOCK_LOAD_SUCCESS]) {
 			hid_dbg(pidff->hid, "device reported free memory: %d bytes\n",
@@ -513,8 +513,8 @@ static void pidff_playback_pid(struct pidff_device *pidff, int pid_id, int n)
 		pidff->effect_operation[PID_LOOP_COUNT].value[0] = n;
 	}
 
-	usbhid_submit_report(pidff->hid, pidff->reports[PID_EFFECT_OPERATION],
-			  USB_DIR_OUT);
+	hid_hw_request(pidff->hid, pidff->reports[PID_EFFECT_OPERATION],
+			HID_REQ_SET_REPORT);
 }
 
 /**
@@ -535,8 +535,8 @@ static int pidff_playback(struct input_dev *dev, int effect_id, int value)
 static void pidff_erase_pid(struct pidff_device *pidff, int pid_id)
 {
 	pidff->block_free[PID_EFFECT_BLOCK_INDEX].value[0] = pid_id;
-	usbhid_submit_report(pidff->hid, pidff->reports[PID_BLOCK_FREE],
-			  USB_DIR_OUT);
+	hid_hw_request(pidff->hid, pidff->reports[PID_BLOCK_FREE],
+			HID_REQ_SET_REPORT);
 }
 
 /*
@@ -551,7 +551,7 @@ static int pidff_erase_effect(struct input_dev *dev, int effect_id)
 		effect_id, pidff->pid_id[effect_id]);
 	/* Wait for the queue to clear. We do not want a full fifo to
 	   prevent the effect removal. */
-	usbhid_wait_io(pidff->hid);
+	hid_hw_wait(pidff->hid);
 	pidff_playback_pid(pidff, pid_id, 0);
 	pidff_erase_pid(pidff, pid_id);
 
@@ -718,8 +718,8 @@ static void pidff_set_gain(struct input_dev *dev, u16 gain)
 	struct pidff_device *pidff = dev->ff->private;
 
 	pidff_set(&pidff->device_gain[PID_DEVICE_GAIN_FIELD], gain);
-	usbhid_submit_report(pidff->hid, pidff->reports[PID_DEVICE_GAIN],
-			  USB_DIR_OUT);
+	hid_hw_request(pidff->hid, pidff->reports[PID_DEVICE_GAIN],
+			HID_REQ_SET_REPORT);
 }
 
 static void pidff_autocenter(struct pidff_device *pidff, u16 magnitude)
@@ -744,8 +744,8 @@ static void pidff_autocenter(struct pidff_device *pidff, u16 magnitude)
 	pidff->set_effect[PID_DIRECTION_ENABLE].value[0] = 1;
 	pidff->set_effect[PID_START_DELAY].value[0] = 0;
 
-	usbhid_submit_report(pidff->hid, pidff->reports[PID_SET_EFFECT],
-			  USB_DIR_OUT);
+	hid_hw_request(pidff->hid, pidff->reports[PID_SET_EFFECT],
+			HID_REQ_SET_REPORT);
 }
 
 /*
@@ -1158,19 +1158,19 @@ static void pidff_reset(struct pidff_device *pidff)
 
 	pidff->device_control->value[0] = pidff->control_id[PID_RESET];
 	/* We reset twice as sometimes hid_wait_io isn't waiting long enough */
-	usbhid_submit_report(hid, pidff->reports[PID_DEVICE_CONTROL], USB_DIR_OUT);
-	usbhid_wait_io(hid);
-	usbhid_submit_report(hid, pidff->reports[PID_DEVICE_CONTROL], USB_DIR_OUT);
-	usbhid_wait_io(hid);
+	hid_hw_request(hid, pidff->reports[PID_DEVICE_CONTROL], HID_REQ_SET_REPORT);
+	hid_hw_wait(hid);
+	hid_hw_request(hid, pidff->reports[PID_DEVICE_CONTROL], HID_REQ_SET_REPORT);
+	hid_hw_wait(hid);
 
 	pidff->device_control->value[0] =
 		pidff->control_id[PID_ENABLE_ACTUATORS];
-	usbhid_submit_report(hid, pidff->reports[PID_DEVICE_CONTROL], USB_DIR_OUT);
-	usbhid_wait_io(hid);
+	hid_hw_request(hid, pidff->reports[PID_DEVICE_CONTROL], HID_REQ_SET_REPORT);
+	hid_hw_wait(hid);
 
 	/* pool report is sometimes messed up, refetch it */
-	usbhid_submit_report(hid, pidff->reports[PID_POOL], USB_DIR_IN);
-	usbhid_wait_io(hid);
+	hid_hw_request(hid, pidff->reports[PID_POOL], HID_REQ_GET_REPORT);
+	hid_hw_wait(hid);
 
 	if (pidff->pool[PID_SIMULTANEOUS_MAX].value) {
 		while (pidff->pool[PID_SIMULTANEOUS_MAX].value[0] < 2) {
@@ -1181,9 +1181,9 @@ static void pidff_reset(struct pidff_device *pidff)
 				break;
 			}
 			hid_dbg(pidff->hid, "pid_pool requested again\n");
-			usbhid_submit_report(hid, pidff->reports[PID_POOL],
-					  USB_DIR_IN);
-			usbhid_wait_io(hid);
+			hid_hw_request(hid, pidff->reports[PID_POOL],
+					  HID_REQ_GET_REPORT);
+			hid_hw_wait(hid);
 		}
 	}
 }
@@ -1269,8 +1269,8 @@ int hid_pidff_init(struct hid_device *hid)
 
 	if (test_bit(FF_GAIN, dev->ffbit)) {
 		pidff_set(&pidff->device_gain[PID_DEVICE_GAIN_FIELD], 0xffff);
-		usbhid_submit_report(hid, pidff->reports[PID_DEVICE_GAIN],
-				     USB_DIR_OUT);
+		hid_hw_request(hid, pidff->reports[PID_DEVICE_GAIN],
+				     HID_REQ_SET_REPORT);
 	}
 
 	error = pidff_check_autocenter(pidff, dev);
diff --git a/drivers/hid/usbhid/hid-quirks.c b/drivers/hid/usbhid/hid-quirks.c
index e0e6abf1cd3..19b8360f233 100644
--- a/drivers/hid/usbhid/hid-quirks.c
+++ b/drivers/hid/usbhid/hid-quirks.c
@@ -73,6 +73,7 @@ static const struct hid_blacklist {
 	{ USB_VENDOR_ID_FORMOSA, USB_DEVICE_ID_FORMOSA_IR_RECEIVER, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_FREESCALE, USB_DEVICE_ID_FREESCALE_MX28, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_MGE, USB_DEVICE_ID_MGE_UPS, HID_QUIRK_NOGET },
+	{ USB_VENDOR_ID_MSI, USB_DEVICE_ID_MSI_GX680R_LED_PANEL, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_NOVATEK, USB_DEVICE_ID_NOVATEK_MOUSE, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_PIXART, USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_PIXART, USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN1, HID_QUIRK_NO_INIT_REPORTS },
@@ -80,6 +81,7 @@ static const struct hid_blacklist {
 	{ USB_VENDOR_ID_PRODIGE, USB_DEVICE_ID_PRODIGE_CORDLESS, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_QUANTA, USB_DEVICE_ID_QUANTA_OPTICAL_TOUCH_3001, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_QUANTA, USB_DEVICE_ID_QUANTA_OPTICAL_TOUCH_3008, HID_QUIRK_NOGET },
+	{ USB_VENDOR_ID_REALTEK, USB_DEVICE_ID_REALTEK_READER, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_SENNHEISER, USB_DEVICE_ID_SENNHEISER_BTD500USB, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_SIGMATEL, USB_DEVICE_ID_SIGMATEL_STMP3780, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_SUN, USB_DEVICE_ID_RARITAN_KVM_DONGLE, HID_QUIRK_NOGET },
diff --git a/drivers/hid/usbhid/hiddev.c b/drivers/hid/usbhid/hiddev.c
index 87bd64959a9..2f1ddca6f2e 100644
--- a/drivers/hid/usbhid/hiddev.c
+++ b/drivers/hid/usbhid/hiddev.c
@@ -705,8 +705,8 @@ static long hiddev_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		if (report == NULL)
 			break;
 
-		usbhid_submit_report(hid, report, USB_DIR_IN);
-		usbhid_wait_io(hid);
+		hid_hw_request(hid, report, HID_REQ_GET_REPORT);
+		hid_hw_wait(hid);
 
 		r = 0;
 		break;
@@ -724,8 +724,8 @@ static long hiddev_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		if (report == NULL)
 			break;
 
-		usbhid_submit_report(hid, report, USB_DIR_OUT);
-		usbhid_wait_io(hid);
+		hid_hw_request(hid, report, HID_REQ_SET_REPORT);
+		hid_hw_wait(hid);
 
 		r = 0;
 		break;
diff --git a/drivers/hid/usbhid/usbhid.h b/drivers/hid/usbhid/usbhid.h
index bd87a61e530..dbb6af69913 100644
--- a/drivers/hid/usbhid/usbhid.h
+++ b/drivers/hid/usbhid/usbhid.h
@@ -34,12 +34,9 @@
 #include <linux/input.h>
 
 /*  API provided by hid-core.c for USB HID drivers */
-int usbhid_wait_io(struct hid_device* hid);
 void usbhid_close(struct hid_device *hid);
 int usbhid_open(struct hid_device *hid);
 void usbhid_init_reports(struct hid_device *hid);
-void usbhid_submit_report
-(struct hid_device *hid, struct hid_report *report, unsigned char dir);
 int usbhid_get_power(struct hid_device *hid);
 void usbhid_put_power(struct hid_device *hid);
 struct usb_interface *usbhid_find_interface(int minor);
diff --git a/drivers/hv/Kconfig b/drivers/hv/Kconfig
index 64630f15f18..0403b51d20b 100644
--- a/drivers/hv/Kconfig
+++ b/drivers/hv/Kconfig
@@ -2,7 +2,7 @@ menu "Microsoft Hyper-V guest support"
 
 config HYPERV
 	tristate "Microsoft Hyper-V client drivers"
-	depends on X86 && ACPI && PCI && X86_LOCAL_APIC
+	depends on X86 && ACPI && PCI && X86_LOCAL_APIC && HYPERVISOR_GUEST
 	help
 	  Select this option to run Linux as a Hyper-V client operating
 	  system.
diff --git a/drivers/hv/Makefile b/drivers/hv/Makefile
index e6abfa02d8b..0a74b566118 100644
--- a/drivers/hv/Makefile
+++ b/drivers/hv/Makefile
@@ -5,4 +5,4 @@ obj-$(CONFIG_HYPERV_BALLOON)	+= hv_balloon.o
 hv_vmbus-y := vmbus_drv.o \
 		 hv.o connection.o channel.o \
 		 channel_mgmt.o ring_buffer.o
-hv_utils-y := hv_util.o hv_kvp.o
+hv_utils-y := hv_util.o hv_kvp.o hv_snapshot.o
diff --git a/drivers/hv/channel_mgmt.c b/drivers/hv/channel_mgmt.c
index ff1be167eb0..bad8128b283 100644
--- a/drivers/hv/channel_mgmt.c
+++ b/drivers/hv/channel_mgmt.c
@@ -165,8 +165,19 @@ static void vmbus_process_rescind_offer(struct work_struct *work)
 	struct vmbus_channel *channel = container_of(work,
 						     struct vmbus_channel,
 						     work);
+	unsigned long flags;
+	struct vmbus_channel_relid_released msg;
 
 	vmbus_device_unregister(channel->device_obj);
+	memset(&msg, 0, sizeof(struct vmbus_channel_relid_released));
+	msg.child_relid = channel->offermsg.child_relid;
+	msg.header.msgtype = CHANNELMSG_RELID_RELEASED;
+	vmbus_post_msg(&msg, sizeof(struct vmbus_channel_relid_released));
+
+	spin_lock_irqsave(&vmbus_connection.channel_lock, flags);
+	list_del(&channel->listentry);
+	spin_unlock_irqrestore(&vmbus_connection.channel_lock, flags);
+	free_channel(channel);
 }
 
 void vmbus_free_channels(void)
diff --git a/drivers/hv/hv.c b/drivers/hv/hv.c
index 731158910c1..ae4923756d9 100644
--- a/drivers/hv/hv.c
+++ b/drivers/hv/hv.c
@@ -289,9 +289,8 @@ void hv_synic_init(void *arg)
 	/* Check the version */
 	rdmsrl(HV_X64_MSR_SVERSION, version);
 
-	hv_context.event_dpc[cpu] = (struct tasklet_struct *)
-					kmalloc(sizeof(struct tasklet_struct),
-						GFP_ATOMIC);
+	hv_context.event_dpc[cpu] = kmalloc(sizeof(struct tasklet_struct),
+					    GFP_ATOMIC);
 	if (hv_context.event_dpc[cpu] == NULL) {
 		pr_err("Unable to allocate event dpc\n");
 		goto cleanup;
diff --git a/drivers/hv/hv_balloon.c b/drivers/hv/hv_balloon.c
index 37873213e24..4c605c70ebf 100644
--- a/drivers/hv/hv_balloon.c
+++ b/drivers/hv/hv_balloon.c
@@ -117,7 +117,14 @@ union dm_caps {
 	struct {
 		__u64 balloon:1;
 		__u64 hot_add:1;
-		__u64 reservedz:62;
+		/*
+		 * To support guests that may have alignment
+		 * limitations on hot-add, the guest can specify
+		 * its alignment requirements; a value of n
+		 * represents an alignment of 2^n in mega bytes.
+		 */
+		__u64 hot_add_alignment:4;
+		__u64 reservedz:58;
 	} cap_bits;
 	__u64 caps;
 } __packed;
@@ -412,13 +419,45 @@ struct dm_info_msg {
  * End protocol definitions.
  */
 
-static bool hot_add;
+/*
+ * State to manage hot adding memory into the guest.
+ * The range start_pfn : end_pfn specifies the range
+ * that the host has asked us to hot add. The range
+ * start_pfn : ha_end_pfn specifies the range that we have
+ * currently hot added. We hot add in multiples of 128M
+ * chunks; it is possible that we may not be able to bring
+ * online all the pages in the region. The range
+ * covered_start_pfn : covered_end_pfn defines the pages that can
+ * be brough online.
+ */
+
+struct hv_hotadd_state {
+	struct list_head list;
+	unsigned long start_pfn;
+	unsigned long covered_start_pfn;
+	unsigned long covered_end_pfn;
+	unsigned long ha_end_pfn;
+	unsigned long end_pfn;
+};
+
+struct balloon_state {
+	__u32 num_pages;
+	struct work_struct wrk;
+};
+
+struct hot_add_wrk {
+	union dm_mem_page_range ha_page_range;
+	union dm_mem_page_range ha_region_range;
+	struct work_struct wrk;
+};
+
+static bool hot_add = true;
 static bool do_hot_add;
 /*
  * Delay reporting memory pressure by
  * the specified number of seconds.
  */
-static uint pressure_report_delay = 30;
+static uint pressure_report_delay = 45;
 
 module_param(hot_add, bool, (S_IRUGO | S_IWUSR));
 MODULE_PARM_DESC(hot_add, "If set attempt memory hot_add");
@@ -446,6 +485,7 @@ enum hv_dm_state {
 static __u8 recv_buffer[PAGE_SIZE];
 static __u8 *send_buffer;
 #define PAGES_IN_2M	512
+#define HA_CHUNK (32 * 1024)
 
 struct hv_dynmem_device {
 	struct hv_device *dev;
@@ -459,7 +499,28 @@ struct hv_dynmem_device {
 	unsigned int num_pages_ballooned;
 
 	/*
-	 * This thread handles both balloon/hot-add
+	 * State to manage the ballooning (up) operation.
+	 */
+	struct balloon_state balloon_wrk;
+
+	/*
+	 * State to execute the "hot-add" operation.
+	 */
+	struct hot_add_wrk ha_wrk;
+
+	/*
+	 * This state tracks if the host has specified a hot-add
+	 * region.
+	 */
+	bool host_specified_ha_region;
+
+	/*
+	 * State to synchronize hot-add.
+	 */
+	struct completion  ol_waitevent;
+	bool ha_waiting;
+	/*
+	 * This thread handles hot-add
 	 * requests from the host as well as notifying
 	 * the host with regards to memory pressure in
 	 * the guest.
@@ -467,6 +528,11 @@ struct hv_dynmem_device {
 	struct task_struct *thread;
 
 	/*
+	 * A list of hot-add regions.
+	 */
+	struct list_head ha_region_list;
+
+	/*
 	 * We start with the highest version we can support
 	 * and downgrade based on the host; we save here the
 	 * next version to try.
@@ -476,35 +542,358 @@ struct hv_dynmem_device {
 
 static struct hv_dynmem_device dm_device;
 
-static void hot_add_req(struct hv_dynmem_device *dm, struct dm_hot_add *msg)
+#ifdef CONFIG_MEMORY_HOTPLUG
+
+static void hv_bring_pgs_online(unsigned long start_pfn, unsigned long size)
 {
+	int i;
 
-	struct dm_hot_add_response resp;
+	for (i = 0; i < size; i++) {
+		struct page *pg;
+		pg = pfn_to_page(start_pfn + i);
+		__online_page_set_limits(pg);
+		__online_page_increment_counters(pg);
+		__online_page_free(pg);
+	}
+}
 
-	if (do_hot_add) {
+static void hv_mem_hot_add(unsigned long start, unsigned long size,
+				unsigned long pfn_count,
+				struct hv_hotadd_state *has)
+{
+	int ret = 0;
+	int i, nid, t;
+	unsigned long start_pfn;
+	unsigned long processed_pfn;
+	unsigned long total_pfn = pfn_count;
+
+	for (i = 0; i < (size/HA_CHUNK); i++) {
+		start_pfn = start + (i * HA_CHUNK);
+		has->ha_end_pfn +=  HA_CHUNK;
+
+		if (total_pfn > HA_CHUNK) {
+			processed_pfn = HA_CHUNK;
+			total_pfn -= HA_CHUNK;
+		} else {
+			processed_pfn = total_pfn;
+			total_pfn = 0;
+		}
+
+		has->covered_end_pfn +=  processed_pfn;
+
+		init_completion(&dm_device.ol_waitevent);
+		dm_device.ha_waiting = true;
+
+		nid = memory_add_physaddr_to_nid(PFN_PHYS(start_pfn));
+		ret = add_memory(nid, PFN_PHYS((start_pfn)),
+				(HA_CHUNK << PAGE_SHIFT));
 
-		pr_info("Memory hot add not supported\n");
+		if (ret) {
+			pr_info("hot_add memory failed error is %d\n", ret);
+			if (ret == -EEXIST) {
+				/*
+				 * This error indicates that the error
+				 * is not a transient failure. This is the
+				 * case where the guest's physical address map
+				 * precludes hot adding memory. Stop all further
+				 * memory hot-add.
+				 */
+				do_hot_add = false;
+			}
+			has->ha_end_pfn -= HA_CHUNK;
+			has->covered_end_pfn -=  processed_pfn;
+			break;
+		}
 
 		/*
-		 * Currently we do not support hot add.
-		 * Just fail the request.
+		 * Wait for the memory block to be onlined.
 		 */
+		t = wait_for_completion_timeout(&dm_device.ol_waitevent, 5*HZ);
+		if (t == 0) {
+			pr_info("hot_add memory timedout\n");
+			has->ha_end_pfn -= HA_CHUNK;
+			has->covered_end_pfn -=  processed_pfn;
+			break;
+		}
+
+	}
+
+	return;
+}
+
+static void hv_online_page(struct page *pg)
+{
+	struct list_head *cur;
+	struct hv_hotadd_state *has;
+	unsigned long cur_start_pgp;
+	unsigned long cur_end_pgp;
+
+	if (dm_device.ha_waiting) {
+		dm_device.ha_waiting = false;
+		complete(&dm_device.ol_waitevent);
+	}
+
+	list_for_each(cur, &dm_device.ha_region_list) {
+		has = list_entry(cur, struct hv_hotadd_state, list);
+		cur_start_pgp = (unsigned long)
+				pfn_to_page(has->covered_start_pfn);
+		cur_end_pgp = (unsigned long)pfn_to_page(has->covered_end_pfn);
+
+		if (((unsigned long)pg >= cur_start_pgp) &&
+			((unsigned long)pg < cur_end_pgp)) {
+			/*
+			 * This frame is currently backed; online the
+			 * page.
+			 */
+			__online_page_set_limits(pg);
+			__online_page_increment_counters(pg);
+			__online_page_free(pg);
+			has->covered_start_pfn++;
+		}
+	}
+}
+
+static bool pfn_covered(unsigned long start_pfn, unsigned long pfn_cnt)
+{
+	struct list_head *cur;
+	struct hv_hotadd_state *has;
+	unsigned long residual, new_inc;
+
+	if (list_empty(&dm_device.ha_region_list))
+		return false;
+
+	list_for_each(cur, &dm_device.ha_region_list) {
+		has = list_entry(cur, struct hv_hotadd_state, list);
+
+		/*
+		 * If the pfn range we are dealing with is not in the current
+		 * "hot add block", move on.
+		 */
+		if ((start_pfn >= has->end_pfn))
+			continue;
+		/*
+		 * If the current hot add-request extends beyond
+		 * our current limit; extend it.
+		 */
+		if ((start_pfn + pfn_cnt) > has->end_pfn) {
+			residual = (start_pfn + pfn_cnt - has->end_pfn);
+			/*
+			 * Extend the region by multiples of HA_CHUNK.
+			 */
+			new_inc = (residual / HA_CHUNK) * HA_CHUNK;
+			if (residual % HA_CHUNK)
+				new_inc += HA_CHUNK;
+
+			has->end_pfn += new_inc;
+		}
+
+		/*
+		 * If the current start pfn is not where the covered_end
+		 * is, update it.
+		 */
+
+		if (has->covered_end_pfn != start_pfn) {
+			has->covered_end_pfn = start_pfn;
+			has->covered_start_pfn = start_pfn;
+		}
+		return true;
+
 	}
 
+	return false;
+}
+
+static unsigned long handle_pg_range(unsigned long pg_start,
+					unsigned long pg_count)
+{
+	unsigned long start_pfn = pg_start;
+	unsigned long pfn_cnt = pg_count;
+	unsigned long size;
+	struct list_head *cur;
+	struct hv_hotadd_state *has;
+	unsigned long pgs_ol = 0;
+	unsigned long old_covered_state;
+
+	if (list_empty(&dm_device.ha_region_list))
+		return 0;
+
+	list_for_each(cur, &dm_device.ha_region_list) {
+		has = list_entry(cur, struct hv_hotadd_state, list);
+
+		/*
+		 * If the pfn range we are dealing with is not in the current
+		 * "hot add block", move on.
+		 */
+		if ((start_pfn >= has->end_pfn))
+			continue;
+
+		old_covered_state = has->covered_end_pfn;
+
+		if (start_pfn < has->ha_end_pfn) {
+			/*
+			 * This is the case where we are backing pages
+			 * in an already hot added region. Bring
+			 * these pages online first.
+			 */
+			pgs_ol = has->ha_end_pfn - start_pfn;
+			if (pgs_ol > pfn_cnt)
+				pgs_ol = pfn_cnt;
+			hv_bring_pgs_online(start_pfn, pgs_ol);
+			has->covered_end_pfn +=  pgs_ol;
+			has->covered_start_pfn +=  pgs_ol;
+			pfn_cnt -= pgs_ol;
+		}
+
+		if ((has->ha_end_pfn < has->end_pfn) && (pfn_cnt > 0)) {
+			/*
+			 * We have some residual hot add range
+			 * that needs to be hot added; hot add
+			 * it now. Hot add a multiple of
+			 * of HA_CHUNK that fully covers the pages
+			 * we have.
+			 */
+			size = (has->end_pfn - has->ha_end_pfn);
+			if (pfn_cnt <= size) {
+				size = ((pfn_cnt / HA_CHUNK) * HA_CHUNK);
+				if (pfn_cnt % HA_CHUNK)
+					size += HA_CHUNK;
+			} else {
+				pfn_cnt = size;
+			}
+			hv_mem_hot_add(has->ha_end_pfn, size, pfn_cnt, has);
+		}
+		/*
+		 * If we managed to online any pages that were given to us,
+		 * we declare success.
+		 */
+		return has->covered_end_pfn - old_covered_state;
+
+	}
+
+	return 0;
+}
+
+static unsigned long process_hot_add(unsigned long pg_start,
+					unsigned long pfn_cnt,
+					unsigned long rg_start,
+					unsigned long rg_size)
+{
+	struct hv_hotadd_state *ha_region = NULL;
+
+	if (pfn_cnt == 0)
+		return 0;
+
+	if (!dm_device.host_specified_ha_region)
+		if (pfn_covered(pg_start, pfn_cnt))
+			goto do_pg_range;
+
+	/*
+	 * If the host has specified a hot-add range; deal with it first.
+	 */
+
+	if (rg_size != 0) {
+		ha_region = kzalloc(sizeof(struct hv_hotadd_state), GFP_KERNEL);
+		if (!ha_region)
+			return 0;
+
+		INIT_LIST_HEAD(&ha_region->list);
+
+		list_add_tail(&ha_region->list, &dm_device.ha_region_list);
+		ha_region->start_pfn = rg_start;
+		ha_region->ha_end_pfn = rg_start;
+		ha_region->covered_start_pfn = pg_start;
+		ha_region->covered_end_pfn = pg_start;
+		ha_region->end_pfn = rg_start + rg_size;
+	}
+
+do_pg_range:
+	/*
+	 * Process the page range specified; bringing them
+	 * online if possible.
+	 */
+	return handle_pg_range(pg_start, pfn_cnt);
+}
+
+#endif
+
+static void hot_add_req(struct work_struct *dummy)
+{
+	struct dm_hot_add_response resp;
+#ifdef CONFIG_MEMORY_HOTPLUG
+	unsigned long pg_start, pfn_cnt;
+	unsigned long rg_start, rg_sz;
+#endif
+	struct hv_dynmem_device *dm = &dm_device;
+
 	memset(&resp, 0, sizeof(struct dm_hot_add_response));
 	resp.hdr.type = DM_MEM_HOT_ADD_RESPONSE;
 	resp.hdr.size = sizeof(struct dm_hot_add_response);
 	resp.hdr.trans_id = atomic_inc_return(&trans_id);
 
-	resp.page_count = 0;
-	resp.result = 0;
+#ifdef CONFIG_MEMORY_HOTPLUG
+	pg_start = dm->ha_wrk.ha_page_range.finfo.start_page;
+	pfn_cnt = dm->ha_wrk.ha_page_range.finfo.page_cnt;
+
+	rg_start = dm->ha_wrk.ha_region_range.finfo.start_page;
+	rg_sz = dm->ha_wrk.ha_region_range.finfo.page_cnt;
+
+	if ((rg_start == 0) && (!dm->host_specified_ha_region)) {
+		unsigned long region_size;
+		unsigned long region_start;
+
+		/*
+		 * The host has not specified the hot-add region.
+		 * Based on the hot-add page range being specified,
+		 * compute a hot-add region that can cover the pages
+		 * that need to be hot-added while ensuring the alignment
+		 * and size requirements of Linux as it relates to hot-add.
+		 */
+		region_start = pg_start;
+		region_size = (pfn_cnt / HA_CHUNK) * HA_CHUNK;
+		if (pfn_cnt % HA_CHUNK)
+			region_size += HA_CHUNK;
+
+		region_start = (pg_start / HA_CHUNK) * HA_CHUNK;
+
+		rg_start = region_start;
+		rg_sz = region_size;
+	}
+
+	if (do_hot_add)
+		resp.page_count = process_hot_add(pg_start, pfn_cnt,
+						rg_start, rg_sz);
+#endif
+	/*
+	 * The result field of the response structure has the
+	 * following semantics:
+	 *
+	 * 1. If all or some pages hot-added: Guest should return success.
+	 *
+	 * 2. If no pages could be hot-added:
+	 *
+	 * If the guest returns success, then the host
+	 * will not attempt any further hot-add operations. This
+	 * signifies a permanent failure.
+	 *
+	 * If the guest returns failure, then this failure will be
+	 * treated as a transient failure and the host may retry the
+	 * hot-add operation after some delay.
+	 */
+	if (resp.page_count > 0)
+		resp.result = 1;
+	else if (!do_hot_add)
+		resp.result = 1;
+	else
+		resp.result = 0;
+
+	if (!do_hot_add || (resp.page_count == 0))
+		pr_info("Memory hot add failed\n");
 
 	dm->state = DM_INITIALIZED;
 	vmbus_sendpacket(dm->dev->channel, &resp,
 			sizeof(struct dm_hot_add_response),
 			(unsigned long)NULL,
 			VM_PKT_DATA_INBAND, 0);
-
 }
 
 static void process_info(struct hv_dynmem_device *dm, struct dm_info_msg *msg)
@@ -523,7 +912,7 @@ static void process_info(struct hv_dynmem_device *dm, struct dm_info_msg *msg)
 	}
 }
 
-unsigned long compute_balloon_floor(void)
+static unsigned long compute_balloon_floor(void)
 {
 	unsigned long min_pages;
 #define MB2PAGES(mb) ((mb) << (20 - PAGE_SHIFT))
@@ -644,6 +1033,14 @@ static int  alloc_balloon_pages(struct hv_dynmem_device *dm, int num_pages,
 
 		dm->num_pages_ballooned += alloc_unit;
 
+		/*
+		 * If we allocatted 2M pages; split them so we
+		 * can free them in any order we get.
+		 */
+
+		if (alloc_unit != 1)
+			split_page(pg, get_order(alloc_unit << PAGE_SHIFT));
+
 		bl_resp->range_count++;
 		bl_resp->range_array[i].finfo.start_page =
 			page_to_pfn(pg);
@@ -657,9 +1054,9 @@ static int  alloc_balloon_pages(struct hv_dynmem_device *dm, int num_pages,
 
 
 
-static void balloon_up(struct hv_dynmem_device *dm, struct dm_balloon *req)
+static void balloon_up(struct work_struct *dummy)
 {
-	int num_pages = req->num_pages;
+	int num_pages = dm_device.balloon_wrk.num_pages;
 	int num_ballooned = 0;
 	struct dm_balloon_response *bl_resp;
 	int alloc_unit;
@@ -670,9 +1067,10 @@ static void balloon_up(struct hv_dynmem_device *dm, struct dm_balloon *req)
 
 
 	/*
-	 * Currently, we only support 4k allocations.
+	 * We will attempt 2M allocations. However, if we fail to
+	 * allocate 2M chunks, we will go back to 4k allocations.
 	 */
-	alloc_unit = 1;
+	alloc_unit = 512;
 
 	while (!done) {
 		bl_resp = (struct dm_balloon_response *)send_buffer;
@@ -684,14 +1082,19 @@ static void balloon_up(struct hv_dynmem_device *dm, struct dm_balloon *req)
 
 
 		num_pages -= num_ballooned;
-		num_ballooned = alloc_balloon_pages(dm, num_pages,
+		num_ballooned = alloc_balloon_pages(&dm_device, num_pages,
 						bl_resp, alloc_unit,
 						 &alloc_error);
 
+		if ((alloc_error) && (alloc_unit != 1)) {
+			alloc_unit = 1;
+			continue;
+		}
+
 		if ((alloc_error) || (num_ballooned == num_pages)) {
 			bl_resp->more_pages = 0;
 			done = true;
-			dm->state = DM_INITIALIZED;
+			dm_device.state = DM_INITIALIZED;
 		}
 
 		/*
@@ -719,7 +1122,7 @@ static void balloon_up(struct hv_dynmem_device *dm, struct dm_balloon *req)
 			pr_info("Balloon response failed\n");
 
 			for (i = 0; i < bl_resp->range_count; i++)
-				free_balloon_pages(dm,
+				free_balloon_pages(&dm_device,
 						 &bl_resp->range_array[i]);
 
 			done = true;
@@ -761,7 +1164,6 @@ static int dm_thread_func(void *dm_dev)
 {
 	struct hv_dynmem_device *dm = dm_dev;
 	int t;
-	unsigned long  scan_start;
 
 	while (!kthread_should_stop()) {
 		t = wait_for_completion_timeout(&dm_device.config_event, 1*HZ);
@@ -773,22 +1175,6 @@ static int dm_thread_func(void *dm_dev)
 		if (t == 0)
 			post_status(dm);
 
-		scan_start = jiffies;
-		switch (dm->state) {
-		case DM_BALLOON_UP:
-			balloon_up(dm, (struct dm_balloon *)recv_buffer);
-			break;
-
-		case DM_HOT_ADD:
-			hot_add_req(dm, (struct dm_hot_add *)recv_buffer);
-			break;
-		default:
-			break;
-		}
-
-		if (!time_in_range(jiffies, scan_start, scan_start + HZ))
-			post_status(dm);
-
 	}
 
 	return 0;
@@ -861,6 +1247,10 @@ static void balloon_onchannelcallback(void *context)
 	struct dm_message *dm_msg;
 	struct dm_header *dm_hdr;
 	struct hv_dynmem_device *dm = hv_get_drvdata(dev);
+	struct dm_balloon *bal_msg;
+	struct dm_hot_add *ha_msg;
+	union dm_mem_page_range *ha_pg_range;
+	union dm_mem_page_range *ha_region;
 
 	memset(recv_buffer, 0, sizeof(recv_buffer));
 	vmbus_recvpacket(dev->channel, recv_buffer,
@@ -882,8 +1272,12 @@ static void balloon_onchannelcallback(void *context)
 			break;
 
 		case DM_BALLOON_REQUEST:
+			if (dm->state == DM_BALLOON_UP)
+				pr_warn("Currently ballooning\n");
+			bal_msg = (struct dm_balloon *)recv_buffer;
 			dm->state = DM_BALLOON_UP;
-			complete(&dm->config_event);
+			dm_device.balloon_wrk.num_pages = bal_msg->num_pages;
+			schedule_work(&dm_device.balloon_wrk.wrk);
 			break;
 
 		case DM_UNBALLOON_REQUEST:
@@ -893,8 +1287,31 @@ static void balloon_onchannelcallback(void *context)
 			break;
 
 		case DM_MEM_HOT_ADD_REQUEST:
+			if (dm->state == DM_HOT_ADD)
+				pr_warn("Currently hot-adding\n");
 			dm->state = DM_HOT_ADD;
-			complete(&dm->config_event);
+			ha_msg = (struct dm_hot_add *)recv_buffer;
+			if (ha_msg->hdr.size == sizeof(struct dm_hot_add)) {
+				/*
+				 * This is a normal hot-add request specifying
+				 * hot-add memory.
+				 */
+				ha_pg_range = &ha_msg->range;
+				dm->ha_wrk.ha_page_range = *ha_pg_range;
+				dm->ha_wrk.ha_region_range.page_range = 0;
+			} else {
+				/*
+				 * Host is specifying that we first hot-add
+				 * a region and then partially populate this
+				 * region.
+				 */
+				dm->host_specified_ha_region = true;
+				ha_pg_range = &ha_msg->range;
+				ha_region = &ha_pg_range[1];
+				dm->ha_wrk.ha_page_range = *ha_pg_range;
+				dm->ha_wrk.ha_region_range = *ha_region;
+			}
+			schedule_work(&dm_device.ha_wrk.wrk);
 			break;
 
 		case DM_INFO_MESSAGE:
@@ -937,6 +1354,10 @@ static int balloon_probe(struct hv_device *dev,
 	dm_device.next_version = DYNMEM_PROTOCOL_VERSION_WIN7;
 	init_completion(&dm_device.host_event);
 	init_completion(&dm_device.config_event);
+	INIT_LIST_HEAD(&dm_device.ha_region_list);
+	INIT_WORK(&dm_device.balloon_wrk.wrk, balloon_up);
+	INIT_WORK(&dm_device.ha_wrk.wrk, hot_add_req);
+	dm_device.host_specified_ha_region = false;
 
 	dm_device.thread =
 		 kthread_run(dm_thread_func, &dm_device, "hv_balloon");
@@ -945,6 +1366,10 @@ static int balloon_probe(struct hv_device *dev,
 		goto probe_error1;
 	}
 
+#ifdef CONFIG_MEMORY_HOTPLUG
+	set_online_page_callback(&hv_online_page);
+#endif
+
 	hv_set_drvdata(dev, &dm_device);
 	/*
 	 * Initiate the hand shake with the host and negotiate
@@ -962,8 +1387,7 @@ static int balloon_probe(struct hv_device *dev,
 	ret = vmbus_sendpacket(dev->channel, &version_req,
 				sizeof(struct dm_version_request),
 				(unsigned long)NULL,
-				VM_PKT_DATA_INBAND,
-				VMBUS_DATA_PACKET_FLAG_COMPLETION_REQUESTED);
+				VM_PKT_DATA_INBAND, 0);
 	if (ret)
 		goto probe_error2;
 
@@ -990,13 +1414,13 @@ static int balloon_probe(struct hv_device *dev,
 	cap_msg.hdr.trans_id = atomic_inc_return(&trans_id);
 
 	cap_msg.caps.cap_bits.balloon = 1;
+	cap_msg.caps.cap_bits.hot_add = 1;
+
 	/*
-	 * While we currently don't support hot-add,
-	 * we still advertise this capability since the
-	 * host requires that guests partcipating in the
-	 * dynamic memory protocol support hot add.
+	 * Specify our alignment requirements as it relates
+	 * memory hot-add. Specify 128MB alignment.
 	 */
-	cap_msg.caps.cap_bits.hot_add = 1;
+	cap_msg.caps.cap_bits.hot_add_alignment = 7;
 
 	/*
 	 * Currently the host does not use these
@@ -1009,8 +1433,7 @@ static int balloon_probe(struct hv_device *dev,
 	ret = vmbus_sendpacket(dev->channel, &cap_msg,
 				sizeof(struct dm_capabilities),
 				(unsigned long)NULL,
-				VM_PKT_DATA_INBAND,
-				VMBUS_DATA_PACKET_FLAG_COMPLETION_REQUESTED);
+				VM_PKT_DATA_INBAND, 0);
 	if (ret)
 		goto probe_error2;
 
@@ -1034,6 +1457,9 @@ static int balloon_probe(struct hv_device *dev,
 	return 0;
 
 probe_error2:
+#ifdef CONFIG_MEMORY_HOTPLUG
+	restore_online_page_callback(&hv_online_page);
+#endif
 	kthread_stop(dm_device.thread);
 
 probe_error1:
@@ -1046,13 +1472,26 @@ probe_error0:
 static int balloon_remove(struct hv_device *dev)
 {
 	struct hv_dynmem_device *dm = hv_get_drvdata(dev);
+	struct list_head *cur, *tmp;
+	struct hv_hotadd_state *has;
 
 	if (dm->num_pages_ballooned != 0)
 		pr_warn("Ballooned pages: %d\n", dm->num_pages_ballooned);
 
+	cancel_work_sync(&dm->balloon_wrk.wrk);
+	cancel_work_sync(&dm->ha_wrk.wrk);
+
 	vmbus_close(dev->channel);
 	kthread_stop(dm->thread);
 	kfree(send_buffer);
+#ifdef CONFIG_MEMORY_HOTPLUG
+	restore_online_page_callback(&hv_online_page);
+#endif
+	list_for_each_safe(cur, tmp, &dm->ha_region_list) {
+		has = list_entry(cur, struct hv_hotadd_state, list);
+		list_del(&has->list);
+		kfree(has);
+	}
 
 	return 0;
 }
@@ -1079,14 +1518,7 @@ static int __init init_balloon_drv(void)
 	return vmbus_driver_register(&balloon_drv);
 }
 
-static void exit_balloon_drv(void)
-{
-
-	vmbus_driver_unregister(&balloon_drv);
-}
-
 module_init(init_balloon_drv);
-module_exit(exit_balloon_drv);
 
 MODULE_DESCRIPTION("Hyper-V Balloon");
 MODULE_VERSION(HV_DRV_VERSION);
diff --git a/drivers/hv/hv_snapshot.c b/drivers/hv/hv_snapshot.c
new file mode 100644
index 00000000000..8ad5653ce44
--- /dev/null
+++ b/drivers/hv/hv_snapshot.c
@@ -0,0 +1,287 @@
+/*
+ * An implementation of host initiated guest snapshot.
+ *
+ *
+ * Copyright (C) 2013, Microsoft, Inc.
+ * Author : K. Y. Srinivasan <kys@microsoft.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, GOOD TITLE or
+ * NON INFRINGEMENT.  See the GNU General Public License for more
+ * details.
+ *
+ */
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/net.h>
+#include <linux/nls.h>
+#include <linux/connector.h>
+#include <linux/workqueue.h>
+#include <linux/hyperv.h>
+
+
+
+/*
+ * Global state maintained for transaction that is being processed.
+ * Note that only one transaction can be active at any point in time.
+ *
+ * This state is set when we receive a request from the host; we
+ * cleanup this state when the transaction is completed - when we respond
+ * to the host with the key value.
+ */
+
+static struct {
+	bool active; /* transaction status - active or not */
+	int recv_len; /* number of bytes received. */
+	struct vmbus_channel *recv_channel; /* chn we got the request */
+	u64 recv_req_id; /* request ID. */
+	struct hv_vss_msg  *msg; /* current message */
+} vss_transaction;
+
+
+static void vss_respond_to_host(int error);
+
+static struct cb_id vss_id = { CN_VSS_IDX, CN_VSS_VAL };
+static const char vss_name[] = "vss_kernel_module";
+static __u8 *recv_buffer;
+
+static void vss_send_op(struct work_struct *dummy);
+static DECLARE_WORK(vss_send_op_work, vss_send_op);
+
+/*
+ * Callback when data is received from user mode.
+ */
+
+static void
+vss_cn_callback(struct cn_msg *msg, struct netlink_skb_parms *nsp)
+{
+	struct hv_vss_msg *vss_msg;
+
+	vss_msg = (struct hv_vss_msg *)msg->data;
+
+	if (vss_msg->vss_hdr.operation == VSS_OP_REGISTER) {
+		pr_info("VSS daemon registered\n");
+		vss_transaction.active = false;
+		if (vss_transaction.recv_channel != NULL)
+			hv_vss_onchannelcallback(vss_transaction.recv_channel);
+		return;
+
+	}
+	vss_respond_to_host(vss_msg->error);
+}
+
+
+static void vss_send_op(struct work_struct *dummy)
+{
+	int op = vss_transaction.msg->vss_hdr.operation;
+	struct cn_msg *msg;
+	struct hv_vss_msg *vss_msg;
+
+	msg = kzalloc(sizeof(*msg) + sizeof(*vss_msg), GFP_ATOMIC);
+	if (!msg)
+		return;
+
+	vss_msg = (struct hv_vss_msg *)msg->data;
+
+	msg->id.idx =  CN_VSS_IDX;
+	msg->id.val = CN_VSS_VAL;
+
+	vss_msg->vss_hdr.operation = op;
+	msg->len = sizeof(struct hv_vss_msg);
+
+	cn_netlink_send(msg, 0, GFP_ATOMIC);
+	kfree(msg);
+
+	return;
+}
+
+/*
+ * Send a response back to the host.
+ */
+
+static void
+vss_respond_to_host(int error)
+{
+	struct icmsg_hdr *icmsghdrp;
+	u32	buf_len;
+	struct vmbus_channel *channel;
+	u64	req_id;
+
+	/*
+	 * If a transaction is not active; log and return.
+	 */
+
+	if (!vss_transaction.active) {
+		/*
+		 * This is a spurious call!
+		 */
+		pr_warn("VSS: Transaction not active\n");
+		return;
+	}
+	/*
+	 * Copy the global state for completing the transaction. Note that
+	 * only one transaction can be active at a time.
+	 */
+
+	buf_len = vss_transaction.recv_len;
+	channel = vss_transaction.recv_channel;
+	req_id = vss_transaction.recv_req_id;
+	vss_transaction.active = false;
+
+	icmsghdrp = (struct icmsg_hdr *)
+			&recv_buffer[sizeof(struct vmbuspipe_hdr)];
+
+	if (channel->onchannel_callback == NULL)
+		/*
+		 * We have raced with util driver being unloaded;
+		 * silently return.
+		 */
+		return;
+
+	icmsghdrp->status = error;
+
+	icmsghdrp->icflags = ICMSGHDRFLAG_TRANSACTION | ICMSGHDRFLAG_RESPONSE;
+
+	vmbus_sendpacket(channel, recv_buffer, buf_len, req_id,
+				VM_PKT_DATA_INBAND, 0);
+
+}
+
+/*
+ * This callback is invoked when we get a VSS message from the host.
+ * The host ensures that only one VSS transaction can be active at a time.
+ */
+
+void hv_vss_onchannelcallback(void *context)
+{
+	struct vmbus_channel *channel = context;
+	u32 recvlen;
+	u64 requestid;
+	struct hv_vss_msg *vss_msg;
+
+
+	struct icmsg_hdr *icmsghdrp;
+	struct icmsg_negotiate *negop = NULL;
+
+	if (vss_transaction.active) {
+		/*
+		 * We will defer processing this callback once
+		 * the current transaction is complete.
+		 */
+		vss_transaction.recv_channel = channel;
+		return;
+	}
+
+	vmbus_recvpacket(channel, recv_buffer, PAGE_SIZE * 2, &recvlen,
+			 &requestid);
+
+	if (recvlen > 0) {
+		icmsghdrp = (struct icmsg_hdr *)&recv_buffer[
+			sizeof(struct vmbuspipe_hdr)];
+
+		if (icmsghdrp->icmsgtype == ICMSGTYPE_NEGOTIATE) {
+			vmbus_prep_negotiate_resp(icmsghdrp, negop,
+				 recv_buffer, MAX_SRV_VER, MAX_SRV_VER);
+			/*
+			 * We currently negotiate the highest number the
+			 * host has presented. If this version is not
+			 * atleast 5.0, reject.
+			 */
+			negop = (struct icmsg_negotiate *)&recv_buffer[
+				sizeof(struct vmbuspipe_hdr) +
+				sizeof(struct icmsg_hdr)];
+
+			if (negop->icversion_data[1].major < 5)
+				negop->icframe_vercnt = 0;
+		} else {
+			vss_msg = (struct hv_vss_msg *)&recv_buffer[
+				sizeof(struct vmbuspipe_hdr) +
+				sizeof(struct icmsg_hdr)];
+
+			/*
+			 * Stash away this global state for completing the
+			 * transaction; note transactions are serialized.
+			 */
+
+			vss_transaction.recv_len = recvlen;
+			vss_transaction.recv_channel = channel;
+			vss_transaction.recv_req_id = requestid;
+			vss_transaction.active = true;
+			vss_transaction.msg = (struct hv_vss_msg *)vss_msg;
+
+			switch (vss_msg->vss_hdr.operation) {
+				/*
+				 * Initiate a "freeze/thaw"
+				 * operation in the guest.
+				 * We respond to the host once
+				 * the operation is complete.
+				 *
+				 * We send the message to the
+				 * user space daemon and the
+				 * operation is performed in
+				 * the daemon.
+				 */
+			case VSS_OP_FREEZE:
+			case VSS_OP_THAW:
+				schedule_work(&vss_send_op_work);
+				return;
+
+			case VSS_OP_HOT_BACKUP:
+				vss_msg->vss_cf.flags =
+					 VSS_HBU_NO_AUTO_RECOVERY;
+				vss_respond_to_host(0);
+				return;
+
+			case VSS_OP_GET_DM_INFO:
+				vss_msg->dm_info.flags = 0;
+				vss_respond_to_host(0);
+				return;
+
+			default:
+				vss_respond_to_host(0);
+				return;
+
+			}
+
+		}
+
+		icmsghdrp->icflags = ICMSGHDRFLAG_TRANSACTION
+			| ICMSGHDRFLAG_RESPONSE;
+
+		vmbus_sendpacket(channel, recv_buffer,
+				       recvlen, requestid,
+				       VM_PKT_DATA_INBAND, 0);
+	}
+
+}
+
+int
+hv_vss_init(struct hv_util_service *srv)
+{
+	int err;
+
+	err = cn_add_callback(&vss_id, vss_name, vss_cn_callback);
+	if (err)
+		return err;
+	recv_buffer = srv->recv_buffer;
+
+	/*
+	 * When this driver loads, the user level daemon that
+	 * processes the host requests may not yet be running.
+	 * Defer processing channel callbacks until the daemon
+	 * has registered.
+	 */
+	vss_transaction.active = true;
+	return 0;
+}
+
+void hv_vss_deinit(void)
+{
+	cn_del_callback(&vss_id);
+	cancel_work_sync(&vss_send_op_work);
+}
diff --git a/drivers/hv/hv_util.c b/drivers/hv/hv_util.c
index 1d4cbd8e826..2f561c5dfe2 100644
--- a/drivers/hv/hv_util.c
+++ b/drivers/hv/hv_util.c
@@ -49,6 +49,12 @@ static struct hv_util_service util_kvp = {
 	.util_deinit = hv_kvp_deinit,
 };
 
+static struct hv_util_service util_vss = {
+	.util_cb = hv_vss_onchannelcallback,
+	.util_init = hv_vss_init,
+	.util_deinit = hv_vss_deinit,
+};
+
 static void perform_shutdown(struct work_struct *dummy)
 {
 	orderly_poweroff(true);
@@ -339,6 +345,10 @@ static const struct hv_vmbus_device_id id_table[] = {
 	{ HV_KVP_GUID,
 	  .driver_data = (unsigned long)&util_kvp
 	},
+	/* VSS GUID */
+	{ HV_VSS_GUID,
+	  .driver_data = (unsigned long)&util_vss
+	},
 	{ },
 };
 
diff --git a/drivers/hv/ring_buffer.c b/drivers/hv/ring_buffer.c
index cafa72ffdc3..d6fbb5772b8 100644
--- a/drivers/hv/ring_buffer.c
+++ b/drivers/hv/ring_buffer.c
@@ -71,6 +71,7 @@ u32 hv_end_read(struct hv_ring_buffer_info *rbi)
 
 static bool hv_need_to_signal(u32 old_write, struct hv_ring_buffer_info *rbi)
 {
+	smp_mb();
 	if (rbi->ring_buffer->interrupt_mask)
 		return false;
 
diff --git a/drivers/hwmon/Kconfig b/drivers/hwmon/Kconfig
index 108e0c11c7a..9c333d47143 100644
--- a/drivers/hwmon/Kconfig
+++ b/drivers/hwmon/Kconfig
@@ -192,9 +192,29 @@ config SENSORS_ADM9240
 	  This driver can also be built as a module.  If so, the module
 	  will be called adm9240.
 
+config SENSORS_ADT7X10
+	tristate
+	help
+	  This module contains common code shared by the ADT7310/ADT7320 and
+	  ADT7410/ADT7420 temperature monitoring chip drivers.
+
+	  If build as a module, the module will be called adt7x10.
+
+config SENSORS_ADT7310
+	tristate "Analog Devices ADT7310/ADT7320"
+	depends on SPI_MASTER
+	select SENSORS_ADT7X10
+	help
+	  If you say yes here you get support for the Analog Devices
+	  ADT7310 and ADT7320 temperature monitoring chips.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called adt7310.
+
 config SENSORS_ADT7410
 	tristate "Analog Devices ADT7410/ADT7420"
 	depends on I2C
+	select SENSORS_ADT7X10
 	help
 	  If you say yes here you get support for the Analog Devices
 	  ADT7410 and ADT7420 temperature monitoring chips.
@@ -512,6 +532,15 @@ config SENSORS_IBMPEX
 	  This driver can also be built as a module.  If so, the module
 	  will be called ibmpex.
 
+config SENSORS_IIO_HWMON
+	tristate "Hwmon driver that uses channels specified via iio maps"
+	depends on IIO
+	help
+	  This is a platform driver that in combination with a suitable
+	  map allows IIO devices to provide basic hwmon functionality
+	  for those channels specified in the map.  This map can be provided
+	  either via platform data or the device tree bindings.
+
 config SENSORS_IT87
 	tristate "ITE IT87xx and compatibles"
 	depends on !PPC
@@ -764,6 +793,16 @@ config SENSORS_LTC4261
 	  This driver can also be built as a module. If so, the module will
 	  be called ltc4261.
 
+config SENSORS_LM95234
+	tristate "National Semiconductor LM95234"
+	depends on I2C
+	help
+	  If you say yes here you get support for the LM95234 temperature
+	  sensor.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called lm95234.
+
 config SENSORS_LM95241
 	tristate "National Semiconductor LM95241 and compatibles"
 	depends on I2C
@@ -890,8 +929,22 @@ config SENSORS_MCP3021
 	  This driver can also be built as a module.  If so, the module
 	  will be called mcp3021.
 
+config SENSORS_NCT6775
+	tristate "Nuvoton NCT6775F and compatibles"
+	depends on !PPC
+	select HWMON_VID
+	help
+	  If you say yes here you get support for the hardware monitoring
+	  functionality of the Nuvoton NCT6775F, NCT6776F, NCT6779D
+	  and compatible Super-I/O chips. This driver replaces the
+	  w83627ehf driver for NCT6775F and NCT6776F.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called nct6775.
+
 config SENSORS_NTC_THERMISTOR
 	tristate "NTC thermistor support"
+	depends on (!OF && !IIO) || (OF && IIO)
 	help
 	  This driver supports NTC thermistors sensor reading and its
 	  interpretation. The driver can also monitor the temperature and
@@ -1217,8 +1270,8 @@ config SENSORS_TMP401
 	tristate "Texas Instruments TMP401 and compatibles"
 	depends on I2C
 	help
-	  If you say yes here you get support for Texas Instruments TMP401 and
-	  TMP411 temperature sensor chips.
+	  If you say yes here you get support for Texas Instruments TMP401,
+	  TMP411, TMP431, and TMP432 temperature sensor chips.
 
 	  This driver can also be built as a module.  If so, the module
 	  will be called tmp401.
diff --git a/drivers/hwmon/Makefile b/drivers/hwmon/Makefile
index 526a37db952..d17d3e64f9f 100644
--- a/drivers/hwmon/Makefile
+++ b/drivers/hwmon/Makefile
@@ -35,6 +35,8 @@ obj-$(CONFIG_SENSORS_ADM9240)	+= adm9240.o
 obj-$(CONFIG_SENSORS_ADS1015)	+= ads1015.o
 obj-$(CONFIG_SENSORS_ADS7828)	+= ads7828.o
 obj-$(CONFIG_SENSORS_ADS7871)	+= ads7871.o
+obj-$(CONFIG_SENSORS_ADT7X10)	+= adt7x10.o
+obj-$(CONFIG_SENSORS_ADT7310)	+= adt7310.o
 obj-$(CONFIG_SENSORS_ADT7410)	+= adt7410.o
 obj-$(CONFIG_SENSORS_ADT7411)	+= adt7411.o
 obj-$(CONFIG_SENSORS_ADT7462)	+= adt7462.o
@@ -66,6 +68,7 @@ obj-$(CONFIG_SENSORS_ULTRA45)	+= ultra45_env.o
 obj-$(CONFIG_SENSORS_I5K_AMB)	+= i5k_amb.o
 obj-$(CONFIG_SENSORS_IBMAEM)	+= ibmaem.o
 obj-$(CONFIG_SENSORS_IBMPEX)	+= ibmpex.o
+obj-$(CONFIG_SENSORS_IIO_HWMON) += iio_hwmon.o
 obj-$(CONFIG_SENSORS_INA209)	+= ina209.o
 obj-$(CONFIG_SENSORS_INA2XX)	+= ina2xx.o
 obj-$(CONFIG_SENSORS_IT87)	+= it87.o
@@ -87,6 +90,7 @@ obj-$(CONFIG_SENSORS_LM87)	+= lm87.o
 obj-$(CONFIG_SENSORS_LM90)	+= lm90.o
 obj-$(CONFIG_SENSORS_LM92)	+= lm92.o
 obj-$(CONFIG_SENSORS_LM93)	+= lm93.o
+obj-$(CONFIG_SENSORS_LM95234)	+= lm95234.o
 obj-$(CONFIG_SENSORS_LM95241)	+= lm95241.o
 obj-$(CONFIG_SENSORS_LM95245)	+= lm95245.o
 obj-$(CONFIG_SENSORS_LTC4151)	+= ltc4151.o
@@ -104,6 +108,7 @@ obj-$(CONFIG_SENSORS_MAX6650)	+= max6650.o
 obj-$(CONFIG_SENSORS_MAX6697)	+= max6697.o
 obj-$(CONFIG_SENSORS_MC13783_ADC)+= mc13783-adc.o
 obj-$(CONFIG_SENSORS_MCP3021)	+= mcp3021.o
+obj-$(CONFIG_SENSORS_NCT6775)	+= nct6775.o
 obj-$(CONFIG_SENSORS_NTC_THERMISTOR)	+= ntc_thermistor.o
 obj-$(CONFIG_SENSORS_PC87360)	+= pc87360.o
 obj-$(CONFIG_SENSORS_PC87427)	+= pc87427.o
diff --git a/drivers/hwmon/abituguru.c b/drivers/hwmon/abituguru.c
index 6119ff8e8c8..df0b6998791 100644
--- a/drivers/hwmon/abituguru.c
+++ b/drivers/hwmon/abituguru.c
@@ -96,9 +96,12 @@
 #define ABIT_UGURU_MAX_TIMEOUTS			2
 /* utility macros */
 #define ABIT_UGURU_NAME				"abituguru"
-#define ABIT_UGURU_DEBUG(level, format, arg...)				\
-	if (level <= verbose)						\
-		printk(KERN_DEBUG ABIT_UGURU_NAME ": "	format , ## arg)
+#define ABIT_UGURU_DEBUG(level, format, arg...)		\
+	do {						\
+		if (level <= verbose)			\
+			pr_debug(format , ## arg);	\
+	} while (0)
+
 /* Macros to help calculate the sysfs_names array length */
 /*
  * sum of strlen of: in??_input\0, in??_{min,max}\0, in??_{min,max}_alarm\0,
@@ -1533,7 +1536,7 @@ static int abituguru_resume(struct device *dev)
 }
 
 static SIMPLE_DEV_PM_OPS(abituguru_pm, abituguru_suspend, abituguru_resume);
-#define ABIT_UGURU_PM	&abituguru_pm
+#define ABIT_UGURU_PM	(&abituguru_pm)
 #else
 #define ABIT_UGURU_PM	NULL
 #endif /* CONFIG_PM */
diff --git a/drivers/hwmon/abituguru3.c b/drivers/hwmon/abituguru3.c
index 205327d33c4..1d2da31c27c 100644
--- a/drivers/hwmon/abituguru3.c
+++ b/drivers/hwmon/abituguru3.c
@@ -76,9 +76,11 @@
 #define ABIT_UGURU3_SYNCHRONIZE_TIMEOUT		5
 /* utility macros */
 #define ABIT_UGURU3_NAME			"abituguru3"
-#define ABIT_UGURU3_DEBUG(format, arg...)	\
-	if (verbose)				\
-		printk(KERN_DEBUG ABIT_UGURU3_NAME ": "	format , ## arg)
+#define ABIT_UGURU3_DEBUG(format, arg...)		\
+	do {						\
+		if (verbose)				\
+			pr_debug(format , ## arg);	\
+	} while (0)
 
 /* Macros to help calculate the sysfs_names array length */
 #define ABIT_UGURU3_MAX_NO_SENSORS 26
@@ -1159,7 +1161,7 @@ static int abituguru3_resume(struct device *dev)
 }
 
 static SIMPLE_DEV_PM_OPS(abituguru3_pm, abituguru3_suspend, abituguru3_resume);
-#define ABIT_UGURU3_PM	&abituguru3_pm
+#define ABIT_UGURU3_PM	(&abituguru3_pm)
 #else
 #define ABIT_UGURU3_PM	NULL
 #endif /* CONFIG_PM */
diff --git a/drivers/hwmon/ad7314.c b/drivers/hwmon/ad7314.c
index a57584d28a4..f4f9b219bf1 100644
--- a/drivers/hwmon/ad7314.c
+++ b/drivers/hwmon/ad7314.c
@@ -116,7 +116,7 @@ static int ad7314_probe(struct spi_device *spi_dev)
 	if (chip == NULL)
 		return -ENOMEM;
 
-	dev_set_drvdata(&spi_dev->dev, chip);
+	spi_set_drvdata(spi_dev, chip);
 
 	ret = sysfs_create_group(&spi_dev->dev.kobj, &ad7314_group);
 	if (ret < 0)
@@ -137,7 +137,7 @@ error_remove_group:
 
 static int ad7314_remove(struct spi_device *spi_dev)
 {
-	struct ad7314_data *chip = dev_get_drvdata(&spi_dev->dev);
+	struct ad7314_data *chip = spi_get_drvdata(spi_dev);
 
 	hwmon_device_unregister(chip->hwmon_dev);
 	sysfs_remove_group(&spi_dev->dev.kobj, &ad7314_group);
@@ -166,6 +166,5 @@ static struct spi_driver ad7314_driver = {
 module_spi_driver(ad7314_driver);
 
 MODULE_AUTHOR("Sonic Zhang <sonic.zhang@analog.com>");
-MODULE_DESCRIPTION("Analog Devices AD7314, ADT7301 and ADT7302 digital"
-			" temperature sensor driver");
+MODULE_DESCRIPTION("Analog Devices AD7314, ADT7301 and ADT7302 digital temperature sensor driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/hwmon/adm1021.c b/drivers/hwmon/adm1021.c
index 71bcba8abfc..7e76922a4ba 100644
--- a/drivers/hwmon/adm1021.c
+++ b/drivers/hwmon/adm1021.c
@@ -312,8 +312,7 @@ static int adm1021_detect(struct i2c_client *client,
 	int conv_rate, status, config, man_id, dev_id;
 
 	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA)) {
-		pr_debug("adm1021: detect failed, "
-			 "smbus byte data not supported!\n");
+		pr_debug("detect failed, smbus byte data not supported!\n");
 		return -ENODEV;
 	}
 
@@ -324,7 +323,7 @@ static int adm1021_detect(struct i2c_client *client,
 
 	/* Check unused bits */
 	if ((status & 0x03) || (config & 0x3F) || (conv_rate & 0xF8)) {
-		pr_debug("adm1021: detect failed, chip not detected!\n");
+		pr_debug("detect failed, chip not detected!\n");
 		return -ENODEV;
 	}
 
@@ -353,7 +352,7 @@ static int adm1021_detect(struct i2c_client *client,
 	else
 		type_name = "max1617";
 
-	pr_debug("adm1021: Detected chip %s at adapter %d, address 0x%02x.\n",
+	pr_debug("Detected chip %s at adapter %d, address 0x%02x.\n",
 		 type_name, i2c_adapter_id(adapter), client->addr);
 	strlcpy(info->type, type_name, I2C_NAME_SIZE);
 
@@ -368,10 +367,8 @@ static int adm1021_probe(struct i2c_client *client,
 
 	data = devm_kzalloc(&client->dev, sizeof(struct adm1021_data),
 			    GFP_KERNEL);
-	if (!data) {
-		pr_debug("adm1021: detect failed, devm_kzalloc failed!\n");
+	if (!data)
 		return -ENOMEM;
-	}
 
 	i2c_set_clientdata(client, data);
 	data->type = id->driver_data;
diff --git a/drivers/hwmon/adm1026.c b/drivers/hwmon/adm1026.c
index ea09046e651..3a6d9ef1c16 100644
--- a/drivers/hwmon/adm1026.c
+++ b/drivers/hwmon/adm1026.c
@@ -49,14 +49,14 @@ static int gpio_fan[8] = { -1, -1, -1, -1, -1, -1, -1, -1 };
 module_param_array(gpio_input, int, NULL, 0);
 MODULE_PARM_DESC(gpio_input, "List of GPIO pins (0-16) to program as inputs");
 module_param_array(gpio_output, int, NULL, 0);
-MODULE_PARM_DESC(gpio_output, "List of GPIO pins (0-16) to program as "
-	"outputs");
+MODULE_PARM_DESC(gpio_output,
+		 "List of GPIO pins (0-16) to program as outputs");
 module_param_array(gpio_inverted, int, NULL, 0);
-MODULE_PARM_DESC(gpio_inverted, "List of GPIO pins (0-16) to program as "
-	"inverted");
+MODULE_PARM_DESC(gpio_inverted,
+		 "List of GPIO pins (0-16) to program as inverted");
 module_param_array(gpio_normal, int, NULL, 0);
-MODULE_PARM_DESC(gpio_normal, "List of GPIO pins (0-16) to program as "
-	"normal/non-inverted");
+MODULE_PARM_DESC(gpio_normal,
+		 "List of GPIO pins (0-16) to program as normal/non-inverted");
 module_param_array(gpio_fan, int, NULL, 0);
 MODULE_PARM_DESC(gpio_fan, "List of GPIO pins (0-7) to program as fan tachs");
 
@@ -372,31 +372,31 @@ static void adm1026_init_client(struct i2c_client *client)
 	dev_dbg(&client->dev, "ADM1026_REG_CONFIG1 is: 0x%02x\n",
 		data->config1);
 	if ((data->config1 & CFG1_MONITOR) == 0) {
-		dev_dbg(&client->dev, "Monitoring not currently "
-			"enabled.\n");
+		dev_dbg(&client->dev,
+			"Monitoring not currently enabled.\n");
 	}
 	if (data->config1 & CFG1_INT_ENABLE) {
-		dev_dbg(&client->dev, "SMBALERT interrupts are "
-			"enabled.\n");
+		dev_dbg(&client->dev,
+			"SMBALERT interrupts are enabled.\n");
 	}
 	if (data->config1 & CFG1_AIN8_9) {
-		dev_dbg(&client->dev, "in8 and in9 enabled. "
-			"temp3 disabled.\n");
+		dev_dbg(&client->dev,
+			"in8 and in9 enabled. temp3 disabled.\n");
 	} else {
-		dev_dbg(&client->dev, "temp3 enabled.  in8 and "
-			"in9 disabled.\n");
+		dev_dbg(&client->dev,
+			"temp3 enabled.  in8 and in9 disabled.\n");
 	}
 	if (data->config1 & CFG1_THERM_HOT) {
-		dev_dbg(&client->dev, "Automatic THERM, PWM, "
-			"and temp limits enabled.\n");
+		dev_dbg(&client->dev,
+			"Automatic THERM, PWM, and temp limits enabled.\n");
 	}
 
 	if (data->config3 & CFG3_GPIO16_ENABLE) {
-		dev_dbg(&client->dev, "GPIO16 enabled.  THERM "
-			"pin disabled.\n");
+		dev_dbg(&client->dev,
+			"GPIO16 enabled.  THERM pin disabled.\n");
 	} else {
-		dev_dbg(&client->dev, "THERM pin enabled.  "
-			"GPIO16 disabled.\n");
+		dev_dbg(&client->dev,
+			"THERM pin enabled.  GPIO16 disabled.\n");
 	}
 	if (data->config3 & CFG3_VREF_250)
 		dev_dbg(&client->dev, "Vref is 2.50 Volts.\n");
@@ -1798,8 +1798,8 @@ static int adm1026_detect(struct i2c_client *client,
 	company = adm1026_read_value(client, ADM1026_REG_COMPANY);
 	verstep = adm1026_read_value(client, ADM1026_REG_VERSTEP);
 
-	dev_dbg(&adapter->dev, "Detecting device at %d,0x%02x with"
-		" COMPANY: 0x%02x and VERSTEP: 0x%02x\n",
+	dev_dbg(&adapter->dev,
+		"Detecting device at %d,0x%02x with COMPANY: 0x%02x and VERSTEP: 0x%02x\n",
 		i2c_adapter_id(client->adapter), client->addr,
 		company, verstep);
 
@@ -1811,11 +1811,12 @@ static int adm1026_detect(struct i2c_client *client,
 		/* Analog Devices ADM1026 */
 	} else if (company == ADM1026_COMPANY_ANALOG_DEV
 		&& (verstep & 0xf0) == ADM1026_VERSTEP_GENERIC) {
-		dev_err(&adapter->dev, "Unrecognized stepping "
-			"0x%02x. Defaulting to ADM1026.\n", verstep);
+		dev_err(&adapter->dev,
+			"Unrecognized stepping 0x%02x. Defaulting to ADM1026.\n",
+			verstep);
 	} else if ((verstep & 0xf0) == ADM1026_VERSTEP_GENERIC) {
-		dev_err(&adapter->dev, "Found version/stepping "
-			"0x%02x. Assuming generic ADM1026.\n",
+		dev_err(&adapter->dev,
+			"Found version/stepping 0x%02x. Assuming generic ADM1026.\n",
 			verstep);
 	} else {
 		dev_dbg(&adapter->dev, "Autodetection failed\n");
diff --git a/drivers/hwmon/adm1029.c b/drivers/hwmon/adm1029.c
index 97f4718382f..9ee5e066423 100644
--- a/drivers/hwmon/adm1029.c
+++ b/drivers/hwmon/adm1029.c
@@ -224,8 +224,9 @@ static ssize_t set_fan_div(struct device *dev,
 		break;
 	default:
 		mutex_unlock(&data->update_lock);
-		dev_err(&client->dev, "fan_div value %ld not "
-			"supported. Choose one of 1, 2 or 4!\n", val);
+		dev_err(&client->dev,
+			"fan_div value %ld not supported. Choose one of 1, 2 or 4!\n",
+			val);
 		return -EINVAL;
 	}
 	/* Update the value */
@@ -326,8 +327,8 @@ static int adm1029_detect(struct i2c_client *client,
 		 * There are no "official" CHIP ID, so actually
 		 * we use Major/Minor revision for that
 		 */
-		pr_info("adm1029: Unknown major revision %x, "
-			"please let us know\n", chip_id);
+		pr_info("Unknown major revision %x, please let us know\n",
+			chip_id);
 		return -ENODEV;
 	}
 
diff --git a/drivers/hwmon/adm9240.c b/drivers/hwmon/adm9240.c
index 2416628e0ab..086d02a9ecd 100644
--- a/drivers/hwmon/adm9240.c
+++ b/drivers/hwmon/adm9240.c
@@ -351,8 +351,9 @@ static void adm9240_write_fan_div(struct i2c_client *client, int nr,
 	reg &= ~(3 << shift);
 	reg |= (fan_div << shift);
 	i2c_smbus_write_byte_data(client, ADM9240_REG_VID_FAN_DIV, reg);
-	dev_dbg(&client->dev, "fan%d clock divider changed from %u "
-			"to %u\n", nr + 1, 1 << old, 1 << fan_div);
+	dev_dbg(&client->dev,
+		"fan%d clock divider changed from %u to %u\n",
+		nr + 1, 1 << old, 1 << fan_div);
 }
 
 /*
@@ -699,8 +700,8 @@ static void adm9240_init_client(struct i2c_client *client)
 		/* start measurement cycle */
 		i2c_smbus_write_byte_data(client, ADM9240_REG_CONFIG, 1);
 
-		dev_info(&client->dev, "cold start: config was 0x%02x "
-				"mode %u\n", conf, mode);
+		dev_info(&client->dev,
+			 "cold start: config was 0x%02x mode %u\n", conf, mode);
 	}
 }
 
diff --git a/drivers/hwmon/ads7871.c b/drivers/hwmon/ads7871.c
index a79875986f9..3eff73b6220 100644
--- a/drivers/hwmon/ads7871.c
+++ b/drivers/hwmon/ads7871.c
@@ -40,25 +40,25 @@
  * the instruction byte
  */
 /*Instruction Bit masks*/
-#define INST_MODE_bm	(1<<7)
-#define INST_READ_bm	(1<<6)
-#define INST_16BIT_bm	(1<<5)
+#define INST_MODE_BM	(1 << 7)
+#define INST_READ_BM	(1 << 6)
+#define INST_16BIT_BM	(1 << 5)
 
 /*From figure 18 in the datasheet*/
 /*bit masks for Rev/Oscillator Control Register*/
-#define MUX_CNV_bv	7
-#define MUX_CNV_bm	(1<<MUX_CNV_bv)
-#define MUX_M3_bm	(1<<3) /*M3 selects single ended*/
-#define MUX_G_bv	4 /*allows for reg = (gain << MUX_G_bv) | ...*/
+#define MUX_CNV_BV	7
+#define MUX_CNV_BM	(1 << MUX_CNV_BV)
+#define MUX_M3_BM	(1 << 3) /*M3 selects single ended*/
+#define MUX_G_BV	4 /*allows for reg = (gain << MUX_G_BV) | ...*/
 
 /*From figure 18 in the datasheet*/
 /*bit masks for Rev/Oscillator Control Register*/
-#define OSC_OSCR_bm	(1<<5)
-#define OSC_OSCE_bm	(1<<4)
-#define OSC_REFE_bm	(1<<3)
-#define OSC_BUFE_bm	(1<<2)
-#define OSC_R2V_bm	(1<<1)
-#define OSC_RBG_bm	(1<<0)
+#define OSC_OSCR_BM	(1 << 5)
+#define OSC_OSCE_BM	(1 << 4)
+#define OSC_REFE_BM	(1 << 3)
+#define OSC_BUFE_BM	(1 << 2)
+#define OSC_R2V_BM	(1 << 1)
+#define OSC_RBG_BM	(1 << 0)
 
 #include <linux/module.h>
 #include <linux/init.h>
@@ -79,7 +79,7 @@ struct ads7871_data {
 static int ads7871_read_reg8(struct spi_device *spi, int reg)
 {
 	int ret;
-	reg = reg | INST_READ_bm;
+	reg = reg | INST_READ_BM;
 	ret = spi_w8r8(spi, reg);
 	return ret;
 }
@@ -87,7 +87,7 @@ static int ads7871_read_reg8(struct spi_device *spi, int reg)
 static int ads7871_read_reg16(struct spi_device *spi, int reg)
 {
 	int ret;
-	reg = reg | INST_READ_bm | INST_16BIT_bm;
+	reg = reg | INST_READ_BM | INST_16BIT_BM;
 	ret = spi_w8r16(spi, reg);
 	return ret;
 }
@@ -111,13 +111,13 @@ static ssize_t show_voltage(struct device *dev,
 	 * TODO: add support for conversions
 	 * other than single ended with a gain of 1
 	 */
-	/*MUX_M3_bm forces single ended*/
+	/*MUX_M3_BM forces single ended*/
 	/*This is also where the gain of the PGA would be set*/
 	ads7871_write_reg8(spi, REG_GAIN_MUX,
-		(MUX_CNV_bm | MUX_M3_bm | channel));
+		(MUX_CNV_BM | MUX_M3_BM | channel));
 
 	ret = ads7871_read_reg8(spi, REG_GAIN_MUX);
-	mux_cnv = ((ret & MUX_CNV_bm)>>MUX_CNV_bv);
+	mux_cnv = ((ret & MUX_CNV_BM) >> MUX_CNV_BV);
 	/*
 	 * on 400MHz arm9 platform the conversion
 	 * is already done when we do this test
@@ -125,14 +125,14 @@ static ssize_t show_voltage(struct device *dev,
 	while ((i < 2) && mux_cnv) {
 		i++;
 		ret = ads7871_read_reg8(spi, REG_GAIN_MUX);
-		mux_cnv = ((ret & MUX_CNV_bm)>>MUX_CNV_bv);
+		mux_cnv = ((ret & MUX_CNV_BM) >> MUX_CNV_BV);
 		msleep_interruptible(1);
 	}
 
 	if (mux_cnv == 0) {
 		val = ads7871_read_reg16(spi, REG_LS_BYTE);
 		/*result in volts*10000 = (val/8192)*2.5*10000*/
-		val = ((val>>2) * 25000) / 8192;
+		val = ((val >> 2) * 25000) / 8192;
 		return sprintf(buf, "%d\n", val);
 	} else {
 		return -1;
@@ -189,7 +189,7 @@ static int ads7871_probe(struct spi_device *spi)
 	ads7871_write_reg8(spi, REG_SER_CONTROL, 0);
 	ads7871_write_reg8(spi, REG_AD_CONTROL, 0);
 
-	val = (OSC_OSCR_bm | OSC_OSCE_bm | OSC_REFE_bm | OSC_BUFE_bm);
+	val = (OSC_OSCR_BM | OSC_OSCE_BM | OSC_REFE_BM | OSC_BUFE_BM);
 	ads7871_write_reg8(spi, REG_OSC_CONTROL, val);
 	ret = ads7871_read_reg8(spi, REG_OSC_CONTROL);
 
diff --git a/drivers/hwmon/adt7310.c b/drivers/hwmon/adt7310.c
new file mode 100644
index 00000000000..da5f0789fb9
--- /dev/null
+++ b/drivers/hwmon/adt7310.c
@@ -0,0 +1,123 @@
+/*
+ * ADT7310/ADT7310 digital temperature sensor driver
+ *
+ * Copyright 2012-2013 Analog Devices Inc.
+ *   Author: Lars-Peter Clausen <lars@metafoo.de>
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/spi/spi.h>
+#include <asm/unaligned.h>
+
+#include "adt7x10.h"
+
+#define ADT7310_STATUS			0
+#define ADT7310_CONFIG			1
+#define ADT7310_TEMPERATURE		2
+#define ADT7310_ID			3
+#define ADT7310_T_CRIT			4
+#define ADT7310_T_HYST			5
+#define ADT7310_T_ALARM_HIGH		6
+#define ADT7310_T_ALARM_LOW		7
+
+static const u8 adt7310_reg_table[] = {
+	[ADT7X10_TEMPERATURE]   = ADT7310_TEMPERATURE,
+	[ADT7X10_STATUS]	= ADT7310_STATUS,
+	[ADT7X10_CONFIG]	= ADT7310_CONFIG,
+	[ADT7X10_T_ALARM_HIGH]	= ADT7310_T_ALARM_HIGH,
+	[ADT7X10_T_ALARM_LOW]	= ADT7310_T_ALARM_LOW,
+	[ADT7X10_T_CRIT]	= ADT7310_T_CRIT,
+	[ADT7X10_T_HYST]	= ADT7310_T_HYST,
+	[ADT7X10_ID]		= ADT7310_ID,
+};
+
+#define ADT7310_CMD_REG_OFFSET	3
+#define ADT7310_CMD_READ	0x40
+
+#define AD7310_COMMAND(reg) (adt7310_reg_table[(reg)] << ADT7310_CMD_REG_OFFSET)
+
+static int adt7310_spi_read_word(struct device *dev, u8 reg)
+{
+	struct spi_device *spi = to_spi_device(dev);
+	int ret;
+
+	ret = spi_w8r16(spi, AD7310_COMMAND(reg) | ADT7310_CMD_READ);
+	if (ret < 0)
+		return ret;
+
+	return be16_to_cpu((__force __be16)ret);
+}
+
+static int adt7310_spi_write_word(struct device *dev, u8 reg, u16 data)
+{
+	struct spi_device *spi = to_spi_device(dev);
+	u8 buf[3];
+
+	buf[0] = AD7310_COMMAND(reg);
+	put_unaligned_be16(data, &buf[1]);
+
+	return spi_write(spi, buf, sizeof(buf));
+}
+
+static int adt7310_spi_read_byte(struct device *dev, u8 reg)
+{
+	struct spi_device *spi = to_spi_device(dev);
+
+	return spi_w8r8(spi, AD7310_COMMAND(reg) | ADT7310_CMD_READ);
+}
+
+static int adt7310_spi_write_byte(struct device *dev, u8 reg,
+	u8 data)
+{
+	struct spi_device *spi = to_spi_device(dev);
+	u8 buf[2];
+
+	buf[0] = AD7310_COMMAND(reg);
+	buf[1] = data;
+
+	return spi_write(spi, buf, sizeof(buf));
+}
+
+static const struct adt7x10_ops adt7310_spi_ops = {
+	.read_word = adt7310_spi_read_word,
+	.write_word = adt7310_spi_write_word,
+	.read_byte = adt7310_spi_read_byte,
+	.write_byte = adt7310_spi_write_byte,
+};
+
+static int adt7310_spi_probe(struct spi_device *spi)
+{
+	return adt7x10_probe(&spi->dev, spi_get_device_id(spi)->name, spi->irq,
+			&adt7310_spi_ops);
+}
+
+static int adt7310_spi_remove(struct spi_device *spi)
+{
+	return adt7x10_remove(&spi->dev, spi->irq);
+}
+
+static const struct spi_device_id adt7310_id[] = {
+	{ "adt7310", 0 },
+	{ "adt7320", 0 },
+	{}
+};
+MODULE_DEVICE_TABLE(spi, adt7310_id);
+
+static struct spi_driver adt7310_driver = {
+	.driver = {
+		.name	= "adt7310",
+		.owner	= THIS_MODULE,
+		.pm	= ADT7X10_DEV_PM_OPS,
+	},
+	.probe		= adt7310_spi_probe,
+	.remove		= adt7310_spi_remove,
+	.id_table	= adt7310_id,
+};
+module_spi_driver(adt7310_driver);
+
+MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
+MODULE_DESCRIPTION("ADT7310/ADT7320 driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/adt7410.c b/drivers/hwmon/adt7410.c
index 99a7290da0a..0dc066a939b 100644
--- a/drivers/hwmon/adt7410.c
+++ b/drivers/hwmon/adt7410.c
@@ -1,460 +1,80 @@
 /*
- * adt7410.c - Part of lm_sensors, Linux kernel modules for hardware
- *	 monitoring
- * This driver handles the ADT7410 and compatible digital temperature sensors.
- * Hartmut Knaack <knaack.h@gmx.de> 2012-07-22
- * based on lm75.c by Frodo Looijaard <frodol@dds.nl>
- * and adt7410.c from iio-staging by Sonic Zhang <sonic.zhang@analog.com>
+ * ADT7410/ADT7420 digital temperature sensor driver
  *
- * 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.
+ * Copyright 2012-2013 Analog Devices Inc.
+ *   Author: Lars-Peter Clausen <lars@metafoo.de>
  *
- * 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.
+ * Licensed under the GPL-2 or later.
  */
 
 #include <linux/module.h>
 #include <linux/init.h>
-#include <linux/slab.h>
-#include <linux/jiffies.h>
 #include <linux/i2c.h>
-#include <linux/hwmon.h>
-#include <linux/hwmon-sysfs.h>
-#include <linux/err.h>
-#include <linux/mutex.h>
-#include <linux/delay.h>
-
-/*
- * ADT7410 registers definition
- */
-
-#define ADT7410_TEMPERATURE		0
-#define ADT7410_STATUS			2
-#define ADT7410_CONFIG			3
-#define ADT7410_T_ALARM_HIGH		4
-#define ADT7410_T_ALARM_LOW		6
-#define ADT7410_T_CRIT			8
-#define ADT7410_T_HYST			0xA
-
-/*
- * ADT7410 status
- */
-#define ADT7410_STAT_T_LOW		(1 << 4)
-#define ADT7410_STAT_T_HIGH		(1 << 5)
-#define ADT7410_STAT_T_CRIT		(1 << 6)
-#define ADT7410_STAT_NOT_RDY		(1 << 7)
-
-/*
- * ADT7410 config
- */
-#define ADT7410_FAULT_QUEUE_MASK	(1 << 0 | 1 << 1)
-#define ADT7410_CT_POLARITY		(1 << 2)
-#define ADT7410_INT_POLARITY		(1 << 3)
-#define ADT7410_EVENT_MODE		(1 << 4)
-#define ADT7410_MODE_MASK		(1 << 5 | 1 << 6)
-#define ADT7410_FULL			(0 << 5 | 0 << 6)
-#define ADT7410_PD			(1 << 5 | 1 << 6)
-#define ADT7410_RESOLUTION		(1 << 7)
-
-/*
- * ADT7410 masks
- */
-#define ADT7410_T13_VALUE_MASK			0xFFF8
-#define ADT7410_T_HYST_MASK			0xF
-
-/* straight from the datasheet */
-#define ADT7410_TEMP_MIN (-55000)
-#define ADT7410_TEMP_MAX 150000
-
-enum adt7410_type {		/* keep sorted in alphabetical order */
-	adt7410,
-};
-
-static const u8 ADT7410_REG_TEMP[4] = {
-	ADT7410_TEMPERATURE,		/* input */
-	ADT7410_T_ALARM_HIGH,		/* high */
-	ADT7410_T_ALARM_LOW,		/* low */
-	ADT7410_T_CRIT,			/* critical */
-};
-
-/* Each client has this additional data */
-struct adt7410_data {
-	struct device		*hwmon_dev;
-	struct mutex		update_lock;
-	u8			config;
-	u8			oldconfig;
-	bool			valid;		/* true if registers valid */
-	unsigned long		last_updated;	/* In jiffies */
-	s16			temp[4];	/* Register values,
-						   0 = input
-						   1 = high
-						   2 = low
-						   3 = critical */
-	u8			hyst;		/* hysteresis offset */
-};
-
-/*
- * adt7410 register access by I2C
- */
-static int adt7410_temp_ready(struct i2c_client *client)
-{
-	int i, status;
-
-	for (i = 0; i < 6; i++) {
-		status = i2c_smbus_read_byte_data(client, ADT7410_STATUS);
-		if (status < 0)
-			return status;
-		if (!(status & ADT7410_STAT_NOT_RDY))
-			return 0;
-		msleep(60);
-	}
-	return -ETIMEDOUT;
-}
-
-static struct adt7410_data *adt7410_update_device(struct device *dev)
-{
-	struct i2c_client *client = to_i2c_client(dev);
-	struct adt7410_data *data = i2c_get_clientdata(client);
-	struct adt7410_data *ret = data;
-	mutex_lock(&data->update_lock);
-
-	if (time_after(jiffies, data->last_updated + HZ + HZ / 2)
-	    || !data->valid) {
-		int i, status;
 
-		dev_dbg(&client->dev, "Starting update\n");
+#include "adt7x10.h"
 
-		status = adt7410_temp_ready(client); /* check for new value */
-		if (unlikely(status)) {
-			ret = ERR_PTR(status);
-			goto abort;
-		}
-		for (i = 0; i < ARRAY_SIZE(data->temp); i++) {
-			status = i2c_smbus_read_word_swapped(client,
-							ADT7410_REG_TEMP[i]);
-			if (unlikely(status < 0)) {
-				dev_dbg(dev,
-					"Failed to read value: reg %d, error %d\n",
-					ADT7410_REG_TEMP[i], status);
-				ret = ERR_PTR(status);
-				goto abort;
-			}
-			data->temp[i] = status;
-		}
-		status = i2c_smbus_read_byte_data(client, ADT7410_T_HYST);
-		if (unlikely(status < 0)) {
-			dev_dbg(dev,
-				"Failed to read value: reg %d, error %d\n",
-				ADT7410_T_HYST, status);
-			ret = ERR_PTR(status);
-			goto abort;
-		}
-		data->hyst = status;
-		data->last_updated = jiffies;
-		data->valid = true;
-	}
-
-abort:
-	mutex_unlock(&data->update_lock);
-	return ret;
-}
-
-static s16 ADT7410_TEMP_TO_REG(long temp)
+static int adt7410_i2c_read_word(struct device *dev, u8 reg)
 {
-	return DIV_ROUND_CLOSEST(clamp_val(temp, ADT7410_TEMP_MIN,
-					   ADT7410_TEMP_MAX) * 128, 1000);
+	return i2c_smbus_read_word_swapped(to_i2c_client(dev), reg);
 }
 
-static int ADT7410_REG_TO_TEMP(struct adt7410_data *data, s16 reg)
+static int adt7410_i2c_write_word(struct device *dev, u8 reg, u16 data)
 {
-	/* in 13 bit mode, bits 0-2 are status flags - mask them out */
-	if (!(data->config & ADT7410_RESOLUTION))
-		reg &= ADT7410_T13_VALUE_MASK;
-	/*
-	 * temperature is stored in twos complement format, in steps of
-	 * 1/128°C
-	 */
-	return DIV_ROUND_CLOSEST(reg * 1000, 128);
+	return i2c_smbus_write_word_swapped(to_i2c_client(dev), reg, data);
 }
 
-/*-----------------------------------------------------------------------*/
-
-/* sysfs attributes for hwmon */
-
-static ssize_t adt7410_show_temp(struct device *dev,
-				 struct device_attribute *da, char *buf)
+static int adt7410_i2c_read_byte(struct device *dev, u8 reg)
 {
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
-	struct adt7410_data *data = adt7410_update_device(dev);
-
-	if (IS_ERR(data))
-		return PTR_ERR(data);
-
-	return sprintf(buf, "%d\n", ADT7410_REG_TO_TEMP(data,
-		       data->temp[attr->index]));
+	return i2c_smbus_read_byte_data(to_i2c_client(dev), reg);
 }
 
-static ssize_t adt7410_set_temp(struct device *dev,
-				struct device_attribute *da,
-				const char *buf, size_t count)
+static int adt7410_i2c_write_byte(struct device *dev, u8 reg, u8 data)
 {
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
-	struct i2c_client *client = to_i2c_client(dev);
-	struct adt7410_data *data = i2c_get_clientdata(client);
-	int nr = attr->index;
-	long temp;
-	int ret;
-
-	ret = kstrtol(buf, 10, &temp);
-	if (ret)
-		return ret;
-
-	mutex_lock(&data->update_lock);
-	data->temp[nr] = ADT7410_TEMP_TO_REG(temp);
-	ret = i2c_smbus_write_word_swapped(client, ADT7410_REG_TEMP[nr],
-					   data->temp[nr]);
-	if (ret)
-		count = ret;
-	mutex_unlock(&data->update_lock);
-	return count;
+	return i2c_smbus_write_byte_data(to_i2c_client(dev), reg, data);
 }
 
-static ssize_t adt7410_show_t_hyst(struct device *dev,
-				   struct device_attribute *da,
-				   char *buf)
-{
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
-	struct adt7410_data *data;
-	int nr = attr->index;
-	int hyst;
-
-	data = adt7410_update_device(dev);
-	if (IS_ERR(data))
-		return PTR_ERR(data);
-	hyst = (data->hyst & ADT7410_T_HYST_MASK) * 1000;
-
-	/*
-	 * hysteresis is stored as a 4 bit offset in the device, convert it
-	 * to an absolute value
-	 */
-	if (nr == 2)	/* min has positive offset, others have negative */
-		hyst = -hyst;
-	return sprintf(buf, "%d\n",
-		       ADT7410_REG_TO_TEMP(data, data->temp[nr]) - hyst);
-}
-
-static ssize_t adt7410_set_t_hyst(struct device *dev,
-				  struct device_attribute *da,
-				  const char *buf, size_t count)
-{
-	struct i2c_client *client = to_i2c_client(dev);
-	struct adt7410_data *data = i2c_get_clientdata(client);
-	int limit, ret;
-	long hyst;
-
-	ret = kstrtol(buf, 10, &hyst);
-	if (ret)
-		return ret;
-	/* convert absolute hysteresis value to a 4 bit delta value */
-	limit = ADT7410_REG_TO_TEMP(data, data->temp[1]);
-	hyst = clamp_val(hyst, ADT7410_TEMP_MIN, ADT7410_TEMP_MAX);
-	data->hyst = clamp_val(DIV_ROUND_CLOSEST(limit - hyst, 1000), 0,
-			       ADT7410_T_HYST_MASK);
-	ret = i2c_smbus_write_byte_data(client, ADT7410_T_HYST, data->hyst);
-	if (ret)
-		return ret;
-
-	return count;
-}
-
-static ssize_t adt7410_show_alarm(struct device *dev,
-				  struct device_attribute *da,
-				  char *buf)
-{
-	struct i2c_client *client = to_i2c_client(dev);
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
-	int ret;
-
-	ret = i2c_smbus_read_byte_data(client, ADT7410_STATUS);
-	if (ret < 0)
-		return ret;
-
-	return sprintf(buf, "%d\n", !!(ret & attr->index));
-}
-
-static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, adt7410_show_temp, NULL, 0);
-static SENSOR_DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO,
-			  adt7410_show_temp, adt7410_set_temp, 1);
-static SENSOR_DEVICE_ATTR(temp1_min, S_IWUSR | S_IRUGO,
-			  adt7410_show_temp, adt7410_set_temp, 2);
-static SENSOR_DEVICE_ATTR(temp1_crit, S_IWUSR | S_IRUGO,
-			  adt7410_show_temp, adt7410_set_temp, 3);
-static SENSOR_DEVICE_ATTR(temp1_max_hyst, S_IWUSR | S_IRUGO,
-			  adt7410_show_t_hyst, adt7410_set_t_hyst, 1);
-static SENSOR_DEVICE_ATTR(temp1_min_hyst, S_IRUGO,
-			  adt7410_show_t_hyst, NULL, 2);
-static SENSOR_DEVICE_ATTR(temp1_crit_hyst, S_IRUGO,
-			  adt7410_show_t_hyst, NULL, 3);
-static SENSOR_DEVICE_ATTR(temp1_min_alarm, S_IRUGO, adt7410_show_alarm,
-			  NULL, ADT7410_STAT_T_LOW);
-static SENSOR_DEVICE_ATTR(temp1_max_alarm, S_IRUGO, adt7410_show_alarm,
-			  NULL, ADT7410_STAT_T_HIGH);
-static SENSOR_DEVICE_ATTR(temp1_crit_alarm, S_IRUGO, adt7410_show_alarm,
-			  NULL, ADT7410_STAT_T_CRIT);
-
-static struct attribute *adt7410_attributes[] = {
-	&sensor_dev_attr_temp1_input.dev_attr.attr,
-	&sensor_dev_attr_temp1_max.dev_attr.attr,
-	&sensor_dev_attr_temp1_min.dev_attr.attr,
-	&sensor_dev_attr_temp1_crit.dev_attr.attr,
-	&sensor_dev_attr_temp1_max_hyst.dev_attr.attr,
-	&sensor_dev_attr_temp1_min_hyst.dev_attr.attr,
-	&sensor_dev_attr_temp1_crit_hyst.dev_attr.attr,
-	&sensor_dev_attr_temp1_min_alarm.dev_attr.attr,
-	&sensor_dev_attr_temp1_max_alarm.dev_attr.attr,
-	&sensor_dev_attr_temp1_crit_alarm.dev_attr.attr,
-	NULL
+static const struct adt7x10_ops adt7410_i2c_ops = {
+	.read_word = adt7410_i2c_read_word,
+	.write_word = adt7410_i2c_write_word,
+	.read_byte = adt7410_i2c_read_byte,
+	.write_byte = adt7410_i2c_write_byte,
 };
 
-static const struct attribute_group adt7410_group = {
-	.attrs = adt7410_attributes,
-};
-
-/*-----------------------------------------------------------------------*/
-
-/* device probe and removal */
-
-static int adt7410_probe(struct i2c_client *client,
-			 const struct i2c_device_id *id)
+static int adt7410_i2c_probe(struct i2c_client *client,
+	const struct i2c_device_id *id)
 {
-	struct adt7410_data *data;
-	int ret;
-
 	if (!i2c_check_functionality(client->adapter,
 			I2C_FUNC_SMBUS_BYTE_DATA | I2C_FUNC_SMBUS_WORD_DATA))
 		return -ENODEV;
 
-	data = devm_kzalloc(&client->dev, sizeof(struct adt7410_data),
-			    GFP_KERNEL);
-	if (!data)
-		return -ENOMEM;
-
-	i2c_set_clientdata(client, data);
-	mutex_init(&data->update_lock);
-
-	/* configure as specified */
-	ret = i2c_smbus_read_byte_data(client, ADT7410_CONFIG);
-	if (ret < 0) {
-		dev_dbg(&client->dev, "Can't read config? %d\n", ret);
-		return ret;
-	}
-	data->oldconfig = ret;
-	/*
-	 * Set to 16 bit resolution, continous conversion and comparator mode.
-	 */
-	ret &= ~ADT7410_MODE_MASK;
-	data->config = ret | ADT7410_FULL | ADT7410_RESOLUTION |
-			ADT7410_EVENT_MODE;
-	if (data->config != data->oldconfig) {
-		ret = i2c_smbus_write_byte_data(client, ADT7410_CONFIG,
-						data->config);
-		if (ret)
-			return ret;
-	}
-	dev_dbg(&client->dev, "Config %02x\n", data->config);
-
-	/* Register sysfs hooks */
-	ret = sysfs_create_group(&client->dev.kobj, &adt7410_group);
-	if (ret)
-		goto exit_restore;
-
-	data->hwmon_dev = hwmon_device_register(&client->dev);
-	if (IS_ERR(data->hwmon_dev)) {
-		ret = PTR_ERR(data->hwmon_dev);
-		goto exit_remove;
-	}
-
-	dev_info(&client->dev, "sensor '%s'\n", client->name);
-
-	return 0;
-
-exit_remove:
-	sysfs_remove_group(&client->dev.kobj, &adt7410_group);
-exit_restore:
-	i2c_smbus_write_byte_data(client, ADT7410_CONFIG, data->oldconfig);
-	return ret;
+	return adt7x10_probe(&client->dev, NULL, client->irq, &adt7410_i2c_ops);
 }
 
-static int adt7410_remove(struct i2c_client *client)
+static int adt7410_i2c_remove(struct i2c_client *client)
 {
-	struct adt7410_data *data = i2c_get_clientdata(client);
-
-	hwmon_device_unregister(data->hwmon_dev);
-	sysfs_remove_group(&client->dev.kobj, &adt7410_group);
-	if (data->oldconfig != data->config)
-		i2c_smbus_write_byte_data(client, ADT7410_CONFIG,
-					  data->oldconfig);
-	return 0;
+	return adt7x10_remove(&client->dev, client->irq);
 }
 
 static const struct i2c_device_id adt7410_ids[] = {
-	{ "adt7410", adt7410, },
-	{ "adt7420", adt7410, },
-	{ /* LIST END */ }
+	{ "adt7410", 0 },
+	{ "adt7420", 0 },
+	{}
 };
 MODULE_DEVICE_TABLE(i2c, adt7410_ids);
 
-#ifdef CONFIG_PM_SLEEP
-static int adt7410_suspend(struct device *dev)
-{
-	int ret;
-	struct i2c_client *client = to_i2c_client(dev);
-	struct adt7410_data *data = i2c_get_clientdata(client);
-
-	ret = i2c_smbus_write_byte_data(client, ADT7410_CONFIG,
-					data->config | ADT7410_PD);
-	return ret;
-}
-
-static int adt7410_resume(struct device *dev)
-{
-	int ret;
-	struct i2c_client *client = to_i2c_client(dev);
-	struct adt7410_data *data = i2c_get_clientdata(client);
-
-	ret = i2c_smbus_write_byte_data(client, ADT7410_CONFIG, data->config);
-	return ret;
-}
-
-static SIMPLE_DEV_PM_OPS(adt7410_dev_pm_ops, adt7410_suspend, adt7410_resume);
-
-#define ADT7410_DEV_PM_OPS (&adt7410_dev_pm_ops)
-#else
-#define ADT7410_DEV_PM_OPS NULL
-#endif /* CONFIG_PM */
-
 static struct i2c_driver adt7410_driver = {
 	.class		= I2C_CLASS_HWMON,
 	.driver = {
 		.name	= "adt7410",
-		.pm	= ADT7410_DEV_PM_OPS,
+		.pm	= ADT7X10_DEV_PM_OPS,
 	},
-	.probe		= adt7410_probe,
-	.remove		= adt7410_remove,
+	.probe		= adt7410_i2c_probe,
+	.remove		= adt7410_i2c_remove,
 	.id_table	= adt7410_ids,
 	.address_list	= I2C_ADDRS(0x48, 0x49, 0x4a, 0x4b),
 };
-
 module_i2c_driver(adt7410_driver);
 
-MODULE_AUTHOR("Hartmut Knaack");
-MODULE_DESCRIPTION("ADT7410/ADT7420 driver");
+MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
+MODULE_DESCRIPTION("ADT7410/AD7420 driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/adt7411.c b/drivers/hwmon/adt7411.c
index 34ff03abb50..d9299dee37d 100644
--- a/drivers/hwmon/adt7411.c
+++ b/drivers/hwmon/adt7411.c
@@ -259,15 +259,17 @@ static int adt7411_detect(struct i2c_client *client,
 
 	val = i2c_smbus_read_byte_data(client, ADT7411_REG_MANUFACTURER_ID);
 	if (val < 0 || val != ADT7411_MANUFACTURER_ID) {
-		dev_dbg(&client->dev, "Wrong manufacturer ID. Got %d, "
-			"expected %d\n", val, ADT7411_MANUFACTURER_ID);
+		dev_dbg(&client->dev,
+			"Wrong manufacturer ID. Got %d, expected %d\n",
+			val, ADT7411_MANUFACTURER_ID);
 		return -ENODEV;
 	}
 
 	val = i2c_smbus_read_byte_data(client, ADT7411_REG_DEVICE_ID);
 	if (val < 0 || val != ADT7411_DEVICE_ID) {
-		dev_dbg(&client->dev, "Wrong device ID. Got %d, "
-			"expected %d\n", val, ADT7411_DEVICE_ID);
+		dev_dbg(&client->dev,
+			"Wrong device ID. Got %d, expected %d\n",
+			val, ADT7411_DEVICE_ID);
 		return -ENODEV;
 	}
 
diff --git a/drivers/hwmon/adt7x10.c b/drivers/hwmon/adt7x10.c
new file mode 100644
index 00000000000..98141f48316
--- /dev/null
+++ b/drivers/hwmon/adt7x10.c
@@ -0,0 +1,511 @@
+/*
+ * adt7x10.c - Part of lm_sensors, Linux kernel modules for hardware
+ *	 monitoring
+ * This driver handles the ADT7410 and compatible digital temperature sensors.
+ * Hartmut Knaack <knaack.h@gmx.de> 2012-07-22
+ * based on lm75.c by Frodo Looijaard <frodol@dds.nl>
+ * and adt7410.c from iio-staging by Sonic Zhang <sonic.zhang@analog.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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/jiffies.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/err.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+
+#include "adt7x10.h"
+
+/*
+ * ADT7X10 status
+ */
+#define ADT7X10_STAT_T_LOW		(1 << 4)
+#define ADT7X10_STAT_T_HIGH		(1 << 5)
+#define ADT7X10_STAT_T_CRIT		(1 << 6)
+#define ADT7X10_STAT_NOT_RDY		(1 << 7)
+
+/*
+ * ADT7X10 config
+ */
+#define ADT7X10_FAULT_QUEUE_MASK	(1 << 0 | 1 << 1)
+#define ADT7X10_CT_POLARITY		(1 << 2)
+#define ADT7X10_INT_POLARITY		(1 << 3)
+#define ADT7X10_EVENT_MODE		(1 << 4)
+#define ADT7X10_MODE_MASK		(1 << 5 | 1 << 6)
+#define ADT7X10_FULL			(0 << 5 | 0 << 6)
+#define ADT7X10_PD			(1 << 5 | 1 << 6)
+#define ADT7X10_RESOLUTION		(1 << 7)
+
+/*
+ * ADT7X10 masks
+ */
+#define ADT7X10_T13_VALUE_MASK		0xFFF8
+#define ADT7X10_T_HYST_MASK		0xF
+
+/* straight from the datasheet */
+#define ADT7X10_TEMP_MIN (-55000)
+#define ADT7X10_TEMP_MAX 150000
+
+/* Each client has this additional data */
+struct adt7x10_data {
+	const struct adt7x10_ops *ops;
+	const char		*name;
+	struct device		*hwmon_dev;
+	struct mutex		update_lock;
+	u8			config;
+	u8			oldconfig;
+	bool			valid;		/* true if registers valid */
+	unsigned long		last_updated;	/* In jiffies */
+	s16			temp[4];	/* Register values,
+						   0 = input
+						   1 = high
+						   2 = low
+						   3 = critical */
+	u8			hyst;		/* hysteresis offset */
+};
+
+static int adt7x10_read_byte(struct device *dev, u8 reg)
+{
+	struct adt7x10_data *d = dev_get_drvdata(dev);
+	return d->ops->read_byte(dev, reg);
+}
+
+static int adt7x10_write_byte(struct device *dev, u8 reg, u8 data)
+{
+	struct adt7x10_data *d = dev_get_drvdata(dev);
+	return d->ops->write_byte(dev, reg, data);
+}
+
+static int adt7x10_read_word(struct device *dev, u8 reg)
+{
+	struct adt7x10_data *d = dev_get_drvdata(dev);
+	return d->ops->read_word(dev, reg);
+}
+
+static int adt7x10_write_word(struct device *dev, u8 reg, u16 data)
+{
+	struct adt7x10_data *d = dev_get_drvdata(dev);
+	return d->ops->write_word(dev, reg, data);
+}
+
+static const u8 ADT7X10_REG_TEMP[4] = {
+	ADT7X10_TEMPERATURE,		/* input */
+	ADT7X10_T_ALARM_HIGH,		/* high */
+	ADT7X10_T_ALARM_LOW,		/* low */
+	ADT7X10_T_CRIT,			/* critical */
+};
+
+static irqreturn_t adt7x10_irq_handler(int irq, void *private)
+{
+	struct device *dev = private;
+	int status;
+
+	status = adt7x10_read_byte(dev, ADT7X10_STATUS);
+	if (status < 0)
+		return IRQ_HANDLED;
+
+	if (status & ADT7X10_STAT_T_HIGH)
+		sysfs_notify(&dev->kobj, NULL, "temp1_max_alarm");
+	if (status & ADT7X10_STAT_T_LOW)
+		sysfs_notify(&dev->kobj, NULL, "temp1_min_alarm");
+	if (status & ADT7X10_STAT_T_CRIT)
+		sysfs_notify(&dev->kobj, NULL, "temp1_crit_alarm");
+
+	return IRQ_HANDLED;
+}
+
+static int adt7x10_temp_ready(struct device *dev)
+{
+	int i, status;
+
+	for (i = 0; i < 6; i++) {
+		status = adt7x10_read_byte(dev, ADT7X10_STATUS);
+		if (status < 0)
+			return status;
+		if (!(status & ADT7X10_STAT_NOT_RDY))
+			return 0;
+		msleep(60);
+	}
+	return -ETIMEDOUT;
+}
+
+static int adt7x10_update_temp(struct device *dev)
+{
+	struct adt7x10_data *data = dev_get_drvdata(dev);
+	int ret = 0;
+
+	mutex_lock(&data->update_lock);
+
+	if (time_after(jiffies, data->last_updated + HZ + HZ / 2)
+	    || !data->valid) {
+		int temp;
+
+		dev_dbg(dev, "Starting update\n");
+
+		ret = adt7x10_temp_ready(dev); /* check for new value */
+		if (ret)
+			goto abort;
+
+		temp = adt7x10_read_word(dev, ADT7X10_REG_TEMP[0]);
+		if (temp < 0) {
+			ret = temp;
+			dev_dbg(dev, "Failed to read value: reg %d, error %d\n",
+				ADT7X10_REG_TEMP[0], ret);
+			goto abort;
+		}
+		data->temp[0] = temp;
+		data->last_updated = jiffies;
+		data->valid = true;
+	}
+
+abort:
+	mutex_unlock(&data->update_lock);
+	return ret;
+}
+
+static int adt7x10_fill_cache(struct device *dev)
+{
+	struct adt7x10_data *data = dev_get_drvdata(dev);
+	int ret;
+	int i;
+
+	for (i = 1; i < ARRAY_SIZE(data->temp); i++) {
+		ret = adt7x10_read_word(dev, ADT7X10_REG_TEMP[i]);
+		if (ret < 0) {
+			dev_dbg(dev, "Failed to read value: reg %d, error %d\n",
+				ADT7X10_REG_TEMP[i], ret);
+			return ret;
+		}
+		data->temp[i] = ret;
+	}
+
+	ret = adt7x10_read_byte(dev, ADT7X10_T_HYST);
+	if (ret < 0) {
+		dev_dbg(dev, "Failed to read value: reg %d, error %d\n",
+				ADT7X10_T_HYST, ret);
+		return ret;
+	}
+	data->hyst = ret;
+
+	return 0;
+}
+
+static s16 ADT7X10_TEMP_TO_REG(long temp)
+{
+	return DIV_ROUND_CLOSEST(clamp_val(temp, ADT7X10_TEMP_MIN,
+					       ADT7X10_TEMP_MAX) * 128, 1000);
+}
+
+static int ADT7X10_REG_TO_TEMP(struct adt7x10_data *data, s16 reg)
+{
+	/* in 13 bit mode, bits 0-2 are status flags - mask them out */
+	if (!(data->config & ADT7X10_RESOLUTION))
+		reg &= ADT7X10_T13_VALUE_MASK;
+	/*
+	 * temperature is stored in twos complement format, in steps of
+	 * 1/128°C
+	 */
+	return DIV_ROUND_CLOSEST(reg * 1000, 128);
+}
+
+/*-----------------------------------------------------------------------*/
+
+/* sysfs attributes for hwmon */
+
+static ssize_t adt7x10_show_temp(struct device *dev,
+				 struct device_attribute *da,
+				 char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
+	struct adt7x10_data *data = dev_get_drvdata(dev);
+
+
+	if (attr->index == 0) {
+		int ret;
+
+		ret = adt7x10_update_temp(dev);
+		if (ret)
+			return ret;
+	}
+
+	return sprintf(buf, "%d\n", ADT7X10_REG_TO_TEMP(data,
+		       data->temp[attr->index]));
+}
+
+static ssize_t adt7x10_set_temp(struct device *dev,
+				struct device_attribute *da,
+				const char *buf, size_t count)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
+	struct adt7x10_data *data = dev_get_drvdata(dev);
+	int nr = attr->index;
+	long temp;
+	int ret;
+
+	ret = kstrtol(buf, 10, &temp);
+	if (ret)
+		return ret;
+
+	mutex_lock(&data->update_lock);
+	data->temp[nr] = ADT7X10_TEMP_TO_REG(temp);
+	ret = adt7x10_write_word(dev, ADT7X10_REG_TEMP[nr], data->temp[nr]);
+	if (ret)
+		count = ret;
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+static ssize_t adt7x10_show_t_hyst(struct device *dev,
+				   struct device_attribute *da,
+				   char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
+	struct adt7x10_data *data = dev_get_drvdata(dev);
+	int nr = attr->index;
+	int hyst;
+
+	hyst = (data->hyst & ADT7X10_T_HYST_MASK) * 1000;
+
+	/*
+	 * hysteresis is stored as a 4 bit offset in the device, convert it
+	 * to an absolute value
+	 */
+	if (nr == 2)	/* min has positive offset, others have negative */
+		hyst = -hyst;
+	return sprintf(buf, "%d\n",
+		       ADT7X10_REG_TO_TEMP(data, data->temp[nr]) - hyst);
+}
+
+static ssize_t adt7x10_set_t_hyst(struct device *dev,
+				  struct device_attribute *da,
+				  const char *buf, size_t count)
+{
+	struct adt7x10_data *data = dev_get_drvdata(dev);
+	int limit, ret;
+	long hyst;
+
+	ret = kstrtol(buf, 10, &hyst);
+	if (ret)
+		return ret;
+	/* convert absolute hysteresis value to a 4 bit delta value */
+	limit = ADT7X10_REG_TO_TEMP(data, data->temp[1]);
+	hyst = clamp_val(hyst, ADT7X10_TEMP_MIN, ADT7X10_TEMP_MAX);
+	data->hyst = clamp_val(DIV_ROUND_CLOSEST(limit - hyst, 1000),
+				   0, ADT7X10_T_HYST_MASK);
+	ret = adt7x10_write_byte(dev, ADT7X10_T_HYST, data->hyst);
+	if (ret)
+		return ret;
+
+	return count;
+}
+
+static ssize_t adt7x10_show_alarm(struct device *dev,
+				  struct device_attribute *da,
+				  char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
+	int ret;
+
+	ret = adt7x10_read_byte(dev, ADT7X10_STATUS);
+	if (ret < 0)
+		return ret;
+
+	return sprintf(buf, "%d\n", !!(ret & attr->index));
+}
+
+static ssize_t adt7x10_show_name(struct device *dev,
+				 struct device_attribute *da,
+				 char *buf)
+{
+	struct adt7x10_data *data = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%s\n", data->name);
+}
+
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, adt7x10_show_temp, NULL, 0);
+static SENSOR_DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO,
+			  adt7x10_show_temp, adt7x10_set_temp, 1);
+static SENSOR_DEVICE_ATTR(temp1_min, S_IWUSR | S_IRUGO,
+			  adt7x10_show_temp, adt7x10_set_temp, 2);
+static SENSOR_DEVICE_ATTR(temp1_crit, S_IWUSR | S_IRUGO,
+			  adt7x10_show_temp, adt7x10_set_temp, 3);
+static SENSOR_DEVICE_ATTR(temp1_max_hyst, S_IWUSR | S_IRUGO,
+			  adt7x10_show_t_hyst, adt7x10_set_t_hyst, 1);
+static SENSOR_DEVICE_ATTR(temp1_min_hyst, S_IRUGO,
+			  adt7x10_show_t_hyst, NULL, 2);
+static SENSOR_DEVICE_ATTR(temp1_crit_hyst, S_IRUGO,
+			  adt7x10_show_t_hyst, NULL, 3);
+static SENSOR_DEVICE_ATTR(temp1_min_alarm, S_IRUGO, adt7x10_show_alarm,
+			  NULL, ADT7X10_STAT_T_LOW);
+static SENSOR_DEVICE_ATTR(temp1_max_alarm, S_IRUGO, adt7x10_show_alarm,
+			  NULL, ADT7X10_STAT_T_HIGH);
+static SENSOR_DEVICE_ATTR(temp1_crit_alarm, S_IRUGO, adt7x10_show_alarm,
+			  NULL, ADT7X10_STAT_T_CRIT);
+static DEVICE_ATTR(name, S_IRUGO, adt7x10_show_name, NULL);
+
+static struct attribute *adt7x10_attributes[] = {
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	&sensor_dev_attr_temp1_max.dev_attr.attr,
+	&sensor_dev_attr_temp1_min.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp1_min_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp1_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit_alarm.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group adt7x10_group = {
+	.attrs = adt7x10_attributes,
+};
+
+int adt7x10_probe(struct device *dev, const char *name, int irq,
+		  const struct adt7x10_ops *ops)
+{
+	struct adt7x10_data *data;
+	int ret;
+
+	data = devm_kzalloc(dev, sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	data->ops = ops;
+	data->name = name;
+
+	dev_set_drvdata(dev, data);
+	mutex_init(&data->update_lock);
+
+	/* configure as specified */
+	ret = adt7x10_read_byte(dev, ADT7X10_CONFIG);
+	if (ret < 0) {
+		dev_dbg(dev, "Can't read config? %d\n", ret);
+		return ret;
+	}
+	data->oldconfig = ret;
+
+	/*
+	 * Set to 16 bit resolution, continous conversion and comparator mode.
+	 */
+	data->config = data->oldconfig;
+	data->config &= ~(ADT7X10_MODE_MASK | ADT7X10_CT_POLARITY |
+			ADT7X10_INT_POLARITY);
+	data->config |= ADT7X10_FULL | ADT7X10_RESOLUTION | ADT7X10_EVENT_MODE;
+
+	if (data->config != data->oldconfig) {
+		ret = adt7x10_write_byte(dev, ADT7X10_CONFIG, data->config);
+		if (ret)
+			return ret;
+	}
+	dev_dbg(dev, "Config %02x\n", data->config);
+
+	ret = adt7x10_fill_cache(dev);
+	if (ret)
+		goto exit_restore;
+
+	/* Register sysfs hooks */
+	ret = sysfs_create_group(&dev->kobj, &adt7x10_group);
+	if (ret)
+		goto exit_restore;
+
+	/*
+	 * The I2C device will already have it's own 'name' attribute, but for
+	 * the SPI device we need to register it. name will only be non NULL if
+	 * the device doesn't register the 'name' attribute on its own.
+	 */
+	if (name) {
+		ret = device_create_file(dev, &dev_attr_name);
+		if (ret)
+			goto exit_remove;
+	}
+
+	data->hwmon_dev = hwmon_device_register(dev);
+	if (IS_ERR(data->hwmon_dev)) {
+		ret = PTR_ERR(data->hwmon_dev);
+		goto exit_remove_name;
+	}
+
+	if (irq > 0) {
+		ret = request_threaded_irq(irq, NULL, adt7x10_irq_handler,
+				IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+				dev_name(dev), dev);
+		if (ret)
+			goto exit_hwmon_device_unregister;
+	}
+
+	return 0;
+
+exit_hwmon_device_unregister:
+	hwmon_device_unregister(data->hwmon_dev);
+exit_remove_name:
+	if (name)
+		device_remove_file(dev, &dev_attr_name);
+exit_remove:
+	sysfs_remove_group(&dev->kobj, &adt7x10_group);
+exit_restore:
+	adt7x10_write_byte(dev, ADT7X10_CONFIG, data->oldconfig);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(adt7x10_probe);
+
+int adt7x10_remove(struct device *dev, int irq)
+{
+	struct adt7x10_data *data = dev_get_drvdata(dev);
+
+	if (irq > 0)
+		free_irq(irq, dev);
+
+	hwmon_device_unregister(data->hwmon_dev);
+	if (data->name)
+		device_remove_file(dev, &dev_attr_name);
+	sysfs_remove_group(&dev->kobj, &adt7x10_group);
+	if (data->oldconfig != data->config)
+		adt7x10_write_byte(dev, ADT7X10_CONFIG, data->oldconfig);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(adt7x10_remove);
+
+#ifdef CONFIG_PM_SLEEP
+
+static int adt7x10_suspend(struct device *dev)
+{
+	struct adt7x10_data *data = dev_get_drvdata(dev);
+
+	return adt7x10_write_byte(dev, ADT7X10_CONFIG,
+		data->config | ADT7X10_PD);
+}
+
+static int adt7x10_resume(struct device *dev)
+{
+	struct adt7x10_data *data = dev_get_drvdata(dev);
+
+	return adt7x10_write_byte(dev, ADT7X10_CONFIG, data->config);
+}
+
+SIMPLE_DEV_PM_OPS(adt7x10_dev_pm_ops, adt7x10_suspend, adt7x10_resume);
+EXPORT_SYMBOL_GPL(adt7x10_dev_pm_ops);
+
+#endif /* CONFIG_PM_SLEEP */
+
+MODULE_AUTHOR("Hartmut Knaack");
+MODULE_DESCRIPTION("ADT7410/ADT7420, ADT7310/ADT7320 common code");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/adt7x10.h b/drivers/hwmon/adt7x10.h
new file mode 100644
index 00000000000..d491c698529
--- /dev/null
+++ b/drivers/hwmon/adt7x10.h
@@ -0,0 +1,37 @@
+#ifndef __HWMON_ADT7X10_H__
+#define __HWMON_ADT7X10_H__
+
+#include <linux/types.h>
+#include <linux/pm.h>
+
+/* ADT7410 registers definition */
+#define ADT7X10_TEMPERATURE		0
+#define ADT7X10_STATUS			2
+#define ADT7X10_CONFIG			3
+#define ADT7X10_T_ALARM_HIGH		4
+#define ADT7X10_T_ALARM_LOW		6
+#define ADT7X10_T_CRIT			8
+#define ADT7X10_T_HYST			0xA
+#define ADT7X10_ID			0xB
+
+struct device;
+
+struct adt7x10_ops {
+	int (*read_byte)(struct device *, u8 reg);
+	int (*write_byte)(struct device *, u8 reg, u8 data);
+	int (*read_word)(struct device *, u8 reg);
+	int (*write_word)(struct device *, u8 reg, u16 data);
+};
+
+int adt7x10_probe(struct device *dev, const char *name, int irq,
+	const struct adt7x10_ops *ops);
+int adt7x10_remove(struct device *dev, int irq);
+
+#ifdef CONFIG_PM_SLEEP
+extern const struct dev_pm_ops adt7x10_dev_pm_ops;
+#define ADT7X10_DEV_PM_OPS (&adt7x10_dev_pm_ops)
+#else
+#define ADT7X10_DEV_PM_OPS NULL
+#endif
+
+#endif
diff --git a/drivers/hwmon/applesmc.c b/drivers/hwmon/applesmc.c
index b41baffa20f..62c2e32e25e 100644
--- a/drivers/hwmon/applesmc.c
+++ b/drivers/hwmon/applesmc.c
@@ -922,7 +922,7 @@ static void applesmc_brightness_set(struct led_classdev *led_cdev,
 	ret = queue_work(applesmc_led_wq, &backlight_work);
 
 	if (debug && (!ret))
-		printk(KERN_DEBUG "applesmc: work was already on the queue.\n");
+		dev_dbg(led_cdev->dev, "work was already on the queue.\n");
 }
 
 static ssize_t applesmc_key_count_show(struct device *dev,
diff --git a/drivers/hwmon/asb100.c b/drivers/hwmon/asb100.c
index 6ac612cabda..f96063680e5 100644
--- a/drivers/hwmon/asb100.c
+++ b/drivers/hwmon/asb100.c
@@ -55,8 +55,8 @@ static const unsigned short normal_i2c[] = { 0x2d, I2C_CLIENT_END };
 
 static unsigned short force_subclients[4];
 module_param_array(force_subclients, short, NULL, 0);
-MODULE_PARM_DESC(force_subclients, "List of subclient addresses: "
-	"{bus, clientaddr, subclientaddr1, subclientaddr2}");
+MODULE_PARM_DESC(force_subclients,
+	"List of subclient addresses: {bus, clientaddr, subclientaddr1, subclientaddr2}");
 
 /* Voltage IN registers 0-6 */
 #define ASB100_REG_IN(nr)	(0x20 + (nr))
@@ -689,8 +689,8 @@ static int asb100_detect_subclients(struct i2c_client *client)
 		for (i = 2; i <= 3; i++) {
 			if (force_subclients[i] < 0x48 ||
 			    force_subclients[i] > 0x4f) {
-				dev_err(&client->dev, "invalid subclient "
-					"address %d; must be 0x48-0x4f\n",
+				dev_err(&client->dev,
+					"invalid subclient address %d; must be 0x48-0x4f\n",
 					force_subclients[i]);
 				err = -ENODEV;
 				goto ERROR_SC_2;
@@ -708,24 +708,27 @@ static int asb100_detect_subclients(struct i2c_client *client)
 	}
 
 	if (sc_addr[0] == sc_addr[1]) {
-		dev_err(&client->dev, "duplicate addresses 0x%x "
-				"for subclients\n", sc_addr[0]);
+		dev_err(&client->dev,
+			"duplicate addresses 0x%x for subclients\n",
+			sc_addr[0]);
 		err = -ENODEV;
 		goto ERROR_SC_2;
 	}
 
 	data->lm75[0] = i2c_new_dummy(adapter, sc_addr[0]);
 	if (!data->lm75[0]) {
-		dev_err(&client->dev, "subclient %d registration "
-			"at address 0x%x failed.\n", 1, sc_addr[0]);
+		dev_err(&client->dev,
+			"subclient %d registration at address 0x%x failed.\n",
+			1, sc_addr[0]);
 		err = -ENOMEM;
 		goto ERROR_SC_2;
 	}
 
 	data->lm75[1] = i2c_new_dummy(adapter, sc_addr[1]);
 	if (!data->lm75[1]) {
-		dev_err(&client->dev, "subclient %d registration "
-			"at address 0x%x failed.\n", 2, sc_addr[1]);
+		dev_err(&client->dev,
+			"subclient %d registration at address 0x%x failed.\n",
+			2, sc_addr[1]);
 		err = -ENOMEM;
 		goto ERROR_SC_3;
 	}
diff --git a/drivers/hwmon/asc7621.c b/drivers/hwmon/asc7621.c
index da7f5b5d5db..3ad9d849add 100644
--- a/drivers/hwmon/asc7621.c
+++ b/drivers/hwmon/asc7621.c
@@ -159,12 +159,12 @@ static inline int write_byte(struct i2c_client *client, u8 reg, u8 data)
  * and retrieval of like parameters.
  */
 
-#define SETUP_SHOW_data_param(d, a) \
+#define SETUP_SHOW_DATA_PARAM(d, a) \
 	struct sensor_device_attribute *sda = to_sensor_dev_attr(a); \
 	struct asc7621_data *data = asc7621_update_device(d); \
 	struct asc7621_param *param = to_asc7621_param(sda)
 
-#define SETUP_STORE_data_param(d, a) \
+#define SETUP_STORE_DATA_PARAM(d, a) \
 	struct sensor_device_attribute *sda = to_sensor_dev_attr(a); \
 	struct i2c_client *client = to_i2c_client(d); \
 	struct asc7621_data *data = i2c_get_clientdata(client); \
@@ -177,7 +177,7 @@ static inline int write_byte(struct i2c_client *client, u8 reg, u8 data)
 static ssize_t show_u8(struct device *dev, struct device_attribute *attr,
 		       char *buf)
 {
-	SETUP_SHOW_data_param(dev, attr);
+	SETUP_SHOW_DATA_PARAM(dev, attr);
 
 	return sprintf(buf, "%u\n", data->reg[param->msb[0]]);
 }
@@ -185,7 +185,7 @@ static ssize_t show_u8(struct device *dev, struct device_attribute *attr,
 static ssize_t store_u8(struct device *dev, struct device_attribute *attr,
 			const char *buf, size_t count)
 {
-	SETUP_STORE_data_param(dev, attr);
+	SETUP_STORE_DATA_PARAM(dev, attr);
 	long reqval;
 
 	if (kstrtol(buf, 10, &reqval))
@@ -206,7 +206,7 @@ static ssize_t store_u8(struct device *dev, struct device_attribute *attr,
 static ssize_t show_bitmask(struct device *dev,
 			    struct device_attribute *attr, char *buf)
 {
-	SETUP_SHOW_data_param(dev, attr);
+	SETUP_SHOW_DATA_PARAM(dev, attr);
 
 	return sprintf(buf, "%u\n",
 		       (data->reg[param->msb[0]] >> param->
@@ -217,7 +217,7 @@ static ssize_t store_bitmask(struct device *dev,
 			     struct device_attribute *attr,
 			     const char *buf, size_t count)
 {
-	SETUP_STORE_data_param(dev, attr);
+	SETUP_STORE_DATA_PARAM(dev, attr);
 	long reqval;
 	u8 currval;
 
@@ -246,7 +246,7 @@ static ssize_t store_bitmask(struct device *dev,
 static ssize_t show_fan16(struct device *dev,
 			  struct device_attribute *attr, char *buf)
 {
-	SETUP_SHOW_data_param(dev, attr);
+	SETUP_SHOW_DATA_PARAM(dev, attr);
 	u16 regval;
 
 	mutex_lock(&data->update_lock);
@@ -262,7 +262,7 @@ static ssize_t store_fan16(struct device *dev,
 			   struct device_attribute *attr, const char *buf,
 			   size_t count)
 {
-	SETUP_STORE_data_param(dev, attr);
+	SETUP_STORE_DATA_PARAM(dev, attr);
 	long reqval;
 
 	if (kstrtol(buf, 10, &reqval))
@@ -307,7 +307,7 @@ static int asc7621_in_scaling[] = {
 static ssize_t show_in10(struct device *dev, struct device_attribute *attr,
 			 char *buf)
 {
-	SETUP_SHOW_data_param(dev, attr);
+	SETUP_SHOW_DATA_PARAM(dev, attr);
 	u16 regval;
 	u8 nr = sda->index;
 
@@ -325,7 +325,7 @@ static ssize_t show_in10(struct device *dev, struct device_attribute *attr,
 static ssize_t show_in8(struct device *dev, struct device_attribute *attr,
 			char *buf)
 {
-	SETUP_SHOW_data_param(dev, attr);
+	SETUP_SHOW_DATA_PARAM(dev, attr);
 	u8 nr = sda->index;
 
 	return sprintf(buf, "%u\n",
@@ -336,7 +336,7 @@ static ssize_t show_in8(struct device *dev, struct device_attribute *attr,
 static ssize_t store_in8(struct device *dev, struct device_attribute *attr,
 			 const char *buf, size_t count)
 {
-	SETUP_STORE_data_param(dev, attr);
+	SETUP_STORE_DATA_PARAM(dev, attr);
 	long reqval;
 	u8 nr = sda->index;
 
@@ -360,7 +360,7 @@ static ssize_t store_in8(struct device *dev, struct device_attribute *attr,
 static ssize_t show_temp8(struct device *dev,
 			  struct device_attribute *attr, char *buf)
 {
-	SETUP_SHOW_data_param(dev, attr);
+	SETUP_SHOW_DATA_PARAM(dev, attr);
 
 	return sprintf(buf, "%d\n", ((s8) data->reg[param->msb[0]]) * 1000);
 }
@@ -369,7 +369,7 @@ static ssize_t store_temp8(struct device *dev,
 			   struct device_attribute *attr, const char *buf,
 			   size_t count)
 {
-	SETUP_STORE_data_param(dev, attr);
+	SETUP_STORE_DATA_PARAM(dev, attr);
 	long reqval;
 	s8 temp;
 
@@ -397,7 +397,7 @@ static ssize_t store_temp8(struct device *dev,
 static ssize_t show_temp10(struct device *dev,
 			   struct device_attribute *attr, char *buf)
 {
-	SETUP_SHOW_data_param(dev, attr);
+	SETUP_SHOW_DATA_PARAM(dev, attr);
 	u8 msb, lsb;
 	int temp;
 
@@ -414,7 +414,7 @@ static ssize_t show_temp10(struct device *dev,
 static ssize_t show_temp62(struct device *dev,
 			   struct device_attribute *attr, char *buf)
 {
-	SETUP_SHOW_data_param(dev, attr);
+	SETUP_SHOW_DATA_PARAM(dev, attr);
 	u8 regval = data->reg[param->msb[0]];
 	int temp = ((s8) (regval & 0xfc) * 1000) + ((regval & 0x03) * 250);
 
@@ -425,7 +425,7 @@ static ssize_t store_temp62(struct device *dev,
 			    struct device_attribute *attr, const char *buf,
 			    size_t count)
 {
-	SETUP_STORE_data_param(dev, attr);
+	SETUP_STORE_DATA_PARAM(dev, attr);
 	long reqval, i, f;
 	s8 temp;
 
@@ -459,7 +459,7 @@ static u32 asc7621_range_map[] = {
 static ssize_t show_ap2_temp(struct device *dev,
 			     struct device_attribute *attr, char *buf)
 {
-	SETUP_SHOW_data_param(dev, attr);
+	SETUP_SHOW_DATA_PARAM(dev, attr);
 	long auto_point1;
 	u8 regval;
 	int temp;
@@ -479,7 +479,7 @@ static ssize_t store_ap2_temp(struct device *dev,
 			      struct device_attribute *attr,
 			      const char *buf, size_t count)
 {
-	SETUP_STORE_data_param(dev, attr);
+	SETUP_STORE_DATA_PARAM(dev, attr);
 	long reqval, auto_point1;
 	int i;
 	u8 currval, newval = 0;
@@ -510,7 +510,7 @@ static ssize_t store_ap2_temp(struct device *dev,
 static ssize_t show_pwm_ac(struct device *dev,
 			   struct device_attribute *attr, char *buf)
 {
-	SETUP_SHOW_data_param(dev, attr);
+	SETUP_SHOW_DATA_PARAM(dev, attr);
 	u8 config, altbit, regval;
 	u8 map[] = {
 		0x01, 0x02, 0x04, 0x1f, 0x00, 0x06, 0x07, 0x10,
@@ -530,7 +530,7 @@ static ssize_t store_pwm_ac(struct device *dev,
 			    struct device_attribute *attr,
 			    const char *buf, size_t count)
 {
-	SETUP_STORE_data_param(dev, attr);
+	SETUP_STORE_DATA_PARAM(dev, attr);
 	unsigned long reqval;
 	u8 currval, config, altbit, newval;
 	u16 map[] = {
@@ -569,7 +569,7 @@ static ssize_t store_pwm_ac(struct device *dev,
 static ssize_t show_pwm_enable(struct device *dev,
 			       struct device_attribute *attr, char *buf)
 {
-	SETUP_SHOW_data_param(dev, attr);
+	SETUP_SHOW_DATA_PARAM(dev, attr);
 	u8 config, altbit, minoff, val, newval;
 
 	mutex_lock(&data->update_lock);
@@ -599,7 +599,7 @@ static ssize_t store_pwm_enable(struct device *dev,
 				struct device_attribute *attr,
 				const char *buf, size_t count)
 {
-	SETUP_STORE_data_param(dev, attr);
+	SETUP_STORE_DATA_PARAM(dev, attr);
 	long reqval;
 	u8 currval, config, altbit, newval, minoff = 255;
 
@@ -659,7 +659,7 @@ static u32 asc7621_pwm_freq_map[] = {
 static ssize_t show_pwm_freq(struct device *dev,
 			     struct device_attribute *attr, char *buf)
 {
-	SETUP_SHOW_data_param(dev, attr);
+	SETUP_SHOW_DATA_PARAM(dev, attr);
 	u8 regval =
 	    (data->reg[param->msb[0]] >> param->shift[0]) & param->mask[0];
 
@@ -672,7 +672,7 @@ static ssize_t store_pwm_freq(struct device *dev,
 			      struct device_attribute *attr,
 			      const char *buf, size_t count)
 {
-	SETUP_STORE_data_param(dev, attr);
+	SETUP_STORE_DATA_PARAM(dev, attr);
 	unsigned long reqval;
 	u8 currval, newval = 255;
 	int i;
@@ -707,7 +707,7 @@ static u32 asc7621_pwm_auto_spinup_map[] =  {
 static ssize_t show_pwm_ast(struct device *dev,
 			    struct device_attribute *attr, char *buf)
 {
-	SETUP_SHOW_data_param(dev, attr);
+	SETUP_SHOW_DATA_PARAM(dev, attr);
 	u8 regval =
 	    (data->reg[param->msb[0]] >> param->shift[0]) & param->mask[0];
 
@@ -721,7 +721,7 @@ static ssize_t store_pwm_ast(struct device *dev,
 			     struct device_attribute *attr,
 			     const char *buf, size_t count)
 {
-	SETUP_STORE_data_param(dev, attr);
+	SETUP_STORE_DATA_PARAM(dev, attr);
 	long reqval;
 	u8 currval, newval = 255;
 	u32 i;
@@ -756,7 +756,7 @@ static u32 asc7621_temp_smoothing_time_map[] = {
 static ssize_t show_temp_st(struct device *dev,
 			    struct device_attribute *attr, char *buf)
 {
-	SETUP_SHOW_data_param(dev, attr);
+	SETUP_SHOW_DATA_PARAM(dev, attr);
 	u8 regval =
 	    (data->reg[param->msb[0]] >> param->shift[0]) & param->mask[0];
 	regval = clamp_val(regval, 0, 7);
@@ -768,7 +768,7 @@ static ssize_t store_temp_st(struct device *dev,
 			     struct device_attribute *attr,
 			     const char *buf, size_t count)
 {
-	SETUP_STORE_data_param(dev, attr);
+	SETUP_STORE_DATA_PARAM(dev, attr);
 	long reqval;
 	u8 currval, newval = 255;
 	u32 i;
diff --git a/drivers/hwmon/coretemp.c b/drivers/hwmon/coretemp.c
index 3f1e297663a..658ce3a8717 100644
--- a/drivers/hwmon/coretemp.c
+++ b/drivers/hwmon/coretemp.c
@@ -411,8 +411,7 @@ static int __cpuinit chk_ucode_version(unsigned int cpu)
 	 * fixed for stepping D0 (6EC).
 	 */
 	if (c->x86_model == 0xe && c->x86_mask < 0xc && c->microcode < 0x39) {
-		pr_err("Errata AE18 not fixed, update BIOS or "
-		       "microcode of the CPU!\n");
+		pr_err("Errata AE18 not fixed, update BIOS or microcode of the CPU!\n");
 		return -ENODEV;
 	}
 	return 0;
diff --git a/drivers/hwmon/da9052-hwmon.c b/drivers/hwmon/da9052-hwmon.c
index ab4452c5a98..960fac3fb16 100644
--- a/drivers/hwmon/da9052-hwmon.c
+++ b/drivers/hwmon/da9052-hwmon.c
@@ -43,19 +43,19 @@ static const char * const input_names[] = {
 };
 
 /* Conversion function for VDDOUT and VBAT */
-static inline int volt_reg_to_mV(int value)
+static inline int volt_reg_to_mv(int value)
 {
 	return DIV_ROUND_CLOSEST(value * 1000, 512) + 2500;
 }
 
 /* Conversion function for ADC channels 4, 5 and 6 */
-static inline int input_reg_to_mV(int value)
+static inline int input_reg_to_mv(int value)
 {
 	return DIV_ROUND_CLOSEST(value * 2500, 1023);
 }
 
 /* Conversion function for VBBAT */
-static inline int vbbat_reg_to_mV(int value)
+static inline int vbbat_reg_to_mv(int value)
 {
 	return DIV_ROUND_CLOSEST(value * 2500, 512);
 }
@@ -96,7 +96,7 @@ static ssize_t da9052_read_vddout(struct device *dev,
 		goto hwmon_err;
 
 	mutex_unlock(&hwmon->hwmon_lock);
-	return sprintf(buf, "%d\n", volt_reg_to_mV(vdd));
+	return sprintf(buf, "%d\n", volt_reg_to_mv(vdd));
 
 hwmon_err_release:
 	da9052_disable_vddout_channel(hwmon->da9052);
@@ -137,7 +137,7 @@ static ssize_t da9052_read_vbat(struct device *dev,
 	if (ret < 0)
 		return ret;
 
-	return sprintf(buf, "%d\n", volt_reg_to_mV(ret));
+	return sprintf(buf, "%d\n", volt_reg_to_mv(ret));
 }
 
 static ssize_t da9052_read_misc_channel(struct device *dev,
@@ -152,7 +152,7 @@ static ssize_t da9052_read_misc_channel(struct device *dev,
 	if (ret < 0)
 		return ret;
 
-	return sprintf(buf, "%d\n", input_reg_to_mV(ret));
+	return sprintf(buf, "%d\n", input_reg_to_mv(ret));
 }
 
 static ssize_t da9052_read_tjunc(struct device *dev,
@@ -187,7 +187,7 @@ static ssize_t da9052_read_vbbat(struct device *dev,
 	if (ret < 0)
 		return ret;
 
-	return sprintf(buf, "%d\n", vbbat_reg_to_mV(ret));
+	return sprintf(buf, "%d\n", vbbat_reg_to_mv(ret));
 }
 
 static ssize_t da9052_hwmon_show_name(struct device *dev,
diff --git a/drivers/hwmon/da9055-hwmon.c b/drivers/hwmon/da9055-hwmon.c
index 9465c050c32..029ecabc438 100644
--- a/drivers/hwmon/da9055-hwmon.c
+++ b/drivers/hwmon/da9055-hwmon.c
@@ -119,7 +119,7 @@ static irqreturn_t da9055_auxadc_irq(int irq, void *irq_data)
 }
 
 /* Conversion function for VSYS and ADCINx */
-static inline int volt_reg_to_mV(int value, int channel)
+static inline int volt_reg_to_mv(int value, int channel)
 {
 	if (channel == DA9055_ADC_VSYS)
 		return DIV_ROUND_CLOSEST(value * 1000, DA9055_VSYS_DIV) + 2500;
@@ -168,7 +168,7 @@ static ssize_t da9055_read_auto_ch(struct device *dev,
 
 	mutex_unlock(&hwmon->hwmon_lock);
 
-	return sprintf(buf, "%d\n", volt_reg_to_mV(adc, channel));
+	return sprintf(buf, "%d\n", volt_reg_to_mv(adc, channel));
 
 hwmon_err_release:
 	da9055_disable_auto_mode(hwmon->da9055, channel);
diff --git a/drivers/hwmon/dme1737.c b/drivers/hwmon/dme1737.c
index c347c94f2f7..4ae3fff13f4 100644
--- a/drivers/hwmon/dme1737.c
+++ b/drivers/hwmon/dme1737.c
@@ -55,14 +55,16 @@ MODULE_PARM_DESC(force_id, "Override the detected device ID");
 
 static bool probe_all_addr;
 module_param(probe_all_addr, bool, 0);
-MODULE_PARM_DESC(probe_all_addr, "Include probing of non-standard LPC "
-		 "addresses");
+MODULE_PARM_DESC(probe_all_addr,
+		 "Include probing of non-standard LPC addresses");
 
 /* Addresses to scan */
 static const unsigned short normal_i2c[] = {0x2c, 0x2d, 0x2e, I2C_CLIENT_END};
 
 enum chips { dme1737, sch5027, sch311x, sch5127 };
 
+#define	DO_REPORT "Please report to the driver maintainer."
+
 /* ---------------------------------------------------------------------
  * Registers
  *
@@ -566,9 +568,9 @@ static u8 dme1737_read(const struct dme1737_data *data, u8 reg)
 		val = i2c_smbus_read_byte_data(client, reg);
 
 		if (val < 0) {
-			dev_warn(&client->dev, "Read from register "
-				 "0x%02x failed! Please report to the driver "
-				 "maintainer.\n", reg);
+			dev_warn(&client->dev,
+				 "Read from register 0x%02x failed! %s\n",
+				 reg, DO_REPORT);
 		}
 	} else { /* ISA device */
 		outb(reg, data->addr);
@@ -587,9 +589,9 @@ static s32 dme1737_write(const struct dme1737_data *data, u8 reg, u8 val)
 		res = i2c_smbus_write_byte_data(client, reg, val);
 
 		if (res < 0) {
-			dev_warn(&client->dev, "Write to register "
-				 "0x%02x failed! Please report to the driver "
-				 "maintainer.\n", reg);
+			dev_warn(&client->dev,
+				 "Write to register 0x%02x failed! %s\n",
+				 reg, DO_REPORT);
 		}
 	} else { /* ISA device */
 		outb(reg, data->addr);
@@ -1167,8 +1169,8 @@ static ssize_t set_fan(struct device *dev, struct device_attribute *attr,
 		/* Only valid for fan[1-4] */
 		if (!(val == 1 || val == 2 || val == 4)) {
 			count = -EINVAL;
-			dev_warn(dev, "Fan type value %ld not "
-				 "supported. Choose one of 1, 2, or 4.\n",
+			dev_warn(dev,
+				 "Fan type value %ld not supported. Choose one of 1, 2, or 4.\n",
 				 val);
 			goto exit;
 		}
@@ -1294,8 +1296,8 @@ static ssize_t set_pwm(struct device *dev, struct device_attribute *attr,
 		/* Only valid for pwm[1-3] */
 		if (val < 0 || val > 2) {
 			count = -EINVAL;
-			dev_warn(dev, "PWM enable %ld not "
-				 "supported. Choose one of 0, 1, or 2.\n",
+			dev_warn(dev,
+				 "PWM enable %ld not supported. Choose one of 0, 1, or 2.\n",
 				 val);
 			goto exit;
 		}
@@ -1399,8 +1401,8 @@ static ssize_t set_pwm(struct device *dev, struct device_attribute *attr,
 		if (!(val == 1 || val == 2 || val == 4 ||
 		      val == 6 || val == 7)) {
 			count = -EINVAL;
-			dev_warn(dev, "PWM auto channels zone %ld "
-				 "not supported. Choose one of 1, 2, 4, 6, "
+			dev_warn(dev,
+				 "PWM auto channels zone %ld not supported. Choose one of 1, 2, 4, 6, "
 				 "or 7.\n", val);
 			goto exit;
 		}
@@ -2178,8 +2180,8 @@ static int dme1737_create_files(struct device *dev)
 	 * selected attributes from read-only to read-writeable.
 	 */
 	if (data->config & 0x02) {
-		dev_info(dev, "Device is locked. Some attributes "
-			 "will be read-only.\n");
+		dev_info(dev,
+			 "Device is locked. Some attributes will be read-only.\n");
 	} else {
 		/* Change permissions of zone sysfs attributes */
 		dme1737_chmod_group(dev, &dme1737_zone_chmod_group,
@@ -2247,9 +2249,8 @@ static int dme1737_init_device(struct device *dev)
 	/* Inform if part is not monitoring/started */
 	if (!(data->config & 0x01)) {
 		if (!force_start) {
-			dev_err(dev, "Device is not monitoring. "
-				"Use the force_start load parameter to "
-				"override.\n");
+			dev_err(dev,
+				"Device is not monitoring. Use the force_start load parameter to override.\n");
 			return -EFAULT;
 		}
 
@@ -2289,8 +2290,8 @@ static int dme1737_init_device(struct device *dev)
 		 */
 		if (dme1737_i2c_get_features(0x2e, data) &&
 		    dme1737_i2c_get_features(0x4e, data)) {
-			dev_warn(dev, "Failed to query Super-IO for optional "
-				 "features.\n");
+			dev_warn(dev,
+				 "Failed to query Super-IO for optional features.\n");
 		}
 	}
 
@@ -2317,8 +2318,8 @@ static int dme1737_init_device(struct device *dev)
 		break;
 	}
 
-	dev_info(dev, "Optional features: pwm3=%s, pwm5=%s, pwm6=%s, "
-		 "fan3=%s, fan4=%s, fan5=%s, fan6=%s.\n",
+	dev_info(dev,
+		 "Optional features: pwm3=%s, pwm5=%s, pwm6=%s, fan3=%s, fan4=%s, fan5=%s, fan6=%s.\n",
 		 (data->has_features & HAS_PWM(2)) ? "yes" : "no",
 		 (data->has_features & HAS_PWM(4)) ? "yes" : "no",
 		 (data->has_features & HAS_PWM(5)) ? "yes" : "no",
@@ -2330,18 +2331,16 @@ static int dme1737_init_device(struct device *dev)
 	reg = dme1737_read(data, DME1737_REG_TACH_PWM);
 	/* Inform if fan-to-pwm mapping differs from the default */
 	if (client && reg != 0xa4) {   /* I2C chip */
-		dev_warn(dev, "Non-standard fan to pwm mapping: "
-			 "fan1->pwm%d, fan2->pwm%d, fan3->pwm%d, "
-			 "fan4->pwm%d. Please report to the driver "
-			 "maintainer.\n",
+		dev_warn(dev,
+			 "Non-standard fan to pwm mapping: fan1->pwm%d, fan2->pwm%d, fan3->pwm%d, fan4->pwm%d. %s\n",
 			 (reg & 0x03) + 1, ((reg >> 2) & 0x03) + 1,
-			 ((reg >> 4) & 0x03) + 1, ((reg >> 6) & 0x03) + 1);
+			 ((reg >> 4) & 0x03) + 1, ((reg >> 6) & 0x03) + 1,
+			 DO_REPORT);
 	} else if (!client && reg != 0x24) {   /* ISA chip */
-		dev_warn(dev, "Non-standard fan to pwm mapping: "
-			 "fan1->pwm%d, fan2->pwm%d, fan3->pwm%d. "
-			 "Please report to the driver maintainer.\n",
+		dev_warn(dev,
+			 "Non-standard fan to pwm mapping: fan1->pwm%d, fan2->pwm%d, fan3->pwm%d. %s\n",
 			 (reg & 0x03) + 1, ((reg >> 2) & 0x03) + 1,
-			 ((reg >> 4) & 0x03) + 1);
+			 ((reg >> 4) & 0x03) + 1, DO_REPORT);
 	}
 
 	/*
@@ -2355,8 +2354,9 @@ static int dme1737_init_device(struct device *dev)
 						DME1737_REG_PWM_CONFIG(ix));
 			if ((data->has_features & HAS_PWM(ix)) &&
 			    (PWM_EN_FROM_REG(data->pwm_config[ix]) == -1)) {
-				dev_info(dev, "Switching pwm%d to "
-					 "manual mode.\n", ix + 1);
+				dev_info(dev,
+					 "Switching pwm%d to manual mode.\n",
+					 ix + 1);
 				data->pwm_config[ix] = PWM_EN_TO_REG(1,
 							data->pwm_config[ix]);
 				dme1737_write(data, DME1737_REG_PWM(ix), 0);
diff --git a/drivers/hwmon/f71805f.c b/drivers/hwmon/f71805f.c
index a9816979c5d..0c9f3da242b 100644
--- a/drivers/hwmon/f71805f.c
+++ b/drivers/hwmon/f71805f.c
@@ -1350,8 +1350,7 @@ static void f71805f_init_device(struct f71805f_data *data)
 
 	reg = f71805f_read8(data, F71805F_REG_START);
 	if ((reg & 0x41) != 0x01) {
-		printk(KERN_DEBUG DRVNAME ": Starting monitoring "
-		       "operations\n");
+		pr_debug("Starting monitoring operations\n");
 		f71805f_write8(data, F71805F_REG_START, (reg | 0x01) & ~0x40);
 	}
 
diff --git a/drivers/hwmon/fam15h_power.c b/drivers/hwmon/fam15h_power.c
index b757088aedd..dff841085ba 100644
--- a/drivers/hwmon/fam15h_power.c
+++ b/drivers/hwmon/fam15h_power.c
@@ -189,8 +189,8 @@ static void fam15h_power_init_data(struct pci_dev *f4,
 
 	/* result not allowed to be >= 256W */
 	if ((tmp >> 16) >= 256)
-		dev_warn(&f4->dev, "Bogus value for ProcessorPwrWatts "
-			 "(processor_pwr_watts>=%u)\n",
+		dev_warn(&f4->dev,
+			 "Bogus value for ProcessorPwrWatts (processor_pwr_watts>=%u)\n",
 			 (unsigned int) (tmp >> 16));
 
 	/* convert to microWatt */
diff --git a/drivers/hwmon/fschmd.c b/drivers/hwmon/fschmd.c
index 8af2755cdb8..d58abdc5a4c 100644
--- a/drivers/hwmon/fschmd.c
+++ b/drivers/hwmon/fschmd.c
@@ -463,8 +463,9 @@ static ssize_t store_fan_div(struct device *dev, struct device_attribute
 		v = 3;
 		break;
 	default:
-		dev_err(dev, "fan_div value %lu not supported. "
-			"Choose one of 2, 4 or 8!\n", v);
+		dev_err(dev,
+			"fan_div value %lu not supported. Choose one of 2, 4 or 8!\n",
+			v);
 		return -EINVAL;
 	}
 
@@ -1249,8 +1250,8 @@ static int fschmd_probe(struct i2c_client *client,
 	}
 	if (i == ARRAY_SIZE(watchdog_minors)) {
 		data->watchdog_miscdev.minor = 0;
-		dev_warn(&client->dev, "Couldn't register watchdog chardev "
-			"(due to no free minor)\n");
+		dev_warn(&client->dev,
+			 "Couldn't register watchdog chardev (due to no free minor)\n");
 	}
 	mutex_unlock(&watchdog_data_mutex);
 
diff --git a/drivers/hwmon/gl518sm.c b/drivers/hwmon/gl518sm.c
index e2e5909a34d..95257a5621d 100644
--- a/drivers/hwmon/gl518sm.c
+++ b/drivers/hwmon/gl518sm.c
@@ -344,8 +344,9 @@ static ssize_t set_fan_div(struct device *dev, struct device_attribute *attr,
 		val = 3;
 		break;
 	default:
-		dev_err(dev, "Invalid fan clock divider %lu, choose one "
-			"of 1, 2, 4 or 8\n", val);
+		dev_err(dev,
+			"Invalid fan clock divider %lu, choose one of 1, 2, 4 or 8\n",
+			val);
 		return -EINVAL;
 	}
 
diff --git a/drivers/hwmon/gpio-fan.c b/drivers/hwmon/gpio-fan.c
index 39781945a5d..3104149795c 100644
--- a/drivers/hwmon/gpio-fan.c
+++ b/drivers/hwmon/gpio-fan.c
@@ -105,10 +105,6 @@ static int fan_alarm_init(struct gpio_fan_data *fan_data,
 	if (err)
 		return err;
 
-	err = device_create_file(&pdev->dev, &dev_attr_fan1_alarm);
-	if (err)
-		return err;
-
 	/*
 	 * If the alarm GPIO don't support interrupts, just leave
 	 * without initializing the fail notification support.
@@ -121,23 +117,9 @@ static int fan_alarm_init(struct gpio_fan_data *fan_data,
 	irq_set_irq_type(alarm_irq, IRQ_TYPE_EDGE_BOTH);
 	err = devm_request_irq(&pdev->dev, alarm_irq, fan_alarm_irq_handler,
 			       IRQF_SHARED, "GPIO fan alarm", fan_data);
-	if (err)
-		goto err_free_sysfs;
-
-	return 0;
-
-err_free_sysfs:
-	device_remove_file(&pdev->dev, &dev_attr_fan1_alarm);
 	return err;
 }
 
-static void fan_alarm_free(struct gpio_fan_data *fan_data)
-{
-	struct platform_device *pdev = fan_data->pdev;
-
-	device_remove_file(&pdev->dev, &dev_attr_fan1_alarm);
-}
-
 /*
  * Control GPIOs.
  */
@@ -327,6 +309,12 @@ exit_unlock:
 	return ret;
 }
 
+static ssize_t show_name(struct device *dev,
+			 struct device_attribute *attr, char *buf)
+{
+	return sprintf(buf, "gpio-fan\n");
+}
+
 static DEVICE_ATTR(pwm1, S_IRUGO | S_IWUSR, show_pwm, set_pwm);
 static DEVICE_ATTR(pwm1_enable, S_IRUGO | S_IWUSR,
 		   show_pwm_enable, set_pwm_enable);
@@ -336,8 +324,26 @@ static DEVICE_ATTR(fan1_max, S_IRUGO, show_rpm_max, NULL);
 static DEVICE_ATTR(fan1_input, S_IRUGO, show_rpm, NULL);
 static DEVICE_ATTR(fan1_target, S_IRUGO | S_IWUSR, show_rpm, set_rpm);
 
-static struct attribute *gpio_fan_ctrl_attributes[] = {
-	&dev_attr_pwm1.attr,
+static DEVICE_ATTR(name, S_IRUGO, show_name, NULL);
+
+static umode_t gpio_fan_is_visible(struct kobject *kobj,
+				   struct attribute *attr, int index)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct gpio_fan_data *data = dev_get_drvdata(dev);
+
+	if (index == 1 && !data->alarm)
+		return 0;
+	if (index > 1 && !data->ctrl)
+		return 0;
+
+	return attr->mode;
+}
+
+static struct attribute *gpio_fan_attributes[] = {
+	&dev_attr_name.attr,
+	&dev_attr_fan1_alarm.attr,		/* 1 */
+	&dev_attr_pwm1.attr,			/* 2 */
 	&dev_attr_pwm1_enable.attr,
 	&dev_attr_pwm1_mode.attr,
 	&dev_attr_fan1_input.attr,
@@ -347,8 +353,9 @@ static struct attribute *gpio_fan_ctrl_attributes[] = {
 	NULL
 };
 
-static const struct attribute_group gpio_fan_ctrl_group = {
-	.attrs = gpio_fan_ctrl_attributes,
+static const struct attribute_group gpio_fan_group = {
+	.attrs = gpio_fan_attributes,
+	.is_visible = gpio_fan_is_visible,
 };
 
 static int fan_ctrl_init(struct gpio_fan_data *fan_data,
@@ -379,30 +386,9 @@ static int fan_ctrl_init(struct gpio_fan_data *fan_data,
 	if (fan_data->speed_index < 0)
 		return -ENODEV;
 
-	err = sysfs_create_group(&pdev->dev.kobj, &gpio_fan_ctrl_group);
-	return err;
-}
-
-static void fan_ctrl_free(struct gpio_fan_data *fan_data)
-{
-	struct platform_device *pdev = fan_data->pdev;
-
-	sysfs_remove_group(&pdev->dev.kobj, &gpio_fan_ctrl_group);
-}
-
-/*
- * Platform driver.
- */
-
-static ssize_t show_name(struct device *dev,
-			 struct device_attribute *attr, char *buf)
-{
-	return sprintf(buf, "gpio-fan\n");
+	return 0;
 }
 
-static DEVICE_ATTR(name, S_IRUGO, show_name, NULL);
-
-
 #ifdef CONFIG_OF_GPIO
 /*
  * Translate OpenFirmware node properties into platform_data
@@ -546,38 +532,30 @@ static int gpio_fan_probe(struct platform_device *pdev)
 
 	/* Configure control GPIOs if available. */
 	if (pdata->ctrl && pdata->num_ctrl > 0) {
-		if (!pdata->speed || pdata->num_speed <= 1) {
-			err = -EINVAL;
-			goto err_free_alarm;
-		}
+		if (!pdata->speed || pdata->num_speed <= 1)
+			return -EINVAL;
 		err = fan_ctrl_init(fan_data, pdata);
 		if (err)
-			goto err_free_alarm;
+			return err;
 	}
 
-	err = device_create_file(&pdev->dev, &dev_attr_name);
+	err = sysfs_create_group(&pdev->dev.kobj, &gpio_fan_group);
 	if (err)
-		goto err_free_ctrl;
+		return err;
 
 	/* Make this driver part of hwmon class. */
 	fan_data->hwmon_dev = hwmon_device_register(&pdev->dev);
 	if (IS_ERR(fan_data->hwmon_dev)) {
 		err = PTR_ERR(fan_data->hwmon_dev);
-		goto err_remove_name;
+		goto err_remove;
 	}
 
 	dev_info(&pdev->dev, "GPIO fan initialized\n");
 
 	return 0;
 
-err_remove_name:
-	device_remove_file(&pdev->dev, &dev_attr_name);
-err_free_ctrl:
-	if (fan_data->ctrl)
-		fan_ctrl_free(fan_data);
-err_free_alarm:
-	if (fan_data->alarm)
-		fan_alarm_free(fan_data);
+err_remove:
+	sysfs_remove_group(&pdev->dev.kobj, &gpio_fan_group);
 	return err;
 }
 
@@ -586,11 +564,7 @@ static int gpio_fan_remove(struct platform_device *pdev)
 	struct gpio_fan_data *fan_data = platform_get_drvdata(pdev);
 
 	hwmon_device_unregister(fan_data->hwmon_dev);
-	device_remove_file(&pdev->dev, &dev_attr_name);
-	if (fan_data->alarm)
-		fan_alarm_free(fan_data);
-	if (fan_data->ctrl)
-		fan_ctrl_free(fan_data);
+	sysfs_remove_group(&pdev->dev.kobj, &gpio_fan_group);
 
 	return 0;
 }
@@ -619,7 +593,7 @@ static int gpio_fan_resume(struct device *dev)
 }
 
 static SIMPLE_DEV_PM_OPS(gpio_fan_pm, gpio_fan_suspend, gpio_fan_resume);
-#define GPIO_FAN_PM	&gpio_fan_pm
+#define GPIO_FAN_PM	(&gpio_fan_pm)
 #else
 #define GPIO_FAN_PM	NULL
 #endif
diff --git a/drivers/hwmon/ibmaem.c b/drivers/hwmon/ibmaem.c
index a14f634248e..1429f6e177f 100644
--- a/drivers/hwmon/ibmaem.c
+++ b/drivers/hwmon/ibmaem.c
@@ -289,8 +289,9 @@ static int aem_init_ipmi_data(struct aem_ipmi_data *data, int iface,
 	err = ipmi_create_user(data->interface, &driver_data.ipmi_hndlrs,
 			       data, &data->user);
 	if (err < 0) {
-		dev_err(bmc, "Unable to register user with IPMI "
-			"interface %d\n", data->interface);
+		dev_err(bmc,
+			"Unable to register user with IPMI interface %d\n",
+			data->interface);
 		return -EACCES;
 	}
 
@@ -328,8 +329,8 @@ static void aem_msg_handler(struct ipmi_recv_msg *msg, void *user_msg_data)
 	struct aem_ipmi_data *data = user_msg_data;
 
 	if (msg->msgid != data->tx_msgid) {
-		dev_err(data->bmc_device, "Mismatch between received msgid "
-			"(%02x) and transmitted msgid (%02x)!\n",
+		dev_err(data->bmc_device,
+			"Mismatch between received msgid (%02x) and transmitted msgid (%02x)!\n",
 			(int)msg->msgid,
 			(int)data->tx_msgid);
 		ipmi_free_recv_msg(msg);
@@ -575,8 +576,8 @@ static int aem_init_aem1_inst(struct aem_ipmi_data *probe, u8 module_handle)
 	/* Register with hwmon */
 	data->hwmon_dev = hwmon_device_register(&data->pdev->dev);
 	if (IS_ERR(data->hwmon_dev)) {
-		dev_err(&data->pdev->dev, "Unable to register hwmon "
-			"device for IPMI interface %d\n",
+		dev_err(&data->pdev->dev,
+			"Unable to register hwmon device for IPMI interface %d\n",
 			probe->interface);
 		res = PTR_ERR(data->hwmon_dev);
 		goto hwmon_reg_err;
@@ -715,8 +716,8 @@ static int aem_init_aem2_inst(struct aem_ipmi_data *probe,
 	/* Register with hwmon */
 	data->hwmon_dev = hwmon_device_register(&data->pdev->dev);
 	if (IS_ERR(data->hwmon_dev)) {
-		dev_err(&data->pdev->dev, "Unable to register hwmon "
-			"device for IPMI interface %d\n",
+		dev_err(&data->pdev->dev,
+			"Unable to register hwmon device for IPMI interface %d\n",
 			probe->interface);
 		res = PTR_ERR(data->hwmon_dev);
 		goto hwmon_reg_err;
@@ -768,8 +769,8 @@ static void aem_init_aem2(struct aem_ipmi_data *probe)
 
 	while (!aem_find_aem2(probe, &fi_resp, i)) {
 		if (fi_resp.major != 2) {
-			dev_err(probe->bmc_device, "Unknown AEM v%d; please "
-				"report this to the maintainer.\n",
+			dev_err(probe->bmc_device,
+				"Unknown AEM v%d; please report this to the maintainer.\n",
 				fi_resp.major);
 			i++;
 			continue;
diff --git a/drivers/hwmon/ibmpex.c b/drivers/hwmon/ibmpex.c
index b622a93ec32..74b365ea01c 100644
--- a/drivers/hwmon/ibmpex.c
+++ b/drivers/hwmon/ibmpex.c
@@ -163,8 +163,8 @@ static int ibmpex_ver_check(struct ibmpex_bmc_data *data)
 	data->sensor_major = data->rx_msg_data[0];
 	data->sensor_minor = data->rx_msg_data[1];
 
-	dev_info(data->bmc_device, "Found BMC with sensor interface "
-		 "v%d.%d %d-%02d-%02d on interface %d\n",
+	dev_info(data->bmc_device,
+		 "Found BMC with sensor interface v%d.%d %d-%02d-%02d on interface %d\n",
 		 data->sensor_major,
 		 data->sensor_minor,
 		 extract_value(data->rx_msg_data, 2),
@@ -478,8 +478,9 @@ static void ibmpex_register_bmc(int iface, struct device *dev)
 	err = ipmi_create_user(data->interface, &driver_data.ipmi_hndlrs,
 			       data, &data->user);
 	if (err < 0) {
-		dev_err(dev, "Unable to register user with IPMI "
-			"interface %d\n", data->interface);
+		dev_err(dev,
+			"Unable to register user with IPMI interface %d\n",
+			data->interface);
 		goto out;
 	}
 
@@ -501,8 +502,8 @@ static void ibmpex_register_bmc(int iface, struct device *dev)
 	data->hwmon_dev = hwmon_device_register(data->bmc_device);
 
 	if (IS_ERR(data->hwmon_dev)) {
-		dev_err(data->bmc_device, "Unable to register hwmon "
-			"device for IPMI interface %d\n",
+		dev_err(data->bmc_device,
+			"Unable to register hwmon device for IPMI interface %d\n",
 			data->interface);
 		goto out_user;
 	}
@@ -567,8 +568,8 @@ static void ibmpex_msg_handler(struct ipmi_recv_msg *msg, void *user_msg_data)
 	struct ibmpex_bmc_data *data = (struct ibmpex_bmc_data *)user_msg_data;
 
 	if (msg->msgid != data->tx_msgid) {
-		dev_err(data->bmc_device, "Mismatch between received msgid "
-			"(%02x) and transmitted msgid (%02x)!\n",
+		dev_err(data->bmc_device,
+			"Mismatch between received msgid (%02x) and transmitted msgid (%02x)!\n",
 			(int)msg->msgid,
 			(int)data->tx_msgid);
 		ipmi_free_recv_msg(msg);
diff --git a/drivers/staging/iio/iio_hwmon.c b/drivers/hwmon/iio_hwmon.c
index 93af756ba48..aafa4531b96 100644
--- a/drivers/staging/iio/iio_hwmon.c
+++ b/drivers/hwmon/iio_hwmon.c
@@ -13,6 +13,7 @@
 #include <linux/err.h>
 #include <linux/platform_device.h>
 #include <linux/hwmon.h>
+#include <linux/of.h>
 #include <linux/hwmon-sysfs.h>
 #include <linux/iio/consumer.h>
 #include <linux/iio/types.h>
@@ -58,7 +59,12 @@ static ssize_t iio_hwmon_read_val(struct device *dev,
 static ssize_t show_name(struct device *dev, struct device_attribute *attr,
 			 char *buf)
 {
-	return sprintf(buf, "iio_hwmon\n");
+	const char *name = "iio_hwmon";
+
+	if (dev->of_node && dev->of_node->name)
+		name = dev->of_node->name;
+
+	return sprintf(buf, "%s\n", name);
 }
 
 static DEVICE_ATTR(name, S_IRUGO, show_name, NULL);
diff --git a/drivers/hwmon/ina2xx.c b/drivers/hwmon/ina2xx.c
index 8e7158c3ad2..4958b2f89dc 100644
--- a/drivers/hwmon/ina2xx.c
+++ b/drivers/hwmon/ina2xx.c
@@ -186,20 +186,20 @@ static ssize_t ina2xx_show_value(struct device *dev,
 }
 
 /* shunt voltage */
-static SENSOR_DEVICE_ATTR(in0_input, S_IRUGO, \
-	ina2xx_show_value, NULL, INA2XX_SHUNT_VOLTAGE);
+static SENSOR_DEVICE_ATTR(in0_input, S_IRUGO, ina2xx_show_value, NULL,
+			  INA2XX_SHUNT_VOLTAGE);
 
 /* bus voltage */
-static SENSOR_DEVICE_ATTR(in1_input, S_IRUGO, \
-	ina2xx_show_value, NULL, INA2XX_BUS_VOLTAGE);
+static SENSOR_DEVICE_ATTR(in1_input, S_IRUGO, ina2xx_show_value, NULL,
+			  INA2XX_BUS_VOLTAGE);
 
 /* calculated current */
-static SENSOR_DEVICE_ATTR(curr1_input, S_IRUGO, \
-	ina2xx_show_value, NULL, INA2XX_CURRENT);
+static SENSOR_DEVICE_ATTR(curr1_input, S_IRUGO, ina2xx_show_value, NULL,
+			  INA2XX_CURRENT);
 
 /* calculated power */
-static SENSOR_DEVICE_ATTR(power1_input, S_IRUGO, \
-	ina2xx_show_value, NULL, INA2XX_POWER);
+static SENSOR_DEVICE_ATTR(power1_input, S_IRUGO, ina2xx_show_value, NULL,
+			  INA2XX_POWER);
 
 /* pointers to created device attributes */
 static struct attribute *ina2xx_attributes[] = {
diff --git a/drivers/hwmon/it87.c b/drivers/hwmon/it87.c
index 37fc980fde2..72b21d5b1c6 100644
--- a/drivers/hwmon/it87.c
+++ b/drivers/hwmon/it87.c
@@ -1778,7 +1778,7 @@ static int __init it87_find(unsigned short *address,
 		superio_select(5);
 		sio_data->beep_pin = superio_inb(IT87_SIO_BEEP_PIN_REG) & 0x3f;
 	} else if (sio_data->type == it8783) {
-		int reg25, reg27, reg2A, reg2C, regEF;
+		int reg25, reg27, reg2a, reg2c, regef;
 
 		sio_data->skip_vid = 1;	/* No VID */
 
@@ -1786,15 +1786,15 @@ static int __init it87_find(unsigned short *address,
 
 		reg25 = superio_inb(IT87_SIO_GPIO1_REG);
 		reg27 = superio_inb(IT87_SIO_GPIO3_REG);
-		reg2A = superio_inb(IT87_SIO_PINX1_REG);
-		reg2C = superio_inb(IT87_SIO_PINX2_REG);
-		regEF = superio_inb(IT87_SIO_SPI_REG);
+		reg2a = superio_inb(IT87_SIO_PINX1_REG);
+		reg2c = superio_inb(IT87_SIO_PINX2_REG);
+		regef = superio_inb(IT87_SIO_SPI_REG);
 
 		/* Check if fan3 is there or not */
-		if ((reg27 & (1 << 0)) || !(reg2C & (1 << 2)))
+		if ((reg27 & (1 << 0)) || !(reg2c & (1 << 2)))
 			sio_data->skip_fan |= (1 << 2);
 		if ((reg25 & (1 << 4))
-		    || (!(reg2A & (1 << 1)) && (regEF & (1 << 0))))
+		    || (!(reg2a & (1 << 1)) && (regef & (1 << 0))))
 			sio_data->skip_pwm |= (1 << 2);
 
 		/* Check if fan2 is there or not */
@@ -1804,7 +1804,7 @@ static int __init it87_find(unsigned short *address,
 			sio_data->skip_pwm |= (1 << 1);
 
 		/* VIN5 */
-		if ((reg27 & (1 << 0)) || (reg2C & (1 << 2)))
+		if ((reg27 & (1 << 0)) || (reg2c & (1 << 2)))
 			sio_data->skip_in |= (1 << 5); /* No VIN5 */
 
 		/* VIN6 */
@@ -1829,18 +1829,18 @@ static int __init it87_find(unsigned short *address,
 			 * not the case, and ask the user to report if the
 			 * resulting voltage is sane.
 			 */
-			if (!(reg2C & (1 << 1))) {
-				reg2C |= (1 << 1);
-				superio_outb(IT87_SIO_PINX2_REG, reg2C);
+			if (!(reg2c & (1 << 1))) {
+				reg2c |= (1 << 1);
+				superio_outb(IT87_SIO_PINX2_REG, reg2c);
 				pr_notice("Routing internal VCCH5V to in7.\n");
 			}
 			pr_notice("in7 routed to internal voltage divider, with external pin disabled.\n");
 			pr_notice("Please report if it displays a reasonable voltage.\n");
 		}
 
-		if (reg2C & (1 << 0))
+		if (reg2c & (1 << 0))
 			sio_data->internal |= (1 << 0);
-		if (reg2C & (1 << 1))
+		if (reg2c & (1 << 1))
 			sio_data->internal |= (1 << 1);
 
 		sio_data->beep_pin = superio_inb(IT87_SIO_BEEP_PIN_REG) & 0x3f;
diff --git a/drivers/hwmon/k8temp.c b/drivers/hwmon/k8temp.c
index 9f3c0aeacdb..5b50e9e4f96 100644
--- a/drivers/hwmon/k8temp.c
+++ b/drivers/hwmon/k8temp.c
@@ -200,8 +200,8 @@ static int k8temp_probe(struct pci_dev *pdev,
 	 */
 	if (model >= 0x40) {
 		data->swap_core_select = 1;
-		dev_warn(&pdev->dev, "Temperature readouts might be wrong - "
-			 "check erratum #141\n");
+		dev_warn(&pdev->dev,
+			 "Temperature readouts might be wrong - check erratum #141\n");
 	}
 
 	/*
diff --git a/drivers/hwmon/lm78.c b/drivers/hwmon/lm78.c
index 483538fa1bd..6cf6bff7900 100644
--- a/drivers/hwmon/lm78.c
+++ b/drivers/hwmon/lm78.c
@@ -386,8 +386,9 @@ static ssize_t set_fan_div(struct device *dev, struct device_attribute *da,
 		data->fan_div[nr] = 3;
 		break;
 	default:
-		dev_err(dev, "fan_div value %ld not "
-			"supported. Choose one of 1, 2, 4 or 8!\n", val);
+		dev_err(dev,
+			"fan_div value %ld not supported. Choose one of 1, 2, 4 or 8!\n",
+			val);
 		mutex_unlock(&data->update_lock);
 		return -EINVAL;
 	}
@@ -636,8 +637,9 @@ static int lm78_i2c_detect(struct i2c_client *client,
 		goto err_nodev;
 
 	if (lm78_alias_detect(client, i)) {
-		dev_dbg(&adapter->dev, "Device at 0x%02x appears to "
-			"be the same as ISA device\n", address);
+		dev_dbg(&adapter->dev,
+			"Device at 0x%02x appears to be the same as ISA device\n",
+			address);
 		goto err_nodev;
 	}
 
diff --git a/drivers/hwmon/lm80.c b/drivers/hwmon/lm80.c
index 357fbb99872..eba89aac3ec 100644
--- a/drivers/hwmon/lm80.c
+++ b/drivers/hwmon/lm80.c
@@ -286,8 +286,9 @@ static ssize_t set_fan_div(struct device *dev, struct device_attribute *attr,
 		data->fan_div[nr] = 3;
 		break;
 	default:
-		dev_err(&client->dev, "fan_div value %ld not "
-			"supported. Choose one of 1, 2, 4 or 8!\n", val);
+		dev_err(&client->dev,
+			"fan_div value %ld not supported. Choose one of 1, 2, 4 or 8!\n",
+			val);
 		mutex_unlock(&data->update_lock);
 		return -EINVAL;
 	}
diff --git a/drivers/hwmon/lm85.c b/drivers/hwmon/lm85.c
index 47ade8ba152..3894c408fda 100644
--- a/drivers/hwmon/lm85.c
+++ b/drivers/hwmon/lm85.c
@@ -1293,8 +1293,8 @@ static int lm85_detect(struct i2c_client *client, struct i2c_board_info *info)
 	company = lm85_read_value(client, LM85_REG_COMPANY);
 	verstep = lm85_read_value(client, LM85_REG_VERSTEP);
 
-	dev_dbg(&adapter->dev, "Detecting device at 0x%02x with "
-		"COMPANY: 0x%02x and VERSTEP: 0x%02x\n",
+	dev_dbg(&adapter->dev,
+		"Detecting device at 0x%02x with COMPANY: 0x%02x and VERSTEP: 0x%02x\n",
 		address, company, verstep);
 
 	/* All supported chips have the version in common */
diff --git a/drivers/hwmon/lm93.c b/drivers/hwmon/lm93.c
index b40f34cdb3c..a6f46058b1b 100644
--- a/drivers/hwmon/lm93.c
+++ b/drivers/hwmon/lm93.c
@@ -354,12 +354,12 @@ static const unsigned long lm93_vin_val_max[16] = {
 
 static unsigned LM93_IN_FROM_REG(int nr, u8 reg)
 {
-	const long uV_max = lm93_vin_val_max[nr] * 1000;
-	const long uV_min = lm93_vin_val_min[nr] * 1000;
+	const long uv_max = lm93_vin_val_max[nr] * 1000;
+	const long uv_min = lm93_vin_val_min[nr] * 1000;
 
-	const long slope = (uV_max - uV_min) /
+	const long slope = (uv_max - uv_min) /
 		(lm93_vin_reg_max[nr] - lm93_vin_reg_min[nr]);
-	const long intercept = uV_min - slope * lm93_vin_reg_min[nr];
+	const long intercept = uv_min - slope * lm93_vin_reg_min[nr];
 
 	return (slope * reg + intercept + 500) / 1000;
 }
@@ -371,20 +371,20 @@ static unsigned LM93_IN_FROM_REG(int nr, u8 reg)
 static u8 LM93_IN_TO_REG(int nr, unsigned val)
 {
 	/* range limit */
-	const long mV = clamp_val(val,
+	const long mv = clamp_val(val,
 				  lm93_vin_val_min[nr], lm93_vin_val_max[nr]);
 
 	/* try not to lose too much precision here */
-	const long uV = mV * 1000;
-	const long uV_max = lm93_vin_val_max[nr] * 1000;
-	const long uV_min = lm93_vin_val_min[nr] * 1000;
+	const long uv = mv * 1000;
+	const long uv_max = lm93_vin_val_max[nr] * 1000;
+	const long uv_min = lm93_vin_val_min[nr] * 1000;
 
 	/* convert */
-	const long slope = (uV_max - uV_min) /
+	const long slope = (uv_max - uv_min) /
 		(lm93_vin_reg_max[nr] - lm93_vin_reg_min[nr]);
-	const long intercept = uV_min - slope * lm93_vin_reg_min[nr];
+	const long intercept = uv_min - slope * lm93_vin_reg_min[nr];
 
-	u8 result = ((uV - intercept + (slope/2)) / slope);
+	u8 result = ((uv - intercept + (slope/2)) / slope);
 	result = clamp_val(result,
 			   lm93_vin_reg_min[nr], lm93_vin_reg_max[nr]);
 	return result;
@@ -393,10 +393,10 @@ static u8 LM93_IN_TO_REG(int nr, unsigned val)
 /* vid in mV, upper == 0 indicates low limit, otherwise upper limit */
 static unsigned LM93_IN_REL_FROM_REG(u8 reg, int upper, int vid)
 {
-	const long uV_offset = upper ? (((reg >> 4 & 0x0f) + 1) * 12500) :
+	const long uv_offset = upper ? (((reg >> 4 & 0x0f) + 1) * 12500) :
 				(((reg >> 0 & 0x0f) + 1) * -25000);
-	const long uV_vid = vid * 1000;
-	return (uV_vid + uV_offset + 5000) / 10000;
+	const long uv_vid = vid * 1000;
+	return (uv_vid + uv_offset + 5000) / 10000;
 }
 
 #define LM93_IN_MIN_FROM_REG(reg, vid)	LM93_IN_REL_FROM_REG((reg), 0, (vid))
@@ -409,13 +409,13 @@ static unsigned LM93_IN_REL_FROM_REG(u8 reg, int upper, int vid)
  */
 static u8 LM93_IN_REL_TO_REG(unsigned val, int upper, int vid)
 {
-	long uV_offset = vid * 1000 - val * 10000;
+	long uv_offset = vid * 1000 - val * 10000;
 	if (upper) {
-		uV_offset = clamp_val(uV_offset, 12500, 200000);
-		return (u8)((uV_offset /  12500 - 1) << 4);
+		uv_offset = clamp_val(uv_offset, 12500, 200000);
+		return (u8)((uv_offset /  12500 - 1) << 4);
 	} else {
-		uV_offset = clamp_val(uV_offset, -400000, -25000);
-		return (u8)((uV_offset / -25000 - 1) << 0);
+		uv_offset = clamp_val(uv_offset, -400000, -25000);
+		return (u8)((uv_offset / -25000 - 1) << 0);
 	}
 }
 
@@ -818,8 +818,9 @@ static u8 lm93_read_byte(struct i2c_client *client, u8 reg)
 		if (value >= 0) {
 			return value;
 		} else {
-			dev_warn(&client->dev, "lm93: read byte data failed, "
-				"address 0x%02x.\n", reg);
+			dev_warn(&client->dev,
+				 "lm93: read byte data failed, address 0x%02x.\n",
+				 reg);
 			mdelay(i + 3);
 		}
 
@@ -838,8 +839,9 @@ static int lm93_write_byte(struct i2c_client *client, u8 reg, u8 value)
 	result = i2c_smbus_write_byte_data(client, reg, value);
 
 	if (result < 0)
-		dev_warn(&client->dev, "lm93: write byte data failed, "
-			 "0x%02x at address 0x%02x.\n", value, reg);
+		dev_warn(&client->dev,
+			 "lm93: write byte data failed, 0x%02x at address 0x%02x.\n",
+			 value, reg);
 
 	return result;
 }
@@ -854,8 +856,9 @@ static u16 lm93_read_word(struct i2c_client *client, u8 reg)
 		if (value >= 0) {
 			return value;
 		} else {
-			dev_warn(&client->dev, "lm93: read word data failed, "
-				 "address 0x%02x.\n", reg);
+			dev_warn(&client->dev,
+				 "lm93: read word data failed, address 0x%02x.\n",
+				 reg);
 			mdelay(i + 3);
 		}
 
@@ -874,8 +877,9 @@ static int lm93_write_word(struct i2c_client *client, u8 reg, u16 value)
 	result = i2c_smbus_write_word_data(client, reg, value);
 
 	if (result < 0)
-		dev_warn(&client->dev, "lm93: write word data failed, "
-			 "0x%04x at address 0x%02x.\n", value, reg);
+		dev_warn(&client->dev,
+			 "lm93: write word data failed, 0x%04x at address 0x%02x.\n",
+			 value, reg);
 
 	return result;
 }
@@ -898,8 +902,8 @@ static void lm93_read_block(struct i2c_client *client, u8 fbn, u8 *values)
 		if (result == lm93_block_read_cmds[fbn].len) {
 			break;
 		} else {
-			dev_warn(&client->dev, "lm93: block read data failed, "
-				 "command 0x%02x.\n",
+			dev_warn(&client->dev,
+				 "lm93: block read data failed, command 0x%02x.\n",
 				 lm93_block_read_cmds[fbn].cmd);
 			mdelay(i + 3);
 		}
@@ -2672,8 +2676,8 @@ static void lm93_init_client(struct i2c_client *client)
 			return;
 	}
 
-	dev_warn(&client->dev, "timed out waiting for sensor "
-		 "chip to signal ready!\n");
+	dev_warn(&client->dev,
+		 "timed out waiting for sensor chip to signal ready!\n");
 }
 
 /* Return 0 if detection is successful, -ENODEV otherwise */
@@ -2733,12 +2737,12 @@ static int lm93_probe(struct i2c_client *client,
 		dev_dbg(&client->dev, "using SMBus block data transactions\n");
 		update = lm93_update_client_full;
 	} else if ((LM93_SMBUS_FUNC_MIN & func) == LM93_SMBUS_FUNC_MIN) {
-		dev_dbg(&client->dev, "disabled SMBus block data "
-			"transactions\n");
+		dev_dbg(&client->dev,
+			"disabled SMBus block data transactions\n");
 		update = lm93_update_client_min;
 	} else {
-		dev_dbg(&client->dev, "detect failed, "
-			"smbus byte and/or word data not supported!\n");
+		dev_dbg(&client->dev,
+			"detect failed, smbus byte and/or word data not supported!\n");
 		return -ENODEV;
 	}
 
diff --git a/drivers/hwmon/lm95234.c b/drivers/hwmon/lm95234.c
new file mode 100644
index 00000000000..307c9eaeeb9
--- /dev/null
+++ b/drivers/hwmon/lm95234.c
@@ -0,0 +1,769 @@
+/*
+ * Driver for Texas Instruments / National Semiconductor LM95234
+ *
+ * Copyright (c) 2013 Guenter Roeck <linux@roeck-us.net>
+ *
+ * Derived from lm95241.c
+ * Copyright (C) 2008, 2010 Davide Rizzo <elpa.rizzo@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.
+ *
+ * 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/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/jiffies.h>
+#include <linux/i2c.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/err.h>
+#include <linux/mutex.h>
+#include <linux/sysfs.h>
+
+#define DRVNAME "lm95234"
+
+static const unsigned short normal_i2c[] = { 0x18, 0x4d, 0x4e, I2C_CLIENT_END };
+
+/* LM95234 registers */
+#define LM95234_REG_MAN_ID		0xFE
+#define LM95234_REG_CHIP_ID		0xFF
+#define LM95234_REG_STATUS		0x02
+#define LM95234_REG_CONFIG		0x03
+#define LM95234_REG_CONVRATE		0x04
+#define LM95234_REG_STS_FAULT		0x07
+#define LM95234_REG_STS_TCRIT1		0x08
+#define LM95234_REG_STS_TCRIT2		0x09
+#define LM95234_REG_TEMPH(x)		((x) + 0x10)
+#define LM95234_REG_TEMPL(x)		((x) + 0x20)
+#define LM95234_REG_UTEMPH(x)		((x) + 0x19)	/* Remote only */
+#define LM95234_REG_UTEMPL(x)		((x) + 0x29)
+#define LM95234_REG_REM_MODEL		0x30
+#define LM95234_REG_REM_MODEL_STS	0x38
+#define LM95234_REG_OFFSET(x)		((x) + 0x31)	/* Remote only */
+#define LM95234_REG_TCRIT1(x)		((x) + 0x40)
+#define LM95234_REG_TCRIT2(x)		((x) + 0x49)	/* Remote channel 1,2 */
+#define LM95234_REG_TCRIT_HYST		0x5a
+
+#define NATSEMI_MAN_ID			0x01
+#define LM95234_CHIP_ID			0x79
+
+/* Client data (each client gets its own) */
+struct lm95234_data {
+	struct device *hwmon_dev;
+	struct mutex update_lock;
+	unsigned long last_updated, interval;	/* in jiffies */
+	bool valid;		/* false until following fields are valid */
+	/* registers values */
+	int temp[5];		/* temperature (signed) */
+	u32 status;		/* fault/alarm status */
+	u8 tcrit1[5];		/* critical temperature limit */
+	u8 tcrit2[2];		/* high temperature limit */
+	s8 toffset[4];		/* remote temperature offset */
+	u8 thyst;		/* common hysteresis */
+
+	u8 sensor_type;		/* temperature sensor type */
+};
+
+static int lm95234_read_temp(struct i2c_client *client, int index, int *t)
+{
+	int val;
+	u16 temp = 0;
+
+	if (index) {
+		val = i2c_smbus_read_byte_data(client,
+					       LM95234_REG_UTEMPH(index - 1));
+		if (val < 0)
+			return val;
+		temp = val << 8;
+		val = i2c_smbus_read_byte_data(client,
+					       LM95234_REG_UTEMPL(index - 1));
+		if (val < 0)
+			return val;
+		temp |= val;
+		*t = temp;
+	}
+	/*
+	 * Read signed temperature if unsigned temperature is 0,
+	 * or if this is the local sensor.
+	 */
+	if (!temp) {
+		val = i2c_smbus_read_byte_data(client,
+					       LM95234_REG_TEMPH(index));
+		if (val < 0)
+			return val;
+		temp = val << 8;
+		val = i2c_smbus_read_byte_data(client,
+					       LM95234_REG_TEMPL(index));
+		if (val < 0)
+			return val;
+		temp |= val;
+		*t = (s16)temp;
+	}
+	return 0;
+}
+
+static u16 update_intervals[] = { 143, 364, 1000, 2500 };
+
+/* Fill value cache. Must be called with update lock held. */
+
+static int lm95234_fill_cache(struct i2c_client *client)
+{
+	struct lm95234_data *data = i2c_get_clientdata(client);
+	int i, ret;
+
+	ret = i2c_smbus_read_byte_data(client, LM95234_REG_CONVRATE);
+	if (ret < 0)
+		return ret;
+
+	data->interval = msecs_to_jiffies(update_intervals[ret & 0x03]);
+
+	for (i = 0; i < ARRAY_SIZE(data->tcrit1); i++) {
+		ret = i2c_smbus_read_byte_data(client, LM95234_REG_TCRIT1(i));
+		if (ret < 0)
+			return ret;
+		data->tcrit1[i] = ret;
+	}
+	for (i = 0; i < ARRAY_SIZE(data->tcrit2); i++) {
+		ret = i2c_smbus_read_byte_data(client, LM95234_REG_TCRIT2(i));
+		if (ret < 0)
+			return ret;
+		data->tcrit2[i] = ret;
+	}
+	for (i = 0; i < ARRAY_SIZE(data->toffset); i++) {
+		ret = i2c_smbus_read_byte_data(client, LM95234_REG_OFFSET(i));
+		if (ret < 0)
+			return ret;
+		data->toffset[i] = ret;
+	}
+
+	ret = i2c_smbus_read_byte_data(client, LM95234_REG_TCRIT_HYST);
+	if (ret < 0)
+		return ret;
+	data->thyst = ret;
+
+	ret = i2c_smbus_read_byte_data(client, LM95234_REG_REM_MODEL);
+	if (ret < 0)
+		return ret;
+	data->sensor_type = ret;
+
+	return 0;
+}
+
+static int lm95234_update_device(struct i2c_client *client,
+				 struct lm95234_data *data)
+{
+	int ret;
+
+	mutex_lock(&data->update_lock);
+
+	if (time_after(jiffies, data->last_updated + data->interval) ||
+	    !data->valid) {
+		int i;
+
+		if (!data->valid) {
+			ret = lm95234_fill_cache(client);
+			if (ret < 0)
+				goto abort;
+		}
+
+		data->valid = false;
+		for (i = 0; i < ARRAY_SIZE(data->temp); i++) {
+			ret = lm95234_read_temp(client, i, &data->temp[i]);
+			if (ret < 0)
+				goto abort;
+		}
+
+		ret = i2c_smbus_read_byte_data(client, LM95234_REG_STS_FAULT);
+		if (ret < 0)
+			goto abort;
+		data->status = ret;
+
+		ret = i2c_smbus_read_byte_data(client, LM95234_REG_STS_TCRIT1);
+		if (ret < 0)
+			goto abort;
+		data->status |= ret << 8;
+
+		ret = i2c_smbus_read_byte_data(client, LM95234_REG_STS_TCRIT2);
+		if (ret < 0)
+			goto abort;
+		data->status |= ret << 16;
+
+		data->last_updated = jiffies;
+		data->valid = true;
+	}
+	ret = 0;
+abort:
+	mutex_unlock(&data->update_lock);
+
+	return ret;
+}
+
+static ssize_t show_temp(struct device *dev, struct device_attribute *attr,
+			 char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct lm95234_data *data = i2c_get_clientdata(client);
+	int index = to_sensor_dev_attr(attr)->index;
+	int ret = lm95234_update_device(client, data);
+
+	if (ret)
+		return ret;
+
+	return sprintf(buf, "%d\n",
+		       DIV_ROUND_CLOSEST(data->temp[index] * 125, 32));
+}
+
+static ssize_t show_alarm(struct device *dev,
+			  struct device_attribute *attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct lm95234_data *data = i2c_get_clientdata(client);
+	u32 mask = to_sensor_dev_attr(attr)->index;
+	int ret = lm95234_update_device(client, data);
+
+	if (ret)
+		return ret;
+
+	return sprintf(buf, "%u", !!(data->status & mask));
+}
+
+static ssize_t show_type(struct device *dev, struct device_attribute *attr,
+			 char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct lm95234_data *data = i2c_get_clientdata(client);
+	u8 mask = to_sensor_dev_attr(attr)->index;
+	int ret = lm95234_update_device(client, data);
+
+	if (ret)
+		return ret;
+
+	return sprintf(buf, data->sensor_type & mask ? "1\n" : "2\n");
+}
+
+static ssize_t set_type(struct device *dev, struct device_attribute *attr,
+			const char *buf, size_t count)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct lm95234_data *data = i2c_get_clientdata(client);
+	unsigned long val;
+	u8 mask = to_sensor_dev_attr(attr)->index;
+	int ret = lm95234_update_device(client, data);
+
+	if (ret)
+		return ret;
+
+	ret = kstrtoul(buf, 10, &val);
+	if (ret < 0)
+		return ret;
+
+	if (val != 1 && val != 2)
+		return -EINVAL;
+
+	mutex_lock(&data->update_lock);
+	if (val == 1)
+		data->sensor_type |= mask;
+	else
+		data->sensor_type &= ~mask;
+	data->valid = false;
+	i2c_smbus_write_byte_data(client, LM95234_REG_REM_MODEL,
+				  data->sensor_type);
+	mutex_unlock(&data->update_lock);
+
+	return count;
+}
+
+static ssize_t show_tcrit2(struct device *dev, struct device_attribute *attr,
+			   char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct lm95234_data *data = i2c_get_clientdata(client);
+	int index = to_sensor_dev_attr(attr)->index;
+	int ret = lm95234_update_device(client, data);
+
+	if (ret)
+		return ret;
+
+	return sprintf(buf, "%u", data->tcrit2[index] * 1000);
+}
+
+static ssize_t set_tcrit2(struct device *dev, struct device_attribute *attr,
+			  const char *buf, size_t count)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct lm95234_data *data = i2c_get_clientdata(client);
+	int index = to_sensor_dev_attr(attr)->index;
+	long val;
+	int ret = lm95234_update_device(client, data);
+
+	if (ret)
+		return ret;
+
+	ret = kstrtol(buf, 10, &val);
+	if (ret < 0)
+		return ret;
+
+	val = clamp_val(DIV_ROUND_CLOSEST(val, 1000), 0, index ? 255 : 127);
+
+	mutex_lock(&data->update_lock);
+	data->tcrit2[index] = val;
+	i2c_smbus_write_byte_data(client, LM95234_REG_TCRIT2(index), val);
+	mutex_unlock(&data->update_lock);
+
+	return count;
+}
+
+static ssize_t show_tcrit2_hyst(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct lm95234_data *data = i2c_get_clientdata(client);
+	int index = to_sensor_dev_attr(attr)->index;
+	int ret = lm95234_update_device(client, data);
+
+	if (ret)
+		return ret;
+
+	/* Result can be negative, so be careful with unsigned operands */
+	return sprintf(buf, "%d",
+		       ((int)data->tcrit2[index] - (int)data->thyst) * 1000);
+}
+
+static ssize_t show_tcrit1(struct device *dev, struct device_attribute *attr,
+			   char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct lm95234_data *data = i2c_get_clientdata(client);
+	int index = to_sensor_dev_attr(attr)->index;
+
+	return sprintf(buf, "%u", data->tcrit1[index] * 1000);
+}
+
+static ssize_t set_tcrit1(struct device *dev, struct device_attribute *attr,
+			  const char *buf, size_t count)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct lm95234_data *data = i2c_get_clientdata(client);
+	int index = to_sensor_dev_attr(attr)->index;
+	long val;
+	int ret = lm95234_update_device(client, data);
+
+	if (ret)
+		return ret;
+
+	ret = kstrtol(buf, 10, &val);
+	if (ret < 0)
+		return ret;
+
+	val = clamp_val(DIV_ROUND_CLOSEST(val, 1000), 0, 255);
+
+	mutex_lock(&data->update_lock);
+	data->tcrit1[index] = val;
+	i2c_smbus_write_byte_data(client, LM95234_REG_TCRIT1(index), val);
+	mutex_unlock(&data->update_lock);
+
+	return count;
+}
+
+static ssize_t show_tcrit1_hyst(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct lm95234_data *data = i2c_get_clientdata(client);
+	int index = to_sensor_dev_attr(attr)->index;
+	int ret = lm95234_update_device(client, data);
+
+	if (ret)
+		return ret;
+
+	/* Result can be negative, so be careful with unsigned operands */
+	return sprintf(buf, "%d",
+		       ((int)data->tcrit1[index] - (int)data->thyst) * 1000);
+}
+
+static ssize_t set_tcrit1_hyst(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf, size_t count)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct lm95234_data *data = i2c_get_clientdata(client);
+	int index = to_sensor_dev_attr(attr)->index;
+	long val;
+	int ret = lm95234_update_device(client, data);
+
+	if (ret)
+		return ret;
+
+	ret = kstrtol(buf, 10, &val);
+	if (ret < 0)
+		return ret;
+
+	val = DIV_ROUND_CLOSEST(val, 1000);
+	val = clamp_val((int)data->tcrit1[index] - val, 0, 31);
+
+	mutex_lock(&data->update_lock);
+	data->thyst = val;
+	i2c_smbus_write_byte_data(client, LM95234_REG_TCRIT_HYST, val);
+	mutex_unlock(&data->update_lock);
+
+	return count;
+}
+
+static ssize_t show_offset(struct device *dev, struct device_attribute *attr,
+			   char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct lm95234_data *data = i2c_get_clientdata(client);
+	int index = to_sensor_dev_attr(attr)->index;
+	int ret = lm95234_update_device(client, data);
+
+	if (ret)
+		return ret;
+
+	return sprintf(buf, "%d", data->toffset[index] * 500);
+}
+
+static ssize_t set_offset(struct device *dev, struct device_attribute *attr,
+			  const char *buf, size_t count)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct lm95234_data *data = i2c_get_clientdata(client);
+	int index = to_sensor_dev_attr(attr)->index;
+	long val;
+	int ret = lm95234_update_device(client, data);
+
+	if (ret)
+		return ret;
+
+	ret = kstrtol(buf, 10, &val);
+	if (ret < 0)
+		return ret;
+
+	/* Accuracy is 1/2 degrees C */
+	val = clamp_val(DIV_ROUND_CLOSEST(val, 500), -128, 127);
+
+	mutex_lock(&data->update_lock);
+	data->toffset[index] = val;
+	i2c_smbus_write_byte_data(client, LM95234_REG_OFFSET(index), val);
+	mutex_unlock(&data->update_lock);
+
+	return count;
+}
+
+static ssize_t show_interval(struct device *dev, struct device_attribute *attr,
+			     char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct lm95234_data *data = i2c_get_clientdata(client);
+	int ret = lm95234_update_device(client, data);
+
+	if (ret)
+		return ret;
+
+	return sprintf(buf, "%lu\n",
+		       DIV_ROUND_CLOSEST(data->interval * 1000, HZ));
+}
+
+static ssize_t set_interval(struct device *dev, struct device_attribute *attr,
+			    const char *buf, size_t count)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct lm95234_data *data = i2c_get_clientdata(client);
+	unsigned long val;
+	u8 regval;
+	int ret = lm95234_update_device(client, data);
+
+	if (ret)
+		return ret;
+
+	ret = kstrtoul(buf, 10, &val);
+	if (ret < 0)
+		return ret;
+
+	for (regval = 0; regval < 3; regval++) {
+		if (val <= update_intervals[regval])
+			break;
+	}
+
+	mutex_lock(&data->update_lock);
+	data->interval = msecs_to_jiffies(update_intervals[regval]);
+	i2c_smbus_write_byte_data(client, LM95234_REG_CONVRATE, regval);
+	mutex_unlock(&data->update_lock);
+
+	return count;
+}
+
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_temp, NULL, 0);
+static SENSOR_DEVICE_ATTR(temp2_input, S_IRUGO, show_temp, NULL, 1);
+static SENSOR_DEVICE_ATTR(temp3_input, S_IRUGO, show_temp, NULL, 2);
+static SENSOR_DEVICE_ATTR(temp4_input, S_IRUGO, show_temp, NULL, 3);
+static SENSOR_DEVICE_ATTR(temp5_input, S_IRUGO, show_temp, NULL, 4);
+
+static SENSOR_DEVICE_ATTR(temp2_fault, S_IRUGO, show_alarm, NULL,
+			  BIT(0) | BIT(1));
+static SENSOR_DEVICE_ATTR(temp3_fault, S_IRUGO, show_alarm, NULL,
+			  BIT(2) | BIT(3));
+static SENSOR_DEVICE_ATTR(temp4_fault, S_IRUGO, show_alarm, NULL,
+			  BIT(4) | BIT(5));
+static SENSOR_DEVICE_ATTR(temp5_fault, S_IRUGO, show_alarm, NULL,
+			  BIT(6) | BIT(7));
+
+static SENSOR_DEVICE_ATTR(temp2_type, S_IWUSR | S_IRUGO, show_type, set_type,
+			  BIT(1));
+static SENSOR_DEVICE_ATTR(temp3_type, S_IWUSR | S_IRUGO, show_type, set_type,
+			  BIT(2));
+static SENSOR_DEVICE_ATTR(temp4_type, S_IWUSR | S_IRUGO, show_type, set_type,
+			  BIT(3));
+static SENSOR_DEVICE_ATTR(temp5_type, S_IWUSR | S_IRUGO, show_type, set_type,
+			  BIT(4));
+
+static SENSOR_DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO, show_tcrit1,
+			  set_tcrit1, 0);
+static SENSOR_DEVICE_ATTR(temp2_max, S_IWUSR | S_IRUGO, show_tcrit2,
+			  set_tcrit2, 0);
+static SENSOR_DEVICE_ATTR(temp3_max, S_IWUSR | S_IRUGO, show_tcrit2,
+			  set_tcrit2, 1);
+static SENSOR_DEVICE_ATTR(temp4_max, S_IWUSR | S_IRUGO, show_tcrit1,
+			  set_tcrit1, 3);
+static SENSOR_DEVICE_ATTR(temp5_max, S_IWUSR | S_IRUGO, show_tcrit1,
+			  set_tcrit1, 4);
+
+static SENSOR_DEVICE_ATTR(temp1_max_hyst, S_IWUSR | S_IRUGO, show_tcrit1_hyst,
+			  set_tcrit1_hyst, 0);
+static SENSOR_DEVICE_ATTR(temp2_max_hyst, S_IRUGO, show_tcrit2_hyst, NULL, 0);
+static SENSOR_DEVICE_ATTR(temp3_max_hyst, S_IRUGO, show_tcrit2_hyst, NULL, 1);
+static SENSOR_DEVICE_ATTR(temp4_max_hyst, S_IRUGO, show_tcrit1_hyst, NULL, 3);
+static SENSOR_DEVICE_ATTR(temp5_max_hyst, S_IRUGO, show_tcrit1_hyst, NULL, 4);
+
+static SENSOR_DEVICE_ATTR(temp1_max_alarm, S_IRUGO, show_alarm, NULL,
+			  BIT(0 + 8));
+static SENSOR_DEVICE_ATTR(temp2_max_alarm, S_IRUGO, show_alarm, NULL,
+			  BIT(1 + 16));
+static SENSOR_DEVICE_ATTR(temp3_max_alarm, S_IRUGO, show_alarm, NULL,
+			  BIT(2 + 16));
+static SENSOR_DEVICE_ATTR(temp4_max_alarm, S_IRUGO, show_alarm, NULL,
+			  BIT(3 + 8));
+static SENSOR_DEVICE_ATTR(temp5_max_alarm, S_IRUGO, show_alarm, NULL,
+			  BIT(4 + 8));
+
+static SENSOR_DEVICE_ATTR(temp2_crit, S_IWUSR | S_IRUGO, show_tcrit1,
+			  set_tcrit1, 1);
+static SENSOR_DEVICE_ATTR(temp3_crit, S_IWUSR | S_IRUGO, show_tcrit1,
+			  set_tcrit1, 2);
+
+static SENSOR_DEVICE_ATTR(temp2_crit_hyst, S_IRUGO, show_tcrit1_hyst, NULL, 1);
+static SENSOR_DEVICE_ATTR(temp3_crit_hyst, S_IRUGO, show_tcrit1_hyst, NULL, 2);
+
+static SENSOR_DEVICE_ATTR(temp2_crit_alarm, S_IRUGO, show_alarm, NULL,
+			  BIT(1 + 8));
+static SENSOR_DEVICE_ATTR(temp3_crit_alarm, S_IRUGO, show_alarm, NULL,
+			  BIT(2 + 8));
+
+static SENSOR_DEVICE_ATTR(temp2_offset, S_IWUSR | S_IRUGO, show_offset,
+			  set_offset, 0);
+static SENSOR_DEVICE_ATTR(temp3_offset, S_IWUSR | S_IRUGO, show_offset,
+			  set_offset, 1);
+static SENSOR_DEVICE_ATTR(temp4_offset, S_IWUSR | S_IRUGO, show_offset,
+			  set_offset, 2);
+static SENSOR_DEVICE_ATTR(temp5_offset, S_IWUSR | S_IRUGO, show_offset,
+			  set_offset, 3);
+
+static DEVICE_ATTR(update_interval, S_IWUSR | S_IRUGO, show_interval,
+		   set_interval);
+
+static struct attribute *lm95234_attributes[] = {
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	&sensor_dev_attr_temp2_input.dev_attr.attr,
+	&sensor_dev_attr_temp3_input.dev_attr.attr,
+	&sensor_dev_attr_temp4_input.dev_attr.attr,
+	&sensor_dev_attr_temp5_input.dev_attr.attr,
+	&sensor_dev_attr_temp2_fault.dev_attr.attr,
+	&sensor_dev_attr_temp3_fault.dev_attr.attr,
+	&sensor_dev_attr_temp4_fault.dev_attr.attr,
+	&sensor_dev_attr_temp5_fault.dev_attr.attr,
+	&sensor_dev_attr_temp2_type.dev_attr.attr,
+	&sensor_dev_attr_temp3_type.dev_attr.attr,
+	&sensor_dev_attr_temp4_type.dev_attr.attr,
+	&sensor_dev_attr_temp5_type.dev_attr.attr,
+	&sensor_dev_attr_temp1_max.dev_attr.attr,
+	&sensor_dev_attr_temp2_max.dev_attr.attr,
+	&sensor_dev_attr_temp3_max.dev_attr.attr,
+	&sensor_dev_attr_temp4_max.dev_attr.attr,
+	&sensor_dev_attr_temp5_max.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp2_max_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp3_max_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp4_max_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp5_max_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp2_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp3_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp4_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp5_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp2_crit.dev_attr.attr,
+	&sensor_dev_attr_temp3_crit.dev_attr.attr,
+	&sensor_dev_attr_temp2_crit_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp3_crit_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp2_crit_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp3_crit_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp2_offset.dev_attr.attr,
+	&sensor_dev_attr_temp3_offset.dev_attr.attr,
+	&sensor_dev_attr_temp4_offset.dev_attr.attr,
+	&sensor_dev_attr_temp5_offset.dev_attr.attr,
+	&dev_attr_update_interval.attr,
+	NULL
+};
+
+static const struct attribute_group lm95234_group = {
+	.attrs = lm95234_attributes,
+};
+
+static int lm95234_detect(struct i2c_client *client,
+			  struct i2c_board_info *info)
+{
+	struct i2c_adapter *adapter = client->adapter;
+	int mfg_id, chip_id, val;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -ENODEV;
+
+	mfg_id = i2c_smbus_read_byte_data(client, LM95234_REG_MAN_ID);
+	if (mfg_id != NATSEMI_MAN_ID)
+		return -ENODEV;
+
+	chip_id = i2c_smbus_read_byte_data(client, LM95234_REG_CHIP_ID);
+	if (chip_id != LM95234_CHIP_ID)
+		return -ENODEV;
+
+	val = i2c_smbus_read_byte_data(client, LM95234_REG_STATUS);
+	if (val & 0x30)
+		return -ENODEV;
+
+	val = i2c_smbus_read_byte_data(client, LM95234_REG_CONFIG);
+	if (val & 0xbc)
+		return -ENODEV;
+
+	val = i2c_smbus_read_byte_data(client, LM95234_REG_CONVRATE);
+	if (val & 0xfc)
+		return -ENODEV;
+
+	val = i2c_smbus_read_byte_data(client, LM95234_REG_REM_MODEL);
+	if (val & 0xe1)
+		return -ENODEV;
+
+	val = i2c_smbus_read_byte_data(client, LM95234_REG_REM_MODEL_STS);
+	if (val & 0xe1)
+		return -ENODEV;
+
+	strlcpy(info->type, "lm95234", I2C_NAME_SIZE);
+	return 0;
+}
+
+static int lm95234_init_client(struct i2c_client *client)
+{
+	int val, model;
+
+	/* start conversion if necessary */
+	val = i2c_smbus_read_byte_data(client, LM95234_REG_CONFIG);
+	if (val < 0)
+		return val;
+	if (val & 0x40)
+		i2c_smbus_write_byte_data(client, LM95234_REG_CONFIG,
+					  val & ~0x40);
+
+	/* If diode type status reports an error, try to fix it */
+	val = i2c_smbus_read_byte_data(client, LM95234_REG_REM_MODEL_STS);
+	if (val < 0)
+		return val;
+	model = i2c_smbus_read_byte_data(client, LM95234_REG_REM_MODEL);
+	if (model < 0)
+		return model;
+	if (model & val) {
+		dev_notice(&client->dev,
+			   "Fixing remote diode type misconfiguration (0x%x)\n",
+			   val);
+		i2c_smbus_write_byte_data(client, LM95234_REG_REM_MODEL,
+					  model & ~val);
+	}
+	return 0;
+}
+
+static int lm95234_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct device *dev = &client->dev;
+	struct lm95234_data *data;
+	int err;
+
+	data = devm_kzalloc(dev, sizeof(struct lm95234_data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	i2c_set_clientdata(client, data);
+	mutex_init(&data->update_lock);
+
+	/* Initialize the LM95234 chip */
+	err = lm95234_init_client(client);
+	if (err < 0)
+		return err;
+
+	/* Register sysfs hooks */
+	err = sysfs_create_group(&dev->kobj, &lm95234_group);
+	if (err)
+		return err;
+
+	data->hwmon_dev = hwmon_device_register(dev);
+	if (IS_ERR(data->hwmon_dev)) {
+		err = PTR_ERR(data->hwmon_dev);
+		goto exit_remove_files;
+	}
+
+	return 0;
+
+exit_remove_files:
+	sysfs_remove_group(&dev->kobj, &lm95234_group);
+	return err;
+}
+
+static int lm95234_remove(struct i2c_client *client)
+{
+	struct lm95234_data *data = i2c_get_clientdata(client);
+
+	hwmon_device_unregister(data->hwmon_dev);
+	sysfs_remove_group(&client->dev.kobj, &lm95234_group);
+
+	return 0;
+}
+
+/* Driver data (common to all clients) */
+static const struct i2c_device_id lm95234_id[] = {
+	{ "lm95234", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, lm95234_id);
+
+static struct i2c_driver lm95234_driver = {
+	.class		= I2C_CLASS_HWMON,
+	.driver = {
+		.name	= DRVNAME,
+	},
+	.probe		= lm95234_probe,
+	.remove		= lm95234_remove,
+	.id_table	= lm95234_id,
+	.detect		= lm95234_detect,
+	.address_list	= normal_i2c,
+};
+
+module_i2c_driver(lm95234_driver);
+
+MODULE_AUTHOR("Guenter Roeck <linux@roeck-us.net>");
+MODULE_DESCRIPTION("LM95234 sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/ltc4151.c b/drivers/hwmon/ltc4151.c
index 4319a94f549..af81be1237c 100644
--- a/drivers/hwmon/ltc4151.c
+++ b/drivers/hwmon/ltc4151.c
@@ -146,14 +146,14 @@ static ssize_t ltc4151_show_value(struct device *dev,
 /*
  * Input voltages.
  */
-static SENSOR_DEVICE_ATTR(in1_input, S_IRUGO, \
-	ltc4151_show_value, NULL, LTC4151_VIN_H);
-static SENSOR_DEVICE_ATTR(in2_input, S_IRUGO, \
-	ltc4151_show_value, NULL, LTC4151_ADIN_H);
+static SENSOR_DEVICE_ATTR(in1_input, S_IRUGO, ltc4151_show_value, NULL,
+			  LTC4151_VIN_H);
+static SENSOR_DEVICE_ATTR(in2_input, S_IRUGO, ltc4151_show_value, NULL,
+			  LTC4151_ADIN_H);
 
 /* Currents (via sense resistor) */
-static SENSOR_DEVICE_ATTR(curr1_input, S_IRUGO, \
-	ltc4151_show_value, NULL, LTC4151_SENSE_H);
+static SENSOR_DEVICE_ATTR(curr1_input, S_IRUGO, ltc4151_show_value, NULL,
+			  LTC4151_SENSE_H);
 
 /*
  * Finally, construct an array of pointers to members of the above objects,
diff --git a/drivers/hwmon/ltc4215.c b/drivers/hwmon/ltc4215.c
index e8876108a6b..8a142960d69 100644
--- a/drivers/hwmon/ltc4215.c
+++ b/drivers/hwmon/ltc4215.c
@@ -172,12 +172,12 @@ static ssize_t ltc4215_show_alarm(struct device *dev,
 					  struct device_attribute *da,
 					  char *buf)
 {
-	struct sensor_device_attribute_2 *attr = to_sensor_dev_attr_2(da);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
 	struct ltc4215_data *data = ltc4215_update_device(dev);
-	const u8 reg = data->regs[attr->index];
-	const u32 mask = attr->nr;
+	const u8 reg = data->regs[LTC4215_STATUS];
+	const u32 mask = attr->index;
 
-	return snprintf(buf, PAGE_SIZE, "%u\n", (reg & mask) ? 1 : 0);
+	return snprintf(buf, PAGE_SIZE, "%u\n", !!(reg & mask));
 }
 
 /*
@@ -186,39 +186,29 @@ static ssize_t ltc4215_show_alarm(struct device *dev,
  * for each register.
  */
 
-#define LTC4215_VOLTAGE(name, ltc4215_cmd_idx) \
-	static SENSOR_DEVICE_ATTR(name, S_IRUGO, \
-	ltc4215_show_voltage, NULL, ltc4215_cmd_idx)
-
-#define LTC4215_CURRENT(name) \
-	static SENSOR_DEVICE_ATTR(name, S_IRUGO, \
-	ltc4215_show_current, NULL, 0);
-
-#define LTC4215_POWER(name) \
-	static SENSOR_DEVICE_ATTR(name, S_IRUGO, \
-	ltc4215_show_power, NULL, 0);
-
-#define LTC4215_ALARM(name, mask, reg) \
-	static SENSOR_DEVICE_ATTR_2(name, S_IRUGO, \
-	ltc4215_show_alarm, NULL, (mask), reg)
-
 /* Construct a sensor_device_attribute structure for each register */
 
 /* Current */
-LTC4215_CURRENT(curr1_input);
-LTC4215_ALARM(curr1_max_alarm,	(1 << 2),	LTC4215_STATUS);
+static SENSOR_DEVICE_ATTR(curr1_input, S_IRUGO, ltc4215_show_current, NULL, 0);
+static SENSOR_DEVICE_ATTR(curr1_max_alarm, S_IRUGO, ltc4215_show_alarm, NULL,
+			  1 << 2);
 
 /* Power (virtual) */
-LTC4215_POWER(power1_input);
+static SENSOR_DEVICE_ATTR(power1_input, S_IRUGO, ltc4215_show_power, NULL, 0);
 
 /* Input Voltage */
-LTC4215_VOLTAGE(in1_input,			LTC4215_ADIN);
-LTC4215_ALARM(in1_max_alarm,	(1 << 0),	LTC4215_STATUS);
-LTC4215_ALARM(in1_min_alarm,	(1 << 1),	LTC4215_STATUS);
+static SENSOR_DEVICE_ATTR(in1_input, S_IRUGO, ltc4215_show_voltage, NULL,
+			  LTC4215_ADIN);
+static SENSOR_DEVICE_ATTR(in1_max_alarm, S_IRUGO, ltc4215_show_alarm, NULL,
+			  1 << 0);
+static SENSOR_DEVICE_ATTR(in1_min_alarm, S_IRUGO, ltc4215_show_alarm, NULL,
+			  1 << 1);
 
 /* Output Voltage */
-LTC4215_VOLTAGE(in2_input,			LTC4215_SOURCE);
-LTC4215_ALARM(in2_min_alarm,	(1 << 3),	LTC4215_STATUS);
+static SENSOR_DEVICE_ATTR(in2_input, S_IRUGO, ltc4215_show_voltage, NULL,
+			  LTC4215_SOURCE);
+static SENSOR_DEVICE_ATTR(in2_min_alarm, S_IRUGO, ltc4215_show_alarm, NULL,
+			  1 << 3);
 
 /*
  * Finally, construct an array of pointers to members of the above objects,
diff --git a/drivers/hwmon/ltc4245.c b/drivers/hwmon/ltc4245.c
index 3653f79dc2d..cdc1ecc6734 100644
--- a/drivers/hwmon/ltc4245.c
+++ b/drivers/hwmon/ltc4245.c
@@ -319,80 +319,82 @@ static ssize_t ltc4245_show_gpio(struct device *dev,
 	return snprintf(buf, PAGE_SIZE, "%u\n", val * 10);
 }
 
-/*
- * These macros are used below in constructing device attribute objects
- * for use with sysfs_create_group() to make a sysfs device file
- * for each register.
- */
-
-#define LTC4245_VOLTAGE(name, ltc4245_cmd_idx) \
-	static SENSOR_DEVICE_ATTR(name, S_IRUGO, \
-	ltc4245_show_voltage, NULL, ltc4245_cmd_idx)
-
-#define LTC4245_CURRENT(name, ltc4245_cmd_idx) \
-	static SENSOR_DEVICE_ATTR(name, S_IRUGO, \
-	ltc4245_show_current, NULL, ltc4245_cmd_idx)
-
-#define LTC4245_POWER(name, ltc4245_cmd_idx) \
-	static SENSOR_DEVICE_ATTR(name, S_IRUGO, \
-	ltc4245_show_power, NULL, ltc4245_cmd_idx)
-
-#define LTC4245_ALARM(name, mask, reg) \
-	static SENSOR_DEVICE_ATTR_2(name, S_IRUGO, \
-	ltc4245_show_alarm, NULL, (mask), reg)
-
-#define LTC4245_GPIO_VOLTAGE(name, gpio_num) \
-	static SENSOR_DEVICE_ATTR(name, S_IRUGO, \
-	ltc4245_show_gpio, NULL, gpio_num)
-
 /* Construct a sensor_device_attribute structure for each register */
 
 /* Input voltages */
-LTC4245_VOLTAGE(in1_input,			LTC4245_12VIN);
-LTC4245_VOLTAGE(in2_input,			LTC4245_5VIN);
-LTC4245_VOLTAGE(in3_input,			LTC4245_3VIN);
-LTC4245_VOLTAGE(in4_input,			LTC4245_VEEIN);
+static SENSOR_DEVICE_ATTR(in1_input, S_IRUGO, ltc4245_show_voltage, NULL,
+			  LTC4245_12VIN);
+static SENSOR_DEVICE_ATTR(in2_input, S_IRUGO, ltc4245_show_voltage, NULL,
+			  LTC4245_5VIN);
+static SENSOR_DEVICE_ATTR(in3_input, S_IRUGO, ltc4245_show_voltage, NULL,
+			  LTC4245_3VIN);
+static SENSOR_DEVICE_ATTR(in4_input, S_IRUGO, ltc4245_show_voltage, NULL,
+			  LTC4245_VEEIN);
 
 /* Input undervoltage alarms */
-LTC4245_ALARM(in1_min_alarm,	(1 << 0),	LTC4245_FAULT1);
-LTC4245_ALARM(in2_min_alarm,	(1 << 1),	LTC4245_FAULT1);
-LTC4245_ALARM(in3_min_alarm,	(1 << 2),	LTC4245_FAULT1);
-LTC4245_ALARM(in4_min_alarm,	(1 << 3),	LTC4245_FAULT1);
+static SENSOR_DEVICE_ATTR_2(in1_min_alarm, S_IRUGO, ltc4245_show_alarm, NULL,
+			    1 << 0, LTC4245_FAULT1);
+static SENSOR_DEVICE_ATTR_2(in2_min_alarm, S_IRUGO, ltc4245_show_alarm, NULL,
+			    1 << 1, LTC4245_FAULT1);
+static SENSOR_DEVICE_ATTR_2(in3_min_alarm, S_IRUGO, ltc4245_show_alarm, NULL,
+			    1 << 2, LTC4245_FAULT1);
+static SENSOR_DEVICE_ATTR_2(in4_min_alarm, S_IRUGO, ltc4245_show_alarm, NULL,
+			    1 << 3, LTC4245_FAULT1);
 
 /* Currents (via sense resistor) */
-LTC4245_CURRENT(curr1_input,			LTC4245_12VSENSE);
-LTC4245_CURRENT(curr2_input,			LTC4245_5VSENSE);
-LTC4245_CURRENT(curr3_input,			LTC4245_3VSENSE);
-LTC4245_CURRENT(curr4_input,			LTC4245_VEESENSE);
+static SENSOR_DEVICE_ATTR(curr1_input, S_IRUGO, ltc4245_show_current, NULL,
+			  LTC4245_12VSENSE);
+static SENSOR_DEVICE_ATTR(curr2_input, S_IRUGO, ltc4245_show_current, NULL,
+			  LTC4245_5VSENSE);
+static SENSOR_DEVICE_ATTR(curr3_input, S_IRUGO, ltc4245_show_current, NULL,
+			  LTC4245_3VSENSE);
+static SENSOR_DEVICE_ATTR(curr4_input, S_IRUGO, ltc4245_show_current, NULL,
+			  LTC4245_VEESENSE);
 
 /* Overcurrent alarms */
-LTC4245_ALARM(curr1_max_alarm,	(1 << 4),	LTC4245_FAULT1);
-LTC4245_ALARM(curr2_max_alarm,	(1 << 5),	LTC4245_FAULT1);
-LTC4245_ALARM(curr3_max_alarm,	(1 << 6),	LTC4245_FAULT1);
-LTC4245_ALARM(curr4_max_alarm,	(1 << 7),	LTC4245_FAULT1);
+static SENSOR_DEVICE_ATTR_2(curr1_max_alarm, S_IRUGO, ltc4245_show_alarm, NULL,
+			    1 << 4, LTC4245_FAULT1);
+static SENSOR_DEVICE_ATTR_2(curr2_max_alarm, S_IRUGO, ltc4245_show_alarm, NULL,
+			    1 << 5, LTC4245_FAULT1);
+static SENSOR_DEVICE_ATTR_2(curr3_max_alarm, S_IRUGO, ltc4245_show_alarm, NULL,
+			    1 << 6, LTC4245_FAULT1);
+static SENSOR_DEVICE_ATTR_2(curr4_max_alarm, S_IRUGO, ltc4245_show_alarm, NULL,
+			    1 << 7, LTC4245_FAULT1);
 
 /* Output voltages */
-LTC4245_VOLTAGE(in5_input,			LTC4245_12VOUT);
-LTC4245_VOLTAGE(in6_input,			LTC4245_5VOUT);
-LTC4245_VOLTAGE(in7_input,			LTC4245_3VOUT);
-LTC4245_VOLTAGE(in8_input,			LTC4245_VEEOUT);
+static SENSOR_DEVICE_ATTR(in5_input, S_IRUGO, ltc4245_show_voltage, NULL,
+			  LTC4245_12VOUT);
+static SENSOR_DEVICE_ATTR(in6_input, S_IRUGO, ltc4245_show_voltage, NULL,
+			  LTC4245_5VOUT);
+static SENSOR_DEVICE_ATTR(in7_input, S_IRUGO, ltc4245_show_voltage, NULL,
+			  LTC4245_3VOUT);
+static SENSOR_DEVICE_ATTR(in8_input, S_IRUGO, ltc4245_show_voltage, NULL,
+			  LTC4245_VEEOUT);
 
 /* Power Bad alarms */
-LTC4245_ALARM(in5_min_alarm,	(1 << 0),	LTC4245_FAULT2);
-LTC4245_ALARM(in6_min_alarm,	(1 << 1),	LTC4245_FAULT2);
-LTC4245_ALARM(in7_min_alarm,	(1 << 2),	LTC4245_FAULT2);
-LTC4245_ALARM(in8_min_alarm,	(1 << 3),	LTC4245_FAULT2);
+static SENSOR_DEVICE_ATTR_2(in5_min_alarm, S_IRUGO, ltc4245_show_alarm, NULL,
+			    1 << 0, LTC4245_FAULT2);
+static SENSOR_DEVICE_ATTR_2(in6_min_alarm, S_IRUGO, ltc4245_show_alarm, NULL,
+			    1 << 1, LTC4245_FAULT2);
+static SENSOR_DEVICE_ATTR_2(in7_min_alarm, S_IRUGO, ltc4245_show_alarm, NULL,
+			    1 << 2, LTC4245_FAULT2);
+static SENSOR_DEVICE_ATTR_2(in8_min_alarm, S_IRUGO, ltc4245_show_alarm, NULL,
+			    1 << 3, LTC4245_FAULT2);
 
 /* GPIO voltages */
-LTC4245_GPIO_VOLTAGE(in9_input,			0);
-LTC4245_GPIO_VOLTAGE(in10_input,		1);
-LTC4245_GPIO_VOLTAGE(in11_input,		2);
+static SENSOR_DEVICE_ATTR(in9_input, S_IRUGO, ltc4245_show_gpio, NULL, 0);
+static SENSOR_DEVICE_ATTR(in10_input, S_IRUGO, ltc4245_show_gpio, NULL, 1);
+static SENSOR_DEVICE_ATTR(in11_input, S_IRUGO, ltc4245_show_gpio, NULL, 2);
 
 /* Power Consumption (virtual) */
-LTC4245_POWER(power1_input,			LTC4245_12VSENSE);
-LTC4245_POWER(power2_input,			LTC4245_5VSENSE);
-LTC4245_POWER(power3_input,			LTC4245_3VSENSE);
-LTC4245_POWER(power4_input,			LTC4245_VEESENSE);
+static SENSOR_DEVICE_ATTR(power1_input, S_IRUGO, ltc4245_show_power, NULL,
+			  LTC4245_12VSENSE);
+static SENSOR_DEVICE_ATTR(power2_input, S_IRUGO, ltc4245_show_power, NULL,
+			  LTC4245_5VSENSE);
+static SENSOR_DEVICE_ATTR(power3_input, S_IRUGO, ltc4245_show_power, NULL,
+			  LTC4245_3VSENSE);
+static SENSOR_DEVICE_ATTR(power4_input, S_IRUGO, ltc4245_show_power, NULL,
+			  LTC4245_VEESENSE);
 
 /*
  * Finally, construct an array of pointers to members of the above objects,
diff --git a/drivers/hwmon/ltc4261.c b/drivers/hwmon/ltc4261.c
index 84a2d2872b2..487da58ec86 100644
--- a/drivers/hwmon/ltc4261.c
+++ b/drivers/hwmon/ltc4261.c
@@ -165,24 +165,12 @@ static ssize_t ltc4261_show_bool(struct device *dev,
 }
 
 /*
- * These macros are used below in constructing device attribute objects
- * for use with sysfs_create_group() to make a sysfs device file
- * for each register.
- */
-
-#define LTC4261_VALUE(name, ltc4261_cmd_idx) \
-	static SENSOR_DEVICE_ATTR(name, S_IRUGO, \
-	ltc4261_show_value, NULL, ltc4261_cmd_idx)
-
-#define LTC4261_BOOL(name, mask) \
-	static SENSOR_DEVICE_ATTR(name, S_IRUGO, \
-	ltc4261_show_bool, NULL, (mask))
-
-/*
  * Input voltages.
  */
-LTC4261_VALUE(in1_input, LTC4261_ADIN_H);
-LTC4261_VALUE(in2_input, LTC4261_ADIN2_H);
+static SENSOR_DEVICE_ATTR(in1_input, S_IRUGO, ltc4261_show_value, NULL,
+			  LTC4261_ADIN_H);
+static SENSOR_DEVICE_ATTR(in2_input, S_IRUGO, ltc4261_show_value, NULL,
+			  LTC4261_ADIN2_H);
 
 /*
  * Voltage alarms. The chip has only one set of voltage alarm status bits,
@@ -192,16 +180,22 @@ LTC4261_VALUE(in2_input, LTC4261_ADIN2_H);
  * To ensure that the alarm condition is reported to the user, report it
  * with both voltage sensors.
  */
-LTC4261_BOOL(in1_min_alarm, FAULT_UV);
-LTC4261_BOOL(in1_max_alarm, FAULT_OV);
-LTC4261_BOOL(in2_min_alarm, FAULT_UV);
-LTC4261_BOOL(in2_max_alarm, FAULT_OV);
+static SENSOR_DEVICE_ATTR(in1_min_alarm, S_IRUGO, ltc4261_show_bool, NULL,
+			  FAULT_UV);
+static SENSOR_DEVICE_ATTR(in1_max_alarm, S_IRUGO, ltc4261_show_bool, NULL,
+			  FAULT_OV);
+static SENSOR_DEVICE_ATTR(in2_min_alarm, S_IRUGO, ltc4261_show_bool, NULL,
+			  FAULT_UV);
+static SENSOR_DEVICE_ATTR(in2_max_alarm, S_IRUGO, ltc4261_show_bool, NULL,
+			  FAULT_OV);
 
 /* Currents (via sense resistor) */
-LTC4261_VALUE(curr1_input, LTC4261_SENSE_H);
+static SENSOR_DEVICE_ATTR(curr1_input, S_IRUGO, ltc4261_show_value, NULL,
+			  LTC4261_SENSE_H);
 
 /* Overcurrent alarm */
-LTC4261_BOOL(curr1_max_alarm, FAULT_OC);
+static SENSOR_DEVICE_ATTR(curr1_max_alarm, S_IRUGO, ltc4261_show_bool, NULL,
+			  FAULT_OC);
 
 static struct attribute *ltc4261_attributes[] = {
 	&sensor_dev_attr_in1_input.dev_attr.attr,
diff --git a/drivers/hwmon/max6697.c b/drivers/hwmon/max6697.c
index bf4aa3777fc..328fb0353c1 100644
--- a/drivers/hwmon/max6697.c
+++ b/drivers/hwmon/max6697.c
@@ -399,82 +399,95 @@ static SENSOR_DEVICE_ATTR(temp6_fault, S_IRUGO, show_alarm, NULL, 5);
 static SENSOR_DEVICE_ATTR(temp7_fault, S_IRUGO, show_alarm, NULL, 6);
 static SENSOR_DEVICE_ATTR(temp8_fault, S_IRUGO, show_alarm, NULL, 7);
 
-static struct attribute *max6697_attributes[8][7] = {
-	{
-		&sensor_dev_attr_temp1_input.dev_attr.attr,
-		&sensor_dev_attr_temp1_max.dev_attr.attr,
-		&sensor_dev_attr_temp1_max_alarm.dev_attr.attr,
-		&sensor_dev_attr_temp1_crit.dev_attr.attr,
-		&sensor_dev_attr_temp1_crit_alarm.dev_attr.attr,
-		NULL
-	}, {
-		&sensor_dev_attr_temp2_input.dev_attr.attr,
-		&sensor_dev_attr_temp2_max.dev_attr.attr,
-		&sensor_dev_attr_temp2_max_alarm.dev_attr.attr,
-		&sensor_dev_attr_temp2_crit.dev_attr.attr,
-		&sensor_dev_attr_temp2_crit_alarm.dev_attr.attr,
-		&sensor_dev_attr_temp2_fault.dev_attr.attr,
-		NULL
-	}, {
-		&sensor_dev_attr_temp3_input.dev_attr.attr,
-		&sensor_dev_attr_temp3_max.dev_attr.attr,
-		&sensor_dev_attr_temp3_max_alarm.dev_attr.attr,
-		&sensor_dev_attr_temp3_crit.dev_attr.attr,
-		&sensor_dev_attr_temp3_crit_alarm.dev_attr.attr,
-		&sensor_dev_attr_temp3_fault.dev_attr.attr,
-		NULL
-	}, {
-		&sensor_dev_attr_temp4_input.dev_attr.attr,
-		&sensor_dev_attr_temp4_max.dev_attr.attr,
-		&sensor_dev_attr_temp4_max_alarm.dev_attr.attr,
-		&sensor_dev_attr_temp4_crit.dev_attr.attr,
-		&sensor_dev_attr_temp4_crit_alarm.dev_attr.attr,
-		&sensor_dev_attr_temp4_fault.dev_attr.attr,
-		NULL
-	}, {
-		&sensor_dev_attr_temp5_input.dev_attr.attr,
-		&sensor_dev_attr_temp5_max.dev_attr.attr,
-		&sensor_dev_attr_temp5_max_alarm.dev_attr.attr,
-		&sensor_dev_attr_temp5_crit.dev_attr.attr,
-		&sensor_dev_attr_temp5_crit_alarm.dev_attr.attr,
-		&sensor_dev_attr_temp5_fault.dev_attr.attr,
-		NULL
-	}, {
-		&sensor_dev_attr_temp6_input.dev_attr.attr,
-		&sensor_dev_attr_temp6_max.dev_attr.attr,
-		&sensor_dev_attr_temp6_max_alarm.dev_attr.attr,
-		&sensor_dev_attr_temp6_crit.dev_attr.attr,
-		&sensor_dev_attr_temp6_crit_alarm.dev_attr.attr,
-		&sensor_dev_attr_temp6_fault.dev_attr.attr,
-		NULL
-	}, {
-		&sensor_dev_attr_temp7_input.dev_attr.attr,
-		&sensor_dev_attr_temp7_max.dev_attr.attr,
-		&sensor_dev_attr_temp7_max_alarm.dev_attr.attr,
-		&sensor_dev_attr_temp7_crit.dev_attr.attr,
-		&sensor_dev_attr_temp7_crit_alarm.dev_attr.attr,
-		&sensor_dev_attr_temp7_fault.dev_attr.attr,
-		NULL
-	}, {
-		&sensor_dev_attr_temp8_input.dev_attr.attr,
-		&sensor_dev_attr_temp8_max.dev_attr.attr,
-		&sensor_dev_attr_temp8_max_alarm.dev_attr.attr,
-		&sensor_dev_attr_temp8_crit.dev_attr.attr,
-		&sensor_dev_attr_temp8_crit_alarm.dev_attr.attr,
-		&sensor_dev_attr_temp8_fault.dev_attr.attr,
-		NULL
-	}
+static DEVICE_ATTR(dummy, 0, NULL, NULL);
+
+static umode_t max6697_is_visible(struct kobject *kobj, struct attribute *attr,
+				  int index)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct i2c_client *client = to_i2c_client(dev);
+	struct max6697_data *data = i2c_get_clientdata(client);
+	const struct max6697_chip_data *chip = data->chip;
+	int channel = index / 6;	/* channel number */
+	int nr = index % 6;		/* attribute index within channel */
+
+	if (channel >= chip->channels)
+		return 0;
+
+	if ((nr == 3 || nr == 4) && !(chip->have_crit & (1 << channel)))
+		return 0;
+	if (nr == 5 && !(chip->have_fault & (1 << channel)))
+		return 0;
+
+	return attr->mode;
+}
+
+/*
+ * max6697_is_visible uses the index into the following array to determine
+ * if attributes should be created or not. Any change in order or content
+ * must be matched in max6697_is_visible.
+ */
+static struct attribute *max6697_attributes[] = {
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	&sensor_dev_attr_temp1_max.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit_alarm.dev_attr.attr,
+	&dev_attr_dummy.attr,
+
+	&sensor_dev_attr_temp2_input.dev_attr.attr,
+	&sensor_dev_attr_temp2_max.dev_attr.attr,
+	&sensor_dev_attr_temp2_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp2_crit.dev_attr.attr,
+	&sensor_dev_attr_temp2_crit_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp2_fault.dev_attr.attr,
+
+	&sensor_dev_attr_temp3_input.dev_attr.attr,
+	&sensor_dev_attr_temp3_max.dev_attr.attr,
+	&sensor_dev_attr_temp3_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp3_crit.dev_attr.attr,
+	&sensor_dev_attr_temp3_crit_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp3_fault.dev_attr.attr,
+
+	&sensor_dev_attr_temp4_input.dev_attr.attr,
+	&sensor_dev_attr_temp4_max.dev_attr.attr,
+	&sensor_dev_attr_temp4_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp4_crit.dev_attr.attr,
+	&sensor_dev_attr_temp4_crit_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp4_fault.dev_attr.attr,
+
+	&sensor_dev_attr_temp5_input.dev_attr.attr,
+	&sensor_dev_attr_temp5_max.dev_attr.attr,
+	&sensor_dev_attr_temp5_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp5_crit.dev_attr.attr,
+	&sensor_dev_attr_temp5_crit_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp5_fault.dev_attr.attr,
+
+	&sensor_dev_attr_temp6_input.dev_attr.attr,
+	&sensor_dev_attr_temp6_max.dev_attr.attr,
+	&sensor_dev_attr_temp6_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp6_crit.dev_attr.attr,
+	&sensor_dev_attr_temp6_crit_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp6_fault.dev_attr.attr,
+
+	&sensor_dev_attr_temp7_input.dev_attr.attr,
+	&sensor_dev_attr_temp7_max.dev_attr.attr,
+	&sensor_dev_attr_temp7_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp7_crit.dev_attr.attr,
+	&sensor_dev_attr_temp7_crit_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp7_fault.dev_attr.attr,
+
+	&sensor_dev_attr_temp8_input.dev_attr.attr,
+	&sensor_dev_attr_temp8_max.dev_attr.attr,
+	&sensor_dev_attr_temp8_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp8_crit.dev_attr.attr,
+	&sensor_dev_attr_temp8_crit_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp8_fault.dev_attr.attr,
+	NULL
 };
 
-static const struct attribute_group max6697_group[8] = {
-	{ .attrs = max6697_attributes[0] },
-	{ .attrs = max6697_attributes[1] },
-	{ .attrs = max6697_attributes[2] },
-	{ .attrs = max6697_attributes[3] },
-	{ .attrs = max6697_attributes[4] },
-	{ .attrs = max6697_attributes[5] },
-	{ .attrs = max6697_attributes[6] },
-	{ .attrs = max6697_attributes[7] },
+static const struct attribute_group max6697_group = {
+	.attrs = max6697_attributes, .is_visible = max6697_is_visible,
 };
 
 static void max6697_get_config_of(struct device_node *node,
@@ -606,21 +619,13 @@ done:
 	return 0;
 }
 
-static void max6697_remove_files(struct i2c_client *client)
-{
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(max6697_group); i++)
-		sysfs_remove_group(&client->dev.kobj, &max6697_group[i]);
-}
-
 static int max6697_probe(struct i2c_client *client,
 			 const struct i2c_device_id *id)
 {
 	struct i2c_adapter *adapter = client->adapter;
 	struct device *dev = &client->dev;
 	struct max6697_data *data;
-	int i, err;
+	int err;
 
 	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
 		return -ENODEV;
@@ -639,37 +644,9 @@ static int max6697_probe(struct i2c_client *client,
 	if (err)
 		return err;
 
-	for (i = 0; i < data->chip->channels; i++) {
-		err = sysfs_create_file(&dev->kobj,
-					max6697_attributes[i][0]);
-		if (err)
-			goto error;
-		err = sysfs_create_file(&dev->kobj,
-					max6697_attributes[i][1]);
-		if (err)
-			goto error;
-		err = sysfs_create_file(&dev->kobj,
-					max6697_attributes[i][2]);
-		if (err)
-			goto error;
-
-		if (data->chip->have_crit & (1 << i)) {
-			err = sysfs_create_file(&dev->kobj,
-						max6697_attributes[i][3]);
-			if (err)
-				goto error;
-			err = sysfs_create_file(&dev->kobj,
-						max6697_attributes[i][4]);
-			if (err)
-				goto error;
-		}
-		if (data->chip->have_fault & (1 << i)) {
-			err = sysfs_create_file(&dev->kobj,
-						max6697_attributes[i][5]);
-			if (err)
-				goto error;
-		}
-	}
+	err = sysfs_create_group(&client->dev.kobj, &max6697_group);
+	if (err)
+		return err;
 
 	data->hwmon_dev = hwmon_device_register(dev);
 	if (IS_ERR(data->hwmon_dev)) {
@@ -680,7 +657,7 @@ static int max6697_probe(struct i2c_client *client,
 	return 0;
 
 error:
-	max6697_remove_files(client);
+	sysfs_remove_group(&client->dev.kobj, &max6697_group);
 	return err;
 }
 
@@ -689,7 +666,7 @@ static int max6697_remove(struct i2c_client *client)
 	struct max6697_data *data = i2c_get_clientdata(client);
 
 	hwmon_device_unregister(data->hwmon_dev);
-	max6697_remove_files(client);
+	sysfs_remove_group(&client->dev.kobj, &max6697_group);
 
 	return 0;
 }
diff --git a/drivers/hwmon/mc13783-adc.c b/drivers/hwmon/mc13783-adc.c
index 2a7f331cd3c..982d8622c09 100644
--- a/drivers/hwmon/mc13783-adc.c
+++ b/drivers/hwmon/mc13783-adc.c
@@ -273,18 +273,7 @@ static struct platform_driver mc13783_adc_driver = {
 	.id_table	= mc13783_adc_idtable,
 };
 
-static int __init mc13783_adc_init(void)
-{
-	return platform_driver_probe(&mc13783_adc_driver, mc13783_adc_probe);
-}
-
-static void __exit mc13783_adc_exit(void)
-{
-	platform_driver_unregister(&mc13783_adc_driver);
-}
-
-module_init(mc13783_adc_init);
-module_exit(mc13783_adc_exit);
+module_platform_driver_probe(mc13783_adc_driver, mc13783_adc_probe);
 
 MODULE_DESCRIPTION("MC13783 ADC driver");
 MODULE_AUTHOR("Luotao Fu <l.fu@pengutronix.de>");
diff --git a/drivers/hwmon/nct6775.c b/drivers/hwmon/nct6775.c
new file mode 100644
index 00000000000..f43f5e571db
--- /dev/null
+++ b/drivers/hwmon/nct6775.c
@@ -0,0 +1,4191 @@
+/*
+ * nct6775 - Driver for the hardware monitoring functionality of
+ *	       Nuvoton NCT677x Super-I/O chips
+ *
+ * Copyright (C) 2012  Guenter Roeck <linux@roeck-us.net>
+ *
+ * Derived from w83627ehf driver
+ * Copyright (C) 2005-2012  Jean Delvare <khali@linux-fr.org>
+ * Copyright (C) 2006  Yuan Mu (Winbond),
+ *		       Rudolf Marek <r.marek@assembler.cz>
+ *		       David Hubbard <david.c.hubbard@gmail.com>
+ *		       Daniel J Blueman <daniel.blueman@gmail.com>
+ * Copyright (C) 2010  Sheng-Yuan Huang (Nuvoton) (PS00)
+ *
+ * Shamelessly ripped from the w83627hf driver
+ * Copyright (C) 2003  Mark Studebaker
+ *
+ * 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.
+ *
+ *
+ * Supports the following chips:
+ *
+ * Chip        #vin    #fan    #pwm    #temp  chip IDs       man ID
+ * nct6775f     9      4       3       6+3    0xb470 0xc1    0x5ca3
+ * nct6776f     9      5       3       6+3    0xc330 0xc1    0x5ca3
+ * nct6779d    15      5       5       2+6    0xc560 0xc1    0x5ca3
+ *
+ * #temp lists the number of monitored temperature sources (first value) plus
+ * the number of directly connectable temperature sensors (second value).
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/jiffies.h>
+#include <linux/platform_device.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/hwmon-vid.h>
+#include <linux/err.h>
+#include <linux/mutex.h>
+#include <linux/acpi.h>
+#include <linux/io.h>
+#include "lm75.h"
+
+#define USE_ALTERNATE
+
+enum kinds { nct6775, nct6776, nct6779 };
+
+/* used to set data->name = nct6775_device_names[data->sio_kind] */
+static const char * const nct6775_device_names[] = {
+	"nct6775",
+	"nct6776",
+	"nct6779",
+};
+
+static unsigned short force_id;
+module_param(force_id, ushort, 0);
+MODULE_PARM_DESC(force_id, "Override the detected device ID");
+
+static unsigned short fan_debounce;
+module_param(fan_debounce, ushort, 0);
+MODULE_PARM_DESC(fan_debounce, "Enable debouncing for fan RPM signal");
+
+#define DRVNAME "nct6775"
+
+/*
+ * Super-I/O constants and functions
+ */
+
+#define NCT6775_LD_ACPI		0x0a
+#define NCT6775_LD_HWM		0x0b
+#define NCT6775_LD_VID		0x0d
+
+#define SIO_REG_LDSEL		0x07	/* Logical device select */
+#define SIO_REG_DEVID		0x20	/* Device ID (2 bytes) */
+#define SIO_REG_ENABLE		0x30	/* Logical device enable */
+#define SIO_REG_ADDR		0x60	/* Logical device address (2 bytes) */
+
+#define SIO_NCT6775_ID		0xb470
+#define SIO_NCT6776_ID		0xc330
+#define SIO_NCT6779_ID		0xc560
+#define SIO_ID_MASK		0xFFF0
+
+enum pwm_enable { off, manual, thermal_cruise, speed_cruise, sf3, sf4 };
+
+static inline void
+superio_outb(int ioreg, int reg, int val)
+{
+	outb(reg, ioreg);
+	outb(val, ioreg + 1);
+}
+
+static inline int
+superio_inb(int ioreg, int reg)
+{
+	outb(reg, ioreg);
+	return inb(ioreg + 1);
+}
+
+static inline void
+superio_select(int ioreg, int ld)
+{
+	outb(SIO_REG_LDSEL, ioreg);
+	outb(ld, ioreg + 1);
+}
+
+static inline int
+superio_enter(int ioreg)
+{
+	/*
+	 * Try to reserve <ioreg> and <ioreg + 1> for exclusive access.
+	 */
+	if (!request_muxed_region(ioreg, 2, DRVNAME))
+		return -EBUSY;
+
+	outb(0x87, ioreg);
+	outb(0x87, ioreg);
+
+	return 0;
+}
+
+static inline void
+superio_exit(int ioreg)
+{
+	outb(0xaa, ioreg);
+	outb(0x02, ioreg);
+	outb(0x02, ioreg + 1);
+	release_region(ioreg, 2);
+}
+
+/*
+ * ISA constants
+ */
+
+#define IOREGION_ALIGNMENT	(~7)
+#define IOREGION_OFFSET		5
+#define IOREGION_LENGTH		2
+#define ADDR_REG_OFFSET		0
+#define DATA_REG_OFFSET		1
+
+#define NCT6775_REG_BANK	0x4E
+#define NCT6775_REG_CONFIG	0x40
+
+/*
+ * Not currently used:
+ * REG_MAN_ID has the value 0x5ca3 for all supported chips.
+ * REG_CHIP_ID == 0x88/0xa1/0xc1 depending on chip model.
+ * REG_MAN_ID is at port 0x4f
+ * REG_CHIP_ID is at port 0x58
+ */
+
+#define NUM_TEMP	10	/* Max number of temp attribute sets w/ limits*/
+#define NUM_TEMP_FIXED	6	/* Max number of fixed temp attribute sets */
+
+#define NUM_REG_ALARM	4	/* Max number of alarm registers */
+
+/* Common and NCT6775 specific data */
+
+/* Voltage min/max registers for nr=7..14 are in bank 5 */
+
+static const u16 NCT6775_REG_IN_MAX[] = {
+	0x2b, 0x2d, 0x2f, 0x31, 0x33, 0x35, 0x37, 0x554, 0x556, 0x558, 0x55a,
+	0x55c, 0x55e, 0x560, 0x562 };
+static const u16 NCT6775_REG_IN_MIN[] = {
+	0x2c, 0x2e, 0x30, 0x32, 0x34, 0x36, 0x38, 0x555, 0x557, 0x559, 0x55b,
+	0x55d, 0x55f, 0x561, 0x563 };
+static const u16 NCT6775_REG_IN[] = {
+	0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, 0x550, 0x551, 0x552
+};
+
+#define NCT6775_REG_VBAT		0x5D
+#define NCT6775_REG_DIODE		0x5E
+
+#define NCT6775_REG_FANDIV1		0x506
+#define NCT6775_REG_FANDIV2		0x507
+
+#define NCT6775_REG_CR_FAN_DEBOUNCE	0xf0
+
+static const u16 NCT6775_REG_ALARM[NUM_REG_ALARM] = { 0x459, 0x45A, 0x45B };
+
+/* 0..15 voltages, 16..23 fans, 24..31 temperatures */
+
+static const s8 NCT6775_ALARM_BITS[] = {
+	0, 1, 2, 3, 8, 21, 20, 16,	/* in0.. in7 */
+	17, -1, -1, -1, -1, -1, -1,	/* in8..in14 */
+	-1,				/* unused */
+	6, 7, 11, 10, 23,		/* fan1..fan5 */
+	-1, -1, -1,			/* unused */
+	4, 5, 13, -1, -1, -1,		/* temp1..temp6 */
+	12, -1 };			/* intrusion0, intrusion1 */
+
+#define FAN_ALARM_BASE		16
+#define TEMP_ALARM_BASE		24
+#define INTRUSION_ALARM_BASE	30
+
+static const u8 NCT6775_REG_CR_CASEOPEN_CLR[] = { 0xe6, 0xee };
+static const u8 NCT6775_CR_CASEOPEN_CLR_MASK[] = { 0x20, 0x01 };
+
+/* DC or PWM output fan configuration */
+static const u8 NCT6775_REG_PWM_MODE[] = { 0x04, 0x04, 0x12 };
+static const u8 NCT6775_PWM_MODE_MASK[] = { 0x01, 0x02, 0x01 };
+
+/* Advanced Fan control, some values are common for all fans */
+
+static const u16 NCT6775_REG_TARGET[] = { 0x101, 0x201, 0x301, 0x801, 0x901 };
+static const u16 NCT6775_REG_FAN_MODE[] = { 0x102, 0x202, 0x302, 0x802, 0x902 };
+static const u16 NCT6775_REG_FAN_STEP_DOWN_TIME[] = {
+	0x103, 0x203, 0x303, 0x803, 0x903 };
+static const u16 NCT6775_REG_FAN_STEP_UP_TIME[] = {
+	0x104, 0x204, 0x304, 0x804, 0x904 };
+static const u16 NCT6775_REG_FAN_STOP_OUTPUT[] = {
+	0x105, 0x205, 0x305, 0x805, 0x905 };
+static const u16 NCT6775_REG_FAN_START_OUTPUT[]
+	= { 0x106, 0x206, 0x306, 0x806, 0x906 };
+static const u16 NCT6775_REG_FAN_MAX_OUTPUT[] = { 0x10a, 0x20a, 0x30a };
+static const u16 NCT6775_REG_FAN_STEP_OUTPUT[] = { 0x10b, 0x20b, 0x30b };
+
+static const u16 NCT6775_REG_FAN_STOP_TIME[] = {
+	0x107, 0x207, 0x307, 0x807, 0x907 };
+static const u16 NCT6775_REG_PWM[] = { 0x109, 0x209, 0x309, 0x809, 0x909 };
+static const u16 NCT6775_REG_PWM_READ[] = { 0x01, 0x03, 0x11, 0x13, 0x15 };
+
+static const u16 NCT6775_REG_FAN[] = { 0x630, 0x632, 0x634, 0x636, 0x638 };
+static const u16 NCT6775_REG_FAN_MIN[] = { 0x3b, 0x3c, 0x3d };
+static const u16 NCT6775_REG_FAN_PULSES[] = { 0x641, 0x642, 0x643, 0x644, 0 };
+
+static const u16 NCT6775_REG_TEMP[] = {
+	0x27, 0x150, 0x250, 0x62b, 0x62c, 0x62d };
+
+static const u16 NCT6775_REG_TEMP_CONFIG[ARRAY_SIZE(NCT6775_REG_TEMP)] = {
+	0, 0x152, 0x252, 0x628, 0x629, 0x62A };
+static const u16 NCT6775_REG_TEMP_HYST[ARRAY_SIZE(NCT6775_REG_TEMP)] = {
+	0x3a, 0x153, 0x253, 0x673, 0x678, 0x67D };
+static const u16 NCT6775_REG_TEMP_OVER[ARRAY_SIZE(NCT6775_REG_TEMP)] = {
+	0x39, 0x155, 0x255, 0x672, 0x677, 0x67C };
+
+static const u16 NCT6775_REG_TEMP_SOURCE[ARRAY_SIZE(NCT6775_REG_TEMP)] = {
+	0x621, 0x622, 0x623, 0x624, 0x625, 0x626 };
+
+static const u16 NCT6775_REG_TEMP_SEL[] = {
+	0x100, 0x200, 0x300, 0x800, 0x900 };
+
+static const u16 NCT6775_REG_WEIGHT_TEMP_SEL[] = {
+	0x139, 0x239, 0x339, 0x839, 0x939 };
+static const u16 NCT6775_REG_WEIGHT_TEMP_STEP[] = {
+	0x13a, 0x23a, 0x33a, 0x83a, 0x93a };
+static const u16 NCT6775_REG_WEIGHT_TEMP_STEP_TOL[] = {
+	0x13b, 0x23b, 0x33b, 0x83b, 0x93b };
+static const u16 NCT6775_REG_WEIGHT_DUTY_STEP[] = {
+	0x13c, 0x23c, 0x33c, 0x83c, 0x93c };
+static const u16 NCT6775_REG_WEIGHT_TEMP_BASE[] = {
+	0x13d, 0x23d, 0x33d, 0x83d, 0x93d };
+
+static const u16 NCT6775_REG_TEMP_OFFSET[] = { 0x454, 0x455, 0x456 };
+
+static const u16 NCT6775_REG_AUTO_TEMP[] = {
+	0x121, 0x221, 0x321, 0x821, 0x921 };
+static const u16 NCT6775_REG_AUTO_PWM[] = {
+	0x127, 0x227, 0x327, 0x827, 0x927 };
+
+#define NCT6775_AUTO_TEMP(data, nr, p)	((data)->REG_AUTO_TEMP[nr] + (p))
+#define NCT6775_AUTO_PWM(data, nr, p)	((data)->REG_AUTO_PWM[nr] + (p))
+
+static const u16 NCT6775_REG_CRITICAL_ENAB[] = { 0x134, 0x234, 0x334 };
+
+static const u16 NCT6775_REG_CRITICAL_TEMP[] = {
+	0x135, 0x235, 0x335, 0x835, 0x935 };
+static const u16 NCT6775_REG_CRITICAL_TEMP_TOLERANCE[] = {
+	0x138, 0x238, 0x338, 0x838, 0x938 };
+
+static const char *const nct6775_temp_label[] = {
+	"",
+	"SYSTIN",
+	"CPUTIN",
+	"AUXTIN",
+	"AMD SB-TSI",
+	"PECI Agent 0",
+	"PECI Agent 1",
+	"PECI Agent 2",
+	"PECI Agent 3",
+	"PECI Agent 4",
+	"PECI Agent 5",
+	"PECI Agent 6",
+	"PECI Agent 7",
+	"PCH_CHIP_CPU_MAX_TEMP",
+	"PCH_CHIP_TEMP",
+	"PCH_CPU_TEMP",
+	"PCH_MCH_TEMP",
+	"PCH_DIM0_TEMP",
+	"PCH_DIM1_TEMP",
+	"PCH_DIM2_TEMP",
+	"PCH_DIM3_TEMP"
+};
+
+static const u16 NCT6775_REG_TEMP_ALTERNATE[ARRAY_SIZE(nct6775_temp_label) - 1]
+	= { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0x661, 0x662, 0x664 };
+
+static const u16 NCT6775_REG_TEMP_CRIT[ARRAY_SIZE(nct6775_temp_label) - 1]
+	= { 0, 0, 0, 0, 0xa00, 0xa01, 0xa02, 0xa03, 0xa04, 0xa05, 0xa06,
+	    0xa07 };
+
+/* NCT6776 specific data */
+
+static const s8 NCT6776_ALARM_BITS[] = {
+	0, 1, 2, 3, 8, 21, 20, 16,	/* in0.. in7 */
+	17, -1, -1, -1, -1, -1, -1,	/* in8..in14 */
+	-1,				/* unused */
+	6, 7, 11, 10, 23,		/* fan1..fan5 */
+	-1, -1, -1,			/* unused */
+	4, 5, 13, -1, -1, -1,		/* temp1..temp6 */
+	12, 9 };			/* intrusion0, intrusion1 */
+
+static const u16 NCT6776_REG_TOLERANCE_H[] = {
+	0x10c, 0x20c, 0x30c, 0x80c, 0x90c };
+
+static const u8 NCT6776_REG_PWM_MODE[] = { 0x04, 0, 0 };
+static const u8 NCT6776_PWM_MODE_MASK[] = { 0x01, 0, 0 };
+
+static const u16 NCT6776_REG_FAN_MIN[] = { 0x63a, 0x63c, 0x63e, 0x640, 0x642 };
+static const u16 NCT6776_REG_FAN_PULSES[] = { 0x644, 0x645, 0x646, 0, 0 };
+
+static const u16 NCT6776_REG_WEIGHT_DUTY_BASE[] = {
+	0x13e, 0x23e, 0x33e, 0x83e, 0x93e };
+
+static const u16 NCT6776_REG_TEMP_CONFIG[ARRAY_SIZE(NCT6775_REG_TEMP)] = {
+	0x18, 0x152, 0x252, 0x628, 0x629, 0x62A };
+
+static const char *const nct6776_temp_label[] = {
+	"",
+	"SYSTIN",
+	"CPUTIN",
+	"AUXTIN",
+	"SMBUSMASTER 0",
+	"SMBUSMASTER 1",
+	"SMBUSMASTER 2",
+	"SMBUSMASTER 3",
+	"SMBUSMASTER 4",
+	"SMBUSMASTER 5",
+	"SMBUSMASTER 6",
+	"SMBUSMASTER 7",
+	"PECI Agent 0",
+	"PECI Agent 1",
+	"PCH_CHIP_CPU_MAX_TEMP",
+	"PCH_CHIP_TEMP",
+	"PCH_CPU_TEMP",
+	"PCH_MCH_TEMP",
+	"PCH_DIM0_TEMP",
+	"PCH_DIM1_TEMP",
+	"PCH_DIM2_TEMP",
+	"PCH_DIM3_TEMP",
+	"BYTE_TEMP"
+};
+
+static const u16 NCT6776_REG_TEMP_ALTERNATE[ARRAY_SIZE(nct6776_temp_label) - 1]
+	= { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0x401, 0x402, 0x404 };
+
+static const u16 NCT6776_REG_TEMP_CRIT[ARRAY_SIZE(nct6776_temp_label) - 1]
+	= { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0x709, 0x70a };
+
+/* NCT6779 specific data */
+
+static const u16 NCT6779_REG_IN[] = {
+	0x480, 0x481, 0x482, 0x483, 0x484, 0x485, 0x486, 0x487,
+	0x488, 0x489, 0x48a, 0x48b, 0x48c, 0x48d, 0x48e };
+
+static const u16 NCT6779_REG_ALARM[NUM_REG_ALARM] = {
+	0x459, 0x45A, 0x45B, 0x568 };
+
+static const s8 NCT6779_ALARM_BITS[] = {
+	0, 1, 2, 3, 8, 21, 20, 16,	/* in0.. in7 */
+	17, 24, 25, 26, 27, 28, 29,	/* in8..in14 */
+	-1,				/* unused */
+	6, 7, 11, 10, 23,		/* fan1..fan5 */
+	-1, -1, -1,			/* unused */
+	4, 5, 13, -1, -1, -1,		/* temp1..temp6 */
+	12, 9 };			/* intrusion0, intrusion1 */
+
+static const u16 NCT6779_REG_FAN[] = { 0x4b0, 0x4b2, 0x4b4, 0x4b6, 0x4b8 };
+static const u16 NCT6779_REG_FAN_PULSES[] = {
+	0x644, 0x645, 0x646, 0x647, 0x648 };
+
+static const u16 NCT6779_REG_CRITICAL_PWM_ENABLE[] = {
+	0x136, 0x236, 0x336, 0x836, 0x936 };
+static const u16 NCT6779_REG_CRITICAL_PWM[] = {
+	0x137, 0x237, 0x337, 0x837, 0x937 };
+
+static const u16 NCT6779_REG_TEMP[] = { 0x27, 0x150 };
+static const u16 NCT6779_REG_TEMP_CONFIG[ARRAY_SIZE(NCT6779_REG_TEMP)] = {
+	0x18, 0x152 };
+static const u16 NCT6779_REG_TEMP_HYST[ARRAY_SIZE(NCT6779_REG_TEMP)] = {
+	0x3a, 0x153 };
+static const u16 NCT6779_REG_TEMP_OVER[ARRAY_SIZE(NCT6779_REG_TEMP)] = {
+	0x39, 0x155 };
+
+static const u16 NCT6779_REG_TEMP_OFFSET[] = {
+	0x454, 0x455, 0x456, 0x44a, 0x44b, 0x44c };
+
+static const char *const nct6779_temp_label[] = {
+	"",
+	"SYSTIN",
+	"CPUTIN",
+	"AUXTIN0",
+	"AUXTIN1",
+	"AUXTIN2",
+	"AUXTIN3",
+	"",
+	"SMBUSMASTER 0",
+	"SMBUSMASTER 1",
+	"SMBUSMASTER 2",
+	"SMBUSMASTER 3",
+	"SMBUSMASTER 4",
+	"SMBUSMASTER 5",
+	"SMBUSMASTER 6",
+	"SMBUSMASTER 7",
+	"PECI Agent 0",
+	"PECI Agent 1",
+	"PCH_CHIP_CPU_MAX_TEMP",
+	"PCH_CHIP_TEMP",
+	"PCH_CPU_TEMP",
+	"PCH_MCH_TEMP",
+	"PCH_DIM0_TEMP",
+	"PCH_DIM1_TEMP",
+	"PCH_DIM2_TEMP",
+	"PCH_DIM3_TEMP",
+	"BYTE_TEMP"
+};
+
+static const u16 NCT6779_REG_TEMP_ALTERNATE[ARRAY_SIZE(nct6779_temp_label) - 1]
+	= { 0x490, 0x491, 0x492, 0x493, 0x494, 0x495, 0, 0,
+	    0, 0, 0, 0, 0, 0, 0, 0,
+	    0, 0x400, 0x401, 0x402, 0x404, 0x405, 0x406, 0x407,
+	    0x408, 0 };
+
+static const u16 NCT6779_REG_TEMP_CRIT[ARRAY_SIZE(nct6779_temp_label) - 1]
+	= { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0x709, 0x70a };
+
+static enum pwm_enable reg_to_pwm_enable(int pwm, int mode)
+{
+	if (mode == 0 && pwm == 255)
+		return off;
+	return mode + 1;
+}
+
+static int pwm_enable_to_reg(enum pwm_enable mode)
+{
+	if (mode == off)
+		return 0;
+	return mode - 1;
+}
+
+/*
+ * Conversions
+ */
+
+/* 1 is DC mode, output in ms */
+static unsigned int step_time_from_reg(u8 reg, u8 mode)
+{
+	return mode ? 400 * reg : 100 * reg;
+}
+
+static u8 step_time_to_reg(unsigned int msec, u8 mode)
+{
+	return clamp_val((mode ? (msec + 200) / 400 :
+					(msec + 50) / 100), 1, 255);
+}
+
+static unsigned int fan_from_reg8(u16 reg, unsigned int divreg)
+{
+	if (reg == 0 || reg == 255)
+		return 0;
+	return 1350000U / (reg << divreg);
+}
+
+static unsigned int fan_from_reg13(u16 reg, unsigned int divreg)
+{
+	if ((reg & 0xff1f) == 0xff1f)
+		return 0;
+
+	reg = (reg & 0x1f) | ((reg & 0xff00) >> 3);
+
+	if (reg == 0)
+		return 0;
+
+	return 1350000U / reg;
+}
+
+static unsigned int fan_from_reg16(u16 reg, unsigned int divreg)
+{
+	if (reg == 0 || reg == 0xffff)
+		return 0;
+
+	/*
+	 * Even though the registers are 16 bit wide, the fan divisor
+	 * still applies.
+	 */
+	return 1350000U / (reg << divreg);
+}
+
+static u16 fan_to_reg(u32 fan, unsigned int divreg)
+{
+	if (!fan)
+		return 0;
+
+	return (1350000U / fan) >> divreg;
+}
+
+static inline unsigned int
+div_from_reg(u8 reg)
+{
+	return 1 << reg;
+}
+
+/*
+ * Some of the voltage inputs have internal scaling, the tables below
+ * contain 8 (the ADC LSB in mV) * scaling factor * 100
+ */
+static const u16 scale_in[15] = {
+	800, 800, 1600, 1600, 800, 800, 800, 1600, 1600, 800, 800, 800, 800,
+	800, 800
+};
+
+static inline long in_from_reg(u8 reg, u8 nr)
+{
+	return DIV_ROUND_CLOSEST(reg * scale_in[nr], 100);
+}
+
+static inline u8 in_to_reg(u32 val, u8 nr)
+{
+	return clamp_val(DIV_ROUND_CLOSEST(val * 100, scale_in[nr]), 0, 255);
+}
+
+/*
+ * Data structures and manipulation thereof
+ */
+
+struct nct6775_data {
+	int addr;	/* IO base of hw monitor block */
+	enum kinds kind;
+	const char *name;
+
+	struct device *hwmon_dev;
+
+	u16 reg_temp[4][NUM_TEMP]; /* 0=temp, 1=temp_over, 2=temp_hyst,
+				    * 3=temp_crit
+				    */
+	u8 temp_src[NUM_TEMP];
+	u16 reg_temp_config[NUM_TEMP];
+	const char * const *temp_label;
+	int temp_label_num;
+
+	u16 REG_CONFIG;
+	u16 REG_VBAT;
+	u16 REG_DIODE;
+
+	const s8 *ALARM_BITS;
+
+	const u16 *REG_VIN;
+	const u16 *REG_IN_MINMAX[2];
+
+	const u16 *REG_TARGET;
+	const u16 *REG_FAN;
+	const u16 *REG_FAN_MODE;
+	const u16 *REG_FAN_MIN;
+	const u16 *REG_FAN_PULSES;
+	const u16 *REG_FAN_TIME[3];
+
+	const u16 *REG_TOLERANCE_H;
+
+	const u8 *REG_PWM_MODE;
+	const u8 *PWM_MODE_MASK;
+
+	const u16 *REG_PWM[7];	/* [0]=pwm, [1]=pwm_start, [2]=pwm_floor,
+				 * [3]=pwm_max, [4]=pwm_step,
+				 * [5]=weight_duty_step, [6]=weight_duty_base
+				 */
+	const u16 *REG_PWM_READ;
+
+	const u16 *REG_AUTO_TEMP;
+	const u16 *REG_AUTO_PWM;
+
+	const u16 *REG_CRITICAL_TEMP;
+	const u16 *REG_CRITICAL_TEMP_TOLERANCE;
+
+	const u16 *REG_TEMP_SOURCE;	/* temp register sources */
+	const u16 *REG_TEMP_SEL;
+	const u16 *REG_WEIGHT_TEMP_SEL;
+	const u16 *REG_WEIGHT_TEMP[3];	/* 0=base, 1=tolerance, 2=step */
+
+	const u16 *REG_TEMP_OFFSET;
+
+	const u16 *REG_ALARM;
+
+	unsigned int (*fan_from_reg)(u16 reg, unsigned int divreg);
+	unsigned int (*fan_from_reg_min)(u16 reg, unsigned int divreg);
+
+	struct mutex update_lock;
+	bool valid;		/* true if following fields are valid */
+	unsigned long last_updated;	/* In jiffies */
+
+	/* Register values */
+	u8 bank;		/* current register bank */
+	u8 in_num;		/* number of in inputs we have */
+	u8 in[15][3];		/* [0]=in, [1]=in_max, [2]=in_min */
+	unsigned int rpm[5];
+	u16 fan_min[5];
+	u8 fan_pulses[5];
+	u8 fan_div[5];
+	u8 has_pwm;
+	u8 has_fan;		/* some fan inputs can be disabled */
+	u8 has_fan_min;		/* some fans don't have min register */
+	bool has_fan_div;
+
+	u8 temp_fixed_num;	/* 3 or 6 */
+	u8 temp_type[NUM_TEMP_FIXED];
+	s8 temp_offset[NUM_TEMP_FIXED];
+	s16 temp[4][NUM_TEMP]; /* 0=temp, 1=temp_over, 2=temp_hyst,
+				* 3=temp_crit */
+	u64 alarms;
+
+	u8 pwm_num;	/* number of pwm */
+	u8 pwm_mode[5]; /* 1->DC variable voltage, 0->PWM variable duty cycle */
+	enum pwm_enable pwm_enable[5];
+			/* 0->off
+			 * 1->manual
+			 * 2->thermal cruise mode (also called SmartFan I)
+			 * 3->fan speed cruise mode
+			 * 4->SmartFan III
+			 * 5->enhanced variable thermal cruise (SmartFan IV)
+			 */
+	u8 pwm[7][5];	/* [0]=pwm, [1]=pwm_start, [2]=pwm_floor,
+			 * [3]=pwm_max, [4]=pwm_step,
+			 * [5]=weight_duty_step, [6]=weight_duty_base
+			 */
+
+	u8 target_temp[5];
+	u8 target_temp_mask;
+	u32 target_speed[5];
+	u32 target_speed_tolerance[5];
+	u8 speed_tolerance_limit;
+
+	u8 temp_tolerance[2][5];
+	u8 tolerance_mask;
+
+	u8 fan_time[3][5]; /* 0 = stop_time, 1 = step_up, 2 = step_down */
+
+	/* Automatic fan speed control registers */
+	int auto_pwm_num;
+	u8 auto_pwm[5][7];
+	u8 auto_temp[5][7];
+	u8 pwm_temp_sel[5];
+	u8 pwm_weight_temp_sel[5];
+	u8 weight_temp[3][5];	/* 0->temp_step, 1->temp_step_tol,
+				 * 2->temp_base
+				 */
+
+	u8 vid;
+	u8 vrm;
+
+	u16 have_temp;
+	u16 have_temp_fixed;
+	u16 have_in;
+#ifdef CONFIG_PM
+	/* Remember extra register values over suspend/resume */
+	u8 vbat;
+	u8 fandiv1;
+	u8 fandiv2;
+#endif
+};
+
+struct nct6775_sio_data {
+	int sioreg;
+	enum kinds kind;
+};
+
+static bool is_word_sized(struct nct6775_data *data, u16 reg)
+{
+	switch (data->kind) {
+	case nct6775:
+		return (((reg & 0xff00) == 0x100 ||
+		    (reg & 0xff00) == 0x200) &&
+		   ((reg & 0x00ff) == 0x50 ||
+		    (reg & 0x00ff) == 0x53 ||
+		    (reg & 0x00ff) == 0x55)) ||
+		  (reg & 0xfff0) == 0x630 ||
+		  reg == 0x640 || reg == 0x642 ||
+		  reg == 0x662 ||
+		  ((reg & 0xfff0) == 0x650 && (reg & 0x000f) >= 0x06) ||
+		  reg == 0x73 || reg == 0x75 || reg == 0x77;
+	case nct6776:
+		return (((reg & 0xff00) == 0x100 ||
+		    (reg & 0xff00) == 0x200) &&
+		   ((reg & 0x00ff) == 0x50 ||
+		    (reg & 0x00ff) == 0x53 ||
+		    (reg & 0x00ff) == 0x55)) ||
+		  (reg & 0xfff0) == 0x630 ||
+		  reg == 0x402 ||
+		  reg == 0x640 || reg == 0x642 ||
+		  ((reg & 0xfff0) == 0x650 && (reg & 0x000f) >= 0x06) ||
+		  reg == 0x73 || reg == 0x75 || reg == 0x77;
+	case nct6779:
+		return reg == 0x150 || reg == 0x153 || reg == 0x155 ||
+		  ((reg & 0xfff0) == 0x4b0 && (reg & 0x000f) < 0x09) ||
+		  reg == 0x402 ||
+		  reg == 0x63a || reg == 0x63c || reg == 0x63e ||
+		  reg == 0x640 || reg == 0x642 ||
+		  reg == 0x73 || reg == 0x75 || reg == 0x77 || reg == 0x79 ||
+		  reg == 0x7b;
+	}
+	return false;
+}
+
+/*
+ * On older chips, only registers 0x50-0x5f are banked.
+ * On more recent chips, all registers are banked.
+ * Assume that is the case and set the bank number for each access.
+ * Cache the bank number so it only needs to be set if it changes.
+ */
+static inline void nct6775_set_bank(struct nct6775_data *data, u16 reg)
+{
+	u8 bank = reg >> 8;
+	if (data->bank != bank) {
+		outb_p(NCT6775_REG_BANK, data->addr + ADDR_REG_OFFSET);
+		outb_p(bank, data->addr + DATA_REG_OFFSET);
+		data->bank = bank;
+	}
+}
+
+static u16 nct6775_read_value(struct nct6775_data *data, u16 reg)
+{
+	int res, word_sized = is_word_sized(data, reg);
+
+	nct6775_set_bank(data, reg);
+	outb_p(reg & 0xff, data->addr + ADDR_REG_OFFSET);
+	res = inb_p(data->addr + DATA_REG_OFFSET);
+	if (word_sized) {
+		outb_p((reg & 0xff) + 1,
+		       data->addr + ADDR_REG_OFFSET);
+		res = (res << 8) + inb_p(data->addr + DATA_REG_OFFSET);
+	}
+	return res;
+}
+
+static int nct6775_write_value(struct nct6775_data *data, u16 reg, u16 value)
+{
+	int word_sized = is_word_sized(data, reg);
+
+	nct6775_set_bank(data, reg);
+	outb_p(reg & 0xff, data->addr + ADDR_REG_OFFSET);
+	if (word_sized) {
+		outb_p(value >> 8, data->addr + DATA_REG_OFFSET);
+		outb_p((reg & 0xff) + 1,
+		       data->addr + ADDR_REG_OFFSET);
+	}
+	outb_p(value & 0xff, data->addr + DATA_REG_OFFSET);
+	return 0;
+}
+
+/* We left-align 8-bit temperature values to make the code simpler */
+static u16 nct6775_read_temp(struct nct6775_data *data, u16 reg)
+{
+	u16 res;
+
+	res = nct6775_read_value(data, reg);
+	if (!is_word_sized(data, reg))
+		res <<= 8;
+
+	return res;
+}
+
+static int nct6775_write_temp(struct nct6775_data *data, u16 reg, u16 value)
+{
+	if (!is_word_sized(data, reg))
+		value >>= 8;
+	return nct6775_write_value(data, reg, value);
+}
+
+/* This function assumes that the caller holds data->update_lock */
+static void nct6775_write_fan_div(struct nct6775_data *data, int nr)
+{
+	u8 reg;
+
+	switch (nr) {
+	case 0:
+		reg = (nct6775_read_value(data, NCT6775_REG_FANDIV1) & 0x70)
+		    | (data->fan_div[0] & 0x7);
+		nct6775_write_value(data, NCT6775_REG_FANDIV1, reg);
+		break;
+	case 1:
+		reg = (nct6775_read_value(data, NCT6775_REG_FANDIV1) & 0x7)
+		    | ((data->fan_div[1] << 4) & 0x70);
+		nct6775_write_value(data, NCT6775_REG_FANDIV1, reg);
+		break;
+	case 2:
+		reg = (nct6775_read_value(data, NCT6775_REG_FANDIV2) & 0x70)
+		    | (data->fan_div[2] & 0x7);
+		nct6775_write_value(data, NCT6775_REG_FANDIV2, reg);
+		break;
+	case 3:
+		reg = (nct6775_read_value(data, NCT6775_REG_FANDIV2) & 0x7)
+		    | ((data->fan_div[3] << 4) & 0x70);
+		nct6775_write_value(data, NCT6775_REG_FANDIV2, reg);
+		break;
+	}
+}
+
+static void nct6775_write_fan_div_common(struct nct6775_data *data, int nr)
+{
+	if (data->kind == nct6775)
+		nct6775_write_fan_div(data, nr);
+}
+
+static void nct6775_update_fan_div(struct nct6775_data *data)
+{
+	u8 i;
+
+	i = nct6775_read_value(data, NCT6775_REG_FANDIV1);
+	data->fan_div[0] = i & 0x7;
+	data->fan_div[1] = (i & 0x70) >> 4;
+	i = nct6775_read_value(data, NCT6775_REG_FANDIV2);
+	data->fan_div[2] = i & 0x7;
+	if (data->has_fan & (1 << 3))
+		data->fan_div[3] = (i & 0x70) >> 4;
+}
+
+static void nct6775_update_fan_div_common(struct nct6775_data *data)
+{
+	if (data->kind == nct6775)
+		nct6775_update_fan_div(data);
+}
+
+static void nct6775_init_fan_div(struct nct6775_data *data)
+{
+	int i;
+
+	nct6775_update_fan_div_common(data);
+	/*
+	 * For all fans, start with highest divider value if the divider
+	 * register is not initialized. This ensures that we get a
+	 * reading from the fan count register, even if it is not optimal.
+	 * We'll compute a better divider later on.
+	 */
+	for (i = 0; i < ARRAY_SIZE(data->fan_div); i++) {
+		if (!(data->has_fan & (1 << i)))
+			continue;
+		if (data->fan_div[i] == 0) {
+			data->fan_div[i] = 7;
+			nct6775_write_fan_div_common(data, i);
+		}
+	}
+}
+
+static void nct6775_init_fan_common(struct device *dev,
+				    struct nct6775_data *data)
+{
+	int i;
+	u8 reg;
+
+	if (data->has_fan_div)
+		nct6775_init_fan_div(data);
+
+	/*
+	 * If fan_min is not set (0), set it to 0xff to disable it. This
+	 * prevents the unnecessary warning when fanX_min is reported as 0.
+	 */
+	for (i = 0; i < ARRAY_SIZE(data->fan_min); i++) {
+		if (data->has_fan_min & (1 << i)) {
+			reg = nct6775_read_value(data, data->REG_FAN_MIN[i]);
+			if (!reg)
+				nct6775_write_value(data, data->REG_FAN_MIN[i],
+						    data->has_fan_div ? 0xff
+								      : 0xff1f);
+		}
+	}
+}
+
+static void nct6775_select_fan_div(struct device *dev,
+				   struct nct6775_data *data, int nr, u16 reg)
+{
+	u8 fan_div = data->fan_div[nr];
+	u16 fan_min;
+
+	if (!data->has_fan_div)
+		return;
+
+	/*
+	 * If we failed to measure the fan speed, or the reported value is not
+	 * in the optimal range, and the clock divider can be modified,
+	 * let's try that for next time.
+	 */
+	if (reg == 0x00 && fan_div < 0x07)
+		fan_div++;
+	else if (reg != 0x00 && reg < 0x30 && fan_div > 0)
+		fan_div--;
+
+	if (fan_div != data->fan_div[nr]) {
+		dev_dbg(dev, "Modifying fan%d clock divider from %u to %u\n",
+			nr + 1, div_from_reg(data->fan_div[nr]),
+			div_from_reg(fan_div));
+
+		/* Preserve min limit if possible */
+		if (data->has_fan_min & (1 << nr)) {
+			fan_min = data->fan_min[nr];
+			if (fan_div > data->fan_div[nr]) {
+				if (fan_min != 255 && fan_min > 1)
+					fan_min >>= 1;
+			} else {
+				if (fan_min != 255) {
+					fan_min <<= 1;
+					if (fan_min > 254)
+						fan_min = 254;
+				}
+			}
+			if (fan_min != data->fan_min[nr]) {
+				data->fan_min[nr] = fan_min;
+				nct6775_write_value(data, data->REG_FAN_MIN[nr],
+						    fan_min);
+			}
+		}
+		data->fan_div[nr] = fan_div;
+		nct6775_write_fan_div_common(data, nr);
+	}
+}
+
+static void nct6775_update_pwm(struct device *dev)
+{
+	struct nct6775_data *data = dev_get_drvdata(dev);
+	int i, j;
+	int fanmodecfg, reg;
+	bool duty_is_dc;
+
+	for (i = 0; i < data->pwm_num; i++) {
+		if (!(data->has_pwm & (1 << i)))
+			continue;
+
+		duty_is_dc = data->REG_PWM_MODE[i] &&
+		  (nct6775_read_value(data, data->REG_PWM_MODE[i])
+		   & data->PWM_MODE_MASK[i]);
+		data->pwm_mode[i] = duty_is_dc;
+
+		fanmodecfg = nct6775_read_value(data, data->REG_FAN_MODE[i]);
+		for (j = 0; j < ARRAY_SIZE(data->REG_PWM); j++) {
+			if (data->REG_PWM[j] && data->REG_PWM[j][i]) {
+				data->pwm[j][i]
+				  = nct6775_read_value(data,
+						       data->REG_PWM[j][i]);
+			}
+		}
+
+		data->pwm_enable[i] = reg_to_pwm_enable(data->pwm[0][i],
+							(fanmodecfg >> 4) & 7);
+
+		if (!data->temp_tolerance[0][i] ||
+		    data->pwm_enable[i] != speed_cruise)
+			data->temp_tolerance[0][i] = fanmodecfg & 0x0f;
+		if (!data->target_speed_tolerance[i] ||
+		    data->pwm_enable[i] == speed_cruise) {
+			u8 t = fanmodecfg & 0x0f;
+			if (data->REG_TOLERANCE_H) {
+				t |= (nct6775_read_value(data,
+				      data->REG_TOLERANCE_H[i]) & 0x70) >> 1;
+			}
+			data->target_speed_tolerance[i] = t;
+		}
+
+		data->temp_tolerance[1][i] =
+			nct6775_read_value(data,
+					data->REG_CRITICAL_TEMP_TOLERANCE[i]);
+
+		reg = nct6775_read_value(data, data->REG_TEMP_SEL[i]);
+		data->pwm_temp_sel[i] = reg & 0x1f;
+		/* If fan can stop, report floor as 0 */
+		if (reg & 0x80)
+			data->pwm[2][i] = 0;
+
+		reg = nct6775_read_value(data, data->REG_WEIGHT_TEMP_SEL[i]);
+		data->pwm_weight_temp_sel[i] = reg & 0x1f;
+		/* If weight is disabled, report weight source as 0 */
+		if (j == 1 && !(reg & 0x80))
+			data->pwm_weight_temp_sel[i] = 0;
+
+		/* Weight temp data */
+		for (j = 0; j < ARRAY_SIZE(data->weight_temp); j++) {
+			data->weight_temp[j][i]
+			  = nct6775_read_value(data,
+					       data->REG_WEIGHT_TEMP[j][i]);
+		}
+	}
+}
+
+static void nct6775_update_pwm_limits(struct device *dev)
+{
+	struct nct6775_data *data = dev_get_drvdata(dev);
+	int i, j;
+	u8 reg;
+	u16 reg_t;
+
+	for (i = 0; i < data->pwm_num; i++) {
+		if (!(data->has_pwm & (1 << i)))
+			continue;
+
+		for (j = 0; j < ARRAY_SIZE(data->fan_time); j++) {
+			data->fan_time[j][i] =
+			  nct6775_read_value(data, data->REG_FAN_TIME[j][i]);
+		}
+
+		reg_t = nct6775_read_value(data, data->REG_TARGET[i]);
+		/* Update only in matching mode or if never updated */
+		if (!data->target_temp[i] ||
+		    data->pwm_enable[i] == thermal_cruise)
+			data->target_temp[i] = reg_t & data->target_temp_mask;
+		if (!data->target_speed[i] ||
+		    data->pwm_enable[i] == speed_cruise) {
+			if (data->REG_TOLERANCE_H) {
+				reg_t |= (nct6775_read_value(data,
+					data->REG_TOLERANCE_H[i]) & 0x0f) << 8;
+			}
+			data->target_speed[i] = reg_t;
+		}
+
+		for (j = 0; j < data->auto_pwm_num; j++) {
+			data->auto_pwm[i][j] =
+			  nct6775_read_value(data,
+					     NCT6775_AUTO_PWM(data, i, j));
+			data->auto_temp[i][j] =
+			  nct6775_read_value(data,
+					     NCT6775_AUTO_TEMP(data, i, j));
+		}
+
+		/* critical auto_pwm temperature data */
+		data->auto_temp[i][data->auto_pwm_num] =
+			nct6775_read_value(data, data->REG_CRITICAL_TEMP[i]);
+
+		switch (data->kind) {
+		case nct6775:
+			reg = nct6775_read_value(data,
+						 NCT6775_REG_CRITICAL_ENAB[i]);
+			data->auto_pwm[i][data->auto_pwm_num] =
+						(reg & 0x02) ? 0xff : 0x00;
+			break;
+		case nct6776:
+			data->auto_pwm[i][data->auto_pwm_num] = 0xff;
+			break;
+		case nct6779:
+			reg = nct6775_read_value(data,
+					NCT6779_REG_CRITICAL_PWM_ENABLE[i]);
+			if (reg & 1)
+				data->auto_pwm[i][data->auto_pwm_num] =
+				  nct6775_read_value(data,
+					NCT6779_REG_CRITICAL_PWM[i]);
+			else
+				data->auto_pwm[i][data->auto_pwm_num] = 0xff;
+			break;
+		}
+	}
+}
+
+static struct nct6775_data *nct6775_update_device(struct device *dev)
+{
+	struct nct6775_data *data = dev_get_drvdata(dev);
+	int i, j;
+
+	mutex_lock(&data->update_lock);
+
+	if (time_after(jiffies, data->last_updated + HZ + HZ / 2)
+	    || !data->valid) {
+		/* Fan clock dividers */
+		nct6775_update_fan_div_common(data);
+
+		/* Measured voltages and limits */
+		for (i = 0; i < data->in_num; i++) {
+			if (!(data->have_in & (1 << i)))
+				continue;
+
+			data->in[i][0] = nct6775_read_value(data,
+							    data->REG_VIN[i]);
+			data->in[i][1] = nct6775_read_value(data,
+					  data->REG_IN_MINMAX[0][i]);
+			data->in[i][2] = nct6775_read_value(data,
+					  data->REG_IN_MINMAX[1][i]);
+		}
+
+		/* Measured fan speeds and limits */
+		for (i = 0; i < ARRAY_SIZE(data->rpm); i++) {
+			u16 reg;
+
+			if (!(data->has_fan & (1 << i)))
+				continue;
+
+			reg = nct6775_read_value(data, data->REG_FAN[i]);
+			data->rpm[i] = data->fan_from_reg(reg,
+							  data->fan_div[i]);
+
+			if (data->has_fan_min & (1 << i))
+				data->fan_min[i] = nct6775_read_value(data,
+					   data->REG_FAN_MIN[i]);
+			data->fan_pulses[i] =
+			  nct6775_read_value(data, data->REG_FAN_PULSES[i]);
+
+			nct6775_select_fan_div(dev, data, i, reg);
+		}
+
+		nct6775_update_pwm(dev);
+		nct6775_update_pwm_limits(dev);
+
+		/* Measured temperatures and limits */
+		for (i = 0; i < NUM_TEMP; i++) {
+			if (!(data->have_temp & (1 << i)))
+				continue;
+			for (j = 0; j < ARRAY_SIZE(data->reg_temp); j++) {
+				if (data->reg_temp[j][i])
+					data->temp[j][i]
+					  = nct6775_read_temp(data,
+						data->reg_temp[j][i]);
+			}
+			if (!(data->have_temp_fixed & (1 << i)))
+				continue;
+			data->temp_offset[i]
+			  = nct6775_read_value(data, data->REG_TEMP_OFFSET[i]);
+		}
+
+		data->alarms = 0;
+		for (i = 0; i < NUM_REG_ALARM; i++) {
+			u8 alarm;
+			if (!data->REG_ALARM[i])
+				continue;
+			alarm = nct6775_read_value(data, data->REG_ALARM[i]);
+			data->alarms |= ((u64)alarm) << (i << 3);
+		}
+
+		data->last_updated = jiffies;
+		data->valid = true;
+	}
+
+	mutex_unlock(&data->update_lock);
+	return data;
+}
+
+/*
+ * Sysfs callback functions
+ */
+static ssize_t
+show_in_reg(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	int nr = sattr->nr;
+	int index = sattr->index;
+	return sprintf(buf, "%ld\n", in_from_reg(data->in[nr][index], nr));
+}
+
+static ssize_t
+store_in_reg(struct device *dev, struct device_attribute *attr, const char *buf,
+	     size_t count)
+{
+	struct nct6775_data *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	int nr = sattr->nr;
+	int index = sattr->index;
+	unsigned long val;
+	int err = kstrtoul(buf, 10, &val);
+	if (err < 0)
+		return err;
+	mutex_lock(&data->update_lock);
+	data->in[nr][index] = in_to_reg(val, nr);
+	nct6775_write_value(data, data->REG_IN_MINMAX[index - 1][nr],
+			    data->in[nr][index]);
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+static ssize_t
+show_alarm(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	int nr = data->ALARM_BITS[sattr->index];
+	return sprintf(buf, "%u\n",
+		       (unsigned int)((data->alarms >> nr) & 0x01));
+}
+
+static SENSOR_DEVICE_ATTR_2(in0_input, S_IRUGO, show_in_reg, NULL, 0, 0);
+static SENSOR_DEVICE_ATTR_2(in1_input, S_IRUGO, show_in_reg, NULL, 1, 0);
+static SENSOR_DEVICE_ATTR_2(in2_input, S_IRUGO, show_in_reg, NULL, 2, 0);
+static SENSOR_DEVICE_ATTR_2(in3_input, S_IRUGO, show_in_reg, NULL, 3, 0);
+static SENSOR_DEVICE_ATTR_2(in4_input, S_IRUGO, show_in_reg, NULL, 4, 0);
+static SENSOR_DEVICE_ATTR_2(in5_input, S_IRUGO, show_in_reg, NULL, 5, 0);
+static SENSOR_DEVICE_ATTR_2(in6_input, S_IRUGO, show_in_reg, NULL, 6, 0);
+static SENSOR_DEVICE_ATTR_2(in7_input, S_IRUGO, show_in_reg, NULL, 7, 0);
+static SENSOR_DEVICE_ATTR_2(in8_input, S_IRUGO, show_in_reg, NULL, 8, 0);
+static SENSOR_DEVICE_ATTR_2(in9_input, S_IRUGO, show_in_reg, NULL, 9, 0);
+static SENSOR_DEVICE_ATTR_2(in10_input, S_IRUGO, show_in_reg, NULL, 10, 0);
+static SENSOR_DEVICE_ATTR_2(in11_input, S_IRUGO, show_in_reg, NULL, 11, 0);
+static SENSOR_DEVICE_ATTR_2(in12_input, S_IRUGO, show_in_reg, NULL, 12, 0);
+static SENSOR_DEVICE_ATTR_2(in13_input, S_IRUGO, show_in_reg, NULL, 13, 0);
+static SENSOR_DEVICE_ATTR_2(in14_input, S_IRUGO, show_in_reg, NULL, 14, 0);
+
+static SENSOR_DEVICE_ATTR(in0_alarm, S_IRUGO, show_alarm, NULL, 0);
+static SENSOR_DEVICE_ATTR(in1_alarm, S_IRUGO, show_alarm, NULL, 1);
+static SENSOR_DEVICE_ATTR(in2_alarm, S_IRUGO, show_alarm, NULL, 2);
+static SENSOR_DEVICE_ATTR(in3_alarm, S_IRUGO, show_alarm, NULL, 3);
+static SENSOR_DEVICE_ATTR(in4_alarm, S_IRUGO, show_alarm, NULL, 4);
+static SENSOR_DEVICE_ATTR(in5_alarm, S_IRUGO, show_alarm, NULL, 5);
+static SENSOR_DEVICE_ATTR(in6_alarm, S_IRUGO, show_alarm, NULL, 6);
+static SENSOR_DEVICE_ATTR(in7_alarm, S_IRUGO, show_alarm, NULL, 7);
+static SENSOR_DEVICE_ATTR(in8_alarm, S_IRUGO, show_alarm, NULL, 8);
+static SENSOR_DEVICE_ATTR(in9_alarm, S_IRUGO, show_alarm, NULL, 9);
+static SENSOR_DEVICE_ATTR(in10_alarm, S_IRUGO, show_alarm, NULL, 10);
+static SENSOR_DEVICE_ATTR(in11_alarm, S_IRUGO, show_alarm, NULL, 11);
+static SENSOR_DEVICE_ATTR(in12_alarm, S_IRUGO, show_alarm, NULL, 12);
+static SENSOR_DEVICE_ATTR(in13_alarm, S_IRUGO, show_alarm, NULL, 13);
+static SENSOR_DEVICE_ATTR(in14_alarm, S_IRUGO, show_alarm, NULL, 14);
+
+static SENSOR_DEVICE_ATTR_2(in0_min, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 0, 1);
+static SENSOR_DEVICE_ATTR_2(in1_min, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 1, 1);
+static SENSOR_DEVICE_ATTR_2(in2_min, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 2, 1);
+static SENSOR_DEVICE_ATTR_2(in3_min, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 3, 1);
+static SENSOR_DEVICE_ATTR_2(in4_min, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 4, 1);
+static SENSOR_DEVICE_ATTR_2(in5_min, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 5, 1);
+static SENSOR_DEVICE_ATTR_2(in6_min, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 6, 1);
+static SENSOR_DEVICE_ATTR_2(in7_min, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 7, 1);
+static SENSOR_DEVICE_ATTR_2(in8_min, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 8, 1);
+static SENSOR_DEVICE_ATTR_2(in9_min, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 9, 1);
+static SENSOR_DEVICE_ATTR_2(in10_min, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 10, 1);
+static SENSOR_DEVICE_ATTR_2(in11_min, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 11, 1);
+static SENSOR_DEVICE_ATTR_2(in12_min, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 12, 1);
+static SENSOR_DEVICE_ATTR_2(in13_min, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 13, 1);
+static SENSOR_DEVICE_ATTR_2(in14_min, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 14, 1);
+
+static SENSOR_DEVICE_ATTR_2(in0_max, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 0, 2);
+static SENSOR_DEVICE_ATTR_2(in1_max, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 1, 2);
+static SENSOR_DEVICE_ATTR_2(in2_max, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 2, 2);
+static SENSOR_DEVICE_ATTR_2(in3_max, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 3, 2);
+static SENSOR_DEVICE_ATTR_2(in4_max, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 4, 2);
+static SENSOR_DEVICE_ATTR_2(in5_max, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 5, 2);
+static SENSOR_DEVICE_ATTR_2(in6_max, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 6, 2);
+static SENSOR_DEVICE_ATTR_2(in7_max, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 7, 2);
+static SENSOR_DEVICE_ATTR_2(in8_max, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 8, 2);
+static SENSOR_DEVICE_ATTR_2(in9_max, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 9, 2);
+static SENSOR_DEVICE_ATTR_2(in10_max, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 10, 2);
+static SENSOR_DEVICE_ATTR_2(in11_max, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 11, 2);
+static SENSOR_DEVICE_ATTR_2(in12_max, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 12, 2);
+static SENSOR_DEVICE_ATTR_2(in13_max, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 13, 2);
+static SENSOR_DEVICE_ATTR_2(in14_max, S_IWUSR | S_IRUGO, show_in_reg,
+			    store_in_reg, 14, 2);
+
+static struct attribute *nct6775_attributes_in[15][5] = {
+	{
+		&sensor_dev_attr_in0_input.dev_attr.attr,
+		&sensor_dev_attr_in0_min.dev_attr.attr,
+		&sensor_dev_attr_in0_max.dev_attr.attr,
+		&sensor_dev_attr_in0_alarm.dev_attr.attr,
+		NULL
+	},
+	{
+		&sensor_dev_attr_in1_input.dev_attr.attr,
+		&sensor_dev_attr_in1_min.dev_attr.attr,
+		&sensor_dev_attr_in1_max.dev_attr.attr,
+		&sensor_dev_attr_in1_alarm.dev_attr.attr,
+		NULL
+	},
+	{
+		&sensor_dev_attr_in2_input.dev_attr.attr,
+		&sensor_dev_attr_in2_min.dev_attr.attr,
+		&sensor_dev_attr_in2_max.dev_attr.attr,
+		&sensor_dev_attr_in2_alarm.dev_attr.attr,
+		NULL
+	},
+	{
+		&sensor_dev_attr_in3_input.dev_attr.attr,
+		&sensor_dev_attr_in3_min.dev_attr.attr,
+		&sensor_dev_attr_in3_max.dev_attr.attr,
+		&sensor_dev_attr_in3_alarm.dev_attr.attr,
+		NULL
+	},
+	{
+		&sensor_dev_attr_in4_input.dev_attr.attr,
+		&sensor_dev_attr_in4_min.dev_attr.attr,
+		&sensor_dev_attr_in4_max.dev_attr.attr,
+		&sensor_dev_attr_in4_alarm.dev_attr.attr,
+		NULL
+	},
+	{
+		&sensor_dev_attr_in5_input.dev_attr.attr,
+		&sensor_dev_attr_in5_min.dev_attr.attr,
+		&sensor_dev_attr_in5_max.dev_attr.attr,
+		&sensor_dev_attr_in5_alarm.dev_attr.attr,
+		NULL
+	},
+	{
+		&sensor_dev_attr_in6_input.dev_attr.attr,
+		&sensor_dev_attr_in6_min.dev_attr.attr,
+		&sensor_dev_attr_in6_max.dev_attr.attr,
+		&sensor_dev_attr_in6_alarm.dev_attr.attr,
+		NULL
+	},
+	{
+		&sensor_dev_attr_in7_input.dev_attr.attr,
+		&sensor_dev_attr_in7_min.dev_attr.attr,
+		&sensor_dev_attr_in7_max.dev_attr.attr,
+		&sensor_dev_attr_in7_alarm.dev_attr.attr,
+		NULL
+	},
+	{
+		&sensor_dev_attr_in8_input.dev_attr.attr,
+		&sensor_dev_attr_in8_min.dev_attr.attr,
+		&sensor_dev_attr_in8_max.dev_attr.attr,
+		&sensor_dev_attr_in8_alarm.dev_attr.attr,
+		NULL
+	},
+	{
+		&sensor_dev_attr_in9_input.dev_attr.attr,
+		&sensor_dev_attr_in9_min.dev_attr.attr,
+		&sensor_dev_attr_in9_max.dev_attr.attr,
+		&sensor_dev_attr_in9_alarm.dev_attr.attr,
+		NULL
+	},
+	{
+		&sensor_dev_attr_in10_input.dev_attr.attr,
+		&sensor_dev_attr_in10_min.dev_attr.attr,
+		&sensor_dev_attr_in10_max.dev_attr.attr,
+		&sensor_dev_attr_in10_alarm.dev_attr.attr,
+		NULL
+	},
+	{
+		&sensor_dev_attr_in11_input.dev_attr.attr,
+		&sensor_dev_attr_in11_min.dev_attr.attr,
+		&sensor_dev_attr_in11_max.dev_attr.attr,
+		&sensor_dev_attr_in11_alarm.dev_attr.attr,
+		NULL
+	},
+	{
+		&sensor_dev_attr_in12_input.dev_attr.attr,
+		&sensor_dev_attr_in12_min.dev_attr.attr,
+		&sensor_dev_attr_in12_max.dev_attr.attr,
+		&sensor_dev_attr_in12_alarm.dev_attr.attr,
+		NULL
+	},
+	{
+		&sensor_dev_attr_in13_input.dev_attr.attr,
+		&sensor_dev_attr_in13_min.dev_attr.attr,
+		&sensor_dev_attr_in13_max.dev_attr.attr,
+		&sensor_dev_attr_in13_alarm.dev_attr.attr,
+		NULL
+	},
+	{
+		&sensor_dev_attr_in14_input.dev_attr.attr,
+		&sensor_dev_attr_in14_min.dev_attr.attr,
+		&sensor_dev_attr_in14_max.dev_attr.attr,
+		&sensor_dev_attr_in14_alarm.dev_attr.attr,
+		NULL
+	},
+};
+
+static const struct attribute_group nct6775_group_in[15] = {
+	{ .attrs = nct6775_attributes_in[0] },
+	{ .attrs = nct6775_attributes_in[1] },
+	{ .attrs = nct6775_attributes_in[2] },
+	{ .attrs = nct6775_attributes_in[3] },
+	{ .attrs = nct6775_attributes_in[4] },
+	{ .attrs = nct6775_attributes_in[5] },
+	{ .attrs = nct6775_attributes_in[6] },
+	{ .attrs = nct6775_attributes_in[7] },
+	{ .attrs = nct6775_attributes_in[8] },
+	{ .attrs = nct6775_attributes_in[9] },
+	{ .attrs = nct6775_attributes_in[10] },
+	{ .attrs = nct6775_attributes_in[11] },
+	{ .attrs = nct6775_attributes_in[12] },
+	{ .attrs = nct6775_attributes_in[13] },
+	{ .attrs = nct6775_attributes_in[14] },
+};
+
+static ssize_t
+show_fan(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	int nr = sattr->index;
+	return sprintf(buf, "%d\n", data->rpm[nr]);
+}
+
+static ssize_t
+show_fan_min(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	int nr = sattr->index;
+	return sprintf(buf, "%d\n",
+		       data->fan_from_reg_min(data->fan_min[nr],
+					      data->fan_div[nr]));
+}
+
+static ssize_t
+show_fan_div(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	int nr = sattr->index;
+	return sprintf(buf, "%u\n", div_from_reg(data->fan_div[nr]));
+}
+
+static ssize_t
+store_fan_min(struct device *dev, struct device_attribute *attr,
+	      const char *buf, size_t count)
+{
+	struct nct6775_data *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	int nr = sattr->index;
+	unsigned long val;
+	int err;
+	unsigned int reg;
+	u8 new_div;
+
+	err = kstrtoul(buf, 10, &val);
+	if (err < 0)
+		return err;
+
+	mutex_lock(&data->update_lock);
+	if (!data->has_fan_div) {
+		/* NCT6776F or NCT6779D; we know this is a 13 bit register */
+		if (!val) {
+			val = 0xff1f;
+		} else {
+			if (val > 1350000U)
+				val = 135000U;
+			val = 1350000U / val;
+			val = (val & 0x1f) | ((val << 3) & 0xff00);
+		}
+		data->fan_min[nr] = val;
+		goto write_min;	/* Leave fan divider alone */
+	}
+	if (!val) {
+		/* No min limit, alarm disabled */
+		data->fan_min[nr] = 255;
+		new_div = data->fan_div[nr]; /* No change */
+		dev_info(dev, "fan%u low limit and alarm disabled\n", nr + 1);
+		goto write_div;
+	}
+	reg = 1350000U / val;
+	if (reg >= 128 * 255) {
+		/*
+		 * Speed below this value cannot possibly be represented,
+		 * even with the highest divider (128)
+		 */
+		data->fan_min[nr] = 254;
+		new_div = 7; /* 128 == (1 << 7) */
+		dev_warn(dev,
+			 "fan%u low limit %lu below minimum %u, set to minimum\n",
+			 nr + 1, val, data->fan_from_reg_min(254, 7));
+	} else if (!reg) {
+		/*
+		 * Speed above this value cannot possibly be represented,
+		 * even with the lowest divider (1)
+		 */
+		data->fan_min[nr] = 1;
+		new_div = 0; /* 1 == (1 << 0) */
+		dev_warn(dev,
+			 "fan%u low limit %lu above maximum %u, set to maximum\n",
+			 nr + 1, val, data->fan_from_reg_min(1, 0));
+	} else {
+		/*
+		 * Automatically pick the best divider, i.e. the one such
+		 * that the min limit will correspond to a register value
+		 * in the 96..192 range
+		 */
+		new_div = 0;
+		while (reg > 192 && new_div < 7) {
+			reg >>= 1;
+			new_div++;
+		}
+		data->fan_min[nr] = reg;
+	}
+
+write_div:
+	/*
+	 * Write both the fan clock divider (if it changed) and the new
+	 * fan min (unconditionally)
+	 */
+	if (new_div != data->fan_div[nr]) {
+		dev_dbg(dev, "fan%u clock divider changed from %u to %u\n",
+			nr + 1, div_from_reg(data->fan_div[nr]),
+			div_from_reg(new_div));
+		data->fan_div[nr] = new_div;
+		nct6775_write_fan_div_common(data, nr);
+		/* Give the chip time to sample a new speed value */
+		data->last_updated = jiffies;
+	}
+
+write_min:
+	nct6775_write_value(data, data->REG_FAN_MIN[nr], data->fan_min[nr]);
+	mutex_unlock(&data->update_lock);
+
+	return count;
+}
+
+static ssize_t
+show_fan_pulses(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	int p = data->fan_pulses[sattr->index];
+
+	return sprintf(buf, "%d\n", p ? : 4);
+}
+
+static ssize_t
+store_fan_pulses(struct device *dev, struct device_attribute *attr,
+		 const char *buf, size_t count)
+{
+	struct nct6775_data *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	int nr = sattr->index;
+	unsigned long val;
+	int err;
+
+	err = kstrtoul(buf, 10, &val);
+	if (err < 0)
+		return err;
+
+	if (val > 4)
+		return -EINVAL;
+
+	mutex_lock(&data->update_lock);
+	data->fan_pulses[nr] = val & 3;
+	nct6775_write_value(data, data->REG_FAN_PULSES[nr], val & 3);
+	mutex_unlock(&data->update_lock);
+
+	return count;
+}
+
+static struct sensor_device_attribute sda_fan_input[] = {
+	SENSOR_ATTR(fan1_input, S_IRUGO, show_fan, NULL, 0),
+	SENSOR_ATTR(fan2_input, S_IRUGO, show_fan, NULL, 1),
+	SENSOR_ATTR(fan3_input, S_IRUGO, show_fan, NULL, 2),
+	SENSOR_ATTR(fan4_input, S_IRUGO, show_fan, NULL, 3),
+	SENSOR_ATTR(fan5_input, S_IRUGO, show_fan, NULL, 4),
+};
+
+static struct sensor_device_attribute sda_fan_alarm[] = {
+	SENSOR_ATTR(fan1_alarm, S_IRUGO, show_alarm, NULL, FAN_ALARM_BASE),
+	SENSOR_ATTR(fan2_alarm, S_IRUGO, show_alarm, NULL, FAN_ALARM_BASE + 1),
+	SENSOR_ATTR(fan3_alarm, S_IRUGO, show_alarm, NULL, FAN_ALARM_BASE + 2),
+	SENSOR_ATTR(fan4_alarm, S_IRUGO, show_alarm, NULL, FAN_ALARM_BASE + 3),
+	SENSOR_ATTR(fan5_alarm, S_IRUGO, show_alarm, NULL, FAN_ALARM_BASE + 4),
+};
+
+static struct sensor_device_attribute sda_fan_min[] = {
+	SENSOR_ATTR(fan1_min, S_IWUSR | S_IRUGO, show_fan_min,
+		    store_fan_min, 0),
+	SENSOR_ATTR(fan2_min, S_IWUSR | S_IRUGO, show_fan_min,
+		    store_fan_min, 1),
+	SENSOR_ATTR(fan3_min, S_IWUSR | S_IRUGO, show_fan_min,
+		    store_fan_min, 2),
+	SENSOR_ATTR(fan4_min, S_IWUSR | S_IRUGO, show_fan_min,
+		    store_fan_min, 3),
+	SENSOR_ATTR(fan5_min, S_IWUSR | S_IRUGO, show_fan_min,
+		    store_fan_min, 4),
+};
+
+static struct sensor_device_attribute sda_fan_pulses[] = {
+	SENSOR_ATTR(fan1_pulses, S_IWUSR | S_IRUGO, show_fan_pulses,
+		    store_fan_pulses, 0),
+	SENSOR_ATTR(fan2_pulses, S_IWUSR | S_IRUGO, show_fan_pulses,
+		    store_fan_pulses, 1),
+	SENSOR_ATTR(fan3_pulses, S_IWUSR | S_IRUGO, show_fan_pulses,
+		    store_fan_pulses, 2),
+	SENSOR_ATTR(fan4_pulses, S_IWUSR | S_IRUGO, show_fan_pulses,
+		    store_fan_pulses, 3),
+	SENSOR_ATTR(fan5_pulses, S_IWUSR | S_IRUGO, show_fan_pulses,
+		    store_fan_pulses, 4),
+};
+
+static struct sensor_device_attribute sda_fan_div[] = {
+	SENSOR_ATTR(fan1_div, S_IRUGO, show_fan_div, NULL, 0),
+	SENSOR_ATTR(fan2_div, S_IRUGO, show_fan_div, NULL, 1),
+	SENSOR_ATTR(fan3_div, S_IRUGO, show_fan_div, NULL, 2),
+	SENSOR_ATTR(fan4_div, S_IRUGO, show_fan_div, NULL, 3),
+	SENSOR_ATTR(fan5_div, S_IRUGO, show_fan_div, NULL, 4),
+};
+
+static ssize_t
+show_temp_label(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	int nr = sattr->index;
+	return sprintf(buf, "%s\n", data->temp_label[data->temp_src[nr]]);
+}
+
+static ssize_t
+show_temp(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	int nr = sattr->nr;
+	int index = sattr->index;
+
+	return sprintf(buf, "%d\n", LM75_TEMP_FROM_REG(data->temp[index][nr]));
+}
+
+static ssize_t
+store_temp(struct device *dev, struct device_attribute *attr, const char *buf,
+	   size_t count)
+{
+	struct nct6775_data *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	int nr = sattr->nr;
+	int index = sattr->index;
+	int err;
+	long val;
+
+	err = kstrtol(buf, 10, &val);
+	if (err < 0)
+		return err;
+
+	mutex_lock(&data->update_lock);
+	data->temp[index][nr] = LM75_TEMP_TO_REG(val);
+	nct6775_write_temp(data, data->reg_temp[index][nr],
+			   data->temp[index][nr]);
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+static ssize_t
+show_temp_offset(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+
+	return sprintf(buf, "%d\n", data->temp_offset[sattr->index] * 1000);
+}
+
+static ssize_t
+store_temp_offset(struct device *dev, struct device_attribute *attr,
+		  const char *buf, size_t count)
+{
+	struct nct6775_data *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	int nr = sattr->index;
+	long val;
+	int err;
+
+	err = kstrtol(buf, 10, &val);
+	if (err < 0)
+		return err;
+
+	val = clamp_val(DIV_ROUND_CLOSEST(val, 1000), -128, 127);
+
+	mutex_lock(&data->update_lock);
+	data->temp_offset[nr] = val;
+	nct6775_write_value(data, data->REG_TEMP_OFFSET[nr], val);
+	mutex_unlock(&data->update_lock);
+
+	return count;
+}
+
+static ssize_t
+show_temp_type(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	int nr = sattr->index;
+	return sprintf(buf, "%d\n", (int)data->temp_type[nr]);
+}
+
+static ssize_t
+store_temp_type(struct device *dev, struct device_attribute *attr,
+		const char *buf, size_t count)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	int nr = sattr->index;
+	unsigned long val;
+	int err;
+	u8 vbat, diode, bit;
+
+	err = kstrtoul(buf, 10, &val);
+	if (err < 0)
+		return err;
+
+	if (val != 1 && val != 3 && val != 4)
+		return -EINVAL;
+
+	mutex_lock(&data->update_lock);
+
+	data->temp_type[nr] = val;
+	vbat = nct6775_read_value(data, data->REG_VBAT) & ~(0x02 << nr);
+	diode = nct6775_read_value(data, data->REG_DIODE) & ~(0x02 << nr);
+	bit = 0x02 << nr;
+	switch (val) {
+	case 1:	/* CPU diode (diode, current mode) */
+		vbat |= bit;
+		diode |= bit;
+		break;
+	case 3: /* diode, voltage mode */
+		vbat |= bit;
+		break;
+	case 4:	/* thermistor */
+		break;
+	}
+	nct6775_write_value(data, data->REG_VBAT, vbat);
+	nct6775_write_value(data, data->REG_DIODE, diode);
+
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+static struct sensor_device_attribute_2 sda_temp_input[] = {
+	SENSOR_ATTR_2(temp1_input, S_IRUGO, show_temp, NULL, 0, 0),
+	SENSOR_ATTR_2(temp2_input, S_IRUGO, show_temp, NULL, 1, 0),
+	SENSOR_ATTR_2(temp3_input, S_IRUGO, show_temp, NULL, 2, 0),
+	SENSOR_ATTR_2(temp4_input, S_IRUGO, show_temp, NULL, 3, 0),
+	SENSOR_ATTR_2(temp5_input, S_IRUGO, show_temp, NULL, 4, 0),
+	SENSOR_ATTR_2(temp6_input, S_IRUGO, show_temp, NULL, 5, 0),
+	SENSOR_ATTR_2(temp7_input, S_IRUGO, show_temp, NULL, 6, 0),
+	SENSOR_ATTR_2(temp8_input, S_IRUGO, show_temp, NULL, 7, 0),
+	SENSOR_ATTR_2(temp9_input, S_IRUGO, show_temp, NULL, 8, 0),
+	SENSOR_ATTR_2(temp10_input, S_IRUGO, show_temp, NULL, 9, 0),
+};
+
+static struct sensor_device_attribute sda_temp_label[] = {
+	SENSOR_ATTR(temp1_label, S_IRUGO, show_temp_label, NULL, 0),
+	SENSOR_ATTR(temp2_label, S_IRUGO, show_temp_label, NULL, 1),
+	SENSOR_ATTR(temp3_label, S_IRUGO, show_temp_label, NULL, 2),
+	SENSOR_ATTR(temp4_label, S_IRUGO, show_temp_label, NULL, 3),
+	SENSOR_ATTR(temp5_label, S_IRUGO, show_temp_label, NULL, 4),
+	SENSOR_ATTR(temp6_label, S_IRUGO, show_temp_label, NULL, 5),
+	SENSOR_ATTR(temp7_label, S_IRUGO, show_temp_label, NULL, 6),
+	SENSOR_ATTR(temp8_label, S_IRUGO, show_temp_label, NULL, 7),
+	SENSOR_ATTR(temp9_label, S_IRUGO, show_temp_label, NULL, 8),
+	SENSOR_ATTR(temp10_label, S_IRUGO, show_temp_label, NULL, 9),
+};
+
+static struct sensor_device_attribute_2 sda_temp_max[] = {
+	SENSOR_ATTR_2(temp1_max, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      0, 1),
+	SENSOR_ATTR_2(temp2_max, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      1, 1),
+	SENSOR_ATTR_2(temp3_max, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      2, 1),
+	SENSOR_ATTR_2(temp4_max, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      3, 1),
+	SENSOR_ATTR_2(temp5_max, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      4, 1),
+	SENSOR_ATTR_2(temp6_max, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      5, 1),
+	SENSOR_ATTR_2(temp7_max, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      6, 1),
+	SENSOR_ATTR_2(temp8_max, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      7, 1),
+	SENSOR_ATTR_2(temp9_max, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      8, 1),
+	SENSOR_ATTR_2(temp10_max, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      9, 1),
+};
+
+static struct sensor_device_attribute_2 sda_temp_max_hyst[] = {
+	SENSOR_ATTR_2(temp1_max_hyst, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      0, 2),
+	SENSOR_ATTR_2(temp2_max_hyst, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      1, 2),
+	SENSOR_ATTR_2(temp3_max_hyst, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      2, 2),
+	SENSOR_ATTR_2(temp4_max_hyst, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      3, 2),
+	SENSOR_ATTR_2(temp5_max_hyst, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      4, 2),
+	SENSOR_ATTR_2(temp6_max_hyst, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      5, 2),
+	SENSOR_ATTR_2(temp7_max_hyst, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      6, 2),
+	SENSOR_ATTR_2(temp8_max_hyst, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      7, 2),
+	SENSOR_ATTR_2(temp9_max_hyst, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      8, 2),
+	SENSOR_ATTR_2(temp10_max_hyst, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      9, 2),
+};
+
+static struct sensor_device_attribute_2 sda_temp_crit[] = {
+	SENSOR_ATTR_2(temp1_crit, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      0, 3),
+	SENSOR_ATTR_2(temp2_crit, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      1, 3),
+	SENSOR_ATTR_2(temp3_crit, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      2, 3),
+	SENSOR_ATTR_2(temp4_crit, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      3, 3),
+	SENSOR_ATTR_2(temp5_crit, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      4, 3),
+	SENSOR_ATTR_2(temp6_crit, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      5, 3),
+	SENSOR_ATTR_2(temp7_crit, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      6, 3),
+	SENSOR_ATTR_2(temp8_crit, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      7, 3),
+	SENSOR_ATTR_2(temp9_crit, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      8, 3),
+	SENSOR_ATTR_2(temp10_crit, S_IRUGO | S_IWUSR, show_temp, store_temp,
+		      9, 3),
+};
+
+static struct sensor_device_attribute sda_temp_offset[] = {
+	SENSOR_ATTR(temp1_offset, S_IRUGO | S_IWUSR, show_temp_offset,
+		    store_temp_offset, 0),
+	SENSOR_ATTR(temp2_offset, S_IRUGO | S_IWUSR, show_temp_offset,
+		    store_temp_offset, 1),
+	SENSOR_ATTR(temp3_offset, S_IRUGO | S_IWUSR, show_temp_offset,
+		    store_temp_offset, 2),
+	SENSOR_ATTR(temp4_offset, S_IRUGO | S_IWUSR, show_temp_offset,
+		    store_temp_offset, 3),
+	SENSOR_ATTR(temp5_offset, S_IRUGO | S_IWUSR, show_temp_offset,
+		    store_temp_offset, 4),
+	SENSOR_ATTR(temp6_offset, S_IRUGO | S_IWUSR, show_temp_offset,
+		    store_temp_offset, 5),
+};
+
+static struct sensor_device_attribute sda_temp_type[] = {
+	SENSOR_ATTR(temp1_type, S_IRUGO | S_IWUSR, show_temp_type,
+		    store_temp_type, 0),
+	SENSOR_ATTR(temp2_type, S_IRUGO | S_IWUSR, show_temp_type,
+		    store_temp_type, 1),
+	SENSOR_ATTR(temp3_type, S_IRUGO | S_IWUSR, show_temp_type,
+		    store_temp_type, 2),
+	SENSOR_ATTR(temp4_type, S_IRUGO | S_IWUSR, show_temp_type,
+		    store_temp_type, 3),
+	SENSOR_ATTR(temp5_type, S_IRUGO | S_IWUSR, show_temp_type,
+		    store_temp_type, 4),
+	SENSOR_ATTR(temp6_type, S_IRUGO | S_IWUSR, show_temp_type,
+		    store_temp_type, 5),
+};
+
+static struct sensor_device_attribute sda_temp_alarm[] = {
+	SENSOR_ATTR(temp1_alarm, S_IRUGO, show_alarm, NULL,
+		    TEMP_ALARM_BASE),
+	SENSOR_ATTR(temp2_alarm, S_IRUGO, show_alarm, NULL,
+		    TEMP_ALARM_BASE + 1),
+	SENSOR_ATTR(temp3_alarm, S_IRUGO, show_alarm, NULL,
+		    TEMP_ALARM_BASE + 2),
+	SENSOR_ATTR(temp4_alarm, S_IRUGO, show_alarm, NULL,
+		    TEMP_ALARM_BASE + 3),
+	SENSOR_ATTR(temp5_alarm, S_IRUGO, show_alarm, NULL,
+		    TEMP_ALARM_BASE + 4),
+	SENSOR_ATTR(temp6_alarm, S_IRUGO, show_alarm, NULL,
+		    TEMP_ALARM_BASE + 5),
+};
+
+#define NUM_TEMP_ALARM	ARRAY_SIZE(sda_temp_alarm)
+
+static ssize_t
+show_pwm_mode(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+
+	return sprintf(buf, "%d\n", !data->pwm_mode[sattr->index]);
+}
+
+static ssize_t
+store_pwm_mode(struct device *dev, struct device_attribute *attr,
+	       const char *buf, size_t count)
+{
+	struct nct6775_data *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	int nr = sattr->index;
+	unsigned long val;
+	int err;
+	u8 reg;
+
+	err = kstrtoul(buf, 10, &val);
+	if (err < 0)
+		return err;
+
+	if (val > 1)
+		return -EINVAL;
+
+	/* Setting DC mode is not supported for all chips/channels */
+	if (data->REG_PWM_MODE[nr] == 0) {
+		if (val)
+			return -EINVAL;
+		return count;
+	}
+
+	mutex_lock(&data->update_lock);
+	data->pwm_mode[nr] = val;
+	reg = nct6775_read_value(data, data->REG_PWM_MODE[nr]);
+	reg &= ~data->PWM_MODE_MASK[nr];
+	if (val)
+		reg |= data->PWM_MODE_MASK[nr];
+	nct6775_write_value(data, data->REG_PWM_MODE[nr], reg);
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+static ssize_t
+show_pwm(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	int nr = sattr->nr;
+	int index = sattr->index;
+	int pwm;
+
+	/*
+	 * For automatic fan control modes, show current pwm readings.
+	 * Otherwise, show the configured value.
+	 */
+	if (index == 0 && data->pwm_enable[nr] > manual)
+		pwm = nct6775_read_value(data, data->REG_PWM_READ[nr]);
+	else
+		pwm = data->pwm[index][nr];
+
+	return sprintf(buf, "%d\n", pwm);
+}
+
+static ssize_t
+store_pwm(struct device *dev, struct device_attribute *attr, const char *buf,
+	  size_t count)
+{
+	struct nct6775_data *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	int nr = sattr->nr;
+	int index = sattr->index;
+	unsigned long val;
+	int minval[7] = { 0, 1, 1, data->pwm[2][nr], 0, 0, 0 };
+	int maxval[7]
+	  = { 255, 255, data->pwm[3][nr] ? : 255, 255, 255, 255, 255 };
+	int err;
+	u8 reg;
+
+	err = kstrtoul(buf, 10, &val);
+	if (err < 0)
+		return err;
+	val = clamp_val(val, minval[index], maxval[index]);
+
+	mutex_lock(&data->update_lock);
+	data->pwm[index][nr] = val;
+	nct6775_write_value(data, data->REG_PWM[index][nr], val);
+	if (index == 2)	{ /* floor: disable if val == 0 */
+		reg = nct6775_read_value(data, data->REG_TEMP_SEL[nr]);
+		reg &= 0x7f;
+		if (val)
+			reg |= 0x80;
+		nct6775_write_value(data, data->REG_TEMP_SEL[nr], reg);
+	}
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+/* Returns 0 if OK, -EINVAL otherwise */
+static int check_trip_points(struct nct6775_data *data, int nr)
+{
+	int i;
+
+	for (i = 0; i < data->auto_pwm_num - 1; i++) {
+		if (data->auto_temp[nr][i] > data->auto_temp[nr][i + 1])
+			return -EINVAL;
+	}
+	for (i = 0; i < data->auto_pwm_num - 1; i++) {
+		if (data->auto_pwm[nr][i] > data->auto_pwm[nr][i + 1])
+			return -EINVAL;
+	}
+	/* validate critical temperature and pwm if enabled (pwm > 0) */
+	if (data->auto_pwm[nr][data->auto_pwm_num]) {
+		if (data->auto_temp[nr][data->auto_pwm_num - 1] >
+				data->auto_temp[nr][data->auto_pwm_num] ||
+		    data->auto_pwm[nr][data->auto_pwm_num - 1] >
+				data->auto_pwm[nr][data->auto_pwm_num])
+			return -EINVAL;
+	}
+	return 0;
+}
+
+static void pwm_update_registers(struct nct6775_data *data, int nr)
+{
+	u8 reg;
+
+	switch (data->pwm_enable[nr]) {
+	case off:
+	case manual:
+		break;
+	case speed_cruise:
+		reg = nct6775_read_value(data, data->REG_FAN_MODE[nr]);
+		reg = (reg & ~data->tolerance_mask) |
+		  (data->target_speed_tolerance[nr] & data->tolerance_mask);
+		nct6775_write_value(data, data->REG_FAN_MODE[nr], reg);
+		nct6775_write_value(data, data->REG_TARGET[nr],
+				    data->target_speed[nr] & 0xff);
+		if (data->REG_TOLERANCE_H) {
+			reg = (data->target_speed[nr] >> 8) & 0x0f;
+			reg |= (data->target_speed_tolerance[nr] & 0x38) << 1;
+			nct6775_write_value(data,
+					    data->REG_TOLERANCE_H[nr],
+					    reg);
+		}
+		break;
+	case thermal_cruise:
+		nct6775_write_value(data, data->REG_TARGET[nr],
+				    data->target_temp[nr]);
+		/* intentional */
+	default:
+		reg = nct6775_read_value(data, data->REG_FAN_MODE[nr]);
+		reg = (reg & ~data->tolerance_mask) |
+		  data->temp_tolerance[0][nr];
+		nct6775_write_value(data, data->REG_FAN_MODE[nr], reg);
+		break;
+	}
+}
+
+static ssize_t
+show_pwm_enable(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+
+	return sprintf(buf, "%d\n", data->pwm_enable[sattr->index]);
+}
+
+static ssize_t
+store_pwm_enable(struct device *dev, struct device_attribute *attr,
+		 const char *buf, size_t count)
+{
+	struct nct6775_data *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	int nr = sattr->index;
+	unsigned long val;
+	int err;
+	u16 reg;
+
+	err = kstrtoul(buf, 10, &val);
+	if (err < 0)
+		return err;
+
+	if (val > sf4)
+		return -EINVAL;
+
+	if (val == sf3 && data->kind != nct6775)
+		return -EINVAL;
+
+	if (val == sf4 && check_trip_points(data, nr)) {
+		dev_err(dev, "Inconsistent trip points, not switching to SmartFan IV mode\n");
+		dev_err(dev, "Adjust trip points and try again\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&data->update_lock);
+	data->pwm_enable[nr] = val;
+	if (val == off) {
+		/*
+		 * turn off pwm control: select manual mode, set pwm to maximum
+		 */
+		data->pwm[0][nr] = 255;
+		nct6775_write_value(data, data->REG_PWM[0][nr], 255);
+	}
+	pwm_update_registers(data, nr);
+	reg = nct6775_read_value(data, data->REG_FAN_MODE[nr]);
+	reg &= 0x0f;
+	reg |= pwm_enable_to_reg(val) << 4;
+	nct6775_write_value(data, data->REG_FAN_MODE[nr], reg);
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+static ssize_t
+show_pwm_temp_sel_common(struct nct6775_data *data, char *buf, int src)
+{
+	int i, sel = 0;
+
+	for (i = 0; i < NUM_TEMP; i++) {
+		if (!(data->have_temp & (1 << i)))
+			continue;
+		if (src == data->temp_src[i]) {
+			sel = i + 1;
+			break;
+		}
+	}
+
+	return sprintf(buf, "%d\n", sel);
+}
+
+static ssize_t
+show_pwm_temp_sel(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	int index = sattr->index;
+
+	return show_pwm_temp_sel_common(data, buf, data->pwm_temp_sel[index]);
+}
+
+static ssize_t
+store_pwm_temp_sel(struct device *dev, struct device_attribute *attr,
+		   const char *buf, size_t count)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	int nr = sattr->index;
+	unsigned long val;
+	int err, reg, src;
+
+	err = kstrtoul(buf, 10, &val);
+	if (err < 0)
+		return err;
+	if (val == 0 || val > NUM_TEMP)
+		return -EINVAL;
+	if (!(data->have_temp & (1 << (val - 1))) || !data->temp_src[val - 1])
+		return -EINVAL;
+
+	mutex_lock(&data->update_lock);
+	src = data->temp_src[val - 1];
+	data->pwm_temp_sel[nr] = src;
+	reg = nct6775_read_value(data, data->REG_TEMP_SEL[nr]);
+	reg &= 0xe0;
+	reg |= src;
+	nct6775_write_value(data, data->REG_TEMP_SEL[nr], reg);
+	mutex_unlock(&data->update_lock);
+
+	return count;
+}
+
+static ssize_t
+show_pwm_weight_temp_sel(struct device *dev, struct device_attribute *attr,
+			 char *buf)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	int index = sattr->index;
+
+	return show_pwm_temp_sel_common(data, buf,
+					data->pwm_weight_temp_sel[index]);
+}
+
+static ssize_t
+store_pwm_weight_temp_sel(struct device *dev, struct device_attribute *attr,
+			  const char *buf, size_t count)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	int nr = sattr->index;
+	unsigned long val;
+	int err, reg, src;
+
+	err = kstrtoul(buf, 10, &val);
+	if (err < 0)
+		return err;
+	if (val > NUM_TEMP)
+		return -EINVAL;
+	if (val && (!(data->have_temp & (1 << (val - 1))) ||
+		    !data->temp_src[val - 1]))
+		return -EINVAL;
+
+	mutex_lock(&data->update_lock);
+	if (val) {
+		src = data->temp_src[val - 1];
+		data->pwm_weight_temp_sel[nr] = src;
+		reg = nct6775_read_value(data, data->REG_WEIGHT_TEMP_SEL[nr]);
+		reg &= 0xe0;
+		reg |= (src | 0x80);
+		nct6775_write_value(data, data->REG_WEIGHT_TEMP_SEL[nr], reg);
+	} else {
+		data->pwm_weight_temp_sel[nr] = 0;
+		reg = nct6775_read_value(data, data->REG_WEIGHT_TEMP_SEL[nr]);
+		reg &= 0x7f;
+		nct6775_write_value(data, data->REG_WEIGHT_TEMP_SEL[nr], reg);
+	}
+	mutex_unlock(&data->update_lock);
+
+	return count;
+}
+
+static ssize_t
+show_target_temp(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+
+	return sprintf(buf, "%d\n", data->target_temp[sattr->index] * 1000);
+}
+
+static ssize_t
+store_target_temp(struct device *dev, struct device_attribute *attr,
+		  const char *buf, size_t count)
+{
+	struct nct6775_data *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	int nr = sattr->index;
+	unsigned long val;
+	int err;
+
+	err = kstrtoul(buf, 10, &val);
+	if (err < 0)
+		return err;
+
+	val = clamp_val(DIV_ROUND_CLOSEST(val, 1000), 0,
+			data->target_temp_mask);
+
+	mutex_lock(&data->update_lock);
+	data->target_temp[nr] = val;
+	pwm_update_registers(data, nr);
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+static ssize_t
+show_target_speed(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	int nr = sattr->index;
+
+	return sprintf(buf, "%d\n",
+		       fan_from_reg16(data->target_speed[nr],
+				      data->fan_div[nr]));
+}
+
+static ssize_t
+store_target_speed(struct device *dev, struct device_attribute *attr,
+		   const char *buf, size_t count)
+{
+	struct nct6775_data *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	int nr = sattr->index;
+	unsigned long val;
+	int err;
+	u16 speed;
+
+	err = kstrtoul(buf, 10, &val);
+	if (err < 0)
+		return err;
+
+	val = clamp_val(val, 0, 1350000U);
+	speed = fan_to_reg(val, data->fan_div[nr]);
+
+	mutex_lock(&data->update_lock);
+	data->target_speed[nr] = speed;
+	pwm_update_registers(data, nr);
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+static ssize_t
+show_temp_tolerance(struct device *dev, struct device_attribute *attr,
+		    char *buf)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	int nr = sattr->nr;
+	int index = sattr->index;
+
+	return sprintf(buf, "%d\n", data->temp_tolerance[index][nr] * 1000);
+}
+
+static ssize_t
+store_temp_tolerance(struct device *dev, struct device_attribute *attr,
+		     const char *buf, size_t count)
+{
+	struct nct6775_data *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	int nr = sattr->nr;
+	int index = sattr->index;
+	unsigned long val;
+	int err;
+
+	err = kstrtoul(buf, 10, &val);
+	if (err < 0)
+		return err;
+
+	/* Limit tolerance as needed */
+	val = clamp_val(DIV_ROUND_CLOSEST(val, 1000), 0, data->tolerance_mask);
+
+	mutex_lock(&data->update_lock);
+	data->temp_tolerance[index][nr] = val;
+	if (index)
+		pwm_update_registers(data, nr);
+	else
+		nct6775_write_value(data,
+				    data->REG_CRITICAL_TEMP_TOLERANCE[nr],
+				    val);
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+/*
+ * Fan speed tolerance is a tricky beast, since the associated register is
+ * a tick counter, but the value is reported and configured as rpm.
+ * Compute resulting low and high rpm values and report the difference.
+ */
+static ssize_t
+show_speed_tolerance(struct device *dev, struct device_attribute *attr,
+		     char *buf)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	int nr = sattr->index;
+	int low = data->target_speed[nr] - data->target_speed_tolerance[nr];
+	int high = data->target_speed[nr] + data->target_speed_tolerance[nr];
+	int tolerance;
+
+	if (low <= 0)
+		low = 1;
+	if (high > 0xffff)
+		high = 0xffff;
+	if (high < low)
+		high = low;
+
+	tolerance = (fan_from_reg16(low, data->fan_div[nr])
+		     - fan_from_reg16(high, data->fan_div[nr])) / 2;
+
+	return sprintf(buf, "%d\n", tolerance);
+}
+
+static ssize_t
+store_speed_tolerance(struct device *dev, struct device_attribute *attr,
+		      const char *buf, size_t count)
+{
+	struct nct6775_data *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	int nr = sattr->index;
+	unsigned long val;
+	int err;
+	int low, high;
+
+	err = kstrtoul(buf, 10, &val);
+	if (err < 0)
+		return err;
+
+	high = fan_from_reg16(data->target_speed[nr],
+			      data->fan_div[nr]) + val;
+	low = fan_from_reg16(data->target_speed[nr],
+			     data->fan_div[nr]) - val;
+	if (low <= 0)
+		low = 1;
+	if (high < low)
+		high = low;
+
+	val = (fan_to_reg(low, data->fan_div[nr]) -
+	       fan_to_reg(high, data->fan_div[nr])) / 2;
+
+	/* Limit tolerance as needed */
+	val = clamp_val(val, 0, data->speed_tolerance_limit);
+
+	mutex_lock(&data->update_lock);
+	data->target_speed_tolerance[nr] = val;
+	pwm_update_registers(data, nr);
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+static SENSOR_DEVICE_ATTR_2(pwm1, S_IWUSR | S_IRUGO, show_pwm, store_pwm, 0, 0);
+static SENSOR_DEVICE_ATTR_2(pwm2, S_IWUSR | S_IRUGO, show_pwm, store_pwm, 1, 0);
+static SENSOR_DEVICE_ATTR_2(pwm3, S_IWUSR | S_IRUGO, show_pwm, store_pwm, 2, 0);
+static SENSOR_DEVICE_ATTR_2(pwm4, S_IWUSR | S_IRUGO, show_pwm, store_pwm, 3, 0);
+static SENSOR_DEVICE_ATTR_2(pwm5, S_IWUSR | S_IRUGO, show_pwm, store_pwm, 4, 0);
+
+static SENSOR_DEVICE_ATTR(pwm1_mode, S_IWUSR | S_IRUGO, show_pwm_mode,
+			  store_pwm_mode, 0);
+static SENSOR_DEVICE_ATTR(pwm2_mode, S_IWUSR | S_IRUGO, show_pwm_mode,
+			  store_pwm_mode, 1);
+static SENSOR_DEVICE_ATTR(pwm3_mode, S_IWUSR | S_IRUGO, show_pwm_mode,
+			  store_pwm_mode, 2);
+static SENSOR_DEVICE_ATTR(pwm4_mode, S_IWUSR | S_IRUGO, show_pwm_mode,
+			  store_pwm_mode, 3);
+static SENSOR_DEVICE_ATTR(pwm5_mode, S_IWUSR | S_IRUGO, show_pwm_mode,
+			  store_pwm_mode, 4);
+
+static SENSOR_DEVICE_ATTR(pwm1_enable, S_IWUSR | S_IRUGO, show_pwm_enable,
+			  store_pwm_enable, 0);
+static SENSOR_DEVICE_ATTR(pwm2_enable, S_IWUSR | S_IRUGO, show_pwm_enable,
+			  store_pwm_enable, 1);
+static SENSOR_DEVICE_ATTR(pwm3_enable, S_IWUSR | S_IRUGO, show_pwm_enable,
+			  store_pwm_enable, 2);
+static SENSOR_DEVICE_ATTR(pwm4_enable, S_IWUSR | S_IRUGO, show_pwm_enable,
+			  store_pwm_enable, 3);
+static SENSOR_DEVICE_ATTR(pwm5_enable, S_IWUSR | S_IRUGO, show_pwm_enable,
+			  store_pwm_enable, 4);
+
+static SENSOR_DEVICE_ATTR(pwm1_temp_sel, S_IWUSR | S_IRUGO,
+			    show_pwm_temp_sel, store_pwm_temp_sel, 0);
+static SENSOR_DEVICE_ATTR(pwm2_temp_sel, S_IWUSR | S_IRUGO,
+			    show_pwm_temp_sel, store_pwm_temp_sel, 1);
+static SENSOR_DEVICE_ATTR(pwm3_temp_sel, S_IWUSR | S_IRUGO,
+			    show_pwm_temp_sel, store_pwm_temp_sel, 2);
+static SENSOR_DEVICE_ATTR(pwm4_temp_sel, S_IWUSR | S_IRUGO,
+			    show_pwm_temp_sel, store_pwm_temp_sel, 3);
+static SENSOR_DEVICE_ATTR(pwm5_temp_sel, S_IWUSR | S_IRUGO,
+			    show_pwm_temp_sel, store_pwm_temp_sel, 4);
+
+static SENSOR_DEVICE_ATTR(pwm1_target_temp, S_IWUSR | S_IRUGO, show_target_temp,
+			  store_target_temp, 0);
+static SENSOR_DEVICE_ATTR(pwm2_target_temp, S_IWUSR | S_IRUGO, show_target_temp,
+			  store_target_temp, 1);
+static SENSOR_DEVICE_ATTR(pwm3_target_temp, S_IWUSR | S_IRUGO, show_target_temp,
+			  store_target_temp, 2);
+static SENSOR_DEVICE_ATTR(pwm4_target_temp, S_IWUSR | S_IRUGO, show_target_temp,
+			  store_target_temp, 3);
+static SENSOR_DEVICE_ATTR(pwm5_target_temp, S_IWUSR | S_IRUGO, show_target_temp,
+			  store_target_temp, 4);
+
+static SENSOR_DEVICE_ATTR(fan1_target, S_IWUSR | S_IRUGO, show_target_speed,
+			  store_target_speed, 0);
+static SENSOR_DEVICE_ATTR(fan2_target, S_IWUSR | S_IRUGO, show_target_speed,
+			  store_target_speed, 1);
+static SENSOR_DEVICE_ATTR(fan3_target, S_IWUSR | S_IRUGO, show_target_speed,
+			  store_target_speed, 2);
+static SENSOR_DEVICE_ATTR(fan4_target, S_IWUSR | S_IRUGO, show_target_speed,
+			  store_target_speed, 3);
+static SENSOR_DEVICE_ATTR(fan5_target, S_IWUSR | S_IRUGO, show_target_speed,
+			  store_target_speed, 4);
+
+static SENSOR_DEVICE_ATTR(fan1_tolerance, S_IWUSR | S_IRUGO,
+			    show_speed_tolerance, store_speed_tolerance, 0);
+static SENSOR_DEVICE_ATTR(fan2_tolerance, S_IWUSR | S_IRUGO,
+			    show_speed_tolerance, store_speed_tolerance, 1);
+static SENSOR_DEVICE_ATTR(fan3_tolerance, S_IWUSR | S_IRUGO,
+			    show_speed_tolerance, store_speed_tolerance, 2);
+static SENSOR_DEVICE_ATTR(fan4_tolerance, S_IWUSR | S_IRUGO,
+			    show_speed_tolerance, store_speed_tolerance, 3);
+static SENSOR_DEVICE_ATTR(fan5_tolerance, S_IWUSR | S_IRUGO,
+			    show_speed_tolerance, store_speed_tolerance, 4);
+
+/* Smart Fan registers */
+
+static ssize_t
+show_weight_temp(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	int nr = sattr->nr;
+	int index = sattr->index;
+
+	return sprintf(buf, "%d\n", data->weight_temp[index][nr] * 1000);
+}
+
+static ssize_t
+store_weight_temp(struct device *dev, struct device_attribute *attr,
+		  const char *buf, size_t count)
+{
+	struct nct6775_data *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	int nr = sattr->nr;
+	int index = sattr->index;
+	unsigned long val;
+	int err;
+
+	err = kstrtoul(buf, 10, &val);
+	if (err < 0)
+		return err;
+
+	val = clamp_val(DIV_ROUND_CLOSEST(val, 1000), 0, 255);
+
+	mutex_lock(&data->update_lock);
+	data->weight_temp[index][nr] = val;
+	nct6775_write_value(data, data->REG_WEIGHT_TEMP[index][nr], val);
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+static SENSOR_DEVICE_ATTR(pwm1_weight_temp_sel, S_IWUSR | S_IRUGO,
+			    show_pwm_weight_temp_sel, store_pwm_weight_temp_sel,
+			    0);
+static SENSOR_DEVICE_ATTR(pwm2_weight_temp_sel, S_IWUSR | S_IRUGO,
+			    show_pwm_weight_temp_sel, store_pwm_weight_temp_sel,
+			    1);
+static SENSOR_DEVICE_ATTR(pwm3_weight_temp_sel, S_IWUSR | S_IRUGO,
+			    show_pwm_weight_temp_sel, store_pwm_weight_temp_sel,
+			    2);
+static SENSOR_DEVICE_ATTR(pwm4_weight_temp_sel, S_IWUSR | S_IRUGO,
+			    show_pwm_weight_temp_sel, store_pwm_weight_temp_sel,
+			    3);
+static SENSOR_DEVICE_ATTR(pwm5_weight_temp_sel, S_IWUSR | S_IRUGO,
+			    show_pwm_weight_temp_sel, store_pwm_weight_temp_sel,
+			    4);
+
+static SENSOR_DEVICE_ATTR_2(pwm1_weight_temp_step, S_IWUSR | S_IRUGO,
+			    show_weight_temp, store_weight_temp, 0, 0);
+static SENSOR_DEVICE_ATTR_2(pwm2_weight_temp_step, S_IWUSR | S_IRUGO,
+			    show_weight_temp, store_weight_temp, 1, 0);
+static SENSOR_DEVICE_ATTR_2(pwm3_weight_temp_step, S_IWUSR | S_IRUGO,
+			    show_weight_temp, store_weight_temp, 2, 0);
+static SENSOR_DEVICE_ATTR_2(pwm4_weight_temp_step, S_IWUSR | S_IRUGO,
+			    show_weight_temp, store_weight_temp, 3, 0);
+static SENSOR_DEVICE_ATTR_2(pwm5_weight_temp_step, S_IWUSR | S_IRUGO,
+			    show_weight_temp, store_weight_temp, 4, 0);
+
+static SENSOR_DEVICE_ATTR_2(pwm1_weight_temp_step_tol, S_IWUSR | S_IRUGO,
+			    show_weight_temp, store_weight_temp, 0, 1);
+static SENSOR_DEVICE_ATTR_2(pwm2_weight_temp_step_tol, S_IWUSR | S_IRUGO,
+			    show_weight_temp, store_weight_temp, 1, 1);
+static SENSOR_DEVICE_ATTR_2(pwm3_weight_temp_step_tol, S_IWUSR | S_IRUGO,
+			    show_weight_temp, store_weight_temp, 2, 1);
+static SENSOR_DEVICE_ATTR_2(pwm4_weight_temp_step_tol, S_IWUSR | S_IRUGO,
+			    show_weight_temp, store_weight_temp, 3, 1);
+static SENSOR_DEVICE_ATTR_2(pwm5_weight_temp_step_tol, S_IWUSR | S_IRUGO,
+			    show_weight_temp, store_weight_temp, 4, 1);
+
+static SENSOR_DEVICE_ATTR_2(pwm1_weight_temp_step_base, S_IWUSR | S_IRUGO,
+			    show_weight_temp, store_weight_temp, 0, 2);
+static SENSOR_DEVICE_ATTR_2(pwm2_weight_temp_step_base, S_IWUSR | S_IRUGO,
+			    show_weight_temp, store_weight_temp, 1, 2);
+static SENSOR_DEVICE_ATTR_2(pwm3_weight_temp_step_base, S_IWUSR | S_IRUGO,
+			    show_weight_temp, store_weight_temp, 2, 2);
+static SENSOR_DEVICE_ATTR_2(pwm4_weight_temp_step_base, S_IWUSR | S_IRUGO,
+			    show_weight_temp, store_weight_temp, 3, 2);
+static SENSOR_DEVICE_ATTR_2(pwm5_weight_temp_step_base, S_IWUSR | S_IRUGO,
+			    show_weight_temp, store_weight_temp, 4, 2);
+
+static SENSOR_DEVICE_ATTR_2(pwm1_weight_duty_step, S_IWUSR | S_IRUGO,
+			    show_pwm, store_pwm, 0, 5);
+static SENSOR_DEVICE_ATTR_2(pwm2_weight_duty_step, S_IWUSR | S_IRUGO,
+			    show_pwm, store_pwm, 1, 5);
+static SENSOR_DEVICE_ATTR_2(pwm3_weight_duty_step, S_IWUSR | S_IRUGO,
+			    show_pwm, store_pwm, 2, 5);
+static SENSOR_DEVICE_ATTR_2(pwm4_weight_duty_step, S_IWUSR | S_IRUGO,
+			    show_pwm, store_pwm, 3, 5);
+static SENSOR_DEVICE_ATTR_2(pwm5_weight_duty_step, S_IWUSR | S_IRUGO,
+			    show_pwm, store_pwm, 4, 5);
+
+/* duty_base is not supported on all chips */
+static struct sensor_device_attribute_2 sda_weight_duty_base[] = {
+	SENSOR_ATTR_2(pwm1_weight_duty_base, S_IWUSR | S_IRUGO,
+		      show_pwm, store_pwm, 0, 6),
+	SENSOR_ATTR_2(pwm2_weight_duty_base, S_IWUSR | S_IRUGO,
+		      show_pwm, store_pwm, 1, 6),
+	SENSOR_ATTR_2(pwm3_weight_duty_base, S_IWUSR | S_IRUGO,
+		      show_pwm, store_pwm, 2, 6),
+	SENSOR_ATTR_2(pwm4_weight_duty_base, S_IWUSR | S_IRUGO,
+		      show_pwm, store_pwm, 3, 6),
+	SENSOR_ATTR_2(pwm5_weight_duty_base, S_IWUSR | S_IRUGO,
+		      show_pwm, store_pwm, 4, 6),
+};
+
+static ssize_t
+show_fan_time(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	int nr = sattr->nr;
+	int index = sattr->index;
+
+	return sprintf(buf, "%d\n",
+		       step_time_from_reg(data->fan_time[index][nr],
+					  data->pwm_mode[nr]));
+}
+
+static ssize_t
+store_fan_time(struct device *dev, struct device_attribute *attr,
+	       const char *buf, size_t count)
+{
+	struct nct6775_data *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	int nr = sattr->nr;
+	int index = sattr->index;
+	unsigned long val;
+	int err;
+
+	err = kstrtoul(buf, 10, &val);
+	if (err < 0)
+		return err;
+
+	val = step_time_to_reg(val, data->pwm_mode[nr]);
+	mutex_lock(&data->update_lock);
+	data->fan_time[index][nr] = val;
+	nct6775_write_value(data, data->REG_FAN_TIME[index][nr], val);
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+static ssize_t
+show_name(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6775_data *data = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%s\n", data->name);
+}
+
+static DEVICE_ATTR(name, S_IRUGO, show_name, NULL);
+
+static SENSOR_DEVICE_ATTR_2(pwm1_stop_time, S_IWUSR | S_IRUGO, show_fan_time,
+			    store_fan_time, 0, 0);
+static SENSOR_DEVICE_ATTR_2(pwm2_stop_time, S_IWUSR | S_IRUGO, show_fan_time,
+			    store_fan_time, 1, 0);
+static SENSOR_DEVICE_ATTR_2(pwm3_stop_time, S_IWUSR | S_IRUGO, show_fan_time,
+			    store_fan_time, 2, 0);
+static SENSOR_DEVICE_ATTR_2(pwm4_stop_time, S_IWUSR | S_IRUGO, show_fan_time,
+			    store_fan_time, 3, 0);
+static SENSOR_DEVICE_ATTR_2(pwm5_stop_time, S_IWUSR | S_IRUGO, show_fan_time,
+			    store_fan_time, 4, 0);
+
+static SENSOR_DEVICE_ATTR_2(pwm1_step_up_time, S_IWUSR | S_IRUGO, show_fan_time,
+			    store_fan_time, 0, 1);
+static SENSOR_DEVICE_ATTR_2(pwm2_step_up_time, S_IWUSR | S_IRUGO, show_fan_time,
+			    store_fan_time, 1, 1);
+static SENSOR_DEVICE_ATTR_2(pwm3_step_up_time, S_IWUSR | S_IRUGO, show_fan_time,
+			    store_fan_time, 2, 1);
+static SENSOR_DEVICE_ATTR_2(pwm4_step_up_time, S_IWUSR | S_IRUGO, show_fan_time,
+			    store_fan_time, 3, 1);
+static SENSOR_DEVICE_ATTR_2(pwm5_step_up_time, S_IWUSR | S_IRUGO, show_fan_time,
+			    store_fan_time, 4, 1);
+
+static SENSOR_DEVICE_ATTR_2(pwm1_step_down_time, S_IWUSR | S_IRUGO,
+			    show_fan_time, store_fan_time, 0, 2);
+static SENSOR_DEVICE_ATTR_2(pwm2_step_down_time, S_IWUSR | S_IRUGO,
+			    show_fan_time, store_fan_time, 1, 2);
+static SENSOR_DEVICE_ATTR_2(pwm3_step_down_time, S_IWUSR | S_IRUGO,
+			    show_fan_time, store_fan_time, 2, 2);
+static SENSOR_DEVICE_ATTR_2(pwm4_step_down_time, S_IWUSR | S_IRUGO,
+			    show_fan_time, store_fan_time, 3, 2);
+static SENSOR_DEVICE_ATTR_2(pwm5_step_down_time, S_IWUSR | S_IRUGO,
+			    show_fan_time, store_fan_time, 4, 2);
+
+static SENSOR_DEVICE_ATTR_2(pwm1_start, S_IWUSR | S_IRUGO, show_pwm,
+			    store_pwm, 0, 1);
+static SENSOR_DEVICE_ATTR_2(pwm2_start, S_IWUSR | S_IRUGO, show_pwm,
+			    store_pwm, 1, 1);
+static SENSOR_DEVICE_ATTR_2(pwm3_start, S_IWUSR | S_IRUGO, show_pwm,
+			    store_pwm, 2, 1);
+static SENSOR_DEVICE_ATTR_2(pwm4_start, S_IWUSR | S_IRUGO, show_pwm,
+			    store_pwm, 3, 1);
+static SENSOR_DEVICE_ATTR_2(pwm5_start, S_IWUSR | S_IRUGO, show_pwm,
+			    store_pwm, 4, 1);
+
+static SENSOR_DEVICE_ATTR_2(pwm1_floor, S_IWUSR | S_IRUGO, show_pwm,
+			    store_pwm, 0, 2);
+static SENSOR_DEVICE_ATTR_2(pwm2_floor, S_IWUSR | S_IRUGO, show_pwm,
+			    store_pwm, 1, 2);
+static SENSOR_DEVICE_ATTR_2(pwm3_floor, S_IWUSR | S_IRUGO, show_pwm,
+			    store_pwm, 2, 2);
+static SENSOR_DEVICE_ATTR_2(pwm4_floor, S_IWUSR | S_IRUGO, show_pwm,
+			    store_pwm, 3, 2);
+static SENSOR_DEVICE_ATTR_2(pwm5_floor, S_IWUSR | S_IRUGO, show_pwm,
+			    store_pwm, 4, 2);
+
+static SENSOR_DEVICE_ATTR_2(pwm1_temp_tolerance, S_IWUSR | S_IRUGO,
+			    show_temp_tolerance, store_temp_tolerance, 0, 0);
+static SENSOR_DEVICE_ATTR_2(pwm2_temp_tolerance, S_IWUSR | S_IRUGO,
+			    show_temp_tolerance, store_temp_tolerance, 1, 0);
+static SENSOR_DEVICE_ATTR_2(pwm3_temp_tolerance, S_IWUSR | S_IRUGO,
+			    show_temp_tolerance, store_temp_tolerance, 2, 0);
+static SENSOR_DEVICE_ATTR_2(pwm4_temp_tolerance, S_IWUSR | S_IRUGO,
+			    show_temp_tolerance, store_temp_tolerance, 3, 0);
+static SENSOR_DEVICE_ATTR_2(pwm5_temp_tolerance, S_IWUSR | S_IRUGO,
+			    show_temp_tolerance, store_temp_tolerance, 4, 0);
+
+static SENSOR_DEVICE_ATTR_2(pwm1_crit_temp_tolerance, S_IWUSR | S_IRUGO,
+			    show_temp_tolerance, store_temp_tolerance, 0, 1);
+static SENSOR_DEVICE_ATTR_2(pwm2_crit_temp_tolerance, S_IWUSR | S_IRUGO,
+			    show_temp_tolerance, store_temp_tolerance, 1, 1);
+static SENSOR_DEVICE_ATTR_2(pwm3_crit_temp_tolerance, S_IWUSR | S_IRUGO,
+			    show_temp_tolerance, store_temp_tolerance, 2, 1);
+static SENSOR_DEVICE_ATTR_2(pwm4_crit_temp_tolerance, S_IWUSR | S_IRUGO,
+			    show_temp_tolerance, store_temp_tolerance, 3, 1);
+static SENSOR_DEVICE_ATTR_2(pwm5_crit_temp_tolerance, S_IWUSR | S_IRUGO,
+			    show_temp_tolerance, store_temp_tolerance, 4, 1);
+
+/* pwm_max is not supported on all chips */
+static struct sensor_device_attribute_2 sda_pwm_max[] = {
+	SENSOR_ATTR_2(pwm1_max, S_IWUSR | S_IRUGO, show_pwm, store_pwm,
+		      0, 3),
+	SENSOR_ATTR_2(pwm2_max, S_IWUSR | S_IRUGO, show_pwm, store_pwm,
+		      1, 3),
+	SENSOR_ATTR_2(pwm3_max, S_IWUSR | S_IRUGO, show_pwm, store_pwm,
+		      2, 3),
+	SENSOR_ATTR_2(pwm4_max, S_IWUSR | S_IRUGO, show_pwm, store_pwm,
+		      3, 3),
+	SENSOR_ATTR_2(pwm5_max, S_IWUSR | S_IRUGO, show_pwm, store_pwm,
+		      4, 3),
+};
+
+/* pwm_step is not supported on all chips */
+static struct sensor_device_attribute_2 sda_pwm_step[] = {
+	SENSOR_ATTR_2(pwm1_step, S_IWUSR | S_IRUGO, show_pwm, store_pwm, 0, 4),
+	SENSOR_ATTR_2(pwm2_step, S_IWUSR | S_IRUGO, show_pwm, store_pwm, 1, 4),
+	SENSOR_ATTR_2(pwm3_step, S_IWUSR | S_IRUGO, show_pwm, store_pwm, 2, 4),
+	SENSOR_ATTR_2(pwm4_step, S_IWUSR | S_IRUGO, show_pwm, store_pwm, 3, 4),
+	SENSOR_ATTR_2(pwm5_step, S_IWUSR | S_IRUGO, show_pwm, store_pwm, 4, 4),
+};
+
+static struct attribute *nct6775_attributes_pwm[5][20] = {
+	{
+		&sensor_dev_attr_pwm1.dev_attr.attr,
+		&sensor_dev_attr_pwm1_mode.dev_attr.attr,
+		&sensor_dev_attr_pwm1_enable.dev_attr.attr,
+		&sensor_dev_attr_pwm1_temp_sel.dev_attr.attr,
+		&sensor_dev_attr_pwm1_temp_tolerance.dev_attr.attr,
+		&sensor_dev_attr_pwm1_crit_temp_tolerance.dev_attr.attr,
+		&sensor_dev_attr_pwm1_target_temp.dev_attr.attr,
+		&sensor_dev_attr_fan1_target.dev_attr.attr,
+		&sensor_dev_attr_fan1_tolerance.dev_attr.attr,
+		&sensor_dev_attr_pwm1_stop_time.dev_attr.attr,
+		&sensor_dev_attr_pwm1_step_up_time.dev_attr.attr,
+		&sensor_dev_attr_pwm1_step_down_time.dev_attr.attr,
+		&sensor_dev_attr_pwm1_start.dev_attr.attr,
+		&sensor_dev_attr_pwm1_floor.dev_attr.attr,
+		&sensor_dev_attr_pwm1_weight_temp_sel.dev_attr.attr,
+		&sensor_dev_attr_pwm1_weight_temp_step.dev_attr.attr,
+		&sensor_dev_attr_pwm1_weight_temp_step_tol.dev_attr.attr,
+		&sensor_dev_attr_pwm1_weight_temp_step_base.dev_attr.attr,
+		&sensor_dev_attr_pwm1_weight_duty_step.dev_attr.attr,
+		NULL
+	},
+	{
+		&sensor_dev_attr_pwm2.dev_attr.attr,
+		&sensor_dev_attr_pwm2_mode.dev_attr.attr,
+		&sensor_dev_attr_pwm2_enable.dev_attr.attr,
+		&sensor_dev_attr_pwm2_temp_sel.dev_attr.attr,
+		&sensor_dev_attr_pwm2_temp_tolerance.dev_attr.attr,
+		&sensor_dev_attr_pwm2_crit_temp_tolerance.dev_attr.attr,
+		&sensor_dev_attr_pwm2_target_temp.dev_attr.attr,
+		&sensor_dev_attr_fan2_target.dev_attr.attr,
+		&sensor_dev_attr_fan2_tolerance.dev_attr.attr,
+		&sensor_dev_attr_pwm2_stop_time.dev_attr.attr,
+		&sensor_dev_attr_pwm2_step_up_time.dev_attr.attr,
+		&sensor_dev_attr_pwm2_step_down_time.dev_attr.attr,
+		&sensor_dev_attr_pwm2_start.dev_attr.attr,
+		&sensor_dev_attr_pwm2_floor.dev_attr.attr,
+		&sensor_dev_attr_pwm2_weight_temp_sel.dev_attr.attr,
+		&sensor_dev_attr_pwm2_weight_temp_step.dev_attr.attr,
+		&sensor_dev_attr_pwm2_weight_temp_step_tol.dev_attr.attr,
+		&sensor_dev_attr_pwm2_weight_temp_step_base.dev_attr.attr,
+		&sensor_dev_attr_pwm2_weight_duty_step.dev_attr.attr,
+		NULL
+	},
+	{
+		&sensor_dev_attr_pwm3.dev_attr.attr,
+		&sensor_dev_attr_pwm3_mode.dev_attr.attr,
+		&sensor_dev_attr_pwm3_enable.dev_attr.attr,
+		&sensor_dev_attr_pwm3_temp_sel.dev_attr.attr,
+		&sensor_dev_attr_pwm3_temp_tolerance.dev_attr.attr,
+		&sensor_dev_attr_pwm3_crit_temp_tolerance.dev_attr.attr,
+		&sensor_dev_attr_pwm3_target_temp.dev_attr.attr,
+		&sensor_dev_attr_fan3_target.dev_attr.attr,
+		&sensor_dev_attr_fan3_tolerance.dev_attr.attr,
+		&sensor_dev_attr_pwm3_stop_time.dev_attr.attr,
+		&sensor_dev_attr_pwm3_step_up_time.dev_attr.attr,
+		&sensor_dev_attr_pwm3_step_down_time.dev_attr.attr,
+		&sensor_dev_attr_pwm3_start.dev_attr.attr,
+		&sensor_dev_attr_pwm3_floor.dev_attr.attr,
+		&sensor_dev_attr_pwm3_weight_temp_sel.dev_attr.attr,
+		&sensor_dev_attr_pwm3_weight_temp_step.dev_attr.attr,
+		&sensor_dev_attr_pwm3_weight_temp_step_tol.dev_attr.attr,
+		&sensor_dev_attr_pwm3_weight_temp_step_base.dev_attr.attr,
+		&sensor_dev_attr_pwm3_weight_duty_step.dev_attr.attr,
+		NULL
+	},
+	{
+		&sensor_dev_attr_pwm4.dev_attr.attr,
+		&sensor_dev_attr_pwm4_mode.dev_attr.attr,
+		&sensor_dev_attr_pwm4_enable.dev_attr.attr,
+		&sensor_dev_attr_pwm4_temp_sel.dev_attr.attr,
+		&sensor_dev_attr_pwm4_temp_tolerance.dev_attr.attr,
+		&sensor_dev_attr_pwm4_crit_temp_tolerance.dev_attr.attr,
+		&sensor_dev_attr_pwm4_target_temp.dev_attr.attr,
+		&sensor_dev_attr_fan4_target.dev_attr.attr,
+		&sensor_dev_attr_fan4_tolerance.dev_attr.attr,
+		&sensor_dev_attr_pwm4_stop_time.dev_attr.attr,
+		&sensor_dev_attr_pwm4_step_up_time.dev_attr.attr,
+		&sensor_dev_attr_pwm4_step_down_time.dev_attr.attr,
+		&sensor_dev_attr_pwm4_start.dev_attr.attr,
+		&sensor_dev_attr_pwm4_floor.dev_attr.attr,
+		&sensor_dev_attr_pwm4_weight_temp_sel.dev_attr.attr,
+		&sensor_dev_attr_pwm4_weight_temp_step.dev_attr.attr,
+		&sensor_dev_attr_pwm4_weight_temp_step_tol.dev_attr.attr,
+		&sensor_dev_attr_pwm4_weight_temp_step_base.dev_attr.attr,
+		&sensor_dev_attr_pwm4_weight_duty_step.dev_attr.attr,
+		NULL
+	},
+	{
+		&sensor_dev_attr_pwm5.dev_attr.attr,
+		&sensor_dev_attr_pwm5_mode.dev_attr.attr,
+		&sensor_dev_attr_pwm5_enable.dev_attr.attr,
+		&sensor_dev_attr_pwm5_temp_sel.dev_attr.attr,
+		&sensor_dev_attr_pwm5_temp_tolerance.dev_attr.attr,
+		&sensor_dev_attr_pwm5_crit_temp_tolerance.dev_attr.attr,
+		&sensor_dev_attr_pwm5_target_temp.dev_attr.attr,
+		&sensor_dev_attr_fan5_target.dev_attr.attr,
+		&sensor_dev_attr_fan5_tolerance.dev_attr.attr,
+		&sensor_dev_attr_pwm5_stop_time.dev_attr.attr,
+		&sensor_dev_attr_pwm5_step_up_time.dev_attr.attr,
+		&sensor_dev_attr_pwm5_step_down_time.dev_attr.attr,
+		&sensor_dev_attr_pwm5_start.dev_attr.attr,
+		&sensor_dev_attr_pwm5_floor.dev_attr.attr,
+		&sensor_dev_attr_pwm5_weight_temp_sel.dev_attr.attr,
+		&sensor_dev_attr_pwm5_weight_temp_step.dev_attr.attr,
+		&sensor_dev_attr_pwm5_weight_temp_step_tol.dev_attr.attr,
+		&sensor_dev_attr_pwm5_weight_temp_step_base.dev_attr.attr,
+		&sensor_dev_attr_pwm5_weight_duty_step.dev_attr.attr,
+		NULL
+	},
+};
+
+static const struct attribute_group nct6775_group_pwm[5] = {
+	{ .attrs = nct6775_attributes_pwm[0] },
+	{ .attrs = nct6775_attributes_pwm[1] },
+	{ .attrs = nct6775_attributes_pwm[2] },
+	{ .attrs = nct6775_attributes_pwm[3] },
+	{ .attrs = nct6775_attributes_pwm[4] },
+};
+
+static ssize_t
+show_auto_pwm(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+
+	return sprintf(buf, "%d\n", data->auto_pwm[sattr->nr][sattr->index]);
+}
+
+static ssize_t
+store_auto_pwm(struct device *dev, struct device_attribute *attr,
+	       const char *buf, size_t count)
+{
+	struct nct6775_data *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	int nr = sattr->nr;
+	int point = sattr->index;
+	unsigned long val;
+	int err;
+	u8 reg;
+
+	err = kstrtoul(buf, 10, &val);
+	if (err < 0)
+		return err;
+	if (val > 255)
+		return -EINVAL;
+
+	if (point == data->auto_pwm_num) {
+		if (data->kind != nct6775 && !val)
+			return -EINVAL;
+		if (data->kind != nct6779 && val)
+			val = 0xff;
+	}
+
+	mutex_lock(&data->update_lock);
+	data->auto_pwm[nr][point] = val;
+	if (point < data->auto_pwm_num) {
+		nct6775_write_value(data,
+				    NCT6775_AUTO_PWM(data, nr, point),
+				    data->auto_pwm[nr][point]);
+	} else {
+		switch (data->kind) {
+		case nct6775:
+			/* disable if needed (pwm == 0) */
+			reg = nct6775_read_value(data,
+						 NCT6775_REG_CRITICAL_ENAB[nr]);
+			if (val)
+				reg |= 0x02;
+			else
+				reg &= ~0x02;
+			nct6775_write_value(data, NCT6775_REG_CRITICAL_ENAB[nr],
+					    reg);
+			break;
+		case nct6776:
+			break; /* always enabled, nothing to do */
+		case nct6779:
+			nct6775_write_value(data, NCT6779_REG_CRITICAL_PWM[nr],
+					    val);
+			reg = nct6775_read_value(data,
+					NCT6779_REG_CRITICAL_PWM_ENABLE[nr]);
+			if (val == 255)
+				reg &= ~0x01;
+			else
+				reg |= 0x01;
+			nct6775_write_value(data,
+					    NCT6779_REG_CRITICAL_PWM_ENABLE[nr],
+					    reg);
+			break;
+		}
+	}
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+static ssize_t
+show_auto_temp(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	int nr = sattr->nr;
+	int point = sattr->index;
+
+	/*
+	 * We don't know for sure if the temperature is signed or unsigned.
+	 * Assume it is unsigned.
+	 */
+	return sprintf(buf, "%d\n", data->auto_temp[nr][point] * 1000);
+}
+
+static ssize_t
+store_auto_temp(struct device *dev, struct device_attribute *attr,
+		const char *buf, size_t count)
+{
+	struct nct6775_data *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	int nr = sattr->nr;
+	int point = sattr->index;
+	unsigned long val;
+	int err;
+
+	err = kstrtoul(buf, 10, &val);
+	if (err)
+		return err;
+	if (val > 255000)
+		return -EINVAL;
+
+	mutex_lock(&data->update_lock);
+	data->auto_temp[nr][point] = DIV_ROUND_CLOSEST(val, 1000);
+	if (point < data->auto_pwm_num) {
+		nct6775_write_value(data,
+				    NCT6775_AUTO_TEMP(data, nr, point),
+				    data->auto_temp[nr][point]);
+	} else {
+		nct6775_write_value(data, data->REG_CRITICAL_TEMP[nr],
+				    data->auto_temp[nr][point]);
+	}
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+/*
+ * The number of auto-point trip points is chip dependent.
+ * Need to check support while generating/removing attribute files.
+ */
+static struct sensor_device_attribute_2 sda_auto_pwm_arrays[] = {
+	SENSOR_ATTR_2(pwm1_auto_point1_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 0, 0),
+	SENSOR_ATTR_2(pwm1_auto_point1_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 0, 0),
+	SENSOR_ATTR_2(pwm1_auto_point2_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 0, 1),
+	SENSOR_ATTR_2(pwm1_auto_point2_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 0, 1),
+	SENSOR_ATTR_2(pwm1_auto_point3_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 0, 2),
+	SENSOR_ATTR_2(pwm1_auto_point3_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 0, 2),
+	SENSOR_ATTR_2(pwm1_auto_point4_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 0, 3),
+	SENSOR_ATTR_2(pwm1_auto_point4_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 0, 3),
+	SENSOR_ATTR_2(pwm1_auto_point5_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 0, 4),
+	SENSOR_ATTR_2(pwm1_auto_point5_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 0, 4),
+	SENSOR_ATTR_2(pwm1_auto_point6_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 0, 5),
+	SENSOR_ATTR_2(pwm1_auto_point6_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 0, 5),
+	SENSOR_ATTR_2(pwm1_auto_point7_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 0, 6),
+	SENSOR_ATTR_2(pwm1_auto_point7_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 0, 6),
+
+	SENSOR_ATTR_2(pwm2_auto_point1_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 1, 0),
+	SENSOR_ATTR_2(pwm2_auto_point1_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 1, 0),
+	SENSOR_ATTR_2(pwm2_auto_point2_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 1, 1),
+	SENSOR_ATTR_2(pwm2_auto_point2_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 1, 1),
+	SENSOR_ATTR_2(pwm2_auto_point3_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 1, 2),
+	SENSOR_ATTR_2(pwm2_auto_point3_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 1, 2),
+	SENSOR_ATTR_2(pwm2_auto_point4_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 1, 3),
+	SENSOR_ATTR_2(pwm2_auto_point4_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 1, 3),
+	SENSOR_ATTR_2(pwm2_auto_point5_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 1, 4),
+	SENSOR_ATTR_2(pwm2_auto_point5_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 1, 4),
+	SENSOR_ATTR_2(pwm2_auto_point6_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 1, 5),
+	SENSOR_ATTR_2(pwm2_auto_point6_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 1, 5),
+	SENSOR_ATTR_2(pwm2_auto_point7_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 1, 6),
+	SENSOR_ATTR_2(pwm2_auto_point7_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 1, 6),
+
+	SENSOR_ATTR_2(pwm3_auto_point1_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 2, 0),
+	SENSOR_ATTR_2(pwm3_auto_point1_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 2, 0),
+	SENSOR_ATTR_2(pwm3_auto_point2_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 2, 1),
+	SENSOR_ATTR_2(pwm3_auto_point2_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 2, 1),
+	SENSOR_ATTR_2(pwm3_auto_point3_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 2, 2),
+	SENSOR_ATTR_2(pwm3_auto_point3_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 2, 2),
+	SENSOR_ATTR_2(pwm3_auto_point4_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 2, 3),
+	SENSOR_ATTR_2(pwm3_auto_point4_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 2, 3),
+	SENSOR_ATTR_2(pwm3_auto_point5_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 2, 4),
+	SENSOR_ATTR_2(pwm3_auto_point5_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 2, 4),
+	SENSOR_ATTR_2(pwm3_auto_point6_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 2, 5),
+	SENSOR_ATTR_2(pwm3_auto_point6_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 2, 5),
+	SENSOR_ATTR_2(pwm3_auto_point7_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 2, 6),
+	SENSOR_ATTR_2(pwm3_auto_point7_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 2, 6),
+
+	SENSOR_ATTR_2(pwm4_auto_point1_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 3, 0),
+	SENSOR_ATTR_2(pwm4_auto_point1_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 3, 0),
+	SENSOR_ATTR_2(pwm4_auto_point2_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 3, 1),
+	SENSOR_ATTR_2(pwm4_auto_point2_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 3, 1),
+	SENSOR_ATTR_2(pwm4_auto_point3_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 3, 2),
+	SENSOR_ATTR_2(pwm4_auto_point3_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 3, 2),
+	SENSOR_ATTR_2(pwm4_auto_point4_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 3, 3),
+	SENSOR_ATTR_2(pwm4_auto_point4_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 3, 3),
+	SENSOR_ATTR_2(pwm4_auto_point5_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 3, 4),
+	SENSOR_ATTR_2(pwm4_auto_point5_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 3, 4),
+	SENSOR_ATTR_2(pwm4_auto_point6_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 3, 5),
+	SENSOR_ATTR_2(pwm4_auto_point6_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 3, 5),
+	SENSOR_ATTR_2(pwm4_auto_point7_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 3, 6),
+	SENSOR_ATTR_2(pwm4_auto_point7_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 3, 6),
+
+	SENSOR_ATTR_2(pwm5_auto_point1_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 4, 0),
+	SENSOR_ATTR_2(pwm5_auto_point1_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 4, 0),
+	SENSOR_ATTR_2(pwm5_auto_point2_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 4, 1),
+	SENSOR_ATTR_2(pwm5_auto_point2_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 4, 1),
+	SENSOR_ATTR_2(pwm5_auto_point3_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 4, 2),
+	SENSOR_ATTR_2(pwm5_auto_point3_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 4, 2),
+	SENSOR_ATTR_2(pwm5_auto_point4_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 4, 3),
+	SENSOR_ATTR_2(pwm5_auto_point4_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 4, 3),
+	SENSOR_ATTR_2(pwm5_auto_point5_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 4, 4),
+	SENSOR_ATTR_2(pwm5_auto_point5_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 4, 4),
+	SENSOR_ATTR_2(pwm5_auto_point6_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 4, 5),
+	SENSOR_ATTR_2(pwm5_auto_point6_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 4, 5),
+	SENSOR_ATTR_2(pwm5_auto_point7_pwm, S_IWUSR | S_IRUGO,
+		      show_auto_pwm, store_auto_pwm, 4, 6),
+	SENSOR_ATTR_2(pwm5_auto_point7_temp, S_IWUSR | S_IRUGO,
+		      show_auto_temp, store_auto_temp, 4, 6),
+};
+
+static ssize_t
+show_vid(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6775_data *data = dev_get_drvdata(dev);
+	return sprintf(buf, "%d\n", vid_from_reg(data->vid, data->vrm));
+}
+
+static DEVICE_ATTR(cpu0_vid, S_IRUGO, show_vid, NULL);
+
+/* Case open detection */
+
+static ssize_t
+clear_caseopen(struct device *dev, struct device_attribute *attr,
+	       const char *buf, size_t count)
+{
+	struct nct6775_data *data = dev_get_drvdata(dev);
+	struct nct6775_sio_data *sio_data = dev->platform_data;
+	int nr = to_sensor_dev_attr(attr)->index - INTRUSION_ALARM_BASE;
+	unsigned long val;
+	u8 reg;
+	int ret;
+
+	if (kstrtoul(buf, 10, &val) || val != 0)
+		return -EINVAL;
+
+	mutex_lock(&data->update_lock);
+
+	/*
+	 * Use CR registers to clear caseopen status.
+	 * The CR registers are the same for all chips, and not all chips
+	 * support clearing the caseopen status through "regular" registers.
+	 */
+	ret = superio_enter(sio_data->sioreg);
+	if (ret) {
+		count = ret;
+		goto error;
+	}
+
+	superio_select(sio_data->sioreg, NCT6775_LD_ACPI);
+	reg = superio_inb(sio_data->sioreg, NCT6775_REG_CR_CASEOPEN_CLR[nr]);
+	reg |= NCT6775_CR_CASEOPEN_CLR_MASK[nr];
+	superio_outb(sio_data->sioreg, NCT6775_REG_CR_CASEOPEN_CLR[nr], reg);
+	reg &= ~NCT6775_CR_CASEOPEN_CLR_MASK[nr];
+	superio_outb(sio_data->sioreg, NCT6775_REG_CR_CASEOPEN_CLR[nr], reg);
+	superio_exit(sio_data->sioreg);
+
+	data->valid = false;	/* Force cache refresh */
+error:
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+static struct sensor_device_attribute sda_caseopen[] = {
+	SENSOR_ATTR(intrusion0_alarm, S_IWUSR | S_IRUGO, show_alarm,
+		    clear_caseopen, INTRUSION_ALARM_BASE),
+	SENSOR_ATTR(intrusion1_alarm, S_IWUSR | S_IRUGO, show_alarm,
+		    clear_caseopen, INTRUSION_ALARM_BASE + 1),
+};
+
+/*
+ * Driver and device management
+ */
+
+static void nct6775_device_remove_files(struct device *dev)
+{
+	/*
+	 * some entries in the following arrays may not have been used in
+	 * device_create_file(), but device_remove_file() will ignore them
+	 */
+	int i;
+	struct nct6775_data *data = dev_get_drvdata(dev);
+
+	for (i = 0; i < data->pwm_num; i++)
+		sysfs_remove_group(&dev->kobj, &nct6775_group_pwm[i]);
+
+	for (i = 0; i < ARRAY_SIZE(sda_pwm_max); i++)
+		device_remove_file(dev, &sda_pwm_max[i].dev_attr);
+
+	for (i = 0; i < ARRAY_SIZE(sda_pwm_step); i++)
+		device_remove_file(dev, &sda_pwm_step[i].dev_attr);
+
+	for (i = 0; i < ARRAY_SIZE(sda_weight_duty_base); i++)
+		device_remove_file(dev, &sda_weight_duty_base[i].dev_attr);
+
+	for (i = 0; i < ARRAY_SIZE(sda_auto_pwm_arrays); i++)
+		device_remove_file(dev, &sda_auto_pwm_arrays[i].dev_attr);
+
+	for (i = 0; i < data->in_num; i++)
+		sysfs_remove_group(&dev->kobj, &nct6775_group_in[i]);
+
+	for (i = 0; i < 5; i++) {
+		device_remove_file(dev, &sda_fan_input[i].dev_attr);
+		device_remove_file(dev, &sda_fan_alarm[i].dev_attr);
+		device_remove_file(dev, &sda_fan_div[i].dev_attr);
+		device_remove_file(dev, &sda_fan_min[i].dev_attr);
+		device_remove_file(dev, &sda_fan_pulses[i].dev_attr);
+	}
+	for (i = 0; i < NUM_TEMP; i++) {
+		if (!(data->have_temp & (1 << i)))
+			continue;
+		device_remove_file(dev, &sda_temp_input[i].dev_attr);
+		device_remove_file(dev, &sda_temp_label[i].dev_attr);
+		device_remove_file(dev, &sda_temp_max[i].dev_attr);
+		device_remove_file(dev, &sda_temp_max_hyst[i].dev_attr);
+		device_remove_file(dev, &sda_temp_crit[i].dev_attr);
+		if (!(data->have_temp_fixed & (1 << i)))
+			continue;
+		device_remove_file(dev, &sda_temp_type[i].dev_attr);
+		device_remove_file(dev, &sda_temp_offset[i].dev_attr);
+		if (i >= NUM_TEMP_ALARM)
+			continue;
+		device_remove_file(dev, &sda_temp_alarm[i].dev_attr);
+	}
+
+	device_remove_file(dev, &sda_caseopen[0].dev_attr);
+	device_remove_file(dev, &sda_caseopen[1].dev_attr);
+
+	device_remove_file(dev, &dev_attr_name);
+	device_remove_file(dev, &dev_attr_cpu0_vid);
+}
+
+/* Get the monitoring functions started */
+static inline void nct6775_init_device(struct nct6775_data *data)
+{
+	int i;
+	u8 tmp, diode;
+
+	/* Start monitoring if needed */
+	if (data->REG_CONFIG) {
+		tmp = nct6775_read_value(data, data->REG_CONFIG);
+		if (!(tmp & 0x01))
+			nct6775_write_value(data, data->REG_CONFIG, tmp | 0x01);
+	}
+
+	/* Enable temperature sensors if needed */
+	for (i = 0; i < NUM_TEMP; i++) {
+		if (!(data->have_temp & (1 << i)))
+			continue;
+		if (!data->reg_temp_config[i])
+			continue;
+		tmp = nct6775_read_value(data, data->reg_temp_config[i]);
+		if (tmp & 0x01)
+			nct6775_write_value(data, data->reg_temp_config[i],
+					    tmp & 0xfe);
+	}
+
+	/* Enable VBAT monitoring if needed */
+	tmp = nct6775_read_value(data, data->REG_VBAT);
+	if (!(tmp & 0x01))
+		nct6775_write_value(data, data->REG_VBAT, tmp | 0x01);
+
+	diode = nct6775_read_value(data, data->REG_DIODE);
+
+	for (i = 0; i < data->temp_fixed_num; i++) {
+		if (!(data->have_temp_fixed & (1 << i)))
+			continue;
+		if ((tmp & (0x02 << i)))	/* diode */
+			data->temp_type[i] = 3 - ((diode >> i) & 0x02);
+		else				/* thermistor */
+			data->temp_type[i] = 4;
+	}
+}
+
+static int
+nct6775_check_fan_inputs(const struct nct6775_sio_data *sio_data,
+			 struct nct6775_data *data)
+{
+	int regval;
+	bool fan3pin, fan3min, fan4pin, fan4min, fan5pin;
+	bool pwm3pin, pwm4pin, pwm5pin;
+	int ret;
+
+	ret = superio_enter(sio_data->sioreg);
+	if (ret)
+		return ret;
+
+	/* fan4 and fan5 share some pins with the GPIO and serial flash */
+	if (data->kind == nct6775) {
+		regval = superio_inb(sio_data->sioreg, 0x2c);
+
+		fan3pin = regval & (1 << 6);
+		fan3min = fan3pin;
+		pwm3pin = regval & (1 << 7);
+
+		/* On NCT6775, fan4 shares pins with the fdc interface */
+		fan4pin = !(superio_inb(sio_data->sioreg, 0x2A) & 0x80);
+		fan4min = 0;
+		fan5pin = 0;
+		pwm4pin = 0;
+		pwm5pin = 0;
+	} else if (data->kind == nct6776) {
+		bool gpok = superio_inb(sio_data->sioreg, 0x27) & 0x80;
+
+		superio_select(sio_data->sioreg, NCT6775_LD_HWM);
+		regval = superio_inb(sio_data->sioreg, SIO_REG_ENABLE);
+
+		if (regval & 0x80)
+			fan3pin = gpok;
+		else
+			fan3pin = !(superio_inb(sio_data->sioreg, 0x24) & 0x40);
+
+		if (regval & 0x40)
+			fan4pin = gpok;
+		else
+			fan4pin = superio_inb(sio_data->sioreg, 0x1C) & 0x01;
+
+		if (regval & 0x20)
+			fan5pin = gpok;
+		else
+			fan5pin = superio_inb(sio_data->sioreg, 0x1C) & 0x02;
+
+		fan4min = fan4pin;
+		fan3min = fan3pin;
+		pwm3pin = fan3pin;
+		pwm4pin = 0;
+		pwm5pin = 0;
+	} else {	/* NCT6779D */
+		regval = superio_inb(sio_data->sioreg, 0x1c);
+
+		fan3pin = !(regval & (1 << 5));
+		fan4pin = !(regval & (1 << 6));
+		fan5pin = !(regval & (1 << 7));
+
+		pwm3pin = !(regval & (1 << 0));
+		pwm4pin = !(regval & (1 << 1));
+		pwm5pin = !(regval & (1 << 2));
+
+		fan3min = fan3pin;
+		fan4min = fan4pin;
+	}
+
+	superio_exit(sio_data->sioreg);
+
+	data->has_fan = data->has_fan_min = 0x03; /* fan1 and fan2 */
+	data->has_fan |= fan3pin << 2;
+	data->has_fan_min |= fan3min << 2;
+
+	data->has_fan |= (fan4pin << 3) | (fan5pin << 4);
+	data->has_fan_min |= (fan4min << 3) | (fan5pin << 4);
+
+	data->has_pwm = 0x03 | (pwm3pin << 2) | (pwm4pin << 3) | (pwm5pin << 4);
+
+	return 0;
+}
+
+static void add_temp_sensors(struct nct6775_data *data, const u16 *regp,
+			     int *available, int *mask)
+{
+	int i;
+	u8 src;
+
+	for (i = 0; i < data->pwm_num && *available; i++) {
+		int index;
+
+		if (!regp[i])
+			continue;
+		src = nct6775_read_value(data, regp[i]);
+		src &= 0x1f;
+		if (!src || (*mask & (1 << src)))
+			continue;
+		if (src >= data->temp_label_num ||
+		    !strlen(data->temp_label[src]))
+			continue;
+
+		index = __ffs(*available);
+		nct6775_write_value(data, data->REG_TEMP_SOURCE[index], src);
+		*available &= ~(1 << index);
+		*mask |= 1 << src;
+	}
+}
+
+static int nct6775_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct nct6775_sio_data *sio_data = dev->platform_data;
+	struct nct6775_data *data;
+	struct resource *res;
+	int i, s, err = 0;
+	int src, mask, available;
+	const u16 *reg_temp, *reg_temp_over, *reg_temp_hyst, *reg_temp_config;
+	const u16 *reg_temp_alternate, *reg_temp_crit;
+	int num_reg_temp;
+	bool have_vid = false;
+	u8 cr2a;
+
+	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
+	if (!devm_request_region(&pdev->dev, res->start, IOREGION_LENGTH,
+				 DRVNAME))
+		return -EBUSY;
+
+	data = devm_kzalloc(&pdev->dev, sizeof(struct nct6775_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	data->kind = sio_data->kind;
+	data->addr = res->start;
+	mutex_init(&data->update_lock);
+	data->name = nct6775_device_names[data->kind];
+	data->bank = 0xff;		/* Force initial bank selection */
+	platform_set_drvdata(pdev, data);
+
+	switch (data->kind) {
+	case nct6775:
+		data->in_num = 9;
+		data->pwm_num = 3;
+		data->auto_pwm_num = 6;
+		data->has_fan_div = true;
+		data->temp_fixed_num = 3;
+
+		data->ALARM_BITS = NCT6775_ALARM_BITS;
+
+		data->fan_from_reg = fan_from_reg16;
+		data->fan_from_reg_min = fan_from_reg8;
+		data->target_temp_mask = 0x7f;
+		data->tolerance_mask = 0x0f;
+		data->speed_tolerance_limit = 15;
+
+		data->temp_label = nct6775_temp_label;
+		data->temp_label_num = ARRAY_SIZE(nct6775_temp_label);
+
+		data->REG_CONFIG = NCT6775_REG_CONFIG;
+		data->REG_VBAT = NCT6775_REG_VBAT;
+		data->REG_DIODE = NCT6775_REG_DIODE;
+		data->REG_VIN = NCT6775_REG_IN;
+		data->REG_IN_MINMAX[0] = NCT6775_REG_IN_MIN;
+		data->REG_IN_MINMAX[1] = NCT6775_REG_IN_MAX;
+		data->REG_TARGET = NCT6775_REG_TARGET;
+		data->REG_FAN = NCT6775_REG_FAN;
+		data->REG_FAN_MODE = NCT6775_REG_FAN_MODE;
+		data->REG_FAN_MIN = NCT6775_REG_FAN_MIN;
+		data->REG_FAN_PULSES = NCT6775_REG_FAN_PULSES;
+		data->REG_FAN_TIME[0] = NCT6775_REG_FAN_STOP_TIME;
+		data->REG_FAN_TIME[1] = NCT6775_REG_FAN_STEP_UP_TIME;
+		data->REG_FAN_TIME[2] = NCT6775_REG_FAN_STEP_DOWN_TIME;
+		data->REG_PWM[0] = NCT6775_REG_PWM;
+		data->REG_PWM[1] = NCT6775_REG_FAN_START_OUTPUT;
+		data->REG_PWM[2] = NCT6775_REG_FAN_STOP_OUTPUT;
+		data->REG_PWM[3] = NCT6775_REG_FAN_MAX_OUTPUT;
+		data->REG_PWM[4] = NCT6775_REG_FAN_STEP_OUTPUT;
+		data->REG_PWM[5] = NCT6775_REG_WEIGHT_DUTY_STEP;
+		data->REG_PWM_READ = NCT6775_REG_PWM_READ;
+		data->REG_PWM_MODE = NCT6775_REG_PWM_MODE;
+		data->PWM_MODE_MASK = NCT6775_PWM_MODE_MASK;
+		data->REG_AUTO_TEMP = NCT6775_REG_AUTO_TEMP;
+		data->REG_AUTO_PWM = NCT6775_REG_AUTO_PWM;
+		data->REG_CRITICAL_TEMP = NCT6775_REG_CRITICAL_TEMP;
+		data->REG_CRITICAL_TEMP_TOLERANCE
+		  = NCT6775_REG_CRITICAL_TEMP_TOLERANCE;
+		data->REG_TEMP_OFFSET = NCT6775_REG_TEMP_OFFSET;
+		data->REG_TEMP_SOURCE = NCT6775_REG_TEMP_SOURCE;
+		data->REG_TEMP_SEL = NCT6775_REG_TEMP_SEL;
+		data->REG_WEIGHT_TEMP_SEL = NCT6775_REG_WEIGHT_TEMP_SEL;
+		data->REG_WEIGHT_TEMP[0] = NCT6775_REG_WEIGHT_TEMP_STEP;
+		data->REG_WEIGHT_TEMP[1] = NCT6775_REG_WEIGHT_TEMP_STEP_TOL;
+		data->REG_WEIGHT_TEMP[2] = NCT6775_REG_WEIGHT_TEMP_BASE;
+		data->REG_ALARM = NCT6775_REG_ALARM;
+
+		reg_temp = NCT6775_REG_TEMP;
+		num_reg_temp = ARRAY_SIZE(NCT6775_REG_TEMP);
+		reg_temp_over = NCT6775_REG_TEMP_OVER;
+		reg_temp_hyst = NCT6775_REG_TEMP_HYST;
+		reg_temp_config = NCT6775_REG_TEMP_CONFIG;
+		reg_temp_alternate = NCT6775_REG_TEMP_ALTERNATE;
+		reg_temp_crit = NCT6775_REG_TEMP_CRIT;
+
+		break;
+	case nct6776:
+		data->in_num = 9;
+		data->pwm_num = 3;
+		data->auto_pwm_num = 4;
+		data->has_fan_div = false;
+		data->temp_fixed_num = 3;
+
+		data->ALARM_BITS = NCT6776_ALARM_BITS;
+
+		data->fan_from_reg = fan_from_reg13;
+		data->fan_from_reg_min = fan_from_reg13;
+		data->target_temp_mask = 0xff;
+		data->tolerance_mask = 0x07;
+		data->speed_tolerance_limit = 63;
+
+		data->temp_label = nct6776_temp_label;
+		data->temp_label_num = ARRAY_SIZE(nct6776_temp_label);
+
+		data->REG_CONFIG = NCT6775_REG_CONFIG;
+		data->REG_VBAT = NCT6775_REG_VBAT;
+		data->REG_DIODE = NCT6775_REG_DIODE;
+		data->REG_VIN = NCT6775_REG_IN;
+		data->REG_IN_MINMAX[0] = NCT6775_REG_IN_MIN;
+		data->REG_IN_MINMAX[1] = NCT6775_REG_IN_MAX;
+		data->REG_TARGET = NCT6775_REG_TARGET;
+		data->REG_FAN = NCT6775_REG_FAN;
+		data->REG_FAN_MODE = NCT6775_REG_FAN_MODE;
+		data->REG_FAN_MIN = NCT6776_REG_FAN_MIN;
+		data->REG_FAN_PULSES = NCT6776_REG_FAN_PULSES;
+		data->REG_FAN_TIME[0] = NCT6775_REG_FAN_STOP_TIME;
+		data->REG_FAN_TIME[1] = NCT6775_REG_FAN_STEP_UP_TIME;
+		data->REG_FAN_TIME[2] = NCT6775_REG_FAN_STEP_DOWN_TIME;
+		data->REG_TOLERANCE_H = NCT6776_REG_TOLERANCE_H;
+		data->REG_PWM[0] = NCT6775_REG_PWM;
+		data->REG_PWM[1] = NCT6775_REG_FAN_START_OUTPUT;
+		data->REG_PWM[2] = NCT6775_REG_FAN_STOP_OUTPUT;
+		data->REG_PWM[5] = NCT6775_REG_WEIGHT_DUTY_STEP;
+		data->REG_PWM[6] = NCT6776_REG_WEIGHT_DUTY_BASE;
+		data->REG_PWM_READ = NCT6775_REG_PWM_READ;
+		data->REG_PWM_MODE = NCT6776_REG_PWM_MODE;
+		data->PWM_MODE_MASK = NCT6776_PWM_MODE_MASK;
+		data->REG_AUTO_TEMP = NCT6775_REG_AUTO_TEMP;
+		data->REG_AUTO_PWM = NCT6775_REG_AUTO_PWM;
+		data->REG_CRITICAL_TEMP = NCT6775_REG_CRITICAL_TEMP;
+		data->REG_CRITICAL_TEMP_TOLERANCE
+		  = NCT6775_REG_CRITICAL_TEMP_TOLERANCE;
+		data->REG_TEMP_OFFSET = NCT6775_REG_TEMP_OFFSET;
+		data->REG_TEMP_SOURCE = NCT6775_REG_TEMP_SOURCE;
+		data->REG_TEMP_SEL = NCT6775_REG_TEMP_SEL;
+		data->REG_WEIGHT_TEMP_SEL = NCT6775_REG_WEIGHT_TEMP_SEL;
+		data->REG_WEIGHT_TEMP[0] = NCT6775_REG_WEIGHT_TEMP_STEP;
+		data->REG_WEIGHT_TEMP[1] = NCT6775_REG_WEIGHT_TEMP_STEP_TOL;
+		data->REG_WEIGHT_TEMP[2] = NCT6775_REG_WEIGHT_TEMP_BASE;
+		data->REG_ALARM = NCT6775_REG_ALARM;
+
+		reg_temp = NCT6775_REG_TEMP;
+		num_reg_temp = ARRAY_SIZE(NCT6775_REG_TEMP);
+		reg_temp_over = NCT6775_REG_TEMP_OVER;
+		reg_temp_hyst = NCT6775_REG_TEMP_HYST;
+		reg_temp_config = NCT6776_REG_TEMP_CONFIG;
+		reg_temp_alternate = NCT6776_REG_TEMP_ALTERNATE;
+		reg_temp_crit = NCT6776_REG_TEMP_CRIT;
+
+		break;
+	case nct6779:
+		data->in_num = 15;
+		data->pwm_num = 5;
+		data->auto_pwm_num = 4;
+		data->has_fan_div = false;
+		data->temp_fixed_num = 6;
+
+		data->ALARM_BITS = NCT6779_ALARM_BITS;
+
+		data->fan_from_reg = fan_from_reg13;
+		data->fan_from_reg_min = fan_from_reg13;
+		data->target_temp_mask = 0xff;
+		data->tolerance_mask = 0x07;
+		data->speed_tolerance_limit = 63;
+
+		data->temp_label = nct6779_temp_label;
+		data->temp_label_num = ARRAY_SIZE(nct6779_temp_label);
+
+		data->REG_CONFIG = NCT6775_REG_CONFIG;
+		data->REG_VBAT = NCT6775_REG_VBAT;
+		data->REG_DIODE = NCT6775_REG_DIODE;
+		data->REG_VIN = NCT6779_REG_IN;
+		data->REG_IN_MINMAX[0] = NCT6775_REG_IN_MIN;
+		data->REG_IN_MINMAX[1] = NCT6775_REG_IN_MAX;
+		data->REG_TARGET = NCT6775_REG_TARGET;
+		data->REG_FAN = NCT6779_REG_FAN;
+		data->REG_FAN_MODE = NCT6775_REG_FAN_MODE;
+		data->REG_FAN_MIN = NCT6776_REG_FAN_MIN;
+		data->REG_FAN_PULSES = NCT6779_REG_FAN_PULSES;
+		data->REG_FAN_TIME[0] = NCT6775_REG_FAN_STOP_TIME;
+		data->REG_FAN_TIME[1] = NCT6775_REG_FAN_STEP_UP_TIME;
+		data->REG_FAN_TIME[2] = NCT6775_REG_FAN_STEP_DOWN_TIME;
+		data->REG_TOLERANCE_H = NCT6776_REG_TOLERANCE_H;
+		data->REG_PWM[0] = NCT6775_REG_PWM;
+		data->REG_PWM[1] = NCT6775_REG_FAN_START_OUTPUT;
+		data->REG_PWM[2] = NCT6775_REG_FAN_STOP_OUTPUT;
+		data->REG_PWM[5] = NCT6775_REG_WEIGHT_DUTY_STEP;
+		data->REG_PWM[6] = NCT6776_REG_WEIGHT_DUTY_BASE;
+		data->REG_PWM_READ = NCT6775_REG_PWM_READ;
+		data->REG_PWM_MODE = NCT6776_REG_PWM_MODE;
+		data->PWM_MODE_MASK = NCT6776_PWM_MODE_MASK;
+		data->REG_AUTO_TEMP = NCT6775_REG_AUTO_TEMP;
+		data->REG_AUTO_PWM = NCT6775_REG_AUTO_PWM;
+		data->REG_CRITICAL_TEMP = NCT6775_REG_CRITICAL_TEMP;
+		data->REG_CRITICAL_TEMP_TOLERANCE
+		  = NCT6775_REG_CRITICAL_TEMP_TOLERANCE;
+		data->REG_TEMP_OFFSET = NCT6779_REG_TEMP_OFFSET;
+		data->REG_TEMP_SOURCE = NCT6775_REG_TEMP_SOURCE;
+		data->REG_TEMP_SEL = NCT6775_REG_TEMP_SEL;
+		data->REG_WEIGHT_TEMP_SEL = NCT6775_REG_WEIGHT_TEMP_SEL;
+		data->REG_WEIGHT_TEMP[0] = NCT6775_REG_WEIGHT_TEMP_STEP;
+		data->REG_WEIGHT_TEMP[1] = NCT6775_REG_WEIGHT_TEMP_STEP_TOL;
+		data->REG_WEIGHT_TEMP[2] = NCT6775_REG_WEIGHT_TEMP_BASE;
+		data->REG_ALARM = NCT6779_REG_ALARM;
+
+		reg_temp = NCT6779_REG_TEMP;
+		num_reg_temp = ARRAY_SIZE(NCT6779_REG_TEMP);
+		reg_temp_over = NCT6779_REG_TEMP_OVER;
+		reg_temp_hyst = NCT6779_REG_TEMP_HYST;
+		reg_temp_config = NCT6779_REG_TEMP_CONFIG;
+		reg_temp_alternate = NCT6779_REG_TEMP_ALTERNATE;
+		reg_temp_crit = NCT6779_REG_TEMP_CRIT;
+
+		break;
+	default:
+		return -ENODEV;
+	}
+	data->have_in = (1 << data->in_num) - 1;
+	data->have_temp = 0;
+
+	/*
+	 * On some boards, not all available temperature sources are monitored,
+	 * even though some of the monitoring registers are unused.
+	 * Get list of unused monitoring registers, then detect if any fan
+	 * controls are configured to use unmonitored temperature sources.
+	 * If so, assign the unmonitored temperature sources to available
+	 * monitoring registers.
+	 */
+	mask = 0;
+	available = 0;
+	for (i = 0; i < num_reg_temp; i++) {
+		if (reg_temp[i] == 0)
+			continue;
+
+		src = nct6775_read_value(data, data->REG_TEMP_SOURCE[i]) & 0x1f;
+		if (!src || (mask & (1 << src)))
+			available |= 1 << i;
+
+		mask |= 1 << src;
+	}
+
+	/*
+	 * Now find unmonitored temperature registers and enable monitoring
+	 * if additional monitoring registers are available.
+	 */
+	add_temp_sensors(data, data->REG_TEMP_SEL, &available, &mask);
+	add_temp_sensors(data, data->REG_WEIGHT_TEMP_SEL, &available, &mask);
+
+	mask = 0;
+	s = NUM_TEMP_FIXED;	/* First dynamic temperature attribute */
+	for (i = 0; i < num_reg_temp; i++) {
+		if (reg_temp[i] == 0)
+			continue;
+
+		src = nct6775_read_value(data, data->REG_TEMP_SOURCE[i]) & 0x1f;
+		if (!src || (mask & (1 << src)))
+			continue;
+
+		if (src >= data->temp_label_num ||
+		    !strlen(data->temp_label[src])) {
+			dev_info(dev,
+				 "Invalid temperature source %d at index %d, source register 0x%x, temp register 0x%x\n",
+				 src, i, data->REG_TEMP_SOURCE[i], reg_temp[i]);
+			continue;
+		}
+
+		mask |= 1 << src;
+
+		/* Use fixed index for SYSTIN(1), CPUTIN(2), AUXTIN(3) */
+		if (src <= data->temp_fixed_num) {
+			data->have_temp |= 1 << (src - 1);
+			data->have_temp_fixed |= 1 << (src - 1);
+			data->reg_temp[0][src - 1] = reg_temp[i];
+			data->reg_temp[1][src - 1] = reg_temp_over[i];
+			data->reg_temp[2][src - 1] = reg_temp_hyst[i];
+			data->reg_temp_config[src - 1] = reg_temp_config[i];
+			data->temp_src[src - 1] = src;
+			continue;
+		}
+
+		if (s >= NUM_TEMP)
+			continue;
+
+		/* Use dynamic index for other sources */
+		data->have_temp |= 1 << s;
+		data->reg_temp[0][s] = reg_temp[i];
+		data->reg_temp[1][s] = reg_temp_over[i];
+		data->reg_temp[2][s] = reg_temp_hyst[i];
+		data->reg_temp_config[s] = reg_temp_config[i];
+		if (reg_temp_crit[src - 1])
+			data->reg_temp[3][s] = reg_temp_crit[src - 1];
+
+		data->temp_src[s] = src;
+		s++;
+	}
+
+#ifdef USE_ALTERNATE
+	/*
+	 * Go through the list of alternate temp registers and enable
+	 * if possible.
+	 * The temperature is already monitored if the respective bit in <mask>
+	 * is set.
+	 */
+	for (i = 0; i < data->temp_label_num - 1; i++) {
+		if (!reg_temp_alternate[i])
+			continue;
+		if (mask & (1 << (i + 1)))
+			continue;
+		if (i < data->temp_fixed_num) {
+			if (data->have_temp & (1 << i))
+				continue;
+			data->have_temp |= 1 << i;
+			data->have_temp_fixed |= 1 << i;
+			data->reg_temp[0][i] = reg_temp_alternate[i];
+			data->reg_temp[1][i] = reg_temp_over[i];
+			data->reg_temp[2][i] = reg_temp_hyst[i];
+			data->temp_src[i] = i + 1;
+			continue;
+		}
+
+		if (s >= NUM_TEMP)	/* Abort if no more space */
+			break;
+
+		data->have_temp |= 1 << s;
+		data->reg_temp[0][s] = reg_temp_alternate[i];
+		data->temp_src[s] = i + 1;
+		s++;
+	}
+#endif /* USE_ALTERNATE */
+
+	/* Initialize the chip */
+	nct6775_init_device(data);
+
+	err = superio_enter(sio_data->sioreg);
+	if (err)
+		return err;
+
+	cr2a = superio_inb(sio_data->sioreg, 0x2a);
+	switch (data->kind) {
+	case nct6775:
+		have_vid = (cr2a & 0x40);
+		break;
+	case nct6776:
+		have_vid = (cr2a & 0x60) == 0x40;
+		break;
+	case nct6779:
+		break;
+	}
+
+	/*
+	 * Read VID value
+	 * We can get the VID input values directly at logical device D 0xe3.
+	 */
+	if (have_vid) {
+		superio_select(sio_data->sioreg, NCT6775_LD_VID);
+		data->vid = superio_inb(sio_data->sioreg, 0xe3);
+		data->vrm = vid_which_vrm();
+	}
+
+	if (fan_debounce) {
+		u8 tmp;
+
+		superio_select(sio_data->sioreg, NCT6775_LD_HWM);
+		tmp = superio_inb(sio_data->sioreg,
+				  NCT6775_REG_CR_FAN_DEBOUNCE);
+		switch (data->kind) {
+		case nct6775:
+			tmp |= 0x1e;
+			break;
+		case nct6776:
+		case nct6779:
+			tmp |= 0x3e;
+			break;
+		}
+		superio_outb(sio_data->sioreg, NCT6775_REG_CR_FAN_DEBOUNCE,
+			     tmp);
+		dev_info(&pdev->dev, "Enabled fan debounce for chip %s\n",
+			 data->name);
+	}
+
+	superio_exit(sio_data->sioreg);
+
+	if (have_vid) {
+		err = device_create_file(dev, &dev_attr_cpu0_vid);
+		if (err)
+			return err;
+	}
+
+	err = nct6775_check_fan_inputs(sio_data, data);
+	if (err)
+		goto exit_remove;
+
+	/* Read fan clock dividers immediately */
+	nct6775_init_fan_common(dev, data);
+
+	/* Register sysfs hooks */
+	for (i = 0; i < data->pwm_num; i++) {
+		if (!(data->has_pwm & (1 << i)))
+			continue;
+
+		err = sysfs_create_group(&dev->kobj, &nct6775_group_pwm[i]);
+		if (err)
+			goto exit_remove;
+
+		if (data->REG_PWM[3]) {
+			err = device_create_file(dev,
+					&sda_pwm_max[i].dev_attr);
+			if (err)
+				goto exit_remove;
+		}
+		if (data->REG_PWM[4]) {
+			err = device_create_file(dev,
+					&sda_pwm_step[i].dev_attr);
+			if (err)
+				goto exit_remove;
+		}
+		if (data->REG_PWM[6]) {
+			err = device_create_file(dev,
+					&sda_weight_duty_base[i].dev_attr);
+			if (err)
+				goto exit_remove;
+		}
+	}
+	for (i = 0; i < ARRAY_SIZE(sda_auto_pwm_arrays); i++) {
+		struct sensor_device_attribute_2 *attr =
+			&sda_auto_pwm_arrays[i];
+
+		if (!(data->has_pwm & (1 << attr->nr)))
+			continue;
+		if (attr->index > data->auto_pwm_num)
+			continue;
+		err = device_create_file(dev, &attr->dev_attr);
+		if (err)
+			goto exit_remove;
+	}
+
+	for (i = 0; i < data->in_num; i++) {
+		if (!(data->have_in & (1 << i)))
+			continue;
+		err = sysfs_create_group(&dev->kobj, &nct6775_group_in[i]);
+		if (err)
+			goto exit_remove;
+	}
+
+	for (i = 0; i < 5; i++) {
+		if (data->has_fan & (1 << i)) {
+			err = device_create_file(dev,
+						 &sda_fan_input[i].dev_attr);
+			if (err)
+				goto exit_remove;
+			err = device_create_file(dev,
+						 &sda_fan_alarm[i].dev_attr);
+			if (err)
+				goto exit_remove;
+			if (data->kind != nct6776 &&
+			    data->kind != nct6779) {
+				err = device_create_file(dev,
+						&sda_fan_div[i].dev_attr);
+				if (err)
+					goto exit_remove;
+			}
+			if (data->has_fan_min & (1 << i)) {
+				err = device_create_file(dev,
+						&sda_fan_min[i].dev_attr);
+				if (err)
+					goto exit_remove;
+			}
+			err = device_create_file(dev,
+						&sda_fan_pulses[i].dev_attr);
+			if (err)
+				goto exit_remove;
+		}
+	}
+
+	for (i = 0; i < NUM_TEMP; i++) {
+		if (!(data->have_temp & (1 << i)))
+			continue;
+		err = device_create_file(dev, &sda_temp_input[i].dev_attr);
+		if (err)
+			goto exit_remove;
+		if (data->temp_label) {
+			err = device_create_file(dev,
+						 &sda_temp_label[i].dev_attr);
+			if (err)
+				goto exit_remove;
+		}
+		if (data->reg_temp[1][i]) {
+			err = device_create_file(dev,
+						 &sda_temp_max[i].dev_attr);
+			if (err)
+				goto exit_remove;
+		}
+		if (data->reg_temp[2][i]) {
+			err = device_create_file(dev,
+					&sda_temp_max_hyst[i].dev_attr);
+			if (err)
+				goto exit_remove;
+		}
+		if (data->reg_temp[3][i]) {
+			err = device_create_file(dev,
+						 &sda_temp_crit[i].dev_attr);
+			if (err)
+				goto exit_remove;
+		}
+		if (!(data->have_temp_fixed & (1 << i)))
+			continue;
+		err = device_create_file(dev, &sda_temp_type[i].dev_attr);
+		if (err)
+			goto exit_remove;
+		err = device_create_file(dev, &sda_temp_offset[i].dev_attr);
+		if (err)
+			goto exit_remove;
+		if (i >= NUM_TEMP_ALARM ||
+		    data->ALARM_BITS[TEMP_ALARM_BASE + i] < 0)
+			continue;
+		err = device_create_file(dev, &sda_temp_alarm[i].dev_attr);
+		if (err)
+			goto exit_remove;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(sda_caseopen); i++) {
+		if (data->ALARM_BITS[INTRUSION_ALARM_BASE + i] < 0)
+			continue;
+		err = device_create_file(dev, &sda_caseopen[i].dev_attr);
+		if (err)
+			goto exit_remove;
+	}
+
+	err = device_create_file(dev, &dev_attr_name);
+	if (err)
+		goto exit_remove;
+
+	data->hwmon_dev = hwmon_device_register(dev);
+	if (IS_ERR(data->hwmon_dev)) {
+		err = PTR_ERR(data->hwmon_dev);
+		goto exit_remove;
+	}
+
+	return 0;
+
+exit_remove:
+	nct6775_device_remove_files(dev);
+	return err;
+}
+
+static int nct6775_remove(struct platform_device *pdev)
+{
+	struct nct6775_data *data = platform_get_drvdata(pdev);
+
+	hwmon_device_unregister(data->hwmon_dev);
+	nct6775_device_remove_files(&pdev->dev);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int nct6775_suspend(struct device *dev)
+{
+	struct nct6775_data *data = nct6775_update_device(dev);
+	struct nct6775_sio_data *sio_data = dev->platform_data;
+
+	mutex_lock(&data->update_lock);
+	data->vbat = nct6775_read_value(data, data->REG_VBAT);
+	if (sio_data->kind == nct6775) {
+		data->fandiv1 = nct6775_read_value(data, NCT6775_REG_FANDIV1);
+		data->fandiv2 = nct6775_read_value(data, NCT6775_REG_FANDIV2);
+	}
+	mutex_unlock(&data->update_lock);
+
+	return 0;
+}
+
+static int nct6775_resume(struct device *dev)
+{
+	struct nct6775_data *data = dev_get_drvdata(dev);
+	struct nct6775_sio_data *sio_data = dev->platform_data;
+	int i, j;
+
+	mutex_lock(&data->update_lock);
+	data->bank = 0xff;		/* Force initial bank selection */
+
+	/* Restore limits */
+	for (i = 0; i < data->in_num; i++) {
+		if (!(data->have_in & (1 << i)))
+			continue;
+
+		nct6775_write_value(data, data->REG_IN_MINMAX[0][i],
+				    data->in[i][1]);
+		nct6775_write_value(data, data->REG_IN_MINMAX[1][i],
+				    data->in[i][2]);
+	}
+
+	for (i = 0; i < ARRAY_SIZE(data->fan_min); i++) {
+		if (!(data->has_fan_min & (1 << i)))
+			continue;
+
+		nct6775_write_value(data, data->REG_FAN_MIN[i],
+				    data->fan_min[i]);
+	}
+
+	for (i = 0; i < NUM_TEMP; i++) {
+		if (!(data->have_temp & (1 << i)))
+			continue;
+
+		for (j = 1; j < ARRAY_SIZE(data->reg_temp); j++)
+			if (data->reg_temp[j][i])
+				nct6775_write_temp(data, data->reg_temp[j][i],
+						   data->temp[j][i]);
+	}
+
+	/* Restore other settings */
+	nct6775_write_value(data, data->REG_VBAT, data->vbat);
+	if (sio_data->kind == nct6775) {
+		nct6775_write_value(data, NCT6775_REG_FANDIV1, data->fandiv1);
+		nct6775_write_value(data, NCT6775_REG_FANDIV2, data->fandiv2);
+	}
+
+	/* Force re-reading all values */
+	data->valid = false;
+	mutex_unlock(&data->update_lock);
+
+	return 0;
+}
+
+static const struct dev_pm_ops nct6775_dev_pm_ops = {
+	.suspend = nct6775_suspend,
+	.resume = nct6775_resume,
+};
+
+#define NCT6775_DEV_PM_OPS	(&nct6775_dev_pm_ops)
+#else
+#define NCT6775_DEV_PM_OPS	NULL
+#endif /* CONFIG_PM */
+
+static struct platform_driver nct6775_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= DRVNAME,
+		.pm	= NCT6775_DEV_PM_OPS,
+	},
+	.probe		= nct6775_probe,
+	.remove		= nct6775_remove,
+};
+
+static const char * const nct6775_sio_names[] __initconst = {
+	"NCT6775F",
+	"NCT6776D/F",
+	"NCT6779D",
+};
+
+/* nct6775_find() looks for a '627 in the Super-I/O config space */
+static int __init nct6775_find(int sioaddr, unsigned short *addr,
+			       struct nct6775_sio_data *sio_data)
+{
+	u16 val;
+	int err;
+
+	err = superio_enter(sioaddr);
+	if (err)
+		return err;
+
+	if (force_id)
+		val = force_id;
+	else
+		val = (superio_inb(sioaddr, SIO_REG_DEVID) << 8)
+		    | superio_inb(sioaddr, SIO_REG_DEVID + 1);
+	switch (val & SIO_ID_MASK) {
+	case SIO_NCT6775_ID:
+		sio_data->kind = nct6775;
+		break;
+	case SIO_NCT6776_ID:
+		sio_data->kind = nct6776;
+		break;
+	case SIO_NCT6779_ID:
+		sio_data->kind = nct6779;
+		break;
+	default:
+		if (val != 0xffff)
+			pr_debug("unsupported chip ID: 0x%04x\n", val);
+		superio_exit(sioaddr);
+		return -ENODEV;
+	}
+
+	/* We have a known chip, find the HWM I/O address */
+	superio_select(sioaddr, NCT6775_LD_HWM);
+	val = (superio_inb(sioaddr, SIO_REG_ADDR) << 8)
+	    | superio_inb(sioaddr, SIO_REG_ADDR + 1);
+	*addr = val & IOREGION_ALIGNMENT;
+	if (*addr == 0) {
+		pr_err("Refusing to enable a Super-I/O device with a base I/O port 0\n");
+		superio_exit(sioaddr);
+		return -ENODEV;
+	}
+
+	/* Activate logical device if needed */
+	val = superio_inb(sioaddr, SIO_REG_ENABLE);
+	if (!(val & 0x01)) {
+		pr_warn("Forcibly enabling Super-I/O. Sensor is probably unusable.\n");
+		superio_outb(sioaddr, SIO_REG_ENABLE, val | 0x01);
+	}
+
+	superio_exit(sioaddr);
+	pr_info("Found %s or compatible chip at %#x\n",
+		nct6775_sio_names[sio_data->kind], *addr);
+	sio_data->sioreg = sioaddr;
+
+	return 0;
+}
+
+/*
+ * when Super-I/O functions move to a separate file, the Super-I/O
+ * bus will manage the lifetime of the device and this module will only keep
+ * track of the nct6775 driver. But since we platform_device_alloc(), we
+ * must keep track of the device
+ */
+static struct platform_device *pdev;
+
+static int __init sensors_nct6775_init(void)
+{
+	int err;
+	unsigned short address;
+	struct resource res;
+	struct nct6775_sio_data sio_data;
+
+	/*
+	 * initialize sio_data->kind and sio_data->sioreg.
+	 *
+	 * when Super-I/O functions move to a separate file, the Super-I/O
+	 * driver will probe 0x2e and 0x4e and auto-detect the presence of a
+	 * nct6775 hardware monitor, and call probe()
+	 */
+	if (nct6775_find(0x2e, &address, &sio_data) &&
+	    nct6775_find(0x4e, &address, &sio_data))
+		return -ENODEV;
+
+	err = platform_driver_register(&nct6775_driver);
+	if (err)
+		goto exit;
+
+	pdev = platform_device_alloc(DRVNAME, address);
+	if (!pdev) {
+		err = -ENOMEM;
+		pr_err("Device allocation failed\n");
+		goto exit_unregister;
+	}
+
+	err = platform_device_add_data(pdev, &sio_data,
+				       sizeof(struct nct6775_sio_data));
+	if (err) {
+		pr_err("Platform data allocation failed\n");
+		goto exit_device_put;
+	}
+
+	memset(&res, 0, sizeof(res));
+	res.name = DRVNAME;
+	res.start = address + IOREGION_OFFSET;
+	res.end = address + IOREGION_OFFSET + IOREGION_LENGTH - 1;
+	res.flags = IORESOURCE_IO;
+
+	err = acpi_check_resource_conflict(&res);
+	if (err)
+		goto exit_device_put;
+
+	err = platform_device_add_resources(pdev, &res, 1);
+	if (err) {
+		pr_err("Device resource addition failed (%d)\n", err);
+		goto exit_device_put;
+	}
+
+	/* platform_device_add calls probe() */
+	err = platform_device_add(pdev);
+	if (err) {
+		pr_err("Device addition failed (%d)\n", err);
+		goto exit_device_put;
+	}
+
+	return 0;
+
+exit_device_put:
+	platform_device_put(pdev);
+exit_unregister:
+	platform_driver_unregister(&nct6775_driver);
+exit:
+	return err;
+}
+
+static void __exit sensors_nct6775_exit(void)
+{
+	platform_device_unregister(pdev);
+	platform_driver_unregister(&nct6775_driver);
+}
+
+MODULE_AUTHOR("Guenter Roeck <linux@roeck-us.net>");
+MODULE_DESCRIPTION("NCT6775F/NCT6776F/NCT6779D driver");
+MODULE_LICENSE("GPL");
+
+module_init(sensors_nct6775_init);
+module_exit(sensors_nct6775_exit);
diff --git a/drivers/hwmon/ntc_thermistor.c b/drivers/hwmon/ntc_thermistor.c
index b5f63f9c0ce..d6d640a733d 100644
--- a/drivers/hwmon/ntc_thermistor.c
+++ b/drivers/hwmon/ntc_thermistor.c
@@ -26,17 +26,33 @@
 #include <linux/math64.h>
 #include <linux/platform_device.h>
 #include <linux/err.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
 
 #include <linux/platform_data/ntc_thermistor.h>
 
+#include <linux/iio/iio.h>
+#include <linux/iio/machine.h>
+#include <linux/iio/driver.h>
+#include <linux/iio/consumer.h>
+
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
 
 struct ntc_compensation {
-	int		temp_C;
+	int		temp_c;
 	unsigned int	ohm;
 };
 
+static const struct platform_device_id ntc_thermistor_id[] = {
+	{ "ncp15wb473", TYPE_NCPXXWB473 },
+	{ "ncp18wb473", TYPE_NCPXXWB473 },
+	{ "ncp21wb473", TYPE_NCPXXWB473 },
+	{ "ncp03wb473", TYPE_NCPXXWB473 },
+	{ "ncp15wl333", TYPE_NCPXXWL333 },
+	{ },
+};
+
 /*
  * A compensation table should be sorted by the values of .ohm
  * in descending order.
@@ -44,76 +60,76 @@ struct ntc_compensation {
  * Thermistors Datasheet
  */
 static const struct ntc_compensation ncpXXwb473[] = {
-	{ .temp_C	= -40, .ohm	= 1747920 },
-	{ .temp_C	= -35, .ohm	= 1245428 },
-	{ .temp_C	= -30, .ohm	= 898485 },
-	{ .temp_C	= -25, .ohm	= 655802 },
-	{ .temp_C	= -20, .ohm	= 483954 },
-	{ .temp_C	= -15, .ohm	= 360850 },
-	{ .temp_C	= -10, .ohm	= 271697 },
-	{ .temp_C	= -5, .ohm	= 206463 },
-	{ .temp_C	= 0, .ohm	= 158214 },
-	{ .temp_C	= 5, .ohm	= 122259 },
-	{ .temp_C	= 10, .ohm	= 95227 },
-	{ .temp_C	= 15, .ohm	= 74730 },
-	{ .temp_C	= 20, .ohm	= 59065 },
-	{ .temp_C	= 25, .ohm	= 47000 },
-	{ .temp_C	= 30, .ohm	= 37643 },
-	{ .temp_C	= 35, .ohm	= 30334 },
-	{ .temp_C	= 40, .ohm	= 24591 },
-	{ .temp_C	= 45, .ohm	= 20048 },
-	{ .temp_C	= 50, .ohm	= 16433 },
-	{ .temp_C	= 55, .ohm	= 13539 },
-	{ .temp_C	= 60, .ohm	= 11209 },
-	{ .temp_C	= 65, .ohm	= 9328 },
-	{ .temp_C	= 70, .ohm	= 7798 },
-	{ .temp_C	= 75, .ohm	= 6544 },
-	{ .temp_C	= 80, .ohm	= 5518 },
-	{ .temp_C	= 85, .ohm	= 4674 },
-	{ .temp_C	= 90, .ohm	= 3972 },
-	{ .temp_C	= 95, .ohm	= 3388 },
-	{ .temp_C	= 100, .ohm	= 2902 },
-	{ .temp_C	= 105, .ohm	= 2494 },
-	{ .temp_C	= 110, .ohm	= 2150 },
-	{ .temp_C	= 115, .ohm	= 1860 },
-	{ .temp_C	= 120, .ohm	= 1615 },
-	{ .temp_C	= 125, .ohm	= 1406 },
+	{ .temp_c	= -40, .ohm	= 1747920 },
+	{ .temp_c	= -35, .ohm	= 1245428 },
+	{ .temp_c	= -30, .ohm	= 898485 },
+	{ .temp_c	= -25, .ohm	= 655802 },
+	{ .temp_c	= -20, .ohm	= 483954 },
+	{ .temp_c	= -15, .ohm	= 360850 },
+	{ .temp_c	= -10, .ohm	= 271697 },
+	{ .temp_c	= -5, .ohm	= 206463 },
+	{ .temp_c	= 0, .ohm	= 158214 },
+	{ .temp_c	= 5, .ohm	= 122259 },
+	{ .temp_c	= 10, .ohm	= 95227 },
+	{ .temp_c	= 15, .ohm	= 74730 },
+	{ .temp_c	= 20, .ohm	= 59065 },
+	{ .temp_c	= 25, .ohm	= 47000 },
+	{ .temp_c	= 30, .ohm	= 37643 },
+	{ .temp_c	= 35, .ohm	= 30334 },
+	{ .temp_c	= 40, .ohm	= 24591 },
+	{ .temp_c	= 45, .ohm	= 20048 },
+	{ .temp_c	= 50, .ohm	= 16433 },
+	{ .temp_c	= 55, .ohm	= 13539 },
+	{ .temp_c	= 60, .ohm	= 11209 },
+	{ .temp_c	= 65, .ohm	= 9328 },
+	{ .temp_c	= 70, .ohm	= 7798 },
+	{ .temp_c	= 75, .ohm	= 6544 },
+	{ .temp_c	= 80, .ohm	= 5518 },
+	{ .temp_c	= 85, .ohm	= 4674 },
+	{ .temp_c	= 90, .ohm	= 3972 },
+	{ .temp_c	= 95, .ohm	= 3388 },
+	{ .temp_c	= 100, .ohm	= 2902 },
+	{ .temp_c	= 105, .ohm	= 2494 },
+	{ .temp_c	= 110, .ohm	= 2150 },
+	{ .temp_c	= 115, .ohm	= 1860 },
+	{ .temp_c	= 120, .ohm	= 1615 },
+	{ .temp_c	= 125, .ohm	= 1406 },
 };
 static const struct ntc_compensation ncpXXwl333[] = {
-	{ .temp_C	= -40, .ohm	= 1610154 },
-	{ .temp_C	= -35, .ohm	= 1130850 },
-	{ .temp_C	= -30, .ohm	= 802609 },
-	{ .temp_C	= -25, .ohm	= 575385 },
-	{ .temp_C	= -20, .ohm	= 416464 },
-	{ .temp_C	= -15, .ohm	= 304219 },
-	{ .temp_C	= -10, .ohm	= 224193 },
-	{ .temp_C	= -5, .ohm	= 166623 },
-	{ .temp_C	= 0, .ohm	= 124850 },
-	{ .temp_C	= 5, .ohm	= 94287 },
-	{ .temp_C	= 10, .ohm	= 71747 },
-	{ .temp_C	= 15, .ohm	= 54996 },
-	{ .temp_C	= 20, .ohm	= 42455 },
-	{ .temp_C	= 25, .ohm	= 33000 },
-	{ .temp_C	= 30, .ohm	= 25822 },
-	{ .temp_C	= 35, .ohm	= 20335 },
-	{ .temp_C	= 40, .ohm	= 16115 },
-	{ .temp_C	= 45, .ohm	= 12849 },
-	{ .temp_C	= 50, .ohm	= 10306 },
-	{ .temp_C	= 55, .ohm	= 8314 },
-	{ .temp_C	= 60, .ohm	= 6746 },
-	{ .temp_C	= 65, .ohm	= 5503 },
-	{ .temp_C	= 70, .ohm	= 4513 },
-	{ .temp_C	= 75, .ohm	= 3721 },
-	{ .temp_C	= 80, .ohm	= 3084 },
-	{ .temp_C	= 85, .ohm	= 2569 },
-	{ .temp_C	= 90, .ohm	= 2151 },
-	{ .temp_C	= 95, .ohm	= 1809 },
-	{ .temp_C	= 100, .ohm	= 1529 },
-	{ .temp_C	= 105, .ohm	= 1299 },
-	{ .temp_C	= 110, .ohm	= 1108 },
-	{ .temp_C	= 115, .ohm	= 949 },
-	{ .temp_C	= 120, .ohm	= 817 },
-	{ .temp_C	= 125, .ohm	= 707 },
+	{ .temp_c	= -40, .ohm	= 1610154 },
+	{ .temp_c	= -35, .ohm	= 1130850 },
+	{ .temp_c	= -30, .ohm	= 802609 },
+	{ .temp_c	= -25, .ohm	= 575385 },
+	{ .temp_c	= -20, .ohm	= 416464 },
+	{ .temp_c	= -15, .ohm	= 304219 },
+	{ .temp_c	= -10, .ohm	= 224193 },
+	{ .temp_c	= -5, .ohm	= 166623 },
+	{ .temp_c	= 0, .ohm	= 124850 },
+	{ .temp_c	= 5, .ohm	= 94287 },
+	{ .temp_c	= 10, .ohm	= 71747 },
+	{ .temp_c	= 15, .ohm	= 54996 },
+	{ .temp_c	= 20, .ohm	= 42455 },
+	{ .temp_c	= 25, .ohm	= 33000 },
+	{ .temp_c	= 30, .ohm	= 25822 },
+	{ .temp_c	= 35, .ohm	= 20335 },
+	{ .temp_c	= 40, .ohm	= 16115 },
+	{ .temp_c	= 45, .ohm	= 12849 },
+	{ .temp_c	= 50, .ohm	= 10306 },
+	{ .temp_c	= 55, .ohm	= 8314 },
+	{ .temp_c	= 60, .ohm	= 6746 },
+	{ .temp_c	= 65, .ohm	= 5503 },
+	{ .temp_c	= 70, .ohm	= 4513 },
+	{ .temp_c	= 75, .ohm	= 3721 },
+	{ .temp_c	= 80, .ohm	= 3084 },
+	{ .temp_c	= 85, .ohm	= 2569 },
+	{ .temp_c	= 90, .ohm	= 2151 },
+	{ .temp_c	= 95, .ohm	= 1809 },
+	{ .temp_c	= 100, .ohm	= 1529 },
+	{ .temp_c	= 105, .ohm	= 1299 },
+	{ .temp_c	= 110, .ohm	= 1108 },
+	{ .temp_c	= 115, .ohm	= 949 },
+	{ .temp_c	= 120, .ohm	= 817 },
+	{ .temp_c	= 125, .ohm	= 707 },
 };
 
 struct ntc_data {
@@ -125,6 +141,92 @@ struct ntc_data {
 	char name[PLATFORM_NAME_SIZE];
 };
 
+#ifdef CONFIG_OF
+static int ntc_adc_iio_read(struct ntc_thermistor_platform_data *pdata)
+{
+	struct iio_channel *channel = pdata->chan;
+	unsigned int result;
+	int val, ret;
+
+	ret = iio_read_channel_raw(channel, &val);
+	if (ret < 0) {
+		pr_err("read channel() error: %d\n", ret);
+		return ret;
+	}
+
+	/* unit: mV */
+	result = pdata->pullup_uv * val;
+	result >>= 12;
+
+	return result;
+}
+
+static const struct of_device_id ntc_match[] = {
+	{ .compatible = "ntc,ncp15wb473",
+		.data = &ntc_thermistor_id[TYPE_NCPXXWB473] },
+	{ .compatible = "ntc,ncp18wb473",
+		.data = &ntc_thermistor_id[TYPE_NCPXXWB473] },
+	{ .compatible = "ntc,ncp21wb473",
+		.data = &ntc_thermistor_id[TYPE_NCPXXWB473] },
+	{ .compatible = "ntc,ncp03wb473",
+		.data = &ntc_thermistor_id[TYPE_NCPXXWB473] },
+	{ .compatible = "ntc,ncp15wl333",
+		.data = &ntc_thermistor_id[TYPE_NCPXXWL333] },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, ntc_match);
+
+static struct ntc_thermistor_platform_data *
+ntc_thermistor_parse_dt(struct platform_device *pdev)
+{
+	struct iio_channel *chan;
+	struct device_node *np = pdev->dev.of_node;
+	struct ntc_thermistor_platform_data *pdata;
+
+	if (!np)
+		return NULL;
+
+	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return ERR_PTR(-ENOMEM);
+
+	chan = iio_channel_get(&pdev->dev, NULL);
+	if (IS_ERR(chan))
+		return ERR_CAST(chan);
+
+	if (of_property_read_u32(np, "pullup-uv", &pdata->pullup_uv))
+		return ERR_PTR(-ENODEV);
+	if (of_property_read_u32(np, "pullup-ohm", &pdata->pullup_ohm))
+		return ERR_PTR(-ENODEV);
+	if (of_property_read_u32(np, "pulldown-ohm", &pdata->pulldown_ohm))
+		return ERR_PTR(-ENODEV);
+
+	if (of_find_property(np, "connected-positive", NULL))
+		pdata->connect = NTC_CONNECTED_POSITIVE;
+	else /* status change should be possible if not always on. */
+		pdata->connect = NTC_CONNECTED_GROUND;
+
+	pdata->chan = chan;
+	pdata->read_uv = ntc_adc_iio_read;
+
+	return pdata;
+}
+static void ntc_iio_channel_release(struct ntc_thermistor_platform_data *pdata)
+{
+	if (pdata->chan)
+		iio_channel_release(pdata->chan);
+}
+#else
+static struct ntc_thermistor_platform_data *
+ntc_thermistor_parse_dt(struct platform_device *pdev)
+{
+	return NULL;
+}
+
+static void ntc_iio_channel_release(struct ntc_thermistor_platform_data *pdata)
+{ }
+#endif
+
 static inline u64 div64_u64_safe(u64 dividend, u64 divisor)
 {
 	if (divisor == 0 && dividend == 0)
@@ -134,37 +236,37 @@ static inline u64 div64_u64_safe(u64 dividend, u64 divisor)
 	return div64_u64(dividend, divisor);
 }
 
-static int get_ohm_of_thermistor(struct ntc_data *data, unsigned int uV)
+static int get_ohm_of_thermistor(struct ntc_data *data, unsigned int uv)
 {
 	struct ntc_thermistor_platform_data *pdata = data->pdata;
-	u64 mV = uV / 1000;
-	u64 pmV = pdata->pullup_uV / 1000;
-	u64 N, puO, pdO;
-	puO = pdata->pullup_ohm;
-	pdO = pdata->pulldown_ohm;
+	u64 mv = uv / 1000;
+	u64 pmv = pdata->pullup_uv / 1000;
+	u64 n, puo, pdo;
+	puo = pdata->pullup_ohm;
+	pdo = pdata->pulldown_ohm;
 
-	if (mV == 0) {
+	if (mv == 0) {
 		if (pdata->connect == NTC_CONNECTED_POSITIVE)
 			return INT_MAX;
 		return 0;
 	}
-	if (mV >= pmV)
+	if (mv >= pmv)
 		return (pdata->connect == NTC_CONNECTED_POSITIVE) ?
 			0 : INT_MAX;
 
-	if (pdata->connect == NTC_CONNECTED_POSITIVE && puO == 0)
-		N = div64_u64_safe(pdO * (pmV - mV), mV);
-	else if (pdata->connect == NTC_CONNECTED_GROUND && pdO == 0)
-		N = div64_u64_safe(puO * mV, pmV - mV);
+	if (pdata->connect == NTC_CONNECTED_POSITIVE && puo == 0)
+		n = div64_u64_safe(pdo * (pmv - mv), mv);
+	else if (pdata->connect == NTC_CONNECTED_GROUND && pdo == 0)
+		n = div64_u64_safe(puo * mv, pmv - mv);
 	else if (pdata->connect == NTC_CONNECTED_POSITIVE)
-		N = div64_u64_safe(pdO * puO * (pmV - mV),
-				puO * mV - pdO * (pmV - mV));
+		n = div64_u64_safe(pdo * puo * (pmv - mv),
+				puo * mv - pdo * (pmv - mv));
 	else
-		N = div64_u64_safe(pdO * puO * mV, pdO * (pmV - mV) - puO * mV);
+		n = div64_u64_safe(pdo * puo * mv, pdo * (pmv - mv) - puo * mv);
 
-	if (N > INT_MAX)
-		N = INT_MAX;
-	return N;
+	if (n > INT_MAX)
+		n = INT_MAX;
+	return n;
 }
 
 static void lookup_comp(struct ntc_data *data, unsigned int ohm,
@@ -233,7 +335,7 @@ static void lookup_comp(struct ntc_data *data, unsigned int ohm,
 		*i_high = end - 1;
 }
 
-static int get_temp_mC(struct ntc_data *data, unsigned int ohm)
+static int get_temp_mc(struct ntc_data *data, unsigned int ohm)
 {
 	int low, high;
 	int temp;
@@ -241,10 +343,10 @@ static int get_temp_mC(struct ntc_data *data, unsigned int ohm)
 	lookup_comp(data, ohm, &low, &high);
 	if (low == high) {
 		/* Unable to use linear approximation */
-		temp = data->comp[low].temp_C * 1000;
+		temp = data->comp[low].temp_c * 1000;
 	} else {
-		temp = data->comp[low].temp_C * 1000 +
-			((data->comp[high].temp_C - data->comp[low].temp_C) *
+		temp = data->comp[low].temp_c * 1000 +
+			((data->comp[high].temp_c - data->comp[low].temp_c) *
 			 1000 * ((int)ohm - (int)data->comp[low].ohm)) /
 			((int)data->comp[high].ohm - (int)data->comp[low].ohm);
 	}
@@ -253,16 +355,16 @@ static int get_temp_mC(struct ntc_data *data, unsigned int ohm)
 
 static int ntc_thermistor_get_ohm(struct ntc_data *data)
 {
-	int read_uV;
+	int read_uv;
 
 	if (data->pdata->read_ohm)
 		return data->pdata->read_ohm();
 
-	if (data->pdata->read_uV) {
-		read_uV = data->pdata->read_uV();
-		if (read_uV < 0)
-			return read_uV;
-		return get_ohm_of_thermistor(data, read_uV);
+	if (data->pdata->read_uv) {
+		read_uv = data->pdata->read_uv(data->pdata);
+		if (read_uv < 0)
+			return read_uv;
+		return get_ohm_of_thermistor(data, read_uv);
 	}
 	return -EINVAL;
 }
@@ -291,7 +393,7 @@ static ssize_t ntc_show_temp(struct device *dev,
 	if (ohm < 0)
 		return ohm;
 
-	return sprintf(buf, "%d\n", get_temp_mC(data, ohm));
+	return sprintf(buf, "%d\n", get_temp_mc(data, ohm));
 }
 
 static SENSOR_DEVICE_ATTR(temp1_type, S_IRUGO, ntc_show_type, NULL, 0);
@@ -311,9 +413,18 @@ static const struct attribute_group ntc_attr_group = {
 
 static int ntc_thermistor_probe(struct platform_device *pdev)
 {
+	const struct of_device_id *of_id =
+			of_match_device(of_match_ptr(ntc_match), &pdev->dev);
+	const struct platform_device_id *pdev_id;
+	struct ntc_thermistor_platform_data *pdata;
 	struct ntc_data *data;
-	struct ntc_thermistor_platform_data *pdata = pdev->dev.platform_data;
-	int ret = 0;
+	int ret;
+
+	pdata = ntc_thermistor_parse_dt(pdev);
+	if (IS_ERR(pdata))
+		return PTR_ERR(pdata);
+	else if (pdata == NULL)
+		pdata = pdev->dev.platform_data;
 
 	if (!pdata) {
 		dev_err(&pdev->dev, "No platform init data supplied.\n");
@@ -321,19 +432,19 @@ static int ntc_thermistor_probe(struct platform_device *pdev)
 	}
 
 	/* Either one of the two is required. */
-	if (!pdata->read_uV && !pdata->read_ohm) {
+	if (!pdata->read_uv && !pdata->read_ohm) {
 		dev_err(&pdev->dev,
-			"Both read_uV and read_ohm missing. Need either one of the two.\n");
+			"Both read_uv and read_ohm missing. Need either one of the two.\n");
 		return -EINVAL;
 	}
 
-	if (pdata->read_uV && pdata->read_ohm) {
+	if (pdata->read_uv && pdata->read_ohm) {
 		dev_warn(&pdev->dev,
-			 "Only one of read_uV and read_ohm is needed; ignoring read_uV.\n");
-		pdata->read_uV = NULL;
+			 "Only one of read_uv and read_ohm is needed; ignoring read_uv.\n");
+		pdata->read_uv = NULL;
 	}
 
-	if (pdata->read_uV && (pdata->pullup_uV == 0 ||
+	if (pdata->read_uv && (pdata->pullup_uv == 0 ||
 				(pdata->pullup_ohm == 0 && pdata->connect ==
 				 NTC_CONNECTED_GROUND) ||
 				(pdata->pulldown_ohm == 0 && pdata->connect ==
@@ -341,7 +452,7 @@ static int ntc_thermistor_probe(struct platform_device *pdev)
 				(pdata->connect != NTC_CONNECTED_POSITIVE &&
 				 pdata->connect != NTC_CONNECTED_GROUND))) {
 		dev_err(&pdev->dev,
-			"Required data to use read_uV not supplied.\n");
+			"Required data to use read_uv not supplied.\n");
 		return -EINVAL;
 	}
 
@@ -349,11 +460,13 @@ static int ntc_thermistor_probe(struct platform_device *pdev)
 	if (!data)
 		return -ENOMEM;
 
+	pdev_id = of_id ? of_id->data : platform_get_device_id(pdev);
+
 	data->dev = &pdev->dev;
 	data->pdata = pdata;
-	strlcpy(data->name, pdev->id_entry->name, sizeof(data->name));
+	strlcpy(data->name, pdev_id->name, sizeof(data->name));
 
-	switch (pdev->id_entry->driver_data) {
+	switch (pdev_id->driver_data) {
 	case TYPE_NCPXXWB473:
 		data->comp = ncpXXwb473;
 		data->n_comp = ARRAY_SIZE(ncpXXwb473);
@@ -364,8 +477,7 @@ static int ntc_thermistor_probe(struct platform_device *pdev)
 		break;
 	default:
 		dev_err(&pdev->dev, "Unknown device type: %lu(%s)\n",
-				pdev->id_entry->driver_data,
-				pdev->id_entry->name);
+				pdev_id->driver_data, pdev_id->name);
 		return -EINVAL;
 	}
 
@@ -384,39 +496,34 @@ static int ntc_thermistor_probe(struct platform_device *pdev)
 		goto err_after_sysfs;
 	}
 
-	dev_info(&pdev->dev, "Thermistor %s:%d (type: %s/%lu) successfully probed.\n",
-			pdev->name, pdev->id, pdev->id_entry->name,
-			pdev->id_entry->driver_data);
+	dev_info(&pdev->dev, "Thermistor type: %s successfully probed.\n",
+								pdev->name);
+
 	return 0;
 err_after_sysfs:
 	sysfs_remove_group(&data->dev->kobj, &ntc_attr_group);
+	ntc_iio_channel_release(pdata);
 	return ret;
 }
 
 static int ntc_thermistor_remove(struct platform_device *pdev)
 {
 	struct ntc_data *data = platform_get_drvdata(pdev);
+	struct ntc_thermistor_platform_data *pdata = data->pdata;
 
 	hwmon_device_unregister(data->hwmon_dev);
 	sysfs_remove_group(&data->dev->kobj, &ntc_attr_group);
+	ntc_iio_channel_release(pdata);
 	platform_set_drvdata(pdev, NULL);
 
 	return 0;
 }
 
-static const struct platform_device_id ntc_thermistor_id[] = {
-	{ "ncp15wb473", TYPE_NCPXXWB473 },
-	{ "ncp18wb473", TYPE_NCPXXWB473 },
-	{ "ncp21wb473", TYPE_NCPXXWB473 },
-	{ "ncp03wb473", TYPE_NCPXXWB473 },
-	{ "ncp15wl333", TYPE_NCPXXWL333 },
-	{ },
-};
-
 static struct platform_driver ntc_thermistor_driver = {
 	.driver = {
 		.name = "ntc-thermistor",
 		.owner = THIS_MODULE,
+		.of_match_table = of_match_ptr(ntc_match),
 	},
 	.probe = ntc_thermistor_probe,
 	.remove = ntc_thermistor_remove,
diff --git a/drivers/hwmon/pc87360.c b/drivers/hwmon/pc87360.c
index e35856bb79b..aa615ba73d4 100644
--- a/drivers/hwmon/pc87360.c
+++ b/drivers/hwmon/pc87360.c
@@ -1190,8 +1190,7 @@ static int __init pc87360_find(int sioaddr, u8 *devid,
 				confreg[3] = superio_inb(sioaddr, 0x25);
 
 				if (confreg[2] & 0x40) {
-					pr_info("Using thermistors for "
-						"temperature monitoring\n");
+					pr_info("Using thermistors for temperature monitoring\n");
 				}
 				if (confreg[3] & 0xE0) {
 					pr_info("VID inputs routed (mode %u)\n",
@@ -1271,9 +1270,9 @@ static int pc87360_probe(struct platform_device *pdev)
 		if (data->address[i]
 		 && !devm_request_region(dev, extra_isa[i], PC87360_EXTENT,
 					 pc87360_driver.driver.name)) {
-			dev_err(dev, "Region 0x%x-0x%x already "
-				"in use!\n", extra_isa[i],
-				extra_isa[i]+PC87360_EXTENT-1);
+			dev_err(dev,
+				"Region 0x%x-0x%x already in use!\n",
+				extra_isa[i], extra_isa[i]+PC87360_EXTENT-1);
 			return -EBUSY;
 		}
 	}
@@ -1435,8 +1434,8 @@ static void pc87360_init_device(struct platform_device *pdev,
 	if (init >= 2 && data->innr) {
 		reg = pc87360_read_value(data, LD_IN, NO_BANK,
 					 PC87365_REG_IN_CONVRATE);
-		dev_info(&pdev->dev, "VLM conversion set to "
-			 "1s period, 160us delay\n");
+		dev_info(&pdev->dev,
+			 "VLM conversion set to 1s period, 160us delay\n");
 		pc87360_write_value(data, LD_IN, NO_BANK,
 				    PC87365_REG_IN_CONVRATE,
 				    (reg & 0xC0) | 0x11);
@@ -1450,8 +1449,8 @@ static void pc87360_init_device(struct platform_device *pdev,
 		if (init >= init_in[i]) {
 			/* Forcibly enable voltage channel */
 			if (!(reg & CHAN_ENA)) {
-				dev_dbg(&pdev->dev, "Forcibly "
-					"enabling in%d\n", i);
+				dev_dbg(&pdev->dev, "Forcibly enabling in%d\n",
+					i);
 				pc87360_write_value(data, LD_IN, i,
 						    PC87365_REG_IN_STATUS,
 						    (reg & 0x68) | 0x87);
@@ -1575,8 +1574,8 @@ static void pc87360_autodiv(struct device *dev, int nr)
 			data->fan_status[nr] += 0x20;
 			data->fan_min[nr] >>= 1;
 			data->fan[nr] >>= 1;
-			dev_dbg(dev, "Increasing "
-				"clock divider to %d for fan %d\n",
+			dev_dbg(dev,
+				"Increasing clock divider to %d for fan %d\n",
 				FAN_DIV_FROM_REG(data->fan_status[nr]), nr + 1);
 		}
 	} else {
@@ -1587,8 +1586,8 @@ static void pc87360_autodiv(struct device *dev, int nr)
 			data->fan_status[nr] -= 0x20;
 			data->fan_min[nr] <<= 1;
 			data->fan[nr] <<= 1;
-			dev_dbg(dev, "Decreasing "
-				"clock divider to %d for fan %d\n",
+			dev_dbg(dev,
+				"Decreasing clock divider to %d for fan %d\n",
 				FAN_DIV_FROM_REG(data->fan_status[nr]),
 				nr + 1);
 		}
diff --git a/drivers/hwmon/pc87427.c b/drivers/hwmon/pc87427.c
index 6086ad039d7..ea606860d2b 100644
--- a/drivers/hwmon/pc87427.c
+++ b/drivers/hwmon/pc87427.c
@@ -627,8 +627,9 @@ static ssize_t set_pwm(struct device *dev, struct device_attribute
 	pc87427_readall_pwm(data, nr);
 	mode = data->pwm_enable[nr] & PWM_ENABLE_MODE_MASK;
 	if (mode != PWM_MODE_MANUAL && mode != PWM_MODE_OFF) {
-		dev_notice(dev, "Can't set PWM%d duty cycle while not in "
-			   "manual mode\n", nr + 1);
+		dev_notice(dev,
+			   "Can't set PWM%d duty cycle while not in manual mode\n",
+			   nr + 1);
 		mutex_unlock(&data->lock);
 		return -EPERM;
 	}
@@ -1245,16 +1246,16 @@ static int __init pc87427_find(int sioaddr, struct pc87427_sio_data *sio_data)
 
 		val = superio_inb(sioaddr, SIOREG_MAP);
 		if (val & 0x01) {
-			pr_warn("Logical device 0x%02x is memory-mapped, "
-				"can't use\n", logdev[i]);
+			pr_warn("Logical device 0x%02x is memory-mapped, can't use\n",
+				logdev[i]);
 			continue;
 		}
 
 		val = (superio_inb(sioaddr, SIOREG_IOBASE) << 8)
 		    | superio_inb(sioaddr, SIOREG_IOBASE + 1);
 		if (!val) {
-			pr_info("I/O base address not set for logical device "
-				"0x%02x\n", logdev[i]);
+			pr_info("I/O base address not set for logical device 0x%02x\n",
+				logdev[i]);
 			continue;
 		}
 		sio_data->address[i] = val;
diff --git a/drivers/hwmon/pmbus/Kconfig b/drivers/hwmon/pmbus/Kconfig
index 4f9eb0af522..39cc63edfbb 100644
--- a/drivers/hwmon/pmbus/Kconfig
+++ b/drivers/hwmon/pmbus/Kconfig
@@ -42,17 +42,17 @@ config SENSORS_LM25066
 	default n
 	help
 	  If you say yes here you get hardware monitoring support for National
-	  Semiconductor LM25066, LM5064, and LM5066.
+	  Semiconductor LM25056, LM25066, LM5064, and LM5066.
 
 	  This driver can also be built as a module. If so, the module will
 	  be called lm25066.
 
 config SENSORS_LTC2978
-	tristate "Linear Technologies LTC2978 and LTC3880"
+	tristate "Linear Technologies LTC2974, LTC2978, LTC3880, and LTC3883"
 	default n
 	help
 	  If you say yes here you get hardware monitoring support for Linear
-	  Technology LTC2978 and LTC3880.
+	  Technology LTC2974, LTC2978, LTC3880, and LTC3883.
 
 	  This driver can also be built as a module. If so, the module will
 	  be called ltc2978.
diff --git a/drivers/hwmon/pmbus/lm25066.c b/drivers/hwmon/pmbus/lm25066.c
index c299392716a..6a9d6edaacb 100644
--- a/drivers/hwmon/pmbus/lm25066.c
+++ b/drivers/hwmon/pmbus/lm25066.c
@@ -1,7 +1,8 @@
 /*
- * Hardware monitoring driver for LM25066 / LM5064 / LM5066
+ * Hardware monitoring driver for LM25056 / LM25066 / LM5064 / LM5066
  *
  * Copyright (c) 2011 Ericsson AB.
+ * Copyright (c) 2013 Guenter Roeck
  *
  * 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
@@ -26,7 +27,7 @@
 #include <linux/i2c.h>
 #include "pmbus.h"
 
-enum chips { lm25066, lm5064, lm5066 };
+enum chips { lm25056, lm25066, lm5064, lm5066 };
 
 #define LM25066_READ_VAUX		0xd0
 #define LM25066_MFR_READ_IIN		0xd1
@@ -43,6 +44,138 @@ enum chips { lm25066, lm5064, lm5066 };
 
 #define LM25066_DEV_SETUP_CL		(1 << 4)	/* Current limit */
 
+/* LM25056 only */
+
+#define LM25056_VAUX_OV_WARN_LIMIT	0xe3
+#define LM25056_VAUX_UV_WARN_LIMIT	0xe4
+
+#define LM25056_MFR_STS_VAUX_OV_WARN	(1 << 1)
+#define LM25056_MFR_STS_VAUX_UV_WARN	(1 << 0)
+
+struct __coeff {
+	short m, b, R;
+};
+
+#define PSC_CURRENT_IN_L	(PSC_NUM_CLASSES)
+#define PSC_POWER_L		(PSC_NUM_CLASSES + 1)
+
+static struct __coeff lm25066_coeff[4][PSC_NUM_CLASSES + 2] = {
+	[lm25056] = {
+		[PSC_VOLTAGE_IN] = {
+			.m = 16296,
+			.R = -2,
+		},
+		[PSC_CURRENT_IN] = {
+			.m = 13797,
+			.R = -2,
+		},
+		[PSC_CURRENT_IN_L] = {
+			.m = 6726,
+			.R = -2,
+		},
+		[PSC_POWER] = {
+			.m = 5501,
+			.R = -3,
+		},
+		[PSC_POWER_L] = {
+			.m = 26882,
+			.R = -4,
+		},
+		[PSC_TEMPERATURE] = {
+			.m = 1580,
+			.b = -14500,
+			.R = -2,
+		},
+	},
+	[lm25066] = {
+		[PSC_VOLTAGE_IN] = {
+			.m = 22070,
+			.R = -2,
+		},
+		[PSC_VOLTAGE_OUT] = {
+			.m = 22070,
+			.R = -2,
+		},
+		[PSC_CURRENT_IN] = {
+			.m = 13661,
+			.R = -2,
+		},
+		[PSC_CURRENT_IN_L] = {
+			.m = 6852,
+			.R = -2,
+		},
+		[PSC_POWER] = {
+			.m = 736,
+			.R = -2,
+		},
+		[PSC_POWER_L] = {
+			.m = 369,
+			.R = -2,
+		},
+		[PSC_TEMPERATURE] = {
+			.m = 16,
+		},
+	},
+	[lm5064] = {
+		[PSC_VOLTAGE_IN] = {
+			.m = 4611,
+			.R = -2,
+		},
+		[PSC_VOLTAGE_OUT] = {
+			.m = 4621,
+			.R = -2,
+		},
+		[PSC_CURRENT_IN] = {
+			.m = 10742,
+			.R = -2,
+		},
+		[PSC_CURRENT_IN_L] = {
+			.m = 5456,
+			.R = -2,
+		},
+		[PSC_POWER] = {
+			.m = 1204,
+			.R = -3,
+		},
+		[PSC_POWER_L] = {
+			.m = 612,
+			.R = -3,
+		},
+		[PSC_TEMPERATURE] = {
+			.m = 16,
+		},
+	},
+	[lm5066] = {
+		[PSC_VOLTAGE_IN] = {
+			.m = 4587,
+			.R = -2,
+		},
+		[PSC_VOLTAGE_OUT] = {
+			.m = 4587,
+			.R = -2,
+		},
+		[PSC_CURRENT_IN] = {
+			.m = 10753,
+			.R = -2,
+		},
+		[PSC_CURRENT_IN_L] = {
+			.m = 5405,
+			.R = -2,
+		},
+		[PSC_POWER] = {
+			.m = 1204,
+			.R = -3,
+		},
+		[PSC_POWER_L] = {
+			.m = 605,
+			.R = -3,
+		},
+		[PSC_TEMPERATURE] = {
+			.m = 16,
+		},
+	},
+};
+
 struct lm25066_data {
 	int id;
 	struct pmbus_driver_info info;
@@ -56,42 +189,31 @@ static int lm25066_read_word_data(struct i2c_client *client, int page, int reg)
 	const struct lm25066_data *data = to_lm25066_data(info);
 	int ret;
 
-	if (page > 1)
-		return -ENXIO;
-
-	/* Map READ_VAUX into READ_VOUT register on page 1 */
-	if (page == 1) {
-		switch (reg) {
-		case PMBUS_READ_VOUT:
-			ret = pmbus_read_word_data(client, 0,
-						   LM25066_READ_VAUX);
-			if (ret < 0)
-				break;
-			/* Adjust returned value to match VOUT coefficients */
-			switch (data->id) {
-			case lm25066:
-				/* VOUT: 4.54 mV VAUX: 283.2 uV LSB */
-				ret = DIV_ROUND_CLOSEST(ret * 2832, 45400);
-				break;
-			case lm5064:
-				/* VOUT: 4.53 mV VAUX: 700 uV LSB */
-				ret = DIV_ROUND_CLOSEST(ret * 70, 453);
-				break;
-			case lm5066:
-				/* VOUT: 2.18 mV VAUX: 725 uV LSB */
-				ret = DIV_ROUND_CLOSEST(ret * 725, 2180);
-				break;
-			}
+	switch (reg) {
+	case PMBUS_VIRT_READ_VMON:
+		ret = pmbus_read_word_data(client, 0, LM25066_READ_VAUX);
+		if (ret < 0)
+			break;
+		/* Adjust returned value to match VIN coefficients */
+		switch (data->id) {
+		case lm25056:
+			/* VIN: 6.14 mV VAUX: 293 uV LSB */
+			ret = DIV_ROUND_CLOSEST(ret * 293, 6140);
 			break;
-		default:
-			/* No other valid registers on page 1 */
-			ret = -ENXIO;
+		case lm25066:
+			/* VIN: 4.54 mV VAUX: 283.2 uV LSB */
+			ret = DIV_ROUND_CLOSEST(ret * 2832, 45400);
+			break;
+		case lm5064:
+			/* VIN: 4.53 mV VAUX: 700 uV LSB */
+			ret = DIV_ROUND_CLOSEST(ret * 70, 453);
+			break;
+		case lm5066:
+			/* VIN: 2.18 mV VAUX: 725 uV LSB */
+			ret = DIV_ROUND_CLOSEST(ret * 725, 2180);
 			break;
 		}
-		goto done;
-	}
-
-	switch (reg) {
+		break;
 	case PMBUS_READ_IIN:
 		ret = pmbus_read_word_data(client, 0, LM25066_MFR_READ_IIN);
 		break;
@@ -128,7 +250,58 @@ static int lm25066_read_word_data(struct i2c_client *client, int page, int reg)
 		ret = -ENODATA;
 		break;
 	}
-done:
+	return ret;
+}
+
+static int lm25056_read_word_data(struct i2c_client *client, int page, int reg)
+{
+	int ret;
+
+	switch (reg) {
+	case PMBUS_VIRT_VMON_UV_WARN_LIMIT:
+		ret = pmbus_read_word_data(client, 0,
+					   LM25056_VAUX_UV_WARN_LIMIT);
+		if (ret < 0)
+			break;
+		/* Adjust returned value to match VIN coefficients */
+		ret = DIV_ROUND_CLOSEST(ret * 293, 6140);
+		break;
+	case PMBUS_VIRT_VMON_OV_WARN_LIMIT:
+		ret = pmbus_read_word_data(client, 0,
+					   LM25056_VAUX_OV_WARN_LIMIT);
+		if (ret < 0)
+			break;
+		/* Adjust returned value to match VIN coefficients */
+		ret = DIV_ROUND_CLOSEST(ret * 293, 6140);
+		break;
+	default:
+		ret = lm25066_read_word_data(client, page, reg);
+		break;
+	}
+	return ret;
+}
+
+static int lm25056_read_byte_data(struct i2c_client *client, int page, int reg)
+{
+	int ret, s;
+
+	switch (reg) {
+	case PMBUS_VIRT_STATUS_VMON:
+		ret = pmbus_read_byte_data(client, 0,
+					   PMBUS_STATUS_MFR_SPECIFIC);
+		if (ret < 0)
+			break;
+		s = 0;
+		if (ret & LM25056_MFR_STS_VAUX_UV_WARN)
+			s |= PB_VOLTAGE_UV_WARNING;
+		if (ret & LM25056_MFR_STS_VAUX_OV_WARN)
+			s |= PB_VOLTAGE_OV_WARNING;
+		ret = s;
+		break;
+	default:
+		ret = -ENODATA;
+		break;
+	}
 	return ret;
 }
 
@@ -137,19 +310,45 @@ static int lm25066_write_word_data(struct i2c_client *client, int page, int reg,
 {
 	int ret;
 
-	if (page > 1)
-		return -ENXIO;
-
 	switch (reg) {
+	case PMBUS_VOUT_UV_WARN_LIMIT:
+	case PMBUS_OT_FAULT_LIMIT:
+	case PMBUS_OT_WARN_LIMIT:
+	case PMBUS_VIN_UV_WARN_LIMIT:
+	case PMBUS_VIN_OV_WARN_LIMIT:
+		word = ((s16)word < 0) ? 0 : clamp_val(word, 0, 0x0fff);
+		ret = pmbus_write_word_data(client, 0, reg, word);
+		pmbus_clear_cache(client);
+		break;
 	case PMBUS_IIN_OC_WARN_LIMIT:
+		word = ((s16)word < 0) ? 0 : clamp_val(word, 0, 0x0fff);
 		ret = pmbus_write_word_data(client, 0,
 					    LM25066_MFR_IIN_OC_WARN_LIMIT,
 					    word);
+		pmbus_clear_cache(client);
 		break;
 	case PMBUS_PIN_OP_WARN_LIMIT:
+		word = ((s16)word < 0) ? 0 : clamp_val(word, 0, 0x0fff);
 		ret = pmbus_write_word_data(client, 0,
 					    LM25066_MFR_PIN_OP_WARN_LIMIT,
 					    word);
+		pmbus_clear_cache(client);
+		break;
+	case PMBUS_VIRT_VMON_UV_WARN_LIMIT:
+		/* Adjust from VIN coefficients (for LM25056) */
+		word = DIV_ROUND_CLOSEST((int)word * 6140, 293);
+		word = ((s16)word < 0) ? 0 : clamp_val(word, 0, 0x0fff);
+		ret = pmbus_write_word_data(client, 0,
+					    LM25056_VAUX_UV_WARN_LIMIT, word);
+		pmbus_clear_cache(client);
+		break;
+	case PMBUS_VIRT_VMON_OV_WARN_LIMIT:
+		/* Adjust from VIN coefficients (for LM25056) */
+		word = DIV_ROUND_CLOSEST((int)word * 6140, 293);
+		word = ((s16)word < 0) ? 0 : clamp_val(word, 0, 0x0fff);
+		ret = pmbus_write_word_data(client, 0,
+					    LM25056_VAUX_OV_WARN_LIMIT, word);
+		pmbus_clear_cache(client);
 		break;
 	case PMBUS_VIRT_RESET_PIN_HISTORY:
 		ret = pmbus_write_byte(client, 0, LM25066_CLEAR_PIN_PEAK);
@@ -161,23 +360,13 @@ static int lm25066_write_word_data(struct i2c_client *client, int page, int reg,
 	return ret;
 }
 
-static int lm25066_write_byte(struct i2c_client *client, int page, u8 value)
-{
-	if (page > 1)
-		return -ENXIO;
-
-	if (page <= 0)
-		return pmbus_write_byte(client, page, value);
-
-	return 0;
-}
-
 static int lm25066_probe(struct i2c_client *client,
 			  const struct i2c_device_id *id)
 {
 	int config;
 	struct lm25066_data *data;
 	struct pmbus_driver_info *info;
+	struct __coeff *coeff;
 
 	if (!i2c_check_functionality(client->adapter,
 				     I2C_FUNC_SMBUS_READ_BYTE_DATA))
@@ -195,107 +384,54 @@ static int lm25066_probe(struct i2c_client *client,
 	data->id = id->driver_data;
 	info = &data->info;
 
-	info->pages = 2;
+	info->pages = 1;
 	info->format[PSC_VOLTAGE_IN] = direct;
 	info->format[PSC_VOLTAGE_OUT] = direct;
 	info->format[PSC_CURRENT_IN] = direct;
 	info->format[PSC_TEMPERATURE] = direct;
 	info->format[PSC_POWER] = direct;
 
-	info->m[PSC_TEMPERATURE] = 16;
-	info->b[PSC_TEMPERATURE] = 0;
-	info->R[PSC_TEMPERATURE] = 0;
-
-	info->func[0] = PMBUS_HAVE_VIN | PMBUS_HAVE_VOUT
-	  | PMBUS_HAVE_STATUS_VOUT | PMBUS_HAVE_PIN | PMBUS_HAVE_IIN
-	  | PMBUS_HAVE_STATUS_INPUT | PMBUS_HAVE_TEMP | PMBUS_HAVE_STATUS_TEMP;
-	info->func[1] = PMBUS_HAVE_VOUT;
-
-	info->read_word_data = lm25066_read_word_data;
+	info->func[0] = PMBUS_HAVE_VIN | PMBUS_HAVE_VMON
+	  | PMBUS_HAVE_PIN | PMBUS_HAVE_IIN | PMBUS_HAVE_STATUS_INPUT
+	  | PMBUS_HAVE_TEMP | PMBUS_HAVE_STATUS_TEMP;
+
+	if (data->id == lm25056) {
+		info->func[0] |= PMBUS_HAVE_STATUS_VMON;
+		info->read_word_data = lm25056_read_word_data;
+		info->read_byte_data = lm25056_read_byte_data;
+	} else {
+		info->func[0] |= PMBUS_HAVE_VOUT | PMBUS_HAVE_STATUS_VOUT;
+		info->read_word_data = lm25066_read_word_data;
+	}
 	info->write_word_data = lm25066_write_word_data;
-	info->write_byte = lm25066_write_byte;
-
-	switch (id->driver_data) {
-	case lm25066:
-		info->m[PSC_VOLTAGE_IN] = 22070;
-		info->b[PSC_VOLTAGE_IN] = 0;
-		info->R[PSC_VOLTAGE_IN] = -2;
-		info->m[PSC_VOLTAGE_OUT] = 22070;
-		info->b[PSC_VOLTAGE_OUT] = 0;
-		info->R[PSC_VOLTAGE_OUT] = -2;
-
-		if (config & LM25066_DEV_SETUP_CL) {
-			info->m[PSC_CURRENT_IN] = 6852;
-			info->b[PSC_CURRENT_IN] = 0;
-			info->R[PSC_CURRENT_IN] = -2;
-			info->m[PSC_POWER] = 369;
-			info->b[PSC_POWER] = 0;
-			info->R[PSC_POWER] = -2;
-		} else {
-			info->m[PSC_CURRENT_IN] = 13661;
-			info->b[PSC_CURRENT_IN] = 0;
-			info->R[PSC_CURRENT_IN] = -2;
-			info->m[PSC_POWER] = 736;
-			info->b[PSC_POWER] = 0;
-			info->R[PSC_POWER] = -2;
-		}
-		break;
-	case lm5064:
-		info->m[PSC_VOLTAGE_IN] = 22075;
-		info->b[PSC_VOLTAGE_IN] = 0;
-		info->R[PSC_VOLTAGE_IN] = -2;
-		info->m[PSC_VOLTAGE_OUT] = 22075;
-		info->b[PSC_VOLTAGE_OUT] = 0;
-		info->R[PSC_VOLTAGE_OUT] = -2;
-
-		if (config & LM25066_DEV_SETUP_CL) {
-			info->m[PSC_CURRENT_IN] = 6713;
-			info->b[PSC_CURRENT_IN] = 0;
-			info->R[PSC_CURRENT_IN] = -2;
-			info->m[PSC_POWER] = 3619;
-			info->b[PSC_POWER] = 0;
-			info->R[PSC_POWER] = -3;
-		} else {
-			info->m[PSC_CURRENT_IN] = 13426;
-			info->b[PSC_CURRENT_IN] = 0;
-			info->R[PSC_CURRENT_IN] = -2;
-			info->m[PSC_POWER] = 7238;
-			info->b[PSC_POWER] = 0;
-			info->R[PSC_POWER] = -3;
-		}
-		break;
-	case lm5066:
-		info->m[PSC_VOLTAGE_IN] = 4587;
-		info->b[PSC_VOLTAGE_IN] = 0;
-		info->R[PSC_VOLTAGE_IN] = -2;
-		info->m[PSC_VOLTAGE_OUT] = 4587;
-		info->b[PSC_VOLTAGE_OUT] = 0;
-		info->R[PSC_VOLTAGE_OUT] = -2;
-
-		if (config & LM25066_DEV_SETUP_CL) {
-			info->m[PSC_CURRENT_IN] = 10753;
-			info->b[PSC_CURRENT_IN] = 0;
-			info->R[PSC_CURRENT_IN] = -2;
-			info->m[PSC_POWER] = 1204;
-			info->b[PSC_POWER] = 0;
-			info->R[PSC_POWER] = -3;
-		} else {
-			info->m[PSC_CURRENT_IN] = 5405;
-			info->b[PSC_CURRENT_IN] = 0;
-			info->R[PSC_CURRENT_IN] = -2;
-			info->m[PSC_POWER] = 605;
-			info->b[PSC_POWER] = 0;
-			info->R[PSC_POWER] = -3;
-		}
-		break;
-	default:
-		return -ENODEV;
+
+	coeff = &lm25066_coeff[data->id][0];
+	info->m[PSC_TEMPERATURE] = coeff[PSC_TEMPERATURE].m;
+	info->b[PSC_TEMPERATURE] = coeff[PSC_TEMPERATURE].b;
+	info->R[PSC_TEMPERATURE] = coeff[PSC_TEMPERATURE].R;
+	info->m[PSC_VOLTAGE_IN] = coeff[PSC_VOLTAGE_IN].m;
+	info->b[PSC_VOLTAGE_IN] = coeff[PSC_VOLTAGE_IN].b;
+	info->R[PSC_VOLTAGE_IN] = coeff[PSC_VOLTAGE_IN].R;
+	info->m[PSC_VOLTAGE_OUT] = coeff[PSC_VOLTAGE_OUT].m;
+	info->b[PSC_VOLTAGE_OUT] = coeff[PSC_VOLTAGE_OUT].b;
+	info->R[PSC_VOLTAGE_OUT] = coeff[PSC_VOLTAGE_OUT].R;
+	info->b[PSC_CURRENT_IN] = coeff[PSC_CURRENT_IN].b;
+	info->R[PSC_CURRENT_IN] = coeff[PSC_CURRENT_IN].R;
+	info->b[PSC_POWER] = coeff[PSC_POWER].b;
+	info->R[PSC_POWER] = coeff[PSC_POWER].R;
+	if (config & LM25066_DEV_SETUP_CL) {
+		info->m[PSC_CURRENT_IN] = coeff[PSC_CURRENT_IN_L].m;
+		info->m[PSC_POWER] = coeff[PSC_POWER_L].m;
+	} else {
+		info->m[PSC_CURRENT_IN] = coeff[PSC_CURRENT_IN].m;
+		info->m[PSC_POWER] = coeff[PSC_POWER].m;
 	}
 
 	return pmbus_do_probe(client, id, info);
 }
 
 static const struct i2c_device_id lm25066_id[] = {
+	{"lm25056", lm25056},
 	{"lm25066", lm25066},
 	{"lm5064", lm5064},
 	{"lm5066", lm5066},
@@ -317,5 +453,5 @@ static struct i2c_driver lm25066_driver = {
 module_i2c_driver(lm25066_driver);
 
 MODULE_AUTHOR("Guenter Roeck");
-MODULE_DESCRIPTION("PMBus driver for LM25066/LM5064/LM5066");
+MODULE_DESCRIPTION("PMBus driver for LM25056/LM25066/LM5064/LM5066");
 MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/pmbus/ltc2978.c b/drivers/hwmon/pmbus/ltc2978.c
index 6d6130752f9..586a89ef9e0 100644
--- a/drivers/hwmon/pmbus/ltc2978.c
+++ b/drivers/hwmon/pmbus/ltc2978.c
@@ -1,7 +1,8 @@
 /*
- * Hardware monitoring driver for LTC2978 and LTC3880
+ * Hardware monitoring driver for LTC2974, LTC2978, LTC3880, and LTC3883
  *
  * Copyright (c) 2011 Ericsson AB.
+ * Copyright (c) 2013 Guenter Roeck
  *
  * 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
@@ -26,28 +27,43 @@
 #include <linux/i2c.h>
 #include "pmbus.h"
 
-enum chips { ltc2978, ltc3880 };
+enum chips { ltc2974, ltc2978, ltc3880, ltc3883 };
 
-/* LTC2978 and LTC3880 */
+/* Common for all chips */
 #define LTC2978_MFR_VOUT_PEAK		0xdd
 #define LTC2978_MFR_VIN_PEAK		0xde
 #define LTC2978_MFR_TEMPERATURE_PEAK	0xdf
 #define LTC2978_MFR_SPECIAL_ID		0xe7
 
-/* LTC2978 only */
+/* LTC2974 and LTC2978 */
 #define LTC2978_MFR_VOUT_MIN		0xfb
 #define LTC2978_MFR_VIN_MIN		0xfc
 #define LTC2978_MFR_TEMPERATURE_MIN	0xfd
 
-/* LTC3880 only */
+/* LTC2974 only */
+#define LTC2974_MFR_IOUT_PEAK		0xd7
+#define LTC2974_MFR_IOUT_MIN		0xd8
+
+/* LTC3880 and LTC3883 */
 #define LTC3880_MFR_IOUT_PEAK		0xd7
 #define LTC3880_MFR_CLEAR_PEAKS		0xe3
 #define LTC3880_MFR_TEMPERATURE2_PEAK	0xf4
 
+/* LTC3883 only */
+#define LTC3883_MFR_IIN_PEAK		0xe1
+
+#define LTC2974_ID			0x0212
 #define LTC2978_ID_REV1			0x0121
 #define LTC2978_ID_REV2			0x0122
 #define LTC3880_ID			0x4000
 #define LTC3880_ID_MASK			0xff00
+#define LTC3883_ID			0x4300
+#define LTC3883_ID_MASK			0xff00
+
+#define LTC2974_NUM_PAGES		4
+#define LTC2978_NUM_PAGES		8
+#define LTC3880_NUM_PAGES		2
+#define LTC3883_NUM_PAGES		1
 
 /*
  * LTC2978 clears peak data whenever the CLEAR_FAULTS command is executed, which
@@ -56,13 +72,15 @@ enum chips { ltc2978, ltc3880 };
  * internal cache of measured peak data, which is only cleared if an explicit
  * "clear peak" command is executed for the sensor in question.
  */
+
 struct ltc2978_data {
 	enum chips id;
-	int vin_min, vin_max;
-	int temp_min, temp_max[2];
-	int vout_min[8], vout_max[8];
-	int iout_max[2];
-	int temp2_max;
+	u16 vin_min, vin_max;
+	u16 temp_min[LTC2974_NUM_PAGES], temp_max[LTC2974_NUM_PAGES];
+	u16 vout_min[LTC2978_NUM_PAGES], vout_max[LTC2978_NUM_PAGES];
+	u16 iout_min[LTC2974_NUM_PAGES], iout_max[LTC2974_NUM_PAGES];
+	u16 iin_max;
+	u16 temp2_max;
 	struct pmbus_driver_info info;
 };
 
@@ -167,9 +185,9 @@ static int ltc2978_read_word_data(struct i2c_client *client, int page, int reg)
 					   LTC2978_MFR_TEMPERATURE_MIN);
 		if (ret >= 0) {
 			if (lin11_to_val(ret)
-			    < lin11_to_val(data->temp_min))
-				data->temp_min = ret;
-			ret = data->temp_min;
+			    < lin11_to_val(data->temp_min[page]))
+				data->temp_min[page] = ret;
+			ret = data->temp_min[page];
 		}
 		break;
 	case PMBUS_VIRT_READ_IOUT_MAX:
@@ -185,6 +203,41 @@ static int ltc2978_read_word_data(struct i2c_client *client, int page, int reg)
 	return ret;
 }
 
+static int ltc2974_read_word_data(struct i2c_client *client, int page, int reg)
+{
+	const struct pmbus_driver_info *info = pmbus_get_driver_info(client);
+	struct ltc2978_data *data = to_ltc2978_data(info);
+	int ret;
+
+	switch (reg) {
+	case PMBUS_VIRT_READ_IOUT_MAX:
+		ret = pmbus_read_word_data(client, page, LTC2974_MFR_IOUT_PEAK);
+		if (ret >= 0) {
+			if (lin11_to_val(ret)
+			    > lin11_to_val(data->iout_max[page]))
+				data->iout_max[page] = ret;
+			ret = data->iout_max[page];
+		}
+		break;
+	case PMBUS_VIRT_READ_IOUT_MIN:
+		ret = pmbus_read_word_data(client, page, LTC2974_MFR_IOUT_MIN);
+		if (ret >= 0) {
+			if (lin11_to_val(ret)
+			    < lin11_to_val(data->iout_min[page]))
+				data->iout_min[page] = ret;
+			ret = data->iout_min[page];
+		}
+		break;
+	case PMBUS_VIRT_RESET_IOUT_HISTORY:
+		ret = 0;
+		break;
+	default:
+		ret = ltc2978_read_word_data(client, page, reg);
+		break;
+	}
+	return ret;
+}
+
 static int ltc3880_read_word_data(struct i2c_client *client, int page, int reg)
 {
 	const struct pmbus_driver_info *info = pmbus_get_driver_info(client);
@@ -226,15 +279,41 @@ static int ltc3880_read_word_data(struct i2c_client *client, int page, int reg)
 	return ret;
 }
 
+static int ltc3883_read_word_data(struct i2c_client *client, int page, int reg)
+{
+	const struct pmbus_driver_info *info = pmbus_get_driver_info(client);
+	struct ltc2978_data *data = to_ltc2978_data(info);
+	int ret;
+
+	switch (reg) {
+	case PMBUS_VIRT_READ_IIN_MAX:
+		ret = pmbus_read_word_data(client, page, LTC3883_MFR_IIN_PEAK);
+		if (ret >= 0) {
+			if (lin11_to_val(ret)
+			    > lin11_to_val(data->iin_max))
+				data->iin_max = ret;
+			ret = data->iin_max;
+		}
+		break;
+	case PMBUS_VIRT_RESET_IIN_HISTORY:
+		ret = 0;
+		break;
+	default:
+		ret = ltc3880_read_word_data(client, page, reg);
+		break;
+	}
+	return ret;
+}
+
 static int ltc2978_clear_peaks(struct i2c_client *client, int page,
 			       enum chips id)
 {
 	int ret;
 
-	if (id == ltc2978)
-		ret = pmbus_write_byte(client, page, PMBUS_CLEAR_FAULTS);
-	else
+	if (id == ltc3880 || id == ltc3883)
 		ret = pmbus_write_byte(client, 0, LTC3880_MFR_CLEAR_PEAKS);
+	else
+		ret = pmbus_write_byte(client, page, PMBUS_CLEAR_FAULTS);
 
 	return ret;
 }
@@ -247,8 +326,13 @@ static int ltc2978_write_word_data(struct i2c_client *client, int page,
 	int ret;
 
 	switch (reg) {
+	case PMBUS_VIRT_RESET_IIN_HISTORY:
+		data->iin_max = 0x7c00;
+		ret = ltc2978_clear_peaks(client, page, data->id);
+		break;
 	case PMBUS_VIRT_RESET_IOUT_HISTORY:
 		data->iout_max[page] = 0x7c00;
+		data->iout_min[page] = 0xfbff;
 		ret = ltc2978_clear_peaks(client, page, data->id);
 		break;
 	case PMBUS_VIRT_RESET_TEMP2_HISTORY:
@@ -266,7 +350,7 @@ static int ltc2978_write_word_data(struct i2c_client *client, int page,
 		ret = ltc2978_clear_peaks(client, page, data->id);
 		break;
 	case PMBUS_VIRT_RESET_TEMP_HISTORY:
-		data->temp_min = 0x7bff;
+		data->temp_min[page] = 0x7bff;
 		data->temp_max[page] = 0x7c00;
 		ret = ltc2978_clear_peaks(client, page, data->id);
 		break;
@@ -278,8 +362,10 @@ static int ltc2978_write_word_data(struct i2c_client *client, int page,
 }
 
 static const struct i2c_device_id ltc2978_id[] = {
+	{"ltc2974", ltc2974},
 	{"ltc2978", ltc2978},
 	{"ltc3880", ltc3880},
+	{"ltc3883", ltc3883},
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, ltc2978_id);
@@ -304,10 +390,14 @@ static int ltc2978_probe(struct i2c_client *client,
 	if (chip_id < 0)
 		return chip_id;
 
-	if (chip_id == LTC2978_ID_REV1 || chip_id == LTC2978_ID_REV2) {
+	if (chip_id == LTC2974_ID) {
+		data->id = ltc2974;
+	} else if (chip_id == LTC2978_ID_REV1 || chip_id == LTC2978_ID_REV2) {
 		data->id = ltc2978;
 	} else if ((chip_id & LTC3880_ID_MASK) == LTC3880_ID) {
 		data->id = ltc3880;
+	} else if ((chip_id & LTC3883_ID_MASK) == LTC3883_ID) {
+		data->id = ltc3883;
 	} else {
 		dev_err(&client->dev, "Unsupported chip ID 0x%x\n", chip_id);
 		return -ENODEV;
@@ -323,26 +413,45 @@ static int ltc2978_probe(struct i2c_client *client,
 
 	data->vin_min = 0x7bff;
 	data->vin_max = 0x7c00;
-	data->temp_min = 0x7bff;
+	for (i = 0; i < ARRAY_SIZE(data->vout_min); i++)
+		data->vout_min[i] = 0xffff;
+	for (i = 0; i < ARRAY_SIZE(data->iout_min); i++)
+		data->iout_min[i] = 0xfbff;
+	for (i = 0; i < ARRAY_SIZE(data->iout_max); i++)
+		data->iout_max[i] = 0x7c00;
+	for (i = 0; i < ARRAY_SIZE(data->temp_min); i++)
+		data->temp_min[i] = 0x7bff;
 	for (i = 0; i < ARRAY_SIZE(data->temp_max); i++)
 		data->temp_max[i] = 0x7c00;
 	data->temp2_max = 0x7c00;
 
 	switch (data->id) {
+	case ltc2974:
+		info->read_word_data = ltc2974_read_word_data;
+		info->pages = LTC2974_NUM_PAGES;
+		info->func[0] = PMBUS_HAVE_VIN | PMBUS_HAVE_STATUS_INPUT
+		  | PMBUS_HAVE_TEMP2;
+		for (i = 0; i < info->pages; i++) {
+			info->func[i] |= PMBUS_HAVE_VOUT
+			  | PMBUS_HAVE_STATUS_VOUT | PMBUS_HAVE_POUT
+			  | PMBUS_HAVE_TEMP | PMBUS_HAVE_STATUS_TEMP
+			  | PMBUS_HAVE_IOUT | PMBUS_HAVE_STATUS_IOUT;
+		}
+		break;
 	case ltc2978:
 		info->read_word_data = ltc2978_read_word_data;
-		info->pages = 8;
+		info->pages = LTC2978_NUM_PAGES;
 		info->func[0] = PMBUS_HAVE_VIN | PMBUS_HAVE_STATUS_INPUT
 		  | PMBUS_HAVE_VOUT | PMBUS_HAVE_STATUS_VOUT
 		  | PMBUS_HAVE_TEMP | PMBUS_HAVE_STATUS_TEMP;
-		for (i = 1; i < 8; i++) {
+		for (i = 1; i < LTC2978_NUM_PAGES; i++) {
 			info->func[i] = PMBUS_HAVE_VOUT
 			  | PMBUS_HAVE_STATUS_VOUT;
 		}
 		break;
 	case ltc3880:
 		info->read_word_data = ltc3880_read_word_data;
-		info->pages = 2;
+		info->pages = LTC3880_NUM_PAGES;
 		info->func[0] = PMBUS_HAVE_VIN | PMBUS_HAVE_IIN
 		  | PMBUS_HAVE_STATUS_INPUT
 		  | PMBUS_HAVE_VOUT | PMBUS_HAVE_STATUS_VOUT
@@ -353,15 +462,20 @@ static int ltc2978_probe(struct i2c_client *client,
 		  | PMBUS_HAVE_IOUT | PMBUS_HAVE_STATUS_IOUT
 		  | PMBUS_HAVE_POUT
 		  | PMBUS_HAVE_TEMP | PMBUS_HAVE_STATUS_TEMP;
-		data->iout_max[0] = 0x7c00;
-		data->iout_max[1] = 0x7c00;
+		break;
+	case ltc3883:
+		info->read_word_data = ltc3883_read_word_data;
+		info->pages = LTC3883_NUM_PAGES;
+		info->func[0] = PMBUS_HAVE_VIN | PMBUS_HAVE_IIN
+		  | PMBUS_HAVE_STATUS_INPUT
+		  | PMBUS_HAVE_VOUT | PMBUS_HAVE_STATUS_VOUT
+		  | PMBUS_HAVE_IOUT | PMBUS_HAVE_STATUS_IOUT
+		  | PMBUS_HAVE_PIN | PMBUS_HAVE_POUT | PMBUS_HAVE_TEMP
+		  | PMBUS_HAVE_TEMP2 | PMBUS_HAVE_STATUS_TEMP;
 		break;
 	default:
 		return -ENODEV;
 	}
-	for (i = 0; i < info->pages; i++)
-		data->vout_min[i] = 0xffff;
-
 	return pmbus_do_probe(client, id, info);
 }
 
@@ -378,5 +492,5 @@ static struct i2c_driver ltc2978_driver = {
 module_i2c_driver(ltc2978_driver);
 
 MODULE_AUTHOR("Guenter Roeck");
-MODULE_DESCRIPTION("PMBus driver for LTC2978 and LTC3880");
+MODULE_DESCRIPTION("PMBus driver for LTC2974, LTC2978, LTC3880, and LTC3883");
 MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/s3c-hwmon.c b/drivers/hwmon/s3c-hwmon.c
index ff2ae0252a4..a9f7e804f1e 100644
--- a/drivers/hwmon/s3c-hwmon.c
+++ b/drivers/hwmon/s3c-hwmon.c
@@ -107,17 +107,14 @@ static ssize_t s3c_hwmon_show_raw(struct device *dev,
 	return  (ret < 0) ? ret : snprintf(buf, PAGE_SIZE, "%d\n", ret);
 }
 
-#define DEF_ADC_ATTR(x)	\
-	static SENSOR_DEVICE_ATTR(adc##x##_raw, S_IRUGO, s3c_hwmon_show_raw, NULL, x)
-
-DEF_ADC_ATTR(0);
-DEF_ADC_ATTR(1);
-DEF_ADC_ATTR(2);
-DEF_ADC_ATTR(3);
-DEF_ADC_ATTR(4);
-DEF_ADC_ATTR(5);
-DEF_ADC_ATTR(6);
-DEF_ADC_ATTR(7);
+static SENSOR_DEVICE_ATTR(adc0_raw, S_IRUGO, s3c_hwmon_show_raw, NULL, 0);
+static SENSOR_DEVICE_ATTR(adc1_raw, S_IRUGO, s3c_hwmon_show_raw, NULL, 1);
+static SENSOR_DEVICE_ATTR(adc2_raw, S_IRUGO, s3c_hwmon_show_raw, NULL, 2);
+static SENSOR_DEVICE_ATTR(adc3_raw, S_IRUGO, s3c_hwmon_show_raw, NULL, 3);
+static SENSOR_DEVICE_ATTR(adc4_raw, S_IRUGO, s3c_hwmon_show_raw, NULL, 4);
+static SENSOR_DEVICE_ATTR(adc5_raw, S_IRUGO, s3c_hwmon_show_raw, NULL, 5);
+static SENSOR_DEVICE_ATTR(adc6_raw, S_IRUGO, s3c_hwmon_show_raw, NULL, 6);
+static SENSOR_DEVICE_ATTR(adc7_raw, S_IRUGO, s3c_hwmon_show_raw, NULL, 7);
 
 static struct attribute *s3c_hwmon_attrs[9] = {
 	&sensor_dev_attr_adc0_raw.dev_attr.attr,
diff --git a/drivers/hwmon/sch56xx-common.c b/drivers/hwmon/sch56xx-common.c
index d00b30adc34..73868198328 100644
--- a/drivers/hwmon/sch56xx-common.c
+++ b/drivers/hwmon/sch56xx-common.c
@@ -161,8 +161,8 @@ static int sch56xx_send_cmd(u16 addr, u8 cmd, u16 reg, u8 v)
 			break;
 	}
 	if (i == max_busy_polls + max_lazy_polls) {
-		pr_err("Max retries exceeded reading virtual "
-		       "register 0x%04hx (%d)\n", reg, 1);
+		pr_err("Max retries exceeded reading virtual register 0x%04hx (%d)\n",
+		       reg, 1);
 		return -EIO;
 	}
 
@@ -178,12 +178,12 @@ static int sch56xx_send_cmd(u16 addr, u8 cmd, u16 reg, u8 v)
 			break;
 
 		if (i == 0)
-			pr_warn("EC reports: 0x%02x reading virtual register "
-				"0x%04hx\n", (unsigned int)val, reg);
+			pr_warn("EC reports: 0x%02x reading virtual register 0x%04hx\n",
+				(unsigned int)val, reg);
 	}
 	if (i == max_busy_polls) {
-		pr_err("Max retries exceeded reading virtual "
-		       "register 0x%04hx (%d)\n", reg, 2);
+		pr_err("Max retries exceeded reading virtual register 0x%04hx (%d)\n",
+		       reg, 2);
 		return -EIO;
 	}
 
diff --git a/drivers/hwmon/sis5595.c b/drivers/hwmon/sis5595.c
index c35847a1a0a..1404e6319de 100644
--- a/drivers/hwmon/sis5595.c
+++ b/drivers/hwmon/sis5595.c
@@ -456,8 +456,9 @@ static ssize_t set_fan_div(struct device *dev, struct device_attribute *da,
 		data->fan_div[nr] = 3;
 		break;
 	default:
-		dev_err(dev, "fan_div value %ld not "
-			"supported. Choose one of 1, 2, 4 or 8!\n", val);
+		dev_err(dev,
+			"fan_div value %ld not supported. Choose one of 1, 2, 4 or 8!\n",
+			val);
 		mutex_unlock(&data->update_lock);
 		return -EINVAL;
 	}
diff --git a/drivers/hwmon/thmc50.c b/drivers/hwmon/thmc50.c
index 4b59eb53b18..db288db7d3e 100644
--- a/drivers/hwmon/thmc50.c
+++ b/drivers/hwmon/thmc50.c
@@ -41,8 +41,8 @@ enum chips { thmc50, adm1022 };
 static unsigned short adm1022_temp3[16];
 static unsigned int adm1022_temp3_num;
 module_param_array(adm1022_temp3, ushort, &adm1022_temp3_num, 0);
-MODULE_PARM_DESC(adm1022_temp3, "List of adapter,address pairs "
-			"to enable 3rd temperature (ADM1022 only)");
+MODULE_PARM_DESC(adm1022_temp3,
+		 "List of adapter,address pairs to enable 3rd temperature (ADM1022 only)");
 
 /* Many THMC50 constants specified below */
 
@@ -312,8 +312,7 @@ static int thmc50_detect(struct i2c_client *client,
 	const char *type_name;
 
 	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA)) {
-		pr_debug("thmc50: detect failed, "
-			 "smbus byte data not supported!\n");
+		pr_debug("thmc50: detect failed, smbus byte data not supported!\n");
 		return -ENODEV;
 	}
 
diff --git a/drivers/hwmon/tmp102.c b/drivers/hwmon/tmp102.c
index 523dd89ba49..d7b47abf37f 100644
--- a/drivers/hwmon/tmp102.c
+++ b/drivers/hwmon/tmp102.c
@@ -155,8 +155,8 @@ static int tmp102_probe(struct i2c_client *client,
 
 	if (!i2c_check_functionality(client->adapter,
 				     I2C_FUNC_SMBUS_WORD_DATA)) {
-		dev_err(&client->dev, "adapter doesn't support SMBus word "
-			"transactions\n");
+		dev_err(&client->dev,
+			"adapter doesn't support SMBus word transactions\n");
 		return -ENODEV;
 	}
 
diff --git a/drivers/hwmon/tmp401.c b/drivers/hwmon/tmp401.c
index c85f6967ccc..a478454f690 100644
--- a/drivers/hwmon/tmp401.c
+++ b/drivers/hwmon/tmp401.c
@@ -5,6 +5,9 @@
  * Gabriel Konat, Sander Leget, Wouter Willems
  * Copyright (C) 2009 Andre Prendel <andre.prendel@gmx.de>
  *
+ * Cleanup and support for TMP431 and TMP432 by Guenter Roeck
+ * Copyright (c) 2013 Guenter Roeck <linux@roeck-us.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
@@ -30,6 +33,7 @@
 
 #include <linux/module.h>
 #include <linux/init.h>
+#include <linux/bitops.h>
 #include <linux/slab.h>
 #include <linux/jiffies.h>
 #include <linux/i2c.h>
@@ -40,9 +44,9 @@
 #include <linux/sysfs.h>
 
 /* Addresses to scan */
-static const unsigned short normal_i2c[] = { 0x4c, I2C_CLIENT_END };
+static const unsigned short normal_i2c[] = { 0x4c, 0x4d, 0x4e, I2C_CLIENT_END };
 
-enum chips { tmp401, tmp411 };
+enum chips { tmp401, tmp411, tmp431, tmp432 };
 
 /*
  * The TMP401 registers, note some registers have different addresses for
@@ -54,42 +58,84 @@ enum chips { tmp401, tmp411 };
 #define TMP401_CONVERSION_RATE_READ		0x04
 #define TMP401_CONVERSION_RATE_WRITE		0x0A
 #define TMP401_TEMP_CRIT_HYST			0x21
-#define TMP401_CONSECUTIVE_ALERT		0x22
 #define TMP401_MANUFACTURER_ID_REG		0xFE
 #define TMP401_DEVICE_ID_REG			0xFF
-#define TMP411_N_FACTOR_REG			0x18
-
-static const u8 TMP401_TEMP_MSB[2]			= { 0x00, 0x01 };
-static const u8 TMP401_TEMP_LSB[2]			= { 0x15, 0x10 };
-static const u8 TMP401_TEMP_LOW_LIMIT_MSB_READ[2]	= { 0x06, 0x08 };
-static const u8 TMP401_TEMP_LOW_LIMIT_MSB_WRITE[2]	= { 0x0C, 0x0E };
-static const u8 TMP401_TEMP_LOW_LIMIT_LSB[2]		= { 0x17, 0x14 };
-static const u8 TMP401_TEMP_HIGH_LIMIT_MSB_READ[2]	= { 0x05, 0x07 };
-static const u8 TMP401_TEMP_HIGH_LIMIT_MSB_WRITE[2]	= { 0x0B, 0x0D };
-static const u8 TMP401_TEMP_HIGH_LIMIT_LSB[2]		= { 0x16, 0x13 };
-/* These are called the THERM limit / hysteresis / mask in the datasheet */
-static const u8 TMP401_TEMP_CRIT_LIMIT[2]		= { 0x20, 0x19 };
-
-static const u8 TMP411_TEMP_LOWEST_MSB[2]		= { 0x30, 0x34 };
-static const u8 TMP411_TEMP_LOWEST_LSB[2]		= { 0x31, 0x35 };
-static const u8 TMP411_TEMP_HIGHEST_MSB[2]		= { 0x32, 0x36 };
-static const u8 TMP411_TEMP_HIGHEST_LSB[2]		= { 0x33, 0x37 };
+
+static const u8 TMP401_TEMP_MSB_READ[6][2] = {
+	{ 0x00, 0x01 },	/* temp */
+	{ 0x06, 0x08 },	/* low limit */
+	{ 0x05, 0x07 },	/* high limit */
+	{ 0x20, 0x19 },	/* therm (crit) limit */
+	{ 0x30, 0x34 },	/* lowest */
+	{ 0x32, 0x36 },	/* highest */
+};
+
+static const u8 TMP401_TEMP_MSB_WRITE[6][2] = {
+	{ 0, 0 },	/* temp (unused) */
+	{ 0x0C, 0x0E },	/* low limit */
+	{ 0x0B, 0x0D },	/* high limit */
+	{ 0x20, 0x19 },	/* therm (crit) limit */
+	{ 0x30, 0x34 },	/* lowest */
+	{ 0x32, 0x36 },	/* highest */
+};
+
+static const u8 TMP401_TEMP_LSB[6][2] = {
+	{ 0x15, 0x10 },	/* temp */
+	{ 0x17, 0x14 },	/* low limit */
+	{ 0x16, 0x13 },	/* high limit */
+	{ 0, 0 },	/* therm (crit) limit (unused) */
+	{ 0x31, 0x35 },	/* lowest */
+	{ 0x33, 0x37 },	/* highest */
+};
+
+static const u8 TMP432_TEMP_MSB_READ[4][3] = {
+	{ 0x00, 0x01, 0x23 },	/* temp */
+	{ 0x06, 0x08, 0x16 },	/* low limit */
+	{ 0x05, 0x07, 0x15 },	/* high limit */
+	{ 0x20, 0x19, 0x1A },	/* therm (crit) limit */
+};
+
+static const u8 TMP432_TEMP_MSB_WRITE[4][3] = {
+	{ 0, 0, 0 },		/* temp  - unused */
+	{ 0x0C, 0x0E, 0x16 },	/* low limit */
+	{ 0x0B, 0x0D, 0x15 },	/* high limit */
+	{ 0x20, 0x19, 0x1A },	/* therm (crit) limit */
+};
+
+static const u8 TMP432_TEMP_LSB[3][3] = {
+	{ 0x29, 0x10, 0x24 },	/* temp */
+	{ 0x3E, 0x14, 0x18 },	/* low limit */
+	{ 0x3D, 0x13, 0x17 },	/* high limit */
+};
+
+/* [0] = fault, [1] = low, [2] = high, [3] = therm/crit */
+static const u8 TMP432_STATUS_REG[] = {
+	0x1b, 0x36, 0x35, 0x37 };
 
 /* Flags */
-#define TMP401_CONFIG_RANGE		0x04
-#define TMP401_CONFIG_SHUTDOWN		0x40
-#define TMP401_STATUS_LOCAL_CRIT		0x01
-#define TMP401_STATUS_REMOTE_CRIT		0x02
-#define TMP401_STATUS_REMOTE_OPEN		0x04
-#define TMP401_STATUS_REMOTE_LOW		0x08
-#define TMP401_STATUS_REMOTE_HIGH		0x10
-#define TMP401_STATUS_LOCAL_LOW		0x20
-#define TMP401_STATUS_LOCAL_HIGH		0x40
+#define TMP401_CONFIG_RANGE			BIT(2)
+#define TMP401_CONFIG_SHUTDOWN			BIT(6)
+#define TMP401_STATUS_LOCAL_CRIT		BIT(0)
+#define TMP401_STATUS_REMOTE_CRIT		BIT(1)
+#define TMP401_STATUS_REMOTE_OPEN		BIT(2)
+#define TMP401_STATUS_REMOTE_LOW		BIT(3)
+#define TMP401_STATUS_REMOTE_HIGH		BIT(4)
+#define TMP401_STATUS_LOCAL_LOW			BIT(5)
+#define TMP401_STATUS_LOCAL_HIGH		BIT(6)
+
+/* On TMP432, each status has its own register */
+#define TMP432_STATUS_LOCAL			BIT(0)
+#define TMP432_STATUS_REMOTE1			BIT(1)
+#define TMP432_STATUS_REMOTE2			BIT(2)
 
 /* Manufacturer / Device ID's */
 #define TMP401_MANUFACTURER_ID			0x55
 #define TMP401_DEVICE_ID			0x11
-#define TMP411_DEVICE_ID			0x12
+#define TMP411A_DEVICE_ID			0x12
+#define TMP411B_DEVICE_ID			0x13
+#define TMP411C_DEVICE_ID			0x10
+#define TMP431_DEVICE_ID			0x31
+#define TMP432_DEVICE_ID			0x32
 
 /*
  * Driver data (common to all clients)
@@ -98,6 +144,8 @@ static const u8 TMP411_TEMP_HIGHEST_LSB[2]		= { 0x33, 0x37 };
 static const struct i2c_device_id tmp401_id[] = {
 	{ "tmp401", tmp401 },
 	{ "tmp411", tmp411 },
+	{ "tmp431", tmp431 },
+	{ "tmp432", tmp432 },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, tmp401_id);
@@ -113,16 +161,13 @@ struct tmp401_data {
 	unsigned long last_updated; /* in jiffies */
 	enum chips kind;
 
+	unsigned int update_interval;	/* in milliseconds */
+
 	/* register values */
-	u8 status;
+	u8 status[4];
 	u8 config;
-	u16 temp[2];
-	u16 temp_low[2];
-	u16 temp_high[2];
-	u8 temp_crit[2];
+	u16 temp[6][3];
 	u8 temp_crit_hyst;
-	u16 temp_lowest[2];
-	u16 temp_highest[2];
 };
 
 /*
@@ -136,31 +181,10 @@ static int tmp401_register_to_temp(u16 reg, u8 config)
 	if (config & TMP401_CONFIG_RANGE)
 		temp -= 64 * 256;
 
-	return (temp * 625 + 80) / 160;
-}
-
-static u16 tmp401_temp_to_register(long temp, u8 config)
-{
-	if (config & TMP401_CONFIG_RANGE) {
-		temp = clamp_val(temp, -64000, 191000);
-		temp += 64000;
-	} else
-		temp = clamp_val(temp, 0, 127000);
-
-	return (temp * 160 + 312) / 625;
-}
-
-static int tmp401_crit_register_to_temp(u8 reg, u8 config)
-{
-	int temp = reg;
-
-	if (config & TMP401_CONFIG_RANGE)
-		temp -= 64;
-
-	return temp * 1000;
+	return DIV_ROUND_CLOSEST(temp * 125, 32);
 }
 
-static u8 tmp401_crit_temp_to_register(long temp, u8 config)
+static u16 tmp401_temp_to_register(long temp, u8 config, int zbits)
 {
 	if (config & TMP401_CONFIG_RANGE) {
 		temp = clamp_val(temp, -64000, 191000);
@@ -168,113 +192,127 @@ static u8 tmp401_crit_temp_to_register(long temp, u8 config)
 	} else
 		temp = clamp_val(temp, 0, 127000);
 
-	return (temp + 500) / 1000;
+	return DIV_ROUND_CLOSEST(temp * (1 << (8 - zbits)), 1000) << zbits;
 }
 
-static struct tmp401_data *tmp401_update_device_reg16(
-	struct i2c_client *client, struct tmp401_data *data)
+static int tmp401_update_device_reg16(struct i2c_client *client,
+				      struct tmp401_data *data)
 {
-	int i;
-
-	for (i = 0; i < 2; i++) {
-		/*
-		 * High byte must be read first immediately followed
-		 * by the low byte
-		 */
-		data->temp[i] = i2c_smbus_read_byte_data(client,
-			TMP401_TEMP_MSB[i]) << 8;
-		data->temp[i] |= i2c_smbus_read_byte_data(client,
-			TMP401_TEMP_LSB[i]);
-		data->temp_low[i] = i2c_smbus_read_byte_data(client,
-			TMP401_TEMP_LOW_LIMIT_MSB_READ[i]) << 8;
-		data->temp_low[i] |= i2c_smbus_read_byte_data(client,
-			TMP401_TEMP_LOW_LIMIT_LSB[i]);
-		data->temp_high[i] = i2c_smbus_read_byte_data(client,
-			TMP401_TEMP_HIGH_LIMIT_MSB_READ[i]) << 8;
-		data->temp_high[i] |= i2c_smbus_read_byte_data(client,
-			TMP401_TEMP_HIGH_LIMIT_LSB[i]);
-		data->temp_crit[i] = i2c_smbus_read_byte_data(client,
-			TMP401_TEMP_CRIT_LIMIT[i]);
-
-		if (data->kind == tmp411) {
-			data->temp_lowest[i] = i2c_smbus_read_byte_data(client,
-				TMP411_TEMP_LOWEST_MSB[i]) << 8;
-			data->temp_lowest[i] |= i2c_smbus_read_byte_data(
-				client, TMP411_TEMP_LOWEST_LSB[i]);
-
-			data->temp_highest[i] = i2c_smbus_read_byte_data(
-				client, TMP411_TEMP_HIGHEST_MSB[i]) << 8;
-			data->temp_highest[i] |= i2c_smbus_read_byte_data(
-				client, TMP411_TEMP_HIGHEST_LSB[i]);
+	int i, j, val;
+	int num_regs = data->kind == tmp411 ? 6 : 4;
+	int num_sensors = data->kind == tmp432 ? 3 : 2;
+
+	for (i = 0; i < num_sensors; i++) {		/* local / r1 / r2 */
+		for (j = 0; j < num_regs; j++) {	/* temp / low / ... */
+			u8 regaddr;
+			/*
+			 * High byte must be read first immediately followed
+			 * by the low byte
+			 */
+			regaddr = data->kind == tmp432 ?
+						TMP432_TEMP_MSB_READ[j][i] :
+						TMP401_TEMP_MSB_READ[j][i];
+			val = i2c_smbus_read_byte_data(client, regaddr);
+			if (val < 0)
+				return val;
+			data->temp[j][i] = val << 8;
+			if (j == 3)		/* crit is msb only */
+				continue;
+			regaddr = data->kind == tmp432 ? TMP432_TEMP_LSB[j][i]
+						       : TMP401_TEMP_LSB[j][i];
+			val = i2c_smbus_read_byte_data(client, regaddr);
+			if (val < 0)
+				return val;
+			data->temp[j][i] |= val;
 		}
 	}
-	return data;
+	return 0;
 }
 
 static struct tmp401_data *tmp401_update_device(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct tmp401_data *data = i2c_get_clientdata(client);
+	struct tmp401_data *ret = data;
+	int i, val;
+	unsigned long next_update;
 
 	mutex_lock(&data->update_lock);
 
-	if (time_after(jiffies, data->last_updated + HZ) || !data->valid) {
-		data->status = i2c_smbus_read_byte_data(client, TMP401_STATUS);
-		data->config = i2c_smbus_read_byte_data(client,
-						TMP401_CONFIG_READ);
-		tmp401_update_device_reg16(client, data);
+	next_update = data->last_updated +
+		      msecs_to_jiffies(data->update_interval) + 1;
+	if (time_after(jiffies, next_update) || !data->valid) {
+		if (data->kind != tmp432) {
+			/*
+			 * The driver uses the TMP432 status format internally.
+			 * Convert status to TMP432 format for other chips.
+			 */
+			val = i2c_smbus_read_byte_data(client, TMP401_STATUS);
+			if (val < 0) {
+				ret = ERR_PTR(val);
+				goto abort;
+			}
+			data->status[0] =
+			  (val & TMP401_STATUS_REMOTE_OPEN) >> 1;
+			data->status[1] =
+			  ((val & TMP401_STATUS_REMOTE_LOW) >> 2) |
+			  ((val & TMP401_STATUS_LOCAL_LOW) >> 5);
+			data->status[2] =
+			  ((val & TMP401_STATUS_REMOTE_HIGH) >> 3) |
+			  ((val & TMP401_STATUS_LOCAL_HIGH) >> 6);
+			data->status[3] = val & (TMP401_STATUS_LOCAL_CRIT
+						| TMP401_STATUS_REMOTE_CRIT);
+		} else {
+			for (i = 0; i < ARRAY_SIZE(data->status); i++) {
+				val = i2c_smbus_read_byte_data(client,
+							TMP432_STATUS_REG[i]);
+				if (val < 0) {
+					ret = ERR_PTR(val);
+					goto abort;
+				}
+				data->status[i] = val;
+			}
+		}
 
-		data->temp_crit_hyst = i2c_smbus_read_byte_data(client,
-						TMP401_TEMP_CRIT_HYST);
+		val = i2c_smbus_read_byte_data(client, TMP401_CONFIG_READ);
+		if (val < 0) {
+			ret = ERR_PTR(val);
+			goto abort;
+		}
+		data->config = val;
+		val = tmp401_update_device_reg16(client, data);
+		if (val < 0) {
+			ret = ERR_PTR(val);
+			goto abort;
+		}
+		val = i2c_smbus_read_byte_data(client, TMP401_TEMP_CRIT_HYST);
+		if (val < 0) {
+			ret = ERR_PTR(val);
+			goto abort;
+		}
+		data->temp_crit_hyst = val;
 
 		data->last_updated = jiffies;
 		data->valid = 1;
 	}
 
+abort:
 	mutex_unlock(&data->update_lock);
-
-	return data;
-}
-
-static ssize_t show_temp_value(struct device *dev,
-	struct device_attribute *devattr, char *buf)
-{
-	int index = to_sensor_dev_attr(devattr)->index;
-	struct tmp401_data *data = tmp401_update_device(dev);
-
-	return sprintf(buf, "%d\n",
-		tmp401_register_to_temp(data->temp[index], data->config));
-}
-
-static ssize_t show_temp_min(struct device *dev,
-	struct device_attribute *devattr, char *buf)
-{
-	int index = to_sensor_dev_attr(devattr)->index;
-	struct tmp401_data *data = tmp401_update_device(dev);
-
-	return sprintf(buf, "%d\n",
-		tmp401_register_to_temp(data->temp_low[index], data->config));
+	return ret;
 }
 
-static ssize_t show_temp_max(struct device *dev,
-	struct device_attribute *devattr, char *buf)
+static ssize_t show_temp(struct device *dev,
+			 struct device_attribute *devattr, char *buf)
 {
-	int index = to_sensor_dev_attr(devattr)->index;
+	int nr = to_sensor_dev_attr_2(devattr)->nr;
+	int index = to_sensor_dev_attr_2(devattr)->index;
 	struct tmp401_data *data = tmp401_update_device(dev);
 
-	return sprintf(buf, "%d\n",
-		tmp401_register_to_temp(data->temp_high[index], data->config));
-}
-
-static ssize_t show_temp_crit(struct device *dev,
-	struct device_attribute *devattr, char *buf)
-{
-	int index = to_sensor_dev_attr(devattr)->index;
-	struct tmp401_data *data = tmp401_update_device(dev);
+	if (IS_ERR(data))
+		return PTR_ERR(data);
 
 	return sprintf(buf, "%d\n",
-			tmp401_crit_register_to_temp(data->temp_crit[index],
-							data->config));
+		tmp401_register_to_temp(data->temp[nr][index], data->config));
 }
 
 static ssize_t show_temp_crit_hyst(struct device *dev,
@@ -283,122 +321,60 @@ static ssize_t show_temp_crit_hyst(struct device *dev,
 	int temp, index = to_sensor_dev_attr(devattr)->index;
 	struct tmp401_data *data = tmp401_update_device(dev);
 
+	if (IS_ERR(data))
+		return PTR_ERR(data);
+
 	mutex_lock(&data->update_lock);
-	temp = tmp401_crit_register_to_temp(data->temp_crit[index],
-						data->config);
+	temp = tmp401_register_to_temp(data->temp[3][index], data->config);
 	temp -= data->temp_crit_hyst * 1000;
 	mutex_unlock(&data->update_lock);
 
 	return sprintf(buf, "%d\n", temp);
 }
 
-static ssize_t show_temp_lowest(struct device *dev,
-	struct device_attribute *devattr, char *buf)
-{
-	int index = to_sensor_dev_attr(devattr)->index;
-	struct tmp401_data *data = tmp401_update_device(dev);
-
-	return sprintf(buf, "%d\n",
-		tmp401_register_to_temp(data->temp_lowest[index],
-					data->config));
-}
-
-static ssize_t show_temp_highest(struct device *dev,
-	struct device_attribute *devattr, char *buf)
-{
-	int index = to_sensor_dev_attr(devattr)->index;
-	struct tmp401_data *data = tmp401_update_device(dev);
-
-	return sprintf(buf, "%d\n",
-		tmp401_register_to_temp(data->temp_highest[index],
-					data->config));
-}
-
 static ssize_t show_status(struct device *dev,
 	struct device_attribute *devattr, char *buf)
 {
-	int mask = to_sensor_dev_attr(devattr)->index;
-	struct tmp401_data *data = tmp401_update_device(dev);
-
-	if (data->status & mask)
-		return sprintf(buf, "1\n");
-	else
-		return sprintf(buf, "0\n");
-}
-
-static ssize_t store_temp_min(struct device *dev, struct device_attribute
-	*devattr, const char *buf, size_t count)
-{
-	int index = to_sensor_dev_attr(devattr)->index;
+	int nr = to_sensor_dev_attr_2(devattr)->nr;
+	int mask = to_sensor_dev_attr_2(devattr)->index;
 	struct tmp401_data *data = tmp401_update_device(dev);
-	long val;
-	u16 reg;
-
-	if (kstrtol(buf, 10, &val))
-		return -EINVAL;
-
-	reg = tmp401_temp_to_register(val, data->config);
-
-	mutex_lock(&data->update_lock);
-
-	i2c_smbus_write_byte_data(to_i2c_client(dev),
-		TMP401_TEMP_LOW_LIMIT_MSB_WRITE[index], reg >> 8);
-	i2c_smbus_write_byte_data(to_i2c_client(dev),
-		TMP401_TEMP_LOW_LIMIT_LSB[index], reg & 0xFF);
-
-	data->temp_low[index] = reg;
 
-	mutex_unlock(&data->update_lock);
+	if (IS_ERR(data))
+		return PTR_ERR(data);
 
-	return count;
+	return sprintf(buf, "%d\n", !!(data->status[nr] & mask));
 }
 
-static ssize_t store_temp_max(struct device *dev, struct device_attribute
-	*devattr, const char *buf, size_t count)
+static ssize_t store_temp(struct device *dev, struct device_attribute *devattr,
+			  const char *buf, size_t count)
 {
-	int index = to_sensor_dev_attr(devattr)->index;
+	int nr = to_sensor_dev_attr_2(devattr)->nr;
+	int index = to_sensor_dev_attr_2(devattr)->index;
+	struct i2c_client *client = to_i2c_client(dev);
 	struct tmp401_data *data = tmp401_update_device(dev);
 	long val;
 	u16 reg;
+	u8 regaddr;
 
-	if (kstrtol(buf, 10, &val))
-		return -EINVAL;
-
-	reg = tmp401_temp_to_register(val, data->config);
-
-	mutex_lock(&data->update_lock);
-
-	i2c_smbus_write_byte_data(to_i2c_client(dev),
-		TMP401_TEMP_HIGH_LIMIT_MSB_WRITE[index], reg >> 8);
-	i2c_smbus_write_byte_data(to_i2c_client(dev),
-		TMP401_TEMP_HIGH_LIMIT_LSB[index], reg & 0xFF);
-
-	data->temp_high[index] = reg;
-
-	mutex_unlock(&data->update_lock);
-
-	return count;
-}
-
-static ssize_t store_temp_crit(struct device *dev, struct device_attribute
-	*devattr, const char *buf, size_t count)
-{
-	int index = to_sensor_dev_attr(devattr)->index;
-	struct tmp401_data *data = tmp401_update_device(dev);
-	long val;
-	u8 reg;
+	if (IS_ERR(data))
+		return PTR_ERR(data);
 
 	if (kstrtol(buf, 10, &val))
 		return -EINVAL;
 
-	reg = tmp401_crit_temp_to_register(val, data->config);
+	reg = tmp401_temp_to_register(val, data->config, nr == 3 ? 8 : 4);
 
 	mutex_lock(&data->update_lock);
 
-	i2c_smbus_write_byte_data(to_i2c_client(dev),
-		TMP401_TEMP_CRIT_LIMIT[index], reg);
-
-	data->temp_crit[index] = reg;
+	regaddr = data->kind == tmp432 ? TMP432_TEMP_MSB_WRITE[nr][index]
+				       : TMP401_TEMP_MSB_WRITE[nr][index];
+	i2c_smbus_write_byte_data(client, regaddr, reg >> 8);
+	if (nr != 3) {
+		regaddr = data->kind == tmp432 ? TMP432_TEMP_LSB[nr][index]
+					       : TMP401_TEMP_LSB[nr][index];
+		i2c_smbus_write_byte_data(client, regaddr, reg & 0xFF);
+	}
+	data->temp[nr][index] = reg;
 
 	mutex_unlock(&data->update_lock);
 
@@ -413,6 +389,9 @@ static ssize_t store_temp_crit_hyst(struct device *dev, struct device_attribute
 	long val;
 	u8 reg;
 
+	if (IS_ERR(data))
+		return PTR_ERR(data);
+
 	if (kstrtol(buf, 10, &val))
 		return -EINVAL;
 
@@ -422,13 +401,12 @@ static ssize_t store_temp_crit_hyst(struct device *dev, struct device_attribute
 		val = clamp_val(val, 0, 127000);
 
 	mutex_lock(&data->update_lock);
-	temp = tmp401_crit_register_to_temp(data->temp_crit[index],
-						data->config);
+	temp = tmp401_register_to_temp(data->temp[3][index], data->config);
 	val = clamp_val(val, temp - 255000, temp);
 	reg = ((temp - val) + 500) / 1000;
 
-	i2c_smbus_write_byte_data(to_i2c_client(dev),
-		TMP401_TEMP_CRIT_HYST, reg);
+	i2c_smbus_write_byte_data(to_i2c_client(dev), TMP401_TEMP_CRIT_HYST,
+				  reg);
 
 	data->temp_crit_hyst = reg;
 
@@ -445,54 +423,130 @@ static ssize_t store_temp_crit_hyst(struct device *dev, struct device_attribute
 static ssize_t reset_temp_history(struct device *dev,
 	struct device_attribute	*devattr, const char *buf, size_t count)
 {
+	struct i2c_client *client = to_i2c_client(dev);
+	struct tmp401_data *data = i2c_get_clientdata(client);
 	long val;
 
 	if (kstrtol(buf, 10, &val))
 		return -EINVAL;
 
 	if (val != 1) {
-		dev_err(dev, "temp_reset_history value %ld not"
-			" supported. Use 1 to reset the history!\n", val);
+		dev_err(dev,
+			"temp_reset_history value %ld not supported. Use 1 to reset the history!\n",
+			val);
 		return -EINVAL;
 	}
-	i2c_smbus_write_byte_data(to_i2c_client(dev),
-		TMP411_TEMP_LOWEST_MSB[0], val);
+	mutex_lock(&data->update_lock);
+	i2c_smbus_write_byte_data(client, TMP401_TEMP_MSB_WRITE[5][0], val);
+	data->valid = 0;
+	mutex_unlock(&data->update_lock);
+
+	return count;
+}
+
+static ssize_t show_update_interval(struct device *dev,
+				    struct device_attribute *attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct tmp401_data *data = i2c_get_clientdata(client);
+
+	return sprintf(buf, "%u\n", data->update_interval);
+}
+
+static ssize_t set_update_interval(struct device *dev,
+				   struct device_attribute *attr,
+				   const char *buf, size_t count)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct tmp401_data *data = i2c_get_clientdata(client);
+	unsigned long val;
+	int err, rate;
+
+	err = kstrtoul(buf, 10, &val);
+	if (err)
+		return err;
+
+	/*
+	 * For valid rates, interval can be calculated as
+	 *	interval = (1 << (7 - rate)) * 125;
+	 * Rounded rate is therefore
+	 *	rate = 7 - __fls(interval * 4 / (125 * 3));
+	 * Use clamp_val() to avoid overflows, and to ensure valid input
+	 * for __fls.
+	 */
+	val = clamp_val(val, 125, 16000);
+	rate = 7 - __fls(val * 4 / (125 * 3));
+	mutex_lock(&data->update_lock);
+	i2c_smbus_write_byte_data(client, TMP401_CONVERSION_RATE_WRITE, rate);
+	data->update_interval = (1 << (7 - rate)) * 125;
+	mutex_unlock(&data->update_lock);
 
 	return count;
 }
 
-static struct sensor_device_attribute tmp401_attr[] = {
-	SENSOR_ATTR(temp1_input, S_IRUGO, show_temp_value, NULL, 0),
-	SENSOR_ATTR(temp1_min, S_IWUSR | S_IRUGO, show_temp_min,
-		    store_temp_min, 0),
-	SENSOR_ATTR(temp1_max, S_IWUSR | S_IRUGO, show_temp_max,
-		    store_temp_max, 0),
-	SENSOR_ATTR(temp1_crit, S_IWUSR | S_IRUGO, show_temp_crit,
-		    store_temp_crit, 0),
-	SENSOR_ATTR(temp1_crit_hyst, S_IWUSR | S_IRUGO, show_temp_crit_hyst,
-		    store_temp_crit_hyst, 0),
-	SENSOR_ATTR(temp1_min_alarm, S_IRUGO, show_status, NULL,
-		    TMP401_STATUS_LOCAL_LOW),
-	SENSOR_ATTR(temp1_max_alarm, S_IRUGO, show_status, NULL,
-		    TMP401_STATUS_LOCAL_HIGH),
-	SENSOR_ATTR(temp1_crit_alarm, S_IRUGO, show_status, NULL,
-		    TMP401_STATUS_LOCAL_CRIT),
-	SENSOR_ATTR(temp2_input, S_IRUGO, show_temp_value, NULL, 1),
-	SENSOR_ATTR(temp2_min, S_IWUSR | S_IRUGO, show_temp_min,
-		    store_temp_min, 1),
-	SENSOR_ATTR(temp2_max, S_IWUSR | S_IRUGO, show_temp_max,
-		    store_temp_max, 1),
-	SENSOR_ATTR(temp2_crit, S_IWUSR | S_IRUGO, show_temp_crit,
-		    store_temp_crit, 1),
-	SENSOR_ATTR(temp2_crit_hyst, S_IRUGO, show_temp_crit_hyst, NULL, 1),
-	SENSOR_ATTR(temp2_fault, S_IRUGO, show_status, NULL,
-		    TMP401_STATUS_REMOTE_OPEN),
-	SENSOR_ATTR(temp2_min_alarm, S_IRUGO, show_status, NULL,
-		    TMP401_STATUS_REMOTE_LOW),
-	SENSOR_ATTR(temp2_max_alarm, S_IRUGO, show_status, NULL,
-		    TMP401_STATUS_REMOTE_HIGH),
-	SENSOR_ATTR(temp2_crit_alarm, S_IRUGO, show_status, NULL,
-		    TMP401_STATUS_REMOTE_CRIT),
+static SENSOR_DEVICE_ATTR_2(temp1_input, S_IRUGO, show_temp, NULL, 0, 0);
+static SENSOR_DEVICE_ATTR_2(temp1_min, S_IWUSR | S_IRUGO, show_temp,
+			    store_temp, 1, 0);
+static SENSOR_DEVICE_ATTR_2(temp1_max, S_IWUSR | S_IRUGO, show_temp,
+			    store_temp, 2, 0);
+static SENSOR_DEVICE_ATTR_2(temp1_crit, S_IWUSR | S_IRUGO, show_temp,
+			    store_temp, 3, 0);
+static SENSOR_DEVICE_ATTR(temp1_crit_hyst, S_IWUSR | S_IRUGO,
+			  show_temp_crit_hyst, store_temp_crit_hyst, 0);
+static SENSOR_DEVICE_ATTR_2(temp1_min_alarm, S_IRUGO, show_status, NULL,
+			    1, TMP432_STATUS_LOCAL);
+static SENSOR_DEVICE_ATTR_2(temp1_max_alarm, S_IRUGO, show_status, NULL,
+			    2, TMP432_STATUS_LOCAL);
+static SENSOR_DEVICE_ATTR_2(temp1_crit_alarm, S_IRUGO, show_status, NULL,
+			    3, TMP432_STATUS_LOCAL);
+static SENSOR_DEVICE_ATTR_2(temp2_input, S_IRUGO, show_temp, NULL, 0, 1);
+static SENSOR_DEVICE_ATTR_2(temp2_min, S_IWUSR | S_IRUGO, show_temp,
+			    store_temp, 1, 1);
+static SENSOR_DEVICE_ATTR_2(temp2_max, S_IWUSR | S_IRUGO, show_temp,
+			    store_temp, 2, 1);
+static SENSOR_DEVICE_ATTR_2(temp2_crit, S_IWUSR | S_IRUGO, show_temp,
+			    store_temp, 3, 1);
+static SENSOR_DEVICE_ATTR(temp2_crit_hyst, S_IRUGO, show_temp_crit_hyst,
+			  NULL, 1);
+static SENSOR_DEVICE_ATTR_2(temp2_fault, S_IRUGO, show_status, NULL,
+			    0, TMP432_STATUS_REMOTE1);
+static SENSOR_DEVICE_ATTR_2(temp2_min_alarm, S_IRUGO, show_status, NULL,
+			    1, TMP432_STATUS_REMOTE1);
+static SENSOR_DEVICE_ATTR_2(temp2_max_alarm, S_IRUGO, show_status, NULL,
+			    2, TMP432_STATUS_REMOTE1);
+static SENSOR_DEVICE_ATTR_2(temp2_crit_alarm, S_IRUGO, show_status, NULL,
+			    3, TMP432_STATUS_REMOTE1);
+
+static DEVICE_ATTR(update_interval, S_IRUGO | S_IWUSR, show_update_interval,
+		   set_update_interval);
+
+static struct attribute *tmp401_attributes[] = {
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	&sensor_dev_attr_temp1_min.dev_attr.attr,
+	&sensor_dev_attr_temp1_max.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp1_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit_alarm.dev_attr.attr,
+
+	&sensor_dev_attr_temp2_input.dev_attr.attr,
+	&sensor_dev_attr_temp2_min.dev_attr.attr,
+	&sensor_dev_attr_temp2_max.dev_attr.attr,
+	&sensor_dev_attr_temp2_crit.dev_attr.attr,
+	&sensor_dev_attr_temp2_crit_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp2_fault.dev_attr.attr,
+	&sensor_dev_attr_temp2_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp2_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp2_crit_alarm.dev_attr.attr,
+
+	&dev_attr_update_interval.attr,
+
+	NULL
+};
+
+static const struct attribute_group tmp401_group = {
+	.attrs = tmp401_attributes,
 };
 
 /*
@@ -502,12 +556,60 @@ static struct sensor_device_attribute tmp401_attr[] = {
  * minimum and maximum register reset for both the local
  * and remote channels.
  */
-static struct sensor_device_attribute tmp411_attr[] = {
-	SENSOR_ATTR(temp1_highest, S_IRUGO, show_temp_highest, NULL, 0),
-	SENSOR_ATTR(temp1_lowest, S_IRUGO, show_temp_lowest, NULL, 0),
-	SENSOR_ATTR(temp2_highest, S_IRUGO, show_temp_highest, NULL, 1),
-	SENSOR_ATTR(temp2_lowest, S_IRUGO, show_temp_lowest, NULL, 1),
-	SENSOR_ATTR(temp_reset_history, S_IWUSR, NULL, reset_temp_history, 0),
+static SENSOR_DEVICE_ATTR_2(temp1_lowest, S_IRUGO, show_temp, NULL, 4, 0);
+static SENSOR_DEVICE_ATTR_2(temp1_highest, S_IRUGO, show_temp, NULL, 5, 0);
+static SENSOR_DEVICE_ATTR_2(temp2_lowest, S_IRUGO, show_temp, NULL, 4, 1);
+static SENSOR_DEVICE_ATTR_2(temp2_highest, S_IRUGO, show_temp, NULL, 5, 1);
+static SENSOR_DEVICE_ATTR(temp_reset_history, S_IWUSR, NULL, reset_temp_history,
+			  0);
+
+static struct attribute *tmp411_attributes[] = {
+	&sensor_dev_attr_temp1_highest.dev_attr.attr,
+	&sensor_dev_attr_temp1_lowest.dev_attr.attr,
+	&sensor_dev_attr_temp2_highest.dev_attr.attr,
+	&sensor_dev_attr_temp2_lowest.dev_attr.attr,
+	&sensor_dev_attr_temp_reset_history.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group tmp411_group = {
+	.attrs = tmp411_attributes,
+};
+
+static SENSOR_DEVICE_ATTR_2(temp3_input, S_IRUGO, show_temp, NULL, 0, 2);
+static SENSOR_DEVICE_ATTR_2(temp3_min, S_IWUSR | S_IRUGO, show_temp,
+			    store_temp, 1, 2);
+static SENSOR_DEVICE_ATTR_2(temp3_max, S_IWUSR | S_IRUGO, show_temp,
+			    store_temp, 2, 2);
+static SENSOR_DEVICE_ATTR_2(temp3_crit, S_IWUSR | S_IRUGO, show_temp,
+			    store_temp, 3, 2);
+static SENSOR_DEVICE_ATTR(temp3_crit_hyst, S_IRUGO, show_temp_crit_hyst,
+			  NULL, 2);
+static SENSOR_DEVICE_ATTR_2(temp3_fault, S_IRUGO, show_status, NULL,
+			    0, TMP432_STATUS_REMOTE2);
+static SENSOR_DEVICE_ATTR_2(temp3_min_alarm, S_IRUGO, show_status, NULL,
+			    1, TMP432_STATUS_REMOTE2);
+static SENSOR_DEVICE_ATTR_2(temp3_max_alarm, S_IRUGO, show_status, NULL,
+			    2, TMP432_STATUS_REMOTE2);
+static SENSOR_DEVICE_ATTR_2(temp3_crit_alarm, S_IRUGO, show_status, NULL,
+			    3, TMP432_STATUS_REMOTE2);
+
+static struct attribute *tmp432_attributes[] = {
+	&sensor_dev_attr_temp3_input.dev_attr.attr,
+	&sensor_dev_attr_temp3_min.dev_attr.attr,
+	&sensor_dev_attr_temp3_max.dev_attr.attr,
+	&sensor_dev_attr_temp3_crit.dev_attr.attr,
+	&sensor_dev_attr_temp3_crit_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp3_fault.dev_attr.attr,
+	&sensor_dev_attr_temp3_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp3_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp3_crit_alarm.dev_attr.attr,
+
+	NULL
+};
+
+static const struct attribute_group tmp432_group = {
+	.attrs = tmp432_attributes,
 };
 
 /*
@@ -517,9 +619,11 @@ static struct sensor_device_attribute tmp411_attr[] = {
 static void tmp401_init_client(struct i2c_client *client)
 {
 	int config, config_orig;
+	struct tmp401_data *data = i2c_get_clientdata(client);
 
 	/* Set the conversion rate to 2 Hz */
 	i2c_smbus_write_byte_data(client, TMP401_CONVERSION_RATE_WRITE, 5);
+	data->update_interval = 500;
 
 	/* Start conversions (disable shutdown if necessary) */
 	config = i2c_smbus_read_byte_data(client, TMP401_CONFIG_READ);
@@ -554,11 +658,35 @@ static int tmp401_detect(struct i2c_client *client,
 
 	switch (reg) {
 	case TMP401_DEVICE_ID:
+		if (client->addr != 0x4c)
+			return -ENODEV;
 		kind = tmp401;
 		break;
-	case TMP411_DEVICE_ID:
+	case TMP411A_DEVICE_ID:
+		if (client->addr != 0x4c)
+			return -ENODEV;
+		kind = tmp411;
+		break;
+	case TMP411B_DEVICE_ID:
+		if (client->addr != 0x4d)
+			return -ENODEV;
 		kind = tmp411;
 		break;
+	case TMP411C_DEVICE_ID:
+		if (client->addr != 0x4e)
+			return -ENODEV;
+		kind = tmp411;
+		break;
+	case TMP431_DEVICE_ID:
+		if (client->addr == 0x4e)
+			return -ENODEV;
+		kind = tmp431;
+		break;
+	case TMP432_DEVICE_ID:
+		if (client->addr == 0x4e)
+			return -ENODEV;
+		kind = tmp432;
+		break;
 	default:
 		return -ENODEV;
 	}
@@ -579,20 +707,19 @@ static int tmp401_detect(struct i2c_client *client,
 
 static int tmp401_remove(struct i2c_client *client)
 {
+	struct device *dev = &client->dev;
 	struct tmp401_data *data = i2c_get_clientdata(client);
-	int i;
 
 	if (data->hwmon_dev)
 		hwmon_device_unregister(data->hwmon_dev);
 
-	for (i = 0; i < ARRAY_SIZE(tmp401_attr); i++)
-		device_remove_file(&client->dev, &tmp401_attr[i].dev_attr);
+	sysfs_remove_group(&dev->kobj, &tmp401_group);
 
-	if (data->kind == tmp411) {
-		for (i = 0; i < ARRAY_SIZE(tmp411_attr); i++)
-			device_remove_file(&client->dev,
-					   &tmp411_attr[i].dev_attr);
-	}
+	if (data->kind == tmp411)
+		sysfs_remove_group(&dev->kobj, &tmp411_group);
+
+	if (data->kind == tmp432)
+		sysfs_remove_group(&dev->kobj, &tmp432_group);
 
 	return 0;
 }
@@ -600,12 +727,12 @@ static int tmp401_remove(struct i2c_client *client)
 static int tmp401_probe(struct i2c_client *client,
 			const struct i2c_device_id *id)
 {
-	int i, err = 0;
+	struct device *dev = &client->dev;
+	int err;
 	struct tmp401_data *data;
-	const char *names[] = { "TMP401", "TMP411" };
+	const char *names[] = { "TMP401", "TMP411", "TMP431", "TMP432" };
 
-	data = devm_kzalloc(&client->dev, sizeof(struct tmp401_data),
-			    GFP_KERNEL);
+	data = devm_kzalloc(dev, sizeof(struct tmp401_data), GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
 
@@ -617,31 +744,32 @@ static int tmp401_probe(struct i2c_client *client,
 	tmp401_init_client(client);
 
 	/* Register sysfs hooks */
-	for (i = 0; i < ARRAY_SIZE(tmp401_attr); i++) {
-		err = device_create_file(&client->dev,
-					 &tmp401_attr[i].dev_attr);
+	err = sysfs_create_group(&dev->kobj, &tmp401_group);
+	if (err)
+		return err;
+
+	/* Register additional tmp411 sysfs hooks */
+	if (data->kind == tmp411) {
+		err = sysfs_create_group(&dev->kobj, &tmp411_group);
 		if (err)
 			goto exit_remove;
 	}
 
-	/* Register additional tmp411 sysfs hooks */
-	if (data->kind == tmp411) {
-		for (i = 0; i < ARRAY_SIZE(tmp411_attr); i++) {
-			err = device_create_file(&client->dev,
-						 &tmp411_attr[i].dev_attr);
-			if (err)
-				goto exit_remove;
-		}
+	/* Register additional tmp432 sysfs hooks */
+	if (data->kind == tmp432) {
+		err = sysfs_create_group(&dev->kobj, &tmp432_group);
+		if (err)
+			goto exit_remove;
 	}
 
-	data->hwmon_dev = hwmon_device_register(&client->dev);
+	data->hwmon_dev = hwmon_device_register(dev);
 	if (IS_ERR(data->hwmon_dev)) {
 		err = PTR_ERR(data->hwmon_dev);
 		data->hwmon_dev = NULL;
 		goto exit_remove;
 	}
 
-	dev_info(&client->dev, "Detected TI %s chip\n", names[data->kind]);
+	dev_info(dev, "Detected TI %s chip\n", names[data->kind]);
 
 	return 0;
 
diff --git a/drivers/hwmon/tmp421.c b/drivers/hwmon/tmp421.c
index 6a8ded29f1e..964c1d68827 100644
--- a/drivers/hwmon/tmp421.c
+++ b/drivers/hwmon/tmp421.c
@@ -208,8 +208,8 @@ static int tmp421_init_client(struct i2c_client *client)
 	/* Start conversions (disable shutdown if necessary) */
 	config = i2c_smbus_read_byte_data(client, TMP421_CONFIG_REG_1);
 	if (config < 0) {
-		dev_err(&client->dev, "Could not read configuration"
-			 " register (%d)\n", config);
+		dev_err(&client->dev,
+			"Could not read configuration register (%d)\n", config);
 		return -ENODEV;
 	}
 
@@ -322,6 +322,5 @@ static struct i2c_driver tmp421_driver = {
 module_i2c_driver(tmp421_driver);
 
 MODULE_AUTHOR("Andre Prendel <andre.prendel@gmx.de>");
-MODULE_DESCRIPTION("Texas Instruments TMP421/422/423 temperature sensor"
-		   " driver");
+MODULE_DESCRIPTION("Texas Instruments TMP421/422/423 temperature sensor driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/via686a.c b/drivers/hwmon/via686a.c
index 3123b30208c..c9dcce8c3dc 100644
--- a/drivers/hwmon/via686a.c
+++ b/drivers/hwmon/via686a.c
@@ -125,7 +125,7 @@ static const u8 VIA686A_REG_TEMP_HYST[]	= { 0x3a, 0x3e, 0x1e };
  * (These conversions were contributed by Jonathan Teh Soon Yew
  * <j.teh@iname.com>)
  */
-static inline u8 IN_TO_REG(long val, int inNum)
+static inline u8 IN_TO_REG(long val, int in_num)
 {
 	/*
 	 * To avoid floating point, we multiply constants by 10 (100 for +12V).
@@ -134,29 +134,29 @@ static inline u8 IN_TO_REG(long val, int inNum)
 	 * by an additional 10000 (100000 for +12V): 1000 for val and 10 (100)
 	 * for the constants.
 	 */
-	if (inNum <= 1)
+	if (in_num <= 1)
 		return (u8) clamp_val((val * 21024 - 1205000) / 250000, 0, 255);
-	else if (inNum == 2)
+	else if (in_num == 2)
 		return (u8) clamp_val((val * 15737 - 1205000) / 250000, 0, 255);
-	else if (inNum == 3)
+	else if (in_num == 3)
 		return (u8) clamp_val((val * 10108 - 1205000) / 250000, 0, 255);
 	else
 		return (u8) clamp_val((val * 41714 - 12050000) / 2500000, 0,
 				      255);
 }
 
-static inline long IN_FROM_REG(u8 val, int inNum)
+static inline long IN_FROM_REG(u8 val, int in_num)
 {
 	/*
 	 * To avoid floating point, we multiply constants by 10 (100 for +12V).
 	 * We also multiply them by 1000 because we want 0.001V/bit for the
 	 * output value. Rounding is done.
 	 */
-	if (inNum <= 1)
+	if (in_num <= 1)
 		return (long) ((250000 * val + 1330000 + 21024 / 2) / 21024);
-	else if (inNum == 2)
+	else if (in_num == 2)
 		return (long) ((250000 * val + 1330000 + 15737 / 2) / 15737);
-	else if (inNum == 3)
+	else if (in_num == 3)
 		return (long) ((250000 * val + 1330000 + 10108 / 2) / 10108);
 	else
 		return (long) ((2500000 * val + 13300000 + 41714 / 2) / 41714);
@@ -210,10 +210,10 @@ static inline u8 FAN_TO_REG(long rpm, int div)
  * VIA register values 0-255.  I *10 before rounding, so we get tenth-degree
  * precision.  (I could have done all 1024 values for our 10-bit readings,
  * but the function is very linear in the useful range (0-80 deg C), so
- * we'll just use linear interpolation for 10-bit readings.)  So, tempLUT
+ * we'll just use linear interpolation for 10-bit readings.)  So, temp_lut
  * is the temp at via register values 0-255:
  */
-static const s16 tempLUT[] = {
+static const s16 temp_lut[] = {
 	-709, -688, -667, -646, -627, -607, -589, -570, -553, -536, -519,
 	-503, -487, -471, -456, -442, -428, -414, -400, -387, -375,
 	-362, -350, -339, -327, -316, -305, -295, -285, -275, -265,
@@ -261,7 +261,7 @@ static const s16 tempLUT[] = {
  * - 2.525453e-04*val^3 + 1.424593e-02*val^2 + 2.148941e+00*val +7.275808e+01)
  * Note that n=161:
  */
-static const u8 viaLUT[] = {
+static const u8 via_lut[] = {
 	12, 12, 13, 14, 14, 15, 16, 16, 17, 18, 18, 19, 20, 20, 21, 22, 23,
 	23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 35, 36, 37, 39, 40,
 	41, 43, 45, 46, 48, 49, 51, 53, 55, 57, 59, 60, 62, 64, 66,
@@ -284,26 +284,26 @@ static const u8 viaLUT[] = {
  */
 static inline u8 TEMP_TO_REG(long val)
 {
-	return viaLUT[val <= -50000 ? 0 : val >= 110000 ? 160 :
+	return via_lut[val <= -50000 ? 0 : val >= 110000 ? 160 :
 		      (val < 0 ? val - 500 : val + 500) / 1000 + 50];
 }
 
 /* for 8-bit temperature hyst and over registers */
-#define TEMP_FROM_REG(val)	((long)tempLUT[val] * 100)
+#define TEMP_FROM_REG(val)	((long)temp_lut[val] * 100)
 
 /* for 10-bit temperature readings */
 static inline long TEMP_FROM_REG10(u16 val)
 {
-	u16 eightBits = val >> 2;
-	u16 twoBits = val & 3;
+	u16 eight_bits = val >> 2;
+	u16 two_bits = val & 3;
 
 	/* no interpolation for these */
-	if (twoBits == 0 || eightBits == 255)
-		return TEMP_FROM_REG(eightBits);
+	if (two_bits == 0 || eight_bits == 255)
+		return TEMP_FROM_REG(eight_bits);
 
 	/* do some linear interpolation */
-	return (tempLUT[eightBits] * (4 - twoBits) +
-		tempLUT[eightBits + 1] * twoBits) * 25;
+	return (temp_lut[eight_bits] * (4 - two_bits) +
+		temp_lut[eight_bits + 1] * two_bits) * 25;
 }
 
 #define DIV_FROM_REG(val) (1 << (val))
@@ -889,8 +889,8 @@ static int via686a_pci_probe(struct pci_dev *dev,
 
 	address = val & ~(VIA686A_EXTENT - 1);
 	if (address == 0) {
-		dev_err(&dev->dev, "base address not set - upgrade BIOS "
-			"or use force_addr=0xaddr\n");
+		dev_err(&dev->dev,
+			"base address not set - upgrade BIOS or use force_addr=0xaddr\n");
 		return -ENODEV;
 	}
 
@@ -899,8 +899,9 @@ static int via686a_pci_probe(struct pci_dev *dev,
 		return -ENODEV;
 	if (!(val & 0x0001)) {
 		if (!force_addr) {
-			dev_warn(&dev->dev, "Sensors disabled, enable "
-				 "with force_addr=0x%x\n", address);
+			dev_warn(&dev->dev,
+				 "Sensors disabled, enable with force_addr=0x%x\n",
+				 address);
 			return -ENODEV;
 		}
 
diff --git a/drivers/hwmon/vt1211.c b/drivers/hwmon/vt1211.c
index dcc62f80f67..6b2f1a42b3f 100644
--- a/drivers/hwmon/vt1211.c
+++ b/drivers/hwmon/vt1211.c
@@ -571,8 +571,9 @@ static ssize_t set_fan(struct device *dev, struct device_attribute *attr,
 			break;
 		default:
 			count = -EINVAL;
-			dev_warn(dev, "fan div value %ld not supported. "
-				 "Choose one of 1, 2, 4, or 8.\n", val);
+			dev_warn(dev,
+				 "fan div value %ld not supported. Choose one of 1, 2, 4, or 8.\n",
+				 val);
 			goto EXIT;
 		}
 		vt1211_write8(data, VT1211_REG_FAN_DIV,
@@ -674,8 +675,9 @@ static ssize_t set_pwm(struct device *dev, struct device_attribute *attr,
 			break;
 		default:
 			count = -EINVAL;
-			dev_warn(dev, "pwm mode %ld not supported. "
-				 "Choose one of 0 or 2.\n", val);
+			dev_warn(dev,
+				 "pwm mode %ld not supported. Choose one of 0 or 2.\n",
+				 val);
 			goto EXIT;
 		}
 		vt1211_write8(data, VT1211_REG_PWM_CTL,
@@ -700,8 +702,9 @@ static ssize_t set_pwm(struct device *dev, struct device_attribute *attr,
 	case SHOW_SET_PWM_AUTO_CHANNELS_TEMP:
 		if (val < 1 || val > 7) {
 			count = -EINVAL;
-			dev_warn(dev, "temp channel %ld not supported. "
-				 "Choose a value between 1 and 7.\n", val);
+			dev_warn(dev,
+				 "temp channel %ld not supported. Choose a value between 1 and 7.\n",
+				 val);
 			goto EXIT;
 		}
 		if (!ISTEMP(val - 1, data->uch_config)) {
@@ -1325,15 +1328,15 @@ static int __init vt1211_init(void)
 
 	if ((uch_config < -1) || (uch_config > 31)) {
 		err = -EINVAL;
-		pr_warn("Invalid UCH configuration %d. "
-			"Choose a value between 0 and 31.\n", uch_config);
+		pr_warn("Invalid UCH configuration %d. Choose a value between 0 and 31.\n",
+			uch_config);
 		goto EXIT;
 	}
 
 	if ((int_mode < -1) || (int_mode > 0)) {
 		err = -EINVAL;
-		pr_warn("Invalid interrupt mode %d. "
-			"Only mode 0 is supported.\n", int_mode);
+		pr_warn("Invalid interrupt mode %d. Only mode 0 is supported.\n",
+			int_mode);
 		goto EXIT;
 	}
 
diff --git a/drivers/hwmon/vt8231.c b/drivers/hwmon/vt8231.c
index 988a2a79676..0e7017841f7 100644
--- a/drivers/hwmon/vt8231.c
+++ b/drivers/hwmon/vt8231.c
@@ -573,8 +573,9 @@ static ssize_t set_fan_div(struct device *dev, struct device_attribute *attr,
 		data->fan_div[nr] = 3;
 		break;
 	default:
-		dev_err(dev, "fan_div value %ld not supported. "
-			"Choose one of 1, 2, 4 or 8!\n", val);
+		dev_err(dev,
+			"fan_div value %ld not supported. Choose one of 1, 2, 4 or 8!\n",
+			val);
 		mutex_unlock(&data->update_lock);
 		return -EINVAL;
 	}
diff --git a/drivers/hwmon/w83627ehf.c b/drivers/hwmon/w83627ehf.c
index 0a89211c25f..016027229e8 100644
--- a/drivers/hwmon/w83627ehf.c
+++ b/drivers/hwmon/w83627ehf.c
@@ -840,8 +840,8 @@ static struct w83627ehf_data *w83627ehf_update_device(struct device *dev)
 			    && (reg >= 0xff || (sio_data->kind == nct6775
 						&& reg == 0x00))
 			    && data->fan_div[i] < 0x07) {
-				dev_dbg(dev, "Increasing fan%d "
-					"clock divider from %u to %u\n",
+				dev_dbg(dev,
+					"Increasing fan%d clock divider from %u to %u\n",
 					i + 1, div_from_reg(data->fan_div[i]),
 					div_from_reg(data->fan_div[i] + 1));
 				data->fan_div[i]++;
@@ -1110,9 +1110,9 @@ store_fan_min(struct device *dev, struct device_attribute *attr,
 		 */
 		data->fan_min[nr] = 254;
 		new_div = 7; /* 128 == (1 << 7) */
-		dev_warn(dev, "fan%u low limit %lu below minimum %u, set to "
-			 "minimum\n", nr + 1, val,
-			 data->fan_from_reg_min(254, 7));
+		dev_warn(dev,
+			 "fan%u low limit %lu below minimum %u, set to minimum\n",
+			 nr + 1, val, data->fan_from_reg_min(254, 7));
 	} else if (!reg) {
 		/*
 		 * Speed above this value cannot possibly be represented,
@@ -1120,9 +1120,9 @@ store_fan_min(struct device *dev, struct device_attribute *attr,
 		 */
 		data->fan_min[nr] = 1;
 		new_div = 0; /* 1 == (1 << 0) */
-		dev_warn(dev, "fan%u low limit %lu above maximum %u, set to "
-			 "maximum\n", nr + 1, val,
-			 data->fan_from_reg_min(1, 0));
+		dev_warn(dev,
+			 "fan%u low limit %lu above maximum %u, set to maximum\n",
+			 nr + 1, val, data->fan_from_reg_min(1, 0));
 	} else {
 		/*
 		 * Automatically pick the best divider, i.e. the one such
@@ -2396,15 +2396,15 @@ static int w83627ehf_probe(struct platform_device *pdev)
 				en_vrm10 = superio_inb(sio_data->sioreg,
 						       SIO_REG_EN_VRM10);
 				if ((en_vrm10 & 0x08) && data->vrm == 90) {
-					dev_warn(dev, "Setting VID input "
-						 "voltage to TTL\n");
+					dev_warn(dev,
+						 "Setting VID input voltage to TTL\n");
 					superio_outb(sio_data->sioreg,
 						     SIO_REG_EN_VRM10,
 						     en_vrm10 & ~0x08);
 				} else if (!(en_vrm10 & 0x08)
 					   && data->vrm == 100) {
-					dev_warn(dev, "Setting VID input "
-						 "voltage to VRM10\n");
+					dev_warn(dev,
+						 "Setting VID input voltage to VRM10\n");
 					superio_outb(sio_data->sioreg,
 						     SIO_REG_EN_VRM10,
 						     en_vrm10 | 0x08);
@@ -2420,8 +2420,8 @@ static int w83627ehf_probe(struct platform_device *pdev)
 			if (err)
 				goto exit_release;
 		} else {
-			dev_info(dev, "VID pins in output mode, CPU VID not "
-				 "available\n");
+			dev_info(dev,
+				 "VID pins in output mode, CPU VID not available\n");
 		}
 	}
 
@@ -2795,8 +2795,7 @@ static int __init w83627ehf_find(int sioaddr, unsigned short *addr,
 	/* Activate logical device if needed */
 	val = superio_inb(sioaddr, SIO_REG_ENABLE);
 	if (!(val & 0x01)) {
-		pr_warn("Forcibly enabling Super-I/O. "
-			"Sensor is probably unusable.\n");
+		pr_warn("Forcibly enabling Super-I/O. Sensor is probably unusable.\n");
 		superio_outb(sioaddr, SIO_REG_ENABLE, val | 0x01);
 	}
 
diff --git a/drivers/hwmon/w83781d.c b/drivers/hwmon/w83781d.c
index aeec5b1d81c..f9d513949a3 100644
--- a/drivers/hwmon/w83781d.c
+++ b/drivers/hwmon/w83781d.c
@@ -64,8 +64,8 @@ enum chips { w83781d, w83782d, w83783s, as99127f };
 /* Insmod parameters */
 static unsigned short force_subclients[4];
 module_param_array(force_subclients, short, NULL, 0);
-MODULE_PARM_DESC(force_subclients, "List of subclient addresses: "
-		    "{bus, clientaddr, subclientaddr1, subclientaddr2}");
+MODULE_PARM_DESC(force_subclients,
+		 "List of subclient addresses: {bus, clientaddr, subclientaddr1, subclientaddr2}");
 
 static bool reset;
 module_param(reset, bool, 0);
@@ -826,8 +826,9 @@ store_sensor(struct device *dev, struct device_attribute *da,
 		data->sens[nr] = val;
 		break;
 	case W83781D_DEFAULT_BETA:
-		dev_warn(dev, "Sensor type %d is deprecated, please use 4 "
-			 "instead\n", W83781D_DEFAULT_BETA);
+		dev_warn(dev,
+			 "Sensor type %d is deprecated, please use 4 instead\n",
+			 W83781D_DEFAULT_BETA);
 		/* fall through */
 	case 4:		/* thermistor */
 		tmp = w83781d_read_value(data, W83781D_REG_SCFG1);
@@ -874,8 +875,8 @@ w83781d_detect_subclients(struct i2c_client *new_client)
 		for (i = 2; i <= 3; i++) {
 			if (force_subclients[i] < 0x48 ||
 			    force_subclients[i] > 0x4f) {
-				dev_err(&new_client->dev, "Invalid subclient "
-					"address %d; must be 0x48-0x4f\n",
+				dev_err(&new_client->dev,
+					"Invalid subclient address %d; must be 0x48-0x4f\n",
 					force_subclients[i]);
 				err = -EINVAL;
 				goto ERROR_SC_1;
@@ -910,9 +911,9 @@ w83781d_detect_subclients(struct i2c_client *new_client)
 	for (i = 0; i < num_sc; i++) {
 		data->lm75[i] = i2c_new_dummy(adapter, sc_addr[i]);
 		if (!data->lm75[i]) {
-			dev_err(&new_client->dev, "Subclient %d "
-				"registration at address 0x%x "
-				"failed.\n", i, sc_addr[i]);
+			dev_err(&new_client->dev,
+				"Subclient %d registration at address 0x%x failed.\n",
+				i, sc_addr[i]);
 			err = -ENOMEM;
 			if (i == 1)
 				goto ERROR_SC_3;
@@ -1176,8 +1177,9 @@ w83781d_detect(struct i2c_client *client, struct i2c_board_info *info)
 		goto err_nodev;
 
 	if (val1 <= 0x30 && w83781d_alias_detect(client, val1)) {
-		dev_dbg(&adapter->dev, "Device at 0x%02x appears to "
-			"be the same as ISA device\n", address);
+		dev_dbg(&adapter->dev,
+			"Device at 0x%02x appears to be the same as ISA device\n",
+			address);
 		goto err_nodev;
 	}
 
@@ -1367,8 +1369,8 @@ w83781d_init_device(struct device *dev)
 		 * as I see very little reason why this would be needed at
 		 * all.
 		 */
-		dev_info(dev, "If reset=1 solved a problem you were "
-			 "having, please report!\n");
+		dev_info(dev,
+			 "If reset=1 solved a problem you were having, please report!\n");
 
 		/* save these registers */
 		i = w83781d_read_value(data, W83781D_REG_BEEP_CONFIG);
@@ -1425,8 +1427,8 @@ w83781d_init_device(struct device *dev)
 		/* Enable temp2 */
 		tmp = w83781d_read_value(data, W83781D_REG_TEMP2_CONFIG);
 		if (tmp & 0x01) {
-			dev_warn(dev, "Enabling temp2, readings "
-				 "might not make sense\n");
+			dev_warn(dev,
+				 "Enabling temp2, readings might not make sense\n");
 			w83781d_write_value(data, W83781D_REG_TEMP2_CONFIG,
 				tmp & 0xfe);
 		}
@@ -1436,8 +1438,8 @@ w83781d_init_device(struct device *dev)
 			tmp = w83781d_read_value(data,
 				W83781D_REG_TEMP3_CONFIG);
 			if (tmp & 0x01) {
-				dev_warn(dev, "Enabling temp3, "
-					 "readings might not make sense\n");
+				dev_warn(dev,
+					 "Enabling temp3, readings might not make sense\n");
 				w83781d_write_value(data,
 					W83781D_REG_TEMP3_CONFIG, tmp & 0xfe);
 			}
diff --git a/drivers/hwmon/w83791d.c b/drivers/hwmon/w83791d.c
index 38dddddf887..a3feee332e2 100644
--- a/drivers/hwmon/w83791d.c
+++ b/drivers/hwmon/w83791d.c
@@ -56,8 +56,8 @@ static const unsigned short normal_i2c[] = { 0x2c, 0x2d, 0x2e, 0x2f,
 
 static unsigned short force_subclients[4];
 module_param_array(force_subclients, short, NULL, 0);
-MODULE_PARM_DESC(force_subclients, "List of subclient addresses: "
-			"{bus, clientaddr, subclientaddr1, subclientaddr2}");
+MODULE_PARM_DESC(force_subclients,
+		 "List of subclient addresses: {bus, clientaddr, subclientaddr1, subclientaddr2}");
 
 static bool reset;
 module_param(reset, bool, 0);
diff --git a/drivers/hwmon/w83792d.c b/drivers/hwmon/w83792d.c
index 5cb83ddf2cc..0b804895be4 100644
--- a/drivers/hwmon/w83792d.c
+++ b/drivers/hwmon/w83792d.c
@@ -54,8 +54,8 @@ static const unsigned short normal_i2c[] = { 0x2c, 0x2d, 0x2e, 0x2f,
 
 static unsigned short force_subclients[4];
 module_param_array(force_subclients, short, NULL, 0);
-MODULE_PARM_DESC(force_subclients, "List of subclient addresses: "
-			"{bus, clientaddr, subclientaddr1, subclientaddr2}");
+MODULE_PARM_DESC(force_subclients,
+		 "List of subclient addresses: {bus, clientaddr, subclientaddr1, subclientaddr2}");
 
 static bool init;
 module_param(init, bool, 0);
@@ -951,8 +951,8 @@ w83792d_detect_subclients(struct i2c_client *new_client)
 		for (i = 2; i <= 3; i++) {
 			if (force_subclients[i] < 0x48 ||
 			    force_subclients[i] > 0x4f) {
-				dev_err(&new_client->dev, "invalid subclient "
-					"address %d; must be 0x48-0x4f\n",
+				dev_err(&new_client->dev,
+					"invalid subclient address %d; must be 0x48-0x4f\n",
 					force_subclients[i]);
 				err = -ENODEV;
 				goto ERROR_SC_0;
@@ -969,8 +969,9 @@ w83792d_detect_subclients(struct i2c_client *new_client)
 	if (!(val & 0x80)) {
 		if ((data->lm75[0] != NULL) &&
 			((val & 0x7) == ((val >> 4) & 0x7))) {
-			dev_err(&new_client->dev, "duplicate addresses 0x%x, "
-				"use force_subclient\n", data->lm75[0]->addr);
+			dev_err(&new_client->dev,
+				"duplicate addresses 0x%x, use force_subclient\n",
+				data->lm75[0]->addr);
 			err = -ENODEV;
 			goto ERROR_SC_1;
 		}
diff --git a/drivers/hwmon/w83793.c b/drivers/hwmon/w83793.c
index 660427520c5..b0c30a546ff 100644
--- a/drivers/hwmon/w83793.c
+++ b/drivers/hwmon/w83793.c
@@ -59,8 +59,8 @@ static const unsigned short normal_i2c[] = { 0x2c, 0x2d, 0x2e, 0x2f,
 
 static unsigned short force_subclients[4];
 module_param_array(force_subclients, short, NULL, 0);
-MODULE_PARM_DESC(force_subclients, "List of subclient addresses: "
-		       "{bus, clientaddr, subclientaddr1, subclientaddr2}");
+MODULE_PARM_DESC(force_subclients,
+		 "List of subclient addresses: {bus, clientaddr, subclientaddr1, subclientaddr2}");
 
 static bool reset;
 module_param(reset, bool, 0);
@@ -1921,8 +1921,8 @@ static int w83793_probe(struct i2c_client *client,
 	}
 	if (i == ARRAY_SIZE(watchdog_minors)) {
 		data->watchdog_miscdev.minor = 0;
-		dev_warn(&client->dev, "Couldn't register watchdog chardev "
-			"(due to no free minor)\n");
+		dev_warn(&client->dev,
+			 "Couldn't register watchdog chardev (due to no free minor)\n");
 	}
 
 	mutex_unlock(&watchdog_data_mutex);
diff --git a/drivers/hwmon/w83795.c b/drivers/hwmon/w83795.c
index e226096148e..908209d2466 100644
--- a/drivers/hwmon/w83795.c
+++ b/drivers/hwmon/w83795.c
@@ -2120,11 +2120,12 @@ static void w83795_check_dynamic_in_limits(struct i2c_client *client)
 					   &w83795_in[i][3].dev_attr.attr,
 					   S_IRUGO);
 		if (err_max || err_min)
-			dev_warn(&client->dev, "Failed to set in%d limits "
-				 "read-only (%d, %d)\n", i, err_max, err_min);
+			dev_warn(&client->dev,
+				 "Failed to set in%d limits read-only (%d, %d)\n",
+				 i, err_max, err_min);
 		else
-			dev_info(&client->dev, "in%d limits set dynamically "
-				 "from VID\n", i);
+			dev_info(&client->dev,
+				 "in%d limits set dynamically from VID\n", i);
 	}
 }
 
diff --git a/drivers/hwspinlock/hwspinlock_core.c b/drivers/hwspinlock/hwspinlock_core.c
index db713c0dfba..461a0d739d7 100644
--- a/drivers/hwspinlock/hwspinlock_core.c
+++ b/drivers/hwspinlock/hwspinlock_core.c
@@ -416,6 +416,8 @@ static int __hwspin_lock_request(struct hwspinlock *hwlock)
 	ret = pm_runtime_get_sync(dev);
 	if (ret < 0) {
 		dev_err(dev, "%s: can't power on device\n", __func__);
+		pm_runtime_put_noidle(dev);
+		module_put(dev->driver->owner);
 		return ret;
 	}
 
diff --git a/drivers/i2c/busses/i2c-designware-platdrv.c b/drivers/i2c/busses/i2c-designware-platdrv.c
index 0ceb6e1b0f6..e3085c487ac 100644
--- a/drivers/i2c/busses/i2c-designware-platdrv.c
+++ b/drivers/i2c/busses/i2c-designware-platdrv.c
@@ -182,7 +182,6 @@ static int dw_i2c_probe(struct platform_device *pdev)
 	adap->algo = &i2c_dw_algo;
 	adap->dev.parent = &pdev->dev;
 	adap->dev.of_node = pdev->dev.of_node;
-	ACPI_HANDLE_SET(&adap->dev, ACPI_HANDLE(&pdev->dev));
 
 	r = i2c_add_numbered_adapter(adap);
 	if (r) {
diff --git a/drivers/i2c/busses/i2c-puv3.c b/drivers/i2c/busses/i2c-puv3.c
index 261d7db437e..8acef657abf 100644
--- a/drivers/i2c/busses/i2c-puv3.c
+++ b/drivers/i2c/busses/i2c-puv3.c
@@ -199,7 +199,7 @@ static int puv3_i2c_probe(struct platform_device *pdev)
 
 	adapter = kzalloc(sizeof(struct i2c_adapter), GFP_KERNEL);
 	if (adapter == NULL) {
-		dev_err(&pdev->dev, "can't allocate inteface!\n");
+		dev_err(&pdev->dev, "can't allocate interface!\n");
 		rc = -ENOMEM;
 		goto fail_nomem;
 	}
diff --git a/drivers/idle/intel_idle.c b/drivers/idle/intel_idle.c
index 5d667501386..1a38dd7dfe4 100644
--- a/drivers/idle/intel_idle.c
+++ b/drivers/idle/intel_idle.c
@@ -465,6 +465,7 @@ static const struct x86_cpu_id intel_idle_ids[] = {
 	ICPU(0x3c, idle_cpu_hsw),
 	ICPU(0x3f, idle_cpu_hsw),
 	ICPU(0x45, idle_cpu_hsw),
+	ICPU(0x46, idle_cpu_hsw),
 	{}
 };
 MODULE_DEVICE_TABLE(x86cpu, intel_idle_ids);
diff --git a/drivers/iio/accel/hid-sensor-accel-3d.c b/drivers/iio/accel/hid-sensor-accel-3d.c
index dd8ea428493..bbcbd7101f3 100644
--- a/drivers/iio/accel/hid-sensor-accel-3d.c
+++ b/drivers/iio/accel/hid-sensor-accel-3d.c
@@ -60,28 +60,28 @@ static const struct iio_chan_spec accel_3d_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
-		.info_mask = IIO_CHAN_INFO_OFFSET_SHARED_BIT |
-		IIO_CHAN_INFO_SCALE_SHARED_BIT |
-		IIO_CHAN_INFO_SAMP_FREQ_SHARED_BIT |
-		IIO_CHAN_INFO_HYSTERESIS_SHARED_BIT,
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
+		BIT(IIO_CHAN_INFO_SCALE) |
+		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+		BIT(IIO_CHAN_INFO_HYSTERESIS),
 		.scan_index = CHANNEL_SCAN_INDEX_X,
 	}, {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
-		.info_mask = IIO_CHAN_INFO_OFFSET_SHARED_BIT |
-		IIO_CHAN_INFO_SCALE_SHARED_BIT |
-		IIO_CHAN_INFO_SAMP_FREQ_SHARED_BIT |
-		IIO_CHAN_INFO_HYSTERESIS_SHARED_BIT,
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
+		BIT(IIO_CHAN_INFO_SCALE) |
+		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+		BIT(IIO_CHAN_INFO_HYSTERESIS),
 		.scan_index = CHANNEL_SCAN_INDEX_Y,
 	}, {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
-		.info_mask = IIO_CHAN_INFO_OFFSET_SHARED_BIT |
-		IIO_CHAN_INFO_SCALE_SHARED_BIT |
-		IIO_CHAN_INFO_SAMP_FREQ_SHARED_BIT |
-		IIO_CHAN_INFO_HYSTERESIS_SHARED_BIT,
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
+		BIT(IIO_CHAN_INFO_SCALE) |
+		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+		BIT(IIO_CHAN_INFO_HYSTERESIS),
 		.scan_index = CHANNEL_SCAN_INDEX_Z,
 	}
 };
diff --git a/drivers/iio/accel/kxsd9.c b/drivers/iio/accel/kxsd9.c
index c2229a521ab..7229645bf1d 100644
--- a/drivers/iio/accel/kxsd9.c
+++ b/drivers/iio/accel/kxsd9.c
@@ -177,8 +177,8 @@ error_ret:
 		.type = IIO_ACCEL,					\
 		.modified = 1,						\
 		.channel2 = IIO_MOD_##axis,				\
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |		\
-			IIO_CHAN_INFO_SCALE_SHARED_BIT,			\
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),	\
 		.address = KXSD9_REG_##axis,				\
 	}
 
@@ -186,7 +186,7 @@ static const struct iio_chan_spec kxsd9_channels[] = {
 	KXSD9_ACCEL_CHAN(X), KXSD9_ACCEL_CHAN(Y), KXSD9_ACCEL_CHAN(Z),
 	{
 		.type = IIO_VOLTAGE,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.indexed = 1,
 		.address = KXSD9_REG_AUX,
 	}
diff --git a/drivers/iio/accel/st_accel_buffer.c b/drivers/iio/accel/st_accel_buffer.c
index 6bd82c7f769..d9b350756f9 100644
--- a/drivers/iio/accel/st_accel_buffer.c
+++ b/drivers/iio/accel/st_accel_buffer.c
@@ -25,7 +25,7 @@
 
 int st_accel_trig_set_state(struct iio_trigger *trig, bool state)
 {
-	struct iio_dev *indio_dev = trig->private_data;
+	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
 
 	return st_sensors_set_dataready_irq(indio_dev, state);
 }
diff --git a/drivers/iio/adc/Kconfig b/drivers/iio/adc/Kconfig
index e372257a849..ab0767e6727 100644
--- a/drivers/iio/adc/Kconfig
+++ b/drivers/iio/adc/Kconfig
@@ -30,6 +30,18 @@ config AD7298
 	  To compile this driver as a module, choose M here: the
 	  module will be called ad7298.
 
+config AD7923
+	tristate "Analog Devices AD7923 and similar ADCs driver"
+	depends on SPI
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	help
+	  Say yes here to build support for Analog Devices
+	  AD7904, AD7914, AD7923, AD7924 4 Channel ADCs.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ad7923.
+
 config AD7791
 	tristate "Analog Devices AD7791 ADC driver"
 	depends on SPI
@@ -91,6 +103,14 @@ config AT91_ADC
 	help
 	  Say yes here to build support for Atmel AT91 ADC.
 
+config EXYNOS_ADC
+	bool "Exynos ADC driver support"
+	depends on OF
+	help
+	  Core support for the ADC block found in the Samsung EXYNOS series
+	  of SoCs for drivers such as the touchscreen and hwmon to use to share
+	  this resource.
+
 config LP8788_ADC
 	bool "LP8788 ADC driver"
 	depends on MFD_LP8788
diff --git a/drivers/iio/adc/Makefile b/drivers/iio/adc/Makefile
index 2d5f10080d8..0a825bed43f 100644
--- a/drivers/iio/adc/Makefile
+++ b/drivers/iio/adc/Makefile
@@ -5,11 +5,13 @@
 obj-$(CONFIG_AD_SIGMA_DELTA) += ad_sigma_delta.o
 obj-$(CONFIG_AD7266) += ad7266.o
 obj-$(CONFIG_AD7298) += ad7298.o
+obj-$(CONFIG_AD7923) += ad7923.o
 obj-$(CONFIG_AD7476) += ad7476.o
 obj-$(CONFIG_AD7791) += ad7791.o
 obj-$(CONFIG_AD7793) += ad7793.o
 obj-$(CONFIG_AD7887) += ad7887.o
 obj-$(CONFIG_AT91_ADC) += at91_adc.o
+obj-$(CONFIG_EXYNOS_ADC) += exynos_adc.o
 obj-$(CONFIG_LP8788_ADC) += lp8788_adc.o
 obj-$(CONFIG_MAX1363) += max1363.o
 obj-$(CONFIG_TI_ADC081C) += ti-adc081c.o
diff --git a/drivers/iio/adc/ad7266.c b/drivers/iio/adc/ad7266.c
index bbad9b94cd7..c2744a75c3b 100644
--- a/drivers/iio/adc/ad7266.c
+++ b/drivers/iio/adc/ad7266.c
@@ -201,9 +201,9 @@ static int ad7266_read_raw(struct iio_dev *indio_dev,
 	.indexed = 1,					\
 	.channel = (_chan),				\
 	.address = (_chan),				\
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT	\
-		| IIO_CHAN_INFO_SCALE_SHARED_BIT	\
-		| IIO_CHAN_INFO_OFFSET_SHARED_BIT,	\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),	\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) \
+		| BIT(IIO_CHAN_INFO_OFFSET),			\
 	.scan_index = (_chan),				\
 	.scan_type = {					\
 		.sign = (_sign),			\
@@ -249,9 +249,9 @@ static AD7266_DECLARE_SINGLE_ENDED_CHANNELS_FIXED(s, 's');
 	.channel = (_chan) * 2,				\
 	.channel2 = (_chan) * 2 + 1,			\
 	.address = (_chan),				\
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT	\
-		| IIO_CHAN_INFO_SCALE_SHARED_BIT	\
-		| IIO_CHAN_INFO_OFFSET_SHARED_BIT,	\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),	\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE)	\
+		| BIT(IIO_CHAN_INFO_OFFSET),			\
 	.scan_index = (_chan),				\
 	.scan_type = {					\
 		.sign = _sign,			\
diff --git a/drivers/iio/adc/ad7298.c b/drivers/iio/adc/ad7298.c
index b34d754994d..03b77189dbf 100644
--- a/drivers/iio/adc/ad7298.c
+++ b/drivers/iio/adc/ad7298.c
@@ -63,8 +63,8 @@ struct ad7298_state {
 		.type = IIO_VOLTAGE,					\
 		.indexed = 1,						\
 		.channel = index,					\
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |		\
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,				\
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),	\
 		.address = index,					\
 		.scan_index = index,					\
 		.scan_type = {						\
@@ -80,9 +80,9 @@ static const struct iio_chan_spec ad7298_channels[] = {
 		.type = IIO_TEMP,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
-			IIO_CHAN_INFO_OFFSET_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_SCALE) |
+			BIT(IIO_CHAN_INFO_OFFSET),
 		.address = AD7298_CH_TEMP,
 		.scan_index = -1,
 		.scan_type = {
diff --git a/drivers/iio/adc/ad7476.c b/drivers/iio/adc/ad7476.c
index 1491fa6debb..2e98bef4af6 100644
--- a/drivers/iio/adc/ad7476.c
+++ b/drivers/iio/adc/ad7476.c
@@ -140,12 +140,12 @@ static int ad7476_read_raw(struct iio_dev *indio_dev,
 	return -EINVAL;
 }
 
-#define _AD7476_CHAN(bits, _shift, _info_mask)			\
+#define _AD7476_CHAN(bits, _shift, _info_mask_sep)		\
 	{							\
 	.type = IIO_VOLTAGE,					\
 	.indexed = 1,						\
-	.info_mask = _info_mask |				\
-	IIO_CHAN_INFO_SCALE_SHARED_BIT,				\
+	.info_mask_separate = _info_mask_sep,			\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),	\
 	.scan_type = {						\
 		.sign = 'u',					\
 		.realbits = (bits),				\
@@ -156,9 +156,9 @@ static int ad7476_read_raw(struct iio_dev *indio_dev,
 }
 
 #define AD7476_CHAN(bits) _AD7476_CHAN((bits), 13 - (bits), \
-		IIO_CHAN_INFO_RAW_SEPARATE_BIT)
+		BIT(IIO_CHAN_INFO_RAW))
 #define AD7940_CHAN(bits) _AD7476_CHAN((bits), 15 - (bits), \
-		IIO_CHAN_INFO_RAW_SEPARATE_BIT)
+		BIT(IIO_CHAN_INFO_RAW))
 #define AD7091R_CHAN(bits) _AD7476_CHAN((bits), 16 - (bits), 0)
 
 static const struct ad7476_chip_info ad7476_chip_info_tbl[] = {
diff --git a/drivers/iio/adc/ad7887.c b/drivers/iio/adc/ad7887.c
index a33d5cd1a53..dd15a5b0f70 100644
--- a/drivers/iio/adc/ad7887.c
+++ b/drivers/iio/adc/ad7887.c
@@ -207,8 +207,8 @@ static const struct ad7887_chip_info ad7887_chip_info_tbl[] = {
 			.type = IIO_VOLTAGE,
 			.indexed = 1,
 			.channel = 1,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SHARED_BIT,
+			.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+			.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
 			.address = 1,
 			.scan_index = 1,
 			.scan_type = IIO_ST('u', 12, 16, 0),
@@ -217,8 +217,8 @@ static const struct ad7887_chip_info ad7887_chip_info_tbl[] = {
 			.type = IIO_VOLTAGE,
 			.indexed = 1,
 			.channel = 0,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SHARED_BIT,
+			.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+			.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
 			.address = 0,
 			.scan_index = 0,
 			.scan_type = IIO_ST('u', 12, 16, 0),
diff --git a/drivers/iio/adc/ad7923.c b/drivers/iio/adc/ad7923.c
new file mode 100644
index 00000000000..97fa0d3dc4a
--- /dev/null
+++ b/drivers/iio/adc/ad7923.c
@@ -0,0 +1,383 @@
+/*
+ * AD7904/AD7914/AD7923/AD7924 SPI ADC driver
+ *
+ * Copyright 2011 Analog Devices Inc (from AD7923 Driver)
+ * Copyright 2012 CS Systemes d'Information
+ *
+ * Licensed under the GPL-2.
+ */
+
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/sysfs.h>
+#include <linux/spi/spi.h>
+#include <linux/regulator/consumer.h>
+#include <linux/err.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
+
+#define AD7923_WRITE_CR		(1 << 11)	/* write control register */
+#define AD7923_RANGE		(1 << 1)	/* range to REFin */
+#define AD7923_CODING		(1 << 0)	/* coding is straight binary */
+#define AD7923_PM_MODE_AS	(1)		/* auto shutdown */
+#define AD7923_PM_MODE_FS	(2)		/* full shutdown */
+#define AD7923_PM_MODE_OPS	(3)		/* normal operation */
+#define AD7923_CHANNEL_0	(0)		/* analog input 0 */
+#define AD7923_CHANNEL_1	(1)		/* analog input 1 */
+#define AD7923_CHANNEL_2	(2)		/* analog input 2 */
+#define AD7923_CHANNEL_3	(3)		/* analog input 3 */
+#define AD7923_SEQUENCE_OFF	(0)		/* no sequence fonction */
+#define AD7923_SEQUENCE_PROTECT	(2)		/* no interrupt write cycle */
+#define AD7923_SEQUENCE_ON	(3)		/* continuous sequence */
+
+#define AD7923_MAX_CHAN		4
+
+#define AD7923_PM_MODE_WRITE(mode)	(mode << 4)	/* write mode */
+#define AD7923_CHANNEL_WRITE(channel)	(channel << 6)	/* write channel */
+#define AD7923_SEQUENCE_WRITE(sequence)	(((sequence & 1) << 3) \
+					+ ((sequence & 2) << 9))
+						/* write sequence fonction */
+/* left shift for CR : bit 11 transmit in first */
+#define AD7923_SHIFT_REGISTER	4
+
+/* val = value, dec = left shift, bits = number of bits of the mask */
+#define EXTRACT(val, dec, bits)		((val >> dec) & ((1 << bits) - 1))
+
+struct ad7923_state {
+	struct spi_device		*spi;
+	struct spi_transfer		ring_xfer[5];
+	struct spi_transfer		scan_single_xfer[2];
+	struct spi_message		ring_msg;
+	struct spi_message		scan_single_msg;
+
+	struct regulator		*reg;
+
+	unsigned int			settings;
+
+	/*
+	 * DMA (thus cache coherency maintenance) requires the
+	 * transfer buffers to live in their own cache lines.
+	 */
+	__be16				rx_buf[4] ____cacheline_aligned;
+	__be16				tx_buf[4];
+};
+
+struct ad7923_chip_info {
+	const struct iio_chan_spec *channels;
+	unsigned int num_channels;
+};
+
+enum ad7923_id {
+	AD7904,
+	AD7914,
+	AD7924,
+};
+
+#define AD7923_V_CHAN(index, bits)					\
+	{								\
+		.type = IIO_VOLTAGE,					\
+		.indexed = 1,						\
+		.channel = index,					\
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),	\
+		.address = index,					\
+		.scan_index = index,					\
+		.scan_type = {						\
+			.sign = 'u',					\
+			.realbits = (bits),				\
+			.storagebits = 16,				\
+			.endianness = IIO_BE,				\
+		},							\
+	}
+
+#define DECLARE_AD7923_CHANNELS(name, bits) \
+const struct iio_chan_spec name ## _channels[] = { \
+	AD7923_V_CHAN(0, bits), \
+	AD7923_V_CHAN(1, bits), \
+	AD7923_V_CHAN(2, bits), \
+	AD7923_V_CHAN(3, bits), \
+	IIO_CHAN_SOFT_TIMESTAMP(4), \
+}
+
+static DECLARE_AD7923_CHANNELS(ad7904, 8);
+static DECLARE_AD7923_CHANNELS(ad7914, 10);
+static DECLARE_AD7923_CHANNELS(ad7924, 12);
+
+static const struct ad7923_chip_info ad7923_chip_info[] = {
+	[AD7904] = {
+		.channels = ad7904_channels,
+		.num_channels = ARRAY_SIZE(ad7904_channels),
+	},
+	[AD7914] = {
+		.channels = ad7914_channels,
+		.num_channels = ARRAY_SIZE(ad7914_channels),
+	},
+	[AD7924] = {
+		.channels = ad7924_channels,
+		.num_channels = ARRAY_SIZE(ad7924_channels),
+	},
+};
+
+/**
+ * ad7923_update_scan_mode() setup the spi transfer buffer for the new scan mask
+ **/
+static int ad7923_update_scan_mode(struct iio_dev *indio_dev,
+	const unsigned long *active_scan_mask)
+{
+	struct ad7923_state *st = iio_priv(indio_dev);
+	int i, cmd, len;
+
+	len = 0;
+	for_each_set_bit(i, active_scan_mask, AD7923_MAX_CHAN) {
+		cmd = AD7923_WRITE_CR | AD7923_CHANNEL_WRITE(i) |
+			AD7923_SEQUENCE_WRITE(AD7923_SEQUENCE_OFF) |
+			st->settings;
+		cmd <<= AD7923_SHIFT_REGISTER;
+		st->tx_buf[len++] = cpu_to_be16(cmd);
+	}
+	/* build spi ring message */
+	st->ring_xfer[0].tx_buf = &st->tx_buf[0];
+	st->ring_xfer[0].len = len;
+	st->ring_xfer[0].cs_change = 1;
+
+	spi_message_init(&st->ring_msg);
+	spi_message_add_tail(&st->ring_xfer[0], &st->ring_msg);
+
+	for (i = 0; i < len; i++) {
+		st->ring_xfer[i + 1].rx_buf = &st->rx_buf[i];
+		st->ring_xfer[i + 1].len = 2;
+		st->ring_xfer[i + 1].cs_change = 1;
+		spi_message_add_tail(&st->ring_xfer[i + 1], &st->ring_msg);
+	}
+	/* make sure last transfer cs_change is not set */
+	st->ring_xfer[i + 1].cs_change = 0;
+
+	return 0;
+}
+
+/**
+ * ad7923_trigger_handler() bh of trigger launched polling to ring buffer
+ *
+ * Currently there is no option in this driver to disable the saving of
+ * timestamps within the ring.
+ **/
+static irqreturn_t ad7923_trigger_handler(int irq, void *p)
+{
+	struct iio_poll_func *pf = p;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct ad7923_state *st = iio_priv(indio_dev);
+	s64 time_ns = 0;
+	int b_sent;
+
+	b_sent = spi_sync(st->spi, &st->ring_msg);
+	if (b_sent)
+		goto done;
+
+	if (indio_dev->scan_timestamp) {
+		time_ns = iio_get_time_ns();
+		memcpy((u8 *)st->rx_buf + indio_dev->scan_bytes - sizeof(s64),
+			&time_ns, sizeof(time_ns));
+	}
+
+	iio_push_to_buffers(indio_dev, (u8 *)st->rx_buf);
+
+done:
+	iio_trigger_notify_done(indio_dev->trig);
+
+	return IRQ_HANDLED;
+}
+
+static int ad7923_scan_direct(struct ad7923_state *st, unsigned ch)
+{
+	int ret, cmd;
+
+	cmd = AD7923_WRITE_CR | AD7923_CHANNEL_WRITE(ch) |
+		AD7923_SEQUENCE_WRITE(AD7923_SEQUENCE_OFF) |
+		st->settings;
+	cmd <<= AD7923_SHIFT_REGISTER;
+	st->tx_buf[0] = cpu_to_be16(cmd);
+
+	ret = spi_sync(st->spi, &st->scan_single_msg);
+	if (ret)
+		return ret;
+
+	return be16_to_cpu(st->rx_buf[0]);
+}
+
+static int ad7923_get_range(struct ad7923_state *st)
+{
+	int vref;
+
+	vref = regulator_get_voltage(st->reg);
+	if (vref < 0)
+		return vref;
+
+	vref /= 1000;
+
+	if (!(st->settings & AD7923_RANGE))
+		vref *= 2;
+
+	return vref;
+}
+
+static int ad7923_read_raw(struct iio_dev *indio_dev,
+			   struct iio_chan_spec const *chan,
+			   int *val,
+			   int *val2,
+			   long m)
+{
+	int ret;
+	struct ad7923_state *st = iio_priv(indio_dev);
+
+	switch (m) {
+	case IIO_CHAN_INFO_RAW:
+		mutex_lock(&indio_dev->mlock);
+		if (iio_buffer_enabled(indio_dev))
+			ret = -EBUSY;
+		else
+			ret = ad7923_scan_direct(st, chan->address);
+		mutex_unlock(&indio_dev->mlock);
+
+		if (ret < 0)
+			return ret;
+
+		if (chan->address == EXTRACT(ret, 12, 4))
+			*val = EXTRACT(ret, 0, 12);
+		else
+			return -EIO;
+
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		ret = ad7923_get_range(st);
+		if (ret < 0)
+			return ret;
+		*val = ret;
+		*val2 = chan->scan_type.realbits;
+		return IIO_VAL_FRACTIONAL_LOG2;
+	}
+	return -EINVAL;
+}
+
+static const struct iio_info ad7923_info = {
+	.read_raw = &ad7923_read_raw,
+	.update_scan_mode = ad7923_update_scan_mode,
+	.driver_module = THIS_MODULE,
+};
+
+static int ad7923_probe(struct spi_device *spi)
+{
+	struct ad7923_state *st;
+	struct iio_dev *indio_dev = iio_device_alloc(sizeof(*st));
+	const struct ad7923_chip_info *info;
+	int ret;
+
+	if (indio_dev == NULL)
+		return -ENOMEM;
+
+	st = iio_priv(indio_dev);
+
+	spi_set_drvdata(spi, indio_dev);
+
+	st->spi = spi;
+	st->settings = AD7923_CODING | AD7923_RANGE |
+			AD7923_PM_MODE_WRITE(AD7923_PM_MODE_OPS);
+
+	info = &ad7923_chip_info[spi_get_device_id(spi)->driver_data];
+
+	indio_dev->name = spi_get_device_id(spi)->name;
+	indio_dev->dev.parent = &spi->dev;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = info->channels;
+	indio_dev->num_channels = info->num_channels;
+	indio_dev->info = &ad7923_info;
+
+	/* Setup default message */
+
+	st->scan_single_xfer[0].tx_buf = &st->tx_buf[0];
+	st->scan_single_xfer[0].len = 2;
+	st->scan_single_xfer[0].cs_change = 1;
+	st->scan_single_xfer[1].rx_buf = &st->rx_buf[0];
+	st->scan_single_xfer[1].len = 2;
+
+	spi_message_init(&st->scan_single_msg);
+	spi_message_add_tail(&st->scan_single_xfer[0], &st->scan_single_msg);
+	spi_message_add_tail(&st->scan_single_xfer[1], &st->scan_single_msg);
+
+	st->reg = regulator_get(&spi->dev, "refin");
+	if (IS_ERR(st->reg)) {
+		ret = PTR_ERR(st->reg);
+		goto error_free;
+	}
+	ret = regulator_enable(st->reg);
+	if (ret)
+		goto error_put_reg;
+
+	ret = iio_triggered_buffer_setup(indio_dev, NULL,
+			&ad7923_trigger_handler, NULL);
+	if (ret)
+		goto error_disable_reg;
+
+	ret = iio_device_register(indio_dev);
+	if (ret)
+		goto error_cleanup_ring;
+
+	return 0;
+
+error_cleanup_ring:
+	iio_triggered_buffer_cleanup(indio_dev);
+error_disable_reg:
+	regulator_disable(st->reg);
+error_put_reg:
+	regulator_put(st->reg);
+error_free:
+	iio_device_free(indio_dev);
+
+	return ret;
+}
+
+static int ad7923_remove(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+	struct ad7923_state *st = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+	iio_triggered_buffer_cleanup(indio_dev);
+	regulator_disable(st->reg);
+	regulator_put(st->reg);
+	iio_device_free(indio_dev);
+
+	return 0;
+}
+
+static const struct spi_device_id ad7923_id[] = {
+	{"ad7904", AD7904},
+	{"ad7914", AD7914},
+	{"ad7923", AD7924},
+	{"ad7924", AD7924},
+	{}
+};
+MODULE_DEVICE_TABLE(spi, ad7923_id);
+
+static struct spi_driver ad7923_driver = {
+	.driver = {
+		.name	= "ad7923",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= ad7923_probe,
+	.remove		= ad7923_remove,
+	.id_table	= ad7923_id,
+};
+module_spi_driver(ad7923_driver);
+
+MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
+MODULE_AUTHOR("Patrick Vasseur <patrick.vasseur@c-s.fr>");
+MODULE_DESCRIPTION("Analog Devices AD7904/AD7914/AD7923/AD7924 ADC");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/ad_sigma_delta.c b/drivers/iio/adc/ad_sigma_delta.c
index afe6d78c8ff..f0d6335ae08 100644
--- a/drivers/iio/adc/ad_sigma_delta.c
+++ b/drivers/iio/adc/ad_sigma_delta.c
@@ -470,7 +470,7 @@ static int ad_sd_probe_trigger(struct iio_dev *indio_dev)
 		disable_irq_nosync(sigma_delta->spi->irq);
 	}
 	sigma_delta->trig->dev.parent = &sigma_delta->spi->dev;
-	sigma_delta->trig->private_data = sigma_delta;
+	iio_trigger_set_drvdata(sigma_delta->trig, sigma_delta);
 
 	ret = iio_trigger_register(sigma_delta->trig);
 	if (ret)
diff --git a/drivers/iio/adc/at91_adc.c b/drivers/iio/adc/at91_adc.c
index 83c836ba600..e5b88d5d3b5 100644
--- a/drivers/iio/adc/at91_adc.c
+++ b/drivers/iio/adc/at91_adc.c
@@ -52,11 +52,15 @@ struct at91_adc_state {
 	void __iomem		*reg_base;
 	struct at91_adc_reg_desc *registers;
 	u8			startup_time;
+	u8			sample_hold_time;
+	bool			sleep_mode;
 	struct iio_trigger	**trig;
 	struct at91_adc_trigger	*trigger_list;
 	u32			trigger_number;
 	bool			use_external;
 	u32			vref_mv;
+	u32			res;		/* resolution used for convertions */
+	bool			low_res;	/* the resolution corresponds to the lowest one */
 	wait_queue_head_t	wq_data_avail;
 };
 
@@ -138,10 +142,10 @@ static int at91_adc_channel_init(struct iio_dev *idev)
 		chan->channel = bit;
 		chan->scan_index = idx;
 		chan->scan_type.sign = 'u';
-		chan->scan_type.realbits = 10;
+		chan->scan_type.realbits = st->res;
 		chan->scan_type.storagebits = 16;
-		chan->info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT |
-			IIO_CHAN_INFO_RAW_SEPARATE_BIT;
+		chan->info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE);
+		chan->info_mask_separate = BIT(IIO_CHAN_INFO_RAW);
 		idx++;
 	}
 	timestamp = chan_array + idx;
@@ -188,7 +192,7 @@ static u8 at91_adc_get_trigger_value_by_name(struct iio_dev *idev,
 
 static int at91_adc_configure_trigger(struct iio_trigger *trig, bool state)
 {
-	struct iio_dev *idev = trig->private_data;
+	struct iio_dev *idev = iio_trigger_get_drvdata(trig);
 	struct at91_adc_state *st = iio_priv(idev);
 	struct iio_buffer *buffer = idev->buffer;
 	struct at91_adc_reg_desc *reg = st->registers;
@@ -254,7 +258,7 @@ static struct iio_trigger *at91_adc_allocate_trigger(struct iio_dev *idev,
 		return NULL;
 
 	trig->dev.parent = idev->dev.parent;
-	trig->private_data = idev;
+	iio_trigger_set_drvdata(trig, idev);
 	trig->ops = &at91_adc_trigger_ops;
 
 	ret = iio_trigger_register(trig);
@@ -372,6 +376,59 @@ static int at91_adc_read_raw(struct iio_dev *idev,
 	return -EINVAL;
 }
 
+static int at91_adc_of_get_resolution(struct at91_adc_state *st,
+				      struct platform_device *pdev)
+{
+	struct iio_dev *idev = iio_priv_to_dev(st);
+	struct device_node *np = pdev->dev.of_node;
+	int count, i, ret = 0;
+	char *res_name, *s;
+	u32 *resolutions;
+
+	count = of_property_count_strings(np, "atmel,adc-res-names");
+	if (count < 2) {
+		dev_err(&idev->dev, "You must specified at least two resolution names for "
+				    "adc-res-names property in the DT\n");
+		return count;
+	}
+
+	resolutions = kmalloc(count * sizeof(*resolutions), GFP_KERNEL);
+	if (!resolutions)
+		return -ENOMEM;
+
+	if (of_property_read_u32_array(np, "atmel,adc-res", resolutions, count)) {
+		dev_err(&idev->dev, "Missing adc-res property in the DT.\n");
+		ret = -ENODEV;
+		goto ret;
+	}
+
+	if (of_property_read_string(np, "atmel,adc-use-res", (const char **)&res_name))
+		res_name = "highres";
+
+	for (i = 0; i < count; i++) {
+		if (of_property_read_string_index(np, "atmel,adc-res-names", i, (const char **)&s))
+			continue;
+
+		if (strcmp(res_name, s))
+			continue;
+
+		st->res = resolutions[i];
+		if (!strcmp(res_name, "lowres"))
+			st->low_res = true;
+		else
+			st->low_res = false;
+
+		dev_info(&idev->dev, "Resolution used: %u bits\n", st->res);
+		goto ret;
+	}
+
+	dev_err(&idev->dev, "There is no resolution for %s\n", res_name);
+
+ret:
+	kfree(resolutions);
+	return ret;
+}
+
 static int at91_adc_probe_dt(struct at91_adc_state *st,
 			     struct platform_device *pdev)
 {
@@ -400,6 +457,8 @@ static int at91_adc_probe_dt(struct at91_adc_state *st,
 	}
 	st->num_channels = prop;
 
+	st->sleep_mode = of_property_read_bool(node, "atmel,adc-sleep-mode");
+
 	if (of_property_read_u32(node, "atmel,adc-startup-time", &prop)) {
 		dev_err(&idev->dev, "Missing adc-startup-time property in the DT.\n");
 		ret = -EINVAL;
@@ -407,6 +466,9 @@ static int at91_adc_probe_dt(struct at91_adc_state *st,
 	}
 	st->startup_time = prop;
 
+	prop = 0;
+	of_property_read_u32(node, "atmel,adc-sample-hold-time", &prop);
+	st->sample_hold_time = prop;
 
 	if (of_property_read_u32(node, "atmel,adc-vref", &prop)) {
 		dev_err(&idev->dev, "Missing adc-vref property in the DT.\n");
@@ -415,6 +477,10 @@ static int at91_adc_probe_dt(struct at91_adc_state *st,
 	}
 	st->vref_mv = prop;
 
+	ret = at91_adc_of_get_resolution(st, pdev);
+	if (ret)
+		goto error_ret;
+
 	st->registers = devm_kzalloc(&idev->dev,
 				     sizeof(struct at91_adc_reg_desc),
 				     GFP_KERNEL);
@@ -516,11 +582,12 @@ static const struct iio_info at91_adc_info = {
 
 static int at91_adc_probe(struct platform_device *pdev)
 {
-	unsigned int prsc, mstrclk, ticks, adc_clk;
+	unsigned int prsc, mstrclk, ticks, adc_clk, shtim;
 	int ret;
 	struct iio_dev *idev;
 	struct at91_adc_state *st;
 	struct resource *res;
+	u32 reg;
 
 	idev = iio_device_alloc(sizeof(struct at91_adc_state));
 	if (idev == NULL) {
@@ -628,9 +695,22 @@ static int at91_adc_probe(struct platform_device *pdev)
 	 */
 	ticks = round_up((st->startup_time * adc_clk /
 			  1000000) - 1, 8) / 8;
-	at91_adc_writel(st, AT91_ADC_MR,
-			(AT91_ADC_PRESCAL_(prsc) & AT91_ADC_PRESCAL) |
-			(AT91_ADC_STARTUP_(ticks) & AT91_ADC_STARTUP));
+	/*
+	 * a minimal Sample and Hold Time is necessary for the ADC to guarantee
+	 * the best converted final value between two channels selection
+	 * The formula thus is : Sample and Hold Time = (shtim + 1) / ADCClock
+	 */
+	shtim = round_up((st->sample_hold_time * adc_clk /
+			  1000000) - 1, 1);
+
+	reg = AT91_ADC_PRESCAL_(prsc) & AT91_ADC_PRESCAL;
+	reg |= AT91_ADC_STARTUP_(ticks) & AT91_ADC_STARTUP;
+	if (st->low_res)
+		reg |= AT91_ADC_LOWRES;
+	if (st->sleep_mode)
+		reg |= AT91_ADC_SLEEP;
+	reg |= AT91_ADC_SHTIM_(shtim) & AT91_ADC_SHTIM;
+	at91_adc_writel(st, AT91_ADC_MR, reg);
 
 	/* Setup the ADC channels available on the board */
 	ret = at91_adc_channel_init(idev);
diff --git a/drivers/iio/adc/exynos_adc.c b/drivers/iio/adc/exynos_adc.c
new file mode 100644
index 00000000000..9f3a8ef1fb3
--- /dev/null
+++ b/drivers/iio/adc/exynos_adc.c
@@ -0,0 +1,452 @@
+/*
+ *  exynos_adc.c - Support for ADC in EXYNOS SoCs
+ *
+ *  8 ~ 10 channel, 10/12-bit ADC
+ *
+ *  Copyright (C) 2013 Naveen Krishna Chatradhi <ch.naveen@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.
+ *
+ *  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/module.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/regulator/consumer.h>
+#include <linux/of_platform.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/machine.h>
+#include <linux/iio/driver.h>
+
+enum adc_version {
+	ADC_V1,
+	ADC_V2
+};
+
+/* EXYNOS4412/5250 ADC_V1 registers definitions */
+#define ADC_V1_CON(x)		((x) + 0x00)
+#define ADC_V1_DLY(x)		((x) + 0x08)
+#define ADC_V1_DATX(x)		((x) + 0x0C)
+#define ADC_V1_INTCLR(x)	((x) + 0x18)
+#define ADC_V1_MUX(x)		((x) + 0x1c)
+
+/* Future ADC_V2 registers definitions */
+#define ADC_V2_CON1(x)		((x) + 0x00)
+#define ADC_V2_CON2(x)		((x) + 0x04)
+#define ADC_V2_STAT(x)		((x) + 0x08)
+#define ADC_V2_INT_EN(x)	((x) + 0x10)
+#define ADC_V2_INT_ST(x)	((x) + 0x14)
+#define ADC_V2_VER(x)		((x) + 0x20)
+
+/* Bit definitions for ADC_V1 */
+#define ADC_V1_CON_RES		(1u << 16)
+#define ADC_V1_CON_PRSCEN	(1u << 14)
+#define ADC_V1_CON_PRSCLV(x)	(((x) & 0xFF) << 6)
+#define ADC_V1_CON_STANDBY	(1u << 2)
+
+/* Bit definitions for ADC_V2 */
+#define ADC_V2_CON1_SOFT_RESET	(1u << 2)
+
+#define ADC_V2_CON2_OSEL	(1u << 10)
+#define ADC_V2_CON2_ESEL	(1u << 9)
+#define ADC_V2_CON2_HIGHF	(1u << 8)
+#define ADC_V2_CON2_C_TIME(x)	(((x) & 7) << 4)
+#define ADC_V2_CON2_ACH_SEL(x)	(((x) & 0xF) << 0)
+#define ADC_V2_CON2_ACH_MASK	0xF
+
+#define MAX_ADC_V2_CHANNELS	10
+#define MAX_ADC_V1_CHANNELS	8
+
+/* Bit definitions common for ADC_V1 and ADC_V2 */
+#define ADC_CON_EN_START	(1u << 0)
+#define ADC_DATX_MASK		0xFFF
+
+#define EXYNOS_ADC_TIMEOUT	(msecs_to_jiffies(1000))
+
+struct exynos_adc {
+	void __iomem		*regs;
+	void __iomem		*enable_reg;
+	struct clk		*clk;
+	unsigned int		irq;
+	struct regulator	*vdd;
+
+	struct completion	completion;
+
+	u32			value;
+	unsigned int            version;
+};
+
+static const struct of_device_id exynos_adc_match[] = {
+	{ .compatible = "samsung,exynos-adc-v1", .data = (void *)ADC_V1 },
+	{ .compatible = "samsung,exynos-adc-v2", .data = (void *)ADC_V2 },
+	{},
+};
+MODULE_DEVICE_TABLE(of, exynos_adc_match);
+
+static inline unsigned int exynos_adc_get_version(struct platform_device *pdev)
+{
+	const struct of_device_id *match;
+
+	match = of_match_node(exynos_adc_match, pdev->dev.of_node);
+	return (unsigned int)match->data;
+}
+
+static int exynos_read_raw(struct iio_dev *indio_dev,
+				struct iio_chan_spec const *chan,
+				int *val,
+				int *val2,
+				long mask)
+{
+	struct exynos_adc *info = iio_priv(indio_dev);
+	unsigned long timeout;
+	u32 con1, con2;
+
+	if (mask != IIO_CHAN_INFO_RAW)
+		return -EINVAL;
+
+	mutex_lock(&indio_dev->mlock);
+
+	/* Select the channel to be used and Trigger conversion */
+	if (info->version == ADC_V2) {
+		con2 = readl(ADC_V2_CON2(info->regs));
+		con2 &= ~ADC_V2_CON2_ACH_MASK;
+		con2 |= ADC_V2_CON2_ACH_SEL(chan->address);
+		writel(con2, ADC_V2_CON2(info->regs));
+
+		con1 = readl(ADC_V2_CON1(info->regs));
+		writel(con1 | ADC_CON_EN_START,
+				ADC_V2_CON1(info->regs));
+	} else {
+		writel(chan->address, ADC_V1_MUX(info->regs));
+
+		con1 = readl(ADC_V1_CON(info->regs));
+		writel(con1 | ADC_CON_EN_START,
+				ADC_V1_CON(info->regs));
+	}
+
+	timeout = wait_for_completion_interruptible_timeout
+			(&info->completion, EXYNOS_ADC_TIMEOUT);
+	*val = info->value;
+
+	mutex_unlock(&indio_dev->mlock);
+
+	if (timeout == 0)
+		return -ETIMEDOUT;
+
+	return IIO_VAL_INT;
+}
+
+static irqreturn_t exynos_adc_isr(int irq, void *dev_id)
+{
+	struct exynos_adc *info = (struct exynos_adc *)dev_id;
+
+	/* Read value */
+	info->value = readl(ADC_V1_DATX(info->regs)) &
+						ADC_DATX_MASK;
+	/* clear irq */
+	if (info->version == ADC_V2)
+		writel(1, ADC_V2_INT_ST(info->regs));
+	else
+		writel(1, ADC_V1_INTCLR(info->regs));
+
+	complete(&info->completion);
+
+	return IRQ_HANDLED;
+}
+
+static int exynos_adc_reg_access(struct iio_dev *indio_dev,
+			      unsigned reg, unsigned writeval,
+			      unsigned *readval)
+{
+	struct exynos_adc *info = iio_priv(indio_dev);
+
+	if (readval == NULL)
+		return -EINVAL;
+
+	*readval = readl(info->regs + reg);
+
+	return 0;
+}
+
+static const struct iio_info exynos_adc_iio_info = {
+	.read_raw = &exynos_read_raw,
+	.debugfs_reg_access = &exynos_adc_reg_access,
+	.driver_module = THIS_MODULE,
+};
+
+#define ADC_CHANNEL(_index, _id) {			\
+	.type = IIO_VOLTAGE,				\
+	.indexed = 1,					\
+	.channel = _index,				\
+	.address = _index,				\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),	\
+	.datasheet_name = _id,				\
+}
+
+static const struct iio_chan_spec exynos_adc_iio_channels[] = {
+	ADC_CHANNEL(0, "adc0"),
+	ADC_CHANNEL(1, "adc1"),
+	ADC_CHANNEL(2, "adc2"),
+	ADC_CHANNEL(3, "adc3"),
+	ADC_CHANNEL(4, "adc4"),
+	ADC_CHANNEL(5, "adc5"),
+	ADC_CHANNEL(6, "adc6"),
+	ADC_CHANNEL(7, "adc7"),
+	ADC_CHANNEL(8, "adc8"),
+	ADC_CHANNEL(9, "adc9"),
+};
+
+static int exynos_adc_remove_devices(struct device *dev, void *c)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+
+	platform_device_unregister(pdev);
+
+	return 0;
+}
+
+static void exynos_adc_hw_init(struct exynos_adc *info)
+{
+	u32 con1, con2;
+
+	if (info->version == ADC_V2) {
+		con1 = ADC_V2_CON1_SOFT_RESET;
+		writel(con1, ADC_V2_CON1(info->regs));
+
+		con2 = ADC_V2_CON2_OSEL | ADC_V2_CON2_ESEL |
+			ADC_V2_CON2_HIGHF | ADC_V2_CON2_C_TIME(0);
+		writel(con2, ADC_V2_CON2(info->regs));
+
+		/* Enable interrupts */
+		writel(1, ADC_V2_INT_EN(info->regs));
+	} else {
+		/* set default prescaler values and Enable prescaler */
+		con1 =  ADC_V1_CON_PRSCLV(49) | ADC_V1_CON_PRSCEN;
+
+		/* Enable 12-bit ADC resolution */
+		con1 |= ADC_V1_CON_RES;
+		writel(con1, ADC_V1_CON(info->regs));
+	}
+}
+
+static int exynos_adc_probe(struct platform_device *pdev)
+{
+	struct exynos_adc *info = NULL;
+	struct device_node *np = pdev->dev.of_node;
+	struct iio_dev *indio_dev = NULL;
+	struct resource	*mem;
+	int ret = -ENODEV;
+	int irq;
+
+	if (!np)
+		return ret;
+
+	indio_dev = iio_device_alloc(sizeof(struct exynos_adc));
+	if (!indio_dev) {
+		dev_err(&pdev->dev, "failed allocating iio device\n");
+		return -ENOMEM;
+	}
+
+	info = iio_priv(indio_dev);
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	info->regs = devm_request_and_ioremap(&pdev->dev, mem);
+	if (!info->regs) {
+		ret = -ENOMEM;
+		goto err_iio;
+	}
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	info->enable_reg = devm_request_and_ioremap(&pdev->dev, mem);
+	if (!info->enable_reg) {
+		ret = -ENOMEM;
+		goto err_iio;
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "no irq resource?\n");
+		ret = irq;
+		goto err_iio;
+	}
+
+	info->irq = irq;
+
+	init_completion(&info->completion);
+
+	ret = request_irq(info->irq, exynos_adc_isr,
+					0, dev_name(&pdev->dev), info);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed requesting irq, irq = %d\n",
+							info->irq);
+		goto err_iio;
+	}
+
+	writel(1, info->enable_reg);
+
+	info->clk = devm_clk_get(&pdev->dev, "adc");
+	if (IS_ERR(info->clk)) {
+		dev_err(&pdev->dev, "failed getting clock, err = %ld\n",
+							PTR_ERR(info->clk));
+		ret = PTR_ERR(info->clk);
+		goto err_irq;
+	}
+
+	info->vdd = devm_regulator_get(&pdev->dev, "vdd");
+	if (IS_ERR(info->vdd)) {
+		dev_err(&pdev->dev, "failed getting regulator, err = %ld\n",
+							PTR_ERR(info->vdd));
+		ret = PTR_ERR(info->vdd);
+		goto err_irq;
+	}
+
+	info->version = exynos_adc_get_version(pdev);
+
+	platform_set_drvdata(pdev, indio_dev);
+
+	indio_dev->name = dev_name(&pdev->dev);
+	indio_dev->dev.parent = &pdev->dev;
+	indio_dev->dev.of_node = pdev->dev.of_node;
+	indio_dev->info = &exynos_adc_iio_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = exynos_adc_iio_channels;
+
+	if (info->version == ADC_V1)
+		indio_dev->num_channels = MAX_ADC_V1_CHANNELS;
+	else
+		indio_dev->num_channels = MAX_ADC_V2_CHANNELS;
+
+	ret = iio_device_register(indio_dev);
+	if (ret)
+		goto err_irq;
+
+	ret = regulator_enable(info->vdd);
+	if (ret)
+		goto err_iio_dev;
+
+	clk_prepare_enable(info->clk);
+
+	exynos_adc_hw_init(info);
+
+	ret = of_platform_populate(np, exynos_adc_match, NULL, &pdev->dev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed adding child nodes\n");
+		goto err_of_populate;
+	}
+
+	return 0;
+
+err_of_populate:
+	device_for_each_child(&pdev->dev, NULL,
+				exynos_adc_remove_devices);
+	regulator_disable(info->vdd);
+	clk_disable_unprepare(info->clk);
+err_iio_dev:
+	iio_device_unregister(indio_dev);
+err_irq:
+	free_irq(info->irq, info);
+err_iio:
+	iio_device_free(indio_dev);
+	return ret;
+}
+
+static int exynos_adc_remove(struct platform_device *pdev)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+	struct exynos_adc *info = iio_priv(indio_dev);
+
+	device_for_each_child(&pdev->dev, NULL,
+				exynos_adc_remove_devices);
+	regulator_disable(info->vdd);
+	clk_disable_unprepare(info->clk);
+	writel(0, info->enable_reg);
+	iio_device_unregister(indio_dev);
+	free_irq(info->irq, info);
+	iio_device_free(indio_dev);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int exynos_adc_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct exynos_adc *info = platform_get_drvdata(pdev);
+	u32 con;
+
+	if (info->version == ADC_V2) {
+		con = readl(ADC_V2_CON1(info->regs));
+		con &= ~ADC_CON_EN_START;
+		writel(con, ADC_V2_CON1(info->regs));
+	} else {
+		con = readl(ADC_V1_CON(info->regs));
+		con |= ADC_V1_CON_STANDBY;
+		writel(con, ADC_V1_CON(info->regs));
+	}
+
+	clk_disable_unprepare(info->clk);
+	writel(0, info->enable_reg);
+	regulator_disable(info->vdd);
+
+	return 0;
+}
+
+static int exynos_adc_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct exynos_adc *info = platform_get_drvdata(pdev);
+	int ret;
+
+	ret = regulator_enable(info->vdd);
+	if (ret)
+		return ret;
+
+	writel(1, info->enable_reg);
+	clk_prepare_enable(info->clk);
+
+	exynos_adc_hw_init(info);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(exynos_adc_pm_ops,
+			exynos_adc_suspend,
+			exynos_adc_resume);
+
+static struct platform_driver exynos_adc_driver = {
+	.probe		= exynos_adc_probe,
+	.remove		= exynos_adc_remove,
+	.driver		= {
+		.name	= "exynos-adc",
+		.owner	= THIS_MODULE,
+		.of_match_table = exynos_adc_match,
+		.pm	= &exynos_adc_pm_ops,
+	},
+};
+
+module_platform_driver(exynos_adc_driver);
+
+MODULE_AUTHOR("Naveen Krishna Chatradhi <ch.naveen@samsung.com>");
+MODULE_DESCRIPTION("Samsung EXYNOS5 ADC driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/lp8788_adc.c b/drivers/iio/adc/lp8788_adc.c
index 763f57565ee..62bc39e9c94 100644
--- a/drivers/iio/adc/lp8788_adc.c
+++ b/drivers/iio/adc/lp8788_adc.c
@@ -132,8 +132,8 @@ static const struct iio_info lp8788_adc_info = {
 		.type = _type,					\
 		.indexed = 1,					\
 		.channel = LPADC_##_id,				\
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |	\
-			IIO_CHAN_INFO_SCALE_SEPARATE_BIT,	\
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |	\
+			BIT(IIO_CHAN_INFO_SCALE),		\
 		.datasheet_name = #_id,				\
 }
 
diff --git a/drivers/iio/adc/max1363.c b/drivers/iio/adc/max1363.c
index 6c1cfb74bdf..9e6da72ad82 100644
--- a/drivers/iio/adc/max1363.c
+++ b/drivers/iio/adc/max1363.c
@@ -427,15 +427,15 @@ static const enum max1363_modes max1363_mode_list[] = {
 #define MAX1363_EV_M						\
 	(IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING)	\
 	 | IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_FALLING))
-#define MAX1363_INFO_MASK (IIO_CHAN_INFO_RAW_SEPARATE_BIT |	\
-			   IIO_CHAN_INFO_SCALE_SHARED_BIT)
+
 #define MAX1363_CHAN_U(num, addr, si, bits, evmask)			\
 	{								\
 		.type = IIO_VOLTAGE,					\
 		.indexed = 1,						\
 		.channel = num,						\
 		.address = addr,					\
-		.info_mask = MAX1363_INFO_MASK,				\
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),	\
 		.datasheet_name = "AIN"#num,				\
 		.scan_type = {						\
 			.sign = 'u',					\
@@ -456,7 +456,8 @@ static const enum max1363_modes max1363_mode_list[] = {
 		.channel = num,						\
 		.channel2 = num2,					\
 		.address = addr,					\
-		.info_mask = MAX1363_INFO_MASK,				\
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),	\
 		.datasheet_name = "AIN"#num"-AIN"#num2,			\
 		.scan_type = {						\
 			.sign = 's',					\
diff --git a/drivers/iio/adc/ti-adc081c.c b/drivers/iio/adc/ti-adc081c.c
index f4a46dd8f43..2826faae706 100644
--- a/drivers/iio/adc/ti-adc081c.c
+++ b/drivers/iio/adc/ti-adc081c.c
@@ -55,8 +55,8 @@ static int adc081c_read_raw(struct iio_dev *iio,
 
 static const struct iio_chan_spec adc081c_channel = {
 	.type = IIO_VOLTAGE,
-	.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT |
-		     IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 };
 
 static const struct iio_info adc081c_info = {
diff --git a/drivers/iio/adc/ti_am335x_adc.c b/drivers/iio/adc/ti_am335x_adc.c
index cd030e100c3..5f9a7e7d313 100644
--- a/drivers/iio/adc/ti_am335x_adc.c
+++ b/drivers/iio/adc/ti_am335x_adc.c
@@ -89,7 +89,7 @@ static int tiadc_channel_init(struct iio_dev *indio_dev, int channels)
 		chan->type = IIO_VOLTAGE;
 		chan->indexed = 1;
 		chan->channel = i;
-		chan->info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT;
+		chan->info_mask_separate = BIT(IIO_CHAN_INFO_RAW);
 	}
 
 	indio_dev->channels = chan_array;
diff --git a/drivers/iio/adc/viperboard_adc.c b/drivers/iio/adc/viperboard_adc.c
index ad0261533de..56ac481c73c 100644
--- a/drivers/iio/adc/viperboard_adc.c
+++ b/drivers/iio/adc/viperboard_adc.c
@@ -41,7 +41,7 @@ struct vprbrd_adc {
 	.type = IIO_VOLTAGE,				\
 	.indexed = 1,					\
 	.channel = _index,				\
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,	\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),	\
 	.scan_index = _index,				\
 	.scan_type = {					\
 		.sign = 'u',				\
diff --git a/drivers/iio/amplifiers/ad8366.c b/drivers/iio/amplifiers/ad8366.c
index d6c0af23a2a..d354554b51b 100644
--- a/drivers/iio/amplifiers/ad8366.c
+++ b/drivers/iio/amplifiers/ad8366.c
@@ -125,7 +125,7 @@ static const struct iio_info ad8366_info = {
 	.output = 1,					\
 	.indexed = 1,					\
 	.channel = _channel,				\
-	.info_mask = IIO_CHAN_INFO_HARDWAREGAIN_SEPARATE_BIT,\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_HARDWAREGAIN),\
 }
 
 static const struct iio_chan_spec ad8366_channels[] = {
diff --git a/drivers/iio/common/hid-sensors/hid-sensor-trigger.c b/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
index 7a525a91105..87419c41b99 100644
--- a/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
+++ b/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
@@ -31,7 +31,7 @@
 static int hid_sensor_data_rdy_trigger_set_state(struct iio_trigger *trig,
 						bool state)
 {
-	struct hid_sensor_common *st = trig->private_data;
+	struct hid_sensor_common *st = iio_trigger_get_drvdata(trig);
 	int state_val;
 
 	state_val = state ? 1 : 0;
@@ -76,7 +76,7 @@ int hid_sensor_setup_trigger(struct iio_dev *indio_dev, const char *name,
 	}
 
 	trig->dev.parent = indio_dev->dev.parent;
-	trig->private_data = attrb;
+	iio_trigger_set_drvdata(trig, attrb);
 	trig->ops = &hid_sensor_trigger_ops;
 	ret = iio_trigger_register(trig);
 
diff --git a/drivers/iio/common/st_sensors/st_sensors_spi.c b/drivers/iio/common/st_sensors/st_sensors_spi.c
index f0aa2f10522..251baf6abc2 100644
--- a/drivers/iio/common/st_sensors/st_sensors_spi.c
+++ b/drivers/iio/common/st_sensors/st_sensors_spi.c
@@ -29,7 +29,6 @@ static unsigned int st_sensors_spi_get_irq(struct iio_dev *indio_dev)
 static int st_sensors_spi_read(struct st_sensor_transfer_buffer *tb,
 	struct device *dev, u8 reg_addr, int len, u8 *data, bool multiread_bit)
 {
-	struct spi_message msg;
 	int err;
 
 	struct spi_transfer xfers[] = {
@@ -51,10 +50,7 @@ static int st_sensors_spi_read(struct st_sensor_transfer_buffer *tb,
 	else
 		tb->tx_buf[0] = reg_addr | ST_SENSORS_SPI_READ;
 
-	spi_message_init(&msg);
-	spi_message_add_tail(&xfers[0], &msg);
-	spi_message_add_tail(&xfers[1], &msg);
-	err = spi_sync(to_spi_device(dev), &msg);
+	err = spi_sync_transfer(to_spi_device(dev), xfers, ARRAY_SIZE(xfers));
 	if (err)
 		goto acc_spi_read_error;
 
@@ -83,7 +79,6 @@ static int st_sensors_spi_read_multiple_byte(
 static int st_sensors_spi_write_byte(struct st_sensor_transfer_buffer *tb,
 				struct device *dev, u8 reg_addr, u8 data)
 {
-	struct spi_message msg;
 	int err;
 
 	struct spi_transfer xfers = {
@@ -96,9 +91,7 @@ static int st_sensors_spi_write_byte(struct st_sensor_transfer_buffer *tb,
 	tb->tx_buf[0] = reg_addr;
 	tb->tx_buf[1] = data;
 
-	spi_message_init(&msg);
-	spi_message_add_tail(&xfers, &msg);
-	err = spi_sync(to_spi_device(dev), &msg);
+	err = spi_sync_transfer(to_spi_device(dev), &xfers, 1);
 	mutex_unlock(&tb->buf_lock);
 
 	return err;
diff --git a/drivers/iio/common/st_sensors/st_sensors_trigger.c b/drivers/iio/common/st_sensors/st_sensors_trigger.c
index 139ed030abb..8fc3a97eb26 100644
--- a/drivers/iio/common/st_sensors/st_sensors_trigger.c
+++ b/drivers/iio/common/st_sensors/st_sensors_trigger.c
@@ -40,7 +40,7 @@ int st_sensors_allocate_trigger(struct iio_dev *indio_dev,
 	if (err)
 		goto request_irq_error;
 
-	sdata->trig->private_data = indio_dev;
+	iio_trigger_set_drvdata(sdata->trig, indio_dev);
 	sdata->trig->ops = trigger_ops;
 	sdata->trig->dev.parent = sdata->dev;
 
diff --git a/drivers/iio/dac/ad5064.c b/drivers/iio/dac/ad5064.c
index 74f2d52795f..aa26d50ab63 100644
--- a/drivers/iio/dac/ad5064.c
+++ b/drivers/iio/dac/ad5064.c
@@ -296,8 +296,8 @@ static const struct iio_chan_spec_ext_info ad5064_ext_info[] = {
 	.indexed = 1,						\
 	.output = 1,						\
 	.channel = (chan),					\
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |		\
-	IIO_CHAN_INFO_SCALE_SEPARATE_BIT,			\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |		\
+	BIT(IIO_CHAN_INFO_SCALE),					\
 	.address = addr,					\
 	.scan_type = IIO_ST('u', (bits), 16, 20 - (bits)),	\
 	.ext_info = ad5064_ext_info,				\
diff --git a/drivers/iio/dac/ad5360.c b/drivers/iio/dac/ad5360.c
index 92771217f66..80923af424f 100644
--- a/drivers/iio/dac/ad5360.c
+++ b/drivers/iio/dac/ad5360.c
@@ -102,11 +102,11 @@ enum ad5360_type {
 	.type = IIO_VOLTAGE,					\
 	.indexed = 1,						\
 	.output = 1,						\
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |		\
-		IIO_CHAN_INFO_SCALE_SEPARATE_BIT |		\
-		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |		\
-		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |	\
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,	\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |		\
+		BIT(IIO_CHAN_INFO_SCALE) |				\
+		BIT(IIO_CHAN_INFO_OFFSET) |				\
+		BIT(IIO_CHAN_INFO_CALIBSCALE) |			\
+		BIT(IIO_CHAN_INFO_CALIBBIAS),			\
 	.scan_type = IIO_ST('u', (bits), 16, 16 - (bits))	\
 }
 
diff --git a/drivers/iio/dac/ad5380.c b/drivers/iio/dac/ad5380.c
index 483fc379a2d..bf2db02215c 100644
--- a/drivers/iio/dac/ad5380.c
+++ b/drivers/iio/dac/ad5380.c
@@ -257,10 +257,10 @@ static struct iio_chan_spec_ext_info ad5380_ext_info[] = {
 	.type = IIO_VOLTAGE,					\
 	.indexed = 1,						\
 	.output = 1,						\
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |		\
-		IIO_CHAN_INFO_SCALE_SHARED_BIT |		\
-		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |		\
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,		\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |		\
+		BIT(IIO_CHAN_INFO_CALIBSCALE) |			\
+		BIT(IIO_CHAN_INFO_CALIBBIAS),			\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),	\
 	.scan_type = IIO_ST('u', (_bits), 16, 14 - (_bits)),	\
 	.ext_info = ad5380_ext_info,				\
 }
diff --git a/drivers/iio/dac/ad5421.c b/drivers/iio/dac/ad5421.c
index 6b86a638dad..98f24407c3c 100644
--- a/drivers/iio/dac/ad5421.c
+++ b/drivers/iio/dac/ad5421.c
@@ -86,11 +86,11 @@ static const struct iio_chan_spec ad5421_channels[] = {
 		.indexed = 1,
 		.output = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SHARED_BIT |
-			IIO_CHAN_INFO_OFFSET_SHARED_BIT |
-			IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
-			IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_CALIBSCALE) |
+			BIT(IIO_CHAN_INFO_CALIBBIAS),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) |
+			BIT(IIO_CHAN_INFO_OFFSET),
 		.scan_type = IIO_ST('u', 16, 16, 0),
 		.event_mask = IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING) |
 			IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_FALLING),
diff --git a/drivers/iio/dac/ad5446.c b/drivers/iio/dac/ad5446.c
index f5583aedfb5..cae8f6056ac 100644
--- a/drivers/iio/dac/ad5446.c
+++ b/drivers/iio/dac/ad5446.c
@@ -143,8 +143,8 @@ static const struct iio_chan_spec_ext_info ad5446_ext_info_powerdown[] = {
 	.indexed = 1, \
 	.output = 1, \
 	.channel = 0, \
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
-	IIO_CHAN_INFO_SCALE_SHARED_BIT,	\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \
 	.scan_type = IIO_ST('u', (bits), (storage), (shift)), \
 	.ext_info = (ext), \
 }
diff --git a/drivers/iio/dac/ad5449.c b/drivers/iio/dac/ad5449.c
index c4731b7b577..ba1c914b039 100644
--- a/drivers/iio/dac/ad5449.c
+++ b/drivers/iio/dac/ad5449.c
@@ -206,8 +206,8 @@ static const struct iio_info ad5449_info = {
 	.indexed = 1,						\
 	.output = 1,						\
 	.channel = (chan),					\
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |		\
-		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,		\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |		\
+		BIT(IIO_CHAN_INFO_SCALE),			\
 	.address = (chan),					\
 	.scan_type = IIO_ST('u', (bits), 16, 12 - (bits)),	\
 }
diff --git a/drivers/iio/dac/ad5504.c b/drivers/iio/dac/ad5504.c
index e5e59749f10..139206e84cb 100644
--- a/drivers/iio/dac/ad5504.c
+++ b/drivers/iio/dac/ad5504.c
@@ -259,8 +259,8 @@ static const struct iio_chan_spec_ext_info ad5504_ext_info[] = {
 	.indexed = 1, \
 	.output = 1, \
 	.channel = (_chan), \
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
-		     IIO_CHAN_INFO_SCALE_SHARED_BIT, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \
 	.address = AD5504_ADDR_DAC(_chan), \
 	.scan_type = IIO_ST('u', 12, 16, 0), \
 	.ext_info = ad5504_ext_info, \
diff --git a/drivers/iio/dac/ad5624r_spi.c b/drivers/iio/dac/ad5624r_spi.c
index f6e116627b7..bb298aaff32 100644
--- a/drivers/iio/dac/ad5624r_spi.c
+++ b/drivers/iio/dac/ad5624r_spi.c
@@ -174,8 +174,8 @@ static const struct iio_chan_spec_ext_info ad5624r_ext_info[] = {
 	.indexed = 1, \
 	.output = 1, \
 	.channel = (_chan), \
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
-		     IIO_CHAN_INFO_SCALE_SHARED_BIT, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \
 	.address = (_chan), \
 	.scan_type = IIO_ST('u', (_bits), 16, 16 - (_bits)), \
 	.ext_info = ad5624r_ext_info, \
diff --git a/drivers/iio/dac/ad5686.c b/drivers/iio/dac/ad5686.c
index 5e554af2170..06439b1af9b 100644
--- a/drivers/iio/dac/ad5686.c
+++ b/drivers/iio/dac/ad5686.c
@@ -276,9 +276,9 @@ static const struct iio_chan_spec_ext_info ad5686_ext_info[] = {
 		.indexed = 1,					\
 		.output = 1,					\
 		.channel = chan,				\
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |	\
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,			\
-		.address = AD5686_ADDR_DAC(chan),			\
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),	\
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),\
+		.address = AD5686_ADDR_DAC(chan),		\
 		.scan_type = IIO_ST('u', bits, 16, shift),	\
 		.ext_info = ad5686_ext_info,			\
 }
diff --git a/drivers/iio/dac/ad5755.c b/drivers/iio/dac/ad5755.c
index 71faabc6b14..12bb315e55f 100644
--- a/drivers/iio/dac/ad5755.c
+++ b/drivers/iio/dac/ad5755.c
@@ -393,11 +393,11 @@ static const struct iio_chan_spec_ext_info ad5755_ext_info[] = {
 #define AD5755_CHANNEL(_bits) {					\
 	.indexed = 1,						\
 	.output = 1,						\
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |		\
-		IIO_CHAN_INFO_SCALE_SEPARATE_BIT |		\
-		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |		\
-		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |		\
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,		\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |		\
+		BIT(IIO_CHAN_INFO_SCALE) |			\
+		BIT(IIO_CHAN_INFO_OFFSET) |			\
+		BIT(IIO_CHAN_INFO_CALIBSCALE) |			\
+		BIT(IIO_CHAN_INFO_CALIBBIAS),			\
 	.scan_type = IIO_ST('u', (_bits), 16, 16 - (_bits)),	\
 	.ext_info = ad5755_ext_info,				\
 }
diff --git a/drivers/iio/dac/ad5764.c b/drivers/iio/dac/ad5764.c
index 5b7acd3a2c7..7a53f7d70da 100644
--- a/drivers/iio/dac/ad5764.c
+++ b/drivers/iio/dac/ad5764.c
@@ -78,11 +78,11 @@ enum ad5764_type {
 	.output = 1,						\
 	.channel = (_chan),					\
 	.address = (_chan),					\
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |		\
-		IIO_CHAN_INFO_OFFSET_SHARED_BIT |		\
-		IIO_CHAN_INFO_SCALE_SEPARATE_BIT |		\
-		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |		\
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,		\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |		\
+		BIT(IIO_CHAN_INFO_SCALE) |			\
+		BIT(IIO_CHAN_INFO_CALIBSCALE) |			\
+		BIT(IIO_CHAN_INFO_CALIBBIAS),			\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET),	\
 	.scan_type = IIO_ST('u', (_bits), 16, 16 - (_bits))	\
 }
 
diff --git a/drivers/iio/dac/ad5791.c b/drivers/iio/dac/ad5791.c
index 8dfd3da8a07..97c1e5d780d 100644
--- a/drivers/iio/dac/ad5791.c
+++ b/drivers/iio/dac/ad5791.c
@@ -302,9 +302,9 @@ static const struct iio_chan_spec_ext_info ad5791_ext_info[] = {
 	.indexed = 1,					\
 	.address = AD5791_ADDR_DAC0,			\
 	.channel = 0,					\
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |	\
-		IIO_CHAN_INFO_SCALE_SHARED_BIT |	\
-		IIO_CHAN_INFO_OFFSET_SHARED_BIT,	\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),	\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) |	\
+		BIT(IIO_CHAN_INFO_OFFSET),		\
 	.scan_type = IIO_ST('u', bits, 24, shift),	\
 	.ext_info = ad5791_ext_info,			\
 }
diff --git a/drivers/iio/dac/max517.c b/drivers/iio/dac/max517.c
index 352abe2004a..ebfaa415624 100644
--- a/drivers/iio/dac/max517.c
+++ b/drivers/iio/dac/max517.c
@@ -146,8 +146,8 @@ static const struct iio_info max517_info = {
 	.indexed = 1,					\
 	.output = 1,					\
 	.channel = (chan),				\
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |	\
-	IIO_CHAN_INFO_SCALE_SEPARATE_BIT,		\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |	\
+	BIT(IIO_CHAN_INFO_SCALE),			\
 	.scan_type = IIO_ST('u', 8, 8, 0),		\
 }
 
diff --git a/drivers/iio/dac/mcp4725.c b/drivers/iio/dac/mcp4725.c
index 8f88cc4059a..a612ec766d9 100644
--- a/drivers/iio/dac/mcp4725.c
+++ b/drivers/iio/dac/mcp4725.c
@@ -69,8 +69,8 @@ static const struct iio_chan_spec mcp4725_channel = {
 	.indexed	= 1,
 	.output		= 1,
 	.channel	= 0,
-	.info_mask	= IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			  IIO_CHAN_INFO_SCALE_SHARED_BIT,
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
 	.scan_type	= IIO_ST('u', 12, 16, 0),
 };
 
diff --git a/drivers/iio/frequency/ad9523.c b/drivers/iio/frequency/ad9523.c
index 1ea132e239e..92276deeb02 100644
--- a/drivers/iio/frequency/ad9523.c
+++ b/drivers/iio/frequency/ad9523.c
@@ -920,10 +920,10 @@ static int ad9523_setup(struct iio_dev *indio_dev)
 			st->ad9523_channels[i].channel = chan->channel_num;
 			st->ad9523_channels[i].extend_name =
 				chan->extended_name;
-			st->ad9523_channels[i].info_mask =
-				IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-				IIO_CHAN_INFO_PHASE_SEPARATE_BIT |
-				IIO_CHAN_INFO_FREQUENCY_SEPARATE_BIT;
+			st->ad9523_channels[i].info_mask_separate =
+				BIT(IIO_CHAN_INFO_RAW) |
+				BIT(IIO_CHAN_INFO_PHASE) |
+				BIT(IIO_CHAN_INFO_FREQUENCY);
 		}
 	}
 
diff --git a/drivers/iio/gyro/adis16080.c b/drivers/iio/gyro/adis16080.c
index 1861287911f..e1bb5f994a5 100644
--- a/drivers/iio/gyro/adis16080.c
+++ b/drivers/iio/gyro/adis16080.c
@@ -136,32 +136,32 @@ static const struct iio_chan_spec adis16080_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_SCALE),
 		.address = ADIS16080_DIN_GYRO,
 	}, {
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
-			IIO_CHAN_INFO_OFFSET_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_SCALE) |
+			BIT(IIO_CHAN_INFO_OFFSET),
 		.address = ADIS16080_DIN_AIN1,
 	}, {
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
 		.channel = 1,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
-			IIO_CHAN_INFO_OFFSET_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_SCALE) |
+			BIT(IIO_CHAN_INFO_OFFSET),
 		.address = ADIS16080_DIN_AIN2,
 	}, {
 		.type = IIO_TEMP,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
-			IIO_CHAN_INFO_OFFSET_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_SCALE) |
+			BIT(IIO_CHAN_INFO_OFFSET),
 		.address = ADIS16080_DIN_TEMP,
 	}
 };
diff --git a/drivers/iio/gyro/adis16136.c b/drivers/iio/gyro/adis16136.c
index 8cb0bcbfd60..058e6d5c955 100644
--- a/drivers/iio/gyro/adis16136.c
+++ b/drivers/iio/gyro/adis16136.c
@@ -357,10 +357,11 @@ static const struct iio_chan_spec adis16136_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SHARED_BIT |
-			IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_CALIBBIAS) |
+			BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
+
 		.address = ADIS16136_REG_GYRO_OUT2,
 		.scan_index = ADIS16136_SCAN_GYRO,
 		.scan_type = {
@@ -373,8 +374,8 @@ static const struct iio_chan_spec adis16136_channels[] = {
 		.type = IIO_TEMP,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_SCALE),
 		.address = ADIS16136_REG_TEMP_OUT,
 		.scan_index = ADIS16136_SCAN_TEMP,
 		.scan_type = {
diff --git a/drivers/iio/gyro/adxrs450.c b/drivers/iio/gyro/adxrs450.c
index 5b79953f701..8bd72b490b7 100644
--- a/drivers/iio/gyro/adxrs450.c
+++ b/drivers/iio/gyro/adxrs450.c
@@ -383,16 +383,16 @@ static const struct iio_chan_spec adxrs450_channels[2][2] = {
 			.type = IIO_ANGL_VEL,
 			.modified = 1,
 			.channel2 = IIO_MOD_Z,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
-			IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+			.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_CALIBBIAS) |
+			BIT(IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW) |
+			BIT(IIO_CHAN_INFO_SCALE),
 		}, {
 			.type = IIO_TEMP,
 			.indexed = 1,
 			.channel = 0,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+			.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_SCALE),
 		}
 	},
 	[ID_ADXRS453] = {
@@ -400,15 +400,15 @@ static const struct iio_chan_spec adxrs450_channels[2][2] = {
 			.type = IIO_ANGL_VEL,
 			.modified = 1,
 			.channel2 = IIO_MOD_Z,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
-			IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW_SEPARATE_BIT,
+			.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_SCALE) |
+			BIT(IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW),
 		}, {
 			.type = IIO_TEMP,
 			.indexed = 1,
 			.channel = 0,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+			.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_SCALE),
 		}
 	},
 };
diff --git a/drivers/iio/gyro/hid-sensor-gyro-3d.c b/drivers/iio/gyro/hid-sensor-gyro-3d.c
index fcfc83a9f86..bc943dd47da 100644
--- a/drivers/iio/gyro/hid-sensor-gyro-3d.c
+++ b/drivers/iio/gyro/hid-sensor-gyro-3d.c
@@ -60,28 +60,28 @@ static const struct iio_chan_spec gyro_3d_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
-		.info_mask = IIO_CHAN_INFO_OFFSET_SHARED_BIT |
-		IIO_CHAN_INFO_SCALE_SHARED_BIT |
-		IIO_CHAN_INFO_SAMP_FREQ_SHARED_BIT |
-		IIO_CHAN_INFO_HYSTERESIS_SHARED_BIT,
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
+		BIT(IIO_CHAN_INFO_SCALE) |
+		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+		BIT(IIO_CHAN_INFO_HYSTERESIS),
 		.scan_index = CHANNEL_SCAN_INDEX_X,
 	}, {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
-		.info_mask = IIO_CHAN_INFO_OFFSET_SHARED_BIT |
-		IIO_CHAN_INFO_SCALE_SHARED_BIT |
-		IIO_CHAN_INFO_SAMP_FREQ_SHARED_BIT |
-		IIO_CHAN_INFO_HYSTERESIS_SHARED_BIT,
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
+		BIT(IIO_CHAN_INFO_SCALE) |
+		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+		BIT(IIO_CHAN_INFO_HYSTERESIS),
 		.scan_index = CHANNEL_SCAN_INDEX_Y,
 	}, {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
-		.info_mask = IIO_CHAN_INFO_OFFSET_SHARED_BIT |
-		IIO_CHAN_INFO_SCALE_SHARED_BIT |
-		IIO_CHAN_INFO_SAMP_FREQ_SHARED_BIT |
-		IIO_CHAN_INFO_HYSTERESIS_SHARED_BIT,
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
+		BIT(IIO_CHAN_INFO_SCALE) |
+		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+		BIT(IIO_CHAN_INFO_HYSTERESIS),
 		.scan_index = CHANNEL_SCAN_INDEX_Z,
 	}
 };
diff --git a/drivers/iio/gyro/itg3200_buffer.c b/drivers/iio/gyro/itg3200_buffer.c
index f667d2c8c00..6c43af9bb0a 100644
--- a/drivers/iio/gyro/itg3200_buffer.c
+++ b/drivers/iio/gyro/itg3200_buffer.c
@@ -81,7 +81,7 @@ void itg3200_buffer_unconfigure(struct iio_dev *indio_dev)
 static int itg3200_data_rdy_trigger_set_state(struct iio_trigger *trig,
 		bool state)
 {
-	struct iio_dev *indio_dev = trig->private_data;
+	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
 	int ret;
 	u8 msc;
 
@@ -129,7 +129,7 @@ int itg3200_probe_trigger(struct iio_dev *indio_dev)
 
 	st->trig->dev.parent = &st->i2c->dev;
 	st->trig->ops = &itg3200_trigger_ops;
-	st->trig->private_data = indio_dev;
+	iio_trigger_set_drvdata(st->trig, indio_dev);
 	ret = iio_trigger_register(st->trig);
 	if (ret)
 		goto error_free_irq;
diff --git a/drivers/iio/gyro/itg3200_core.c b/drivers/iio/gyro/itg3200_core.c
index df2e6aa5d73..d66605d2629 100644
--- a/drivers/iio/gyro/itg3200_core.c
+++ b/drivers/iio/gyro/itg3200_core.c
@@ -248,12 +248,6 @@ err_ret:
 	return ret;
 }
 
-#define ITG3200_TEMP_INFO_MASK	(IIO_CHAN_INFO_OFFSET_SHARED_BIT | \
-				 IIO_CHAN_INFO_SCALE_SHARED_BIT | \
-				 IIO_CHAN_INFO_RAW_SEPARATE_BIT)
-#define ITG3200_GYRO_INFO_MASK	(IIO_CHAN_INFO_SCALE_SHARED_BIT | \
-				 IIO_CHAN_INFO_RAW_SEPARATE_BIT)
-
 #define ITG3200_ST						\
 	{ .sign = 's', .realbits = 16, .storagebits = 16, .endianness = IIO_BE }
 
@@ -261,7 +255,8 @@ err_ret:
 	.type = IIO_ANGL_VEL, \
 	.modified = 1, \
 	.channel2 = IIO_MOD_ ## _mod, \
-	.info_mask = ITG3200_GYRO_INFO_MASK, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \
 	.address = ITG3200_REG_GYRO_ ## _mod ## OUT_H, \
 	.scan_index = ITG3200_SCAN_GYRO_ ## _mod, \
 	.scan_type = ITG3200_ST, \
@@ -271,7 +266,9 @@ static const struct iio_chan_spec itg3200_channels[] = {
 	{
 		.type = IIO_TEMP,
 		.channel2 = IIO_NO_MOD,
-		.info_mask = ITG3200_TEMP_INFO_MASK,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
+		BIT(IIO_CHAN_INFO_SCALE),
 		.address = ITG3200_REG_TEMP_OUT_H,
 		.scan_index = ITG3200_SCAN_TEMP,
 		.scan_type = ITG3200_ST,
diff --git a/drivers/iio/gyro/st_gyro_buffer.c b/drivers/iio/gyro/st_gyro_buffer.c
index da4d122ec7d..69017c7ec30 100644
--- a/drivers/iio/gyro/st_gyro_buffer.c
+++ b/drivers/iio/gyro/st_gyro_buffer.c
@@ -25,7 +25,7 @@
 
 int st_gyro_trig_set_state(struct iio_trigger *trig, bool state)
 {
-	struct iio_dev *indio_dev = trig->private_data;
+	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
 
 	return st_sensors_set_dataready_irq(indio_dev, state);
 }
diff --git a/drivers/iio/iio_core.h b/drivers/iio/iio_core.h
index f652e6ae5a3..05c1b74502a 100644
--- a/drivers/iio/iio_core.h
+++ b/drivers/iio/iio_core.h
@@ -18,6 +18,7 @@
 struct iio_chan_spec;
 struct iio_dev;
 
+extern struct device_type iio_device_type;
 
 int __iio_add_chan_devattr(const char *postfix,
 			   struct iio_chan_spec const *chan,
diff --git a/drivers/iio/imu/adis16400_core.c b/drivers/iio/imu/adis16400_core.c
index b7f215eab5d..f60591f0b92 100644
--- a/drivers/iio/imu/adis16400_core.c
+++ b/drivers/iio/imu/adis16400_core.c
@@ -484,8 +484,8 @@ static int adis16400_read_raw(struct iio_dev *indio_dev,
 	.indexed = 1, \
 	.channel = 0, \
 	.extend_name = name, \
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
-		IIO_CHAN_INFO_SCALE_SEPARATE_BIT, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+		BIT(IIO_CHAN_INFO_SCALE), \
 	.address = (addr), \
 	.scan_index = (si), \
 	.scan_type = { \
@@ -507,10 +507,10 @@ static int adis16400_read_raw(struct iio_dev *indio_dev,
 	.type = IIO_ANGL_VEL, \
 	.modified = 1, \
 	.channel2 = IIO_MOD_ ## mod, \
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT | \
-		IIO_CHAN_INFO_SCALE_SHARED_BIT | \
-		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+		BIT(IIO_CHAN_INFO_CALIBBIAS),		  \
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) | \
+		BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY), \
 	.address = addr, \
 	.scan_index = ADIS16400_SCAN_GYRO_ ## mod, \
 	.scan_type = { \
@@ -526,10 +526,10 @@ static int adis16400_read_raw(struct iio_dev *indio_dev,
 	.type = IIO_ACCEL, \
 	.modified = 1, \
 	.channel2 = IIO_MOD_ ## mod, \
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT | \
-		IIO_CHAN_INFO_SCALE_SHARED_BIT | \
-		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+		BIT(IIO_CHAN_INFO_CALIBBIAS), \
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) | \
+		BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY), \
 	.address = (addr), \
 	.scan_index = ADIS16400_SCAN_ACC_ ## mod, \
 	.scan_type = { \
@@ -545,9 +545,9 @@ static int adis16400_read_raw(struct iio_dev *indio_dev,
 	.type = IIO_MAGN, \
 	.modified = 1, \
 	.channel2 = IIO_MOD_ ## mod, \
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
-		IIO_CHAN_INFO_SCALE_SHARED_BIT | \
-		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) | \
+		BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY), \
 	.address = (addr), \
 	.scan_index = ADIS16400_SCAN_MAGN_ ## mod, \
 	.scan_type = { \
@@ -568,10 +568,11 @@ static int adis16400_read_raw(struct iio_dev *indio_dev,
 	.indexed = 1, \
 	.channel = 0, \
 	.extend_name = ADIS16400_MOD_TEMP_NAME_ ## mod, \
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
-		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT | \
-		IIO_CHAN_INFO_SCALE_SEPARATE_BIT | \
-		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+		BIT(IIO_CHAN_INFO_OFFSET) | \
+		BIT(IIO_CHAN_INFO_SCALE), \
+	.info_mask_shared_by_type = \
+		BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY), \
 	.address = (addr), \
 	.scan_index = ADIS16350_SCAN_TEMP_ ## mod, \
 	.scan_type = { \
@@ -587,9 +588,9 @@ static int adis16400_read_raw(struct iio_dev *indio_dev,
 	.type = IIO_TEMP, \
 	.indexed = 1, \
 	.channel = 0, \
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
-		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT | \
-		IIO_CHAN_INFO_SCALE_SEPARATE_BIT, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+		BIT(IIO_CHAN_INFO_OFFSET) | \
+		BIT(IIO_CHAN_INFO_SCALE), \
 	.address = (addr), \
 	.scan_index = ADIS16350_SCAN_TEMP_X, \
 	.scan_type = { \
@@ -605,8 +606,8 @@ static int adis16400_read_raw(struct iio_dev *indio_dev,
 	.type = IIO_INCLI, \
 	.modified = 1, \
 	.channel2 = IIO_MOD_ ## mod, \
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
-		IIO_CHAN_INFO_SCALE_SHARED_BIT, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \
 	.address = (addr), \
 	.scan_index = ADIS16300_SCAN_INCLI_ ## mod, \
 	.scan_type = { \
@@ -646,8 +647,8 @@ static const struct iio_chan_spec adis16448_channels[] = {
 	ADIS16400_MAGN_CHAN(Z, ADIS16400_ZMAGN_OUT, 16),
 	{
 		.type = IIO_PRESSURE,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
 		.address = ADIS16448_BARO_OUT,
 		.scan_index = ADIS16400_SCAN_BARO,
 		.scan_type = IIO_ST('s', 16, 16, 0),
diff --git a/drivers/iio/imu/adis16480.c b/drivers/iio/imu/adis16480.c
index 8c26a5f7cd5..b7db3837629 100644
--- a/drivers/iio/imu/adis16480.c
+++ b/drivers/iio/imu/adis16480.c
@@ -591,15 +591,15 @@ static int adis16480_write_raw(struct iio_dev *indio_dev,
 	}
 }
 
-#define ADIS16480_MOD_CHANNEL(_type, _mod, _address, _si, _info, _bits) \
+#define ADIS16480_MOD_CHANNEL(_type, _mod, _address, _si, _info_sep, _bits) \
 	{ \
 		.type = (_type), \
 		.modified = 1, \
 		.channel2 = (_mod), \
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
-			IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT | \
-			IIO_CHAN_INFO_SCALE_SHARED_BIT | \
-			_info, \
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+			BIT(IIO_CHAN_INFO_CALIBBIAS) | \
+			_info_sep, \
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \
 		.address = (_address), \
 		.scan_index = (_si), \
 		.scan_type = { \
@@ -613,21 +613,21 @@ static int adis16480_write_raw(struct iio_dev *indio_dev,
 #define ADIS16480_GYRO_CHANNEL(_mod) \
 	ADIS16480_MOD_CHANNEL(IIO_ANGL_VEL, IIO_MOD_ ## _mod, \
 	ADIS16480_REG_ ## _mod ## _GYRO_OUT, ADIS16480_SCAN_GYRO_ ## _mod, \
-	IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SEPARATE_BIT | \
-	IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT, \
+	BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY) | \
+	BIT(IIO_CHAN_INFO_CALIBSCALE), \
 	32)
 
 #define ADIS16480_ACCEL_CHANNEL(_mod) \
 	ADIS16480_MOD_CHANNEL(IIO_ACCEL, IIO_MOD_ ## _mod, \
 	ADIS16480_REG_ ## _mod ## _ACCEL_OUT, ADIS16480_SCAN_ACCEL_ ## _mod, \
-	IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SEPARATE_BIT | \
-	IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT, \
+	BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY) | \
+	BIT(IIO_CHAN_INFO_CALIBSCALE), \
 	32)
 
 #define ADIS16480_MAGN_CHANNEL(_mod) \
 	ADIS16480_MOD_CHANNEL(IIO_MAGN, IIO_MOD_ ## _mod, \
 	ADIS16480_REG_ ## _mod ## _MAGN_OUT, ADIS16480_SCAN_MAGN_ ## _mod, \
-	IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SEPARATE_BIT, \
+	BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY), \
 	16)
 
 #define ADIS16480_PRESSURE_CHANNEL() \
@@ -635,9 +635,9 @@ static int adis16480_write_raw(struct iio_dev *indio_dev,
 		.type = IIO_PRESSURE, \
 		.indexed = 1, \
 		.channel = 0, \
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
-			IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT | \
-			IIO_CHAN_INFO_SCALE_SEPARATE_BIT, \
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+			BIT(IIO_CHAN_INFO_CALIBBIAS) | \
+			BIT(IIO_CHAN_INFO_SCALE), \
 		.address = ADIS16480_REG_BAROM_OUT, \
 		.scan_index = ADIS16480_SCAN_BARO, \
 		.scan_type = { \
@@ -652,9 +652,9 @@ static int adis16480_write_raw(struct iio_dev *indio_dev,
 		.type = IIO_TEMP, \
 		.indexed = 1, \
 		.channel = 0, \
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
-			IIO_CHAN_INFO_SCALE_SEPARATE_BIT | \
-			IIO_CHAN_INFO_OFFSET_SEPARATE_BIT, \
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+			BIT(IIO_CHAN_INFO_SCALE) | \
+			BIT(IIO_CHAN_INFO_OFFSET), \
 		.address = ADIS16480_REG_TEMP_OUT, \
 		.scan_index = ADIS16480_SCAN_TEMP, \
 		.scan_type = { \
diff --git a/drivers/iio/imu/adis_trigger.c b/drivers/iio/imu/adis_trigger.c
index 5a24c9cac34..e0017c22bb9 100644
--- a/drivers/iio/imu/adis_trigger.c
+++ b/drivers/iio/imu/adis_trigger.c
@@ -19,7 +19,7 @@
 static int adis_data_rdy_trigger_set_state(struct iio_trigger *trig,
 						bool state)
 {
-	struct adis *adis = trig->private_data;
+	struct adis *adis = iio_trigger_get_drvdata(trig);
 
 	return adis_enable_irq(adis, state);
 }
@@ -57,7 +57,7 @@ int adis_probe_trigger(struct adis *adis, struct iio_dev *indio_dev)
 
 	adis->trig->dev.parent = &adis->spi->dev;
 	adis->trig->ops = &adis_trigger_ops;
-	adis->trig->private_data = adis;
+	iio_trigger_set_drvdata(adis->trig, adis);
 	ret = iio_trigger_register(adis->trig);
 
 	indio_dev->trig = adis->trig;
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index 37ca05b47e4..fe4c61e219f 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -544,8 +544,8 @@ static int inv_mpu6050_validate_trigger(struct iio_dev *indio_dev,
 		.type = _type,                                        \
 		.modified = 1,                                        \
 		.channel2 = _channel2,                                \
-		.info_mask =  IIO_CHAN_INFO_SCALE_SHARED_BIT          \
-				| IIO_CHAN_INFO_RAW_SEPARATE_BIT,     \
+		.info_mask_shared_by_type =  BIT(IIO_CHAN_INFO_SCALE), \
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),         \
 		.scan_index = _index,                                 \
 		.scan_type = {                                        \
 				.sign = 's',                          \
@@ -564,9 +564,9 @@ static const struct iio_chan_spec inv_mpu_channels[] = {
 	 */
 	{
 		.type = IIO_TEMP,
-		.info_mask =  IIO_CHAN_INFO_RAW_SEPARATE_BIT
-				| IIO_CHAN_INFO_OFFSET_SEPARATE_BIT
-				| IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.info_mask_separate =  BIT(IIO_CHAN_INFO_RAW)
+				| BIT(IIO_CHAN_INFO_OFFSET)
+				| BIT(IIO_CHAN_INFO_SCALE),
 		.scan_index = -1,
 	},
 	INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_X, INV_MPU6050_SCAN_GYRO_X),
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
index 331781ffbb1..7da0832f187 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
@@ -105,9 +105,8 @@ irqreturn_t inv_mpu6050_irq_handler(int irq, void *p)
 	s64 timestamp;
 
 	timestamp = iio_get_time_ns();
-	spin_lock(&st->time_stamp_lock);
-	kfifo_in(&st->timestamps, &timestamp, 1);
-	spin_unlock(&st->time_stamp_lock);
+	kfifo_in_spinlocked(&st->timestamps, &timestamp, 1,
+				&st->time_stamp_lock);
 
 	return IRQ_WAKE_THREAD;
 }
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
index e1d0869e0ad..03b9372c121 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
@@ -103,7 +103,7 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
 static int inv_mpu_data_rdy_trigger_set_state(struct iio_trigger *trig,
 						bool state)
 {
-	return inv_mpu6050_set_enable(trig->private_data, state);
+	return inv_mpu6050_set_enable(iio_trigger_get_drvdata(trig), state);
 }
 
 static const struct iio_trigger_ops inv_mpu_trigger_ops = {
@@ -130,8 +130,8 @@ int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev)
 	if (ret)
 		goto error_free_trig;
 	st->trig->dev.parent = &st->client->dev;
-	st->trig->private_data = indio_dev;
 	st->trig->ops = &inv_mpu_trigger_ops;
+	iio_trigger_set_drvdata(st->trig, indio_dev);
 	ret = iio_trigger_register(st->trig);
 	if (ret)
 		goto error_free_irq;
diff --git a/drivers/iio/industrialio-core.c b/drivers/iio/industrialio-core.c
index 8848f16c547..e145931ef1b 100644
--- a/drivers/iio/industrialio-core.c
+++ b/drivers/iio/industrialio-core.c
@@ -691,21 +691,34 @@ static int iio_device_add_channel_sysfs(struct iio_dev *indio_dev,
 
 	if (chan->channel < 0)
 		return 0;
-	for_each_set_bit(i, &chan->info_mask, sizeof(long)*8) {
-		ret = __iio_add_chan_devattr(iio_chan_info_postfix[i/2],
+	for_each_set_bit(i, &chan->info_mask_separate, sizeof(long)*8) {
+		ret = __iio_add_chan_devattr(iio_chan_info_postfix[i],
 					     chan,
 					     &iio_read_channel_info,
 					     &iio_write_channel_info,
-					     i/2,
-					     !(i%2),
+					     i,
+					     0,
 					     &indio_dev->dev,
 					     &indio_dev->channel_attr_list);
-		if (ret == -EBUSY && (i%2 == 0)) {
+		if (ret < 0)
+			goto error_ret;
+		attrcount++;
+	}
+	for_each_set_bit(i, &chan->info_mask_shared_by_type, sizeof(long)*8) {
+		ret = __iio_add_chan_devattr(iio_chan_info_postfix[i],
+					     chan,
+					     &iio_read_channel_info,
+					     &iio_write_channel_info,
+					     i,
+					     1,
+					     &indio_dev->dev,
+					     &indio_dev->channel_attr_list);
+		if (ret == -EBUSY) {
 			ret = 0;
 			continue;
-		}
-		if (ret < 0)
+		} else if (ret < 0) {
 			goto error_ret;
+		}
 		attrcount++;
 	}
 
@@ -847,7 +860,7 @@ static void iio_dev_release(struct device *device)
 	kfree(indio_dev);
 }
 
-static struct device_type iio_dev_type = {
+struct device_type iio_device_type = {
 	.name = "iio_device",
 	.release = iio_dev_release,
 };
@@ -869,7 +882,7 @@ struct iio_dev *iio_device_alloc(int sizeof_priv)
 
 	if (dev) {
 		dev->dev.groups = dev->groups;
-		dev->dev.type = &iio_dev_type;
+		dev->dev.type = &iio_device_type;
 		dev->dev.bus = &iio_bus_type;
 		device_initialize(&dev->dev);
 		dev_set_drvdata(&dev->dev, (void *)dev);
@@ -960,6 +973,10 @@ int iio_device_register(struct iio_dev *indio_dev)
 {
 	int ret;
 
+	/* If the calling driver did not initialize of_node, do it here */
+	if (!indio_dev->dev.of_node && indio_dev->dev.parent)
+		indio_dev->dev.of_node = indio_dev->dev.parent->of_node;
+
 	/* configure elements for the chrdev */
 	indio_dev->dev.devt = MKDEV(MAJOR(iio_devt), indio_dev->id);
 
diff --git a/drivers/iio/industrialio-event.c b/drivers/iio/industrialio-event.c
index 261cae00557..10aa9ef86ce 100644
--- a/drivers/iio/industrialio-event.c
+++ b/drivers/iio/industrialio-event.c
@@ -46,10 +46,11 @@ int iio_push_event(struct iio_dev *indio_dev, u64 ev_code, s64 timestamp)
 {
 	struct iio_event_interface *ev_int = indio_dev->event_interface;
 	struct iio_event_data ev;
+	unsigned long flags;
 	int copied;
 
 	/* Does anyone care? */
-	spin_lock(&ev_int->wait.lock);
+	spin_lock_irqsave(&ev_int->wait.lock, flags);
 	if (test_bit(IIO_BUSY_BIT_POS, &ev_int->flags)) {
 
 		ev.id = ev_code;
@@ -59,7 +60,7 @@ int iio_push_event(struct iio_dev *indio_dev, u64 ev_code, s64 timestamp)
 		if (copied != 0)
 			wake_up_locked_poll(&ev_int->wait, POLLIN);
 	}
-	spin_unlock(&ev_int->wait.lock);
+	spin_unlock_irqrestore(&ev_int->wait.lock, flags);
 
 	return 0;
 }
@@ -76,10 +77,10 @@ static unsigned int iio_event_poll(struct file *filep,
 
 	poll_wait(filep, &ev_int->wait, wait);
 
-	spin_lock(&ev_int->wait.lock);
+	spin_lock_irq(&ev_int->wait.lock);
 	if (!kfifo_is_empty(&ev_int->det_events))
 		events = POLLIN | POLLRDNORM;
-	spin_unlock(&ev_int->wait.lock);
+	spin_unlock_irq(&ev_int->wait.lock);
 
 	return events;
 }
@@ -96,14 +97,14 @@ static ssize_t iio_event_chrdev_read(struct file *filep,
 	if (count < sizeof(struct iio_event_data))
 		return -EINVAL;
 
-	spin_lock(&ev_int->wait.lock);
+	spin_lock_irq(&ev_int->wait.lock);
 	if (kfifo_is_empty(&ev_int->det_events)) {
 		if (filep->f_flags & O_NONBLOCK) {
 			ret = -EAGAIN;
 			goto error_unlock;
 		}
 		/* Blocking on device; waiting for something to be there */
-		ret = wait_event_interruptible_locked(ev_int->wait,
+		ret = wait_event_interruptible_locked_irq(ev_int->wait,
 					!kfifo_is_empty(&ev_int->det_events));
 		if (ret)
 			goto error_unlock;
@@ -113,7 +114,7 @@ static ssize_t iio_event_chrdev_read(struct file *filep,
 	ret = kfifo_to_user(&ev_int->det_events, buf, count, &copied);
 
 error_unlock:
-	spin_unlock(&ev_int->wait.lock);
+	spin_unlock_irq(&ev_int->wait.lock);
 
 	return ret ? ret : copied;
 }
@@ -122,7 +123,7 @@ static int iio_event_chrdev_release(struct inode *inode, struct file *filep)
 {
 	struct iio_event_interface *ev_int = filep->private_data;
 
-	spin_lock(&ev_int->wait.lock);
+	spin_lock_irq(&ev_int->wait.lock);
 	__clear_bit(IIO_BUSY_BIT_POS, &ev_int->flags);
 	/*
 	 * In order to maintain a clean state for reopening,
@@ -130,7 +131,7 @@ static int iio_event_chrdev_release(struct inode *inode, struct file *filep)
 	 * any new __iio_push_event calls running.
 	 */
 	kfifo_reset_out(&ev_int->det_events);
-	spin_unlock(&ev_int->wait.lock);
+	spin_unlock_irq(&ev_int->wait.lock);
 
 	return 0;
 }
@@ -151,18 +152,18 @@ int iio_event_getfd(struct iio_dev *indio_dev)
 	if (ev_int == NULL)
 		return -ENODEV;
 
-	spin_lock(&ev_int->wait.lock);
+	spin_lock_irq(&ev_int->wait.lock);
 	if (__test_and_set_bit(IIO_BUSY_BIT_POS, &ev_int->flags)) {
-		spin_unlock(&ev_int->wait.lock);
+		spin_unlock_irq(&ev_int->wait.lock);
 		return -EBUSY;
 	}
-	spin_unlock(&ev_int->wait.lock);
+	spin_unlock_irq(&ev_int->wait.lock);
 	fd = anon_inode_getfd("iio:event",
 				&iio_event_chrdev_fileops, ev_int, O_RDONLY);
 	if (fd < 0) {
-		spin_lock(&ev_int->wait.lock);
+		spin_lock_irq(&ev_int->wait.lock);
 		__clear_bit(IIO_BUSY_BIT_POS, &ev_int->flags);
-		spin_unlock(&ev_int->wait.lock);
+		spin_unlock_irq(&ev_int->wait.lock);
 	}
 	return fd;
 }
diff --git a/drivers/iio/inkern.c b/drivers/iio/inkern.c
index b289915b846..795d100b4c3 100644
--- a/drivers/iio/inkern.c
+++ b/drivers/iio/inkern.c
@@ -10,6 +10,7 @@
 #include <linux/export.h>
 #include <linux/slab.h>
 #include <linux/mutex.h>
+#include <linux/of.h>
 
 #include <linux/iio/iio.h>
 #include "iio_core.h"
@@ -92,6 +93,164 @@ static const struct iio_chan_spec
 	return chan;
 }
 
+#ifdef CONFIG_OF
+
+static int iio_dev_node_match(struct device *dev, void *data)
+{
+	return dev->of_node == data && dev->type == &iio_device_type;
+}
+
+static int __of_iio_channel_get(struct iio_channel *channel,
+				struct device_node *np, int index)
+{
+	struct device *idev;
+	struct iio_dev *indio_dev;
+	int err;
+	struct of_phandle_args iiospec;
+
+	err = of_parse_phandle_with_args(np, "io-channels",
+					 "#io-channel-cells",
+					 index, &iiospec);
+	if (err)
+		return err;
+
+	idev = bus_find_device(&iio_bus_type, NULL, iiospec.np,
+			       iio_dev_node_match);
+	of_node_put(iiospec.np);
+	if (idev == NULL)
+		return -EPROBE_DEFER;
+
+	indio_dev = dev_to_iio_dev(idev);
+	channel->indio_dev = indio_dev;
+	index = iiospec.args_count ? iiospec.args[0] : 0;
+	if (index >= indio_dev->num_channels) {
+		return -EINVAL;
+		goto err_put;
+	}
+	channel->channel = &indio_dev->channels[index];
+
+	return 0;
+
+err_put:
+	iio_device_put(indio_dev);
+	return err;
+}
+
+static struct iio_channel *of_iio_channel_get(struct device_node *np, int index)
+{
+	struct iio_channel *channel;
+	int err;
+
+	if (index < 0)
+		return ERR_PTR(-EINVAL);
+
+	channel = kzalloc(sizeof(*channel), GFP_KERNEL);
+	if (channel == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	err = __of_iio_channel_get(channel, np, index);
+	if (err)
+		goto err_free_channel;
+
+	return channel;
+
+err_free_channel:
+	kfree(channel);
+	return ERR_PTR(err);
+}
+
+static struct iio_channel *of_iio_channel_get_by_name(struct device_node *np,
+						      const char *name)
+{
+	struct iio_channel *chan = NULL;
+
+	/* Walk up the tree of devices looking for a matching iio channel */
+	while (np) {
+		int index = 0;
+
+		/*
+		 * For named iio channels, first look up the name in the
+		 * "io-channel-names" property.  If it cannot be found, the
+		 * index will be an error code, and of_iio_channel_get()
+		 * will fail.
+		 */
+		if (name)
+			index = of_property_match_string(np, "io-channel-names",
+							 name);
+		chan = of_iio_channel_get(np, index);
+		if (!IS_ERR(chan))
+			break;
+		else if (name && index >= 0) {
+			pr_err("ERROR: could not get IIO channel %s:%s(%i)\n",
+				np->full_name, name ? name : "", index);
+			return chan;
+		}
+
+		/*
+		 * No matching IIO channel found on this node.
+		 * If the parent node has a "io-channel-ranges" property,
+		 * then we can try one of its channels.
+		 */
+		np = np->parent;
+		if (np && !of_get_property(np, "io-channel-ranges", NULL))
+			break;
+	}
+	return chan;
+}
+
+static struct iio_channel *of_iio_channel_get_all(struct device *dev)
+{
+	struct iio_channel *chans;
+	int i, mapind, nummaps = 0;
+	int ret;
+
+	do {
+		ret = of_parse_phandle_with_args(dev->of_node,
+						 "io-channels",
+						 "#io-channel-cells",
+						 nummaps, NULL);
+		if (ret < 0)
+			break;
+	} while (++nummaps);
+
+	if (nummaps == 0)	/* no error, return NULL to search map table */
+		return NULL;
+
+	/* NULL terminated array to save passing size */
+	chans = kcalloc(nummaps + 1, sizeof(*chans), GFP_KERNEL);
+	if (chans == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	/* Search for OF matches */
+	for (mapind = 0; mapind < nummaps; mapind++) {
+		ret = __of_iio_channel_get(&chans[mapind], dev->of_node,
+					   mapind);
+		if (ret)
+			goto error_free_chans;
+	}
+	return chans;
+
+error_free_chans:
+	for (i = 0; i < mapind; i++)
+		iio_device_put(chans[i].indio_dev);
+	kfree(chans);
+	return ERR_PTR(ret);
+}
+
+#else /* CONFIG_OF */
+
+static inline struct iio_channel *
+of_iio_channel_get_by_name(struct device_node *np, const char *name)
+{
+	return NULL;
+}
+
+static inline struct iio_channel *of_iio_channel_get_all(struct device *dev)
+{
+	return NULL;
+}
+
+#endif /* CONFIG_OF */
 
 static struct iio_channel *iio_channel_get_sys(const char *name,
 					       const char *channel_name)
@@ -150,7 +309,14 @@ struct iio_channel *iio_channel_get(struct device *dev,
 				    const char *channel_name)
 {
 	const char *name = dev ? dev_name(dev) : NULL;
+	struct iio_channel *channel;
 
+	if (dev) {
+		channel = of_iio_channel_get_by_name(dev->of_node,
+						     channel_name);
+		if (channel != NULL)
+			return channel;
+	}
 	return iio_channel_get_sys(name, channel_name);
 }
 EXPORT_SYMBOL_GPL(iio_channel_get);
@@ -173,6 +339,11 @@ struct iio_channel *iio_channel_get_all(struct device *dev)
 
 	if (dev == NULL)
 		return ERR_PTR(-EINVAL);
+
+	chans = of_iio_channel_get_all(dev);
+	if (chans)
+		return chans;
+
 	name = dev_name(dev);
 
 	mutex_lock(&iio_map_list_lock);
diff --git a/drivers/iio/light/adjd_s311.c b/drivers/iio/light/adjd_s311.c
index d5b9d39d95b..5f4749e60b0 100644
--- a/drivers/iio/light/adjd_s311.c
+++ b/drivers/iio/light/adjd_s311.c
@@ -207,8 +207,8 @@ static const struct iio_chan_spec_ext_info adjd_s311_ext_info[] = {
 	.type = IIO_INTENSITY, \
 	.modified = 1, \
 	.address = (IDX_##_color), \
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
-		IIO_CHAN_INFO_HARDWAREGAIN_SEPARATE_BIT, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+		BIT(IIO_CHAN_INFO_HARDWAREGAIN), \
 	.channel2 = (IIO_MOD_LIGHT_##_color), \
 	.scan_index = (_scan_idx), \
 	.scan_type = IIO_ST('u', 10, 16, 0), \
diff --git a/drivers/iio/light/hid-sensor-als.c b/drivers/iio/light/hid-sensor-als.c
index 3d7e8c9b4be..80d68ff02d2 100644
--- a/drivers/iio/light/hid-sensor-als.c
+++ b/drivers/iio/light/hid-sensor-als.c
@@ -49,10 +49,10 @@ static const struct iio_chan_spec als_channels[] = {
 		.type = IIO_INTENSITY,
 		.modified = 1,
 		.channel2 = IIO_MOD_LIGHT_BOTH,
-		.info_mask = IIO_CHAN_INFO_OFFSET_SHARED_BIT |
-		IIO_CHAN_INFO_SCALE_SHARED_BIT |
-		IIO_CHAN_INFO_SAMP_FREQ_SHARED_BIT |
-		IIO_CHAN_INFO_HYSTERESIS_SHARED_BIT,
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
+		BIT(IIO_CHAN_INFO_SCALE) |
+		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+		BIT(IIO_CHAN_INFO_HYSTERESIS),
 		.scan_index = CHANNEL_SCAN_INDEX_ILLUM,
 	}
 };
diff --git a/drivers/iio/light/lm3533-als.c b/drivers/iio/light/lm3533-als.c
index 7503012ce93..5fa31a4ef82 100644
--- a/drivers/iio/light/lm3533-als.c
+++ b/drivers/iio/light/lm3533-als.c
@@ -231,7 +231,7 @@ static int lm3533_als_read_raw(struct iio_dev *indio_dev,
 		.channel	= _channel,				\
 		.indexed	= true,					\
 		.output		= true,					\
-		.info_mask	= IIO_CHAN_INFO_RAW_SEPARATE_BIT,	\
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
 	}
 
 static const struct iio_chan_spec lm3533_als_channels[] = {
@@ -239,8 +239,8 @@ static const struct iio_chan_spec lm3533_als_channels[] = {
 		.type		= IIO_LIGHT,
 		.channel	= 0,
 		.indexed	= true,
-		.info_mask	= (IIO_CHAN_INFO_AVERAGE_RAW_SEPARATE_BIT |
-				   IIO_CHAN_INFO_RAW_SEPARATE_BIT),
+		.info_mask_separate = BIT(IIO_CHAN_INFO_AVERAGE_RAW) |
+				   BIT(IIO_CHAN_INFO_RAW),
 	},
 	CHANNEL_CURRENT(0),
 	CHANNEL_CURRENT(1),
diff --git a/drivers/iio/light/tsl2563.c b/drivers/iio/light/tsl2563.c
index fd8be69b7d0..1f529f36f13 100644
--- a/drivers/iio/light/tsl2563.c
+++ b/drivers/iio/light/tsl2563.c
@@ -530,14 +530,14 @@ static const struct iio_chan_spec tsl2563_channels[] = {
 	{
 		.type = IIO_LIGHT,
 		.indexed = 1,
-		.info_mask = IIO_CHAN_INFO_PROCESSED_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED),
 		.channel = 0,
 	}, {
 		.type = IIO_INTENSITY,
 		.modified = 1,
 		.channel2 = IIO_MOD_LIGHT_BOTH,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+		BIT(IIO_CHAN_INFO_CALIBSCALE),
 		.event_mask = (IIO_EV_BIT(IIO_EV_TYPE_THRESH,
 					  IIO_EV_DIR_RISING) |
 			       IIO_EV_BIT(IIO_EV_TYPE_THRESH,
@@ -546,8 +546,8 @@ static const struct iio_chan_spec tsl2563_channels[] = {
 		.type = IIO_INTENSITY,
 		.modified = 1,
 		.channel2 = IIO_MOD_LIGHT_IR,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+		BIT(IIO_CHAN_INFO_CALIBSCALE),
 	}
 };
 
diff --git a/drivers/iio/light/vcnl4000.c b/drivers/iio/light/vcnl4000.c
index 2aa748fbdc0..1014943d949 100644
--- a/drivers/iio/light/vcnl4000.c
+++ b/drivers/iio/light/vcnl4000.c
@@ -93,11 +93,11 @@ static int vcnl4000_measure(struct vcnl4000_data *data, u8 req_mask,
 static const struct iio_chan_spec vcnl4000_channels[] = {
 	{
 		.type = IIO_LIGHT,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_SCALE),
 	}, {
 		.type = IIO_PROXIMITY,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 	}
 };
 
diff --git a/drivers/iio/magnetometer/Kconfig b/drivers/iio/magnetometer/Kconfig
index cd29be54f64..bd1cfb66669 100644
--- a/drivers/iio/magnetometer/Kconfig
+++ b/drivers/iio/magnetometer/Kconfig
@@ -3,6 +3,17 @@
 #
 menu "Magnetometer sensors"
 
+config AK8975
+	tristate "Asahi Kasei AK8975 3-Axis Magnetometer"
+	depends on I2C
+	depends on GPIOLIB
+	help
+	  Say yes here to build support for Asahi Kasei AK8975 3-Axis
+	  Magnetometer.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called ak8975.
+
 config HID_SENSOR_MAGNETOMETER_3D
 	depends on HID_SENSOR_HUB
 	select IIO_BUFFER
diff --git a/drivers/iio/magnetometer/Makefile b/drivers/iio/magnetometer/Makefile
index e78672876dc..7f328e37fba 100644
--- a/drivers/iio/magnetometer/Makefile
+++ b/drivers/iio/magnetometer/Makefile
@@ -2,6 +2,7 @@
 # Makefile for industrial I/O Magnetometer sensor drivers
 #
 
+obj-$(CONFIG_AK8975)	+= ak8975.o
 obj-$(CONFIG_HID_SENSOR_MAGNETOMETER_3D) += hid-sensor-magn-3d.o
 
 obj-$(CONFIG_IIO_ST_MAGN_3AXIS) += st_magn.o
diff --git a/drivers/staging/iio/magnetometer/ak8975.c b/drivers/iio/magnetometer/ak8975.c
index 28f080e9eee..af6c320a534 100644
--- a/drivers/staging/iio/magnetometer/ak8975.c
+++ b/drivers/iio/magnetometer/ak8975.c
@@ -94,7 +94,6 @@ struct ak8975_data {
 	long			raw_to_gauss[3];
 	u8			reg_cache[AK8975_MAX_REGS];
 	int			eoc_gpio;
-	int			eoc_irq;
 };
 
 static const int ak8975_index_to_reg[] = {
@@ -124,36 +123,6 @@ static int ak8975_write_data(struct i2c_client *client,
 }
 
 /*
- * Helper function to read a contiguous set of the I2C device's registers.
- */
-static int ak8975_read_data(struct i2c_client *client,
-			    u8 reg, u8 length, u8 *buffer)
-{
-	int ret;
-	struct i2c_msg msg[2] = {
-		{
-			.addr = client->addr,
-			.flags = I2C_M_NOSTART,
-			.len = 1,
-			.buf = &reg,
-		}, {
-			.addr = client->addr,
-			.flags = I2C_M_RD,
-			.len = length,
-			.buf = buffer,
-		}
-	};
-
-	ret = i2c_transfer(client->adapter, msg, 2);
-	if (ret < 0) {
-		dev_err(&client->dev, "Read from device fails\n");
-		return ret;
-	}
-
-	return 0;
-}
-
-/*
  * Perform some start-of-day setup, including reading the asa calibration
  * values and caching them.
  */
@@ -165,11 +134,12 @@ static int ak8975_setup(struct i2c_client *client)
 	int ret;
 
 	/* Confirm that the device we're talking to is really an AK8975. */
-	ret = ak8975_read_data(client, AK8975_REG_WIA, 1, &device_id);
+	ret = i2c_smbus_read_byte_data(client, AK8975_REG_WIA);
 	if (ret < 0) {
 		dev_err(&client->dev, "Error reading WIA\n");
 		return ret;
 	}
+	device_id = ret;
 	if (device_id != AK8975_DEVICE_ID) {
 		dev_err(&client->dev, "Device ak8975 not found\n");
 		return -ENODEV;
@@ -187,7 +157,8 @@ static int ak8975_setup(struct i2c_client *client)
 	}
 
 	/* Get asa data and store in the device data. */
-	ret = ak8975_read_data(client, AK8975_REG_ASAX, 3, data->asa);
+	ret = i2c_smbus_read_i2c_block_data(client, AK8975_REG_ASAX,
+					    3, data->asa);
 	if (ret < 0) {
 		dev_err(&client->dev, "Not able to read asa data\n");
 		return ret;
@@ -249,7 +220,6 @@ static int ak8975_setup(struct i2c_client *client)
 static int wait_conversion_complete_gpio(struct ak8975_data *data)
 {
 	struct i2c_client *client = data->client;
-	u8 read_status;
 	u32 timeout_ms = AK8975_MAX_CONVERSION_TIMEOUT;
 	int ret;
 
@@ -265,12 +235,11 @@ static int wait_conversion_complete_gpio(struct ak8975_data *data)
 		return -EINVAL;
 	}
 
-	ret = ak8975_read_data(client, AK8975_REG_ST1, 1, &read_status);
-	if (ret < 0) {
+	ret = i2c_smbus_read_byte_data(client, AK8975_REG_ST1);
+	if (ret < 0)
 		dev_err(&client->dev, "Error in reading ST1\n");
-		return ret;
-	}
-	return read_status;
+
+	return ret;
 }
 
 static int wait_conversion_complete_polled(struct ak8975_data *data)
@@ -283,11 +252,12 @@ static int wait_conversion_complete_polled(struct ak8975_data *data)
 	/* Wait for the conversion to complete. */
 	while (timeout_ms) {
 		msleep(AK8975_CONVERSION_DONE_POLL_TIME);
-		ret = ak8975_read_data(client, AK8975_REG_ST1, 1, &read_status);
+		ret = i2c_smbus_read_byte_data(client, AK8975_REG_ST1);
 		if (ret < 0) {
 			dev_err(&client->dev, "Error in reading ST1\n");
 			return ret;
 		}
+		read_status = ret;
 		if (read_status)
 			break;
 		timeout_ms -= AK8975_CONVERSION_DONE_POLL_TIME;
@@ -308,7 +278,6 @@ static int ak8975_read_axis(struct iio_dev *indio_dev, int index, int *val)
 	struct i2c_client *client = data->client;
 	u16 meas_reg;
 	s16 raw;
-	u8 read_status;
 	int ret;
 
 	mutex_lock(&data->lock);
@@ -332,18 +301,15 @@ static int ak8975_read_axis(struct iio_dev *indio_dev, int index, int *val)
 	if (ret < 0)
 		goto exit;
 
-	read_status = ret;
-
-	if (read_status & AK8975_REG_ST1_DRDY_MASK) {
-		ret = ak8975_read_data(client, AK8975_REG_ST2, 1, &read_status);
+	if (ret & AK8975_REG_ST1_DRDY_MASK) {
+		ret = i2c_smbus_read_byte_data(client, AK8975_REG_ST2);
 		if (ret < 0) {
 			dev_err(&client->dev, "Error in reading ST2\n");
 			goto exit;
 		}
-		if (read_status & (AK8975_REG_ST2_DERR_MASK |
-				   AK8975_REG_ST2_HOFL_MASK)) {
-			dev_err(&client->dev, "ST2 status error 0x%x\n",
-				read_status);
+		if (ret & (AK8975_REG_ST2_DERR_MASK |
+			   AK8975_REG_ST2_HOFL_MASK)) {
+			dev_err(&client->dev, "ST2 status error 0x%x\n", ret);
 			ret = -EINVAL;
 			goto exit;
 		}
@@ -351,12 +317,12 @@ static int ak8975_read_axis(struct iio_dev *indio_dev, int index, int *val)
 
 	/* Read the flux value from the appropriate register
 	   (the register is specified in the iio device attributes). */
-	ret = ak8975_read_data(client, ak8975_index_to_reg[index],
-			       2, (u8 *)&meas_reg);
+	ret = i2c_smbus_read_word_data(client, ak8975_index_to_reg[index]);
 	if (ret < 0) {
 		dev_err(&client->dev, "Read axis data fails\n");
 		goto exit;
 	}
+	meas_reg = ret;
 
 	mutex_unlock(&data->lock);
 
@@ -395,8 +361,8 @@ static int ak8975_read_raw(struct iio_dev *indio_dev,
 		.type = IIO_MAGN,					\
 		.modified = 1,						\
 		.channel2 = IIO_MOD_##axis,				\
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |		\
-			     IIO_CHAN_INFO_SCALE_SEPARATE_BIT,		\
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |		\
+			     BIT(IIO_CHAN_INFO_SCALE),			\
 		.address = index,					\
 	}
 
@@ -452,7 +418,6 @@ static int ak8975_probe(struct i2c_client *client,
 
 	data->client = client;
 	mutex_init(&data->lock);
-	data->eoc_irq = client->irq;
 	data->eoc_gpio = eoc_gpio;
 	indio_dev->dev.parent = &client->dev;
 	indio_dev->channels = ak8975_channels;
diff --git a/drivers/iio/magnetometer/hid-sensor-magn-3d.c b/drivers/iio/magnetometer/hid-sensor-magn-3d.c
index d8d01265220..99f4e494513 100644
--- a/drivers/iio/magnetometer/hid-sensor-magn-3d.c
+++ b/drivers/iio/magnetometer/hid-sensor-magn-3d.c
@@ -60,28 +60,28 @@ static const struct iio_chan_spec magn_3d_channels[] = {
 		.type = IIO_MAGN,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
-		.info_mask = IIO_CHAN_INFO_OFFSET_SHARED_BIT |
-		IIO_CHAN_INFO_SCALE_SHARED_BIT |
-		IIO_CHAN_INFO_SAMP_FREQ_SHARED_BIT |
-		IIO_CHAN_INFO_HYSTERESIS_SHARED_BIT,
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
+		BIT(IIO_CHAN_INFO_SCALE) |
+		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+		BIT(IIO_CHAN_INFO_HYSTERESIS),
 		.scan_index = CHANNEL_SCAN_INDEX_X,
 	}, {
 		.type = IIO_MAGN,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
-		.info_mask = IIO_CHAN_INFO_OFFSET_SHARED_BIT |
-		IIO_CHAN_INFO_SCALE_SHARED_BIT |
-		IIO_CHAN_INFO_SAMP_FREQ_SHARED_BIT |
-		IIO_CHAN_INFO_HYSTERESIS_SHARED_BIT,
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
+		BIT(IIO_CHAN_INFO_SCALE) |
+		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+		BIT(IIO_CHAN_INFO_HYSTERESIS),
 		.scan_index = CHANNEL_SCAN_INDEX_Y,
 	}, {
 		.type = IIO_MAGN,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
-		.info_mask = IIO_CHAN_INFO_OFFSET_SHARED_BIT |
-		IIO_CHAN_INFO_SCALE_SHARED_BIT |
-		IIO_CHAN_INFO_SAMP_FREQ_SHARED_BIT |
-		IIO_CHAN_INFO_HYSTERESIS_SHARED_BIT,
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
+		BIT(IIO_CHAN_INFO_SCALE) |
+		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+		BIT(IIO_CHAN_INFO_HYSTERESIS),
 		.scan_index = CHANNEL_SCAN_INDEX_Z,
 	}
 };
diff --git a/drivers/infiniband/Kconfig b/drivers/infiniband/Kconfig
index a0f29c1d03b..c85b56c2809 100644
--- a/drivers/infiniband/Kconfig
+++ b/drivers/infiniband/Kconfig
@@ -59,5 +59,6 @@ source "drivers/infiniband/ulp/srp/Kconfig"
 source "drivers/infiniband/ulp/srpt/Kconfig"
 
 source "drivers/infiniband/ulp/iser/Kconfig"
+source "drivers/infiniband/ulp/isert/Kconfig"
 
 endif # INFINIBAND
diff --git a/drivers/infiniband/Makefile b/drivers/infiniband/Makefile
index bf846a14b9d..b126fefe0b1 100644
--- a/drivers/infiniband/Makefile
+++ b/drivers/infiniband/Makefile
@@ -13,3 +13,4 @@ obj-$(CONFIG_INFINIBAND_IPOIB)		+= ulp/ipoib/
 obj-$(CONFIG_INFINIBAND_SRP)		+= ulp/srp/
 obj-$(CONFIG_INFINIBAND_SRPT)		+= ulp/srpt/
 obj-$(CONFIG_INFINIBAND_ISER)		+= ulp/iser/
+obj-$(CONFIG_INFINIBAND_ISERT)		+= ulp/isert/
diff --git a/drivers/infiniband/hw/amso1100/c2.h b/drivers/infiniband/hw/amso1100/c2.h
index ba7a1208ff9..d619d735838 100644
--- a/drivers/infiniband/hw/amso1100/c2.h
+++ b/drivers/infiniband/hw/amso1100/c2.h
@@ -265,7 +265,6 @@ struct c2_pd_table {
 struct c2_qp_table {
 	struct idr idr;
 	spinlock_t lock;
-	int last;
 };
 
 struct c2_element {
diff --git a/drivers/infiniband/hw/amso1100/c2_qp.c b/drivers/infiniband/hw/amso1100/c2_qp.c
index 0ab826b280b..86708dee58b 100644
--- a/drivers/infiniband/hw/amso1100/c2_qp.c
+++ b/drivers/infiniband/hw/amso1100/c2_qp.c
@@ -385,8 +385,7 @@ static int c2_alloc_qpn(struct c2_dev *c2dev, struct c2_qp *qp)
 	idr_preload(GFP_KERNEL);
 	spin_lock_irq(&c2dev->qp_table.lock);
 
-	ret = idr_alloc(&c2dev->qp_table.idr, qp, c2dev->qp_table.last++, 0,
-			GFP_NOWAIT);
+	ret = idr_alloc_cyclic(&c2dev->qp_table.idr, qp, 0, 0, GFP_NOWAIT);
 	if (ret >= 0)
 		qp->qpn = ret;
 
diff --git a/drivers/infiniband/hw/cxgb4/qp.c b/drivers/infiniband/hw/cxgb4/qp.c
index 17ba4f8bc12..70b1808a08f 100644
--- a/drivers/infiniband/hw/cxgb4/qp.c
+++ b/drivers/infiniband/hw/cxgb4/qp.c
@@ -186,8 +186,10 @@ static int create_qp(struct c4iw_rdev *rdev, struct t4_wq *wq,
 	wq->rq.queue = dma_alloc_coherent(&(rdev->lldi.pdev->dev),
 					  wq->rq.memsize, &(wq->rq.dma_addr),
 					  GFP_KERNEL);
-	if (!wq->rq.queue)
+	if (!wq->rq.queue) {
+		ret = -ENOMEM;
 		goto free_sq;
+	}
 	PDBG("%s sq base va 0x%p pa 0x%llx rq base va 0x%p pa 0x%llx\n",
 		__func__, wq->sq.queue,
 		(unsigned long long)virt_to_phys(wq->sq.queue),
diff --git a/drivers/infiniband/hw/ipath/ipath_verbs.c b/drivers/infiniband/hw/ipath/ipath_verbs.c
index 439c35d4a66..ea93870266e 100644
--- a/drivers/infiniband/hw/ipath/ipath_verbs.c
+++ b/drivers/infiniband/hw/ipath/ipath_verbs.c
@@ -620,7 +620,7 @@ void ipath_ib_rcv(struct ipath_ibdev *dev, void *rhdr, void *data,
 		goto bail;
 	}
 
-	opcode = be32_to_cpu(ohdr->bth[0]) >> 24;
+	opcode = (be32_to_cpu(ohdr->bth[0]) >> 24) & 0x7f;
 	dev->opstats[opcode].n_bytes += tlen;
 	dev->opstats[opcode].n_packets++;
 
diff --git a/drivers/infiniband/hw/mlx4/cm.c b/drivers/infiniband/hw/mlx4/cm.c
index add98d01476..d1f5f1dd77b 100644
--- a/drivers/infiniband/hw/mlx4/cm.c
+++ b/drivers/infiniband/hw/mlx4/cm.c
@@ -204,7 +204,6 @@ static struct id_map_entry *
 id_map_alloc(struct ib_device *ibdev, int slave_id, u32 sl_cm_id)
 {
 	int ret;
-	static int next_id;
 	struct id_map_entry *ent;
 	struct mlx4_ib_sriov *sriov = &to_mdev(ibdev)->sriov;
 
@@ -223,9 +222,8 @@ id_map_alloc(struct ib_device *ibdev, int slave_id, u32 sl_cm_id)
 	idr_preload(GFP_KERNEL);
 	spin_lock(&to_mdev(ibdev)->sriov.id_map_lock);
 
-	ret = idr_alloc(&sriov->pv_id_table, ent, next_id, 0, GFP_NOWAIT);
+	ret = idr_alloc_cyclic(&sriov->pv_id_table, ent, 0, 0, GFP_NOWAIT);
 	if (ret >= 0) {
-		next_id = max(ret + 1, 0);
 		ent->pv_cm_id = (u32)ret;
 		sl_id_map_add(ibdev, ent);
 		list_add_tail(&ent->list, &sriov->cm_list);
diff --git a/drivers/infiniband/hw/qib/Kconfig b/drivers/infiniband/hw/qib/Kconfig
index 8349f9c5064..1e603a37506 100644
--- a/drivers/infiniband/hw/qib/Kconfig
+++ b/drivers/infiniband/hw/qib/Kconfig
@@ -1,7 +1,7 @@
 config INFINIBAND_QIB
-	tristate "QLogic PCIe HCA support"
+	tristate "Intel PCIe HCA support"
 	depends on 64BIT
 	---help---
-	This is a low-level driver for QLogic PCIe QLE InfiniBand host
-	channel adapters.  This driver does not support the QLogic
+	This is a low-level driver for Intel PCIe QLE InfiniBand host
+	channel adapters.  This driver does not support the Intel
 	HyperTransport card (model QHT7140).
diff --git a/drivers/infiniband/hw/qib/qib_driver.c b/drivers/infiniband/hw/qib/qib_driver.c
index 5423edcab51..216092477df 100644
--- a/drivers/infiniband/hw/qib/qib_driver.c
+++ b/drivers/infiniband/hw/qib/qib_driver.c
@@ -1,4 +1,5 @@
 /*
+ * Copyright (c) 2013 Intel Corporation. All rights reserved.
  * Copyright (c) 2006, 2007, 2008, 2009 QLogic Corporation. All rights reserved.
  * Copyright (c) 2003, 2004, 2005, 2006 PathScale, Inc. All rights reserved.
  *
@@ -63,8 +64,8 @@ MODULE_PARM_DESC(compat_ddr_negotiate,
 		 "Attempt pre-IBTA 1.2 DDR speed negotiation");
 
 MODULE_LICENSE("Dual BSD/GPL");
-MODULE_AUTHOR("QLogic <support@qlogic.com>");
-MODULE_DESCRIPTION("QLogic IB driver");
+MODULE_AUTHOR("Intel <ibsupport@intel.com>");
+MODULE_DESCRIPTION("Intel IB driver");
 MODULE_VERSION(QIB_DRIVER_VERSION);
 
 /*
diff --git a/drivers/infiniband/hw/qib/qib_iba6120.c b/drivers/infiniband/hw/qib/qib_iba6120.c
index a099ac171e2..0232ae56b1f 100644
--- a/drivers/infiniband/hw/qib/qib_iba6120.c
+++ b/drivers/infiniband/hw/qib/qib_iba6120.c
@@ -1,4 +1,5 @@
 /*
+ * Copyright (c) 2013 Intel Corporation. All rights reserved.
  * Copyright (c) 2006, 2007, 2008, 2009, 2010 QLogic Corporation.
  * All rights reserved.
  * Copyright (c) 2003, 2004, 2005, 2006 PathScale, Inc. All rights reserved.
@@ -51,7 +52,7 @@ static u32 qib_6120_iblink_state(u64);
 
 /*
  * This file contains all the chip-specific register information and
- * access functions for the QLogic QLogic_IB PCI-Express chip.
+ * access functions for the Intel Intel_IB PCI-Express chip.
  *
  */
 
diff --git a/drivers/infiniband/hw/qib/qib_init.c b/drivers/infiniband/hw/qib/qib_init.c
index 50e33aa0b4e..173f805790d 100644
--- a/drivers/infiniband/hw/qib/qib_init.c
+++ b/drivers/infiniband/hw/qib/qib_init.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012 Intel Corporation.  All rights reserved.
+ * Copyright (c) 2012, 2013 Intel Corporation.  All rights reserved.
  * Copyright (c) 2006 - 2012 QLogic Corporation. All rights reserved.
  * Copyright (c) 2003, 2004, 2005, 2006 PathScale, Inc. All rights reserved.
  *
@@ -1138,7 +1138,7 @@ void qib_disable_after_error(struct qib_devdata *dd)
 static void qib_remove_one(struct pci_dev *);
 static int qib_init_one(struct pci_dev *, const struct pci_device_id *);
 
-#define DRIVER_LOAD_MSG "QLogic " QIB_DRV_NAME " loaded: "
+#define DRIVER_LOAD_MSG "Intel " QIB_DRV_NAME " loaded: "
 #define PFX QIB_DRV_NAME ": "
 
 static DEFINE_PCI_DEVICE_TABLE(qib_pci_tbl) = {
@@ -1355,7 +1355,7 @@ static int qib_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		dd = qib_init_iba6120_funcs(pdev, ent);
 #else
 		qib_early_err(&pdev->dev,
-			"QLogic PCIE device 0x%x cannot work if CONFIG_PCI_MSI is not enabled\n",
+			"Intel PCIE device 0x%x cannot work if CONFIG_PCI_MSI is not enabled\n",
 			ent->device);
 		dd = ERR_PTR(-ENODEV);
 #endif
@@ -1371,7 +1371,7 @@ static int qib_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	default:
 		qib_early_err(&pdev->dev,
-			"Failing on unknown QLogic deviceid 0x%x\n",
+			"Failing on unknown Intel deviceid 0x%x\n",
 			ent->device);
 		ret = -ENODEV;
 	}
diff --git a/drivers/infiniband/hw/qib/qib_sd7220.c b/drivers/infiniband/hw/qib/qib_sd7220.c
index 50a8a0d4fe6..911205d3d5a 100644
--- a/drivers/infiniband/hw/qib/qib_sd7220.c
+++ b/drivers/infiniband/hw/qib/qib_sd7220.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012 Intel Corporation. All rights reserved.
+ * Copyright (c) 2013 Intel Corporation. All rights reserved.
  * Copyright (c) 2006 - 2012 QLogic Corporation. All rights reserved.
  * Copyright (c) 2003, 2004, 2005, 2006 PathScale, Inc. All rights reserved.
  *
diff --git a/drivers/infiniband/hw/qib/qib_verbs.c b/drivers/infiniband/hw/qib/qib_verbs.c
index ba51a4715a1..7c0ab16a2fe 100644
--- a/drivers/infiniband/hw/qib/qib_verbs.c
+++ b/drivers/infiniband/hw/qib/qib_verbs.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012 Intel Corporation.  All rights reserved.
+ * Copyright (c) 2012, 2013 Intel Corporation.  All rights reserved.
  * Copyright (c) 2006 - 2012 QLogic Corporation. All rights reserved.
  * Copyright (c) 2005, 2006 PathScale, Inc. All rights reserved.
  *
@@ -2224,7 +2224,7 @@ int qib_register_ib_device(struct qib_devdata *dd)
 	ibdev->dma_ops = &qib_dma_mapping_ops;
 
 	snprintf(ibdev->node_desc, sizeof(ibdev->node_desc),
-		 "QLogic Infiniband HCA %s", init_utsname()->nodename);
+		 "Intel Infiniband HCA %s", init_utsname()->nodename);
 
 	ret = ib_register_device(ibdev, qib_create_port_files);
 	if (ret)
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_cm.c b/drivers/infiniband/ulp/ipoib/ipoib_cm.c
index 67b0c1d2367..1ef880de3a4 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_cm.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_cm.c
@@ -758,9 +758,13 @@ void ipoib_cm_send(struct net_device *dev, struct sk_buff *skb, struct ipoib_cm_
 		if (++priv->tx_outstanding == ipoib_sendq_size) {
 			ipoib_dbg(priv, "TX ring 0x%x full, stopping kernel net queue\n",
 				  tx->qp->qp_num);
-			if (ib_req_notify_cq(priv->send_cq, IB_CQ_NEXT_COMP))
-				ipoib_warn(priv, "request notify on send CQ failed\n");
 			netif_stop_queue(dev);
+			rc = ib_req_notify_cq(priv->send_cq,
+				IB_CQ_NEXT_COMP | IB_CQ_REPORT_MISSED_EVENTS);
+			if (rc < 0)
+				ipoib_warn(priv, "request notify on send CQ failed\n");
+			else if (rc)
+				ipoib_send_comp_handler(priv->send_cq, dev);
 		}
 	}
 }
diff --git a/drivers/infiniband/ulp/isert/Kconfig b/drivers/infiniband/ulp/isert/Kconfig
new file mode 100644
index 00000000000..ce3fd32167d
--- /dev/null
+++ b/drivers/infiniband/ulp/isert/Kconfig
@@ -0,0 +1,5 @@
+config INFINIBAND_ISERT
+	tristate "iSCSI Extentions for RDMA (iSER) target support"
+	depends on INET && INFINIBAND_ADDR_TRANS && TARGET_CORE && ISCSI_TARGET
+	---help---
+	Support for iSCSI Extentions for RDMA (iSER) Target on Infiniband fabrics.
diff --git a/drivers/infiniband/ulp/isert/Makefile b/drivers/infiniband/ulp/isert/Makefile
new file mode 100644
index 00000000000..c8bf2421f5b
--- /dev/null
+++ b/drivers/infiniband/ulp/isert/Makefile
@@ -0,0 +1,2 @@
+ccflags-y		:= -Idrivers/target -Idrivers/target/iscsi
+obj-$(CONFIG_INFINIBAND_ISERT)	+= ib_isert.o
diff --git a/drivers/infiniband/ulp/isert/ib_isert.c b/drivers/infiniband/ulp/isert/ib_isert.c
new file mode 100644
index 00000000000..41712f09651
--- /dev/null
+++ b/drivers/infiniband/ulp/isert/ib_isert.c
@@ -0,0 +1,2281 @@
+/*******************************************************************************
+ * This file contains iSCSI extentions for RDMA (iSER) Verbs
+ *
+ * (c) Copyright 2013 RisingTide Systems LLC.
+ *
+ * Nicholas A. Bellinger <nab@linux-iscsi.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.
+ ****************************************************************************/
+
+#include <linux/string.h>
+#include <linux/module.h>
+#include <linux/scatterlist.h>
+#include <linux/socket.h>
+#include <linux/in.h>
+#include <linux/in6.h>
+#include <rdma/ib_verbs.h>
+#include <rdma/rdma_cm.h>
+#include <target/target_core_base.h>
+#include <target/target_core_fabric.h>
+#include <target/iscsi/iscsi_transport.h>
+
+#include "isert_proto.h"
+#include "ib_isert.h"
+
+#define	ISERT_MAX_CONN		8
+#define ISER_MAX_RX_CQ_LEN	(ISERT_QP_MAX_RECV_DTOS * ISERT_MAX_CONN)
+#define ISER_MAX_TX_CQ_LEN	(ISERT_QP_MAX_REQ_DTOS  * ISERT_MAX_CONN)
+
+static DEFINE_MUTEX(device_list_mutex);
+static LIST_HEAD(device_list);
+static struct workqueue_struct *isert_rx_wq;
+static struct workqueue_struct *isert_comp_wq;
+static struct kmem_cache *isert_cmd_cache;
+
+static void
+isert_qp_event_callback(struct ib_event *e, void *context)
+{
+	struct isert_conn *isert_conn = (struct isert_conn *)context;
+
+	pr_err("isert_qp_event_callback event: %d\n", e->event);
+	switch (e->event) {
+	case IB_EVENT_COMM_EST:
+		rdma_notify(isert_conn->conn_cm_id, IB_EVENT_COMM_EST);
+		break;
+	case IB_EVENT_QP_LAST_WQE_REACHED:
+		pr_warn("Reached TX IB_EVENT_QP_LAST_WQE_REACHED:\n");
+		break;
+	default:
+		break;
+	}
+}
+
+static int
+isert_query_device(struct ib_device *ib_dev, struct ib_device_attr *devattr)
+{
+	int ret;
+
+	ret = ib_query_device(ib_dev, devattr);
+	if (ret) {
+		pr_err("ib_query_device() failed: %d\n", ret);
+		return ret;
+	}
+	pr_debug("devattr->max_sge: %d\n", devattr->max_sge);
+	pr_debug("devattr->max_sge_rd: %d\n", devattr->max_sge_rd);
+
+	return 0;
+}
+
+static int
+isert_conn_setup_qp(struct isert_conn *isert_conn, struct rdma_cm_id *cma_id)
+{
+	struct isert_device *device = isert_conn->conn_device;
+	struct ib_qp_init_attr attr;
+	struct ib_device_attr devattr;
+	int ret, index, min_index = 0;
+
+	memset(&devattr, 0, sizeof(struct ib_device_attr));
+	ret = isert_query_device(cma_id->device, &devattr);
+	if (ret)
+		return ret;
+
+	mutex_lock(&device_list_mutex);
+	for (index = 0; index < device->cqs_used; index++)
+		if (device->cq_active_qps[index] <
+		    device->cq_active_qps[min_index])
+			min_index = index;
+	device->cq_active_qps[min_index]++;
+	pr_debug("isert_conn_setup_qp: Using min_index: %d\n", min_index);
+	mutex_unlock(&device_list_mutex);
+
+	memset(&attr, 0, sizeof(struct ib_qp_init_attr));
+	attr.event_handler = isert_qp_event_callback;
+	attr.qp_context = isert_conn;
+	attr.send_cq = device->dev_tx_cq[min_index];
+	attr.recv_cq = device->dev_rx_cq[min_index];
+	attr.cap.max_send_wr = ISERT_QP_MAX_REQ_DTOS;
+	attr.cap.max_recv_wr = ISERT_QP_MAX_RECV_DTOS;
+	/*
+	 * FIXME: Use devattr.max_sge - 2 for max_send_sge as
+	 * work-around for RDMA_READ..
+	 */
+	attr.cap.max_send_sge = devattr.max_sge - 2;
+	isert_conn->max_sge = attr.cap.max_send_sge;
+
+	attr.cap.max_recv_sge = 1;
+	attr.sq_sig_type = IB_SIGNAL_REQ_WR;
+	attr.qp_type = IB_QPT_RC;
+
+	pr_debug("isert_conn_setup_qp cma_id->device: %p\n",
+		 cma_id->device);
+	pr_debug("isert_conn_setup_qp conn_pd->device: %p\n",
+		 isert_conn->conn_pd->device);
+
+	ret = rdma_create_qp(cma_id, isert_conn->conn_pd, &attr);
+	if (ret) {
+		pr_err("rdma_create_qp failed for cma_id %d\n", ret);
+		return ret;
+	}
+	isert_conn->conn_qp = cma_id->qp;
+	pr_debug("rdma_create_qp() returned success >>>>>>>>>>>>>>>>>>>>>>>>>.\n");
+
+	return 0;
+}
+
+static void
+isert_cq_event_callback(struct ib_event *e, void *context)
+{
+	pr_debug("isert_cq_event_callback event: %d\n", e->event);
+}
+
+static int
+isert_alloc_rx_descriptors(struct isert_conn *isert_conn)
+{
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+	struct iser_rx_desc *rx_desc;
+	struct ib_sge *rx_sg;
+	u64 dma_addr;
+	int i, j;
+
+	isert_conn->conn_rx_descs = kzalloc(ISERT_QP_MAX_RECV_DTOS *
+				sizeof(struct iser_rx_desc), GFP_KERNEL);
+	if (!isert_conn->conn_rx_descs)
+		goto fail;
+
+	rx_desc = isert_conn->conn_rx_descs;
+
+	for (i = 0; i < ISERT_QP_MAX_RECV_DTOS; i++, rx_desc++)  {
+		dma_addr = ib_dma_map_single(ib_dev, (void *)rx_desc,
+					ISER_RX_PAYLOAD_SIZE, DMA_FROM_DEVICE);
+		if (ib_dma_mapping_error(ib_dev, dma_addr))
+			goto dma_map_fail;
+
+		rx_desc->dma_addr = dma_addr;
+
+		rx_sg = &rx_desc->rx_sg;
+		rx_sg->addr = rx_desc->dma_addr;
+		rx_sg->length = ISER_RX_PAYLOAD_SIZE;
+		rx_sg->lkey = isert_conn->conn_mr->lkey;
+	}
+
+	isert_conn->conn_rx_desc_head = 0;
+	return 0;
+
+dma_map_fail:
+	rx_desc = isert_conn->conn_rx_descs;
+	for (j = 0; j < i; j++, rx_desc++) {
+		ib_dma_unmap_single(ib_dev, rx_desc->dma_addr,
+				    ISER_RX_PAYLOAD_SIZE, DMA_FROM_DEVICE);
+	}
+	kfree(isert_conn->conn_rx_descs);
+	isert_conn->conn_rx_descs = NULL;
+fail:
+	return -ENOMEM;
+}
+
+static void
+isert_free_rx_descriptors(struct isert_conn *isert_conn)
+{
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+	struct iser_rx_desc *rx_desc;
+	int i;
+
+	if (!isert_conn->conn_rx_descs)
+		return;
+
+	rx_desc = isert_conn->conn_rx_descs;
+	for (i = 0; i < ISERT_QP_MAX_RECV_DTOS; i++, rx_desc++)  {
+		ib_dma_unmap_single(ib_dev, rx_desc->dma_addr,
+				    ISER_RX_PAYLOAD_SIZE, DMA_FROM_DEVICE);
+	}
+
+	kfree(isert_conn->conn_rx_descs);
+	isert_conn->conn_rx_descs = NULL;
+}
+
+static void isert_cq_tx_callback(struct ib_cq *, void *);
+static void isert_cq_rx_callback(struct ib_cq *, void *);
+
+static int
+isert_create_device_ib_res(struct isert_device *device)
+{
+	struct ib_device *ib_dev = device->ib_device;
+	struct isert_cq_desc *cq_desc;
+	int ret = 0, i, j;
+
+	device->cqs_used = min_t(int, num_online_cpus(),
+				 device->ib_device->num_comp_vectors);
+	device->cqs_used = min(ISERT_MAX_CQ, device->cqs_used);
+	pr_debug("Using %d CQs, device %s supports %d vectors\n",
+		 device->cqs_used, device->ib_device->name,
+		 device->ib_device->num_comp_vectors);
+	device->cq_desc = kzalloc(sizeof(struct isert_cq_desc) *
+				device->cqs_used, GFP_KERNEL);
+	if (!device->cq_desc) {
+		pr_err("Unable to allocate device->cq_desc\n");
+		return -ENOMEM;
+	}
+	cq_desc = device->cq_desc;
+
+	device->dev_pd = ib_alloc_pd(ib_dev);
+	if (IS_ERR(device->dev_pd)) {
+		ret = PTR_ERR(device->dev_pd);
+		pr_err("ib_alloc_pd failed for dev_pd: %d\n", ret);
+		goto out_cq_desc;
+	}
+
+	for (i = 0; i < device->cqs_used; i++) {
+		cq_desc[i].device = device;
+		cq_desc[i].cq_index = i;
+
+		device->dev_rx_cq[i] = ib_create_cq(device->ib_device,
+						isert_cq_rx_callback,
+						isert_cq_event_callback,
+						(void *)&cq_desc[i],
+						ISER_MAX_RX_CQ_LEN, i);
+		if (IS_ERR(device->dev_rx_cq[i]))
+			goto out_cq;
+
+		device->dev_tx_cq[i] = ib_create_cq(device->ib_device,
+						isert_cq_tx_callback,
+						isert_cq_event_callback,
+						(void *)&cq_desc[i],
+						ISER_MAX_TX_CQ_LEN, i);
+		if (IS_ERR(device->dev_tx_cq[i]))
+			goto out_cq;
+
+		if (ib_req_notify_cq(device->dev_rx_cq[i], IB_CQ_NEXT_COMP))
+			goto out_cq;
+
+		if (ib_req_notify_cq(device->dev_tx_cq[i], IB_CQ_NEXT_COMP))
+			goto out_cq;
+	}
+
+	device->dev_mr = ib_get_dma_mr(device->dev_pd, IB_ACCESS_LOCAL_WRITE);
+	if (IS_ERR(device->dev_mr)) {
+		ret = PTR_ERR(device->dev_mr);
+		pr_err("ib_get_dma_mr failed for dev_mr: %d\n", ret);
+		goto out_cq;
+	}
+
+	return 0;
+
+out_cq:
+	for (j = 0; j < i; j++) {
+		cq_desc = &device->cq_desc[j];
+
+		if (device->dev_rx_cq[j]) {
+			cancel_work_sync(&cq_desc->cq_rx_work);
+			ib_destroy_cq(device->dev_rx_cq[j]);
+		}
+		if (device->dev_tx_cq[j]) {
+			cancel_work_sync(&cq_desc->cq_tx_work);
+			ib_destroy_cq(device->dev_tx_cq[j]);
+		}
+	}
+	ib_dealloc_pd(device->dev_pd);
+
+out_cq_desc:
+	kfree(device->cq_desc);
+
+	return ret;
+}
+
+static void
+isert_free_device_ib_res(struct isert_device *device)
+{
+	struct isert_cq_desc *cq_desc;
+	int i;
+
+	for (i = 0; i < device->cqs_used; i++) {
+		cq_desc = &device->cq_desc[i];
+
+		cancel_work_sync(&cq_desc->cq_rx_work);
+		cancel_work_sync(&cq_desc->cq_tx_work);
+		ib_destroy_cq(device->dev_rx_cq[i]);
+		ib_destroy_cq(device->dev_tx_cq[i]);
+		device->dev_rx_cq[i] = NULL;
+		device->dev_tx_cq[i] = NULL;
+	}
+
+	ib_dereg_mr(device->dev_mr);
+	ib_dealloc_pd(device->dev_pd);
+	kfree(device->cq_desc);
+}
+
+static void
+isert_device_try_release(struct isert_device *device)
+{
+	mutex_lock(&device_list_mutex);
+	device->refcount--;
+	if (!device->refcount) {
+		isert_free_device_ib_res(device);
+		list_del(&device->dev_node);
+		kfree(device);
+	}
+	mutex_unlock(&device_list_mutex);
+}
+
+static struct isert_device *
+isert_device_find_by_ib_dev(struct rdma_cm_id *cma_id)
+{
+	struct isert_device *device;
+	int ret;
+
+	mutex_lock(&device_list_mutex);
+	list_for_each_entry(device, &device_list, dev_node) {
+		if (device->ib_device->node_guid == cma_id->device->node_guid) {
+			device->refcount++;
+			mutex_unlock(&device_list_mutex);
+			return device;
+		}
+	}
+
+	device = kzalloc(sizeof(struct isert_device), GFP_KERNEL);
+	if (!device) {
+		mutex_unlock(&device_list_mutex);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	INIT_LIST_HEAD(&device->dev_node);
+
+	device->ib_device = cma_id->device;
+	ret = isert_create_device_ib_res(device);
+	if (ret) {
+		kfree(device);
+		mutex_unlock(&device_list_mutex);
+		return ERR_PTR(ret);
+	}
+
+	device->refcount++;
+	list_add_tail(&device->dev_node, &device_list);
+	mutex_unlock(&device_list_mutex);
+
+	return device;
+}
+
+static int
+isert_connect_request(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
+{
+	struct iscsi_np *np = cma_id->context;
+	struct isert_np *isert_np = np->np_context;
+	struct isert_conn *isert_conn;
+	struct isert_device *device;
+	struct ib_device *ib_dev = cma_id->device;
+	int ret = 0;
+
+	pr_debug("Entering isert_connect_request cma_id: %p, context: %p\n",
+		 cma_id, cma_id->context);
+
+	isert_conn = kzalloc(sizeof(struct isert_conn), GFP_KERNEL);
+	if (!isert_conn) {
+		pr_err("Unable to allocate isert_conn\n");
+		return -ENOMEM;
+	}
+	isert_conn->state = ISER_CONN_INIT;
+	INIT_LIST_HEAD(&isert_conn->conn_accept_node);
+	init_completion(&isert_conn->conn_login_comp);
+	init_waitqueue_head(&isert_conn->conn_wait);
+	init_waitqueue_head(&isert_conn->conn_wait_comp_err);
+	kref_init(&isert_conn->conn_kref);
+	kref_get(&isert_conn->conn_kref);
+
+	cma_id->context = isert_conn;
+	isert_conn->conn_cm_id = cma_id;
+	isert_conn->responder_resources = event->param.conn.responder_resources;
+	isert_conn->initiator_depth = event->param.conn.initiator_depth;
+	pr_debug("Using responder_resources: %u initiator_depth: %u\n",
+		 isert_conn->responder_resources, isert_conn->initiator_depth);
+
+	isert_conn->login_buf = kzalloc(ISCSI_DEF_MAX_RECV_SEG_LEN +
+					ISER_RX_LOGIN_SIZE, GFP_KERNEL);
+	if (!isert_conn->login_buf) {
+		pr_err("Unable to allocate isert_conn->login_buf\n");
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	isert_conn->login_req_buf = isert_conn->login_buf;
+	isert_conn->login_rsp_buf = isert_conn->login_buf +
+				    ISCSI_DEF_MAX_RECV_SEG_LEN;
+	pr_debug("Set login_buf: %p login_req_buf: %p login_rsp_buf: %p\n",
+		 isert_conn->login_buf, isert_conn->login_req_buf,
+		 isert_conn->login_rsp_buf);
+
+	isert_conn->login_req_dma = ib_dma_map_single(ib_dev,
+				(void *)isert_conn->login_req_buf,
+				ISCSI_DEF_MAX_RECV_SEG_LEN, DMA_FROM_DEVICE);
+
+	ret = ib_dma_mapping_error(ib_dev, isert_conn->login_req_dma);
+	if (ret) {
+		pr_err("ib_dma_mapping_error failed for login_req_dma: %d\n",
+		       ret);
+		isert_conn->login_req_dma = 0;
+		goto out_login_buf;
+	}
+
+	isert_conn->login_rsp_dma = ib_dma_map_single(ib_dev,
+					(void *)isert_conn->login_rsp_buf,
+					ISER_RX_LOGIN_SIZE, DMA_TO_DEVICE);
+
+	ret = ib_dma_mapping_error(ib_dev, isert_conn->login_rsp_dma);
+	if (ret) {
+		pr_err("ib_dma_mapping_error failed for login_rsp_dma: %d\n",
+		       ret);
+		isert_conn->login_rsp_dma = 0;
+		goto out_req_dma_map;
+	}
+
+	device = isert_device_find_by_ib_dev(cma_id);
+	if (IS_ERR(device)) {
+		ret = PTR_ERR(device);
+		goto out_rsp_dma_map;
+	}
+
+	isert_conn->conn_device = device;
+	isert_conn->conn_pd = device->dev_pd;
+	isert_conn->conn_mr = device->dev_mr;
+
+	ret = isert_conn_setup_qp(isert_conn, cma_id);
+	if (ret)
+		goto out_conn_dev;
+
+	mutex_lock(&isert_np->np_accept_mutex);
+	list_add_tail(&isert_np->np_accept_list, &isert_conn->conn_accept_node);
+	mutex_unlock(&isert_np->np_accept_mutex);
+
+	pr_debug("isert_connect_request() waking up np_accept_wq: %p\n", np);
+	wake_up(&isert_np->np_accept_wq);
+	return 0;
+
+out_conn_dev:
+	isert_device_try_release(device);
+out_rsp_dma_map:
+	ib_dma_unmap_single(ib_dev, isert_conn->login_rsp_dma,
+			    ISER_RX_LOGIN_SIZE, DMA_TO_DEVICE);
+out_req_dma_map:
+	ib_dma_unmap_single(ib_dev, isert_conn->login_req_dma,
+			    ISCSI_DEF_MAX_RECV_SEG_LEN, DMA_FROM_DEVICE);
+out_login_buf:
+	kfree(isert_conn->login_buf);
+out:
+	kfree(isert_conn);
+	return ret;
+}
+
+static void
+isert_connect_release(struct isert_conn *isert_conn)
+{
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+	struct isert_device *device = isert_conn->conn_device;
+	int cq_index;
+
+	pr_debug("Entering isert_connect_release(): >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
+
+	if (isert_conn->conn_qp) {
+		cq_index = ((struct isert_cq_desc *)
+			isert_conn->conn_qp->recv_cq->cq_context)->cq_index;
+		pr_debug("isert_connect_release: cq_index: %d\n", cq_index);
+		isert_conn->conn_device->cq_active_qps[cq_index]--;
+
+		rdma_destroy_qp(isert_conn->conn_cm_id);
+	}
+
+	isert_free_rx_descriptors(isert_conn);
+	rdma_destroy_id(isert_conn->conn_cm_id);
+
+	if (isert_conn->login_buf) {
+		ib_dma_unmap_single(ib_dev, isert_conn->login_rsp_dma,
+				    ISER_RX_LOGIN_SIZE, DMA_TO_DEVICE);
+		ib_dma_unmap_single(ib_dev, isert_conn->login_req_dma,
+				    ISCSI_DEF_MAX_RECV_SEG_LEN,
+				    DMA_FROM_DEVICE);
+		kfree(isert_conn->login_buf);
+	}
+	kfree(isert_conn);
+
+	if (device)
+		isert_device_try_release(device);
+
+	pr_debug("Leaving isert_connect_release >>>>>>>>>>>>\n");
+}
+
+static void
+isert_connected_handler(struct rdma_cm_id *cma_id)
+{
+	return;
+}
+
+static void
+isert_release_conn_kref(struct kref *kref)
+{
+	struct isert_conn *isert_conn = container_of(kref,
+				struct isert_conn, conn_kref);
+
+	pr_debug("Calling isert_connect_release for final kref %s/%d\n",
+		 current->comm, current->pid);
+
+	isert_connect_release(isert_conn);
+}
+
+static void
+isert_put_conn(struct isert_conn *isert_conn)
+{
+	kref_put(&isert_conn->conn_kref, isert_release_conn_kref);
+}
+
+static void
+isert_disconnect_work(struct work_struct *work)
+{
+	struct isert_conn *isert_conn = container_of(work,
+				struct isert_conn, conn_logout_work);
+
+	pr_debug("isert_disconnect_work(): >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
+
+	isert_conn->state = ISER_CONN_DOWN;
+
+	if (isert_conn->post_recv_buf_count == 0 &&
+	    atomic_read(&isert_conn->post_send_buf_count) == 0) {
+		pr_debug("Calling wake_up(&isert_conn->conn_wait);\n");
+		wake_up(&isert_conn->conn_wait);
+	}
+
+	isert_put_conn(isert_conn);
+}
+
+static void
+isert_disconnected_handler(struct rdma_cm_id *cma_id)
+{
+	struct isert_conn *isert_conn = (struct isert_conn *)cma_id->context;
+
+	INIT_WORK(&isert_conn->conn_logout_work, isert_disconnect_work);
+	schedule_work(&isert_conn->conn_logout_work);
+}
+
+static int
+isert_cma_handler(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
+{
+	int ret = 0;
+
+	pr_debug("isert_cma_handler: event %d status %d conn %p id %p\n",
+		 event->event, event->status, cma_id->context, cma_id);
+
+	switch (event->event) {
+	case RDMA_CM_EVENT_CONNECT_REQUEST:
+		pr_debug("RDMA_CM_EVENT_CONNECT_REQUEST: >>>>>>>>>>>>>>>\n");
+		ret = isert_connect_request(cma_id, event);
+		break;
+	case RDMA_CM_EVENT_ESTABLISHED:
+		pr_debug("RDMA_CM_EVENT_ESTABLISHED >>>>>>>>>>>>>>\n");
+		isert_connected_handler(cma_id);
+		break;
+	case RDMA_CM_EVENT_DISCONNECTED:
+		pr_debug("RDMA_CM_EVENT_DISCONNECTED: >>>>>>>>>>>>>>\n");
+		isert_disconnected_handler(cma_id);
+		break;
+	case RDMA_CM_EVENT_DEVICE_REMOVAL:
+	case RDMA_CM_EVENT_ADDR_CHANGE:
+		break;
+	case RDMA_CM_EVENT_CONNECT_ERROR:
+	default:
+		pr_err("Unknown RDMA CMA event: %d\n", event->event);
+		break;
+	}
+
+	if (ret != 0) {
+		pr_err("isert_cma_handler failed RDMA_CM_EVENT: 0x%08x %d\n",
+		       event->event, ret);
+		dump_stack();
+	}
+
+	return ret;
+}
+
+static int
+isert_post_recv(struct isert_conn *isert_conn, u32 count)
+{
+	struct ib_recv_wr *rx_wr, *rx_wr_failed;
+	int i, ret;
+	unsigned int rx_head = isert_conn->conn_rx_desc_head;
+	struct iser_rx_desc *rx_desc;
+
+	for (rx_wr = isert_conn->conn_rx_wr, i = 0; i < count; i++, rx_wr++) {
+		rx_desc		= &isert_conn->conn_rx_descs[rx_head];
+		rx_wr->wr_id	= (unsigned long)rx_desc;
+		rx_wr->sg_list	= &rx_desc->rx_sg;
+		rx_wr->num_sge	= 1;
+		rx_wr->next	= rx_wr + 1;
+		rx_head = (rx_head + 1) & (ISERT_QP_MAX_RECV_DTOS - 1);
+	}
+
+	rx_wr--;
+	rx_wr->next = NULL; /* mark end of work requests list */
+
+	isert_conn->post_recv_buf_count += count;
+	ret = ib_post_recv(isert_conn->conn_qp, isert_conn->conn_rx_wr,
+				&rx_wr_failed);
+	if (ret) {
+		pr_err("ib_post_recv() failed with ret: %d\n", ret);
+		isert_conn->post_recv_buf_count -= count;
+	} else {
+		pr_debug("isert_post_recv(): Posted %d RX buffers\n", count);
+		isert_conn->conn_rx_desc_head = rx_head;
+	}
+	return ret;
+}
+
+static int
+isert_post_send(struct isert_conn *isert_conn, struct iser_tx_desc *tx_desc)
+{
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+	struct ib_send_wr send_wr, *send_wr_failed;
+	int ret;
+
+	ib_dma_sync_single_for_device(ib_dev, tx_desc->dma_addr,
+				      ISER_HEADERS_LEN, DMA_TO_DEVICE);
+
+	send_wr.next	= NULL;
+	send_wr.wr_id	= (unsigned long)tx_desc;
+	send_wr.sg_list	= tx_desc->tx_sg;
+	send_wr.num_sge	= tx_desc->num_sge;
+	send_wr.opcode	= IB_WR_SEND;
+	send_wr.send_flags = IB_SEND_SIGNALED;
+
+	atomic_inc(&isert_conn->post_send_buf_count);
+
+	ret = ib_post_send(isert_conn->conn_qp, &send_wr, &send_wr_failed);
+	if (ret) {
+		pr_err("ib_post_send() failed, ret: %d\n", ret);
+		atomic_dec(&isert_conn->post_send_buf_count);
+	}
+
+	return ret;
+}
+
+static void
+isert_create_send_desc(struct isert_conn *isert_conn,
+		       struct isert_cmd *isert_cmd,
+		       struct iser_tx_desc *tx_desc)
+{
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+
+	ib_dma_sync_single_for_cpu(ib_dev, tx_desc->dma_addr,
+				   ISER_HEADERS_LEN, DMA_TO_DEVICE);
+
+	memset(&tx_desc->iser_header, 0, sizeof(struct iser_hdr));
+	tx_desc->iser_header.flags = ISER_VER;
+
+	tx_desc->num_sge = 1;
+	tx_desc->isert_cmd = isert_cmd;
+
+	if (tx_desc->tx_sg[0].lkey != isert_conn->conn_mr->lkey) {
+		tx_desc->tx_sg[0].lkey = isert_conn->conn_mr->lkey;
+		pr_debug("tx_desc %p lkey mismatch, fixing\n", tx_desc);
+	}
+}
+
+static int
+isert_init_tx_hdrs(struct isert_conn *isert_conn,
+		   struct iser_tx_desc *tx_desc)
+{
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+	u64 dma_addr;
+
+	dma_addr = ib_dma_map_single(ib_dev, (void *)tx_desc,
+			ISER_HEADERS_LEN, DMA_TO_DEVICE);
+	if (ib_dma_mapping_error(ib_dev, dma_addr)) {
+		pr_err("ib_dma_mapping_error() failed\n");
+		return -ENOMEM;
+	}
+
+	tx_desc->dma_addr = dma_addr;
+	tx_desc->tx_sg[0].addr	= tx_desc->dma_addr;
+	tx_desc->tx_sg[0].length = ISER_HEADERS_LEN;
+	tx_desc->tx_sg[0].lkey = isert_conn->conn_mr->lkey;
+
+	pr_debug("isert_init_tx_hdrs: Setup tx_sg[0].addr: 0x%llx length: %u"
+		 " lkey: 0x%08x\n", tx_desc->tx_sg[0].addr,
+		 tx_desc->tx_sg[0].length, tx_desc->tx_sg[0].lkey);
+
+	return 0;
+}
+
+static void
+isert_init_send_wr(struct isert_cmd *isert_cmd, struct ib_send_wr *send_wr)
+{
+	isert_cmd->rdma_wr.iser_ib_op = ISER_IB_SEND;
+	send_wr->wr_id = (unsigned long)&isert_cmd->tx_desc;
+	send_wr->opcode = IB_WR_SEND;
+	send_wr->send_flags = IB_SEND_SIGNALED;
+	send_wr->sg_list = &isert_cmd->tx_desc.tx_sg[0];
+	send_wr->num_sge = isert_cmd->tx_desc.num_sge;
+}
+
+static int
+isert_rdma_post_recvl(struct isert_conn *isert_conn)
+{
+	struct ib_recv_wr rx_wr, *rx_wr_fail;
+	struct ib_sge sge;
+	int ret;
+
+	memset(&sge, 0, sizeof(struct ib_sge));
+	sge.addr = isert_conn->login_req_dma;
+	sge.length = ISER_RX_LOGIN_SIZE;
+	sge.lkey = isert_conn->conn_mr->lkey;
+
+	pr_debug("Setup sge: addr: %llx length: %d 0x%08x\n",
+		sge.addr, sge.length, sge.lkey);
+
+	memset(&rx_wr, 0, sizeof(struct ib_recv_wr));
+	rx_wr.wr_id = (unsigned long)isert_conn->login_req_buf;
+	rx_wr.sg_list = &sge;
+	rx_wr.num_sge = 1;
+
+	isert_conn->post_recv_buf_count++;
+	ret = ib_post_recv(isert_conn->conn_qp, &rx_wr, &rx_wr_fail);
+	if (ret) {
+		pr_err("ib_post_recv() failed: %d\n", ret);
+		isert_conn->post_recv_buf_count--;
+	}
+
+	pr_debug("ib_post_recv(): returned success >>>>>>>>>>>>>>>>>>>>>>>>\n");
+	return ret;
+}
+
+static int
+isert_put_login_tx(struct iscsi_conn *conn, struct iscsi_login *login,
+		   u32 length)
+{
+	struct isert_conn *isert_conn = conn->context;
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+	struct iser_tx_desc *tx_desc = &isert_conn->conn_login_tx_desc;
+	int ret;
+
+	isert_create_send_desc(isert_conn, NULL, tx_desc);
+
+	memcpy(&tx_desc->iscsi_header, &login->rsp[0],
+	       sizeof(struct iscsi_hdr));
+
+	isert_init_tx_hdrs(isert_conn, tx_desc);
+
+	if (length > 0) {
+		struct ib_sge *tx_dsg = &tx_desc->tx_sg[1];
+
+		ib_dma_sync_single_for_cpu(ib_dev, isert_conn->login_rsp_dma,
+					   length, DMA_TO_DEVICE);
+
+		memcpy(isert_conn->login_rsp_buf, login->rsp_buf, length);
+
+		ib_dma_sync_single_for_device(ib_dev, isert_conn->login_rsp_dma,
+					      length, DMA_TO_DEVICE);
+
+		tx_dsg->addr	= isert_conn->login_rsp_dma;
+		tx_dsg->length	= length;
+		tx_dsg->lkey	= isert_conn->conn_mr->lkey;
+		tx_desc->num_sge = 2;
+	}
+	if (!login->login_failed) {
+		if (login->login_complete) {
+			ret = isert_alloc_rx_descriptors(isert_conn);
+			if (ret)
+				return ret;
+
+			ret = isert_post_recv(isert_conn, ISERT_MIN_POSTED_RX);
+			if (ret)
+				return ret;
+
+			isert_conn->state = ISER_CONN_UP;
+			goto post_send;
+		}
+
+		ret = isert_rdma_post_recvl(isert_conn);
+		if (ret)
+			return ret;
+	}
+post_send:
+	ret = isert_post_send(isert_conn, tx_desc);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static void
+isert_rx_login_req(struct iser_rx_desc *rx_desc, int rx_buflen,
+		   struct isert_conn *isert_conn)
+{
+	struct iscsi_conn *conn = isert_conn->conn;
+	struct iscsi_login *login = conn->conn_login;
+	int size;
+
+	if (!login) {
+		pr_err("conn->conn_login is NULL\n");
+		dump_stack();
+		return;
+	}
+
+	if (login->first_request) {
+		struct iscsi_login_req *login_req =
+			(struct iscsi_login_req *)&rx_desc->iscsi_header;
+		/*
+		 * Setup the initial iscsi_login values from the leading
+		 * login request PDU.
+		 */
+		login->leading_connection = (!login_req->tsih) ? 1 : 0;
+		login->current_stage =
+			(login_req->flags & ISCSI_FLAG_LOGIN_CURRENT_STAGE_MASK)
+			 >> 2;
+		login->version_min	= login_req->min_version;
+		login->version_max	= login_req->max_version;
+		memcpy(login->isid, login_req->isid, 6);
+		login->cmd_sn		= be32_to_cpu(login_req->cmdsn);
+		login->init_task_tag	= login_req->itt;
+		login->initial_exp_statsn = be32_to_cpu(login_req->exp_statsn);
+		login->cid		= be16_to_cpu(login_req->cid);
+		login->tsih		= be16_to_cpu(login_req->tsih);
+	}
+
+	memcpy(&login->req[0], (void *)&rx_desc->iscsi_header, ISCSI_HDR_LEN);
+
+	size = min(rx_buflen, MAX_KEY_VALUE_PAIRS);
+	pr_debug("Using login payload size: %d, rx_buflen: %d MAX_KEY_VALUE_PAIRS: %d\n",
+		 size, rx_buflen, MAX_KEY_VALUE_PAIRS);
+	memcpy(login->req_buf, &rx_desc->data[0], size);
+
+	complete(&isert_conn->conn_login_comp);
+}
+
+static void
+isert_release_cmd(struct iscsi_cmd *cmd)
+{
+	struct isert_cmd *isert_cmd = container_of(cmd, struct isert_cmd,
+						   iscsi_cmd);
+
+	pr_debug("Entering isert_release_cmd %p >>>>>>>>>>>>>>>.\n", isert_cmd);
+
+	kfree(cmd->buf_ptr);
+	kfree(cmd->tmr_req);
+
+	kmem_cache_free(isert_cmd_cache, isert_cmd);
+}
+
+static struct iscsi_cmd
+*isert_alloc_cmd(struct iscsi_conn *conn, gfp_t gfp)
+{
+	struct isert_conn *isert_conn = (struct isert_conn *)conn->context;
+	struct isert_cmd *isert_cmd;
+
+	isert_cmd = kmem_cache_zalloc(isert_cmd_cache, gfp);
+	if (!isert_cmd) {
+		pr_err("Unable to allocate isert_cmd\n");
+		return NULL;
+	}
+	isert_cmd->conn = isert_conn;
+	isert_cmd->iscsi_cmd.release_cmd = &isert_release_cmd;
+
+	return &isert_cmd->iscsi_cmd;
+}
+
+static int
+isert_handle_scsi_cmd(struct isert_conn *isert_conn,
+		      struct isert_cmd *isert_cmd, struct iser_rx_desc *rx_desc,
+		      unsigned char *buf)
+{
+	struct iscsi_cmd *cmd = &isert_cmd->iscsi_cmd;
+	struct iscsi_conn *conn = isert_conn->conn;
+	struct iscsi_scsi_req *hdr = (struct iscsi_scsi_req *)buf;
+	struct scatterlist *sg;
+	int imm_data, imm_data_len, unsol_data, sg_nents, rc;
+	bool dump_payload = false;
+
+	rc = iscsit_setup_scsi_cmd(conn, cmd, buf);
+	if (rc < 0)
+		return rc;
+
+	imm_data = cmd->immediate_data;
+	imm_data_len = cmd->first_burst_len;
+	unsol_data = cmd->unsolicited_data;
+
+	rc = iscsit_process_scsi_cmd(conn, cmd, hdr);
+	if (rc < 0) {
+		return 0;
+	} else if (rc > 0) {
+		dump_payload = true;
+		goto sequence_cmd;
+	}
+
+	if (!imm_data)
+		return 0;
+
+	sg = &cmd->se_cmd.t_data_sg[0];
+	sg_nents = max(1UL, DIV_ROUND_UP(imm_data_len, PAGE_SIZE));
+
+	pr_debug("Copying Immediate SG: %p sg_nents: %u from %p imm_data_len: %d\n",
+		 sg, sg_nents, &rx_desc->data[0], imm_data_len);
+
+	sg_copy_from_buffer(sg, sg_nents, &rx_desc->data[0], imm_data_len);
+
+	cmd->write_data_done += imm_data_len;
+
+	if (cmd->write_data_done == cmd->se_cmd.data_length) {
+		spin_lock_bh(&cmd->istate_lock);
+		cmd->cmd_flags |= ICF_GOT_LAST_DATAOUT;
+		cmd->i_state = ISTATE_RECEIVED_LAST_DATAOUT;
+		spin_unlock_bh(&cmd->istate_lock);
+	}
+
+sequence_cmd:
+	rc = iscsit_sequence_cmd(conn, cmd, hdr->cmdsn);
+
+	if (!rc && dump_payload == false && unsol_data)
+		iscsit_set_unsoliticed_dataout(cmd);
+
+	if (rc == CMDSN_ERROR_CANNOT_RECOVER)
+		return iscsit_add_reject_from_cmd(
+			   ISCSI_REASON_PROTOCOL_ERROR,
+			   1, 0, (unsigned char *)hdr, cmd);
+
+	return 0;
+}
+
+static int
+isert_handle_iscsi_dataout(struct isert_conn *isert_conn,
+			   struct iser_rx_desc *rx_desc, unsigned char *buf)
+{
+	struct scatterlist *sg_start;
+	struct iscsi_conn *conn = isert_conn->conn;
+	struct iscsi_cmd *cmd = NULL;
+	struct iscsi_data *hdr = (struct iscsi_data *)buf;
+	u32 unsol_data_len = ntoh24(hdr->dlength);
+	int rc, sg_nents, sg_off, page_off;
+
+	rc = iscsit_check_dataout_hdr(conn, buf, &cmd);
+	if (rc < 0)
+		return rc;
+	else if (!cmd)
+		return 0;
+	/*
+	 * FIXME: Unexpected unsolicited_data out
+	 */
+	if (!cmd->unsolicited_data) {
+		pr_err("Received unexpected solicited data payload\n");
+		dump_stack();
+		return -1;
+	}
+
+	pr_debug("Unsolicited DataOut unsol_data_len: %u, write_data_done: %u, data_length: %u\n",
+		 unsol_data_len, cmd->write_data_done, cmd->se_cmd.data_length);
+
+	sg_off = cmd->write_data_done / PAGE_SIZE;
+	sg_start = &cmd->se_cmd.t_data_sg[sg_off];
+	sg_nents = max(1UL, DIV_ROUND_UP(unsol_data_len, PAGE_SIZE));
+	page_off = cmd->write_data_done % PAGE_SIZE;
+	/*
+	 * FIXME: Non page-aligned unsolicited_data out
+	 */
+	if (page_off) {
+		pr_err("Received unexpected non-page aligned data payload\n");
+		dump_stack();
+		return -1;
+	}
+	pr_debug("Copying DataOut: sg_start: %p, sg_off: %u sg_nents: %u from %p %u\n",
+		 sg_start, sg_off, sg_nents, &rx_desc->data[0], unsol_data_len);
+
+	sg_copy_from_buffer(sg_start, sg_nents, &rx_desc->data[0],
+			    unsol_data_len);
+
+	rc = iscsit_check_dataout_payload(cmd, hdr, false);
+	if (rc < 0)
+		return rc;
+
+	return 0;
+}
+
+static int
+isert_rx_opcode(struct isert_conn *isert_conn, struct iser_rx_desc *rx_desc,
+		uint32_t read_stag, uint64_t read_va,
+		uint32_t write_stag, uint64_t write_va)
+{
+	struct iscsi_hdr *hdr = &rx_desc->iscsi_header;
+	struct iscsi_conn *conn = isert_conn->conn;
+	struct iscsi_cmd *cmd;
+	struct isert_cmd *isert_cmd;
+	int ret = -EINVAL;
+	u8 opcode = (hdr->opcode & ISCSI_OPCODE_MASK);
+
+	switch (opcode) {
+	case ISCSI_OP_SCSI_CMD:
+		cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
+		if (!cmd)
+			break;
+
+		isert_cmd = container_of(cmd, struct isert_cmd, iscsi_cmd);
+		isert_cmd->read_stag = read_stag;
+		isert_cmd->read_va = read_va;
+		isert_cmd->write_stag = write_stag;
+		isert_cmd->write_va = write_va;
+
+		ret = isert_handle_scsi_cmd(isert_conn, isert_cmd,
+					rx_desc, (unsigned char *)hdr);
+		break;
+	case ISCSI_OP_NOOP_OUT:
+		cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
+		if (!cmd)
+			break;
+
+		ret = iscsit_handle_nop_out(conn, cmd, (unsigned char *)hdr);
+		break;
+	case ISCSI_OP_SCSI_DATA_OUT:
+		ret = isert_handle_iscsi_dataout(isert_conn, rx_desc,
+						(unsigned char *)hdr);
+		break;
+	case ISCSI_OP_SCSI_TMFUNC:
+		cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
+		if (!cmd)
+			break;
+
+		ret = iscsit_handle_task_mgt_cmd(conn, cmd,
+						(unsigned char *)hdr);
+		break;
+	case ISCSI_OP_LOGOUT:
+		cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
+		if (!cmd)
+			break;
+
+		ret = iscsit_handle_logout_cmd(conn, cmd, (unsigned char *)hdr);
+		if (ret > 0)
+			wait_for_completion_timeout(&conn->conn_logout_comp,
+						    SECONDS_FOR_LOGOUT_COMP *
+						    HZ);
+		break;
+	default:
+		pr_err("Got unknown iSCSI OpCode: 0x%02x\n", opcode);
+		dump_stack();
+		break;
+	}
+
+	return ret;
+}
+
+static void
+isert_rx_do_work(struct iser_rx_desc *rx_desc, struct isert_conn *isert_conn)
+{
+	struct iser_hdr *iser_hdr = &rx_desc->iser_header;
+	uint64_t read_va = 0, write_va = 0;
+	uint32_t read_stag = 0, write_stag = 0;
+	int rc;
+
+	switch (iser_hdr->flags & 0xF0) {
+	case ISCSI_CTRL:
+		if (iser_hdr->flags & ISER_RSV) {
+			read_stag = be32_to_cpu(iser_hdr->read_stag);
+			read_va = be64_to_cpu(iser_hdr->read_va);
+			pr_debug("ISER_RSV: read_stag: 0x%08x read_va: 0x%16llx\n",
+				 read_stag, (unsigned long long)read_va);
+		}
+		if (iser_hdr->flags & ISER_WSV) {
+			write_stag = be32_to_cpu(iser_hdr->write_stag);
+			write_va = be64_to_cpu(iser_hdr->write_va);
+			pr_debug("ISER_WSV: write__stag: 0x%08x write_va: 0x%16llx\n",
+				 write_stag, (unsigned long long)write_va);
+		}
+
+		pr_debug("ISER ISCSI_CTRL PDU\n");
+		break;
+	case ISER_HELLO:
+		pr_err("iSER Hello message\n");
+		break;
+	default:
+		pr_warn("Unknown iSER hdr flags: 0x%02x\n", iser_hdr->flags);
+		break;
+	}
+
+	rc = isert_rx_opcode(isert_conn, rx_desc,
+			     read_stag, read_va, write_stag, write_va);
+}
+
+static void
+isert_rx_completion(struct iser_rx_desc *desc, struct isert_conn *isert_conn,
+		    unsigned long xfer_len)
+{
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+	struct iscsi_hdr *hdr;
+	u64 rx_dma;
+	int rx_buflen, outstanding;
+
+	if ((char *)desc == isert_conn->login_req_buf) {
+		rx_dma = isert_conn->login_req_dma;
+		rx_buflen = ISER_RX_LOGIN_SIZE;
+		pr_debug("ISER login_buf: Using rx_dma: 0x%llx, rx_buflen: %d\n",
+			 rx_dma, rx_buflen);
+	} else {
+		rx_dma = desc->dma_addr;
+		rx_buflen = ISER_RX_PAYLOAD_SIZE;
+		pr_debug("ISER req_buf: Using rx_dma: 0x%llx, rx_buflen: %d\n",
+			 rx_dma, rx_buflen);
+	}
+
+	ib_dma_sync_single_for_cpu(ib_dev, rx_dma, rx_buflen, DMA_FROM_DEVICE);
+
+	hdr = &desc->iscsi_header;
+	pr_debug("iSCSI opcode: 0x%02x, ITT: 0x%08x, flags: 0x%02x dlen: %d\n",
+		 hdr->opcode, hdr->itt, hdr->flags,
+		 (int)(xfer_len - ISER_HEADERS_LEN));
+
+	if ((char *)desc == isert_conn->login_req_buf)
+		isert_rx_login_req(desc, xfer_len - ISER_HEADERS_LEN,
+				   isert_conn);
+	else
+		isert_rx_do_work(desc, isert_conn);
+
+	ib_dma_sync_single_for_device(ib_dev, rx_dma, rx_buflen,
+				      DMA_FROM_DEVICE);
+
+	isert_conn->post_recv_buf_count--;
+	pr_debug("iSERT: Decremented post_recv_buf_count: %d\n",
+		 isert_conn->post_recv_buf_count);
+
+	if ((char *)desc == isert_conn->login_req_buf)
+		return;
+
+	outstanding = isert_conn->post_recv_buf_count;
+	if (outstanding + ISERT_MIN_POSTED_RX <= ISERT_QP_MAX_RECV_DTOS) {
+		int err, count = min(ISERT_QP_MAX_RECV_DTOS - outstanding,
+				ISERT_MIN_POSTED_RX);
+		err = isert_post_recv(isert_conn, count);
+		if (err) {
+			pr_err("isert_post_recv() count: %d failed, %d\n",
+			       count, err);
+		}
+	}
+}
+
+static void
+isert_unmap_cmd(struct isert_cmd *isert_cmd, struct isert_conn *isert_conn)
+{
+	struct isert_rdma_wr *wr = &isert_cmd->rdma_wr;
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+
+	pr_debug("isert_unmap_cmd >>>>>>>>>>>>>>>>>>>>>>>\n");
+
+	if (wr->sge) {
+		ib_dma_unmap_sg(ib_dev, wr->sge, wr->num_sge, DMA_TO_DEVICE);
+		wr->sge = NULL;
+	}
+
+	kfree(wr->send_wr);
+	wr->send_wr = NULL;
+
+	kfree(isert_cmd->ib_sge);
+	isert_cmd->ib_sge = NULL;
+}
+
+static void
+isert_put_cmd(struct isert_cmd *isert_cmd)
+{
+	struct iscsi_cmd *cmd = &isert_cmd->iscsi_cmd;
+	struct isert_conn *isert_conn = isert_cmd->conn;
+	struct iscsi_conn *conn;
+
+	pr_debug("Entering isert_put_cmd: %p\n", isert_cmd);
+
+	switch (cmd->iscsi_opcode) {
+	case ISCSI_OP_SCSI_CMD:
+		conn = isert_conn->conn;
+
+		spin_lock_bh(&conn->cmd_lock);
+		if (!list_empty(&cmd->i_conn_node))
+			list_del(&cmd->i_conn_node);
+		spin_unlock_bh(&conn->cmd_lock);
+
+		if (cmd->data_direction == DMA_TO_DEVICE)
+			iscsit_stop_dataout_timer(cmd);
+
+		isert_unmap_cmd(isert_cmd, isert_conn);
+		/*
+		 * Fall-through
+		 */
+	case ISCSI_OP_SCSI_TMFUNC:
+		transport_generic_free_cmd(&cmd->se_cmd, 0);
+		break;
+	case ISCSI_OP_REJECT:
+	case ISCSI_OP_NOOP_OUT:
+		conn = isert_conn->conn;
+
+		spin_lock_bh(&conn->cmd_lock);
+		if (!list_empty(&cmd->i_conn_node))
+			list_del(&cmd->i_conn_node);
+		spin_unlock_bh(&conn->cmd_lock);
+
+		/*
+		 * Handle special case for REJECT when iscsi_add_reject*() has
+		 * overwritten the original iscsi_opcode assignment, and the
+		 * associated cmd->se_cmd needs to be released.
+		 */
+		if (cmd->se_cmd.se_tfo != NULL) {
+			transport_generic_free_cmd(&cmd->se_cmd, 0);
+			break;
+		}
+		/*
+		 * Fall-through
+		 */
+	default:
+		isert_release_cmd(cmd);
+		break;
+	}
+}
+
+static void
+isert_unmap_tx_desc(struct iser_tx_desc *tx_desc, struct ib_device *ib_dev)
+{
+	if (tx_desc->dma_addr != 0) {
+		pr_debug("Calling ib_dma_unmap_single for tx_desc->dma_addr\n");
+		ib_dma_unmap_single(ib_dev, tx_desc->dma_addr,
+				    ISER_HEADERS_LEN, DMA_TO_DEVICE);
+		tx_desc->dma_addr = 0;
+	}
+}
+
+static void
+isert_completion_put(struct iser_tx_desc *tx_desc, struct isert_cmd *isert_cmd,
+		     struct ib_device *ib_dev)
+{
+	if (isert_cmd->sense_buf_dma != 0) {
+		pr_debug("Calling ib_dma_unmap_single for isert_cmd->sense_buf_dma\n");
+		ib_dma_unmap_single(ib_dev, isert_cmd->sense_buf_dma,
+				    isert_cmd->sense_buf_len, DMA_TO_DEVICE);
+		isert_cmd->sense_buf_dma = 0;
+	}
+
+	isert_unmap_tx_desc(tx_desc, ib_dev);
+	isert_put_cmd(isert_cmd);
+}
+
+static void
+isert_completion_rdma_read(struct iser_tx_desc *tx_desc,
+			   struct isert_cmd *isert_cmd)
+{
+	struct isert_rdma_wr *wr = &isert_cmd->rdma_wr;
+	struct iscsi_cmd *cmd = &isert_cmd->iscsi_cmd;
+	struct se_cmd *se_cmd = &cmd->se_cmd;
+	struct ib_device *ib_dev = isert_cmd->conn->conn_cm_id->device;
+
+	iscsit_stop_dataout_timer(cmd);
+
+	if (wr->sge) {
+		pr_debug("isert_do_rdma_read_comp: Unmapping wr->sge from t_data_sg\n");
+		ib_dma_unmap_sg(ib_dev, wr->sge, wr->num_sge, DMA_TO_DEVICE);
+		wr->sge = NULL;
+	}
+
+	if (isert_cmd->ib_sge) {
+		pr_debug("isert_do_rdma_read_comp: Freeing isert_cmd->ib_sge\n");
+		kfree(isert_cmd->ib_sge);
+		isert_cmd->ib_sge = NULL;
+	}
+
+	cmd->write_data_done = se_cmd->data_length;
+
+	pr_debug("isert_do_rdma_read_comp, calling target_execute_cmd\n");
+	spin_lock_bh(&cmd->istate_lock);
+	cmd->cmd_flags |= ICF_GOT_LAST_DATAOUT;
+	cmd->i_state = ISTATE_RECEIVED_LAST_DATAOUT;
+	spin_unlock_bh(&cmd->istate_lock);
+
+	target_execute_cmd(se_cmd);
+}
+
+static void
+isert_do_control_comp(struct work_struct *work)
+{
+	struct isert_cmd *isert_cmd = container_of(work,
+			struct isert_cmd, comp_work);
+	struct isert_conn *isert_conn = isert_cmd->conn;
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+	struct iscsi_cmd *cmd = &isert_cmd->iscsi_cmd;
+
+	switch (cmd->i_state) {
+	case ISTATE_SEND_TASKMGTRSP:
+		pr_debug("Calling iscsit_tmr_post_handler >>>>>>>>>>>>>>>>>\n");
+
+		atomic_dec(&isert_conn->post_send_buf_count);
+		iscsit_tmr_post_handler(cmd, cmd->conn);
+
+		cmd->i_state = ISTATE_SENT_STATUS;
+		isert_completion_put(&isert_cmd->tx_desc, isert_cmd, ib_dev);
+		break;
+	case ISTATE_SEND_REJECT:
+		pr_debug("Got isert_do_control_comp ISTATE_SEND_REJECT: >>>\n");
+		atomic_dec(&isert_conn->post_send_buf_count);
+
+		cmd->i_state = ISTATE_SENT_STATUS;
+		complete(&cmd->reject_comp);
+		isert_completion_put(&isert_cmd->tx_desc, isert_cmd, ib_dev);
+	case ISTATE_SEND_LOGOUTRSP:
+		pr_debug("Calling iscsit_logout_post_handler >>>>>>>>>>>>>>\n");
+		/*
+		 * Call atomic_dec(&isert_conn->post_send_buf_count)
+		 * from isert_free_conn()
+		 */
+		isert_conn->logout_posted = true;
+		iscsit_logout_post_handler(cmd, cmd->conn);
+		break;
+	default:
+		pr_err("Unknown do_control_comp i_state %d\n", cmd->i_state);
+		dump_stack();
+		break;
+	}
+}
+
+static void
+isert_response_completion(struct iser_tx_desc *tx_desc,
+			  struct isert_cmd *isert_cmd,
+			  struct isert_conn *isert_conn,
+			  struct ib_device *ib_dev)
+{
+	struct iscsi_cmd *cmd = &isert_cmd->iscsi_cmd;
+
+	if (cmd->i_state == ISTATE_SEND_TASKMGTRSP ||
+	    cmd->i_state == ISTATE_SEND_LOGOUTRSP) {
+		isert_unmap_tx_desc(tx_desc, ib_dev);
+
+		INIT_WORK(&isert_cmd->comp_work, isert_do_control_comp);
+		queue_work(isert_comp_wq, &isert_cmd->comp_work);
+		return;
+	}
+	atomic_dec(&isert_conn->post_send_buf_count);
+
+	cmd->i_state = ISTATE_SENT_STATUS;
+	isert_completion_put(tx_desc, isert_cmd, ib_dev);
+}
+
+static void
+isert_send_completion(struct iser_tx_desc *tx_desc,
+		      struct isert_conn *isert_conn)
+{
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+	struct isert_cmd *isert_cmd = tx_desc->isert_cmd;
+	struct isert_rdma_wr *wr;
+
+	if (!isert_cmd) {
+		atomic_dec(&isert_conn->post_send_buf_count);
+		isert_unmap_tx_desc(tx_desc, ib_dev);
+		return;
+	}
+	wr = &isert_cmd->rdma_wr;
+
+	switch (wr->iser_ib_op) {
+	case ISER_IB_RECV:
+		pr_err("isert_send_completion: Got ISER_IB_RECV\n");
+		dump_stack();
+		break;
+	case ISER_IB_SEND:
+		pr_debug("isert_send_completion: Got ISER_IB_SEND\n");
+		isert_response_completion(tx_desc, isert_cmd,
+					  isert_conn, ib_dev);
+		break;
+	case ISER_IB_RDMA_WRITE:
+		pr_err("isert_send_completion: Got ISER_IB_RDMA_WRITE\n");
+		dump_stack();
+		break;
+	case ISER_IB_RDMA_READ:
+		pr_debug("isert_send_completion: Got ISER_IB_RDMA_READ:\n");
+
+		atomic_dec(&isert_conn->post_send_buf_count);
+		isert_completion_rdma_read(tx_desc, isert_cmd);
+		break;
+	default:
+		pr_err("Unknown wr->iser_ib_op: 0x%02x\n", wr->iser_ib_op);
+		dump_stack();
+		break;
+	}
+}
+
+static void
+isert_cq_comp_err(struct iser_tx_desc *tx_desc, struct isert_conn *isert_conn)
+{
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+
+	if (tx_desc) {
+		struct isert_cmd *isert_cmd = tx_desc->isert_cmd;
+
+		if (!isert_cmd)
+			isert_unmap_tx_desc(tx_desc, ib_dev);
+		else
+			isert_completion_put(tx_desc, isert_cmd, ib_dev);
+	}
+
+	if (isert_conn->post_recv_buf_count == 0 &&
+	    atomic_read(&isert_conn->post_send_buf_count) == 0) {
+		pr_debug("isert_cq_comp_err >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
+		pr_debug("Calling wake_up from isert_cq_comp_err\n");
+
+		isert_conn->state = ISER_CONN_TERMINATING;
+		wake_up(&isert_conn->conn_wait_comp_err);
+	}
+}
+
+static void
+isert_cq_tx_work(struct work_struct *work)
+{
+	struct isert_cq_desc *cq_desc = container_of(work,
+				struct isert_cq_desc, cq_tx_work);
+	struct isert_device *device = cq_desc->device;
+	int cq_index = cq_desc->cq_index;
+	struct ib_cq *tx_cq = device->dev_tx_cq[cq_index];
+	struct isert_conn *isert_conn;
+	struct iser_tx_desc *tx_desc;
+	struct ib_wc wc;
+
+	while (ib_poll_cq(tx_cq, 1, &wc) == 1) {
+		tx_desc = (struct iser_tx_desc *)(unsigned long)wc.wr_id;
+		isert_conn = wc.qp->qp_context;
+
+		if (wc.status == IB_WC_SUCCESS) {
+			isert_send_completion(tx_desc, isert_conn);
+		} else {
+			pr_debug("TX wc.status != IB_WC_SUCCESS >>>>>>>>>>>>>>\n");
+			pr_debug("TX wc.status: 0x%08x\n", wc.status);
+			atomic_dec(&isert_conn->post_send_buf_count);
+			isert_cq_comp_err(tx_desc, isert_conn);
+		}
+	}
+
+	ib_req_notify_cq(tx_cq, IB_CQ_NEXT_COMP);
+}
+
+static void
+isert_cq_tx_callback(struct ib_cq *cq, void *context)
+{
+	struct isert_cq_desc *cq_desc = (struct isert_cq_desc *)context;
+
+	INIT_WORK(&cq_desc->cq_tx_work, isert_cq_tx_work);
+	queue_work(isert_comp_wq, &cq_desc->cq_tx_work);
+}
+
+static void
+isert_cq_rx_work(struct work_struct *work)
+{
+	struct isert_cq_desc *cq_desc = container_of(work,
+			struct isert_cq_desc, cq_rx_work);
+	struct isert_device *device = cq_desc->device;
+	int cq_index = cq_desc->cq_index;
+	struct ib_cq *rx_cq = device->dev_rx_cq[cq_index];
+	struct isert_conn *isert_conn;
+	struct iser_rx_desc *rx_desc;
+	struct ib_wc wc;
+	unsigned long xfer_len;
+
+	while (ib_poll_cq(rx_cq, 1, &wc) == 1) {
+		rx_desc = (struct iser_rx_desc *)(unsigned long)wc.wr_id;
+		isert_conn = wc.qp->qp_context;
+
+		if (wc.status == IB_WC_SUCCESS) {
+			xfer_len = (unsigned long)wc.byte_len;
+			isert_rx_completion(rx_desc, isert_conn, xfer_len);
+		} else {
+			pr_debug("RX wc.status != IB_WC_SUCCESS >>>>>>>>>>>>>>\n");
+			if (wc.status != IB_WC_WR_FLUSH_ERR)
+				pr_debug("RX wc.status: 0x%08x\n", wc.status);
+
+			isert_conn->post_recv_buf_count--;
+			isert_cq_comp_err(NULL, isert_conn);
+		}
+	}
+
+	ib_req_notify_cq(rx_cq, IB_CQ_NEXT_COMP);
+}
+
+static void
+isert_cq_rx_callback(struct ib_cq *cq, void *context)
+{
+	struct isert_cq_desc *cq_desc = (struct isert_cq_desc *)context;
+
+	INIT_WORK(&cq_desc->cq_rx_work, isert_cq_rx_work);
+	queue_work(isert_rx_wq, &cq_desc->cq_rx_work);
+}
+
+static int
+isert_post_response(struct isert_conn *isert_conn, struct isert_cmd *isert_cmd)
+{
+	struct ib_send_wr *wr_failed;
+	int ret;
+
+	atomic_inc(&isert_conn->post_send_buf_count);
+
+	ret = ib_post_send(isert_conn->conn_qp, &isert_cmd->tx_desc.send_wr,
+			   &wr_failed);
+	if (ret) {
+		pr_err("ib_post_send failed with %d\n", ret);
+		atomic_dec(&isert_conn->post_send_buf_count);
+		return ret;
+	}
+	return ret;
+}
+
+static int
+isert_put_response(struct iscsi_conn *conn, struct iscsi_cmd *cmd)
+{
+	struct isert_cmd *isert_cmd = container_of(cmd,
+					struct isert_cmd, iscsi_cmd);
+	struct isert_conn *isert_conn = (struct isert_conn *)conn->context;
+	struct ib_send_wr *send_wr = &isert_cmd->tx_desc.send_wr;
+	struct iscsi_scsi_rsp *hdr = (struct iscsi_scsi_rsp *)
+				&isert_cmd->tx_desc.iscsi_header;
+
+	isert_create_send_desc(isert_conn, isert_cmd, &isert_cmd->tx_desc);
+	iscsit_build_rsp_pdu(cmd, conn, true, hdr);
+	isert_init_tx_hdrs(isert_conn, &isert_cmd->tx_desc);
+	/*
+	 * Attach SENSE DATA payload to iSCSI Response PDU
+	 */
+	if (cmd->se_cmd.sense_buffer &&
+	    ((cmd->se_cmd.se_cmd_flags & SCF_TRANSPORT_TASK_SENSE) ||
+	    (cmd->se_cmd.se_cmd_flags & SCF_EMULATED_TASK_SENSE))) {
+		struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+		struct ib_sge *tx_dsg = &isert_cmd->tx_desc.tx_sg[1];
+		u32 padding, sense_len;
+
+		put_unaligned_be16(cmd->se_cmd.scsi_sense_length,
+				   cmd->sense_buffer);
+		cmd->se_cmd.scsi_sense_length += sizeof(__be16);
+
+		padding = -(cmd->se_cmd.scsi_sense_length) & 3;
+		hton24(hdr->dlength, (u32)cmd->se_cmd.scsi_sense_length);
+		sense_len = cmd->se_cmd.scsi_sense_length + padding;
+
+		isert_cmd->sense_buf_dma = ib_dma_map_single(ib_dev,
+				(void *)cmd->sense_buffer, sense_len,
+				DMA_TO_DEVICE);
+
+		isert_cmd->sense_buf_len = sense_len;
+		tx_dsg->addr	= isert_cmd->sense_buf_dma;
+		tx_dsg->length	= sense_len;
+		tx_dsg->lkey	= isert_conn->conn_mr->lkey;
+		isert_cmd->tx_desc.num_sge = 2;
+	}
+
+	isert_init_send_wr(isert_cmd, send_wr);
+
+	pr_debug("Posting SCSI Response IB_WR_SEND >>>>>>>>>>>>>>>>>>>>>>\n");
+
+	return isert_post_response(isert_conn, isert_cmd);
+}
+
+static int
+isert_put_nopin(struct iscsi_cmd *cmd, struct iscsi_conn *conn,
+		bool nopout_response)
+{
+	struct isert_cmd *isert_cmd = container_of(cmd,
+				struct isert_cmd, iscsi_cmd);
+	struct isert_conn *isert_conn = (struct isert_conn *)conn->context;
+	struct ib_send_wr *send_wr = &isert_cmd->tx_desc.send_wr;
+
+	isert_create_send_desc(isert_conn, isert_cmd, &isert_cmd->tx_desc);
+	iscsit_build_nopin_rsp(cmd, conn, (struct iscsi_nopin *)
+			       &isert_cmd->tx_desc.iscsi_header,
+			       nopout_response);
+	isert_init_tx_hdrs(isert_conn, &isert_cmd->tx_desc);
+	isert_init_send_wr(isert_cmd, send_wr);
+
+	pr_debug("Posting NOPIN Reponse IB_WR_SEND >>>>>>>>>>>>>>>>>>>>>>\n");
+
+	return isert_post_response(isert_conn, isert_cmd);
+}
+
+static int
+isert_put_logout_rsp(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
+{
+	struct isert_cmd *isert_cmd = container_of(cmd,
+				struct isert_cmd, iscsi_cmd);
+	struct isert_conn *isert_conn = (struct isert_conn *)conn->context;
+	struct ib_send_wr *send_wr = &isert_cmd->tx_desc.send_wr;
+
+	isert_create_send_desc(isert_conn, isert_cmd, &isert_cmd->tx_desc);
+	iscsit_build_logout_rsp(cmd, conn, (struct iscsi_logout_rsp *)
+				&isert_cmd->tx_desc.iscsi_header);
+	isert_init_tx_hdrs(isert_conn, &isert_cmd->tx_desc);
+	isert_init_send_wr(isert_cmd, send_wr);
+
+	pr_debug("Posting Logout Response IB_WR_SEND >>>>>>>>>>>>>>>>>>>>>>\n");
+
+	return isert_post_response(isert_conn, isert_cmd);
+}
+
+static int
+isert_put_tm_rsp(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
+{
+	struct isert_cmd *isert_cmd = container_of(cmd,
+				struct isert_cmd, iscsi_cmd);
+	struct isert_conn *isert_conn = (struct isert_conn *)conn->context;
+	struct ib_send_wr *send_wr = &isert_cmd->tx_desc.send_wr;
+
+	isert_create_send_desc(isert_conn, isert_cmd, &isert_cmd->tx_desc);
+	iscsit_build_task_mgt_rsp(cmd, conn, (struct iscsi_tm_rsp *)
+				  &isert_cmd->tx_desc.iscsi_header);
+	isert_init_tx_hdrs(isert_conn, &isert_cmd->tx_desc);
+	isert_init_send_wr(isert_cmd, send_wr);
+
+	pr_debug("Posting Task Management Response IB_WR_SEND >>>>>>>>>>>>>>>>>>>>>>\n");
+
+	return isert_post_response(isert_conn, isert_cmd);
+}
+
+static int
+isert_put_reject(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
+{
+	struct isert_cmd *isert_cmd = container_of(cmd,
+				struct isert_cmd, iscsi_cmd);
+	struct isert_conn *isert_conn = (struct isert_conn *)conn->context;
+	struct ib_send_wr *send_wr = &isert_cmd->tx_desc.send_wr;
+
+	isert_create_send_desc(isert_conn, isert_cmd, &isert_cmd->tx_desc);
+	iscsit_build_reject(cmd, conn, (struct iscsi_reject *)
+				&isert_cmd->tx_desc.iscsi_header);
+	isert_init_tx_hdrs(isert_conn, &isert_cmd->tx_desc);
+	isert_init_send_wr(isert_cmd, send_wr);
+
+	pr_debug("Posting Reject IB_WR_SEND >>>>>>>>>>>>>>>>>>>>>>\n");
+
+	return isert_post_response(isert_conn, isert_cmd);
+}
+
+static int
+isert_build_rdma_wr(struct isert_conn *isert_conn, struct isert_cmd *isert_cmd,
+		    struct ib_sge *ib_sge, struct ib_send_wr *send_wr,
+		    u32 data_left, u32 offset)
+{
+	struct iscsi_cmd *cmd = &isert_cmd->iscsi_cmd;
+	struct scatterlist *sg_start, *tmp_sg;
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+	u32 sg_off, page_off;
+	int i = 0, sg_nents;
+
+	sg_off = offset / PAGE_SIZE;
+	sg_start = &cmd->se_cmd.t_data_sg[sg_off];
+	sg_nents = min(cmd->se_cmd.t_data_nents - sg_off, isert_conn->max_sge);
+	page_off = offset % PAGE_SIZE;
+
+	send_wr->sg_list = ib_sge;
+	send_wr->num_sge = sg_nents;
+	send_wr->wr_id = (unsigned long)&isert_cmd->tx_desc;
+	/*
+	 * Perform mapping of TCM scatterlist memory ib_sge dma_addr.
+	 */
+	for_each_sg(sg_start, tmp_sg, sg_nents, i) {
+		pr_debug("ISER RDMA from SGL dma_addr: 0x%16llx dma_len: %u, page_off: %u\n",
+			 (unsigned long long)tmp_sg->dma_address,
+			 tmp_sg->length, page_off);
+
+		ib_sge->addr = ib_sg_dma_address(ib_dev, tmp_sg) + page_off;
+		ib_sge->length = min_t(u32, data_left,
+				ib_sg_dma_len(ib_dev, tmp_sg) - page_off);
+		ib_sge->lkey = isert_conn->conn_mr->lkey;
+
+		pr_debug("RDMA ib_sge: addr: 0x%16llx  length: %u\n",
+			 ib_sge->addr, ib_sge->length);
+		page_off = 0;
+		data_left -= ib_sge->length;
+		ib_sge++;
+		pr_debug("Incrementing ib_sge pointer to %p\n", ib_sge);
+	}
+
+	pr_debug("Set outgoing sg_list: %p num_sg: %u from TCM SGLs\n",
+		 send_wr->sg_list, send_wr->num_sge);
+
+	return sg_nents;
+}
+
+static int
+isert_put_datain(struct iscsi_conn *conn, struct iscsi_cmd *cmd)
+{
+	struct se_cmd *se_cmd = &cmd->se_cmd;
+	struct isert_cmd *isert_cmd = container_of(cmd,
+					struct isert_cmd, iscsi_cmd);
+	struct isert_rdma_wr *wr = &isert_cmd->rdma_wr;
+	struct isert_conn *isert_conn = (struct isert_conn *)conn->context;
+	struct ib_send_wr *wr_failed, *send_wr;
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+	struct ib_sge *ib_sge;
+	struct scatterlist *sg;
+	u32 offset = 0, data_len, data_left, rdma_write_max;
+	int rc, ret = 0, count, sg_nents, i, ib_sge_cnt;
+
+	pr_debug("RDMA_WRITE: data_length: %u\n", se_cmd->data_length);
+
+	sg = &se_cmd->t_data_sg[0];
+	sg_nents = se_cmd->t_data_nents;
+
+	count = ib_dma_map_sg(ib_dev, sg, sg_nents, DMA_TO_DEVICE);
+	if (unlikely(!count)) {
+		pr_err("Unable to map put_datain SGs\n");
+		return -EINVAL;
+	}
+	wr->sge = sg;
+	wr->num_sge = sg_nents;
+	pr_debug("Mapped IB count: %u sg: %p sg_nents: %u for RDMA_WRITE\n",
+		 count, sg, sg_nents);
+
+	ib_sge = kzalloc(sizeof(struct ib_sge) * sg_nents, GFP_KERNEL);
+	if (!ib_sge) {
+		pr_warn("Unable to allocate datain ib_sge\n");
+		ret = -ENOMEM;
+		goto unmap_sg;
+	}
+	isert_cmd->ib_sge = ib_sge;
+
+	pr_debug("Allocated ib_sge: %p from t_data_ents: %d for RDMA_WRITE\n",
+		 ib_sge, se_cmd->t_data_nents);
+
+	wr->send_wr_num = DIV_ROUND_UP(sg_nents, isert_conn->max_sge);
+	wr->send_wr = kzalloc(sizeof(struct ib_send_wr) * wr->send_wr_num,
+				GFP_KERNEL);
+	if (!wr->send_wr) {
+		pr_err("Unable to allocate wr->send_wr\n");
+		ret = -ENOMEM;
+		goto unmap_sg;
+	}
+	pr_debug("Allocated wr->send_wr: %p wr->send_wr_num: %u\n",
+		 wr->send_wr, wr->send_wr_num);
+
+	iscsit_increment_maxcmdsn(cmd, conn->sess);
+	cmd->stat_sn = conn->stat_sn++;
+
+	wr->isert_cmd = isert_cmd;
+	rdma_write_max = isert_conn->max_sge * PAGE_SIZE;
+	data_left = se_cmd->data_length;
+
+	for (i = 0; i < wr->send_wr_num; i++) {
+		send_wr = &isert_cmd->rdma_wr.send_wr[i];
+		data_len = min(data_left, rdma_write_max);
+
+		send_wr->opcode = IB_WR_RDMA_WRITE;
+		send_wr->send_flags = 0;
+		send_wr->wr.rdma.remote_addr = isert_cmd->read_va + offset;
+		send_wr->wr.rdma.rkey = isert_cmd->read_stag;
+
+		ib_sge_cnt = isert_build_rdma_wr(isert_conn, isert_cmd, ib_sge,
+					send_wr, data_len, offset);
+		ib_sge += ib_sge_cnt;
+
+		if (i + 1 == wr->send_wr_num)
+			send_wr->next = &isert_cmd->tx_desc.send_wr;
+		else
+			send_wr->next = &wr->send_wr[i + 1];
+
+		offset += data_len;
+		data_left -= data_len;
+	}
+	/*
+	 * Build isert_conn->tx_desc for iSCSI response PDU and attach
+	 */
+	isert_create_send_desc(isert_conn, isert_cmd, &isert_cmd->tx_desc);
+	iscsit_build_rsp_pdu(cmd, conn, false, (struct iscsi_scsi_rsp *)
+			     &isert_cmd->tx_desc.iscsi_header);
+	isert_init_tx_hdrs(isert_conn, &isert_cmd->tx_desc);
+	isert_init_send_wr(isert_cmd, &isert_cmd->tx_desc.send_wr);
+
+	atomic_inc(&isert_conn->post_send_buf_count);
+
+	rc = ib_post_send(isert_conn->conn_qp, wr->send_wr, &wr_failed);
+	if (rc) {
+		pr_warn("ib_post_send() failed for IB_WR_RDMA_WRITE\n");
+		atomic_dec(&isert_conn->post_send_buf_count);
+	}
+	pr_debug("Posted RDMA_WRITE + Response for iSER Data READ\n");
+	return 1;
+
+unmap_sg:
+	ib_dma_unmap_sg(ib_dev, sg, sg_nents, DMA_TO_DEVICE);
+	return ret;
+}
+
+static int
+isert_get_dataout(struct iscsi_conn *conn, struct iscsi_cmd *cmd, bool recovery)
+{
+	struct se_cmd *se_cmd = &cmd->se_cmd;
+	struct isert_cmd *isert_cmd = container_of(cmd,
+					struct isert_cmd, iscsi_cmd);
+	struct isert_rdma_wr *wr = &isert_cmd->rdma_wr;
+	struct isert_conn *isert_conn = (struct isert_conn *)conn->context;
+	struct ib_send_wr *wr_failed, *send_wr;
+	struct ib_sge *ib_sge;
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+	struct scatterlist *sg_start;
+	u32 sg_off, sg_nents, page_off, va_offset = 0;
+	u32 offset = 0, data_len, data_left, rdma_write_max;
+	int rc, ret = 0, count, i, ib_sge_cnt;
+
+	pr_debug("RDMA_READ: data_length: %u write_data_done: %u\n",
+		 se_cmd->data_length, cmd->write_data_done);
+
+	sg_off = cmd->write_data_done / PAGE_SIZE;
+	sg_start = &cmd->se_cmd.t_data_sg[sg_off];
+	page_off = cmd->write_data_done % PAGE_SIZE;
+
+	pr_debug("RDMA_READ: sg_off: %d, sg_start: %p page_off: %d\n",
+		 sg_off, sg_start, page_off);
+
+	data_left = se_cmd->data_length - cmd->write_data_done;
+	sg_nents = se_cmd->t_data_nents - sg_off;
+
+	pr_debug("RDMA_READ: data_left: %d, sg_nents: %d\n",
+		 data_left, sg_nents);
+
+	count = ib_dma_map_sg(ib_dev, sg_start, sg_nents, DMA_FROM_DEVICE);
+	if (unlikely(!count)) {
+		pr_err("Unable to map get_dataout SGs\n");
+		return -EINVAL;
+	}
+	wr->sge = sg_start;
+	wr->num_sge = sg_nents;
+	pr_debug("Mapped IB count: %u sg_start: %p sg_nents: %u for RDMA_READ\n",
+		 count, sg_start, sg_nents);
+
+	ib_sge = kzalloc(sizeof(struct ib_sge) * sg_nents, GFP_KERNEL);
+	if (!ib_sge) {
+		pr_warn("Unable to allocate dataout ib_sge\n");
+		ret = -ENOMEM;
+		goto unmap_sg;
+	}
+	isert_cmd->ib_sge = ib_sge;
+
+	pr_debug("Using ib_sge: %p from sg_ents: %d for RDMA_READ\n",
+		 ib_sge, sg_nents);
+
+	wr->send_wr_num = DIV_ROUND_UP(sg_nents, isert_conn->max_sge);
+	wr->send_wr = kzalloc(sizeof(struct ib_send_wr) * wr->send_wr_num,
+				GFP_KERNEL);
+	if (!wr->send_wr) {
+		pr_debug("Unable to allocate wr->send_wr\n");
+		ret = -ENOMEM;
+		goto unmap_sg;
+	}
+	pr_debug("Allocated wr->send_wr: %p wr->send_wr_num: %u\n",
+		 wr->send_wr, wr->send_wr_num);
+
+	isert_cmd->tx_desc.isert_cmd = isert_cmd;
+
+	wr->iser_ib_op = ISER_IB_RDMA_READ;
+	wr->isert_cmd = isert_cmd;
+	rdma_write_max = isert_conn->max_sge * PAGE_SIZE;
+	offset = cmd->write_data_done;
+
+	for (i = 0; i < wr->send_wr_num; i++) {
+		send_wr = &isert_cmd->rdma_wr.send_wr[i];
+		data_len = min(data_left, rdma_write_max);
+
+		send_wr->opcode = IB_WR_RDMA_READ;
+		send_wr->wr.rdma.remote_addr = isert_cmd->write_va + va_offset;
+		send_wr->wr.rdma.rkey = isert_cmd->write_stag;
+
+		ib_sge_cnt = isert_build_rdma_wr(isert_conn, isert_cmd, ib_sge,
+					send_wr, data_len, offset);
+		ib_sge += ib_sge_cnt;
+
+		if (i + 1 == wr->send_wr_num)
+			send_wr->send_flags = IB_SEND_SIGNALED;
+		else
+			send_wr->next = &wr->send_wr[i + 1];
+
+		offset += data_len;
+		va_offset += data_len;
+		data_left -= data_len;
+	}
+
+	atomic_inc(&isert_conn->post_send_buf_count);
+
+	rc = ib_post_send(isert_conn->conn_qp, wr->send_wr, &wr_failed);
+	if (rc) {
+		pr_warn("ib_post_send() failed for IB_WR_RDMA_READ\n");
+		atomic_dec(&isert_conn->post_send_buf_count);
+	}
+	pr_debug("Posted RDMA_READ memory for ISER Data WRITE\n");
+	return 0;
+
+unmap_sg:
+	ib_dma_unmap_sg(ib_dev, sg_start, sg_nents, DMA_FROM_DEVICE);
+	return ret;
+}
+
+static int
+isert_immediate_queue(struct iscsi_conn *conn, struct iscsi_cmd *cmd, int state)
+{
+	int ret;
+
+	switch (state) {
+	case ISTATE_SEND_NOPIN_WANT_RESPONSE:
+		ret = isert_put_nopin(cmd, conn, false);
+		break;
+	default:
+		pr_err("Unknown immediate state: 0x%02x\n", state);
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static int
+isert_response_queue(struct iscsi_conn *conn, struct iscsi_cmd *cmd, int state)
+{
+	int ret;
+
+	switch (state) {
+	case ISTATE_SEND_LOGOUTRSP:
+		ret = isert_put_logout_rsp(cmd, conn);
+		if (!ret) {
+			pr_debug("Returning iSER Logout -EAGAIN\n");
+			ret = -EAGAIN;
+		}
+		break;
+	case ISTATE_SEND_NOPIN:
+		ret = isert_put_nopin(cmd, conn, true);
+		break;
+	case ISTATE_SEND_TASKMGTRSP:
+		ret = isert_put_tm_rsp(cmd, conn);
+		break;
+	case ISTATE_SEND_REJECT:
+		ret = isert_put_reject(cmd, conn);
+		break;
+	case ISTATE_SEND_STATUS:
+		/*
+		 * Special case for sending non GOOD SCSI status from TX thread
+		 * context during pre se_cmd excecution failure.
+		 */
+		ret = isert_put_response(conn, cmd);
+		break;
+	default:
+		pr_err("Unknown response state: 0x%02x\n", state);
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static int
+isert_setup_np(struct iscsi_np *np,
+	       struct __kernel_sockaddr_storage *ksockaddr)
+{
+	struct isert_np *isert_np;
+	struct rdma_cm_id *isert_lid;
+	struct sockaddr *sa;
+	int ret;
+
+	isert_np = kzalloc(sizeof(struct isert_np), GFP_KERNEL);
+	if (!isert_np) {
+		pr_err("Unable to allocate struct isert_np\n");
+		return -ENOMEM;
+	}
+	init_waitqueue_head(&isert_np->np_accept_wq);
+	mutex_init(&isert_np->np_accept_mutex);
+	INIT_LIST_HEAD(&isert_np->np_accept_list);
+	init_completion(&isert_np->np_login_comp);
+
+	sa = (struct sockaddr *)ksockaddr;
+	pr_debug("ksockaddr: %p, sa: %p\n", ksockaddr, sa);
+	/*
+	 * Setup the np->np_sockaddr from the passed sockaddr setup
+	 * in iscsi_target_configfs.c code..
+	 */
+	memcpy(&np->np_sockaddr, ksockaddr,
+	       sizeof(struct __kernel_sockaddr_storage));
+
+	isert_lid = rdma_create_id(isert_cma_handler, np, RDMA_PS_TCP,
+				IB_QPT_RC);
+	if (IS_ERR(isert_lid)) {
+		pr_err("rdma_create_id() for isert_listen_handler failed: %ld\n",
+		       PTR_ERR(isert_lid));
+		ret = PTR_ERR(isert_lid);
+		goto out;
+	}
+
+	ret = rdma_bind_addr(isert_lid, sa);
+	if (ret) {
+		pr_err("rdma_bind_addr() for isert_lid failed: %d\n", ret);
+		goto out_lid;
+	}
+
+	ret = rdma_listen(isert_lid, ISERT_RDMA_LISTEN_BACKLOG);
+	if (ret) {
+		pr_err("rdma_listen() for isert_lid failed: %d\n", ret);
+		goto out_lid;
+	}
+
+	isert_np->np_cm_id = isert_lid;
+	np->np_context = isert_np;
+	pr_debug("Setup isert_lid->context: %p\n", isert_lid->context);
+
+	return 0;
+
+out_lid:
+	rdma_destroy_id(isert_lid);
+out:
+	kfree(isert_np);
+	return ret;
+}
+
+static int
+isert_check_accept_queue(struct isert_np *isert_np)
+{
+	int empty;
+
+	mutex_lock(&isert_np->np_accept_mutex);
+	empty = list_empty(&isert_np->np_accept_list);
+	mutex_unlock(&isert_np->np_accept_mutex);
+
+	return empty;
+}
+
+static int
+isert_rdma_accept(struct isert_conn *isert_conn)
+{
+	struct rdma_cm_id *cm_id = isert_conn->conn_cm_id;
+	struct rdma_conn_param cp;
+	int ret;
+
+	memset(&cp, 0, sizeof(struct rdma_conn_param));
+	cp.responder_resources = isert_conn->responder_resources;
+	cp.initiator_depth = isert_conn->initiator_depth;
+	cp.retry_count = 7;
+	cp.rnr_retry_count = 7;
+
+	pr_debug("Before rdma_accept >>>>>>>>>>>>>>>>>>>>.\n");
+
+	ret = rdma_accept(cm_id, &cp);
+	if (ret) {
+		pr_err("rdma_accept() failed with: %d\n", ret);
+		return ret;
+	}
+
+	pr_debug("After rdma_accept >>>>>>>>>>>>>>>>>>>>>.\n");
+
+	return 0;
+}
+
+static int
+isert_get_login_rx(struct iscsi_conn *conn, struct iscsi_login *login)
+{
+	struct isert_conn *isert_conn = (struct isert_conn *)conn->context;
+	int ret;
+
+	pr_debug("isert_get_login_rx before conn_login_comp conn: %p\n", conn);
+
+	ret = wait_for_completion_interruptible(&isert_conn->conn_login_comp);
+	if (ret)
+		return ret;
+
+	pr_debug("isert_get_login_rx processing login->req: %p\n", login->req);
+	return 0;
+}
+
+static void
+isert_set_conn_info(struct iscsi_np *np, struct iscsi_conn *conn,
+		    struct isert_conn *isert_conn)
+{
+	struct rdma_cm_id *cm_id = isert_conn->conn_cm_id;
+	struct rdma_route *cm_route = &cm_id->route;
+	struct sockaddr_in *sock_in;
+	struct sockaddr_in6 *sock_in6;
+
+	conn->login_family = np->np_sockaddr.ss_family;
+
+	if (np->np_sockaddr.ss_family == AF_INET6) {
+		sock_in6 = (struct sockaddr_in6 *)&cm_route->addr.dst_addr;
+		snprintf(conn->login_ip, sizeof(conn->login_ip), "%pI6c",
+			 &sock_in6->sin6_addr.in6_u);
+		conn->login_port = ntohs(sock_in6->sin6_port);
+
+		sock_in6 = (struct sockaddr_in6 *)&cm_route->addr.src_addr;
+		snprintf(conn->local_ip, sizeof(conn->local_ip), "%pI6c",
+			 &sock_in6->sin6_addr.in6_u);
+		conn->local_port = ntohs(sock_in6->sin6_port);
+	} else {
+		sock_in = (struct sockaddr_in *)&cm_route->addr.dst_addr;
+		sprintf(conn->login_ip, "%pI4",
+			&sock_in->sin_addr.s_addr);
+		conn->login_port = ntohs(sock_in->sin_port);
+
+		sock_in = (struct sockaddr_in *)&cm_route->addr.src_addr;
+		sprintf(conn->local_ip, "%pI4",
+			&sock_in->sin_addr.s_addr);
+		conn->local_port = ntohs(sock_in->sin_port);
+	}
+}
+
+static int
+isert_accept_np(struct iscsi_np *np, struct iscsi_conn *conn)
+{
+	struct isert_np *isert_np = (struct isert_np *)np->np_context;
+	struct isert_conn *isert_conn;
+	int max_accept = 0, ret;
+
+accept_wait:
+	ret = wait_event_interruptible(isert_np->np_accept_wq,
+			!isert_check_accept_queue(isert_np) ||
+			np->np_thread_state == ISCSI_NP_THREAD_RESET);
+	if (max_accept > 5)
+		return -ENODEV;
+
+	spin_lock_bh(&np->np_thread_lock);
+	if (np->np_thread_state == ISCSI_NP_THREAD_RESET) {
+		spin_unlock_bh(&np->np_thread_lock);
+		pr_err("ISCSI_NP_THREAD_RESET for isert_accept_np\n");
+		return -ENODEV;
+	}
+	spin_unlock_bh(&np->np_thread_lock);
+
+	mutex_lock(&isert_np->np_accept_mutex);
+	if (list_empty(&isert_np->np_accept_list)) {
+		mutex_unlock(&isert_np->np_accept_mutex);
+		max_accept++;
+		goto accept_wait;
+	}
+	isert_conn = list_first_entry(&isert_np->np_accept_list,
+			struct isert_conn, conn_accept_node);
+	list_del_init(&isert_conn->conn_accept_node);
+	mutex_unlock(&isert_np->np_accept_mutex);
+
+	conn->context = isert_conn;
+	isert_conn->conn = conn;
+	max_accept = 0;
+
+	ret = isert_rdma_post_recvl(isert_conn);
+	if (ret)
+		return ret;
+
+	ret = isert_rdma_accept(isert_conn);
+	if (ret)
+		return ret;
+
+	isert_set_conn_info(np, conn, isert_conn);
+
+	pr_debug("Processing isert_accept_np: isert_conn: %p\n", isert_conn);
+	return 0;
+}
+
+static void
+isert_free_np(struct iscsi_np *np)
+{
+	struct isert_np *isert_np = (struct isert_np *)np->np_context;
+
+	rdma_destroy_id(isert_np->np_cm_id);
+
+	np->np_context = NULL;
+	kfree(isert_np);
+}
+
+static void isert_free_conn(struct iscsi_conn *conn)
+{
+	struct isert_conn *isert_conn = conn->context;
+
+	pr_debug("isert_free_conn: Starting \n");
+	/*
+	 * Decrement post_send_buf_count for special case when called
+	 * from isert_do_control_comp() -> iscsit_logout_post_handler()
+	 */
+	if (isert_conn->logout_posted)
+		atomic_dec(&isert_conn->post_send_buf_count);
+
+	if (isert_conn->conn_cm_id)
+		rdma_disconnect(isert_conn->conn_cm_id);
+	/*
+	 * Only wait for conn_wait_comp_err if the isert_conn made it
+	 * into full feature phase..
+	 */
+	if (isert_conn->state > ISER_CONN_INIT) {
+		pr_debug("isert_free_conn: Before wait_event comp_err %d\n",
+			 isert_conn->state);
+		wait_event(isert_conn->conn_wait_comp_err,
+			   isert_conn->state == ISER_CONN_TERMINATING);
+		pr_debug("isert_free_conn: After wait_event #1 >>>>>>>>>>>>\n");
+	}
+
+	pr_debug("isert_free_conn: wait_event conn_wait %d\n", isert_conn->state);
+	wait_event(isert_conn->conn_wait, isert_conn->state == ISER_CONN_DOWN);
+	pr_debug("isert_free_conn: After wait_event #2 >>>>>>>>>>>>>>>>>>>>\n");
+
+	isert_put_conn(isert_conn);
+}
+
+static struct iscsit_transport iser_target_transport = {
+	.name			= "IB/iSER",
+	.transport_type		= ISCSI_INFINIBAND,
+	.owner			= THIS_MODULE,
+	.iscsit_setup_np	= isert_setup_np,
+	.iscsit_accept_np	= isert_accept_np,
+	.iscsit_free_np		= isert_free_np,
+	.iscsit_free_conn	= isert_free_conn,
+	.iscsit_alloc_cmd	= isert_alloc_cmd,
+	.iscsit_get_login_rx	= isert_get_login_rx,
+	.iscsit_put_login_tx	= isert_put_login_tx,
+	.iscsit_immediate_queue	= isert_immediate_queue,
+	.iscsit_response_queue	= isert_response_queue,
+	.iscsit_get_dataout	= isert_get_dataout,
+	.iscsit_queue_data_in	= isert_put_datain,
+	.iscsit_queue_status	= isert_put_response,
+};
+
+static int __init isert_init(void)
+{
+	int ret;
+
+	isert_rx_wq = alloc_workqueue("isert_rx_wq", 0, 0);
+	if (!isert_rx_wq) {
+		pr_err("Unable to allocate isert_rx_wq\n");
+		return -ENOMEM;
+	}
+
+	isert_comp_wq = alloc_workqueue("isert_comp_wq", 0, 0);
+	if (!isert_comp_wq) {
+		pr_err("Unable to allocate isert_comp_wq\n");
+		ret = -ENOMEM;
+		goto destroy_rx_wq;
+	}
+
+	isert_cmd_cache = kmem_cache_create("isert_cmd_cache",
+			sizeof(struct isert_cmd), __alignof__(struct isert_cmd),
+			0, NULL);
+	if (!isert_cmd_cache) {
+		pr_err("Unable to create isert_cmd_cache\n");
+		ret = -ENOMEM;
+		goto destroy_tx_cq;
+	}
+
+	iscsit_register_transport(&iser_target_transport);
+	pr_debug("iSER_TARGET[0] - Loaded iser_target_transport\n");
+	return 0;
+
+destroy_tx_cq:
+	destroy_workqueue(isert_comp_wq);
+destroy_rx_wq:
+	destroy_workqueue(isert_rx_wq);
+	return ret;
+}
+
+static void __exit isert_exit(void)
+{
+	kmem_cache_destroy(isert_cmd_cache);
+	destroy_workqueue(isert_comp_wq);
+	destroy_workqueue(isert_rx_wq);
+	iscsit_unregister_transport(&iser_target_transport);
+	pr_debug("iSER_TARGET[0] - Released iser_target_transport\n");
+}
+
+MODULE_DESCRIPTION("iSER-Target for mainline target infrastructure");
+MODULE_VERSION("0.1");
+MODULE_AUTHOR("nab@Linux-iSCSI.org");
+MODULE_LICENSE("GPL");
+
+module_init(isert_init);
+module_exit(isert_exit);
diff --git a/drivers/infiniband/ulp/isert/ib_isert.h b/drivers/infiniband/ulp/isert/ib_isert.h
new file mode 100644
index 00000000000..b104f4c2cd3
--- /dev/null
+++ b/drivers/infiniband/ulp/isert/ib_isert.h
@@ -0,0 +1,138 @@
+#include <linux/socket.h>
+#include <linux/in.h>
+#include <linux/in6.h>
+#include <rdma/ib_verbs.h>
+#include <rdma/rdma_cm.h>
+
+#define ISERT_RDMA_LISTEN_BACKLOG	10
+
+enum isert_desc_type {
+	ISCSI_TX_CONTROL,
+	ISCSI_TX_DATAIN
+};
+
+enum iser_ib_op_code {
+	ISER_IB_RECV,
+	ISER_IB_SEND,
+	ISER_IB_RDMA_WRITE,
+	ISER_IB_RDMA_READ,
+};
+
+enum iser_conn_state {
+	ISER_CONN_INIT,
+	ISER_CONN_UP,
+	ISER_CONN_TERMINATING,
+	ISER_CONN_DOWN,
+};
+
+struct iser_rx_desc {
+	struct iser_hdr iser_header;
+	struct iscsi_hdr iscsi_header;
+	char		data[ISER_RECV_DATA_SEG_LEN];
+	u64		dma_addr;
+	struct ib_sge	rx_sg;
+	char		pad[ISER_RX_PAD_SIZE];
+} __packed;
+
+struct iser_tx_desc {
+	struct iser_hdr iser_header;
+	struct iscsi_hdr iscsi_header;
+	enum isert_desc_type type;
+	u64		dma_addr;
+	struct ib_sge	tx_sg[2];
+	int		num_sge;
+	struct isert_cmd *isert_cmd;
+	struct ib_send_wr send_wr;
+} __packed;
+
+struct isert_rdma_wr {
+	struct list_head	wr_list;
+	struct isert_cmd	*isert_cmd;
+	enum iser_ib_op_code	iser_ib_op;
+	struct ib_sge		*ib_sge;
+	int			num_sge;
+	struct scatterlist	*sge;
+	int			send_wr_num;
+	struct ib_send_wr	*send_wr;
+};
+
+struct isert_cmd {
+	uint32_t		read_stag;
+	uint32_t		write_stag;
+	uint64_t		read_va;
+	uint64_t		write_va;
+	u64			sense_buf_dma;
+	u32			sense_buf_len;
+	u32			read_va_off;
+	u32			write_va_off;
+	u32			rdma_wr_num;
+	struct isert_conn	*conn;
+	struct iscsi_cmd	iscsi_cmd;
+	struct ib_sge		*ib_sge;
+	struct iser_tx_desc	tx_desc;
+	struct isert_rdma_wr	rdma_wr;
+	struct work_struct	comp_work;
+};
+
+struct isert_device;
+
+struct isert_conn {
+	enum iser_conn_state	state;
+	bool			logout_posted;
+	int			post_recv_buf_count;
+	atomic_t		post_send_buf_count;
+	u32			responder_resources;
+	u32			initiator_depth;
+	u32			max_sge;
+	char			*login_buf;
+	char			*login_req_buf;
+	char			*login_rsp_buf;
+	u64			login_req_dma;
+	u64			login_rsp_dma;
+	unsigned int		conn_rx_desc_head;
+	struct iser_rx_desc	*conn_rx_descs;
+	struct ib_recv_wr	conn_rx_wr[ISERT_MIN_POSTED_RX];
+	struct iscsi_conn	*conn;
+	struct list_head	conn_accept_node;
+	struct completion	conn_login_comp;
+	struct iser_tx_desc	conn_login_tx_desc;
+	struct rdma_cm_id	*conn_cm_id;
+	struct ib_pd		*conn_pd;
+	struct ib_mr		*conn_mr;
+	struct ib_qp		*conn_qp;
+	struct isert_device	*conn_device;
+	struct work_struct	conn_logout_work;
+	wait_queue_head_t	conn_wait;
+	wait_queue_head_t	conn_wait_comp_err;
+	struct kref		conn_kref;
+};
+
+#define ISERT_MAX_CQ 64
+
+struct isert_cq_desc {
+	struct isert_device	*device;
+	int			cq_index;
+	struct work_struct	cq_rx_work;
+	struct work_struct	cq_tx_work;
+};
+
+struct isert_device {
+	int			cqs_used;
+	int			refcount;
+	int			cq_active_qps[ISERT_MAX_CQ];
+	struct ib_device	*ib_device;
+	struct ib_pd		*dev_pd;
+	struct ib_mr		*dev_mr;
+	struct ib_cq		*dev_rx_cq[ISERT_MAX_CQ];
+	struct ib_cq		*dev_tx_cq[ISERT_MAX_CQ];
+	struct isert_cq_desc	*cq_desc;
+	struct list_head	dev_node;
+};
+
+struct isert_np {
+	wait_queue_head_t	np_accept_wq;
+	struct rdma_cm_id	*np_cm_id;
+	struct mutex		np_accept_mutex;
+	struct list_head	np_accept_list;
+	struct completion	np_login_comp;
+};
diff --git a/drivers/infiniband/ulp/isert/isert_proto.h b/drivers/infiniband/ulp/isert/isert_proto.h
new file mode 100644
index 00000000000..4dccd313b77
--- /dev/null
+++ b/drivers/infiniband/ulp/isert/isert_proto.h
@@ -0,0 +1,47 @@
+/* From iscsi_iser.h */
+
+struct iser_hdr {
+	u8	flags;
+	u8	rsvd[3];
+	__be32	write_stag; /* write rkey */
+	__be64	write_va;
+	__be32	read_stag;  /* read rkey */
+	__be64	read_va;
+} __packed;
+
+/*Constant PDU lengths calculations */
+#define ISER_HEADERS_LEN  (sizeof(struct iser_hdr) + sizeof(struct iscsi_hdr))
+
+#define ISER_RECV_DATA_SEG_LEN  8192
+#define ISER_RX_PAYLOAD_SIZE    (ISER_HEADERS_LEN + ISER_RECV_DATA_SEG_LEN)
+#define ISER_RX_LOGIN_SIZE      (ISER_HEADERS_LEN + ISCSI_DEF_MAX_RECV_SEG_LEN)
+
+/* QP settings */
+/* Maximal bounds on received asynchronous PDUs */
+#define ISERT_MAX_TX_MISC_PDUS	4 /* NOOP_IN(2) , ASYNC_EVENT(2)   */
+
+#define ISERT_MAX_RX_MISC_PDUS	6 /* NOOP_OUT(2), TEXT(1),         *
+				   * SCSI_TMFUNC(2), LOGOUT(1) */
+
+#define ISCSI_DEF_XMIT_CMDS_MAX 128 /* from libiscsi.h, must be power of 2 */
+
+#define ISERT_QP_MAX_RECV_DTOS	(ISCSI_DEF_XMIT_CMDS_MAX)
+
+#define ISERT_MIN_POSTED_RX	(ISCSI_DEF_XMIT_CMDS_MAX >> 2)
+
+#define ISERT_INFLIGHT_DATAOUTS	8
+
+#define ISERT_QP_MAX_REQ_DTOS	(ISCSI_DEF_XMIT_CMDS_MAX *    \
+				(1 + ISERT_INFLIGHT_DATAOUTS) + \
+				ISERT_MAX_TX_MISC_PDUS	+ \
+				ISERT_MAX_RX_MISC_PDUS)
+
+#define ISER_RX_PAD_SIZE	(ISER_RECV_DATA_SEG_LEN + 4096 - \
+		(ISER_RX_PAYLOAD_SIZE + sizeof(u64) + sizeof(struct ib_sge)))
+
+#define ISER_VER	0x10
+#define ISER_WSV	0x08
+#define ISER_RSV	0x04
+#define ISCSI_CTRL	0x10
+#define ISER_HELLO	0x20
+#define ISER_HELLORPLY	0x30
diff --git a/drivers/input/tablet/wacom_wac.c b/drivers/input/tablet/wacom_wac.c
index 1daa97913b7..0bfd8cf2520 100644
--- a/drivers/input/tablet/wacom_wac.c
+++ b/drivers/input/tablet/wacom_wac.c
@@ -359,7 +359,7 @@ static int wacom_intuos_inout(struct wacom_wac *wacom)
 		case 0x802: /* Intuos4 General Pen */
 		case 0x804: /* Intuos4 Marker Pen */
 		case 0x40802: /* Intuos4 Classic Pen */
-		case 0x18803: /* DTH2242 Grip Pen */
+		case 0x18802: /* DTH2242 Grip Pen */
 		case 0x022:
 			wacom->tool[idx] = BTN_TOOL_PEN;
 			break;
@@ -1912,7 +1912,7 @@ static const struct wacom_features wacom_features_0xBB =
 	{ "Wacom Intuos4 12x19",  WACOM_PKGLEN_INTUOS,    97536, 60960, 2047,
 	  63, INTUOS4L, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES };
 static const struct wacom_features wacom_features_0xBC =
-	{ "Wacom Intuos4 WL",     WACOM_PKGLEN_INTUOS,    40840, 25400, 2047,
+	{ "Wacom Intuos4 WL",     WACOM_PKGLEN_INTUOS,    40640, 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,
@@ -2144,7 +2144,7 @@ const struct usb_device_id wacom_ids[] = {
 	{ USB_DEVICE_WACOM(0x44) },
 	{ USB_DEVICE_WACOM(0x45) },
 	{ USB_DEVICE_WACOM(0x59) },
-	{ USB_DEVICE_WACOM(0x5D) },
+	{ USB_DEVICE_DETAILED(0x5D, USB_CLASS_HID, 0, 0) },
 	{ USB_DEVICE_WACOM(0xB0) },
 	{ USB_DEVICE_WACOM(0xB1) },
 	{ USB_DEVICE_WACOM(0xB2) },
@@ -2209,7 +2209,7 @@ const struct usb_device_id wacom_ids[] = {
 	{ USB_DEVICE_WACOM(0x47) },
 	{ USB_DEVICE_WACOM(0xF4) },
 	{ USB_DEVICE_WACOM(0xF8) },
-	{ USB_DEVICE_WACOM(0xF6) },
+	{ USB_DEVICE_DETAILED(0xF6, USB_CLASS_HID, 0, 0) },
 	{ USB_DEVICE_WACOM(0xFA) },
 	{ USB_DEVICE_LENOVO(0x6004) },
 	{ }
diff --git a/drivers/iommu/Kconfig b/drivers/iommu/Kconfig
index 5c514d0711d..c332fb98480 100644
--- a/drivers/iommu/Kconfig
+++ b/drivers/iommu/Kconfig
@@ -130,7 +130,7 @@ config IRQ_REMAP
 # OMAP IOMMU support
 config OMAP_IOMMU
 	bool "OMAP IOMMU Support"
-	depends on ARCH_OMAP
+	depends on ARCH_OMAP2PLUS
 	select IOMMU_API
 
 config OMAP_IOVMM
diff --git a/drivers/iommu/amd_iommu.c b/drivers/iommu/amd_iommu.c
index 98f555dafb5..830183737b0 100644
--- a/drivers/iommu/amd_iommu.c
+++ b/drivers/iommu/amd_iommu.c
@@ -173,7 +173,7 @@ static inline u16 get_device_id(struct device *dev)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
 
-	return calc_devid(pdev->bus->number, pdev->devfn);
+	return PCI_DEVID(pdev->bus->number, pdev->devfn);
 }
 
 static struct iommu_dev_data *get_dev_data(struct device *dev)
@@ -649,26 +649,26 @@ retry:
 	case EVENT_TYPE_ILL_DEV:
 		printk("ILLEGAL_DEV_TABLE_ENTRY device=%02x:%02x.%x "
 		       "address=0x%016llx flags=0x%04x]\n",
-		       PCI_BUS(devid), PCI_SLOT(devid), PCI_FUNC(devid),
+		       PCI_BUS_NUM(devid), PCI_SLOT(devid), PCI_FUNC(devid),
 		       address, flags);
 		dump_dte_entry(devid);
 		break;
 	case EVENT_TYPE_IO_FAULT:
 		printk("IO_PAGE_FAULT device=%02x:%02x.%x "
 		       "domain=0x%04x address=0x%016llx flags=0x%04x]\n",
-		       PCI_BUS(devid), PCI_SLOT(devid), PCI_FUNC(devid),
+		       PCI_BUS_NUM(devid), PCI_SLOT(devid), PCI_FUNC(devid),
 		       domid, address, flags);
 		break;
 	case EVENT_TYPE_DEV_TAB_ERR:
 		printk("DEV_TAB_HARDWARE_ERROR device=%02x:%02x.%x "
 		       "address=0x%016llx flags=0x%04x]\n",
-		       PCI_BUS(devid), PCI_SLOT(devid), PCI_FUNC(devid),
+		       PCI_BUS_NUM(devid), PCI_SLOT(devid), PCI_FUNC(devid),
 		       address, flags);
 		break;
 	case EVENT_TYPE_PAGE_TAB_ERR:
 		printk("PAGE_TAB_HARDWARE_ERROR device=%02x:%02x.%x "
 		       "domain=0x%04x address=0x%016llx flags=0x%04x]\n",
-		       PCI_BUS(devid), PCI_SLOT(devid), PCI_FUNC(devid),
+		       PCI_BUS_NUM(devid), PCI_SLOT(devid), PCI_FUNC(devid),
 		       domid, address, flags);
 		break;
 	case EVENT_TYPE_ILL_CMD:
@@ -682,13 +682,13 @@ retry:
 	case EVENT_TYPE_IOTLB_INV_TO:
 		printk("IOTLB_INV_TIMEOUT device=%02x:%02x.%x "
 		       "address=0x%016llx]\n",
-		       PCI_BUS(devid), PCI_SLOT(devid), PCI_FUNC(devid),
+		       PCI_BUS_NUM(devid), PCI_SLOT(devid), PCI_FUNC(devid),
 		       address);
 		break;
 	case EVENT_TYPE_INV_DEV_REQ:
 		printk("INVALID_DEVICE_REQUEST device=%02x:%02x.%x "
 		       "address=0x%016llx flags=0x%04x]\n",
-		       PCI_BUS(devid), PCI_SLOT(devid), PCI_FUNC(devid),
+		       PCI_BUS_NUM(devid), PCI_SLOT(devid), PCI_FUNC(devid),
 		       address, flags);
 		break;
 	default:
@@ -2466,18 +2466,16 @@ static int device_change_notifier(struct notifier_block *nb,
 
 		/* allocate a protection domain if a device is added */
 		dma_domain = find_protection_domain(devid);
-		if (dma_domain)
-			goto out;
-		dma_domain = dma_ops_domain_alloc();
-		if (!dma_domain)
-			goto out;
-		dma_domain->target_dev = devid;
-
-		spin_lock_irqsave(&iommu_pd_list_lock, flags);
-		list_add_tail(&dma_domain->list, &iommu_pd_list);
-		spin_unlock_irqrestore(&iommu_pd_list_lock, flags);
-
-		dev_data = get_dev_data(dev);
+		if (!dma_domain) {
+			dma_domain = dma_ops_domain_alloc();
+			if (!dma_domain)
+				goto out;
+			dma_domain->target_dev = devid;
+
+			spin_lock_irqsave(&iommu_pd_list_lock, flags);
+			list_add_tail(&dma_domain->list, &iommu_pd_list);
+			spin_unlock_irqrestore(&iommu_pd_list_lock, flags);
+		}
 
 		dev->archdata.dma_ops = &amd_iommu_dma_ops;
 
diff --git a/drivers/iommu/amd_iommu_init.c b/drivers/iommu/amd_iommu_init.c
index b6ecddb63cd..2f46881256a 100644
--- a/drivers/iommu/amd_iommu_init.c
+++ b/drivers/iommu/amd_iommu_init.c
@@ -406,7 +406,7 @@ static int __init find_last_devid_on_pci(int bus, int dev, int fn, int cap_ptr)
 	u32 cap;
 
 	cap = read_pci_config(bus, dev, fn, cap_ptr+MMIO_RANGE_OFFSET);
-	update_last_devid(calc_devid(MMIO_GET_BUS(cap), MMIO_GET_LD(cap)));
+	update_last_devid(PCI_DEVID(MMIO_GET_BUS(cap), MMIO_GET_LD(cap)));
 
 	return 0;
 }
@@ -423,7 +423,7 @@ static int __init find_last_devid_from_ivhd(struct ivhd_header *h)
 	p += sizeof(*h);
 	end += h->length;
 
-	find_last_devid_on_pci(PCI_BUS(h->devid),
+	find_last_devid_on_pci(PCI_BUS_NUM(h->devid),
 			PCI_SLOT(h->devid),
 			PCI_FUNC(h->devid),
 			h->cap_ptr);
@@ -784,10 +784,10 @@ static int __init init_iommu_from_acpi(struct amd_iommu *iommu,
 
 			DUMP_printk("  DEV_ALL\t\t\t first devid: %02x:%02x.%x"
 				    " last device %02x:%02x.%x flags: %02x\n",
-				    PCI_BUS(iommu->first_device),
+				    PCI_BUS_NUM(iommu->first_device),
 				    PCI_SLOT(iommu->first_device),
 				    PCI_FUNC(iommu->first_device),
-				    PCI_BUS(iommu->last_device),
+				    PCI_BUS_NUM(iommu->last_device),
 				    PCI_SLOT(iommu->last_device),
 				    PCI_FUNC(iommu->last_device),
 				    e->flags);
@@ -801,7 +801,7 @@ static int __init init_iommu_from_acpi(struct amd_iommu *iommu,
 
 			DUMP_printk("  DEV_SELECT\t\t\t devid: %02x:%02x.%x "
 				    "flags: %02x\n",
-				    PCI_BUS(e->devid),
+				    PCI_BUS_NUM(e->devid),
 				    PCI_SLOT(e->devid),
 				    PCI_FUNC(e->devid),
 				    e->flags);
@@ -813,7 +813,7 @@ static int __init init_iommu_from_acpi(struct amd_iommu *iommu,
 
 			DUMP_printk("  DEV_SELECT_RANGE_START\t "
 				    "devid: %02x:%02x.%x flags: %02x\n",
-				    PCI_BUS(e->devid),
+				    PCI_BUS_NUM(e->devid),
 				    PCI_SLOT(e->devid),
 				    PCI_FUNC(e->devid),
 				    e->flags);
@@ -827,11 +827,11 @@ static int __init init_iommu_from_acpi(struct amd_iommu *iommu,
 
 			DUMP_printk("  DEV_ALIAS\t\t\t devid: %02x:%02x.%x "
 				    "flags: %02x devid_to: %02x:%02x.%x\n",
-				    PCI_BUS(e->devid),
+				    PCI_BUS_NUM(e->devid),
 				    PCI_SLOT(e->devid),
 				    PCI_FUNC(e->devid),
 				    e->flags,
-				    PCI_BUS(e->ext >> 8),
+				    PCI_BUS_NUM(e->ext >> 8),
 				    PCI_SLOT(e->ext >> 8),
 				    PCI_FUNC(e->ext >> 8));
 
@@ -846,11 +846,11 @@ static int __init init_iommu_from_acpi(struct amd_iommu *iommu,
 			DUMP_printk("  DEV_ALIAS_RANGE\t\t "
 				    "devid: %02x:%02x.%x flags: %02x "
 				    "devid_to: %02x:%02x.%x\n",
-				    PCI_BUS(e->devid),
+				    PCI_BUS_NUM(e->devid),
 				    PCI_SLOT(e->devid),
 				    PCI_FUNC(e->devid),
 				    e->flags,
-				    PCI_BUS(e->ext >> 8),
+				    PCI_BUS_NUM(e->ext >> 8),
 				    PCI_SLOT(e->ext >> 8),
 				    PCI_FUNC(e->ext >> 8));
 
@@ -864,7 +864,7 @@ static int __init init_iommu_from_acpi(struct amd_iommu *iommu,
 
 			DUMP_printk("  DEV_EXT_SELECT\t\t devid: %02x:%02x.%x "
 				    "flags: %02x ext: %08x\n",
-				    PCI_BUS(e->devid),
+				    PCI_BUS_NUM(e->devid),
 				    PCI_SLOT(e->devid),
 				    PCI_FUNC(e->devid),
 				    e->flags, e->ext);
@@ -877,7 +877,7 @@ static int __init init_iommu_from_acpi(struct amd_iommu *iommu,
 
 			DUMP_printk("  DEV_EXT_SELECT_RANGE\t devid: "
 				    "%02x:%02x.%x flags: %02x ext: %08x\n",
-				    PCI_BUS(e->devid),
+				    PCI_BUS_NUM(e->devid),
 				    PCI_SLOT(e->devid),
 				    PCI_FUNC(e->devid),
 				    e->flags, e->ext);
@@ -890,7 +890,7 @@ static int __init init_iommu_from_acpi(struct amd_iommu *iommu,
 		case IVHD_DEV_RANGE_END:
 
 			DUMP_printk("  DEV_RANGE_END\t\t devid: %02x:%02x.%x\n",
-				    PCI_BUS(e->devid),
+				    PCI_BUS_NUM(e->devid),
 				    PCI_SLOT(e->devid),
 				    PCI_FUNC(e->devid));
 
@@ -924,7 +924,7 @@ static int __init init_iommu_from_acpi(struct amd_iommu *iommu,
 
 			DUMP_printk("  DEV_SPECIAL(%s[%d])\t\tdevid: %02x:%02x.%x\n",
 				    var, (int)handle,
-				    PCI_BUS(devid),
+				    PCI_BUS_NUM(devid),
 				    PCI_SLOT(devid),
 				    PCI_FUNC(devid));
 
@@ -980,7 +980,7 @@ static void __init free_iommu_all(void)
  *     BIOS should disable L2B micellaneous clock gating by setting
  *     L2_L2B_CK_GATE_CONTROL[CKGateL2BMiscDisable](D0F2xF4_x90[2]) = 1b
  */
-static void __init amd_iommu_erratum_746_workaround(struct amd_iommu *iommu)
+static void amd_iommu_erratum_746_workaround(struct amd_iommu *iommu)
 {
 	u32 value;
 
@@ -1086,7 +1086,7 @@ static int __init init_iommu_all(struct acpi_table_header *table)
 
 			DUMP_printk("device: %02x:%02x.%01x cap: %04x "
 				    "seg: %d flags: %01x info %04x\n",
-				    PCI_BUS(h->devid), PCI_SLOT(h->devid),
+				    PCI_BUS_NUM(h->devid), PCI_SLOT(h->devid),
 				    PCI_FUNC(h->devid), h->cap_ptr,
 				    h->pci_seg, h->flags, h->info);
 			DUMP_printk("       mmio-addr: %016llx\n",
@@ -1116,7 +1116,7 @@ static int iommu_init_pci(struct amd_iommu *iommu)
 	int cap_ptr = iommu->cap_ptr;
 	u32 range, misc, low, high;
 
-	iommu->dev = pci_get_bus_and_slot(PCI_BUS(iommu->devid),
+	iommu->dev = pci_get_bus_and_slot(PCI_BUS_NUM(iommu->devid),
 					  iommu->devid & 0xff);
 	if (!iommu->dev)
 		return -ENODEV;
@@ -1128,9 +1128,9 @@ static int iommu_init_pci(struct amd_iommu *iommu)
 	pci_read_config_dword(iommu->dev, cap_ptr + MMIO_MISC_OFFSET,
 			      &misc);
 
-	iommu->first_device = calc_devid(MMIO_GET_BUS(range),
+	iommu->first_device = PCI_DEVID(MMIO_GET_BUS(range),
 					 MMIO_GET_FD(range));
-	iommu->last_device = calc_devid(MMIO_GET_BUS(range),
+	iommu->last_device = PCI_DEVID(MMIO_GET_BUS(range),
 					MMIO_GET_LD(range));
 
 	if (!(iommu->cap & (1 << IOMMU_CAP_IOTLB)))
@@ -1388,8 +1388,8 @@ static int __init init_unity_map_range(struct ivmd_header *m)
 
 	DUMP_printk("%s devid_start: %02x:%02x.%x devid_end: %02x:%02x.%x"
 		    " range_start: %016llx range_end: %016llx flags: %x\n", s,
-		    PCI_BUS(e->devid_start), PCI_SLOT(e->devid_start),
-		    PCI_FUNC(e->devid_start), PCI_BUS(e->devid_end),
+		    PCI_BUS_NUM(e->devid_start), PCI_SLOT(e->devid_start),
+		    PCI_FUNC(e->devid_start), PCI_BUS_NUM(e->devid_end),
 		    PCI_SLOT(e->devid_end), PCI_FUNC(e->devid_end),
 		    e->address_start, e->address_end, m->flags);
 
diff --git a/drivers/iommu/amd_iommu_types.h b/drivers/iommu/amd_iommu_types.h
index e38ab438bb3..ec36cf63e0c 100644
--- a/drivers/iommu/amd_iommu_types.h
+++ b/drivers/iommu/amd_iommu_types.h
@@ -24,6 +24,7 @@
 #include <linux/mutex.h>
 #include <linux/list.h>
 #include <linux/spinlock.h>
+#include <linux/pci.h>
 
 /*
  * Maximum number of IOMMUs supported
@@ -315,9 +316,6 @@
 
 #define MAX_DOMAIN_ID 65536
 
-/* FIXME: move this macro to <linux/pci.h> */
-#define PCI_BUS(x) (((x) >> 8) & 0xff)
-
 /* Protection domain flags */
 #define PD_DMA_OPS_MASK		(1UL << 0) /* domain used for dma_ops */
 #define PD_DEFAULT_MASK		(1UL << 1) /* domain is a default dma_ops
@@ -703,13 +701,6 @@ extern int amd_iommu_max_glx_val;
  */
 extern void iommu_flush_all_caches(struct amd_iommu *iommu);
 
-/* takes bus and device/function and returns the device id
- * FIXME: should that be in generic PCI code? */
-static inline u16 calc_devid(u8 bus, u8 devfn)
-{
-	return (((u16)bus) << 8) | devfn;
-}
-
 static inline int get_ioapic_devid(int id)
 {
 	struct devid_map *entry;
diff --git a/drivers/iommu/dmar.c b/drivers/iommu/dmar.c
index e5cdaf87822..b8008f679bc 100644
--- a/drivers/iommu/dmar.c
+++ b/drivers/iommu/dmar.c
@@ -129,7 +129,8 @@ int __init dmar_parse_dev_scope(void *start, void *end, int *cnt,
 		if (scope->entry_type == ACPI_DMAR_SCOPE_TYPE_ENDPOINT ||
 		    scope->entry_type == ACPI_DMAR_SCOPE_TYPE_BRIDGE)
 			(*cnt)++;
-		else if (scope->entry_type != ACPI_DMAR_SCOPE_TYPE_IOAPIC) {
+		else if (scope->entry_type != ACPI_DMAR_SCOPE_TYPE_IOAPIC &&
+			scope->entry_type != ACPI_DMAR_SCOPE_TYPE_HPET) {
 			pr_warn("Unsupported device scope\n");
 		}
 		start += scope->length;
diff --git a/drivers/iommu/irq_remapping.c b/drivers/iommu/irq_remapping.c
index d56f8c17c5f..7c11ff368d0 100644
--- a/drivers/iommu/irq_remapping.c
+++ b/drivers/iommu/irq_remapping.c
@@ -2,7 +2,6 @@
 #include <linux/cpumask.h>
 #include <linux/kernel.h>
 #include <linux/string.h>
-#include <linux/cpumask.h>
 #include <linux/errno.h>
 #include <linux/msi.h>
 #include <linux/irq.h>
diff --git a/drivers/ipack/carriers/tpci200.c b/drivers/ipack/carriers/tpci200.c
index 0246b1fddff..c276fde318e 100644
--- a/drivers/ipack/carriers/tpci200.c
+++ b/drivers/ipack/carriers/tpci200.c
@@ -480,6 +480,7 @@ static void tpci200_release_device(struct ipack_device *dev)
 
 static int tpci200_create_device(struct tpci200_board *tpci200, int i)
 {
+	int ret;
 	enum ipack_space space;
 	struct ipack_device *dev =
 		kzalloc(sizeof(struct ipack_device), GFP_KERNEL);
@@ -495,7 +496,18 @@ static int tpci200_create_device(struct tpci200_board *tpci200, int i)
 			+ tpci200_space_interval[space] * i;
 		dev->region[space].size = tpci200_space_size[space];
 	}
-	return ipack_device_register(dev);
+
+	ret = ipack_device_init(dev);
+	if (ret < 0) {
+		ipack_put_device(dev);
+		return ret;
+	}
+
+	ret = ipack_device_add(dev);
+	if (ret < 0)
+		ipack_put_device(dev);
+
+	return ret;
 }
 
 static int tpci200_pci_probe(struct pci_dev *pdev,
diff --git a/drivers/ipack/ipack.c b/drivers/ipack/ipack.c
index 7ec6b208b1c..6e066c53acc 100644
--- a/drivers/ipack/ipack.c
+++ b/drivers/ipack/ipack.c
@@ -227,7 +227,7 @@ static int ipack_unregister_bus_member(struct device *dev, void *data)
 	struct ipack_bus_device *bus = data;
 
 	if (idev->bus == bus)
-		ipack_device_unregister(idev);
+		ipack_device_del(idev);
 
 	return 1;
 }
@@ -419,7 +419,7 @@ out:
 	return ret;
 }
 
-int ipack_device_register(struct ipack_device *dev)
+int ipack_device_init(struct ipack_device *dev)
 {
 	int ret;
 
@@ -428,6 +428,7 @@ int ipack_device_register(struct ipack_device *dev)
 	dev->dev.parent = dev->bus->parent;
 	dev_set_name(&dev->dev,
 		     "ipack-dev.%u.%u", dev->bus->bus_nr, dev->slot);
+	device_initialize(&dev->dev);
 
 	if (dev->bus->ops->set_clockrate(dev, 8))
 		dev_warn(&dev->dev, "failed to switch to 8 MHz operation for reading of device ID.\n");
@@ -447,19 +448,34 @@ int ipack_device_register(struct ipack_device *dev)
 			dev_err(&dev->dev, "failed to switch to 32 MHz operation.\n");
 	}
 
-	ret = device_register(&dev->dev);
-	if (ret < 0)
-		kfree(dev->id);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipack_device_init);
 
-	return ret;
+int ipack_device_add(struct ipack_device *dev)
+{
+	return device_add(&dev->dev);
+}
+EXPORT_SYMBOL_GPL(ipack_device_add);
+
+void ipack_device_del(struct ipack_device *dev)
+{
+	device_del(&dev->dev);
+	ipack_put_device(dev);
+}
+EXPORT_SYMBOL_GPL(ipack_device_del);
+
+void ipack_get_device(struct ipack_device *dev)
+{
+	get_device(&dev->dev);
 }
-EXPORT_SYMBOL_GPL(ipack_device_register);
+EXPORT_SYMBOL_GPL(ipack_get_device);
 
-void ipack_device_unregister(struct ipack_device *dev)
+void ipack_put_device(struct ipack_device *dev)
 {
-	device_unregister(&dev->dev);
+	put_device(&dev->dev);
 }
-EXPORT_SYMBOL_GPL(ipack_device_unregister);
+EXPORT_SYMBOL_GPL(ipack_put_device);
 
 static int __init ipack_init(void)
 {
diff --git a/drivers/irqchip/irq-gic.c b/drivers/irqchip/irq-gic.c
index a32e0d5aa45..fc6aebf1e4b 100644
--- a/drivers/irqchip/irq-gic.c
+++ b/drivers/irqchip/irq-gic.c
@@ -236,7 +236,8 @@ static int gic_retrigger(struct irq_data *d)
 	if (gic_arch_extn.irq_retrigger)
 		return gic_arch_extn.irq_retrigger(d);
 
-	return -ENXIO;
+	/* the genirq layer expects 0 if we can't retrigger in hardware */
+	return 0;
 }
 
 #ifdef CONFIG_SMP
diff --git a/drivers/irqchip/irq-versatile-fpga.c b/drivers/irqchip/irq-versatile-fpga.c
index 9dbd82b716d..065b7a31a47 100644
--- a/drivers/irqchip/irq-versatile-fpga.c
+++ b/drivers/irqchip/irq-versatile-fpga.c
@@ -139,7 +139,7 @@ void __init fpga_irq_init(void __iomem *base, const char *name, int irq_start,
 	int i;
 
 	if (fpga_irq_id >= ARRAY_SIZE(fpga_irq_devices)) {
-		pr_err("%s: too few FPGA IRQ controllers, increase CONFIG_PLAT_VERSATILE_FPGA_IRQ_NR\n", __func__);
+		pr_err("%s: too few FPGA IRQ controllers, increase CONFIG_VERSATILE_FPGA_IRQ_NR\n", __func__);
 		return;
 	}
 	f = &fpga_irq_devices[fpga_irq_id];
diff --git a/drivers/isdn/capi/capi.c b/drivers/isdn/capi/capi.c
index 89562a845f6..ac6f72b455d 100644
--- a/drivers/isdn/capi/capi.c
+++ b/drivers/isdn/capi/capi.c
@@ -569,7 +569,6 @@ static void capi_recv_message(struct capi20_appl *ap, struct sk_buff *skb)
 {
 	struct capidev *cdev = ap->private;
 #ifdef CONFIG_ISDN_CAPI_MIDDLEWARE
-	struct tty_struct *tty;
 	struct capiminor *mp;
 	u16 datahandle;
 	struct capincci *np;
@@ -627,11 +626,7 @@ static void capi_recv_message(struct capi20_appl *ap, struct sk_buff *skb)
 			 CAPIMSG_U16(skb->data, CAPIMSG_BASELEN + 4 + 2));
 		kfree_skb(skb);
 		capiminor_del_ack(mp, datahandle);
-		tty = tty_port_tty_get(&mp->port);
-		if (tty) {
-			tty_wakeup(tty);
-			tty_kref_put(tty);
-		}
+		tty_port_tty_wakeup(&mp->port);
 		handle_minor_send(mp);
 
 	} else {
diff --git a/drivers/isdn/gigaset/interface.c b/drivers/isdn/gigaset/interface.c
index e2b539675b6..600c79b030c 100644
--- a/drivers/isdn/gigaset/interface.c
+++ b/drivers/isdn/gigaset/interface.c
@@ -487,12 +487,8 @@ static const struct tty_operations if_ops = {
 static void if_wake(unsigned long data)
 {
 	struct cardstate *cs = (struct cardstate *)data;
-	struct tty_struct *tty = tty_port_tty_get(&cs->port);
 
-	if (tty) {
-		tty_wakeup(tty);
-		tty_kref_put(tty);
-	}
+	tty_port_tty_wakeup(&cs->port);
 }
 
 /*** interface to common ***/
diff --git a/drivers/isdn/hardware/avm/avm_cs.c b/drivers/isdn/hardware/avm/avm_cs.c
index c21353d8e91..62b8030ee33 100644
--- a/drivers/isdn/hardware/avm/avm_cs.c
+++ b/drivers/isdn/hardware/avm/avm_cs.c
@@ -163,16 +163,4 @@ static struct pcmcia_driver avmcs_driver = {
 	.remove	= avmcs_detach,
 	.id_table = avmcs_ids,
 };
-
-static int __init avmcs_init(void)
-{
-	return pcmcia_register_driver(&avmcs_driver);
-}
-
-static void __exit avmcs_exit(void)
-{
-	pcmcia_unregister_driver(&avmcs_driver);
-}
-
-module_init(avmcs_init);
-module_exit(avmcs_exit);
+module_pcmcia_driver(avmcs_driver);
diff --git a/drivers/isdn/hisax/avma1_cs.c b/drivers/isdn/hisax/avma1_cs.c
index 4e676bcf850..baad94ec1f4 100644
--- a/drivers/isdn/hisax/avma1_cs.c
+++ b/drivers/isdn/hisax/avma1_cs.c
@@ -159,16 +159,4 @@ static struct pcmcia_driver avma1cs_driver = {
 	.remove		= avma1cs_detach,
 	.id_table	= avma1cs_ids,
 };
-
-static int __init init_avma1_cs(void)
-{
-	return pcmcia_register_driver(&avma1cs_driver);
-}
-
-static void __exit exit_avma1_cs(void)
-{
-	pcmcia_unregister_driver(&avma1cs_driver);
-}
-
-module_init(init_avma1_cs);
-module_exit(exit_avma1_cs);
+module_pcmcia_driver(avma1cs_driver);
diff --git a/drivers/isdn/hisax/elsa_cs.c b/drivers/isdn/hisax/elsa_cs.c
index ebe56918f6f..40f6fad79de 100644
--- a/drivers/isdn/hisax/elsa_cs.c
+++ b/drivers/isdn/hisax/elsa_cs.c
@@ -215,16 +215,4 @@ static struct pcmcia_driver elsa_cs_driver = {
 	.suspend	= elsa_suspend,
 	.resume		= elsa_resume,
 };
-
-static int __init init_elsa_cs(void)
-{
-	return pcmcia_register_driver(&elsa_cs_driver);
-}
-
-static void __exit exit_elsa_cs(void)
-{
-	pcmcia_unregister_driver(&elsa_cs_driver);
-}
-
-module_init(init_elsa_cs);
-module_exit(exit_elsa_cs);
+module_pcmcia_driver(elsa_cs_driver);
diff --git a/drivers/isdn/hisax/sedlbauer_cs.c b/drivers/isdn/hisax/sedlbauer_cs.c
index 90f81291641..92ef62d4caf 100644
--- a/drivers/isdn/hisax/sedlbauer_cs.c
+++ b/drivers/isdn/hisax/sedlbauer_cs.c
@@ -206,16 +206,4 @@ static struct pcmcia_driver sedlbauer_driver = {
 	.suspend	= sedlbauer_suspend,
 	.resume		= sedlbauer_resume,
 };
-
-static int __init init_sedlbauer_cs(void)
-{
-	return pcmcia_register_driver(&sedlbauer_driver);
-}
-
-static void __exit exit_sedlbauer_cs(void)
-{
-	pcmcia_unregister_driver(&sedlbauer_driver);
-}
-
-module_init(init_sedlbauer_cs);
-module_exit(exit_sedlbauer_cs);
+module_pcmcia_driver(sedlbauer_driver);
diff --git a/drivers/isdn/hisax/teles_cs.c b/drivers/isdn/hisax/teles_cs.c
index f2476ffb04f..b8dd1495875 100644
--- a/drivers/isdn/hisax/teles_cs.c
+++ b/drivers/isdn/hisax/teles_cs.c
@@ -197,16 +197,4 @@ static struct pcmcia_driver teles_cs_driver = {
 	.suspend	= teles_suspend,
 	.resume		= teles_resume,
 };
-
-static int __init init_teles_cs(void)
-{
-	return pcmcia_register_driver(&teles_cs_driver);
-}
-
-static void __exit exit_teles_cs(void)
-{
-	pcmcia_unregister_driver(&teles_cs_driver);
-}
-
-module_init(init_teles_cs);
-module_exit(exit_teles_cs);
+module_pcmcia_driver(teles_cs_driver);
diff --git a/drivers/isdn/i4l/isdn_tty.c b/drivers/isdn/i4l/isdn_tty.c
index ebaebdf30f9..3c5f2491a16 100644
--- a/drivers/isdn/i4l/isdn_tty.c
+++ b/drivers/isdn/i4l/isdn_tty.c
@@ -1472,9 +1472,6 @@ isdn_tty_set_termios(struct tty_struct *tty, struct ktermios *old_termios)
 		    tty->termios.c_ospeed == old_termios->c_ospeed)
 			return;
 		isdn_tty_change_speed(info);
-		if ((old_termios->c_cflag & CRTSCTS) &&
-		    !(tty->termios.c_cflag & CRTSCTS))
-			tty->hw_stopped = 0;
 	}
 }
 
@@ -3427,7 +3424,6 @@ isdn_tty_parse_at(modem_info *info)
 			p++;
 			isdn_tty_cmd_ATA(info);
 			return;
-			break;
 		case 'D':
 			/* D - Dial */
 			if (info->msr & UART_MSR_DCD)
diff --git a/drivers/lguest/page_tables.c b/drivers/lguest/page_tables.c
index 3b62be160a6..864baabaee2 100644
--- a/drivers/lguest/page_tables.c
+++ b/drivers/lguest/page_tables.c
@@ -686,7 +686,7 @@ static unsigned int new_pgdir(struct lg_cpu *cpu,
 	 * We pick one entry at random to throw out.  Choosing the Least
 	 * Recently Used might be better, but this is easy.
 	 */
-	next = random32() % ARRAY_SIZE(cpu->lg->pgdirs);
+	next = prandom_u32() % ARRAY_SIZE(cpu->lg->pgdirs);
 	/* If it's never been allocated at all before, try now. */
 	if (!cpu->lg->pgdirs[next].pgdir) {
 		cpu->lg->pgdirs[next].pgdir =
diff --git a/drivers/md/dm-cache-target.c b/drivers/md/dm-cache-target.c
index 66120bd46d1..10744091e6c 100644
--- a/drivers/md/dm-cache-target.c
+++ b/drivers/md/dm-cache-target.c
@@ -6,6 +6,7 @@
 
 #include "dm.h"
 #include "dm-bio-prison.h"
+#include "dm-bio-record.h"
 #include "dm-cache-metadata.h"
 
 #include <linux/dm-io.h>
@@ -201,10 +202,15 @@ struct per_bio_data {
 	unsigned req_nr:2;
 	struct dm_deferred_entry *all_io_entry;
 
-	/* writethrough fields */
+	/*
+	 * writethrough fields.  These MUST remain at the end of this
+	 * structure and the 'cache' member must be the first as it
+	 * is used to determine the offsetof the writethrough fields.
+	 */
 	struct cache *cache;
 	dm_cblock_t cblock;
 	bio_end_io_t *saved_bi_end_io;
+	struct dm_bio_details bio_details;
 };
 
 struct dm_cache_migration {
@@ -513,16 +519,28 @@ static void save_stats(struct cache *cache)
 /*----------------------------------------------------------------
  * Per bio data
  *--------------------------------------------------------------*/
-static struct per_bio_data *get_per_bio_data(struct bio *bio)
+
+/*
+ * If using writeback, leave out struct per_bio_data's writethrough fields.
+ */
+#define PB_DATA_SIZE_WB (offsetof(struct per_bio_data, cache))
+#define PB_DATA_SIZE_WT (sizeof(struct per_bio_data))
+
+static size_t get_per_bio_data_size(struct cache *cache)
+{
+	return cache->features.write_through ? PB_DATA_SIZE_WT : PB_DATA_SIZE_WB;
+}
+
+static struct per_bio_data *get_per_bio_data(struct bio *bio, size_t data_size)
 {
-	struct per_bio_data *pb = dm_per_bio_data(bio, sizeof(struct per_bio_data));
+	struct per_bio_data *pb = dm_per_bio_data(bio, data_size);
 	BUG_ON(!pb);
 	return pb;
 }
 
-static struct per_bio_data *init_per_bio_data(struct bio *bio)
+static struct per_bio_data *init_per_bio_data(struct bio *bio, size_t data_size)
 {
-	struct per_bio_data *pb = get_per_bio_data(bio);
+	struct per_bio_data *pb = get_per_bio_data(bio, data_size);
 
 	pb->tick = false;
 	pb->req_nr = dm_bio_get_target_bio_nr(bio);
@@ -556,7 +574,8 @@ static void remap_to_cache(struct cache *cache, struct bio *bio,
 static void check_if_tick_bio_needed(struct cache *cache, struct bio *bio)
 {
 	unsigned long flags;
-	struct per_bio_data *pb = get_per_bio_data(bio);
+	size_t pb_data_size = get_per_bio_data_size(cache);
+	struct per_bio_data *pb = get_per_bio_data(bio, pb_data_size);
 
 	spin_lock_irqsave(&cache->lock, flags);
 	if (cache->need_tick_bio &&
@@ -635,7 +654,7 @@ static void defer_writethrough_bio(struct cache *cache, struct bio *bio)
 
 static void writethrough_endio(struct bio *bio, int err)
 {
-	struct per_bio_data *pb = get_per_bio_data(bio);
+	struct per_bio_data *pb = get_per_bio_data(bio, PB_DATA_SIZE_WT);
 	bio->bi_end_io = pb->saved_bi_end_io;
 
 	if (err) {
@@ -643,6 +662,7 @@ static void writethrough_endio(struct bio *bio, int err)
 		return;
 	}
 
+	dm_bio_restore(&pb->bio_details, bio);
 	remap_to_cache(pb->cache, bio, pb->cblock);
 
 	/*
@@ -662,11 +682,12 @@ static void writethrough_endio(struct bio *bio, int err)
 static void remap_to_origin_then_cache(struct cache *cache, struct bio *bio,
 				       dm_oblock_t oblock, dm_cblock_t cblock)
 {
-	struct per_bio_data *pb = get_per_bio_data(bio);
+	struct per_bio_data *pb = get_per_bio_data(bio, PB_DATA_SIZE_WT);
 
 	pb->cache = cache;
 	pb->cblock = cblock;
 	pb->saved_bi_end_io = bio->bi_end_io;
+	dm_bio_record(&pb->bio_details, bio);
 	bio->bi_end_io = writethrough_endio;
 
 	remap_to_origin_clear_discard(pb->cache, bio, oblock);
@@ -1035,7 +1056,8 @@ static void defer_bio(struct cache *cache, struct bio *bio)
 
 static void process_flush_bio(struct cache *cache, struct bio *bio)
 {
-	struct per_bio_data *pb = get_per_bio_data(bio);
+	size_t pb_data_size = get_per_bio_data_size(cache);
+	struct per_bio_data *pb = get_per_bio_data(bio, pb_data_size);
 
 	BUG_ON(bio->bi_size);
 	if (!pb->req_nr)
@@ -1107,7 +1129,8 @@ static void process_bio(struct cache *cache, struct prealloc *structs,
 	dm_oblock_t block = get_bio_block(cache, bio);
 	struct dm_bio_prison_cell *cell_prealloc, *old_ocell, *new_ocell;
 	struct policy_result lookup_result;
-	struct per_bio_data *pb = get_per_bio_data(bio);
+	size_t pb_data_size = get_per_bio_data_size(cache);
+	struct per_bio_data *pb = get_per_bio_data(bio, pb_data_size);
 	bool discarded_block = is_discarded_oblock(cache, block);
 	bool can_migrate = discarded_block || spare_migration_bandwidth(cache);
 
@@ -1881,7 +1904,6 @@ static int cache_create(struct cache_args *ca, struct cache **result)
 
 	cache->ti = ca->ti;
 	ti->private = cache;
-	ti->per_bio_data_size = sizeof(struct per_bio_data);
 	ti->num_flush_bios = 2;
 	ti->flush_supported = true;
 
@@ -1890,6 +1912,7 @@ static int cache_create(struct cache_args *ca, struct cache **result)
 	ti->discard_zeroes_data_unsupported = true;
 
 	memcpy(&cache->features, &ca->features, sizeof(cache->features));
+	ti->per_bio_data_size = get_per_bio_data_size(cache);
 
 	cache->callbacks.congested_fn = cache_is_congested;
 	dm_table_add_target_callbacks(ti->table, &cache->callbacks);
@@ -2092,6 +2115,7 @@ static int cache_map(struct dm_target *ti, struct bio *bio)
 
 	int r;
 	dm_oblock_t block = get_bio_block(cache, bio);
+	size_t pb_data_size = get_per_bio_data_size(cache);
 	bool can_migrate = false;
 	bool discarded_block;
 	struct dm_bio_prison_cell *cell;
@@ -2108,7 +2132,7 @@ static int cache_map(struct dm_target *ti, struct bio *bio)
 		return DM_MAPIO_REMAPPED;
 	}
 
-	pb = init_per_bio_data(bio);
+	pb = init_per_bio_data(bio, pb_data_size);
 
 	if (bio->bi_rw & (REQ_FLUSH | REQ_FUA | REQ_DISCARD)) {
 		defer_bio(cache, bio);
@@ -2193,7 +2217,8 @@ static int cache_end_io(struct dm_target *ti, struct bio *bio, int error)
 {
 	struct cache *cache = ti->private;
 	unsigned long flags;
-	struct per_bio_data *pb = get_per_bio_data(bio);
+	size_t pb_data_size = get_per_bio_data_size(cache);
+	struct per_bio_data *pb = get_per_bio_data(bio, pb_data_size);
 
 	if (pb->tick) {
 		policy_tick(cache->policy);
diff --git a/drivers/md/dm.c b/drivers/md/dm.c
index 7e469260fe5..9a0bdad9ad8 100644
--- a/drivers/md/dm.c
+++ b/drivers/md/dm.c
@@ -611,6 +611,7 @@ static void dec_pending(struct dm_io *io, int error)
 			queue_io(md, bio);
 		} else {
 			/* done with normal IO or empty flush */
+			trace_block_bio_complete(md->queue, bio, io_error);
 			bio_endio(bio, io_error);
 		}
 	}
diff --git a/drivers/md/raid5.c b/drivers/md/raid5.c
index 24909eb13fe..f4e87bfc756 100644
--- a/drivers/md/raid5.c
+++ b/drivers/md/raid5.c
@@ -184,6 +184,8 @@ static void return_io(struct bio *return_bi)
 		return_bi = bi->bi_next;
 		bi->bi_next = NULL;
 		bi->bi_size = 0;
+		trace_block_bio_complete(bdev_get_queue(bi->bi_bdev),
+					 bi, 0);
 		bio_endio(bi, 0);
 		bi = return_bi;
 	}
@@ -3914,6 +3916,8 @@ static void raid5_align_endio(struct bio *bi, int error)
 	rdev_dec_pending(rdev, conf->mddev);
 
 	if (!error && uptodate) {
+		trace_block_bio_complete(bdev_get_queue(raid_bi->bi_bdev),
+					 raid_bi, 0);
 		bio_endio(raid_bi, 0);
 		if (atomic_dec_and_test(&conf->active_aligned_reads))
 			wake_up(&conf->wait_for_stripe);
@@ -4382,6 +4386,8 @@ static void make_request(struct mddev *mddev, struct bio * bi)
 		if ( rw == WRITE )
 			md_write_end(mddev);
 
+		trace_block_bio_complete(bdev_get_queue(bi->bi_bdev),
+					 bi, 0);
 		bio_endio(bi, 0);
 	}
 }
@@ -4758,8 +4764,11 @@ static int  retry_aligned_read(struct r5conf *conf, struct bio *raid_bio)
 		handled++;
 	}
 	remaining = raid5_dec_bi_active_stripes(raid_bio);
-	if (remaining == 0)
+	if (remaining == 0) {
+		trace_block_bio_complete(bdev_get_queue(raid_bio->bi_bdev),
+					 raid_bio, 0);
 		bio_endio(raid_bio, 0);
+	}
 	if (atomic_dec_and_test(&conf->active_aligned_reads))
 		wake_up(&conf->wait_for_stripe);
 	return handled;
diff --git a/drivers/media/common/Kconfig b/drivers/media/common/Kconfig
index 56c25e6299e..b85f88c8ddb 100644
--- a/drivers/media/common/Kconfig
+++ b/drivers/media/common/Kconfig
@@ -16,6 +16,10 @@ config VIDEO_TVEEPROM
 	tristate
 	depends on I2C
 
+config CYPRESS_FIRMWARE
+	tristate "Cypress firmware helper routines"
+	depends on USB
+
 source "drivers/media/common/b2c2/Kconfig"
 source "drivers/media/common/saa7146/Kconfig"
 source "drivers/media/common/siano/Kconfig"
diff --git a/drivers/media/common/Makefile b/drivers/media/common/Makefile
index 8f8d18755d1..d208de3b7cc 100644
--- a/drivers/media/common/Makefile
+++ b/drivers/media/common/Makefile
@@ -2,3 +2,4 @@ obj-y += b2c2/ saa7146/ siano/
 obj-$(CONFIG_VIDEO_CX2341X) += cx2341x.o
 obj-$(CONFIG_VIDEO_BTCX)  += btcx-risc.o
 obj-$(CONFIG_VIDEO_TVEEPROM) += tveeprom.o
+obj-$(CONFIG_CYPRESS_FIRMWARE) += cypress_firmware.o
diff --git a/drivers/media/common/b2c2/flexcop-fe-tuner.c b/drivers/media/common/b2c2/flexcop-fe-tuner.c
index 850a6c60675..7e14e90d292 100644
--- a/drivers/media/common/b2c2/flexcop-fe-tuner.c
+++ b/drivers/media/common/b2c2/flexcop-fe-tuner.c
@@ -325,7 +325,7 @@ static int skystar2_rev27_attach(struct flexcop_device *fc,
 	/* enable no_base_addr - no repeated start when reading */
 	fc->fc_i2c_adap[2].no_base_addr = 1;
 	if (!dvb_attach(isl6421_attach, fc->fe, &fc->fc_i2c_adap[2].i2c_adap,
-			0x08, 1, 1)) {
+			0x08, 1, 1, false)) {
 		err("ISL6421 could NOT be attached");
 		goto fail_isl;
 	}
@@ -391,7 +391,7 @@ static int skystar2_rev28_attach(struct flexcop_device *fc,
 
 	fc->fc_i2c_adap[2].no_base_addr = 1;
 	if (!dvb_attach(isl6421_attach, fc->fe, &fc->fc_i2c_adap[2].i2c_adap,
-			0x08, 0, 0)) {
+			0x08, 0, 0, false)) {
 		err("ISL6421 could NOT be attached");
 		fc->fc_i2c_adap[2].no_base_addr = 0;
 		return 0;
diff --git a/drivers/media/usb/dvb-usb-v2/cypress_firmware.c b/drivers/media/common/cypress_firmware.c
index 211df549f26..577e82058fd 100644
--- a/drivers/media/usb/dvb-usb-v2/cypress_firmware.c
+++ b/drivers/media/common/cypress_firmware.c
@@ -8,7 +8,10 @@
  *
  */
 
-#include "dvb_usb.h"
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/usb.h>
+#include <linux/firmware.h>
 #include "cypress_firmware.h"
 
 struct usb_cypress_controller {
@@ -30,14 +33,42 @@ static const struct usb_cypress_controller cypress[] = {
 static int usb_cypress_writemem(struct usb_device *udev, u16 addr, u8 *data,
 		u8 len)
 {
-	dvb_usb_dbg_usb_control_msg(udev,
-			0xa0, USB_TYPE_VENDOR, addr, 0x00, data, len);
-
 	return usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
 			0xa0, USB_TYPE_VENDOR, addr, 0x00, data, len, 5000);
 }
 
-int usbv2_cypress_load_firmware(struct usb_device *udev,
+static int cypress_get_hexline(const struct firmware *fw,
+				struct hexline *hx, int *pos)
+{
+	u8 *b = (u8 *) &fw->data[*pos];
+	int data_offs = 4;
+
+	if (*pos >= fw->size)
+		return 0;
+
+	memset(hx, 0, sizeof(struct hexline));
+	hx->len = b[0];
+
+	if ((*pos + hx->len + 4) >= fw->size)
+		return -EINVAL;
+
+	hx->addr = b[1] | (b[2] << 8);
+	hx->type = b[3];
+
+	if (hx->type == 0x04) {
+		/* b[4] and b[5] are the Extended linear address record data
+		 * field */
+		hx->addr |= (b[4] << 24) | (b[5] << 16);
+	}
+
+	memcpy(hx->data, &b[data_offs], hx->len);
+	hx->chk = b[hx->len + data_offs];
+	*pos += hx->len + 5;
+
+	return *pos;
+}
+
+int cypress_load_firmware(struct usb_device *udev,
 		const struct firmware *fw, int type)
 {
 	struct hexline *hx;
@@ -61,7 +92,7 @@ int usbv2_cypress_load_firmware(struct usb_device *udev,
 
 	/* write firmware to memory */
 	for (;;) {
-		ret = dvb_usbv2_get_hexline(fw, hx, &pos);
+		ret = cypress_get_hexline(fw, hx, &pos);
 		if (ret < 0)
 			goto err_kfree;
 		else if (ret == 0)
@@ -71,9 +102,8 @@ int usbv2_cypress_load_firmware(struct usb_device *udev,
 		if (ret < 0) {
 			goto err_kfree;
 		} else if (ret != hx->len) {
-			dev_err(&udev->dev, "%s: error while transferring " \
-					"firmware (transferred size=%d, " \
-					"block size=%d)\n",
+			dev_err(&udev->dev,
+					"%s: error while transferring firmware (transferred size=%d, block size=%d)\n",
 					KBUILD_MODNAME, ret, hx->len);
 			ret = -EIO;
 			goto err_kfree;
@@ -95,39 +125,7 @@ err_kfree:
 	kfree(hx);
 	return ret;
 }
-EXPORT_SYMBOL(usbv2_cypress_load_firmware);
-
-int dvb_usbv2_get_hexline(const struct firmware *fw, struct hexline *hx,
-		int *pos)
-{
-	u8 *b = (u8 *) &fw->data[*pos];
-	int data_offs = 4;
-
-	if (*pos >= fw->size)
-		return 0;
-
-	memset(hx, 0, sizeof(struct hexline));
-	hx->len = b[0];
-
-	if ((*pos + hx->len + 4) >= fw->size)
-		return -EINVAL;
-
-	hx->addr = b[1] | (b[2] << 8);
-	hx->type = b[3];
-
-	if (hx->type == 0x04) {
-		/* b[4] and b[5] are the Extended linear address record data
-		 * field */
-		hx->addr |= (b[4] << 24) | (b[5] << 16);
-	}
-
-	memcpy(hx->data, &b[data_offs], hx->len);
-	hx->chk = b[hx->len + data_offs];
-	*pos += hx->len + 5;
-
-	return *pos;
-}
-EXPORT_SYMBOL(dvb_usbv2_get_hexline);
+EXPORT_SYMBOL(cypress_load_firmware);
 
 MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
 MODULE_DESCRIPTION("Cypress firmware download");
diff --git a/drivers/media/usb/dvb-usb-v2/cypress_firmware.h b/drivers/media/common/cypress_firmware.h
index 80085fd4132..e493cbc7a52 100644
--- a/drivers/media/usb/dvb-usb-v2/cypress_firmware.h
+++ b/drivers/media/common/cypress_firmware.h
@@ -1,5 +1,4 @@
-/* cypress_firmware.h is part of the DVB USB library.
- *
+/*
  * Copyright (C) 2004-6 Patrick Boettcher (patrick.boettcher@desy.de)
  * see dvb-usb-init.c for copyright information.
  *
@@ -23,9 +22,7 @@ struct hexline {
 	u8 data[255];
 	u8 chk;
 };
-extern int usbv2_cypress_load_firmware(struct usb_device *,
-		const struct firmware *, int);
-extern int dvb_usbv2_get_hexline(const struct firmware *,
-		struct hexline *, int *);
+
+int cypress_load_firmware(struct usb_device *, const struct firmware *, int);
 
 #endif
diff --git a/drivers/media/common/saa7146/saa7146_video.c b/drivers/media/common/saa7146/saa7146_video.c
index 4143d61f79b..fe907f2e8f5 100644
--- a/drivers/media/common/saa7146/saa7146_video.c
+++ b/drivers/media/common/saa7146/saa7146_video.c
@@ -832,7 +832,7 @@ static int vidioc_g_std(struct file *file, void *fh, v4l2_std_id *norm)
 	}
 	*/
 
-static int vidioc_s_std(struct file *file, void *fh, v4l2_std_id *id)
+static int vidioc_s_std(struct file *file, void *fh, v4l2_std_id id)
 {
 	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
 	struct saa7146_vv *vv = dev->vv_data;
@@ -856,7 +856,7 @@ static int vidioc_s_std(struct file *file, void *fh, v4l2_std_id *id)
 	}
 
 	for (i = 0; i < dev->ext_vv_data->num_stds; i++)
-		if (*id & dev->ext_vv_data->stds[i].id)
+		if (id & dev->ext_vv_data->stds[i].id)
 			break;
 	if (i != dev->ext_vv_data->num_stds) {
 		vv->standard = &dev->ext_vv_data->stds[i];
diff --git a/drivers/media/common/siano/Kconfig b/drivers/media/common/siano/Kconfig
index 68f0f604678..f3f5ec44e68 100644
--- a/drivers/media/common/siano/Kconfig
+++ b/drivers/media/common/siano/Kconfig
@@ -17,3 +17,15 @@ config SMS_SIANO_RC
 	default y
 	---help---
 	  Choose Y to select Remote Controller support for Siano driver.
+
+config SMS_SIANO_DEBUGFS
+	bool "Enable debugfs for smsdvb"
+	depends on SMS_SIANO_MDTV
+	depends on DEBUG_FS
+	depends on SMS_USB_DRV
+	---help---
+	  Choose Y to enable visualizing a dump of the frontend
+	  statistics response packets via debugfs. Currently, works
+	  only with Siano USB devices.
+
+	  Useful only for developers. In doubt, say N.
diff --git a/drivers/media/common/siano/Makefile b/drivers/media/common/siano/Makefile
index 81b1e985bea..4c0567f106b 100644
--- a/drivers/media/common/siano/Makefile
+++ b/drivers/media/common/siano/Makefile
@@ -1,4 +1,5 @@
 smsmdtv-objs := smscoreapi.o sms-cards.o smsendian.o
+smsdvb-objs := smsdvb-main.o
 
 obj-$(CONFIG_SMS_SIANO_MDTV) += smsmdtv.o smsdvb.o
 
@@ -6,6 +7,10 @@ ifeq ($(CONFIG_SMS_SIANO_RC),y)
   smsmdtv-objs += smsir.o
 endif
 
+ifeq ($(CONFIG_SMS_SIANO_DEBUGFS),y)
+  smsdvb-objs += smsdvb-debugfs.o
+endif
+
 ccflags-y += -Idrivers/media/dvb-core
 ccflags-y += $(extra-cflags-y) $(extra-cflags-m)
 
diff --git a/drivers/media/common/siano/sms-cards.c b/drivers/media/common/siano/sms-cards.c
index 680c781c8dd..82769993eeb 100644
--- a/drivers/media/common/siano/sms-cards.c
+++ b/drivers/media/common/siano/sms-cards.c
@@ -28,43 +28,53 @@ MODULE_PARM_DESC(cards_dbg, "set debug level (info=1, adv=2 (or-able))");
 static struct sms_board sms_boards[] = {
 	[SMS_BOARD_UNKNOWN] = {
 		.name	= "Unknown board",
+		.type = SMS_UNKNOWN_TYPE,
+		.default_mode = DEVICE_MODE_NONE,
 	},
 	[SMS1XXX_BOARD_SIANO_STELLAR] = {
 		.name	= "Siano Stellar Digital Receiver",
 		.type	= SMS_STELLAR,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
 	},
 	[SMS1XXX_BOARD_SIANO_NOVA_A] = {
 		.name	= "Siano Nova A Digital Receiver",
 		.type	= SMS_NOVA_A0,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
 	},
 	[SMS1XXX_BOARD_SIANO_NOVA_B] = {
 		.name	= "Siano Nova B Digital Receiver",
 		.type	= SMS_NOVA_B0,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
 	},
 	[SMS1XXX_BOARD_SIANO_VEGA] = {
 		.name	= "Siano Vega Digital Receiver",
 		.type	= SMS_VEGA,
+		.default_mode = DEVICE_MODE_CMMB,
 	},
 	[SMS1XXX_BOARD_HAUPPAUGE_CATAMOUNT] = {
 		.name	= "Hauppauge Catamount",
 		.type	= SMS_STELLAR,
-		.fw[DEVICE_MODE_DVBT_BDA] = "sms1xxx-stellar-dvbt-01.fw",
+		.fw[DEVICE_MODE_DVBT_BDA] = SMS_FW_DVBT_STELLAR,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
 	},
 	[SMS1XXX_BOARD_HAUPPAUGE_OKEMO_A] = {
 		.name	= "Hauppauge Okemo-A",
 		.type	= SMS_NOVA_A0,
-		.fw[DEVICE_MODE_DVBT_BDA] = "sms1xxx-nova-a-dvbt-01.fw",
+		.fw[DEVICE_MODE_DVBT_BDA] = SMS_FW_DVBT_NOVA_A,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
 	},
 	[SMS1XXX_BOARD_HAUPPAUGE_OKEMO_B] = {
 		.name	= "Hauppauge Okemo-B",
 		.type	= SMS_NOVA_B0,
-		.fw[DEVICE_MODE_DVBT_BDA] = "sms1xxx-nova-b-dvbt-01.fw",
+		.fw[DEVICE_MODE_DVBT_BDA] = SMS_FW_DVBT_NOVA_B,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
 	},
 	[SMS1XXX_BOARD_HAUPPAUGE_WINDHAM] = {
 		.name	= "Hauppauge WinTV MiniStick",
 		.type	= SMS_NOVA_B0,
-		.fw[DEVICE_MODE_ISDBT_BDA] = "sms1xxx-hcw-55xxx-isdbt-02.fw",
-		.fw[DEVICE_MODE_DVBT_BDA] = "sms1xxx-hcw-55xxx-dvbt-02.fw",
+		.fw[DEVICE_MODE_ISDBT_BDA] = SMS_FW_ISDBT_HCW_55XXX,
+		.fw[DEVICE_MODE_DVBT_BDA]  = SMS_FW_DVBT_HCW_55XXX,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
 		.rc_codes = RC_MAP_HAUPPAUGE,
 		.board_cfg.leds_power = 26,
 		.board_cfg.led0 = 27,
@@ -77,7 +87,8 @@ static struct sms_board sms_boards[] = {
 	[SMS1XXX_BOARD_HAUPPAUGE_TIGER_MINICARD] = {
 		.name	= "Hauppauge WinTV MiniCard",
 		.type	= SMS_NOVA_B0,
-		.fw[DEVICE_MODE_DVBT_BDA] = "sms1xxx-hcw-55xxx-dvbt-02.fw",
+		.fw[DEVICE_MODE_DVBT_BDA] = SMS_FW_DVBT_HCW_55XXX,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
 		.lna_ctrl  = 29,
 		.board_cfg.foreign_lna0_ctrl = 29,
 		.rf_switch = 17,
@@ -86,18 +97,65 @@ static struct sms_board sms_boards[] = {
 	[SMS1XXX_BOARD_HAUPPAUGE_TIGER_MINICARD_R2] = {
 		.name	= "Hauppauge WinTV MiniCard",
 		.type	= SMS_NOVA_B0,
-		.fw[DEVICE_MODE_DVBT_BDA] = "sms1xxx-hcw-55xxx-dvbt-02.fw",
+		.fw[DEVICE_MODE_DVBT_BDA] = SMS_FW_DVBT_HCW_55XXX,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
 		.lna_ctrl  = -1,
 	},
 	[SMS1XXX_BOARD_SIANO_NICE] = {
-	/* 11 */
 		.name = "Siano Nice Digital Receiver",
 		.type = SMS_NOVA_B0,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
 	},
 	[SMS1XXX_BOARD_SIANO_VENICE] = {
-	/* 12 */
 		.name = "Siano Venice Digital Receiver",
 		.type = SMS_VEGA,
+		.default_mode = DEVICE_MODE_CMMB,
+	},
+	[SMS1XXX_BOARD_SIANO_STELLAR_ROM] = {
+		.name = "Siano Stellar Digital Receiver ROM",
+		.type = SMS_STELLAR,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
+		.intf_num = 1,
+	},
+	[SMS1XXX_BOARD_ZTE_DVB_DATA_CARD] = {
+		.name = "ZTE Data Card Digital Receiver",
+		.type = SMS_NOVA_B0,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
+		.intf_num = 5,
+		.mtu = 15792,
+	},
+	[SMS1XXX_BOARD_ONDA_MDTV_DATA_CARD] = {
+		.name = "ONDA Data Card Digital Receiver",
+		.type = SMS_NOVA_B0,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
+		.intf_num = 6,
+		.mtu = 15792,
+	},
+	[SMS1XXX_BOARD_SIANO_MING] = {
+		.name = "Siano Ming Digital Receiver",
+		.type = SMS_MING,
+		.default_mode = DEVICE_MODE_CMMB,
+	},
+	[SMS1XXX_BOARD_SIANO_PELE] = {
+		.name = "Siano Pele Digital Receiver",
+		.type = SMS_PELE,
+		.default_mode = DEVICE_MODE_ISDBT_BDA,
+	},
+	[SMS1XXX_BOARD_SIANO_RIO] = {
+		.name = "Siano Rio Digital Receiver",
+		.type = SMS_RIO,
+		.default_mode = DEVICE_MODE_ISDBT_BDA,
+	},
+	[SMS1XXX_BOARD_SIANO_DENVER_1530] = {
+		.name = "Siano Denver (ATSC-M/H) Digital Receiver",
+		.type = SMS_DENVER_1530,
+		.default_mode = DEVICE_MODE_ATSC,
+		.crystal = 2400,
+	},
+	[SMS1XXX_BOARD_SIANO_DENVER_2160] = {
+		.name = "Siano Denver (TDMB) Digital Receiver",
+		.type = SMS_DENVER_2160,
+		.default_mode = DEVICE_MODE_DAB_TDMB,
 	},
 };
 
@@ -109,20 +167,21 @@ struct sms_board *sms_get_board(unsigned id)
 }
 EXPORT_SYMBOL_GPL(sms_get_board);
 static inline void sms_gpio_assign_11xx_default_led_config(
-		struct smscore_gpio_config *pGpioConfig) {
-	pGpioConfig->Direction = SMS_GPIO_DIRECTION_OUTPUT;
-	pGpioConfig->InputCharacteristics =
-		SMS_GPIO_INPUT_CHARACTERISTICS_NORMAL;
-	pGpioConfig->OutputDriving = SMS_GPIO_OUTPUT_DRIVING_4mA;
-	pGpioConfig->OutputSlewRate = SMS_GPIO_OUTPUT_SLEW_RATE_0_45_V_NS;
-	pGpioConfig->PullUpDown = SMS_GPIO_PULL_UP_DOWN_NONE;
+		struct smscore_config_gpio *p_gpio_config) {
+	p_gpio_config->direction = SMS_GPIO_DIRECTION_OUTPUT;
+	p_gpio_config->inputcharacteristics =
+		SMS_GPIO_INPUTCHARACTERISTICS_NORMAL;
+	p_gpio_config->outputdriving = SMS_GPIO_OUTPUTDRIVING_4mA;
+	p_gpio_config->outputslewrate = SMS_GPIO_OUTPUT_SLEW_RATE_0_45_V_NS;
+	p_gpio_config->pullupdown = SMS_GPIO_PULLUPDOWN_NONE;
 }
 
 int sms_board_event(struct smscore_device_t *coredev,
-		enum SMS_BOARD_EVENTS gevent) {
-	struct smscore_gpio_config MyGpioConfig;
+		    enum SMS_BOARD_EVENTS gevent)
+{
+	struct smscore_config_gpio my_gpio_config;
 
-	sms_gpio_assign_11xx_default_led_config(&MyGpioConfig);
+	sms_gpio_assign_11xx_default_led_config(&my_gpio_config);
 
 	switch (gevent) {
 	case BOARD_EVENT_POWER_INIT: /* including hotplug */
@@ -182,8 +241,8 @@ static int sms_set_gpio(struct smscore_device_t *coredev, int pin, int enable)
 		.direction            = SMS_GPIO_DIRECTION_OUTPUT,
 		.pullupdown           = SMS_GPIO_PULLUPDOWN_NONE,
 		.inputcharacteristics = SMS_GPIO_INPUTCHARACTERISTICS_NORMAL,
-		.outputslewrate       = SMS_GPIO_OUTPUTSLEWRATE_FAST,
-		.outputdriving        = SMS_GPIO_OUTPUTDRIVING_4mA,
+		.outputslewrate       = SMS_GPIO_OUTPUT_SLEW_RATE_FAST,
+		.outputdriving        = SMS_GPIO_OUTPUTDRIVING_S_4mA,
 	};
 
 	if (pin == 0)
@@ -293,19 +352,7 @@ EXPORT_SYMBOL_GPL(sms_board_lna_control);
 
 int sms_board_load_modules(int id)
 {
-	switch (id) {
-	case SMS1XXX_BOARD_HAUPPAUGE_CATAMOUNT:
-	case SMS1XXX_BOARD_HAUPPAUGE_OKEMO_A:
-	case SMS1XXX_BOARD_HAUPPAUGE_OKEMO_B:
-	case SMS1XXX_BOARD_HAUPPAUGE_WINDHAM:
-	case SMS1XXX_BOARD_HAUPPAUGE_TIGER_MINICARD:
-	case SMS1XXX_BOARD_HAUPPAUGE_TIGER_MINICARD_R2:
-		request_module("smsdvb");
-		break;
-	default:
-		/* do nothing */
-		break;
-	}
+	request_module("smsdvb");
 	return 0;
 }
 EXPORT_SYMBOL_GPL(sms_board_load_modules);
diff --git a/drivers/media/common/siano/sms-cards.h b/drivers/media/common/siano/sms-cards.h
index d8cdf756f7c..c63b544c49c 100644
--- a/drivers/media/common/siano/sms-cards.h
+++ b/drivers/media/common/siano/sms-cards.h
@@ -37,6 +37,14 @@
 #define SMS1XXX_BOARD_HAUPPAUGE_TIGER_MINICARD_R2 10
 #define SMS1XXX_BOARD_SIANO_NICE	11
 #define SMS1XXX_BOARD_SIANO_VENICE	12
+#define SMS1XXX_BOARD_SIANO_STELLAR_ROM 13
+#define SMS1XXX_BOARD_ZTE_DVB_DATA_CARD	14
+#define SMS1XXX_BOARD_ONDA_MDTV_DATA_CARD 15
+#define SMS1XXX_BOARD_SIANO_MING	16
+#define SMS1XXX_BOARD_SIANO_PELE	17
+#define SMS1XXX_BOARD_SIANO_RIO		18
+#define SMS1XXX_BOARD_SIANO_DENVER_1530	19
+#define SMS1XXX_BOARD_SIANO_DENVER_2160 20
 
 struct sms_board_gpio_cfg {
 	int lna_vhf_exist;
@@ -79,6 +87,12 @@ struct sms_board {
 
 	/* gpios */
 	int led_power, led_hi, led_lo, lna_ctrl, rf_switch;
+
+	char intf_num;
+	int default_mode;
+	unsigned int mtu;
+	unsigned int crystal;
+	struct sms_antenna_config_ST *antenna_config;
 };
 
 struct sms_board *sms_get_board(unsigned id);
diff --git a/drivers/media/common/siano/smscoreapi.c b/drivers/media/common/siano/smscoreapi.c
index 1842e64e633..45ac9eea488 100644
--- a/drivers/media/common/siano/smscoreapi.c
+++ b/drivers/media/common/siano/smscoreapi.c
@@ -37,7 +37,6 @@
 #include "smscoreapi.h"
 #include "sms-cards.h"
 #include "smsir.h"
-#include "smsendian.h"
 
 static int sms_dbg;
 module_param_named(debug, sms_dbg, int, 0644);
@@ -58,11 +57,350 @@ struct smscore_client_t {
 	struct list_head entry;
 	struct smscore_device_t *coredev;
 	void			*context;
-	struct list_head 	idlist;
-	onresponse_t	onresponse_handler;
+	struct list_head	idlist;
+	onresponse_t		onresponse_handler;
 	onremove_t		onremove_handler;
 };
 
+static char *siano_msgs[] = {
+	[MSG_TYPE_BASE_VAL                           - MSG_TYPE_BASE_VAL] = "MSG_TYPE_BASE_VAL",
+	[MSG_SMS_GET_VERSION_REQ                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_GET_VERSION_REQ",
+	[MSG_SMS_GET_VERSION_RES                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_GET_VERSION_RES",
+	[MSG_SMS_MULTI_BRIDGE_CFG                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_MULTI_BRIDGE_CFG",
+	[MSG_SMS_GPIO_CONFIG_REQ                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_GPIO_CONFIG_REQ",
+	[MSG_SMS_GPIO_CONFIG_RES                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_GPIO_CONFIG_RES",
+	[MSG_SMS_GPIO_SET_LEVEL_REQ                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_GPIO_SET_LEVEL_REQ",
+	[MSG_SMS_GPIO_SET_LEVEL_RES                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_GPIO_SET_LEVEL_RES",
+	[MSG_SMS_GPIO_GET_LEVEL_REQ                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_GPIO_GET_LEVEL_REQ",
+	[MSG_SMS_GPIO_GET_LEVEL_RES                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_GPIO_GET_LEVEL_RES",
+	[MSG_SMS_EEPROM_BURN_IND                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_EEPROM_BURN_IND",
+	[MSG_SMS_LOG_ENABLE_CHANGE_REQ               - MSG_TYPE_BASE_VAL] = "MSG_SMS_LOG_ENABLE_CHANGE_REQ",
+	[MSG_SMS_LOG_ENABLE_CHANGE_RES               - MSG_TYPE_BASE_VAL] = "MSG_SMS_LOG_ENABLE_CHANGE_RES",
+	[MSG_SMS_SET_MAX_TX_MSG_LEN_REQ              - MSG_TYPE_BASE_VAL] = "MSG_SMS_SET_MAX_TX_MSG_LEN_REQ",
+	[MSG_SMS_SET_MAX_TX_MSG_LEN_RES              - MSG_TYPE_BASE_VAL] = "MSG_SMS_SET_MAX_TX_MSG_LEN_RES",
+	[MSG_SMS_SPI_HALFDUPLEX_TOKEN_HOST_TO_DEVICE - MSG_TYPE_BASE_VAL] = "MSG_SMS_SPI_HALFDUPLEX_TOKEN_HOST_TO_DEVICE",
+	[MSG_SMS_SPI_HALFDUPLEX_TOKEN_DEVICE_TO_HOST - MSG_TYPE_BASE_VAL] = "MSG_SMS_SPI_HALFDUPLEX_TOKEN_DEVICE_TO_HOST",
+	[MSG_SMS_BACKGROUND_SCAN_FLAG_CHANGE_REQ     - MSG_TYPE_BASE_VAL] = "MSG_SMS_BACKGROUND_SCAN_FLAG_CHANGE_REQ",
+	[MSG_SMS_BACKGROUND_SCAN_FLAG_CHANGE_RES     - MSG_TYPE_BASE_VAL] = "MSG_SMS_BACKGROUND_SCAN_FLAG_CHANGE_RES",
+	[MSG_SMS_BACKGROUND_SCAN_SIGNAL_DETECTED_IND - MSG_TYPE_BASE_VAL] = "MSG_SMS_BACKGROUND_SCAN_SIGNAL_DETECTED_IND",
+	[MSG_SMS_BACKGROUND_SCAN_NO_SIGNAL_IND       - MSG_TYPE_BASE_VAL] = "MSG_SMS_BACKGROUND_SCAN_NO_SIGNAL_IND",
+	[MSG_SMS_CONFIGURE_RF_SWITCH_REQ             - MSG_TYPE_BASE_VAL] = "MSG_SMS_CONFIGURE_RF_SWITCH_REQ",
+	[MSG_SMS_CONFIGURE_RF_SWITCH_RES             - MSG_TYPE_BASE_VAL] = "MSG_SMS_CONFIGURE_RF_SWITCH_RES",
+	[MSG_SMS_MRC_PATH_DISCONNECT_REQ             - MSG_TYPE_BASE_VAL] = "MSG_SMS_MRC_PATH_DISCONNECT_REQ",
+	[MSG_SMS_MRC_PATH_DISCONNECT_RES             - MSG_TYPE_BASE_VAL] = "MSG_SMS_MRC_PATH_DISCONNECT_RES",
+	[MSG_SMS_RECEIVE_1SEG_THROUGH_FULLSEG_REQ    - MSG_TYPE_BASE_VAL] = "MSG_SMS_RECEIVE_1SEG_THROUGH_FULLSEG_REQ",
+	[MSG_SMS_RECEIVE_1SEG_THROUGH_FULLSEG_RES    - MSG_TYPE_BASE_VAL] = "MSG_SMS_RECEIVE_1SEG_THROUGH_FULLSEG_RES",
+	[MSG_SMS_RECEIVE_VHF_VIA_VHF_INPUT_REQ       - MSG_TYPE_BASE_VAL] = "MSG_SMS_RECEIVE_VHF_VIA_VHF_INPUT_REQ",
+	[MSG_SMS_RECEIVE_VHF_VIA_VHF_INPUT_RES       - MSG_TYPE_BASE_VAL] = "MSG_SMS_RECEIVE_VHF_VIA_VHF_INPUT_RES",
+	[MSG_WR_REG_RFT_REQ                          - MSG_TYPE_BASE_VAL] = "MSG_WR_REG_RFT_REQ",
+	[MSG_WR_REG_RFT_RES                          - MSG_TYPE_BASE_VAL] = "MSG_WR_REG_RFT_RES",
+	[MSG_RD_REG_RFT_REQ                          - MSG_TYPE_BASE_VAL] = "MSG_RD_REG_RFT_REQ",
+	[MSG_RD_REG_RFT_RES                          - MSG_TYPE_BASE_VAL] = "MSG_RD_REG_RFT_RES",
+	[MSG_RD_REG_ALL_RFT_REQ                      - MSG_TYPE_BASE_VAL] = "MSG_RD_REG_ALL_RFT_REQ",
+	[MSG_RD_REG_ALL_RFT_RES                      - MSG_TYPE_BASE_VAL] = "MSG_RD_REG_ALL_RFT_RES",
+	[MSG_HELP_INT                                - MSG_TYPE_BASE_VAL] = "MSG_HELP_INT",
+	[MSG_RUN_SCRIPT_INT                          - MSG_TYPE_BASE_VAL] = "MSG_RUN_SCRIPT_INT",
+	[MSG_SMS_EWS_INBAND_REQ                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_EWS_INBAND_REQ",
+	[MSG_SMS_EWS_INBAND_RES                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_EWS_INBAND_RES",
+	[MSG_SMS_RFS_SELECT_REQ                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_RFS_SELECT_REQ",
+	[MSG_SMS_RFS_SELECT_RES                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_RFS_SELECT_RES",
+	[MSG_SMS_MB_GET_VER_REQ                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_MB_GET_VER_REQ",
+	[MSG_SMS_MB_GET_VER_RES                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_MB_GET_VER_RES",
+	[MSG_SMS_MB_WRITE_CFGFILE_REQ                - MSG_TYPE_BASE_VAL] = "MSG_SMS_MB_WRITE_CFGFILE_REQ",
+	[MSG_SMS_MB_WRITE_CFGFILE_RES                - MSG_TYPE_BASE_VAL] = "MSG_SMS_MB_WRITE_CFGFILE_RES",
+	[MSG_SMS_MB_READ_CFGFILE_REQ                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_MB_READ_CFGFILE_REQ",
+	[MSG_SMS_MB_READ_CFGFILE_RES                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_MB_READ_CFGFILE_RES",
+	[MSG_SMS_RD_MEM_REQ                          - MSG_TYPE_BASE_VAL] = "MSG_SMS_RD_MEM_REQ",
+	[MSG_SMS_RD_MEM_RES                          - MSG_TYPE_BASE_VAL] = "MSG_SMS_RD_MEM_RES",
+	[MSG_SMS_WR_MEM_REQ                          - MSG_TYPE_BASE_VAL] = "MSG_SMS_WR_MEM_REQ",
+	[MSG_SMS_WR_MEM_RES                          - MSG_TYPE_BASE_VAL] = "MSG_SMS_WR_MEM_RES",
+	[MSG_SMS_UPDATE_MEM_REQ                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_UPDATE_MEM_REQ",
+	[MSG_SMS_UPDATE_MEM_RES                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_UPDATE_MEM_RES",
+	[MSG_SMS_ISDBT_ENABLE_FULL_PARAMS_SET_REQ    - MSG_TYPE_BASE_VAL] = "MSG_SMS_ISDBT_ENABLE_FULL_PARAMS_SET_REQ",
+	[MSG_SMS_ISDBT_ENABLE_FULL_PARAMS_SET_RES    - MSG_TYPE_BASE_VAL] = "MSG_SMS_ISDBT_ENABLE_FULL_PARAMS_SET_RES",
+	[MSG_SMS_RF_TUNE_REQ                         - MSG_TYPE_BASE_VAL] = "MSG_SMS_RF_TUNE_REQ",
+	[MSG_SMS_RF_TUNE_RES                         - MSG_TYPE_BASE_VAL] = "MSG_SMS_RF_TUNE_RES",
+	[MSG_SMS_ISDBT_ENABLE_HIGH_MOBILITY_REQ      - MSG_TYPE_BASE_VAL] = "MSG_SMS_ISDBT_ENABLE_HIGH_MOBILITY_REQ",
+	[MSG_SMS_ISDBT_ENABLE_HIGH_MOBILITY_RES      - MSG_TYPE_BASE_VAL] = "MSG_SMS_ISDBT_ENABLE_HIGH_MOBILITY_RES",
+	[MSG_SMS_ISDBT_SB_RECEPTION_REQ              - MSG_TYPE_BASE_VAL] = "MSG_SMS_ISDBT_SB_RECEPTION_REQ",
+	[MSG_SMS_ISDBT_SB_RECEPTION_RES              - MSG_TYPE_BASE_VAL] = "MSG_SMS_ISDBT_SB_RECEPTION_RES",
+	[MSG_SMS_GENERIC_EPROM_WRITE_REQ             - MSG_TYPE_BASE_VAL] = "MSG_SMS_GENERIC_EPROM_WRITE_REQ",
+	[MSG_SMS_GENERIC_EPROM_WRITE_RES             - MSG_TYPE_BASE_VAL] = "MSG_SMS_GENERIC_EPROM_WRITE_RES",
+	[MSG_SMS_GENERIC_EPROM_READ_REQ              - MSG_TYPE_BASE_VAL] = "MSG_SMS_GENERIC_EPROM_READ_REQ",
+	[MSG_SMS_GENERIC_EPROM_READ_RES              - MSG_TYPE_BASE_VAL] = "MSG_SMS_GENERIC_EPROM_READ_RES",
+	[MSG_SMS_EEPROM_WRITE_REQ                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_EEPROM_WRITE_REQ",
+	[MSG_SMS_EEPROM_WRITE_RES                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_EEPROM_WRITE_RES",
+	[MSG_SMS_CUSTOM_READ_REQ                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_CUSTOM_READ_REQ",
+	[MSG_SMS_CUSTOM_READ_RES                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_CUSTOM_READ_RES",
+	[MSG_SMS_CUSTOM_WRITE_REQ                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_CUSTOM_WRITE_REQ",
+	[MSG_SMS_CUSTOM_WRITE_RES                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_CUSTOM_WRITE_RES",
+	[MSG_SMS_INIT_DEVICE_REQ                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_INIT_DEVICE_REQ",
+	[MSG_SMS_INIT_DEVICE_RES                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_INIT_DEVICE_RES",
+	[MSG_SMS_ATSC_SET_ALL_IP_REQ                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_SET_ALL_IP_REQ",
+	[MSG_SMS_ATSC_SET_ALL_IP_RES                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_SET_ALL_IP_RES",
+	[MSG_SMS_ATSC_START_ENSEMBLE_REQ             - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_START_ENSEMBLE_REQ",
+	[MSG_SMS_ATSC_START_ENSEMBLE_RES             - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_START_ENSEMBLE_RES",
+	[MSG_SMS_SET_OUTPUT_MODE_REQ                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_SET_OUTPUT_MODE_REQ",
+	[MSG_SMS_SET_OUTPUT_MODE_RES                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_SET_OUTPUT_MODE_RES",
+	[MSG_SMS_ATSC_IP_FILTER_GET_LIST_REQ         - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_IP_FILTER_GET_LIST_REQ",
+	[MSG_SMS_ATSC_IP_FILTER_GET_LIST_RES         - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_IP_FILTER_GET_LIST_RES",
+	[MSG_SMS_SUB_CHANNEL_START_REQ               - MSG_TYPE_BASE_VAL] = "MSG_SMS_SUB_CHANNEL_START_REQ",
+	[MSG_SMS_SUB_CHANNEL_START_RES               - MSG_TYPE_BASE_VAL] = "MSG_SMS_SUB_CHANNEL_START_RES",
+	[MSG_SMS_SUB_CHANNEL_STOP_REQ                - MSG_TYPE_BASE_VAL] = "MSG_SMS_SUB_CHANNEL_STOP_REQ",
+	[MSG_SMS_SUB_CHANNEL_STOP_RES                - MSG_TYPE_BASE_VAL] = "MSG_SMS_SUB_CHANNEL_STOP_RES",
+	[MSG_SMS_ATSC_IP_FILTER_ADD_REQ              - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_IP_FILTER_ADD_REQ",
+	[MSG_SMS_ATSC_IP_FILTER_ADD_RES              - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_IP_FILTER_ADD_RES",
+	[MSG_SMS_ATSC_IP_FILTER_REMOVE_REQ           - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_IP_FILTER_REMOVE_REQ",
+	[MSG_SMS_ATSC_IP_FILTER_REMOVE_RES           - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_IP_FILTER_REMOVE_RES",
+	[MSG_SMS_ATSC_IP_FILTER_REMOVE_ALL_REQ       - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_IP_FILTER_REMOVE_ALL_REQ",
+	[MSG_SMS_ATSC_IP_FILTER_REMOVE_ALL_RES       - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_IP_FILTER_REMOVE_ALL_RES",
+	[MSG_SMS_WAIT_CMD                            - MSG_TYPE_BASE_VAL] = "MSG_SMS_WAIT_CMD",
+	[MSG_SMS_ADD_PID_FILTER_REQ                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_ADD_PID_FILTER_REQ",
+	[MSG_SMS_ADD_PID_FILTER_RES                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_ADD_PID_FILTER_RES",
+	[MSG_SMS_REMOVE_PID_FILTER_REQ               - MSG_TYPE_BASE_VAL] = "MSG_SMS_REMOVE_PID_FILTER_REQ",
+	[MSG_SMS_REMOVE_PID_FILTER_RES               - MSG_TYPE_BASE_VAL] = "MSG_SMS_REMOVE_PID_FILTER_RES",
+	[MSG_SMS_FAST_INFORMATION_CHANNEL_REQ        - MSG_TYPE_BASE_VAL] = "MSG_SMS_FAST_INFORMATION_CHANNEL_REQ",
+	[MSG_SMS_FAST_INFORMATION_CHANNEL_RES        - MSG_TYPE_BASE_VAL] = "MSG_SMS_FAST_INFORMATION_CHANNEL_RES",
+	[MSG_SMS_DAB_CHANNEL                         - MSG_TYPE_BASE_VAL] = "MSG_SMS_DAB_CHANNEL",
+	[MSG_SMS_GET_PID_FILTER_LIST_REQ             - MSG_TYPE_BASE_VAL] = "MSG_SMS_GET_PID_FILTER_LIST_REQ",
+	[MSG_SMS_GET_PID_FILTER_LIST_RES             - MSG_TYPE_BASE_VAL] = "MSG_SMS_GET_PID_FILTER_LIST_RES",
+	[MSG_SMS_POWER_DOWN_REQ                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_POWER_DOWN_REQ",
+	[MSG_SMS_POWER_DOWN_RES                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_POWER_DOWN_RES",
+	[MSG_SMS_ATSC_SLT_EXIST_IND                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_SLT_EXIST_IND",
+	[MSG_SMS_ATSC_NO_SLT_IND                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_NO_SLT_IND",
+	[MSG_SMS_GET_STATISTICS_REQ                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_GET_STATISTICS_REQ",
+	[MSG_SMS_GET_STATISTICS_RES                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_GET_STATISTICS_RES",
+	[MSG_SMS_SEND_DUMP                           - MSG_TYPE_BASE_VAL] = "MSG_SMS_SEND_DUMP",
+	[MSG_SMS_SCAN_START_REQ                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_SCAN_START_REQ",
+	[MSG_SMS_SCAN_START_RES                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_SCAN_START_RES",
+	[MSG_SMS_SCAN_STOP_REQ                       - MSG_TYPE_BASE_VAL] = "MSG_SMS_SCAN_STOP_REQ",
+	[MSG_SMS_SCAN_STOP_RES                       - MSG_TYPE_BASE_VAL] = "MSG_SMS_SCAN_STOP_RES",
+	[MSG_SMS_SCAN_PROGRESS_IND                   - MSG_TYPE_BASE_VAL] = "MSG_SMS_SCAN_PROGRESS_IND",
+	[MSG_SMS_SCAN_COMPLETE_IND                   - MSG_TYPE_BASE_VAL] = "MSG_SMS_SCAN_COMPLETE_IND",
+	[MSG_SMS_LOG_ITEM                            - MSG_TYPE_BASE_VAL] = "MSG_SMS_LOG_ITEM",
+	[MSG_SMS_DAB_SUBCHANNEL_RECONFIG_REQ         - MSG_TYPE_BASE_VAL] = "MSG_SMS_DAB_SUBCHANNEL_RECONFIG_REQ",
+	[MSG_SMS_DAB_SUBCHANNEL_RECONFIG_RES         - MSG_TYPE_BASE_VAL] = "MSG_SMS_DAB_SUBCHANNEL_RECONFIG_RES",
+	[MSG_SMS_HO_PER_SLICES_IND                   - MSG_TYPE_BASE_VAL] = "MSG_SMS_HO_PER_SLICES_IND",
+	[MSG_SMS_HO_INBAND_POWER_IND                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_HO_INBAND_POWER_IND",
+	[MSG_SMS_MANUAL_DEMOD_REQ                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_MANUAL_DEMOD_REQ",
+	[MSG_SMS_HO_TUNE_ON_REQ                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_HO_TUNE_ON_REQ",
+	[MSG_SMS_HO_TUNE_ON_RES                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_HO_TUNE_ON_RES",
+	[MSG_SMS_HO_TUNE_OFF_REQ                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_HO_TUNE_OFF_REQ",
+	[MSG_SMS_HO_TUNE_OFF_RES                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_HO_TUNE_OFF_RES",
+	[MSG_SMS_HO_PEEK_FREQ_REQ                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_HO_PEEK_FREQ_REQ",
+	[MSG_SMS_HO_PEEK_FREQ_RES                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_HO_PEEK_FREQ_RES",
+	[MSG_SMS_HO_PEEK_FREQ_IND                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_HO_PEEK_FREQ_IND",
+	[MSG_SMS_MB_ATTEN_SET_REQ                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_MB_ATTEN_SET_REQ",
+	[MSG_SMS_MB_ATTEN_SET_RES                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_MB_ATTEN_SET_RES",
+	[MSG_SMS_ENABLE_STAT_IN_I2C_REQ              - MSG_TYPE_BASE_VAL] = "MSG_SMS_ENABLE_STAT_IN_I2C_REQ",
+	[MSG_SMS_ENABLE_STAT_IN_I2C_RES              - MSG_TYPE_BASE_VAL] = "MSG_SMS_ENABLE_STAT_IN_I2C_RES",
+	[MSG_SMS_SET_ANTENNA_CONFIG_REQ              - MSG_TYPE_BASE_VAL] = "MSG_SMS_SET_ANTENNA_CONFIG_REQ",
+	[MSG_SMS_SET_ANTENNA_CONFIG_RES              - MSG_TYPE_BASE_VAL] = "MSG_SMS_SET_ANTENNA_CONFIG_RES",
+	[MSG_SMS_GET_STATISTICS_EX_REQ               - MSG_TYPE_BASE_VAL] = "MSG_SMS_GET_STATISTICS_EX_REQ",
+	[MSG_SMS_GET_STATISTICS_EX_RES               - MSG_TYPE_BASE_VAL] = "MSG_SMS_GET_STATISTICS_EX_RES",
+	[MSG_SMS_SLEEP_RESUME_COMP_IND               - MSG_TYPE_BASE_VAL] = "MSG_SMS_SLEEP_RESUME_COMP_IND",
+	[MSG_SMS_SWITCH_HOST_INTERFACE_REQ           - MSG_TYPE_BASE_VAL] = "MSG_SMS_SWITCH_HOST_INTERFACE_REQ",
+	[MSG_SMS_SWITCH_HOST_INTERFACE_RES           - MSG_TYPE_BASE_VAL] = "MSG_SMS_SWITCH_HOST_INTERFACE_RES",
+	[MSG_SMS_DATA_DOWNLOAD_REQ                   - MSG_TYPE_BASE_VAL] = "MSG_SMS_DATA_DOWNLOAD_REQ",
+	[MSG_SMS_DATA_DOWNLOAD_RES                   - MSG_TYPE_BASE_VAL] = "MSG_SMS_DATA_DOWNLOAD_RES",
+	[MSG_SMS_DATA_VALIDITY_REQ                   - MSG_TYPE_BASE_VAL] = "MSG_SMS_DATA_VALIDITY_REQ",
+	[MSG_SMS_DATA_VALIDITY_RES                   - MSG_TYPE_BASE_VAL] = "MSG_SMS_DATA_VALIDITY_RES",
+	[MSG_SMS_SWDOWNLOAD_TRIGGER_REQ              - MSG_TYPE_BASE_VAL] = "MSG_SMS_SWDOWNLOAD_TRIGGER_REQ",
+	[MSG_SMS_SWDOWNLOAD_TRIGGER_RES              - MSG_TYPE_BASE_VAL] = "MSG_SMS_SWDOWNLOAD_TRIGGER_RES",
+	[MSG_SMS_SWDOWNLOAD_BACKDOOR_REQ             - MSG_TYPE_BASE_VAL] = "MSG_SMS_SWDOWNLOAD_BACKDOOR_REQ",
+	[MSG_SMS_SWDOWNLOAD_BACKDOOR_RES             - MSG_TYPE_BASE_VAL] = "MSG_SMS_SWDOWNLOAD_BACKDOOR_RES",
+	[MSG_SMS_GET_VERSION_EX_REQ                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_GET_VERSION_EX_REQ",
+	[MSG_SMS_GET_VERSION_EX_RES                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_GET_VERSION_EX_RES",
+	[MSG_SMS_CLOCK_OUTPUT_CONFIG_REQ             - MSG_TYPE_BASE_VAL] = "MSG_SMS_CLOCK_OUTPUT_CONFIG_REQ",
+	[MSG_SMS_CLOCK_OUTPUT_CONFIG_RES             - MSG_TYPE_BASE_VAL] = "MSG_SMS_CLOCK_OUTPUT_CONFIG_RES",
+	[MSG_SMS_I2C_SET_FREQ_REQ                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_I2C_SET_FREQ_REQ",
+	[MSG_SMS_I2C_SET_FREQ_RES                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_I2C_SET_FREQ_RES",
+	[MSG_SMS_GENERIC_I2C_REQ                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_GENERIC_I2C_REQ",
+	[MSG_SMS_GENERIC_I2C_RES                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_GENERIC_I2C_RES",
+	[MSG_SMS_DVBT_BDA_DATA                       - MSG_TYPE_BASE_VAL] = "MSG_SMS_DVBT_BDA_DATA",
+	[MSG_SW_RELOAD_REQ                           - MSG_TYPE_BASE_VAL] = "MSG_SW_RELOAD_REQ",
+	[MSG_SMS_DATA_MSG                            - MSG_TYPE_BASE_VAL] = "MSG_SMS_DATA_MSG",
+	[MSG_TABLE_UPLOAD_REQ                        - MSG_TYPE_BASE_VAL] = "MSG_TABLE_UPLOAD_REQ",
+	[MSG_TABLE_UPLOAD_RES                        - MSG_TYPE_BASE_VAL] = "MSG_TABLE_UPLOAD_RES",
+	[MSG_SW_RELOAD_START_REQ                     - MSG_TYPE_BASE_VAL] = "MSG_SW_RELOAD_START_REQ",
+	[MSG_SW_RELOAD_START_RES                     - MSG_TYPE_BASE_VAL] = "MSG_SW_RELOAD_START_RES",
+	[MSG_SW_RELOAD_EXEC_REQ                      - MSG_TYPE_BASE_VAL] = "MSG_SW_RELOAD_EXEC_REQ",
+	[MSG_SW_RELOAD_EXEC_RES                      - MSG_TYPE_BASE_VAL] = "MSG_SW_RELOAD_EXEC_RES",
+	[MSG_SMS_SPI_INT_LINE_SET_REQ                - MSG_TYPE_BASE_VAL] = "MSG_SMS_SPI_INT_LINE_SET_REQ",
+	[MSG_SMS_SPI_INT_LINE_SET_RES                - MSG_TYPE_BASE_VAL] = "MSG_SMS_SPI_INT_LINE_SET_RES",
+	[MSG_SMS_GPIO_CONFIG_EX_REQ                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_GPIO_CONFIG_EX_REQ",
+	[MSG_SMS_GPIO_CONFIG_EX_RES                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_GPIO_CONFIG_EX_RES",
+	[MSG_SMS_WATCHDOG_ACT_REQ                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_WATCHDOG_ACT_REQ",
+	[MSG_SMS_WATCHDOG_ACT_RES                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_WATCHDOG_ACT_RES",
+	[MSG_SMS_LOOPBACK_REQ                        - MSG_TYPE_BASE_VAL] = "MSG_SMS_LOOPBACK_REQ",
+	[MSG_SMS_LOOPBACK_RES                        - MSG_TYPE_BASE_VAL] = "MSG_SMS_LOOPBACK_RES",
+	[MSG_SMS_RAW_CAPTURE_START_REQ               - MSG_TYPE_BASE_VAL] = "MSG_SMS_RAW_CAPTURE_START_REQ",
+	[MSG_SMS_RAW_CAPTURE_START_RES               - MSG_TYPE_BASE_VAL] = "MSG_SMS_RAW_CAPTURE_START_RES",
+	[MSG_SMS_RAW_CAPTURE_ABORT_REQ               - MSG_TYPE_BASE_VAL] = "MSG_SMS_RAW_CAPTURE_ABORT_REQ",
+	[MSG_SMS_RAW_CAPTURE_ABORT_RES               - MSG_TYPE_BASE_VAL] = "MSG_SMS_RAW_CAPTURE_ABORT_RES",
+	[MSG_SMS_RAW_CAPTURE_COMPLETE_IND            - MSG_TYPE_BASE_VAL] = "MSG_SMS_RAW_CAPTURE_COMPLETE_IND",
+	[MSG_SMS_DATA_PUMP_IND                       - MSG_TYPE_BASE_VAL] = "MSG_SMS_DATA_PUMP_IND",
+	[MSG_SMS_DATA_PUMP_REQ                       - MSG_TYPE_BASE_VAL] = "MSG_SMS_DATA_PUMP_REQ",
+	[MSG_SMS_DATA_PUMP_RES                       - MSG_TYPE_BASE_VAL] = "MSG_SMS_DATA_PUMP_RES",
+	[MSG_SMS_FLASH_DL_REQ                        - MSG_TYPE_BASE_VAL] = "MSG_SMS_FLASH_DL_REQ",
+	[MSG_SMS_EXEC_TEST_1_REQ                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_EXEC_TEST_1_REQ",
+	[MSG_SMS_EXEC_TEST_1_RES                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_EXEC_TEST_1_RES",
+	[MSG_SMS_ENBALE_TS_INTERFACE_REQ             - MSG_TYPE_BASE_VAL] = "MSG_SMS_ENBALE_TS_INTERFACE_REQ",
+	[MSG_SMS_ENBALE_TS_INTERFACE_RES             - MSG_TYPE_BASE_VAL] = "MSG_SMS_ENBALE_TS_INTERFACE_RES",
+	[MSG_SMS_SPI_SET_BUS_WIDTH_REQ               - MSG_TYPE_BASE_VAL] = "MSG_SMS_SPI_SET_BUS_WIDTH_REQ",
+	[MSG_SMS_SPI_SET_BUS_WIDTH_RES               - MSG_TYPE_BASE_VAL] = "MSG_SMS_SPI_SET_BUS_WIDTH_RES",
+	[MSG_SMS_SEND_EMM_REQ                        - MSG_TYPE_BASE_VAL] = "MSG_SMS_SEND_EMM_REQ",
+	[MSG_SMS_SEND_EMM_RES                        - MSG_TYPE_BASE_VAL] = "MSG_SMS_SEND_EMM_RES",
+	[MSG_SMS_DISABLE_TS_INTERFACE_REQ            - MSG_TYPE_BASE_VAL] = "MSG_SMS_DISABLE_TS_INTERFACE_REQ",
+	[MSG_SMS_DISABLE_TS_INTERFACE_RES            - MSG_TYPE_BASE_VAL] = "MSG_SMS_DISABLE_TS_INTERFACE_RES",
+	[MSG_SMS_IS_BUF_FREE_REQ                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_IS_BUF_FREE_REQ",
+	[MSG_SMS_IS_BUF_FREE_RES                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_IS_BUF_FREE_RES",
+	[MSG_SMS_EXT_ANTENNA_REQ                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_EXT_ANTENNA_REQ",
+	[MSG_SMS_EXT_ANTENNA_RES                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_EXT_ANTENNA_RES",
+	[MSG_SMS_CMMB_GET_NET_OF_FREQ_REQ_OBSOLETE   - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_GET_NET_OF_FREQ_REQ_OBSOLETE",
+	[MSG_SMS_CMMB_GET_NET_OF_FREQ_RES_OBSOLETE   - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_GET_NET_OF_FREQ_RES_OBSOLETE",
+	[MSG_SMS_BATTERY_LEVEL_REQ                   - MSG_TYPE_BASE_VAL] = "MSG_SMS_BATTERY_LEVEL_REQ",
+	[MSG_SMS_BATTERY_LEVEL_RES                   - MSG_TYPE_BASE_VAL] = "MSG_SMS_BATTERY_LEVEL_RES",
+	[MSG_SMS_CMMB_INJECT_TABLE_REQ_OBSOLETE      - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_INJECT_TABLE_REQ_OBSOLETE",
+	[MSG_SMS_CMMB_INJECT_TABLE_RES_OBSOLETE      - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_INJECT_TABLE_RES_OBSOLETE",
+	[MSG_SMS_FM_RADIO_BLOCK_IND                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_FM_RADIO_BLOCK_IND",
+	[MSG_SMS_HOST_NOTIFICATION_IND               - MSG_TYPE_BASE_VAL] = "MSG_SMS_HOST_NOTIFICATION_IND",
+	[MSG_SMS_CMMB_GET_CONTROL_TABLE_REQ_OBSOLETE - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_GET_CONTROL_TABLE_REQ_OBSOLETE",
+	[MSG_SMS_CMMB_GET_CONTROL_TABLE_RES_OBSOLETE - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_GET_CONTROL_TABLE_RES_OBSOLETE",
+	[MSG_SMS_CMMB_GET_NETWORKS_REQ               - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_GET_NETWORKS_REQ",
+	[MSG_SMS_CMMB_GET_NETWORKS_RES               - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_GET_NETWORKS_RES",
+	[MSG_SMS_CMMB_START_SERVICE_REQ              - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_START_SERVICE_REQ",
+	[MSG_SMS_CMMB_START_SERVICE_RES              - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_START_SERVICE_RES",
+	[MSG_SMS_CMMB_STOP_SERVICE_REQ               - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_STOP_SERVICE_REQ",
+	[MSG_SMS_CMMB_STOP_SERVICE_RES               - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_STOP_SERVICE_RES",
+	[MSG_SMS_CMMB_ADD_CHANNEL_FILTER_REQ         - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_ADD_CHANNEL_FILTER_REQ",
+	[MSG_SMS_CMMB_ADD_CHANNEL_FILTER_RES         - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_ADD_CHANNEL_FILTER_RES",
+	[MSG_SMS_CMMB_REMOVE_CHANNEL_FILTER_REQ      - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_REMOVE_CHANNEL_FILTER_REQ",
+	[MSG_SMS_CMMB_REMOVE_CHANNEL_FILTER_RES      - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_REMOVE_CHANNEL_FILTER_RES",
+	[MSG_SMS_CMMB_START_CONTROL_INFO_REQ         - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_START_CONTROL_INFO_REQ",
+	[MSG_SMS_CMMB_START_CONTROL_INFO_RES         - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_START_CONTROL_INFO_RES",
+	[MSG_SMS_CMMB_STOP_CONTROL_INFO_REQ          - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_STOP_CONTROL_INFO_REQ",
+	[MSG_SMS_CMMB_STOP_CONTROL_INFO_RES          - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_STOP_CONTROL_INFO_RES",
+	[MSG_SMS_ISDBT_TUNE_REQ                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_ISDBT_TUNE_REQ",
+	[MSG_SMS_ISDBT_TUNE_RES                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_ISDBT_TUNE_RES",
+	[MSG_SMS_TRANSMISSION_IND                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_TRANSMISSION_IND",
+	[MSG_SMS_PID_STATISTICS_IND                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_PID_STATISTICS_IND",
+	[MSG_SMS_POWER_DOWN_IND                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_POWER_DOWN_IND",
+	[MSG_SMS_POWER_DOWN_CONF                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_POWER_DOWN_CONF",
+	[MSG_SMS_POWER_UP_IND                        - MSG_TYPE_BASE_VAL] = "MSG_SMS_POWER_UP_IND",
+	[MSG_SMS_POWER_UP_CONF                       - MSG_TYPE_BASE_VAL] = "MSG_SMS_POWER_UP_CONF",
+	[MSG_SMS_POWER_MODE_SET_REQ                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_POWER_MODE_SET_REQ",
+	[MSG_SMS_POWER_MODE_SET_RES                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_POWER_MODE_SET_RES",
+	[MSG_SMS_DEBUG_HOST_EVENT_REQ                - MSG_TYPE_BASE_VAL] = "MSG_SMS_DEBUG_HOST_EVENT_REQ",
+	[MSG_SMS_DEBUG_HOST_EVENT_RES                - MSG_TYPE_BASE_VAL] = "MSG_SMS_DEBUG_HOST_EVENT_RES",
+	[MSG_SMS_NEW_CRYSTAL_REQ                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_NEW_CRYSTAL_REQ",
+	[MSG_SMS_NEW_CRYSTAL_RES                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_NEW_CRYSTAL_RES",
+	[MSG_SMS_CONFIG_SPI_REQ                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_CONFIG_SPI_REQ",
+	[MSG_SMS_CONFIG_SPI_RES                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_CONFIG_SPI_RES",
+	[MSG_SMS_I2C_SHORT_STAT_IND                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_I2C_SHORT_STAT_IND",
+	[MSG_SMS_START_IR_REQ                        - MSG_TYPE_BASE_VAL] = "MSG_SMS_START_IR_REQ",
+	[MSG_SMS_START_IR_RES                        - MSG_TYPE_BASE_VAL] = "MSG_SMS_START_IR_RES",
+	[MSG_SMS_IR_SAMPLES_IND                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_IR_SAMPLES_IND",
+	[MSG_SMS_CMMB_CA_SERVICE_IND                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_CA_SERVICE_IND",
+	[MSG_SMS_SLAVE_DEVICE_DETECTED               - MSG_TYPE_BASE_VAL] = "MSG_SMS_SLAVE_DEVICE_DETECTED",
+	[MSG_SMS_INTERFACE_LOCK_IND                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_INTERFACE_LOCK_IND",
+	[MSG_SMS_INTERFACE_UNLOCK_IND                - MSG_TYPE_BASE_VAL] = "MSG_SMS_INTERFACE_UNLOCK_IND",
+	[MSG_SMS_SEND_ROSUM_BUFF_REQ                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_SEND_ROSUM_BUFF_REQ",
+	[MSG_SMS_SEND_ROSUM_BUFF_RES                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_SEND_ROSUM_BUFF_RES",
+	[MSG_SMS_ROSUM_BUFF                          - MSG_TYPE_BASE_VAL] = "MSG_SMS_ROSUM_BUFF",
+	[MSG_SMS_SET_AES128_KEY_REQ                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_SET_AES128_KEY_REQ",
+	[MSG_SMS_SET_AES128_KEY_RES                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_SET_AES128_KEY_RES",
+	[MSG_SMS_MBBMS_WRITE_REQ                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_MBBMS_WRITE_REQ",
+	[MSG_SMS_MBBMS_WRITE_RES                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_MBBMS_WRITE_RES",
+	[MSG_SMS_MBBMS_READ_IND                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_MBBMS_READ_IND",
+	[MSG_SMS_IQ_STREAM_START_REQ                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_IQ_STREAM_START_REQ",
+	[MSG_SMS_IQ_STREAM_START_RES                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_IQ_STREAM_START_RES",
+	[MSG_SMS_IQ_STREAM_STOP_REQ                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_IQ_STREAM_STOP_REQ",
+	[MSG_SMS_IQ_STREAM_STOP_RES                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_IQ_STREAM_STOP_RES",
+	[MSG_SMS_IQ_STREAM_DATA_BLOCK                - MSG_TYPE_BASE_VAL] = "MSG_SMS_IQ_STREAM_DATA_BLOCK",
+	[MSG_SMS_GET_EEPROM_VERSION_REQ              - MSG_TYPE_BASE_VAL] = "MSG_SMS_GET_EEPROM_VERSION_REQ",
+	[MSG_SMS_GET_EEPROM_VERSION_RES              - MSG_TYPE_BASE_VAL] = "MSG_SMS_GET_EEPROM_VERSION_RES",
+	[MSG_SMS_SIGNAL_DETECTED_IND                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_SIGNAL_DETECTED_IND",
+	[MSG_SMS_NO_SIGNAL_IND                       - MSG_TYPE_BASE_VAL] = "MSG_SMS_NO_SIGNAL_IND",
+	[MSG_SMS_MRC_SHUTDOWN_SLAVE_REQ              - MSG_TYPE_BASE_VAL] = "MSG_SMS_MRC_SHUTDOWN_SLAVE_REQ",
+	[MSG_SMS_MRC_SHUTDOWN_SLAVE_RES              - MSG_TYPE_BASE_VAL] = "MSG_SMS_MRC_SHUTDOWN_SLAVE_RES",
+	[MSG_SMS_MRC_BRINGUP_SLAVE_REQ               - MSG_TYPE_BASE_VAL] = "MSG_SMS_MRC_BRINGUP_SLAVE_REQ",
+	[MSG_SMS_MRC_BRINGUP_SLAVE_RES               - MSG_TYPE_BASE_VAL] = "MSG_SMS_MRC_BRINGUP_SLAVE_RES",
+	[MSG_SMS_EXTERNAL_LNA_CTRL_REQ               - MSG_TYPE_BASE_VAL] = "MSG_SMS_EXTERNAL_LNA_CTRL_REQ",
+	[MSG_SMS_EXTERNAL_LNA_CTRL_RES               - MSG_TYPE_BASE_VAL] = "MSG_SMS_EXTERNAL_LNA_CTRL_RES",
+	[MSG_SMS_SET_PERIODIC_STATISTICS_REQ         - MSG_TYPE_BASE_VAL] = "MSG_SMS_SET_PERIODIC_STATISTICS_REQ",
+	[MSG_SMS_SET_PERIODIC_STATISTICS_RES         - MSG_TYPE_BASE_VAL] = "MSG_SMS_SET_PERIODIC_STATISTICS_RES",
+	[MSG_SMS_CMMB_SET_AUTO_OUTPUT_TS0_REQ        - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_SET_AUTO_OUTPUT_TS0_REQ",
+	[MSG_SMS_CMMB_SET_AUTO_OUTPUT_TS0_RES        - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_SET_AUTO_OUTPUT_TS0_RES",
+	[LOCAL_TUNE                                  - MSG_TYPE_BASE_VAL] = "LOCAL_TUNE",
+	[LOCAL_IFFT_H_ICI                            - MSG_TYPE_BASE_VAL] = "LOCAL_IFFT_H_ICI",
+	[MSG_RESYNC_REQ                              - MSG_TYPE_BASE_VAL] = "MSG_RESYNC_REQ",
+	[MSG_SMS_CMMB_GET_MRC_STATISTICS_REQ         - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_GET_MRC_STATISTICS_REQ",
+	[MSG_SMS_CMMB_GET_MRC_STATISTICS_RES         - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_GET_MRC_STATISTICS_RES",
+	[MSG_SMS_LOG_EX_ITEM                         - MSG_TYPE_BASE_VAL] = "MSG_SMS_LOG_EX_ITEM",
+	[MSG_SMS_DEVICE_DATA_LOSS_IND                - MSG_TYPE_BASE_VAL] = "MSG_SMS_DEVICE_DATA_LOSS_IND",
+	[MSG_SMS_MRC_WATCHDOG_TRIGGERED_IND          - MSG_TYPE_BASE_VAL] = "MSG_SMS_MRC_WATCHDOG_TRIGGERED_IND",
+	[MSG_SMS_USER_MSG_REQ                        - MSG_TYPE_BASE_VAL] = "MSG_SMS_USER_MSG_REQ",
+	[MSG_SMS_USER_MSG_RES                        - MSG_TYPE_BASE_VAL] = "MSG_SMS_USER_MSG_RES",
+	[MSG_SMS_SMART_CARD_INIT_REQ                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_SMART_CARD_INIT_REQ",
+	[MSG_SMS_SMART_CARD_INIT_RES                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_SMART_CARD_INIT_RES",
+	[MSG_SMS_SMART_CARD_WRITE_REQ                - MSG_TYPE_BASE_VAL] = "MSG_SMS_SMART_CARD_WRITE_REQ",
+	[MSG_SMS_SMART_CARD_WRITE_RES                - MSG_TYPE_BASE_VAL] = "MSG_SMS_SMART_CARD_WRITE_RES",
+	[MSG_SMS_SMART_CARD_READ_IND                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_SMART_CARD_READ_IND",
+	[MSG_SMS_TSE_ENABLE_REQ                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_TSE_ENABLE_REQ",
+	[MSG_SMS_TSE_ENABLE_RES                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_TSE_ENABLE_RES",
+	[MSG_SMS_CMMB_GET_SHORT_STATISTICS_REQ       - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_GET_SHORT_STATISTICS_REQ",
+	[MSG_SMS_CMMB_GET_SHORT_STATISTICS_RES       - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_GET_SHORT_STATISTICS_RES",
+	[MSG_SMS_LED_CONFIG_REQ                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_LED_CONFIG_REQ",
+	[MSG_SMS_LED_CONFIG_RES                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_LED_CONFIG_RES",
+	[MSG_PWM_ANTENNA_REQ                         - MSG_TYPE_BASE_VAL] = "MSG_PWM_ANTENNA_REQ",
+	[MSG_PWM_ANTENNA_RES                         - MSG_TYPE_BASE_VAL] = "MSG_PWM_ANTENNA_RES",
+	[MSG_SMS_CMMB_SMD_SN_REQ                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_SMD_SN_REQ",
+	[MSG_SMS_CMMB_SMD_SN_RES                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_SMD_SN_RES",
+	[MSG_SMS_CMMB_SET_CA_CW_REQ                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_SET_CA_CW_REQ",
+	[MSG_SMS_CMMB_SET_CA_CW_RES                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_SET_CA_CW_RES",
+	[MSG_SMS_CMMB_SET_CA_SALT_REQ                - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_SET_CA_SALT_REQ",
+	[MSG_SMS_CMMB_SET_CA_SALT_RES                - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_SET_CA_SALT_RES",
+	[MSG_SMS_NSCD_INIT_REQ                       - MSG_TYPE_BASE_VAL] = "MSG_SMS_NSCD_INIT_REQ",
+	[MSG_SMS_NSCD_INIT_RES                       - MSG_TYPE_BASE_VAL] = "MSG_SMS_NSCD_INIT_RES",
+	[MSG_SMS_NSCD_PROCESS_SECTION_REQ            - MSG_TYPE_BASE_VAL] = "MSG_SMS_NSCD_PROCESS_SECTION_REQ",
+	[MSG_SMS_NSCD_PROCESS_SECTION_RES            - MSG_TYPE_BASE_VAL] = "MSG_SMS_NSCD_PROCESS_SECTION_RES",
+	[MSG_SMS_DBD_CREATE_OBJECT_REQ               - MSG_TYPE_BASE_VAL] = "MSG_SMS_DBD_CREATE_OBJECT_REQ",
+	[MSG_SMS_DBD_CREATE_OBJECT_RES               - MSG_TYPE_BASE_VAL] = "MSG_SMS_DBD_CREATE_OBJECT_RES",
+	[MSG_SMS_DBD_CONFIGURE_REQ                   - MSG_TYPE_BASE_VAL] = "MSG_SMS_DBD_CONFIGURE_REQ",
+	[MSG_SMS_DBD_CONFIGURE_RES                   - MSG_TYPE_BASE_VAL] = "MSG_SMS_DBD_CONFIGURE_RES",
+	[MSG_SMS_DBD_SET_KEYS_REQ                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_DBD_SET_KEYS_REQ",
+	[MSG_SMS_DBD_SET_KEYS_RES                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_DBD_SET_KEYS_RES",
+	[MSG_SMS_DBD_PROCESS_HEADER_REQ              - MSG_TYPE_BASE_VAL] = "MSG_SMS_DBD_PROCESS_HEADER_REQ",
+	[MSG_SMS_DBD_PROCESS_HEADER_RES              - MSG_TYPE_BASE_VAL] = "MSG_SMS_DBD_PROCESS_HEADER_RES",
+	[MSG_SMS_DBD_PROCESS_DATA_REQ                - MSG_TYPE_BASE_VAL] = "MSG_SMS_DBD_PROCESS_DATA_REQ",
+	[MSG_SMS_DBD_PROCESS_DATA_RES                - MSG_TYPE_BASE_VAL] = "MSG_SMS_DBD_PROCESS_DATA_RES",
+	[MSG_SMS_DBD_PROCESS_GET_DATA_REQ            - MSG_TYPE_BASE_VAL] = "MSG_SMS_DBD_PROCESS_GET_DATA_REQ",
+	[MSG_SMS_DBD_PROCESS_GET_DATA_RES            - MSG_TYPE_BASE_VAL] = "MSG_SMS_DBD_PROCESS_GET_DATA_RES",
+	[MSG_SMS_NSCD_OPEN_SESSION_REQ               - MSG_TYPE_BASE_VAL] = "MSG_SMS_NSCD_OPEN_SESSION_REQ",
+	[MSG_SMS_NSCD_OPEN_SESSION_RES               - MSG_TYPE_BASE_VAL] = "MSG_SMS_NSCD_OPEN_SESSION_RES",
+	[MSG_SMS_SEND_HOST_DATA_TO_DEMUX_REQ         - MSG_TYPE_BASE_VAL] = "MSG_SMS_SEND_HOST_DATA_TO_DEMUX_REQ",
+	[MSG_SMS_SEND_HOST_DATA_TO_DEMUX_RES         - MSG_TYPE_BASE_VAL] = "MSG_SMS_SEND_HOST_DATA_TO_DEMUX_RES",
+	[MSG_LAST_MSG_TYPE                           - MSG_TYPE_BASE_VAL] = "MSG_LAST_MSG_TYPE",
+};
+
+char *smscore_translate_msg(enum msg_types msgtype)
+{
+	int i = msgtype - MSG_TYPE_BASE_VAL;
+	char *msg;
+
+	if (i < 0 || i >= ARRAY_SIZE(siano_msgs))
+		return "Unknown msg type";
+
+	msg = siano_msgs[i];
+
+	if (!*msg)
+		return "Unknown msg type";
+
+	return msg;
+}
+EXPORT_SYMBOL_GPL(smscore_translate_msg);
+
 void smscore_set_board_id(struct smscore_device_t *core, int id)
 {
 	core->board_id = id;
@@ -96,7 +434,7 @@ static struct mutex g_smscore_deviceslock;
 static struct list_head g_smscore_registry;
 static struct mutex g_smscore_registrylock;
 
-static int default_mode = 4;
+static int default_mode = DEVICE_MODE_NONE;
 
 module_param(default_mode, int, 0644);
 MODULE_PARM_DESC(default_mode, "default firmware id (device mode)");
@@ -151,10 +489,10 @@ static enum sms_device_type_st smscore_registry_gettype(char *devpath)
 	else
 		sms_err("No registry found.");
 
-	return -1;
+	return -EINVAL;
 }
 
-void smscore_registry_setmode(char *devpath, int mode)
+static void smscore_registry_setmode(char *devpath, int mode)
 {
 	struct smscore_registry_entry_t *entry;
 
@@ -294,10 +632,11 @@ static struct
 smscore_buffer_t *smscore_createbuffer(u8 *buffer, void *common_buffer,
 				       dma_addr_t common_buffer_phys)
 {
-	struct smscore_buffer_t *cb =
-		kmalloc(sizeof(struct smscore_buffer_t), GFP_KERNEL);
+	struct smscore_buffer_t *cb;
+
+	cb = kzalloc(sizeof(struct smscore_buffer_t), GFP_KERNEL);
 	if (!cb) {
-		sms_info("kmalloc(...) failed");
+		sms_info("kzalloc(...) failed");
 		return NULL;
 	}
 
@@ -344,6 +683,7 @@ int smscore_register_device(struct smsdevice_params_t *params,
 	/* init completion events */
 	init_completion(&dev->version_ex_done);
 	init_completion(&dev->data_download_done);
+	init_completion(&dev->data_validity_done);
 	init_completion(&dev->trigger_done);
 	init_completion(&dev->init_device_done);
 	init_completion(&dev->reload_start_done);
@@ -370,9 +710,10 @@ int smscore_register_device(struct smsdevice_params_t *params,
 	for (buffer = dev->common_buffer;
 	     dev->num_buffers < params->num_buffers;
 	     dev->num_buffers++, buffer += params->buffer_size) {
-		struct smscore_buffer_t *cb =
-			smscore_createbuffer(buffer, dev->common_buffer,
-					     dev->common_buffer_phys);
+		struct smscore_buffer_t *cb;
+
+		cb = smscore_createbuffer(buffer, dev->common_buffer,
+					  dev->common_buffer_phys);
 		if (!cb) {
 			smscore_unregister_device(dev);
 			return -ENOMEM;
@@ -384,6 +725,7 @@ int smscore_register_device(struct smsdevice_params_t *params,
 	sms_info("allocated %d buffers", dev->num_buffers);
 
 	dev->mode = DEVICE_MODE_NONE;
+	dev->board_id = SMS_BOARD_UNKNOWN;
 	dev->context = params->context;
 	dev->device = params->device;
 	dev->setmode_handler = params->setmode_handler;
@@ -413,7 +755,13 @@ EXPORT_SYMBOL_GPL(smscore_register_device);
 
 static int smscore_sendrequest_and_wait(struct smscore_device_t *coredev,
 		void *buffer, size_t size, struct completion *completion) {
-	int rc = coredev->sendrequest_handler(coredev->context, buffer, size);
+	int rc;
+
+	if (completion == NULL)
+		return -EINVAL;
+	init_completion(completion);
+
+	rc = coredev->sendrequest_handler(coredev->context, buffer, size);
 	if (rc < 0) {
 		sms_info("sendrequest returned error %d", rc);
 		return rc;
@@ -444,24 +792,22 @@ static int smscore_init_ir(struct smscore_device_t *coredev)
 		if	(rc != 0)
 			sms_err("Error initialization DTV IR sub-module");
 		else {
-			buffer = kmalloc(sizeof(struct SmsMsgData_ST2) +
+			buffer = kmalloc(sizeof(struct sms_msg_data2) +
 						SMS_DMA_ALIGNMENT,
 						GFP_KERNEL | GFP_DMA);
 			if (buffer) {
-				struct SmsMsgData_ST2 *msg =
-				(struct SmsMsgData_ST2 *)
+				struct sms_msg_data2 *msg =
+				(struct sms_msg_data2 *)
 				SMS_ALIGN_ADDRESS(buffer);
 
-				SMS_INIT_MSG(&msg->xMsgHeader,
+				SMS_INIT_MSG(&msg->x_msg_header,
 						MSG_SMS_START_IR_REQ,
-						sizeof(struct SmsMsgData_ST2));
-				msg->msgData[0] = coredev->ir.controller;
-				msg->msgData[1] = coredev->ir.timeout;
+						sizeof(struct sms_msg_data2));
+				msg->msg_data[0] = coredev->ir.controller;
+				msg->msg_data[1] = coredev->ir.timeout;
 
-				smsendian_handle_tx_message(
-					(struct SmsMsgHdr_ST2 *)msg);
 				rc = smscore_sendrequest_and_wait(coredev, msg,
-						msg->xMsgHeader. msgLength,
+						msg->x_msg_header. msg_length,
 						&coredev->ir_init_done);
 
 				kfree(buffer);
@@ -476,21 +822,82 @@ static int smscore_init_ir(struct smscore_device_t *coredev)
 }
 
 /**
+ * configures device features according to board configuration structure.
+ *
+ * @param coredev pointer to a coredev object returned by
+ *                smscore_register_device
+ *
+ * @return 0 on success, <0 on error.
+ */
+static int smscore_configure_board(struct smscore_device_t *coredev)
+{
+	struct sms_board *board;
+
+	board = sms_get_board(coredev->board_id);
+	if (!board) {
+		sms_err("no board configuration exist.");
+		return -EINVAL;
+	}
+
+	if (board->mtu) {
+		struct sms_msg_data mtu_msg;
+		sms_debug("set max transmit unit %d", board->mtu);
+
+		mtu_msg.x_msg_header.msg_src_id = 0;
+		mtu_msg.x_msg_header.msg_dst_id = HIF_TASK;
+		mtu_msg.x_msg_header.msg_flags = 0;
+		mtu_msg.x_msg_header.msg_type = MSG_SMS_SET_MAX_TX_MSG_LEN_REQ;
+		mtu_msg.x_msg_header.msg_length = sizeof(mtu_msg);
+		mtu_msg.msg_data[0] = board->mtu;
+
+		coredev->sendrequest_handler(coredev->context, &mtu_msg,
+					     sizeof(mtu_msg));
+	}
+
+	if (board->crystal) {
+		struct sms_msg_data crys_msg;
+		sms_debug("set crystal value %d", board->crystal);
+
+		SMS_INIT_MSG(&crys_msg.x_msg_header,
+				MSG_SMS_NEW_CRYSTAL_REQ,
+				sizeof(crys_msg));
+		crys_msg.msg_data[0] = board->crystal;
+
+		coredev->sendrequest_handler(coredev->context, &crys_msg,
+					     sizeof(crys_msg));
+	}
+
+	return 0;
+}
+
+/**
  * sets initial device mode and notifies client hotplugs that device is ready
  *
  * @param coredev pointer to a coredev object returned by
- * 		  smscore_register_device
+ *		  smscore_register_device
  *
  * @return 0 on success, <0 on error.
  */
 int smscore_start_device(struct smscore_device_t *coredev)
 {
-	int rc = smscore_set_device_mode(
-			coredev, smscore_registry_getmode(coredev->devpath));
+	int rc;
+	int board_id = smscore_get_board_id(coredev);
+	int mode = smscore_registry_getmode(coredev->devpath);
+
+	/* Device is initialized as DEVICE_MODE_NONE */
+	if (board_id != SMS_BOARD_UNKNOWN && mode == DEVICE_MODE_NONE)
+		mode = sms_get_board(board_id)->default_mode;
+
+	rc = smscore_set_device_mode(coredev, mode);
 	if (rc < 0) {
 		sms_info("set device mode faile , rc %d", rc);
 		return rc;
 	}
+	rc = smscore_configure_board(coredev);
+	if (rc < 0) {
+		sms_info("configure board failed , rc %d", rc);
+		return rc;
+	}
 
 	kmutex_lock(&g_smscore_deviceslock);
 
@@ -509,18 +916,19 @@ EXPORT_SYMBOL_GPL(smscore_start_device);
 static int smscore_load_firmware_family2(struct smscore_device_t *coredev,
 					 void *buffer, size_t size)
 {
-	struct SmsFirmware_ST *firmware = (struct SmsFirmware_ST *) buffer;
-	struct SmsMsgHdr_ST *msg;
-	u32 mem_address;
-	u8 *payload = firmware->Payload;
+	struct sms_firmware *firmware = (struct sms_firmware *) buffer;
+	struct sms_msg_data4 *msg;
+	u32 mem_address,  calc_checksum = 0;
+	u32 i, *ptr;
+	u8 *payload = firmware->payload;
 	int rc = 0;
-	firmware->StartAddress = le32_to_cpu(firmware->StartAddress);
-	firmware->Length = le32_to_cpu(firmware->Length);
+	firmware->start_address = le32_to_cpu(firmware->start_address);
+	firmware->length = le32_to_cpu(firmware->length);
 
-	mem_address = firmware->StartAddress;
+	mem_address = firmware->start_address;
 
 	sms_info("loading FW to addr 0x%x size %d",
-		 mem_address, firmware->Length);
+		 mem_address, firmware->length);
 	if (coredev->preload_handler) {
 		rc = coredev->preload_handler(coredev->context);
 		if (rc < 0)
@@ -534,35 +942,36 @@ static int smscore_load_firmware_family2(struct smscore_device_t *coredev,
 
 	if (coredev->mode != DEVICE_MODE_NONE) {
 		sms_debug("sending reload command.");
-		SMS_INIT_MSG(msg, MSG_SW_RELOAD_START_REQ,
-			     sizeof(struct SmsMsgHdr_ST));
+		SMS_INIT_MSG(&msg->x_msg_header, MSG_SW_RELOAD_START_REQ,
+			     sizeof(struct sms_msg_hdr));
 		rc = smscore_sendrequest_and_wait(coredev, msg,
-						  msg->msgLength,
+						  msg->x_msg_header.msg_length,
 						  &coredev->reload_start_done);
+		if (rc < 0) {
+			sms_err("device reload failed, rc %d", rc);
+			goto exit_fw_download;
+		}
 		mem_address = *(u32 *) &payload[20];
 	}
 
+	for (i = 0, ptr = (u32 *)firmware->payload; i < firmware->length/4 ;
+	     i++, ptr++)
+		calc_checksum += *ptr;
+
 	while (size && rc >= 0) {
-		struct SmsDataDownload_ST *DataMsg =
-			(struct SmsDataDownload_ST *) msg;
-		int payload_size = min((int) size, SMS_MAX_PAYLOAD_SIZE);
+		struct sms_data_download *data_msg =
+			(struct sms_data_download *) msg;
+		int payload_size = min_t(int, size, SMS_MAX_PAYLOAD_SIZE);
 
-		SMS_INIT_MSG(msg, MSG_SMS_DATA_DOWNLOAD_REQ,
-			     (u16)(sizeof(struct SmsMsgHdr_ST) +
+		SMS_INIT_MSG(&msg->x_msg_header, MSG_SMS_DATA_DOWNLOAD_REQ,
+			     (u16)(sizeof(struct sms_msg_hdr) +
 				      sizeof(u32) + payload_size));
 
-		DataMsg->MemAddr = mem_address;
-		memcpy(DataMsg->Payload, payload, payload_size);
+		data_msg->mem_addr = mem_address;
+		memcpy(data_msg->payload, payload, payload_size);
 
-		if ((coredev->device_flags & SMS_ROM_NO_RESPONSE) &&
-		    (coredev->mode == DEVICE_MODE_NONE))
-			rc = coredev->sendrequest_handler(
-				coredev->context, DataMsg,
-				DataMsg->xMsgHeader.msgLength);
-		else
-			rc = smscore_sendrequest_and_wait(
-				coredev, DataMsg,
-				DataMsg->xMsgHeader.msgLength,
+		rc = smscore_sendrequest_and_wait(coredev, data_msg,
+				data_msg->x_msg_header.msg_length,
 				&coredev->data_download_done);
 
 		payload += payload_size;
@@ -570,50 +979,158 @@ static int smscore_load_firmware_family2(struct smscore_device_t *coredev,
 		mem_address += payload_size;
 	}
 
-	if (rc >= 0) {
-		if (coredev->mode == DEVICE_MODE_NONE) {
-			struct SmsMsgData_ST *TriggerMsg =
-				(struct SmsMsgData_ST *) msg;
-
-			SMS_INIT_MSG(msg, MSG_SMS_SWDOWNLOAD_TRIGGER_REQ,
-				     sizeof(struct SmsMsgHdr_ST) +
-				     sizeof(u32) * 5);
-
-			TriggerMsg->msgData[0] = firmware->StartAddress;
-						/* Entry point */
-			TriggerMsg->msgData[1] = 5; /* Priority */
-			TriggerMsg->msgData[2] = 0x200; /* Stack size */
-			TriggerMsg->msgData[3] = 0; /* Parameter */
-			TriggerMsg->msgData[4] = 4; /* Task ID */
-
-			if (coredev->device_flags & SMS_ROM_NO_RESPONSE) {
-				rc = coredev->sendrequest_handler(
-					coredev->context, TriggerMsg,
-					TriggerMsg->xMsgHeader.msgLength);
-				msleep(100);
-			} else
-				rc = smscore_sendrequest_and_wait(
-					coredev, TriggerMsg,
-					TriggerMsg->xMsgHeader.msgLength,
+	if (rc < 0)
+		goto exit_fw_download;
+
+	sms_err("sending MSG_SMS_DATA_VALIDITY_REQ expecting 0x%x",
+		calc_checksum);
+	SMS_INIT_MSG(&msg->x_msg_header, MSG_SMS_DATA_VALIDITY_REQ,
+			sizeof(msg->x_msg_header) +
+			sizeof(u32) * 3);
+	msg->msg_data[0] = firmware->start_address;
+		/* Entry point */
+	msg->msg_data[1] = firmware->length;
+	msg->msg_data[2] = 0; /* Regular checksum*/
+	rc = smscore_sendrequest_and_wait(coredev, msg,
+					  msg->x_msg_header.msg_length,
+					  &coredev->data_validity_done);
+	if (rc < 0)
+		goto exit_fw_download;
+
+	if (coredev->mode == DEVICE_MODE_NONE) {
+		struct sms_msg_data *trigger_msg =
+			(struct sms_msg_data *) msg;
+
+		sms_debug("sending MSG_SMS_SWDOWNLOAD_TRIGGER_REQ");
+		SMS_INIT_MSG(&msg->x_msg_header,
+				MSG_SMS_SWDOWNLOAD_TRIGGER_REQ,
+				sizeof(struct sms_msg_hdr) +
+				sizeof(u32) * 5);
+
+		trigger_msg->msg_data[0] = firmware->start_address;
+					/* Entry point */
+		trigger_msg->msg_data[1] = 6; /* Priority */
+		trigger_msg->msg_data[2] = 0x200; /* Stack size */
+		trigger_msg->msg_data[3] = 0; /* Parameter */
+		trigger_msg->msg_data[4] = 4; /* Task ID */
+
+		rc = smscore_sendrequest_and_wait(coredev, trigger_msg,
+					trigger_msg->x_msg_header.msg_length,
 					&coredev->trigger_done);
-		} else {
-			SMS_INIT_MSG(msg, MSG_SW_RELOAD_EXEC_REQ,
-				     sizeof(struct SmsMsgHdr_ST));
-
-			rc = coredev->sendrequest_handler(coredev->context,
-							  msg, msg->msgLength);
-		}
-		msleep(500);
+	} else {
+		SMS_INIT_MSG(&msg->x_msg_header, MSG_SW_RELOAD_EXEC_REQ,
+				sizeof(struct sms_msg_hdr));
+		rc = coredev->sendrequest_handler(coredev->context, msg,
+				msg->x_msg_header.msg_length);
 	}
 
-	sms_debug("rc=%d, postload=%p ", rc,
-		  coredev->postload_handler);
+	if (rc < 0)
+		goto exit_fw_download;
+
+	/*
+	 * backward compatibility - wait to device_ready_done for
+	 * not more than 400 ms
+	 */
+	msleep(400);
 
+exit_fw_download:
 	kfree(msg);
 
-	return ((rc >= 0) && coredev->postload_handler) ?
-		coredev->postload_handler(coredev->context) :
-		rc;
+	if (coredev->postload_handler) {
+		sms_debug("rc=%d, postload=0x%p", rc, coredev->postload_handler);
+		if (rc >= 0)
+			return coredev->postload_handler(coredev->context);
+	}
+
+	sms_debug("rc=%d", rc);
+	return rc;
+}
+
+static char *smscore_fw_lkup[][DEVICE_MODE_MAX] = {
+	[SMS_NOVA_A0] = {
+		[DEVICE_MODE_DVBT]		= SMS_FW_DVB_NOVA_12MHZ,
+		[DEVICE_MODE_DVBH]		= SMS_FW_DVB_NOVA_12MHZ,
+		[DEVICE_MODE_DAB_TDMB]		= SMS_FW_TDMB_NOVA_12MHZ,
+		[DEVICE_MODE_DVBT_BDA]		= SMS_FW_DVB_NOVA_12MHZ,
+		[DEVICE_MODE_ISDBT]		= SMS_FW_ISDBT_NOVA_12MHZ,
+		[DEVICE_MODE_ISDBT_BDA]		= SMS_FW_ISDBT_NOVA_12MHZ,
+	},
+	[SMS_NOVA_B0] = {
+		[DEVICE_MODE_DVBT]		= SMS_FW_DVB_NOVA_12MHZ_B0,
+		[DEVICE_MODE_DVBH]		= SMS_FW_DVB_NOVA_12MHZ_B0,
+		[DEVICE_MODE_DAB_TDMB]		= SMS_FW_TDMB_NOVA_12MHZ_B0,
+		[DEVICE_MODE_DVBT_BDA]		= SMS_FW_DVB_NOVA_12MHZ_B0,
+		[DEVICE_MODE_ISDBT]		= SMS_FW_ISDBT_NOVA_12MHZ_B0,
+		[DEVICE_MODE_ISDBT_BDA]		= SMS_FW_ISDBT_NOVA_12MHZ_B0,
+		[DEVICE_MODE_FM_RADIO]		= SMS_FW_FM_RADIO,
+		[DEVICE_MODE_FM_RADIO_BDA]	= SMS_FW_FM_RADIO,
+	},
+	[SMS_VEGA] = {
+		[DEVICE_MODE_CMMB]		= SMS_FW_CMMB_VEGA_12MHZ,
+	},
+	[SMS_VENICE] = {
+		[DEVICE_MODE_CMMB]		= SMS_FW_CMMB_VENICE_12MHZ,
+	},
+	[SMS_MING] = {
+		[DEVICE_MODE_CMMB]		= SMS_FW_CMMB_MING_APP,
+	},
+	[SMS_PELE] = {
+		[DEVICE_MODE_ISDBT]		= SMS_FW_ISDBT_PELE,
+		[DEVICE_MODE_ISDBT_BDA]		= SMS_FW_ISDBT_PELE,
+	},
+	[SMS_RIO] = {
+		[DEVICE_MODE_DVBT]		= SMS_FW_DVB_RIO,
+		[DEVICE_MODE_DVBH]		= SMS_FW_DVBH_RIO,
+		[DEVICE_MODE_DVBT_BDA]		= SMS_FW_DVB_RIO,
+		[DEVICE_MODE_ISDBT]		= SMS_FW_ISDBT_RIO,
+		[DEVICE_MODE_ISDBT_BDA]		= SMS_FW_ISDBT_RIO,
+		[DEVICE_MODE_FM_RADIO]		= SMS_FW_FM_RADIO_RIO,
+		[DEVICE_MODE_FM_RADIO_BDA]	= SMS_FW_FM_RADIO_RIO,
+	},
+	[SMS_DENVER_1530] = {
+		[DEVICE_MODE_ATSC]		= SMS_FW_ATSC_DENVER,
+	},
+	[SMS_DENVER_2160] = {
+		[DEVICE_MODE_DAB_TDMB]		= SMS_FW_TDMB_DENVER,
+	},
+};
+
+/**
+ * get firmware file name from one of the two mechanisms : sms_boards or
+ * smscore_fw_lkup.
+ * @param coredev pointer to a coredev object returned by
+ *		  smscore_register_device
+ * @param mode requested mode of operation
+ * @param lookup if 1, always get the fw filename from smscore_fw_lkup
+ *	 table. if 0, try first to get from sms_boards
+ *
+ * @return 0 on success, <0 on error.
+ */
+static char *smscore_get_fw_filename(struct smscore_device_t *coredev,
+			      int mode)
+{
+	char **fw;
+	int board_id = smscore_get_board_id(coredev);
+	enum sms_device_type_st type;
+
+	type = smscore_registry_gettype(coredev->devpath);
+
+	/* Prevent looking outside the smscore_fw_lkup table */
+	if (type <= SMS_UNKNOWN_TYPE || type >= SMS_NUM_OF_DEVICE_TYPES)
+		return NULL;
+	if (mode <= DEVICE_MODE_NONE || mode >= DEVICE_MODE_MAX)
+		return NULL;
+
+	sms_debug("trying to get fw name from sms_boards board_id %d mode %d",
+		  board_id, mode);
+	fw = sms_get_board(board_id)->fw;
+	if (!fw || !fw[mode]) {
+		sms_debug("cannot find fw name in sms_boards, getting from lookup table mode %d type %d",
+			  mode, type);
+		return smscore_fw_lkup[type][mode];
+	}
+
+	return fw[mode];
 }
 
 /**
@@ -627,41 +1144,46 @@ static int smscore_load_firmware_family2(struct smscore_device_t *coredev,
  * @return 0 on success, <0 on error.
  */
 static int smscore_load_firmware_from_file(struct smscore_device_t *coredev,
-					   char *filename,
+					   int mode,
 					   loadfirmware_t loadfirmware_handler)
 {
 	int rc = -ENOENT;
+	u8 *fw_buf;
+	u32 fw_buf_size;
 	const struct firmware *fw;
-	u8 *fw_buffer;
 
-	if (loadfirmware_handler == NULL && !(coredev->device_flags &
-					      SMS_DEVICE_FAMILY2))
+	char *fw_filename = smscore_get_fw_filename(coredev, mode);
+	if (!fw_filename) {
+		sms_info("mode %d not supported on this device", mode);
+		return -ENOENT;
+	}
+	sms_debug("Firmware name: %s", fw_filename);
+
+	if (loadfirmware_handler == NULL && !(coredev->device_flags
+			& SMS_DEVICE_FAMILY2))
 		return -EINVAL;
 
-	rc = request_firmware(&fw, filename, coredev->device);
+	rc = request_firmware(&fw, fw_filename, coredev->device);
 	if (rc < 0) {
-		sms_info("failed to open \"%s\"", filename);
+		sms_info("failed to open \"%s\"", fw_filename);
 		return rc;
 	}
-	sms_info("read FW %s, size=%zd", filename, fw->size);
-	fw_buffer = kmalloc(ALIGN(fw->size, SMS_ALLOC_ALIGNMENT),
-			    GFP_KERNEL | GFP_DMA);
-	if (fw_buffer) {
-		memcpy(fw_buffer, fw->data, fw->size);
-
-		rc = (coredev->device_flags & SMS_DEVICE_FAMILY2) ?
-		      smscore_load_firmware_family2(coredev,
-						    fw_buffer,
-						    fw->size) :
-		      loadfirmware_handler(coredev->context,
-					   fw_buffer, fw->size);
-
-		kfree(fw_buffer);
-	} else {
+	sms_info("read fw %s, buffer size=0x%zx", fw_filename, fw->size);
+	fw_buf = kmalloc(ALIGN(fw->size, SMS_ALLOC_ALIGNMENT),
+			 GFP_KERNEL | GFP_DMA);
+	if (!fw_buf) {
 		sms_info("failed to allocate firmware buffer");
-		rc = -ENOMEM;
+		return -ENOMEM;
 	}
+	memcpy(fw_buf, fw->data, fw->size);
+	fw_buf_size = fw->size;
+
+	rc = (coredev->device_flags & SMS_DEVICE_FAMILY2) ?
+		smscore_load_firmware_family2(coredev, fw_buf, fw_buf_size)
+		: loadfirmware_handler(coredev->context, fw_buf,
+		fw_buf_size);
 
+	kfree(fw_buf);
 	release_firmware(fw);
 
 	return rc;
@@ -703,14 +1225,15 @@ void smscore_unregister_device(struct smscore_device_t *coredev)
 		if (num_buffers == coredev->num_buffers)
 			break;
 		if (++retry > 10) {
-			sms_info("exiting although "
-				 "not all buffers released.");
+			sms_info("exiting although not all buffers released.");
 			break;
 		}
 
 		sms_info("waiting for %d buffer(s)",
 			 coredev->num_buffers - num_buffers);
+		kmutex_unlock(&g_smscore_deviceslock);
 		msleep(100);
+		kmutex_lock(&g_smscore_deviceslock);
 	}
 
 	sms_info("freed %d buffers", num_buffers);
@@ -719,8 +1242,7 @@ void smscore_unregister_device(struct smscore_device_t *coredev)
 		dma_free_coherent(NULL, coredev->common_buffer_size,
 			coredev->common_buffer, coredev->common_buffer_phys);
 
-	if (coredev->fw_buf != NULL)
-		kfree(coredev->fw_buf);
+	kfree(coredev->fw_buf);
 
 	list_del(&coredev->entry);
 	kfree(coredev);
@@ -733,19 +1255,19 @@ EXPORT_SYMBOL_GPL(smscore_unregister_device);
 
 static int smscore_detect_mode(struct smscore_device_t *coredev)
 {
-	void *buffer = kmalloc(sizeof(struct SmsMsgHdr_ST) + SMS_DMA_ALIGNMENT,
+	void *buffer = kmalloc(sizeof(struct sms_msg_hdr) + SMS_DMA_ALIGNMENT,
 			       GFP_KERNEL | GFP_DMA);
-	struct SmsMsgHdr_ST *msg =
-		(struct SmsMsgHdr_ST *) SMS_ALIGN_ADDRESS(buffer);
+	struct sms_msg_hdr *msg =
+		(struct sms_msg_hdr *) SMS_ALIGN_ADDRESS(buffer);
 	int rc;
 
 	if (!buffer)
 		return -ENOMEM;
 
 	SMS_INIT_MSG(msg, MSG_SMS_GET_VERSION_EX_REQ,
-		     sizeof(struct SmsMsgHdr_ST));
+		     sizeof(struct sms_msg_hdr));
 
-	rc = smscore_sendrequest_and_wait(coredev, msg, msg->msgLength,
+	rc = smscore_sendrequest_and_wait(coredev, msg, msg->msg_length,
 					  &coredev->version_ex_done);
 	if (rc == -ETIME) {
 		sms_err("MSG_SMS_GET_VERSION_EX_REQ failed first try");
@@ -753,11 +1275,11 @@ static int smscore_detect_mode(struct smscore_device_t *coredev)
 		if (wait_for_completion_timeout(&coredev->resume_done,
 						msecs_to_jiffies(5000))) {
 			rc = smscore_sendrequest_and_wait(
-				coredev, msg, msg->msgLength,
+				coredev, msg, msg->msg_length,
 				&coredev->version_ex_done);
 			if (rc < 0)
-				sms_err("MSG_SMS_GET_VERSION_EX_REQ failed "
-					"second try, rc %d", rc);
+				sms_err("MSG_SMS_GET_VERSION_EX_REQ failed second try, rc %d",
+					rc);
 		} else
 			rc = -ETIME;
 	}
@@ -767,31 +1289,39 @@ static int smscore_detect_mode(struct smscore_device_t *coredev)
 	return rc;
 }
 
-static char *smscore_fw_lkup[][SMS_NUM_OF_DEVICE_TYPES] = {
-	/*Stellar		NOVA A0		Nova B0		VEGA*/
-	/*DVBT*/
-	{"none", "dvb_nova_12mhz.inp", "dvb_nova_12mhz_b0.inp", "none"},
-	/*DVBH*/
-	{"none", "dvb_nova_12mhz.inp", "dvb_nova_12mhz_b0.inp", "none"},
-	/*TDMB*/
-	{"none", "tdmb_nova_12mhz.inp", "tdmb_nova_12mhz_b0.inp", "none"},
-	/*DABIP*/
-	{"none", "none", "none", "none"},
-	/*BDA*/
-	{"none", "dvb_nova_12mhz.inp", "dvb_nova_12mhz_b0.inp", "none"},
-	/*ISDBT*/
-	{"none", "isdbt_nova_12mhz.inp", "isdbt_nova_12mhz_b0.inp", "none"},
-	/*ISDBTBDA*/
-	{"none", "isdbt_nova_12mhz.inp", "isdbt_nova_12mhz_b0.inp", "none"},
-	/*CMMB*/
-	{"none", "none", "none", "cmmb_vega_12mhz.inp"}
-};
-
-static inline char *sms_get_fw_name(struct smscore_device_t *coredev,
-				    int mode, enum sms_device_type_st type)
+/**
+ * send init device request and wait for response
+ *
+ * @param coredev pointer to a coredev object returned by
+ *                smscore_register_device
+ * @param mode requested mode of operation
+ *
+ * @return 0 on success, <0 on error.
+ */
+static int smscore_init_device(struct smscore_device_t *coredev, int mode)
 {
-	char **fw = sms_get_board(smscore_get_board_id(coredev))->fw;
-	return (fw && fw[mode]) ? fw[mode] : smscore_fw_lkup[mode][type];
+	void *buffer;
+	struct sms_msg_data *msg;
+	int rc = 0;
+
+	buffer = kmalloc(sizeof(struct sms_msg_data) +
+			SMS_DMA_ALIGNMENT, GFP_KERNEL | GFP_DMA);
+	if (!buffer) {
+		sms_err("Could not allocate buffer for init device message.");
+		return -ENOMEM;
+	}
+
+	msg = (struct sms_msg_data *)SMS_ALIGN_ADDRESS(buffer);
+	SMS_INIT_MSG(&msg->x_msg_header, MSG_SMS_INIT_DEVICE_REQ,
+			sizeof(struct sms_msg_data));
+	msg->msg_data[0] = mode;
+
+	rc = smscore_sendrequest_and_wait(coredev, msg,
+			msg->x_msg_header. msg_length,
+			&coredev->init_device_done);
+
+	kfree(buffer);
+	return rc;
 }
 
 /**
@@ -806,13 +1336,11 @@ static inline char *sms_get_fw_name(struct smscore_device_t *coredev,
  */
 int smscore_set_device_mode(struct smscore_device_t *coredev, int mode)
 {
-	void *buffer;
 	int rc = 0;
-	enum sms_device_type_st type;
 
 	sms_debug("set device mode to %d", mode);
 	if (coredev->device_flags & SMS_DEVICE_FAMILY2) {
-		if (mode < DEVICE_MODE_DVBT || mode >= DEVICE_MODE_RAW_TUNER) {
+		if (mode <= DEVICE_MODE_NONE || mode >= DEVICE_MODE_MAX) {
 			sms_err("invalid mode specified %d", mode);
 			return -EINVAL;
 		}
@@ -833,58 +1361,21 @@ int smscore_set_device_mode(struct smscore_device_t *coredev, int mode)
 		}
 
 		if (!(coredev->modes_supported & (1 << mode))) {
-			char *fw_filename;
-
-			type = smscore_registry_gettype(coredev->devpath);
-			fw_filename = sms_get_fw_name(coredev, mode, type);
-
 			rc = smscore_load_firmware_from_file(coredev,
-							     fw_filename, NULL);
-			if (rc < 0) {
-				sms_warn("error %d loading firmware: %s, "
-					 "trying again with default firmware",
-					 rc, fw_filename);
-
-				/* try again with the default firmware */
-				fw_filename = smscore_fw_lkup[mode][type];
-				rc = smscore_load_firmware_from_file(coredev,
-							     fw_filename, NULL);
-
-				if (rc < 0) {
-					sms_warn("error %d loading "
-						 "firmware: %s", rc,
-						 fw_filename);
-					return rc;
-				}
-			}
-			sms_log("firmware download success: %s", fw_filename);
-		} else
-			sms_info("mode %d supported by running "
-				 "firmware", mode);
-
-		buffer = kmalloc(sizeof(struct SmsMsgData_ST) +
-				 SMS_DMA_ALIGNMENT, GFP_KERNEL | GFP_DMA);
-		if (buffer) {
-			struct SmsMsgData_ST *msg =
-				(struct SmsMsgData_ST *)
-					SMS_ALIGN_ADDRESS(buffer);
-
-			SMS_INIT_MSG(&msg->xMsgHeader, MSG_SMS_INIT_DEVICE_REQ,
-				     sizeof(struct SmsMsgData_ST));
-			msg->msgData[0] = mode;
-
-			rc = smscore_sendrequest_and_wait(
-				coredev, msg, msg->xMsgHeader.msgLength,
-				&coredev->init_device_done);
-
-			kfree(buffer);
+							     mode, NULL);
+			if (rc >= 0)
+				sms_info("firmware download success");
 		} else {
-			sms_err("Could not allocate buffer for "
-				"init device message.");
-			rc = -ENOMEM;
+			sms_info("mode %d is already supported by running firmware",
+				 mode);
+		}
+		if (coredev->fw_version >= 0x800) {
+			rc = smscore_init_device(coredev, mode);
+			if (rc < 0)
+				sms_err("device init failed, rc %d.", rc);
 		}
 	} else {
-		if (mode < DEVICE_MODE_DVBT || mode > DEVICE_MODE_DVBT_BDA) {
+		if (mode <= DEVICE_MODE_NONE || mode >= DEVICE_MODE_MAX) {
 			sms_err("invalid mode specified %d", mode);
 			return -EINVAL;
 		}
@@ -900,12 +1391,32 @@ int smscore_set_device_mode(struct smscore_device_t *coredev, int mode)
 	}
 
 	if (rc >= 0) {
+		char *buffer;
 		coredev->mode = mode;
 		coredev->device_flags &= ~SMS_DEVICE_NOT_READY;
+
+		buffer = kmalloc(sizeof(struct sms_msg_data) +
+				 SMS_DMA_ALIGNMENT, GFP_KERNEL | GFP_DMA);
+		if (buffer) {
+			struct sms_msg_data *msg = (struct sms_msg_data *) SMS_ALIGN_ADDRESS(buffer);
+
+			SMS_INIT_MSG(&msg->x_msg_header, MSG_SMS_INIT_DEVICE_REQ,
+				     sizeof(struct sms_msg_data));
+			msg->msg_data[0] = mode;
+
+			rc = smscore_sendrequest_and_wait(
+				coredev, msg, msg->x_msg_header.msg_length,
+				&coredev->init_device_done);
+
+			kfree(buffer);
+		}
 	}
 
 	if (rc < 0)
 		sms_err("return error code %d.", rc);
+	else
+		sms_debug("Success setting device mode.");
+
 	return rc;
 }
 
@@ -971,7 +1482,7 @@ found:
  */
 void smscore_onresponse(struct smscore_device_t *coredev,
 		struct smscore_buffer_t *cb) {
-	struct SmsMsgHdr_ST *phdr = (struct SmsMsgHdr_ST *) ((u8 *) cb->p
+	struct sms_msg_hdr *phdr = (struct sms_msg_hdr *) ((u8 *) cb->p
 			+ cb->offset);
 	struct smscore_client_t *client;
 	int rc = -EBUSY;
@@ -983,7 +1494,7 @@ void smscore_onresponse(struct smscore_device_t *coredev,
 		last_sample_time = time_now;
 
 	if (time_now - last_sample_time > 10000) {
-		sms_debug("\ndata rate %d bytes/secs",
+		sms_debug("data rate %d bytes/secs",
 			  (int)((data_total * 1000) /
 				(time_now - last_sample_time)));
 
@@ -993,14 +1504,14 @@ void smscore_onresponse(struct smscore_device_t *coredev,
 
 	data_total += cb->size;
 	/* Do we need to re-route? */
-	if ((phdr->msgType == MSG_SMS_HO_PER_SLICES_IND) ||
-			(phdr->msgType == MSG_SMS_TRANSMISSION_IND)) {
+	if ((phdr->msg_type == MSG_SMS_HO_PER_SLICES_IND) ||
+			(phdr->msg_type == MSG_SMS_TRANSMISSION_IND)) {
 		if (coredev->mode == DEVICE_MODE_DVBT_BDA)
-			phdr->msgDstId = DVBT_BDA_CONTROL_MSG_ID;
+			phdr->msg_dst_id = DVBT_BDA_CONTROL_MSG_ID;
 	}
 
 
-	client = smscore_find_client(coredev, phdr->msgType, phdr->msgDstId);
+	client = smscore_find_client(coredev, phdr->msg_type, phdr->msg_dst_id);
 
 	/* If no client registered for type & id,
 	 * check for control client where type is not registered */
@@ -1008,57 +1519,75 @@ void smscore_onresponse(struct smscore_device_t *coredev,
 		rc = client->onresponse_handler(client->context, cb);
 
 	if (rc < 0) {
-		switch (phdr->msgType) {
+		switch (phdr->msg_type) {
+		case MSG_SMS_ISDBT_TUNE_RES:
+			break;
+		case MSG_SMS_RF_TUNE_RES:
+			break;
+		case MSG_SMS_SIGNAL_DETECTED_IND:
+			break;
+		case MSG_SMS_NO_SIGNAL_IND:
+			break;
+		case MSG_SMS_SPI_INT_LINE_SET_RES:
+			break;
+		case MSG_SMS_INTERFACE_LOCK_IND:
+			break;
+		case MSG_SMS_INTERFACE_UNLOCK_IND:
+			break;
 		case MSG_SMS_GET_VERSION_EX_RES:
 		{
-			struct SmsVersionRes_ST *ver =
-				(struct SmsVersionRes_ST *) phdr;
-			sms_debug("MSG_SMS_GET_VERSION_EX_RES "
-				  "id %d prots 0x%x ver %d.%d",
-				  ver->FirmwareId, ver->SupportedProtocols,
-				  ver->RomVersionMajor, ver->RomVersionMinor);
-
-			coredev->mode = ver->FirmwareId == 255 ?
-				DEVICE_MODE_NONE : ver->FirmwareId;
-			coredev->modes_supported = ver->SupportedProtocols;
+			struct sms_version_res *ver =
+				(struct sms_version_res *) phdr;
+			sms_debug("Firmware id %d prots 0x%x ver %d.%d",
+				  ver->firmware_id, ver->supported_protocols,
+				  ver->rom_ver_major, ver->rom_ver_minor);
+
+			coredev->mode = ver->firmware_id == 255 ?
+				DEVICE_MODE_NONE : ver->firmware_id;
+			coredev->modes_supported = ver->supported_protocols;
+			coredev->fw_version = ver->rom_ver_major << 8 |
+					      ver->rom_ver_minor;
 
 			complete(&coredev->version_ex_done);
 			break;
 		}
 		case MSG_SMS_INIT_DEVICE_RES:
-			sms_debug("MSG_SMS_INIT_DEVICE_RES");
 			complete(&coredev->init_device_done);
 			break;
 		case MSG_SW_RELOAD_START_RES:
-			sms_debug("MSG_SW_RELOAD_START_RES");
 			complete(&coredev->reload_start_done);
 			break;
+		case MSG_SMS_DATA_VALIDITY_RES:
+		{
+			struct sms_msg_data *validity = (struct sms_msg_data *) phdr;
+
+			sms_err("MSG_SMS_DATA_VALIDITY_RES, checksum = 0x%x",
+				validity->msg_data[0]);
+			complete(&coredev->data_validity_done);
+			break;
+		}
 		case MSG_SMS_DATA_DOWNLOAD_RES:
 			complete(&coredev->data_download_done);
 			break;
 		case MSG_SW_RELOAD_EXEC_RES:
-			sms_debug("MSG_SW_RELOAD_EXEC_RES");
 			break;
 		case MSG_SMS_SWDOWNLOAD_TRIGGER_RES:
-			sms_debug("MSG_SMS_SWDOWNLOAD_TRIGGER_RES");
 			complete(&coredev->trigger_done);
 			break;
 		case MSG_SMS_SLEEP_RESUME_COMP_IND:
 			complete(&coredev->resume_done);
 			break;
 		case MSG_SMS_GPIO_CONFIG_EX_RES:
-			sms_debug("MSG_SMS_GPIO_CONFIG_EX_RES");
 			complete(&coredev->gpio_configuration_done);
 			break;
 		case MSG_SMS_GPIO_SET_LEVEL_RES:
-			sms_debug("MSG_SMS_GPIO_SET_LEVEL_RES");
 			complete(&coredev->gpio_set_level_done);
 			break;
 		case MSG_SMS_GPIO_GET_LEVEL_RES:
 		{
 			u32 *msgdata = (u32 *) phdr;
 			coredev->gpio_get_res = msgdata[1];
-			sms_debug("MSG_SMS_GPIO_GET_LEVEL_RES gpio level %d",
+			sms_debug("gpio level %d",
 					coredev->gpio_get_res);
 			complete(&coredev->gpio_get_level_done);
 			break;
@@ -1070,12 +1599,24 @@ void smscore_onresponse(struct smscore_device_t *coredev,
 			sms_ir_event(coredev,
 				(const char *)
 				((char *)phdr
-				+ sizeof(struct SmsMsgHdr_ST)),
-				(int)phdr->msgLength
-				- sizeof(struct SmsMsgHdr_ST));
+				+ sizeof(struct sms_msg_hdr)),
+				(int)phdr->msg_length
+				- sizeof(struct sms_msg_hdr));
+			break;
+
+		case MSG_SMS_DVBT_BDA_DATA:
+			/*
+			 * It can be received here, if the frontend is
+			 * tuned into a valid channel and the proper firmware
+			 * is loaded. That happens when the module got removed
+			 * and re-inserted, without powering the device off
+			 */
 			break;
 
 		default:
+			sms_debug("message %s(%d) not handled.",
+				  smscore_translate_msg(phdr->msg_type),
+				  phdr->msg_type);
 			break;
 		}
 		smscore_putbuffer(coredev, cb);
@@ -1257,7 +1798,7 @@ int smsclient_sendrequest(struct smscore_client_t *client,
 			  void *buffer, size_t size)
 {
 	struct smscore_device_t *coredev;
-	struct SmsMsgHdr_ST *phdr = (struct SmsMsgHdr_ST *) buffer;
+	struct sms_msg_hdr *phdr = (struct sms_msg_hdr *) buffer;
 	int rc;
 
 	if (client == NULL) {
@@ -1274,7 +1815,7 @@ int smsclient_sendrequest(struct smscore_client_t *client,
 	}
 
 	rc = smscore_validate_client(client->coredev, client, 0,
-				     phdr->msgSrcId);
+				     phdr->msg_src_id);
 	if (rc < 0)
 		return rc;
 
@@ -1288,16 +1829,16 @@ int smscore_configure_gpio(struct smscore_device_t *coredev, u32 pin,
 			   struct smscore_config_gpio *pinconfig)
 {
 	struct {
-		struct SmsMsgHdr_ST hdr;
+		struct sms_msg_hdr hdr;
 		u32 data[6];
 	} msg;
 
 	if (coredev->device_flags & SMS_DEVICE_FAMILY2) {
-		msg.hdr.msgSrcId = DVBT_BDA_CONTROL_MSG_ID;
-		msg.hdr.msgDstId = HIF_TASK;
-		msg.hdr.msgFlags = 0;
-		msg.hdr.msgType  = MSG_SMS_GPIO_CONFIG_EX_REQ;
-		msg.hdr.msgLength = sizeof(msg);
+		msg.hdr.msg_src_id = DVBT_BDA_CONTROL_MSG_ID;
+		msg.hdr.msg_dst_id = HIF_TASK;
+		msg.hdr.msg_flags = 0;
+		msg.hdr.msg_type  = MSG_SMS_GPIO_CONFIG_EX_REQ;
+		msg.hdr.msg_length = sizeof(msg);
 
 		msg.data[0] = pin;
 		msg.data[1] = pinconfig->pullupdown;
@@ -1306,16 +1847,16 @@ int smscore_configure_gpio(struct smscore_device_t *coredev, u32 pin,
 		msg.data[2] = pinconfig->outputslewrate == 0 ? 3 : 0;
 
 		switch (pinconfig->outputdriving) {
-		case SMS_GPIO_OUTPUTDRIVING_16mA:
+		case SMS_GPIO_OUTPUTDRIVING_S_16mA:
 			msg.data[3] = 7; /* Nova - 16mA */
 			break;
-		case SMS_GPIO_OUTPUTDRIVING_12mA:
+		case SMS_GPIO_OUTPUTDRIVING_S_12mA:
 			msg.data[3] = 5; /* Nova - 11mA */
 			break;
-		case SMS_GPIO_OUTPUTDRIVING_8mA:
+		case SMS_GPIO_OUTPUTDRIVING_S_8mA:
 			msg.data[3] = 3; /* Nova - 7mA */
 			break;
-		case SMS_GPIO_OUTPUTDRIVING_4mA:
+		case SMS_GPIO_OUTPUTDRIVING_S_4mA:
 		default:
 			msg.data[3] = 2; /* Nova - 4mA */
 			break;
@@ -1333,18 +1874,18 @@ int smscore_configure_gpio(struct smscore_device_t *coredev, u32 pin,
 int smscore_set_gpio(struct smscore_device_t *coredev, u32 pin, int level)
 {
 	struct {
-		struct SmsMsgHdr_ST hdr;
+		struct sms_msg_hdr hdr;
 		u32 data[3];
 	} msg;
 
 	if (pin > MAX_GPIO_PIN_NUMBER)
 		return -EINVAL;
 
-	msg.hdr.msgSrcId = DVBT_BDA_CONTROL_MSG_ID;
-	msg.hdr.msgDstId = HIF_TASK;
-	msg.hdr.msgFlags = 0;
-	msg.hdr.msgType  = MSG_SMS_GPIO_SET_LEVEL_REQ;
-	msg.hdr.msgLength = sizeof(msg);
+	msg.hdr.msg_src_id = DVBT_BDA_CONTROL_MSG_ID;
+	msg.hdr.msg_dst_id = HIF_TASK;
+	msg.hdr.msg_flags = 0;
+	msg.hdr.msg_type  = MSG_SMS_GPIO_SET_LEVEL_REQ;
+	msg.hdr.msg_length = sizeof(msg);
 
 	msg.data[0] = pin;
 	msg.data[1] = level ? 1 : 0;
@@ -1355,122 +1896,121 @@ int smscore_set_gpio(struct smscore_device_t *coredev, u32 pin, int level)
 }
 
 /* new GPIO management implementation */
-static int GetGpioPinParams(u32 PinNum, u32 *pTranslatedPinNum,
-		u32 *pGroupNum, u32 *pGroupCfg) {
-
-	*pGroupCfg = 1;
-
-	if (PinNum <= 1)	{
-		*pTranslatedPinNum = 0;
-		*pGroupNum = 9;
-		*pGroupCfg = 2;
-	} else if (PinNum >= 2 && PinNum <= 6) {
-		*pTranslatedPinNum = 2;
-		*pGroupNum = 0;
-		*pGroupCfg = 2;
-	} else if (PinNum >= 7 && PinNum <= 11) {
-		*pTranslatedPinNum = 7;
-		*pGroupNum = 1;
-	} else if (PinNum >= 12 && PinNum <= 15) {
-		*pTranslatedPinNum = 12;
-		*pGroupNum = 2;
-		*pGroupCfg = 3;
-	} else if (PinNum == 16) {
-		*pTranslatedPinNum = 16;
-		*pGroupNum = 23;
-	} else if (PinNum >= 17 && PinNum <= 24) {
-		*pTranslatedPinNum = 17;
-		*pGroupNum = 3;
-	} else if (PinNum == 25) {
-		*pTranslatedPinNum = 25;
-		*pGroupNum = 6;
-	} else if (PinNum >= 26 && PinNum <= 28) {
-		*pTranslatedPinNum = 26;
-		*pGroupNum = 4;
-	} else if (PinNum == 29) {
-		*pTranslatedPinNum = 29;
-		*pGroupNum = 5;
-		*pGroupCfg = 2;
-	} else if (PinNum == 30) {
-		*pTranslatedPinNum = 30;
-		*pGroupNum = 8;
-	} else if (PinNum == 31) {
-		*pTranslatedPinNum = 31;
-		*pGroupNum = 17;
+static int get_gpio_pin_params(u32 pin_num, u32 *p_translatedpin_num,
+		u32 *p_group_num, u32 *p_group_cfg) {
+
+	*p_group_cfg = 1;
+
+	if (pin_num <= 1)	{
+		*p_translatedpin_num = 0;
+		*p_group_num = 9;
+		*p_group_cfg = 2;
+	} else if (pin_num >= 2 && pin_num <= 6) {
+		*p_translatedpin_num = 2;
+		*p_group_num = 0;
+		*p_group_cfg = 2;
+	} else if (pin_num >= 7 && pin_num <= 11) {
+		*p_translatedpin_num = 7;
+		*p_group_num = 1;
+	} else if (pin_num >= 12 && pin_num <= 15) {
+		*p_translatedpin_num = 12;
+		*p_group_num = 2;
+		*p_group_cfg = 3;
+	} else if (pin_num == 16) {
+		*p_translatedpin_num = 16;
+		*p_group_num = 23;
+	} else if (pin_num >= 17 && pin_num <= 24) {
+		*p_translatedpin_num = 17;
+		*p_group_num = 3;
+	} else if (pin_num == 25) {
+		*p_translatedpin_num = 25;
+		*p_group_num = 6;
+	} else if (pin_num >= 26 && pin_num <= 28) {
+		*p_translatedpin_num = 26;
+		*p_group_num = 4;
+	} else if (pin_num == 29) {
+		*p_translatedpin_num = 29;
+		*p_group_num = 5;
+		*p_group_cfg = 2;
+	} else if (pin_num == 30) {
+		*p_translatedpin_num = 30;
+		*p_group_num = 8;
+	} else if (pin_num == 31) {
+		*p_translatedpin_num = 31;
+		*p_group_num = 17;
 	} else
 		return -1;
 
-	*pGroupCfg <<= 24;
+	*p_group_cfg <<= 24;
 
 	return 0;
 }
 
-int smscore_gpio_configure(struct smscore_device_t *coredev, u8 PinNum,
-		struct smscore_gpio_config *pGpioConfig) {
+int smscore_gpio_configure(struct smscore_device_t *coredev, u8 pin_num,
+		struct smscore_config_gpio *p_gpio_config) {
 
-	u32 totalLen;
-	u32 TranslatedPinNum = 0;
-	u32 GroupNum = 0;
-	u32 ElectricChar;
-	u32 groupCfg;
+	u32 total_len;
+	u32 translatedpin_num = 0;
+	u32 group_num = 0;
+	u32 electric_char;
+	u32 group_cfg;
 	void *buffer;
 	int rc;
 
-	struct SetGpioMsg {
-		struct SmsMsgHdr_ST xMsgHeader;
-		u32 msgData[6];
-	} *pMsg;
+	struct set_gpio_msg {
+		struct sms_msg_hdr x_msg_header;
+		u32 msg_data[6];
+	} *p_msg;
 
 
-	if (PinNum > MAX_GPIO_PIN_NUMBER)
+	if (pin_num > MAX_GPIO_PIN_NUMBER)
 		return -EINVAL;
 
-	if (pGpioConfig == NULL)
+	if (p_gpio_config == NULL)
 		return -EINVAL;
 
-	totalLen = sizeof(struct SmsMsgHdr_ST) + (sizeof(u32) * 6);
+	total_len = sizeof(struct sms_msg_hdr) + (sizeof(u32) * 6);
 
-	buffer = kmalloc(totalLen + SMS_DMA_ALIGNMENT,
+	buffer = kmalloc(total_len + SMS_DMA_ALIGNMENT,
 			GFP_KERNEL | GFP_DMA);
 	if (!buffer)
 		return -ENOMEM;
 
-	pMsg = (struct SetGpioMsg *) SMS_ALIGN_ADDRESS(buffer);
+	p_msg = (struct set_gpio_msg *) SMS_ALIGN_ADDRESS(buffer);
 
-	pMsg->xMsgHeader.msgSrcId = DVBT_BDA_CONTROL_MSG_ID;
-	pMsg->xMsgHeader.msgDstId = HIF_TASK;
-	pMsg->xMsgHeader.msgFlags = 0;
-	pMsg->xMsgHeader.msgLength = (u16) totalLen;
-	pMsg->msgData[0] = PinNum;
+	p_msg->x_msg_header.msg_src_id = DVBT_BDA_CONTROL_MSG_ID;
+	p_msg->x_msg_header.msg_dst_id = HIF_TASK;
+	p_msg->x_msg_header.msg_flags = 0;
+	p_msg->x_msg_header.msg_length = (u16) total_len;
+	p_msg->msg_data[0] = pin_num;
 
 	if (!(coredev->device_flags & SMS_DEVICE_FAMILY2)) {
-		pMsg->xMsgHeader.msgType = MSG_SMS_GPIO_CONFIG_REQ;
-		if (GetGpioPinParams(PinNum, &TranslatedPinNum, &GroupNum,
-				&groupCfg) != 0) {
+		p_msg->x_msg_header.msg_type = MSG_SMS_GPIO_CONFIG_REQ;
+		if (get_gpio_pin_params(pin_num, &translatedpin_num, &group_num,
+				&group_cfg) != 0) {
 			rc = -EINVAL;
 			goto free;
 		}
 
-		pMsg->msgData[1] = TranslatedPinNum;
-		pMsg->msgData[2] = GroupNum;
-		ElectricChar = (pGpioConfig->PullUpDown)
-				| (pGpioConfig->InputCharacteristics << 2)
-				| (pGpioConfig->OutputSlewRate << 3)
-				| (pGpioConfig->OutputDriving << 4);
-		pMsg->msgData[3] = ElectricChar;
-		pMsg->msgData[4] = pGpioConfig->Direction;
-		pMsg->msgData[5] = groupCfg;
+		p_msg->msg_data[1] = translatedpin_num;
+		p_msg->msg_data[2] = group_num;
+		electric_char = (p_gpio_config->pullupdown)
+				| (p_gpio_config->inputcharacteristics << 2)
+				| (p_gpio_config->outputslewrate << 3)
+				| (p_gpio_config->outputdriving << 4);
+		p_msg->msg_data[3] = electric_char;
+		p_msg->msg_data[4] = p_gpio_config->direction;
+		p_msg->msg_data[5] = group_cfg;
 	} else {
-		pMsg->xMsgHeader.msgType = MSG_SMS_GPIO_CONFIG_EX_REQ;
-		pMsg->msgData[1] = pGpioConfig->PullUpDown;
-		pMsg->msgData[2] = pGpioConfig->OutputSlewRate;
-		pMsg->msgData[3] = pGpioConfig->OutputDriving;
-		pMsg->msgData[4] = pGpioConfig->Direction;
-		pMsg->msgData[5] = 0;
+		p_msg->x_msg_header.msg_type = MSG_SMS_GPIO_CONFIG_EX_REQ;
+		p_msg->msg_data[1] = p_gpio_config->pullupdown;
+		p_msg->msg_data[2] = p_gpio_config->outputslewrate;
+		p_msg->msg_data[3] = p_gpio_config->outputdriving;
+		p_msg->msg_data[4] = p_gpio_config->direction;
+		p_msg->msg_data[5] = 0;
 	}
 
-	smsendian_handle_tx_message((struct SmsMsgHdr_ST *)pMsg);
-	rc = smscore_sendrequest_and_wait(coredev, pMsg, totalLen,
+	rc = smscore_sendrequest_and_wait(coredev, p_msg, total_len,
 			&coredev->gpio_configuration_done);
 
 	if (rc != 0) {
@@ -1485,42 +2025,41 @@ free:
 	return rc;
 }
 
-int smscore_gpio_set_level(struct smscore_device_t *coredev, u8 PinNum,
-		u8 NewLevel) {
+int smscore_gpio_set_level(struct smscore_device_t *coredev, u8 pin_num,
+		u8 new_level) {
 
-	u32 totalLen;
+	u32 total_len;
 	int rc;
 	void *buffer;
 
-	struct SetGpioMsg {
-		struct SmsMsgHdr_ST xMsgHeader;
-		u32 msgData[3]; /* keep it 3 ! */
-	} *pMsg;
+	struct set_gpio_msg {
+		struct sms_msg_hdr x_msg_header;
+		u32 msg_data[3]; /* keep it 3 ! */
+	} *p_msg;
 
-	if ((NewLevel > 1) || (PinNum > MAX_GPIO_PIN_NUMBER))
+	if ((new_level > 1) || (pin_num > MAX_GPIO_PIN_NUMBER))
 		return -EINVAL;
 
-	totalLen = sizeof(struct SmsMsgHdr_ST) +
+	total_len = sizeof(struct sms_msg_hdr) +
 			(3 * sizeof(u32)); /* keep it 3 ! */
 
-	buffer = kmalloc(totalLen + SMS_DMA_ALIGNMENT,
+	buffer = kmalloc(total_len + SMS_DMA_ALIGNMENT,
 			GFP_KERNEL | GFP_DMA);
 	if (!buffer)
 		return -ENOMEM;
 
-	pMsg = (struct SetGpioMsg *) SMS_ALIGN_ADDRESS(buffer);
+	p_msg = (struct set_gpio_msg *) SMS_ALIGN_ADDRESS(buffer);
 
-	pMsg->xMsgHeader.msgSrcId = DVBT_BDA_CONTROL_MSG_ID;
-	pMsg->xMsgHeader.msgDstId = HIF_TASK;
-	pMsg->xMsgHeader.msgFlags = 0;
-	pMsg->xMsgHeader.msgType = MSG_SMS_GPIO_SET_LEVEL_REQ;
-	pMsg->xMsgHeader.msgLength = (u16) totalLen;
-	pMsg->msgData[0] = PinNum;
-	pMsg->msgData[1] = NewLevel;
+	p_msg->x_msg_header.msg_src_id = DVBT_BDA_CONTROL_MSG_ID;
+	p_msg->x_msg_header.msg_dst_id = HIF_TASK;
+	p_msg->x_msg_header.msg_flags = 0;
+	p_msg->x_msg_header.msg_type = MSG_SMS_GPIO_SET_LEVEL_REQ;
+	p_msg->x_msg_header.msg_length = (u16) total_len;
+	p_msg->msg_data[0] = pin_num;
+	p_msg->msg_data[1] = new_level;
 
 	/* Send message to SMS */
-	smsendian_handle_tx_message((struct SmsMsgHdr_ST *)pMsg);
-	rc = smscore_sendrequest_and_wait(coredev, pMsg, totalLen,
+	rc = smscore_sendrequest_and_wait(coredev, p_msg, total_len,
 			&coredev->gpio_set_level_done);
 
 	if (rc != 0) {
@@ -1534,42 +2073,41 @@ int smscore_gpio_set_level(struct smscore_device_t *coredev, u8 PinNum,
 	return rc;
 }
 
-int smscore_gpio_get_level(struct smscore_device_t *coredev, u8 PinNum,
+int smscore_gpio_get_level(struct smscore_device_t *coredev, u8 pin_num,
 		u8 *level) {
 
-	u32 totalLen;
+	u32 total_len;
 	int rc;
 	void *buffer;
 
-	struct SetGpioMsg {
-		struct SmsMsgHdr_ST xMsgHeader;
-		u32 msgData[2];
-	} *pMsg;
+	struct set_gpio_msg {
+		struct sms_msg_hdr x_msg_header;
+		u32 msg_data[2];
+	} *p_msg;
 
 
-	if (PinNum > MAX_GPIO_PIN_NUMBER)
+	if (pin_num > MAX_GPIO_PIN_NUMBER)
 		return -EINVAL;
 
-	totalLen = sizeof(struct SmsMsgHdr_ST) + (2 * sizeof(u32));
+	total_len = sizeof(struct sms_msg_hdr) + (2 * sizeof(u32));
 
-	buffer = kmalloc(totalLen + SMS_DMA_ALIGNMENT,
+	buffer = kmalloc(total_len + SMS_DMA_ALIGNMENT,
 			GFP_KERNEL | GFP_DMA);
 	if (!buffer)
 		return -ENOMEM;
 
-	pMsg = (struct SetGpioMsg *) SMS_ALIGN_ADDRESS(buffer);
+	p_msg = (struct set_gpio_msg *) SMS_ALIGN_ADDRESS(buffer);
 
-	pMsg->xMsgHeader.msgSrcId = DVBT_BDA_CONTROL_MSG_ID;
-	pMsg->xMsgHeader.msgDstId = HIF_TASK;
-	pMsg->xMsgHeader.msgFlags = 0;
-	pMsg->xMsgHeader.msgType = MSG_SMS_GPIO_GET_LEVEL_REQ;
-	pMsg->xMsgHeader.msgLength = (u16) totalLen;
-	pMsg->msgData[0] = PinNum;
-	pMsg->msgData[1] = 0;
+	p_msg->x_msg_header.msg_src_id = DVBT_BDA_CONTROL_MSG_ID;
+	p_msg->x_msg_header.msg_dst_id = HIF_TASK;
+	p_msg->x_msg_header.msg_flags = 0;
+	p_msg->x_msg_header.msg_type = MSG_SMS_GPIO_GET_LEVEL_REQ;
+	p_msg->x_msg_header.msg_length = (u16) total_len;
+	p_msg->msg_data[0] = pin_num;
+	p_msg->msg_data[1] = 0;
 
 	/* Send message to SMS */
-	smsendian_handle_tx_message((struct SmsMsgHdr_ST *)pMsg);
-	rc = smscore_sendrequest_and_wait(coredev, pMsg, totalLen,
+	rc = smscore_sendrequest_and_wait(coredev, p_msg, total_len,
 			&coredev->gpio_get_level_done);
 
 	if (rc != 0) {
@@ -1635,3 +2173,27 @@ module_exit(smscore_module_exit);
 MODULE_DESCRIPTION("Siano MDTV Core module");
 MODULE_AUTHOR("Siano Mobile Silicon, Inc. (uris@siano-ms.com)");
 MODULE_LICENSE("GPL");
+
+/* This should match what's defined at smscoreapi.h */
+MODULE_FIRMWARE(SMS_FW_ATSC_DENVER);
+MODULE_FIRMWARE(SMS_FW_CMMB_MING_APP);
+MODULE_FIRMWARE(SMS_FW_CMMB_VEGA_12MHZ);
+MODULE_FIRMWARE(SMS_FW_CMMB_VENICE_12MHZ);
+MODULE_FIRMWARE(SMS_FW_DVBH_RIO);
+MODULE_FIRMWARE(SMS_FW_DVB_NOVA_12MHZ_B0);
+MODULE_FIRMWARE(SMS_FW_DVB_NOVA_12MHZ);
+MODULE_FIRMWARE(SMS_FW_DVB_RIO);
+MODULE_FIRMWARE(SMS_FW_FM_RADIO);
+MODULE_FIRMWARE(SMS_FW_FM_RADIO_RIO);
+MODULE_FIRMWARE(SMS_FW_DVBT_HCW_55XXX);
+MODULE_FIRMWARE(SMS_FW_ISDBT_HCW_55XXX);
+MODULE_FIRMWARE(SMS_FW_ISDBT_NOVA_12MHZ_B0);
+MODULE_FIRMWARE(SMS_FW_ISDBT_NOVA_12MHZ);
+MODULE_FIRMWARE(SMS_FW_ISDBT_PELE);
+MODULE_FIRMWARE(SMS_FW_ISDBT_RIO);
+MODULE_FIRMWARE(SMS_FW_DVBT_NOVA_A);
+MODULE_FIRMWARE(SMS_FW_DVBT_NOVA_B);
+MODULE_FIRMWARE(SMS_FW_DVBT_STELLAR);
+MODULE_FIRMWARE(SMS_FW_TDMB_DENVER);
+MODULE_FIRMWARE(SMS_FW_TDMB_NOVA_12MHZ_B0);
+MODULE_FIRMWARE(SMS_FW_TDMB_NOVA_12MHZ);
diff --git a/drivers/media/common/siano/smscoreapi.h b/drivers/media/common/siano/smscoreapi.h
index c592ae09039..d0799e32336 100644
--- a/drivers/media/common/siano/smscoreapi.h
+++ b/drivers/media/common/siano/smscoreapi.h
@@ -40,9 +40,33 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define kmutex_trylock(_p_) mutex_trylock(_p_)
 #define kmutex_unlock(_p_) mutex_unlock(_p_)
 
-#ifndef min
-#define min(a, b) (((a) < (b)) ? (a) : (b))
-#endif
+/*
+ * Define the firmware names used by the driver.
+ * Those should match what's used at smscoreapi.c and sms-cards.c
+ * including the MODULE_FIRMWARE() macros at the end of smscoreapi.c
+ */
+#define SMS_FW_ATSC_DENVER         "atsc_denver.inp"
+#define SMS_FW_CMMB_MING_APP       "cmmb_ming_app.inp"
+#define SMS_FW_CMMB_VEGA_12MHZ     "cmmb_vega_12mhz.inp"
+#define SMS_FW_CMMB_VENICE_12MHZ   "cmmb_venice_12mhz.inp"
+#define SMS_FW_DVBH_RIO            "dvbh_rio.inp"
+#define SMS_FW_DVB_NOVA_12MHZ_B0   "dvb_nova_12mhz_b0.inp"
+#define SMS_FW_DVB_NOVA_12MHZ      "dvb_nova_12mhz.inp"
+#define SMS_FW_DVB_RIO             "dvb_rio.inp"
+#define SMS_FW_FM_RADIO            "fm_radio.inp"
+#define SMS_FW_FM_RADIO_RIO        "fm_radio_rio.inp"
+#define SMS_FW_DVBT_HCW_55XXX      "sms1xxx-hcw-55xxx-dvbt-02.fw"
+#define SMS_FW_ISDBT_HCW_55XXX     "sms1xxx-hcw-55xxx-isdbt-02.fw"
+#define SMS_FW_ISDBT_NOVA_12MHZ_B0 "isdbt_nova_12mhz_b0.inp"
+#define SMS_FW_ISDBT_NOVA_12MHZ    "isdbt_nova_12mhz.inp"
+#define SMS_FW_ISDBT_PELE          "isdbt_pele.inp"
+#define SMS_FW_ISDBT_RIO           "isdbt_rio.inp"
+#define SMS_FW_DVBT_NOVA_A         "sms1xxx-nova-a-dvbt-01.fw"
+#define SMS_FW_DVBT_NOVA_B         "sms1xxx-nova-b-dvbt-01.fw"
+#define SMS_FW_DVBT_STELLAR        "sms1xxx-stellar-dvbt-01.fw"
+#define SMS_FW_TDMB_DENVER         "tdmb_denver.inp"
+#define SMS_FW_TDMB_NOVA_12MHZ_B0  "tdmb_nova_12mhz_b0.inp"
+#define SMS_FW_TDMB_NOVA_12MHZ     "tdmb_nova_12mhz.inp"
 
 #define SMS_PROTOCOL_MAX_RAOUNDTRIP_MS			(10000)
 #define SMS_ALLOC_ALIGNMENT				128
@@ -50,18 +74,31 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define SMS_ALIGN_ADDRESS(addr) \
 	((((uintptr_t)(addr)) + (SMS_DMA_ALIGNMENT-1)) & ~(SMS_DMA_ALIGNMENT-1))
 
+#define SMS_DEVICE_FAMILY1				0
 #define SMS_DEVICE_FAMILY2				1
 #define SMS_ROM_NO_RESPONSE				2
 #define SMS_DEVICE_NOT_READY				0x8000000
 
 enum sms_device_type_st {
+	SMS_UNKNOWN_TYPE = -1,
 	SMS_STELLAR = 0,
 	SMS_NOVA_A0,
 	SMS_NOVA_B0,
 	SMS_VEGA,
+	SMS_VENICE,
+	SMS_MING,
+	SMS_PELE,
+	SMS_RIO,
+	SMS_DENVER_1530,
+	SMS_DENVER_2160,
 	SMS_NUM_OF_DEVICE_TYPES
 };
 
+enum sms_power_mode_st {
+	SMS_POWER_MODE_ACTIVE,
+	SMS_POWER_MODE_SUSPENDED
+};
+
 struct smscore_device_t;
 struct smscore_client_t;
 struct smscore_buffer_t;
@@ -149,6 +186,7 @@ struct smscore_device_t {
 
 	/* host <--> device messages */
 	struct completion version_ex_done, data_download_done, trigger_done;
+	struct completion data_validity_done, device_ready_done;
 	struct completion init_device_done, reload_start_done, resume_done;
 	struct completion gpio_configuration_done, gpio_set_level_done;
 	struct completion gpio_get_level_done, ir_init_done;
@@ -165,10 +203,17 @@ struct smscore_device_t {
 	/* Firmware */
 	u8 *fw_buf;
 	u32 fw_buf_size;
+	u16 fw_version;
 
 	/* Infrared (IR) */
 	struct ir_t ir;
 
+	/*
+	 * Identify if device is USB or not.
+	 * Used by smsdvb-sysfs to know the root node for debugfs
+	 */
+	bool is_usb_device;
+
 	int led_state;
 };
 
@@ -176,81 +221,363 @@ struct smscore_device_t {
 #define SMS_ANTENNA_GPIO_0					1
 #define SMS_ANTENNA_GPIO_1					0
 
-#define BW_8_MHZ							0
-#define BW_7_MHZ							1
-#define BW_6_MHZ							2
-#define BW_5_MHZ							3
-#define BW_ISDBT_1SEG						4
-#define BW_ISDBT_3SEG						5
+enum sms_bandwidth_mode {
+	BW_8_MHZ = 0,
+	BW_7_MHZ = 1,
+	BW_6_MHZ = 2,
+	BW_5_MHZ = 3,
+	BW_ISDBT_1SEG = 4,
+	BW_ISDBT_3SEG = 5,
+	BW_2_MHZ = 6,
+	BW_FM_RADIO = 7,
+	BW_ISDBT_13SEG = 8,
+	BW_1_5_MHZ = 15,
+	BW_UNKNOWN = 0xffff
+};
+
 
 #define MSG_HDR_FLAG_SPLIT_MSG				4
 
 #define MAX_GPIO_PIN_NUMBER					31
 
 #define HIF_TASK							11
+#define HIF_TASK_SLAVE					22
+#define HIF_TASK_SLAVE2					33
+#define HIF_TASK_SLAVE3					44
 #define SMS_HOST_LIB						150
 #define DVBT_BDA_CONTROL_MSG_ID				201
 
 #define SMS_MAX_PAYLOAD_SIZE				240
 #define SMS_TUNE_TIMEOUT					500
 
-#define MSG_SMS_GPIO_CONFIG_REQ				507
-#define MSG_SMS_GPIO_CONFIG_RES				508
-#define MSG_SMS_GPIO_SET_LEVEL_REQ			509
-#define MSG_SMS_GPIO_SET_LEVEL_RES			510
-#define MSG_SMS_GPIO_GET_LEVEL_REQ			511
-#define MSG_SMS_GPIO_GET_LEVEL_RES			512
-#define MSG_SMS_RF_TUNE_REQ					561
-#define MSG_SMS_RF_TUNE_RES					562
-#define MSG_SMS_INIT_DEVICE_REQ				578
-#define MSG_SMS_INIT_DEVICE_RES				579
-#define MSG_SMS_ADD_PID_FILTER_REQ			601
-#define MSG_SMS_ADD_PID_FILTER_RES			602
-#define MSG_SMS_REMOVE_PID_FILTER_REQ			603
-#define MSG_SMS_REMOVE_PID_FILTER_RES			604
-#define MSG_SMS_DAB_CHANNEL				607
-#define MSG_SMS_GET_PID_FILTER_LIST_REQ			608
-#define MSG_SMS_GET_PID_FILTER_LIST_RES			609
-#define MSG_SMS_GET_STATISTICS_RES			616
-#define MSG_SMS_GET_STATISTICS_REQ			615
-#define MSG_SMS_HO_PER_SLICES_IND			630
-#define MSG_SMS_SET_ANTENNA_CONFIG_REQ			651
-#define MSG_SMS_SET_ANTENNA_CONFIG_RES			652
-#define MSG_SMS_SLEEP_RESUME_COMP_IND			655
-#define MSG_SMS_DATA_DOWNLOAD_REQ			660
-#define MSG_SMS_DATA_DOWNLOAD_RES			661
-#define MSG_SMS_SWDOWNLOAD_TRIGGER_REQ		664
-#define MSG_SMS_SWDOWNLOAD_TRIGGER_RES		665
-#define MSG_SMS_SWDOWNLOAD_BACKDOOR_REQ		666
-#define MSG_SMS_SWDOWNLOAD_BACKDOOR_RES		667
-#define MSG_SMS_GET_VERSION_EX_REQ			668
-#define MSG_SMS_GET_VERSION_EX_RES			669
-#define MSG_SMS_SET_CLOCK_OUTPUT_REQ		670
-#define MSG_SMS_I2C_SET_FREQ_REQ			685
-#define MSG_SMS_GENERIC_I2C_REQ				687
-#define MSG_SMS_GENERIC_I2C_RES				688
-#define MSG_SMS_DVBT_BDA_DATA				693
-#define MSG_SW_RELOAD_REQ					697
-#define MSG_SMS_DATA_MSG					699
-#define MSG_SW_RELOAD_START_REQ				702
-#define MSG_SW_RELOAD_START_RES				703
-#define MSG_SW_RELOAD_EXEC_REQ				704
-#define MSG_SW_RELOAD_EXEC_RES				705
-#define MSG_SMS_SPI_INT_LINE_SET_REQ		710
-#define MSG_SMS_GPIO_CONFIG_EX_REQ			712
-#define MSG_SMS_GPIO_CONFIG_EX_RES			713
-#define MSG_SMS_ISDBT_TUNE_REQ				776
-#define MSG_SMS_ISDBT_TUNE_RES				777
-#define MSG_SMS_TRANSMISSION_IND			782
-#define MSG_SMS_START_IR_REQ				800
-#define MSG_SMS_START_IR_RES				801
-#define MSG_SMS_IR_SAMPLES_IND				802
-#define MSG_SMS_SIGNAL_DETECTED_IND			827
-#define MSG_SMS_NO_SIGNAL_IND				828
+enum msg_types {
+	MSG_TYPE_BASE_VAL = 500,
+	MSG_SMS_GET_VERSION_REQ = 503,
+	MSG_SMS_GET_VERSION_RES = 504,
+	MSG_SMS_MULTI_BRIDGE_CFG = 505,
+	MSG_SMS_GPIO_CONFIG_REQ = 507,
+	MSG_SMS_GPIO_CONFIG_RES = 508,
+	MSG_SMS_GPIO_SET_LEVEL_REQ = 509,
+	MSG_SMS_GPIO_SET_LEVEL_RES = 510,
+	MSG_SMS_GPIO_GET_LEVEL_REQ = 511,
+	MSG_SMS_GPIO_GET_LEVEL_RES = 512,
+	MSG_SMS_EEPROM_BURN_IND = 513,
+	MSG_SMS_LOG_ENABLE_CHANGE_REQ = 514,
+	MSG_SMS_LOG_ENABLE_CHANGE_RES = 515,
+	MSG_SMS_SET_MAX_TX_MSG_LEN_REQ = 516,
+	MSG_SMS_SET_MAX_TX_MSG_LEN_RES = 517,
+	MSG_SMS_SPI_HALFDUPLEX_TOKEN_HOST_TO_DEVICE = 518,
+	MSG_SMS_SPI_HALFDUPLEX_TOKEN_DEVICE_TO_HOST = 519,
+	MSG_SMS_BACKGROUND_SCAN_FLAG_CHANGE_REQ = 520,
+	MSG_SMS_BACKGROUND_SCAN_FLAG_CHANGE_RES = 521,
+	MSG_SMS_BACKGROUND_SCAN_SIGNAL_DETECTED_IND = 522,
+	MSG_SMS_BACKGROUND_SCAN_NO_SIGNAL_IND = 523,
+	MSG_SMS_CONFIGURE_RF_SWITCH_REQ = 524,
+	MSG_SMS_CONFIGURE_RF_SWITCH_RES = 525,
+	MSG_SMS_MRC_PATH_DISCONNECT_REQ = 526,
+	MSG_SMS_MRC_PATH_DISCONNECT_RES = 527,
+	MSG_SMS_RECEIVE_1SEG_THROUGH_FULLSEG_REQ = 528,
+	MSG_SMS_RECEIVE_1SEG_THROUGH_FULLSEG_RES = 529,
+	MSG_SMS_RECEIVE_VHF_VIA_VHF_INPUT_REQ = 530,
+	MSG_SMS_RECEIVE_VHF_VIA_VHF_INPUT_RES = 531,
+	MSG_WR_REG_RFT_REQ = 533,
+	MSG_WR_REG_RFT_RES = 534,
+	MSG_RD_REG_RFT_REQ = 535,
+	MSG_RD_REG_RFT_RES = 536,
+	MSG_RD_REG_ALL_RFT_REQ = 537,
+	MSG_RD_REG_ALL_RFT_RES = 538,
+	MSG_HELP_INT = 539,
+	MSG_RUN_SCRIPT_INT = 540,
+	MSG_SMS_EWS_INBAND_REQ = 541,
+	MSG_SMS_EWS_INBAND_RES = 542,
+	MSG_SMS_RFS_SELECT_REQ = 543,
+	MSG_SMS_RFS_SELECT_RES = 544,
+	MSG_SMS_MB_GET_VER_REQ = 545,
+	MSG_SMS_MB_GET_VER_RES = 546,
+	MSG_SMS_MB_WRITE_CFGFILE_REQ = 547,
+	MSG_SMS_MB_WRITE_CFGFILE_RES = 548,
+	MSG_SMS_MB_READ_CFGFILE_REQ = 549,
+	MSG_SMS_MB_READ_CFGFILE_RES = 550,
+	MSG_SMS_RD_MEM_REQ = 552,
+	MSG_SMS_RD_MEM_RES = 553,
+	MSG_SMS_WR_MEM_REQ = 554,
+	MSG_SMS_WR_MEM_RES = 555,
+	MSG_SMS_UPDATE_MEM_REQ = 556,
+	MSG_SMS_UPDATE_MEM_RES = 557,
+	MSG_SMS_ISDBT_ENABLE_FULL_PARAMS_SET_REQ = 558,
+	MSG_SMS_ISDBT_ENABLE_FULL_PARAMS_SET_RES = 559,
+	MSG_SMS_RF_TUNE_REQ = 561,
+	MSG_SMS_RF_TUNE_RES = 562,
+	MSG_SMS_ISDBT_ENABLE_HIGH_MOBILITY_REQ = 563,
+	MSG_SMS_ISDBT_ENABLE_HIGH_MOBILITY_RES = 564,
+	MSG_SMS_ISDBT_SB_RECEPTION_REQ = 565,
+	MSG_SMS_ISDBT_SB_RECEPTION_RES = 566,
+	MSG_SMS_GENERIC_EPROM_WRITE_REQ = 567,
+	MSG_SMS_GENERIC_EPROM_WRITE_RES = 568,
+	MSG_SMS_GENERIC_EPROM_READ_REQ = 569,
+	MSG_SMS_GENERIC_EPROM_READ_RES = 570,
+	MSG_SMS_EEPROM_WRITE_REQ = 571,
+	MSG_SMS_EEPROM_WRITE_RES = 572,
+	MSG_SMS_CUSTOM_READ_REQ = 574,
+	MSG_SMS_CUSTOM_READ_RES = 575,
+	MSG_SMS_CUSTOM_WRITE_REQ = 576,
+	MSG_SMS_CUSTOM_WRITE_RES = 577,
+	MSG_SMS_INIT_DEVICE_REQ = 578,
+	MSG_SMS_INIT_DEVICE_RES = 579,
+	MSG_SMS_ATSC_SET_ALL_IP_REQ = 580,
+	MSG_SMS_ATSC_SET_ALL_IP_RES = 581,
+	MSG_SMS_ATSC_START_ENSEMBLE_REQ = 582,
+	MSG_SMS_ATSC_START_ENSEMBLE_RES = 583,
+	MSG_SMS_SET_OUTPUT_MODE_REQ = 584,
+	MSG_SMS_SET_OUTPUT_MODE_RES = 585,
+	MSG_SMS_ATSC_IP_FILTER_GET_LIST_REQ = 586,
+	MSG_SMS_ATSC_IP_FILTER_GET_LIST_RES = 587,
+	MSG_SMS_SUB_CHANNEL_START_REQ = 589,
+	MSG_SMS_SUB_CHANNEL_START_RES = 590,
+	MSG_SMS_SUB_CHANNEL_STOP_REQ = 591,
+	MSG_SMS_SUB_CHANNEL_STOP_RES = 592,
+	MSG_SMS_ATSC_IP_FILTER_ADD_REQ = 593,
+	MSG_SMS_ATSC_IP_FILTER_ADD_RES = 594,
+	MSG_SMS_ATSC_IP_FILTER_REMOVE_REQ = 595,
+	MSG_SMS_ATSC_IP_FILTER_REMOVE_RES = 596,
+	MSG_SMS_ATSC_IP_FILTER_REMOVE_ALL_REQ = 597,
+	MSG_SMS_ATSC_IP_FILTER_REMOVE_ALL_RES = 598,
+	MSG_SMS_WAIT_CMD = 599,
+	MSG_SMS_ADD_PID_FILTER_REQ = 601,
+	MSG_SMS_ADD_PID_FILTER_RES = 602,
+	MSG_SMS_REMOVE_PID_FILTER_REQ = 603,
+	MSG_SMS_REMOVE_PID_FILTER_RES = 604,
+	MSG_SMS_FAST_INFORMATION_CHANNEL_REQ = 605,
+	MSG_SMS_FAST_INFORMATION_CHANNEL_RES = 606,
+	MSG_SMS_DAB_CHANNEL = 607,
+	MSG_SMS_GET_PID_FILTER_LIST_REQ = 608,
+	MSG_SMS_GET_PID_FILTER_LIST_RES = 609,
+	MSG_SMS_POWER_DOWN_REQ = 610,
+	MSG_SMS_POWER_DOWN_RES = 611,
+	MSG_SMS_ATSC_SLT_EXIST_IND = 612,
+	MSG_SMS_ATSC_NO_SLT_IND = 613,
+	MSG_SMS_GET_STATISTICS_REQ = 615,
+	MSG_SMS_GET_STATISTICS_RES = 616,
+	MSG_SMS_SEND_DUMP = 617,
+	MSG_SMS_SCAN_START_REQ = 618,
+	MSG_SMS_SCAN_START_RES = 619,
+	MSG_SMS_SCAN_STOP_REQ = 620,
+	MSG_SMS_SCAN_STOP_RES = 621,
+	MSG_SMS_SCAN_PROGRESS_IND = 622,
+	MSG_SMS_SCAN_COMPLETE_IND = 623,
+	MSG_SMS_LOG_ITEM = 624,
+	MSG_SMS_DAB_SUBCHANNEL_RECONFIG_REQ = 628,
+	MSG_SMS_DAB_SUBCHANNEL_RECONFIG_RES = 629,
+	MSG_SMS_HO_PER_SLICES_IND = 630,
+	MSG_SMS_HO_INBAND_POWER_IND = 631,
+	MSG_SMS_MANUAL_DEMOD_REQ = 632,
+	MSG_SMS_HO_TUNE_ON_REQ = 636,
+	MSG_SMS_HO_TUNE_ON_RES = 637,
+	MSG_SMS_HO_TUNE_OFF_REQ = 638,
+	MSG_SMS_HO_TUNE_OFF_RES = 639,
+	MSG_SMS_HO_PEEK_FREQ_REQ = 640,
+	MSG_SMS_HO_PEEK_FREQ_RES = 641,
+	MSG_SMS_HO_PEEK_FREQ_IND = 642,
+	MSG_SMS_MB_ATTEN_SET_REQ = 643,
+	MSG_SMS_MB_ATTEN_SET_RES = 644,
+	MSG_SMS_ENABLE_STAT_IN_I2C_REQ = 649,
+	MSG_SMS_ENABLE_STAT_IN_I2C_RES = 650,
+	MSG_SMS_SET_ANTENNA_CONFIG_REQ = 651,
+	MSG_SMS_SET_ANTENNA_CONFIG_RES = 652,
+	MSG_SMS_GET_STATISTICS_EX_REQ = 653,
+	MSG_SMS_GET_STATISTICS_EX_RES = 654,
+	MSG_SMS_SLEEP_RESUME_COMP_IND = 655,
+	MSG_SMS_SWITCH_HOST_INTERFACE_REQ = 656,
+	MSG_SMS_SWITCH_HOST_INTERFACE_RES = 657,
+	MSG_SMS_DATA_DOWNLOAD_REQ = 660,
+	MSG_SMS_DATA_DOWNLOAD_RES = 661,
+	MSG_SMS_DATA_VALIDITY_REQ = 662,
+	MSG_SMS_DATA_VALIDITY_RES = 663,
+	MSG_SMS_SWDOWNLOAD_TRIGGER_REQ = 664,
+	MSG_SMS_SWDOWNLOAD_TRIGGER_RES = 665,
+	MSG_SMS_SWDOWNLOAD_BACKDOOR_REQ = 666,
+	MSG_SMS_SWDOWNLOAD_BACKDOOR_RES = 667,
+	MSG_SMS_GET_VERSION_EX_REQ = 668,
+	MSG_SMS_GET_VERSION_EX_RES = 669,
+	MSG_SMS_CLOCK_OUTPUT_CONFIG_REQ = 670,
+	MSG_SMS_CLOCK_OUTPUT_CONFIG_RES = 671,
+	MSG_SMS_I2C_SET_FREQ_REQ = 685,
+	MSG_SMS_I2C_SET_FREQ_RES = 686,
+	MSG_SMS_GENERIC_I2C_REQ = 687,
+	MSG_SMS_GENERIC_I2C_RES = 688,
+	MSG_SMS_DVBT_BDA_DATA = 693,
+	MSG_SW_RELOAD_REQ = 697,
+	MSG_SMS_DATA_MSG = 699,
+	MSG_TABLE_UPLOAD_REQ = 700,
+	MSG_TABLE_UPLOAD_RES = 701,
+	MSG_SW_RELOAD_START_REQ = 702,
+	MSG_SW_RELOAD_START_RES = 703,
+	MSG_SW_RELOAD_EXEC_REQ = 704,
+	MSG_SW_RELOAD_EXEC_RES = 705,
+	MSG_SMS_SPI_INT_LINE_SET_REQ = 710,
+	MSG_SMS_SPI_INT_LINE_SET_RES = 711,
+	MSG_SMS_GPIO_CONFIG_EX_REQ = 712,
+	MSG_SMS_GPIO_CONFIG_EX_RES = 713,
+	MSG_SMS_WATCHDOG_ACT_REQ = 716,
+	MSG_SMS_WATCHDOG_ACT_RES = 717,
+	MSG_SMS_LOOPBACK_REQ = 718,
+	MSG_SMS_LOOPBACK_RES = 719,
+	MSG_SMS_RAW_CAPTURE_START_REQ = 720,
+	MSG_SMS_RAW_CAPTURE_START_RES = 721,
+	MSG_SMS_RAW_CAPTURE_ABORT_REQ = 722,
+	MSG_SMS_RAW_CAPTURE_ABORT_RES = 723,
+	MSG_SMS_RAW_CAPTURE_COMPLETE_IND = 728,
+	MSG_SMS_DATA_PUMP_IND = 729,
+	MSG_SMS_DATA_PUMP_REQ = 730,
+	MSG_SMS_DATA_PUMP_RES = 731,
+	MSG_SMS_FLASH_DL_REQ = 732,
+	MSG_SMS_EXEC_TEST_1_REQ = 734,
+	MSG_SMS_EXEC_TEST_1_RES = 735,
+	MSG_SMS_ENBALE_TS_INTERFACE_REQ = 736,
+	MSG_SMS_ENBALE_TS_INTERFACE_RES = 737,
+	MSG_SMS_SPI_SET_BUS_WIDTH_REQ = 738,
+	MSG_SMS_SPI_SET_BUS_WIDTH_RES = 739,
+	MSG_SMS_SEND_EMM_REQ = 740,
+	MSG_SMS_SEND_EMM_RES = 741,
+	MSG_SMS_DISABLE_TS_INTERFACE_REQ = 742,
+	MSG_SMS_DISABLE_TS_INTERFACE_RES = 743,
+	MSG_SMS_IS_BUF_FREE_REQ = 744,
+	MSG_SMS_IS_BUF_FREE_RES = 745,
+	MSG_SMS_EXT_ANTENNA_REQ = 746,
+	MSG_SMS_EXT_ANTENNA_RES = 747,
+	MSG_SMS_CMMB_GET_NET_OF_FREQ_REQ_OBSOLETE = 748,
+	MSG_SMS_CMMB_GET_NET_OF_FREQ_RES_OBSOLETE = 749,
+	MSG_SMS_BATTERY_LEVEL_REQ = 750,
+	MSG_SMS_BATTERY_LEVEL_RES = 751,
+	MSG_SMS_CMMB_INJECT_TABLE_REQ_OBSOLETE = 752,
+	MSG_SMS_CMMB_INJECT_TABLE_RES_OBSOLETE = 753,
+	MSG_SMS_FM_RADIO_BLOCK_IND = 754,
+	MSG_SMS_HOST_NOTIFICATION_IND = 755,
+	MSG_SMS_CMMB_GET_CONTROL_TABLE_REQ_OBSOLETE = 756,
+	MSG_SMS_CMMB_GET_CONTROL_TABLE_RES_OBSOLETE = 757,
+	MSG_SMS_CMMB_GET_NETWORKS_REQ = 760,
+	MSG_SMS_CMMB_GET_NETWORKS_RES = 761,
+	MSG_SMS_CMMB_START_SERVICE_REQ = 762,
+	MSG_SMS_CMMB_START_SERVICE_RES = 763,
+	MSG_SMS_CMMB_STOP_SERVICE_REQ = 764,
+	MSG_SMS_CMMB_STOP_SERVICE_RES = 765,
+	MSG_SMS_CMMB_ADD_CHANNEL_FILTER_REQ = 768,
+	MSG_SMS_CMMB_ADD_CHANNEL_FILTER_RES = 769,
+	MSG_SMS_CMMB_REMOVE_CHANNEL_FILTER_REQ = 770,
+	MSG_SMS_CMMB_REMOVE_CHANNEL_FILTER_RES = 771,
+	MSG_SMS_CMMB_START_CONTROL_INFO_REQ = 772,
+	MSG_SMS_CMMB_START_CONTROL_INFO_RES = 773,
+	MSG_SMS_CMMB_STOP_CONTROL_INFO_REQ = 774,
+	MSG_SMS_CMMB_STOP_CONTROL_INFO_RES = 775,
+	MSG_SMS_ISDBT_TUNE_REQ = 776,
+	MSG_SMS_ISDBT_TUNE_RES = 777,
+	MSG_SMS_TRANSMISSION_IND = 782,
+	MSG_SMS_PID_STATISTICS_IND = 783,
+	MSG_SMS_POWER_DOWN_IND = 784,
+	MSG_SMS_POWER_DOWN_CONF = 785,
+	MSG_SMS_POWER_UP_IND = 786,
+	MSG_SMS_POWER_UP_CONF = 787,
+	MSG_SMS_POWER_MODE_SET_REQ = 790,
+	MSG_SMS_POWER_MODE_SET_RES = 791,
+	MSG_SMS_DEBUG_HOST_EVENT_REQ = 792,
+	MSG_SMS_DEBUG_HOST_EVENT_RES = 793,
+	MSG_SMS_NEW_CRYSTAL_REQ = 794,
+	MSG_SMS_NEW_CRYSTAL_RES = 795,
+	MSG_SMS_CONFIG_SPI_REQ = 796,
+	MSG_SMS_CONFIG_SPI_RES = 797,
+	MSG_SMS_I2C_SHORT_STAT_IND = 798,
+	MSG_SMS_START_IR_REQ = 800,
+	MSG_SMS_START_IR_RES = 801,
+	MSG_SMS_IR_SAMPLES_IND = 802,
+	MSG_SMS_CMMB_CA_SERVICE_IND = 803,
+	MSG_SMS_SLAVE_DEVICE_DETECTED = 804,
+	MSG_SMS_INTERFACE_LOCK_IND = 805,
+	MSG_SMS_INTERFACE_UNLOCK_IND = 806,
+	MSG_SMS_SEND_ROSUM_BUFF_REQ = 810,
+	MSG_SMS_SEND_ROSUM_BUFF_RES = 811,
+	MSG_SMS_ROSUM_BUFF = 812,
+	MSG_SMS_SET_AES128_KEY_REQ = 815,
+	MSG_SMS_SET_AES128_KEY_RES = 816,
+	MSG_SMS_MBBMS_WRITE_REQ = 817,
+	MSG_SMS_MBBMS_WRITE_RES = 818,
+	MSG_SMS_MBBMS_READ_IND = 819,
+	MSG_SMS_IQ_STREAM_START_REQ = 820,
+	MSG_SMS_IQ_STREAM_START_RES = 821,
+	MSG_SMS_IQ_STREAM_STOP_REQ = 822,
+	MSG_SMS_IQ_STREAM_STOP_RES = 823,
+	MSG_SMS_IQ_STREAM_DATA_BLOCK = 824,
+	MSG_SMS_GET_EEPROM_VERSION_REQ = 825,
+	MSG_SMS_GET_EEPROM_VERSION_RES = 826,
+	MSG_SMS_SIGNAL_DETECTED_IND = 827,
+	MSG_SMS_NO_SIGNAL_IND = 828,
+	MSG_SMS_MRC_SHUTDOWN_SLAVE_REQ = 830,
+	MSG_SMS_MRC_SHUTDOWN_SLAVE_RES = 831,
+	MSG_SMS_MRC_BRINGUP_SLAVE_REQ = 832,
+	MSG_SMS_MRC_BRINGUP_SLAVE_RES = 833,
+	MSG_SMS_EXTERNAL_LNA_CTRL_REQ = 834,
+	MSG_SMS_EXTERNAL_LNA_CTRL_RES = 835,
+	MSG_SMS_SET_PERIODIC_STATISTICS_REQ = 836,
+	MSG_SMS_SET_PERIODIC_STATISTICS_RES = 837,
+	MSG_SMS_CMMB_SET_AUTO_OUTPUT_TS0_REQ = 838,
+	MSG_SMS_CMMB_SET_AUTO_OUTPUT_TS0_RES = 839,
+	LOCAL_TUNE = 850,
+	LOCAL_IFFT_H_ICI = 851,
+	MSG_RESYNC_REQ = 852,
+	MSG_SMS_CMMB_GET_MRC_STATISTICS_REQ = 853,
+	MSG_SMS_CMMB_GET_MRC_STATISTICS_RES = 854,
+	MSG_SMS_LOG_EX_ITEM = 855,
+	MSG_SMS_DEVICE_DATA_LOSS_IND = 856,
+	MSG_SMS_MRC_WATCHDOG_TRIGGERED_IND = 857,
+	MSG_SMS_USER_MSG_REQ = 858,
+	MSG_SMS_USER_MSG_RES = 859,
+	MSG_SMS_SMART_CARD_INIT_REQ = 860,
+	MSG_SMS_SMART_CARD_INIT_RES = 861,
+	MSG_SMS_SMART_CARD_WRITE_REQ = 862,
+	MSG_SMS_SMART_CARD_WRITE_RES = 863,
+	MSG_SMS_SMART_CARD_READ_IND = 864,
+	MSG_SMS_TSE_ENABLE_REQ = 866,
+	MSG_SMS_TSE_ENABLE_RES = 867,
+	MSG_SMS_CMMB_GET_SHORT_STATISTICS_REQ = 868,
+	MSG_SMS_CMMB_GET_SHORT_STATISTICS_RES = 869,
+	MSG_SMS_LED_CONFIG_REQ = 870,
+	MSG_SMS_LED_CONFIG_RES = 871,
+	MSG_PWM_ANTENNA_REQ = 872,
+	MSG_PWM_ANTENNA_RES = 873,
+	MSG_SMS_CMMB_SMD_SN_REQ = 874,
+	MSG_SMS_CMMB_SMD_SN_RES = 875,
+	MSG_SMS_CMMB_SET_CA_CW_REQ = 876,
+	MSG_SMS_CMMB_SET_CA_CW_RES = 877,
+	MSG_SMS_CMMB_SET_CA_SALT_REQ = 878,
+	MSG_SMS_CMMB_SET_CA_SALT_RES = 879,
+	MSG_SMS_NSCD_INIT_REQ = 880,
+	MSG_SMS_NSCD_INIT_RES = 881,
+	MSG_SMS_NSCD_PROCESS_SECTION_REQ = 882,
+	MSG_SMS_NSCD_PROCESS_SECTION_RES = 883,
+	MSG_SMS_DBD_CREATE_OBJECT_REQ = 884,
+	MSG_SMS_DBD_CREATE_OBJECT_RES = 885,
+	MSG_SMS_DBD_CONFIGURE_REQ = 886,
+	MSG_SMS_DBD_CONFIGURE_RES = 887,
+	MSG_SMS_DBD_SET_KEYS_REQ = 888,
+	MSG_SMS_DBD_SET_KEYS_RES = 889,
+	MSG_SMS_DBD_PROCESS_HEADER_REQ = 890,
+	MSG_SMS_DBD_PROCESS_HEADER_RES = 891,
+	MSG_SMS_DBD_PROCESS_DATA_REQ = 892,
+	MSG_SMS_DBD_PROCESS_DATA_RES = 893,
+	MSG_SMS_DBD_PROCESS_GET_DATA_REQ = 894,
+	MSG_SMS_DBD_PROCESS_GET_DATA_RES = 895,
+	MSG_SMS_NSCD_OPEN_SESSION_REQ = 896,
+	MSG_SMS_NSCD_OPEN_SESSION_RES = 897,
+	MSG_SMS_SEND_HOST_DATA_TO_DEMUX_REQ = 898,
+	MSG_SMS_SEND_HOST_DATA_TO_DEMUX_RES = 899,
+	MSG_LAST_MSG_TYPE = 900,
+};
 
 #define SMS_INIT_MSG_EX(ptr, type, src, dst, len) do { \
-	(ptr)->msgType = type; (ptr)->msgSrcId = src; (ptr)->msgDstId = dst; \
-	(ptr)->msgLength = len; (ptr)->msgFlags = 0; \
+	(ptr)->msg_type = type; \
+	(ptr)->msg_src_id = src; \
+	(ptr)->msg_dst_id = dst; \
+	(ptr)->msg_length = len; \
+	(ptr)->msg_flags = 0; \
 } while (0)
 
 #define SMS_INIT_MSG(ptr, type, len) \
@@ -277,228 +604,296 @@ enum SMS_DEVICE_MODE {
 	DEVICE_MODE_ISDBT_BDA,
 	DEVICE_MODE_CMMB,
 	DEVICE_MODE_RAW_TUNER,
+	DEVICE_MODE_FM_RADIO,
+	DEVICE_MODE_FM_RADIO_BDA,
+	DEVICE_MODE_ATSC,
 	DEVICE_MODE_MAX,
 };
 
-struct SmsMsgHdr_ST {
-	u16	msgType;
-	u8	msgSrcId;
-	u8	msgDstId;
-	u16	msgLength; /* Length of entire message, including header */
-	u16	msgFlags;
+struct sms_msg_hdr {
+	u16	msg_type;
+	u8	msg_src_id;
+	u8	msg_dst_id;
+	u16	msg_length; /* length of entire message, including header */
+	u16	msg_flags;
+};
+
+struct sms_msg_data {
+	struct sms_msg_hdr x_msg_header;
+	u32 msg_data[1];
 };
 
-struct SmsMsgData_ST {
-	struct SmsMsgHdr_ST xMsgHeader;
-	u32 msgData[1];
+struct sms_msg_data2 {
+	struct sms_msg_hdr x_msg_header;
+	u32 msg_data[2];
 };
 
-struct SmsMsgData_ST2 {
-	struct SmsMsgHdr_ST xMsgHeader;
-	u32 msgData[2];
+struct sms_msg_data4 {
+	struct sms_msg_hdr x_msg_header;
+	u32 msg_data[4];
 };
 
-struct SmsDataDownload_ST {
-	struct SmsMsgHdr_ST	xMsgHeader;
-	u32			MemAddr;
-	u8			Payload[SMS_MAX_PAYLOAD_SIZE];
+struct sms_data_download {
+	struct sms_msg_hdr	x_msg_header;
+	u32			mem_addr;
+	u8			payload[SMS_MAX_PAYLOAD_SIZE];
 };
 
-struct SmsVersionRes_ST {
-	struct SmsMsgHdr_ST	xMsgHeader;
+struct sms_version_res {
+	struct sms_msg_hdr	x_msg_header;
 
-	u16		ChipModel; /* e.g. 0x1102 for SMS-1102 "Nova" */
-	u8		Step; /* 0 - Step A */
-	u8		MetalFix; /* 0 - Metal 0 */
+	u16		chip_model; /* e.g. 0x1102 for SMS-1102 "Nova" */
+	u8		step; /* 0 - step A */
+	u8		metal_fix; /* 0 - Metal 0 */
 
-	/* FirmwareId 0xFF if ROM, otherwise the
+	/* firmware_id 0xFF if ROM, otherwise the
 	 * value indicated by SMSHOSTLIB_DEVICE_MODES_E */
-	u8 FirmwareId;
-	/* SupportedProtocols Bitwise OR combination of
+	u8 firmware_id;
+	/* supported_protocols Bitwise OR combination of
 					     * supported protocols */
-	u8 SupportedProtocols;
+	u8 supported_protocols;
 
-	u8		VersionMajor;
-	u8		VersionMinor;
-	u8		VersionPatch;
-	u8		VersionFieldPatch;
+	u8		version_major;
+	u8		version_minor;
+	u8		version_patch;
+	u8		version_field_patch;
 
-	u8		RomVersionMajor;
-	u8		RomVersionMinor;
-	u8		RomVersionPatch;
-	u8		RomVersionFieldPatch;
+	u8		rom_ver_major;
+	u8		rom_ver_minor;
+	u8		rom_ver_patch;
+	u8		rom_ver_field_patch;
 
 	u8		TextLabel[34];
 };
 
-struct SmsFirmware_ST {
-	u32			CheckSum;
-	u32			Length;
-	u32			StartAddress;
-	u8			Payload[1];
+struct sms_firmware {
+	u32			check_sum;
+	u32			length;
+	u32			start_address;
+	u8			payload[1];
 };
 
-/* Statistics information returned as response for
- * SmsHostApiGetStatistics_Req */
-struct SMSHOSTLIB_STATISTICS_ST {
-	u32 Reserved;		/* Reserved */
+/* statistics information returned as response for
+ * SmsHostApiGetstatistics_Req */
+struct sms_stats {
+	u32 reserved;		/* reserved */
 
 	/* Common parameters */
-	u32 IsRfLocked;		/* 0 - not locked, 1 - locked */
-	u32 IsDemodLocked;	/* 0 - not locked, 1 - locked */
-	u32 IsExternalLNAOn;	/* 0 - external LNA off, 1 - external LNA on */
+	u32 is_rf_locked;		/* 0 - not locked, 1 - locked */
+	u32 is_demod_locked;	/* 0 - not locked, 1 - locked */
+	u32 is_external_lna_on;	/* 0 - external LNA off, 1 - external LNA on */
 
 	/* Reception quality */
 	s32 SNR;		/* dB */
-	u32 BER;		/* Post Viterbi BER [1E-5] */
+	u32 ber;		/* Post Viterbi ber [1E-5] */
 	u32 FIB_CRC;		/* CRC errors percentage, valid only for DAB */
-	u32 TS_PER;		/* Transport stream PER,
+	u32 ts_per;		/* Transport stream PER,
 	0xFFFFFFFF indicate N/A, valid only for DVB-T/H */
 	u32 MFER;		/* DVB-H frame error rate in percentage,
 	0xFFFFFFFF indicate N/A, valid only for DVB-H */
 	s32 RSSI;		/* dBm */
-	s32 InBandPwr;		/* In band power in dBM */
-	s32 CarrierOffset;	/* Carrier Offset in bin/1024 */
+	s32 in_band_pwr;		/* In band power in dBM */
+	s32 carrier_offset;	/* Carrier Offset in bin/1024 */
 
 	/* Transmission parameters */
-	u32 Frequency;		/* Frequency in Hz */
-	u32 Bandwidth;		/* Bandwidth in MHz, valid only for DVB-T/H */
-	u32 TransmissionMode;	/* Transmission Mode, for DAB modes 1-4,
+	u32 frequency;		/* frequency in Hz */
+	u32 bandwidth;		/* bandwidth in MHz, valid only for DVB-T/H */
+	u32 transmission_mode;	/* Transmission Mode, for DAB modes 1-4,
 	for DVB-T/H FFT mode carriers in Kilos */
-	u32 ModemState;		/* from SMSHOSTLIB_DVB_MODEM_STATE_ET,
+	u32 modem_state;		/* from SMSHOSTLIB_DVB_MODEM_STATE_ET,
 	valid only for DVB-T/H */
-	u32 GuardInterval;	/* Guard Interval from
-	SMSHOSTLIB_GUARD_INTERVALS_ET, 	valid only for DVB-T/H */
-	u32 CodeRate;		/* Code Rate from SMSHOSTLIB_CODE_RATE_ET,
+	u32 guard_interval;	/* Guard Interval from
+	SMSHOSTLIB_GUARD_INTERVALS_ET,	valid only for DVB-T/H */
+	u32 code_rate;		/* Code Rate from SMSHOSTLIB_CODE_RATE_ET,
 	valid only for DVB-T/H */
-	u32 LPCodeRate;		/* Low Priority Code Rate from
+	u32 lp_code_rate;		/* Low Priority Code Rate from
 	SMSHOSTLIB_CODE_RATE_ET, valid only for DVB-T/H */
-	u32 Hierarchy;		/* Hierarchy from SMSHOSTLIB_HIERARCHY_ET,
+	u32 hierarchy;		/* hierarchy from SMSHOSTLIB_HIERARCHY_ET,
 	valid only for DVB-T/H */
-	u32 Constellation;	/* Constellation from
+	u32 constellation;	/* constellation from
 	SMSHOSTLIB_CONSTELLATION_ET, valid only for DVB-T/H */
 
 	/* Burst parameters, valid only for DVB-H */
-	u32 BurstSize;		/* Current burst size in bytes,
+	u32 burst_size;		/* Current burst size in bytes,
 	valid only for DVB-H */
-	u32 BurstDuration;	/* Current burst duration in mSec,
+	u32 burst_duration;	/* Current burst duration in mSec,
 	valid only for DVB-H */
-	u32 BurstCycleTime;	/* Current burst cycle time in mSec,
+	u32 burst_cycle_time;	/* Current burst cycle time in mSec,
 	valid only for DVB-H */
-	u32 CalculatedBurstCycleTime;/* Current burst cycle time in mSec,
+	u32 calc_burst_cycle_time;/* Current burst cycle time in mSec,
 	as calculated by demodulator, valid only for DVB-H */
-	u32 NumOfRows;		/* Number of rows in MPE table,
+	u32 num_of_rows;		/* Number of rows in MPE table,
 	valid only for DVB-H */
-	u32 NumOfPaddCols;	/* Number of padding columns in MPE table,
+	u32 num_of_padd_cols;	/* Number of padding columns in MPE table,
 	valid only for DVB-H */
-	u32 NumOfPunctCols;	/* Number of puncturing columns in MPE table,
+	u32 num_of_punct_cols;	/* Number of puncturing columns in MPE table,
 	valid only for DVB-H */
-	u32 ErrorTSPackets;	/* Number of erroneous
+	u32 error_ts_packets;	/* Number of erroneous
 	transport-stream packets */
-	u32 TotalTSPackets;	/* Total number of transport-stream packets */
-	u32 NumOfValidMpeTlbs;	/* Number of MPE tables which do not include
+	u32 total_ts_packets;	/* Total number of transport-stream packets */
+	u32 num_of_valid_mpe_tlbs;	/* Number of MPE tables which do not include
 	errors after MPE RS decoding */
-	u32 NumOfInvalidMpeTlbs;/* Number of MPE tables which include errors
+	u32 num_of_invalid_mpe_tlbs;/* Number of MPE tables which include errors
 	after MPE RS decoding */
-	u32 NumOfCorrectedMpeTlbs;/* Number of MPE tables which were
+	u32 num_of_corrected_mpe_tlbs;/* Number of MPE tables which were
 	corrected by MPE RS decoding */
 	/* Common params */
-	u32 BERErrorCount;	/* Number of errornous SYNC bits. */
-	u32 BERBitCount;	/* Total number of SYNC bits. */
+	u32 ber_error_count;	/* Number of errornous SYNC bits. */
+	u32 ber_bit_count;	/* Total number of SYNC bits. */
 
 	/* Interface information */
-	u32 SmsToHostTxErrors;	/* Total number of transmission errors. */
+	u32 sms_to_host_tx_errors;	/* Total number of transmission errors. */
 
 	/* DAB/T-DMB */
-	u32 PreBER; 		/* DAB/T-DMB only: Pre Viterbi BER [1E-5] */
+	u32 pre_ber;		/* DAB/T-DMB only: Pre Viterbi ber [1E-5] */
 
 	/* DVB-H TPS parameters */
-	u32 CellId;		/* TPS Cell ID in bits 15..0, bits 31..16 zero;
+	u32 cell_id;		/* TPS Cell ID in bits 15..0, bits 31..16 zero;
 	 if set to 0xFFFFFFFF cell_id not yet recovered */
-	u32 DvbhSrvIndHP;	/* DVB-H service indication info, bit 1 -
+	u32 dvbh_srv_ind_hp;	/* DVB-H service indication info, bit 1 -
 	Time Slicing indicator, bit 0 - MPE-FEC indicator */
-	u32 DvbhSrvIndLP;	/* DVB-H service indication info, bit 1 -
+	u32 dvbh_srv_ind_lp;	/* DVB-H service indication info, bit 1 -
 	Time Slicing indicator, bit 0 - MPE-FEC indicator */
 
-	u32 NumMPEReceived;	/* DVB-H, Num MPE section received */
+	u32 num_mpe_received;	/* DVB-H, Num MPE section received */
 
-	u32 ReservedFields[10];	/* Reserved */
+	u32 reservedFields[10];	/* reserved */
 };
 
-struct SmsMsgStatisticsInfo_ST {
-	u32 RequestResult;
+struct sms_msg_statistics_info {
+	u32 request_result;
 
-	struct SMSHOSTLIB_STATISTICS_ST Stat;
+	struct sms_stats stat;
 
 	/* Split the calc of the SNR in DAB */
-	u32 Signal; /* dB */
-	u32 Noise; /* dB */
+	u32 signal; /* dB */
+	u32 noise; /* dB */
 
 };
 
-struct SMSHOSTLIB_ISDBT_LAYER_STAT_ST {
+struct sms_isdbt_layer_stats {
 	/* Per-layer information */
-	u32 CodeRate; /* Code Rate from SMSHOSTLIB_CODE_RATE_ET,
+	u32 code_rate; /* Code Rate from SMSHOSTLIB_CODE_RATE_ET,
 		       * 255 means layer does not exist */
-	u32 Constellation; /* Constellation from SMSHOSTLIB_CONSTELLATION_ET,
+	u32 constellation; /* constellation from SMSHOSTLIB_CONSTELLATION_ET,
 			    * 255 means layer does not exist */
-	u32 BER; /* Post Viterbi BER [1E-5], 0xFFFFFFFF indicate N/A */
-	u32 BERErrorCount; /* Post Viterbi Error Bits Count */
-	u32 BERBitCount; /* Post Viterbi Total Bits Count */
-	u32 PreBER; /* Pre Viterbi BER [1E-5], 0xFFFFFFFF indicate N/A */
-	u32 TS_PER; /* Transport stream PER [%], 0xFFFFFFFF indicate N/A */
-	u32 ErrorTSPackets; /* Number of erroneous transport-stream packets */
-	u32 TotalTSPackets; /* Total number of transport-stream packets */
-	u32 TILdepthI; /* Time interleaver depth I parameter,
+	u32 ber; /* Post Viterbi ber [1E-5], 0xFFFFFFFF indicate N/A */
+	u32 ber_error_count; /* Post Viterbi Error Bits Count */
+	u32 ber_bit_count; /* Post Viterbi Total Bits Count */
+	u32 pre_ber; /* Pre Viterbi ber [1E-5], 0xFFFFFFFF indicate N/A */
+	u32 ts_per; /* Transport stream PER [%], 0xFFFFFFFF indicate N/A */
+	u32 error_ts_packets; /* Number of erroneous transport-stream packets */
+	u32 total_ts_packets; /* Total number of transport-stream packets */
+	u32 ti_ldepth_i; /* Time interleaver depth I parameter,
 			* 255 means layer does not exist */
-	u32 NumberOfSegments; /* Number of segments in layer A,
+	u32 number_of_segments; /* Number of segments in layer A,
 			       * 255 means layer does not exist */
-	u32 TMCCErrors; /* TMCC errors */
+	u32 tmcc_errors; /* TMCC errors */
 };
 
-struct SMSHOSTLIB_STATISTICS_ISDBT_ST {
-	u32 StatisticsType; /* Enumerator identifying the type of the
+struct sms_isdbt_stats {
+	u32 statistics_type; /* Enumerator identifying the type of the
 				* structure.  Values are the same as
 				* SMSHOSTLIB_DEVICE_MODES_E
 				*
 				* This field MUST always be first in any
 				* statistics structure */
 
-	u32 FullSize; /* Total size of the structure returned by the modem.
+	u32 full_size; /* Total size of the structure returned by the modem.
 		       * If the size requested by the host is smaller than
-		       * FullSize, the struct will be truncated */
+		       * full_size, the struct will be truncated */
 
 	/* Common parameters */
-	u32 IsRfLocked; /* 0 - not locked, 1 - locked */
-	u32 IsDemodLocked; /* 0 - not locked, 1 - locked */
-	u32 IsExternalLNAOn; /* 0 - external LNA off, 1 - external LNA on */
+	u32 is_rf_locked; /* 0 - not locked, 1 - locked */
+	u32 is_demod_locked; /* 0 - not locked, 1 - locked */
+	u32 is_external_lna_on; /* 0 - external LNA off, 1 - external LNA on */
 
 	/* Reception quality */
 	s32  SNR; /* dB */
 	s32  RSSI; /* dBm */
-	s32  InBandPwr; /* In band power in dBM */
-	s32  CarrierOffset; /* Carrier Offset in Hz */
+	s32  in_band_pwr; /* In band power in dBM */
+	s32  carrier_offset; /* Carrier Offset in Hz */
 
 	/* Transmission parameters */
-	u32 Frequency; /* Frequency in Hz */
-	u32 Bandwidth; /* Bandwidth in MHz */
-	u32 TransmissionMode; /* ISDB-T transmission mode */
-	u32 ModemState; /* 0 - Acquisition, 1 - Locked */
-	u32 GuardInterval; /* Guard Interval, 1 divided by value */
-	u32 SystemType; /* ISDB-T system type (ISDB-T / ISDB-Tsb) */
-	u32 PartialReception; /* TRUE - partial reception, FALSE otherwise */
-	u32 NumOfLayers; /* Number of ISDB-T layers in the network */
+	u32 frequency; /* frequency in Hz */
+	u32 bandwidth; /* bandwidth in MHz */
+	u32 transmission_mode; /* ISDB-T transmission mode */
+	u32 modem_state; /* 0 - Acquisition, 1 - Locked */
+	u32 guard_interval; /* Guard Interval, 1 divided by value */
+	u32 system_type; /* ISDB-T system type (ISDB-T / ISDB-Tsb) */
+	u32 partial_reception; /* TRUE - partial reception, FALSE otherwise */
+	u32 num_of_layers; /* Number of ISDB-T layers in the network */
 
 	/* Per-layer information */
 	/* Layers A, B and C */
-	struct SMSHOSTLIB_ISDBT_LAYER_STAT_ST	LayerInfo[3];
-	/* Per-layer statistics, see SMSHOSTLIB_ISDBT_LAYER_STAT_ST */
+	struct sms_isdbt_layer_stats	layer_info[3];
+	/* Per-layer statistics, see sms_isdbt_layer_stats */
 
 	/* Interface information */
-	u32 SmsToHostTxErrors; /* Total number of transmission errors. */
+	u32 sms_to_host_tx_errors; /* Total number of transmission errors. */
 };
 
-struct PID_STATISTICS_DATA_S {
+struct sms_isdbt_stats_ex {
+	u32 statistics_type; /* Enumerator identifying the type of the
+				* structure.  Values are the same as
+				* SMSHOSTLIB_DEVICE_MODES_E
+				*
+				* This field MUST always be first in any
+				* statistics structure */
+
+	u32 full_size; /* Total size of the structure returned by the modem.
+		       * If the size requested by the host is smaller than
+		       * full_size, the struct will be truncated */
+
+	/* Common parameters */
+	u32 is_rf_locked; /* 0 - not locked, 1 - locked */
+	u32 is_demod_locked; /* 0 - not locked, 1 - locked */
+	u32 is_external_lna_on; /* 0 - external LNA off, 1 - external LNA on */
+
+	/* Reception quality */
+	s32  SNR; /* dB */
+	s32  RSSI; /* dBm */
+	s32  in_band_pwr; /* In band power in dBM */
+	s32  carrier_offset; /* Carrier Offset in Hz */
+
+	/* Transmission parameters */
+	u32 frequency; /* frequency in Hz */
+	u32 bandwidth; /* bandwidth in MHz */
+	u32 transmission_mode; /* ISDB-T transmission mode */
+	u32 modem_state; /* 0 - Acquisition, 1 - Locked */
+	u32 guard_interval; /* Guard Interval, 1 divided by value */
+	u32 system_type; /* ISDB-T system type (ISDB-T / ISDB-Tsb) */
+	u32 partial_reception; /* TRUE - partial reception, FALSE otherwise */
+	u32 num_of_layers; /* Number of ISDB-T layers in the network */
+
+	u32 segment_number; /* Segment number for ISDB-Tsb */
+	u32 tune_bw;	   /* Tuned bandwidth - BW_ISDBT_1SEG / BW_ISDBT_3SEG */
+
+	/* Per-layer information */
+	/* Layers A, B and C */
+	struct sms_isdbt_layer_stats	layer_info[3];
+	/* Per-layer statistics, see sms_isdbt_layer_stats */
+
+	/* Interface information */
+	u32 reserved1;    /* Was sms_to_host_tx_errors - obsolete . */
+ /* Proprietary information */
+	u32 ext_antenna;    /* Obsolete field. */
+	u32 reception_quality;
+	u32 ews_alert_active;   /* signals if EWS alert is currently on */
+	u32 lna_on_off;	/* Internal LNA state: 0: OFF, 1: ON */
+
+	u32 rf_agc_level;	 /* RF AGC Level [linear units], full gain = 65535 (20dB) */
+	u32 bb_agc_level;    /* Baseband AGC level [linear units], full gain = 65535 (71.5dB) */
+	u32 fw_errors_counter;   /* Application errors - should be always zero */
+	u8 FwErrorsHistoryArr[8]; /* Last FW errors IDs - first is most recent, last is oldest */
+
+	s32  MRC_SNR;     /* dB */
+	u32 snr_full_res;    /* dB x 65536 */
+	u32 reserved4[4];
+};
+
+
+struct sms_pid_stats_data {
 	struct PID_BURST_S {
 		u32 size;
 		u32 padding_cols;
@@ -513,112 +908,155 @@ struct PID_STATISTICS_DATA_S {
 	u32 tot_cor_tbl;
 };
 
-struct PID_DATA_S {
+struct sms_pid_data {
 	u32 pid;
 	u32 num_rows;
-	struct PID_STATISTICS_DATA_S pid_statistics;
+	struct sms_pid_stats_data pid_statistics;
 };
 
 #define CORRECT_STAT_RSSI(_stat) ((_stat).RSSI *= -1)
-#define CORRECT_STAT_BANDWIDTH(_stat) (_stat.Bandwidth = 8 - _stat.Bandwidth)
+#define CORRECT_STAT_BANDWIDTH(_stat) (_stat.bandwidth = 8 - _stat.bandwidth)
 #define CORRECT_STAT_TRANSMISSON_MODE(_stat) \
-	if (_stat.TransmissionMode == 0) \
-		_stat.TransmissionMode = 2; \
-	else if (_stat.TransmissionMode == 1) \
-		_stat.TransmissionMode = 8; \
+	if (_stat.transmission_mode == 0) \
+		_stat.transmission_mode = 2; \
+	else if (_stat.transmission_mode == 1) \
+		_stat.transmission_mode = 8; \
 		else \
-			_stat.TransmissionMode = 4;
+			_stat.transmission_mode = 4;
 
-struct TRANSMISSION_STATISTICS_S {
-	u32 Frequency;		/* Frequency in Hz */
-	u32 Bandwidth;		/* Bandwidth in MHz */
-	u32 TransmissionMode;	/* FFT mode carriers in Kilos */
-	u32 GuardInterval;	/* Guard Interval from
+struct sms_tx_stats {
+	u32 frequency;		/* frequency in Hz */
+	u32 bandwidth;		/* bandwidth in MHz */
+	u32 transmission_mode;	/* FFT mode carriers in Kilos */
+	u32 guard_interval;	/* Guard Interval from
 	SMSHOSTLIB_GUARD_INTERVALS_ET */
-	u32 CodeRate;		/* Code Rate from SMSHOSTLIB_CODE_RATE_ET */
-	u32 LPCodeRate;		/* Low Priority Code Rate from
+	u32 code_rate;		/* Code Rate from SMSHOSTLIB_CODE_RATE_ET */
+	u32 lp_code_rate;		/* Low Priority Code Rate from
 	SMSHOSTLIB_CODE_RATE_ET */
-	u32 Hierarchy;		/* Hierarchy from SMSHOSTLIB_HIERARCHY_ET */
-	u32 Constellation;	/* Constellation from
+	u32 hierarchy;		/* hierarchy from SMSHOSTLIB_HIERARCHY_ET */
+	u32 constellation;	/* constellation from
 	SMSHOSTLIB_CONSTELLATION_ET */
 
 	/* DVB-H TPS parameters */
-	u32 CellId;		/* TPS Cell ID in bits 15..0, bits 31..16 zero;
+	u32 cell_id;		/* TPS Cell ID in bits 15..0, bits 31..16 zero;
 	 if set to 0xFFFFFFFF cell_id not yet recovered */
-	u32 DvbhSrvIndHP;	/* DVB-H service indication info, bit 1 -
+	u32 dvbh_srv_ind_hp;	/* DVB-H service indication info, bit 1 -
 	 Time Slicing indicator, bit 0 - MPE-FEC indicator */
-	u32 DvbhSrvIndLP;	/* DVB-H service indication info, bit 1 -
+	u32 dvbh_srv_ind_lp;	/* DVB-H service indication info, bit 1 -
 	 Time Slicing indicator, bit 0 - MPE-FEC indicator */
-	u32 IsDemodLocked;	/* 0 - not locked, 1 - locked */
+	u32 is_demod_locked;	/* 0 - not locked, 1 - locked */
+};
+
+struct sms_rx_stats {
+	u32 is_rf_locked;		/* 0 - not locked, 1 - locked */
+	u32 is_demod_locked;	/* 0 - not locked, 1 - locked */
+	u32 is_external_lna_on;	/* 0 - external LNA off, 1 - external LNA on */
+
+	u32 modem_state;		/* from SMSHOSTLIB_DVB_MODEM_STATE_ET */
+	s32 SNR;		/* dB */
+	u32 ber;		/* Post Viterbi ber [1E-5] */
+	u32 ber_error_count;	/* Number of erronous SYNC bits. */
+	u32 ber_bit_count;	/* Total number of SYNC bits. */
+	u32 ts_per;		/* Transport stream PER,
+	0xFFFFFFFF indicate N/A */
+	u32 MFER;		/* DVB-H frame error rate in percentage,
+	0xFFFFFFFF indicate N/A, valid only for DVB-H */
+	s32 RSSI;		/* dBm */
+	s32 in_band_pwr;		/* In band power in dBM */
+	s32 carrier_offset;	/* Carrier Offset in bin/1024 */
+	u32 error_ts_packets;	/* Number of erroneous
+	transport-stream packets */
+	u32 total_ts_packets;	/* Total number of transport-stream packets */
+
+	s32 MRC_SNR;		/* dB */
+	s32 MRC_RSSI;		/* dBm */
+	s32 mrc_in_band_pwr;	/* In band power in dBM */
 };
 
-struct RECEPTION_STATISTICS_S {
-	u32 IsRfLocked;		/* 0 - not locked, 1 - locked */
-	u32 IsDemodLocked;	/* 0 - not locked, 1 - locked */
-	u32 IsExternalLNAOn;	/* 0 - external LNA off, 1 - external LNA on */
+struct sms_rx_stats_ex {
+	u32 is_rf_locked;		/* 0 - not locked, 1 - locked */
+	u32 is_demod_locked;	/* 0 - not locked, 1 - locked */
+	u32 is_external_lna_on;	/* 0 - external LNA off, 1 - external LNA on */
 
-	u32 ModemState;		/* from SMSHOSTLIB_DVB_MODEM_STATE_ET */
+	u32 modem_state;		/* from SMSHOSTLIB_DVB_MODEM_STATE_ET */
 	s32 SNR;		/* dB */
-	u32 BER;		/* Post Viterbi BER [1E-5] */
-	u32 BERErrorCount;	/* Number of erronous SYNC bits. */
-	u32 BERBitCount;	/* Total number of SYNC bits. */
-	u32 TS_PER;		/* Transport stream PER,
+	u32 ber;		/* Post Viterbi ber [1E-5] */
+	u32 ber_error_count;	/* Number of erronous SYNC bits. */
+	u32 ber_bit_count;	/* Total number of SYNC bits. */
+	u32 ts_per;		/* Transport stream PER,
 	0xFFFFFFFF indicate N/A */
 	u32 MFER;		/* DVB-H frame error rate in percentage,
 	0xFFFFFFFF indicate N/A, valid only for DVB-H */
 	s32 RSSI;		/* dBm */
-	s32 InBandPwr;		/* In band power in dBM */
-	s32 CarrierOffset;	/* Carrier Offset in bin/1024 */
-	u32 ErrorTSPackets;	/* Number of erroneous
+	s32 in_band_pwr;		/* In band power in dBM */
+	s32 carrier_offset;	/* Carrier Offset in bin/1024 */
+	u32 error_ts_packets;	/* Number of erroneous
 	transport-stream packets */
-	u32 TotalTSPackets;	/* Total number of transport-stream packets */
+	u32 total_ts_packets;	/* Total number of transport-stream packets */
+
+	s32  ref_dev_ppm;
+	s32  freq_dev_hz;
 
 	s32 MRC_SNR;		/* dB */
 	s32 MRC_RSSI;		/* dBm */
-	s32 MRC_InBandPwr;	/* In band power in dBM */
+	s32 mrc_in_band_pwr;	/* In band power in dBM */
 };
 
 
-/* Statistics information returned as response for
- * SmsHostApiGetStatisticsEx_Req for DVB applications, SMS1100 and up */
-struct SMSHOSTLIB_STATISTICS_DVB_S {
+/* statistics information returned as response for
+ * SmsHostApiGetstatisticsEx_Req for DVB applications, SMS1100 and up */
+struct sms_stats_dvb {
 	/* Reception */
-	struct RECEPTION_STATISTICS_S ReceptionData;
+	struct sms_rx_stats reception_data;
 
 	/* Transmission parameters */
-	struct TRANSMISSION_STATISTICS_S TransmissionData;
+	struct sms_tx_stats transmission_data;
 
 	/* Burst parameters, valid only for DVB-H */
 #define	SRVM_MAX_PID_FILTERS 8
-	struct PID_DATA_S PidData[SRVM_MAX_PID_FILTERS];
+	struct sms_pid_data pid_data[SRVM_MAX_PID_FILTERS];
 };
 
-struct SRVM_SIGNAL_STATUS_S {
+/* statistics information returned as response for
+ * SmsHostApiGetstatisticsEx_Req for DVB applications, SMS1100 and up */
+struct sms_stats_dvb_ex {
+	/* Reception */
+	struct sms_rx_stats_ex reception_data;
+
+	/* Transmission parameters */
+	struct sms_tx_stats transmission_data;
+
+	/* Burst parameters, valid only for DVB-H */
+#define	SRVM_MAX_PID_FILTERS 8
+	struct sms_pid_data pid_data[SRVM_MAX_PID_FILTERS];
+};
+
+struct sms_srvm_signal_status {
 	u32 result;
 	u32 snr;
-	u32 tsPackets;
-	u32 etsPackets;
+	u32 ts_packets;
+	u32 ets_packets;
 	u32 constellation;
-	u32 hpCode;
-	u32 tpsSrvIndLP;
-	u32 tpsSrvIndHP;
-	u32 cellId;
+	u32 hp_code;
+	u32 tps_srv_ind_lp;
+	u32 tps_srv_ind_hp;
+	u32 cell_id;
 	u32 reason;
 
-	s32 inBandPower;
-	u32 requestId;
+	s32 in_band_power;
+	u32 request_id;
 };
 
-struct SMSHOSTLIB_I2C_REQ_ST {
-	u32	DeviceAddress; /* I2c device address */
-	u32	WriteCount; /* number of bytes to write */
-	u32	ReadCount; /* number of bytes to read */
+struct sms_i2c_req {
+	u32	device_address; /* I2c device address */
+	u32	write_count; /* number of bytes to write */
+	u32	read_count; /* number of bytes to read */
 	u8	Data[1];
 };
 
-struct SMSHOSTLIB_I2C_RES_ST {
-	u32	Status; /* non-zero value in case of failure */
-	u32	ReadCount; /* number of bytes read */
+struct sms_i2c_res {
+	u32	status; /* non-zero value in case of failure */
+	u32	read_count; /* number of bytes read */
 	u8	Data[1];
 };
 
@@ -638,59 +1076,39 @@ struct smscore_config_gpio {
 #define SMS_GPIO_INPUTCHARACTERISTICS_SCHMITT 1
 	u8 inputcharacteristics;
 
-#define SMS_GPIO_OUTPUTSLEWRATE_FAST 0
-#define SMS_GPIO_OUTPUTSLEWRATE_SLOW 1
-	u8 outputslewrate;
-
-#define SMS_GPIO_OUTPUTDRIVING_4mA  0
-#define SMS_GPIO_OUTPUTDRIVING_8mA  1
-#define SMS_GPIO_OUTPUTDRIVING_12mA 2
-#define SMS_GPIO_OUTPUTDRIVING_16mA 3
-	u8 outputdriving;
-};
-
-struct smscore_gpio_config {
-#define SMS_GPIO_DIRECTION_INPUT  0
-#define SMS_GPIO_DIRECTION_OUTPUT 1
-	u8 Direction;
-
-#define SMS_GPIO_PULL_UP_DOWN_NONE     0
-#define SMS_GPIO_PULL_UP_DOWN_PULLDOWN 1
-#define SMS_GPIO_PULL_UP_DOWN_PULLUP   2
-#define SMS_GPIO_PULL_UP_DOWN_KEEPER   3
-	u8 PullUpDown;
-
-#define SMS_GPIO_INPUT_CHARACTERISTICS_NORMAL  0
-#define SMS_GPIO_INPUT_CHARACTERISTICS_SCHMITT 1
-	u8 InputCharacteristics;
-
-#define SMS_GPIO_OUTPUT_SLEW_RATE_SLOW		1 /* 10xx */
-#define SMS_GPIO_OUTPUT_SLEW_RATE_FAST		0 /* 10xx */
+	/* 10xx */
+#define SMS_GPIO_OUTPUT_SLEW_RATE_FAST 0
+#define SMS_GPIO_OUTPUT_SLEW_WRATE_SLOW 1
 
+	/* 11xx */
+#define SMS_GPIO_OUTPUT_SLEW_RATE_0_45_V_NS	0
+#define SMS_GPIO_OUTPUT_SLEW_RATE_0_9_V_NS	1
+#define SMS_GPIO_OUTPUT_SLEW_RATE_1_7_V_NS	2
+#define SMS_GPIO_OUTPUT_SLEW_RATE_3_3_V_NS	3
 
-#define SMS_GPIO_OUTPUT_SLEW_RATE_0_45_V_NS	0 /* 11xx */
-#define SMS_GPIO_OUTPUT_SLEW_RATE_0_9_V_NS	1 /* 11xx */
-#define SMS_GPIO_OUTPUT_SLEW_RATE_1_7_V_NS	2 /* 11xx */
-#define SMS_GPIO_OUTPUT_SLEW_RATE_3_3_V_NS	3 /* 11xx */
-	u8 OutputSlewRate;
+	u8 outputslewrate;
 
-#define SMS_GPIO_OUTPUT_DRIVING_S_4mA		0 /* 10xx */
-#define SMS_GPIO_OUTPUT_DRIVING_S_8mA		1 /* 10xx */
-#define SMS_GPIO_OUTPUT_DRIVING_S_12mA		2 /* 10xx */
-#define SMS_GPIO_OUTPUT_DRIVING_S_16mA		3 /* 10xx */
+	/* 10xx */
+#define SMS_GPIO_OUTPUTDRIVING_S_4mA  0
+#define SMS_GPIO_OUTPUTDRIVING_S_8mA  1
+#define SMS_GPIO_OUTPUTDRIVING_S_12mA 2
+#define SMS_GPIO_OUTPUTDRIVING_S_16mA 3
+
+	/* 11xx*/
+#define SMS_GPIO_OUTPUTDRIVING_1_5mA	0
+#define SMS_GPIO_OUTPUTDRIVING_2_8mA	1
+#define SMS_GPIO_OUTPUTDRIVING_4mA	2
+#define SMS_GPIO_OUTPUTDRIVING_7mA	3
+#define SMS_GPIO_OUTPUTDRIVING_10mA	4
+#define SMS_GPIO_OUTPUTDRIVING_11mA	5
+#define SMS_GPIO_OUTPUTDRIVING_14mA	6
+#define SMS_GPIO_OUTPUTDRIVING_16mA	7
 
-#define SMS_GPIO_OUTPUT_DRIVING_1_5mA		0 /* 11xx */
-#define SMS_GPIO_OUTPUT_DRIVING_2_8mA		1 /* 11xx */
-#define SMS_GPIO_OUTPUT_DRIVING_4mA		2 /* 11xx */
-#define SMS_GPIO_OUTPUT_DRIVING_7mA		3 /* 11xx */
-#define SMS_GPIO_OUTPUT_DRIVING_10mA		4 /* 11xx */
-#define SMS_GPIO_OUTPUT_DRIVING_11mA		5 /* 11xx */
-#define SMS_GPIO_OUTPUT_DRIVING_14mA		6 /* 11xx */
-#define SMS_GPIO_OUTPUT_DRIVING_16mA		7 /* 11xx */
-	u8 OutputDriving;
+	u8 outputdriving;
 };
 
-extern void smscore_registry_setmode(char *devpath, int mode);
+char *smscore_translate_msg(enum msg_types msgtype);
+
 extern int smscore_registry_getmode(char *devpath);
 
 extern int smscore_register_hotplug(hotplug_t hotplug);
@@ -721,8 +1139,6 @@ extern void smscore_onresponse(struct smscore_device_t *coredev,
 extern int smscore_get_common_buffer_size(struct smscore_device_t *coredev);
 extern int smscore_map_common_buffer(struct smscore_device_t *coredev,
 				      struct vm_area_struct *vma);
-extern int smscore_get_fw_filename(struct smscore_device_t *coredev,
-				   int mode, char *filename);
 extern int smscore_send_fw_file(struct smscore_device_t *coredev,
 				u8 *ufwbuf, int size);
 
@@ -737,11 +1153,11 @@ int smscore_configure_gpio(struct smscore_device_t *coredev, u32 pin,
 int smscore_set_gpio(struct smscore_device_t *coredev, u32 pin, int level);
 
 /* new GPIO management */
-extern int smscore_gpio_configure(struct smscore_device_t *coredev, u8 PinNum,
-		struct smscore_gpio_config *pGpioConfig);
-extern int smscore_gpio_set_level(struct smscore_device_t *coredev, u8 PinNum,
-		u8 NewLevel);
-extern int smscore_gpio_get_level(struct smscore_device_t *coredev, u8 PinNum,
+extern int smscore_gpio_configure(struct smscore_device_t *coredev, u8 pin_num,
+		struct smscore_config_gpio *p_gpio_config);
+extern int smscore_gpio_set_level(struct smscore_device_t *coredev, u8 pin_num,
+		u8 new_level);
+extern int smscore_gpio_get_level(struct smscore_device_t *coredev, u8 pin_num,
 		u8 *level);
 
 void smscore_set_board_id(struct smscore_device_t *core, int id);
@@ -760,7 +1176,8 @@ int smscore_led_state(struct smscore_device_t *core, int led);
 
 #define dprintk(kern, lvl, fmt, arg...) do {\
 	if (sms_dbg & lvl) \
-		sms_printk(kern, fmt, ##arg); } while (0)
+		sms_printk(kern, fmt, ##arg); \
+} while (0)
 
 #define sms_log(fmt, arg...) sms_printk(KERN_INFO, fmt, ##arg)
 #define sms_err(fmt, arg...) \
diff --git a/drivers/media/common/siano/smsdvb-debugfs.c b/drivers/media/common/siano/smsdvb-debugfs.c
new file mode 100644
index 00000000000..0bb4430535f
--- /dev/null
+++ b/drivers/media/common/siano/smsdvb-debugfs.c
@@ -0,0 +1,551 @@
+/***********************************************************************
+ *
+ * Copyright(c) 2013 Mauro Carvalho Chehab <mchehab@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, see <http://www.gnu.org/licenses/>.
+ *
+ ***********************************************************************/
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/debugfs.h>
+#include <linux/spinlock.h>
+#include <linux/usb.h>
+
+#include "dmxdev.h"
+#include "dvbdev.h"
+#include "dvb_demux.h"
+#include "dvb_frontend.h"
+
+#include "smscoreapi.h"
+
+#include "smsdvb.h"
+
+static struct dentry *smsdvb_debugfs_usb_root;
+
+struct smsdvb_debugfs {
+	struct kref		refcount;
+	spinlock_t		lock;
+
+	char			stats_data[PAGE_SIZE];
+	unsigned		stats_count;
+	bool			stats_was_read;
+
+	wait_queue_head_t	stats_queue;
+};
+
+static void smsdvb_print_dvb_stats(struct smsdvb_debugfs *debug_data,
+			    struct sms_stats *p)
+{
+	int n = 0;
+	char *buf;
+
+	spin_lock(&debug_data->lock);
+	if (debug_data->stats_count) {
+		spin_unlock(&debug_data->lock);
+		return;
+	}
+
+	buf = debug_data->stats_data;
+
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "is_rf_locked = %d\n", p->is_rf_locked);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "is_demod_locked = %d\n", p->is_demod_locked);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "is_external_lna_on = %d\n", p->is_external_lna_on);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "SNR = %d\n", p->SNR);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "ber = %d\n", p->ber);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "FIB_CRC = %d\n", p->FIB_CRC);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "ts_per = %d\n", p->ts_per);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "MFER = %d\n", p->MFER);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "RSSI = %d\n", p->RSSI);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "in_band_pwr = %d\n", p->in_band_pwr);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "carrier_offset = %d\n", p->carrier_offset);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "modem_state = %d\n", p->modem_state);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "frequency = %d\n", p->frequency);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "bandwidth = %d\n", p->bandwidth);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "transmission_mode = %d\n", p->transmission_mode);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "modem_state = %d\n", p->modem_state);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "guard_interval = %d\n", p->guard_interval);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "code_rate = %d\n", p->code_rate);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "lp_code_rate = %d\n", p->lp_code_rate);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "hierarchy = %d\n", p->hierarchy);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "constellation = %d\n", p->constellation);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "burst_size = %d\n", p->burst_size);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "burst_duration = %d\n", p->burst_duration);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "burst_cycle_time = %d\n", p->burst_cycle_time);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "calc_burst_cycle_time = %d\n",
+		      p->calc_burst_cycle_time);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "num_of_rows = %d\n", p->num_of_rows);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "num_of_padd_cols = %d\n", p->num_of_padd_cols);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "num_of_punct_cols = %d\n", p->num_of_punct_cols);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "error_ts_packets = %d\n", p->error_ts_packets);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "total_ts_packets = %d\n", p->total_ts_packets);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "num_of_valid_mpe_tlbs = %d\n", p->num_of_valid_mpe_tlbs);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "num_of_invalid_mpe_tlbs = %d\n", p->num_of_invalid_mpe_tlbs);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "num_of_corrected_mpe_tlbs = %d\n", p->num_of_corrected_mpe_tlbs);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "ber_error_count = %d\n", p->ber_error_count);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "ber_bit_count = %d\n", p->ber_bit_count);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "sms_to_host_tx_errors = %d\n", p->sms_to_host_tx_errors);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "pre_ber = %d\n", p->pre_ber);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "cell_id = %d\n", p->cell_id);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "dvbh_srv_ind_hp = %d\n", p->dvbh_srv_ind_hp);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "dvbh_srv_ind_lp = %d\n", p->dvbh_srv_ind_lp);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "num_mpe_received = %d\n", p->num_mpe_received);
+
+	debug_data->stats_count = n;
+	spin_unlock(&debug_data->lock);
+	wake_up(&debug_data->stats_queue);
+}
+
+static void smsdvb_print_isdb_stats(struct smsdvb_debugfs *debug_data,
+			     struct sms_isdbt_stats *p)
+{
+	int i, n = 0;
+	char *buf;
+
+	spin_lock(&debug_data->lock);
+	if (debug_data->stats_count) {
+		spin_unlock(&debug_data->lock);
+		return;
+	}
+
+	buf = debug_data->stats_data;
+
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "statistics_type = %d\t", p->statistics_type);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "full_size = %d\n", p->full_size);
+
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "is_rf_locked = %d\t\t", p->is_rf_locked);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "is_demod_locked = %d\t", p->is_demod_locked);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "is_external_lna_on = %d\n", p->is_external_lna_on);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "SNR = %d dB\t\t", p->SNR);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "RSSI = %d dBm\t\t", p->RSSI);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "in_band_pwr = %d dBm\n", p->in_band_pwr);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "carrier_offset = %d\t", p->carrier_offset);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "bandwidth = %d\t\t", p->bandwidth);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "frequency = %d Hz\n", p->frequency);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "transmission_mode = %d\t", p->transmission_mode);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "modem_state = %d\t\t", p->modem_state);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "guard_interval = %d\n", p->guard_interval);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "system_type = %d\t\t", p->system_type);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "partial_reception = %d\t", p->partial_reception);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "num_of_layers = %d\n", p->num_of_layers);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "sms_to_host_tx_errors = %d\n", p->sms_to_host_tx_errors);
+
+	for (i = 0; i < 3; i++) {
+		if (p->layer_info[i].number_of_segments < 1 ||
+		    p->layer_info[i].number_of_segments > 13)
+			continue;
+
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\nLayer %d\n", i);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\tcode_rate = %d\t",
+			      p->layer_info[i].code_rate);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "constellation = %d\n",
+			      p->layer_info[i].constellation);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\tber = %-5d\t",
+			      p->layer_info[i].ber);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\tber_error_count = %-5d\t",
+			      p->layer_info[i].ber_error_count);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "ber_bit_count = %-5d\n",
+			      p->layer_info[i].ber_bit_count);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\tpre_ber = %-5d\t",
+			      p->layer_info[i].pre_ber);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\tts_per = %-5d\n",
+			      p->layer_info[i].ts_per);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\terror_ts_packets = %-5d\t",
+			      p->layer_info[i].error_ts_packets);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "total_ts_packets = %-5d\t",
+			      p->layer_info[i].total_ts_packets);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "ti_ldepth_i = %d\n",
+			      p->layer_info[i].ti_ldepth_i);
+		n += snprintf(&buf[n], PAGE_SIZE - n,
+			      "\tnumber_of_segments = %d\t",
+			      p->layer_info[i].number_of_segments);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "tmcc_errors = %d\n",
+			      p->layer_info[i].tmcc_errors);
+	}
+
+	debug_data->stats_count = n;
+	spin_unlock(&debug_data->lock);
+	wake_up(&debug_data->stats_queue);
+}
+
+static void smsdvb_print_isdb_stats_ex(struct smsdvb_debugfs *debug_data,
+				struct sms_isdbt_stats_ex *p)
+{
+	int i, n = 0;
+	char *buf;
+
+	spin_lock(&debug_data->lock);
+	if (debug_data->stats_count) {
+		spin_unlock(&debug_data->lock);
+		return;
+	}
+
+	buf = debug_data->stats_data;
+
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "statistics_type = %d\t", p->statistics_type);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "full_size = %d\n", p->full_size);
+
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "is_rf_locked = %d\t\t", p->is_rf_locked);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "is_demod_locked = %d\t", p->is_demod_locked);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "is_external_lna_on = %d\n", p->is_external_lna_on);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "SNR = %d dB\t\t", p->SNR);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "RSSI = %d dBm\t\t", p->RSSI);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "in_band_pwr = %d dBm\n", p->in_band_pwr);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "carrier_offset = %d\t", p->carrier_offset);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "bandwidth = %d\t\t", p->bandwidth);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "frequency = %d Hz\n", p->frequency);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "transmission_mode = %d\t", p->transmission_mode);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "modem_state = %d\t\t", p->modem_state);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "guard_interval = %d\n", p->guard_interval);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "system_type = %d\t\t", p->system_type);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "partial_reception = %d\t", p->partial_reception);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "num_of_layers = %d\n", p->num_of_layers);
+	n += snprintf(&buf[n], PAGE_SIZE - n, "segment_number = %d\t",
+		      p->segment_number);
+	n += snprintf(&buf[n], PAGE_SIZE - n, "tune_bw = %d\n",
+		      p->tune_bw);
+
+	for (i = 0; i < 3; i++) {
+		if (p->layer_info[i].number_of_segments < 1 ||
+		    p->layer_info[i].number_of_segments > 13)
+			continue;
+
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\nLayer %d\n", i);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\tcode_rate = %d\t",
+			      p->layer_info[i].code_rate);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "constellation = %d\n",
+			      p->layer_info[i].constellation);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\tber = %-5d\t",
+			      p->layer_info[i].ber);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\tber_error_count = %-5d\t",
+			      p->layer_info[i].ber_error_count);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "ber_bit_count = %-5d\n",
+			      p->layer_info[i].ber_bit_count);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\tpre_ber = %-5d\t",
+			      p->layer_info[i].pre_ber);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\tts_per = %-5d\n",
+			      p->layer_info[i].ts_per);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\terror_ts_packets = %-5d\t",
+			      p->layer_info[i].error_ts_packets);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "total_ts_packets = %-5d\t",
+			      p->layer_info[i].total_ts_packets);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "ti_ldepth_i = %d\n",
+			      p->layer_info[i].ti_ldepth_i);
+		n += snprintf(&buf[n], PAGE_SIZE - n,
+			      "\tnumber_of_segments = %d\t",
+			      p->layer_info[i].number_of_segments);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "tmcc_errors = %d\n",
+			      p->layer_info[i].tmcc_errors);
+	}
+
+
+	debug_data->stats_count = n;
+	spin_unlock(&debug_data->lock);
+
+	wake_up(&debug_data->stats_queue);
+}
+
+static int smsdvb_stats_open(struct inode *inode, struct file *file)
+{
+	struct smsdvb_client_t *client = inode->i_private;
+	struct smsdvb_debugfs *debug_data = client->debug_data;
+
+	kref_get(&debug_data->refcount);
+
+	spin_lock(&debug_data->lock);
+	debug_data->stats_count = 0;
+	debug_data->stats_was_read = false;
+	spin_unlock(&debug_data->lock);
+
+	file->private_data = debug_data;
+
+	return 0;
+}
+
+static void smsdvb_debugfs_data_release(struct kref *ref)
+{
+	struct smsdvb_debugfs *debug_data;
+
+	debug_data = container_of(ref, struct smsdvb_debugfs, refcount);
+	kfree(debug_data);
+}
+
+static int smsdvb_stats_wait_read(struct smsdvb_debugfs *debug_data)
+{
+	int rc = 1;
+
+	spin_lock(&debug_data->lock);
+
+	if (debug_data->stats_was_read)
+		goto exit;
+
+	rc = debug_data->stats_count;
+
+exit:
+	spin_unlock(&debug_data->lock);
+	return rc;
+}
+
+static unsigned int smsdvb_stats_poll(struct file *file, poll_table *wait)
+{
+	struct smsdvb_debugfs *debug_data = file->private_data;
+	int rc;
+
+	kref_get(&debug_data->refcount);
+
+	poll_wait(file, &debug_data->stats_queue, wait);
+
+	rc = smsdvb_stats_wait_read(debug_data);
+	if (rc > 0)
+		rc = POLLIN | POLLRDNORM;
+
+	kref_put(&debug_data->refcount, smsdvb_debugfs_data_release);
+
+	return rc;
+}
+
+static ssize_t smsdvb_stats_read(struct file *file, char __user *user_buf,
+				      size_t nbytes, loff_t *ppos)
+{
+	int rc = 0, len;
+	struct smsdvb_debugfs *debug_data = file->private_data;
+
+	kref_get(&debug_data->refcount);
+
+	if (file->f_flags & O_NONBLOCK) {
+		rc = smsdvb_stats_wait_read(debug_data);
+		if (!rc) {
+			rc = -EWOULDBLOCK;
+			goto ret;
+		}
+	} else {
+		rc = wait_event_interruptible(debug_data->stats_queue,
+				      smsdvb_stats_wait_read(debug_data));
+		if (rc < 0)
+			goto ret;
+	}
+
+	if (debug_data->stats_was_read) {
+		rc = 0;	/* EOF */
+		goto ret;
+	}
+
+	len = debug_data->stats_count - *ppos;
+	if (len >= 0)
+		rc = simple_read_from_buffer(user_buf, nbytes, ppos,
+					     debug_data->stats_data, len);
+	else
+		rc = 0;
+
+	if (*ppos >= debug_data->stats_count) {
+		spin_lock(&debug_data->lock);
+		debug_data->stats_was_read = true;
+		spin_unlock(&debug_data->lock);
+	}
+ret:
+	kref_put(&debug_data->refcount, smsdvb_debugfs_data_release);
+	return rc;
+}
+
+static int smsdvb_stats_release(struct inode *inode, struct file *file)
+{
+	struct smsdvb_debugfs *debug_data = file->private_data;
+
+	spin_lock(&debug_data->lock);
+	debug_data->stats_was_read = true;	/* return EOF to read() */
+	spin_unlock(&debug_data->lock);
+	wake_up_interruptible_sync(&debug_data->stats_queue);
+
+	kref_put(&debug_data->refcount, smsdvb_debugfs_data_release);
+	file->private_data = NULL;
+
+	return 0;
+}
+
+static const struct file_operations debugfs_stats_ops = {
+	.open = smsdvb_stats_open,
+	.poll = smsdvb_stats_poll,
+	.read = smsdvb_stats_read,
+	.release = smsdvb_stats_release,
+	.llseek = generic_file_llseek,
+};
+
+/*
+ * Functions used by smsdvb, in order to create the interfaces
+ */
+
+int smsdvb_debugfs_create(struct smsdvb_client_t *client)
+{
+	struct smscore_device_t *coredev = client->coredev;
+	struct dentry *d;
+	struct smsdvb_debugfs *debug_data;
+
+	if (!smsdvb_debugfs_usb_root || !coredev->is_usb_device)
+		return -ENODEV;
+
+	client->debugfs = debugfs_create_dir(coredev->devpath,
+					     smsdvb_debugfs_usb_root);
+	if (IS_ERR_OR_NULL(client->debugfs)) {
+		pr_info("Unable to create debugfs %s directory.\n",
+			coredev->devpath);
+		return -ENODEV;
+	}
+
+	d = debugfs_create_file("stats", S_IRUGO | S_IWUSR, client->debugfs,
+				client, &debugfs_stats_ops);
+	if (!d) {
+		debugfs_remove(client->debugfs);
+		return -ENOMEM;
+	}
+
+	debug_data = kzalloc(sizeof(*client->debug_data), GFP_KERNEL);
+	if (!debug_data)
+		return -ENOMEM;
+
+	client->debug_data        = debug_data;
+	client->prt_dvb_stats     = smsdvb_print_dvb_stats;
+	client->prt_isdb_stats    = smsdvb_print_isdb_stats;
+	client->prt_isdb_stats_ex = smsdvb_print_isdb_stats_ex;
+
+	init_waitqueue_head(&debug_data->stats_queue);
+	spin_lock_init(&debug_data->lock);
+	kref_init(&debug_data->refcount);
+
+	return 0;
+}
+
+void smsdvb_debugfs_release(struct smsdvb_client_t *client)
+{
+	if (!client->debugfs)
+		return;
+
+	client->prt_dvb_stats     = NULL;
+	client->prt_isdb_stats    = NULL;
+	client->prt_isdb_stats_ex = NULL;
+
+	debugfs_remove_recursive(client->debugfs);
+	kref_put(&client->debug_data->refcount, smsdvb_debugfs_data_release);
+
+	client->debug_data = NULL;
+	client->debugfs = NULL;
+}
+
+int smsdvb_debugfs_register(void)
+{
+	struct dentry *d;
+
+	/*
+	 * FIXME: This was written to debug Siano USB devices. So, it creates
+	 * the debugfs node under <debugfs>/usb.
+	 * A similar logic would be needed for Siano sdio devices, but, in that
+	 * case, usb_debug_root is not a good choice.
+	 *
+	 * Perhaps the right fix here would be to create another sysfs root
+	 * node for sdio-based boards, but this may need some logic at sdio
+	 * subsystem.
+	 */
+	d = debugfs_create_dir("smsdvb", usb_debug_root);
+	if (IS_ERR_OR_NULL(d)) {
+		sms_err("Couldn't create sysfs node for smsdvb");
+		return PTR_ERR(d);
+	} else {
+		smsdvb_debugfs_usb_root = d;
+	}
+	return 0;
+}
+
+void smsdvb_debugfs_unregister(void)
+{
+	debugfs_remove_recursive(smsdvb_debugfs_usb_root);
+	smsdvb_debugfs_usb_root = NULL;
+}
diff --git a/drivers/media/common/siano/smsdvb-main.c b/drivers/media/common/siano/smsdvb-main.c
new file mode 100644
index 00000000000..297f1b2f9a3
--- /dev/null
+++ b/drivers/media/common/siano/smsdvb-main.c
@@ -0,0 +1,1230 @@
+/****************************************************************
+
+Siano Mobile Silicon, Inc.
+MDTV receiver kernel modules.
+Copyright (C) 2006-2008, Uri Shkolnik
+
+This program is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 2 of the License, or
+(at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+****************************************************************/
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <asm/div64.h>
+
+#include "dmxdev.h"
+#include "dvbdev.h"
+#include "dvb_demux.h"
+#include "dvb_frontend.h"
+
+#include "smscoreapi.h"
+#include "sms-cards.h"
+
+#include "smsdvb.h"
+
+DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
+
+static struct list_head g_smsdvb_clients;
+static struct mutex g_smsdvb_clientslock;
+
+static int sms_dbg;
+module_param_named(debug, sms_dbg, int, 0644);
+MODULE_PARM_DESC(debug, "set debug level (info=1, adv=2 (or-able))");
+
+
+u32 sms_to_guard_interval_table[] = {
+	[0] = GUARD_INTERVAL_1_32,
+	[1] = GUARD_INTERVAL_1_16,
+	[2] = GUARD_INTERVAL_1_8,
+	[3] = GUARD_INTERVAL_1_4,
+};
+
+u32 sms_to_code_rate_table[] = {
+	[0] = FEC_1_2,
+	[1] = FEC_2_3,
+	[2] = FEC_3_4,
+	[3] = FEC_5_6,
+	[4] = FEC_7_8,
+};
+
+
+u32 sms_to_hierarchy_table[] = {
+	[0] = HIERARCHY_NONE,
+	[1] = HIERARCHY_1,
+	[2] = HIERARCHY_2,
+	[3] = HIERARCHY_4,
+};
+
+u32 sms_to_modulation_table[] = {
+	[0] = QPSK,
+	[1] = QAM_16,
+	[2] = QAM_64,
+	[3] = DQPSK,
+};
+
+
+/* Events that may come from DVB v3 adapter */
+static void sms_board_dvb3_event(struct smsdvb_client_t *client,
+		enum SMS_DVB3_EVENTS event) {
+
+	struct smscore_device_t *coredev = client->coredev;
+	switch (event) {
+	case DVB3_EVENT_INIT:
+		sms_debug("DVB3_EVENT_INIT");
+		sms_board_event(coredev, BOARD_EVENT_BIND);
+		break;
+	case DVB3_EVENT_SLEEP:
+		sms_debug("DVB3_EVENT_SLEEP");
+		sms_board_event(coredev, BOARD_EVENT_POWER_SUSPEND);
+		break;
+	case DVB3_EVENT_HOTPLUG:
+		sms_debug("DVB3_EVENT_HOTPLUG");
+		sms_board_event(coredev, BOARD_EVENT_POWER_INIT);
+		break;
+	case DVB3_EVENT_FE_LOCK:
+		if (client->event_fe_state != DVB3_EVENT_FE_LOCK) {
+			client->event_fe_state = DVB3_EVENT_FE_LOCK;
+			sms_debug("DVB3_EVENT_FE_LOCK");
+			sms_board_event(coredev, BOARD_EVENT_FE_LOCK);
+		}
+		break;
+	case DVB3_EVENT_FE_UNLOCK:
+		if (client->event_fe_state != DVB3_EVENT_FE_UNLOCK) {
+			client->event_fe_state = DVB3_EVENT_FE_UNLOCK;
+			sms_debug("DVB3_EVENT_FE_UNLOCK");
+			sms_board_event(coredev, BOARD_EVENT_FE_UNLOCK);
+		}
+		break;
+	case DVB3_EVENT_UNC_OK:
+		if (client->event_unc_state != DVB3_EVENT_UNC_OK) {
+			client->event_unc_state = DVB3_EVENT_UNC_OK;
+			sms_debug("DVB3_EVENT_UNC_OK");
+			sms_board_event(coredev, BOARD_EVENT_MULTIPLEX_OK);
+		}
+		break;
+	case DVB3_EVENT_UNC_ERR:
+		if (client->event_unc_state != DVB3_EVENT_UNC_ERR) {
+			client->event_unc_state = DVB3_EVENT_UNC_ERR;
+			sms_debug("DVB3_EVENT_UNC_ERR");
+			sms_board_event(coredev, BOARD_EVENT_MULTIPLEX_ERRORS);
+		}
+		break;
+
+	default:
+		sms_err("Unknown dvb3 api event");
+		break;
+	}
+}
+
+static void smsdvb_stats_not_ready(struct dvb_frontend *fe)
+{
+	struct smsdvb_client_t *client =
+		container_of(fe, struct smsdvb_client_t, frontend);
+	struct smscore_device_t *coredev = client->coredev;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int i, n_layers;
+
+	switch (smscore_get_device_mode(coredev)) {
+	case DEVICE_MODE_ISDBT:
+	case DEVICE_MODE_ISDBT_BDA:
+		n_layers = 4;
+	default:
+		n_layers = 1;
+	}
+
+	/* Global stats */
+	c->strength.len = 1;
+	c->cnr.len = 1;
+	c->strength.stat[0].scale = FE_SCALE_DECIBEL;
+	c->cnr.stat[0].scale = FE_SCALE_DECIBEL;
+
+	/* Per-layer stats */
+	c->post_bit_error.len = n_layers;
+	c->post_bit_count.len = n_layers;
+	c->block_error.len = n_layers;
+	c->block_count.len = n_layers;
+
+	/*
+	 * Put all of them at FE_SCALE_NOT_AVAILABLE. They're dynamically
+	 * changed when the stats become available.
+	 */
+	for (i = 0; i < n_layers; i++) {
+		c->post_bit_error.stat[i].scale = FE_SCALE_NOT_AVAILABLE;
+		c->post_bit_count.stat[i].scale = FE_SCALE_NOT_AVAILABLE;
+		c->block_error.stat[i].scale = FE_SCALE_NOT_AVAILABLE;
+		c->block_count.stat[i].scale = FE_SCALE_NOT_AVAILABLE;
+	}
+}
+
+static inline int sms_to_mode(u32 mode)
+{
+	switch (mode) {
+	case 2:
+		return TRANSMISSION_MODE_2K;
+	case 4:
+		return TRANSMISSION_MODE_4K;
+	case 8:
+		return TRANSMISSION_MODE_8K;
+	}
+	return TRANSMISSION_MODE_AUTO;
+}
+
+static inline int sms_to_status(u32 is_demod_locked, u32 is_rf_locked)
+{
+	if (is_demod_locked)
+		return FE_HAS_SIGNAL  | FE_HAS_CARRIER | FE_HAS_VITERBI |
+		       FE_HAS_SYNC    | FE_HAS_LOCK;
+
+	if (is_rf_locked)
+		return FE_HAS_SIGNAL | FE_HAS_CARRIER;
+
+	return 0;
+}
+
+static inline u32 sms_to_bw(u32 value)
+{
+	return value * 1000000;
+}
+
+#define convert_from_table(value, table, defval) ({			\
+	u32 __ret;							\
+	if (value < ARRAY_SIZE(table))					\
+		__ret = table[value];					\
+	else								\
+		__ret = defval;						\
+	__ret;								\
+})
+
+#define sms_to_guard_interval(value)					\
+	convert_from_table(value, sms_to_guard_interval_table,		\
+			   GUARD_INTERVAL_AUTO);
+
+#define sms_to_code_rate(value)						\
+	convert_from_table(value, sms_to_code_rate_table,		\
+			   FEC_NONE);
+
+#define sms_to_hierarchy(value)						\
+	convert_from_table(value, sms_to_hierarchy_table,		\
+			   FEC_NONE);
+
+#define sms_to_modulation(value)					\
+	convert_from_table(value, sms_to_modulation_table,		\
+			   FEC_NONE);
+
+static void smsdvb_update_tx_params(struct smsdvb_client_t *client,
+				    struct sms_tx_stats *p)
+{
+	struct dvb_frontend *fe = &client->frontend;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+
+	c->frequency = p->frequency;
+	client->fe_status = sms_to_status(p->is_demod_locked, 0);
+	c->bandwidth_hz = sms_to_bw(p->bandwidth);
+	c->transmission_mode = sms_to_mode(p->transmission_mode);
+	c->guard_interval = sms_to_guard_interval(p->guard_interval);
+	c->code_rate_HP = sms_to_code_rate(p->code_rate);
+	c->code_rate_LP = sms_to_code_rate(p->lp_code_rate);
+	c->hierarchy = sms_to_hierarchy(p->hierarchy);
+	c->modulation = sms_to_modulation(p->constellation);
+}
+
+static void smsdvb_update_per_slices(struct smsdvb_client_t *client,
+				     struct RECEPTION_STATISTICS_PER_SLICES_S *p)
+{
+	struct dvb_frontend *fe = &client->frontend;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	u64 tmp;
+
+	client->fe_status = sms_to_status(p->is_demod_locked, p->is_rf_locked);
+	c->modulation = sms_to_modulation(p->constellation);
+
+	/* signal Strength, in DBm */
+	c->strength.stat[0].uvalue = p->in_band_power * 1000;
+
+	/* Carrier to noise ratio, in DB */
+	c->cnr.stat[0].svalue = p->snr * 1000;
+
+	/* PER/BER requires demod lock */
+	if (!p->is_demod_locked)
+		return;
+
+	/* TS PER */
+	client->last_per = c->block_error.stat[0].uvalue;
+	c->block_error.stat[0].scale = FE_SCALE_COUNTER;
+	c->block_count.stat[0].scale = FE_SCALE_COUNTER;
+	c->block_error.stat[0].uvalue += p->ets_packets;
+	c->block_count.stat[0].uvalue += p->ets_packets + p->ts_packets;
+
+	/* ber */
+	c->post_bit_error.stat[0].scale = FE_SCALE_COUNTER;
+	c->post_bit_count.stat[0].scale = FE_SCALE_COUNTER;
+	c->post_bit_error.stat[0].uvalue += p->ber_error_count;
+	c->post_bit_count.stat[0].uvalue += p->ber_bit_count;
+
+	/* Legacy PER/BER */
+	tmp = p->ets_packets * 65535;
+	do_div(tmp, p->ts_packets + p->ets_packets);
+	client->legacy_per = tmp;
+}
+
+static void smsdvb_update_dvb_stats(struct smsdvb_client_t *client,
+				    struct sms_stats *p)
+{
+	struct dvb_frontend *fe = &client->frontend;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+
+	if (client->prt_dvb_stats)
+		client->prt_dvb_stats(client->debug_data, p);
+
+	client->fe_status = sms_to_status(p->is_demod_locked, p->is_rf_locked);
+
+	/* Update DVB modulation parameters */
+	c->frequency = p->frequency;
+	client->fe_status = sms_to_status(p->is_demod_locked, 0);
+	c->bandwidth_hz = sms_to_bw(p->bandwidth);
+	c->transmission_mode = sms_to_mode(p->transmission_mode);
+	c->guard_interval = sms_to_guard_interval(p->guard_interval);
+	c->code_rate_HP = sms_to_code_rate(p->code_rate);
+	c->code_rate_LP = sms_to_code_rate(p->lp_code_rate);
+	c->hierarchy = sms_to_hierarchy(p->hierarchy);
+	c->modulation = sms_to_modulation(p->constellation);
+
+	/* update reception data */
+	c->lna = p->is_external_lna_on ? 1 : 0;
+
+	/* Carrier to noise ratio, in DB */
+	c->cnr.stat[0].svalue = p->SNR * 1000;
+
+	/* signal Strength, in DBm */
+	c->strength.stat[0].uvalue = p->in_band_pwr * 1000;
+
+	/* PER/BER requires demod lock */
+	if (!p->is_demod_locked)
+		return;
+
+	/* TS PER */
+	client->last_per = c->block_error.stat[0].uvalue;
+	c->block_error.stat[0].scale = FE_SCALE_COUNTER;
+	c->block_count.stat[0].scale = FE_SCALE_COUNTER;
+	c->block_error.stat[0].uvalue += p->error_ts_packets;
+	c->block_count.stat[0].uvalue += p->total_ts_packets;
+
+	/* ber */
+	c->post_bit_error.stat[0].scale = FE_SCALE_COUNTER;
+	c->post_bit_count.stat[0].scale = FE_SCALE_COUNTER;
+	c->post_bit_error.stat[0].uvalue += p->ber_error_count;
+	c->post_bit_count.stat[0].uvalue += p->ber_bit_count;
+
+	/* Legacy PER/BER */
+	client->legacy_ber = p->ber;
+};
+
+static void smsdvb_update_isdbt_stats(struct smsdvb_client_t *client,
+				      struct sms_isdbt_stats *p)
+{
+	struct dvb_frontend *fe = &client->frontend;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct sms_isdbt_layer_stats *lr;
+	int i, n_layers;
+
+	if (client->prt_isdb_stats)
+		client->prt_isdb_stats(client->debug_data, p);
+
+	client->fe_status = sms_to_status(p->is_demod_locked, p->is_rf_locked);
+
+	/*
+	 * Firmware 2.1 seems to report only lock status and
+	 * signal strength. The signal strength indicator is at the
+	 * wrong field.
+	 */
+	if (p->statistics_type == 0) {
+		c->strength.stat[0].uvalue = ((s32)p->transmission_mode) * 1000;
+		c->cnr.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		return;
+	}
+
+	/* Update ISDB-T transmission parameters */
+	c->frequency = p->frequency;
+	c->bandwidth_hz = sms_to_bw(p->bandwidth);
+	c->transmission_mode = sms_to_mode(p->transmission_mode);
+	c->guard_interval = sms_to_guard_interval(p->guard_interval);
+	c->isdbt_partial_reception = p->partial_reception ? 1 : 0;
+	n_layers = p->num_of_layers;
+	if (n_layers < 1)
+		n_layers = 1;
+	if (n_layers > 3)
+		n_layers = 3;
+	c->isdbt_layer_enabled = 0;
+
+	/* update reception data */
+	c->lna = p->is_external_lna_on ? 1 : 0;
+
+	/* Carrier to noise ratio, in DB */
+	c->cnr.stat[0].svalue = p->SNR * 1000;
+
+	/* signal Strength, in DBm */
+	c->strength.stat[0].uvalue = p->in_band_pwr * 1000;
+
+	/* PER/BER and per-layer stats require demod lock */
+	if (!p->is_demod_locked)
+		return;
+
+	client->last_per = c->block_error.stat[0].uvalue;
+
+	/* Clears global counters, as the code below will sum it again */
+	c->block_error.stat[0].uvalue = 0;
+	c->block_count.stat[0].uvalue = 0;
+	c->block_error.stat[0].scale = FE_SCALE_COUNTER;
+	c->block_count.stat[0].scale = FE_SCALE_COUNTER;
+	c->post_bit_error.stat[0].uvalue = 0;
+	c->post_bit_count.stat[0].uvalue = 0;
+	c->post_bit_error.stat[0].scale = FE_SCALE_COUNTER;
+	c->post_bit_count.stat[0].scale = FE_SCALE_COUNTER;
+
+	for (i = 0; i < n_layers; i++) {
+		lr = &p->layer_info[i];
+
+		/* Update per-layer transmission parameters */
+		if (lr->number_of_segments > 0 && lr->number_of_segments < 13) {
+			c->isdbt_layer_enabled |= 1 << i;
+			c->layer[i].segment_count = lr->number_of_segments;
+		} else {
+			continue;
+		}
+		c->layer[i].modulation = sms_to_modulation(lr->constellation);
+
+		/* TS PER */
+		c->block_error.stat[i + 1].scale = FE_SCALE_COUNTER;
+		c->block_count.stat[i + 1].scale = FE_SCALE_COUNTER;
+		c->block_error.stat[i + 1].uvalue += lr->error_ts_packets;
+		c->block_count.stat[i + 1].uvalue += lr->total_ts_packets;
+
+		/* Update global PER counter */
+		c->block_error.stat[0].uvalue += lr->error_ts_packets;
+		c->block_count.stat[0].uvalue += lr->total_ts_packets;
+
+		/* BER */
+		c->post_bit_error.stat[i + 1].scale = FE_SCALE_COUNTER;
+		c->post_bit_count.stat[i + 1].scale = FE_SCALE_COUNTER;
+		c->post_bit_error.stat[i + 1].uvalue += lr->ber_error_count;
+		c->post_bit_count.stat[i + 1].uvalue += lr->ber_bit_count;
+
+		/* Update global BER counter */
+		c->post_bit_error.stat[0].uvalue += lr->ber_error_count;
+		c->post_bit_count.stat[0].uvalue += lr->ber_bit_count;
+	}
+}
+
+static void smsdvb_update_isdbt_stats_ex(struct smsdvb_client_t *client,
+					 struct sms_isdbt_stats_ex *p)
+{
+	struct dvb_frontend *fe = &client->frontend;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct sms_isdbt_layer_stats *lr;
+	int i, n_layers;
+
+	if (client->prt_isdb_stats_ex)
+		client->prt_isdb_stats_ex(client->debug_data, p);
+
+	/* Update ISDB-T transmission parameters */
+	c->frequency = p->frequency;
+	client->fe_status = sms_to_status(p->is_demod_locked, 0);
+	c->bandwidth_hz = sms_to_bw(p->bandwidth);
+	c->transmission_mode = sms_to_mode(p->transmission_mode);
+	c->guard_interval = sms_to_guard_interval(p->guard_interval);
+	c->isdbt_partial_reception = p->partial_reception ? 1 : 0;
+	n_layers = p->num_of_layers;
+	if (n_layers < 1)
+		n_layers = 1;
+	if (n_layers > 3)
+		n_layers = 3;
+	c->isdbt_layer_enabled = 0;
+
+	/* update reception data */
+	c->lna = p->is_external_lna_on ? 1 : 0;
+
+	/* Carrier to noise ratio, in DB */
+	c->cnr.stat[0].svalue = p->SNR * 1000;
+
+	/* signal Strength, in DBm */
+	c->strength.stat[0].uvalue = p->in_band_pwr * 1000;
+
+	/* PER/BER and per-layer stats require demod lock */
+	if (!p->is_demod_locked)
+		return;
+
+	client->last_per = c->block_error.stat[0].uvalue;
+
+	/* Clears global counters, as the code below will sum it again */
+	c->block_error.stat[0].uvalue = 0;
+	c->block_count.stat[0].uvalue = 0;
+	c->block_error.stat[0].scale = FE_SCALE_COUNTER;
+	c->block_count.stat[0].scale = FE_SCALE_COUNTER;
+	c->post_bit_error.stat[0].uvalue = 0;
+	c->post_bit_count.stat[0].uvalue = 0;
+	c->post_bit_error.stat[0].scale = FE_SCALE_COUNTER;
+	c->post_bit_count.stat[0].scale = FE_SCALE_COUNTER;
+
+	c->post_bit_error.len = n_layers + 1;
+	c->post_bit_count.len = n_layers + 1;
+	c->block_error.len = n_layers + 1;
+	c->block_count.len = n_layers + 1;
+	for (i = 0; i < n_layers; i++) {
+		lr = &p->layer_info[i];
+
+		/* Update per-layer transmission parameters */
+		if (lr->number_of_segments > 0 && lr->number_of_segments < 13) {
+			c->isdbt_layer_enabled |= 1 << i;
+			c->layer[i].segment_count = lr->number_of_segments;
+		} else {
+			continue;
+		}
+		c->layer[i].modulation = sms_to_modulation(lr->constellation);
+
+		/* TS PER */
+		c->block_error.stat[i + 1].scale = FE_SCALE_COUNTER;
+		c->block_count.stat[i + 1].scale = FE_SCALE_COUNTER;
+		c->block_error.stat[i + 1].uvalue += lr->error_ts_packets;
+		c->block_count.stat[i + 1].uvalue += lr->total_ts_packets;
+
+		/* Update global PER counter */
+		c->block_error.stat[0].uvalue += lr->error_ts_packets;
+		c->block_count.stat[0].uvalue += lr->total_ts_packets;
+
+		/* ber */
+		c->post_bit_error.stat[i + 1].scale = FE_SCALE_COUNTER;
+		c->post_bit_count.stat[i + 1].scale = FE_SCALE_COUNTER;
+		c->post_bit_error.stat[i + 1].uvalue += lr->ber_error_count;
+		c->post_bit_count.stat[i + 1].uvalue += lr->ber_bit_count;
+
+		/* Update global ber counter */
+		c->post_bit_error.stat[0].uvalue += lr->ber_error_count;
+		c->post_bit_count.stat[0].uvalue += lr->ber_bit_count;
+	}
+}
+
+static int smsdvb_onresponse(void *context, struct smscore_buffer_t *cb)
+{
+	struct smsdvb_client_t *client = (struct smsdvb_client_t *) context;
+	struct sms_msg_hdr *phdr = (struct sms_msg_hdr *) (((u8 *) cb->p)
+			+ cb->offset);
+	void *p = phdr + 1;
+	struct dvb_frontend *fe = &client->frontend;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	bool is_status_update = false;
+
+	switch (phdr->msg_type) {
+	case MSG_SMS_DVBT_BDA_DATA:
+		/*
+		 * Only feed data to dvb demux if are there any feed listening
+		 * to it and if the device has tuned
+		 */
+		if (client->feed_users && client->has_tuned)
+			dvb_dmx_swfilter(&client->demux, p,
+					 cb->size - sizeof(struct sms_msg_hdr));
+		break;
+
+	case MSG_SMS_RF_TUNE_RES:
+	case MSG_SMS_ISDBT_TUNE_RES:
+		complete(&client->tune_done);
+		break;
+
+	case MSG_SMS_SIGNAL_DETECTED_IND:
+		client->fe_status = FE_HAS_SIGNAL  | FE_HAS_CARRIER |
+				    FE_HAS_VITERBI | FE_HAS_SYNC    |
+				    FE_HAS_LOCK;
+
+		is_status_update = true;
+		break;
+
+	case MSG_SMS_NO_SIGNAL_IND:
+		client->fe_status = 0;
+
+		is_status_update = true;
+		break;
+
+	case MSG_SMS_TRANSMISSION_IND:
+		smsdvb_update_tx_params(client, p);
+
+		is_status_update = true;
+		break;
+
+	case MSG_SMS_HO_PER_SLICES_IND:
+		smsdvb_update_per_slices(client, p);
+
+		is_status_update = true;
+		break;
+
+	case MSG_SMS_GET_STATISTICS_RES:
+		switch (smscore_get_device_mode(client->coredev)) {
+		case DEVICE_MODE_ISDBT:
+		case DEVICE_MODE_ISDBT_BDA:
+			smsdvb_update_isdbt_stats(client, p);
+			break;
+		default:
+			/* Skip sms_msg_statistics_info:request_result field */
+			smsdvb_update_dvb_stats(client, p + sizeof(u32));
+		}
+
+		is_status_update = true;
+		break;
+
+	/* Only for ISDB-T */
+	case MSG_SMS_GET_STATISTICS_EX_RES:
+		/* Skip sms_msg_statistics_info:request_result field? */
+		smsdvb_update_isdbt_stats_ex(client, p + sizeof(u32));
+		is_status_update = true;
+		break;
+	default:
+		sms_info("message not handled");
+	}
+	smscore_putbuffer(client->coredev, cb);
+
+	if (is_status_update) {
+		if (client->fe_status & FE_HAS_LOCK) {
+			sms_board_dvb3_event(client, DVB3_EVENT_FE_LOCK);
+			if (client->last_per == c->block_error.stat[0].uvalue)
+				sms_board_dvb3_event(client, DVB3_EVENT_UNC_OK);
+			else
+				sms_board_dvb3_event(client, DVB3_EVENT_UNC_ERR);
+			client->has_tuned = true;
+		} else {
+			smsdvb_stats_not_ready(fe);
+			client->has_tuned = false;
+			sms_board_dvb3_event(client, DVB3_EVENT_FE_UNLOCK);
+		}
+		complete(&client->stats_done);
+	}
+
+	return 0;
+}
+
+static void smsdvb_unregister_client(struct smsdvb_client_t *client)
+{
+	/* must be called under clientslock */
+
+	list_del(&client->entry);
+
+	smsdvb_debugfs_release(client);
+	smscore_unregister_client(client->smsclient);
+	dvb_unregister_frontend(&client->frontend);
+	dvb_dmxdev_release(&client->dmxdev);
+	dvb_dmx_release(&client->demux);
+	dvb_unregister_adapter(&client->adapter);
+	kfree(client);
+}
+
+static void smsdvb_onremove(void *context)
+{
+	kmutex_lock(&g_smsdvb_clientslock);
+
+	smsdvb_unregister_client((struct smsdvb_client_t *) context);
+
+	kmutex_unlock(&g_smsdvb_clientslock);
+}
+
+static int smsdvb_start_feed(struct dvb_demux_feed *feed)
+{
+	struct smsdvb_client_t *client =
+		container_of(feed->demux, struct smsdvb_client_t, demux);
+	struct sms_msg_data pid_msg;
+
+	sms_debug("add pid %d(%x)",
+		  feed->pid, feed->pid);
+
+	client->feed_users++;
+
+	pid_msg.x_msg_header.msg_src_id = DVBT_BDA_CONTROL_MSG_ID;
+	pid_msg.x_msg_header.msg_dst_id = HIF_TASK;
+	pid_msg.x_msg_header.msg_flags = 0;
+	pid_msg.x_msg_header.msg_type  = MSG_SMS_ADD_PID_FILTER_REQ;
+	pid_msg.x_msg_header.msg_length = sizeof(pid_msg);
+	pid_msg.msg_data[0] = feed->pid;
+
+	return smsclient_sendrequest(client->smsclient,
+				     &pid_msg, sizeof(pid_msg));
+}
+
+static int smsdvb_stop_feed(struct dvb_demux_feed *feed)
+{
+	struct smsdvb_client_t *client =
+		container_of(feed->demux, struct smsdvb_client_t, demux);
+	struct sms_msg_data pid_msg;
+
+	sms_debug("remove pid %d(%x)",
+		  feed->pid, feed->pid);
+
+	client->feed_users--;
+
+	pid_msg.x_msg_header.msg_src_id = DVBT_BDA_CONTROL_MSG_ID;
+	pid_msg.x_msg_header.msg_dst_id = HIF_TASK;
+	pid_msg.x_msg_header.msg_flags = 0;
+	pid_msg.x_msg_header.msg_type  = MSG_SMS_REMOVE_PID_FILTER_REQ;
+	pid_msg.x_msg_header.msg_length = sizeof(pid_msg);
+	pid_msg.msg_data[0] = feed->pid;
+
+	return smsclient_sendrequest(client->smsclient,
+				     &pid_msg, sizeof(pid_msg));
+}
+
+static int smsdvb_sendrequest_and_wait(struct smsdvb_client_t *client,
+					void *buffer, size_t size,
+					struct completion *completion)
+{
+	int rc;
+
+	rc = smsclient_sendrequest(client->smsclient, buffer, size);
+	if (rc < 0)
+		return rc;
+
+	return wait_for_completion_timeout(completion,
+					   msecs_to_jiffies(2000)) ?
+						0 : -ETIME;
+}
+
+static int smsdvb_send_statistics_request(struct smsdvb_client_t *client)
+{
+	int rc;
+	struct sms_msg_hdr msg;
+
+	/* Don't request stats too fast */
+	if (client->get_stats_jiffies &&
+	   (!time_after(jiffies, client->get_stats_jiffies)))
+		return 0;
+	client->get_stats_jiffies = jiffies + msecs_to_jiffies(100);
+
+	msg.msg_src_id = DVBT_BDA_CONTROL_MSG_ID;
+	msg.msg_dst_id = HIF_TASK;
+	msg.msg_flags = 0;
+	msg.msg_length = sizeof(msg);
+
+	switch (smscore_get_device_mode(client->coredev)) {
+	case DEVICE_MODE_ISDBT:
+	case DEVICE_MODE_ISDBT_BDA:
+		/*
+		* Check for firmware version, to avoid breaking for old cards
+		*/
+		if (client->coredev->fw_version >= 0x800)
+			msg.msg_type = MSG_SMS_GET_STATISTICS_EX_REQ;
+		else
+			msg.msg_type = MSG_SMS_GET_STATISTICS_REQ;
+		break;
+	default:
+		msg.msg_type = MSG_SMS_GET_STATISTICS_REQ;
+	}
+
+	rc = smsdvb_sendrequest_and_wait(client, &msg, sizeof(msg),
+					 &client->stats_done);
+
+	return rc;
+}
+
+static inline int led_feedback(struct smsdvb_client_t *client)
+{
+	if (!(client->fe_status & FE_HAS_LOCK))
+		return sms_board_led_feedback(client->coredev, SMS_LED_OFF);
+
+	return sms_board_led_feedback(client->coredev,
+				     (client->legacy_ber == 0) ?
+				     SMS_LED_HI : SMS_LED_LO);
+}
+
+static int smsdvb_read_status(struct dvb_frontend *fe, fe_status_t *stat)
+{
+	int rc;
+	struct smsdvb_client_t *client;
+	client = container_of(fe, struct smsdvb_client_t, frontend);
+
+	rc = smsdvb_send_statistics_request(client);
+
+	*stat = client->fe_status;
+
+	led_feedback(client);
+
+	return rc;
+}
+
+static int smsdvb_read_ber(struct dvb_frontend *fe, u32 *ber)
+{
+	int rc;
+	struct smsdvb_client_t *client;
+
+	client = container_of(fe, struct smsdvb_client_t, frontend);
+
+	rc = smsdvb_send_statistics_request(client);
+
+	*ber = client->legacy_ber;
+
+	led_feedback(client);
+
+	return rc;
+}
+
+static int smsdvb_read_signal_strength(struct dvb_frontend *fe, u16 *strength)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int rc;
+	s32 power = (s32) c->strength.stat[0].uvalue;
+	struct smsdvb_client_t *client;
+
+	client = container_of(fe, struct smsdvb_client_t, frontend);
+
+	rc = smsdvb_send_statistics_request(client);
+
+	if (power < -95)
+		*strength = 0;
+		else if (power > -29)
+			*strength = 65535;
+		else
+			*strength = (power + 95) * 65535 / 66;
+
+	led_feedback(client);
+
+	return rc;
+}
+
+static int smsdvb_read_snr(struct dvb_frontend *fe, u16 *snr)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int rc;
+	struct smsdvb_client_t *client;
+
+	client = container_of(fe, struct smsdvb_client_t, frontend);
+
+	rc = smsdvb_send_statistics_request(client);
+
+	/* Preferred scale for SNR with legacy API: 0.1 dB */
+	*snr = ((u32)c->cnr.stat[0].svalue) / 100;
+
+	led_feedback(client);
+
+	return rc;
+}
+
+static int smsdvb_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
+{
+	int rc;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct smsdvb_client_t *client;
+
+	client = container_of(fe, struct smsdvb_client_t, frontend);
+
+	rc = smsdvb_send_statistics_request(client);
+
+	*ucblocks = c->block_error.stat[0].uvalue;
+
+	led_feedback(client);
+
+	return rc;
+}
+
+static int smsdvb_get_tune_settings(struct dvb_frontend *fe,
+				    struct dvb_frontend_tune_settings *tune)
+{
+	sms_debug("");
+
+	tune->min_delay_ms = 400;
+	tune->step_size = 250000;
+	tune->max_drift = 0;
+	return 0;
+}
+
+static int smsdvb_dvbt_set_frontend(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct smsdvb_client_t *client =
+		container_of(fe, struct smsdvb_client_t, frontend);
+
+	struct {
+		struct sms_msg_hdr	msg;
+		u32		Data[3];
+	} msg;
+
+	int ret;
+
+	client->fe_status = 0;
+	client->event_fe_state = -1;
+	client->event_unc_state = -1;
+	fe->dtv_property_cache.delivery_system = SYS_DVBT;
+
+	msg.msg.msg_src_id = DVBT_BDA_CONTROL_MSG_ID;
+	msg.msg.msg_dst_id = HIF_TASK;
+	msg.msg.msg_flags = 0;
+	msg.msg.msg_type = MSG_SMS_RF_TUNE_REQ;
+	msg.msg.msg_length = sizeof(msg);
+	msg.Data[0] = c->frequency;
+	msg.Data[2] = 12000000;
+
+	sms_info("%s: freq %d band %d", __func__, c->frequency,
+		 c->bandwidth_hz);
+
+	switch (c->bandwidth_hz / 1000000) {
+	case 8:
+		msg.Data[1] = BW_8_MHZ;
+		break;
+	case 7:
+		msg.Data[1] = BW_7_MHZ;
+		break;
+	case 6:
+		msg.Data[1] = BW_6_MHZ;
+		break;
+	case 0:
+		return -EOPNOTSUPP;
+	default:
+		return -EINVAL;
+	}
+	/* Disable LNA, if any. An error is returned if no LNA is present */
+	ret = sms_board_lna_control(client->coredev, 0);
+	if (ret == 0) {
+		fe_status_t status;
+
+		/* tune with LNA off at first */
+		ret = smsdvb_sendrequest_and_wait(client, &msg, sizeof(msg),
+						  &client->tune_done);
+
+		smsdvb_read_status(fe, &status);
+
+		if (status & FE_HAS_LOCK)
+			return ret;
+
+		/* previous tune didn't lock - enable LNA and tune again */
+		sms_board_lna_control(client->coredev, 1);
+	}
+
+	return smsdvb_sendrequest_and_wait(client, &msg, sizeof(msg),
+					   &client->tune_done);
+}
+
+static int smsdvb_isdbt_set_frontend(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct smsdvb_client_t *client =
+		container_of(fe, struct smsdvb_client_t, frontend);
+	int board_id = smscore_get_board_id(client->coredev);
+	struct sms_board *board = sms_get_board(board_id);
+	enum sms_device_type_st type = board->type;
+	int ret;
+
+	struct {
+		struct sms_msg_hdr	msg;
+		u32		Data[4];
+	} msg;
+
+	fe->dtv_property_cache.delivery_system = SYS_ISDBT;
+
+	msg.msg.msg_src_id  = DVBT_BDA_CONTROL_MSG_ID;
+	msg.msg.msg_dst_id  = HIF_TASK;
+	msg.msg.msg_flags  = 0;
+	msg.msg.msg_type   = MSG_SMS_ISDBT_TUNE_REQ;
+	msg.msg.msg_length = sizeof(msg);
+
+	if (c->isdbt_sb_segment_idx == -1)
+		c->isdbt_sb_segment_idx = 0;
+
+	if (!c->isdbt_layer_enabled)
+		c->isdbt_layer_enabled = 7;
+
+	msg.Data[0] = c->frequency;
+	msg.Data[1] = BW_ISDBT_1SEG;
+	msg.Data[2] = 12000000;
+	msg.Data[3] = c->isdbt_sb_segment_idx;
+
+	if (c->isdbt_partial_reception) {
+		if ((type == SMS_PELE || type == SMS_RIO) &&
+		    c->isdbt_sb_segment_count > 3)
+			msg.Data[1] = BW_ISDBT_13SEG;
+		else if (c->isdbt_sb_segment_count > 1)
+			msg.Data[1] = BW_ISDBT_3SEG;
+	} else if (type == SMS_PELE || type == SMS_RIO)
+		msg.Data[1] = BW_ISDBT_13SEG;
+
+	c->bandwidth_hz = 6000000;
+
+	sms_info("%s: freq %d segwidth %d segindex %d", __func__,
+		 c->frequency, c->isdbt_sb_segment_count,
+		 c->isdbt_sb_segment_idx);
+
+	/* Disable LNA, if any. An error is returned if no LNA is present */
+	ret = sms_board_lna_control(client->coredev, 0);
+	if (ret == 0) {
+		fe_status_t status;
+
+		/* tune with LNA off at first */
+		ret = smsdvb_sendrequest_and_wait(client, &msg, sizeof(msg),
+						  &client->tune_done);
+
+		smsdvb_read_status(fe, &status);
+
+		if (status & FE_HAS_LOCK)
+			return ret;
+
+		/* previous tune didn't lock - enable LNA and tune again */
+		sms_board_lna_control(client->coredev, 1);
+	}
+	return smsdvb_sendrequest_and_wait(client, &msg, sizeof(msg),
+					   &client->tune_done);
+}
+
+static int smsdvb_set_frontend(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct smsdvb_client_t *client =
+		container_of(fe, struct smsdvb_client_t, frontend);
+	struct smscore_device_t *coredev = client->coredev;
+
+	smsdvb_stats_not_ready(fe);
+	c->strength.stat[0].uvalue = 0;
+	c->cnr.stat[0].uvalue = 0;
+
+	client->has_tuned = false;
+
+	switch (smscore_get_device_mode(coredev)) {
+	case DEVICE_MODE_DVBT:
+	case DEVICE_MODE_DVBT_BDA:
+		return smsdvb_dvbt_set_frontend(fe);
+	case DEVICE_MODE_ISDBT:
+	case DEVICE_MODE_ISDBT_BDA:
+		return smsdvb_isdbt_set_frontend(fe);
+	default:
+		return -EINVAL;
+	}
+}
+
+/* Nothing to do here, as stats are automatically updated */
+static int smsdvb_get_frontend(struct dvb_frontend *fe)
+{
+	return 0;
+}
+
+static int smsdvb_init(struct dvb_frontend *fe)
+{
+	struct smsdvb_client_t *client =
+		container_of(fe, struct smsdvb_client_t, frontend);
+
+	sms_board_power(client->coredev, 1);
+
+	sms_board_dvb3_event(client, DVB3_EVENT_INIT);
+	return 0;
+}
+
+static int smsdvb_sleep(struct dvb_frontend *fe)
+{
+	struct smsdvb_client_t *client =
+		container_of(fe, struct smsdvb_client_t, frontend);
+
+	sms_board_led_feedback(client->coredev, SMS_LED_OFF);
+	sms_board_power(client->coredev, 0);
+
+	sms_board_dvb3_event(client, DVB3_EVENT_SLEEP);
+
+	return 0;
+}
+
+static void smsdvb_release(struct dvb_frontend *fe)
+{
+	/* do nothing */
+}
+
+static struct dvb_frontend_ops smsdvb_fe_ops = {
+	.info = {
+		.name			= "Siano Mobile Digital MDTV Receiver",
+		.frequency_min		= 44250000,
+		.frequency_max		= 867250000,
+		.frequency_stepsize	= 250000,
+		.caps = FE_CAN_INVERSION_AUTO |
+			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_RECOVER |
+			FE_CAN_HIERARCHY_AUTO,
+	},
+
+	.release = smsdvb_release,
+
+	.set_frontend = smsdvb_set_frontend,
+	.get_frontend = smsdvb_get_frontend,
+	.get_tune_settings = smsdvb_get_tune_settings,
+
+	.read_status = smsdvb_read_status,
+	.read_ber = smsdvb_read_ber,
+	.read_signal_strength = smsdvb_read_signal_strength,
+	.read_snr = smsdvb_read_snr,
+	.read_ucblocks = smsdvb_read_ucblocks,
+
+	.init = smsdvb_init,
+	.sleep = smsdvb_sleep,
+};
+
+static int smsdvb_hotplug(struct smscore_device_t *coredev,
+			  struct device *device, int arrival)
+{
+	struct smsclient_params_t params;
+	struct smsdvb_client_t *client;
+	int rc;
+
+	/* device removal handled by onremove callback */
+	if (!arrival)
+		return 0;
+	client = kzalloc(sizeof(struct smsdvb_client_t), GFP_KERNEL);
+	if (!client) {
+		sms_err("kmalloc() failed");
+		return -ENOMEM;
+	}
+
+	/* register dvb adapter */
+	rc = dvb_register_adapter(&client->adapter,
+				  sms_get_board(
+					smscore_get_board_id(coredev))->name,
+				  THIS_MODULE, device, adapter_nr);
+	if (rc < 0) {
+		sms_err("dvb_register_adapter() failed %d", rc);
+		goto adapter_error;
+	}
+
+	/* init dvb demux */
+	client->demux.dmx.capabilities = DMX_TS_FILTERING;
+	client->demux.filternum = 32; /* todo: nova ??? */
+	client->demux.feednum = 32;
+	client->demux.start_feed = smsdvb_start_feed;
+	client->demux.stop_feed = smsdvb_stop_feed;
+
+	rc = dvb_dmx_init(&client->demux);
+	if (rc < 0) {
+		sms_err("dvb_dmx_init failed %d", rc);
+		goto dvbdmx_error;
+	}
+
+	/* init dmxdev */
+	client->dmxdev.filternum = 32;
+	client->dmxdev.demux = &client->demux.dmx;
+	client->dmxdev.capabilities = 0;
+
+	rc = dvb_dmxdev_init(&client->dmxdev, &client->adapter);
+	if (rc < 0) {
+		sms_err("dvb_dmxdev_init failed %d", rc);
+		goto dmxdev_error;
+	}
+
+	/* init and register frontend */
+	memcpy(&client->frontend.ops, &smsdvb_fe_ops,
+	       sizeof(struct dvb_frontend_ops));
+
+	switch (smscore_get_device_mode(coredev)) {
+	case DEVICE_MODE_DVBT:
+	case DEVICE_MODE_DVBT_BDA:
+		client->frontend.ops.delsys[0] = SYS_DVBT;
+		break;
+	case DEVICE_MODE_ISDBT:
+	case DEVICE_MODE_ISDBT_BDA:
+		client->frontend.ops.delsys[0] = SYS_ISDBT;
+		break;
+	}
+
+	rc = dvb_register_frontend(&client->adapter, &client->frontend);
+	if (rc < 0) {
+		sms_err("frontend registration failed %d", rc);
+		goto frontend_error;
+	}
+
+	params.initial_id = 1;
+	params.data_type = MSG_SMS_DVBT_BDA_DATA;
+	params.onresponse_handler = smsdvb_onresponse;
+	params.onremove_handler = smsdvb_onremove;
+	params.context = client;
+
+	rc = smscore_register_client(coredev, &params, &client->smsclient);
+	if (rc < 0) {
+		sms_err("smscore_register_client() failed %d", rc);
+		goto client_error;
+	}
+
+	client->coredev = coredev;
+
+	init_completion(&client->tune_done);
+	init_completion(&client->stats_done);
+
+	kmutex_lock(&g_smsdvb_clientslock);
+
+	list_add(&client->entry, &g_smsdvb_clients);
+
+	kmutex_unlock(&g_smsdvb_clientslock);
+
+	client->event_fe_state = -1;
+	client->event_unc_state = -1;
+	sms_board_dvb3_event(client, DVB3_EVENT_HOTPLUG);
+
+	sms_info("success");
+	sms_board_setup(coredev);
+
+	if (smsdvb_debugfs_create(client) < 0)
+		sms_info("failed to create debugfs node");
+
+	return 0;
+
+client_error:
+	dvb_unregister_frontend(&client->frontend);
+
+frontend_error:
+	dvb_dmxdev_release(&client->dmxdev);
+
+dmxdev_error:
+	dvb_dmx_release(&client->demux);
+
+dvbdmx_error:
+	dvb_unregister_adapter(&client->adapter);
+
+adapter_error:
+	kfree(client);
+	return rc;
+}
+
+static int __init smsdvb_module_init(void)
+{
+	int rc;
+
+	INIT_LIST_HEAD(&g_smsdvb_clients);
+	kmutex_init(&g_smsdvb_clientslock);
+
+	smsdvb_debugfs_register();
+
+	rc = smscore_register_hotplug(smsdvb_hotplug);
+
+	sms_debug("");
+
+	return rc;
+}
+
+static void __exit smsdvb_module_exit(void)
+{
+	smscore_unregister_hotplug(smsdvb_hotplug);
+
+	kmutex_lock(&g_smsdvb_clientslock);
+
+	while (!list_empty(&g_smsdvb_clients))
+		smsdvb_unregister_client((struct smsdvb_client_t *)g_smsdvb_clients.next);
+
+	smsdvb_debugfs_unregister();
+
+	kmutex_unlock(&g_smsdvb_clientslock);
+}
+
+module_init(smsdvb_module_init);
+module_exit(smsdvb_module_exit);
+
+MODULE_DESCRIPTION("SMS DVB subsystem adaptation module");
+MODULE_AUTHOR("Siano Mobile Silicon, Inc. (uris@siano-ms.com)");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/common/siano/smsdvb.c b/drivers/media/common/siano/smsdvb.c
deleted file mode 100644
index aa77e54a8fa..00000000000
--- a/drivers/media/common/siano/smsdvb.c
+++ /dev/null
@@ -1,1078 +0,0 @@
-/****************************************************************
-
-Siano Mobile Silicon, Inc.
-MDTV receiver kernel modules.
-Copyright (C) 2006-2008, Uri Shkolnik
-
-This program is free software: you can redistribute it and/or modify
-it under the terms of the GNU General Public License as published by
-the Free Software Foundation, either version 2 of the License, or
-(at your option) any later version.
-
- This program is distributed in the hope that it will be useful,
-but WITHOUT ANY WARRANTY; without even the implied warranty of
-MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-GNU General Public License for more details.
-
-You should have received a copy of the GNU General Public License
-along with this program.  If not, see <http://www.gnu.org/licenses/>.
-
-****************************************************************/
-
-#include <linux/module.h>
-#include <linux/slab.h>
-#include <linux/init.h>
-
-#include "dmxdev.h"
-#include "dvbdev.h"
-#include "dvb_demux.h"
-#include "dvb_frontend.h"
-
-#include "smscoreapi.h"
-#include "smsendian.h"
-#include "sms-cards.h"
-
-DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
-
-struct smsdvb_client_t {
-	struct list_head entry;
-
-	struct smscore_device_t *coredev;
-	struct smscore_client_t *smsclient;
-
-	struct dvb_adapter      adapter;
-	struct dvb_demux        demux;
-	struct dmxdev           dmxdev;
-	struct dvb_frontend     frontend;
-
-	fe_status_t             fe_status;
-
-	struct completion       tune_done;
-
-	struct SMSHOSTLIB_STATISTICS_DVB_S sms_stat_dvb;
-	int event_fe_state;
-	int event_unc_state;
-};
-
-static struct list_head g_smsdvb_clients;
-static struct mutex g_smsdvb_clientslock;
-
-static int sms_dbg;
-module_param_named(debug, sms_dbg, int, 0644);
-MODULE_PARM_DESC(debug, "set debug level (info=1, adv=2 (or-able))");
-
-/* Events that may come from DVB v3 adapter */
-static void sms_board_dvb3_event(struct smsdvb_client_t *client,
-		enum SMS_DVB3_EVENTS event) {
-
-	struct smscore_device_t *coredev = client->coredev;
-	switch (event) {
-	case DVB3_EVENT_INIT:
-		sms_debug("DVB3_EVENT_INIT");
-		sms_board_event(coredev, BOARD_EVENT_BIND);
-		break;
-	case DVB3_EVENT_SLEEP:
-		sms_debug("DVB3_EVENT_SLEEP");
-		sms_board_event(coredev, BOARD_EVENT_POWER_SUSPEND);
-		break;
-	case DVB3_EVENT_HOTPLUG:
-		sms_debug("DVB3_EVENT_HOTPLUG");
-		sms_board_event(coredev, BOARD_EVENT_POWER_INIT);
-		break;
-	case DVB3_EVENT_FE_LOCK:
-		if (client->event_fe_state != DVB3_EVENT_FE_LOCK) {
-			client->event_fe_state = DVB3_EVENT_FE_LOCK;
-			sms_debug("DVB3_EVENT_FE_LOCK");
-			sms_board_event(coredev, BOARD_EVENT_FE_LOCK);
-		}
-		break;
-	case DVB3_EVENT_FE_UNLOCK:
-		if (client->event_fe_state != DVB3_EVENT_FE_UNLOCK) {
-			client->event_fe_state = DVB3_EVENT_FE_UNLOCK;
-			sms_debug("DVB3_EVENT_FE_UNLOCK");
-			sms_board_event(coredev, BOARD_EVENT_FE_UNLOCK);
-		}
-		break;
-	case DVB3_EVENT_UNC_OK:
-		if (client->event_unc_state != DVB3_EVENT_UNC_OK) {
-			client->event_unc_state = DVB3_EVENT_UNC_OK;
-			sms_debug("DVB3_EVENT_UNC_OK");
-			sms_board_event(coredev, BOARD_EVENT_MULTIPLEX_OK);
-		}
-		break;
-	case DVB3_EVENT_UNC_ERR:
-		if (client->event_unc_state != DVB3_EVENT_UNC_ERR) {
-			client->event_unc_state = DVB3_EVENT_UNC_ERR;
-			sms_debug("DVB3_EVENT_UNC_ERR");
-			sms_board_event(coredev, BOARD_EVENT_MULTIPLEX_ERRORS);
-		}
-		break;
-
-	default:
-		sms_err("Unknown dvb3 api event");
-		break;
-	}
-}
-
-
-static void smsdvb_update_dvb_stats(struct RECEPTION_STATISTICS_S *pReceptionData,
-				   struct SMSHOSTLIB_STATISTICS_ST *p)
-{
-	if (sms_dbg & 2) {
-		printk(KERN_DEBUG "Reserved = %d", p->Reserved);
-		printk(KERN_DEBUG "IsRfLocked = %d", p->IsRfLocked);
-		printk(KERN_DEBUG "IsDemodLocked = %d", p->IsDemodLocked);
-		printk(KERN_DEBUG "IsExternalLNAOn = %d", p->IsExternalLNAOn);
-		printk(KERN_DEBUG "SNR = %d", p->SNR);
-		printk(KERN_DEBUG "BER = %d", p->BER);
-		printk(KERN_DEBUG "FIB_CRC = %d", p->FIB_CRC);
-		printk(KERN_DEBUG "TS_PER = %d", p->TS_PER);
-		printk(KERN_DEBUG "MFER = %d", p->MFER);
-		printk(KERN_DEBUG "RSSI = %d", p->RSSI);
-		printk(KERN_DEBUG "InBandPwr = %d", p->InBandPwr);
-		printk(KERN_DEBUG "CarrierOffset = %d", p->CarrierOffset);
-		printk(KERN_DEBUG "Frequency = %d", p->Frequency);
-		printk(KERN_DEBUG "Bandwidth = %d", p->Bandwidth);
-		printk(KERN_DEBUG "TransmissionMode = %d", p->TransmissionMode);
-		printk(KERN_DEBUG "ModemState = %d", p->ModemState);
-		printk(KERN_DEBUG "GuardInterval = %d", p->GuardInterval);
-		printk(KERN_DEBUG "CodeRate = %d", p->CodeRate);
-		printk(KERN_DEBUG "LPCodeRate = %d", p->LPCodeRate);
-		printk(KERN_DEBUG "Hierarchy = %d", p->Hierarchy);
-		printk(KERN_DEBUG "Constellation = %d", p->Constellation);
-		printk(KERN_DEBUG "BurstSize = %d", p->BurstSize);
-		printk(KERN_DEBUG "BurstDuration = %d", p->BurstDuration);
-		printk(KERN_DEBUG "BurstCycleTime = %d", p->BurstCycleTime);
-		printk(KERN_DEBUG "CalculatedBurstCycleTime = %d", p->CalculatedBurstCycleTime);
-		printk(KERN_DEBUG "NumOfRows = %d", p->NumOfRows);
-		printk(KERN_DEBUG "NumOfPaddCols = %d", p->NumOfPaddCols);
-		printk(KERN_DEBUG "NumOfPunctCols = %d", p->NumOfPunctCols);
-		printk(KERN_DEBUG "ErrorTSPackets = %d", p->ErrorTSPackets);
-		printk(KERN_DEBUG "TotalTSPackets = %d", p->TotalTSPackets);
-		printk(KERN_DEBUG "NumOfValidMpeTlbs = %d", p->NumOfValidMpeTlbs);
-		printk(KERN_DEBUG "NumOfInvalidMpeTlbs = %d", p->NumOfInvalidMpeTlbs);
-		printk(KERN_DEBUG "NumOfCorrectedMpeTlbs = %d", p->NumOfCorrectedMpeTlbs);
-		printk(KERN_DEBUG "BERErrorCount = %d", p->BERErrorCount);
-		printk(KERN_DEBUG "BERBitCount = %d", p->BERBitCount);
-		printk(KERN_DEBUG "SmsToHostTxErrors = %d", p->SmsToHostTxErrors);
-		printk(KERN_DEBUG "PreBER = %d", p->PreBER);
-		printk(KERN_DEBUG "CellId = %d", p->CellId);
-		printk(KERN_DEBUG "DvbhSrvIndHP = %d", p->DvbhSrvIndHP);
-		printk(KERN_DEBUG "DvbhSrvIndLP = %d", p->DvbhSrvIndLP);
-		printk(KERN_DEBUG "NumMPEReceived = %d", p->NumMPEReceived);
-	}
-
-	pReceptionData->IsDemodLocked = p->IsDemodLocked;
-
-	pReceptionData->SNR = p->SNR;
-	pReceptionData->BER = p->BER;
-	pReceptionData->BERErrorCount = p->BERErrorCount;
-	pReceptionData->InBandPwr = p->InBandPwr;
-	pReceptionData->ErrorTSPackets = p->ErrorTSPackets;
-};
-
-
-static void smsdvb_update_isdbt_stats(struct RECEPTION_STATISTICS_S *pReceptionData,
-				    struct SMSHOSTLIB_STATISTICS_ISDBT_ST *p)
-{
-	int i;
-
-	if (sms_dbg & 2) {
-		printk(KERN_DEBUG "IsRfLocked = %d", p->IsRfLocked);
-		printk(KERN_DEBUG "IsDemodLocked = %d", p->IsDemodLocked);
-		printk(KERN_DEBUG "IsExternalLNAOn = %d", p->IsExternalLNAOn);
-		printk(KERN_DEBUG "SNR = %d", p->SNR);
-		printk(KERN_DEBUG "RSSI = %d", p->RSSI);
-		printk(KERN_DEBUG "InBandPwr = %d", p->InBandPwr);
-		printk(KERN_DEBUG "CarrierOffset = %d", p->CarrierOffset);
-		printk(KERN_DEBUG "Frequency = %d", p->Frequency);
-		printk(KERN_DEBUG "Bandwidth = %d", p->Bandwidth);
-		printk(KERN_DEBUG "TransmissionMode = %d", p->TransmissionMode);
-		printk(KERN_DEBUG "ModemState = %d", p->ModemState);
-		printk(KERN_DEBUG "GuardInterval = %d", p->GuardInterval);
-		printk(KERN_DEBUG "SystemType = %d", p->SystemType);
-		printk(KERN_DEBUG "PartialReception = %d", p->PartialReception);
-		printk(KERN_DEBUG "NumOfLayers = %d", p->NumOfLayers);
-		printk(KERN_DEBUG "SmsToHostTxErrors = %d", p->SmsToHostTxErrors);
-
-		for (i = 0; i < 3; i++) {
-			printk(KERN_DEBUG "%d: CodeRate = %d", i, p->LayerInfo[i].CodeRate);
-			printk(KERN_DEBUG "%d: Constellation = %d", i, p->LayerInfo[i].Constellation);
-			printk(KERN_DEBUG "%d: BER = %d", i, p->LayerInfo[i].BER);
-			printk(KERN_DEBUG "%d: BERErrorCount = %d", i, p->LayerInfo[i].BERErrorCount);
-			printk(KERN_DEBUG "%d: BERBitCount = %d", i, p->LayerInfo[i].BERBitCount);
-			printk(KERN_DEBUG "%d: PreBER = %d", i, p->LayerInfo[i].PreBER);
-			printk(KERN_DEBUG "%d: TS_PER = %d", i, p->LayerInfo[i].TS_PER);
-			printk(KERN_DEBUG "%d: ErrorTSPackets = %d", i, p->LayerInfo[i].ErrorTSPackets);
-			printk(KERN_DEBUG "%d: TotalTSPackets = %d", i, p->LayerInfo[i].TotalTSPackets);
-			printk(KERN_DEBUG "%d: TILdepthI = %d", i, p->LayerInfo[i].TILdepthI);
-			printk(KERN_DEBUG "%d: NumberOfSegments = %d", i, p->LayerInfo[i].NumberOfSegments);
-			printk(KERN_DEBUG "%d: TMCCErrors = %d", i, p->LayerInfo[i].TMCCErrors);
-		}
-	}
-
-	pReceptionData->IsDemodLocked = p->IsDemodLocked;
-
-	pReceptionData->SNR = p->SNR;
-	pReceptionData->InBandPwr = p->InBandPwr;
-
-	pReceptionData->ErrorTSPackets = 0;
-	pReceptionData->BER = 0;
-	pReceptionData->BERErrorCount = 0;
-	for (i = 0; i < 3; i++) {
-		pReceptionData->BER += p->LayerInfo[i].BER;
-		pReceptionData->BERErrorCount += p->LayerInfo[i].BERErrorCount;
-		pReceptionData->ErrorTSPackets += p->LayerInfo[i].ErrorTSPackets;
-	}
-}
-
-static int smsdvb_onresponse(void *context, struct smscore_buffer_t *cb)
-{
-	struct smsdvb_client_t *client = (struct smsdvb_client_t *) context;
-	struct SmsMsgHdr_ST *phdr = (struct SmsMsgHdr_ST *) (((u8 *) cb->p)
-			+ cb->offset);
-	u32 *pMsgData = (u32 *) phdr + 1;
-	/*u32 MsgDataLen = phdr->msgLength - sizeof(struct SmsMsgHdr_ST);*/
-	bool is_status_update = false;
-
-	smsendian_handle_rx_message((struct SmsMsgData_ST *) phdr);
-
-	switch (phdr->msgType) {
-	case MSG_SMS_DVBT_BDA_DATA:
-		dvb_dmx_swfilter(&client->demux, (u8 *)(phdr + 1),
-				 cb->size - sizeof(struct SmsMsgHdr_ST));
-		break;
-
-	case MSG_SMS_RF_TUNE_RES:
-	case MSG_SMS_ISDBT_TUNE_RES:
-		complete(&client->tune_done);
-		break;
-
-	case MSG_SMS_SIGNAL_DETECTED_IND:
-		sms_info("MSG_SMS_SIGNAL_DETECTED_IND");
-		client->sms_stat_dvb.TransmissionData.IsDemodLocked = true;
-		is_status_update = true;
-		break;
-
-	case MSG_SMS_NO_SIGNAL_IND:
-		sms_info("MSG_SMS_NO_SIGNAL_IND");
-		client->sms_stat_dvb.TransmissionData.IsDemodLocked = false;
-		is_status_update = true;
-		break;
-
-	case MSG_SMS_TRANSMISSION_IND: {
-		sms_info("MSG_SMS_TRANSMISSION_IND");
-
-		pMsgData++;
-		memcpy(&client->sms_stat_dvb.TransmissionData, pMsgData,
-				sizeof(struct TRANSMISSION_STATISTICS_S));
-
-		/* Mo need to correct guard interval
-		 * (as opposed to old statistics message).
-		 */
-		CORRECT_STAT_BANDWIDTH(client->sms_stat_dvb.TransmissionData);
-		CORRECT_STAT_TRANSMISSON_MODE(
-				client->sms_stat_dvb.TransmissionData);
-		is_status_update = true;
-		break;
-	}
-	case MSG_SMS_HO_PER_SLICES_IND: {
-		struct RECEPTION_STATISTICS_S *pReceptionData =
-				&client->sms_stat_dvb.ReceptionData;
-		struct SRVM_SIGNAL_STATUS_S SignalStatusData;
-
-		/*sms_info("MSG_SMS_HO_PER_SLICES_IND");*/
-		pMsgData++;
-		SignalStatusData.result = pMsgData[0];
-		SignalStatusData.snr = pMsgData[1];
-		SignalStatusData.inBandPower = (s32) pMsgData[2];
-		SignalStatusData.tsPackets = pMsgData[3];
-		SignalStatusData.etsPackets = pMsgData[4];
-		SignalStatusData.constellation = pMsgData[5];
-		SignalStatusData.hpCode = pMsgData[6];
-		SignalStatusData.tpsSrvIndLP = pMsgData[7] & 0x03;
-		SignalStatusData.tpsSrvIndHP = pMsgData[8] & 0x03;
-		SignalStatusData.cellId = pMsgData[9] & 0xFFFF;
-		SignalStatusData.reason = pMsgData[10];
-		SignalStatusData.requestId = pMsgData[11];
-		pReceptionData->IsRfLocked = pMsgData[16];
-		pReceptionData->IsDemodLocked = pMsgData[17];
-		pReceptionData->ModemState = pMsgData[12];
-		pReceptionData->SNR = pMsgData[1];
-		pReceptionData->BER = pMsgData[13];
-		pReceptionData->RSSI = pMsgData[14];
-		CORRECT_STAT_RSSI(client->sms_stat_dvb.ReceptionData);
-
-		pReceptionData->InBandPwr = (s32) pMsgData[2];
-		pReceptionData->CarrierOffset = (s32) pMsgData[15];
-		pReceptionData->TotalTSPackets = pMsgData[3];
-		pReceptionData->ErrorTSPackets = pMsgData[4];
-
-		/* TS PER */
-		if ((SignalStatusData.tsPackets + SignalStatusData.etsPackets)
-				> 0) {
-			pReceptionData->TS_PER = (SignalStatusData.etsPackets
-					* 100) / (SignalStatusData.tsPackets
-					+ SignalStatusData.etsPackets);
-		} else {
-			pReceptionData->TS_PER = 0;
-		}
-
-		pReceptionData->BERBitCount = pMsgData[18];
-		pReceptionData->BERErrorCount = pMsgData[19];
-
-		pReceptionData->MRC_SNR = pMsgData[20];
-		pReceptionData->MRC_InBandPwr = pMsgData[21];
-		pReceptionData->MRC_RSSI = pMsgData[22];
-
-		is_status_update = true;
-		break;
-	}
-	case MSG_SMS_GET_STATISTICS_RES: {
-		union {
-			struct SMSHOSTLIB_STATISTICS_ISDBT_ST  isdbt;
-			struct SmsMsgStatisticsInfo_ST         dvb;
-		} *p = (void *) (phdr + 1);
-		struct RECEPTION_STATISTICS_S *pReceptionData =
-				&client->sms_stat_dvb.ReceptionData;
-
-		sms_info("MSG_SMS_GET_STATISTICS_RES");
-
-		is_status_update = true;
-
-		switch (smscore_get_device_mode(client->coredev)) {
-		case DEVICE_MODE_ISDBT:
-		case DEVICE_MODE_ISDBT_BDA:
-			smsdvb_update_isdbt_stats(pReceptionData, &p->isdbt);
-			break;
-		default:
-			smsdvb_update_dvb_stats(pReceptionData, &p->dvb.Stat);
-		}
-		if (!pReceptionData->IsDemodLocked) {
-			pReceptionData->SNR = 0;
-			pReceptionData->BER = 0;
-			pReceptionData->BERErrorCount = 0;
-			pReceptionData->InBandPwr = 0;
-			pReceptionData->ErrorTSPackets = 0;
-		}
-
-		complete(&client->tune_done);
-		break;
-	}
-	default:
-		sms_info("Unhandled message %d", phdr->msgType);
-
-	}
-	smscore_putbuffer(client->coredev, cb);
-
-	if (is_status_update) {
-		if (client->sms_stat_dvb.ReceptionData.IsDemodLocked) {
-			client->fe_status = FE_HAS_SIGNAL | FE_HAS_CARRIER
-				| FE_HAS_VITERBI | FE_HAS_SYNC | FE_HAS_LOCK;
-			sms_board_dvb3_event(client, DVB3_EVENT_FE_LOCK);
-			if (client->sms_stat_dvb.ReceptionData.ErrorTSPackets
-					== 0)
-				sms_board_dvb3_event(client, DVB3_EVENT_UNC_OK);
-			else
-				sms_board_dvb3_event(client,
-						DVB3_EVENT_UNC_ERR);
-
-		} else {
-			if (client->sms_stat_dvb.ReceptionData.IsRfLocked)
-				client->fe_status = FE_HAS_SIGNAL | FE_HAS_CARRIER;
-			else
-				client->fe_status = 0;
-			sms_board_dvb3_event(client, DVB3_EVENT_FE_UNLOCK);
-		}
-	}
-
-	return 0;
-}
-
-static void smsdvb_unregister_client(struct smsdvb_client_t *client)
-{
-	/* must be called under clientslock */
-
-	list_del(&client->entry);
-
-	smscore_unregister_client(client->smsclient);
-	dvb_unregister_frontend(&client->frontend);
-	dvb_dmxdev_release(&client->dmxdev);
-	dvb_dmx_release(&client->demux);
-	dvb_unregister_adapter(&client->adapter);
-	kfree(client);
-}
-
-static void smsdvb_onremove(void *context)
-{
-	kmutex_lock(&g_smsdvb_clientslock);
-
-	smsdvb_unregister_client((struct smsdvb_client_t *) context);
-
-	kmutex_unlock(&g_smsdvb_clientslock);
-}
-
-static int smsdvb_start_feed(struct dvb_demux_feed *feed)
-{
-	struct smsdvb_client_t *client =
-		container_of(feed->demux, struct smsdvb_client_t, demux);
-	struct SmsMsgData_ST PidMsg;
-
-	sms_debug("add pid %d(%x)",
-		  feed->pid, feed->pid);
-
-	PidMsg.xMsgHeader.msgSrcId = DVBT_BDA_CONTROL_MSG_ID;
-	PidMsg.xMsgHeader.msgDstId = HIF_TASK;
-	PidMsg.xMsgHeader.msgFlags = 0;
-	PidMsg.xMsgHeader.msgType  = MSG_SMS_ADD_PID_FILTER_REQ;
-	PidMsg.xMsgHeader.msgLength = sizeof(PidMsg);
-	PidMsg.msgData[0] = feed->pid;
-
-	smsendian_handle_tx_message((struct SmsMsgHdr_ST *)&PidMsg);
-	return smsclient_sendrequest(client->smsclient,
-				     &PidMsg, sizeof(PidMsg));
-}
-
-static int smsdvb_stop_feed(struct dvb_demux_feed *feed)
-{
-	struct smsdvb_client_t *client =
-		container_of(feed->demux, struct smsdvb_client_t, demux);
-	struct SmsMsgData_ST PidMsg;
-
-	sms_debug("remove pid %d(%x)",
-		  feed->pid, feed->pid);
-
-	PidMsg.xMsgHeader.msgSrcId = DVBT_BDA_CONTROL_MSG_ID;
-	PidMsg.xMsgHeader.msgDstId = HIF_TASK;
-	PidMsg.xMsgHeader.msgFlags = 0;
-	PidMsg.xMsgHeader.msgType  = MSG_SMS_REMOVE_PID_FILTER_REQ;
-	PidMsg.xMsgHeader.msgLength = sizeof(PidMsg);
-	PidMsg.msgData[0] = feed->pid;
-
-	smsendian_handle_tx_message((struct SmsMsgHdr_ST *)&PidMsg);
-	return smsclient_sendrequest(client->smsclient,
-				     &PidMsg, sizeof(PidMsg));
-}
-
-static int smsdvb_sendrequest_and_wait(struct smsdvb_client_t *client,
-					void *buffer, size_t size,
-					struct completion *completion)
-{
-	int rc;
-
-	smsendian_handle_tx_message((struct SmsMsgHdr_ST *)buffer);
-	rc = smsclient_sendrequest(client->smsclient, buffer, size);
-	if (rc < 0)
-		return rc;
-
-	return wait_for_completion_timeout(completion,
-					   msecs_to_jiffies(2000)) ?
-						0 : -ETIME;
-}
-
-static int smsdvb_send_statistics_request(struct smsdvb_client_t *client)
-{
-	int rc;
-	struct SmsMsgHdr_ST Msg = { MSG_SMS_GET_STATISTICS_REQ,
-				    DVBT_BDA_CONTROL_MSG_ID,
-				    HIF_TASK,
-				    sizeof(struct SmsMsgHdr_ST), 0 };
-
-	rc = smsdvb_sendrequest_and_wait(client, &Msg, sizeof(Msg),
-					  &client->tune_done);
-
-	return rc;
-}
-
-static inline int led_feedback(struct smsdvb_client_t *client)
-{
-	if (client->fe_status & FE_HAS_LOCK)
-		return sms_board_led_feedback(client->coredev,
-			(client->sms_stat_dvb.ReceptionData.BER
-			== 0) ? SMS_LED_HI : SMS_LED_LO);
-	else
-		return sms_board_led_feedback(client->coredev, SMS_LED_OFF);
-}
-
-static int smsdvb_read_status(struct dvb_frontend *fe, fe_status_t *stat)
-{
-	int rc;
-	struct smsdvb_client_t *client;
-	client = container_of(fe, struct smsdvb_client_t, frontend);
-
-	rc = smsdvb_send_statistics_request(client);
-
-	*stat = client->fe_status;
-
-	led_feedback(client);
-
-	return rc;
-}
-
-static int smsdvb_read_ber(struct dvb_frontend *fe, u32 *ber)
-{
-	int rc;
-	struct smsdvb_client_t *client;
-	client = container_of(fe, struct smsdvb_client_t, frontend);
-
-	rc = smsdvb_send_statistics_request(client);
-
-	*ber = client->sms_stat_dvb.ReceptionData.BER;
-
-	led_feedback(client);
-
-	return rc;
-}
-
-static int smsdvb_read_signal_strength(struct dvb_frontend *fe, u16 *strength)
-{
-	int rc;
-
-	struct smsdvb_client_t *client;
-	client = container_of(fe, struct smsdvb_client_t, frontend);
-
-	rc = smsdvb_send_statistics_request(client);
-
-	if (client->sms_stat_dvb.ReceptionData.InBandPwr < -95)
-		*strength = 0;
-		else if (client->sms_stat_dvb.ReceptionData.InBandPwr > -29)
-			*strength = 100;
-		else
-			*strength =
-				(client->sms_stat_dvb.ReceptionData.InBandPwr
-				+ 95) * 3 / 2;
-
-	led_feedback(client);
-
-	return rc;
-}
-
-static int smsdvb_read_snr(struct dvb_frontend *fe, u16 *snr)
-{
-	int rc;
-	struct smsdvb_client_t *client;
-	client = container_of(fe, struct smsdvb_client_t, frontend);
-
-	rc = smsdvb_send_statistics_request(client);
-
-	*snr = client->sms_stat_dvb.ReceptionData.SNR;
-
-	led_feedback(client);
-
-	return rc;
-}
-
-static int smsdvb_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
-{
-	int rc;
-	struct smsdvb_client_t *client;
-	client = container_of(fe, struct smsdvb_client_t, frontend);
-
-	rc = smsdvb_send_statistics_request(client);
-
-	*ucblocks = client->sms_stat_dvb.ReceptionData.ErrorTSPackets;
-
-	led_feedback(client);
-
-	return rc;
-}
-
-static int smsdvb_get_tune_settings(struct dvb_frontend *fe,
-				    struct dvb_frontend_tune_settings *tune)
-{
-	sms_debug("");
-
-	tune->min_delay_ms = 400;
-	tune->step_size = 250000;
-	tune->max_drift = 0;
-	return 0;
-}
-
-static int smsdvb_dvbt_set_frontend(struct dvb_frontend *fe)
-{
-	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-	struct smsdvb_client_t *client =
-		container_of(fe, struct smsdvb_client_t, frontend);
-
-	struct {
-		struct SmsMsgHdr_ST	Msg;
-		u32		Data[3];
-	} Msg;
-
-	int ret;
-
-	client->fe_status = FE_HAS_SIGNAL;
-	client->event_fe_state = -1;
-	client->event_unc_state = -1;
-	fe->dtv_property_cache.delivery_system = SYS_DVBT;
-
-	Msg.Msg.msgSrcId = DVBT_BDA_CONTROL_MSG_ID;
-	Msg.Msg.msgDstId = HIF_TASK;
-	Msg.Msg.msgFlags = 0;
-	Msg.Msg.msgType = MSG_SMS_RF_TUNE_REQ;
-	Msg.Msg.msgLength = sizeof(Msg);
-	Msg.Data[0] = c->frequency;
-	Msg.Data[2] = 12000000;
-
-	sms_info("%s: freq %d band %d", __func__, c->frequency,
-		 c->bandwidth_hz);
-
-	switch (c->bandwidth_hz / 1000000) {
-	case 8:
-		Msg.Data[1] = BW_8_MHZ;
-		break;
-	case 7:
-		Msg.Data[1] = BW_7_MHZ;
-		break;
-	case 6:
-		Msg.Data[1] = BW_6_MHZ;
-		break;
-	case 0:
-		return -EOPNOTSUPP;
-	default:
-		return -EINVAL;
-	}
-	/* Disable LNA, if any. An error is returned if no LNA is present */
-	ret = sms_board_lna_control(client->coredev, 0);
-	if (ret == 0) {
-		fe_status_t status;
-
-		/* tune with LNA off at first */
-		ret = smsdvb_sendrequest_and_wait(client, &Msg, sizeof(Msg),
-						  &client->tune_done);
-
-		smsdvb_read_status(fe, &status);
-
-		if (status & FE_HAS_LOCK)
-			return ret;
-
-		/* previous tune didn't lock - enable LNA and tune again */
-		sms_board_lna_control(client->coredev, 1);
-	}
-
-	return smsdvb_sendrequest_and_wait(client, &Msg, sizeof(Msg),
-					   &client->tune_done);
-}
-
-static int smsdvb_isdbt_set_frontend(struct dvb_frontend *fe)
-{
-	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-	struct smsdvb_client_t *client =
-		container_of(fe, struct smsdvb_client_t, frontend);
-
-	struct {
-		struct SmsMsgHdr_ST	Msg;
-		u32		Data[4];
-	} Msg;
-
-	fe->dtv_property_cache.delivery_system = SYS_ISDBT;
-
-	Msg.Msg.msgSrcId  = DVBT_BDA_CONTROL_MSG_ID;
-	Msg.Msg.msgDstId  = HIF_TASK;
-	Msg.Msg.msgFlags  = 0;
-	Msg.Msg.msgType   = MSG_SMS_ISDBT_TUNE_REQ;
-	Msg.Msg.msgLength = sizeof(Msg);
-
-	if (c->isdbt_sb_segment_idx == -1)
-		c->isdbt_sb_segment_idx = 0;
-
-	switch (c->isdbt_sb_segment_count) {
-	case 3:
-		Msg.Data[1] = BW_ISDBT_3SEG;
-		break;
-	case 1:
-		Msg.Data[1] = BW_ISDBT_1SEG;
-		break;
-	case 0:	/* AUTO */
-		switch (c->bandwidth_hz / 1000000) {
-		case 8:
-		case 7:
-			c->isdbt_sb_segment_count = 3;
-			Msg.Data[1] = BW_ISDBT_3SEG;
-			break;
-		case 6:
-			c->isdbt_sb_segment_count = 1;
-			Msg.Data[1] = BW_ISDBT_1SEG;
-			break;
-		default: /* Assumes 6 MHZ bw */
-			c->isdbt_sb_segment_count = 1;
-			c->bandwidth_hz = 6000;
-			Msg.Data[1] = BW_ISDBT_1SEG;
-			break;
-		}
-		break;
-	default:
-		sms_info("Segment count %d not supported", c->isdbt_sb_segment_count);
-		return -EINVAL;
-	}
-
-	Msg.Data[0] = c->frequency;
-	Msg.Data[2] = 12000000;
-	Msg.Data[3] = c->isdbt_sb_segment_idx;
-
-	sms_info("%s: freq %d segwidth %d segindex %d\n", __func__,
-		 c->frequency, c->isdbt_sb_segment_count,
-		 c->isdbt_sb_segment_idx);
-
-	return smsdvb_sendrequest_and_wait(client, &Msg, sizeof(Msg),
-					   &client->tune_done);
-}
-
-static int smsdvb_set_frontend(struct dvb_frontend *fe)
-{
-	struct smsdvb_client_t *client =
-		container_of(fe, struct smsdvb_client_t, frontend);
-	struct smscore_device_t *coredev = client->coredev;
-
-	switch (smscore_get_device_mode(coredev)) {
-	case DEVICE_MODE_DVBT:
-	case DEVICE_MODE_DVBT_BDA:
-		return smsdvb_dvbt_set_frontend(fe);
-	case DEVICE_MODE_ISDBT:
-	case DEVICE_MODE_ISDBT_BDA:
-		return smsdvb_isdbt_set_frontend(fe);
-	default:
-		return -EINVAL;
-	}
-}
-
-static int smsdvb_get_frontend(struct dvb_frontend *fe)
-{
-	struct dtv_frontend_properties *fep = &fe->dtv_property_cache;
-	struct smsdvb_client_t *client =
-		container_of(fe, struct smsdvb_client_t, frontend);
-	struct smscore_device_t *coredev = client->coredev;
-	struct TRANSMISSION_STATISTICS_S *td =
-		&client->sms_stat_dvb.TransmissionData;
-
-	switch (smscore_get_device_mode(coredev)) {
-	case DEVICE_MODE_DVBT:
-	case DEVICE_MODE_DVBT_BDA:
-		fep->frequency = td->Frequency;
-
-		switch (td->Bandwidth) {
-		case 6:
-			fep->bandwidth_hz = 6000000;
-			break;
-		case 7:
-			fep->bandwidth_hz = 7000000;
-			break;
-		case 8:
-			fep->bandwidth_hz = 8000000;
-			break;
-		}
-
-		switch (td->TransmissionMode) {
-		case 2:
-			fep->transmission_mode = TRANSMISSION_MODE_2K;
-			break;
-		case 8:
-			fep->transmission_mode = TRANSMISSION_MODE_8K;
-		}
-
-		switch (td->GuardInterval) {
-		case 0:
-			fep->guard_interval = GUARD_INTERVAL_1_32;
-			break;
-		case 1:
-			fep->guard_interval = GUARD_INTERVAL_1_16;
-			break;
-		case 2:
-			fep->guard_interval = GUARD_INTERVAL_1_8;
-			break;
-		case 3:
-			fep->guard_interval = GUARD_INTERVAL_1_4;
-			break;
-		}
-
-		switch (td->CodeRate) {
-		case 0:
-			fep->code_rate_HP = FEC_1_2;
-			break;
-		case 1:
-			fep->code_rate_HP = FEC_2_3;
-			break;
-		case 2:
-			fep->code_rate_HP = FEC_3_4;
-			break;
-		case 3:
-			fep->code_rate_HP = FEC_5_6;
-			break;
-		case 4:
-			fep->code_rate_HP = FEC_7_8;
-			break;
-		}
-
-		switch (td->LPCodeRate) {
-		case 0:
-			fep->code_rate_LP = FEC_1_2;
-			break;
-		case 1:
-			fep->code_rate_LP = FEC_2_3;
-			break;
-		case 2:
-			fep->code_rate_LP = FEC_3_4;
-			break;
-		case 3:
-			fep->code_rate_LP = FEC_5_6;
-			break;
-		case 4:
-			fep->code_rate_LP = FEC_7_8;
-			break;
-		}
-
-		switch (td->Constellation) {
-		case 0:
-			fep->modulation = QPSK;
-			break;
-		case 1:
-			fep->modulation = QAM_16;
-			break;
-		case 2:
-			fep->modulation = QAM_64;
-			break;
-		}
-
-		switch (td->Hierarchy) {
-		case 0:
-			fep->hierarchy = HIERARCHY_NONE;
-			break;
-		case 1:
-			fep->hierarchy = HIERARCHY_1;
-			break;
-		case 2:
-			fep->hierarchy = HIERARCHY_2;
-			break;
-		case 3:
-			fep->hierarchy = HIERARCHY_4;
-			break;
-		}
-
-		fep->inversion = INVERSION_AUTO;
-		break;
-	case DEVICE_MODE_ISDBT:
-	case DEVICE_MODE_ISDBT_BDA:
-		fep->frequency = td->Frequency;
-		fep->bandwidth_hz = 6000000;
-		/* todo: retrive the other parameters */
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static int smsdvb_init(struct dvb_frontend *fe)
-{
-	struct smsdvb_client_t *client =
-		container_of(fe, struct smsdvb_client_t, frontend);
-
-	sms_board_power(client->coredev, 1);
-
-	sms_board_dvb3_event(client, DVB3_EVENT_INIT);
-	return 0;
-}
-
-static int smsdvb_sleep(struct dvb_frontend *fe)
-{
-	struct smsdvb_client_t *client =
-		container_of(fe, struct smsdvb_client_t, frontend);
-
-	sms_board_led_feedback(client->coredev, SMS_LED_OFF);
-	sms_board_power(client->coredev, 0);
-
-	sms_board_dvb3_event(client, DVB3_EVENT_SLEEP);
-
-	return 0;
-}
-
-static void smsdvb_release(struct dvb_frontend *fe)
-{
-	/* do nothing */
-}
-
-static struct dvb_frontend_ops smsdvb_fe_ops = {
-	.info = {
-		.name			= "Siano Mobile Digital MDTV Receiver",
-		.frequency_min		= 44250000,
-		.frequency_max		= 867250000,
-		.frequency_stepsize	= 250000,
-		.caps = FE_CAN_INVERSION_AUTO |
-			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_RECOVER |
-			FE_CAN_HIERARCHY_AUTO,
-	},
-
-	.release = smsdvb_release,
-
-	.set_frontend = smsdvb_set_frontend,
-	.get_frontend = smsdvb_get_frontend,
-	.get_tune_settings = smsdvb_get_tune_settings,
-
-	.read_status = smsdvb_read_status,
-	.read_ber = smsdvb_read_ber,
-	.read_signal_strength = smsdvb_read_signal_strength,
-	.read_snr = smsdvb_read_snr,
-	.read_ucblocks = smsdvb_read_ucblocks,
-
-	.init = smsdvb_init,
-	.sleep = smsdvb_sleep,
-};
-
-static int smsdvb_hotplug(struct smscore_device_t *coredev,
-			  struct device *device, int arrival)
-{
-	struct smsclient_params_t params;
-	struct smsdvb_client_t *client;
-	int rc;
-
-	/* device removal handled by onremove callback */
-	if (!arrival)
-		return 0;
-	client = kzalloc(sizeof(struct smsdvb_client_t), GFP_KERNEL);
-	if (!client) {
-		sms_err("kmalloc() failed");
-		return -ENOMEM;
-	}
-
-	/* register dvb adapter */
-	rc = dvb_register_adapter(&client->adapter,
-				  sms_get_board(
-					smscore_get_board_id(coredev))->name,
-				  THIS_MODULE, device, adapter_nr);
-	if (rc < 0) {
-		sms_err("dvb_register_adapter() failed %d", rc);
-		goto adapter_error;
-	}
-
-	/* init dvb demux */
-	client->demux.dmx.capabilities = DMX_TS_FILTERING;
-	client->demux.filternum = 32; /* todo: nova ??? */
-	client->demux.feednum = 32;
-	client->demux.start_feed = smsdvb_start_feed;
-	client->demux.stop_feed = smsdvb_stop_feed;
-
-	rc = dvb_dmx_init(&client->demux);
-	if (rc < 0) {
-		sms_err("dvb_dmx_init failed %d", rc);
-		goto dvbdmx_error;
-	}
-
-	/* init dmxdev */
-	client->dmxdev.filternum = 32;
-	client->dmxdev.demux = &client->demux.dmx;
-	client->dmxdev.capabilities = 0;
-
-	rc = dvb_dmxdev_init(&client->dmxdev, &client->adapter);
-	if (rc < 0) {
-		sms_err("dvb_dmxdev_init failed %d", rc);
-		goto dmxdev_error;
-	}
-
-	/* init and register frontend */
-	memcpy(&client->frontend.ops, &smsdvb_fe_ops,
-	       sizeof(struct dvb_frontend_ops));
-
-	switch (smscore_get_device_mode(coredev)) {
-	case DEVICE_MODE_DVBT:
-	case DEVICE_MODE_DVBT_BDA:
-		client->frontend.ops.delsys[0] = SYS_DVBT;
-		break;
-	case DEVICE_MODE_ISDBT:
-	case DEVICE_MODE_ISDBT_BDA:
-		client->frontend.ops.delsys[0] = SYS_ISDBT;
-		break;
-	}
-
-	rc = dvb_register_frontend(&client->adapter, &client->frontend);
-	if (rc < 0) {
-		sms_err("frontend registration failed %d", rc);
-		goto frontend_error;
-	}
-
-	params.initial_id = 1;
-	params.data_type = MSG_SMS_DVBT_BDA_DATA;
-	params.onresponse_handler = smsdvb_onresponse;
-	params.onremove_handler = smsdvb_onremove;
-	params.context = client;
-
-	rc = smscore_register_client(coredev, &params, &client->smsclient);
-	if (rc < 0) {
-		sms_err("smscore_register_client() failed %d", rc);
-		goto client_error;
-	}
-
-	client->coredev = coredev;
-
-	init_completion(&client->tune_done);
-
-	kmutex_lock(&g_smsdvb_clientslock);
-
-	list_add(&client->entry, &g_smsdvb_clients);
-
-	kmutex_unlock(&g_smsdvb_clientslock);
-
-	client->event_fe_state = -1;
-	client->event_unc_state = -1;
-	sms_board_dvb3_event(client, DVB3_EVENT_HOTPLUG);
-
-	sms_info("success");
-	sms_board_setup(coredev);
-
-	return 0;
-
-client_error:
-	dvb_unregister_frontend(&client->frontend);
-
-frontend_error:
-	dvb_dmxdev_release(&client->dmxdev);
-
-dmxdev_error:
-	dvb_dmx_release(&client->demux);
-
-dvbdmx_error:
-	dvb_unregister_adapter(&client->adapter);
-
-adapter_error:
-	kfree(client);
-	return rc;
-}
-
-static int __init smsdvb_module_init(void)
-{
-	int rc;
-
-	INIT_LIST_HEAD(&g_smsdvb_clients);
-	kmutex_init(&g_smsdvb_clientslock);
-
-	rc = smscore_register_hotplug(smsdvb_hotplug);
-
-	sms_debug("");
-
-	return rc;
-}
-
-static void __exit smsdvb_module_exit(void)
-{
-	smscore_unregister_hotplug(smsdvb_hotplug);
-
-	kmutex_lock(&g_smsdvb_clientslock);
-
-	while (!list_empty(&g_smsdvb_clients))
-	       smsdvb_unregister_client(
-			(struct smsdvb_client_t *) g_smsdvb_clients.next);
-
-	kmutex_unlock(&g_smsdvb_clientslock);
-}
-
-module_init(smsdvb_module_init);
-module_exit(smsdvb_module_exit);
-
-MODULE_DESCRIPTION("SMS DVB subsystem adaptation module");
-MODULE_AUTHOR("Siano Mobile Silicon, Inc. (uris@siano-ms.com)");
-MODULE_LICENSE("GPL");
diff --git a/drivers/media/common/siano/smsdvb.h b/drivers/media/common/siano/smsdvb.h
new file mode 100644
index 00000000000..92c413ba0c7
--- /dev/null
+++ b/drivers/media/common/siano/smsdvb.h
@@ -0,0 +1,130 @@
+/***********************************************************************
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+
+ *  This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ ***********************************************************************/
+
+struct smsdvb_debugfs;
+struct smsdvb_client_t;
+
+typedef void (*sms_prt_dvb_stats_t)(struct smsdvb_debugfs *debug_data,
+				    struct sms_stats *p);
+
+typedef void (*sms_prt_isdb_stats_t)(struct smsdvb_debugfs *debug_data,
+				     struct sms_isdbt_stats *p);
+
+typedef void (*sms_prt_isdb_stats_ex_t)
+			(struct smsdvb_debugfs *debug_data,
+			 struct sms_isdbt_stats_ex *p);
+
+
+struct smsdvb_client_t {
+	struct list_head entry;
+
+	struct smscore_device_t *coredev;
+	struct smscore_client_t *smsclient;
+
+	struct dvb_adapter      adapter;
+	struct dvb_demux        demux;
+	struct dmxdev           dmxdev;
+	struct dvb_frontend     frontend;
+
+	fe_status_t             fe_status;
+
+	struct completion       tune_done;
+	struct completion       stats_done;
+
+	int last_per;
+
+	int legacy_ber, legacy_per;
+
+	int event_fe_state;
+	int event_unc_state;
+
+	unsigned long		get_stats_jiffies;
+
+	int			feed_users;
+	bool			has_tuned;
+
+	/* stats debugfs data */
+	struct dentry		*debugfs;
+
+	struct smsdvb_debugfs	*debug_data;
+
+	sms_prt_dvb_stats_t	prt_dvb_stats;
+	sms_prt_isdb_stats_t	prt_isdb_stats;
+	sms_prt_isdb_stats_ex_t	prt_isdb_stats_ex;
+};
+
+/*
+ * This struct is a mix of struct sms_rx_stats_ex and
+ * struct sms_srvm_signal_status.
+ * It was obtained by comparing the way it was filled by the original code
+ */
+struct RECEPTION_STATISTICS_PER_SLICES_S {
+	u32 result;
+	u32 snr;
+	s32 in_band_power;
+	u32 ts_packets;
+	u32 ets_packets;
+	u32 constellation;
+	u32 hp_code;
+	u32 tps_srv_ind_lp;
+	u32 tps_srv_ind_hp;
+	u32 cell_id;
+	u32 reason;
+	u32 request_id;
+	u32 modem_state;		/* from SMSHOSTLIB_DVB_MODEM_STATE_ET */
+
+	u32 ber;		/* Post Viterbi BER [1E-5] */
+	s32 RSSI;		/* dBm */
+	s32 carrier_offset;	/* Carrier Offset in bin/1024 */
+
+	u32 is_rf_locked;		/* 0 - not locked, 1 - locked */
+	u32 is_demod_locked;	/* 0 - not locked, 1 - locked */
+
+	u32 ber_bit_count;	/* Total number of SYNC bits. */
+	u32 ber_error_count;	/* Number of erronous SYNC bits. */
+
+	s32 MRC_SNR;		/* dB */
+	s32 mrc_in_band_pwr;	/* In band power in dBM */
+	s32 MRC_RSSI;		/* dBm */
+};
+
+/* From smsdvb-debugfs.c */
+#ifdef CONFIG_SMS_SIANO_DEBUGFS
+
+int smsdvb_debugfs_create(struct smsdvb_client_t *client);
+void smsdvb_debugfs_release(struct smsdvb_client_t *client);
+int smsdvb_debugfs_register(void);
+void smsdvb_debugfs_unregister(void);
+
+#else
+
+static inline int smsdvb_debugfs_create(struct smsdvb_client_t *client)
+{
+	return 0;
+}
+
+static inline void smsdvb_debugfs_release(struct smsdvb_client_t *client) {}
+
+static inline int smsdvb_debugfs_register(void)
+{
+	return 0;
+};
+
+static inline void smsdvb_debugfs_unregister(void) {};
+
+#endif
+
diff --git a/drivers/media/common/siano/smsendian.c b/drivers/media/common/siano/smsendian.c
index e2657c2f010..bfe831c10b1 100644
--- a/drivers/media/common/siano/smsendian.c
+++ b/drivers/media/common/siano/smsendian.c
@@ -28,23 +28,23 @@
 void smsendian_handle_tx_message(void *buffer)
 {
 #ifdef __BIG_ENDIAN
-	struct SmsMsgData_ST *msg = (struct SmsMsgData_ST *)buffer;
+	struct sms_msg_data *msg = (struct sms_msg_data *)buffer;
 	int i;
-	int msgWords;
+	int msg_words;
 
-	switch (msg->xMsgHeader.msgType) {
+	switch (msg->x_msg_header.msg_type) {
 	case MSG_SMS_DATA_DOWNLOAD_REQ:
 	{
-		msg->msgData[0] = le32_to_cpu(msg->msgData[0]);
+		msg->msg_data[0] = le32_to_cpu(msg->msg_data[0]);
 		break;
 	}
 
 	default:
-		msgWords = (msg->xMsgHeader.msgLength -
-				sizeof(struct SmsMsgHdr_ST))/4;
+		msg_words = (msg->x_msg_header.msg_length -
+				sizeof(struct sms_msg_hdr))/4;
 
-		for (i = 0; i < msgWords; i++)
-			msg->msgData[i] = le32_to_cpu(msg->msgData[i]);
+		for (i = 0; i < msg_words; i++)
+			msg->msg_data[i] = le32_to_cpu(msg->msg_data[i]);
 
 		break;
 	}
@@ -55,16 +55,16 @@ EXPORT_SYMBOL_GPL(smsendian_handle_tx_message);
 void smsendian_handle_rx_message(void *buffer)
 {
 #ifdef __BIG_ENDIAN
-	struct SmsMsgData_ST *msg = (struct SmsMsgData_ST *)buffer;
+	struct sms_msg_data *msg = (struct sms_msg_data *)buffer;
 	int i;
-	int msgWords;
+	int msg_words;
 
-	switch (msg->xMsgHeader.msgType) {
+	switch (msg->x_msg_header.msg_type) {
 	case MSG_SMS_GET_VERSION_EX_RES:
 	{
-		struct SmsVersionRes_ST *ver =
-			(struct SmsVersionRes_ST *) msg;
-		ver->ChipModel = le16_to_cpu(ver->ChipModel);
+		struct sms_version_res *ver =
+			(struct sms_version_res *) msg;
+		ver->chip_model = le16_to_cpu(ver->chip_model);
 		break;
 	}
 
@@ -77,11 +77,11 @@ void smsendian_handle_rx_message(void *buffer)
 
 	default:
 	{
-		msgWords = (msg->xMsgHeader.msgLength -
-				sizeof(struct SmsMsgHdr_ST))/4;
+		msg_words = (msg->x_msg_header.msg_length -
+				sizeof(struct sms_msg_hdr))/4;
 
-		for (i = 0; i < msgWords; i++)
-			msg->msgData[i] = le32_to_cpu(msg->msgData[i]);
+		for (i = 0; i < msg_words; i++)
+			msg->msg_data[i] = le32_to_cpu(msg->msg_data[i]);
 
 		break;
 	}
@@ -93,11 +93,11 @@ EXPORT_SYMBOL_GPL(smsendian_handle_rx_message);
 void smsendian_handle_message_header(void *msg)
 {
 #ifdef __BIG_ENDIAN
-	struct SmsMsgHdr_ST *phdr = (struct SmsMsgHdr_ST *)msg;
+	struct sms_msg_hdr *phdr = (struct sms_msg_hdr *)msg;
 
-	phdr->msgType = le16_to_cpu(phdr->msgType);
-	phdr->msgLength = le16_to_cpu(phdr->msgLength);
-	phdr->msgFlags = le16_to_cpu(phdr->msgFlags);
+	phdr->msg_type = le16_to_cpu(phdr->msg_type);
+	phdr->msg_length = le16_to_cpu(phdr->msg_length);
+	phdr->msg_flags = le16_to_cpu(phdr->msg_flags);
 #endif /* __BIG_ENDIAN */
 }
 EXPORT_SYMBOL_GPL(smsendian_handle_message_header);
diff --git a/drivers/media/common/siano/smsir.h b/drivers/media/common/siano/smsir.h
index 69b59b9eee2..fc8b7925c53 100644
--- a/drivers/media/common/siano/smsir.h
+++ b/drivers/media/common/siano/smsir.h
@@ -40,7 +40,6 @@ struct ir_t {
 	char phys[32];
 
 	char *rc_codes;
-	u64 protocol;
 
 	u32 timeout;
 	u32 controller;
diff --git a/drivers/media/dvb-core/demux.h b/drivers/media/dvb-core/demux.h
index eb91fd808c1..833191bcd81 100644
--- a/drivers/media/dvb-core/demux.h
+++ b/drivers/media/dvb-core/demux.h
@@ -83,45 +83,6 @@ enum dmx_success {
 #define TS_DEMUX        8   /* in case TS_PACKET is set, send the TS to
 			       the demux device, not to the dvr device */
 
-/* PES type for filters which write to built-in decoder */
-/* these should be kept identical to the types in dmx.h */
-
-enum dmx_ts_pes
-{  /* also send packets to decoder (if it exists) */
-	DMX_TS_PES_AUDIO0,
-	DMX_TS_PES_VIDEO0,
-	DMX_TS_PES_TELETEXT0,
-	DMX_TS_PES_SUBTITLE0,
-	DMX_TS_PES_PCR0,
-
-	DMX_TS_PES_AUDIO1,
-	DMX_TS_PES_VIDEO1,
-	DMX_TS_PES_TELETEXT1,
-	DMX_TS_PES_SUBTITLE1,
-	DMX_TS_PES_PCR1,
-
-	DMX_TS_PES_AUDIO2,
-	DMX_TS_PES_VIDEO2,
-	DMX_TS_PES_TELETEXT2,
-	DMX_TS_PES_SUBTITLE2,
-	DMX_TS_PES_PCR2,
-
-	DMX_TS_PES_AUDIO3,
-	DMX_TS_PES_VIDEO3,
-	DMX_TS_PES_TELETEXT3,
-	DMX_TS_PES_SUBTITLE3,
-	DMX_TS_PES_PCR3,
-
-	DMX_TS_PES_OTHER
-};
-
-#define DMX_TS_PES_AUDIO    DMX_TS_PES_AUDIO0
-#define DMX_TS_PES_VIDEO    DMX_TS_PES_VIDEO0
-#define DMX_TS_PES_TELETEXT DMX_TS_PES_TELETEXT0
-#define DMX_TS_PES_SUBTITLE DMX_TS_PES_SUBTITLE0
-#define DMX_TS_PES_PCR      DMX_TS_PES_PCR0
-
-
 struct dmx_ts_feed {
 	int is_filtering; /* Set to non-zero when filtering in progress */
 	struct dmx_demux *parent; /* Back-pointer */
diff --git a/drivers/media/dvb-core/dmxdev.c b/drivers/media/dvb-core/dmxdev.c
index d81dbb22aa8..a1a3a5159d7 100644
--- a/drivers/media/dvb-core/dmxdev.c
+++ b/drivers/media/dvb-core/dmxdev.c
@@ -569,7 +569,7 @@ static int dvb_dmxdev_start_feed(struct dmxdev *dmxdev,
 	dmx_output_t otype;
 	int ret;
 	int ts_type;
-	dmx_pes_type_t ts_pes;
+	enum dmx_ts_pes ts_pes;
 	struct dmx_ts_feed *tsfeed;
 
 	feed->ts = NULL;
@@ -852,7 +852,8 @@ static int dvb_dmxdev_filter_set(struct dmxdev *dmxdev,
 				 struct dmxdev_filter *dmxdevfilter,
 				 struct dmx_sct_filter_params *params)
 {
-	dprintk("function : %s\n", __func__);
+	dprintk("function : %s, PID=0x%04x, flags=%02x, timeout=%d\n",
+		__func__, params->pid, params->flags, params->timeout);
 
 	dvb_dmxdev_filter_stop(dmxdevfilter);
 
diff --git a/drivers/media/dvb-core/dvb-usb-ids.h b/drivers/media/dvb-core/dvb-usb-ids.h
index 399e1042d35..335a8f4695b 100644
--- a/drivers/media/dvb-core/dvb-usb-ids.h
+++ b/drivers/media/dvb-core/dvb-usb-ids.h
@@ -124,8 +124,7 @@
 #define USB_PID_DIBCOM_STK7770P				0x1e80
 #define USB_PID_DIBCOM_NIM7090				0x1bb2
 #define USB_PID_DIBCOM_TFE7090PVR			0x1bb4
-#define USB_PID_DIBCOM_TFE7090E				0x1bb7
-#define USB_PID_DIBCOM_TFE7790E				0x1e6e
+#define USB_PID_DIBCOM_TFE7790P				0x1e6e
 #define USB_PID_DIBCOM_NIM9090M				0x2383
 #define USB_PID_DIBCOM_NIM9090MD			0x2384
 #define USB_PID_DPOSH_M9206_COLD			0x9206
diff --git a/drivers/media/dvb-core/dvb_demux.c b/drivers/media/dvb-core/dvb_demux.c
index d319717eb53..3485655fa08 100644
--- a/drivers/media/dvb-core/dvb_demux.c
+++ b/drivers/media/dvb-core/dvb_demux.c
@@ -440,20 +440,22 @@ static void dvb_dmx_swfilter_packet(struct dvb_demux *demux, const u8 *buf)
 		if (!dvb_demux_feed_err_pkts)
 			return;
 	} else /* if TEI bit is set, pid may be wrong- skip pkt counter */
-	if (demux->cnt_storage && dvb_demux_tscheck) {
-		/* check pkt counter */
-		if (pid < MAX_PID) {
-			if ((buf[3] & 0xf) != demux->cnt_storage[pid])
-				dprintk_tscheck("TS packet counter mismatch. "
-						"PID=0x%x expected 0x%x "
-						"got 0x%x\n",
+		if (demux->cnt_storage && dvb_demux_tscheck) {
+			/* check pkt counter */
+			if (pid < MAX_PID) {
+				if (buf[3] & 0x10)
+					demux->cnt_storage[pid] =
+						(demux->cnt_storage[pid] + 1) & 0xf;
+
+				if ((buf[3] & 0xf) != demux->cnt_storage[pid]) {
+					dprintk_tscheck("TS packet counter mismatch. PID=0x%x expected 0x%x got 0x%x\n",
 						pid, demux->cnt_storage[pid],
 						buf[3] & 0xf);
-
-			demux->cnt_storage[pid] = ((buf[3] & 0xf) + 1)&0xf;
+					demux->cnt_storage[pid] = buf[3] & 0xf;
+				}
+			}
+			/* end check */
 		}
-		/* end check */
-	}
 
 	list_for_each_entry(feed, &demux->feed_list, list_head) {
 		if ((feed->pid != pid) && (feed->pid != 0x2000))
@@ -672,7 +674,7 @@ static int dmx_ts_feed_set(struct dmx_ts_feed *ts_feed, u16 pid, int ts_type,
 		return -ERESTARTSYS;
 
 	if (ts_type & TS_DECODER) {
-		if (pes_type >= DMX_TS_PES_OTHER) {
+		if (pes_type >= DMX_PES_OTHER) {
 			mutex_unlock(&demux->mutex);
 			return -EINVAL;
 		}
@@ -844,7 +846,7 @@ static int dvbdmx_release_ts_feed(struct dmx_demux *dmx,
 
 	feed->pid = 0xffff;
 
-	if (feed->ts_type & TS_DECODER && feed->pes_type < DMX_TS_PES_OTHER)
+	if (feed->ts_type & TS_DECODER && feed->pes_type < DMX_PES_OTHER)
 		demux->pesfilter[feed->pes_type] = NULL;
 
 	mutex_unlock(&demux->mutex);
@@ -1266,7 +1268,7 @@ int dvb_dmx_init(struct dvb_demux *dvbdemux)
 
 	INIT_LIST_HEAD(&dvbdemux->frontend_list);
 
-	for (i = 0; i < DMX_TS_PES_OTHER; i++) {
+	for (i = 0; i < DMX_PES_OTHER; i++) {
 		dvbdemux->pesfilter[i] = NULL;
 		dvbdemux->pids[i] = 0xffff;
 	}
diff --git a/drivers/media/dvb-core/dvb_demux.h b/drivers/media/dvb-core/dvb_demux.h
index fa7188a253a..ae7fc33c323 100644
--- a/drivers/media/dvb-core/dvb_demux.h
+++ b/drivers/media/dvb-core/dvb_demux.h
@@ -119,8 +119,8 @@ struct dvb_demux {
 
 	struct list_head frontend_list;
 
-	struct dvb_demux_feed *pesfilter[DMX_TS_PES_OTHER];
-	u16 pids[DMX_TS_PES_OTHER];
+	struct dvb_demux_feed *pesfilter[DMX_PES_OTHER];
+	u16 pids[DMX_PES_OTHER];
 	int playing;
 	int recording;
 
diff --git a/drivers/media/dvb-core/dvb_frontend.c b/drivers/media/dvb-core/dvb_frontend.c
index 6e50a758156..57601c0704c 100644
--- a/drivers/media/dvb-core/dvb_frontend.c
+++ b/drivers/media/dvb-core/dvb_frontend.c
@@ -920,7 +920,7 @@ static int dvb_frontend_clear_cache(struct dvb_frontend *fe)
 	u32 delsys;
 
 	delsys = c->delivery_system;
-	memset(c, 0, sizeof(struct dtv_frontend_properties));
+	memset(c, 0, offsetof(struct dtv_frontend_properties, strength));
 	c->delivery_system = delsys;
 
 	c->state = DTV_CLEAR;
@@ -1509,9 +1509,74 @@ static bool is_dvbv3_delsys(u32 delsys)
 	return status;
 }
 
-static int set_delivery_system(struct dvb_frontend *fe, u32 desired_system)
+/**
+ * emulate_delivery_system - emulate a DVBv5 delivery system with a DVBv3 type
+ * @fe:			struct frontend;
+ * @delsys:			DVBv5 type that will be used for emulation
+ *
+ * Provides emulation for delivery systems that are compatible with the old
+ * DVBv3 call. Among its usages, it provices support for ISDB-T, and allows
+ * using a DVB-S2 only frontend just like it were a DVB-S, if the frontent
+ * parameters are compatible with DVB-S spec.
+ */
+static int emulate_delivery_system(struct dvb_frontend *fe, u32 delsys)
 {
-	int ncaps, i;
+	int i;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+
+	c->delivery_system = delsys;
+
+	/*
+	 * If the call is for ISDB-T, put it into full-seg, auto mode, TV
+	 */
+	if (c->delivery_system == SYS_ISDBT) {
+		dev_dbg(fe->dvb->device,
+			"%s: Using defaults for SYS_ISDBT\n",
+			__func__);
+
+		if (!c->bandwidth_hz)
+			c->bandwidth_hz = 6000000;
+
+		c->isdbt_partial_reception = 0;
+		c->isdbt_sb_mode = 0;
+		c->isdbt_sb_subchannel = 0;
+		c->isdbt_sb_segment_idx = 0;
+		c->isdbt_sb_segment_count = 0;
+		c->isdbt_layer_enabled = 7;
+		for (i = 0; i < 3; i++) {
+			c->layer[i].fec = FEC_AUTO;
+			c->layer[i].modulation = QAM_AUTO;
+			c->layer[i].interleaving = 0;
+			c->layer[i].segment_count = 0;
+		}
+	}
+	dev_dbg(fe->dvb->device, "%s: change delivery system on cache to %d\n",
+		__func__, c->delivery_system);
+
+	return 0;
+}
+
+/**
+ * dvbv5_set_delivery_system - Sets the delivery system for a DVBv5 API call
+ * @fe:			frontend struct
+ * @desired_system:	delivery system requested by the user
+ *
+ * A DVBv5 call know what's the desired system it wants. So, set it.
+ *
+ * There are, however, a few known issues with early DVBv5 applications that
+ * are also handled by this logic:
+ *
+ * 1) Some early apps use SYS_UNDEFINED as the desired delivery system.
+ *    This is an API violation, but, as we don't want to break userspace,
+ *    convert it to the first supported delivery system.
+ * 2) Some apps might be using a DVBv5 call in a wrong way, passing, for
+ *    example, SYS_DVBT instead of SYS_ISDBT. This is because early usage of
+ *    ISDB-T provided backward compat with DVB-T.
+ */
+static int dvbv5_set_delivery_system(struct dvb_frontend *fe,
+				     u32 desired_system)
+{
+	int ncaps;
 	u32 delsys = SYS_UNDEFINED;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	enum dvbv3_emulation_type type;
@@ -1522,166 +1587,136 @@ static int set_delivery_system(struct dvb_frontend *fe, u32 desired_system)
 	 * assume that the application wants to use the first supported
 	 * delivery system.
 	 */
-	if (c->delivery_system == SYS_UNDEFINED)
-	        c->delivery_system = fe->ops.delsys[0];
+	if (desired_system == SYS_UNDEFINED)
+		desired_system = fe->ops.delsys[0];
 
-	if (desired_system == SYS_UNDEFINED) {
-		/*
-		 * A DVBv3 call doesn't know what's the desired system.
-		 * Also, DVBv3 applications don't know that ops.info->type
-		 * could be changed, and they simply dies when it doesn't
-		 * match.
-		 * So, don't change the current delivery system, as it
-		 * may be trying to do the wrong thing, like setting an
-		 * ISDB-T frontend as DVB-T. Instead, find the closest
-		 * DVBv3 system that matches the delivery system.
-		 */
-		if (is_dvbv3_delsys(c->delivery_system)) {
+	/*
+	 * This is a DVBv5 call. So, it likely knows the supported
+	 * delivery systems. So, check if the desired delivery system is
+	 * supported
+	 */
+	ncaps = 0;
+	while (fe->ops.delsys[ncaps] && ncaps < MAX_DELSYS) {
+		if (fe->ops.delsys[ncaps] == desired_system) {
+			c->delivery_system = desired_system;
 			dev_dbg(fe->dvb->device,
-					"%s: Using delivery system to %d\n",
-					__func__, c->delivery_system);
-			return 0;
-		}
-		type = dvbv3_type(c->delivery_system);
-		switch (type) {
-		case DVBV3_QPSK:
-			desired_system = SYS_DVBS;
-			break;
-		case DVBV3_QAM:
-			desired_system = SYS_DVBC_ANNEX_A;
-			break;
-		case DVBV3_ATSC:
-			desired_system = SYS_ATSC;
-			break;
-		case DVBV3_OFDM:
-			desired_system = SYS_DVBT;
-			break;
-		default:
-			dev_dbg(fe->dvb->device, "%s: This frontend doesn't support DVBv3 calls\n",
-					__func__);
-			return -EINVAL;
-		}
-		/*
-		 * Get a delivery system that is compatible with DVBv3
-		 * NOTE: in order for this to work with softwares like Kaffeine that
-		 *	uses a DVBv5 call for DVB-S2 and a DVBv3 call to go back to
-		 *	DVB-S, drivers that support both should put the SYS_DVBS entry
-		 *	before the SYS_DVBS2, otherwise it won't switch back to DVB-S.
-		 *	The real fix is that userspace applications should not use DVBv3
-		 *	and not trust on calling FE_SET_FRONTEND to switch the delivery
-		 *	system.
-		 */
-		ncaps = 0;
-		while (fe->ops.delsys[ncaps] && ncaps < MAX_DELSYS) {
-			if (fe->ops.delsys[ncaps] == desired_system) {
-				delsys = desired_system;
-				break;
-			}
-			ncaps++;
-		}
-		if (delsys == SYS_UNDEFINED) {
-			dev_dbg(fe->dvb->device, "%s: Couldn't find a delivery system that matches %d\n",
+					"%s: Changing delivery system to %d\n",
 					__func__, desired_system);
+			return 0;
 		}
-	} else {
-		/*
-		 * This is a DVBv5 call. So, it likely knows the supported
-		 * delivery systems.
-		 */
+		ncaps++;
+	}
 
-		/* Check if the desired delivery system is supported */
-		ncaps = 0;
-		while (fe->ops.delsys[ncaps] && ncaps < MAX_DELSYS) {
-			if (fe->ops.delsys[ncaps] == desired_system) {
-				c->delivery_system = desired_system;
-				dev_dbg(fe->dvb->device,
-						"%s: Changing delivery system to %d\n",
-						__func__, desired_system);
-				return 0;
-			}
-			ncaps++;
-		}
-		type = dvbv3_type(desired_system);
+	/*
+	 * The requested delivery system isn't supported. Maybe userspace
+	 * is requesting a DVBv3 compatible delivery system.
+	 *
+	 * The emulation only works if the desired system is one of the
+	 * delivery systems supported by DVBv3 API
+	 */
+	if (!is_dvbv3_delsys(desired_system)) {
+		dev_dbg(fe->dvb->device,
+			"%s: Delivery system %d not supported.\n",
+			__func__, desired_system);
+		return -EINVAL;
+	}
 
-		/*
-		 * The delivery system is not supported. See if it can be
-		 * emulated.
-		 * The emulation only works if the desired system is one of the
-		 * DVBv3 delivery systems
-		 */
-		if (!is_dvbv3_delsys(desired_system)) {
-			dev_dbg(fe->dvb->device,
-					"%s: can't use a DVBv3 FE_SET_FRONTEND call on this frontend\n",
-					__func__);
-			return -EINVAL;
-		}
+	type = dvbv3_type(desired_system);
 
-		/*
-		 * Get the last non-DVBv3 delivery system that has the same type
-		 * of the desired system
-		 */
-		ncaps = 0;
-		while (fe->ops.delsys[ncaps] && ncaps < MAX_DELSYS) {
-			if ((dvbv3_type(fe->ops.delsys[ncaps]) == type) &&
-			    !is_dvbv3_delsys(fe->ops.delsys[ncaps]))
-				delsys = fe->ops.delsys[ncaps];
-			ncaps++;
-		}
-		/* There's nothing compatible with the desired delivery system */
-		if (delsys == SYS_UNDEFINED) {
-			dev_dbg(fe->dvb->device,
-					"%s: Incompatible DVBv3 FE_SET_FRONTEND call for this frontend\n",
-					__func__);
-			return -EINVAL;
-		}
+	/*
+	* Get the last non-DVBv3 delivery system that has the same type
+	* of the desired system
+	*/
+	ncaps = 0;
+	while (fe->ops.delsys[ncaps] && ncaps < MAX_DELSYS) {
+		if (dvbv3_type(fe->ops.delsys[ncaps]) == type)
+			delsys = fe->ops.delsys[ncaps];
+		ncaps++;
 	}
 
-	c->delivery_system = delsys;
+	/* There's nothing compatible with the desired delivery system */
+	if (delsys == SYS_UNDEFINED) {
+		dev_dbg(fe->dvb->device,
+			"%s: Delivery system %d not supported on emulation mode.\n",
+			__func__, desired_system);
+		return -EINVAL;
+	}
+
+	dev_dbg(fe->dvb->device,
+		"%s: Using delivery system %d emulated as if it were %d\n",
+		__func__, delsys, desired_system);
+
+	return emulate_delivery_system(fe, desired_system);
+}
+
+/**
+ * dvbv3_set_delivery_system - Sets the delivery system for a DVBv3 API call
+ * @fe:	frontend struct
+ *
+ * A DVBv3 call doesn't know what's the desired system it wants. It also
+ * doesn't allow to switch between different types. Due to that, userspace
+ * should use DVBv5 instead.
+ * However, in order to avoid breaking userspace API, limited backward
+ * compatibility support is provided.
+ *
+ * There are some delivery systems that are incompatible with DVBv3 calls.
+ *
+ * This routine should work fine for frontends that support just one delivery
+ * system.
+ *
+ * For frontends that support multiple frontends:
+ * 1) It defaults to use the first supported delivery system. There's an
+ *    userspace application that allows changing it at runtime;
+ *
+ * 2) If the current delivery system is not compatible with DVBv3, it gets
+ *    the first one that it is compatible.
+ *
+ * NOTE: in order for this to work with applications like Kaffeine that
+ *	uses a DVBv5 call for DVB-S2 and a DVBv3 call to go back to
+ *	DVB-S, drivers that support both DVB-S and DVB-S2 should have the
+ *	SYS_DVBS entry before the SYS_DVBS2, otherwise it won't switch back
+ *	to DVB-S.
+ */
+static int dvbv3_set_delivery_system(struct dvb_frontend *fe)
+{
+	int ncaps;
+	u32 delsys = SYS_UNDEFINED;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+
+	/* If not set yet, defaults to the first supported delivery system */
+	if (c->delivery_system == SYS_UNDEFINED)
+		c->delivery_system = fe->ops.delsys[0];
 
 	/*
-	 * The DVBv3 or DVBv5 call is requesting a different system. So,
-	 * emulation is needed.
-	 *
-	 * Emulate newer delivery systems like ISDBT, DVBT and DTMB
-	 * for older DVBv5 applications. The emulation will try to use
-	 * the auto mode for most things, and will assume that the desired
-	 * delivery system is the last one at the ops.delsys[] array
+	 * Trivial case: just use the current one, if it already a DVBv3
+	 * delivery system
 	 */
-	dev_dbg(fe->dvb->device,
-			"%s: Using delivery system %d emulated as if it were a %d\n",
-			__func__, delsys, desired_system);
+	if (is_dvbv3_delsys(c->delivery_system)) {
+		dev_dbg(fe->dvb->device,
+				"%s: Using delivery system to %d\n",
+				__func__, c->delivery_system);
+		return 0;
+	}
 
 	/*
-	 * For now, handles ISDB-T calls. More code may be needed here for the
-	 * other emulated stuff
+	 * Seek for the first delivery system that it is compatible with a
+	 * DVBv3 standard
 	 */
-	if (type == DVBV3_OFDM) {
-		if (c->delivery_system == SYS_ISDBT) {
-			dev_dbg(fe->dvb->device,
-					"%s: Using defaults for SYS_ISDBT\n",
-					__func__);
-
-			if (!c->bandwidth_hz)
-				c->bandwidth_hz = 6000000;
-
-			c->isdbt_partial_reception = 0;
-			c->isdbt_sb_mode = 0;
-			c->isdbt_sb_subchannel = 0;
-			c->isdbt_sb_segment_idx = 0;
-			c->isdbt_sb_segment_count = 0;
-			c->isdbt_layer_enabled = 0;
-			for (i = 0; i < 3; i++) {
-				c->layer[i].fec = FEC_AUTO;
-				c->layer[i].modulation = QAM_AUTO;
-				c->layer[i].interleaving = 0;
-				c->layer[i].segment_count = 0;
-			}
+	ncaps = 0;
+	while (fe->ops.delsys[ncaps] && ncaps < MAX_DELSYS) {
+		if (dvbv3_type(fe->ops.delsys[ncaps]) != DVBV3_UNKNOWN) {
+			delsys = fe->ops.delsys[ncaps];
+			break;
 		}
+		ncaps++;
 	}
-	dev_dbg(fe->dvb->device, "%s: change delivery system on cache to %d\n",
-			__func__, c->delivery_system);
-
-	return 0;
+	if (delsys == SYS_UNDEFINED) {
+		dev_dbg(fe->dvb->device,
+			"%s: Couldn't find a delivery system that works with FE_SET_FRONTEND\n",
+			__func__);
+		return -EINVAL;
+	}
+	return emulate_delivery_system(fe, delsys);
 }
 
 static int dtv_property_process_set(struct dvb_frontend *fe,
@@ -1742,7 +1777,7 @@ static int dtv_property_process_set(struct dvb_frontend *fe,
 		c->rolloff = tvp->u.data;
 		break;
 	case DTV_DELIVERY_SYSTEM:
-		r = set_delivery_system(fe, tvp->u.data);
+		r = dvbv5_set_delivery_system(fe, tvp->u.data);
 		break;
 	case DTV_VOLTAGE:
 		c->voltage = tvp->u.data;
@@ -2335,7 +2370,7 @@ static int dvb_frontend_ioctl_legacy(struct file *file,
 		break;
 
 	case FE_SET_FRONTEND:
-		err = set_delivery_system(fe, SYS_UNDEFINED);
+		err = dvbv3_set_delivery_system(fe);
 		if (err)
 			break;
 
@@ -2594,7 +2629,7 @@ int dvb_register_frontend(struct dvb_adapter* dvb,
 	 * first supported delivery system (ops->delsys[0])
 	 */
 
-        fe->dtv_property_cache.delivery_system = fe->ops.delsys[0];
+	fe->dtv_property_cache.delivery_system = fe->ops.delsys[0];
 	dvb_frontend_clear_cache(fe);
 
 	mutex_unlock(&frontend_mutex);
diff --git a/drivers/media/dvb-core/dvb_frontend.h b/drivers/media/dvb-core/dvb_frontend.h
index b34922a0815..371b6caf486 100644
--- a/drivers/media/dvb-core/dvb_frontend.h
+++ b/drivers/media/dvb-core/dvb_frontend.h
@@ -245,8 +245,8 @@ struct analog_demod_ops {
 
 	void (*set_params)(struct dvb_frontend *fe,
 			   struct analog_parameters *params);
-	int  (*has_signal)(struct dvb_frontend *fe);
-	int  (*get_afc)(struct dvb_frontend *fe);
+	int  (*has_signal)(struct dvb_frontend *fe, u16 *signal);
+	int  (*get_afc)(struct dvb_frontend *fe, s32 *afc);
 	void (*tuner_status)(struct dvb_frontend *fe);
 	void (*standby)(struct dvb_frontend *fe);
 	void (*release)(struct dvb_frontend *fe);
diff --git a/drivers/media/dvb-core/dvb_net.c b/drivers/media/dvb-core/dvb_net.c
index 44225b186f6..e17cb85d3ec 100644
--- a/drivers/media/dvb-core/dvb_net.c
+++ b/drivers/media/dvb-core/dvb_net.c
@@ -1044,7 +1044,7 @@ static int dvb_net_feed_start(struct net_device *dev)
 		ret = priv->tsfeed->set(priv->tsfeed,
 					priv->pid, /* pid */
 					TS_PACKET, /* type */
-					DMX_TS_PES_OTHER, /* pes type */
+					DMX_PES_OTHER, /* pes type */
 					32768,     /* circular buffer size */
 					timeout    /* timeout */
 					);
diff --git a/drivers/media/dvb-frontends/Kconfig b/drivers/media/dvb-frontends/Kconfig
index 6f809a70c78..0e2ec6f73b0 100644
--- a/drivers/media/dvb-frontends/Kconfig
+++ b/drivers/media/dvb-frontends/Kconfig
@@ -210,7 +210,7 @@ config DVB_SI21XX
 config DVB_TS2020
 	tristate "Montage Tehnology TS2020 based tuners"
 	depends on DVB_CORE && I2C
-	default m if DVB_FE_CUSTOMISE
+	default m if !MEDIA_SUBDRV_AUTOSELECT
 	help
 	  A DVB-S/S2 silicon tuner. Say Y when you want to support this tuner.
 
diff --git a/drivers/media/dvb-frontends/a8293.h b/drivers/media/dvb-frontends/a8293.h
index ed29e5504f7..b6ef6427cfa 100644
--- a/drivers/media/dvb-frontends/a8293.h
+++ b/drivers/media/dvb-frontends/a8293.h
@@ -21,12 +21,13 @@
 #ifndef A8293_H
 #define A8293_H
 
+#include <linux/kconfig.h>
+
 struct a8293_config {
 	u8 i2c_addr;
 };
 
-#if defined(CONFIG_DVB_A8293) || \
-	(defined(CONFIG_DVB_A8293_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_A8293)
 extern struct dvb_frontend *a8293_attach(struct dvb_frontend *fe,
 	struct i2c_adapter *i2c, const struct a8293_config *cfg);
 #else
diff --git a/drivers/media/dvb-frontends/af9013.h b/drivers/media/dvb-frontends/af9013.h
index dc837d91327..09273b2cd31 100644
--- a/drivers/media/dvb-frontends/af9013.h
+++ b/drivers/media/dvb-frontends/af9013.h
@@ -25,6 +25,7 @@
 #ifndef AF9013_H
 #define AF9013_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 /* AF9013/5 GPIOs (mostly guessed)
@@ -102,8 +103,7 @@ struct af9013_config {
 	u8 gpio[4];
 };
 
-#if defined(CONFIG_DVB_AF9013) || \
-	(defined(CONFIG_DVB_AF9013_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_AF9013)
 extern struct dvb_frontend *af9013_attach(const struct af9013_config *config,
 	struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/af9033.c b/drivers/media/dvb-frontends/af9033.c
index c9cad989b8b..a777b4b944e 100644
--- a/drivers/media/dvb-frontends/af9033.c
+++ b/drivers/media/dvb-frontends/af9033.c
@@ -156,6 +156,37 @@ static int af9033_rd_reg_mask(struct af9033_state *state, u32 reg, u8 *val,
 	return 0;
 }
 
+/* write reg val table using reg addr auto increment */
+static int af9033_wr_reg_val_tab(struct af9033_state *state,
+		const struct reg_val *tab, int tab_len)
+{
+	int ret, i, j;
+	u8 buf[tab_len];
+
+	dev_dbg(&state->i2c->dev, "%s: tab_len=%d\n", __func__, tab_len);
+
+	for (i = 0, j = 0; i < tab_len; i++) {
+		buf[j] = tab[i].val;
+
+		if (i == tab_len - 1 || tab[i].reg != tab[i + 1].reg - 1) {
+			ret = af9033_wr_regs(state, tab[i].reg - j, buf, j + 1);
+			if (ret < 0)
+				goto err;
+
+			j = 0;
+		} else {
+			j++;
+		}
+	}
+
+	return 0;
+
+err:
+	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
 static u32 af9033_div(struct af9033_state *state, u32 a, u32 b, u32 x)
 {
 	u32 r = 0, c = 0, i;
@@ -223,6 +254,7 @@ static int af9033_init(struct dvb_frontend *fe)
 		{ 0x80f986, state->ts_mode_parallel, 0x01 },
 		{ 0x00d827, 0x00, 0xff },
 		{ 0x00d829, 0x00, 0xff },
+		{ 0x800045, state->cfg.adc_multiplier, 0xff },
 	};
 
 	/* program clock control */
@@ -286,14 +318,29 @@ static int af9033_init(struct dvb_frontend *fe)
 
 	/* load OFSM settings */
 	dev_dbg(&state->i2c->dev, "%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;
+	switch (state->cfg.tuner) {
+	case AF9033_TUNER_IT9135_38:
+	case AF9033_TUNER_IT9135_51:
+	case AF9033_TUNER_IT9135_52:
+		len = ARRAY_SIZE(ofsm_init_it9135_v1);
+		init = ofsm_init_it9135_v1;
+		break;
+	case AF9033_TUNER_IT9135_60:
+	case AF9033_TUNER_IT9135_61:
+	case AF9033_TUNER_IT9135_62:
+		len = ARRAY_SIZE(ofsm_init_it9135_v2);
+		init = ofsm_init_it9135_v2;
+		break;
+	default:
+		len = ARRAY_SIZE(ofsm_init);
+		init = ofsm_init;
+		break;
 	}
 
+	ret = af9033_wr_reg_val_tab(state, init, len);
+	if (ret < 0)
+		goto err;
+
 	/* load tuner specific settings */
 	dev_dbg(&state->i2c->dev, "%s: load tuner specific settings\n",
 			__func__);
@@ -322,6 +369,30 @@ static int af9033_init(struct dvb_frontend *fe)
 		len = ARRAY_SIZE(tuner_init_fc0012);
 		init = tuner_init_fc0012;
 		break;
+	case AF9033_TUNER_IT9135_38:
+		len = ARRAY_SIZE(tuner_init_it9135_38);
+		init = tuner_init_it9135_38;
+		break;
+	case AF9033_TUNER_IT9135_51:
+		len = ARRAY_SIZE(tuner_init_it9135_51);
+		init = tuner_init_it9135_51;
+		break;
+	case AF9033_TUNER_IT9135_52:
+		len = ARRAY_SIZE(tuner_init_it9135_52);
+		init = tuner_init_it9135_52;
+		break;
+	case AF9033_TUNER_IT9135_60:
+		len = ARRAY_SIZE(tuner_init_it9135_60);
+		init = tuner_init_it9135_60;
+		break;
+	case AF9033_TUNER_IT9135_61:
+		len = ARRAY_SIZE(tuner_init_it9135_61);
+		init = tuner_init_it9135_61;
+		break;
+	case AF9033_TUNER_IT9135_62:
+		len = ARRAY_SIZE(tuner_init_it9135_62);
+		init = tuner_init_it9135_62;
+		break;
 	default:
 		dev_dbg(&state->i2c->dev, "%s: unsupported tuner ID=%d\n",
 				__func__, state->cfg.tuner);
@@ -329,11 +400,9 @@ static int af9033_init(struct dvb_frontend *fe)
 		goto err;
 	}
 
-	for (i = 0; i < len; i++) {
-		ret = af9033_wr_reg(state, init[i].reg, init[i].val);
-		if (ret < 0)
-			goto err;
-	}
+	ret = af9033_wr_reg_val_tab(state, init, len);
+	if (ret < 0)
+		goto err;
 
 	if (state->cfg.ts_mode == AF9033_TS_MODE_SERIAL) {
 		ret = af9033_wr_reg_mask(state, 0x00d91c, 0x01, 0x01);
@@ -349,6 +418,15 @@ static int af9033_init(struct dvb_frontend *fe)
 			goto err;
 	}
 
+	switch (state->cfg.tuner) {
+	case AF9033_TUNER_IT9135_60:
+	case AF9033_TUNER_IT9135_61:
+	case AF9033_TUNER_IT9135_62:
+		ret = af9033_wr_reg(state, 0x800000, 0x01);
+		if (ret < 0)
+			goto err;
+	}
+
 	state->bandwidth_hz = 0; /* force to program all parameters */
 
 	return 0;
@@ -415,7 +493,8 @@ err:
 static int af9033_get_tune_settings(struct dvb_frontend *fe,
 		struct dvb_frontend_tune_settings *fesettings)
 {
-	fesettings->min_delay_ms = 800;
+	/* 800 => 2000 because IT9135 v2 is slow to gain lock */
+	fesettings->min_delay_ms = 2000;
 	fesettings->step_size = 0;
 	fesettings->max_drift = 0;
 
@@ -498,17 +577,17 @@ static int af9033_set_frontend(struct dvb_frontend *fe)
 		if (spec_inv == -1)
 			freq_cw = 0x800000 - freq_cw;
 
-		/* get adc multiplies */
-		ret = af9033_rd_reg(state, 0x800045, &tmp);
-		if (ret < 0)
-			goto err;
-
-		if (tmp == 1)
+		if (state->cfg.adc_multiplier == AF9033_ADC_MULTIPLIER_2X)
 			freq_cw /= 2;
 
 		buf[0] = (freq_cw >>  0) & 0xff;
 		buf[1] = (freq_cw >>  8) & 0xff;
 		buf[2] = (freq_cw >> 16) & 0x7f;
+
+		/* FIXME: there seems to be calculation error here... */
+		if (if_frequency == 0)
+			buf[2] = 0;
+
 		ret = af9033_wr_regs(state, 0x800029, buf, 3);
 		if (ret < 0)
 			goto err;
@@ -934,13 +1013,24 @@ struct dvb_frontend *af9033_attach(const struct af9033_config *config,
 			buf[2], buf[3], buf[4], buf[5], buf[6], buf[7]);
 
 	/* sleep */
-	ret = af9033_wr_reg(state, 0x80004c, 1);
-	if (ret < 0)
-		goto err;
+	switch (state->cfg.tuner) {
+	case AF9033_TUNER_IT9135_38:
+	case AF9033_TUNER_IT9135_51:
+	case AF9033_TUNER_IT9135_52:
+	case AF9033_TUNER_IT9135_60:
+	case AF9033_TUNER_IT9135_61:
+	case AF9033_TUNER_IT9135_62:
+		/* IT9135 did not like to sleep at that early */
+		break;
+	default:
+		ret = af9033_wr_reg(state, 0x80004c, 1);
+		if (ret < 0)
+			goto err;
 
-	ret = af9033_wr_reg(state, 0x800000, 0);
-	if (ret < 0)
-		goto err;
+		ret = af9033_wr_reg(state, 0x800000, 0);
+		if (ret < 0)
+			goto err;
+	}
 
 	/* configure internal TS mode */
 	switch (state->cfg.ts_mode) {
diff --git a/drivers/media/dvb-frontends/af9033.h b/drivers/media/dvb-frontends/af9033.h
index 82bd8c1513b..c286e8f1ec0 100644
--- a/drivers/media/dvb-frontends/af9033.h
+++ b/drivers/media/dvb-frontends/af9033.h
@@ -22,6 +22,8 @@
 #ifndef AF9033_H
 #define AF9033_H
 
+#include <linux/kconfig.h>
+
 struct af9033_config {
 	/*
 	 * I2C address
@@ -36,6 +38,13 @@ struct af9033_config {
 	u32 clock;
 
 	/*
+	 * ADC multiplier
+	 */
+#define AF9033_ADC_MULTIPLIER_1X   0
+#define AF9033_ADC_MULTIPLIER_2X   1
+	u8 adc_multiplier;
+
+	/*
 	 * tuner
 	 */
 #define AF9033_TUNER_TUA9001     0x27 /* Infineon TUA 9001 */
@@ -44,6 +53,14 @@ struct af9033_config {
 #define AF9033_TUNER_MXL5007T    0xa0 /* MaxLinear MxL5007T */
 #define AF9033_TUNER_TDA18218    0xa1 /* NXP TDA 18218HN */
 #define AF9033_TUNER_FC2580      0x32 /* FCI FC2580 */
+/* 50-5f Omega */
+#define AF9033_TUNER_IT9135_38   0x38 /* Omega */
+#define AF9033_TUNER_IT9135_51   0x51 /* Omega LNA config 1 */
+#define AF9033_TUNER_IT9135_52   0x52 /* Omega LNA config 2 */
+/* 60-6f Omega v2 */
+#define AF9033_TUNER_IT9135_60   0x60 /* Omega v2 */
+#define AF9033_TUNER_IT9135_61   0x61 /* Omega v2 LNA config 1 */
+#define AF9033_TUNER_IT9135_62   0x62 /* Omega v2 LNA config 2 */
 	u8 tuner;
 
 	/*
@@ -61,8 +78,7 @@ struct af9033_config {
 };
 
 
-#if defined(CONFIG_DVB_AF9033) || \
-	(defined(CONFIG_DVB_AF9033_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_AF9033)
 extern struct dvb_frontend *af9033_attach(const struct af9033_config *config,
 	struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/af9033_priv.h b/drivers/media/dvb-frontends/af9033_priv.h
index e9bd7826554..fc2ad581e30 100644
--- a/drivers/media/dvb-frontends/af9033_priv.h
+++ b/drivers/media/dvb-frontends/af9033_priv.h
@@ -547,5 +547,1509 @@ static const struct reg_val tuner_init_fc2580[] = {
 	{ 0x80f1e6, 0x01 },
 };
 
-#endif /* AF9033_PRIV_H */
+static const struct reg_val ofsm_init_it9135_v1[] = {
+	{ 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 },
+	{ 0x8000b0, 0x01 },
+	{ 0x8000c2, 0x05 },
+	{ 0x8000c6, 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 },
+};
+
+/* ITE Tech IT9135 Omega tuner init
+   AF9033_TUNER_IT9135_38   = 0x38 */
+static const struct reg_val tuner_init_it9135_38[] = {
+	{ 0x800043, 0x00 },
+	{ 0x800046, 0x38 },
+	{ 0x800051, 0x01 },
+	{ 0x80005f, 0x00 },
+	{ 0x800060, 0x00 },
+	{ 0x800068, 0x0a },
+	{ 0x800070, 0x0a },
+	{ 0x800071, 0x05 },
+	{ 0x800072, 0x02 },
+	{ 0x800075, 0x8c },
+	{ 0x800076, 0x8c },
+	{ 0x800077, 0x8c },
+	{ 0x800078, 0xc8 },
+	{ 0x800079, 0x01 },
+	{ 0x80007e, 0x04 },
+	{ 0x80007f, 0x00 },
+	{ 0x800081, 0x0a },
+	{ 0x800082, 0x12 },
+	{ 0x800083, 0x02 },
+	{ 0x800084, 0x0a },
+	{ 0x800085, 0x03 },
+	{ 0x800086, 0xc8 },
+	{ 0x800087, 0xb8 },
+	{ 0x800088, 0xd0 },
+	{ 0x800089, 0xc3 },
+	{ 0x80008a, 0x01 },
+	{ 0x80008e, 0x01 },
+	{ 0x800092, 0x06 },
+	{ 0x800093, 0x00 },
+	{ 0x800094, 0x00 },
+	{ 0x800095, 0x00 },
+	{ 0x800096, 0x00 },
+	{ 0x800099, 0x01 },
+	{ 0x80009b, 0x3c },
+	{ 0x80009c, 0x28 },
+	{ 0x80009f, 0xe1 },
+	{ 0x8000a0, 0xcf },
+	{ 0x8000a3, 0x01 },
+	{ 0x8000a4, 0x5a },
+	{ 0x8000a5, 0x01 },
+	{ 0x8000a6, 0x01 },
+	{ 0x8000a9, 0x00 },
+	{ 0x8000aa, 0x01 },
+	{ 0x8000b0, 0x01 },
+	{ 0x8000b3, 0x02 },
+	{ 0x8000b4, 0x32 },
+	{ 0x8000b6, 0x14 },
+	{ 0x8000c0, 0x11 },
+	{ 0x8000c1, 0x00 },
+	{ 0x8000c2, 0x05 },
+	{ 0x8000c4, 0x00 },
+	{ 0x8000c6, 0x19 },
+	{ 0x8000c7, 0x00 },
+	{ 0x8000cc, 0x2e },
+	{ 0x8000cd, 0x51 },
+	{ 0x8000ce, 0x33 },
+	{ 0x8000f3, 0x05 },
+	{ 0x8000f4, 0x8c },
+	{ 0x8000f5, 0x8c },
+	{ 0x8000f8, 0x03 },
+	{ 0x8000f9, 0x06 },
+	{ 0x8000fa, 0x06 },
+	{ 0x8000fc, 0x02 },
+	{ 0x8000fd, 0x02 },
+	{ 0x8000fe, 0x02 },
+	{ 0x8000ff, 0x09 },
+	{ 0x800100, 0x50 },
+	{ 0x800101, 0x7b },
+	{ 0x800102, 0x77 },
+	{ 0x800103, 0x00 },
+	{ 0x800104, 0x02 },
+	{ 0x800105, 0xc8 },
+	{ 0x800106, 0x05 },
+	{ 0x800107, 0x7b },
+	{ 0x800109, 0x02 },
+	{ 0x800115, 0x0a },
+	{ 0x800116, 0x03 },
+	{ 0x800117, 0x02 },
+	{ 0x800118, 0x80 },
+	{ 0x80011a, 0xc8 },
+	{ 0x80011b, 0x7b },
+	{ 0x80011c, 0x8a },
+	{ 0x80011d, 0xa0 },
+	{ 0x800122, 0x02 },
+	{ 0x800123, 0x18 },
+	{ 0x800124, 0xc3 },
+	{ 0x800127, 0x00 },
+	{ 0x800128, 0x07 },
+	{ 0x80012a, 0x53 },
+	{ 0x80012b, 0x51 },
+	{ 0x80012c, 0x4e },
+	{ 0x80012d, 0x43 },
+	{ 0x800137, 0x01 },
+	{ 0x800138, 0x00 },
+	{ 0x800139, 0x07 },
+	{ 0x80013a, 0x00 },
+	{ 0x80013b, 0x06 },
+	{ 0x80013d, 0x00 },
+	{ 0x80013e, 0x01 },
+	{ 0x80013f, 0x5b },
+	{ 0x800140, 0xc8 },
+	{ 0x800141, 0x59 },
+	{ 0x80f000, 0x0f },
+	{ 0x80f016, 0x10 },
+	{ 0x80f017, 0x04 },
+	{ 0x80f018, 0x05 },
+	{ 0x80f019, 0x04 },
+	{ 0x80f01a, 0x05 },
+	{ 0x80f01f, 0x8c },
+	{ 0x80f020, 0x00 },
+	{ 0x80f021, 0x03 },
+	{ 0x80f022, 0x0a },
+	{ 0x80f023, 0x0a },
+	{ 0x80f029, 0x8c },
+	{ 0x80f02a, 0x00 },
+	{ 0x80f02b, 0x00 },
+	{ 0x80f02c, 0x01 },
+	{ 0x80f064, 0x03 },
+	{ 0x80f065, 0xf9 },
+	{ 0x80f066, 0x03 },
+	{ 0x80f067, 0x01 },
+	{ 0x80f06f, 0xe0 },
+	{ 0x80f070, 0x03 },
+	{ 0x80f072, 0x0f },
+	{ 0x80f073, 0x03 },
+	{ 0x80f077, 0x01 },
+	{ 0x80f078, 0x00 },
+	{ 0x80f085, 0x00 },
+	{ 0x80f086, 0x02 },
+	{ 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 },
+	{ 0x80f24c, 0x88 },
+	{ 0x80f24d, 0x95 },
+	{ 0x80f24e, 0x9a },
+	{ 0x80f24f, 0x90 },
+	{ 0x80f25a, 0x07 },
+	{ 0x80f25b, 0xe8 },
+	{ 0x80f25c, 0x03 },
+	{ 0x80f25d, 0xb0 },
+	{ 0x80f25e, 0x04 },
+	{ 0x80f270, 0x01 },
+	{ 0x80f271, 0x02 },
+	{ 0x80f272, 0x01 },
+	{ 0x80f273, 0x02 },
+	{ 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 },
+};
+
+/* ITE Tech IT9135 Omega LNA config 1 tuner init
+   AF9033_TUNER_IT9135_51   = 0x51 */
+static const struct reg_val tuner_init_it9135_51[] = {
+	{ 0x800043, 0x00 },
+	{ 0x800046, 0x51 },
+	{ 0x800051, 0x01 },
+	{ 0x80005f, 0x00 },
+	{ 0x800060, 0x00 },
+	{ 0x800068, 0x0a },
+	{ 0x800070, 0x0a },
+	{ 0x800071, 0x06 },
+	{ 0x800072, 0x02 },
+	{ 0x800075, 0x8c },
+	{ 0x800076, 0x8c },
+	{ 0x800077, 0x8c },
+	{ 0x800078, 0xc8 },
+	{ 0x800079, 0x01 },
+	{ 0x80007e, 0x04 },
+	{ 0x80007f, 0x00 },
+	{ 0x800081, 0x0a },
+	{ 0x800082, 0x12 },
+	{ 0x800083, 0x02 },
+	{ 0x800084, 0x0a },
+	{ 0x800085, 0x03 },
+	{ 0x800086, 0xc0 },
+	{ 0x800087, 0x96 },
+	{ 0x800088, 0xcf },
+	{ 0x800089, 0xc3 },
+	{ 0x80008a, 0x01 },
+	{ 0x80008e, 0x01 },
+	{ 0x800092, 0x06 },
+	{ 0x800093, 0x00 },
+	{ 0x800094, 0x00 },
+	{ 0x800095, 0x00 },
+	{ 0x800096, 0x00 },
+	{ 0x800099, 0x01 },
+	{ 0x80009b, 0x3c },
+	{ 0x80009c, 0x28 },
+	{ 0x80009f, 0xe1 },
+	{ 0x8000a0, 0xcf },
+	{ 0x8000a3, 0x01 },
+	{ 0x8000a4, 0x5a },
+	{ 0x8000a5, 0x01 },
+	{ 0x8000a6, 0x01 },
+	{ 0x8000a9, 0x00 },
+	{ 0x8000aa, 0x01 },
+	{ 0x8000b0, 0x01 },
+	{ 0x8000b3, 0x02 },
+	{ 0x8000b4, 0x3c },
+	{ 0x8000b6, 0x14 },
+	{ 0x8000c0, 0x11 },
+	{ 0x8000c1, 0x00 },
+	{ 0x8000c2, 0x05 },
+	{ 0x8000c4, 0x00 },
+	{ 0x8000c6, 0x19 },
+	{ 0x8000c7, 0x00 },
+	{ 0x8000cc, 0x2e },
+	{ 0x8000cd, 0x51 },
+	{ 0x8000ce, 0x33 },
+	{ 0x8000f3, 0x05 },
+	{ 0x8000f4, 0x8c },
+	{ 0x8000f5, 0x8c },
+	{ 0x8000f8, 0x03 },
+	{ 0x8000f9, 0x06 },
+	{ 0x8000fa, 0x06 },
+	{ 0x8000fc, 0x03 },
+	{ 0x8000fd, 0x02 },
+	{ 0x8000fe, 0x02 },
+	{ 0x8000ff, 0x09 },
+	{ 0x800100, 0x50 },
+	{ 0x800101, 0x7a },
+	{ 0x800102, 0x77 },
+	{ 0x800103, 0x01 },
+	{ 0x800104, 0x02 },
+	{ 0x800105, 0xb0 },
+	{ 0x800106, 0x02 },
+	{ 0x800107, 0x7a },
+	{ 0x800109, 0x02 },
+	{ 0x800115, 0x0a },
+	{ 0x800116, 0x03 },
+	{ 0x800117, 0x02 },
+	{ 0x800118, 0x80 },
+	{ 0x80011a, 0xc0 },
+	{ 0x80011b, 0x7a },
+	{ 0x80011c, 0xac },
+	{ 0x80011d, 0x8c },
+	{ 0x800122, 0x02 },
+	{ 0x800123, 0x70 },
+	{ 0x800124, 0xa4 },
+	{ 0x800127, 0x00 },
+	{ 0x800128, 0x07 },
+	{ 0x80012a, 0x53 },
+	{ 0x80012b, 0x51 },
+	{ 0x80012c, 0x4e },
+	{ 0x80012d, 0x43 },
+	{ 0x800137, 0x01 },
+	{ 0x800138, 0x00 },
+	{ 0x800139, 0x07 },
+	{ 0x80013a, 0x00 },
+	{ 0x80013b, 0x06 },
+	{ 0x80013d, 0x00 },
+	{ 0x80013e, 0x01 },
+	{ 0x80013f, 0x5b },
+	{ 0x800140, 0xc0 },
+	{ 0x800141, 0x59 },
+	{ 0x80f000, 0x0f },
+	{ 0x80f016, 0x10 },
+	{ 0x80f017, 0x04 },
+	{ 0x80f018, 0x05 },
+	{ 0x80f019, 0x04 },
+	{ 0x80f01a, 0x05 },
+	{ 0x80f01f, 0x8c },
+	{ 0x80f020, 0x00 },
+	{ 0x80f021, 0x03 },
+	{ 0x80f022, 0x0a },
+	{ 0x80f023, 0x0a },
+	{ 0x80f029, 0x8c },
+	{ 0x80f02a, 0x00 },
+	{ 0x80f02b, 0x00 },
+	{ 0x80f02c, 0x01 },
+	{ 0x80f064, 0x03 },
+	{ 0x80f065, 0xf9 },
+	{ 0x80f066, 0x03 },
+	{ 0x80f067, 0x01 },
+	{ 0x80f06f, 0xe0 },
+	{ 0x80f070, 0x03 },
+	{ 0x80f072, 0x0f },
+	{ 0x80f073, 0x03 },
+	{ 0x80f077, 0x01 },
+	{ 0x80f078, 0x00 },
+	{ 0x80f085, 0xc0 },
+	{ 0x80f086, 0x01 },
+	{ 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 },
+	{ 0x80f24c, 0x88 },
+	{ 0x80f24d, 0x95 },
+	{ 0x80f24e, 0x9a },
+	{ 0x80f24f, 0x90 },
+	{ 0x80f25a, 0x07 },
+	{ 0x80f25b, 0xe8 },
+	{ 0x80f25c, 0x03 },
+	{ 0x80f25d, 0xb0 },
+	{ 0x80f25e, 0x04 },
+	{ 0x80f270, 0x01 },
+	{ 0x80f271, 0x02 },
+	{ 0x80f272, 0x01 },
+	{ 0x80f273, 0x02 },
+	{ 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 },
+};
+
+/* ITE Tech IT9135 Omega LNA config 2 tuner init
+   AF9033_TUNER_IT9135_52   = 0x52 */
+static const struct reg_val tuner_init_it9135_52[] = {
+	{ 0x800043, 0x00 },
+	{ 0x800046, 0x52 },
+	{ 0x800051, 0x01 },
+	{ 0x80005f, 0x00 },
+	{ 0x800060, 0x00 },
+	{ 0x800068, 0x10 },
+	{ 0x800070, 0x0a },
+	{ 0x800071, 0x05 },
+	{ 0x800072, 0x02 },
+	{ 0x800075, 0x8c },
+	{ 0x800076, 0x8c },
+	{ 0x800077, 0x8c },
+	{ 0x800078, 0xa0 },
+	{ 0x800079, 0x01 },
+	{ 0x80007e, 0x04 },
+	{ 0x80007f, 0x00 },
+	{ 0x800081, 0x0a },
+	{ 0x800082, 0x17 },
+	{ 0x800083, 0x03 },
+	{ 0x800084, 0x0a },
+	{ 0x800085, 0x03 },
+	{ 0x800086, 0xb3 },
+	{ 0x800087, 0x97 },
+	{ 0x800088, 0xc0 },
+	{ 0x800089, 0x9e },
+	{ 0x80008a, 0x01 },
+	{ 0x80008e, 0x01 },
+	{ 0x800092, 0x06 },
+	{ 0x800093, 0x00 },
+	{ 0x800094, 0x00 },
+	{ 0x800095, 0x00 },
+	{ 0x800096, 0x00 },
+	{ 0x800099, 0x01 },
+	{ 0x80009b, 0x3c },
+	{ 0x80009c, 0x28 },
+	{ 0x80009f, 0xe1 },
+	{ 0x8000a0, 0xcf },
+	{ 0x8000a3, 0x01 },
+	{ 0x8000a4, 0x5c },
+	{ 0x8000a5, 0x01 },
+	{ 0x8000a6, 0x01 },
+	{ 0x8000a9, 0x00 },
+	{ 0x8000aa, 0x01 },
+	{ 0x8000b0, 0x01 },
+	{ 0x8000b3, 0x02 },
+	{ 0x8000b4, 0x3c },
+	{ 0x8000b6, 0x14 },
+	{ 0x8000c0, 0x11 },
+	{ 0x8000c1, 0x00 },
+	{ 0x8000c2, 0x05 },
+	{ 0x8000c4, 0x00 },
+	{ 0x8000c6, 0x19 },
+	{ 0x8000c7, 0x00 },
+	{ 0x8000cc, 0x2e },
+	{ 0x8000cd, 0x51 },
+	{ 0x8000ce, 0x33 },
+	{ 0x8000f3, 0x05 },
+	{ 0x8000f4, 0x91 },
+	{ 0x8000f5, 0x8c },
+	{ 0x8000f8, 0x03 },
+	{ 0x8000f9, 0x06 },
+	{ 0x8000fa, 0x06 },
+	{ 0x8000fc, 0x03 },
+	{ 0x8000fd, 0x02 },
+	{ 0x8000fe, 0x02 },
+	{ 0x8000ff, 0x09 },
+	{ 0x800100, 0x50 },
+	{ 0x800101, 0x74 },
+	{ 0x800102, 0x77 },
+	{ 0x800103, 0x02 },
+	{ 0x800104, 0x02 },
+	{ 0x800105, 0xa4 },
+	{ 0x800106, 0x02 },
+	{ 0x800107, 0x6e },
+	{ 0x800109, 0x02 },
+	{ 0x800115, 0x0a },
+	{ 0x800116, 0x03 },
+	{ 0x800117, 0x02 },
+	{ 0x800118, 0x80 },
+	{ 0x80011a, 0xcd },
+	{ 0x80011b, 0x62 },
+	{ 0x80011c, 0xa4 },
+	{ 0x80011d, 0x8c },
+	{ 0x800122, 0x03 },
+	{ 0x800123, 0x18 },
+	{ 0x800124, 0x9e },
+	{ 0x800127, 0x00 },
+	{ 0x800128, 0x07 },
+	{ 0x80012a, 0x53 },
+	{ 0x80012b, 0x51 },
+	{ 0x80012c, 0x4e },
+	{ 0x80012d, 0x43 },
+	{ 0x800137, 0x00 },
+	{ 0x800138, 0x00 },
+	{ 0x800139, 0x07 },
+	{ 0x80013a, 0x00 },
+	{ 0x80013b, 0x06 },
+	{ 0x80013d, 0x00 },
+	{ 0x80013e, 0x01 },
+	{ 0x80013f, 0x5b },
+	{ 0x800140, 0xb6 },
+	{ 0x800141, 0x59 },
+	{ 0x80f000, 0x0f },
+	{ 0x80f016, 0x10 },
+	{ 0x80f017, 0x04 },
+	{ 0x80f018, 0x05 },
+	{ 0x80f019, 0x04 },
+	{ 0x80f01a, 0x05 },
+	{ 0x80f01f, 0x8c },
+	{ 0x80f020, 0x00 },
+	{ 0x80f021, 0x03 },
+	{ 0x80f022, 0x0a },
+	{ 0x80f023, 0x0a },
+	{ 0x80f029, 0x8c },
+	{ 0x80f02a, 0x00 },
+	{ 0x80f02b, 0x00 },
+	{ 0x80f02c, 0x01 },
+	{ 0x80f064, 0x03 },
+	{ 0x80f065, 0xf9 },
+	{ 0x80f066, 0x03 },
+	{ 0x80f067, 0x01 },
+	{ 0x80f06f, 0xe0 },
+	{ 0x80f070, 0x03 },
+	{ 0x80f072, 0x0f },
+	{ 0x80f073, 0x03 },
+	{ 0x80f077, 0x01 },
+	{ 0x80f078, 0x00 },
+	{ 0x80f085, 0xc0 },
+	{ 0x80f086, 0x01 },
+	{ 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 },
+	{ 0x80f24c, 0x88 },
+	{ 0x80f24d, 0x95 },
+	{ 0x80f24e, 0x9a },
+	{ 0x80f24f, 0x90 },
+	{ 0x80f25a, 0x07 },
+	{ 0x80f25b, 0xe8 },
+	{ 0x80f25c, 0x03 },
+	{ 0x80f25d, 0xb0 },
+	{ 0x80f25e, 0x04 },
+	{ 0x80f270, 0x01 },
+	{ 0x80f271, 0x02 },
+	{ 0x80f272, 0x01 },
+	{ 0x80f273, 0x02 },
+	{ 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 },
+};
+
+static const struct reg_val ofsm_init_it9135_v2[] = {
+	{ 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 },
+	{ 0x8000b0, 0x01 },
+	{ 0x8000c2, 0x05 },
+	{ 0x8000c6, 0x19 },
+	{ 0x80f000, 0x0f },
+	{ 0x80f02b, 0x00 },
+	{ 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 },
+	{ 0x80f5e3, 0x09 },
+	{ 0x80f5e4, 0x01 },
+	{ 0x80f5e5, 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 },
+};
+
+/* ITE Tech IT9135 Omega v2 tuner init
+   AF9033_TUNER_IT9135_60   = 0x60 */
+static const struct reg_val tuner_init_it9135_60[] = {
+	{ 0x800043, 0x00 },
+	{ 0x800046, 0x60 },
+	{ 0x800051, 0x01 },
+	{ 0x80005f, 0x00 },
+	{ 0x800060, 0x00 },
+	{ 0x800068, 0x0a },
+	{ 0x80006a, 0x03 },
+	{ 0x800070, 0x0a },
+	{ 0x800071, 0x05 },
+	{ 0x800072, 0x02 },
+	{ 0x800075, 0x8c },
+	{ 0x800076, 0x8c },
+	{ 0x800077, 0x8c },
+	{ 0x800078, 0x8c },
+	{ 0x800079, 0x01 },
+	{ 0x80007e, 0x04 },
+	{ 0x800081, 0x0a },
+	{ 0x800082, 0x18 },
+	{ 0x800084, 0x0a },
+	{ 0x800085, 0x33 },
+	{ 0x800086, 0xbe },
+	{ 0x800087, 0xa0 },
+	{ 0x800088, 0xc6 },
+	{ 0x800089, 0xb6 },
+	{ 0x80008a, 0x01 },
+	{ 0x80008e, 0x01 },
+	{ 0x800092, 0x06 },
+	{ 0x800093, 0x00 },
+	{ 0x800094, 0x00 },
+	{ 0x800095, 0x00 },
+	{ 0x800096, 0x00 },
+	{ 0x800099, 0x01 },
+	{ 0x80009b, 0x3c },
+	{ 0x80009c, 0x28 },
+	{ 0x80009f, 0xe1 },
+	{ 0x8000a0, 0xcf },
+	{ 0x8000a3, 0x01 },
+	{ 0x8000a4, 0x5a },
+	{ 0x8000a5, 0x01 },
+	{ 0x8000a6, 0x01 },
+	{ 0x8000a9, 0x00 },
+	{ 0x8000aa, 0x01 },
+	{ 0x8000b0, 0x01 },
+	{ 0x8000b3, 0x02 },
+	{ 0x8000b4, 0x3a },
+	{ 0x8000b6, 0x14 },
+	{ 0x8000c0, 0x11 },
+	{ 0x8000c1, 0x00 },
+	{ 0x8000c2, 0x05 },
+	{ 0x8000c3, 0x01 },
+	{ 0x8000c4, 0x00 },
+	{ 0x8000c6, 0x19 },
+	{ 0x8000c7, 0x00 },
+	{ 0x8000cb, 0x32 },
+	{ 0x8000cc, 0x2c },
+	{ 0x8000cd, 0x4f },
+	{ 0x8000ce, 0x30 },
+	{ 0x8000f3, 0x05 },
+	{ 0x8000f4, 0xa0 },
+	{ 0x8000f5, 0x8c },
+	{ 0x8000f8, 0x03 },
+	{ 0x8000f9, 0x06 },
+	{ 0x8000fa, 0x06 },
+	{ 0x8000fc, 0x03 },
+	{ 0x8000fd, 0x03 },
+	{ 0x8000fe, 0x02 },
+	{ 0x8000ff, 0x0a },
+	{ 0x800100, 0x50 },
+	{ 0x800101, 0x7b },
+	{ 0x800102, 0x8c },
+	{ 0x800103, 0x00 },
+	{ 0x800104, 0x02 },
+	{ 0x800105, 0xbe },
+	{ 0x800106, 0x00 },
+	{ 0x800109, 0x02 },
+	{ 0x800115, 0x0a },
+	{ 0x800116, 0x03 },
+	{ 0x80011a, 0xbe },
+	{ 0x800124, 0xae },
+	{ 0x800127, 0x00 },
+	{ 0x80012a, 0x56 },
+	{ 0x80012b, 0x50 },
+	{ 0x80012c, 0x47 },
+	{ 0x80012d, 0x42 },
+	{ 0x800137, 0x00 },
+	{ 0x80013b, 0x08 },
+	{ 0x80013f, 0x5b },
+	{ 0x800141, 0x59 },
+	{ 0x800142, 0xf9 },
+	{ 0x800143, 0x19 },
+	{ 0x800144, 0x00 },
+	{ 0x800145, 0x8c },
+	{ 0x800146, 0x8c },
+	{ 0x800147, 0x8c },
+	{ 0x800148, 0x6e },
+	{ 0x800149, 0x8c },
+	{ 0x80014a, 0x50 },
+	{ 0x80014b, 0x8c },
+	{ 0x80014d, 0xac },
+	{ 0x80014e, 0xc6 },
+	{ 0x80014f, 0x03 },
+	{ 0x800151, 0x1e },
+	{ 0x800153, 0xbc },
+	{ 0x800178, 0x09 },
+	{ 0x800181, 0x94 },
+	{ 0x800182, 0x6e },
+	{ 0x800185, 0x24 },
+	{ 0x800189, 0xbe },
+	{ 0x80018c, 0x03 },
+	{ 0x80018d, 0x5f },
+	{ 0x80018f, 0xa0 },
+	{ 0x800190, 0x5a },
+	{ 0x80ed02, 0xff },
+	{ 0x80ee42, 0xff },
+	{ 0x80ee82, 0xff },
+	{ 0x80f000, 0x0f },
+	{ 0x80f01f, 0x8c },
+	{ 0x80f020, 0x00 },
+	{ 0x80f029, 0x8c },
+	{ 0x80f02a, 0x00 },
+	{ 0x80f02b, 0x00 },
+	{ 0x80f064, 0x03 },
+	{ 0x80f065, 0xf9 },
+	{ 0x80f066, 0x03 },
+	{ 0x80f067, 0x01 },
+	{ 0x80f06f, 0xe0 },
+	{ 0x80f070, 0x03 },
+	{ 0x80f072, 0x0f },
+	{ 0x80f073, 0x03 },
+	{ 0x80f077, 0x01 },
+	{ 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 },
+	{ 0x80f24c, 0x88 },
+	{ 0x80f24d, 0x95 },
+	{ 0x80f24e, 0x9a },
+	{ 0x80f24f, 0x90 },
+	{ 0x80f25a, 0x07 },
+	{ 0x80f25b, 0xe8 },
+	{ 0x80f25c, 0x03 },
+	{ 0x80f25d, 0xb0 },
+	{ 0x80f25e, 0x04 },
+	{ 0x80f270, 0x01 },
+	{ 0x80f271, 0x02 },
+	{ 0x80f272, 0x01 },
+	{ 0x80f273, 0x02 },
+	{ 0x80f40e, 0x0a },
+	{ 0x80f40f, 0x40 },
+	{ 0x80f410, 0x08 },
+	{ 0x80f55f, 0x0a },
+	{ 0x80f561, 0x15 },
+	{ 0x80f562, 0x20 },
+	{ 0x80f5e3, 0x09 },
+	{ 0x80f5e4, 0x01 },
+	{ 0x80f5e5, 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 },
+};
 
+/* ITE Tech IT9135 Omega v2 LNA config 1 tuner init
+   AF9033_TUNER_IT9135_61   = 0x61 */
+static const struct reg_val tuner_init_it9135_61[] = {
+	{ 0x800043, 0x00 },
+	{ 0x800046, 0x61 },
+	{ 0x800051, 0x01 },
+	{ 0x80005f, 0x00 },
+	{ 0x800060, 0x00 },
+	{ 0x800068, 0x06 },
+	{ 0x80006a, 0x03 },
+	{ 0x800070, 0x0a },
+	{ 0x800071, 0x05 },
+	{ 0x800072, 0x02 },
+	{ 0x800075, 0x8c },
+	{ 0x800076, 0x8c },
+	{ 0x800077, 0x8c },
+	{ 0x800078, 0x90 },
+	{ 0x800079, 0x01 },
+	{ 0x80007e, 0x04 },
+	{ 0x800081, 0x0a },
+	{ 0x800082, 0x12 },
+	{ 0x800084, 0x0a },
+	{ 0x800085, 0x33 },
+	{ 0x800086, 0xbc },
+	{ 0x800087, 0x9c },
+	{ 0x800088, 0xcc },
+	{ 0x800089, 0xa8 },
+	{ 0x80008a, 0x01 },
+	{ 0x80008e, 0x01 },
+	{ 0x800092, 0x06 },
+	{ 0x800093, 0x00 },
+	{ 0x800094, 0x00 },
+	{ 0x800095, 0x00 },
+	{ 0x800096, 0x00 },
+	{ 0x800099, 0x01 },
+	{ 0x80009b, 0x3c },
+	{ 0x80009c, 0x28 },
+	{ 0x80009f, 0xe1 },
+	{ 0x8000a0, 0xcf },
+	{ 0x8000a3, 0x01 },
+	{ 0x8000a4, 0x5c },
+	{ 0x8000a5, 0x01 },
+	{ 0x8000a6, 0x01 },
+	{ 0x8000a9, 0x00 },
+	{ 0x8000aa, 0x01 },
+	{ 0x8000b0, 0x01 },
+	{ 0x8000b3, 0x02 },
+	{ 0x8000b4, 0x3a },
+	{ 0x8000b6, 0x14 },
+	{ 0x8000c0, 0x11 },
+	{ 0x8000c1, 0x00 },
+	{ 0x8000c2, 0x05 },
+	{ 0x8000c3, 0x01 },
+	{ 0x8000c4, 0x00 },
+	{ 0x8000c6, 0x19 },
+	{ 0x8000c7, 0x00 },
+	{ 0x8000cb, 0x32 },
+	{ 0x8000cc, 0x2c },
+	{ 0x8000cd, 0x4f },
+	{ 0x8000ce, 0x30 },
+	{ 0x8000f3, 0x05 },
+	{ 0x8000f4, 0xa0 },
+	{ 0x8000f5, 0x8c },
+	{ 0x8000f8, 0x03 },
+	{ 0x8000f9, 0x06 },
+	{ 0x8000fa, 0x06 },
+	{ 0x8000fc, 0x03 },
+	{ 0x8000fd, 0x03 },
+	{ 0x8000fe, 0x02 },
+	{ 0x8000ff, 0x08 },
+	{ 0x800100, 0x50 },
+	{ 0x800101, 0x7b },
+	{ 0x800102, 0x8c },
+	{ 0x800103, 0x01 },
+	{ 0x800104, 0x02 },
+	{ 0x800105, 0xc8 },
+	{ 0x800106, 0x00 },
+	{ 0x800109, 0x02 },
+	{ 0x800115, 0x0a },
+	{ 0x800116, 0x03 },
+	{ 0x80011a, 0xc6 },
+	{ 0x800124, 0xa8 },
+	{ 0x800127, 0x00 },
+	{ 0x80012a, 0x59 },
+	{ 0x80012b, 0x50 },
+	{ 0x80012c, 0x47 },
+	{ 0x80012d, 0x42 },
+	{ 0x800137, 0x00 },
+	{ 0x80013b, 0x05 },
+	{ 0x80013f, 0x5b },
+	{ 0x800141, 0x59 },
+	{ 0x800142, 0xf9 },
+	{ 0x800143, 0x59 },
+	{ 0x800144, 0x01 },
+	{ 0x800145, 0x8c },
+	{ 0x800146, 0x8c },
+	{ 0x800147, 0x8c },
+	{ 0x800148, 0x7b },
+	{ 0x800149, 0x8c },
+	{ 0x80014a, 0x50 },
+	{ 0x80014b, 0x8c },
+	{ 0x80014d, 0xa8 },
+	{ 0x80014e, 0xc6 },
+	{ 0x80014f, 0x03 },
+	{ 0x800151, 0x28 },
+	{ 0x800153, 0xcc },
+	{ 0x800178, 0x09 },
+	{ 0x800181, 0x9c },
+	{ 0x800182, 0x76 },
+	{ 0x800185, 0x28 },
+	{ 0x800189, 0xaa },
+	{ 0x80018c, 0x03 },
+	{ 0x80018d, 0x5f },
+	{ 0x80018f, 0xfb },
+	{ 0x800190, 0x5c },
+	{ 0x80ed02, 0xff },
+	{ 0x80ee42, 0xff },
+	{ 0x80ee82, 0xff },
+	{ 0x80f000, 0x0f },
+	{ 0x80f01f, 0x8c },
+	{ 0x80f020, 0x00 },
+	{ 0x80f029, 0x8c },
+	{ 0x80f02a, 0x00 },
+	{ 0x80f02b, 0x00 },
+	{ 0x80f064, 0x03 },
+	{ 0x80f065, 0xf9 },
+	{ 0x80f066, 0x03 },
+	{ 0x80f067, 0x01 },
+	{ 0x80f06f, 0xe0 },
+	{ 0x80f070, 0x03 },
+	{ 0x80f072, 0x0f },
+	{ 0x80f073, 0x03 },
+	{ 0x80f077, 0x01 },
+	{ 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 },
+	{ 0x80f24c, 0x88 },
+	{ 0x80f24d, 0x95 },
+	{ 0x80f24e, 0x9a },
+	{ 0x80f24f, 0x90 },
+	{ 0x80f25a, 0x07 },
+	{ 0x80f25b, 0xe8 },
+	{ 0x80f25c, 0x03 },
+	{ 0x80f25d, 0xb0 },
+	{ 0x80f25e, 0x04 },
+	{ 0x80f270, 0x01 },
+	{ 0x80f271, 0x02 },
+	{ 0x80f272, 0x01 },
+	{ 0x80f273, 0x02 },
+	{ 0x80f40e, 0x0a },
+	{ 0x80f40f, 0x40 },
+	{ 0x80f410, 0x08 },
+	{ 0x80f55f, 0x0a },
+	{ 0x80f561, 0x15 },
+	{ 0x80f562, 0x20 },
+	{ 0x80f5e3, 0x09 },
+	{ 0x80f5e4, 0x01 },
+	{ 0x80f5e5, 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 },
+};
+
+/* ITE Tech IT9135 Omega v2 LNA config 2 tuner init
+   AF9033_TUNER_IT9135_62   = 0x62 */
+static const struct reg_val tuner_init_it9135_62[] = {
+	{ 0x800043, 0x00 },
+	{ 0x800046, 0x62 },
+	{ 0x800051, 0x01 },
+	{ 0x80005f, 0x00 },
+	{ 0x800060, 0x00 },
+	{ 0x800068, 0x0a },
+	{ 0x80006a, 0x03 },
+	{ 0x800070, 0x0a },
+	{ 0x800071, 0x05 },
+	{ 0x800072, 0x02 },
+	{ 0x800075, 0x8c },
+	{ 0x800076, 0x8c },
+	{ 0x800077, 0x8c },
+	{ 0x800078, 0x8c },
+	{ 0x800079, 0x01 },
+	{ 0x80007e, 0x04 },
+	{ 0x800081, 0x0a },
+	{ 0x800082, 0x12 },
+	{ 0x800084, 0x0a },
+	{ 0x800085, 0x33 },
+	{ 0x800086, 0xb8 },
+	{ 0x800087, 0x9c },
+	{ 0x800088, 0xb2 },
+	{ 0x800089, 0xa6 },
+	{ 0x80008a, 0x01 },
+	{ 0x80008e, 0x01 },
+	{ 0x800092, 0x06 },
+	{ 0x800093, 0x00 },
+	{ 0x800094, 0x00 },
+	{ 0x800095, 0x00 },
+	{ 0x800096, 0x00 },
+	{ 0x800099, 0x01 },
+	{ 0x80009b, 0x3c },
+	{ 0x80009c, 0x28 },
+	{ 0x80009f, 0xe1 },
+	{ 0x8000a0, 0xcf },
+	{ 0x8000a3, 0x01 },
+	{ 0x8000a4, 0x5a },
+	{ 0x8000a5, 0x01 },
+	{ 0x8000a6, 0x01 },
+	{ 0x8000a9, 0x00 },
+	{ 0x8000aa, 0x01 },
+	{ 0x8000b0, 0x01 },
+	{ 0x8000b3, 0x02 },
+	{ 0x8000b4, 0x3a },
+	{ 0x8000b6, 0x14 },
+	{ 0x8000c0, 0x11 },
+	{ 0x8000c1, 0x00 },
+	{ 0x8000c2, 0x05 },
+	{ 0x8000c3, 0x01 },
+	{ 0x8000c4, 0x00 },
+	{ 0x8000c6, 0x19 },
+	{ 0x8000c7, 0x00 },
+	{ 0x8000cb, 0x32 },
+	{ 0x8000cc, 0x2c },
+	{ 0x8000cd, 0x4f },
+	{ 0x8000ce, 0x30 },
+	{ 0x8000f3, 0x05 },
+	{ 0x8000f4, 0x8c },
+	{ 0x8000f5, 0x8c },
+	{ 0x8000f8, 0x03 },
+	{ 0x8000f9, 0x06 },
+	{ 0x8000fa, 0x06 },
+	{ 0x8000fc, 0x02 },
+	{ 0x8000fd, 0x03 },
+	{ 0x8000fe, 0x02 },
+	{ 0x8000ff, 0x09 },
+	{ 0x800100, 0x50 },
+	{ 0x800101, 0x6e },
+	{ 0x800102, 0x8c },
+	{ 0x800103, 0x02 },
+	{ 0x800104, 0x02 },
+	{ 0x800105, 0xc2 },
+	{ 0x800106, 0x00 },
+	{ 0x800109, 0x02 },
+	{ 0x800115, 0x0a },
+	{ 0x800116, 0x03 },
+	{ 0x80011a, 0xb8 },
+	{ 0x800124, 0xa8 },
+	{ 0x800127, 0x00 },
+	{ 0x80012a, 0x53 },
+	{ 0x80012b, 0x51 },
+	{ 0x80012c, 0x4e },
+	{ 0x80012d, 0x43 },
+	{ 0x800137, 0x00 },
+	{ 0x80013b, 0x05 },
+	{ 0x80013f, 0x5b },
+	{ 0x800141, 0x59 },
+	{ 0x800142, 0xf9 },
+	{ 0x800143, 0x59 },
+	{ 0x800144, 0x00 },
+	{ 0x800145, 0x8c },
+	{ 0x800146, 0x8c },
+	{ 0x800147, 0x8c },
+	{ 0x800148, 0x7b },
+	{ 0x800149, 0x8c },
+	{ 0x80014a, 0x50 },
+	{ 0x80014b, 0x70 },
+	{ 0x80014d, 0x96 },
+	{ 0x80014e, 0xd0 },
+	{ 0x80014f, 0x03 },
+	{ 0x800151, 0x28 },
+	{ 0x800153, 0xb2 },
+	{ 0x800178, 0x09 },
+	{ 0x800181, 0x9c },
+	{ 0x800182, 0x6e },
+	{ 0x800185, 0x24 },
+	{ 0x800189, 0xb8 },
+	{ 0x80018c, 0x03 },
+	{ 0x80018d, 0x5f },
+	{ 0x80018f, 0xfb },
+	{ 0x800190, 0x5a },
+	{ 0x80ed02, 0xff },
+	{ 0x80ee42, 0xff },
+	{ 0x80ee82, 0xff },
+	{ 0x80f000, 0x0f },
+	{ 0x80f01f, 0x8c },
+	{ 0x80f020, 0x00 },
+	{ 0x80f029, 0x8c },
+	{ 0x80f02a, 0x00 },
+	{ 0x80f02b, 0x00 },
+	{ 0x80f064, 0x03 },
+	{ 0x80f065, 0xf9 },
+	{ 0x80f066, 0x03 },
+	{ 0x80f067, 0x01 },
+	{ 0x80f06f, 0xe0 },
+	{ 0x80f070, 0x03 },
+	{ 0x80f072, 0x0f },
+	{ 0x80f073, 0x03 },
+	{ 0x80f077, 0x01 },
+	{ 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 },
+	{ 0x80f24c, 0x88 },
+	{ 0x80f24d, 0x95 },
+	{ 0x80f24e, 0x9a },
+	{ 0x80f24f, 0x90 },
+	{ 0x80f25a, 0x07 },
+	{ 0x80f25b, 0xe8 },
+	{ 0x80f25c, 0x03 },
+	{ 0x80f25d, 0xb0 },
+	{ 0x80f25e, 0x04 },
+	{ 0x80f270, 0x01 },
+	{ 0x80f271, 0x02 },
+	{ 0x80f272, 0x01 },
+	{ 0x80f273, 0x02 },
+	{ 0x80f40e, 0x0a },
+	{ 0x80f40f, 0x40 },
+	{ 0x80f410, 0x08 },
+	{ 0x80f55f, 0x0a },
+	{ 0x80f561, 0x15 },
+	{ 0x80f562, 0x20 },
+	{ 0x80f5e3, 0x09 },
+	{ 0x80f5e4, 0x01 },
+	{ 0x80f5e5, 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 },
+};
+
+#endif /* AF9033_PRIV_H */
diff --git a/drivers/media/dvb-frontends/atbm8830.h b/drivers/media/dvb-frontends/atbm8830.h
index 024273374bd..8e0ac98f8d0 100644
--- a/drivers/media/dvb-frontends/atbm8830.h
+++ b/drivers/media/dvb-frontends/atbm8830.h
@@ -22,6 +22,7 @@
 #ifndef __ATBM8830_H__
 #define __ATBM8830_H__
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include <linux/i2c.h>
 
@@ -60,8 +61,7 @@ struct atbm8830_config {
 	u8 agc_hold_loop;
 };
 
-#if defined(CONFIG_DVB_ATBM8830) || \
-	(defined(CONFIG_DVB_ATBM8830_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_ATBM8830)
 extern struct dvb_frontend *atbm8830_attach(const struct atbm8830_config *config,
 		struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/au8522.h b/drivers/media/dvb-frontends/au8522.h
index 565dcf31af5..83fe9a61561 100644
--- a/drivers/media/dvb-frontends/au8522.h
+++ b/drivers/media/dvb-frontends/au8522.h
@@ -22,6 +22,7 @@
 #ifndef __AU8522_H__
 #define __AU8522_H__
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 enum au8522_if_freq {
@@ -60,8 +61,7 @@ struct au8522_config {
 	enum au8522_if_freq qam_if;
 };
 
-#if defined(CONFIG_DVB_AU8522) || 				\
-	    (defined(CONFIG_DVB_AU8522_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_AU8522_DTV)
 extern struct dvb_frontend *au8522_attach(const struct au8522_config *config,
 					  struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/au8522_decoder.c b/drivers/media/dvb-frontends/au8522_decoder.c
index 5243ba6295c..2099f21e374 100644
--- a/drivers/media/dvb-frontends/au8522_decoder.c
+++ b/drivers/media/dvb-frontends/au8522_decoder.c
@@ -229,15 +229,11 @@ static void setup_decoder_defaults(struct au8522_state *state, u8 input_mode)
 	/* Provide reasonable defaults for picture tuning values */
 	au8522_writereg(state, AU8522_TVDEC_SHARPNESSREG009H, 0x07);
 	au8522_writereg(state, AU8522_TVDEC_BRIGHTNESS_REG00AH, 0xed);
-	state->brightness = 0xed - 128;
 	au8522_writereg(state, AU8522_TVDEC_CONTRAST_REG00BH, 0x79);
-	state->contrast = 0x79;
 	au8522_writereg(state, AU8522_TVDEC_SATURATION_CB_REG00CH, 0x80);
 	au8522_writereg(state, AU8522_TVDEC_SATURATION_CR_REG00DH, 0x80);
-	state->saturation = 0x80;
 	au8522_writereg(state, AU8522_TVDEC_HUE_H_REG00EH, 0x00);
 	au8522_writereg(state, AU8522_TVDEC_HUE_L_REG00FH, 0x00);
-	state->hue = 0x00;
 
 	/* Other decoder registers */
 	au8522_writereg(state, AU8522_TVDEC_INT_MASK_REG010H, 0x00);
@@ -489,75 +485,32 @@ static void set_audio_input(struct au8522_state *state, int aud_input)
 
 /* ----------------------------------------------------------------------- */
 
-static int au8522_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int au8522_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct au8522_state *state = to_state(sd);
+	struct au8522_state *state =
+		container_of(ctrl->handler, struct au8522_state, hdl);
 
 	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		state->brightness = ctrl->value;
 		au8522_writereg(state, AU8522_TVDEC_BRIGHTNESS_REG00AH,
-				ctrl->value - 128);
+				ctrl->val - 128);
 		break;
 	case V4L2_CID_CONTRAST:
-		state->contrast = ctrl->value;
 		au8522_writereg(state, AU8522_TVDEC_CONTRAST_REG00BH,
-				ctrl->value);
+				ctrl->val);
 		break;
 	case V4L2_CID_SATURATION:
-		state->saturation = ctrl->value;
 		au8522_writereg(state, AU8522_TVDEC_SATURATION_CB_REG00CH,
-				ctrl->value);
+				ctrl->val);
 		au8522_writereg(state, AU8522_TVDEC_SATURATION_CR_REG00DH,
-				ctrl->value);
+				ctrl->val);
 		break;
 	case V4L2_CID_HUE:
-		state->hue = ctrl->value;
 		au8522_writereg(state, AU8522_TVDEC_HUE_H_REG00EH,
-				ctrl->value >> 8);
+				ctrl->val >> 8);
 		au8522_writereg(state, AU8522_TVDEC_HUE_L_REG00FH,
-				ctrl->value & 0xFF);
-		break;
-	case V4L2_CID_AUDIO_VOLUME:
-	case V4L2_CID_AUDIO_BASS:
-	case V4L2_CID_AUDIO_TREBLE:
-	case V4L2_CID_AUDIO_BALANCE:
-	case V4L2_CID_AUDIO_MUTE:
-		/* Not yet implemented */
-	default:
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static int au8522_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-	struct au8522_state *state = to_state(sd);
-
-	/* Note that we are using values cached in the state structure instead
-	   of reading the registers due to issues with i2c reads not working
-	   properly/consistently yet on the HVR-950q */
-
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		ctrl->value = state->brightness;
-		break;
-	case V4L2_CID_CONTRAST:
-		ctrl->value = state->contrast;
+				ctrl->val & 0xFF);
 		break;
-	case V4L2_CID_SATURATION:
-		ctrl->value = state->saturation;
-		break;
-	case V4L2_CID_HUE:
-		ctrl->value = state->hue;
-		break;
-	case V4L2_CID_AUDIO_VOLUME:
-	case V4L2_CID_AUDIO_BASS:
-	case V4L2_CID_AUDIO_TREBLE:
-	case V4L2_CID_AUDIO_BALANCE:
-	case V4L2_CID_AUDIO_MUTE:
-		/* Not yet supported */
 	default:
 		return -EINVAL;
 	}
@@ -583,7 +536,7 @@ static int au8522_g_register(struct v4l2_subdev *sd,
 }
 
 static int au8522_s_register(struct v4l2_subdev *sd,
-			     struct v4l2_dbg_register *reg)
+			     const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct au8522_state *state = to_state(sd);
@@ -616,26 +569,6 @@ static int au8522_s_stream(struct v4l2_subdev *sd, int enable)
 	return 0;
 }
 
-static int au8522_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc)
-{
-	switch (qc->id) {
-	case V4L2_CID_CONTRAST:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1,
-					    AU8522_TVDEC_CONTRAST_REG00BH_CVBS);
-	case V4L2_CID_BRIGHTNESS:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 109);
-	case V4L2_CID_SATURATION:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 128);
-	case V4L2_CID_HUE:
-		return v4l2_ctrl_query_fill(qc, -32768, 32768, 1, 0);
-	default:
-		break;
-	}
-
-	qc->type = 0;
-	return -EINVAL;
-}
-
 static int au8522_reset(struct v4l2_subdev *sd, u32 val)
 {
 	struct au8522_state *state = to_state(sd);
@@ -712,20 +645,11 @@ static int au8522_g_chip_ident(struct v4l2_subdev *sd,
 	return v4l2_chip_ident_i2c_client(client, chip, state->id, state->rev);
 }
 
-static int au8522_log_status(struct v4l2_subdev *sd)
-{
-	/* FIXME: Add some status info here */
-	return 0;
-}
-
 /* ----------------------------------------------------------------------- */
 
 static const struct v4l2_subdev_core_ops au8522_core_ops = {
-	.log_status = au8522_log_status,
+	.log_status = v4l2_ctrl_subdev_log_status,
 	.g_chip_ident = au8522_g_chip_ident,
-	.g_ctrl = au8522_g_ctrl,
-	.s_ctrl = au8522_s_ctrl,
-	.queryctrl = au8522_queryctrl,
 	.reset = au8522_reset,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 	.g_register = au8522_g_register,
@@ -753,12 +677,17 @@ static const struct v4l2_subdev_ops au8522_ops = {
 	.video = &au8522_video_ops,
 };
 
+static const struct v4l2_ctrl_ops au8522_ctrl_ops = {
+	.s_ctrl = au8522_s_ctrl,
+};
+
 /* ----------------------------------------------------------------------- */
 
 static int au8522_probe(struct i2c_client *client,
 			const struct i2c_device_id *did)
 {
 	struct au8522_state *state;
+	struct v4l2_ctrl_handler *hdl;
 	struct v4l2_subdev *sd;
 	int instance;
 	struct au8522_config *demod_config;
@@ -799,6 +728,27 @@ static int au8522_probe(struct i2c_client *client,
 	sd = &state->sd;
 	v4l2_i2c_subdev_init(sd, client, &au8522_ops);
 
+	hdl = &state->hdl;
+	v4l2_ctrl_handler_init(hdl, 4);
+	v4l2_ctrl_new_std(hdl, &au8522_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 109);
+	v4l2_ctrl_new_std(hdl, &au8522_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1,
+			AU8522_TVDEC_CONTRAST_REG00BH_CVBS);
+	v4l2_ctrl_new_std(hdl, &au8522_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(hdl, &au8522_ctrl_ops,
+			V4L2_CID_HUE, -32768, 32767, 1, 0);
+	sd->ctrl_handler = hdl;
+	if (hdl->error) {
+		int err = hdl->error;
+
+		v4l2_ctrl_handler_free(hdl);
+		kfree(demod_config);
+		kfree(state);
+		return err;
+	}
+
 	state->c = client;
 	state->vid_input = AU8522_COMPOSITE_CH1;
 	state->aud_input = AU8522_AUDIO_NONE;
@@ -815,6 +765,7 @@ static int au8522_remove(struct i2c_client *client)
 {
 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
 	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
 	au8522_release_state(to_state(sd));
 	return 0;
 }
diff --git a/drivers/media/dvb-frontends/au8522_priv.h b/drivers/media/dvb-frontends/au8522_priv.h
index 0529699a27b..aa0f16d6b61 100644
--- a/drivers/media/dvb-frontends/au8522_priv.h
+++ b/drivers/media/dvb-frontends/au8522_priv.h
@@ -29,6 +29,7 @@
 #include <linux/delay.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
 #include "au8522.h"
@@ -65,10 +66,7 @@ struct au8522_state {
 	int aud_input;
 	u32 id;
 	u32 rev;
-	u8 brightness;
-	u8 contrast;
-	u8 saturation;
-	s16 hue;
+	struct v4l2_ctrl_handler hdl;
 };
 
 /* These are routines shared by both the VSB/QAM demodulator and the analog
diff --git a/drivers/media/dvb-frontends/cx22702.h b/drivers/media/dvb-frontends/cx22702.h
index f154e1f428e..0b1a6c2f9d5 100644
--- a/drivers/media/dvb-frontends/cx22702.h
+++ b/drivers/media/dvb-frontends/cx22702.h
@@ -28,6 +28,7 @@
 #ifndef CX22702_H
 #define CX22702_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct cx22702_config {
@@ -40,8 +41,7 @@ struct cx22702_config {
 	u8 output_mode;
 };
 
-#if defined(CONFIG_DVB_CX22702) || (defined(CONFIG_DVB_CX22702_MODULE) \
-	&& defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_CX22702)
 extern struct dvb_frontend *cx22702_attach(
 	const struct cx22702_config *config,
 	struct i2c_adapter *i2c);
diff --git a/drivers/media/dvb-frontends/cx24113.h b/drivers/media/dvb-frontends/cx24113.h
index 01eb7b9c28f..782711ba1a3 100644
--- a/drivers/media/dvb-frontends/cx24113.h
+++ b/drivers/media/dvb-frontends/cx24113.h
@@ -22,6 +22,8 @@
 #ifndef CX24113_H
 #define CX24113_H
 
+#include <linux/kconfig.h>
+
 struct dvb_frontend;
 
 struct cx24113_config {
@@ -30,8 +32,7 @@ struct cx24113_config {
 	u32 xtal_khz;
 };
 
-#if defined(CONFIG_DVB_TUNER_CX24113) || \
-	(defined(CONFIG_DVB_TUNER_CX24113_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_TUNER_CX24113)
 extern struct dvb_frontend *cx24113_attach(struct dvb_frontend *,
 	const struct cx24113_config *config, struct i2c_adapter *i2c);
 
diff --git a/drivers/media/dvb-frontends/cx24116.h b/drivers/media/dvb-frontends/cx24116.h
index 7d90ab949c0..2ec84fae3f9 100644
--- a/drivers/media/dvb-frontends/cx24116.h
+++ b/drivers/media/dvb-frontends/cx24116.h
@@ -21,6 +21,7 @@
 #ifndef CX24116_H
 #define CX24116_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct cx24116_config {
@@ -40,8 +41,7 @@ struct cx24116_config {
 	u16 i2c_wr_max;
 };
 
-#if defined(CONFIG_DVB_CX24116) || \
-	(defined(CONFIG_DVB_CX24116_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_CX24116)
 extern struct dvb_frontend *cx24116_attach(
 	const struct cx24116_config *config,
 	struct i2c_adapter *i2c);
diff --git a/drivers/media/dvb-frontends/cx24123.c b/drivers/media/dvb-frontends/cx24123.c
index 68c88ab58e7..a771da3e9f9 100644
--- a/drivers/media/dvb-frontends/cx24123.c
+++ b/drivers/media/dvb-frontends/cx24123.c
@@ -26,6 +26,7 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/init.h>
+#include <asm/div64.h>
 
 #include "dvb_frontend.h"
 #include "cx24123.h"
@@ -452,7 +453,8 @@ static u32 cx24123_int_log2(u32 a, u32 b)
 
 static int cx24123_set_symbolrate(struct cx24123_state *state, u32 srate)
 {
-	u32 tmp, sample_rate, ratio, sample_gain;
+	u64 tmp;
+	u32 sample_rate, ratio, sample_gain;
 	u8 pll_mult;
 
 	/*  check if symbol rate is within limits */
@@ -482,27 +484,11 @@ static int cx24123_set_symbolrate(struct cx24123_state *state, u32 srate)
 
 	sample_rate = pll_mult * XTAL;
 
-	/*
-	    SYSSymbolRate[21:0] = (srate << 23) / sample_rate
-
-	    We have to use 32 bit unsigned arithmetic without precision loss.
-	    The maximum srate is 45000000 or 0x02AEA540. This number has
-	    only 6 clear bits on top, hence we can shift it left only 6 bits
-	    at a time. Borrowed from cx24110.c
-	*/
-
-	tmp = srate << 6;
-	ratio = tmp / sample_rate;
-
-	tmp = (tmp % sample_rate) << 6;
-	ratio = (ratio << 6) + (tmp / sample_rate);
-
-	tmp = (tmp % sample_rate) << 6;
-	ratio = (ratio << 6) + (tmp / sample_rate);
-
-	tmp = (tmp % sample_rate) << 5;
-	ratio = (ratio << 5) + (tmp / sample_rate);
+	/* SYSSymbolRate[21:0] = (srate << 23) / sample_rate */
 
+	tmp = ((u64)srate) << 23;
+	do_div(tmp, sample_rate);
+	ratio = (u32) tmp;
 
 	cx24123_writereg(state, 0x01, pll_mult * 6);
 
diff --git a/drivers/media/dvb-frontends/cx24123.h b/drivers/media/dvb-frontends/cx24123.h
index 51ae866e9fe..102e70d17c4 100644
--- a/drivers/media/dvb-frontends/cx24123.h
+++ b/drivers/media/dvb-frontends/cx24123.h
@@ -21,6 +21,7 @@
 #ifndef CX24123_H
 #define CX24123_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct cx24123_config {
@@ -38,8 +39,7 @@ struct cx24123_config {
 	void (*agc_callback) (struct dvb_frontend *);
 };
 
-#if defined(CONFIG_DVB_CX24123) || (defined(CONFIG_DVB_CX24123_MODULE) \
-	&& defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_CX24123)
 extern struct dvb_frontend *cx24123_attach(const struct cx24123_config *config,
 					   struct i2c_adapter *i2c);
 extern struct i2c_adapter *cx24123_get_tuner_i2c_adapter(struct dvb_frontend *);
diff --git a/drivers/media/dvb-frontends/cxd2820r.h b/drivers/media/dvb-frontends/cxd2820r.h
index 6acc21c581c..82b3d93718f 100644
--- a/drivers/media/dvb-frontends/cxd2820r.h
+++ b/drivers/media/dvb-frontends/cxd2820r.h
@@ -22,6 +22,7 @@
 #ifndef CXD2820R_H
 #define CXD2820R_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 #define CXD2820R_GPIO_D (0 << 0) /* disable */
@@ -65,8 +66,7 @@ struct cxd2820r_config {
 };
 
 
-#if defined(CONFIG_DVB_CXD2820R) || \
-	(defined(CONFIG_DVB_CXD2820R_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_CXD2820R)
 extern struct dvb_frontend *cxd2820r_attach(
 	const struct cxd2820r_config *config,
 	struct i2c_adapter *i2c,
diff --git a/drivers/media/dvb-frontends/cxd2820r_core.c b/drivers/media/dvb-frontends/cxd2820r_core.c
index 9b658c1cf39..7ca5c69dd20 100644
--- a/drivers/media/dvb-frontends/cxd2820r_core.c
+++ b/drivers/media/dvb-frontends/cxd2820r_core.c
@@ -660,7 +660,8 @@ static const struct dvb_frontend_ops cxd2820r_ops = {
 			FE_CAN_GUARD_INTERVAL_AUTO	|
 			FE_CAN_HIERARCHY_AUTO		|
 			FE_CAN_MUTE_TS			|
-			FE_CAN_2G_MODULATION
+			FE_CAN_2G_MODULATION		|
+			FE_CAN_MULTISTREAM
 		},
 
 	.release		= cxd2820r_release,
diff --git a/drivers/media/dvb-frontends/cxd2820r_t2.c b/drivers/media/dvb-frontends/cxd2820r_t2.c
index e82d82a7a2e..2ba130e245b 100644
--- a/drivers/media/dvb-frontends/cxd2820r_t2.c
+++ b/drivers/media/dvb-frontends/cxd2820r_t2.c
@@ -124,6 +124,23 @@ int cxd2820r_set_frontend_t2(struct dvb_frontend *fe)
 	buf[1] = ((if_ctl >>  8) & 0xff);
 	buf[2] = ((if_ctl >>  0) & 0xff);
 
+	/* PLP filtering */
+	if (c->stream_id > 255) {
+		dev_dbg(&priv->i2c->dev, "%s: Disable PLP filtering\n", __func__);
+		ret = cxd2820r_wr_reg(priv, 0x023ad , 0);
+		if (ret)
+			goto error;
+	} else {
+		dev_dbg(&priv->i2c->dev, "%s: Enable PLP filtering = %d\n", __func__,
+				c->stream_id);
+		ret = cxd2820r_wr_reg(priv, 0x023af , c->stream_id & 0xFF);
+		if (ret)
+			goto error;
+		ret = cxd2820r_wr_reg(priv, 0x023ad , 1);
+		if (ret)
+			goto error;
+	}
+
 	ret = cxd2820r_wr_regs(priv, 0x020b6, buf, 3);
 	if (ret)
 		goto error;
diff --git a/drivers/media/dvb-frontends/dib0090.c b/drivers/media/dvb-frontends/dib0090.c
index d9fe60b4be4..3ee22ff7631 100644
--- a/drivers/media/dvb-frontends/dib0090.c
+++ b/drivers/media/dvb-frontends/dib0090.c
@@ -528,20 +528,19 @@ static void dib0090_reset_digital(struct dvb_frontend *fe, const struct dib0090_
 	u16 PllCfg, i, v;
 
 	HARD_RESET(state);
-
 	dib0090_write_reg(state, 0x24, EN_PLL | EN_CRYSTAL);
-	dib0090_write_reg(state, 0x1b, EN_DIGCLK | EN_PLL | EN_CRYSTAL);	/* PLL, DIG_CLK and CRYSTAL remain */
+	if (cfg->in_soc)
+		return;
 
-	if (!cfg->in_soc) {
-		/* adcClkOutRatio=8->7, release reset */
-		dib0090_write_reg(state, 0x20, ((cfg->io.adc_clock_ratio - 1) << 11) | (0 << 10) | (1 << 9) | (1 << 8) | (0 << 4) | 0);
-		if (cfg->clkoutdrive != 0)
-			dib0090_write_reg(state, 0x23, (0 << 15) | ((!cfg->analog_output) << 14) | (2 << 10) | (1 << 9) | (0 << 8)
-					  | (cfg->clkoutdrive << 5) | (cfg->clkouttobamse << 4) | (0 << 2) | (0));
-		else
-			dib0090_write_reg(state, 0x23, (0 << 15) | ((!cfg->analog_output) << 14) | (2 << 10) | (1 << 9) | (0 << 8)
-					  | (7 << 5) | (cfg->clkouttobamse << 4) | (0 << 2) | (0));
-	}
+	dib0090_write_reg(state, 0x1b, EN_DIGCLK | EN_PLL | EN_CRYSTAL);	/* PLL, DIG_CLK and CRYSTAL remain */
+	/* adcClkOutRatio=8->7, release reset */
+	dib0090_write_reg(state, 0x20, ((cfg->io.adc_clock_ratio - 1) << 11) | (0 << 10) | (1 << 9) | (1 << 8) | (0 << 4) | 0);
+	if (cfg->clkoutdrive != 0)
+		dib0090_write_reg(state, 0x23, (0 << 15) | ((!cfg->analog_output) << 14) | (2 << 10) | (1 << 9) | (0 << 8)
+				| (cfg->clkoutdrive << 5) | (cfg->clkouttobamse << 4) | (0 << 2) | (0));
+	else
+		dib0090_write_reg(state, 0x23, (0 << 15) | ((!cfg->analog_output) << 14) | (2 << 10) | (1 << 9) | (0 << 8)
+				| (7 << 5) | (cfg->clkouttobamse << 4) | (0 << 2) | (0));
 
 	/* Read Pll current config * */
 	PllCfg = dib0090_read_reg(state, 0x21);
@@ -694,192 +693,174 @@ void dib0090_dcc_freq(struct dvb_frontend *fe, u8 fast)
 EXPORT_SYMBOL(dib0090_dcc_freq);
 
 static const u16 bb_ramp_pwm_normal_socs[] = {
-	550,			/* max BB gain in 10th of dB */
-	(1 << 9) | 8,		/* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> BB_RAMP2 */
+	550, /* max BB gain in 10th of dB */
+	(1<<9) | 8, /* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> BB_RAMP2 */
 	440,
-	(4 << 9) | 0,		/* BB_RAMP3 = 26dB */
-	(0 << 9) | 208,		/* BB_RAMP4 */
-	(4 << 9) | 208,		/* BB_RAMP5 = 29dB */
-	(0 << 9) | 440,		/* BB_RAMP6 */
+	(4  << 9) | 0, /* BB_RAMP3 = 26dB */
+	(0  << 9) | 208, /* BB_RAMP4 */
+	(4  << 9) | 208, /* BB_RAMP5 = 29dB */
+	(0  << 9) | 440, /* BB_RAMP6 */
 };
 
-static const u16 rf_ramp_pwm_cband_7090[] = {
-	280,			/* max RF gain in 10th of dB */
-	18,			/* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
-	504,			/* ramp_max = maximum X used on the ramp */
-	(29 << 10) | 364,	/* RF_RAMP5, LNA 1 = 8dB */
-	(0 << 10) | 504,	/* RF_RAMP6, LNA 1 */
-	(60 << 10) | 228,	/* RF_RAMP7, LNA 2 = 7.7dB */
-	(0 << 10) | 364,	/* RF_RAMP8, LNA 2 */
-	(34 << 10) | 109,	/* GAIN_4_1, LNA 3 = 6.8dB */
-	(0 << 10) | 228,	/* GAIN_4_2, LNA 3 */
-	(37 << 10) | 0,		/* RF_RAMP3, LNA 4 = 6.2dB */
-	(0 << 10) | 109,	/* RF_RAMP4, LNA 4 */
+static const u16 rf_ramp_pwm_cband_7090p[] = {
+	280, /* max RF gain in 10th of dB */
+	18, /* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
+	504, /* ramp_max = maximum X used on the ramp */
+	(29 << 10) | 364, /* RF_RAMP5, LNA 1 = 8dB */
+	(0  << 10) | 504, /* RF_RAMP6, LNA 1 */
+	(60 << 10) | 228, /* RF_RAMP7, LNA 2 = 7.7dB */
+	(0  << 10) | 364, /* RF_RAMP8, LNA 2 */
+	(34 << 10) | 109, /* GAIN_4_1, LNA 3 = 6.8dB */
+	(0  << 10) | 228, /* GAIN_4_2, LNA 3 */
+	(37 << 10) | 0, /* RF_RAMP3, LNA 4 = 6.2dB */
+	(0  << 10) | 109, /* RF_RAMP4, LNA 4 */
 };
 
-static const uint16_t rf_ramp_pwm_cband_7090e_sensitivity[] = {
-	186,
-	40,
-	746,
-	(10 << 10) | 345,
-	(0  << 10) | 746,
-	(0 << 10) | 0,
-	(0  << 10) | 0,
-	(28 << 10) | 200,
-	(0  << 10) | 345,
-	(20 << 10) | 0,
-	(0  << 10) | 200,
+static const u16 rf_ramp_pwm_cband_7090e_sensitivity[] = {
+	186, /* max RF gain in 10th of dB */
+	40, /* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
+	746, /* ramp_max = maximum X used on the ramp */
+	(10 << 10) | 345, /* RF_RAMP5, LNA 1 = 10dB */
+	(0  << 10) | 746, /* RF_RAMP6, LNA 1 */
+	(0 << 10) | 0, /* RF_RAMP7, LNA 2 = 0 dB */
+	(0  << 10) | 0, /* RF_RAMP8, LNA 2 */
+	(28 << 10) | 200, /* GAIN_4_1, LNA 3 = 6.8dB */ /* 3.61 dB */
+	(0  << 10) | 345, /* GAIN_4_2, LNA 3 */
+	(20 << 10) | 0, /* RF_RAMP3, LNA 4 = 6.2dB */ /* 4.96 dB */
+	(0  << 10) | 200, /* RF_RAMP4, LNA 4 */
 };
 
-static const uint16_t rf_ramp_pwm_cband_7090e_aci[] = {
-	86,
-	40,
-	345,
-	(0 << 10) | 0,
-	(0 << 10) | 0,
-	(0 << 10) | 0,
-	(0 << 10) | 0,
-	(28 << 10) | 200,
-	(0  << 10) | 345,
-	(20 << 10) | 0,
-	(0  << 10) | 200,
+static const u16 rf_ramp_pwm_cband_7090e_aci[] = {
+	86, /* max RF gain in 10th of dB */
+	40, /* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
+	345, /* ramp_max = maximum X used on the ramp */
+	(0 << 10) | 0, /* RF_RAMP5, LNA 1 = 8dB */ /* 7.47 dB */
+	(0 << 10) | 0, /* RF_RAMP6, LNA 1 */
+	(0 << 10) | 0, /* RF_RAMP7, LNA 2 = 0 dB */
+	(0 << 10) | 0, /* RF_RAMP8, LNA 2 */
+	(28 << 10) | 200, /* GAIN_4_1, LNA 3 = 6.8dB */ /* 3.61 dB */
+	(0  << 10) | 345, /* GAIN_4_2, LNA 3 */
+	(20 << 10) | 0, /* RF_RAMP3, LNA 4 = 6.2dB */ /* 4.96 dB */
+	(0  << 10) | 200, /* RF_RAMP4, LNA 4 */
 };
 
 static const u16 rf_ramp_pwm_cband_8090[] = {
-	345,			/* max RF gain in 10th of dB */
-	29,			/* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
-	1000,			/* ramp_max = maximum X used on the ramp */
-	(35 << 10) | 772,	/* RF_RAMP3, LNA 1 = 8dB */
-	(0 << 10) | 1000,	/* RF_RAMP4, LNA 1 */
-	(58 << 10) | 496,	/* RF_RAMP5, LNA 2 = 9.5dB */
-	(0 << 10) | 772,	/* RF_RAMP6, LNA 2 */
-	(27 << 10) | 200,	/* RF_RAMP7, LNA 3 = 10.5dB */
-	(0 << 10) | 496,	/* RF_RAMP8, LNA 3 */
-	(40 << 10) | 0,		/* GAIN_4_1, LNA 4 = 7dB */
-	(0 << 10) | 200,	/* GAIN_4_2, LNA 4 */
+	345, /* max RF gain in 10th of dB */
+	29, /* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
+	1000, /* ramp_max = maximum X used on the ramp */
+	(35 << 10) | 772, /* RF_RAMP3, LNA 1 = 8dB */
+	(0  << 10) | 1000, /* RF_RAMP4, LNA 1 */
+	(58 << 10) | 496, /* RF_RAMP5, LNA 2 = 9.5dB */
+	(0  << 10) | 772, /* RF_RAMP6, LNA 2 */
+	(27 << 10) | 200, /* RF_RAMP7, LNA 3 = 10.5dB */
+	(0  << 10) | 496, /* RF_RAMP8, LNA 3 */
+	(40 << 10) | 0, /* GAIN_4_1, LNA 4 = 7dB */
+	(0  << 10) | 200, /* GAIN_4_2, LNA 4 */
 };
 
 static const u16 rf_ramp_pwm_uhf_7090[] = {
-	407,			/* max RF gain in 10th of dB */
-	13,			/* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
-	529,			/* ramp_max = maximum X used on the ramp */
-	(23 << 10) | 0,		/* RF_RAMP3, LNA 1 = 14.7dB */
-	(0 << 10) | 176,	/* RF_RAMP4, LNA 1 */
-	(63 << 10) | 400,	/* RF_RAMP5, LNA 2 = 8dB */
-	(0 << 10) | 529,	/* RF_RAMP6, LNA 2 */
-	(48 << 10) | 316,	/* RF_RAMP7, LNA 3 = 6.8dB */
-	(0 << 10) | 400,	/* RF_RAMP8, LNA 3 */
-	(29 << 10) | 176,	/* GAIN_4_1, LNA 4 = 11.5dB */
-	(0 << 10) | 316,	/* GAIN_4_2, LNA 4 */
+	407, /* max RF gain in 10th of dB */
+	13, /* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
+	529, /* ramp_max = maximum X used on the ramp */
+	(23 << 10) | 0, /* RF_RAMP3, LNA 1 = 14.7dB */
+	(0  << 10) | 176, /* RF_RAMP4, LNA 1 */
+	(63 << 10) | 400, /* RF_RAMP5, LNA 2 = 8dB */
+	(0  << 10) | 529, /* RF_RAMP6, LNA 2 */
+	(48 << 10) | 316, /* RF_RAMP7, LNA 3 = 6.8dB */
+	(0  << 10) | 400, /* RF_RAMP8, LNA 3 */
+	(29 << 10) | 176, /* GAIN_4_1, LNA 4 = 11.5dB */
+	(0  << 10) | 316, /* GAIN_4_2, LNA 4 */
 };
 
 static const u16 rf_ramp_pwm_uhf_8090[] = {
-	388,			/* max RF gain in 10th of dB */
-	26,			/* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
-	1008,			/* ramp_max = maximum X used on the ramp */
-	(11 << 10) | 0,		/* RF_RAMP3, LNA 1 = 14.7dB */
-	(0 << 10) | 369,	/* RF_RAMP4, LNA 1 */
-	(41 << 10) | 809,	/* RF_RAMP5, LNA 2 = 8dB */
-	(0 << 10) | 1008,	/* RF_RAMP6, LNA 2 */
-	(27 << 10) | 659,	/* RF_RAMP7, LNA 3 = 6dB */
-	(0 << 10) | 809,	/* RF_RAMP8, LNA 3 */
-	(14 << 10) | 369,	/* GAIN_4_1, LNA 4 = 11.5dB */
-	(0 << 10) | 659,	/* GAIN_4_2, LNA 4 */
+	388, /* max RF gain in 10th of dB */
+	26, /* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
+	1008, /* ramp_max = maximum X used on the ramp */
+	(11 << 10) | 0, /* RF_RAMP3, LNA 1 = 14.7dB */
+	(0  << 10) | 369, /* RF_RAMP4, LNA 1 */
+	(41 << 10) | 809, /* RF_RAMP5, LNA 2 = 8dB */
+	(0  << 10) | 1008, /* RF_RAMP6, LNA 2 */
+	(27 << 10) | 659, /* RF_RAMP7, LNA 3 = 6dB */
+	(0  << 10) | 809, /* RF_RAMP8, LNA 3 */
+	(14 << 10) | 369, /* GAIN_4_1, LNA 4 = 11.5dB */
+	(0  << 10) | 659, /* GAIN_4_2, LNA 4 */
 };
 
-static const u16 rf_ramp_pwm_cband[] = {
-	0,			/* max RF gain in 10th of dB */
-	0,			/* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> 0x2b */
-	0,			/* ramp_max = maximum X used on the ramp */
-	(0 << 10) | 0,		/* 0x2c, LNA 1 = 0dB */
-	(0 << 10) | 0,		/* 0x2d, LNA 1 */
-	(0 << 10) | 0,		/* 0x2e, LNA 2 = 0dB */
-	(0 << 10) | 0,		/* 0x2f, LNA 2 */
-	(0 << 10) | 0,		/* 0x30, LNA 3 = 0dB */
-	(0 << 10) | 0,		/* 0x31, LNA 3 */
-	(0 << 10) | 0,		/* GAIN_4_1, LNA 4 = 0dB */
-	(0 << 10) | 0,		/* GAIN_4_2, LNA 4 */
-};
-
-static const u16 rf_ramp_vhf[] = {
-	412,			/* max RF gain in 10th of dB */
-	132, 307, 127,		/* LNA1,  13.2dB */
-	105, 412, 255,		/* LNA2,  10.5dB */
-	50, 50, 127,		/* LNA3,  5dB */
-	125, 175, 127,		/* LNA4,  12.5dB */
-	0, 0, 127,		/* CBAND, 0dB */
-};
-
-static const u16 rf_ramp_uhf[] = {
-	412,			/* max RF gain in 10th of dB */
-	132, 307, 127,		/* LNA1  : total gain = 13.2dB, point on the ramp where this amp is full gain, value to write to get full gain */
-	105, 412, 255,		/* LNA2  : 10.5 dB */
-	50, 50, 127,		/* LNA3  :  5.0 dB */
-	125, 175, 127,		/* LNA4  : 12.5 dB */
-	0, 0, 127,		/* CBAND :  0.0 dB */
+/* GENERAL PWM ramp definition for all other Krosus */
+static const u16 bb_ramp_pwm_normal[] = {
+	500, /* max BB gain in 10th of dB */
+	8, /* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> BB_RAMP2 */
+	400,
+	(2  << 9) | 0, /* BB_RAMP3 = 21dB */
+	(0  << 9) | 168, /* BB_RAMP4 */
+	(2  << 9) | 168, /* BB_RAMP5 = 29dB */
+	(0  << 9) | 400, /* BB_RAMP6 */
 };
 
-static const u16 rf_ramp_cband_broadmatching[] =	/* for p1G only */
-{
-	314,			/* Calibrated at 200MHz order has been changed g4-g3-g2-g1 */
-	84, 314, 127,		/* LNA1 */
-	80, 230, 255,		/* LNA2 */
-	80, 150, 127,		/* LNA3  It was measured 12dB, do not lock if 120 */
-	70, 70, 127,		/* LNA4 */
-	0, 0, 127,		/* CBAND */
+static const u16 bb_ramp_pwm_boost[] = {
+	550, /* max BB gain in 10th of dB */
+	8, /* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> BB_RAMP2 */
+	440,
+	(2  << 9) | 0, /* BB_RAMP3 = 26dB */
+	(0  << 9) | 208, /* BB_RAMP4 */
+	(2  << 9) | 208, /* BB_RAMP5 = 29dB */
+	(0  << 9) | 440, /* BB_RAMP6 */
 };
 
-static const u16 rf_ramp_cband[] = {
-	332,			/* max RF gain in 10th of dB */
-	132, 252, 127,		/* LNA1,  dB */
-	80, 332, 255,		/* LNA2,  dB */
-	0, 0, 127,		/* LNA3,  dB */
-	0, 0, 127,		/* LNA4,  dB */
-	120, 120, 127,		/* LT1 CBAND */
+static const u16 rf_ramp_pwm_cband[] = {
+	314, /* max RF gain in 10th of dB */
+	33, /* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
+	1023, /* ramp_max = maximum X used on the ramp */
+	(8  << 10) | 743, /* RF_RAMP3, LNA 1 = 0dB */
+	(0  << 10) | 1023, /* RF_RAMP4, LNA 1 */
+	(15 << 10) | 469, /* RF_RAMP5, LNA 2 = 0dB */
+	(0  << 10) | 742, /* RF_RAMP6, LNA 2 */
+	(9  << 10) | 234, /* RF_RAMP7, LNA 3 = 0dB */
+	(0  << 10) | 468, /* RF_RAMP8, LNA 3 */
+	(9  << 10) | 0, /* GAIN_4_1, LNA 4 = 0dB */
+	(0  << 10) | 233, /* GAIN_4_2, LNA 4 */
 };
 
 static const u16 rf_ramp_pwm_vhf[] = {
-	404,			/* max RF gain in 10th of dB */
-	25,			/* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> 0x2b */
-	1011,			/* ramp_max = maximum X used on the ramp */
-	(6 << 10) | 417,	/* 0x2c, LNA 1 = 13.2dB */
-	(0 << 10) | 756,	/* 0x2d, LNA 1 */
-	(16 << 10) | 756,	/* 0x2e, LNA 2 = 10.5dB */
-	(0 << 10) | 1011,	/* 0x2f, LNA 2 */
-	(16 << 10) | 290,	/* 0x30, LNA 3 = 5dB */
-	(0 << 10) | 417,	/* 0x31, LNA 3 */
-	(7 << 10) | 0,		/* GAIN_4_1, LNA 4 = 12.5dB */
-	(0 << 10) | 290,	/* GAIN_4_2, LNA 4 */
+	398, /* max RF gain in 10th of dB */
+	24, /* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
+	954, /* ramp_max = maximum X used on the ramp */
+	(7  << 10) | 0, /* RF_RAMP3, LNA 1 = 13.2dB */
+	(0  << 10) | 290, /* RF_RAMP4, LNA 1 */
+	(16 << 10) | 699, /* RF_RAMP5, LNA 2 = 10.5dB */
+	(0  << 10) | 954, /* RF_RAMP6, LNA 2 */
+	(17 << 10) | 580, /* RF_RAMP7, LNA 3 = 5dB */
+	(0  << 10) | 699, /* RF_RAMP8, LNA 3 */
+	(7  << 10) | 290, /* GAIN_4_1, LNA 4 = 12.5dB */
+	(0  << 10) | 580, /* GAIN_4_2, LNA 4 */
 };
 
 static const u16 rf_ramp_pwm_uhf[] = {
-	404,			/* max RF gain in 10th of dB */
-	25,			/* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> 0x2b */
-	1011,			/* ramp_max = maximum X used on the ramp */
-	(6 << 10) | 417,	/* 0x2c, LNA 1 = 13.2dB */
-	(0 << 10) | 756,	/* 0x2d, LNA 1 */
-	(16 << 10) | 756,	/* 0x2e, LNA 2 = 10.5dB */
-	(0 << 10) | 1011,	/* 0x2f, LNA 2 */
-	(16 << 10) | 0,		/* 0x30, LNA 3 = 5dB */
-	(0 << 10) | 127,	/* 0x31, LNA 3 */
-	(7 << 10) | 127,	/* GAIN_4_1, LNA 4 = 12.5dB */
-	(0 << 10) | 417,	/* GAIN_4_2, LNA 4 */
+	398, /* max RF gain in 10th of dB */
+	24, /* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
+	954, /* ramp_max = maximum X used on the ramp */
+	(7  << 10) | 0, /* RF_RAMP3, LNA 1 = 13.2dB */
+	(0  << 10) | 290, /* RF_RAMP4, LNA 1 */
+	(16 << 10) | 699, /* RF_RAMP5, LNA 2 = 10.5dB */
+	(0  << 10) | 954, /* RF_RAMP6, LNA 2 */
+	(17 << 10) | 580, /* RF_RAMP7, LNA 3 = 5dB */
+	(0  << 10) | 699, /* RF_RAMP8, LNA 3 */
+	(7  << 10) | 290, /* GAIN_4_1, LNA 4 = 12.5dB */
+	(0  << 10) | 580, /* GAIN_4_2, LNA 4 */
 };
 
-static const u16 bb_ramp_boost[] = {
-	550,			/* max BB gain in 10th of dB */
-	260, 260, 26,		/* BB1, 26dB */
-	290, 550, 29,		/* BB2, 29dB */
-};
-
-static const u16 bb_ramp_pwm_normal[] = {
-	500,			/* max RF gain in 10th of dB */
-	8,			/* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> 0x34 */
-	400,
-	(2 << 9) | 0,		/* 0x35 = 21dB */
-	(0 << 9) | 168,		/* 0x36 */
-	(2 << 9) | 168,		/* 0x37 = 29dB */
-	(0 << 9) | 400,		/* 0x38 */
+static const u16 rf_ramp_pwm_sband[] = {
+	253, /* max RF gain in 10th of dB */
+	38, /* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
+	961,
+	(4  << 10) | 0, /* RF_RAMP3, LNA 1 = 14.1dB */
+	(0  << 10) | 508, /* RF_RAMP4, LNA 1 */
+	(9  << 10) | 508, /* RF_RAMP5, LNA 2 = 11.2dB */
+	(0  << 10) | 961, /* RF_RAMP6, LNA 2 */
+	(0  << 10) | 0, /* RF_RAMP7, LNA 3 = 0dB */
+	(0  << 10) | 0, /* RF_RAMP8, LNA 3 */
+	(0  << 10) | 0, /* GAIN_4_1, LNA 4 = 0dB */
+	(0  << 10) | 0, /* GAIN_4_2, LNA 4 */
 };
 
 struct slope {
@@ -1089,70 +1070,69 @@ static void dib0090_set_bbramp_pwm(struct dib0090_state *state, const u16 * cfg)
 void dib0090_pwm_gain_reset(struct dvb_frontend *fe)
 {
 	struct dib0090_state *state = fe->tuner_priv;
-	/* reset the AGC */
+	u16 *bb_ramp = (u16 *)&bb_ramp_pwm_normal; /* default baseband config */
+	u16 *rf_ramp = NULL;
+	u8 en_pwm_rf_mux = 1;
 
+	/* reset the AGC */
 	if (state->config->use_pwm_agc) {
-#ifdef CONFIG_BAND_SBAND
-		if (state->current_band == BAND_SBAND) {
-			dib0090_set_rframp_pwm(state, rf_ramp_pwm_sband);
-			dib0090_set_bbramp_pwm(state, bb_ramp_pwm_boost);
-		} else
-#endif
-#ifdef CONFIG_BAND_CBAND
 		if (state->current_band == BAND_CBAND) {
 			if (state->identity.in_soc) {
-				dib0090_set_bbramp_pwm(state, bb_ramp_pwm_normal_socs);
+				bb_ramp = (u16 *)&bb_ramp_pwm_normal_socs;
 				if (state->identity.version == SOC_8090_P1G_11R1 || state->identity.version == SOC_8090_P1G_21R1)
-					dib0090_set_rframp_pwm(state, rf_ramp_pwm_cband_8090);
-				else if (state->identity.version == SOC_7090_P1G_11R1
-						|| state->identity.version == SOC_7090_P1G_21R1) {
+					rf_ramp = (u16 *)&rf_ramp_pwm_cband_8090;
+				else if (state->identity.version == SOC_7090_P1G_11R1 || state->identity.version == SOC_7090_P1G_21R1) {
 					if (state->config->is_dib7090e) {
 						if (state->rf_ramp == NULL)
-							dib0090_set_rframp_pwm(state, rf_ramp_pwm_cband_7090e_sensitivity);
+							rf_ramp = (u16 *)&rf_ramp_pwm_cband_7090e_sensitivity;
 						else
-							dib0090_set_rframp_pwm(state, state->rf_ramp);
+							rf_ramp = (u16 *)state->rf_ramp;
 					} else
-						dib0090_set_rframp_pwm(state, rf_ramp_pwm_cband_7090);
+						rf_ramp = (u16 *)&rf_ramp_pwm_cband_7090p;
 				}
-			} else {
-				dib0090_set_rframp_pwm(state, rf_ramp_pwm_cband);
-				dib0090_set_bbramp_pwm(state, bb_ramp_pwm_normal);
-			}
+			} else
+				rf_ramp = (u16 *)&rf_ramp_pwm_cband;
 		} else
-#endif
-#ifdef CONFIG_BAND_VHF
-		if (state->current_band == BAND_VHF) {
-			if (state->identity.in_soc) {
-				dib0090_set_bbramp_pwm(state, bb_ramp_pwm_normal_socs);
-			} else {
-				dib0090_set_rframp_pwm(state, rf_ramp_pwm_vhf);
-				dib0090_set_bbramp_pwm(state, bb_ramp_pwm_normal);
+
+			if (state->current_band == BAND_VHF) {
+				if (state->identity.in_soc) {
+					bb_ramp = (u16 *)&bb_ramp_pwm_normal_socs;
+					/* rf_ramp = &rf_ramp_pwm_vhf_socs; */ /* TODO */
+				} else
+					rf_ramp = (u16 *)&rf_ramp_pwm_vhf;
+			} else if (state->current_band == BAND_UHF) {
+				if (state->identity.in_soc) {
+					bb_ramp = (u16 *)&bb_ramp_pwm_normal_socs;
+					if (state->identity.version == SOC_8090_P1G_11R1 || state->identity.version == SOC_8090_P1G_21R1)
+						rf_ramp = (u16 *)&rf_ramp_pwm_uhf_8090;
+					else if (state->identity.version == SOC_7090_P1G_11R1 || state->identity.version == SOC_7090_P1G_21R1)
+						rf_ramp = (u16 *)&rf_ramp_pwm_uhf_7090;
+				} else
+					rf_ramp = (u16 *)&rf_ramp_pwm_uhf;
 			}
+		if (rf_ramp)
+			dib0090_set_rframp_pwm(state, rf_ramp);
+		dib0090_set_bbramp_pwm(state, bb_ramp);
+
+		/* activate the ramp generator using PWM control */
+		dprintk("ramp RF gain = %d BAND = %s version = %d", state->rf_ramp[0], (state->current_band == BAND_CBAND) ? "CBAND" : "NOT CBAND", state->identity.version & 0x1f);
+
+		if ((state->rf_ramp[0] == 0) || (state->current_band == BAND_CBAND && (state->identity.version & 0x1f) <= P1D_E_F)) {
+			dprintk("DE-Engage mux for direct gain reg control");
+			en_pwm_rf_mux = 0;
 		} else
-#endif
-		{
-			if (state->identity.in_soc) {
-				if (state->identity.version == SOC_8090_P1G_11R1 || state->identity.version == SOC_8090_P1G_21R1)
-					dib0090_set_rframp_pwm(state, rf_ramp_pwm_uhf_8090);
-				else if (state->identity.version == SOC_7090_P1G_11R1 || state->identity.version == SOC_7090_P1G_21R1)
-					dib0090_set_rframp_pwm(state, rf_ramp_pwm_uhf_7090);
-				dib0090_set_bbramp_pwm(state, bb_ramp_pwm_normal_socs);
-			} else {
-				dib0090_set_rframp_pwm(state, rf_ramp_pwm_uhf);
-				dib0090_set_bbramp_pwm(state, bb_ramp_pwm_normal);
-			}
-		}
+			dprintk("Engage mux for PWM control");
 
-		if (state->rf_ramp[0] != 0)
-			dib0090_write_reg(state, 0x32, (3 << 11));
-		else
-			dib0090_write_reg(state, 0x32, (0 << 11));
+		dib0090_write_reg(state, 0x32, (en_pwm_rf_mux << 12) | (en_pwm_rf_mux << 11));
 
-		dib0090_write_reg(state, 0x04, 0x03);
-		dib0090_write_reg(state, 0x39, (1 << 10));
+		/* Set fast servo cutoff to start AGC; 0 = 1KHz ; 1 = 50Hz ; 2 = 150Hz ; 3 = 50KHz ; 4 = servo fast*/
+		if (state->identity.version == SOC_7090_P1G_11R1 || state->identity.version == SOC_7090_P1G_21R1)
+			dib0090_write_reg(state, 0x04, 3);
+		else
+			dib0090_write_reg(state, 0x04, 1);
+		dib0090_write_reg(state, 0x39, (1 << 10)); /* 0 gain by default */
 	}
 }
-
 EXPORT_SYMBOL(dib0090_pwm_gain_reset);
 
 void dib0090_set_dc_servo(struct dvb_frontend *fe, u8 DC_servo_cutoff)
@@ -1193,22 +1173,22 @@ int dib0090_gain_control(struct dvb_frontend *fe)
 #endif
 #ifdef CONFIG_BAND_VHF
 		if (state->current_band == BAND_VHF && !state->identity.p1g) {
-			dib0090_set_rframp(state, rf_ramp_vhf);
-			dib0090_set_bbramp(state, bb_ramp_boost);
+			dib0090_set_rframp(state, rf_ramp_pwm_vhf);
+			dib0090_set_bbramp(state, bb_ramp_pwm_normal);
 		} else
 #endif
 #ifdef CONFIG_BAND_CBAND
 		if (state->current_band == BAND_CBAND && !state->identity.p1g) {
-			dib0090_set_rframp(state, rf_ramp_cband);
-			dib0090_set_bbramp(state, bb_ramp_boost);
+			dib0090_set_rframp(state, rf_ramp_pwm_cband);
+			dib0090_set_bbramp(state, bb_ramp_pwm_normal);
 		} else
 #endif
 		if ((state->current_band == BAND_CBAND || state->current_band == BAND_VHF) && state->identity.p1g) {
-			dib0090_set_rframp(state, rf_ramp_cband_broadmatching);
-			dib0090_set_bbramp(state, bb_ramp_boost);
+			dib0090_set_rframp(state, rf_ramp_pwm_cband_7090p);
+			dib0090_set_bbramp(state, bb_ramp_pwm_normal_socs);
 		} else {
-			dib0090_set_rframp(state, rf_ramp_uhf);
-			dib0090_set_bbramp(state, bb_ramp_boost);
+			dib0090_set_rframp(state, rf_ramp_pwm_uhf);
+			dib0090_set_bbramp(state, bb_ramp_pwm_normal);
 		}
 
 		dib0090_write_reg(state, 0x32, 0);
@@ -1553,14 +1533,16 @@ static void dib0090_set_EFUSE(struct dib0090_state *state)
 
 		if ((c >= CAP_VALUE_MAX) || (c <= CAP_VALUE_MIN))
 			c = 32;
+		else
+			c += 14;
 		if ((h >= HR_MAX) || (h <= HR_MIN))
 			h = 34;
 		if ((n >= POLY_MAX) || (n <= POLY_MIN))
 			n = 3;
 
-		dib0090_write_reg(state, 0x13, (h << 10)) ;
-		e2 = (n<<11) | ((h>>2)<<6) | (c);
-		dib0090_write_reg(state, 0x2, e2) ; /* Load the BB_2 */
+		dib0090_write_reg(state, 0x13, (h << 10));
+		e2 = (n << 11) | ((h >> 2)<<6) | c;
+		dib0090_write_reg(state, 0x2, e2); /* Load the BB_2 */
 	}
 }
 
diff --git a/drivers/media/dvb-frontends/dib3000mc.h b/drivers/media/dvb-frontends/dib3000mc.h
index d75ffad2d75..129d1425516 100644
--- a/drivers/media/dvb-frontends/dib3000mc.h
+++ b/drivers/media/dvb-frontends/dib3000mc.h
@@ -13,6 +13,8 @@
 #ifndef DIB3000MC_H
 #define DIB3000MC_H
 
+#include <linux/kconfig.h>
+
 #include "dibx000_common.h"
 
 struct dib3000mc_config {
@@ -39,8 +41,7 @@ struct dib3000mc_config {
 #define DEFAULT_DIB3000MC_I2C_ADDRESS 16
 #define DEFAULT_DIB3000P_I2C_ADDRESS  24
 
-#if defined(CONFIG_DVB_DIB3000MC) || (defined(CONFIG_DVB_DIB3000MC_MODULE) && \
-				      defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_DIB3000MC)
 extern struct dvb_frontend *dib3000mc_attach(struct i2c_adapter *i2c_adap,
 					     u8 i2c_addr,
 					     struct dib3000mc_config *cfg);
diff --git a/drivers/media/dvb-frontends/dib7000m.h b/drivers/media/dvb-frontends/dib7000m.h
index 81fcf2241c6..b585413f9a2 100644
--- a/drivers/media/dvb-frontends/dib7000m.h
+++ b/drivers/media/dvb-frontends/dib7000m.h
@@ -1,6 +1,8 @@
 #ifndef DIB7000M_H
 #define DIB7000M_H
 
+#include <linux/kconfig.h>
+
 #include "dibx000_common.h"
 
 struct dib7000m_config {
@@ -38,8 +40,7 @@ struct dib7000m_config {
 
 #define DEFAULT_DIB7000M_I2C_ADDRESS 18
 
-#if defined(CONFIG_DVB_DIB7000M) || (defined(CONFIG_DVB_DIB7000M_MODULE) && \
-				     defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_DIB7000M)
 extern struct dvb_frontend *dib7000m_attach(struct i2c_adapter *i2c_adap,
 					    u8 i2c_addr,
 					    struct dib7000m_config *cfg);
diff --git a/drivers/media/dvb-frontends/dib7000p.c b/drivers/media/dvb-frontends/dib7000p.c
index 3e1eefada0e..effb87f773b 100644
--- a/drivers/media/dvb-frontends/dib7000p.c
+++ b/drivers/media/dvb-frontends/dib7000p.c
@@ -429,6 +429,13 @@ int dib7000p_get_agc_values(struct dvb_frontend *fe,
 }
 EXPORT_SYMBOL(dib7000p_get_agc_values);
 
+int dib7000p_set_agc1_min(struct dvb_frontend *fe, u16 v)
+{
+	struct dib7000p_state *state = fe->demodulator_priv;
+	return dib7000p_write_word(state, 108,  v);
+}
+EXPORT_SYMBOL(dib7000p_set_agc1_min);
+
 static void dib7000p_reset_pll(struct dib7000p_state *state)
 {
 	struct dibx000_bandwidth_config *bw = &state->cfg.bw[0];
@@ -821,6 +828,7 @@ static int dib7000p_agc_startup(struct dvb_frontend *demod)
 	u8 agc_split;
 	u16 reg;
 	u32 upd_demod_gain_period = 0x1000;
+	s32 frequency_offset = 0;
 
 	switch (state->agc_state) {
 	case 0:
@@ -841,7 +849,14 @@ static int dib7000p_agc_startup(struct dvb_frontend *demod)
 		if (dib7000p_set_agc_config(state, BAND_OF_FREQUENCY(ch->frequency / 1000)) != 0)
 			return -1;
 
-		dib7000p_set_dds(state, 0);
+		if (demod->ops.tuner_ops.get_frequency) {
+			u32 frequency_tuner;
+
+			demod->ops.tuner_ops.get_frequency(demod, &frequency_tuner);
+			frequency_offset = (s32)frequency_tuner / 1000 - ch->frequency / 1000;
+		}
+
+		dib7000p_set_dds(state, frequency_offset);
 		ret = 7;
 		(*agc_state)++;
 		break;
diff --git a/drivers/media/dvb-frontends/dib7000p.h b/drivers/media/dvb-frontends/dib7000p.h
index b61b03a6e1e..d08cdff59bd 100644
--- a/drivers/media/dvb-frontends/dib7000p.h
+++ b/drivers/media/dvb-frontends/dib7000p.h
@@ -1,6 +1,8 @@
 #ifndef DIB7000P_H
 #define DIB7000P_H
 
+#include <linux/kconfig.h>
+
 #include "dibx000_common.h"
 
 struct dib7000p_config {
@@ -44,8 +46,7 @@ struct dib7000p_config {
 
 #define DEFAULT_DIB7000P_I2C_ADDRESS 18
 
-#if defined(CONFIG_DVB_DIB7000P) || (defined(CONFIG_DVB_DIB7000P_MODULE) && \
-					defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_DIB7000P)
 extern struct dvb_frontend *dib7000p_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr, struct dib7000p_config *cfg);
 extern struct i2c_adapter *dib7000p_get_i2c_master(struct dvb_frontend *, enum dibx000_i2c_interface, int);
 extern int dib7000p_i2c_enumeration(struct i2c_adapter *i2c, int no_of_demods, u8 default_addr, struct dib7000p_config cfg[]);
@@ -62,6 +63,7 @@ extern struct i2c_adapter *dib7090_get_i2c_tuner(struct dvb_frontend *fe);
 extern int dib7090_slave_reset(struct dvb_frontend *fe);
 extern int dib7000p_get_agc_values(struct dvb_frontend *fe,
 		u16 *agc_global, u16 *agc1, u16 *agc2, u16 *wbd);
+extern int dib7000p_set_agc1_min(struct dvb_frontend *fe, u16 v);
 #else
 static inline struct dvb_frontend *dib7000p_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr, struct dib7000p_config *cfg)
 {
@@ -153,6 +155,12 @@ static inline int dib7000p_get_agc_values(struct dvb_frontend *fe,
 	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
 	return -ENODEV;
 }
+
+static inline int dib7000p_set_agc1_min(struct dvb_frontend *fe, u16 v)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return -ENODEV;
+}
 #endif
 
 #endif
diff --git a/drivers/media/dvb-frontends/dib8000.c b/drivers/media/dvb-frontends/dib8000.c
index 1f3bcb5a1de..a54182dd0e9 100644
--- a/drivers/media/dvb-frontends/dib8000.c
+++ b/drivers/media/dvb-frontends/dib8000.c
@@ -23,8 +23,8 @@
 #define LAYER_B   2
 #define LAYER_C   3
 
-#define FE_CALLBACK_TIME_NEVER 0xffffffff
 #define MAX_NUMBER_OF_FRONTENDS 6
+/* #define DIB8000_AGC_FREEZE */
 
 static int debug;
 module_param(debug, int, 0644);
@@ -32,8 +32,6 @@ MODULE_PARM_DESC(debug, "turn on debugging (default: 0)");
 
 #define dprintk(args...) do { if (debug) { printk(KERN_DEBUG "DiB8000: "); printk(args); printk("\n"); } } while (0)
 
-#define FE_STATUS_TUNE_FAILED 0
-
 struct i2c_device {
 	struct i2c_adapter *adap;
 	u8 addr;
@@ -42,6 +40,23 @@ struct i2c_device {
 	struct mutex *i2c_buffer_lock;
 };
 
+enum param_loop_step {
+	LOOP_TUNE_1,
+	LOOP_TUNE_2
+};
+
+enum dib8000_autosearch_step {
+	AS_START = 0,
+	AS_SEARCHING_FFT,
+	AS_SEARCHING_GUARD,
+	AS_DONE = 100,
+};
+
+enum timeout_mode {
+	SYMBOL_DEPENDENT_OFF = 0,
+	SYMBOL_DEPENDENT_ON,
+};
+
 struct dib8000_state {
 	struct dib8000_config cfg;
 
@@ -72,7 +87,7 @@ struct dib8000_state {
 	u16 revision;
 	u8 isdbt_cfg_loaded;
 	enum frontend_tune_state tune_state;
-	u32 status;
+	s32 status;
 
 	struct dvb_frontend *fe[MAX_NUMBER_OF_FRONTENDS];
 
@@ -85,6 +100,30 @@ struct dib8000_state {
 
 	u16 tuner_enable;
 	struct i2c_adapter dib8096p_tuner_adap;
+	u16 current_demod_bw;
+
+	u16 seg_mask;
+	u16 seg_diff_mask;
+	u16 mode;
+	u8 layer_b_nb_seg;
+	u8 layer_c_nb_seg;
+
+	u8 channel_parameters_set;
+	u16 autosearch_state;
+	u16 found_nfft;
+	u16 found_guard;
+	u8 subchannel;
+	u8 symbol_duration;
+	u32 timeout;
+	u8 longest_intlv_layer;
+	u16 output_mode;
+
+#ifdef DIB8000_AGC_FREEZE
+	u16 agc1_max;
+	u16 agc1_min;
+	u16 agc2_max;
+	u16 agc2_min;
+#endif
 };
 
 enum dib8000_power_mode {
@@ -338,9 +377,9 @@ static void dib8000_set_acquisition_mode(struct dib8000_state *state)
 static int dib8000_set_output_mode(struct dvb_frontend *fe, int mode)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
-
 	u16 outreg, fifo_threshold, smo_mode, sram = 0x0205;	/* by default SDRAM deintlv is enabled */
 
+	state->output_mode = mode;
 	outreg = 0;
 	fifo_threshold = 1792;
 	smo_mode = (dib8000_read_word(state, 299) & 0x0050) | (1 << 1);
@@ -399,8 +438,9 @@ static int dib8000_set_output_mode(struct dvb_frontend *fe, int mode)
 static int dib8000_set_diversity_in(struct dvb_frontend *fe, int onoff)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
-	u16 sync_wait = dib8000_read_word(state, 273) & 0xfff0;
+	u16 tmp, sync_wait = dib8000_read_word(state, 273) & 0xfff0;
 
+	dprintk("set diversity input to %i", onoff);
 	if (!state->differential_constellation) {
 		dib8000_write_word(state, 272, 1 << 9);	//dvsy_off_lmod4 = 1
 		dib8000_write_word(state, 273, sync_wait | (1 << 2) | 2);	// sync_enable = 1; comb_mode = 2
@@ -424,6 +464,13 @@ static int dib8000_set_diversity_in(struct dvb_frontend *fe, int onoff)
 		dib8000_write_word(state, 271, 1);
 		break;
 	}
+
+	if (state->revision == 0x8002) {
+		tmp = dib8000_read_word(state, 903);
+		dib8000_write_word(state, 903, tmp & ~(1 << 3));
+		msleep(30);
+		dib8000_write_word(state, 903, tmp | (1 << 3));
+	}
 	return 0;
 }
 
@@ -468,27 +515,6 @@ static void dib8000_set_power_mode(struct dib8000_state *state, enum dib8000_pow
 	dib8000_write_word(state, 1280, reg_1280);
 }
 
-static int dib8000_init_sdram(struct dib8000_state *state)
-{
-	u16 reg = 0;
-	dprintk("Init sdram");
-
-	reg = dib8000_read_word(state, 274)&0xfff0;
-	/* P_dintlv_delay_ram = 7 because of MobileSdram */
-	dib8000_write_word(state, 274, reg | 0x7);
-
-	dib8000_write_word(state, 1803, (7<<2));
-
-	reg = dib8000_read_word(state, 1280);
-	/* force restart P_restart_sdram */
-	dib8000_write_word(state, 1280,  reg | (1<<2));
-
-	/* release restart P_restart_sdram */
-	dib8000_write_word(state, 1280,  reg);
-
-	return 0;
-}
-
 static int dib8000_set_adc_state(struct dib8000_state *state, enum dibx000_adc_states no)
 {
 	int ret = 0;
@@ -584,18 +610,23 @@ static int dib8000_set_bandwidth(struct dvb_frontend *fe, u32 bw)
 
 static int dib8000_sad_calib(struct dib8000_state *state)
 {
+	u8 sad_sel = 3;
+
 	if (state->revision == 0x8090) {
-		dprintk("%s: the sad calibration is not needed for the dib8096P",
-				__func__);
-		return 0;
-	}
-	/* internal */
-	dib8000_write_word(state, 923, (0 << 1) | (0 << 0));
-	dib8000_write_word(state, 924, 776);	// 0.625*3.3 / 4096
+		dib8000_write_word(state, 922, (sad_sel << 2));
+		dib8000_write_word(state, 923, 2048);
+
+		dib8000_write_word(state, 922, (sad_sel << 2) | 0x1);
+		dib8000_write_word(state, 922, (sad_sel << 2));
+	} else {
+		/* internal */
+		dib8000_write_word(state, 923, (0 << 1) | (0 << 0));
+		dib8000_write_word(state, 924, 776);
 
-	/* do the calibration */
-	dib8000_write_word(state, 923, (1 << 0));
-	dib8000_write_word(state, 923, (0 << 0));
+		/* do the calibration */
+		dib8000_write_word(state, 923, (1 << 0));
+		dib8000_write_word(state, 923, (0 << 0));
+	}
 
 	msleep(1);
 	return 0;
@@ -609,8 +640,8 @@ int dib8000_set_wbd_ref(struct dvb_frontend *fe, u16 value)
 	state->wbd_ref = value;
 	return dib8000_write_word(state, 106, value);
 }
-
 EXPORT_SYMBOL(dib8000_set_wbd_ref);
+
 static void dib8000_reset_pll_common(struct dib8000_state *state, const struct dibx000_bandwidth_config *bw)
 {
 	dprintk("ifreq: %d %x, inversion: %d", bw->ifreq, bw->ifreq, bw->ifreq >> 25);
@@ -685,20 +716,23 @@ static void dib8000_reset_pll(struct dib8000_state *state)
 }
 
 int dib8000_update_pll(struct dvb_frontend *fe,
-		struct dibx000_bandwidth_config *pll)
+		struct dibx000_bandwidth_config *pll, u32 bw, u8 ratio)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
 	u16 reg_1857, reg_1856 = dib8000_read_word(state, 1856);
-	u8 loopdiv, prediv;
+	u8 loopdiv, prediv, oldprediv = state->cfg.pll->pll_prediv ;
 	u32 internal, xtal;
 
 	/* get back old values */
 	prediv = reg_1856 & 0x3f;
 	loopdiv = (reg_1856 >> 6) & 0x3f;
 
-	if ((pll != NULL) && (pll->pll_prediv != prediv ||
-				pll->pll_ratio != loopdiv)) {
-		dprintk("Updating pll (prediv: old =  %d new = %d ; loopdiv : old = %d new = %d)", prediv, pll->pll_prediv, loopdiv, pll->pll_ratio);
+	if ((pll == NULL) || (pll->pll_prediv == prediv &&
+				pll->pll_ratio == loopdiv))
+		return -EINVAL;
+
+	dprintk("Updating pll (prediv: old =  %d new = %d ; loopdiv : old = %d new = %d)", prediv, pll->pll_prediv, loopdiv, pll->pll_ratio);
+	if (state->revision == 0x8090) {
 		reg_1856 &= 0xf000;
 		reg_1857 = dib8000_read_word(state, 1857);
 		/* disable PLL */
@@ -729,10 +763,33 @@ int dib8000_update_pll(struct dvb_frontend *fe,
 		reg_1856 = dib8000_read_word(state, 1856);
 		dprintk("PLL Updated with prediv = %d and loopdiv = %d",
 				reg_1856&0x3f, (reg_1856>>6)&0x3f);
+	} else {
+		if (bw != state->current_demod_bw) {
+			/** Bandwidth change => force PLL update **/
+			dprintk("PLL: Bandwidth Change %d MHz -> %d MHz (prediv: %d->%d)", state->current_demod_bw / 1000, bw / 1000, oldprediv, state->cfg.pll->pll_prediv);
+
+			if (state->cfg.pll->pll_prediv != oldprediv) {
+				/** Full PLL change only if prediv is changed **/
+
+				/** full update => bypass and reconfigure **/
+				dprintk("PLL: New Setting for %d MHz Bandwidth (prediv: %d, ratio: %d)", bw/1000, state->cfg.pll->pll_prediv, state->cfg.pll->pll_ratio);
+				dib8000_write_word(state, 902, dib8000_read_word(state, 902) | (1<<3)); /* bypass PLL */
+				dib8000_reset_pll(state);
+				dib8000_write_word(state, 898, 0x0004); /* sad */
+			} else
+				ratio = state->cfg.pll->pll_ratio;
 
-		return 0;
-	}
-	return -EINVAL;
+			state->current_demod_bw = bw;
+		}
+
+		if (ratio != 0) {
+			/** ratio update => only change ratio **/
+			dprintk("PLL: Update ratio (prediv: %d, ratio: %d)", state->cfg.pll->pll_prediv, ratio);
+			dib8000_write_word(state, 901, (state->cfg.pll->pll_prediv << 8) | (ratio << 0)); /* only the PLL ratio is updated. */
+		}
+}
+
+	return 0;
 }
 EXPORT_SYMBOL(dib8000_update_pll);
 
@@ -928,7 +985,7 @@ static int dib8000_reset(struct dvb_frontend *fe)
 	dib8000_set_power_mode(state, DIB8000_POWER_ALL);
 
 	/* always leave the VBG voltage on - it consumes almost nothing but takes a long time to start */
-	dib8000_set_adc_state(state, DIBX000_VBG_ENABLE);
+	dib8000_set_adc_state(state, DIBX000_ADC_OFF);
 
 	/* restart all parts */
 	dib8000_write_word(state, 770, 0xffff);
@@ -992,12 +1049,11 @@ static int dib8000_reset(struct dvb_frontend *fe)
 			l = *n++;
 		}
 	}
-	if (state->revision != 0x8090)
-		dib8000_write_word(state, 903, (0 << 4) | 2);
+
 	state->isdbt_cfg_loaded = 0;
 
 	//div_cfg override for special configs
-	if (state->cfg.div_cfg != 0)
+	if ((state->revision != 8090) && (state->cfg.div_cfg != 0))
 		dib8000_write_word(state, 903, state->cfg.div_cfg);
 
 	/* unforce divstr regardless whether i2c enumeration was done or not */
@@ -1006,10 +1062,12 @@ static int dib8000_reset(struct dvb_frontend *fe)
 	dib8000_set_bandwidth(fe, 6000);
 
 	dib8000_set_adc_state(state, DIBX000_SLOW_ADC_ON);
-	if (state->revision != 0x8090) {
-		dib8000_sad_calib(state);
+	dib8000_sad_calib(state);
+	if (state->revision != 0x8090)
 		dib8000_set_adc_state(state, DIBX000_SLOW_ADC_OFF);
-	}
+
+	/* ber_rs_len = 3 */
+	dib8000_write_word(state, 285, (dib8000_read_word(state, 285) & ~0x60) | (3 << 5));
 
 	dib8000_set_power_mode(state, DIB8000_POWER_INTERFACE_ONLY);
 
@@ -1441,6 +1499,7 @@ static int dib8096p_set_output_mode(struct dvb_frontend *fe, int mode)
 	u8 prefer_mpeg_mux_use = 1;
 	int ret = 0;
 
+	state->output_mode = mode;
 	dib8096p_host_bus_drive(state, 1);
 
 	fifo_threshold = 1792;
@@ -1879,782 +1938,643 @@ static const u16 adc_target_16dB[11] = {
 };
 static const u8 permu_seg[] = { 6, 5, 7, 4, 8, 3, 9, 2, 10, 1, 11, 0, 12 };
 
-static void dib8000_set_channel(struct dib8000_state *state, u8 seq, u8 autosearching)
+static u16 dib8000_set_layer(struct dib8000_state *state, u8 layer_index, u16 max_constellation)
 {
-	u16 mode, max_constellation, seg_diff_mask = 0, nbseg_diff = 0;
-	u8 guard, crate, constellation, timeI;
-	u16 i, coeff[4], P_cfr_left_edge = 0, P_cfr_right_edge = 0, seg_mask13 = 0x1fff;	// All 13 segments enabled
-	const s16 *ncoeff = NULL, *ana_fe;
-	u16 tmcc_pow = 0;
-	u16 coff_pow = 0x2800;
-	u16 init_prbs = 0xfff;
-	u16 ana_gain = 0;
-
-	if (state->revision == 0x8090)
-		dib8000_init_sdram(state);
+	u8  cr, constellation, time_intlv;
+	struct dtv_frontend_properties *c = &state->fe[0]->dtv_property_cache;
 
-	if (state->ber_monitored_layer != LAYER_ALL)
-		dib8000_write_word(state, 285, (dib8000_read_word(state, 285) & 0x60) | state->ber_monitored_layer);
-	else
-		dib8000_write_word(state, 285, dib8000_read_word(state, 285) & 0x60);
-
-	i = dib8000_read_word(state, 26) & 1;	// P_dds_invspec
-	dib8000_write_word(state, 26, state->fe[0]->dtv_property_cache.inversion^i);
-
-	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode) {
-		//compute new dds_freq for the seg and adjust prbs
-		int seg_offset =
-			state->fe[0]->dtv_property_cache.isdbt_sb_segment_idx -
-			(state->fe[0]->dtv_property_cache.isdbt_sb_segment_count / 2) -
-			(state->fe[0]->dtv_property_cache.isdbt_sb_segment_count % 2);
-		int clk = state->cfg.pll->internal;
-		u32 segtodds = ((u32) (430 << 23) / clk) << 3;	// segtodds = SegBW / Fclk * pow(2,26)
-		int dds_offset = seg_offset * segtodds;
-		int new_dds, sub_channel;
-		if ((state->fe[0]->dtv_property_cache.isdbt_sb_segment_count % 2) == 0)
-			dds_offset -= (int)(segtodds / 2);
-
-		if (state->cfg.pll->ifreq == 0) {
-			if ((state->fe[0]->dtv_property_cache.inversion ^ i) == 0) {
-				dib8000_write_word(state, 26, dib8000_read_word(state, 26) | 1);
-				new_dds = dds_offset;
-			} else
-				new_dds = dds_offset;
-
-			// We shift tuning frequency if the wanted segment is :
-			//  - the segment of center frequency with an odd total number of segments
-			//  - the segment to the left of center frequency with an even total number of segments
-			//  - the segment to the right of center frequency with an even total number of segments
-			if ((state->fe[0]->dtv_property_cache.delivery_system == SYS_ISDBT)
-				&& (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1)
-					&& (((state->fe[0]->dtv_property_cache.isdbt_sb_segment_count % 2)
-					  && (state->fe[0]->dtv_property_cache.isdbt_sb_segment_idx ==
-				  ((state->fe[0]->dtv_property_cache.isdbt_sb_segment_count / 2) + 1)))
-					 || (((state->fe[0]->dtv_property_cache.isdbt_sb_segment_count % 2) == 0)
-						 && (state->fe[0]->dtv_property_cache.isdbt_sb_segment_idx == (state->fe[0]->dtv_property_cache.isdbt_sb_segment_count / 2)))
-					 || (((state->fe[0]->dtv_property_cache.isdbt_sb_segment_count % 2) == 0)
-						 && (state->fe[0]->dtv_property_cache.isdbt_sb_segment_idx ==
-							 ((state->fe[0]->dtv_property_cache.isdbt_sb_segment_count / 2) + 1)))
-					)) {
-				new_dds -= ((u32) (850 << 22) / clk) << 4;	// new_dds = 850 (freq shift in KHz) / Fclk * pow(2,26)
-			}
-		} else {
-			if ((state->fe[0]->dtv_property_cache.inversion ^ i) == 0)
-				new_dds = state->cfg.pll->ifreq - dds_offset;
-			else
-				new_dds = state->cfg.pll->ifreq + dds_offset;
-		}
-		dib8000_write_word(state, 27, (u16) ((new_dds >> 16) & 0x01ff));
-		dib8000_write_word(state, 28, (u16) (new_dds & 0xffff));
-		if (state->fe[0]->dtv_property_cache.isdbt_sb_segment_count % 2)
-			sub_channel = ((state->fe[0]->dtv_property_cache.isdbt_sb_subchannel + (3 * seg_offset) + 1) % 41) / 3;
-		else
-			sub_channel = ((state->fe[0]->dtv_property_cache.isdbt_sb_subchannel + (3 * seg_offset)) % 41) / 3;
-		sub_channel -= 6;
-
-		if (state->fe[0]->dtv_property_cache.transmission_mode == TRANSMISSION_MODE_2K
-				|| state->fe[0]->dtv_property_cache.transmission_mode == TRANSMISSION_MODE_4K) {
-			dib8000_write_word(state, 219, dib8000_read_word(state, 219) | 0x1);	//adp_pass =1
-			dib8000_write_word(state, 190, dib8000_read_word(state, 190) | (0x1 << 14));	//pha3_force_pha_shift = 1
-		} else {
-			dib8000_write_word(state, 219, dib8000_read_word(state, 219) & 0xfffe);	//adp_pass =0
-			dib8000_write_word(state, 190, dib8000_read_word(state, 190) & 0xbfff);	//pha3_force_pha_shift = 0
-		}
-
-		switch (state->fe[0]->dtv_property_cache.transmission_mode) {
-		case TRANSMISSION_MODE_2K:
-			switch (sub_channel) {
-			case -6:
-				init_prbs = 0x0;
-				break;	// 41, 0, 1
-			case -5:
-				init_prbs = 0x423;
-				break;	// 02~04
-			case -4:
-				init_prbs = 0x9;
-				break;	// 05~07
-			case -3:
-				init_prbs = 0x5C7;
-				break;	// 08~10
-			case -2:
-				init_prbs = 0x7A6;
-				break;	// 11~13
-			case -1:
-				init_prbs = 0x3D8;
-				break;	// 14~16
-			case 0:
-				init_prbs = 0x527;
-				break;	// 17~19
-			case 1:
-				init_prbs = 0x7FF;
-				break;	// 20~22
-			case 2:
-				init_prbs = 0x79B;
-				break;	// 23~25
-			case 3:
-				init_prbs = 0x3D6;
-				break;	// 26~28
-			case 4:
-				init_prbs = 0x3A2;
-				break;	// 29~31
-			case 5:
-				init_prbs = 0x53B;
-				break;	// 32~34
-			case 6:
-				init_prbs = 0x2F4;
-				break;	// 35~37
-			default:
-			case 7:
-				init_prbs = 0x213;
-				break;	// 38~40
-			}
-			break;
-
-		case TRANSMISSION_MODE_4K:
-			switch (sub_channel) {
-			case -6:
-				init_prbs = 0x0;
-				break;	// 41, 0, 1
-			case -5:
-				init_prbs = 0x208;
-				break;	// 02~04
-			case -4:
-				init_prbs = 0xC3;
-				break;	// 05~07
-			case -3:
-				init_prbs = 0x7B9;
-				break;	// 08~10
-			case -2:
-				init_prbs = 0x423;
-				break;	// 11~13
-			case -1:
-				init_prbs = 0x5C7;
-				break;	// 14~16
-			case 0:
-				init_prbs = 0x3D8;
-				break;	// 17~19
-			case 1:
-				init_prbs = 0x7FF;
-				break;	// 20~22
-			case 2:
-				init_prbs = 0x3D6;
-				break;	// 23~25
-			case 3:
-				init_prbs = 0x53B;
-				break;	// 26~28
-			case 4:
-				init_prbs = 0x213;
-				break;	// 29~31
-			case 5:
-				init_prbs = 0x29;
-				break;	// 32~34
-			case 6:
-				init_prbs = 0xD0;
-				break;	// 35~37
-			default:
-			case 7:
-				init_prbs = 0x48E;
-				break;	// 38~40
-			}
-			break;
-
-		default:
-		case TRANSMISSION_MODE_8K:
-			switch (sub_channel) {
-			case -6:
-				init_prbs = 0x0;
-				break;	// 41, 0, 1
-			case -5:
-				init_prbs = 0x740;
-				break;	// 02~04
-			case -4:
-				init_prbs = 0x069;
-				break;	// 05~07
-			case -3:
-				init_prbs = 0x7DD;
-				break;	// 08~10
-			case -2:
-				init_prbs = 0x208;
-				break;	// 11~13
-			case -1:
-				init_prbs = 0x7B9;
-				break;	// 14~16
-			case 0:
-				init_prbs = 0x5C7;
-				break;	// 17~19
-			case 1:
-				init_prbs = 0x7FF;
-				break;	// 20~22
-			case 2:
-				init_prbs = 0x53B;
-				break;	// 23~25
-			case 3:
-				init_prbs = 0x29;
-				break;	// 26~28
-			case 4:
-				init_prbs = 0x48E;
-				break;	// 29~31
-			case 5:
-				init_prbs = 0x4C4;
-				break;	// 32~34
-			case 6:
-				init_prbs = 0x367;
-				break;	// 33~37
-			default:
-			case 7:
-				init_prbs = 0x684;
-				break;	// 38~40
-			}
-			break;
-		}
-	} else {
-		dib8000_write_word(state, 27, (u16) ((state->cfg.pll->ifreq >> 16) & 0x01ff));
-		dib8000_write_word(state, 28, (u16) (state->cfg.pll->ifreq & 0xffff));
-		dib8000_write_word(state, 26, (u16) ((state->cfg.pll->ifreq >> 25) & 0x0003));
-	}
-	/*P_mode == ?? */
-	dib8000_write_word(state, 10, (seq << 4));
-	//  dib8000_write_word(state, 287, (dib8000_read_word(state, 287) & 0xe000) | 0x1000);
-
-	switch (state->fe[0]->dtv_property_cache.guard_interval) {
-	case GUARD_INTERVAL_1_32:
-		guard = 0;
-		break;
-	case GUARD_INTERVAL_1_16:
-		guard = 1;
-		break;
-	case GUARD_INTERVAL_1_8:
-		guard = 2;
-		break;
-	case GUARD_INTERVAL_1_4:
-	default:
-		guard = 3;
-		break;
-	}
-
-	dib8000_write_word(state, 1, (init_prbs << 2) | (guard & 0x3));	// ADDR 1
-
-	max_constellation = DQPSK;
-	for (i = 0; i < 3; i++) {
-		switch (state->fe[0]->dtv_property_cache.layer[i].modulation) {
-		case DQPSK:
+	switch (c->layer[layer_index].modulation) {
+	case DQPSK:
 			constellation = 0;
 			break;
-		case QPSK:
+	case  QPSK:
 			constellation = 1;
 			break;
-		case QAM_16:
+	case QAM_16:
 			constellation = 2;
 			break;
-		case QAM_64:
-		default:
+	case QAM_64:
+	default:
 			constellation = 3;
 			break;
-		}
+	}
 
-		switch (state->fe[0]->dtv_property_cache.layer[i].fec) {
-		case FEC_1_2:
-			crate = 1;
+	switch (c->layer[layer_index].fec) {
+	case FEC_1_2:
+			cr = 1;
 			break;
-		case FEC_2_3:
-			crate = 2;
+	case FEC_2_3:
+			cr = 2;
 			break;
-		case FEC_3_4:
-			crate = 3;
+	case FEC_3_4:
+			cr = 3;
 			break;
-		case FEC_5_6:
-			crate = 5;
+	case FEC_5_6:
+			cr = 5;
 			break;
-		case FEC_7_8:
-		default:
-			crate = 7;
+	case FEC_7_8:
+	default:
+			cr = 7;
 			break;
-		}
+	}
 
-		if ((state->fe[0]->dtv_property_cache.layer[i].interleaving > 0) &&
-				((state->fe[0]->dtv_property_cache.layer[i].interleaving <= 3) ||
-				 (state->fe[0]->dtv_property_cache.layer[i].interleaving == 4 && state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1))
-			)
-			timeI = state->fe[0]->dtv_property_cache.layer[i].interleaving;
-		else
-			timeI = 0;
-		dib8000_write_word(state, 2 + i, (constellation << 10) | ((state->fe[0]->dtv_property_cache.layer[i].segment_count & 0xf) << 6) |
-					(crate << 3) | timeI);
-		if (state->fe[0]->dtv_property_cache.layer[i].segment_count > 0) {
-			switch (max_constellation) {
-			case DQPSK:
-			case QPSK:
-				if (state->fe[0]->dtv_property_cache.layer[i].modulation == QAM_16 ||
-					state->fe[0]->dtv_property_cache.layer[i].modulation == QAM_64)
-					max_constellation = state->fe[0]->dtv_property_cache.layer[i].modulation;
+	if ((c->layer[layer_index].interleaving > 0) && ((c->layer[layer_index].interleaving <= 3) || (c->layer[layer_index].interleaving == 4 && c->isdbt_sb_mode == 1)))
+		time_intlv = c->layer[layer_index].interleaving;
+	else
+		time_intlv = 0;
+
+	dib8000_write_word(state, 2 + layer_index, (constellation << 10) | ((c->layer[layer_index].segment_count & 0xf) << 6) | (cr << 3) | time_intlv);
+	if (c->layer[layer_index].segment_count > 0) {
+		switch (max_constellation) {
+		case DQPSK:
+		case QPSK:
+				if (c->layer[layer_index].modulation == QAM_16 || c->layer[layer_index].modulation == QAM_64)
+					max_constellation = c->layer[layer_index].modulation;
 				break;
-			case QAM_16:
-				if (state->fe[0]->dtv_property_cache.layer[i].modulation == QAM_64)
-					max_constellation = state->fe[0]->dtv_property_cache.layer[i].modulation;
+		case QAM_16:
+				if (c->layer[layer_index].modulation == QAM_64)
+					max_constellation = c->layer[layer_index].modulation;
 				break;
-			}
 		}
 	}
 
-	mode = fft_to_mode(state);
+	return  max_constellation;
+}
 
-	//dib8000_write_word(state, 5, 13); /*p_last_seg = 13*/
+static const u16 adp_Q64[4] = {0x0148, 0xfff0, 0x00a4, 0xfff8}; /* P_adp_regul_cnt 0.04, P_adp_noise_cnt -0.002, P_adp_regul_ext 0.02, P_adp_noise_ext -0.001 */
+static const u16 adp_Q16[4] = {0x023d, 0xffdf, 0x00a4, 0xfff0}; /* P_adp_regul_cnt 0.07, P_adp_noise_cnt -0.004, P_adp_regul_ext 0.02, P_adp_noise_ext -0.002 */
+static const u16 adp_Qdefault[4] = {0x099a, 0xffae, 0x0333, 0xfff8}; /* P_adp_regul_cnt 0.3,  P_adp_noise_cnt -0.01,  P_adp_regul_ext 0.1,  P_adp_noise_ext -0.002 */
+static u16 dib8000_adp_fine_tune(struct dib8000_state *state, u16 max_constellation)
+{
+	u16 i, ana_gain = 0;
+	const u16 *adp;
 
-	dib8000_write_word(state, 274, (dib8000_read_word(state, 274) & 0xffcf) |
-				((state->fe[0]->dtv_property_cache.isdbt_partial_reception & 1) << 5) | ((state->fe[0]->dtv_property_cache.
-												 isdbt_sb_mode & 1) << 4));
+	/* channel estimation fine configuration */
+	switch (max_constellation) {
+	case QAM_64:
+			ana_gain = 0x7;
+			adp = &adp_Q64[0];
+			break;
+	case QAM_16:
+			ana_gain = 0x7;
+			adp = &adp_Q16[0];
+			break;
+	default:
+			ana_gain = 0;
+			adp = &adp_Qdefault[0];
+			break;
+	}
 
-	dprintk("mode = %d ; guard = %d", mode, state->fe[0]->dtv_property_cache.guard_interval);
+	for (i = 0; i < 4; i++)
+		dib8000_write_word(state, 215 + i, adp[i]);
 
-	/* signal optimization parameter */
+	return ana_gain;
+}
 
-	if (state->fe[0]->dtv_property_cache.isdbt_partial_reception) {
-		seg_diff_mask = (state->fe[0]->dtv_property_cache.layer[0].modulation == DQPSK) << permu_seg[0];
-		for (i = 1; i < 3; i++)
-			nbseg_diff +=
-				(state->fe[0]->dtv_property_cache.layer[i].modulation == DQPSK) * state->fe[0]->dtv_property_cache.layer[i].segment_count;
-		for (i = 0; i < nbseg_diff; i++)
-			seg_diff_mask |= 1 << permu_seg[i + 1];
-	} else {
-		for (i = 0; i < 3; i++)
-			nbseg_diff +=
-				(state->fe[0]->dtv_property_cache.layer[i].modulation == DQPSK) * state->fe[0]->dtv_property_cache.layer[i].segment_count;
-		for (i = 0; i < nbseg_diff; i++)
-			seg_diff_mask |= 1 << permu_seg[i];
+static void dib8000_update_ana_gain(struct dib8000_state *state, u16 ana_gain)
+{
+	u16 i;
+
+	dib8000_write_word(state, 116, ana_gain);
+
+	/* update ADC target depending on ana_gain */
+	if (ana_gain) { /* set -16dB ADC target for ana_gain=-1 */
+		for (i = 0; i < 10; i++)
+			dib8000_write_word(state, 80 + i, adc_target_16dB[i]);
+	} else { /* set -22dB ADC target for ana_gain=0 */
+		for (i = 0; i < 10; i++)
+			dib8000_write_word(state, 80 + i, adc_target_16dB[i] - 355);
 	}
-	dprintk("nbseg_diff = %X (%d)", seg_diff_mask, seg_diff_mask);
+}
 
-	state->differential_constellation = (seg_diff_mask != 0);
-	if (state->revision != 0x8090)
-		dib8000_set_diversity_in(state->fe[0], state->diversity_onoff);
-	else
-		dib8096p_set_diversity_in(state->fe[0], state->diversity_onoff);
+static void dib8000_load_ana_fe_coefs(struct dib8000_state *state, const s16 *ana_fe)
+{
+	u16 mode = 0;
 
-	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1) {
-		if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 1)
-			seg_mask13 = 0x00E0;
-		else		// 1-segment
-			seg_mask13 = 0x0040;
-	} else
-		seg_mask13 = 0x1fff;
+	if (state->isdbt_cfg_loaded == 0)
+		for (mode = 0; mode < 24; mode++)
+			dib8000_write_word(state, 117 + mode, ana_fe[mode]);
+}
 
-	// WRITE: Mode & Diff mask
-	dib8000_write_word(state, 0, (mode << 13) | seg_diff_mask);
+static const u16 lut_prbs_2k[14] = {
+	0, 0x423, 0x009, 0x5C7, 0x7A6, 0x3D8, 0x527, 0x7FF, 0x79B, 0x3D6, 0x3A2, 0x53B, 0x2F4, 0x213
+};
+static const u16 lut_prbs_4k[14] = {
+	0, 0x208, 0x0C3, 0x7B9, 0x423, 0x5C7, 0x3D8, 0x7FF, 0x3D6, 0x53B, 0x213, 0x029, 0x0D0, 0x48E
+};
+static const u16 lut_prbs_8k[14] = {
+	0, 0x740, 0x069, 0x7DD, 0x208, 0x7B9, 0x5C7, 0x7FF, 0x53B, 0x029, 0x48E, 0x4C4, 0x367, 0x684
+};
 
-	if ((seg_diff_mask) || (state->fe[0]->dtv_property_cache.isdbt_sb_mode))
-		dib8000_write_word(state, 268, (dib8000_read_word(state, 268) & 0xF9FF) | 0x0200);
-	else
-		dib8000_write_word(state, 268, (2 << 9) | 39);	//init value
+static u16 dib8000_get_init_prbs(struct dib8000_state *state, u16 subchannel)
+{
+	int sub_channel_prbs_group = 0;
 
-	// ---- SMALL ----
-	// P_small_seg_diff
-	dib8000_write_word(state, 352, seg_diff_mask);	// ADDR 352
+	sub_channel_prbs_group = (subchannel / 3) + 1;
+	dprintk("sub_channel_prbs_group = %d , subchannel =%d prbs = 0x%04x", sub_channel_prbs_group, subchannel, lut_prbs_8k[sub_channel_prbs_group]);
 
-	dib8000_write_word(state, 353, seg_mask13);	// ADDR 353
+	switch (state->fe[0]->dtv_property_cache.transmission_mode) {
+	case TRANSMISSION_MODE_2K:
+			return lut_prbs_2k[sub_channel_prbs_group];
+	case TRANSMISSION_MODE_4K:
+			return lut_prbs_4k[sub_channel_prbs_group];
+	default:
+	case TRANSMISSION_MODE_8K:
+			return lut_prbs_8k[sub_channel_prbs_group];
+	}
+}
+
+static void dib8000_set_13seg_channel(struct dib8000_state *state)
+{
+	u16 i;
+	u16 coff_pow = 0x2800;
 
-/*	// P_small_narrow_band=0, P_small_last_seg=13, P_small_offset_num_car=5 */
+	state->seg_mask = 0x1fff; /* All 13 segments enabled */
 
-	// ---- SMALL ----
-	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1) {
-		switch (state->fe[0]->dtv_property_cache.transmission_mode) {
+	/* ---- COFF ---- Carloff, the most robust --- */
+	if (state->isdbt_cfg_loaded == 0) {  /* if not Sound Broadcasting mode : put default values for 13 segments */
+		dib8000_write_word(state, 180, (16 << 6) | 9);
+		dib8000_write_word(state, 187, (4 << 12) | (8 << 5) | 0x2);
+		coff_pow = 0x2800;
+		for (i = 0; i < 6; i++)
+			dib8000_write_word(state, 181+i, coff_pow);
+
+		/* P_ctrl_corm_thres4pre_freq_inh=1, P_ctrl_pre_freq_mode_sat=1 */
+		/* P_ctrl_pre_freq_mode_sat=1, P_ctrl_pre_freq_inh=0, P_ctrl_pre_freq_step = 3, P_pre_freq_win_len=1 */
+		dib8000_write_word(state, 338, (1 << 12) | (1 << 10) | (0 << 9) | (3 << 5) | 1);
+
+		/* P_ctrl_pre_freq_win_len=8, P_ctrl_pre_freq_thres_lockin=6 */
+		dib8000_write_word(state, 340, (8 << 6) | (6 << 0));
+		/* P_ctrl_pre_freq_thres_lockout=4, P_small_use_tmcc/ac/cp=1 */
+		dib8000_write_word(state, 341, (4 << 3) | (1 << 2) | (1 << 1) | (1 << 0));
+
+		dib8000_write_word(state, 228, 0);  /* default value */
+		dib8000_write_word(state, 265, 31); /* default value */
+		dib8000_write_word(state, 205, 0x200f); /* init value */
+	}
+
+	/*
+	 * make the cpil_coff_lock more robust but slower p_coff_winlen
+	 * 6bits; p_coff_thres_lock 6bits (for coff lock if needed)
+	 */
+
+	if (state->cfg.pll->ifreq == 0)
+		dib8000_write_word(state, 266, ~state->seg_mask | state->seg_diff_mask | 0x40); /* P_equal_noise_seg_inh */
+
+	dib8000_load_ana_fe_coefs(state, ana_fe_coeff_13seg);
+}
+
+static void dib8000_set_subchannel_prbs(struct dib8000_state *state, u16 init_prbs)
+{
+	u16 reg_1;
+
+	reg_1 = dib8000_read_word(state, 1);
+	dib8000_write_word(state, 1, (init_prbs << 2) | (reg_1 & 0x3)); /* ADDR 1 */
+}
+
+static void dib8000_small_fine_tune(struct dib8000_state *state)
+{
+	u16 i;
+	const s16 *ncoeff;
+	struct dtv_frontend_properties *c = &state->fe[0]->dtv_property_cache;
+
+	dib8000_write_word(state, 352, state->seg_diff_mask);
+	dib8000_write_word(state, 353, state->seg_mask);
+
+	/* P_small_coef_ext_enable=ISDB-Tsb, P_small_narrow_band=ISDB-Tsb, P_small_last_seg=13, P_small_offset_num_car=5 */
+	dib8000_write_word(state, 351, (c->isdbt_sb_mode << 9) | (c->isdbt_sb_mode << 8) | (13 << 4) | 5);
+
+	if (c->isdbt_sb_mode) {
+		/* ---- SMALL ---- */
+		switch (c->transmission_mode) {
 		case TRANSMISSION_MODE_2K:
-			if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0) {
-				if (state->fe[0]->dtv_property_cache.layer[0].modulation == DQPSK)
-					ncoeff = coeff_2k_sb_1seg_dqpsk;
-				else	// QPSK or QAM
-					ncoeff = coeff_2k_sb_1seg;
-			} else {	// 3-segments
-				if (state->fe[0]->dtv_property_cache.layer[0].modulation == DQPSK) {
-					if (state->fe[0]->dtv_property_cache.layer[1].modulation == DQPSK)
-						ncoeff = coeff_2k_sb_3seg_0dqpsk_1dqpsk;
-					else	// QPSK or QAM on external segments
-						ncoeff = coeff_2k_sb_3seg_0dqpsk;
-				} else {	// QPSK or QAM on central segment
-					if (state->fe[0]->dtv_property_cache.layer[1].modulation == DQPSK)
-						ncoeff = coeff_2k_sb_3seg_1dqpsk;
-					else	// QPSK or QAM on external segments
-						ncoeff = coeff_2k_sb_3seg;
+				if (c->isdbt_partial_reception == 0) { /* 1-seg */
+					if (c->layer[0].modulation == DQPSK) /* DQPSK */
+						ncoeff = coeff_2k_sb_1seg_dqpsk;
+					else /* QPSK or QAM */
+						ncoeff = coeff_2k_sb_1seg;
+				} else { /* 3-segments */
+					if (c->layer[0].modulation == DQPSK) { /* DQPSK on central segment */
+						if (c->layer[1].modulation == DQPSK) /* DQPSK on external segments */
+							ncoeff = coeff_2k_sb_3seg_0dqpsk_1dqpsk;
+						else /* QPSK or QAM on external segments */
+							ncoeff = coeff_2k_sb_3seg_0dqpsk;
+					} else { /* QPSK or QAM on central segment */
+						if (c->layer[1].modulation == DQPSK) /* DQPSK on external segments */
+							ncoeff = coeff_2k_sb_3seg_1dqpsk;
+						else /* QPSK or QAM on external segments */
+							ncoeff = coeff_2k_sb_3seg;
+					}
 				}
-			}
-			break;
-
+				break;
 		case TRANSMISSION_MODE_4K:
-			if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0) {
-				if (state->fe[0]->dtv_property_cache.layer[0].modulation == DQPSK)
-					ncoeff = coeff_4k_sb_1seg_dqpsk;
-				else	// QPSK or QAM
-					ncoeff = coeff_4k_sb_1seg;
-			} else {	// 3-segments
-				if (state->fe[0]->dtv_property_cache.layer[0].modulation == DQPSK) {
-					if (state->fe[0]->dtv_property_cache.layer[1].modulation == DQPSK) {
-						ncoeff = coeff_4k_sb_3seg_0dqpsk_1dqpsk;
-					} else {	// QPSK or QAM on external segments
-						ncoeff = coeff_4k_sb_3seg_0dqpsk;
+				if (c->isdbt_partial_reception == 0) { /* 1-seg */
+					if (c->layer[0].modulation == DQPSK) /* DQPSK */
+						ncoeff = coeff_4k_sb_1seg_dqpsk;
+					else /* QPSK or QAM */
+						ncoeff = coeff_4k_sb_1seg;
+				} else { /* 3-segments */
+					if (c->layer[0].modulation == DQPSK) { /* DQPSK on central segment */
+						if (c->layer[1].modulation == DQPSK) /* DQPSK on external segments */
+							ncoeff = coeff_4k_sb_3seg_0dqpsk_1dqpsk;
+						else /* QPSK or QAM on external segments */
+							ncoeff = coeff_4k_sb_3seg_0dqpsk;
+					} else { /* QPSK or QAM on central segment */
+						if (c->layer[1].modulation == DQPSK) /* DQPSK on external segments */
+							ncoeff = coeff_4k_sb_3seg_1dqpsk;
+						else /* QPSK or QAM on external segments */
+							ncoeff = coeff_4k_sb_3seg;
 					}
-				} else {	// QPSK or QAM on central segment
-					if (state->fe[0]->dtv_property_cache.layer[1].modulation == DQPSK) {
-						ncoeff = coeff_4k_sb_3seg_1dqpsk;
-					} else	// QPSK or QAM on external segments
-						ncoeff = coeff_4k_sb_3seg;
 				}
-			}
-			break;
-
+				break;
 		case TRANSMISSION_MODE_AUTO:
 		case TRANSMISSION_MODE_8K:
 		default:
-			if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0) {
-				if (state->fe[0]->dtv_property_cache.layer[0].modulation == DQPSK)
-					ncoeff = coeff_8k_sb_1seg_dqpsk;
-				else	// QPSK or QAM
-					ncoeff = coeff_8k_sb_1seg;
-			} else {	// 3-segments
-				if (state->fe[0]->dtv_property_cache.layer[0].modulation == DQPSK) {
-					if (state->fe[0]->dtv_property_cache.layer[1].modulation == DQPSK) {
-						ncoeff = coeff_8k_sb_3seg_0dqpsk_1dqpsk;
-					} else {	// QPSK or QAM on external segments
-						ncoeff = coeff_8k_sb_3seg_0dqpsk;
+				if (c->isdbt_partial_reception == 0) { /* 1-seg */
+					if (c->layer[0].modulation == DQPSK) /* DQPSK */
+						ncoeff = coeff_8k_sb_1seg_dqpsk;
+					else /* QPSK or QAM */
+						ncoeff = coeff_8k_sb_1seg;
+				} else { /* 3-segments */
+					if (c->layer[0].modulation == DQPSK) { /* DQPSK on central segment */
+						if (c->layer[1].modulation == DQPSK) /* DQPSK on external segments */
+							ncoeff = coeff_8k_sb_3seg_0dqpsk_1dqpsk;
+						else /* QPSK or QAM on external segments */
+							ncoeff = coeff_8k_sb_3seg_0dqpsk;
+					} else { /* QPSK or QAM on central segment */
+						if (c->layer[1].modulation == DQPSK) /* DQPSK on external segments */
+							ncoeff = coeff_8k_sb_3seg_1dqpsk;
+						else /* QPSK or QAM on external segments */
+							ncoeff = coeff_8k_sb_3seg;
 					}
-				} else {	// QPSK or QAM on central segment
-					if (state->fe[0]->dtv_property_cache.layer[1].modulation == DQPSK) {
-						ncoeff = coeff_8k_sb_3seg_1dqpsk;
-					} else	// QPSK or QAM on external segments
-						ncoeff = coeff_8k_sb_3seg;
 				}
-			}
-			break;
+				break;
 		}
+
 		for (i = 0; i < 8; i++)
 			dib8000_write_word(state, 343 + i, ncoeff[i]);
 	}
+}
 
-	// P_small_coef_ext_enable=ISDB-Tsb, P_small_narrow_band=ISDB-Tsb, P_small_last_seg=13, P_small_offset_num_car=5
-	dib8000_write_word(state, 351,
-				(state->fe[0]->dtv_property_cache.isdbt_sb_mode << 9) | (state->fe[0]->dtv_property_cache.isdbt_sb_mode << 8) | (13 << 4) | 5);
+static const u16 coff_thres_1seg[3] = {300, 150, 80};
+static const u16 coff_thres_3seg[3] = {350, 300, 250};
+static void dib8000_set_sb_channel(struct dib8000_state *state)
+{
+	struct dtv_frontend_properties *c = &state->fe[0]->dtv_property_cache;
+	const u16 *coff;
+	u16 i;
 
-	// ---- COFF ----
-	// Carloff, the most robust
-	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1) {
+	if (c->transmission_mode == TRANSMISSION_MODE_2K || c->transmission_mode == TRANSMISSION_MODE_4K) {
+		dib8000_write_word(state, 219, dib8000_read_word(state, 219) | 0x1); /* adp_pass =1 */
+		dib8000_write_word(state, 190, dib8000_read_word(state, 190) | (0x1 << 14)); /* pha3_force_pha_shift = 1 */
+	} else {
+		dib8000_write_word(state, 219, dib8000_read_word(state, 219) & 0xfffe); /* adp_pass =0 */
+		dib8000_write_word(state, 190, dib8000_read_word(state, 190) & 0xbfff); /* pha3_force_pha_shift = 0 */
+	}
 
-		// P_coff_cpil_alpha=4, P_coff_inh=0, P_coff_cpil_winlen=64
-		// P_coff_narrow_band=1, P_coff_square_val=1, P_coff_one_seg=~partial_rcpt, P_coff_use_tmcc=1, P_coff_use_ac=1
-		dib8000_write_word(state, 187,
-					(4 << 12) | (0 << 11) | (63 << 5) | (0x3 << 3) | ((~state->fe[0]->dtv_property_cache.isdbt_partial_reception & 1) << 2)
-					| 0x3);
+	if (c->isdbt_partial_reception == 1) /* 3-segments */
+		state->seg_mask = 0x00E0;
+	else /* 1-segment */
+		state->seg_mask = 0x0040;
 
-/*		// P_small_coef_ext_enable = 1 */
-/*		dib8000_write_word(state, 351, dib8000_read_word(state, 351) | 0x200); */
+	dib8000_write_word(state, 268, (dib8000_read_word(state, 268) & 0xF9FF) | 0x0200);
 
-		if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0) {
+	/* ---- COFF ---- Carloff, the most robust --- */
+	/* P_coff_cpil_alpha=4, P_coff_inh=0, P_coff_cpil_winlen=64, P_coff_narrow_band=1, P_coff_square_val=1, P_coff_one_seg=~partial_rcpt, P_coff_use_tmcc=1, P_coff_use_ac=1 */
+	dib8000_write_word(state, 187, (4 << 12) | (0 << 11) | (63 << 5) | (0x3 << 3) | ((~c->isdbt_partial_reception & 1) << 2) | 0x3);
 
-			// P_coff_winlen=63, P_coff_thres_lock=15, P_coff_one_seg_width= (P_mode == 3) , P_coff_one_seg_sym= (P_mode-1)
-			if (mode == 3)
-				dib8000_write_word(state, 180, 0x1fcf | ((mode - 1) << 14));
-			else
-				dib8000_write_word(state, 180, 0x0fcf | ((mode - 1) << 14));
-			// P_ctrl_corm_thres4pre_freq_inh=1,P_ctrl_pre_freq_mode_sat=1,
-			// P_ctrl_pre_freq_inh=0, P_ctrl_pre_freq_step = 5, P_pre_freq_win_len=4
-			dib8000_write_word(state, 338, (1 << 12) | (1 << 10) | (0 << 9) | (5 << 5) | 4);
-			// P_ctrl_pre_freq_win_len=16, P_ctrl_pre_freq_thres_lockin=8
-			dib8000_write_word(state, 340, (16 << 6) | (8 << 0));
-			// P_ctrl_pre_freq_thres_lockout=6, P_small_use_tmcc/ac/cp=1
-			dib8000_write_word(state, 341, (6 << 3) | (1 << 2) | (1 << 1) | (1 << 0));
-
-			// P_coff_corthres_8k, 4k, 2k and P_coff_cpilthres_8k, 4k, 2k
-			dib8000_write_word(state, 181, 300);
-			dib8000_write_word(state, 182, 150);
-			dib8000_write_word(state, 183, 80);
-			dib8000_write_word(state, 184, 300);
-			dib8000_write_word(state, 185, 150);
-			dib8000_write_word(state, 186, 80);
-		} else {	// Sound Broadcasting mode 3 seg
-			// P_coff_one_seg_sym= 1, P_coff_one_seg_width= 1, P_coff_winlen=63, P_coff_thres_lock=15
-			/*	if (mode == 3) */
-			/*		dib8000_write_word(state, 180, 0x2fca | ((0) << 14)); */
-			/*	else */
-			/*		dib8000_write_word(state, 180, 0x2fca | ((1) << 14)); */
-			dib8000_write_word(state, 180, 0x1fcf | (1 << 14));
-
-			// P_ctrl_corm_thres4pre_freq_inh = 1, P_ctrl_pre_freq_mode_sat=1,
-			// P_ctrl_pre_freq_inh=0, P_ctrl_pre_freq_step = 4, P_pre_freq_win_len=4
-			dib8000_write_word(state, 338, (1 << 12) | (1 << 10) | (0 << 9) | (4 << 5) | 4);
-			// P_ctrl_pre_freq_win_len=16, P_ctrl_pre_freq_thres_lockin=8
-			dib8000_write_word(state, 340, (16 << 6) | (8 << 0));
-			//P_ctrl_pre_freq_thres_lockout=6, P_small_use_tmcc/ac/cp=1
-			dib8000_write_word(state, 341, (6 << 3) | (1 << 2) | (1 << 1) | (1 << 0));
-
-			// P_coff_corthres_8k, 4k, 2k and P_coff_cpilthres_8k, 4k, 2k
-			dib8000_write_word(state, 181, 350);
-			dib8000_write_word(state, 182, 300);
-			dib8000_write_word(state, 183, 250);
-			dib8000_write_word(state, 184, 350);
-			dib8000_write_word(state, 185, 300);
-			dib8000_write_word(state, 186, 250);
-		}
+	dib8000_write_word(state, 340, (16 << 6) | (8 << 0)); /* P_ctrl_pre_freq_win_len=16, P_ctrl_pre_freq_thres_lockin=8 */
+	dib8000_write_word(state, 341, (6 << 3) | (1 << 2) | (1 << 1) | (1 << 0));/* P_ctrl_pre_freq_thres_lockout=6, P_small_use_tmcc/ac/cp=1 */
 
-	} else if (state->isdbt_cfg_loaded == 0) {	// if not Sound Broadcasting mode : put default values for 13 segments
-		dib8000_write_word(state, 180, (16 << 6) | 9);
-		dib8000_write_word(state, 187, (4 << 12) | (8 << 5) | 0x2);
-		coff_pow = 0x2800;
-		for (i = 0; i < 6; i++)
-			dib8000_write_word(state, 181 + i, coff_pow);
+	/* Sound Broadcasting mode 1 seg */
+	if (c->isdbt_partial_reception == 0) {
+		/* P_coff_winlen=63, P_coff_thres_lock=15, P_coff_one_seg_width = (P_mode == 3) , P_coff_one_seg_sym = (P_mode-1) */
+		if (state->mode == 3)
+			dib8000_write_word(state, 180, 0x1fcf | ((state->mode - 1) << 14));
+		else
+			dib8000_write_word(state, 180, 0x0fcf | ((state->mode - 1) << 14));
 
-		// P_ctrl_corm_thres4pre_freq_inh=1, P_ctrl_pre_freq_mode_sat=1,
-		// P_ctrl_pre_freq_mode_sat=1, P_ctrl_pre_freq_inh=0, P_ctrl_pre_freq_step = 3, P_pre_freq_win_len=1
-		dib8000_write_word(state, 338, (1 << 12) | (1 << 10) | (0 << 9) | (3 << 5) | 1);
+		/* P_ctrl_corm_thres4pre_freq_inh=1,P_ctrl_pre_freq_mode_sat=1, P_ctrl_pre_freq_inh=0, P_ctrl_pre_freq_step = 5, P_pre_freq_win_len=4 */
+		dib8000_write_word(state, 338, (1 << 12) | (1 << 10) | (0 << 9) | (5 << 5) | 4);
+		coff = &coff_thres_1seg[0];
+	} else {   /* Sound Broadcasting mode 3 seg */
+		dib8000_write_word(state, 180, 0x1fcf | (1 << 14));
+		/* P_ctrl_corm_thres4pre_freq_inh = 1, P_ctrl_pre_freq_mode_sat=1, P_ctrl_pre_freq_inh=0, P_ctrl_pre_freq_step = 4, P_pre_freq_win_len=4 */
+		dib8000_write_word(state, 338, (1 << 12) | (1 << 10) | (0 << 9) | (4 << 5) | 4);
+		coff = &coff_thres_3seg[0];
+	}
 
-		// P_ctrl_pre_freq_win_len=8, P_ctrl_pre_freq_thres_lockin=6
-		dib8000_write_word(state, 340, (8 << 6) | (6 << 0));
-		// P_ctrl_pre_freq_thres_lockout=4, P_small_use_tmcc/ac/cp=1
-		dib8000_write_word(state, 341, (4 << 3) | (1 << 2) | (1 << 1) | (1 << 0));
+	dib8000_write_word(state, 228, 1); /* P_2d_mode_byp=1 */
+	dib8000_write_word(state, 205, dib8000_read_word(state, 205) & 0xfff0); /* P_cspu_win_cut = 0 */
+
+	if (c->isdbt_partial_reception == 0 && c->transmission_mode == TRANSMISSION_MODE_2K)
+		dib8000_write_word(state, 265, 15); /* P_equal_noise_sel = 15 */
+
+	/* Write COFF thres */
+	for (i = 0 ; i < 3; i++) {
+		dib8000_write_word(state, 181+i, coff[i]);
+		dib8000_write_word(state, 184+i, coff[i]);
 	}
-	// ---- FFT ----
-	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1 && state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0)
-		dib8000_write_word(state, 178, 64);	// P_fft_powrange=64
-	else
-		dib8000_write_word(state, 178, 32);	// P_fft_powrange=32
 
-	/* make the cpil_coff_lock more robust but slower p_coff_winlen
+	/*
+	 * make the cpil_coff_lock more robust but slower p_coff_winlen
 	 * 6bits; p_coff_thres_lock 6bits (for coff lock if needed)
 	 */
-	/* if ( ( nbseg_diff>0)&&(nbseg_diff<13))
-		dib8000_write_word(state, 187, (dib8000_read_word(state, 187) & 0xfffb) | (1 << 3)); */
-
-	dib8000_write_word(state, 189, ~seg_mask13 | seg_diff_mask);	/* P_lmod4_seg_inh       */
-	dib8000_write_word(state, 192, ~seg_mask13 | seg_diff_mask);	/* P_pha3_seg_inh        */
-	dib8000_write_word(state, 225, ~seg_mask13 | seg_diff_mask);	/* P_tac_seg_inh         */
-	if ((!state->fe[0]->dtv_property_cache.isdbt_sb_mode) && (state->cfg.pll->ifreq == 0))
-		dib8000_write_word(state, 266, ~seg_mask13 | seg_diff_mask | 0x40);	/* P_equal_noise_seg_inh */
-	else
-		dib8000_write_word(state, 266, ~seg_mask13 | seg_diff_mask);	/* P_equal_noise_seg_inh */
-	dib8000_write_word(state, 287, ~seg_mask13 | 0x1000);	/* P_tmcc_seg_inh        */
-	//dib8000_write_word(state, 288, ~seg_mask13 | seg_diff_mask); /* P_tmcc_seg_eq_inh */
-	if (!autosearching)
-		dib8000_write_word(state, 288, (~seg_mask13 | seg_diff_mask) & 0x1fff);	/* P_tmcc_seg_eq_inh */
+
+	dib8000_write_word(state, 266, ~state->seg_mask | state->seg_diff_mask); /* P_equal_noise_seg_inh */
+
+	if (c->isdbt_partial_reception == 0)
+		dib8000_write_word(state, 178, 64); /* P_fft_powrange = 64 */
 	else
-		dib8000_write_word(state, 288, 0x1fff);	//disable equalisation of the tmcc when autosearch to be able to find the DQPSK channels.
-	dprintk("287 = %X (%d)", ~seg_mask13 | 0x1000, ~seg_mask13 | 0x1000);
-
-	dib8000_write_word(state, 211, seg_mask13 & (~seg_diff_mask));	/* P_des_seg_enabled     */
-
-	/* offset loop parameters */
-	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1) {
-		if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0)
-			/* P_timf_alpha = (11-P_mode), P_corm_alpha=6, P_corm_thres=0x80 */
-			dib8000_write_word(state, 32, ((11 - mode) << 12) | (6 << 8) | 0x40);
-
-		else		// Sound Broadcasting mode 3 seg
-			/* P_timf_alpha = (10-P_mode), P_corm_alpha=6, P_corm_thres=0x80 */
-			dib8000_write_word(state, 32, ((10 - mode) << 12) | (6 << 8) | 0x60);
-	} else
-		// TODO in 13 seg, timf_alpha can always be the same or not ?
-		/* P_timf_alpha = (9-P_mode, P_corm_alpha=6, P_corm_thres=0x80 */
-		dib8000_write_word(state, 32, ((9 - mode) << 12) | (6 << 8) | 0x80);
-
-	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1) {
-		if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0)
-			/* P_ctrl_pha_off_max=3   P_ctrl_sfreq_inh =0  P_ctrl_sfreq_step = (11-P_mode)  */
-			dib8000_write_word(state, 37, (3 << 5) | (0 << 4) | (10 - mode));
-
-		else		// Sound Broadcasting mode 3 seg
-			/* P_ctrl_pha_off_max=3   P_ctrl_sfreq_inh =0  P_ctrl_sfreq_step = (10-P_mode)  */
-			dib8000_write_word(state, 37, (3 << 5) | (0 << 4) | (9 - mode));
-	} else
-		/* P_ctrl_pha_off_max=3   P_ctrl_sfreq_inh =0  P_ctrl_sfreq_step = 9  */
-		dib8000_write_word(state, 37, (3 << 5) | (0 << 4) | (8 - mode));
+		dib8000_write_word(state, 178, 32); /* P_fft_powrange = 32 */
+}
 
-	/* P_dvsy_sync_wait - reuse mode */
-	switch (state->fe[0]->dtv_property_cache.transmission_mode) {
-	case TRANSMISSION_MODE_8K:
-		mode = 256;
-		break;
-	case TRANSMISSION_MODE_4K:
-		mode = 128;
-		break;
-	default:
-	case TRANSMISSION_MODE_2K:
-		mode = 64;
-		break;
+static void dib8000_set_isdbt_common_channel(struct dib8000_state *state, u8 seq, u8 autosearching)
+{
+	u16 p_cfr_left_edge  = 0, p_cfr_right_edge = 0;
+	u16 tmcc_pow = 0, ana_gain = 0, tmp = 0, i = 0, nbseg_diff = 0 ;
+	u16 max_constellation = DQPSK;
+	int init_prbs;
+	struct dtv_frontend_properties *c = &state->fe[0]->dtv_property_cache;
+
+	/* P_mode */
+	dib8000_write_word(state, 10, (seq << 4));
+
+	/* init mode */
+	state->mode = fft_to_mode(state);
+
+	/* set guard */
+	tmp = dib8000_read_word(state, 1);
+	dib8000_write_word(state, 1, (tmp&0xfffc) | (c->guard_interval & 0x3));
+
+	dib8000_write_word(state, 274, (dib8000_read_word(state, 274) & 0xffcf) | ((c->isdbt_partial_reception & 1) << 5) | ((c->isdbt_sb_mode & 1) << 4));
+
+	/* signal optimization parameter */
+	if (c->isdbt_partial_reception) {
+		state->seg_diff_mask = (c->layer[0].modulation == DQPSK) << permu_seg[0];
+		for (i = 1; i < 3; i++)
+			nbseg_diff += (c->layer[i].modulation == DQPSK) * c->layer[i].segment_count;
+		for (i = 0; i < nbseg_diff; i++)
+			state->seg_diff_mask |= 1 << permu_seg[i+1];
+	} else {
+		for (i = 0; i < 3; i++)
+			nbseg_diff += (c->layer[i].modulation == DQPSK) * c->layer[i].segment_count;
+		for (i = 0; i < nbseg_diff; i++)
+			state->seg_diff_mask |= 1 << permu_seg[i];
 	}
-	if (state->cfg.diversity_delay == 0)
-		mode = (mode * (1 << (guard)) * 3) / 2 + 48;	// add 50% SFN margin + compensate for one DVSY-fifo
+
+	if (state->seg_diff_mask)
+		dib8000_write_word(state, 268, (dib8000_read_word(state, 268) & 0xF9FF) | 0x0200);
 	else
-		mode = (mode * (1 << (guard)) * 3) / 2 + state->cfg.diversity_delay;	// add 50% SFN margin + compensate for DVSY-fifo
-	mode <<= 4;
-	dib8000_write_word(state, 273, (dib8000_read_word(state, 273) & 0x000f) | mode);
+		dib8000_write_word(state, 268, (2 << 9) | 39); /*init value */
 
-	/* channel estimation fine configuration */
-	switch (max_constellation) {
-	case QAM_64:
-		ana_gain = 0x7;	// -1 : avoid def_est saturation when ADC target is -16dB
-		coeff[0] = 0x0148;	/* P_adp_regul_cnt 0.04 */
-		coeff[1] = 0xfff0;	/* P_adp_noise_cnt -0.002 */
-		coeff[2] = 0x00a4;	/* P_adp_regul_ext 0.02 */
-		coeff[3] = 0xfff8;	/* P_adp_noise_ext -0.001 */
-		//if (!state->cfg.hostbus_diversity) //if diversity, we should prehaps use the configuration of the max_constallation -1
-		break;
-	case QAM_16:
-		ana_gain = 0x7;	// -1 : avoid def_est saturation when ADC target is -16dB
-		coeff[0] = 0x023d;	/* P_adp_regul_cnt 0.07 */
-		coeff[1] = 0xffdf;	/* P_adp_noise_cnt -0.004 */
-		coeff[2] = 0x00a4;	/* P_adp_regul_ext 0.02 */
-		coeff[3] = 0xfff0;	/* P_adp_noise_ext -0.002 */
-		//if (!((state->cfg.hostbus_diversity) && (max_constellation == QAM_16)))
-		break;
-	default:
-		ana_gain = 0;	// 0 : goes along with ADC target at -22dB to keep good mobile performance and lock at sensitivity level
-		coeff[0] = 0x099a;	/* P_adp_regul_cnt 0.3 */
-		coeff[1] = 0xffae;	/* P_adp_noise_cnt -0.01 */
-		coeff[2] = 0x0333;	/* P_adp_regul_ext 0.1 */
-		coeff[3] = 0xfff8;	/* P_adp_noise_ext -0.002 */
-		break;
+	for (i = 0; i < 3; i++)
+		max_constellation = dib8000_set_layer(state, i, max_constellation);
+	if (autosearching == 0) {
+		state->layer_b_nb_seg = c->layer[1].segment_count;
+		state->layer_c_nb_seg = c->layer[2].segment_count;
 	}
-	for (mode = 0; mode < 4; mode++)
-		dib8000_write_word(state, 215 + mode, coeff[mode]);
 
-	// update ana_gain depending on max constellation
-	dib8000_write_word(state, 116, ana_gain);
-	// update ADC target depending on ana_gain
-	if (ana_gain) {		// set -16dB ADC target for ana_gain=-1
-		for (i = 0; i < 10; i++)
-			dib8000_write_word(state, 80 + i, adc_target_16dB[i]);
-	} else {		// set -22dB ADC target for ana_gain=0
-		for (i = 0; i < 10; i++)
-			dib8000_write_word(state, 80 + i, adc_target_16dB[i] - 355);
+	/* WRITE: Mode & Diff mask */
+	dib8000_write_word(state, 0, (state->mode << 13) | state->seg_diff_mask);
+
+	state->differential_constellation = (state->seg_diff_mask != 0);
+
+	/* channel estimation fine configuration */
+	ana_gain = dib8000_adp_fine_tune(state, max_constellation);
+
+	/* update ana_gain depending on max constellation */
+	dib8000_update_ana_gain(state, ana_gain);
+
+	/* ---- ANA_FE ---- */
+	if (c->isdbt_partial_reception) /* 3-segments */
+		dib8000_load_ana_fe_coefs(state, ana_fe_coeff_3seg);
+	else
+		dib8000_load_ana_fe_coefs(state, ana_fe_coeff_1seg); /* 1-segment */
+
+	/* TSB or ISDBT ? apply it now */
+	if (c->isdbt_sb_mode) {
+		dib8000_set_sb_channel(state);
+		if (c->isdbt_sb_subchannel < 14)
+			init_prbs = dib8000_get_init_prbs(state, c->isdbt_sb_subchannel);
+		else
+			init_prbs = 0;
+	} else {
+		dib8000_set_13seg_channel(state);
+		init_prbs = 0xfff;
 	}
 
-	// ---- ANA_FE ----
-	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode) {
-		if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 1)
-			ana_fe = ana_fe_coeff_3seg;
-		else		// 1-segment
-			ana_fe = ana_fe_coeff_1seg;
-	} else
-		ana_fe = ana_fe_coeff_13seg;
+	/* SMALL */
+	dib8000_small_fine_tune(state);
 
-	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1 || state->isdbt_cfg_loaded == 0)
-		for (mode = 0; mode < 24; mode++)
-			dib8000_write_word(state, 117 + mode, ana_fe[mode]);
+	dib8000_set_subchannel_prbs(state, init_prbs);
 
-	// ---- CHAN_BLK ----
+	/* ---- CHAN_BLK ---- */
 	for (i = 0; i < 13; i++) {
-		if ((((~seg_diff_mask) >> i) & 1) == 1) {
-			P_cfr_left_edge += (1 << i) * ((i == 0) || ((((seg_mask13 & (~seg_diff_mask)) >> (i - 1)) & 1) == 0));
-			P_cfr_right_edge += (1 << i) * ((i == 12) || ((((seg_mask13 & (~seg_diff_mask)) >> (i + 1)) & 1) == 0));
+		if ((((~state->seg_diff_mask) >> i) & 1) == 1) {
+			p_cfr_left_edge  += (1 << i) * ((i == 0) || ((((state->seg_mask & (~state->seg_diff_mask)) >> (i - 1)) & 1) == 0));
+			p_cfr_right_edge += (1 << i) * ((i == 12) || ((((state->seg_mask & (~state->seg_diff_mask)) >> (i + 1)) & 1) == 0));
 		}
 	}
-	dib8000_write_word(state, 222, P_cfr_left_edge);	// P_cfr_left_edge
-	dib8000_write_word(state, 223, P_cfr_right_edge);	// P_cfr_right_edge
-	// "P_cspu_left_edge"  not used => do not care
-	// "P_cspu_right_edge" not used => do not care
-
-	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1) {
-		dib8000_write_word(state, 228, 1);	// P_2d_mode_byp=1
-		dib8000_write_word(state, 205, dib8000_read_word(state, 205) & 0xfff0);	// P_cspu_win_cut = 0
-		if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0
-			&& state->fe[0]->dtv_property_cache.transmission_mode == TRANSMISSION_MODE_2K) {
-			//dib8000_write_word(state, 219, dib8000_read_word(state, 219) & 0xfffe); // P_adp_pass = 0
-			dib8000_write_word(state, 265, 15);	// P_equal_noise_sel = 15
-		}
-	} else if (state->isdbt_cfg_loaded == 0) {
-		dib8000_write_word(state, 228, 0);	// default value
-		dib8000_write_word(state, 265, 31);	// default value
-		dib8000_write_word(state, 205, 0x200f);	// init value
-	}
-	// ---- TMCC ----
+	dib8000_write_word(state, 222, p_cfr_left_edge); /* p_cfr_left_edge */
+	dib8000_write_word(state, 223, p_cfr_right_edge); /* p_cfr_right_edge */
+	/* "P_cspu_left_edge" & "P_cspu_right_edge" not used => do not care */
+
+	dib8000_write_word(state, 189, ~state->seg_mask | state->seg_diff_mask); /* P_lmod4_seg_inh */
+	dib8000_write_word(state, 192, ~state->seg_mask | state->seg_diff_mask); /* P_pha3_seg_inh */
+	dib8000_write_word(state, 225, ~state->seg_mask | state->seg_diff_mask); /* P_tac_seg_inh */
+
+	if (!autosearching)
+		dib8000_write_word(state, 288, (~state->seg_mask | state->seg_diff_mask) & 0x1fff); /* P_tmcc_seg_eq_inh */
+	else
+		dib8000_write_word(state, 288, 0x1fff); /*disable equalisation of the tmcc when autosearch to be able to find the DQPSK channels. */
+
+	dib8000_write_word(state, 211, state->seg_mask & (~state->seg_diff_mask)); /* P_des_seg_enabled */
+	dib8000_write_word(state, 287, ~state->seg_mask | 0x1000); /* P_tmcc_seg_inh */
+
+	dib8000_write_word(state, 178, 32); /* P_fft_powrange = 32 */
+
+	/* ---- TMCC ---- */
 	for (i = 0; i < 3; i++)
-		tmcc_pow +=
-			(((state->fe[0]->dtv_property_cache.layer[i].modulation == DQPSK) * 4 + 1) * state->fe[0]->dtv_property_cache.layer[i].segment_count);
-	// Quantif of "P_tmcc_dec_thres_?k" is (0, 5+mode, 9);
-	// Threshold is set at 1/4 of max power.
-	tmcc_pow *= (1 << (9 - 2));
-
-	dib8000_write_word(state, 290, tmcc_pow);	// P_tmcc_dec_thres_2k
-	dib8000_write_word(state, 291, tmcc_pow);	// P_tmcc_dec_thres_4k
-	dib8000_write_word(state, 292, tmcc_pow);	// P_tmcc_dec_thres_8k
-	//dib8000_write_word(state, 287, (1 << 13) | 0x1000 );
-	// ---- PHA3 ----
+		tmcc_pow += (((c->layer[i].modulation == DQPSK) * 4 + 1) * c->layer[i].segment_count) ;
+
+	/* Quantif of "P_tmcc_dec_thres_?k" is (0, 5+mode, 9); */
+	/* Threshold is set at 1/4 of max power. */
+	tmcc_pow *= (1 << (9-2));
+	dib8000_write_word(state, 290, tmcc_pow); /* P_tmcc_dec_thres_2k */
+	dib8000_write_word(state, 291, tmcc_pow); /* P_tmcc_dec_thres_4k */
+	dib8000_write_word(state, 292, tmcc_pow); /* P_tmcc_dec_thres_8k */
+	/*dib8000_write_word(state, 287, (1 << 13) | 0x1000 ); */
 
+	/* ---- PHA3 ---- */
 	if (state->isdbt_cfg_loaded == 0)
-		dib8000_write_word(state, 250, 3285);	/*p_2d_hspeed_thr0 */
+		dib8000_write_word(state, 250, 3285); /* p_2d_hspeed_thr0 */
 
-	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1)
-		state->isdbt_cfg_loaded = 0;
-	else
-		state->isdbt_cfg_loaded = 1;
+	state->isdbt_cfg_loaded = 0;
+}
 
+static u32 dib8000_wait_lock(struct dib8000_state *state, u32 internal,
+			     u32 wait0_ms, u32 wait1_ms, u32 wait2_ms)
+{
+	u32 value = 0;	/* P_search_end0 wait time */
+	u16 reg = 11;	/* P_search_end0 start addr */
+
+	for (reg = 11; reg < 16; reg += 2) {
+		if (reg == 11) {
+			if (state->revision == 0x8090)
+				value = internal * wait1_ms;
+			else
+				value = internal * wait0_ms;
+		} else if (reg == 13)
+			value = internal * wait1_ms;
+		else if (reg == 15)
+			value = internal * wait2_ms;
+		dib8000_write_word(state, reg, (u16)((value >> 16) & 0xffff));
+		dib8000_write_word(state, (reg + 1), (u16)(value & 0xffff));
+	}
+	return value;
 }
 
 static int dib8000_autosearch_start(struct dvb_frontend *fe)
 {
-	u8 factor;
-	u32 value;
 	struct dib8000_state *state = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &state->fe[0]->dtv_property_cache;
+	u8 slist = 0;
+	u32 value, internal = state->cfg.pll->internal;
 
-	int slist = 0;
-
-	state->fe[0]->dtv_property_cache.inversion = 0;
-	if (!state->fe[0]->dtv_property_cache.isdbt_sb_mode)
-		state->fe[0]->dtv_property_cache.layer[0].segment_count = 13;
-	state->fe[0]->dtv_property_cache.layer[0].modulation = QAM_64;
-	state->fe[0]->dtv_property_cache.layer[0].fec = FEC_2_3;
-	state->fe[0]->dtv_property_cache.layer[0].interleaving = 0;
-
-	//choose the right list, in sb, always do everything
-	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode) {
-		state->fe[0]->dtv_property_cache.transmission_mode = TRANSMISSION_MODE_8K;
-		state->fe[0]->dtv_property_cache.guard_interval = GUARD_INTERVAL_1_8;
-		slist = 7;
-		dib8000_write_word(state, 0, (dib8000_read_word(state, 0) & 0x9fff) | (1 << 13));
-	} else {
-		if (state->fe[0]->dtv_property_cache.guard_interval == GUARD_INTERVAL_AUTO) {
-			if (state->fe[0]->dtv_property_cache.transmission_mode == TRANSMISSION_MODE_AUTO) {
-				slist = 7;
-				dib8000_write_word(state, 0, (dib8000_read_word(state, 0) & 0x9fff) | (1 << 13));	// P_mode = 1 to have autosearch start ok with mode2
-			} else
-				slist = 3;
-		} else {
-			if (state->fe[0]->dtv_property_cache.transmission_mode == TRANSMISSION_MODE_AUTO) {
-				slist = 2;
-				dib8000_write_word(state, 0, (dib8000_read_word(state, 0) & 0x9fff) | (1 << 13));	// P_mode = 1
-			} else
-				slist = 0;
-		}
+	if (state->revision == 0x8090)
+		internal = dib8000_read32(state, 23) / 1000;
+
+	if (state->autosearch_state == AS_SEARCHING_FFT) {
+		dib8000_write_word(state,  37, 0x0065); /* P_ctrl_pha_off_max default values */
+		dib8000_write_word(state, 116, 0x0000); /* P_ana_gain to 0 */
 
-		if (state->fe[0]->dtv_property_cache.transmission_mode == TRANSMISSION_MODE_AUTO)
-			state->fe[0]->dtv_property_cache.transmission_mode = TRANSMISSION_MODE_8K;
-		if (state->fe[0]->dtv_property_cache.guard_interval == GUARD_INTERVAL_AUTO)
-			state->fe[0]->dtv_property_cache.guard_interval = GUARD_INTERVAL_1_8;
+		dib8000_write_word(state, 0, (dib8000_read_word(state, 0) & 0x1fff) | (0 << 13) | (1 << 15)); /* P_mode = 0, P_restart_search=1 */
+		dib8000_write_word(state, 1, (dib8000_read_word(state, 1) & 0xfffc) | 0); /* P_guard = 0 */
+		dib8000_write_word(state, 6, 0); /* P_lock0_mask = 0 */
+		dib8000_write_word(state, 7, 0); /* P_lock1_mask = 0 */
+		dib8000_write_word(state, 8, 0); /* P_lock2_mask = 0 */
+		dib8000_write_word(state, 10, (dib8000_read_word(state, 10) & 0x200) | (16 << 4) | (0 << 0)); /* P_search_list=16, P_search_maxtrial=0 */
 
-		dprintk("using list for autosearch : %d", slist);
-		dib8000_set_channel(state, (unsigned char)slist, 1);
-		//dib8000_write_word(state, 0, (dib8000_read_word(state, 0) & 0x9fff) | (1 << 13));  // P_mode = 1
+		if (state->revision == 0x8090)
+			value = dib8000_wait_lock(state, internal, 10, 10, 10); /* time in ms configure P_search_end0 P_search_end1 P_search_end2 */
+		else
+			value = dib8000_wait_lock(state, internal, 20, 20, 20); /* time in ms configure P_search_end0 P_search_end1 P_search_end2 */
 
-		factor = 1;
+		dib8000_write_word(state, 17, 0);
+		dib8000_write_word(state, 18, 200); /* P_search_rstst = 200 */
+		dib8000_write_word(state, 19, 0);
+		dib8000_write_word(state, 20, 400); /* P_search_rstend = 400 */
+		dib8000_write_word(state, 21, (value >> 16) & 0xffff); /* P_search_checkst */
+		dib8000_write_word(state, 22, value & 0xffff);
 
-		//set lock_mask values
+		if (state->revision == 0x8090)
+			dib8000_write_word(state, 32, (dib8000_read_word(state, 32) & 0xf0ff) | (0 << 8)); /* P_corm_alpha = 0 */
+		else
+			dib8000_write_word(state, 32, (dib8000_read_word(state, 32) & 0xf0ff) | (9 << 8)); /* P_corm_alpha = 3 */
+		dib8000_write_word(state, 355, 2); /* P_search_param_max = 2 */
+
+		/* P_search_param_select = (1 | 1<<4 | 1 << 8) */
+		dib8000_write_word(state, 356, 0);
+		dib8000_write_word(state, 357, 0x111);
+
+		dib8000_write_word(state, 770, (dib8000_read_word(state, 770) & 0xdfff) | (1 << 13)); /* P_restart_ccg = 1 */
+		dib8000_write_word(state, 770, (dib8000_read_word(state, 770) & 0xdfff) | (0 << 13)); /* P_restart_ccg = 0 */
+		dib8000_write_word(state, 0, (dib8000_read_word(state, 0) & 0x7ff) | (0 << 15) | (1 << 13)); /* P_restart_search = 0; */
+	} else if (state->autosearch_state == AS_SEARCHING_GUARD) {
+		c->transmission_mode = TRANSMISSION_MODE_8K;
+		c->guard_interval = GUARD_INTERVAL_1_8;
+		c->inversion = 0;
+		c->layer[0].modulation = QAM_64;
+		c->layer[0].fec = FEC_2_3;
+		c->layer[0].interleaving = 0;
+		c->layer[0].segment_count = 13;
+
+		slist = 16;
+		c->transmission_mode = state->found_nfft;
+
+		dib8000_set_isdbt_common_channel(state, slist, 1);
+
+		/* set lock_mask values */
 		dib8000_write_word(state, 6, 0x4);
-		dib8000_write_word(state, 7, 0x8);
+		if (state->revision == 0x8090)
+			dib8000_write_word(state, 7, ((1 << 12) | (1 << 11) | (1 << 10)));/* tmcc_dec_lock, tmcc_sync_lock, tmcc_data_lock, tmcc_bch_uncor */
+		else
+			dib8000_write_word(state, 7, 0x8);
 		dib8000_write_word(state, 8, 0x1000);
 
-		//set lock_mask wait time values
-		value = 50 * state->cfg.pll->internal * factor;
-		dib8000_write_word(state, 11, (u16) ((value >> 16) & 0xffff));	// lock0 wait time
-		dib8000_write_word(state, 12, (u16) (value & 0xffff));	// lock0 wait time
-		value = 100 * state->cfg.pll->internal * factor;
-		dib8000_write_word(state, 13, (u16) ((value >> 16) & 0xffff));	// lock1 wait time
-		dib8000_write_word(state, 14, (u16) (value & 0xffff));	// lock1 wait time
-		value = 1000 * state->cfg.pll->internal * factor;
-		dib8000_write_word(state, 15, (u16) ((value >> 16) & 0xffff));	// lock2 wait time
-		dib8000_write_word(state, 16, (u16) (value & 0xffff));	// lock2 wait time
+		/* set lock_mask wait time values */
+		if (state->revision == 0x8090)
+			dib8000_wait_lock(state, internal, 50, 100, 1000); /* time in ms configure P_search_end0 P_search_end1 P_search_end2 */
+		else
+			dib8000_wait_lock(state, internal, 50, 200, 1000); /* time in ms configure P_search_end0 P_search_end1 P_search_end2 */
+
+		dib8000_write_word(state, 355, 3); /* P_search_param_max = 3 */
+
+		/* P_search_param_select = 0xf; look for the 4 different guard intervals */
+		dib8000_write_word(state, 356, 0);
+		dib8000_write_word(state, 357, 0xf);
 
 		value = dib8000_read_word(state, 0);
-		dib8000_write_word(state, 0, (u16) ((1 << 15) | value));
-		dib8000_read_word(state, 1284);	// reset the INT. n_irq_pending
-		dib8000_write_word(state, 0, (u16) value);
+		dib8000_write_word(state, 0, (u16)((1 << 15) | value));
+		dib8000_read_word(state, 1284);  /* reset the INT. n_irq_pending */
+		dib8000_write_word(state, 0, (u16)value);
+	} else {
+		c->inversion = 0;
+		c->layer[0].modulation = QAM_64;
+		c->layer[0].fec = FEC_2_3;
+		c->layer[0].interleaving = 0;
+		c->layer[0].segment_count = 13;
+		if (!c->isdbt_sb_mode)
+			c->layer[0].segment_count = 13;
+
+		/* choose the right list, in sb, always do everything */
+		if (c->isdbt_sb_mode) {
+			slist = 7;
+			dib8000_write_word(state, 0, (dib8000_read_word(state, 0) & 0x9fff) | (1 << 13));
+		} else {
+			if (c->guard_interval == GUARD_INTERVAL_AUTO) {
+				if (c->transmission_mode == TRANSMISSION_MODE_AUTO) {
+					c->transmission_mode = TRANSMISSION_MODE_8K;
+					c->guard_interval = GUARD_INTERVAL_1_8;
+					slist = 7;
+					dib8000_write_word(state, 0, (dib8000_read_word(state, 0) & 0x9fff) | (1 << 13));  /* P_mode = 1 to have autosearch start ok with mode2 */
+				} else {
+					c->guard_interval = GUARD_INTERVAL_1_8;
+					slist = 3;
+				}
+			} else {
+				if (c->transmission_mode == TRANSMISSION_MODE_AUTO) {
+					c->transmission_mode = TRANSMISSION_MODE_8K;
+					slist = 2;
+					dib8000_write_word(state, 0, (dib8000_read_word(state, 0) & 0x9fff) | (1 << 13));  /* P_mode = 1 */
+				} else
+					slist = 0;
+			}
+		}
+		dprintk("Using list for autosearch : %d", slist);
 
-	}
+		dib8000_set_isdbt_common_channel(state, slist, 1);
+
+		/* set lock_mask values */
+		dib8000_write_word(state, 6, 0x4);
+		if (state->revision == 0x8090)
+			dib8000_write_word(state, 7, (1 << 12) | (1 << 11) | (1 << 10));
+		else
+			dib8000_write_word(state, 7, 0x8);
+		dib8000_write_word(state, 8, 0x1000);
 
+		/* set lock_mask wait time values */
+		if (state->revision == 0x8090)
+			dib8000_wait_lock(state, internal, 50, 200, 1000); /* time in ms configure P_search_end0 P_search_end1 P_search_end2 */
+		else
+			dib8000_wait_lock(state, internal, 50, 100, 1000); /* time in ms configure P_search_end0 P_search_end1 P_search_end2 */
+
+		value = dib8000_read_word(state, 0);
+		dib8000_write_word(state, 0, (u16)((1 << 15) | value));
+		dib8000_read_word(state, 1284);  /* reset the INT. n_irq_pending */
+		dib8000_write_word(state, 0, (u16)value);
+	}
 	return 0;
 }
 
@@ -2663,96 +2583,635 @@ static int dib8000_autosearch_irq(struct dvb_frontend *fe)
 	struct dib8000_state *state = fe->demodulator_priv;
 	u16 irq_pending = dib8000_read_word(state, 1284);
 
-	if (irq_pending & 0x1) {	// failed
-		dprintk("dib8000_autosearch_irq failed");
-		return 1;
-	}
+	if (state->autosearch_state == AS_SEARCHING_FFT) {
+		if (irq_pending & 0x1) {
+			dprintk("dib8000_autosearch_irq: max correlation result available");
+			return 3;
+		}
+	} else {
+		if (irq_pending & 0x1) {	/* failed */
+			dprintk("dib8000_autosearch_irq failed");
+			return 1;
+		}
 
-	if (irq_pending & 0x2) {	// succeeded
-		dprintk("dib8000_autosearch_irq succeeded");
-		return 2;
+		if (irq_pending & 0x2) {	/* succeeded */
+			dprintk("dib8000_autosearch_irq succeeded");
+			return 2;
+		}
 	}
 
 	return 0;		// still pending
 }
 
-static int dib8000_tune(struct dvb_frontend *fe)
+static void dib8000_viterbi_state(struct dib8000_state *state, u8 onoff)
 {
-	struct dib8000_state *state = fe->demodulator_priv;
-	int ret = 0;
-	u16 lock, value, mode;
+	u16 tmp;
 
-	// we are already tuned - just resuming from suspend
-	if (state == NULL)
-		return -EINVAL;
+	tmp = dib8000_read_word(state, 771);
+	if (onoff) /* start P_restart_chd : channel_decoder */
+		dib8000_write_word(state, 771, tmp & 0xfffd);
+	else /* stop P_restart_chd : channel_decoder */
+		dib8000_write_word(state, 771, tmp | (1<<1));
+}
+
+static void dib8000_set_dds(struct dib8000_state *state, s32 offset_khz)
+{
+	s16 unit_khz_dds_val;
+	u32 abs_offset_khz = ABS(offset_khz);
+	u32 dds = state->cfg.pll->ifreq & 0x1ffffff;
+	u8 invert = !!(state->cfg.pll->ifreq & (1 << 25));
+	u8 ratio;
 
-	mode = fft_to_mode(state);
+	if (state->revision == 0x8090) {
+		ratio = 4;
+		unit_khz_dds_val = (1<<26) / (dib8000_read32(state, 23) / 1000);
+		if (offset_khz < 0)
+			dds = (1 << 26) - (abs_offset_khz * unit_khz_dds_val);
+		else
+			dds = (abs_offset_khz * unit_khz_dds_val);
 
-	dib8000_set_bandwidth(fe, state->fe[0]->dtv_property_cache.bandwidth_hz / 1000);
-	dib8000_set_channel(state, 0, 0);
+		if (invert)
+			dds = (1<<26) - dds;
+	} else {
+		ratio = 2;
+		unit_khz_dds_val = (u16) (67108864 / state->cfg.pll->internal);
 
-	// restart demod
-	ret |= dib8000_write_word(state, 770, 0x4000);
-	ret |= dib8000_write_word(state, 770, 0x0000);
-	msleep(45);
+		if (offset_khz < 0)
+			unit_khz_dds_val *= -1;
 
-	/* P_ctrl_inh_cor=0, P_ctrl_alpha_cor=4, P_ctrl_inh_isi=0, P_ctrl_alpha_isi=3 */
-	/*  ret |= dib8000_write_word(state, 29, (0 << 9) | (4 << 5) | (0 << 4) | (3 << 0) );  workaround inh_isi stays at 1 */
+		/* IF tuner */
+		if (invert)
+			dds -= abs_offset_khz * unit_khz_dds_val;
+		else
+			dds += abs_offset_khz * unit_khz_dds_val;
+	}
 
-	// never achieved a lock before - wait for timfreq to update
-	if (state->timf == 0) {
-		if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1) {
-			if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0)
-				msleep(300);
-			else	// Sound Broadcasting mode 3 seg
-				msleep(500);
-		} else		// 13 seg
-			msleep(200);
+	dprintk("setting a DDS frequency offset of %c%dkHz", invert ? '-' : ' ', dds / unit_khz_dds_val);
+
+	if (abs_offset_khz <= (state->cfg.pll->internal / ratio)) {
+		/* Max dds offset is the half of the demod freq */
+		dib8000_write_word(state, 26, invert);
+		dib8000_write_word(state, 27, (u16)(dds >> 16) & 0x1ff);
+		dib8000_write_word(state, 28, (u16)(dds & 0xffff));
 	}
-	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1) {
-		if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0) {
+}
 
-			/* P_timf_alpha = (13-P_mode) , P_corm_alpha=6, P_corm_thres=0x40  alpha to check on board */
-			dib8000_write_word(state, 32, ((13 - mode) << 12) | (6 << 8) | 0x40);
-			//dib8000_write_word(state, 32, (8 << 12) | (6 << 8) | 0x80);
+static void dib8000_set_frequency_offset(struct dib8000_state *state)
+{
+	struct dtv_frontend_properties *c = &state->fe[0]->dtv_property_cache;
+	int i;
+	u32 current_rf;
+	int total_dds_offset_khz;
 
-			/*  P_ctrl_sfreq_step= (12-P_mode)   P_ctrl_sfreq_inh =0     P_ctrl_pha_off_max  */
-			ret |= dib8000_write_word(state, 37, (12 - mode) | ((5 + mode) << 5));
+	if (state->fe[0]->ops.tuner_ops.get_frequency)
+		state->fe[0]->ops.tuner_ops.get_frequency(state->fe[0], &current_rf);
+	else
+		current_rf = c->frequency;
+	current_rf /= 1000;
+	total_dds_offset_khz = (int)current_rf - (int)c->frequency / 1000;
 
-		} else {	// Sound Broadcasting mode 3 seg
+	if (c->isdbt_sb_mode) {
+		state->subchannel = c->isdbt_sb_subchannel;
 
-			/* P_timf_alpha = (12-P_mode) , P_corm_alpha=6, P_corm_thres=0x60  alpha to check on board */
-			dib8000_write_word(state, 32, ((12 - mode) << 12) | (6 << 8) | 0x60);
+		i = dib8000_read_word(state, 26) & 1; /* P_dds_invspec */
+		dib8000_write_word(state, 26, c->inversion ^ i);
 
-			ret |= dib8000_write_word(state, 37, (11 - mode) | ((5 + mode) << 5));
+		if (state->cfg.pll->ifreq == 0) { /* low if tuner */
+			if ((c->inversion ^ i) == 0)
+				dib8000_write_word(state, 26, dib8000_read_word(state, 26) | 1);
+		} else {
+			if ((c->inversion ^ i) == 0)
+				total_dds_offset_khz *= -1;
 		}
+	}
 
-	} else {		// 13 seg
-		/* P_timf_alpha = 8 , P_corm_alpha=6, P_corm_thres=0x80  alpha to check on board */
-		dib8000_write_word(state, 32, ((11 - mode) << 12) | (6 << 8) | 0x80);
+	dprintk("%dkhz tuner offset (frequency = %dHz & current_rf = %dHz) total_dds_offset_hz = %d", c->frequency - current_rf, c->frequency, current_rf, total_dds_offset_khz);
 
-		ret |= dib8000_write_word(state, 37, (10 - mode) | ((5 + mode) << 5));
+	/* apply dds offset now */
+	dib8000_set_dds(state, total_dds_offset_khz);
+}
+
+static u16 LUT_isdbt_symbol_duration[4] = { 26, 101, 63 };
+
+static u32 dib8000_get_symbol_duration(struct dib8000_state *state)
+{
+	struct dtv_frontend_properties *c = &state->fe[0]->dtv_property_cache;
+	u16 i;
 
+	switch (c->transmission_mode) {
+	case TRANSMISSION_MODE_2K:
+			i = 0;
+			break;
+	case TRANSMISSION_MODE_4K:
+			i = 2;
+			break;
+	default:
+	case TRANSMISSION_MODE_AUTO:
+	case TRANSMISSION_MODE_8K:
+			i = 1;
+			break;
 	}
 
-	// we achieved a coff_cpil_lock - it's time to update the timf
-	if (state->revision != 0x8090)
-		lock = dib8000_read_word(state, 568);
+	return (LUT_isdbt_symbol_duration[i] / (c->bandwidth_hz / 1000)) + 1;
+}
+
+static void dib8000_set_isdbt_loop_params(struct dib8000_state *state, enum param_loop_step loop_step)
+{
+	struct dtv_frontend_properties *c = &state->fe[0]->dtv_property_cache;
+	u16 reg_32 = 0, reg_37 = 0;
+
+	switch (loop_step) {
+	case LOOP_TUNE_1:
+			if (c->isdbt_sb_mode)  {
+				if (c->isdbt_partial_reception == 0) {
+					reg_32 = ((11 - state->mode) << 12) | (6 << 8) | 0x40; /* P_timf_alpha = (11-P_mode), P_corm_alpha=6, P_corm_thres=0x40 */
+					reg_37 = (3 << 5) | (0 << 4) | (10 - state->mode); /* P_ctrl_pha_off_max=3   P_ctrl_sfreq_inh =0  P_ctrl_sfreq_step = (10-P_mode)  */
+				} else { /* Sound Broadcasting mode 3 seg */
+					reg_32 = ((10 - state->mode) << 12) | (6 << 8) | 0x60; /* P_timf_alpha = (10-P_mode), P_corm_alpha=6, P_corm_thres=0x60 */
+					reg_37 = (3 << 5) | (0 << 4) | (9 - state->mode); /* P_ctrl_pha_off_max=3   P_ctrl_sfreq_inh =0  P_ctrl_sfreq_step = (9-P_mode)  */
+				}
+			} else { /* 13-seg start conf offset loop parameters */
+				reg_32 = ((9 - state->mode) << 12) | (6 << 8) | 0x80; /* P_timf_alpha = (9-P_mode, P_corm_alpha=6, P_corm_thres=0x80 */
+				reg_37 = (3 << 5) | (0 << 4) | (8 - state->mode); /* P_ctrl_pha_off_max=3   P_ctrl_sfreq_inh =0  P_ctrl_sfreq_step = 9  */
+			}
+			break;
+	case LOOP_TUNE_2:
+			if (c->isdbt_sb_mode)  {
+				if (c->isdbt_partial_reception == 0) {  /* Sound Broadcasting mode 1 seg */
+					reg_32 = ((13-state->mode) << 12) | (6 << 8) | 0x40; /* P_timf_alpha = (13-P_mode) , P_corm_alpha=6, P_corm_thres=0x40*/
+					reg_37 = (12-state->mode) | ((5 + state->mode) << 5);
+				} else {  /* Sound Broadcasting mode 3 seg */
+					reg_32 = ((12-state->mode) << 12) | (6 << 8) | 0x60; /* P_timf_alpha = (12-P_mode) , P_corm_alpha=6, P_corm_thres=0x60 */
+					reg_37 = (11-state->mode) | ((5 + state->mode) << 5);
+				}
+			} else {  /* 13 seg */
+				reg_32 = ((11-state->mode) << 12) | (6 << 8) | 0x80; /* P_timf_alpha = 8 , P_corm_alpha=6, P_corm_thres=0x80 */
+				reg_37 = ((5+state->mode) << 5) | (10 - state->mode);
+			}
+			break;
+	}
+	dib8000_write_word(state, 32, reg_32);
+	dib8000_write_word(state, 37, reg_37);
+}
+
+static void dib8000_demod_restart(struct dib8000_state *state)
+{
+	dib8000_write_word(state, 770, 0x4000);
+	dib8000_write_word(state, 770, 0x0000);
+	return;
+}
+
+static void dib8000_set_sync_wait(struct dib8000_state *state)
+{
+	struct dtv_frontend_properties *c = &state->fe[0]->dtv_property_cache;
+	u16 sync_wait = 64;
+
+	/* P_dvsy_sync_wait - reuse mode */
+	switch (c->transmission_mode) {
+	case TRANSMISSION_MODE_8K:
+			sync_wait = 256;
+			break;
+	case TRANSMISSION_MODE_4K:
+			sync_wait = 128;
+			break;
+	default:
+	case TRANSMISSION_MODE_2K:
+			sync_wait =  64;
+			break;
+	}
+
+	if (state->cfg.diversity_delay == 0)
+		sync_wait = (sync_wait * (1 << (c->guard_interval)) * 3) / 2 + 48; /* add 50% SFN margin + compensate for one DVSY-fifo */
 	else
-		lock = dib8000_read_word(state, 570);
-	if ((lock >> 11) & 0x1)
-		dib8000_update_timf(state);
+		sync_wait = (sync_wait * (1 << (c->guard_interval)) * 3) / 2 + state->cfg.diversity_delay; /* add 50% SFN margin + compensate for DVSY-fifo */
 
-	//now that tune is finished, lock0 should lock on fec_mpeg to output this lock on MP_LOCK. It's changed in autosearch start
-	dib8000_write_word(state, 6, 0x200);
+	dib8000_write_word(state, 273, (dib8000_read_word(state, 273) & 0x000f) | (sync_wait << 4));
+}
 
-	if (state->revision == 0x8002) {
-		value = dib8000_read_word(state, 903);
-		dib8000_write_word(state, 903, value & ~(1 << 3));
-		msleep(1);
-		dib8000_write_word(state, 903, value | (1 << 3));
+static u32 dib8000_get_timeout(struct dib8000_state *state, u32 delay, enum timeout_mode mode)
+{
+	if (mode == SYMBOL_DEPENDENT_ON)
+		return systime() + (delay * state->symbol_duration);
+	else
+		return systime() + delay;
+}
+
+static s32 dib8000_get_status(struct dvb_frontend *fe)
+{
+	struct dib8000_state *state = fe->demodulator_priv;
+	return state->status;
+}
+
+enum frontend_tune_state dib8000_get_tune_state(struct dvb_frontend *fe)
+{
+	struct dib8000_state *state = fe->demodulator_priv;
+	return state->tune_state;
+}
+EXPORT_SYMBOL(dib8000_get_tune_state);
+
+int dib8000_set_tune_state(struct dvb_frontend *fe, enum frontend_tune_state tune_state)
+{
+	struct dib8000_state *state = fe->demodulator_priv;
+
+	state->tune_state = tune_state;
+	return 0;
+}
+EXPORT_SYMBOL(dib8000_set_tune_state);
+
+static int dib8000_tune_restart_from_demod(struct dvb_frontend *fe)
+{
+	struct dib8000_state *state = fe->demodulator_priv;
+
+	state->status = FE_STATUS_TUNE_PENDING;
+	state->tune_state = CT_DEMOD_START;
+	return 0;
+}
+
+static u16 dib8000_read_lock(struct dvb_frontend *fe)
+{
+	struct dib8000_state *state = fe->demodulator_priv;
+
+	if (state->revision == 0x8090)
+		return dib8000_read_word(state, 570);
+	return dib8000_read_word(state, 568);
+}
+
+static int dib8090p_init_sdram(struct dib8000_state *state)
+{
+	u16 reg = 0;
+	dprintk("init sdram");
+
+	reg = dib8000_read_word(state, 274) & 0xfff0;
+	dib8000_write_word(state, 274, reg | 0x7); /* P_dintlv_delay_ram = 7 because of MobileSdram */
+
+	dib8000_write_word(state, 1803, (7 << 2));
+
+	reg = dib8000_read_word(state, 1280);
+	dib8000_write_word(state, 1280,  reg | (1 << 2)); /* force restart P_restart_sdram */
+	dib8000_write_word(state, 1280,  reg); /* release restart P_restart_sdram */
+
+	return 0;
+}
+
+static int dib8000_tune(struct dvb_frontend *fe)
+{
+	struct dib8000_state *state = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &state->fe[0]->dtv_property_cache;
+	enum frontend_tune_state *tune_state = &state->tune_state;
+
+	u16 locks, deeper_interleaver = 0, i;
+	int ret = 1; /* 1 symbol duration (in 100us unit) delay most of the time */
+
+	u32 *timeout = &state->timeout;
+	u32 now = systime();
+#ifdef DIB8000_AGC_FREEZE
+	u16 agc1, agc2;
+#endif
+
+	u32 corm[4] = {0, 0, 0, 0};
+	u8 find_index, max_value;
+
+#if 0
+	if (*tune_state < CT_DEMOD_STOP)
+		dprintk("IN: context status = %d, TUNE_STATE %d autosearch step = %u systime = %u", state->channel_parameters_set, *tune_state, state->autosearch_state, now);
+#endif
+
+	switch (*tune_state) {
+	case CT_DEMOD_START: /* 30 */
+			if (state->revision == 0x8090)
+				dib8090p_init_sdram(state);
+			state->status = FE_STATUS_TUNE_PENDING;
+			if ((c->delivery_system != SYS_ISDBT) ||
+					(c->inversion == INVERSION_AUTO) ||
+					(c->transmission_mode == TRANSMISSION_MODE_AUTO) ||
+					(c->guard_interval == GUARD_INTERVAL_AUTO) ||
+					(((c->isdbt_layer_enabled & (1 << 0)) != 0) &&
+					 (c->layer[0].segment_count != 0xff) &&
+					 (c->layer[0].segment_count != 0) &&
+					 ((c->layer[0].modulation == QAM_AUTO) ||
+					  (c->layer[0].fec == FEC_AUTO))) ||
+					(((c->isdbt_layer_enabled & (1 << 1)) != 0) &&
+					 (c->layer[1].segment_count != 0xff) &&
+					 (c->layer[1].segment_count != 0) &&
+					 ((c->layer[1].modulation == QAM_AUTO) ||
+					  (c->layer[1].fec == FEC_AUTO))) ||
+					(((c->isdbt_layer_enabled & (1 << 2)) != 0) &&
+					 (c->layer[2].segment_count != 0xff) &&
+					 (c->layer[2].segment_count != 0) &&
+					 ((c->layer[2].modulation == QAM_AUTO) ||
+					  (c->layer[2].fec == FEC_AUTO))) ||
+					(((c->layer[0].segment_count == 0) ||
+					  ((c->isdbt_layer_enabled & (1 << 0)) == 0)) &&
+					 ((c->layer[1].segment_count == 0) ||
+					  ((c->isdbt_layer_enabled & (2 << 0)) == 0)) &&
+					 ((c->layer[2].segment_count == 0) || ((c->isdbt_layer_enabled & (3 << 0)) == 0))))
+				state->channel_parameters_set = 0; /* auto search */
+			else
+				state->channel_parameters_set = 1; /* channel parameters are known */
+
+			dib8000_viterbi_state(state, 0); /* force chan dec in restart */
+
+			/* Layer monit */
+			dib8000_write_word(state, 285, dib8000_read_word(state, 285) & 0x60);
+
+			dib8000_set_frequency_offset(state);
+			dib8000_set_bandwidth(fe, c->bandwidth_hz / 1000);
+
+			if (state->channel_parameters_set == 0) { /* The channel struct is unknown, search it ! */
+#ifdef DIB8000_AGC_FREEZE
+				if (state->revision != 0x8090) {
+					state->agc1_max = dib8000_read_word(state, 108);
+					state->agc1_min = dib8000_read_word(state, 109);
+					state->agc2_max = dib8000_read_word(state, 110);
+					state->agc2_min = dib8000_read_word(state, 111);
+					agc1 = dib8000_read_word(state, 388);
+					agc2 = dib8000_read_word(state, 389);
+					dib8000_write_word(state, 108, agc1);
+					dib8000_write_word(state, 109, agc1);
+					dib8000_write_word(state, 110, agc2);
+					dib8000_write_word(state, 111, agc2);
+				}
+#endif
+				state->autosearch_state = AS_SEARCHING_FFT;
+				state->found_nfft = TRANSMISSION_MODE_AUTO;
+				state->found_guard = GUARD_INTERVAL_AUTO;
+				*tune_state = CT_DEMOD_SEARCH_NEXT;
+			} else { /* we already know the channel struct so TUNE only ! */
+				state->autosearch_state = AS_DONE;
+				*tune_state = CT_DEMOD_STEP_3;
+			}
+			state->symbol_duration = dib8000_get_symbol_duration(state);
+			break;
+
+	case CT_DEMOD_SEARCH_NEXT: /* 51 */
+			dib8000_autosearch_start(fe);
+			if (state->revision == 0x8090)
+				ret = 50;
+			else
+				ret = 15;
+			*tune_state = CT_DEMOD_STEP_1;
+			break;
+
+	case CT_DEMOD_STEP_1: /* 31 */
+			switch (dib8000_autosearch_irq(fe)) {
+			case 1: /* fail */
+					state->status = FE_STATUS_TUNE_FAILED;
+					state->autosearch_state = AS_DONE;
+					*tune_state = CT_DEMOD_STOP; /* else we are done here */
+					break;
+			case 2: /* Succes */
+					state->status = FE_STATUS_FFT_SUCCESS; /* signal to the upper layer, that there was a channel found and the parameters can be read */
+					*tune_state = CT_DEMOD_STEP_3;
+					if (state->autosearch_state == AS_SEARCHING_GUARD)
+						*tune_state = CT_DEMOD_STEP_2;
+					else
+						state->autosearch_state = AS_DONE;
+					break;
+			case 3: /* Autosearch FFT max correlation endded */
+					*tune_state = CT_DEMOD_STEP_2;
+					break;
+			}
+			break;
+
+	case CT_DEMOD_STEP_2:
+			switch (state->autosearch_state) {
+			case AS_SEARCHING_FFT:
+					/* searching for the correct FFT */
+				if (state->revision == 0x8090) {
+					corm[2] = (dib8000_read_word(state, 596) << 16) | (dib8000_read_word(state, 597));
+					corm[1] = (dib8000_read_word(state, 598) << 16) | (dib8000_read_word(state, 599));
+					corm[0] = (dib8000_read_word(state, 600) << 16) | (dib8000_read_word(state, 601));
+				} else {
+					corm[2] = (dib8000_read_word(state, 594) << 16) | (dib8000_read_word(state, 595));
+					corm[1] = (dib8000_read_word(state, 596) << 16) | (dib8000_read_word(state, 597));
+					corm[0] = (dib8000_read_word(state, 598) << 16) | (dib8000_read_word(state, 599));
+				}
+					/* dprintk("corm fft: %u %u %u", corm[0], corm[1], corm[2]); */
+
+					max_value = 0;
+					for (find_index = 1 ; find_index < 3 ; find_index++) {
+						if (corm[max_value] < corm[find_index])
+							max_value = find_index ;
+					}
+
+					switch (max_value) {
+					case 0:
+							state->found_nfft = TRANSMISSION_MODE_2K;
+							break;
+					case 1:
+							state->found_nfft = TRANSMISSION_MODE_4K;
+							break;
+					case 2:
+					default:
+							state->found_nfft = TRANSMISSION_MODE_8K;
+							break;
+					}
+					/* dprintk("Autosearch FFT has found Mode %d", max_value + 1); */
+
+					*tune_state = CT_DEMOD_SEARCH_NEXT;
+					state->autosearch_state = AS_SEARCHING_GUARD;
+					if (state->revision == 0x8090)
+						ret = 50;
+					else
+						ret = 10;
+					break;
+			case AS_SEARCHING_GUARD:
+					/* searching for the correct guard interval */
+					if (state->revision == 0x8090)
+						state->found_guard = dib8000_read_word(state, 572) & 0x3;
+					else
+						state->found_guard = dib8000_read_word(state, 570) & 0x3;
+					/* dprintk("guard interval found=%i", state->found_guard); */
+
+					*tune_state = CT_DEMOD_STEP_3;
+					break;
+			default:
+					/* the demod should never be in this state */
+					state->status = FE_STATUS_TUNE_FAILED;
+					state->autosearch_state = AS_DONE;
+					*tune_state = CT_DEMOD_STOP; /* else we are done here */
+					break;
+			}
+			break;
+
+	case CT_DEMOD_STEP_3: /* 33 */
+			state->symbol_duration = dib8000_get_symbol_duration(state);
+			dib8000_set_isdbt_loop_params(state, LOOP_TUNE_1);
+			dib8000_set_isdbt_common_channel(state, 0, 0);/* setting the known channel parameters here */
+			*tune_state = CT_DEMOD_STEP_4;
+			break;
+
+	case CT_DEMOD_STEP_4: /* (34) */
+			dib8000_demod_restart(state);
+
+			dib8000_set_sync_wait(state);
+			dib8000_set_diversity_in(state->fe[0], state->diversity_onoff);
+
+			locks = (dib8000_read_word(state, 180) >> 6) & 0x3f; /* P_coff_winlen ? */
+			/* coff should lock over P_coff_winlen ofdm symbols : give 3 times this lenght to lock */
+			*timeout = dib8000_get_timeout(state, 2 * locks, SYMBOL_DEPENDENT_ON);
+			*tune_state = CT_DEMOD_STEP_5;
+			break;
+
+	case CT_DEMOD_STEP_5: /* (35) */
+			locks = dib8000_read_lock(fe);
+			if (locks & (0x3 << 11)) { /* coff-lock and off_cpil_lock achieved */
+				dib8000_update_timf(state); /* we achieved a coff_cpil_lock - it's time to update the timf */
+				if (!state->differential_constellation) {
+					/* 2 times lmod4_win_len + 10 symbols (pipe delay after coff + nb to compute a 1st correlation) */
+					*timeout = dib8000_get_timeout(state, (20 * ((dib8000_read_word(state, 188)>>5)&0x1f)), SYMBOL_DEPENDENT_ON);
+					*tune_state = CT_DEMOD_STEP_7;
+				} else {
+					*tune_state = CT_DEMOD_STEP_8;
+				}
+			} else if (now > *timeout) {
+				*tune_state = CT_DEMOD_STEP_6; /* goto check for diversity input connection */
+			}
+			break;
+
+	case CT_DEMOD_STEP_6: /* (36)  if there is an input (diversity) */
+			if ((state->fe[1] != NULL) && (state->output_mode != OUTMODE_DIVERSITY)) {
+				/* if there is a diversity fe in input and this fe is has not already failled : wait here until this this fe has succedeed or failled */
+				if (dib8000_get_status(state->fe[1]) <= FE_STATUS_STD_SUCCESS) /* Something is locked on the input fe */
+					*tune_state = CT_DEMOD_STEP_8; /* go for mpeg */
+				else if (dib8000_get_status(state->fe[1]) >= FE_STATUS_TUNE_TIME_TOO_SHORT) { /* fe in input failled also, break the current one */
+					*tune_state = CT_DEMOD_STOP; /* else we are done here ; step 8 will close the loops and exit */
+					dib8000_viterbi_state(state, 1); /* start viterbi chandec */
+					dib8000_set_isdbt_loop_params(state, LOOP_TUNE_2);
+					state->status = FE_STATUS_TUNE_FAILED;
+				}
+			} else {
+				dib8000_viterbi_state(state, 1); /* start viterbi chandec */
+				dib8000_set_isdbt_loop_params(state, LOOP_TUNE_2);
+				*tune_state = CT_DEMOD_STOP; /* else we are done here ; step 8 will close the loops and exit */
+				state->status = FE_STATUS_TUNE_FAILED;
+			}
+			break;
+
+	case CT_DEMOD_STEP_7: /* 37 */
+			locks = dib8000_read_lock(fe);
+			if (locks & (1<<10)) { /* lmod4_lock */
+				ret = 14; /* wait for 14 symbols */
+				*tune_state = CT_DEMOD_STEP_8;
+			} else if (now > *timeout)
+				*tune_state = CT_DEMOD_STEP_6; /* goto check for diversity input connection */
+			break;
+
+	case CT_DEMOD_STEP_8: /* 38 */
+			dib8000_viterbi_state(state, 1); /* start viterbi chandec */
+			dib8000_set_isdbt_loop_params(state, LOOP_TUNE_2);
+
+			/* mpeg will never lock on this condition because init_prbs is not set : search for it !*/
+			if (c->isdbt_sb_mode
+			    && c->isdbt_sb_subchannel < 14
+			    && !state->differential_constellation) {
+				state->subchannel = 0;
+				*tune_state = CT_DEMOD_STEP_11;
+			} else {
+				*tune_state = CT_DEMOD_STEP_9;
+				state->status = FE_STATUS_LOCKED;
+			}
+			break;
+
+	case CT_DEMOD_STEP_9: /* 39 */
+			if ((state->revision == 0x8090) || ((dib8000_read_word(state, 1291) >> 9) & 0x1)) { /* fe capable of deinterleaving : esram */
+				/* defines timeout for mpeg lock depending on interleaver lenght of longest layer */
+				for (i = 0; i < 3; i++) {
+					if (c->layer[i].interleaving >= deeper_interleaver) {
+						dprintk("layer%i: time interleaver = %d ", i, c->layer[i].interleaving);
+						if (c->layer[i].segment_count > 0) { /* valid layer */
+							deeper_interleaver = c->layer[0].interleaving;
+							state->longest_intlv_layer = i;
+						}
+					}
+				}
+
+				if (deeper_interleaver == 0)
+					locks = 2; /* locks is the tmp local variable name */
+				else if (deeper_interleaver == 3)
+					locks = 8;
+				else
+					locks = 2 * deeper_interleaver;
+
+				if (state->diversity_onoff != 0) /* because of diversity sync */
+					locks *= 2;
+
+				*timeout = now + (2000 * locks); /* give the mpeg lock 800ms if sram is present */
+				dprintk("Deeper interleaver mode = %d on layer %d : timeout mult factor = %d => will use timeout = %d", deeper_interleaver, state->longest_intlv_layer, locks, *timeout);
+
+				*tune_state = CT_DEMOD_STEP_10;
+			} else
+				*tune_state = CT_DEMOD_STOP;
+			break;
+
+	case CT_DEMOD_STEP_10: /* 40 */
+			locks = dib8000_read_lock(fe);
+			if (locks&(1<<(7-state->longest_intlv_layer))) { /* mpeg lock : check the longest one */
+				dprintk("Mpeg locks [ L0 : %d | L1 : %d | L2 : %d ]", (locks>>7)&0x1, (locks>>6)&0x1, (locks>>5)&0x1);
+				if (c->isdbt_sb_mode
+				    && c->isdbt_sb_subchannel < 14
+				    && !state->differential_constellation)
+					/* signal to the upper layer, that there was a channel found and the parameters can be read */
+					state->status = FE_STATUS_DEMOD_SUCCESS;
+				else
+					state->status = FE_STATUS_DATA_LOCKED;
+				*tune_state = CT_DEMOD_STOP;
+			} else if (now > *timeout) {
+				if (c->isdbt_sb_mode
+				    && c->isdbt_sb_subchannel < 14
+				    && !state->differential_constellation) { /* continue to try init prbs autosearch */
+					state->subchannel += 3;
+					*tune_state = CT_DEMOD_STEP_11;
+				} else { /* we are done mpeg of the longest interleaver xas not locking but let's try if an other layer has locked in the same time */
+					if (locks & (0x7<<5)) {
+						dprintk("Mpeg locks [ L0 : %d | L1 : %d | L2 : %d ]", (locks>>7)&0x1, (locks>>6)&0x1, (locks>>5)&0x1);
+						state->status = FE_STATUS_DATA_LOCKED;
+					} else
+						state->status = FE_STATUS_TUNE_FAILED;
+					*tune_state = CT_DEMOD_STOP;
+				}
+			}
+			break;
+
+	case CT_DEMOD_STEP_11:  /* 41 : init prbs autosearch */
+			if (state->subchannel <= 41) {
+				dib8000_set_subchannel_prbs(state, dib8000_get_init_prbs(state, state->subchannel));
+				*tune_state = CT_DEMOD_STEP_9;
+			} else {
+				*tune_state = CT_DEMOD_STOP;
+				state->status = FE_STATUS_TUNE_FAILED;
+			}
+			break;
+
+	default:
+			break;
+	}
+
+	/* tuning is finished - cleanup the demod */
+	switch (*tune_state) {
+	case CT_DEMOD_STOP: /* (42) */
+#ifdef DIB8000_AGC_FREEZE
+			if ((state->revision != 0x8090) && (state->agc1_max != 0)) {
+				dib8000_write_word(state, 108, state->agc1_max);
+				dib8000_write_word(state, 109, state->agc1_min);
+				dib8000_write_word(state, 110, state->agc2_max);
+				dib8000_write_word(state, 111, state->agc2_min);
+				state->agc1_max = 0;
+				state->agc1_min = 0;
+				state->agc2_max = 0;
+				state->agc2_min = 0;
+			}
+#endif
+			ret = FE_CALLBACK_TIME_NEVER;
+			break;
+	default:
+			break;
 	}
 
+	if ((ret > 0) && (*tune_state > CT_DEMOD_STEP_3))
+		return ret * state->symbol_duration;
+	if ((ret > 0) && (ret < state->symbol_duration))
+		return state->symbol_duration; /* at least one symbol */
 	return ret;
 }
 
@@ -2767,7 +3226,7 @@ static int dib8000_wakeup(struct dvb_frontend *fe)
 	if (dib8000_set_adc_state(state, DIBX000_SLOW_ADC_ON) != 0)
 		dprintk("could not start Slow ADC");
 
-	if (state->revision != 0x8090)
+	if (state->revision == 0x8090)
 		dib8000_sad_calib(state);
 
 	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
@@ -2797,21 +3256,6 @@ static int dib8000_sleep(struct dvb_frontend *fe)
 	return dib8000_set_adc_state(state, DIBX000_SLOW_ADC_OFF) | dib8000_set_adc_state(state, DIBX000_ADC_OFF);
 }
 
-enum frontend_tune_state dib8000_get_tune_state(struct dvb_frontend *fe)
-{
-	struct dib8000_state *state = fe->demodulator_priv;
-	return state->tune_state;
-}
-EXPORT_SYMBOL(dib8000_get_tune_state);
-
-int dib8000_set_tune_state(struct dvb_frontend *fe, enum frontend_tune_state tune_state)
-{
-	struct dib8000_state *state = fe->demodulator_priv;
-	state->tune_state = tune_state;
-	return 0;
-}
-EXPORT_SYMBOL(dib8000_set_tune_state);
-
 static int dib8000_get_frontend(struct dvb_frontend *fe)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
@@ -2961,19 +3405,19 @@ static int dib8000_get_frontend(struct dvb_frontend *fe)
 static int dib8000_set_frontend(struct dvb_frontend *fe)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
-	u8 nbr_pending, exit_condition, index_frontend;
-	s8 index_frontend_success = -1;
-	int time, ret;
-	int  time_slave = FE_CALLBACK_TIME_NEVER;
+	struct dtv_frontend_properties *c = &state->fe[0]->dtv_property_cache;
+	int l, i, active, time, ret, time_slave = FE_CALLBACK_TIME_NEVER;
+	u8 exit_condition, index_frontend;
+	u32 delay, callback_time;
 
-	if (state->fe[0]->dtv_property_cache.frequency == 0) {
+	if (c->frequency == 0) {
 		dprintk("dib8000: must at least specify frequency ");
 		return 0;
 	}
 
-	if (state->fe[0]->dtv_property_cache.bandwidth_hz == 0) {
+	if (c->bandwidth_hz == 0) {
 		dprintk("dib8000: no bandwidth specified, set to default ");
-		state->fe[0]->dtv_property_cache.bandwidth_hz = 6000000;
+		c->bandwidth_hz = 6000000;
 	}
 
 	for (index_frontend = 0; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
@@ -2981,18 +3425,36 @@ static int dib8000_set_frontend(struct dvb_frontend *fe)
 		state->fe[index_frontend]->dtv_property_cache.delivery_system = SYS_ISDBT;
 		memcpy(&state->fe[index_frontend]->dtv_property_cache, &fe->dtv_property_cache, sizeof(struct dtv_frontend_properties));
 
-		if (state->revision != 0x8090)
-			dib8000_set_output_mode(state->fe[index_frontend],
-					OUTMODE_HIGH_Z);
-		else
-			dib8096p_set_output_mode(state->fe[index_frontend],
-					OUTMODE_HIGH_Z);
+		/* set output mode and diversity input */
+		if (state->revision != 0x8090) {
+			dib8000_set_diversity_in(state->fe[index_frontend], 1);
+			if (index_frontend != 0)
+				dib8000_set_output_mode(state->fe[index_frontend],
+						OUTMODE_DIVERSITY);
+			else
+				dib8000_set_output_mode(state->fe[0], OUTMODE_HIGH_Z);
+		} else {
+			dib8096p_set_diversity_in(state->fe[index_frontend], 1);
+			if (index_frontend != 0)
+				dib8096p_set_output_mode(state->fe[index_frontend],
+						OUTMODE_DIVERSITY);
+			else
+				dib8096p_set_output_mode(state->fe[0], OUTMODE_HIGH_Z);
+		}
+
+		/* tune the tuner */
 		if (state->fe[index_frontend]->ops.tuner_ops.set_params)
 			state->fe[index_frontend]->ops.tuner_ops.set_params(state->fe[index_frontend]);
 
 		dib8000_set_tune_state(state->fe[index_frontend], CT_AGC_START);
 	}
 
+	/* turn off the diversity of the last chip */
+	if (state->revision != 0x8090)
+		dib8000_set_diversity_in(state->fe[index_frontend - 1], 0);
+	else
+		dib8096p_set_diversity_in(state->fe[index_frontend - 1], 0);
+
 	/* start up the AGC */
 	do {
 		time = dib8000_agc_startup(state->fe[0]);
@@ -3019,139 +3481,88 @@ static int dib8000_set_frontend(struct dvb_frontend *fe)
 	for (index_frontend = 0; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++)
 		dib8000_set_tune_state(state->fe[index_frontend], CT_DEMOD_START);
 
-	if ((state->fe[0]->dtv_property_cache.delivery_system != SYS_ISDBT) ||
-			(state->fe[0]->dtv_property_cache.inversion == INVERSION_AUTO) ||
-			(state->fe[0]->dtv_property_cache.transmission_mode == TRANSMISSION_MODE_AUTO) ||
-			(state->fe[0]->dtv_property_cache.guard_interval == GUARD_INTERVAL_AUTO) ||
-			(((state->fe[0]->dtv_property_cache.isdbt_layer_enabled & (1 << 0)) != 0) &&
-			 (state->fe[0]->dtv_property_cache.layer[0].segment_count != 0xff) &&
-			 (state->fe[0]->dtv_property_cache.layer[0].segment_count != 0) &&
-			 ((state->fe[0]->dtv_property_cache.layer[0].modulation == QAM_AUTO) ||
-			  (state->fe[0]->dtv_property_cache.layer[0].fec == FEC_AUTO))) ||
-			(((state->fe[0]->dtv_property_cache.isdbt_layer_enabled & (1 << 1)) != 0) &&
-			 (state->fe[0]->dtv_property_cache.layer[1].segment_count != 0xff) &&
-			 (state->fe[0]->dtv_property_cache.layer[1].segment_count != 0) &&
-			 ((state->fe[0]->dtv_property_cache.layer[1].modulation == QAM_AUTO) ||
-			  (state->fe[0]->dtv_property_cache.layer[1].fec == FEC_AUTO))) ||
-			(((state->fe[0]->dtv_property_cache.isdbt_layer_enabled & (1 << 2)) != 0) &&
-			 (state->fe[0]->dtv_property_cache.layer[2].segment_count != 0xff) &&
-			 (state->fe[0]->dtv_property_cache.layer[2].segment_count != 0) &&
-			 ((state->fe[0]->dtv_property_cache.layer[2].modulation == QAM_AUTO) ||
-			  (state->fe[0]->dtv_property_cache.layer[2].fec == FEC_AUTO))) ||
-			(((state->fe[0]->dtv_property_cache.layer[0].segment_count == 0) ||
-			  ((state->fe[0]->dtv_property_cache.isdbt_layer_enabled & (1 << 0)) == 0)) &&
-			 ((state->fe[0]->dtv_property_cache.layer[1].segment_count == 0) ||
-			  ((state->fe[0]->dtv_property_cache.isdbt_layer_enabled & (2 << 0)) == 0)) &&
-			 ((state->fe[0]->dtv_property_cache.layer[2].segment_count == 0) || ((state->fe[0]->dtv_property_cache.isdbt_layer_enabled & (3 << 0)) == 0)))) {
-		int i = 100;
-		u8 found = 0;
-		u8 tune_failed = 0;
-
+	active = 1;
+	do {
+		callback_time = FE_CALLBACK_TIME_NEVER;
 		for (index_frontend = 0; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
-			dib8000_set_bandwidth(state->fe[index_frontend], fe->dtv_property_cache.bandwidth_hz / 1000);
-			dib8000_autosearch_start(state->fe[index_frontend]);
-		}
-
-		do {
-			msleep(20);
-			nbr_pending = 0;
-			exit_condition = 0; /* 0: tune pending; 1: tune failed; 2:tune success */
-			for (index_frontend = 0; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
-				if (((tune_failed >> index_frontend) & 0x1) == 0) {
-					found = dib8000_autosearch_irq(state->fe[index_frontend]);
-					switch (found) {
-					case 0: /* tune pending */
-						 nbr_pending++;
-						 break;
-					case 2:
-						 dprintk("autosearch succeed on the frontend%i", index_frontend);
-						 exit_condition = 2;
-						 index_frontend_success = index_frontend;
-						 break;
-					default:
-						 dprintk("unhandled autosearch result");
-					case 1:
-						 tune_failed |= (1 << index_frontend);
-						 dprintk("autosearch failed for the frontend%i", index_frontend);
-						 break;
+			delay = dib8000_tune(state->fe[index_frontend]);
+			if (delay != FE_CALLBACK_TIME_NEVER)
+				delay += systime();
+
+			/* we are in autosearch */
+			if (state->channel_parameters_set == 0) { /* searching */
+				if ((dib8000_get_status(state->fe[index_frontend]) == FE_STATUS_DEMOD_SUCCESS) || (dib8000_get_status(state->fe[index_frontend]) == FE_STATUS_FFT_SUCCESS)) {
+					dprintk("autosearch succeeded on fe%i", index_frontend);
+					dib8000_get_frontend(state->fe[index_frontend]); /* we read the channel parameters from the frontend which was successful */
+					state->channel_parameters_set = 1;
+
+					for (l = 0; (l < MAX_NUMBER_OF_FRONTENDS) && (state->fe[l] != NULL); l++) {
+						if (l != index_frontend) { /* and for all frontend except the successful one */
+							dib8000_tune_restart_from_demod(state->fe[l]);
+
+							state->fe[l]->dtv_property_cache.isdbt_sb_mode = state->fe[index_frontend]->dtv_property_cache.isdbt_sb_mode;
+							state->fe[l]->dtv_property_cache.inversion = state->fe[index_frontend]->dtv_property_cache.inversion;
+							state->fe[l]->dtv_property_cache.transmission_mode = state->fe[index_frontend]->dtv_property_cache.transmission_mode;
+							state->fe[l]->dtv_property_cache.guard_interval = state->fe[index_frontend]->dtv_property_cache.guard_interval;
+							state->fe[l]->dtv_property_cache.isdbt_partial_reception = state->fe[index_frontend]->dtv_property_cache.isdbt_partial_reception;
+							for (i = 0; i < 3; i++) {
+								state->fe[l]->dtv_property_cache.layer[i].segment_count = state->fe[index_frontend]->dtv_property_cache.layer[i].segment_count;
+								state->fe[l]->dtv_property_cache.layer[i].interleaving = state->fe[index_frontend]->dtv_property_cache.layer[i].interleaving;
+								state->fe[l]->dtv_property_cache.layer[i].fec = state->fe[index_frontend]->dtv_property_cache.layer[i].fec;
+								state->fe[l]->dtv_property_cache.layer[i].modulation = state->fe[index_frontend]->dtv_property_cache.layer[i].modulation;
+							}
+
+						}
 					}
 				}
 			}
-
-			/* if all tune are done and no success, exit: tune failed */
-			if ((nbr_pending == 0) && (exit_condition == 0))
-				exit_condition = 1;
-		} while ((exit_condition == 0) && i--);
-
-		if (exit_condition == 1) { /* tune failed */
-			dprintk("tune failed");
-			return 0;
+			if (delay < callback_time)
+				callback_time = delay;
+		}
+		/* tuning is done when the master frontend is done (failed or success) */
+		if (dib8000_get_status(state->fe[0]) == FE_STATUS_TUNE_FAILED ||
+				dib8000_get_status(state->fe[0]) == FE_STATUS_LOCKED ||
+				dib8000_get_status(state->fe[0]) == FE_STATUS_DATA_LOCKED) {
+			active = 0;
+			/* we need to wait for all frontends to be finished */
+			for (index_frontend = 0; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
+				if (dib8000_get_tune_state(state->fe[index_frontend]) != CT_DEMOD_STOP)
+					active = 1;
+			}
+			if (active == 0)
+				dprintk("tuning done with status %d", dib8000_get_status(state->fe[0]));
 		}
 
-		dprintk("tune success on frontend%i", index_frontend_success);
-
-		dib8000_get_frontend(fe);
-	}
+		if ((active == 1) && (callback_time == FE_CALLBACK_TIME_NEVER)) {
+			dprintk("strange callback time something went wrong");
+			active = 0;
+		}
 
-	for (index_frontend = 0, ret = 0; (ret >= 0) && (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++)
-		ret = dib8000_tune(state->fe[index_frontend]);
+		while ((active == 1) && (systime() < callback_time))
+			msleep(100);
+	} while (active);
 
-	/* set output mode and diversity input */
-	if (state->revision != 0x8090) {
+	/* set output mode */
+	if (state->revision != 0x8090)
 		dib8000_set_output_mode(state->fe[0], state->cfg.output_mode);
-		for (index_frontend = 1;
-				(index_frontend < MAX_NUMBER_OF_FRONTENDS) &&
-				(state->fe[index_frontend] != NULL);
-				index_frontend++) {
-			dib8000_set_output_mode(state->fe[index_frontend],
-					OUTMODE_DIVERSITY);
-			dib8000_set_diversity_in(state->fe[index_frontend-1], 1);
-		}
-
-		/* turn off the diversity of the last chip */
-		dib8000_set_diversity_in(state->fe[index_frontend-1], 0);
-	} else {
+	else {
 		dib8096p_set_output_mode(state->fe[0], state->cfg.output_mode);
 		if (state->cfg.enMpegOutput == 0) {
 			dib8096p_setDibTxMux(state, MPEG_ON_DIBTX);
 			dib8096p_setHostBusMux(state, DIBTX_ON_HOSTBUS);
 		}
-		for (index_frontend = 1;
-				(index_frontend < MAX_NUMBER_OF_FRONTENDS) &&
-				(state->fe[index_frontend] != NULL);
-				index_frontend++) {
-			dib8096p_set_output_mode(state->fe[index_frontend],
-					OUTMODE_DIVERSITY);
-			dib8096p_set_diversity_in(state->fe[index_frontend-1], 1);
-		}
-
-		/* turn off the diversity of the last chip */
-		dib8096p_set_diversity_in(state->fe[index_frontend-1], 0);
 	}
 
 	return ret;
 }
 
-static u16 dib8000_read_lock(struct dvb_frontend *fe)
-{
-	struct dib8000_state *state = fe->demodulator_priv;
-
-	if (state->revision == 0x8090)
-		return dib8000_read_word(state, 570);
-	return dib8000_read_word(state, 568);
-}
-
 static int dib8000_read_status(struct dvb_frontend *fe, fe_status_t * stat)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
 	u16 lock_slave = 0, lock;
 	u8 index_frontend;
 
-	if (state->revision == 0x8090)
-		lock = dib8000_read_word(state, 570);
-	else
-		lock = dib8000_read_word(state, 568);
-
+	lock = dib8000_read_lock(fe);
 	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++)
 		lock_slave |= dib8000_read_lock(state->fe[index_frontend]);
 
@@ -3545,10 +3956,11 @@ struct dvb_frontend *dib8000_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr, s
 	dib8000_reset(fe);
 
 	dib8000_write_word(state, 285, (dib8000_read_word(state, 285) & ~0x60) | (3 << 5));	/* ber_rs_len = 3 */
+	state->current_demod_bw = 6000;
 
 	return fe;
 
- error:
+error:
 	kfree(state);
 	return NULL;
 }
diff --git a/drivers/media/dvb-frontends/dib8000.h b/drivers/media/dvb-frontends/dib8000.h
index 9e7a2b170d5..b8c11e52c51 100644
--- a/drivers/media/dvb-frontends/dib8000.h
+++ b/drivers/media/dvb-frontends/dib8000.h
@@ -33,6 +33,8 @@ struct dib8000_config {
 	u8 output_mode;
 	u8 refclksel;
 	u8 enMpegOutput:1;
+
+	struct dibx000_bandwidth_config *plltable;
 };
 
 #define DEFAULT_DIB8000_I2C_ADDRESS 18
@@ -58,7 +60,7 @@ extern int dib8090p_get_dc_power(struct dvb_frontend *fe, u8 IQ);
 extern u32 dib8000_ctrl_timf(struct dvb_frontend *fe,
 		uint8_t op, uint32_t timf);
 extern int dib8000_update_pll(struct dvb_frontend *fe,
-		struct dibx000_bandwidth_config *pll);
+		struct dibx000_bandwidth_config *pll, u32 bw, u8 ratio);
 extern int dib8000_set_slave_frontend(struct dvb_frontend *fe, struct dvb_frontend *fe_slave);
 extern int dib8000_remove_slave_frontend(struct dvb_frontend *fe);
 extern struct dvb_frontend *dib8000_get_slave_frontend(struct dvb_frontend *fe, int slave_index);
@@ -147,7 +149,7 @@ static inline u32 dib8000_ctrl_timf(struct dvb_frontend *fe,
 	return 0;
 }
 static inline int dib8000_update_pll(struct dvb_frontend *fe,
-		struct dibx000_bandwidth_config *pll)
+		struct dibx000_bandwidth_config *pll, u32 bw, u8 ratio)
 {
 	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
 	return -ENODEV;
diff --git a/drivers/media/dvb-frontends/dibx000_common.h b/drivers/media/dvb-frontends/dibx000_common.h
index 5f484881d7b..b538e0555c9 100644
--- a/drivers/media/dvb-frontends/dibx000_common.h
+++ b/drivers/media/dvb-frontends/dibx000_common.h
@@ -193,7 +193,8 @@ enum frontend_tune_state {
 	CT_DEMOD_STEP_8,
 	CT_DEMOD_STEP_9,
 	CT_DEMOD_STEP_10,
-	CT_DEMOD_SEARCH_NEXT = 41,
+	CT_DEMOD_STEP_11,
+	CT_DEMOD_SEARCH_NEXT = 51,
 	CT_DEMOD_STEP_LOCKED,
 	CT_DEMOD_STOP,
 
diff --git a/drivers/media/dvb-frontends/drxd.h b/drivers/media/dvb-frontends/drxd.h
index 216c8c3702f..5f1d6b5f168 100644
--- a/drivers/media/dvb-frontends/drxd.h
+++ b/drivers/media/dvb-frontends/drxd.h
@@ -24,6 +24,7 @@
 #ifndef _DRXD_H_
 #define _DRXD_H_
 
+#include <linux/kconfig.h>
 #include <linux/types.h>
 #include <linux/i2c.h>
 
@@ -51,8 +52,7 @@ struct drxd_config {
 	 s16(*osc_deviation) (void *priv, s16 dev, int flag);
 };
 
-#if defined(CONFIG_DVB_DRXD) || \
-			(defined(CONFIG_DVB_DRXD_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_DRXD)
 extern
 struct dvb_frontend *drxd_attach(const struct drxd_config *config,
 				 void *priv, struct i2c_adapter *i2c,
diff --git a/drivers/media/dvb-frontends/drxk.h b/drivers/media/dvb-frontends/drxk.h
index 94fecfbf14c..e6667189ddc 100644
--- a/drivers/media/dvb-frontends/drxk.h
+++ b/drivers/media/dvb-frontends/drxk.h
@@ -1,6 +1,7 @@
 #ifndef _DRXK_H_
 #define _DRXK_H_
 
+#include <linux/kconfig.h>
 #include <linux/types.h>
 #include <linux/i2c.h>
 
@@ -52,8 +53,7 @@ struct drxk_config {
 	int		 qam_demod_parameter_count;
 };
 
-#if defined(CONFIG_DVB_DRXK) || (defined(CONFIG_DVB_DRXK_MODULE) \
-        && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_DRXK)
 extern struct dvb_frontend *drxk_attach(const struct drxk_config *config,
 					struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/drxk_hard.c b/drivers/media/dvb-frontends/drxk_hard.c
index c2fc7da0d6b..ec24d71e153 100644
--- a/drivers/media/dvb-frontends/drxk_hard.c
+++ b/drivers/media/dvb-frontends/drxk_hard.c
@@ -1947,8 +1947,7 @@ static int ShutDown(struct drxk_state *state)
 	return 0;
 }
 
-static int GetLockStatus(struct drxk_state *state, u32 *pLockStatus,
-			 u32 Time)
+static int GetLockStatus(struct drxk_state *state, u32 *pLockStatus)
 {
 	int status = -EINVAL;
 
@@ -2490,32 +2489,6 @@ error:
 	return status;
 }
 
-static int ReadIFAgc(struct drxk_state *state, u32 *pValue)
-{
-	u16 agcDacLvl;
-	int status;
-	u16 Level = 0;
-
-	dprintk(1, "\n");
-
-	status = read16(state, IQM_AF_AGC_IF__A, &agcDacLvl);
-	if (status < 0) {
-		printk(KERN_ERR "drxk: Error %d on %s\n", status, __func__);
-		return status;
-	}
-
-	*pValue = 0;
-
-	if (agcDacLvl > DRXK_AGC_DAC_OFFSET)
-		Level = agcDacLvl - DRXK_AGC_DAC_OFFSET;
-	if (Level < 14000)
-		*pValue = (14000 - Level) / 4;
-	else
-		*pValue = 0;
-
-	return status;
-}
-
 static int GetQAMSignalToNoise(struct drxk_state *state,
 			       s32 *pSignalToNoise)
 {
@@ -2654,12 +2627,7 @@ static int GetDVBTSignalToNoise(struct drxk_state *state,
 		/* log(x) x = (16bits + 16bits) << 15 ->32 bits  */
 		c = Log10Times100(SqrErrIQ);
 
-		iMER = a + b;
-		/* No negative MER, clip to zero */
-		if (iMER > c)
-			iMER -= c;
-		else
-			iMER = 0;
+		iMER = a + b - c;
 	}
 	*pSignalToNoise = iMER;
 
@@ -6380,46 +6348,257 @@ static int drxk_set_parameters(struct dvb_frontend *fe)
 	fe->ops.tuner_ops.get_if_frequency(fe, &IF);
 	Start(state, 0, IF);
 
+	/* After set_frontend, stats aren't avaliable */
+	p->strength.stat[0].scale = FE_SCALE_RELATIVE;
+	p->cnr.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	p->block_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	p->block_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	p->pre_bit_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	p->pre_bit_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	p->post_bit_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	p->post_bit_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+
 	/* printk(KERN_DEBUG "drxk: %s IF=%d done\n", __func__, IF); */
 
 	return 0;
 }
 
-static int drxk_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int get_strength(struct drxk_state *state, u64 *strength)
 {
+	int status;
+	struct SCfgAgc   rfAgc, ifAgc;
+	u32          totalGain  = 0;
+	u32          atten      = 0;
+	u32          agcRange   = 0;
+	u16            scu_lvl  = 0;
+	u16            scu_coc  = 0;
+	/* FIXME: those are part of the tuner presets */
+	u16 tunerRfGain         = 50; /* Default value on az6007 driver */
+	u16 tunerIfGain         = 40; /* Default value on az6007 driver */
+
+	*strength = 0;
+
+	if (IsDVBT(state)) {
+		rfAgc = state->m_dvbtRfAgcCfg;
+		ifAgc = state->m_dvbtIfAgcCfg;
+	} else if (IsQAM(state)) {
+		rfAgc = state->m_qamRfAgcCfg;
+		ifAgc = state->m_qamIfAgcCfg;
+	} else {
+		rfAgc = state->m_atvRfAgcCfg;
+		ifAgc = state->m_atvIfAgcCfg;
+	}
+
+	if (rfAgc.ctrlMode == DRXK_AGC_CTRL_AUTO) {
+		/* SCU outputLevel */
+		status = read16(state, SCU_RAM_AGC_RF_IACCU_HI__A, &scu_lvl);
+		if (status < 0)
+			return status;
+
+		/* SCU c.o.c. */
+		read16(state, SCU_RAM_AGC_RF_IACCU_HI_CO__A, &scu_coc);
+		if (status < 0)
+			return status;
+
+		if (((u32) scu_lvl + (u32) scu_coc) < 0xffff)
+			rfAgc.outputLevel = scu_lvl + scu_coc;
+		else
+			rfAgc.outputLevel = 0xffff;
+
+		/* Take RF gain into account */
+		totalGain += tunerRfGain;
+
+		/* clip output value */
+		if (rfAgc.outputLevel < rfAgc.minOutputLevel)
+			rfAgc.outputLevel = rfAgc.minOutputLevel;
+		if (rfAgc.outputLevel > rfAgc.maxOutputLevel)
+			rfAgc.outputLevel = rfAgc.maxOutputLevel;
+
+		agcRange = (u32) (rfAgc.maxOutputLevel - rfAgc.minOutputLevel);
+		if (agcRange > 0) {
+			atten += 100UL *
+				((u32)(tunerRfGain)) *
+				((u32)(rfAgc.outputLevel - rfAgc.minOutputLevel))
+				/ agcRange;
+		}
+	}
+
+	if (ifAgc.ctrlMode == DRXK_AGC_CTRL_AUTO) {
+		status = read16(state, SCU_RAM_AGC_IF_IACCU_HI__A,
+				&ifAgc.outputLevel);
+		if (status < 0)
+			return status;
+
+		status = read16(state, SCU_RAM_AGC_INGAIN_TGT_MIN__A,
+				&ifAgc.top);
+		if (status < 0)
+			return status;
+
+		/* Take IF gain into account */
+		totalGain += (u32) tunerIfGain;
+
+		/* clip output value */
+		if (ifAgc.outputLevel < ifAgc.minOutputLevel)
+			ifAgc.outputLevel = ifAgc.minOutputLevel;
+		if (ifAgc.outputLevel > ifAgc.maxOutputLevel)
+			ifAgc.outputLevel = ifAgc.maxOutputLevel;
+
+		agcRange  = (u32) (ifAgc.maxOutputLevel - ifAgc.minOutputLevel);
+		if (agcRange > 0) {
+			atten += 100UL *
+				((u32)(tunerIfGain)) *
+				((u32)(ifAgc.outputLevel - ifAgc.minOutputLevel))
+				/ agcRange;
+		}
+	}
+
+	/*
+	 * Convert to 0..65535 scale.
+	 * If it can't be measured (AGC is disabled), just show 100%.
+	 */
+	if (totalGain > 0)
+		*strength = (65535UL * atten / totalGain / 100);
+	else
+		*strength = 65535;
+
+	return 0;
+}
+
+static int drxk_get_stats(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	struct drxk_state *state = fe->demodulator_priv;
+	int status;
 	u32 stat;
-
-	dprintk(1, "\n");
+	u16 reg16;
+	u32 post_bit_count;
+	u32 post_bit_err_count;
+	u32 post_bit_error_scale;
+	u32 pre_bit_err_count;
+	u32 pre_bit_count;
+	u32 pkt_count;
+	u32 pkt_error_count;
+	s32 cnr;
 
 	if (state->m_DrxkState == DRXK_NO_DEV)
 		return -ENODEV;
 	if (state->m_DrxkState == DRXK_UNINITIALIZED)
 		return -EAGAIN;
 
-	*status = 0;
-	GetLockStatus(state, &stat, 0);
+	/* get status */
+	state->fe_status = 0;
+	GetLockStatus(state, &stat);
 	if (stat == MPEG_LOCK)
-		*status |= 0x1f;
+		state->fe_status |= 0x1f;
 	if (stat == FEC_LOCK)
-		*status |= 0x0f;
+		state->fe_status |= 0x0f;
 	if (stat == DEMOD_LOCK)
-		*status |= 0x07;
-	return 0;
+		state->fe_status |= 0x07;
+
+	/*
+	 * Estimate signal strength from AGC
+	 */
+	get_strength(state, &c->strength.stat[0].uvalue);
+	c->strength.stat[0].scale = FE_SCALE_RELATIVE;
+
+
+	if (stat >= DEMOD_LOCK) {
+		GetSignalToNoise(state, &cnr);
+		c->cnr.stat[0].svalue = cnr * 100;
+		c->cnr.stat[0].scale = FE_SCALE_DECIBEL;
+	} else {
+		c->cnr.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	}
+
+	if (stat < FEC_LOCK) {
+		c->block_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		c->block_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		c->pre_bit_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		c->pre_bit_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		c->post_bit_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		c->post_bit_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		return 0;
+	}
+
+	/* Get post BER */
+
+	/* BER measurement is valid if at least FEC lock is achieved */
+
+	/* OFDM_EC_VD_REQ_SMB_CNT__A and/or OFDM_EC_VD_REQ_BIT_CNT can be written
+		to set nr of symbols or bits over which
+		to measure EC_VD_REG_ERR_BIT_CNT__A . See CtrlSetCfg(). */
+
+	/* Read registers for post/preViterbi BER calculation */
+	status = read16(state, OFDM_EC_VD_ERR_BIT_CNT__A, &reg16);
+	if (status < 0)
+		goto error;
+	pre_bit_err_count = reg16;
+
+	status = read16(state, OFDM_EC_VD_IN_BIT_CNT__A , &reg16);
+	if (status < 0)
+		goto error;
+	pre_bit_count = reg16;
+
+	/* Number of bit-errors */
+	status = read16(state, FEC_RS_NR_BIT_ERRORS__A, &reg16);
+	if (status < 0)
+		goto error;
+	post_bit_err_count = reg16;
+
+	status = read16(state, FEC_RS_MEASUREMENT_PRESCALE__A, &reg16);
+	if (status < 0)
+		goto error;
+	post_bit_error_scale = reg16;
+
+	status = read16(state, FEC_RS_MEASUREMENT_PERIOD__A, &reg16);
+	if (status < 0)
+		goto error;
+	pkt_count = reg16;
+
+	status = read16(state, SCU_RAM_FEC_ACCUM_PKT_FAILURES__A, &reg16);
+	if (status < 0)
+		goto error;
+	pkt_error_count = reg16;
+	write16(state, SCU_RAM_FEC_ACCUM_PKT_FAILURES__A, 0);
+
+	post_bit_err_count *= post_bit_error_scale;
+
+	post_bit_count = pkt_count * 204 * 8;
+
+	/* Store the results */
+	c->block_error.stat[0].scale = FE_SCALE_COUNTER;
+	c->block_error.stat[0].uvalue += pkt_error_count;
+	c->block_count.stat[0].scale = FE_SCALE_COUNTER;
+	c->block_count.stat[0].uvalue += pkt_count;
+
+	c->pre_bit_error.stat[0].scale = FE_SCALE_COUNTER;
+	c->pre_bit_error.stat[0].uvalue += pre_bit_err_count;
+	c->pre_bit_count.stat[0].scale = FE_SCALE_COUNTER;
+	c->pre_bit_count.stat[0].uvalue += pre_bit_count;
+
+	c->post_bit_error.stat[0].scale = FE_SCALE_COUNTER;
+	c->post_bit_error.stat[0].uvalue += post_bit_err_count;
+	c->post_bit_count.stat[0].scale = FE_SCALE_COUNTER;
+	c->post_bit_count.stat[0].uvalue += post_bit_count;
+
+error:
+	return status;
 }
 
-static int drxk_read_ber(struct dvb_frontend *fe, u32 *ber)
+
+static int drxk_read_status(struct dvb_frontend *fe, fe_status_t *status)
 {
 	struct drxk_state *state = fe->demodulator_priv;
+	int rc;
 
 	dprintk(1, "\n");
 
-	if (state->m_DrxkState == DRXK_NO_DEV)
-		return -ENODEV;
-	if (state->m_DrxkState == DRXK_UNINITIALIZED)
-		return -EAGAIN;
+	rc = drxk_get_stats(fe);
+	if (rc < 0)
+		return rc;
+
+	*status = state->fe_status;
 
-	*ber = 0;
 	return 0;
 }
 
@@ -6427,7 +6606,7 @@ static int drxk_read_signal_strength(struct dvb_frontend *fe,
 				     u16 *strength)
 {
 	struct drxk_state *state = fe->demodulator_priv;
-	u32 val = 0;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 
 	dprintk(1, "\n");
 
@@ -6436,8 +6615,7 @@ static int drxk_read_signal_strength(struct dvb_frontend *fe,
 	if (state->m_DrxkState == DRXK_UNINITIALIZED)
 		return -EAGAIN;
 
-	ReadIFAgc(state, &val);
-	*strength = val & 0xffff;
+	*strength = c->strength.stat[0].uvalue;
 	return 0;
 }
 
@@ -6454,6 +6632,10 @@ static int drxk_read_snr(struct dvb_frontend *fe, u16 *snr)
 		return -EAGAIN;
 
 	GetSignalToNoise(state, &snr2);
+
+	/* No negative SNR, clip to zero */
+	if (snr2 < 0)
+		snr2 = 0;
 	*snr = snr2 & 0xffff;
 	return 0;
 }
@@ -6529,7 +6711,6 @@ static struct dvb_frontend_ops drxk_ops = {
 	.get_tune_settings = drxk_get_tune_settings,
 
 	.read_status = drxk_read_status,
-	.read_ber = drxk_read_ber,
 	.read_signal_strength = drxk_read_signal_strength,
 	.read_snr = drxk_read_snr,
 	.read_ucblocks = drxk_read_ucblocks,
@@ -6538,6 +6719,7 @@ static struct dvb_frontend_ops drxk_ops = {
 struct dvb_frontend *drxk_attach(const struct drxk_config *config,
 				 struct i2c_adapter *i2c)
 {
+	struct dtv_frontend_properties *p;
 	struct drxk_state *state = NULL;
 	u8 adr = config->adr;
 	int status;
@@ -6618,6 +6800,27 @@ struct dvb_frontend *drxk_attach(const struct drxk_config *config,
 	} else if (init_drxk(state) < 0)
 		goto error;
 
+
+	/* Initialize stats */
+	p = &state->frontend.dtv_property_cache;
+	p->strength.len = 1;
+	p->cnr.len = 1;
+	p->block_error.len = 1;
+	p->block_count.len = 1;
+	p->pre_bit_error.len = 1;
+	p->pre_bit_count.len = 1;
+	p->post_bit_error.len = 1;
+	p->post_bit_count.len = 1;
+
+	p->strength.stat[0].scale = FE_SCALE_RELATIVE;
+	p->cnr.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	p->block_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	p->block_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	p->pre_bit_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	p->pre_bit_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	p->post_bit_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	p->post_bit_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+
 	printk(KERN_INFO "drxk: frontend initialized.\n");
 	return &state->frontend;
 
diff --git a/drivers/media/dvb-frontends/drxk_hard.h b/drivers/media/dvb-frontends/drxk_hard.h
index d18a896a983..b8424f15f9a 100644
--- a/drivers/media/dvb-frontends/drxk_hard.h
+++ b/drivers/media/dvb-frontends/drxk_hard.h
@@ -345,6 +345,8 @@ struct drxk_state {
 	bool	antenna_dvbt;
 	u16	antenna_gpio;
 
+	fe_status_t fe_status;
+
 	/* Firmware */
 	const char *microcode_name;
 	struct completion fw_wait_load;
diff --git a/drivers/media/dvb-frontends/drxk_map.h b/drivers/media/dvb-frontends/drxk_map.h
index 23e16c12f23..761613f9fd5 100644
--- a/drivers/media/dvb-frontends/drxk_map.h
+++ b/drivers/media/dvb-frontends/drxk_map.h
@@ -10,6 +10,7 @@
 #define    FEC_RS_COMM_EXEC_STOP                                           0x0
 #define  FEC_RS_MEASUREMENT_PERIOD__A                                      0x1C30012
 #define  FEC_RS_MEASUREMENT_PRESCALE__A                                    0x1C30013
+#define FEC_RS_NR_BIT_ERRORS__A                                            0x1C30014
 #define  FEC_OC_MODE__A                                                    0x1C40011
 #define    FEC_OC_MODE_PARITY__M                                           0x1
 #define  FEC_OC_DTO_MODE__A                                                0x1C40014
@@ -129,6 +130,8 @@
 #define  OFDM_EC_SB_PRIOR__A                                               0x3410013
 #define    OFDM_EC_SB_PRIOR_HI                                             0x0
 #define    OFDM_EC_SB_PRIOR_LO                                             0x1
+#define OFDM_EC_VD_ERR_BIT_CNT__A                                          0x3420017
+#define OFDM_EC_VD_IN_BIT_CNT__A                                           0x3420018
 #define  OFDM_EQ_TOP_TD_TPS_CONST__A                                       0x3010054
 #define  OFDM_EQ_TOP_TD_TPS_CONST__M                                       0x3
 #define    OFDM_EQ_TOP_TD_TPS_CONST_64QAM                                  0x2
diff --git a/drivers/media/dvb-frontends/ds3000.h b/drivers/media/dvb-frontends/ds3000.h
index 478ad66c63d..f9c21fb7af1 100644
--- a/drivers/media/dvb-frontends/ds3000.h
+++ b/drivers/media/dvb-frontends/ds3000.h
@@ -22,6 +22,7 @@
 #ifndef DS3000_H
 #define DS3000_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct ds3000_config {
@@ -34,8 +35,7 @@ struct ds3000_config {
 	void (*set_lock_led)(struct dvb_frontend *fe, int offon);
 };
 
-#if defined(CONFIG_DVB_DS3000) || \
-			(defined(CONFIG_DVB_DS3000_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_DS3000)
 extern struct dvb_frontend *ds3000_attach(const struct ds3000_config *config,
 					struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/dvb_dummy_fe.h b/drivers/media/dvb-frontends/dvb_dummy_fe.h
index 1fcb987d638..0cbf9610563 100644
--- a/drivers/media/dvb-frontends/dvb_dummy_fe.h
+++ b/drivers/media/dvb-frontends/dvb_dummy_fe.h
@@ -22,11 +22,11 @@
 #ifndef DVB_DUMMY_FE_H
 #define DVB_DUMMY_FE_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include "dvb_frontend.h"
 
-#if defined(CONFIG_DVB_DUMMY_FE) || (defined(CONFIG_DVB_DUMMY_FE_MODULE) && \
-defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_DUMMY_FE)
 extern struct dvb_frontend* dvb_dummy_fe_ofdm_attach(void);
 extern struct dvb_frontend* dvb_dummy_fe_qpsk_attach(void);
 extern struct dvb_frontend* dvb_dummy_fe_qam_attach(void);
diff --git a/drivers/media/dvb-frontends/ec100.h b/drivers/media/dvb-frontends/ec100.h
index b8479719d7f..37558403068 100644
--- a/drivers/media/dvb-frontends/ec100.h
+++ b/drivers/media/dvb-frontends/ec100.h
@@ -22,6 +22,7 @@
 #ifndef EC100_H
 #define EC100_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct ec100_config {
@@ -30,8 +31,7 @@ struct ec100_config {
 };
 
 
-#if defined(CONFIG_DVB_EC100) || \
-	(defined(CONFIG_DVB_EC100_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_EC100)
 extern struct dvb_frontend *ec100_attach(const struct ec100_config *config,
 	struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/hd29l2.h b/drivers/media/dvb-frontends/hd29l2.h
index 4ad00d79aa7..05cd13028a9 100644
--- a/drivers/media/dvb-frontends/hd29l2.h
+++ b/drivers/media/dvb-frontends/hd29l2.h
@@ -23,6 +23,7 @@
 #ifndef HD29L2_H
 #define HD29L2_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct hd29l2_config {
@@ -50,8 +51,7 @@ struct hd29l2_config {
 };
 
 
-#if defined(CONFIG_DVB_HD29L2) || \
-	(defined(CONFIG_DVB_HD29L2_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_HD29L2)
 extern struct dvb_frontend *hd29l2_attach(const struct hd29l2_config *config,
 	struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/isl6421.c b/drivers/media/dvb-frontends/isl6421.c
index 0cb3f0f74c9..c77002fcc8e 100644
--- a/drivers/media/dvb-frontends/isl6421.c
+++ b/drivers/media/dvb-frontends/isl6421.c
@@ -89,6 +89,30 @@ static int isl6421_enable_high_lnb_voltage(struct dvb_frontend *fe, long arg)
 	return (i2c_transfer(isl6421->i2c, &msg, 1) == 1) ? 0 : -EIO;
 }
 
+static int isl6421_set_tone(struct dvb_frontend* fe, fe_sec_tone_mode_t tone)
+{
+	struct isl6421 *isl6421 = (struct isl6421 *) fe->sec_priv;
+	struct i2c_msg msg = { .addr = isl6421->i2c_addr, .flags = 0,
+			       .buf = &isl6421->config,
+			       .len = sizeof(isl6421->config) };
+
+	switch (tone) {
+	case SEC_TONE_ON:
+		isl6421->config |= ISL6421_ENT1;
+		break;
+	case SEC_TONE_OFF:
+		isl6421->config &= ~ISL6421_ENT1;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	isl6421->config |= isl6421->override_or;
+	isl6421->config &= isl6421->override_and;
+
+	return (i2c_transfer(isl6421->i2c, &msg, 1) == 1) ? 0 : -EIO;
+}
+
 static void isl6421_release(struct dvb_frontend *fe)
 {
 	/* power off */
@@ -100,7 +124,7 @@ static void isl6421_release(struct dvb_frontend *fe)
 }
 
 struct dvb_frontend *isl6421_attach(struct dvb_frontend *fe, struct i2c_adapter *i2c, u8 i2c_addr,
-		   u8 override_set, u8 override_clear)
+		   u8 override_set, u8 override_clear, bool override_tone)
 {
 	struct isl6421 *isl6421 = kmalloc(sizeof(struct isl6421), GFP_KERNEL);
 	if (!isl6421)
@@ -131,6 +155,8 @@ struct dvb_frontend *isl6421_attach(struct dvb_frontend *fe, struct i2c_adapter
 	/* override frontend ops */
 	fe->ops.set_voltage = isl6421_set_voltage;
 	fe->ops.enable_high_lnb_voltage = isl6421_enable_high_lnb_voltage;
+	if (override_tone)
+		fe->ops.set_tone = isl6421_set_tone;
 
 	return fe;
 }
diff --git a/drivers/media/dvb-frontends/isl6421.h b/drivers/media/dvb-frontends/isl6421.h
index e7ca7d12b50..630e7f8a150 100644
--- a/drivers/media/dvb-frontends/isl6421.h
+++ b/drivers/media/dvb-frontends/isl6421.h
@@ -42,10 +42,10 @@
 #if IS_ENABLED(CONFIG_DVB_ISL6421)
 /* override_set and override_clear control which system register bits (above) to always set & clear */
 extern struct dvb_frontend *isl6421_attach(struct dvb_frontend *fe, struct i2c_adapter *i2c, u8 i2c_addr,
-			  u8 override_set, u8 override_clear);
+			  u8 override_set, u8 override_clear, bool override_tone);
 #else
 static inline struct dvb_frontend *isl6421_attach(struct dvb_frontend *fe, struct i2c_adapter *i2c, u8 i2c_addr,
-						  u8 override_set, u8 override_clear)
+						  u8 override_set, u8 override_clear, bool override_tone)
 {
 	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
 	return NULL;
diff --git a/drivers/media/dvb-frontends/it913x-fe.h b/drivers/media/dvb-frontends/it913x-fe.h
index 07fa4594c12..df0ad420734 100644
--- a/drivers/media/dvb-frontends/it913x-fe.h
+++ b/drivers/media/dvb-frontends/it913x-fe.h
@@ -21,6 +21,7 @@
 #ifndef IT913X_FE_H
 #define IT913X_FE_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include "dvb_frontend.h"
 
@@ -38,8 +39,7 @@ struct ite_config {
 	u8 read_slevel;
 };
 
-#if defined(CONFIG_DVB_IT913X_FE) || (defined(CONFIG_DVB_IT913X_FE_MODULE) && \
-defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_IT913X_FE)
 extern struct dvb_frontend *it913x_fe_attach(struct i2c_adapter *i2c_adap,
 			u8 i2c_addr, struct ite_config *config);
 #else
diff --git a/drivers/media/dvb-frontends/ix2505v.h b/drivers/media/dvb-frontends/ix2505v.h
index 67e89d616d5..1a735a75aa9 100644
--- a/drivers/media/dvb-frontends/ix2505v.h
+++ b/drivers/media/dvb-frontends/ix2505v.h
@@ -20,6 +20,7 @@
 #ifndef DVB_IX2505V_H
 #define DVB_IX2505V_H
 
+#include <linux/kconfig.h>
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
 
@@ -48,8 +49,7 @@ struct ix2505v_config {
 
 };
 
-#if defined(CONFIG_DVB_IX2505V) || \
-	(defined(CONFIG_DVB_IX2505V_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_IX2505V)
 extern struct dvb_frontend *ix2505v_attach(struct dvb_frontend *fe,
 	const struct ix2505v_config *config, struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/lg2160.h b/drivers/media/dvb-frontends/lg2160.h
index 9e2c0f41199..194a07a78dc 100644
--- a/drivers/media/dvb-frontends/lg2160.h
+++ b/drivers/media/dvb-frontends/lg2160.h
@@ -22,6 +22,7 @@
 #ifndef _LG2160_H_
 #define _LG2160_H_
 
+#include <linux/kconfig.h>
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
 
@@ -56,18 +57,17 @@ struct lg2160_config {
 	u16 if_khz;
 
 	/* disable i2c repeater - 0:repeater enabled 1:repeater disabled */
-	int deny_i2c_rptr:1;
+	unsigned int deny_i2c_rptr:1;
 
 	/* spectral inversion - 0:disabled 1:enabled */
-	int spectral_inversion:1;
+	unsigned 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))
+#if IS_ENABLED(CONFIG_DVB_LG2160)
 extern
 struct dvb_frontend *lg2160_attach(const struct lg2160_config *config,
 				     struct i2c_adapter *i2c_adap);
diff --git a/drivers/media/dvb-frontends/lgdt3305.h b/drivers/media/dvb-frontends/lgdt3305.h
index 02172eca4d4..d9ab556c1b2 100644
--- a/drivers/media/dvb-frontends/lgdt3305.h
+++ b/drivers/media/dvb-frontends/lgdt3305.h
@@ -22,6 +22,7 @@
 #ifndef _LGDT3305_H_
 #define _LGDT3305_H_
 
+#include <linux/kconfig.h>
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
 
@@ -73,8 +74,7 @@ struct lgdt3305_config {
 	enum lgdt_demod_chip_type demod_chip;
 };
 
-#if defined(CONFIG_DVB_LGDT3305) || (defined(CONFIG_DVB_LGDT3305_MODULE) && \
-				     defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_LGDT3305)
 extern
 struct dvb_frontend *lgdt3305_attach(const struct lgdt3305_config *config,
 				     struct i2c_adapter *i2c_adap);
diff --git a/drivers/media/dvb-frontends/lgs8gl5.h b/drivers/media/dvb-frontends/lgs8gl5.h
index d14176787a7..c2da5961472 100644
--- a/drivers/media/dvb-frontends/lgs8gl5.h
+++ b/drivers/media/dvb-frontends/lgs8gl5.h
@@ -23,6 +23,7 @@
 #ifndef LGS8GL5_H
 #define LGS8GL5_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct lgs8gl5_config {
@@ -30,8 +31,7 @@ struct lgs8gl5_config {
 	u8 demod_address;
 };
 
-#if defined(CONFIG_DVB_LGS8GL5) || \
-	(defined(CONFIG_DVB_LGS8GL5_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_LGS8GL5)
 extern struct dvb_frontend *lgs8gl5_attach(
 	const struct lgs8gl5_config *config, struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/lgs8gxx.h b/drivers/media/dvb-frontends/lgs8gxx.h
index 33c3c5e162f..dadb78bf61a 100644
--- a/drivers/media/dvb-frontends/lgs8gxx.h
+++ b/drivers/media/dvb-frontends/lgs8gxx.h
@@ -26,6 +26,7 @@
 #ifndef __LGS8GXX_H__
 #define __LGS8GXX_H__
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include <linux/i2c.h>
 
@@ -79,8 +80,7 @@ struct lgs8gxx_config {
 	u8 tuner_address;
 };
 
-#if defined(CONFIG_DVB_LGS8GXX) || \
-	(defined(CONFIG_DVB_LGS8GXX_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_LGS8GXX)
 extern struct dvb_frontend *lgs8gxx_attach(const struct lgs8gxx_config *config,
 					   struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/lnbh24.h b/drivers/media/dvb-frontends/lnbh24.h
index c059b165318..b327a4f31d1 100644
--- a/drivers/media/dvb-frontends/lnbh24.h
+++ b/drivers/media/dvb-frontends/lnbh24.h
@@ -23,6 +23,8 @@
 #ifndef _LNBH24_H
 #define _LNBH24_H
 
+#include <linux/kconfig.h>
+
 /* system register bits */
 #define LNBH24_OLF	0x01
 #define LNBH24_OTF	0x02
@@ -35,8 +37,7 @@
 
 #include <linux/dvb/frontend.h>
 
-#if defined(CONFIG_DVB_LNBP21) || (defined(CONFIG_DVB_LNBP21_MODULE) \
-							&& defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_LNBP21)
 /* override_set and override_clear control which
    system register bits (above) to always set & clear */
 extern struct dvb_frontend *lnbh24_attach(struct dvb_frontend *fe,
diff --git a/drivers/media/dvb-frontends/lnbp21.h b/drivers/media/dvb-frontends/lnbp21.h
index fcdf1c650dd..dbcbcc2f20a 100644
--- a/drivers/media/dvb-frontends/lnbp21.h
+++ b/drivers/media/dvb-frontends/lnbp21.h
@@ -27,6 +27,8 @@
 #ifndef _LNBP21_H
 #define _LNBP21_H
 
+#include <linux/kconfig.h>
+
 /* system register bits */
 /* [RO] 0=OK; 1=over current limit flag */
 #define LNBP21_OLF	0x01
@@ -55,8 +57,7 @@
 
 #include <linux/dvb/frontend.h>
 
-#if defined(CONFIG_DVB_LNBP21) || (defined(CONFIG_DVB_LNBP21_MODULE) \
-							&& defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_LNBP21)
 /* override_set and override_clear control which
  system register bits (above) to always set & clear */
 extern struct dvb_frontend *lnbp21_attach(struct dvb_frontend *fe,
diff --git a/drivers/media/dvb-frontends/lnbp22.h b/drivers/media/dvb-frontends/lnbp22.h
index 63e2dec7e68..63861b311dd 100644
--- a/drivers/media/dvb-frontends/lnbp22.h
+++ b/drivers/media/dvb-frontends/lnbp22.h
@@ -28,6 +28,8 @@
 #ifndef _LNBP22_H
 #define _LNBP22_H
 
+#include <linux/kconfig.h>
+
 /* Enable */
 #define LNBP22_EN	  0x10
 /* Voltage selection */
@@ -37,8 +39,7 @@
 
 #include <linux/dvb/frontend.h>
 
-#if defined(CONFIG_DVB_LNBP22) || \
-		(defined(CONFIG_DVB_LNBP22_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_LNBP22)
 /*
  * override_set and override_clear control which system register bits (above)
  * to always set & clear
diff --git a/drivers/media/dvb-frontends/m88rs2000.h b/drivers/media/dvb-frontends/m88rs2000.h
index 5a8023e5a4b..14ce31e76ae 100644
--- a/drivers/media/dvb-frontends/m88rs2000.h
+++ b/drivers/media/dvb-frontends/m88rs2000.h
@@ -20,6 +20,7 @@
 #ifndef M88RS2000_H
 #define M88RS2000_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include "dvb_frontend.h"
 
@@ -40,8 +41,7 @@ enum {
 	CALL_IS_READ,
 };
 
-#if defined(CONFIG_DVB_M88RS2000) || (defined(CONFIG_DVB_M88RS2000_MODULE) && \
-							defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_M88RS2000)
 extern struct dvb_frontend *m88rs2000_attach(
 	const struct m88rs2000_config *config, struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/mb86a20s.c b/drivers/media/dvb-frontends/mb86a20s.c
index f19cd736704..856374bd367 100644
--- a/drivers/media/dvb-frontends/mb86a20s.c
+++ b/drivers/media/dvb-frontends/mb86a20s.c
@@ -20,10 +20,24 @@
 #include "dvb_frontend.h"
 #include "mb86a20s.h"
 
+#define NUM_LAYERS 3
+
 static int debug = 1;
 module_param(debug, int, 0644);
 MODULE_PARM_DESC(debug, "Activates frontend debugging (default:0)");
 
+enum mb86a20s_bandwidth {
+	MB86A20S_13SEG = 0,
+	MB86A20S_13SEG_PARTIAL = 1,
+	MB86A20S_1SEG = 2,
+	MB86A20S_3SEG = 3,
+};
+
+u8 mb86a20s_subchannel[] = {
+	0xb0, 0xc0, 0xd0, 0xe0,
+	0xf0, 0x00, 0x10, 0x20,
+};
+
 struct mb86a20s_state {
 	struct i2c_adapter *i2c;
 	const struct mb86a20s_config *config;
@@ -31,7 +45,13 @@ struct mb86a20s_state {
 
 	struct dvb_frontend frontend;
 
-	u32 estimated_rate[3];
+	u32 if_freq;
+	enum mb86a20s_bandwidth bw;
+	bool inversion;
+	u32 subchannel;
+
+	u32 estimated_rate[NUM_LAYERS];
+	unsigned long get_strength_time;
 
 	bool need_init;
 };
@@ -47,35 +67,33 @@ struct regdata {
  * Initialization sequence: Use whatevere default values that PV SBTVD
  * does on its initialisation, obtained via USB snoop
  */
-static struct regdata mb86a20s_init[] = {
+static struct regdata mb86a20s_init1[] = {
 	{ 0x70, 0x0f },
 	{ 0x70, 0xff },
 	{ 0x08, 0x01 },
-	{ 0x09, 0x3e },
-	{ 0x50, 0xd1 }, { 0x51, 0x22 },
-	{ 0x39, 0x01 },
-	{ 0x71, 0x00 },
-	{ 0x28, 0x2a }, { 0x29, 0x00 }, { 0x2a, 0xff }, { 0x2b, 0x80 },
-	{ 0x28, 0x20 }, { 0x29, 0x33 }, { 0x2a, 0xdf }, { 0x2b, 0xa9 },
+	{ 0x50, 0xd1 }, { 0x51, 0x20 },
+};
+
+static struct regdata mb86a20s_init2[] = {
 	{ 0x28, 0x22 }, { 0x29, 0x00 }, { 0x2a, 0x1f }, { 0x2b, 0xf0 },
 	{ 0x3b, 0x21 },
-	{ 0x3c, 0x3a },
+	{ 0x3c, 0x38 },
 	{ 0x01, 0x0d },
-	{ 0x04, 0x08 }, { 0x05, 0x05 },
+	{ 0x04, 0x08 }, { 0x05, 0x03 },
 	{ 0x04, 0x0e }, { 0x05, 0x00 },
-	{ 0x04, 0x0f }, { 0x05, 0x14 },
-	{ 0x04, 0x0b }, { 0x05, 0x8c },
+	{ 0x04, 0x0f }, { 0x05, 0x37 },
+	{ 0x04, 0x0b }, { 0x05, 0x78 },
 	{ 0x04, 0x00 }, { 0x05, 0x00 },
-	{ 0x04, 0x01 }, { 0x05, 0x07 },
-	{ 0x04, 0x02 }, { 0x05, 0x0f },
-	{ 0x04, 0x03 }, { 0x05, 0xa0 },
+	{ 0x04, 0x01 }, { 0x05, 0x1e },
+	{ 0x04, 0x02 }, { 0x05, 0x07 },
+	{ 0x04, 0x03 }, { 0x05, 0xd0 },
 	{ 0x04, 0x09 }, { 0x05, 0x00 },
 	{ 0x04, 0x0a }, { 0x05, 0xff },
-	{ 0x04, 0x27 }, { 0x05, 0x64 },
+	{ 0x04, 0x27 }, { 0x05, 0x00 },
 	{ 0x04, 0x28 }, { 0x05, 0x00 },
-	{ 0x04, 0x1e }, { 0x05, 0xff },
-	{ 0x04, 0x29 }, { 0x05, 0x0a },
-	{ 0x04, 0x32 }, { 0x05, 0x0a },
+	{ 0x04, 0x1e }, { 0x05, 0x00 },
+	{ 0x04, 0x29 }, { 0x05, 0x64 },
+	{ 0x04, 0x32 }, { 0x05, 0x02 },
 	{ 0x04, 0x14 }, { 0x05, 0x02 },
 	{ 0x04, 0x04 }, { 0x05, 0x00 },
 	{ 0x04, 0x05 }, { 0x05, 0x22 },
@@ -142,39 +160,39 @@ static struct regdata mb86a20s_init[] = {
 	{ 0x50, 0xd5 }, { 0x51, 0x01 },		/* Serial */
 	{ 0x50, 0xd6 }, { 0x51, 0x1f },
 	{ 0x50, 0xd2 }, { 0x51, 0x03 },
-	{ 0x50, 0xd7 }, { 0x51, 0x3f },
-	{ 0x28, 0x74 }, { 0x29, 0x00 }, { 0x28, 0x74 }, { 0x29, 0x40 },
-	{ 0x28, 0x46 }, { 0x29, 0x2c }, { 0x28, 0x46 }, { 0x29, 0x0c },
+	{ 0x50, 0xd7 }, { 0x51, 0xbf },
+	{ 0x28, 0x74 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0xff },
+	{ 0x28, 0x46 }, { 0x29, 0x00 }, { 0x2a, 0x1a }, { 0x2b, 0x0c },
 
 	{ 0x04, 0x40 }, { 0x05, 0x00 },
-	{ 0x28, 0x00 }, { 0x29, 0x10 },
-	{ 0x28, 0x05 }, { 0x29, 0x02 },
+	{ 0x28, 0x00 }, { 0x2b, 0x08 },
+	{ 0x28, 0x05 }, { 0x2b, 0x00 },
 	{ 0x1c, 0x01 },
-	{ 0x28, 0x06 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x03 },
-	{ 0x28, 0x07 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x0d },
-	{ 0x28, 0x08 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x02 },
-	{ 0x28, 0x09 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x01 },
-	{ 0x28, 0x0a }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x21 },
-	{ 0x28, 0x0b }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x29 },
-	{ 0x28, 0x0c }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x16 },
-	{ 0x28, 0x0d }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x31 },
-	{ 0x28, 0x0e }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x0e },
-	{ 0x28, 0x0f }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x4e },
-	{ 0x28, 0x10 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x46 },
-	{ 0x28, 0x11 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x0f },
-	{ 0x28, 0x12 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x56 },
-	{ 0x28, 0x13 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x35 },
-	{ 0x28, 0x14 }, { 0x29, 0x00 }, { 0x2a, 0x01 }, { 0x2b, 0xbe },
-	{ 0x28, 0x15 }, { 0x29, 0x00 }, { 0x2a, 0x01 }, { 0x2b, 0x84 },
-	{ 0x28, 0x16 }, { 0x29, 0x00 }, { 0x2a, 0x03 }, { 0x2b, 0xee },
-	{ 0x28, 0x17 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x98 },
-	{ 0x28, 0x18 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x9f },
-	{ 0x28, 0x19 }, { 0x29, 0x00 }, { 0x2a, 0x07 }, { 0x2b, 0xb2 },
-	{ 0x28, 0x1a }, { 0x29, 0x00 }, { 0x2a, 0x06 }, { 0x2b, 0xc2 },
-	{ 0x28, 0x1b }, { 0x29, 0x00 }, { 0x2a, 0x07 }, { 0x2b, 0x4a },
-	{ 0x28, 0x1c }, { 0x29, 0x00 }, { 0x2a, 0x01 }, { 0x2b, 0xbc },
-	{ 0x28, 0x1d }, { 0x29, 0x00 }, { 0x2a, 0x04 }, { 0x2b, 0xba },
-	{ 0x28, 0x1e }, { 0x29, 0x00 }, { 0x2a, 0x06 }, { 0x2b, 0x14 },
+	{ 0x28, 0x06 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x1f },
+	{ 0x28, 0x07 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x18 },
+	{ 0x28, 0x08 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x12 },
+	{ 0x28, 0x09 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x30 },
+	{ 0x28, 0x0a }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x37 },
+	{ 0x28, 0x0b }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x02 },
+	{ 0x28, 0x0c }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x09 },
+	{ 0x28, 0x0d }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x06 },
+	{ 0x28, 0x0e }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x7b },
+	{ 0x28, 0x0f }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x76 },
+	{ 0x28, 0x10 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x7d },
+	{ 0x28, 0x11 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x08 },
+	{ 0x28, 0x12 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x0b },
+	{ 0x28, 0x13 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x00 },
+	{ 0x28, 0x14 }, { 0x29, 0x00 }, { 0x2a, 0x01 }, { 0x2b, 0xf2 },
+	{ 0x28, 0x15 }, { 0x29, 0x00 }, { 0x2a, 0x01 }, { 0x2b, 0xf3 },
+	{ 0x28, 0x16 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x05 },
+	{ 0x28, 0x17 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x16 },
+	{ 0x28, 0x18 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x0f },
+	{ 0x28, 0x19 }, { 0x29, 0x00 }, { 0x2a, 0x07 }, { 0x2b, 0xef },
+	{ 0x28, 0x1a }, { 0x29, 0x00 }, { 0x2a, 0x07 }, { 0x2b, 0xd8 },
+	{ 0x28, 0x1b }, { 0x29, 0x00 }, { 0x2a, 0x07 }, { 0x2b, 0xf1 },
+	{ 0x28, 0x1c }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x3d },
+	{ 0x28, 0x1d }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x94 },
+	{ 0x28, 0x1e }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0xba },
 	{ 0x50, 0x1e }, { 0x51, 0x5d },
 	{ 0x50, 0x22 }, { 0x51, 0x00 },
 	{ 0x50, 0x23 }, { 0x51, 0xc8 },
@@ -183,6 +201,8 @@ static struct regdata mb86a20s_init[] = {
 	{ 0x50, 0x26 }, { 0x51, 0x00 },
 	{ 0x50, 0x27 }, { 0x51, 0xc3 },
 	{ 0x50, 0x39 }, { 0x51, 0x02 },
+	{ 0xec, 0x0f },
+	{ 0xeb, 0x1f },
 	{ 0x28, 0x6a }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x00 },
 	{ 0xd0, 0x00 },
 };
@@ -308,15 +328,23 @@ static int mb86a20s_read_status(struct dvb_frontend *fe, fe_status_t *status)
 	dev_dbg(&state->i2c->dev, "%s: Status = 0x%02x (state = %d)\n",
 		 __func__, *status, val);
 
-	return 0;
+	return val;
 }
 
 static int mb86a20s_read_signal_strength(struct dvb_frontend *fe)
 {
 	struct mb86a20s_state *state = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int rc;
 	unsigned rf_max, rf_min, rf;
 
+	if (state->get_strength_time &&
+	   (!time_after(jiffies, state->get_strength_time)))
+		return c->strength.stat[0].uvalue;
+
+	/* Reset its value if an error happen */
+	c->strength.stat[0].uvalue = 0;
+
 	/* Does a binary search to get RF strength */
 	rf_max = 0xfff;
 	rf_min = 0;
@@ -331,7 +359,7 @@ static int mb86a20s_read_signal_strength(struct dvb_frontend *fe)
 		rc = mb86a20s_writereg(state, 0x04, 0x20);
 		if (rc < 0)
 			return rc;
-		rc = mb86a20s_writereg(state, 0x04, rf);
+		rc = mb86a20s_writereg(state, 0x05, rf);
 		if (rc < 0)
 			return rc;
 
@@ -346,15 +374,19 @@ static int mb86a20s_read_signal_strength(struct dvb_frontend *fe)
 			rf = (rf_max + rf_min) / 2;
 
 			/* Rescale it from 2^12 (4096) to 2^16 */
-			rf <<= (16 - 12);
+			rf = rf << (16 - 12);
+			if (rf)
+				rf |= (1 << 12) - 1;
+
 			dev_dbg(&state->i2c->dev,
 				"%s: signal strength = %d (%d < RF=%d < %d)\n",
 				__func__, rf, rf_min, rf >> 4, rf_max);
-			return rf;
+			c->strength.stat[0].uvalue = rf;
+			state->get_strength_time = jiffies +
+						   msecs_to_jiffies(1000);
+			return 0;
 		}
 	} while (1);
-
-	return 0;
 }
 
 static int mb86a20s_get_modulation(struct mb86a20s_state *state,
@@ -534,12 +566,13 @@ static u32 isdbt_rate[3][5][4] = {
 };
 
 static void mb86a20s_layer_bitrate(struct dvb_frontend *fe, u32 layer,
-				   u32 modulation, u32 fec, u32 interleaving,
+				   u32 modulation, u32 forward_error_correction,
+				   u32 interleaving,
 				   u32 segment)
 {
 	struct mb86a20s_state *state = fe->demodulator_priv;
 	u32 rate;
-	int m, f, i;
+	int mod, fec, guard;
 
 	/*
 	 * If modulation/fec/interleaving is not detected, the default is
@@ -550,54 +583,54 @@ static void mb86a20s_layer_bitrate(struct dvb_frontend *fe, u32 layer,
 	case DQPSK:
 	case QPSK:
 	default:
-		m = 0;
+		mod = 0;
 		break;
 	case QAM_16:
-		m = 1;
+		mod = 1;
 		break;
 	case QAM_64:
-		m = 2;
+		mod = 2;
 		break;
 	}
 
-	switch (fec) {
+	switch (forward_error_correction) {
 	default:
 	case FEC_1_2:
 	case FEC_AUTO:
-		f = 0;
+		fec = 0;
 		break;
 	case FEC_2_3:
-		f = 1;
+		fec = 1;
 		break;
 	case FEC_3_4:
-		f = 2;
+		fec = 2;
 		break;
 	case FEC_5_6:
-		f = 3;
+		fec = 3;
 		break;
 	case FEC_7_8:
-		f = 4;
+		fec = 4;
 		break;
 	}
 
 	switch (interleaving) {
 	default:
 	case GUARD_INTERVAL_1_4:
-		i = 0;
+		guard = 0;
 		break;
 	case GUARD_INTERVAL_1_8:
-		i = 1;
+		guard = 1;
 		break;
 	case GUARD_INTERVAL_1_16:
-		i = 2;
+		guard = 2;
 		break;
 	case GUARD_INTERVAL_1_32:
-		i = 3;
+		guard = 3;
 		break;
 	}
 
 	/* Samples BER at BER_SAMPLING_RATE seconds */
-	rate = isdbt_rate[m][f][i] * segment * BER_SAMPLING_RATE;
+	rate = isdbt_rate[mod][fec][guard] * segment * BER_SAMPLING_RATE;
 
 	/* Avoids sampling too quickly or to overflow the register */
 	if (rate < 256)
@@ -607,18 +640,18 @@ static void mb86a20s_layer_bitrate(struct dvb_frontend *fe, u32 layer,
 
 	dev_dbg(&state->i2c->dev,
 		"%s: layer %c bitrate: %d kbps; counter = %d (0x%06x)\n",
-	       __func__, 'A' + layer, segment * isdbt_rate[m][f][i]/1000,
+		__func__, 'A' + layer,
+		segment * isdbt_rate[mod][fec][guard]/1000,
 		rate, rate);
 
-	state->estimated_rate[i] = rate;
+	state->estimated_rate[layer] = rate;
 }
 
-
 static int mb86a20s_get_frontend(struct dvb_frontend *fe)
 {
 	struct mb86a20s_state *state = fe->demodulator_priv;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-	int i, rc;
+	int layer, rc;
 
 	dev_dbg(&state->i2c->dev, "%s called.\n", __func__);
 
@@ -636,43 +669,43 @@ static int mb86a20s_get_frontend(struct dvb_frontend *fe)
 
 	/* Get per-layer data */
 
-	for (i = 0; i < 3; i++) {
+	for (layer = 0; layer < NUM_LAYERS; layer++) {
 		dev_dbg(&state->i2c->dev, "%s: getting data for layer %c.\n",
-			__func__, 'A' + i);
+			__func__, 'A' + layer);
 
-		rc = mb86a20s_get_segment_count(state, i);
+		rc = mb86a20s_get_segment_count(state, layer);
 		if (rc < 0)
 			goto noperlayer_error;
 		if (rc >= 0 && rc < 14) {
-			c->layer[i].segment_count = rc;
+			c->layer[layer].segment_count = rc;
 		} else {
-			c->layer[i].segment_count = 0;
-			state->estimated_rate[i] = 0;
+			c->layer[layer].segment_count = 0;
+			state->estimated_rate[layer] = 0;
 			continue;
 		}
-		c->isdbt_layer_enabled |= 1 << i;
-		rc = mb86a20s_get_modulation(state, i);
+		c->isdbt_layer_enabled |= 1 << layer;
+		rc = mb86a20s_get_modulation(state, layer);
 		if (rc < 0)
 			goto noperlayer_error;
 		dev_dbg(&state->i2c->dev, "%s: modulation %d.\n",
 			__func__, rc);
-		c->layer[i].modulation = rc;
-		rc = mb86a20s_get_fec(state, i);
+		c->layer[layer].modulation = rc;
+		rc = mb86a20s_get_fec(state, layer);
 		if (rc < 0)
 			goto noperlayer_error;
 		dev_dbg(&state->i2c->dev, "%s: FEC %d.\n",
 			__func__, rc);
-		c->layer[i].fec = rc;
-		rc = mb86a20s_get_interleaving(state, i);
+		c->layer[layer].fec = rc;
+		rc = mb86a20s_get_interleaving(state, layer);
 		if (rc < 0)
 			goto noperlayer_error;
 		dev_dbg(&state->i2c->dev, "%s: interleaving %d.\n",
 			__func__, rc);
-		c->layer[i].interleaving = rc;
-		mb86a20s_layer_bitrate(fe, i, c->layer[i].modulation,
-				       c->layer[i].fec,
-				       c->layer[i].interleaving,
-				       c->layer[i].segment_count);
+		c->layer[layer].interleaving = rc;
+		mb86a20s_layer_bitrate(fe, layer, c->layer[layer].modulation,
+				       c->layer[layer].fec,
+				       c->layer[layer].interleaving,
+				       c->layer[layer].segment_count);
 	}
 
 	rc = mb86a20s_writereg(state, 0x6d, 0x84);
@@ -735,7 +768,6 @@ static int mb86a20s_reset_counters(struct dvb_frontend *fe)
 
 	/* Reset the counters, if the channel changed */
 	if (state->last_frequency != c->frequency) {
-		memset(&c->strength, 0, sizeof(c->strength));
 		memset(&c->cnr, 0, sizeof(c->cnr));
 		memset(&c->pre_bit_error, 0, sizeof(c->pre_bit_error));
 		memset(&c->pre_bit_count, 0, sizeof(c->pre_bit_count));
@@ -799,7 +831,7 @@ static int mb86a20s_get_pre_ber(struct dvb_frontend *fe,
 
 	dev_dbg(&state->i2c->dev, "%s called.\n", __func__);
 
-	if (layer >= 3)
+	if (layer >= NUM_LAYERS)
 		return -EINVAL;
 
 	/* Check if the BER measures are already available */
@@ -933,7 +965,7 @@ static int mb86a20s_get_post_ber(struct dvb_frontend *fe,
 
 	dev_dbg(&state->i2c->dev, "%s called.\n", __func__);
 
-	if (layer >= 3)
+	if (layer >= NUM_LAYERS)
 		return -EINVAL;
 
 	/* Check if the BER measures are already available */
@@ -1060,7 +1092,7 @@ static int mb86a20s_get_blk_error(struct dvb_frontend *fe,
 	u32 collect_rate;
 	dev_dbg(&state->i2c->dev, "%s called.\n", __func__);
 
-	if (layer >= 3)
+	if (layer >= NUM_LAYERS)
 		return -EINVAL;
 
 	/* Check if the PER measures are already available */
@@ -1095,7 +1127,7 @@ static int mb86a20s_get_blk_error(struct dvb_frontend *fe,
 	if (rc < 0)
 		return rc;
 	*error |= rc;
-	dev_err(&state->i2c->dev, "%s: block error for layer %c: %d.\n",
+	dev_dbg(&state->i2c->dev, "%s: block error for layer %c: %d.\n",
 		__func__, 'A' + layer, *error);
 
 	/* Read Bit Count */
@@ -1386,7 +1418,7 @@ static int mb86a20s_get_main_CNR(struct dvb_frontend *fe)
 		return rc;
 
 	if (!(rc & 0x40)) {
-		dev_info(&state->i2c->dev, "%s: CNR is not available yet.\n",
+		dev_dbg(&state->i2c->dev, "%s: CNR is not available yet.\n",
 			 __func__);
 		return -EBUSY;
 	}
@@ -1425,7 +1457,7 @@ static int mb86a20s_get_blk_error_layer_CNR(struct dvb_frontend *fe)
 	struct mb86a20s_state *state = fe->demodulator_priv;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	u32 mer, cnr;
-	int rc, val, i;
+	int rc, val, layer;
 	struct linear_segments *segs;
 	unsigned segs_len;
 
@@ -1441,33 +1473,33 @@ static int mb86a20s_get_blk_error_layer_CNR(struct dvb_frontend *fe)
 
 	/* Check if data is available */
 	if (!(rc & 0x01)) {
-		dev_info(&state->i2c->dev,
+		dev_dbg(&state->i2c->dev,
 			"%s: MER measures aren't available yet.\n", __func__);
 		return -EBUSY;
 	}
 
 	/* Read all layers */
-	for (i = 0; i < 3; i++) {
-		if (!(c->isdbt_layer_enabled & (1 << i))) {
-			c->cnr.stat[1 + i].scale = FE_SCALE_NOT_AVAILABLE;
+	for (layer = 0; layer < NUM_LAYERS; layer++) {
+		if (!(c->isdbt_layer_enabled & (1 << layer))) {
+			c->cnr.stat[1 + layer].scale = FE_SCALE_NOT_AVAILABLE;
 			continue;
 		}
 
-		rc = mb86a20s_writereg(state, 0x50, 0x52 + i * 3);
+		rc = mb86a20s_writereg(state, 0x50, 0x52 + layer * 3);
 		if (rc < 0)
 			return rc;
 		rc = mb86a20s_readreg(state, 0x51);
 		if (rc < 0)
 			return rc;
 		mer = rc << 16;
-		rc = mb86a20s_writereg(state, 0x50, 0x53 + i * 3);
+		rc = mb86a20s_writereg(state, 0x50, 0x53 + layer * 3);
 		if (rc < 0)
 			return rc;
 		rc = mb86a20s_readreg(state, 0x51);
 		if (rc < 0)
 			return rc;
 		mer |= rc << 8;
-		rc = mb86a20s_writereg(state, 0x50, 0x54 + i * 3);
+		rc = mb86a20s_writereg(state, 0x50, 0x54 + layer * 3);
 		if (rc < 0)
 			return rc;
 		rc = mb86a20s_readreg(state, 0x51);
@@ -1475,7 +1507,7 @@ static int mb86a20s_get_blk_error_layer_CNR(struct dvb_frontend *fe)
 			return rc;
 		mer |= rc;
 
-		switch (c->layer[i].modulation) {
+		switch (c->layer[layer].modulation) {
 		case DQPSK:
 		case QPSK:
 			segs = cnr_qpsk_table;
@@ -1493,12 +1525,12 @@ static int mb86a20s_get_blk_error_layer_CNR(struct dvb_frontend *fe)
 		}
 		cnr = interpolate_value(mer, segs, segs_len);
 
-		c->cnr.stat[1 + i].scale = FE_SCALE_DECIBEL;
-		c->cnr.stat[1 + i].svalue = cnr;
+		c->cnr.stat[1 + layer].scale = FE_SCALE_DECIBEL;
+		c->cnr.stat[1 + layer].svalue = cnr;
 
 		dev_dbg(&state->i2c->dev,
 			"%s: CNR for layer %c is %d.%03d dB (MER = %d).\n",
-			__func__, 'A' + i, cnr / 1000, cnr % 1000, mer);
+			__func__, 'A' + layer, cnr / 1000, cnr % 1000, mer);
 
 	}
 
@@ -1526,7 +1558,7 @@ static void mb86a20s_stats_not_ready(struct dvb_frontend *fe)
 {
 	struct mb86a20s_state *state = fe->demodulator_priv;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-	int i;
+	int layer;
 
 	dev_dbg(&state->i2c->dev, "%s called.\n", __func__);
 
@@ -1536,35 +1568,35 @@ static void mb86a20s_stats_not_ready(struct dvb_frontend *fe)
 	c->strength.len = 1;
 
 	/* Per-layer stats - 3 layers + global */
-	c->cnr.len = 4;
-	c->pre_bit_error.len = 4;
-	c->pre_bit_count.len = 4;
-	c->post_bit_error.len = 4;
-	c->post_bit_count.len = 4;
-	c->block_error.len = 4;
-	c->block_count.len = 4;
+	c->cnr.len = NUM_LAYERS + 1;
+	c->pre_bit_error.len = NUM_LAYERS + 1;
+	c->pre_bit_count.len = NUM_LAYERS + 1;
+	c->post_bit_error.len = NUM_LAYERS + 1;
+	c->post_bit_count.len = NUM_LAYERS + 1;
+	c->block_error.len = NUM_LAYERS + 1;
+	c->block_count.len = NUM_LAYERS + 1;
 
 	/* Signal is always available */
 	c->strength.stat[0].scale = FE_SCALE_RELATIVE;
 	c->strength.stat[0].uvalue = 0;
 
 	/* Put all of them at FE_SCALE_NOT_AVAILABLE */
-	for (i = 0; i < 4; i++) {
-		c->cnr.stat[i].scale = FE_SCALE_NOT_AVAILABLE;
-		c->pre_bit_error.stat[i].scale = FE_SCALE_NOT_AVAILABLE;
-		c->pre_bit_count.stat[i].scale = FE_SCALE_NOT_AVAILABLE;
-		c->post_bit_error.stat[i].scale = FE_SCALE_NOT_AVAILABLE;
-		c->post_bit_count.stat[i].scale = FE_SCALE_NOT_AVAILABLE;
-		c->block_error.stat[i].scale = FE_SCALE_NOT_AVAILABLE;
-		c->block_count.stat[i].scale = FE_SCALE_NOT_AVAILABLE;
+	for (layer = 0; layer < NUM_LAYERS + 1; layer++) {
+		c->cnr.stat[layer].scale = FE_SCALE_NOT_AVAILABLE;
+		c->pre_bit_error.stat[layer].scale = FE_SCALE_NOT_AVAILABLE;
+		c->pre_bit_count.stat[layer].scale = FE_SCALE_NOT_AVAILABLE;
+		c->post_bit_error.stat[layer].scale = FE_SCALE_NOT_AVAILABLE;
+		c->post_bit_count.stat[layer].scale = FE_SCALE_NOT_AVAILABLE;
+		c->block_error.stat[layer].scale = FE_SCALE_NOT_AVAILABLE;
+		c->block_count.stat[layer].scale = FE_SCALE_NOT_AVAILABLE;
 	}
 }
 
-static int mb86a20s_get_stats(struct dvb_frontend *fe)
+static int mb86a20s_get_stats(struct dvb_frontend *fe, int status_nr)
 {
 	struct mb86a20s_state *state = fe->demodulator_priv;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-	int rc = 0, i;
+	int rc = 0, layer;
 	u32 bit_error = 0, bit_count = 0;
 	u32 t_pre_bit_error = 0, t_pre_bit_count = 0;
 	u32 t_post_bit_error = 0, t_post_bit_count = 0;
@@ -1580,90 +1612,98 @@ static int mb86a20s_get_stats(struct dvb_frontend *fe)
 	/* Get per-layer stats */
 	mb86a20s_get_blk_error_layer_CNR(fe);
 
-	for (i = 0; i < 3; i++) {
-		if (c->isdbt_layer_enabled & (1 << i)) {
+	/*
+	 * At state 7, only CNR is available
+	 * For BER measures, state=9 is required
+	 * FIXME: we may get MER measures with state=8
+	 */
+	if (status_nr < 9)
+		return 0;
+
+	for (layer = 0; layer < NUM_LAYERS; layer++) {
+		if (c->isdbt_layer_enabled & (1 << layer)) {
 			/* Layer is active and has rc segments */
 			active_layers++;
 
 			/* Handle BER before vterbi */
-			rc = mb86a20s_get_pre_ber(fe, i,
+			rc = mb86a20s_get_pre_ber(fe, layer,
 						  &bit_error, &bit_count);
 			if (rc >= 0) {
-				c->pre_bit_error.stat[1 + i].scale = FE_SCALE_COUNTER;
-				c->pre_bit_error.stat[1 + i].uvalue += bit_error;
-				c->pre_bit_count.stat[1 + i].scale = FE_SCALE_COUNTER;
-				c->pre_bit_count.stat[1 + i].uvalue += bit_count;
+				c->pre_bit_error.stat[1 + layer].scale = FE_SCALE_COUNTER;
+				c->pre_bit_error.stat[1 + layer].uvalue += bit_error;
+				c->pre_bit_count.stat[1 + layer].scale = FE_SCALE_COUNTER;
+				c->pre_bit_count.stat[1 + layer].uvalue += bit_count;
 			} else if (rc != -EBUSY) {
 				/*
 					* If an I/O error happened,
 					* measures are now unavailable
 					*/
-				c->pre_bit_error.stat[1 + i].scale = FE_SCALE_NOT_AVAILABLE;
-				c->pre_bit_count.stat[1 + i].scale = FE_SCALE_NOT_AVAILABLE;
+				c->pre_bit_error.stat[1 + layer].scale = FE_SCALE_NOT_AVAILABLE;
+				c->pre_bit_count.stat[1 + layer].scale = FE_SCALE_NOT_AVAILABLE;
 				dev_err(&state->i2c->dev,
 					"%s: Can't get BER for layer %c (error %d).\n",
-					__func__, 'A' + i, rc);
+					__func__, 'A' + layer, rc);
 			}
-			if (c->block_error.stat[1 + i].scale != FE_SCALE_NOT_AVAILABLE)
+			if (c->block_error.stat[1 + layer].scale != FE_SCALE_NOT_AVAILABLE)
 				pre_ber_layers++;
 
 			/* Handle BER post vterbi */
-			rc = mb86a20s_get_post_ber(fe, i,
+			rc = mb86a20s_get_post_ber(fe, layer,
 						   &bit_error, &bit_count);
 			if (rc >= 0) {
-				c->post_bit_error.stat[1 + i].scale = FE_SCALE_COUNTER;
-				c->post_bit_error.stat[1 + i].uvalue += bit_error;
-				c->post_bit_count.stat[1 + i].scale = FE_SCALE_COUNTER;
-				c->post_bit_count.stat[1 + i].uvalue += bit_count;
+				c->post_bit_error.stat[1 + layer].scale = FE_SCALE_COUNTER;
+				c->post_bit_error.stat[1 + layer].uvalue += bit_error;
+				c->post_bit_count.stat[1 + layer].scale = FE_SCALE_COUNTER;
+				c->post_bit_count.stat[1 + layer].uvalue += bit_count;
 			} else if (rc != -EBUSY) {
 				/*
 					* If an I/O error happened,
 					* measures are now unavailable
 					*/
-				c->post_bit_error.stat[1 + i].scale = FE_SCALE_NOT_AVAILABLE;
-				c->post_bit_count.stat[1 + i].scale = FE_SCALE_NOT_AVAILABLE;
+				c->post_bit_error.stat[1 + layer].scale = FE_SCALE_NOT_AVAILABLE;
+				c->post_bit_count.stat[1 + layer].scale = FE_SCALE_NOT_AVAILABLE;
 				dev_err(&state->i2c->dev,
 					"%s: Can't get BER for layer %c (error %d).\n",
-					__func__, 'A' + i, rc);
+					__func__, 'A' + layer, rc);
 			}
-			if (c->block_error.stat[1 + i].scale != FE_SCALE_NOT_AVAILABLE)
+			if (c->block_error.stat[1 + layer].scale != FE_SCALE_NOT_AVAILABLE)
 				post_ber_layers++;
 
 			/* Handle Block errors for PER/UCB reports */
-			rc = mb86a20s_get_blk_error(fe, i,
+			rc = mb86a20s_get_blk_error(fe, layer,
 						&block_error,
 						&block_count);
 			if (rc >= 0) {
-				c->block_error.stat[1 + i].scale = FE_SCALE_COUNTER;
-				c->block_error.stat[1 + i].uvalue += block_error;
-				c->block_count.stat[1 + i].scale = FE_SCALE_COUNTER;
-				c->block_count.stat[1 + i].uvalue += block_count;
+				c->block_error.stat[1 + layer].scale = FE_SCALE_COUNTER;
+				c->block_error.stat[1 + layer].uvalue += block_error;
+				c->block_count.stat[1 + layer].scale = FE_SCALE_COUNTER;
+				c->block_count.stat[1 + layer].uvalue += block_count;
 			} else if (rc != -EBUSY) {
 				/*
 					* If an I/O error happened,
 					* measures are now unavailable
 					*/
-				c->block_error.stat[1 + i].scale = FE_SCALE_NOT_AVAILABLE;
-				c->block_count.stat[1 + i].scale = FE_SCALE_NOT_AVAILABLE;
+				c->block_error.stat[1 + layer].scale = FE_SCALE_NOT_AVAILABLE;
+				c->block_count.stat[1 + layer].scale = FE_SCALE_NOT_AVAILABLE;
 				dev_err(&state->i2c->dev,
 					"%s: Can't get PER for layer %c (error %d).\n",
-					__func__, 'A' + i, rc);
+					__func__, 'A' + layer, rc);
 
 			}
-			if (c->block_error.stat[1 + i].scale != FE_SCALE_NOT_AVAILABLE)
+			if (c->block_error.stat[1 + layer].scale != FE_SCALE_NOT_AVAILABLE)
 				per_layers++;
 
 			/* Update total preBER */
-			t_pre_bit_error += c->pre_bit_error.stat[1 + i].uvalue;
-			t_pre_bit_count += c->pre_bit_count.stat[1 + i].uvalue;
+			t_pre_bit_error += c->pre_bit_error.stat[1 + layer].uvalue;
+			t_pre_bit_count += c->pre_bit_count.stat[1 + layer].uvalue;
 
 			/* Update total postBER */
-			t_post_bit_error += c->post_bit_error.stat[1 + i].uvalue;
-			t_post_bit_count += c->post_bit_count.stat[1 + i].uvalue;
+			t_post_bit_error += c->post_bit_error.stat[1 + layer].uvalue;
+			t_post_bit_count += c->post_bit_count.stat[1 + layer].uvalue;
 
 			/* Update total PER */
-			t_block_error += c->block_error.stat[1 + i].uvalue;
-			t_block_count += c->block_count.stat[1 + i].uvalue;
+			t_block_error += c->block_error.stat[1 + layer].uvalue;
+			t_block_count += c->block_count.stat[1 + layer].uvalue;
 		}
 	}
 
@@ -1737,8 +1777,10 @@ static int mb86a20s_get_stats(struct dvb_frontend *fe)
 static int mb86a20s_initfe(struct dvb_frontend *fe)
 {
 	struct mb86a20s_state *state = fe->demodulator_priv;
+	u64 pll;
+	u32 fclk;
 	int rc;
-	u8  regD5 = 1;
+	u8  regD5 = 1, reg71, reg09 = 0x3a;
 
 	dev_dbg(&state->i2c->dev, "%s called.\n", __func__);
 
@@ -1746,10 +1788,78 @@ static int mb86a20s_initfe(struct dvb_frontend *fe)
 		fe->ops.i2c_gate_ctrl(fe, 0);
 
 	/* Initialize the frontend */
-	rc = mb86a20s_writeregdata(state, mb86a20s_init);
+	rc = mb86a20s_writeregdata(state, mb86a20s_init1);
 	if (rc < 0)
 		goto err;
 
+	if (!state->inversion)
+		reg09 |= 0x04;
+	rc = mb86a20s_writereg(state, 0x09, reg09);
+	if (rc < 0)
+		goto err;
+	if (!state->bw)
+		reg71 = 1;
+	else
+		reg71 = 0;
+	rc = mb86a20s_writereg(state, 0x39, reg71);
+	if (rc < 0)
+		goto err;
+	rc = mb86a20s_writereg(state, 0x71, state->bw);
+	if (rc < 0)
+		goto err;
+	if (state->subchannel) {
+		rc = mb86a20s_writereg(state, 0x44, state->subchannel);
+		if (rc < 0)
+			goto err;
+	}
+
+	fclk = state->config->fclk;
+	if (!fclk)
+		fclk = 32571428;
+
+	/* Adjust IF frequency to match tuner */
+	if (fe->ops.tuner_ops.get_if_frequency)
+		fe->ops.tuner_ops.get_if_frequency(fe, &state->if_freq);
+
+	if (!state->if_freq)
+		state->if_freq = 3300000;
+
+	pll = (((u64)1) << 34) * state->if_freq;
+	do_div(pll, 63 * fclk);
+	pll = (1 << 25) - pll;
+	rc = mb86a20s_writereg(state, 0x28, 0x2a);
+	if (rc < 0)
+		goto err;
+	rc = mb86a20s_writereg(state, 0x29, (pll >> 16) & 0xff);
+	if (rc < 0)
+		goto err;
+	rc = mb86a20s_writereg(state, 0x2a, (pll >> 8) & 0xff);
+	if (rc < 0)
+		goto err;
+	rc = mb86a20s_writereg(state, 0x2b, pll & 0xff);
+	if (rc < 0)
+		goto err;
+	dev_dbg(&state->i2c->dev, "%s: fclk=%d, IF=%d, clock reg=0x%06llx\n",
+		__func__, fclk, state->if_freq, (long long)pll);
+
+	/* pll = freq[Hz] * 2^24/10^6 / 16.285714286 */
+	pll = state->if_freq * 1677721600L;
+	do_div(pll, 1628571429L);
+	rc = mb86a20s_writereg(state, 0x28, 0x20);
+	if (rc < 0)
+		goto err;
+	rc = mb86a20s_writereg(state, 0x29, (pll >> 16) & 0xff);
+	if (rc < 0)
+		goto err;
+	rc = mb86a20s_writereg(state, 0x2a, (pll >> 8) & 0xff);
+	if (rc < 0)
+		goto err;
+	rc = mb86a20s_writereg(state, 0x2b, pll & 0xff);
+	if (rc < 0)
+		goto err;
+	dev_dbg(&state->i2c->dev, "%s: IF=%d, IF reg=0x%06llx\n",
+		__func__, state->if_freq, (long long)pll);
+
 	if (!state->config->is_serial) {
 		regD5 &= ~1;
 
@@ -1761,6 +1871,11 @@ static int mb86a20s_initfe(struct dvb_frontend *fe)
 			goto err;
 	}
 
+	rc = mb86a20s_writeregdata(state, mb86a20s_init2);
+	if (rc < 0)
+		goto err;
+
+
 err:
 	if (fe->ops.i2c_gate_ctrl)
 		fe->ops.i2c_gate_ctrl(fe, 1);
@@ -1779,15 +1894,34 @@ err:
 static int mb86a20s_set_frontend(struct dvb_frontend *fe)
 {
 	struct mb86a20s_state *state = fe->demodulator_priv;
-	int rc;
-#if 0
-	/*
-	 * FIXME: Properly implement the set frontend properties
-	 */
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-#endif
+	int rc, if_freq;
 	dev_dbg(&state->i2c->dev, "%s called.\n", __func__);
 
+	if (!c->isdbt_layer_enabled)
+		c->isdbt_layer_enabled = 7;
+
+	if (c->isdbt_layer_enabled == 1)
+		state->bw = MB86A20S_1SEG;
+	else if (c->isdbt_partial_reception)
+		state->bw = MB86A20S_13SEG_PARTIAL;
+	else
+		state->bw = MB86A20S_13SEG;
+
+	if (c->inversion == INVERSION_ON)
+		state->inversion = true;
+	else
+		state->inversion = false;
+
+	if (!c->isdbt_sb_mode) {
+		state->subchannel = 0;
+	} else {
+		if (c->isdbt_sb_subchannel >= ARRAY_SIZE(mb86a20s_subchannel))
+			c->isdbt_sb_subchannel = 0;
+
+		state->subchannel = mb86a20s_subchannel[c->isdbt_sb_subchannel];
+	}
+
 	/*
 	 * Gate should already be opened, but it doesn't hurt to
 	 * double-check
@@ -1796,6 +1930,9 @@ static int mb86a20s_set_frontend(struct dvb_frontend *fe)
 		fe->ops.i2c_gate_ctrl(fe, 1);
 	fe->ops.tuner_ops.set_params(fe);
 
+	if (fe->ops.tuner_ops.get_if_frequency)
+		fe->ops.tuner_ops.get_if_frequency(fe, &if_freq);
+
 	/*
 	 * Make it more reliable: if, for some reason, the initial
 	 * device initialization doesn't happen, initialize it when
@@ -1805,15 +1942,22 @@ static int mb86a20s_set_frontend(struct dvb_frontend *fe)
 	 * the agc callback logic is not called during DVB attach time,
 	 * causing mb86a20s to not be initialized with Kworld SBTVD.
 	 * So, this hack is needed, in order to make Kworld SBTVD to work.
+	 *
+	 * It is also needed to change the IF after the initial init.
+	 *
+	 * HACK: Always init the frontend when set_frontend is called:
+	 * it was noticed that, on some devices, it fails to lock on a
+	 * different channel. So, it is better to reset everything, even
+	 * wasting some time, than to loose channel lock.
 	 */
-	if (state->need_init)
-		mb86a20s_initfe(fe);
+	mb86a20s_initfe(fe);
 
 	if (fe->ops.i2c_gate_ctrl)
 		fe->ops.i2c_gate_ctrl(fe, 0);
 
 	rc = mb86a20s_writeregdata(state, mb86a20s_reset_reception);
 	mb86a20s_reset_counters(fe);
+	mb86a20s_stats_not_ready(fe);
 
 	if (fe->ops.i2c_gate_ctrl)
 		fe->ops.i2c_gate_ctrl(fe, 1);
@@ -1825,8 +1969,7 @@ static int mb86a20s_read_status_and_stats(struct dvb_frontend *fe,
 					  fe_status_t *status)
 {
 	struct mb86a20s_state *state = fe->demodulator_priv;
-	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-	int rc;
+	int rc, status_nr;
 
 	dev_dbg(&state->i2c->dev, "%s called.\n", __func__);
 
@@ -1834,12 +1977,12 @@ static int mb86a20s_read_status_and_stats(struct dvb_frontend *fe,
 		fe->ops.i2c_gate_ctrl(fe, 0);
 
 	/* Get lock */
-	rc = mb86a20s_read_status(fe, status);
-	if (!(*status & FE_HAS_LOCK)) {
+	status_nr = mb86a20s_read_status(fe, status);
+	if (status_nr < 7) {
 		mb86a20s_stats_not_ready(fe);
 		mb86a20s_reset_frontend_cache(fe);
 	}
-	if (rc < 0) {
+	if (status_nr < 0) {
 		dev_err(&state->i2c->dev,
 			"%s: Can't read frontend lock status\n", __func__);
 		goto error;
@@ -1856,10 +1999,8 @@ static int mb86a20s_read_status_and_stats(struct dvb_frontend *fe,
 		rc = 0;		/* Status is OK */
 		goto error;
 	}
-	/* Fill signal strength */
-	c->strength.stat[0].uvalue = rc;
 
-	if (*status & FE_HAS_LOCK) {
+	if (status_nr >= 7) {
 		/* Get TMCC info*/
 		rc = mb86a20s_get_frontend(fe);
 		if (rc < 0) {
@@ -1870,7 +2011,7 @@ static int mb86a20s_read_status_and_stats(struct dvb_frontend *fe,
 		}
 
 		/* Get statistics */
-		rc = mb86a20s_get_stats(fe);
+		rc = mb86a20s_get_stats(fe, status_nr);
 		if (rc < 0 && rc != -EBUSY) {
 			dev_err(&state->i2c->dev,
 				"%s: Can't get FE statistics.\n", __func__);
@@ -1994,7 +2135,7 @@ static struct dvb_frontend_ops mb86a20s_ops = {
 	/* Use dib8000 values per default */
 	.info = {
 		.name = "Fujitsu mb86A20s",
-		.caps = FE_CAN_INVERSION_AUTO | FE_CAN_RECOVER |
+		.caps = FE_CAN_RECOVER  |
 			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 |
diff --git a/drivers/media/dvb-frontends/mb86a20s.h b/drivers/media/dvb-frontends/mb86a20s.h
index bf22e77888b..6627a397608 100644
--- a/drivers/media/dvb-frontends/mb86a20s.h
+++ b/drivers/media/dvb-frontends/mb86a20s.h
@@ -16,21 +16,25 @@
 #ifndef MB86A20S_H
 #define MB86A20S_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 /**
  * struct mb86a20s_config - Define the per-device attributes of the frontend
  *
+ * @fclk:		Clock frequency. If zero, assumes the default
+ *			(32.57142 Mhz)
  * @demod_address:	the demodulator's i2c address
+ * @is_serial:		if true, TS is serial. Otherwise, TS is parallel
  */
 
 struct mb86a20s_config {
-	u8 demod_address;
-	bool is_serial;
+	u32	fclk;
+	u8	demod_address;
+	bool	is_serial;
 };
 
-#if defined(CONFIG_DVB_MB86A20S) || (defined(CONFIG_DVB_MB86A20S_MODULE) \
-	&& defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_MB86A20S)
 extern struct dvb_frontend *mb86a20s_attach(const struct mb86a20s_config *config,
 					   struct i2c_adapter *i2c);
 extern struct i2c_adapter *mb86a20s_get_tuner_i2c_adapter(struct dvb_frontend *);
diff --git a/drivers/media/dvb-frontends/rtl2830.h b/drivers/media/dvb-frontends/rtl2830.h
index f4349a1fc03..3313847fb0b 100644
--- a/drivers/media/dvb-frontends/rtl2830.h
+++ b/drivers/media/dvb-frontends/rtl2830.h
@@ -21,6 +21,7 @@
 #ifndef RTL2830_H
 #define RTL2830_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct rtl2830_config {
@@ -59,8 +60,7 @@ struct rtl2830_config {
 	u8 agc_targ_val;
 };
 
-#if defined(CONFIG_DVB_RTL2830) || \
-	(defined(CONFIG_DVB_RTL2830_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_RTL2830)
 extern struct dvb_frontend *rtl2830_attach(
 	const struct rtl2830_config *config,
 	struct i2c_adapter *i2c
diff --git a/drivers/media/dvb-frontends/rtl2832.c b/drivers/media/dvb-frontends/rtl2832.c
index 73887690b04..facb8484151 100644
--- a/drivers/media/dvb-frontends/rtl2832.c
+++ b/drivers/media/dvb-frontends/rtl2832.c
@@ -380,13 +380,41 @@ err:
 	return ret;
 }
 
-static int rtl2832_init(struct dvb_frontend *fe)
+
+static int rtl2832_set_if(struct dvb_frontend *fe, u32 if_freq)
 {
 	struct rtl2832_priv *priv = fe->demodulator_priv;
-	int i, ret, len;
-	u8 en_bbin;
+	int ret;
 	u64 pset_iffreq;
+	u8 en_bbin = (if_freq == 0 ? 0x1 : 0x0);
+
+	/*
+	* PSET_IFFREQ = - floor((IfFreqHz % CrystalFreqHz) * pow(2, 22)
+	*		/ CrystalFreqHz)
+	*/
+
+	pset_iffreq = if_freq % priv->cfg.xtal;
+	pset_iffreq *= 0x400000;
+	pset_iffreq = div_u64(pset_iffreq, priv->cfg.xtal);
+	pset_iffreq = -pset_iffreq;
+	pset_iffreq = pset_iffreq & 0x3fffff;
+	dev_dbg(&priv->i2c->dev, "%s: if_frequency=%d pset_iffreq=%08x\n",
+			__func__, if_freq, (unsigned)pset_iffreq);
+
+	ret = rtl2832_wr_demod_reg(priv, DVBT_EN_BBIN, en_bbin);
+	if (ret)
+		return ret;
+
+	ret = rtl2832_wr_demod_reg(priv, DVBT_PSET_IFFREQ, pset_iffreq);
+
+	return (ret);
+}
+
+static int rtl2832_init(struct dvb_frontend *fe)
+{
+	struct rtl2832_priv *priv = fe->demodulator_priv;
 	const struct rtl2832_reg_value *init;
+	int i, ret, len;
 
 	/* initialization values for the demodulator registers */
 	struct rtl2832_reg_value rtl2832_initial_regs[] = {
@@ -432,22 +460,10 @@ static int rtl2832_init(struct dvb_frontend *fe)
 		{DVBT_TR_THD_SET2,		0x6},
 		{DVBT_TRK_KC_I2,		0x5},
 		{DVBT_CR_THD_SET2,		0x1},
-		{DVBT_SPEC_INV,			0x0},
 	};
 
 	dev_dbg(&priv->i2c->dev, "%s:\n", __func__);
 
-	en_bbin = (priv->cfg.if_dvbt == 0 ? 0x1 : 0x0);
-
-	/*
-	* PSET_IFFREQ = - floor((IfFreqHz % CrystalFreqHz) * pow(2, 22)
-	*		/ CrystalFreqHz)
-	*/
-	pset_iffreq = priv->cfg.if_dvbt % priv->cfg.xtal;
-	pset_iffreq *= 0x400000;
-	pset_iffreq = div_u64(pset_iffreq, priv->cfg.xtal);
-	pset_iffreq = pset_iffreq & 0x3fffff;
-
 	for (i = 0; i < ARRAY_SIZE(rtl2832_initial_regs); i++) {
 		ret = rtl2832_wr_demod_reg(priv, rtl2832_initial_regs[i].reg,
 			rtl2832_initial_regs[i].value);
@@ -472,6 +488,10 @@ static int rtl2832_init(struct dvb_frontend *fe)
 		len = ARRAY_SIZE(rtl2832_tuner_init_e4000);
 		init = rtl2832_tuner_init_e4000;
 		break;
+	case RTL2832_TUNER_R820T:
+		len = ARRAY_SIZE(rtl2832_tuner_init_r820t);
+		init = rtl2832_tuner_init_r820t;
+		break;
 	default:
 		ret = -EINVAL;
 		goto err;
@@ -483,14 +503,26 @@ static int rtl2832_init(struct dvb_frontend *fe)
 			goto err;
 	}
 
-	/* if frequency settings */
-	ret = rtl2832_wr_demod_reg(priv, DVBT_EN_BBIN, en_bbin);
+	if (!fe->ops.tuner_ops.get_if_frequency) {
+		ret = rtl2832_set_if(fe, priv->cfg.if_dvbt);
 		if (ret)
 			goto err;
+	}
 
-	ret = rtl2832_wr_demod_reg(priv, DVBT_PSET_IFFREQ, pset_iffreq);
-		if (ret)
-			goto err;
+	/*
+	 * r820t NIM code does a software reset here at the demod -
+	 * may not be needed, as there's already a software reset at set_params()
+	 */
+#if 1
+	/* soft reset */
+	ret = rtl2832_wr_demod_reg(priv, DVBT_SOFT_RST, 0x1);
+	if (ret)
+		goto err;
+
+	ret = rtl2832_wr_demod_reg(priv, DVBT_SOFT_RST, 0x0);
+	if (ret)
+		goto err;
+#endif
 
 	priv->sleeping = false;
 
@@ -564,6 +596,19 @@ static int rtl2832_set_frontend(struct dvb_frontend *fe)
 	if (fe->ops.tuner_ops.set_params)
 		fe->ops.tuner_ops.set_params(fe);
 
+	/* If the frontend has get_if_frequency(), use it */
+	if (fe->ops.tuner_ops.get_if_frequency) {
+		u32 if_freq;
+
+		ret = fe->ops.tuner_ops.get_if_frequency(fe, &if_freq);
+		if (ret)
+			goto err;
+
+		ret = rtl2832_set_if(fe, if_freq);
+		if (ret)
+			goto err;
+	}
+
 	switch (c->bandwidth_hz) {
 	case 6000000:
 		i = 0;
diff --git a/drivers/media/dvb-frontends/rtl2832.h b/drivers/media/dvb-frontends/rtl2832.h
index 785a466eb06..91b2dcf5a6e 100644
--- a/drivers/media/dvb-frontends/rtl2832.h
+++ b/drivers/media/dvb-frontends/rtl2832.h
@@ -21,6 +21,7 @@
 #ifndef RTL2832_H
 #define RTL2832_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct rtl2832_config {
@@ -51,11 +52,11 @@ struct rtl2832_config {
 #define RTL2832_TUNER_FC0012    0x26
 #define RTL2832_TUNER_E4000     0x27
 #define RTL2832_TUNER_FC0013    0x29
+#define RTL2832_TUNER_R820T	0x2a
 	u8 tuner;
 };
 
-#if defined(CONFIG_DVB_RTL2832) || \
-	(defined(CONFIG_DVB_RTL2832_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_RTL2832)
 extern struct dvb_frontend *rtl2832_attach(
 	const struct rtl2832_config *cfg,
 	struct i2c_adapter *i2c
diff --git a/drivers/media/dvb-frontends/rtl2832_priv.h b/drivers/media/dvb-frontends/rtl2832_priv.h
index 7d97ce9d219..b5f2b80092e 100644
--- a/drivers/media/dvb-frontends/rtl2832_priv.h
+++ b/drivers/media/dvb-frontends/rtl2832_priv.h
@@ -267,6 +267,7 @@ static const struct rtl2832_reg_value rtl2832_tuner_init_tua9001[] = {
 	{DVBT_OPT_ADC_IQ,                0x1},
 	{DVBT_AD_AVI,                    0x0},
 	{DVBT_AD_AVQ,                    0x0},
+	{DVBT_SPEC_INV,			 0x0},
 };
 
 static const struct rtl2832_reg_value rtl2832_tuner_init_fc0012[] = {
@@ -300,6 +301,7 @@ static const struct rtl2832_reg_value rtl2832_tuner_init_fc0012[] = {
 	{DVBT_GI_PGA_STATE,              0x0},
 	{DVBT_EN_AGC_PGA,                0x1},
 	{DVBT_IF_AGC_MAN,                0x0},
+	{DVBT_SPEC_INV,			 0x0},
 };
 
 static const struct rtl2832_reg_value rtl2832_tuner_init_e4000[] = {
@@ -337,6 +339,32 @@ static const struct rtl2832_reg_value rtl2832_tuner_init_e4000[] = {
 	{DVBT_REG_MONSEL,                0x1},
 	{DVBT_REG_MON,                   0x1},
 	{DVBT_REG_4MSEL,                 0x0},
+	{DVBT_SPEC_INV,			 0x0},
+};
+
+static const struct rtl2832_reg_value rtl2832_tuner_init_r820t[] = {
+	{DVBT_DAGC_TRG_VAL,		0x39},
+	{DVBT_AGC_TARG_VAL_0,		0x0},
+	{DVBT_AGC_TARG_VAL_8_1,		0x40},
+	{DVBT_AAGC_LOOP_GAIN,		0x16},
+	{DVBT_LOOP_GAIN2_3_0,		0x8},
+	{DVBT_LOOP_GAIN2_4,		0x1},
+	{DVBT_LOOP_GAIN3,		0x18},
+	{DVBT_VTOP1,			0x35},
+	{DVBT_VTOP2,			0x21},
+	{DVBT_VTOP3,			0x21},
+	{DVBT_KRF1,			0x0},
+	{DVBT_KRF2,			0x40},
+	{DVBT_KRF3,			0x10},
+	{DVBT_KRF4,			0x10},
+	{DVBT_IF_AGC_MIN,		0x80},
+	{DVBT_IF_AGC_MAX,		0x7f},
+	{DVBT_RF_AGC_MIN,		0x80},
+	{DVBT_RF_AGC_MAX,		0x7f},
+	{DVBT_POLAR_RF_AGC,		0x0},
+	{DVBT_POLAR_IF_AGC,		0x0},
+	{DVBT_AD7_SETTING,		0xe9f4},
+	{DVBT_SPEC_INV,			0x1},
 };
 
 #endif /* RTL2832_PRIV_H */
diff --git a/drivers/media/dvb-frontends/s5h1409.h b/drivers/media/dvb-frontends/s5h1409.h
index 91f2ebd1a53..63b1e0a34e4 100644
--- a/drivers/media/dvb-frontends/s5h1409.h
+++ b/drivers/media/dvb-frontends/s5h1409.h
@@ -22,6 +22,7 @@
 #ifndef __S5H1409_H__
 #define __S5H1409_H__
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct s5h1409_config {
@@ -66,8 +67,7 @@ struct s5h1409_config {
 	u8 hvr1600_opt;
 };
 
-#if defined(CONFIG_DVB_S5H1409) || (defined(CONFIG_DVB_S5H1409_MODULE) \
-	&& defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_S5H1409)
 extern struct dvb_frontend *s5h1409_attach(const struct s5h1409_config *config,
 					   struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/s5h1411.h b/drivers/media/dvb-frontends/s5h1411.h
index 45ec0f82989..e4f56871f98 100644
--- a/drivers/media/dvb-frontends/s5h1411.h
+++ b/drivers/media/dvb-frontends/s5h1411.h
@@ -22,6 +22,7 @@
 #ifndef __S5H1411_H__
 #define __S5H1411_H__
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 #define S5H1411_I2C_TOP_ADDR (0x32 >> 1)
@@ -68,8 +69,7 @@ struct s5h1411_config {
 	u8 status_mode;
 };
 
-#if defined(CONFIG_DVB_S5H1411) || \
-	(defined(CONFIG_DVB_S5H1411_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_S5H1411)
 extern struct dvb_frontend *s5h1411_attach(const struct s5h1411_config *config,
 					   struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/s5h1432.h b/drivers/media/dvb-frontends/s5h1432.h
index b57438c3254..70917dd2533 100644
--- a/drivers/media/dvb-frontends/s5h1432.h
+++ b/drivers/media/dvb-frontends/s5h1432.h
@@ -22,6 +22,7 @@
 #ifndef __S5H1432_H__
 #define __S5H1432_H__
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 #define S5H1432_I2C_TOP_ADDR (0x02 >> 1)
@@ -74,8 +75,7 @@ struct s5h1432_config {
 	u8 status_mode;
 };
 
-#if defined(CONFIG_DVB_S5H1432) || \
-	(defined(CONFIG_DVB_S5H1432_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_S5H1432)
 extern struct dvb_frontend *s5h1432_attach(const struct s5h1432_config *config,
 					   struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/s921.h b/drivers/media/dvb-frontends/s921.h
index f220d8299c8..8d5e2a6e187 100644
--- a/drivers/media/dvb-frontends/s921.h
+++ b/drivers/media/dvb-frontends/s921.h
@@ -17,6 +17,7 @@
 #ifndef S921_H
 #define S921_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct s921_config {
@@ -24,8 +25,7 @@ struct s921_config {
 	u8 demod_address;
 };
 
-#if defined(CONFIG_DVB_S921) || (defined(CONFIG_DVB_S921_MODULE) \
-	&& defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_S921)
 extern struct dvb_frontend *s921_attach(const struct s921_config *config,
 					   struct i2c_adapter *i2c);
 extern struct i2c_adapter *s921_get_tuner_i2c_adapter(struct dvb_frontend *);
diff --git a/drivers/media/dvb-frontends/si21xx.h b/drivers/media/dvb-frontends/si21xx.h
index 141b5b8a5f6..1509fed44a3 100644
--- a/drivers/media/dvb-frontends/si21xx.h
+++ b/drivers/media/dvb-frontends/si21xx.h
@@ -1,6 +1,7 @@
 #ifndef SI21XX_H
 #define SI21XX_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include "dvb_frontend.h"
 
@@ -12,8 +13,7 @@ struct si21xx_config {
 	int min_delay_ms;
 };
 
-#if defined(CONFIG_DVB_SI21XX) || \
-		(defined(CONFIG_DVB_SI21XX_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_SI21XX)
 extern struct dvb_frontend *si21xx_attach(const struct si21xx_config *config,
 						struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/stb6000.h b/drivers/media/dvb-frontends/stb6000.h
index 7be479c22d5..a768189bfaa 100644
--- a/drivers/media/dvb-frontends/stb6000.h
+++ b/drivers/media/dvb-frontends/stb6000.h
@@ -23,6 +23,7 @@
 #ifndef __DVB_STB6000_H__
 #define __DVB_STB6000_H__
 
+#include <linux/kconfig.h>
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
 
@@ -34,8 +35,7 @@
  * @param i2c i2c adapter to use.
  * @return FE pointer on success, NULL on failure.
  */
-#if defined(CONFIG_DVB_STB6000) || (defined(CONFIG_DVB_STB6000_MODULE) \
-							&& defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_STB6000)
 extern struct dvb_frontend *stb6000_attach(struct dvb_frontend *fe, int addr,
 					   struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/stv0288.h b/drivers/media/dvb-frontends/stv0288.h
index f2b53db0606..a0bd9310715 100644
--- a/drivers/media/dvb-frontends/stv0288.h
+++ b/drivers/media/dvb-frontends/stv0288.h
@@ -27,6 +27,7 @@
 #ifndef STV0288_H
 #define STV0288_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include "dvb_frontend.h"
 
@@ -42,8 +43,7 @@ struct stv0288_config {
 	int (*set_ts_params)(struct dvb_frontend *fe, int is_punctured);
 };
 
-#if defined(CONFIG_DVB_STV0288) || (defined(CONFIG_DVB_STV0288_MODULE) && \
-							defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_STV0288)
 extern struct dvb_frontend *stv0288_attach(const struct stv0288_config *config,
 					   struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/stv0367.h b/drivers/media/dvb-frontends/stv0367.h
index 93cc4a57eea..ea80b341f09 100644
--- a/drivers/media/dvb-frontends/stv0367.h
+++ b/drivers/media/dvb-frontends/stv0367.h
@@ -26,6 +26,7 @@
 #ifndef STV0367_H
 #define STV0367_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include "dvb_frontend.h"
 
@@ -38,8 +39,7 @@ struct stv0367_config {
 	int clk_pol;
 };
 
-#if defined(CONFIG_DVB_STV0367) || (defined(CONFIG_DVB_STV0367_MODULE) \
-							&& defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_STV0367)
 extern struct
 dvb_frontend *stv0367ter_attach(const struct stv0367_config *config,
 					struct i2c_adapter *i2c);
diff --git a/drivers/media/dvb-frontends/stv0900.h b/drivers/media/dvb-frontends/stv0900.h
index 91c7ee8b231..e2a6dc69ecb 100644
--- a/drivers/media/dvb-frontends/stv0900.h
+++ b/drivers/media/dvb-frontends/stv0900.h
@@ -26,6 +26,7 @@
 #ifndef STV0900_H
 #define STV0900_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include "dvb_frontend.h"
 
@@ -57,8 +58,7 @@ struct stv0900_config {
 	void (*set_lock_led)(struct dvb_frontend *fe, int offon);
 };
 
-#if defined(CONFIG_DVB_STV0900) || (defined(CONFIG_DVB_STV0900_MODULE) \
-							&& defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_STV0900)
 extern struct dvb_frontend *stv0900_attach(const struct stv0900_config *config,
 					struct i2c_adapter *i2c, int demod);
 #else
diff --git a/drivers/media/dvb-frontends/stv090x.c b/drivers/media/dvb-frontends/stv090x.c
index f36eeefb76a..56d470ad5a8 100644
--- a/drivers/media/dvb-frontends/stv090x.c
+++ b/drivers/media/dvb-frontends/stv090x.c
@@ -3906,12 +3906,12 @@ static int stv090x_sleep(struct dvb_frontend *fe)
 		reg = stv090x_read_reg(state, STV090x_TSTTNR1);
 		STV090x_SETFIELD(reg, ADC1_PON_FIELD, 0);
 		if (stv090x_write_reg(state, STV090x_TSTTNR1, reg) < 0)
-			goto err;
+			goto err_unlock;
 		/* power off DiSEqC 1 */
 		reg = stv090x_read_reg(state, STV090x_TSTTNR2);
 		STV090x_SETFIELD(reg, DISEQC1_PON_FIELD, 0);
 		if (stv090x_write_reg(state, STV090x_TSTTNR2, reg) < 0)
-			goto err;
+			goto err_unlock;
 
 		/* check whether path 2 is already sleeping, that is when
 		   ADC2 is off */
@@ -3930,7 +3930,7 @@ static int stv090x_sleep(struct dvb_frontend *fe)
 		if (full_standby)
 			STV090x_SETFIELD(reg, STOP_CLKFEC_FIELD, 1);
 		if (stv090x_write_reg(state, STV090x_STOPCLK1, reg) < 0)
-			goto err;
+			goto err_unlock;
 		reg = stv090x_read_reg(state, STV090x_STOPCLK2);
 		/* sampling 1 clock */
 		STV090x_SETFIELD(reg, STOP_CLKSAMP1_FIELD, 1);
@@ -3941,7 +3941,7 @@ static int stv090x_sleep(struct dvb_frontend *fe)
 		if (full_standby)
 			STV090x_SETFIELD(reg, STOP_CLKTS_FIELD, 1);
 		if (stv090x_write_reg(state, STV090x_STOPCLK2, reg) < 0)
-			goto err;
+			goto err_unlock;
 		break;
 
 	case STV090x_DEMODULATOR_1:
@@ -3949,12 +3949,12 @@ static int stv090x_sleep(struct dvb_frontend *fe)
 		reg = stv090x_read_reg(state, STV090x_TSTTNR3);
 		STV090x_SETFIELD(reg, ADC2_PON_FIELD, 0);
 		if (stv090x_write_reg(state, STV090x_TSTTNR3, reg) < 0)
-			goto err;
+			goto err_unlock;
 		/* power off DiSEqC 2 */
 		reg = stv090x_read_reg(state, STV090x_TSTTNR4);
 		STV090x_SETFIELD(reg, DISEQC2_PON_FIELD, 0);
 		if (stv090x_write_reg(state, STV090x_TSTTNR4, reg) < 0)
-			goto err;
+			goto err_unlock;
 
 		/* check whether path 1 is already sleeping, that is when
 		   ADC1 is off */
@@ -3973,7 +3973,7 @@ static int stv090x_sleep(struct dvb_frontend *fe)
 		if (full_standby)
 			STV090x_SETFIELD(reg, STOP_CLKFEC_FIELD, 1);
 		if (stv090x_write_reg(state, STV090x_STOPCLK1, reg) < 0)
-			goto err;
+			goto err_unlock;
 		reg = stv090x_read_reg(state, STV090x_STOPCLK2);
 		/* sampling 2 clock */
 		STV090x_SETFIELD(reg, STOP_CLKSAMP2_FIELD, 1);
@@ -3984,7 +3984,7 @@ static int stv090x_sleep(struct dvb_frontend *fe)
 		if (full_standby)
 			STV090x_SETFIELD(reg, STOP_CLKTS_FIELD, 1);
 		if (stv090x_write_reg(state, STV090x_STOPCLK2, reg) < 0)
-			goto err;
+			goto err_unlock;
 		break;
 
 	default:
@@ -3997,7 +3997,7 @@ static int stv090x_sleep(struct dvb_frontend *fe)
 		reg = stv090x_read_reg(state, STV090x_SYNTCTRL);
 		STV090x_SETFIELD(reg, STANDBY_FIELD, 0x01);
 		if (stv090x_write_reg(state, STV090x_SYNTCTRL, reg) < 0)
-			goto err;
+			goto err_unlock;
 	}
 
 	mutex_unlock(&state->internal->demod_lock);
@@ -4005,8 +4005,10 @@ static int stv090x_sleep(struct dvb_frontend *fe)
 
 err_gateoff:
 	stv090x_i2c_gate_ctrl(state, 0);
-err:
+	goto err;
+err_unlock:
 	mutex_unlock(&state->internal->demod_lock);
+err:
 	dprintk(FE_ERROR, 1, "I/O error");
 	return -1;
 }
diff --git a/drivers/media/dvb-frontends/stv6110.h b/drivers/media/dvb-frontends/stv6110.h
index fe71bba6a26..8fa07e6a674 100644
--- a/drivers/media/dvb-frontends/stv6110.h
+++ b/drivers/media/dvb-frontends/stv6110.h
@@ -25,6 +25,7 @@
 #ifndef __DVB_STV6110_H__
 #define __DVB_STV6110_H__
 
+#include <linux/kconfig.h>
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
 
@@ -45,8 +46,7 @@ struct stv6110_config {
 	u8 clk_div;	/* divisor value for the output clock */
 };
 
-#if defined(CONFIG_DVB_STV6110) || (defined(CONFIG_DVB_STV6110_MODULE) \
-							&& defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_STV6110)
 extern struct dvb_frontend *stv6110_attach(struct dvb_frontend *fe,
 					const struct stv6110_config *config,
 					struct i2c_adapter *i2c);
diff --git a/drivers/media/dvb-frontends/tda10048.h b/drivers/media/dvb-frontends/tda10048.h
index fb2ef5ac948..5e7bf4e47cb 100644
--- a/drivers/media/dvb-frontends/tda10048.h
+++ b/drivers/media/dvb-frontends/tda10048.h
@@ -22,6 +22,7 @@
 #ifndef TDA10048_H
 #define TDA10048_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include <linux/firmware.h>
 
@@ -72,8 +73,7 @@ struct tda10048_config {
 	u8 pll_n;
 };
 
-#if defined(CONFIG_DVB_TDA10048) || \
-	(defined(CONFIG_DVB_TDA10048_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_TDA10048)
 extern struct dvb_frontend *tda10048_attach(
 	const struct tda10048_config *config,
 	struct i2c_adapter *i2c);
diff --git a/drivers/media/dvb-frontends/tda10071.h b/drivers/media/dvb-frontends/tda10071.h
index bff1c38df80..f9542f68fe7 100644
--- a/drivers/media/dvb-frontends/tda10071.h
+++ b/drivers/media/dvb-frontends/tda10071.h
@@ -21,6 +21,7 @@
 #ifndef TDA10071_H
 #define TDA10071_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct tda10071_config {
@@ -71,8 +72,7 @@ struct tda10071_config {
 };
 
 
-#if defined(CONFIG_DVB_TDA10071) || \
-	(defined(CONFIG_DVB_TDA10071_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_TDA10071)
 extern struct dvb_frontend *tda10071_attach(
 	const struct tda10071_config *config, struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/tda18271c2dd.h b/drivers/media/dvb-frontends/tda18271c2dd.h
index 1389c74e12c..dd84f7b69be 100644
--- a/drivers/media/dvb-frontends/tda18271c2dd.h
+++ b/drivers/media/dvb-frontends/tda18271c2dd.h
@@ -1,7 +1,9 @@
 #ifndef _TDA18271C2DD_H_
 #define _TDA18271C2DD_H_
-#if defined(CONFIG_DVB_TDA18271C2DD) || (defined(CONFIG_DVB_TDA18271C2DD_MODULE) \
-        && defined(MODULE))
+
+#include <linux/kconfig.h>
+
+#if IS_ENABLED(CONFIG_DVB_TDA18271C2DD)
 struct dvb_frontend *tda18271c2dd_attach(struct dvb_frontend *fe,
 					 struct i2c_adapter *i2c, u8 adr);
 #else
diff --git a/drivers/media/dvb-frontends/ts2020.h b/drivers/media/dvb-frontends/ts2020.h
index c7e64afa614..5bcb9a71ca8 100644
--- a/drivers/media/dvb-frontends/ts2020.h
+++ b/drivers/media/dvb-frontends/ts2020.h
@@ -22,6 +22,7 @@
 #ifndef TS2020_H
 #define TS2020_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct ts2020_config {
@@ -29,8 +30,7 @@ struct ts2020_config {
 	u8 clk_out_div;
 };
 
-#if defined(CONFIG_DVB_TS2020) || \
-	(defined(CONFIG_DVB_TS2020_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_TS2020)
 
 extern struct dvb_frontend *ts2020_attach(
 	struct dvb_frontend *fe,
diff --git a/drivers/media/dvb-frontends/zl10036.h b/drivers/media/dvb-frontends/zl10036.h
index d84b8f8215e..5f1e8217eeb 100644
--- a/drivers/media/dvb-frontends/zl10036.h
+++ b/drivers/media/dvb-frontends/zl10036.h
@@ -21,6 +21,7 @@
 #ifndef DVB_ZL10036_H
 #define DVB_ZL10036_H
 
+#include <linux/kconfig.h>
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
 
@@ -37,8 +38,7 @@ struct zl10036_config {
 	int rf_loop_enable;
 };
 
-#if defined(CONFIG_DVB_ZL10036) || \
-	(defined(CONFIG_DVB_ZL10036_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_ZL10036)
 extern struct dvb_frontend *zl10036_attach(struct dvb_frontend *fe,
 	const struct zl10036_config *config, struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/zl10039.h b/drivers/media/dvb-frontends/zl10039.h
index 5eee7ea162a..750b9bca9d0 100644
--- a/drivers/media/dvb-frontends/zl10039.h
+++ b/drivers/media/dvb-frontends/zl10039.h
@@ -22,8 +22,9 @@
 #ifndef ZL10039_H
 #define ZL10039_H
 
-#if defined(CONFIG_DVB_ZL10039) || (defined(CONFIG_DVB_ZL10039_MODULE) \
-	    && defined(MODULE))
+#include <linux/kconfig.h>
+
+#if IS_ENABLED(CONFIG_DVB_ZL10039)
 struct dvb_frontend *zl10039_attach(struct dvb_frontend *fe,
 					u8 i2c_addr,
 					struct i2c_adapter *i2c);
diff --git a/drivers/media/firewire/firedtv-dvb.c b/drivers/media/firewire/firedtv-dvb.c
index eb7496eab13..f710e17953e 100644
--- a/drivers/media/firewire/firedtv-dvb.c
+++ b/drivers/media/firewire/firedtv-dvb.c
@@ -71,11 +71,11 @@ int fdtv_start_feed(struct dvb_demux_feed *dvbdmxfeed)
 
 	if (dvbdmxfeed->type == DMX_TYPE_TS) {
 		switch (dvbdmxfeed->pes_type) {
-		case DMX_TS_PES_VIDEO:
-		case DMX_TS_PES_AUDIO:
-		case DMX_TS_PES_TELETEXT:
-		case DMX_TS_PES_PCR:
-		case DMX_TS_PES_OTHER:
+		case DMX_PES_VIDEO:
+		case DMX_PES_AUDIO:
+		case DMX_PES_TELETEXT:
+		case DMX_PES_PCR:
+		case DMX_PES_OTHER:
 			c = alloc_channel(fdtv);
 			break;
 		default:
@@ -132,7 +132,7 @@ int fdtv_stop_feed(struct dvb_demux_feed *dvbdmxfeed)
 	      (demux->dmx.frontend->source != DMX_MEMORY_FE))) {
 
 		if (dvbdmxfeed->ts_type & TS_DECODER) {
-			if (dvbdmxfeed->pes_type >= DMX_TS_PES_OTHER ||
+			if (dvbdmxfeed->pes_type >= DMX_PES_OTHER ||
 			    !demux->pesfilter[dvbdmxfeed->pes_type])
 				return -EINVAL;
 
@@ -141,7 +141,7 @@ int fdtv_stop_feed(struct dvb_demux_feed *dvbdmxfeed)
 		}
 
 		if (!(dvbdmxfeed->ts_type & TS_DECODER &&
-		      dvbdmxfeed->pes_type < DMX_TS_PES_OTHER))
+		      dvbdmxfeed->pes_type < DMX_PES_OTHER))
 			return 0;
 	}
 
diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig
index 7b771baa221..f981d50a2a8 100644
--- a/drivers/media/i2c/Kconfig
+++ b/drivers/media/i2c/Kconfig
@@ -112,6 +112,15 @@ config VIDEO_TLV320AIC23B
 	  To compile this driver as a module, choose M here: the
 	  module will be called tlv320aic23b.
 
+config VIDEO_UDA1342
+	tristate "Philips UDA1342 audio codec"
+	depends on VIDEO_V4L2 && I2C
+	---help---
+	  Support for the Philips UDA1342 audio codec.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called uda1342.
+
 config VIDEO_WM8775
 	tristate "Wolfson Microelectronics WM8775 audio ADC with input mixer"
 	depends on VIDEO_V4L2 && I2C
@@ -133,7 +142,7 @@ config VIDEO_WM8739
 	  module will be called wm8739.
 
 config VIDEO_VP27SMPX
-	tristate "Panasonic VP27s internal MPX"
+	tristate "Panasonic VP27's internal MPX"
 	depends on VIDEO_V4L2 && I2C
 	---help---
 	  Support for the internal MPX of the Panasonic VP27s tuner.
@@ -141,6 +150,15 @@ config VIDEO_VP27SMPX
 	  To compile this driver as a module, choose M here: the
 	  module will be called vp27smpx.
 
+config VIDEO_SONY_BTF_MPX
+	tristate "Sony BTF's internal MPX"
+	depends on VIDEO_V4L2 && I2C
+	help
+	  Support for the internal MPX of the Sony BTF-PG472Z tuner.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called sony-btf-mpx.
+
 comment "RDS decoders"
 
 config VIDEO_SAA6588
@@ -283,6 +301,35 @@ config VIDEO_TVP7002
 	  To compile this driver as a module, choose M here: the
 	  module will be called tvp7002.
 
+config VIDEO_TW2804
+	tristate "Techwell TW2804 multiple video decoder"
+	depends on VIDEO_V4L2 && I2C
+	---help---
+	  Support for the Techwell tw2804 multiple video decoder.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tw2804.
+
+config VIDEO_TW9903
+	tristate "Techwell TW9903 video decoder"
+	depends on VIDEO_V4L2 && I2C
+	---help---
+	  Support for the Techwell tw9903 multi-standard video decoder
+	  with high quality down scaler.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tw9903.
+
+config VIDEO_TW9906
+	tristate "Techwell TW9906 video decoder"
+	depends on VIDEO_V4L2 && I2C
+	---help---
+	  Support for the Techwell tw9906 enhanced multi-standard comb filter
+	  video decoder with YCbCr input support.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tw9906.
+
 config VIDEO_VPX3220
 	tristate "vpx3220a, vpx3216b & vpx3214c video decoders"
 	depends on VIDEO_V4L2 && I2C
@@ -386,6 +433,17 @@ config VIDEO_APTINA_PLL
 config VIDEO_SMIAPP_PLL
 	tristate
 
+config VIDEO_OV7640
+	tristate "OmniVision OV7640 sensor support"
+	depends on I2C && VIDEO_V4L2
+	depends on MEDIA_CAMERA_SUPPORT
+	---help---
+	  This is a Video4Linux2 sensor-level driver for the OmniVision
+	  OV7640 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov7640.
+
 config VIDEO_OV7670
 	tristate "OmniVision OV7670 sensor support"
 	depends on I2C && VIDEO_V4L2
@@ -501,8 +559,8 @@ config VIDEO_S5C73M3
 	tristate "Samsung S5C73M3 sensor support"
 	depends on I2C && SPI && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
 	---help---
-	This is a V4L2 sensor-level driver for Samsung S5C73M3
-	8 Mpixel camera.
+	  This is a V4L2 sensor-level driver for Samsung S5C73M3
+	  8 Mpixel camera.
 
 comment "Flash devices"
 
@@ -550,10 +608,10 @@ config VIDEO_UPD64083
 comment "Miscelaneous helper chips"
 
 config VIDEO_THS7303
-	tristate "THS7303 Video Amplifier"
-	depends on I2C
+	tristate "THS7303/53 Video Amplifier"
+	depends on VIDEO_V4L2 && I2C
 	help
-	  Support for TI THS7303 video amplifier
+	  Support for TI THS7303/53 video amplifier
 
 	  To compile this driver as a module, choose M here: the
 	  module will be called ths7303.
diff --git a/drivers/media/i2c/Makefile b/drivers/media/i2c/Makefile
index cfefd30cc1b..720f42d9d9f 100644
--- a/drivers/media/i2c/Makefile
+++ b/drivers/media/i2c/Makefile
@@ -37,16 +37,22 @@ obj-$(CONFIG_VIDEO_THS7303) += ths7303.o
 obj-$(CONFIG_VIDEO_TVP5150) += tvp5150.o
 obj-$(CONFIG_VIDEO_TVP514X) += tvp514x.o
 obj-$(CONFIG_VIDEO_TVP7002) += tvp7002.o
+obj-$(CONFIG_VIDEO_TW2804) += tw2804.o
+obj-$(CONFIG_VIDEO_TW9903) += tw9903.o
+obj-$(CONFIG_VIDEO_TW9906) += tw9906.o
 obj-$(CONFIG_VIDEO_CS5345) += cs5345.o
 obj-$(CONFIG_VIDEO_CS53L32A) += cs53l32a.o
 obj-$(CONFIG_VIDEO_M52790) += m52790.o
 obj-$(CONFIG_VIDEO_TLV320AIC23B) += tlv320aic23b.o
+obj-$(CONFIG_VIDEO_UDA1342) += uda1342.o
 obj-$(CONFIG_VIDEO_WM8775) += wm8775.o
 obj-$(CONFIG_VIDEO_WM8739) += wm8739.o
 obj-$(CONFIG_VIDEO_VP27SMPX) += vp27smpx.o
+obj-$(CONFIG_VIDEO_SONY_BTF_MPX) += sony-btf-mpx.o
 obj-$(CONFIG_VIDEO_UPD64031A) += upd64031a.o
 obj-$(CONFIG_VIDEO_UPD64083) += upd64083.o
-obj-$(CONFIG_VIDEO_OV7670) 	+= ov7670.o
+obj-$(CONFIG_VIDEO_OV7640) += ov7640.o
+obj-$(CONFIG_VIDEO_OV7670) += ov7670.o
 obj-$(CONFIG_VIDEO_OV9650) += ov9650.o
 obj-$(CONFIG_VIDEO_TCM825X) += tcm825x.o
 obj-$(CONFIG_VIDEO_MT9M032) += mt9m032.o
diff --git a/drivers/media/i2c/ad9389b.c b/drivers/media/i2c/ad9389b.c
index c2886b6a727..58344b6c3a5 100644
--- a/drivers/media/i2c/ad9389b.c
+++ b/drivers/media/i2c/ad9389b.c
@@ -354,7 +354,7 @@ static int ad9389b_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *
 	return 0;
 }
 
-static int ad9389b_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int ad9389b_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/adp1653.c b/drivers/media/i2c/adp1653.c
index df163800c8e..ef75abe5984 100644
--- a/drivers/media/i2c/adp1653.c
+++ b/drivers/media/i2c/adp1653.c
@@ -447,7 +447,7 @@ free_and_quit:
 	return ret;
 }
 
-static int __exit adp1653_remove(struct i2c_client *client)
+static int adp1653_remove(struct i2c_client *client)
 {
 	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
 	struct adp1653_flash *flash = to_adp1653_flash(subdev);
@@ -476,7 +476,7 @@ static struct i2c_driver adp1653_i2c_driver = {
 		.pm	= &adp1653_pm_ops,
 	},
 	.probe		= adp1653_probe,
-	.remove		= __exit_p(adp1653_remove),
+	.remove		= adp1653_remove,
 	.id_table	= adp1653_id_table,
 };
 
diff --git a/drivers/media/i2c/adv7180.c b/drivers/media/i2c/adv7180.c
index 34f39d3b3e3..afd561ab190 100644
--- a/drivers/media/i2c/adv7180.c
+++ b/drivers/media/i2c/adv7180.c
@@ -135,6 +135,10 @@ struct adv7180_state {
 
 static v4l2_std_id adv7180_std_to_v4l2(u8 status1)
 {
+	/* in case V4L2_IN_ST_NO_SIGNAL */
+	if (!(status1 & ADV7180_STATUS1_IN_LOCK))
+		return V4L2_STD_UNKNOWN;
+
 	switch (status1 & ADV7180_STATUS1_AUTOD_MASK) {
 	case ADV7180_STATUS1_AUTOD_NTSM_M_J:
 		return V4L2_STD_NTSC;
diff --git a/drivers/media/i2c/adv7183.c b/drivers/media/i2c/adv7183.c
index 6fed5b74e74..56a1fa4af0f 100644
--- a/drivers/media/i2c/adv7183.c
+++ b/drivers/media/i2c/adv7183.c
@@ -507,7 +507,7 @@ static int adv7183_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *
 	return 0;
 }
 
-static int adv7183_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int adv7183_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/adv7604.c b/drivers/media/i2c/adv7604.c
index f47555b1000..31a63c9324f 100644
--- a/drivers/media/i2c/adv7604.c
+++ b/drivers/media/i2c/adv7604.c
@@ -699,7 +699,7 @@ static int adv7604_g_register(struct v4l2_subdev *sd,
 }
 
 static int adv7604_s_register(struct v4l2_subdev *sd,
-					struct v4l2_dbg_register *reg)
+					const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/ak881x.c b/drivers/media/i2c/ak881x.c
index ba674656b10..fd47465e4f6 100644
--- a/drivers/media/i2c/ak881x.c
+++ b/drivers/media/i2c/ak881x.c
@@ -101,7 +101,7 @@ static int ak881x_g_register(struct v4l2_subdev *sd,
 }
 
 static int ak881x_s_register(struct v4l2_subdev *sd,
-			     struct v4l2_dbg_register *reg)
+			     const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/cs5345.c b/drivers/media/i2c/cs5345.c
index c8581e26fa9..1d2f7c8512b 100644
--- a/drivers/media/i2c/cs5345.c
+++ b/drivers/media/i2c/cs5345.c
@@ -110,7 +110,7 @@ static int cs5345_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *r
 	return 0;
 }
 
-static int cs5345_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int cs5345_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/cx25840/cx25840-core.c b/drivers/media/i2c/cx25840/cx25840-core.c
index f4149eb4d7b..12fb9b2eb88 100644
--- a/drivers/media/i2c/cx25840/cx25840-core.c
+++ b/drivers/media/i2c/cx25840/cx25840-core.c
@@ -1671,7 +1671,7 @@ static int cx25840_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *
 	return 0;
 }
 
-static int cx25840_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int cx25840_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
@@ -1835,7 +1835,7 @@ static int cx25840_s_audio_routing(struct v4l2_subdev *sd,
 	return set_input(client, state->vid_input, input);
 }
 
-static int cx25840_s_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *freq)
+static int cx25840_s_frequency(struct v4l2_subdev *sd, const struct v4l2_frequency *freq)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
@@ -1881,7 +1881,7 @@ static int cx25840_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
 	return 0;
 }
 
-static int cx25840_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+static int cx25840_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
 {
 	struct cx25840_state *state = to_state(sd);
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
diff --git a/drivers/media/i2c/ir-kbd-i2c.c b/drivers/media/i2c/ir-kbd-i2c.c
index 08ae067b2b6..8e2f79cb045 100644
--- a/drivers/media/i2c/ir-kbd-i2c.c
+++ b/drivers/media/i2c/ir-kbd-i2c.c
@@ -230,7 +230,7 @@ static int get_key_avermedia_cardbus(struct IR_i2c *ir,
 		return 0;
 
 	dprintk(1, "read key 0x%02x/0x%02x\n", key, keygroup);
-	if (keygroup < 2 || keygroup > 3) {
+	if (keygroup < 2 || keygroup > 4) {
 		/* Only a warning */
 		dprintk(1, "warning: invalid key group 0x%02x for key 0x%02x\n",
 								keygroup, key);
@@ -239,6 +239,10 @@ static int get_key_avermedia_cardbus(struct IR_i2c *ir,
 
 	*ir_key = key;
 	*ir_raw = key;
+	if (!strcmp(ir->ir_codes, RC_MAP_AVERMEDIA_M733A_RM_K6)) {
+		*ir_key |= keygroup << 8;
+		*ir_raw |= keygroup << 8;
+	}
 	return 1;
 }
 
@@ -332,6 +336,13 @@ static int ir_probe(struct i2c_client *client, const struct i2c_device_id *id)
 		rc_type     = RC_BIT_OTHER;
 		ir_codes    = RC_MAP_AVERMEDIA_CARDBUS;
 		break;
+	case 0x41:
+		name        = "AVerMedia EM78P153";
+		ir->get_key = get_key_avermedia_cardbus;
+		rc_type     = RC_BIT_OTHER;
+		/* RM-KV remote, seems to be same as RM-K6 */
+		ir_codes    = RC_MAP_AVERMEDIA_M733A_RM_K6;
+		break;
 	case 0x71:
 		name        = "Hauppauge/Zilog Z8";
 		ir->get_key = get_key_haup_xvr;
@@ -423,6 +434,7 @@ static int ir_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	 */
 	rc->map_name       = ir->ir_codes;
 	rc->allowed_protos = rc_type;
+	rc->enabled_protocols = rc_type;
 	if (!rc->driver_name)
 		rc->driver_name = MODULE_NAME;
 
diff --git a/drivers/media/i2c/m52790.c b/drivers/media/i2c/m52790.c
index 0991576f4c8..39f50fd2b8d 100644
--- a/drivers/media/i2c/m52790.c
+++ b/drivers/media/i2c/m52790.c
@@ -96,7 +96,7 @@ static int m52790_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *r
 	return 0;
 }
 
-static int m52790_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int m52790_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct m52790_state *state = to_state(sd);
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
diff --git a/drivers/media/i2c/m5mols/m5mols_core.c b/drivers/media/i2c/m5mols/m5mols_core.c
index d4e7567b367..0b899cb6cda 100644
--- a/drivers/media/i2c/m5mols/m5mols_core.c
+++ b/drivers/media/i2c/m5mols/m5mols_core.c
@@ -724,7 +724,7 @@ static int m5mols_s_stream(struct v4l2_subdev *sd, int enable)
 	if (enable) {
 		if (is_code(code, M5MOLS_RESTYPE_MONITOR))
 			ret = m5mols_start_monitor(info);
-		if (is_code(code, M5MOLS_RESTYPE_CAPTURE))
+		else if (is_code(code, M5MOLS_RESTYPE_CAPTURE))
 			ret = m5mols_start_capture(info);
 		else
 			ret = -EINVAL;
diff --git a/drivers/media/i2c/msp3400-driver.c b/drivers/media/i2c/msp3400-driver.c
index 766305f69a2..54a9dd394f4 100644
--- a/drivers/media/i2c/msp3400-driver.c
+++ b/drivers/media/i2c/msp3400-driver.c
@@ -445,7 +445,7 @@ static int msp_s_radio(struct v4l2_subdev *sd)
 	return 0;
 }
 
-static int msp_s_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *freq)
+static int msp_s_frequency(struct v4l2_subdev *sd, const struct v4l2_frequency *freq)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
@@ -535,7 +535,7 @@ static int msp_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
 	return 0;
 }
 
-static int msp_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+static int msp_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
 {
 	struct msp_state *state = to_state(sd);
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
diff --git a/drivers/media/i2c/mt9m032.c b/drivers/media/i2c/mt9m032.c
index f80c1d7ec88..8edb3d8f7b9 100644
--- a/drivers/media/i2c/mt9m032.c
+++ b/drivers/media/i2c/mt9m032.c
@@ -87,9 +87,27 @@
 #define MT9M032_RESTART					0x0b
 #define MT9M032_RESET					0x0d
 #define MT9M032_PLL_CONFIG1				0x11
-#define		MT9M032_PLL_CONFIG1_OUTDIV_MASK		0x3f
+#define		MT9M032_PLL_CONFIG1_PREDIV_MASK		0x3f
 #define		MT9M032_PLL_CONFIG1_MUL_SHIFT		8
 #define MT9M032_READ_MODE1				0x1e
+#define		MT9M032_READ_MODE1_OUTPUT_BAD_FRAMES	(1 << 13)
+#define		MT9M032_READ_MODE1_MAINTAIN_FRAME_RATE	(1 << 12)
+#define		MT9M032_READ_MODE1_XOR_LINE_VALID	(1 << 11)
+#define		MT9M032_READ_MODE1_CONT_LINE_VALID	(1 << 10)
+#define		MT9M032_READ_MODE1_INVERT_TRIGGER	(1 << 9)
+#define		MT9M032_READ_MODE1_SNAPSHOT		(1 << 8)
+#define		MT9M032_READ_MODE1_GLOBAL_RESET		(1 << 7)
+#define		MT9M032_READ_MODE1_BULB_EXPOSURE	(1 << 6)
+#define		MT9M032_READ_MODE1_INVERT_STROBE	(1 << 5)
+#define		MT9M032_READ_MODE1_STROBE_ENABLE	(1 << 4)
+#define		MT9M032_READ_MODE1_STROBE_START_TRIG1	(0 << 2)
+#define		MT9M032_READ_MODE1_STROBE_START_EXP	(1 << 2)
+#define		MT9M032_READ_MODE1_STROBE_START_SHUTTER	(2 << 2)
+#define		MT9M032_READ_MODE1_STROBE_START_TRIG2	(3 << 2)
+#define		MT9M032_READ_MODE1_STROBE_END_TRIG1	(0 << 0)
+#define		MT9M032_READ_MODE1_STROBE_END_EXP	(1 << 0)
+#define		MT9M032_READ_MODE1_STROBE_END_SHUTTER	(2 << 0)
+#define		MT9M032_READ_MODE1_STROBE_END_TRIG2	(3 << 0)
 #define MT9M032_READ_MODE2				0x20
 #define		MT9M032_READ_MODE2_VFLIP_SHIFT		15
 #define		MT9M032_READ_MODE2_HFLIP_SHIFT		14
@@ -106,6 +124,8 @@
 #define		MT9M032_GAIN_AMUL_SHIFT			6
 #define		MT9M032_GAIN_ANALOG_MASK		0x3f
 #define MT9M032_FORMATTER1				0x9e
+#define		MT9M032_FORMATTER1_PLL_P1_6		(1 << 8)
+#define		MT9M032_FORMATTER1_PARALLEL		(1 << 12)
 #define MT9M032_FORMATTER2				0x9f
 #define		MT9M032_FORMATTER2_DOUT_EN		0x1000
 #define		MT9M032_FORMATTER2_PIXCLK_EN		0x2000
@@ -121,8 +141,6 @@
 #define		MT9P031_PLL_CONTROL_PWROFF		0x0050
 #define		MT9P031_PLL_CONTROL_PWRON		0x0051
 #define		MT9P031_PLL_CONTROL_USEPLL		0x0052
-#define MT9P031_PLL_CONFIG2				0x11
-#define		MT9P031_PLL_CONFIG2_P1_DIV_MASK		0x1f
 
 struct mt9m032 {
 	struct v4l2_subdev subdev;
@@ -255,13 +273,14 @@ static int mt9m032_setup_pll(struct mt9m032 *sensor)
 		.n_max = 64,
 		.m_min = 16,
 		.m_max = 255,
-		.p1_min = 1,
-		.p1_max = 128,
+		.p1_min = 6,
+		.p1_max = 7,
 	};
 
 	struct i2c_client *client = v4l2_get_subdevdata(&sensor->subdev);
 	struct mt9m032_platform_data *pdata = sensor->pdata;
 	struct aptina_pll pll;
+	u16 reg_val;
 	int ret;
 
 	pll.ext_clock = pdata->ext_clock;
@@ -274,18 +293,21 @@ static int mt9m032_setup_pll(struct mt9m032 *sensor)
 	sensor->pix_clock = pdata->pix_clock;
 
 	ret = mt9m032_write(client, MT9M032_PLL_CONFIG1,
-			    (pll.m << MT9M032_PLL_CONFIG1_MUL_SHIFT)
-			    | (pll.p1 - 1));
-	if (!ret)
-		ret = mt9m032_write(client, MT9P031_PLL_CONFIG2, pll.n - 1);
+			    (pll.m << MT9M032_PLL_CONFIG1_MUL_SHIFT) |
+			    ((pll.n - 1) & MT9M032_PLL_CONFIG1_PREDIV_MASK));
 	if (!ret)
 		ret = mt9m032_write(client, MT9P031_PLL_CONTROL,
 				    MT9P031_PLL_CONTROL_PWRON |
 				    MT9P031_PLL_CONTROL_USEPLL);
 	if (!ret)		/* more reserved, Continuous, Master Mode */
-		ret = mt9m032_write(client, MT9M032_READ_MODE1, 0x8006);
-	if (!ret)		/* Set 14-bit mode, select 7 divider */
-		ret = mt9m032_write(client, MT9M032_FORMATTER1, 0x111e);
+		ret = mt9m032_write(client, MT9M032_READ_MODE1, 0x8000 |
+				    MT9M032_READ_MODE1_STROBE_START_EXP |
+				    MT9M032_READ_MODE1_STROBE_END_SHUTTER);
+	if (!ret) {
+		reg_val = (pll.p1 == 6 ? MT9M032_FORMATTER1_PLL_P1_6 : 0)
+			| MT9M032_FORMATTER1_PARALLEL | 0x001e; /* 14-bit */
+		ret = mt9m032_write(client, MT9M032_FORMATTER1, reg_val);
+	}
 
 	return ret;
 }
@@ -548,7 +570,7 @@ static int mt9m032_g_register(struct v4l2_subdev *sd,
 }
 
 static int mt9m032_s_register(struct v4l2_subdev *sd,
-			      struct v4l2_dbg_register *reg)
+			      const struct v4l2_dbg_register *reg)
 {
 	struct mt9m032 *sensor = to_mt9m032(sd);
 	struct i2c_client *client = v4l2_get_subdevdata(&sensor->subdev);
diff --git a/drivers/media/i2c/mt9p031.c b/drivers/media/i2c/mt9p031.c
index e32833262d3..28cf95b3728 100644
--- a/drivers/media/i2c/mt9p031.c
+++ b/drivers/media/i2c/mt9p031.c
@@ -12,6 +12,7 @@
  * published by the Free Software Foundation.
  */
 
+#include <linux/clk.h>
 #include <linux/delay.h>
 #include <linux/device.h>
 #include <linux/gpio.h>
@@ -19,6 +20,7 @@
 #include <linux/i2c.h>
 #include <linux/log2.h>
 #include <linux/pm.h>
+#include <linux/regulator/consumer.h>
 #include <linux/slab.h>
 #include <linux/videodev2.h>
 
@@ -121,6 +123,11 @@ struct mt9p031 {
 	struct mutex power_lock; /* lock to protect power_count */
 	int power_count;
 
+	struct clk *clk;
+	struct regulator *vaa;
+	struct regulator *vdd;
+	struct regulator *vdd_io;
+
 	enum mt9p031_model model;
 	struct aptina_pll pll;
 	int reset;
@@ -195,7 +202,7 @@ static int mt9p031_reset(struct mt9p031 *mt9p031)
 					  0);
 }
 
-static int mt9p031_pll_setup(struct mt9p031 *mt9p031)
+static int mt9p031_clk_setup(struct mt9p031 *mt9p031)
 {
 	static const struct aptina_pll_limits limits = {
 		.ext_clock_min = 6000000,
@@ -216,6 +223,12 @@ static int mt9p031_pll_setup(struct mt9p031 *mt9p031)
 	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
 	struct mt9p031_platform_data *pdata = mt9p031->pdata;
 
+	mt9p031->clk = devm_clk_get(&client->dev, NULL);
+	if (IS_ERR(mt9p031->clk))
+		return PTR_ERR(mt9p031->clk);
+
+	clk_set_rate(mt9p031->clk, pdata->ext_freq);
+
 	mt9p031->pll.ext_clock = pdata->ext_freq;
 	mt9p031->pll.pix_clock = pdata->target_freq;
 
@@ -264,10 +277,14 @@ static int mt9p031_power_on(struct mt9p031 *mt9p031)
 		usleep_range(1000, 2000);
 	}
 
+	/* Bring up the supplies */
+	regulator_enable(mt9p031->vdd);
+	regulator_enable(mt9p031->vdd_io);
+	regulator_enable(mt9p031->vaa);
+
 	/* Emable clock */
-	if (mt9p031->pdata->set_xclk)
-		mt9p031->pdata->set_xclk(&mt9p031->subdev,
-					 mt9p031->pdata->ext_freq);
+	if (mt9p031->clk)
+		clk_prepare_enable(mt9p031->clk);
 
 	/* Now RESET_BAR must be high */
 	if (mt9p031->reset != -1) {
@@ -285,8 +302,12 @@ static void mt9p031_power_off(struct mt9p031 *mt9p031)
 		usleep_range(1000, 2000);
 	}
 
-	if (mt9p031->pdata->set_xclk)
-		mt9p031->pdata->set_xclk(&mt9p031->subdev, 0);
+	regulator_disable(mt9p031->vaa);
+	regulator_disable(mt9p031->vdd_io);
+	regulator_disable(mt9p031->vdd);
+
+	if (mt9p031->clk)
+		clk_disable_unprepare(mt9p031->clk);
 }
 
 static int __mt9p031_set_power(struct mt9p031 *mt9p031, bool on)
@@ -927,7 +948,7 @@ static int mt9p031_probe(struct i2c_client *client,
 		return -EIO;
 	}
 
-	mt9p031 = kzalloc(sizeof(*mt9p031), GFP_KERNEL);
+	mt9p031 = devm_kzalloc(&client->dev, sizeof(*mt9p031), GFP_KERNEL);
 	if (mt9p031 == NULL)
 		return -ENOMEM;
 
@@ -937,6 +958,16 @@ static int mt9p031_probe(struct i2c_client *client,
 	mt9p031->model = did->driver_data;
 	mt9p031->reset = -1;
 
+	mt9p031->vaa = devm_regulator_get(&client->dev, "vaa");
+	mt9p031->vdd = devm_regulator_get(&client->dev, "vdd");
+	mt9p031->vdd_io = devm_regulator_get(&client->dev, "vdd_io");
+
+	if (IS_ERR(mt9p031->vaa) || IS_ERR(mt9p031->vdd) ||
+	    IS_ERR(mt9p031->vdd_io)) {
+		dev_err(&client->dev, "Unable to get regulators\n");
+		return -ENODEV;
+	}
+
 	v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 6);
 
 	v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
@@ -1001,24 +1032,20 @@ static int mt9p031_probe(struct i2c_client *client,
 	mt9p031->format.colorspace = V4L2_COLORSPACE_SRGB;
 
 	if (pdata->reset != -1) {
-		ret = gpio_request_one(pdata->reset, GPIOF_OUT_INIT_LOW,
-				       "mt9p031_rst");
+		ret = devm_gpio_request_one(&client->dev, pdata->reset,
+					    GPIOF_OUT_INIT_LOW, "mt9p031_rst");
 		if (ret < 0)
 			goto done;
 
 		mt9p031->reset = pdata->reset;
 	}
 
-	ret = mt9p031_pll_setup(mt9p031);
+	ret = mt9p031_clk_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);
 	}
 
 	return ret;
@@ -1032,9 +1059,6 @@ 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;
 }
diff --git a/drivers/media/i2c/mt9v011.c b/drivers/media/i2c/mt9v011.c
index 73b7688cbeb..3f415fd12de 100644
--- a/drivers/media/i2c/mt9v011.c
+++ b/drivers/media/i2c/mt9v011.c
@@ -421,7 +421,7 @@ static int mt9v011_g_register(struct v4l2_subdev *sd,
 }
 
 static int mt9v011_s_register(struct v4l2_subdev *sd,
-			      struct v4l2_dbg_register *reg)
+			      const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/staging/media/go7007/wis-ov7640.c b/drivers/media/i2c/ov7640.c
index 9f01657f884..b0cc927e8b1 100644
--- a/drivers/staging/media/go7007/wis-ov7640.c
+++ b/drivers/media/i2c/ov7640.c
@@ -15,21 +15,18 @@
  * Inc., 59 Temple Place - Suite 330, Boston MA 02111-1307, USA.
  */
 
-#include <linux/module.h>
 #include <linux/init.h>
+#include <linux/module.h>
 #include <linux/i2c.h>
 #include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.h>
+#include <linux/slab.h>
 
-#include "wis-i2c.h"
-
-struct wis_ov7640 {
-	int brightness;
-	int contrast;
-	int saturation;
-	int hue;
-};
+MODULE_DESCRIPTION("OmniVision ov7640 sensor driver");
+MODULE_LICENSE("GPL v2");
 
-static u8 initial_registers[] = {
+static const u8 initial_registers[] = {
 	0x12, 0x80,
 	0x12, 0x54,
 	0x14, 0x24,
@@ -39,7 +36,7 @@ static u8 initial_registers[] = {
 	0xFF, 0xFF, /* Terminator (reg 0xFF is unused) */
 };
 
-static int write_regs(struct i2c_client *client, u8 *regs)
+static int write_regs(struct i2c_client *client, const u8 *regs)
 {
 	int i;
 
@@ -49,48 +46,61 @@ static int write_regs(struct i2c_client *client, u8 *regs)
 	return 0;
 }
 
-static int wis_ov7640_probe(struct i2c_client *client,
-			    const struct i2c_device_id *id)
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_ops ov7640_ops;
+
+static int ov7640_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
 {
 	struct i2c_adapter *adapter = client->adapter;
+	struct v4l2_subdev *sd;
 
 	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
 		return -ENODEV;
 
+	sd = kzalloc(sizeof(struct v4l2_subdev), GFP_KERNEL);
+	if (sd == NULL)
+		return -ENOMEM;
+	v4l2_i2c_subdev_init(sd, client, &ov7640_ops);
+
 	client->flags = I2C_CLIENT_SCCB;
 
-	dev_dbg(&client->dev,
-		"wis-ov7640: initializing OV7640 at address %d on %s\n",
-		client->addr, adapter->name);
+	v4l_info(client, "chip found @ 0x%02x (%s)\n",
+			client->addr << 1, client->adapter->name);
 
 	if (write_regs(client, initial_registers) < 0) {
-		dev_err(&client->dev, "wis-ov7640: error initializing OV7640\n");
+		v4l_err(client, "error initializing OV7640\n");
+		kfree(sd);
 		return -ENODEV;
 	}
 
 	return 0;
 }
 
-static int wis_ov7640_remove(struct i2c_client *client)
+
+static int ov7640_remove(struct i2c_client *client)
 {
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(sd);
 	return 0;
 }
 
-static const struct i2c_device_id wis_ov7640_id[] = {
-	{ "wis_ov7640", 0 },
+static const struct i2c_device_id ov7640_id[] = {
+	{ "ov7640", 0 },
 	{ }
 };
-MODULE_DEVICE_TABLE(i2c, wis_ov7640_id);
+MODULE_DEVICE_TABLE(i2c, ov7640_id);
 
-static struct i2c_driver wis_ov7640_driver = {
+static struct i2c_driver ov7640_driver = {
 	.driver = {
-		.name	= "WIS OV7640 I2C driver",
+		.owner	= THIS_MODULE,
+		.name	= "ov7640",
 	},
-	.probe		= wis_ov7640_probe,
-	.remove		= wis_ov7640_remove,
-	.id_table	= wis_ov7640_id,
+	.probe = ov7640_probe,
+	.remove = ov7640_remove,
+	.id_table = ov7640_id,
 };
-
-module_i2c_driver(wis_ov7640_driver);
-
-MODULE_LICENSE("GPL v2");
+module_i2c_driver(ov7640_driver);
diff --git a/drivers/media/i2c/ov7670.c b/drivers/media/i2c/ov7670.c
index 05ed5b8e7f8..617ad3fff4a 100644
--- a/drivers/media/i2c/ov7670.c
+++ b/drivers/media/i2c/ov7670.c
@@ -1487,7 +1487,7 @@ static int ov7670_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *r
 	return ret;
 }
 
-static int ov7670_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int ov7670_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/s5c73m3/s5c73m3-core.c b/drivers/media/i2c/s5c73m3/s5c73m3-core.c
index 5dbb65e1f6b..cb52438e53a 100644
--- a/drivers/media/i2c/s5c73m3/s5c73m3-core.c
+++ b/drivers/media/i2c/s5c73m3/s5c73m3-core.c
@@ -357,7 +357,7 @@ static int s5c73m3_load_fw(struct v4l2_subdev *sd)
 		return -EINVAL;
 	}
 
-	v4l2_info(sd, "Loading firmware (%s, %d B)\n", fw_name, fw->size);
+	v4l2_info(sd, "Loading firmware (%s, %zu B)\n", fw_name, fw->size);
 
 	ret = s5c73m3_spi_write(state, fw->data, fw->size, 64);
 
@@ -1457,6 +1457,12 @@ static int s5c73m3_oif_registered(struct v4l2_subdev *sd)
 	return ret;
 }
 
+static void s5c73m3_oif_unregistered(struct v4l2_subdev *sd)
+{
+	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
+	v4l2_device_unregister_subdev(&state->sensor_sd);
+}
+
 static const struct v4l2_subdev_internal_ops s5c73m3_internal_ops = {
 	.open		= s5c73m3_open,
 };
@@ -1474,6 +1480,7 @@ static const struct v4l2_subdev_ops s5c73m3_subdev_ops = {
 
 static const struct v4l2_subdev_internal_ops oif_internal_ops = {
 	.registered	= s5c73m3_oif_registered,
+	.unregistered	= s5c73m3_oif_unregistered,
 	.open		= s5c73m3_oif_open,
 };
 
@@ -1668,13 +1675,17 @@ out_err1:
 
 static int s5c73m3_remove(struct i2c_client *client)
 {
-	struct v4l2_subdev *sd = i2c_get_clientdata(client);
-	struct s5c73m3 *state = sensor_sd_to_s5c73m3(sd);
+	struct v4l2_subdev *oif_sd = i2c_get_clientdata(client);
+	struct s5c73m3 *state = oif_sd_to_s5c73m3(oif_sd);
+	struct v4l2_subdev *sensor_sd = &state->sensor_sd;
 
-	v4l2_device_unregister_subdev(sd);
+	v4l2_device_unregister_subdev(oif_sd);
 
-	v4l2_ctrl_handler_free(sd->ctrl_handler);
-	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(oif_sd->ctrl_handler);
+	media_entity_cleanup(&oif_sd->entity);
+
+	v4l2_device_unregister_subdev(sensor_sd);
+	media_entity_cleanup(&sensor_sd->entity);
 
 	s5c73m3_unregister_spi_driver(state);
 	s5c73m3_free_gpios(state);
diff --git a/drivers/media/i2c/saa6588.c b/drivers/media/i2c/saa6588.c
index 0caac50d7cf..b4e1ccbd87e 100644
--- a/drivers/media/i2c/saa6588.c
+++ b/drivers/media/i2c/saa6588.c
@@ -435,7 +435,7 @@ static int saa6588_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
 	return 0;
 }
 
-static int saa6588_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+static int saa6588_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
 {
 	struct saa6588 *s = to_saa6588(sd);
 
diff --git a/drivers/media/i2c/saa7115.c b/drivers/media/i2c/saa7115.c
index 6b6788cd08f..52c717d977c 100644
--- a/drivers/media/i2c/saa7115.c
+++ b/drivers/media/i2c/saa7115.c
@@ -83,9 +83,10 @@ struct saa711x_state {
 	u32 ident;
 	u32 audclk_freq;
 	u32 crystal_freq;
-	u8 ucgc;
+	bool ucgc;
 	u8 cgcdiv;
-	u8 apll;
+	bool apll;
+	bool double_asclk;
 };
 
 static inline struct saa711x_state *to_state(struct v4l2_subdev *sd)
@@ -732,8 +733,12 @@ static int saa711x_s_clock_freq(struct v4l2_subdev *sd, u32 freq)
 	if (state->apll)
 		acc |= 0x08;
 
+	if (state->double_asclk) {
+		acpf <<= 1;
+		acni <<= 1;
+	}
 	saa711x_write(sd, R_38_CLK_RATIO_AMXCLK_TO_ASCLK, 0x03);
-	saa711x_write(sd, R_39_CLK_RATIO_ASCLK_TO_ALRCLK, 0x10);
+	saa711x_write(sd, R_39_CLK_RATIO_ASCLK_TO_ALRCLK, 0x10 << state->double_asclk);
 	saa711x_write(sd, R_3A_AUD_CLK_GEN_BASIC_SETUP, acc);
 
 	saa711x_write(sd, R_30_AUD_MAST_CLK_CYCLES_PER_FIELD, acpf & 0xff);
@@ -1259,6 +1264,12 @@ static int saa711x_s_routing(struct v4l2_subdev *sd,
 				(saa711x_read(sd, R_83_X_PORT_I_O_ENA_AND_OUT_CLK) & 0xfe) |
 				(state->output & 0x01));
 	}
+	if (state->ident > V4L2_IDENT_SAA7111A) {
+		if (config & SAA7115_IDQ_IS_DEFAULT)
+			saa711x_write(sd, R_85_I_PORT_SIGNAL_POLAR, 0x20);
+		else
+			saa711x_write(sd, R_85_I_PORT_SIGNAL_POLAR, 0x21);
+	}
 	return 0;
 }
 
@@ -1296,9 +1307,10 @@ static int saa711x_s_crystal_freq(struct v4l2_subdev *sd, u32 freq, u32 flags)
 	if (freq != SAA7115_FREQ_32_11_MHZ && freq != SAA7115_FREQ_24_576_MHZ)
 		return -EINVAL;
 	state->crystal_freq = freq;
+	state->double_asclk = flags & SAA7115_FREQ_FL_DOUBLE_ASCLK;
 	state->cgcdiv = (flags & SAA7115_FREQ_FL_CGCDIV) ? 3 : 4;
-	state->ucgc = (flags & SAA7115_FREQ_FL_UCGC) ? 1 : 0;
-	state->apll = (flags & SAA7115_FREQ_FL_APLL) ? 1 : 0;
+	state->ucgc = flags & SAA7115_FREQ_FL_UCGC;
+	state->apll = flags & SAA7115_FREQ_FL_APLL;
 	saa711x_s_clock_freq(sd, state->audclk_freq);
 	return 0;
 }
@@ -1354,6 +1366,34 @@ static int saa711x_querystd(struct v4l2_subdev *sd, v4l2_std_id *std)
 	 */
 
 	reg1f = saa711x_read(sd, R_1F_STATUS_BYTE_2_VD_DEC);
+
+	if (state->ident == V4L2_IDENT_SAA7115) {
+		reg1e = saa711x_read(sd, R_1E_STATUS_BYTE_1_VD_DEC);
+
+		v4l2_dbg(1, debug, sd, "Status byte 1 (0x1e)=0x%02x\n", reg1e);
+
+		switch (reg1e & 0x03) {
+		case 1:
+			*std &= V4L2_STD_NTSC;
+			break;
+		case 2:
+			/*
+			 * V4L2_STD_PAL just cover the european PAL standards.
+			 * This is wrong, as the device could also be using an
+			 * other PAL standard.
+			 */
+			*std &= V4L2_STD_PAL   | V4L2_STD_PAL_N  | V4L2_STD_PAL_Nc |
+				V4L2_STD_PAL_M | V4L2_STD_PAL_60;
+			break;
+		case 3:
+			*std &= V4L2_STD_SECAM;
+			break;
+		default:
+			/* Can't detect anything */
+			break;
+		}
+	}
+
 	v4l2_dbg(1, debug, sd, "Status byte 2 (0x1f)=0x%02x\n", reg1f);
 
 	/* horizontal/vertical not locked */
@@ -1365,34 +1405,6 @@ static int saa711x_querystd(struct v4l2_subdev *sd, v4l2_std_id *std)
 	else
 		*std &= V4L2_STD_625_50;
 
-	if (state->ident != V4L2_IDENT_SAA7115)
-		goto ret;
-
-	reg1e = saa711x_read(sd, R_1E_STATUS_BYTE_1_VD_DEC);
-
-	switch (reg1e & 0x03) {
-	case 1:
-		*std &= V4L2_STD_NTSC;
-		break;
-	case 2:
-		/*
-		 * V4L2_STD_PAL just cover the european PAL standards.
-		 * This is wrong, as the device could also be using an
-		 * other PAL standard.
-		 */
-		*std &= V4L2_STD_PAL   | V4L2_STD_PAL_N  | V4L2_STD_PAL_Nc |
-			V4L2_STD_PAL_M | V4L2_STD_PAL_60;
-		break;
-	case 3:
-		*std &= V4L2_STD_SECAM;
-		break;
-	default:
-		/* Can't detect anything */
-		break;
-	}
-
-	v4l2_dbg(1, debug, sd, "Status byte 1 (0x1e)=0x%02x\n", reg1e);
-
 ret:
 	v4l2_dbg(1, debug, sd, "detected std mask = %08Lx\n", *std);
 
@@ -1428,7 +1440,7 @@ static int saa711x_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *
 	return 0;
 }
 
-static int saa711x_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int saa711x_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/saa7127.c b/drivers/media/i2c/saa7127.c
index b745f68fbc9..8a47ac10927 100644
--- a/drivers/media/i2c/saa7127.c
+++ b/drivers/media/i2c/saa7127.c
@@ -672,7 +672,7 @@ static int saa7127_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *
 	return 0;
 }
 
-static int saa7127_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int saa7127_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/saa717x.c b/drivers/media/i2c/saa717x.c
index 1e84466515a..cf3a0aa7e45 100644
--- a/drivers/media/i2c/saa717x.c
+++ b/drivers/media/i2c/saa717x.c
@@ -988,7 +988,7 @@ static int saa717x_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *
 	return 0;
 }
 
-static int saa717x_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int saa717x_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	u16 addr = reg->reg & 0xffff;
@@ -1113,7 +1113,7 @@ static int saa717x_s_stream(struct v4l2_subdev *sd, int enable)
 }
 
 /* change audio mode */
-static int saa717x_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+static int saa717x_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
 {
 	struct saa717x_state *decoder = to_state(sd);
 	int audio_mode;
diff --git a/drivers/media/i2c/smiapp/smiapp-core.c b/drivers/media/i2c/smiapp/smiapp-core.c
index 83c7ed7ffcc..cae4f468385 100644
--- a/drivers/media/i2c/smiapp/smiapp-core.c
+++ b/drivers/media/i2c/smiapp/smiapp-core.c
@@ -2833,7 +2833,7 @@ static int smiapp_probe(struct i2c_client *client,
 				 sensor->src->pads, 0);
 }
 
-static int __exit smiapp_remove(struct i2c_client *client)
+static int smiapp_remove(struct i2c_client *client)
 {
 	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
 	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
@@ -2881,7 +2881,7 @@ static struct i2c_driver smiapp_i2c_driver = {
 		.pm = &smiapp_pm_ops,
 	},
 	.probe	= smiapp_probe,
-	.remove	= __exit_p(smiapp_remove),
+	.remove	= smiapp_remove,
 	.id_table = smiapp_id_table,
 };
 
diff --git a/drivers/media/i2c/soc_camera/Kconfig b/drivers/media/i2c/soc_camera/Kconfig
index 6dff2b7ad52..23d352f0adf 100644
--- a/drivers/media/i2c/soc_camera/Kconfig
+++ b/drivers/media/i2c/soc_camera/Kconfig
@@ -9,7 +9,6 @@ config SOC_CAMERA_IMX074
 config SOC_CAMERA_MT9M001
 	tristate "mt9m001 support"
 	depends on SOC_CAMERA && I2C
-	select GPIO_PCA953X if MT9M001_PCA9536_SWITCH
 	help
 	  This driver supports MT9M001 cameras from Micron, monochrome
 	  and colour models.
@@ -36,7 +35,6 @@ config SOC_CAMERA_MT9T112
 config SOC_CAMERA_MT9V022
 	tristate "mt9v022 and mt9v024 support"
 	depends on SOC_CAMERA && I2C
-	select GPIO_PCA953X if MT9V022_PCA9536_SWITCH
 	help
 	  This driver supports MT9V022 cameras from Micron
 
diff --git a/drivers/media/i2c/soc_camera/mt9m001.c b/drivers/media/i2c/soc_camera/mt9m001.c
index bcdc8617554..dd908980575 100644
--- a/drivers/media/i2c/soc_camera/mt9m001.c
+++ b/drivers/media/i2c/soc_camera/mt9m001.c
@@ -360,7 +360,7 @@ static int mt9m001_g_register(struct v4l2_subdev *sd,
 }
 
 static int mt9m001_s_register(struct v4l2_subdev *sd,
-			      struct v4l2_dbg_register *reg)
+			      const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/soc_camera/mt9m111.c b/drivers/media/i2c/soc_camera/mt9m111.c
index bbc4ff99603..8bd4e0d2ea0 100644
--- a/drivers/media/i2c/soc_camera/mt9m111.c
+++ b/drivers/media/i2c/soc_camera/mt9m111.c
@@ -641,7 +641,7 @@ static int mt9m111_g_register(struct v4l2_subdev *sd,
 }
 
 static int mt9m111_s_register(struct v4l2_subdev *sd,
-			      struct v4l2_dbg_register *reg)
+			      const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
@@ -701,11 +701,11 @@ static int mt9m111_set_global_gain(struct mt9m111 *mt9m111, int gain)
 	return reg_write(GLOBAL_GAIN, val);
 }
 
-static int mt9m111_set_autoexposure(struct mt9m111 *mt9m111, int on)
+static int mt9m111_set_autoexposure(struct mt9m111 *mt9m111, int val)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 
-	if (on)
+	if (val == V4L2_EXPOSURE_AUTO)
 		return reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
 	return reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
 }
@@ -785,8 +785,6 @@ static int mt9m111_init(struct mt9m111 *mt9m111)
 	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 	int ret;
 
-	/* Default HIGHPOWER context */
-	mt9m111->ctx = &context_b;
 	ret = mt9m111_enable(mt9m111);
 	if (!ret)
 		ret = mt9m111_reset(mt9m111);
@@ -975,6 +973,9 @@ static int mt9m111_probe(struct i2c_client *client,
 	if (!mt9m111)
 		return -ENOMEM;
 
+	/* Default HIGHPOWER context */
+	mt9m111->ctx = &context_b;
+
 	v4l2_i2c_subdev_init(&mt9m111->subdev, client, &mt9m111_subdev_ops);
 	v4l2_ctrl_handler_init(&mt9m111->hdl, 5);
 	v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
diff --git a/drivers/media/i2c/soc_camera/mt9t031.c b/drivers/media/i2c/soc_camera/mt9t031.c
index d80d044ebf1..26a15b87a9a 100644
--- a/drivers/media/i2c/soc_camera/mt9t031.c
+++ b/drivers/media/i2c/soc_camera/mt9t031.c
@@ -430,7 +430,7 @@ static int mt9t031_g_register(struct v4l2_subdev *sd,
 }
 
 static int mt9t031_s_register(struct v4l2_subdev *sd,
-			      struct v4l2_dbg_register *reg)
+			      const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/soc_camera/mt9t112.c b/drivers/media/i2c/soc_camera/mt9t112.c
index 188e29b0327..a7256b73280 100644
--- a/drivers/media/i2c/soc_camera/mt9t112.c
+++ b/drivers/media/i2c/soc_camera/mt9t112.c
@@ -766,7 +766,7 @@ static int mt9t112_g_register(struct v4l2_subdev *sd,
 }
 
 static int mt9t112_s_register(struct v4l2_subdev *sd,
-			      struct v4l2_dbg_register *reg)
+			      const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	int ret;
diff --git a/drivers/media/i2c/soc_camera/mt9v022.c b/drivers/media/i2c/soc_camera/mt9v022.c
index a5e65d6a078..a295e598486 100644
--- a/drivers/media/i2c/soc_camera/mt9v022.c
+++ b/drivers/media/i2c/soc_camera/mt9v022.c
@@ -275,6 +275,7 @@ static int mt9v022_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct mt9v022 *mt9v022 = to_mt9v022(client);
 	struct v4l2_rect rect = a->c;
+	int min_row, min_blank;
 	int ret;
 
 	/* Bayer format - even size lengths */
@@ -310,13 +311,21 @@ static int mt9v022_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
 		ret = reg_write(client, MT9V022_COLUMN_START, rect.left);
 	if (!ret)
 		ret = reg_write(client, MT9V022_ROW_START, rect.top);
+	/*
+	 * mt9v022: min total row time is 660 columns, min blanking is 43
+	 * mt9v024: min total row time is 690 columns, min blanking is 61
+	 */
+	if (is_mt9v024(mt9v022->chip_version)) {
+		min_row = 690;
+		min_blank = 61;
+	} else {
+		min_row = 660;
+		min_blank = 43;
+	}
 	if (!ret)
-		/*
-		 * Default 94, Phytec driver says:
-		 * "width + horizontal blank >= 660"
-		 */
 		ret = v4l2_ctrl_s_ctrl(mt9v022->hblank,
-				rect.width > 660 - 43 ? 43 : 660 - rect.width);
+				rect.width > min_row - min_blank ?
+				min_blank : min_row - rect.width);
 	if (!ret)
 		ret = v4l2_ctrl_s_ctrl(mt9v022->vblank, 45);
 	if (!ret)
@@ -488,7 +497,7 @@ static int mt9v022_g_register(struct v4l2_subdev *sd,
 }
 
 static int mt9v022_s_register(struct v4l2_subdev *sd,
-			      struct v4l2_dbg_register *reg)
+			      const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/soc_camera/ov2640.c b/drivers/media/i2c/soc_camera/ov2640.c
index 0f520f693b6..e3168424f9b 100644
--- a/drivers/media/i2c/soc_camera/ov2640.c
+++ b/drivers/media/i2c/soc_camera/ov2640.c
@@ -756,7 +756,7 @@ static int ov2640_g_register(struct v4l2_subdev *sd,
 }
 
 static int ov2640_s_register(struct v4l2_subdev *sd,
-			     struct v4l2_dbg_register *reg)
+			     const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/soc_camera/ov5642.c b/drivers/media/i2c/soc_camera/ov5642.c
index 9d53309619d..9aa56de69ee 100644
--- a/drivers/media/i2c/soc_camera/ov5642.c
+++ b/drivers/media/i2c/soc_camera/ov5642.c
@@ -708,7 +708,7 @@ static int ov5642_get_register(struct v4l2_subdev *sd, struct v4l2_dbg_register
 	return ret;
 }
 
-static int ov5642_set_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int ov5642_set_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/soc_camera/ov6650.c b/drivers/media/i2c/soc_camera/ov6650.c
index dbe4f564e6b..991202d4bba 100644
--- a/drivers/media/i2c/soc_camera/ov6650.c
+++ b/drivers/media/i2c/soc_camera/ov6650.c
@@ -421,7 +421,7 @@ static int ov6650_get_register(struct v4l2_subdev *sd,
 }
 
 static int ov6650_set_register(struct v4l2_subdev *sd,
-				struct v4l2_dbg_register *reg)
+				const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/soc_camera/ov772x.c b/drivers/media/i2c/soc_camera/ov772x.c
index fbeb5b2f3ae..713d62e349f 100644
--- a/drivers/media/i2c/soc_camera/ov772x.c
+++ b/drivers/media/i2c/soc_camera/ov772x.c
@@ -652,7 +652,7 @@ static int ov772x_g_register(struct v4l2_subdev *sd,
 }
 
 static int ov772x_s_register(struct v4l2_subdev *sd,
-			     struct v4l2_dbg_register *reg)
+			     const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/soc_camera/ov9640.c b/drivers/media/i2c/soc_camera/ov9640.c
index 05993041be3..20ca62d371c 100644
--- a/drivers/media/i2c/soc_camera/ov9640.c
+++ b/drivers/media/i2c/soc_camera/ov9640.c
@@ -322,7 +322,7 @@ static int ov9640_get_register(struct v4l2_subdev *sd,
 }
 
 static int ov9640_set_register(struct v4l2_subdev *sd,
-				struct v4l2_dbg_register *reg)
+				const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/soc_camera/ov9740.c b/drivers/media/i2c/soc_camera/ov9740.c
index 2f236da8016..012bd627112 100644
--- a/drivers/media/i2c/soc_camera/ov9740.c
+++ b/drivers/media/i2c/soc_camera/ov9740.c
@@ -835,7 +835,7 @@ static int ov9740_get_register(struct v4l2_subdev *sd,
 }
 
 static int ov9740_set_register(struct v4l2_subdev *sd,
-			       struct v4l2_dbg_register *reg)
+			       const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/soc_camera/rj54n1cb0c.c b/drivers/media/i2c/soc_camera/rj54n1cb0c.c
index 5c92679bfef..1f9ec3b06b4 100644
--- a/drivers/media/i2c/soc_camera/rj54n1cb0c.c
+++ b/drivers/media/i2c/soc_camera/rj54n1cb0c.c
@@ -1161,7 +1161,7 @@ static int rj54n1_g_register(struct v4l2_subdev *sd,
 }
 
 static int rj54n1_s_register(struct v4l2_subdev *sd,
-			     struct v4l2_dbg_register *reg)
+			     const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/soc_camera/tw9910.c b/drivers/media/i2c/soc_camera/tw9910.c
index 7d207460188..bad90b16a6d 100644
--- a/drivers/media/i2c/soc_camera/tw9910.c
+++ b/drivers/media/i2c/soc_camera/tw9910.c
@@ -554,7 +554,7 @@ static int tw9910_g_register(struct v4l2_subdev *sd,
 }
 
 static int tw9910_s_register(struct v4l2_subdev *sd,
-			     struct v4l2_dbg_register *reg)
+			     const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/sony-btf-mpx.c b/drivers/media/i2c/sony-btf-mpx.c
new file mode 100644
index 00000000000..38cbea98764
--- /dev/null
+++ b/drivers/media/i2c/sony-btf-mpx.c
@@ -0,0 +1,399 @@
+/*
+ * Copyright (C) 2005-2006 Micronas USA 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.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/tuner.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-device.h>
+#include <linux/slab.h>
+
+MODULE_DESCRIPTION("sony-btf-mpx driver");
+MODULE_LICENSE("GPL v2");
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "debug level 0=off(default) 1=on\n");
+
+/* #define MPX_DEBUG */
+
+/*
+ * Note:
+ *
+ * AS(IF/MPX) pin:      LOW      HIGH/OPEN
+ * IF/MPX address:   0x42/0x40   0x43/0x44
+ */
+
+
+static int force_mpx_mode = -1;
+module_param(force_mpx_mode, int, 0644);
+
+struct sony_btf_mpx {
+	struct v4l2_subdev sd;
+	int mpxmode;
+	u32 audmode;
+};
+
+static inline struct sony_btf_mpx *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct sony_btf_mpx, sd);
+}
+
+static int mpx_write(struct i2c_client *client, int dev, int addr, int val)
+{
+	u8 buffer[5];
+	struct i2c_msg msg;
+
+	buffer[0] = dev;
+	buffer[1] = addr >> 8;
+	buffer[2] = addr & 0xff;
+	buffer[3] = val >> 8;
+	buffer[4] = val & 0xff;
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 5;
+	msg.buf = buffer;
+	i2c_transfer(client->adapter, &msg, 1);
+	return 0;
+}
+
+/*
+ * MPX register values for the BTF-PG472Z:
+ *
+ *                                 FM_     NICAM_  SCART_
+ *          MODUS  SOURCE    ACB   PRESCAL PRESCAL PRESCAL SYSTEM  VOLUME
+ *         10/0030 12/0008 12/0013 12/000E 12/0010 12/0000 10/0020 12/0000
+ *         ---------------------------------------------------------------
+ * Auto     1003    0020    0100    2603    5000    XXXX    0001    7500
+ *
+ * B/G
+ *  Mono    1003    0020    0100    2603    5000    XXXX    0003    7500
+ *  A2      1003    0020    0100    2601    5000    XXXX    0003    7500
+ *  NICAM   1003    0120    0100    2603    5000    XXXX    0008    7500
+ *
+ * I
+ *  Mono    1003    0020    0100    2603    7900    XXXX    000A    7500
+ *  NICAM   1003    0120    0100    2603    7900    XXXX    000A    7500
+ *
+ * D/K
+ *  Mono    1003    0020    0100    2603    5000    XXXX    0004    7500
+ *  A2-1    1003    0020    0100    2601    5000    XXXX    0004    7500
+ *  A2-2    1003    0020    0100    2601    5000    XXXX    0005    7500
+ *  A2-3    1003    0020    0100    2601    5000    XXXX    0007    7500
+ *  NICAM   1003    0120    0100    2603    5000    XXXX    000B    7500
+ *
+ * L/L'
+ *  Mono    0003    0200    0100    7C03    5000    2200    0009    7500
+ *  NICAM   0003    0120    0100    7C03    5000    XXXX    0009    7500
+ *
+ * M
+ *  Mono    1003    0200    0100    2B03    5000    2B00    0002    7500
+ *
+ * For Asia, replace the 0x26XX in FM_PRESCALE with 0x14XX.
+ *
+ * Bilingual selection in A2/NICAM:
+ *
+ *         High byte of SOURCE     Left chan   Right chan
+ *                 0x01              MAIN         SUB
+ *                 0x03              MAIN         MAIN
+ *                 0x04              SUB          SUB
+ *
+ * Force mono in NICAM by setting the high byte of SOURCE to 0x02 (L/L') or
+ * 0x00 (all other bands).  Force mono in A2 with FMONO_A2:
+ *
+ *                      FMONO_A2
+ *                      10/0022
+ *                      --------
+ *     Forced mono ON     07F0
+ *     Forced mono OFF    0190
+ */
+
+static const struct {
+	enum { AUD_MONO, AUD_A2, AUD_NICAM, AUD_NICAM_L } audio_mode;
+	u16 modus;
+	u16 source;
+	u16 acb;
+	u16 fm_prescale;
+	u16 nicam_prescale;
+	u16 scart_prescale;
+	u16 system;
+	u16 volume;
+} mpx_audio_modes[] = {
+	/* Auto */	{ AUD_MONO,	0x1003, 0x0020, 0x0100, 0x2603,
+					0x5000, 0x0000, 0x0001, 0x7500 },
+	/* B/G Mono */	{ AUD_MONO,	0x1003, 0x0020, 0x0100, 0x2603,
+					0x5000, 0x0000, 0x0003, 0x7500 },
+	/* B/G A2 */	{ AUD_A2,	0x1003, 0x0020, 0x0100, 0x2601,
+					0x5000, 0x0000, 0x0003, 0x7500 },
+	/* B/G NICAM */ { AUD_NICAM,	0x1003, 0x0120, 0x0100, 0x2603,
+					0x5000, 0x0000, 0x0008, 0x7500 },
+	/* I Mono */	{ AUD_MONO,	0x1003, 0x0020, 0x0100, 0x2603,
+					0x7900, 0x0000, 0x000A, 0x7500 },
+	/* I NICAM */	{ AUD_NICAM,	0x1003, 0x0120, 0x0100, 0x2603,
+					0x7900, 0x0000, 0x000A, 0x7500 },
+	/* D/K Mono */	{ AUD_MONO,	0x1003, 0x0020, 0x0100, 0x2603,
+					0x5000, 0x0000, 0x0004, 0x7500 },
+	/* D/K A2-1 */	{ AUD_A2,	0x1003, 0x0020, 0x0100, 0x2601,
+					0x5000, 0x0000, 0x0004, 0x7500 },
+	/* D/K A2-2 */	{ AUD_A2,	0x1003, 0x0020, 0x0100, 0x2601,
+					0x5000, 0x0000, 0x0005, 0x7500 },
+	/* D/K A2-3 */	{ AUD_A2,	0x1003, 0x0020, 0x0100, 0x2601,
+					0x5000, 0x0000, 0x0007, 0x7500 },
+	/* D/K NICAM */	{ AUD_NICAM,	0x1003, 0x0120, 0x0100, 0x2603,
+					0x5000, 0x0000, 0x000B, 0x7500 },
+	/* L/L' Mono */	{ AUD_MONO,	0x0003, 0x0200, 0x0100, 0x7C03,
+					0x5000, 0x2200, 0x0009, 0x7500 },
+	/* L/L' NICAM */{ AUD_NICAM_L,	0x0003, 0x0120, 0x0100, 0x7C03,
+					0x5000, 0x0000, 0x0009, 0x7500 },
+};
+
+#define MPX_NUM_MODES	ARRAY_SIZE(mpx_audio_modes)
+
+static int mpx_setup(struct sony_btf_mpx *t)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&t->sd);
+	u16 source = 0;
+	u8 buffer[3];
+	struct i2c_msg msg;
+	int mode = t->mpxmode;
+
+	/* reset MPX */
+	buffer[0] = 0x00;
+	buffer[1] = 0x80;
+	buffer[2] = 0x00;
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 3;
+	msg.buf = buffer;
+	i2c_transfer(client->adapter, &msg, 1);
+	buffer[1] = 0x00;
+	i2c_transfer(client->adapter, &msg, 1);
+
+	if (t->audmode != V4L2_TUNER_MODE_MONO)
+		mode++;
+
+	if (mpx_audio_modes[mode].audio_mode != AUD_MONO) {
+		switch (t->audmode) {
+		case V4L2_TUNER_MODE_MONO:
+			switch (mpx_audio_modes[mode].audio_mode) {
+			case AUD_A2:
+				source = mpx_audio_modes[mode].source;
+				break;
+			case AUD_NICAM:
+				source = 0x0000;
+				break;
+			case AUD_NICAM_L:
+				source = 0x0200;
+				break;
+			default:
+				break;
+			}
+			break;
+		case V4L2_TUNER_MODE_STEREO:
+			source = mpx_audio_modes[mode].source;
+			break;
+		case V4L2_TUNER_MODE_LANG1:
+			source = 0x0300;
+			break;
+		case V4L2_TUNER_MODE_LANG2:
+			source = 0x0400;
+			break;
+		}
+		source |= mpx_audio_modes[mode].source & 0x00ff;
+	} else
+		source = mpx_audio_modes[mode].source;
+
+	mpx_write(client, 0x10, 0x0030, mpx_audio_modes[mode].modus);
+	mpx_write(client, 0x12, 0x0008, source);
+	mpx_write(client, 0x12, 0x0013, mpx_audio_modes[mode].acb);
+	mpx_write(client, 0x12, 0x000e,
+			mpx_audio_modes[mode].fm_prescale);
+	mpx_write(client, 0x12, 0x0010,
+			mpx_audio_modes[mode].nicam_prescale);
+	mpx_write(client, 0x12, 0x000d,
+			mpx_audio_modes[mode].scart_prescale);
+	mpx_write(client, 0x10, 0x0020, mpx_audio_modes[mode].system);
+	mpx_write(client, 0x12, 0x0000, mpx_audio_modes[mode].volume);
+	if (mpx_audio_modes[mode].audio_mode == AUD_A2)
+		mpx_write(client, 0x10, 0x0022,
+			t->audmode == V4L2_TUNER_MODE_MONO ? 0x07f0 : 0x0190);
+
+#ifdef MPX_DEBUG
+	{
+		u8 buf1[3], buf2[2];
+		struct i2c_msg msgs[2];
+
+		v4l2_info(client,
+			"MPX registers: %04x %04x %04x %04x %04x %04x %04x %04x\n",
+			mpx_audio_modes[mode].modus,
+			source,
+			mpx_audio_modes[mode].acb,
+			mpx_audio_modes[mode].fm_prescale,
+			mpx_audio_modes[mode].nicam_prescale,
+			mpx_audio_modes[mode].scart_prescale,
+			mpx_audio_modes[mode].system,
+			mpx_audio_modes[mode].volume);
+		buf1[0] = 0x11;
+		buf1[1] = 0x00;
+		buf1[2] = 0x7e;
+		msgs[0].addr = client->addr;
+		msgs[0].flags = 0;
+		msgs[0].len = 3;
+		msgs[0].buf = buf1;
+		msgs[1].addr = client->addr;
+		msgs[1].flags = I2C_M_RD;
+		msgs[1].len = 2;
+		msgs[1].buf = buf2;
+		i2c_transfer(client->adapter, msgs, 2);
+		v4l2_info(client, "MPX system: %02x%02x\n",
+				buf2[0], buf2[1]);
+		buf1[0] = 0x11;
+		buf1[1] = 0x02;
+		buf1[2] = 0x00;
+		i2c_transfer(client->adapter, msgs, 2);
+		v4l2_info(client, "MPX status: %02x%02x\n",
+				buf2[0], buf2[1]);
+	}
+#endif
+	return 0;
+}
+
+
+static int sony_btf_mpx_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct sony_btf_mpx *t = to_state(sd);
+	int default_mpx_mode = 0;
+
+	if (std & V4L2_STD_PAL_BG)
+		default_mpx_mode = 1;
+	else if (std & V4L2_STD_PAL_I)
+		default_mpx_mode = 4;
+	else if (std & V4L2_STD_PAL_DK)
+		default_mpx_mode = 6;
+	else if (std & V4L2_STD_SECAM_L)
+		default_mpx_mode = 11;
+
+	if (default_mpx_mode != t->mpxmode) {
+		t->mpxmode = default_mpx_mode;
+		mpx_setup(t);
+	}
+	return 0;
+}
+
+static int sony_btf_mpx_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+{
+	struct sony_btf_mpx *t = to_state(sd);
+
+	vt->capability = V4L2_TUNER_CAP_NORM |
+		V4L2_TUNER_CAP_STEREO | V4L2_TUNER_CAP_LANG1 |
+		V4L2_TUNER_CAP_LANG2;
+	vt->rxsubchans = V4L2_TUNER_SUB_MONO |
+		V4L2_TUNER_SUB_STEREO | V4L2_TUNER_SUB_LANG1 |
+		V4L2_TUNER_SUB_LANG2;
+	vt->audmode = t->audmode;
+	return 0;
+}
+
+static int sony_btf_mpx_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
+{
+	struct sony_btf_mpx *t = to_state(sd);
+
+	if (vt->type != V4L2_TUNER_ANALOG_TV)
+		return -EINVAL;
+
+	if (vt->audmode != t->audmode) {
+		t->audmode = vt->audmode;
+		mpx_setup(t);
+	}
+	return 0;
+}
+
+/* --------------------------------------------------------------------------*/
+
+static const struct v4l2_subdev_core_ops sony_btf_mpx_core_ops = {
+	.s_std = sony_btf_mpx_s_std,
+};
+
+static const struct v4l2_subdev_tuner_ops sony_btf_mpx_tuner_ops = {
+	.s_tuner = sony_btf_mpx_s_tuner,
+	.g_tuner = sony_btf_mpx_g_tuner,
+};
+
+static const struct v4l2_subdev_ops sony_btf_mpx_ops = {
+	.core = &sony_btf_mpx_core_ops,
+	.tuner = &sony_btf_mpx_tuner_ops,
+};
+
+/* --------------------------------------------------------------------------*/
+
+static int sony_btf_mpx_probe(struct i2c_client *client,
+				const struct i2c_device_id *id)
+{
+	struct sony_btf_mpx *t;
+	struct v4l2_subdev *sd;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_I2C_BLOCK))
+		return -ENODEV;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	t = kzalloc(sizeof(struct sony_btf_mpx), GFP_KERNEL);
+	if (t == NULL)
+		return -ENOMEM;
+
+	sd = &t->sd;
+	v4l2_i2c_subdev_init(sd, client, &sony_btf_mpx_ops);
+
+	/* Initialize sony_btf_mpx */
+	t->mpxmode = 0;
+	t->audmode = V4L2_TUNER_MODE_STEREO;
+
+	return 0;
+}
+
+static int sony_btf_mpx_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(to_state(sd));
+
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id sony_btf_mpx_id[] = {
+	{ "sony-btf-mpx", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, sony_btf_mpx_id);
+
+static struct i2c_driver sony_btf_mpx_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "sony-btf-mpx",
+	},
+	.probe = sony_btf_mpx_probe,
+	.remove = sony_btf_mpx_remove,
+	.id_table = sony_btf_mpx_id,
+};
+module_i2c_driver(sony_btf_mpx_driver);
diff --git a/drivers/media/i2c/tda7432.c b/drivers/media/i2c/tda7432.c
index f7707e65761..28b5121881f 100644
--- a/drivers/media/i2c/tda7432.c
+++ b/drivers/media/i2c/tda7432.c
@@ -35,6 +35,7 @@
 
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
 #include <media/i2c-addr.h>
 
 #ifndef VIDEO_AUDIO_BALANCE
@@ -60,13 +61,17 @@ MODULE_PARM_DESC(maxvol, "Set maximium volume to +20dB(0) else +0dB(1). Default
 
 struct tda7432 {
 	struct v4l2_subdev sd;
-	int addr;
-	int input;
-	int volume;
-	int muted;
-	int bass, treble;
-	int lf, lr, rf, rr;
-	int loud;
+	struct v4l2_ctrl_handler hdl;
+	struct {
+		/* bass/treble cluster */
+		struct v4l2_ctrl *bass;
+		struct v4l2_ctrl *treble;
+	};
+	struct {
+		/* mute/balance cluster */
+		struct v4l2_ctrl *mute;
+		struct v4l2_ctrl *balance;
+	};
 };
 
 static inline struct tda7432 *to_state(struct v4l2_subdev *sd)
@@ -74,6 +79,11 @@ static inline struct tda7432 *to_state(struct v4l2_subdev *sd)
 	return container_of(sd, struct tda7432, sd);
 }
 
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct tda7432, hdl)->sd;
+}
+
 /* The TDA7432 is made by STS-Thompson
  * http://www.st.com
  * http://us.st.com/stonline/books/pdf/docs/4056.pdf
@@ -227,24 +237,22 @@ static int tda7432_write(struct v4l2_subdev *sd, int subaddr, int val)
 static int tda7432_set(struct v4l2_subdev *sd)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	struct tda7432 *t = to_state(sd);
 	unsigned char buf[16];
 
-	v4l2_dbg(1, debug, sd,
-		"tda7432: 7432_set(0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x)\n",
-		t->input, t->volume, t->bass, t->treble, t->lf, t->lr,
-		t->rf, t->rr, t->loud);
 	buf[0]  = TDA7432_IN;
-	buf[1]  = t->input;
-	buf[2]  = t->volume;
-	buf[3]  = t->bass;
-	buf[4]	= t->treble;
-	buf[5]  = t->lf;
-	buf[6]  = t->lr;
-	buf[7]  = t->rf;
-	buf[8]  = t->rr;
-	buf[9]  = t->loud;
-	if (10 != i2c_master_send(client, buf, 10)) {
+	buf[1]  = TDA7432_STEREO_IN |  /* Main (stereo) input   */
+		  TDA7432_BASS_SYM  |  /* Symmetric bass cut    */
+		  TDA7432_BASS_NORM;   /* Normal bass range     */
+	buf[2]  = 0x3b;
+	if (loudness)			 /* Turn loudness on?     */
+		buf[2] |= TDA7432_LD_ON;
+	buf[3]  = TDA7432_TREBLE_0DB | (TDA7432_BASS_0DB << 4);
+	buf[4]  = TDA7432_ATTEN_0DB;
+	buf[5]  = TDA7432_ATTEN_0DB;
+	buf[6]  = TDA7432_ATTEN_0DB;
+	buf[7]  = TDA7432_ATTEN_0DB;
+	buf[8]  = loudness;
+	if (9 != i2c_master_send(client, buf, 9)) {
 		v4l2_err(sd, "I/O error, trying tda7432_set\n");
 		return -1;
 	}
@@ -252,174 +260,86 @@ static int tda7432_set(struct v4l2_subdev *sd)
 	return 0;
 }
 
-static void do_tda7432_init(struct v4l2_subdev *sd)
-{
-	struct tda7432 *t = to_state(sd);
-
-	v4l2_dbg(2, debug, sd, "In tda7432_init\n");
-
-	t->input  = TDA7432_STEREO_IN |  /* Main (stereo) input   */
-		    TDA7432_BASS_SYM  |  /* Symmetric bass cut    */
-		    TDA7432_BASS_NORM;   /* Normal bass range     */
-	t->volume =  0x3b ;				 /* -27dB Volume            */
-	if (loudness)			 /* Turn loudness on?     */
-		t->volume |= TDA7432_LD_ON;
-	t->muted    = 1;
-	t->treble   = TDA7432_TREBLE_0DB; /* 0dB Treble            */
-	t->bass		= TDA7432_BASS_0DB; 	 /* 0dB Bass              */
-	t->lf     = TDA7432_ATTEN_0DB;	 /* 0dB attenuation       */
-	t->lr     = TDA7432_ATTEN_0DB;	 /* 0dB attenuation       */
-	t->rf     = TDA7432_ATTEN_0DB;	 /* 0dB attenuation       */
-	t->rr     = TDA7432_ATTEN_0DB;	 /* 0dB attenuation       */
-	t->loud   = loudness;		 /* insmod parameter      */
-
-	tda7432_set(sd);
-}
-
-static int tda7432_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int tda7432_log_status(struct v4l2_subdev *sd)
 {
-	struct tda7432 *t = to_state(sd);
+	struct tda7432 *state = to_state(sd);
 
-	switch (ctrl->id) {
-	case V4L2_CID_AUDIO_MUTE:
-		ctrl->value=t->muted;
-		return 0;
-	case V4L2_CID_AUDIO_VOLUME:
-		if (!maxvol){  /* max +20db */
-			ctrl->value = ( 0x6f - (t->volume & 0x7F) ) * 630;
-		} else {       /* max 0db   */
-			ctrl->value = ( 0x6f - (t->volume & 0x7F) ) * 829;
-		}
-		return 0;
-	case V4L2_CID_AUDIO_BALANCE:
-	{
-		if ( (t->lf) < (t->rf) )
-			/* right is attenuated, balance shifted left */
-			ctrl->value = (32768 - 1057*(t->rf));
-		else
-			/* left is attenuated, balance shifted right */
-			ctrl->value = (32768 + 1057*(t->lf));
-		return 0;
-	}
-	case V4L2_CID_AUDIO_BASS:
-	{
-		/* Bass/treble 4 bits each */
-		int bass=t->bass;
-		if(bass >= 0x8)
-			bass = ~(bass - 0x8) & 0xf;
-		ctrl->value = (bass << 12)+(bass << 8)+(bass << 4)+(bass);
-		return 0;
-	}
-	case V4L2_CID_AUDIO_TREBLE:
-	{
-		int treble=t->treble;
-		if(treble >= 0x8)
-			treble = ~(treble - 0x8) & 0xf;
-		ctrl->value = (treble << 12)+(treble << 8)+(treble << 4)+(treble);
-		return 0;
-	}
-	}
-	return -EINVAL;
+	v4l2_ctrl_handler_log_status(&state->hdl, sd->name);
+	return 0;
 }
 
-static int tda7432_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int tda7432_s_ctrl(struct v4l2_ctrl *ctrl)
 {
+	struct v4l2_subdev *sd = to_sd(ctrl);
 	struct tda7432 *t = to_state(sd);
+	u8 bass, treble, volume;
+	u8 lf, lr, rf, rr;
 
 	switch (ctrl->id) {
 	case V4L2_CID_AUDIO_MUTE:
-		t->muted=ctrl->value;
-		break;
-	case V4L2_CID_AUDIO_VOLUME:
-		if(!maxvol){ /* max +20db */
-			t->volume = 0x6f - ((ctrl->value)/630);
-		} else {    /* max 0db   */
-			t->volume = 0x6f - ((ctrl->value)/829);
-		}
-		if (loudness)		/* Turn on the loudness bit */
-			t->volume |= TDA7432_LD_ON;
-
-		tda7432_write(sd, TDA7432_VL, t->volume);
-		return 0;
-	case V4L2_CID_AUDIO_BALANCE:
-		if (ctrl->value < 32768) {
+		if (t->balance->val < 0) {
 			/* shifted to left, attenuate right */
-			t->rr = (32768 - ctrl->value)/1057;
-			t->rf = t->rr;
-			t->lr = TDA7432_ATTEN_0DB;
-			t->lf = TDA7432_ATTEN_0DB;
-		} else if(ctrl->value > 32769) {
+			rr = rf = -t->balance->val;
+			lr = lf = TDA7432_ATTEN_0DB;
+		} else if (t->balance->val > 0) {
 			/* shifted to right, attenuate left */
-			t->lf = (ctrl->value - 32768)/1057;
-			t->lr = t->lf;
-			t->rr = TDA7432_ATTEN_0DB;
-			t->rf = TDA7432_ATTEN_0DB;
+			rr = rf = TDA7432_ATTEN_0DB;
+			lr = lf = t->balance->val;
 		} else {
 			/* centered */
-			t->rr = TDA7432_ATTEN_0DB;
-			t->rf = TDA7432_ATTEN_0DB;
-			t->lf = TDA7432_ATTEN_0DB;
-			t->lr = TDA7432_ATTEN_0DB;
+			rr = rf = TDA7432_ATTEN_0DB;
+			lr = lf = TDA7432_ATTEN_0DB;
 		}
-		break;
-	case V4L2_CID_AUDIO_BASS:
-		t->bass = ctrl->value >> 12;
-		if(t->bass>= 0x8)
-				t->bass = (~t->bass & 0xf) + 0x8 ;
-
-		tda7432_write(sd, TDA7432_TN, 0x10 | (t->bass << 4) | t->treble);
+		if (t->mute->val) {
+			lf |= TDA7432_MUTE;
+			lr |= TDA7432_MUTE;
+			lf |= TDA7432_MUTE;
+			rr |= TDA7432_MUTE;
+		}
+		/* Mute & update balance*/
+		tda7432_write(sd, TDA7432_LF, lf);
+		tda7432_write(sd, TDA7432_LR, lr);
+		tda7432_write(sd, TDA7432_RF, rf);
+		tda7432_write(sd, TDA7432_RR, rr);
 		return 0;
-	case V4L2_CID_AUDIO_TREBLE:
-		t->treble= ctrl->value >> 12;
-		if(t->treble>= 0x8)
-				t->treble = (~t->treble & 0xf) + 0x8 ;
+	case V4L2_CID_AUDIO_VOLUME:
+		volume = 0x6f - ctrl->val;
+		if (loudness)		/* Turn on the loudness bit */
+			volume |= TDA7432_LD_ON;
 
-		tda7432_write(sd, TDA7432_TN, 0x10 | (t->bass << 4) | t->treble);
+		tda7432_write(sd, TDA7432_VL, volume);
 		return 0;
-	default:
-		return -EINVAL;
-	}
-
-	/* Used for both mute and balance changes */
-	if (t->muted)
-	{
-		/* Mute & update balance*/
-		tda7432_write(sd, TDA7432_LF, t->lf | TDA7432_MUTE);
-		tda7432_write(sd, TDA7432_LR, t->lr | TDA7432_MUTE);
-		tda7432_write(sd, TDA7432_RF, t->rf | TDA7432_MUTE);
-		tda7432_write(sd, TDA7432_RR, t->rr | TDA7432_MUTE);
-	} else {
-		tda7432_write(sd, TDA7432_LF, t->lf);
-		tda7432_write(sd, TDA7432_LR, t->lr);
-		tda7432_write(sd, TDA7432_RF, t->rf);
-		tda7432_write(sd, TDA7432_RR, t->rr);
-	}
-	return 0;
-}
-
-static int tda7432_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc)
-{
-	switch (qc->id) {
-	case V4L2_CID_AUDIO_VOLUME:
-		return v4l2_ctrl_query_fill(qc, 0, 65535, 65535 / 100, 58880);
-	case V4L2_CID_AUDIO_MUTE:
-		return v4l2_ctrl_query_fill(qc, 0, 1, 1, 0);
-	case V4L2_CID_AUDIO_BALANCE:
 	case V4L2_CID_AUDIO_BASS:
-	case V4L2_CID_AUDIO_TREBLE:
-		return v4l2_ctrl_query_fill(qc, 0, 65535, 65535 / 100, 32768);
+		bass = t->bass->val;
+		treble = t->treble->val;
+		if (bass >= 0x8)
+			bass = 14 - (bass - 8);
+		if (treble >= 0x8)
+			treble = 14 - (treble - 8);
+
+		tda7432_write(sd, TDA7432_TN, 0x10 | (bass << 4) | treble);
+		return 0;
 	}
 	return -EINVAL;
 }
 
 /* ----------------------------------------------------------------------- */
 
-static const struct v4l2_subdev_core_ops tda7432_core_ops = {
-	.queryctrl = tda7432_queryctrl,
-	.g_ctrl = tda7432_g_ctrl,
+static const struct v4l2_ctrl_ops tda7432_ctrl_ops = {
 	.s_ctrl = tda7432_s_ctrl,
 };
 
+static const struct v4l2_subdev_core_ops tda7432_core_ops = {
+	.log_status = tda7432_log_status,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
+};
+
 static const struct v4l2_subdev_ops tda7432_ops = {
 	.core = &tda7432_core_ops,
 };
@@ -444,6 +364,28 @@ static int tda7432_probe(struct i2c_client *client,
 		return -ENOMEM;
 	sd = &t->sd;
 	v4l2_i2c_subdev_init(sd, client, &tda7432_ops);
+	v4l2_ctrl_handler_init(&t->hdl, 5);
+	v4l2_ctrl_new_std(&t->hdl, &tda7432_ctrl_ops,
+		V4L2_CID_AUDIO_VOLUME, 0, maxvol ? 0x68 : 0x4f, 1, maxvol ? 0x5d : 0x47);
+	t->mute = v4l2_ctrl_new_std(&t->hdl, &tda7432_ctrl_ops,
+		V4L2_CID_AUDIO_MUTE, 0, 1, 1, 0);
+	t->balance = v4l2_ctrl_new_std(&t->hdl, &tda7432_ctrl_ops,
+		V4L2_CID_AUDIO_BALANCE, -31, 31, 1, 0);
+	t->bass = v4l2_ctrl_new_std(&t->hdl, &tda7432_ctrl_ops,
+		V4L2_CID_AUDIO_BASS, 0, 14, 1, 7);
+	t->treble = v4l2_ctrl_new_std(&t->hdl, &tda7432_ctrl_ops,
+		V4L2_CID_AUDIO_TREBLE, 0, 14, 1, 7);
+	sd->ctrl_handler = &t->hdl;
+	if (t->hdl.error) {
+		int err = t->hdl.error;
+
+		v4l2_ctrl_handler_free(&t->hdl);
+		kfree(t);
+		return err;
+	}
+	v4l2_ctrl_cluster(2, &t->bass);
+	v4l2_ctrl_cluster(2, &t->mute);
+	v4l2_ctrl_handler_setup(&t->hdl);
 	if (loudness < 0 || loudness > 15) {
 		v4l2_warn(sd, "loudness parameter must be between 0 and 15\n");
 		if (loudness < 0)
@@ -452,17 +394,19 @@ static int tda7432_probe(struct i2c_client *client,
 			loudness = 15;
 	}
 
-	do_tda7432_init(sd);
+	tda7432_set(sd);
 	return 0;
 }
 
 static int tda7432_remove(struct i2c_client *client)
 {
 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct tda7432 *t = to_state(sd);
 
-	do_tda7432_init(sd);
+	tda7432_set(sd);
 	v4l2_device_unregister_subdev(sd);
-	kfree(to_state(sd));
+	v4l2_ctrl_handler_free(&t->hdl);
+	kfree(t);
 	return 0;
 }
 
diff --git a/drivers/media/i2c/tda9840.c b/drivers/media/i2c/tda9840.c
index 3d7ddd93282..01441e35d88 100644
--- a/drivers/media/i2c/tda9840.c
+++ b/drivers/media/i2c/tda9840.c
@@ -87,7 +87,7 @@ static int tda9840_status(struct v4l2_subdev *sd)
 	return byte & 0x60;
 }
 
-static int tda9840_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *t)
+static int tda9840_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *t)
 {
 	int stat = tda9840_status(sd);
 	int byte;
diff --git a/drivers/media/i2c/ths7303.c b/drivers/media/i2c/ths7303.c
index e747524ba6e..c4339556a2e 100644
--- a/drivers/media/i2c/ths7303.c
+++ b/drivers/media/i2c/ths7303.c
@@ -1,7 +1,15 @@
 /*
- * ths7303- THS7303 Video Amplifier driver
+ * ths7303/53- THS7303/53 Video Amplifier driver
  *
  * Copyright (C) 2009 Texas Instruments Incorporated - http://www.ti.com/
+ * Copyright 2013 Cisco Systems, Inc. and/or its affiliates.
+ *
+ * Author: Chaithrika U S <chaithrika@ti.com>
+ *
+ * Contributors:
+ *     Hans Verkuil <hans.verkuil@cisco.com>
+ *     Lad, Prabhakar <prabhakar.lad@ti.com>
+ *     Martin Bugge <marbugge@cisco.com>
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License as
@@ -13,25 +21,27 @@
  * GNU General Public License for more details.
  */
 
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/ctype.h>
-#include <linux/slab.h>
 #include <linux/i2c.h>
-#include <linux/device.h>
-#include <linux/delay.h>
 #include <linux/module.h>
-#include <linux/uaccess.h>
-#include <linux/videodev2.h>
+#include <linux/slab.h>
 
-#include <media/v4l2-device.h>
-#include <media/v4l2-subdev.h>
+#include <media/ths7303.h>
 #include <media/v4l2-chip-ident.h>
+#include <media/v4l2-device.h>
 
 #define THS7303_CHANNEL_1	1
 #define THS7303_CHANNEL_2	2
 #define THS7303_CHANNEL_3	3
 
+struct ths7303_state {
+	struct v4l2_subdev		sd;
+	struct ths7303_platform_data	pdata;
+	struct v4l2_bt_timings		bt;
+	int std_id;
+	int stream_on;
+	int driver_data;
+};
+
 enum ths7303_filter_mode {
 	THS7303_FILTER_MODE_480I_576I,
 	THS7303_FILTER_MODE_480P_576P,
@@ -48,64 +58,84 @@ static int debug;
 module_param(debug, int, 0644);
 MODULE_PARM_DESC(debug, "Debug level 0-1");
 
+static inline struct ths7303_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ths7303_state, sd);
+}
+
+static int ths7303_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_read_byte_data(client, reg);
+}
+
+static int ths7303_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	int i;
+
+	for (i = 0; i < 3; i++) {
+		ret = i2c_smbus_write_byte_data(client, reg, val);
+		if (ret == 0)
+			return 0;
+	}
+	return ret;
+}
+
 /* following function is used to set ths7303 */
 int ths7303_setval(struct v4l2_subdev *sd, enum ths7303_filter_mode mode)
 {
-	u8 input_bias_chroma = 3;
-	u8 input_bias_luma = 3;
-	int disable = 0;
-	int err = 0;
-	u8 val = 0;
-	u8 temp;
-
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ths7303_state *state = to_state(sd);
+	struct ths7303_platform_data *pdata = &state->pdata;
+	u8 val, sel = 0;
+	int err, disable = 0;
 
 	if (!client)
 		return -EINVAL;
 
 	switch (mode) {
 	case THS7303_FILTER_MODE_1080P:
-		val = (3 << 6);
-		val |= (3 << 3);
+		sel = 0x3;	/*1080p and SXGA/UXGA */
 		break;
 	case THS7303_FILTER_MODE_720P_1080I:
-		val = (2 << 6);
-		val |= (2 << 3);
+		sel = 0x2;	/*720p, 1080i and SVGA/XGA */
 		break;
 	case THS7303_FILTER_MODE_480P_576P:
-		val = (1 << 6);
-		val |= (1 << 3);
+		sel = 0x1;	/* EDTV 480p/576p and VGA */
 		break;
 	case THS7303_FILTER_MODE_480I_576I:
+		sel = 0x0;	/* SDTV, S-Video, 480i/576i */
 		break;
-	case THS7303_FILTER_MODE_DISABLE:
-		pr_info("mode disabled\n");
-		/* disable all channels */
-		disable = 1;
 	default:
 		/* disable all channels */
 		disable = 1;
 	}
-	/* Setup channel 2 - Luma - Green */
-	temp = val;
+
+	val = (sel << 6) | (sel << 3);
 	if (!disable)
-		val |= input_bias_luma;
-	err = i2c_smbus_write_byte_data(client, THS7303_CHANNEL_2, val);
+		val |= (pdata->ch_1 & 0x27);
+	err = ths7303_write(sd, THS7303_CHANNEL_1, val);
 	if (err)
 		goto out;
 
-	/* setup two chroma channels */
+	val = (sel << 6) | (sel << 3);
 	if (!disable)
-		temp |= input_bias_chroma;
-
-	err = i2c_smbus_write_byte_data(client, THS7303_CHANNEL_1, temp);
+		val |= (pdata->ch_2 & 0x27);
+	err = ths7303_write(sd, THS7303_CHANNEL_2, val);
 	if (err)
 		goto out;
 
-	err = i2c_smbus_write_byte_data(client, THS7303_CHANNEL_3, temp);
+	val = (sel << 6) | (sel << 3);
+	if (!disable)
+		val |= (pdata->ch_3 & 0x27);
+	err = ths7303_write(sd, THS7303_CHANNEL_3, val);
 	if (err)
 		goto out;
-	return err;
+
+	return 0;
 out:
 	pr_info("write byte data failed\n");
 	return err;
@@ -113,49 +143,209 @@ out:
 
 static int ths7303_s_std_output(struct v4l2_subdev *sd, v4l2_std_id norm)
 {
-	if (norm & (V4L2_STD_ALL & ~V4L2_STD_SECAM))
+	struct ths7303_state *state = to_state(sd);
+
+	if (norm & (V4L2_STD_ALL & ~V4L2_STD_SECAM)) {
+		state->std_id = 1;
+		state->bt.pixelclock = 0;
 		return ths7303_setval(sd, THS7303_FILTER_MODE_480I_576I);
-	else
-		return ths7303_setval(sd, THS7303_FILTER_MODE_DISABLE);
+	}
+
+	return ths7303_setval(sd, THS7303_FILTER_MODE_DISABLE);
 }
 
-/* for setting filter for HD output */
-static int ths7303_s_dv_timings(struct v4l2_subdev *sd,
-			       struct v4l2_dv_timings *dv_timings)
+static int ths7303_config(struct v4l2_subdev *sd)
 {
-	u32 height = dv_timings->bt.height;
-	int interlaced = dv_timings->bt.interlaced;
-	int res = 0;
+	struct ths7303_state *state = to_state(sd);
+	int res;
+
+	if (!state->stream_on) {
+		ths7303_write(sd, THS7303_CHANNEL_1,
+			      (ths7303_read(sd, THS7303_CHANNEL_1) & 0xf8) |
+			      0x00);
+		ths7303_write(sd, THS7303_CHANNEL_2,
+			      (ths7303_read(sd, THS7303_CHANNEL_2) & 0xf8) |
+			      0x00);
+		ths7303_write(sd, THS7303_CHANNEL_3,
+			      (ths7303_read(sd, THS7303_CHANNEL_3) & 0xf8) |
+			      0x00);
+		return 0;
+	}
 
-	if (height == 1080 && !interlaced)
+	if (state->bt.pixelclock > 120000000)
 		res = ths7303_setval(sd, THS7303_FILTER_MODE_1080P);
-	else if ((height == 720 && !interlaced) ||
-			(height == 1080 && interlaced))
+	else if (state->bt.pixelclock > 70000000)
 		res = ths7303_setval(sd, THS7303_FILTER_MODE_720P_1080I);
-	else if ((height == 480 || height == 576) && !interlaced)
+	else if (state->bt.pixelclock > 20000000)
 		res = ths7303_setval(sd, THS7303_FILTER_MODE_480P_576P);
+	else if (state->std_id)
+		res = ths7303_setval(sd, THS7303_FILTER_MODE_480I_576I);
 	else
 		/* disable all channels */
 		res = ths7303_setval(sd, THS7303_FILTER_MODE_DISABLE);
 
 	return res;
+
+}
+
+static int ths7303_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ths7303_state *state = to_state(sd);
+
+	state->stream_on = enable;
+
+	return ths7303_config(sd);
+}
+
+/* for setting filter for HD output */
+static int ths7303_s_dv_timings(struct v4l2_subdev *sd,
+			       struct v4l2_dv_timings *dv_timings)
+{
+	struct ths7303_state *state = to_state(sd);
+
+	if (!dv_timings || dv_timings->type != V4L2_DV_BT_656_1120)
+		return -EINVAL;
+
+	state->bt = dv_timings->bt;
+	state->std_id = 0;
+
+	return ths7303_config(sd);
 }
 
 static int ths7303_g_chip_ident(struct v4l2_subdev *sd,
 				struct v4l2_dbg_chip_ident *chip)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ths7303_state *state = to_state(sd);
 
-	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_THS7303, 0);
+	return v4l2_chip_ident_i2c_client(client, chip, state->driver_data, 0);
 }
 
 static const struct v4l2_subdev_video_ops ths7303_video_ops = {
+	.s_stream	= ths7303_s_stream,
 	.s_std_output	= ths7303_s_std_output,
-	.s_dv_timings    = ths7303_s_dv_timings,
+	.s_dv_timings   = ths7303_s_dv_timings,
 };
 
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+
+static int ths7303_g_register(struct v4l2_subdev *sd,
+			      struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+
+	reg->size = 1;
+	reg->val = ths7303_read(sd, reg->reg);
+	return 0;
+}
+
+static int ths7303_s_register(struct v4l2_subdev *sd,
+			      const struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+
+	ths7303_write(sd, reg->reg, reg->val);
+	return 0;
+}
+#endif
+
+static const char * const stc_lpf_sel_txt[4] = {
+	"500-kHz Filter",
+	"2.5-MHz Filter",
+	"5-MHz Filter",
+	"5-MHz Filter",
+};
+
+static const char * const in_mux_sel_txt[2] = {
+	"Input A Select",
+	"Input B Select",
+};
+
+static const char * const lpf_freq_sel_txt[4] = {
+	"9-MHz LPF",
+	"16-MHz LPF",
+	"35-MHz LPF",
+	"Bypass LPF",
+};
+
+static const char * const in_bias_sel_dis_cont_txt[8] = {
+	"Disable Channel",
+	"Mute Function - No Output",
+	"DC Bias Select",
+	"DC Bias + 250 mV Offset Select",
+	"AC Bias Select",
+	"Sync Tip Clamp with low bias",
+	"Sync Tip Clamp with mid bias",
+	"Sync Tip Clamp with high bias",
+};
+
+static void ths7303_log_channel_status(struct v4l2_subdev *sd, u8 reg)
+{
+	u8 val = ths7303_read(sd, reg);
+
+	if ((val & 0x7) == 0) {
+		v4l2_info(sd, "Channel %d Off\n", reg);
+		return;
+	}
+
+	v4l2_info(sd, "Channel %d On\n", reg);
+	v4l2_info(sd, "  value 0x%x\n", val);
+	v4l2_info(sd, "  %s\n", stc_lpf_sel_txt[(val >> 6) & 0x3]);
+	v4l2_info(sd, "  %s\n", in_mux_sel_txt[(val >> 5) & 0x1]);
+	v4l2_info(sd, "  %s\n", lpf_freq_sel_txt[(val >> 3) & 0x3]);
+	v4l2_info(sd, "  %s\n", in_bias_sel_dis_cont_txt[(val >> 0) & 0x7]);
+}
+
+static int ths7303_log_status(struct v4l2_subdev *sd)
+{
+	struct ths7303_state *state = to_state(sd);
+
+	v4l2_info(sd, "stream %s\n", state->stream_on ? "On" : "Off");
+
+	if (state->bt.pixelclock) {
+		struct v4l2_bt_timings *bt = bt = &state->bt;
+		u32 frame_width, frame_height;
+
+		frame_width = bt->width + bt->hfrontporch +
+			      bt->hsync + bt->hbackporch;
+		frame_height = bt->height + bt->vfrontporch +
+			       bt->vsync + bt->vbackporch;
+		v4l2_info(sd,
+			  "timings: %dx%d%s%d (%dx%d). Pix freq. = %d Hz. Polarities = 0x%x\n",
+			  bt->width, bt->height, bt->interlaced ? "i" : "p",
+			  (frame_height * frame_width) > 0 ?
+			  (int)bt->pixelclock /
+			  (frame_height * frame_width) : 0,
+			  frame_width, frame_height,
+			  (int)bt->pixelclock, bt->polarities);
+	} else {
+		v4l2_info(sd, "no timings set\n");
+	}
+
+	ths7303_log_channel_status(sd, THS7303_CHANNEL_1);
+	ths7303_log_channel_status(sd, THS7303_CHANNEL_2);
+	ths7303_log_channel_status(sd, THS7303_CHANNEL_3);
+
+	return 0;
+}
+
 static const struct v4l2_subdev_core_ops ths7303_core_ops = {
 	.g_chip_ident = ths7303_g_chip_ident,
+	.log_status = ths7303_log_status,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = ths7303_g_register,
+	.s_register = ths7303_s_register,
+#endif
 };
 
 static const struct v4l2_subdev_ops ths7303_ops = {
@@ -163,11 +353,38 @@ static const struct v4l2_subdev_ops ths7303_ops = {
 	.video 	= &ths7303_video_ops,
 };
 
+static int ths7303_setup(struct v4l2_subdev *sd)
+{
+	struct ths7303_state *state = to_state(sd);
+	struct ths7303_platform_data *pdata = &state->pdata;
+	int ret;
+	u8 mask;
+
+	state->stream_on = pdata->init_enable;
+
+	mask = state->stream_on ? 0xff : 0xf8;
+
+	ret = ths7303_write(sd, THS7303_CHANNEL_1, pdata->ch_1 & mask);
+	if (ret)
+		return ret;
+
+	ret = ths7303_write(sd, THS7303_CHANNEL_2, pdata->ch_2 & mask);
+	if (ret)
+		return ret;
+
+	ret = ths7303_write(sd, THS7303_CHANNEL_3, pdata->ch_3 & mask);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
 static int ths7303_probe(struct i2c_client *client,
 			const struct i2c_device_id *id)
 {
+	struct ths7303_platform_data *pdata = client->dev.platform_data;
+	struct ths7303_state *state;
 	struct v4l2_subdev *sd;
-	v4l2_std_id std_id = V4L2_STD_NTSC;
 
 	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
 		return -ENODEV;
@@ -175,13 +392,28 @@ static int ths7303_probe(struct i2c_client *client,
 	v4l_info(client, "chip found @ 0x%x (%s)\n",
 			client->addr << 1, client->adapter->name);
 
-	sd = devm_kzalloc(&client->dev, sizeof(struct v4l2_subdev), GFP_KERNEL);
-	if (sd == NULL)
+	state = devm_kzalloc(&client->dev, sizeof(struct ths7303_state),
+			     GFP_KERNEL);
+	if (!state)
 		return -ENOMEM;
 
+	if (!pdata)
+		v4l_warn(client, "No platform data, using default data!\n");
+	else
+		state->pdata = *pdata;
+
+	sd = &state->sd;
 	v4l2_i2c_subdev_init(sd, client, &ths7303_ops);
 
-	return ths7303_s_std_output(sd, std_id);
+	/* store the driver data to differntiate the chip */
+	state->driver_data = (int)id->driver_data;
+
+	if (ths7303_setup(sd) < 0) {
+		v4l_err(client, "init failed\n");
+		return -EIO;
+	}
+
+	return 0;
 }
 
 static int ths7303_remove(struct i2c_client *client)
@@ -194,7 +426,8 @@ static int ths7303_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id ths7303_id[] = {
-	{"ths7303", 0},
+	{"ths7303", V4L2_IDENT_THS7303},
+	{"ths7353", V4L2_IDENT_THS7353},
 	{},
 };
 
@@ -203,7 +436,7 @@ MODULE_DEVICE_TABLE(i2c, ths7303_id);
 static struct i2c_driver ths7303_driver = {
 	.driver = {
 		.owner	= THIS_MODULE,
-		.name	= "ths7303",
+		.name	= "ths73x3",
 	},
 	.probe		= ths7303_probe,
 	.remove		= ths7303_remove,
diff --git a/drivers/media/i2c/tvaudio.c b/drivers/media/i2c/tvaudio.c
index e3b33b78dd2..b72a59d3216 100644
--- a/drivers/media/i2c/tvaudio.c
+++ b/drivers/media/i2c/tvaudio.c
@@ -1761,7 +1761,7 @@ static int tvaudio_s_routing(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int tvaudio_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+static int tvaudio_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
 {
 	struct CHIPSTATE *chip = to_state(sd);
 	struct CHIPDESC *desc = chip->desc;
@@ -1803,7 +1803,7 @@ static int tvaudio_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
 
 	vt->audmode = chip->audmode;
 	vt->rxsubchans = desc->getrxsubchans(chip);
-	vt->capability = V4L2_TUNER_CAP_STEREO |
+	vt->capability |= V4L2_TUNER_CAP_STEREO |
 		V4L2_TUNER_CAP_LANG1 | V4L2_TUNER_CAP_LANG2;
 
 	return 0;
@@ -1817,7 +1817,7 @@ static int tvaudio_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
 	return 0;
 }
 
-static int tvaudio_s_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *freq)
+static int tvaudio_s_frequency(struct v4l2_subdev *sd, const struct v4l2_frequency *freq)
 {
 	struct CHIPSTATE *chip = to_state(sd);
 	struct CHIPDESC *desc = chip->desc;
diff --git a/drivers/media/i2c/tvp514x.c b/drivers/media/i2c/tvp514x.c
index aa94ebc2d75..ab8f3fee7e9 100644
--- a/drivers/media/i2c/tvp514x.c
+++ b/drivers/media/i2c/tvp514x.c
@@ -12,6 +12,7 @@
  *     Hardik Shah <hardik.shah@ti.com>
  *     Manjunath Hadli <mrh@ti.com>
  *     Karicheri Muralidharan <m-karicheri2@ti.com>
+ *     Prabhakar Lad <prabhakar.lad@ti.com>
  *
  * This package is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -33,6 +34,7 @@
 #include <linux/delay.h>
 #include <linux/videodev2.h>
 #include <linux/module.h>
+#include <linux/v4l2-mediabus.h>
 
 #include <media/v4l2-device.h>
 #include <media/v4l2-common.h>
@@ -40,12 +42,10 @@
 #include <media/v4l2-chip-ident.h>
 #include <media/v4l2-ctrls.h>
 #include <media/tvp514x.h>
+#include <media/media-entity.h>
 
 #include "tvp514x_regs.h"
 
-/* Module Name */
-#define TVP514X_MODULE_NAME		"tvp514x"
-
 /* Private macros for TVP */
 #define I2C_RETRY_COUNT                 (5)
 #define LOCK_RETRY_COUNT                (5)
@@ -91,6 +91,9 @@ static int tvp514x_s_stream(struct v4l2_subdev *sd, int enable);
  * @pdata: Board specific
  * @ver: Chip version
  * @streaming: TVP5146/47 decoder streaming - enabled or disabled.
+ * @pix: Current pixel format
+ * @num_fmts: Number of formats
+ * @fmt_list: Format list
  * @current_std: Current standard
  * @num_stds: Number of standards
  * @std_list: Standards list
@@ -106,12 +109,20 @@ struct tvp514x_decoder {
 	int ver;
 	int streaming;
 
+	struct v4l2_pix_format pix;
+	int num_fmts;
+	const struct v4l2_fmtdesc *fmt_list;
+
 	enum tvp514x_std current_std;
 	int num_stds;
 	const struct tvp514x_std_info *std_list;
 	/* Input and Output Routing parameters */
 	u32 input;
 	u32 output;
+
+	/* mc related members */
+	struct media_pad pad;
+	struct v4l2_mbus_framefmt format;
 };
 
 /* TVP514x default register values */
@@ -200,6 +211,21 @@ static struct tvp514x_reg tvp514x_reg_list_default[] = {
 };
 
 /**
+ * List of image formats supported by TVP5146/47 decoder
+ * Currently we are using 8 bit mode only, but can be
+ * extended to 10/20 bit mode.
+ */
+static const struct v4l2_fmtdesc tvp514x_fmt_list[] = {
+	{
+	 .index		= 0,
+	 .type		= V4L2_BUF_TYPE_VIDEO_CAPTURE,
+	 .flags		= 0,
+	 .description	= "8-bit UYVY 4:2:2 Format",
+	 .pixelformat	= V4L2_PIX_FMT_UYVY,
+	},
+};
+
+/**
  * Supported standards -
  *
  * Currently supports two standards only, need to add support for rest of the
@@ -733,7 +759,7 @@ tvp514x_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned index,
 }
 
 /**
- * tvp514x_mbus_fmt_cap() - V4L2 decoder interface handler for try/s/g_mbus_fmt
+ * tvp514x_mbus_fmt() - V4L2 decoder interface handler for try/s/g_mbus_fmt
  * @sd: pointer to standard V4L2 sub-device structure
  * @f: pointer to the mediabus format structure
  *
@@ -751,12 +777,11 @@ tvp514x_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *f)
 	/* Calculate height and width based on current standard */
 	current_std = decoder->current_std;
 
-	f->code = V4L2_MBUS_FMT_YUYV10_2X10;
+	f->code = V4L2_MBUS_FMT_YUYV8_2X8;
 	f->width = decoder->std_list[current_std].width;
 	f->height = decoder->std_list[current_std].height;
 	f->field = V4L2_FIELD_INTERLACED;
 	f->colorspace = V4L2_COLORSPACE_SMPTE170M;
-
 	v4l2_dbg(1, debug, sd, "MBUS_FMT: Width - %d, Height - %d\n",
 			f->width, f->height);
 	return 0;
@@ -892,6 +917,88 @@ static const struct v4l2_ctrl_ops tvp514x_ctrl_ops = {
 	.s_ctrl = tvp514x_s_ctrl,
 };
 
+/**
+ * tvp514x_enum_mbus_code() - V4L2 decoder interface handler for enum_mbus_code
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @fh: file handle
+ * @code: pointer to v4l2_subdev_mbus_code_enum structure
+ *
+ * Enumertaes mbus codes supported
+ */
+static int tvp514x_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_fh *fh,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	u32 pad = code->pad;
+	u32 index = code->index;
+
+	memset(code, 0, sizeof(*code));
+	code->index = index;
+	code->pad = pad;
+
+	if (index != 0)
+		return -EINVAL;
+
+	code->code = V4L2_MBUS_FMT_YUYV8_2X8;
+
+	return 0;
+}
+
+/**
+ * tvp514x_get_pad_format() - V4L2 decoder interface handler for get pad format
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @fh: file handle
+ * @format: pointer to v4l2_subdev_format structure
+ *
+ * Retrieves pad format which is active or tried based on requirement
+ */
+static int tvp514x_get_pad_format(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_fh *fh,
+				  struct v4l2_subdev_format *format)
+{
+	struct tvp514x_decoder *decoder = to_decoder(sd);
+	__u32 which = format->which;
+
+	if (which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		format->format = decoder->format;
+		return 0;
+	}
+
+	format->format.code = V4L2_MBUS_FMT_YUYV8_2X8;
+	format->format.width = tvp514x_std_list[decoder->current_std].width;
+	format->format.height = tvp514x_std_list[decoder->current_std].height;
+	format->format.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	format->format.field = V4L2_FIELD_INTERLACED;
+
+	return 0;
+}
+
+/**
+ * tvp514x_set_pad_format() - V4L2 decoder interface handler for set pad format
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @fh: file handle
+ * @format: pointer to v4l2_subdev_format structure
+ *
+ * Set pad format for the output pad
+ */
+static int tvp514x_set_pad_format(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_fh *fh,
+				  struct v4l2_subdev_format *fmt)
+{
+	struct tvp514x_decoder *decoder = to_decoder(sd);
+
+	if (fmt->format.field != V4L2_FIELD_INTERLACED ||
+	    fmt->format.code != V4L2_MBUS_FMT_YUYV8_2X8 ||
+	    fmt->format.colorspace != V4L2_COLORSPACE_SMPTE170M ||
+	    fmt->format.width != tvp514x_std_list[decoder->current_std].width ||
+	    fmt->format.height != tvp514x_std_list[decoder->current_std].height)
+		return -EINVAL;
+
+	decoder->format = fmt->format;
+
+	return 0;
+}
+
 static const struct v4l2_subdev_core_ops tvp514x_core_ops = {
 	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
 	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
@@ -915,13 +1022,33 @@ static const struct v4l2_subdev_video_ops tvp514x_video_ops = {
 	.s_stream = tvp514x_s_stream,
 };
 
+static const struct v4l2_subdev_pad_ops tvp514x_pad_ops = {
+	.enum_mbus_code = tvp514x_enum_mbus_code,
+	.get_fmt = tvp514x_get_pad_format,
+	.set_fmt = tvp514x_set_pad_format,
+};
+
 static const struct v4l2_subdev_ops tvp514x_ops = {
 	.core = &tvp514x_core_ops,
 	.video = &tvp514x_video_ops,
+	.pad = &tvp514x_pad_ops,
 };
 
 static struct tvp514x_decoder tvp514x_dev = {
 	.streaming = 0,
+	.fmt_list = tvp514x_fmt_list,
+	.num_fmts = ARRAY_SIZE(tvp514x_fmt_list),
+	.pix = {
+		/* Default to NTSC 8-bit YUV 422 */
+		.width		= NTSC_NUM_ACTIVE_PIXELS,
+		.height		= NTSC_NUM_ACTIVE_LINES,
+		.pixelformat	= V4L2_PIX_FMT_UYVY,
+		.field		= V4L2_FIELD_INTERLACED,
+		.bytesperline	= NTSC_NUM_ACTIVE_PIXELS * 2,
+		.sizeimage	= NTSC_NUM_ACTIVE_PIXELS * 2 *
+					NTSC_NUM_ACTIVE_LINES,
+		.colorspace	= V4L2_COLORSPACE_SMPTE170M,
+		},
 	.current_std = STD_NTSC_MJ,
 	.std_list = tvp514x_std_list,
 	.num_stds = ARRAY_SIZE(tvp514x_std_list),
@@ -941,6 +1068,7 @@ tvp514x_probe(struct i2c_client *client, const struct i2c_device_id *id)
 {
 	struct tvp514x_decoder *decoder;
 	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))
@@ -981,7 +1109,21 @@ tvp514x_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	/* Register with V4L2 layer as slave device */
 	sd = &decoder->sd;
 	v4l2_i2c_subdev_init(sd, client, &tvp514x_ops);
-
+	strlcpy(sd->name, TVP514X_MODULE_NAME, sizeof(sd->name));
+
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	decoder->pad.flags = MEDIA_PAD_FL_SOURCE;
+	decoder->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	decoder->sd.entity.flags |= MEDIA_ENT_T_V4L2_SUBDEV_DECODER;
+
+	ret = media_entity_init(&decoder->sd.entity, 1, &decoder->pad, 0);
+	if (ret < 0) {
+		v4l2_err(sd, "%s decoder driver failed to register !!\n",
+			 sd->name);
+		kfree(decoder);
+		return ret;
+	}
+#endif
 	v4l2_ctrl_handler_init(&decoder->hdl, 5);
 	v4l2_ctrl_new_std(&decoder->hdl, &tvp514x_ctrl_ops,
 		V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
@@ -995,10 +1137,10 @@ tvp514x_probe(struct i2c_client *client, const struct i2c_device_id *id)
 		V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
 	sd->ctrl_handler = &decoder->hdl;
 	if (decoder->hdl.error) {
-		int err = decoder->hdl.error;
+		ret = decoder->hdl.error;
 
 		v4l2_ctrl_handler_free(&decoder->hdl);
-		return err;
+		return ret;
 	}
 	v4l2_ctrl_handler_setup(&decoder->hdl);
 
@@ -1021,6 +1163,9 @@ static int tvp514x_remove(struct i2c_client *client)
 	struct tvp514x_decoder *decoder = to_decoder(sd);
 
 	v4l2_device_unregister_subdev(sd);
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	media_entity_cleanup(&decoder->sd.entity);
+#endif
 	v4l2_ctrl_handler_free(&decoder->hdl);
 	return 0;
 }
diff --git a/drivers/media/i2c/tvp5150.c b/drivers/media/i2c/tvp5150.c
index 5967e1a0c80..485159a3c0b 100644
--- a/drivers/media/i2c/tvp5150.c
+++ b/drivers/media/i2c/tvp5150.c
@@ -1067,7 +1067,7 @@ static int tvp5150_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *
 	return 0;
 }
 
-static int tvp5150_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int tvp5150_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/tvp7002.c b/drivers/media/i2c/tvp7002.c
index 537f6b4d491..027809cca5f 100644
--- a/drivers/media/i2c/tvp7002.c
+++ b/drivers/media/i2c/tvp7002.c
@@ -326,9 +326,8 @@ static const struct i2c_reg_value tvp7002_parms_720P50[] = {
 	{ TVP7002_EOR, 0xff, TVP7002_RESERVED }
 };
 
-/* Preset definition for handling device operation */
-struct tvp7002_preset_definition {
-	u32 preset;
+/* Timings definition for handling device operation */
+struct tvp7002_timings_definition {
 	struct v4l2_dv_timings timings;
 	const struct i2c_reg_value *p_settings;
 	enum v4l2_colorspace color_space;
@@ -339,10 +338,9 @@ struct tvp7002_preset_definition {
 	u16 cpl_max;
 };
 
-/* Struct list for digital video presets */
-static const struct tvp7002_preset_definition tvp7002_presets[] = {
+/* Struct list for digital video timings */
+static const struct tvp7002_timings_definition tvp7002_timings[] = {
 	{
-		V4L2_DV_720P60,
 		V4L2_DV_BT_CEA_1280X720P60,
 		tvp7002_parms_720P60,
 		V4L2_COLORSPACE_REC709,
@@ -353,7 +351,6 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 		153
 	},
 	{
-		V4L2_DV_1080I60,
 		V4L2_DV_BT_CEA_1920X1080I60,
 		tvp7002_parms_1080I60,
 		V4L2_COLORSPACE_REC709,
@@ -364,7 +361,6 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 		205
 	},
 	{
-		V4L2_DV_1080I50,
 		V4L2_DV_BT_CEA_1920X1080I50,
 		tvp7002_parms_1080I50,
 		V4L2_COLORSPACE_REC709,
@@ -375,7 +371,6 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 		245
 	},
 	{
-		V4L2_DV_720P50,
 		V4L2_DV_BT_CEA_1280X720P50,
 		tvp7002_parms_720P50,
 		V4L2_COLORSPACE_REC709,
@@ -386,7 +381,6 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 		183
 	},
 	{
-		V4L2_DV_1080P60,
 		V4L2_DV_BT_CEA_1920X1080P60,
 		tvp7002_parms_1080P60,
 		V4L2_COLORSPACE_REC709,
@@ -397,7 +391,6 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 		102
 	},
 	{
-		V4L2_DV_480P59_94,
 		V4L2_DV_BT_CEA_720X480P59_94,
 		tvp7002_parms_480P,
 		V4L2_COLORSPACE_SMPTE170M,
@@ -408,7 +401,6 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 		0xffff
 	},
 	{
-		V4L2_DV_576P50,
 		V4L2_DV_BT_CEA_720X576P50,
 		tvp7002_parms_576P,
 		V4L2_COLORSPACE_SMPTE170M,
@@ -420,7 +412,7 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 	}
 };
 
-#define NUM_PRESETS	ARRAY_SIZE(tvp7002_presets)
+#define NUM_TIMINGS ARRAY_SIZE(tvp7002_timings)
 
 /* Device definition */
 struct tvp7002 {
@@ -431,7 +423,7 @@ struct tvp7002 {
 	int ver;
 	int streaming;
 
-	const struct tvp7002_preset_definition *current_preset;
+	const struct tvp7002_timings_definition *current_timings;
 };
 
 /*
@@ -588,32 +580,6 @@ static int tvp7002_write_inittab(struct v4l2_subdev *sd,
 	return error;
 }
 
-/*
- * tvp7002_s_dv_preset() - Set digital video preset
- * @sd: ptr to v4l2_subdev struct
- * @dv_preset: ptr to v4l2_dv_preset struct
- *
- * Set the digital video preset for a TVP7002 decoder device.
- * Returns zero when successful or -EINVAL if register access fails.
- */
-static int tvp7002_s_dv_preset(struct v4l2_subdev *sd,
-					struct v4l2_dv_preset *dv_preset)
-{
-	struct tvp7002 *device = to_tvp7002(sd);
-	u32 preset;
-	int i;
-
-	for (i = 0; i < NUM_PRESETS; i++) {
-		preset = tvp7002_presets[i].preset;
-		if (preset == dv_preset->preset) {
-			device->current_preset = &tvp7002_presets[i];
-			return tvp7002_write_inittab(sd, tvp7002_presets[i].p_settings);
-		}
-	}
-
-	return -EINVAL;
-}
-
 static int tvp7002_s_dv_timings(struct v4l2_subdev *sd,
 					struct v4l2_dv_timings *dv_timings)
 {
@@ -623,12 +589,12 @@ static int tvp7002_s_dv_timings(struct v4l2_subdev *sd,
 
 	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;
+	for (i = 0; i < NUM_TIMINGS; i++) {
+		const struct v4l2_bt_timings *t = &tvp7002_timings[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);
+			device->current_timings = &tvp7002_timings[i];
+			return tvp7002_write_inittab(sd, tvp7002_timings[i].p_settings);
 		}
 	}
 	return -EINVAL;
@@ -639,7 +605,7 @@ static int tvp7002_g_dv_timings(struct v4l2_subdev *sd,
 {
 	struct tvp7002 *device = to_tvp7002(sd);
 
-	*dv_timings = device->current_preset->timings;
+	*dv_timings = device->current_timings->timings;
 	return 0;
 }
 
@@ -677,19 +643,13 @@ static int tvp7002_s_ctrl(struct v4l2_ctrl *ctrl)
 static int tvp7002_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *f)
 {
 	struct tvp7002 *device = to_tvp7002(sd);
-	struct v4l2_dv_enum_preset e_preset;
-	int error;
+	const struct v4l2_bt_timings *bt = &device->current_timings->timings.bt;
 
-	/* Calculate height and width based on current standard */
-	error = v4l_fill_dv_preset_info(device->current_preset->preset, &e_preset);
-	if (error)
-		return error;
-
-	f->width = e_preset.width;
-	f->height = e_preset.height;
+	f->width = bt->width;
+	f->height = bt->height;
 	f->code = V4L2_MBUS_FMT_YUYV10_1X20;
-	f->field = device->current_preset->scanmode;
-	f->colorspace = device->current_preset->color_space;
+	f->field = device->current_timings->scanmode;
+	f->colorspace = device->current_timings->color_space;
 
 	v4l2_dbg(1, debug, sd, "MBUS_FMT: Width - %d, Height - %d",
 			f->width, f->height);
@@ -697,16 +657,16 @@ static int tvp7002_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *f
 }
 
 /*
- * tvp7002_query_dv_preset() - query DV preset
+ * tvp7002_query_dv() - query DV timings
  * @sd: pointer to standard V4L2 sub-device structure
- * @qpreset: standard V4L2 v4l2_dv_preset structure
+ * @index: index into the tvp7002_timings array
  *
- * Returns the current DV preset by TVP7002. If no active input is
+ * Returns the current DV timings detected by TVP7002. If no active input is
  * detected, returns -EINVAL
  */
 static int tvp7002_query_dv(struct v4l2_subdev *sd, int *index)
 {
-	const struct tvp7002_preset_definition *presets = tvp7002_presets;
+	const struct tvp7002_timings_definition *timings = tvp7002_timings;
 	u8 progressive;
 	u32 lpfr;
 	u32 cpln;
@@ -717,7 +677,7 @@ static int tvp7002_query_dv(struct v4l2_subdev *sd, int *index)
 	u8 cpl_msb;
 
 	/* Return invalid index if no active input is detected */
-	*index = NUM_PRESETS;
+	*index = NUM_TIMINGS;
 
 	/* Read standards from device registers */
 	tvp7002_read_err(sd, TVP7002_L_FRAME_STAT_LSBS, &lpf_lsb, &error);
@@ -738,39 +698,23 @@ static int tvp7002_query_dv(struct v4l2_subdev *sd, int *index)
 	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 &&
-			progressive == presets->progressive) {
-			if (presets->cpl_min == 0xffff)
+	for (*index = 0; *index < NUM_TIMINGS; (*index)++, timings++)
+		if (lpfr == timings->lines_per_frame &&
+			progressive == timings->progressive) {
+			if (timings->cpl_min == 0xffff)
 				break;
-			if (cpln >= presets->cpl_min && cpln <= presets->cpl_max)
+			if (cpln >= timings->cpl_min && cpln <= timings->cpl_max)
 				break;
 		}
 
-	if (*index == NUM_PRESETS) {
+	if (*index == NUM_TIMINGS) {
 		v4l2_dbg(1, debug, sd, "detection failed: lpf = %x, cpl = %x\n",
 								lpfr, cpln);
 		return -ENOLINK;
 	}
 
 	/* Update lines per frame and clocks per line info */
-	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;
+	v4l2_dbg(1, debug, sd, "detected timings: %d\n", *index);
 	return 0;
 }
 
@@ -782,7 +726,7 @@ static int tvp7002_query_dv_timings(struct v4l2_subdev *sd,
 
 	if (err)
 		return err;
-	*timings = tvp7002_presets[index].timings;
+	*timings = tvp7002_timings[index].timings;
 	return 0;
 }
 
@@ -824,7 +768,7 @@ static int tvp7002_g_register(struct v4l2_subdev *sd,
  * -EPERM if call not allowed.
  */
 static int tvp7002_s_register(struct v4l2_subdev *sd,
-						struct v4l2_dbg_register *reg)
+						const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
@@ -896,35 +840,21 @@ static int tvp7002_s_stream(struct v4l2_subdev *sd, int enable)
  */
 static int tvp7002_log_status(struct v4l2_subdev *sd)
 {
-	const struct tvp7002_preset_definition *presets = tvp7002_presets;
 	struct tvp7002 *device = to_tvp7002(sd);
-	struct v4l2_dv_enum_preset e_preset;
-	struct v4l2_dv_preset detected;
-	int i;
-
-	detected.preset = V4L2_DV_INVALID;
-	/* Find my current standard*/
-	tvp7002_query_dv_preset(sd, &detected);
+	const struct v4l2_bt_timings *bt;
+	int detected;
 
-	/* Print standard related code values */
-	for (i = 0; i < NUM_PRESETS; i++, presets++)
-		if (presets->preset == detected.preset)
-			break;
+	/* Find my current timings */
+	tvp7002_query_dv(sd, &detected);
 
-	if (v4l_fill_dv_preset_info(device->current_preset->preset, &e_preset))
-		return -EINVAL;
-
-	v4l2_info(sd, "Selected DV Preset: %s\n", e_preset.name);
-	v4l2_info(sd, "   Pixels per line: %u\n", e_preset.width);
-	v4l2_info(sd, "   Lines per frame: %u\n\n", e_preset.height);
-	if (i == NUM_PRESETS) {
-		v4l2_info(sd, "Detected DV Preset: None\n");
+	bt = &device->current_timings->timings.bt;
+	v4l2_info(sd, "Selected DV Timings: %ux%u\n", bt->width, bt->height);
+	if (detected == NUM_TIMINGS) {
+		v4l2_info(sd, "Detected DV Timings: None\n");
 	} else {
-		if (v4l_fill_dv_preset_info(presets->preset, &e_preset))
-			return -EINVAL;
-		v4l2_info(sd, "Detected DV Preset: %s\n", e_preset.name);
-		v4l2_info(sd, "  Pixels per line: %u\n", e_preset.width);
-		v4l2_info(sd, "  Lines per frame: %u\n\n", e_preset.height);
+		bt = &tvp7002_timings[detected].timings.bt;
+		v4l2_info(sd, "Detected DV Timings: %ux%u\n",
+				bt->width, bt->height);
 	}
 	v4l2_info(sd, "Streaming enabled: %s\n",
 					device->streaming ? "yes" : "no");
@@ -935,31 +865,14 @@ static int tvp7002_log_status(struct v4l2_subdev *sd)
 	return 0;
 }
 
-/*
- * tvp7002_enum_dv_presets() - Enum supported digital video formats
- * @sd: pointer to standard V4L2 sub-device structure
- * @preset: pointer to format struct
- *
- * Enumerate supported digital video formats.
- */
-static int tvp7002_enum_dv_presets(struct v4l2_subdev *sd,
-		struct v4l2_dv_enum_preset *preset)
-{
-	/* Check requested format index is within range */
-	if (preset->index >= NUM_PRESETS)
-		return -EINVAL;
-
-	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)
+	if (timings->index >= NUM_TIMINGS)
 		return -EINVAL;
 
-	timings->timings = tvp7002_presets[timings->index].timings;
+	timings->timings = tvp7002_timings[timings->index].timings;
 	return 0;
 }
 
@@ -986,9 +899,6 @@ static const struct v4l2_subdev_core_ops tvp7002_core_ops = {
 
 /* Specific video subsystem operation handlers */
 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,
@@ -1019,7 +929,7 @@ static int tvp7002_probe(struct i2c_client *c, const struct i2c_device_id *id)
 {
 	struct v4l2_subdev *sd;
 	struct tvp7002 *device;
-	struct v4l2_dv_preset preset;
+	struct v4l2_dv_timings timings;
 	int polarity_a;
 	int polarity_b;
 	u8 revision;
@@ -1043,7 +953,7 @@ static int tvp7002_probe(struct i2c_client *c, const struct i2c_device_id *id)
 
 	sd = &device->sd;
 	device->pdata = c->dev.platform_data;
-	device->current_preset = tvp7002_presets;
+	device->current_timings = tvp7002_timings;
 
 	/* Tell v4l2 the device is ready */
 	v4l2_i2c_subdev_init(sd, c, &tvp7002_ops);
@@ -1080,8 +990,8 @@ static int tvp7002_probe(struct i2c_client *c, const struct i2c_device_id *id)
 		return error;
 
 	/* Set registers according to default video mode */
-	preset.preset = device->current_preset->preset;
-	error = tvp7002_s_dv_preset(sd, &preset);
+	timings = device->current_timings->timings;
+	error = tvp7002_s_dv_timings(sd, &timings);
 
 	v4l2_ctrl_handler_init(&device->hdl, 1);
 	v4l2_ctrl_new_std(&device->hdl, &tvp7002_ctrl_ops,
diff --git a/drivers/media/i2c/tw2804.c b/drivers/media/i2c/tw2804.c
new file mode 100644
index 00000000000..c5dc2c3bf2d
--- /dev/null
+++ b/drivers/media/i2c/tw2804.c
@@ -0,0 +1,453 @@
+/*
+ * Copyright (C) 2005-2006 Micronas USA 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.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <linux/ioctl.h>
+#include <linux/slab.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-ctrls.h>
+
+#define TW2804_REG_AUTOGAIN		0x02
+#define TW2804_REG_HUE			0x0f
+#define TW2804_REG_SATURATION		0x10
+#define TW2804_REG_CONTRAST		0x11
+#define TW2804_REG_BRIGHTNESS		0x12
+#define TW2804_REG_COLOR_KILLER		0x14
+#define TW2804_REG_GAIN			0x3c
+#define TW2804_REG_CHROMA_GAIN		0x3d
+#define TW2804_REG_BLUE_BALANCE		0x3e
+#define TW2804_REG_RED_BALANCE		0x3f
+
+struct tw2804 {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	u8 channel:2;
+	u8 input:1;
+	int norm;
+};
+
+static const u8 global_registers[] = {
+	0x39, 0x00,
+	0x3a, 0xff,
+	0x3b, 0x84,
+	0x3c, 0x80,
+	0x3d, 0x80,
+	0x3e, 0x82,
+	0x3f, 0x82,
+	0x78, 0x00,
+	0xff, 0xff, /* Terminator (reg 0xff does not exist) */
+};
+
+static const u8 channel_registers[] = {
+	0x01, 0xc4,
+	0x02, 0xa5,
+	0x03, 0x20,
+	0x04, 0xd0,
+	0x05, 0x20,
+	0x06, 0xd0,
+	0x07, 0x88,
+	0x08, 0x20,
+	0x09, 0x07,
+	0x0a, 0xf0,
+	0x0b, 0x07,
+	0x0c, 0xf0,
+	0x0d, 0x40,
+	0x0e, 0xd2,
+	0x0f, 0x80,
+	0x10, 0x80,
+	0x11, 0x80,
+	0x12, 0x80,
+	0x13, 0x1f,
+	0x14, 0x00,
+	0x15, 0x00,
+	0x16, 0x00,
+	0x17, 0x00,
+	0x18, 0xff,
+	0x19, 0xff,
+	0x1a, 0xff,
+	0x1b, 0xff,
+	0x1c, 0xff,
+	0x1d, 0xff,
+	0x1e, 0xff,
+	0x1f, 0xff,
+	0x20, 0x07,
+	0x21, 0x07,
+	0x22, 0x00,
+	0x23, 0x91,
+	0x24, 0x51,
+	0x25, 0x03,
+	0x26, 0x00,
+	0x27, 0x00,
+	0x28, 0x00,
+	0x29, 0x00,
+	0x2a, 0x00,
+	0x2b, 0x00,
+	0x2c, 0x00,
+	0x2d, 0x00,
+	0x2e, 0x00,
+	0x2f, 0x00,
+	0x30, 0x00,
+	0x31, 0x00,
+	0x32, 0x00,
+	0x33, 0x00,
+	0x34, 0x00,
+	0x35, 0x00,
+	0x36, 0x00,
+	0x37, 0x00,
+	0xff, 0xff, /* Terminator (reg 0xff does not exist) */
+};
+
+static int write_reg(struct i2c_client *client, u8 reg, u8 value, u8 channel)
+{
+	return i2c_smbus_write_byte_data(client, reg | (channel << 6), value);
+}
+
+static int write_regs(struct i2c_client *client, const u8 *regs, u8 channel)
+{
+	int ret;
+	int i;
+
+	for (i = 0; regs[i] != 0xff; i += 2) {
+		ret = i2c_smbus_write_byte_data(client,
+				regs[i] | (channel << 6), regs[i + 1]);
+		if (ret < 0)
+			return ret;
+	}
+	return 0;
+}
+
+static int read_reg(struct i2c_client *client, u8 reg, u8 channel)
+{
+	return i2c_smbus_read_byte_data(client, (reg) | (channel << 6));
+}
+
+static inline struct tw2804 *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct tw2804, sd);
+}
+
+static inline struct tw2804 *to_state_from_ctrl(struct v4l2_ctrl *ctrl)
+{
+	return container_of(ctrl->handler, struct tw2804, hdl);
+}
+
+static int tw2804_log_status(struct v4l2_subdev *sd)
+{
+	struct tw2804 *state = to_state(sd);
+
+	v4l2_info(sd, "Standard: %s\n",
+			state->norm & V4L2_STD_525_60 ? "60 Hz" : "50 Hz");
+	v4l2_info(sd, "Channel: %d\n", state->channel);
+	v4l2_info(sd, "Input: %d\n", state->input);
+	return v4l2_ctrl_subdev_log_status(sd);
+}
+
+/*
+ * These volatile controls are needed because all four channels share
+ * these controls. So a change made to them through one channel would
+ * require another channel to be updated.
+ *
+ * Normally this would have been done in a different way, but since the one
+ * board that uses this driver sees this single chip as if it was on four
+ * different i2c adapters (each adapter belonging to a separate instance of
+ * the same USB driver) there is no reliable method that I have found to let
+ * the instances know about each other.
+ *
+ * So implementing these global registers as volatile is the best we can do.
+ */
+static int tw2804_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct tw2804 *state = to_state_from_ctrl(ctrl);
+	struct i2c_client *client = v4l2_get_subdevdata(&state->sd);
+
+	switch (ctrl->id) {
+	case V4L2_CID_GAIN:
+		ctrl->val = read_reg(client, TW2804_REG_GAIN, 0);
+		return 0;
+
+	case V4L2_CID_CHROMA_GAIN:
+		ctrl->val = read_reg(client, TW2804_REG_CHROMA_GAIN, 0);
+		return 0;
+
+	case V4L2_CID_BLUE_BALANCE:
+		ctrl->val = read_reg(client, TW2804_REG_BLUE_BALANCE, 0);
+		return 0;
+
+	case V4L2_CID_RED_BALANCE:
+		ctrl->val = read_reg(client, TW2804_REG_RED_BALANCE, 0);
+		return 0;
+	}
+	return 0;
+}
+
+static int tw2804_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct tw2804 *state = to_state_from_ctrl(ctrl);
+	struct i2c_client *client = v4l2_get_subdevdata(&state->sd);
+	int addr;
+	int reg;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		addr = TW2804_REG_AUTOGAIN;
+		reg = read_reg(client, addr, state->channel);
+		if (reg < 0)
+			return reg;
+		if (ctrl->val == 0)
+			reg &= ~(1 << 7);
+		else
+			reg |= 1 << 7;
+		return write_reg(client, addr, reg, state->channel);
+
+	case V4L2_CID_COLOR_KILLER:
+		addr = TW2804_REG_COLOR_KILLER;
+		reg = read_reg(client, addr, state->channel);
+		if (reg < 0)
+			return reg;
+		reg = (reg & ~(0x03)) | (ctrl->val == 0 ? 0x02 : 0x03);
+		return write_reg(client, addr, reg, state->channel);
+
+	case V4L2_CID_GAIN:
+		return write_reg(client, TW2804_REG_GAIN, ctrl->val, 0);
+
+	case V4L2_CID_CHROMA_GAIN:
+		return write_reg(client, TW2804_REG_CHROMA_GAIN, ctrl->val, 0);
+
+	case V4L2_CID_BLUE_BALANCE:
+		return write_reg(client, TW2804_REG_BLUE_BALANCE, ctrl->val, 0);
+
+	case V4L2_CID_RED_BALANCE:
+		return write_reg(client, TW2804_REG_RED_BALANCE, ctrl->val, 0);
+
+	case V4L2_CID_BRIGHTNESS:
+		return write_reg(client, TW2804_REG_BRIGHTNESS,
+				ctrl->val, state->channel);
+
+	case V4L2_CID_CONTRAST:
+		return write_reg(client, TW2804_REG_CONTRAST,
+				ctrl->val, state->channel);
+
+	case V4L2_CID_SATURATION:
+		return write_reg(client, TW2804_REG_SATURATION,
+				ctrl->val, state->channel);
+
+	case V4L2_CID_HUE:
+		return write_reg(client, TW2804_REG_HUE,
+				ctrl->val, state->channel);
+
+	default:
+		break;
+	}
+	return -EINVAL;
+}
+
+static int tw2804_s_std(struct v4l2_subdev *sd, v4l2_std_id norm)
+{
+	struct tw2804 *dec = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	bool is_60hz = norm & V4L2_STD_525_60;
+	u8 regs[] = {
+		0x01, is_60hz ? 0xc4 : 0x84,
+		0x09, is_60hz ? 0x07 : 0x04,
+		0x0a, is_60hz ? 0xf0 : 0x20,
+		0x0b, is_60hz ? 0x07 : 0x04,
+		0x0c, is_60hz ? 0xf0 : 0x20,
+		0x0d, is_60hz ? 0x40 : 0x4a,
+		0x16, is_60hz ? 0x00 : 0x40,
+		0x17, is_60hz ? 0x00 : 0x40,
+		0x20, is_60hz ? 0x07 : 0x0f,
+		0x21, is_60hz ? 0x07 : 0x0f,
+		0xff, 0xff,
+	};
+
+	write_regs(client, regs, dec->channel);
+	dec->norm = norm;
+	return 0;
+}
+
+static int tw2804_s_video_routing(struct v4l2_subdev *sd, u32 input, u32 output,
+	u32 config)
+{
+	struct tw2804 *dec = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int reg;
+
+	if (config && config - 1 != dec->channel) {
+		if (config > 4) {
+			dev_err(&client->dev,
+				"channel %d is not between 1 and 4!\n", config);
+			return -EINVAL;
+		}
+		dec->channel = config - 1;
+		dev_dbg(&client->dev, "initializing TW2804 channel %d\n",
+			dec->channel);
+		if (dec->channel == 0 &&
+				write_regs(client, global_registers, 0) < 0) {
+			dev_err(&client->dev,
+				"error initializing TW2804 global registers\n");
+			return -EIO;
+		}
+		if (write_regs(client, channel_registers, dec->channel) < 0) {
+			dev_err(&client->dev,
+				"error initializing TW2804 channel %d\n",
+				dec->channel);
+			return -EIO;
+		}
+	}
+
+	if (input > 1)
+		return -EINVAL;
+
+	if (input == dec->input)
+		return 0;
+
+	reg = read_reg(client, 0x22, dec->channel);
+
+	if (reg >= 0) {
+		if (input == 0)
+			reg &= ~(1 << 2);
+		else
+			reg |= 1 << 2;
+		reg = write_reg(client, 0x22, reg, dec->channel);
+	}
+
+	if (reg >= 0)
+		dec->input = input;
+	else
+		return reg;
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops tw2804_ctrl_ops = {
+	.g_volatile_ctrl = tw2804_g_volatile_ctrl,
+	.s_ctrl = tw2804_s_ctrl,
+};
+
+static const struct v4l2_subdev_video_ops tw2804_video_ops = {
+	.s_routing = tw2804_s_video_routing,
+};
+
+static const struct v4l2_subdev_core_ops tw2804_core_ops = {
+	.log_status = tw2804_log_status,
+	.s_std = tw2804_s_std,
+};
+
+static const struct v4l2_subdev_ops tw2804_ops = {
+	.core = &tw2804_core_ops,
+	.video = &tw2804_video_ops,
+};
+
+static int tw2804_probe(struct i2c_client *client,
+			    const struct i2c_device_id *id)
+{
+	struct i2c_adapter *adapter = client->adapter;
+	struct tw2804 *state;
+	struct v4l2_subdev *sd;
+	struct v4l2_ctrl *ctrl;
+	int err;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -ENODEV;
+
+	state = kzalloc(sizeof(struct tw2804), GFP_KERNEL);
+
+	if (state == NULL)
+		return -ENOMEM;
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &tw2804_ops);
+	state->channel = -1;
+	state->norm = V4L2_STD_NTSC;
+
+	v4l2_ctrl_handler_init(&state->hdl, 10);
+	v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_CONTRAST, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_SATURATION, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_HUE, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_COLOR_KILLER, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_AUTOGAIN, 0, 1, 1, 0);
+	ctrl = v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_GAIN, 0, 255, 1, 128);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;
+	ctrl = v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_CHROMA_GAIN, 0, 255, 1, 128);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;
+	ctrl = v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_BLUE_BALANCE, 0, 255, 1, 122);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;
+	ctrl = v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_RED_BALANCE, 0, 255, 1, 122);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;
+	sd->ctrl_handler = &state->hdl;
+	err = state->hdl.error;
+	if (err) {
+		v4l2_ctrl_handler_free(&state->hdl);
+		kfree(state);
+		return err;
+	}
+
+	v4l_info(client, "chip found @ 0x%02x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	return 0;
+}
+
+static int tw2804_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct tw2804 *state = to_state(sd);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&state->hdl);
+	kfree(state);
+	return 0;
+}
+
+static const struct i2c_device_id tw2804_id[] = {
+	{ "tw2804", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tw2804_id);
+
+static struct i2c_driver tw2804_driver = {
+	.driver = {
+		.name	= "tw2804",
+	},
+	.probe		= tw2804_probe,
+	.remove		= tw2804_remove,
+	.id_table	= tw2804_id,
+};
+
+module_i2c_driver(tw2804_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("TW2804/TW2802 V4L2 i2c driver");
+MODULE_AUTHOR("Micronas USA Inc");
diff --git a/drivers/media/i2c/tw9903.c b/drivers/media/i2c/tw9903.c
new file mode 100644
index 00000000000..87880b19d8c
--- /dev/null
+++ b/drivers/media/i2c/tw9903.c
@@ -0,0 +1,279 @@
+/*
+ * Copyright (C) 2005-2006 Micronas USA 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.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <linux/ioctl.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <linux/slab.h>
+
+MODULE_DESCRIPTION("TW9903 I2C subdev driver");
+MODULE_LICENSE("GPL v2");
+
+/*
+ * This driver is based on the wis-tw9903.c source that was in
+ * drivers/staging/media/go7007. That source had commented out code for
+ * saturation and scaling (neither seemed to work). If anyone ever gets
+ * hardware to test this driver, then that code might be useful to look at.
+ * You need to get the kernel sources of, say, kernel 3.8 where that
+ * wis-tw9903 driver is still present.
+ */
+
+struct tw9903 {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	v4l2_std_id norm;
+};
+
+static inline struct tw9903 *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct tw9903, sd);
+}
+
+static const u8 initial_registers[] = {
+	0x02, 0x44, /* input 1, composite */
+	0x03, 0x92, /* correct digital format */
+	0x04, 0x00,
+	0x05, 0x80, /* or 0x00 for PAL */
+	0x06, 0x40, /* second internal current reference */
+	0x07, 0x02, /* window */
+	0x08, 0x14, /* window */
+	0x09, 0xf0, /* window */
+	0x0a, 0x81, /* window */
+	0x0b, 0xd0, /* window */
+	0x0c, 0x8c,
+	0x0d, 0x00, /* scaling */
+	0x0e, 0x11, /* scaling */
+	0x0f, 0x00, /* scaling */
+	0x10, 0x00, /* brightness */
+	0x11, 0x60, /* contrast */
+	0x12, 0x01, /* sharpness */
+	0x13, 0x7f, /* U gain */
+	0x14, 0x5a, /* V gain */
+	0x15, 0x00, /* hue */
+	0x16, 0xc3, /* sharpness */
+	0x18, 0x00,
+	0x19, 0x58, /* vbi */
+	0x1a, 0x80,
+	0x1c, 0x0f, /* video norm */
+	0x1d, 0x7f, /* video norm */
+	0x20, 0xa0, /* clamping gain (working 0x50) */
+	0x21, 0x22,
+	0x22, 0xf0,
+	0x23, 0xfe,
+	0x24, 0x3c,
+	0x25, 0x38,
+	0x26, 0x44,
+	0x27, 0x20,
+	0x28, 0x00,
+	0x29, 0x15,
+	0x2a, 0xa0,
+	0x2b, 0x44,
+	0x2c, 0x37,
+	0x2d, 0x00,
+	0x2e, 0xa5, /* burst PLL control (working: a9) */
+	0x2f, 0xe0, /* 0xea is blue test frame -- 0xe0 for normal */
+	0x31, 0x00,
+	0x33, 0x22,
+	0x34, 0x11,
+	0x35, 0x35,
+	0x3b, 0x05,
+	0x06, 0xc0, /* reset device */
+	0x00, 0x00, /* Terminator (reg 0x00 is read-only) */
+};
+
+static int write_reg(struct v4l2_subdev *sd, u8 reg, u8 value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_write_byte_data(client, reg, value);
+}
+
+static int write_regs(struct v4l2_subdev *sd, const u8 *regs)
+{
+	int i;
+
+	for (i = 0; regs[i] != 0x00; i += 2)
+		if (write_reg(sd, regs[i], regs[i + 1]) < 0)
+			return -1;
+	return 0;
+}
+
+static int tw9903_s_video_routing(struct v4l2_subdev *sd, u32 input,
+				      u32 output, u32 config)
+{
+	write_reg(sd, 0x02, 0x40 | (input << 1));
+	return 0;
+}
+
+static int tw9903_s_std(struct v4l2_subdev *sd, v4l2_std_id norm)
+{
+	struct tw9903 *dec = to_state(sd);
+	bool is_60hz = norm & V4L2_STD_525_60;
+	static const u8 config_60hz[] = {
+		0x05, 0x80,
+		0x07, 0x02,
+		0x08, 0x14,
+		0x09, 0xf0,
+		0,    0,
+	};
+	static const u8 config_50hz[] = {
+		0x05, 0x00,
+		0x07, 0x12,
+		0x08, 0x18,
+		0x09, 0x20,
+		0,    0,
+	};
+
+	write_regs(sd, is_60hz ? config_60hz : config_50hz);
+	dec->norm = norm;
+	return 0;
+}
+
+
+static int tw9903_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct tw9903 *dec = container_of(ctrl->handler, struct tw9903, hdl);
+	struct v4l2_subdev *sd = &dec->sd;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		write_reg(sd, 0x10, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		write_reg(sd, 0x11, ctrl->val);
+		break;
+	case V4L2_CID_HUE:
+		write_reg(sd, 0x15, ctrl->val);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int tw9903_log_status(struct v4l2_subdev *sd)
+{
+	struct tw9903 *dec = to_state(sd);
+	bool is_60hz = dec->norm & V4L2_STD_525_60;
+
+	v4l2_info(sd, "Standard: %d Hz\n", is_60hz ? 60 : 50);
+	v4l2_ctrl_subdev_log_status(sd);
+	return 0;
+}
+
+/* --------------------------------------------------------------------------*/
+
+static const struct v4l2_ctrl_ops tw9903_ctrl_ops = {
+	.s_ctrl = tw9903_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops tw9903_core_ops = {
+	.log_status = tw9903_log_status,
+	.s_std = tw9903_s_std,
+};
+
+static const struct v4l2_subdev_video_ops tw9903_video_ops = {
+	.s_routing = tw9903_s_video_routing,
+};
+
+static const struct v4l2_subdev_ops tw9903_ops = {
+	.core = &tw9903_core_ops,
+	.video = &tw9903_video_ops,
+};
+
+/* --------------------------------------------------------------------------*/
+
+static int tw9903_probe(struct i2c_client *client,
+			     const struct i2c_device_id *id)
+{
+	struct tw9903 *dec;
+	struct v4l2_subdev *sd;
+	struct v4l2_ctrl_handler *hdl;
+
+	/* 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);
+
+	dec = kzalloc(sizeof(struct tw9903), GFP_KERNEL);
+	if (dec == NULL)
+		return -ENOMEM;
+	sd = &dec->sd;
+	v4l2_i2c_subdev_init(sd, client, &tw9903_ops);
+	hdl = &dec->hdl;
+	v4l2_ctrl_handler_init(hdl, 4);
+	v4l2_ctrl_new_std(hdl, &tw9903_ctrl_ops,
+		V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, &tw9903_ctrl_ops,
+		V4L2_CID_CONTRAST, 0, 255, 1, 0x60);
+	v4l2_ctrl_new_std(hdl, &tw9903_ctrl_ops,
+		V4L2_CID_HUE, -128, 127, 1, 0);
+	sd->ctrl_handler = hdl;
+	if (hdl->error) {
+		int err = hdl->error;
+
+		v4l2_ctrl_handler_free(hdl);
+		kfree(dec);
+		return err;
+	}
+
+	/* Initialize tw9903 */
+	dec->norm = V4L2_STD_NTSC;
+
+	if (write_regs(sd, initial_registers) < 0) {
+		v4l2_err(client, "error initializing TW9903\n");
+		kfree(dec);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int tw9903_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&to_state(sd)->hdl);
+	kfree(to_state(sd));
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id tw9903_id[] = {
+	{ "tw9903", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tw9903_id);
+
+static struct i2c_driver tw9903_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "tw9903",
+	},
+	.probe = tw9903_probe,
+	.remove = tw9903_remove,
+	.id_table = tw9903_id,
+};
+module_i2c_driver(tw9903_driver);
diff --git a/drivers/media/i2c/tw9906.c b/drivers/media/i2c/tw9906.c
new file mode 100644
index 00000000000..accd79e5a7f
--- /dev/null
+++ b/drivers/media/i2c/tw9906.c
@@ -0,0 +1,247 @@
+/*
+ * Copyright (C) 2005-2006 Micronas USA 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.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <linux/ioctl.h>
+#include <linux/slab.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+
+MODULE_DESCRIPTION("TW9906 I2C subdev driver");
+MODULE_LICENSE("GPL v2");
+
+struct tw9906 {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	v4l2_std_id norm;
+};
+
+static inline struct tw9906 *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct tw9906, sd);
+}
+
+static const u8 initial_registers[] = {
+	0x02, 0x40, /* input 0, composite */
+	0x03, 0xa2, /* correct digital format */
+	0x05, 0x81, /* or 0x01 for PAL */
+	0x07, 0x02, /* window */
+	0x08, 0x14, /* window */
+	0x09, 0xf0, /* window */
+	0x0a, 0x10, /* window */
+	0x0b, 0xd0, /* window */
+	0x0d, 0x00, /* scaling */
+	0x0e, 0x11, /* scaling */
+	0x0f, 0x00, /* scaling */
+	0x10, 0x00, /* brightness */
+	0x11, 0x60, /* contrast */
+	0x12, 0x11, /* sharpness */
+	0x13, 0x7e, /* U gain */
+	0x14, 0x7e, /* V gain */
+	0x15, 0x00, /* hue */
+	0x19, 0x57, /* vbi */
+	0x1a, 0x0f,
+	0x1b, 0x40,
+	0x29, 0x03,
+	0x55, 0x00,
+	0x6b, 0x26,
+	0x6c, 0x36,
+	0x6d, 0xf0,
+	0x6e, 0x41,
+	0x6f, 0x13,
+	0xad, 0x70,
+	0x00, 0x00, /* Terminator (reg 0x00 is read-only) */
+};
+
+static int write_reg(struct v4l2_subdev *sd, u8 reg, u8 value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_write_byte_data(client, reg, value);
+}
+
+static int write_regs(struct v4l2_subdev *sd, const u8 *regs)
+{
+	int i;
+
+	for (i = 0; regs[i] != 0x00; i += 2)
+		if (write_reg(sd, regs[i], regs[i + 1]) < 0)
+			return -1;
+	return 0;
+}
+
+static int tw9906_s_video_routing(struct v4l2_subdev *sd, u32 input,
+				      u32 output, u32 config)
+{
+	write_reg(sd, 0x02, 0x40 | (input << 1));
+	return 0;
+}
+
+static int tw9906_s_std(struct v4l2_subdev *sd, v4l2_std_id norm)
+{
+	struct tw9906 *dec = to_state(sd);
+	bool is_60hz = norm & V4L2_STD_525_60;
+	static const u8 config_60hz[] = {
+		0x05, 0x81,
+		0x07, 0x02,
+		0x08, 0x14,
+		0x09, 0xf0,
+		0,    0,
+	};
+	static const u8 config_50hz[] = {
+		0x05, 0x01,
+		0x07, 0x12,
+		0x08, 0x18,
+		0x09, 0x20,
+		0,    0,
+	};
+
+	write_regs(sd, is_60hz ? config_60hz : config_50hz);
+	dec->norm = norm;
+	return 0;
+}
+
+static int tw9906_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct tw9906 *dec = container_of(ctrl->handler, struct tw9906, hdl);
+	struct v4l2_subdev *sd = &dec->sd;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		write_reg(sd, 0x10, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		write_reg(sd, 0x11, ctrl->val);
+		break;
+	case V4L2_CID_HUE:
+		write_reg(sd, 0x15, ctrl->val);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int tw9906_log_status(struct v4l2_subdev *sd)
+{
+	struct tw9906 *dec = to_state(sd);
+	bool is_60hz = dec->norm & V4L2_STD_525_60;
+
+	v4l2_info(sd, "Standard: %d Hz\n", is_60hz ? 60 : 50);
+	v4l2_ctrl_subdev_log_status(sd);
+	return 0;
+}
+
+/* --------------------------------------------------------------------------*/
+
+static const struct v4l2_ctrl_ops tw9906_ctrl_ops = {
+	.s_ctrl = tw9906_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops tw9906_core_ops = {
+	.log_status = tw9906_log_status,
+	.s_std = tw9906_s_std,
+};
+
+static const struct v4l2_subdev_video_ops tw9906_video_ops = {
+	.s_routing = tw9906_s_video_routing,
+};
+
+static const struct v4l2_subdev_ops tw9906_ops = {
+	.core = &tw9906_core_ops,
+	.video = &tw9906_video_ops,
+};
+
+static int tw9906_probe(struct i2c_client *client,
+			     const struct i2c_device_id *id)
+{
+	struct tw9906 *dec;
+	struct v4l2_subdev *sd;
+	struct v4l2_ctrl_handler *hdl;
+
+	/* 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);
+
+	dec = kzalloc(sizeof(struct tw9906), GFP_KERNEL);
+	if (dec == NULL)
+		return -ENOMEM;
+	sd = &dec->sd;
+	v4l2_i2c_subdev_init(sd, client, &tw9906_ops);
+	hdl = &dec->hdl;
+	v4l2_ctrl_handler_init(hdl, 4);
+	v4l2_ctrl_new_std(hdl, &tw9906_ctrl_ops,
+		V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, &tw9906_ctrl_ops,
+		V4L2_CID_CONTRAST, 0, 255, 1, 0x60);
+	v4l2_ctrl_new_std(hdl, &tw9906_ctrl_ops,
+		V4L2_CID_HUE, -128, 127, 1, 0);
+	sd->ctrl_handler = hdl;
+	if (hdl->error) {
+		int err = hdl->error;
+
+		v4l2_ctrl_handler_free(hdl);
+		kfree(dec);
+		return err;
+	}
+
+	/* Initialize tw9906 */
+	dec->norm = V4L2_STD_NTSC;
+
+	if (write_regs(sd, initial_registers) < 0) {
+		v4l2_err(client, "error initializing TW9906\n");
+		kfree(dec);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int tw9906_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&to_state(sd)->hdl);
+	kfree(to_state(sd));
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id tw9906_id[] = {
+	{ "tw9906", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tw9906_id);
+
+static struct i2c_driver tw9906_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "tw9906",
+	},
+	.probe = tw9906_probe,
+	.remove = tw9906_remove,
+	.id_table = tw9906_id,
+};
+module_i2c_driver(tw9906_driver);
diff --git a/drivers/staging/media/go7007/wis-uda1342.c b/drivers/media/i2c/uda1342.c
index 582ea120a53..3af408556d2 100644
--- a/drivers/staging/media/go7007/wis-uda1342.c
+++ b/drivers/media/i2c/uda1342.c
@@ -19,10 +19,9 @@
 #include <linux/init.h>
 #include <linux/i2c.h>
 #include <linux/videodev2.h>
-#include <media/tvaudio.h>
-#include <media/v4l2-common.h>
-
-#include "wis-i2c.h"
+#include <media/v4l2-device.h>
+#include <media/uda1342.h>
+#include <linux/slab.h>
 
 static int write_reg(struct i2c_client *client, int reg, int value)
 {
@@ -31,38 +30,38 @@ static int write_reg(struct i2c_client *client, int reg, int value)
 	return 0;
 }
 
-static int wis_uda1342_command(struct i2c_client *client,
-				unsigned int cmd, void *arg)
+static int uda1342_s_routing(struct v4l2_subdev *sd,
+		u32 input, u32 output, u32 config)
 {
-	switch (cmd) {
-	case VIDIOC_S_AUDIO:
-	{
-		int *inp = arg;
-
-		switch (*inp) {
-		case TVAUDIO_INPUT_TUNER:
-			write_reg(client, 0x00, 0x1441); /* select input 2 */
-			break;
-		case TVAUDIO_INPUT_EXTERN:
-			write_reg(client, 0x00, 0x1241); /* select input 1 */
-			break;
-		default:
-			dev_err(&client->dev, "input %d not supported\n",
-				*inp);
-			break;
-		}
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	switch (input) {
+	case UDA1342_IN1:
+		write_reg(client, 0x00, 0x1241); /* select input 1 */
+		break;
+	case UDA1342_IN2:
+		write_reg(client, 0x00, 0x1441); /* select input 2 */
 		break;
-	}
 	default:
+		v4l2_err(sd, "input %d not supported\n", input);
 		break;
 	}
 	return 0;
 }
 
-static int wis_uda1342_probe(struct i2c_client *client,
+static const struct v4l2_subdev_audio_ops uda1342_audio_ops = {
+	.s_routing = uda1342_s_routing,
+};
+
+static const struct v4l2_subdev_ops uda1342_ops = {
+	.audio = &uda1342_audio_ops,
+};
+
+static int uda1342_probe(struct i2c_client *client,
 			     const struct i2c_device_id *id)
 {
 	struct i2c_adapter *adapter = client->adapter;
+	struct v4l2_subdev *sd;
 
 	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA))
 		return -ENODEV;
@@ -70,33 +69,45 @@ static int wis_uda1342_probe(struct i2c_client *client,
 	dev_dbg(&client->dev, "initializing UDA1342 at address %d on %s\n",
 		client->addr, adapter->name);
 
+	sd = kzalloc(sizeof(struct v4l2_subdev), GFP_KERNEL);
+	if (sd == NULL)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(sd, client, &uda1342_ops);
+
 	write_reg(client, 0x00, 0x8000); /* reset registers */
 	write_reg(client, 0x00, 0x1241); /* select input 1 */
 
+	v4l_info(client, "chip found @ 0x%02x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
 	return 0;
 }
 
-static int wis_uda1342_remove(struct i2c_client *client)
+static int uda1342_remove(struct i2c_client *client)
 {
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(sd);
 	return 0;
 }
 
-static const struct i2c_device_id wis_uda1342_id[] = {
-	{ "wis_uda1342", 0 },
+static const struct i2c_device_id uda1342_id[] = {
+	{ "uda1342", 0 },
 	{ }
 };
-MODULE_DEVICE_TABLE(i2c, wis_uda1342_id);
+MODULE_DEVICE_TABLE(i2c, uda1342_id);
 
-static struct i2c_driver wis_uda1342_driver = {
+static struct i2c_driver uda1342_driver = {
 	.driver = {
-		.name	= "WIS UDA1342 I2C driver",
+		.name	= "uda1342",
 	},
-	.probe		= wis_uda1342_probe,
-	.remove		= wis_uda1342_remove,
-	.command	= wis_uda1342_command,
-	.id_table	= wis_uda1342_id,
+	.probe		= uda1342_probe,
+	.remove		= uda1342_remove,
+	.id_table	= uda1342_id,
 };
 
-module_i2c_driver(wis_uda1342_driver);
+module_i2c_driver(uda1342_driver);
 
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/upd64031a.c b/drivers/media/i2c/upd64031a.c
index 1e744654209..f0a09214c51 100644
--- a/drivers/media/i2c/upd64031a.c
+++ b/drivers/media/i2c/upd64031a.c
@@ -111,7 +111,7 @@ static void upd64031a_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 /* ------------------------------------------------------------------------ */
 
 /* The input changed due to new input or channel changed */
-static int upd64031a_s_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *freq)
+static int upd64031a_s_frequency(struct v4l2_subdev *sd, const struct v4l2_frequency *freq)
 {
 	struct upd64031a_state *state = to_state(sd);
 	u8 reg = state->regs[R00];
@@ -175,7 +175,7 @@ static int upd64031a_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register
 	return 0;
 }
 
-static int upd64031a_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int upd64031a_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/upd64083.c b/drivers/media/i2c/upd64083.c
index 75d6acc6201..343e0215f74 100644
--- a/drivers/media/i2c/upd64083.c
+++ b/drivers/media/i2c/upd64083.c
@@ -133,7 +133,7 @@ static int upd64083_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register
 	return 0;
 }
 
-static int upd64083_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int upd64083_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/vp27smpx.c b/drivers/media/i2c/vp27smpx.c
index 7cfbc9d94a4..e71f139695a 100644
--- a/drivers/media/i2c/vp27smpx.c
+++ b/drivers/media/i2c/vp27smpx.c
@@ -90,7 +90,7 @@ static int vp27smpx_s_std(struct v4l2_subdev *sd, v4l2_std_id norm)
 	return 0;
 }
 
-static int vp27smpx_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+static int vp27smpx_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
 {
 	struct vp27smpx_state *state = to_state(sd);
 
diff --git a/drivers/media/i2c/vs6624.c b/drivers/media/i2c/vs6624.c
index 9ac1b8c3a83..f366fad6269 100644
--- a/drivers/media/i2c/vs6624.c
+++ b/drivers/media/i2c/vs6624.c
@@ -748,7 +748,7 @@ static int vs6624_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *r
 	return 0;
 }
 
-static int vs6624_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int vs6624_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/wm8775.c b/drivers/media/i2c/wm8775.c
index bee77ea9f49..27c27b4ae23 100644
--- a/drivers/media/i2c/wm8775.c
+++ b/drivers/media/i2c/wm8775.c
@@ -174,7 +174,7 @@ static int wm8775_log_status(struct v4l2_subdev *sd)
 	return 0;
 }
 
-static int wm8775_s_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *freq)
+static int wm8775_s_frequency(struct v4l2_subdev *sd, const struct v4l2_frequency *freq)
 {
 	wm8775_set_audio(sd, 0);
 	return 0;
diff --git a/drivers/media/media-device.c b/drivers/media/media-device.c
index d01fcb7e87c..1957c0df08f 100644
--- a/drivers/media/media-device.c
+++ b/drivers/media/media-device.c
@@ -20,10 +20,11 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  */
 
-#include <linux/types.h>
+#include <linux/compat.h>
+#include <linux/export.h>
 #include <linux/ioctl.h>
 #include <linux/media.h>
-#include <linux/export.h>
+#include <linux/types.h>
 
 #include <media/media-device.h>
 #include <media/media-devnode.h>
@@ -101,9 +102,12 @@ static long media_device_enum_entities(struct media_device *mdev,
 		return -EINVAL;
 
 	u_ent.id = ent->id;
-	u_ent.name[0] = '\0';
-	if (ent->name)
-		strlcpy(u_ent.name, ent->name, sizeof(u_ent.name));
+	if (ent->name) {
+		strncpy(u_ent.name, ent->name, sizeof(u_ent.name));
+		u_ent.name[sizeof(u_ent.name) - 1] = '\0';
+	} else {
+		memset(u_ent.name, 0, sizeof(u_ent.name));
+	}
 	u_ent.type = ent->type;
 	u_ent.revision = ent->revision;
 	u_ent.flags = ent->flags;
@@ -124,35 +128,31 @@ static void media_device_kpad_to_upad(const struct media_pad *kpad,
 	upad->flags = kpad->flags;
 }
 
-static long media_device_enum_links(struct media_device *mdev,
-				    struct media_links_enum __user *ulinks)
+static long __media_device_enum_links(struct media_device *mdev,
+				      struct media_links_enum *links)
 {
 	struct media_entity *entity;
-	struct media_links_enum links;
 
-	if (copy_from_user(&links, ulinks, sizeof(links)))
-		return -EFAULT;
-
-	entity = find_entity(mdev, links.entity);
+	entity = find_entity(mdev, links->entity);
 	if (entity == NULL)
 		return -EINVAL;
 
-	if (links.pads) {
+	if (links->pads) {
 		unsigned int p;
 
 		for (p = 0; p < entity->num_pads; p++) {
 			struct media_pad_desc pad;
 			media_device_kpad_to_upad(&entity->pads[p], &pad);
-			if (copy_to_user(&links.pads[p], &pad, sizeof(pad)))
+			if (copy_to_user(&links->pads[p], &pad, sizeof(pad)))
 				return -EFAULT;
 		}
 	}
 
-	if (links.links) {
+	if (links->links) {
 		struct media_link_desc __user *ulink;
 		unsigned int l;
 
-		for (l = 0, ulink = links.links; l < entity->num_links; l++) {
+		for (l = 0, ulink = links->links; l < entity->num_links; l++) {
 			struct media_link_desc link;
 
 			/* Ignore backlinks. */
@@ -169,8 +169,26 @@ static long media_device_enum_links(struct media_device *mdev,
 			ulink++;
 		}
 	}
+
+	return 0;
+}
+
+static long media_device_enum_links(struct media_device *mdev,
+				    struct media_links_enum __user *ulinks)
+{
+	struct media_links_enum links;
+	int rval;
+
+	if (copy_from_user(&links, ulinks, sizeof(links)))
+		return -EFAULT;
+
+	rval = __media_device_enum_links(mdev, &links);
+	if (rval < 0)
+		return rval;
+
 	if (copy_to_user(ulinks, &links, sizeof(*ulinks)))
 		return -EFAULT;
+
 	return 0;
 }
 
@@ -251,10 +269,71 @@ static long media_device_ioctl(struct file *filp, unsigned int cmd,
 	return ret;
 }
 
+#ifdef CONFIG_COMPAT
+
+struct media_links_enum32 {
+	__u32 entity;
+	compat_uptr_t pads; /* struct media_pad_desc * */
+	compat_uptr_t links; /* struct media_link_desc * */
+	__u32 reserved[4];
+};
+
+static long media_device_enum_links32(struct media_device *mdev,
+				      struct media_links_enum32 __user *ulinks)
+{
+	struct media_links_enum links;
+	compat_uptr_t pads_ptr, links_ptr;
+
+	memset(&links, 0, sizeof(links));
+
+	if (get_user(links.entity, &ulinks->entity)
+	    || get_user(pads_ptr, &ulinks->pads)
+	    || get_user(links_ptr, &ulinks->links))
+		return -EFAULT;
+
+	links.pads = compat_ptr(pads_ptr);
+	links.links = compat_ptr(links_ptr);
+
+	return __media_device_enum_links(mdev, &links);
+}
+
+#define MEDIA_IOC_ENUM_LINKS32		_IOWR('|', 0x02, struct media_links_enum32)
+
+static long media_device_compat_ioctl(struct file *filp, unsigned int cmd,
+				      unsigned long arg)
+{
+	struct media_devnode *devnode = media_devnode_data(filp);
+	struct media_device *dev = to_media_device(devnode);
+	long ret;
+
+	switch (cmd) {
+	case MEDIA_IOC_DEVICE_INFO:
+	case MEDIA_IOC_ENUM_ENTITIES:
+	case MEDIA_IOC_SETUP_LINK:
+		return media_device_ioctl(filp, cmd, arg);
+
+	case MEDIA_IOC_ENUM_LINKS32:
+		mutex_lock(&dev->graph_mutex);
+		ret = media_device_enum_links32(dev,
+				(struct media_links_enum32 __user *)arg);
+		mutex_unlock(&dev->graph_mutex);
+		break;
+
+	default:
+		ret = -ENOIOCTLCMD;
+	}
+
+	return ret;
+}
+#endif /* CONFIG_COMPAT */
+
 static const struct media_file_operations media_device_fops = {
 	.owner = THIS_MODULE,
 	.open = media_device_open,
 	.ioctl = media_device_ioctl,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl = media_device_compat_ioctl,
+#endif /* CONFIG_COMPAT */
 	.release = media_device_close,
 };
 
diff --git a/drivers/media/media-devnode.c b/drivers/media/media-devnode.c
index 023b2a1cbb9..fb0f0469fad 100644
--- a/drivers/media/media-devnode.c
+++ b/drivers/media/media-devnode.c
@@ -116,19 +116,41 @@ static unsigned int media_poll(struct file *filp,
 	return mdev->fops->poll(filp, poll);
 }
 
-static long media_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+static long
+__media_ioctl(struct file *filp, unsigned int cmd, unsigned long arg,
+	      long (*ioctl_func)(struct file *filp, unsigned int cmd,
+				 unsigned long arg))
 {
 	struct media_devnode *mdev = media_devnode_data(filp);
 
-	if (!mdev->fops->ioctl)
+	if (!ioctl_func)
 		return -ENOTTY;
 
 	if (!media_devnode_is_registered(mdev))
 		return -EIO;
 
-	return mdev->fops->ioctl(filp, cmd, arg);
+	return ioctl_func(filp, cmd, arg);
+}
+
+static long media_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+	struct media_devnode *mdev = media_devnode_data(filp);
+
+	return __media_ioctl(filp, cmd, arg, mdev->fops->ioctl);
 }
 
+#ifdef CONFIG_COMPAT
+
+static long media_compat_ioctl(struct file *filp, unsigned int cmd,
+			       unsigned long arg)
+{
+	struct media_devnode *mdev = media_devnode_data(filp);
+
+	return __media_ioctl(filp, cmd, arg, mdev->fops->compat_ioctl);
+}
+
+#endif /* CONFIG_COMPAT */
+
 /* Override for the open function */
 static int media_open(struct inode *inode, struct file *filp)
 {
@@ -188,6 +210,9 @@ static const struct file_operations media_devnode_fops = {
 	.write = media_write,
 	.open = media_open,
 	.unlocked_ioctl = media_ioctl,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl = media_compat_ioctl,
+#endif /* CONFIG_COMPAT */
 	.release = media_release,
 	.poll = media_poll,
 	.llseek = no_llseek,
diff --git a/drivers/media/mmc/siano/smssdio.c b/drivers/media/mmc/siano/smssdio.c
index 15d34935e00..912c2814c6c 100644
--- a/drivers/media/mmc/siano/smssdio.c
+++ b/drivers/media/mmc/siano/smssdio.c
@@ -43,6 +43,7 @@
 
 #include "smscoreapi.h"
 #include "sms-cards.h"
+#include "smsendian.h"
 
 /* Registers */
 
@@ -61,6 +62,16 @@ static const struct sdio_device_id smssdio_ids[] = {
 	 .driver_data = SMS1XXX_BOARD_SIANO_VEGA},
 	{SDIO_DEVICE(SDIO_VENDOR_ID_SIANO, SDIO_DEVICE_ID_SIANO_VENICE),
 	 .driver_data = SMS1XXX_BOARD_SIANO_VEGA},
+	{SDIO_DEVICE(SDIO_VENDOR_ID_SIANO, 0x302),
+	.driver_data = SMS1XXX_BOARD_SIANO_MING},
+	{SDIO_DEVICE(SDIO_VENDOR_ID_SIANO, 0x500),
+	.driver_data = SMS1XXX_BOARD_SIANO_PELE},
+	{SDIO_DEVICE(SDIO_VENDOR_ID_SIANO, 0x600),
+	.driver_data = SMS1XXX_BOARD_SIANO_RIO},
+	{SDIO_DEVICE(SDIO_VENDOR_ID_SIANO, 0x700),
+	.driver_data = SMS1XXX_BOARD_SIANO_DENVER_2160},
+	{SDIO_DEVICE(SDIO_VENDOR_ID_SIANO, 0x800),
+	.driver_data = SMS1XXX_BOARD_SIANO_DENVER_1530},
 	{ /* end: all zeroes */ },
 };
 
@@ -87,6 +98,7 @@ static int smssdio_sendrequest(void *context, void *buffer, size_t size)
 
 	sdio_claim_host(smsdev->func);
 
+	smsendian_handle_tx_message((struct sms_msg_data *) buffer);
 	while (size >= smsdev->func->cur_blksize) {
 		ret = sdio_memcpy_toio(smsdev->func, SMSSDIO_DATA,
 					buffer, smsdev->func->cur_blksize);
@@ -118,7 +130,7 @@ static void smssdio_interrupt(struct sdio_func *func)
 
 	struct smssdio_device *smsdev;
 	struct smscore_buffer_t *cb;
-	struct SmsMsgHdr_ST *hdr;
+	struct sms_msg_hdr *hdr;
 	size_t size;
 
 	smsdev = sdio_get_drvdata(func);
@@ -151,20 +163,20 @@ static void smssdio_interrupt(struct sdio_func *func)
 
 		hdr = cb->p;
 
-		if (hdr->msgFlags & MSG_HDR_FLAG_SPLIT_MSG) {
+		if (hdr->msg_flags & MSG_HDR_FLAG_SPLIT_MSG) {
 			smsdev->split_cb = cb;
 			return;
 		}
 
-		if (hdr->msgLength > smsdev->func->cur_blksize)
-			size = hdr->msgLength - smsdev->func->cur_blksize;
+		if (hdr->msg_length > smsdev->func->cur_blksize)
+			size = hdr->msg_length - smsdev->func->cur_blksize;
 		else
 			size = 0;
 	} else {
 		cb = smsdev->split_cb;
 		hdr = cb->p;
 
-		size = hdr->msgLength - sizeof(struct SmsMsgHdr_ST);
+		size = hdr->msg_length - sizeof(struct sms_msg_hdr);
 
 		smsdev->split_cb = NULL;
 	}
@@ -172,7 +184,7 @@ static void smssdio_interrupt(struct sdio_func *func)
 	if (size) {
 		void *buffer;
 
-		buffer = cb->p + (hdr->msgLength - size);
+		buffer = cb->p + (hdr->msg_length - size);
 		size = ALIGN(size, SMSSDIO_BLOCK_SIZE);
 
 		BUG_ON(smsdev->func->cur_blksize != SMSSDIO_BLOCK_SIZE);
@@ -218,9 +230,10 @@ static void smssdio_interrupt(struct sdio_func *func)
 		}
 	}
 
-	cb->size = hdr->msgLength;
+	cb->size = hdr->msg_length;
 	cb->offset = 0;
 
+	smsendian_handle_rx_message((struct sms_msg_data *) cb->p);
 	smscore_onresponse(smsdev->coredev, cb);
 }
 
diff --git a/drivers/media/parport/pms.c b/drivers/media/parport/pms.c
index 77f9c92186f..66c957a02ba 100644
--- a/drivers/media/parport/pms.c
+++ b/drivers/media/parport/pms.c
@@ -735,12 +735,12 @@ static int pms_g_std(struct file *file, void *fh, v4l2_std_id *std)
 	return 0;
 }
 
-static int pms_s_std(struct file *file, void *fh, v4l2_std_id *std)
+static int pms_s_std(struct file *file, void *fh, v4l2_std_id std)
 {
 	struct pms *dev = video_drvdata(file);
 	int ret = 0;
 
-	dev->std = *std;
+	dev->std = std;
 	if (dev->std & V4L2_STD_NTSC) {
 		pms_framerate(dev, 30);
 		pms_secamcross(dev, 0);
diff --git a/drivers/media/pci/bt8xx/bttv-cards.c b/drivers/media/pci/bt8xx/bttv-cards.c
index c4c59175e52..b7dc921e1b9 100644
--- a/drivers/media/pci/bt8xx/bttv-cards.c
+++ b/drivers/media/pci/bt8xx/bttv-cards.c
@@ -3547,6 +3547,16 @@ void bttv_init_card2(struct bttv *btv)
 	if (btv->sd_msp34xx)
 		return;
 
+	/* Now see if we can find one of the tvaudio devices. */
+	btv->sd_tvaudio = v4l2_i2c_new_subdev(&btv->c.v4l2_dev,
+		&btv->c.i2c_adap, "tvaudio", 0, tvaudio_addrs());
+	if (btv->sd_tvaudio) {
+		/* There may be two tvaudio chips on the card, so try to
+		   find another. */
+		v4l2_i2c_new_subdev(&btv->c.v4l2_dev,
+			&btv->c.i2c_adap, "tvaudio", 0, tvaudio_addrs());
+	}
+
 	/* it might also be a tda7432. */
 	if (!bttv_tvcards[btv->c.type].no_tda7432) {
 		static const unsigned short addrs[] = {
@@ -3554,14 +3564,11 @@ void bttv_init_card2(struct bttv *btv)
 			I2C_CLIENT_END
 		};
 
-		if (v4l2_i2c_new_subdev(&btv->c.v4l2_dev,
-				&btv->c.i2c_adap, "tda7432", 0, addrs))
+		btv->sd_tda7432 = v4l2_i2c_new_subdev(&btv->c.v4l2_dev,
+				&btv->c.i2c_adap, "tda7432", 0, addrs);
+		if (btv->sd_tda7432)
 			return;
 	}
-
-	/* Now see if we can find one of the tvaudio devices. */
-	btv->sd_tvaudio = v4l2_i2c_new_subdev(&btv->c.v4l2_dev,
-		&btv->c.i2c_adap, "tvaudio", 0, tvaudio_addrs());
 	if (btv->sd_tvaudio)
 		return;
 
@@ -3940,7 +3947,7 @@ static void avermedia_eeprom(struct bttv *btv)
 u32 bttv_tda9880_setnorm(struct bttv *btv, u32 gpiobits)
 {
 
-	if (btv->audio == TVAUDIO_INPUT_TUNER) {
+	if (btv->audio_input == TVAUDIO_INPUT_TUNER) {
 		if (bttv_tvnorms[btv->tvnorm].v4l2_id & V4L2_STD_MN)
 			gpiobits |= 0x10000;
 		else
diff --git a/drivers/media/pci/bt8xx/bttv-driver.c b/drivers/media/pci/bt8xx/bttv-driver.c
index ccd18e4ee78..e7d08841341 100644
--- a/drivers/media/pci/bt8xx/bttv-driver.c
+++ b/drivers/media/pci/bt8xx/bttv-driver.c
@@ -49,6 +49,8 @@
 #include "bttvp.h"
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-chip-ident.h>
 #include <media/tvaudio.h>
 #include <media/msp3400.h>
 
@@ -93,7 +95,7 @@ static unsigned int combfilter;
 static unsigned int lumafilter;
 static unsigned int automute    = 1;
 static unsigned int chroma_agc;
-static unsigned int adc_crush   = 1;
+static unsigned int agc_crush   = 1;
 static unsigned int whitecrush_upper = 0xCF;
 static unsigned int whitecrush_lower = 0x7F;
 static unsigned int vcr_hack;
@@ -125,7 +127,7 @@ module_param(combfilter,        int, 0444);
 module_param(lumafilter,        int, 0444);
 module_param(automute,          int, 0444);
 module_param(chroma_agc,        int, 0444);
-module_param(adc_crush,         int, 0444);
+module_param(agc_crush,         int, 0444);
 module_param(whitecrush_upper,  int, 0444);
 module_param(whitecrush_lower,  int, 0444);
 module_param(vcr_hack,          int, 0444);
@@ -138,27 +140,27 @@ module_param_array(video_nr,    int, NULL, 0444);
 module_param_array(radio_nr,    int, NULL, 0444);
 module_param_array(vbi_nr,      int, NULL, 0444);
 
-MODULE_PARM_DESC(radio,"The TV card supports radio, default is 0 (no)");
-MODULE_PARM_DESC(bigendian,"byte order of the framebuffer, default is native endian");
-MODULE_PARM_DESC(bttv_verbose,"verbose startup messages, default is 1 (yes)");
-MODULE_PARM_DESC(bttv_gpio,"log gpio changes, default is 0 (no)");
-MODULE_PARM_DESC(bttv_debug,"debug messages, default is 0 (no)");
-MODULE_PARM_DESC(irq_debug,"irq handler debug messages, default is 0 (no)");
+MODULE_PARM_DESC(radio, "The TV card supports radio, default is 0 (no)");
+MODULE_PARM_DESC(bigendian, "byte order of the framebuffer, default is native endian");
+MODULE_PARM_DESC(bttv_verbose, "verbose startup messages, default is 1 (yes)");
+MODULE_PARM_DESC(bttv_gpio, "log gpio changes, default is 0 (no)");
+MODULE_PARM_DESC(bttv_debug, "debug messages, default is 0 (no)");
+MODULE_PARM_DESC(irq_debug, "irq handler debug messages, default is 0 (no)");
 MODULE_PARM_DESC(disable_ir, "disable infrared remote support");
-MODULE_PARM_DESC(gbuffers,"number of capture buffers. range 2-32, default 8");
-MODULE_PARM_DESC(gbufsize,"size of the capture buffers, default is 0x208000");
-MODULE_PARM_DESC(reset_crop,"reset cropping parameters at open(), default "
+MODULE_PARM_DESC(gbuffers, "number of capture buffers. range 2-32, default 8");
+MODULE_PARM_DESC(gbufsize, "size of the capture buffers, default is 0x208000");
+MODULE_PARM_DESC(reset_crop, "reset cropping parameters at open(), default "
 		 "is 1 (yes) for compatibility with older applications");
-MODULE_PARM_DESC(automute,"mute audio on bad/missing video signal, default is 1 (yes)");
-MODULE_PARM_DESC(chroma_agc,"enables the AGC of chroma signal, default is 0 (no)");
-MODULE_PARM_DESC(adc_crush,"enables the luminance ADC crush, default is 1 (yes)");
-MODULE_PARM_DESC(whitecrush_upper,"sets the white crush upper value, default is 207");
-MODULE_PARM_DESC(whitecrush_lower,"sets the white crush lower value, default is 127");
-MODULE_PARM_DESC(vcr_hack,"enables the VCR hack (improves synch on poor VCR tapes), default is 0 (no)");
-MODULE_PARM_DESC(irq_iswitch,"switch inputs in irq handler");
-MODULE_PARM_DESC(uv_ratio,"ratio between u and v gains, default is 50");
-MODULE_PARM_DESC(full_luma_range,"use the full luma range, default is 0 (no)");
-MODULE_PARM_DESC(coring,"set the luma coring level, default is 0 (no)");
+MODULE_PARM_DESC(automute, "mute audio on bad/missing video signal, default is 1 (yes)");
+MODULE_PARM_DESC(chroma_agc, "enables the AGC of chroma signal, default is 0 (no)");
+MODULE_PARM_DESC(agc_crush, "enables the luminance AGC crush, default is 1 (yes)");
+MODULE_PARM_DESC(whitecrush_upper, "sets the white crush upper value, default is 207");
+MODULE_PARM_DESC(whitecrush_lower, "sets the white crush lower value, default is 127");
+MODULE_PARM_DESC(vcr_hack, "enables the VCR hack (improves synch on poor VCR tapes), default is 0 (no)");
+MODULE_PARM_DESC(irq_iswitch, "switch inputs in irq handler");
+MODULE_PARM_DESC(uv_ratio, "ratio between u and v gains, default is 50");
+MODULE_PARM_DESC(full_luma_range, "use the full luma range, default is 0 (no)");
+MODULE_PARM_DESC(coring, "set the luma coring level, default is 0 (no)");
 MODULE_PARM_DESC(video_nr, "video device numbers");
 MODULE_PARM_DESC(vbi_nr, "vbi device numbers");
 MODULE_PARM_DESC(radio_nr, "radio device numbers");
@@ -168,6 +170,17 @@ MODULE_AUTHOR("Ralph Metzler & Marcus Metzler & Gerd Knorr");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(BTTV_VERSION);
 
+#define V4L2_CID_PRIVATE_COMBFILTER		(V4L2_CID_USER_BTTV_BASE + 0)
+#define V4L2_CID_PRIVATE_AUTOMUTE		(V4L2_CID_USER_BTTV_BASE + 1)
+#define V4L2_CID_PRIVATE_LUMAFILTER		(V4L2_CID_USER_BTTV_BASE + 2)
+#define V4L2_CID_PRIVATE_AGC_CRUSH		(V4L2_CID_USER_BTTV_BASE + 3)
+#define V4L2_CID_PRIVATE_VCR_HACK		(V4L2_CID_USER_BTTV_BASE + 4)
+#define V4L2_CID_PRIVATE_WHITECRUSH_LOWER	(V4L2_CID_USER_BTTV_BASE + 5)
+#define V4L2_CID_PRIVATE_WHITECRUSH_UPPER	(V4L2_CID_USER_BTTV_BASE + 6)
+#define V4L2_CID_PRIVATE_UV_RATIO		(V4L2_CID_USER_BTTV_BASE + 7)
+#define V4L2_CID_PRIVATE_FULL_LUMA_RANGE	(V4L2_CID_USER_BTTV_BASE + 8)
+#define V4L2_CID_PRIVATE_CORING			(V4L2_CID_USER_BTTV_BASE + 9)
+
 /* ----------------------------------------------------------------------- */
 /* sysfs                                                                   */
 
@@ -250,17 +263,19 @@ static u8 SRAM_Table[][60] =
    vdelay	start of active video in 2 * field lines relative to
 		trailing edge of /VRESET pulse (VDELAY register).
    sheight	height of active video in 2 * field lines.
+   extraheight	Added to sheight for cropcap.bounds.height only
    videostart0	ITU-R frame line number of the line corresponding
 		to vdelay in the first field. */
 #define CROPCAP(minhdelayx1, hdelayx1, swidth, totalwidth, sqwidth,	 \
-		vdelay, sheight, videostart0)				 \
+		vdelay, sheight, extraheight, videostart0)		 \
 	.cropcap.bounds.left = minhdelayx1,				 \
 	/* * 2 because vertically we count field lines times two, */	 \
 	/* e.g. 23 * 2 to 23 * 2 + 576 in PAL-BGHI defrect. */		 \
 	.cropcap.bounds.top = (videostart0) * 2 - (vdelay) + MIN_VDELAY, \
 	/* 4 is a safety margin at the end of the line. */		 \
 	.cropcap.bounds.width = (totalwidth) - (minhdelayx1) - 4,	 \
-	.cropcap.bounds.height = (sheight) + (vdelay) - MIN_VDELAY,	 \
+	.cropcap.bounds.height = (sheight) + (extraheight) + (vdelay) -	 \
+				 MIN_VDELAY,				 \
 	.cropcap.defrect.left = hdelayx1,				 \
 	.cropcap.defrect.top = (videostart0) * 2,			 \
 	.cropcap.defrect.width = swidth,				 \
@@ -301,9 +316,10 @@ const struct bttv_tvnorm bttv_tvnorms[] = {
 			/* totalwidth */ 1135,
 			/* sqwidth */ 944,
 			/* vdelay */ 0x20,
-		/* bt878 (and bt848?) can capture another
-		   line below active video. */
-			/* sheight */ (576 + 2) + 0x20 - 2,
+			/* sheight */ 576,
+			/* bt878 (and bt848?) can capture another
+			   line below active video. */
+			/* extraheight */ 2,
 			/* videostart0 */ 23)
 	},{
 		.v4l2_id        = V4L2_STD_NTSC_M | V4L2_STD_NTSC_M_KR,
@@ -330,6 +346,7 @@ const struct bttv_tvnorm bttv_tvnorms[] = {
 			/* sqwidth */ 780,
 			/* vdelay */ 0x1a,
 			/* sheight */ 480,
+			/* extraheight */ 0,
 			/* videostart0 */ 23)
 	},{
 		.v4l2_id        = V4L2_STD_SECAM,
@@ -355,6 +372,7 @@ const struct bttv_tvnorm bttv_tvnorms[] = {
 			/* sqwidth */ 944,
 			/* vdelay */ 0x20,
 			/* sheight */ 576,
+			/* extraheight */ 0,
 			/* videostart0 */ 23)
 	},{
 		.v4l2_id        = V4L2_STD_PAL_Nc,
@@ -380,6 +398,7 @@ const struct bttv_tvnorm bttv_tvnorms[] = {
 			/* sqwidth */ 780,
 			/* vdelay */ 0x1a,
 			/* sheight */ 576,
+			/* extraheight */ 0,
 			/* videostart0 */ 23)
 	},{
 		.v4l2_id        = V4L2_STD_PAL_M,
@@ -405,6 +424,7 @@ const struct bttv_tvnorm bttv_tvnorms[] = {
 			/* sqwidth */ 780,
 			/* vdelay */ 0x1a,
 			/* sheight */ 480,
+			/* extraheight */ 0,
 			/* videostart0 */ 23)
 	},{
 		.v4l2_id        = V4L2_STD_PAL_N,
@@ -430,6 +450,7 @@ const struct bttv_tvnorm bttv_tvnorms[] = {
 			/* sqwidth */ 944,
 			/* vdelay */ 0x20,
 			/* sheight */ 576,
+			/* extraheight */ 0,
 			/* videostart0 */ 23)
 	},{
 		.v4l2_id        = V4L2_STD_NTSC_M_JP,
@@ -455,6 +476,7 @@ const struct bttv_tvnorm bttv_tvnorms[] = {
 			/* sqwidth */ 780,
 			/* vdelay */ 0x16,
 			/* sheight */ 480,
+			/* extraheight */ 0,
 			/* videostart0 */ 23)
 	},{
 		/* that one hopefully works with the strange timing
@@ -484,6 +506,7 @@ const struct bttv_tvnorm bttv_tvnorms[] = {
 			/* sqwidth */ 944,
 			/* vdelay */ 0x1a,
 			/* sheight */ 480,
+			/* extraheight */ 0,
 			/* videostart0 */ 23)
 	}
 };
@@ -622,198 +645,6 @@ static const struct bttv_format formats[] = {
 static const unsigned int FORMATS = ARRAY_SIZE(formats);
 
 /* ----------------------------------------------------------------------- */
-
-#define V4L2_CID_PRIVATE_CHROMA_AGC  (V4L2_CID_PRIVATE_BASE + 0)
-#define V4L2_CID_PRIVATE_COMBFILTER  (V4L2_CID_PRIVATE_BASE + 1)
-#define V4L2_CID_PRIVATE_AUTOMUTE    (V4L2_CID_PRIVATE_BASE + 2)
-#define V4L2_CID_PRIVATE_LUMAFILTER  (V4L2_CID_PRIVATE_BASE + 3)
-#define V4L2_CID_PRIVATE_AGC_CRUSH   (V4L2_CID_PRIVATE_BASE + 4)
-#define V4L2_CID_PRIVATE_VCR_HACK    (V4L2_CID_PRIVATE_BASE + 5)
-#define V4L2_CID_PRIVATE_WHITECRUSH_UPPER   (V4L2_CID_PRIVATE_BASE + 6)
-#define V4L2_CID_PRIVATE_WHITECRUSH_LOWER   (V4L2_CID_PRIVATE_BASE + 7)
-#define V4L2_CID_PRIVATE_UV_RATIO    (V4L2_CID_PRIVATE_BASE + 8)
-#define V4L2_CID_PRIVATE_FULL_LUMA_RANGE    (V4L2_CID_PRIVATE_BASE + 9)
-#define V4L2_CID_PRIVATE_CORING      (V4L2_CID_PRIVATE_BASE + 10)
-#define V4L2_CID_PRIVATE_LASTP1      (V4L2_CID_PRIVATE_BASE + 11)
-
-static const struct v4l2_queryctrl no_ctl = {
-	.name  = "42",
-	.flags = V4L2_CTRL_FLAG_DISABLED,
-};
-static const struct v4l2_queryctrl bttv_ctls[] = {
-	/* --- video --- */
-	{
-		.id            = V4L2_CID_BRIGHTNESS,
-		.name          = "Brightness",
-		.minimum       = 0,
-		.maximum       = 65535,
-		.step          = 256,
-		.default_value = 32768,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},{
-		.id            = V4L2_CID_CONTRAST,
-		.name          = "Contrast",
-		.minimum       = 0,
-		.maximum       = 65535,
-		.step          = 128,
-		.default_value = 27648,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},{
-		.id            = V4L2_CID_SATURATION,
-		.name          = "Saturation",
-		.minimum       = 0,
-		.maximum       = 65535,
-		.step          = 128,
-		.default_value = 32768,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},{
-		.id            = V4L2_CID_COLOR_KILLER,
-		.name          = "Color killer",
-		.minimum       = 0,
-		.maximum       = 1,
-		.type          = V4L2_CTRL_TYPE_BOOLEAN,
-	}, {
-		.id            = V4L2_CID_HUE,
-		.name          = "Hue",
-		.minimum       = 0,
-		.maximum       = 65535,
-		.step          = 256,
-		.default_value = 32768,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},
-	/* --- audio --- */
-	{
-		.id            = V4L2_CID_AUDIO_MUTE,
-		.name          = "Mute",
-		.minimum       = 0,
-		.maximum       = 1,
-		.type          = V4L2_CTRL_TYPE_BOOLEAN,
-	},{
-		.id            = V4L2_CID_AUDIO_VOLUME,
-		.name          = "Volume",
-		.minimum       = 0,
-		.maximum       = 65535,
-		.step          = 65535/100,
-		.default_value = 65535,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},{
-		.id            = V4L2_CID_AUDIO_BALANCE,
-		.name          = "Balance",
-		.minimum       = 0,
-		.maximum       = 65535,
-		.step          = 65535/100,
-		.default_value = 32768,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},{
-		.id            = V4L2_CID_AUDIO_BASS,
-		.name          = "Bass",
-		.minimum       = 0,
-		.maximum       = 65535,
-		.step          = 65535/100,
-		.default_value = 32768,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},{
-		.id            = V4L2_CID_AUDIO_TREBLE,
-		.name          = "Treble",
-		.minimum       = 0,
-		.maximum       = 65535,
-		.step          = 65535/100,
-		.default_value = 32768,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},
-	/* --- private --- */
-	{
-		.id            = V4L2_CID_PRIVATE_CHROMA_AGC,
-		.name          = "chroma agc",
-		.minimum       = 0,
-		.maximum       = 1,
-		.type          = V4L2_CTRL_TYPE_BOOLEAN,
-	},{
-		.id            = V4L2_CID_PRIVATE_COMBFILTER,
-		.name          = "combfilter",
-		.minimum       = 0,
-		.maximum       = 1,
-		.type          = V4L2_CTRL_TYPE_BOOLEAN,
-	},{
-		.id            = V4L2_CID_PRIVATE_AUTOMUTE,
-		.name          = "automute",
-		.minimum       = 0,
-		.maximum       = 1,
-		.type          = V4L2_CTRL_TYPE_BOOLEAN,
-	},{
-		.id            = V4L2_CID_PRIVATE_LUMAFILTER,
-		.name          = "luma decimation filter",
-		.minimum       = 0,
-		.maximum       = 1,
-		.type          = V4L2_CTRL_TYPE_BOOLEAN,
-	},{
-		.id            = V4L2_CID_PRIVATE_AGC_CRUSH,
-		.name          = "agc crush",
-		.minimum       = 0,
-		.maximum       = 1,
-		.type          = V4L2_CTRL_TYPE_BOOLEAN,
-	},{
-		.id            = V4L2_CID_PRIVATE_VCR_HACK,
-		.name          = "vcr hack",
-		.minimum       = 0,
-		.maximum       = 1,
-		.type          = V4L2_CTRL_TYPE_BOOLEAN,
-	},{
-		.id            = V4L2_CID_PRIVATE_WHITECRUSH_UPPER,
-		.name          = "whitecrush upper",
-		.minimum       = 0,
-		.maximum       = 255,
-		.step          = 1,
-		.default_value = 0xCF,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},{
-		.id            = V4L2_CID_PRIVATE_WHITECRUSH_LOWER,
-		.name          = "whitecrush lower",
-		.minimum       = 0,
-		.maximum       = 255,
-		.step          = 1,
-		.default_value = 0x7F,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},{
-		.id            = V4L2_CID_PRIVATE_UV_RATIO,
-		.name          = "uv ratio",
-		.minimum       = 0,
-		.maximum       = 100,
-		.step          = 1,
-		.default_value = 50,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},{
-		.id            = V4L2_CID_PRIVATE_FULL_LUMA_RANGE,
-		.name          = "full luma range",
-		.minimum       = 0,
-		.maximum       = 1,
-		.type          = V4L2_CTRL_TYPE_BOOLEAN,
-	},{
-		.id            = V4L2_CID_PRIVATE_CORING,
-		.name          = "coring",
-		.minimum       = 0,
-		.maximum       = 3,
-		.step          = 1,
-		.default_value = 0,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	}
-
-
-
-};
-
-static const struct v4l2_queryctrl *ctrl_by_id(int id)
-{
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(bttv_ctls); i++)
-		if (bttv_ctls[i].id == id)
-			return bttv_ctls+i;
-
-	return NULL;
-}
-
-/* ----------------------------------------------------------------------- */
 /* resource management                                                     */
 
 /*
@@ -1168,23 +999,20 @@ static char *audio_modes[] = {
 	"audio: intern", "audio: mute"
 };
 
-static int
-audio_mux(struct bttv *btv, int input, int mute)
+static void
+audio_mux_gpio(struct bttv *btv, int input, int mute)
 {
-	int gpio_val, signal;
-	struct v4l2_control ctrl;
+	int gpio_val, signal, mute_gpio;
 
 	gpio_inout(bttv_tvcards[btv->c.type].gpiomask,
 		   bttv_tvcards[btv->c.type].gpiomask);
 	signal = btread(BT848_DSTATUS) & BT848_DSTATUS_HLOC;
 
-	btv->mute = mute;
-	btv->audio = input;
-
 	/* automute */
-	mute = mute || (btv->opt_automute && !signal && !btv->radio_user);
+	mute_gpio = mute || (btv->opt_automute && (!signal || !btv->users)
+				&& !btv->has_radio_tuner);
 
-	if (mute)
+	if (mute_gpio)
 		gpio_val = bttv_tvcards[btv->c.type].gpiomute;
 	else
 		gpio_val = bttv_tvcards[btv->c.type].gpiomux[input];
@@ -1200,13 +1028,39 @@ audio_mux(struct bttv *btv, int input, int mute)
 	}
 
 	if (bttv_gpio)
-		bttv_gpio_tracking(btv, audio_modes[mute ? 4 : input]);
-	if (in_interrupt())
-		return 0;
+		bttv_gpio_tracking(btv, audio_modes[mute_gpio ? 4 : input]);
+}
+
+static int
+audio_mute(struct bttv *btv, int mute)
+{
+	struct v4l2_ctrl *ctrl;
+
+	audio_mux_gpio(btv, btv->audio_input, mute);
+
+	if (btv->sd_msp34xx) {
+		ctrl = v4l2_ctrl_find(btv->sd_msp34xx->ctrl_handler, V4L2_CID_AUDIO_MUTE);
+		if (ctrl)
+			v4l2_ctrl_s_ctrl(ctrl, mute);
+	}
+	if (btv->sd_tvaudio) {
+		ctrl = v4l2_ctrl_find(btv->sd_tvaudio->ctrl_handler, V4L2_CID_AUDIO_MUTE);
+		if (ctrl)
+			v4l2_ctrl_s_ctrl(ctrl, mute);
+	}
+	if (btv->sd_tda7432) {
+		ctrl = v4l2_ctrl_find(btv->sd_tda7432->ctrl_handler, V4L2_CID_AUDIO_MUTE);
+		if (ctrl)
+			v4l2_ctrl_s_ctrl(ctrl, mute);
+	}
+	return 0;
+}
+
+static int
+audio_input(struct bttv *btv, int input)
+{
+	audio_mux_gpio(btv, input, btv->mute);
 
-	ctrl.id = V4L2_CID_AUDIO_MUTE;
-	ctrl.value = btv->mute;
-	bttv_call_all(btv, core, s_ctrl, &ctrl);
 	if (btv->sd_msp34xx) {
 		u32 in;
 
@@ -1255,23 +1109,11 @@ audio_mux(struct bttv *btv, int input, int mute)
 	}
 	if (btv->sd_tvaudio) {
 		v4l2_subdev_call(btv->sd_tvaudio, audio, s_routing,
-				input, 0, 0);
+				 input, 0, 0);
 	}
 	return 0;
 }
 
-static inline int
-audio_mute(struct bttv *btv, int mute)
-{
-	return audio_mux(btv, btv->audio, mute);
-}
-
-static inline int
-audio_input(struct bttv *btv, int input)
-{
-	return audio_mux(btv, input, btv->mute);
-}
-
 static void
 bttv_crop_calc_limits(struct bttv_crop *c)
 {
@@ -1365,8 +1207,9 @@ set_input(struct bttv *btv, unsigned int input, unsigned int norm)
 	} else {
 		video_mux(btv,input);
 	}
-	audio_input(btv, (btv->tuner_type != TUNER_ABSENT && input == 0) ?
-			 TVAUDIO_INPUT_TUNER : TVAUDIO_INPUT_EXTERN);
+	btv->audio_input = (btv->tuner_type != TUNER_ABSENT && input == 0) ?
+				TVAUDIO_INPUT_TUNER : TVAUDIO_INPUT_EXTERN;
+	audio_input(btv, btv->audio_input);
 	set_tvnorm(btv, norm);
 }
 
@@ -1394,8 +1237,6 @@ static void init_irqreg(struct bttv *btv)
 
 static void init_bt848(struct bttv *btv)
 {
-	int val;
-
 	if (bttv_tvcards[btv->c.type].no_video) {
 		/* very basic init only */
 		init_irqreg(btv);
@@ -1415,30 +1256,10 @@ static void init_bt848(struct bttv *btv)
 		BT848_GPIO_DMA_CTL_GPINTI,
 		BT848_GPIO_DMA_CTL);
 
-	val = btv->opt_chroma_agc ? BT848_SCLOOP_CAGC : 0;
-	btwrite(val, BT848_E_SCLOOP);
-	btwrite(val, BT848_O_SCLOOP);
-
 	btwrite(0x20, BT848_E_VSCALE_HI);
 	btwrite(0x20, BT848_O_VSCALE_HI);
-	btwrite(BT848_ADC_RESERVED | (btv->opt_adc_crush ? BT848_ADC_CRUSH : 0),
-		BT848_ADC);
-
-	btwrite(whitecrush_upper, BT848_WC_UP);
-	btwrite(whitecrush_lower, BT848_WC_DOWN);
-
-	if (btv->opt_lumafilter) {
-		btwrite(0, BT848_E_CONTROL);
-		btwrite(0, BT848_O_CONTROL);
-	} else {
-		btwrite(BT848_CONTROL_LDEC, BT848_E_CONTROL);
-		btwrite(BT848_CONTROL_LDEC, BT848_O_CONTROL);
-	}
 
-	bt848_bright(btv,   btv->bright);
-	bt848_hue(btv,      btv->hue);
-	bt848_contrast(btv, btv->contrast);
-	bt848_sat(btv,      btv->saturation);
+	v4l2_ctrl_handler_setup(&btv->ctrl_handler);
 
 	/* interrupt */
 	init_irqreg(btv);
@@ -1460,103 +1281,26 @@ static void bttv_reinit_bt848(struct bttv *btv)
 	set_input(btv, btv->input, btv->tvnorm);
 }
 
-static int bttv_g_ctrl(struct file *file, void *priv,
-					struct v4l2_control *c)
-{
-	struct bttv_fh *fh = priv;
-	struct bttv *btv = fh->btv;
-
-	switch (c->id) {
-	case V4L2_CID_BRIGHTNESS:
-		c->value = btv->bright;
-		break;
-	case V4L2_CID_HUE:
-		c->value = btv->hue;
-		break;
-	case V4L2_CID_CONTRAST:
-		c->value = btv->contrast;
-		break;
-	case V4L2_CID_SATURATION:
-		c->value = btv->saturation;
-		break;
-	case V4L2_CID_COLOR_KILLER:
-		c->value = btv->opt_color_killer;
-		break;
-
-	case V4L2_CID_AUDIO_MUTE:
-	case V4L2_CID_AUDIO_VOLUME:
-	case V4L2_CID_AUDIO_BALANCE:
-	case V4L2_CID_AUDIO_BASS:
-	case V4L2_CID_AUDIO_TREBLE:
-		bttv_call_all(btv, core, g_ctrl, c);
-		break;
-
-	case V4L2_CID_PRIVATE_CHROMA_AGC:
-		c->value = btv->opt_chroma_agc;
-		break;
-	case V4L2_CID_PRIVATE_COMBFILTER:
-		c->value = btv->opt_combfilter;
-		break;
-	case V4L2_CID_PRIVATE_LUMAFILTER:
-		c->value = btv->opt_lumafilter;
-		break;
-	case V4L2_CID_PRIVATE_AUTOMUTE:
-		c->value = btv->opt_automute;
-		break;
-	case V4L2_CID_PRIVATE_AGC_CRUSH:
-		c->value = btv->opt_adc_crush;
-		break;
-	case V4L2_CID_PRIVATE_VCR_HACK:
-		c->value = btv->opt_vcr_hack;
-		break;
-	case V4L2_CID_PRIVATE_WHITECRUSH_UPPER:
-		c->value = btv->opt_whitecrush_upper;
-		break;
-	case V4L2_CID_PRIVATE_WHITECRUSH_LOWER:
-		c->value = btv->opt_whitecrush_lower;
-		break;
-	case V4L2_CID_PRIVATE_UV_RATIO:
-		c->value = btv->opt_uv_ratio;
-		break;
-	case V4L2_CID_PRIVATE_FULL_LUMA_RANGE:
-		c->value = btv->opt_full_luma_range;
-		break;
-	case V4L2_CID_PRIVATE_CORING:
-		c->value = btv->opt_coring;
-		break;
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static int bttv_s_ctrl(struct file *file, void *f,
-					struct v4l2_control *c)
+static int bttv_s_ctrl(struct v4l2_ctrl *c)
 {
-	int err;
-	struct bttv_fh *fh = f;
-	struct bttv *btv = fh->btv;
-
-	err = v4l2_prio_check(&btv->prio, fh->prio);
-	if (0 != err)
-		return err;
+	struct bttv *btv = container_of(c->handler, struct bttv, ctrl_handler);
+	int val;
 
 	switch (c->id) {
 	case V4L2_CID_BRIGHTNESS:
-		bt848_bright(btv, c->value);
+		bt848_bright(btv, c->val);
 		break;
 	case V4L2_CID_HUE:
-		bt848_hue(btv, c->value);
+		bt848_hue(btv, c->val);
 		break;
 	case V4L2_CID_CONTRAST:
-		bt848_contrast(btv, c->value);
+		bt848_contrast(btv, c->val);
 		break;
 	case V4L2_CID_SATURATION:
-		bt848_sat(btv, c->value);
+		bt848_sat(btv, c->val);
 		break;
 	case V4L2_CID_COLOR_KILLER:
-		btv->opt_color_killer = c->value;
-		if (btv->opt_color_killer) {
+		if (c->val) {
 			btor(BT848_SCLOOP_CKILL, BT848_E_SCLOOP);
 			btor(BT848_SCLOOP_CKILL, BT848_O_SCLOOP);
 		} else {
@@ -1565,36 +1309,23 @@ static int bttv_s_ctrl(struct file *file, void *f,
 		}
 		break;
 	case V4L2_CID_AUDIO_MUTE:
-		audio_mute(btv, c->value);
-		/* fall through */
-	case V4L2_CID_AUDIO_VOLUME:
-		if (btv->volume_gpio)
-			btv->volume_gpio(btv, c->value);
-
-		bttv_call_all(btv, core, s_ctrl, c);
+		audio_mute(btv, c->val);
+		btv->mute = c->val;
 		break;
-	case V4L2_CID_AUDIO_BALANCE:
-	case V4L2_CID_AUDIO_BASS:
-	case V4L2_CID_AUDIO_TREBLE:
-		bttv_call_all(btv, core, s_ctrl, c);
+	case V4L2_CID_AUDIO_VOLUME:
+		btv->volume_gpio(btv, c->val);
 		break;
 
-	case V4L2_CID_PRIVATE_CHROMA_AGC:
-		btv->opt_chroma_agc = c->value;
-		if (btv->opt_chroma_agc) {
-			btor(BT848_SCLOOP_CAGC, BT848_E_SCLOOP);
-			btor(BT848_SCLOOP_CAGC, BT848_O_SCLOOP);
-		} else {
-			btand(~BT848_SCLOOP_CAGC, BT848_E_SCLOOP);
-			btand(~BT848_SCLOOP_CAGC, BT848_O_SCLOOP);
-		}
+	case V4L2_CID_CHROMA_AGC:
+		val = c->val ? BT848_SCLOOP_CAGC : 0;
+		btwrite(val, BT848_E_SCLOOP);
+		btwrite(val, BT848_O_SCLOOP);
 		break;
 	case V4L2_CID_PRIVATE_COMBFILTER:
-		btv->opt_combfilter = c->value;
+		btv->opt_combfilter = c->val;
 		break;
 	case V4L2_CID_PRIVATE_LUMAFILTER:
-		btv->opt_lumafilter = c->value;
-		if (btv->opt_lumafilter) {
+		if (c->val) {
 			btand(~BT848_CONTROL_LDEC, BT848_E_CONTROL);
 			btand(~BT848_CONTROL_LDEC, BT848_O_CONTROL);
 		} else {
@@ -1603,36 +1334,31 @@ static int bttv_s_ctrl(struct file *file, void *f,
 		}
 		break;
 	case V4L2_CID_PRIVATE_AUTOMUTE:
-		btv->opt_automute = c->value;
+		btv->opt_automute = c->val;
 		break;
 	case V4L2_CID_PRIVATE_AGC_CRUSH:
-		btv->opt_adc_crush = c->value;
 		btwrite(BT848_ADC_RESERVED |
-				(btv->opt_adc_crush ? BT848_ADC_CRUSH : 0),
+				(c->val ? BT848_ADC_CRUSH : 0),
 				BT848_ADC);
 		break;
 	case V4L2_CID_PRIVATE_VCR_HACK:
-		btv->opt_vcr_hack = c->value;
+		btv->opt_vcr_hack = c->val;
 		break;
 	case V4L2_CID_PRIVATE_WHITECRUSH_UPPER:
-		btv->opt_whitecrush_upper = c->value;
-		btwrite(c->value, BT848_WC_UP);
+		btwrite(c->val, BT848_WC_UP);
 		break;
 	case V4L2_CID_PRIVATE_WHITECRUSH_LOWER:
-		btv->opt_whitecrush_lower = c->value;
-		btwrite(c->value, BT848_WC_DOWN);
+		btwrite(c->val, BT848_WC_DOWN);
 		break;
 	case V4L2_CID_PRIVATE_UV_RATIO:
-		btv->opt_uv_ratio = c->value;
+		btv->opt_uv_ratio = c->val;
 		bt848_sat(btv, btv->saturation);
 		break;
 	case V4L2_CID_PRIVATE_FULL_LUMA_RANGE:
-		btv->opt_full_luma_range = c->value;
-		btaor((c->value<<7), ~BT848_OFORM_RANGE, BT848_OFORM);
+		btaor((c->val << 7), ~BT848_OFORM_RANGE, BT848_OFORM);
 		break;
 	case V4L2_CID_PRIVATE_CORING:
-		btv->opt_coring = c->value;
-		btaor((c->value<<5), ~BT848_OFORM_CORE32, BT848_OFORM);
+		btaor((c->val << 5), ~BT848_OFORM_CORE32, BT848_OFORM);
 		break;
 	default:
 		return -EINVAL;
@@ -1642,6 +1368,121 @@ static int bttv_s_ctrl(struct file *file, void *f,
 
 /* ----------------------------------------------------------------------- */
 
+static const struct v4l2_ctrl_ops bttv_ctrl_ops = {
+	.s_ctrl = bttv_s_ctrl,
+};
+
+static struct v4l2_ctrl_config bttv_ctrl_combfilter = {
+	.ops = &bttv_ctrl_ops,
+	.id = V4L2_CID_PRIVATE_COMBFILTER,
+	.name = "Comb Filter",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.min = 0,
+	.max = 1,
+	.step = 1,
+	.def = 1,
+};
+
+static struct v4l2_ctrl_config bttv_ctrl_automute = {
+	.ops = &bttv_ctrl_ops,
+	.id = V4L2_CID_PRIVATE_AUTOMUTE,
+	.name = "Auto Mute",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.min = 0,
+	.max = 1,
+	.step = 1,
+	.def = 1,
+};
+
+static struct v4l2_ctrl_config bttv_ctrl_lumafilter = {
+	.ops = &bttv_ctrl_ops,
+	.id = V4L2_CID_PRIVATE_LUMAFILTER,
+	.name = "Luma Decimation Filter",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.min = 0,
+	.max = 1,
+	.step = 1,
+	.def = 1,
+};
+
+static struct v4l2_ctrl_config bttv_ctrl_agc_crush = {
+	.ops = &bttv_ctrl_ops,
+	.id = V4L2_CID_PRIVATE_AGC_CRUSH,
+	.name = "AGC Crush",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.min = 0,
+	.max = 1,
+	.step = 1,
+	.def = 1,
+};
+
+static struct v4l2_ctrl_config bttv_ctrl_vcr_hack = {
+	.ops = &bttv_ctrl_ops,
+	.id = V4L2_CID_PRIVATE_VCR_HACK,
+	.name = "VCR Hack",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.min = 0,
+	.max = 1,
+	.step = 1,
+	.def = 1,
+};
+
+static struct v4l2_ctrl_config bttv_ctrl_whitecrush_lower = {
+	.ops = &bttv_ctrl_ops,
+	.id = V4L2_CID_PRIVATE_WHITECRUSH_LOWER,
+	.name = "Whitecrush Lower",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 0,
+	.max = 255,
+	.step = 1,
+	.def = 0x7f,
+};
+
+static struct v4l2_ctrl_config bttv_ctrl_whitecrush_upper = {
+	.ops = &bttv_ctrl_ops,
+	.id = V4L2_CID_PRIVATE_WHITECRUSH_UPPER,
+	.name = "Whitecrush Upper",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 0,
+	.max = 255,
+	.step = 1,
+	.def = 0xcf,
+};
+
+static struct v4l2_ctrl_config bttv_ctrl_uv_ratio = {
+	.ops = &bttv_ctrl_ops,
+	.id = V4L2_CID_PRIVATE_UV_RATIO,
+	.name = "UV Ratio",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 0,
+	.max = 100,
+	.step = 1,
+	.def = 50,
+};
+
+static struct v4l2_ctrl_config bttv_ctrl_full_luma = {
+	.ops = &bttv_ctrl_ops,
+	.id = V4L2_CID_PRIVATE_FULL_LUMA_RANGE,
+	.name = "Full Luma Range",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.min = 0,
+	.max = 1,
+	.step = 1,
+};
+
+static struct v4l2_ctrl_config bttv_ctrl_coring = {
+	.ops = &bttv_ctrl_ops,
+	.id = V4L2_CID_PRIVATE_CORING,
+	.name = "Coring",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 0,
+	.max = 3,
+	.step = 1,
+};
+
+
+/* ----------------------------------------------------------------------- */
+
 void bttv_gpio_tracking(struct bttv *btv, char *comment)
 {
 	unsigned int outbits, data;
@@ -1871,25 +1712,33 @@ static struct videobuf_queue_ops bttv_video_qops = {
 	.buf_release  = buffer_release,
 };
 
-static int bttv_s_std(struct file *file, void *priv, v4l2_std_id *id)
+static void radio_enable(struct bttv *btv)
+{
+	/* Switch to the radio tuner */
+	if (!btv->has_radio_tuner) {
+		btv->has_radio_tuner = 1;
+		bttv_call_all(btv, tuner, s_radio);
+		btv->audio_input = TVAUDIO_INPUT_RADIO;
+		audio_input(btv, btv->audio_input);
+	}
+}
+
+static int bttv_s_std(struct file *file, void *priv, v4l2_std_id id)
 {
 	struct bttv_fh *fh  = priv;
 	struct bttv *btv = fh->btv;
 	unsigned int i;
-	int err;
-
-	err = v4l2_prio_check(&btv->prio, fh->prio);
-	if (err)
-		goto err;
+	int err = 0;
 
 	for (i = 0; i < BTTV_TVNORMS; i++)
-		if (*id & bttv_tvnorms[i].v4l2_id)
+		if (id & bttv_tvnorms[i].v4l2_id)
 			break;
 	if (i == BTTV_TVNORMS) {
 		err = -EINVAL;
 		goto err;
 	}
 
+	btv->std = id;
 	set_tvnorm(btv, i);
 
 err:
@@ -1897,6 +1746,15 @@ err:
 	return err;
 }
 
+static int bttv_g_std(struct file *file, void *priv, v4l2_std_id *id)
+{
+	struct bttv_fh *fh  = priv;
+	struct bttv *btv = fh->btv;
+
+	*id = btv->std;
+	return 0;
+}
+
 static int bttv_querystd(struct file *file, void *f, v4l2_std_id *id)
 {
 	struct bttv_fh *fh = f;
@@ -1922,7 +1780,7 @@ static int bttv_enum_input(struct file *file, void *priv,
 	}
 
 	i->type     = V4L2_INPUT_TYPE_CAMERA;
-	i->audioset = 1;
+	i->audioset = 0;
 
 	if (btv->tuner_type != TUNER_ABSENT && i->index == 0) {
 		sprintf(i->name, "Television");
@@ -1964,49 +1822,29 @@ static int bttv_s_input(struct file *file, void *priv, unsigned int i)
 	struct bttv_fh *fh  = priv;
 	struct bttv *btv = fh->btv;
 
-	int err;
-
-	err = v4l2_prio_check(&btv->prio, fh->prio);
-	if (unlikely(err))
-		goto err;
-
-	if (i > bttv_tvcards[btv->c.type].video_inputs) {
-		err = -EINVAL;
-		goto err;
-	}
+	if (i >= bttv_tvcards[btv->c.type].video_inputs)
+		return -EINVAL;
 
 	set_input(btv, i, btv->tvnorm);
-
-err:
 	return 0;
 }
 
 static int bttv_s_tuner(struct file *file, void *priv,
-					struct v4l2_tuner *t)
+					const struct v4l2_tuner *t)
 {
 	struct bttv_fh *fh  = priv;
 	struct bttv *btv = fh->btv;
-	int err;
 
-	if (unlikely(0 != t->index))
+	if (t->index)
 		return -EINVAL;
 
-	if (unlikely(btv->tuner_type == TUNER_ABSENT)) {
-		err = -EINVAL;
-		goto err;
-	}
-
-	err = v4l2_prio_check(&btv->prio, fh->prio);
-	if (unlikely(err))
-		goto err;
-
 	bttv_call_all(btv, tuner, s_tuner, t);
 
-	if (btv->audio_mode_gpio)
-		btv->audio_mode_gpio(btv, t, 1);
-
-err:
+	if (btv->audio_mode_gpio) {
+		struct v4l2_tuner copy = *t;
 
+		btv->audio_mode_gpio(btv, &copy, 1);
+	}
 	return 0;
 }
 
@@ -2016,49 +1854,81 @@ static int bttv_g_frequency(struct file *file, void *priv,
 	struct bttv_fh *fh  = priv;
 	struct bttv *btv = fh->btv;
 
-	f->type = btv->radio_user ? V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
-	f->frequency = btv->freq;
+	if (f->tuner)
+		return -EINVAL;
+
+	if (f->type == V4L2_TUNER_RADIO)
+		radio_enable(btv);
+	f->frequency = f->type == V4L2_TUNER_RADIO ?
+				btv->radio_freq : btv->tv_freq;
 
 	return 0;
 }
 
+static void bttv_set_frequency(struct bttv *btv, const struct v4l2_frequency *f)
+{
+	struct v4l2_frequency new_freq = *f;
+
+	bttv_call_all(btv, tuner, s_frequency, f);
+	/* s_frequency may clamp the frequency, so get the actual
+	   frequency before assigning radio/tv_freq. */
+	bttv_call_all(btv, tuner, g_frequency, &new_freq);
+	if (new_freq.type == V4L2_TUNER_RADIO) {
+		radio_enable(btv);
+		btv->radio_freq = new_freq.frequency;
+		if (btv->has_matchbox)
+			tea5757_set_freq(btv, btv->radio_freq);
+	} else {
+		btv->tv_freq = new_freq.frequency;
+	}
+}
+
 static int bttv_s_frequency(struct file *file, void *priv,
-					struct v4l2_frequency *f)
+					const struct v4l2_frequency *f)
 {
 	struct bttv_fh *fh  = priv;
 	struct bttv *btv = fh->btv;
-	int err;
 
-	if (unlikely(f->tuner != 0))
+	if (f->tuner)
 		return -EINVAL;
 
-	err = v4l2_prio_check(&btv->prio, fh->prio);
-	if (unlikely(err))
-		goto err;
-
-	if (unlikely(f->type != (btv->radio_user
-		? V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV))) {
-		err = -EINVAL;
-		goto err;
-	}
-	btv->freq = f->frequency;
-	bttv_call_all(btv, tuner, s_frequency, f);
-	if (btv->has_matchbox && btv->radio_user)
-		tea5757_set_freq(btv, btv->freq);
-err:
-
+	bttv_set_frequency(btv, f);
 	return 0;
 }
 
 static int bttv_log_status(struct file *file, void *f)
 {
+	struct video_device *vdev = video_devdata(file);
 	struct bttv_fh *fh  = f;
 	struct bttv *btv = fh->btv;
 
+	v4l2_ctrl_handler_log_status(vdev->ctrl_handler, btv->c.v4l2_dev.name);
 	bttv_call_all(btv, core, log_status);
 	return 0;
 }
 
+static int bttv_g_chip_ident(struct file *file, void *f, struct v4l2_dbg_chip_ident *chip)
+{
+	struct bttv_fh *fh  = f;
+	struct bttv *btv = fh->btv;
+
+	chip->ident = V4L2_IDENT_NONE;
+	chip->revision = 0;
+	if (chip->match.type == V4L2_CHIP_MATCH_HOST) {
+		if (v4l2_chip_match_host(&chip->match)) {
+			chip->ident = btv->id;
+			if (chip->ident == PCI_DEVICE_ID_FUSION879)
+				chip->ident = V4L2_IDENT_BT879;
+		}
+		return 0;
+	}
+	if (chip->match.type != V4L2_CHIP_MATCH_I2C_DRIVER &&
+	    chip->match.type != V4L2_CHIP_MATCH_I2C_ADDR)
+		return -EINVAL;
+	/* TODO: is this correct? */
+	return bttv_call_all_err(btv, core, g_chip_ident, chip);
+}
+
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 static int bttv_g_register(struct file *file, void *f,
 					struct v4l2_dbg_register *reg)
@@ -2069,8 +1939,12 @@ static int bttv_g_register(struct file *file, void *f,
 	if (!capable(CAP_SYS_ADMIN))
 		return -EPERM;
 
-	if (!v4l2_chip_match_host(&reg->match))
-		return -EINVAL;
+	if (!v4l2_chip_match_host(&reg->match)) {
+		/* TODO: subdev errors should not be ignored, this should become a
+		   subdev helper function. */
+		bttv_call_all(btv, core, g_register, reg);
+		return 0;
+	}
 
 	/* bt848 has a 12-bit register space */
 	reg->reg &= 0xfff;
@@ -2081,7 +1955,7 @@ static int bttv_g_register(struct file *file, void *f,
 }
 
 static int bttv_s_register(struct file *file, void *f,
-					struct v4l2_dbg_register *reg)
+					const struct v4l2_dbg_register *reg)
 {
 	struct bttv_fh *fh = f;
 	struct bttv *btv = fh->btv;
@@ -2089,12 +1963,15 @@ static int bttv_s_register(struct file *file, void *f,
 	if (!capable(CAP_SYS_ADMIN))
 		return -EPERM;
 
-	if (!v4l2_chip_match_host(&reg->match))
-		return -EINVAL;
+	if (!v4l2_chip_match_host(&reg->match)) {
+		/* TODO: subdev errors should not be ignored, this should become a
+		   subdev helper function. */
+		bttv_call_all(btv, core, s_register, reg);
+		return 0;
+	}
 
 	/* bt848 has a 12-bit register space */
-	reg->reg &= 0xfff;
-	btwrite(reg->val, reg->reg);
+	btwrite(reg->val, reg->reg & 0xfff);
 
 	return 0;
 }
@@ -2263,22 +2140,33 @@ limit_scaled_size_lock       (struct bttv_fh *               fh,
    may also adjust the current cropping parameters to get closer
    to the desired window size. */
 static int
-verify_window_lock		(struct bttv_fh *               fh,
-			 struct v4l2_window *           win,
-			 int                            adjust_size,
-			 int                            adjust_crop)
+verify_window_lock(struct bttv_fh *fh, struct v4l2_window *win,
+			 int adjust_size, int adjust_crop)
 {
 	enum v4l2_field field;
 	unsigned int width_mask;
 	int rc;
 
-	if (win->w.width  < 48 || win->w.height < 32)
-		return -EINVAL;
+	if (win->w.width < 48)
+		win->w.width = 48;
+	if (win->w.height < 32)
+		win->w.height = 32;
 	if (win->clipcount > 2048)
-		return -EINVAL;
+		win->clipcount = 2048;
 
+	win->chromakey = 0;
+	win->global_alpha = 0;
 	field = win->field;
 
+	switch (field) {
+	case V4L2_FIELD_TOP:
+	case V4L2_FIELD_BOTTOM:
+	case V4L2_FIELD_INTERLACED:
+		break;
+	default:
+		field = V4L2_FIELD_ANY;
+		break;
+	}
 	if (V4L2_FIELD_ANY == field) {
 		__s32 height2;
 
@@ -2287,18 +2175,11 @@ verify_window_lock		(struct bttv_fh *               fh,
 			? V4L2_FIELD_INTERLACED
 			: V4L2_FIELD_TOP;
 	}
-	switch (field) {
-	case V4L2_FIELD_TOP:
-	case V4L2_FIELD_BOTTOM:
-	case V4L2_FIELD_INTERLACED:
-		break;
-	default:
-		return -EINVAL;
-	}
+	win->field = field;
 
-	/* 4-byte alignment. */
 	if (NULL == fh->ovfmt)
 		return -EINVAL;
+	/* 4-byte alignment. */
 	width_mask = ~0;
 	switch (fh->ovfmt->depth) {
 	case 8:
@@ -2323,8 +2204,6 @@ verify_window_lock		(struct bttv_fh *               fh,
 			       adjust_size, adjust_crop);
 	if (0 != rc)
 		return rc;
-
-	win->field = field;
 	return 0;
 }
 
@@ -2481,6 +2360,7 @@ static int bttv_g_fmt_vid_cap(struct file *file, void *priv,
 				fh->width, fh->height);
 	f->fmt.pix.field        = fh->cap.field;
 	f->fmt.pix.pixelformat  = fh->fmt->fourcc;
+	f->fmt.pix.colorspace   = V4L2_COLORSPACE_SMPTE170M;
 
 	return 0;
 }
@@ -2504,6 +2384,7 @@ static int bttv_try_fmt_vid_cap(struct file *file, void *priv,
 	struct bttv *btv = fh->btv;
 	enum v4l2_field field;
 	__s32 width, height;
+	__s32 height2;
 	int rc;
 
 	fmt = format_by_fourcc(f->fmt.pix.pixelformat);
@@ -2512,30 +2393,25 @@ static int bttv_try_fmt_vid_cap(struct file *file, void *priv,
 
 	field = f->fmt.pix.field;
 
-	if (V4L2_FIELD_ANY == field) {
-		__s32 height2;
-
-		height2 = btv->crop[!!fh->do_crop].rect.height >> 1;
-		field = (f->fmt.pix.height > height2)
-			? V4L2_FIELD_INTERLACED
-			: V4L2_FIELD_BOTTOM;
-	}
-
-	if (V4L2_FIELD_SEQ_BT == field)
-		field = V4L2_FIELD_SEQ_TB;
-
 	switch (field) {
 	case V4L2_FIELD_TOP:
 	case V4L2_FIELD_BOTTOM:
 	case V4L2_FIELD_ALTERNATE:
 	case V4L2_FIELD_INTERLACED:
 		break;
+	case V4L2_FIELD_SEQ_BT:
 	case V4L2_FIELD_SEQ_TB:
-		if (fmt->flags & FORMAT_FLAGS_PLANAR)
-			return -EINVAL;
+		if (!(fmt->flags & FORMAT_FLAGS_PLANAR)) {
+			field = V4L2_FIELD_SEQ_TB;
+			break;
+		}
+		/* fall through */
+	default: /* FIELD_ANY case */
+		height2 = btv->crop[!!fh->do_crop].rect.height >> 1;
+		field = (f->fmt.pix.height > height2)
+			? V4L2_FIELD_INTERLACED
+			: V4L2_FIELD_BOTTOM;
 		break;
-	default:
-		return -EINVAL;
 	}
 
 	width = f->fmt.pix.width;
@@ -2552,6 +2428,7 @@ static int bttv_try_fmt_vid_cap(struct file *file, void *priv,
 	/* update data for the application */
 	f->fmt.pix.field = field;
 	pix_format_set_size(&f->fmt.pix, fmt, width, height);
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 
 	return 0;
 }
@@ -2561,9 +2438,10 @@ static int bttv_try_fmt_vid_overlay(struct file *file, void *priv,
 {
 	struct bttv_fh *fh = priv;
 
-	return verify_window_lock(fh, &f->fmt.win,
+	verify_window_lock(fh, &f->fmt.win,
 			/* adjust_size */ 1,
 			/* adjust_crop */ 0);
+	return 0;
 }
 
 static int bttv_s_fmt_vid_cap(struct file *file, void *priv,
@@ -2630,6 +2508,7 @@ static int bttv_s_fmt_vid_overlay(struct file *file, void *priv,
 static int bttv_querycap(struct file *file, void  *priv,
 				struct v4l2_capability *cap)
 {
+	struct video_device *vdev = video_devdata(file);
 	struct bttv_fh *fh = priv;
 	struct bttv *btv = fh->btv;
 
@@ -2642,11 +2521,15 @@ static int bttv_querycap(struct file *file, void  *priv,
 		 "PCI:%s", pci_name(btv->c.pci));
 	cap->capabilities =
 		V4L2_CAP_VIDEO_CAPTURE |
-		V4L2_CAP_VBI_CAPTURE |
 		V4L2_CAP_READWRITE |
-		V4L2_CAP_STREAMING;
+		V4L2_CAP_STREAMING |
+		V4L2_CAP_DEVICE_CAPS;
 	if (no_overlay <= 0)
 		cap->capabilities |= V4L2_CAP_VIDEO_OVERLAY;
+	if (btv->vbi_dev)
+		cap->capabilities |= V4L2_CAP_VBI_CAPTURE;
+	if (btv->radio_dev)
+		cap->capabilities |= V4L2_CAP_RADIO;
 
 	/*
 	 * No need to lock here: those vars are initialized during board
@@ -2656,6 +2539,25 @@ static int bttv_querycap(struct file *file, void  *priv,
 		cap->capabilities |= V4L2_CAP_RDS_CAPTURE;
 	if (btv->tuner_type != TUNER_ABSENT)
 		cap->capabilities |= V4L2_CAP_TUNER;
+	if (vdev->vfl_type == VFL_TYPE_GRABBER)
+		cap->device_caps = cap->capabilities &
+			(V4L2_CAP_VIDEO_CAPTURE |
+			 V4L2_CAP_READWRITE |
+			 V4L2_CAP_STREAMING |
+			 V4L2_CAP_VIDEO_OVERLAY |
+			 V4L2_CAP_TUNER);
+	else if (vdev->vfl_type == VFL_TYPE_VBI)
+		cap->device_caps = cap->capabilities &
+			(V4L2_CAP_VBI_CAPTURE |
+			 V4L2_CAP_READWRITE |
+			 V4L2_CAP_STREAMING |
+			 V4L2_CAP_TUNER);
+	else {
+		cap->device_caps = V4L2_CAP_RADIO | V4L2_CAP_TUNER;
+		if (btv->has_saa6588)
+			cap->device_caps |= V4L2_CAP_READWRITE |
+						V4L2_CAP_RDS_CAPTURE;
+	}
 	return 0;
 }
 
@@ -2718,6 +2620,7 @@ static int bttv_g_fbuf(struct file *file, void *f,
 
 	*fb = btv->fbuf;
 	fb->capability = V4L2_FBUF_CAP_LIST_CLIPPING;
+	fb->flags = V4L2_FBUF_FLAG_PRIMARY;
 	if (fh->ovfmt)
 		fb->fmt.pixelformat  = fh->ovfmt->fourcc;
 	return 0;
@@ -2891,36 +2794,15 @@ static int bttv_streamoff(struct file *file, void *priv,
 	return 0;
 }
 
-static int bttv_queryctrl(struct file *file, void *priv,
-					struct v4l2_queryctrl *c)
-{
-	struct bttv_fh *fh = priv;
-	struct bttv *btv = fh->btv;
-	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;
-
-	if (!btv->volume_gpio && (c->id == V4L2_CID_AUDIO_VOLUME))
-		*c = no_ctl;
-	else {
-		ctrl = ctrl_by_id(c->id);
-
-		*c = (NULL != ctrl) ? *ctrl : no_ctl;
-	}
-
-	return 0;
-}
-
 static int bttv_g_parm(struct file *file, void *f,
 				struct v4l2_streamparm *parm)
 {
 	struct bttv_fh *fh = f;
 	struct bttv *btv = fh->btv;
 
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	parm->parm.capture.readbuffers = gbuffers;
 	v4l2_video_std_frame_period(bttv_tvnorms[btv->tvnorm].v4l2_id,
 				    &parm->parm.capture.timeperframe);
 
@@ -2933,15 +2815,13 @@ static int bttv_g_tuner(struct file *file, void *priv,
 	struct bttv_fh *fh = priv;
 	struct bttv *btv = fh->btv;
 
-	if (btv->tuner_type == TUNER_ABSENT)
-		return -EINVAL;
 	if (0 != t->index)
 		return -EINVAL;
 
 	t->rxsubchans = V4L2_TUNER_SUB_MONO;
+	t->capability = V4L2_TUNER_CAP_NORM;
 	bttv_call_all(btv, tuner, g_tuner, t);
 	strcpy(t->name, "Television");
-	t->capability = V4L2_TUNER_CAP_NORM;
 	t->type       = V4L2_TUNER_ANALOG_TV;
 	if (btread(BT848_DSTATUS)&BT848_DSTATUS_HLOC)
 		t->signal = 0xffff;
@@ -2952,28 +2832,6 @@ static int bttv_g_tuner(struct file *file, void *priv,
 	return 0;
 }
 
-static int bttv_g_priority(struct file *file, void *f, enum v4l2_priority *p)
-{
-	struct bttv_fh *fh = f;
-	struct bttv *btv = fh->btv;
-
-	*p = v4l2_prio_max(&btv->prio);
-
-	return 0;
-}
-
-static int bttv_s_priority(struct file *file, void *f,
-					enum v4l2_priority prio)
-{
-	struct bttv_fh *fh = f;
-	struct bttv *btv = fh->btv;
-	int	rc;
-
-	rc = v4l2_prio_change(&btv->prio, &fh->prio, prio);
-
-	return rc;
-}
-
 static int bttv_cropcap(struct file *file, void *priv,
 				struct v4l2_cropcap *cap)
 {
@@ -3026,11 +2884,6 @@ static int bttv_s_crop(struct file *file, void *f, const struct v4l2_crop *crop)
 	/* Make sure tvnorm, vbi_end and the current cropping
 	   parameters remain consistent until we're done. Note
 	   read() may change vbi_end in check_alloc_btres_lock(). */
-	retval = v4l2_prio_check(&btv->prio, fh->prio);
-	if (0 != retval) {
-		return retval;
-	}
-
 	retval = -EBUSY;
 
 	if (locked_btres(fh->btv, VIDEO_RESOURCES)) {
@@ -3088,23 +2941,6 @@ static int bttv_s_crop(struct file *file, void *f, const struct v4l2_crop *crop)
 	return 0;
 }
 
-static int bttv_g_audio(struct file *file, void *priv, struct v4l2_audio *a)
-{
-	if (unlikely(a->index))
-		return -EINVAL;
-
-	strcpy(a->name, "audio");
-	return 0;
-}
-
-static int bttv_s_audio(struct file *file, void *priv, const struct v4l2_audio *a)
-{
-	if (unlikely(a->index))
-		return -EINVAL;
-
-	return 0;
-}
-
 static ssize_t bttv_read(struct file *file, char __user *data,
 			 size_t count, loff_t *ppos)
 {
@@ -3144,34 +2980,43 @@ static unsigned int bttv_poll(struct file *file, poll_table *wait)
 	struct bttv_fh *fh = file->private_data;
 	struct bttv_buffer *buf;
 	enum v4l2_field field;
-	unsigned int rc = POLLERR;
+	unsigned int rc = 0;
+	unsigned long req_events = poll_requested_events(wait);
+
+	if (v4l2_event_pending(&fh->fh))
+		rc = POLLPRI;
+	else if (req_events & POLLPRI)
+		poll_wait(file, &fh->fh.wait, wait);
+
+	if (!(req_events & (POLLIN | POLLRDNORM)))
+		return rc;
 
 	if (V4L2_BUF_TYPE_VBI_CAPTURE == fh->type) {
 		if (!check_alloc_btres_lock(fh->btv,fh,RESOURCE_VBI))
-			return POLLERR;
-		return videobuf_poll_stream(file, &fh->vbi, wait);
+			return rc | POLLERR;
+		return rc | videobuf_poll_stream(file, &fh->vbi, wait);
 	}
 
 	if (check_btres(fh,RESOURCE_VIDEO_STREAM)) {
 		/* streaming capture */
 		if (list_empty(&fh->cap.stream))
-			goto err;
+			return rc | POLLERR;
 		buf = list_entry(fh->cap.stream.next,struct bttv_buffer,vb.stream);
 	} else {
 		/* read() capture */
 		if (NULL == fh->cap.read_buf) {
 			/* need to capture a new frame */
 			if (locked_btres(fh->btv,RESOURCE_VIDEO_STREAM))
-				goto err;
+				return rc | POLLERR;
 			fh->cap.read_buf = videobuf_sg_alloc(fh->cap.msize);
 			if (NULL == fh->cap.read_buf)
-				goto err;
+				return rc | POLLERR;
 			fh->cap.read_buf->memory = V4L2_MEMORY_USERPTR;
 			field = videobuf_next_field(&fh->cap);
 			if (0 != fh->cap.ops->buf_prepare(&fh->cap,fh->cap.read_buf,field)) {
 				kfree (fh->cap.read_buf);
 				fh->cap.read_buf = NULL;
-				goto err;
+				return rc | POLLERR;
 			}
 			fh->cap.ops->buf_queue(&fh->cap,fh->cap.read_buf);
 			fh->cap.read_off = 0;
@@ -3182,10 +3027,7 @@ static unsigned int bttv_poll(struct file *file, poll_table *wait)
 	poll_wait(file, &buf->vb.done, wait);
 	if (buf->vb.state == VIDEOBUF_DONE ||
 	    buf->vb.state == VIDEOBUF_ERROR)
-		rc =  POLLIN|POLLRDNORM;
-	else
-		rc = 0;
-err:
+		rc = rc | POLLIN|POLLRDNORM;
 	return rc;
 }
 
@@ -3214,15 +3056,15 @@ static int bttv_open(struct file *file)
 	fh = kmalloc(sizeof(*fh), GFP_KERNEL);
 	if (unlikely(!fh))
 		return -ENOMEM;
+	btv->users++;
 	file->private_data = fh;
 
 	*fh = btv->init;
+	v4l2_fh_init(&fh->fh, vdev);
 
 	fh->type = type;
 	fh->ov.setup_ok = 0;
 
-	v4l2_prio_open(&btv->prio, &fh->prio);
-
 	videobuf_queue_sg_init(&fh->cap, &bttv_video_qops,
 			    &btv->c.pci->dev, &btv->s_lock,
 			    V4L2_BUF_TYPE_VIDEO_CAPTURE,
@@ -3237,8 +3079,7 @@ static int bttv_open(struct file *file)
 			    fh, &btv->lock);
 	set_tvnorm(btv,btv->tvnorm);
 	set_input(btv, btv->input, btv->tvnorm);
-
-	btv->users++;
+	audio_mute(btv, btv->mute);
 
 	/* The V4L2 spec requires one global set of cropping parameters
 	   which only change on request. These are stored in btv->crop[1].
@@ -3257,6 +3098,7 @@ static int bttv_open(struct file *file)
 	bttv_vbi_fmt_reset(&fh->vbi_fmt, btv->tvnorm);
 
 	bttv_field_count(btv);
+	v4l2_fh_add(&fh->fh);
 	return 0;
 }
 
@@ -3292,16 +3134,17 @@ static int bttv_release(struct file *file)
 
 	videobuf_mmap_free(&fh->cap);
 	videobuf_mmap_free(&fh->vbi);
-	v4l2_prio_close(&btv->prio, fh->prio);
 	file->private_data = NULL;
-	kfree(fh);
 
 	btv->users--;
 	bttv_field_count(btv);
 
 	if (!btv->users)
-		audio_mute(btv, 1);
+		audio_mute(btv, btv->mute);
 
+	v4l2_fh_del(&fh->fh);
+	v4l2_fh_exit(&fh->fh);
+	kfree(fh);
 	return 0;
 }
 
@@ -3340,20 +3183,16 @@ static const struct v4l2_ioctl_ops bttv_ioctl_ops = {
 	.vidioc_g_fmt_vbi_cap           = bttv_g_fmt_vbi_cap,
 	.vidioc_try_fmt_vbi_cap         = bttv_try_fmt_vbi_cap,
 	.vidioc_s_fmt_vbi_cap           = bttv_s_fmt_vbi_cap,
-	.vidioc_g_audio                 = bttv_g_audio,
-	.vidioc_s_audio                 = bttv_s_audio,
 	.vidioc_cropcap                 = bttv_cropcap,
 	.vidioc_reqbufs                 = bttv_reqbufs,
 	.vidioc_querybuf                = bttv_querybuf,
 	.vidioc_qbuf                    = bttv_qbuf,
 	.vidioc_dqbuf                   = bttv_dqbuf,
 	.vidioc_s_std                   = bttv_s_std,
+	.vidioc_g_std                   = bttv_g_std,
 	.vidioc_enum_input              = bttv_enum_input,
 	.vidioc_g_input                 = bttv_g_input,
 	.vidioc_s_input                 = bttv_s_input,
-	.vidioc_queryctrl               = bttv_queryctrl,
-	.vidioc_g_ctrl                  = bttv_g_ctrl,
-	.vidioc_s_ctrl                  = bttv_s_ctrl,
 	.vidioc_streamon                = bttv_streamon,
 	.vidioc_streamoff               = bttv_streamoff,
 	.vidioc_g_tuner                 = bttv_g_tuner,
@@ -3363,13 +3202,14 @@ static const struct v4l2_ioctl_ops bttv_ioctl_ops = {
 	.vidioc_g_fbuf                  = bttv_g_fbuf,
 	.vidioc_s_fbuf                  = bttv_s_fbuf,
 	.vidioc_overlay                 = bttv_overlay,
-	.vidioc_g_priority              = bttv_g_priority,
-	.vidioc_s_priority              = bttv_s_priority,
 	.vidioc_g_parm                  = bttv_g_parm,
 	.vidioc_g_frequency             = bttv_g_frequency,
 	.vidioc_s_frequency             = bttv_s_frequency,
 	.vidioc_log_status		= bttv_log_status,
 	.vidioc_querystd		= bttv_querystd,
+	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+	.vidioc_g_chip_ident		= bttv_g_chip_ident,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 	.vidioc_g_register		= bttv_g_register,
 	.vidioc_s_register		= bttv_s_register,
@@ -3380,7 +3220,6 @@ static struct video_device bttv_video_template = {
 	.fops         = &bttv_fops,
 	.ioctl_ops    = &bttv_ioctl_ops,
 	.tvnorms      = BTTV_NORMS,
-	.current_norm = V4L2_STD_PAL,
 };
 
 /* ----------------------------------------------------------------------- */
@@ -3402,13 +3241,12 @@ static int radio_open(struct file *file)
 		return -ENOMEM;
 	file->private_data = fh;
 	*fh = btv->init;
-
-	v4l2_prio_open(&btv->prio, &fh->prio);
+	v4l2_fh_init(&fh->fh, vdev);
 
 	btv->radio_user++;
+	audio_mute(btv, btv->mute);
 
-	bttv_call_all(btv, tuner, s_radio);
-	audio_input(btv,TVAUDIO_INPUT_RADIO);
+	v4l2_fh_add(&fh->fh);
 
 	return 0;
 }
@@ -3419,28 +3257,17 @@ static int radio_release(struct file *file)
 	struct bttv *btv = fh->btv;
 	struct saa6588_command cmd;
 
-	v4l2_prio_close(&btv->prio, fh->prio);
 	file->private_data = NULL;
+	v4l2_fh_del(&fh->fh);
+	v4l2_fh_exit(&fh->fh);
 	kfree(fh);
 
 	btv->radio_user--;
 
 	bttv_call_all(btv, core, ioctl, SAA6588_CMD_CLOSE, &cmd);
 
-	return 0;
-}
-
-static int radio_querycap(struct file *file, void *priv,
-					struct v4l2_capability *cap)
-{
-	struct bttv_fh *fh = priv;
-	struct bttv *btv = fh->btv;
-
-	strcpy(cap->driver, "bttv");
-	strlcpy(cap->card, btv->radio_dev->name, sizeof(cap->card));
-	sprintf(cap->bus_info, "PCI:%s", pci_name(btv->c.pci));
-	cap->capabilities = V4L2_CAP_TUNER;
-
+	if (btv->radio_user == 0)
+		btv->has_radio_tuner = 0;
 	return 0;
 }
 
@@ -3449,12 +3276,11 @@ static int radio_g_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
 	struct bttv_fh *fh = priv;
 	struct bttv *btv = fh->btv;
 
-	if (btv->tuner_type == TUNER_ABSENT)
-		return -EINVAL;
 	if (0 != t->index)
 		return -EINVAL;
 	strcpy(t->name, "Radio");
 	t->type = V4L2_TUNER_RADIO;
+	radio_enable(btv);
 
 	bttv_call_all(btv, tuner, g_tuner, t);
 
@@ -3464,31 +3290,8 @@ static int radio_g_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
 	return 0;
 }
 
-static int radio_enum_input(struct file *file, void *priv,
-				struct v4l2_input *i)
-{
-	if (i->index != 0)
-		return -EINVAL;
-
-	strcpy(i->name, "Radio");
-	i->type = V4L2_INPUT_TYPE_TUNER;
-
-	return 0;
-}
-
-static int radio_g_audio(struct file *file, void *priv,
-					struct v4l2_audio *a)
-{
-	if (unlikely(a->index))
-		return -EINVAL;
-
-	strcpy(a->name, "Radio");
-
-	return 0;
-}
-
 static int radio_s_tuner(struct file *file, void *priv,
-					struct v4l2_tuner *t)
+					const struct v4l2_tuner *t)
 {
 	struct bttv_fh *fh = priv;
 	struct bttv *btv = fh->btv;
@@ -3496,56 +3299,11 @@ static int radio_s_tuner(struct file *file, void *priv,
 	if (0 != t->index)
 		return -EINVAL;
 
+	radio_enable(btv);
 	bttv_call_all(btv, tuner, s_tuner, t);
 	return 0;
 }
 
-static int radio_s_audio(struct file *file, void *priv,
-					const struct v4l2_audio *a)
-{
-	if (unlikely(a->index))
-		return -EINVAL;
-
-	return 0;
-}
-
-static int radio_s_input(struct file *filp, void *priv, unsigned int i)
-{
-	if (unlikely(i))
-		return -EINVAL;
-
-	return 0;
-}
-
-static int radio_s_std(struct file *file, void *fh, v4l2_std_id *norm)
-{
-	return 0;
-}
-
-static int radio_queryctrl(struct file *file, void *priv,
-					struct v4l2_queryctrl *c)
-{
-	const struct v4l2_queryctrl *ctrl;
-
-	if (c->id <  V4L2_CID_BASE ||
-			c->id >= V4L2_CID_LASTP1)
-		return -EINVAL;
-
-	if (c->id == V4L2_CID_AUDIO_MUTE) {
-		ctrl = ctrl_by_id(c->id);
-		*c = *ctrl;
-	} else
-		*c = no_ctl;
-
-	return 0;
-}
-
-static int radio_g_input(struct file *filp, void *priv, unsigned int *i)
-{
-	*i = 0;
-	return 0;
-}
-
 static ssize_t radio_read(struct file *file, char __user *data,
 			 size_t count, loff_t *ppos)
 {
@@ -3556,6 +3314,7 @@ static ssize_t radio_read(struct file *file, char __user *data,
 	cmd.buffer = data;
 	cmd.instance = file;
 	cmd.result = -ENODEV;
+	radio_enable(btv);
 
 	bttv_call_all(btv, core, ioctl, SAA6588_CMD_READ, &cmd);
 
@@ -3566,10 +3325,18 @@ static unsigned int radio_poll(struct file *file, poll_table *wait)
 {
 	struct bttv_fh *fh = file->private_data;
 	struct bttv *btv = fh->btv;
+	unsigned long req_events = poll_requested_events(wait);
 	struct saa6588_command cmd;
+	unsigned int res = 0;
+
+	if (v4l2_event_pending(&fh->fh))
+		res = POLLPRI;
+	else if (req_events & POLLPRI)
+		poll_wait(file, &fh->fh.wait, wait);
+	radio_enable(btv);
 	cmd.instance = file;
 	cmd.event_list = wait;
-	cmd.result = -ENODEV;
+	cmd.result = res;
 	bttv_call_all(btv, core, ioctl, SAA6588_CMD_POLL, &cmd);
 
 	return cmd.result;
@@ -3586,20 +3353,14 @@ static const struct v4l2_file_operations radio_fops =
 };
 
 static const struct v4l2_ioctl_ops radio_ioctl_ops = {
-	.vidioc_querycap        = radio_querycap,
+	.vidioc_querycap        = bttv_querycap,
+	.vidioc_log_status	= bttv_log_status,
 	.vidioc_g_tuner         = radio_g_tuner,
-	.vidioc_enum_input      = radio_enum_input,
-	.vidioc_g_audio         = radio_g_audio,
 	.vidioc_s_tuner         = radio_s_tuner,
-	.vidioc_s_audio         = radio_s_audio,
-	.vidioc_s_input         = radio_s_input,
-	.vidioc_s_std           = radio_s_std,
-	.vidioc_queryctrl       = radio_queryctrl,
-	.vidioc_g_input         = radio_g_input,
-	.vidioc_g_ctrl          = bttv_g_ctrl,
-	.vidioc_s_ctrl          = bttv_s_ctrl,
 	.vidioc_g_frequency     = bttv_g_frequency,
 	.vidioc_s_frequency     = bttv_s_frequency,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 static struct video_device radio_template = {
@@ -4102,7 +3863,8 @@ static irqreturn_t bttv_irq(int irq, void *dev_id)
 			bttv_irq_switch_video(btv);
 
 		if ((astat & BT848_INT_HLOCK)  &&  btv->opt_automute)
-			audio_mute(btv, btv->mute);  /* trigger automute */
+			/* trigger automute */
+			audio_mux_gpio(btv, btv->audio_input, btv->mute);
 
 		if (astat & (BT848_INT_SCERR|BT848_INT_OCERR)) {
 			pr_info("%d: %s%s @ %08x,",
@@ -4151,7 +3913,7 @@ static irqreturn_t bttv_irq(int irq, void *dev_id)
 
 
 /* ----------------------------------------------------------------------- */
-/* initialitation                                                          */
+/* initialization                                                          */
 
 static struct video_device *vdev_init(struct bttv *btv,
 				      const struct video_device *template,
@@ -4166,10 +3928,17 @@ static struct video_device *vdev_init(struct bttv *btv,
 	vfd->v4l2_dev = &btv->c.v4l2_dev;
 	vfd->release = video_device_release;
 	vfd->debug   = bttv_debug;
+	set_bit(V4L2_FL_USE_FH_PRIO, &vfd->flags);
 	video_set_drvdata(vfd, btv);
 	snprintf(vfd->name, sizeof(vfd->name), "BT%d%s %s (%s)",
 		 btv->id, (btv->id==848 && btv->revision==0x12) ? "A" : "",
 		 type_name, bttv_tvcards[btv->c.type].name);
+	if (btv->tuner_type == TUNER_ABSENT) {
+		v4l2_disable_ioctl(vfd, VIDIOC_G_FREQUENCY);
+		v4l2_disable_ioctl(vfd, VIDIOC_S_FREQUENCY);
+		v4l2_disable_ioctl(vfd, VIDIOC_G_TUNER);
+		v4l2_disable_ioctl(vfd, VIDIOC_S_TUNER);
+	}
 	return vfd;
 }
 
@@ -4237,6 +4006,7 @@ static int bttv_register_video(struct bttv *btv)
 	btv->radio_dev = vdev_init(btv, &radio_template, "radio");
 	if (NULL == btv->radio_dev)
 		goto err;
+	btv->radio_dev->ctrl_handler = &btv->radio_ctrl_handler;
 	if (video_register_device(btv->radio_dev, VFL_TYPE_RADIO,
 				  radio_nr[btv->c.nr]) < 0)
 		goto err;
@@ -4267,9 +4037,15 @@ static void pci_set_command(struct pci_dev *dev)
 
 static int bttv_probe(struct pci_dev *dev, const struct pci_device_id *pci_id)
 {
+	struct v4l2_frequency init_freq = {
+		.tuner = 0,
+		.type = V4L2_TUNER_ANALOG_TV,
+		.frequency = 980,
+	};
 	int result;
 	unsigned char lat;
 	struct bttv *btv;
+	struct v4l2_ctrl_handler *hdl;
 
 	if (bttv_num == BTTV_MAX)
 		return -ENOMEM;
@@ -4291,7 +4067,6 @@ static int bttv_probe(struct pci_dev *dev, const struct pci_device_id *pci_id)
 	INIT_LIST_HEAD(&btv->c.subs);
 	INIT_LIST_HEAD(&btv->capture);
 	INIT_LIST_HEAD(&btv->vcapture);
-	v4l2_prio_init(&btv->prio);
 
 	init_timer(&btv->timeout);
 	btv->timeout.function = bttv_irq_timeout;
@@ -4329,6 +4104,10 @@ static int bttv_probe(struct pci_dev *dev, const struct pci_device_id *pci_id)
 		pr_warn("%d: v4l2_device_register() failed\n", btv->c.nr);
 		goto fail0;
 	}
+	hdl = &btv->ctrl_handler;
+	v4l2_ctrl_handler_init(hdl, 20);
+	btv->c.v4l2_dev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(&btv->radio_ctrl_handler, 6);
 
 	btv->revision = dev->revision;
 	pci_read_config_byte(dev, PCI_LATENCY_TIMER, &lat);
@@ -4365,16 +4144,19 @@ static int bttv_probe(struct pci_dev *dev, const struct pci_device_id *pci_id)
 
 	/* init options from insmod args */
 	btv->opt_combfilter = combfilter;
-	btv->opt_lumafilter = lumafilter;
+	bttv_ctrl_combfilter.def = combfilter;
+	bttv_ctrl_lumafilter.def = lumafilter;
 	btv->opt_automute   = automute;
-	btv->opt_chroma_agc = chroma_agc;
-	btv->opt_adc_crush  = adc_crush;
+	bttv_ctrl_automute.def = automute;
+	bttv_ctrl_agc_crush.def = agc_crush;
 	btv->opt_vcr_hack   = vcr_hack;
-	btv->opt_whitecrush_upper  = whitecrush_upper;
-	btv->opt_whitecrush_lower  = whitecrush_lower;
+	bttv_ctrl_vcr_hack.def = vcr_hack;
+	bttv_ctrl_whitecrush_upper.def = whitecrush_upper;
+	bttv_ctrl_whitecrush_lower.def = whitecrush_lower;
 	btv->opt_uv_ratio   = uv_ratio;
-	btv->opt_full_luma_range   = full_luma_range;
-	btv->opt_coring     = coring;
+	bttv_ctrl_uv_ratio.def = uv_ratio;
+	bttv_ctrl_full_luma.def = full_luma_range;
+	bttv_ctrl_coring.def = coring;
 
 	/* fill struct bttv with some useful defaults */
 	btv->init.btv         = btv;
@@ -4383,8 +4165,39 @@ static int bttv_probe(struct pci_dev *dev, const struct pci_device_id *pci_id)
 	btv->init.fmt         = format_by_fourcc(V4L2_PIX_FMT_BGR24);
 	btv->init.width       = 320;
 	btv->init.height      = 240;
+	btv->init.ov.w.width  = 320;
+	btv->init.ov.w.height = 240;
+	btv->init.ov.field    = V4L2_FIELD_INTERLACED;
 	btv->input = 0;
 
+	v4l2_ctrl_new_std(hdl, &bttv_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 0xff00, 0x100, 32768);
+	v4l2_ctrl_new_std(hdl, &bttv_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 0xff80, 0x80, 0x6c00);
+	v4l2_ctrl_new_std(hdl, &bttv_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 0xff80, 0x80, 32768);
+	v4l2_ctrl_new_std(hdl, &bttv_ctrl_ops,
+			V4L2_CID_COLOR_KILLER, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(hdl, &bttv_ctrl_ops,
+			V4L2_CID_HUE, 0, 0xff00, 0x100, 32768);
+	v4l2_ctrl_new_std(hdl, &bttv_ctrl_ops,
+			V4L2_CID_CHROMA_AGC, 0, 1, 1, !!chroma_agc);
+	v4l2_ctrl_new_std(hdl, &bttv_ctrl_ops,
+		V4L2_CID_AUDIO_MUTE, 0, 1, 1, 0);
+	if (btv->volume_gpio)
+		v4l2_ctrl_new_std(hdl, &bttv_ctrl_ops,
+			V4L2_CID_AUDIO_VOLUME, 0, 0xff00, 0x100, 0xff00);
+	v4l2_ctrl_new_custom(hdl, &bttv_ctrl_combfilter, NULL);
+	v4l2_ctrl_new_custom(hdl, &bttv_ctrl_automute, NULL);
+	v4l2_ctrl_new_custom(hdl, &bttv_ctrl_lumafilter, NULL);
+	v4l2_ctrl_new_custom(hdl, &bttv_ctrl_agc_crush, NULL);
+	v4l2_ctrl_new_custom(hdl, &bttv_ctrl_vcr_hack, NULL);
+	v4l2_ctrl_new_custom(hdl, &bttv_ctrl_whitecrush_lower, NULL);
+	v4l2_ctrl_new_custom(hdl, &bttv_ctrl_whitecrush_upper, NULL);
+	v4l2_ctrl_new_custom(hdl, &bttv_ctrl_uv_ratio, NULL);
+	v4l2_ctrl_new_custom(hdl, &bttv_ctrl_full_luma, NULL);
+	v4l2_ctrl_new_custom(hdl, &bttv_ctrl_coring, NULL);
+
 	/* initialize hardware */
 	if (bttv_gpio)
 		bttv_gpio_tracking(btv,"pre-init");
@@ -4407,21 +4220,34 @@ static int bttv_probe(struct pci_dev *dev, const struct pci_device_id *pci_id)
 	/* some card-specific stuff (needs working i2c) */
 	bttv_init_card2(btv);
 	bttv_init_tuner(btv);
+	if (btv->tuner_type != TUNER_ABSENT) {
+		bttv_set_frequency(btv, &init_freq);
+		btv->radio_freq = 90500 * 16; /* 90.5Mhz default */
+	}
+	btv->std = V4L2_STD_PAL;
 	init_irqreg(btv);
+	v4l2_ctrl_handler_setup(hdl);
+	if (hdl->error) {
+		result = hdl->error;
+		goto fail2;
+	}
+	/* mute device */
+	audio_mute(btv, 1);
 
 	/* register video4linux + input */
 	if (!bttv_tvcards[btv->c.type].no_video) {
-		bttv_register_video(btv);
-		bt848_bright(btv,32768);
-		bt848_contrast(btv, 27648);
-		bt848_hue(btv,32768);
-		bt848_sat(btv,32768);
-		audio_mute(btv, 1);
+		v4l2_ctrl_add_handler(&btv->radio_ctrl_handler, hdl,
+				v4l2_ctrl_radio_filter);
+		if (btv->radio_ctrl_handler.error) {
+			result = btv->radio_ctrl_handler.error;
+			goto fail2;
+		}
 		set_input(btv, 0, btv->tvnorm);
 		bttv_crop_reset(&btv->crop[0], btv->tvnorm);
 		btv->crop[1] = btv->crop[0]; /* current = default */
 		disclaim_vbi_lines(btv);
 		disclaim_video_lines(btv);
+		bttv_register_video(btv);
 	}
 
 	/* add subdevices and autoload dvb-bt8xx if needed */
@@ -4443,6 +4269,8 @@ fail2:
 	free_irq(btv->c.pci->irq,btv);
 
 fail1:
+	v4l2_ctrl_handler_free(&btv->ctrl_handler);
+	v4l2_ctrl_handler_free(&btv->radio_ctrl_handler);
 	v4l2_device_unregister(&btv->c.v4l2_dev);
 
 fail0:
@@ -4484,9 +4312,11 @@ static void bttv_remove(struct pci_dev *pci_dev)
 	bttv_unregister_video(btv);
 
 	/* free allocated memory */
+	v4l2_ctrl_handler_free(&btv->ctrl_handler);
+	v4l2_ctrl_handler_free(&btv->radio_ctrl_handler);
 	btcx_riscmem_free(btv->c.pci,&btv->main);
 
-	/* free ressources */
+	/* free resources */
 	free_irq(btv->c.pci->irq,btv);
 	iounmap(btv->bt848_mmio);
 	release_mem_region(pci_resource_start(btv->c.pci,0),
diff --git a/drivers/media/pci/bt8xx/bttv-i2c.c b/drivers/media/pci/bt8xx/bttv-i2c.c
index c63c643ed1f..b7c52dc8999 100644
--- a/drivers/media/pci/bt8xx/bttv-i2c.c
+++ b/drivers/media/pci/bt8xx/bttv-i2c.c
@@ -394,3 +394,11 @@ int init_bttv_i2c(struct bttv *btv)
 
 	return btv->i2c_rc;
 }
+
+int fini_bttv_i2c(struct bttv *btv)
+{
+	if (0 != btv->i2c_rc)
+		return 0;
+
+	return i2c_del_adapter(&btv->c.i2c_adap);
+}
diff --git a/drivers/media/pci/bt8xx/bttv-input.c b/drivers/media/pci/bt8xx/bttv-input.c
index 04207a79905..f36821367d8 100644
--- a/drivers/media/pci/bt8xx/bttv-input.c
+++ b/drivers/media/pci/bt8xx/bttv-input.c
@@ -375,6 +375,7 @@ void init_bttv_i2c_ir(struct bttv *btv)
 		I2C_CLIENT_END
 	};
 	struct i2c_board_info info;
+	struct i2c_client *i2c_dev;
 
 	if (0 != btv->i2c_rc)
 		return;
@@ -390,7 +391,12 @@ void init_bttv_i2c_ir(struct bttv *btv)
 		btv->init_data.ir_codes = RC_MAP_PV951;
 		info.addr = 0x4b;
 		break;
-	default:
+	}
+
+	if (btv->init_data.name) {
+		info.platform_data = &btv->init_data;
+		i2c_dev = i2c_new_device(&btv->c.i2c_adap, &info);
+	} else {
 		/*
 		 * The external IR receiver is at i2c address 0x34 (0x35 for
 		 * reads).  Future Hauppauge cards will have an internal
@@ -399,24 +405,14 @@ void init_bttv_i2c_ir(struct bttv *btv)
 		 * internal.
 		 * That's why we probe 0x1a (~0x34) first. CB
 		 */
-
-		i2c_new_probed_device(&btv->c.i2c_adap, &info, addr_list, NULL);
-		return;
+		i2c_dev = i2c_new_probed_device(&btv->c.i2c_adap, &info, addr_list, NULL);
 	}
+	if (NULL == i2c_dev)
+		return;
 
-	if (btv->init_data.name)
-		info.platform_data = &btv->init_data;
-	i2c_new_device(&btv->c.i2c_adap, &info);
-
-	return;
-}
-
-int fini_bttv_i2c(struct bttv *btv)
-{
-	if (0 != btv->i2c_rc)
-		return 0;
-
-	return i2c_del_adapter(&btv->c.i2c_adap);
+#if defined(CONFIG_MODULES) && defined(MODULE)
+	request_module("ir-kbd-i2c");
+#endif
 }
 
 int bttv_input_init(struct bttv *btv)
diff --git a/drivers/media/pci/bt8xx/bttv.h b/drivers/media/pci/bt8xx/bttv.h
index 79a11240a59..6139ce26dc2 100644
--- a/drivers/media/pci/bt8xx/bttv.h
+++ b/drivers/media/pci/bt8xx/bttv.h
@@ -359,6 +359,9 @@ void bttv_gpio_bits(struct bttv_core *core, u32 mask, u32 bits);
 #define bttv_call_all(btv, o, f, args...) \
 	v4l2_device_call_all(&btv->c.v4l2_dev, 0, o, f, ##args)
 
+#define bttv_call_all_err(btv, o, f, args...) \
+	v4l2_device_call_until_err(&btv->c.v4l2_dev, 0, o, f, ##args)
+
 extern int bttv_I2CRead(struct bttv *btv, unsigned char addr, char *probe_for);
 extern int bttv_I2CWrite(struct bttv *btv, unsigned char addr, unsigned char b1,
 			 unsigned char b2, int both);
diff --git a/drivers/media/pci/bt8xx/bttvp.h b/drivers/media/pci/bt8xx/bttvp.h
index 9ec0adba236..9c1cc2c50ee 100644
--- a/drivers/media/pci/bt8xx/bttvp.h
+++ b/drivers/media/pci/bt8xx/bttvp.h
@@ -33,9 +33,11 @@
 #include <linux/input.h>
 #include <linux/mutex.h>
 #include <linux/scatterlist.h>
+#include <linux/device.h>
 #include <asm/io.h>
 #include <media/v4l2-common.h>
-#include <linux/device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fh.h>
 #include <media/videobuf-dma-sg.h>
 #include <media/tveeprom.h>
 #include <media/rc-core.h>
@@ -214,11 +216,11 @@ struct bttv_crop {
 };
 
 struct bttv_fh {
+	/* This must be the first field in this struct */
+	struct v4l2_fh		 fh;
+
 	struct bttv              *btv;
 	int resources;
-#ifdef VIDIOC_G_PRIORITY
-	enum v4l2_priority       prio;
-#endif
 	enum v4l2_buf_type       type;
 
 	/* video capture */
@@ -298,6 +300,10 @@ extern int no_overlay;
 /* bttv-input.c                                               */
 
 extern void init_bttv_i2c_ir(struct bttv *btv);
+
+/* ---------------------------------------------------------- */
+/* bttv-i2c.c                                                 */
+extern int init_bttv_i2c(struct bttv *btv);
 extern int fini_bttv_i2c(struct bttv *btv);
 
 /* ---------------------------------------------------------- */
@@ -308,7 +314,6 @@ extern unsigned int bttv_verbose;
 extern unsigned int bttv_debug;
 extern unsigned int bttv_gpio;
 extern void bttv_gpio_tracking(struct bttv *btv, char *comment);
-extern int init_bttv_i2c(struct bttv *btv);
 
 #define dprintk(fmt, ...)			\
 do {						\
@@ -393,12 +398,17 @@ struct bttv {
 	wait_queue_head_t          i2c_queue;
 	struct v4l2_subdev 	  *sd_msp34xx;
 	struct v4l2_subdev 	  *sd_tvaudio;
+	struct v4l2_subdev	  *sd_tda7432;
 
 	/* video4linux (1) */
 	struct video_device *video_dev;
 	struct video_device *radio_dev;
 	struct video_device *vbi_dev;
 
+	/* controls */
+	struct v4l2_ctrl_handler   ctrl_handler;
+	struct v4l2_ctrl_handler   radio_ctrl_handler;
+
 	/* infrared remote */
 	int has_remote;
 	struct bttv_ir *remote;
@@ -410,38 +420,30 @@ struct bttv {
 	spinlock_t s_lock;
 	struct mutex lock;
 	int resources;
-#ifdef VIDIOC_G_PRIORITY
-	struct v4l2_prio_state prio;
-#endif
 
 	/* video state */
 	unsigned int input;
-	unsigned int audio;
+	unsigned int audio_input;
 	unsigned int mute;
-	unsigned long freq;
+	unsigned long tv_freq;
 	unsigned int tvnorm;
+	v4l2_std_id std;
 	int hue, contrast, bright, saturation;
 	struct v4l2_framebuffer fbuf;
 	unsigned int field_count;
 
 	/* various options */
 	int opt_combfilter;
-	int opt_lumafilter;
 	int opt_automute;
-	int opt_chroma_agc;
-	int opt_color_killer;
-	int opt_adc_crush;
 	int opt_vcr_hack;
-	int opt_whitecrush_upper;
-	int opt_whitecrush_lower;
 	int opt_uv_ratio;
-	int opt_full_luma_range;
-	int opt_coring;
 
 	/* radio data/state */
 	int has_radio;
+	int has_radio_tuner;
 	int radio_user;
 	int radio_uses_msp_demodulator;
+	unsigned long radio_freq;
 
 	/* miro/pinnacle + Aimslab VHX
 	   philips matchbox (tea5757 radio tuner) support */
diff --git a/drivers/media/pci/cx18/cx18-av-core.c b/drivers/media/pci/cx18/cx18-av-core.c
index f164b7f610a..38b1d64ffc2 100644
--- a/drivers/media/pci/cx18/cx18-av-core.c
+++ b/drivers/media/pci/cx18/cx18-av-core.c
@@ -576,7 +576,7 @@ static void input_change(struct cx18 *cx)
 }
 
 static int cx18_av_s_frequency(struct v4l2_subdev *sd,
-			       struct v4l2_frequency *freq)
+			       const struct v4l2_frequency *freq)
 {
 	struct cx18 *cx = v4l2_get_subdevdata(sd);
 	input_change(cx);
@@ -809,7 +809,7 @@ static int cx18_av_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
 	return 0;
 }
 
-static int cx18_av_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+static int cx18_av_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
 {
 	struct cx18_av_state *state = to_cx18_av_state(sd);
 	struct cx18 *cx = v4l2_get_subdevdata(sd);
@@ -1266,7 +1266,7 @@ static int cx18_av_g_register(struct v4l2_subdev *sd,
 }
 
 static int cx18_av_s_register(struct v4l2_subdev *sd,
-			      struct v4l2_dbg_register *reg)
+			      const struct v4l2_dbg_register *reg)
 {
 	struct cx18 *cx = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/pci/cx18/cx18-driver.c b/drivers/media/pci/cx18/cx18-driver.c
index 613e5ae7d5c..67b61cf3e03 100644
--- a/drivers/media/pci/cx18/cx18-driver.c
+++ b/drivers/media/pci/cx18/cx18-driver.c
@@ -1243,7 +1243,7 @@ int cx18_init_on_first_open(struct cx18 *cx)
 	   in one place. */
 	cx->std++;		/* Force full standard initialization */
 	std = (cx->tuner_std == V4L2_STD_ALL) ? V4L2_STD_NTSC_M : cx->tuner_std;
-	cx18_s_std(NULL, &fh, &std);
+	cx18_s_std(NULL, &fh, std);
 	cx18_s_frequency(NULL, &fh, &vf);
 	return 0;
 }
diff --git a/drivers/media/pci/cx18/cx18-ioctl.c b/drivers/media/pci/cx18/cx18-ioctl.c
index cd8d2c2b162..aee7b6dacbf 100644
--- a/drivers/media/pci/cx18/cx18-ioctl.c
+++ b/drivers/media/pci/cx18/cx18-ioctl.c
@@ -415,42 +415,34 @@ static int cx18_g_chip_ident(struct file *file, void *fh,
 }
 
 #ifdef CONFIG_VIDEO_ADV_DEBUG
-static int cx18_cxc(struct cx18 *cx, unsigned int cmd, void *arg)
-{
-	struct v4l2_dbg_register *regs = arg;
-
-	if (!capable(CAP_SYS_ADMIN))
-		return -EPERM;
-	if (regs->reg >= CX18_MEM_OFFSET + CX18_MEM_SIZE)
-		return -EINVAL;
-
-	regs->size = 4;
-	if (cmd == VIDIOC_DBG_S_REGISTER)
-		cx18_write_enc(cx, regs->val, regs->reg);
-	else
-		regs->val = cx18_read_enc(cx, regs->reg);
-	return 0;
-}
-
 static int cx18_g_register(struct file *file, void *fh,
 				struct v4l2_dbg_register *reg)
 {
 	struct cx18 *cx = fh2id(fh)->cx;
 
-	if (v4l2_chip_match_host(&reg->match))
-		return cx18_cxc(cx, VIDIOC_DBG_G_REGISTER, reg);
+	if (v4l2_chip_match_host(&reg->match)) {
+		if (reg->reg >= CX18_MEM_OFFSET + CX18_MEM_SIZE)
+			return -EINVAL;
+		reg->size = 4;
+		reg->val = cx18_read_enc(cx, reg->reg);
+		return 0;
+	}
 	/* FIXME - errors shouldn't be ignored */
 	cx18_call_all(cx, core, g_register, reg);
 	return 0;
 }
 
 static int cx18_s_register(struct file *file, void *fh,
-				struct v4l2_dbg_register *reg)
+				const struct v4l2_dbg_register *reg)
 {
 	struct cx18 *cx = fh2id(fh)->cx;
 
-	if (v4l2_chip_match_host(&reg->match))
-		return cx18_cxc(cx, VIDIOC_DBG_S_REGISTER, reg);
+	if (v4l2_chip_match_host(&reg->match)) {
+		if (reg->reg >= CX18_MEM_OFFSET + CX18_MEM_SIZE)
+			return -EINVAL;
+		cx18_write_enc(cx, reg->val, reg->reg);
+		return 0;
+	}
 	/* FIXME - errors shouldn't be ignored */
 	cx18_call_all(cx, core, s_register, reg);
 	return 0;
@@ -614,7 +606,7 @@ static int cx18_g_frequency(struct file *file, void *fh,
 	return 0;
 }
 
-int cx18_s_frequency(struct file *file, void *fh, struct v4l2_frequency *vf)
+int cx18_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *vf)
 {
 	struct cx18_open_id *id = fh2id(fh);
 	struct cx18 *cx = id->cx;
@@ -637,15 +629,15 @@ static int cx18_g_std(struct file *file, void *fh, v4l2_std_id *std)
 	return 0;
 }
 
-int cx18_s_std(struct file *file, void *fh, v4l2_std_id *std)
+int cx18_s_std(struct file *file, void *fh, v4l2_std_id std)
 {
 	struct cx18_open_id *id = fh2id(fh);
 	struct cx18 *cx = id->cx;
 
-	if ((*std & V4L2_STD_ALL) == 0)
+	if ((std & V4L2_STD_ALL) == 0)
 		return -EINVAL;
 
-	if (*std == cx->std)
+	if (std == cx->std)
 		return 0;
 
 	if (test_bit(CX18_F_I_RADIO_USER, &cx->i_flags) ||
@@ -656,8 +648,8 @@ int cx18_s_std(struct file *file, void *fh, v4l2_std_id *std)
 		return -EBUSY;
 	}
 
-	cx->std = *std;
-	cx->is_60hz = (*std & V4L2_STD_525_60) ? 1 : 0;
+	cx->std = std;
+	cx->is_60hz = (std & V4L2_STD_525_60) ? 1 : 0;
 	cx->is_50hz = !cx->is_60hz;
 	cx2341x_handler_set_50hz(&cx->cxhdl, cx->is_50hz);
 	cx->cxhdl.width = 720;
@@ -673,7 +665,7 @@ int cx18_s_std(struct file *file, void *fh, v4l2_std_id *std)
 	return 0;
 }
 
-static int cx18_s_tuner(struct file *file, void *fh, struct v4l2_tuner *vt)
+static int cx18_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt)
 {
 	struct cx18_open_id *id = fh2id(fh);
 	struct cx18 *cx = id->cx;
@@ -1118,7 +1110,7 @@ static int cx18_log_status(struct file *file, void *fh)
 }
 
 static long cx18_default(struct file *file, void *fh, bool valid_prio,
-							int cmd, void *arg)
+			 unsigned int cmd, void *arg)
 {
 	struct cx18 *cx = fh2id(fh)->cx;
 
diff --git a/drivers/media/pci/cx18/cx18-ioctl.h b/drivers/media/pci/cx18/cx18-ioctl.h
index 2f9dd591ee0..43433969d63 100644
--- a/drivers/media/pci/cx18/cx18-ioctl.h
+++ b/drivers/media/pci/cx18/cx18-ioctl.h
@@ -26,6 +26,6 @@ u16 cx18_service2vbi(int type);
 void cx18_expand_service_set(struct v4l2_sliced_vbi_format *fmt, int is_pal);
 u16 cx18_get_service_set(struct v4l2_sliced_vbi_format *fmt);
 void cx18_set_funcs(struct video_device *vdev);
-int cx18_s_std(struct file *file, void *fh, v4l2_std_id *std);
-int cx18_s_frequency(struct file *file, void *fh, struct v4l2_frequency *vf);
+int cx18_s_std(struct file *file, void *fh, v4l2_std_id std);
+int cx18_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *vf);
 int cx18_s_input(struct file *file, void *fh, unsigned int inp);
diff --git a/drivers/media/pci/cx23885/altera-ci.h b/drivers/media/pci/cx23885/altera-ci.h
index 70e4fd69ad9..4998c96caeb 100644
--- a/drivers/media/pci/cx23885/altera-ci.h
+++ b/drivers/media/pci/cx23885/altera-ci.h
@@ -24,6 +24,8 @@
 #ifndef __ALTERA_CI_H
 #define __ALTERA_CI_H
 
+#include <linux/kconfig.h>
+
 #define ALT_DATA	0x000000ff
 #define ALT_TDI		0x00008000
 #define ALT_TDO		0x00004000
@@ -41,8 +43,7 @@ struct altera_ci_config {
 	int (*fpga_rw) (void *dev, int ad_rg, int val, int rw);
 };
 
-#if defined(CONFIG_MEDIA_ALTERA_CI) || (defined(CONFIG_MEDIA_ALTERA_CI_MODULE) \
-							&& defined(MODULE))
+#if IS_ENABLED(CONFIG_MEDIA_ALTERA_CI)
 
 extern int altera_ci_init(struct altera_ci_config *config, int ci_nr);
 extern void altera_ci_release(void *dev, int ci_nr);
diff --git a/drivers/media/pci/cx23885/cx23885-417.c b/drivers/media/pci/cx23885/cx23885-417.c
index 5d5052d0253..6dea11a7a85 100644
--- a/drivers/media/pci/cx23885/cx23885-417.c
+++ b/drivers/media/pci/cx23885/cx23885-417.c
@@ -1222,14 +1222,14 @@ static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *id)
 	return 0;
 }
 
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *id)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id id)
 {
 	struct cx23885_fh  *fh  = file->private_data;
 	struct cx23885_dev *dev = fh->dev;
 	unsigned int i;
 
 	for (i = 0; i < ARRAY_SIZE(cx23885_tvnorms); i++)
-		if (*id & cx23885_tvnorms[i].id)
+		if (id & cx23885_tvnorms[i].id)
 			break;
 	if (i == ARRAY_SIZE(cx23885_tvnorms))
 		return -EINVAL;
@@ -1237,7 +1237,7 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *id)
 
 	/* Have the drier core notify the subdevices */
 	mutex_lock(&dev->lock);
-	cx23885_set_tvnorm(dev, *id);
+	cx23885_set_tvnorm(dev, id);
 	mutex_unlock(&dev->lock);
 
 	return 0;
@@ -1280,7 +1280,7 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *t)
+				const struct v4l2_tuner *t)
 {
 	struct cx23885_fh  *fh  = file->private_data;
 	struct cx23885_dev *dev = fh->dev;
@@ -1311,7 +1311,7 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-	struct v4l2_frequency *f)
+	const struct v4l2_frequency *f)
 {
 	return cx23885_set_frequency(file, priv, f);
 }
diff --git a/drivers/media/pci/cx23885/cx23885-ioctl.c b/drivers/media/pci/cx23885/cx23885-ioctl.c
index ea9a614f3bb..acdb6d58db5 100644
--- a/drivers/media/pci/cx23885/cx23885-ioctl.c
+++ b/drivers/media/pci/cx23885/cx23885-ioctl.c
@@ -158,18 +158,17 @@ int cx23885_g_register(struct file *file, void *fh,
 }
 
 static int cx23885_s_host_register(struct cx23885_dev *dev,
-				   struct v4l2_dbg_register *reg)
+				   const struct v4l2_dbg_register *reg)
 {
 	if ((reg->reg & 0x3) != 0 || reg->reg >= pci_resource_len(dev->pci, 0))
 		return -EINVAL;
 
-	reg->size = 4;
 	cx_write(reg->reg, reg->val);
 	return 0;
 }
 
 static int cx23417_s_register(struct cx23885_dev *dev,
-			      struct v4l2_dbg_register *reg)
+			      const struct v4l2_dbg_register *reg)
 {
 	if (dev->v4l_device == NULL)
 		return -EINVAL;
@@ -179,13 +178,11 @@ static int cx23417_s_register(struct cx23885_dev *dev,
 
 	if (mc417_register_write(dev, (u16) reg->reg, (u32) reg->val))
 		return -EINVAL; /* V4L2 spec, but -EREMOTEIO really */
-
-	reg->size = 4;
 	return 0;
 }
 
 int cx23885_s_register(struct file *file, void *fh,
-		       struct v4l2_dbg_register *reg)
+		       const struct v4l2_dbg_register *reg)
 {
 	struct cx23885_dev *dev = ((struct cx23885_fh *)fh)->dev;
 
diff --git a/drivers/media/pci/cx23885/cx23885-ioctl.h b/drivers/media/pci/cx23885/cx23885-ioctl.h
index 315be0ca5a0..a6080964a9e 100644
--- a/drivers/media/pci/cx23885/cx23885-ioctl.h
+++ b/drivers/media/pci/cx23885/cx23885-ioctl.h
@@ -33,7 +33,7 @@ int cx23885_g_register(struct file *file, void *fh,
 
 
 int cx23885_s_register(struct file *file, void *fh,
-		       struct v4l2_dbg_register *reg);
+		       const struct v4l2_dbg_register *reg);
 
 #endif
 #endif
diff --git a/drivers/media/pci/cx23885/cx23885-video.c b/drivers/media/pci/cx23885/cx23885-video.c
index 5991bc8dc15..ed08c89adde 100644
--- a/drivers/media/pci/cx23885/cx23885-video.c
+++ b/drivers/media/pci/cx23885/cx23885-video.c
@@ -1259,13 +1259,13 @@ static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *id)
 	return 0;
 }
 
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *tvnorms)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id tvnorms)
 {
 	struct cx23885_dev *dev = ((struct cx23885_fh *)priv)->dev;
 	dprintk(1, "%s()\n", __func__);
 
 	mutex_lock(&dev->lock);
-	cx23885_set_tvnorm(dev, *tvnorms);
+	cx23885_set_tvnorm(dev, tvnorms);
 	mutex_unlock(&dev->lock);
 
 	return 0;
@@ -1486,7 +1486,7 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *t)
+				const struct v4l2_tuner *t)
 {
 	struct cx23885_dev *dev = ((struct cx23885_fh *)priv)->dev;
 
@@ -1518,7 +1518,7 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 	return 0;
 }
 
-static int cx23885_set_freq(struct cx23885_dev *dev, struct v4l2_frequency *f)
+static int cx23885_set_freq(struct cx23885_dev *dev, const struct v4l2_frequency *f)
 {
 	struct v4l2_control ctrl;
 
@@ -1550,7 +1550,7 @@ static int cx23885_set_freq(struct cx23885_dev *dev, struct v4l2_frequency *f)
 }
 
 static int cx23885_set_freq_via_ops(struct cx23885_dev *dev,
-	struct v4l2_frequency *f)
+	const struct v4l2_frequency *f)
 {
 	struct v4l2_control ctrl;
 	struct videobuf_dvb_frontend *vfe;
@@ -1608,7 +1608,7 @@ static int cx23885_set_freq_via_ops(struct cx23885_dev *dev,
 }
 
 int cx23885_set_frequency(struct file *file, void *priv,
-	struct v4l2_frequency *f)
+	const struct v4l2_frequency *f)
 {
 	struct cx23885_fh *fh = priv;
 	struct cx23885_dev *dev = fh->dev;
@@ -1628,7 +1628,7 @@ int cx23885_set_frequency(struct file *file, void *priv,
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-	struct v4l2_frequency *f)
+	const struct v4l2_frequency *f)
 {
 	return cx23885_set_frequency(file, priv, f);
 }
diff --git a/drivers/media/pci/cx23885/cx23885.h b/drivers/media/pci/cx23885/cx23885.h
index 59c322d870f..5687d3f678d 100644
--- a/drivers/media/pci/cx23885/cx23885.h
+++ b/drivers/media/pci/cx23885/cx23885.h
@@ -587,7 +587,7 @@ extern void cx23885_video_wakeup(struct cx23885_dev *dev,
 int cx23885_enum_input(struct cx23885_dev *dev, struct v4l2_input *i);
 int cx23885_set_input(struct file *file, void *priv, unsigned int i);
 int cx23885_get_input(struct file *file, void *priv, unsigned int *i);
-int cx23885_set_frequency(struct file *file, void *priv, struct v4l2_frequency *f);
+int cx23885_set_frequency(struct file *file, void *priv, const struct v4l2_frequency *f);
 int cx23885_set_control(struct cx23885_dev *dev, struct v4l2_control *ctl);
 int cx23885_get_control(struct cx23885_dev *dev, struct v4l2_control *ctl);
 int cx23885_set_tvnorm(struct cx23885_dev *dev, v4l2_std_id norm);
diff --git a/drivers/media/pci/cx23885/cx23888-ir.c b/drivers/media/pci/cx23885/cx23888-ir.c
index d51eed051d5..fa672fe4107 100644
--- a/drivers/media/pci/cx23885/cx23888-ir.c
+++ b/drivers/media/pci/cx23885/cx23888-ir.c
@@ -1124,7 +1124,7 @@ static int cx23888_ir_g_register(struct v4l2_subdev *sd,
 }
 
 static int cx23888_ir_s_register(struct v4l2_subdev *sd,
-				 struct v4l2_dbg_register *reg)
+				 const struct v4l2_dbg_register *reg)
 {
 	struct cx23888_ir_state *state = to_state(sd);
 	u32 addr = CX23888_IR_REG_BASE + (u32) reg->reg;
diff --git a/drivers/media/pci/cx25821/Kconfig b/drivers/media/pci/cx25821/Kconfig
index 4017c942034..6439a847680 100644
--- a/drivers/media/pci/cx25821/Kconfig
+++ b/drivers/media/pci/cx25821/Kconfig
@@ -1,14 +1,9 @@
 config VIDEO_CX25821
 	tristate "Conexant cx25821 support"
-	depends on DVB_CORE && VIDEO_DEV && PCI && I2C
+	depends on VIDEO_DEV && PCI && I2C
 	select I2C_ALGOBIT
 	select VIDEO_BTCX
-	select VIDEO_TVEEPROM
-	depends on RC_CORE
-	select VIDEOBUF_DVB
 	select VIDEOBUF_DMA_SG
-	select VIDEO_CX25840
-	select VIDEO_CX2341X
 	---help---
 	  This is a video4linux driver for Conexant 25821 based
 	  TV cards.
diff --git a/drivers/media/pci/cx25821/Makefile b/drivers/media/pci/cx25821/Makefile
index caa32b7b51f..fb76c3d3713 100644
--- a/drivers/media/pci/cx25821/Makefile
+++ b/drivers/media/pci/cx25821/Makefile
@@ -1,14 +1,9 @@
 cx25821-y   := cx25821-core.o cx25821-cards.o cx25821-i2c.o \
 		       cx25821-gpio.o cx25821-medusa-video.o \
-		       cx25821-video.o cx25821-video-upstream.o \
-		       cx25821-video-upstream-ch2.o \
-		       cx25821-audio-upstream.o
+		       cx25821-video.o cx25821-video-upstream.o
 
 obj-$(CONFIG_VIDEO_CX25821) += cx25821.o
 obj-$(CONFIG_VIDEO_CX25821_ALSA) += cx25821-alsa.o
 
 ccflags-y += -Idrivers/media/i2c
 ccflags-y += -Idrivers/media/common
-ccflags-y += -Idrivers/media/tuners
-ccflags-y += -Idrivers/media/dvb-core
-ccflags-y += -Idrivers/media/dvb-frontends
diff --git a/drivers/media/pci/cx25821/cx25821-alsa.c b/drivers/media/pci/cx25821/cx25821-alsa.c
index 1858a45dd08..6e91e84d6bf 100644
--- a/drivers/media/pci/cx25821/cx25821-alsa.c
+++ b/drivers/media/pci/cx25821/cx25821-alsa.c
@@ -59,7 +59,6 @@ do {									\
 	Data type declarations - Can be moded to a header file later
  ****************************************************************************/
 
-static struct snd_card *snd_cx25821_cards[SNDRV_CARDS];
 static int devno;
 
 struct cx25821_audio_buffer {
@@ -151,7 +150,7 @@ static int _cx25821_start_audio_dma(struct cx25821_audio_dev *chip)
 {
 	struct cx25821_audio_buffer *buf = chip->buf;
 	struct cx25821_dev *dev = chip->dev;
-	struct sram_channel *audio_ch =
+	const struct sram_channel *audio_ch =
 	    &cx25821_sram_channels[AUDIO_SRAM_CHANNEL];
 	u32 tmp = 0;
 
@@ -627,34 +626,6 @@ static DEFINE_PCI_DEVICE_TABLE(cx25821_audio_pci_tbl) = {
 MODULE_DEVICE_TABLE(pci, cx25821_audio_pci_tbl);
 
 /*
- * Not used in the function snd_cx25821_dev_free so removing
- * from the file.
- */
-/*
-static int snd_cx25821_free(struct cx25821_audio_dev *chip)
-{
-	if (chip->irq >= 0)
-		free_irq(chip->irq, chip);
-
-	cx25821_dev_unregister(chip->dev);
-	pci_disable_device(chip->pci);
-
-	return 0;
-}
-*/
-
-/*
- * Component Destructor
- */
-static void snd_cx25821_dev_free(struct snd_card *card)
-{
-	struct cx25821_audio_dev *chip = card->private_data;
-
-	/* snd_cx25821_free(chip); */
-	snd_card_free(chip->card);
-}
-
-/*
  * Alsa Constructor - Component probe
  */
 static int cx25821_audio_initdev(struct cx25821_dev *dev)
@@ -685,7 +656,6 @@ static int cx25821_audio_initdev(struct cx25821_dev *dev)
 	strcpy(card->driver, "cx25821");
 
 	/* Card "creation" */
-	card->private_free = snd_cx25821_dev_free;
 	chip = card->private_data;
 	spin_lock_init(&chip->reg_lock);
 
@@ -729,8 +699,7 @@ static int cx25821_audio_initdev(struct cx25821_dev *dev)
 		goto error;
 	}
 
-	snd_cx25821_cards[devno] = card;
-
+	dev->card = card;
 	devno++;
 	return 0;
 
@@ -742,9 +711,33 @@ error:
 /****************************************************************************
 				LINUX MODULE INIT
  ****************************************************************************/
+
+static int cx25821_alsa_exit_callback(struct device *dev, void *data)
+{
+	struct v4l2_device *v4l2_dev = dev_get_drvdata(dev);
+	struct cx25821_dev *cxdev = get_cx25821(v4l2_dev);
+
+	snd_card_free(cxdev->card);
+	return 0;
+}
+
 static void cx25821_audio_fini(void)
 {
-	snd_card_free(snd_cx25821_cards[0]);
+	struct device_driver *drv = driver_find("cx25821", &pci_bus_type);
+	int ret;
+
+	ret = driver_for_each_device(drv, NULL, NULL, cx25821_alsa_exit_callback);
+	if (ret)
+		pr_err("%s failed to find a cx25821 driver.\n", __func__);
+}
+
+static int cx25821_alsa_init_callback(struct device *dev, void *data)
+{
+	struct v4l2_device *v4l2_dev = dev_get_drvdata(dev);
+	struct cx25821_dev *cxdev = get_cx25821(v4l2_dev);
+
+	cx25821_audio_initdev(cxdev);
+	return 0;
 }
 
 /*
@@ -756,29 +749,11 @@ static void cx25821_audio_fini(void)
  */
 static int cx25821_alsa_init(void)
 {
-	struct cx25821_dev *dev = NULL;
-	struct list_head *list;
+	struct device_driver *drv = driver_find("cx25821", &pci_bus_type);
 
-	mutex_lock(&cx25821_devlist_mutex);
-	list_for_each(list, &cx25821_devlist) {
-		dev = list_entry(list, struct cx25821_dev, devlist);
-		cx25821_audio_initdev(dev);
-	}
-	mutex_unlock(&cx25821_devlist_mutex);
-
-	if (dev == NULL)
-		pr_info("ERROR ALSA: no cx25821 cards found\n");
-
-	return 0;
+	return driver_for_each_device(drv, NULL, NULL, cx25821_alsa_init_callback);
 
 }
 
 late_initcall(cx25821_alsa_init);
 module_exit(cx25821_audio_fini);
-
-/* ----------------------------------------------------------- */
-/*
- * Local variables:
- * c-basic-offset: 8
- * End:
- */
diff --git a/drivers/media/pci/cx25821/cx25821-audio-upstream.c b/drivers/media/pci/cx25821/cx25821-audio-upstream.c
index 87491ca05ee..b9be535e32b 100644
--- a/drivers/media/pci/cx25821/cx25821-audio-upstream.c
+++ b/drivers/media/pci/cx25821/cx25821-audio-upstream.c
@@ -45,7 +45,7 @@ static int _intr_msk = FLD_AUD_SRC_RISCI1 | FLD_AUD_SRC_OF |
 			FLD_AUD_SRC_SYNC | FLD_AUD_SRC_OPC_ERR;
 
 static int cx25821_sram_channel_setup_upstream_audio(struct cx25821_dev *dev,
-					      struct sram_channel *ch,
+					      const struct sram_channel *ch,
 					      unsigned int bpl, u32 risc)
 {
 	unsigned int i, lines;
@@ -106,7 +106,7 @@ static __le32 *cx25821_risc_field_upstream_audio(struct cx25821_dev *dev,
 						 int fifo_enable)
 {
 	unsigned int line;
-	struct sram_channel *sram_ch =
+	const struct sram_channel *sram_ch =
 		dev->channels[dev->_audio_upstream_channel].sram_channels;
 	int offset = 0;
 
@@ -215,7 +215,7 @@ static void cx25821_free_memory_audio(struct cx25821_dev *dev)
 
 void cx25821_stop_upstream_audio(struct cx25821_dev *dev)
 {
-	struct sram_channel *sram_ch =
+	const struct sram_channel *sram_ch =
 		dev->channels[AUDIO_UPSTREAM_SRAM_CHANNEL_B].sram_channels;
 	u32 tmp = 0;
 
@@ -257,7 +257,7 @@ void cx25821_free_mem_upstream_audio(struct cx25821_dev *dev)
 }
 
 static int cx25821_get_audio_data(struct cx25821_dev *dev,
-			   struct sram_channel *sram_ch)
+			   const struct sram_channel *sram_ch)
 {
 	struct file *myfile;
 	int frame_index_temp = dev->_audioframe_index;
@@ -352,7 +352,7 @@ static void cx25821_audioups_handler(struct work_struct *work)
 }
 
 static int cx25821_openfile_audio(struct cx25821_dev *dev,
-			   struct sram_channel *sram_ch)
+			   const struct sram_channel *sram_ch)
 {
 	struct file *myfile;
 	int i = 0, j = 0;
@@ -433,7 +433,7 @@ static int cx25821_openfile_audio(struct cx25821_dev *dev,
 }
 
 static int cx25821_audio_upstream_buffer_prepare(struct cx25821_dev *dev,
-						 struct sram_channel *sram_ch,
+						 const struct sram_channel *sram_ch,
 						 int bpl)
 {
 	int ret = 0;
@@ -495,7 +495,7 @@ static int cx25821_audio_upstream_irq(struct cx25821_dev *dev, int chan_num,
 {
 	int i = 0;
 	u32 int_msk_tmp;
-	struct sram_channel *channel = dev->channels[chan_num].sram_channels;
+	const struct sram_channel *channel = dev->channels[chan_num].sram_channels;
 	dma_addr_t risc_phys_jump_addr;
 	__le32 *rp;
 
@@ -587,7 +587,7 @@ static irqreturn_t cx25821_upstream_irq_audio(int irq, void *dev_id)
 	struct cx25821_dev *dev = dev_id;
 	u32 audio_status;
 	int handled = 0;
-	struct sram_channel *sram_ch;
+	const struct sram_channel *sram_ch;
 
 	if (!dev)
 		return -1;
@@ -611,7 +611,7 @@ static irqreturn_t cx25821_upstream_irq_audio(int irq, void *dev_id)
 }
 
 static void cx25821_wait_fifo_enable(struct cx25821_dev *dev,
-				     struct sram_channel *sram_ch)
+				     const struct sram_channel *sram_ch)
 {
 	int count = 0;
 	u32 tmp;
@@ -635,7 +635,7 @@ static void cx25821_wait_fifo_enable(struct cx25821_dev *dev,
 }
 
 static int cx25821_start_audio_dma_upstream(struct cx25821_dev *dev,
-					    struct sram_channel *sram_ch)
+					    const struct sram_channel *sram_ch)
 {
 	u32 tmp = 0;
 	int err = 0;
@@ -699,7 +699,7 @@ fail_irq:
 
 int cx25821_audio_upstream_init(struct cx25821_dev *dev, int channel_select)
 {
-	struct sram_channel *sram_ch;
+	const struct sram_channel *sram_ch;
 	int err = 0;
 
 	if (dev->_audio_is_running) {
@@ -728,26 +728,17 @@ int cx25821_audio_upstream_init(struct cx25821_dev *dev, int channel_select)
 	dev->_audio_lines_count = LINES_PER_AUDIO_BUFFER;
 	_line_size = AUDIO_LINE_SIZE;
 
-	if (dev->input_audiofilename) {
+	if ((dev->input_audiofilename) &&
+	    (strcmp(dev->input_audiofilename, "") != 0))
 		dev->_audiofilename = kstrdup(dev->input_audiofilename,
 					      GFP_KERNEL);
-
-		if (!dev->_audiofilename) {
-			err = -ENOMEM;
-			goto error;
-		}
-
-		/* Default if filename is empty string */
-		if (strcmp(dev->input_audiofilename, "") == 0)
-			dev->_audiofilename = "/root/audioGOOD.wav";
-	} else {
+	else
 		dev->_audiofilename = kstrdup(_defaultAudioName,
 					      GFP_KERNEL);
 
-		if (!dev->_audiofilename) {
-			err = -ENOMEM;
-			goto error;
-		}
+	if (!dev->_audiofilename) {
+		err = -ENOMEM;
+		goto error;
 	}
 
 	cx25821_sram_channel_setup_upstream_audio(dev, sram_ch,
diff --git a/drivers/media/pci/cx25821/cx25821-cards.c b/drivers/media/pci/cx25821/cx25821-cards.c
index 99988c98809..3b409feb03d 100644
--- a/drivers/media/pci/cx25821/cx25821-cards.c
+++ b/drivers/media/pci/cx25821/cx25821-cards.c
@@ -26,11 +26,8 @@
 #include <linux/init.h>
 #include <linux/module.h>
 #include <linux/pci.h>
-#include <linux/delay.h>
-#include <media/cx25840.h>
 
 #include "cx25821.h"
-#include "tuner-xc2028.h"
 
 /* board config info */
 
@@ -45,28 +42,8 @@ struct cx25821_board cx25821_boards[] = {
 		.name = "CX25821",
 		.portb = CX25821_RAW,
 		.portc = CX25821_264,
-		.input[0].type = CX25821_VMUX_COMPOSITE,
 	},
 
 };
 
 const unsigned int cx25821_bcount = ARRAY_SIZE(cx25821_boards);
-
-struct cx25821_subid cx25821_subids[] = {
-	{
-		.subvendor = 0x14f1,
-		.subdevice = 0x0920,
-		.card = CX25821_BOARD,
-	},
-};
-
-void cx25821_card_setup(struct cx25821_dev *dev)
-{
-	static u8 eeprom[256];
-
-	if (dev->i2c_bus[0].i2c_rc == 0) {
-		dev->i2c_bus[0].i2c_client.addr = 0xa0 >> 1;
-		tveeprom_read(&dev->i2c_bus[0].i2c_client, eeprom,
-				sizeof(eeprom));
-	}
-}
diff --git a/drivers/media/pci/cx25821/cx25821-core.c b/drivers/media/pci/cx25821/cx25821-core.c
index 1884e2cc35e..b762c5b2ca1 100644
--- a/drivers/media/pci/cx25821/cx25821-core.c
+++ b/drivers/media/pci/cx25821/cx25821-core.c
@@ -41,14 +41,7 @@ static unsigned int card[] = {[0 ... (CX25821_MAXBOARDS - 1)] = UNSET };
 module_param_array(card, int, NULL, 0444);
 MODULE_PARM_DESC(card, "card type");
 
-static unsigned int cx25821_devcount;
-
-DEFINE_MUTEX(cx25821_devlist_mutex);
-EXPORT_SYMBOL(cx25821_devlist_mutex);
-LIST_HEAD(cx25821_devlist);
-EXPORT_SYMBOL(cx25821_devlist);
-
-struct sram_channel cx25821_sram_channels[] = {
+const struct sram_channel cx25821_sram_channels[] = {
 	[SRAM_CH00] = {
 		.i = SRAM_CH00,
 		.name = "VID A",
@@ -317,20 +310,6 @@ struct sram_channel cx25821_sram_channels[] = {
 };
 EXPORT_SYMBOL(cx25821_sram_channels);
 
-struct sram_channel *channel0 = &cx25821_sram_channels[SRAM_CH00];
-struct sram_channel *channel1 = &cx25821_sram_channels[SRAM_CH01];
-struct sram_channel *channel2 = &cx25821_sram_channels[SRAM_CH02];
-struct sram_channel *channel3 = &cx25821_sram_channels[SRAM_CH03];
-struct sram_channel *channel4 = &cx25821_sram_channels[SRAM_CH04];
-struct sram_channel *channel5 = &cx25821_sram_channels[SRAM_CH05];
-struct sram_channel *channel6 = &cx25821_sram_channels[SRAM_CH06];
-struct sram_channel *channel7 = &cx25821_sram_channels[SRAM_CH07];
-struct sram_channel *channel9 = &cx25821_sram_channels[SRAM_CH09];
-struct sram_channel *channel10 = &cx25821_sram_channels[SRAM_CH10];
-struct sram_channel *channel11 = &cx25821_sram_channels[SRAM_CH11];
-
-struct cx25821_dmaqueue mpegq;
-
 static int cx25821_risc_decode(u32 risc)
 {
 	static const char * const instr[16] = {
@@ -457,7 +436,7 @@ static void cx25821_registers_init(struct cx25821_dev *dev)
 }
 
 int cx25821_sram_channel_setup(struct cx25821_dev *dev,
-			       struct sram_channel *ch,
+			       const struct sram_channel *ch,
 			       unsigned int bpl, u32 risc)
 {
 	unsigned int i, lines;
@@ -523,10 +502,9 @@ int cx25821_sram_channel_setup(struct cx25821_dev *dev,
 
 	return 0;
 }
-EXPORT_SYMBOL(cx25821_sram_channel_setup);
 
 int cx25821_sram_channel_setup_audio(struct cx25821_dev *dev,
-				     struct sram_channel *ch,
+				     const struct sram_channel *ch,
 				     unsigned int bpl, u32 risc)
 {
 	unsigned int i, lines;
@@ -592,7 +570,7 @@ int cx25821_sram_channel_setup_audio(struct cx25821_dev *dev,
 }
 EXPORT_SYMBOL(cx25821_sram_channel_setup_audio);
 
-void cx25821_sram_channel_dump(struct cx25821_dev *dev, struct sram_channel *ch)
+void cx25821_sram_channel_dump(struct cx25821_dev *dev, const struct sram_channel *ch)
 {
 	static char *name[] = {
 		"init risc lo",
@@ -652,10 +630,9 @@ void cx25821_sram_channel_dump(struct cx25821_dev *dev, struct sram_channel *ch)
 	pr_warn("        :   cnt2_reg: 0x%08x\n",
 		cx_read(ch->cnt2_reg));
 }
-EXPORT_SYMBOL(cx25821_sram_channel_dump);
 
 void cx25821_sram_channel_dump_audio(struct cx25821_dev *dev,
-				     struct sram_channel *ch)
+				     const struct sram_channel *ch)
 {
 	static const char * const name[] = {
 		"init risc lo",
@@ -798,12 +775,12 @@ void cx25821_set_pixel_format(struct cx25821_dev *dev, int channel_select,
 	if (channel_select <= 7 && channel_select >= 0) {
 		cx_write(dev->channels[channel_select].sram_channels->pix_frmt,
 				format);
-		dev->channels[channel_select].pixel_formats = format;
 	}
+	dev->channels[channel_select].pixel_formats = format;
 }
 
 static void cx25821_set_vip_mode(struct cx25821_dev *dev,
-				 struct sram_channel *ch)
+				 const struct sram_channel *ch)
 {
 	cx_write(ch->pix_frmt, PIXEL_FRMT_422);
 	cx_write(ch->vip_ctl, PIXEL_ENGINE_VIP1);
@@ -837,12 +814,13 @@ static void cx25821_initialize(struct cx25821_dev *dev)
 		cx25821_sram_channel_setup(dev, dev->channels[i].sram_channels,
 						1440, 0);
 		dev->channels[i].pixel_formats = PIXEL_FRMT_422;
-		dev->channels[i].use_cif_resolution = FALSE;
+		dev->channels[i].use_cif_resolution = 0;
 	}
 
 	/* Probably only affect Downstream */
 	for (i = VID_UPSTREAM_SRAM_CHANNEL_I;
 		i <= VID_UPSTREAM_SRAM_CHANNEL_J; i++) {
+		dev->channels[i].pixel_formats = PIXEL_FRMT_422;
 		cx25821_set_vip_mode(dev, dev->channels[i].sram_channels);
 	}
 
@@ -868,8 +846,7 @@ static void cx25821_dev_checkrevision(struct cx25821_dev *dev)
 {
 	dev->hwrevision = cx_read(RDR_CFG2) & 0xff;
 
-	pr_info("%s(): Hardware revision = 0x%02x\n",
-		__func__, dev->hwrevision);
+	pr_info("Hardware revision = 0x%02x\n", dev->hwrevision);
 }
 
 static void cx25821_iounmap(struct cx25821_dev *dev)
@@ -879,7 +856,6 @@ static void cx25821_iounmap(struct cx25821_dev *dev)
 
 	/* Releasing IO memory */
 	if (dev->lmmio != NULL) {
-		CX25821_INFO("Releasing lmmio.\n");
 		iounmap(dev->lmmio);
 		dev->lmmio = NULL;
 	}
@@ -887,23 +863,14 @@ static void cx25821_iounmap(struct cx25821_dev *dev)
 
 static int cx25821_dev_setup(struct cx25821_dev *dev)
 {
+	static unsigned int cx25821_devcount;
 	int i;
 
-	pr_info("\n***********************************\n");
-	pr_info("cx25821 set up\n");
-	pr_info("***********************************\n\n");
-
 	mutex_init(&dev->lock);
 
-	atomic_inc(&dev->refcount);
-
 	dev->nr = ++cx25821_devcount;
 	sprintf(dev->name, "cx25821[%d]", dev->nr);
 
-	mutex_lock(&cx25821_devlist_mutex);
-	list_add_tail(&dev->devlist, &cx25821_devlist);
-	mutex_unlock(&cx25821_devlist_mutex);
-
 	if (dev->pci->device != 0x8210) {
 		pr_info("%s(): Exiting. Incorrect Hardware device = 0x%02x\n",
 			__func__, dev->pci->device);
@@ -914,8 +881,11 @@ static int cx25821_dev_setup(struct cx25821_dev *dev)
 
 	/* Apply a sensible clock frequency for the PCIe bridge */
 	dev->clk_freq = 28000000;
-	for (i = 0; i < MAX_VID_CHANNEL_NUM; i++)
+	for (i = 0; i < MAX_VID_CHANNEL_NUM; i++) {
+		dev->channels[i].dev = dev;
+		dev->channels[i].id = i;
 		dev->channels[i].sram_channels = &cx25821_sram_channels[i];
+	}
 
 	if (dev->nr > 1)
 		CX25821_INFO("dev->nr > 1!");
@@ -978,63 +948,15 @@ static int cx25821_dev_setup(struct cx25821_dev *dev)
 /*  cx25821_i2c_register(&dev->i2c_bus[1]);
  *  cx25821_i2c_register(&dev->i2c_bus[2]); */
 
-	CX25821_INFO("i2c register! bus->i2c_rc = %d\n",
-			dev->i2c_bus[0].i2c_rc);
-
-	cx25821_card_setup(dev);
-
 	if (medusa_video_init(dev) < 0)
 		CX25821_ERR("%s(): Failed to initialize medusa!\n", __func__);
 
 	cx25821_video_register(dev);
 
-	/* register IOCTL device */
-	dev->ioctl_dev = cx25821_vdev_init(dev, dev->pci,
-			&cx25821_videoioctl_template, "video");
-
-	if (video_register_device
-	    (dev->ioctl_dev, VFL_TYPE_GRABBER, VIDEO_IOCTL_CH) < 0) {
-		cx25821_videoioctl_unregister(dev);
-		pr_err("%s(): Failed to register video adapter for IOCTL, so unregistering videoioctl device\n",
-		       __func__);
-	}
-
 	cx25821_dev_checkrevision(dev);
-	CX25821_INFO("setup done!\n");
-
 	return 0;
 }
 
-void cx25821_start_upstream_video_ch1(struct cx25821_dev *dev,
-				      struct upstream_user_struct *up_data)
-{
-	dev->_isNTSC = !strcmp(dev->vid_stdname, "NTSC") ? 1 : 0;
-
-	dev->tvnorm = !dev->_isNTSC ? V4L2_STD_PAL_BG : V4L2_STD_NTSC_M;
-	medusa_set_videostandard(dev);
-
-	cx25821_vidupstream_init_ch1(dev, dev->channel_select,
-				     dev->pixel_format);
-}
-
-void cx25821_start_upstream_video_ch2(struct cx25821_dev *dev,
-				      struct upstream_user_struct *up_data)
-{
-	dev->_isNTSC_ch2 = !strcmp(dev->vid_stdname_ch2, "NTSC") ? 1 : 0;
-
-	dev->tvnorm = !dev->_isNTSC_ch2 ? V4L2_STD_PAL_BG : V4L2_STD_NTSC_M;
-	medusa_set_videostandard(dev);
-
-	cx25821_vidupstream_init_ch2(dev, dev->channel_select_ch2,
-				     dev->pixel_format_ch2);
-}
-
-void cx25821_start_upstream_audio(struct cx25821_dev *dev,
-				  struct upstream_user_struct *up_data)
-{
-	cx25821_audio_upstream_init(dev, AUDIO_UPSTREAM_SRAM_CHANNEL_B);
-}
-
 void cx25821_dev_unregister(struct cx25821_dev *dev)
 {
 	int i;
@@ -1042,25 +964,16 @@ void cx25821_dev_unregister(struct cx25821_dev *dev)
 	if (!dev->base_io_addr)
 		return;
 
-	cx25821_free_mem_upstream_ch1(dev);
-	cx25821_free_mem_upstream_ch2(dev);
-	cx25821_free_mem_upstream_audio(dev);
-
 	release_mem_region(dev->base_io_addr, pci_resource_len(dev->pci, 0));
 
-	if (!atomic_dec_and_test(&dev->refcount))
-		return;
-
-	for (i = 0; i < VID_CHANNEL_NUM; i++)
-		cx25821_video_unregister(dev, i);
-
-	for (i = VID_UPSTREAM_SRAM_CHANNEL_I;
-	     i <= AUDIO_UPSTREAM_SRAM_CHANNEL_B; i++) {
+	for (i = 0; i < MAX_VID_CHANNEL_NUM - 1; i++) {
+		if (i == SRAM_CH08) /* audio channel */
+			continue;
+		if (i == SRAM_CH09 || i == SRAM_CH10)
+			cx25821_free_mem_upstream(&dev->channels[i]);
 		cx25821_video_unregister(dev, i);
 	}
 
-	cx25821_videoioctl_unregister(dev);
-
 	cx25821_i2c_unregister(&dev->i2c_bus[0]);
 	cx25821_iounmap(dev);
 }
@@ -1385,8 +1298,6 @@ static int cx25821_initdev(struct pci_dev *pci_dev,
 		goto fail_unregister_device;
 	}
 
-	pr_info("Athena pci enable !\n");
-
 	err = cx25821_dev_setup(dev);
 	if (err) {
 		if (err == -EBUSY)
@@ -1445,10 +1356,6 @@ static void cx25821_finidev(struct pci_dev *pci_dev)
 	if (pci_dev->irq)
 		free_irq(pci_dev->irq, dev);
 
-	mutex_lock(&cx25821_devlist_mutex);
-	list_del(&dev->devlist);
-	mutex_unlock(&cx25821_devlist_mutex);
-
 	cx25821_dev_unregister(dev);
 	v4l2_device_unregister(v4l2_dev);
 	kfree(dev);
diff --git a/drivers/media/pci/cx25821/cx25821-gpio.c b/drivers/media/pci/cx25821/cx25821-gpio.c
index 29e43b03c85..95e8ddf6294 100644
--- a/drivers/media/pci/cx25821/cx25821-gpio.c
+++ b/drivers/media/pci/cx25821/cx25821-gpio.c
@@ -20,6 +20,7 @@
  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
+#include <linux/module.h>
 #include "cx25821.h"
 
 /********************* GPIO stuffs *********************/
diff --git a/drivers/media/pci/cx25821/cx25821-i2c.c b/drivers/media/pci/cx25821/cx25821-i2c.c
index a8dc945bbe1..dca37c7dba7 100644
--- a/drivers/media/pci/cx25821/cx25821-i2c.c
+++ b/drivers/media/pci/cx25821/cx25821-i2c.c
@@ -23,8 +23,9 @@
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
-#include "cx25821.h"
+#include <linux/module.h>
 #include <linux/i2c.h>
+#include "cx25821.h"
 
 static unsigned int i2c_debug;
 module_param(i2c_debug, int, 0644);
diff --git a/drivers/media/pci/cx25821/cx25821-medusa-video.c b/drivers/media/pci/cx25821/cx25821-medusa-video.c
index 6a92e5c70c2..22fa04415cc 100644
--- a/drivers/media/pci/cx25821/cx25821-medusa-video.c
+++ b/drivers/media/pci/cx25821/cx25821-medusa-video.c
@@ -94,8 +94,6 @@ static int medusa_initialize_ntsc(struct cx25821_dev *dev)
 	u32 value = 0;
 	u32 tmp = 0;
 
-	mutex_lock(&dev->lock);
-
 	for (i = 0; i < MAX_DECODERS; i++) {
 		/* set video format NTSC-M */
 		value = cx25821_i2c_read(&dev->i2c_bus[0],
@@ -222,8 +220,6 @@ static int medusa_initialize_ntsc(struct cx25821_dev *dev)
 	value |= 0x00080200;
 	ret_val = cx25821_i2c_write(&dev->i2c_bus[0], BYP_AB_CTRL, value);
 
-	mutex_unlock(&dev->lock);
-
 	return ret_val;
 }
 
@@ -265,8 +261,6 @@ static int medusa_initialize_pal(struct cx25821_dev *dev)
 	u32 value = 0;
 	u32 tmp = 0;
 
-	mutex_lock(&dev->lock);
-
 	for (i = 0; i < MAX_DECODERS; i++) {
 		/* set video format PAL-BDGHI */
 		value = cx25821_i2c_read(&dev->i2c_bus[0],
@@ -397,14 +391,12 @@ static int medusa_initialize_pal(struct cx25821_dev *dev)
 	value &= 0xFFF7FDFF;
 	ret_val = cx25821_i2c_write(&dev->i2c_bus[0], BYP_AB_CTRL, value);
 
-	mutex_unlock(&dev->lock);
-
 	return ret_val;
 }
 
 int medusa_set_videostandard(struct cx25821_dev *dev)
 {
-	int status = STATUS_SUCCESS;
+	int status = 0;
 	u32 value = 0, tmp = 0;
 
 	if (dev->tvnorm & V4L2_STD_PAL_BG || dev->tvnorm & V4L2_STD_PAL_DK)
@@ -434,8 +426,6 @@ void medusa_set_resolution(struct cx25821_dev *dev, int width,
 	u32 vscale = 0x0;
 	const int MAX_WIDTH = 720;
 
-	mutex_lock(&dev->lock);
-
 	/* validate the width */
 	if (width > MAX_WIDTH) {
 		pr_info("%s(): width %d > MAX_WIDTH %d ! resetting to MAX_WIDTH\n",
@@ -485,8 +475,6 @@ void medusa_set_resolution(struct cx25821_dev *dev, int width,
 		cx25821_i2c_write(&dev->i2c_bus[0],
 				VSCALE_CTRL + (0x200 * decoder), vscale);
 	}
-
-	mutex_unlock(&dev->lock);
 }
 
 static void medusa_set_decoderduration(struct cx25821_dev *dev, int decoder,
@@ -496,11 +484,8 @@ static void medusa_set_decoderduration(struct cx25821_dev *dev, int decoder,
 	u32 tmp = 0;
 	u32 disp_cnt_reg = DISP_AB_CNT;
 
-	mutex_lock(&dev->lock);
-
 	/* no support */
 	if (decoder < VDEC_A || decoder > VDEC_H) {
-		mutex_unlock(&dev->lock);
 		return;
 	}
 
@@ -535,8 +520,6 @@ static void medusa_set_decoderduration(struct cx25821_dev *dev, int decoder,
 	}
 
 	cx25821_i2c_write(&dev->i2c_bus[0], disp_cnt_reg, fld_cnt);
-
-	mutex_unlock(&dev->lock);
 }
 
 /* Map to Medusa register setting */
@@ -587,10 +570,8 @@ int medusa_set_brightness(struct cx25821_dev *dev, int brightness, int decoder)
 	int value = 0;
 	u32 val = 0, tmp = 0;
 
-	mutex_lock(&dev->lock);
 	if ((brightness > VIDEO_PROCAMP_MAX) ||
 	    (brightness < VIDEO_PROCAMP_MIN)) {
-		mutex_unlock(&dev->lock);
 		return -1;
 	}
 	ret_val = mapM(VIDEO_PROCAMP_MIN, VIDEO_PROCAMP_MAX, brightness,
@@ -601,7 +582,6 @@ int medusa_set_brightness(struct cx25821_dev *dev, int brightness, int decoder)
 	val &= 0xFFFFFF00;
 	ret_val |= cx25821_i2c_write(&dev->i2c_bus[0],
 			VDEC_A_BRITE_CTRL + (0x200 * decoder), val | value);
-	mutex_unlock(&dev->lock);
 	return ret_val;
 }
 
@@ -611,10 +591,7 @@ int medusa_set_contrast(struct cx25821_dev *dev, int contrast, int decoder)
 	int value = 0;
 	u32 val = 0, tmp = 0;
 
-	mutex_lock(&dev->lock);
-
 	if ((contrast > VIDEO_PROCAMP_MAX) || (contrast < VIDEO_PROCAMP_MIN)) {
-		mutex_unlock(&dev->lock);
 		return -1;
 	}
 
@@ -626,7 +603,6 @@ int medusa_set_contrast(struct cx25821_dev *dev, int contrast, int decoder)
 	ret_val |= cx25821_i2c_write(&dev->i2c_bus[0],
 			VDEC_A_CNTRST_CTRL + (0x200 * decoder), val | value);
 
-	mutex_unlock(&dev->lock);
 	return ret_val;
 }
 
@@ -636,10 +612,7 @@ int medusa_set_hue(struct cx25821_dev *dev, int hue, int decoder)
 	int value = 0;
 	u32 val = 0, tmp = 0;
 
-	mutex_lock(&dev->lock);
-
 	if ((hue > VIDEO_PROCAMP_MAX) || (hue < VIDEO_PROCAMP_MIN)) {
-		mutex_unlock(&dev->lock);
 		return -1;
 	}
 
@@ -654,7 +627,6 @@ int medusa_set_hue(struct cx25821_dev *dev, int hue, int decoder)
 	ret_val |= cx25821_i2c_write(&dev->i2c_bus[0],
 			VDEC_A_HUE_CTRL + (0x200 * decoder), val | value);
 
-	mutex_unlock(&dev->lock);
 	return ret_val;
 }
 
@@ -664,11 +636,8 @@ int medusa_set_saturation(struct cx25821_dev *dev, int saturation, int decoder)
 	int value = 0;
 	u32 val = 0, tmp = 0;
 
-	mutex_lock(&dev->lock);
-
 	if ((saturation > VIDEO_PROCAMP_MAX) ||
 	    (saturation < VIDEO_PROCAMP_MIN)) {
-		mutex_unlock(&dev->lock);
 		return -1;
 	}
 
@@ -687,7 +656,6 @@ int medusa_set_saturation(struct cx25821_dev *dev, int saturation, int decoder)
 	ret_val |= cx25821_i2c_write(&dev->i2c_bus[0],
 			VDEC_A_VSAT_CTRL + (0x200 * decoder), val | value);
 
-	mutex_unlock(&dev->lock);
 	return ret_val;
 }
 
@@ -699,8 +667,6 @@ int medusa_video_init(struct cx25821_dev *dev)
 	int ret_val = 0;
 	int i = 0;
 
-	mutex_lock(&dev->lock);
-
 	_num_decoders = dev->_max_num_decoders;
 
 	/* disable Auto source selection on all video decoders */
@@ -719,13 +685,9 @@ int medusa_video_init(struct cx25821_dev *dev)
 	if (ret_val < 0)
 		goto error;
 
-	mutex_unlock(&dev->lock);
-
 	for (i = 0; i < _num_decoders; i++)
 		medusa_set_decoderduration(dev, i, _display_field_cnt[i]);
 
-	mutex_lock(&dev->lock);
-
 	/* Select monitor as DENC A input, power up the DAC */
 	value = cx25821_i2c_read(&dev->i2c_bus[0], DENC_AB_CTRL, &tmp);
 	value &= 0xFF70FF70;
@@ -774,14 +736,8 @@ int medusa_video_init(struct cx25821_dev *dev)
 	if (ret_val < 0)
 		goto error;
 
-
-	mutex_unlock(&dev->lock);
-
 	ret_val = medusa_set_videostandard(dev);
 
-	return ret_val;
-
 error:
-	mutex_unlock(&dev->lock);
 	return ret_val;
 }
diff --git a/drivers/media/pci/cx25821/cx25821-video-upstream-ch2.c b/drivers/media/pci/cx25821/cx25821-video-upstream-ch2.c
deleted file mode 100644
index cf2723c7197..00000000000
--- a/drivers/media/pci/cx25821/cx25821-video-upstream-ch2.c
+++ /dev/null
@@ -1,800 +0,0 @@
-/*
- *  Driver for the Conexant CX25821 PCIe bridge
- *
- *  Copyright (C) 2009 Conexant Systems Inc.
- *  Authors  <hiep.huynh@conexant.com>, <shu.lin@conexant.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., 675 Mass Ave, Cambridge, MA 02139, USA.
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include "cx25821-video.h"
-#include "cx25821-video-upstream-ch2.h"
-
-#include <linux/fs.h>
-#include <linux/errno.h>
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/module.h>
-#include <linux/syscalls.h>
-#include <linux/file.h>
-#include <linux/fcntl.h>
-#include <linux/slab.h>
-#include <linux/uaccess.h>
-
-MODULE_DESCRIPTION("v4l2 driver module for cx25821 based TV cards");
-MODULE_AUTHOR("Hiep Huynh <hiep.huynh@conexant.com>");
-MODULE_LICENSE("GPL");
-
-static int _intr_msk = FLD_VID_SRC_RISC1 | FLD_VID_SRC_UF | FLD_VID_SRC_SYNC |
-			FLD_VID_SRC_OPC_ERR;
-
-static __le32 *cx25821_update_riscprogram_ch2(struct cx25821_dev *dev,
-					      __le32 *rp, unsigned int offset,
-					      unsigned int bpl, u32 sync_line,
-					      unsigned int lines,
-					      int fifo_enable, int field_type)
-{
-	unsigned int line, i;
-	int dist_betwn_starts = bpl * 2;
-
-	*(rp++) = cpu_to_le32(RISC_RESYNC | sync_line);
-
-	if (USE_RISC_NOOP_VIDEO) {
-		for (i = 0; i < NUM_NO_OPS; i++)
-			*(rp++) = cpu_to_le32(RISC_NOOP);
-	}
-
-	/* scan lines */
-	for (line = 0; line < lines; line++) {
-		*(rp++) = cpu_to_le32(RISC_READ | RISC_SOL | RISC_EOL | bpl);
-		*(rp++) = cpu_to_le32(dev->_data_buf_phys_addr_ch2 + offset);
-		*(rp++) = cpu_to_le32(0);	/* bits 63-32 */
-
-		if ((lines <= NTSC_FIELD_HEIGHT) ||
-		    (line < (NTSC_FIELD_HEIGHT - 1)) || !(dev->_isNTSC_ch2)) {
-			offset += dist_betwn_starts;
-		}
-	}
-
-	return rp;
-}
-
-static __le32 *cx25821_risc_field_upstream_ch2(struct cx25821_dev *dev,
-					       __le32 *rp,
-					       dma_addr_t databuf_phys_addr,
-					       unsigned int offset,
-					       u32 sync_line, unsigned int bpl,
-					       unsigned int lines,
-					       int fifo_enable, int field_type)
-{
-	unsigned int line, i;
-	struct sram_channel *sram_ch =
-		dev->channels[dev->_channel2_upstream_select].sram_channels;
-	int dist_betwn_starts = bpl * 2;
-
-	/* sync instruction */
-	if (sync_line != NO_SYNC_LINE)
-		*(rp++) = cpu_to_le32(RISC_RESYNC | sync_line);
-
-	if (USE_RISC_NOOP_VIDEO) {
-		for (i = 0; i < NUM_NO_OPS; i++)
-			*(rp++) = cpu_to_le32(RISC_NOOP);
-	}
-
-	/* scan lines */
-	for (line = 0; line < lines; line++) {
-		*(rp++) = cpu_to_le32(RISC_READ | RISC_SOL | RISC_EOL | bpl);
-		*(rp++) = cpu_to_le32(databuf_phys_addr + offset);
-		*(rp++) = cpu_to_le32(0);	/* bits 63-32 */
-
-		if ((lines <= NTSC_FIELD_HEIGHT) ||
-		    (line < (NTSC_FIELD_HEIGHT - 1)) || !(dev->_isNTSC_ch2)) {
-			offset += dist_betwn_starts;
-		}
-
-	       /*
-		 check if we need to enable the FIFO after the first 4 lines
-		  For the upstream video channel, the risc engine will enable
-		  the FIFO.
-	       */
-		if (fifo_enable && line == 3) {
-			*(rp++) = RISC_WRITECR;
-			*(rp++) = sram_ch->dma_ctl;
-			*(rp++) = FLD_VID_FIFO_EN;
-			*(rp++) = 0x00000001;
-		}
-	}
-
-	return rp;
-}
-
-static int cx25821_risc_buffer_upstream_ch2(struct cx25821_dev *dev,
-					    struct pci_dev *pci,
-					    unsigned int top_offset,
-					    unsigned int bpl,
-					    unsigned int lines)
-{
-	__le32 *rp;
-	int fifo_enable = 0;
-	int singlefield_lines = lines >> 1; /*get line count for single field */
-	int odd_num_lines = singlefield_lines;
-	int frame = 0;
-	int frame_size = 0;
-	int databuf_offset = 0;
-	int risc_program_size = 0;
-	int risc_flag = RISC_CNT_RESET;
-	unsigned int bottom_offset = bpl;
-	dma_addr_t risc_phys_jump_addr;
-
-	if (dev->_isNTSC_ch2) {
-		odd_num_lines = singlefield_lines + 1;
-		risc_program_size = FRAME1_VID_PROG_SIZE;
-		if (bpl == Y411_LINE_SZ)
-			frame_size = FRAME_SIZE_NTSC_Y411;
-		else
-			frame_size = FRAME_SIZE_NTSC_Y422;
-	} else {
-		risc_program_size = PAL_VID_PROG_SIZE;
-		if (bpl == Y411_LINE_SZ)
-			frame_size = FRAME_SIZE_PAL_Y411;
-		else
-			frame_size = FRAME_SIZE_PAL_Y422;
-	}
-
-	/* Virtual address of Risc buffer program */
-	rp = dev->_dma_virt_addr_ch2;
-
-	for (frame = 0; frame < NUM_FRAMES; frame++) {
-		databuf_offset = frame_size * frame;
-
-		if (UNSET != top_offset) {
-			fifo_enable = (frame == 0) ? FIFO_ENABLE : FIFO_DISABLE;
-			rp = cx25821_risc_field_upstream_ch2(dev, rp,
-				dev->_data_buf_phys_addr_ch2 + databuf_offset,
-				top_offset, 0, bpl, odd_num_lines, fifo_enable,
-				ODD_FIELD);
-		}
-
-		fifo_enable = FIFO_DISABLE;
-
-		/* Even field */
-		rp = cx25821_risc_field_upstream_ch2(dev, rp,
-				dev->_data_buf_phys_addr_ch2 + databuf_offset,
-				bottom_offset, 0x200, bpl, singlefield_lines,
-				fifo_enable, EVEN_FIELD);
-
-		if (frame == 0) {
-			risc_flag = RISC_CNT_RESET;
-			risc_phys_jump_addr = dev->_dma_phys_start_addr_ch2 +
-					risc_program_size;
-		} else {
-			risc_flag = RISC_CNT_INC;
-			risc_phys_jump_addr = dev->_dma_phys_start_addr_ch2;
-		}
-
-	       /*
-		* Loop to 2ndFrameRISC or to Start of
-		* Risc program & generate IRQ
-		*/
-		*(rp++) = cpu_to_le32(RISC_JUMP | RISC_IRQ1 | risc_flag);
-		*(rp++) = cpu_to_le32(risc_phys_jump_addr);
-		*(rp++) = cpu_to_le32(0);
-	}
-
-	return 0;
-}
-
-void cx25821_stop_upstream_video_ch2(struct cx25821_dev *dev)
-{
-	struct sram_channel *sram_ch =
-		dev->channels[VID_UPSTREAM_SRAM_CHANNEL_J].sram_channels;
-	u32 tmp = 0;
-
-	if (!dev->_is_running_ch2) {
-		pr_info("No video file is currently running so return!\n");
-		return;
-	}
-	/* Disable RISC interrupts */
-	tmp = cx_read(sram_ch->int_msk);
-	cx_write(sram_ch->int_msk, tmp & ~_intr_msk);
-
-	/* Turn OFF risc and fifo */
-	tmp = cx_read(sram_ch->dma_ctl);
-	cx_write(sram_ch->dma_ctl, tmp & ~(FLD_VID_FIFO_EN | FLD_VID_RISC_EN));
-
-	/* Clear data buffer memory */
-	if (dev->_data_buf_virt_addr_ch2)
-		memset(dev->_data_buf_virt_addr_ch2, 0,
-		       dev->_data_buf_size_ch2);
-
-	dev->_is_running_ch2 = 0;
-	dev->_is_first_frame_ch2 = 0;
-	dev->_frame_count_ch2 = 0;
-	dev->_file_status_ch2 = END_OF_FILE;
-
-	kfree(dev->_irq_queues_ch2);
-	dev->_irq_queues_ch2 = NULL;
-
-	kfree(dev->_filename_ch2);
-
-	tmp = cx_read(VID_CH_MODE_SEL);
-	cx_write(VID_CH_MODE_SEL, tmp & 0xFFFFFE00);
-}
-
-void cx25821_free_mem_upstream_ch2(struct cx25821_dev *dev)
-{
-	if (dev->_is_running_ch2)
-		cx25821_stop_upstream_video_ch2(dev);
-
-	if (dev->_dma_virt_addr_ch2) {
-		pci_free_consistent(dev->pci, dev->_risc_size_ch2,
-				    dev->_dma_virt_addr_ch2,
-				    dev->_dma_phys_addr_ch2);
-		dev->_dma_virt_addr_ch2 = NULL;
-	}
-
-	if (dev->_data_buf_virt_addr_ch2) {
-		pci_free_consistent(dev->pci, dev->_data_buf_size_ch2,
-				    dev->_data_buf_virt_addr_ch2,
-				    dev->_data_buf_phys_addr_ch2);
-		dev->_data_buf_virt_addr_ch2 = NULL;
-	}
-}
-
-static int cx25821_get_frame_ch2(struct cx25821_dev *dev,
-				 struct sram_channel *sram_ch)
-{
-	struct file *myfile;
-	int frame_index_temp = dev->_frame_index_ch2;
-	int i = 0;
-	int line_size = (dev->_pixel_format_ch2 == PIXEL_FRMT_411) ?
-		Y411_LINE_SZ : Y422_LINE_SZ;
-	int frame_size = 0;
-	int frame_offset = 0;
-	ssize_t vfs_read_retval = 0;
-	char mybuf[line_size];
-	loff_t file_offset;
-	loff_t pos;
-	mm_segment_t old_fs;
-
-	if (dev->_file_status_ch2 == END_OF_FILE)
-		return 0;
-
-	if (dev->_isNTSC_ch2) {
-		frame_size = (line_size == Y411_LINE_SZ) ?
-			FRAME_SIZE_NTSC_Y411 : FRAME_SIZE_NTSC_Y422;
-	} else {
-		frame_size = (line_size == Y411_LINE_SZ) ?
-			FRAME_SIZE_PAL_Y411 : FRAME_SIZE_PAL_Y422;
-	}
-
-	frame_offset = (frame_index_temp > 0) ? frame_size : 0;
-	file_offset = dev->_frame_count_ch2 * frame_size;
-
-	myfile = filp_open(dev->_filename_ch2, O_RDONLY | O_LARGEFILE, 0);
-	if (IS_ERR(myfile)) {
-		const int open_errno = -PTR_ERR(myfile);
-		pr_err("%s(): ERROR opening file(%s) with errno = %d!\n",
-		       __func__, dev->_filename_ch2, open_errno);
-		return PTR_ERR(myfile);
-	} else {
-		if (!(myfile->f_op)) {
-			pr_err("%s(): File has no file operations registered!\n",
-			       __func__);
-			filp_close(myfile, NULL);
-			return -EIO;
-		}
-
-		if (!myfile->f_op->read) {
-			pr_err("%s(): File has no READ operations registered!\n",
-			       __func__);
-			filp_close(myfile, NULL);
-			return -EIO;
-		}
-
-		pos = myfile->f_pos;
-		old_fs = get_fs();
-		set_fs(KERNEL_DS);
-
-		for (i = 0; i < dev->_lines_count_ch2; i++) {
-			pos = file_offset;
-
-			vfs_read_retval = vfs_read(myfile, mybuf, line_size,
-					&pos);
-
-			if (vfs_read_retval > 0 && vfs_read_retval == line_size
-			    && dev->_data_buf_virt_addr_ch2 != NULL) {
-				memcpy((void *)(dev->_data_buf_virt_addr_ch2 +
-						frame_offset / 4), mybuf,
-						vfs_read_retval);
-			}
-
-			file_offset += vfs_read_retval;
-			frame_offset += vfs_read_retval;
-
-			if (vfs_read_retval < line_size) {
-				pr_info("Done: exit %s() since no more bytes to read from Video file\n",
-					__func__);
-				break;
-			}
-		}
-
-		if (i > 0)
-			dev->_frame_count_ch2++;
-
-		dev->_file_status_ch2 = (vfs_read_retval == line_size) ?
-			IN_PROGRESS : END_OF_FILE;
-
-		set_fs(old_fs);
-		filp_close(myfile, NULL);
-	}
-
-	return 0;
-}
-
-static void cx25821_vidups_handler_ch2(struct work_struct *work)
-{
-	struct cx25821_dev *dev = container_of(work, struct cx25821_dev,
-			_irq_work_entry_ch2);
-
-	if (!dev) {
-		pr_err("ERROR %s(): since container_of(work_struct) FAILED!\n",
-		       __func__);
-		return;
-	}
-
-	cx25821_get_frame_ch2(dev, dev->channels[dev->
-			_channel2_upstream_select].sram_channels);
-}
-
-static int cx25821_openfile_ch2(struct cx25821_dev *dev,
-				struct sram_channel *sram_ch)
-{
-	struct file *myfile;
-	int i = 0, j = 0;
-	int line_size = (dev->_pixel_format_ch2 == PIXEL_FRMT_411) ?
-		Y411_LINE_SZ : Y422_LINE_SZ;
-	ssize_t vfs_read_retval = 0;
-	char mybuf[line_size];
-	loff_t pos;
-	loff_t offset = (unsigned long)0;
-	mm_segment_t old_fs;
-
-	myfile = filp_open(dev->_filename_ch2, O_RDONLY | O_LARGEFILE, 0);
-
-	if (IS_ERR(myfile)) {
-		const int open_errno = -PTR_ERR(myfile);
-		pr_err("%s(): ERROR opening file(%s) with errno = %d!\n",
-		       __func__, dev->_filename_ch2, open_errno);
-		return PTR_ERR(myfile);
-	} else {
-		if (!(myfile->f_op)) {
-			pr_err("%s(): File has no file operations registered!\n",
-			       __func__);
-			filp_close(myfile, NULL);
-			return -EIO;
-		}
-
-		if (!myfile->f_op->read) {
-			pr_err("%s(): File has no READ operations registered!  Returning\n",
-			       __func__);
-			filp_close(myfile, NULL);
-			return -EIO;
-		}
-
-		pos = myfile->f_pos;
-		old_fs = get_fs();
-		set_fs(KERNEL_DS);
-
-		for (j = 0; j < NUM_FRAMES; j++) {
-			for (i = 0; i < dev->_lines_count_ch2; i++) {
-				pos = offset;
-
-				vfs_read_retval = vfs_read(myfile, mybuf,
-						line_size, &pos);
-
-				if (vfs_read_retval > 0 &&
-				    vfs_read_retval == line_size &&
-				    dev->_data_buf_virt_addr_ch2 != NULL) {
-					memcpy((void *)(dev->
-							_data_buf_virt_addr_ch2
-							+ offset / 4), mybuf,
-							vfs_read_retval);
-				}
-
-				offset += vfs_read_retval;
-
-				if (vfs_read_retval < line_size) {
-					pr_info("Done: exit %s() since no more bytes to read from Video file\n",
-						__func__);
-					break;
-				}
-			}
-
-			if (i > 0)
-				dev->_frame_count_ch2++;
-
-			if (vfs_read_retval < line_size)
-				break;
-		}
-
-		dev->_file_status_ch2 = (vfs_read_retval == line_size) ?
-			IN_PROGRESS : END_OF_FILE;
-
-		set_fs(old_fs);
-		myfile->f_pos = 0;
-		filp_close(myfile, NULL);
-	}
-
-	return 0;
-}
-
-static int cx25821_upstream_buffer_prepare_ch2(struct cx25821_dev *dev,
-					       struct sram_channel *sram_ch,
-					       int bpl)
-{
-	int ret = 0;
-	dma_addr_t dma_addr;
-	dma_addr_t data_dma_addr;
-
-	if (dev->_dma_virt_addr_ch2 != NULL) {
-		pci_free_consistent(dev->pci, dev->upstream_riscbuf_size_ch2,
-				    dev->_dma_virt_addr_ch2,
-				    dev->_dma_phys_addr_ch2);
-	}
-
-	dev->_dma_virt_addr_ch2 = pci_alloc_consistent(dev->pci,
-			dev->upstream_riscbuf_size_ch2, &dma_addr);
-	dev->_dma_virt_start_addr_ch2 = dev->_dma_virt_addr_ch2;
-	dev->_dma_phys_start_addr_ch2 = dma_addr;
-	dev->_dma_phys_addr_ch2 = dma_addr;
-	dev->_risc_size_ch2 = dev->upstream_riscbuf_size_ch2;
-
-	if (!dev->_dma_virt_addr_ch2) {
-		pr_err("FAILED to allocate memory for Risc buffer! Returning\n");
-		return -ENOMEM;
-	}
-
-	/* Iniitize at this address until n bytes to 0 */
-	memset(dev->_dma_virt_addr_ch2, 0, dev->_risc_size_ch2);
-
-	if (dev->_data_buf_virt_addr_ch2 != NULL) {
-		pci_free_consistent(dev->pci, dev->upstream_databuf_size_ch2,
-				    dev->_data_buf_virt_addr_ch2,
-				    dev->_data_buf_phys_addr_ch2);
-	}
-	/* For Video Data buffer allocation */
-	dev->_data_buf_virt_addr_ch2 = pci_alloc_consistent(dev->pci,
-			dev->upstream_databuf_size_ch2, &data_dma_addr);
-	dev->_data_buf_phys_addr_ch2 = data_dma_addr;
-	dev->_data_buf_size_ch2 = dev->upstream_databuf_size_ch2;
-
-	if (!dev->_data_buf_virt_addr_ch2) {
-		pr_err("FAILED to allocate memory for data buffer! Returning\n");
-		return -ENOMEM;
-	}
-
-	/* Initialize at this address until n bytes to 0 */
-	memset(dev->_data_buf_virt_addr_ch2, 0, dev->_data_buf_size_ch2);
-
-	ret = cx25821_openfile_ch2(dev, sram_ch);
-	if (ret < 0)
-		return ret;
-
-	/* Creating RISC programs */
-	ret = cx25821_risc_buffer_upstream_ch2(dev, dev->pci, 0, bpl,
-						dev->_lines_count_ch2);
-	if (ret < 0) {
-		pr_info("Failed creating Video Upstream Risc programs!\n");
-		goto error;
-	}
-
-	return 0;
-
-error:
-	return ret;
-}
-
-static int cx25821_video_upstream_irq_ch2(struct cx25821_dev *dev,
-					  int chan_num,
-					  u32 status)
-{
-	u32 int_msk_tmp;
-	struct sram_channel *channel = dev->channels[chan_num].sram_channels;
-	int singlefield_lines = NTSC_FIELD_HEIGHT;
-	int line_size_in_bytes = Y422_LINE_SZ;
-	int odd_risc_prog_size = 0;
-	dma_addr_t risc_phys_jump_addr;
-	__le32 *rp;
-
-	if (status & FLD_VID_SRC_RISC1) {
-		/* We should only process one program per call */
-		u32 prog_cnt = cx_read(channel->gpcnt);
-
-		/*
-		 *  Since we've identified our IRQ, clear our bits from the
-		 *  interrupt mask and interrupt status registers
-		 */
-		int_msk_tmp = cx_read(channel->int_msk);
-		cx_write(channel->int_msk, int_msk_tmp & ~_intr_msk);
-		cx_write(channel->int_stat, _intr_msk);
-
-		spin_lock(&dev->slock);
-
-		dev->_frame_index_ch2 = prog_cnt;
-
-		queue_work(dev->_irq_queues_ch2, &dev->_irq_work_entry_ch2);
-
-		if (dev->_is_first_frame_ch2) {
-			dev->_is_first_frame_ch2 = 0;
-
-			if (dev->_isNTSC_ch2) {
-				singlefield_lines += 1;
-				odd_risc_prog_size = ODD_FLD_NTSC_PROG_SIZE;
-			} else {
-				singlefield_lines = PAL_FIELD_HEIGHT;
-				odd_risc_prog_size = ODD_FLD_PAL_PROG_SIZE;
-			}
-
-			if (dev->_dma_virt_start_addr_ch2 != NULL) {
-				if (dev->_pixel_format_ch2 == PIXEL_FRMT_411)
-					line_size_in_bytes = Y411_LINE_SZ;
-				else
-					line_size_in_bytes = Y422_LINE_SZ;
-				risc_phys_jump_addr =
-					dev->_dma_phys_start_addr_ch2 +
-					odd_risc_prog_size;
-
-				rp = cx25821_update_riscprogram_ch2(dev,
-						dev->_dma_virt_start_addr_ch2,
-						TOP_OFFSET, line_size_in_bytes,
-						0x0, singlefield_lines,
-						FIFO_DISABLE, ODD_FIELD);
-
-			       /* Jump to Even Risc program of 1st Frame */
-				*(rp++) = cpu_to_le32(RISC_JUMP);
-				*(rp++) = cpu_to_le32(risc_phys_jump_addr);
-				*(rp++) = cpu_to_le32(0);
-			}
-		}
-
-		spin_unlock(&dev->slock);
-	}
-
-	if (dev->_file_status_ch2 == END_OF_FILE) {
-		pr_info("EOF Channel 2 Framecount = %d\n",
-			dev->_frame_count_ch2);
-		return -1;
-	}
-	/* ElSE, set the interrupt mask register, re-enable irq. */
-	int_msk_tmp = cx_read(channel->int_msk);
-	cx_write(channel->int_msk, int_msk_tmp |= _intr_msk);
-
-	return 0;
-}
-
-static irqreturn_t cx25821_upstream_irq_ch2(int irq, void *dev_id)
-{
-	struct cx25821_dev *dev = dev_id;
-	u32 vid_status;
-	int handled = 0;
-	int channel_num = 0;
-	struct sram_channel *sram_ch;
-
-	if (!dev)
-		return -1;
-
-	channel_num = VID_UPSTREAM_SRAM_CHANNEL_J;
-	sram_ch = dev->channels[channel_num].sram_channels;
-
-	vid_status = cx_read(sram_ch->int_stat);
-
-	/* Only deal with our interrupt */
-	if (vid_status)
-		handled = cx25821_video_upstream_irq_ch2(dev, channel_num,
-				vid_status);
-
-	if (handled < 0)
-		cx25821_stop_upstream_video_ch2(dev);
-	else
-		handled += handled;
-
-	return IRQ_RETVAL(handled);
-}
-
-static void cx25821_set_pixelengine_ch2(struct cx25821_dev *dev,
-					struct sram_channel *ch, int pix_format)
-{
-	int width = WIDTH_D1;
-	int height = dev->_lines_count_ch2;
-	int num_lines, odd_num_lines;
-	u32 value;
-	int vip_mode = PIXEL_ENGINE_VIP1;
-
-	value = ((pix_format & 0x3) << 12) | (vip_mode & 0x7);
-	value &= 0xFFFFFFEF;
-	value |= dev->_isNTSC_ch2 ? 0 : 0x10;
-	cx_write(ch->vid_fmt_ctl, value);
-
-	/*
-	 *  set number of active pixels in each line. Default is 720
-	 * pixels in both NTSC and PAL format
-	 */
-	cx_write(ch->vid_active_ctl1, width);
-
-	num_lines = (height / 2) & 0x3FF;
-	odd_num_lines = num_lines;
-
-	if (dev->_isNTSC_ch2)
-		odd_num_lines += 1;
-
-	value = (num_lines << 16) | odd_num_lines;
-
-	/* set number of active lines in field 0 (top) and field 1 (bottom) */
-	cx_write(ch->vid_active_ctl2, value);
-
-	cx_write(ch->vid_cdt_size, VID_CDT_SIZE >> 3);
-}
-
-static int cx25821_start_video_dma_upstream_ch2(struct cx25821_dev *dev,
-						struct sram_channel *sram_ch)
-{
-	u32 tmp = 0;
-	int err = 0;
-
-	/*
-	 *  656/VIP SRC Upstream Channel I & J and 7 - Host Bus Interface
-	 * for channel A-C
-	 */
-	tmp = cx_read(VID_CH_MODE_SEL);
-	cx_write(VID_CH_MODE_SEL, tmp | 0x1B0001FF);
-
-	/*
-	 *  Set the physical start address of the RISC program in the initial
-	 *  program counter(IPC) member of the cmds.
-	 */
-	cx_write(sram_ch->cmds_start + 0, dev->_dma_phys_addr_ch2);
-	cx_write(sram_ch->cmds_start + 4, 0); /* Risc IPC High 64 bits 63-32 */
-
-	/* reset counter */
-	cx_write(sram_ch->gpcnt_ctl, 3);
-
-	/* Clear our bits from the interrupt status register. */
-	cx_write(sram_ch->int_stat, _intr_msk);
-
-	/* Set the interrupt mask register, enable irq. */
-	cx_set(PCI_INT_MSK, cx_read(PCI_INT_MSK) | (1 << sram_ch->irq_bit));
-	tmp = cx_read(sram_ch->int_msk);
-	cx_write(sram_ch->int_msk, tmp |= _intr_msk);
-
-	err = request_irq(dev->pci->irq, cx25821_upstream_irq_ch2,
-			IRQF_SHARED, dev->name, dev);
-	if (err < 0) {
-		pr_err("%s: can't get upstream IRQ %d\n",
-		       dev->name, dev->pci->irq);
-		goto fail_irq;
-	}
-	/* Start the DMA  engine */
-	tmp = cx_read(sram_ch->dma_ctl);
-	cx_set(sram_ch->dma_ctl, tmp | FLD_VID_RISC_EN);
-
-	dev->_is_running_ch2 = 1;
-	dev->_is_first_frame_ch2 = 1;
-
-	return 0;
-
-fail_irq:
-	cx25821_dev_unregister(dev);
-	return err;
-}
-
-int cx25821_vidupstream_init_ch2(struct cx25821_dev *dev, int channel_select,
-				 int pixel_format)
-{
-	struct sram_channel *sram_ch;
-	u32 tmp;
-	int err = 0;
-	int data_frame_size = 0;
-	int risc_buffer_size = 0;
-
-	if (dev->_is_running_ch2) {
-		pr_info("Video Channel is still running so return!\n");
-		return 0;
-	}
-
-	dev->_channel2_upstream_select = channel_select;
-	sram_ch = dev->channels[channel_select].sram_channels;
-
-	INIT_WORK(&dev->_irq_work_entry_ch2, cx25821_vidups_handler_ch2);
-	dev->_irq_queues_ch2 =
-	    create_singlethread_workqueue("cx25821_workqueue2");
-
-	if (!dev->_irq_queues_ch2) {
-		pr_err("create_singlethread_workqueue() for Video FAILED!\n");
-		return -ENOMEM;
-	}
-	/*
-	 * 656/VIP SRC Upstream Channel I & J and 7 -
-	 * Host Bus Interface for channel A-C
-	 */
-	tmp = cx_read(VID_CH_MODE_SEL);
-	cx_write(VID_CH_MODE_SEL, tmp | 0x1B0001FF);
-
-	dev->_is_running_ch2 = 0;
-	dev->_frame_count_ch2 = 0;
-	dev->_file_status_ch2 = RESET_STATUS;
-	dev->_lines_count_ch2 = dev->_isNTSC_ch2 ? 480 : 576;
-	dev->_pixel_format_ch2 = pixel_format;
-	dev->_line_size_ch2 = (dev->_pixel_format_ch2 == PIXEL_FRMT_422) ?
-		(WIDTH_D1 * 2) : (WIDTH_D1 * 3) / 2;
-	data_frame_size = dev->_isNTSC_ch2 ? NTSC_DATA_BUF_SZ : PAL_DATA_BUF_SZ;
-	risc_buffer_size = dev->_isNTSC_ch2 ?
-		NTSC_RISC_BUF_SIZE : PAL_RISC_BUF_SIZE;
-
-	if (dev->input_filename_ch2)
-		dev->_filename_ch2 = kstrdup(dev->input_filename_ch2,
-								GFP_KERNEL);
-	else
-		dev->_filename_ch2 = kstrdup(dev->_defaultname_ch2,
-								GFP_KERNEL);
-
-	if (!dev->_filename_ch2) {
-		err = -ENOENT;
-		goto error;
-	}
-
-	/* Default if filename is empty string */
-	if (strcmp(dev->_filename_ch2, "") == 0) {
-		if (dev->_isNTSC_ch2) {
-			dev->_filename_ch2 = (dev->_pixel_format_ch2 ==
-				PIXEL_FRMT_411) ? "/root/vid411.yuv" :
-				"/root/vidtest.yuv";
-		} else {
-			dev->_filename_ch2 = (dev->_pixel_format_ch2 ==
-				PIXEL_FRMT_411) ? "/root/pal411.yuv" :
-				"/root/pal422.yuv";
-		}
-	}
-
-	err = cx25821_sram_channel_setup_upstream(dev, sram_ch,
-						dev->_line_size_ch2, 0);
-
-	/* setup fifo + format */
-	cx25821_set_pixelengine_ch2(dev, sram_ch, dev->_pixel_format_ch2);
-
-	dev->upstream_riscbuf_size_ch2 = risc_buffer_size * 2;
-	dev->upstream_databuf_size_ch2 = data_frame_size * 2;
-
-	/* Allocating buffers and prepare RISC program */
-	err = cx25821_upstream_buffer_prepare_ch2(dev, sram_ch,
-						dev->_line_size_ch2);
-	if (err < 0) {
-		pr_err("%s: Failed to set up Video upstream buffers!\n",
-		       dev->name);
-		goto error;
-	}
-
-	cx25821_start_video_dma_upstream_ch2(dev, sram_ch);
-
-	return 0;
-
-error:
-	cx25821_dev_unregister(dev);
-
-	return err;
-}
diff --git a/drivers/media/pci/cx25821/cx25821-video-upstream-ch2.h b/drivers/media/pci/cx25821/cx25821-video-upstream-ch2.h
deleted file mode 100644
index d42dab59b66..00000000000
--- a/drivers/media/pci/cx25821/cx25821-video-upstream-ch2.h
+++ /dev/null
@@ -1,138 +0,0 @@
-/*
- *  Driver for the Conexant CX25821 PCIe bridge
- *
- *  Copyright (C) 2009 Conexant Systems Inc.
- *  Authors  <hiep.huynh@conexant.com>, <shu.lin@conexant.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., 675 Mass Ave, Cambridge, MA 02139, USA.
- */
-
-#include <linux/mutex.h>
-#include <linux/workqueue.h>
-
-#define OPEN_FILE_1           0
-#define NUM_PROGS             8
-#define NUM_FRAMES            2
-#define ODD_FIELD             0
-#define EVEN_FIELD            1
-#define TOP_OFFSET            0
-#define FIFO_DISABLE          0
-#define FIFO_ENABLE           1
-#define TEST_FRAMES           5
-#define END_OF_FILE           0
-#define IN_PROGRESS           1
-#define RESET_STATUS          -1
-#define NUM_NO_OPS            5
-
-/* PAL and NTSC line sizes and number of lines. */
-#define WIDTH_D1              720
-#define NTSC_LINES_PER_FRAME  480
-#define PAL_LINES_PER_FRAME   576
-#define PAL_LINE_SZ           1440
-#define Y422_LINE_SZ          1440
-#define Y411_LINE_SZ          1080
-#define NTSC_FIELD_HEIGHT     240
-#define NTSC_ODD_FLD_LINES    241
-#define PAL_FIELD_HEIGHT      288
-
-#define FRAME_SIZE_NTSC_Y422    (NTSC_LINES_PER_FRAME * Y422_LINE_SZ)
-#define FRAME_SIZE_NTSC_Y411    (NTSC_LINES_PER_FRAME * Y411_LINE_SZ)
-#define FRAME_SIZE_PAL_Y422     (PAL_LINES_PER_FRAME * Y422_LINE_SZ)
-#define FRAME_SIZE_PAL_Y411     (PAL_LINES_PER_FRAME * Y411_LINE_SZ)
-
-#define NTSC_DATA_BUF_SZ        (Y422_LINE_SZ * NTSC_LINES_PER_FRAME)
-#define PAL_DATA_BUF_SZ         (Y422_LINE_SZ * PAL_LINES_PER_FRAME)
-
-#define RISC_WRITECR_INSTRUCTION_SIZE   16
-#define RISC_SYNC_INSTRUCTION_SIZE      4
-#define JUMP_INSTRUCTION_SIZE           12
-#define MAXSIZE_NO_OPS                  36
-#define DWORD_SIZE                      4
-
-#define USE_RISC_NOOP_VIDEO   1
-
-#ifdef USE_RISC_NOOP_VIDEO
-#define PAL_US_VID_PROG_SIZE						\
-	(PAL_FIELD_HEIGHT * 3 * DWORD_SIZE +				\
-	 RISC_WRITECR_INSTRUCTION_SIZE + RISC_SYNC_INSTRUCTION_SIZE +	\
-	 NUM_NO_OPS * DWORD_SIZE)
-
-#define PAL_RISC_BUF_SIZE         (2 * PAL_US_VID_PROG_SIZE)
-
-#define PAL_VID_PROG_SIZE						\
-	((PAL_FIELD_HEIGHT * 2) * 3 * DWORD_SIZE +			\
-	 2 * RISC_SYNC_INSTRUCTION_SIZE + RISC_WRITECR_INSTRUCTION_SIZE + \
-	 JUMP_INSTRUCTION_SIZE + 2 * NUM_NO_OPS * DWORD_SIZE)
-
-#define ODD_FLD_PAL_PROG_SIZE						\
-	(PAL_FIELD_HEIGHT * 3 * DWORD_SIZE +				\
-	 RISC_SYNC_INSTRUCTION_SIZE + RISC_WRITECR_INSTRUCTION_SIZE +	\
-	 NUM_NO_OPS * DWORD_SIZE)
-
-#define NTSC_US_VID_PROG_SIZE						\
-	((NTSC_ODD_FLD_LINES + 1) * 3 * DWORD_SIZE +			\
-	 RISC_WRITECR_INSTRUCTION_SIZE + JUMP_INSTRUCTION_SIZE +	\
-	 NUM_NO_OPS * DWORD_SIZE)
-
-#define NTSC_RISC_BUF_SIZE						\
-	(2 * (RISC_SYNC_INSTRUCTION_SIZE + NTSC_US_VID_PROG_SIZE))
-
-#define FRAME1_VID_PROG_SIZE						\
-	((NTSC_ODD_FLD_LINES + NTSC_FIELD_HEIGHT) *			\
-	 3 * DWORD_SIZE + 2 * RISC_SYNC_INSTRUCTION_SIZE +		\
-	 RISC_WRITECR_INSTRUCTION_SIZE + JUMP_INSTRUCTION_SIZE +	\
-	 2 * NUM_NO_OPS * DWORD_SIZE)
-
-#define ODD_FLD_NTSC_PROG_SIZE						\
-	(NTSC_ODD_FLD_LINES * 3 * DWORD_SIZE +				\
-	 RISC_SYNC_INSTRUCTION_SIZE + RISC_WRITECR_INSTRUCTION_SIZE +	\
-	 NUM_NO_OPS * DWORD_SIZE)
-#endif
-
-#ifndef USE_RISC_NOOP_VIDEO
-#define PAL_US_VID_PROG_SIZE						\
-	((PAL_FIELD_HEIGHT + 1) * 3 * DWORD_SIZE +			\
-	 RISC_WRITECR_INSTRUCTION_SIZE)
-
-#define PAL_RISC_BUF_SIZE						\
-	(2 * (RISC_SYNC_INSTRUCTION_SIZE + PAL_US_VID_PROG_SIZE))
-
-#define PAL_VID_PROG_SIZE						\
-	((PAL_FIELD_HEIGHT * 2) * 3 * DWORD_SIZE +			\
-	 2 * RISC_SYNC_INSTRUCTION_SIZE + RISC_WRITECR_INSTRUCTION_SIZE + \
-	 JUMP_INSTRUCTION_SIZE)
-
-#define ODD_FLD_PAL_PROG_SIZE						\
-	(PAL_FIELD_HEIGHT * 3 * DWORD_SIZE +				\
-	 RISC_SYNC_INSTRUCTION_SIZE + RISC_WRITECR_INSTRUCTION_SIZE)
-
-#define ODD_FLD_NTSC_PROG_SIZE						\
-	(NTSC_ODD_FLD_LINES * 3 * DWORD_SIZE +				\
-	 RISC_SYNC_INSTRUCTION_SIZE + RISC_WRITECR_INSTRUCTION_SIZE)
-
-#define NTSC_US_VID_PROG_SIZE						\
-	((NTSC_ODD_FLD_LINES + 1) * 3 * DWORD_SIZE +			\
-	 RISC_WRITECR_INSTRUCTION_SIZE + JUMP_INSTRUCTION_SIZE)
-
-#define NTSC_RISC_BUF_SIZE						\
-	(2 * (RISC_SYNC_INSTRUCTION_SIZE + NTSC_US_VID_PROG_SIZE))
-
-#define FRAME1_VID_PROG_SIZE						\
-	((NTSC_ODD_FLD_LINES + NTSC_FIELD_HEIGHT) *			\
-	 3 * DWORD_SIZE + 2 * RISC_SYNC_INSTRUCTION_SIZE +		\
-	 RISC_WRITECR_INSTRUCTION_SIZE + JUMP_INSTRUCTION_SIZE)
-
-#endif
diff --git a/drivers/media/pci/cx25821/cx25821-video-upstream.c b/drivers/media/pci/cx25821/cx25821-video-upstream.c
index 7fc97110d97..88ffef410c5 100644
--- a/drivers/media/pci/cx25821/cx25821-video-upstream.c
+++ b/drivers/media/pci/cx25821/cx25821-video-upstream.c
@@ -25,16 +25,11 @@
 #include "cx25821-video.h"
 #include "cx25821-video-upstream.h"
 
-#include <linux/fs.h>
 #include <linux/errno.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/module.h>
-#include <linux/syscalls.h>
-#include <linux/file.h>
-#include <linux/fcntl.h>
 #include <linux/slab.h>
-#include <linux/uaccess.h>
 
 MODULE_DESCRIPTION("v4l2 driver module for cx25821 based TV cards");
 MODULE_AUTHOR("Hiep Huynh <hiep.huynh@conexant.com>");
@@ -44,7 +39,7 @@ static int _intr_msk = FLD_VID_SRC_RISC1 | FLD_VID_SRC_UF | FLD_VID_SRC_SYNC |
 			FLD_VID_SRC_OPC_ERR;
 
 int cx25821_sram_channel_setup_upstream(struct cx25821_dev *dev,
-					struct sram_channel *ch,
+					const struct sram_channel *ch,
 					unsigned int bpl, u32 risc)
 {
 	unsigned int i, lines;
@@ -97,12 +92,13 @@ int cx25821_sram_channel_setup_upstream(struct cx25821_dev *dev,
 	return 0;
 }
 
-static __le32 *cx25821_update_riscprogram(struct cx25821_dev *dev,
+static __le32 *cx25821_update_riscprogram(struct cx25821_channel *chan,
 					  __le32 *rp, unsigned int offset,
 					  unsigned int bpl, u32 sync_line,
 					  unsigned int lines, int fifo_enable,
 					  int field_type)
 {
+	struct cx25821_video_out_data *out = chan->out;
 	unsigned int line, i;
 	int dist_betwn_starts = bpl * 2;
 
@@ -116,11 +112,11 @@ static __le32 *cx25821_update_riscprogram(struct cx25821_dev *dev,
 	/* scan lines */
 	for (line = 0; line < lines; line++) {
 		*(rp++) = cpu_to_le32(RISC_READ | RISC_SOL | RISC_EOL | bpl);
-		*(rp++) = cpu_to_le32(dev->_data_buf_phys_addr + offset);
+		*(rp++) = cpu_to_le32(out->_data_buf_phys_addr + offset);
 		*(rp++) = cpu_to_le32(0);	/* bits 63-32 */
 
 		if ((lines <= NTSC_FIELD_HEIGHT)
-		    || (line < (NTSC_FIELD_HEIGHT - 1)) || !(dev->_isNTSC)) {
+		    || (line < (NTSC_FIELD_HEIGHT - 1)) || !(out->is_60hz)) {
 			offset += dist_betwn_starts;
 		}
 	}
@@ -128,15 +124,15 @@ static __le32 *cx25821_update_riscprogram(struct cx25821_dev *dev,
 	return rp;
 }
 
-static __le32 *cx25821_risc_field_upstream(struct cx25821_dev *dev, __le32 * rp,
+static __le32 *cx25821_risc_field_upstream(struct cx25821_channel *chan, __le32 *rp,
 					   dma_addr_t databuf_phys_addr,
 					   unsigned int offset, u32 sync_line,
 					   unsigned int bpl, unsigned int lines,
 					   int fifo_enable, int field_type)
 {
+	struct cx25821_video_out_data *out = chan->out;
 	unsigned int line, i;
-	struct sram_channel *sram_ch =
-		dev->channels[dev->_channel_upstream_select].sram_channels;
+	const struct sram_channel *sram_ch = chan->sram_channels;
 	int dist_betwn_starts = bpl * 2;
 
 	/* sync instruction */
@@ -155,7 +151,7 @@ static __le32 *cx25821_risc_field_upstream(struct cx25821_dev *dev, __le32 * rp,
 		*(rp++) = cpu_to_le32(0);	/* bits 63-32 */
 
 		if ((lines <= NTSC_FIELD_HEIGHT)
-		    || (line < (NTSC_FIELD_HEIGHT - 1)) || !(dev->_isNTSC))
+		    || (line < (NTSC_FIELD_HEIGHT - 1)) || !(out->is_60hz))
 			/* to skip the other field line */
 			offset += dist_betwn_starts;
 
@@ -173,11 +169,12 @@ static __le32 *cx25821_risc_field_upstream(struct cx25821_dev *dev, __le32 * rp,
 	return rp;
 }
 
-static int cx25821_risc_buffer_upstream(struct cx25821_dev *dev,
+static int cx25821_risc_buffer_upstream(struct cx25821_channel *chan,
 					struct pci_dev *pci,
 					unsigned int top_offset,
 					unsigned int bpl, unsigned int lines)
 {
+	struct cx25821_video_out_data *out = chan->out;
 	__le32 *rp;
 	int fifo_enable = 0;
 	/* get line count for single field */
@@ -191,7 +188,7 @@ static int cx25821_risc_buffer_upstream(struct cx25821_dev *dev,
 	unsigned int bottom_offset = bpl;
 	dma_addr_t risc_phys_jump_addr;
 
-	if (dev->_isNTSC) {
+	if (out->is_60hz) {
 		odd_num_lines = singlefield_lines + 1;
 		risc_program_size = FRAME1_VID_PROG_SIZE;
 		frame_size = (bpl == Y411_LINE_SZ) ?
@@ -203,15 +200,15 @@ static int cx25821_risc_buffer_upstream(struct cx25821_dev *dev,
 	}
 
 	/* Virtual address of Risc buffer program */
-	rp = dev->_dma_virt_addr;
+	rp = out->_dma_virt_addr;
 
 	for (frame = 0; frame < NUM_FRAMES; frame++) {
 		databuf_offset = frame_size * frame;
 
 		if (UNSET != top_offset) {
 			fifo_enable = (frame == 0) ? FIFO_ENABLE : FIFO_DISABLE;
-			rp = cx25821_risc_field_upstream(dev, rp,
-					dev->_data_buf_phys_addr +
+			rp = cx25821_risc_field_upstream(chan, rp,
+					out->_data_buf_phys_addr +
 					databuf_offset, top_offset, 0, bpl,
 					odd_num_lines, fifo_enable, ODD_FIELD);
 		}
@@ -219,18 +216,18 @@ static int cx25821_risc_buffer_upstream(struct cx25821_dev *dev,
 		fifo_enable = FIFO_DISABLE;
 
 		/* Even Field */
-		rp = cx25821_risc_field_upstream(dev, rp,
-						 dev->_data_buf_phys_addr +
+		rp = cx25821_risc_field_upstream(chan, rp,
+						 out->_data_buf_phys_addr +
 						 databuf_offset, bottom_offset,
 						 0x200, bpl, singlefield_lines,
 						 fifo_enable, EVEN_FIELD);
 
 		if (frame == 0) {
 			risc_flag = RISC_CNT_RESET;
-			risc_phys_jump_addr = dev->_dma_phys_start_addr +
+			risc_phys_jump_addr = out->_dma_phys_start_addr +
 				risc_program_size;
 		} else {
-			risc_phys_jump_addr = dev->_dma_phys_start_addr;
+			risc_phys_jump_addr = out->_dma_phys_start_addr;
 			risc_flag = RISC_CNT_INC;
 		}
 
@@ -245,16 +242,21 @@ static int cx25821_risc_buffer_upstream(struct cx25821_dev *dev,
 	return 0;
 }
 
-void cx25821_stop_upstream_video_ch1(struct cx25821_dev *dev)
+void cx25821_stop_upstream_video(struct cx25821_channel *chan)
 {
-	struct sram_channel *sram_ch =
-		dev->channels[VID_UPSTREAM_SRAM_CHANNEL_I].sram_channels;
+	struct cx25821_video_out_data *out = chan->out;
+	struct cx25821_dev *dev = chan->dev;
+	const struct sram_channel *sram_ch = chan->sram_channels;
 	u32 tmp = 0;
 
-	if (!dev->_is_running) {
+	if (!out->_is_running) {
 		pr_info("No video file is currently running so return!\n");
 		return;
 	}
+
+	/* Set the interrupt mask register, disable irq. */
+	cx_set(PCI_INT_MSK, cx_read(PCI_INT_MSK) & ~(1 << sram_ch->irq_bit));
+
 	/* Disable RISC interrupts */
 	tmp = cx_read(sram_ch->int_msk);
 	cx_write(sram_ch->int_msk, tmp & ~_intr_msk);
@@ -263,283 +265,133 @@ void cx25821_stop_upstream_video_ch1(struct cx25821_dev *dev)
 	tmp = cx_read(sram_ch->dma_ctl);
 	cx_write(sram_ch->dma_ctl, tmp & ~(FLD_VID_FIFO_EN | FLD_VID_RISC_EN));
 
-	/* Clear data buffer memory */
-	if (dev->_data_buf_virt_addr)
-		memset(dev->_data_buf_virt_addr, 0, dev->_data_buf_size);
+	free_irq(dev->pci->irq, chan);
 
-	dev->_is_running = 0;
-	dev->_is_first_frame = 0;
-	dev->_frame_count = 0;
-	dev->_file_status = END_OF_FILE;
-
-	kfree(dev->_irq_queues);
-	dev->_irq_queues = NULL;
+	/* Clear data buffer memory */
+	if (out->_data_buf_virt_addr)
+		memset(out->_data_buf_virt_addr, 0, out->_data_buf_size);
 
-	kfree(dev->_filename);
+	out->_is_running = 0;
+	out->_is_first_frame = 0;
+	out->_frame_count = 0;
+	out->_file_status = END_OF_FILE;
 
 	tmp = cx_read(VID_CH_MODE_SEL);
 	cx_write(VID_CH_MODE_SEL, tmp & 0xFFFFFE00);
 }
 
-void cx25821_free_mem_upstream_ch1(struct cx25821_dev *dev)
+void cx25821_free_mem_upstream(struct cx25821_channel *chan)
 {
-	if (dev->_is_running)
-		cx25821_stop_upstream_video_ch1(dev);
+	struct cx25821_video_out_data *out = chan->out;
+	struct cx25821_dev *dev = chan->dev;
 
-	if (dev->_dma_virt_addr) {
-		pci_free_consistent(dev->pci, dev->_risc_size,
-				    dev->_dma_virt_addr, dev->_dma_phys_addr);
-		dev->_dma_virt_addr = NULL;
+	if (out->_is_running)
+		cx25821_stop_upstream_video(chan);
+
+	if (out->_dma_virt_addr) {
+		pci_free_consistent(dev->pci, out->_risc_size,
+				    out->_dma_virt_addr, out->_dma_phys_addr);
+		out->_dma_virt_addr = NULL;
 	}
 
-	if (dev->_data_buf_virt_addr) {
-		pci_free_consistent(dev->pci, dev->_data_buf_size,
-				    dev->_data_buf_virt_addr,
-				    dev->_data_buf_phys_addr);
-		dev->_data_buf_virt_addr = NULL;
+	if (out->_data_buf_virt_addr) {
+		pci_free_consistent(dev->pci, out->_data_buf_size,
+				    out->_data_buf_virt_addr,
+				    out->_data_buf_phys_addr);
+		out->_data_buf_virt_addr = NULL;
 	}
 }
 
-static int cx25821_get_frame(struct cx25821_dev *dev,
-			     struct sram_channel *sram_ch)
+int cx25821_write_frame(struct cx25821_channel *chan,
+		const char __user *data, size_t count)
 {
-	struct file *myfile;
-	int frame_index_temp = dev->_frame_index;
-	int i = 0;
-	int line_size = (dev->_pixel_format == PIXEL_FRMT_411) ?
+	struct cx25821_video_out_data *out = chan->out;
+	int line_size = (out->_pixel_format == PIXEL_FRMT_411) ?
 		Y411_LINE_SZ : Y422_LINE_SZ;
 	int frame_size = 0;
 	int frame_offset = 0;
-	ssize_t vfs_read_retval = 0;
-	char mybuf[line_size];
-	loff_t file_offset;
-	loff_t pos;
-	mm_segment_t old_fs;
-
-	if (dev->_file_status == END_OF_FILE)
-		return 0;
+	int curpos = out->curpos;
 
-	if (dev->_isNTSC)
+	if (out->is_60hz)
 		frame_size = (line_size == Y411_LINE_SZ) ?
 			FRAME_SIZE_NTSC_Y411 : FRAME_SIZE_NTSC_Y422;
 	else
 		frame_size = (line_size == Y411_LINE_SZ) ?
 			FRAME_SIZE_PAL_Y411 : FRAME_SIZE_PAL_Y422;
 
-	frame_offset = (frame_index_temp > 0) ? frame_size : 0;
-	file_offset = dev->_frame_count * frame_size;
-
-	myfile = filp_open(dev->_filename, O_RDONLY | O_LARGEFILE, 0);
-
-	if (IS_ERR(myfile)) {
-		const int open_errno = -PTR_ERR(myfile);
-		pr_err("%s(): ERROR opening file(%s) with errno = %d!\n",
-		       __func__, dev->_filename, open_errno);
-		return PTR_ERR(myfile);
-	} else {
-		if (!(myfile->f_op)) {
-			pr_err("%s(): File has no file operations registered!\n",
-			       __func__);
-			filp_close(myfile, NULL);
-			return -EIO;
-		}
-
-		if (!myfile->f_op->read) {
-			pr_err("%s(): File has no READ operations registered!\n",
-			       __func__);
-			filp_close(myfile, NULL);
-			return -EIO;
-		}
-
-		pos = myfile->f_pos;
-		old_fs = get_fs();
-		set_fs(KERNEL_DS);
-
-		for (i = 0; i < dev->_lines_count; i++) {
-			pos = file_offset;
-
-			vfs_read_retval = vfs_read(myfile, mybuf, line_size,
-					&pos);
-
-			if (vfs_read_retval > 0 && vfs_read_retval == line_size
-			    && dev->_data_buf_virt_addr != NULL) {
-				memcpy((void *)(dev->_data_buf_virt_addr +
-						frame_offset / 4), mybuf,
-				       vfs_read_retval);
-			}
-
-			file_offset += vfs_read_retval;
-			frame_offset += vfs_read_retval;
-
-			if (vfs_read_retval < line_size) {
-				pr_info("Done: exit %s() since no more bytes to read from Video file\n",
-					__func__);
-				break;
-			}
-		}
-
-		if (i > 0)
-			dev->_frame_count++;
-
-		dev->_file_status = (vfs_read_retval == line_size) ?
-			IN_PROGRESS : END_OF_FILE;
-
-		set_fs(old_fs);
-		filp_close(myfile, NULL);
-	}
-
-	return 0;
-}
-
-static void cx25821_vidups_handler(struct work_struct *work)
-{
-	struct cx25821_dev *dev = container_of(work, struct cx25821_dev,
-			_irq_work_entry);
-
-	if (!dev) {
-		pr_err("ERROR %s(): since container_of(work_struct) FAILED!\n",
-		       __func__);
-		return;
+	if (curpos == 0) {
+		out->cur_frame_index = out->_frame_index;
+		if (wait_event_interruptible(out->waitq, out->cur_frame_index != out->_frame_index))
+			return -EINTR;
+		out->cur_frame_index = out->_frame_index;
 	}
 
-	cx25821_get_frame(dev, dev->channels[dev->_channel_upstream_select].
-			sram_channels);
-}
-
-static int cx25821_openfile(struct cx25821_dev *dev,
-			    struct sram_channel *sram_ch)
-{
-	struct file *myfile;
-	int i = 0, j = 0;
-	int line_size = (dev->_pixel_format == PIXEL_FRMT_411) ?
-		Y411_LINE_SZ : Y422_LINE_SZ;
-	ssize_t vfs_read_retval = 0;
-	char mybuf[line_size];
-	loff_t pos;
-	loff_t offset = (unsigned long)0;
-	mm_segment_t old_fs;
-
-	myfile = filp_open(dev->_filename, O_RDONLY | O_LARGEFILE, 0);
-
-	if (IS_ERR(myfile)) {
-		const int open_errno = -PTR_ERR(myfile);
-		pr_err("%s(): ERROR opening file(%s) with errno = %d!\n",
-		       __func__, dev->_filename, open_errno);
-		return PTR_ERR(myfile);
-	} else {
-		if (!(myfile->f_op)) {
-			pr_err("%s(): File has no file operations registered!\n",
-			       __func__);
-			filp_close(myfile, NULL);
-			return -EIO;
-		}
-
-		if (!myfile->f_op->read) {
-			pr_err("%s(): File has no READ operations registered!  Returning\n",
-			       __func__);
-			filp_close(myfile, NULL);
-			return -EIO;
-		}
-
-		pos = myfile->f_pos;
-		old_fs = get_fs();
-		set_fs(KERNEL_DS);
-
-		for (j = 0; j < NUM_FRAMES; j++) {
-			for (i = 0; i < dev->_lines_count; i++) {
-				pos = offset;
-
-				vfs_read_retval = vfs_read(myfile, mybuf,
-						line_size, &pos);
-
-				if (vfs_read_retval > 0
-				    && vfs_read_retval == line_size
-				    && dev->_data_buf_virt_addr != NULL) {
-					memcpy((void *)(dev->
-							_data_buf_virt_addr +
-							offset / 4), mybuf,
-					       vfs_read_retval);
-				}
-
-				offset += vfs_read_retval;
-
-				if (vfs_read_retval < line_size) {
-					pr_info("Done: exit %s() since no more bytes to read from Video file\n",
-						__func__);
-					break;
-				}
-			}
+	frame_offset = out->cur_frame_index ? frame_size : 0;
 
-			if (i > 0)
-				dev->_frame_count++;
-
-			if (vfs_read_retval < line_size)
-				break;
-		}
-
-		dev->_file_status = (vfs_read_retval == line_size) ?
-			IN_PROGRESS : END_OF_FILE;
-
-		set_fs(old_fs);
-		myfile->f_pos = 0;
-		filp_close(myfile, NULL);
+	if (frame_size - curpos < count)
+		count = frame_size - curpos;
+	memcpy((char *)out->_data_buf_virt_addr + frame_offset + curpos,
+			data, count);
+	curpos += count;
+	if (curpos == frame_size) {
+		out->_frame_count++;
+		curpos = 0;
 	}
+	out->curpos = curpos;
 
-	return 0;
+	return count;
 }
 
-static int cx25821_upstream_buffer_prepare(struct cx25821_dev *dev,
-					   struct sram_channel *sram_ch,
+static int cx25821_upstream_buffer_prepare(struct cx25821_channel *chan,
+					   const struct sram_channel *sram_ch,
 					   int bpl)
 {
+	struct cx25821_video_out_data *out = chan->out;
+	struct cx25821_dev *dev = chan->dev;
 	int ret = 0;
 	dma_addr_t dma_addr;
 	dma_addr_t data_dma_addr;
 
-	if (dev->_dma_virt_addr != NULL)
-		pci_free_consistent(dev->pci, dev->upstream_riscbuf_size,
-				dev->_dma_virt_addr, dev->_dma_phys_addr);
+	if (out->_dma_virt_addr != NULL)
+		pci_free_consistent(dev->pci, out->upstream_riscbuf_size,
+				out->_dma_virt_addr, out->_dma_phys_addr);
 
-	dev->_dma_virt_addr = pci_alloc_consistent(dev->pci,
-			dev->upstream_riscbuf_size, &dma_addr);
-	dev->_dma_virt_start_addr = dev->_dma_virt_addr;
-	dev->_dma_phys_start_addr = dma_addr;
-	dev->_dma_phys_addr = dma_addr;
-	dev->_risc_size = dev->upstream_riscbuf_size;
+	out->_dma_virt_addr = pci_alloc_consistent(dev->pci,
+			out->upstream_riscbuf_size, &dma_addr);
+	out->_dma_virt_start_addr = out->_dma_virt_addr;
+	out->_dma_phys_start_addr = dma_addr;
+	out->_dma_phys_addr = dma_addr;
+	out->_risc_size = out->upstream_riscbuf_size;
 
-	if (!dev->_dma_virt_addr) {
+	if (!out->_dma_virt_addr) {
 		pr_err("FAILED to allocate memory for Risc buffer! Returning\n");
 		return -ENOMEM;
 	}
 
 	/* Clear memory at address */
-	memset(dev->_dma_virt_addr, 0, dev->_risc_size);
+	memset(out->_dma_virt_addr, 0, out->_risc_size);
 
-	if (dev->_data_buf_virt_addr != NULL)
-		pci_free_consistent(dev->pci, dev->upstream_databuf_size,
-				dev->_data_buf_virt_addr,
-				dev->_data_buf_phys_addr);
+	if (out->_data_buf_virt_addr != NULL)
+		pci_free_consistent(dev->pci, out->upstream_databuf_size,
+				out->_data_buf_virt_addr,
+				out->_data_buf_phys_addr);
 	/* For Video Data buffer allocation */
-	dev->_data_buf_virt_addr = pci_alloc_consistent(dev->pci,
-			dev->upstream_databuf_size, &data_dma_addr);
-	dev->_data_buf_phys_addr = data_dma_addr;
-	dev->_data_buf_size = dev->upstream_databuf_size;
+	out->_data_buf_virt_addr = pci_alloc_consistent(dev->pci,
+			out->upstream_databuf_size, &data_dma_addr);
+	out->_data_buf_phys_addr = data_dma_addr;
+	out->_data_buf_size = out->upstream_databuf_size;
 
-	if (!dev->_data_buf_virt_addr) {
+	if (!out->_data_buf_virt_addr) {
 		pr_err("FAILED to allocate memory for data buffer! Returning\n");
 		return -ENOMEM;
 	}
 
 	/* Clear memory at address */
-	memset(dev->_data_buf_virt_addr, 0, dev->_data_buf_size);
-
-	ret = cx25821_openfile(dev, sram_ch);
-	if (ret < 0)
-		return ret;
+	memset(out->_data_buf_virt_addr, 0, out->_data_buf_size);
 
 	/* Create RISC programs */
-	ret = cx25821_risc_buffer_upstream(dev, dev->pci, 0, bpl,
-			dev->_lines_count);
+	ret = cx25821_risc_buffer_upstream(chan, dev->pci, 0, bpl,
+			out->_lines_count);
 	if (ret < 0) {
 		pr_info("Failed creating Video Upstream Risc programs!\n");
 		goto error;
@@ -551,11 +403,12 @@ error:
 	return ret;
 }
 
-static int cx25821_video_upstream_irq(struct cx25821_dev *dev, int chan_num,
-				      u32 status)
+static int cx25821_video_upstream_irq(struct cx25821_channel *chan, u32 status)
 {
+	struct cx25821_video_out_data *out = chan->out;
+	struct cx25821_dev *dev = chan->dev;
 	u32 int_msk_tmp;
-	struct sram_channel *channel = dev->channels[chan_num].sram_channels;
+	const struct sram_channel *channel = chan->sram_channels;
 	int singlefield_lines = NTSC_FIELD_HEIGHT;
 	int line_size_in_bytes = Y422_LINE_SZ;
 	int odd_risc_prog_size = 0;
@@ -572,16 +425,16 @@ static int cx25821_video_upstream_irq(struct cx25821_dev *dev, int chan_num,
 		cx_write(channel->int_msk, int_msk_tmp & ~_intr_msk);
 		cx_write(channel->int_stat, _intr_msk);
 
-		spin_lock(&dev->slock);
+		wake_up(&out->waitq);
 
-		dev->_frame_index = prog_cnt;
+		spin_lock(&dev->slock);
 
-		queue_work(dev->_irq_queues, &dev->_irq_work_entry);
+		out->_frame_index = prog_cnt;
 
-		if (dev->_is_first_frame) {
-			dev->_is_first_frame = 0;
+		if (out->_is_first_frame) {
+			out->_is_first_frame = 0;
 
-			if (dev->_isNTSC) {
+			if (out->is_60hz) {
 				singlefield_lines += 1;
 				odd_risc_prog_size = ODD_FLD_NTSC_PROG_SIZE;
 			} else {
@@ -589,17 +442,17 @@ static int cx25821_video_upstream_irq(struct cx25821_dev *dev, int chan_num,
 				odd_risc_prog_size = ODD_FLD_PAL_PROG_SIZE;
 			}
 
-			if (dev->_dma_virt_start_addr != NULL) {
+			if (out->_dma_virt_start_addr != NULL) {
 				line_size_in_bytes =
-				    (dev->_pixel_format ==
+				    (out->_pixel_format ==
 				     PIXEL_FRMT_411) ? Y411_LINE_SZ :
 				    Y422_LINE_SZ;
 				risc_phys_jump_addr =
-				    dev->_dma_phys_start_addr +
+				    out->_dma_phys_start_addr +
 				    odd_risc_prog_size;
 
-				rp = cx25821_update_riscprogram(dev,
-					dev->_dma_virt_start_addr, TOP_OFFSET,
+				rp = cx25821_update_riscprogram(chan,
+					out->_dma_virt_start_addr, TOP_OFFSET,
 					line_size_in_bytes, 0x0,
 					singlefield_lines, FIFO_DISABLE,
 					ODD_FIELD);
@@ -626,8 +479,8 @@ static int cx25821_video_upstream_irq(struct cx25821_dev *dev, int chan_num,
 			       __func__);
 	}
 
-	if (dev->_file_status == END_OF_FILE) {
-		pr_err("EOF Channel 1 Framecount = %d\n", dev->_frame_count);
+	if (out->_file_status == END_OF_FILE) {
+		pr_err("EOF Channel 1 Framecount = %d\n", out->_frame_count);
 		return -1;
 	}
 	/* ElSE, set the interrupt mask register, re-enable irq. */
@@ -639,47 +492,41 @@ static 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;
+	struct cx25821_channel *chan = dev_id;
+	struct cx25821_dev *dev = chan->dev;
 	u32 vid_status;
 	int handled = 0;
-	int channel_num = 0;
-	struct sram_channel *sram_ch;
+	const struct sram_channel *sram_ch;
 
 	if (!dev)
 		return -1;
 
-	channel_num = VID_UPSTREAM_SRAM_CHANNEL_I;
-
-	sram_ch = dev->channels[channel_num].sram_channels;
+	sram_ch = chan->sram_channels;
 
 	vid_status = cx_read(sram_ch->int_stat);
 
 	/* Only deal with our interrupt */
 	if (vid_status)
-		handled = cx25821_video_upstream_irq(dev, channel_num,
-				vid_status);
-
-	if (handled < 0)
-		cx25821_stop_upstream_video_ch1(dev);
-	else
-		handled += handled;
+		handled = cx25821_video_upstream_irq(chan, vid_status);
 
 	return IRQ_RETVAL(handled);
 }
 
-static void cx25821_set_pixelengine(struct cx25821_dev *dev,
-				    struct sram_channel *ch,
+static void cx25821_set_pixelengine(struct cx25821_channel *chan,
+				    const struct sram_channel *ch,
 				    int pix_format)
 {
+	struct cx25821_video_out_data *out = chan->out;
+	struct cx25821_dev *dev = chan->dev;
 	int width = WIDTH_D1;
-	int height = dev->_lines_count;
+	int height = out->_lines_count;
 	int num_lines, odd_num_lines;
 	u32 value;
 	int vip_mode = OUTPUT_FRMT_656;
 
 	value = ((pix_format & 0x3) << 12) | (vip_mode & 0x7);
 	value &= 0xFFFFFFEF;
-	value |= dev->_isNTSC ? 0 : 0x10;
+	value |= out->is_60hz ? 0 : 0x10;
 	cx_write(ch->vid_fmt_ctl, value);
 
 	/* set number of active pixels in each line.
@@ -689,7 +536,7 @@ static void cx25821_set_pixelengine(struct cx25821_dev *dev,
 	num_lines = (height / 2) & 0x3FF;
 	odd_num_lines = num_lines;
 
-	if (dev->_isNTSC)
+	if (out->is_60hz)
 		odd_num_lines += 1;
 
 	value = (num_lines << 16) | odd_num_lines;
@@ -700,9 +547,11 @@ static void cx25821_set_pixelengine(struct cx25821_dev *dev,
 	cx_write(ch->vid_cdt_size, VID_CDT_SIZE >> 3);
 }
 
-static int cx25821_start_video_dma_upstream(struct cx25821_dev *dev,
-					    struct sram_channel *sram_ch)
+static int cx25821_start_video_dma_upstream(struct cx25821_channel *chan,
+					    const struct sram_channel *sram_ch)
 {
+	struct cx25821_video_out_data *out = chan->out;
+	struct cx25821_dev *dev = chan->dev;
 	u32 tmp = 0;
 	int err = 0;
 
@@ -715,7 +564,7 @@ static int cx25821_start_video_dma_upstream(struct cx25821_dev *dev,
 	/* Set the physical start address of the RISC program in the initial
 	 * program counter(IPC) member of the cmds.
 	 */
-	cx_write(sram_ch->cmds_start + 0, dev->_dma_phys_addr);
+	cx_write(sram_ch->cmds_start + 0, out->_dma_phys_addr);
 	/* Risc IPC High 64 bits 63-32 */
 	cx_write(sram_ch->cmds_start + 4, 0);
 
@@ -731,7 +580,7 @@ static int cx25821_start_video_dma_upstream(struct cx25821_dev *dev,
 	cx_write(sram_ch->int_msk, tmp |= _intr_msk);
 
 	err = request_irq(dev->pci->irq, cx25821_upstream_irq,
-			IRQF_SHARED, dev->name, dev);
+			IRQF_SHARED, dev->name, chan);
 	if (err < 0) {
 		pr_err("%s: can't get upstream IRQ %d\n",
 		       dev->name, dev->pci->irq);
@@ -742,8 +591,8 @@ static int cx25821_start_video_dma_upstream(struct cx25821_dev *dev,
 	tmp = cx_read(sram_ch->dma_ctl);
 	cx_set(sram_ch->dma_ctl, tmp | FLD_VID_RISC_EN);
 
-	dev->_is_running = 1;
-	dev->_is_first_frame = 1;
+	out->_is_running = 1;
+	out->_is_first_frame = 1;
 
 	return 0;
 
@@ -752,107 +601,71 @@ fail_irq:
 	return err;
 }
 
-int cx25821_vidupstream_init_ch1(struct cx25821_dev *dev, int channel_select,
+int cx25821_vidupstream_init(struct cx25821_channel *chan,
 				 int pixel_format)
 {
-	struct sram_channel *sram_ch;
+	struct cx25821_video_out_data *out = chan->out;
+	struct cx25821_dev *dev = chan->dev;
+	const struct sram_channel *sram_ch;
 	u32 tmp;
 	int err = 0;
 	int data_frame_size = 0;
 	int risc_buffer_size = 0;
-	int str_length = 0;
 
-	if (dev->_is_running) {
+	if (out->_is_running) {
 		pr_info("Video Channel is still running so return!\n");
 		return 0;
 	}
 
-	dev->_channel_upstream_select = channel_select;
-	sram_ch = dev->channels[channel_select].sram_channels;
+	sram_ch = chan->sram_channels;
 
-	INIT_WORK(&dev->_irq_work_entry, cx25821_vidups_handler);
-	dev->_irq_queues = create_singlethread_workqueue("cx25821_workqueue");
+	out->is_60hz = dev->tvnorm & V4L2_STD_525_60;
 
-	if (!dev->_irq_queues) {
-		pr_err("create_singlethread_workqueue() for Video FAILED!\n");
-		return -ENOMEM;
-	}
 	/* 656/VIP SRC Upstream Channel I & J and 7 - Host Bus Interface for
 	 * channel A-C
 	 */
 	tmp = cx_read(VID_CH_MODE_SEL);
 	cx_write(VID_CH_MODE_SEL, tmp | 0x1B0001FF);
 
-	dev->_is_running = 0;
-	dev->_frame_count = 0;
-	dev->_file_status = RESET_STATUS;
-	dev->_lines_count = dev->_isNTSC ? 480 : 576;
-	dev->_pixel_format = pixel_format;
-	dev->_line_size = (dev->_pixel_format == PIXEL_FRMT_422) ?
+	out->_is_running = 0;
+	out->_frame_count = 0;
+	out->_file_status = RESET_STATUS;
+	out->_lines_count = out->is_60hz ? 480 : 576;
+	out->_pixel_format = pixel_format;
+	out->_line_size = (out->_pixel_format == PIXEL_FRMT_422) ?
 		(WIDTH_D1 * 2) : (WIDTH_D1 * 3) / 2;
-	data_frame_size = dev->_isNTSC ? NTSC_DATA_BUF_SZ : PAL_DATA_BUF_SZ;
-	risc_buffer_size = dev->_isNTSC ?
+	data_frame_size = out->is_60hz ? NTSC_DATA_BUF_SZ : PAL_DATA_BUF_SZ;
+	risc_buffer_size = out->is_60hz ?
 		NTSC_RISC_BUF_SIZE : PAL_RISC_BUF_SIZE;
 
-	if (dev->input_filename) {
-		str_length = strlen(dev->input_filename);
-		dev->_filename = kmemdup(dev->input_filename, str_length + 1,
-					 GFP_KERNEL);
-
-		if (!dev->_filename) {
-			err = -ENOENT;
-			goto error;
-		}
-	} else {
-		str_length = strlen(dev->_defaultname);
-		dev->_filename = kmemdup(dev->_defaultname, str_length + 1,
-					 GFP_KERNEL);
-
-		if (!dev->_filename) {
-			err = -ENOENT;
-			goto error;
-		}
-	}
-
-	/* Default if filename is empty string */
-	if (strcmp(dev->_filename, "") == 0) {
-		if (dev->_isNTSC) {
-			dev->_filename =
-				(dev->_pixel_format == PIXEL_FRMT_411) ?
-				"/root/vid411.yuv" : "/root/vidtest.yuv";
-		} else {
-			dev->_filename =
-				(dev->_pixel_format == PIXEL_FRMT_411) ?
-				"/root/pal411.yuv" : "/root/pal422.yuv";
-		}
-	}
-
-	dev->_is_running = 0;
-	dev->_frame_count = 0;
-	dev->_file_status = RESET_STATUS;
-	dev->_lines_count = dev->_isNTSC ? 480 : 576;
-	dev->_pixel_format = pixel_format;
-	dev->_line_size = (dev->_pixel_format == PIXEL_FRMT_422) ?
+	out->_is_running = 0;
+	out->_frame_count = 0;
+	out->_file_status = RESET_STATUS;
+	out->_lines_count = out->is_60hz ? 480 : 576;
+	out->_pixel_format = pixel_format;
+	out->_line_size = (out->_pixel_format == PIXEL_FRMT_422) ?
 		(WIDTH_D1 * 2) : (WIDTH_D1 * 3) / 2;
+	out->curpos = 0;
+	init_waitqueue_head(&out->waitq);
 
 	err = cx25821_sram_channel_setup_upstream(dev, sram_ch,
-			dev->_line_size, 0);
+			out->_line_size, 0);
 
 	/* setup fifo + format */
-	cx25821_set_pixelengine(dev, sram_ch, dev->_pixel_format);
+	cx25821_set_pixelengine(chan, sram_ch, out->_pixel_format);
 
-	dev->upstream_riscbuf_size = risc_buffer_size * 2;
-	dev->upstream_databuf_size = data_frame_size * 2;
+	out->upstream_riscbuf_size = risc_buffer_size * 2;
+	out->upstream_databuf_size = data_frame_size * 2;
 
 	/* Allocating buffers and prepare RISC program */
-	err = cx25821_upstream_buffer_prepare(dev, sram_ch, dev->_line_size);
+	err = cx25821_upstream_buffer_prepare(chan, sram_ch, out->_line_size);
 	if (err < 0) {
 		pr_err("%s: Failed to set up Video upstream buffers!\n",
 		       dev->name);
 		goto error;
 	}
 
-	cx25821_start_video_dma_upstream(dev, sram_ch);
+	cx25821_start_video_dma_upstream(chan, sram_ch);
 
 	return 0;
 
diff --git a/drivers/media/pci/cx25821/cx25821-video.c b/drivers/media/pci/cx25821/cx25821-video.c
index d4de021dc84..d270819fd87 100644
--- a/drivers/media/pci/cx25821/cx25821-video.c
+++ b/drivers/media/pci/cx25821/cx25821-video.c
@@ -33,13 +33,10 @@ MODULE_AUTHOR("Hiep Huynh <hiep.huynh@conexant.com>");
 MODULE_LICENSE("GPL");
 
 static unsigned int video_nr[] = {[0 ... (CX25821_MAXBOARDS - 1)] = UNSET };
-static unsigned int radio_nr[] = {[0 ... (CX25821_MAXBOARDS - 1)] = UNSET };
 
 module_param_array(video_nr, int, NULL, 0444);
-module_param_array(radio_nr, int, NULL, 0444);
 
 MODULE_PARM_DESC(video_nr, "video device numbers");
-MODULE_PARM_DESC(radio_nr, "radio device numbers");
 
 static unsigned int video_debug = VIDEO_DEBUG;
 module_param(video_debug, int, 0644);
@@ -49,24 +46,14 @@ static unsigned int irq_debug;
 module_param(irq_debug, int, 0644);
 MODULE_PARM_DESC(irq_debug, "enable debug messages [IRQ handler]");
 
-unsigned int vid_limit = 16;
+static unsigned int vid_limit = 16;
 module_param(vid_limit, int, 0644);
 MODULE_PARM_DESC(vid_limit, "capture memory limit in megabytes");
 
-static void cx25821_init_controls(struct cx25821_dev *dev, int chan_num);
-
-static const struct v4l2_file_operations video_fops;
-static const struct v4l2_ioctl_ops video_ioctl_ops;
-
 #define FORMAT_FLAGS_PACKED       0x01
 
-struct cx25821_fmt formats[] = {
+static const struct cx25821_fmt formats[] = {
 	{
-		.name = "8 bpp, gray",
-		.fourcc = V4L2_PIX_FMT_GREY,
-		.depth = 8,
-		.flags = FORMAT_FLAGS_PACKED,
-	 }, {
 		.name = "4:1:1, packed, Y41P",
 		.fourcc = V4L2_PIX_FMT_Y41P,
 		.depth = 12,
@@ -76,36 +63,16 @@ struct cx25821_fmt formats[] = {
 		.fourcc = V4L2_PIX_FMT_YUYV,
 		.depth = 16,
 		.flags = FORMAT_FLAGS_PACKED,
-	}, {
-		.name = "4:2:2, packed, UYVY",
-		.fourcc = V4L2_PIX_FMT_UYVY,
-		.depth = 16,
-		.flags = FORMAT_FLAGS_PACKED,
-	}, {
-		.name = "4:2:0, YUV",
-		.fourcc = V4L2_PIX_FMT_YUV420,
-		.depth = 12,
-		.flags = FORMAT_FLAGS_PACKED,
 	},
 };
 
-int cx25821_get_format_size(void)
-{
-	return ARRAY_SIZE(formats);
-}
-
-struct cx25821_fmt *cx25821_format_by_fourcc(unsigned int fourcc)
+static const struct cx25821_fmt *cx25821_format_by_fourcc(unsigned int fourcc)
 {
 	unsigned int i;
 
-	if (fourcc == V4L2_PIX_FMT_Y41P || fourcc == V4L2_PIX_FMT_YUV411P)
-		return formats + 1;
-
 	for (i = 0; i < ARRAY_SIZE(formats); i++)
 		if (formats[i].fourcc == fourcc)
 			return formats + i;
-
-	pr_err("%s(0x%08x) NOT FOUND\n", __func__, fourcc);
 	return NULL;
 }
 
@@ -144,129 +111,10 @@ void cx25821_video_wakeup(struct cx25821_dev *dev, struct cx25821_dmaqueue *q,
 		pr_err("%s: %d buffers handled (should be 1)\n", __func__, bc);
 }
 
-#ifdef TUNER_FLAG
-int cx25821_set_tvnorm(struct cx25821_dev *dev, v4l2_std_id norm)
-{
-	dprintk(1, "%s(norm = 0x%08x) name: [%s]\n",
-		__func__, (unsigned int)norm, v4l2_norm_to_name(norm));
-
-	dev->tvnorm = norm;
-
-	/* Tell the internal A/V decoder */
-	cx25821_call_all(dev, core, s_std, norm);
-
-	return 0;
-}
-#endif
-
-struct video_device *cx25821_vdev_init(struct cx25821_dev *dev,
-				       struct pci_dev *pci,
-				       struct video_device *template,
-				       char *type)
-{
-	struct video_device *vfd;
-	dprintk(1, "%s()\n", __func__);
-
-	vfd = video_device_alloc();
-	if (NULL == vfd)
-		return NULL;
-	*vfd = *template;
-	vfd->v4l2_dev = &dev->v4l2_dev;
-	vfd->release = video_device_release;
-	snprintf(vfd->name, sizeof(vfd->name), "%s %s (%s)", dev->name, type,
-		 cx25821_boards[dev->board].name);
-	video_set_drvdata(vfd, dev);
-	return vfd;
-}
-
-/*
-static int cx25821_ctrl_query(struct v4l2_queryctrl *qctrl)
-{
-	int i;
-
-	if (qctrl->id < V4L2_CID_BASE || qctrl->id >= V4L2_CID_LASTP1)
-		return -EINVAL;
-	for (i = 0; i < CX25821_CTLS; i++)
-		if (cx25821_ctls[i].v.id == qctrl->id)
-			break;
-	if (i == CX25821_CTLS) {
-		*qctrl = no_ctl;
-		return 0;
-	}
-	*qctrl = cx25821_ctls[i].v;
-	return 0;
-}
-*/
-
-/* resource management */
-int cx25821_res_get(struct cx25821_dev *dev, struct cx25821_fh *fh,
-		    unsigned int bit)
-{
-	dprintk(1, "%s()\n", __func__);
-	if (fh->resources & bit)
-		/* have it already allocated */
-		return 1;
-
-	/* is it free? */
-	mutex_lock(&dev->lock);
-	if (dev->channels[fh->channel_id].resources & bit) {
-		/* no, someone else uses it */
-		mutex_unlock(&dev->lock);
-		return 0;
-	}
-	/* it's free, grab it */
-	fh->resources |= bit;
-	dev->channels[fh->channel_id].resources |= bit;
-	dprintk(1, "res: get %d\n", bit);
-	mutex_unlock(&dev->lock);
-	return 1;
-}
-
-int cx25821_res_check(struct cx25821_fh *fh, unsigned int bit)
-{
-	return fh->resources & bit;
-}
-
-int cx25821_res_locked(struct cx25821_fh *fh, unsigned int bit)
-{
-	return fh->dev->channels[fh->channel_id].resources & bit;
-}
-
-void cx25821_res_free(struct cx25821_dev *dev, struct cx25821_fh *fh,
-		      unsigned int bits)
-{
-	BUG_ON((fh->resources & bits) != bits);
-	dprintk(1, "%s()\n", __func__);
-
-	mutex_lock(&dev->lock);
-	fh->resources &= ~bits;
-	dev->channels[fh->channel_id].resources &= ~bits;
-	dprintk(1, "res: put %d\n", bits);
-	mutex_unlock(&dev->lock);
-}
-
-int cx25821_video_mux(struct cx25821_dev *dev, unsigned int input)
-{
-	struct v4l2_routing route;
-	memset(&route, 0, sizeof(route));
-
-	dprintk(1, "%s(): video_mux: %d [vmux=%d, gpio=0x%x,0x%x,0x%x,0x%x]\n",
-		__func__, input, INPUT(input)->vmux, INPUT(input)->gpio0,
-		INPUT(input)->gpio1, INPUT(input)->gpio2, INPUT(input)->gpio3);
-	dev->input = input;
-
-	route.input = INPUT(input)->vmux;
-
-	/* Tell the internal A/V decoder */
-	cx25821_call_all(dev, video, s_routing, INPUT(input)->vmux, 0, 0);
-
-	return 0;
-}
-
 int cx25821_start_video_dma(struct cx25821_dev *dev,
 			    struct cx25821_dmaqueue *q,
 			    struct cx25821_buffer *buf,
-			    struct sram_channel *channel)
+			    const struct sram_channel *channel)
 {
 	int tmp = 0;
 
@@ -293,7 +141,7 @@ int cx25821_start_video_dma(struct cx25821_dev *dev,
 
 static int cx25821_restart_video_queue(struct cx25821_dev *dev,
 				       struct cx25821_dmaqueue *q,
-				       struct sram_channel *channel)
+				       const struct sram_channel *channel)
 {
 	struct cx25821_buffer *buf, *prev;
 	struct list_head *item;
@@ -346,8 +194,8 @@ static void cx25821_vid_timeout(unsigned long data)
 {
 	struct cx25821_data *timeout_data = (struct cx25821_data *)data;
 	struct cx25821_dev *dev = timeout_data->dev;
-	struct sram_channel *channel = timeout_data->channel;
-	struct cx25821_dmaqueue *q = &dev->channels[channel->i].vidq;
+	const struct sram_channel *channel = timeout_data->channel;
+	struct cx25821_dmaqueue *q = &dev->channels[channel->i].dma_vidq;
 	struct cx25821_buffer *buf;
 	unsigned long flags;
 
@@ -373,7 +221,7 @@ int cx25821_video_irq(struct cx25821_dev *dev, int chan_num, u32 status)
 	u32 count = 0;
 	int handled = 0;
 	u32 mask;
-	struct sram_channel *channel = dev->channels[chan_num].sram_channels;
+	const struct sram_channel *channel = dev->channels[chan_num].sram_channels;
 
 	mask = cx_read(channel->int_msk);
 	if (0 == (status & mask))
@@ -393,7 +241,7 @@ int cx25821_video_irq(struct cx25821_dev *dev, int chan_num, u32 status)
 	if (status & FLD_VID_DST_RISC1) {
 		spin_lock(&dev->slock);
 		count = cx_read(channel->gpcnt);
-		cx25821_video_wakeup(dev, &dev->channels[channel->i].vidq,
+		cx25821_video_wakeup(dev, &dev->channels[channel->i].dma_vidq,
 				count);
 		spin_unlock(&dev->slock);
 		handled++;
@@ -404,122 +252,19 @@ int cx25821_video_irq(struct cx25821_dev *dev, int chan_num, u32 status)
 		dprintk(2, "stopper video\n");
 		spin_lock(&dev->slock);
 		cx25821_restart_video_queue(dev,
-				&dev->channels[channel->i].vidq, channel);
+				&dev->channels[channel->i].dma_vidq, channel);
 		spin_unlock(&dev->slock);
 		handled++;
 	}
 	return handled;
 }
 
-void cx25821_videoioctl_unregister(struct cx25821_dev *dev)
-{
-	if (dev->ioctl_dev) {
-		if (video_is_registered(dev->ioctl_dev))
-			video_unregister_device(dev->ioctl_dev);
-		else
-			video_device_release(dev->ioctl_dev);
-
-		dev->ioctl_dev = NULL;
-	}
-}
-
-void cx25821_video_unregister(struct cx25821_dev *dev, int chan_num)
-{
-	cx_clear(PCI_INT_MSK, 1);
-
-	if (dev->channels[chan_num].video_dev) {
-		if (video_is_registered(dev->channels[chan_num].video_dev))
-			video_unregister_device(
-					dev->channels[chan_num].video_dev);
-		else
-			video_device_release(
-					dev->channels[chan_num].video_dev);
-
-		dev->channels[chan_num].video_dev = NULL;
-
-		btcx_riscmem_free(dev->pci,
-				&dev->channels[chan_num].vidq.stopper);
-
-		pr_warn("device %d released!\n", chan_num);
-	}
-
-}
-
-int cx25821_video_register(struct cx25821_dev *dev)
-{
-	int err;
-	int i;
-
-	struct video_device cx25821_video_device = {
-		.name = "cx25821-video",
-		.fops = &video_fops,
-		.minor = -1,
-		.ioctl_ops = &video_ioctl_ops,
-		.tvnorms = CX25821_NORMS,
-		.current_norm = V4L2_STD_NTSC_M,
-	};
-
-	spin_lock_init(&dev->slock);
-
-	for (i = 0; i < MAX_VID_CHANNEL_NUM - 1; ++i) {
-		cx25821_init_controls(dev, i);
-
-		cx25821_risc_stopper(dev->pci, &dev->channels[i].vidq.stopper,
-			dev->channels[i].sram_channels->dma_ctl, 0x11, 0);
-
-		dev->channels[i].sram_channels = &cx25821_sram_channels[i];
-		dev->channels[i].video_dev = NULL;
-		dev->channels[i].resources = 0;
-
-		cx_write(dev->channels[i].sram_channels->int_stat, 0xffffffff);
-
-		INIT_LIST_HEAD(&dev->channels[i].vidq.active);
-		INIT_LIST_HEAD(&dev->channels[i].vidq.queued);
-
-		dev->channels[i].timeout_data.dev = dev;
-		dev->channels[i].timeout_data.channel =
-			&cx25821_sram_channels[i];
-		dev->channels[i].vidq.timeout.function = cx25821_vid_timeout;
-		dev->channels[i].vidq.timeout.data =
-			(unsigned long)&dev->channels[i].timeout_data;
-		init_timer(&dev->channels[i].vidq.timeout);
-
-		/* register v4l devices */
-		dev->channels[i].video_dev = cx25821_vdev_init(dev, dev->pci,
-				&cx25821_video_device, "video");
-
-		err = video_register_device(dev->channels[i].video_dev,
-				VFL_TYPE_GRABBER, video_nr[dev->nr]);
-
-		if (err < 0)
-			goto fail_unreg;
-
-	}
-
-	/* set PCI interrupt */
-	cx_set(PCI_INT_MSK, 0xff);
-
-	/* initial device configuration */
-	mutex_lock(&dev->lock);
-#ifdef TUNER_FLAG
-	dev->tvnorm = cx25821_video_device.current_norm;
-	cx25821_set_tvnorm(dev, dev->tvnorm);
-#endif
-	mutex_unlock(&dev->lock);
-
-	return 0;
-
-fail_unreg:
-	cx25821_video_unregister(dev, i);
-	return err;
-}
-
-int cx25821_buffer_setup(struct videobuf_queue *q, unsigned int *count,
+static int cx25821_buffer_setup(struct videobuf_queue *q, unsigned int *count,
 		 unsigned int *size)
 {
-	struct cx25821_fh *fh = q->priv_data;
+	struct cx25821_channel *chan = q->priv_data;
 
-	*size = fh->fmt->depth * fh->width * fh->height >> 3;
+	*size = chan->fmt->depth * chan->width * chan->height >> 3;
 
 	if (0 == *count)
 		*count = 32;
@@ -530,35 +275,34 @@ int cx25821_buffer_setup(struct videobuf_queue *q, unsigned int *count,
 	return 0;
 }
 
-int cx25821_buffer_prepare(struct videobuf_queue *q, struct videobuf_buffer *vb,
+static int cx25821_buffer_prepare(struct videobuf_queue *q, struct videobuf_buffer *vb,
 		   enum v4l2_field field)
 {
-	struct cx25821_fh *fh = q->priv_data;
-	struct cx25821_dev *dev = fh->dev;
+	struct cx25821_channel *chan = q->priv_data;
+	struct cx25821_dev *dev = chan->dev;
 	struct cx25821_buffer *buf =
 		container_of(vb, struct cx25821_buffer, vb);
 	int rc, init_buffer = 0;
 	u32 line0_offset;
 	struct videobuf_dmabuf *dma = videobuf_to_dma(&buf->vb);
 	int bpl_local = LINE_SIZE_D1;
-	int channel_opened = fh->channel_id;
 
-	BUG_ON(NULL == fh->fmt);
-	if (fh->width < 48 || fh->width > 720 ||
-	    fh->height < 32 || fh->height > 576)
+	BUG_ON(NULL == chan->fmt);
+	if (chan->width < 48 || chan->width > 720 ||
+	    chan->height < 32 || chan->height > 576)
 		return -EINVAL;
 
-	buf->vb.size = (fh->width * fh->height * fh->fmt->depth) >> 3;
+	buf->vb.size = (chan->width * chan->height * chan->fmt->depth) >> 3;
 
 	if (0 != buf->vb.baddr && buf->vb.bsize < buf->vb.size)
 		return -EINVAL;
 
-	if (buf->fmt != fh->fmt ||
-	    buf->vb.width != fh->width ||
-	    buf->vb.height != fh->height || buf->vb.field != field) {
-		buf->fmt = fh->fmt;
-		buf->vb.width = fh->width;
-		buf->vb.height = fh->height;
+	if (buf->fmt != chan->fmt ||
+	    buf->vb.width != chan->width ||
+	    buf->vb.height != chan->height || buf->vb.field != field) {
+		buf->fmt = chan->fmt;
+		buf->vb.width = chan->width;
+		buf->vb.height = chan->height;
 		buf->vb.field = field;
 		init_buffer = 1;
 	}
@@ -575,34 +319,21 @@ int cx25821_buffer_prepare(struct videobuf_queue *q, struct videobuf_buffer *vb,
 	dprintk(1, "init_buffer=%d\n", init_buffer);
 
 	if (init_buffer) {
-
-		channel_opened = dev->channel_opened;
-		if (channel_opened < 0 || channel_opened > 7)
-			channel_opened = 7;
-
-		if (dev->channels[channel_opened].pixel_formats ==
-				PIXEL_FRMT_411)
+		if (chan->pixel_formats == PIXEL_FRMT_411)
 			buf->bpl = (buf->fmt->depth * buf->vb.width) >> 3;
 		else
 			buf->bpl = (buf->fmt->depth >> 3) * (buf->vb.width);
 
-		if (dev->channels[channel_opened].pixel_formats ==
-				PIXEL_FRMT_411) {
+		if (chan->pixel_formats == PIXEL_FRMT_411) {
 			bpl_local = buf->bpl;
 		} else {
 			bpl_local = buf->bpl;   /* Default */
 
-			if (channel_opened >= 0 && channel_opened <= 7) {
-				if (dev->channels[channel_opened]
-						.use_cif_resolution) {
-					if (dev->tvnorm & V4L2_STD_PAL_BG ||
-					    dev->tvnorm & V4L2_STD_PAL_DK)
-						bpl_local = 352 << 1;
-					else
-						bpl_local = dev->channels[
-							channel_opened].
-							cif_width << 1;
-				}
+			if (chan->use_cif_resolution) {
+				if (dev->tvnorm & V4L2_STD_625_50)
+					bpl_local = 352 << 1;
+				else
+					bpl_local = chan->cif_width << 1;
 			}
 		}
 
@@ -645,8 +376,8 @@ int cx25821_buffer_prepare(struct videobuf_queue *q, struct videobuf_buffer *vb,
 	}
 
 	dprintk(2, "[%p/%d] buffer_prep - %dx%d %dbpp \"%s\" - dma=0x%08lx\n",
-		buf, buf->vb.i, fh->width, fh->height, fh->fmt->depth,
-		fh->fmt->name, (unsigned long)buf->risc.dma);
+		buf, buf->vb.i, chan->width, chan->height, chan->fmt->depth,
+		chan->fmt->name, (unsigned long)buf->risc.dma);
 
 	buf->vb.state = VIDEOBUF_PREPARED;
 
@@ -657,7 +388,7 @@ fail:
 	return rc;
 }
 
-void cx25821_buffer_release(struct videobuf_queue *q,
+static void cx25821_buffer_release(struct videobuf_queue *q,
 			    struct videobuf_buffer *vb)
 {
 	struct cx25821_buffer *buf =
@@ -666,33 +397,11 @@ void cx25821_buffer_release(struct videobuf_queue *q,
 	cx25821_free_buffer(q, buf);
 }
 
-struct videobuf_queue *get_queue(struct cx25821_fh *fh)
+static int cx25821_video_mmap(struct file *file, struct vm_area_struct *vma)
 {
-	switch (fh->type) {
-	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
-		return &fh->vidq;
-	default:
-		BUG();
-		return NULL;
-	}
-}
+	struct cx25821_channel *chan = video_drvdata(file);
 
-int cx25821_get_resource(struct cx25821_fh *fh, int resource)
-{
-	switch (fh->type) {
-	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
-		return resource;
-	default:
-		BUG();
-		return 0;
-	}
-}
-
-int cx25821_video_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	struct cx25821_fh *fh = file->private_data;
-
-	return videobuf_mmap_mapper(get_queue(fh), vma);
+	return videobuf_mmap_mapper(&chan->vidq, vma);
 }
 
 
@@ -701,9 +410,9 @@ static void buffer_queue(struct videobuf_queue *vq, struct videobuf_buffer *vb)
 	struct cx25821_buffer *buf =
 		container_of(vb, struct cx25821_buffer, vb);
 	struct cx25821_buffer *prev;
-	struct cx25821_fh *fh = vq->priv_data;
-	struct cx25821_dev *dev = fh->dev;
-	struct cx25821_dmaqueue *q = &dev->channels[fh->channel_id].vidq;
+	struct cx25821_channel *chan = vq->priv_data;
+	struct cx25821_dev *dev = chan->dev;
+	struct cx25821_dmaqueue *q = &dev->channels[chan->id].dma_vidq;
 
 	/* add jump to stopper */
 	buf->risc.jmp[0] = cpu_to_le32(RISC_JUMP | RISC_IRQ1 | RISC_CNT_INC);
@@ -720,8 +429,7 @@ static void buffer_queue(struct videobuf_queue *vq, struct videobuf_buffer *vb)
 
 	} else if (list_empty(&q->active)) {
 		list_add_tail(&buf->vb.queue, &q->active);
-		cx25821_start_video_dma(dev, q, buf,
-				dev->channels[fh->channel_id].sram_channels);
+		cx25821_start_video_dma(dev, q, buf, chan->sram_channels);
 		buf->vb.state = VIDEOBUF_ACTIVE;
 		buf->count = q->count++;
 		mod_timer(&q->timeout, jiffies + BUFFER_TIMEOUT);
@@ -762,1183 +470,487 @@ static struct videobuf_queue_ops cx25821_video_qops = {
 	.buf_release = cx25821_buffer_release,
 };
 
-static int video_open(struct file *file)
-{
-	struct video_device *vdev = video_devdata(file);
-	struct cx25821_dev *h, *dev = video_drvdata(file);
-	struct cx25821_fh *fh;
-	struct list_head *list;
-	int minor = video_devdata(file)->minor;
-	enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	u32 pix_format;
-	int ch_id = 0;
-	int i;
-
-	dprintk(1, "open dev=%s type=%s\n", video_device_node_name(vdev),
-			v4l2_type_names[type]);
-
-	/* allocate + initialize per filehandle data */
-	fh = kzalloc(sizeof(*fh), GFP_KERNEL);
-	if (NULL == fh)
-		return -ENOMEM;
-
-	mutex_lock(&cx25821_devlist_mutex);
-
-	list_for_each(list, &cx25821_devlist)
-	{
-		h = list_entry(list, struct cx25821_dev, devlist);
-
-		for (i = 0; i < MAX_VID_CHANNEL_NUM; i++) {
-			if (h->channels[i].video_dev &&
-			    h->channels[i].video_dev->minor == minor) {
-				dev = h;
-				ch_id = i;
-				type  = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-			}
-		}
-	}
-
-	if (NULL == dev) {
-		mutex_unlock(&cx25821_devlist_mutex);
-		kfree(fh);
-		return -ENODEV;
-	}
-
-	file->private_data = fh;
-	fh->dev = dev;
-	fh->type = type;
-	fh->width = 720;
-	fh->channel_id = ch_id;
-
-	if (dev->tvnorm & V4L2_STD_PAL_BG || dev->tvnorm & V4L2_STD_PAL_DK)
-		fh->height = 576;
-	else
-		fh->height = 480;
-
-	dev->channel_opened = fh->channel_id;
-	if (dev->channels[ch_id].pixel_formats == PIXEL_FRMT_411)
-		pix_format = V4L2_PIX_FMT_Y41P;
-	else
-		pix_format = V4L2_PIX_FMT_YUYV;
-	fh->fmt = cx25821_format_by_fourcc(pix_format);
-
-	v4l2_prio_open(&dev->channels[ch_id].prio, &fh->prio);
-
-	videobuf_queue_sg_init(&fh->vidq, &cx25821_video_qops, &dev->pci->dev,
-			&dev->slock, V4L2_BUF_TYPE_VIDEO_CAPTURE,
-			V4L2_FIELD_INTERLACED, sizeof(struct cx25821_buffer),
-			fh, NULL);
-
-	dprintk(1, "post videobuf_queue_init()\n");
-	mutex_unlock(&cx25821_devlist_mutex);
-
-	return 0;
-}
-
 static ssize_t video_read(struct file *file, char __user * data, size_t count,
 			 loff_t *ppos)
 {
-	struct cx25821_fh *fh = file->private_data;
+	struct v4l2_fh *fh = file->private_data;
+	struct cx25821_channel *chan = video_drvdata(file);
+	struct cx25821_dev *dev = chan->dev;
+	int err = 0;
 
-	switch (fh->type) {
-	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
-		if (cx25821_res_locked(fh, RESOURCE_VIDEO0))
-			return -EBUSY;
-
-		return videobuf_read_one(&fh->vidq, data, count, ppos,
-					file->f_flags & O_NONBLOCK);
-
-	default:
-		BUG();
-		return 0;
+	if (mutex_lock_interruptible(&dev->lock))
+		return -ERESTARTSYS;
+	if (chan->streaming_fh && chan->streaming_fh != fh) {
+		err = -EBUSY;
+		goto unlock;
 	}
+	chan->streaming_fh = fh;
+
+	err = videobuf_read_one(&chan->vidq, data, count, ppos,
+				file->f_flags & O_NONBLOCK);
+unlock:
+	mutex_unlock(&dev->lock);
+	return err;
 }
 
 static unsigned int video_poll(struct file *file,
 			      struct poll_table_struct *wait)
 {
-	struct cx25821_fh *fh = file->private_data;
-	struct cx25821_buffer *buf;
-
-	if (cx25821_res_check(fh, RESOURCE_VIDEO0)) {
-		/* streaming capture */
-		if (list_empty(&fh->vidq.stream))
-			return POLLERR;
-		buf = list_entry(fh->vidq.stream.next,
-				struct cx25821_buffer, vb.stream);
-	} else {
-		/* read() capture */
-		buf = (struct cx25821_buffer *)fh->vidq.read_buf;
-		if (NULL == buf)
-			return POLLERR;
-	}
-
-	poll_wait(file, &buf->vb.done, wait);
-	if (buf->vb.state == VIDEOBUF_DONE || buf->vb.state == VIDEOBUF_ERROR) {
-		if (buf->vb.state == VIDEOBUF_DONE) {
-			struct cx25821_dev *dev = fh->dev;
-
-			if (dev && dev->channels[fh->channel_id]
-					.use_cif_resolution) {
-				u8 cam_id = *((char *)buf->vb.baddr + 3);
-				memcpy((char *)buf->vb.baddr,
-				      (char *)buf->vb.baddr + (fh->width * 2),
-				      (fh->width * 2));
-				*((char *)buf->vb.baddr + 3) = cam_id;
-			}
+	struct cx25821_channel *chan = video_drvdata(file);
+	unsigned long req_events = poll_requested_events(wait);
+	unsigned int res = v4l2_ctrl_poll(file, wait);
+
+	if (req_events & (POLLIN | POLLRDNORM))
+		res |= videobuf_poll_stream(file, &chan->vidq, wait);
+	return res;
+
+	/* This doesn't belong in poll(). This can be done
+	 * much better with vb2. We keep this code here as a
+	 * reminder.
+	if ((res & POLLIN) && buf->vb.state == VIDEOBUF_DONE) {
+		struct cx25821_dev *dev = chan->dev;
+
+		if (dev && chan->use_cif_resolution) {
+			u8 cam_id = *((char *)buf->vb.baddr + 3);
+			memcpy((char *)buf->vb.baddr,
+					(char *)buf->vb.baddr + (chan->width * 2),
+					(chan->width * 2));
+			*((char *)buf->vb.baddr + 3) = cam_id;
 		}
-
-		return POLLIN | POLLRDNORM;
 	}
-
-	return 0;
+	 */
 }
 
 static int video_release(struct file *file)
 {
-	struct cx25821_fh *fh = file->private_data;
-	struct cx25821_dev *dev = fh->dev;
+	struct cx25821_channel *chan = video_drvdata(file);
+	struct v4l2_fh *fh = file->private_data;
+	struct cx25821_dev *dev = chan->dev;
+	const struct sram_channel *sram_ch =
+		dev->channels[0].sram_channels;
 
+	mutex_lock(&dev->lock);
 	/* stop the risc engine and fifo */
-	cx_write(channel0->dma_ctl, 0); /* FIFO and RISC disable */
+	cx_write(sram_ch->dma_ctl, 0); /* FIFO and RISC disable */
 
 	/* stop video capture */
-	if (cx25821_res_check(fh, RESOURCE_VIDEO0)) {
-		videobuf_queue_cancel(&fh->vidq);
-		cx25821_res_free(dev, fh, RESOURCE_VIDEO0);
+	if (chan->streaming_fh == fh) {
+		videobuf_queue_cancel(&chan->vidq);
+		chan->streaming_fh = NULL;
 	}
 
-	if (fh->vidq.read_buf) {
-		cx25821_buffer_release(&fh->vidq, fh->vidq.read_buf);
-		kfree(fh->vidq.read_buf);
+	if (chan->vidq.read_buf) {
+		cx25821_buffer_release(&chan->vidq, chan->vidq.read_buf);
+		kfree(chan->vidq.read_buf);
 	}
 
-	videobuf_mmap_free(&fh->vidq);
-
-	v4l2_prio_close(&dev->channels[fh->channel_id].prio, fh->prio);
-	file->private_data = NULL;
-	kfree(fh);
+	videobuf_mmap_free(&chan->vidq);
+	mutex_unlock(&dev->lock);
 
-	return 0;
+	return v4l2_fh_release(file);
 }
 
-static int vidioc_streamon(struct file *file, void *priv, enum v4l2_buf_type i)
-{
-	struct cx25821_fh *fh = priv;
-	struct cx25821_dev *dev = fh->dev;
+/* VIDEO IOCTLS */
 
-	if (unlikely(fh->type != V4L2_BUF_TYPE_VIDEO_CAPTURE))
+static int cx25821_vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
+			    struct v4l2_fmtdesc *f)
+{
+	if (unlikely(f->index >= ARRAY_SIZE(formats)))
 		return -EINVAL;
 
-	if (unlikely(i != fh->type))
-		return -EINVAL;
+	strlcpy(f->description, formats[f->index].name, sizeof(f->description));
+	f->pixelformat = formats[f->index].fourcc;
 
-	if (unlikely(!cx25821_res_get(dev, fh, cx25821_get_resource(fh,
-						RESOURCE_VIDEO0))))
-		return -EBUSY;
+	return 0;
+}
+
+static int cx25821_vidioc_g_fmt_vid_cap(struct file *file, void *priv,
+				 struct v4l2_format *f)
+{
+	struct cx25821_channel *chan = video_drvdata(file);
 
-	return videobuf_streamon(get_queue(fh));
+	f->fmt.pix.width = chan->width;
+	f->fmt.pix.height = chan->height;
+	f->fmt.pix.field = chan->vidq.field;
+	f->fmt.pix.pixelformat = chan->fmt->fourcc;
+	f->fmt.pix.bytesperline = (chan->width * chan->fmt->depth) >> 3;
+	f->fmt.pix.sizeimage = chan->height * f->fmt.pix.bytesperline;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	f->fmt.pix.priv = 0;
+
+	return 0;
 }
 
-static int vidioc_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
+static int cx25821_vidioc_try_fmt_vid_cap(struct file *file, void *priv,
+				   struct v4l2_format *f)
 {
-	struct cx25821_fh *fh = priv;
-	struct cx25821_dev *dev = fh->dev;
-	int err, res;
+	struct cx25821_channel *chan = video_drvdata(file);
+	struct cx25821_dev *dev = chan->dev;
+	const struct cx25821_fmt *fmt;
+	enum v4l2_field field = f->fmt.pix.field;
+	unsigned int maxh;
+	unsigned w;
 
-	if (fh->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-	if (i != fh->type)
+	fmt = cx25821_format_by_fourcc(f->fmt.pix.pixelformat);
+	if (NULL == fmt)
 		return -EINVAL;
+	maxh = (dev->tvnorm & V4L2_STD_625_50) ? 576 : 480;
+
+	w = f->fmt.pix.width;
+	if (field != V4L2_FIELD_BOTTOM)
+		field = V4L2_FIELD_TOP;
+	if (w < 352) {
+		w = 176;
+		f->fmt.pix.height = maxh / 4;
+	} else if (w < 720) {
+		w = 352;
+		f->fmt.pix.height = maxh / 2;
+	} else {
+		w = 720;
+		f->fmt.pix.height = maxh;
+		field = V4L2_FIELD_INTERLACED;
+	}
+	f->fmt.pix.field = field;
+	f->fmt.pix.width = w;
+	f->fmt.pix.bytesperline = (f->fmt.pix.width * fmt->depth) >> 3;
+	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	f->fmt.pix.priv = 0;
 
-	res = cx25821_get_resource(fh, RESOURCE_VIDEO0);
-	err = videobuf_streamoff(get_queue(fh));
-	if (err < 0)
-		return err;
-	cx25821_res_free(dev, fh, res);
 	return 0;
 }
 
 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 				struct v4l2_format *f)
 {
-	struct cx25821_fh *fh = priv;
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
-	struct v4l2_mbus_framefmt mbus_fmt;
-	int err;
+	struct cx25821_channel *chan = video_drvdata(file);
+	struct cx25821_dev *dev = chan->dev;
 	int pix_format = PIXEL_FRMT_422;
+	int err;
 
-	if (fh) {
-		err = v4l2_prio_check(&dev->channels[fh->channel_id].prio,
-				      fh->prio);
-		if (0 != err)
-			return err;
-	}
-
-	dprintk(2, "%s()\n", __func__);
 	err = cx25821_vidioc_try_fmt_vid_cap(file, priv, f);
 
 	if (0 != err)
 		return err;
 
-	fh->fmt = cx25821_format_by_fourcc(f->fmt.pix.pixelformat);
-	fh->vidq.field = f->fmt.pix.field;
-
-	/* check if width and height is valid based on set standard */
-	if (cx25821_is_valid_width(f->fmt.pix.width, dev->tvnorm))
-		fh->width = f->fmt.pix.width;
-
-	if (cx25821_is_valid_height(f->fmt.pix.height, dev->tvnorm))
-		fh->height = f->fmt.pix.height;
+	chan->fmt = cx25821_format_by_fourcc(f->fmt.pix.pixelformat);
+	chan->vidq.field = f->fmt.pix.field;
+	chan->width = f->fmt.pix.width;
+	chan->height = f->fmt.pix.height;
 
 	if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_Y41P)
 		pix_format = PIXEL_FRMT_411;
-	else if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_YUYV)
-		pix_format = PIXEL_FRMT_422;
 	else
-		return -EINVAL;
+		pix_format = PIXEL_FRMT_422;
 
 	cx25821_set_pixel_format(dev, SRAM_CH00, pix_format);
 
 	/* check if cif resolution */
-	if (fh->width == 320 || fh->width == 352)
-		dev->channels[fh->channel_id].use_cif_resolution = 1;
+	if (chan->width == 320 || chan->width == 352)
+		chan->use_cif_resolution = 1;
 	else
-		dev->channels[fh->channel_id].use_cif_resolution = 0;
-
-	dev->channels[fh->channel_id].cif_width = fh->width;
-	medusa_set_resolution(dev, fh->width, SRAM_CH00);
-
-	dprintk(2, "%s(): width=%d height=%d field=%d\n", __func__, fh->width,
-		fh->height, fh->vidq.field);
-	v4l2_fill_mbus_format(&mbus_fmt, &f->fmt.pix, V4L2_MBUS_FMT_FIXED);
-	cx25821_call_all(dev, video, s_mbus_fmt, &mbus_fmt);
+		chan->use_cif_resolution = 0;
 
+	chan->cif_width = chan->width;
+	medusa_set_resolution(dev, chan->width, SRAM_CH00);
 	return 0;
 }
 
-static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *p)
+static int vidioc_streamon(struct file *file, void *priv, enum v4l2_buf_type i)
 {
-	int ret_val = 0;
-	struct cx25821_fh *fh = priv;
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
+	struct cx25821_channel *chan = video_drvdata(file);
 
-	ret_val = videobuf_dqbuf(get_queue(fh), p, file->f_flags & O_NONBLOCK);
+	if (i != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
 
-	p->sequence = dev->channels[fh->channel_id].vidq.count;
+	if (chan->streaming_fh && chan->streaming_fh != priv)
+		return -EBUSY;
+	chan->streaming_fh = priv;
 
-	return ret_val;
+	return videobuf_streamon(&chan->vidq);
 }
 
-static int vidioc_log_status(struct file *file, void *priv)
+static int vidioc_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
 {
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
-	struct cx25821_fh *fh = priv;
-	char name[32 + 2];
+	struct cx25821_channel *chan = video_drvdata(file);
 
-	struct sram_channel *sram_ch = dev->channels[fh->channel_id]
-								.sram_channels;
-	u32 tmp = 0;
-
-	snprintf(name, sizeof(name), "%s/2", dev->name);
-	pr_info("%s/2: ============  START LOG STATUS  ============\n",
-		dev->name);
-	cx25821_call_all(dev, core, log_status);
-	tmp = cx_read(sram_ch->dma_ctl);
-	pr_info("Video input 0 is %s\n",
-		(tmp & 0x11) ? "streaming" : "stopped");
-	pr_info("%s/2: =============  END LOG STATUS  =============\n",
-		dev->name);
-	return 0;
-}
-
-static int vidioc_s_ctrl(struct file *file, void *priv,
-			struct v4l2_control *ctl)
-{
-	struct cx25821_fh *fh = priv;
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
-	int err;
+	if (i != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
 
-	if (fh) {
-		err = v4l2_prio_check(&dev->channels[fh->channel_id].prio,
-				      fh->prio);
-		if (0 != err)
-			return err;
-	}
+	if (chan->streaming_fh && chan->streaming_fh != priv)
+		return -EBUSY;
+	if (chan->streaming_fh == NULL)
+		return 0;
 
-	return cx25821_set_control(dev, ctl, fh->channel_id);
+	chan->streaming_fh = NULL;
+	return videobuf_streamoff(&chan->vidq);
 }
 
-/* VIDEO IOCTLS */
-int cx25821_vidioc_g_fmt_vid_cap(struct file *file, void *priv,
-				 struct v4l2_format *f)
+static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *p)
 {
-	struct cx25821_fh *fh = priv;
+	int ret_val = 0;
+	struct cx25821_channel *chan = video_drvdata(file);
 
-	f->fmt.pix.width = fh->width;
-	f->fmt.pix.height = fh->height;
-	f->fmt.pix.field = fh->vidq.field;
-	f->fmt.pix.pixelformat = fh->fmt->fourcc;
-	f->fmt.pix.bytesperline = (f->fmt.pix.width * fh->fmt->depth) >> 3;
-	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+	ret_val = videobuf_dqbuf(&chan->vidq, p, file->f_flags & O_NONBLOCK);
+	p->sequence = chan->dma_vidq.count;
 
-	return 0;
+	return ret_val;
 }
 
-int cx25821_vidioc_try_fmt_vid_cap(struct file *file, void *priv,
-				   struct v4l2_format *f)
+static int vidioc_log_status(struct file *file, void *priv)
 {
-	struct cx25821_fmt *fmt;
-	enum v4l2_field field;
-	unsigned int maxw, maxh;
-
-	fmt = cx25821_format_by_fourcc(f->fmt.pix.pixelformat);
-	if (NULL == fmt)
-		return -EINVAL;
-
-	field = f->fmt.pix.field;
-	maxw = 720;
-	maxh = 576;
-
-	if (V4L2_FIELD_ANY == field) {
-		if (f->fmt.pix.height > maxh / 2)
-			field = V4L2_FIELD_INTERLACED;
-		else
-			field = V4L2_FIELD_TOP;
-	}
-
-	switch (field) {
-	case V4L2_FIELD_TOP:
-	case V4L2_FIELD_BOTTOM:
-		maxh = maxh / 2;
-		break;
-	case V4L2_FIELD_INTERLACED:
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	f->fmt.pix.field = field;
-	if (f->fmt.pix.height < 32)
-		f->fmt.pix.height = 32;
-	if (f->fmt.pix.height > maxh)
-		f->fmt.pix.height = maxh;
-	if (f->fmt.pix.width < 48)
-		f->fmt.pix.width = 48;
-	if (f->fmt.pix.width > maxw)
-		f->fmt.pix.width = maxw;
-	f->fmt.pix.width &= ~0x03;
-	f->fmt.pix.bytesperline = (f->fmt.pix.width * fmt->depth) >> 3;
-	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+	struct cx25821_channel *chan = video_drvdata(file);
+	struct cx25821_dev *dev = chan->dev;
+	const struct sram_channel *sram_ch = chan->sram_channels;
+	u32 tmp = 0;
 
+	tmp = cx_read(sram_ch->dma_ctl);
+	pr_info("Video input 0 is %s\n",
+		(tmp & 0x11) ? "streaming" : "stopped");
 	return 0;
 }
 
-int cx25821_vidioc_querycap(struct file *file, void *priv,
+
+static int cx25821_vidioc_querycap(struct file *file, void *priv,
 			    struct v4l2_capability *cap)
 {
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
+	struct cx25821_channel *chan = video_drvdata(file);
+	struct cx25821_dev *dev = chan->dev;
+	const u32 cap_input = V4L2_CAP_VIDEO_CAPTURE |
+			V4L2_CAP_READWRITE | V4L2_CAP_STREAMING;
+	const u32 cap_output = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_READWRITE;
 
 	strcpy(cap->driver, "cx25821");
 	strlcpy(cap->card, cx25821_boards[dev->board].name, sizeof(cap->card));
 	sprintf(cap->bus_info, "PCIe:%s", pci_name(dev->pci));
-	cap->version = CX25821_VERSION_CODE;
-	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE |
-		V4L2_CAP_STREAMING;
-	if (UNSET != dev->tuner_type)
-		cap->capabilities |= V4L2_CAP_TUNER;
-	return 0;
-}
-
-int cx25821_vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
-			    struct v4l2_fmtdesc *f)
-{
-	if (unlikely(f->index >= ARRAY_SIZE(formats)))
-		return -EINVAL;
-
-	strlcpy(f->description, formats[f->index].name, sizeof(f->description));
-	f->pixelformat = formats[f->index].fourcc;
-
+	if (chan->id >= VID_CHANNEL_NUM)
+		cap->device_caps = cap_output;
+	else
+		cap->device_caps = cap_input;
+	cap->capabilities = cap_input | cap_output | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
-int cx25821_vidioc_reqbufs(struct file *file, void *priv,
+static int cx25821_vidioc_reqbufs(struct file *file, void *priv,
 			   struct v4l2_requestbuffers *p)
 {
-	struct cx25821_fh *fh = priv;
-	return videobuf_reqbufs(get_queue(fh), p);
+	struct cx25821_channel *chan = video_drvdata(file);
+
+	return videobuf_reqbufs(&chan->vidq, p);
 }
 
-int cx25821_vidioc_querybuf(struct file *file, void *priv,
+static int cx25821_vidioc_querybuf(struct file *file, void *priv,
 			    struct v4l2_buffer *p)
 {
-	struct cx25821_fh *fh = priv;
-	return videobuf_querybuf(get_queue(fh), p);
-}
+	struct cx25821_channel *chan = video_drvdata(file);
 
-int cx25821_vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *p)
-{
-	struct cx25821_fh *fh = priv;
-	return videobuf_qbuf(get_queue(fh), p);
+	return videobuf_querybuf(&chan->vidq, p);
 }
 
-int cx25821_vidioc_g_priority(struct file *file, void *f, enum v4l2_priority *p)
+static int cx25821_vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *p)
 {
-	struct cx25821_dev *dev = ((struct cx25821_fh *)f)->dev;
-	struct cx25821_fh *fh = f;
+	struct cx25821_channel *chan = video_drvdata(file);
 
-	*p = v4l2_prio_max(&dev->channels[fh->channel_id].prio);
-
-	return 0;
+	return videobuf_qbuf(&chan->vidq, p);
 }
 
-int cx25821_vidioc_s_priority(struct file *file, void *f,
-			      enum v4l2_priority prio)
+static int cx25821_vidioc_g_std(struct file *file, void *priv, v4l2_std_id *tvnorms)
 {
-	struct cx25821_fh *fh = f;
-	struct cx25821_dev *dev = ((struct cx25821_fh *)f)->dev;
+	struct cx25821_channel *chan = video_drvdata(file);
 
-	return v4l2_prio_change(&dev->channels[fh->channel_id].prio, &fh->prio,
-			prio);
+	*tvnorms = chan->dev->tvnorm;
+	return 0;
 }
 
-#ifdef TUNER_FLAG
-int cx25821_vidioc_s_std(struct file *file, void *priv, v4l2_std_id * tvnorms)
+static int cx25821_vidioc_s_std(struct file *file, void *priv,
+				v4l2_std_id tvnorms)
 {
-	struct cx25821_fh *fh = priv;
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
-	int err;
-
-	dprintk(1, "%s()\n", __func__);
-
-	if (fh) {
-		err = v4l2_prio_check(&dev->channels[fh->channel_id].prio,
-				      fh->prio);
-		if (0 != err)
-			return err;
-	}
+	struct cx25821_channel *chan = video_drvdata(file);
+	struct cx25821_dev *dev = chan->dev;
 
-	if (dev->tvnorm == *tvnorms)
+	if (dev->tvnorm == tvnorms)
 		return 0;
 
-	mutex_lock(&dev->lock);
-	cx25821_set_tvnorm(dev, *tvnorms);
-	mutex_unlock(&dev->lock);
+	dev->tvnorm = tvnorms;
+	chan->width = 720;
+	chan->height = (dev->tvnorm & V4L2_STD_625_50) ? 576 : 480;
 
 	medusa_set_videostandard(dev);
 
 	return 0;
 }
-#endif
 
-int cx25821_enum_input(struct cx25821_dev *dev, struct v4l2_input *i)
+static int cx25821_vidioc_enum_input(struct file *file, void *priv,
+			      struct v4l2_input *i)
 {
-	static const char * const iname[] = {
-		[CX25821_VMUX_COMPOSITE] = "Composite",
-		[CX25821_VMUX_SVIDEO] = "S-Video",
-		[CX25821_VMUX_DEBUG] = "for debug only",
-	};
-	unsigned int n;
-	dprintk(1, "%s()\n", __func__);
-
-	n = i->index;
-	if (n >= 2)
-		return -EINVAL;
-
-	if (0 == INPUT(n)->type)
+	if (i->index)
 		return -EINVAL;
 
 	i->type = V4L2_INPUT_TYPE_CAMERA;
-	strcpy(i->name, iname[INPUT(n)->type]);
-
 	i->std = CX25821_NORMS;
+	strcpy(i->name, "Composite");
 	return 0;
 }
 
-int cx25821_vidioc_enum_input(struct file *file, void *priv,
-			      struct v4l2_input *i)
-{
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
-	dprintk(1, "%s()\n", __func__);
-	return cx25821_enum_input(dev, i);
-}
-
-int cx25821_vidioc_g_input(struct file *file, void *priv, unsigned int *i)
-{
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
-
-	*i = dev->input;
-	dprintk(1, "%s(): returns %d\n", __func__, *i);
-	return 0;
-}
-
-int cx25821_vidioc_s_input(struct file *file, void *priv, unsigned int i)
-{
-	struct cx25821_fh *fh = priv;
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
-	int err;
-
-	dprintk(1, "%s(%d)\n", __func__, i);
-
-	if (fh) {
-		err = v4l2_prio_check(&dev->channels[fh->channel_id].prio,
-				      fh->prio);
-		if (0 != err)
-			return err;
-	}
-
-	if (i >= CX25821_NR_INPUT) {
-		dprintk(1, "%s(): -EINVAL\n", __func__);
-		return -EINVAL;
-	}
-
-	mutex_lock(&dev->lock);
-	cx25821_video_mux(dev, i);
-	mutex_unlock(&dev->lock);
-	return 0;
-}
-
-#ifdef TUNER_FLAG
-int cx25821_vidioc_g_frequency(struct file *file, void *priv,
-			       struct v4l2_frequency *f)
-{
-	struct cx25821_fh *fh = priv;
-	struct cx25821_dev *dev = fh->dev;
-
-	f->frequency = dev->freq;
-
-	cx25821_call_all(dev, tuner, g_frequency, f);
-
-	return 0;
-}
-
-int cx25821_set_freq(struct cx25821_dev *dev, struct v4l2_frequency *f)
-{
-	mutex_lock(&dev->lock);
-	dev->freq = f->frequency;
-
-	cx25821_call_all(dev, tuner, s_frequency, f);
-
-	/* When changing channels it is required to reset TVAUDIO */
-	msleep(10);
-
-	mutex_unlock(&dev->lock);
-
-	return 0;
-}
-
-int cx25821_vidioc_s_frequency(struct file *file, void *priv,
-			       struct v4l2_frequency *f)
-{
-	struct cx25821_fh *fh = priv;
-	struct cx25821_dev *dev;
-	int err;
-
-	if (fh) {
-		dev = fh->dev;
-		err = v4l2_prio_check(&dev->channels[fh->channel_id].prio,
-				      fh->prio);
-		if (0 != err)
-			return err;
-	} else {
-		pr_err("Invalid fh pointer!\n");
-		return -EINVAL;
-	}
-
-	return cx25821_set_freq(dev, f);
-}
-#endif
-
-#ifdef CONFIG_VIDEO_ADV_DEBUG
-int cx25821_vidioc_g_register(struct file *file, void *fh,
-		      struct v4l2_dbg_register *reg)
-{
-	struct cx25821_dev *dev = ((struct cx25821_fh *)fh)->dev;
-
-	if (!v4l2_chip_match_host(&reg->match))
-		return -EINVAL;
-
-	cx25821_call_all(dev, core, g_register, reg);
-
-	return 0;
-}
-
-int cx25821_vidioc_s_register(struct file *file, void *fh,
-		      struct v4l2_dbg_register *reg)
+static int cx25821_vidioc_g_input(struct file *file, void *priv, unsigned int *i)
 {
-	struct cx25821_dev *dev = ((struct cx25821_fh *)fh)->dev;
-
-	if (!v4l2_chip_match_host(&reg->match))
-		return -EINVAL;
-
-	cx25821_call_all(dev, core, s_register, reg);
-
+	*i = 0;
 	return 0;
 }
 
-#endif
-
-#ifdef TUNER_FLAG
-int cx25821_vidioc_g_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
+static int cx25821_vidioc_s_input(struct file *file, void *priv, unsigned int i)
 {
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
-
-	if (unlikely(UNSET == dev->tuner_type))
-		return -EINVAL;
-	if (0 != t->index)
-		return -EINVAL;
-
-	strcpy(t->name, "Television");
-	t->type = V4L2_TUNER_ANALOG_TV;
-	t->capability = V4L2_TUNER_CAP_NORM;
-	t->rangehigh = 0xffffffffUL;
-
-	t->signal = 0xffff;	/* LOCKED */
-	return 0;
+	return i ? -EINVAL : 0;
 }
 
-int cx25821_vidioc_s_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
+static int cx25821_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
-	struct cx25821_fh *fh = priv;
-	int err;
-
-	if (fh) {
-		err = v4l2_prio_check(&dev->channels[fh->channel_id].prio,
-				      fh->prio);
-		if (0 != err)
-			return err;
-	}
+	struct cx25821_channel *chan =
+		container_of(ctrl->handler, struct cx25821_channel, hdl);
+	struct cx25821_dev *dev = chan->dev;
 
-	dprintk(1, "%s()\n", __func__);
-	if (UNSET == dev->tuner_type)
-		return -EINVAL;
-	if (0 != t->index)
-		return -EINVAL;
-
-	return 0;
-}
-
-#endif
-/*****************************************************************************/
-static const struct v4l2_queryctrl no_ctl = {
-	.name = "42",
-	.flags = V4L2_CTRL_FLAG_DISABLED,
-};
-
-static struct v4l2_queryctrl cx25821_ctls[] = {
-	/* --- video --- */
-	{
-		.id = V4L2_CID_BRIGHTNESS,
-		.name = "Brightness",
-		.minimum = 0,
-		.maximum = 10000,
-		.step = 1,
-		.default_value = 6200,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-	}, {
-		.id = V4L2_CID_CONTRAST,
-		.name = "Contrast",
-		.minimum = 0,
-		.maximum = 10000,
-		.step = 1,
-		.default_value = 5000,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-	}, {
-		.id = V4L2_CID_SATURATION,
-		.name = "Saturation",
-		.minimum = 0,
-		.maximum = 10000,
-		.step = 1,
-		.default_value = 5000,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-	}, {
-		.id = V4L2_CID_HUE,
-		.name = "Hue",
-		.minimum = 0,
-		.maximum = 10000,
-		.step = 1,
-		.default_value = 5000,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-	}
-};
-static const int CX25821_CTLS = ARRAY_SIZE(cx25821_ctls);
-
-static int cx25821_ctrl_query(struct v4l2_queryctrl *qctrl)
-{
-	int i;
-
-	if (qctrl->id < V4L2_CID_BASE || qctrl->id >= V4L2_CID_LASTP1)
-		return -EINVAL;
-	for (i = 0; i < CX25821_CTLS; i++)
-		if (cx25821_ctls[i].id == qctrl->id)
-			break;
-	if (i == CX25821_CTLS) {
-		*qctrl = no_ctl;
-		return 0;
-	}
-	*qctrl = cx25821_ctls[i];
-	return 0;
-}
-
-int cx25821_vidioc_queryctrl(struct file *file, void *priv,
-		     struct v4l2_queryctrl *qctrl)
-{
-	return cx25821_ctrl_query(qctrl);
-}
-
-/* ------------------------------------------------------------------ */
-/* VIDEO CTRL IOCTLS                                                  */
-
-static const struct v4l2_queryctrl *ctrl_by_id(unsigned int id)
-{
-	unsigned int i;
-
-	for (i = 0; i < CX25821_CTLS; i++)
-		if (cx25821_ctls[i].id == id)
-			return cx25821_ctls + i;
-	return NULL;
-}
-
-int cx25821_vidioc_g_ctrl(struct file *file, void *priv,
-			  struct v4l2_control *ctl)
-{
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
-	struct cx25821_fh *fh = priv;
-
-	const struct v4l2_queryctrl *ctrl;
-
-	ctrl = ctrl_by_id(ctl->id);
-
-	if (NULL == ctrl)
-		return -EINVAL;
-	switch (ctl->id) {
+	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		ctl->value = dev->channels[fh->channel_id].ctl_bright;
+		medusa_set_brightness(dev, ctrl->val, chan->id);
 		break;
 	case V4L2_CID_HUE:
-		ctl->value = dev->channels[fh->channel_id].ctl_hue;
+		medusa_set_hue(dev, ctrl->val, chan->id);
 		break;
 	case V4L2_CID_CONTRAST:
-		ctl->value = dev->channels[fh->channel_id].ctl_contrast;
+		medusa_set_contrast(dev, ctrl->val, chan->id);
 		break;
 	case V4L2_CID_SATURATION:
-		ctl->value = dev->channels[fh->channel_id].ctl_saturation;
-		break;
-	}
-	return 0;
-}
-
-int cx25821_set_control(struct cx25821_dev *dev,
-			struct v4l2_control *ctl, int chan_num)
-{
-	int err;
-	const struct v4l2_queryctrl *ctrl;
-
-	err = -EINVAL;
-
-	ctrl = ctrl_by_id(ctl->id);
-
-	if (NULL == ctrl)
-		return err;
-
-	switch (ctrl->type) {
-	case V4L2_CTRL_TYPE_BOOLEAN:
-	case V4L2_CTRL_TYPE_MENU:
-	case V4L2_CTRL_TYPE_INTEGER:
-		if (ctl->value < ctrl->minimum)
-			ctl->value = ctrl->minimum;
-		if (ctl->value > ctrl->maximum)
-			ctl->value = ctrl->maximum;
+		medusa_set_saturation(dev, ctrl->val, chan->id);
 		break;
 	default:
-		/* nothing */ ;
-	}
-
-	switch (ctl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		dev->channels[chan_num].ctl_bright = ctl->value;
-		medusa_set_brightness(dev, ctl->value, chan_num);
-		break;
-	case V4L2_CID_HUE:
-		dev->channels[chan_num].ctl_hue = ctl->value;
-		medusa_set_hue(dev, ctl->value, chan_num);
-		break;
-	case V4L2_CID_CONTRAST:
-		dev->channels[chan_num].ctl_contrast = ctl->value;
-		medusa_set_contrast(dev, ctl->value, chan_num);
-		break;
-	case V4L2_CID_SATURATION:
-		dev->channels[chan_num].ctl_saturation = ctl->value;
-		medusa_set_saturation(dev, ctl->value, chan_num);
-		break;
-	}
-
-	err = 0;
-
-	return err;
-}
-
-static void cx25821_init_controls(struct cx25821_dev *dev, int chan_num)
-{
-	struct v4l2_control ctrl;
-	int i;
-	for (i = 0; i < CX25821_CTLS; i++) {
-		ctrl.id = cx25821_ctls[i].id;
-		ctrl.value = cx25821_ctls[i].default_value;
-
-		cx25821_set_control(dev, &ctrl, chan_num);
-	}
-}
-
-int cx25821_vidioc_cropcap(struct file *file, void *priv,
-			   struct v4l2_cropcap *cropcap)
-{
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
-
-	if (cropcap->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
-	cropcap->bounds.top = 0;
-	cropcap->bounds.left = 0;
-	cropcap->bounds.width = 720;
-	cropcap->bounds.height = dev->tvnorm == V4L2_STD_PAL_BG ? 576 : 480;
-	cropcap->pixelaspect.numerator =
-		dev->tvnorm == V4L2_STD_PAL_BG ? 59 : 10;
-	cropcap->pixelaspect.denominator =
-		dev->tvnorm == V4L2_STD_PAL_BG ? 54 : 11;
-	cropcap->defrect = cropcap->bounds;
-	return 0;
-}
-
-int cx25821_vidioc_s_crop(struct file *file, void *priv, const struct v4l2_crop *crop)
-{
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
-	struct cx25821_fh *fh = priv;
-	int err;
-
-	if (fh) {
-		err = v4l2_prio_check(&dev->channels[fh->channel_id].prio,
-				      fh->prio);
-		if (0 != err)
-			return err;
 	}
-	/* cx25821_vidioc_s_crop not supported */
-	return -EINVAL;
-}
-
-int cx25821_vidioc_g_crop(struct file *file, void *priv, struct v4l2_crop *crop)
-{
-	/* cx25821_vidioc_g_crop not supported */
-	return -EINVAL;
+	return 0;
 }
 
-int cx25821_vidioc_querystd(struct file *file, void *priv, v4l2_std_id * norm)
+static int cx25821_vidioc_enum_output(struct file *file, void *priv,
+			      struct v4l2_output *o)
 {
-	/* medusa does not support video standard sensing of current input */
-	*norm = CX25821_NORMS;
+	if (o->index)
+		return -EINVAL;
 
+	o->type = V4L2_INPUT_TYPE_CAMERA;
+	o->std = CX25821_NORMS;
+	strcpy(o->name, "Composite");
 	return 0;
 }
 
-int cx25821_is_valid_width(u32 width, v4l2_std_id tvnorm)
+static int cx25821_vidioc_g_output(struct file *file, void *priv, unsigned int *o)
 {
-	if (tvnorm == V4L2_STD_PAL_BG) {
-		if (width == 352 || width == 720)
-			return 1;
-		else
-			return 0;
-	}
-
-	if (tvnorm == V4L2_STD_NTSC_M) {
-		if (width == 320 || width == 352 || width == 720)
-			return 1;
-		else
-			return 0;
-	}
+	*o = 0;
 	return 0;
 }
 
-int cx25821_is_valid_height(u32 height, v4l2_std_id tvnorm)
+static int cx25821_vidioc_s_output(struct file *file, void *priv, unsigned int o)
 {
-	if (tvnorm == V4L2_STD_PAL_BG) {
-		if (height == 576 || height == 288)
-			return 1;
-		else
-			return 0;
-	}
-
-	if (tvnorm == V4L2_STD_NTSC_M) {
-		if (height == 480 || height == 240)
-			return 1;
-		else
-			return 0;
-	}
-
-	return 0;
+	return o ? -EINVAL : 0;
 }
 
-static long video_ioctl_upstream9(struct file *file, unsigned int cmd,
-				 unsigned long arg)
+static int cx25821_vidioc_try_fmt_vid_out(struct file *file, void *priv,
+				   struct v4l2_format *f)
 {
-	struct cx25821_fh *fh = file->private_data;
-	struct cx25821_dev *dev = fh->dev;
-	int command = 0;
-	struct upstream_user_struct *data_from_user;
-
-	data_from_user = (struct upstream_user_struct *)arg;
-
-	if (!data_from_user) {
-		pr_err("%s(): Upstream data is INVALID. Returning\n", __func__);
-		return 0;
-	}
-
-	command = data_from_user->command;
-
-	if (command != UPSTREAM_START_VIDEO && command != UPSTREAM_STOP_VIDEO)
-		return 0;
-
-	dev->input_filename = data_from_user->input_filename;
-	dev->input_audiofilename = data_from_user->input_filename;
-	dev->vid_stdname = data_from_user->vid_stdname;
-	dev->pixel_format = data_from_user->pixel_format;
-	dev->channel_select = data_from_user->channel_select;
-	dev->command = data_from_user->command;
-
-	switch (command) {
-	case UPSTREAM_START_VIDEO:
-		cx25821_start_upstream_video_ch1(dev, data_from_user);
-		break;
-
-	case UPSTREAM_STOP_VIDEO:
-		cx25821_stop_upstream_video_ch1(dev);
-		break;
-	}
+	struct cx25821_channel *chan = video_drvdata(file);
+	struct cx25821_dev *dev = chan->dev;
+	const struct cx25821_fmt *fmt;
 
+	fmt = cx25821_format_by_fourcc(f->fmt.pix.pixelformat);
+	if (NULL == fmt)
+		return -EINVAL;
+	f->fmt.pix.width = 720;
+	f->fmt.pix.height = (dev->tvnorm & V4L2_STD_625_50) ? 576 : 480;
+	f->fmt.pix.field = V4L2_FIELD_INTERLACED;
+	f->fmt.pix.bytesperline = (f->fmt.pix.width * fmt->depth) >> 3;
+	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	f->fmt.pix.priv = 0;
 	return 0;
 }
 
-static long video_ioctl_upstream10(struct file *file, unsigned int cmd,
-				  unsigned long arg)
+static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
+				struct v4l2_format *f)
 {
-	struct cx25821_fh *fh = file->private_data;
-	struct cx25821_dev *dev = fh->dev;
-	int command = 0;
-	struct upstream_user_struct *data_from_user;
-
-	data_from_user = (struct upstream_user_struct *)arg;
-
-	if (!data_from_user) {
-		pr_err("%s(): Upstream data is INVALID. Returning\n", __func__);
-		return 0;
-	}
-
-	command = data_from_user->command;
-
-	if (command != UPSTREAM_START_VIDEO && command != UPSTREAM_STOP_VIDEO)
-		return 0;
-
-	dev->input_filename_ch2 = data_from_user->input_filename;
-	dev->input_audiofilename = data_from_user->input_filename;
-	dev->vid_stdname_ch2 = data_from_user->vid_stdname;
-	dev->pixel_format_ch2 = data_from_user->pixel_format;
-	dev->channel_select_ch2 = data_from_user->channel_select;
-	dev->command_ch2 = data_from_user->command;
+	struct cx25821_channel *chan = video_drvdata(file);
+	int err;
 
-	switch (command) {
-	case UPSTREAM_START_VIDEO:
-		cx25821_start_upstream_video_ch2(dev, data_from_user);
-		break;
+	err = cx25821_vidioc_try_fmt_vid_out(file, priv, f);
 
-	case UPSTREAM_STOP_VIDEO:
-		cx25821_stop_upstream_video_ch2(dev);
-		break;
-	}
+	if (0 != err)
+		return err;
 
+	chan->fmt = cx25821_format_by_fourcc(f->fmt.pix.pixelformat);
+	chan->vidq.field = f->fmt.pix.field;
+	chan->width = f->fmt.pix.width;
+	chan->height = f->fmt.pix.height;
+	if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_Y41P)
+		chan->pixel_formats = PIXEL_FRMT_411;
+	else
+		chan->pixel_formats = PIXEL_FRMT_422;
 	return 0;
 }
 
-static long video_ioctl_upstream11(struct file *file, unsigned int cmd,
-				  unsigned long arg)
+static ssize_t video_write(struct file *file, const char __user *data, size_t count,
+			 loff_t *ppos)
 {
-	struct cx25821_fh *fh = file->private_data;
-	struct cx25821_dev *dev = fh->dev;
-	int command = 0;
-	struct upstream_user_struct *data_from_user;
+	struct cx25821_channel *chan = video_drvdata(file);
+	struct cx25821_dev *dev = chan->dev;
+	struct v4l2_fh *fh = file->private_data;
+	int err = 0;
 
-	data_from_user = (struct upstream_user_struct *)arg;
-
-	if (!data_from_user) {
-		pr_err("%s(): Upstream data is INVALID. Returning\n", __func__);
-		return 0;
+	if (mutex_lock_interruptible(&dev->lock))
+		return -ERESTARTSYS;
+	if (chan->streaming_fh && chan->streaming_fh != fh) {
+		err = -EBUSY;
+		goto unlock;
 	}
-
-	command = data_from_user->command;
-
-	if (command != UPSTREAM_START_AUDIO && command != UPSTREAM_STOP_AUDIO)
-		return 0;
-
-	dev->input_filename = data_from_user->input_filename;
-	dev->input_audiofilename = data_from_user->input_filename;
-	dev->vid_stdname = data_from_user->vid_stdname;
-	dev->pixel_format = data_from_user->pixel_format;
-	dev->channel_select = data_from_user->channel_select;
-	dev->command = data_from_user->command;
-
-	switch (command) {
-	case UPSTREAM_START_AUDIO:
-		cx25821_start_upstream_audio(dev, data_from_user);
-		break;
-
-	case UPSTREAM_STOP_AUDIO:
-		cx25821_stop_upstream_audio(dev);
-		break;
+	if (!chan->streaming_fh) {
+		err = cx25821_vidupstream_init(chan, chan->pixel_formats);
+		if (err)
+			goto unlock;
+		chan->streaming_fh = fh;
 	}
 
-	return 0;
+	err = cx25821_write_frame(chan, data, count);
+	count -= err;
+	*ppos += err;
+
+unlock:
+	mutex_unlock(&dev->lock);
+	return err;
 }
 
-static long video_ioctl_set(struct file *file, unsigned int cmd,
-			   unsigned long arg)
+static int video_out_release(struct file *file)
 {
-	struct cx25821_fh *fh = file->private_data;
-	struct cx25821_dev *dev = fh->dev;
-	struct downstream_user_struct *data_from_user;
-	int command;
-	int width = 720;
-	int selected_channel = 0;
-	int pix_format = 0;
-	int i = 0;
-	int cif_enable = 0;
-	int cif_width = 0;
-
-	data_from_user = (struct downstream_user_struct *)arg;
-
-	if (!data_from_user) {
-		pr_err("%s(): User data is INVALID. Returning\n", __func__);
-		return 0;
-	}
-
-	command = data_from_user->command;
-
-	if (command != SET_VIDEO_STD && command != SET_PIXEL_FORMAT
-	   && command != ENABLE_CIF_RESOLUTION && command != REG_READ
-	   && command != REG_WRITE && command != MEDUSA_READ
-	   && command != MEDUSA_WRITE) {
-		return 0;
-	}
-
-	switch (command) {
-	case SET_VIDEO_STD:
-		if (!strcmp(data_from_user->vid_stdname, "PAL"))
-			dev->tvnorm = V4L2_STD_PAL_BG;
-		else
-			dev->tvnorm = V4L2_STD_NTSC_M;
-		medusa_set_videostandard(dev);
-		break;
-
-	case SET_PIXEL_FORMAT:
-		selected_channel = data_from_user->decoder_select;
-		pix_format = data_from_user->pixel_format;
-
-		if (!(selected_channel <= 7 && selected_channel >= 0)) {
-			selected_channel -= 4;
-			selected_channel = selected_channel % 8;
-		}
-
-		if (selected_channel >= 0)
-			cx25821_set_pixel_format(dev, selected_channel,
-						pix_format);
-
-		break;
-
-	case ENABLE_CIF_RESOLUTION:
-		selected_channel = data_from_user->decoder_select;
-		cif_enable = data_from_user->cif_resolution_enable;
-		cif_width = data_from_user->cif_width;
-
-		if (cif_enable) {
-			if (dev->tvnorm & V4L2_STD_PAL_BG
-			    || dev->tvnorm & V4L2_STD_PAL_DK) {
-				width = 352;
-			} else {
-				width = cif_width;
-				if (cif_width != 320 && cif_width != 352)
-					width = 320;
-			}
-		}
-
-		if (!(selected_channel <= 7 && selected_channel >= 0)) {
-			selected_channel -= 4;
-			selected_channel = selected_channel % 8;
-		}
+	struct cx25821_channel *chan = video_drvdata(file);
+	struct cx25821_dev *dev = chan->dev;
+	struct v4l2_fh *fh = file->private_data;
 
-		if (selected_channel <= 7 && selected_channel >= 0) {
-			dev->channels[selected_channel].use_cif_resolution =
-				cif_enable;
-			dev->channels[selected_channel].cif_width = width;
-		} else {
-			for (i = 0; i < VID_CHANNEL_NUM; i++) {
-				dev->channels[i].use_cif_resolution =
-					cif_enable;
-				dev->channels[i].cif_width = width;
-			}
-		}
-
-		medusa_set_resolution(dev, width, selected_channel);
-		break;
-	case REG_READ:
-		data_from_user->reg_data = cx_read(data_from_user->reg_address);
-		break;
-	case REG_WRITE:
-		cx_write(data_from_user->reg_address, data_from_user->reg_data);
-		break;
-	case MEDUSA_READ:
-		cx25821_i2c_read(&dev->i2c_bus[0],
-					 (u16) data_from_user->reg_address,
-					 &data_from_user->reg_data);
-		break;
-	case MEDUSA_WRITE:
-		cx25821_i2c_write(&dev->i2c_bus[0],
-				  (u16) data_from_user->reg_address,
-				  data_from_user->reg_data);
-		break;
+	mutex_lock(&dev->lock);
+	if (chan->streaming_fh == fh) {
+		cx25821_stop_upstream_video(chan);
+		chan->streaming_fh = NULL;
 	}
+	mutex_unlock(&dev->lock);
 
-	return 0;
+	return v4l2_fh_release(file);
 }
 
-static long cx25821_video_ioctl(struct file *file,
-				unsigned int cmd, unsigned long arg)
-{
-	int ret = 0;
-
-	struct cx25821_fh *fh = file->private_data;
-
-	/* check to see if it's the video upstream */
-	if (fh->channel_id == SRAM_CH09) {
-		ret = video_ioctl_upstream9(file, cmd, arg);
-		return ret;
-	} else if (fh->channel_id == SRAM_CH10) {
-		ret = video_ioctl_upstream10(file, cmd, arg);
-		return ret;
-	} else if (fh->channel_id == SRAM_CH11) {
-		ret = video_ioctl_upstream11(file, cmd, arg);
-		ret = video_ioctl_set(file, cmd, arg);
-		return ret;
-	}
-
-	return video_ioctl2(file, cmd, arg);
-}
+static const struct v4l2_ctrl_ops cx25821_ctrl_ops = {
+	.s_ctrl = cx25821_s_ctrl,
+};
 
-/* exported stuff */
 static const struct v4l2_file_operations video_fops = {
 	.owner = THIS_MODULE,
-	.open = video_open,
+	.open = v4l2_fh_open,
 	.release = video_release,
 	.read = video_read,
 	.poll = video_poll,
 	.mmap = cx25821_video_mmap,
-	.ioctl = cx25821_video_ioctl,
+	.unlocked_ioctl = video_ioctl2,
 };
 
 static const struct v4l2_ioctl_ops video_ioctl_ops = {
@@ -1951,40 +963,170 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_querybuf = cx25821_vidioc_querybuf,
 	.vidioc_qbuf = cx25821_vidioc_qbuf,
 	.vidioc_dqbuf = vidioc_dqbuf,
-#ifdef TUNER_FLAG
+	.vidioc_g_std = cx25821_vidioc_g_std,
 	.vidioc_s_std = cx25821_vidioc_s_std,
-	.vidioc_querystd = cx25821_vidioc_querystd,
-#endif
-	.vidioc_cropcap = cx25821_vidioc_cropcap,
-	.vidioc_s_crop = cx25821_vidioc_s_crop,
-	.vidioc_g_crop = cx25821_vidioc_g_crop,
 	.vidioc_enum_input = cx25821_vidioc_enum_input,
 	.vidioc_g_input = cx25821_vidioc_g_input,
 	.vidioc_s_input = cx25821_vidioc_s_input,
-	.vidioc_g_ctrl = cx25821_vidioc_g_ctrl,
-	.vidioc_s_ctrl = vidioc_s_ctrl,
-	.vidioc_queryctrl = cx25821_vidioc_queryctrl,
 	.vidioc_streamon = vidioc_streamon,
 	.vidioc_streamoff = vidioc_streamoff,
 	.vidioc_log_status = vidioc_log_status,
-	.vidioc_g_priority = cx25821_vidioc_g_priority,
-	.vidioc_s_priority = cx25821_vidioc_s_priority,
-#ifdef TUNER_FLAG
-	.vidioc_g_tuner = cx25821_vidioc_g_tuner,
-	.vidioc_s_tuner = cx25821_vidioc_s_tuner,
-	.vidioc_g_frequency = cx25821_vidioc_g_frequency,
-	.vidioc_s_frequency = cx25821_vidioc_s_frequency,
-#endif
-#ifdef CONFIG_VIDEO_ADV_DEBUG
-	.vidioc_g_register = cx25821_vidioc_g_register,
-	.vidioc_s_register = cx25821_vidioc_s_register,
-#endif
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
-struct video_device cx25821_videoioctl_template = {
-	.name = "cx25821-videoioctl",
+static const struct video_device cx25821_video_device = {
+	.name = "cx25821-video",
 	.fops = &video_fops,
+	.release = video_device_release_empty,
+	.minor = -1,
 	.ioctl_ops = &video_ioctl_ops,
 	.tvnorms = CX25821_NORMS,
-	.current_norm = V4L2_STD_NTSC_M,
 };
+
+static const struct v4l2_file_operations video_out_fops = {
+	.owner = THIS_MODULE,
+	.open = v4l2_fh_open,
+	.write = video_write,
+	.release = video_out_release,
+	.unlocked_ioctl = video_ioctl2,
+};
+
+static const struct v4l2_ioctl_ops video_out_ioctl_ops = {
+	.vidioc_querycap = cx25821_vidioc_querycap,
+	.vidioc_enum_fmt_vid_out = cx25821_vidioc_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_out = cx25821_vidioc_g_fmt_vid_cap,
+	.vidioc_try_fmt_vid_out = cx25821_vidioc_try_fmt_vid_out,
+	.vidioc_s_fmt_vid_out = vidioc_s_fmt_vid_out,
+	.vidioc_g_std = cx25821_vidioc_g_std,
+	.vidioc_s_std = cx25821_vidioc_s_std,
+	.vidioc_enum_output = cx25821_vidioc_enum_output,
+	.vidioc_g_output = cx25821_vidioc_g_output,
+	.vidioc_s_output = cx25821_vidioc_s_output,
+	.vidioc_log_status = vidioc_log_status,
+};
+
+static const struct video_device cx25821_video_out_device = {
+	.name = "cx25821-video",
+	.fops = &video_out_fops,
+	.release = video_device_release_empty,
+	.minor = -1,
+	.ioctl_ops = &video_out_ioctl_ops,
+	.tvnorms = CX25821_NORMS,
+};
+
+void cx25821_video_unregister(struct cx25821_dev *dev, int chan_num)
+{
+	cx_clear(PCI_INT_MSK, 1);
+
+	if (video_is_registered(&dev->channels[chan_num].vdev)) {
+		video_unregister_device(&dev->channels[chan_num].vdev);
+		v4l2_ctrl_handler_free(&dev->channels[chan_num].hdl);
+
+		btcx_riscmem_free(dev->pci,
+				&dev->channels[chan_num].dma_vidq.stopper);
+	}
+}
+
+int cx25821_video_register(struct cx25821_dev *dev)
+{
+	int err;
+	int i;
+
+	/* initial device configuration */
+	dev->tvnorm = V4L2_STD_NTSC_M;
+
+	spin_lock_init(&dev->slock);
+
+	for (i = 0; i < MAX_VID_CHANNEL_NUM - 1; ++i) {
+		struct cx25821_channel *chan = &dev->channels[i];
+		struct video_device *vdev = &chan->vdev;
+		struct v4l2_ctrl_handler *hdl = &chan->hdl;
+		bool is_output = i > SRAM_CH08;
+
+		if (i == SRAM_CH08) /* audio channel */
+			continue;
+
+		if (!is_output) {
+			v4l2_ctrl_handler_init(hdl, 4);
+			v4l2_ctrl_new_std(hdl, &cx25821_ctrl_ops,
+					V4L2_CID_BRIGHTNESS, 0, 10000, 1, 6200);
+			v4l2_ctrl_new_std(hdl, &cx25821_ctrl_ops,
+					V4L2_CID_CONTRAST, 0, 10000, 1, 5000);
+			v4l2_ctrl_new_std(hdl, &cx25821_ctrl_ops,
+					V4L2_CID_SATURATION, 0, 10000, 1, 5000);
+			v4l2_ctrl_new_std(hdl, &cx25821_ctrl_ops,
+					V4L2_CID_HUE, 0, 10000, 1, 5000);
+			if (hdl->error) {
+				err = hdl->error;
+				goto fail_unreg;
+			}
+			err = v4l2_ctrl_handler_setup(hdl);
+			if (err)
+				goto fail_unreg;
+		} else {
+			chan->out = &dev->vid_out_data[i - SRAM_CH09];
+			chan->out->chan = chan;
+		}
+
+		cx25821_risc_stopper(dev->pci, &chan->dma_vidq.stopper,
+			chan->sram_channels->dma_ctl, 0x11, 0);
+
+		chan->sram_channels = &cx25821_sram_channels[i];
+		chan->width = 720;
+		if (dev->tvnorm & V4L2_STD_625_50)
+			chan->height = 576;
+		else
+			chan->height = 480;
+
+		if (chan->pixel_formats == PIXEL_FRMT_411)
+			chan->fmt = cx25821_format_by_fourcc(V4L2_PIX_FMT_Y41P);
+		else
+			chan->fmt = cx25821_format_by_fourcc(V4L2_PIX_FMT_YUYV);
+
+		cx_write(chan->sram_channels->int_stat, 0xffffffff);
+
+		INIT_LIST_HEAD(&chan->dma_vidq.active);
+		INIT_LIST_HEAD(&chan->dma_vidq.queued);
+
+		chan->timeout_data.dev = dev;
+		chan->timeout_data.channel = &cx25821_sram_channels[i];
+		chan->dma_vidq.timeout.function = cx25821_vid_timeout;
+		chan->dma_vidq.timeout.data = (unsigned long)&chan->timeout_data;
+		init_timer(&chan->dma_vidq.timeout);
+
+		if (!is_output)
+			videobuf_queue_sg_init(&chan->vidq, &cx25821_video_qops, &dev->pci->dev,
+				&dev->slock, V4L2_BUF_TYPE_VIDEO_CAPTURE,
+				V4L2_FIELD_INTERLACED, sizeof(struct cx25821_buffer),
+				chan, &dev->lock);
+
+		/* register v4l devices */
+		*vdev = is_output ? cx25821_video_out_device : cx25821_video_device;
+		vdev->v4l2_dev = &dev->v4l2_dev;
+		if (!is_output)
+			vdev->ctrl_handler = hdl;
+		else
+			vdev->vfl_dir = VFL_DIR_TX;
+		vdev->lock = &dev->lock;
+		set_bit(V4L2_FL_USE_FH_PRIO, &vdev->flags);
+		snprintf(vdev->name, sizeof(vdev->name), "%s #%d", dev->name, i);
+		video_set_drvdata(vdev, chan);
+
+		err = video_register_device(vdev, VFL_TYPE_GRABBER,
+					    video_nr[dev->nr]);
+
+		if (err < 0)
+			goto fail_unreg;
+	}
+
+	/* set PCI interrupt */
+	cx_set(PCI_INT_MSK, 0xff);
+
+	return 0;
+
+fail_unreg:
+	while (i >= 0)
+		cx25821_video_unregister(dev, i--);
+	return err;
+}
diff --git a/drivers/media/pci/cx25821/cx25821-video.h b/drivers/media/pci/cx25821/cx25821-video.h
index c265e35b37c..ab63b3858ac 100644
--- a/drivers/media/pci/cx25821/cx25821-video.h
+++ b/drivers/media/pci/cx25821/cx25821-video.h
@@ -39,8 +39,7 @@
 #include "cx25821.h"
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
-
-#define TUNER_FLAG
+#include <media/v4l2-event.h>
 
 #define VIDEO_DEBUG 0
 
@@ -50,137 +49,17 @@ do {									\
 		printk(KERN_DEBUG "%s/0: " fmt, dev->name, ##arg);	\
 } while (0)
 
-/* For IOCTL to identify running upstream */
-#define UPSTREAM_START_VIDEO        700
-#define UPSTREAM_STOP_VIDEO         701
-#define UPSTREAM_START_AUDIO        702
-#define UPSTREAM_STOP_AUDIO         703
-#define UPSTREAM_DUMP_REGISTERS     702
-#define SET_VIDEO_STD               800
-#define SET_PIXEL_FORMAT            1000
-#define ENABLE_CIF_RESOLUTION       1001
-
-#define REG_READ		    900
-#define REG_WRITE		    901
-#define MEDUSA_READ		    910
-#define MEDUSA_WRITE		    911
-
-extern struct sram_channel *channel0;
-extern struct sram_channel *channel1;
-extern struct sram_channel *channel2;
-extern struct sram_channel *channel3;
-extern struct sram_channel *channel4;
-extern struct sram_channel *channel5;
-extern struct sram_channel *channel6;
-extern struct sram_channel *channel7;
-extern struct sram_channel *channel9;
-extern struct sram_channel *channel10;
-extern struct sram_channel *channel11;
-extern struct video_device cx25821_videoioctl_template;
-/* extern const u32 *ctrl_classes[]; */
-
-extern unsigned int vid_limit;
-
 #define FORMAT_FLAGS_PACKED       0x01
-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_video_wakeup(struct cx25821_dev *dev,
 				 struct cx25821_dmaqueue *q, u32 count);
 
-#ifdef TUNER_FLAG
-extern int cx25821_set_tvnorm(struct cx25821_dev *dev, v4l2_std_id norm);
-#endif
-
-extern int cx25821_res_get(struct cx25821_dev *dev, struct cx25821_fh *fh,
-			   unsigned int bit);
-extern int cx25821_res_check(struct cx25821_fh *fh, unsigned int bit);
-extern int cx25821_res_locked(struct cx25821_fh *fh, unsigned int bit);
-extern void cx25821_res_free(struct cx25821_dev *dev, struct cx25821_fh *fh,
-			     unsigned int bits);
-extern int cx25821_video_mux(struct cx25821_dev *dev, unsigned int input);
 extern int cx25821_start_video_dma(struct cx25821_dev *dev,
 				   struct cx25821_dmaqueue *q,
 				   struct cx25821_buffer *buf,
-				   struct sram_channel *channel);
+				   const struct sram_channel *channel);
 
-extern int cx25821_set_scale(struct cx25821_dev *dev, unsigned int width,
-			     unsigned int height, enum v4l2_field field);
 extern int cx25821_video_irq(struct cx25821_dev *dev, int chan_num, u32 status);
 extern void cx25821_video_unregister(struct cx25821_dev *dev, int chan_num);
 extern int cx25821_video_register(struct cx25821_dev *dev);
-extern int cx25821_get_format_size(void);
-
-extern int cx25821_buffer_setup(struct videobuf_queue *q, unsigned int *count,
-				unsigned int *size);
-extern int cx25821_buffer_prepare(struct videobuf_queue *q,
-				  struct videobuf_buffer *vb,
-				  enum v4l2_field field);
-extern void cx25821_buffer_release(struct videobuf_queue *q,
-				   struct videobuf_buffer *vb);
-extern struct videobuf_queue *get_queue(struct cx25821_fh *fh);
-extern int cx25821_get_resource(struct cx25821_fh *fh, int resource);
-extern int cx25821_video_mmap(struct file *file, struct vm_area_struct *vma);
-extern int cx25821_vidioc_try_fmt_vid_cap(struct file *file, void *priv,
-					  struct v4l2_format *f);
-extern int cx25821_vidioc_querycap(struct file *file, void *priv,
-				   struct v4l2_capability *cap);
-extern int cx25821_vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
-					   struct v4l2_fmtdesc *f);
-extern int cx25821_vidioc_reqbufs(struct file *file, void *priv,
-				  struct v4l2_requestbuffers *p);
-extern int cx25821_vidioc_querybuf(struct file *file, void *priv,
-				   struct v4l2_buffer *p);
-extern int cx25821_vidioc_qbuf(struct file *file, void *priv,
-			       struct v4l2_buffer *p);
-extern int cx25821_vidioc_s_std(struct file *file, void *priv,
-				v4l2_std_id *tvnorms);
-extern int cx25821_enum_input(struct cx25821_dev *dev, struct v4l2_input *i);
-extern int cx25821_vidioc_enum_input(struct file *file, void *priv,
-				     struct v4l2_input *i);
-extern int cx25821_vidioc_g_input(struct file *file, void *priv,
-				  unsigned int *i);
-extern int cx25821_vidioc_s_input(struct file *file, void *priv,
-				  unsigned int i);
-extern int cx25821_vidioc_g_ctrl(struct file *file, void *priv,
-				 struct v4l2_control *ctl);
-extern int cx25821_vidioc_g_fmt_vid_cap(struct file *file, void *priv,
-					struct v4l2_format *f);
-extern int cx25821_vidioc_g_frequency(struct file *file, void *priv,
-				      struct v4l2_frequency *f);
-extern int cx25821_set_freq(struct cx25821_dev *dev, struct v4l2_frequency *f);
-extern int cx25821_vidioc_s_frequency(struct file *file, void *priv,
-				      struct v4l2_frequency *f);
-extern int cx25821_vidioc_g_register(struct file *file, void *fh,
-				     struct v4l2_dbg_register *reg);
-extern int cx25821_vidioc_s_register(struct file *file, void *fh,
-				     struct v4l2_dbg_register *reg);
-extern int cx25821_vidioc_g_tuner(struct file *file, void *priv,
-				  struct v4l2_tuner *t);
-extern int cx25821_vidioc_s_tuner(struct file *file, void *priv,
-				  struct v4l2_tuner *t);
-
-extern int cx25821_is_valid_width(u32 width, v4l2_std_id tvnorm);
-extern int cx25821_is_valid_height(u32 height, v4l2_std_id tvnorm);
-
-extern int cx25821_vidioc_g_priority(struct file *file, void *f,
-				     enum v4l2_priority *p);
-extern int cx25821_vidioc_s_priority(struct file *file, void *f,
-				     enum v4l2_priority prio);
-
-extern int cx25821_vidioc_queryctrl(struct file *file, void *priv,
-				    struct v4l2_queryctrl *qctrl);
-extern int cx25821_set_control(struct cx25821_dev *dev,
-			       struct v4l2_control *ctrl, int chan_num);
-
-extern int cx25821_vidioc_cropcap(struct file *file, void *fh,
-				  struct v4l2_cropcap *cropcap);
-extern int cx25821_vidioc_s_crop(struct file *file, void *priv,
-				 const struct v4l2_crop *crop);
-extern int cx25821_vidioc_g_crop(struct file *file, void *priv,
-				 struct v4l2_crop *crop);
 
-extern int cx25821_vidioc_querystd(struct file *file, void *priv,
-				   v4l2_std_id *norm);
 #endif
diff --git a/drivers/media/pci/cx25821/cx25821.h b/drivers/media/pci/cx25821/cx25821.h
index 8a9c0c86941..90bdc196929 100644
--- a/drivers/media/pci/cx25821/cx25821.h
+++ b/drivers/media/pci/cx25821/cx25821.h
@@ -33,17 +33,14 @@
 
 #include <media/v4l2-common.h>
 #include <media/v4l2-device.h>
-#include <media/tuner.h>
-#include <media/tveeprom.h>
+#include <media/v4l2-ctrls.h>
 #include <media/videobuf-dma-sg.h>
-#include <media/videobuf-dvb.h>
 
 #include "btcx-risc.h"
 #include "cx25821-reg.h"
 #include "cx25821-medusa-reg.h"
 #include "cx25821-sram.h"
 #include "cx25821-audio.h"
-#include "media/cx2341x.h"
 
 #include <linux/version.h>
 #include <linux/mutex.h>
@@ -55,8 +52,6 @@
 
 #define CX25821_MAXBOARDS 2
 
-#define TRUE    1
-#define FALSE   0
 #define LINE_SIZE_D1    1440
 
 /* Number of decoders and encoders */
@@ -67,7 +62,6 @@
 
 /* Max number of inputs by card */
 #define MAX_CX25821_INPUT     8
-#define INPUT(nr) (&cx25821_boards[dev->board].input[nr])
 #define RESOURCE_VIDEO0       1
 #define RESOURCE_VIDEO1       2
 #define RESOURCE_VIDEO2       4
@@ -80,7 +74,6 @@
 #define RESOURCE_VIDEO9       512
 #define RESOURCE_VIDEO10      1024
 #define RESOURCE_VIDEO11      2048
-#define RESOURCE_VIDEO_IOCTL  4096
 
 #define BUFFER_TIMEOUT     (HZ)	/* 0.5 seconds */
 
@@ -97,7 +90,6 @@
 #define CX25821_BOARD_CONEXANT_ATHENA10 1
 #define MAX_VID_CHANNEL_NUM     12
 #define VID_CHANNEL_NUM 8
-#define CX25821_NR_INPUT 2
 
 struct cx25821_fmt {
 	char *name;
@@ -107,14 +99,6 @@ struct cx25821_fmt {
 	u32 cxformat;
 };
 
-struct cx25821_ctrl {
-	struct v4l2_queryctrl v;
-	u32 off;
-	u32 reg;
-	u32 mask;
-	u32 shift;
-};
-
 struct cx25821_tvnorm {
 	char *name;
 	v4l2_std_id id;
@@ -122,40 +106,6 @@ struct cx25821_tvnorm {
 	u32 cxoformat;
 };
 
-struct cx25821_fh {
-	struct cx25821_dev *dev;
-	enum v4l2_buf_type type;
-	int radio;
-	u32 resources;
-
-	enum v4l2_priority prio;
-
-	/* video overlay */
-	struct v4l2_window win;
-	struct v4l2_clip *clips;
-	unsigned int nclips;
-
-	/* video capture */
-	struct cx25821_fmt *fmt;
-	unsigned int width, height;
-	int channel_id;
-
-	/* vbi capture */
-	struct videobuf_queue vidq;
-	struct videobuf_queue vbiq;
-
-	/* H264 Encoder specifics ONLY */
-	struct videobuf_queue mpegq;
-	atomic_t v4l_reading;
-};
-
-enum cx25821_itype {
-	CX25821_VMUX_COMPOSITE = 1,
-	CX25821_VMUX_SVIDEO,
-	CX25821_VMUX_DEBUG,
-	CX25821_RADIO,
-};
-
 enum cx25821_src_sel_type {
 	CX25821_SRC_SEL_EXT_656_VIDEO = 0,
 	CX25821_SRC_SEL_PARALLEL_MPEG_VIDEO
@@ -169,16 +119,10 @@ struct cx25821_buffer {
 	/* cx25821 specific */
 	unsigned int bpl;
 	struct btcx_riscmem risc;
-	struct cx25821_fmt *fmt;
+	const struct cx25821_fmt *fmt;
 	u32 count;
 };
 
-struct cx25821_input {
-	enum cx25821_itype type;
-	unsigned int vmux;
-	u32 gpio0, gpio1, gpio2, gpio3;
-};
-
 enum port {
 	CX25821_UNDEFINED = 0,
 	CX25821_RAW,
@@ -190,19 +134,8 @@ struct cx25821_board {
 	enum port porta;
 	enum port portb;
 	enum port portc;
-	unsigned int tuner_type;
-	unsigned int radio_type;
-	unsigned char tuner_addr;
-	unsigned char radio_addr;
 
 	u32 clk_freq;
-	struct cx25821_input input[CX25821_NR_INPUT];
-};
-
-struct cx25821_subid {
-	u16 subvendor;
-	u16 subdevice;
-	u32 card;
 };
 
 struct cx25821_i2c {
@@ -234,34 +167,70 @@ struct cx25821_dmaqueue {
 
 struct cx25821_data {
 	struct cx25821_dev *dev;
-	struct sram_channel *channel;
+	const struct sram_channel *channel;
+};
+
+struct cx25821_dev;
+
+struct cx25821_channel;
+
+struct cx25821_video_out_data {
+	struct cx25821_channel *chan;
+	int _line_size;
+	int _prog_cnt;
+	int _pixel_format;
+	int _is_first_frame;
+	int _is_running;
+	int _file_status;
+	int _lines_count;
+	int _frame_count;
+	unsigned int _risc_size;
+
+	__le32 *_dma_virt_start_addr;
+	__le32 *_dma_virt_addr;
+	dma_addr_t _dma_phys_addr;
+	dma_addr_t _dma_phys_start_addr;
+
+	unsigned int _data_buf_size;
+	__le32 *_data_buf_virt_addr;
+	dma_addr_t _data_buf_phys_addr;
+
+	u32 upstream_riscbuf_size;
+	u32 upstream_databuf_size;
+	int is_60hz;
+	int _frame_index;
+	int cur_frame_index;
+	int curpos;
+	wait_queue_head_t waitq;
 };
 
 struct cx25821_channel {
-	struct v4l2_prio_state prio;
+	unsigned id;
+	struct cx25821_dev *dev;
+	struct v4l2_fh *streaming_fh;
 
-	int ctl_bright;
-	int ctl_contrast;
-	int ctl_hue;
-	int ctl_saturation;
+	struct v4l2_ctrl_handler hdl;
 	struct cx25821_data timeout_data;
 
-	struct video_device *video_dev;
-	struct cx25821_dmaqueue vidq;
-
-	struct sram_channel *sram_channels;
+	struct video_device vdev;
+	struct cx25821_dmaqueue dma_vidq;
+	struct videobuf_queue vidq;
 
-	struct mutex lock;
-	int resources;
+	const struct sram_channel *sram_channels;
 
+	const struct cx25821_fmt *fmt;
+	unsigned int width, height;
 	int pixel_formats;
 	int use_cif_resolution;
 	int cif_width;
+
+	/* video output data for the video output channel */
+	struct cx25821_video_out_data *out;
 };
 
+struct snd_card;
+
 struct cx25821_dev {
-	struct list_head devlist;
-	atomic_t refcount;
 	struct v4l2_device v4l2_dev;
 
 	/* pci stuff */
@@ -273,6 +242,8 @@ struct cx25821_dev {
 	u8 __iomem *bmmio;
 	int pci_irqmask;
 	int hwrevision;
+	/* used by cx25821-alsa */
+	struct snd_card *card;
 
 	u32 clk_freq;
 
@@ -289,17 +260,8 @@ struct cx25821_dev {
 	char name[32];
 
 	/* Analog video */
-	u32 resources;
 	unsigned int input;
-	u32 tvaudio;
 	v4l2_std_id tvnorm;
-	unsigned int tuner_type;
-	unsigned char tuner_addr;
-	unsigned int radio_type;
-	unsigned char radio_addr;
-	unsigned int has_radio;
-	unsigned int videc_type;
-	unsigned char videc_addr;
 	unsigned short _max_num_decoders;
 
 	/* Analog Audio Upstream */
@@ -323,132 +285,26 @@ struct cx25821_dev {
 	__le32 *_audiodata_buf_virt_addr;
 	dma_addr_t _audiodata_buf_phys_addr;
 	char *_audiofilename;
-
-	/* V4l */
-	u32 freq;
-	struct video_device *vbi_dev;
-	struct video_device *radio_dev;
-	struct video_device *ioctl_dev;
-
-	spinlock_t slock;
-
-	/* Video Upstream */
-	int _line_size;
-	int _prog_cnt;
-	int _pixel_format;
-	int _is_first_frame;
-	int _is_running;
-	int _file_status;
-	int _lines_count;
-	int _frame_count;
-	int _channel_upstream_select;
-	unsigned int _risc_size;
-
-	__le32 *_dma_virt_start_addr;
-	__le32 *_dma_virt_addr;
-	dma_addr_t _dma_phys_addr;
-	dma_addr_t _dma_phys_start_addr;
-
-	unsigned int _data_buf_size;
-	__le32 *_data_buf_virt_addr;
-	dma_addr_t _data_buf_phys_addr;
-	char *_filename;
-	char *_defaultname;
-
-	int _line_size_ch2;
-	int _prog_cnt_ch2;
-	int _pixel_format_ch2;
-	int _is_first_frame_ch2;
-	int _is_running_ch2;
-	int _file_status_ch2;
-	int _lines_count_ch2;
-	int _frame_count_ch2;
-	int _channel2_upstream_select;
-	unsigned int _risc_size_ch2;
-
-	__le32 *_dma_virt_start_addr_ch2;
-	__le32 *_dma_virt_addr_ch2;
-	dma_addr_t _dma_phys_addr_ch2;
-	dma_addr_t _dma_phys_start_addr_ch2;
-
-	unsigned int _data_buf_size_ch2;
-	__le32 *_data_buf_virt_addr_ch2;
-	dma_addr_t _data_buf_phys_addr_ch2;
-	char *_filename_ch2;
-	char *_defaultname_ch2;
-
-	/* MPEG Encoder ONLY settings */
-	u32 cx23417_mailbox;
-	struct cx2341x_mpeg_params mpeg_params;
-	struct video_device *v4l_device;
-	atomic_t v4l_reader_count;
-	struct cx25821_tvnorm encodernorm;
-
-	u32 upstream_riscbuf_size;
-	u32 upstream_databuf_size;
-	u32 upstream_riscbuf_size_ch2;
-	u32 upstream_databuf_size_ch2;
 	u32 audio_upstream_riscbuf_size;
 	u32 audio_upstream_databuf_size;
-	int _isNTSC;
-	int _frame_index;
 	int _audioframe_index;
-	struct workqueue_struct *_irq_queues;
-	struct work_struct _irq_work_entry;
-	struct workqueue_struct *_irq_queues_ch2;
-	struct work_struct _irq_work_entry_ch2;
 	struct workqueue_struct *_irq_audio_queues;
 	struct work_struct _audio_work_entry;
-	char *input_filename;
-	char *input_filename_ch2;
-	int _frame_index_ch2;
-	int _isNTSC_ch2;
-	char *vid_stdname_ch2;
-	int pixel_format_ch2;
-	int channel_select_ch2;
-	int command_ch2;
 	char *input_audiofilename;
-	char *vid_stdname;
-	int pixel_format;
-	int channel_select;
-	int command;
-	int channel_opened;
-};
 
-struct upstream_user_struct {
-	char *input_filename;
-	char *vid_stdname;
-	int pixel_format;
-	int channel_select;
-	int command;
-};
+	/* V4l */
+	spinlock_t slock;
 
-struct downstream_user_struct {
-	char *vid_stdname;
-	int pixel_format;
-	int cif_resolution_enable;
-	int cif_width;
-	int decoder_select;
-	int command;
-	int reg_address;
-	int reg_data;
+	/* Video Upstream */
+	struct cx25821_video_out_data vid_out_data[2];
 };
 
-extern struct upstream_user_struct *up_data;
-
 static inline struct cx25821_dev *get_cx25821(struct v4l2_device *v4l2_dev)
 {
 	return container_of(v4l2_dev, struct cx25821_dev, v4l2_dev);
 }
 
-#define cx25821_call_all(dev, o, f, args...) \
-	v4l2_device_call_all(&dev->v4l2_dev, 0, o, f, ##args)
-
-extern struct list_head cx25821_devlist;
-extern struct mutex cx25821_devlist_mutex;
-
 extern struct cx25821_board cx25821_boards[];
-extern struct cx25821_subid cx25821_subids[];
 
 #define SRAM_CH00  0		/* Video A */
 #define SRAM_CH01  1		/* Video B */
@@ -467,7 +323,6 @@ extern struct cx25821_subid cx25821_subids[];
 #define VID_UPSTREAM_SRAM_CHANNEL_I     SRAM_CH09
 #define VID_UPSTREAM_SRAM_CHANNEL_J     SRAM_CH10
 #define AUDIO_UPSTREAM_SRAM_CHANNEL_B   SRAM_CH11
-#define VIDEO_IOCTL_CH  11
 
 struct sram_channel {
 	char *name;
@@ -503,10 +358,8 @@ struct sram_channel {
 	u32 jumponly;
 	u32 irq_bit;
 };
-extern struct sram_channel cx25821_sram_channels[];
 
-#define STATUS_SUCCESS         0
-#define STATUS_UNSUCCESSFUL    -1
+extern const struct sram_channel cx25821_sram_channels[];
 
 #define cx_read(reg)             readl(dev->lmmio + ((reg)>>2))
 #define cx_write(reg, value)     writel((value), dev->lmmio + ((reg)>>2))
@@ -529,8 +382,6 @@ extern struct sram_channel cx25821_sram_channels[];
 	pr_info("(%d): " fmt, dev->board, ##args)
 
 extern int cx25821_i2c_register(struct cx25821_i2c *bus);
-extern void cx25821_card_setup(struct cx25821_dev *dev);
-extern int cx25821_ir_init(struct cx25821_dev *dev);
 extern int cx25821_i2c_read(struct cx25821_i2c *bus, u16 reg_addr, int *value);
 extern int cx25821_i2c_write(struct cx25821_i2c *bus, u16 reg_addr, int value);
 extern int cx25821_i2c_unregister(struct cx25821_i2c *bus);
@@ -551,7 +402,7 @@ extern int medusa_set_saturation(struct cx25821_dev *dev, int saturation,
 				 int decoder);
 
 extern int cx25821_sram_channel_setup(struct cx25821_dev *dev,
-				      struct sram_channel *ch, unsigned int bpl,
+				      const struct sram_channel *ch, unsigned int bpl,
 				      u32 risc);
 
 extern int cx25821_risc_buffer(struct pci_dev *pci, struct btcx_riscmem *risc,
@@ -570,46 +421,31 @@ extern void cx25821_free_buffer(struct videobuf_queue *q,
 extern int cx25821_risc_stopper(struct pci_dev *pci, struct btcx_riscmem *risc,
 				u32 reg, u32 mask, u32 value);
 extern void cx25821_sram_channel_dump(struct cx25821_dev *dev,
-				      struct sram_channel *ch);
+				      const struct sram_channel *ch);
 extern void cx25821_sram_channel_dump_audio(struct cx25821_dev *dev,
-					    struct sram_channel *ch);
+					    const struct sram_channel *ch);
 
 extern struct cx25821_dev *cx25821_dev_get(struct pci_dev *pci);
 extern void cx25821_print_irqbits(char *name, char *tag, char **strings,
 				  int len, u32 bits, u32 mask);
 extern void cx25821_dev_unregister(struct cx25821_dev *dev);
 extern int cx25821_sram_channel_setup_audio(struct cx25821_dev *dev,
-					    struct sram_channel *ch,
+					    const struct sram_channel *ch,
 					    unsigned int bpl, u32 risc);
 
-extern int cx25821_vidupstream_init_ch1(struct cx25821_dev *dev,
-					int channel_select, int pixel_format);
-extern int cx25821_vidupstream_init_ch2(struct cx25821_dev *dev,
-					int channel_select, int pixel_format);
+extern int cx25821_vidupstream_init(struct cx25821_channel *chan, int pixel_format);
 extern int cx25821_audio_upstream_init(struct cx25821_dev *dev,
 				       int channel_select);
-extern void cx25821_free_mem_upstream_ch1(struct cx25821_dev *dev);
-extern void cx25821_free_mem_upstream_ch2(struct cx25821_dev *dev);
+extern int cx25821_write_frame(struct cx25821_channel *chan,
+		const char __user *data, size_t count);
+extern void cx25821_free_mem_upstream(struct cx25821_channel *chan);
 extern void cx25821_free_mem_upstream_audio(struct cx25821_dev *dev);
-extern void cx25821_start_upstream_video_ch1(struct cx25821_dev *dev,
-					     struct upstream_user_struct
-					     *up_data);
-extern void cx25821_start_upstream_video_ch2(struct cx25821_dev *dev,
-					     struct upstream_user_struct
-					     *up_data);
-extern void cx25821_start_upstream_audio(struct cx25821_dev *dev,
-					 struct upstream_user_struct *up_data);
-extern void cx25821_stop_upstream_video_ch1(struct cx25821_dev *dev);
-extern void cx25821_stop_upstream_video_ch2(struct cx25821_dev *dev);
+extern void cx25821_stop_upstream_video(struct cx25821_channel *chan);
 extern void cx25821_stop_upstream_audio(struct cx25821_dev *dev);
 extern int cx25821_sram_channel_setup_upstream(struct cx25821_dev *dev,
-					       struct sram_channel *ch,
+					       const struct sram_channel *ch,
 					       unsigned int bpl, u32 risc);
 extern void cx25821_set_pixel_format(struct cx25821_dev *dev, int channel,
 				     u32 format);
-extern void cx25821_videoioctl_unregister(struct cx25821_dev *dev);
-extern struct video_device *cx25821_vdev_init(struct cx25821_dev *dev,
-					      struct pci_dev *pci,
-					      struct video_device *template,
-					      char *type);
+
 #endif
diff --git a/drivers/media/pci/cx88/cx88-blackbird.c b/drivers/media/pci/cx88/cx88-blackbird.c
index a6ff8a6f4fc..150bb76e783 100644
--- a/drivers/media/pci/cx88/cx88-blackbird.c
+++ b/drivers/media/pci/cx88/cx88-blackbird.c
@@ -815,7 +815,7 @@ static int vidioc_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
 }
 
 static int vidioc_s_frequency (struct file *file, void *priv,
-				struct v4l2_frequency *f)
+				const struct v4l2_frequency *f)
 {
 	struct cx8802_fh  *fh   = priv;
 	struct cx8802_dev *dev  = fh->dev;
@@ -918,7 +918,7 @@ static int vidioc_g_tuner (struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner (struct file *file, void *priv,
-				struct v4l2_tuner *t)
+				const struct v4l2_tuner *t)
 {
 	struct cx88_core  *core = ((struct cx8802_fh *)priv)->dev->core;
 
@@ -939,12 +939,12 @@ static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *tvnorm)
 	return 0;
 }
 
-static int vidioc_s_std (struct file *file, void *priv, v4l2_std_id *id)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id id)
 {
 	struct cx88_core  *core = ((struct cx8802_fh *)priv)->dev->core;
 
 	mutex_lock(&core->lock);
-	cx88_set_tvnorm(core,*id);
+	cx88_set_tvnorm(core, id);
 	mutex_unlock(&core->lock);
 	return 0;
 }
diff --git a/drivers/media/pci/cx88/cx88-cards.c b/drivers/media/pci/cx88/cx88-cards.c
index e2e0b8faf7a..a87a0e19593 100644
--- a/drivers/media/pci/cx88/cx88-cards.c
+++ b/drivers/media/pci/cx88/cx88-cards.c
@@ -59,6 +59,11 @@ MODULE_PARM_DESC(disable_ir, "Disable IR support");
 #define err_printk(core, fmt, arg...) \
 	printk(KERN_ERR "%s: " fmt, core->name , ## arg)
 
+#define dprintk(level,fmt, arg...)	do {				\
+	if (cx88_core_debug >= level)					\
+		printk(KERN_DEBUG "%s: " fmt, core->name , ## arg);	\
+	} while(0)
+
 
 /* ------------------------------------------------------------------ */
 /* board config info                                                  */
@@ -2855,6 +2860,7 @@ static void hauppauge_eeprom(struct cx88_core *core, u8 *eeprom_data)
 	core->board.tuner_type = tv.tuner_type;
 	core->tuner_formats = tv.tuner_formats;
 	core->board.radio.type = tv.has_radio ? CX88_RADIO : 0;
+	core->model = tv.model;
 
 	/* Make sure we support the board model */
 	switch (tv.model)
@@ -3133,7 +3139,7 @@ static int cx88_xc2028_tuner_callback(struct cx88_core *core,
 	case XC2028_TUNER_RESET:
 		switch (INPUT(core->input).type) {
 		case CX88_RADIO:
-			info_printk(core, "setting GPIO to radio!\n");
+			dprintk(1, "setting GPIO to radio!\n");
 			cx_write(MO_GP0_IO, 0x4ff);
 			mdelay(250);
 			cx_write(MO_GP2_IO, 0xff);
@@ -3141,7 +3147,7 @@ static int cx88_xc2028_tuner_callback(struct cx88_core *core,
 			break;
 		case CX88_VMUX_DVB:	/* Digital TV*/
 		default:		/* Analog TV */
-			info_printk(core, "setting GPIO to TV!\n");
+			dprintk(1, "setting GPIO to TV!\n");
 			break;
 		}
 		cx_write(MO_GP1_IO, 0x101010);
@@ -3199,8 +3205,7 @@ static int cx88_xc5000_tuner_callback(struct cx88_core *core,
 			   not having any tuning at all. */
 			return 0;
 		} else {
-			err_printk(core, "xc5000: unknown tuner "
-				   "callback command.\n");
+			dprintk(1, "xc5000: unknown tuner callback command.\n");
 			return -EINVAL;
 		}
 		break;
@@ -3211,8 +3216,7 @@ static int cx88_xc5000_tuner_callback(struct cx88_core *core,
 			cx_set(MO_GP0_IO, 0x00000010);
 			return 0;
 		} else {
-			printk(KERN_ERR
-				"xc5000: unknown tuner callback command.\n");
+			dprintk(1, "xc5000: unknown tuner callback command.\n");
 			return -EINVAL;
 		}
 		break;
@@ -3242,13 +3246,13 @@ int cx88_tuner_callback(void *priv, int component, int command, int arg)
 
 	switch (core->board.tuner_type) {
 		case TUNER_XC2028:
-			info_printk(core, "Calling XC2028/3028 callback\n");
+			dprintk(1, "Calling XC2028/3028 callback\n");
 			return cx88_xc2028_tuner_callback(core, command, arg);
 		case TUNER_XC4000:
-			info_printk(core, "Calling XC4000 callback\n");
+			dprintk(1, "Calling XC4000 callback\n");
 			return cx88_xc4000_tuner_callback(core, command, arg);
 		case TUNER_XC5000:
-			info_printk(core, "Calling XC5000 callback\n");
+			dprintk(1, "Calling XC5000 callback\n");
 			return cx88_xc5000_tuner_callback(core, command, arg);
 	}
 	err_printk(core, "Error: Calling callback for tuner %d\n",
@@ -3589,8 +3593,8 @@ static void cx88_card_setup(struct cx88_core *core)
 		memset(&xc2028_cfg, 0, sizeof(xc2028_cfg));
 		xc2028_cfg.tuner = TUNER_XC2028;
 		xc2028_cfg.priv  = &ctl;
-		info_printk(core, "Asking xc2028/3028 to load firmware %s\n",
-			    ctl.fname);
+		dprintk(1, "Asking xc2028/3028 to load firmware %s\n",
+			ctl.fname);
 		call_all(core, tuner, s_config, &xc2028_cfg);
 	}
 	call_all(core, core, s_power, 0);
@@ -3759,8 +3763,8 @@ struct cx88_core *cx88_core_create(struct pci_dev *pci, int nr)
 	if (radio[core->nr] != UNSET)
 		core->board.radio_type = radio[core->nr];
 
-	info_printk(core, "TV tuner type %d, Radio tuner type %d\n",
-		    core->board.tuner_type, core->board.radio_type);
+	dprintk(1, "TV tuner type %d, Radio tuner type %d\n",
+		core->board.tuner_type, core->board.radio_type);
 
 	/* init hardware */
 	cx88_reset(core);
diff --git a/drivers/media/pci/cx88/cx88-core.c b/drivers/media/pci/cx88/cx88-core.c
index 39f095c37ff..c8f3dcc579d 100644
--- a/drivers/media/pci/cx88/cx88-core.c
+++ b/drivers/media/pci/cx88/cx88-core.c
@@ -48,9 +48,9 @@ MODULE_LICENSE("GPL");
 
 /* ------------------------------------------------------------------ */
 
-static unsigned int core_debug;
-module_param(core_debug,int,0644);
-MODULE_PARM_DESC(core_debug,"enable debug messages [core]");
+unsigned int cx88_core_debug;
+module_param_named(core_debug, cx88_core_debug, int, 0644);
+MODULE_PARM_DESC(core_debug, "enable debug messages [core]");
 
 static unsigned int nicam;
 module_param(nicam,int,0644);
@@ -60,8 +60,10 @@ static unsigned int nocomb;
 module_param(nocomb,int,0644);
 MODULE_PARM_DESC(nocomb,"disable comb filter");
 
-#define dprintk(level,fmt, arg...)	if (core_debug >= level)	\
-	printk(KERN_DEBUG "%s: " fmt, core->name , ## arg)
+#define dprintk(level,fmt, arg...)	do {				\
+	if (cx88_core_debug >= level)					\
+		printk(KERN_DEBUG "%s: " fmt, core->name , ## arg);	\
+	} while(0)
 
 static unsigned int cx88_devcount;
 static LIST_HEAD(cx88_devlist);
diff --git a/drivers/media/pci/cx88/cx88-dvb.c b/drivers/media/pci/cx88/cx88-dvb.c
index 672b267a2d3..053ed1ba1d8 100644
--- a/drivers/media/pci/cx88/cx88-dvb.c
+++ b/drivers/media/pci/cx88/cx88-dvb.c
@@ -1042,7 +1042,7 @@ static int dvb_register(struct cx8802_dev *dev)
 			if (!dvb_attach(isl6421_attach,
 					fe0->dvb.frontend,
 					&dev->core->i2c_adap,
-					0x08, ISL6421_DCL, 0x00))
+					0x08, ISL6421_DCL, 0x00, false))
 				goto frontend_detach;
 		}
 		/* MFE frontend 2 */
@@ -1279,8 +1279,16 @@ static int dvb_register(struct cx8802_dev *dev)
 					       &hauppauge_novas_config,
 					       &core->i2c_adap);
 		if (fe0->dvb.frontend) {
+			bool override_tone;
+
+			if (core->model == 92001)
+				override_tone = true;
+			else
+				override_tone = false;
+
 			if (!dvb_attach(isl6421_attach, fe0->dvb.frontend,
-					&core->i2c_adap, 0x08, ISL6421_DCL, 0x00))
+					&core->i2c_adap, 0x08, ISL6421_DCL, 0x00,
+					override_tone))
 				goto frontend_detach;
 		}
 		break;
@@ -1403,7 +1411,7 @@ static int dvb_register(struct cx8802_dev *dev)
 			if (!dvb_attach(isl6421_attach,
 					fe0->dvb.frontend,
 					&dev->core->i2c_adap,
-					0x08, ISL6421_DCL, 0x00))
+					0x08, ISL6421_DCL, 0x00, false))
 				goto frontend_detach;
 		}
 		/* MFE frontend 2 */
@@ -1431,7 +1439,7 @@ static int dvb_register(struct cx8802_dev *dev)
 			if (!dvb_attach(isl6421_attach,
 					fe0->dvb.frontend,
 					&dev->core->i2c_adap,
-					0x08, ISL6421_DCL, 0x00))
+					0x08, ISL6421_DCL, 0x00, false))
 				goto frontend_detach;
 		}
 		break;
diff --git a/drivers/media/pci/cx88/cx88-mpeg.c b/drivers/media/pci/cx88/cx88-mpeg.c
index c9d3182f79d..2d3507eb489 100644
--- a/drivers/media/pci/cx88/cx88-mpeg.c
+++ b/drivers/media/pci/cx88/cx88-mpeg.c
@@ -532,16 +532,17 @@ static int cx8802_suspend_common(struct pci_dev *pci_dev, pm_message_t state)
 {
 	struct cx8802_dev *dev = pci_get_drvdata(pci_dev);
 	struct cx88_core *core = dev->core;
+	unsigned long flags;
 
 	/* stop mpeg dma */
-	spin_lock(&dev->slock);
+	spin_lock_irqsave(&dev->slock, flags);
 	if (!list_empty(&dev->mpegq.active)) {
 		dprintk( 2, "suspend\n" );
 		printk("%s: suspend mpeg\n", core->name);
 		cx8802_stop_dma(dev);
 		del_timer(&dev->mpegq.timeout);
 	}
-	spin_unlock(&dev->slock);
+	spin_unlock_irqrestore(&dev->slock, flags);
 
 	/* FIXME -- shutdown device */
 	cx88_shutdown(dev->core);
@@ -558,6 +559,7 @@ static int cx8802_resume_common(struct pci_dev *pci_dev)
 {
 	struct cx8802_dev *dev = pci_get_drvdata(pci_dev);
 	struct cx88_core *core = dev->core;
+	unsigned long flags;
 	int err;
 
 	if (dev->state.disabled) {
@@ -584,12 +586,12 @@ static int cx8802_resume_common(struct pci_dev *pci_dev)
 	cx88_reset(dev->core);
 
 	/* restart video+vbi capture */
-	spin_lock(&dev->slock);
+	spin_lock_irqsave(&dev->slock, flags);
 	if (!list_empty(&dev->mpegq.active)) {
 		printk("%s: resume mpeg\n", core->name);
 		cx8802_restart_queue(dev,&dev->mpegq);
 	}
-	spin_unlock(&dev->slock);
+	spin_unlock_irqrestore(&dev->slock, flags);
 
 	return 0;
 }
diff --git a/drivers/media/pci/cx88/cx88-video.c b/drivers/media/pci/cx88/cx88-video.c
index bc78354262a..1b00615fd39 100644
--- a/drivers/media/pci/cx88/cx88-video.c
+++ b/drivers/media/pci/cx88/cx88-video.c
@@ -1193,12 +1193,12 @@ static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *tvnorm)
 	return 0;
 }
 
-static int vidioc_s_std (struct file *file, void *priv, v4l2_std_id *tvnorms)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id tvnorms)
 {
 	struct cx88_core  *core = ((struct cx8800_fh *)priv)->dev->core;
 
 	mutex_lock(&core->lock);
-	cx88_set_tvnorm(core,*tvnorms);
+	cx88_set_tvnorm(core, tvnorms);
 	mutex_unlock(&core->lock);
 
 	return 0;
@@ -1289,7 +1289,7 @@ static int vidioc_g_tuner (struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner (struct file *file, void *priv,
-				struct v4l2_tuner *t)
+				const struct v4l2_tuner *t)
 {
 	struct cx88_core  *core = ((struct cx8800_fh *)priv)->dev->core;
 
@@ -1321,8 +1321,10 @@ static int vidioc_g_frequency (struct file *file, void *priv,
 }
 
 int cx88_set_freq (struct cx88_core  *core,
-				struct v4l2_frequency *f)
+				const struct v4l2_frequency *f)
 {
+	struct v4l2_frequency new_freq = *f;
+
 	if (unlikely(UNSET == core->board.tuner_type))
 		return -EINVAL;
 	if (unlikely(f->tuner != 0))
@@ -1331,8 +1333,8 @@ int cx88_set_freq (struct cx88_core  *core,
 	mutex_lock(&core->lock);
 	cx88_newstation(core);
 	call_all(core, tuner, s_frequency, f);
-	call_all(core, tuner, g_frequency, f);
-	core->freq = f->frequency;
+	call_all(core, tuner, g_frequency, &new_freq);
+	core->freq = new_freq.frequency;
 
 	/* When changing channels it is required to reset TVAUDIO */
 	msleep (10);
@@ -1345,7 +1347,7 @@ int cx88_set_freq (struct cx88_core  *core,
 EXPORT_SYMBOL(cx88_set_freq);
 
 static int vidioc_s_frequency (struct file *file, void *priv,
-				struct v4l2_frequency *f)
+				const struct v4l2_frequency *f)
 {
 	struct cx8800_fh  *fh   = priv;
 	struct cx88_core  *core = fh->dev->core;
@@ -1378,7 +1380,7 @@ static int vidioc_g_register (struct file *file, void *fh,
 }
 
 static int vidioc_s_register (struct file *file, void *fh,
-				struct v4l2_dbg_register *reg)
+				const struct v4l2_dbg_register *reg)
 {
 	struct cx88_core *core = ((struct cx8800_fh*)fh)->dev->core;
 
@@ -1407,20 +1409,15 @@ static int radio_g_tuner (struct file *file, void *priv,
 	return 0;
 }
 
-/* FIXME: Should add a standard for radio */
-
 static int radio_s_tuner (struct file *file, void *priv,
-				struct v4l2_tuner *t)
+				const struct v4l2_tuner *t)
 {
 	struct cx88_core  *core = ((struct cx8800_fh *)priv)->dev->core;
 
 	if (0 != t->index)
 		return -EINVAL;
-	if (t->audmode > V4L2_TUNER_MODE_STEREO)
-		t->audmode = V4L2_TUNER_MODE_STEREO;
 
 	call_all(core, tuner, s_tuner, t);
-
 	return 0;
 }
 
@@ -1957,9 +1954,10 @@ static int cx8800_suspend(struct pci_dev *pci_dev, pm_message_t state)
 {
 	struct cx8800_dev *dev = pci_get_drvdata(pci_dev);
 	struct cx88_core *core = dev->core;
+	unsigned long flags;
 
 	/* stop video+vbi capture */
-	spin_lock(&dev->slock);
+	spin_lock_irqsave(&dev->slock, flags);
 	if (!list_empty(&dev->vidq.active)) {
 		printk("%s/0: suspend video\n", core->name);
 		stop_video_dma(dev);
@@ -1970,7 +1968,7 @@ static int cx8800_suspend(struct pci_dev *pci_dev, pm_message_t state)
 		cx8800_stop_vbi_dma(dev);
 		del_timer(&dev->vbiq.timeout);
 	}
-	spin_unlock(&dev->slock);
+	spin_unlock_irqrestore(&dev->slock, flags);
 
 	if (core->ir)
 		cx88_ir_stop(core);
@@ -1989,6 +1987,7 @@ static int cx8800_resume(struct pci_dev *pci_dev)
 {
 	struct cx8800_dev *dev = pci_get_drvdata(pci_dev);
 	struct cx88_core *core = dev->core;
+	unsigned long flags;
 	int err;
 
 	if (dev->state.disabled) {
@@ -2019,7 +2018,7 @@ static int cx8800_resume(struct pci_dev *pci_dev)
 	cx_set(MO_PCI_INTMSK, core->pci_irqmask);
 
 	/* restart video+vbi capture */
-	spin_lock(&dev->slock);
+	spin_lock_irqsave(&dev->slock, flags);
 	if (!list_empty(&dev->vidq.active)) {
 		printk("%s/0: resume video\n", core->name);
 		restart_video_queue(dev,&dev->vidq);
@@ -2028,7 +2027,7 @@ static int cx8800_resume(struct pci_dev *pci_dev)
 		printk("%s/0: resume vbi\n", core->name);
 		cx8800_restart_vbi_queue(dev,&dev->vbiq);
 	}
-	spin_unlock(&dev->slock);
+	spin_unlock_irqrestore(&dev->slock, flags);
 
 	return 0;
 }
diff --git a/drivers/media/pci/cx88/cx88.h b/drivers/media/pci/cx88/cx88.h
index feff53c0a25..51ce2c0e8bc 100644
--- a/drivers/media/pci/cx88/cx88.h
+++ b/drivers/media/pci/cx88/cx88.h
@@ -334,6 +334,7 @@ struct cx88_core {
 	/* board name */
 	int                        nr;
 	char                       name[32];
+	u32			   model;
 
 	/* pci stuff */
 	int                        pci_bus;
@@ -617,6 +618,8 @@ struct cx8802_dev {
 /* ----------------------------------------------------------- */
 /* cx88-core.c                                                 */
 
+extern unsigned int cx88_core_debug;
+
 extern void cx88_print_irqbits(const char *name, const char *tag, const char *strings[],
 			       int len, u32 bits, u32 mask);
 
@@ -740,7 +743,7 @@ void cx8802_cancel_buffers(struct cx8802_dev *dev);
 /* ----------------------------------------------------------- */
 /* cx88-video.c*/
 int cx88_enum_input (struct cx88_core  *core,struct v4l2_input *i);
-int cx88_set_freq (struct cx88_core  *core,struct v4l2_frequency *f);
+int cx88_set_freq(struct cx88_core  *core, const struct v4l2_frequency *f);
 int cx88_video_mux(struct cx88_core *core, unsigned int input);
 void cx88_querycap(struct file *file, struct cx88_core *core,
 		struct v4l2_capability *cap);
diff --git a/drivers/media/pci/ivtv/ivtv-driver.c b/drivers/media/pci/ivtv/ivtv-driver.c
index 2928e7287da..07b8460953b 100644
--- a/drivers/media/pci/ivtv/ivtv-driver.c
+++ b/drivers/media/pci/ivtv/ivtv-driver.c
@@ -1387,7 +1387,7 @@ int ivtv_init_on_first_open(struct ivtv *itv)
 	if (!itv->has_cx23415)
 		write_reg_sync(0x03, IVTV_REG_DMACONTROL);
 
-	ivtv_s_std_enc(itv, &itv->tuner_std);
+	ivtv_s_std_enc(itv, itv->tuner_std);
 
 	/* Default interrupts enabled. For the PVR350 this includes the
 	   decoder VSYNC interrupt, which is always on. It is not only used
@@ -1397,7 +1397,7 @@ int ivtv_init_on_first_open(struct ivtv *itv)
 	if (itv->v4l2_cap & V4L2_CAP_VIDEO_OUTPUT) {
 		ivtv_clear_irq_mask(itv, IVTV_IRQ_MASK_INIT | IVTV_IRQ_DEC_VSYNC);
 		ivtv_set_osd_alpha(itv);
-		ivtv_s_std_dec(itv, &itv->tuner_std);
+		ivtv_s_std_dec(itv, itv->tuner_std);
 	} else {
 		ivtv_clear_irq_mask(itv, IVTV_IRQ_MASK_INIT);
 	}
diff --git a/drivers/media/pci/ivtv/ivtv-firmware.c b/drivers/media/pci/ivtv/ivtv-firmware.c
index 68387d4369d..ed73edd2bcd 100644
--- a/drivers/media/pci/ivtv/ivtv-firmware.c
+++ b/drivers/media/pci/ivtv/ivtv-firmware.c
@@ -302,7 +302,7 @@ static int ivtv_firmware_restart(struct ivtv *itv)
 	/* Restore encoder video standard */
 	std = itv->std;
 	itv->std = 0;
-	ivtv_s_std_enc(itv, &std);
+	ivtv_s_std_enc(itv, std);
 
 	if (itv->v4l2_cap & V4L2_CAP_VIDEO_OUTPUT) {
 		ivtv_init_mpeg_decoder(itv);
@@ -310,7 +310,7 @@ static int ivtv_firmware_restart(struct ivtv *itv)
 		/* Restore decoder video standard */
 		std = itv->std_out;
 		itv->std_out = 0;
-		ivtv_s_std_dec(itv, &std);
+		ivtv_s_std_dec(itv, std);
 
 		/* Restore framebuffer if active */
 		if (itv->ivtvfb_restore)
diff --git a/drivers/media/pci/ivtv/ivtv-gpio.c b/drivers/media/pci/ivtv/ivtv-gpio.c
index 8f0d0778905..af52def700c 100644
--- a/drivers/media/pci/ivtv/ivtv-gpio.c
+++ b/drivers/media/pci/ivtv/ivtv-gpio.c
@@ -192,7 +192,7 @@ static int subdev_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
 	return 0;
 }
 
-static int subdev_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+static int subdev_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
 {
 	struct ivtv *itv = sd_to_ivtv(sd);
 	u16 mask, data;
diff --git a/drivers/media/pci/ivtv/ivtv-ioctl.c b/drivers/media/pci/ivtv/ivtv-ioctl.c
index 7a8b0d0b612..9cbbce0eaed 100644
--- a/drivers/media/pci/ivtv/ivtv-ioctl.c
+++ b/drivers/media/pci/ivtv/ivtv-ioctl.c
@@ -711,28 +711,26 @@ static int ivtv_g_chip_ident(struct file *file, void *fh, struct v4l2_dbg_chip_i
 }
 
 #ifdef CONFIG_VIDEO_ADV_DEBUG
-static int ivtv_itvc(struct ivtv *itv, unsigned int cmd, void *arg)
+static int ivtv_itvc(struct ivtv *itv, bool get, u64 reg, u64 *val)
 {
-	struct v4l2_dbg_register *regs = arg;
 	volatile u8 __iomem *reg_start;
 
 	if (!capable(CAP_SYS_ADMIN))
 		return -EPERM;
-	if (regs->reg >= IVTV_REG_OFFSET && regs->reg < IVTV_REG_OFFSET + IVTV_REG_SIZE)
+	if (reg >= IVTV_REG_OFFSET && reg < IVTV_REG_OFFSET + IVTV_REG_SIZE)
 		reg_start = itv->reg_mem - IVTV_REG_OFFSET;
-	else if (itv->has_cx23415 && regs->reg >= IVTV_DECODER_OFFSET &&
-			regs->reg < IVTV_DECODER_OFFSET + IVTV_DECODER_SIZE)
+	else if (itv->has_cx23415 && reg >= IVTV_DECODER_OFFSET &&
+			reg < IVTV_DECODER_OFFSET + IVTV_DECODER_SIZE)
 		reg_start = itv->dec_mem - IVTV_DECODER_OFFSET;
-	else if (regs->reg < IVTV_ENCODER_SIZE)
+	else if (reg < IVTV_ENCODER_SIZE)
 		reg_start = itv->enc_mem;
 	else
 		return -EINVAL;
 
-	regs->size = 4;
-	if (cmd == VIDIOC_DBG_G_REGISTER)
-		regs->val = readl(regs->reg + reg_start);
+	if (get)
+		*val = readl(reg + reg_start);
 	else
-		writel(regs->val, regs->reg + reg_start);
+		writel(*val, reg + reg_start);
 	return 0;
 }
 
@@ -740,20 +738,25 @@ static int ivtv_g_register(struct file *file, void *fh, struct v4l2_dbg_register
 {
 	struct ivtv *itv = fh2id(fh)->itv;
 
-	if (v4l2_chip_match_host(&reg->match))
-		return ivtv_itvc(itv, VIDIOC_DBG_G_REGISTER, reg);
+	if (v4l2_chip_match_host(&reg->match)) {
+		reg->size = 4;
+		return ivtv_itvc(itv, true, reg->reg, &reg->val);
+	}
 	/* TODO: subdev errors should not be ignored, this should become a
 	   subdev helper function. */
 	ivtv_call_all(itv, core, g_register, reg);
 	return 0;
 }
 
-static int ivtv_s_register(struct file *file, void *fh, struct v4l2_dbg_register *reg)
+static int ivtv_s_register(struct file *file, void *fh, const struct v4l2_dbg_register *reg)
 {
 	struct ivtv *itv = fh2id(fh)->itv;
 
-	if (v4l2_chip_match_host(&reg->match))
-		return ivtv_itvc(itv, VIDIOC_DBG_S_REGISTER, reg);
+	if (v4l2_chip_match_host(&reg->match)) {
+		u64 val = reg->val;
+
+		return ivtv_itvc(itv, false, reg->reg, &val);
+	}
 	/* TODO: subdev errors should not be ignored, this should become a
 	   subdev helper function. */
 	ivtv_call_all(itv, core, s_register, reg);
@@ -1078,7 +1081,7 @@ static int ivtv_g_frequency(struct file *file, void *fh, struct v4l2_frequency *
 	return 0;
 }
 
-int ivtv_s_frequency(struct file *file, void *fh, struct v4l2_frequency *vf)
+int ivtv_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *vf)
 {
 	struct ivtv *itv = fh2id(fh)->itv;
 	struct ivtv_stream *s = &itv->streams[fh2id(fh)->type];
@@ -1103,10 +1106,10 @@ static int ivtv_g_std(struct file *file, void *fh, v4l2_std_id *std)
 	return 0;
 }
 
-void ivtv_s_std_enc(struct ivtv *itv, v4l2_std_id *std)
+void ivtv_s_std_enc(struct ivtv *itv, v4l2_std_id std)
 {
-	itv->std = *std;
-	itv->is_60hz = (*std & V4L2_STD_525_60) ? 1 : 0;
+	itv->std = std;
+	itv->is_60hz = (std & V4L2_STD_525_60) ? 1 : 0;
 	itv->is_50hz = !itv->is_60hz;
 	cx2341x_handler_set_50hz(&itv->cxhdl, itv->is_50hz);
 	itv->cxhdl.width = 720;
@@ -1122,15 +1125,15 @@ void ivtv_s_std_enc(struct ivtv *itv, v4l2_std_id *std)
 	ivtv_call_all(itv, core, s_std, itv->std);
 }
 
-void ivtv_s_std_dec(struct ivtv *itv, v4l2_std_id *std)
+void ivtv_s_std_dec(struct ivtv *itv, v4l2_std_id std)
 {
 	struct yuv_playback_info *yi = &itv->yuv_info;
 	DEFINE_WAIT(wait);
 	int f;
 
 	/* set display standard */
-	itv->std_out = *std;
-	itv->is_out_60hz = (*std & V4L2_STD_525_60) ? 1 : 0;
+	itv->std_out = std;
+	itv->is_out_60hz = (std & V4L2_STD_525_60) ? 1 : 0;
 	itv->is_out_50hz = !itv->is_out_60hz;
 	ivtv_call_all(itv, video, s_std_output, itv->std_out);
 
@@ -1168,14 +1171,14 @@ void ivtv_s_std_dec(struct ivtv *itv, v4l2_std_id *std)
 	}
 }
 
-static int ivtv_s_std(struct file *file, void *fh, v4l2_std_id *std)
+static int ivtv_s_std(struct file *file, void *fh, v4l2_std_id std)
 {
 	struct ivtv *itv = fh2id(fh)->itv;
 
-	if ((*std & V4L2_STD_ALL) == 0)
+	if ((std & V4L2_STD_ALL) == 0)
 		return -EINVAL;
 
-	if (*std == itv->std)
+	if (std == itv->std)
 		return 0;
 
 	if (test_bit(IVTV_F_I_RADIO_USER, &itv->i_flags) ||
@@ -1196,7 +1199,7 @@ static int ivtv_s_std(struct file *file, void *fh, v4l2_std_id *std)
 	return 0;
 }
 
-static int ivtv_s_tuner(struct file *file, void *fh, struct v4l2_tuner *vt)
+static int ivtv_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt)
 {
 	struct ivtv_open_id *id = fh2id(fh);
 	struct ivtv *itv = id->itv;
@@ -1804,7 +1807,7 @@ static int ivtv_decoder_ioctls(struct file *filp, unsigned int cmd, void *arg)
 }
 
 static long ivtv_default(struct file *file, void *fh, bool valid_prio,
-			 int cmd, void *arg)
+			 unsigned int cmd, void *arg)
 {
 	struct ivtv *itv = fh2id(fh)->itv;
 
diff --git a/drivers/media/pci/ivtv/ivtv-ioctl.h b/drivers/media/pci/ivtv/ivtv-ioctl.h
index 7c553d16579..75c39775611 100644
--- a/drivers/media/pci/ivtv/ivtv-ioctl.h
+++ b/drivers/media/pci/ivtv/ivtv-ioctl.h
@@ -27,9 +27,9 @@ u16 ivtv_get_service_set(struct v4l2_sliced_vbi_format *fmt);
 void ivtv_set_osd_alpha(struct ivtv *itv);
 int ivtv_set_speed(struct ivtv *itv, int speed);
 void ivtv_set_funcs(struct video_device *vdev);
-void ivtv_s_std_enc(struct ivtv *itv, v4l2_std_id *std);
-void ivtv_s_std_dec(struct ivtv *itv, v4l2_std_id *std);
-int ivtv_s_frequency(struct file *file, void *fh, struct v4l2_frequency *vf);
+void ivtv_s_std_enc(struct ivtv *itv, v4l2_std_id std);
+void ivtv_s_std_dec(struct ivtv *itv, v4l2_std_id std);
+int ivtv_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *vf);
 int ivtv_s_input(struct file *file, void *fh, unsigned int inp);
 
 #endif
diff --git a/drivers/media/pci/ivtv/ivtvfb.c b/drivers/media/pci/ivtv/ivtvfb.c
index 05b94aa8ba3..9ff1230192e 100644
--- a/drivers/media/pci/ivtv/ivtvfb.c
+++ b/drivers/media/pci/ivtv/ivtvfb.c
@@ -1171,8 +1171,7 @@ static void ivtvfb_release_buffers (struct ivtv *itv)
 		fb_dealloc_cmap(&oi->ivtvfb_info.cmap);
 
 	/* Release pseudo palette */
-	if (oi->ivtvfb_info.pseudo_palette)
-		kfree(oi->ivtvfb_info.pseudo_palette);
+	kfree(oi->ivtvfb_info.pseudo_palette);
 
 #ifdef CONFIG_MTRR
 	if (oi->fb_end_aligned_physaddr) {
diff --git a/drivers/media/pci/meye/meye.c b/drivers/media/pci/meye/meye.c
index 7859c43479d..2381b05432e 100644
--- a/drivers/media/pci/meye/meye.c
+++ b/drivers/media/pci/meye/meye.c
@@ -1410,7 +1410,7 @@ static int vidioc_streamoff(struct file *file, void *fh, enum v4l2_buf_type i)
 }
 
 static long vidioc_default(struct file *file, void *fh, bool valid_prio,
-						int cmd, void *arg)
+			   unsigned int cmd, void *arg)
 {
 	switch (cmd) {
 	case MEYEIOC_G_PARAMS:
diff --git a/drivers/media/pci/saa7134/saa7134-cards.c b/drivers/media/pci/saa7134/saa7134-cards.c
index dc68cf1070f..d45e7f6ff33 100644
--- a/drivers/media/pci/saa7134/saa7134-cards.c
+++ b/drivers/media/pci/saa7134/saa7134-cards.c
@@ -50,6 +50,11 @@ static char name_svideo[]  = "S-Video";
 /* ------------------------------------------------------------------ */
 /* board config info                                                  */
 
+static struct tda18271_std_map aver_a706_std_map = {
+	.fm_radio = { .if_freq = 5500, .fm_rfn = 0, .agc_mode = 3, .std = 0,
+		      .if_lvl = 0, .rfagc_top = 0x2c, },
+};
+
 /* If radio_type !=UNSET, radio_addr should be specified
  */
 
@@ -2760,7 +2765,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.tuner_config   = 0,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_OFF },
 		.mpeg           = SAA7134_MPEG_DVB,
 		.gpiomask       = 0x0200000,
 		.inputs = {{
@@ -3291,7 +3296,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.tuner_config   = 1,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_GP0_HIGH_ON },
 		.mpeg           = SAA7134_MPEG_DVB,
 		.gpiomask       = 0x000200000,
 		.inputs         = {{
@@ -3395,7 +3400,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.tuner_config   = 1,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_GP0_HIGH_ON },
 		.mpeg           = SAA7134_MPEG_DVB,
 		.gpiomask       = 0x0200100,
 		.inputs         = {{
@@ -3426,7 +3431,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.tuner_config   = 3,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_ON_BRIDGE },
 		.mpeg           = SAA7134_MPEG_DVB,
 		.ts_type	= SAA7134_MPEG_TS_SERIAL,
 		.ts_force_val   = 1,
@@ -3459,7 +3464,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.tuner_config   = 3,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_ON_BRIDGE },
 		.mpeg           = SAA7134_MPEG_DVB,
 		.ts_type	= SAA7134_MPEG_TS_SERIAL,
 		.gpiomask       = 0x0800100, /* GPIO 21 is an INPUT */
@@ -3683,7 +3688,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.tuner_config   = 2,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_GP0_HIGH_OFF },
 		.mpeg           = SAA7134_MPEG_DVB,
 		.gpiomask       = 0x0200000,
 		.inputs = {{
@@ -3736,7 +3741,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.tuner_config   = 2,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_GP0_HIGH_OFF },
 		.mpeg           = SAA7134_MPEG_DVB,
 		.gpiomask       = 0x0200000,
 		.inputs = {{
@@ -3754,7 +3759,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.tuner_config   = 2,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_GP0_HIGH_OFF },
 		.gpiomask	= 1 << 21,
 		.mpeg           = SAA7134_MPEG_DVB,
 		.inputs         = {{
@@ -3887,7 +3892,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.tuner_config   = 0,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_OFF },
 		.mpeg           = SAA7134_MPEG_DVB,
 		.inputs = {{
 			.name   = name_tv, /* FIXME: analog tv untested */
@@ -3903,7 +3908,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.tuner_config   = 2,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_GP0_HIGH_OFF },
 		.gpiomask       = 0x020200000,
 		.inputs         = {{
 			.name = name_tv,
@@ -3937,7 +3942,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type	= UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.tuner_config	= 0,
+		.tda829x_conf	= { .lna_cfg = TDA8290_LNA_OFF },
 		.gpiomask	= 0x020200000,
 		.inputs		= {{
 			.name = name_tv,
@@ -4737,7 +4742,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.tuner_config   = 2,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_GP0_HIGH_OFF },
 		.mpeg           = SAA7134_MPEG_DVB,
 		.gpiomask       = 0x0200000,
 		.inputs = {{
@@ -4823,7 +4828,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type   = UNSET,
 		.tuner_addr   = ADDR_UNSET,
 		.radio_addr   = ADDR_UNSET,
-		.tuner_config = 0,
+		.tda829x_conf = { .lna_cfg = TDA8290_LNA_OFF },
 		.mpeg         = SAA7134_MPEG_DVB,
 		.inputs       = {{
 			.name = name_tv,
@@ -4847,7 +4852,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.tuner_config   = 2,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_GP0_HIGH_OFF },
 		.mpeg           = SAA7134_MPEG_DVB,
 		.gpiomask       = 0x0200000,
 		.inputs = { {
@@ -5057,7 +5062,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.tuner_config   = 2,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_GP0_HIGH_OFF },
 		.gpiomask       = 1 << 21,
 		.mpeg           = SAA7134_MPEG_DVB,
 		.inputs         = {{
@@ -5087,7 +5092,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.tuner_config   = 2,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_GP0_HIGH_OFF },
 		.gpiomask       = 1 << 21,
 		.mpeg           = SAA7134_MPEG_DVB,
 		.inputs         = {{
@@ -5176,7 +5181,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.tuner_config   = 0,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_OFF },
 		.mpeg           = SAA7134_MPEG_DVB,
 		.gpiomask       = 0x0200000,
 		.inputs = { {
@@ -5406,7 +5411,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.tuner_config   = 0,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_OFF },
 		.mpeg           = SAA7134_MPEG_DVB,
 		.ts_type	= SAA7134_MPEG_TS_PARALLEL,
 		.inputs         = {{
@@ -5629,7 +5634,7 @@ struct saa7134_board saa7134_boards[] = {
 		.audio_clock	= 0x00187de7,
 		.tuner_type	= TUNER_PHILIPS_TDA8290,
 		.radio_type	= UNSET,
-		.tuner_config	= 3,
+		.tda829x_conf	= { .lna_cfg = TDA8290_LNA_ON_BRIDGE },
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
 		.gpiomask	= 0x02050000,
@@ -5790,6 +5795,38 @@ struct saa7134_board saa7134_boards[] = {
 			.gpio = 0x6010000,
 		} },
 	},
+	[SAA7134_BOARD_AVERMEDIA_A706] = {
+		.name           = "AverMedia AverTV Satellite Hybrid+FM A706",
+		.audio_clock    = 0x00187de7,
+		.tuner_type     = TUNER_PHILIPS_TDA8290,
+		.radio_type     = UNSET,
+		.tuner_addr     = ADDR_UNSET,
+		.radio_addr     = ADDR_UNSET,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_OFF,
+				    .no_i2c_gate = 1,
+				    .tda18271_std_map = &aver_a706_std_map },
+		.gpiomask       = 1 << 11,
+		.mpeg           = SAA7134_MPEG_DVB,
+		.inputs         = {{
+			.name = name_tv,
+			.vmux = 1,
+			.amux = TV,
+			.tv   = 1,
+		}, {
+			.name = name_comp,
+			.vmux = 4,
+			.amux = LINE1,
+		}, {
+			.name = name_svideo,
+			.vmux = 8,
+			.amux = LINE1,
+		} },
+		.radio = {
+			.name = name_radio,
+			.amux = TV,
+			.gpio = 0x0000800,
+		},
+	},
 
 };
 
@@ -7037,6 +7074,12 @@ struct pci_device_id saa7134_pci_tbl[] = {
 		.subdevice    = 0x0911,
 		.driver_data  = SAA7134_BOARD_SENSORAY811_911,
 	}, {
+		.vendor       = PCI_VENDOR_ID_PHILIPS,
+		.device       = PCI_DEVICE_ID_PHILIPS_SAA7133,
+		.subvendor    = 0x1461, /* Avermedia Technologies Inc */
+		.subdevice    = 0x2055, /* AverTV Satellite Hybrid+FM A706 */
+		.driver_data  = SAA7134_BOARD_AVERMEDIA_A706,
+	}, {
 		/* --- boards without eeprom + subsystem ID --- */
 		.vendor       = PCI_VENDOR_ID_PHILIPS,
 		.device       = PCI_DEVICE_ID_PHILIPS_SAA7134,
@@ -7585,6 +7628,17 @@ int saa7134_board_init1(struct saa7134_dev *dev)
 		saa_andorl(SAA7134_GPIO_GPMODE0 >> 2,   0x80040100, 0x80040100);
 		saa_andorl(SAA7134_GPIO_GPSTATUS0 >> 2, 0x80040100, 0x00040100);
 		break;
+	case SAA7134_BOARD_AVERMEDIA_A706:
+		/* radio antenna select: tristate both as in Windows driver */
+		saa7134_set_gpio(dev, 12, 3);	/* TV antenna */
+		saa7134_set_gpio(dev, 13, 3);	/* FM antenna */
+		dev->has_remote = SAA7134_REMOTE_I2C;
+		/*
+		 * Disable CE5039 DVB-S tuner now (SLEEP pin high) to prevent
+		 * it from interfering with analog tuner detection
+		 */
+		saa7134_set_gpio(dev, 23, 1);
+		break;
 	case SAA7134_BOARD_VIDEOMATE_S350:
 		dev->has_remote = SAA7134_REMOTE_GPIO;
 		saa_andorl(SAA7134_GPIO_GPMODE0 >> 2,   0x0000C000, 0x0000C000);
@@ -7633,7 +7687,7 @@ static void saa7134_tuner_setup(struct saa7134_dev *dev)
 	if ((dev->tuner_type != TUNER_ABSENT) && (dev->tuner_type != UNSET)) {
 		tun_setup.type = dev->tuner_type;
 		tun_setup.addr = dev->tuner_addr;
-		tun_setup.config = saa7134_boards[dev->board].tuner_config;
+		tun_setup.config = &saa7134_boards[dev->board].tda829x_conf;
 		tun_setup.tuner_callback = saa7134_tuner_callback;
 
 		tun_setup.mode_mask = mode_mask;
diff --git a/drivers/media/pci/saa7134/saa7134-core.c b/drivers/media/pci/saa7134/saa7134-core.c
index 8fd24e7c940..45f0aca597a 100644
--- a/drivers/media/pci/saa7134/saa7134-core.c
+++ b/drivers/media/pci/saa7134/saa7134-core.c
@@ -805,6 +805,7 @@ static struct video_device *vdev_init(struct saa7134_dev *dev,
 	vfd->debug   = video_debug;
 	snprintf(vfd->name, sizeof(vfd->name), "%s %s (%s)",
 		 dev->name, type, saa7134_boards[dev->board].name);
+	set_bit(V4L2_FL_USE_FH_PRIO, &vfd->flags);
 	video_set_drvdata(vfd, dev);
 	return vfd;
 }
@@ -1028,8 +1029,6 @@ static int saa7134_initdev(struct pci_dev *pci_dev,
 		}
 	}
 
-	v4l2_prio_init(&dev->prio);
-
 	mutex_lock(&saa7134_devlist_lock);
 	list_for_each_entry(mops, &mops_list, next)
 		mpeg_ops_attach(mops, dev);
diff --git a/drivers/media/pci/saa7134/saa7134-dvb.c b/drivers/media/pci/saa7134/saa7134-dvb.c
index 27915e501db..4a08ae31df2 100644
--- a/drivers/media/pci/saa7134/saa7134-dvb.c
+++ b/drivers/media/pci/saa7134/saa7134-dvb.c
@@ -1073,6 +1073,10 @@ static struct mt312_config zl10313_compro_s350_config = {
 	.demod_address = 0x0e,
 };
 
+static struct mt312_config zl10313_avermedia_a706_config = {
+	.demod_address = 0x0e,
+};
+
 static struct lgdt3305_config hcw_lgdt3305_config = {
 	.i2c_addr           = 0x0e,
 	.mpeg_mode          = LGDT3305_MPEG_SERIAL,
@@ -1391,8 +1395,9 @@ static int dvb_init(struct saa7134_dev *dev)
 					wprintk("%s: Lifeview Trio, No tda826x found!\n", __func__);
 					goto detach_frontend;
 				}
-				if (dvb_attach(isl6421_attach, fe0->dvb.frontend, &dev->i2c_adap,
-										0x08, 0, 0) == NULL) {
+				if (dvb_attach(isl6421_attach, fe0->dvb.frontend,
+					       &dev->i2c_adap,
+					       0x08, 0, 0, false) == NULL) {
 					wprintk("%s: Lifeview Trio, No ISL6421 found!\n", __func__);
 					goto detach_frontend;
 				}
@@ -1509,7 +1514,8 @@ static int dvb_init(struct saa7134_dev *dev)
 				goto detach_frontend;
 			}
 			if (dvb_attach(isl6421_attach, fe0->dvb.frontend,
-				       &dev->i2c_adap, 0x08, 0, 0) == NULL) {
+				       &dev->i2c_adap,
+				       0x08, 0, 0, false) == NULL) {
 				wprintk("%s: No ISL6421 found!\n", __func__);
 				goto detach_frontend;
 			}
@@ -1820,6 +1826,25 @@ static int dvb_init(struct saa7134_dev *dev)
 				   &prohdtv_pro2_tda18271_config);
 		}
 		break;
+	case SAA7134_BOARD_AVERMEDIA_A706:
+		/* Enable all DVB-S devices now */
+		/* CE5039 DVB-S tuner SLEEP pin low */
+		saa7134_set_gpio(dev, 23, 0);
+		/* CE6313 DVB-S demod SLEEP pin low */
+		saa7134_set_gpio(dev, 9, 0);
+		/* CE6313 DVB-S demod RESET# pin high */
+		saa7134_set_gpio(dev, 25, 1);
+		msleep(1);
+		fe0->dvb.frontend = dvb_attach(mt312_attach,
+				&zl10313_avermedia_a706_config, &dev->i2c_adap);
+		if (fe0->dvb.frontend) {
+			fe0->dvb.frontend->ops.i2c_gate_ctrl = NULL;
+			if (dvb_attach(zl10039_attach, fe0->dvb.frontend,
+					0x60, &dev->i2c_adap) == NULL)
+				wprintk("%s: No zl10039 found!\n",
+					__func__);
+		}
+		break;
 	default:
 		wprintk("Huh? unknown DVB card?\n");
 		break;
diff --git a/drivers/media/pci/saa7134/saa7134-empress.c b/drivers/media/pci/saa7134/saa7134-empress.c
index 4df79c65690..66a70814004 100644
--- a/drivers/media/pci/saa7134/saa7134-empress.c
+++ b/drivers/media/pci/saa7134/saa7134-empress.c
@@ -428,7 +428,7 @@ static int empress_g_chip_ident(struct file *file, void *fh,
 	return -EINVAL;
 }
 
-static int empress_s_std(struct file *file, void *priv, v4l2_std_id *id)
+static int empress_s_std(struct file *file, void *priv, v4l2_std_id id)
 {
 	struct saa7134_dev *dev = file->private_data;
 
diff --git a/drivers/media/pci/saa7134/saa7134-i2c.c b/drivers/media/pci/saa7134/saa7134-i2c.c
index a176ec3285e..c68169d7580 100644
--- a/drivers/media/pci/saa7134/saa7134-i2c.c
+++ b/drivers/media/pci/saa7134/saa7134-i2c.c
@@ -256,6 +256,7 @@ static int saa7134_i2c_xfer(struct i2c_adapter *i2c_adap,
 				addr |= 1;
 			if (i > 0 && msgs[i].flags &
 			    I2C_M_RD && msgs[i].addr != 0x40 &&
+			    msgs[i].addr != 0x41 &&
 			    msgs[i].addr != 0x19) {
 				/* workaround for a saa7134 i2c bug
 				 * needed to talk to the mt352 demux
diff --git a/drivers/media/pci/saa7134/saa7134-input.c b/drivers/media/pci/saa7134/saa7134-input.c
index e761262f747..6f4312663bd 100644
--- a/drivers/media/pci/saa7134/saa7134-input.c
+++ b/drivers/media/pci/saa7134/saa7134-input.c
@@ -997,6 +997,9 @@ void saa7134_probe_i2c_ir(struct saa7134_dev *dev)
 	case SAA7134_BOARD_AVERMEDIA_CARDBUS_506:
 		info.addr = 0x40;
 		break;
+	case SAA7134_BOARD_AVERMEDIA_A706:
+		info.addr = 0x41;
+		break;
 	case SAA7134_BOARD_FLYDVB_TRIO:
 		dev->init_data.name = "FlyDVB Trio";
 		dev->init_data.get_key = get_key_flydvb_trio;
diff --git a/drivers/media/pci/saa7134/saa7134-tvaudio.c b/drivers/media/pci/saa7134/saa7134-tvaudio.c
index b7a99bee2f9..0f34e09d98d 100644
--- a/drivers/media/pci/saa7134/saa7134-tvaudio.c
+++ b/drivers/media/pci/saa7134/saa7134-tvaudio.c
@@ -796,6 +796,7 @@ static int tvaudio_thread_ddep(void *data)
 			dprintk("FM Radio\n");
 			if (dev->tuner_type == TUNER_PHILIPS_TDA8290) {
 				norms = (0x11 << 2) | 0x01;
+				/* set IF frequency to 5.5 MHz */
 				saa_dsp_writel(dev, 0x42c >> 2, 0x729555);
 			} else {
 				norms = (0x0f << 2) | 0x01;
diff --git a/drivers/media/pci/saa7134/saa7134-video.c b/drivers/media/pci/saa7134/saa7134-video.c
index 7c503fb6852..cc409380ee1 100644
--- a/drivers/media/pci/saa7134/saa7134-video.c
+++ b/drivers/media/pci/saa7134/saa7134-video.c
@@ -1176,14 +1176,6 @@ int saa7134_s_ctrl_internal(struct saa7134_dev *dev,  struct saa7134_fh *fh, str
 	int restart_overlay = 0;
 	int err;
 
-	/* When called from the empress code fh == NULL.
-	   That needs to be fixed somehow, but for now this is
-	   good enough. */
-	if (fh) {
-		err = v4l2_prio_check(&dev->prio, fh->prio);
-		if (0 != err)
-			return err;
-	}
 	err = -EINVAL;
 
 	mutex_lock(&dev->lock);
@@ -1352,6 +1344,7 @@ static int video_open(struct file *file)
 	if (NULL == fh)
 		return -ENOMEM;
 
+	v4l2_fh_init(&fh->fh, vdev);
 	file->private_data = fh;
 	fh->dev      = dev;
 	fh->radio    = radio;
@@ -1359,7 +1352,6 @@ static int video_open(struct file *file)
 	fh->fmt      = format_by_fourcc(V4L2_PIX_FMT_BGR24);
 	fh->width    = 720;
 	fh->height   = 576;
-	v4l2_prio_open(&dev->prio, &fh->prio);
 
 	videobuf_queue_sg_init(&fh->cap, &video_qops,
 			    &dev->pci->dev, &dev->slock,
@@ -1384,6 +1376,8 @@ static int video_open(struct file *file)
 		/* switch to video/vbi mode */
 		video_mux(dev,dev->ctl_input);
 	}
+	v4l2_fh_add(&fh->fh);
+
 	return 0;
 }
 
@@ -1504,7 +1498,8 @@ static int video_release(struct file *file)
 	saa7134_pgtable_free(dev->pci,&fh->pt_cap);
 	saa7134_pgtable_free(dev->pci,&fh->pt_vbi);
 
-	v4l2_prio_close(&dev->prio, fh->prio);
+	v4l2_fh_del(&fh->fh);
+	v4l2_fh_exit(&fh->fh);
 	file->private_data = NULL;
 	kfree(fh);
 	return 0;
@@ -1557,6 +1552,7 @@ static int saa7134_try_get_set_fmt_vbi_cap(struct file *file, void *priv,
 	struct saa7134_dev *dev = fh->dev;
 	struct saa7134_tvnorm *norm = dev->tvnorm;
 
+	memset(&f->fmt.vbi.reserved, 0, sizeof(f->fmt.vbi.reserved));
 	f->fmt.vbi.sampling_rate = 6750000 * 4;
 	f->fmt.vbi.samples_per_line = 2048 /* VBI_LINE_LENGTH */;
 	f->fmt.vbi.sample_format = V4L2_PIX_FMT_GREY;
@@ -1755,7 +1751,6 @@ static int saa7134_enum_input(struct file *file, void *priv,
 	strcpy(i->name, card_in(dev, n).name);
 	if (card_in(dev, n).tv)
 		i->type = V4L2_INPUT_TYPE_TUNER;
-	i->audioset = 1;
 	if (n == dev->ctl_input) {
 		int v1 = saa_readb(SAA7134_STATUS_VIDEO1);
 		int v2 = saa_readb(SAA7134_STATUS_VIDEO2);
@@ -1784,11 +1779,6 @@ static int saa7134_s_input(struct file *file, void *priv, unsigned int i)
 {
 	struct saa7134_fh *fh = priv;
 	struct saa7134_dev *dev = fh->dev;
-	int err;
-
-	err = v4l2_prio_check(&dev->prio, fh->prio);
-	if (0 != err)
-		return err;
 
 	if (i >= SAA7134_INPUT_MAX)
 		return -EINVAL;
@@ -1805,6 +1795,8 @@ static int saa7134_querycap(struct file *file, void  *priv,
 {
 	struct saa7134_fh *fh = priv;
 	struct saa7134_dev *dev = fh->dev;
+	struct video_device *vdev = video_devdata(file);
+	u32 radio_caps, video_caps, vbi_caps;
 
 	unsigned int tuner_type = dev->tuner_type;
 
@@ -1812,54 +1804,67 @@ static int saa7134_querycap(struct file *file, void  *priv,
 	strlcpy(cap->card, saa7134_boards[dev->board].name,
 		sizeof(cap->card));
 	sprintf(cap->bus_info, "PCI:%s", pci_name(dev->pci));
-	cap->capabilities =
-		V4L2_CAP_VIDEO_CAPTURE |
-		V4L2_CAP_VBI_CAPTURE |
-		V4L2_CAP_READWRITE |
-		V4L2_CAP_STREAMING |
-		V4L2_CAP_TUNER;
+
+	cap->device_caps = V4L2_CAP_READWRITE | V4L2_CAP_STREAMING;
+	if ((tuner_type != TUNER_ABSENT) && (tuner_type != UNSET))
+		cap->device_caps |= V4L2_CAP_TUNER;
+
+	radio_caps = V4L2_CAP_RADIO;
 	if (dev->has_rds)
-		cap->capabilities |= V4L2_CAP_RDS_CAPTURE;
+		radio_caps |= V4L2_CAP_RDS_CAPTURE;
+
+	video_caps = V4L2_CAP_VIDEO_CAPTURE;
 	if (saa7134_no_overlay <= 0)
-		cap->capabilities |= V4L2_CAP_VIDEO_OVERLAY;
+		video_caps |= V4L2_CAP_VIDEO_OVERLAY;
+
+	vbi_caps = V4L2_CAP_VBI_CAPTURE;
+
+	switch (vdev->vfl_type) {
+	case VFL_TYPE_RADIO:
+		cap->device_caps |= radio_caps;
+		break;
+	case VFL_TYPE_GRABBER:
+		cap->device_caps |= video_caps;
+		break;
+	case VFL_TYPE_VBI:
+		cap->device_caps |= vbi_caps;
+		break;
+	}
+	cap->capabilities = radio_caps | video_caps | vbi_caps |
+		cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	if (vdev->vfl_type == VFL_TYPE_RADIO) {
+		cap->device_caps &= ~V4L2_CAP_STREAMING;
+		if (!dev->has_rds)
+			cap->device_caps &= ~V4L2_CAP_READWRITE;
+	}
 
-	if ((tuner_type == TUNER_ABSENT) || (tuner_type == UNSET))
-		cap->capabilities &= ~V4L2_CAP_TUNER;
 	return 0;
 }
 
-int saa7134_s_std_internal(struct saa7134_dev *dev, struct saa7134_fh *fh, v4l2_std_id *id)
+int saa7134_s_std_internal(struct saa7134_dev *dev, struct saa7134_fh *fh, v4l2_std_id id)
 {
 	unsigned long flags;
 	unsigned int i;
 	v4l2_std_id fixup;
-	int err;
 
-	/* When called from the empress code fh == NULL.
-	   That needs to be fixed somehow, but for now this is
-	   good enough. */
-	if (fh) {
-		err = v4l2_prio_check(&dev->prio, fh->prio);
-		if (0 != err)
-			return err;
-	} else if (res_locked(dev, RESOURCE_OVERLAY)) {
+	if (!fh && res_locked(dev, RESOURCE_OVERLAY)) {
 		/* Don't change the std from the mpeg device
 		   if overlay is active. */
 		return -EBUSY;
 	}
 
 	for (i = 0; i < TVNORMS; i++)
-		if (*id == tvnorms[i].id)
+		if (id == tvnorms[i].id)
 			break;
 
 	if (i == TVNORMS)
 		for (i = 0; i < TVNORMS; i++)
-			if (*id & tvnorms[i].id)
+			if (id & tvnorms[i].id)
 				break;
 	if (i == TVNORMS)
 		return -EINVAL;
 
-	if ((*id & V4L2_STD_SECAM) && (secam[0] != '-')) {
+	if ((id & V4L2_STD_SECAM) && (secam[0] != '-')) {
 		if (secam[0] == 'L' || secam[0] == 'l') {
 			if (secam[1] == 'C' || secam[1] == 'c')
 				fixup = V4L2_STD_SECAM_LC;
@@ -1879,7 +1884,7 @@ int saa7134_s_std_internal(struct saa7134_dev *dev, struct saa7134_fh *fh, v4l2_
 			return -EINVAL;
 	}
 
-	*id = tvnorms[i].id;
+	id = tvnorms[i].id;
 
 	mutex_lock(&dev->lock);
 	if (fh && res_check(fh, RESOURCE_OVERLAY)) {
@@ -1901,7 +1906,7 @@ int saa7134_s_std_internal(struct saa7134_dev *dev, struct saa7134_fh *fh, v4l2_
 }
 EXPORT_SYMBOL_GPL(saa7134_s_std_internal);
 
-static int saa7134_s_std(struct file *file, void *priv, v4l2_std_id *id)
+static int saa7134_s_std(struct file *file, void *priv, v4l2_std_id id)
 {
 	struct saa7134_fh *fh = priv;
 
@@ -2009,11 +2014,11 @@ static int saa7134_g_tuner(struct file *file, void *priv,
 	if (NULL != card_in(dev, n).name) {
 		strcpy(t->name, "Television");
 		t->type = V4L2_TUNER_ANALOG_TV;
+		saa_call_all(dev, tuner, g_tuner, t);
 		t->capability = V4L2_TUNER_CAP_NORM |
 			V4L2_TUNER_CAP_STEREO |
 			V4L2_TUNER_CAP_LANG1 |
 			V4L2_TUNER_CAP_LANG2;
-		t->rangehigh = 0xffffffffUL;
 		t->rxsubchans = saa7134_tvaudio_getstereo(dev);
 		t->audmode = saa7134_tvaudio_rx2mode(t->rxsubchans);
 	}
@@ -2023,15 +2028,14 @@ static int saa7134_g_tuner(struct file *file, void *priv,
 }
 
 static int saa7134_s_tuner(struct file *file, void *priv,
-					struct v4l2_tuner *t)
+					const struct v4l2_tuner *t)
 {
 	struct saa7134_fh *fh = priv;
 	struct saa7134_dev *dev = fh->dev;
-	int rx, mode, err;
+	int rx, mode;
 
-	err = v4l2_prio_check(&dev->prio, fh->prio);
-	if (0 != err)
-		return err;
+	if (0 != t->index)
+		return -EINVAL;
 
 	mode = dev->thread.mode;
 	if (UNSET == mode) {
@@ -2050,22 +2054,20 @@ static int saa7134_g_frequency(struct file *file, void *priv,
 	struct saa7134_fh *fh = priv;
 	struct saa7134_dev *dev = fh->dev;
 
+	if (0 != f->tuner)
+		return -EINVAL;
+
 	f->type = fh->radio ? V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
-	f->frequency = dev->ctl_freq;
+	saa_call_all(dev, tuner, g_frequency, f);
 
 	return 0;
 }
 
 static int saa7134_s_frequency(struct file *file, void *priv,
-					struct v4l2_frequency *f)
+					const struct v4l2_frequency *f)
 {
 	struct saa7134_fh *fh = priv;
 	struct saa7134_dev *dev = fh->dev;
-	int err;
-
-	err = v4l2_prio_check(&dev->prio, fh->prio);
-	if (0 != err)
-		return err;
 
 	if (0 != f->tuner)
 		return -EINVAL;
@@ -2074,7 +2076,6 @@ static int saa7134_s_frequency(struct file *file, void *priv,
 	if (1 == fh->radio && V4L2_TUNER_RADIO != f->type)
 		return -EINVAL;
 	mutex_lock(&dev->lock);
-	dev->ctl_freq = f->frequency;
 
 	saa_call_all(dev, tuner, s_frequency, f);
 
@@ -2083,35 +2084,6 @@ static int saa7134_s_frequency(struct file *file, void *priv,
 	return 0;
 }
 
-static int saa7134_g_audio(struct file *file, void *priv, struct v4l2_audio *a)
-{
-	strcpy(a->name, "audio");
-	return 0;
-}
-
-static int saa7134_s_audio(struct file *file, void *priv, const struct v4l2_audio *a)
-{
-	return 0;
-}
-
-static int saa7134_g_priority(struct file *file, void *f, enum v4l2_priority *p)
-{
-	struct saa7134_fh *fh = f;
-	struct saa7134_dev *dev = fh->dev;
-
-	*p = v4l2_prio_max(&dev->prio);
-	return 0;
-}
-
-static int saa7134_s_priority(struct file *file, void *f,
-					enum v4l2_priority prio)
-{
-	struct saa7134_fh *fh = f;
-	struct saa7134_dev *dev = fh->dev;
-
-	return v4l2_prio_change(&dev->prio, &fh->prio, prio);
-}
-
 static int saa7134_enum_fmt_vid_cap(struct file *file, void  *priv,
 					struct v4l2_fmtdesc *f)
 {
@@ -2279,12 +2251,6 @@ static int saa7134_streamoff(struct file *file, void *priv,
 	return 0;
 }
 
-static int saa7134_g_parm(struct file *file, void *fh,
-				struct v4l2_streamparm *parm)
-{
-	return 0;
-}
-
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 static int vidioc_g_register (struct file *file, void *priv,
 			      struct v4l2_dbg_register *reg)
@@ -2300,7 +2266,7 @@ static int vidioc_g_register (struct file *file, void *priv,
 }
 
 static int vidioc_s_register (struct file *file, void *priv,
-				struct v4l2_dbg_register *reg)
+				const struct v4l2_dbg_register *reg)
 {
 	struct saa7134_fh *fh = priv;
 	struct saa7134_dev *dev = fh->dev;
@@ -2312,19 +2278,6 @@ static int vidioc_s_register (struct file *file, void *priv,
 }
 #endif
 
-static int radio_querycap(struct file *file, void *priv,
-					struct v4l2_capability *cap)
-{
-	struct saa7134_fh *fh = file->private_data;
-	struct saa7134_dev *dev = fh->dev;
-
-	strcpy(cap->driver, "saa7134");
-	strlcpy(cap->card, saa7134_boards[dev->board].name, sizeof(cap->card));
-	sprintf(cap->bus_info, "PCI:%s", pci_name(dev->pci));
-	cap->capabilities = V4L2_CAP_TUNER;
-	return 0;
-}
-
 static int radio_g_tuner(struct file *file, void *priv,
 					struct v4l2_tuner *t)
 {
@@ -2339,6 +2292,7 @@ static int radio_g_tuner(struct file *file, void *priv,
 	t->type = V4L2_TUNER_RADIO;
 
 	saa_call_all(dev, tuner, g_tuner, t);
+	t->audmode &= V4L2_TUNER_MODE_MONO | V4L2_TUNER_MODE_STEREO;
 	if (dev->input->amux == TV) {
 		t->signal = 0xf800 - ((saa_readb(0x581) & 0x1f) << 11);
 		t->rxsubchans = (saa_readb(0x529) & 0x08) ?
@@ -2347,7 +2301,7 @@ static int radio_g_tuner(struct file *file, void *priv,
 	return 0;
 }
 static int radio_s_tuner(struct file *file, void *priv,
-					struct v4l2_tuner *t)
+					const struct v4l2_tuner *t)
 {
 	struct saa7134_fh *fh = file->private_data;
 	struct saa7134_dev *dev = fh->dev;
@@ -2377,26 +2331,12 @@ static int radio_g_input(struct file *filp, void *priv, unsigned int *i)
 	return 0;
 }
 
-static int radio_g_audio(struct file *file, void *priv,
-					struct v4l2_audio *a)
-{
-	memset(a, 0, sizeof(*a));
-	strcpy(a->name, "Radio");
-	return 0;
-}
-
-static int radio_s_audio(struct file *file, void *priv,
-					const struct v4l2_audio *a)
-{
-	return 0;
-}
-
 static int radio_s_input(struct file *filp, void *priv, unsigned int i)
 {
 	return 0;
 }
 
-static int radio_s_std(struct file *file, void *fh, v4l2_std_id *norm)
+static int radio_s_std(struct file *file, void *fh, v4l2_std_id norm)
 {
 	return 0;
 }
@@ -2441,8 +2381,6 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_g_fmt_vbi_cap		= saa7134_try_get_set_fmt_vbi_cap,
 	.vidioc_try_fmt_vbi_cap		= saa7134_try_get_set_fmt_vbi_cap,
 	.vidioc_s_fmt_vbi_cap		= saa7134_try_get_set_fmt_vbi_cap,
-	.vidioc_g_audio			= saa7134_g_audio,
-	.vidioc_s_audio			= saa7134_s_audio,
 	.vidioc_cropcap			= saa7134_cropcap,
 	.vidioc_reqbufs			= saa7134_reqbufs,
 	.vidioc_querybuf		= saa7134_querybuf,
@@ -2465,9 +2403,6 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_g_fbuf			= saa7134_g_fbuf,
 	.vidioc_s_fbuf			= saa7134_s_fbuf,
 	.vidioc_overlay			= saa7134_overlay,
-	.vidioc_g_priority		= saa7134_g_priority,
-	.vidioc_s_priority		= saa7134_s_priority,
-	.vidioc_g_parm			= saa7134_g_parm,
 	.vidioc_g_frequency		= saa7134_g_frequency,
 	.vidioc_s_frequency		= saa7134_s_frequency,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
@@ -2486,12 +2421,10 @@ static const struct v4l2_file_operations radio_fops = {
 };
 
 static const struct v4l2_ioctl_ops radio_ioctl_ops = {
-	.vidioc_querycap	= radio_querycap,
+	.vidioc_querycap	= saa7134_querycap,
 	.vidioc_g_tuner		= radio_g_tuner,
 	.vidioc_enum_input	= radio_enum_input,
-	.vidioc_g_audio		= radio_g_audio,
 	.vidioc_s_tuner		= radio_s_tuner,
-	.vidioc_s_audio		= radio_s_audio,
 	.vidioc_s_input		= radio_s_input,
 	.vidioc_s_std		= radio_s_std,
 	.vidioc_queryctrl	= radio_queryctrl,
diff --git a/drivers/media/pci/saa7134/saa7134.h b/drivers/media/pci/saa7134/saa7134.h
index 71eefef5e32..d2ad16c1569 100644
--- a/drivers/media/pci/saa7134/saa7134.h
+++ b/drivers/media/pci/saa7134/saa7134.h
@@ -36,6 +36,7 @@
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-device.h>
+#include <media/v4l2-fh.h>
 #include <media/tuner.h>
 #include <media/rc-core.h>
 #include <media/ir-kbd-i2c.h>
@@ -45,6 +46,7 @@
 #if IS_ENABLED(CONFIG_VIDEO_SAA7134_DVB)
 #include <media/videobuf-dvb.h>
 #endif
+#include "tda8290.h"
 
 #define UNSET (-1U)
 
@@ -334,6 +336,7 @@ struct saa7134_card_ir {
 #define SAA7134_BOARD_KWORLD_PC150U         189
 #define SAA7134_BOARD_ASUSTeK_PS3_100      190
 #define SAA7134_BOARD_HAWELL_HW_9004V1      191
+#define SAA7134_BOARD_AVERMEDIA_A706		192
 
 #define SAA7134_MAXBOARDS 32
 #define SAA7134_INPUT_MAX 8
@@ -390,7 +393,7 @@ struct saa7134_board {
 	unsigned char		rds_addr;
 
 	unsigned int            tda9887_conf;
-	unsigned int            tuner_config;
+	struct tda829x_config   tda829x_conf;
 
 	/* peripheral I/O */
 	enum saa7134_video_out  video_out;
@@ -466,11 +469,11 @@ struct saa7134_dmaqueue {
 
 /* video filehandle status */
 struct saa7134_fh {
+	struct v4l2_fh             fh;
 	struct saa7134_dev         *dev;
 	unsigned int               radio;
 	enum v4l2_buf_type         type;
 	unsigned int               resources;
-	enum v4l2_priority	   prio;
 	struct pm_qos_request	   qos_request;
 
 	/* video overlay */
@@ -542,7 +545,6 @@ struct saa7134_dev {
 	struct list_head           devlist;
 	struct mutex               lock;
 	spinlock_t                 slock;
-	struct v4l2_prio_state     prio;
 	struct v4l2_device         v4l2_dev;
 	/* workstruct for loading modules */
 	struct work_struct request_module_wk;
@@ -605,7 +607,6 @@ struct saa7134_dev {
 	int                        ctl_contrast;
 	int                        ctl_hue;
 	int                        ctl_saturation;
-	int                        ctl_freq;
 	int                        ctl_mute;             /* audio */
 	int                        ctl_volume;
 	int                        ctl_invert;           /* private */
@@ -766,7 +767,7 @@ extern struct video_device saa7134_radio_template;
 int saa7134_s_ctrl_internal(struct saa7134_dev *dev,  struct saa7134_fh *fh, struct v4l2_control *c);
 int saa7134_g_ctrl_internal(struct saa7134_dev *dev,  struct saa7134_fh *fh, struct v4l2_control *c);
 int saa7134_queryctrl(struct file *file, void *priv, struct v4l2_queryctrl *c);
-int saa7134_s_std_internal(struct saa7134_dev *dev,  struct saa7134_fh *fh, v4l2_std_id *id);
+int saa7134_s_std_internal(struct saa7134_dev *dev,  struct saa7134_fh *fh, v4l2_std_id id);
 
 int saa7134_videoport_init(struct saa7134_dev *dev);
 void saa7134_set_tvnorm_hw(struct saa7134_dev *dev);
diff --git a/drivers/media/pci/saa7146/mxb.c b/drivers/media/pci/saa7146/mxb.c
index 91369daad72..71e8bead321 100644
--- a/drivers/media/pci/saa7146/mxb.c
+++ b/drivers/media/pci/saa7146/mxb.c
@@ -560,7 +560,7 @@ static int vidioc_g_tuner(struct file *file, void *fh, struct v4l2_tuner *t)
 	return call_all(dev, tuner, g_tuner, t);
 }
 
-static int vidioc_s_tuner(struct file *file, void *fh, struct v4l2_tuner *t)
+static int vidioc_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *t)
 {
 	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
 	struct mxb *mxb = (struct mxb *)dev->ext_priv;
@@ -595,7 +595,7 @@ static int vidioc_g_frequency(struct file *file, void *fh, struct v4l2_frequency
 	return 0;
 }
 
-static int vidioc_s_frequency(struct file *file, void *fh, struct v4l2_frequency *f)
+static int vidioc_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *f)
 {
 	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
 	struct mxb *mxb = (struct mxb *)dev->ext_priv;
@@ -612,8 +612,8 @@ static int vidioc_s_frequency(struct file *file, void *fh, struct v4l2_frequency
 	/* tune in desired frequency */
 	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;
+	tuner_call(mxb, tuner, g_frequency, &mxb->cur_freq);
 	if (mxb->cur_audinput == 0)
 		mxb_update_audmode(mxb);
 
@@ -680,7 +680,7 @@ static int vidioc_g_register(struct file *file, void *fh, struct v4l2_dbg_regist
 	return 0;
 }
 
-static int vidioc_s_register(struct file *file, void *fh, struct v4l2_dbg_register *reg)
+static int vidioc_s_register(struct file *file, void *fh, const struct v4l2_dbg_register *reg)
 {
 	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
 
@@ -688,7 +688,6 @@ static int vidioc_s_register(struct file *file, void *fh, struct v4l2_dbg_regist
 		return -EPERM;
 	if (v4l2_chip_match_host(&reg->match)) {
 		saa7146_write(dev, reg->reg, reg->val);
-		reg->size = 4;
 		return 0;
 	}
 	return call_all(dev, core, s_register, reg);
diff --git a/drivers/media/pci/saa7164/saa7164-core.c b/drivers/media/pci/saa7164/saa7164-core.c
index 63502e7a2a7..7618fdae811 100644
--- a/drivers/media/pci/saa7164/saa7164-core.c
+++ b/drivers/media/pci/saa7164/saa7164-core.c
@@ -54,7 +54,7 @@ MODULE_PARM_DESC(debug, "enable debug messages");
 
 unsigned int fw_debug;
 module_param(fw_debug, int, 0644);
-MODULE_PARM_DESC(fw_debug, "Firware debug level def:2");
+MODULE_PARM_DESC(fw_debug, "Firmware debug level def:2");
 
 unsigned int encoder_buffers = SAA7164_MAX_ENCODER_BUFFERS;
 module_param(encoder_buffers, int, 0644);
diff --git a/drivers/media/pci/saa7164/saa7164-encoder.c b/drivers/media/pci/saa7164/saa7164-encoder.c
index 9bb0903ee5f..0b74fb2300d 100644
--- a/drivers/media/pci/saa7164/saa7164-encoder.c
+++ b/drivers/media/pci/saa7164/saa7164-encoder.c
@@ -211,17 +211,17 @@ static int saa7164_encoder_initialize(struct saa7164_port *port)
 }
 
 /* -- V4L2 --------------------------------------------------------- */
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *id)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id id)
 {
 	struct saa7164_encoder_fh *fh = file->private_data;
 	struct saa7164_port *port = fh->port;
 	struct saa7164_dev *dev = port->dev;
 	unsigned int i;
 
-	dprintk(DBGLVL_ENC, "%s(id=0x%x)\n", __func__, (u32)*id);
+	dprintk(DBGLVL_ENC, "%s(id=0x%x)\n", __func__, (u32)id);
 
 	for (i = 0; i < ARRAY_SIZE(saa7164_tvnorms); i++) {
-		if (*id & saa7164_tvnorms[i].id)
+		if (id & saa7164_tvnorms[i].id)
 			break;
 	}
 	if (i == ARRAY_SIZE(saa7164_tvnorms))
@@ -234,7 +234,7 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *id)
 	 */
 	saa7164_api_set_audio_std(port);
 
-	dprintk(DBGLVL_ENC, "%s(id=0x%x) OK\n", __func__, (u32)*id);
+	dprintk(DBGLVL_ENC, "%s(id=0x%x) OK\n", __func__, (u32)id);
 
 	return 0;
 }
@@ -318,7 +318,7 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-	struct v4l2_tuner *t)
+	const struct v4l2_tuner *t)
 {
 	/* Update the A/V core */
 	return 0;
@@ -337,7 +337,7 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-	struct v4l2_frequency *f)
+	const struct v4l2_frequency *f)
 {
 	struct saa7164_encoder_fh *fh = file->private_data;
 	struct saa7164_port *port = fh->port;
@@ -1313,7 +1313,7 @@ static int saa7164_g_register(struct file *file, void *fh,
 }
 
 static int saa7164_s_register(struct file *file, void *fh,
-			      struct v4l2_dbg_register *reg)
+			      const struct v4l2_dbg_register *reg)
 {
 	struct saa7164_port *port = ((struct saa7164_encoder_fh *)fh)->port;
 	struct saa7164_dev *dev = port->dev;
diff --git a/drivers/media/pci/saa7164/saa7164-vbi.c b/drivers/media/pci/saa7164/saa7164-vbi.c
index b4532299c0e..da224eb39b9 100644
--- a/drivers/media/pci/saa7164/saa7164-vbi.c
+++ b/drivers/media/pci/saa7164/saa7164-vbi.c
@@ -183,17 +183,17 @@ static int saa7164_vbi_initialize(struct saa7164_port *port)
 }
 
 /* -- V4L2 --------------------------------------------------------- */
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *id)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id id)
 {
 	struct saa7164_vbi_fh *fh = file->private_data;
 	struct saa7164_port *port = fh->port;
 	struct saa7164_dev *dev = port->dev;
 	unsigned int i;
 
-	dprintk(DBGLVL_VBI, "%s(id=0x%x)\n", __func__, (u32)*id);
+	dprintk(DBGLVL_VBI, "%s(id=0x%x)\n", __func__, (u32)id);
 
 	for (i = 0; i < ARRAY_SIZE(saa7164_tvnorms); i++) {
-		if (*id & saa7164_tvnorms[i].id)
+		if (id & saa7164_tvnorms[i].id)
 			break;
 	}
 	if (i == ARRAY_SIZE(saa7164_tvnorms))
@@ -206,7 +206,7 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *id)
 	 */
 	saa7164_api_set_audio_std(port);
 
-	dprintk(DBGLVL_VBI, "%s(id=0x%x) OK\n", __func__, (u32)*id);
+	dprintk(DBGLVL_VBI, "%s(id=0x%x) OK\n", __func__, (u32)id);
 
 	return 0;
 }
@@ -290,7 +290,7 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-	struct v4l2_tuner *t)
+	const struct v4l2_tuner *t)
 {
 	/* Update the A/V core */
 	return 0;
@@ -309,7 +309,7 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-	struct v4l2_frequency *f)
+	const struct v4l2_frequency *f)
 {
 	struct saa7164_vbi_fh *fh = file->private_data;
 	struct saa7164_port *port = fh->port;
diff --git a/drivers/media/pci/sta2x11/Kconfig b/drivers/media/pci/sta2x11/Kconfig
index a94ccad0206..03130157db8 100644
--- a/drivers/media/pci/sta2x11/Kconfig
+++ b/drivers/media/pci/sta2x11/Kconfig
@@ -4,6 +4,7 @@ config STA2X11_VIP
 	select VIDEO_ADV7180 if MEDIA_SUBDRV_AUTOSELECT
 	select VIDEOBUF2_DMA_CONTIG
 	depends on PCI && VIDEO_V4L2 && VIRT_TO_BUS
+	depends on I2C
 	help
 	  Say Y for support for STA2X11 VIP (Video Input Port) capture
 	  device.
diff --git a/drivers/media/pci/sta2x11/sta2x11_vip.c b/drivers/media/pci/sta2x11/sta2x11_vip.c
index 4b703fe8c95..7005695aa4b 100644
--- a/drivers/media/pci/sta2x11/sta2x11_vip.c
+++ b/drivers/media/pci/sta2x11/sta2x11_vip.c
@@ -439,22 +439,22 @@ static int vidioc_querycap(struct file *file, void *priv,
  *
  * other, returned from video DAC.
  */
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *std)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id std)
 {
 	struct sta2x11_vip *vip = video_drvdata(file);
 	v4l2_std_id oldstd = vip->std, newstd;
 	int status;
 
-	if (V4L2_STD_ALL == *std) {
-		v4l2_subdev_call(vip->decoder, core, s_std, *std);
+	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))
+		std = vip->std = newstd;
+		if (oldstd != std) {
+			if (V4L2_STD_525_60 & std)
 				vip->format = formats_60[0];
 			else
 				vip->format = formats_50[0];
@@ -462,14 +462,14 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *std)
 		return 0;
 	}
 
-	if (oldstd != *std) {
-		if (V4L2_STD_525_60 & (*std))
+	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);
+	return v4l2_subdev_call(vip->decoder, core, s_std, std);
 }
 
 /**
diff --git a/drivers/media/pci/ttpci/av7110.c b/drivers/media/pci/ttpci/av7110.c
index 3dc7aa9b6f4..f38329d29da 100644
--- a/drivers/media/pci/ttpci/av7110.c
+++ b/drivers/media/pci/ttpci/av7110.c
@@ -990,7 +990,7 @@ static int av7110_start_feed(struct dvb_demux_feed *feed)
 
 	if (feed->type == DMX_TYPE_TS) {
 		if ((feed->ts_type & TS_DECODER) &&
-		    (feed->pes_type <= DMX_TS_PES_PCR)) {
+		    (feed->pes_type <= DMX_PES_PCR)) {
 			switch (demux->dmx.frontend->source) {
 			case DMX_MEMORY_FE:
 				if (feed->ts_type & TS_DECODER)
@@ -1051,14 +1051,14 @@ static int av7110_stop_feed(struct dvb_demux_feed *feed)
 
 	if (feed->type == DMX_TYPE_TS) {
 		if (feed->ts_type & TS_DECODER) {
-			if (feed->pes_type >= DMX_TS_PES_OTHER ||
+			if (feed->pes_type >= DMX_PES_OTHER ||
 			    !demux->pesfilter[feed->pes_type])
 				return -EINVAL;
 			demux->pids[feed->pes_type] |= 0x8000;
 			demux->pesfilter[feed->pes_type] = NULL;
 		}
 		if (feed->ts_type & TS_DECODER &&
-		    feed->pes_type < DMX_TS_PES_OTHER) {
+		    feed->pes_type < DMX_PES_OTHER) {
 			ret = dvb_feed_stop_pid(feed);
 		} else
 			if ((feed->ts_type & TS_PACKET) &&
diff --git a/drivers/media/pci/ttpci/av7110_v4l.c b/drivers/media/pci/ttpci/av7110_v4l.c
index 730e906ea91..6c4076acb13 100644
--- a/drivers/media/pci/ttpci/av7110_v4l.c
+++ b/drivers/media/pci/ttpci/av7110_v4l.c
@@ -366,7 +366,7 @@ static int vidioc_g_tuner(struct file *file, void *fh, struct v4l2_tuner *t)
 	return 0;
 }
 
-static int vidioc_s_tuner(struct file *file, void *fh, struct v4l2_tuner *t)
+static int vidioc_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *t)
 {
 	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
 	struct av7110 *av7110 = (struct av7110 *)dev->ext_priv;
@@ -426,7 +426,7 @@ static int vidioc_g_frequency(struct file *file, void *fh, struct v4l2_frequency
 	return 0;
 }
 
-static int vidioc_s_frequency(struct file *file, void *fh, struct v4l2_frequency *f)
+static int vidioc_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *f)
 {
 	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
 	struct av7110 *av7110 = (struct av7110 *)dev->ext_priv;
diff --git a/drivers/media/pci/ttpci/budget.c b/drivers/media/pci/ttpci/budget.c
index 7e6e43ae5c5..6ccc48833fd 100644
--- a/drivers/media/pci/ttpci/budget.c
+++ b/drivers/media/pci/ttpci/budget.c
@@ -537,6 +537,16 @@ static void frontend_init(struct budget *budget)
 		}
 		break;
 
+	case 0x4f52: /* Cards based on Philips Semi Sylt PCI ref. design */
+		budget->dvb_frontend = dvb_attach(stv0299_attach, &alps_bsru6_config, &budget->i2c_adap);
+		if (budget->dvb_frontend) {
+			printk(KERN_INFO "budget: tuner ALPS BSRU6 in Philips Semi. Sylt detected\n");
+			budget->dvb_frontend->ops.tuner_ops.set_params = alps_bsru6_tuner_set_params;
+			budget->dvb_frontend->tuner_priv = &budget->i2c_adap;
+			break;
+		}
+		break;
+
 	case 0x4f60: /* Fujitsu Siemens Activy Budget-S PCI rev AL (stv0299/tsa5059) */
 	{
 		int subtype = i2c_readreg(&budget->i2c_adap, 0x50, 0x67);
@@ -818,6 +828,7 @@ MAKE_BUDGET_INFO(fsacs1, "Fujitsu Siemens Activy Budget-S PCI (rev AL/alps front
 MAKE_BUDGET_INFO(fsact,	 "Fujitsu Siemens Activy Budget-T PCI (rev GR/Grundig frontend)", BUDGET_FS_ACTIVY);
 MAKE_BUDGET_INFO(fsact1, "Fujitsu Siemens Activy Budget-T PCI (rev AL/ALPS TDHD1-204A)", BUDGET_FS_ACTIVY);
 MAKE_BUDGET_INFO(omicom, "Omicom S2 PCI", BUDGET_TT);
+MAKE_BUDGET_INFO(sylt,   "Philips Semi Sylt PCI", BUDGET_TT_HW_DISEQC);
 
 static struct pci_device_id pci_tbl[] = {
 	MAKE_EXTENSION_PCI(ttbs,  0x13c2, 0x1003),
@@ -832,6 +843,7 @@ static struct pci_device_id pci_tbl[] = {
 	MAKE_EXTENSION_PCI(fsact1, 0x1131, 0x5f60),
 	MAKE_EXTENSION_PCI(fsact, 0x1131, 0x5f61),
 	MAKE_EXTENSION_PCI(omicom, 0x14c4, 0x1020),
+	MAKE_EXTENSION_PCI(sylt, 0x1131, 0x4f52),
 	{
 		.vendor    = 0,
 	}
diff --git a/drivers/media/pci/zoran/zoran_driver.c b/drivers/media/pci/zoran/zoran_driver.c
index 2e8f518f298..1168a84a737 100644
--- a/drivers/media/pci/zoran/zoran_driver.c
+++ b/drivers/media/pci/zoran/zoran_driver.c
@@ -2435,14 +2435,14 @@ static int zoran_g_std(struct file *file, void *__fh, v4l2_std_id *std)
 	return 0;
 }
 
-static int zoran_s_std(struct file *file, void *__fh, v4l2_std_id *std)
+static int zoran_s_std(struct file *file, void *__fh, v4l2_std_id std)
 {
 	struct zoran_fh *fh = __fh;
 	struct zoran *zr = fh->zr;
 	int res = 0;
 
 	mutex_lock(&zr->resource_lock);
-	res = zoran_set_norm(zr, *std);
+	res = zoran_set_norm(zr, std);
 	if (res)
 		goto sstd_unlock_and_return;
 
diff --git a/drivers/media/pci/zoran/zoran_procfs.c b/drivers/media/pci/zoran/zoran_procfs.c
index e084b0a21b1..1512b5d4053 100644
--- a/drivers/media/pci/zoran/zoran_procfs.c
+++ b/drivers/media/pci/zoran/zoran_procfs.c
@@ -201,7 +201,7 @@ zoran_proc_init (struct zoran *zr)
 		dprintk(2,
 			KERN_INFO
 			"%s: procfs entry /proc/%s allocated. data=%p\n",
-			ZR_DEVNAME(zr), name, zr->zoran_proc->data);
+			ZR_DEVNAME(zr), name, zr);
 	} else {
 		dprintk(1, KERN_ERR "%s: Unable to initialise /proc/%s\n",
 			ZR_DEVNAME(zr), name);
diff --git a/drivers/media/platform/Kconfig b/drivers/media/platform/Kconfig
index 05d7b633346..0494d2769fd 100644
--- a/drivers/media/platform/Kconfig
+++ b/drivers/media/platform/Kconfig
@@ -122,7 +122,7 @@ config VIDEO_S3C_CAMIF
 	  will be called s3c-camif.
 
 source "drivers/media/platform/soc_camera/Kconfig"
-source "drivers/media/platform/s5p-fimc/Kconfig"
+source "drivers/media/platform/exynos4-is/Kconfig"
 source "drivers/media/platform/s5p-tv/Kconfig"
 
 endif # V4L_PLATFORM_DRIVERS
@@ -145,7 +145,6 @@ config VIDEO_CODA
 	depends on VIDEO_DEV && VIDEO_V4L2 && ARCH_MXC
 	select VIDEOBUF2_DMA_CONTIG
 	select V4L2_MEM2MEM_DEV
-	select IRAM_ALLOC if SOC_IMX53
 	---help---
 	   Coda is a range of video codec IPs that supports
 	   H.264, MPEG-4, and other video formats.
@@ -204,7 +203,7 @@ config VIDEO_SAMSUNG_EXYNOS_GSC
 
 config VIDEO_SH_VEU
 	tristate "SuperH VEU mem2mem video processing driver"
-	depends on VIDEO_DEV && VIDEO_V4L2
+	depends on VIDEO_DEV && VIDEO_V4L2 && GENERIC_HARDIRQS
 	select VIDEOBUF2_DMA_CONTIG
 	select V4L2_MEM2MEM_DEV
 	help
diff --git a/drivers/media/platform/Makefile b/drivers/media/platform/Makefile
index 42089ba3600..eee28dd78d7 100644
--- a/drivers/media/platform/Makefile
+++ b/drivers/media/platform/Makefile
@@ -30,7 +30,7 @@ obj-$(CONFIG_VIDEO_SH_VEU)		+= sh_veu.o
 obj-$(CONFIG_VIDEO_MEM2MEM_DEINTERLACE)	+= m2m-deinterlace.o
 
 obj-$(CONFIG_VIDEO_S3C_CAMIF) 		+= s3c-camif/
-obj-$(CONFIG_VIDEO_SAMSUNG_S5P_FIMC) 	+= s5p-fimc/
+obj-$(CONFIG_VIDEO_SAMSUNG_EXYNOS4_IS) 	+= exynos4-is/
 obj-$(CONFIG_VIDEO_SAMSUNG_S5P_JPEG)	+= s5p-jpeg/
 obj-$(CONFIG_VIDEO_SAMSUNG_S5P_MFC)	+= s5p-mfc/
 obj-$(CONFIG_VIDEO_SAMSUNG_S5P_TV)	+= s5p-tv/
diff --git a/drivers/media/platform/blackfin/bfin_capture.c b/drivers/media/platform/blackfin/bfin_capture.c
index 5f209d5810d..0e55b087076 100644
--- a/drivers/media/platform/blackfin/bfin_capture.c
+++ b/drivers/media/platform/blackfin/bfin_capture.c
@@ -384,7 +384,7 @@ static int bcap_start_streaming(struct vb2_queue *vq, unsigned int count)
 	params.ppi_control = bcap_dev->cfg->ppi_control;
 	params.int_mask = bcap_dev->cfg->int_mask;
 	if (bcap_dev->cfg->inputs[bcap_dev->cur_input].capabilities
-			& V4L2_IN_CAP_CUSTOM_TIMINGS) {
+			& V4L2_IN_CAP_DV_TIMINGS) {
 		struct v4l2_bt_timings *bt = &bcap_dev->dv_timings.bt;
 
 		params.hdelay = bt->hsync + bt->hbackporch;
@@ -633,7 +633,7 @@ static int bcap_g_std(struct file *file, void *priv, v4l2_std_id *std)
 	return 0;
 }
 
-static int bcap_s_std(struct file *file, void *priv, v4l2_std_id *std)
+static int bcap_s_std(struct file *file, void *priv, v4l2_std_id std)
 {
 	struct bcap_device *bcap_dev = video_drvdata(file);
 	int ret;
@@ -641,11 +641,11 @@ static int bcap_s_std(struct file *file, void *priv, v4l2_std_id *std)
 	if (vb2_is_busy(&bcap_dev->buffer_queue))
 		return -EBUSY;
 
-	ret = v4l2_subdev_call(bcap_dev->sd, core, s_std, *std);
+	ret = v4l2_subdev_call(bcap_dev->sd, core, s_std, std);
 	if (ret < 0)
 		return ret;
 
-	bcap_dev->std = *std;
+	bcap_dev->std = std;
 	return 0;
 }
 
@@ -890,7 +890,7 @@ static int bcap_dbg_g_register(struct file *file, void *priv,
 }
 
 static int bcap_dbg_s_register(struct file *file, void *priv,
-		struct v4l2_dbg_register *reg)
+		const struct v4l2_dbg_register *reg)
 {
 	struct bcap_device *bcap_dev = video_drvdata(file);
 
@@ -1029,6 +1029,7 @@ static int bcap_probe(struct platform_device *pdev)
 	q->buf_struct_size = sizeof(struct bcap_buffer);
 	q->ops = &bcap_video_qops;
 	q->mem_ops = &vb2_dma_contig_memops;
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 
 	vb2_queue_init(q);
 
@@ -1110,7 +1111,7 @@ static int bcap_probe(struct platform_device *pdev)
 		}
 		bcap_dev->std = std;
 	}
-	if (config->inputs[0].capabilities & V4L2_IN_CAP_CUSTOM_TIMINGS) {
+	if (config->inputs[0].capabilities & V4L2_IN_CAP_DV_TIMINGS) {
 		struct v4l2_dv_timings dv_timings;
 		ret = v4l2_subdev_call(bcap_dev->sd, video,
 				g_dv_timings, &dv_timings);
diff --git a/drivers/media/platform/coda.c b/drivers/media/platform/coda.c
index 20827ba168f..48b8d7af386 100644
--- a/drivers/media/platform/coda.c
+++ b/drivers/media/platform/coda.c
@@ -14,6 +14,7 @@
 #include <linux/clk.h>
 #include <linux/delay.h>
 #include <linux/firmware.h>
+#include <linux/genalloc.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
 #include <linux/irq.h>
@@ -23,7 +24,7 @@
 #include <linux/slab.h>
 #include <linux/videodev2.h>
 #include <linux/of.h>
-#include <linux/platform_data/imx-iram.h>
+#include <linux/platform_data/coda.h>
 
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
@@ -43,6 +44,7 @@
 #define CODA7_WORK_BUF_SIZE	(512 * 1024 + CODA_FMO_BUF_SIZE * 8 * 1024)
 #define CODA_PARA_BUF_SIZE	(10 * 1024)
 #define CODA_ISRAM_SIZE	(2048 * 2)
+#define CODADX6_IRAM_SIZE	0xb000
 #define CODA7_IRAM_SIZE		0x14000 /* 81920 bytes */
 
 #define CODA_MAX_FRAMEBUFFERS	2
@@ -128,7 +130,10 @@ struct coda_dev {
 
 	struct coda_aux_buf	codebuf;
 	struct coda_aux_buf	workbuf;
+	struct gen_pool		*iram_pool;
+	long unsigned int	iram_vaddr;
 	long unsigned int	iram_paddr;
+	unsigned long		iram_size;
 
 	spinlock_t		irqlock;
 	struct mutex		dev_mutex;
@@ -1422,6 +1427,7 @@ static int coda_queue_init(void *priv, struct vb2_queue *src_vq,
 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	src_vq->ops = &coda_qops;
 	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	ret = vb2_queue_init(src_vq);
 	if (ret)
@@ -1433,6 +1439,7 @@ static int coda_queue_init(void *priv, struct vb2_queue *src_vq,
 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	dst_vq->ops = &coda_qops;
 	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	return vb2_queue_init(dst_vq);
 }
@@ -1628,6 +1635,9 @@ static irqreturn_t coda_irq_handler(int irq, void *data)
 		dst_buf->v4l2_buf.flags &= ~V4L2_BUF_FLAG_KEYFRAME;
 	}
 
+	dst_buf->v4l2_buf.timestamp = src_buf->v4l2_buf.timestamp;
+	dst_buf->v4l2_buf.timecode = src_buf->v4l2_buf.timecode;
+
 	v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_DONE);
 	v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_DONE);
 
@@ -1926,6 +1936,9 @@ static int coda_probe(struct platform_device *pdev)
 	const struct of_device_id *of_id =
 			of_match_device(of_match_ptr(coda_dt_ids), &pdev->dev);
 	const struct platform_device_id *pdev_id;
+	struct coda_platform_data *pdata = pdev->dev.platform_data;
+	struct device_node *np = pdev->dev.of_node;
+	struct gen_pool *pool;
 	struct coda_dev *dev;
 	struct resource *res;
 	int ret, irq;
@@ -1988,6 +2001,16 @@ static int coda_probe(struct platform_device *pdev)
 		return -ENOENT;
 	}
 
+	/* Get IRAM pool from device tree or platform data */
+	pool = of_get_named_gen_pool(np, "iram", 0);
+	if (!pool && pdata)
+		pool = dev_get_gen_pool(pdata->iram_dev);
+	if (!pool) {
+		dev_err(&pdev->dev, "iram pool not available\n");
+		return -ENOMEM;
+	}
+	dev->iram_pool = pool;
+
 	ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
 	if (ret)
 		return ret;
@@ -2022,18 +2045,17 @@ static int coda_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
-	if (dev->devtype->product == CODA_DX6) {
-		dev->iram_paddr = 0xffff4c00;
-	} else {
-		void __iomem *iram_vaddr;
-
-		iram_vaddr = iram_alloc(CODA7_IRAM_SIZE,
-					&dev->iram_paddr);
-		if (!iram_vaddr) {
-			dev_err(&pdev->dev, "unable to alloc iram\n");
-			return -ENOMEM;
-		}
+	if (dev->devtype->product == CODA_DX6)
+		dev->iram_size = CODADX6_IRAM_SIZE;
+	else
+		dev->iram_size = CODA7_IRAM_SIZE;
+	dev->iram_vaddr = gen_pool_alloc(dev->iram_pool, dev->iram_size);
+	if (!dev->iram_vaddr) {
+		dev_err(&pdev->dev, "unable to alloc iram\n");
+		return -ENOMEM;
 	}
+	dev->iram_paddr = gen_pool_virt_to_phys(dev->iram_pool,
+						dev->iram_vaddr);
 
 	platform_set_drvdata(pdev, dev);
 
@@ -2050,8 +2072,8 @@ static int coda_remove(struct platform_device *pdev)
 	if (dev->alloc_ctx)
 		vb2_dma_contig_cleanup_ctx(dev->alloc_ctx);
 	v4l2_device_unregister(&dev->v4l2_dev);
-	if (dev->iram_paddr)
-		iram_free(dev->iram_paddr, CODA7_IRAM_SIZE);
+	if (dev->iram_vaddr)
+		gen_pool_free(dev->iram_pool, dev->iram_vaddr, dev->iram_size);
 	if (dev->codebuf.vaddr)
 		dma_free_coherent(&pdev->dev, dev->codebuf.size,
 				  &dev->codebuf.vaddr, dev->codebuf.paddr);
diff --git a/drivers/media/platform/davinci/Kconfig b/drivers/media/platform/davinci/Kconfig
index ccfde4eb626..afb3aec1320 100644
--- a/drivers/media/platform/davinci/Kconfig
+++ b/drivers/media/platform/davinci/Kconfig
@@ -1,79 +1,47 @@
 config VIDEO_DAVINCI_VPIF_DISPLAY
-	tristate "DM646x/DA850/OMAPL138 EVM Video Display"
-	depends on VIDEO_DEV && (MACH_DAVINCI_DM6467_EVM || MACH_DAVINCI_DA850_EVM)
+	tristate "TI DaVinci VPIF V4L2-Display driver"
+	depends on VIDEO_DEV && ARCH_DAVINCI
 	select VIDEOBUF2_DMA_CONTIG
-	select VIDEO_DAVINCI_VPIF
 	select VIDEO_ADV7343 if MEDIA_SUBDRV_AUTOSELECT
 	select VIDEO_THS7303 if MEDIA_SUBDRV_AUTOSELECT
 	help
 	  Enables Davinci VPIF module used for display devices.
-	  This module is common for following DM6467/DA850/OMAPL138
-	  based display devices.
+	  This module is used for display on TI DM6467/DA850/OMAPL138
+	  SoCs.
 
-	  To compile this driver as a module, choose M here: the
-	  module will be called vpif_display.
+	  To compile this driver as a module, choose M here. There will
+	  be two modules called vpif.ko and vpif_display.ko
 
 config VIDEO_DAVINCI_VPIF_CAPTURE
-	tristate "DM646x/DA850/OMAPL138 EVM Video Capture"
-	depends on VIDEO_DEV && (MACH_DAVINCI_DM6467_EVM || MACH_DAVINCI_DA850_EVM)
+	tristate "TI DaVinci VPIF video capture driver"
+	depends on VIDEO_DEV && ARCH_DAVINCI
 	select VIDEOBUF2_DMA_CONTIG
-	select VIDEO_DAVINCI_VPIF
 	help
-	  Enables Davinci VPIF module used for captur devices.
-	  This module is common for following DM6467/DA850/OMAPL138
-	  based capture devices.
+	  Enables Davinci VPIF module used for capture devices.
+	  This module is used for capture on TI DM6467/DA850/OMAPL138
+	  SoCs.
 
-	  To compile this driver as a module, choose M here: the
-	  module will be called vpif_capture.
+	  To compile this driver as a module, choose M here. There will
+	  be two modules called vpif.ko and vpif_capture.ko
 
-config VIDEO_DAVINCI_VPIF
-	tristate "DaVinci VPIF Driver"
-	depends on VIDEO_DAVINCI_VPIF_DISPLAY || VIDEO_DAVINCI_VPIF_CAPTURE
-	help
-	  Support for DaVinci VPIF Driver.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called vpif.
-
-config VIDEO_VPSS_SYSTEM
-	tristate "VPSS System module driver"
-	depends on ARCH_DAVINCI
-	help
-	  Support for vpss system module for video driver
-
-config VIDEO_VPFE_CAPTURE
-	tristate "VPFE Video Capture Driver"
+config VIDEO_DM6446_CCDC
+	tristate "TI DM6446 CCDC 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
-	  common V4L2 module for following DMx/AMx SoCs from Texas
-	  Instruments:- DM6446, DM365, DM355 & AM3517/05.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called vpfe-capture.
-
-config VIDEO_DM6446_CCDC
-	tristate "DM6446 CCDC HW module"
-	depends on VIDEO_VPFE_CAPTURE
-	select VIDEO_VPSS_SYSTEM
-	default y
-	help
 	   Enables DaVinci CCD hw module. DaVinci CCDC hw interfaces
 	   with decoder modules such as TVP5146 over BT656 or
 	   sensor module such as MT9T001 over a raw interface. This
 	   module configures the interface and CCDC/ISIF to do
 	   video frame capture from slave decoders.
 
-	   To compile this driver as a module, choose M here: the
-	   module will be called vpfe.
+	   To compile this driver as a module, choose M here. There will
+	   be three modules called vpfe_capture.ko, vpss.ko and dm644x_ccdc.ko
 
 config VIDEO_DM355_CCDC
-	tristate "DM355 CCDC HW module"
-	depends on ARCH_DAVINCI_DM355 && VIDEO_VPFE_CAPTURE
-	select VIDEO_VPSS_SYSTEM
-	default y
+	tristate "TI DM355 CCDC video capture driver"
+	depends on VIDEO_V4L2 && ARCH_DAVINCI
+	select VIDEOBUF_DMA_CONTIG
 	help
 	   Enables DM355 CCD hw module. DM355 CCDC hw interfaces
 	   with decoder modules such as TVP5146 over BT656 or
@@ -81,31 +49,30 @@ config VIDEO_DM355_CCDC
 	   module configures the interface and CCDC/ISIF to do
 	   video frame capture from a slave decoders
 
-	   To compile this driver as a module, choose M here: the
-	   module will be called vpfe.
+	   To compile this driver as a module, choose M here. There will
+	   be three modules called vpfe_capture.ko, vpss.ko and dm355_ccdc.ko
 
-config VIDEO_ISIF
-	tristate "ISIF HW module"
-	depends on ARCH_DAVINCI_DM365 && VIDEO_VPFE_CAPTURE
-	select VIDEO_VPSS_SYSTEM
-	default y
+config VIDEO_DM365_ISIF
+	tristate "TI DM365 ISIF video capture driver"
+	depends on VIDEO_V4L2 && ARCH_DAVINCI
+	select VIDEOBUF_DMA_CONTIG
 	help
 	   Enables ISIF hw module. This is the hardware module for
-	   configuring ISIF in VPFE to capture Raw Bayer RGB data  from
+	   configuring ISIF in VPFE to capture Raw Bayer RGB data from
 	   a image sensor or YUV data from a YUV source.
 
-	   To compile this driver as a module, choose M here: the
-	   module will be called vpfe.
+	   To compile this driver as a module, choose M here. There will
+	   be three modules called vpfe_capture.ko, vpss.ko and isif.ko
 
 config VIDEO_DAVINCI_VPBE_DISPLAY
-	tristate "DM644X/DM365/DM355 VPBE HW module"
-	depends on ARCH_DAVINCI_DM644x || ARCH_DAVINCI_DM355 || ARCH_DAVINCI_DM365
-	select VIDEO_VPSS_SYSTEM
+	tristate "TI DaVinci VPBE V4L2-Display driver"
+	depends on ARCH_DAVINCI
 	select VIDEOBUF2_DMA_CONTIG
 	help
 	    Enables Davinci VPBE module used for display devices.
-	    This module is common for following DM644x/DM365/DM355
+	    This module is used for display on TI DM644x/DM365/DM355
 	    based display devices.
 
-	    To compile this driver as a module, choose M here: the
-	    module will be called vpbe.
+	    To compile this driver as a module, choose M here. There will
+	    be five modules created called vpss.ko, vpbe.ko, vpbe_osd.ko,
+	    vpbe_venc.ko and vpbe_display.ko
diff --git a/drivers/media/platform/davinci/Makefile b/drivers/media/platform/davinci/Makefile
index f40f5219ca5..d74d9eeb0e9 100644
--- a/drivers/media/platform/davinci/Makefile
+++ b/drivers/media/platform/davinci/Makefile
@@ -2,19 +2,14 @@
 # Makefile for the davinci video device drivers.
 #
 
-# VPIF
-obj-$(CONFIG_VIDEO_DAVINCI_VPIF) += vpif.o
-
 #VPIF Display driver
-obj-$(CONFIG_VIDEO_DAVINCI_VPIF_DISPLAY) += vpif_display.o
+obj-$(CONFIG_VIDEO_DAVINCI_VPIF_DISPLAY) += vpif.o vpif_display.o
 #VPIF Capture driver
-obj-$(CONFIG_VIDEO_DAVINCI_VPIF_CAPTURE) += vpif_capture.o
+obj-$(CONFIG_VIDEO_DAVINCI_VPIF_CAPTURE) += vpif.o vpif_capture.o
 
 # Capture: DM6446 and DM355
-obj-$(CONFIG_VIDEO_VPSS_SYSTEM) += vpss.o
-obj-$(CONFIG_VIDEO_VPFE_CAPTURE) += vpfe_capture.o
-obj-$(CONFIG_VIDEO_DM6446_CCDC) += dm644x_ccdc.o
-obj-$(CONFIG_VIDEO_DM355_CCDC) += dm355_ccdc.o
-obj-$(CONFIG_VIDEO_ISIF) += isif.o
-obj-$(CONFIG_VIDEO_DAVINCI_VPBE_DISPLAY) += vpbe.o vpbe_osd.o \
+obj-$(CONFIG_VIDEO_DM6446_CCDC) += vpfe_capture.o vpss.o dm644x_ccdc.o
+obj-$(CONFIG_VIDEO_DM355_CCDC) += vpfe_capture.o vpss.o dm355_ccdc.o
+obj-$(CONFIG_VIDEO_DM365_ISIF) += vpfe_capture.o vpss.o isif.o
+obj-$(CONFIG_VIDEO_DAVINCI_VPBE_DISPLAY) += vpss.o vpbe.o vpbe_osd.o \
 	vpbe_venc.o vpbe_display.o
diff --git a/drivers/media/platform/davinci/dm355_ccdc.c b/drivers/media/platform/davinci/dm355_ccdc.c
index 4277e4ad810..05f8fb7f7b7 100644
--- a/drivers/media/platform/davinci/dm355_ccdc.c
+++ b/drivers/media/platform/davinci/dm355_ccdc.c
@@ -37,7 +37,6 @@
 #include <linux/platform_device.h>
 #include <linux/uaccess.h>
 #include <linux/videodev2.h>
-#include <linux/clk.h>
 #include <linux/err.h>
 #include <linux/module.h>
 
@@ -59,10 +58,6 @@ static struct ccdc_oper_config {
 	struct ccdc_params_raw bayer;
 	/* YCbCr configuration */
 	struct ccdc_params_ycbcr ycbcr;
-	/* Master clock */
-	struct clk *mclk;
-	/* slave clock */
-	struct clk *sclk;
 	/* ccdc base address */
 	void __iomem *base_addr;
 } ccdc_cfg = {
@@ -85,7 +80,7 @@ static struct ccdc_oper_config {
 			.mfilt1 = CCDC_NO_MEDIAN_FILTER1,
 			.mfilt2 = CCDC_NO_MEDIAN_FILTER2,
 			.alaw = {
-				.gama_wd = 2,
+				.gamma_wd = 2,
 			},
 			.blk_clamp = {
 				.sample_pixel = 1,
@@ -303,8 +298,8 @@ static int validate_ccdc_param(struct ccdc_config_params_raw *ccdcparam)
 	}
 
 	if (ccdcparam->alaw.enable) {
-		if (ccdcparam->alaw.gama_wd < CCDC_GAMMA_BITS_13_4 ||
-		    ccdcparam->alaw.gama_wd > CCDC_GAMMA_BITS_09_0) {
+		if (ccdcparam->alaw.gamma_wd < CCDC_GAMMA_BITS_13_4 ||
+		    ccdcparam->alaw.gamma_wd > CCDC_GAMMA_BITS_09_0) {
 			dev_dbg(ccdc_cfg.dev, "Invalid value of ALAW\n");
 			return -EINVAL;
 		}
@@ -680,8 +675,8 @@ static int ccdc_config_raw(void)
 	/* Enable and configure aLaw register if needed */
 	if (config_params->alaw.enable) {
 		val |= (CCDC_ALAW_ENABLE |
-			((config_params->alaw.gama_wd &
-			CCDC_ALAW_GAMA_WD_MASK) <<
+			((config_params->alaw.gamma_wd &
+			CCDC_ALAW_GAMMA_WD_MASK) <<
 			CCDC_GAMMAWD_INPUT_SHIFT));
 	}
 
@@ -997,32 +992,10 @@ static int dm355_ccdc_probe(struct platform_device *pdev)
 		goto fail_nomem;
 	}
 
-	/* Get and enable Master clock */
-	ccdc_cfg.mclk = clk_get(&pdev->dev, "master");
-	if (IS_ERR(ccdc_cfg.mclk)) {
-		status = PTR_ERR(ccdc_cfg.mclk);
-		goto fail_nomap;
-	}
-	if (clk_prepare_enable(ccdc_cfg.mclk)) {
-		status = -ENODEV;
-		goto fail_mclk;
-	}
-
-	/* Get and enable Slave clock */
-	ccdc_cfg.sclk = clk_get(&pdev->dev, "slave");
-	if (IS_ERR(ccdc_cfg.sclk)) {
-		status = PTR_ERR(ccdc_cfg.sclk);
-		goto fail_mclk;
-	}
-	if (clk_prepare_enable(ccdc_cfg.sclk)) {
-		status = -ENODEV;
-		goto fail_sclk;
-	}
-
 	/* Platform data holds setup_pinmux function ptr */
 	if (NULL == pdev->dev.platform_data) {
 		status = -ENODEV;
-		goto fail_sclk;
+		goto fail_nomap;
 	}
 	setup_pinmux = pdev->dev.platform_data;
 	/*
@@ -1033,12 +1006,6 @@ static int dm355_ccdc_probe(struct platform_device *pdev)
 	ccdc_cfg.dev = &pdev->dev;
 	printk(KERN_NOTICE "%s is registered with vpfe.\n", ccdc_hw_dev.name);
 	return 0;
-fail_sclk:
-	clk_disable_unprepare(ccdc_cfg.sclk);
-	clk_put(ccdc_cfg.sclk);
-fail_mclk:
-	clk_disable_unprepare(ccdc_cfg.mclk);
-	clk_put(ccdc_cfg.mclk);
 fail_nomap:
 	iounmap(ccdc_cfg.base_addr);
 fail_nomem:
@@ -1052,10 +1019,6 @@ static int dm355_ccdc_remove(struct platform_device *pdev)
 {
 	struct resource	*res;
 
-	clk_disable_unprepare(ccdc_cfg.sclk);
-	clk_disable_unprepare(ccdc_cfg.mclk);
-	clk_put(ccdc_cfg.mclk);
-	clk_put(ccdc_cfg.sclk);
 	iounmap(ccdc_cfg.base_addr);
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (res)
diff --git a/drivers/media/platform/davinci/dm355_ccdc_regs.h b/drivers/media/platform/davinci/dm355_ccdc_regs.h
index d6d2ef0533b..2e1946e0b99 100644
--- a/drivers/media/platform/davinci/dm355_ccdc_regs.h
+++ b/drivers/media/platform/davinci/dm355_ccdc_regs.h
@@ -153,7 +153,7 @@
 #define CCDC_VDHDEN_ENABLE			(1 << 16)
 #define CCDC_LPF_ENABLE				(1 << 14)
 #define CCDC_ALAW_ENABLE			1
-#define CCDC_ALAW_GAMA_WD_MASK			7
+#define CCDC_ALAW_GAMMA_WD_MASK			7
 #define CCDC_REC656IF_BT656_EN			3
 
 #define CCDC_FMTCFG_FMTMODE_MASK 		3
diff --git a/drivers/media/platform/davinci/dm644x_ccdc.c b/drivers/media/platform/davinci/dm644x_ccdc.c
index 318e8051299..30fa08405d6 100644
--- a/drivers/media/platform/davinci/dm644x_ccdc.c
+++ b/drivers/media/platform/davinci/dm644x_ccdc.c
@@ -38,7 +38,6 @@
 #include <linux/uaccess.h>
 #include <linux/videodev2.h>
 #include <linux/gfp.h>
-#include <linux/clk.h>
 #include <linux/err.h>
 #include <linux/module.h>
 
@@ -60,10 +59,6 @@ static struct ccdc_oper_config {
 	struct ccdc_params_raw bayer;
 	/* YCbCr configuration */
 	struct ccdc_params_ycbcr ycbcr;
-	/* Master clock */
-	struct clk *mclk;
-	/* slave clock */
-	struct clk *sclk;
 	/* ccdc base address */
 	void __iomem *base_addr;
 } ccdc_cfg = {
@@ -228,9 +223,12 @@ static void ccdc_readregs(void)
 static int validate_ccdc_param(struct ccdc_config_params_raw *ccdcparam)
 {
 	if (ccdcparam->alaw.enable) {
-		if ((ccdcparam->alaw.gama_wd > CCDC_GAMMA_BITS_09_0) ||
-		    (ccdcparam->alaw.gama_wd < CCDC_GAMMA_BITS_15_6) ||
-		    (ccdcparam->alaw.gama_wd < ccdcparam->data_sz)) {
+		u8 max_gamma = ccdc_gamma_width_max_bit(ccdcparam->alaw.gamma_wd);
+		u8 max_data = ccdc_data_size_max_bit(ccdcparam->data_sz);
+
+		if ((ccdcparam->alaw.gamma_wd > CCDC_GAMMA_BITS_09_0) ||
+		    (ccdcparam->alaw.gamma_wd < CCDC_GAMMA_BITS_15_6) ||
+		    (max_gamma > max_data)) {
 			dev_dbg(ccdc_cfg.dev, "\nInvalid data line select");
 			return -1;
 		}
@@ -560,8 +558,8 @@ void ccdc_config_raw(void)
 
 	/* Enable and configure aLaw register if needed */
 	if (config_params->alaw.enable) {
-		val = ((config_params->alaw.gama_wd &
-		      CCDC_ALAW_GAMA_WD_MASK) | CCDC_ALAW_ENABLE);
+		val = ((config_params->alaw.gamma_wd &
+		      CCDC_ALAW_GAMMA_WD_MASK) | CCDC_ALAW_ENABLE);
 		regw(val, CCDC_ALAW);
 		dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to ALAW...\n", val);
 	}
@@ -988,38 +986,9 @@ static int dm644x_ccdc_probe(struct platform_device *pdev)
 		goto fail_nomem;
 	}
 
-	/* Get and enable Master clock */
-	ccdc_cfg.mclk = clk_get(&pdev->dev, "master");
-	if (IS_ERR(ccdc_cfg.mclk)) {
-		status = PTR_ERR(ccdc_cfg.mclk);
-		goto fail_nomap;
-	}
-	if (clk_prepare_enable(ccdc_cfg.mclk)) {
-		status = -ENODEV;
-		goto fail_mclk;
-	}
-
-	/* Get and enable Slave clock */
-	ccdc_cfg.sclk = clk_get(&pdev->dev, "slave");
-	if (IS_ERR(ccdc_cfg.sclk)) {
-		status = PTR_ERR(ccdc_cfg.sclk);
-		goto fail_mclk;
-	}
-	if (clk_prepare_enable(ccdc_cfg.sclk)) {
-		status = -ENODEV;
-		goto fail_sclk;
-	}
 	ccdc_cfg.dev = &pdev->dev;
 	printk(KERN_NOTICE "%s is registered with vpfe.\n", ccdc_hw_dev.name);
 	return 0;
-fail_sclk:
-	clk_disable_unprepare(ccdc_cfg.sclk);
-	clk_put(ccdc_cfg.sclk);
-fail_mclk:
-	clk_disable_unprepare(ccdc_cfg.mclk);
-	clk_put(ccdc_cfg.mclk);
-fail_nomap:
-	iounmap(ccdc_cfg.base_addr);
 fail_nomem:
 	release_mem_region(res->start, resource_size(res));
 fail_nores:
@@ -1031,10 +1000,6 @@ static int dm644x_ccdc_remove(struct platform_device *pdev)
 {
 	struct resource	*res;
 
-	clk_disable_unprepare(ccdc_cfg.mclk);
-	clk_disable_unprepare(ccdc_cfg.sclk);
-	clk_put(ccdc_cfg.mclk);
-	clk_put(ccdc_cfg.sclk);
 	iounmap(ccdc_cfg.base_addr);
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (res)
@@ -1049,18 +1014,12 @@ static int dm644x_ccdc_suspend(struct device *dev)
 	ccdc_save_context();
 	/* Disable CCDC */
 	ccdc_enable(0);
-	/* Disable both master and slave clock */
-	clk_disable_unprepare(ccdc_cfg.mclk);
-	clk_disable_unprepare(ccdc_cfg.sclk);
 
 	return 0;
 }
 
 static int dm644x_ccdc_resume(struct device *dev)
 {
-	/* Enable both master and slave clock */
-	clk_prepare_enable(ccdc_cfg.mclk);
-	clk_prepare_enable(ccdc_cfg.sclk);
 	/* Restore CCDC context */
 	ccdc_restore_context();
 
diff --git a/drivers/media/platform/davinci/dm644x_ccdc_regs.h b/drivers/media/platform/davinci/dm644x_ccdc_regs.h
index 90370e414e2..2b0aca5383f 100644
--- a/drivers/media/platform/davinci/dm644x_ccdc_regs.h
+++ b/drivers/media/platform/davinci/dm644x_ccdc_regs.h
@@ -84,7 +84,7 @@
 #define CCDC_VDHDEN_ENABLE			(1 << 16)
 #define CCDC_LPF_ENABLE				(1 << 14)
 #define CCDC_ALAW_ENABLE			(1 << 3)
-#define CCDC_ALAW_GAMA_WD_MASK			7
+#define CCDC_ALAW_GAMMA_WD_MASK			7
 #define CCDC_BLK_CLAMP_ENABLE			(1 << 31)
 #define CCDC_BLK_SGAIN_MASK			0x1F
 #define CCDC_BLK_ST_PXL_MASK			0x7FFF
diff --git a/drivers/media/platform/davinci/isif.c b/drivers/media/platform/davinci/isif.c
index 5050f9265f4..3332cca632e 100644
--- a/drivers/media/platform/davinci/isif.c
+++ b/drivers/media/platform/davinci/isif.c
@@ -32,7 +32,6 @@
 #include <linux/uaccess.h>
 #include <linux/io.h>
 #include <linux/videodev2.h>
-#include <linux/clk.h>
 #include <linux/err.h>
 #include <linux/module.h>
 
@@ -88,8 +87,6 @@ static struct isif_oper_config {
 	struct isif_ycbcr_config ycbcr;
 	struct isif_params_raw bayer;
 	enum isif_data_pack data_pack;
-	/* Master clock */
-	struct clk *mclk;
 	/* ISIF base address */
 	void __iomem *base_addr;
 	/* ISIF Linear Table 0 */
@@ -604,7 +601,7 @@ static int isif_config_raw(void)
 	if (module_params->compress.alg == ISIF_ALAW)
 		val |= ISIF_ALAW_ENABLE;
 
-	val |= (params->data_msb << ISIF_ALAW_GAMA_WD_SHIFT);
+	val |= (params->data_msb << ISIF_ALAW_GAMMA_WD_SHIFT);
 	regw(val, CGAMMAWD);
 
 	/* Configure DPCM compression settings */
@@ -1039,6 +1036,10 @@ static int isif_probe(struct platform_device *pdev)
 	void *__iomem addr;
 	int status = 0, i;
 
+	/* Platform data holds setup_pinmux function ptr */
+	if (!pdev->dev.platform_data)
+		return -ENODEV;
+
 	/*
 	 * first try to register with vpfe. If not correct platform, then we
 	 * don't have to iomap
@@ -1047,22 +1048,6 @@ static int isif_probe(struct platform_device *pdev)
 	if (status < 0)
 		return status;
 
-	/* Get and enable Master clock */
-	isif_cfg.mclk = clk_get(&pdev->dev, "master");
-	if (IS_ERR(isif_cfg.mclk)) {
-		status = PTR_ERR(isif_cfg.mclk);
-		goto fail_mclk;
-	}
-	if (clk_prepare_enable(isif_cfg.mclk)) {
-		status = -ENODEV;
-		goto fail_mclk;
-	}
-
-	/* Platform data holds setup_pinmux function ptr */
-	if (NULL == pdev->dev.platform_data) {
-		status = -ENODEV;
-		goto fail_mclk;
-	}
 	setup_pinmux = pdev->dev.platform_data;
 	/*
 	 * setup Mux configuration for ccdc which may be different for
@@ -1124,9 +1109,6 @@ fail_nobase_res:
 		release_mem_region(res->start, resource_size(res));
 		i--;
 	}
-fail_mclk:
-	clk_disable_unprepare(isif_cfg.mclk);
-	clk_put(isif_cfg.mclk);
 	vpfe_unregister_ccdc_device(&isif_hw_dev);
 	return status;
 }
@@ -1146,8 +1128,6 @@ static int isif_remove(struct platform_device *pdev)
 		i++;
 	}
 	vpfe_unregister_ccdc_device(&isif_hw_dev);
-	clk_disable_unprepare(isif_cfg.mclk);
-	clk_put(isif_cfg.mclk);
 	return 0;
 }
 
diff --git a/drivers/media/platform/davinci/isif_regs.h b/drivers/media/platform/davinci/isif_regs.h
index aa69a463c12..3993aece821 100644
--- a/drivers/media/platform/davinci/isif_regs.h
+++ b/drivers/media/platform/davinci/isif_regs.h
@@ -203,8 +203,8 @@
 #define ISIF_LPF_MASK				1
 
 /* GAMMAWD registers */
-#define ISIF_ALAW_GAMA_WD_MASK			0xF
-#define ISIF_ALAW_GAMA_WD_SHIFT			1
+#define ISIF_ALAW_GAMMA_WD_MASK			0xF
+#define ISIF_ALAW_GAMMA_WD_SHIFT		1
 #define ISIF_ALAW_ENABLE			1
 #define ISIF_GAMMAWD_CFA_SHIFT			5
 
diff --git a/drivers/media/platform/davinci/vpbe.c b/drivers/media/platform/davinci/vpbe.c
index 4ca0f9a2ad8..33b9660b7f7 100644
--- a/drivers/media/platform/davinci/vpbe.c
+++ b/drivers/media/platform/davinci/vpbe.c
@@ -344,7 +344,7 @@ static int vpbe_s_dv_timings(struct vpbe_device *vpbe_dev,
 		return -EINVAL;
 
 	for (i = 0; i < output->num_modes; i++) {
-		if (output->modes[i].timings_type == VPBE_ENC_CUSTOM_TIMINGS &&
+		if (output->modes[i].timings_type == VPBE_ENC_DV_TIMINGS &&
 		    !memcmp(&output->modes[i].dv_timings,
 				dv_timings, sizeof(*dv_timings)))
 			break;
@@ -385,7 +385,7 @@ static int vpbe_g_dv_timings(struct vpbe_device *vpbe_dev,
 		     struct v4l2_dv_timings *dv_timings)
 {
 	if (vpbe_dev->current_timings.timings_type &
-	  VPBE_ENC_CUSTOM_TIMINGS) {
+	  VPBE_ENC_DV_TIMINGS) {
 		*dv_timings = vpbe_dev->current_timings.dv_timings;
 		return 0;
 	}
@@ -412,7 +412,7 @@ static int vpbe_enum_dv_timings(struct vpbe_device *vpbe_dev,
 		return -EINVAL;
 
 	for (i = 0; i < output->num_modes; i++) {
-		if (output->modes[i].timings_type == VPBE_ENC_CUSTOM_TIMINGS) {
+		if (output->modes[i].timings_type == VPBE_ENC_DV_TIMINGS) {
 			if (j == timings->index)
 				break;
 			j++;
@@ -431,7 +431,7 @@ static int vpbe_enum_dv_timings(struct vpbe_device *vpbe_dev,
  * Sets the standard if supported by the current encoder. Return the status.
  * 0 - success & -EINVAL on error
  */
-static int vpbe_s_std(struct vpbe_device *vpbe_dev, v4l2_std_id *std_id)
+static int vpbe_s_std(struct vpbe_device *vpbe_dev, v4l2_std_id std_id)
 {
 	struct vpbe_config *cfg = vpbe_dev->cfg;
 	int out_index = vpbe_dev->current_out_index;
@@ -442,14 +442,14 @@ static int vpbe_s_std(struct vpbe_device *vpbe_dev, v4l2_std_id *std_id)
 		V4L2_OUT_CAP_STD))
 		return -EINVAL;
 
-	ret = vpbe_get_std_info(vpbe_dev, *std_id);
+	ret = vpbe_get_std_info(vpbe_dev, std_id);
 	if (ret)
 		return ret;
 
 	mutex_lock(&vpbe_dev->lock);
 
 	ret = v4l2_subdev_call(vpbe_dev->encoders[sd_index], video,
-			       s_std_output, *std_id);
+			       s_std_output, std_id);
 	/* set the lcd controller output for the given mode */
 	if (!ret) {
 		struct osd_state *osd_device = vpbe_dev->osd_device;
@@ -513,9 +513,9 @@ static int vpbe_set_mode(struct vpbe_device *vpbe_dev,
 			 */
 			if (preset_mode->timings_type & VPBE_ENC_STD)
 				return vpbe_s_std(vpbe_dev,
-						 &preset_mode->std_id);
+						 preset_mode->std_id);
 			if (preset_mode->timings_type &
-						VPBE_ENC_CUSTOM_TIMINGS) {
+						VPBE_ENC_DV_TIMINGS) {
 				dv_timings =
 					preset_mode->dv_timings;
 				return vpbe_s_dv_timings(vpbe_dev, &dv_timings);
diff --git a/drivers/media/platform/davinci/vpbe_display.c b/drivers/media/platform/davinci/vpbe_display.c
index 5e6b0cab514..1802f11e939 100644
--- a/drivers/media/platform/davinci/vpbe_display.c
+++ b/drivers/media/platform/davinci/vpbe_display.c
@@ -983,7 +983,7 @@ static int vpbe_display_try_fmt(struct file *file, void *priv,
  * 0 - success & -EINVAL on error
  */
 static int vpbe_display_s_std(struct file *file, void *priv,
-				v4l2_std_id *std_id)
+				v4l2_std_id std_id)
 {
 	struct vpbe_fh *fh = priv;
 	struct vpbe_layer *layer = fh->layer;
@@ -1176,10 +1176,6 @@ vpbe_display_s_dv_timings(struct file *file, void *priv,
 			"Failed to set the dv timings info\n");
 		return -EINVAL;
 	}
-	/* set the current norm to zero to be consistent. If STD is used
-	 * v4l2 layer will set the norm properly on successful s_std call
-	 */
-	layer->video_dev.current_norm = 0;
 
 	return 0;
 }
@@ -1202,7 +1198,7 @@ vpbe_display_g_dv_timings(struct file *file, void *priv,
 	/* Get the given standard in the encoder */
 
 	if (vpbe_dev->current_timings.timings_type &
-				VPBE_ENC_CUSTOM_TIMINGS) {
+				VPBE_ENC_DV_TIMINGS) {
 		*dv_timings = vpbe_dev->current_timings.dv_timings;
 	} else {
 		return -EINVAL;
@@ -1404,6 +1400,7 @@ static int vpbe_display_reqbufs(struct file *file, void *priv,
 	q->ops = &video_qops;
 	q->mem_ops = &vb2_dma_contig_memops;
 	q->buf_struct_size = sizeof(struct vpbe_disp_buffer);
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 
 	ret = vb2_queue_init(q);
 	if (ret) {
@@ -1600,7 +1597,7 @@ static int vpbe_display_g_register(struct file *file, void *priv,
 }
 
 static int vpbe_display_s_register(struct file *file, void *priv,
-			struct v4l2_dbg_register *reg)
+			const struct v4l2_dbg_register *reg)
 {
 	return 0;
 }
@@ -1693,12 +1690,8 @@ static int init_vpbe_layer(int i, struct vpbe_display *disp_dev,
 	vbd->vfl_dir	= VFL_DIR_TX;
 
 	if (disp_dev->vpbe_dev->current_timings.timings_type &
-			VPBE_ENC_STD) {
+			VPBE_ENC_STD)
 		vbd->tvnorms = (V4L2_STD_525_60 | V4L2_STD_625_50);
-		vbd->current_norm =
-			disp_dev->vpbe_dev->current_timings.std_id;
-	} else
-		vbd->current_norm = 0;
 
 	snprintf(vbd->name, sizeof(vbd->name),
 			"DaVinci_VPBE Display_DRIVER_V%d.%d.%d",
diff --git a/drivers/media/platform/davinci/vpbe_osd.c b/drivers/media/platform/davinci/vpbe_osd.c
index 12ad17c52ef..396a51cbede 100644
--- a/drivers/media/platform/davinci/vpbe_osd.c
+++ b/drivers/media/platform/davinci/vpbe_osd.c
@@ -52,6 +52,9 @@ static struct platform_device_id vpbe_osd_devtype[] = {
 		.name = DM355_VPBE_OSD_SUBDEV_NAME,
 		.driver_data = VPBE_VERSION_3,
 	},
+	{
+		/* sentinel */
+	}
 };
 
 MODULE_DEVICE_TABLE(platform, vpbe_osd_devtype);
diff --git a/drivers/media/platform/davinci/vpbe_venc.c b/drivers/media/platform/davinci/vpbe_venc.c
index bdbebd59df9..87eef9be08e 100644
--- a/drivers/media/platform/davinci/vpbe_venc.c
+++ b/drivers/media/platform/davinci/vpbe_venc.c
@@ -51,6 +51,9 @@ static struct platform_device_id vpbe_venc_devtype[] = {
 		.name = DM355_VPBE_VENC_SUBDEV_NAME,
 		.driver_data = VPBE_VERSION_3,
 	},
+	{
+		/* sentinel */
+	}
 };
 
 MODULE_DEVICE_TABLE(platform, vpbe_venc_devtype);
@@ -199,6 +202,25 @@ static void venc_enabledigitaloutput(struct v4l2_subdev *sd, int benable)
 	}
 }
 
+static void
+venc_enable_vpss_clock(int venc_type,
+		       enum vpbe_enc_timings_type type,
+		       unsigned int pclock)
+{
+	if (venc_type == VPBE_VERSION_1)
+		return;
+
+	if (venc_type == VPBE_VERSION_2 && (type == VPBE_ENC_STD || (type ==
+	    VPBE_ENC_DV_TIMINGS && pclock <= 27000000))) {
+		vpss_enable_clock(VPSS_VENC_CLOCK_SEL, 1);
+		vpss_enable_clock(VPSS_VPBE_CLOCK, 1);
+		return;
+	}
+
+	if (venc_type == VPBE_VERSION_3 && type == VPBE_ENC_STD)
+		vpss_enable_clock(VPSS_VENC_CLOCK_SEL, 0);
+}
+
 #define VDAC_CONFIG_SD_V3	0x0E21A6B6
 #define VDAC_CONFIG_SD_V2	0x081141CF
 /*
@@ -217,6 +239,7 @@ static int venc_set_ntsc(struct v4l2_subdev *sd)
 	if (pdata->setup_clock(VPBE_ENC_STD, V4L2_STD_525_60) < 0)
 		return -EINVAL;
 
+	venc_enable_vpss_clock(venc->venc_type, VPBE_ENC_STD, V4L2_STD_525_60);
 	venc_enabledigitaloutput(sd, 0);
 
 	if (venc->venc_type == VPBE_VERSION_3) {
@@ -262,6 +285,7 @@ static int venc_set_pal(struct v4l2_subdev *sd)
 	if (venc->pdata->setup_clock(VPBE_ENC_STD, V4L2_STD_625_50) < 0)
 		return -EINVAL;
 
+	venc_enable_vpss_clock(venc->venc_type, VPBE_ENC_STD, V4L2_STD_625_50);
 	venc_enabledigitaloutput(sd, 0);
 
 	if (venc->venc_type == VPBE_VERSION_3) {
@@ -313,9 +337,10 @@ static int venc_set_480p59_94(struct v4l2_subdev *sd)
 		return -EINVAL;
 
 	/* Setup clock at VPSS & VENC for SD */
-	if (pdata->setup_clock(VPBE_ENC_CUSTOM_TIMINGS, 27000000) < 0)
+	if (pdata->setup_clock(VPBE_ENC_DV_TIMINGS, 27000000) < 0)
 		return -EINVAL;
 
+	venc_enable_vpss_clock(venc->venc_type, VPBE_ENC_DV_TIMINGS, 27000000);
 	venc_enabledigitaloutput(sd, 0);
 
 	if (venc->venc_type == VPBE_VERSION_2)
@@ -360,9 +385,10 @@ static int venc_set_576p50(struct v4l2_subdev *sd)
 	    venc->venc_type != VPBE_VERSION_2)
 		return -EINVAL;
 	/* Setup clock at VPSS & VENC for SD */
-	if (pdata->setup_clock(VPBE_ENC_CUSTOM_TIMINGS, 27000000) < 0)
+	if (pdata->setup_clock(VPBE_ENC_DV_TIMINGS, 27000000) < 0)
 		return -EINVAL;
 
+	venc_enable_vpss_clock(venc->venc_type, VPBE_ENC_DV_TIMINGS, 27000000);
 	venc_enabledigitaloutput(sd, 0);
 
 	if (venc->venc_type == VPBE_VERSION_2)
@@ -400,9 +426,10 @@ static int venc_set_720p60_internal(struct v4l2_subdev *sd)
 	struct venc_state *venc = to_state(sd);
 	struct venc_platform_data *pdata = venc->pdata;
 
-	if (pdata->setup_clock(VPBE_ENC_CUSTOM_TIMINGS, 74250000) < 0)
+	if (pdata->setup_clock(VPBE_ENC_DV_TIMINGS, 74250000) < 0)
 		return -EINVAL;
 
+	venc_enable_vpss_clock(venc->venc_type, VPBE_ENC_DV_TIMINGS, 74250000);
 	venc_enabledigitaloutput(sd, 0);
 
 	venc_write(sd, VENC_OSDCLK0, 0);
@@ -428,9 +455,10 @@ static int venc_set_1080i30_internal(struct v4l2_subdev *sd)
 	struct venc_state *venc = to_state(sd);
 	struct venc_platform_data *pdata = venc->pdata;
 
-	if (pdata->setup_clock(VPBE_ENC_CUSTOM_TIMINGS, 74250000) < 0)
+	if (pdata->setup_clock(VPBE_ENC_DV_TIMINGS, 74250000) < 0)
 		return -EINVAL;
 
+	venc_enable_vpss_clock(venc->venc_type, VPBE_ENC_DV_TIMINGS, 74250000);
 	venc_enabledigitaloutput(sd, 0);
 
 	venc_write(sd, VENC_OSDCLK0, 0);
diff --git a/drivers/media/platform/davinci/vpfe_capture.c b/drivers/media/platform/davinci/vpfe_capture.c
index 28d019da4c0..8c50d307486 100644
--- a/drivers/media/platform/davinci/vpfe_capture.c
+++ b/drivers/media/platform/davinci/vpfe_capture.c
@@ -376,7 +376,7 @@ static int vpfe_config_ccdc_image_format(struct vpfe_device *vpfe_dev)
  * values in ccdc
  */
 static int vpfe_config_image_format(struct vpfe_device *vpfe_dev,
-				    const v4l2_std_id *std_id)
+				    v4l2_std_id std_id)
 {
 	struct vpfe_subdev_info *sdinfo = vpfe_dev->current_subdev;
 	struct v4l2_mbus_framefmt mbus_fmt;
@@ -384,7 +384,7 @@ static int vpfe_config_image_format(struct vpfe_device *vpfe_dev,
 	int i, ret = 0;
 
 	for (i = 0; i < ARRAY_SIZE(vpfe_standards); i++) {
-		if (vpfe_standards[i].std_id & *std_id) {
+		if (vpfe_standards[i].std_id & std_id) {
 			vpfe_dev->std_info.active_pixels =
 					vpfe_standards[i].width;
 			vpfe_dev->std_info.active_lines =
@@ -461,7 +461,7 @@ static int vpfe_initialize_device(struct vpfe_device *vpfe_dev)
 
 	/* Configure the default format information */
 	ret = vpfe_config_image_format(vpfe_dev,
-				&vpfe_standards[vpfe_dev->std_index].std_id);
+				vpfe_standards[vpfe_dev->std_index].std_id);
 	if (ret)
 		return ret;
 
@@ -1107,6 +1107,7 @@ static int vpfe_g_input(struct file *file, void *priv, unsigned int *index)
 static int vpfe_s_input(struct file *file, void *priv, unsigned int index)
 {
 	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	struct v4l2_subdev *sd;
 	struct vpfe_subdev_info *sdinfo;
 	int subdev_index, inp_index;
 	struct vpfe_route *route;
@@ -1138,14 +1139,15 @@ static int vpfe_s_input(struct file *file, void *priv, unsigned int index)
 	}
 
 	sdinfo = &vpfe_dev->cfg->sub_devs[subdev_index];
+	sd = vpfe_dev->sd[subdev_index];
 	route = &sdinfo->routes[inp_index];
 	if (route && sdinfo->can_route) {
 		input = route->input;
 		output = route->output;
 	}
 
-	ret = v4l2_device_call_until_err(&vpfe_dev->v4l2_dev, sdinfo->grp_id,
-					 video, s_routing, input, output, 0);
+	if (sd)
+		ret = v4l2_subdev_call(sd, video, s_routing, input, output, 0);
 
 	if (ret) {
 		v4l2_err(&vpfe_dev->v4l2_dev,
@@ -1154,6 +1156,8 @@ static int vpfe_s_input(struct file *file, void *priv, unsigned int index)
 		goto unlock_out;
 	}
 	vpfe_dev->current_subdev = sdinfo;
+	if (sd)
+		vpfe_dev->v4l2_dev.ctrl_handler = sd->ctrl_handler;
 	vpfe_dev->current_input = index;
 	vpfe_dev->std_index = 0;
 
@@ -1164,7 +1168,7 @@ static int vpfe_s_input(struct file *file, void *priv, unsigned int index)
 
 	/* set the default image parameters in the device */
 	ret = vpfe_config_image_format(vpfe_dev,
-				&vpfe_standards[vpfe_dev->std_index].std_id);
+				vpfe_standards[vpfe_dev->std_index].std_id);
 unlock_out:
 	mutex_unlock(&vpfe_dev->lock);
 	return ret;
@@ -1189,7 +1193,7 @@ static int vpfe_querystd(struct file *file, void *priv, v4l2_std_id *std_id)
 	return ret;
 }
 
-static int vpfe_s_std(struct file *file, void *priv, v4l2_std_id *std_id)
+static int vpfe_s_std(struct file *file, void *priv, v4l2_std_id std_id)
 {
 	struct vpfe_device *vpfe_dev = video_drvdata(file);
 	struct vpfe_subdev_info *sdinfo;
@@ -1211,7 +1215,7 @@ static int vpfe_s_std(struct file *file, void *priv, v4l2_std_id *std_id)
 	}
 
 	ret = v4l2_device_call_until_err(&vpfe_dev->v4l2_dev, sdinfo->grp_id,
-					 core, s_std, *std_id);
+					 core, s_std, std_id);
 	if (ret < 0) {
 		v4l2_err(&vpfe_dev->v4l2_dev, "Failed to set standard\n");
 		goto unlock_out;
@@ -1439,41 +1443,6 @@ static int vpfe_dqbuf(struct file *file, void *priv,
 				      buf, file->f_flags & O_NONBLOCK);
 }
 
-static int vpfe_queryctrl(struct file *file, void *priv,
-		struct v4l2_queryctrl *qctrl)
-{
-	struct vpfe_device *vpfe_dev = video_drvdata(file);
-	struct vpfe_subdev_info *sdinfo;
-
-	sdinfo = vpfe_dev->current_subdev;
-
-	return v4l2_device_call_until_err(&vpfe_dev->v4l2_dev, sdinfo->grp_id,
-					 core, queryctrl, qctrl);
-
-}
-
-static int vpfe_g_ctrl(struct file *file, void *priv, struct v4l2_control *ctrl)
-{
-	struct vpfe_device *vpfe_dev = video_drvdata(file);
-	struct vpfe_subdev_info *sdinfo;
-
-	sdinfo = vpfe_dev->current_subdev;
-
-	return v4l2_device_call_until_err(&vpfe_dev->v4l2_dev, sdinfo->grp_id,
-					 core, g_ctrl, ctrl);
-}
-
-static int vpfe_s_ctrl(struct file *file, void *priv, struct v4l2_control *ctrl)
-{
-	struct vpfe_device *vpfe_dev = video_drvdata(file);
-	struct vpfe_subdev_info *sdinfo;
-
-	sdinfo = vpfe_dev->current_subdev;
-
-	return v4l2_device_call_until_err(&vpfe_dev->v4l2_dev, sdinfo->grp_id,
-					 core, s_ctrl, ctrl);
-}
-
 /*
  * vpfe_calculate_offsets : This function calculates buffers offset
  * for top and bottom field
@@ -1717,7 +1686,7 @@ unlock_out:
 
 
 static long vpfe_param_handler(struct file *file, void *priv,
-		bool valid_prio, int cmd, void *param)
+		bool valid_prio, unsigned int cmd, void *param)
 {
 	struct vpfe_device *vpfe_dev = video_drvdata(file);
 	int ret = 0;
@@ -1781,9 +1750,6 @@ static const struct v4l2_ioctl_ops vpfe_ioctl_ops = {
 	.vidioc_querystd	 = vpfe_querystd,
 	.vidioc_s_std		 = vpfe_s_std,
 	.vidioc_g_std		 = vpfe_g_std,
-	.vidioc_queryctrl	 = vpfe_queryctrl,
-	.vidioc_g_ctrl		 = vpfe_g_ctrl,
-	.vidioc_s_ctrl		 = vpfe_s_ctrl,
 	.vidioc_reqbufs		 = vpfe_reqbufs,
 	.vidioc_querybuf	 = vpfe_querybuf,
 	.vidioc_qbuf		 = vpfe_qbuf,
@@ -1918,7 +1884,6 @@ static int vpfe_probe(struct platform_device *pdev)
 	vfd->fops		= &vpfe_fops;
 	vfd->ioctl_ops		= &vpfe_ioctl_ops;
 	vfd->tvnorms		= 0;
-	vfd->current_norm	= V4L2_STD_PAL;
 	vfd->v4l2_dev 		= &vpfe_dev->v4l2_dev;
 	snprintf(vfd->name, sizeof(vfd->name),
 		 "%s_V%d.%d.%d",
@@ -2007,6 +1972,7 @@ static int vpfe_probe(struct platform_device *pdev)
 
 	/* set first sub device as current one */
 	vpfe_dev->current_subdev = &vpfe_cfg->sub_devs[0];
+	vpfe_dev->v4l2_dev.ctrl_handler = vpfe_dev->sd[0]->ctrl_handler;
 
 	/* We have at least one sub device to work with */
 	mutex_unlock(&ccdc_lock);
diff --git a/drivers/media/platform/davinci/vpif.c b/drivers/media/platform/davinci/vpif.c
index 28638a86f12..ea82a8bd280 100644
--- a/drivers/media/platform/davinci/vpif.c
+++ b/drivers/media/platform/davinci/vpif.c
@@ -23,8 +23,8 @@
 #include <linux/spinlock.h>
 #include <linux/kernel.h>
 #include <linux/io.h>
-#include <linux/clk.h>
 #include <linux/err.h>
+#include <linux/pm_runtime.h>
 #include <linux/v4l2-dv-timings.h>
 
 #include <mach/hardware.h>
@@ -44,13 +44,13 @@ static struct resource	*res;
 spinlock_t vpif_lock;
 
 void __iomem *vpif_base;
-struct clk *vpif_clk;
+EXPORT_SYMBOL_GPL(vpif_base);
 
 /**
- * ch_params: video standard configuration parameters for vpif
+ * vpif_ch_params: video standard configuration parameters for vpif
  * The table must include all presets from supported subdevices.
  */
-const struct vpif_channel_config_params ch_params[] = {
+const struct vpif_channel_config_params vpif_ch_params[] = {
 	/* HDTV formats */
 	{
 		.name = "480p59_94",
@@ -220,8 +220,10 @@ const struct vpif_channel_config_params ch_params[] = {
 		.stdid = V4L2_STD_625_50,
 	},
 };
+EXPORT_SYMBOL_GPL(vpif_ch_params);
 
-const unsigned int vpif_ch_params_count = ARRAY_SIZE(ch_params);
+const unsigned int vpif_ch_params_count = ARRAY_SIZE(vpif_ch_params);
+EXPORT_SYMBOL_GPL(vpif_ch_params_count);
 
 static inline void vpif_wr_bit(u32 reg, u32 bit, u32 val)
 {
@@ -439,19 +441,13 @@ static int vpif_probe(struct platform_device *pdev)
 		goto fail;
 	}
 
-	vpif_clk = clk_get(&pdev->dev, "vpif");
-	if (IS_ERR(vpif_clk)) {
-		status = PTR_ERR(vpif_clk);
-		goto clk_fail;
-	}
-	clk_prepare_enable(vpif_clk);
+	pm_runtime_enable(&pdev->dev);
+	pm_runtime_get(&pdev->dev);
 
 	spin_lock_init(&vpif_lock);
 	dev_info(&pdev->dev, "vpif probe success\n");
 	return 0;
 
-clk_fail:
-	iounmap(vpif_base);
 fail:
 	release_mem_region(res->start, res_len);
 	return status;
@@ -459,11 +455,7 @@ fail:
 
 static int vpif_remove(struct platform_device *pdev)
 {
-	if (vpif_clk) {
-		clk_disable_unprepare(vpif_clk);
-		clk_put(vpif_clk);
-	}
-
+	pm_runtime_disable(&pdev->dev);
 	iounmap(vpif_base);
 	release_mem_region(res->start, res_len);
 	return 0;
@@ -472,13 +464,13 @@ static int vpif_remove(struct platform_device *pdev)
 #ifdef CONFIG_PM
 static int vpif_suspend(struct device *dev)
 {
-	clk_disable_unprepare(vpif_clk);
+	pm_runtime_put(dev);
 	return 0;
 }
 
 static int vpif_resume(struct device *dev)
 {
-	clk_prepare_enable(vpif_clk);
+	pm_runtime_get(dev);
 	return 0;
 }
 
diff --git a/drivers/media/platform/davinci/vpif.h b/drivers/media/platform/davinci/vpif.h
index a1ab6a0f4e9..9956e678869 100644
--- a/drivers/media/platform/davinci/vpif.h
+++ b/drivers/media/platform/davinci/vpif.h
@@ -638,7 +638,7 @@ struct vpif_channel_config_params {
 };
 
 extern const unsigned int vpif_ch_params_count;
-extern const struct vpif_channel_config_params ch_params[];
+extern const struct vpif_channel_config_params vpif_ch_params[];
 
 struct vpif_video_params;
 struct vpif_params;
diff --git a/drivers/media/platform/davinci/vpif_capture.c b/drivers/media/platform/davinci/vpif_capture.c
index 5892d2bc8ee..5f98df1fc8a 100644
--- a/drivers/media/platform/davinci/vpif_capture.c
+++ b/drivers/media/platform/davinci/vpif_capture.c
@@ -563,7 +563,7 @@ static int vpif_update_std_info(struct channel_obj *ch)
 	vpif_dbg(2, debug, "vpif_update_std_info\n");
 
 	for (index = 0; index < vpif_ch_params_count; index++) {
-		config = &ch_params[index];
+		config = &vpif_ch_params[index];
 		if (config->hd_sd == 0) {
 			vpif_dbg(2, debug, "SD format\n");
 			if (config->stdid & vid_ch->stdid) {
@@ -1035,6 +1035,7 @@ static int vpif_reqbufs(struct file *file, void *priv,
 	q->ops = &video_qops;
 	q->mem_ops = &vb2_dma_contig_memops;
 	q->buf_struct_size = sizeof(struct vpif_cap_buffer);
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 
 	ret = vb2_queue_init(q);
 	if (ret) {
@@ -1394,7 +1395,7 @@ static int vpif_g_std(struct file *file, void *priv, v4l2_std_id *std)
  * @priv: file handle
  * @std_id: ptr to std id
  */
-static int vpif_s_std(struct file *file, void *priv, v4l2_std_id *std_id)
+static int vpif_s_std(struct file *file, void *priv, v4l2_std_id std_id)
 {
 	struct vpif_fh *fh = priv;
 	struct channel_obj *ch = fh->channel;
@@ -1423,7 +1424,7 @@ static int vpif_s_std(struct file *file, void *priv, v4l2_std_id *std_id)
 	fh->initialized = 1;
 
 	/* Call encoder subdevice function to set the standard */
-	ch->video.stdid = *std_id;
+	ch->video.stdid = std_id;
 	memset(&ch->video.dv_timings, 0, sizeof(ch->video.dv_timings));
 
 	/* Get the information about the standard */
@@ -1436,7 +1437,7 @@ static int vpif_s_std(struct file *file, void *priv, v4l2_std_id *std_id)
 	vpif_config_format(ch);
 
 	/* set standard in the sub device */
-	ret = v4l2_subdev_call(ch->sd, core, s_std, *std_id);
+	ret = v4l2_subdev_call(ch->sd, core, s_std, std_id);
 	if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV) {
 		vpif_dbg(1, debug, "Failed to set standard for sub devices\n");
 		return ret;
@@ -1923,7 +1924,8 @@ static int vpif_dbg_g_register(struct file *file, void *priv,
  * Returns zero or -EINVAL if write operations fails.
  */
 static int vpif_dbg_s_register(struct file *file, void *priv,
-		struct v4l2_dbg_register *reg){
+		const struct v4l2_dbg_register *reg)
+{
 	struct vpif_fh *fh = priv;
 	struct channel_obj *ch = fh->channel;
 
diff --git a/drivers/media/platform/davinci/vpif_display.c b/drivers/media/platform/davinci/vpif_display.c
index dd249c96126..1b3fb5ca2ad 100644
--- a/drivers/media/platform/davinci/vpif_display.c
+++ b/drivers/media/platform/davinci/vpif_display.c
@@ -511,7 +511,7 @@ static int vpif_update_std_info(struct channel_obj *ch)
 	int i;
 
 	for (i = 0; i < vpif_ch_params_count; i++) {
-		config = &ch_params[i];
+		config = &vpif_ch_params[i];
 		if (config->hd_sd == 0) {
 			vpif_dbg(2, debug, "SD format\n");
 			if (config->stdid & vid_ch->stdid) {
@@ -1001,6 +1001,7 @@ static int vpif_reqbufs(struct file *file, void *priv,
 	q->ops = &video_qops;
 	q->mem_ops = &vb2_dma_contig_memops;
 	q->buf_struct_size = sizeof(struct vpif_disp_buffer);
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 
 	ret = vb2_queue_init(q);
 	if (ret) {
@@ -1058,14 +1059,14 @@ static int vpif_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
 	return vb2_qbuf(&common->buffer_queue, buf);
 }
 
-static int vpif_s_std(struct file *file, void *priv, v4l2_std_id *std_id)
+static int vpif_s_std(struct file *file, void *priv, v4l2_std_id std_id)
 {
 	struct vpif_fh *fh = priv;
 	struct channel_obj *ch = fh->channel;
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
 	int ret = 0;
 
-	if (!(*std_id & VPIF_V4L2_STD))
+	if (!(std_id & VPIF_V4L2_STD))
 		return -EINVAL;
 
 	if (common->started) {
@@ -1074,7 +1075,7 @@ static int vpif_s_std(struct file *file, void *priv, v4l2_std_id *std_id)
 	}
 
 	/* Call encoder subdevice function to set the standard */
-	ch->video.stdid = *std_id;
+	ch->video.stdid = std_id;
 	memset(&ch->video.dv_timings, 0, sizeof(ch->video.dv_timings));
 	/* Get the information about the standard */
 	if (vpif_update_resolution(ch))
@@ -1092,14 +1093,14 @@ static int vpif_s_std(struct file *file, void *priv, v4l2_std_id *std_id)
 	vpif_config_format(ch);
 
 	ret = v4l2_device_call_until_err(&vpif_obj.v4l2_dev, 1, video,
-						s_std_output, *std_id);
+						s_std_output, std_id);
 	if (ret < 0) {
 		vpif_err("Failed to set output standard\n");
 		return ret;
 	}
 
 	ret = v4l2_device_call_until_err(&vpif_obj.v4l2_dev, 1, core,
-							s_std, *std_id);
+							s_std, std_id);
 	if (ret < 0)
 		vpif_err("Failed to set standard for sub devices\n");
 	return ret;
@@ -1567,7 +1568,8 @@ static int vpif_dbg_g_register(struct file *file, void *priv,
  * Returns zero or -EINVAL if write operations fails.
  */
 static int vpif_dbg_s_register(struct file *file, void *priv,
-		struct v4l2_dbg_register *reg){
+		const struct v4l2_dbg_register *reg)
+{
 	struct vpif_fh *fh = priv;
 	struct channel_obj *ch = fh->channel;
 
diff --git a/drivers/media/platform/davinci/vpss.c b/drivers/media/platform/davinci/vpss.c
index a19c552232d..8a2f01e344e 100644
--- a/drivers/media/platform/davinci/vpss.c
+++ b/drivers/media/platform/davinci/vpss.c
@@ -17,14 +17,11 @@
  *
  * common vpss system module platform driver for all video drivers.
  */
-#include <linux/kernel.h>
-#include <linux/sched.h>
-#include <linux/init.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
-#include <linux/spinlock.h>
-#include <linux/compiler.h>
 #include <linux/io.h>
+#include <linux/pm_runtime.h>
+
 #include <media/davinci/vpss.h>
 
 MODULE_LICENSE("GPL");
@@ -99,7 +96,7 @@ enum vpss_platform_type {
 
 /*
  * vpss operations. Depends on platform. Not all functions are available
- * on all platforms. The api, first check if a functio is available before
+ * on all platforms. The api, first check if a function is available before
  * invoking it. In the probe, the function ptrs are initialized based on
  * vpss name. vpss name can be "dm355_vpss", "dm644x_vpss" etc.
  */
@@ -114,7 +111,7 @@ struct vpss_hw_ops {
 	void (*set_sync_pol)(struct vpss_sync_pol);
 	/* set the PG_FRAME_SIZE register*/
 	void (*set_pg_frame_size)(struct vpss_pg_frame_size);
-	/* check and clear interrupt if occured */
+	/* check and clear interrupt if occurred */
 	int (*dma_complete_interrupt)(void);
 };
 
@@ -233,7 +230,7 @@ EXPORT_SYMBOL(vpss_clear_wbl_overflow);
 
 /*
  *  dm355_enable_clock - Enable VPSS Clock
- *  @clock_sel: CLock to be enabled/disabled
+ *  @clock_sel: Clock to be enabled/disabled
  *  @en: enable/disable flag
  *
  *  This is called to enable or disable a vpss clock
@@ -490,6 +487,10 @@ static int vpss_probe(struct platform_device *pdev)
 	} else
 		oper_cfg.hw_ops.clear_wbl_overflow = dm644x_clear_wbl_overflow;
 
+	pm_runtime_enable(&pdev->dev);
+
+	pm_runtime_get(&pdev->dev);
+
 	spin_lock_init(&oper_cfg.vpss_lock);
 	dev_info(&pdev->dev, "%s vpss probe success\n", platform_name);
 	return 0;
@@ -507,6 +508,7 @@ static int vpss_remove(struct platform_device *pdev)
 {
 	struct resource		*res;
 
+	pm_runtime_disable(&pdev->dev);
 	iounmap(oper_cfg.vpss_regs_base0);
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	release_mem_region(res->start, resource_size(res));
@@ -518,10 +520,28 @@ static int vpss_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static int vpss_suspend(struct device *dev)
+{
+	pm_runtime_put(dev);
+	return 0;
+}
+
+static int vpss_resume(struct device *dev)
+{
+	pm_runtime_get(dev);
+	return 0;
+}
+
+static const struct dev_pm_ops vpss_pm_ops = {
+	.suspend = vpss_suspend,
+	.resume = vpss_resume,
+};
+
 static struct platform_driver vpss_driver = {
 	.driver = {
 		.name	= "vpss",
 		.owner = THIS_MODULE,
+		.pm = &vpss_pm_ops,
 	},
 	.remove = vpss_remove,
 	.probe = vpss_probe,
diff --git a/drivers/media/platform/exynos-gsc/gsc-core.c b/drivers/media/platform/exynos-gsc/gsc-core.c
index 82d9f6ac12f..33b5ffc8d66 100644
--- a/drivers/media/platform/exynos-gsc/gsc-core.c
+++ b/drivers/media/platform/exynos-gsc/gsc-core.c
@@ -1054,16 +1054,18 @@ static int gsc_m2m_suspend(struct gsc_dev *gsc)
 
 static int gsc_m2m_resume(struct gsc_dev *gsc)
 {
+	struct gsc_ctx *ctx;
 	unsigned long flags;
 
 	spin_lock_irqsave(&gsc->slock, flags);
 	/* Clear for full H/W setup in first run after resume */
+	ctx = gsc->m2m.ctx;
 	gsc->m2m.ctx = NULL;
 	spin_unlock_irqrestore(&gsc->slock, flags);
 
 	if (test_and_clear_bit(ST_M2M_SUSPENDED, &gsc->state))
-		gsc_m2m_job_finish(gsc->m2m.ctx,
-				    VB2_BUF_STATE_ERROR);
+		gsc_m2m_job_finish(ctx, VB2_BUF_STATE_ERROR);
+
 	return 0;
 }
 
@@ -1204,7 +1206,7 @@ static int gsc_resume(struct device *dev)
 	/* Do not resume if the device was idle before system suspend */
 	spin_lock_irqsave(&gsc->slock, flags);
 	if (!test_and_clear_bit(ST_SUSPEND, &gsc->state) ||
-	    !gsc_m2m_active(gsc)) {
+	    !gsc_m2m_opened(gsc)) {
 		spin_unlock_irqrestore(&gsc->slock, flags);
 		return 0;
 	}
diff --git a/drivers/media/platform/exynos-gsc/gsc-m2m.c b/drivers/media/platform/exynos-gsc/gsc-m2m.c
index 386c0a7a3a5..40a73f7d20d 100644
--- a/drivers/media/platform/exynos-gsc/gsc-m2m.c
+++ b/drivers/media/platform/exynos-gsc/gsc-m2m.c
@@ -80,6 +80,9 @@ void gsc_m2m_job_finish(struct gsc_ctx *ctx, int vb_state)
 	dst_vb = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
 
 	if (src_vb && dst_vb) {
+		src_vb->v4l2_buf.timestamp = dst_vb->v4l2_buf.timestamp;
+		src_vb->v4l2_buf.timecode = dst_vb->v4l2_buf.timecode;
+
 		v4l2_m2m_buf_done(src_vb, vb_state);
 		v4l2_m2m_buf_done(dst_vb, vb_state);
 
@@ -584,6 +587,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	src_vq->ops = &gsc_m2m_qops;
 	src_vq->mem_ops = &vb2_dma_contig_memops;
 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	src_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	ret = vb2_queue_init(src_vq);
 	if (ret)
@@ -596,6 +600,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	dst_vq->ops = &gsc_m2m_qops;
 	dst_vq->mem_ops = &vb2_dma_contig_memops;
 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	dst_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	return vb2_queue_init(dst_vq);
 }
diff --git a/drivers/media/platform/exynos-gsc/gsc-regs.c b/drivers/media/platform/exynos-gsc/gsc-regs.c
index 6f5b5a486cf..e22d147a694 100644
--- a/drivers/media/platform/exynos-gsc/gsc-regs.c
+++ b/drivers/media/platform/exynos-gsc/gsc-regs.c
@@ -12,7 +12,6 @@
 
 #include <linux/io.h>
 #include <linux/delay.h>
-#include <mach/map.h>
 
 #include "gsc-core.h"
 
diff --git a/drivers/media/platform/s5p-fimc/Kconfig b/drivers/media/platform/exynos4-is/Kconfig
index f997a5203b7..6ff99b5849f 100644
--- a/drivers/media/platform/s5p-fimc/Kconfig
+++ b/drivers/media/platform/exynos4-is/Kconfig
@@ -1,21 +1,22 @@
 
-config VIDEO_SAMSUNG_S5P_FIMC
-	bool "Samsung S5P/EXYNOS SoC camera interface driver (experimental)"
+config VIDEO_SAMSUNG_EXYNOS4_IS
+	bool "Samsung S5P/EXYNOS4 SoC series Camera Subsystem driver"
 	depends on VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API && PLAT_S5P && PM_RUNTIME
 	help
 	  Say Y here to enable camera host interface devices for
 	  Samsung S5P and EXYNOS SoC series.
 
-if VIDEO_SAMSUNG_S5P_FIMC
+if VIDEO_SAMSUNG_EXYNOS4_IS
 
 config VIDEO_S5P_FIMC
 	tristate "S5P/EXYNOS4 FIMC/CAMIF camera interface driver"
 	depends on I2C
 	select VIDEOBUF2_DMA_CONTIG
 	select V4L2_MEM2MEM_DEV
+	select MFD_SYSCON if OF
 	help
 	  This is a V4L2 driver for Samsung S5P and EXYNOS4 SoC camera host
-	  interface and video postprocessor (FIMC and FIMC-LITE) devices.
+	  interface and video postprocessor (FIMC) devices.
 
 	  To compile this driver as a module, choose M here: the
 	  module will be called s5p-fimc.
@@ -45,4 +46,16 @@ config VIDEO_EXYNOS_FIMC_LITE
 	  module will be called exynos-fimc-lite.
 endif
 
+config VIDEO_EXYNOS4_FIMC_IS
+	tristate "EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver"
+	select VIDEOBUF2_DMA_CONTIG
+	depends on OF
+	select FW_LOADER
+	help
+	  This is a V4L2 driver for Samsung EXYNOS4x12 SoC series
+	  FIMC-IS (Imaging Subsystem).
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called exynos4-fimc-is.
+
 endif # VIDEO_SAMSUNG_S5P_FIMC
diff --git a/drivers/media/platform/s5p-fimc/Makefile b/drivers/media/platform/exynos4-is/Makefile
index 46485143e1c..f25f4637739 100644
--- a/drivers/media/platform/s5p-fimc/Makefile
+++ b/drivers/media/platform/exynos4-is/Makefile
@@ -1,7 +1,10 @@
-s5p-fimc-objs := fimc-core.o fimc-reg.o fimc-m2m.o fimc-capture.o fimc-mdevice.o
+s5p-fimc-objs := fimc-core.o fimc-reg.o fimc-m2m.o fimc-capture.o media-dev.o
 exynos-fimc-lite-objs += fimc-lite-reg.o fimc-lite.o
+exynos-fimc-is-objs := fimc-is.o fimc-isp.o fimc-is-sensor.o fimc-is-regs.o
+exynos-fimc-is-objs += fimc-is-param.o fimc-is-errno.o fimc-is-i2c.o
 s5p-csis-objs := mipi-csis.o
 
 obj-$(CONFIG_VIDEO_S5P_MIPI_CSIS)	+= s5p-csis.o
 obj-$(CONFIG_VIDEO_EXYNOS_FIMC_LITE)	+= exynos-fimc-lite.o
+obj-$(CONFIG_VIDEO_EXYNOS4_FIMC_IS)	+= exynos-fimc-is.o
 obj-$(CONFIG_VIDEO_S5P_FIMC)		+= s5p-fimc.o
diff --git a/drivers/media/platform/s5p-fimc/fimc-capture.c b/drivers/media/platform/exynos4-is/fimc-capture.c
index f553cc2a8ee..528f4136936 100644
--- a/drivers/media/platform/s5p-fimc/fimc-capture.c
+++ b/drivers/media/platform/exynos4-is/fimc-capture.c
@@ -27,32 +27,33 @@
 #include <media/videobuf2-core.h>
 #include <media/videobuf2-dma-contig.h>
 
-#include "fimc-mdevice.h"
+#include "media-dev.h"
 #include "fimc-core.h"
 #include "fimc-reg.h"
 
 static int fimc_capture_hw_init(struct fimc_dev *fimc)
 {
+	struct fimc_source_info *si = &fimc->vid_cap.source_config;
 	struct fimc_ctx *ctx = fimc->vid_cap.ctx;
-	struct fimc_pipeline *p = &fimc->pipeline;
-	struct fimc_sensor_info *sensor;
+	int ret;
 	unsigned long flags;
-	int ret = 0;
 
-	if (p->subdevs[IDX_SENSOR] == NULL || ctx == NULL)
-		return -ENXIO;
-	if (ctx->s_frame.fmt == NULL)
+	if (ctx == NULL || ctx->s_frame.fmt == NULL)
 		return -EINVAL;
 
-	sensor = v4l2_get_subdev_hostdata(p->subdevs[IDX_SENSOR]);
+	if (si->fimc_bus_type == FIMC_BUS_TYPE_ISP_WRITEBACK) {
+		ret = fimc_hw_camblk_cfg_writeback(fimc);
+		if (ret < 0)
+			return ret;
+	}
 
 	spin_lock_irqsave(&fimc->slock, flags);
 	fimc_prepare_dma_offset(ctx, &ctx->d_frame);
 	fimc_set_yuv_order(ctx);
 
-	fimc_hw_set_camera_polarity(fimc, &sensor->pdata);
-	fimc_hw_set_camera_type(fimc, &sensor->pdata);
-	fimc_hw_set_camera_source(fimc, &sensor->pdata);
+	fimc_hw_set_camera_polarity(fimc, si);
+	fimc_hw_set_camera_type(fimc, si);
+	fimc_hw_set_camera_source(fimc, si);
 	fimc_hw_set_camera_offset(fimc, &ctx->s_frame);
 
 	ret = fimc_set_scaler_info(ctx);
@@ -65,7 +66,7 @@ static int fimc_capture_hw_init(struct fimc_dev *fimc)
 		fimc_hw_set_effect(ctx);
 		fimc_hw_set_output_path(ctx);
 		fimc_hw_set_out_dma(ctx);
-		if (fimc->variant->has_alpha)
+		if (fimc->drv_data->alpha_color)
 			fimc_hw_set_rgb_alpha(ctx);
 		clear_bit(ST_CAPT_APPLY_CFG, &fimc->state);
 	}
@@ -168,7 +169,7 @@ static int fimc_capture_config_update(struct fimc_ctx *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)
+	if (fimc->drv_data->alpha_color)
 		fimc_hw_set_rgb_alpha(ctx);
 
 	clear_bit(ST_CAPT_APPLY_CFG, &fimc->state);
@@ -286,8 +287,8 @@ 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_call(fimc, set_stream,
-					   &fimc->pipeline, 1);
+			return fimc_pipeline_call(fimc, set_stream,
+						  &fimc->pipeline, 1);
 	}
 
 	return 0;
@@ -443,35 +444,28 @@ static void buffer_queue(struct vb2_buffer *vb)
 	if (vb2_is_streaming(&vid_cap->vbq) &&
 	    vid_cap->active_buf_cnt >= min_bufs &&
 	    !test_and_set_bit(ST_CAPT_STREAM, &fimc->state)) {
+		int ret;
+
 		fimc_activate_capture(ctx);
 		spin_unlock_irqrestore(&fimc->slock, flags);
 
-		if (!test_and_set_bit(ST_CAPT_ISP_STREAM, &fimc->state))
-			fimc_pipeline_call(fimc, set_stream,
-					   &fimc->pipeline, 1);
+		if (test_and_set_bit(ST_CAPT_ISP_STREAM, &fimc->state))
+			return;
+
+		ret = fimc_pipeline_call(fimc, set_stream, &fimc->pipeline, 1);
+		if (ret < 0)
+			v4l2_err(&vid_cap->vfd, "stream on failed: %d\n", ret);
 		return;
 	}
 	spin_unlock_irqrestore(&fimc->slock, flags);
 }
 
-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_capture_qops = {
 	.queue_setup		= queue_setup,
 	.buf_prepare		= buffer_prepare,
 	.buf_queue		= buffer_queue,
-	.wait_prepare		= fimc_unlock,
-	.wait_finish		= fimc_lock,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
 	.start_streaming	= start_streaming,
 	.stop_streaming		= stop_streaming,
 };
@@ -530,7 +524,7 @@ static int fimc_capture_open(struct file *file)
 		goto unlock;
 	}
 
-	if (++fimc->vid_cap.refcnt == 1) {
+	if (v4l2_fh_is_singular_file(file)) {
 		ret = fimc_pipeline_call(fimc, open, &fimc->pipeline,
 					 &fimc->vid_cap.vfd.entity, true);
 
@@ -543,8 +537,9 @@ static int fimc_capture_open(struct file *file)
 		if (ret < 0) {
 			clear_bit(ST_CAPT_BUSY, &fimc->state);
 			pm_runtime_put_sync(&fimc->pdev->dev);
-			fimc->vid_cap.refcnt--;
 			v4l2_fh_release(file);
+		} else {
+			fimc->vid_cap.refcnt++;
 		}
 	}
 unlock:
@@ -553,59 +548,34 @@ unlock:
 	return ret;
 }
 
-static int fimc_capture_close(struct file *file)
+static int fimc_capture_release(struct file *file)
 {
 	struct fimc_dev *fimc = video_drvdata(file);
+	struct fimc_vid_cap *vc = &fimc->vid_cap;
 	int ret;
 
 	dbg("pid: %d, state: 0x%lx", task_pid_nr(current), fimc->state);
 
 	mutex_lock(&fimc->lock);
 
-	if (--fimc->vid_cap.refcnt == 0) {
+	if (v4l2_fh_is_singular_file(file)) {
+		if (vc->streaming) {
+			media_entity_pipeline_stop(&vc->vfd.entity);
+			vc->streaming = false;
+		}
 		clear_bit(ST_CAPT_BUSY, &fimc->state);
 		fimc_stop_capture(fimc, false);
 		fimc_pipeline_call(fimc, close, &fimc->pipeline);
 		clear_bit(ST_CAPT_SUSPENDED, &fimc->state);
+		fimc->vid_cap.refcnt--;
 	}
 
 	pm_runtime_put(&fimc->pdev->dev);
 
-	if (fimc->vid_cap.refcnt == 0) {
-		vb2_queue_release(&fimc->vid_cap.vbq);
+	if (v4l2_fh_is_singular_file(file))
 		fimc_ctrls_delete(fimc->vid_cap.ctx);
-	}
-
-	ret = v4l2_fh_release(file);
-
-	mutex_unlock(&fimc->lock);
-	return ret;
-}
-
-static unsigned int fimc_capture_poll(struct file *file,
-				      struct poll_table_struct *wait)
-{
-	struct fimc_dev *fimc = video_drvdata(file);
-	int ret;
-
-	if (mutex_lock_interruptible(&fimc->lock))
-		return POLL_ERR;
 
-	ret = vb2_poll(&fimc->vid_cap.vbq, file, wait);
-	mutex_unlock(&fimc->lock);
-
-	return ret;
-}
-
-static int fimc_capture_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	struct fimc_dev *fimc = video_drvdata(file);
-	int ret;
-
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
-	ret = vb2_mmap(&fimc->vid_cap.vbq, vma);
+	ret = vb2_fop_release(file);
 	mutex_unlock(&fimc->lock);
 
 	return ret;
@@ -614,10 +584,10 @@ static int fimc_capture_mmap(struct file *file, struct vm_area_struct *vma)
 static const struct v4l2_file_operations fimc_capture_fops = {
 	.owner		= THIS_MODULE,
 	.open		= fimc_capture_open,
-	.release	= fimc_capture_close,
-	.poll		= fimc_capture_poll,
+	.release	= fimc_capture_release,
+	.poll		= vb2_fop_poll,
 	.unlocked_ioctl	= video_ioctl2,
-	.mmap		= fimc_capture_mmap,
+	.mmap		= vb2_fop_mmap,
 };
 
 /*
@@ -642,18 +612,22 @@ static struct fimc_fmt *fimc_capture_try_format(struct fimc_ctx *ctx,
 	    fimc_fmt_is_user_defined(ctx->s_frame.fmt->color))
 		*code = ctx->s_frame.fmt->mbus_code;
 
-	if (fourcc && *fourcc != V4L2_PIX_FMT_JPEG && pad != FIMC_SD_PAD_SINK)
+	if (fourcc && *fourcc != V4L2_PIX_FMT_JPEG && pad == FIMC_SD_PAD_SOURCE)
 		mask |= FMT_FLAGS_M2M;
 
+	if (pad == FIMC_SD_PAD_SINK_FIFO)
+		mask = FMT_FLAGS_WRITEBACK;
+
 	ffmt = fimc_find_format(fourcc, code, mask, 0);
 	if (WARN_ON(!ffmt))
 		return NULL;
+
 	if (code)
 		*code = ffmt->mbus_code;
 	if (fourcc)
 		*fourcc = ffmt->fourcc;
 
-	if (pad == FIMC_SD_PAD_SINK) {
+	if (pad != FIMC_SD_PAD_SOURCE) {
 		max_w = fimc_fmt_is_user_defined(ffmt->color) ?
 			pl->scaler_dis_w : pl->scaler_en_w;
 		/* Apply the camera input interface pixel constraints */
@@ -768,16 +742,13 @@ static void fimc_capture_try_selection(struct fimc_ctx *ctx,
 /*
  * The video node ioctl operations
  */
-static int fimc_vidioc_querycap_capture(struct file *file, void *priv,
+static int fimc_cap_querycap(struct file *file, void *priv,
 					struct v4l2_capability *cap)
 {
 	struct fimc_dev *fimc = video_drvdata(file);
 
-	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;
-
+	__fimc_vidioc_querycap(&fimc->pdev->dev, cap, V4L2_CAP_STREAMING |
+					V4L2_CAP_VIDEO_CAPTURE_MPLANE);
 	return 0;
 }
 
@@ -887,7 +858,7 @@ static int fimc_pipeline_try_format(struct fimc_ctx *ctx,
 		tfmt->width  = mf->width;
 		tfmt->height = mf->height;
 		ffmt = fimc_capture_try_format(ctx, &tfmt->width, &tfmt->height,
-					NULL, &fcc, FIMC_SD_PAD_SINK);
+					NULL, &fcc, FIMC_SD_PAD_SINK_CAM);
 		ffmt = fimc_capture_try_format(ctx, &tfmt->width, &tfmt->height,
 					NULL, &fcc, FIMC_SD_PAD_SOURCE);
 		if (ffmt && ffmt->mbus_code)
@@ -974,7 +945,7 @@ static int fimc_cap_try_fmt_mplane(struct file *file, void *fh,
 	if (fimc_jpeg_fourcc(pix->pixelformat)) {
 		fimc_capture_try_format(ctx, &pix->width, &pix->height,
 					NULL, &pix->pixelformat,
-					FIMC_SD_PAD_SINK);
+					FIMC_SD_PAD_SINK_CAM);
 		ctx->s_frame.f_width  = pix->width;
 		ctx->s_frame.f_height = pix->height;
 	}
@@ -1028,7 +999,7 @@ static int __fimc_capture_set_format(struct fimc_dev *fimc,
 {
 	struct fimc_ctx *ctx = fimc->vid_cap.ctx;
 	struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
-	struct v4l2_mbus_framefmt *mf = &fimc->vid_cap.mf;
+	struct v4l2_mbus_framefmt *mf = &fimc->vid_cap.ci_fmt;
 	struct fimc_frame *ff = &ctx->d_frame;
 	struct fimc_fmt *s_fmt = NULL;
 	int ret, i;
@@ -1040,7 +1011,7 @@ static int __fimc_capture_set_format(struct fimc_dev *fimc,
 	if (fimc_jpeg_fourcc(pix->pixelformat)) {
 		fimc_capture_try_format(ctx, &pix->width, &pix->height,
 					NULL, &pix->pixelformat,
-					FIMC_SD_PAD_SINK);
+					FIMC_SD_PAD_SINK_CAM);
 		ctx->s_frame.f_width  = pix->width;
 		ctx->s_frame.f_height = pix->height;
 	}
@@ -1157,44 +1128,51 @@ static int fimc_cap_g_input(struct file *file, void *priv, unsigned int *i)
 static int fimc_pipeline_validate(struct fimc_dev *fimc)
 {
 	struct v4l2_subdev_format sink_fmt, src_fmt;
-	struct fimc_vid_cap *vid_cap = &fimc->vid_cap;
-	struct v4l2_subdev *sd;
-	struct media_pad *pad;
-	int ret;
-
-	/* Start with the video capture node pad */
-	pad = media_entity_remote_source(&vid_cap->vd_pad);
-	if (pad == NULL)
-		return -EPIPE;
-	/* FIMC.{N} subdevice */
-	sd = media_entity_to_v4l2_subdev(pad->entity);
+	struct fimc_vid_cap *vc = &fimc->vid_cap;
+	struct v4l2_subdev *sd = &vc->subdev;
+	struct media_pad *sink_pad, *src_pad;
+	int i, ret;
 
 	while (1) {
-		/* Retrieve format at the sink pad */
-		pad = &sd->entity.pads[0];
-		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+		/*
+		 * Find current entity sink pad and any remote sink pad linked
+		 * to it. We stop if there is no sink pad in current entity or
+		 * it is not linked to any other remote entity.
+		 */
+		src_pad = NULL;
+
+		for (i = 0; i < sd->entity.num_pads; i++) {
+			struct media_pad *p = &sd->entity.pads[i];
+
+			if (p->flags & MEDIA_PAD_FL_SINK) {
+				sink_pad = p;
+				src_pad = media_entity_remote_source(sink_pad);
+				if (src_pad)
+					break;
+			}
+		}
+
+		if (src_pad == NULL ||
+		    media_entity_type(src_pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
 			break;
+
 		/* Don't call FIMC subdev operation to avoid nested locking */
-		if (sd == &fimc->vid_cap.subdev) {
-			struct fimc_frame *ff = &vid_cap->ctx->s_frame;
+		if (sd == &vc->subdev) {
+			struct fimc_frame *ff = &vc->ctx->s_frame;
 			sink_fmt.format.width = ff->f_width;
 			sink_fmt.format.height = ff->f_height;
 			sink_fmt.format.code = ff->fmt ? ff->fmt->mbus_code : 0;
 		} else {
-			sink_fmt.pad = pad->index;
+			sink_fmt.pad = sink_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;
+		/* Retrieve format at the source pad */
+		sd = media_entity_to_v4l2_subdev(src_pad->entity);
+		src_fmt.pad = src_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)
@@ -1208,7 +1186,7 @@ static int fimc_pipeline_validate(struct fimc_dev *fimc)
 		if (sd == fimc->pipeline.subdevs[IDX_SENSOR] &&
 		    fimc_user_defined_mbus_fmt(src_fmt.format.code)) {
 			struct v4l2_plane_pix_format plane_fmt[FIMC_MAX_PLANES];
-			struct fimc_frame *frame = &vid_cap->ctx->d_frame;
+			struct fimc_frame *frame = &vc->ctx->d_frame;
 			unsigned int i;
 
 			ret = fimc_get_sensor_frame_desc(sd, plane_fmt,
@@ -1230,96 +1208,80 @@ static int fimc_cap_streamon(struct file *file, void *priv,
 {
 	struct fimc_dev *fimc = video_drvdata(file);
 	struct fimc_pipeline *p = &fimc->pipeline;
-	struct v4l2_subdev *sd = p->subdevs[IDX_SENSOR];
+	struct fimc_vid_cap *vc = &fimc->vid_cap;
+	struct media_entity *entity = &vc->vfd.entity;
+	struct fimc_source_info *si = NULL;
+	struct v4l2_subdev *sd;
 	int ret;
 
 	if (fimc_capture_active(fimc))
 		return -EBUSY;
 
-	ret = media_entity_pipeline_start(&sd->entity, p->m_pipeline);
+	ret = media_entity_pipeline_start(entity, p->m_pipeline);
 	if (ret < 0)
 		return ret;
 
-	if (fimc->vid_cap.user_subdev_api) {
+	sd = p->subdevs[IDX_SENSOR];
+	if (sd)
+		si = v4l2_get_subdev_hostdata(sd);
+
+	if (si == NULL) {
+		ret = -EPIPE;
+		goto err_p_stop;
+	}
+	/*
+	 * Save configuration data related to currently attached image
+	 * sensor or other data source, e.g. FIMC-IS.
+	 */
+	vc->source_config = *si;
+
+	if (vc->input == GRP_ID_FIMC_IS)
+		vc->source_config.fimc_bus_type = FIMC_BUS_TYPE_ISP_WRITEBACK;
+
+	if (vc->user_subdev_api) {
 		ret = fimc_pipeline_validate(fimc);
-		if (ret < 0) {
-			media_entity_pipeline_stop(&sd->entity);
-			return ret;
-		}
+		if (ret < 0)
+			goto err_p_stop;
 	}
-	return vb2_streamon(&fimc->vid_cap.vbq, type);
+
+	ret = vb2_ioctl_streamon(file, priv, type);
+	if (!ret) {
+		vc->streaming = true;
+		return ret;
+	}
+
+err_p_stop:
+	media_entity_pipeline_stop(entity);
+	return ret;
 }
 
 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.subdevs[IDX_SENSOR];
 	int ret;
 
-	ret = vb2_streamoff(&fimc->vid_cap.vbq, type);
-	if (ret == 0)
-		media_entity_pipeline_stop(&sd->entity);
-	return ret;
+	ret = vb2_ioctl_streamoff(file, priv, type);
+	if (ret < 0)
+		return ret;
+
+	media_entity_pipeline_stop(&fimc->vid_cap.vfd.entity);
+	fimc->vid_cap.streaming = false;
+	return 0;
 }
 
 static int fimc_cap_reqbufs(struct file *file, void *priv,
 			    struct v4l2_requestbuffers *reqbufs)
 {
 	struct fimc_dev *fimc = video_drvdata(file);
-	int ret = vb2_reqbufs(&fimc->vid_cap.vbq, reqbufs);
+	int ret;
+
+	ret = vb2_ioctl_reqbufs(file, priv, reqbufs);
 
 	if (!ret)
 		fimc->vid_cap.reqbufs_count = reqbufs->count;
-	return ret;
-}
-
-static int fimc_cap_querybuf(struct file *file, void *priv,
-			   struct v4l2_buffer *buf)
-{
-	struct fimc_dev *fimc = video_drvdata(file);
-
-	return vb2_querybuf(&fimc->vid_cap.vbq, buf);
-}
-
-static int fimc_cap_qbuf(struct file *file, void *priv,
-			  struct v4l2_buffer *buf)
-{
-	struct fimc_dev *fimc = video_drvdata(file);
-
-	return vb2_qbuf(&fimc->vid_cap.vbq, buf);
-}
-
-static int fimc_cap_expbuf(struct file *file, void *priv,
-			  struct v4l2_exportbuffer *eb)
-{
-	struct fimc_dev *fimc = video_drvdata(file);
-
-	return vb2_expbuf(&fimc->vid_cap.vbq, eb);
-}
-
-static int fimc_cap_dqbuf(struct file *file, void *priv,
-			   struct v4l2_buffer *buf)
-{
-	struct fimc_dev *fimc = video_drvdata(file);
-
-	return vb2_dqbuf(&fimc->vid_cap.vbq, buf, file->f_flags & O_NONBLOCK);
-}
-
-static int fimc_cap_create_bufs(struct file *file, void *priv,
-				struct v4l2_create_buffers *create)
-{
-	struct fimc_dev *fimc = video_drvdata(file);
 
-	return vb2_create_bufs(&fimc->vid_cap.vbq, create);
-}
-
-static int fimc_cap_prepare_buf(struct file *file, void *priv,
-				struct v4l2_buffer *b)
-{
-	struct fimc_dev *fimc = video_drvdata(file);
-
-	return vb2_prepare_buf(&fimc->vid_cap.vbq, b);
+	return ret;
 }
 
 static int fimc_cap_g_selection(struct file *file, void *fh,
@@ -1410,7 +1372,7 @@ static int fimc_cap_s_selection(struct file *file, void *fh,
 }
 
 static const struct v4l2_ioctl_ops fimc_capture_ioctl_ops = {
-	.vidioc_querycap		= fimc_vidioc_querycap_capture,
+	.vidioc_querycap		= fimc_cap_querycap,
 
 	.vidioc_enum_fmt_vid_cap_mplane	= fimc_cap_enum_fmt_mplane,
 	.vidioc_try_fmt_vid_cap_mplane	= fimc_cap_try_fmt_mplane,
@@ -1418,14 +1380,12 @@ static const struct v4l2_ioctl_ops fimc_capture_ioctl_ops = {
 	.vidioc_g_fmt_vid_cap_mplane	= fimc_cap_g_fmt_mplane,
 
 	.vidioc_reqbufs			= fimc_cap_reqbufs,
-	.vidioc_querybuf		= fimc_cap_querybuf,
-
-	.vidioc_qbuf			= fimc_cap_qbuf,
-	.vidioc_dqbuf			= fimc_cap_dqbuf,
-	.vidioc_expbuf			= fimc_cap_expbuf,
-
-	.vidioc_prepare_buf		= fimc_cap_prepare_buf,
-	.vidioc_create_bufs		= fimc_cap_create_bufs,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+	.vidioc_expbuf			= vb2_ioctl_expbuf,
+	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
+	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
 
 	.vidioc_streamon		= fimc_cap_streamon,
 	.vidioc_streamoff		= fimc_cap_streamoff,
@@ -1487,7 +1447,7 @@ static const struct media_entity_operations fimc_sd_media_ops = {
 void fimc_sensor_notify(struct v4l2_subdev *sd, unsigned int notification,
 			void *arg)
 {
-	struct fimc_sensor_info	*sensor;
+	struct fimc_source_info	*si;
 	struct fimc_vid_buffer *buf;
 	struct fimc_md *fmd;
 	struct fimc_dev *fimc;
@@ -1496,11 +1456,12 @@ void fimc_sensor_notify(struct v4l2_subdev *sd, unsigned int notification,
 	if (sd == NULL)
 		return;
 
-	sensor = v4l2_get_subdev_hostdata(sd);
+	si = v4l2_get_subdev_hostdata(sd);
 	fmd = entity_to_fimc_mdev(&sd->entity);
 
 	spin_lock_irqsave(&fmd->slock, flags);
-	fimc = sensor ? sensor->host : NULL;
+
+	fimc = si ? source_to_sensor_info(si)->host : NULL;
 
 	if (fimc && arg && notification == S5P_FIMC_TX_END_NOTIFY &&
 	    test_bit(ST_CAPT_PEND, &fimc->state)) {
@@ -1537,25 +1498,37 @@ static int fimc_subdev_get_fmt(struct v4l2_subdev *sd,
 {
 	struct fimc_dev *fimc = v4l2_get_subdevdata(sd);
 	struct fimc_ctx *ctx = fimc->vid_cap.ctx;
+	struct fimc_frame *ff = &ctx->s_frame;
 	struct v4l2_mbus_framefmt *mf;
-	struct fimc_frame *ff;
 
 	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
 		mf = v4l2_subdev_get_try_format(fh, fmt->pad);
 		fmt->format = *mf;
 		return 0;
 	}
-	mf = &fmt->format;
-	mf->colorspace = V4L2_COLORSPACE_JPEG;
-	ff = fmt->pad == FIMC_SD_PAD_SINK ? &ctx->s_frame : &ctx->d_frame;
 
+	mf = &fmt->format;
 	mutex_lock(&fimc->lock);
-	/* The pixel code is same on both input and output pad */
-	if (!WARN_ON(ctx->s_frame.fmt == NULL))
-		mf->code = ctx->s_frame.fmt->mbus_code;
-	mf->width  = ff->f_width;
-	mf->height = ff->f_height;
+
+	switch (fmt->pad) {
+	case FIMC_SD_PAD_SOURCE:
+		if (!WARN_ON(ff->fmt == NULL))
+			mf->code = ff->fmt->mbus_code;
+		/* Sink pads crop rectangle size */
+		mf->width = ff->width;
+		mf->height = ff->height;
+		break;
+	case FIMC_SD_PAD_SINK_FIFO:
+		*mf = fimc->vid_cap.wb_fmt;
+		break;
+	case FIMC_SD_PAD_SINK_CAM:
+	default:
+		*mf = fimc->vid_cap.ci_fmt;
+		break;
+	}
+
 	mutex_unlock(&fimc->lock);
+	mf->colorspace = V4L2_COLORSPACE_JPEG;
 
 	return 0;
 }
@@ -1566,15 +1539,15 @@ static int fimc_subdev_set_fmt(struct v4l2_subdev *sd,
 {
 	struct fimc_dev *fimc = v4l2_get_subdevdata(sd);
 	struct v4l2_mbus_framefmt *mf = &fmt->format;
-	struct fimc_ctx *ctx = fimc->vid_cap.ctx;
+	struct fimc_vid_cap *vc = &fimc->vid_cap;
+	struct fimc_ctx *ctx = vc->ctx;
 	struct fimc_frame *ff;
 	struct fimc_fmt *ffmt;
 
 	dbg("pad%d: code: 0x%x, %dx%d",
 	    fmt->pad, mf->code, mf->width, mf->height);
 
-	if (fmt->pad == FIMC_SD_PAD_SOURCE &&
-	    vb2_is_busy(&fimc->vid_cap.vbq))
+	if (fmt->pad == FIMC_SD_PAD_SOURCE && vb2_is_busy(&vc->vbq))
 		return -EBUSY;
 
 	mutex_lock(&fimc->lock);
@@ -1596,21 +1569,32 @@ static int fimc_subdev_set_fmt(struct v4l2_subdev *sd,
 	fimc_alpha_ctrl_update(ctx);
 
 	fimc_capture_mark_jpeg_xfer(ctx, ffmt->color);
-
-	ff = fmt->pad == FIMC_SD_PAD_SINK ?
-		&ctx->s_frame : &ctx->d_frame;
+	if (fmt->pad == FIMC_SD_PAD_SOURCE) {
+		ff = &ctx->d_frame;
+		/* Sink pads crop rectangle size */
+		mf->width = ctx->s_frame.width;
+		mf->height = ctx->s_frame.height;
+	} else {
+		ff = &ctx->s_frame;
+	}
 
 	mutex_lock(&fimc->lock);
 	set_frame_bounds(ff, mf->width, mf->height);
-	fimc->vid_cap.mf = *mf;
+
+	if (fmt->pad == FIMC_SD_PAD_SINK_FIFO)
+		vc->wb_fmt = *mf;
+	else if (fmt->pad == FIMC_SD_PAD_SINK_CAM)
+		vc->ci_fmt = *mf;
+
 	ff->fmt = ffmt;
 
 	/* Reset the crop rectangle if required. */
 	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)
+	if (fmt->pad != FIMC_SD_PAD_SOURCE)
 		ctx->state &= ~FIMC_COMPOSE;
+
 	mutex_unlock(&fimc->lock);
 	return 0;
 }
@@ -1625,7 +1609,7 @@ static int fimc_subdev_get_selection(struct v4l2_subdev *sd,
 	struct v4l2_rect *r = &sel->r;
 	struct v4l2_rect *try_sel;
 
-	if (sel->pad != FIMC_SD_PAD_SINK)
+	if (sel->pad == FIMC_SD_PAD_SOURCE)
 		return -EINVAL;
 
 	mutex_lock(&fimc->lock);
@@ -1681,7 +1665,7 @@ static int fimc_subdev_set_selection(struct v4l2_subdev *sd,
 	struct v4l2_rect *try_sel;
 	unsigned long flags;
 
-	if (sel->pad != FIMC_SD_PAD_SINK)
+	if (sel->pad == FIMC_SD_PAD_SOURCE)
 		return -EINVAL;
 
 	mutex_lock(&fimc->lock);
@@ -1752,9 +1736,9 @@ static int fimc_register_capture_device(struct fimc_dev *fimc,
 				 struct v4l2_device *v4l2_dev)
 {
 	struct video_device *vfd = &fimc->vid_cap.vfd;
-	struct fimc_vid_cap *vid_cap;
+	struct vb2_queue *q = &fimc->vid_cap.vbq;
 	struct fimc_ctx *ctx;
-	struct vb2_queue *q;
+	struct fimc_vid_cap *vid_cap;
 	int ret = -ENOMEM;
 
 	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
@@ -1776,27 +1760,27 @@ static int fimc_register_capture_device(struct fimc_dev *fimc,
 	vfd->v4l2_dev	= v4l2_dev;
 	vfd->minor	= -1;
 	vfd->release	= video_device_release_empty;
+	vfd->queue	= q;
 	vfd->lock	= &fimc->lock;
 
 	video_set_drvdata(vfd, fimc);
-
 	vid_cap = &fimc->vid_cap;
 	vid_cap->active_buf_cnt = 0;
-	vid_cap->reqbufs_count  = 0;
-	vid_cap->refcnt = 0;
+	vid_cap->reqbufs_count = 0;
+	vid_cap->ctx = ctx;
 
 	INIT_LIST_HEAD(&vid_cap->pending_buf_q);
 	INIT_LIST_HEAD(&vid_cap->active_buf_q);
-	vid_cap->ctx = ctx;
 
-	q = &fimc->vid_cap.vbq;
 	memset(q, 0, sizeof(*q));
 	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
 	q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
-	q->drv_priv = fimc->vid_cap.ctx;
+	q->drv_priv = ctx;
 	q->ops = &fimc_capture_qops;
 	q->mem_ops = &vb2_dma_contig_memops;
 	q->buf_struct_size = sizeof(struct fimc_vid_buffer);
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->lock = &fimc->lock;
 
 	ret = vb2_queue_init(q);
 	if (ret)
@@ -1882,10 +1866,11 @@ int fimc_initialize_capture_subdev(struct fimc_dev *fimc)
 	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);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	snprintf(sd->name, sizeof(sd->name), "FIMC.%d", fimc->id);
 
-	fimc->vid_cap.sd_pads[FIMC_SD_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	fimc->vid_cap.sd_pads[FIMC_SD_PAD_SINK_CAM].flags = MEDIA_PAD_FL_SINK;
+	fimc->vid_cap.sd_pads[FIMC_SD_PAD_SINK_FIFO].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);
diff --git a/drivers/media/platform/s5p-fimc/fimc-core.c b/drivers/media/platform/exynos4-is/fimc-core.c
index e3916bde45c..379a5e9d52a 100644
--- a/drivers/media/platform/s5p-fimc/fimc-core.c
+++ b/drivers/media/platform/exynos4-is/fimc-core.c
@@ -20,7 +20,10 @@
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/list.h>
+#include <linux/mfd/syscon.h>
 #include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
 #include <linux/slab.h>
 #include <linux/clk.h>
 #include <media/v4l2-ioctl.h>
@@ -29,7 +32,7 @@
 
 #include "fimc-core.h"
 #include "fimc-reg.h"
-#include "fimc-mdevice.h"
+#include "media-dev.h"
 
 static char *fimc_clocks[MAX_FIMC_CLOCKS] = {
 	"sclk_fimc", "fimc"
@@ -77,6 +80,10 @@ static struct fimc_fmt fimc_formats[] = {
 		.colplanes	= 1,
 		.flags		= FMT_FLAGS_M2M_OUT | FMT_HAS_ALPHA,
 	}, {
+		.name		= "YUV 4:4:4",
+		.mbus_code	= V4L2_MBUS_FMT_YUV10_1X30,
+		.flags		= FMT_FLAGS_WRITEBACK,
+	}, {
 		.name		= "YUV 4:2:2 packed, YCbYCr",
 		.fourcc		= V4L2_PIX_FMT_YUYV,
 		.depth		= { 16 },
@@ -206,6 +213,17 @@ struct fimc_fmt *fimc_get_format(unsigned int index)
 	return &fimc_formats[index];
 }
 
+void __fimc_vidioc_querycap(struct device *dev, struct v4l2_capability *cap,
+						unsigned int caps)
+{
+	strlcpy(cap->driver, dev->driver->name, sizeof(cap->driver));
+	strlcpy(cap->card, dev->driver->name, sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info),
+				"platform:%s", dev_name(dev));
+	cap->device_caps = caps;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+}
+
 int fimc_check_scaler_ratio(struct fimc_ctx *ctx, int sw, int sh,
 			    int dw, int dh, int rotation)
 {
@@ -405,34 +423,34 @@ void fimc_set_yuv_order(struct fimc_ctx *ctx)
 	/* Set order for 1 plane input formats. */
 	switch (ctx->s_frame.fmt->color) {
 	case FIMC_FMT_YCRYCB422:
-		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_CBYCRY;
+		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_YCRYCB;
 		break;
 	case FIMC_FMT_CBYCRY422:
-		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_YCRYCB;
+		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_CBYCRY;
 		break;
 	case FIMC_FMT_CRYCBY422:
-		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_YCBYCR;
+		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_CRYCBY;
 		break;
 	case FIMC_FMT_YCBYCR422:
 	default:
-		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_CRYCBY;
+		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_YCBYCR;
 		break;
 	}
 	dbg("ctx->in_order_1p= %d", ctx->in_order_1p);
 
 	switch (ctx->d_frame.fmt->color) {
 	case FIMC_FMT_YCRYCB422:
-		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_CBYCRY;
+		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_YCRYCB;
 		break;
 	case FIMC_FMT_CBYCRY422:
-		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_YCRYCB;
+		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_CBYCRY;
 		break;
 	case FIMC_FMT_CRYCBY422:
-		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_YCBYCR;
+		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_CRYCBY;
 		break;
 	case FIMC_FMT_YCBYCR422:
 	default:
-		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_CRYCBY;
+		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_YCBYCR;
 		break;
 	}
 	dbg("ctx->out_order_1p= %d", ctx->out_order_1p);
@@ -440,14 +458,14 @@ void fimc_set_yuv_order(struct fimc_ctx *ctx)
 
 void fimc_prepare_dma_offset(struct fimc_ctx *ctx, struct fimc_frame *f)
 {
-	const struct fimc_variant *variant = ctx->fimc_dev->variant;
+	bool pix_hoff = ctx->fimc_dev->drv_data->dma_pix_hoff;
 	u32 i, depth = 0;
 
 	for (i = 0; i < f->fmt->colplanes; i++)
 		depth += f->fmt->depth[i];
 
 	f->dma_offset.y_h = f->offs_h;
-	if (!variant->pix_hoff)
+	if (!pix_hoff)
 		f->dma_offset.y_h *= (depth >> 3);
 
 	f->dma_offset.y_v = f->offs_v;
@@ -458,7 +476,7 @@ void fimc_prepare_dma_offset(struct fimc_ctx *ctx, struct fimc_frame *f)
 	f->dma_offset.cr_h = f->offs_h;
 	f->dma_offset.cr_v = f->offs_v;
 
-	if (!variant->pix_hoff) {
+	if (!pix_hoff) {
 		if (f->fmt->colplanes == 3) {
 			f->dma_offset.cb_h >>= 1;
 			f->dma_offset.cr_h >>= 1;
@@ -589,7 +607,6 @@ static const struct v4l2_ctrl_ops fimc_ctrl_ops = {
 
 int fimc_ctrls_create(struct fimc_ctx *ctx)
 {
-	const 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;
@@ -606,7 +623,7 @@ int fimc_ctrls_create(struct fimc_ctx *ctx)
 	ctrls->vflip = v4l2_ctrl_new_std(handler, &fimc_ctrl_ops,
 					V4L2_CID_VFLIP, 0, 1, 1, 0);
 
-	if (variant->has_alpha)
+	if (ctx->fimc_dev->drv_data->alpha_color)
 		ctrls->alpha = v4l2_ctrl_new_std(handler, &fimc_ctrl_ops,
 					V4L2_CID_ALPHA_COMPONENT,
 					0, max_alpha, 1, 0);
@@ -677,7 +694,7 @@ void fimc_alpha_ctrl_update(struct fimc_ctx *ctx)
 	struct fimc_dev *fimc = ctx->fimc_dev;
 	struct v4l2_ctrl *ctrl = ctx->ctrls.alpha;
 
-	if (ctrl == NULL || !fimc->variant->has_alpha)
+	if (ctrl == NULL || !fimc->drv_data->alpha_color)
 		return;
 
 	v4l2_ctrl_lock(ctrl);
@@ -850,56 +867,128 @@ static int fimc_m2m_suspend(struct fimc_dev *fimc)
 
 static int fimc_m2m_resume(struct fimc_dev *fimc)
 {
+	struct fimc_ctx *ctx;
 	unsigned long flags;
 
 	spin_lock_irqsave(&fimc->slock, flags);
 	/* Clear for full H/W setup in first run after resume */
+	ctx = fimc->m2m.ctx;
 	fimc->m2m.ctx = NULL;
 	spin_unlock_irqrestore(&fimc->slock, flags);
 
 	if (test_and_clear_bit(ST_M2M_SUSPENDED, &fimc->state))
-		fimc_m2m_job_finish(fimc->m2m.ctx,
-				    VB2_BUF_STATE_ERROR);
+		fimc_m2m_job_finish(ctx, VB2_BUF_STATE_ERROR);
+
+	return 0;
+}
+
+static const struct of_device_id fimc_of_match[];
+
+static int fimc_parse_dt(struct fimc_dev *fimc, u32 *clk_freq)
+{
+	struct device *dev = &fimc->pdev->dev;
+	struct device_node *node = dev->of_node;
+	const struct of_device_id *of_id;
+	struct fimc_variant *v;
+	struct fimc_pix_limit *lim;
+	u32 args[FIMC_PIX_LIMITS_MAX];
+	int ret;
+
+	if (of_property_read_bool(node, "samsung,lcd-wb"))
+		return -ENODEV;
+
+	v = devm_kzalloc(dev, sizeof(*v) + sizeof(*lim), GFP_KERNEL);
+	if (!v)
+		return -ENOMEM;
+
+	of_id = of_match_node(fimc_of_match, node);
+	if (!of_id)
+		return -EINVAL;
+	fimc->drv_data = of_id->data;
+	ret = of_property_read_u32_array(node, "samsung,pix-limits",
+					 args, FIMC_PIX_LIMITS_MAX);
+	if (ret < 0)
+		return ret;
+
+	lim = (struct fimc_pix_limit *)&v[1];
+
+	lim->scaler_en_w = args[0];
+	lim->scaler_dis_w = args[1];
+	lim->out_rot_en_w = args[2];
+	lim->out_rot_dis_w = args[3];
+	v->pix_limit = lim;
+
+	ret = of_property_read_u32_array(node, "samsung,min-pix-sizes",
+								args, 2);
+	v->min_inp_pixsize = ret ? FIMC_DEF_MIN_SIZE : args[0];
+	v->min_out_pixsize = ret ? FIMC_DEF_MIN_SIZE : args[1];
+	ret = of_property_read_u32_array(node, "samsung,min-pix-alignment",
+								args, 2);
+	v->min_vsize_align = ret ? FIMC_DEF_HEIGHT_ALIGN : args[0];
+	v->hor_offs_align = ret ? FIMC_DEF_HOR_OFFS_ALIGN : args[1];
+
+	ret = of_property_read_u32(node, "samsung,rotators", &args[1]);
+	v->has_inp_rot = ret ? 1 : args[1] & 0x01;
+	v->has_out_rot = ret ? 1 : args[1] & 0x10;
+	v->has_mainscaler_ext = of_property_read_bool(node,
+					"samsung,mainscaler-ext");
+
+	v->has_isp_wb = of_property_read_bool(node, "samsung,isp-wb");
+	v->has_cam_if = of_property_read_bool(node, "samsung,cam-if");
+	of_property_read_u32(node, "clock-frequency", clk_freq);
+	fimc->id = of_alias_get_id(node, "fimc");
+
+	fimc->variant = v;
 	return 0;
 }
 
 static int fimc_probe(struct platform_device *pdev)
 {
-	const struct fimc_drvdata *drv_data = fimc_get_drvdata(pdev);
-	struct s5p_platform_fimc *pdata;
+	struct device *dev = &pdev->dev;
+	u32 lclk_freq = 0;
 	struct fimc_dev *fimc;
 	struct resource *res;
 	int ret = 0;
 
-	if (pdev->id >= drv_data->num_entities) {
-		dev_err(&pdev->dev, "Invalid platform device id: %d\n",
-			pdev->id);
-		return -EINVAL;
-	}
-
-	fimc = devm_kzalloc(&pdev->dev, sizeof(*fimc), GFP_KERNEL);
+	fimc = devm_kzalloc(dev, sizeof(*fimc), GFP_KERNEL);
 	if (!fimc)
 		return -ENOMEM;
 
-	fimc->id = pdev->id;
-
-	fimc->variant = drv_data->variant[fimc->id];
 	fimc->pdev = pdev;
-	pdata = pdev->dev.platform_data;
-	fimc->pdata = pdata;
+
+	if (dev->of_node) {
+		ret = fimc_parse_dt(fimc, &lclk_freq);
+		if (ret < 0)
+			return ret;
+	} else {
+		fimc->drv_data = fimc_get_drvdata(pdev);
+		fimc->id = pdev->id;
+	}
+	if (!fimc->drv_data || fimc->id >= fimc->drv_data->num_entities ||
+	    fimc->id < 0) {
+		dev_err(dev, "Invalid driver data or device id (%d)\n",
+			fimc->id);
+		return -EINVAL;
+	}
+	if (!dev->of_node)
+		fimc->variant = fimc->drv_data->variant[fimc->id];
 
 	init_waitqueue_head(&fimc->irq_queue);
 	spin_lock_init(&fimc->slock);
 	mutex_init(&fimc->lock);
 
+	fimc->sysreg = fimc_get_sysreg_regmap(dev->of_node);
+	if (IS_ERR(fimc->sysreg))
+		return PTR_ERR(fimc->sysreg);
+
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	fimc->regs = devm_ioremap_resource(&pdev->dev, res);
+	fimc->regs = devm_ioremap_resource(dev, res);
 	if (IS_ERR(fimc->regs))
 		return PTR_ERR(fimc->regs);
 
 	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	if (res == NULL) {
-		dev_err(&pdev->dev, "Failed to get IRQ resource\n");
+		dev_err(dev, "Failed to get IRQ resource\n");
 		return -ENXIO;
 	}
 
@@ -907,7 +996,10 @@ static int fimc_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-	ret = clk_set_rate(fimc->clock[CLK_BUS], drv_data->lclk_frequency);
+	if (lclk_freq == 0)
+		lclk_freq = fimc->drv_data->lclk_frequency;
+
+	ret = clk_set_rate(fimc->clock[CLK_BUS], lclk_freq);
 	if (ret < 0)
 		return ret;
 
@@ -915,10 +1007,10 @@ static int fimc_probe(struct platform_device *pdev)
 	if (ret < 0)
 		return ret;
 
-	ret = devm_request_irq(&pdev->dev, res->start, fimc_irq_handler,
-			       0, dev_name(&pdev->dev), fimc);
+	ret = devm_request_irq(dev, res->start, fimc_irq_handler,
+			       0, dev_name(dev), fimc);
 	if (ret) {
-		dev_err(&pdev->dev, "failed to install irq (%d)\n", ret);
+		dev_err(dev, "failed to install irq (%d)\n", ret);
 		goto err_clk;
 	}
 
@@ -927,23 +1019,23 @@ static int fimc_probe(struct platform_device *pdev)
 		goto err_clk;
 
 	platform_set_drvdata(pdev, fimc);
-	pm_runtime_enable(&pdev->dev);
-	ret = pm_runtime_get_sync(&pdev->dev);
+	pm_runtime_enable(dev);
+	ret = pm_runtime_get_sync(dev);
 	if (ret < 0)
 		goto err_sd;
 	/* Initialize contiguous memory allocator */
-	fimc->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	fimc->alloc_ctx = vb2_dma_contig_init_ctx(dev);
 	if (IS_ERR(fimc->alloc_ctx)) {
 		ret = PTR_ERR(fimc->alloc_ctx);
 		goto err_pm;
 	}
 
-	dev_dbg(&pdev->dev, "FIMC.%d registered successfully\n", fimc->id);
+	dev_dbg(dev, "FIMC.%d registered successfully\n", fimc->id);
 
-	pm_runtime_put(&pdev->dev);
+	pm_runtime_put(dev);
 	return 0;
 err_pm:
-	pm_runtime_put(&pdev->dev);
+	pm_runtime_put(dev);
 err_sd:
 	fimc_unregister_capture_subdev(fimc);
 err_clk:
@@ -1046,35 +1138,21 @@ static const struct fimc_pix_limit s5p_pix_limit[4] = {
 	[0] = {
 		.scaler_en_w	= 3264,
 		.scaler_dis_w	= 8192,
-		.in_rot_en_h	= 1920,
-		.in_rot_dis_w	= 8192,
 		.out_rot_en_w	= 1920,
 		.out_rot_dis_w	= 4224,
 	},
 	[1] = {
 		.scaler_en_w	= 4224,
 		.scaler_dis_w	= 8192,
-		.in_rot_en_h	= 1920,
-		.in_rot_dis_w	= 8192,
 		.out_rot_en_w	= 1920,
 		.out_rot_dis_w	= 4224,
 	},
 	[2] = {
 		.scaler_en_w	= 1920,
 		.scaler_dis_w	= 8192,
-		.in_rot_en_h	= 1280,
-		.in_rot_dis_w	= 8192,
 		.out_rot_en_w	= 1280,
 		.out_rot_dis_w	= 1920,
 	},
-	[3] = {
-		.scaler_en_w	= 1920,
-		.scaler_dis_w	= 8192,
-		.in_rot_en_h	= 1366,
-		.in_rot_dis_w	= 8192,
-		.out_rot_en_w	= 1366,
-		.out_rot_dis_w	= 1920,
-	},
 };
 
 static const struct fimc_variant fimc0_variant_s5p = {
@@ -1085,7 +1163,6 @@ static const struct fimc_variant fimc0_variant_s5p = {
 	.min_out_pixsize = 16,
 	.hor_offs_align	 = 8,
 	.min_vsize_align = 16,
-	.out_buf_count	 = 4,
 	.pix_limit	 = &s5p_pix_limit[0],
 };
 
@@ -1095,12 +1172,10 @@ static const struct fimc_variant fimc2_variant_s5p = {
 	.min_out_pixsize = 16,
 	.hor_offs_align	 = 8,
 	.min_vsize_align = 16,
-	.out_buf_count	 = 4,
 	.pix_limit	 = &s5p_pix_limit[1],
 };
 
 static const struct fimc_variant fimc0_variant_s5pv210 = {
-	.pix_hoff	 = 1,
 	.has_inp_rot	 = 1,
 	.has_out_rot	 = 1,
 	.has_cam_if	 = 1,
@@ -1108,12 +1183,10 @@ static const struct fimc_variant fimc0_variant_s5pv210 = {
 	.min_out_pixsize = 16,
 	.hor_offs_align	 = 8,
 	.min_vsize_align = 16,
-	.out_buf_count	 = 4,
 	.pix_limit	 = &s5p_pix_limit[1],
 };
 
 static const struct fimc_variant fimc1_variant_s5pv210 = {
-	.pix_hoff	 = 1,
 	.has_inp_rot	 = 1,
 	.has_out_rot	 = 1,
 	.has_cam_if	 = 1,
@@ -1122,80 +1195,18 @@ static const struct fimc_variant fimc1_variant_s5pv210 = {
 	.min_out_pixsize = 16,
 	.hor_offs_align	 = 1,
 	.min_vsize_align = 1,
-	.out_buf_count	 = 4,
 	.pix_limit	 = &s5p_pix_limit[2],
 };
 
 static const struct fimc_variant fimc2_variant_s5pv210 = {
 	.has_cam_if	 = 1,
-	.pix_hoff	 = 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[2],
 };
 
-static const struct fimc_variant fimc0_variant_exynos4210 = {
-	.pix_hoff	 = 1,
-	.has_inp_rot	 = 1,
-	.has_out_rot	 = 1,
-	.has_cam_if	 = 1,
-	.has_cistatus2	 = 1,
-	.has_mainscaler_ext = 1,
-	.has_alpha	 = 1,
-	.min_inp_pixsize = 16,
-	.min_out_pixsize = 16,
-	.hor_offs_align	 = 2,
-	.min_vsize_align = 1,
-	.out_buf_count	 = 32,
-	.pix_limit	 = &s5p_pix_limit[1],
-};
-
-static const struct fimc_variant fimc3_variant_exynos4210 = {
-	.pix_hoff	 = 1,
-	.has_cistatus2	 = 1,
-	.has_mainscaler_ext = 1,
-	.has_alpha	 = 1,
-	.min_inp_pixsize = 16,
-	.min_out_pixsize = 16,
-	.hor_offs_align	 = 2,
-	.min_vsize_align = 1,
-	.out_buf_count	 = 32,
-	.pix_limit	 = &s5p_pix_limit[3],
-};
-
-static const struct fimc_variant fimc0_variant_exynos4x12 = {
-	.pix_hoff		= 1,
-	.has_inp_rot		= 1,
-	.has_out_rot		= 1,
-	.has_cam_if		= 1,
-	.has_isp_wb		= 1,
-	.has_cistatus2		= 1,
-	.has_mainscaler_ext	= 1,
-	.has_alpha		= 1,
-	.min_inp_pixsize	= 16,
-	.min_out_pixsize	= 16,
-	.hor_offs_align		= 2,
-	.min_vsize_align	= 1,
-	.out_buf_count		= 32,
-	.pix_limit		= &s5p_pix_limit[1],
-};
-
-static const struct fimc_variant fimc3_variant_exynos4x12 = {
-	.pix_hoff		= 1,
-	.has_cistatus2		= 1,
-	.has_mainscaler_ext	= 1,
-	.has_alpha		= 1,
-	.min_inp_pixsize	= 16,
-	.min_out_pixsize	= 16,
-	.hor_offs_align		= 2,
-	.min_vsize_align	= 1,
-	.out_buf_count		= 32,
-	.pix_limit		= &s5p_pix_limit[3],
-};
-
 /* S5PC100 */
 static const struct fimc_drvdata fimc_drvdata_s5p = {
 	.variant = {
@@ -1203,8 +1214,9 @@ static const struct fimc_drvdata fimc_drvdata_s5p = {
 		[1] = &fimc0_variant_s5p,
 		[2] = &fimc2_variant_s5p,
 	},
-	.num_entities = 3,
+	.num_entities	= 3,
 	.lclk_frequency = 133000000UL,
+	.out_buf_count	= 4,
 };
 
 /* S5PV210, S5PC110 */
@@ -1214,32 +1226,30 @@ static const struct fimc_drvdata fimc_drvdata_s5pv210 = {
 		[1] = &fimc1_variant_s5pv210,
 		[2] = &fimc2_variant_s5pv210,
 	},
-	.num_entities = 3,
-	.lclk_frequency = 166000000UL,
+	.num_entities	= 3,
+	.lclk_frequency	= 166000000UL,
+	.out_buf_count	= 4,
+	.dma_pix_hoff	= 1,
 };
 
 /* EXYNOS4210, S5PV310, S5PC210 */
 static const struct fimc_drvdata fimc_drvdata_exynos4210 = {
-	.variant = {
-		[0] = &fimc0_variant_exynos4210,
-		[1] = &fimc0_variant_exynos4210,
-		[2] = &fimc0_variant_exynos4210,
-		[3] = &fimc3_variant_exynos4210,
-	},
-	.num_entities = 4,
+	.num_entities	= 4,
 	.lclk_frequency = 166000000UL,
+	.dma_pix_hoff	= 1,
+	.cistatus2	= 1,
+	.alpha_color	= 1,
+	.out_buf_count	= 32,
 };
 
 /* EXYNOS4212, EXYNOS4412 */
 static const struct fimc_drvdata fimc_drvdata_exynos4x12 = {
-	.variant = {
-		[0] = &fimc0_variant_exynos4x12,
-		[1] = &fimc0_variant_exynos4x12,
-		[2] = &fimc0_variant_exynos4x12,
-		[3] = &fimc3_variant_exynos4x12,
-	},
-	.num_entities = 4,
-	.lclk_frequency = 166000000UL,
+	.num_entities	= 4,
+	.lclk_frequency	= 166000000UL,
+	.dma_pix_hoff	= 1,
+	.cistatus2	= 1,
+	.alpha_color	= 1,
+	.out_buf_count	= 32,
 };
 
 static const struct platform_device_id fimc_driver_ids[] = {
@@ -1256,9 +1266,22 @@ static const struct platform_device_id fimc_driver_ids[] = {
 		.name		= "exynos4x12-fimc",
 		.driver_data	= (unsigned long)&fimc_drvdata_exynos4x12,
 	},
-	{},
+	{ },
+};
+
+static const struct of_device_id fimc_of_match[] = {
+	{
+		.compatible = "samsung,s5pv210-fimc",
+		.data = &fimc_drvdata_s5pv210,
+	}, {
+		.compatible = "samsung,exynos4210-fimc",
+		.data = &fimc_drvdata_exynos4210,
+	}, {
+		.compatible = "samsung,exynos4212-fimc",
+		.data = &fimc_drvdata_exynos4x12,
+	},
+	{ /* sentinel */ },
 };
-MODULE_DEVICE_TABLE(platform, fimc_driver_ids);
 
 static const struct dev_pm_ops fimc_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(fimc_suspend, fimc_resume)
@@ -1270,9 +1293,10 @@ static struct platform_driver fimc_driver = {
 	.remove		= fimc_remove,
 	.id_table	= fimc_driver_ids,
 	.driver = {
-		.name	= FIMC_MODULE_NAME,
-		.owner	= THIS_MODULE,
-		.pm     = &fimc_pm_ops,
+		.of_match_table = fimc_of_match,
+		.name		= FIMC_DRIVER_NAME,
+		.owner		= THIS_MODULE,
+		.pm     	= &fimc_pm_ops,
 	}
 };
 
diff --git a/drivers/media/platform/s5p-fimc/fimc-core.h b/drivers/media/platform/exynos4-is/fimc-core.h
index 412d50708f7..539a3f71c16 100644
--- a/drivers/media/platform/s5p-fimc/fimc-core.h
+++ b/drivers/media/platform/exynos4-is/fimc-core.h
@@ -12,8 +12,10 @@
 /*#define DEBUG*/
 
 #include <linux/platform_device.h>
+#include <linux/regmap.h>
 #include <linux/sched.h>
 #include <linux/spinlock.h>
+#include <linux/mfd/syscon.h>
 #include <linux/types.h>
 #include <linux/videodev2.h>
 #include <linux/io.h>
@@ -33,7 +35,7 @@
 /* Time to wait for next frame VSYNC interrupt while stopping operation. */
 #define FIMC_SHUTDOWN_TIMEOUT	((100*HZ)/1000)
 #define MAX_FIMC_CLOCKS		2
-#define FIMC_MODULE_NAME	"s5p-fimc"
+#define FIMC_DRIVER_NAME	"exynos4-fimc"
 #define FIMC_MAX_DEVS		4
 #define FIMC_MAX_OUT_BUFS	4
 #define SCALER_MAX_HRATIO	64
@@ -42,6 +44,10 @@
 #define FIMC_CAMIF_MAX_HEIGHT	0x2000
 #define FIMC_MAX_JPEG_BUF_SIZE	(10 * SZ_1M)
 #define FIMC_MAX_PLANES		3
+#define FIMC_PIX_LIMITS_MAX	4
+#define FIMC_DEF_MIN_SIZE	16
+#define FIMC_DEF_HEIGHT_ALIGN	2
+#define FIMC_DEF_HOR_OFFS_ALIGN	1
 
 /* indices to the clocks array */
 enum {
@@ -132,36 +138,6 @@ enum fimc_color_fmt {
 #define	FIMC_COLOR_RANGE_NARROW		(1 << 3)
 
 /**
- * struct fimc_fmt - the driver's internal color format data
- * @mbus_code: Media Bus pixel code, -1 if not applicable
- * @name: format description
- * @fourcc: the fourcc code for this format, 0 if not applicable
- * @color: the corresponding fimc_color_fmt
- * @memplanes: number of physically non-contiguous data planes
- * @colplanes: number of physically contiguous data planes
- * @depth: per plane driver's private 'number of bits per pixel'
- * @mdataplanes: bitmask indicating meta data plane(s), (1 << plane_no)
- * @flags: flags indicating which operation mode format applies to
- */
-struct fimc_fmt {
-	enum v4l2_mbus_pixelcode mbus_code;
-	char	*name;
-	u32	fourcc;
-	u32	color;
-	u16	memplanes;
-	u16	colplanes;
-	u8	depth[VIDEO_MAX_PLANES];
-	u16	mdataplanes;
-	u16	flags;
-#define FMT_FLAGS_CAM		(1 << 0)
-#define FMT_FLAGS_M2M_IN	(1 << 1)
-#define FMT_FLAGS_M2M_OUT	(1 << 2)
-#define FMT_FLAGS_M2M		(1 << 1 | 1 << 2)
-#define FMT_HAS_ALPHA		(1 << 3)
-#define FMT_FLAGS_COMPRESSED	(1 << 4)
-};
-
-/**
  * struct fimc_dma_offset - pixel offset information for DMA
  * @y_h:	y value horizontal offset
  * @y_v:	y value vertical offset
@@ -299,9 +275,10 @@ struct fimc_m2m_device {
 	int			refcnt;
 };
 
-#define FIMC_SD_PAD_SINK	0
-#define FIMC_SD_PAD_SOURCE	1
-#define FIMC_SD_PADS_NUM	2
+#define FIMC_SD_PAD_SINK_CAM	0
+#define FIMC_SD_PAD_SINK_FIFO	1
+#define FIMC_SD_PAD_SOURCE	2
+#define FIMC_SD_PADS_NUM	3
 
 /**
  * struct fimc_vid_cap - camera capture device information
@@ -310,7 +287,9 @@ struct fimc_m2m_device {
  * @subdev: subdev exposing the FIMC processing block
  * @vd_pad: fimc video capture node pad
  * @sd_pads: fimc video processing block pads
- * @mf: media bus format at the FIMC camera input (and the scaler output) pad
+ * @ci_fmt: image format at the FIMC camera input (and the scaler output)
+ * @wb_fmt: image format at the FIMC ISP Writeback input
+ * @source_config: external image source related configuration structure
  * @pending_buf_q: the pending buffer queue head
  * @active_buf_q: the queue head of buffers scheduled in hardware
  * @vbq: the capture am video buffer queue
@@ -329,8 +308,10 @@ struct fimc_vid_cap {
 	struct video_device		vfd;
 	struct v4l2_subdev		subdev;
 	struct media_pad		vd_pad;
-	struct v4l2_mbus_framefmt	mf;
 	struct media_pad		sd_pads[FIMC_SD_PADS_NUM];
+	struct v4l2_mbus_framefmt	ci_fmt;
+	struct v4l2_mbus_framefmt	wb_fmt;
+	struct fimc_source_info		source_config;
 	struct list_head		pending_buf_q;
 	struct list_head		active_buf_q;
 	struct vb2_queue		vbq;
@@ -338,6 +319,7 @@ struct fimc_vid_cap {
 	int				buf_index;
 	unsigned int			frame_count;
 	unsigned int			reqbufs_count;
+	bool				streaming;
 	int				input_index;
 	int				refcnt;
 	u32				input;
@@ -365,10 +347,8 @@ struct fimc_pix_limit {
 
 /**
  * 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
- * @has_cistatus2: 1 if CISTATUS2 register is present in this IP revision
  * @has_mainscaler_ext: 1 if extended mainscaler ratios in CIEXTEN register
  *			 are present in this IP revision
  * @has_cam_if: set if this instance has a camera input interface
@@ -378,23 +358,18 @@ struct fimc_pix_limit {
  * @min_out_pixsize: minimum output pixel size
  * @hor_offs_align: horizontal pixel offset aligment
  * @min_vsize_align: minimum vertical pixel size alignment
- * @out_buf_count: the number of buffers in output DMA sequence
  */
 struct fimc_variant {
-	unsigned int	pix_hoff:1;
 	unsigned int	has_inp_rot:1;
 	unsigned int	has_out_rot:1;
-	unsigned int	has_cistatus2:1;
 	unsigned int	has_mainscaler_ext:1;
 	unsigned int	has_cam_if:1;
 	unsigned int	has_isp_wb:1;
-	unsigned int	has_alpha:1;
 	const struct fimc_pix_limit *pix_limit;
 	u16		min_inp_pixsize;
 	u16		min_out_pixsize;
 	u16		hor_offs_align;
 	u16		min_vsize_align;
-	u16		out_buf_count;
 };
 
 /**
@@ -402,11 +377,20 @@ struct fimc_variant {
  * @variant: variant information for this device
  * @num_entities: number of fimc instances available in a SoC
  * @lclk_frequency: local bus clock frequency
+ * @cistatus2: 1 if the FIMC IPs have CISTATUS2 register
+ * @dma_pix_hoff: the horizontal DMA offset unit: 1 - pixels, 0 - bytes
+ * @alpha_color: 1 if alpha color component is supported
+ * @out_buf_count: maximum number of output DMA buffers supported
  */
 struct fimc_drvdata {
 	const struct fimc_variant *variant[FIMC_MAX_DEVS];
 	int num_entities;
 	unsigned long lclk_frequency;
+	/* Fields common to all FIMC IP instances */
+	u8 cistatus2;
+	u8 dma_pix_hoff;
+	u8 alpha_color;
+	u8 out_buf_count;
 };
 
 #define fimc_get_drvdata(_pdev) \
@@ -420,6 +404,7 @@ struct fimc_ctx;
  * @lock:	the mutex protecting this data structure
  * @pdev:	pointer to the FIMC platform device
  * @pdata:	pointer to the device platform data
+ * @sysreg:	pointer to the SYSREG regmap
  * @variant:	the IP variant information
  * @id:		FIMC device index (0..FIMC_MAX_DEVS)
  * @clock:	clocks required for FIMC operation
@@ -437,8 +422,10 @@ struct fimc_dev {
 	struct mutex			lock;
 	struct platform_device		*pdev;
 	struct s5p_platform_fimc	*pdata;
+	struct regmap			*sysreg;
 	const struct fimc_variant	*variant;
-	u16				id;
+	const struct fimc_drvdata	*drv_data;
+	int				id;
 	struct clk			*clock[MAX_FIMC_CLOCKS];
 	void __iomem			*regs;
 	wait_queue_head_t		irq_queue;
@@ -633,6 +620,8 @@ static inline struct fimc_frame *ctx_get_frame(struct fimc_ctx *ctx,
 /* fimc-core.c */
 int fimc_vidioc_enum_fmt_mplane(struct file *file, void *priv,
 				struct v4l2_fmtdesc *f);
+void __fimc_vidioc_querycap(struct device *dev, struct v4l2_capability *cap,
+						unsigned int caps);
 int fimc_ctrls_create(struct fimc_ctx *ctx);
 void fimc_ctrls_delete(struct fimc_ctx *ctx);
 void fimc_ctrls_activate(struct fimc_ctx *ctx, bool active);
@@ -660,6 +649,15 @@ void fimc_unregister_m2m_device(struct fimc_dev *fimc);
 int fimc_register_driver(void);
 void fimc_unregister_driver(void);
 
+#ifdef CONFIG_MFD_SYSCON
+static inline struct regmap * fimc_get_sysreg_regmap(struct device_node *node)
+{
+	return syscon_regmap_lookup_by_phandle(node, "samsung,sysreg");
+}
+#else
+#define fimc_get_sysreg_regmap(node) (NULL)
+#endif
+
 /* -----------------------------------------------------*/
 /* fimc-m2m.c */
 void fimc_m2m_job_finish(struct fimc_ctx *ctx, int vb_state);
diff --git a/drivers/media/platform/exynos4-is/fimc-is-command.h b/drivers/media/platform/exynos4-is/fimc-is-command.h
new file mode 100644
index 00000000000..0d1f52e394b
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-command.h
@@ -0,0 +1,137 @@
+/*
+ * Samsung Exynos4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * FIMC-IS command set definitions
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Younghwan Joo <yhwan.joo@samsung.com>
+ *          Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef FIMC_IS_CMD_H_
+#define FIMC_IS_CMD_H_
+
+#define FIMC_IS_COMMAND_VER	110 /* FIMC-IS command set version 1.10 */
+
+/* Enumeration of commands beetween the FIMC-IS and the host processor. */
+
+/* HOST to FIMC-IS */
+#define HIC_PREVIEW_STILL	0x0001
+#define HIC_PREVIEW_VIDEO	0x0002
+#define HIC_CAPTURE_STILL	0x0003
+#define HIC_CAPTURE_VIDEO	0x0004
+#define HIC_STREAM_ON		0x0005
+#define HIC_STREAM_OFF		0x0006
+#define HIC_SET_PARAMETER	0x0007
+#define HIC_GET_PARAMETER	0x0008
+#define HIC_SET_TUNE		0x0009
+#define HIC_GET_STATUS		0x000b
+/* Sensor part */
+#define HIC_OPEN_SENSOR		0x000c
+#define HIC_CLOSE_SENSOR	0x000d
+#define HIC_SIMMIAN_INIT	0x000e
+#define HIC_SIMMIAN_WRITE	0x000f
+#define HIC_SIMMIAN_READ	0x0010
+#define HIC_POWER_DOWN		0x0011
+#define HIC_GET_SET_FILE_ADDR	0x0012
+#define HIC_LOAD_SET_FILE	0x0013
+#define HIC_MSG_CONFIG		0x0014
+#define HIC_MSG_TEST		0x0015
+/* FIMC-IS to HOST */
+#define IHC_GET_SENSOR_NUM	0x1000
+#define IHC_SET_SHOT_MARK	0x1001
+/* parameter1: frame number */
+/* parameter2: confidence level (smile 0~100) */
+/* parameter3: confidence level (blink 0~100) */
+#define IHC_SET_FACE_MARK	0x1002
+/* parameter1: coordinate count */
+/* parameter2: coordinate buffer address */
+#define IHC_FRAME_DONE		0x1003
+/* parameter1: frame start number */
+/* parameter2: frame count */
+#define IHC_AA_DONE		0x1004
+#define IHC_NOT_READY		0x1005
+
+#define IH_REPLY_DONE		0x2000
+#define IH_REPLY_NOT_DONE	0x2001
+
+enum fimc_is_scenario {
+	IS_SC_PREVIEW_STILL,
+	IS_SC_PREVIEW_VIDEO,
+	IS_SC_CAPTURE_STILL,
+	IS_SC_CAPTURE_VIDEO,
+	IS_SC_MAX
+};
+
+enum fimc_is_sub_scenario {
+	IS_SC_SUB_DEFAULT,
+	IS_SC_SUB_PS_VTCALL,
+	IS_SC_SUB_CS_VTCALL,
+	IS_SC_SUB_PV_VTCALL,
+	IS_SC_SUB_CV_VTCALL,
+};
+
+struct is_common_regs {
+	u32 hicmd;
+	u32 hic_sensorid;
+	u32 hic_param[4];
+	u32 reserved1[4];
+
+	u32 ihcmd;
+	u32 ihc_sensorid;
+	u32 ihc_param[4];
+	u32 reserved2[4];
+
+	u32 isp_sensor_id;
+	u32 isp_param[2];
+	u32 reserved3[1];
+
+	u32 scc_sensor_id;
+	u32 scc_param[2];
+	u32 reserved4[1];
+
+	u32 dnr_sensor_id;
+	u32 dnr_param[2];
+	u32 reserved5[1];
+
+	u32 scp_sensor_id;
+	u32 scp_param[2];
+	u32 reserved6[29];
+} __packed;
+
+struct is_mcuctl_reg {
+	u32 mcuctl;
+	u32 bboar;
+
+	u32 intgr0;
+	u32 intcr0;
+	u32 intmr0;
+	u32 intsr0;
+	u32 intmsr0;
+
+	u32 intgr1;
+	u32 intcr1;
+	u32 intmr1;
+	u32 intsr1;
+	u32 intmsr1;
+
+	u32 intcr2;
+	u32 intmr2;
+	u32 intsr2;
+	u32 intmsr2;
+
+	u32 gpoctrl;
+	u32 cpoenctlr;
+	u32 gpictlr;
+
+	u32 reserved[0xd];
+
+	struct is_common_regs common;
+} __packed;
+
+#endif /* FIMC_IS_CMD_H_ */
diff --git a/drivers/media/platform/exynos4-is/fimc-is-errno.c b/drivers/media/platform/exynos4-is/fimc-is-errno.c
new file mode 100644
index 00000000000..e8519e151c1
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-errno.c
@@ -0,0 +1,272 @@
+/*
+ * Samsung Exynos4 SoC series FIMC-IS slave interface driver
+ *
+ * Error log interface functions
+ *
+ * Copyright (C) 2011 - 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Younghwan Joo <yhwan.joo@samsung.com>
+ *          Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include "fimc-is-errno.h"
+
+const char * const fimc_is_param_strerr(unsigned int error)
+{
+	switch (error) {
+	case ERROR_COMMON_CMD:
+		return "ERROR_COMMON_CMD: Invalid Command";
+	case ERROR_COMMON_PARAMETER:
+		return "ERROR_COMMON_PARAMETER: Invalid Parameter";
+	case ERROR_COMMON_SETFILE_LOAD:
+		return "ERROR_COMMON_SETFILE_LOAD: Illegal Setfile Loading";
+	case ERROR_COMMON_SETFILE_ADJUST:
+		return "ERROR_COMMON_SETFILE_ADJUST: Setfile isn't adjusted";
+	case ERROR_COMMON_SETFILE_INDEX:
+		return "ERROR_COMMON_SETFILE_INDEX: Invalid setfile index";
+	case ERROR_COMMON_INPUT_PATH:
+		return "ERROR_COMMON_INPUT_PATH: Input path can be changed in ready state";
+	case ERROR_COMMON_INPUT_INIT:
+		return "ERROR_COMMON_INPUT_INIT: IP can not start if input path is not set";
+	case ERROR_COMMON_OUTPUT_PATH:
+		return "ERROR_COMMON_OUTPUT_PATH: Output path can be changed in ready state (stop)";
+	case ERROR_COMMON_OUTPUT_INIT:
+		return "ERROR_COMMON_OUTPUT_INIT: IP can not start if output path is not set";
+	case ERROR_CONTROL_BYPASS:
+		return "ERROR_CONTROL_BYPASS";
+	case ERROR_OTF_INPUT_FORMAT:
+		return "ERROR_OTF_INPUT_FORMAT: Invalid format  (DRC: YUV444, FD: YUV444, 422, 420)";
+	case ERROR_OTF_INPUT_WIDTH:
+		return "ERROR_OTF_INPUT_WIDTH: Invalid width (DRC: 128~8192, FD: 32~8190)";
+	case ERROR_OTF_INPUT_HEIGHT:
+		return "ERROR_OTF_INPUT_HEIGHT: Invalid bit-width (DRC: 8~12bits, FD: 8bit)";
+	case ERROR_OTF_INPUT_BIT_WIDTH:
+		return "ERROR_OTF_INPUT_BIT_WIDTH: Invalid bit-width (DRC: 8~12bits, FD: 8bit)";
+	case ERROR_DMA_INPUT_WIDTH:
+		return "ERROR_DMA_INPUT_WIDTH: Invalid width (DRC: 128~8192, FD: 32~8190)";
+	case ERROR_DMA_INPUT_HEIGHT:
+		return "ERROR_DMA_INPUT_HEIGHT: Invalid height (DRC: 64~8192, FD: 16~8190)";
+	case ERROR_DMA_INPUT_FORMAT:
+		return "ERROR_DMA_INPUT_FORMAT: Invalid format (DRC: YUV444 or YUV422, FD: YUV444,422,420)";
+	case ERROR_DMA_INPUT_BIT_WIDTH:
+		return "ERROR_DMA_INPUT_BIT_WIDTH: Invalid bit-width (DRC: 8~12bits, FD: 8bit)";
+	case ERROR_DMA_INPUT_ORDER:
+		return "ERROR_DMA_INPUT_ORDER: Invalid order(DRC: YYCbCr,YCbYCr,FD:NO,YYCbCr,YCbYCr,CbCr,CrCb)";
+	case ERROR_DMA_INPUT_PLANE:
+		return "ERROR_DMA_INPUT_PLANE: Invalid palne (DRC: 3, FD: 1, 2, 3)";
+	case ERROR_OTF_OUTPUT_WIDTH:
+		return "ERROR_OTF_OUTPUT_WIDTH: Invalid width (DRC: 128~8192)";
+	case ERROR_OTF_OUTPUT_HEIGHT:
+		return "ERROR_OTF_OUTPUT_HEIGHT: Invalid height (DRC: 64~8192)";
+	case ERROR_OTF_OUTPUT_FORMAT:
+		return "ERROR_OTF_OUTPUT_FORMAT: Invalid format (DRC: YUV444)";
+	case ERROR_OTF_OUTPUT_BIT_WIDTH:
+		return "ERROR_OTF_OUTPUT_BIT_WIDTH: Invalid bit-width (DRC: 8~12bits, FD: 8bit)";
+	case ERROR_DMA_OUTPUT_WIDTH:
+		return "ERROR_DMA_OUTPUT_WIDTH";
+	case ERROR_DMA_OUTPUT_HEIGHT:
+		return "ERROR_DMA_OUTPUT_HEIGHT";
+	case ERROR_DMA_OUTPUT_FORMAT:
+		return "ERROR_DMA_OUTPUT_FORMAT";
+	case ERROR_DMA_OUTPUT_BIT_WIDTH:
+		return "ERROR_DMA_OUTPUT_BIT_WIDTH";
+	case ERROR_DMA_OUTPUT_PLANE:
+		return "ERROR_DMA_OUTPUT_PLANE";
+	case ERROR_DMA_OUTPUT_ORDER:
+		return "ERROR_DMA_OUTPUT_ORDER";
+
+	/* Sensor Error(100~199) */
+	case ERROR_SENSOR_I2C_FAIL:
+		return "ERROR_SENSOR_I2C_FAIL";
+	case ERROR_SENSOR_INVALID_FRAMERATE:
+		return "ERROR_SENSOR_INVALID_FRAMERATE";
+	case ERROR_SENSOR_INVALID_EXPOSURETIME:
+		return "ERROR_SENSOR_INVALID_EXPOSURETIME";
+	case ERROR_SENSOR_INVALID_SIZE:
+		return "ERROR_SENSOR_INVALID_SIZE";
+	case ERROR_SENSOR_INVALID_SETTING:
+		return "ERROR_SENSOR_INVALID_SETTING";
+	case ERROR_SENSOR_ACTURATOR_INIT_FAIL:
+		return "ERROR_SENSOR_ACTURATOR_INIT_FAIL";
+	case ERROR_SENSOR_INVALID_AF_POS:
+		return "ERROR_SENSOR_INVALID_AF_POS";
+	case ERROR_SENSOR_UNSUPPORT_FUNC:
+		return "ERROR_SENSOR_UNSUPPORT_FUNC";
+	case ERROR_SENSOR_UNSUPPORT_PERI:
+		return "ERROR_SENSOR_UNSUPPORT_PERI";
+	case ERROR_SENSOR_UNSUPPORT_AF:
+		return "ERROR_SENSOR_UNSUPPORT_AF";
+
+	/* ISP Error (200~299) */
+	case ERROR_ISP_AF_BUSY:
+		return "ERROR_ISP_AF_BUSY";
+	case ERROR_ISP_AF_INVALID_COMMAND:
+		return "ERROR_ISP_AF_INVALID_COMMAND";
+	case ERROR_ISP_AF_INVALID_MODE:
+		return "ERROR_ISP_AF_INVALID_MODE";
+
+	/* DRC Error (300~399) */
+	/* FD Error  (400~499) */
+	case ERROR_FD_CONFIG_MAX_NUMBER_STATE:
+		return "ERROR_FD_CONFIG_MAX_NUMBER_STATE";
+	case ERROR_FD_CONFIG_MAX_NUMBER_INVALID:
+		return "ERROR_FD_CONFIG_MAX_NUMBER_INVALID";
+	case ERROR_FD_CONFIG_YAW_ANGLE_STATE:
+		return "ERROR_FD_CONFIG_YAW_ANGLE_STATE";
+	case ERROR_FD_CONFIG_YAW_ANGLE_INVALID:
+		return "ERROR_FD_CONFIG_YAW_ANGLE_INVALID\n";
+	case ERROR_FD_CONFIG_ROLL_ANGLE_STATE:
+		return "ERROR_FD_CONFIG_ROLL_ANGLE_STATE";
+	case ERROR_FD_CONFIG_ROLL_ANGLE_INVALID:
+		return "ERROR_FD_CONFIG_ROLL_ANGLE_INVALID";
+	case ERROR_FD_CONFIG_SMILE_MODE_INVALID:
+		return "ERROR_FD_CONFIG_SMILE_MODE_INVALID";
+	case ERROR_FD_CONFIG_BLINK_MODE_INVALID:
+		return "ERROR_FD_CONFIG_BLINK_MODE_INVALID";
+	case ERROR_FD_CONFIG_EYES_DETECT_INVALID:
+		return "ERROR_FD_CONFIG_EYES_DETECT_INVALID";
+	case ERROR_FD_CONFIG_MOUTH_DETECT_INVALID:
+		return "ERROR_FD_CONFIG_MOUTH_DETECT_INVALID";
+	case ERROR_FD_CONFIG_ORIENTATION_STATE:
+		return "ERROR_FD_CONFIG_ORIENTATION_STATE";
+	case ERROR_FD_CONFIG_ORIENTATION_INVALID:
+		return "ERROR_FD_CONFIG_ORIENTATION_INVALID";
+	case ERROR_FD_CONFIG_ORIENTATION_VALUE_INVALID:
+		return "ERROR_FD_CONFIG_ORIENTATION_VALUE_INVALID";
+	case ERROR_FD_RESULT:
+		return "ERROR_FD_RESULT";
+	case ERROR_FD_MODE:
+		return "ERROR_FD_MODE";
+	default:
+		return "Unknown";
+	}
+}
+
+const char * const fimc_is_strerr(unsigned int error)
+{
+	error &= ~IS_ERROR_TIME_OUT_FLAG;
+
+	switch (error) {
+	/* General */
+	case IS_ERROR_INVALID_COMMAND:
+		return "IS_ERROR_INVALID_COMMAND";
+	case IS_ERROR_REQUEST_FAIL:
+		return "IS_ERROR_REQUEST_FAIL";
+	case IS_ERROR_INVALID_SCENARIO:
+		return "IS_ERROR_INVALID_SCENARIO";
+	case IS_ERROR_INVALID_SENSORID:
+		return "IS_ERROR_INVALID_SENSORID";
+	case IS_ERROR_INVALID_MODE_CHANGE:
+		return "IS_ERROR_INVALID_MODE_CHANGE";
+	case IS_ERROR_INVALID_MAGIC_NUMBER:
+		return "IS_ERROR_INVALID_MAGIC_NUMBER";
+	case IS_ERROR_INVALID_SETFILE_HDR:
+		return "IS_ERROR_INVALID_SETFILE_HDR";
+	case IS_ERROR_BUSY:
+		return "IS_ERROR_BUSY";
+	case IS_ERROR_SET_PARAMETER:
+		return "IS_ERROR_SET_PARAMETER";
+	case IS_ERROR_INVALID_PATH:
+		return "IS_ERROR_INVALID_PATH";
+	case IS_ERROR_OPEN_SENSOR_FAIL:
+		return "IS_ERROR_OPEN_SENSOR_FAIL";
+	case IS_ERROR_ENTRY_MSG_THREAD_DOWN:
+		return "IS_ERROR_ENTRY_MSG_THREAD_DOWN";
+	case IS_ERROR_ISP_FRAME_END_NOT_DONE:
+		return "IS_ERROR_ISP_FRAME_END_NOT_DONE";
+	case IS_ERROR_DRC_FRAME_END_NOT_DONE:
+		return "IS_ERROR_DRC_FRAME_END_NOT_DONE";
+	case IS_ERROR_SCALERC_FRAME_END_NOT_DONE:
+		return "IS_ERROR_SCALERC_FRAME_END_NOT_DONE";
+	case IS_ERROR_ODC_FRAME_END_NOT_DONE:
+		return "IS_ERROR_ODC_FRAME_END_NOT_DONE";
+	case IS_ERROR_DIS_FRAME_END_NOT_DONE:
+		return "IS_ERROR_DIS_FRAME_END_NOT_DONE";
+	case IS_ERROR_TDNR_FRAME_END_NOT_DONE:
+		return "IS_ERROR_TDNR_FRAME_END_NOT_DONE";
+	case IS_ERROR_SCALERP_FRAME_END_NOT_DONE:
+		return "IS_ERROR_SCALERP_FRAME_END_NOT_DONE";
+	case IS_ERROR_WAIT_STREAM_OFF_NOT_DONE:
+		return "IS_ERROR_WAIT_STREAM_OFF_NOT_DONE";
+	case IS_ERROR_NO_MSG_IS_RECEIVED:
+		return "IS_ERROR_NO_MSG_IS_RECEIVED";
+	case IS_ERROR_SENSOR_MSG_FAIL:
+		return "IS_ERROR_SENSOR_MSG_FAIL";
+	case IS_ERROR_ISP_MSG_FAIL:
+		return "IS_ERROR_ISP_MSG_FAIL";
+	case IS_ERROR_DRC_MSG_FAIL:
+		return "IS_ERROR_DRC_MSG_FAIL";
+	case IS_ERROR_LHFD_MSG_FAIL:
+		return "IS_ERROR_LHFD_MSG_FAIL";
+	case IS_ERROR_UNKNOWN:
+		return "IS_ERROR_UNKNOWN";
+
+	/* Sensor */
+	case IS_ERROR_SENSOR_PWRDN_FAIL:
+		return "IS_ERROR_SENSOR_PWRDN_FAIL";
+
+	/* ISP */
+	case IS_ERROR_ISP_PWRDN_FAIL:
+		return "IS_ERROR_ISP_PWRDN_FAIL";
+	case IS_ERROR_ISP_MULTIPLE_INPUT:
+		return "IS_ERROR_ISP_MULTIPLE_INPUT";
+	case IS_ERROR_ISP_ABSENT_INPUT:
+		return "IS_ERROR_ISP_ABSENT_INPUT";
+	case IS_ERROR_ISP_ABSENT_OUTPUT:
+		return "IS_ERROR_ISP_ABSENT_OUTPUT";
+	case IS_ERROR_ISP_NONADJACENT_OUTPUT:
+		return "IS_ERROR_ISP_NONADJACENT_OUTPUT";
+	case IS_ERROR_ISP_FORMAT_MISMATCH:
+		return "IS_ERROR_ISP_FORMAT_MISMATCH";
+	case IS_ERROR_ISP_WIDTH_MISMATCH:
+		return "IS_ERROR_ISP_WIDTH_MISMATCH";
+	case IS_ERROR_ISP_HEIGHT_MISMATCH:
+		return "IS_ERROR_ISP_HEIGHT_MISMATCH";
+	case IS_ERROR_ISP_BITWIDTH_MISMATCH:
+		return "IS_ERROR_ISP_BITWIDTH_MISMATCH";
+	case IS_ERROR_ISP_FRAME_END_TIME_OUT:
+		return "IS_ERROR_ISP_FRAME_END_TIME_OUT";
+
+	/* DRC */
+	case IS_ERROR_DRC_PWRDN_FAIL:
+		return "IS_ERROR_DRC_PWRDN_FAIL";
+	case IS_ERROR_DRC_MULTIPLE_INPUT:
+		return "IS_ERROR_DRC_MULTIPLE_INPUT";
+	case IS_ERROR_DRC_ABSENT_INPUT:
+		return "IS_ERROR_DRC_ABSENT_INPUT";
+	case IS_ERROR_DRC_NONADJACENT_INPUT:
+		return "IS_ERROR_DRC_NONADJACENT_INPUT";
+	case IS_ERROR_DRC_ABSENT_OUTPUT:
+		return "IS_ERROR_DRC_ABSENT_OUTPUT";
+	case IS_ERROR_DRC_NONADJACENT_OUTPUT:
+		return "IS_ERROR_DRC_NONADJACENT_OUTPUT";
+	case IS_ERROR_DRC_FORMAT_MISMATCH:
+		return "IS_ERROR_DRC_FORMAT_MISMATCH";
+	case IS_ERROR_DRC_WIDTH_MISMATCH:
+		return "IS_ERROR_DRC_WIDTH_MISMATCH";
+	case IS_ERROR_DRC_HEIGHT_MISMATCH:
+		return "IS_ERROR_DRC_HEIGHT_MISMATCH";
+	case IS_ERROR_DRC_BITWIDTH_MISMATCH:
+		return "IS_ERROR_DRC_BITWIDTH_MISMATCH";
+	case IS_ERROR_DRC_FRAME_END_TIME_OUT:
+		return "IS_ERROR_DRC_FRAME_END_TIME_OUT";
+
+	/* FD */
+	case IS_ERROR_FD_PWRDN_FAIL:
+		return "IS_ERROR_FD_PWRDN_FAIL";
+	case IS_ERROR_FD_MULTIPLE_INPUT:
+		return "IS_ERROR_FD_MULTIPLE_INPUT";
+	case IS_ERROR_FD_ABSENT_INPUT:
+		return "IS_ERROR_FD_ABSENT_INPUT";
+	case IS_ERROR_FD_NONADJACENT_INPUT:
+		return "IS_ERROR_FD_NONADJACENT_INPUT";
+	case IS_ERROR_LHFD_FRAME_END_TIME_OUT:
+		return "IS_ERROR_LHFD_FRAME_END_TIME_OUT";
+	default:
+		return "Unknown";
+	}
+}
diff --git a/drivers/media/platform/exynos4-is/fimc-is-errno.h b/drivers/media/platform/exynos4-is/fimc-is-errno.h
new file mode 100644
index 00000000000..3de6f6da6f8
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-errno.h
@@ -0,0 +1,248 @@
+/*
+ * Samsung Exynos4 SoC series FIMC-IS slave interface driver
+ *
+ * FIMC-IS error code definition
+ *
+ * Copyright (C) 2011 - 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Younghwan Joo <yhwan.joo@samsung.com>
+ *          Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+
+#ifndef FIMC_IS_ERR_H_
+#define FIMC_IS_ERR_H_
+
+#define IS_ERROR_VER			011 /* IS ERROR VERSION 0.11 */
+
+enum {
+	IS_ERROR_NONE,
+
+	/* General 1 ~ 99 */
+	IS_ERROR_INVALID_COMMAND,
+	IS_ERROR_REQUEST_FAIL,
+	IS_ERROR_INVALID_SCENARIO,
+	IS_ERROR_INVALID_SENSORID,
+	IS_ERROR_INVALID_MODE_CHANGE,
+	IS_ERROR_INVALID_MAGIC_NUMBER,
+	IS_ERROR_INVALID_SETFILE_HDR,
+	IS_ERROR_BUSY,
+	IS_ERROR_SET_PARAMETER,
+	IS_ERROR_INVALID_PATH,
+	IS_ERROR_OPEN_SENSOR_FAIL,
+	IS_ERROR_ENTRY_MSG_THREAD_DOWN,
+	IS_ERROR_ISP_FRAME_END_NOT_DONE,
+	IS_ERROR_DRC_FRAME_END_NOT_DONE,
+	IS_ERROR_SCALERC_FRAME_END_NOT_DONE,
+	IS_ERROR_ODC_FRAME_END_NOT_DONE,
+	IS_ERROR_DIS_FRAME_END_NOT_DONE,
+	IS_ERROR_TDNR_FRAME_END_NOT_DONE,
+	IS_ERROR_SCALERP_FRAME_END_NOT_DONE,
+	IS_ERROR_WAIT_STREAM_OFF_NOT_DONE,
+	IS_ERROR_NO_MSG_IS_RECEIVED,
+	IS_ERROR_SENSOR_MSG_FAIL,
+	IS_ERROR_ISP_MSG_FAIL,
+	IS_ERROR_DRC_MSG_FAIL,
+	IS_ERROR_SCALERC_MSG_FAIL,
+	IS_ERROR_ODC_MSG_FAIL,
+	IS_ERROR_DIS_MSG_FAIL,
+	IS_ERROR_TDNR_MSG_FAIL,
+	IS_ERROR_SCALERP_MSG_FAIL,
+	IS_ERROR_LHFD_MSG_FAIL,
+	IS_ERROR_LHFD_INTERNAL_STOP,
+
+	/* Sensor 100 ~ 199 */
+	IS_ERROR_SENSOR_PWRDN_FAIL	= 100,
+	IS_ERROR_SENSOR_STREAM_ON_FAIL,
+	IS_ERROR_SENSOR_STREAM_OFF_FAIL,
+
+	/* ISP 200 ~ 299 */
+	IS_ERROR_ISP_PWRDN_FAIL		= 200,
+	IS_ERROR_ISP_MULTIPLE_INPUT,
+	IS_ERROR_ISP_ABSENT_INPUT,
+	IS_ERROR_ISP_ABSENT_OUTPUT,
+	IS_ERROR_ISP_NONADJACENT_OUTPUT,
+	IS_ERROR_ISP_FORMAT_MISMATCH,
+	IS_ERROR_ISP_WIDTH_MISMATCH,
+	IS_ERROR_ISP_HEIGHT_MISMATCH,
+	IS_ERROR_ISP_BITWIDTH_MISMATCH,
+	IS_ERROR_ISP_FRAME_END_TIME_OUT,
+
+	/* DRC 300 ~ 399 */
+	IS_ERROR_DRC_PWRDN_FAIL		= 300,
+	IS_ERROR_DRC_MULTIPLE_INPUT,
+	IS_ERROR_DRC_ABSENT_INPUT,
+	IS_ERROR_DRC_NONADJACENT_INPUT,
+	IS_ERROR_DRC_ABSENT_OUTPUT,
+	IS_ERROR_DRC_NONADJACENT_OUTPUT,
+	IS_ERROR_DRC_FORMAT_MISMATCH,
+	IS_ERROR_DRC_WIDTH_MISMATCH,
+	IS_ERROR_DRC_HEIGHT_MISMATCH,
+	IS_ERROR_DRC_BITWIDTH_MISMATCH,
+	IS_ERROR_DRC_FRAME_END_TIME_OUT,
+
+	/* SCALERC 400 ~ 499 */
+	IS_ERROR_SCALERC_PWRDN_FAIL	= 400,
+
+	/* ODC 500 ~ 599 */
+	IS_ERROR_ODC_PWRDN_FAIL		= 500,
+
+	/* DIS 600 ~ 699 */
+	IS_ERROR_DIS_PWRDN_FAIL		= 600,
+
+	/* TDNR 700 ~ 799 */
+	IS_ERROR_TDNR_PWRDN_FAIL	= 700,
+
+	/* SCALERC 800 ~ 899 */
+	IS_ERROR_SCALERP_PWRDN_FAIL	= 800,
+
+	/* FD 900 ~ 999 */
+	IS_ERROR_FD_PWRDN_FAIL		= 900,
+	IS_ERROR_FD_MULTIPLE_INPUT,
+	IS_ERROR_FD_ABSENT_INPUT,
+	IS_ERROR_FD_NONADJACENT_INPUT,
+	IS_ERROR_LHFD_FRAME_END_TIME_OUT,
+
+	IS_ERROR_UNKNOWN		= 1000,
+};
+
+#define IS_ERROR_TIME_OUT_FLAG	0x80000000
+
+/* Set parameter error enum */
+enum fimc_is_error {
+	/* Common error (0~99) */
+	ERROR_COMMON_NONE		= 0,
+	ERROR_COMMON_CMD		= 1,	/* Invalid command */
+	ERROR_COMMON_PARAMETER		= 2,	/* Invalid parameter */
+	/* setfile is not loaded before adjusting */
+	ERROR_COMMON_SETFILE_LOAD	= 3,
+	/* setfile is not Adjusted before runnng. */
+	ERROR_COMMON_SETFILE_ADJUST	= 4,
+	/* Index of setfile is not valid (0~MAX_SETFILE_NUM-1) */
+	ERROR_COMMON_SETFILE_INDEX	= 5,
+	/* Input path can be changed in ready state(stop) */
+	ERROR_COMMON_INPUT_PATH		= 6,
+	/* IP can not start if input path is not set */
+	ERROR_COMMON_INPUT_INIT		= 7,
+	/* Output path can be changed in ready state (stop) */
+	ERROR_COMMON_OUTPUT_PATH	= 8,
+	/* IP can not start if output path is not set */
+	ERROR_COMMON_OUTPUT_INIT	= 9,
+
+	ERROR_CONTROL_NONE		= ERROR_COMMON_NONE,
+	ERROR_CONTROL_BYPASS		= 11,	/* Enable or Disable */
+
+	ERROR_OTF_INPUT_NONE		= ERROR_COMMON_NONE,
+	ERROR_OTF_INPUT_CMD		= 21,
+	/* invalid format  (DRC: YUV444, FD: YUV444, 422, 420) */
+	ERROR_OTF_INPUT_FORMAT		= 22,
+	/* invalid width (DRC: 128~8192, FD: 32~8190) */
+	ERROR_OTF_INPUT_WIDTH		= 23,
+	/* invalid height (DRC: 64~8192, FD: 16~8190) */
+	ERROR_OTF_INPUT_HEIGHT		= 24,
+	/* invalid bit-width (DRC: 8~12bits, FD: 8bit) */
+	ERROR_OTF_INPUT_BIT_WIDTH	= 25,
+	/* invalid FrameTime for ISP */
+	ERROR_OTF_INPUT_USER_FRAMETIIME	= 26,
+
+	ERROR_DMA_INPUT_NONE		= ERROR_COMMON_NONE,
+	/* invalid width (DRC: 128~8192, FD: 32~8190) */
+	ERROR_DMA_INPUT_WIDTH		= 31,
+	/* invalid height (DRC: 64~8192, FD: 16~8190) */
+	ERROR_DMA_INPUT_HEIGHT		= 32,
+	/* invalid format (DRC: YUV444 or YUV422, FD: YUV444, 422, 420) */
+	ERROR_DMA_INPUT_FORMAT		= 33,
+	/* invalid bit-width (DRC: 8~12bit, FD: 8bit) */
+	ERROR_DMA_INPUT_BIT_WIDTH	= 34,
+	/* invalid order(DRC: YYCbCrorYCbYCr, FD:NO,YYCbCr,YCbYCr,CbCr,CrCb) */
+	ERROR_DMA_INPUT_ORDER		= 35,
+	/* invalid palne (DRC: 3, FD: 1, 2, 3) */
+	ERROR_DMA_INPUT_PLANE		= 36,
+
+	ERROR_OTF_OUTPUT_NONE		= ERROR_COMMON_NONE,
+	/* invalid width (DRC: 128~8192) */
+	ERROR_OTF_OUTPUT_WIDTH		= 41,
+	/* invalid height (DRC: 64~8192) */
+	ERROR_OTF_OUTPUT_HEIGHT		= 42,
+	/* invalid format (DRC: YUV444) */
+	ERROR_OTF_OUTPUT_FORMAT		= 43,
+	/* invalid bit-width (DRC: 8~12bits) */
+	ERROR_OTF_OUTPUT_BIT_WIDTH	= 44,
+
+	ERROR_DMA_OUTPUT_NONE		= ERROR_COMMON_NONE,
+	ERROR_DMA_OUTPUT_WIDTH		= 51,	/* invalid width */
+	ERROR_DMA_OUTPUT_HEIGHT		= 52,	/* invalid height */
+	ERROR_DMA_OUTPUT_FORMAT		= 53,	/* invalid format */
+	ERROR_DMA_OUTPUT_BIT_WIDTH	= 54,	/* invalid bit-width */
+	ERROR_DMA_OUTPUT_PLANE		= 55,	/* invalid plane */
+	ERROR_DMA_OUTPUT_ORDER		= 56,	/* invalid order */
+
+	ERROR_GLOBAL_SHOTMODE_NONE	= ERROR_COMMON_NONE,
+
+	/* SENSOR Error(100~199) */
+	ERROR_SENSOR_NONE		= ERROR_COMMON_NONE,
+	ERROR_SENSOR_I2C_FAIL		= 101,
+	ERROR_SENSOR_INVALID_FRAMERATE,
+	ERROR_SENSOR_INVALID_EXPOSURETIME,
+	ERROR_SENSOR_INVALID_SIZE,
+	ERROR_SENSOR_INVALID_SETTING,
+	ERROR_SENSOR_ACTURATOR_INIT_FAIL,
+	ERROR_SENSOR_INVALID_AF_POS,
+	ERROR_SENSOR_UNSUPPORT_FUNC,
+	ERROR_SENSOR_UNSUPPORT_PERI,
+	ERROR_SENSOR_UNSUPPORT_AF,
+
+	/* ISP Error (200~299) */
+	ERROR_ISP_AF_NONE		= ERROR_COMMON_NONE,
+	ERROR_ISP_AF_BUSY		= 201,
+	ERROR_ISP_AF_INVALID_COMMAND	= 202,
+	ERROR_ISP_AF_INVALID_MODE	= 203,
+	ERROR_ISP_FLASH_NONE		= ERROR_COMMON_NONE,
+	ERROR_ISP_AWB_NONE		= ERROR_COMMON_NONE,
+	ERROR_ISP_IMAGE_EFFECT_NONE	= ERROR_COMMON_NONE,
+	ERROR_ISP_ISO_NONE		= ERROR_COMMON_NONE,
+	ERROR_ISP_ADJUST_NONE		= ERROR_COMMON_NONE,
+	ERROR_ISP_METERING_NONE		= ERROR_COMMON_NONE,
+	ERROR_ISP_AFC_NONE		= ERROR_COMMON_NONE,
+
+	/* DRC Error (300~399) */
+
+	/* FD Error  (400~499) */
+	ERROR_FD_NONE					= ERROR_COMMON_NONE,
+	/* Invalid max number (1~16) */
+	ERROR_FD_CONFIG_MAX_NUMBER_STATE		= 401,
+	ERROR_FD_CONFIG_MAX_NUMBER_INVALID		= 402,
+	ERROR_FD_CONFIG_YAW_ANGLE_STATE			= 403,
+	ERROR_FD_CONFIG_YAW_ANGLE_INVALID		= 404,
+	ERROR_FD_CONFIG_ROLL_ANGLE_STATE		= 405,
+	ERROR_FD_CONFIG_ROLL_ANGLE_INVALID		= 406,
+	ERROR_FD_CONFIG_SMILE_MODE_INVALID		= 407,
+	ERROR_FD_CONFIG_BLINK_MODE_INVALID		= 408,
+	ERROR_FD_CONFIG_EYES_DETECT_INVALID		= 409,
+	ERROR_FD_CONFIG_MOUTH_DETECT_INVALID		= 410,
+	ERROR_FD_CONFIG_ORIENTATION_STATE		= 411,
+	ERROR_FD_CONFIG_ORIENTATION_INVALID		= 412,
+	ERROR_FD_CONFIG_ORIENTATION_VALUE_INVALID	= 413,
+	/* PARAM_FdResultStr can be only applied in ready-state or stream off */
+	ERROR_FD_RESULT					= 414,
+	/* PARAM_FdModeStr can be only applied in ready-state or stream off */
+	ERROR_FD_MODE					= 415,
+	/* Scaler Error  (500 ~ 599) */
+	ERROR_SCALER_NO_NONE				= ERROR_COMMON_NONE,
+	ERROR_SCALER_DMA_OUTSEL				= 501,
+	ERROR_SCALER_H_RATIO				= 502,
+	ERROR_SCALER_V_RATIO				= 503,
+
+	ERROR_SCALER_IMAGE_EFFECT			= 510,
+
+	ERROR_SCALER_ROTATE				= 520,
+	ERROR_SCALER_FLIP				= 521,
+};
+
+const char * const fimc_is_strerr(unsigned int error);
+const char * const fimc_is_param_strerr(unsigned int error);
+
+#endif /* FIMC_IS_ERR_H_ */
diff --git a/drivers/media/platform/exynos4-is/fimc-is-i2c.c b/drivers/media/platform/exynos4-is/fimc-is-i2c.c
new file mode 100644
index 00000000000..c397777d7cb
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-i2c.c
@@ -0,0 +1,126 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Author: 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/clk.h>
+#include <linux/module.h>
+#include <linux/of_i2c.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include "fimc-is-i2c.h"
+
+struct fimc_is_i2c {
+	struct i2c_adapter adapter;
+	struct clk *clock;
+};
+
+/*
+ * An empty algorithm is used as the actual I2C bus controller driver
+ * is implemented in the FIMC-IS subsystem firmware and the host CPU
+ * doesn't access the I2C bus controller.
+ */
+static const struct i2c_algorithm fimc_is_i2c_algorithm;
+
+static int fimc_is_i2c_probe(struct platform_device *pdev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct fimc_is_i2c *isp_i2c;
+	struct i2c_adapter *i2c_adap;
+	int ret;
+
+	isp_i2c = devm_kzalloc(&pdev->dev, sizeof(*isp_i2c), GFP_KERNEL);
+	if (!isp_i2c)
+		return -ENOMEM;
+
+	isp_i2c->clock = devm_clk_get(&pdev->dev, "i2c_isp");
+	if (IS_ERR(isp_i2c->clock)) {
+		dev_err(&pdev->dev, "failed to get the clock\n");
+		return PTR_ERR(isp_i2c->clock);
+	}
+
+	i2c_adap = &isp_i2c->adapter;
+	i2c_adap->dev.of_node = node;
+	i2c_adap->dev.parent = &pdev->dev;
+	strlcpy(i2c_adap->name, "exynos4x12-isp-i2c", sizeof(i2c_adap->name));
+	i2c_adap->owner = THIS_MODULE;
+	i2c_adap->algo = &fimc_is_i2c_algorithm;
+	i2c_adap->class = I2C_CLASS_SPD;
+
+	ret = i2c_add_adapter(i2c_adap);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to add I2C bus %s\n",
+						node->full_name);
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, isp_i2c);
+
+	pm_runtime_enable(&pdev->dev);
+	pm_runtime_enable(&i2c_adap->dev);
+
+	of_i2c_register_devices(i2c_adap);
+
+	return 0;
+}
+
+static int fimc_is_i2c_remove(struct platform_device *pdev)
+{
+	struct fimc_is_i2c *isp_i2c = platform_get_drvdata(pdev);
+
+	pm_runtime_disable(&isp_i2c->adapter.dev);
+	pm_runtime_disable(&pdev->dev);
+	i2c_del_adapter(&isp_i2c->adapter);
+
+	return 0;
+}
+
+static int fimc_is_i2c_suspend(struct device *dev)
+{
+	struct fimc_is_i2c *isp_i2c = dev_get_drvdata(dev);
+	clk_disable_unprepare(isp_i2c->clock);
+	return 0;
+}
+
+static int fimc_is_i2c_resume(struct device *dev)
+{
+	struct fimc_is_i2c *isp_i2c = dev_get_drvdata(dev);
+	return clk_prepare_enable(isp_i2c->clock);
+}
+
+UNIVERSAL_DEV_PM_OPS(fimc_is_i2c_pm_ops, fimc_is_i2c_suspend,
+		     fimc_is_i2c_resume, NULL);
+
+static const struct of_device_id fimc_is_i2c_of_match[] = {
+	{ .compatible = FIMC_IS_I2C_COMPATIBLE },
+	{ },
+};
+
+static struct platform_driver fimc_is_i2c_driver = {
+	.probe		= fimc_is_i2c_probe,
+	.remove		= fimc_is_i2c_remove,
+	.driver = {
+		.of_match_table = fimc_is_i2c_of_match,
+		.name		= "fimc-isp-i2c",
+		.owner		= THIS_MODULE,
+		.pm		= &fimc_is_i2c_pm_ops,
+	}
+};
+
+int fimc_is_register_i2c_driver(void)
+{
+	return platform_driver_register(&fimc_is_i2c_driver);
+}
+
+void fimc_is_unregister_i2c_driver(void)
+{
+	platform_driver_unregister(&fimc_is_i2c_driver);
+}
diff --git a/drivers/media/platform/exynos4-is/fimc-is-i2c.h b/drivers/media/platform/exynos4-is/fimc-is-i2c.h
new file mode 100644
index 00000000000..0d38d6bb963
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-i2c.h
@@ -0,0 +1,15 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 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 FIMC_IS_I2C_COMPATIBLE	"samsung,exynos4212-i2c-isp"
+
+int fimc_is_register_i2c_driver(void);
+void fimc_is_unregister_i2c_driver(void);
diff --git a/drivers/media/platform/exynos4-is/fimc-is-param.c b/drivers/media/platform/exynos4-is/fimc-is-param.c
new file mode 100644
index 00000000000..53fe2a2b4db
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-param.c
@@ -0,0 +1,900 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Younghwan Joo <yhwan.joo@samsung.com>
+ *          Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#define pr_fmt(fmt) "%s:%d " fmt, __func__, __LINE__
+
+#include <linux/bitops.h>
+#include <linux/bug.h>
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+
+#include "fimc-is.h"
+#include "fimc-is-command.h"
+#include "fimc-is-errno.h"
+#include "fimc-is-param.h"
+#include "fimc-is-regs.h"
+#include "fimc-is-sensor.h"
+
+static void __hw_param_copy(void *dst, void *src)
+{
+	memcpy(dst, src, FIMC_IS_PARAM_MAX_SIZE);
+}
+
+void __fimc_is_hw_update_param_global_shotmode(struct fimc_is *is)
+{
+	struct param_global_shotmode *dst, *src;
+
+	dst = &is->is_p_region->parameter.global.shotmode;
+	src = &is->config[is->config_index].global.shotmode;
+	__hw_param_copy(dst, src);
+}
+
+void __fimc_is_hw_update_param_sensor_framerate(struct fimc_is *is)
+{
+	struct param_sensor_framerate *dst, *src;
+
+	dst = &is->is_p_region->parameter.sensor.frame_rate;
+	src = &is->config[is->config_index].sensor.frame_rate;
+	__hw_param_copy(dst, src);
+}
+
+int __fimc_is_hw_update_param(struct fimc_is *is, u32 offset)
+{
+	struct is_param_region *par = &is->is_p_region->parameter;
+	struct chain_config *cfg = &is->config[is->config_index];
+
+	switch (offset) {
+	case PARAM_ISP_CONTROL:
+		__hw_param_copy(&par->isp.control, &cfg->isp.control);
+		break;
+
+	case PARAM_ISP_OTF_INPUT:
+		__hw_param_copy(&par->isp.otf_input, &cfg->isp.otf_input);
+		break;
+
+	case PARAM_ISP_DMA1_INPUT:
+		__hw_param_copy(&par->isp.dma1_input, &cfg->isp.dma1_input);
+		break;
+
+	case PARAM_ISP_DMA2_INPUT:
+		__hw_param_copy(&par->isp.dma2_input, &cfg->isp.dma2_input);
+		break;
+
+	case PARAM_ISP_AA:
+		__hw_param_copy(&par->isp.aa, &cfg->isp.aa);
+		break;
+
+	case PARAM_ISP_FLASH:
+		__hw_param_copy(&par->isp.flash, &cfg->isp.flash);
+		break;
+
+	case PARAM_ISP_AWB:
+		__hw_param_copy(&par->isp.awb, &cfg->isp.awb);
+		break;
+
+	case PARAM_ISP_IMAGE_EFFECT:
+		__hw_param_copy(&par->isp.effect, &cfg->isp.effect);
+		break;
+
+	case PARAM_ISP_ISO:
+		__hw_param_copy(&par->isp.iso, &cfg->isp.iso);
+		break;
+
+	case PARAM_ISP_ADJUST:
+		__hw_param_copy(&par->isp.adjust, &cfg->isp.adjust);
+		break;
+
+	case PARAM_ISP_METERING:
+		__hw_param_copy(&par->isp.metering, &cfg->isp.metering);
+		break;
+
+	case PARAM_ISP_AFC:
+		__hw_param_copy(&par->isp.afc, &cfg->isp.afc);
+		break;
+
+	case PARAM_ISP_OTF_OUTPUT:
+		__hw_param_copy(&par->isp.otf_output, &cfg->isp.otf_output);
+		break;
+
+	case PARAM_ISP_DMA1_OUTPUT:
+		__hw_param_copy(&par->isp.dma1_output, &cfg->isp.dma1_output);
+		break;
+
+	case PARAM_ISP_DMA2_OUTPUT:
+		__hw_param_copy(&par->isp.dma2_output, &cfg->isp.dma2_output);
+		break;
+
+	case PARAM_DRC_CONTROL:
+		__hw_param_copy(&par->drc.control, &cfg->drc.control);
+		break;
+
+	case PARAM_DRC_OTF_INPUT:
+		__hw_param_copy(&par->drc.otf_input, &cfg->drc.otf_input);
+		break;
+
+	case PARAM_DRC_DMA_INPUT:
+		__hw_param_copy(&par->drc.dma_input, &cfg->drc.dma_input);
+		break;
+
+	case PARAM_DRC_OTF_OUTPUT:
+		__hw_param_copy(&par->drc.otf_output, &cfg->drc.otf_output);
+		break;
+
+	case PARAM_FD_CONTROL:
+		__hw_param_copy(&par->fd.control, &cfg->fd.control);
+		break;
+
+	case PARAM_FD_OTF_INPUT:
+		__hw_param_copy(&par->fd.otf_input, &cfg->fd.otf_input);
+		break;
+
+	case PARAM_FD_DMA_INPUT:
+		__hw_param_copy(&par->fd.dma_input, &cfg->fd.dma_input);
+		break;
+
+	case PARAM_FD_CONFIG:
+		__hw_param_copy(&par->fd.config, &cfg->fd.config);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+unsigned int __get_pending_param_count(struct fimc_is *is)
+{
+	struct chain_config *config = &is->config[is->config_index];
+	unsigned long flags;
+	unsigned int count;
+
+	spin_lock_irqsave(&is->slock, flags);
+	count = hweight32(config->p_region_index1);
+	count += hweight32(config->p_region_index2);
+	spin_unlock_irqrestore(&is->slock, flags);
+
+	return count;
+}
+
+int __is_hw_update_params(struct fimc_is *is)
+{
+	unsigned long *p_index1, *p_index2;
+	int i, id, ret = 0;
+
+	id = is->config_index;
+	p_index1 = &is->config[id].p_region_index1;
+	p_index2 = &is->config[id].p_region_index2;
+
+	if (test_bit(PARAM_GLOBAL_SHOTMODE, p_index1))
+		__fimc_is_hw_update_param_global_shotmode(is);
+
+	if (test_bit(PARAM_SENSOR_FRAME_RATE, p_index1))
+		__fimc_is_hw_update_param_sensor_framerate(is);
+
+	for (i = PARAM_ISP_CONTROL; i < PARAM_DRC_CONTROL; i++) {
+		if (test_bit(i, p_index1))
+			ret = __fimc_is_hw_update_param(is, i);
+	}
+
+	for (i = PARAM_DRC_CONTROL; i < PARAM_SCALERC_CONTROL; i++) {
+		if (test_bit(i, p_index1))
+			ret = __fimc_is_hw_update_param(is, i);
+	}
+
+	for (i = PARAM_FD_CONTROL; i <= PARAM_FD_CONFIG; i++) {
+		if (test_bit((i - 32), p_index2))
+			ret = __fimc_is_hw_update_param(is, i);
+	}
+
+	return ret;
+}
+
+void __is_get_frame_size(struct fimc_is *is, struct v4l2_mbus_framefmt *mf)
+{
+	struct isp_param *isp;
+
+	isp = &is->config[is->config_index].isp;
+	mf->width = isp->otf_input.width;
+	mf->height = isp->otf_input.height;
+}
+
+void __is_set_frame_size(struct fimc_is *is, struct v4l2_mbus_framefmt *mf)
+{
+	unsigned int index = is->config_index;
+	struct isp_param *isp;
+	struct drc_param *drc;
+	struct fd_param *fd;
+
+	isp = &is->config[index].isp;
+	drc = &is->config[index].drc;
+	fd = &is->config[index].fd;
+
+	/* Update isp size info (OTF only) */
+	isp->otf_input.width = mf->width;
+	isp->otf_input.height = mf->height;
+	isp->otf_output.width = mf->width;
+	isp->otf_output.height = mf->height;
+	/* Update drc size info (OTF only) */
+	drc->otf_input.width = mf->width;
+	drc->otf_input.height = mf->height;
+	drc->otf_output.width = mf->width;
+	drc->otf_output.height = mf->height;
+	/* Update fd size info (OTF only) */
+	fd->otf_input.width = mf->width;
+	fd->otf_input.height = mf->height;
+
+	if (test_bit(PARAM_ISP_OTF_INPUT,
+		      &is->config[index].p_region_index1))
+		return;
+
+	/* Update field */
+	fimc_is_set_param_bit(is, PARAM_ISP_OTF_INPUT);
+	fimc_is_set_param_bit(is, PARAM_ISP_OTF_OUTPUT);
+	fimc_is_set_param_bit(is, PARAM_DRC_OTF_INPUT);
+	fimc_is_set_param_bit(is, PARAM_DRC_OTF_OUTPUT);
+	fimc_is_set_param_bit(is, PARAM_FD_OTF_INPUT);
+}
+
+int fimc_is_hw_get_sensor_max_framerate(struct fimc_is *is)
+{
+	switch (is->sensor->drvdata->id) {
+	case FIMC_IS_SENSOR_ID_S5K6A3:
+		return 30;
+	default:
+		return 15;
+	}
+}
+
+void __is_set_sensor(struct fimc_is *is, int fps)
+{
+	unsigned int index = is->config_index;
+	struct sensor_param *sensor;
+	struct isp_param *isp;
+
+	sensor = &is->config[index].sensor;
+	isp = &is->config[index].isp;
+
+	if (fps == 0) {
+		sensor->frame_rate.frame_rate =
+				fimc_is_hw_get_sensor_max_framerate(is);
+		isp->otf_input.frametime_min = 0;
+		isp->otf_input.frametime_max = 66666;
+	} else {
+		sensor->frame_rate.frame_rate = fps;
+		isp->otf_input.frametime_min = 0;
+		isp->otf_input.frametime_max = (u32)1000000 / fps;
+	}
+
+	fimc_is_set_param_bit(is, PARAM_SENSOR_FRAME_RATE);
+	fimc_is_set_param_bit(is, PARAM_ISP_OTF_INPUT);
+}
+
+void __is_set_init_isp_aa(struct fimc_is *is)
+{
+	struct isp_param *isp;
+
+	isp = &is->config[is->config_index].isp;
+
+	isp->aa.cmd = ISP_AA_COMMAND_START;
+	isp->aa.target = ISP_AA_TARGET_AF | ISP_AA_TARGET_AE |
+			 ISP_AA_TARGET_AWB;
+	isp->aa.mode = 0;
+	isp->aa.scene = 0;
+	isp->aa.sleep = 0;
+	isp->aa.face = 0;
+	isp->aa.touch_x = 0;
+	isp->aa.touch_y = 0;
+	isp->aa.manual_af_setting = 0;
+	isp->aa.err = ISP_AF_ERROR_NONE;
+
+	fimc_is_set_param_bit(is, PARAM_ISP_AA);
+}
+
+void __is_set_isp_flash(struct fimc_is *is, u32 cmd, u32 redeye)
+{
+	unsigned int index = is->config_index;
+	struct isp_param *isp = &is->config[index].isp;
+
+	isp->flash.cmd = cmd;
+	isp->flash.redeye = redeye;
+	isp->flash.err = ISP_FLASH_ERROR_NONE;
+
+	fimc_is_set_param_bit(is, PARAM_ISP_FLASH);
+}
+
+void __is_set_isp_awb(struct fimc_is *is, u32 cmd, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct isp_param *isp;
+
+	isp = &is->config[index].isp;
+
+	isp->awb.cmd = cmd;
+	isp->awb.illumination = val;
+	isp->awb.err = ISP_AWB_ERROR_NONE;
+
+	fimc_is_set_param_bit(is, PARAM_ISP_AWB);
+}
+
+void __is_set_isp_effect(struct fimc_is *is, u32 cmd)
+{
+	unsigned int index = is->config_index;
+	struct isp_param *isp;
+
+	isp = &is->config[index].isp;
+
+	isp->effect.cmd = cmd;
+	isp->effect.err = ISP_IMAGE_EFFECT_ERROR_NONE;
+
+	fimc_is_set_param_bit(is, PARAM_ISP_IMAGE_EFFECT);
+}
+
+void __is_set_isp_iso(struct fimc_is *is, u32 cmd, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct isp_param *isp;
+
+	isp = &is->config[index].isp;
+
+	isp->iso.cmd = cmd;
+	isp->iso.value = val;
+	isp->iso.err = ISP_ISO_ERROR_NONE;
+
+	fimc_is_set_param_bit(is, PARAM_ISP_ISO);
+}
+
+void __is_set_isp_adjust(struct fimc_is *is, u32 cmd, u32 val)
+{
+	unsigned int index = is->config_index;
+	unsigned long *p_index;
+	struct isp_param *isp;
+
+	p_index = &is->config[index].p_region_index1;
+	isp = &is->config[index].isp;
+
+	switch (cmd) {
+	case ISP_ADJUST_COMMAND_MANUAL_CONTRAST:
+		isp->adjust.contrast = val;
+		break;
+	case ISP_ADJUST_COMMAND_MANUAL_SATURATION:
+		isp->adjust.saturation = val;
+		break;
+	case ISP_ADJUST_COMMAND_MANUAL_SHARPNESS:
+		isp->adjust.sharpness = val;
+		break;
+	case ISP_ADJUST_COMMAND_MANUAL_EXPOSURE:
+		isp->adjust.exposure = val;
+		break;
+	case ISP_ADJUST_COMMAND_MANUAL_BRIGHTNESS:
+		isp->adjust.brightness = val;
+		break;
+	case ISP_ADJUST_COMMAND_MANUAL_HUE:
+		isp->adjust.hue = val;
+		break;
+	case ISP_ADJUST_COMMAND_AUTO:
+		isp->adjust.contrast = 0;
+		isp->adjust.saturation = 0;
+		isp->adjust.sharpness = 0;
+		isp->adjust.exposure = 0;
+		isp->adjust.brightness = 0;
+		isp->adjust.hue = 0;
+		break;
+	}
+
+	if (!test_bit(PARAM_ISP_ADJUST, p_index)) {
+		isp->adjust.cmd = cmd;
+		isp->adjust.err = ISP_ADJUST_ERROR_NONE;
+		fimc_is_set_param_bit(is, PARAM_ISP_ADJUST);
+	} else {
+		isp->adjust.cmd |= cmd;
+	}
+}
+
+void __is_set_isp_metering(struct fimc_is *is, u32 id, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct isp_param *isp;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index1;
+	isp = &is->config[index].isp;
+
+	switch (id) {
+	case IS_METERING_CONFIG_CMD:
+		isp->metering.cmd = val;
+		break;
+	case IS_METERING_CONFIG_WIN_POS_X:
+		isp->metering.win_pos_x = val;
+		break;
+	case IS_METERING_CONFIG_WIN_POS_Y:
+		isp->metering.win_pos_y = val;
+		break;
+	case IS_METERING_CONFIG_WIN_WIDTH:
+		isp->metering.win_width = val;
+		break;
+	case IS_METERING_CONFIG_WIN_HEIGHT:
+		isp->metering.win_height = val;
+		break;
+	default:
+		return;
+	}
+
+	if (!test_bit(PARAM_ISP_METERING, p_index)) {
+		isp->metering.err = ISP_METERING_ERROR_NONE;
+		fimc_is_set_param_bit(is, PARAM_ISP_METERING);
+	}
+}
+
+void __is_set_isp_afc(struct fimc_is *is, u32 cmd, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct isp_param *isp;
+
+	isp = &is->config[index].isp;
+
+	isp->afc.cmd = cmd;
+	isp->afc.manual = val;
+	isp->afc.err = ISP_AFC_ERROR_NONE;
+
+	fimc_is_set_param_bit(is, PARAM_ISP_AFC);
+}
+
+void __is_set_drc_control(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct drc_param *drc;
+
+	drc = &is->config[index].drc;
+
+	drc->control.bypass = val;
+
+	fimc_is_set_param_bit(is, PARAM_DRC_CONTROL);
+}
+
+void __is_set_fd_control(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index2;
+	fd = &is->config[index].fd;
+
+	fd->control.cmd = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index))
+		fimc_is_set_param_bit(is, PARAM_FD_CONTROL);
+}
+
+void __is_set_fd_config_maxface(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index2;
+	fd = &is->config[index].fd;
+
+	fd->config.max_number = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_MAXIMUM_NUMBER;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_MAXIMUM_NUMBER;
+	}
+}
+
+void __is_set_fd_config_rollangle(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index2;
+	fd = &is->config[index].fd;
+
+	fd->config.roll_angle = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_ROLL_ANGLE;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_ROLL_ANGLE;
+	}
+}
+
+void __is_set_fd_config_yawangle(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index2;
+	fd = &is->config[index].fd;
+
+	fd->config.yaw_angle = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_YAW_ANGLE;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_YAW_ANGLE;
+	}
+}
+
+void __is_set_fd_config_smilemode(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index2;
+	fd = &is->config[index].fd;
+
+	fd->config.smile_mode = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_SMILE_MODE;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_SMILE_MODE;
+	}
+}
+
+void __is_set_fd_config_blinkmode(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index2;
+	fd = &is->config[index].fd;
+
+	fd->config.blink_mode = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_BLINK_MODE;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_BLINK_MODE;
+	}
+}
+
+void __is_set_fd_config_eyedetect(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index2;
+	fd = &is->config[index].fd;
+
+	fd->config.eye_detect = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_EYES_DETECT;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_EYES_DETECT;
+	}
+}
+
+void __is_set_fd_config_mouthdetect(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index2;
+	fd = &is->config[index].fd;
+
+	fd->config.mouth_detect = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_MOUTH_DETECT;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_MOUTH_DETECT;
+	}
+}
+
+void __is_set_fd_config_orientation(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index2;
+	fd = &is->config[index].fd;
+
+	fd->config.orientation = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_ORIENTATION;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_ORIENTATION;
+	}
+}
+
+void __is_set_fd_config_orientation_val(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index2;
+	fd = &is->config[index].fd;
+
+	fd->config.orientation_value = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_ORIENTATION_VALUE;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_ORIENTATION_VALUE;
+	}
+}
+
+void fimc_is_set_initial_params(struct fimc_is *is)
+{
+	struct global_param *global;
+	struct sensor_param *sensor;
+	struct isp_param *isp;
+	struct drc_param *drc;
+	struct fd_param *fd;
+	unsigned long *p_index1, *p_index2;
+	unsigned int index;
+
+	index = is->config_index;
+	global = &is->config[index].global;
+	sensor = &is->config[index].sensor;
+	isp = &is->config[index].isp;
+	drc = &is->config[index].drc;
+	fd = &is->config[index].fd;
+	p_index1 = &is->config[index].p_region_index1;
+	p_index2 = &is->config[index].p_region_index2;
+
+	/* Global */
+	global->shotmode.cmd = 1;
+	fimc_is_set_param_bit(is, PARAM_GLOBAL_SHOTMODE);
+
+	/* ISP */
+	isp->control.cmd = CONTROL_COMMAND_START;
+	isp->control.bypass = CONTROL_BYPASS_DISABLE;
+	isp->control.err = CONTROL_ERROR_NONE;
+	fimc_is_set_param_bit(is, PARAM_ISP_CONTROL);
+
+	isp->otf_input.cmd = OTF_INPUT_COMMAND_ENABLE;
+	if (!test_bit(PARAM_ISP_OTF_INPUT, p_index1)) {
+		isp->otf_input.width = DEFAULT_PREVIEW_STILL_WIDTH;
+		isp->otf_input.height = DEFAULT_PREVIEW_STILL_HEIGHT;
+		fimc_is_set_param_bit(is, PARAM_ISP_OTF_INPUT);
+	}
+	if (is->sensor->test_pattern)
+		isp->otf_input.format = OTF_INPUT_FORMAT_STRGEN_COLORBAR_BAYER;
+	else
+		isp->otf_input.format = OTF_INPUT_FORMAT_BAYER;
+	isp->otf_input.bitwidth = 10;
+	isp->otf_input.order = OTF_INPUT_ORDER_BAYER_GR_BG;
+	isp->otf_input.crop_offset_x = 0;
+	isp->otf_input.crop_offset_y = 0;
+	isp->otf_input.err = OTF_INPUT_ERROR_NONE;
+
+	isp->dma1_input.cmd = DMA_INPUT_COMMAND_DISABLE;
+	isp->dma1_input.width = 0;
+	isp->dma1_input.height = 0;
+	isp->dma1_input.format = 0;
+	isp->dma1_input.bitwidth = 0;
+	isp->dma1_input.plane = 0;
+	isp->dma1_input.order = 0;
+	isp->dma1_input.buffer_number = 0;
+	isp->dma1_input.width = 0;
+	isp->dma1_input.err = DMA_INPUT_ERROR_NONE;
+	fimc_is_set_param_bit(is, PARAM_ISP_DMA1_INPUT);
+
+	isp->dma2_input.cmd = DMA_INPUT_COMMAND_DISABLE;
+	isp->dma2_input.width = 0;
+	isp->dma2_input.height = 0;
+	isp->dma2_input.format = 0;
+	isp->dma2_input.bitwidth = 0;
+	isp->dma2_input.plane = 0;
+	isp->dma2_input.order = 0;
+	isp->dma2_input.buffer_number = 0;
+	isp->dma2_input.width = 0;
+	isp->dma2_input.err = DMA_INPUT_ERROR_NONE;
+	fimc_is_set_param_bit(is, PARAM_ISP_DMA2_INPUT);
+
+	isp->aa.cmd = ISP_AA_COMMAND_START;
+	isp->aa.target = ISP_AA_TARGET_AE | ISP_AA_TARGET_AWB;
+	fimc_is_set_param_bit(is, PARAM_ISP_AA);
+
+	if (!test_bit(PARAM_ISP_FLASH, p_index1))
+		__is_set_isp_flash(is, ISP_FLASH_COMMAND_DISABLE,
+						ISP_FLASH_REDEYE_DISABLE);
+
+	if (!test_bit(PARAM_ISP_AWB, p_index1))
+		__is_set_isp_awb(is, ISP_AWB_COMMAND_AUTO, 0);
+
+	if (!test_bit(PARAM_ISP_IMAGE_EFFECT, p_index1))
+		__is_set_isp_effect(is, ISP_IMAGE_EFFECT_DISABLE);
+
+	if (!test_bit(PARAM_ISP_ISO, p_index1))
+		__is_set_isp_iso(is, ISP_ISO_COMMAND_AUTO, 0);
+
+	if (!test_bit(PARAM_ISP_ADJUST, p_index1)) {
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_CONTRAST, 0);
+		__is_set_isp_adjust(is,
+				ISP_ADJUST_COMMAND_MANUAL_SATURATION, 0);
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_SHARPNESS, 0);
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_EXPOSURE, 0);
+		__is_set_isp_adjust(is,
+				ISP_ADJUST_COMMAND_MANUAL_BRIGHTNESS, 0);
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_HUE, 0);
+	}
+
+	if (!test_bit(PARAM_ISP_METERING, p_index1)) {
+		__is_set_isp_metering(is, 0, ISP_METERING_COMMAND_CENTER);
+		__is_set_isp_metering(is, 1, 0);
+		__is_set_isp_metering(is, 2, 0);
+		__is_set_isp_metering(is, 3, 0);
+		__is_set_isp_metering(is, 4, 0);
+	}
+
+	if (!test_bit(PARAM_ISP_AFC, p_index1))
+		__is_set_isp_afc(is, ISP_AFC_COMMAND_AUTO, 0);
+
+	isp->otf_output.cmd = OTF_OUTPUT_COMMAND_ENABLE;
+	if (!test_bit(PARAM_ISP_OTF_OUTPUT, p_index1)) {
+		isp->otf_output.width = DEFAULT_PREVIEW_STILL_WIDTH;
+		isp->otf_output.height = DEFAULT_PREVIEW_STILL_HEIGHT;
+		fimc_is_set_param_bit(is, PARAM_ISP_OTF_OUTPUT);
+	}
+	isp->otf_output.format = OTF_OUTPUT_FORMAT_YUV444;
+	isp->otf_output.bitwidth = 12;
+	isp->otf_output.order = 0;
+	isp->otf_output.err = OTF_OUTPUT_ERROR_NONE;
+
+	if (!test_bit(PARAM_ISP_DMA1_OUTPUT, p_index1)) {
+		isp->dma1_output.cmd = DMA_OUTPUT_COMMAND_DISABLE;
+		isp->dma1_output.width = 0;
+		isp->dma1_output.height = 0;
+		isp->dma1_output.format = 0;
+		isp->dma1_output.bitwidth = 0;
+		isp->dma1_output.plane = 0;
+		isp->dma1_output.order = 0;
+		isp->dma1_output.buffer_number = 0;
+		isp->dma1_output.buffer_address = 0;
+		isp->dma1_output.notify_dma_done = 0;
+		isp->dma1_output.dma_out_mask = 0;
+		isp->dma1_output.err = DMA_OUTPUT_ERROR_NONE;
+		fimc_is_set_param_bit(is, PARAM_ISP_DMA1_OUTPUT);
+	}
+
+	if (!test_bit(PARAM_ISP_DMA2_OUTPUT, p_index1)) {
+		isp->dma2_output.cmd = DMA_OUTPUT_COMMAND_DISABLE;
+		isp->dma2_output.width = 0;
+		isp->dma2_output.height = 0;
+		isp->dma2_output.format = 0;
+		isp->dma2_output.bitwidth = 0;
+		isp->dma2_output.plane = 0;
+		isp->dma2_output.order = 0;
+		isp->dma2_output.buffer_number = 0;
+		isp->dma2_output.buffer_address = 0;
+		isp->dma2_output.notify_dma_done = 0;
+		isp->dma2_output.dma_out_mask = 0;
+		isp->dma2_output.err = DMA_OUTPUT_ERROR_NONE;
+		fimc_is_set_param_bit(is, PARAM_ISP_DMA2_OUTPUT);
+	}
+
+	/* Sensor */
+	if (!test_bit(PARAM_SENSOR_FRAME_RATE, p_index1)) {
+		if (is->config_index == 0)
+			__is_set_sensor(is, 0);
+	}
+
+	/* DRC */
+	drc->control.cmd = CONTROL_COMMAND_START;
+	__is_set_drc_control(is, CONTROL_BYPASS_ENABLE);
+
+	drc->otf_input.cmd = OTF_INPUT_COMMAND_ENABLE;
+	if (!test_bit(PARAM_DRC_OTF_INPUT, p_index1)) {
+		drc->otf_input.width = DEFAULT_PREVIEW_STILL_WIDTH;
+		drc->otf_input.height = DEFAULT_PREVIEW_STILL_HEIGHT;
+		fimc_is_set_param_bit(is, PARAM_DRC_OTF_INPUT);
+	}
+	drc->otf_input.format = OTF_INPUT_FORMAT_YUV444;
+	drc->otf_input.bitwidth = 12;
+	drc->otf_input.order = 0;
+	drc->otf_input.err = OTF_INPUT_ERROR_NONE;
+
+	drc->dma_input.cmd = DMA_INPUT_COMMAND_DISABLE;
+	drc->dma_input.width = 0;
+	drc->dma_input.height = 0;
+	drc->dma_input.format = 0;
+	drc->dma_input.bitwidth = 0;
+	drc->dma_input.plane = 0;
+	drc->dma_input.order = 0;
+	drc->dma_input.buffer_number = 0;
+	drc->dma_input.width = 0;
+	drc->dma_input.err = DMA_INPUT_ERROR_NONE;
+	fimc_is_set_param_bit(is, PARAM_DRC_DMA_INPUT);
+
+	drc->otf_output.cmd = OTF_OUTPUT_COMMAND_ENABLE;
+	if (!test_bit(PARAM_DRC_OTF_OUTPUT, p_index1)) {
+		drc->otf_output.width = DEFAULT_PREVIEW_STILL_WIDTH;
+		drc->otf_output.height = DEFAULT_PREVIEW_STILL_HEIGHT;
+		fimc_is_set_param_bit(is, PARAM_DRC_OTF_OUTPUT);
+	}
+	drc->otf_output.format = OTF_OUTPUT_FORMAT_YUV444;
+	drc->otf_output.bitwidth = 8;
+	drc->otf_output.order = 0;
+	drc->otf_output.err = OTF_OUTPUT_ERROR_NONE;
+
+	/* FD */
+	__is_set_fd_control(is, CONTROL_COMMAND_STOP);
+	fd->control.bypass = CONTROL_BYPASS_DISABLE;
+
+	fd->otf_input.cmd = OTF_INPUT_COMMAND_ENABLE;
+	if (!test_bit((PARAM_FD_OTF_INPUT - 32), p_index2)) {
+		fd->otf_input.width = DEFAULT_PREVIEW_STILL_WIDTH;
+		fd->otf_input.height = DEFAULT_PREVIEW_STILL_HEIGHT;
+		fimc_is_set_param_bit(is, PARAM_FD_OTF_INPUT);
+	}
+
+	fd->otf_input.format = OTF_INPUT_FORMAT_YUV444;
+	fd->otf_input.bitwidth = 8;
+	fd->otf_input.order = 0;
+	fd->otf_input.err = OTF_INPUT_ERROR_NONE;
+
+	fd->dma_input.cmd = DMA_INPUT_COMMAND_DISABLE;
+	fd->dma_input.width = 0;
+	fd->dma_input.height = 0;
+	fd->dma_input.format = 0;
+	fd->dma_input.bitwidth = 0;
+	fd->dma_input.plane = 0;
+	fd->dma_input.order = 0;
+	fd->dma_input.buffer_number = 0;
+	fd->dma_input.width = 0;
+	fd->dma_input.err = DMA_INPUT_ERROR_NONE;
+	fimc_is_set_param_bit(is, PARAM_FD_DMA_INPUT);
+
+	__is_set_fd_config_maxface(is, 5);
+	__is_set_fd_config_rollangle(is, FD_CONFIG_ROLL_ANGLE_FULL);
+	__is_set_fd_config_yawangle(is, FD_CONFIG_YAW_ANGLE_45_90);
+	__is_set_fd_config_smilemode(is, FD_CONFIG_SMILE_MODE_DISABLE);
+	__is_set_fd_config_blinkmode(is, FD_CONFIG_BLINK_MODE_DISABLE);
+	__is_set_fd_config_eyedetect(is, FD_CONFIG_EYES_DETECT_ENABLE);
+	__is_set_fd_config_mouthdetect(is, FD_CONFIG_MOUTH_DETECT_DISABLE);
+	__is_set_fd_config_orientation(is, FD_CONFIG_ORIENTATION_DISABLE);
+	__is_set_fd_config_orientation_val(is, 0);
+}
diff --git a/drivers/media/platform/exynos4-is/fimc-is-param.h b/drivers/media/platform/exynos4-is/fimc-is-param.h
new file mode 100644
index 00000000000..f9358c27ae2
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-param.h
@@ -0,0 +1,1020 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2011 - 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Younghwan Joo <yhwan.joo@samsung.com>
+ *	    Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#ifndef FIMC_IS_PARAM_H_
+#define FIMC_IS_PARAM_H_
+
+#include <linux/compiler.h>
+
+#define FIMC_IS_CONFIG_TIMEOUT		3000 /* ms */
+#define IS_DEFAULT_WIDTH		1280
+#define IS_DEFAULT_HEIGHT		720
+
+#define DEFAULT_PREVIEW_STILL_WIDTH	IS_DEFAULT_WIDTH
+#define DEFAULT_PREVIEW_STILL_HEIGHT	IS_DEFAULT_HEIGHT
+#define DEFAULT_CAPTURE_STILL_WIDTH	IS_DEFAULT_WIDTH
+#define DEFAULT_CAPTURE_STILL_HEIGHT	IS_DEFAULT_HEIGHT
+#define DEFAULT_PREVIEW_VIDEO_WIDTH	IS_DEFAULT_WIDTH
+#define DEFAULT_PREVIEW_VIDEO_HEIGHT	IS_DEFAULT_HEIGHT
+#define DEFAULT_CAPTURE_VIDEO_WIDTH	IS_DEFAULT_WIDTH
+#define DEFAULT_CAPTURE_VIDEO_HEIGHT	IS_DEFAULT_HEIGHT
+
+#define DEFAULT_PREVIEW_STILL_FRAMERATE	30
+#define DEFAULT_CAPTURE_STILL_FRAMERATE	15
+#define DEFAULT_PREVIEW_VIDEO_FRAMERATE	30
+#define DEFAULT_CAPTURE_VIDEO_FRAMERATE	30
+
+#define FIMC_IS_REGION_VER		124 /* IS REGION VERSION 1.24 */
+#define FIMC_IS_PARAM_SIZE		(FIMC_IS_REGION_SIZE + 1)
+#define FIMC_IS_MAGIC_NUMBER		0x01020304
+#define FIMC_IS_PARAM_MAX_SIZE		64 /* in bytes */
+#define FIMC_IS_PARAM_MAX_ENTRIES	(FIMC_IS_PARAM_MAX_SIZE / 4)
+
+/* The parameter bitmask bit definitions. */
+enum is_param_bit {
+	PARAM_GLOBAL_SHOTMODE,
+	PARAM_SENSOR_CONTROL,
+	PARAM_SENSOR_OTF_OUTPUT,
+	PARAM_SENSOR_FRAME_RATE,
+	PARAM_BUFFER_CONTROL,
+	PARAM_BUFFER_OTF_INPUT,
+	PARAM_BUFFER_OTF_OUTPUT,
+	PARAM_ISP_CONTROL,
+	PARAM_ISP_OTF_INPUT,
+	PARAM_ISP_DMA1_INPUT,
+	/* 10 */
+	PARAM_ISP_DMA2_INPUT,
+	PARAM_ISP_AA,
+	PARAM_ISP_FLASH,
+	PARAM_ISP_AWB,
+	PARAM_ISP_IMAGE_EFFECT,
+	PARAM_ISP_ISO,
+	PARAM_ISP_ADJUST,
+	PARAM_ISP_METERING,
+	PARAM_ISP_AFC,
+	PARAM_ISP_OTF_OUTPUT,
+	/* 20 */
+	PARAM_ISP_DMA1_OUTPUT,
+	PARAM_ISP_DMA2_OUTPUT,
+	PARAM_DRC_CONTROL,
+	PARAM_DRC_OTF_INPUT,
+	PARAM_DRC_DMA_INPUT,
+	PARAM_DRC_OTF_OUTPUT,
+	PARAM_SCALERC_CONTROL,
+	PARAM_SCALERC_OTF_INPUT,
+	PARAM_SCALERC_IMAGE_EFFECT,
+	PARAM_SCALERC_INPUT_CROP,
+	/* 30 */
+	PARAM_SCALERC_OUTPUT_CROP,
+	PARAM_SCALERC_OTF_OUTPUT,
+	PARAM_SCALERC_DMA_OUTPUT,
+	PARAM_ODC_CONTROL,
+	PARAM_ODC_OTF_INPUT,
+	PARAM_ODC_OTF_OUTPUT,
+	PARAM_DIS_CONTROL,
+	PARAM_DIS_OTF_INPUT,
+	PARAM_DIS_OTF_OUTPUT,
+	PARAM_TDNR_CONTROL,
+	/* 40 */
+	PARAM_TDNR_OTF_INPUT,
+	PARAM_TDNR_1ST_FRAME,
+	PARAM_TDNR_OTF_OUTPUT,
+	PARAM_TDNR_DMA_OUTPUT,
+	PARAM_SCALERP_CONTROL,
+	PARAM_SCALERP_OTF_INPUT,
+	PARAM_SCALERP_IMAGE_EFFECT,
+	PARAM_SCALERP_INPUT_CROP,
+	PARAM_SCALERP_OUTPUT_CROP,
+	PARAM_SCALERP_ROTATION,
+	/* 50 */
+	PARAM_SCALERP_FLIP,
+	PARAM_SCALERP_OTF_OUTPUT,
+	PARAM_SCALERP_DMA_OUTPUT,
+	PARAM_FD_CONTROL,
+	PARAM_FD_OTF_INPUT,
+	PARAM_FD_DMA_INPUT,
+	PARAM_FD_CONFIG,
+};
+
+/* Interrupt map */
+#define	FIMC_IS_INT_GENERAL			0
+#define	FIMC_IS_INT_FRAME_DONE_ISP		1
+
+/* Input */
+
+#define CONTROL_COMMAND_STOP			0
+#define CONTROL_COMMAND_START			1
+
+#define CONTROL_BYPASS_DISABLE			0
+#define CONTROL_BYPASS_ENABLE			1
+
+#define CONTROL_ERROR_NONE			0
+
+/* OTF (On-The-Fly) input interface commands */
+#define OTF_INPUT_COMMAND_DISABLE		0
+#define OTF_INPUT_COMMAND_ENABLE		1
+
+/* OTF input interface color formats */
+enum oft_input_fmt {
+	OTF_INPUT_FORMAT_BAYER			= 0, /* 1 channel */
+	OTF_INPUT_FORMAT_YUV444			= 1, /* 3 channels */
+	OTF_INPUT_FORMAT_YUV422			= 2, /* 3 channels */
+	OTF_INPUT_FORMAT_YUV420			= 3, /* 3 channels */
+	OTF_INPUT_FORMAT_STRGEN_COLORBAR_BAYER	= 10,
+	OTF_INPUT_FORMAT_BAYER_DMA		= 11,
+};
+
+#define OTF_INPUT_ORDER_BAYER_GR_BG		0
+
+/* OTF input error codes */
+#define OTF_INPUT_ERROR_NONE			0 /* Input setting is done */
+
+/* DMA input commands */
+#define DMA_INPUT_COMMAND_DISABLE		0
+#define DMA_INPUT_COMMAND_ENABLE		1
+
+/* DMA input color formats */
+enum dma_input_fmt {
+	DMA_INPUT_FORMAT_BAYER			= 0,
+	DMA_INPUT_FORMAT_YUV444			= 1,
+	DMA_INPUT_FORMAT_YUV422			= 2,
+	DMA_INPUT_FORMAT_YUV420			= 3,
+};
+
+enum dma_input_order {
+	/* (for DMA_INPUT_PLANE_3) */
+	DMA_INPUT_ORDER_NO	= 0,
+	/* (only valid at DMA_INPUT_PLANE_2) */
+	DMA_INPUT_ORDER_CBCR	= 1,
+	/* (only valid at DMA_INPUT_PLANE_2) */
+	DMA_INPUT_ORDER_CRCB	= 2,
+	/* (only valid at DMA_INPUT_PLANE_1 & DMA_INPUT_FORMAT_YUV444) */
+	DMA_INPUT_ORDER_YCBCR	= 3,
+	/* (only valid at DMA_INPUT_FORMAT_YUV422 & DMA_INPUT_PLANE_1) */
+	DMA_INPUT_ORDER_YYCBCR	= 4,
+	/* (only valid at DMA_INPUT_FORMAT_YUV422 & DMA_INPUT_PLANE_1) */
+	DMA_INPUT_ORDER_YCBYCR	= 5,
+	/* (only valid at DMA_INPUT_FORMAT_YUV422 & DMA_INPUT_PLANE_1) */
+	DMA_INPUT_ORDER_YCRYCB	= 6,
+	/* (only valid at DMA_INPUT_FORMAT_YUV422 & DMA_INPUT_PLANE_1) */
+	DMA_INPUT_ORDER_CBYCRY	= 7,
+	/* (only valid at DMA_INPUT_FORMAT_YUV422 & DMA_INPUT_PLANE_1) */
+	DMA_INPUT_ORDER_CRYCBY	= 8,
+	/* (only valid at DMA_INPUT_FORMAT_BAYER) */
+	DMA_INPUT_ORDER_GR_BG	= 9
+};
+
+#define DMA_INPUT_ERROR_NONE			0 /* DMA input setting
+						     is done */
+/*
+ * Data output parameter definitions
+ */
+#define OTF_OUTPUT_CROP_DISABLE			0
+#define OTF_OUTPUT_CROP_ENABLE			1
+
+#define OTF_OUTPUT_COMMAND_DISABLE		0
+#define OTF_OUTPUT_COMMAND_ENABLE		1
+
+enum otf_output_fmt {
+	OTF_OUTPUT_FORMAT_YUV444		= 1,
+	OTF_OUTPUT_FORMAT_YUV422		= 2,
+	OTF_OUTPUT_FORMAT_YUV420		= 3,
+	OTF_OUTPUT_FORMAT_RGB			= 4,
+};
+
+#define OTF_OUTPUT_ORDER_BAYER_GR_BG		0
+
+#define OTF_OUTPUT_ERROR_NONE			0 /* Output Setting is done */
+
+#define DMA_OUTPUT_COMMAND_DISABLE		0
+#define DMA_OUTPUT_COMMAND_ENABLE		1
+
+enum dma_output_fmt {
+	DMA_OUTPUT_FORMAT_BAYER			= 0,
+	DMA_OUTPUT_FORMAT_YUV444		= 1,
+	DMA_OUTPUT_FORMAT_YUV422		= 2,
+	DMA_OUTPUT_FORMAT_YUV420		= 3,
+	DMA_OUTPUT_FORMAT_RGB			= 4,
+};
+
+enum dma_output_order {
+	DMA_OUTPUT_ORDER_NO		= 0,
+	/* for DMA_OUTPUT_PLANE_3 */
+	DMA_OUTPUT_ORDER_CBCR		= 1,
+	/* only valid at DMA_INPUT_PLANE_2) */
+	DMA_OUTPUT_ORDER_CRCB		= 2,
+	/* only valid at DMA_OUTPUT_PLANE_2) */
+	DMA_OUTPUT_ORDER_YYCBCR		= 3,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV422 & DMA_OUTPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_YCBYCR		= 4,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV422 & DMA_OUTPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_YCRYCB		= 5,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV422 & DMA_OUTPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_CBYCRY		= 6,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV422 & DMA_OUTPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_CRYCBY		= 7,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV422 & DMA_OUTPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_YCBCR		= 8,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV444 & DMA_OUPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_CRYCB		= 9,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV444 & DMA_OUPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_CRCBY		= 10,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV444 & DMA_OUPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_CBYCR		= 11,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV444 & DMA_OUPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_YCRCB		= 12,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV444 & DMA_OUPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_CBCRY		= 13,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV444 & DMA_OUPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_BGR		= 14,
+	/* only valid at DMA_OUTPUT_FORMAT_RGB */
+	DMA_OUTPUT_ORDER_GB_BG		= 15
+	/* only valid at DMA_OUTPUT_FORMAT_BAYER */
+};
+
+/* enum dma_output_notify_dma_done */
+#define DMA_OUTPUT_NOTIFY_DMA_DONE_DISABLE	0
+#define DMA_OUTPUT_NOTIFY_DMA_DONE_ENABLE	1
+
+/* DMA output error codes */
+#define DMA_OUTPUT_ERROR_NONE			0 /* DMA output setting
+						     is done */
+
+/* ----------------------  Global  ----------------------------------- */
+#define GLOBAL_SHOTMODE_ERROR_NONE		0 /* shot-mode setting
+						     is done */
+/* 3A lock commands */
+#define ISP_AA_COMMAND_START			0
+#define ISP_AA_COMMAND_STOP			1
+
+/* 3A lock target */
+#define ISP_AA_TARGET_AF			1
+#define ISP_AA_TARGET_AE			2
+#define ISP_AA_TARGET_AWB			4
+
+enum isp_af_mode {
+	ISP_AF_MODE_MANUAL			= 0,
+	ISP_AF_MODE_SINGLE			= 1,
+	ISP_AF_MODE_CONTINUOUS			= 2,
+	ISP_AF_MODE_TOUCH			= 3,
+	ISP_AF_MODE_SLEEP			= 4,
+	ISP_AF_MODE_INIT			= 5,
+	ISP_AF_MODE_SET_CENTER_WINDOW		= 6,
+	ISP_AF_MODE_SET_TOUCH_WINDOW		= 7
+};
+
+/* Face AF commands */
+#define ISP_AF_FACE_DISABLE			0
+#define ISP_AF_FACE_ENABLE			1
+
+/* AF range */
+#define ISP_AF_RANGE_NORMAL			0
+#define ISP_AF_RANGE_MACRO			1
+
+/* AF sleep */
+#define ISP_AF_SLEEP_OFF			0
+#define ISP_AF_SLEEP_ON				1
+
+/* Continuous AF commands */
+#define ISP_AF_CONTINUOUS_DISABLE		0
+#define ISP_AF_CONTINUOUS_ENABLE		1
+
+/* ISP AF error codes */
+#define ISP_AF_ERROR_NONE			0 /* AF mode change is done */
+#define ISP_AF_ERROR_NONE_LOCK_DONE		1 /* AF lock is done */
+
+/* Flash commands */
+#define ISP_FLASH_COMMAND_DISABLE		0
+#define ISP_FLASH_COMMAND_MANUAL_ON		1 /* (forced flash) */
+#define ISP_FLASH_COMMAND_AUTO			2
+#define ISP_FLASH_COMMAND_TORCH			3 /* 3 sec */
+
+/* Flash red-eye commads */
+#define ISP_FLASH_REDEYE_DISABLE		0
+#define ISP_FLASH_REDEYE_ENABLE			1
+
+/* Flash error codes */
+#define ISP_FLASH_ERROR_NONE			0 /* Flash setting is done */
+
+/* --------------------------  AWB  ------------------------------------ */
+enum isp_awb_command {
+	ISP_AWB_COMMAND_AUTO			= 0,
+	ISP_AWB_COMMAND_ILLUMINATION		= 1,
+	ISP_AWB_COMMAND_MANUAL			= 2
+};
+
+enum isp_awb_illumination {
+	ISP_AWB_ILLUMINATION_DAYLIGHT		= 0,
+	ISP_AWB_ILLUMINATION_CLOUDY		= 1,
+	ISP_AWB_ILLUMINATION_TUNGSTEN		= 2,
+	ISP_AWB_ILLUMINATION_FLUORESCENT	= 3
+};
+
+/* ISP AWN error codes */
+#define ISP_AWB_ERROR_NONE			0 /* AWB setting is done */
+
+/* --------------------------  Effect  ----------------------------------- */
+enum isp_imageeffect_command {
+	ISP_IMAGE_EFFECT_DISABLE		= 0,
+	ISP_IMAGE_EFFECT_MONOCHROME		= 1,
+	ISP_IMAGE_EFFECT_NEGATIVE_MONO		= 2,
+	ISP_IMAGE_EFFECT_NEGATIVE_COLOR		= 3,
+	ISP_IMAGE_EFFECT_SEPIA			= 4
+};
+
+/* Image effect error codes */
+#define ISP_IMAGE_EFFECT_ERROR_NONE		0 /* Image effect setting
+						     is done */
+/* ISO commands */
+#define ISP_ISO_COMMAND_AUTO			0
+#define ISP_ISO_COMMAND_MANUAL			1
+
+/* ISO error codes */
+#define ISP_ISO_ERROR_NONE			0 /* ISO setting is done */
+
+/* ISP adjust commands */
+#define ISP_ADJUST_COMMAND_AUTO			(0 << 0)
+#define ISP_ADJUST_COMMAND_MANUAL_CONTRAST	(1 << 0)
+#define ISP_ADJUST_COMMAND_MANUAL_SATURATION	(1 << 1)
+#define ISP_ADJUST_COMMAND_MANUAL_SHARPNESS	(1 << 2)
+#define ISP_ADJUST_COMMAND_MANUAL_EXPOSURE	(1 << 3)
+#define ISP_ADJUST_COMMAND_MANUAL_BRIGHTNESS	(1 << 4)
+#define ISP_ADJUST_COMMAND_MANUAL_HUE		(1 << 5)
+#define ISP_ADJUST_COMMAND_MANUAL_ALL		0x7f
+
+/* ISP adjustment error codes */
+#define ISP_ADJUST_ERROR_NONE			0 /* Adjust setting is done */
+
+/*
+ *  Exposure metering
+ */
+enum isp_metering_command {
+	ISP_METERING_COMMAND_AVERAGE	= 0,
+	ISP_METERING_COMMAND_SPOT	= 1,
+	ISP_METERING_COMMAND_MATRIX	= 2,
+	ISP_METERING_COMMAND_CENTER	= 3
+};
+
+/* ISP metering error codes */
+#define ISP_METERING_ERROR_NONE		0 /* Metering setting is done */
+
+/*
+ * AFC
+ */
+enum isp_afc_command {
+	ISP_AFC_COMMAND_DISABLE		= 0,
+	ISP_AFC_COMMAND_AUTO		= 1,
+	ISP_AFC_COMMAND_MANUAL		= 2,
+};
+
+#define ISP_AFC_MANUAL_50HZ		50
+#define ISP_AFC_MANUAL_60HZ		60
+
+/* ------------------------  SCENE MODE--------------------------------- */
+enum isp_scene_mode {
+	ISP_SCENE_NONE			= 0,
+	ISP_SCENE_PORTRAIT		= 1,
+	ISP_SCENE_LANDSCAPE		= 2,
+	ISP_SCENE_SPORTS		= 3,
+	ISP_SCENE_PARTYINDOOR		= 4,
+	ISP_SCENE_BEACHSNOW		= 5,
+	ISP_SCENE_SUNSET		= 6,
+	ISP_SCENE_DAWN			= 7,
+	ISP_SCENE_FALL			= 8,
+	ISP_SCENE_NIGHT			= 9,
+	ISP_SCENE_AGAINSTLIGHTWLIGHT	= 10,
+	ISP_SCENE_AGAINSTLIGHTWOLIGHT	= 11,
+	ISP_SCENE_FIRE			= 12,
+	ISP_SCENE_TEXT			= 13,
+	ISP_SCENE_CANDLE		= 14
+};
+
+/* AFC error codes */
+#define ISP_AFC_ERROR_NONE		0 /* AFC setting is done */
+
+/* ----------------------------  FD  ------------------------------------- */
+enum fd_config_command {
+	FD_CONFIG_COMMAND_MAXIMUM_NUMBER	= 0x1,
+	FD_CONFIG_COMMAND_ROLL_ANGLE		= 0x2,
+	FD_CONFIG_COMMAND_YAW_ANGLE		= 0x4,
+	FD_CONFIG_COMMAND_SMILE_MODE		= 0x8,
+	FD_CONFIG_COMMAND_BLINK_MODE		= 0x10,
+	FD_CONFIG_COMMAND_EYES_DETECT		= 0x20,
+	FD_CONFIG_COMMAND_MOUTH_DETECT		= 0x40,
+	FD_CONFIG_COMMAND_ORIENTATION		= 0x80,
+	FD_CONFIG_COMMAND_ORIENTATION_VALUE	= 0x100
+};
+
+enum fd_config_roll_angle {
+	FD_CONFIG_ROLL_ANGLE_BASIC		= 0,
+	FD_CONFIG_ROLL_ANGLE_PRECISE_BASIC	= 1,
+	FD_CONFIG_ROLL_ANGLE_SIDES		= 2,
+	FD_CONFIG_ROLL_ANGLE_PRECISE_SIDES	= 3,
+	FD_CONFIG_ROLL_ANGLE_FULL		= 4,
+	FD_CONFIG_ROLL_ANGLE_PRECISE_FULL	= 5,
+};
+
+enum fd_config_yaw_angle {
+	FD_CONFIG_YAW_ANGLE_0			= 0,
+	FD_CONFIG_YAW_ANGLE_45			= 1,
+	FD_CONFIG_YAW_ANGLE_90			= 2,
+	FD_CONFIG_YAW_ANGLE_45_90		= 3,
+};
+
+/* Smile mode configuration */
+#define FD_CONFIG_SMILE_MODE_DISABLE		0
+#define FD_CONFIG_SMILE_MODE_ENABLE		1
+
+/* Blink mode configuration */
+#define FD_CONFIG_BLINK_MODE_DISABLE		0
+#define FD_CONFIG_BLINK_MODE_ENABLE		1
+
+/* Eyes detection configuration */
+#define FD_CONFIG_EYES_DETECT_DISABLE		0
+#define FD_CONFIG_EYES_DETECT_ENABLE		1
+
+/* Mouth detection configuration */
+#define FD_CONFIG_MOUTH_DETECT_DISABLE		0
+#define FD_CONFIG_MOUTH_DETECT_ENABLE		1
+
+#define FD_CONFIG_ORIENTATION_DISABLE		0
+#define FD_CONFIG_ORIENTATION_ENABLE		1
+
+struct param_control {
+	u32 cmd;
+	u32 bypass;
+	u32 buffer_address;
+	u32 buffer_size;
+	u32 skip_frames; /* only valid at ISP */
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 6];
+	u32 err;
+};
+
+struct param_otf_input {
+	u32 cmd;
+	u32 width;
+	u32 height;
+	u32 format;
+	u32 bitwidth;
+	u32 order;
+	u32 crop_offset_x;
+	u32 crop_offset_y;
+	u32 crop_width;
+	u32 crop_height;
+	u32 frametime_min;
+	u32 frametime_max;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 13];
+	u32 err;
+};
+
+struct param_dma_input {
+	u32 cmd;
+	u32 width;
+	u32 height;
+	u32 format;
+	u32 bitwidth;
+	u32 plane;
+	u32 order;
+	u32 buffer_number;
+	u32 buffer_address;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 10];
+	u32 err;
+};
+
+struct param_otf_output {
+	u32 cmd;
+	u32 width;
+	u32 height;
+	u32 format;
+	u32 bitwidth;
+	u32 order;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 7];
+	u32 err;
+};
+
+struct param_dma_output {
+	u32 cmd;
+	u32 width;
+	u32 height;
+	u32 format;
+	u32 bitwidth;
+	u32 plane;
+	u32 order;
+	u32 buffer_number;
+	u32 buffer_address;
+	u32 notify_dma_done;
+	u32 dma_out_mask;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 12];
+	u32 err;
+};
+
+struct param_global_shotmode {
+	u32 cmd;
+	u32 skip_frames;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 3];
+	u32 err;
+};
+
+struct param_sensor_framerate {
+	u32 frame_rate;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 2];
+	u32 err;
+};
+
+struct param_isp_aa {
+	u32 cmd;
+	u32 target;
+	u32 mode;
+	u32 scene;
+	u32 sleep;
+	u32 face;
+	u32 touch_x;
+	u32 touch_y;
+	u32 manual_af_setting;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 10];
+	u32 err;
+};
+
+struct param_isp_flash {
+	u32 cmd;
+	u32 redeye;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 3];
+	u32 err;
+};
+
+struct param_isp_awb {
+	u32 cmd;
+	u32 illumination;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 3];
+	u32 err;
+};
+
+struct param_isp_imageeffect {
+	u32 cmd;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 2];
+	u32 err;
+};
+
+struct param_isp_iso {
+	u32 cmd;
+	u32 value;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 3];
+	u32 err;
+};
+
+struct param_isp_adjust {
+	u32 cmd;
+	s32 contrast;
+	s32 saturation;
+	s32 sharpness;
+	s32 exposure;
+	s32 brightness;
+	s32 hue;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 8];
+	u32 err;
+};
+
+struct param_isp_metering {
+	u32 cmd;
+	u32 win_pos_x;
+	u32 win_pos_y;
+	u32 win_width;
+	u32 win_height;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 6];
+	u32 err;
+};
+
+struct param_isp_afc {
+	u32 cmd;
+	u32 manual;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 3];
+	u32 err;
+};
+
+struct param_scaler_imageeffect {
+	u32 cmd;
+	u32 arbitrary_cb;
+	u32 arbitrary_cr;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 4];
+	u32 err;
+};
+
+struct param_scaler_input_crop {
+	u32 cmd;
+	u32 crop_offset_x;
+	u32 crop_offset_y;
+	u32 crop_width;
+	u32 crop_height;
+	u32 in_width;
+	u32 in_height;
+	u32 out_width;
+	u32 out_height;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 10];
+	u32 err;
+};
+
+struct param_scaler_output_crop {
+	u32 cmd;
+	u32 crop_offset_x;
+	u32 crop_offset_y;
+	u32 crop_width;
+	u32 crop_height;
+	u32 out_format;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 7];
+	u32 err;
+};
+
+struct param_scaler_rotation {
+	u32 cmd;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 2];
+	u32 err;
+};
+
+struct param_scaler_flip {
+	u32 cmd;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 2];
+	u32 err;
+};
+
+struct param_3dnr_1stframe {
+	u32 cmd;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 2];
+	u32 err;
+};
+
+struct param_fd_config {
+	u32 cmd;
+	u32 max_number;
+	u32 roll_angle;
+	u32 yaw_angle;
+	u32 smile_mode;
+	u32 blink_mode;
+	u32 eye_detect;
+	u32 mouth_detect;
+	u32 orientation;
+	u32 orientation_value;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 11];
+	u32 err;
+};
+
+struct global_param {
+	struct param_global_shotmode	shotmode;
+};
+
+struct sensor_param {
+	struct param_control		control;
+	struct param_otf_output		otf_output;
+	struct param_sensor_framerate	frame_rate;
+} __packed;
+
+struct buffer_param {
+	struct param_control		control;
+	struct param_otf_input		otf_input;
+	struct param_otf_output		otf_output;
+} __packed;
+
+struct isp_param {
+	struct param_control		control;
+	struct param_otf_input		otf_input;
+	struct param_dma_input		dma1_input;
+	struct param_dma_input		dma2_input;
+	struct param_isp_aa		aa;
+	struct param_isp_flash		flash;
+	struct param_isp_awb		awb;
+	struct param_isp_imageeffect	effect;
+	struct param_isp_iso		iso;
+	struct param_isp_adjust		adjust;
+	struct param_isp_metering	metering;
+	struct param_isp_afc		afc;
+	struct param_otf_output		otf_output;
+	struct param_dma_output		dma1_output;
+	struct param_dma_output		dma2_output;
+} __packed;
+
+struct drc_param {
+	struct param_control		control;
+	struct param_otf_input		otf_input;
+	struct param_dma_input		dma_input;
+	struct param_otf_output		otf_output;
+} __packed;
+
+struct scalerc_param {
+	struct param_control		control;
+	struct param_otf_input		otf_input;
+	struct param_scaler_imageeffect	effect;
+	struct param_scaler_input_crop	input_crop;
+	struct param_scaler_output_crop	output_crop;
+	struct param_otf_output		otf_output;
+	struct param_dma_output		dma_output;
+} __packed;
+
+struct odc_param {
+	struct param_control		control;
+	struct param_otf_input		otf_input;
+	struct param_otf_output		otf_output;
+} __packed;
+
+struct dis_param {
+	struct param_control		control;
+	struct param_otf_output		otf_input;
+	struct param_otf_output		otf_output;
+} __packed;
+
+struct tdnr_param {
+	struct param_control		control;
+	struct param_otf_input		otf_input;
+	struct param_3dnr_1stframe	frame;
+	struct param_otf_output		otf_output;
+	struct param_dma_output		dma_output;
+} __packed;
+
+struct scalerp_param {
+	struct param_control		control;
+	struct param_otf_input		otf_input;
+	struct param_scaler_imageeffect	effect;
+	struct param_scaler_input_crop	input_crop;
+	struct param_scaler_output_crop	output_crop;
+	struct param_scaler_rotation	rotation;
+	struct param_scaler_flip	flip;
+	struct param_otf_output		otf_output;
+	struct param_dma_output		dma_output;
+} __packed;
+
+struct fd_param {
+	struct param_control		control;
+	struct param_otf_input		otf_input;
+	struct param_dma_input		dma_input;
+	struct param_fd_config		config;
+} __packed;
+
+struct is_param_region {
+	struct global_param		global;
+	struct sensor_param		sensor;
+	struct buffer_param		buf;
+	struct isp_param		isp;
+	struct drc_param		drc;
+	struct scalerc_param		scalerc;
+	struct odc_param		odc;
+	struct dis_param		dis;
+	struct tdnr_param		tdnr;
+	struct scalerp_param		scalerp;
+	struct fd_param			fd;
+} __packed;
+
+#define NUMBER_OF_GAMMA_CURVE_POINTS	32
+
+struct is_tune_sensor {
+	u32 exposure;
+	u32 analog_gain;
+	u32 frame_rate;
+	u32 actuator_position;
+};
+
+struct is_tune_gammacurve {
+	u32 num_pts_x[NUMBER_OF_GAMMA_CURVE_POINTS];
+	u32 num_pts_y_r[NUMBER_OF_GAMMA_CURVE_POINTS];
+	u32 num_pts_y_g[NUMBER_OF_GAMMA_CURVE_POINTS];
+	u32 num_pts_y_b[NUMBER_OF_GAMMA_CURVE_POINTS];
+};
+
+struct is_tune_isp {
+	/* Brightness level: range 0...100, default 7. */
+	u32 brightness_level;
+	/* Contrast level: range -127...127, default 0. */
+	s32 contrast_level;
+	/* Saturation level: range -127...127, default 0. */
+	s32 saturation_level;
+	s32 gamma_level;
+	struct is_tune_gammacurve gamma_curve[4];
+	/* Hue: range -127...127, default 0. */
+	s32 hue;
+	/* Sharpness blur: range -127...127, default 0. */
+	s32 sharpness_blur;
+	/* Despeckle : range -127~127, default : 0 */
+	s32 despeckle;
+	/* Edge color supression: range -127...127, default 0. */
+	s32 edge_color_supression;
+	/* Noise reduction: range -127...127, default 0. */
+	s32 noise_reduction;
+	/* (32 * 4 + 9) * 4 = 548 bytes */
+} __packed;
+
+struct is_tune_region {
+	struct is_tune_sensor sensor;
+	struct is_tune_isp isp;
+} __packed;
+
+struct rational {
+	u32 num;
+	u32 den;
+};
+
+struct srational {
+	s32 num;
+	s32 den;
+};
+
+#define FLASH_FIRED_SHIFT			0
+#define FLASH_NOT_FIRED				0
+#define FLASH_FIRED				1
+
+#define FLASH_STROBE_SHIFT			1
+#define FLASH_STROBE_NO_DETECTION		0
+#define FLASH_STROBE_RESERVED			1
+#define FLASH_STROBE_RETURN_LIGHT_NOT_DETECTED	2
+#define FLASH_STROBE_RETURN_LIGHT_DETECTED	3
+
+#define FLASH_MODE_SHIFT			3
+#define FLASH_MODE_UNKNOWN			0
+#define FLASH_MODE_COMPULSORY_FLASH_FIRING	1
+#define FLASH_MODE_COMPULSORY_FLASH_SUPPRESSION	2
+#define FLASH_MODE_AUTO_MODE			3
+
+#define FLASH_FUNCTION_SHIFT			5
+#define FLASH_FUNCTION_PRESENT			0
+#define FLASH_FUNCTION_NONE			1
+
+#define FLASH_RED_EYE_SHIFT			6
+#define FLASH_RED_EYE_DISABLED			0
+#define FLASH_RED_EYE_SUPPORTED			1
+
+enum apex_aperture_value {
+	F1_0	= 0,
+	F1_4	= 1,
+	F2_0	= 2,
+	F2_8	= 3,
+	F4_0	= 4,
+	F5_6	= 5,
+	F8_9	= 6,
+	F11_0	= 7,
+	F16_0	= 8,
+	F22_0	= 9,
+	F32_0	= 10,
+};
+
+struct exif_attribute {
+	struct rational exposure_time;
+	struct srational shutter_speed;
+	u32 iso_speed_rating;
+	u32 flash;
+	struct srational brightness;
+} __packed;
+
+struct is_frame_header {
+	u32 valid;
+	u32 bad_mark;
+	u32 captured;
+	u32 frame_number;
+	struct exif_attribute exif;
+} __packed;
+
+struct is_fd_rect {
+	u32 offset_x;
+	u32 offset_y;
+	u32 width;
+	u32 height;
+};
+
+struct is_face_marker {
+	u32 frame_number;
+	struct is_fd_rect face;
+	struct is_fd_rect left_eye;
+	struct is_fd_rect right_eye;
+	struct is_fd_rect mouth;
+	u32 roll_angle;
+	u32 yaw_angle;
+	u32 confidence;
+	s32 smile_level;
+	s32 blink_level;
+} __packed;
+
+#define MAX_FRAME_COUNT				8
+#define MAX_FRAME_COUNT_PREVIEW			4
+#define MAX_FRAME_COUNT_CAPTURE			1
+#define MAX_FACE_COUNT				16
+#define MAX_SHARED_COUNT			500
+
+struct is_region {
+	struct is_param_region parameter;
+	struct is_tune_region tune;
+	struct is_frame_header header[MAX_FRAME_COUNT];
+	struct is_face_marker face[MAX_FACE_COUNT];
+	u32 shared[MAX_SHARED_COUNT];
+} __packed;
+
+struct is_debug_frame_descriptor {
+	u32 sensor_frame_time;
+	u32 sensor_exposure_time;
+	s32 sensor_analog_gain;
+	/* monitor for AA */
+	u32 req_lei;
+
+	u32 next_next_lei_exp;
+	u32 next_next_lei_a_gain;
+	u32 next_next_lei_d_gain;
+	u32 next_next_lei_statlei;
+	u32 next_next_lei_lei;
+
+	u32 dummy0;
+};
+
+#define MAX_FRAMEDESCRIPTOR_CONTEXT_NUM	(30*20)	/* 600 frames */
+#define MAX_VERSION_DISPLAY_BUF	32
+
+struct is_share_region {
+	u32 frame_time;
+	u32 exposure_time;
+	s32 analog_gain;
+
+	u32 r_gain;
+	u32 g_gain;
+	u32 b_gain;
+
+	u32 af_position;
+	u32 af_status;
+	/* 0 : SIRC_ISP_CAMERA_AUTOFOCUSMESSAGE_NOMESSAGE */
+	/* 1 : SIRC_ISP_CAMERA_AUTOFOCUSMESSAGE_REACHED */
+	/* 2 : SIRC_ISP_CAMERA_AUTOFOCUSMESSAGE_UNABLETOREACH */
+	/* 3 : SIRC_ISP_CAMERA_AUTOFOCUSMESSAGE_LOST */
+	/* default : unknown */
+	u32 af_scene_type;
+
+	u32 frame_descp_onoff_control;
+	u32 frame_descp_update_done;
+	u32 frame_descp_idx;
+	u32 frame_descp_max_idx;
+	struct is_debug_frame_descriptor
+		dbg_frame_descp_ctx[MAX_FRAMEDESCRIPTOR_CONTEXT_NUM];
+
+	u32 chip_id;
+	u32 chip_rev_no;
+	u8 isp_fw_ver_no[MAX_VERSION_DISPLAY_BUF];
+	u8 isp_fw_ver_date[MAX_VERSION_DISPLAY_BUF];
+	u8 sirc_sdk_ver_no[MAX_VERSION_DISPLAY_BUF];
+	u8 sirc_sdk_rev_no[MAX_VERSION_DISPLAY_BUF];
+	u8 sirc_sdk_rev_date[MAX_VERSION_DISPLAY_BUF];
+} __packed;
+
+struct is_debug_control {
+	u32 write_point;	/* 0~ 500KB boundary */
+	u32 assert_flag;	/* 0: Not invoked, 1: Invoked */
+	u32 pabort_flag;	/* 0: Not invoked, 1: Invoked */
+	u32 dabort_flag;	/* 0: Not invoked, 1: Invoked */
+};
+
+struct sensor_open_extended {
+	u32 actuator_type;
+	u32 mclk;
+	u32 mipi_lane_num;
+	u32 mipi_speed;
+	/* Skip setfile loading when fast_open_sensor is not 0 */
+	u32 fast_open_sensor;
+	/* Activating sensor self calibration mode (6A3) */
+	u32 self_calibration_mode;
+	/* This field is to adjust I2c clock based on ACLK200 */
+	/* This value is varied in case of rev 0.2 */
+	u32 i2c_sclk;
+};
+
+struct fimc_is;
+
+int fimc_is_hw_get_sensor_max_framerate(struct fimc_is *is);
+void fimc_is_set_initial_params(struct fimc_is *is);
+unsigned int __get_pending_param_count(struct fimc_is *is);
+
+int  __is_hw_update_params(struct fimc_is *is);
+void __is_get_frame_size(struct fimc_is *is, struct v4l2_mbus_framefmt *mf);
+void __is_set_frame_size(struct fimc_is *is, struct v4l2_mbus_framefmt *mf);
+void __is_set_sensor(struct fimc_is *is, int fps);
+void __is_set_isp_aa_ae(struct fimc_is *is);
+void __is_set_isp_flash(struct fimc_is *is, u32 cmd, u32 redeye);
+void __is_set_isp_awb(struct fimc_is *is, u32 cmd, u32 val);
+void __is_set_isp_effect(struct fimc_is *is, u32 cmd);
+void __is_set_isp_iso(struct fimc_is *is, u32 cmd, u32 val);
+void __is_set_isp_adjust(struct fimc_is *is, u32 cmd, u32 val);
+void __is_set_isp_metering(struct fimc_is *is, u32 id, u32 val);
+void __is_set_isp_afc(struct fimc_is *is, u32 cmd, u32 val);
+void __is_set_drc_control(struct fimc_is *is, u32 val);
+void __is_set_fd_control(struct fimc_is *is, u32 val);
+void __is_set_fd_config_maxface(struct fimc_is *is, u32 val);
+void __is_set_fd_config_rollangle(struct fimc_is *is, u32 val);
+void __is_set_fd_config_yawangle(struct fimc_is *is, u32 val);
+void __is_set_fd_config_smilemode(struct fimc_is *is, u32 val);
+void __is_set_fd_config_blinkmode(struct fimc_is *is, u32 val);
+void __is_set_fd_config_eyedetect(struct fimc_is *is, u32 val);
+void __is_set_fd_config_mouthdetect(struct fimc_is *is, u32 val);
+void __is_set_fd_config_orientation(struct fimc_is *is, u32 val);
+void __is_set_fd_config_orientation_val(struct fimc_is *is, u32 val);
+void __is_set_isp_aa_af_mode(struct fimc_is *is, int cmd);
+void __is_set_isp_aa_af_start_stop(struct fimc_is *is, int cmd);
+
+#endif
diff --git a/drivers/media/platform/exynos4-is/fimc-is-regs.c b/drivers/media/platform/exynos4-is/fimc-is-regs.c
new file mode 100644
index 00000000000..b0ff67bc1b0
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-regs.c
@@ -0,0 +1,243 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2012 - 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Younghwan Joo <yhwan.joo@samsung.com>
+ *          Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/delay.h>
+
+#include "fimc-is.h"
+#include "fimc-is-command.h"
+#include "fimc-is-regs.h"
+#include "fimc-is-sensor.h"
+
+void fimc_is_fw_clear_irq1(struct fimc_is *is, unsigned int nr)
+{
+	mcuctl_write(1UL << nr, is, MCUCTL_REG_INTCR1);
+}
+
+void fimc_is_fw_clear_irq2(struct fimc_is *is)
+{
+	u32 cfg = mcuctl_read(is, MCUCTL_REG_INTSR2);
+	mcuctl_write(cfg, is, MCUCTL_REG_INTCR2);
+}
+
+void fimc_is_hw_set_intgr0_gd0(struct fimc_is *is)
+{
+	mcuctl_write(INTGR0_INTGD(0), is, MCUCTL_REG_INTGR0);
+}
+
+int fimc_is_hw_wait_intsr0_intsd0(struct fimc_is *is)
+{
+	unsigned int timeout = 2000;
+	u32 cfg, status;
+
+	cfg = mcuctl_read(is, MCUCTL_REG_INTSR0);
+	status = INTSR0_GET_INTSD(0, cfg);
+
+	while (status) {
+		cfg = mcuctl_read(is, MCUCTL_REG_INTSR0);
+		status = INTSR0_GET_INTSD(0, cfg);
+		if (timeout == 0) {
+			dev_warn(&is->pdev->dev, "%s timeout\n",
+				 __func__);
+			return -ETIME;
+		}
+		timeout--;
+		udelay(1);
+	}
+	return 0;
+}
+
+int fimc_is_hw_wait_intmsr0_intmsd0(struct fimc_is *is)
+{
+	unsigned int timeout = 2000;
+	u32 cfg, status;
+
+	cfg = mcuctl_read(is, MCUCTL_REG_INTMSR0);
+	status = INTMSR0_GET_INTMSD(0, cfg);
+
+	while (status) {
+		cfg = mcuctl_read(is, MCUCTL_REG_INTMSR0);
+		status = INTMSR0_GET_INTMSD(0, cfg);
+		if (timeout == 0) {
+			dev_warn(&is->pdev->dev, "%s timeout\n",
+				 __func__);
+			return -ETIME;
+		}
+		timeout--;
+		udelay(1);
+	}
+	return 0;
+}
+
+int fimc_is_hw_set_param(struct fimc_is *is)
+{
+	struct chain_config *config = &is->config[is->config_index];
+	unsigned int param_count = __get_pending_param_count(is);
+
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+
+	mcuctl_write(HIC_SET_PARAMETER, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	mcuctl_write(is->config_index, is, MCUCTL_REG_ISSR(2));
+
+	mcuctl_write(param_count, is, MCUCTL_REG_ISSR(3));
+	mcuctl_write(config->p_region_index1, is, MCUCTL_REG_ISSR(4));
+	mcuctl_write(config->p_region_index2, is, MCUCTL_REG_ISSR(5));
+
+	fimc_is_hw_set_intgr0_gd0(is);
+	return 0;
+}
+
+int fimc_is_hw_set_tune(struct fimc_is *is)
+{
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+
+	mcuctl_write(HIC_SET_TUNE, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	mcuctl_write(is->h2i_cmd.entry_id, is, MCUCTL_REG_ISSR(2));
+
+	fimc_is_hw_set_intgr0_gd0(is);
+	return 0;
+}
+
+#define FIMC_IS_MAX_PARAMS	4
+
+int fimc_is_hw_get_params(struct fimc_is *is, unsigned int num_args)
+{
+	int i;
+
+	if (num_args > FIMC_IS_MAX_PARAMS)
+		return -EINVAL;
+
+	is->i2h_cmd.num_args = num_args;
+
+	for (i = 0; i < FIMC_IS_MAX_PARAMS; i++) {
+		if (i < num_args)
+			is->i2h_cmd.args[i] = mcuctl_read(is,
+					MCUCTL_REG_ISSR(12 + i));
+		else
+			is->i2h_cmd.args[i] = 0;
+	}
+	return 0;
+}
+
+void fimc_is_hw_set_sensor_num(struct fimc_is *is)
+{
+	pr_debug("setting sensor index to: %d\n", is->sensor_index);
+
+	mcuctl_write(IH_REPLY_DONE, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	mcuctl_write(IHC_GET_SENSOR_NUM, is, MCUCTL_REG_ISSR(2));
+	mcuctl_write(FIMC_IS_SENSOR_NUM, is, MCUCTL_REG_ISSR(3));
+}
+
+void fimc_is_hw_close_sensor(struct fimc_is *is, unsigned int index)
+{
+	if (is->sensor_index != index)
+		return;
+
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+	mcuctl_write(HIC_CLOSE_SENSOR, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(2));
+	fimc_is_hw_set_intgr0_gd0(is);
+}
+
+void fimc_is_hw_get_setfile_addr(struct fimc_is *is)
+{
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+	mcuctl_write(HIC_GET_SET_FILE_ADDR, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	fimc_is_hw_set_intgr0_gd0(is);
+}
+
+void fimc_is_hw_load_setfile(struct fimc_is *is)
+{
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+	mcuctl_write(HIC_LOAD_SET_FILE, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	fimc_is_hw_set_intgr0_gd0(is);
+}
+
+int fimc_is_hw_change_mode(struct fimc_is *is)
+{
+	const u8 cmd[] = {
+		HIC_PREVIEW_STILL, HIC_PREVIEW_VIDEO,
+		HIC_CAPTURE_STILL, HIC_CAPTURE_VIDEO,
+	};
+
+	if (WARN_ON(is->config_index > ARRAY_SIZE(cmd)))
+		return -EINVAL;
+
+	mcuctl_write(cmd[is->config_index], is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	mcuctl_write(is->setfile.sub_index, is, MCUCTL_REG_ISSR(2));
+	fimc_is_hw_set_intgr0_gd0(is);
+	return 0;
+}
+
+void fimc_is_hw_stream_on(struct fimc_is *is)
+{
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+	mcuctl_write(HIC_STREAM_ON, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	mcuctl_write(0, is, MCUCTL_REG_ISSR(2));
+	fimc_is_hw_set_intgr0_gd0(is);
+}
+
+void fimc_is_hw_stream_off(struct fimc_is *is)
+{
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+	mcuctl_write(HIC_STREAM_OFF, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	fimc_is_hw_set_intgr0_gd0(is);
+}
+
+void fimc_is_hw_subip_power_off(struct fimc_is *is)
+{
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+	mcuctl_write(HIC_POWER_DOWN, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	fimc_is_hw_set_intgr0_gd0(is);
+}
+
+int fimc_is_itf_s_param(struct fimc_is *is, bool update)
+{
+	int ret;
+
+	if (update)
+		__is_hw_update_params(is);
+
+	fimc_is_mem_barrier();
+
+	clear_bit(IS_ST_BLOCK_CMD_CLEARED, &is->state);
+	fimc_is_hw_set_param(is);
+	ret = fimc_is_wait_event(is, IS_ST_BLOCK_CMD_CLEARED, 1,
+				FIMC_IS_CONFIG_TIMEOUT);
+	if (ret < 0)
+		dev_err(&is->pdev->dev, "%s() timeout\n", __func__);
+
+	return ret;
+}
+
+int fimc_is_itf_mode_change(struct fimc_is *is)
+{
+	int ret;
+
+	clear_bit(IS_ST_CHANGE_MODE, &is->state);
+	fimc_is_hw_change_mode(is);
+	ret = fimc_is_wait_event(is, IS_ST_CHANGE_MODE, 1,
+				FIMC_IS_CONFIG_TIMEOUT);
+	if (!ret < 0)
+		dev_err(&is->pdev->dev, "%s(): mode change (%d) timeout\n",
+			__func__, is->config_index);
+	return ret;
+}
diff --git a/drivers/media/platform/exynos4-is/fimc-is-regs.h b/drivers/media/platform/exynos4-is/fimc-is-regs.h
new file mode 100644
index 00000000000..5fa2fda4674
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-regs.h
@@ -0,0 +1,164 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *          Younghwan Joo <yhwan.joo@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#ifndef FIMC_IS_REG_H_
+#define FIMC_IS_REG_H_
+
+/* WDT_ISP register */
+#define REG_WDT_ISP			0x00170000
+
+/* MCUCTL registers base offset */
+#define MCUCTL_BASE			0x00180000
+
+/* MCU Controller Register */
+#define MCUCTL_REG_MCUCTRL		(MCUCTL_BASE + 0x00)
+#define MCUCTRL_MSWRST			(1 << 0)
+
+/* Boot Base Offset Address Register */
+#define MCUCTL_REG_BBOAR		(MCUCTL_BASE + 0x04)
+
+/* Interrupt Generation Register 0 from Host CPU to VIC */
+#define MCUCTL_REG_INTGR0		(MCUCTL_BASE + 0x08)
+/* __n = 0...9 */
+#define INTGR0_INTGC(__n)		(1 << ((__n) + 16))
+/* __n = 0...5 */
+#define INTGR0_INTGD(__n)		(1 << (__n))
+
+/* Interrupt Clear Register 0 from Host CPU to VIC */
+#define MCUCTL_REG_INTCR0		(MCUCTL_BASE + 0x0c)
+/* __n = 0...9 */
+#define INTCR0_INTGC(__n)		(1 << ((__n) + 16))
+/* __n = 0...5 */
+#define INTCR0_INTCD(__n)		(1 << ((__n) + 16))
+
+/* Interrupt Mask Register 0 from Host CPU to VIC */
+#define MCUCTL_REG_INTMR0		(MCUCTL_BASE + 0x10)
+/* __n = 0...9 */
+#define INTMR0_INTMC(__n)		(1 << ((__n) + 16))
+/* __n = 0...5 */
+#define INTMR0_INTMD(__n)		(1 << (__n))
+
+/* Interrupt Status Register 0 from Host CPU to VIC */
+#define MCUCTL_REG_INTSR0		(MCUCTL_BASE + 0x14)
+/* __n (bit number) = 0...4 */
+#define INTSR0_GET_INTSD(x, __n)	(((x) >> (__n)) & 0x1)
+/* __n (bit number) = 0...9 */
+#define INTSR0_GET_INTSC(x, __n)	(((x) >> ((__n) + 16)) & 0x1)
+
+/* Interrupt Mask Status Register 0 from Host CPU to VIC */
+#define MCUCTL_REG_INTMSR0		(MCUCTL_BASE + 0x18)
+/* __n (bit number) = 0...4 */
+#define INTMSR0_GET_INTMSD(x, __n)	(((x) >> (__n)) & 0x1)
+/* __n (bit number) = 0...9 */
+#define INTMSR0_GET_INTMSC(x, __n)	(((x) >> ((__n) + 16)) & 0x1)
+
+/* Interrupt Generation Register 1 from ISP CPU to Host IC */
+#define MCUCTL_REG_INTGR1		(MCUCTL_BASE + 0x1c)
+/* __n = 0...9 */
+#define INTGR1_INTGC(__n)		(1 << (__n))
+
+/* Interrupt Clear Register 1 from ISP CPU to Host IC */
+#define MCUCTL_REG_INTCR1		(MCUCTL_BASE + 0x20)
+/* __n = 0...9 */
+#define INTCR1_INTCC(__n)		(1 << (__n))
+
+/* Interrupt Mask Register 1 from ISP CPU to Host IC */
+#define MCUCTL_REG_INTMR1		(MCUCTL_BASE + 0x24)
+/* __n = 0...9 */
+#define INTMR1_INTMC(__n)		(1 << (__n))
+
+/* Interrupt Status Register 1 from ISP CPU to Host IC */
+#define MCUCTL_REG_INTSR1		(MCUCTL_BASE + 0x28)
+/* Interrupt Mask Status Register 1 from ISP CPU to Host IC */
+#define MCUCTL_REG_INTMSR1		(MCUCTL_BASE + 0x2c)
+
+/* Interrupt Clear Register 2 from ISP BLK's interrupts to Host IC */
+#define MCUCTL_REG_INTCR2		(MCUCTL_BASE + 0x30)
+/* __n = 0...5 */
+#define INTCR2_INTCC(__n)		(1 << ((__n) + 16))
+
+/* Interrupt Mask Register 2 from ISP BLK's interrupts to Host IC */
+#define MCUCTL_REG_INTMR2		(MCUCTL_BASE + 0x34)
+/* __n = 0...25 */
+#define INTMR2_INTMCIS(__n)		(1 << (__n))
+
+/* Interrupt Status Register 2 from ISP BLK's interrupts to Host IC */
+#define MCUCTL_REG_INTSR2		(MCUCTL_BASE + 0x38)
+/* Interrupt Mask Status Register 2 from ISP BLK's interrupts to Host IC */
+#define MCUCTL_REG_INTMSR2		(MCUCTL_BASE + 0x3c)
+
+/* General Purpose Output Control Register (0~17) */
+#define MCUCTL_REG_GPOCTLR		(MCUCTL_BASE + 0x40)
+/* __n = 0...17 */
+#define GPOCTLR_GPOG(__n)		(1 << (__n))
+
+/* General Purpose Pad Output Enable Register (0~17) */
+#define MCUCTL_REG_GPOENCTLR		(MCUCTL_BASE + 0x44)
+/* __n = 0...17 */
+#define GPOENCTLR_GPOEN(__n)		(1 << (__n))
+
+/* General Purpose Input Control Register (0~17) */
+#define MCUCTL_REG_GPICTLR		(MCUCTL_BASE + 0x48)
+
+/* Shared registers between ISP CPU and the host CPU - ISSRxx */
+
+/* ISSR(1): Command Host -> IS */
+/* ISSR(1): Sensor ID for Command, ISSR2...5 = Parameter 1...4 */
+
+/* ISSR(10): Reply IS -> Host */
+/* ISSR(11): Sensor ID for Reply, ISSR12...15 = Parameter 1...4 */
+
+/* ISSR(20): ISP_FRAME_DONE : SENSOR ID */
+/* ISSR(21): ISP_FRAME_DONE : PARAMETER 1 */
+
+/* ISSR(24): SCALERC_FRAME_DONE : SENSOR ID */
+/* ISSR(25): SCALERC_FRAME_DONE : PARAMETER 1 */
+
+/* ISSR(28): 3DNR_FRAME_DONE : SENSOR ID */
+/* ISSR(29): 3DNR_FRAME_DONE : PARAMETER 1 */
+
+/* ISSR(32): SCALERP_FRAME_DONE : SENSOR ID */
+/* ISSR(33): SCALERP_FRAME_DONE : PARAMETER 1 */
+
+/* __n = 0...63 */
+#define MCUCTL_REG_ISSR(__n)		(MCUCTL_BASE + 0x80 + ((__n) * 4))
+
+/* PMU ISP register offsets */
+#define REG_CMU_RESET_ISP_SYS_PWR_REG	0x1174
+#define REG_CMU_SYSCLK_ISP_SYS_PWR_REG	0x13b8
+#define REG_PMU_ISP_ARM_SYS		0x1050
+#define REG_PMU_ISP_ARM_CONFIGURATION	0x2280
+#define REG_PMU_ISP_ARM_STATUS		0x2284
+#define REG_PMU_ISP_ARM_OPTION		0x2288
+
+void fimc_is_fw_clear_irq1(struct fimc_is *is, unsigned int bit);
+void fimc_is_fw_clear_irq2(struct fimc_is *is);
+int fimc_is_hw_get_params(struct fimc_is *is, unsigned int num);
+
+void fimc_is_hw_set_intgr0_gd0(struct fimc_is *is);
+int fimc_is_hw_wait_intsr0_intsd0(struct fimc_is *is);
+int fimc_is_hw_wait_intmsr0_intmsd0(struct fimc_is *is);
+void fimc_is_hw_set_sensor_num(struct fimc_is *is);
+void fimc_is_hw_stream_on(struct fimc_is *is);
+void fimc_is_hw_stream_off(struct fimc_is *is);
+int fimc_is_hw_set_param(struct fimc_is *is);
+int fimc_is_hw_change_mode(struct fimc_is *is);
+
+void fimc_is_hw_close_sensor(struct fimc_is *is, unsigned int index);
+void fimc_is_hw_get_setfile_addr(struct fimc_is *is);
+void fimc_is_hw_load_setfile(struct fimc_is *is);
+void fimc_is_hw_subip_power_off(struct fimc_is *is);
+
+int fimc_is_itf_s_param(struct fimc_is *is, bool update);
+int fimc_is_itf_mode_change(struct fimc_is *is);
+
+#endif /* FIMC_IS_REG_H_ */
diff --git a/drivers/media/platform/exynos4-is/fimc-is-sensor.c b/drivers/media/platform/exynos4-is/fimc-is-sensor.c
new file mode 100644
index 00000000000..6647421e5d3
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-sensor.c
@@ -0,0 +1,305 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Author: 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/delay.h>
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of_gpio.h>
+#include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <media/v4l2-subdev.h>
+
+#include "fimc-is.h"
+#include "fimc-is-sensor.h"
+
+#define DRIVER_NAME "FIMC-IS-SENSOR"
+
+static const char * const sensor_supply_names[] = {
+	"svdda",
+	"svddio",
+};
+
+static const struct v4l2_mbus_framefmt fimc_is_sensor_formats[] = {
+	{
+		.code = V4L2_MBUS_FMT_SGRBG10_1X10,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.field = V4L2_FIELD_NONE,
+	}
+};
+
+static const struct v4l2_mbus_framefmt *find_sensor_format(
+	struct v4l2_mbus_framefmt *mf)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(fimc_is_sensor_formats); i++)
+		if (mf->code == fimc_is_sensor_formats[i].code)
+			return &fimc_is_sensor_formats[i];
+
+	return &fimc_is_sensor_formats[0];
+}
+
+static int fimc_is_sensor_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_fh *fh,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index >= ARRAY_SIZE(fimc_is_sensor_formats))
+		return -EINVAL;
+
+	code->code = fimc_is_sensor_formats[code->index].code;
+	return 0;
+}
+
+static void fimc_is_sensor_try_format(struct fimc_is_sensor *sensor,
+				      struct v4l2_mbus_framefmt *mf)
+{
+	const struct sensor_drv_data *dd = sensor->drvdata;
+	const struct v4l2_mbus_framefmt *fmt;
+
+	fmt = find_sensor_format(mf);
+	mf->code = fmt->code;
+	v4l_bound_align_image(&mf->width, 16 + 8, dd->width, 0,
+			      &mf->height, 12 + 8, dd->height, 0, 0);
+}
+
+static struct v4l2_mbus_framefmt *__fimc_is_sensor_get_format(
+		struct fimc_is_sensor *sensor, struct v4l2_subdev_fh *fh,
+		u32 pad, enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return fh ? v4l2_subdev_get_try_format(fh, pad) : NULL;
+
+	return &sensor->format;
+}
+
+static int fimc_is_sensor_set_fmt(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_fh *fh,
+				  struct v4l2_subdev_format *fmt)
+{
+	struct fimc_is_sensor *sensor = sd_to_fimc_is_sensor(sd);
+	struct v4l2_mbus_framefmt *mf;
+
+	fimc_is_sensor_try_format(sensor, &fmt->format);
+
+	mf = __fimc_is_sensor_get_format(sensor, fh, fmt->pad, fmt->which);
+	if (mf) {
+		mutex_lock(&sensor->lock);
+		if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+			*mf = fmt->format;
+		mutex_unlock(&sensor->lock);
+	}
+	return 0;
+}
+
+static int fimc_is_sensor_get_fmt(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_fh *fh,
+				  struct v4l2_subdev_format *fmt)
+{
+	struct fimc_is_sensor *sensor = sd_to_fimc_is_sensor(sd);
+	struct v4l2_mbus_framefmt *mf;
+
+	mf = __fimc_is_sensor_get_format(sensor, fh, fmt->pad, fmt->which);
+
+	mutex_lock(&sensor->lock);
+	fmt->format = *mf;
+	mutex_unlock(&sensor->lock);
+	return 0;
+}
+
+static struct v4l2_subdev_pad_ops fimc_is_sensor_pad_ops = {
+	.enum_mbus_code	= fimc_is_sensor_enum_mbus_code,
+	.get_fmt	= fimc_is_sensor_get_fmt,
+	.set_fmt	= fimc_is_sensor_set_fmt,
+};
+
+static int fimc_is_sensor_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_mbus_framefmt *format = v4l2_subdev_get_try_format(fh, 0);
+
+	*format		= fimc_is_sensor_formats[0];
+	format->width	= FIMC_IS_SENSOR_DEF_PIX_WIDTH;
+	format->height	= FIMC_IS_SENSOR_DEF_PIX_HEIGHT;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_internal_ops fimc_is_sensor_sd_internal_ops = {
+	.open = fimc_is_sensor_open,
+};
+
+static int fimc_is_sensor_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct fimc_is_sensor *sensor = sd_to_fimc_is_sensor(sd);
+	int gpio = sensor->gpio_reset;
+	int ret;
+
+	if (on) {
+		ret = pm_runtime_get(sensor->dev);
+		if (ret < 0)
+			return ret;
+
+		ret = regulator_bulk_enable(SENSOR_NUM_SUPPLIES,
+					    sensor->supplies);
+		if (ret < 0) {
+			pm_runtime_put(sensor->dev);
+			return ret;
+		}
+		if (gpio_is_valid(gpio)) {
+			gpio_set_value(gpio, 1);
+			usleep_range(600, 800);
+			gpio_set_value(gpio, 0);
+			usleep_range(10000, 11000);
+			gpio_set_value(gpio, 1);
+		}
+
+		/* A delay needed for the sensor initialization. */
+		msleep(20);
+	} else {
+		if (gpio_is_valid(gpio))
+			gpio_set_value(gpio, 0);
+
+		ret = regulator_bulk_disable(SENSOR_NUM_SUPPLIES,
+					     sensor->supplies);
+		if (!ret)
+			pm_runtime_put(sensor->dev);
+	}
+
+	pr_info("%s:%d: on: %d, ret: %d\n", __func__, __LINE__, on, ret);
+
+	return ret;
+}
+
+static struct v4l2_subdev_core_ops fimc_is_sensor_core_ops = {
+	.s_power = fimc_is_sensor_s_power,
+};
+
+static struct v4l2_subdev_ops fimc_is_sensor_subdev_ops = {
+	.core = &fimc_is_sensor_core_ops,
+	.pad = &fimc_is_sensor_pad_ops,
+};
+
+static const struct of_device_id fimc_is_sensor_of_match[];
+
+static int fimc_is_sensor_probe(struct i2c_client *client,
+				const struct i2c_device_id *id)
+{
+	struct device *dev = &client->dev;
+	struct fimc_is_sensor *sensor;
+	const struct of_device_id *of_id;
+	struct v4l2_subdev *sd;
+	int gpio, i, ret;
+
+	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
+	if (!sensor)
+		return -ENOMEM;
+
+	mutex_init(&sensor->lock);
+	sensor->gpio_reset = -EINVAL;
+
+	gpio = of_get_gpio_flags(dev->of_node, 0, NULL);
+	if (gpio_is_valid(gpio)) {
+		ret = devm_gpio_request_one(dev, gpio, GPIOF_OUT_INIT_LOW,
+							DRIVER_NAME);
+		if (ret < 0)
+			return ret;
+	}
+	sensor->gpio_reset = gpio;
+
+	for (i = 0; i < SENSOR_NUM_SUPPLIES; i++)
+		sensor->supplies[i].supply = sensor_supply_names[i];
+
+	ret = devm_regulator_bulk_get(&client->dev, SENSOR_NUM_SUPPLIES,
+				      sensor->supplies);
+	if (ret < 0)
+		return ret;
+
+	of_id = of_match_node(fimc_is_sensor_of_match, dev->of_node);
+	if (!of_id)
+		return -ENODEV;
+
+	sensor->drvdata = of_id->data;
+	sensor->dev = dev;
+
+	sd = &sensor->subdev;
+	v4l2_i2c_subdev_init(sd, client, &fimc_is_sensor_subdev_ops);
+	snprintf(sd->name, sizeof(sd->name), sensor->drvdata->subdev_name);
+	sensor->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	sensor->format.code = fimc_is_sensor_formats[0].code;
+	sensor->format.width = FIMC_IS_SENSOR_DEF_PIX_WIDTH;
+	sensor->format.height = FIMC_IS_SENSOR_DEF_PIX_HEIGHT;
+
+	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_init(&sd->entity, 1, &sensor->pad, 0);
+	if (ret < 0)
+		return ret;
+
+	pm_runtime_no_callbacks(dev);
+	pm_runtime_enable(dev);
+
+	return ret;
+}
+
+static int fimc_is_sensor_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	media_entity_cleanup(&sd->entity);
+	return 0;
+}
+
+static const struct i2c_device_id fimc_is_sensor_ids[] = {
+	{ }
+};
+
+static const struct sensor_drv_data s5k6a3_drvdata = {
+	.id		= FIMC_IS_SENSOR_ID_S5K6A3,
+	.subdev_name	= "S5K6A3",
+	.width		= S5K6A3_SENSOR_WIDTH,
+	.height		= S5K6A3_SENSOR_HEIGHT,
+};
+
+static const struct of_device_id fimc_is_sensor_of_match[] = {
+	{
+		.compatible	= "samsung,s5k6a3",
+		.data		= &s5k6a3_drvdata,
+	},
+	{  }
+};
+
+static struct i2c_driver fimc_is_sensor_driver = {
+	.driver = {
+		.of_match_table	= fimc_is_sensor_of_match,
+		.name		= DRIVER_NAME,
+		.owner		= THIS_MODULE,
+	},
+	.probe		= fimc_is_sensor_probe,
+	.remove		= fimc_is_sensor_remove,
+	.id_table	= fimc_is_sensor_ids,
+};
+
+int fimc_is_register_sensor_driver(void)
+{
+	return i2c_add_driver(&fimc_is_sensor_driver);
+}
+
+void fimc_is_unregister_sensor_driver(void)
+{
+	i2c_del_driver(&fimc_is_sensor_driver);
+}
+
+MODULE_AUTHOR("Sylwester Nawrocki <s.nawrocki@samsung.com>");
+MODULE_DESCRIPTION("Exynos4x12 FIMC-IS image sensor subdev driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/exynos4-is/fimc-is-sensor.h b/drivers/media/platform/exynos4-is/fimc-is-sensor.h
new file mode 100644
index 00000000000..6036d49a6c6
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-sensor.h
@@ -0,0 +1,89 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors:  Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *	     Younghwan Joo <yhwan.joo@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#ifndef FIMC_IS_SENSOR_H_
+#define FIMC_IS_SENSOR_H_
+
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-subdev.h>
+
+#define FIMC_IS_SENSOR_OPEN_TIMEOUT	2000 /* ms */
+
+#define FIMC_IS_SENSOR_DEF_PIX_WIDTH	1296
+#define FIMC_IS_SENSOR_DEF_PIX_HEIGHT	732
+
+#define S5K6A3_SENSOR_WIDTH		1392
+#define S5K6A3_SENSOR_HEIGHT		1392
+
+#define SENSOR_NUM_SUPPLIES		2
+
+enum fimc_is_sensor_id {
+	FIMC_IS_SENSOR_ID_S5K3H2 = 1,
+	FIMC_IS_SENSOR_ID_S5K6A3,
+	FIMC_IS_SENSOR_ID_S5K4E5,
+	FIMC_IS_SENSOR_ID_S5K3H7,
+	FIMC_IS_SENSOR_ID_CUSTOM,
+	FIMC_IS_SENSOR_ID_END
+};
+
+#define IS_SENSOR_CTRL_BUS_I2C0		0
+#define IS_SENSOR_CTRL_BUS_I2C1		1
+
+struct sensor_drv_data {
+	enum fimc_is_sensor_id id;
+	const char * const subdev_name;
+	unsigned int width;
+	unsigned int height;
+};
+
+/**
+ * struct fimc_is_sensor - fimc-is sensor data structure
+ * @dev: pointer to this I2C client device structure
+ * @subdev: the image sensor's v4l2 subdev
+ * @pad: subdev media source pad
+ * @supplies: image sensor's voltage regulator supplies
+ * @gpio_reset: GPIO connected to the sensor's reset pin
+ * @drvdata: a pointer to the sensor's parameters data structure
+ * @i2c_bus: ISP I2C bus index (0...1)
+ * @test_pattern: true to enable video test pattern
+ * @lock: mutex protecting the structure's members below
+ * @format: media bus format at the sensor's source pad
+ */
+struct fimc_is_sensor {
+	struct device *dev;
+	struct v4l2_subdev subdev;
+	struct media_pad pad;
+	struct regulator_bulk_data supplies[SENSOR_NUM_SUPPLIES];
+	int gpio_reset;
+	const struct sensor_drv_data *drvdata;
+	unsigned int i2c_bus;
+	bool test_pattern;
+
+	struct mutex lock;
+	struct v4l2_mbus_framefmt format;
+};
+
+static inline
+struct fimc_is_sensor *sd_to_fimc_is_sensor(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct fimc_is_sensor, subdev);
+}
+
+int fimc_is_register_sensor_driver(void);
+void fimc_is_unregister_sensor_driver(void);
+
+#endif /* FIMC_IS_SENSOR_H_ */
diff --git a/drivers/media/platform/exynos4-is/fimc-is.c b/drivers/media/platform/exynos4-is/fimc-is.c
new file mode 100644
index 00000000000..47c6363d04e
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is.c
@@ -0,0 +1,1007 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *          Younghwan Joo <yhwan.joo@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/device.h>
+#include <linux/debugfs.h>
+#include <linux/delay.h>
+#include <linux/dma-contiguous.h>
+#include <linux/errno.h>
+#include <linux/firmware.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of_i2c.h>
+#include <linux/of_irq.h>
+#include <linux/of_address.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-of.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "media-dev.h"
+#include "fimc-is.h"
+#include "fimc-is-command.h"
+#include "fimc-is-errno.h"
+#include "fimc-is-i2c.h"
+#include "fimc-is-param.h"
+#include "fimc-is-regs.h"
+
+
+static char *fimc_is_clocks[ISS_CLKS_MAX] = {
+	[ISS_CLK_PPMUISPX]		= "ppmuispx",
+	[ISS_CLK_PPMUISPMX]		= "ppmuispmx",
+	[ISS_CLK_LITE0]			= "lite0",
+	[ISS_CLK_LITE1]			= "lite1",
+	[ISS_CLK_MPLL]			= "mpll",
+	[ISS_CLK_SYSREG]		= "sysreg",
+	[ISS_CLK_ISP]			= "isp",
+	[ISS_CLK_DRC]			= "drc",
+	[ISS_CLK_FD]			= "fd",
+	[ISS_CLK_MCUISP]		= "mcuisp",
+	[ISS_CLK_UART]			= "uart",
+	[ISS_CLK_ISP_DIV0]		= "ispdiv0",
+	[ISS_CLK_ISP_DIV1]		= "ispdiv1",
+	[ISS_CLK_MCUISP_DIV0]		= "mcuispdiv0",
+	[ISS_CLK_MCUISP_DIV1]		= "mcuispdiv1",
+	[ISS_CLK_ACLK200]		= "aclk200",
+	[ISS_CLK_ACLK200_DIV]		= "div_aclk200",
+	[ISS_CLK_ACLK400MCUISP]		= "aclk400mcuisp",
+	[ISS_CLK_ACLK400MCUISP_DIV]	= "div_aclk400mcuisp",
+};
+
+static void fimc_is_put_clocks(struct fimc_is *is)
+{
+	int i;
+
+	for (i = 0; i < ISS_CLKS_MAX; i++) {
+		if (IS_ERR(is->clocks[i]))
+			continue;
+		clk_unprepare(is->clocks[i]);
+		clk_put(is->clocks[i]);
+		is->clocks[i] = ERR_PTR(-EINVAL);
+	}
+}
+
+static int fimc_is_get_clocks(struct fimc_is *is)
+{
+	int i, ret;
+
+	for (i = 0; i < ISS_CLKS_MAX; i++)
+		is->clocks[i] = ERR_PTR(-EINVAL);
+
+	for (i = 0; i < ISS_CLKS_MAX; i++) {
+		is->clocks[i] = clk_get(&is->pdev->dev, fimc_is_clocks[i]);
+		if (IS_ERR(is->clocks[i])) {
+			ret = PTR_ERR(is->clocks[i]);
+			goto err;
+		}
+		ret = clk_prepare(is->clocks[i]);
+		if (ret < 0) {
+			clk_put(is->clocks[i]);
+			is->clocks[i] = ERR_PTR(-EINVAL);
+			goto err;
+		}
+	}
+
+	return 0;
+err:
+	fimc_is_put_clocks(is);
+	dev_err(&is->pdev->dev, "failed to get clock: %s\n",
+		fimc_is_clocks[i]);
+	return -ENXIO;
+}
+
+static int fimc_is_setup_clocks(struct fimc_is *is)
+{
+	int ret;
+
+	ret = clk_set_parent(is->clocks[ISS_CLK_ACLK200],
+					is->clocks[ISS_CLK_ACLK200_DIV]);
+	if (ret < 0)
+		return ret;
+
+	ret = clk_set_parent(is->clocks[ISS_CLK_ACLK400MCUISP],
+					is->clocks[ISS_CLK_ACLK400MCUISP_DIV]);
+	if (ret < 0)
+		return ret;
+
+	ret = clk_set_rate(is->clocks[ISS_CLK_ISP_DIV0], ACLK_AXI_FREQUENCY);
+	if (ret < 0)
+		return ret;
+
+	ret = clk_set_rate(is->clocks[ISS_CLK_ISP_DIV1], ACLK_AXI_FREQUENCY);
+	if (ret < 0)
+		return ret;
+
+	ret = clk_set_rate(is->clocks[ISS_CLK_MCUISP_DIV0],
+					ATCLK_MCUISP_FREQUENCY);
+	if (ret < 0)
+		return ret;
+
+	return clk_set_rate(is->clocks[ISS_CLK_MCUISP_DIV1],
+					ATCLK_MCUISP_FREQUENCY);
+}
+
+int fimc_is_enable_clocks(struct fimc_is *is)
+{
+	int i, ret;
+
+	for (i = 0; i < ISS_GATE_CLKS_MAX; i++) {
+		if (IS_ERR(is->clocks[i]))
+			continue;
+		ret = clk_enable(is->clocks[i]);
+		if (ret < 0) {
+			dev_err(&is->pdev->dev, "clock %s enable failed\n",
+				fimc_is_clocks[i]);
+			for (--i; i >= 0; i--)
+				clk_disable(is->clocks[i]);
+			return ret;
+		}
+		pr_debug("enabled clock: %s\n", fimc_is_clocks[i]);
+	}
+	return 0;
+}
+
+void fimc_is_disable_clocks(struct fimc_is *is)
+{
+	int i;
+
+	for (i = 0; i < ISS_GATE_CLKS_MAX; i++) {
+		if (!IS_ERR(is->clocks[i])) {
+			clk_disable(is->clocks[i]);
+			pr_debug("disabled clock: %s\n", fimc_is_clocks[i]);
+		}
+	}
+}
+
+static int fimc_is_parse_sensor_config(struct fimc_is_sensor *sensor,
+				       struct device_node *np)
+{
+	u32 tmp = 0;
+	int ret;
+
+	np = v4l2_of_get_next_endpoint(np, NULL);
+	if (!np)
+		return -ENXIO;
+	np = v4l2_of_get_remote_port(np);
+	if (!np)
+		return -ENXIO;
+
+	/* Use MIPI-CSIS channel id to determine the ISP I2C bus index. */
+	ret = of_property_read_u32(np, "reg", &tmp);
+	sensor->i2c_bus = tmp - FIMC_INPUT_MIPI_CSI2_0;
+
+	return ret;
+}
+
+static int fimc_is_register_subdevs(struct fimc_is *is)
+{
+	struct device_node *adapter, *child;
+	int ret;
+
+	ret = fimc_isp_subdev_create(&is->isp);
+	if (ret < 0)
+		return ret;
+
+	for_each_compatible_node(adapter, NULL, FIMC_IS_I2C_COMPATIBLE) {
+		if (!of_find_device_by_node(adapter)) {
+			of_node_put(adapter);
+			return -EPROBE_DEFER;
+		}
+
+		for_each_available_child_of_node(adapter, child) {
+			struct i2c_client *client;
+			struct v4l2_subdev *sd;
+
+			client = of_find_i2c_device_by_node(child);
+			if (!client)
+				goto e_retry;
+
+			sd = i2c_get_clientdata(client);
+			if (!sd)
+				goto e_retry;
+
+			/* FIXME: Add support for multiple sensors. */
+			if (WARN_ON(is->sensor))
+				continue;
+
+			is->sensor = sd_to_fimc_is_sensor(sd);
+
+			if (fimc_is_parse_sensor_config(is->sensor, child)) {
+				dev_warn(&is->pdev->dev, "DT parse error: %s\n",
+							 child->full_name);
+			}
+			pr_debug("%s(): registered subdev: %p\n",
+				 __func__, sd->name);
+		}
+	}
+	return 0;
+
+e_retry:
+	of_node_put(child);
+	return -EPROBE_DEFER;
+}
+
+static int fimc_is_unregister_subdevs(struct fimc_is *is)
+{
+	fimc_isp_subdev_destroy(&is->isp);
+	is->sensor = NULL;
+	return 0;
+}
+
+static int fimc_is_load_setfile(struct fimc_is *is, char *file_name)
+{
+	const struct firmware *fw;
+	void *buf;
+	int ret;
+
+	ret = request_firmware(&fw, file_name, &is->pdev->dev);
+	if (ret < 0) {
+		dev_err(&is->pdev->dev, "firmware request failed (%d)\n", ret);
+		return ret;
+	}
+	buf = is->memory.vaddr + is->setfile.base;
+	memcpy(buf, fw->data, fw->size);
+	fimc_is_mem_barrier();
+	is->setfile.size = fw->size;
+
+	pr_debug("mem vaddr: %p, setfile buf: %p\n", is->memory.vaddr, buf);
+
+	memcpy(is->fw.setfile_info,
+		fw->data + fw->size - FIMC_IS_SETFILE_INFO_LEN,
+		FIMC_IS_SETFILE_INFO_LEN - 1);
+
+	is->fw.setfile_info[FIMC_IS_SETFILE_INFO_LEN - 1] = '\0';
+	is->setfile.state = 1;
+
+	pr_debug("FIMC-IS setfile loaded: base: %#x, size: %zu B\n",
+		 is->setfile.base, fw->size);
+
+	release_firmware(fw);
+	return ret;
+}
+
+int fimc_is_cpu_set_power(struct fimc_is *is, int on)
+{
+	unsigned int timeout = FIMC_IS_POWER_ON_TIMEOUT;
+
+	if (on) {
+		/* Disable watchdog */
+		mcuctl_write(0, is, REG_WDT_ISP);
+
+		/* Cortex-A5 start address setting */
+		mcuctl_write(is->memory.paddr, is, MCUCTL_REG_BBOAR);
+
+		/* Enable and start Cortex-A5 */
+		pmuisp_write(0x18000, is, REG_PMU_ISP_ARM_OPTION);
+		pmuisp_write(0x1, is, REG_PMU_ISP_ARM_CONFIGURATION);
+	} else {
+		/* A5 power off */
+		pmuisp_write(0x10000, is, REG_PMU_ISP_ARM_OPTION);
+		pmuisp_write(0x0, is, REG_PMU_ISP_ARM_CONFIGURATION);
+
+		while (pmuisp_read(is, REG_PMU_ISP_ARM_STATUS) & 1) {
+			if (timeout == 0)
+				return -ETIME;
+			timeout--;
+			udelay(1);
+		}
+	}
+
+	return 0;
+}
+
+/* Wait until @bit of @is->state is set to @state in the interrupt handler. */
+int fimc_is_wait_event(struct fimc_is *is, unsigned long bit,
+		       unsigned int state, unsigned int timeout)
+{
+
+	int ret = wait_event_timeout(is->irq_queue,
+				     !state ^ test_bit(bit, &is->state),
+				     timeout);
+	if (ret == 0) {
+		dev_WARN(&is->pdev->dev, "%s() timed out\n", __func__);
+		return -ETIME;
+	}
+	return 0;
+}
+
+int fimc_is_start_firmware(struct fimc_is *is)
+{
+	struct device *dev = &is->pdev->dev;
+	int ret;
+
+	memcpy(is->memory.vaddr, is->fw.f_w->data, is->fw.f_w->size);
+	wmb();
+
+	ret = fimc_is_cpu_set_power(is, 1);
+	if (ret < 0)
+		return ret;
+
+	ret = fimc_is_wait_event(is, IS_ST_A5_PWR_ON, 1,
+				 msecs_to_jiffies(FIMC_IS_FW_LOAD_TIMEOUT));
+	if (ret < 0)
+		dev_err(dev, "FIMC-IS CPU power on failed\n");
+
+	return ret;
+}
+
+/* Allocate working memory for the FIMC-IS CPU. */
+static int fimc_is_alloc_cpu_memory(struct fimc_is *is)
+{
+	struct device *dev = &is->pdev->dev;
+
+	is->memory.vaddr = dma_alloc_coherent(dev, FIMC_IS_CPU_MEM_SIZE,
+					      &is->memory.paddr, GFP_KERNEL);
+	if (is->memory.vaddr == NULL)
+		return -ENOMEM;
+
+	is->memory.size = FIMC_IS_CPU_MEM_SIZE;
+	memset(is->memory.vaddr, 0, is->memory.size);
+
+	dev_info(dev, "FIMC-IS CPU memory base: %#x\n", (u32)is->memory.paddr);
+
+	if (((u32)is->memory.paddr) & FIMC_IS_FW_ADDR_MASK) {
+		dev_err(dev, "invalid firmware memory alignment: %#x\n",
+			(u32)is->memory.paddr);
+		dma_free_coherent(dev, is->memory.size, is->memory.vaddr,
+				  is->memory.paddr);
+		return -EIO;
+	}
+
+	is->is_p_region = (struct is_region *)(is->memory.vaddr +
+				FIMC_IS_CPU_MEM_SIZE - FIMC_IS_REGION_SIZE);
+
+	is->is_dma_p_region = is->memory.paddr +
+				FIMC_IS_CPU_MEM_SIZE - FIMC_IS_REGION_SIZE;
+
+	is->is_shared_region = (struct is_share_region *)(is->memory.vaddr +
+				FIMC_IS_SHARED_REGION_OFFSET);
+	return 0;
+}
+
+static void fimc_is_free_cpu_memory(struct fimc_is *is)
+{
+	struct device *dev = &is->pdev->dev;
+
+	dma_free_coherent(dev, is->memory.size, is->memory.vaddr,
+			  is->memory.paddr);
+}
+
+static void fimc_is_load_firmware(const struct firmware *fw, void *context)
+{
+	struct fimc_is *is = context;
+	struct device *dev = &is->pdev->dev;
+	void *buf;
+	int ret;
+
+	if (fw == NULL) {
+		dev_err(dev, "firmware request failed\n");
+		return;
+	}
+	mutex_lock(&is->lock);
+
+	if (fw->size < FIMC_IS_FW_SIZE_MIN || fw->size > FIMC_IS_FW_SIZE_MAX) {
+		dev_err(dev, "wrong firmware size: %d\n", fw->size);
+		goto done;
+	}
+
+	is->fw.size = fw->size;
+
+	ret = fimc_is_alloc_cpu_memory(is);
+	if (ret < 0) {
+		dev_err(dev, "failed to allocate FIMC-IS CPU memory\n");
+		goto done;
+	}
+
+	memcpy(is->memory.vaddr, fw->data, fw->size);
+	wmb();
+
+	/* Read firmware description. */
+	buf = (void *)(is->memory.vaddr + fw->size - FIMC_IS_FW_DESC_LEN);
+	memcpy(&is->fw.info, buf, FIMC_IS_FW_INFO_LEN);
+	is->fw.info[FIMC_IS_FW_INFO_LEN] = 0;
+
+	buf = (void *)(is->memory.vaddr + fw->size - FIMC_IS_FW_VER_LEN);
+	memcpy(&is->fw.version, buf, FIMC_IS_FW_VER_LEN);
+	is->fw.version[FIMC_IS_FW_VER_LEN - 1] = 0;
+
+	is->fw.state = 1;
+
+	dev_info(dev, "loaded firmware: %s, rev. %s\n",
+		 is->fw.info, is->fw.version);
+	dev_dbg(dev, "FW size: %d, paddr: %#x\n", fw->size, is->memory.paddr);
+
+	is->is_shared_region->chip_id = 0xe4412;
+	is->is_shared_region->chip_rev_no = 1;
+
+	fimc_is_mem_barrier();
+
+	/*
+	 * FIXME: The firmware is not being released for now, as it is
+	 * needed around for copying to the IS working memory every
+	 * time before the Cortex-A5 is restarted.
+	 */
+	if (is->fw.f_w)
+		release_firmware(is->fw.f_w);
+	is->fw.f_w = fw;
+done:
+	mutex_unlock(&is->lock);
+}
+
+static int fimc_is_request_firmware(struct fimc_is *is, const char *fw_name)
+{
+	return request_firmware_nowait(THIS_MODULE,
+				FW_ACTION_HOTPLUG, fw_name, &is->pdev->dev,
+				GFP_KERNEL, is, fimc_is_load_firmware);
+}
+
+/* General IS interrupt handler */
+static void fimc_is_general_irq_handler(struct fimc_is *is)
+{
+	is->i2h_cmd.cmd = mcuctl_read(is, MCUCTL_REG_ISSR(10));
+
+	switch (is->i2h_cmd.cmd) {
+	case IHC_GET_SENSOR_NUM:
+		fimc_is_hw_get_params(is, 1);
+		fimc_is_hw_wait_intmsr0_intmsd0(is);
+		fimc_is_hw_set_sensor_num(is);
+		pr_debug("ISP FW version: %#x\n", is->i2h_cmd.args[0]);
+		break;
+	case IHC_SET_FACE_MARK:
+	case IHC_FRAME_DONE:
+		fimc_is_hw_get_params(is, 2);
+		break;
+	case IHC_SET_SHOT_MARK:
+	case IHC_AA_DONE:
+	case IH_REPLY_DONE:
+		fimc_is_hw_get_params(is, 3);
+		break;
+	case IH_REPLY_NOT_DONE:
+		fimc_is_hw_get_params(is, 4);
+		break;
+	case IHC_NOT_READY:
+		break;
+	default:
+		pr_info("unknown command: %#x\n", is->i2h_cmd.cmd);
+	}
+
+	fimc_is_fw_clear_irq1(is, FIMC_IS_INT_GENERAL);
+
+	switch (is->i2h_cmd.cmd) {
+	case IHC_GET_SENSOR_NUM:
+		fimc_is_hw_set_intgr0_gd0(is);
+		set_bit(IS_ST_A5_PWR_ON, &is->state);
+		break;
+
+	case IHC_SET_SHOT_MARK:
+		break;
+
+	case IHC_SET_FACE_MARK:
+		is->fd_header.count = is->i2h_cmd.args[0];
+		is->fd_header.index = is->i2h_cmd.args[1];
+		is->fd_header.offset = 0;
+		break;
+
+	case IHC_FRAME_DONE:
+		break;
+
+	case IHC_AA_DONE:
+		pr_debug("AA_DONE - %d, %d, %d\n", is->i2h_cmd.args[0],
+			 is->i2h_cmd.args[1], is->i2h_cmd.args[2]);
+		break;
+
+	case IH_REPLY_DONE:
+		pr_debug("ISR_DONE: args[0]: %#x\n", is->i2h_cmd.args[0]);
+
+		switch (is->i2h_cmd.args[0]) {
+		case HIC_PREVIEW_STILL...HIC_CAPTURE_VIDEO:
+			/* Get CAC margin */
+			set_bit(IS_ST_CHANGE_MODE, &is->state);
+			is->isp.cac_margin_x = is->i2h_cmd.args[1];
+			is->isp.cac_margin_y = is->i2h_cmd.args[2];
+			pr_debug("CAC margin (x,y): (%d,%d)\n",
+				 is->isp.cac_margin_x, is->isp.cac_margin_y);
+			break;
+
+		case HIC_STREAM_ON:
+			clear_bit(IS_ST_STREAM_OFF, &is->state);
+			set_bit(IS_ST_STREAM_ON, &is->state);
+			break;
+
+		case HIC_STREAM_OFF:
+			clear_bit(IS_ST_STREAM_ON, &is->state);
+			set_bit(IS_ST_STREAM_OFF, &is->state);
+			break;
+
+		case HIC_SET_PARAMETER:
+			is->config[is->config_index].p_region_index1 = 0;
+			is->config[is->config_index].p_region_index2 = 0;
+			set_bit(IS_ST_BLOCK_CMD_CLEARED, &is->state);
+			pr_debug("HIC_SET_PARAMETER\n");
+			break;
+
+		case HIC_GET_PARAMETER:
+			break;
+
+		case HIC_SET_TUNE:
+			break;
+
+		case HIC_GET_STATUS:
+			break;
+
+		case HIC_OPEN_SENSOR:
+			set_bit(IS_ST_OPEN_SENSOR, &is->state);
+			pr_debug("data lanes: %d, settle line: %d\n",
+				 is->i2h_cmd.args[2], is->i2h_cmd.args[1]);
+			break;
+
+		case HIC_CLOSE_SENSOR:
+			clear_bit(IS_ST_OPEN_SENSOR, &is->state);
+			is->sensor_index = 0;
+			break;
+
+		case HIC_MSG_TEST:
+			pr_debug("config MSG level completed\n");
+			break;
+
+		case HIC_POWER_DOWN:
+			clear_bit(IS_ST_PWR_SUBIP_ON, &is->state);
+			break;
+
+		case HIC_GET_SET_FILE_ADDR:
+			is->setfile.base = is->i2h_cmd.args[1];
+			set_bit(IS_ST_SETFILE_LOADED, &is->state);
+			break;
+
+		case HIC_LOAD_SET_FILE:
+			set_bit(IS_ST_SETFILE_LOADED, &is->state);
+			break;
+		}
+		break;
+
+	case IH_REPLY_NOT_DONE:
+		pr_err("ISR_NDONE: %d: %#x, %s\n", is->i2h_cmd.args[0],
+		       is->i2h_cmd.args[1],
+		       fimc_is_strerr(is->i2h_cmd.args[1]));
+
+		if (is->i2h_cmd.args[1] & IS_ERROR_TIME_OUT_FLAG)
+			pr_err("IS_ERROR_TIME_OUT\n");
+
+		switch (is->i2h_cmd.args[1]) {
+		case IS_ERROR_SET_PARAMETER:
+			fimc_is_mem_barrier();
+		}
+
+		switch (is->i2h_cmd.args[0]) {
+		case HIC_SET_PARAMETER:
+			is->config[is->config_index].p_region_index1 = 0;
+			is->config[is->config_index].p_region_index2 = 0;
+			set_bit(IS_ST_BLOCK_CMD_CLEARED, &is->state);
+			break;
+		}
+		break;
+
+	case IHC_NOT_READY:
+		pr_err("IS control sequence error: Not Ready\n");
+		break;
+	}
+
+	wake_up(&is->irq_queue);
+}
+
+static irqreturn_t fimc_is_irq_handler(int irq, void *priv)
+{
+	struct fimc_is *is = priv;
+	unsigned long flags;
+	u32 status;
+
+	spin_lock_irqsave(&is->slock, flags);
+	status = mcuctl_read(is, MCUCTL_REG_INTSR1);
+
+	if (status & (1UL << FIMC_IS_INT_GENERAL))
+		fimc_is_general_irq_handler(is);
+
+	if (status & (1UL << FIMC_IS_INT_FRAME_DONE_ISP))
+		fimc_isp_irq_handler(is);
+
+	spin_unlock_irqrestore(&is->slock, flags);
+	return IRQ_HANDLED;
+}
+
+static int fimc_is_hw_open_sensor(struct fimc_is *is,
+				  struct fimc_is_sensor *sensor)
+{
+	struct sensor_open_extended *soe = (void *)&is->is_p_region->shared;
+
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+
+	soe->self_calibration_mode = 1;
+	soe->actuator_type = 0;
+	soe->mipi_lane_num = 0;
+	soe->mclk = 0;
+	soe->mipi_speed	= 0;
+	soe->fast_open_sensor = 0;
+	soe->i2c_sclk = 88000000;
+
+	fimc_is_mem_barrier();
+
+	mcuctl_write(HIC_OPEN_SENSOR, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	mcuctl_write(sensor->drvdata->id, is, MCUCTL_REG_ISSR(2));
+	mcuctl_write(sensor->i2c_bus, is, MCUCTL_REG_ISSR(3));
+	mcuctl_write(is->is_dma_p_region, is, MCUCTL_REG_ISSR(4));
+
+	fimc_is_hw_set_intgr0_gd0(is);
+
+	return fimc_is_wait_event(is, IS_ST_OPEN_SENSOR, 1,
+				  FIMC_IS_SENSOR_OPEN_TIMEOUT);
+}
+
+
+int fimc_is_hw_initialize(struct fimc_is *is)
+{
+	const int config_ids[] = {
+		IS_SC_PREVIEW_STILL, IS_SC_PREVIEW_VIDEO,
+		IS_SC_CAPTURE_STILL, IS_SC_CAPTURE_VIDEO
+	};
+	struct device *dev = &is->pdev->dev;
+	u32 prev_id;
+	int i, ret;
+
+	/* Sensor initialization. */
+	ret = fimc_is_hw_open_sensor(is, is->sensor);
+	if (ret < 0)
+		return ret;
+
+	/* Get the setfile address. */
+	fimc_is_hw_get_setfile_addr(is);
+
+	ret = fimc_is_wait_event(is, IS_ST_SETFILE_LOADED, 1,
+				 FIMC_IS_CONFIG_TIMEOUT);
+	if (ret < 0) {
+		dev_err(dev, "get setfile address timed out\n");
+		return ret;
+	}
+	pr_debug("setfile.base: %#x\n", is->setfile.base);
+
+	/* Load the setfile. */
+	fimc_is_load_setfile(is, FIMC_IS_SETFILE_6A3);
+	clear_bit(IS_ST_SETFILE_LOADED, &is->state);
+	fimc_is_hw_load_setfile(is);
+	ret = fimc_is_wait_event(is, IS_ST_SETFILE_LOADED, 1,
+				 FIMC_IS_CONFIG_TIMEOUT);
+	if (ret < 0) {
+		dev_err(dev, "loading setfile timed out\n");
+		return ret;
+	}
+
+	pr_debug("setfile: base: %#x, size: %d\n",
+		 is->setfile.base, is->setfile.size);
+	pr_info("FIMC-IS Setfile info: %s\n", is->fw.setfile_info);
+
+	/* Check magic number. */
+	if (is->is_p_region->shared[MAX_SHARED_COUNT - 1] !=
+	    FIMC_IS_MAGIC_NUMBER) {
+		dev_err(dev, "magic number error!\n");
+		return -EIO;
+	}
+
+	pr_debug("shared region: %#x, parameter region: %#x\n",
+		 is->memory.paddr + FIMC_IS_SHARED_REGION_OFFSET,
+		 is->is_dma_p_region);
+
+	is->setfile.sub_index = 0;
+
+	/* Stream off. */
+	fimc_is_hw_stream_off(is);
+	ret = fimc_is_wait_event(is, IS_ST_STREAM_OFF, 1,
+				 FIMC_IS_CONFIG_TIMEOUT);
+	if (ret < 0) {
+		dev_err(dev, "stream off timeout\n");
+		return ret;
+	}
+
+	/* Preserve previous mode. */
+	prev_id = is->config_index;
+
+	/* Set initial parameter values. */
+	for (i = 0; i < ARRAY_SIZE(config_ids); i++) {
+		is->config_index = config_ids[i];
+		fimc_is_set_initial_params(is);
+		ret = fimc_is_itf_s_param(is, true);
+		if (ret < 0) {
+			is->config_index = prev_id;
+			return ret;
+		}
+	}
+	is->config_index = prev_id;
+
+	set_bit(IS_ST_INIT_DONE, &is->state);
+	dev_info(dev, "initialization sequence completed (%d)\n",
+						is->config_index);
+	return 0;
+}
+
+static int fimc_is_log_show(struct seq_file *s, void *data)
+{
+	struct fimc_is *is = s->private;
+	const u8 *buf = is->memory.vaddr + FIMC_IS_DEBUG_REGION_OFFSET;
+
+	if (is->memory.vaddr == NULL) {
+		dev_err(&is->pdev->dev, "firmware memory is not initialized\n");
+		return -EIO;
+	}
+
+	seq_printf(s, "%s\n", buf);
+	return 0;
+}
+
+static int fimc_is_debugfs_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, fimc_is_log_show, inode->i_private);
+}
+
+static const struct file_operations fimc_is_debugfs_fops = {
+	.open		= fimc_is_debugfs_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static void fimc_is_debugfs_remove(struct fimc_is *is)
+{
+	debugfs_remove_recursive(is->debugfs_entry);
+	is->debugfs_entry = NULL;
+}
+
+static int fimc_is_debugfs_create(struct fimc_is *is)
+{
+	struct dentry *dentry;
+
+	is->debugfs_entry = debugfs_create_dir("fimc_is", NULL);
+
+	dentry = debugfs_create_file("fw_log", S_IRUGO, is->debugfs_entry,
+				     is, &fimc_is_debugfs_fops);
+	if (!dentry)
+		fimc_is_debugfs_remove(is);
+
+	return is->debugfs_entry == NULL ? -EIO : 0;
+}
+
+static int fimc_is_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct fimc_is *is;
+	struct resource res;
+	struct device_node *node;
+	int ret;
+
+	is = devm_kzalloc(&pdev->dev, sizeof(*is), GFP_KERNEL);
+	if (!is)
+		return -ENOMEM;
+
+	is->pdev = pdev;
+	is->isp.pdev = pdev;
+
+	init_waitqueue_head(&is->irq_queue);
+	spin_lock_init(&is->slock);
+	mutex_init(&is->lock);
+
+	ret = of_address_to_resource(dev->of_node, 0, &res);
+	if (ret < 0)
+		return ret;
+
+	is->regs = devm_ioremap_resource(dev, &res);
+	if (IS_ERR(is->regs))
+		return PTR_ERR(is->regs);
+
+	node = of_get_child_by_name(dev->of_node, "pmu");
+	if (!node)
+		return -ENODEV;
+
+	is->pmu_regs = of_iomap(node, 0);
+	if (!is->pmu_regs)
+		return -ENOMEM;
+
+	is->irq = irq_of_parse_and_map(dev->of_node, 0);
+	if (is->irq < 0) {
+		dev_err(dev, "no irq found\n");
+		return is->irq;
+	}
+
+	ret = fimc_is_get_clocks(is);
+	if (ret < 0)
+		return ret;
+
+	platform_set_drvdata(pdev, is);
+
+	ret = request_irq(is->irq, fimc_is_irq_handler, 0, dev_name(dev), is);
+	if (ret < 0) {
+		dev_err(dev, "irq request failed\n");
+		goto err_clk;
+	}
+	pm_runtime_enable(dev);
+	/*
+	 * Enable only the ISP power domain, keep FIMC-IS clocks off until
+	 * the whole clock tree is configured. The ISP power domain needs
+	 * be active in order to acces any CMU_ISP clock registers.
+	 */
+	ret = pm_runtime_get_sync(dev);
+	if (ret < 0)
+		goto err_irq;
+
+	ret = fimc_is_setup_clocks(is);
+	pm_runtime_put_sync(dev);
+
+	if (ret < 0)
+		goto err_irq;
+
+	is->clk_init = true;
+
+	is->alloc_ctx = vb2_dma_contig_init_ctx(dev);
+	if (IS_ERR(is->alloc_ctx)) {
+		ret = PTR_ERR(is->alloc_ctx);
+		goto err_irq;
+	}
+	/*
+	 * Register FIMC-IS V4L2 subdevs to this driver. The video nodes
+	 * will be created within the subdev's registered() callback.
+	 */
+	ret = fimc_is_register_subdevs(is);
+	if (ret < 0)
+		goto err_vb;
+
+	ret = fimc_is_debugfs_create(is);
+	if (ret < 0)
+		goto err_sd;
+
+	ret = fimc_is_request_firmware(is, FIMC_IS_FW_FILENAME);
+	if (ret < 0)
+		goto err_dfs;
+
+	dev_dbg(dev, "FIMC-IS registered successfully\n");
+	return 0;
+
+err_dfs:
+	fimc_is_debugfs_remove(is);
+err_vb:
+	vb2_dma_contig_cleanup_ctx(is->alloc_ctx);
+err_sd:
+	fimc_is_unregister_subdevs(is);
+err_irq:
+	free_irq(is->irq, is);
+err_clk:
+	fimc_is_put_clocks(is);
+	return ret;
+}
+
+static int fimc_is_runtime_resume(struct device *dev)
+{
+	struct fimc_is *is = dev_get_drvdata(dev);
+
+	if (!is->clk_init)
+		return 0;
+
+	return fimc_is_enable_clocks(is);
+}
+
+static int fimc_is_runtime_suspend(struct device *dev)
+{
+	struct fimc_is *is = dev_get_drvdata(dev);
+
+	if (is->clk_init)
+		fimc_is_disable_clocks(is);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int fimc_is_resume(struct device *dev)
+{
+	/* TODO: */
+	return 0;
+}
+
+static int fimc_is_suspend(struct device *dev)
+{
+	struct fimc_is *is = dev_get_drvdata(dev);
+
+	/* TODO: */
+	if (test_bit(IS_ST_A5_PWR_ON, &is->state))
+		return -EBUSY;
+
+	return 0;
+}
+#endif /* CONFIG_PM_SLEEP */
+
+static int fimc_is_remove(struct platform_device *pdev)
+{
+	struct fimc_is *is = platform_get_drvdata(pdev);
+
+	pm_runtime_disable(&pdev->dev);
+	pm_runtime_set_suspended(&pdev->dev);
+	free_irq(is->irq, is);
+	fimc_is_unregister_subdevs(is);
+	vb2_dma_contig_cleanup_ctx(is->alloc_ctx);
+	fimc_is_put_clocks(is);
+	fimc_is_debugfs_remove(is);
+	release_firmware(is->fw.f_w);
+	fimc_is_free_cpu_memory(is);
+
+	return 0;
+}
+
+static const struct of_device_id fimc_is_of_match[] = {
+	{ .compatible = "samsung,exynos4212-fimc-is" },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, fimc_is_of_match);
+
+static const struct dev_pm_ops fimc_is_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(fimc_is_suspend, fimc_is_resume)
+	SET_RUNTIME_PM_OPS(fimc_is_runtime_suspend, fimc_is_runtime_resume,
+			   NULL)
+};
+
+static struct platform_driver fimc_is_driver = {
+	.probe		= fimc_is_probe,
+	.remove		= fimc_is_remove,
+	.driver = {
+		.of_match_table	= fimc_is_of_match,
+		.name		= FIMC_IS_DRV_NAME,
+		.owner		= THIS_MODULE,
+		.pm		= &fimc_is_pm_ops,
+	}
+};
+
+static int fimc_is_module_init(void)
+{
+	int ret;
+
+	ret = fimc_is_register_sensor_driver();
+	if (ret < 0)
+		return ret;
+
+	ret = fimc_is_register_i2c_driver();
+	if (ret < 0)
+		goto err_sens;
+
+	ret = platform_driver_register(&fimc_is_driver);
+	if (!ret)
+		return ret;
+
+	fimc_is_unregister_i2c_driver();
+err_sens:
+	fimc_is_unregister_sensor_driver();
+	return ret;
+}
+
+static void fimc_is_module_exit(void)
+{
+	fimc_is_unregister_sensor_driver();
+	fimc_is_unregister_i2c_driver();
+	platform_driver_unregister(&fimc_is_driver);
+}
+
+module_init(fimc_is_module_init);
+module_exit(fimc_is_module_exit);
+
+MODULE_ALIAS("platform:" FIMC_IS_DRV_NAME);
+MODULE_AUTHOR("Younghwan Joo <yhwan.joo@samsung.com>");
+MODULE_AUTHOR("Sylwester Nawrocki <s.nawrocki@samsung.com>");
diff --git a/drivers/media/platform/exynos4-is/fimc-is.h b/drivers/media/platform/exynos4-is/fimc-is.h
new file mode 100644
index 00000000000..f5275a5b015
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is.h
@@ -0,0 +1,345 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Younghwan Joo <yhwan.joo@samsung.com>
+ *          Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#ifndef FIMC_IS_H_
+#define FIMC_IS_H_
+
+#include <asm/barrier.h>
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/platform_device.h>
+#include <linux/sizes.h>
+#include <linux/spinlock.h>
+#include <linux/types.h>
+#include <media/videobuf2-core.h>
+#include <media/v4l2-ctrls.h>
+
+#include "fimc-isp.h"
+#include "fimc-is-command.h"
+#include "fimc-is-sensor.h"
+#include "fimc-is-param.h"
+#include "fimc-is-regs.h"
+
+#define FIMC_IS_DRV_NAME		"exynos4-fimc-is"
+
+#define FIMC_IS_FW_FILENAME		"fimc_is_fw.bin"
+#define FIMC_IS_SETFILE_6A3		"setfile.bin"
+
+#define FIMC_IS_FW_LOAD_TIMEOUT		1000 /* ms */
+#define FIMC_IS_POWER_ON_TIMEOUT	1000 /* us */
+
+#define FIMC_IS_SENSOR_NUM		2
+
+/* Memory definitions */
+#define FIMC_IS_CPU_MEM_SIZE		(0xa00000)
+#define FIMC_IS_CPU_BASE_MASK		((1 << 26) - 1)
+#define FIMC_IS_REGION_SIZE		0x5000
+
+#define FIMC_IS_DEBUG_REGION_OFFSET	0x0084b000
+#define FIMC_IS_SHARED_REGION_OFFSET	0x008c0000
+#define FIMC_IS_FW_INFO_LEN		31
+#define FIMC_IS_FW_VER_LEN		7
+#define FIMC_IS_FW_DESC_LEN		(FIMC_IS_FW_INFO_LEN + \
+					 FIMC_IS_FW_VER_LEN)
+#define FIMC_IS_SETFILE_INFO_LEN	39
+
+#define FIMC_IS_EXTRA_MEM_SIZE		(FIMC_IS_EXTRA_FW_SIZE + \
+					 FIMC_IS_EXTRA_SETFILE_SIZE + 0x1000)
+#define FIMC_IS_EXTRA_FW_SIZE		0x180000
+#define FIMC_IS_EXTRA_SETFILE_SIZE	0x4b000
+
+/* TODO: revisit */
+#define FIMC_IS_FW_ADDR_MASK		((1 << 26) - 1)
+#define FIMC_IS_FW_SIZE_MAX		(SZ_4M)
+#define FIMC_IS_FW_SIZE_MIN		(SZ_32K)
+
+#define ATCLK_MCUISP_FREQUENCY		100000000UL
+#define ACLK_AXI_FREQUENCY		100000000UL
+
+enum {
+	ISS_CLK_PPMUISPX,
+	ISS_CLK_PPMUISPMX,
+	ISS_CLK_LITE0,
+	ISS_CLK_LITE1,
+	ISS_CLK_MPLL,
+	ISS_CLK_SYSREG,
+	ISS_CLK_ISP,
+	ISS_CLK_DRC,
+	ISS_CLK_FD,
+	ISS_CLK_MCUISP,
+	ISS_CLK_UART,
+	ISS_GATE_CLKS_MAX,
+	ISS_CLK_ISP_DIV0 = ISS_GATE_CLKS_MAX,
+	ISS_CLK_ISP_DIV1,
+	ISS_CLK_MCUISP_DIV0,
+	ISS_CLK_MCUISP_DIV1,
+	ISS_CLK_ACLK200,
+	ISS_CLK_ACLK200_DIV,
+	ISS_CLK_ACLK400MCUISP,
+	ISS_CLK_ACLK400MCUISP_DIV,
+	ISS_CLKS_MAX
+};
+
+/* The driver's internal state flags */
+enum {
+	IS_ST_IDLE,
+	IS_ST_PWR_ON,
+	IS_ST_A5_PWR_ON,
+	IS_ST_FW_LOADED,
+	IS_ST_OPEN_SENSOR,
+	IS_ST_SETFILE_LOADED,
+	IS_ST_INIT_DONE,
+	IS_ST_STREAM_ON,
+	IS_ST_STREAM_OFF,
+	IS_ST_CHANGE_MODE,
+	IS_ST_BLOCK_CMD_CLEARED,
+	IS_ST_SET_ZOOM,
+	IS_ST_PWR_SUBIP_ON,
+	IS_ST_END,
+};
+
+enum af_state {
+	FIMC_IS_AF_IDLE		= 0,
+	FIMC_IS_AF_SETCONFIG	= 1,
+	FIMC_IS_AF_RUNNING	= 2,
+	FIMC_IS_AF_LOCK		= 3,
+	FIMC_IS_AF_ABORT	= 4,
+	FIMC_IS_AF_FAILED	= 5,
+};
+
+enum af_lock_state {
+	FIMC_IS_AF_UNLOCKED	= 0,
+	FIMC_IS_AF_LOCKED	= 2
+};
+
+enum ae_lock_state {
+	FIMC_IS_AE_UNLOCKED	= 0,
+	FIMC_IS_AE_LOCKED	= 1
+};
+
+enum awb_lock_state {
+	FIMC_IS_AWB_UNLOCKED	= 0,
+	FIMC_IS_AWB_LOCKED	= 1
+};
+
+enum {
+	IS_METERING_CONFIG_CMD,
+	IS_METERING_CONFIG_WIN_POS_X,
+	IS_METERING_CONFIG_WIN_POS_Y,
+	IS_METERING_CONFIG_WIN_WIDTH,
+	IS_METERING_CONFIG_WIN_HEIGHT,
+	IS_METERING_CONFIG_MAX
+};
+
+struct is_setfile {
+	const struct firmware *info;
+	int state;
+	u32 sub_index;
+	u32 base;
+	size_t size;
+};
+
+struct is_fd_result_header {
+	u32 offset;
+	u32 count;
+	u32 index;
+	u32 curr_index;
+	u32 width;
+	u32 height;
+};
+
+struct is_af_info {
+	u16 mode;
+	u32 af_state;
+	u32 af_lock_state;
+	u32 ae_lock_state;
+	u32 awb_lock_state;
+	u16 pos_x;
+	u16 pos_y;
+	u16 prev_pos_x;
+	u16 prev_pos_y;
+	u16 use_af;
+};
+
+struct fimc_is_firmware {
+	const struct firmware *f_w;
+
+	dma_addr_t paddr;
+	void *vaddr;
+	unsigned int size;
+
+	char info[FIMC_IS_FW_INFO_LEN + 1];
+	char version[FIMC_IS_FW_VER_LEN + 1];
+	char setfile_info[FIMC_IS_SETFILE_INFO_LEN + 1];
+	u8 state;
+};
+
+struct fimc_is_memory {
+	/* physical base address */
+	dma_addr_t paddr;
+	/* virtual base address */
+	void *vaddr;
+	/* total length */
+	unsigned int size;
+};
+
+#define FIMC_IS_I2H_MAX_ARGS	12
+
+struct i2h_cmd {
+	u32 cmd;
+	u32 sensor_id;
+	u16 num_args;
+	u32 args[FIMC_IS_I2H_MAX_ARGS];
+};
+
+struct h2i_cmd {
+	u16 cmd_type;
+	u32 entry_id;
+};
+
+#define FIMC_IS_DEBUG_MSG	0x3f
+#define FIMC_IS_DEBUG_LEVEL	3
+
+struct fimc_is_setfile {
+	const struct firmware *info;
+	unsigned int state;
+	unsigned int size;
+	u32 sub_index;
+	u32 base;
+};
+
+struct chain_config {
+	struct global_param	global;
+	struct sensor_param	sensor;
+	struct isp_param	isp;
+	struct drc_param	drc;
+	struct fd_param		fd;
+
+	unsigned long		p_region_index1;
+	unsigned long		p_region_index2;
+};
+
+/**
+ * struct fimc_is - fimc-is data structure
+ * @pdev: pointer to FIMC-IS platform device
+ * @pctrl: pointer to pinctrl structure for this device
+ * @v4l2_dev: pointer to top the level v4l2_device
+ * @alloc_ctx: videobuf2 memory allocator context
+ * @lock: mutex serializing video device and the subdev operations
+ * @slock: spinlock protecting this data structure and the hw registers
+ * @clocks: FIMC-LITE gate clock
+ * @regs: MCUCTL mmapped registers region
+ * @pmu_regs: PMU ISP mmapped registers region
+ * @irq_queue: interrupt handling waitqueue
+ * @lpm: low power mode flag
+ * @state: internal driver's state flags
+ */
+struct fimc_is {
+	struct platform_device		*pdev;
+	struct pinctrl			*pctrl;
+	struct v4l2_device		*v4l2_dev;
+
+	struct fimc_is_firmware		fw;
+	struct fimc_is_memory		memory;
+	struct firmware			*f_w;
+
+	struct fimc_isp			isp;
+	struct fimc_is_sensor		*sensor;
+	struct fimc_is_setfile		setfile;
+
+	struct vb2_alloc_ctx		*alloc_ctx;
+	struct v4l2_ctrl_handler	ctrl_handler;
+
+	struct mutex			lock;
+	spinlock_t			slock;
+
+	struct clk			*clocks[ISS_CLKS_MAX];
+	bool				clk_init;
+	void __iomem			*regs;
+	void __iomem			*pmu_regs;
+	int				irq;
+	wait_queue_head_t		irq_queue;
+	u8				lpm;
+
+	unsigned long			state;
+	unsigned int			sensor_index;
+
+	struct i2h_cmd			i2h_cmd;
+	struct h2i_cmd			h2i_cmd;
+	struct is_fd_result_header	fd_header;
+
+	struct chain_config		config[IS_SC_MAX];
+	unsigned			config_index;
+
+	struct is_region		*is_p_region;
+	dma_addr_t			is_dma_p_region;
+	struct is_share_region		*is_shared_region;
+	struct is_af_info		af;
+
+	struct dentry			*debugfs_entry;
+};
+
+static inline struct fimc_is *fimc_isp_to_is(struct fimc_isp *isp)
+{
+	return container_of(isp, struct fimc_is, isp);
+}
+
+static inline void fimc_is_mem_barrier(void)
+{
+	mb();
+}
+
+static inline void fimc_is_set_param_bit(struct fimc_is *is, int num)
+{
+	struct chain_config *cfg = &is->config[is->config_index];
+
+	if (num >= 32)
+		set_bit(num - 32, &cfg->p_region_index2);
+	else
+		set_bit(num, &cfg->p_region_index1);
+}
+
+static inline void fimc_is_set_param_ctrl_cmd(struct fimc_is *is, int cmd)
+{
+	is->is_p_region->parameter.isp.control.cmd = cmd;
+}
+
+static inline void mcuctl_write(u32 v, struct fimc_is *is, unsigned int offset)
+{
+	writel(v, is->regs + offset);
+}
+
+static inline u32 mcuctl_read(struct fimc_is *is, unsigned int offset)
+{
+	return readl(is->regs + offset);
+}
+
+static inline void pmuisp_write(u32 v, struct fimc_is *is, unsigned int offset)
+{
+	writel(v, is->pmu_regs + offset);
+}
+
+static inline u32 pmuisp_read(struct fimc_is *is, unsigned int offset)
+{
+	return readl(is->pmu_regs + offset);
+}
+
+int fimc_is_wait_event(struct fimc_is *is, unsigned long bit,
+		       unsigned int state, unsigned int timeout);
+int fimc_is_cpu_set_power(struct fimc_is *is, int on);
+int fimc_is_start_firmware(struct fimc_is *is);
+int fimc_is_hw_initialize(struct fimc_is *is);
+void fimc_is_log_dump(const char *level, const void *buf, size_t len);
+
+#endif /* FIMC_IS_H_ */
diff --git a/drivers/media/platform/exynos4-is/fimc-isp.c b/drivers/media/platform/exynos4-is/fimc-isp.c
new file mode 100644
index 00000000000..d63947f7b30
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-isp.c
@@ -0,0 +1,703 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *          Younghwan Joo <yhwan.joo@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/device.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/printk.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <media/v4l2-device.h>
+
+#include "media-dev.h"
+#include "fimc-is-command.h"
+#include "fimc-is-param.h"
+#include "fimc-is-regs.h"
+#include "fimc-is.h"
+
+static int debug;
+module_param_named(debug_isp, debug, int, S_IRUGO | S_IWUSR);
+
+static const struct fimc_fmt fimc_isp_formats[FIMC_ISP_NUM_FORMATS] = {
+	{
+		.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_isp_find_format - lookup 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_isp_formats array, ignored if negative
+ */
+const struct fimc_fmt *fimc_isp_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_isp_formats))
+		return NULL;
+
+	for (i = 0; i < ARRAY_SIZE(fimc_isp_formats); ++i) {
+		fmt = &fimc_isp_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;
+}
+
+void fimc_isp_irq_handler(struct fimc_is *is)
+{
+	is->i2h_cmd.args[0] = mcuctl_read(is, MCUCTL_REG_ISSR(20));
+	is->i2h_cmd.args[1] = mcuctl_read(is, MCUCTL_REG_ISSR(21));
+
+	fimc_is_fw_clear_irq1(is, FIMC_IS_INT_FRAME_DONE_ISP);
+
+	/* TODO: Complete ISP DMA interrupt handler */
+	wake_up(&is->irq_queue);
+}
+
+/* Capture subdev media entity operations */
+static int fimc_is_link_setup(struct media_entity *entity,
+				const struct media_pad *local,
+				const struct media_pad *remote, u32 flags)
+{
+	return 0;
+}
+
+static const struct media_entity_operations fimc_is_subdev_media_ops = {
+	.link_setup = fimc_is_link_setup,
+};
+
+static int fimc_is_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_isp_find_format(NULL, NULL, code->index);
+	if (!fmt)
+		return -EINVAL;
+	code->code = fmt->mbus_code;
+	return 0;
+}
+
+static int fimc_isp_subdev_get_fmt(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_fh *fh,
+				   struct v4l2_subdev_format *fmt)
+{
+	struct fimc_isp *isp = v4l2_get_subdevdata(sd);
+	struct fimc_is *is = fimc_isp_to_is(isp);
+	struct v4l2_mbus_framefmt *mf = &fmt->format;
+	struct v4l2_mbus_framefmt cur_fmt;
+
+	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(&isp->subdev_lock);
+	__is_get_frame_size(is, &cur_fmt);
+
+	if (fmt->pad == FIMC_ISP_SD_PAD_SINK) {
+		/* full camera input frame size */
+		mf->width = cur_fmt.width + FIMC_ISP_CAC_MARGIN_WIDTH;
+		mf->height = cur_fmt.height + FIMC_ISP_CAC_MARGIN_HEIGHT;
+		mf->code = V4L2_MBUS_FMT_SGRBG10_1X10;
+	} else {
+		/* crop size */
+		mf->width = cur_fmt.width;
+		mf->height = cur_fmt.height;
+		mf->code = V4L2_MBUS_FMT_YUV10_1X30;
+	}
+
+	mutex_unlock(&isp->subdev_lock);
+
+	v4l2_dbg(1, debug, sd, "%s: pad%d: fmt: 0x%x, %dx%d\n",
+		 __func__, fmt->pad, mf->code, mf->width, mf->height);
+
+	return 0;
+}
+
+static void __isp_subdev_try_format(struct fimc_isp *isp,
+				   struct v4l2_subdev_format *fmt)
+{
+	struct v4l2_mbus_framefmt *mf = &fmt->format;
+
+	if (fmt->pad == FIMC_ISP_SD_PAD_SINK) {
+		v4l_bound_align_image(&mf->width, FIMC_ISP_SINK_WIDTH_MIN,
+				FIMC_ISP_SINK_WIDTH_MAX, 0,
+				&mf->height, FIMC_ISP_SINK_HEIGHT_MIN,
+				FIMC_ISP_SINK_HEIGHT_MAX, 0, 0);
+		isp->subdev_fmt = *mf;
+	} else {
+		/* Allow changing format only on sink pad */
+		mf->width = isp->subdev_fmt.width - FIMC_ISP_CAC_MARGIN_WIDTH;
+		mf->height = isp->subdev_fmt.height - FIMC_ISP_CAC_MARGIN_HEIGHT;
+		mf->code = isp->subdev_fmt.code;
+	}
+}
+
+static int fimc_isp_subdev_set_fmt(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_fh *fh,
+				   struct v4l2_subdev_format *fmt)
+{
+	struct fimc_isp *isp = v4l2_get_subdevdata(sd);
+	struct fimc_is *is = fimc_isp_to_is(isp);
+	struct v4l2_mbus_framefmt *mf = &fmt->format;
+	int ret = 0;
+
+	v4l2_dbg(1, debug, sd, "%s: pad%d: code: 0x%x, %dx%d\n",
+		 __func__, fmt->pad, mf->code, mf->width, mf->height);
+
+	mf->colorspace = V4L2_COLORSPACE_JPEG;
+
+	mutex_lock(&isp->subdev_lock);
+	__isp_subdev_try_format(isp, fmt);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		mf = v4l2_subdev_get_try_format(fh, fmt->pad);
+		*mf = fmt->format;
+		mutex_unlock(&isp->subdev_lock);
+		return 0;
+	}
+
+	if (sd->entity.stream_count == 0)
+		__is_set_frame_size(is, mf);
+	else
+		ret = -EBUSY;
+	mutex_unlock(&isp->subdev_lock);
+
+	return ret;
+}
+
+static int fimc_isp_subdev_s_stream(struct v4l2_subdev *sd, int on)
+{
+	struct fimc_isp *isp = v4l2_get_subdevdata(sd);
+	struct fimc_is *is = fimc_isp_to_is(isp);
+	int ret;
+
+	v4l2_dbg(1, debug, sd, "%s: on: %d\n", __func__, on);
+
+	if (!test_bit(IS_ST_INIT_DONE, &is->state))
+		return -EBUSY;
+
+	fimc_is_mem_barrier();
+
+	if (on) {
+		if (__get_pending_param_count(is)) {
+			ret = fimc_is_itf_s_param(is, true);
+			if (ret < 0)
+				return ret;
+		}
+
+		v4l2_dbg(1, debug, sd, "changing mode to %d\n",
+						is->config_index);
+		ret = fimc_is_itf_mode_change(is);
+		if (ret)
+			return -EINVAL;
+
+		clear_bit(IS_ST_STREAM_ON, &is->state);
+		fimc_is_hw_stream_on(is);
+		ret = fimc_is_wait_event(is, IS_ST_STREAM_ON, 1,
+					 FIMC_IS_CONFIG_TIMEOUT);
+		if (ret < 0) {
+			v4l2_err(sd, "stream on timeout\n");
+			return ret;
+		}
+	} else {
+		clear_bit(IS_ST_STREAM_OFF, &is->state);
+		fimc_is_hw_stream_off(is);
+		ret = fimc_is_wait_event(is, IS_ST_STREAM_OFF, 1,
+					 FIMC_IS_CONFIG_TIMEOUT);
+		if (ret < 0) {
+			v4l2_err(sd, "stream off timeout\n");
+			return ret;
+		}
+		is->setfile.sub_index = 0;
+	}
+
+	return 0;
+}
+
+static int fimc_isp_subdev_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct fimc_isp *isp = v4l2_get_subdevdata(sd);
+	struct fimc_is *is = fimc_isp_to_is(isp);
+	int ret = 0;
+
+	pr_debug("on: %d\n", on);
+
+	if (on) {
+		ret = pm_runtime_get_sync(&is->pdev->dev);
+		if (ret < 0)
+			return ret;
+		set_bit(IS_ST_PWR_ON, &is->state);
+
+		ret = fimc_is_start_firmware(is);
+		if (ret < 0) {
+			v4l2_err(sd, "firmware booting failed\n");
+			pm_runtime_put(&is->pdev->dev);
+			return ret;
+		}
+		set_bit(IS_ST_PWR_SUBIP_ON, &is->state);
+
+		ret = fimc_is_hw_initialize(is);
+	} else {
+		/* Close sensor */
+		if (!test_bit(IS_ST_PWR_ON, &is->state)) {
+			fimc_is_hw_close_sensor(is, 0);
+
+			ret = fimc_is_wait_event(is, IS_ST_OPEN_SENSOR, 0,
+						 FIMC_IS_CONFIG_TIMEOUT);
+			if (ret < 0) {
+				v4l2_err(sd, "sensor close timeout\n");
+				return ret;
+			}
+		}
+
+		/* SUB IP power off */
+		if (test_bit(IS_ST_PWR_SUBIP_ON, &is->state)) {
+			fimc_is_hw_subip_power_off(is);
+			ret = fimc_is_wait_event(is, IS_ST_PWR_SUBIP_ON, 0,
+						 FIMC_IS_CONFIG_TIMEOUT);
+			if (ret < 0) {
+				v4l2_err(sd, "sub-IP power off timeout\n");
+				return ret;
+			}
+		}
+
+		fimc_is_cpu_set_power(is, 0);
+		pm_runtime_put_sync(&is->pdev->dev);
+
+		clear_bit(IS_ST_PWR_ON, &is->state);
+		clear_bit(IS_ST_INIT_DONE, &is->state);
+		is->state = 0;
+		is->config[is->config_index].p_region_index1 = 0;
+		is->config[is->config_index].p_region_index2 = 0;
+		set_bit(IS_ST_IDLE, &is->state);
+		wmb();
+	}
+
+	return ret;
+}
+
+static int fimc_isp_subdev_open(struct v4l2_subdev *sd,
+				struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_mbus_framefmt fmt;
+	struct v4l2_mbus_framefmt *format;
+
+	format = v4l2_subdev_get_try_format(fh, FIMC_ISP_SD_PAD_SINK);
+
+	fmt.colorspace = V4L2_COLORSPACE_SRGB;
+	fmt.code = fimc_isp_formats[0].mbus_code;
+	fmt.width = DEFAULT_PREVIEW_STILL_WIDTH + FIMC_ISP_CAC_MARGIN_WIDTH;
+	fmt.height = DEFAULT_PREVIEW_STILL_HEIGHT + FIMC_ISP_CAC_MARGIN_HEIGHT;
+	fmt.field = V4L2_FIELD_NONE;
+	*format = fmt;
+
+	format = v4l2_subdev_get_try_format(fh, FIMC_ISP_SD_PAD_SRC_FIFO);
+	fmt.width = DEFAULT_PREVIEW_STILL_WIDTH;
+	fmt.height = DEFAULT_PREVIEW_STILL_HEIGHT;
+	*format = fmt;
+
+	format = v4l2_subdev_get_try_format(fh, FIMC_ISP_SD_PAD_SRC_DMA);
+	*format = fmt;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_internal_ops fimc_is_subdev_internal_ops = {
+	.open = fimc_isp_subdev_open,
+};
+
+static const struct v4l2_subdev_pad_ops fimc_is_subdev_pad_ops = {
+	.enum_mbus_code = fimc_is_subdev_enum_mbus_code,
+	.get_fmt = fimc_isp_subdev_get_fmt,
+	.set_fmt = fimc_isp_subdev_set_fmt,
+};
+
+static const struct v4l2_subdev_video_ops fimc_is_subdev_video_ops = {
+	.s_stream = fimc_isp_subdev_s_stream,
+};
+
+static const struct v4l2_subdev_core_ops fimc_is_core_ops = {
+	.s_power = fimc_isp_subdev_s_power,
+};
+
+static struct v4l2_subdev_ops fimc_is_subdev_ops = {
+	.core = &fimc_is_core_ops,
+	.video = &fimc_is_subdev_video_ops,
+	.pad = &fimc_is_subdev_pad_ops,
+};
+
+static int __ctrl_set_white_balance(struct fimc_is *is, int value)
+{
+	switch (value) {
+	case V4L2_WHITE_BALANCE_AUTO:
+		__is_set_isp_awb(is, ISP_AWB_COMMAND_AUTO, 0);
+		break;
+	case V4L2_WHITE_BALANCE_DAYLIGHT:
+		__is_set_isp_awb(is, ISP_AWB_COMMAND_ILLUMINATION,
+					ISP_AWB_ILLUMINATION_DAYLIGHT);
+		break;
+	case V4L2_WHITE_BALANCE_CLOUDY:
+		__is_set_isp_awb(is, ISP_AWB_COMMAND_ILLUMINATION,
+					ISP_AWB_ILLUMINATION_CLOUDY);
+		break;
+	case V4L2_WHITE_BALANCE_INCANDESCENT:
+		__is_set_isp_awb(is, ISP_AWB_COMMAND_ILLUMINATION,
+					ISP_AWB_ILLUMINATION_TUNGSTEN);
+		break;
+	case V4L2_WHITE_BALANCE_FLUORESCENT:
+		__is_set_isp_awb(is, ISP_AWB_COMMAND_ILLUMINATION,
+					ISP_AWB_ILLUMINATION_FLUORESCENT);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int __ctrl_set_aewb_lock(struct fimc_is *is,
+				      struct v4l2_ctrl *ctrl)
+{
+	bool awb_lock = ctrl->val & V4L2_LOCK_WHITE_BALANCE;
+	bool ae_lock = ctrl->val & V4L2_LOCK_EXPOSURE;
+	struct isp_param *isp = &is->is_p_region->parameter.isp;
+	int cmd, ret;
+
+	cmd = ae_lock ? ISP_AA_COMMAND_STOP : ISP_AA_COMMAND_START;
+	isp->aa.cmd = cmd;
+	isp->aa.target = ISP_AA_TARGET_AE;
+	fimc_is_set_param_bit(is, PARAM_ISP_AA);
+	is->af.ae_lock_state = ae_lock;
+	wmb();
+
+	ret = fimc_is_itf_s_param(is, false);
+	if (ret < 0)
+		return ret;
+
+	cmd = awb_lock ? ISP_AA_COMMAND_STOP : ISP_AA_COMMAND_START;
+	isp->aa.cmd = cmd;
+	isp->aa.target = ISP_AA_TARGET_AE;
+	fimc_is_set_param_bit(is, PARAM_ISP_AA);
+	is->af.awb_lock_state = awb_lock;
+	wmb();
+
+	return fimc_is_itf_s_param(is, false);
+}
+
+/* Supported manual ISO values */
+static const s64 iso_qmenu[] = {
+	50, 100, 200, 400, 800,
+};
+
+static int __ctrl_set_iso(struct fimc_is *is, int value)
+{
+	unsigned int idx, iso;
+
+	if (value == V4L2_ISO_SENSITIVITY_AUTO) {
+		__is_set_isp_iso(is, ISP_ISO_COMMAND_AUTO, 0);
+		return 0;
+	}
+	idx = is->isp.ctrls.iso->val;
+	if (idx >= ARRAY_SIZE(iso_qmenu))
+		return -EINVAL;
+
+	iso = iso_qmenu[idx];
+	__is_set_isp_iso(is, ISP_ISO_COMMAND_MANUAL, iso);
+	return 0;
+}
+
+static int __ctrl_set_metering(struct fimc_is *is, unsigned int value)
+{
+	unsigned int val;
+
+	switch (value) {
+	case V4L2_EXPOSURE_METERING_AVERAGE:
+		val = ISP_METERING_COMMAND_AVERAGE;
+		break;
+	case V4L2_EXPOSURE_METERING_CENTER_WEIGHTED:
+		val = ISP_METERING_COMMAND_CENTER;
+		break;
+	case V4L2_EXPOSURE_METERING_SPOT:
+		val = ISP_METERING_COMMAND_SPOT;
+		break;
+	case V4L2_EXPOSURE_METERING_MATRIX:
+		val = ISP_METERING_COMMAND_MATRIX;
+		break;
+	default:
+		return -EINVAL;
+	};
+
+	__is_set_isp_metering(is, IS_METERING_CONFIG_CMD, val);
+	return 0;
+}
+
+static int __ctrl_set_afc(struct fimc_is *is, int value)
+{
+	switch (value) {
+	case V4L2_CID_POWER_LINE_FREQUENCY_DISABLED:
+		__is_set_isp_afc(is, ISP_AFC_COMMAND_DISABLE, 0);
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY_50HZ:
+		__is_set_isp_afc(is, ISP_AFC_COMMAND_MANUAL, 50);
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY_60HZ:
+		__is_set_isp_afc(is, ISP_AFC_COMMAND_MANUAL, 60);
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY_AUTO:
+		__is_set_isp_afc(is, ISP_AFC_COMMAND_AUTO, 0);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int __ctrl_set_image_effect(struct fimc_is *is, int value)
+{
+	static const u8 effects[][2] = {
+		{ V4L2_COLORFX_NONE,	 ISP_IMAGE_EFFECT_DISABLE },
+		{ V4L2_COLORFX_BW,	 ISP_IMAGE_EFFECT_MONOCHROME },
+		{ V4L2_COLORFX_SEPIA,	 ISP_IMAGE_EFFECT_SEPIA },
+		{ V4L2_COLORFX_NEGATIVE, ISP_IMAGE_EFFECT_NEGATIVE_MONO },
+		{ 16 /* TODO */,	 ISP_IMAGE_EFFECT_NEGATIVE_COLOR },
+	};
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(effects); i++) {
+		if (effects[i][0] != value)
+			continue;
+
+		__is_set_isp_effect(is, effects[i][1]);
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int fimc_is_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct fimc_isp *isp = ctrl_to_fimc_isp(ctrl);
+	struct fimc_is *is = fimc_isp_to_is(isp);
+	bool set_param = true;
+	int ret = 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_CONTRAST:
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_CONTRAST,
+				    ctrl->val);
+		break;
+
+	case V4L2_CID_SATURATION:
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_SATURATION,
+				    ctrl->val);
+		break;
+
+	case V4L2_CID_SHARPNESS:
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_SHARPNESS,
+				    ctrl->val);
+		break;
+
+	case V4L2_CID_EXPOSURE_ABSOLUTE:
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_EXPOSURE,
+				    ctrl->val);
+		break;
+
+	case V4L2_CID_BRIGHTNESS:
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_BRIGHTNESS,
+				    ctrl->val);
+		break;
+
+	case V4L2_CID_HUE:
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_HUE,
+				    ctrl->val);
+		break;
+
+	case V4L2_CID_EXPOSURE_METERING:
+		ret = __ctrl_set_metering(is, ctrl->val);
+		break;
+
+	case V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE:
+		ret = __ctrl_set_white_balance(is, ctrl->val);
+		break;
+
+	case V4L2_CID_3A_LOCK:
+		ret = __ctrl_set_aewb_lock(is, ctrl);
+		set_param = false;
+		break;
+
+	case V4L2_CID_ISO_SENSITIVITY_AUTO:
+		ret = __ctrl_set_iso(is, ctrl->val);
+		break;
+
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		ret = __ctrl_set_afc(is, ctrl->val);
+		break;
+
+	case V4L2_CID_COLORFX:
+		__ctrl_set_image_effect(is, ctrl->val);
+		break;
+
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	if (ret < 0) {
+		v4l2_err(&isp->subdev, "Failed to set control: %s (%d)\n",
+						ctrl->name, ctrl->val);
+		return ret;
+	}
+
+	if (set_param && test_bit(IS_ST_STREAM_ON, &is->state))
+		return fimc_is_itf_s_param(is, true);
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops fimc_isp_ctrl_ops = {
+	.s_ctrl	= fimc_is_s_ctrl,
+};
+
+int fimc_isp_subdev_create(struct fimc_isp *isp)
+{
+	const struct v4l2_ctrl_ops *ops = &fimc_isp_ctrl_ops;
+	struct v4l2_ctrl_handler *handler = &isp->ctrls.handler;
+	struct v4l2_subdev *sd = &isp->subdev;
+	struct fimc_isp_ctrls *ctrls = &isp->ctrls;
+	int ret;
+
+	mutex_init(&isp->subdev_lock);
+
+	v4l2_subdev_init(sd, &fimc_is_subdev_ops);
+	sd->grp_id = GRP_ID_FIMC_IS;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	snprintf(sd->name, sizeof(sd->name), "FIMC-IS-ISP");
+
+	isp->subdev_pads[FIMC_ISP_SD_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	isp->subdev_pads[FIMC_ISP_SD_PAD_SRC_FIFO].flags = MEDIA_PAD_FL_SOURCE;
+	isp->subdev_pads[FIMC_ISP_SD_PAD_SRC_DMA].flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_init(&sd->entity, FIMC_ISP_SD_PADS_NUM,
+				isp->subdev_pads, 0);
+	if (ret)
+		return ret;
+
+	v4l2_ctrl_handler_init(handler, 20);
+
+	ctrls->saturation = v4l2_ctrl_new_std(handler, ops, V4L2_CID_SATURATION,
+						-2, 2, 1, 0);
+	ctrls->brightness = v4l2_ctrl_new_std(handler, ops, V4L2_CID_BRIGHTNESS,
+						-4, 4, 1, 0);
+	ctrls->contrast = v4l2_ctrl_new_std(handler, ops, V4L2_CID_CONTRAST,
+						-2, 2, 1, 0);
+	ctrls->sharpness = v4l2_ctrl_new_std(handler, ops, V4L2_CID_SHARPNESS,
+						-2, 2, 1, 0);
+	ctrls->hue = v4l2_ctrl_new_std(handler, ops, V4L2_CID_HUE,
+						-2, 2, 1, 0);
+
+	ctrls->auto_wb = v4l2_ctrl_new_std_menu(handler, ops,
+					V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE,
+					8, ~0x14e, V4L2_WHITE_BALANCE_AUTO);
+
+	ctrls->exposure = v4l2_ctrl_new_std(handler, ops,
+					V4L2_CID_EXPOSURE_ABSOLUTE,
+					-4, 4, 1, 0);
+
+	ctrls->exp_metering = v4l2_ctrl_new_std_menu(handler, ops,
+					V4L2_CID_EXPOSURE_METERING, 3,
+					~0xf, V4L2_EXPOSURE_METERING_AVERAGE);
+
+	v4l2_ctrl_new_std_menu(handler, ops, V4L2_CID_POWER_LINE_FREQUENCY,
+					V4L2_CID_POWER_LINE_FREQUENCY_AUTO, 0,
+					V4L2_CID_POWER_LINE_FREQUENCY_AUTO);
+	/* ISO sensitivity */
+	ctrls->auto_iso = v4l2_ctrl_new_std_menu(handler, ops,
+			V4L2_CID_ISO_SENSITIVITY_AUTO, 1, 0,
+			V4L2_ISO_SENSITIVITY_AUTO);
+
+	ctrls->iso = v4l2_ctrl_new_int_menu(handler, ops,
+			V4L2_CID_ISO_SENSITIVITY, ARRAY_SIZE(iso_qmenu) - 1,
+			ARRAY_SIZE(iso_qmenu)/2 - 1, iso_qmenu);
+
+	ctrls->aewb_lock = v4l2_ctrl_new_std(handler, ops,
+					V4L2_CID_3A_LOCK, 0, 0x3, 0, 0);
+
+	/* TODO: Add support for NEGATIVE_COLOR option */
+	ctrls->colorfx = v4l2_ctrl_new_std_menu(handler, ops, V4L2_CID_COLORFX,
+			V4L2_COLORFX_SET_CBCR + 1, ~0x1000f, V4L2_COLORFX_NONE);
+
+	if (handler->error) {
+		media_entity_cleanup(&sd->entity);
+		return handler->error;
+	}
+
+	v4l2_ctrl_auto_cluster(2, &ctrls->auto_iso,
+			V4L2_ISO_SENSITIVITY_MANUAL, false);
+
+	sd->ctrl_handler = handler;
+	sd->internal_ops = &fimc_is_subdev_internal_ops;
+	sd->entity.ops = &fimc_is_subdev_media_ops;
+	v4l2_set_subdevdata(sd, isp);
+
+	return 0;
+}
+
+void fimc_isp_subdev_destroy(struct fimc_isp *isp)
+{
+	struct v4l2_subdev *sd = &isp->subdev;
+
+	v4l2_device_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(&isp->ctrls.handler);
+	v4l2_set_subdevdata(sd, NULL);
+}
diff --git a/drivers/media/platform/exynos4-is/fimc-isp.h b/drivers/media/platform/exynos4-is/fimc-isp.h
new file mode 100644
index 00000000000..800aba7ab4a
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-isp.h
@@ -0,0 +1,181 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *          Younghwan Joo <yhwan.joo@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#ifndef FIMC_ISP_H_
+#define FIMC_ISP_H_
+
+#include <linux/io.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>
+
+/* FIXME: revisit these constraints */
+#define FIMC_ISP_SINK_WIDTH_MIN		(16 + 8)
+#define FIMC_ISP_SINK_HEIGHT_MIN	(12 + 8)
+#define FIMC_ISP_SOURCE_WIDTH_MIN	8
+#define FIMC_ISP_SOURC_HEIGHT_MIN	8
+#define FIMC_ISP_CAC_MARGIN_WIDTH	16
+#define FIMC_ISP_CAC_MARGIN_HEIGHT	12
+
+#define FIMC_ISP_SINK_WIDTH_MAX		(4000 - 16)
+#define FIMC_ISP_SINK_HEIGHT_MAX	(4000 + 12)
+#define FIMC_ISP_SOURCE_WIDTH_MAX	4000
+#define FIMC_ISP_SOURC_HEIGHT_MAX	4000
+
+#define FIMC_ISP_NUM_FORMATS		3
+#define FIMC_ISP_REQ_BUFS_MIN		2
+
+#define FIMC_ISP_SD_PAD_SINK		0
+#define FIMC_ISP_SD_PAD_SRC_FIFO	1
+#define FIMC_ISP_SD_PAD_SRC_DMA		2
+#define FIMC_ISP_SD_PADS_NUM		3
+#define FIMC_ISP_MAX_PLANES		1
+
+/**
+ * struct fimc_isp_frame - source/target frame properties
+ * @width: full image width
+ * @height: full image height
+ * @rect: crop/composition rectangle
+ */
+struct fimc_isp_frame {
+	u16 width;
+	u16 height;
+	struct v4l2_rect rect;
+};
+
+struct fimc_isp_ctrls {
+	struct v4l2_ctrl_handler handler;
+
+	/* Auto white balance */
+	struct v4l2_ctrl *auto_wb;
+	/* Auto ISO control cluster */
+	struct {
+		struct v4l2_ctrl *auto_iso;
+		struct v4l2_ctrl *iso;
+	};
+	/* Adjust - contrast */
+	struct v4l2_ctrl *contrast;
+	/* Adjust - saturation */
+	struct v4l2_ctrl *saturation;
+	/* Adjust - sharpness */
+	struct v4l2_ctrl *sharpness;
+	/* Adjust - brightness */
+	struct v4l2_ctrl *brightness;
+	/* Adjust - hue */
+	struct v4l2_ctrl *hue;
+
+	/* Auto/manual exposure */
+	struct v4l2_ctrl *auto_exp;
+	/* Manual exposure value */
+	struct v4l2_ctrl *exposure;
+	/* AE/AWB lock/unlock */
+	struct v4l2_ctrl *aewb_lock;
+	/* Exposure metering mode */
+	struct v4l2_ctrl *exp_metering;
+	/* AFC */
+	struct v4l2_ctrl *afc;
+	/* ISP image effect */
+	struct v4l2_ctrl *colorfx;
+};
+
+/**
+ * struct fimc_is_video - fimc-is video device structure
+ * @vdev: video_device structure
+ * @type: video device type (CAPTURE/OUTPUT)
+ * @pad: video device media (sink) pad
+ * @pending_buf_q: pending buffers queue head
+ * @active_buf_q: a queue head of buffers scheduled in hardware
+ * @vb_queue: vb2 buffer queue
+ * @active_buf_count: number of video buffers scheduled in hardware
+ * @frame_count: counter of frames dequeued to user space
+ * @reqbufs_count: number of buffers requested with REQBUFS ioctl
+ * @format: current pixel format
+ */
+struct fimc_is_video {
+	struct video_device	vdev;
+	enum v4l2_buf_type	type;
+	struct media_pad	pad;
+	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			streaming;
+	unsigned long		payload[FIMC_ISP_MAX_PLANES];
+	const struct fimc_fmt	*format;
+};
+
+/**
+ * struct fimc_isp - FIMC-IS ISP data structure
+ * @pdev: pointer to FIMC-IS platform device
+ * @alloc_ctx: videobuf2 memory allocator context
+ * @subdev: ISP v4l2_subdev
+ * @subdev_pads: the ISP subdev media pads
+ * @ctrl_handler: v4l2 controls handler
+ * @test_pattern: test pattern controls
+ * @pipeline: video capture pipeline data structure
+ * @video_lock: mutex serializing video device and the subdev operations
+ * @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
+ * @source_subdev_grp_id: group id of remote source subdev
+ * @cac_margin_x: horizontal CAC margin in pixels
+ * @cac_margin_y: vertical CAC margin in pixels
+ * @state: driver state flags
+ * @video_capture: the ISP block video capture device
+ */
+struct fimc_isp {
+	struct platform_device		*pdev;
+	struct vb2_alloc_ctx		*alloc_ctx;
+	struct v4l2_subdev		subdev;
+	struct media_pad		subdev_pads[FIMC_ISP_SD_PADS_NUM];
+	struct v4l2_mbus_framefmt	subdev_fmt;
+	struct v4l2_ctrl		*test_pattern;
+	struct fimc_isp_ctrls		ctrls;
+
+	struct mutex			video_lock;
+	struct mutex			subdev_lock;
+
+	struct fimc_isp_frame		inp_frame;
+	struct fimc_isp_frame		out_frame;
+	unsigned int			source_subdev_grp_id;
+
+	unsigned int			cac_margin_x;
+	unsigned int			cac_margin_y;
+
+	unsigned long			state;
+
+	struct fimc_is_video		video_capture;
+};
+
+#define ctrl_to_fimc_isp(_ctrl) \
+	container_of(ctrl->handler, struct fimc_isp, ctrls.handler)
+
+struct fimc_is;
+
+int fimc_isp_subdev_create(struct fimc_isp *isp);
+void fimc_isp_subdev_destroy(struct fimc_isp *isp);
+void fimc_isp_irq_handler(struct fimc_is *is);
+int fimc_is_create_controls(struct fimc_isp *isp);
+int fimc_is_delete_controls(struct fimc_isp *isp);
+const struct fimc_fmt *fimc_isp_find_format(const u32 *pixelformat,
+					const u32 *mbus_code, int index);
+#endif /* FIMC_ISP_H_ */
diff --git a/drivers/media/platform/s5p-fimc/fimc-lite-reg.c b/drivers/media/platform/exynos4-is/fimc-lite-reg.c
index f0af0754a7b..8cc0d39a2fe 100644
--- a/drivers/media/platform/s5p-fimc/fimc-lite-reg.c
+++ b/drivers/media/platform/exynos4-is/fimc-lite-reg.c
@@ -127,11 +127,11 @@ static const u32 src_pixfmt_map[8][3] = {
 /* 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);
+	enum v4l2_mbus_pixelcode pixelcode = f->fmt->mbus_code;
+	int i = ARRAY_SIZE(src_pixfmt_map);
 	u32 cfg;
 
-	while (i-- >= 0) {
+	while (--i >= 0) {
 		if (src_pixfmt_map[i][0] == pixelcode)
 			break;
 	}
@@ -224,10 +224,10 @@ static void flite_hw_set_out_order(struct fimc_lite *dev, struct flite_frame *f)
 		{ V4L2_MBUS_FMT_VYUY8_2X8, FLITE_REG_CIODMAFMT_CRYCBY },
 	};
 	u32 cfg = readl(dev->regs + FLITE_REG_CIODMAFMT);
-	unsigned int i = ARRAY_SIZE(pixcode);
+	int i = ARRAY_SIZE(pixcode);
 
-	while (i-- >= 0)
-		if (pixcode[i][0] == dev->fmt->mbus_code)
+	while (--i >= 0)
+		if (pixcode[i][0] == f->fmt->mbus_code)
 			break;
 	cfg &= ~FLITE_REG_CIODMAFMT_YCBCR_ORDER_MASK;
 	writel(cfg | pixcode[i][1], dev->regs + FLITE_REG_CIODMAFMT);
diff --git a/drivers/media/platform/s5p-fimc/fimc-lite-reg.h b/drivers/media/platform/exynos4-is/fimc-lite-reg.h
index 0e345844c13..390383941c1 100644
--- a/drivers/media/platform/s5p-fimc/fimc-lite-reg.h
+++ b/drivers/media/platform/exynos4-is/fimc-lite-reg.h
@@ -72,10 +72,10 @@
 #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_YCBYCR		(0 << 4)
+#define FLITE_REG_CIODMAFMT_YCRYCB		(1 << 4)
+#define FLITE_REG_CIODMAFMT_CBYCRY		(2 << 4)
+#define FLITE_REG_CIODMAFMT_CRYCBY		(3 << 4)
 #define FLITE_REG_CIODMAFMT_YCBCR_ORDER_MASK	(0x3 << 4)
 
 /* Camera Output Canvas */
diff --git a/drivers/media/platform/s5p-fimc/fimc-lite.c b/drivers/media/platform/exynos4-is/fimc-lite.c
index bfc4206935c..14bb7bc8adb 100644
--- a/drivers/media/platform/s5p-fimc/fimc-lite.c
+++ b/drivers/media/platform/exynos4-is/fimc-lite.c
@@ -11,12 +11,14 @@
 #define pr_fmt(fmt) "%s:%d " fmt, __func__, __LINE__
 
 #include <linux/bug.h>
+#include <linux/clk.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/of.h>
 #include <linux/types.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
@@ -30,7 +32,6 @@
 #include <media/videobuf2-dma-contig.h>
 #include <media/s5p_fimc.h>
 
-#include "fimc-mdevice.h"
 #include "fimc-core.h"
 #include "fimc-lite.h"
 #include "fimc-lite-reg.h"
@@ -46,6 +47,7 @@ static const struct fimc_fmt fimc_lite_formats[] = {
 		.color		= FIMC_FMT_YCBYCR422,
 		.memplanes	= 1,
 		.mbus_code	= V4L2_MBUS_FMT_YUYV8_2X8,
+		.flags		= FMT_FLAGS_YUV,
 	}, {
 		.name		= "YUV 4:2:2 packed, CbYCrY",
 		.fourcc		= V4L2_PIX_FMT_UYVY,
@@ -53,6 +55,7 @@ static const struct fimc_fmt fimc_lite_formats[] = {
 		.color		= FIMC_FMT_CBYCRY422,
 		.memplanes	= 1,
 		.mbus_code	= V4L2_MBUS_FMT_UYVY8_2X8,
+		.flags		= FMT_FLAGS_YUV,
 	}, {
 		.name		= "YUV 4:2:2 packed, CrYCbY",
 		.fourcc		= V4L2_PIX_FMT_VYUY,
@@ -60,6 +63,7 @@ static const struct fimc_fmt fimc_lite_formats[] = {
 		.color		= FIMC_FMT_CRYCBY422,
 		.memplanes	= 1,
 		.mbus_code	= V4L2_MBUS_FMT_VYUY8_2X8,
+		.flags		= FMT_FLAGS_YUV,
 	}, {
 		.name		= "YUV 4:2:2 packed, YCrYCb",
 		.fourcc		= V4L2_PIX_FMT_YVYU,
@@ -67,6 +71,7 @@ static const struct fimc_fmt fimc_lite_formats[] = {
 		.color		= FIMC_FMT_YCRYCB422,
 		.memplanes	= 1,
 		.mbus_code	= V4L2_MBUS_FMT_YVYU8_2X8,
+		.flags		= FMT_FLAGS_YUV,
 	}, {
 		.name		= "RAW8 (GRBG)",
 		.fourcc		= V4L2_PIX_FMT_SGRBG8,
@@ -74,6 +79,7 @@ static const struct fimc_fmt fimc_lite_formats[] = {
 		.color		= FIMC_FMT_RAW8,
 		.memplanes	= 1,
 		.mbus_code	= V4L2_MBUS_FMT_SGRBG8_1X8,
+		.flags		= FMT_FLAGS_RAW_BAYER,
 	}, {
 		.name		= "RAW10 (GRBG)",
 		.fourcc		= V4L2_PIX_FMT_SGRBG10,
@@ -81,6 +87,7 @@ static const struct fimc_fmt fimc_lite_formats[] = {
 		.color		= FIMC_FMT_RAW10,
 		.memplanes	= 1,
 		.mbus_code	= V4L2_MBUS_FMT_SGRBG10_1X10,
+		.flags		= FMT_FLAGS_RAW_BAYER,
 	}, {
 		.name		= "RAW12 (GRBG)",
 		.fourcc		= V4L2_PIX_FMT_SGRBG12,
@@ -88,6 +95,7 @@ static const struct fimc_fmt fimc_lite_formats[] = {
 		.color		= FIMC_FMT_RAW12,
 		.memplanes	= 1,
 		.mbus_code	= V4L2_MBUS_FMT_SGRBG12_1X12,
+		.flags		= FMT_FLAGS_RAW_BAYER,
 	},
 };
 
@@ -95,10 +103,11 @@ static const struct fimc_fmt fimc_lite_formats[] = {
  * 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
+ * @mask: the color format flags to match
  * @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 u32 *mbus_code, unsigned int mask, int index)
 {
 	const struct fimc_fmt *fmt, *def_fmt = NULL;
 	unsigned int i;
@@ -109,6 +118,8 @@ static const struct fimc_fmt *fimc_lite_find_format(const u32 *pixelformat,
 
 	for (i = 0; i < ARRAY_SIZE(fimc_lite_formats); ++i) {
 		fmt = &fimc_lite_formats[i];
+		if (mask && !(fmt->flags & mask))
+			continue;
 		if (pixelformat && fmt->fourcc == *pixelformat)
 			return fmt;
 		if (mbus_code && fmt->mbus_code == *mbus_code)
@@ -120,26 +131,49 @@ static const struct fimc_fmt *fimc_lite_find_format(const u32 *pixelformat,
 	return def_fmt;
 }
 
+/* Called with the media graph mutex held or @me stream_count > 0. */
+static struct v4l2_subdev *__find_remote_sensor(struct media_entity *me)
+{
+	struct media_pad *pad = &me->pads[0];
+	struct v4l2_subdev *sd;
+
+	while (pad->flags & MEDIA_PAD_FL_SINK) {
+		/* 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 == GRP_ID_FIMC_IS_SENSOR ||
+		    sd->grp_id == GRP_ID_SENSOR)
+			return sd;
+		/* sink pad */
+		pad = &sd->entity.pads[0];
+	}
+	return NULL;
+}
+
 static int fimc_lite_hw_init(struct fimc_lite *fimc, bool isp_output)
 {
-	struct fimc_pipeline *pipeline = &fimc->pipeline;
-	struct v4l2_subdev *sensor;
-	struct fimc_sensor_info *si;
+	struct fimc_source_info *si;
 	unsigned long flags;
 
-	sensor = isp_output ? fimc->sensor : pipeline->subdevs[IDX_SENSOR];
-
-	if (sensor == NULL)
+	if (fimc->sensor == NULL)
 		return -ENXIO;
 
-	if (fimc->fmt == NULL)
+	if (fimc->inp_frame.fmt == NULL || fimc->out_frame.fmt == NULL)
 		return -EINVAL;
 
 	/* Get sensor configuration data from the sensor subdev */
-	si = v4l2_get_subdev_hostdata(sensor);
+	si = v4l2_get_subdev_hostdata(fimc->sensor);
+	if (!si)
+		return -EINVAL;
+
 	spin_lock_irqsave(&fimc->slock, flags);
 
-	flite_hw_set_camera_bus(fimc, &si->pdata);
+	flite_hw_set_camera_bus(fimc, si);
 	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, !isp_output);
@@ -339,13 +373,13 @@ static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *pfmt,
 	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;
+	const struct fimc_fmt *fmt = frame->fmt;
 	unsigned long wh;
 	int i;
 
 	if (pfmt) {
 		pixm = &pfmt->fmt.pix_mp;
-		fmt = fimc_lite_find_format(&pixm->pixelformat, NULL, -1);
+		fmt = fimc_lite_find_format(&pixm->pixelformat, NULL, 0, -1);
 		wh = pixm->width * pixm->height;
 	} else {
 		wh = frame->f_width * frame->f_height;
@@ -374,10 +408,10 @@ static int buffer_prepare(struct vb2_buffer *vb)
 	struct fimc_lite *fimc = vq->drv_priv;
 	int i;
 
-	if (fimc->fmt == NULL)
+	if (fimc->out_frame.fmt == NULL)
 		return -EINVAL;
 
-	for (i = 0; i < fimc->fmt->memplanes; i++) {
+	for (i = 0; i < fimc->out_frame.fmt->memplanes; i++) {
 		unsigned long size = fimc->payload[i];
 
 		if (vb2_plane_size(vb, i) < size) {
@@ -425,24 +459,12 @@ static void buffer_queue(struct vb2_buffer *vb)
 	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,
+	.wait_prepare	 = vb2_ops_wait_prepare,
+	.wait_finish	 = vb2_ops_wait_finish,
 	.start_streaming = start_streaming,
 	.stop_streaming	 = stop_streaming,
 };
@@ -467,99 +489,73 @@ static int fimc_lite_open(struct file *file)
 	mutex_lock(&fimc->lock);
 	if (atomic_read(&fimc->out_path) != FIMC_IO_DMA) {
 		ret = -EBUSY;
-		goto done;
+		goto unlock;
 	}
 
 	set_bit(ST_FLITE_IN_USE, &fimc->state);
 	ret = pm_runtime_get_sync(&fimc->pdev->dev);
 	if (ret < 0)
-		goto done;
+		goto unlock;
 
 	ret = v4l2_fh_open(file);
 	if (ret < 0)
-		goto done;
+		goto err_pm;
 
-	if (++fimc->ref_count == 1 &&
-	    atomic_read(&fimc->out_path) == FIMC_IO_DMA) {
-		ret = fimc_pipeline_call(fimc, open, &fimc->pipeline,
-					 &fimc->vfd.entity, true);
-		if (ret < 0) {
-			pm_runtime_put_sync(&fimc->pdev->dev);
-			fimc->ref_count--;
-			v4l2_fh_release(file);
-			clear_bit(ST_FLITE_IN_USE, &fimc->state);
-		}
+	if (!v4l2_fh_is_singular_file(file) ||
+	    atomic_read(&fimc->out_path) != FIMC_IO_DMA)
+		goto unlock;
 
+	ret = fimc_pipeline_call(fimc, open, &fimc->pipeline,
+						me, true);
+	if (!ret) {
 		fimc_lite_clear_event_counters(fimc);
+		fimc->ref_count++;
+		goto unlock;
 	}
-done:
+
+	v4l2_fh_release(file);
+err_pm:
+	pm_runtime_put_sync(&fimc->pdev->dev);
+	clear_bit(ST_FLITE_IN_USE, &fimc->state);
+unlock:
 	mutex_unlock(&fimc->lock);
 	mutex_unlock(&me->parent->graph_mutex);
 	return ret;
 }
 
-static int fimc_lite_close(struct file *file)
+static int fimc_lite_release(struct file *file)
 {
 	struct fimc_lite *fimc = video_drvdata(file);
-	int ret;
 
 	mutex_lock(&fimc->lock);
 
-	if (--fimc->ref_count == 0 &&
+	if (v4l2_fh_is_singular_file(file) &&
 	    atomic_read(&fimc->out_path) == FIMC_IO_DMA) {
+		if (fimc->streaming) {
+			media_entity_pipeline_stop(&fimc->vfd.entity);
+			fimc->streaming = false;
+		}
 		clear_bit(ST_FLITE_IN_USE, &fimc->state);
 		fimc_lite_stop_capture(fimc, false);
 		fimc_pipeline_call(fimc, close, &fimc->pipeline);
-		clear_bit(ST_FLITE_SUSPENDED, &fimc->state);
+		fimc->ref_count--;
 	}
 
+	vb2_fop_release(file);
 	pm_runtime_put(&fimc->pdev->dev);
+	clear_bit(ST_FLITE_SUSPENDED, &fimc->state);
 
-	if (fimc->ref_count == 0)
-		vb2_queue_release(&fimc->vb_queue);
-
-	ret = v4l2_fh_release(file);
-
-	mutex_unlock(&fimc->lock);
-	return ret;
-}
-
-static unsigned int fimc_lite_poll(struct file *file,
-				   struct poll_table_struct *wait)
-{
-	struct fimc_lite *fimc = video_drvdata(file);
-	int ret;
-
-	if (mutex_lock_interruptible(&fimc->lock))
-		return POLL_ERR;
-
-	ret = vb2_poll(&fimc->vb_queue, file, wait);
-	mutex_unlock(&fimc->lock);
-
-	return ret;
-}
-
-static int fimc_lite_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	struct fimc_lite *fimc = video_drvdata(file);
-	int ret;
-
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
-	ret = vb2_mmap(&fimc->vb_queue, vma);
 	mutex_unlock(&fimc->lock);
-
-	return ret;
+	return 0;
 }
 
 static const struct v4l2_file_operations fimc_lite_fops = {
 	.owner		= THIS_MODULE,
 	.open		= fimc_lite_open,
-	.release	= fimc_lite_close,
-	.poll		= fimc_lite_poll,
+	.release	= fimc_lite_release,
+	.poll		= vb2_fop_poll,
 	.unlocked_ioctl	= video_ioctl2,
-	.mmap		= fimc_lite_mmap,
+	.mmap		= vb2_fop_mmap,
 };
 
 /*
@@ -570,10 +566,23 @@ 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;
+	struct flite_drvdata *dd = fimc->dd;
 	const struct fimc_fmt *fmt;
+	unsigned int flags = 0;
 
-	fmt = fimc_lite_find_format(fourcc, code, 0);
+	if (pad == FLITE_SD_PAD_SINK) {
+		v4l_bound_align_image(width, 8, dd->max_width,
+				      ffs(dd->out_width_align) - 1,
+				      height, 0, dd->max_height, 0, 0);
+	} else {
+		v4l_bound_align_image(width, 8, fimc->inp_frame.rect.width,
+				      ffs(dd->out_width_align) - 1,
+				      height, 0, fimc->inp_frame.rect.height,
+				      0, 0);
+		flags = fimc->inp_frame.fmt->flags;
+	}
+
+	fmt = fimc_lite_find_format(fourcc, code, flags, 0);
 	if (WARN_ON(!fmt))
 		return NULL;
 
@@ -582,17 +591,6 @@ static const struct fimc_fmt *fimc_lite_try_format(struct fimc_lite *fimc,
 	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);
 
@@ -608,7 +606,7 @@ static void fimc_lite_try_crop(struct fimc_lite *fimc, struct v4l2_rect *r)
 
 	/* 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->left = round_down(r->left, fimc->dd->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\n",
@@ -628,7 +626,7 @@ static void fimc_lite_try_compose(struct fimc_lite *fimc, struct v4l2_rect *r)
 
 	/* 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->left = round_down(r->left, fimc->dd->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\n",
@@ -671,7 +669,7 @@ static int fimc_lite_g_fmt_mplane(struct file *file, void *fh,
 	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;
+	const struct fimc_fmt *fmt = frame->fmt;
 
 	plane_fmt->bytesperline = (frame->f_width * fmt->depth[0]) / 8;
 	plane_fmt->sizeimage = plane_fmt->bytesperline * frame->f_height;
@@ -689,18 +687,31 @@ 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;
+	struct flite_drvdata *dd = fimc->dd;
+	const struct fimc_fmt *inp_fmt = fimc->inp_frame.fmt;
 	const struct fimc_fmt *fmt;
 
-	fmt = fimc_lite_find_format(&pixm->pixelformat, NULL, 0);
+	if (WARN_ON(inp_fmt == NULL))
+		return -EINVAL;
+	/*
+	 * We allow some flexibility only for YUV formats. In case of raw
+	 * raw Bayer the FIMC-LITE's output format must match its camera
+	 * interface input format.
+	 */
+	if (inp_fmt->flags & FMT_FLAGS_YUV)
+		fmt = fimc_lite_find_format(&pixm->pixelformat, NULL,
+						inp_fmt->flags, 0);
+	else
+		fmt = inp_fmt;
+
 	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);
+	v4l_bound_align_image(&pixm->width, 8, dd->max_width,
+			      ffs(dd->out_width_align) - 1,
+			      &pixm->height, 0, dd->max_height, 0, 0);
 
 	if ((bpl == 0 || ((bpl * 8) / fmt->depth[0]) < pixm->width))
 		pixm->plane_fmt[0].bytesperline = (pixm->width *
@@ -720,7 +731,6 @@ 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);
 }
 
@@ -740,7 +750,7 @@ static int fimc_lite_s_fmt_mplane(struct file *file, void *priv,
 	if (ret < 0)
 		return ret;
 
-	fimc->fmt = fmt;
+	frame->fmt = fmt;
 	fimc->payload[0] = max((pixm->width * pixm->height * fmt->depth[0]) / 8,
 			       pixm->plane_fmt[0].sizeimage);
 	frame->f_width = pixm->width;
@@ -766,7 +776,7 @@ static int fimc_pipeline_validate(struct fimc_lite *fimc)
 			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;
+			sink_fmt.format.code = fimc->inp_frame.fmt->mbus_code;
 		} else {
 			sink_fmt.pad = pad->index;
 			sink_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
@@ -800,37 +810,47 @@ 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 media_entity *entity = &fimc->vfd.entity;
 	struct fimc_pipeline *p = &fimc->pipeline;
 	int ret;
 
 	if (fimc_lite_active(fimc))
 		return -EBUSY;
 
-	ret = media_entity_pipeline_start(&sensor->entity, p->m_pipeline);
+	ret = media_entity_pipeline_start(entity, p->m_pipeline);
 	if (ret < 0)
 		return ret;
 
 	ret = fimc_pipeline_validate(fimc);
-	if (ret) {
-		media_entity_pipeline_stop(&sensor->entity);
+	if (ret < 0)
+		goto err_p_stop;
+
+	fimc->sensor = __find_remote_sensor(&fimc->subdev.entity);
+
+	ret = vb2_ioctl_streamon(file, priv, type);
+	if (!ret) {
+		fimc->streaming = true;
 		return ret;
 	}
 
-	return vb2_streamon(&fimc->vb_queue, type);
+err_p_stop:
+	media_entity_pipeline_stop(entity);
+	return 0;
 }
 
 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;
+	ret = vb2_ioctl_streamoff(file, priv, type);
+	if (ret < 0)
+		return ret;
+
+	media_entity_pipeline_stop(&fimc->vfd.entity);
+	fimc->streaming = false;
+	return 0;
 }
 
 static int fimc_lite_reqbufs(struct file *file, void *priv,
@@ -840,53 +860,13 @@ static int fimc_lite_reqbufs(struct file *file, void *priv,
 	int ret;
 
 	reqbufs->count = max_t(u32, FLITE_REQ_BUFS_MIN, reqbufs->count);
-	ret = vb2_reqbufs(&fimc->vb_queue, reqbufs);
+	ret = vb2_ioctl_reqbufs(file, priv, reqbufs);
 	if (!ret)
 		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)
 {
@@ -966,38 +946,15 @@ static const struct v4l2_ioctl_ops fimc_lite_ioctl_ops = {
 	.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_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
+	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
 	.vidioc_streamon		= fimc_lite_streamon,
 	.vidioc_streamoff		= fimc_lite_streamoff,
 };
 
-/* Called with the media graph mutex held */
-static struct v4l2_subdev *__find_remote_sensor(struct media_entity *me)
-{
-	struct media_pad *pad = &me->pads[0];
-	struct v4l2_subdev *sd;
-
-	while (pad->flags & MEDIA_PAD_FL_SINK) {
-		/* 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 == GRP_ID_FIMC_IS_SENSOR)
-			return sd;
-		/* sink pad */
-		pad = &sd->entity.pads[0];
-	}
-	return NULL;
-}
-
 /* Capture subdev media entity operations */
 static int fimc_lite_link_setup(struct media_entity *entity,
 				const struct media_pad *local,
@@ -1072,7 +1029,7 @@ static int fimc_lite_subdev_enum_mbus_code(struct v4l2_subdev *sd,
 {
 	const struct fimc_fmt *fmt;
 
-	fmt = fimc_lite_find_format(NULL, NULL, code->index);
+	fmt = fimc_lite_find_format(NULL, NULL, 0, code->index);
 	if (!fmt)
 		return -EINVAL;
 	code->code = fmt->mbus_code;
@@ -1085,7 +1042,7 @@ static int fimc_lite_subdev_get_fmt(struct v4l2_subdev *sd,
 {
 	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
 	struct v4l2_mbus_framefmt *mf = &fmt->format;
-	struct flite_frame *f = &fimc->out_frame;
+	struct flite_frame *f = &fimc->inp_frame;
 
 	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
 		mf = v4l2_subdev_get_try_format(fh, fmt->pad);
@@ -1095,7 +1052,7 @@ static int fimc_lite_subdev_get_fmt(struct v4l2_subdev *sd,
 	mf->colorspace = V4L2_COLORSPACE_JPEG;
 
 	mutex_lock(&fimc->lock);
-	mf->code = fimc->fmt->mbus_code;
+	mf->code = f->fmt->mbus_code;
 
 	if (fmt->pad == FLITE_SD_PAD_SINK) {
 		/* full camera input frame size */
@@ -1147,7 +1104,7 @@ static int fimc_lite_subdev_set_fmt(struct v4l2_subdev *sd,
 	if (fmt->pad == FLITE_SD_PAD_SINK) {
 		sink->f_width = mf->width;
 		sink->f_height = mf->height;
-		fimc->fmt = ffmt;
+		sink->fmt = ffmt;
 		/* Set sink crop rectangle */
 		sink->rect.width = mf->width;
 		sink->rect.height = mf->height;
@@ -1159,7 +1116,7 @@ static int fimc_lite_subdev_set_fmt(struct v4l2_subdev *sd,
 		source->f_height = mf->height;
 	} else {
 		/* Allow changing format only on sink pad */
-		mf->code = fimc->fmt->mbus_code;
+		mf->code = sink->fmt->mbus_code;
 		mf->width = sink->rect.width;
 		mf->height = sink->rect.height;
 	}
@@ -1300,7 +1257,8 @@ static int fimc_lite_subdev_registered(struct v4l2_subdev *sd)
 
 	memset(vfd, 0, sizeof(*vfd));
 
-	fimc->fmt = &fimc_lite_formats[0];
+	fimc->inp_frame.fmt = &fimc_lite_formats[0];
+	fimc->out_frame.fmt = &fimc_lite_formats[0];
 	atomic_set(&fimc->out_path, FIMC_IO_DMA);
 
 	snprintf(vfd->name, sizeof(vfd->name), "fimc-lite.%d.capture",
@@ -1311,8 +1269,7 @@ static int fimc_lite_subdev_registered(struct v4l2_subdev *sd)
 	vfd->v4l2_dev = sd->v4l2_dev;
 	vfd->minor = -1;
 	vfd->release = video_device_release_empty;
-	vfd->lock = &fimc->lock;
-	fimc->ref_count = 0;
+	vfd->queue = q;
 	fimc->reqbufs_count = 0;
 
 	INIT_LIST_HEAD(&fimc->pending_buf_q);
@@ -1325,6 +1282,8 @@ static int fimc_lite_subdev_registered(struct v4l2_subdev *sd)
 	q->mem_ops = &vb2_dma_contig_memops;
 	q->buf_struct_size = sizeof(struct flite_buffer);
 	q->drv_priv = fimc;
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->lock = &fimc->lock;
 
 	ret = vb2_queue_init(q);
 	if (ret < 0)
@@ -1408,6 +1367,7 @@ static const struct v4l2_ctrl_config fimc_lite_ctrl = {
 	.id	= V4L2_CTRL_CLASS_USER | 0x1001,
 	.type	= V4L2_CTRL_TYPE_BOOLEAN,
 	.name	= "Test Pattern 640x480",
+	.step	= 1,
 };
 
 static int fimc_lite_create_capture_subdev(struct fimc_lite *fimc)
@@ -1417,7 +1377,7 @@ static int fimc_lite_create_capture_subdev(struct fimc_lite *fimc)
 	int ret;
 
 	v4l2_subdev_init(sd, &fimc_lite_subdev_ops);
-	sd->flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 	snprintf(sd->name, sizeof(sd->name), "FIMC-LITE.%d", fimc->index);
 
 	fimc->subdev_pads[FLITE_SD_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
@@ -1439,6 +1399,7 @@ static int fimc_lite_create_capture_subdev(struct fimc_lite *fimc)
 	sd->ctrl_handler = handler;
 	sd->internal_ops = &fimc_lite_subdev_internal_ops;
 	sd->entity.ops = &fimc_lite_subdev_media_ops;
+	sd->owner = THIS_MODULE;
 	v4l2_set_subdevdata(sd, fimc);
 
 	return 0;
@@ -1480,19 +1441,35 @@ static int fimc_lite_clk_get(struct fimc_lite *fimc)
 	return ret;
 }
 
+static const struct of_device_id flite_of_match[];
+
 static int fimc_lite_probe(struct platform_device *pdev)
 {
-	struct flite_drvdata *drv_data = fimc_lite_get_drvdata(pdev);
+	struct flite_drvdata *drv_data = NULL;
+	struct device *dev = &pdev->dev;
+	const struct of_device_id *of_id;
 	struct fimc_lite *fimc;
 	struct resource *res;
 	int ret;
 
-	fimc = devm_kzalloc(&pdev->dev, sizeof(*fimc), GFP_KERNEL);
+	fimc = devm_kzalloc(dev, sizeof(*fimc), GFP_KERNEL);
 	if (!fimc)
 		return -ENOMEM;
 
-	fimc->index = pdev->id;
-	fimc->variant = drv_data->variant[fimc->index];
+	if (dev->of_node) {
+		of_id = of_match_node(flite_of_match, dev->of_node);
+		if (of_id)
+			drv_data = (struct flite_drvdata *)of_id->data;
+		fimc->index = of_alias_get_id(dev->of_node, "fimc-lite");
+	} else {
+		drv_data = fimc_lite_get_drvdata(pdev);
+		fimc->index = pdev->id;
+	}
+
+	if (!drv_data || fimc->index < 0 || fimc->index >= FIMC_LITE_MAX_DEVS)
+		return -EINVAL;
+
+	fimc->dd = drv_data;
 	fimc->pdev = pdev;
 
 	init_waitqueue_head(&fimc->irq_queue);
@@ -1500,13 +1477,13 @@ static int fimc_lite_probe(struct platform_device *pdev)
 	mutex_init(&fimc->lock);
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	fimc->regs = devm_ioremap_resource(&pdev->dev, res);
+	fimc->regs = devm_ioremap_resource(dev, res);
 	if (IS_ERR(fimc->regs))
 		return PTR_ERR(fimc->regs);
 
 	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	if (res == NULL) {
-		dev_err(&pdev->dev, "Failed to get IRQ resource\n");
+		dev_err(dev, "Failed to get IRQ resource\n");
 		return -ENXIO;
 	}
 
@@ -1514,10 +1491,10 @@ static int fimc_lite_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-	ret = devm_request_irq(&pdev->dev, res->start, flite_irq_handler,
-			       0, dev_name(&pdev->dev), fimc);
+	ret = devm_request_irq(dev, res->start, flite_irq_handler,
+			       0, dev_name(dev), fimc);
 	if (ret) {
-		dev_err(&pdev->dev, "Failed to install irq (%d)\n", ret);
+		dev_err(dev, "Failed to install irq (%d)\n", ret);
 		goto err_clk;
 	}
 
@@ -1527,23 +1504,23 @@ static int fimc_lite_probe(struct platform_device *pdev)
 		goto err_clk;
 
 	platform_set_drvdata(pdev, fimc);
-	pm_runtime_enable(&pdev->dev);
-	ret = pm_runtime_get_sync(&pdev->dev);
+	pm_runtime_enable(dev);
+	ret = pm_runtime_get_sync(dev);
 	if (ret < 0)
 		goto err_sd;
 
-	fimc->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	fimc->alloc_ctx = vb2_dma_contig_init_ctx(dev);
 	if (IS_ERR(fimc->alloc_ctx)) {
 		ret = PTR_ERR(fimc->alloc_ctx);
 		goto err_pm;
 	}
-	pm_runtime_put(&pdev->dev);
+	pm_runtime_put(dev);
 
-	dev_dbg(&pdev->dev, "FIMC-LITE.%d registered successfully\n",
+	dev_dbg(dev, "FIMC-LITE.%d registered successfully\n",
 		fimc->index);
 	return 0;
 err_pm:
-	pm_runtime_put(&pdev->dev);
+	pm_runtime_put(dev);
 err_sd:
 	fimc_lite_unregister_capture_subdev(fimc);
 err_clk:
@@ -1634,7 +1611,14 @@ static int fimc_lite_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct flite_variant fimc_lite0_variant_exynos4 = {
+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)
+};
+
+/* EXYNOS4212, EXYNOS4412 */
+static struct flite_drvdata fimc_lite_drvdata_exynos4 = {
 	.max_width		= 8192,
 	.max_height		= 8192,
 	.out_width_align	= 8,
@@ -1642,14 +1626,6 @@ static struct flite_variant fimc_lite0_variant_exynos4 = {
 	.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",
@@ -1659,17 +1635,21 @@ static struct platform_device_id fimc_lite_driver_ids[] = {
 };
 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 const struct of_device_id flite_of_match[] = {
+	{
+		.compatible = "samsung,exynos4212-fimc-lite",
+		.data = &fimc_lite_drvdata_exynos4,
+	},
+	{ /* sentinel */ },
 };
+MODULE_DEVICE_TABLE(of, flite_of_match);
 
 static struct platform_driver fimc_lite_driver = {
 	.probe		= fimc_lite_probe,
 	.remove		= fimc_lite_remove,
 	.id_table	= fimc_lite_driver_ids,
 	.driver = {
+		.of_match_table = flite_of_match,
 		.name		= FIMC_LITE_DRV_NAME,
 		.owner		= THIS_MODULE,
 		.pm		= &fimc_lite_pm_ops,
diff --git a/drivers/media/platform/s5p-fimc/fimc-lite.h b/drivers/media/platform/exynos4-is/fimc-lite.h
index 7085761f8c4..47da5e04924 100644
--- a/drivers/media/platform/s5p-fimc/fimc-lite.h
+++ b/drivers/media/platform/exynos4-is/fimc-lite.h
@@ -20,12 +20,11 @@
 
 #include <media/media-entity.h>
 #include <media/videobuf2-core.h>
+#include <media/v4l2-ctrls.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
@@ -49,7 +48,7 @@ enum {
 #define FLITE_SD_PAD_SOURCE_ISP	2
 #define FLITE_SD_PADS_NUM	3
 
-struct flite_variant {
+struct flite_drvdata {
 	unsigned short max_width;
 	unsigned short max_height;
 	unsigned short out_width_align;
@@ -57,10 +56,6 @@ struct flite_variant {
 	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)
 
@@ -75,11 +70,13 @@ struct fimc_lite_events {
  * @f_width: full pixel width
  * @f_height: full pixel height
  * @rect: crop/composition rectangle
+ * @fmt: pointer to pixel format description data structure
  */
 struct flite_frame {
 	u16 f_width;
 	u16 f_height;
 	struct v4l2_rect rect;
+	const struct fimc_fmt *fmt;
 };
 
 /**
@@ -97,7 +94,7 @@ struct flite_buffer {
 /**
  * struct fimc_lite - fimc lite structure
  * @pdev: pointer to FIMC-LITE platform device
- * @variant: variant information for this IP
+ * @dd: SoC specific driver data structure
  * @v4l2_dev: pointer to top the level v4l2_device
  * @vfd: video device node
  * @fh: v4l2 file handle
@@ -116,7 +113,6 @@ struct flite_buffer {
  * @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
@@ -133,7 +129,7 @@ struct flite_buffer {
  */
 struct fimc_lite {
 	struct platform_device	*pdev;
-	struct flite_variant	*variant;
+	struct flite_drvdata	*dd;
 	struct v4l2_device	*v4l2_dev;
 	struct video_device	vfd;
 	struct v4l2_fh		fh;
@@ -144,7 +140,7 @@ struct fimc_lite {
 	struct v4l2_subdev	*sensor;
 	struct v4l2_ctrl_handler ctrl_handler;
 	struct v4l2_ctrl	*test_pattern;
-	u32			index;
+	int			index;
 	struct fimc_pipeline	pipeline;
 	const struct fimc_pipeline_ops *pipeline_ops;
 
@@ -155,7 +151,6 @@ struct fimc_lite {
 	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;
@@ -171,6 +166,7 @@ struct fimc_lite {
 	int			ref_count;
 
 	struct fimc_lite_events	events;
+	bool			streaming;
 };
 
 static inline bool fimc_lite_active(struct fimc_lite *fimc)
diff --git a/drivers/media/platform/s5p-fimc/fimc-m2m.c b/drivers/media/platform/exynos4-is/fimc-m2m.c
index f3d535cdd87..bde1f47f7ed 100644
--- a/drivers/media/platform/s5p-fimc/fimc-m2m.c
+++ b/drivers/media/platform/exynos4-is/fimc-m2m.c
@@ -29,8 +29,7 @@
 
 #include "fimc-core.h"
 #include "fimc-reg.h"
-#include "fimc-mdevice.h"
-
+#include "media-dev.h"
 
 static unsigned int get_m2m_fmt_flags(unsigned int stream_type)
 {
@@ -100,7 +99,7 @@ static int stop_streaming(struct vb2_queue *q)
 
 static void fimc_device_run(void *priv)
 {
-	struct vb2_buffer *vb = NULL;
+	struct vb2_buffer *src_vb, *dst_vb;
 	struct fimc_ctx *ctx = priv;
 	struct fimc_frame *sf, *df;
 	struct fimc_dev *fimc;
@@ -123,16 +122,18 @@ static void fimc_device_run(void *priv)
 		fimc_prepare_dma_offset(ctx, df);
 	}
 
-	vb = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
-	ret = fimc_prepare_addr(ctx, vb, sf, &sf->paddr);
+	src_vb = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
+	ret = fimc_prepare_addr(ctx, src_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);
+	dst_vb = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
+	ret = fimc_prepare_addr(ctx, dst_vb, df, &df->paddr);
 	if (ret)
 		goto dma_unlock;
 
+	dst_vb->v4l2_buf.timestamp = src_vb->v4l2_buf.timestamp;
+
 	/* Reconfigure hardware if the context has changed. */
 	if (fimc->m2m.ctx != ctx) {
 		ctx->state |= FIMC_PARAMS;
@@ -152,7 +153,7 @@ static void fimc_device_run(void *priv)
 		fimc_hw_set_rotation(ctx);
 		fimc_hw_set_effect(ctx);
 		fimc_hw_set_out_dma(ctx);
-		if (fimc->variant->has_alpha)
+		if (fimc->drv_data->alpha_color)
 			fimc_hw_set_rgb_alpha(ctx);
 		fimc_hw_set_output_path(ctx);
 	}
@@ -250,22 +251,20 @@ static struct vb2_ops fimc_qops = {
  * V4L2 ioctl handlers
  */
 static int fimc_m2m_querycap(struct file *file, void *fh,
-			     struct v4l2_capability *cap)
+				     struct v4l2_capability *cap)
 {
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-	struct fimc_dev *fimc = ctx->fimc_dev;
+	struct fimc_dev *fimc = video_drvdata(file);
+	unsigned int caps;
 
-	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;
 	/*
 	 * This is only a mem-to-mem video device. The capture and output
 	 * device capability flags are left only for backward compatibility
 	 * and are scheduled for removal.
 	 */
-	cap->capabilities = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M_MPLANE |
+	caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M_MPLANE |
 		V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_VIDEO_OUTPUT_MPLANE;
 
+	__fimc_vidioc_querycap(&fimc->pdev->dev, cap, caps);
 	return 0;
 }
 
@@ -623,6 +622,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	src_vq->ops = &fimc_qops;
 	src_vq->mem_ops = &vb2_dma_contig_memops;
 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	src_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	ret = vb2_queue_init(src_vq);
 	if (ret)
@@ -634,6 +634,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	dst_vq->ops = &fimc_qops;
 	dst_vq->mem_ops = &vb2_dma_contig_memops;
 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	dst_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	return vb2_queue_init(dst_vq);
 }
@@ -667,16 +668,15 @@ static int fimc_m2m_open(struct file *file)
 	struct fimc_ctx *ctx;
 	int ret = -EBUSY;
 
-	dbg("pid: %d, state: 0x%lx, refcnt: %d",
-	    task_pid_nr(current), fimc->state, fimc->vid_cap.refcnt);
+	pr_debug("pid: %d, state: %#lx\n", task_pid_nr(current), fimc->state);
 
 	if (mutex_lock_interruptible(&fimc->lock))
 		return -ERESTARTSYS;
 	/*
-	 * Return if the corresponding video capture node
-	 * is already opened.
+	 * Don't allow simultaneous open() of the mem-to-mem and the
+	 * capture video node that belong to same FIMC IP instance.
 	 */
-	if (fimc->vid_cap.refcnt > 0)
+	if (test_bit(ST_CAPT_BUSY, &fimc->state))
 		goto unlock;
 
 	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
diff --git a/drivers/media/platform/s5p-fimc/fimc-reg.c b/drivers/media/platform/exynos4-is/fimc-reg.c
index 50b97c75b95..f079f36099d 100644
--- a/drivers/media/platform/s5p-fimc/fimc-reg.c
+++ b/drivers/media/platform/exynos4-is/fimc-reg.c
@@ -1,22 +1,24 @@
 /*
  * Register interface file for Samsung Camera Interface (FIMC) driver
  *
- * Copyright (C) 2010 - 2012 Samsung Electronics Co., Ltd.
- * Sylwester Nawrocki, <s.nawrocki@samsung.com>
+ * Copyright (C) 2010 - 2013 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 <linux/io.h>
+#include <linux/regmap.h>
+
 #include <media/s5p_fimc.h>
+#include "media-dev.h"
 
 #include "fimc-reg.h"
 #include "fimc-core.h"
 
-
 void fimc_hw_reset(struct fimc_dev *dev)
 {
 	u32 cfg;
@@ -35,7 +37,7 @@ void fimc_hw_reset(struct fimc_dev *dev)
 	cfg &= ~FIMC_REG_CIGCTRL_SWRST;
 	writel(cfg, dev->regs + FIMC_REG_CIGCTRL);
 
-	if (dev->variant->out_buf_count > 4)
+	if (dev->drv_data->out_buf_count > 4)
 		fimc_hw_set_dma_seq(dev, 0xF);
 }
 
@@ -447,7 +449,8 @@ void fimc_hw_set_in_dma(struct fimc_ctx *ctx)
 		 | 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);
+		 | FIMC_REG_MSCTRL_2P_IN_ORDER_MASK
+		 | FIMC_REG_MSCTRL_ORDER422_MASK);
 
 	cfg |= (FIMC_REG_MSCTRL_IN_BURST_COUNT(4)
 		| FIMC_REG_MSCTRL_INPUT_MEMORY
@@ -598,7 +601,8 @@ static const struct mbus_pixfmt_desc pix_desc[] = {
 int fimc_hw_set_camera_source(struct fimc_dev *fimc,
 			      struct fimc_source_info *source)
 {
-	struct fimc_frame *f = &fimc->vid_cap.ctx->s_frame;
+	struct fimc_vid_cap *vc = &fimc->vid_cap;
+	struct fimc_frame *f = &vc->ctx->s_frame;
 	u32 bus_width, cfg = 0;
 	int i;
 
@@ -606,7 +610,7 @@ int fimc_hw_set_camera_source(struct fimc_dev *fimc,
 	case FIMC_BUS_TYPE_ITU_601:
 	case FIMC_BUS_TYPE_ITU_656:
 		for (i = 0; i < ARRAY_SIZE(pix_desc); i++) {
-			if (fimc->vid_cap.mf.code == pix_desc[i].pixelcode) {
+			if (vc->ci_fmt.code == pix_desc[i].pixelcode) {
 				cfg = pix_desc[i].cisrcfmt;
 				bus_width = pix_desc[i].bus_width;
 				break;
@@ -614,9 +618,9 @@ int fimc_hw_set_camera_source(struct fimc_dev *fimc,
 		}
 
 		if (i == ARRAY_SIZE(pix_desc)) {
-			v4l2_err(&fimc->vid_cap.vfd,
+			v4l2_err(&vc->vfd,
 				 "Camera color format not supported: %d\n",
-				 fimc->vid_cap.mf.code);
+				 vc->ci_fmt.code);
 			return -EINVAL;
 		}
 
@@ -631,6 +635,10 @@ int fimc_hw_set_camera_source(struct fimc_dev *fimc,
 		if (fimc_fmt_is_user_defined(f->fmt->color))
 			cfg |= FIMC_REG_CISRCFMT_ITU601_8BIT;
 		break;
+	default:
+	case FIMC_BUS_TYPE_ISP_WRITEBACK:
+		/* Anything to do here ? */
+		break;
 	}
 
 	cfg |= (f->o_width << 16) | f->o_height;
@@ -660,16 +668,17 @@ void fimc_hw_set_camera_offset(struct fimc_dev *fimc, struct fimc_frame *f)
 int fimc_hw_set_camera_type(struct fimc_dev *fimc,
 			    struct fimc_source_info *source)
 {
-	u32 cfg, tmp;
 	struct fimc_vid_cap *vid_cap = &fimc->vid_cap;
 	u32 csis_data_alignment = 32;
+	u32 cfg, tmp;
 
 	cfg = readl(fimc->regs + FIMC_REG_CIGCTRL);
 
 	/* Select ITU B interface, disable Writeback path and test pattern. */
 	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);
+		FIMC_REG_CIGCTRL_SELCAM_MIPI_A | FIMC_REG_CIGCTRL_CAM_JPEG |
+		FIMC_REG_CIGCTRL_SELWB_A);
 
 	switch (source->fimc_bus_type) {
 	case FIMC_BUS_TYPE_MIPI_CSI2:
@@ -679,7 +688,7 @@ int fimc_hw_set_camera_type(struct fimc_dev *fimc,
 			cfg |= FIMC_REG_CIGCTRL_SELCAM_MIPI_A;
 
 		/* TODO: add remaining supported formats. */
-		switch (vid_cap->mf.code) {
+		switch (vid_cap->ci_fmt.code) {
 		case V4L2_MBUS_FMT_VYUY8_2X8:
 			tmp = FIMC_REG_CSIIMGFMT_YCBCR422_8BIT;
 			break;
@@ -691,7 +700,7 @@ int fimc_hw_set_camera_type(struct fimc_dev *fimc,
 		default:
 			v4l2_err(&vid_cap->vfd,
 				 "Not supported camera pixel format: %#x\n",
-				 vid_cap->mf.code);
+				 vid_cap->ci_fmt.code);
 			return -EINVAL;
 		}
 		tmp |= (csis_data_alignment == 32) << 8;
@@ -704,6 +713,12 @@ int fimc_hw_set_camera_type(struct fimc_dev *fimc,
 		break;
 	case FIMC_BUS_TYPE_LCD_WRITEBACK_A:
 		cfg |= FIMC_REG_CIGCTRL_CAMIF_SELWB;
+		/* fall through */
+	case FIMC_BUS_TYPE_ISP_WRITEBACK:
+		if (fimc->variant->has_isp_wb)
+			cfg |= FIMC_REG_CIGCTRL_CAMIF_SELWB;
+		else
+			WARN_ONCE(1, "ISP Writeback input is not supported\n");
 		break;
 	default:
 		v4l2_err(&vid_cap->vfd, "Invalid FIMC bus type selected: %d\n",
@@ -747,7 +762,7 @@ s32 fimc_hw_get_frame_index(struct fimc_dev *dev)
 {
 	s32 reg;
 
-	if (dev->variant->has_cistatus2) {
+	if (dev->drv_data->cistatus2) {
 		reg = readl(dev->regs + FIMC_REG_CISTATUS2) & 0x3f;
 		return reg - 1;
 	}
@@ -763,7 +778,7 @@ s32 fimc_hw_get_prev_frame_index(struct fimc_dev *dev)
 {
 	s32 reg;
 
-	if (!dev->variant->has_cistatus2)
+	if (!dev->drv_data->cistatus2)
 		return -1;
 
 	reg = readl(dev->regs + FIMC_REG_CISTATUS2);
@@ -784,3 +799,43 @@ void fimc_deactivate_capture(struct fimc_dev *fimc)
 	fimc_hw_enable_scaler(fimc, false);
 	fimc_hw_en_lastirq(fimc, false);
 }
+
+int fimc_hw_camblk_cfg_writeback(struct fimc_dev *fimc)
+{
+	struct regmap *map = fimc->sysreg;
+	unsigned int mask, val, camblk_cfg;
+	int ret;
+
+	if (map == NULL)
+		return 0;
+
+	ret = regmap_read(map, SYSREG_CAMBLK, &camblk_cfg);
+	if (ret < 0 || ((camblk_cfg & 0x00700000) >> 20 != 0x3))
+		return ret;
+
+	if (!WARN(fimc->id >= 3, "not supported id: %d\n", fimc->id))
+		val = 0x1 << (fimc->id + 20);
+	else
+		val = 0;
+
+	mask = SYSREG_CAMBLK_FIFORST_ISP | SYSREG_CAMBLK_ISPWB_FULL_EN;
+	ret = regmap_update_bits(map, SYSREG_CAMBLK, mask, val);
+	if (ret < 0)
+		return ret;
+
+	usleep_range(1000, 2000);
+
+	val |= SYSREG_CAMBLK_FIFORST_ISP;
+	ret = regmap_update_bits(map, SYSREG_CAMBLK, mask, val);
+	if (ret < 0)
+		return ret;
+
+	mask = SYSREG_ISPBLK_FIFORST_CAM_BLK;
+	ret = regmap_update_bits(map, SYSREG_ISPBLK, mask, ~mask);
+	if (ret < 0)
+		return ret;
+
+	usleep_range(1000, 2000);
+
+	return regmap_update_bits(map, SYSREG_ISPBLK, mask, mask);
+}
diff --git a/drivers/media/platform/s5p-fimc/fimc-reg.h b/drivers/media/platform/exynos4-is/fimc-reg.h
index 1a40df6d1a8..6c97798c75a 100644
--- a/drivers/media/platform/s5p-fimc/fimc-reg.h
+++ b/drivers/media/platform/exynos4-is/fimc-reg.h
@@ -52,6 +52,8 @@
 #define FIMC_REG_CIGCTRL_IRQ_CLR		(1 << 19)
 #define FIMC_REG_CIGCTRL_IRQ_ENABLE		(1 << 16)
 #define FIMC_REG_CIGCTRL_SHDW_DISABLE		(1 << 12)
+/* 0 - selects Writeback A (LCD), 1 - selects Writeback B (LCD/ISP) */
+#define FIMC_REG_CIGCTRL_SELWB_A		(1 << 10)
 #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)
@@ -93,10 +95,10 @@
 /* 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_ORDER422_YCBYCR	(0 << 0)
+#define FIMC_REG_CIOCTRL_ORDER422_YCRYCB	(1 << 0)
+#define FIMC_REG_CIOCTRL_ORDER422_CBYCRY	(2 << 0)
+#define FIMC_REG_CIOCTRL_ORDER422_CRYCBY	(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)
@@ -218,10 +220,10 @@
 #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_CRYCBY		(0 << 4)
+#define FIMC_REG_MSCTRL_ORDER422_YCRYCB		(1 << 4)
+#define FIMC_REG_MSCTRL_ORDER422_CBYCRY		(2 << 4)
+#define FIMC_REG_MSCTRL_ORDER422_YCBYCR		(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)
@@ -276,6 +278,14 @@
 /* Output frame buffer sequence mask */
 #define FIMC_REG_CIFCNTSEQ			0x1fc
 
+/* SYSREG ISP Writeback register address offsets */
+#define SYSREG_ISPBLK				0x020c
+#define SYSREG_ISPBLK_FIFORST_CAM_BLK		(1 << 7)
+
+#define SYSREG_CAMBLK				0x0218
+#define SYSREG_CAMBLK_FIFORST_ISP		(1 << 15)
+#define SYSREG_CAMBLK_ISPWB_FULL_EN		(7 << 20)
+
 /*
  * Function declarations
  */
@@ -309,6 +319,7 @@ void fimc_hw_activate_input_dma(struct fimc_dev *dev, bool on);
 void fimc_hw_disable_capture(struct fimc_dev *dev);
 s32 fimc_hw_get_frame_index(struct fimc_dev *dev);
 s32 fimc_hw_get_prev_frame_index(struct fimc_dev *dev);
+int fimc_hw_camblk_cfg_writeback(struct fimc_dev *fimc);
 void fimc_activate_capture(struct fimc_ctx *ctx);
 void fimc_deactivate_capture(struct fimc_dev *fimc);
 
diff --git a/drivers/media/platform/s5p-fimc/fimc-mdevice.c b/drivers/media/platform/exynos4-is/media-dev.c
index a17fcb2d5d4..15ef8f28239 100644
--- a/drivers/media/platform/s5p-fimc/fimc-mdevice.c
+++ b/drivers/media/platform/exynos4-is/media-dev.c
@@ -17,32 +17,37 @@
 #include <linux/kernel.h>
 #include <linux/list.h>
 #include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/of_device.h>
+#include <linux/of_i2c.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/types.h>
 #include <linux/slab.h>
 #include <media/v4l2-ctrls.h>
+#include <media/v4l2-of.h>
 #include <media/media-device.h>
 #include <media/s5p_fimc.h>
 
+#include "media-dev.h"
 #include "fimc-core.h"
+#include "fimc-is.h"
 #include "fimc-lite.h"
-#include "fimc-mdevice.h"
 #include "mipi-csis.h"
 
 static int __fimc_md_set_camclk(struct fimc_md *fmd,
-				struct fimc_sensor_info *s_info,
+				struct fimc_source_info *si,
 				bool on);
 /**
  * fimc_pipeline_prepare - update pipeline information with subdevice pointers
- * @fimc: fimc device terminating the pipeline
+ * @me: media entity terminating the pipeline
  *
  * Caller holds the graph mutex.
  */
 static void fimc_pipeline_prepare(struct fimc_pipeline *p,
 				  struct media_entity *me)
 {
-	struct media_pad *pad = &me->pads[0];
 	struct v4l2_subdev *sd;
 	int i;
 
@@ -50,15 +55,21 @@ static void fimc_pipeline_prepare(struct fimc_pipeline *p,
 		p->subdevs[i] = NULL;
 
 	while (1) {
-		if (!(pad->flags & MEDIA_PAD_FL_SINK))
-			break;
+		struct media_pad *pad = NULL;
+
+		/* Find remote source pad */
+		for (i = 0; i < me->num_pads; i++) {
+			struct media_pad *spad = &me->pads[i];
+			if (!(spad->flags & MEDIA_PAD_FL_SINK))
+				continue;
+			pad = media_entity_remote_source(spad);
+			if (pad)
+				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);
 
 		switch (sd->grp_id) {
@@ -75,12 +86,15 @@ static void fimc_pipeline_prepare(struct fimc_pipeline *p,
 		case GRP_ID_FIMC:
 			/* No need to control FIMC subdev through subdev ops */
 			break;
+		case GRP_ID_FIMC_IS:
+			p->subdevs[IDX_IS_ISP] = sd;
+			break;
 		default:
-			pr_warn("%s: Unknown subdev grp_id: %#x\n",
-				__func__, sd->grp_id);
+			break;
 		}
-		/* sink pad */
-		pad = &sd->entity.pads[0];
+		me = &sd->entity;
+		if (me->num_pads == 1)
+			break;
 	}
 }
 
@@ -117,49 +131,81 @@ static int __subdev_set_power(struct v4l2_subdev *sd, int on)
  *
  * Needs to be called with the graph mutex held.
  */
-static int fimc_pipeline_s_power(struct fimc_pipeline *p, bool state)
+static int fimc_pipeline_s_power(struct fimc_pipeline *p, bool on)
 {
-	unsigned int i;
-	int ret;
+	static const u8 seq[2][IDX_MAX - 1] = {
+		{ IDX_IS_ISP, IDX_SENSOR, IDX_CSIS, IDX_FLITE },
+		{ IDX_CSIS, IDX_FLITE, IDX_SENSOR, IDX_IS_ISP },
+	};
+	int i, ret = 0;
 
 	if (p->subdevs[IDX_SENSOR] == NULL)
 		return -ENXIO;
 
-	for (i = 0; i < IDX_MAX; i++) {
-		unsigned int idx = state ? (IDX_MAX - 1) - i : i;
+	for (i = 0; i < IDX_MAX - 1; i++) {
+		unsigned int idx = seq[on][i];
+
+		ret = __subdev_set_power(p->subdevs[idx], on);
+
 
-		ret = __subdev_set_power(p->subdevs[idx], state);
 		if (ret < 0 && ret != -ENXIO)
-			return ret;
+			goto error;
 	}
-
 	return 0;
+error:
+	for (; i >= 0; i--) {
+		unsigned int idx = seq[on][i];
+		__subdev_set_power(p->subdevs[idx], !on);
+	}
+	return ret;
 }
 
 /**
  * __fimc_pipeline_open - update the pipeline information, enable power
  *                        of all pipeline subdevs and the sensor clock
  * @me: media entity to start graph walk with
- * @prep: true to acquire sensor (and csis) subdevs
+ * @prepare: true to walk the current pipeline and acquire all subdevs
  *
  * Called with the graph mutex held.
  */
 static int __fimc_pipeline_open(struct fimc_pipeline *p,
-				struct media_entity *me, bool prep)
+				struct media_entity *me, bool prepare)
 {
+	struct fimc_md *fmd = entity_to_fimc_mdev(me);
+	struct v4l2_subdev *sd;
 	int ret;
 
-	if (prep)
+	if (WARN_ON(p == NULL || me == NULL))
+		return -EINVAL;
+
+	if (prepare)
 		fimc_pipeline_prepare(p, me);
 
-	if (p->subdevs[IDX_SENSOR] == NULL)
+	sd = p->subdevs[IDX_SENSOR];
+	if (sd == NULL)
 		return -EINVAL;
 
-	ret = fimc_md_set_camclk(p->subdevs[IDX_SENSOR], true);
-	if (ret)
-		return ret;
+	/* Disable PXLASYNC clock if this pipeline includes FIMC-IS */
+	if (!IS_ERR(fmd->wbclk[CLK_IDX_WB_B]) && p->subdevs[IDX_IS_ISP]) {
+		ret = clk_prepare_enable(fmd->wbclk[CLK_IDX_WB_B]);
+		if (ret < 0)
+			return ret;
+	}
+	ret = fimc_md_set_camclk(sd, true);
+	if (ret < 0)
+		goto err_wbclk;
+
+	ret = fimc_pipeline_s_power(p, 1);
+	if (!ret)
+		return 0;
 
-	return fimc_pipeline_s_power(p, 1);
+	fimc_md_set_camclk(sd, false);
+
+err_wbclk:
+	if (!IS_ERR(fmd->wbclk[CLK_IDX_WB_B]) && p->subdevs[IDX_IS_ISP])
+		clk_disable_unprepare(fmd->wbclk[CLK_IDX_WB_B]);
+
+	return ret;
 }
 
 /**
@@ -170,41 +216,58 @@ static int __fimc_pipeline_open(struct fimc_pipeline *p,
  */
 static int __fimc_pipeline_close(struct fimc_pipeline *p)
 {
+	struct v4l2_subdev *sd = p ? p->subdevs[IDX_SENSOR] : NULL;
+	struct fimc_md *fmd;
 	int ret = 0;
 
-	if (!p || !p->subdevs[IDX_SENSOR])
+	if (WARN_ON(sd == NULL))
 		return -EINVAL;
 
 	if (p->subdevs[IDX_SENSOR]) {
 		ret = fimc_pipeline_s_power(p, 0);
-		fimc_md_set_camclk(p->subdevs[IDX_SENSOR], false);
+		fimc_md_set_camclk(sd, false);
 	}
+
+	fmd = entity_to_fimc_mdev(&sd->entity);
+
+	/* Disable PXLASYNC clock if this pipeline includes FIMC-IS */
+	if (!IS_ERR(fmd->wbclk[CLK_IDX_WB_B]) && p->subdevs[IDX_IS_ISP])
+		clk_disable_unprepare(fmd->wbclk[CLK_IDX_WB_B]);
+
 	return ret == -ENXIO ? 0 : ret;
 }
 
 /**
- * __fimc_pipeline_s_stream - invoke s_stream on pipeline subdevs
+ * __fimc_pipeline_s_stream - call s_stream() on pipeline subdevs
  * @pipeline: video pipeline structure
- * @on: passed as the s_stream call argument
+ * @on: passed as the s_stream() callback argument
  */
 static int __fimc_pipeline_s_stream(struct fimc_pipeline *p, bool on)
 {
-	int i, ret;
+	static const u8 seq[2][IDX_MAX] = {
+		{ IDX_FIMC, IDX_SENSOR, IDX_IS_ISP, IDX_CSIS, IDX_FLITE },
+		{ IDX_CSIS, IDX_FLITE, IDX_FIMC, IDX_SENSOR, IDX_IS_ISP },
+	};
+	int i, ret = 0;
 
 	if (p->subdevs[IDX_SENSOR] == NULL)
 		return -ENODEV;
 
 	for (i = 0; i < IDX_MAX; i++) {
-		unsigned int idx = on ? (IDX_MAX - 1) - i : i;
+		unsigned int idx = seq[on][i];
 
 		ret = v4l2_subdev_call(p->subdevs[idx], video, s_stream, on);
 
 		if (ret < 0 && ret != -ENOIOCTLCMD && ret != -ENODEV)
-			return ret;
+			goto error;
 	}
-
 	return 0;
-
+error:
+	for (; i >= 0; i--) {
+		unsigned int idx = seq[on][i];
+		v4l2_subdev_call(p->subdevs[idx], video, s_stream, !on);
+	}
+	return ret;
 }
 
 /* Media pipeline operations for the FIMC/FIMC-LITE video device driver */
@@ -218,35 +281,40 @@ static const struct fimc_pipeline_ops fimc_pipeline_ops = {
  * Sensor subdevice helper functions
  */
 static struct v4l2_subdev *fimc_md_register_sensor(struct fimc_md *fmd,
-				   struct fimc_sensor_info *s_info)
+						struct fimc_source_info *si)
 {
 	struct i2c_adapter *adapter;
 	struct v4l2_subdev *sd = NULL;
 
-	if (!s_info || !fmd)
+	if (!si || !fmd)
 		return NULL;
+	/*
+	 * If FIMC bus type is not Writeback FIFO assume it is same
+	 * as sensor_bus_type.
+	 */
+	si->fimc_bus_type = si->sensor_bus_type;
 
-	adapter = i2c_get_adapter(s_info->pdata.i2c_bus_num);
+	adapter = i2c_get_adapter(si->i2c_bus_num);
 	if (!adapter) {
 		v4l2_warn(&fmd->v4l2_dev,
 			  "Failed to get I2C adapter %d, deferring probe\n",
-			  s_info->pdata.i2c_bus_num);
+			  si->i2c_bus_num);
 		return ERR_PTR(-EPROBE_DEFER);
 	}
 	sd = v4l2_i2c_new_subdev_board(&fmd->v4l2_dev, adapter,
-				       s_info->pdata.board_info, NULL);
+						si->board_info, NULL);
 	if (IS_ERR_OR_NULL(sd)) {
 		i2c_put_adapter(adapter);
 		v4l2_warn(&fmd->v4l2_dev,
 			  "Failed to acquire subdev %s, deferring probe\n",
-			  s_info->pdata.board_info->type);
+			  si->board_info->type);
 		return ERR_PTR(-EPROBE_DEFER);
 	}
-	v4l2_set_subdev_hostdata(sd, s_info);
+	v4l2_set_subdev_hostdata(sd, si);
 	sd->grp_id = GRP_ID_SENSOR;
 
 	v4l2_info(&fmd->v4l2_dev, "Registered sensor subdevice %s\n",
-		  s_info->pdata.board_info->type);
+		  sd->name);
 	return sd;
 }
 
@@ -257,57 +325,262 @@ static void fimc_md_unregister_sensor(struct v4l2_subdev *sd)
 
 	if (!client)
 		return;
+
 	v4l2_device_unregister_subdev(sd);
-	adapter = client->adapter;
-	i2c_unregister_device(client);
-	if (adapter)
-		i2c_put_adapter(adapter);
+
+	if (!client->dev.of_node) {
+		adapter = client->adapter;
+		i2c_unregister_device(client);
+		if (adapter)
+			i2c_put_adapter(adapter);
+	}
 }
 
+#ifdef CONFIG_OF
+/* Register I2C client subdev associated with @node. */
+static int fimc_md_of_add_sensor(struct fimc_md *fmd,
+				 struct device_node *node, int index)
+{
+	struct fimc_sensor_info *si;
+	struct i2c_client *client;
+	struct v4l2_subdev *sd;
+	int ret;
+
+	if (WARN_ON(index >= ARRAY_SIZE(fmd->sensor)))
+		return -EINVAL;
+	si = &fmd->sensor[index];
+
+	client = of_find_i2c_device_by_node(node);
+	if (!client)
+		return -EPROBE_DEFER;
+
+	device_lock(&client->dev);
+
+	if (!client->driver ||
+	    !try_module_get(client->driver->driver.owner)) {
+		ret = -EPROBE_DEFER;
+		v4l2_info(&fmd->v4l2_dev, "No driver found for %s\n",
+						node->full_name);
+		goto dev_put;
+	}
+
+	/* Enable sensor's master clock */
+	ret = __fimc_md_set_camclk(fmd, &si->pdata, true);
+	if (ret < 0)
+		goto mod_put;
+	sd = i2c_get_clientdata(client);
+
+	ret = v4l2_device_register_subdev(&fmd->v4l2_dev, sd);
+	__fimc_md_set_camclk(fmd, &si->pdata, false);
+	if (ret < 0)
+		goto mod_put;
+
+	v4l2_set_subdev_hostdata(sd, &si->pdata);
+	if (si->pdata.fimc_bus_type == FIMC_BUS_TYPE_ISP_WRITEBACK)
+		sd->grp_id = GRP_ID_FIMC_IS_SENSOR;
+	else
+		sd->grp_id = GRP_ID_SENSOR;
+
+	si->subdev = sd;
+	v4l2_info(&fmd->v4l2_dev, "Registered sensor subdevice: %s (%d)\n",
+		  sd->name, fmd->num_sensors);
+	fmd->num_sensors++;
+
+mod_put:
+	module_put(client->driver->driver.owner);
+dev_put:
+	device_unlock(&client->dev);
+	put_device(&client->dev);
+	return ret;
+}
+
+/* Parse port node and register as a sub-device any sensor specified there. */
+static int fimc_md_parse_port_node(struct fimc_md *fmd,
+				   struct device_node *port,
+				   unsigned int index)
+{
+	struct device_node *rem, *ep, *np;
+	struct fimc_source_info *pd;
+	struct v4l2_of_endpoint endpoint;
+	int ret;
+	u32 val;
+
+	pd = &fmd->sensor[index].pdata;
+
+	/* Assume here a port node can have only one endpoint node. */
+	ep = of_get_next_child(port, NULL);
+	if (!ep)
+		return 0;
+
+	v4l2_of_parse_endpoint(ep, &endpoint);
+	if (WARN_ON(endpoint.port == 0) || index >= FIMC_MAX_SENSORS)
+		return -EINVAL;
+
+	pd->mux_id = (endpoint.port - 1) & 0x1;
+
+	rem = v4l2_of_get_remote_port_parent(ep);
+	of_node_put(ep);
+	if (rem == NULL) {
+		v4l2_info(&fmd->v4l2_dev, "Remote device at %s not found\n",
+							ep->full_name);
+		return 0;
+	}
+	if (!of_property_read_u32(rem, "samsung,camclk-out", &val))
+		pd->clk_id = val;
+
+	if (!of_property_read_u32(rem, "clock-frequency", &val))
+		pd->clk_frequency = val;
+
+	if (pd->clk_frequency == 0) {
+		v4l2_err(&fmd->v4l2_dev, "Wrong clock frequency at node %s\n",
+			 rem->full_name);
+		of_node_put(rem);
+		return -EINVAL;
+	}
+
+	if (fimc_input_is_parallel(endpoint.port)) {
+		if (endpoint.bus_type == V4L2_MBUS_PARALLEL)
+			pd->sensor_bus_type = FIMC_BUS_TYPE_ITU_601;
+		else
+			pd->sensor_bus_type = FIMC_BUS_TYPE_ITU_656;
+		pd->flags = endpoint.bus.parallel.flags;
+	} else if (fimc_input_is_mipi_csi(endpoint.port)) {
+		/*
+		 * MIPI CSI-2: only input mux selection and
+		 * the sensor's clock frequency is needed.
+		 */
+		pd->sensor_bus_type = FIMC_BUS_TYPE_MIPI_CSI2;
+	} else {
+		v4l2_err(&fmd->v4l2_dev, "Wrong port id (%u) at node %s\n",
+			 endpoint.port, rem->full_name);
+	}
+	/*
+	 * For FIMC-IS handled sensors, that are placed under i2c-isp device
+	 * node, FIMC is connected to the FIMC-IS through its ISP Writeback
+	 * input. Sensors are attached to the FIMC-LITE hostdata interface
+	 * directly or through MIPI-CSIS, depending on the external media bus
+	 * used. This needs to be handled in a more reliable way, not by just
+	 * checking parent's node name.
+	 */
+	np = of_get_parent(rem);
+
+	if (np && !of_node_cmp(np->name, "i2c-isp"))
+		pd->fimc_bus_type = FIMC_BUS_TYPE_ISP_WRITEBACK;
+	else
+		pd->fimc_bus_type = pd->sensor_bus_type;
+
+	ret = fimc_md_of_add_sensor(fmd, rem, index);
+	of_node_put(rem);
+
+	return ret;
+}
+
+/* Register all SoC external sub-devices */
+static int fimc_md_of_sensors_register(struct fimc_md *fmd,
+				       struct device_node *np)
+{
+	struct device_node *parent = fmd->pdev->dev.of_node;
+	struct device_node *node, *ports;
+	int index = 0;
+	int ret;
+
+	/* Attach sensors linked to MIPI CSI-2 receivers */
+	for_each_available_child_of_node(parent, node) {
+		struct device_node *port;
+
+		if (of_node_cmp(node->name, "csis"))
+			continue;
+		/* The csis node can have only port subnode. */
+		port = of_get_next_child(node, NULL);
+		if (!port)
+			continue;
+
+		ret = fimc_md_parse_port_node(fmd, port, index);
+		if (ret < 0)
+			return ret;
+		index++;
+	}
+
+	/* Attach sensors listed in the parallel-ports node */
+	ports = of_get_child_by_name(parent, "parallel-ports");
+	if (!ports)
+		return 0;
+
+	for_each_child_of_node(ports, node) {
+		ret = fimc_md_parse_port_node(fmd, node, index);
+		if (ret < 0)
+			break;
+		index++;
+	}
+
+	return 0;
+}
+
+static int __of_get_csis_id(struct device_node *np)
+{
+	u32 reg = 0;
+
+	np = of_get_child_by_name(np, "port");
+	if (!np)
+		return -EINVAL;
+	of_property_read_u32(np, "reg", &reg);
+	return reg - FIMC_INPUT_MIPI_CSI2_0;
+}
+#else
+#define fimc_md_of_sensors_register(fmd, np) (-ENOSYS)
+#define __of_get_csis_id(np) (-ENOSYS)
+#endif
+
 static int fimc_md_register_sensor_entities(struct fimc_md *fmd)
 {
 	struct s5p_platform_fimc *pdata = fmd->pdev->dev.platform_data;
-	struct fimc_dev *fd = NULL;
-	int num_clients, ret, i;
+	struct device_node *of_node = fmd->pdev->dev.of_node;
+	int num_clients = 0;
+	int ret, i;
 
 	/*
 	 * Runtime resume one of the FIMC entities to make sure
 	 * the sclk_cam clocks are not globally disabled.
 	 */
-	for (i = 0; !fd && i < ARRAY_SIZE(fmd->fimc); i++)
-		if (fmd->fimc[i])
-			fd = fmd->fimc[i];
-	if (!fd)
+	if (!fmd->pmf)
 		return -ENXIO;
-	ret = pm_runtime_get_sync(&fd->pdev->dev);
+
+	ret = pm_runtime_get_sync(fmd->pmf);
 	if (ret < 0)
 		return ret;
 
-	WARN_ON(pdata->num_clients > ARRAY_SIZE(fmd->sensor));
-	num_clients = min_t(u32, pdata->num_clients, ARRAY_SIZE(fmd->sensor));
-
-	fmd->num_sensors = num_clients;
-	for (i = 0; i < num_clients; i++) {
-		struct v4l2_subdev *sd;
-
-		fmd->sensor[i].pdata = pdata->source_info[i];
-		ret = __fimc_md_set_camclk(fmd, &fmd->sensor[i], true);
-		if (ret)
-			break;
-		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 (of_node) {
+		fmd->num_sensors = 0;
+		ret = fimc_md_of_sensors_register(fmd, of_node);
+	} else if (pdata) {
+		WARN_ON(pdata->num_clients > ARRAY_SIZE(fmd->sensor));
+		num_clients = min_t(u32, pdata->num_clients,
+				    ARRAY_SIZE(fmd->sensor));
+		fmd->num_sensors = num_clients;
+
+		for (i = 0; i < num_clients; i++) {
+			struct fimc_sensor_info *si = &fmd->sensor[i];
+			struct v4l2_subdev *sd;
+
+			si->pdata = pdata->source_info[i];
+			ret = __fimc_md_set_camclk(fmd, &si->pdata, true);
+			if (ret)
+				break;
+			sd = fimc_md_register_sensor(fmd, &si->pdata);
+			ret = __fimc_md_set_camclk(fmd, &si->pdata, false);
+
+			if (IS_ERR(sd)) {
+				si->subdev = NULL;
+				ret = PTR_ERR(sd);
+				break;
+			}
+			si->subdev = sd;
+			if (ret)
+				break;
 		}
-		if (ret)
-			break;
 	}
-	pm_runtime_put(&fd->pdev->dev);
+
+	pm_runtime_put(fmd->pmf);
 	return ret;
 }
 
@@ -352,6 +625,8 @@ static int register_fimc_entity(struct fimc_md *fmd, struct fimc_dev *fimc)
 
 	ret = v4l2_device_register_subdev(&fmd->v4l2_dev, sd);
 	if (!ret) {
+		if (!fmd->pmf && fimc->pdev)
+			fmd->pmf = &fimc->pdev->dev;
 		fmd->fimc[fimc->id] = fimc;
 		fimc->vid_cap.user_subdev_api = fmd->user_subdev_api;
 	} else {
@@ -368,13 +643,13 @@ static int register_csis_entity(struct fimc_md *fmd,
 	struct device_node *node = pdev->dev.of_node;
 	int id, ret;
 
-	id = node ? of_alias_get_id(node, "csis") : max(0, pdev->id);
+	id = node ? __of_get_csis_id(node) : max(0, pdev->id);
 
-	if (WARN_ON(id >= CSIS_MAX_ENTITIES || fmd->csis[id].sd))
-		return -EBUSY;
+	if (WARN_ON(id < 0 || id >= CSIS_MAX_ENTITIES))
+		return -ENOENT;
 
-	if (WARN_ON(id >= CSIS_MAX_ENTITIES))
-		return 0;
+	if (WARN_ON(fmd->csis[id].sd))
+		return -EBUSY;
 
 	sd->grp_id = GRP_ID_CSIS;
 	ret = v4l2_device_register_subdev(&fmd->v4l2_dev, sd);
@@ -386,6 +661,22 @@ static int register_csis_entity(struct fimc_md *fmd,
 	return ret;
 }
 
+static int register_fimc_is_entity(struct fimc_md *fmd, struct fimc_is *is)
+{
+	struct v4l2_subdev *sd = &is->isp.subdev;
+	int ret;
+
+	ret = v4l2_device_register_subdev(&fmd->v4l2_dev, sd);
+	if (ret) {
+		v4l2_err(&fmd->v4l2_dev,
+			 "Failed to register FIMC-ISP (%d)\n", ret);
+		return ret;
+	}
+
+	fmd->fimc_is = is;
+	return 0;
+}
+
 static int fimc_md_register_platform_entity(struct fimc_md *fmd,
 					    struct platform_device *pdev,
 					    int plat_entity)
@@ -413,6 +704,9 @@ static int fimc_md_register_platform_entity(struct fimc_md *fmd,
 		case IDX_CSIS:
 			ret = register_csis_entity(fmd, pdev, drvdata);
 			break;
+		case IDX_IS_ISP:
+			ret = register_fimc_is_entity(fmd, drvdata);
+			break;
 		default:
 			ret = -ENODEV;
 		}
@@ -457,6 +751,47 @@ static int fimc_md_pdev_match(struct device *dev, void *data)
 	return 0;
 }
 
+/* Register FIMC, FIMC-LITE and CSIS media entities */
+#ifdef CONFIG_OF
+static int fimc_md_register_of_platform_entities(struct fimc_md *fmd,
+						 struct device_node *parent)
+{
+	struct device_node *node;
+	int ret = 0;
+
+	for_each_available_child_of_node(parent, node) {
+		struct platform_device *pdev;
+		int plat_entity = -1;
+
+		pdev = of_find_device_by_node(node);
+		if (!pdev)
+			continue;
+
+		/* If driver of any entity isn't ready try all again later. */
+		if (!strcmp(node->name, CSIS_OF_NODE_NAME))
+			plat_entity = IDX_CSIS;
+		else if	(!strcmp(node->name, FIMC_IS_OF_NODE_NAME))
+			plat_entity = IDX_IS_ISP;
+		else if (!strcmp(node->name, FIMC_LITE_OF_NODE_NAME))
+			plat_entity = IDX_FLITE;
+		else if	(!strcmp(node->name, FIMC_OF_NODE_NAME) &&
+			 !of_property_read_bool(node, "samsung,lcd-wb"))
+			plat_entity = IDX_FIMC;
+
+		if (plat_entity >= 0)
+			ret = fimc_md_register_platform_entity(fmd, pdev,
+							plat_entity);
+		put_device(&pdev->dev);
+		if (ret < 0)
+			break;
+	}
+
+	return ret;
+}
+#else
+#define fimc_md_register_of_platform_entities(fmd, node) (-ENOSYS)
+#endif
+
 static void fimc_md_unregister_entities(struct fimc_md *fmd)
 {
 	int i;
@@ -479,7 +814,6 @@ static void fimc_md_unregister_entities(struct fimc_md *fmd)
 		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++) {
@@ -488,6 +822,10 @@ static void fimc_md_unregister_entities(struct fimc_md *fmd)
 		fimc_md_unregister_sensor(fmd->sensor[i].subdev);
 		fmd->sensor[i].subdev = NULL;
 	}
+
+	if (fmd->fimc_is)
+		v4l2_device_unregister_subdev(&fmd->fimc_is->isp.subdev);
+
 	v4l2_info(&fmd->v4l2_dev, "Unregistered all entities\n");
 }
 
@@ -504,12 +842,19 @@ static int __fimc_md_create_fimc_sink_links(struct fimc_md *fmd,
 					    struct v4l2_subdev *sensor,
 					    int pad, int link_mask)
 {
-	struct fimc_sensor_info *s_info = NULL;
+	struct fimc_source_info *si = NULL;
 	struct media_entity *sink;
 	unsigned int flags = 0;
-	int ret, i;
+	int i, ret = 0;
 
-	for (i = 0; i < FIMC_MAX_DEVS; i++) {
+	if (sensor) {
+		si = v4l2_get_subdev_hostdata(sensor);
+		/* Skip direct FIMC links in the logical FIMC-IS sensor path */
+		if (si && si->fimc_bus_type == FIMC_BUS_TYPE_ISP_WRITEBACK)
+			ret = 1;
+	}
+
+	for (i = 0; !ret && i < FIMC_MAX_DEVS; i++) {
 		if (!fmd->fimc[i])
 			continue;
 		/*
@@ -523,7 +868,7 @@ static int __fimc_md_create_fimc_sink_links(struct fimc_md *fmd,
 
 		sink = &fmd->fimc[i]->vid_cap.subdev.entity;
 		ret = media_entity_create_link(source, pad, sink,
-					      FIMC_SD_PAD_SINK, flags);
+					      FIMC_SD_PAD_SINK_CAM, flags);
 		if (ret)
 			return ret;
 
@@ -538,11 +883,13 @@ static int __fimc_md_create_fimc_sink_links(struct fimc_md *fmd,
 
 		if (flags == 0 || sensor == NULL)
 			continue;
-		s_info = v4l2_get_subdev_hostdata(sensor);
-		if (!WARN_ON(s_info == NULL)) {
+
+		if (!WARN_ON(si == NULL)) {
 			unsigned long irq_flags;
+			struct fimc_sensor_info *inf = source_to_sensor_info(si);
+
 			spin_lock_irqsave(&fmd->slock, irq_flags);
-			s_info->host = fmd->fimc[i];
+			inf->host = fmd->fimc[i];
 			spin_unlock_irqrestore(&fmd->slock, irq_flags);
 		}
 	}
@@ -551,25 +898,20 @@ static int __fimc_md_create_fimc_sink_links(struct fimc_md *fmd,
 		if (!fmd->fimc_lite[i])
 			continue;
 
-		if (link_mask & (1 << (i + FIMC_MAX_DEVS)))
-			flags = MEDIA_LNK_FL_ENABLED;
-		else
-			flags = 0;
-
 		sink = &fmd->fimc_lite[i]->subdev.entity;
 		ret = media_entity_create_link(source, pad, sink,
-					       FLITE_SD_PAD_SINK, flags);
+					       FLITE_SD_PAD_SINK, 0);
 		if (ret)
 			return ret;
 
 		/* Notify FIMC-LITE subdev entity */
 		ret = media_entity_call(sink, link_setup, &sink->pads[0],
-					&source->pads[pad], flags);
+					&source->pads[pad], 0);
 		if (ret)
 			break;
 
-		v4l2_info(&fmd->v4l2_dev, "created link [%s] %c> [%s]\n",
-			  source->name, flags ? '=' : '-', sink->name);
+		v4l2_info(&fmd->v4l2_dev, "created link [%s] -> [%s]\n",
+			  source->name, sink->name);
 	}
 	return 0;
 }
@@ -578,21 +920,50 @@ static int __fimc_md_create_fimc_sink_links(struct fimc_md *fmd,
 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 = 0;
 
 	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, FLITE_SD_PAD_SOURCE_DMA,
-					       sink, 0, flags);
+					       sink, 0, 0);
+		if (ret)
+			break;
+		/* Link from FIMC-LITE to IS-ISP subdev */
+		sink = &fmd->fimc_is->isp.subdev.entity;
+		ret = media_entity_create_link(source, FLITE_SD_PAD_SOURCE_ISP,
+					       sink, 0, 0);
 		if (ret)
 			break;
-		/* TODO: create links to other entities */
+	}
+
+	return ret;
+}
+
+/* Create FIMC-IS links */
+static int __fimc_md_create_fimc_is_links(struct fimc_md *fmd)
+{
+	struct media_entity *source, *sink;
+	int i, ret;
+
+	source = &fmd->fimc_is->isp.subdev.entity;
+
+	for (i = 0; i < FIMC_MAX_DEVS; i++) {
+		if (fmd->fimc[i] == NULL)
+			continue;
+
+		/* Link from IS-ISP subdev to FIMC */
+		sink = &fmd->fimc[i]->vid_cap.subdev.entity;
+		ret = media_entity_create_link(source, FIMC_ISP_SD_PAD_SRC_FIFO,
+					       sink, FIMC_SD_PAD_SINK_FIFO, 0);
+		if (ret)
+			return ret;
 	}
 
 	return ret;
@@ -615,7 +986,6 @@ static int fimc_md_create_links(struct fimc_md *fmd)
 	struct v4l2_subdev *csi_sensors[CSIS_MAX_ENTITIES] = { NULL };
 	struct v4l2_subdev *sensor, *csis;
 	struct fimc_source_info *pdata;
-	struct fimc_sensor_info *s_info;
 	struct media_entity *source, *sink;
 	int i, pad, fimc_id = 0, ret = 0;
 	u32 flags, link_mask = 0;
@@ -625,12 +995,11 @@ static int fimc_md_create_links(struct fimc_md *fmd)
 			continue;
 
 		sensor = fmd->sensor[i].subdev;
-		s_info = v4l2_get_subdev_hostdata(sensor);
-		if (!s_info)
+		pdata = v4l2_get_subdev_hostdata(sensor);
+		if (!pdata)
 			continue;
 
 		source = NULL;
-		pdata = &s_info->pdata;
 
 		switch (pdata->sensor_bus_type) {
 		case FIMC_BUS_TYPE_MIPI_CSI2:
@@ -680,6 +1049,7 @@ static int fimc_md_create_links(struct fimc_md *fmd)
 	for (i = 0; i < CSIS_MAX_ENTITIES; i++) {
 		if (fmd->csis[i].sd == NULL)
 			continue;
+
 		source = &fmd->csis[i].sd->entity;
 		pad = CSIS_PAD_SOURCE;
 		sensor = csi_sensors[i];
@@ -694,19 +1064,28 @@ static int fimc_md_create_links(struct fimc_md *fmd)
 	for (i = 0; i < FIMC_MAX_DEVS; i++) {
 		if (!fmd->fimc[i])
 			continue;
+
 		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);
 		if (ret)
 			break;
 	}
 
-	return __fimc_md_create_flite_source_links(fmd);
+	ret = __fimc_md_create_flite_source_links(fmd);
+	if (ret < 0)
+		return ret;
+
+	if (fmd->use_isp)
+		ret = __fimc_md_create_fimc_is_links(fmd);
+
+	return ret;
 }
 
 /*
- * The peripheral sensor clock management.
+ * The peripheral sensor and CAM_BLK (PIXELASYNCMx) clocks management.
  */
 static void fimc_md_put_clocks(struct fimc_md *fmd)
 {
@@ -719,6 +1098,14 @@ static void fimc_md_put_clocks(struct fimc_md *fmd)
 		clk_put(fmd->camclk[i].clock);
 		fmd->camclk[i].clock = ERR_PTR(-EINVAL);
 	}
+
+	/* Writeback (PIXELASYNCMx) clocks */
+	for (i = 0; i < FIMC_MAX_WBCLKS; i++) {
+		if (IS_ERR(fmd->wbclk[i]))
+			continue;
+		clk_put(fmd->wbclk[i]);
+		fmd->wbclk[i] = ERR_PTR(-EINVAL);
+	}
 }
 
 static int fimc_md_get_clocks(struct fimc_md *fmd)
@@ -755,35 +1142,59 @@ static int fimc_md_get_clocks(struct fimc_md *fmd)
 	if (ret)
 		fimc_md_put_clocks(fmd);
 
+	if (!fmd->use_isp)
+		return 0;
+	/*
+	 * For now get only PIXELASYNCM1 clock (Writeback B/ISP),
+	 * leave PIXELASYNCM0 out for the LCD Writeback driver.
+	 */
+	fmd->wbclk[CLK_IDX_WB_A] = ERR_PTR(-EINVAL);
+
+	for (i = CLK_IDX_WB_B; i < FIMC_MAX_WBCLKS; i++) {
+		snprintf(clk_name, sizeof(clk_name), "pxl_async%u", i);
+		clock = clk_get(dev, clk_name);
+		if (IS_ERR(clock)) {
+			v4l2_err(&fmd->v4l2_dev, "Failed to get clock: %s\n",
+				  clk_name);
+			ret = PTR_ERR(clock);
+			break;
+		}
+		fmd->wbclk[i] = clock;
+	}
+	if (ret)
+		fimc_md_put_clocks(fmd);
+
 	return ret;
 }
 
 static int __fimc_md_set_camclk(struct fimc_md *fmd,
-				struct fimc_sensor_info *s_info,
+				struct fimc_source_info *si,
 				bool on)
 {
-	struct fimc_source_info *pdata = &s_info->pdata;
 	struct fimc_camclk_info *camclk;
 	int ret = 0;
 
-	if (WARN_ON(pdata->clk_id >= FIMC_MAX_CAMCLKS) || fmd == NULL)
+	if (WARN_ON(si->clk_id >= FIMC_MAX_CAMCLKS) || !fmd || !fmd->pmf)
 		return -EINVAL;
 
-	camclk = &fmd->camclk[pdata->clk_id];
+	camclk = &fmd->camclk[si->clk_id];
 
 	dbg("camclk %d, f: %lu, use_count: %d, on: %d",
-	    pdata->clk_id, pdata->clk_frequency, camclk->use_count, on);
+	    si->clk_id, si->clk_frequency, camclk->use_count, on);
 
 	if (on) {
 		if (camclk->use_count > 0 &&
-		    camclk->frequency != pdata->clk_frequency)
+		    camclk->frequency != si->clk_frequency)
 			return -EINVAL;
 
 		if (camclk->use_count++ == 0) {
-			clk_set_rate(camclk->clock, pdata->clk_frequency);
-			camclk->frequency = pdata->clk_frequency;
+			clk_set_rate(camclk->clock, si->clk_frequency);
+			camclk->frequency = si->clk_frequency;
+			ret = pm_runtime_get_sync(fmd->pmf);
+			if (ret < 0)
+				return ret;
 			ret = clk_enable(camclk->clock);
-			dbg("Enabled camclk %d: f: %lu", pdata->clk_id,
+			dbg("Enabled camclk %d: f: %lu", si->clk_id,
 			    clk_get_rate(camclk->clock));
 		}
 		return ret;
@@ -794,7 +1205,8 @@ static int __fimc_md_set_camclk(struct fimc_md *fmd,
 
 	if (--camclk->use_count == 0) {
 		clk_disable(camclk->clock);
-		dbg("Disabled camclk %d", pdata->clk_id);
+		pm_runtime_put(fmd->pmf);
+		dbg("Disabled camclk %d", si->clk_id);
 	}
 	return ret;
 }
@@ -813,10 +1225,10 @@ static int __fimc_md_set_camclk(struct fimc_md *fmd,
  */
 int fimc_md_set_camclk(struct v4l2_subdev *sd, bool on)
 {
-	struct fimc_sensor_info *s_info = v4l2_get_subdev_hostdata(sd);
+	struct fimc_source_info *si = v4l2_get_subdev_hostdata(sd);
 	struct fimc_md *fmd = entity_to_fimc_mdev(&sd->entity);
 
-	return __fimc_md_set_camclk(fmd, s_info, on);
+	return __fimc_md_set_camclk(fmd, si, on);
 }
 
 static int fimc_md_link_notify(struct media_pad *source,
@@ -827,7 +1239,7 @@ static int fimc_md_link_notify(struct media_pad *source,
 	struct fimc_pipeline *pipeline;
 	struct v4l2_subdev *sd;
 	struct mutex *lock;
-	int ret = 0;
+	int i, ret = 0;
 	int ref_count;
 
 	if (media_entity_type(sink->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
@@ -854,29 +1266,28 @@ static int fimc_md_link_notify(struct media_pad *source,
 		return 0;
 	}
 
+	mutex_lock(lock);
+	ref_count = fimc ? fimc->vid_cap.refcnt : fimc_lite->ref_count;
+
 	if (!(flags & MEDIA_LNK_FL_ENABLED)) {
-		int i;
-		mutex_lock(lock);
-		ret = __fimc_pipeline_close(pipeline);
+		if (ref_count > 0) {
+			ret = __fimc_pipeline_close(pipeline);
+			if (!ret && fimc)
+				fimc_ctrls_delete(fimc->vid_cap.ctx);
+		}
 		for (i = 0; i < IDX_MAX; i++)
 			pipeline->subdevs[i] = NULL;
-		if (fimc)
-			fimc_ctrls_delete(fimc->vid_cap.ctx);
-		mutex_unlock(lock);
-		return ret;
+	} else if (ref_count > 0) {
+		/*
+		 * Link activation. Enable power of pipeline elements only if
+		 * the pipeline is already in use, i.e. its video node is open.
+		 * Recreate the controls destroyed during the link deactivation.
+		 */
+		ret = __fimc_pipeline_open(pipeline,
+					   source->entity, true);
+		if (!ret && fimc)
+			ret = fimc_capture_ctrls_create(fimc);
 	}
-	/*
-	 * Link activation. Enable power of pipeline elements only if the
-	 * pipeline is already in use, i.e. its video node is opened.
-	 * Recreate the controls destroyed during the link deactivation.
-	 */
-	mutex_lock(lock);
-
-	ref_count = fimc ? fimc->vid_cap.refcnt : fimc_lite->ref_count;
-	if (ref_count > 0)
-		ret = __fimc_pipeline_open(pipeline, source->entity, true);
-	if (!ret && fimc)
-		ret = fimc_capture_ctrls_create(fimc);
 
 	mutex_unlock(lock);
 	return ret ? -EPIPE : ret;
@@ -927,13 +1338,33 @@ 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 fimc_md_get_pinctrl(struct fimc_md *fmd)
+{
+	struct device *dev = &fmd->pdev->dev;
+	struct fimc_pinctrl *pctl = &fmd->pinctl;
+
+	pctl->pinctrl = devm_pinctrl_get(dev);
+	if (IS_ERR(pctl->pinctrl))
+		return PTR_ERR(pctl->pinctrl);
+
+	pctl->state_default = pinctrl_lookup_state(pctl->pinctrl,
+					PINCTRL_STATE_DEFAULT);
+	if (IS_ERR(pctl->state_default))
+		return PTR_ERR(pctl->state_default);
+
+	pctl->state_idle = pinctrl_lookup_state(pctl->pinctrl,
+					PINCTRL_STATE_IDLE);
+	return 0;
+}
+
 static int fimc_md_probe(struct platform_device *pdev)
 {
+	struct device *dev = &pdev->dev;
 	struct v4l2_device *v4l2_dev;
 	struct fimc_md *fmd;
 	int ret;
 
-	fmd = devm_kzalloc(&pdev->dev, sizeof(*fmd), GFP_KERNEL);
+	fmd = devm_kzalloc(dev, sizeof(*fmd), GFP_KERNEL);
 	if (!fmd)
 		return -ENOMEM;
 
@@ -943,15 +1374,16 @@ static int fimc_md_probe(struct platform_device *pdev)
 	strlcpy(fmd->media_dev.model, "SAMSUNG S5P FIMC",
 		sizeof(fmd->media_dev.model));
 	fmd->media_dev.link_notify = fimc_md_link_notify;
-	fmd->media_dev.dev = &pdev->dev;
+	fmd->media_dev.dev = dev;
 
 	v4l2_dev = &fmd->v4l2_dev;
 	v4l2_dev->mdev = &fmd->media_dev;
 	v4l2_dev->notify = fimc_sensor_notify;
-	snprintf(v4l2_dev->name, sizeof(v4l2_dev->name), "%s",
-		 dev_name(&pdev->dev));
+	strlcpy(v4l2_dev->name, "s5p-fimc-md", sizeof(v4l2_dev->name));
+
+	fmd->use_isp = fimc_md_is_isp_available(dev->of_node);
 
-	ret = v4l2_device_register(&pdev->dev, &fmd->v4l2_dev);
+	ret = v4l2_device_register(dev, &fmd->v4l2_dev);
 	if (ret < 0) {
 		v4l2_err(v4l2_dev, "Failed to register v4l2_device: %d\n", ret);
 		return ret;
@@ -965,21 +1397,32 @@ static int fimc_md_probe(struct platform_device *pdev)
 	if (ret)
 		goto err_clk;
 
-	fmd->user_subdev_api = false;
+	fmd->user_subdev_api = (dev->of_node != NULL);
 
 	/* Protect the media graph while we're registering entities */
 	mutex_lock(&fmd->media_dev.graph_mutex);
 
-	ret = bus_for_each_dev(&platform_bus_type, NULL, fmd,
-					fimc_md_pdev_match);
+	ret = fimc_md_get_pinctrl(fmd);
+	if (ret < 0) {
+		if (ret != EPROBE_DEFER)
+			dev_err(dev, "Failed to get pinctrl: %d\n", ret);
+		goto err_unlock;
+	}
+
+	if (dev->of_node)
+		ret = fimc_md_register_of_platform_entities(fmd, dev->of_node);
+	else
+		ret = bus_for_each_dev(&platform_bus_type, NULL, fmd,
+						fimc_md_pdev_match);
 	if (ret)
 		goto err_unlock;
 
-	if (pdev->dev.platform_data) {
+	if (dev->platform_data || dev->of_node) {
 		ret = fimc_md_register_sensor_entities(fmd);
 		if (ret)
 			goto err_unlock;
 	}
+
 	ret = fimc_md_create_links(fmd);
 	if (ret)
 		goto err_unlock;
@@ -1019,12 +1462,25 @@ static int fimc_md_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static struct platform_device_id fimc_driver_ids[] __always_unused = {
+	{ .name = "s5p-fimc-md" },
+	{ },
+};
+MODULE_DEVICE_TABLE(platform, fimc_driver_ids);
+
+static const struct of_device_id fimc_md_of_match[] = {
+	{ .compatible = "samsung,fimc" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, fimc_md_of_match);
+
 static struct platform_driver fimc_md_driver = {
 	.probe		= fimc_md_probe,
 	.remove		= fimc_md_remove,
 	.driver = {
-		.name	= "s5p-fimc-md",
-		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(fimc_md_of_match),
+		.name		= "s5p-fimc-md",
+		.owner		= THIS_MODULE,
 	}
 };
 
diff --git a/drivers/media/platform/s5p-fimc/fimc-mdevice.h b/drivers/media/platform/exynos4-is/media-dev.h
index 06b0d8276fd..44d86b61d66 100644
--- a/drivers/media/platform/s5p-fimc/fimc-mdevice.h
+++ b/drivers/media/platform/exynos4-is/media-dev.h
@@ -12,6 +12,8 @@
 #include <linux/clk.h>
 #include <linux/platform_device.h>
 #include <linux/mutex.h>
+#include <linux/of.h>
+#include <linux/pinctrl/consumer.h>
 #include <media/media-device.h>
 #include <media/media-entity.h>
 #include <media/v4l2-device.h>
@@ -21,18 +23,23 @@
 #include "fimc-lite.h"
 #include "mipi-csis.h"
 
-/* Group IDs of sensor, MIPI-CSIS, FIMC-LITE and the writeback subdevs. */
-#define GRP_ID_SENSOR		(1 << 8)
-#define GRP_ID_FIMC_IS_SENSOR	(1 << 9)
-#define GRP_ID_WRITEBACK	(1 << 10)
-#define GRP_ID_CSIS		(1 << 11)
-#define GRP_ID_FIMC		(1 << 12)
-#define GRP_ID_FLITE		(1 << 13)
-#define GRP_ID_FIMC_IS		(1 << 14)
+#define FIMC_OF_NODE_NAME	"fimc"
+#define FIMC_LITE_OF_NODE_NAME	"fimc-lite"
+#define FIMC_IS_OF_NODE_NAME	"fimc-is"
+#define CSIS_OF_NODE_NAME	"csis"
+
+#define PINCTRL_STATE_IDLE	"idle"
 
 #define FIMC_MAX_SENSORS	8
 #define FIMC_MAX_CAMCLKS	2
 
+/* LCD/ISP Writeback clocks (PIXELASYNCMx) */
+enum {
+	CLK_IDX_WB_A,
+	CLK_IDX_WB_B,
+	FIMC_MAX_WBCLKS
+};
+
 struct fimc_csis_info {
 	struct v4l2_subdev *sd;
 	int id;
@@ -65,9 +72,15 @@ struct fimc_sensor_info {
  * @num_sensors: actual number of registered sensors
  * @camclk: external sensor clock information
  * @fimc: array of registered fimc devices
+ * @fimc_is: fimc-is data structure
+ * @use_isp: set to true when FIMC-IS subsystem is used
+ * @pmf: handle to the CAMCLK clock control FIMC helper device
  * @media_dev: top level media device
  * @v4l2_dev: top level v4l2_device holding up the subdevs
  * @pdev: platform device this media device is hooked up into
+ * @pinctrl: camera port pinctrl handle
+ * @state_default: pinctrl default state handle
+ * @state_idle: pinctrl idle state handle
  * @user_subdev_api: true if subdevs are not configured by the host driver
  * @slock: spinlock protecting @sensor array
  */
@@ -76,11 +89,20 @@ struct fimc_md {
 	struct fimc_sensor_info sensor[FIMC_MAX_SENSORS];
 	int num_sensors;
 	struct fimc_camclk_info camclk[FIMC_MAX_CAMCLKS];
+	struct clk *wbclk[FIMC_MAX_WBCLKS];
 	struct fimc_lite *fimc_lite[FIMC_LITE_MAX_DEVS];
 	struct fimc_dev *fimc[FIMC_MAX_DEVS];
+	struct fimc_is *fimc_is;
+	bool use_isp;
+	struct device *pmf;
 	struct media_device media_dev;
 	struct v4l2_device v4l2_dev;
 	struct platform_device *pdev;
+	struct fimc_pinctrl {
+		struct pinctrl *pinctrl;
+		struct pinctrl_state *state_default;
+		struct pinctrl_state *state_idle;
+	} pinctl;
 	bool user_subdev_api;
 	spinlock_t slock;
 };
@@ -93,6 +115,12 @@ struct fimc_md {
 
 #define subdev_has_devnode(__sd) (__sd->flags & V4L2_SUBDEV_FL_HAS_DEVNODE)
 
+static inline
+struct fimc_sensor_info *source_to_sensor_info(struct fimc_source_info *si)
+{
+	return container_of(si, struct fimc_sensor_info, pdata);
+}
+
 static inline struct fimc_md *entity_to_fimc_mdev(struct media_entity *me)
 {
 	return me->parent == NULL ? NULL :
@@ -111,4 +139,14 @@ static inline void fimc_md_graph_unlock(struct fimc_dev *fimc)
 
 int fimc_md_set_camclk(struct v4l2_subdev *sd, bool on);
 
+#ifdef CONFIG_OF
+static inline bool fimc_md_is_isp_available(struct device_node *node)
+{
+	node = of_get_child_by_name(node, FIMC_IS_OF_NODE_NAME);
+	return node ? of_device_is_available(node) : false;
+}
+#else
+#define fimc_md_is_isp_available(node) (false)
+#endif /* CONFIG_OF */
+
 #endif
diff --git a/drivers/media/platform/s5p-fimc/mipi-csis.c b/drivers/media/platform/exynos4-is/mipi-csis.c
index 981863d05aa..a2eda9d5ac8 100644
--- a/drivers/media/platform/s5p-fimc/mipi-csis.c
+++ b/drivers/media/platform/exynos4-is/mipi-csis.c
@@ -19,14 +19,18 @@
 #include <linux/kernel.h>
 #include <linux/memory.h>
 #include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_data/mipi-csis.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/regulator/consumer.h>
 #include <linux/slab.h>
 #include <linux/spinlock.h>
 #include <linux/videodev2.h>
+#include <media/s5p_fimc.h>
+#include <media/v4l2-of.h>
 #include <media/v4l2-subdev.h>
-#include <linux/platform_data/mipi-csis.h>
+
 #include "mipi-csis.h"
 
 static int debug;
@@ -113,6 +117,7 @@ static char *csi_clock_name[] = {
 	[CSIS_CLK_GATE] = "csis",
 };
 #define NUM_CSIS_CLOCKS	ARRAY_SIZE(csi_clock_name)
+#define DEFAULT_SCLK_CSIS_FREQ	166000000UL
 
 static const char * const csis_supply_name[] = {
 	"vddcore",  /* CSIS Core (1.0V, 1.1V or 1.2V) suppply */
@@ -167,6 +172,11 @@ struct csis_pktbuf {
  * @clock: CSIS clocks
  * @irq: requested s5p-mipi-csis irq number
  * @flags: the state variable for power and streaming control
+ * @clock_frequency: device bus clock frequency
+ * @hs_settle: HS-RX settle time
+ * @num_lanes: number of MIPI-CSI data lanes used
+ * @max_num_lanes: maximum number of MIPI-CSI data lanes supported
+ * @wclk_ext: CSI wrapper clock: 0 - bus clock, 1 - external SCLK_CAM
  * @csis_fmt: current CSIS pixel format
  * @format: common media bus format for the source and sink pad
  * @slock: spinlock protecting structure members below
@@ -184,6 +194,13 @@ struct csis_state {
 	struct clk *clock[NUM_CSIS_CLOCKS];
 	int irq;
 	u32 flags;
+
+	u32 clk_frequency;
+	u32 hs_settle;
+	u32 num_lanes;
+	u32 max_num_lanes;
+	u8 wclk_ext;
+
 	const struct csis_pix_format *csis_fmt;
 	struct v4l2_mbus_framefmt format;
 
@@ -273,7 +290,6 @@ static void s5pcsis_reset(struct csis_state *state)
 
 static void s5pcsis_system_enable(struct csis_state *state, int on)
 {
-	struct s5p_platform_mipi_csis *pdata = state->pdev->dev.platform_data;
 	u32 val, mask;
 
 	val = s5pcsis_read(state, S5PCSIS_CTRL);
@@ -286,7 +302,7 @@ static void s5pcsis_system_enable(struct csis_state *state, int on)
 	val = s5pcsis_read(state, S5PCSIS_DPHYCTRL);
 	val &= ~S5PCSIS_DPHYCTRL_ENABLE;
 	if (on) {
-		mask = (1 << (pdata->lanes + 1)) - 1;
+		mask = (1 << (state->num_lanes + 1)) - 1;
 		val |= (mask & S5PCSIS_DPHYCTRL_ENABLE);
 	}
 	s5pcsis_write(state, S5PCSIS_DPHYCTRL, val);
@@ -321,15 +337,14 @@ static void s5pcsis_set_hsync_settle(struct csis_state *state, int settle)
 
 static void s5pcsis_set_params(struct csis_state *state)
 {
-	struct s5p_platform_mipi_csis *pdata = state->pdev->dev.platform_data;
 	u32 val;
 
 	val = s5pcsis_read(state, S5PCSIS_CONFIG);
-	val = (val & ~S5PCSIS_CFG_NR_LANE_MASK) | (pdata->lanes - 1);
+	val = (val & ~S5PCSIS_CFG_NR_LANE_MASK) | (state->num_lanes - 1);
 	s5pcsis_write(state, S5PCSIS_CONFIG, val);
 
 	__s5pcsis_set_format(state);
-	s5pcsis_set_hsync_settle(state, pdata->hs_settle);
+	s5pcsis_set_hsync_settle(state, state->hs_settle);
 
 	val = s5pcsis_read(state, S5PCSIS_CTRL);
 	if (state->csis_fmt->data_alignment == 32)
@@ -338,7 +353,7 @@ static void s5pcsis_set_params(struct csis_state *state)
 		val &= ~S5PCSIS_CTRL_ALIGN_32BIT;
 
 	val &= ~S5PCSIS_CTRL_WCLK_EXTCLK;
-	if (pdata->wclk_source)
+	if (state->wclk_ext)
 		val |= S5PCSIS_CTRL_WCLK_EXTCLK;
 	s5pcsis_write(state, S5PCSIS_CTRL, val);
 
@@ -534,10 +549,10 @@ static struct csis_pix_format const *s5pcsis_try_format(
 
 static struct v4l2_mbus_framefmt *__s5pcsis_get_format(
 		struct csis_state *state, struct v4l2_subdev_fh *fh,
-		u32 pad, enum v4l2_subdev_format_whence which)
+		enum v4l2_subdev_format_whence which)
 {
 	if (which == V4L2_SUBDEV_FORMAT_TRY)
-		return fh ? v4l2_subdev_get_try_format(fh, pad) : NULL;
+		return fh ? v4l2_subdev_get_try_format(fh, 0) : NULL;
 
 	return &state->format;
 }
@@ -549,10 +564,7 @@ static int s5pcsis_set_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
 	struct csis_pix_format const *csis_fmt;
 	struct v4l2_mbus_framefmt *mf;
 
-	if (fmt->pad != CSIS_PAD_SOURCE && fmt->pad != CSIS_PAD_SINK)
-		return -EINVAL;
-
-	mf = __s5pcsis_get_format(state, fh, fmt->pad, fmt->which);
+	mf = __s5pcsis_get_format(state, fh, fmt->which);
 
 	if (fmt->pad == CSIS_PAD_SOURCE) {
 		if (mf) {
@@ -579,10 +591,7 @@ static int s5pcsis_get_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
 	struct csis_state *state = sd_to_csis_state(sd);
 	struct v4l2_mbus_framefmt *mf;
 
-	if (fmt->pad != CSIS_PAD_SOURCE && fmt->pad != CSIS_PAD_SINK)
-		return -EINVAL;
-
-	mf = __s5pcsis_get_format(state, fh, fmt->pad, fmt->which);
+	mf = __s5pcsis_get_format(state, fh, fmt->which);
 	if (!mf)
 		return -EINVAL;
 
@@ -701,52 +710,111 @@ static irqreturn_t s5pcsis_irq_handler(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
+static int s5pcsis_get_platform_data(struct platform_device *pdev,
+				     struct csis_state *state)
+{
+	struct s5p_platform_mipi_csis *pdata = pdev->dev.platform_data;
+
+	if (pdata == NULL) {
+		dev_err(&pdev->dev, "Platform data not specified\n");
+		return -EINVAL;
+	}
+
+	state->clk_frequency = pdata->clk_rate;
+	state->num_lanes = pdata->lanes;
+	state->hs_settle = pdata->hs_settle;
+	state->index = max(0, pdev->id);
+	state->max_num_lanes = state->index ? CSIS1_MAX_LANES :
+					      CSIS0_MAX_LANES;
+	return 0;
+}
+
+#ifdef CONFIG_OF
+static int s5pcsis_parse_dt(struct platform_device *pdev,
+			    struct csis_state *state)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct v4l2_of_endpoint endpoint;
+
+	if (of_property_read_u32(node, "clock-frequency",
+				 &state->clk_frequency))
+		state->clk_frequency = DEFAULT_SCLK_CSIS_FREQ;
+	if (of_property_read_u32(node, "bus-width",
+				 &state->max_num_lanes))
+		return -EINVAL;
+
+	node = v4l2_of_get_next_endpoint(node, NULL);
+	if (!node) {
+		dev_err(&pdev->dev, "No port node at %s\n",
+					node->full_name);
+		return -EINVAL;
+	}
+	/* Get port node and validate MIPI-CSI channel id. */
+	v4l2_of_parse_endpoint(node, &endpoint);
+
+	state->index = endpoint.port - FIMC_INPUT_MIPI_CSI2_0;
+	if (state->index < 0 || state->index >= CSIS_MAX_ENTITIES)
+		return -ENXIO;
+
+	/* Get MIPI CSI-2 bus configration from the endpoint node. */
+	of_property_read_u32(node, "samsung,csis-hs-settle",
+					&state->hs_settle);
+	state->wclk_ext = of_property_read_bool(node,
+					"samsung,csis-wclk");
+
+	state->num_lanes = endpoint.bus.mipi_csi2.num_data_lanes;
+
+	of_node_put(node);
+	return 0;
+}
+#else
+#define s5pcsis_parse_dt(pdev, state) (-ENOSYS)
+#endif
+
 static int s5pcsis_probe(struct platform_device *pdev)
 {
-	struct s5p_platform_mipi_csis *pdata;
+	struct device *dev = &pdev->dev;
 	struct resource *mem_res;
 	struct csis_state *state;
 	int ret = -ENOMEM;
 	int i;
 
-	state = devm_kzalloc(&pdev->dev, sizeof(*state), GFP_KERNEL);
+	state = devm_kzalloc(dev, sizeof(*state), GFP_KERNEL);
 	if (!state)
 		return -ENOMEM;
 
 	mutex_init(&state->lock);
 	spin_lock_init(&state->slock);
-
 	state->pdev = pdev;
-	state->index = max(0, pdev->id);
 
-	pdata = pdev->dev.platform_data;
-	if (pdata == NULL) {
-		dev_err(&pdev->dev, "Platform data not fully specified\n");
-		return -EINVAL;
-	}
+	if (dev->of_node)
+		ret = s5pcsis_parse_dt(pdev, state);
+	else
+		ret = s5pcsis_get_platform_data(pdev, state);
+	if (ret < 0)
+		return ret;
 
-	if ((state->index == 1 && pdata->lanes > CSIS1_MAX_LANES) ||
-	    pdata->lanes > CSIS0_MAX_LANES) {
-		dev_err(&pdev->dev, "Unsupported number of data lanes: %d\n",
-			pdata->lanes);
+	if (state->num_lanes == 0 || state->num_lanes > state->max_num_lanes) {
+		dev_err(dev, "Unsupported number of data lanes: %d (max. %d)\n",
+			state->num_lanes, state->max_num_lanes);
 		return -EINVAL;
 	}
 
 	mem_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	state->regs = devm_ioremap_resource(&pdev->dev, mem_res);
+	state->regs = devm_ioremap_resource(dev, mem_res);
 	if (IS_ERR(state->regs))
 		return PTR_ERR(state->regs);
 
 	state->irq = platform_get_irq(pdev, 0);
 	if (state->irq < 0) {
-		dev_err(&pdev->dev, "Failed to get irq\n");
+		dev_err(dev, "Failed to get irq\n");
 		return state->irq;
 	}
 
 	for (i = 0; i < CSIS_NUM_SUPPLIES; i++)
 		state->supplies[i].supply = csis_supply_name[i];
 
-	ret = devm_regulator_bulk_get(&pdev->dev, CSIS_NUM_SUPPLIES,
+	ret = devm_regulator_bulk_get(dev, CSIS_NUM_SUPPLIES,
 				 state->supplies);
 	if (ret)
 		return ret;
@@ -755,11 +823,11 @@ static int s5pcsis_probe(struct platform_device *pdev)
 	if (ret < 0)
 		return ret;
 
-	if (pdata->clk_rate)
+	if (state->clk_frequency)
 		ret = clk_set_rate(state->clock[CSIS_CLK_MUX],
-				   pdata->clk_rate);
+				   state->clk_frequency);
 	else
-		dev_WARN(&pdev->dev, "No clock frequency specified!\n");
+		dev_WARN(dev, "No clock frequency specified!\n");
 	if (ret < 0)
 		goto e_clkput;
 
@@ -767,16 +835,17 @@ static int s5pcsis_probe(struct platform_device *pdev)
 	if (ret < 0)
 		goto e_clkput;
 
-	ret = devm_request_irq(&pdev->dev, state->irq, s5pcsis_irq_handler,
-			       0, dev_name(&pdev->dev), state);
+	ret = devm_request_irq(dev, state->irq, s5pcsis_irq_handler,
+			       0, dev_name(dev), state);
 	if (ret) {
-		dev_err(&pdev->dev, "Interrupt request failed\n");
+		dev_err(dev, "Interrupt request failed\n");
 		goto e_clkdis;
 	}
 
 	v4l2_subdev_init(&state->sd, &s5pcsis_subdev_ops);
 	state->sd.owner = THIS_MODULE;
-	strlcpy(state->sd.name, dev_name(&pdev->dev), sizeof(state->sd.name));
+	snprintf(state->sd.name, sizeof(state->sd.name), "%s.%d",
+		 CSIS_SUBDEV_NAME, state->index);
 	state->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 	state->csis_fmt = &s5pcsis_formats[0];
 
@@ -796,10 +865,12 @@ static int s5pcsis_probe(struct platform_device *pdev)
 
 	/* .. and a pointer to the subdev. */
 	platform_set_drvdata(pdev, &state->sd);
-
 	memcpy(state->events, s5pcsis_events, sizeof(state->events));
+	pm_runtime_enable(dev);
 
-	pm_runtime_enable(&pdev->dev);
+	dev_info(&pdev->dev, "lanes: %d, hs_settle: %d, wclk: %d, freq: %u\n",
+		 state->num_lanes, state->hs_settle, state->wclk_ext,
+		 state->clk_frequency);
 	return 0;
 
 e_clkdis:
@@ -923,13 +994,21 @@ static const struct dev_pm_ops s5pcsis_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(s5pcsis_suspend, s5pcsis_resume)
 };
 
+static const struct of_device_id s5pcsis_of_match[] = {
+	{ .compatible = "samsung,s5pv210-csis" },
+	{ .compatible = "samsung,exynos4210-csis" },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, s5pcsis_of_match);
+
 static struct platform_driver s5pcsis_driver = {
 	.probe		= s5pcsis_probe,
 	.remove		= s5pcsis_remove,
 	.driver		= {
-		.name	= CSIS_DRIVER_NAME,
-		.owner	= THIS_MODULE,
-		.pm	= &s5pcsis_pm_ops,
+		.of_match_table = s5pcsis_of_match,
+		.name		= CSIS_DRIVER_NAME,
+		.owner		= THIS_MODULE,
+		.pm		= &s5pcsis_pm_ops,
 	},
 };
 
diff --git a/drivers/media/platform/s5p-fimc/mipi-csis.h b/drivers/media/platform/exynos4-is/mipi-csis.h
index 2709286396e..28c11c4085d 100644
--- a/drivers/media/platform/s5p-fimc/mipi-csis.h
+++ b/drivers/media/platform/exynos4-is/mipi-csis.h
@@ -11,6 +11,7 @@
 #define S5P_MIPI_CSIS_H_
 
 #define CSIS_DRIVER_NAME	"s5p-mipi-csis"
+#define CSIS_SUBDEV_NAME	CSIS_DRIVER_NAME
 #define CSIS_MAX_ENTITIES	2
 #define CSIS0_MAX_LANES		4
 #define CSIS1_MAX_LANES		2
diff --git a/drivers/media/platform/fsl-viu.c b/drivers/media/platform/fsl-viu.c
index 5f7db3f1f6f..3a6a0dcdc3e 100644
--- a/drivers/media/platform/fsl-viu.c
+++ b/drivers/media/platform/fsl-viu.c
@@ -957,12 +957,12 @@ static int vidioc_querystd(struct file *file, void *priv, v4l2_std_id *std_id)
 	return 0;
 }
 
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *id)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id id)
 {
 	struct viu_fh *fh = priv;
 
-	fh->dev->std = *id;
-	decoder_call(fh->dev, core, s_std, *id);
+	fh->dev->std = id;
+	decoder_call(fh->dev, core, s_std, id);
 	return 0;
 }
 
diff --git a/drivers/media/platform/m2m-deinterlace.c b/drivers/media/platform/m2m-deinterlace.c
index 6c4db9b9898..75856464958 100644
--- a/drivers/media/platform/m2m-deinterlace.c
+++ b/drivers/media/platform/m2m-deinterlace.c
@@ -207,6 +207,9 @@ static void dma_callback(void *data)
 	src_vb = v4l2_m2m_src_buf_remove(curr_ctx->m2m_ctx);
 	dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->m2m_ctx);
 
+	src_vb->v4l2_buf.timestamp = dst_vb->v4l2_buf.timestamp;
+	src_vb->v4l2_buf.timecode = dst_vb->v4l2_buf.timecode;
+
 	v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
 	v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
 
@@ -866,6 +869,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	src_vq->ops = &deinterlace_qops;
 	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 	q_data[V4L2_M2M_SRC].fmt = &formats[0];
 	q_data[V4L2_M2M_SRC].width = 640;
 	q_data[V4L2_M2M_SRC].height = 480;
@@ -882,6 +886,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	dst_vq->ops = &deinterlace_qops;
 	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 	q_data[V4L2_M2M_DST].fmt = &formats[0];
 	q_data[V4L2_M2M_DST].width = 640;
 	q_data[V4L2_M2M_DST].height = 480;
diff --git a/drivers/media/platform/marvell-ccic/mcam-core.c b/drivers/media/platform/marvell-ccic/mcam-core.c
index 92a33f08185..64ab91edfb8 100644
--- a/drivers/media/platform/marvell-ccic/mcam-core.c
+++ b/drivers/media/platform/marvell-ccic/mcam-core.c
@@ -1357,7 +1357,7 @@ static int mcam_vidioc_s_input(struct file *filp, void *priv, unsigned int i)
 }
 
 /* from vivi.c */
-static int mcam_vidioc_s_std(struct file *filp, void *priv, v4l2_std_id *a)
+static int mcam_vidioc_s_std(struct file *filp, void *priv, v4l2_std_id a)
 {
 	return 0;
 }
@@ -1445,7 +1445,7 @@ static int mcam_vidioc_g_register(struct file *file, void *priv,
 }
 
 static int mcam_vidioc_s_register(struct file *file, void *priv,
-		struct v4l2_dbg_register *reg)
+		const struct v4l2_dbg_register *reg)
 {
 	struct mcam_camera *cam = priv;
 
diff --git a/drivers/media/platform/mem2mem_testdev.c b/drivers/media/platform/mem2mem_testdev.c
index 7487d7208de..4cc7f65d7d7 100644
--- a/drivers/media/platform/mem2mem_testdev.c
+++ b/drivers/media/platform/mem2mem_testdev.c
@@ -38,6 +38,10 @@ MODULE_AUTHOR("Pawel Osciak, <pawel@osciak.com>");
 MODULE_LICENSE("GPL");
 MODULE_VERSION("0.1.1");
 
+static unsigned debug;
+module_param(debug, uint, 0644);
+MODULE_PARM_DESC(debug, "activates debug info");
+
 #define MIN_W 32
 #define MIN_H 32
 #define MAX_W 640
@@ -67,7 +71,7 @@ MODULE_VERSION("0.1.1");
 #define MEM2MEM_VFLIP	(1 << 1)
 
 #define dprintk(dev, fmt, arg...) \
-	v4l2_dbg(1, 1, &dev->v4l2_dev, "%s: " fmt, __func__, ## arg)
+	v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: " fmt, __func__, ## arg)
 
 
 static void m2mtest_dev_release(struct device *dev)
@@ -234,6 +238,10 @@ static int device_process(struct m2mtest_ctx *ctx,
 	bytes_left = bytesperline - tile_w * MEM2MEM_NUM_TILES;
 	w = 0;
 
+	memcpy(&out_vb->v4l2_buf.timestamp,
+			&in_vb->v4l2_buf.timestamp,
+			sizeof(struct timeval));
+
 	switch (ctx->mode) {
 	case MEM2MEM_HFLIP | MEM2MEM_VFLIP:
 		p_out += bytesperline * height - bytes_left;
@@ -844,6 +852,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq, struct vb2_queue *ds
 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	src_vq->ops = &m2mtest_qops;
 	src_vq->mem_ops = &vb2_vmalloc_memops;
+	src_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	ret = vb2_queue_init(src_vq);
 	if (ret)
@@ -855,6 +864,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq, struct vb2_queue *ds
 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	dst_vq->ops = &m2mtest_qops;
 	dst_vq->mem_ops = &vb2_vmalloc_memops;
+	dst_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	return vb2_queue_init(dst_vq);
 }
diff --git a/drivers/media/platform/mx2_emmaprp.c b/drivers/media/platform/mx2_emmaprp.c
index 4b9e0a28616..f7440e585b6 100644
--- a/drivers/media/platform/mx2_emmaprp.c
+++ b/drivers/media/platform/mx2_emmaprp.c
@@ -377,6 +377,9 @@ static irqreturn_t emmaprp_irq(int irq_emma, void *data)
 			src_vb = v4l2_m2m_src_buf_remove(curr_ctx->m2m_ctx);
 			dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->m2m_ctx);
 
+			src_vb->v4l2_buf.timestamp = dst_vb->v4l2_buf.timestamp;
+			src_vb->v4l2_buf.timecode = dst_vb->v4l2_buf.timecode;
+
 			spin_lock_irqsave(&pcdev->irqlock, flags);
 			v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
 			v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
@@ -763,6 +766,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	src_vq->ops = &emmaprp_qops;
 	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	ret = vb2_queue_init(src_vq);
 	if (ret)
@@ -774,6 +778,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	dst_vq->ops = &emmaprp_qops;
 	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	return vb2_queue_init(dst_vq);
 }
diff --git a/drivers/media/platform/omap/omap_vout.c b/drivers/media/platform/omap/omap_vout.c
index 96c4a17e428..477268a2415 100644
--- a/drivers/media/platform/omap/omap_vout.c
+++ b/drivers/media/platform/omap/omap_vout.c
@@ -648,9 +648,12 @@ static void omap_vout_isr(void *arg, unsigned int irqstatus)
 
 	/* First save the configuration in ovelray structure */
 	ret = omapvid_init(vout, addr);
-	if (ret)
+	if (ret) {
 		printk(KERN_ERR VOUT_NAME
 			"failed to set overlay info\n");
+		goto vout_isr_err;
+	}
+
 	/* Enable the pipeline and set the Go bit */
 	ret = omapvid_apply_changes(vout);
 	if (ret)
@@ -1660,13 +1663,16 @@ static int vidioc_streamon(struct file *file, void *fh, enum v4l2_buf_type i)
 	mask = DISPC_IRQ_VSYNC | DISPC_IRQ_EVSYNC_EVEN | DISPC_IRQ_EVSYNC_ODD
 		| DISPC_IRQ_VSYNC2;
 
-	omap_dispc_register_isr(omap_vout_isr, vout, mask);
-
 	/* First save the configuration in ovelray structure */
 	ret = omapvid_init(vout, addr);
-	if (ret)
+	if (ret) {
 		v4l2_err(&vout->vid_dev->v4l2_dev,
 				"failed to set overlay info\n");
+		goto streamon_err1;
+	}
+
+	omap_dispc_register_isr(omap_vout_isr, vout, mask);
+
 	/* Enable the pipeline and set the Go bit */
 	ret = omapvid_apply_changes(vout);
 	if (ret)
diff --git a/drivers/media/platform/s3c-camif/camif-capture.c b/drivers/media/platform/s3c-camif/camif-capture.c
index a55793c3d81..70438a0f62a 100644
--- a/drivers/media/platform/s3c-camif/camif-capture.c
+++ b/drivers/media/platform/s3c-camif/camif-capture.c
@@ -934,12 +934,19 @@ static int s3c_camif_reqbufs(struct file *file, void *priv,
 		vp->owner = NULL;
 
 	ret = vb2_reqbufs(&vp->vb_queue, rb);
-	if (!ret) {
-		vp->reqbufs_count = rb->count;
-		if (vp->owner == NULL && rb->count > 0)
-			vp->owner = priv;
+	if (ret < 0)
+		return ret;
+
+	if (rb->count && rb->count < CAMIF_REQ_BUFS_MIN) {
+		rb->count = 0;
+		vb2_reqbufs(&vp->vb_queue, rb);
+		ret = -ENOMEM;
 	}
 
+	vp->reqbufs_count = rb->count;
+	if (vp->owner == NULL && rb->count > 0)
+		vp->owner = priv;
+
 	return ret;
 }
 
@@ -1153,6 +1160,7 @@ int s3c_camif_register_video_node(struct camif_dev *camif, int idx)
 	q->mem_ops = &vb2_dma_contig_memops;
 	q->buf_struct_size = sizeof(struct camif_buffer);
 	q->drv_priv = vp;
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 
 	ret = vb2_queue_init(q);
 	if (ret)
diff --git a/drivers/media/platform/s5p-g2d/g2d.c b/drivers/media/platform/s5p-g2d/g2d.c
index aaaf276a5a6..553d87e5cea 100644
--- a/drivers/media/platform/s5p-g2d/g2d.c
+++ b/drivers/media/platform/s5p-g2d/g2d.c
@@ -18,6 +18,7 @@
 #include <linux/slab.h>
 #include <linux/clk.h>
 #include <linux/interrupt.h>
+#include <linux/of.h>
 
 #include <linux/platform_device.h>
 #include <media/v4l2-mem2mem.h>
@@ -157,6 +158,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	src_vq->ops = &g2d_qops;
 	src_vq->mem_ops = &vb2_dma_contig_memops;
 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	src_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	ret = vb2_queue_init(src_vq);
 	if (ret)
@@ -168,6 +170,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	dst_vq->ops = &g2d_qops;
 	dst_vq->mem_ops = &vb2_dma_contig_memops;
 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	dst_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	return vb2_queue_init(dst_vq);
 }
@@ -634,6 +637,9 @@ static irqreturn_t g2d_isr(int irq, void *prv)
 	BUG_ON(src == NULL);
 	BUG_ON(dst == NULL);
 
+	dst->v4l2_buf.timecode = src->v4l2_buf.timecode;
+	dst->v4l2_buf.timestamp = src->v4l2_buf.timestamp;
+
 	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);
@@ -695,11 +701,14 @@ static struct v4l2_m2m_ops g2d_m2m_ops = {
 	.unlock		= g2d_unlock,
 };
 
+static const struct of_device_id exynos_g2d_match[];
+
 static int g2d_probe(struct platform_device *pdev)
 {
 	struct g2d_dev *dev;
 	struct video_device *vfd;
 	struct resource *res;
+	const struct of_device_id *of_id;
 	int ret = 0;
 
 	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
@@ -794,7 +803,17 @@ static int g2d_probe(struct platform_device *pdev)
 	}
 
 	def_frame.stride = (def_frame.width * def_frame.fmt->depth) >> 3;
-	dev->variant = g2d_get_drv_data(pdev);
+
+	if (!pdev->dev.of_node) {
+		dev->variant = g2d_get_drv_data(pdev);
+	} else {
+		of_id = of_match_node(exynos_g2d_match, pdev->dev.of_node);
+		if (!of_id) {
+			ret = -ENODEV;
+			goto unreg_video_dev;
+		}
+		dev->variant = (struct g2d_variant *)of_id->data;
+	}
 
 	return 0;
 
@@ -835,13 +854,25 @@ static int g2d_remove(struct platform_device *pdev)
 }
 
 static struct g2d_variant g2d_drvdata_v3x = {
-	.hw_rev = TYPE_G2D_3X,
+	.hw_rev = TYPE_G2D_3X, /* Revision 3.0 for S5PV210 and Exynos4210 */
 };
 
 static struct g2d_variant g2d_drvdata_v4x = {
 	.hw_rev = TYPE_G2D_4X, /* Revision 4.1 for Exynos4X12 and Exynos5 */
 };
 
+static const struct of_device_id exynos_g2d_match[] = {
+	{
+		.compatible = "samsung,s5pv210-g2d",
+		.data = &g2d_drvdata_v3x,
+	}, {
+		.compatible = "samsung,exynos4212-g2d",
+		.data = &g2d_drvdata_v4x,
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, exynos_g2d_match);
+
 static struct platform_device_id g2d_driver_ids[] = {
 	{
 		.name = "s5p-g2d",
@@ -861,6 +892,7 @@ static struct platform_driver g2d_pdrv = {
 	.driver		= {
 		.name = G2D_NAME,
 		.owner = THIS_MODULE,
+		.of_match_table = exynos_g2d_match,
 	},
 };
 
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-core.c b/drivers/media/platform/s5p-jpeg/jpeg-core.c
index 3b023752bcb..15d23968d1d 100644
--- a/drivers/media/platform/s5p-jpeg/jpeg-core.c
+++ b/drivers/media/platform/s5p-jpeg/jpeg-core.c
@@ -1229,6 +1229,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	src_vq->ops = &s5p_jpeg_qops;
 	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	ret = vb2_queue_init(src_vq);
 	if (ret)
@@ -1240,6 +1241,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	dst_vq->ops = &s5p_jpeg_qops;
 	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	return vb2_queue_init(dst_vq);
 }
@@ -1287,6 +1289,9 @@ static irqreturn_t s5p_jpeg_irq(int irq, void *dev_id)
 		payload_size = jpeg_compressed_size(jpeg->regs);
 	}
 
+	dst_buf->v4l2_buf.timecode = src_buf->v4l2_buf.timecode;
+	dst_buf->v4l2_buf.timestamp = src_buf->v4l2_buf.timestamp;
+
 	v4l2_m2m_buf_done(src_buf, state);
 	if (curr_ctx->mode == S5P_JPEG_ENCODE)
 		vb2_set_plane_payload(dst_buf, 0, payload_size);
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc.c b/drivers/media/platform/s5p-mfc/s5p_mfc.c
index e84703c314c..01f9ae0dadb 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc.c
@@ -243,12 +243,10 @@ static void s5p_mfc_handle_frame_copy_time(struct s5p_mfc_ctx *ctx)
 	src_buf = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
 	list_for_each_entry(dst_buf, &ctx->dst_queue, list) {
 		if (vb2_dma_contig_plane_dma_addr(dst_buf->b, 0) == dec_y_addr) {
-			memcpy(&dst_buf->b->v4l2_buf.timecode,
-				&src_buf->b->v4l2_buf.timecode,
-				sizeof(struct v4l2_timecode));
-			memcpy(&dst_buf->b->v4l2_buf.timestamp,
-				&src_buf->b->v4l2_buf.timestamp,
-				sizeof(struct timeval));
+			dst_buf->b->v4l2_buf.timecode =
+						src_buf->b->v4l2_buf.timecode;
+			dst_buf->b->v4l2_buf.timestamp =
+						src_buf->b->v4l2_buf.timestamp;
 			switch (frame_type) {
 			case S5P_FIMV_DECODE_FRAME_I_FRAME:
 				dst_buf->b->v4l2_buf.flags |=
@@ -276,7 +274,7 @@ static void s5p_mfc_handle_frame_new(struct s5p_mfc_ctx *ctx, unsigned int err)
 	unsigned int frame_type;
 
 	dspl_y_addr = s5p_mfc_hw_call(dev->mfc_ops, get_dspl_y_adr, dev);
-	frame_type = s5p_mfc_hw_call(dev->mfc_ops, get_dec_frame_type, dev);
+	frame_type = s5p_mfc_hw_call(dev->mfc_ops, get_disp_frame_type, ctx);
 
 	/* If frame is same as previous then skip and do not dequeue */
 	if (frame_type == S5P_FIMV_DECODE_FRAME_SKIPPED) {
@@ -386,6 +384,8 @@ static void s5p_mfc_handle_frame(struct s5p_mfc_ctx *ctx,
 		} else {
 			mfc_debug(2, "MFC needs next buffer\n");
 			ctx->consumed_stream = 0;
+			if (src_buf->flags & MFC_BUF_FLAG_EOS)
+				ctx->state = MFCINST_FINISHING;
 			list_del(&src_buf->list);
 			ctx->src_queue_cnt--;
 			if (s5p_mfc_hw_call(dev->mfc_ops, err_dec, err) > 0)
@@ -804,6 +804,7 @@ static int s5p_mfc_open(struct file *file)
 		goto err_queue_init;
 	}
 	q->mem_ops = (struct vb2_mem_ops *)&vb2_dma_contig_memops;
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 	ret = vb2_queue_init(q);
 	if (ret) {
 		mfc_err("Failed to initialize videobuf2 queue(capture)\n");
@@ -825,6 +826,7 @@ static int s5p_mfc_open(struct file *file)
 		goto err_queue_init;
 	}
 	q->mem_ops = (struct vb2_mem_ops *)&vb2_dma_contig_memops;
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 	ret = vb2_queue_init(q);
 	if (ret) {
 		mfc_err("Failed to initialize videobuf2 queue(output)\n");
@@ -1016,7 +1018,7 @@ static void *mfc_get_drv_data(struct platform_device *pdev);
 
 static int s5p_mfc_alloc_memdevs(struct s5p_mfc_dev *dev)
 {
-	unsigned int mem_info[2];
+	unsigned int mem_info[2] = { };
 
 	dev->mem_dev_l = devm_kzalloc(&dev->plat_dev->dev,
 			sizeof(struct device), GFP_KERNEL);
@@ -1106,7 +1108,8 @@ static int s5p_mfc_probe(struct platform_device *pdev)
 	}
 
 	if (pdev->dev.of_node) {
-		if (s5p_mfc_alloc_memdevs(dev) < 0)
+		ret = s5p_mfc_alloc_memdevs(dev);
+		if (ret < 0)
 			goto err_res;
 	} else {
 		dev->mem_dev_l = device_find_child(&dev->plat_dev->dev,
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c
index 138778083c6..ad4f1df0a18 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c
@@ -16,7 +16,7 @@
 #include "s5p_mfc_debug.h"
 
 /* This function is used to send a command to the MFC */
-int s5p_mfc_cmd_host2risc_v5(struct s5p_mfc_dev *dev, int cmd,
+static int s5p_mfc_cmd_host2risc_v5(struct s5p_mfc_dev *dev, int cmd,
 				struct s5p_mfc_cmd_args *args)
 {
 	int cur_cmd;
@@ -41,7 +41,7 @@ int s5p_mfc_cmd_host2risc_v5(struct s5p_mfc_dev *dev, int cmd,
 }
 
 /* Initialize the MFC */
-int s5p_mfc_sys_init_cmd_v5(struct s5p_mfc_dev *dev)
+static int s5p_mfc_sys_init_cmd_v5(struct s5p_mfc_dev *dev)
 {
 	struct s5p_mfc_cmd_args h2r_args;
 
@@ -52,7 +52,7 @@ int s5p_mfc_sys_init_cmd_v5(struct s5p_mfc_dev *dev)
 }
 
 /* Suspend the MFC hardware */
-int s5p_mfc_sleep_cmd_v5(struct s5p_mfc_dev *dev)
+static int s5p_mfc_sleep_cmd_v5(struct s5p_mfc_dev *dev)
 {
 	struct s5p_mfc_cmd_args h2r_args;
 
@@ -61,7 +61,7 @@ int s5p_mfc_sleep_cmd_v5(struct s5p_mfc_dev *dev)
 }
 
 /* Wake up the MFC hardware */
-int s5p_mfc_wakeup_cmd_v5(struct s5p_mfc_dev *dev)
+static int s5p_mfc_wakeup_cmd_v5(struct s5p_mfc_dev *dev)
 {
 	struct s5p_mfc_cmd_args h2r_args;
 
@@ -71,7 +71,7 @@ int s5p_mfc_wakeup_cmd_v5(struct s5p_mfc_dev *dev)
 }
 
 
-int s5p_mfc_open_inst_cmd_v5(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_open_inst_cmd_v5(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 	struct s5p_mfc_cmd_args h2r_args;
@@ -124,7 +124,7 @@ int s5p_mfc_open_inst_cmd_v5(struct s5p_mfc_ctx *ctx)
 	return ret;
 }
 
-int s5p_mfc_close_inst_cmd_v5(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_close_inst_cmd_v5(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 	struct s5p_mfc_cmd_args h2r_args;
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c
index 754bfbcb1c4..5708fc3d9b4 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c
@@ -17,7 +17,7 @@
 #include "s5p_mfc_intr.h"
 #include "s5p_mfc_opr.h"
 
-int s5p_mfc_cmd_host2risc_v6(struct s5p_mfc_dev *dev, int cmd,
+static int s5p_mfc_cmd_host2risc_v6(struct s5p_mfc_dev *dev, int cmd,
 				struct s5p_mfc_cmd_args *args)
 {
 	mfc_debug(2, "Issue the command: %d\n", cmd);
@@ -32,7 +32,7 @@ int s5p_mfc_cmd_host2risc_v6(struct s5p_mfc_dev *dev, int cmd,
 	return 0;
 }
 
-int s5p_mfc_sys_init_cmd_v6(struct s5p_mfc_dev *dev)
+static int s5p_mfc_sys_init_cmd_v6(struct s5p_mfc_dev *dev)
 {
 	struct s5p_mfc_cmd_args h2r_args;
 	struct s5p_mfc_buf_size_v6 *buf_size = dev->variant->buf_size->priv;
@@ -44,7 +44,7 @@ int s5p_mfc_sys_init_cmd_v6(struct s5p_mfc_dev *dev)
 					&h2r_args);
 }
 
-int s5p_mfc_sleep_cmd_v6(struct s5p_mfc_dev *dev)
+static int s5p_mfc_sleep_cmd_v6(struct s5p_mfc_dev *dev)
 {
 	struct s5p_mfc_cmd_args h2r_args;
 
@@ -53,7 +53,7 @@ int s5p_mfc_sleep_cmd_v6(struct s5p_mfc_dev *dev)
 			&h2r_args);
 }
 
-int s5p_mfc_wakeup_cmd_v6(struct s5p_mfc_dev *dev)
+static int s5p_mfc_wakeup_cmd_v6(struct s5p_mfc_dev *dev)
 {
 	struct s5p_mfc_cmd_args h2r_args;
 
@@ -63,7 +63,7 @@ int s5p_mfc_wakeup_cmd_v6(struct s5p_mfc_dev *dev)
 }
 
 /* Open a new instance and get its number */
-int s5p_mfc_open_inst_cmd_v6(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_open_inst_cmd_v6(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 	struct s5p_mfc_cmd_args h2r_args;
@@ -121,7 +121,7 @@ int s5p_mfc_open_inst_cmd_v6(struct s5p_mfc_ctx *ctx)
 }
 
 /* Close instance */
-int s5p_mfc_close_inst_cmd_v6(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_close_inst_cmd_v6(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 	struct s5p_mfc_cmd_args h2r_args;
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
index 4582473978c..4af53bd2f18 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
@@ -22,6 +22,7 @@
 #include <linux/videodev2.h>
 #include <linux/workqueue.h>
 #include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
 #include <media/videobuf2-core.h>
 #include "s5p_mfc_common.h"
 #include "s5p_mfc_debug.h"
@@ -623,17 +624,27 @@ static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
 /* Dequeue a buffer */
 static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
 {
+	const struct v4l2_event ev = {
+		.type = V4L2_EVENT_EOS
+	};
 	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+	int ret;
 
 	if (ctx->state == MFCINST_ERROR) {
 		mfc_err("Call on DQBUF after unrecoverable error\n");
 		return -EIO;
 	}
 	if (buf->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
-		return vb2_dqbuf(&ctx->vq_src, buf, file->f_flags & O_NONBLOCK);
-	else if (buf->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
-		return vb2_dqbuf(&ctx->vq_dst, buf, file->f_flags & O_NONBLOCK);
-	return -EINVAL;
+		ret = vb2_dqbuf(&ctx->vq_src, buf, file->f_flags & O_NONBLOCK);
+	else if (buf->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		ret = vb2_dqbuf(&ctx->vq_dst, buf, file->f_flags & O_NONBLOCK);
+		if (ret == 0 && ctx->state == MFCINST_FINISHED &&
+				list_empty(&ctx->vq_dst.done_list))
+			v4l2_event_queue_fh(&ctx->fh, &ev);
+	} else {
+		ret = -EINVAL;
+	}
+	return ret;
 }
 
 /* Export DMA buffer */
@@ -809,6 +820,59 @@ static int vidioc_g_crop(struct file *file, void *priv,
 	return 0;
 }
 
+int vidioc_decoder_cmd(struct file *file, void *priv,
+						struct v4l2_decoder_cmd *cmd)
+{
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_buf *buf;
+	unsigned long flags;
+
+	switch (cmd->cmd) {
+	case V4L2_ENC_CMD_STOP:
+		if (cmd->flags != 0)
+			return -EINVAL;
+
+		if (!ctx->vq_src.streaming)
+			return -EINVAL;
+
+		spin_lock_irqsave(&dev->irqlock, flags);
+		if (list_empty(&ctx->src_queue)) {
+			mfc_err("EOS: empty src queue, entering finishing state");
+			ctx->state = MFCINST_FINISHING;
+			if (s5p_mfc_ctx_ready(ctx))
+				set_work_bit_irqsave(ctx);
+			spin_unlock_irqrestore(&dev->irqlock, flags);
+			s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
+		} else {
+			mfc_err("EOS: marking last buffer of stream");
+			buf = list_entry(ctx->src_queue.prev,
+						struct s5p_mfc_buf, list);
+			if (buf->flags & MFC_BUF_FLAG_USED)
+				ctx->state = MFCINST_FINISHING;
+			else
+				buf->flags |= MFC_BUF_FLAG_EOS;
+			spin_unlock_irqrestore(&dev->irqlock, flags);
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int vidioc_subscribe_event(struct v4l2_fh *fh,
+				const struct  v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_EOS:
+		return v4l2_event_subscribe(fh, sub, 2, NULL);
+	default:
+		return -EINVAL;
+	}
+}
+
+
 /* v4l2_ioctl_ops */
 static const struct v4l2_ioctl_ops s5p_mfc_dec_ioctl_ops = {
 	.vidioc_querycap = vidioc_querycap,
@@ -830,6 +894,9 @@ static const struct v4l2_ioctl_ops s5p_mfc_dec_ioctl_ops = {
 	.vidioc_streamon = vidioc_streamon,
 	.vidioc_streamoff = vidioc_streamoff,
 	.vidioc_g_crop = vidioc_g_crop,
+	.vidioc_decoder_cmd = vidioc_decoder_cmd,
+	.vidioc_subscribe_event = vidioc_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 static int s5p_mfc_queue_setup(struct vb2_queue *vq,
@@ -1147,3 +1214,4 @@ void s5p_mfc_dec_init(struct s5p_mfc_ctx *ctx)
 	mfc_debug(2, "Default src_fmt is %x, dest_fmt is %x\n",
 			(unsigned int)ctx->src_fmt, (unsigned int)ctx->dst_fmt);
 }
+
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
index 2356fd52a16..4f6b553c4b2 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
@@ -232,6 +232,7 @@ static struct mfc_control controls[] = {
 		.minimum = 0,
 		.maximum = 1,
 		.default_value = 0,
+		.step = 1,
 		.menu_skip_mask = 0,
 	},
 	{
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
index f61dba83789..0af05a2d1cd 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
@@ -34,7 +34,7 @@
 #define OFFSETB(x)		(((x) - dev->bank2) >> MFC_OFFSET_SHIFT)
 
 /* Allocate temporary buffers for decoding */
-int s5p_mfc_alloc_dec_temp_buffers_v5(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_alloc_dec_temp_buffers_v5(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 	struct s5p_mfc_buf_size_v5 *buf_size = dev->variant->buf_size->priv;
@@ -55,13 +55,13 @@ int s5p_mfc_alloc_dec_temp_buffers_v5(struct s5p_mfc_ctx *ctx)
 
 
 /* Release temporary buffers for decoding */
-void s5p_mfc_release_dec_desc_buffer_v5(struct s5p_mfc_ctx *ctx)
+static void s5p_mfc_release_dec_desc_buffer_v5(struct s5p_mfc_ctx *ctx)
 {
 	s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->dsc);
 }
 
 /* Allocate codec buffers */
-int s5p_mfc_alloc_codec_buffers_v5(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_alloc_codec_buffers_v5(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 	unsigned int enc_ref_y_size = 0;
@@ -193,14 +193,14 @@ int s5p_mfc_alloc_codec_buffers_v5(struct s5p_mfc_ctx *ctx)
 }
 
 /* Release buffers allocated for codec */
-void s5p_mfc_release_codec_buffers_v5(struct s5p_mfc_ctx *ctx)
+static void s5p_mfc_release_codec_buffers_v5(struct s5p_mfc_ctx *ctx)
 {
 	s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->bank1);
 	s5p_mfc_release_priv_buf(ctx->dev->mem_dev_r, &ctx->bank2);
 }
 
 /* Allocate memory for instance data buffer */
-int s5p_mfc_alloc_instance_buffer_v5(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_alloc_instance_buffer_v5(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 	struct s5p_mfc_buf_size_v5 *buf_size = dev->variant->buf_size->priv;
@@ -241,20 +241,20 @@ int s5p_mfc_alloc_instance_buffer_v5(struct s5p_mfc_ctx *ctx)
 }
 
 /* Release instance buffer */
-void s5p_mfc_release_instance_buffer_v5(struct s5p_mfc_ctx *ctx)
+static void s5p_mfc_release_instance_buffer_v5(struct s5p_mfc_ctx *ctx)
 {
 	s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->ctx);
 	s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->shm);
 }
 
-int s5p_mfc_alloc_dev_context_buffer_v5(struct s5p_mfc_dev *dev)
+static int s5p_mfc_alloc_dev_context_buffer_v5(struct s5p_mfc_dev *dev)
 {
 	/* NOP */
 
 	return 0;
 }
 
-void s5p_mfc_release_dev_context_buffer_v5(struct s5p_mfc_dev *dev)
+static void s5p_mfc_release_dev_context_buffer_v5(struct s5p_mfc_dev *dev)
 {
 	/* NOP */
 }
@@ -273,7 +273,7 @@ static unsigned int s5p_mfc_read_info_v5(struct s5p_mfc_ctx *ctx,
 	return readl(ctx->shm.virt + ofs);
 }
 
-void s5p_mfc_dec_calc_dpb_size_v5(struct s5p_mfc_ctx *ctx)
+static void s5p_mfc_dec_calc_dpb_size_v5(struct s5p_mfc_ctx *ctx)
 {
 	unsigned int guard_width, guard_height;
 
@@ -315,7 +315,7 @@ void s5p_mfc_dec_calc_dpb_size_v5(struct s5p_mfc_ctx *ctx)
 	}
 }
 
-void s5p_mfc_enc_calc_src_size_v5(struct s5p_mfc_ctx *ctx)
+static void s5p_mfc_enc_calc_src_size_v5(struct s5p_mfc_ctx *ctx)
 {
 	if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M) {
 		ctx->buf_width = ALIGN(ctx->img_width, S5P_FIMV_NV12M_HALIGN);
@@ -361,8 +361,9 @@ static void s5p_mfc_set_shared_buffer(struct s5p_mfc_ctx *ctx)
 }
 
 /* Set registers for decoding stream buffer */
-int s5p_mfc_set_dec_stream_buffer_v5(struct s5p_mfc_ctx *ctx, int buf_addr,
-		  unsigned int start_num_byte, unsigned int buf_size)
+static int s5p_mfc_set_dec_stream_buffer_v5(struct s5p_mfc_ctx *ctx,
+		int buf_addr, unsigned int start_num_byte,
+		unsigned int buf_size)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 
@@ -374,7 +375,7 @@ int s5p_mfc_set_dec_stream_buffer_v5(struct s5p_mfc_ctx *ctx, int buf_addr,
 }
 
 /* Set decoding frame buffer */
-int s5p_mfc_set_dec_frame_buffer_v5(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_set_dec_frame_buffer_v5(struct s5p_mfc_ctx *ctx)
 {
 	unsigned int frame_size, i;
 	unsigned int frame_size_ch, frame_size_mv;
@@ -506,7 +507,7 @@ int s5p_mfc_set_dec_frame_buffer_v5(struct s5p_mfc_ctx *ctx)
 }
 
 /* Set registers for encoding stream buffer */
-int s5p_mfc_set_enc_stream_buffer_v5(struct s5p_mfc_ctx *ctx,
+static int s5p_mfc_set_enc_stream_buffer_v5(struct s5p_mfc_ctx *ctx,
 		unsigned long addr, unsigned int size)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
@@ -516,7 +517,7 @@ int s5p_mfc_set_enc_stream_buffer_v5(struct s5p_mfc_ctx *ctx,
 	return 0;
 }
 
-void s5p_mfc_set_enc_frame_buffer_v5(struct s5p_mfc_ctx *ctx,
+static void s5p_mfc_set_enc_frame_buffer_v5(struct s5p_mfc_ctx *ctx,
 		unsigned long y_addr, unsigned long c_addr)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
@@ -525,7 +526,7 @@ void s5p_mfc_set_enc_frame_buffer_v5(struct s5p_mfc_ctx *ctx,
 	mfc_write(dev, OFFSETB(c_addr), S5P_FIMV_ENC_SI_CH0_CUR_C_ADR);
 }
 
-void s5p_mfc_get_enc_frame_buffer_v5(struct s5p_mfc_ctx *ctx,
+static void s5p_mfc_get_enc_frame_buffer_v5(struct s5p_mfc_ctx *ctx,
 		unsigned long *y_addr, unsigned long *c_addr)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
@@ -537,7 +538,7 @@ void s5p_mfc_get_enc_frame_buffer_v5(struct s5p_mfc_ctx *ctx,
 }
 
 /* Set encoding ref & codec buffer */
-int s5p_mfc_set_enc_ref_buffer_v5(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_set_enc_ref_buffer_v5(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 	size_t buf_addr1, buf_addr2;
@@ -1041,7 +1042,7 @@ static int s5p_mfc_set_enc_params_h263(struct s5p_mfc_ctx *ctx)
 }
 
 /* Initialize decoding */
-int s5p_mfc_init_decode_v5(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_init_decode_v5(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 
@@ -1077,7 +1078,7 @@ static void s5p_mfc_set_flush(struct s5p_mfc_ctx *ctx, int flush)
 }
 
 /* Decode a single frame */
-int s5p_mfc_decode_one_frame_v5(struct s5p_mfc_ctx *ctx,
+static int s5p_mfc_decode_one_frame_v5(struct s5p_mfc_ctx *ctx,
 					enum s5p_mfc_decode_arg last_frame)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
@@ -1106,7 +1107,7 @@ int s5p_mfc_decode_one_frame_v5(struct s5p_mfc_ctx *ctx,
 	return 0;
 }
 
-int s5p_mfc_init_encode_v5(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_init_encode_v5(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 
@@ -1128,7 +1129,7 @@ int s5p_mfc_init_encode_v5(struct s5p_mfc_ctx *ctx)
 }
 
 /* Encode a single frame */
-int s5p_mfc_encode_one_frame_v5(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_encode_one_frame_v5(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 	int cmd;
@@ -1187,6 +1188,15 @@ static int s5p_mfc_run_dec_frame(struct s5p_mfc_ctx *ctx, int last_frame)
 	unsigned long flags;
 	unsigned int index;
 
+	if (ctx->state == MFCINST_FINISHING) {
+		last_frame = MFC_DEC_LAST_FRAME;
+		s5p_mfc_set_dec_stream_buffer_v5(ctx, 0, 0, 0);
+		dev->curr_ctx = ctx->num;
+		s5p_mfc_clean_ctx_int_flags(ctx);
+		s5p_mfc_decode_one_frame_v5(ctx, last_frame);
+		return 0;
+	}
+
 	spin_lock_irqsave(&dev->irqlock, flags);
 	/* Frames are being decoded */
 	if (list_empty(&ctx->src_queue)) {
@@ -1353,7 +1363,7 @@ static int s5p_mfc_run_init_dec_buffers(struct s5p_mfc_ctx *ctx)
 }
 
 /* Try running an operation on hardware */
-void s5p_mfc_try_run_v5(struct s5p_mfc_dev *dev)
+static void s5p_mfc_try_run_v5(struct s5p_mfc_dev *dev)
 {
 	struct s5p_mfc_ctx *ctx;
 	int new_ctx;
@@ -1469,7 +1479,7 @@ void s5p_mfc_try_run_v5(struct s5p_mfc_dev *dev)
 }
 
 
-void s5p_mfc_cleanup_queue_v5(struct list_head *lh, struct vb2_queue *vq)
+static void s5p_mfc_cleanup_queue_v5(struct list_head *lh, struct vb2_queue *vq)
 {
 	struct s5p_mfc_buf *b;
 	int i;
@@ -1483,52 +1493,52 @@ void s5p_mfc_cleanup_queue_v5(struct list_head *lh, struct vb2_queue *vq)
 	}
 }
 
-void s5p_mfc_clear_int_flags_v5(struct s5p_mfc_dev *dev)
+static void s5p_mfc_clear_int_flags_v5(struct s5p_mfc_dev *dev)
 {
 	mfc_write(dev, 0, S5P_FIMV_RISC_HOST_INT);
 	mfc_write(dev, 0, S5P_FIMV_RISC2HOST_CMD);
 	mfc_write(dev, 0xffff, S5P_FIMV_SI_RTN_CHID);
 }
 
-int s5p_mfc_get_dspl_y_adr_v5(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_dspl_y_adr_v5(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_SI_DISPLAY_Y_ADR) << MFC_OFFSET_SHIFT;
 }
 
-int s5p_mfc_get_dec_y_adr_v5(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_dec_y_adr_v5(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_SI_DECODE_Y_ADR) << MFC_OFFSET_SHIFT;
 }
 
-int s5p_mfc_get_dspl_status_v5(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_dspl_status_v5(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_SI_DISPLAY_STATUS);
 }
 
-int s5p_mfc_get_dec_status_v5(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_dec_status_v5(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_SI_DECODE_STATUS);
 }
 
-int s5p_mfc_get_dec_frame_type_v5(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_dec_frame_type_v5(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_DECODE_FRAME_TYPE) &
 		S5P_FIMV_DECODE_FRAME_MASK;
 }
 
-int s5p_mfc_get_disp_frame_type_v5(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_get_disp_frame_type_v5(struct s5p_mfc_ctx *ctx)
 {
 	return (s5p_mfc_read_info_v5(ctx, DISP_PIC_FRAME_TYPE) >>
 			S5P_FIMV_SHARED_DISP_FRAME_TYPE_SHIFT) &
 			S5P_FIMV_DECODE_FRAME_MASK;
 }
 
-int s5p_mfc_get_consumed_stream_v5(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_consumed_stream_v5(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_SI_CONSUMED_BYTES);
 }
 
-int s5p_mfc_get_int_reason_v5(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_int_reason_v5(struct s5p_mfc_dev *dev)
 {
 	int reason;
 	reason = mfc_read(dev, S5P_FIMV_RISC2HOST_CMD) &
@@ -1576,98 +1586,98 @@ int s5p_mfc_get_int_reason_v5(struct s5p_mfc_dev *dev)
 	return reason;
 }
 
-int s5p_mfc_get_int_err_v5(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_int_err_v5(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_RISC2HOST_ARG2);
 }
 
-int s5p_mfc_err_dec_v5(unsigned int err)
+static int s5p_mfc_err_dec_v5(unsigned int err)
 {
 	return (err & S5P_FIMV_ERR_DEC_MASK) >> S5P_FIMV_ERR_DEC_SHIFT;
 }
 
-int s5p_mfc_err_dspl_v5(unsigned int err)
+static int s5p_mfc_err_dspl_v5(unsigned int err)
 {
 	return (err & S5P_FIMV_ERR_DSPL_MASK) >> S5P_FIMV_ERR_DSPL_SHIFT;
 }
 
-int s5p_mfc_get_img_width_v5(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_img_width_v5(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_SI_HRESOL);
 }
 
-int s5p_mfc_get_img_height_v5(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_img_height_v5(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_SI_VRESOL);
 }
 
-int s5p_mfc_get_dpb_count_v5(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_dpb_count_v5(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_SI_BUF_NUMBER);
 }
 
-int s5p_mfc_get_mv_count_v5(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_mv_count_v5(struct s5p_mfc_dev *dev)
 {
 	/* NOP */
 	return -1;
 }
 
-int s5p_mfc_get_inst_no_v5(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_inst_no_v5(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_RISC2HOST_ARG1);
 }
 
-int s5p_mfc_get_enc_strm_size_v5(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_enc_strm_size_v5(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_ENC_SI_STRM_SIZE);
 }
 
-int s5p_mfc_get_enc_slice_type_v5(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_enc_slice_type_v5(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_ENC_SI_SLICE_TYPE);
 }
 
-int s5p_mfc_get_enc_dpb_count_v5(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_enc_dpb_count_v5(struct s5p_mfc_dev *dev)
 {
 	return -1;
 }
 
-int s5p_mfc_get_enc_pic_count_v5(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_enc_pic_count_v5(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_ENC_SI_PIC_CNT);
 }
 
-int s5p_mfc_get_sei_avail_status_v5(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_get_sei_avail_status_v5(struct s5p_mfc_ctx *ctx)
 {
 	return s5p_mfc_read_info_v5(ctx, FRAME_PACK_SEI_AVAIL);
 }
 
-int s5p_mfc_get_mvc_num_views_v5(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_mvc_num_views_v5(struct s5p_mfc_dev *dev)
 {
 	return -1;
 }
 
-int s5p_mfc_get_mvc_view_id_v5(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_mvc_view_id_v5(struct s5p_mfc_dev *dev)
 {
 	return -1;
 }
 
-unsigned int s5p_mfc_get_pic_type_top_v5(struct s5p_mfc_ctx *ctx)
+static unsigned int s5p_mfc_get_pic_type_top_v5(struct s5p_mfc_ctx *ctx)
 {
 	return s5p_mfc_read_info_v5(ctx, PIC_TIME_TOP);
 }
 
-unsigned int s5p_mfc_get_pic_type_bot_v5(struct s5p_mfc_ctx *ctx)
+static unsigned int s5p_mfc_get_pic_type_bot_v5(struct s5p_mfc_ctx *ctx)
 {
 	return s5p_mfc_read_info_v5(ctx, PIC_TIME_BOT);
 }
 
-unsigned int s5p_mfc_get_crop_info_h_v5(struct s5p_mfc_ctx *ctx)
+static unsigned int s5p_mfc_get_crop_info_h_v5(struct s5p_mfc_ctx *ctx)
 {
 	return s5p_mfc_read_info_v5(ctx, CROP_INFO_H);
 }
 
-unsigned int s5p_mfc_get_crop_info_v_v5(struct s5p_mfc_ctx *ctx)
+static unsigned int s5p_mfc_get_crop_info_v_v5(struct s5p_mfc_ctx *ctx)
 {
 	return s5p_mfc_read_info_v5(ctx, CROP_INFO_V);
 }
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
index beb6dbacebd..7e76fce2e52 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
@@ -49,7 +49,7 @@
 #define OFFSETB(x)		(((x) - dev->port_b) >> S5P_FIMV_MEM_OFFSET)
 
 /* Allocate temporary buffers for decoding */
-int s5p_mfc_alloc_dec_temp_buffers_v6(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_alloc_dec_temp_buffers_v6(struct s5p_mfc_ctx *ctx)
 {
 	/* NOP */
 
@@ -57,19 +57,19 @@ int s5p_mfc_alloc_dec_temp_buffers_v6(struct s5p_mfc_ctx *ctx)
 }
 
 /* Release temproary buffers for decoding */
-void s5p_mfc_release_dec_desc_buffer_v6(struct s5p_mfc_ctx *ctx)
+static void s5p_mfc_release_dec_desc_buffer_v6(struct s5p_mfc_ctx *ctx)
 {
 	/* NOP */
 }
 
-int s5p_mfc_get_dec_status_v6(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_dec_status_v6(struct s5p_mfc_dev *dev)
 {
 	/* NOP */
 	return -1;
 }
 
 /* Allocate codec buffers */
-int s5p_mfc_alloc_codec_buffers_v6(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_alloc_codec_buffers_v6(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 	unsigned int mb_width, mb_height;
@@ -203,13 +203,13 @@ int s5p_mfc_alloc_codec_buffers_v6(struct s5p_mfc_ctx *ctx)
 }
 
 /* Release buffers allocated for codec */
-void s5p_mfc_release_codec_buffers_v6(struct s5p_mfc_ctx *ctx)
+static void s5p_mfc_release_codec_buffers_v6(struct s5p_mfc_ctx *ctx)
 {
 	s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->bank1);
 }
 
 /* Allocate memory for instance data buffer */
-int s5p_mfc_alloc_instance_buffer_v6(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_alloc_instance_buffer_v6(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 	struct s5p_mfc_buf_size_v6 *buf_size = dev->variant->buf_size->priv;
@@ -258,13 +258,13 @@ int s5p_mfc_alloc_instance_buffer_v6(struct s5p_mfc_ctx *ctx)
 }
 
 /* Release instance buffer */
-void s5p_mfc_release_instance_buffer_v6(struct s5p_mfc_ctx *ctx)
+static void s5p_mfc_release_instance_buffer_v6(struct s5p_mfc_ctx *ctx)
 {
 	s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->ctx);
 }
 
 /* Allocate context buffers for SYS_INIT */
-int s5p_mfc_alloc_dev_context_buffer_v6(struct s5p_mfc_dev *dev)
+static int s5p_mfc_alloc_dev_context_buffer_v6(struct s5p_mfc_dev *dev)
 {
 	struct s5p_mfc_buf_size_v6 *buf_size = dev->variant->buf_size->priv;
 	int ret;
@@ -287,7 +287,7 @@ int s5p_mfc_alloc_dev_context_buffer_v6(struct s5p_mfc_dev *dev)
 }
 
 /* Release context buffers for SYS_INIT */
-void s5p_mfc_release_dev_context_buffer_v6(struct s5p_mfc_dev *dev)
+static void s5p_mfc_release_dev_context_buffer_v6(struct s5p_mfc_dev *dev)
 {
 	s5p_mfc_release_priv_buf(dev->mem_dev_l, &dev->ctx_buf);
 }
@@ -306,7 +306,7 @@ static int calc_plane(int width, int height)
 		(mbY * S5P_FIMV_NUM_PIXELS_IN_MB_ROW_V6);
 }
 
-void s5p_mfc_dec_calc_dpb_size_v6(struct s5p_mfc_ctx *ctx)
+static void s5p_mfc_dec_calc_dpb_size_v6(struct s5p_mfc_ctx *ctx)
 {
 	ctx->buf_width = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN_V6);
 	ctx->buf_height = ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN_V6);
@@ -326,7 +326,7 @@ void s5p_mfc_dec_calc_dpb_size_v6(struct s5p_mfc_ctx *ctx)
 	}
 }
 
-void s5p_mfc_enc_calc_src_size_v6(struct s5p_mfc_ctx *ctx)
+static void s5p_mfc_enc_calc_src_size_v6(struct s5p_mfc_ctx *ctx)
 {
 	unsigned int mb_width, mb_height;
 
@@ -339,8 +339,9 @@ void s5p_mfc_enc_calc_src_size_v6(struct s5p_mfc_ctx *ctx)
 }
 
 /* Set registers for decoding stream buffer */
-int s5p_mfc_set_dec_stream_buffer_v6(struct s5p_mfc_ctx *ctx, int buf_addr,
-		  unsigned int start_num_byte, unsigned int strm_size)
+static int s5p_mfc_set_dec_stream_buffer_v6(struct s5p_mfc_ctx *ctx,
+			int buf_addr, unsigned int start_num_byte,
+			unsigned int strm_size)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 	struct s5p_mfc_buf_size *buf_size = dev->variant->buf_size;
@@ -359,7 +360,7 @@ int s5p_mfc_set_dec_stream_buffer_v6(struct s5p_mfc_ctx *ctx, int buf_addr,
 }
 
 /* Set decoding frame buffer */
-int s5p_mfc_set_dec_frame_buffer_v6(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_set_dec_frame_buffer_v6(struct s5p_mfc_ctx *ctx)
 {
 	unsigned int frame_size, i;
 	unsigned int frame_size_ch, frame_size_mv;
@@ -440,7 +441,7 @@ int s5p_mfc_set_dec_frame_buffer_v6(struct s5p_mfc_ctx *ctx)
 }
 
 /* Set registers for encoding stream buffer */
-int s5p_mfc_set_enc_stream_buffer_v6(struct s5p_mfc_ctx *ctx,
+static int s5p_mfc_set_enc_stream_buffer_v6(struct s5p_mfc_ctx *ctx,
 		unsigned long addr, unsigned int size)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
@@ -454,7 +455,7 @@ int s5p_mfc_set_enc_stream_buffer_v6(struct s5p_mfc_ctx *ctx,
 	return 0;
 }
 
-void s5p_mfc_set_enc_frame_buffer_v6(struct s5p_mfc_ctx *ctx,
+static void s5p_mfc_set_enc_frame_buffer_v6(struct s5p_mfc_ctx *ctx,
 		unsigned long y_addr, unsigned long c_addr)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
@@ -466,7 +467,7 @@ void s5p_mfc_set_enc_frame_buffer_v6(struct s5p_mfc_ctx *ctx,
 	mfc_debug(2, "enc src c buf addr: 0x%08lx", c_addr);
 }
 
-void s5p_mfc_get_enc_frame_buffer_v6(struct s5p_mfc_ctx *ctx,
+static void s5p_mfc_get_enc_frame_buffer_v6(struct s5p_mfc_ctx *ctx,
 		unsigned long *y_addr, unsigned long *c_addr)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
@@ -483,7 +484,7 @@ void s5p_mfc_get_enc_frame_buffer_v6(struct s5p_mfc_ctx *ctx,
 }
 
 /* Set encoding ref & codec buffer */
-int s5p_mfc_set_enc_ref_buffer_v6(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_set_enc_ref_buffer_v6(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 	size_t buf_addr1;
@@ -1147,7 +1148,7 @@ static int s5p_mfc_set_enc_params_h263(struct s5p_mfc_ctx *ctx)
 }
 
 /* Initialize decoding */
-int s5p_mfc_init_decode_v6(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_init_decode_v6(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 	unsigned int reg = 0;
@@ -1215,7 +1216,7 @@ static inline void s5p_mfc_set_flush(struct s5p_mfc_ctx *ctx, int flush)
 }
 
 /* Decode a single frame */
-int s5p_mfc_decode_one_frame_v6(struct s5p_mfc_ctx *ctx,
+static int s5p_mfc_decode_one_frame_v6(struct s5p_mfc_ctx *ctx,
 			enum s5p_mfc_decode_arg last_frame)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
@@ -1244,7 +1245,7 @@ int s5p_mfc_decode_one_frame_v6(struct s5p_mfc_ctx *ctx,
 	return 0;
 }
 
-int s5p_mfc_init_encode_v6(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_init_encode_v6(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 
@@ -1267,7 +1268,7 @@ int s5p_mfc_init_encode_v6(struct s5p_mfc_ctx *ctx)
 	return 0;
 }
 
-int s5p_mfc_h264_set_aso_slice_order_v6(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_h264_set_aso_slice_order_v6(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 	struct s5p_mfc_enc_params *p = &ctx->enc_params;
@@ -1283,7 +1284,7 @@ int s5p_mfc_h264_set_aso_slice_order_v6(struct s5p_mfc_ctx *ctx)
 }
 
 /* Encode a single frame */
-int s5p_mfc_encode_one_frame_v6(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_encode_one_frame_v6(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 
@@ -1312,7 +1313,7 @@ static inline int s5p_mfc_get_new_ctx(struct s5p_mfc_dev *dev)
 	int cnt;
 
 	spin_lock_irqsave(&dev->condlock, flags);
-	mfc_debug(2, "Previos context: %d (bits %08lx)\n", dev->curr_ctx,
+	mfc_debug(2, "Previous context: %d (bits %08lx)\n", dev->curr_ctx,
 							dev->ctx_work_bits);
 	new_ctx = (dev->curr_ctx + 1) % MFC_NUM_CONTEXTS;
 	cnt = 0;
@@ -1362,8 +1363,16 @@ static inline int s5p_mfc_run_dec_frame(struct s5p_mfc_ctx *ctx)
 	unsigned long flags;
 	int last_frame = 0;
 
-	spin_lock_irqsave(&dev->irqlock, flags);
+	if (ctx->state == MFCINST_FINISHING) {
+		last_frame = MFC_DEC_LAST_FRAME;
+		s5p_mfc_set_dec_stream_buffer_v6(ctx, 0, 0, 0);
+		dev->curr_ctx = ctx->num;
+		s5p_mfc_clean_ctx_int_flags(ctx);
+		s5p_mfc_decode_one_frame_v6(ctx, last_frame);
+		return 0;
+	}
 
+	spin_lock_irqsave(&dev->irqlock, flags);
 	/* Frames are being decoded */
 	if (list_empty(&ctx->src_queue)) {
 		mfc_debug(2, "No src buffers.\n");
@@ -1540,7 +1549,7 @@ static inline int s5p_mfc_run_init_enc_buffers(struct s5p_mfc_ctx *ctx)
 }
 
 /* Try running an operation on hardware */
-void s5p_mfc_try_run_v6(struct s5p_mfc_dev *dev)
+static void s5p_mfc_try_run_v6(struct s5p_mfc_dev *dev)
 {
 	struct s5p_mfc_ctx *ctx;
 	int new_ctx;
@@ -1663,7 +1672,7 @@ void s5p_mfc_try_run_v6(struct s5p_mfc_dev *dev)
 }
 
 
-void s5p_mfc_cleanup_queue_v6(struct list_head *lh, struct vb2_queue *vq)
+static void s5p_mfc_cleanup_queue_v6(struct list_head *lh, struct vb2_queue *vq)
 {
 	struct s5p_mfc_buf *b;
 	int i;
@@ -1677,13 +1686,13 @@ void s5p_mfc_cleanup_queue_v6(struct list_head *lh, struct vb2_queue *vq)
 	}
 }
 
-void s5p_mfc_clear_int_flags_v6(struct s5p_mfc_dev *dev)
+static void s5p_mfc_clear_int_flags_v6(struct s5p_mfc_dev *dev)
 {
 	mfc_write(dev, 0, S5P_FIMV_RISC2HOST_CMD_V6);
 	mfc_write(dev, 0, S5P_FIMV_RISC2HOST_INT_V6);
 }
 
-void s5p_mfc_write_info_v6(struct s5p_mfc_ctx *ctx, unsigned int data,
+static void s5p_mfc_write_info_v6(struct s5p_mfc_ctx *ctx, unsigned int data,
 		unsigned int ofs)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
@@ -1693,7 +1702,8 @@ void s5p_mfc_write_info_v6(struct s5p_mfc_ctx *ctx, unsigned int data,
 	s5p_mfc_clock_off();
 }
 
-unsigned int s5p_mfc_read_info_v6(struct s5p_mfc_ctx *ctx, unsigned int ofs)
+static unsigned int
+s5p_mfc_read_info_v6(struct s5p_mfc_ctx *ctx, unsigned int ofs)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 	int ret;
@@ -1705,140 +1715,140 @@ unsigned int s5p_mfc_read_info_v6(struct s5p_mfc_ctx *ctx, unsigned int ofs)
 	return ret;
 }
 
-int s5p_mfc_get_dspl_y_adr_v6(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_dspl_y_adr_v6(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_D_DISPLAY_LUMA_ADDR_V6);
 }
 
-int s5p_mfc_get_dec_y_adr_v6(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_dec_y_adr_v6(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_D_DECODED_LUMA_ADDR_V6);
 }
 
-int s5p_mfc_get_dspl_status_v6(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_dspl_status_v6(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_D_DISPLAY_STATUS_V6);
 }
 
-int s5p_mfc_get_decoded_status_v6(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_decoded_status_v6(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_D_DECODED_STATUS_V6);
 }
 
-int s5p_mfc_get_dec_frame_type_v6(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_dec_frame_type_v6(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_D_DECODED_FRAME_TYPE_V6) &
 		S5P_FIMV_DECODE_FRAME_MASK_V6;
 }
 
-int s5p_mfc_get_disp_frame_type_v6(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_get_disp_frame_type_v6(struct s5p_mfc_ctx *ctx)
 {
 	return mfc_read(ctx->dev, S5P_FIMV_D_DISPLAY_FRAME_TYPE_V6) &
 		S5P_FIMV_DECODE_FRAME_MASK_V6;
 }
 
-int s5p_mfc_get_consumed_stream_v6(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_consumed_stream_v6(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_D_DECODED_NAL_SIZE_V6);
 }
 
-int s5p_mfc_get_int_reason_v6(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_int_reason_v6(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_RISC2HOST_CMD_V6) &
 		S5P_FIMV_RISC2HOST_CMD_MASK;
 }
 
-int s5p_mfc_get_int_err_v6(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_int_err_v6(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_ERROR_CODE_V6);
 }
 
-int s5p_mfc_err_dec_v6(unsigned int err)
+static int s5p_mfc_err_dec_v6(unsigned int err)
 {
 	return (err & S5P_FIMV_ERR_DEC_MASK_V6) >> S5P_FIMV_ERR_DEC_SHIFT_V6;
 }
 
-int s5p_mfc_err_dspl_v6(unsigned int err)
+static int s5p_mfc_err_dspl_v6(unsigned int err)
 {
 	return (err & S5P_FIMV_ERR_DSPL_MASK_V6) >> S5P_FIMV_ERR_DSPL_SHIFT_V6;
 }
 
-int s5p_mfc_get_img_width_v6(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_img_width_v6(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_D_DISPLAY_FRAME_WIDTH_V6);
 }
 
-int s5p_mfc_get_img_height_v6(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_img_height_v6(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_D_DISPLAY_FRAME_HEIGHT_V6);
 }
 
-int s5p_mfc_get_dpb_count_v6(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_dpb_count_v6(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_D_MIN_NUM_DPB_V6);
 }
 
-int s5p_mfc_get_mv_count_v6(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_mv_count_v6(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_D_MIN_NUM_MV_V6);
 }
 
-int s5p_mfc_get_inst_no_v6(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_inst_no_v6(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_RET_INSTANCE_ID_V6);
 }
 
-int s5p_mfc_get_enc_dpb_count_v6(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_enc_dpb_count_v6(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_E_NUM_DPB_V6);
 }
 
-int s5p_mfc_get_enc_strm_size_v6(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_enc_strm_size_v6(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_E_STREAM_SIZE_V6);
 }
 
-int s5p_mfc_get_enc_slice_type_v6(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_enc_slice_type_v6(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_E_SLICE_TYPE_V6);
 }
 
-int s5p_mfc_get_enc_pic_count_v6(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_enc_pic_count_v6(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_E_PICTURE_COUNT_V6);
 }
 
-int s5p_mfc_get_sei_avail_status_v6(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_get_sei_avail_status_v6(struct s5p_mfc_ctx *ctx)
 {
 	return mfc_read(ctx->dev, S5P_FIMV_D_FRAME_PACK_SEI_AVAIL_V6);
 }
 
-int s5p_mfc_get_mvc_num_views_v6(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_mvc_num_views_v6(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_D_MVC_NUM_VIEWS_V6);
 }
 
-int s5p_mfc_get_mvc_view_id_v6(struct s5p_mfc_dev *dev)
+static int s5p_mfc_get_mvc_view_id_v6(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_D_MVC_VIEW_ID_V6);
 }
 
-unsigned int s5p_mfc_get_pic_type_top_v6(struct s5p_mfc_ctx *ctx)
+static unsigned int s5p_mfc_get_pic_type_top_v6(struct s5p_mfc_ctx *ctx)
 {
 	return s5p_mfc_read_info_v6(ctx, PIC_TIME_TOP_V6);
 }
 
-unsigned int s5p_mfc_get_pic_type_bot_v6(struct s5p_mfc_ctx *ctx)
+static unsigned int s5p_mfc_get_pic_type_bot_v6(struct s5p_mfc_ctx *ctx)
 {
 	return s5p_mfc_read_info_v6(ctx, PIC_TIME_BOT_V6);
 }
 
-unsigned int s5p_mfc_get_crop_info_h_v6(struct s5p_mfc_ctx *ctx)
+static unsigned int s5p_mfc_get_crop_info_h_v6(struct s5p_mfc_ctx *ctx)
 {
 	return s5p_mfc_read_info_v6(ctx, CROP_INFO_H_V6);
 }
 
-unsigned int s5p_mfc_get_crop_info_v_v6(struct s5p_mfc_ctx *ctx)
+static unsigned int s5p_mfc_get_crop_info_v_v6(struct s5p_mfc_ctx *ctx)
 {
 	return s5p_mfc_read_info_v6(ctx, CROP_INFO_V_V6);
 }
diff --git a/drivers/media/platform/s5p-tv/hdmi_drv.c b/drivers/media/platform/s5p-tv/hdmi_drv.c
index 8de1b3dce45..4e86626dad4 100644
--- a/drivers/media/platform/s5p-tv/hdmi_drv.c
+++ b/drivers/media/platform/s5p-tv/hdmi_drv.c
@@ -31,6 +31,7 @@
 #include <linux/pm_runtime.h>
 #include <linux/clk.h>
 #include <linux/regulator/consumer.h>
+#include <linux/v4l2-dv-timings.h>
 
 #include <media/s5p_hdmi.h>
 #include <media/v4l2-common.h>
@@ -43,9 +44,6 @@ MODULE_AUTHOR("Tomasz Stanislawski, <t.stanislaws@samsung.com>");
 MODULE_DESCRIPTION("Samsung HDMI");
 MODULE_LICENSE("GPL");
 
-/* default preset configured on probe */
-#define HDMI_DEFAULT_PRESET V4L2_DV_480P59_94
-
 struct hdmi_pulse {
 	u32 beg;
 	u32 end;
@@ -91,8 +89,8 @@ struct hdmi_device {
 	const struct hdmi_timings *cur_conf;
 	/** flag indicating that timings are dirty */
 	int cur_conf_dirty;
-	/** current preset */
-	u32 cur_preset;
+	/** current timings */
+	struct v4l2_dv_timings cur_timings;
 	/** other resources */
 	struct hdmi_resources res;
 };
@@ -252,7 +250,6 @@ static int hdmi_conf_apply(struct hdmi_device *hdmi_dev)
 {
 	struct device *dev = hdmi_dev->dev;
 	const struct hdmi_timings *conf = hdmi_dev->cur_conf;
-	struct v4l2_dv_preset preset;
 	int ret;
 
 	dev_dbg(dev, "%s\n", __func__);
@@ -267,11 +264,11 @@ static int hdmi_conf_apply(struct hdmi_device *hdmi_dev)
 	hdmi_write_mask(hdmi_dev, HDMI_PHY_RSTOUT,  0, HDMI_PHY_SW_RSTOUT);
 	mdelay(10);
 
-	/* configure presets */
-	preset.preset = hdmi_dev->cur_preset;
-	ret = v4l2_subdev_call(hdmi_dev->phy_sd, video, s_dv_preset, &preset);
+	/* configure timings */
+	ret = v4l2_subdev_call(hdmi_dev->phy_sd, video, s_dv_timings,
+				&hdmi_dev->cur_timings);
 	if (ret) {
-		dev_err(dev, "failed to set preset (%u)\n", preset.preset);
+		dev_err(dev, "failed to set timings\n");
 		return ret;
 	}
 
@@ -475,33 +472,26 @@ static const struct hdmi_timings hdmi_timings_1080p50 = {
 	.vsyn[0] = { .beg = 0 + 4, .end = 5 + 4},
 };
 
+/* default hdmi_timings index of the timings configured on probe */
+#define HDMI_DEFAULT_TIMINGS_IDX (0)
+
 static const struct {
-	u32 preset;
-	const struct hdmi_timings *timings;
+	bool reduced_fps;
+	const struct v4l2_dv_timings dv_timings;
+	const struct hdmi_timings *hdmi_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 },
+	{ false, V4L2_DV_BT_CEA_720X480P59_94, &hdmi_timings_480p    },
+	{ false, V4L2_DV_BT_CEA_720X576P50,    &hdmi_timings_576p50  },
+	{ false, V4L2_DV_BT_CEA_1280X720P50,   &hdmi_timings_720p50  },
+	{ true,  V4L2_DV_BT_CEA_1280X720P60,   &hdmi_timings_720p60  },
+	{ false, V4L2_DV_BT_CEA_1920X1080P24,  &hdmi_timings_1080p24 },
+	{ false, V4L2_DV_BT_CEA_1920X1080P30,  &hdmi_timings_1080p60 },
+	{ false, V4L2_DV_BT_CEA_1920X1080P50,  &hdmi_timings_1080p50 },
+	{ false, V4L2_DV_BT_CEA_1920X1080I50,  &hdmi_timings_1080i50 },
+	{ false, V4L2_DV_BT_CEA_1920X1080I60,  &hdmi_timings_1080i60 },
+	{ false, V4L2_DV_BT_CEA_1920X1080P60,  &hdmi_timings_1080p60 },
 };
 
-static const struct hdmi_timings *hdmi_preset2timings(u32 preset)
-{
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(hdmi_timings); ++i)
-		if (hdmi_timings[i].preset == preset)
-			return  hdmi_timings[i].timings;
-	return NULL;
-}
-
 static int hdmi_streamon(struct hdmi_device *hdev)
 {
 	struct device *dev = hdev->dev;
@@ -621,29 +611,33 @@ static int hdmi_s_power(struct v4l2_subdev *sd, int on)
 	return IS_ERR_VALUE(ret) ? ret : 0;
 }
 
-static int hdmi_s_dv_preset(struct v4l2_subdev *sd,
-	struct v4l2_dv_preset *preset)
+static int hdmi_s_dv_timings(struct v4l2_subdev *sd,
+	struct v4l2_dv_timings *timings)
 {
 	struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
 	struct device *dev = hdev->dev;
-	const struct hdmi_timings *conf;
+	int i;
 
-	conf = hdmi_preset2timings(preset->preset);
-	if (conf == NULL) {
-		dev_err(dev, "preset (%u) not supported\n", preset->preset);
+	for (i = 0; i < ARRAY_SIZE(hdmi_timings); i++)
+		if (v4l_match_dv_timings(&hdmi_timings[i].dv_timings,
+					timings, 0))
+			break;
+	if (i == ARRAY_SIZE(hdmi_timings)) {
+		dev_err(dev, "timings not supported\n");
 		return -EINVAL;
 	}
-	hdev->cur_conf = conf;
+	hdev->cur_conf = hdmi_timings[i].hdmi_timings;
 	hdev->cur_conf_dirty = 1;
-	hdev->cur_preset = preset->preset;
+	hdev->cur_timings = *timings;
+	if (!hdmi_timings[i].reduced_fps)
+		hdev->cur_timings.bt.flags &= ~V4L2_DV_FL_CAN_REDUCE_FPS;
 	return 0;
 }
 
-static int hdmi_g_dv_preset(struct v4l2_subdev *sd,
-	struct v4l2_dv_preset *preset)
+static int hdmi_g_dv_timings(struct v4l2_subdev *sd,
+	struct v4l2_dv_timings *timings)
 {
-	memset(preset, 0, sizeof(*preset));
-	preset->preset = sd_to_hdmi_dev(sd)->cur_preset;
+	*timings = sd_to_hdmi_dev(sd)->cur_timings;
 	return 0;
 }
 
@@ -670,13 +664,33 @@ static int hdmi_g_mbus_fmt(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int hdmi_enum_dv_presets(struct v4l2_subdev *sd,
-	struct v4l2_dv_enum_preset *preset)
+static int hdmi_enum_dv_timings(struct v4l2_subdev *sd,
+	struct v4l2_enum_dv_timings *timings)
 {
-	if (preset->index >= ARRAY_SIZE(hdmi_timings))
+	if (timings->index >= ARRAY_SIZE(hdmi_timings))
 		return -EINVAL;
-	return v4l_fill_dv_preset_info(hdmi_timings[preset->index].preset,
-		preset);
+	timings->timings = hdmi_timings[timings->index].dv_timings;
+	if (!hdmi_timings[timings->index].reduced_fps)
+		timings->timings.bt.flags &= ~V4L2_DV_FL_CAN_REDUCE_FPS;
+	return 0;
+}
+
+static int hdmi_dv_timings_cap(struct v4l2_subdev *sd,
+	struct v4l2_dv_timings_cap *cap)
+{
+	struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
+
+	/* Let the phy fill in the pixelclock range */
+	v4l2_subdev_call(hdev->phy_sd, video, dv_timings_cap, cap);
+	cap->type = V4L2_DV_BT_656_1120;
+	cap->bt.min_width = 720;
+	cap->bt.max_width = 1920;
+	cap->bt.min_height = 480;
+	cap->bt.max_height = 1080;
+	cap->bt.standards = V4L2_DV_BT_STD_CEA861;
+	cap->bt.capabilities = V4L2_DV_BT_CAP_INTERLACED |
+			       V4L2_DV_BT_CAP_PROGRESSIVE;
+	return 0;
 }
 
 static const struct v4l2_subdev_core_ops hdmi_sd_core_ops = {
@@ -684,9 +698,10 @@ static const struct v4l2_subdev_core_ops hdmi_sd_core_ops = {
 };
 
 static const struct v4l2_subdev_video_ops hdmi_sd_video_ops = {
-	.s_dv_preset = hdmi_s_dv_preset,
-	.g_dv_preset = hdmi_g_dv_preset,
-	.enum_dv_presets = hdmi_enum_dv_presets,
+	.s_dv_timings = hdmi_s_dv_timings,
+	.g_dv_timings = hdmi_g_dv_timings,
+	.enum_dv_timings = hdmi_enum_dv_timings,
+	.dv_timings_cap = hdmi_dv_timings_cap,
 	.g_mbus_fmt = hdmi_g_mbus_fmt,
 	.s_stream = hdmi_s_stream,
 };
@@ -956,9 +971,11 @@ static int hdmi_probe(struct platform_device *pdev)
 	sd->owner = THIS_MODULE;
 
 	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_preset2timings(hdmi_dev->cur_preset);
+	hdmi_dev->cur_timings =
+		hdmi_timings[HDMI_DEFAULT_TIMINGS_IDX].dv_timings;
+	/* FIXME: missing fail timings is not supported */
+	hdmi_dev->cur_conf =
+		hdmi_timings[HDMI_DEFAULT_TIMINGS_IDX].hdmi_timings;
 	hdmi_dev->cur_conf_dirty = 1;
 
 	/* storing subdev for call that have only access to struct device */
diff --git a/drivers/media/platform/s5p-tv/hdmiphy_drv.c b/drivers/media/platform/s5p-tv/hdmiphy_drv.c
index 80717cec76a..e19a0af1ea4 100644
--- a/drivers/media/platform/s5p-tv/hdmiphy_drv.c
+++ b/drivers/media/platform/s5p-tv/hdmiphy_drv.c
@@ -176,35 +176,9 @@ 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 u8 *hdmiphy_find_conf(unsigned long pixclk,
+		const struct hdmiphy_conf *conf)
 {
-	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)
-{
-	unsigned long pixclk;
-
-	pixclk = hdmiphy_preset_to_pixclk(preset);
-	if (!pixclk)
-		return NULL;
-
 	for (; conf->pixclk; ++conf)
 		if (conf->pixclk == pixclk)
 			return conf->data;
@@ -217,8 +191,8 @@ static int hdmiphy_s_power(struct v4l2_subdev *sd, int on)
 	return 0;
 }
 
-static int hdmiphy_s_dv_preset(struct v4l2_subdev *sd,
-	struct v4l2_dv_preset *preset)
+static int hdmiphy_s_dv_timings(struct v4l2_subdev *sd,
+	struct v4l2_dv_timings *timings)
 {
 	const u8 *data;
 	u8 buffer[32];
@@ -226,9 +200,12 @@ static int hdmiphy_s_dv_preset(struct v4l2_subdev *sd,
 	struct hdmiphy_ctx *ctx = sd_to_ctx(sd);
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct device *dev = &client->dev;
+	unsigned long pixclk = timings->bt.pixelclock;
 
-	dev_info(dev, "s_dv_preset(preset = %d)\n", preset->preset);
-	data = hdmiphy_find_conf(preset->preset, ctx->conf_tab);
+	dev_info(dev, "s_dv_timings\n");
+	if ((timings->bt.flags & V4L2_DV_FL_REDUCED_FPS) && pixclk == 74250000)
+		pixclk = 74176000;
+	data = hdmiphy_find_conf(pixclk, ctx->conf_tab);
 	if (!data) {
 		dev_err(dev, "format not supported\n");
 		return -EINVAL;
@@ -245,6 +222,17 @@ static int hdmiphy_s_dv_preset(struct v4l2_subdev *sd,
 	return 0;
 }
 
+static int hdmiphy_dv_timings_cap(struct v4l2_subdev *sd,
+	struct v4l2_dv_timings_cap *cap)
+{
+	cap->type = V4L2_DV_BT_656_1120;
+	/* The phy only determines the pixelclock, leave the other values
+	 * at 0 to signify that we have no information for them. */
+	cap->bt.min_pixelclock = 27000000;
+	cap->bt.max_pixelclock = 148500000;
+	return 0;
+}
+
 static int hdmiphy_s_stream(struct v4l2_subdev *sd, int enable)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
@@ -270,7 +258,8 @@ static const struct v4l2_subdev_core_ops hdmiphy_core_ops = {
 };
 
 static const struct v4l2_subdev_video_ops hdmiphy_video_ops = {
-	.s_dv_preset = hdmiphy_s_dv_preset,
+	.s_dv_timings = hdmiphy_s_dv_timings,
+	.dv_timings_cap = hdmiphy_dv_timings_cap,
 	.s_stream =  hdmiphy_s_stream,
 };
 
diff --git a/drivers/media/platform/s5p-tv/mixer_video.c b/drivers/media/platform/s5p-tv/mixer_video.c
index 82142a2d6d9..ef0efdf422f 100644
--- a/drivers/media/platform/s5p-tv/mixer_video.c
+++ b/drivers/media/platform/s5p-tv/mixer_video.c
@@ -501,8 +501,8 @@ fail:
 	return -ERANGE;
 }
 
-static int mxr_enum_dv_presets(struct file *file, void *fh,
-	struct v4l2_dv_enum_preset *preset)
+static int mxr_enum_dv_timings(struct file *file, void *fh,
+	struct v4l2_enum_dv_timings *timings)
 {
 	struct mxr_layer *layer = video_drvdata(file);
 	struct mxr_device *mdev = layer->mdev;
@@ -510,14 +510,14 @@ static int mxr_enum_dv_presets(struct file *file, void *fh,
 
 	/* lock protects from changing sd_out */
 	mutex_lock(&mdev->mutex);
-	ret = v4l2_subdev_call(to_outsd(mdev), video, enum_dv_presets, preset);
+	ret = v4l2_subdev_call(to_outsd(mdev), video, enum_dv_timings, timings);
 	mutex_unlock(&mdev->mutex);
 
 	return ret ? -EINVAL : 0;
 }
 
-static int mxr_s_dv_preset(struct file *file, void *fh,
-	struct v4l2_dv_preset *preset)
+static int mxr_s_dv_timings(struct file *file, void *fh,
+	struct v4l2_dv_timings *timings)
 {
 	struct mxr_layer *layer = video_drvdata(file);
 	struct mxr_device *mdev = layer->mdev;
@@ -526,7 +526,7 @@ static int mxr_s_dv_preset(struct file *file, void *fh,
 	/* lock protects from changing sd_out */
 	mutex_lock(&mdev->mutex);
 
-	/* preset change cannot be done while there is an entity
+	/* timings change cannot be done while there is an entity
 	 * dependant on output configuration
 	 */
 	if (mdev->n_output > 0) {
@@ -534,7 +534,7 @@ static int mxr_s_dv_preset(struct file *file, void *fh,
 		return -EBUSY;
 	}
 
-	ret = v4l2_subdev_call(to_outsd(mdev), video, s_dv_preset, preset);
+	ret = v4l2_subdev_call(to_outsd(mdev), video, s_dv_timings, timings);
 
 	mutex_unlock(&mdev->mutex);
 
@@ -544,8 +544,8 @@ static int mxr_s_dv_preset(struct file *file, void *fh,
 	return ret ? -EINVAL : 0;
 }
 
-static int mxr_g_dv_preset(struct file *file, void *fh,
-	struct v4l2_dv_preset *preset)
+static int mxr_g_dv_timings(struct file *file, void *fh,
+	struct v4l2_dv_timings *timings)
 {
 	struct mxr_layer *layer = video_drvdata(file);
 	struct mxr_device *mdev = layer->mdev;
@@ -553,13 +553,28 @@ static int mxr_g_dv_preset(struct file *file, void *fh,
 
 	/* lock protects from changing sd_out */
 	mutex_lock(&mdev->mutex);
-	ret = v4l2_subdev_call(to_outsd(mdev), video, g_dv_preset, preset);
+	ret = v4l2_subdev_call(to_outsd(mdev), video, g_dv_timings, timings);
 	mutex_unlock(&mdev->mutex);
 
 	return ret ? -EINVAL : 0;
 }
 
-static int mxr_s_std(struct file *file, void *fh, v4l2_std_id *norm)
+static int mxr_dv_timings_cap(struct file *file, void *fh,
+	struct v4l2_dv_timings_cap *cap)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+	struct mxr_device *mdev = layer->mdev;
+	int ret;
+
+	/* lock protects from changing sd_out */
+	mutex_lock(&mdev->mutex);
+	ret = v4l2_subdev_call(to_outsd(mdev), video, dv_timings_cap, cap);
+	mutex_unlock(&mdev->mutex);
+
+	return ret ? -EINVAL : 0;
+}
+
+static int mxr_s_std(struct file *file, void *fh, v4l2_std_id norm)
 {
 	struct mxr_layer *layer = video_drvdata(file);
 	struct mxr_device *mdev = layer->mdev;
@@ -576,7 +591,7 @@ static int mxr_s_std(struct file *file, void *fh, v4l2_std_id *norm)
 		return -EBUSY;
 	}
 
-	ret = v4l2_subdev_call(to_outsd(mdev), video, s_std_output, *norm);
+	ret = v4l2_subdev_call(to_outsd(mdev), video, s_std_output, norm);
 
 	mutex_unlock(&mdev->mutex);
 
@@ -616,8 +631,8 @@ static int mxr_enum_output(struct file *file, void *fh, struct v4l2_output *a)
 	/* try to obtain supported tv norms */
 	v4l2_subdev_call(sd, video, g_tvnorms_output, &a->std);
 	a->capabilities = 0;
-	if (sd->ops->video && sd->ops->video->s_dv_preset)
-		a->capabilities |= V4L2_OUT_CAP_PRESETS;
+	if (sd->ops->video && sd->ops->video->s_dv_timings)
+		a->capabilities |= V4L2_OUT_CAP_DV_TIMINGS;
 	if (sd->ops->video && sd->ops->video->s_std_output)
 		a->capabilities |= V4L2_OUT_CAP_STD;
 	a->type = V4L2_OUTPUT_TYPE_ANALOG;
@@ -738,10 +753,11 @@ static const struct v4l2_ioctl_ops mxr_ioctl_ops = {
 	/* Streaming control */
 	.vidioc_streamon = mxr_streamon,
 	.vidioc_streamoff = mxr_streamoff,
-	/* Preset functions */
-	.vidioc_enum_dv_presets = mxr_enum_dv_presets,
-	.vidioc_s_dv_preset = mxr_s_dv_preset,
-	.vidioc_g_dv_preset = mxr_g_dv_preset,
+	/* DV Timings functions */
+	.vidioc_enum_dv_timings = mxr_enum_dv_timings,
+	.vidioc_s_dv_timings = mxr_s_dv_timings,
+	.vidioc_g_dv_timings = mxr_g_dv_timings,
+	.vidioc_dv_timings_cap = mxr_dv_timings_cap,
 	/* analog TV standard functions */
 	.vidioc_s_std = mxr_s_std,
 	.vidioc_g_std = mxr_g_std,
diff --git a/drivers/media/platform/s5p-tv/sii9234_drv.c b/drivers/media/platform/s5p-tv/sii9234_drv.c
index d90d2286090..39b77d24b9c 100644
--- a/drivers/media/platform/s5p-tv/sii9234_drv.c
+++ b/drivers/media/platform/s5p-tv/sii9234_drv.c
@@ -23,9 +23,6 @@
 #include <linux/regulator/machine.h>
 #include <linux/slab.h>
 
-#include <mach/gpio.h>
-#include <plat/gpio-cfg.h>
-
 #include <media/sii9234.h>
 #include <media/v4l2-subdev.h>
 
diff --git a/drivers/media/platform/sh_veu.c b/drivers/media/platform/sh_veu.c
index cb54c69d574..0b32cc3f6a4 100644
--- a/drivers/media/platform/sh_veu.c
+++ b/drivers/media/platform/sh_veu.c
@@ -10,6 +10,7 @@
  * published by the Free Software Foundation
  */
 
+#include <linux/err.h>
 #include <linux/fs.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
@@ -1164,9 +1165,9 @@ static int sh_veu_probe(struct platform_device *pdev)
 
 	veu->is_2h = resource_size(reg_res) == 0x22c;
 
-	veu->base = devm_request_and_ioremap(&pdev->dev, reg_res);
-	if (!veu->base)
-		return -ENOMEM;
+	veu->base = devm_ioremap_resource(&pdev->dev, reg_res);
+	if (IS_ERR(veu->base))
+		return PTR_ERR(veu->base);
 
 	ret = devm_request_threaded_irq(&pdev->dev, irq, sh_veu_isr, sh_veu_bh,
 					0, "veu", veu);
@@ -1248,18 +1249,7 @@ static struct platform_driver __refdata sh_veu_pdrv = {
 	},
 };
 
-static int __init sh_veu_init(void)
-{
-	return platform_driver_probe(&sh_veu_pdrv, sh_veu_probe);
-}
-
-static void __exit sh_veu_exit(void)
-{
-	platform_driver_unregister(&sh_veu_pdrv);
-}
-
-module_init(sh_veu_init);
-module_exit(sh_veu_exit);
+module_platform_driver_probe(sh_veu_pdrv, sh_veu_probe);
 
 MODULE_DESCRIPTION("sh-mobile VEU mem2mem driver");
 MODULE_AUTHOR("Guennadi Liakhovetski, <g.liakhovetski@gmx.de>");
diff --git a/drivers/media/platform/sh_vou.c b/drivers/media/platform/sh_vou.c
index 66c8da18df8..7d0235069c8 100644
--- a/drivers/media/platform/sh_vou.c
+++ b/drivers/media/platform/sh_vou.c
@@ -881,29 +881,29 @@ static u32 sh_vou_ntsc_mode(enum sh_vou_bus_fmt bus_fmt)
 	}
 }
 
-static int sh_vou_s_std(struct file *file, void *priv, v4l2_std_id *std_id)
+static int sh_vou_s_std(struct file *file, void *priv, v4l2_std_id std_id)
 {
 	struct sh_vou_device *vou_dev = video_drvdata(file);
 	int ret;
 
-	dev_dbg(vou_dev->v4l2_dev.dev, "%s(): 0x%llx\n", __func__, *std_id);
+	dev_dbg(vou_dev->v4l2_dev.dev, "%s(): 0x%llx\n", __func__, std_id);
 
-	if (*std_id & ~vou_dev->vdev->tvnorms)
+	if (std_id & ~vou_dev->vdev->tvnorms)
 		return -EINVAL;
 
 	ret = v4l2_device_call_until_err(&vou_dev->v4l2_dev, 0, video,
-					 s_std_output, *std_id);
+					 s_std_output, std_id);
 	/* Shall we continue, if the subdev doesn't support .s_std_output()? */
 	if (ret < 0 && ret != -ENOIOCTLCMD)
 		return ret;
 
-	if (*std_id & V4L2_STD_525_60)
+	if (std_id & V4L2_STD_525_60)
 		sh_vou_reg_ab_set(vou_dev, VOUCR,
 			sh_vou_ntsc_mode(vou_dev->pdata->bus_fmt) << 29, 7 << 29);
 	else
 		sh_vou_reg_ab_set(vou_dev, VOUCR, 5 << 29, 7 << 29);
 
-	vou_dev->std = *std_id;
+	vou_dev->std = std_id;
 
 	return 0;
 }
@@ -1266,7 +1266,7 @@ static int sh_vou_g_register(struct file *file, void *fh,
 }
 
 static int sh_vou_s_register(struct file *file, void *fh,
-				 struct v4l2_dbg_register *reg)
+				 const struct v4l2_dbg_register *reg)
 {
 	struct sh_vou_device *vou_dev = video_drvdata(file);
 
@@ -1485,18 +1485,7 @@ static struct platform_driver __refdata sh_vou = {
 	},
 };
 
-static int __init sh_vou_init(void)
-{
-	return platform_driver_probe(&sh_vou, sh_vou_probe);
-}
-
-static void __exit sh_vou_exit(void)
-{
-	platform_driver_unregister(&sh_vou);
-}
-
-module_init(sh_vou_init);
-module_exit(sh_vou_exit);
+module_platform_driver_probe(sh_vou, sh_vou_probe);
 
 MODULE_DESCRIPTION("SuperH VOU driver");
 MODULE_AUTHOR("Guennadi Liakhovetski <g.liakhovetski@gmx.de>");
diff --git a/drivers/media/platform/soc_camera/atmel-isi.c b/drivers/media/platform/soc_camera/atmel-isi.c
index 82dbf99d347..1abbb36d075 100644
--- a/drivers/media/platform/soc_camera/atmel-isi.c
+++ b/drivers/media/platform/soc_camera/atmel-isi.c
@@ -514,6 +514,7 @@ static int isi_camera_init_videobuf(struct vb2_queue *q,
 	q->buf_struct_size = sizeof(struct frame_buffer);
 	q->ops = &isi_video_qops;
 	q->mem_ops = &vb2_dma_contig_memops;
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 
 	return vb2_queue_init(q);
 }
@@ -1020,7 +1021,7 @@ static int atmel_isi_probe(struct platform_device *pdev)
 	isi_writel(isi, ISI_CTRL, ISI_CTRL_DIS);
 
 	irq = platform_get_irq(pdev, 0);
-	if (irq < 0) {
+	if (IS_ERR_VALUE(irq)) {
 		ret = irq;
 		goto err_req_irq;
 	}
@@ -1073,7 +1074,6 @@ err_clk_prepare_pclk:
 }
 
 static struct platform_driver atmel_isi_driver = {
-	.probe		= atmel_isi_probe,
 	.remove		= atmel_isi_remove,
 	.driver		= {
 		.name = "atmel_isi",
@@ -1081,17 +1081,7 @@ static struct platform_driver atmel_isi_driver = {
 	},
 };
 
-static int __init atmel_isi_init_module(void)
-{
-	return  platform_driver_probe(&atmel_isi_driver, &atmel_isi_probe);
-}
-
-static void __exit atmel_isi_exit(void)
-{
-	platform_driver_unregister(&atmel_isi_driver);
-}
-module_init(atmel_isi_init_module);
-module_exit(atmel_isi_exit);
+module_platform_driver_probe(atmel_isi_driver, atmel_isi_probe);
 
 MODULE_AUTHOR("Josh Wu <josh.wu@atmel.com>");
 MODULE_DESCRIPTION("The V4L2 driver for Atmel Linux");
diff --git a/drivers/media/platform/soc_camera/mx1_camera.c b/drivers/media/platform/soc_camera/mx1_camera.c
index 25b2a285dc8..a3fd8d63546 100644
--- a/drivers/media/platform/soc_camera/mx1_camera.c
+++ b/drivers/media/platform/soc_camera/mx1_camera.c
@@ -776,7 +776,7 @@ static int __init mx1_camera_probe(struct platform_device *pdev)
 	/* request irq */
 	err = claim_fiq(&fh);
 	if (err) {
-		dev_err(&pdev->dev, "Camera interrupt register failed \n");
+		dev_err(&pdev->dev, "Camera interrupt register failed\n");
 		goto exit_free_dma;
 	}
 
@@ -853,24 +853,13 @@ static int __exit mx1_camera_remove(struct platform_device *pdev)
 }
 
 static struct platform_driver mx1_camera_driver = {
-	.driver 	= {
+	.driver		= {
 		.name	= DRIVER_NAME,
 	},
 	.remove		= __exit_p(mx1_camera_remove),
 };
 
-static int __init mx1_camera_init(void)
-{
-	return platform_driver_probe(&mx1_camera_driver, mx1_camera_probe);
-}
-
-static void __exit mx1_camera_exit(void)
-{
-	return platform_driver_unregister(&mx1_camera_driver);
-}
-
-module_init(mx1_camera_init);
-module_exit(mx1_camera_exit);
+module_platform_driver_probe(mx1_camera_driver, mx1_camera_probe);
 
 MODULE_DESCRIPTION("i.MX1/i.MXL SoC Camera Host driver");
 MODULE_AUTHOR("Paulius Zaleckas <paulius.zaleckas@teltonika.lt>");
diff --git a/drivers/media/platform/soc_camera/mx2_camera.c b/drivers/media/platform/soc_camera/mx2_camera.c
index ffba7d91f41..5bbeb43e453 100644
--- a/drivers/media/platform/soc_camera/mx2_camera.c
+++ b/drivers/media/platform/soc_camera/mx2_camera.c
@@ -797,6 +797,7 @@ static int mx2_camera_init_videobuf(struct vb2_queue *q,
 	q->ops = &mx2_videobuf_ops;
 	q->mem_ops = &vb2_dma_contig_memops;
 	q->buf_struct_size = sizeof(struct mx2_buffer);
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 
 	return vb2_queue_init(q);
 }
@@ -1453,7 +1454,7 @@ static int mx27_camera_emma_init(struct platform_device *pdev)
 	err = devm_request_irq(pcdev->dev, irq_emma, mx27_camera_emma_irq, 0,
 			       MX2_CAM_DRV_NAME, pcdev);
 	if (err) {
-		dev_err(pcdev->dev, "Camera EMMA interrupt register failed \n");
+		dev_err(pcdev->dev, "Camera EMMA interrupt register failed\n");
 		goto out;
 	}
 
@@ -1614,15 +1615,14 @@ static int mx2_camera_remove(struct platform_device *pdev)
 }
 
 static struct platform_driver mx2_camera_driver = {
-	.driver 	= {
+	.driver		= {
 		.name	= MX2_CAM_DRV_NAME,
 	},
 	.id_table	= mx2_camera_devtype,
 	.remove		= mx2_camera_remove,
-	.probe		= mx2_camera_probe,
 };
 
-module_platform_driver(mx2_camera_driver);
+module_platform_driver_probe(mx2_camera_driver, mx2_camera_probe);
 
 MODULE_DESCRIPTION("i.MX27 SoC Camera Host driver");
 MODULE_AUTHOR("Sascha Hauer <sha@pengutronix.de>");
diff --git a/drivers/media/platform/soc_camera/mx3_camera.c b/drivers/media/platform/soc_camera/mx3_camera.c
index f5cbb92db54..5da337736cd 100644
--- a/drivers/media/platform/soc_camera/mx3_camera.c
+++ b/drivers/media/platform/soc_camera/mx3_camera.c
@@ -455,6 +455,7 @@ static int mx3_camera_init_videobuf(struct vb2_queue *q,
 	q->ops = &mx3_videobuf_ops;
 	q->mem_ops = &vb2_dma_contig_memops;
 	q->buf_struct_size = sizeof(struct mx3_camera_buffer);
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 
 	return vb2_queue_init(q);
 }
@@ -1275,7 +1276,7 @@ static int mx3_camera_remove(struct platform_device *pdev)
 }
 
 static struct platform_driver mx3_camera_driver = {
-	.driver 	= {
+	.driver		= {
 		.name	= MX3_CAM_DRV_NAME,
 	},
 	.probe		= mx3_camera_probe,
diff --git a/drivers/media/platform/soc_camera/omap1_camera.c b/drivers/media/platform/soc_camera/omap1_camera.c
index 2547bf88f79..9689a6e89b7 100644
--- a/drivers/media/platform/soc_camera/omap1_camera.c
+++ b/drivers/media/platform/soc_camera/omap1_camera.c
@@ -1546,7 +1546,7 @@ static struct soc_camera_host_ops omap1_host_ops = {
 	.poll		= omap1_cam_poll,
 };
 
-static int __init omap1_cam_probe(struct platform_device *pdev)
+static int omap1_cam_probe(struct platform_device *pdev)
 {
 	struct omap1_cam_dev *pcdev;
 	struct resource *res;
@@ -1677,7 +1677,7 @@ exit:
 	return err;
 }
 
-static int __exit omap1_cam_remove(struct platform_device *pdev)
+static int omap1_cam_remove(struct platform_device *pdev)
 {
 	struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
 	struct omap1_cam_dev *pcdev = container_of(soc_host,
@@ -1709,7 +1709,7 @@ static struct platform_driver omap1_cam_driver = {
 		.name	= DRIVER_NAME,
 	},
 	.probe		= omap1_cam_probe,
-	.remove		= __exit_p(omap1_cam_remove),
+	.remove		= omap1_cam_remove,
 };
 
 module_platform_driver(omap1_cam_driver);
diff --git a/drivers/media/platform/soc_camera/pxa_camera.c b/drivers/media/platform/soc_camera/pxa_camera.c
index 395e2e04361..d665242e820 100644
--- a/drivers/media/platform/soc_camera/pxa_camera.c
+++ b/drivers/media/platform/soc_camera/pxa_camera.c
@@ -15,6 +15,7 @@
 #include <linux/io.h>
 #include <linux/delay.h>
 #include <linux/dma-mapping.h>
+#include <linux/err.h>
 #include <linux/errno.h>
 #include <linux/fs.h>
 #include <linux/interrupt.h>
@@ -1710,9 +1711,10 @@ static int pxa_camera_probe(struct platform_device *pdev)
 	/*
 	 * Request the regions.
 	 */
-	base = devm_request_and_ioremap(&pdev->dev, res);
-	if (!base)
-		return -ENOMEM;
+	base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
 	pcdev->irq = irq;
 	pcdev->base = base;
 
@@ -1794,13 +1796,13 @@ static int pxa_camera_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct dev_pm_ops pxa_camera_pm = {
+static const struct dev_pm_ops pxa_camera_pm = {
 	.suspend	= pxa_camera_suspend,
 	.resume		= pxa_camera_resume,
 };
 
 static struct platform_driver pxa_camera_driver = {
-	.driver 	= {
+	.driver		= {
 		.name	= PXA_CAM_DRV_NAME,
 		.pm	= &pxa_camera_pm,
 	},
diff --git a/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c b/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c
index bb08a46432f..143d29fe013 100644
--- a/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c
+++ b/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c
@@ -20,6 +20,7 @@
 #include <linux/completion.h>
 #include <linux/delay.h>
 #include <linux/dma-mapping.h>
+#include <linux/err.h>
 #include <linux/errno.h>
 #include <linux/fs.h>
 #include <linux/interrupt.h>
@@ -2026,6 +2027,7 @@ static int sh_mobile_ceu_init_videobuf(struct vb2_queue *q,
 	q->ops = &sh_mobile_ceu_videobuf_ops;
 	q->mem_ops = &vb2_dma_contig_memops;
 	q->buf_struct_size = sizeof(struct sh_mobile_ceu_buffer);
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 
 	return vb2_queue_init(q);
 }
@@ -2110,11 +2112,9 @@ static int sh_mobile_ceu_probe(struct platform_device *pdev)
 	pcdev->max_width = pcdev->pdata->max_width ? : 2560;
 	pcdev->max_height = pcdev->pdata->max_height ? : 1920;
 
-	base = devm_request_and_ioremap(&pdev->dev, res);
-	if (!base) {
-		dev_err(&pdev->dev, "Unable to ioremap CEU registers.\n");
-		return -ENXIO;
-	}
+	base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
 
 	pcdev->irq = irq;
 	pcdev->base = base;
@@ -2288,7 +2288,7 @@ static const struct dev_pm_ops sh_mobile_ceu_dev_pm_ops = {
 };
 
 static struct platform_driver sh_mobile_ceu_driver = {
-	.driver 	= {
+	.driver		= {
 		.name	= "sh_mobile_ceu",
 		.pm	= &sh_mobile_ceu_dev_pm_ops,
 	},
diff --git a/drivers/media/platform/soc_camera/sh_mobile_csi2.c b/drivers/media/platform/soc_camera/sh_mobile_csi2.c
index 42c559eb493..09cb4fc88f3 100644
--- a/drivers/media/platform/soc_camera/sh_mobile_csi2.c
+++ b/drivers/media/platform/soc_camera/sh_mobile_csi2.c
@@ -9,6 +9,7 @@
  */
 
 #include <linux/delay.h>
+#include <linux/err.h>
 #include <linux/i2c.h>
 #include <linux/io.h>
 #include <linux/platform_device.h>
@@ -324,11 +325,9 @@ static int sh_csi2_probe(struct platform_device *pdev)
 
 	priv->irq = irq;
 
-	priv->base = devm_request_and_ioremap(&pdev->dev, res);
-	if (!priv->base) {
-		dev_err(&pdev->dev, "Unable to ioremap CSI2 registers.\n");
-		return -ENXIO;
-	}
+	priv->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(priv->base))
+		return PTR_ERR(priv->base);
 
 	priv->pdev = pdev;
 	platform_set_drvdata(pdev, priv);
diff --git a/drivers/media/platform/soc_camera/soc_camera.c b/drivers/media/platform/soc_camera/soc_camera.c
index 8ec98051ea7..eea832c5fd0 100644
--- a/drivers/media/platform/soc_camera/soc_camera.c
+++ b/drivers/media/platform/soc_camera/soc_camera.c
@@ -256,12 +256,12 @@ static int soc_camera_s_input(struct file *file, void *priv, unsigned int i)
 	return 0;
 }
 
-static int soc_camera_s_std(struct file *file, void *priv, v4l2_std_id *a)
+static int soc_camera_s_std(struct file *file, void *priv, v4l2_std_id a)
 {
 	struct soc_camera_device *icd = file->private_data;
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 
-	return v4l2_subdev_call(sd, core, s_std, *a);
+	return v4l2_subdev_call(sd, core, s_std, a);
 }
 
 static int soc_camera_g_std(struct file *file, void *priv, v4l2_std_id *a)
@@ -508,36 +508,49 @@ static int soc_camera_set_fmt(struct soc_camera_device *icd,
 static int soc_camera_open(struct file *file)
 {
 	struct video_device *vdev = video_devdata(file);
-	struct soc_camera_device *icd = dev_get_drvdata(vdev->parent);
-	struct soc_camera_desc *sdesc = to_soc_camera_desc(icd);
+	struct soc_camera_device *icd;
 	struct soc_camera_host *ici;
 	int ret;
 
-	if (!to_soc_camera_control(icd))
-		/* No device driver attached */
-		return -ENODEV;
-
 	/*
 	 * Don't mess with the host during probe: wait until the loop in
-	 * scan_add_host() completes
+	 * scan_add_host() completes. Also protect against a race with
+	 * soc_camera_host_unregister().
 	 */
 	if (mutex_lock_interruptible(&list_lock))
 		return -ERESTARTSYS;
+
+	if (!vdev || !video_is_registered(vdev)) {
+		mutex_unlock(&list_lock);
+		return -ENODEV;
+	}
+
+	icd = dev_get_drvdata(vdev->parent);
 	ici = to_soc_camera_host(icd->parent);
+
+	ret = try_module_get(ici->ops->owner) ? 0 : -ENODEV;
 	mutex_unlock(&list_lock);
 
-	if (mutex_lock_interruptible(&ici->host_lock))
-		return -ERESTARTSYS;
-	if (!try_module_get(ici->ops->owner)) {
+	if (ret < 0) {
 		dev_err(icd->pdev, "Couldn't lock capture bus driver.\n");
-		ret = -EINVAL;
-		goto emodule;
+		return ret;
 	}
 
+	if (!to_soc_camera_control(icd)) {
+		/* No device driver attached */
+		ret = -ENODEV;
+		goto econtrol;
+	}
+
+	if (mutex_lock_interruptible(&ici->host_lock)) {
+		ret = -ERESTARTSYS;
+		goto elockhost;
+	}
 	icd->use_count++;
 
 	/* Now we really have to activate the camera */
 	if (icd->use_count == 1) {
+		struct soc_camera_desc *sdesc = to_soc_camera_desc(icd);
 		/* Restore parameters before the last close() per V4L2 API */
 		struct v4l2_format f = {
 			.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
@@ -609,9 +622,10 @@ epower:
 	ici->ops->remove(icd);
 eiciadd:
 	icd->use_count--;
-	module_put(ici->ops->owner);
-emodule:
 	mutex_unlock(&ici->host_lock);
+elockhost:
+econtrol:
+	module_put(ici->ops->owner);
 
 	return ret;
 }
@@ -1042,7 +1056,7 @@ static int soc_camera_g_register(struct file *file, void *fh,
 }
 
 static int soc_camera_s_register(struct file *file, void *fh,
-				 struct v4l2_dbg_register *reg)
+				 const struct v4l2_dbg_register *reg)
 {
 	struct soc_camera_device *icd = file->private_data;
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
diff --git a/drivers/media/platform/soc_camera/soc_camera_platform.c b/drivers/media/platform/soc_camera/soc_camera_platform.c
index ce3b1d6a473..1b7a88ca195 100644
--- a/drivers/media/platform/soc_camera/soc_camera_platform.c
+++ b/drivers/media/platform/soc_camera/soc_camera_platform.c
@@ -188,7 +188,7 @@ static int soc_camera_platform_remove(struct platform_device *pdev)
 }
 
 static struct platform_driver soc_camera_platform_driver = {
-	.driver 	= {
+	.driver		= {
 		.name	= "soc_camera_platform",
 		.owner	= THIS_MODULE,
 	},
diff --git a/drivers/media/platform/soc_camera/soc_mediabus.c b/drivers/media/platform/soc_camera/soc_mediabus.c
index 89dce097a82..dc02deca756 100644
--- a/drivers/media/platform/soc_camera/soc_mediabus.c
+++ b/drivers/media/platform/soc_camera/soc_mediabus.c
@@ -73,7 +73,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.name			= "RGB555X",
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
-		.order			= SOC_MBUS_ORDER_LE,
+		.order			= SOC_MBUS_ORDER_BE,
 		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
@@ -93,10 +93,46 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.name			= "RGB565X",
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
-		.order			= SOC_MBUS_ORDER_LE,
+		.order			= SOC_MBUS_ORDER_BE,
 		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
+	.code = V4L2_MBUS_FMT_RGB666_1X18,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_RGB32,
+		.name			= "RGB666/32bpp",
+		.bits_per_sample	= 18,
+		.packing		= SOC_MBUS_PACKING_EXTEND32,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+}, {
+	.code = V4L2_MBUS_FMT_RGB888_1X24,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_RGB32,
+		.name			= "RGB888/32bpp",
+		.bits_per_sample	= 24,
+		.packing		= SOC_MBUS_PACKING_EXTEND32,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+}, {
+	.code = V4L2_MBUS_FMT_RGB888_2X12_BE,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_RGB32,
+		.name			= "RGB888/32bpp",
+		.bits_per_sample	= 12,
+		.packing		= SOC_MBUS_PACKING_EXTEND32,
+		.order			= SOC_MBUS_ORDER_BE,
+	},
+}, {
+	.code = V4L2_MBUS_FMT_RGB888_2X12_LE,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_RGB32,
+		.name			= "RGB888/32bpp",
+		.bits_per_sample	= 12,
+		.packing		= SOC_MBUS_PACKING_EXTEND32,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+}, {
 	.code = V4L2_MBUS_FMT_SBGGR8_1X8,
 	.fmt = {
 		.fourcc			= V4L2_PIX_FMT_SBGGR8,
@@ -358,6 +394,10 @@ int soc_mbus_samples_per_pixel(const struct soc_mbus_pixelfmt *mf,
 		*numerator = 1;
 		*denominator = 1;
 		return 0;
+	case SOC_MBUS_PACKING_EXTEND32:
+		*numerator = 1;
+		*denominator = 1;
+		return 0;
 	case SOC_MBUS_PACKING_2X8_PADHI:
 	case SOC_MBUS_PACKING_2X8_PADLO:
 		*numerator = 2;
@@ -392,6 +432,8 @@ s32 soc_mbus_bytes_per_line(u32 width, const struct soc_mbus_pixelfmt *mf)
 		return width * 3 / 2;
 	case SOC_MBUS_PACKING_VARIABLE:
 		return 0;
+	case SOC_MBUS_PACKING_EXTEND32:
+		return width * 4;
 	}
 	return -EINVAL;
 }
diff --git a/drivers/media/platform/timblogiw.c b/drivers/media/platform/timblogiw.c
index c3a2a448440..a2f7bdd5104 100644
--- a/drivers/media/platform/timblogiw.c
+++ b/drivers/media/platform/timblogiw.c
@@ -78,7 +78,7 @@ struct timblogiw_buffer {
 	struct timblogiw_fh	*fh;
 };
 
-const struct timblogiw_tvnorm timblogiw_tvnorms[] = {
+static const struct timblogiw_tvnorm timblogiw_tvnorms[] = {
 	{
 		.std			= V4L2_STD_PAL,
 		.width			= 720,
@@ -336,7 +336,7 @@ static int timblogiw_g_std(struct file *file, void  *priv, v4l2_std_id *std)
 	return 0;
 }
 
-static int timblogiw_s_std(struct file *file, void  *priv, v4l2_std_id *std)
+static int timblogiw_s_std(struct file *file, void  *priv, v4l2_std_id std)
 {
 	struct video_device *vdev = video_devdata(file);
 	struct timblogiw *lw = video_get_drvdata(vdev);
@@ -348,10 +348,10 @@ static int timblogiw_s_std(struct file *file, void  *priv, v4l2_std_id *std)
 	mutex_lock(&lw->lock);
 
 	if (TIMBLOGIW_HAS_DECODER(lw))
-		err = v4l2_subdev_call(lw->sd_enc, core, s_std, *std);
+		err = v4l2_subdev_call(lw->sd_enc, core, s_std, std);
 
 	if (!err)
-		fh->cur_norm = timblogiw_get_norm(*std);
+		fh->cur_norm = timblogiw_get_norm(std);
 
 	mutex_unlock(&lw->lock);
 
diff --git a/drivers/media/platform/via-camera.c b/drivers/media/platform/via-camera.c
index b051c4a2855..a794cd6c444 100644
--- a/drivers/media/platform/via-camera.c
+++ b/drivers/media/platform/via-camera.c
@@ -847,7 +847,7 @@ static int viacam_s_input(struct file *filp, void *priv, unsigned int i)
 	return 0;
 }
 
-static int viacam_s_std(struct file *filp, void *priv, v4l2_std_id *std)
+static int viacam_s_std(struct file *filp, void *priv, v4l2_std_id std)
 {
 	return 0;
 }
diff --git a/drivers/media/platform/vino.c b/drivers/media/platform/vino.c
index eb5d6f95570..c6af974c5b4 100644
--- a/drivers/media/platform/vino.c
+++ b/drivers/media/platform/vino.c
@@ -3042,7 +3042,7 @@ static int vino_g_std(struct file *file, void *__fh,
 }
 
 static int vino_s_std(struct file *file, void *__fh,
-			   v4l2_std_id *std)
+			   v4l2_std_id std)
 {
 	struct vino_channel_settings *vcs = video_drvdata(file);
 	unsigned long flags;
@@ -3056,7 +3056,7 @@ static int vino_s_std(struct file *file, void *__fh,
 	}
 
 	/* check if the standard is valid for the current input */
-	if ((*std) & vino_inputs[vcs->input].std) {
+	if (std & vino_inputs[vcs->input].std) {
 		dprintk("standard accepted\n");
 
 		/* change the video norm for SAA7191
@@ -3065,13 +3065,13 @@ static int vino_s_std(struct file *file, void *__fh,
 		if (vcs->input == VINO_INPUT_D1)
 			goto out;
 
-		if ((*std) & V4L2_STD_PAL) {
+		if (std & V4L2_STD_PAL) {
 			ret = vino_set_data_norm(vcs, VINO_DATA_NORM_PAL,
 						 &flags);
-		} else if ((*std) & V4L2_STD_NTSC) {
+		} else if (std & V4L2_STD_NTSC) {
 			ret = vino_set_data_norm(vcs, VINO_DATA_NORM_NTSC,
 						 &flags);
-		} else if ((*std) & V4L2_STD_SECAM) {
+		} else if (std & V4L2_STD_SECAM) {
 			ret = vino_set_data_norm(vcs, VINO_DATA_NORM_SECAM,
 						 &flags);
 		} else {
diff --git a/drivers/media/platform/vivi.c b/drivers/media/platform/vivi.c
index 8a33a712f48..85bc314382d 100644
--- a/drivers/media/platform/vivi.c
+++ b/drivers/media/platform/vivi.c
@@ -1093,6 +1093,15 @@ static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
 		return 0;
 
 	dev->input = i;
+	/*
+	 * Modify the brightness range depending on the input.
+	 * This makes it easy to use vivi to test if applications can
+	 * handle control range modifications and is also how this is
+	 * typically used in practice as different inputs may be hooked
+	 * up to different receivers with different control ranges.
+	 */
+	v4l2_ctrl_modify_range(dev->brightness,
+			128 * i, 255 + 128 * i, 1, 127 + 128 * i);
 	precalculate_bars(dev);
 	precalculate_line(dev);
 	return 0;
@@ -1429,6 +1438,7 @@ static int __init vivi_create_instance(int inst)
 	q->buf_struct_size = sizeof(struct vivi_buffer);
 	q->ops = &vivi_video_qops;
 	q->mem_ops = &vb2_vmalloc_memops;
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 
 	ret = vb2_queue_init(q);
 	if (ret)
diff --git a/drivers/media/radio/Kconfig b/drivers/media/radio/Kconfig
index 24e64a09884..c0beee2fa37 100644
--- a/drivers/media/radio/Kconfig
+++ b/drivers/media/radio/Kconfig
@@ -18,6 +18,22 @@ config RADIO_SI470X
 
 source "drivers/media/radio/si470x/Kconfig"
 
+config RADIO_SI476X
+	tristate "Silicon Laboratories Si476x I2C FM Radio"
+	depends on I2C && VIDEO_V4L2
+	depends on MFD_SI476X_CORE
+	select SND_SOC_SI476X
+	---help---
+	  Choose Y here if you have this FM radio chip.
+
+	  In order to control your radio card, you will need to use programs
+	  that are compatible with the Video For Linux 2 API.  Information on
+	  this API and pointers to "v4l2" programs may be found at
+	  <file:Documentation/video4linux/API.html>.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called radio-si476x.
+
 config USB_MR800
 	tristate "AverMedia MR 800 USB FM radio support"
 	depends on USB && VIDEO_V4L2
diff --git a/drivers/media/radio/Makefile b/drivers/media/radio/Makefile
index 303eaebdb85..0dcdb320cfc 100644
--- a/drivers/media/radio/Makefile
+++ b/drivers/media/radio/Makefile
@@ -19,6 +19,7 @@ obj-$(CONFIG_RADIO_GEMTEK) += radio-gemtek.o
 obj-$(CONFIG_RADIO_TRUST) += radio-trust.o
 obj-$(CONFIG_I2C_SI4713) += si4713-i2c.o
 obj-$(CONFIG_RADIO_SI4713) += radio-si4713.o
+obj-$(CONFIG_RADIO_SI476X) += radio-si476x.o
 obj-$(CONFIG_RADIO_MIROPCM20) += radio-miropcm20.o
 obj-$(CONFIG_USB_DSBR) += dsbr100.o
 obj-$(CONFIG_RADIO_SI470X) += si470x/
diff --git a/drivers/media/radio/dsbr100.c b/drivers/media/radio/dsbr100.c
index 63b112b555b..142c2ee64d3 100644
--- a/drivers/media/radio/dsbr100.c
+++ b/drivers/media/radio/dsbr100.c
@@ -208,13 +208,13 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *v)
+				const struct v4l2_tuner *v)
 {
 	return v->index ? -EINVAL : 0;
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
+				const struct v4l2_frequency *f)
 {
 	struct dsbr100_device *radio = video_drvdata(file);
 
diff --git a/drivers/media/radio/radio-cadet.c b/drivers/media/radio/radio-cadet.c
index 643d80ac28f..545c04cf722 100644
--- a/drivers/media/radio/radio-cadet.c
+++ b/drivers/media/radio/radio-cadet.c
@@ -90,6 +90,26 @@ static u16 sigtable[2][4] = {
 	{ 2185, 4369, 13107, 65535 },
 };
 
+static const struct v4l2_frequency_band bands[] = {
+	{
+		.index = 0,
+		.type = V4L2_TUNER_RADIO,
+		.capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow = 8320,      /* 520 kHz */
+		.rangehigh = 26400,    /* 1650 kHz */
+		.modulation = V4L2_BAND_MODULATION_AM,
+	}, {
+		.index = 1,
+		.type = V4L2_TUNER_RADIO,
+		.capability = V4L2_TUNER_CAP_STEREO | V4L2_TUNER_CAP_RDS |
+			V4L2_TUNER_CAP_RDS_BLOCK_IO | V4L2_TUNER_CAP_LOW |
+			V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow = 1400000,   /* 87.5 MHz */
+		.rangehigh = 1728000,  /* 108.0 MHz */
+		.modulation = V4L2_BAND_MODULATION_FM,
+	},
+};
+
 
 static int cadet_getstereo(struct cadet *dev)
 {
@@ -196,6 +216,8 @@ static void cadet_setfreq(struct cadet *dev, unsigned freq)
 	int i, j, test;
 	int curvol;
 
+	freq = clamp(freq, bands[dev->is_fm_band].rangelow,
+			   bands[dev->is_fm_band].rangehigh);
 	dev->curfreq = freq;
 	/*
 	 * Formulate a fifo command
@@ -337,26 +359,6 @@ static int vidioc_querycap(struct file *file, void *priv,
 	return 0;
 }
 
-static const struct v4l2_frequency_band bands[] = {
-	{
-		.index = 0,
-		.type = V4L2_TUNER_RADIO,
-		.capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_FREQ_BANDS,
-		.rangelow = 8320,      /* 520 kHz */
-		.rangehigh = 26400,    /* 1650 kHz */
-		.modulation = V4L2_BAND_MODULATION_AM,
-	}, {
-		.index = 1,
-		.type = V4L2_TUNER_RADIO,
-		.capability = V4L2_TUNER_CAP_STEREO | V4L2_TUNER_CAP_RDS |
-			V4L2_TUNER_CAP_RDS_BLOCK_IO | V4L2_TUNER_CAP_LOW |
-			V4L2_TUNER_CAP_FREQ_BANDS,
-		.rangelow = 1400000,   /* 87.5 MHz */
-		.rangehigh = 1728000,  /* 108.0 MHz */
-		.modulation = V4L2_BAND_MODULATION_FM,
-	},
-};
-
 static int vidioc_g_tuner(struct file *file, void *priv,
 				struct v4l2_tuner *v)
 {
@@ -388,7 +390,7 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *v)
+				const struct v4l2_tuner *v)
 {
 	return v->index ? -EINVAL : 0;
 }
@@ -418,7 +420,7 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
+				const struct v4l2_frequency *f)
 {
 	struct cadet *dev = video_drvdata(file);
 
@@ -426,8 +428,6 @@ static int vidioc_s_frequency(struct file *file, void *priv,
 		return -EINVAL;
 	dev->is_fm_band =
 		f->frequency >= (bands[0].rangehigh + bands[1].rangelow) / 2;
-	clamp(f->frequency, bands[dev->is_fm_band].rangelow,
-			    bands[dev->is_fm_band].rangehigh);
 	cadet_setfreq(dev, f->frequency);
 	return 0;
 }
diff --git a/drivers/media/radio/radio-isa.c b/drivers/media/radio/radio-isa.c
index 84b7b9f4385..6ff350831d5 100644
--- a/drivers/media/radio/radio-isa.c
+++ b/drivers/media/radio/radio-isa.c
@@ -51,8 +51,8 @@ static int radio_isa_querycap(struct file *file, void  *priv,
 	strlcpy(v->card, isa->drv->card, sizeof(v->card));
 	snprintf(v->bus_info, sizeof(v->bus_info), "ISA:%s", isa->v4l2_dev.name);
 
-	v->capabilities = V4L2_CAP_TUNER | V4L2_CAP_RADIO;
-	v->device_caps = v->capabilities | V4L2_CAP_DEVICE_CAPS;
+	v->device_caps = V4L2_CAP_TUNER | V4L2_CAP_RADIO;
+	v->capabilities = v->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
@@ -87,7 +87,7 @@ static int radio_isa_g_tuner(struct file *file, void *priv,
 }
 
 static int radio_isa_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *v)
+				const struct v4l2_tuner *v)
 {
 	struct radio_isa_card *isa = video_drvdata(file);
 	const struct radio_isa_ops *ops = isa->drv->ops;
@@ -102,17 +102,18 @@ static int radio_isa_s_tuner(struct file *file, void *priv,
 }
 
 static int radio_isa_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
+				const struct v4l2_frequency *f)
 {
 	struct radio_isa_card *isa = video_drvdata(file);
+	u32 freq = f->frequency;
 	int res;
 
 	if (f->tuner != 0 || f->type != V4L2_TUNER_RADIO)
 		return -EINVAL;
-	f->frequency = clamp(f->frequency, FREQ_LOW, FREQ_HIGH);
-	res = isa->drv->ops->s_frequency(isa, f->frequency);
+	freq = clamp(freq, FREQ_LOW, FREQ_HIGH);
+	res = isa->drv->ops->s_frequency(isa, freq);
 	if (res == 0)
-		isa->freq = f->frequency;
+		isa->freq = freq;
 	return res;
 }
 
diff --git a/drivers/media/radio/radio-keene.c b/drivers/media/radio/radio-keene.c
index 296941a9ae2..4c9ae767fb3 100644
--- a/drivers/media/radio/radio-keene.c
+++ b/drivers/media/radio/radio-keene.c
@@ -215,15 +215,15 @@ static int vidioc_s_modulator(struct file *file, void *priv,
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
+				const struct v4l2_frequency *f)
 {
 	struct keene_device *radio = video_drvdata(file);
+	unsigned freq = f->frequency;
 
 	if (f->tuner != 0 || f->type != V4L2_TUNER_RADIO)
 		return -EINVAL;
-	f->frequency = clamp(f->frequency,
-			FREQ_MIN * FREQ_MUL, FREQ_MAX * FREQ_MUL);
-	return keene_cmd_main(radio, f->frequency, true);
+	freq = clamp(freq, FREQ_MIN * FREQ_MUL, FREQ_MAX * FREQ_MUL);
+	return keene_cmd_main(radio, freq, true);
 }
 
 static int vidioc_g_frequency(struct file *file, void *priv,
diff --git a/drivers/media/radio/radio-ma901.c b/drivers/media/radio/radio-ma901.c
index c61f590029a..a85b064cb7b 100644
--- a/drivers/media/radio/radio-ma901.c
+++ b/drivers/media/radio/radio-ma901.c
@@ -239,7 +239,7 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 
 /* vidioc_s_tuner - set tuner attributes */
 static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *v)
+				const struct v4l2_tuner *v)
 {
 	struct ma901radio_device *radio = video_drvdata(file);
 
@@ -257,7 +257,7 @@ static int vidioc_s_tuner(struct file *file, void *priv,
 
 /* vidioc_s_frequency - set tuner radio frequency */
 static int vidioc_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
+				const struct v4l2_frequency *f)
 {
 	struct ma901radio_device *radio = video_drvdata(file);
 
@@ -347,9 +347,20 @@ static void usb_ma901radio_release(struct v4l2_device *v4l2_dev)
 static int usb_ma901radio_probe(struct usb_interface *intf,
 				const struct usb_device_id *id)
 {
+	struct usb_device *dev = interface_to_usbdev(intf);
 	struct ma901radio_device *radio;
 	int retval = 0;
 
+	/* Masterkit MA901 usb radio has the same USB ID as many others
+	 * Atmel V-USB devices. Let's make additional checks to be sure
+	 * that this is our device.
+	 */
+
+	if (dev->product && dev->manufacturer &&
+		(strncmp(dev->product, "MA901", 5) != 0
+		|| strncmp(dev->manufacturer, "www.masterkit.ru", 16) != 0))
+		return -ENODEV;
+
 	radio = kzalloc(sizeof(struct ma901radio_device), GFP_KERNEL);
 	if (!radio) {
 		dev_err(&intf->dev, "kzalloc for ma901radio_device failed\n");
diff --git a/drivers/media/radio/radio-miropcm20.c b/drivers/media/radio/radio-miropcm20.c
index 3d0ff4404d1..a7e93d7477d 100644
--- a/drivers/media/radio/radio-miropcm20.c
+++ b/drivers/media/radio/radio-miropcm20.c
@@ -103,16 +103,18 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *v)
+				const struct v4l2_tuner *v)
 {
 	struct pcm20 *dev = video_drvdata(file);
 
 	if (v->index)
 		return -EINVAL;
 	if (v->audmode > V4L2_TUNER_MODE_STEREO)
-		v->audmode = V4L2_TUNER_MODE_STEREO;
+		dev->audmode = V4L2_TUNER_MODE_STEREO;
+	else
+		dev->audmode = v->audmode;
 	snd_aci_cmd(dev->aci, ACI_SET_TUNERMONO,
-			v->audmode == V4L2_TUNER_MODE_MONO, -1);
+			dev->audmode == V4L2_TUNER_MODE_MONO, -1);
 	return 0;
 }
 
@@ -131,14 +133,14 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
+				const struct v4l2_frequency *f)
 {
 	struct pcm20 *dev = video_drvdata(file);
 
 	if (f->tuner != 0 || f->type != V4L2_TUNER_RADIO)
 		return -EINVAL;
 
-	dev->freq = clamp(f->frequency, 87 * 16000U, 108 * 16000U);
+	dev->freq = clamp_t(u32, f->frequency, 87 * 16000U, 108 * 16000U);
 	pcm20_setfreq(dev, dev->freq);
 	return 0;
 }
diff --git a/drivers/media/radio/radio-mr800.c b/drivers/media/radio/radio-mr800.c
index 9c5a267b60b..a360227ca3a 100644
--- a/drivers/media/radio/radio-mr800.c
+++ b/drivers/media/radio/radio-mr800.c
@@ -203,10 +203,14 @@ static int amradio_set_mute(struct amradio_device *radio, bool mute)
 /* set a frequency, freq is defined by v4l's TUNER_LOW, i.e. 1/16th kHz */
 static int amradio_set_freq(struct amradio_device *radio, int freq)
 {
-	unsigned short freq_send = 0x10 + (freq >> 3) / 25;
+	unsigned short freq_send;
 	u8 buf[3];
 	int retval;
 
+	/* we need to be sure that frequency isn't out of range */
+	freq = clamp_t(unsigned, freq, FREQ_MIN * FREQ_MUL, FREQ_MAX * FREQ_MUL);
+	freq_send = 0x10 + (freq >> 3) / 25;
+
 	/* frequency is calculated from freq_send and placed in first 2 bytes */
 	buf[0] = (freq_send >> 8) & 0xff;
 	buf[1] = freq_send & 0xff;
@@ -305,7 +309,7 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 
 /* vidioc_s_tuner - set tuner attributes */
 static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *v)
+				const struct v4l2_tuner *v)
 {
 	struct amradio_device *radio = video_drvdata(file);
 
@@ -323,14 +327,13 @@ static int vidioc_s_tuner(struct file *file, void *priv,
 
 /* vidioc_s_frequency - set tuner radio frequency */
 static int vidioc_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
+				const struct v4l2_frequency *f)
 {
 	struct amradio_device *radio = video_drvdata(file);
 
 	if (f->tuner != 0)
 		return -EINVAL;
-	return amradio_set_freq(radio, clamp_t(unsigned, f->frequency,
-				FREQ_MIN * FREQ_MUL, FREQ_MAX * FREQ_MUL));
+	return amradio_set_freq(radio, f->frequency);
 }
 
 /* vidioc_g_frequency - get tuner radio frequency */
@@ -389,6 +392,7 @@ static int vidioc_s_hw_freq_seek(struct file *file, void *priv,
 			continue;
 		amradio_send_cmd(radio, AMRADIO_GET_FREQ, 0, NULL, 0, true);
 		if (radio->buffer[1] || radio->buffer[2]) {
+			/* To check: sometimes radio->curfreq is set to out of range value */
 			radio->curfreq = (radio->buffer[1] << 8) | radio->buffer[2];
 			radio->curfreq = (radio->curfreq - 0x10) * 200;
 			amradio_send_cmd(radio, AMRADIO_STOP_SEARCH,
diff --git a/drivers/media/radio/radio-rtrack2.c b/drivers/media/radio/radio-rtrack2.c
index b1f844c64fd..09cfbc373c9 100644
--- a/drivers/media/radio/radio-rtrack2.c
+++ b/drivers/media/radio/radio-rtrack2.c
@@ -8,6 +8,8 @@
  *
  * Converted to the radio-isa framework by Hans Verkuil <hans.verkuil@cisco.com>
  * Converted to V4L2 API by Mauro Carvalho Chehab <mchehab@infradead.org>
+ *
+ * Fully tested with actual hardware and the v4l2-compliance tool.
  */
 
 #include <linux/module.h>	/* Modules 			*/
@@ -81,8 +83,7 @@ static int rtrack2_s_frequency(struct radio_isa_card *isa, u32 freq)
 			zero(isa);
 
 	outb_p(0xc8, isa->io);
-	if (!v4l2_ctrl_g_ctrl(isa->mute))
-		outb_p(0, isa->io);
+	outb_p(v4l2_ctrl_g_ctrl(isa->mute), isa->io);
 	return 0;
 }
 
diff --git a/drivers/media/radio/radio-sf16fmi.c b/drivers/media/radio/radio-sf16fmi.c
index 637a5556495..adfcc61bdf0 100644
--- a/drivers/media/radio/radio-sf16fmi.c
+++ b/drivers/media/radio/radio-sf16fmi.c
@@ -145,13 +145,13 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-					struct v4l2_tuner *v)
+					const struct v4l2_tuner *v)
 {
 	return v->index ? -EINVAL : 0;
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-					struct v4l2_frequency *f)
+					const struct v4l2_frequency *f)
 {
 	struct fmi *fmi = video_drvdata(file);
 
diff --git a/drivers/media/radio/radio-shark.c b/drivers/media/radio/radio-shark.c
index 8c309c7134d..8fa18ab5b72 100644
--- a/drivers/media/radio/radio-shark.c
+++ b/drivers/media/radio/radio-shark.c
@@ -284,7 +284,7 @@ static void shark_resume_leds(struct shark_device *shark)
 static int shark_register_leds(struct shark_device *shark, struct device *dev)
 {
 	v4l2_warn(&shark->v4l2_dev,
-		  "CONFIG_LED_CLASS not enabled, LED support disabled\n");
+		  "CONFIG_LEDS_CLASS not enabled, LED support disabled\n");
 	return 0;
 }
 static inline void shark_unregister_leds(struct shark_device *shark) { }
diff --git a/drivers/media/radio/radio-shark2.c b/drivers/media/radio/radio-shark2.c
index ef65ebbd536..9fb669721e6 100644
--- a/drivers/media/radio/radio-shark2.c
+++ b/drivers/media/radio/radio-shark2.c
@@ -250,7 +250,7 @@ static void shark_resume_leds(struct shark_device *shark)
 static int shark_register_leds(struct shark_device *shark, struct device *dev)
 {
 	v4l2_warn(&shark->v4l2_dev,
-		  "CONFIG_LED_CLASS not enabled, LED support disabled\n");
+		  "CONFIG_LEDS_CLASS not enabled, LED support disabled\n");
 	return 0;
 }
 static inline void shark_unregister_leds(struct shark_device *shark) { }
diff --git a/drivers/media/radio/radio-si4713.c b/drivers/media/radio/radio-si4713.c
index 1507c9d508d..ba4cfc94686 100644
--- a/drivers/media/radio/radio-si4713.c
+++ b/drivers/media/radio/radio-si4713.c
@@ -31,6 +31,9 @@
 #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>
 #include <media/radio-si4713.h>
 
 /* module parameters */
@@ -39,54 +42,30 @@ module_param(radio_nr, int, 0);
 MODULE_PARM_DESC(radio_nr,
 		 "Minor number for radio device (-1 ==> auto assign)");
 
-MODULE_LICENSE("GPL");
+MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Eduardo Valentin <eduardo.valentin@nokia.com>");
 MODULE_DESCRIPTION("Platform driver for Si4713 FM Radio Transmitter");
 MODULE_VERSION("0.0.1");
+MODULE_ALIAS("platform:radio-si4713");
 
 /* Driver state struct */
 struct radio_si4713_device {
 	struct v4l2_device		v4l2_dev;
-	struct video_device		*radio_dev;
+	struct video_device		radio_dev;
+	struct mutex lock;
 };
 
 /* radio_si4713_fops - file operations interface */
 static const struct v4l2_file_operations radio_si4713_fops = {
 	.owner		= THIS_MODULE,
+	.open = v4l2_fh_open,
+	.release = v4l2_fh_release,
+	.poll = v4l2_ctrl_poll,
 	/* Note: locking is done at the subdev level in the i2c driver. */
 	.unlocked_ioctl	= video_ioctl2,
 };
 
 /* Video4Linux Interface */
-static int radio_si4713_fill_audout(struct v4l2_audioout *vao)
-{
-	/* TODO: check presence of audio output */
-	strlcpy(vao->name, "FM Modulator Audio Out", 32);
-
-	return 0;
-}
-
-static int radio_si4713_enumaudout(struct file *file, void *priv,
-						struct v4l2_audioout *vao)
-{
-	return radio_si4713_fill_audout(vao);
-}
-
-static int radio_si4713_g_audout(struct file *file, void *priv,
-					struct v4l2_audioout *vao)
-{
-	int rval = radio_si4713_fill_audout(vao);
-
-	vao->index = 0;
-
-	return rval;
-}
-
-static int radio_si4713_s_audout(struct file *file, void *priv,
-					const struct v4l2_audioout *vao)
-{
-	return vao->index ? -EINVAL : 0;
-}
 
 /* radio_si4713_querycap - query device capabilities */
 static int radio_si4713_querycap(struct file *file, void *priv,
@@ -94,67 +73,15 @@ static int radio_si4713_querycap(struct file *file, void *priv,
 {
 	strlcpy(capability->driver, "radio-si4713", sizeof(capability->driver));
 	strlcpy(capability->card, "Silicon Labs Si4713 Modulator",
-				sizeof(capability->card));
-	capability->capabilities = V4L2_CAP_MODULATOR | V4L2_CAP_RDS_OUTPUT;
+		sizeof(capability->card));
+	strlcpy(capability->bus_info, "platform:radio-si4713",
+		sizeof(capability->bus_info));
+	capability->device_caps = V4L2_CAP_MODULATOR | V4L2_CAP_RDS_OUTPUT;
+	capability->capabilities = capability->device_caps | V4L2_CAP_DEVICE_CAPS;
 
 	return 0;
 }
 
-/* radio_si4713_queryctrl - enumerate control items */
-static int radio_si4713_queryctrl(struct file *file, void *priv,
-						struct v4l2_queryctrl *qc)
-{
-	/* Must be sorted from low to high control ID! */
-	static const u32 user_ctrls[] = {
-		V4L2_CID_USER_CLASS,
-		V4L2_CID_AUDIO_MUTE,
-		0
-	};
-
-	/* Must be sorted from low to high control ID! */
-	static const u32 fmtx_ctrls[] = {
-		V4L2_CID_FM_TX_CLASS,
-		V4L2_CID_RDS_TX_DEVIATION,
-		V4L2_CID_RDS_TX_PI,
-		V4L2_CID_RDS_TX_PTY,
-		V4L2_CID_RDS_TX_PS_NAME,
-		V4L2_CID_RDS_TX_RADIO_TEXT,
-		V4L2_CID_AUDIO_LIMITER_ENABLED,
-		V4L2_CID_AUDIO_LIMITER_RELEASE_TIME,
-		V4L2_CID_AUDIO_LIMITER_DEVIATION,
-		V4L2_CID_AUDIO_COMPRESSION_ENABLED,
-		V4L2_CID_AUDIO_COMPRESSION_GAIN,
-		V4L2_CID_AUDIO_COMPRESSION_THRESHOLD,
-		V4L2_CID_AUDIO_COMPRESSION_ATTACK_TIME,
-		V4L2_CID_AUDIO_COMPRESSION_RELEASE_TIME,
-		V4L2_CID_PILOT_TONE_ENABLED,
-		V4L2_CID_PILOT_TONE_DEVIATION,
-		V4L2_CID_PILOT_TONE_FREQUENCY,
-		V4L2_CID_TUNE_PREEMPHASIS,
-		V4L2_CID_TUNE_POWER_LEVEL,
-		V4L2_CID_TUNE_ANTENNA_CAPACITOR,
-		0
-	};
-	static const u32 *ctrl_classes[] = {
-		user_ctrls,
-		fmtx_ctrls,
-		NULL
-	};
-	struct radio_si4713_device *rsdev;
-
-	rsdev = video_get_drvdata(video_devdata(file));
-
-	qc->id = v4l2_ctrl_next(ctrl_classes, qc->id);
-	if (qc->id == 0)
-		return -EINVAL;
-
-	if (qc->id == V4L2_CID_USER_CLASS || qc->id == V4L2_CID_FM_TX_CLASS)
-		return v4l2_ctrl_query_fill(qc, 0, 0, 0, 0);
-
-	return v4l2_device_call_until_err(&rsdev->v4l2_dev, 0, core,
-						queryctrl, qc);
-}
-
 /*
  * v4l2 ioctl call backs.
  * we are just a wrapper for v4l2_sub_devs.
@@ -164,83 +91,50 @@ static inline struct v4l2_device *get_v4l2_dev(struct file *file)
 	return &((struct radio_si4713_device *)video_drvdata(file))->v4l2_dev;
 }
 
-static int radio_si4713_g_ext_ctrls(struct file *file, void *p,
-						struct v4l2_ext_controls *vecs)
-{
-	return v4l2_device_call_until_err(get_v4l2_dev(file), 0, core,
-							g_ext_ctrls, vecs);
-}
-
-static int radio_si4713_s_ext_ctrls(struct file *file, void *p,
-						struct v4l2_ext_controls *vecs)
-{
-	return v4l2_device_call_until_err(get_v4l2_dev(file), 0, core,
-							s_ext_ctrls, vecs);
-}
-
-static int radio_si4713_g_ctrl(struct file *file, void *p,
-						struct v4l2_control *vc)
-{
-	return v4l2_device_call_until_err(get_v4l2_dev(file), 0, core,
-							g_ctrl, vc);
-}
-
-static int radio_si4713_s_ctrl(struct file *file, void *p,
-						struct v4l2_control *vc)
-{
-	return v4l2_device_call_until_err(get_v4l2_dev(file), 0, core,
-							s_ctrl, vc);
-}
-
 static int radio_si4713_g_modulator(struct file *file, void *p,
-						struct v4l2_modulator *vm)
+				    struct v4l2_modulator *vm)
 {
 	return v4l2_device_call_until_err(get_v4l2_dev(file), 0, tuner,
-							g_modulator, vm);
+					  g_modulator, vm);
 }
 
 static int radio_si4713_s_modulator(struct file *file, void *p,
-						const struct v4l2_modulator *vm)
+				    const struct v4l2_modulator *vm)
 {
 	return v4l2_device_call_until_err(get_v4l2_dev(file), 0, tuner,
-							s_modulator, vm);
+					  s_modulator, vm);
 }
 
 static int radio_si4713_g_frequency(struct file *file, void *p,
-						struct v4l2_frequency *vf)
+				    struct v4l2_frequency *vf)
 {
 	return v4l2_device_call_until_err(get_v4l2_dev(file), 0, tuner,
-							g_frequency, vf);
+					  g_frequency, vf);
 }
 
 static int radio_si4713_s_frequency(struct file *file, void *p,
-						struct v4l2_frequency *vf)
+				    const struct v4l2_frequency *vf)
 {
 	return v4l2_device_call_until_err(get_v4l2_dev(file), 0, tuner,
-							s_frequency, vf);
+					  s_frequency, vf);
 }
 
 static long radio_si4713_default(struct file *file, void *p,
-				bool valid_prio, int cmd, void *arg)
+				 bool valid_prio, unsigned int cmd, void *arg)
 {
 	return v4l2_device_call_until_err(get_v4l2_dev(file), 0, core,
-							ioctl, cmd, arg);
+					  ioctl, cmd, arg);
 }
 
 static struct v4l2_ioctl_ops radio_si4713_ioctl_ops = {
-	.vidioc_enumaudout	= radio_si4713_enumaudout,
-	.vidioc_g_audout	= radio_si4713_g_audout,
-	.vidioc_s_audout	= radio_si4713_s_audout,
 	.vidioc_querycap	= radio_si4713_querycap,
-	.vidioc_queryctrl	= radio_si4713_queryctrl,
-	.vidioc_g_ext_ctrls	= radio_si4713_g_ext_ctrls,
-	.vidioc_s_ext_ctrls	= radio_si4713_s_ext_ctrls,
-	.vidioc_g_ctrl		= radio_si4713_g_ctrl,
-	.vidioc_s_ctrl		= radio_si4713_s_ctrl,
 	.vidioc_g_modulator	= radio_si4713_g_modulator,
 	.vidioc_s_modulator	= radio_si4713_s_modulator,
 	.vidioc_g_frequency	= radio_si4713_g_frequency,
 	.vidioc_s_frequency	= radio_si4713_s_frequency,
+	.vidioc_log_status      = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 	.vidioc_default		= radio_si4713_default,
 };
 
@@ -248,7 +142,7 @@ static struct v4l2_ioctl_ops radio_si4713_ioctl_ops = {
 static struct video_device radio_si4713_vdev_template = {
 	.fops			= &radio_si4713_fops,
 	.name			= "radio-si4713",
-	.release		= video_device_release,
+	.release		= video_device_release_empty,
 	.ioctl_ops		= &radio_si4713_ioctl_ops,
 	.vfl_dir		= VFL_DIR_TX,
 };
@@ -275,6 +169,7 @@ static int radio_si4713_pdriver_probe(struct platform_device *pdev)
 		rval = -ENOMEM;
 		goto exit;
 	}
+	mutex_init(&rsdev->lock);
 
 	rval = v4l2_device_register(&pdev->dev, &rsdev->v4l2_dev);
 	if (rval) {
@@ -285,40 +180,35 @@ static int radio_si4713_pdriver_probe(struct platform_device *pdev)
 	adapter = i2c_get_adapter(pdata->i2c_bus);
 	if (!adapter) {
 		dev_err(&pdev->dev, "Cannot get i2c adapter %d\n",
-							pdata->i2c_bus);
+			pdata->i2c_bus);
 		rval = -ENODEV;
 		goto unregister_v4l2_dev;
 	}
 
 	sd = v4l2_i2c_new_subdev_board(&rsdev->v4l2_dev, adapter,
-					pdata->subdev_board_info, NULL);
+				       pdata->subdev_board_info, NULL);
 	if (!sd) {
 		dev_err(&pdev->dev, "Cannot get v4l2 subdevice\n");
 		rval = -ENODEV;
 		goto put_adapter;
 	}
 
-	rsdev->radio_dev = video_device_alloc();
-	if (!rsdev->radio_dev) {
-		dev_err(&pdev->dev, "Failed to alloc video device.\n");
-		rval = -ENOMEM;
-		goto put_adapter;
-	}
-
-	memcpy(rsdev->radio_dev, &radio_si4713_vdev_template,
-			sizeof(radio_si4713_vdev_template));
-	video_set_drvdata(rsdev->radio_dev, rsdev);
-	if (video_register_device(rsdev->radio_dev, VFL_TYPE_RADIO, radio_nr)) {
+	rsdev->radio_dev = radio_si4713_vdev_template;
+	rsdev->radio_dev.v4l2_dev = &rsdev->v4l2_dev;
+	rsdev->radio_dev.ctrl_handler = sd->ctrl_handler;
+	set_bit(V4L2_FL_USE_FH_PRIO, &rsdev->radio_dev.flags);
+	/* Serialize all access to the si4713 */
+	rsdev->radio_dev.lock = &rsdev->lock;
+	video_set_drvdata(&rsdev->radio_dev, rsdev);
+	if (video_register_device(&rsdev->radio_dev, VFL_TYPE_RADIO, radio_nr)) {
 		dev_err(&pdev->dev, "Could not register video device.\n");
 		rval = -EIO;
-		goto free_vdev;
+		goto put_adapter;
 	}
 	dev_info(&pdev->dev, "New device successfully probed\n");
 
 	goto exit;
 
-free_vdev:
-	video_device_release(rsdev->radio_dev);
 put_adapter:
 	i2c_put_adapter(adapter);
 unregister_v4l2_dev:
@@ -328,17 +218,16 @@ exit:
 }
 
 /* radio_si4713_pdriver_remove - remove the device */
-static int __exit radio_si4713_pdriver_remove(struct platform_device *pdev)
+static int radio_si4713_pdriver_remove(struct platform_device *pdev)
 {
 	struct v4l2_device *v4l2_dev = platform_get_drvdata(pdev);
-	struct radio_si4713_device *rsdev = container_of(v4l2_dev,
-						struct radio_si4713_device,
-						v4l2_dev);
 	struct v4l2_subdev *sd = list_entry(v4l2_dev->subdevs.next,
 					    struct v4l2_subdev, list);
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct radio_si4713_device *rsdev;
 
-	video_unregister_device(rsdev->radio_dev);
+	rsdev = container_of(v4l2_dev, struct radio_si4713_device, v4l2_dev);
+	video_unregister_device(&rsdev->radio_dev);
 	i2c_put_adapter(client->adapter);
 	v4l2_device_unregister(&rsdev->v4l2_dev);
 
@@ -348,9 +237,10 @@ static int __exit radio_si4713_pdriver_remove(struct platform_device *pdev)
 static struct platform_driver radio_si4713_pdriver = {
 	.driver		= {
 		.name	= "radio-si4713",
+		.owner	= THIS_MODULE,
 	},
 	.probe		= radio_si4713_pdriver_probe,
-	.remove         = __exit_p(radio_si4713_pdriver_remove),
+	.remove         = radio_si4713_pdriver_remove,
 };
 
 module_platform_driver(radio_si4713_pdriver);
diff --git a/drivers/media/radio/radio-si476x.c b/drivers/media/radio/radio-si476x.c
new file mode 100644
index 00000000000..9430c6a2993
--- /dev/null
+++ b/drivers/media/radio/radio-si476x.c
@@ -0,0 +1,1599 @@
+/*
+ * drivers/media/radio/radio-si476x.c -- V4L2 driver for SI476X chips
+ *
+ * Copyright (C) 2012 Innovative Converged Devices(ICD)
+ * Copyright (C) 2013 Andrey Smirnov
+ *
+ * Author: Andrey Smirnov <andrew.smirnov@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; version 2 of the License.
+ *
+ * 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/module.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/atomic.h>
+#include <linux/videodev2.h>
+#include <linux/mutex.h>
+#include <linux/debugfs.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 <media/si476x.h>
+#include <linux/mfd/si476x-core.h>
+
+#define FM_FREQ_RANGE_LOW   64000000
+#define FM_FREQ_RANGE_HIGH 108000000
+
+#define AM_FREQ_RANGE_LOW    520000
+#define AM_FREQ_RANGE_HIGH 30000000
+
+#define PWRLINEFLTR (1 << 8)
+
+#define FREQ_MUL (10000000 / 625)
+
+#define SI476X_PHDIV_STATUS_LINK_LOCKED(status) (0b10000000 & (status))
+
+#define DRIVER_NAME "si476x-radio"
+#define DRIVER_CARD "SI476x AM/FM Receiver"
+
+enum si476x_freq_bands {
+	SI476X_BAND_FM,
+	SI476X_BAND_AM,
+};
+
+static const struct v4l2_frequency_band si476x_bands[] = {
+	[SI476X_BAND_FM] = {
+		.type		= V4L2_TUNER_RADIO,
+		.index		= SI476X_BAND_FM,
+		.capability	= V4L2_TUNER_CAP_LOW
+		| V4L2_TUNER_CAP_STEREO
+		| V4L2_TUNER_CAP_RDS
+		| V4L2_TUNER_CAP_RDS_BLOCK_IO
+		| V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow	=  64 * FREQ_MUL,
+		.rangehigh	= 108 * FREQ_MUL,
+		.modulation	= V4L2_BAND_MODULATION_FM,
+	},
+	[SI476X_BAND_AM] = {
+		.type		= V4L2_TUNER_RADIO,
+		.index		= SI476X_BAND_AM,
+		.capability	= V4L2_TUNER_CAP_LOW
+		| V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow	= 0.52 * FREQ_MUL,
+		.rangehigh	= 30 * FREQ_MUL,
+		.modulation	= V4L2_BAND_MODULATION_AM,
+	},
+};
+
+static inline bool si476x_radio_freq_is_inside_of_the_band(u32 freq, int band)
+{
+	return freq >= si476x_bands[band].rangelow &&
+		freq <= si476x_bands[band].rangehigh;
+}
+
+static inline bool si476x_radio_range_is_inside_of_the_band(u32 low, u32 high,
+							    int band)
+{
+	return low  >= si476x_bands[band].rangelow &&
+		high <= si476x_bands[band].rangehigh;
+}
+
+static int si476x_radio_s_ctrl(struct v4l2_ctrl *ctrl);
+static int si476x_radio_g_volatile_ctrl(struct v4l2_ctrl *ctrl);
+
+enum phase_diversity_modes_idx {
+	SI476X_IDX_PHDIV_DISABLED,
+	SI476X_IDX_PHDIV_PRIMARY_COMBINING,
+	SI476X_IDX_PHDIV_PRIMARY_ANTENNA,
+	SI476X_IDX_PHDIV_SECONDARY_ANTENNA,
+	SI476X_IDX_PHDIV_SECONDARY_COMBINING,
+};
+
+static const char * const phase_diversity_modes[] = {
+	[SI476X_IDX_PHDIV_DISABLED]		= "Disabled",
+	[SI476X_IDX_PHDIV_PRIMARY_COMBINING]	= "Primary with Secondary",
+	[SI476X_IDX_PHDIV_PRIMARY_ANTENNA]	= "Primary Antenna",
+	[SI476X_IDX_PHDIV_SECONDARY_ANTENNA]	= "Secondary Antenna",
+	[SI476X_IDX_PHDIV_SECONDARY_COMBINING]	= "Secondary with Primary",
+};
+
+static inline enum phase_diversity_modes_idx
+si476x_phase_diversity_mode_to_idx(enum si476x_phase_diversity_mode mode)
+{
+	switch (mode) {
+	default:		/* FALLTHROUGH */
+	case SI476X_PHDIV_DISABLED:
+		return SI476X_IDX_PHDIV_DISABLED;
+	case SI476X_PHDIV_PRIMARY_COMBINING:
+		return SI476X_IDX_PHDIV_PRIMARY_COMBINING;
+	case SI476X_PHDIV_PRIMARY_ANTENNA:
+		return SI476X_IDX_PHDIV_PRIMARY_ANTENNA;
+	case SI476X_PHDIV_SECONDARY_ANTENNA:
+		return SI476X_IDX_PHDIV_SECONDARY_ANTENNA;
+	case SI476X_PHDIV_SECONDARY_COMBINING:
+		return SI476X_IDX_PHDIV_SECONDARY_COMBINING;
+	}
+}
+
+static inline enum si476x_phase_diversity_mode
+si476x_phase_diversity_idx_to_mode(enum phase_diversity_modes_idx idx)
+{
+	static const int idx_to_value[] = {
+		[SI476X_IDX_PHDIV_DISABLED]		= SI476X_PHDIV_DISABLED,
+		[SI476X_IDX_PHDIV_PRIMARY_COMBINING]	= SI476X_PHDIV_PRIMARY_COMBINING,
+		[SI476X_IDX_PHDIV_PRIMARY_ANTENNA]	= SI476X_PHDIV_PRIMARY_ANTENNA,
+		[SI476X_IDX_PHDIV_SECONDARY_ANTENNA]	= SI476X_PHDIV_SECONDARY_ANTENNA,
+		[SI476X_IDX_PHDIV_SECONDARY_COMBINING]	= SI476X_PHDIV_SECONDARY_COMBINING,
+	};
+
+	return idx_to_value[idx];
+}
+
+static const struct v4l2_ctrl_ops si476x_ctrl_ops = {
+	.g_volatile_ctrl	= si476x_radio_g_volatile_ctrl,
+	.s_ctrl			= si476x_radio_s_ctrl,
+};
+
+
+enum si476x_ctrl_idx {
+	SI476X_IDX_RSSI_THRESHOLD,
+	SI476X_IDX_SNR_THRESHOLD,
+	SI476X_IDX_MAX_TUNE_ERROR,
+	SI476X_IDX_HARMONICS_COUNT,
+	SI476X_IDX_DIVERSITY_MODE,
+	SI476X_IDX_INTERCHIP_LINK,
+};
+static struct v4l2_ctrl_config si476x_ctrls[] = {
+
+	/**
+	 * SI476X during its station seeking(or tuning) process uses several
+	 * parameters to detrmine if "the station" is valid:
+	 *
+	 *	- Signal's SNR(in dBuV) must be lower than
+	 *	#V4L2_CID_SI476X_SNR_THRESHOLD
+	 *	- Signal's RSSI(in dBuV) must be greater than
+	 *	#V4L2_CID_SI476X_RSSI_THRESHOLD
+	 *	- Signal's frequency deviation(in units of 2ppm) must not be
+	 *	more than #V4L2_CID_SI476X_MAX_TUNE_ERROR
+	 */
+	[SI476X_IDX_RSSI_THRESHOLD] = {
+		.ops	= &si476x_ctrl_ops,
+		.id	= V4L2_CID_SI476X_RSSI_THRESHOLD,
+		.name	= "Valid RSSI Threshold",
+		.type	= V4L2_CTRL_TYPE_INTEGER,
+		.min	= -128,
+		.max	= 127,
+		.step	= 1,
+	},
+	[SI476X_IDX_SNR_THRESHOLD] = {
+		.ops	= &si476x_ctrl_ops,
+		.id	= V4L2_CID_SI476X_SNR_THRESHOLD,
+		.type	= V4L2_CTRL_TYPE_INTEGER,
+		.name	= "Valid SNR Threshold",
+		.min	= -128,
+		.max	= 127,
+		.step	= 1,
+	},
+	[SI476X_IDX_MAX_TUNE_ERROR] = {
+		.ops	= &si476x_ctrl_ops,
+		.id	= V4L2_CID_SI476X_MAX_TUNE_ERROR,
+		.type	= V4L2_CTRL_TYPE_INTEGER,
+		.name	= "Max Tune Errors",
+		.min	= 0,
+		.max	= 126 * 2,
+		.step	= 2,
+	},
+
+	/**
+	 * #V4L2_CID_SI476X_HARMONICS_COUNT -- number of harmonics
+	 * built-in power-line noise supression filter is to reject
+	 * during AM-mode operation.
+	 */
+	[SI476X_IDX_HARMONICS_COUNT] = {
+		.ops	= &si476x_ctrl_ops,
+		.id	= V4L2_CID_SI476X_HARMONICS_COUNT,
+		.type	= V4L2_CTRL_TYPE_INTEGER,
+
+		.name	= "Count of Harmonics to Reject",
+		.min	= 0,
+		.max	= 20,
+		.step	= 1,
+	},
+
+	/**
+	 * #V4L2_CID_SI476X_DIVERSITY_MODE -- configuration which
+	 * two tuners working in diversity mode are to work in.
+	 *
+	 *  - #SI476X_IDX_PHDIV_DISABLED diversity mode disabled
+	 *  - #SI476X_IDX_PHDIV_PRIMARY_COMBINING diversity mode is
+	 *  on, primary tuner's antenna is the main one.
+	 *  - #SI476X_IDX_PHDIV_PRIMARY_ANTENNA diversity mode is
+	 *  off, primary tuner's antenna is the main one.
+	 *  - #SI476X_IDX_PHDIV_SECONDARY_ANTENNA diversity mode is
+	 *  off, secondary tuner's antenna is the main one.
+	 *  - #SI476X_IDX_PHDIV_SECONDARY_COMBINING diversity mode is
+	 *  on, secondary tuner's antenna is the main one.
+	 */
+	[SI476X_IDX_DIVERSITY_MODE] = {
+		.ops	= &si476x_ctrl_ops,
+		.id	= V4L2_CID_SI476X_DIVERSITY_MODE,
+		.type	= V4L2_CTRL_TYPE_MENU,
+		.name	= "Phase Diversity Mode",
+		.qmenu	= phase_diversity_modes,
+		.min	= 0,
+		.max	= ARRAY_SIZE(phase_diversity_modes) - 1,
+	},
+
+	/**
+	 * #V4L2_CID_SI476X_INTERCHIP_LINK -- inter-chip link in
+	 * diversity mode indicator. Allows user to determine if two
+	 * chips working in diversity mode have established a link
+	 * between each other and if the system as a whole uses
+	 * signals from both antennas to receive FM radio.
+	 */
+	[SI476X_IDX_INTERCHIP_LINK] = {
+		.ops	= &si476x_ctrl_ops,
+		.id	= V4L2_CID_SI476X_INTERCHIP_LINK,
+		.type	= V4L2_CTRL_TYPE_BOOLEAN,
+		.flags  = V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_VOLATILE,
+		.name	= "Inter-Chip Link",
+		.min	= 0,
+		.max	= 1,
+		.step	= 1,
+	},
+};
+
+struct si476x_radio;
+
+/**
+ * struct si476x_radio_ops - vtable of tuner functions
+ *
+ * This table holds pointers to functions implementing particular
+ * operations depending on the mode in which the tuner chip was
+ * configured to start in. If the function is not supported
+ * corresponding element is set to #NULL.
+ *
+ * @tune_freq: Tune chip to a specific frequency
+ * @seek_start: Star station seeking
+ * @rsq_status: Get Recieved Signal Quality(RSQ) status
+ * @rds_blckcnt: Get recived RDS blocks count
+ * @phase_diversity: Change phase diversity mode of the tuner
+ * @phase_div_status: Get phase diversity mode status
+ * @acf_status: Get the status of Automatically Controlled
+ * Features(ACF)
+ * @agc_status: Get Automatic Gain Control(AGC) status
+ */
+struct si476x_radio_ops {
+	int (*tune_freq)(struct si476x_core *, struct si476x_tune_freq_args *);
+	int (*seek_start)(struct si476x_core *, bool, bool);
+	int (*rsq_status)(struct si476x_core *, struct si476x_rsq_status_args *,
+			  struct si476x_rsq_status_report *);
+	int (*rds_blckcnt)(struct si476x_core *, bool,
+			   struct si476x_rds_blockcount_report *);
+
+	int (*phase_diversity)(struct si476x_core *,
+			       enum si476x_phase_diversity_mode);
+	int (*phase_div_status)(struct si476x_core *);
+	int (*acf_status)(struct si476x_core *,
+			  struct si476x_acf_status_report *);
+	int (*agc_status)(struct si476x_core *,
+			  struct si476x_agc_status_report *);
+};
+
+/**
+ * struct si476x_radio - radio device
+ *
+ * @core: Pointer to underlying core device
+ * @videodev: Pointer to video device created by V4L2 subsystem
+ * @ops: Vtable of functions. See struct si476x_radio_ops for details
+ * @kref: Reference counter
+ * @core_lock: An r/w semaphore to brebvent the deletion of underlying
+ * core structure is the radio device is being used
+ */
+struct si476x_radio {
+	struct v4l2_device v4l2dev;
+	struct video_device videodev;
+	struct v4l2_ctrl_handler ctrl_handler;
+
+	struct si476x_core  *core;
+	/* This field should not be accesses unless core lock is held */
+	const struct si476x_radio_ops *ops;
+
+	struct dentry	*debugfs;
+	u32 audmode;
+};
+
+static inline struct si476x_radio *
+v4l2_dev_to_radio(struct v4l2_device *d)
+{
+	return container_of(d, struct si476x_radio, v4l2dev);
+}
+
+static inline struct si476x_radio *
+v4l2_ctrl_handler_to_radio(struct v4l2_ctrl_handler *d)
+{
+	return container_of(d, struct si476x_radio, ctrl_handler);
+}
+
+/*
+ * si476x_vidioc_querycap - query device capabilities
+ */
+static int si476x_radio_querycap(struct file *file, void *priv,
+				 struct v4l2_capability *capability)
+{
+	struct si476x_radio *radio = video_drvdata(file);
+
+	strlcpy(capability->driver, radio->v4l2dev.name,
+		sizeof(capability->driver));
+	strlcpy(capability->card,   DRIVER_CARD, sizeof(capability->card));
+	snprintf(capability->bus_info, sizeof(capability->bus_info),
+		 "platform:%s", radio->v4l2dev.name);
+
+	capability->device_caps = V4L2_CAP_TUNER
+		| V4L2_CAP_RADIO
+		| V4L2_CAP_HW_FREQ_SEEK;
+
+	si476x_core_lock(radio->core);
+	if (!si476x_core_is_a_secondary_tuner(radio->core))
+		capability->device_caps |= V4L2_CAP_RDS_CAPTURE
+			| V4L2_CAP_READWRITE;
+	si476x_core_unlock(radio->core);
+
+	capability->capabilities = capability->device_caps
+		| V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+static int si476x_radio_enum_freq_bands(struct file *file, void *priv,
+					struct v4l2_frequency_band *band)
+{
+	int err;
+	struct si476x_radio *radio = video_drvdata(file);
+
+	if (band->tuner != 0)
+		return -EINVAL;
+
+	switch (radio->core->chip_id) {
+		/* AM/FM tuners -- all bands are supported */
+	case SI476X_CHIP_SI4761:
+	case SI476X_CHIP_SI4764:
+		if (band->index < ARRAY_SIZE(si476x_bands)) {
+			*band = si476x_bands[band->index];
+			err = 0;
+		} else {
+			err = -EINVAL;
+		}
+		break;
+		/* FM companion tuner chips -- only FM bands are
+		 * supported */
+	case SI476X_CHIP_SI4768:
+		if (band->index == SI476X_BAND_FM) {
+			*band = si476x_bands[band->index];
+			err = 0;
+		} else {
+			err = -EINVAL;
+		}
+		break;
+	default:
+		err = -EINVAL;
+	}
+
+	return err;
+}
+
+static int si476x_radio_g_tuner(struct file *file, void *priv,
+				struct v4l2_tuner *tuner)
+{
+	int err;
+	struct si476x_rsq_status_report report;
+	struct si476x_radio *radio = video_drvdata(file);
+
+	struct si476x_rsq_status_args args = {
+		.primary	= false,
+		.rsqack		= false,
+		.attune		= false,
+		.cancel		= false,
+		.stcack		= false,
+	};
+
+	if (tuner->index != 0)
+		return -EINVAL;
+
+	tuner->type       = V4L2_TUNER_RADIO;
+	tuner->capability = V4L2_TUNER_CAP_LOW /* Measure frequencies
+						 * in multiples of
+						 * 62.5 Hz */
+		| V4L2_TUNER_CAP_STEREO
+		| V4L2_TUNER_CAP_HWSEEK_BOUNDED
+		| V4L2_TUNER_CAP_HWSEEK_WRAP
+		| V4L2_TUNER_CAP_HWSEEK_PROG_LIM;
+
+	si476x_core_lock(radio->core);
+
+	if (si476x_core_is_a_secondary_tuner(radio->core)) {
+		strlcpy(tuner->name, "FM (secondary)", sizeof(tuner->name));
+		tuner->rxsubchans = 0;
+		tuner->rangelow = si476x_bands[SI476X_BAND_FM].rangelow;
+	} else if (si476x_core_has_am(radio->core)) {
+		if (si476x_core_is_a_primary_tuner(radio->core))
+			strlcpy(tuner->name, "AM/FM (primary)",
+				sizeof(tuner->name));
+		else
+			strlcpy(tuner->name, "AM/FM", sizeof(tuner->name));
+
+		tuner->rxsubchans = V4L2_TUNER_SUB_MONO | V4L2_TUNER_SUB_STEREO
+			| V4L2_TUNER_SUB_RDS;
+		tuner->capability |= V4L2_TUNER_CAP_RDS
+			| V4L2_TUNER_CAP_RDS_BLOCK_IO
+			| V4L2_TUNER_CAP_FREQ_BANDS;
+
+		tuner->rangelow = si476x_bands[SI476X_BAND_AM].rangelow;
+	} else {
+		strlcpy(tuner->name, "FM", sizeof(tuner->name));
+		tuner->rxsubchans = V4L2_TUNER_SUB_RDS;
+		tuner->capability |= V4L2_TUNER_CAP_RDS
+			| V4L2_TUNER_CAP_RDS_BLOCK_IO
+			| V4L2_TUNER_CAP_FREQ_BANDS;
+		tuner->rangelow = si476x_bands[SI476X_BAND_FM].rangelow;
+	}
+
+	tuner->audmode = radio->audmode;
+
+	tuner->afc = 1;
+	tuner->rangehigh = si476x_bands[SI476X_BAND_FM].rangehigh;
+
+	err = radio->ops->rsq_status(radio->core,
+				     &args, &report);
+	if (err < 0) {
+		tuner->signal = 0;
+	} else {
+		/*
+		 * tuner->signal value range: 0x0000 .. 0xFFFF,
+		 * report.rssi: -128 .. 127
+		 */
+		tuner->signal = (report.rssi + 128) * 257;
+	}
+	si476x_core_unlock(radio->core);
+
+	return err;
+}
+
+static int si476x_radio_s_tuner(struct file *file, void *priv,
+				const struct v4l2_tuner *tuner)
+{
+	struct si476x_radio *radio = video_drvdata(file);
+
+	if (tuner->index != 0)
+		return -EINVAL;
+
+	if (tuner->audmode == V4L2_TUNER_MODE_MONO ||
+	    tuner->audmode == V4L2_TUNER_MODE_STEREO)
+		radio->audmode = tuner->audmode;
+	else
+		radio->audmode = V4L2_TUNER_MODE_STEREO;
+
+	return 0;
+}
+
+static int si476x_radio_init_vtable(struct si476x_radio *radio,
+				    enum si476x_func func)
+{
+	static const struct si476x_radio_ops fm_ops = {
+		.tune_freq		= si476x_core_cmd_fm_tune_freq,
+		.seek_start		= si476x_core_cmd_fm_seek_start,
+		.rsq_status		= si476x_core_cmd_fm_rsq_status,
+		.rds_blckcnt		= si476x_core_cmd_fm_rds_blockcount,
+		.phase_diversity	= si476x_core_cmd_fm_phase_diversity,
+		.phase_div_status	= si476x_core_cmd_fm_phase_div_status,
+		.acf_status		= si476x_core_cmd_fm_acf_status,
+		.agc_status		= si476x_core_cmd_agc_status,
+	};
+
+	static const struct si476x_radio_ops am_ops = {
+		.tune_freq		= si476x_core_cmd_am_tune_freq,
+		.seek_start		= si476x_core_cmd_am_seek_start,
+		.rsq_status		= si476x_core_cmd_am_rsq_status,
+		.rds_blckcnt		= NULL,
+		.phase_diversity	= NULL,
+		.phase_div_status	= NULL,
+		.acf_status		= si476x_core_cmd_am_acf_status,
+		.agc_status		= NULL,
+	};
+
+	switch (func) {
+	case SI476X_FUNC_FM_RECEIVER:
+		radio->ops = &fm_ops;
+		return 0;
+
+	case SI476X_FUNC_AM_RECEIVER:
+		radio->ops = &am_ops;
+		return 0;
+	default:
+		WARN(1, "Unexpected tuner function value\n");
+		return -EINVAL;
+	}
+}
+
+static int si476x_radio_pretune(struct si476x_radio *radio,
+				enum si476x_func func)
+{
+	int retval;
+
+	struct si476x_tune_freq_args args = {
+		.zifsr		= false,
+		.hd		= false,
+		.injside	= SI476X_INJSIDE_AUTO,
+		.tunemode	= SI476X_TM_VALIDATED_NORMAL_TUNE,
+		.smoothmetrics	= SI476X_SM_INITIALIZE_AUDIO,
+		.antcap		= 0,
+	};
+
+	switch (func) {
+	case SI476X_FUNC_FM_RECEIVER:
+		args.freq = v4l2_to_si476x(radio->core,
+					   92 * FREQ_MUL);
+		retval = radio->ops->tune_freq(radio->core, &args);
+		break;
+	case SI476X_FUNC_AM_RECEIVER:
+		args.freq = v4l2_to_si476x(radio->core,
+					   0.6 * FREQ_MUL);
+		retval = radio->ops->tune_freq(radio->core, &args);
+		break;
+	default:
+		WARN(1, "Unexpected tuner function value\n");
+		retval = -EINVAL;
+	}
+
+	return retval;
+}
+static int si476x_radio_do_post_powerup_init(struct si476x_radio *radio,
+					     enum si476x_func func)
+{
+	int err;
+
+	/* regcache_mark_dirty(radio->core->regmap); */
+	err = regcache_sync_region(radio->core->regmap,
+				   SI476X_PROP_DIGITAL_IO_INPUT_SAMPLE_RATE,
+				   SI476X_PROP_DIGITAL_IO_OUTPUT_FORMAT);
+		if (err < 0)
+			return err;
+
+	err = regcache_sync_region(radio->core->regmap,
+				   SI476X_PROP_AUDIO_DEEMPHASIS,
+				   SI476X_PROP_AUDIO_PWR_LINE_FILTER);
+	if (err < 0)
+		return err;
+
+	err = regcache_sync_region(radio->core->regmap,
+				   SI476X_PROP_INT_CTL_ENABLE,
+				   SI476X_PROP_INT_CTL_ENABLE);
+	if (err < 0)
+		return err;
+
+	/*
+	 * Is there any point in restoring SNR and the like
+	 * when switching between AM/FM?
+	 */
+	err = regcache_sync_region(radio->core->regmap,
+				   SI476X_PROP_VALID_MAX_TUNE_ERROR,
+				   SI476X_PROP_VALID_MAX_TUNE_ERROR);
+	if (err < 0)
+		return err;
+
+	err = regcache_sync_region(radio->core->regmap,
+				   SI476X_PROP_VALID_SNR_THRESHOLD,
+				   SI476X_PROP_VALID_RSSI_THRESHOLD);
+	if (err < 0)
+		return err;
+
+	if (func == SI476X_FUNC_FM_RECEIVER) {
+		if (si476x_core_has_diversity(radio->core)) {
+			err = si476x_core_cmd_fm_phase_diversity(radio->core,
+								 radio->core->diversity_mode);
+			if (err < 0)
+				return err;
+		}
+
+		err = regcache_sync_region(radio->core->regmap,
+					   SI476X_PROP_FM_RDS_INTERRUPT_SOURCE,
+					   SI476X_PROP_FM_RDS_CONFIG);
+		if (err < 0)
+			return err;
+	}
+
+	return si476x_radio_init_vtable(radio, func);
+
+}
+
+static int si476x_radio_change_func(struct si476x_radio *radio,
+				    enum si476x_func func)
+{
+	int err;
+	bool soft;
+	/*
+	 * Since power/up down is a very time consuming operation,
+	 * try to avoid doing it if the requested mode matches the one
+	 * the tuner is in
+	 */
+	if (func == radio->core->power_up_parameters.func)
+		return 0;
+
+	soft = true;
+	err = si476x_core_stop(radio->core, soft);
+	if (err < 0) {
+		/*
+		 * OK, if the chip does not want to play nice let's
+		 * try to reset it in more brutal way
+		 */
+		soft = false;
+		err = si476x_core_stop(radio->core, soft);
+		if (err < 0)
+			return err;
+	}
+	/*
+	  Set the desired radio tuner function
+	 */
+	radio->core->power_up_parameters.func = func;
+
+	err = si476x_core_start(radio->core, soft);
+	if (err < 0)
+		return err;
+
+	/*
+	 * No need to do the rest of manipulations for the bootlader
+	 * mode
+	 */
+	if (func != SI476X_FUNC_FM_RECEIVER &&
+	    func != SI476X_FUNC_AM_RECEIVER)
+		return err;
+
+	return si476x_radio_do_post_powerup_init(radio, func);
+}
+
+static int si476x_radio_g_frequency(struct file *file, void *priv,
+			      struct v4l2_frequency *f)
+{
+	int err;
+	struct si476x_radio *radio = video_drvdata(file);
+
+	if (f->tuner != 0 ||
+	    f->type  != V4L2_TUNER_RADIO)
+		return -EINVAL;
+
+	si476x_core_lock(radio->core);
+
+	if (radio->ops->rsq_status) {
+		struct si476x_rsq_status_report report;
+		struct si476x_rsq_status_args   args = {
+			.primary	= false,
+			.rsqack		= false,
+			.attune		= true,
+			.cancel		= false,
+			.stcack		= false,
+		};
+
+		err = radio->ops->rsq_status(radio->core, &args, &report);
+		if (!err)
+			f->frequency = si476x_to_v4l2(radio->core,
+						      report.readfreq);
+	} else {
+		err = -EINVAL;
+	}
+
+	si476x_core_unlock(radio->core);
+
+	return err;
+}
+
+static int si476x_radio_s_frequency(struct file *file, void *priv,
+				    const struct v4l2_frequency *f)
+{
+	int err;
+	u32 freq = f->frequency;
+	struct si476x_tune_freq_args args;
+	struct si476x_radio *radio = video_drvdata(file);
+
+	const u32 midrange = (si476x_bands[SI476X_BAND_AM].rangehigh +
+			      si476x_bands[SI476X_BAND_FM].rangelow) / 2;
+	const int band = (freq > midrange) ?
+		SI476X_BAND_FM : SI476X_BAND_AM;
+	const enum si476x_func func = (band == SI476X_BAND_AM) ?
+		SI476X_FUNC_AM_RECEIVER : SI476X_FUNC_FM_RECEIVER;
+
+	if (f->tuner != 0 ||
+	    f->type  != V4L2_TUNER_RADIO)
+		return -EINVAL;
+
+	si476x_core_lock(radio->core);
+
+	freq = clamp(freq,
+		     si476x_bands[band].rangelow,
+		     si476x_bands[band].rangehigh);
+
+	if (si476x_radio_freq_is_inside_of_the_band(freq,
+						    SI476X_BAND_AM) &&
+	    (!si476x_core_has_am(radio->core) ||
+	     si476x_core_is_a_secondary_tuner(radio->core))) {
+		err = -EINVAL;
+		goto unlock;
+	}
+
+	err = si476x_radio_change_func(radio, func);
+	if (err < 0)
+		goto unlock;
+
+	args.zifsr		= false;
+	args.hd			= false;
+	args.injside		= SI476X_INJSIDE_AUTO;
+	args.freq		= v4l2_to_si476x(radio->core, freq);
+	args.tunemode		= SI476X_TM_VALIDATED_NORMAL_TUNE;
+	args.smoothmetrics	= SI476X_SM_INITIALIZE_AUDIO;
+	args.antcap		= 0;
+
+	err = radio->ops->tune_freq(radio->core, &args);
+
+unlock:
+	si476x_core_unlock(radio->core);
+	return err;
+}
+
+static int si476x_radio_s_hw_freq_seek(struct file *file, void *priv,
+				       const struct v4l2_hw_freq_seek *seek)
+{
+	int err;
+	enum si476x_func func;
+	u32 rangelow, rangehigh;
+	struct si476x_radio *radio = video_drvdata(file);
+
+	if (file->f_flags & O_NONBLOCK)
+		return -EAGAIN;
+
+	if (seek->tuner != 0 ||
+	    seek->type  != V4L2_TUNER_RADIO)
+		return -EINVAL;
+
+	si476x_core_lock(radio->core);
+
+	if (!seek->rangelow) {
+		err = regmap_read(radio->core->regmap,
+				  SI476X_PROP_SEEK_BAND_BOTTOM,
+				  &rangelow);
+		if (!err)
+			rangelow = si476x_to_v4l2(radio->core, rangelow);
+		else
+			goto unlock;
+	}
+	if (!seek->rangehigh) {
+		err = regmap_read(radio->core->regmap,
+				  SI476X_PROP_SEEK_BAND_TOP,
+				  &rangehigh);
+		if (!err)
+			rangehigh = si476x_to_v4l2(radio->core, rangehigh);
+		else
+			goto unlock;
+	}
+
+	if (rangelow > rangehigh) {
+		err = -EINVAL;
+		goto unlock;
+	}
+
+	if (si476x_radio_range_is_inside_of_the_band(rangelow, rangehigh,
+						     SI476X_BAND_FM)) {
+		func = SI476X_FUNC_FM_RECEIVER;
+
+	} else if (si476x_core_has_am(radio->core) &&
+		   si476x_radio_range_is_inside_of_the_band(rangelow, rangehigh,
+							    SI476X_BAND_AM)) {
+		func = SI476X_FUNC_AM_RECEIVER;
+	} else {
+		err = -EINVAL;
+		goto unlock;
+	}
+
+	err = si476x_radio_change_func(radio, func);
+	if (err < 0)
+		goto unlock;
+
+	if (seek->rangehigh) {
+		err = regmap_write(radio->core->regmap,
+				   SI476X_PROP_SEEK_BAND_TOP,
+				   v4l2_to_si476x(radio->core,
+						  seek->rangehigh));
+		if (err)
+			goto unlock;
+	}
+	if (seek->rangelow) {
+		err = regmap_write(radio->core->regmap,
+				   SI476X_PROP_SEEK_BAND_BOTTOM,
+				   v4l2_to_si476x(radio->core,
+						  seek->rangelow));
+		if (err)
+			goto unlock;
+	}
+	if (seek->spacing) {
+		err = regmap_write(radio->core->regmap,
+				     SI476X_PROP_SEEK_FREQUENCY_SPACING,
+				     v4l2_to_si476x(radio->core,
+						    seek->spacing));
+		if (err)
+			goto unlock;
+	}
+
+	err = radio->ops->seek_start(radio->core,
+				     seek->seek_upward,
+				     seek->wrap_around);
+unlock:
+	si476x_core_unlock(radio->core);
+
+
+
+	return err;
+}
+
+static int si476x_radio_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	int retval;
+	struct si476x_radio *radio = v4l2_ctrl_handler_to_radio(ctrl->handler);
+
+	si476x_core_lock(radio->core);
+
+	switch (ctrl->id) {
+	case V4L2_CID_SI476X_INTERCHIP_LINK:
+		if (si476x_core_has_diversity(radio->core)) {
+			if (radio->ops->phase_diversity) {
+				retval = radio->ops->phase_div_status(radio->core);
+				if (retval < 0)
+					break;
+
+				ctrl->val = !!SI476X_PHDIV_STATUS_LINK_LOCKED(retval);
+				retval = 0;
+				break;
+			} else {
+				retval = -ENOTTY;
+				break;
+			}
+		}
+		retval = -EINVAL;
+		break;
+	default:
+		retval = -EINVAL;
+		break;
+	}
+	si476x_core_unlock(radio->core);
+	return retval;
+
+}
+
+static int si476x_radio_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	int retval;
+	enum si476x_phase_diversity_mode mode;
+	struct si476x_radio *radio = v4l2_ctrl_handler_to_radio(ctrl->handler);
+
+	si476x_core_lock(radio->core);
+
+	switch (ctrl->id) {
+	case V4L2_CID_SI476X_HARMONICS_COUNT:
+		retval = regmap_update_bits(radio->core->regmap,
+					    SI476X_PROP_AUDIO_PWR_LINE_FILTER,
+					    SI476X_PROP_PWR_HARMONICS_MASK,
+					    ctrl->val);
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		switch (ctrl->val) {
+		case V4L2_CID_POWER_LINE_FREQUENCY_DISABLED:
+			retval = regmap_update_bits(radio->core->regmap,
+						    SI476X_PROP_AUDIO_PWR_LINE_FILTER,
+						    SI476X_PROP_PWR_ENABLE_MASK,
+						    0);
+			break;
+		case V4L2_CID_POWER_LINE_FREQUENCY_50HZ:
+			retval = regmap_update_bits(radio->core->regmap,
+						    SI476X_PROP_AUDIO_PWR_LINE_FILTER,
+						    SI476X_PROP_PWR_GRID_MASK,
+						    SI476X_PROP_PWR_GRID_50HZ);
+			break;
+		case V4L2_CID_POWER_LINE_FREQUENCY_60HZ:
+			retval = regmap_update_bits(radio->core->regmap,
+						    SI476X_PROP_AUDIO_PWR_LINE_FILTER,
+						    SI476X_PROP_PWR_GRID_MASK,
+						    SI476X_PROP_PWR_GRID_60HZ);
+			break;
+		default:
+			retval = -EINVAL;
+			break;
+		}
+		break;
+	case V4L2_CID_SI476X_RSSI_THRESHOLD:
+		retval = regmap_write(radio->core->regmap,
+				      SI476X_PROP_VALID_RSSI_THRESHOLD,
+				      ctrl->val);
+		break;
+	case V4L2_CID_SI476X_SNR_THRESHOLD:
+		retval = regmap_write(radio->core->regmap,
+				      SI476X_PROP_VALID_SNR_THRESHOLD,
+				      ctrl->val);
+		break;
+	case V4L2_CID_SI476X_MAX_TUNE_ERROR:
+		retval = regmap_write(radio->core->regmap,
+				      SI476X_PROP_VALID_MAX_TUNE_ERROR,
+				      ctrl->val);
+		break;
+	case V4L2_CID_RDS_RECEPTION:
+		/*
+		 * It looks like RDS related properties are
+		 * inaccesable when tuner is in AM mode, so cache the
+		 * changes
+		 */
+		if (si476x_core_is_in_am_receiver_mode(radio->core))
+			regcache_cache_only(radio->core->regmap, true);
+
+		if (ctrl->val) {
+			retval = regmap_write(radio->core->regmap,
+					      SI476X_PROP_FM_RDS_INTERRUPT_FIFO_COUNT,
+					      radio->core->rds_fifo_depth);
+			if (retval < 0)
+				break;
+
+			if (radio->core->client->irq) {
+				retval = regmap_write(radio->core->regmap,
+						      SI476X_PROP_FM_RDS_INTERRUPT_SOURCE,
+						      SI476X_RDSRECV);
+				if (retval < 0)
+					break;
+			}
+
+			/* Drain RDS FIFO before enabling RDS processing */
+			retval = si476x_core_cmd_fm_rds_status(radio->core,
+							       false,
+							       true,
+							       true,
+							       NULL);
+			if (retval < 0)
+				break;
+
+			retval = regmap_update_bits(radio->core->regmap,
+						    SI476X_PROP_FM_RDS_CONFIG,
+						    SI476X_PROP_RDSEN_MASK,
+						    SI476X_PROP_RDSEN);
+		} else {
+			retval = regmap_update_bits(radio->core->regmap,
+						    SI476X_PROP_FM_RDS_CONFIG,
+						    SI476X_PROP_RDSEN_MASK,
+						    !SI476X_PROP_RDSEN);
+		}
+
+		if (si476x_core_is_in_am_receiver_mode(radio->core))
+			regcache_cache_only(radio->core->regmap, false);
+		break;
+	case V4L2_CID_TUNE_DEEMPHASIS:
+		retval = regmap_write(radio->core->regmap,
+				      SI476X_PROP_AUDIO_DEEMPHASIS,
+				      ctrl->val);
+		break;
+
+	case V4L2_CID_SI476X_DIVERSITY_MODE:
+		mode = si476x_phase_diversity_idx_to_mode(ctrl->val);
+
+		if (mode == radio->core->diversity_mode) {
+			retval = 0;
+			break;
+		}
+
+		if (si476x_core_is_in_am_receiver_mode(radio->core)) {
+			/*
+			 * Diversity cannot be configured while tuner
+			 * is in AM mode so save the changes and carry on.
+			 */
+			radio->core->diversity_mode = mode;
+			retval = 0;
+		} else {
+			retval = radio->ops->phase_diversity(radio->core, mode);
+			if (!retval)
+				radio->core->diversity_mode = mode;
+		}
+		break;
+
+	default:
+		retval = -EINVAL;
+		break;
+	}
+
+	si476x_core_unlock(radio->core);
+
+	return retval;
+}
+
+static int si476x_radio_g_chip_ident(struct file *file, void *fh,
+				     struct v4l2_dbg_chip_ident *chip)
+{
+	if (chip->match.type == V4L2_CHIP_MATCH_HOST &&
+	    v4l2_chip_match_host(&chip->match))
+		return 0;
+	return -EINVAL;
+}
+
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int si476x_radio_g_register(struct file *file, void *fh,
+				   struct v4l2_dbg_register *reg)
+{
+	int err;
+	unsigned int value;
+	struct si476x_radio *radio = video_drvdata(file);
+
+	si476x_core_lock(radio->core);
+	reg->size = 2;
+	err = regmap_read(radio->core->regmap,
+			  (unsigned int)reg->reg, &value);
+	reg->val = value;
+	si476x_core_unlock(radio->core);
+
+	return err;
+}
+static int si476x_radio_s_register(struct file *file, void *fh,
+				   const struct v4l2_dbg_register *reg)
+{
+
+	int err;
+	struct si476x_radio *radio = video_drvdata(file);
+
+	si476x_core_lock(radio->core);
+	err = regmap_write(radio->core->regmap,
+			   (unsigned int)reg->reg,
+			   (unsigned int)reg->val);
+	si476x_core_unlock(radio->core);
+
+	return err;
+}
+#endif
+
+static int si476x_radio_fops_open(struct file *file)
+{
+	struct si476x_radio *radio = video_drvdata(file);
+	int err;
+
+	err = v4l2_fh_open(file);
+	if (err)
+		return err;
+
+	if (v4l2_fh_is_singular_file(file)) {
+		si476x_core_lock(radio->core);
+		err = si476x_core_set_power_state(radio->core,
+						  SI476X_POWER_UP_FULL);
+		if (err < 0)
+			goto done;
+
+		err = si476x_radio_do_post_powerup_init(radio,
+							radio->core->power_up_parameters.func);
+		if (err < 0)
+			goto power_down;
+
+		err = si476x_radio_pretune(radio,
+					   radio->core->power_up_parameters.func);
+		if (err < 0)
+			goto power_down;
+
+		si476x_core_unlock(radio->core);
+		/*Must be done after si476x_core_unlock to prevent a deadlock*/
+		v4l2_ctrl_handler_setup(&radio->ctrl_handler);
+	}
+
+	return err;
+
+power_down:
+	si476x_core_set_power_state(radio->core,
+				    SI476X_POWER_DOWN);
+done:
+	si476x_core_unlock(radio->core);
+	v4l2_fh_release(file);
+
+	return err;
+}
+
+static int si476x_radio_fops_release(struct file *file)
+{
+	int err;
+	struct si476x_radio *radio = video_drvdata(file);
+
+	if (v4l2_fh_is_singular_file(file) &&
+	    atomic_read(&radio->core->is_alive))
+		si476x_core_set_power_state(radio->core,
+					    SI476X_POWER_DOWN);
+
+	err = v4l2_fh_release(file);
+
+	return err;
+}
+
+static ssize_t si476x_radio_fops_read(struct file *file, char __user *buf,
+				      size_t count, loff_t *ppos)
+{
+	ssize_t      rval;
+	size_t       fifo_len;
+	unsigned int copied;
+
+	struct si476x_radio *radio = video_drvdata(file);
+
+	/* block if no new data available */
+	if (kfifo_is_empty(&radio->core->rds_fifo)) {
+		if (file->f_flags & O_NONBLOCK)
+			return -EWOULDBLOCK;
+
+		rval = wait_event_interruptible(radio->core->rds_read_queue,
+						(!kfifo_is_empty(&radio->core->rds_fifo) ||
+						 !atomic_read(&radio->core->is_alive)));
+		if (rval < 0)
+			return -EINTR;
+
+		if (!atomic_read(&radio->core->is_alive))
+			return -ENODEV;
+	}
+
+	fifo_len = kfifo_len(&radio->core->rds_fifo);
+
+	if (kfifo_to_user(&radio->core->rds_fifo, buf,
+			  min(fifo_len, count),
+			  &copied) != 0) {
+		dev_warn(&radio->videodev.dev,
+			 "Error during FIFO to userspace copy\n");
+		rval = -EIO;
+	} else {
+		rval = (ssize_t)copied;
+	}
+
+	return rval;
+}
+
+static unsigned int si476x_radio_fops_poll(struct file *file,
+				struct poll_table_struct *pts)
+{
+	struct si476x_radio *radio = video_drvdata(file);
+	unsigned long req_events = poll_requested_events(pts);
+	unsigned int err = v4l2_ctrl_poll(file, pts);
+
+	if (req_events & (POLLIN | POLLRDNORM)) {
+		if (atomic_read(&radio->core->is_alive))
+			poll_wait(file, &radio->core->rds_read_queue, pts);
+
+		if (!atomic_read(&radio->core->is_alive))
+			err = POLLHUP;
+
+		if (!kfifo_is_empty(&radio->core->rds_fifo))
+			err = POLLIN | POLLRDNORM;
+	}
+
+	return err;
+}
+
+static const struct v4l2_file_operations si476x_fops = {
+	.owner			= THIS_MODULE,
+	.read			= si476x_radio_fops_read,
+	.poll			= si476x_radio_fops_poll,
+	.unlocked_ioctl		= video_ioctl2,
+	.open			= si476x_radio_fops_open,
+	.release		= si476x_radio_fops_release,
+};
+
+
+static const struct v4l2_ioctl_ops si4761_ioctl_ops = {
+	.vidioc_querycap		= si476x_radio_querycap,
+	.vidioc_g_tuner			= si476x_radio_g_tuner,
+	.vidioc_s_tuner			= si476x_radio_s_tuner,
+
+	.vidioc_g_frequency		= si476x_radio_g_frequency,
+	.vidioc_s_frequency		= si476x_radio_s_frequency,
+	.vidioc_s_hw_freq_seek		= si476x_radio_s_hw_freq_seek,
+	.vidioc_enum_freq_bands		= si476x_radio_enum_freq_bands,
+
+	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+
+	.vidioc_g_chip_ident		= si476x_radio_g_chip_ident,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.vidioc_g_register		= si476x_radio_g_register,
+	.vidioc_s_register		= si476x_radio_s_register,
+#endif
+};
+
+
+static const struct video_device si476x_viddev_template = {
+	.fops			= &si476x_fops,
+	.name			= DRIVER_NAME,
+	.release		= video_device_release_empty,
+};
+
+
+
+static ssize_t si476x_radio_read_acf_blob(struct file *file,
+					  char __user *user_buf,
+					  size_t count, loff_t *ppos)
+{
+	int err;
+	struct si476x_radio *radio = file->private_data;
+	struct si476x_acf_status_report report;
+
+	si476x_core_lock(radio->core);
+	if (radio->ops->acf_status)
+		err = radio->ops->acf_status(radio->core, &report);
+	else
+		err = -ENOENT;
+	si476x_core_unlock(radio->core);
+
+	if (err < 0)
+		return err;
+
+	return simple_read_from_buffer(user_buf, count, ppos, &report,
+				       sizeof(report));
+}
+
+static const struct file_operations radio_acf_fops = {
+	.open	= simple_open,
+	.llseek = default_llseek,
+	.read	= si476x_radio_read_acf_blob,
+};
+
+static ssize_t si476x_radio_read_rds_blckcnt_blob(struct file *file,
+						  char __user *user_buf,
+						  size_t count, loff_t *ppos)
+{
+	int err;
+	struct si476x_radio *radio = file->private_data;
+	struct si476x_rds_blockcount_report report;
+
+	si476x_core_lock(radio->core);
+	if (radio->ops->rds_blckcnt)
+		err = radio->ops->rds_blckcnt(radio->core, true,
+					       &report);
+	else
+		err = -ENOENT;
+	si476x_core_unlock(radio->core);
+
+	if (err < 0)
+		return err;
+
+	return simple_read_from_buffer(user_buf, count, ppos, &report,
+				       sizeof(report));
+}
+
+static const struct file_operations radio_rds_blckcnt_fops = {
+	.open	= simple_open,
+	.llseek = default_llseek,
+	.read	= si476x_radio_read_rds_blckcnt_blob,
+};
+
+static ssize_t si476x_radio_read_agc_blob(struct file *file,
+					  char __user *user_buf,
+					  size_t count, loff_t *ppos)
+{
+	int err;
+	struct si476x_radio *radio = file->private_data;
+	struct si476x_agc_status_report report;
+
+	si476x_core_lock(radio->core);
+	if (radio->ops->rds_blckcnt)
+		err = radio->ops->agc_status(radio->core, &report);
+	else
+		err = -ENOENT;
+	si476x_core_unlock(radio->core);
+
+	if (err < 0)
+		return err;
+
+	return simple_read_from_buffer(user_buf, count, ppos, &report,
+				       sizeof(report));
+}
+
+static const struct file_operations radio_agc_fops = {
+	.open	= simple_open,
+	.llseek = default_llseek,
+	.read	= si476x_radio_read_agc_blob,
+};
+
+static ssize_t si476x_radio_read_rsq_blob(struct file *file,
+					  char __user *user_buf,
+					  size_t count, loff_t *ppos)
+{
+	int err;
+	struct si476x_radio *radio = file->private_data;
+	struct si476x_rsq_status_report report;
+	struct si476x_rsq_status_args args = {
+		.primary	= false,
+		.rsqack		= false,
+		.attune		= false,
+		.cancel		= false,
+		.stcack		= false,
+	};
+
+	si476x_core_lock(radio->core);
+	if (radio->ops->rds_blckcnt)
+		err = radio->ops->rsq_status(radio->core, &args, &report);
+	else
+		err = -ENOENT;
+	si476x_core_unlock(radio->core);
+
+	if (err < 0)
+		return err;
+
+	return simple_read_from_buffer(user_buf, count, ppos, &report,
+				       sizeof(report));
+}
+
+static const struct file_operations radio_rsq_fops = {
+	.open	= simple_open,
+	.llseek = default_llseek,
+	.read	= si476x_radio_read_rsq_blob,
+};
+
+static ssize_t si476x_radio_read_rsq_primary_blob(struct file *file,
+						  char __user *user_buf,
+						  size_t count, loff_t *ppos)
+{
+	int err;
+	struct si476x_radio *radio = file->private_data;
+	struct si476x_rsq_status_report report;
+	struct si476x_rsq_status_args args = {
+		.primary	= true,
+		.rsqack		= false,
+		.attune		= false,
+		.cancel		= false,
+		.stcack		= false,
+	};
+
+	si476x_core_lock(radio->core);
+	if (radio->ops->rds_blckcnt)
+		err = radio->ops->rsq_status(radio->core, &args, &report);
+	else
+		err = -ENOENT;
+	si476x_core_unlock(radio->core);
+
+	if (err < 0)
+		return err;
+
+	return simple_read_from_buffer(user_buf, count, ppos, &report,
+				       sizeof(report));
+}
+
+static const struct file_operations radio_rsq_primary_fops = {
+	.open	= simple_open,
+	.llseek = default_llseek,
+	.read	= si476x_radio_read_rsq_primary_blob,
+};
+
+
+static int si476x_radio_init_debugfs(struct si476x_radio *radio)
+{
+	struct dentry	*dentry;
+	int		ret;
+
+	dentry = debugfs_create_dir(dev_name(radio->v4l2dev.dev), NULL);
+	if (IS_ERR(dentry)) {
+		ret = PTR_ERR(dentry);
+		goto exit;
+	}
+	radio->debugfs = dentry;
+
+	dentry = debugfs_create_file("acf", S_IRUGO,
+				     radio->debugfs, radio, &radio_acf_fops);
+	if (IS_ERR(dentry)) {
+		ret = PTR_ERR(dentry);
+		goto cleanup;
+	}
+
+	dentry = debugfs_create_file("rds_blckcnt", S_IRUGO,
+				     radio->debugfs, radio,
+				     &radio_rds_blckcnt_fops);
+	if (IS_ERR(dentry)) {
+		ret = PTR_ERR(dentry);
+		goto cleanup;
+	}
+
+	dentry = debugfs_create_file("agc", S_IRUGO,
+				     radio->debugfs, radio, &radio_agc_fops);
+	if (IS_ERR(dentry)) {
+		ret = PTR_ERR(dentry);
+		goto cleanup;
+	}
+
+	dentry = debugfs_create_file("rsq", S_IRUGO,
+				     radio->debugfs, radio, &radio_rsq_fops);
+	if (IS_ERR(dentry)) {
+		ret = PTR_ERR(dentry);
+		goto cleanup;
+	}
+
+	dentry = debugfs_create_file("rsq_primary", S_IRUGO,
+				     radio->debugfs, radio,
+				     &radio_rsq_primary_fops);
+	if (IS_ERR(dentry)) {
+		ret = PTR_ERR(dentry);
+		goto cleanup;
+	}
+
+	return 0;
+cleanup:
+	debugfs_remove_recursive(radio->debugfs);
+exit:
+	return ret;
+}
+
+
+static int si476x_radio_add_new_custom(struct si476x_radio *radio,
+				       enum si476x_ctrl_idx idx)
+{
+	int rval;
+	struct v4l2_ctrl *ctrl;
+
+	ctrl = v4l2_ctrl_new_custom(&radio->ctrl_handler,
+				    &si476x_ctrls[idx],
+				    NULL);
+	rval = radio->ctrl_handler.error;
+	if (ctrl == NULL && rval)
+		dev_err(radio->v4l2dev.dev,
+			"Could not initialize '%s' control %d\n",
+			si476x_ctrls[idx].name, rval);
+
+	return rval;
+}
+
+static int si476x_radio_probe(struct platform_device *pdev)
+{
+	int rval;
+	struct si476x_radio *radio;
+	struct v4l2_ctrl *ctrl;
+
+	static atomic_t instance = ATOMIC_INIT(0);
+
+	radio = devm_kzalloc(&pdev->dev, sizeof(*radio), GFP_KERNEL);
+	if (!radio)
+		return -ENOMEM;
+
+	radio->core = i2c_mfd_cell_to_core(&pdev->dev);
+
+	v4l2_device_set_name(&radio->v4l2dev, DRIVER_NAME, &instance);
+
+	rval = v4l2_device_register(&pdev->dev, &radio->v4l2dev);
+	if (rval) {
+		dev_err(&pdev->dev, "Cannot register v4l2_device.\n");
+		return rval;
+	}
+
+	memcpy(&radio->videodev, &si476x_viddev_template,
+	       sizeof(struct video_device));
+
+	radio->videodev.v4l2_dev  = &radio->v4l2dev;
+	radio->videodev.ioctl_ops = &si4761_ioctl_ops;
+
+	video_set_drvdata(&radio->videodev, radio);
+	platform_set_drvdata(pdev, radio);
+
+	set_bit(V4L2_FL_USE_FH_PRIO, &radio->videodev.flags);
+
+	radio->v4l2dev.ctrl_handler = &radio->ctrl_handler;
+	v4l2_ctrl_handler_init(&radio->ctrl_handler,
+			       1 + ARRAY_SIZE(si476x_ctrls));
+
+	if (si476x_core_has_am(radio->core)) {
+		ctrl = v4l2_ctrl_new_std_menu(&radio->ctrl_handler,
+					      &si476x_ctrl_ops,
+					      V4L2_CID_POWER_LINE_FREQUENCY,
+					      V4L2_CID_POWER_LINE_FREQUENCY_60HZ,
+					      0, 0);
+		rval = radio->ctrl_handler.error;
+		if (ctrl == NULL && rval) {
+			dev_err(&pdev->dev, "Could not initialize V4L2_CID_POWER_LINE_FREQUENCY control %d\n",
+				rval);
+			goto exit;
+		}
+
+		rval = si476x_radio_add_new_custom(radio,
+						   SI476X_IDX_HARMONICS_COUNT);
+		if (rval < 0)
+			goto exit;
+	}
+
+	rval = si476x_radio_add_new_custom(radio, SI476X_IDX_RSSI_THRESHOLD);
+	if (rval < 0)
+		goto exit;
+
+	rval = si476x_radio_add_new_custom(radio, SI476X_IDX_SNR_THRESHOLD);
+	if (rval < 0)
+		goto exit;
+
+	rval = si476x_radio_add_new_custom(radio, SI476X_IDX_MAX_TUNE_ERROR);
+	if (rval < 0)
+		goto exit;
+
+	ctrl = v4l2_ctrl_new_std_menu(&radio->ctrl_handler,
+				      &si476x_ctrl_ops,
+				      V4L2_CID_TUNE_DEEMPHASIS,
+				      V4L2_DEEMPHASIS_75_uS, 0, 0);
+	rval = radio->ctrl_handler.error;
+	if (ctrl == NULL && rval) {
+		dev_err(&pdev->dev, "Could not initialize V4L2_CID_TUNE_DEEMPHASIS control %d\n",
+			rval);
+		goto exit;
+	}
+
+	ctrl = v4l2_ctrl_new_std(&radio->ctrl_handler, &si476x_ctrl_ops,
+				 V4L2_CID_RDS_RECEPTION,
+				 0, 1, 1, 1);
+	rval = radio->ctrl_handler.error;
+	if (ctrl == NULL && rval) {
+		dev_err(&pdev->dev, "Could not initialize V4L2_CID_RDS_RECEPTION control %d\n",
+			rval);
+		goto exit;
+	}
+
+	if (si476x_core_has_diversity(radio->core)) {
+		si476x_ctrls[SI476X_IDX_DIVERSITY_MODE].def =
+			si476x_phase_diversity_mode_to_idx(radio->core->diversity_mode);
+		si476x_radio_add_new_custom(radio, SI476X_IDX_DIVERSITY_MODE);
+		if (rval < 0)
+			goto exit;
+
+		si476x_radio_add_new_custom(radio, SI476X_IDX_INTERCHIP_LINK);
+		if (rval < 0)
+			goto exit;
+	}
+
+	/* register video device */
+	rval = video_register_device(&radio->videodev, VFL_TYPE_RADIO, -1);
+	if (rval < 0) {
+		dev_err(&pdev->dev, "Could not register video device\n");
+		goto exit;
+	}
+
+	rval = si476x_radio_init_debugfs(radio);
+	if (rval < 0) {
+		dev_err(&pdev->dev, "Could not creat debugfs interface\n");
+		goto exit;
+	}
+
+	return 0;
+exit:
+	v4l2_ctrl_handler_free(radio->videodev.ctrl_handler);
+	return rval;
+}
+
+static int si476x_radio_remove(struct platform_device *pdev)
+{
+	struct si476x_radio *radio = platform_get_drvdata(pdev);
+
+	v4l2_ctrl_handler_free(radio->videodev.ctrl_handler);
+	video_unregister_device(&radio->videodev);
+	v4l2_device_unregister(&radio->v4l2dev);
+	debugfs_remove_recursive(radio->debugfs);
+
+	return 0;
+}
+
+MODULE_ALIAS("platform:si476x-radio");
+
+static struct platform_driver si476x_radio_driver = {
+	.driver		= {
+		.name	= DRIVER_NAME,
+		.owner	= THIS_MODULE,
+	},
+	.probe		= si476x_radio_probe,
+	.remove		= si476x_radio_remove,
+};
+module_platform_driver(si476x_radio_driver);
+
+MODULE_AUTHOR("Andrey Smirnov <andrew.smirnov@gmail.com>");
+MODULE_DESCRIPTION("Driver for Si4761/64/68 AM/FM Radio MFD Cell");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/radio/radio-tea5764.c b/drivers/media/radio/radio-tea5764.c
index 1978516af67..38d563d6259 100644
--- a/drivers/media/radio/radio-tea5764.c
+++ b/drivers/media/radio/radio-tea5764.c
@@ -339,7 +339,7 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *v)
+				const struct v4l2_tuner *v)
 {
 	struct tea5764_device *radio = video_drvdata(file);
 
@@ -351,7 +351,7 @@ static int vidioc_s_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
+				const struct v4l2_frequency *f)
 {
 	struct tea5764_device *radio = video_drvdata(file);
 
diff --git a/drivers/media/radio/radio-tea5777.c b/drivers/media/radio/radio-tea5777.c
index 4b5190d4fea..e2455970725 100644
--- a/drivers/media/radio/radio-tea5777.c
+++ b/drivers/media/radio/radio-tea5777.c
@@ -336,7 +336,7 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-					struct v4l2_tuner *v)
+					const struct v4l2_tuner *v)
 {
 	struct radio_tea5777 *tea = video_drvdata(file);
 	u32 orig_audmode = tea->audmode;
@@ -344,10 +344,9 @@ static int vidioc_s_tuner(struct file *file, void *priv,
 	if (v->index)
 		return -EINVAL;
 
-	if (v->audmode > V4L2_TUNER_MODE_STEREO)
-		v->audmode = V4L2_TUNER_MODE_STEREO;
-
 	tea->audmode = v->audmode;
+	if (tea->audmode > V4L2_TUNER_MODE_STEREO)
+		tea->audmode = V4L2_TUNER_MODE_STEREO;
 
 	if (tea->audmode != orig_audmode && tea->band == BAND_FM)
 		return radio_tea5777_set_freq(tea);
@@ -368,7 +367,7 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-					struct v4l2_frequency *f)
+					const struct v4l2_frequency *f)
 {
 	struct radio_tea5777 *tea = video_drvdata(file);
 
diff --git a/drivers/media/radio/radio-timb.c b/drivers/media/radio/radio-timb.c
index b87effeb5dc..bb7b143b65d 100644
--- a/drivers/media/radio/radio-timb.c
+++ b/drivers/media/radio/radio-timb.c
@@ -56,7 +56,7 @@ static int timbradio_vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int timbradio_vidioc_s_tuner(struct file *file, void *priv,
-	struct v4l2_tuner *v)
+	const struct v4l2_tuner *v)
 {
 	struct timbradio *tr = video_drvdata(file);
 	return v4l2_subdev_call(tr->sd_tuner, tuner, s_tuner, v);
@@ -91,7 +91,7 @@ static int timbradio_vidioc_s_audio(struct file *file, void *priv,
 }
 
 static int timbradio_vidioc_s_frequency(struct file *file, void *priv,
-	struct v4l2_frequency *f)
+	const struct v4l2_frequency *f)
 {
 	struct timbradio *tr = video_drvdata(file);
 	return v4l2_subdev_call(tr->sd_tuner, tuner, s_frequency, f);
diff --git a/drivers/media/radio/radio-wl1273.c b/drivers/media/radio/radio-wl1273.c
index 02151e0e6e6..97c2c18803e 100644
--- a/drivers/media/radio/radio-wl1273.c
+++ b/drivers/media/radio/radio-wl1273.c
@@ -1559,7 +1559,7 @@ out:
 }
 
 static int wl1273_fm_vidioc_s_tuner(struct file *file, void *priv,
-				    struct v4l2_tuner *tuner)
+				    const struct v4l2_tuner *tuner)
 {
 	struct wl1273_device *radio = video_get_drvdata(video_devdata(file));
 	struct wl1273_core *core = radio->core;
@@ -1640,7 +1640,7 @@ static int wl1273_fm_vidioc_g_frequency(struct file *file, void *priv,
 }
 
 static int wl1273_fm_vidioc_s_frequency(struct file *file, void *priv,
-					struct v4l2_frequency *freq)
+					const struct v4l2_frequency *freq)
 {
 	struct wl1273_device *radio = video_get_drvdata(video_devdata(file));
 	struct wl1273_core *core = radio->core;
diff --git a/drivers/media/radio/si470x/radio-si470x-common.c b/drivers/media/radio/si470x/radio-si470x-common.c
index 18989388ddc..5c57e5b0f94 100644
--- a/drivers/media/radio/si470x/radio-si470x-common.c
+++ b/drivers/media/radio/si470x/radio-si470x-common.c
@@ -636,7 +636,7 @@ static int si470x_vidioc_g_tuner(struct file *file, void *priv,
  * si470x_vidioc_s_tuner - set tuner attributes
  */
 static int si470x_vidioc_s_tuner(struct file *file, void *priv,
-		struct v4l2_tuner *tuner)
+		const struct v4l2_tuner *tuner)
 {
 	struct si470x_device *radio = video_drvdata(file);
 
@@ -678,7 +678,7 @@ static int si470x_vidioc_g_frequency(struct file *file, void *priv,
  * si470x_vidioc_s_frequency - set tuner or modulator radio frequency
  */
 static int si470x_vidioc_s_frequency(struct file *file, void *priv,
-		struct v4l2_frequency *freq)
+		const struct v4l2_frequency *freq)
 {
 	struct si470x_device *radio = video_drvdata(file);
 	int retval;
diff --git a/drivers/media/radio/si4713-i2c.c b/drivers/media/radio/si4713-i2c.c
index bd61b3bd0ca..fe160882ee1 100644
--- a/drivers/media/radio/si4713-i2c.c
+++ b/drivers/media/radio/si4713-i2c.c
@@ -21,7 +21,6 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
-#include <linux/mutex.h>
 #include <linux/completion.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
@@ -53,8 +52,6 @@ static const char *si4713_supply_names[SI4713_NUM_SUPPLIES] = {
 
 #define DEFAULT_RDS_PI			0x00
 #define DEFAULT_RDS_PTY			0x00
-#define DEFAULT_RDS_PS_NAME		""
-#define DEFAULT_RDS_RADIO_TEXT		DEFAULT_RDS_PS_NAME
 #define DEFAULT_RDS_DEVIATION		0x00C8
 #define DEFAULT_RDS_PS_REPEAT_COUNT	0x0003
 #define DEFAULT_LIMITER_RTIME		0x1392
@@ -108,7 +105,6 @@ static const char *si4713_supply_names[SI4713_NUM_SUPPLIES] = {
 					(status & SI4713_ERR))
 /* mute definition */
 #define set_mute(p)	((p & 1) | ((p & 1) << 1));
-#define get_mute(p)	(p & 0x01)
 
 #ifdef DEBUG
 #define DBG_BUFFER(device, message, buffer, size)			\
@@ -190,21 +186,6 @@ static int usecs_to_dev(unsigned long usecs, unsigned long const array[],
 	return rval;
 }
 
-static unsigned long dev_to_usecs(int value, unsigned long const array[],
-			int size)
-{
-	int i;
-	int rval = -EINVAL;
-
-	for (i = 0; i < size / 2; i++)
-		if (array[i * 2] == value) {
-			rval = array[(i * 2) + 1];
-			break;
-		}
-
-	return rval;
-}
-
 /* si4713_handler: IRQ handler, just complete work */
 static irqreturn_t si4713_handler(int irq, void *dev)
 {
@@ -458,15 +439,13 @@ static int si4713_checkrev(struct si4713_device *sdev)
 	int rval;
 	u8 resp[SI4713_GETREV_NRESP];
 
-	mutex_lock(&sdev->mutex);
-
 	rval = si4713_send_command(sdev, SI4713_CMD_GET_REV,
 					NULL, 0,
 					resp, ARRAY_SIZE(resp),
 					DEFAULT_TIMEOUT);
 
 	if (rval < 0)
-		goto unlock;
+		return rval;
 
 	if (resp[1] == SI4713_PRODUCT_NUMBER) {
 		v4l2_info(&sdev->sd, "chip found @ 0x%02x (%s)\n",
@@ -475,9 +454,6 @@ static int si4713_checkrev(struct si4713_device *sdev)
 		v4l2_err(&sdev->sd, "Invalid product number\n");
 		rval = -EINVAL;
 	}
-
-unlock:
-	mutex_unlock(&sdev->mutex);
 	return rval;
 }
 
@@ -778,17 +754,9 @@ static int si4713_tx_rds_ps(struct si4713_device *sdev, u8 psid,
 
 static int si4713_set_power_state(struct si4713_device *sdev, u8 value)
 {
-	int rval;
-
-	mutex_lock(&sdev->mutex);
-
 	if (value)
-		rval = si4713_powerup(sdev);
-	else
-		rval = si4713_powerdown(sdev);
-
-	mutex_unlock(&sdev->mutex);
-	return rval;
+		return si4713_powerup(sdev);
+	return si4713_powerdown(sdev);
 }
 
 static int si4713_set_mute(struct si4713_device *sdev, u16 mute)
@@ -797,17 +765,10 @@ static int si4713_set_mute(struct si4713_device *sdev, u16 mute)
 
 	mute = set_mute(mute);
 
-	mutex_lock(&sdev->mutex);
-
 	if (sdev->power_state)
 		rval = si4713_write_property(sdev,
 				SI4713_TX_LINE_INPUT_MUTE, mute);
 
-	if (rval >= 0)
-		sdev->mute = get_mute(mute);
-
-	mutex_unlock(&sdev->mutex);
-
 	return rval;
 }
 
@@ -820,15 +781,13 @@ static int si4713_set_rds_ps_name(struct si4713_device *sdev, char *ps_name)
 	if (!strlen(ps_name))
 		memset(ps_name, 0, MAX_RDS_PS_NAME + 1);
 
-	mutex_lock(&sdev->mutex);
-
 	if (sdev->power_state) {
 		/* Write the new ps name and clear the padding */
 		for (i = 0; i < MAX_RDS_PS_NAME; i += (RDS_BLOCK / 2)) {
 			rval = si4713_tx_rds_ps(sdev, (i / (RDS_BLOCK / 2)),
 						ps_name + i);
 			if (rval < 0)
-				goto unlock;
+				return rval;
 		}
 
 		/* Setup the size to be sent */
@@ -841,19 +800,15 @@ static int si4713_set_rds_ps_name(struct si4713_device *sdev, char *ps_name)
 				SI4713_TX_RDS_PS_MESSAGE_COUNT,
 				rds_ps_nblocks(len));
 		if (rval < 0)
-			goto unlock;
+			return rval;
 
 		rval = si4713_write_property(sdev,
 				SI4713_TX_RDS_PS_REPEAT_COUNT,
 				DEFAULT_RDS_PS_REPEAT_COUNT * 2);
 		if (rval < 0)
-			goto unlock;
+			return rval;
 	}
 
-	strncpy(sdev->rds_info.ps_name, ps_name, MAX_RDS_PS_NAME);
-
-unlock:
-	mutex_unlock(&sdev->mutex);
 	return rval;
 }
 
@@ -864,27 +819,24 @@ static int si4713_set_rds_radio_text(struct si4713_device *sdev, char *rt)
 	u8 b_index = 0, cr_inserted = 0;
 	s8 left;
 
-	mutex_lock(&sdev->mutex);
-
 	if (!sdev->power_state)
-		goto copy;
+		return rval;
 
 	rval = si4713_tx_rds_buff(sdev, RDS_BLOCK_CLEAR, 0, 0, 0, &left);
 	if (rval < 0)
-		goto unlock;
+		return rval;
 
 	if (!strlen(rt))
-		goto copy;
+		return rval;
 
 	do {
 		/* RDS spec says that if the last block isn't used,
 		 * then apply a carriage return
 		 */
-		if (t_index < (RDS_RADIOTEXT_INDEX_MAX *
-			RDS_RADIOTEXT_BLK_SIZE)) {
+		if (t_index < (RDS_RADIOTEXT_INDEX_MAX * RDS_RADIOTEXT_BLK_SIZE)) {
 			for (i = 0; i < RDS_RADIOTEXT_BLK_SIZE; i++) {
-				if (!rt[t_index + i] || rt[t_index + i] ==
-					RDS_CARRIAGE_RETURN) {
+				if (!rt[t_index + i] ||
+				    rt[t_index + i] == RDS_CARRIAGE_RETURN) {
 					rt[t_index + i] = RDS_CARRIAGE_RETURN;
 					cr_inserted = 1;
 					break;
@@ -898,7 +850,7 @@ static int si4713_set_rds_radio_text(struct si4713_device *sdev, char *rt)
 				compose_u16(rt[t_index + 2], rt[t_index + 3]),
 				&left);
 		if (rval < 0)
-			goto unlock;
+			return rval;
 
 		t_index += RDS_RADIOTEXT_BLK_SIZE;
 
@@ -906,16 +858,38 @@ static int si4713_set_rds_radio_text(struct si4713_device *sdev, char *rt)
 			break;
 	} while (left > 0);
 
-copy:
-	strncpy(sdev->rds_info.radio_text, rt, MAX_RDS_RADIO_TEXT);
+	return rval;
+}
 
-unlock:
-	mutex_unlock(&sdev->mutex);
+/*
+ * si4713_update_tune_status - update properties from tx_tune_status
+ * command. Must be called with sdev->mutex held.
+ * @sdev: si4713_device structure for the device we are communicating
+ */
+static int si4713_update_tune_status(struct si4713_device *sdev)
+{
+	int rval;
+	u16 f = 0;
+	u8 p = 0, a = 0, n = 0;
+
+	rval = si4713_tx_tune_status(sdev, 0x00, &f, &p, &a, &n);
+
+	if (rval < 0)
+		goto exit;
+
+/*	TODO: check that power_level and antenna_capacitor really are not
+	changed by the hardware. If they are, then these controls should become
+	volatiles.
+	sdev->power_level = p;
+	sdev->antenna_capacitor = a;*/
+	sdev->tune_rnl = n;
+
+exit:
 	return rval;
 }
 
 static int si4713_choose_econtrol_action(struct si4713_device *sdev, u32 id,
-		u32 **shadow, s32 *bit, s32 *mask, u16 *property, int *mul,
+		s32 *bit, s32 *mask, u16 *property, int *mul,
 		unsigned long **table, int *size)
 {
 	s32 rval = 0;
@@ -925,157 +899,71 @@ static int si4713_choose_econtrol_action(struct si4713_device *sdev, u32 id,
 	case V4L2_CID_RDS_TX_PI:
 		*property = SI4713_TX_RDS_PI;
 		*mul = 1;
-		*shadow = &sdev->rds_info.pi;
 		break;
 	case V4L2_CID_AUDIO_COMPRESSION_THRESHOLD:
 		*property = SI4713_TX_ACOMP_THRESHOLD;
 		*mul = 1;
-		*shadow = &sdev->acomp_info.threshold;
 		break;
 	case V4L2_CID_AUDIO_COMPRESSION_GAIN:
 		*property = SI4713_TX_ACOMP_GAIN;
 		*mul = 1;
-		*shadow = &sdev->acomp_info.gain;
 		break;
 	case V4L2_CID_PILOT_TONE_FREQUENCY:
 		*property = SI4713_TX_PILOT_FREQUENCY;
 		*mul = 1;
-		*shadow = &sdev->pilot_info.frequency;
 		break;
 	case V4L2_CID_AUDIO_COMPRESSION_ATTACK_TIME:
 		*property = SI4713_TX_ACOMP_ATTACK_TIME;
 		*mul = ATTACK_TIME_UNIT;
-		*shadow = &sdev->acomp_info.attack_time;
 		break;
 	case V4L2_CID_PILOT_TONE_DEVIATION:
 		*property = SI4713_TX_PILOT_DEVIATION;
 		*mul = 10;
-		*shadow = &sdev->pilot_info.deviation;
 		break;
 	case V4L2_CID_AUDIO_LIMITER_DEVIATION:
 		*property = SI4713_TX_AUDIO_DEVIATION;
 		*mul = 10;
-		*shadow = &sdev->limiter_info.deviation;
 		break;
 	case V4L2_CID_RDS_TX_DEVIATION:
 		*property = SI4713_TX_RDS_DEVIATION;
 		*mul = 1;
-		*shadow = &sdev->rds_info.deviation;
 		break;
 
 	case V4L2_CID_RDS_TX_PTY:
 		*property = SI4713_TX_RDS_PS_MISC;
 		*bit = 5;
 		*mask = 0x1F << 5;
-		*shadow = &sdev->rds_info.pty;
 		break;
 	case V4L2_CID_AUDIO_LIMITER_ENABLED:
 		*property = SI4713_TX_ACOMP_ENABLE;
 		*bit = 1;
 		*mask = 1 << 1;
-		*shadow = &sdev->limiter_info.enabled;
 		break;
 	case V4L2_CID_AUDIO_COMPRESSION_ENABLED:
 		*property = SI4713_TX_ACOMP_ENABLE;
 		*bit = 0;
 		*mask = 1 << 0;
-		*shadow = &sdev->acomp_info.enabled;
 		break;
 	case V4L2_CID_PILOT_TONE_ENABLED:
 		*property = SI4713_TX_COMPONENT_ENABLE;
 		*bit = 0;
 		*mask = 1 << 0;
-		*shadow = &sdev->pilot_info.enabled;
 		break;
 
 	case V4L2_CID_AUDIO_LIMITER_RELEASE_TIME:
 		*property = SI4713_TX_LIMITER_RELEASE_TIME;
 		*table = limiter_times;
 		*size = ARRAY_SIZE(limiter_times);
-		*shadow = &sdev->limiter_info.release_time;
 		break;
 	case V4L2_CID_AUDIO_COMPRESSION_RELEASE_TIME:
 		*property = SI4713_TX_ACOMP_RELEASE_TIME;
 		*table = acomp_rtimes;
 		*size = ARRAY_SIZE(acomp_rtimes);
-		*shadow = &sdev->acomp_info.release_time;
 		break;
 	case V4L2_CID_TUNE_PREEMPHASIS:
 		*property = SI4713_TX_PREEMPHASIS;
 		*table = preemphasis_values;
 		*size = ARRAY_SIZE(preemphasis_values);
-		*shadow = &sdev->preemphasis;
-		break;
-
-	default:
-		rval = -EINVAL;
-	}
-
-	return rval;
-}
-
-static int si4713_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc);
-
-/* write string property */
-static int si4713_write_econtrol_string(struct si4713_device *sdev,
-				struct v4l2_ext_control *control)
-{
-	struct v4l2_queryctrl vqc;
-	int len;
-	s32 rval = 0;
-
-	vqc.id = control->id;
-	rval = si4713_queryctrl(&sdev->sd, &vqc);
-	if (rval < 0)
-		goto exit;
-
-	switch (control->id) {
-	case V4L2_CID_RDS_TX_PS_NAME: {
-		char ps_name[MAX_RDS_PS_NAME + 1];
-
-		len = control->size - 1;
-		if (len < 0 || len > MAX_RDS_PS_NAME) {
-			rval = -ERANGE;
-			goto exit;
-		}
-		rval = copy_from_user(ps_name, control->string, len);
-		if (rval) {
-			rval = -EFAULT;
-			goto exit;
-		}
-		ps_name[len] = '\0';
-
-		if (strlen(ps_name) % vqc.step) {
-			rval = -ERANGE;
-			goto exit;
-		}
-
-		rval = si4713_set_rds_ps_name(sdev, ps_name);
-	}
-		break;
-
-	case V4L2_CID_RDS_TX_RADIO_TEXT: {
-		char radio_text[MAX_RDS_RADIO_TEXT + 1];
-
-		len = control->size - 1;
-		if (len < 0 || len > MAX_RDS_RADIO_TEXT) {
-			rval = -ERANGE;
-			goto exit;
-		}
-		rval = copy_from_user(radio_text, control->string, len);
-		if (rval) {
-			rval = -EFAULT;
-			goto exit;
-		}
-		radio_text[len] = '\0';
-
-		if (strlen(radio_text) % vqc.step) {
-			rval = -ERANGE;
-			goto exit;
-		}
-
-		rval = si4713_set_rds_radio_text(sdev, radio_text);
-	}
 		break;
 
 	default:
@@ -1083,136 +971,10 @@ static int si4713_write_econtrol_string(struct si4713_device *sdev,
 		break;
 	}
 
-exit:
-	return rval;
-}
-
-static int validate_range(struct v4l2_subdev *sd,
-					struct v4l2_ext_control *control)
-{
-	struct v4l2_queryctrl vqc;
-	int rval;
-
-	vqc.id = control->id;
-	rval = si4713_queryctrl(sd, &vqc);
-	if (rval < 0)
-		goto exit;
-
-	if (control->value < vqc.minimum || control->value > vqc.maximum)
-		rval = -ERANGE;
-
-exit:
-	return rval;
-}
-
-/* properties which use tx_tune_power*/
-static int si4713_write_econtrol_tune(struct si4713_device *sdev,
-				struct v4l2_ext_control *control)
-{
-	s32 rval = 0;
-	u8 power, antcap;
-
-	rval = validate_range(&sdev->sd, control);
-	if (rval < 0)
-		goto exit;
-
-	mutex_lock(&sdev->mutex);
-
-	switch (control->id) {
-	case V4L2_CID_TUNE_POWER_LEVEL:
-		power = control->value;
-		antcap = sdev->antenna_capacitor;
-		break;
-	case V4L2_CID_TUNE_ANTENNA_CAPACITOR:
-		power = sdev->power_level;
-		antcap = control->value;
-		break;
-	default:
-		rval = -EINVAL;
-		goto unlock;
-	}
-
-	if (sdev->power_state)
-		rval = si4713_tx_tune_power(sdev, power, antcap);
-
-	if (rval == 0) {
-		sdev->power_level = power;
-		sdev->antenna_capacitor = antcap;
-	}
-
-unlock:
-	mutex_unlock(&sdev->mutex);
-exit:
-	return rval;
-}
-
-static int si4713_write_econtrol_integers(struct si4713_device *sdev,
-					struct v4l2_ext_control *control)
-{
-	s32 rval;
-	u32 *shadow = NULL, val = 0;
-	s32 bit = 0, mask = 0;
-	u16 property = 0;
-	int mul = 0;
-	unsigned long *table = NULL;
-	int size = 0;
-
-	rval = validate_range(&sdev->sd, control);
-	if (rval < 0)
-		goto exit;
-
-	rval = si4713_choose_econtrol_action(sdev, control->id, &shadow, &bit,
-			&mask, &property, &mul, &table, &size);
-	if (rval < 0)
-		goto exit;
-
-	val = control->value;
-	if (mul) {
-		val = control->value / mul;
-	} else if (table) {
-		rval = usecs_to_dev(control->value, table, size);
-		if (rval < 0)
-			goto exit;
-		val = rval;
-		rval = 0;
-	}
-
-	mutex_lock(&sdev->mutex);
-
-	if (sdev->power_state) {
-		if (mask) {
-			rval = si4713_read_property(sdev, property, &val);
-			if (rval < 0)
-				goto unlock;
-			val = set_bits(val, control->value, bit, mask);
-		}
-
-		rval = si4713_write_property(sdev, property, val);
-		if (rval < 0)
-			goto unlock;
-		if (mask)
-			val = control->value;
-	}
-
-	if (mul) {
-		*shadow = val * mul;
-	} else if (table) {
-		rval = dev_to_usecs(val, table, size);
-		if (rval < 0)
-			goto unlock;
-		*shadow = rval;
-		rval = 0;
-	} else {
-		*shadow = val;
-	}
-
-unlock:
-	mutex_unlock(&sdev->mutex);
-exit:
 	return rval;
 }
 
-static int si4713_s_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *f);
+static int si4713_s_frequency(struct v4l2_subdev *sd, const struct v4l2_frequency *f);
 static int si4713_s_modulator(struct v4l2_subdev *sd, const struct v4l2_modulator *);
 /*
  * si4713_setup - Sets the device up with current configuration.
@@ -1220,111 +982,25 @@ static int si4713_s_modulator(struct v4l2_subdev *sd, const struct v4l2_modulato
  */
 static int si4713_setup(struct si4713_device *sdev)
 {
-	struct v4l2_ext_control ctrl;
 	struct v4l2_frequency f;
 	struct v4l2_modulator vm;
-	struct si4713_device *tmp;
-	int rval = 0;
-
-	tmp = kmalloc(sizeof(*tmp), GFP_KERNEL);
-	if (!tmp)
-		return -ENOMEM;
-
-	/* Get a local copy to avoid race */
-	mutex_lock(&sdev->mutex);
-	memcpy(tmp, sdev, sizeof(*sdev));
-	mutex_unlock(&sdev->mutex);
-
-	ctrl.id = V4L2_CID_RDS_TX_PI;
-	ctrl.value = tmp->rds_info.pi;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_AUDIO_COMPRESSION_THRESHOLD;
-	ctrl.value = tmp->acomp_info.threshold;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_AUDIO_COMPRESSION_GAIN;
-	ctrl.value = tmp->acomp_info.gain;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_PILOT_TONE_FREQUENCY;
-	ctrl.value = tmp->pilot_info.frequency;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_AUDIO_COMPRESSION_ATTACK_TIME;
-	ctrl.value = tmp->acomp_info.attack_time;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_PILOT_TONE_DEVIATION;
-	ctrl.value = tmp->pilot_info.deviation;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_AUDIO_LIMITER_DEVIATION;
-	ctrl.value = tmp->limiter_info.deviation;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_RDS_TX_DEVIATION;
-	ctrl.value = tmp->rds_info.deviation;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_RDS_TX_PTY;
-	ctrl.value = tmp->rds_info.pty;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_AUDIO_LIMITER_ENABLED;
-	ctrl.value = tmp->limiter_info.enabled;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_AUDIO_COMPRESSION_ENABLED;
-	ctrl.value = tmp->acomp_info.enabled;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_PILOT_TONE_ENABLED;
-	ctrl.value = tmp->pilot_info.enabled;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_AUDIO_LIMITER_RELEASE_TIME;
-	ctrl.value = tmp->limiter_info.release_time;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_AUDIO_COMPRESSION_RELEASE_TIME;
-	ctrl.value = tmp->acomp_info.release_time;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_TUNE_PREEMPHASIS;
-	ctrl.value = tmp->preemphasis;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_RDS_TX_PS_NAME;
-	rval |= si4713_set_rds_ps_name(sdev, tmp->rds_info.ps_name);
-
-	ctrl.id = V4L2_CID_RDS_TX_RADIO_TEXT;
-	rval |= si4713_set_rds_radio_text(sdev, tmp->rds_info.radio_text);
+	int rval;
 
 	/* Device procedure needs to set frequency first */
-	f.frequency = tmp->frequency ? tmp->frequency : DEFAULT_FREQUENCY;
+	f.tuner = 0;
+	f.frequency = sdev->frequency ? sdev->frequency : DEFAULT_FREQUENCY;
 	f.frequency = si4713_to_v4l2(f.frequency);
-	rval |= si4713_s_frequency(&sdev->sd, &f);
-
-	ctrl.id = V4L2_CID_TUNE_POWER_LEVEL;
-	ctrl.value = tmp->power_level;
-	rval |= si4713_write_econtrol_tune(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_TUNE_ANTENNA_CAPACITOR;
-	ctrl.value = tmp->antenna_capacitor;
-	rval |= si4713_write_econtrol_tune(sdev, &ctrl);
+	rval = si4713_s_frequency(&sdev->sd, &f);
 
 	vm.index = 0;
-	if (tmp->stereo)
+	if (sdev->stereo)
 		vm.txsubchans = V4L2_TUNER_SUB_STEREO;
 	else
 		vm.txsubchans = V4L2_TUNER_SUB_MONO;
-	if (tmp->rds_info.enabled)
+	if (sdev->rds_enabled)
 		vm.txsubchans |= V4L2_TUNER_SUB_RDS;
 	si4713_s_modulator(&sdev->sd, &vm);
 
-	kfree(tmp);
-
 	return rval;
 }
 
@@ -1338,434 +1014,126 @@ static int si4713_initialize(struct si4713_device *sdev)
 
 	rval = si4713_set_power_state(sdev, POWER_ON);
 	if (rval < 0)
-		goto exit;
+		return rval;
 
 	rval = si4713_checkrev(sdev);
 	if (rval < 0)
-		goto exit;
+		return rval;
 
 	rval = si4713_set_power_state(sdev, POWER_OFF);
 	if (rval < 0)
-		goto exit;
-
-	mutex_lock(&sdev->mutex);
-
-	sdev->rds_info.pi = DEFAULT_RDS_PI;
-	sdev->rds_info.pty = DEFAULT_RDS_PTY;
-	sdev->rds_info.deviation = DEFAULT_RDS_DEVIATION;
-	strlcpy(sdev->rds_info.ps_name, DEFAULT_RDS_PS_NAME, MAX_RDS_PS_NAME);
-	strlcpy(sdev->rds_info.radio_text, DEFAULT_RDS_RADIO_TEXT,
-							MAX_RDS_RADIO_TEXT);
-	sdev->rds_info.enabled = 1;
-
-	sdev->limiter_info.release_time = DEFAULT_LIMITER_RTIME;
-	sdev->limiter_info.deviation = DEFAULT_LIMITER_DEV;
-	sdev->limiter_info.enabled = 1;
-
-	sdev->pilot_info.deviation = DEFAULT_PILOT_DEVIATION;
-	sdev->pilot_info.frequency = DEFAULT_PILOT_FREQUENCY;
-	sdev->pilot_info.enabled = 1;
+		return rval;
 
-	sdev->acomp_info.release_time = DEFAULT_ACOMP_RTIME;
-	sdev->acomp_info.attack_time = DEFAULT_ACOMP_ATIME;
-	sdev->acomp_info.threshold = DEFAULT_ACOMP_THRESHOLD;
-	sdev->acomp_info.gain = DEFAULT_ACOMP_GAIN;
-	sdev->acomp_info.enabled = 1;
 
 	sdev->frequency = DEFAULT_FREQUENCY;
-	sdev->preemphasis = DEFAULT_PREEMPHASIS;
-	sdev->mute = DEFAULT_MUTE;
-	sdev->power_level = DEFAULT_POWER_LEVEL;
-	sdev->antenna_capacitor = 0;
 	sdev->stereo = 1;
 	sdev->tune_rnl = DEFAULT_TUNE_RNL;
-
-	mutex_unlock(&sdev->mutex);
-
-exit:
-	return rval;
-}
-
-/* read string property */
-static int si4713_read_econtrol_string(struct si4713_device *sdev,
-				struct v4l2_ext_control *control)
-{
-	s32 rval = 0;
-
-	switch (control->id) {
-	case V4L2_CID_RDS_TX_PS_NAME:
-		if (strlen(sdev->rds_info.ps_name) + 1 > control->size) {
-			control->size = MAX_RDS_PS_NAME + 1;
-			rval = -ENOSPC;
-			goto exit;
-		}
-		rval = copy_to_user(control->string, sdev->rds_info.ps_name,
-					strlen(sdev->rds_info.ps_name) + 1);
-		if (rval)
-			rval = -EFAULT;
-		break;
-
-	case V4L2_CID_RDS_TX_RADIO_TEXT:
-		if (strlen(sdev->rds_info.radio_text) + 1 > control->size) {
-			control->size = MAX_RDS_RADIO_TEXT + 1;
-			rval = -ENOSPC;
-			goto exit;
-		}
-		rval = copy_to_user(control->string, sdev->rds_info.radio_text,
-					strlen(sdev->rds_info.radio_text) + 1);
-		if (rval)
-			rval = -EFAULT;
-		break;
-
-	default:
-		rval = -EINVAL;
-		break;
-	}
-
-exit:
-	return rval;
-}
-
-/*
- * si4713_update_tune_status - update properties from tx_tune_status
- * command. Must be called with sdev->mutex held.
- * @sdev: si4713_device structure for the device we are communicating
- */
-static int si4713_update_tune_status(struct si4713_device *sdev)
-{
-	int rval;
-	u16 f = 0;
-	u8 p = 0, a = 0, n = 0;
-
-	rval = si4713_tx_tune_status(sdev, 0x00, &f, &p, &a, &n);
-
-	if (rval < 0)
-		goto exit;
-
-	sdev->power_level = p;
-	sdev->antenna_capacitor = a;
-	sdev->tune_rnl = n;
-
-exit:
-	return rval;
-}
-
-/* properties which use tx_tune_status */
-static int si4713_read_econtrol_tune(struct si4713_device *sdev,
-				struct v4l2_ext_control *control)
-{
-	s32 rval = 0;
-
-	mutex_lock(&sdev->mutex);
-
-	if (sdev->power_state) {
-		rval = si4713_update_tune_status(sdev);
-		if (rval < 0)
-			goto unlock;
-	}
-
-	switch (control->id) {
-	case V4L2_CID_TUNE_POWER_LEVEL:
-		control->value = sdev->power_level;
-		break;
-	case V4L2_CID_TUNE_ANTENNA_CAPACITOR:
-		control->value = sdev->antenna_capacitor;
-		break;
-	default:
-		rval = -EINVAL;
-	}
-
-unlock:
-	mutex_unlock(&sdev->mutex);
-	return rval;
+	return 0;
 }
 
-static int si4713_read_econtrol_integers(struct si4713_device *sdev,
-				struct v4l2_ext_control *control)
+/* si4713_s_ctrl - set the value of a control */
+static int si4713_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	s32 rval;
-	u32 *shadow = NULL, val = 0;
+	struct si4713_device *sdev =
+		container_of(ctrl->handler, struct si4713_device, ctrl_handler);
+	u32 val = 0;
 	s32 bit = 0, mask = 0;
 	u16 property = 0;
 	int mul = 0;
 	unsigned long *table = NULL;
 	int size = 0;
+	bool force = false;
+	int c;
+	int ret = 0;
 
-	rval = si4713_choose_econtrol_action(sdev, control->id, &shadow, &bit,
-			&mask, &property, &mul, &table, &size);
-	if (rval < 0)
-		goto exit;
-
-	mutex_lock(&sdev->mutex);
-
-	if (sdev->power_state) {
-		rval = si4713_read_property(sdev, property, &val);
-		if (rval < 0)
-			goto unlock;
-
-		/* Keep negative values for threshold */
-		if (control->id == V4L2_CID_AUDIO_COMPRESSION_THRESHOLD)
-			*shadow = (s16)val;
-		else if (mask)
-			*shadow = get_status_bit(val, bit, mask);
-		else if (mul)
-			*shadow = val * mul;
-		else
-			*shadow = dev_to_usecs(val, table, size);
-	}
-
-	control->value = *shadow;
-
-unlock:
-	mutex_unlock(&sdev->mutex);
-exit:
-	return rval;
-}
-
-/*
- * Video4Linux Subdev Interface
- */
-/* si4713_s_ext_ctrls - set extended controls value */
-static int si4713_s_ext_ctrls(struct v4l2_subdev *sd,
-				struct v4l2_ext_controls *ctrls)
-{
-	struct si4713_device *sdev = to_si4713_device(sd);
-	int i;
-
-	if (ctrls->ctrl_class != V4L2_CTRL_CLASS_FM_TX)
+	if (ctrl->id != V4L2_CID_AUDIO_MUTE)
 		return -EINVAL;
-
-	for (i = 0; i < ctrls->count; i++) {
-		int err;
-
-		switch ((ctrls->controls + i)->id) {
-		case V4L2_CID_RDS_TX_PS_NAME:
-		case V4L2_CID_RDS_TX_RADIO_TEXT:
-			err = si4713_write_econtrol_string(sdev,
-							ctrls->controls + i);
-			break;
-		case V4L2_CID_TUNE_ANTENNA_CAPACITOR:
-		case V4L2_CID_TUNE_POWER_LEVEL:
-			err = si4713_write_econtrol_tune(sdev,
-							ctrls->controls + i);
-			break;
-		default:
-			err = si4713_write_econtrol_integers(sdev,
-							ctrls->controls + i);
-		}
-
-		if (err < 0) {
-			ctrls->error_idx = i;
-			return err;
+	if (ctrl->is_new) {
+		if (ctrl->val) {
+			ret = si4713_set_mute(sdev, ctrl->val);
+			if (!ret)
+				ret = si4713_set_power_state(sdev, POWER_DOWN);
+			return ret;
 		}
+		ret = si4713_set_power_state(sdev, POWER_UP);
+		if (!ret)
+			ret = si4713_set_mute(sdev, ctrl->val);
+		if (!ret)
+			ret = si4713_setup(sdev);
+		if (ret)
+			return ret;
+		force = true;
 	}
 
-	return 0;
-}
+	if (!sdev->power_state)
+		return 0;
 
-/* si4713_g_ext_ctrls - get extended controls value */
-static int si4713_g_ext_ctrls(struct v4l2_subdev *sd,
-				struct v4l2_ext_controls *ctrls)
-{
-	struct si4713_device *sdev = to_si4713_device(sd);
-	int i;
+	for (c = 1; !ret && c < ctrl->ncontrols; c++) {
+		ctrl = ctrl->cluster[c];
 
-	if (ctrls->ctrl_class != V4L2_CTRL_CLASS_FM_TX)
-		return -EINVAL;
+		if (!force && !ctrl->is_new)
+			continue;
 
-	for (i = 0; i < ctrls->count; i++) {
-		int err;
-
-		switch ((ctrls->controls + i)->id) {
+		switch (ctrl->id) {
 		case V4L2_CID_RDS_TX_PS_NAME:
+			ret = si4713_set_rds_ps_name(sdev, ctrl->string);
+			break;
+
 		case V4L2_CID_RDS_TX_RADIO_TEXT:
-			err = si4713_read_econtrol_string(sdev,
-							ctrls->controls + i);
+			ret = si4713_set_rds_radio_text(sdev, ctrl->string);
 			break;
+
 		case V4L2_CID_TUNE_ANTENNA_CAPACITOR:
+			/* don't handle this control if we force setting all
+			 * controls since in that case it will be handled by
+			 * V4L2_CID_TUNE_POWER_LEVEL. */
+			if (force)
+				break;
+			/* fall through */
 		case V4L2_CID_TUNE_POWER_LEVEL:
-			err = si4713_read_econtrol_tune(sdev,
-							ctrls->controls + i);
+			ret = si4713_tx_tune_power(sdev,
+				sdev->tune_pwr_level->val, sdev->tune_ant_cap->val);
+			if (!ret) {
+				/* Make sure we don't set this twice */
+				sdev->tune_ant_cap->is_new = false;
+				sdev->tune_pwr_level->is_new = false;
+			}
 			break;
-		default:
-			err = si4713_read_econtrol_integers(sdev,
-							ctrls->controls + i);
-		}
-
-		if (err < 0) {
-			ctrls->error_idx = i;
-			return err;
-		}
-	}
-
-	return 0;
-}
-
-/* si4713_queryctrl - enumerate control items */
-static int si4713_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc)
-{
-	int rval = 0;
-
-	switch (qc->id) {
-	/* User class controls */
-	case V4L2_CID_AUDIO_MUTE:
-		rval = v4l2_ctrl_query_fill(qc, 0, 1, 1, DEFAULT_MUTE);
-		break;
-	/* FM_TX class controls */
-	case V4L2_CID_RDS_TX_PI:
-		rval = v4l2_ctrl_query_fill(qc, 0, 0xFFFF, 1, DEFAULT_RDS_PI);
-		break;
-	case V4L2_CID_RDS_TX_PTY:
-		rval = v4l2_ctrl_query_fill(qc, 0, 31, 1, DEFAULT_RDS_PTY);
-		break;
-	case V4L2_CID_RDS_TX_DEVIATION:
-		rval = v4l2_ctrl_query_fill(qc, 0, MAX_RDS_DEVIATION,
-						10, DEFAULT_RDS_DEVIATION);
-		break;
-	case V4L2_CID_RDS_TX_PS_NAME:
-		/*
-		 * Report step as 8. From RDS spec, psname
-		 * should be 8. But there are receivers which scroll strings
-		 * sized as 8xN.
-		 */
-		rval = v4l2_ctrl_query_fill(qc, 0, MAX_RDS_PS_NAME, 8, 0);
-		break;
-	case V4L2_CID_RDS_TX_RADIO_TEXT:
-		/*
-		 * Report step as 32 (2A block). From RDS spec,
-		 * radio text should be 32 for 2A block. But there are receivers
-		 * which scroll strings sized as 32xN. Setting default to 32.
-		 */
-		rval = v4l2_ctrl_query_fill(qc, 0, MAX_RDS_RADIO_TEXT, 32, 0);
-		break;
-
-	case V4L2_CID_AUDIO_LIMITER_ENABLED:
-		rval = v4l2_ctrl_query_fill(qc, 0, 1, 1, 1);
-		break;
-	case V4L2_CID_AUDIO_LIMITER_RELEASE_TIME:
-		rval = v4l2_ctrl_query_fill(qc, 250, MAX_LIMITER_RELEASE_TIME,
-						50, DEFAULT_LIMITER_RTIME);
-		break;
-	case V4L2_CID_AUDIO_LIMITER_DEVIATION:
-		rval = v4l2_ctrl_query_fill(qc, 0, MAX_LIMITER_DEVIATION,
-						10, DEFAULT_LIMITER_DEV);
-		break;
-
-	case V4L2_CID_AUDIO_COMPRESSION_ENABLED:
-		rval = v4l2_ctrl_query_fill(qc, 0, 1, 1, 1);
-		break;
-	case V4L2_CID_AUDIO_COMPRESSION_GAIN:
-		rval = v4l2_ctrl_query_fill(qc, 0, MAX_ACOMP_GAIN, 1,
-						DEFAULT_ACOMP_GAIN);
-		break;
-	case V4L2_CID_AUDIO_COMPRESSION_THRESHOLD:
-		rval = v4l2_ctrl_query_fill(qc, MIN_ACOMP_THRESHOLD,
-						MAX_ACOMP_THRESHOLD, 1,
-						DEFAULT_ACOMP_THRESHOLD);
-		break;
-	case V4L2_CID_AUDIO_COMPRESSION_ATTACK_TIME:
-		rval = v4l2_ctrl_query_fill(qc, 0, MAX_ACOMP_ATTACK_TIME,
-						500, DEFAULT_ACOMP_ATIME);
-		break;
-	case V4L2_CID_AUDIO_COMPRESSION_RELEASE_TIME:
-		rval = v4l2_ctrl_query_fill(qc, 100000, MAX_ACOMP_RELEASE_TIME,
-						100000, DEFAULT_ACOMP_RTIME);
-		break;
-
-	case V4L2_CID_PILOT_TONE_ENABLED:
-		rval = v4l2_ctrl_query_fill(qc, 0, 1, 1, 1);
-		break;
-	case V4L2_CID_PILOT_TONE_DEVIATION:
-		rval = v4l2_ctrl_query_fill(qc, 0, MAX_PILOT_DEVIATION,
-						10, DEFAULT_PILOT_DEVIATION);
-		break;
-	case V4L2_CID_PILOT_TONE_FREQUENCY:
-		rval = v4l2_ctrl_query_fill(qc, 0, MAX_PILOT_FREQUENCY,
-						1, DEFAULT_PILOT_FREQUENCY);
-		break;
-
-	case V4L2_CID_TUNE_PREEMPHASIS:
-		rval = v4l2_ctrl_query_fill(qc, V4L2_PREEMPHASIS_DISABLED,
-						V4L2_PREEMPHASIS_75_uS, 1,
-						V4L2_PREEMPHASIS_50_uS);
-		break;
-	case V4L2_CID_TUNE_POWER_LEVEL:
-		rval = v4l2_ctrl_query_fill(qc, 0, 120, 1, DEFAULT_POWER_LEVEL);
-		break;
-	case V4L2_CID_TUNE_ANTENNA_CAPACITOR:
-		rval = v4l2_ctrl_query_fill(qc, 0, 191, 1, 0);
-		break;
-	default:
-		rval = -EINVAL;
-		break;
-	}
-
-	return rval;
-}
-
-/* si4713_g_ctrl - get the value of a control */
-static int si4713_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-	struct si4713_device *sdev = to_si4713_device(sd);
-	int rval = 0;
-
-	if (!sdev)
-		return -ENODEV;
-
-	mutex_lock(&sdev->mutex);
-
-	if (sdev->power_state) {
-		rval = si4713_read_property(sdev, SI4713_TX_LINE_INPUT_MUTE,
-						&sdev->mute);
-
-		if (rval < 0)
-			goto unlock;
-	}
-
-	switch (ctrl->id) {
-	case V4L2_CID_AUDIO_MUTE:
-		ctrl->value = get_mute(sdev->mute);
-		break;
-	}
-
-unlock:
-	mutex_unlock(&sdev->mutex);
-	return rval;
-}
-
-/* si4713_s_ctrl - set the value of a control */
-static int si4713_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-	struct si4713_device *sdev = to_si4713_device(sd);
-	int rval = 0;
-
-	if (!sdev)
-		return -ENODEV;
-
-	switch (ctrl->id) {
-	case V4L2_CID_AUDIO_MUTE:
-		if (ctrl->value) {
-			rval = si4713_set_mute(sdev, ctrl->value);
-			if (rval < 0)
-				goto exit;
 
-			rval = si4713_set_power_state(sdev, POWER_DOWN);
-		} else {
-			rval = si4713_set_power_state(sdev, POWER_UP);
-			if (rval < 0)
-				goto exit;
+		default:
+			ret = si4713_choose_econtrol_action(sdev, ctrl->id, &bit,
+					&mask, &property, &mul, &table, &size);
+			if (ret < 0)
+				break;
+
+			val = ctrl->val;
+			if (mul) {
+				val = val / mul;
+			} else if (table) {
+				ret = usecs_to_dev(val, table, size);
+				if (ret < 0)
+					break;
+				val = ret;
+				ret = 0;
+			}
 
-			rval = si4713_setup(sdev);
-			if (rval < 0)
-				goto exit;
+			if (mask) {
+				ret = si4713_read_property(sdev, property, &val);
+				if (ret < 0)
+					break;
+				val = set_bits(val, ctrl->val, bit, mask);
+			}
 
-			rval = si4713_set_mute(sdev, ctrl->value);
+			ret = si4713_write_property(sdev, property, val);
+			if (ret < 0)
+				break;
+			if (mask)
+				val = ctrl->val;
+			break;
 		}
-		break;
 	}
 
-exit:
-	return rval;
+	return ret;
 }
 
 /* si4713_ioctl - deal with private ioctls (only rnl for now) */
@@ -1779,7 +1147,6 @@ static long si4713_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
 	if (!arg)
 		return -EINVAL;
 
-	mutex_lock(&sdev->mutex);
 	switch (cmd) {
 	case SI4713_IOC_MEASURE_RNL:
 		frequency = v4l2_to_si4713(rnl->frequency);
@@ -1788,11 +1155,11 @@ static long si4713_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
 			/* Set desired measurement frequency */
 			rval = si4713_tx_tune_measure(sdev, frequency, 0);
 			if (rval < 0)
-				goto unlock;
+				return rval;
 			/* get results from tune status */
 			rval = si4713_update_tune_status(sdev);
 			if (rval < 0)
-				goto unlock;
+				return rval;
 		}
 		rnl->rnl = sdev->tune_rnl;
 		break;
@@ -1802,35 +1169,20 @@ static long si4713_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
 		rval = -ENOIOCTLCMD;
 	}
 
-unlock:
-	mutex_unlock(&sdev->mutex);
 	return rval;
 }
 
-static const struct v4l2_subdev_core_ops si4713_subdev_core_ops = {
-	.queryctrl	= si4713_queryctrl,
-	.g_ext_ctrls	= si4713_g_ext_ctrls,
-	.s_ext_ctrls	= si4713_s_ext_ctrls,
-	.g_ctrl		= si4713_g_ctrl,
-	.s_ctrl		= si4713_s_ctrl,
-	.ioctl		= si4713_ioctl,
-};
-
 /* si4713_g_modulator - get modulator attributes */
 static int si4713_g_modulator(struct v4l2_subdev *sd, struct v4l2_modulator *vm)
 {
 	struct si4713_device *sdev = to_si4713_device(sd);
 	int rval = 0;
 
-	if (!sdev) {
-		rval = -ENODEV;
-		goto exit;
-	}
+	if (!sdev)
+		return -ENODEV;
 
-	if (vm->index > 0) {
-		rval = -EINVAL;
-		goto exit;
-	}
+	if (vm->index > 0)
+		return -EINVAL;
 
 	strncpy(vm->name, "FM Modulator", 32);
 	vm->capability = V4L2_TUNER_CAP_STEREO | V4L2_TUNER_CAP_LOW |
@@ -1840,18 +1192,15 @@ static int si4713_g_modulator(struct v4l2_subdev *sd, struct v4l2_modulator *vm)
 	vm->rangelow = si4713_to_v4l2(FREQ_RANGE_LOW);
 	vm->rangehigh = si4713_to_v4l2(FREQ_RANGE_HIGH);
 
-	mutex_lock(&sdev->mutex);
-
 	if (sdev->power_state) {
 		u32 comp_en = 0;
 
 		rval = si4713_read_property(sdev, SI4713_TX_COMPONENT_ENABLE,
 						&comp_en);
 		if (rval < 0)
-			goto unlock;
+			return rval;
 
 		sdev->stereo = get_status_bit(comp_en, 1, 1 << 1);
-		sdev->rds_info.enabled = get_status_bit(comp_en, 2, 1 << 2);
 	}
 
 	/* Report current audio mode: mono or stereo */
@@ -1861,14 +1210,11 @@ static int si4713_g_modulator(struct v4l2_subdev *sd, struct v4l2_modulator *vm)
 		vm->txsubchans = V4L2_TUNER_SUB_MONO;
 
 	/* Report rds feature status */
-	if (sdev->rds_info.enabled)
+	if (sdev->rds_enabled)
 		vm->txsubchans |= V4L2_TUNER_SUB_RDS;
 	else
 		vm->txsubchans &= ~V4L2_TUNER_SUB_RDS;
 
-unlock:
-	mutex_unlock(&sdev->mutex);
-exit:
 	return rval;
 }
 
@@ -1896,13 +1242,11 @@ static int si4713_s_modulator(struct v4l2_subdev *sd, const struct v4l2_modulato
 
 	rds = !!(vm->txsubchans & V4L2_TUNER_SUB_RDS);
 
-	mutex_lock(&sdev->mutex);
-
 	if (sdev->power_state) {
 		rval = si4713_read_property(sdev,
 						SI4713_TX_COMPONENT_ENABLE, &p);
 		if (rval < 0)
-			goto unlock;
+			return rval;
 
 		p = set_bits(p, stereo, 1, 1 << 1);
 		p = set_bits(p, rds, 2, 1 << 2);
@@ -1910,14 +1254,12 @@ static int si4713_s_modulator(struct v4l2_subdev *sd, const struct v4l2_modulato
 		rval = si4713_write_property(sdev,
 						SI4713_TX_COMPONENT_ENABLE, p);
 		if (rval < 0)
-			goto unlock;
+			return rval;
 	}
 
 	sdev->stereo = stereo;
-	sdev->rds_info.enabled = rds;
+	sdev->rds_enabled = rds;
 
-unlock:
-	mutex_unlock(&sdev->mutex);
 	return rval;
 }
 
@@ -1927,9 +1269,8 @@ static int si4713_g_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *f)
 	struct si4713_device *sdev = to_si4713_device(sd);
 	int rval = 0;
 
-	f->type = V4L2_TUNER_RADIO;
-
-	mutex_lock(&sdev->mutex);
+	if (f->tuner)
+		return -EINVAL;
 
 	if (sdev->power_state) {
 		u16 freq;
@@ -1937,46 +1278,49 @@ static int si4713_g_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *f)
 
 		rval = si4713_tx_tune_status(sdev, 0x00, &freq, &p, &a, &n);
 		if (rval < 0)
-			goto unlock;
+			return rval;
 
 		sdev->frequency = freq;
 	}
 
 	f->frequency = si4713_to_v4l2(sdev->frequency);
 
-unlock:
-	mutex_unlock(&sdev->mutex);
 	return rval;
 }
 
 /* si4713_s_frequency - set tuner or modulator radio frequency */
-static int si4713_s_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *f)
+static int si4713_s_frequency(struct v4l2_subdev *sd, const struct v4l2_frequency *f)
 {
 	struct si4713_device *sdev = to_si4713_device(sd);
 	int rval = 0;
 	u16 frequency = v4l2_to_si4713(f->frequency);
 
-	/* Check frequency range */
-	if (frequency < FREQ_RANGE_LOW || frequency > FREQ_RANGE_HIGH)
-		return -EDOM;
+	if (f->tuner)
+		return -EINVAL;
 
-	mutex_lock(&sdev->mutex);
+	/* Check frequency range */
+	frequency = clamp_t(u16, frequency, FREQ_RANGE_LOW, FREQ_RANGE_HIGH);
 
 	if (sdev->power_state) {
 		rval = si4713_tx_tune_freq(sdev, frequency);
 		if (rval < 0)
-			goto unlock;
+			return rval;
 		frequency = rval;
 		rval = 0;
 	}
 	sdev->frequency = frequency;
-	f->frequency = si4713_to_v4l2(frequency);
 
-unlock:
-	mutex_unlock(&sdev->mutex);
 	return rval;
 }
 
+static const struct v4l2_ctrl_ops si4713_ctrl_ops = {
+	.s_ctrl = si4713_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops si4713_subdev_core_ops = {
+	.ioctl		= si4713_ioctl,
+};
+
 static const struct v4l2_subdev_tuner_ops si4713_subdev_tuner_ops = {
 	.g_frequency	= si4713_g_frequency,
 	.s_frequency	= si4713_s_frequency,
@@ -1998,6 +1342,7 @@ static int si4713_probe(struct i2c_client *client,
 {
 	struct si4713_device *sdev;
 	struct si4713_platform_data *pdata = client->dev.platform_data;
+	struct v4l2_ctrl_handler *hdl;
 	int rval, i;
 
 	sdev = kzalloc(sizeof *sdev, GFP_KERNEL);
@@ -2031,9 +1376,84 @@ static int si4713_probe(struct i2c_client *client,
 
 	v4l2_i2c_subdev_init(&sdev->sd, client, &si4713_subdev_ops);
 
-	mutex_init(&sdev->mutex);
 	init_completion(&sdev->work);
 
+	hdl = &sdev->ctrl_handler;
+	v4l2_ctrl_handler_init(hdl, 20);
+	sdev->mute = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_AUDIO_MUTE, 0, 1, 1, DEFAULT_MUTE);
+
+	sdev->rds_pi = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_RDS_TX_PI, 0, 0xffff, 1, DEFAULT_RDS_PI);
+	sdev->rds_pty = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_RDS_TX_PTY, 0, 31, 1, DEFAULT_RDS_PTY);
+	sdev->rds_deviation = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_RDS_TX_DEVIATION, 0, MAX_RDS_DEVIATION,
+			10, DEFAULT_RDS_DEVIATION);
+	/*
+	 * Report step as 8. From RDS spec, psname
+	 * should be 8. But there are receivers which scroll strings
+	 * sized as 8xN.
+	 */
+	sdev->rds_ps_name = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_RDS_TX_PS_NAME, 0, MAX_RDS_PS_NAME, 8, 0);
+	/*
+	 * Report step as 32 (2A block). From RDS spec,
+	 * radio text should be 32 for 2A block. But there are receivers
+	 * which scroll strings sized as 32xN. Setting default to 32.
+	 */
+	sdev->rds_radio_text = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_RDS_TX_RADIO_TEXT, 0, MAX_RDS_RADIO_TEXT, 32, 0);
+
+	sdev->limiter_enabled = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_AUDIO_LIMITER_ENABLED, 0, 1, 1, 1);
+	sdev->limiter_release_time = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_AUDIO_LIMITER_RELEASE_TIME, 250,
+			MAX_LIMITER_RELEASE_TIME, 10, DEFAULT_LIMITER_RTIME);
+	sdev->limiter_deviation = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_AUDIO_LIMITER_DEVIATION, 0,
+			MAX_LIMITER_DEVIATION, 10, DEFAULT_LIMITER_DEV);
+
+	sdev->compression_enabled = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_AUDIO_COMPRESSION_ENABLED, 0, 1, 1, 1);
+	sdev->compression_gain = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_AUDIO_COMPRESSION_GAIN, 0, MAX_ACOMP_GAIN, 1,
+			DEFAULT_ACOMP_GAIN);
+	sdev->compression_threshold = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_AUDIO_COMPRESSION_THRESHOLD, MIN_ACOMP_THRESHOLD,
+			MAX_ACOMP_THRESHOLD, 1,
+			DEFAULT_ACOMP_THRESHOLD);
+	sdev->compression_attack_time = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_AUDIO_COMPRESSION_ATTACK_TIME, 0,
+			MAX_ACOMP_ATTACK_TIME, 500, DEFAULT_ACOMP_ATIME);
+	sdev->compression_release_time = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_AUDIO_COMPRESSION_RELEASE_TIME, 100000,
+			MAX_ACOMP_RELEASE_TIME, 100000, DEFAULT_ACOMP_RTIME);
+
+	sdev->pilot_tone_enabled = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_PILOT_TONE_ENABLED, 0, 1, 1, 1);
+	sdev->pilot_tone_deviation = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_PILOT_TONE_DEVIATION, 0, MAX_PILOT_DEVIATION,
+			10, DEFAULT_PILOT_DEVIATION);
+	sdev->pilot_tone_freq = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_PILOT_TONE_FREQUENCY, 0, MAX_PILOT_FREQUENCY,
+			1, DEFAULT_PILOT_FREQUENCY);
+
+	sdev->tune_preemphasis = v4l2_ctrl_new_std_menu(hdl, &si4713_ctrl_ops,
+			V4L2_CID_TUNE_PREEMPHASIS,
+			V4L2_PREEMPHASIS_75_uS, 0, V4L2_PREEMPHASIS_50_uS);
+	sdev->tune_pwr_level = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_TUNE_POWER_LEVEL, 0, 120, 1, DEFAULT_POWER_LEVEL);
+	sdev->tune_ant_cap = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_TUNE_ANTENNA_CAPACITOR, 0, 191, 1, 0);
+
+	if (hdl->error) {
+		rval = hdl->error;
+		goto free_ctrls;
+	}
+	v4l2_ctrl_cluster(20, &sdev->mute);
+	sdev->sd.ctrl_handler = hdl;
+
 	if (client->irq) {
 		rval = request_irq(client->irq,
 			si4713_handler, IRQF_TRIGGER_FALLING | IRQF_DISABLED,
@@ -2058,6 +1478,8 @@ static int si4713_probe(struct i2c_client *client,
 free_irq:
 	if (client->irq)
 		free_irq(client->irq, sdev);
+free_ctrls:
+	v4l2_ctrl_handler_free(hdl);
 put_reg:
 	regulator_bulk_free(ARRAY_SIZE(sdev->supplies), sdev->supplies);
 free_gpio:
@@ -2082,6 +1504,7 @@ static int si4713_remove(struct i2c_client *client)
 		free_irq(client->irq, sdev);
 
 	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
 	regulator_bulk_free(ARRAY_SIZE(sdev->supplies), sdev->supplies);
 	if (gpio_is_valid(sdev->gpio_reset))
 		gpio_free(sdev->gpio_reset);
diff --git a/drivers/media/radio/si4713-i2c.h b/drivers/media/radio/si4713-i2c.h
index c6dfa7fb101..25cdea26343 100644
--- a/drivers/media/radio/si4713-i2c.h
+++ b/drivers/media/radio/si4713-i2c.h
@@ -16,6 +16,7 @@
 #define SI4713_I2C_H
 
 #include <media/v4l2-subdev.h>
+#include <media/v4l2-ctrls.h>
 #include <media/si4713.h>
 
 #define SI4713_PRODUCT_NUMBER		0x0D
@@ -160,56 +161,33 @@
 #define POWER_UP			0x01
 #define POWER_DOWN			0x00
 
-struct rds_info {
-	u32 pi;
 #define MAX_RDS_PTY			31
-	u32 pty;
 #define MAX_RDS_DEVIATION		90000
-	u32 deviation;
+
 /*
  * PSNAME is known to be defined as 8 character sized (RDS Spec).
  * However, there is receivers which scroll PSNAME 8xN sized.
  */
 #define MAX_RDS_PS_NAME			96
-	u8 ps_name[MAX_RDS_PS_NAME + 1];
+
 /*
  * MAX_RDS_RADIO_TEXT is known to be defined as 32 (2A group) or 64 (2B group)
  * character sized (RDS Spec).
  * However, there is receivers which scroll them as well.
  */
 #define MAX_RDS_RADIO_TEXT		384
-	u8 radio_text[MAX_RDS_RADIO_TEXT + 1];
-	u32 enabled;
-};
 
-struct limiter_info {
 #define MAX_LIMITER_RELEASE_TIME	102390
-	u32 release_time;
 #define MAX_LIMITER_DEVIATION		90000
-	u32 deviation;
-	u32 enabled;
-};
 
-struct pilot_info {
 #define MAX_PILOT_DEVIATION		90000
-	u32 deviation;
 #define MAX_PILOT_FREQUENCY		19000
-	u32 frequency;
-	u32 enabled;
-};
 
-struct acomp_info {
 #define MAX_ACOMP_RELEASE_TIME		1000000
-	u32 release_time;
 #define MAX_ACOMP_ATTACK_TIME		5000
-	u32 attack_time;
 #define MAX_ACOMP_THRESHOLD		0
 #define MIN_ACOMP_THRESHOLD		(-40)
-	s32 threshold;
 #define MAX_ACOMP_GAIN			20
-	u32 gain;
-	u32 enabled;
-};
 
 #define SI4713_NUM_SUPPLIES		2
 
@@ -219,21 +197,41 @@ struct acomp_info {
 struct si4713_device {
 	/* v4l2_subdev and i2c reference (v4l2_subdev priv data) */
 	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler ctrl_handler;
 	/* private data structures */
-	struct mutex mutex;
+	struct { /* si4713 control cluster */
+		/* This is one big cluster since the mute control
+		 * powers off the device and after unmuting again all
+		 * controls need to be set at once. The only way of doing
+		 * that is by making it one big cluster. */
+		struct v4l2_ctrl *mute;
+		struct v4l2_ctrl *rds_ps_name;
+		struct v4l2_ctrl *rds_radio_text;
+		struct v4l2_ctrl *rds_pi;
+		struct v4l2_ctrl *rds_deviation;
+		struct v4l2_ctrl *rds_pty;
+		struct v4l2_ctrl *compression_enabled;
+		struct v4l2_ctrl *compression_threshold;
+		struct v4l2_ctrl *compression_gain;
+		struct v4l2_ctrl *compression_attack_time;
+		struct v4l2_ctrl *compression_release_time;
+		struct v4l2_ctrl *pilot_tone_enabled;
+		struct v4l2_ctrl *pilot_tone_freq;
+		struct v4l2_ctrl *pilot_tone_deviation;
+		struct v4l2_ctrl *limiter_enabled;
+		struct v4l2_ctrl *limiter_deviation;
+		struct v4l2_ctrl *limiter_release_time;
+		struct v4l2_ctrl *tune_preemphasis;
+		struct v4l2_ctrl *tune_pwr_level;
+		struct v4l2_ctrl *tune_ant_cap;
+	};
 	struct completion work;
-	struct rds_info rds_info;
-	struct limiter_info limiter_info;
-	struct pilot_info pilot_info;
-	struct acomp_info acomp_info;
 	struct regulator_bulk_data supplies[SI4713_NUM_SUPPLIES];
 	int gpio_reset;
+	u32 power_state;
+	u32 rds_enabled;
 	u32 frequency;
 	u32 preemphasis;
-	u32 mute;
-	u32 power_level;
-	u32 power_state;
-	u32 antenna_capacitor;
 	u32 stereo;
 	u32 tune_rnl;
 };
diff --git a/drivers/media/radio/tef6862.c b/drivers/media/radio/tef6862.c
index b18c2dc268b..82c6c9475d7 100644
--- a/drivers/media/radio/tef6862.c
+++ b/drivers/media/radio/tef6862.c
@@ -96,12 +96,12 @@ static int tef6862_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *v)
 	return 0;
 }
 
-static int tef6862_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *v)
+static int tef6862_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *v)
 {
 	return v->index ? -EINVAL : 0;
 }
 
-static int tef6862_s_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *f)
+static int tef6862_s_frequency(struct v4l2_subdev *sd, const struct v4l2_frequency *f)
 {
 	struct tef6862_state *state = to_state(sd);
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
diff --git a/drivers/media/radio/wl128x/fmdrv_v4l2.c b/drivers/media/radio/wl128x/fmdrv_v4l2.c
index 0a8ee8fab92..5dec323f424 100644
--- a/drivers/media/radio/wl128x/fmdrv_v4l2.c
+++ b/drivers/media/radio/wl128x/fmdrv_v4l2.c
@@ -331,7 +331,7 @@ static int fm_v4l2_vidioc_g_tuner(struct file *file, void *priv,
  * Should we set other tuner attributes, too?
  */
 static int fm_v4l2_vidioc_s_tuner(struct file *file, void *priv,
-		struct v4l2_tuner *tuner)
+		const struct v4l2_tuner *tuner)
 {
 	struct fmdev *fmdev = video_drvdata(file);
 	u16 aud_mode;
@@ -388,7 +388,7 @@ static int fm_v4l2_vidioc_g_freq(struct file *file, void *priv,
 
 /* Set tuner or modulator radio frequency */
 static int fm_v4l2_vidioc_s_freq(struct file *file, void *priv,
-		struct v4l2_frequency *freq)
+		const struct v4l2_frequency *freq)
 {
 	struct fmdev *fmdev = video_drvdata(file);
 
@@ -396,9 +396,7 @@ static int fm_v4l2_vidioc_s_freq(struct file *file, void *priv,
 	 * As V4L2_TUNER_CAP_LOW is set 1 user sends the frequency
 	 * in units of 62.5 Hz.
 	 */
-	freq->frequency = (u32)(freq->frequency / 16);
-
-	return fmc_set_freq(fmdev, freq->frequency);
+	return fmc_set_freq(fmdev, freq->frequency / 16);
 }
 
 /* Set hardware frequency seek. If current mode is NOT RX, set it RX. */
diff --git a/drivers/media/rc/Kconfig b/drivers/media/rc/Kconfig
index 19f3563c61d..5a79c333d45 100644
--- a/drivers/media/rc/Kconfig
+++ b/drivers/media/rc/Kconfig
@@ -291,7 +291,7 @@ config IR_TTUSBIR
 
 config IR_RX51
 	tristate "Nokia N900 IR transmitter diode"
-	depends on OMAP_DM_TIMER && LIRC && !ARCH_MULTIPLATFORM
+	depends on OMAP_DM_TIMER && ARCH_OMAP2PLUS && LIRC && !ARCH_MULTIPLATFORM
 	---help---
 	   Say Y or M here if you want to enable support for the IR
 	   transmitter diode built in the Nokia N900 (RX51) device.
diff --git a/drivers/media/rc/ene_ir.c b/drivers/media/rc/ene_ir.c
index ee6c984cade..ed184f68c17 100644
--- a/drivers/media/rc/ene_ir.c
+++ b/drivers/media/rc/ene_ir.c
@@ -1098,6 +1098,7 @@ exit_release_hw_io:
 	release_region(dev->hw_io, ENE_IO_SIZE);
 exit_unregister_device:
 	rc_unregister_device(rdev);
+	rdev = NULL;
 exit_free_dev_rdev:
 	rc_free_device(rdev);
 	kfree(dev);
diff --git a/drivers/media/rc/imon.c b/drivers/media/rc/imon.c
index dec203bb06f..72e3fa65248 100644
--- a/drivers/media/rc/imon.c
+++ b/drivers/media/rc/imon.c
@@ -112,6 +112,7 @@ struct imon_context {
 	bool tx_control;
 	unsigned char usb_rx_buf[8];
 	unsigned char usb_tx_buf[8];
+	unsigned int send_packet_delay;
 
 	struct tx_t {
 		unsigned char data_buf[35];	/* user data buffer */
@@ -185,6 +186,10 @@ enum {
 	IMON_KEY_PANEL	= 2,
 };
 
+enum {
+	IMON_NEED_20MS_PKT_DELAY = 1
+};
+
 /*
  * USB Device ID for iMON USB Control Boards
  *
@@ -215,7 +220,7 @@ static struct usb_device_id imon_usb_id_table[] = {
 	/* SoundGraph iMON OEM Touch LCD (IR & 4.3" VGA LCD) */
 	{ USB_DEVICE(0x15c2, 0x0035) },
 	/* SoundGraph iMON OEM VFD (IR & VFD) */
-	{ USB_DEVICE(0x15c2, 0x0036) },
+	{ USB_DEVICE(0x15c2, 0x0036), .driver_info = IMON_NEED_20MS_PKT_DELAY },
 	/* device specifics unknown */
 	{ USB_DEVICE(0x15c2, 0x0037) },
 	/* SoundGraph iMON OEM LCD (IR & LCD) */
@@ -523,8 +528,10 @@ static int send_packet(struct imon_context *ictx)
 		mutex_unlock(&ictx->lock);
 		retval = wait_for_completion_interruptible(
 				&ictx->tx.finished);
-		if (retval)
+		if (retval) {
+			usb_kill_urb(ictx->tx_urb);
 			pr_err_ratelimited("task interrupted\n");
+		}
 		mutex_lock(&ictx->lock);
 
 		retval = ictx->tx.status;
@@ -535,12 +542,12 @@ static int send_packet(struct imon_context *ictx)
 	kfree(control_req);
 
 	/*
-	 * Induce a mandatory 5ms delay before returning, as otherwise,
+	 * Induce a mandatory delay before returning, as otherwise,
 	 * send_packet can get called so rapidly as to overwhelm the device,
 	 * particularly on faster systems and/or those with quirky usb.
 	 */
-	timeout = msecs_to_jiffies(5);
-	set_current_state(TASK_UNINTERRUPTIBLE);
+	timeout = msecs_to_jiffies(ictx->send_packet_delay);
+	set_current_state(TASK_INTERRUPTIBLE);
 	schedule_timeout(timeout);
 
 	return retval;
@@ -1568,11 +1575,6 @@ static void imon_incoming_packet(struct imon_context *ictx,
 	if (press_type < 0)
 		goto not_input_data;
 
-	spin_lock_irqsave(&ictx->kc_lock, flags);
-	if (ictx->kc == KEY_UNKNOWN)
-		goto unknown_key;
-	spin_unlock_irqrestore(&ictx->kc_lock, flags);
-
 	if (ktype != IMON_KEY_PANEL) {
 		if (press_type == 0)
 			rc_keyup(ictx->rdev);
@@ -1615,12 +1617,6 @@ static void imon_incoming_packet(struct imon_context *ictx,
 
 	return;
 
-unknown_key:
-	spin_unlock_irqrestore(&ictx->kc_lock, flags);
-	dev_info(dev, "%s: unknown keypress, code 0x%llx\n", __func__,
-		 (long long)scancode);
-	return;
-
 not_input_data:
 	if (len != 8) {
 		dev_warn(dev, "imon %s: invalid incoming packet "
@@ -2099,7 +2095,8 @@ static bool imon_find_endpoints(struct imon_context *ictx,
 
 }
 
-static struct imon_context *imon_init_intf0(struct usb_interface *intf)
+static struct imon_context *imon_init_intf0(struct usb_interface *intf,
+					    const struct usb_device_id *id)
 {
 	struct imon_context *ictx;
 	struct urb *rx_urb;
@@ -2139,6 +2136,10 @@ static struct imon_context *imon_init_intf0(struct usb_interface *intf)
 	ictx->vendor  = le16_to_cpu(ictx->usbdev_intf0->descriptor.idVendor);
 	ictx->product = le16_to_cpu(ictx->usbdev_intf0->descriptor.idProduct);
 
+	/* default send_packet delay is 5ms but some devices need more */
+	ictx->send_packet_delay = id->driver_info & IMON_NEED_20MS_PKT_DELAY ?
+				  20 : 5;
+
 	ret = -ENODEV;
 	iface_desc = intf->cur_altsetting;
 	if (!imon_find_endpoints(ictx, iface_desc)) {
@@ -2317,7 +2318,7 @@ static int imon_probe(struct usb_interface *interface,
 	first_if_ctx = usb_get_intfdata(first_if);
 
 	if (ifnum == 0) {
-		ictx = imon_init_intf0(interface);
+		ictx = imon_init_intf0(interface, id);
 		if (!ictx) {
 			pr_err("failed to initialize context!\n");
 			ret = -ENODEV;
@@ -2325,7 +2326,14 @@ static int imon_probe(struct usb_interface *interface,
 		}
 
 	} else {
-	/* this is the secondary interface on the device */
+		/* this is the secondary interface on the device */
+
+		/* fail early if first intf failed to register */
+		if (!first_if_ctx) {
+			ret = -ENODEV;
+			goto fail;
+		}
+
 		ictx = imon_init_intf1(interface, first_if_ctx);
 		if (!ictx) {
 			pr_err("failed to attach to context!\n");
diff --git a/drivers/media/rc/ir-jvc-decoder.c b/drivers/media/rc/ir-jvc-decoder.c
index 69edffb9fe9..3948138ca87 100644
--- a/drivers/media/rc/ir-jvc-decoder.c
+++ b/drivers/media/rc/ir-jvc-decoder.c
@@ -47,7 +47,7 @@ static int ir_jvc_decode(struct rc_dev *dev, struct ir_raw_event ev)
 {
 	struct jvc_dec *data = &dev->raw->jvc;
 
-	if (!(dev->raw->enabled_protocols & RC_BIT_JVC))
+	if (!(dev->enabled_protocols & RC_BIT_JVC))
 		return 0;
 
 	if (!is_timing_event(ev)) {
diff --git a/drivers/media/rc/ir-lirc-codec.c b/drivers/media/rc/ir-lirc-codec.c
index 9945e5e7f61..ff4d93d1907 100644
--- a/drivers/media/rc/ir-lirc-codec.c
+++ b/drivers/media/rc/ir-lirc-codec.c
@@ -35,7 +35,7 @@ static int ir_lirc_decode(struct rc_dev *dev, struct ir_raw_event ev)
 	struct lirc_codec *lirc = &dev->raw->lirc;
 	int sample;
 
-	if (!(dev->raw->enabled_protocols & RC_BIT_LIRC))
+	if (!(dev->enabled_protocols & RC_BIT_LIRC))
 		return 0;
 
 	if (!dev->raw->lirc.drv || !dev->raw->lirc.drv->rbuf)
diff --git a/drivers/media/rc/ir-mce_kbd-decoder.c b/drivers/media/rc/ir-mce_kbd-decoder.c
index 33fafa4cf7c..9f3c9b59f30 100644
--- a/drivers/media/rc/ir-mce_kbd-decoder.c
+++ b/drivers/media/rc/ir-mce_kbd-decoder.c
@@ -216,7 +216,7 @@ static int ir_mce_kbd_decode(struct rc_dev *dev, struct ir_raw_event ev)
 	u32 scancode;
 	unsigned long delay;
 
-	if (!(dev->raw->enabled_protocols & RC_BIT_MCE_KBD))
+	if (!(dev->enabled_protocols & RC_BIT_MCE_KBD))
 		return 0;
 
 	if (!is_timing_event(ev)) {
diff --git a/drivers/media/rc/ir-nec-decoder.c b/drivers/media/rc/ir-nec-decoder.c
index a47ee363496..9a900941143 100644
--- a/drivers/media/rc/ir-nec-decoder.c
+++ b/drivers/media/rc/ir-nec-decoder.c
@@ -52,7 +52,7 @@ static int ir_nec_decode(struct rc_dev *dev, struct ir_raw_event ev)
 	u8 address, not_address, command, not_command;
 	bool send_32bits = false;
 
-	if (!(dev->raw->enabled_protocols & RC_BIT_NEC))
+	if (!(dev->enabled_protocols & RC_BIT_NEC))
 		return 0;
 
 	if (!is_timing_event(ev)) {
diff --git a/drivers/media/rc/ir-raw.c b/drivers/media/rc/ir-raw.c
index 17c94be9f24..5c42750c7b7 100644
--- a/drivers/media/rc/ir-raw.c
+++ b/drivers/media/rc/ir-raw.c
@@ -256,7 +256,7 @@ int ir_raw_event_register(struct rc_dev *dev)
 		return -ENOMEM;
 
 	dev->raw->dev = dev;
-	dev->raw->enabled_protocols = ~0;
+	dev->enabled_protocols = ~0;
 	rc = kfifo_alloc(&dev->raw->kfifo,
 			 sizeof(struct ir_raw_event) * MAX_IR_EVENT_SIZE,
 			 GFP_KERNEL);
diff --git a/drivers/media/rc/ir-rc5-decoder.c b/drivers/media/rc/ir-rc5-decoder.c
index 5b4d1ddeac4..4e53a319c5d 100644
--- a/drivers/media/rc/ir-rc5-decoder.c
+++ b/drivers/media/rc/ir-rc5-decoder.c
@@ -52,7 +52,7 @@ static int ir_rc5_decode(struct rc_dev *dev, struct ir_raw_event ev)
 	u8 toggle;
 	u32 scancode;
 
-	if (!(dev->raw->enabled_protocols & (RC_BIT_RC5 | RC_BIT_RC5X)))
+	if (!(dev->enabled_protocols & (RC_BIT_RC5 | RC_BIT_RC5X)))
 		return 0;
 
 	if (!is_timing_event(ev)) {
@@ -128,7 +128,7 @@ again:
 		if (data->wanted_bits == RC5X_NBITS) {
 			/* RC5X */
 			u8 xdata, command, system;
-			if (!(dev->raw->enabled_protocols & RC_BIT_RC5X)) {
+			if (!(dev->enabled_protocols & RC_BIT_RC5X)) {
 				data->state = STATE_INACTIVE;
 				return 0;
 			}
@@ -145,7 +145,7 @@ again:
 		} else {
 			/* RC5 */
 			u8 command, system;
-			if (!(dev->raw->enabled_protocols & RC_BIT_RC5)) {
+			if (!(dev->enabled_protocols & RC_BIT_RC5)) {
 				data->state = STATE_INACTIVE;
 				return 0;
 			}
diff --git a/drivers/media/rc/ir-rc5-sz-decoder.c b/drivers/media/rc/ir-rc5-sz-decoder.c
index fd807a8308d..865fe84fd85 100644
--- a/drivers/media/rc/ir-rc5-sz-decoder.c
+++ b/drivers/media/rc/ir-rc5-sz-decoder.c
@@ -48,7 +48,7 @@ static int ir_rc5_sz_decode(struct rc_dev *dev, struct ir_raw_event ev)
 	u8 toggle, command, system;
 	u32 scancode;
 
-	if (!(dev->raw->enabled_protocols & RC_BIT_RC5_SZ))
+	if (!(dev->enabled_protocols & RC_BIT_RC5_SZ))
 		return 0;
 
 	if (!is_timing_event(ev)) {
diff --git a/drivers/media/rc/ir-rc6-decoder.c b/drivers/media/rc/ir-rc6-decoder.c
index e19072ffb36..7cba7d33a3f 100644
--- a/drivers/media/rc/ir-rc6-decoder.c
+++ b/drivers/media/rc/ir-rc6-decoder.c
@@ -89,7 +89,7 @@ static int ir_rc6_decode(struct rc_dev *dev, struct ir_raw_event ev)
 	u32 scancode;
 	u8 toggle;
 
-	if (!(dev->raw->enabled_protocols &
+	if (!(dev->enabled_protocols &
 	      (RC_BIT_RC6_0 | RC_BIT_RC6_6A_20 | RC_BIT_RC6_6A_24 |
 	       RC_BIT_RC6_6A_32 | RC_BIT_RC6_MCE)))
 		return 0;
diff --git a/drivers/media/rc/ir-rx51.c b/drivers/media/rc/ir-rx51.c
index 8ead492d03a..31b955bf766 100644
--- a/drivers/media/rc/ir-rx51.c
+++ b/drivers/media/rc/ir-rx51.c
@@ -464,14 +464,14 @@ static int lirc_rx51_probe(struct platform_device *dev)
 	return 0;
 }
 
-static int __exit lirc_rx51_remove(struct platform_device *dev)
+static int lirc_rx51_remove(struct platform_device *dev)
 {
 	return lirc_unregister_driver(lirc_rx51_driver.minor);
 }
 
 struct platform_driver lirc_rx51_platform_driver = {
 	.probe		= lirc_rx51_probe,
-	.remove		= __exit_p(lirc_rx51_remove),
+	.remove		= lirc_rx51_remove,
 	.suspend	= lirc_rx51_suspend,
 	.resume		= lirc_rx51_resume,
 	.driver		= {
diff --git a/drivers/media/rc/ir-sanyo-decoder.c b/drivers/media/rc/ir-sanyo-decoder.c
index 7e69a3b6537..0a06205b567 100644
--- a/drivers/media/rc/ir-sanyo-decoder.c
+++ b/drivers/media/rc/ir-sanyo-decoder.c
@@ -58,7 +58,7 @@ static int ir_sanyo_decode(struct rc_dev *dev, struct ir_raw_event ev)
 	u32 scancode;
 	u8 address, command, not_command;
 
-	if (!(dev->raw->enabled_protocols & RC_BIT_SANYO))
+	if (!(dev->enabled_protocols & RC_BIT_SANYO))
 		return 0;
 
 	if (!is_timing_event(ev)) {
diff --git a/drivers/media/rc/ir-sony-decoder.c b/drivers/media/rc/ir-sony-decoder.c
index fb914342cf4..29ab9c2db06 100644
--- a/drivers/media/rc/ir-sony-decoder.c
+++ b/drivers/media/rc/ir-sony-decoder.c
@@ -45,7 +45,7 @@ static int ir_sony_decode(struct rc_dev *dev, struct ir_raw_event ev)
 	u32 scancode;
 	u8 device, subdevice, function;
 
-	if (!(dev->raw->enabled_protocols &
+	if (!(dev->enabled_protocols &
 	      (RC_BIT_SONY12 | RC_BIT_SONY15 | RC_BIT_SONY20)))
 		return 0;
 
@@ -124,7 +124,7 @@ static int ir_sony_decode(struct rc_dev *dev, struct ir_raw_event ev)
 
 		switch (data->count) {
 		case 12:
-			if (!(dev->raw->enabled_protocols & RC_BIT_SONY12)) {
+			if (!(dev->enabled_protocols & RC_BIT_SONY12)) {
 				data->state = STATE_INACTIVE;
 				return 0;
 			}
@@ -133,7 +133,7 @@ static int ir_sony_decode(struct rc_dev *dev, struct ir_raw_event ev)
 			function  = bitrev8((data->bits >>  4) & 0xFE);
 			break;
 		case 15:
-			if (!(dev->raw->enabled_protocols & RC_BIT_SONY15)) {
+			if (!(dev->enabled_protocols & RC_BIT_SONY15)) {
 				data->state = STATE_INACTIVE;
 				return 0;
 			}
@@ -142,7 +142,7 @@ static int ir_sony_decode(struct rc_dev *dev, struct ir_raw_event ev)
 			function  = bitrev8((data->bits >>  7) & 0xFE);
 			break;
 		case 20:
-			if (!(dev->raw->enabled_protocols & RC_BIT_SONY20)) {
+			if (!(dev->enabled_protocols & RC_BIT_SONY20)) {
 				data->state = STATE_INACTIVE;
 				return 0;
 			}
diff --git a/drivers/media/rc/ite-cir.c b/drivers/media/rc/ite-cir.c
index dd8237324c0..63b42252166 100644
--- a/drivers/media/rc/ite-cir.c
+++ b/drivers/media/rc/ite-cir.c
@@ -1613,6 +1613,7 @@ exit_release_cir_addr:
 	release_region(itdev->cir_addr, itdev->params.io_region_size);
 exit_unregister_device:
 	rc_unregister_device(rdev);
+	rdev = NULL;
 exit_free_dev_rdev:
 	rc_free_device(rdev);
 	kfree(itdev);
diff --git a/drivers/media/rc/keymaps/Makefile b/drivers/media/rc/keymaps/Makefile
index 778661971ae..5ab94ea4bc2 100644
--- a/drivers/media/rc/keymaps/Makefile
+++ b/drivers/media/rc/keymaps/Makefile
@@ -78,6 +78,7 @@ obj-$(CONFIG_RC_MAP) += rc-adstech-dvb-t-pci.o \
 			rc-hauppauge.o \
 			rc-rc6-mce.o \
 			rc-real-audio-220-32-keys.o \
+			rc-reddo.o \
 			rc-snapstream-firefly.o \
 			rc-streamzap.o \
 			rc-tbs-nec.o \
@@ -88,7 +89,7 @@ obj-$(CONFIG_RC_MAP) += rc-adstech-dvb-t-pci.o \
 			rc-tevii-nec.o \
 			rc-tivo.o \
 			rc-total-media-in-hand.o \
-                       rc-total-media-in-hand-02.o \
+			rc-total-media-in-hand-02.o \
 			rc-trekstor.o \
 			rc-tt-1500.o \
 			rc-twinhan1027.o \
diff --git a/drivers/media/rc/keymaps/rc-reddo.c b/drivers/media/rc/keymaps/rc-reddo.c
new file mode 100644
index 00000000000..b80b336e928
--- /dev/null
+++ b/drivers/media/rc/keymaps/rc-reddo.c
@@ -0,0 +1,86 @@
+/*
+ * MSI DIGIVOX mini III remote controller keytable
+ *
+ * Copyright (C) 2013 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 <media/rc-map.h>
+#include <linux/module.h>
+
+/*
+ * Derived from MSI DIGIVOX mini III remote (rc-msi-digivox-iii.c)
+ *
+ * Differences between these remotes are:
+ *
+ * 1) scancode 0x61d601 is mapped to different button:
+ *    MSI DIGIVOX mini III   "Source" = KEY_VIDEO
+ *    Reddo                     "EPG" = KEY_EPG
+ *
+ * 2) Reddo remote has less buttons. Missing buttons are: colored buttons,
+ *    navigation buttons and main power button.
+ */
+
+static struct rc_map_table reddo[] = {
+	{ 0x61d601, KEY_EPG },             /* EPG */
+	{ 0x61d602, KEY_3 },
+	{ 0x61d604, KEY_1 },
+	{ 0x61d605, KEY_5 },
+	{ 0x61d606, KEY_6 },
+	{ 0x61d607, KEY_CHANNELDOWN },     /* CH- */
+	{ 0x61d608, KEY_2 },
+	{ 0x61d609, KEY_CHANNELUP },       /* CH+ */
+	{ 0x61d60a, KEY_9 },
+	{ 0x61d60b, KEY_ZOOM },            /* Zoom */
+	{ 0x61d60c, KEY_7 },
+	{ 0x61d60d, KEY_8 },
+	{ 0x61d60e, KEY_VOLUMEUP },        /* Vol+ */
+	{ 0x61d60f, KEY_4 },
+	{ 0x61d610, KEY_ESC },             /* [back up arrow] */
+	{ 0x61d611, KEY_0 },
+	{ 0x61d612, KEY_OK },              /* [enter arrow] */
+	{ 0x61d613, KEY_VOLUMEDOWN },      /* Vol- */
+	{ 0x61d614, KEY_RECORD },          /* Rec */
+	{ 0x61d615, KEY_STOP },            /* Stop */
+	{ 0x61d616, KEY_PLAY },            /* Play */
+	{ 0x61d617, KEY_MUTE },            /* Mute */
+	{ 0x61d643, KEY_POWER2 },          /* [red power button] */
+};
+
+static struct rc_map_list reddo_map = {
+	.map = {
+		.scan    = reddo,
+		.size    = ARRAY_SIZE(reddo),
+		.rc_type = RC_TYPE_NEC,
+		.name    = RC_MAP_REDDO,
+	}
+};
+
+static int __init init_rc_map_reddo(void)
+{
+	return rc_map_register(&reddo_map);
+}
+
+static void __exit exit_rc_map_reddo(void)
+{
+	rc_map_unregister(&reddo_map);
+}
+
+module_init(init_rc_map_reddo)
+module_exit(exit_rc_map_reddo)
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
diff --git a/drivers/media/rc/mceusb.c b/drivers/media/rc/mceusb.c
index 5b5b6e6f79e..3c761014d3c 100644
--- a/drivers/media/rc/mceusb.c
+++ b/drivers/media/rc/mceusb.c
@@ -482,7 +482,7 @@ static char SET_RX_SENSOR[]	= {MCE_CMD_PORT_IR,
 				   MCE_RSP_EQIRRXPORTEN, 0x00};
 */
 
-static int mceusb_cmdsize(u8 cmd, u8 subcmd)
+static int mceusb_cmd_datasize(u8 cmd, u8 subcmd)
 {
 	int datasize = 0;
 
@@ -493,6 +493,9 @@ static int mceusb_cmdsize(u8 cmd, u8 subcmd)
 		break;
 	case MCE_CMD_PORT_SYS:
 		switch (subcmd) {
+		case MCE_RSP_GETPORTSTATUS:
+			datasize = 5;
+			break;
 		case MCE_RSP_EQWAKEVERSION:
 			datasize = 4;
 			break;
@@ -500,6 +503,9 @@ static int mceusb_cmdsize(u8 cmd, u8 subcmd)
 			datasize = 2;
 			break;
 		case MCE_RSP_EQWAKESUPPORT:
+		case MCE_RSP_GETWAKESOURCE:
+		case MCE_RSP_EQDEVDETAILS:
+		case MCE_RSP_EQEMVER:
 			datasize = 1;
 			break;
 		}
@@ -509,6 +515,7 @@ static int mceusb_cmdsize(u8 cmd, u8 subcmd)
 		case MCE_RSP_EQIRCFS:
 		case MCE_RSP_EQIRTIMEOUT:
 		case MCE_RSP_EQIRRXCFCNT:
+		case MCE_RSP_EQIRNUMPORTS:
 			datasize = 2;
 			break;
 		case MCE_CMD_SIG_END:
@@ -968,7 +975,7 @@ static void mceusb_process_ir_data(struct mceusb_dev *ir, int buf_len)
 	for (; i < buf_len; i++) {
 		switch (ir->parser_state) {
 		case SUBCMD:
-			ir->rem = mceusb_cmdsize(ir->cmd, ir->buf_in[i]);
+			ir->rem = mceusb_cmd_datasize(ir->cmd, ir->buf_in[i]);
 			mceusb_dev_printdata(ir, ir->buf_in, i - 1,
 					     ir->rem + 2, false);
 			mceusb_handle_command(ir, i);
diff --git a/drivers/media/rc/nuvoton-cir.c b/drivers/media/rc/nuvoton-cir.c
index 40125d77904..21ee0dc1b7e 100644
--- a/drivers/media/rc/nuvoton-cir.c
+++ b/drivers/media/rc/nuvoton-cir.c
@@ -1107,6 +1107,7 @@ exit_release_cir_addr:
 	release_region(nvt->cir_addr, CIR_IOREG_LENGTH);
 exit_unregister_device:
 	rc_unregister_device(rdev);
+	rdev = NULL;
 exit_free_dev_rdev:
 	rc_free_device(rdev);
 	kfree(nvt);
diff --git a/drivers/media/rc/rc-core-priv.h b/drivers/media/rc/rc-core-priv.h
index 5d87287ed37..70a180bb0bd 100644
--- a/drivers/media/rc/rc-core-priv.h
+++ b/drivers/media/rc/rc-core-priv.h
@@ -39,7 +39,6 @@ struct ir_raw_event_ctrl {
 	ktime_t				last_event;	/* when last event occurred */
 	enum raw_event_type		last_type;	/* last event type */
 	struct rc_dev			*dev;		/* pointer to the parent rc_dev */
-	u64				enabled_protocols; /* enabled raw protocol decoders */
 
 	/* raw decoder state follows */
 	struct ir_raw_event prev_ev;
diff --git a/drivers/media/rc/rc-main.c b/drivers/media/rc/rc-main.c
index 759a40a42ea..1cf382a0b27 100644
--- a/drivers/media/rc/rc-main.c
+++ b/drivers/media/rc/rc-main.c
@@ -715,14 +715,14 @@ static void ir_close(struct input_dev *idev)
 }
 
 /* class for /sys/class/rc */
-static char *ir_devnode(struct device *dev, umode_t *mode)
+static char *rc_devnode(struct device *dev, umode_t *mode)
 {
 	return kasprintf(GFP_KERNEL, "rc/%s", dev_name(dev));
 }
 
-static struct class ir_input_class = {
+static struct class rc_class = {
 	.name		= "rc",
-	.devnode	= ir_devnode,
+	.devnode	= rc_devnode,
 };
 
 /*
@@ -783,13 +783,12 @@ static ssize_t show_protocols(struct device *device,
 
 	mutex_lock(&dev->lock);
 
-	if (dev->driver_type == RC_DRIVER_SCANCODE) {
-		enabled = dev->rc_map.rc_type;
+	enabled = dev->enabled_protocols;
+	if (dev->driver_type == RC_DRIVER_SCANCODE)
 		allowed = dev->allowed_protos;
-	} else if (dev->raw) {
-		enabled = dev->raw->enabled_protocols;
+	else if (dev->raw)
 		allowed = ir_raw_get_allowed_protocols();
-	} else {
+	else {
 		mutex_unlock(&dev->lock);
 		return -ENODEV;
 	}
@@ -847,7 +846,6 @@ static ssize_t store_protocols(struct device *device,
 	u64 type;
 	u64 mask;
 	int rc, i, count = 0;
-	unsigned long flags;
 	ssize_t ret;
 
 	/* Device is being removed */
@@ -856,15 +854,12 @@ static ssize_t store_protocols(struct device *device,
 
 	mutex_lock(&dev->lock);
 
-	if (dev->driver_type == RC_DRIVER_SCANCODE)
-		type = dev->rc_map.rc_type;
-	else if (dev->raw)
-		type = dev->raw->enabled_protocols;
-	else {
+	if (dev->driver_type != RC_DRIVER_SCANCODE && !dev->raw) {
 		IR_dprintk(1, "Protocol switching not supported\n");
 		ret = -EINVAL;
 		goto out;
 	}
+	type = dev->enabled_protocols;
 
 	while ((tmp = strsep((char **) &data, " \n")) != NULL) {
 		if (!*tmp)
@@ -922,14 +917,7 @@ static ssize_t store_protocols(struct device *device,
 		}
 	}
 
-	if (dev->driver_type == RC_DRIVER_SCANCODE) {
-		spin_lock_irqsave(&dev->rc_map.lock, flags);
-		dev->rc_map.rc_type = type;
-		spin_unlock_irqrestore(&dev->rc_map.lock, flags);
-	} else {
-		dev->raw->enabled_protocols = type;
-	}
-
+	dev->enabled_protocols = type;
 	IR_dprintk(1, "Current protocol(s): 0x%llx\n",
 		   (long long)type);
 
@@ -1016,7 +1004,7 @@ struct rc_dev *rc_allocate_device(void)
 	setup_timer(&dev->timer_keyup, ir_timer_keyup, (unsigned long)dev);
 
 	dev->dev.type = &rc_dev_type;
-	dev->dev.class = &ir_input_class;
+	dev->dev.class = &rc_class;
 	device_initialize(&dev->dev);
 
 	__module_get(THIS_MODULE);
@@ -1068,9 +1056,8 @@ int rc_register_device(struct rc_dev *dev)
 	/*
 	 * Take the lock here, as the device sysfs node will appear
 	 * when device_add() is called, which may trigger an ir-keytable udev
-	 * rule, which will in turn call show_protocols and access either
-	 * dev->rc_map.rc_type or dev->raw->enabled_protocols before it has
-	 * been initialized.
+	 * rule, which will in turn call show_protocols and access
+	 * dev->enabled_protocols before it has been initialized.
 	 */
 	mutex_lock(&dev->lock);
 
@@ -1132,6 +1119,7 @@ int rc_register_device(struct rc_dev *dev)
 		rc = dev->change_protocol(dev, &rc_type);
 		if (rc < 0)
 			goto out_raw;
+		dev->enabled_protocols = rc_type;
 	}
 
 	mutex_unlock(&dev->lock);
@@ -1190,7 +1178,7 @@ EXPORT_SYMBOL_GPL(rc_unregister_device);
 
 static int __init rc_core_init(void)
 {
-	int rc = class_register(&ir_input_class);
+	int rc = class_register(&rc_class);
 	if (rc) {
 		printk(KERN_ERR "rc_core: unable to register rc class\n");
 		return rc;
@@ -1203,11 +1191,11 @@ static int __init rc_core_init(void)
 
 static void __exit rc_core_exit(void)
 {
-	class_unregister(&ir_input_class);
+	class_unregister(&rc_class);
 	rc_map_unregister(&empty_map);
 }
 
-module_init(rc_core_init);
+subsys_initcall(rc_core_init);
 module_exit(rc_core_exit);
 
 int rc_core_debug;    /* ir_debug level (0,1,2) */
diff --git a/drivers/media/rc/redrat3.c b/drivers/media/rc/redrat3.c
index 1b37fe2779f..12167a6b547 100644
--- a/drivers/media/rc/redrat3.c
+++ b/drivers/media/rc/redrat3.c
@@ -45,6 +45,7 @@
  *
  */
 
+#include <asm/unaligned.h>
 #include <linux/device.h>
 #include <linux/module.h>
 #include <linux/slab.h>
@@ -53,7 +54,6 @@
 #include <media/rc-core.h>
 
 /* Driver Information */
-#define DRIVER_VERSION "0.70"
 #define DRIVER_AUTHOR "Jarod Wilson <jarod@redhat.com>"
 #define DRIVER_AUTHOR2 "The Dweller, Stephen Cox"
 #define DRIVER_DESC "RedRat3 USB IR Transceiver Driver"
@@ -129,25 +129,11 @@ static int debug;
 /* USB bulk-in IR data endpoint address */
 #define RR3_BULK_IN_EP_ADDR	0x82
 
-/* Raw Modulated signal data value offsets */
-#define RR3_PAUSE_OFFSET	0
-#define RR3_FREQ_COUNT_OFFSET	4
-#define RR3_NUM_PERIOD_OFFSET	6
-#define RR3_MAX_LENGTHS_OFFSET	8
-#define RR3_NUM_LENGTHS_OFFSET	9
-#define RR3_MAX_SIGS_OFFSET	10
-#define RR3_NUM_SIGS_OFFSET	12
-#define RR3_REPEATS_OFFSET	14
-
 /* Size of the fixed-length portion of the signal */
-#define RR3_HEADER_LENGTH	15
 #define RR3_DRIVER_MAXLENS	128
 #define RR3_MAX_SIG_SIZE	512
-#define RR3_MAX_BUF_SIZE	\
-	((2 * RR3_HEADER_LENGTH) + RR3_DRIVER_MAXLENS + RR3_MAX_SIG_SIZE)
 #define RR3_TIME_UNIT		50
 #define RR3_END_OF_SIGNAL	0x7f
-#define RR3_TX_HEADER_OFFSET	4
 #define RR3_TX_TRAILER_LEN	2
 #define RR3_RX_MIN_TIMEOUT	5
 #define RR3_RX_MAX_TIMEOUT	2000
@@ -159,6 +145,32 @@ static int debug;
 #define USB_RR3USB_PRODUCT_ID	0x0001
 #define USB_RR3IIUSB_PRODUCT_ID	0x0005
 
+struct redrat3_header {
+	__be16 length;
+	__be16 transfer_type;
+} __packed;
+
+/* sending and receiving irdata */
+struct redrat3_irdata {
+	struct redrat3_header header;
+	__be32 pause;
+	__be16 mod_freq_count;
+	__be16 num_periods;
+	__u8 max_lengths;
+	__u8 no_lengths;
+	__be16 max_sig_size;
+	__be16 sig_size;
+	__u8 no_repeats;
+	__be16 lens[RR3_DRIVER_MAXLENS]; /* not aligned */
+	__u8 sigdata[RR3_MAX_SIG_SIZE];
+} __packed;
+
+/* firmware errors */
+struct redrat3_error {
+	struct redrat3_header header;
+	__be16 fw_error;
+} __packed;
+
 /* table of devices that work with this driver */
 static struct usb_device_id redrat3_dev_table[] = {
 	/* Original version of the RedRat3 */
@@ -180,20 +192,15 @@ struct redrat3_dev {
 	/* the receive endpoint */
 	struct usb_endpoint_descriptor *ep_in;
 	/* the buffer to receive data */
-	unsigned char *bulk_in_buf;
+	void *bulk_in_buf;
 	/* urb used to read ir data */
 	struct urb *read_urb;
 
 	/* the send endpoint */
 	struct usb_endpoint_descriptor *ep_out;
-	/* the buffer to send data */
-	unsigned char *bulk_out_buf;
-	/* the urb used to send data */
-	struct urb *write_urb;
 
 	/* usb dma */
 	dma_addr_t dma_in;
-	dma_addr_t dma_out;
 
 	/* rx signal timeout timer */
 	struct timer_list rx_timeout;
@@ -205,72 +212,15 @@ struct redrat3_dev {
 	bool transmitting;
 
 	/* store for current packet */
-	char pbuf[RR3_MAX_BUF_SIZE];
-	u16 pktlen;
-	u16 pkttype;
+	struct redrat3_irdata irdata;
 	u16 bytes_read;
-	/* indicate whether we are going to reprocess
-	 * the USB callback with a bigger buffer */
-	int buftoosmall;
-	char *datap;
 
 	u32 carrier;
 
-	char name[128];
+	char name[64];
 	char phys[64];
 };
 
-/* All incoming data buffers adhere to a very specific data format */
-struct redrat3_signal_header {
-	u16 length;	/* Length of data being transferred */
-	u16 transfer_type; /* Type of data transferred */
-	u32 pause;	/* Pause between main and repeat signals */
-	u16 mod_freq_count; /* Value of timer on mod. freq. measurement */
-	u16 no_periods;	/* No. of periods over which mod. freq. is measured */
-	u8 max_lengths;	/* Max no. of lengths (i.e. size of array) */
-	u8 no_lengths;	/* Actual no. of elements in lengths array */
-	u16 max_sig_size; /* Max no. of values in signal data array */
-	u16 sig_size;	/* Acuto no. of values in signal data array */
-	u8 no_repeats;	/* No. of repeats of repeat signal section */
-	/* Here forward is the lengths and signal data */
-};
-
-static void redrat3_dump_signal_header(struct redrat3_signal_header *header)
-{
-	pr_info("%s:\n", __func__);
-	pr_info(" * length: %u, transfer_type: 0x%02x\n",
-		header->length, header->transfer_type);
-	pr_info(" * pause: %u, freq_count: %u, no_periods: %u\n",
-		header->pause, header->mod_freq_count, header->no_periods);
-	pr_info(" * lengths: %u (max: %u)\n",
-		header->no_lengths, header->max_lengths);
-	pr_info(" * sig_size: %u (max: %u)\n",
-		header->sig_size, header->max_sig_size);
-	pr_info(" * repeats: %u\n", header->no_repeats);
-}
-
-static void redrat3_dump_signal_data(char *buffer, u16 len)
-{
-	int offset, i;
-	char *data_vals;
-
-	pr_info("%s:", __func__);
-
-	offset = RR3_TX_HEADER_OFFSET + RR3_HEADER_LENGTH
-		 + (RR3_DRIVER_MAXLENS * sizeof(u16));
-
-	/* read RR3_DRIVER_MAXLENS from ctrl msg */
-	data_vals = buffer + offset;
-
-	for (i = 0; i < len; i++) {
-		if (i % 10 == 0)
-			pr_cont("\n * ");
-		pr_cont("%02x ", *data_vals++);
-	}
-
-	pr_cont("\n");
-}
-
 /*
  * redrat3_issue_async
  *
@@ -283,7 +233,6 @@ static void redrat3_issue_async(struct redrat3_dev *rr3)
 
 	rr3_ftr(rr3->dev, "Entering %s\n", __func__);
 
-	memset(rr3->bulk_in_buf, 0, rr3->ep_in->wMaxPacketSize);
 	res = usb_submit_urb(rr3->read_urb, GFP_ATOMIC);
 	if (res)
 		rr3_dbg(rr3->dev, "%s: receive request FAILED! "
@@ -352,13 +301,14 @@ static void redrat3_dump_fw_error(struct redrat3_dev *rr3, int code)
 	}
 }
 
-static u32 redrat3_val_to_mod_freq(struct redrat3_signal_header *ph)
+static u32 redrat3_val_to_mod_freq(struct redrat3_irdata *irdata)
 {
 	u32 mod_freq = 0;
+	u16 mod_freq_count = be16_to_cpu(irdata->mod_freq_count);
 
-	if (ph->mod_freq_count != 0)
-		mod_freq = (RR3_CLK * ph->no_periods) /
-				(ph->mod_freq_count * RR3_CLK_PER_COUNT);
+	if (mod_freq_count != 0)
+		mod_freq = (RR3_CLK * be16_to_cpu(irdata->num_periods)) /
+			(mod_freq_count * RR3_CLK_PER_COUNT);
 
 	return mod_freq;
 }
@@ -396,7 +346,6 @@ static u32 redrat3_us_to_len(u32 microsec)
 
 	/* don't allow zero lengths to go back, breaks lirc */
 	return result ? result : 1;
-
 }
 
 /* timer callback to send reset event */
@@ -411,16 +360,11 @@ static void redrat3_rx_timeout(unsigned long data)
 static void redrat3_process_ir_data(struct redrat3_dev *rr3)
 {
 	DEFINE_IR_RAW_EVENT(rawir);
-	struct redrat3_signal_header header;
 	struct device *dev;
-	int i, trailer = 0;
+	unsigned i, trailer = 0;
+	unsigned sig_size, single_len, offset, val;
 	unsigned long delay;
-	u32 mod_freq, single_len;
-	u16 *len_vals;
-	u8 *data_vals;
-	u32 tmp32;
-	u16 tmp16;
-	char *sig_data;
+	u32 mod_freq;
 
 	if (!rr3) {
 		pr_err("%s called with no context!\n", __func__);
@@ -430,57 +374,20 @@ static void redrat3_process_ir_data(struct redrat3_dev *rr3)
 	rr3_ftr(rr3->dev, "Entered %s\n", __func__);
 
 	dev = rr3->dev;
-	sig_data = rr3->pbuf;
-
-	header.length = rr3->pktlen;
-	header.transfer_type = rr3->pkttype;
-
-	/* Sanity check */
-	if (!(header.length >= RR3_HEADER_LENGTH))
-		dev_warn(dev, "read returned less than rr3 header len\n");
 
 	/* Make sure we reset the IR kfifo after a bit of inactivity */
 	delay = usecs_to_jiffies(rr3->hw_timeout);
 	mod_timer(&rr3->rx_timeout, jiffies + delay);
 
-	memcpy(&tmp32, sig_data + RR3_PAUSE_OFFSET, sizeof(tmp32));
-	header.pause = be32_to_cpu(tmp32);
-
-	memcpy(&tmp16, sig_data + RR3_FREQ_COUNT_OFFSET, sizeof(tmp16));
-	header.mod_freq_count = be16_to_cpu(tmp16);
-
-	memcpy(&tmp16, sig_data + RR3_NUM_PERIOD_OFFSET, sizeof(tmp16));
-	header.no_periods = be16_to_cpu(tmp16);
-
-	header.max_lengths = sig_data[RR3_MAX_LENGTHS_OFFSET];
-	header.no_lengths = sig_data[RR3_NUM_LENGTHS_OFFSET];
-
-	memcpy(&tmp16, sig_data + RR3_MAX_SIGS_OFFSET, sizeof(tmp16));
-	header.max_sig_size = be16_to_cpu(tmp16);
-
-	memcpy(&tmp16, sig_data + RR3_NUM_SIGS_OFFSET, sizeof(tmp16));
-	header.sig_size = be16_to_cpu(tmp16);
-
-	header.no_repeats= sig_data[RR3_REPEATS_OFFSET];
-
-	if (debug) {
-		redrat3_dump_signal_header(&header);
-		redrat3_dump_signal_data(sig_data, header.sig_size);
-	}
-
-	mod_freq = redrat3_val_to_mod_freq(&header);
+	mod_freq = redrat3_val_to_mod_freq(&rr3->irdata);
 	rr3_dbg(dev, "Got mod_freq of %u\n", mod_freq);
 
-	/* Here we pull out the 'length' values from the signal */
-	len_vals = (u16 *)(sig_data + RR3_HEADER_LENGTH);
-
-	data_vals = sig_data + RR3_HEADER_LENGTH +
-		    (header.max_lengths * sizeof(u16));
-
 	/* process each rr3 encoded byte into an int */
-	for (i = 0; i < header.sig_size; i++) {
-		u16 val = len_vals[data_vals[i]];
-		single_len = redrat3_len_to_us((u32)be16_to_cpu(val));
+	sig_size = be16_to_cpu(rr3->irdata.sig_size);
+	for (i = 0; i < sig_size; i++) {
+		offset = rr3->irdata.sigdata[i];
+		val = get_unaligned_be16(&rr3->irdata.lens[offset]);
+		single_len = redrat3_len_to_us(val);
 
 		/* we should always get pulse/space/pulse/space samples */
 		if (i % 2)
@@ -515,8 +422,6 @@ static void redrat3_process_ir_data(struct redrat3_dev *rr3)
 
 	rr3_dbg(dev, "calling ir_raw_event_handle\n");
 	ir_raw_event_handle(rr3->rc);
-
-	return;
 }
 
 /* Util fn to send rr3 cmds */
@@ -540,7 +445,7 @@ static u8 redrat3_send_cmd(int cmd, struct redrat3_dev *rr3)
 			__func__, res, *data);
 		res = -EIO;
 	} else
-		res = (u8)data[0];
+		res = data[0];
 
 	kfree(data);
 
@@ -598,22 +503,18 @@ static inline void redrat3_delete(struct redrat3_dev *rr3,
 {
 	rr3_ftr(rr3->dev, "%s cleaning up\n", __func__);
 	usb_kill_urb(rr3->read_urb);
-	usb_kill_urb(rr3->write_urb);
 
 	usb_free_urb(rr3->read_urb);
-	usb_free_urb(rr3->write_urb);
 
-	usb_free_coherent(udev, rr3->ep_in->wMaxPacketSize,
+	usb_free_coherent(udev, le16_to_cpu(rr3->ep_in->wMaxPacketSize),
 			  rr3->bulk_in_buf, rr3->dma_in);
-	usb_free_coherent(udev, rr3->ep_out->wMaxPacketSize,
-			  rr3->bulk_out_buf, rr3->dma_out);
 
 	kfree(rr3);
 }
 
 static u32 redrat3_get_timeout(struct redrat3_dev *rr3)
 {
-	u32 *tmp;
+	__be32 *tmp;
 	u32 timeout = MS_TO_US(150); /* a sane default, if things go haywire */
 	int len, ret, pipe;
 
@@ -628,14 +529,16 @@ static u32 redrat3_get_timeout(struct redrat3_dev *rr3)
 	ret = usb_control_msg(rr3->udev, pipe, RR3_GET_IR_PARAM,
 			      USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
 			      RR3_IR_IO_SIG_TIMEOUT, 0, tmp, len, HZ * 5);
-	if (ret != len) {
+	if (ret != len)
 		dev_warn(rr3->dev, "Failed to read timeout from hardware\n");
-		return timeout;
+	else {
+		timeout = redrat3_len_to_us(be32_to_cpup(tmp));
+
+		rr3_dbg(rr3->dev, "Got timeout of %d ms\n", timeout / 1000);
 	}
 
-	timeout = redrat3_len_to_us(be32_to_cpu(*tmp));
+	kfree(tmp);
 
-	rr3_dbg(rr3->dev, "Got timeout of %d ms\n", timeout / 1000);
 	return timeout;
 }
 
@@ -652,7 +555,7 @@ static void redrat3_reset(struct redrat3_dev *rr3)
 	rxpipe = usb_rcvctrlpipe(udev, 0);
 	txpipe = usb_sndctrlpipe(udev, 0);
 
-	val = kzalloc(len, GFP_KERNEL);
+	val = kmalloc(len, GFP_KERNEL);
 	if (!val) {
 		dev_err(dev, "Memory allocation failure\n");
 		return;
@@ -706,82 +609,74 @@ static void redrat3_get_firmware_rev(struct redrat3_dev *rr3)
 	rr3_ftr(rr3->dev, "Exiting %s\n", __func__);
 }
 
-static void redrat3_read_packet_start(struct redrat3_dev *rr3, int len)
+static void redrat3_read_packet_start(struct redrat3_dev *rr3, unsigned len)
 {
-	u16 tx_error;
-	u16 hdrlen;
+	struct redrat3_header *header = rr3->bulk_in_buf;
+	unsigned pktlen, pkttype;
 
 	rr3_ftr(rr3->dev, "Entering %s\n", __func__);
 
 	/* grab the Length and type of transfer */
-	memcpy(&(rr3->pktlen), (unsigned char *) rr3->bulk_in_buf,
-	       sizeof(rr3->pktlen));
-	memcpy(&(rr3->pkttype), ((unsigned char *) rr3->bulk_in_buf +
-		sizeof(rr3->pktlen)),
-	       sizeof(rr3->pkttype));
+	pktlen = be16_to_cpu(header->length);
+	pkttype = be16_to_cpu(header->transfer_type);
 
-	/*data needs conversion to know what its real values are*/
-	rr3->pktlen = be16_to_cpu(rr3->pktlen);
-	rr3->pkttype = be16_to_cpu(rr3->pkttype);
+	if (pktlen > sizeof(rr3->irdata)) {
+		dev_warn(rr3->dev, "packet length %u too large\n", pktlen);
+		return;
+	}
 
-	switch (rr3->pkttype) {
+	switch (pkttype) {
 	case RR3_ERROR:
-		memcpy(&tx_error, ((unsigned char *)rr3->bulk_in_buf
-			+ (sizeof(rr3->pktlen) + sizeof(rr3->pkttype))),
-		       sizeof(tx_error));
-		tx_error = be16_to_cpu(tx_error);
-		redrat3_dump_fw_error(rr3, tx_error);
+		if (len >= sizeof(struct redrat3_error)) {
+			struct redrat3_error *error = rr3->bulk_in_buf;
+			unsigned fw_error = be16_to_cpu(error->fw_error);
+			redrat3_dump_fw_error(rr3, fw_error);
+		}
 		break;
 
 	case RR3_MOD_SIGNAL_IN:
-		hdrlen = sizeof(rr3->pktlen) + sizeof(rr3->pkttype);
+		memcpy(&rr3->irdata, rr3->bulk_in_buf, len);
 		rr3->bytes_read = len;
-		rr3->bytes_read -= hdrlen;
-		rr3->datap = &(rr3->pbuf[0]);
-
-		memcpy(rr3->datap, ((unsigned char *)rr3->bulk_in_buf + hdrlen),
-		       rr3->bytes_read);
-		rr3->datap += rr3->bytes_read;
 		rr3_dbg(rr3->dev, "bytes_read %d, pktlen %d\n",
-			rr3->bytes_read, rr3->pktlen);
+			rr3->bytes_read, pktlen);
 		break;
 
 	default:
-		rr3_dbg(rr3->dev, "ignoring packet with type 0x%02x, "
-			"len of %d, 0x%02x\n", rr3->pkttype, len, rr3->pktlen);
+		rr3_dbg(rr3->dev, "ignoring packet with type 0x%02x, len of %d, 0x%02x\n",
+						pkttype, len, pktlen);
 		break;
 	}
 }
 
-static void redrat3_read_packet_continue(struct redrat3_dev *rr3, int len)
+static void redrat3_read_packet_continue(struct redrat3_dev *rr3, unsigned len)
 {
+	void *irdata = &rr3->irdata;
 
 	rr3_ftr(rr3->dev, "Entering %s\n", __func__);
 
-	memcpy(rr3->datap, (unsigned char *)rr3->bulk_in_buf, len);
-	rr3->datap += len;
+	if (len + rr3->bytes_read > sizeof(rr3->irdata)) {
+		dev_warn(rr3->dev, "too much data for packet\n");
+		rr3->bytes_read = 0;
+		return;
+	}
+
+	memcpy(irdata + rr3->bytes_read, rr3->bulk_in_buf, len);
 
 	rr3->bytes_read += len;
-	rr3_dbg(rr3->dev, "bytes_read %d, pktlen %d\n",
-		rr3->bytes_read, rr3->pktlen);
+	rr3_dbg(rr3->dev, "bytes_read %d, pktlen %d\n", rr3->bytes_read,
+				 be16_to_cpu(rr3->irdata.header.length));
 }
 
 /* gather IR data from incoming urb, process it when we have enough */
-static int redrat3_get_ir_data(struct redrat3_dev *rr3, int len)
+static int redrat3_get_ir_data(struct redrat3_dev *rr3, unsigned len)
 {
 	struct device *dev = rr3->dev;
+	unsigned pkttype;
 	int ret = 0;
 
 	rr3_ftr(dev, "Entering %s\n", __func__);
 
-	if (rr3->pktlen > RR3_MAX_BUF_SIZE) {
-		dev_err(rr3->dev, "error: packet larger than buffer\n");
-		ret = -EINVAL;
-		goto out;
-	}
-
-	if ((rr3->bytes_read == 0) &&
-	    (len >= (sizeof(rr3->pkttype) + sizeof(rr3->pktlen)))) {
+	if (rr3->bytes_read == 0 && len >= sizeof(struct redrat3_header)) {
 		redrat3_read_packet_start(rr3, len);
 	} else if (rr3->bytes_read != 0) {
 		redrat3_read_packet_continue(rr3, len);
@@ -791,31 +686,25 @@ static int redrat3_get_ir_data(struct redrat3_dev *rr3, int len)
 		goto out;
 	}
 
-	if (rr3->bytes_read > rr3->pktlen) {
-		dev_err(dev, "bytes_read (%d) greater than pktlen (%d)\n",
-			rr3->bytes_read, rr3->pktlen);
-		ret = -EINVAL;
-		goto out;
-	} else if (rr3->bytes_read < rr3->pktlen)
+	if (rr3->bytes_read < be16_to_cpu(rr3->irdata.header.length))
 		/* we're still accumulating data */
 		return 0;
 
 	/* if we get here, we've got IR data to decode */
-	if (rr3->pkttype == RR3_MOD_SIGNAL_IN)
+	pkttype = be16_to_cpu(rr3->irdata.header.transfer_type);
+	if (pkttype == RR3_MOD_SIGNAL_IN)
 		redrat3_process_ir_data(rr3);
 	else
-		rr3_dbg(dev, "discarding non-signal data packet "
-			"(type 0x%02x)\n", rr3->pkttype);
+		rr3_dbg(dev, "discarding non-signal data packet (type 0x%02x)\n",
+								pkttype);
 
 out:
 	rr3->bytes_read = 0;
-	rr3->pktlen = 0;
-	rr3->pkttype = 0;
 	return ret;
 }
 
 /* callback function from USB when async USB request has completed */
-static void redrat3_handle_async(struct urb *urb, struct pt_regs *regs)
+static void redrat3_handle_async(struct urb *urb)
 {
 	struct redrat3_dev *rr3;
 	int ret;
@@ -851,34 +740,16 @@ static void redrat3_handle_async(struct urb *urb, struct pt_regs *regs)
 	default:
 		dev_warn(rr3->dev, "Error: urb status = %d\n", urb->status);
 		rr3->bytes_read = 0;
-		rr3->pktlen = 0;
-		rr3->pkttype = 0;
 		break;
 	}
 }
 
-static void redrat3_write_bulk_callback(struct urb *urb, struct pt_regs *regs)
-{
-	struct redrat3_dev *rr3;
-	int len;
-
-	if (!urb)
-		return;
-
-	rr3 = urb->context;
-	if (rr3) {
-		len = urb->actual_length;
-		rr3_ftr(rr3->dev, "%s: called (status=%d len=%d)\n",
-			__func__, urb->status, len);
-	}
-}
-
 static u16 mod_freq_to_val(unsigned int mod_freq)
 {
 	int mult = 6000000;
 
 	/* Clk used in mod. freq. generation is CLK24/4. */
-	return (u16)(65536 - (mult / mod_freq));
+	return 65536 - (mult / mod_freq);
 }
 
 static int redrat3_set_tx_carrier(struct rc_dev *rcdev, u32 carrier)
@@ -900,17 +771,12 @@ static int redrat3_transmit_ir(struct rc_dev *rcdev, unsigned *txbuf,
 {
 	struct redrat3_dev *rr3 = rcdev->priv;
 	struct device *dev = rr3->dev;
-	struct redrat3_signal_header header;
-	int i, j, ret, ret_len, offset;
+	struct redrat3_irdata *irdata = NULL;
+	int ret, ret_len;
 	int lencheck, cur_sample_len, pipe;
-	char *buffer = NULL, *sigdata = NULL;
 	int *sample_lens = NULL;
-	u32 tmpi;
-	u16 tmps;
-	u8 *datap;
 	u8 curlencheck = 0;
-	u16 *lengths_ptr;
-	int sendbuf_len;
+	unsigned i, sendbuf_len;
 
 	rr3_ftr(dev, "Entering %s\n", __func__);
 
@@ -931,8 +797,19 @@ static int redrat3_transmit_ir(struct rc_dev *rcdev, unsigned *txbuf,
 		goto out;
 	}
 
+	irdata = kzalloc(sizeof(*irdata), GFP_KERNEL);
+	if (!irdata) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
 	for (i = 0; i < count; i++) {
 		cur_sample_len = redrat3_us_to_len(txbuf[i]);
+		if (cur_sample_len > 0xffff) {
+			dev_warn(dev, "transmit period of %uus truncated to %uus\n",
+					txbuf[i], redrat3_len_to_us(0xffff));
+			cur_sample_len = 0xffff;
+		}
 		for (lencheck = 0; lencheck < curlencheck; lencheck++) {
 			if (sample_lens[lencheck] == cur_sample_len)
 				break;
@@ -944,94 +821,41 @@ static int redrat3_transmit_ir(struct rc_dev *rcdev, unsigned *txbuf,
 				/* now convert the value to a proper
 				 * rr3 value.. */
 				sample_lens[curlencheck] = cur_sample_len;
+				put_unaligned_be16(cur_sample_len,
+						&irdata->lens[curlencheck]);
 				curlencheck++;
 			} else {
 				count = i - 1;
 				break;
 			}
 		}
+		irdata->sigdata[i] = lencheck;
 	}
 
-	sigdata = kzalloc((count + RR3_TX_TRAILER_LEN), GFP_KERNEL);
-	if (!sigdata) {
-		ret = -ENOMEM;
-		goto out;
-	}
-
-	sigdata[count] = RR3_END_OF_SIGNAL;
-	sigdata[count + 1] = RR3_END_OF_SIGNAL;
-	for (i = 0; i < count; i++) {
-		for (j = 0; j < curlencheck; j++) {
-			if (sample_lens[j] == redrat3_us_to_len(txbuf[i]))
-				sigdata[i] = j;
-		}
-	}
-
-	offset = RR3_TX_HEADER_OFFSET;
-	sendbuf_len = RR3_HEADER_LENGTH + (sizeof(u16) * RR3_DRIVER_MAXLENS)
-			+ count + RR3_TX_TRAILER_LEN + offset;
-
-	buffer = kzalloc(sendbuf_len, GFP_KERNEL);
-	if (!buffer) {
-		ret = -ENOMEM;
-		goto out;
-	}
+	irdata->sigdata[count] = RR3_END_OF_SIGNAL;
+	irdata->sigdata[count + 1] = RR3_END_OF_SIGNAL;
 
+	sendbuf_len = offsetof(struct redrat3_irdata,
+					sigdata[count + RR3_TX_TRAILER_LEN]);
 	/* fill in our packet header */
-	header.length = sendbuf_len - offset;
-	header.transfer_type = RR3_MOD_SIGNAL_OUT;
-	header.pause = redrat3_len_to_us(100);
-	header.mod_freq_count = mod_freq_to_val(rr3->carrier);
-	header.no_periods = 0; /* n/a to transmit */
-	header.max_lengths = RR3_DRIVER_MAXLENS;
-	header.no_lengths = curlencheck;
-	header.max_sig_size = RR3_MAX_SIG_SIZE;
-	header.sig_size = count + RR3_TX_TRAILER_LEN;
-	/* we currently rely on repeat handling in the IR encoding source */
-	header.no_repeats = 0;
-
-	tmps = cpu_to_be16(header.length);
-	memcpy(buffer, &tmps, 2);
-
-	tmps = cpu_to_be16(header.transfer_type);
-	memcpy(buffer + 2, &tmps, 2);
-
-	tmpi = cpu_to_be32(header.pause);
-	memcpy(buffer + offset, &tmpi, sizeof(tmpi));
-
-	tmps = cpu_to_be16(header.mod_freq_count);
-	memcpy(buffer + offset + RR3_FREQ_COUNT_OFFSET, &tmps, 2);
-
-	buffer[offset + RR3_NUM_LENGTHS_OFFSET] = header.no_lengths;
-
-	tmps = cpu_to_be16(header.sig_size);
-	memcpy(buffer + offset + RR3_NUM_SIGS_OFFSET, &tmps, 2);
-
-	buffer[offset + RR3_REPEATS_OFFSET] = header.no_repeats;
-
-	lengths_ptr = (u16 *)(buffer + offset + RR3_HEADER_LENGTH);
-	for (i = 0; i < curlencheck; ++i)
-		lengths_ptr[i] = cpu_to_be16(sample_lens[i]);
-
-	datap = (u8 *)(buffer + offset + RR3_HEADER_LENGTH +
-			    (sizeof(u16) * RR3_DRIVER_MAXLENS));
-	memcpy(datap, sigdata, (count + RR3_TX_TRAILER_LEN));
-
-	if (debug) {
-		redrat3_dump_signal_header(&header);
-		redrat3_dump_signal_data(buffer, header.sig_size);
-	}
+	irdata->header.length = cpu_to_be16(sendbuf_len -
+						sizeof(struct redrat3_header));
+	irdata->header.transfer_type = cpu_to_be16(RR3_MOD_SIGNAL_OUT);
+	irdata->pause = cpu_to_be32(redrat3_len_to_us(100));
+	irdata->mod_freq_count = cpu_to_be16(mod_freq_to_val(rr3->carrier));
+	irdata->no_lengths = curlencheck;
+	irdata->sig_size = cpu_to_be16(count + RR3_TX_TRAILER_LEN);
 
 	pipe = usb_sndbulkpipe(rr3->udev, rr3->ep_out->bEndpointAddress);
-	tmps = usb_bulk_msg(rr3->udev, pipe, buffer,
+	ret = usb_bulk_msg(rr3->udev, pipe, irdata,
 			    sendbuf_len, &ret_len, 10 * HZ);
-	rr3_dbg(dev, "sent %d bytes, (ret %d)\n", ret_len, tmps);
+	rr3_dbg(dev, "sent %d bytes, (ret %d)\n", ret_len, ret);
 
 	/* now tell the hardware to transmit what we sent it */
 	pipe = usb_rcvctrlpipe(rr3->udev, 0);
 	ret = usb_control_msg(rr3->udev, pipe, RR3_TX_SEND_SIGNAL,
 			      USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
-			      0, 0, buffer, 2, HZ * 10);
+			      0, 0, irdata, 2, HZ * 10);
 
 	if (ret < 0)
 		dev_err(dev, "Error: control msg send failed, rc %d\n", ret);
@@ -1040,8 +864,7 @@ static int redrat3_transmit_ir(struct rc_dev *rcdev, unsigned *txbuf,
 
 out:
 	kfree(sample_lens);
-	kfree(buffer);
-	kfree(sigdata);
+	kfree(irdata);
 
 	rr3->transmitting = false;
 	/* rr3 re-enables rc detector because it was enabled before */
@@ -1165,38 +988,18 @@ static int redrat3_dev_probe(struct usb_interface *intf,
 	}
 
 	rr3->ep_in = ep_in;
-	rr3->bulk_in_buf = usb_alloc_coherent(udev, ep_in->wMaxPacketSize,
-					      GFP_ATOMIC, &rr3->dma_in);
+	rr3->bulk_in_buf = usb_alloc_coherent(udev,
+		le16_to_cpu(ep_in->wMaxPacketSize), GFP_ATOMIC, &rr3->dma_in);
 	if (!rr3->bulk_in_buf) {
 		dev_err(dev, "Read buffer allocation failure\n");
 		goto error;
 	}
 
 	pipe = usb_rcvbulkpipe(udev, ep_in->bEndpointAddress);
-	usb_fill_bulk_urb(rr3->read_urb, udev, pipe,
-			  rr3->bulk_in_buf, ep_in->wMaxPacketSize,
-			  (usb_complete_t)redrat3_handle_async, rr3);
-
-	/* set up bulk-out endpoint*/
-	rr3->write_urb = usb_alloc_urb(0, GFP_KERNEL);
-	if (!rr3->write_urb) {
-		dev_err(dev, "Write urb allocation failure\n");
-		goto error;
-	}
+	usb_fill_bulk_urb(rr3->read_urb, udev, pipe, rr3->bulk_in_buf,
+		le16_to_cpu(ep_in->wMaxPacketSize), redrat3_handle_async, rr3);
 
 	rr3->ep_out = ep_out;
-	rr3->bulk_out_buf = usb_alloc_coherent(udev, ep_out->wMaxPacketSize,
-					       GFP_ATOMIC, &rr3->dma_out);
-	if (!rr3->bulk_out_buf) {
-		dev_err(dev, "Write buffer allocation failure\n");
-		goto error;
-	}
-
-	pipe = usb_sndbulkpipe(udev, ep_out->bEndpointAddress);
-	usb_fill_bulk_urb(rr3->write_urb, udev, pipe,
-			  rr3->bulk_out_buf, ep_out->wMaxPacketSize,
-			  (usb_complete_t)redrat3_write_bulk_callback, rr3);
-
 	rr3->udev = udev;
 
 	redrat3_reset(rr3);
diff --git a/drivers/media/rc/ttusbir.c b/drivers/media/rc/ttusbir.c
index cf0d47f57fb..891762d167e 100644
--- a/drivers/media/rc/ttusbir.c
+++ b/drivers/media/rc/ttusbir.c
@@ -347,6 +347,7 @@ static int ttusbir_probe(struct usb_interface *intf,
 	return 0;
 out3:
 	rc_unregister_device(rc);
+	rc = NULL;
 out2:
 	led_classdev_unregister(&tt->led);
 out:
diff --git a/drivers/media/rc/winbond-cir.c b/drivers/media/rc/winbond-cir.c
index 535a18dccbd..87af2d3ba60 100644
--- a/drivers/media/rc/winbond-cir.c
+++ b/drivers/media/rc/winbond-cir.c
@@ -1151,6 +1151,7 @@ exit_release_wbase:
 	release_region(data->wbase, WAKEUP_IOMEM_LEN);
 exit_unregister_device:
 	rc_unregister_device(data->dev);
+	data->dev = NULL;
 exit_free_rc:
 	rc_free_device(data->dev);
 exit_unregister_led:
diff --git a/drivers/media/tuners/Kconfig b/drivers/media/tuners/Kconfig
index e8fdf713fce..f6768cad001 100644
--- a/drivers/media/tuners/Kconfig
+++ b/drivers/media/tuners/Kconfig
@@ -241,4 +241,18 @@ config MEDIA_TUNER_TUA9001
 	default m if !MEDIA_SUBDRV_AUTOSELECT
 	help
 	  Infineon TUA 9001 silicon tuner driver.
+
+config MEDIA_TUNER_IT913X
+	tristate "ITE Tech IT913x silicon tuner"
+	depends on MEDIA_SUPPORT && I2C
+	default m if !MEDIA_SUBDRV_AUTOSELECT
+	help
+	  ITE Tech IT913x silicon tuner driver.
+
+config MEDIA_TUNER_R820T
+	tristate "Rafael Micro R820T silicon tuner"
+	depends on MEDIA_SUPPORT && I2C
+	default m if !MEDIA_SUBDRV_AUTOSELECT
+	help
+	  Rafael Micro R820T silicon tuner driver.
 endmenu
diff --git a/drivers/media/tuners/Makefile b/drivers/media/tuners/Makefile
index 5e569b1083c..308f108eadb 100644
--- a/drivers/media/tuners/Makefile
+++ b/drivers/media/tuners/Makefile
@@ -34,6 +34,8 @@ 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
+obj-$(CONFIG_MEDIA_TUNER_IT913X) += tuner_it913x.o
+obj-$(CONFIG_MEDIA_TUNER_R820T) += r820t.o
 
 ccflags-y += -I$(srctree)/drivers/media/dvb-core
 ccflags-y += -I$(srctree)/drivers/media/dvb-frontends
diff --git a/drivers/media/tuners/e4000.h b/drivers/media/tuners/e4000.h
index 71b1935eb3d..3783a0bdf85 100644
--- a/drivers/media/tuners/e4000.h
+++ b/drivers/media/tuners/e4000.h
@@ -21,6 +21,7 @@
 #ifndef E4000_H
 #define E4000_H
 
+#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 
 struct e4000_config {
@@ -36,8 +37,7 @@ struct e4000_config {
 	u32 clock;
 };
 
-#if defined(CONFIG_MEDIA_TUNER_E4000) || \
-	(defined(CONFIG_MEDIA_TUNER_E4000_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_MEDIA_TUNER_E4000)
 extern struct dvb_frontend *e4000_attach(struct dvb_frontend *fe,
 		struct i2c_adapter *i2c, const struct e4000_config *cfg);
 #else
diff --git a/drivers/media/tuners/fc0011.h b/drivers/media/tuners/fc0011.h
index 0ee581f122d..43ec893a687 100644
--- a/drivers/media/tuners/fc0011.h
+++ b/drivers/media/tuners/fc0011.h
@@ -1,6 +1,7 @@
 #ifndef LINUX_FC0011_H_
 #define LINUX_FC0011_H_
 
+#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 
 
@@ -22,8 +23,7 @@ enum fc0011_fe_callback_commands {
 	FC0011_FE_CALLBACK_RESET,
 };
 
-#if defined(CONFIG_MEDIA_TUNER_FC0011) ||\
-    defined(CONFIG_MEDIA_TUNER_FC0011_MODULE)
+#if IS_ENABLED(CONFIG_MEDIA_TUNER_FC0011)
 struct dvb_frontend *fc0011_attach(struct dvb_frontend *fe,
 				   struct i2c_adapter *i2c,
 				   const struct fc0011_config *config);
diff --git a/drivers/media/tuners/fc0012.h b/drivers/media/tuners/fc0012.h
index 54508fcc346..1d08057e327 100644
--- a/drivers/media/tuners/fc0012.h
+++ b/drivers/media/tuners/fc0012.h
@@ -21,6 +21,7 @@
 #ifndef _FC0012_H_
 #define _FC0012_H_
 
+#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 #include "fc001x-common.h"
 
@@ -48,8 +49,7 @@ struct fc0012_config {
 	bool clock_out;
 };
 
-#if defined(CONFIG_MEDIA_TUNER_FC0012) || \
-	(defined(CONFIG_MEDIA_TUNER_FC0012_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_MEDIA_TUNER_FC0012)
 extern struct dvb_frontend *fc0012_attach(struct dvb_frontend *fe,
 					struct i2c_adapter *i2c,
 					const struct fc0012_config *cfg);
diff --git a/drivers/media/tuners/fc0013.h b/drivers/media/tuners/fc0013.h
index 594efd64aee..d65d5b37f56 100644
--- a/drivers/media/tuners/fc0013.h
+++ b/drivers/media/tuners/fc0013.h
@@ -22,11 +22,11 @@
 #ifndef _FC0013_H_
 #define _FC0013_H_
 
+#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 #include "fc001x-common.h"
 
-#if defined(CONFIG_MEDIA_TUNER_FC0013) || \
-	(defined(CONFIG_MEDIA_TUNER_FC0013_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_MEDIA_TUNER_FC0013)
 extern struct dvb_frontend *fc0013_attach(struct dvb_frontend *fe,
 					struct i2c_adapter *i2c,
 					u8 i2c_address, int dual_master,
diff --git a/drivers/media/tuners/fc2580.h b/drivers/media/tuners/fc2580.h
index 222601e5d23..9c43c1cc82d 100644
--- a/drivers/media/tuners/fc2580.h
+++ b/drivers/media/tuners/fc2580.h
@@ -21,6 +21,7 @@
 #ifndef FC2580_H
 #define FC2580_H
 
+#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 
 struct fc2580_config {
@@ -36,8 +37,7 @@ struct fc2580_config {
 	u32 clock;
 };
 
-#if defined(CONFIG_MEDIA_TUNER_FC2580) || \
-	(defined(CONFIG_MEDIA_TUNER_FC2580_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_MEDIA_TUNER_FC2580)
 extern struct dvb_frontend *fc2580_attach(struct dvb_frontend *fe,
 	struct i2c_adapter *i2c, const struct fc2580_config *cfg);
 #else
diff --git a/drivers/media/tuners/max2165.h b/drivers/media/tuners/max2165.h
index c063c36a93d..26e1dc64bb6 100644
--- a/drivers/media/tuners/max2165.h
+++ b/drivers/media/tuners/max2165.h
@@ -22,6 +22,8 @@
 #ifndef __MAX2165_H__
 #define __MAX2165_H__
 
+#include <linux/kconfig.h>
+
 struct dvb_frontend;
 struct i2c_adapter;
 
@@ -30,8 +32,7 @@ struct max2165_config {
 	u8 osc_clk; /* in MHz, selectable values: 4,16,18,20,22,24,26,28 */
 };
 
-#if defined(CONFIG_MEDIA_TUNER_MAX2165) || \
-    (defined(CONFIG_MEDIA_TUNER_MAX2165_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_MEDIA_TUNER_MAX2165)
 extern struct dvb_frontend *max2165_attach(struct dvb_frontend *fe,
 	struct i2c_adapter *i2c,
 	struct max2165_config *cfg);
diff --git a/drivers/media/tuners/mc44s803.h b/drivers/media/tuners/mc44s803.h
index 34f3892d3f6..9aae50aca2b 100644
--- a/drivers/media/tuners/mc44s803.h
+++ b/drivers/media/tuners/mc44s803.h
@@ -22,6 +22,8 @@
 #ifndef MC44S803_H
 #define MC44S803_H
 
+#include <linux/kconfig.h>
+
 struct dvb_frontend;
 struct i2c_adapter;
 
@@ -30,8 +32,7 @@ struct mc44s803_config {
 	u8 dig_out;
 };
 
-#if defined(CONFIG_MEDIA_TUNER_MC44S803) || \
-    (defined(CONFIG_MEDIA_TUNER_MC44S803_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_MEDIA_TUNER_MC44S803)
 extern struct dvb_frontend *mc44s803_attach(struct dvb_frontend *fe,
 	 struct i2c_adapter *i2c, struct mc44s803_config *cfg);
 #else
diff --git a/drivers/media/tuners/mxl5005s.h b/drivers/media/tuners/mxl5005s.h
index fc8a1ffc53b..ae8db885ad8 100644
--- a/drivers/media/tuners/mxl5005s.h
+++ b/drivers/media/tuners/mxl5005s.h
@@ -23,6 +23,8 @@
 #ifndef __MXL5005S_H
 #define __MXL5005S_H
 
+#include <linux/kconfig.h>
+
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
 
@@ -116,8 +118,7 @@ struct mxl5005s_config {
 	u8 AgcMasterByte;
 };
 
-#if defined(CONFIG_MEDIA_TUNER_MXL5005S) || \
-	(defined(CONFIG_MEDIA_TUNER_MXL5005S_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_MEDIA_TUNER_MXL5005S)
 extern struct dvb_frontend *mxl5005s_attach(struct dvb_frontend *fe,
 					    struct i2c_adapter *i2c,
 					    struct mxl5005s_config *config);
diff --git a/drivers/media/tuners/r820t.c b/drivers/media/tuners/r820t.c
new file mode 100644
index 00000000000..4835021aa3b
--- /dev/null
+++ b/drivers/media/tuners/r820t.c
@@ -0,0 +1,2355 @@
+/*
+ * Rafael Micro R820T driver
+ *
+ * Copyright (C) 2013 Mauro Carvalho Chehab <mchehab@redhat.com>
+ *
+ * This driver was written from scratch, based on an existing driver
+ * that it is part of rtl-sdr git tree, released under GPLv2:
+ *	https://groups.google.com/forum/#!topic/ultra-cheap-sdr/Y3rBEOFtHug
+ *	https://github.com/n1gp/gr-baz
+ *
+ * From what I understood from the threads, the original driver was converted
+ * to userspace from a Realtek tree. I couldn't find the original tree.
+ * However, the original driver look awkward on my eyes. So, I decided to
+ * write a new version from it from the scratch, while trying to reproduce
+ * everything found there.
+ *
+ * TODO:
+ *	After locking, the original driver seems to have some routines to
+ *		improve reception. This was not implemented here yet.
+ *
+ *	RF Gain set/get is not implemented.
+ *
+ *    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.
+ *
+ */
+
+#include <linux/videodev2.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <linux/bitrev.h>
+
+#include "tuner-i2c.h"
+#include "r820t.h"
+
+/*
+ * FIXME: I think that there are only 32 registers, but better safe than
+ *	  sorry. After finishing the driver, we may review it.
+ */
+#define REG_SHADOW_START	5
+#define NUM_REGS		27
+#define NUM_IMR			5
+#define IMR_TRIAL		9
+
+#define VER_NUM  49
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "enable verbose debug messages");
+
+static int no_imr_cal;
+module_param(no_imr_cal, int, 0444);
+MODULE_PARM_DESC(no_imr_cal, "Disable IMR calibration at module init");
+
+
+/*
+ * enums and structures
+ */
+
+enum xtal_cap_value {
+	XTAL_LOW_CAP_30P = 0,
+	XTAL_LOW_CAP_20P,
+	XTAL_LOW_CAP_10P,
+	XTAL_LOW_CAP_0P,
+	XTAL_HIGH_CAP_0P
+};
+
+struct r820t_sect_type {
+	u8	phase_y;
+	u8	gain_x;
+	u16	value;
+};
+
+struct r820t_priv {
+	struct list_head		hybrid_tuner_instance_list;
+	const struct r820t_config	*cfg;
+	struct tuner_i2c_props		i2c_props;
+	struct mutex			lock;
+
+	u8				regs[NUM_REGS];
+	u8				buf[NUM_REGS + 1];
+	enum xtal_cap_value		xtal_cap_sel;
+	u16				pll;	/* kHz */
+	u32				int_freq;
+	u8				fil_cal_code;
+	bool				imr_done;
+	bool				has_lock;
+	bool				init_done;
+	struct r820t_sect_type		imr_data[NUM_IMR];
+
+	/* Store current mode */
+	u32				delsys;
+	enum v4l2_tuner_type		type;
+	v4l2_std_id			std;
+	u32				bw;	/* in MHz */
+};
+
+struct r820t_freq_range {
+	u32	freq;
+	u8	open_d;
+	u8	rf_mux_ploy;
+	u8	tf_c;
+	u8	xtal_cap20p;
+	u8	xtal_cap10p;
+	u8	xtal_cap0p;
+	u8	imr_mem;		/* Not used, currently */
+};
+
+#define VCO_POWER_REF   0x02
+#define DIP_FREQ	32000000
+
+/*
+ * Static constants
+ */
+
+static LIST_HEAD(hybrid_tuner_instance_list);
+static DEFINE_MUTEX(r820t_list_mutex);
+
+/* Those initial values start from REG_SHADOW_START */
+static const u8 r820t_init_array[NUM_REGS] = {
+	0x83, 0x32, 0x75,			/* 05 to 07 */
+	0xc0, 0x40, 0xd6, 0x6c,			/* 08 to 0b */
+	0xf5, 0x63, 0x75, 0x68,			/* 0c to 0f */
+	0x6c, 0x83, 0x80, 0x00,			/* 10 to 13 */
+	0x0f, 0x00, 0xc0, 0x30,			/* 14 to 17 */
+	0x48, 0xcc, 0x60, 0x00,			/* 18 to 1b */
+	0x54, 0xae, 0x4a, 0xc0			/* 1c to 1f */
+};
+
+/* Tuner frequency ranges */
+static const struct r820t_freq_range freq_ranges[] = {
+	{
+		.freq = 0,
+		.open_d = 0x08,		/* low */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0xdf,		/* R27[7:0]  band2,band0 */
+		.xtal_cap20p = 0x02,	/* R16[1:0]  20pF (10)   */
+		.xtal_cap10p = 0x01,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 0,
+	}, {
+		.freq = 50,		/* Start freq, in MHz */
+		.open_d = 0x08,		/* low */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0xbe,		/* R27[7:0]  band4,band1  */
+		.xtal_cap20p = 0x02,	/* R16[1:0]  20pF (10)   */
+		.xtal_cap10p = 0x01,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 0,
+	}, {
+		.freq = 55,		/* Start freq, in MHz */
+		.open_d = 0x08,		/* low */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x8b,		/* R27[7:0]  band7,band4 */
+		.xtal_cap20p = 0x02,	/* R16[1:0]  20pF (10)   */
+		.xtal_cap10p = 0x01,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 0,
+	}, {
+		.freq = 60,		/* Start freq, in MHz */
+		.open_d = 0x08,		/* low */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x7b,		/* R27[7:0]  band8,band4 */
+		.xtal_cap20p = 0x02,	/* R16[1:0]  20pF (10)   */
+		.xtal_cap10p = 0x01,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 0,
+	}, {
+		.freq = 65,		/* Start freq, in MHz */
+		.open_d = 0x08,		/* low */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x69,		/* R27[7:0]  band9,band6 */
+		.xtal_cap20p = 0x02,	/* R16[1:0]  20pF (10)   */
+		.xtal_cap10p = 0x01,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 0,
+	}, {
+		.freq = 70,		/* Start freq, in MHz */
+		.open_d = 0x08,		/* low */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x58,		/* R27[7:0]  band10,band7 */
+		.xtal_cap20p = 0x02,	/* R16[1:0]  20pF (10)   */
+		.xtal_cap10p = 0x01,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 0,
+	}, {
+		.freq = 75,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x44,		/* R27[7:0]  band11,band11 */
+		.xtal_cap20p = 0x02,	/* R16[1:0]  20pF (10)   */
+		.xtal_cap10p = 0x01,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 0,
+	}, {
+		.freq = 80,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x44,		/* R27[7:0]  band11,band11 */
+		.xtal_cap20p = 0x02,	/* R16[1:0]  20pF (10)   */
+		.xtal_cap10p = 0x01,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 0,
+	}, {
+		.freq = 90,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x34,		/* R27[7:0]  band12,band11 */
+		.xtal_cap20p = 0x01,	/* R16[1:0]  10pF (01)   */
+		.xtal_cap10p = 0x01,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 0,
+	}, {
+		.freq = 100,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x34,		/* R27[7:0]  band12,band11 */
+		.xtal_cap20p = 0x01,	/* R16[1:0]  10pF (01)    */
+		.xtal_cap10p = 0x01,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 0,
+	}, {
+		.freq = 110,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x24,		/* R27[7:0]  band13,band11 */
+		.xtal_cap20p = 0x01,	/* R16[1:0]  10pF (01)   */
+		.xtal_cap10p = 0x01,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 1,
+	}, {
+		.freq = 120,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x24,		/* R27[7:0]  band13,band11 */
+		.xtal_cap20p = 0x01,	/* R16[1:0]  10pF (01)   */
+		.xtal_cap10p = 0x01,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 1,
+	}, {
+		.freq = 140,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x14,		/* R27[7:0]  band14,band11 */
+		.xtal_cap20p = 0x01,	/* R16[1:0]  10pF (01)   */
+		.xtal_cap10p = 0x01,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 1,
+	}, {
+		.freq = 180,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x13,		/* R27[7:0]  band14,band12 */
+		.xtal_cap20p = 0x00,	/* R16[1:0]  0pF (00)   */
+		.xtal_cap10p = 0x00,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 1,
+	}, {
+		.freq = 220,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x13,		/* R27[7:0]  band14,band12 */
+		.xtal_cap20p = 0x00,	/* R16[1:0]  0pF (00)   */
+		.xtal_cap10p = 0x00,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 2,
+	}, {
+		.freq = 250,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x11,		/* R27[7:0]  highest,highest */
+		.xtal_cap20p = 0x00,	/* R16[1:0]  0pF (00)   */
+		.xtal_cap10p = 0x00,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 2,
+	}, {
+		.freq = 280,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x00,		/* R27[7:0]  highest,highest */
+		.xtal_cap20p = 0x00,	/* R16[1:0]  0pF (00)   */
+		.xtal_cap10p = 0x00,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 2,
+	}, {
+		.freq = 310,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x41,	/* R26[7:6]=1 (bypass)  R26[1:0]=1 (middle) */
+		.tf_c = 0x00,		/* R27[7:0]  highest,highest */
+		.xtal_cap20p = 0x00,	/* R16[1:0]  0pF (00)   */
+		.xtal_cap10p = 0x00,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 2,
+	}, {
+		.freq = 450,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x41,	/* R26[7:6]=1 (bypass)  R26[1:0]=1 (middle) */
+		.tf_c = 0x00,		/* R27[7:0]  highest,highest */
+		.xtal_cap20p = 0x00,	/* R16[1:0]  0pF (00)   */
+		.xtal_cap10p = 0x00,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 3,
+	}, {
+		.freq = 588,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x40,	/* R26[7:6]=1 (bypass)  R26[1:0]=0 (highest) */
+		.tf_c = 0x00,		/* R27[7:0]  highest,highest */
+		.xtal_cap20p = 0x00,	/* R16[1:0]  0pF (00)   */
+		.xtal_cap10p = 0x00,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 3,
+	}, {
+		.freq = 650,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x40,	/* R26[7:6]=1 (bypass)  R26[1:0]=0 (highest) */
+		.tf_c = 0x00,		/* R27[7:0]  highest,highest */
+		.xtal_cap20p = 0x00,	/* R16[1:0]  0pF (00)   */
+		.xtal_cap10p = 0x00,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 4,
+	}
+};
+
+static int r820t_xtal_capacitor[][2] = {
+	{ 0x0b, XTAL_LOW_CAP_30P },
+	{ 0x02, XTAL_LOW_CAP_20P },
+	{ 0x01, XTAL_LOW_CAP_10P },
+	{ 0x00, XTAL_LOW_CAP_0P  },
+	{ 0x10, XTAL_HIGH_CAP_0P },
+};
+
+/*
+ * measured with a Racal 6103E GSM test set at 928 MHz with -60 dBm
+ * input power, for raw results see:
+ *	http://steve-m.de/projects/rtl-sdr/gain_measurement/r820t/
+ */
+
+static const int r820t_lna_gain_steps[]  = {
+	0, 9, 13, 40, 38, 13, 31, 22, 26, 31, 26, 14, 19, 5, 35, 13
+};
+
+static const int r820t_mixer_gain_steps[]  = {
+	0, 5, 10, 10, 19, 9, 10, 25, 17, 10, 8, 16, 13, 6, 3, -8
+};
+
+/*
+ * I2C read/write code and shadow registers logic
+ */
+static void shadow_store(struct r820t_priv *priv, u8 reg, const u8 *val,
+			 int len)
+{
+	int r = reg - REG_SHADOW_START;
+
+	if (r < 0) {
+		len += r;
+		r = 0;
+	}
+	if (len <= 0)
+		return;
+	if (len > NUM_REGS)
+		len = NUM_REGS;
+
+	tuner_dbg("%s: prev  reg=%02x len=%d: %*ph\n",
+		  __func__, r + REG_SHADOW_START, len, len, val);
+
+	memcpy(&priv->regs[r], val, len);
+}
+
+static int r820t_write(struct r820t_priv *priv, u8 reg, const u8 *val,
+		       int len)
+{
+	int rc, size, pos = 0;
+
+	/* Store the shadow registers */
+	shadow_store(priv, reg, val, len);
+
+	do {
+		if (len > priv->cfg->max_i2c_msg_len - 1)
+			size = priv->cfg->max_i2c_msg_len - 1;
+		else
+			size = len;
+
+		/* Fill I2C buffer */
+		priv->buf[0] = reg;
+		memcpy(&priv->buf[1], &val[pos], size);
+
+		rc = tuner_i2c_xfer_send(&priv->i2c_props, priv->buf, size + 1);
+		if (rc != size + 1) {
+			tuner_info("%s: i2c wr failed=%d reg=%02x len=%d: %*ph\n",
+				   __func__, rc, reg, size, size, &priv->buf[1]);
+			if (rc < 0)
+				return rc;
+			return -EREMOTEIO;
+		}
+		tuner_dbg("%s: i2c wr reg=%02x len=%d: %*ph\n",
+			  __func__, reg, size, size, &priv->buf[1]);
+
+		reg += size;
+		len -= size;
+		pos += size;
+	} while (len > 0);
+
+	return 0;
+}
+
+static int r820t_write_reg(struct r820t_priv *priv, u8 reg, u8 val)
+{
+	return r820t_write(priv, reg, &val, 1);
+}
+
+static int r820t_read_cache_reg(struct r820t_priv *priv, int reg)
+{
+	reg -= REG_SHADOW_START;
+
+	if (reg >= 0 && reg < NUM_REGS)
+		return priv->regs[reg];
+	else
+		return -EINVAL;
+}
+
+static int r820t_write_reg_mask(struct r820t_priv *priv, u8 reg, u8 val,
+				u8 bit_mask)
+{
+	int rc = r820t_read_cache_reg(priv, reg);
+
+	if (rc < 0)
+		return rc;
+
+	val = (rc & ~bit_mask) | (val & bit_mask);
+
+	return r820t_write(priv, reg, &val, 1);
+}
+
+static int r820t_read(struct r820t_priv *priv, u8 reg, u8 *val, int len)
+{
+	int rc, i;
+	u8 *p = &priv->buf[1];
+
+	priv->buf[0] = reg;
+
+	rc = tuner_i2c_xfer_send_recv(&priv->i2c_props, priv->buf, 1, p, len);
+	if (rc != len) {
+		tuner_info("%s: i2c rd failed=%d reg=%02x len=%d: %*ph\n",
+			   __func__, rc, reg, len, len, p);
+		if (rc < 0)
+			return rc;
+		return -EREMOTEIO;
+	}
+
+	/* Copy data to the output buffer */
+	for (i = 0; i < len; i++)
+		val[i] = bitrev8(p[i]);
+
+	tuner_dbg("%s: i2c rd reg=%02x len=%d: %*ph\n",
+		  __func__, reg, len, len, val);
+
+	return 0;
+}
+
+/*
+ * r820t tuning logic
+ */
+
+static int r820t_set_mux(struct r820t_priv *priv, u32 freq)
+{
+	const struct r820t_freq_range *range;
+	int i, rc;
+	u8 val, reg08, reg09;
+
+	/* Get the proper frequency range */
+	freq = freq / 1000000;
+	for (i = 0; i < ARRAY_SIZE(freq_ranges) - 1; i++) {
+		if (freq < freq_ranges[i + 1].freq)
+			break;
+	}
+	range = &freq_ranges[i];
+
+	tuner_dbg("set r820t range#%d for frequency %d MHz\n", i, freq);
+
+	/* Open Drain */
+	rc = r820t_write_reg_mask(priv, 0x17, range->open_d, 0x08);
+	if (rc < 0)
+		return rc;
+
+	/* RF_MUX,Polymux */
+	rc = r820t_write_reg_mask(priv, 0x1a, range->rf_mux_ploy, 0xc3);
+	if (rc < 0)
+		return rc;
+
+	/* TF BAND */
+	rc = r820t_write_reg(priv, 0x1b, range->tf_c);
+	if (rc < 0)
+		return rc;
+
+	/* XTAL CAP & Drive */
+	switch (priv->xtal_cap_sel) {
+	case XTAL_LOW_CAP_30P:
+	case XTAL_LOW_CAP_20P:
+		val = range->xtal_cap20p | 0x08;
+		break;
+	case XTAL_LOW_CAP_10P:
+		val = range->xtal_cap10p | 0x08;
+		break;
+	case XTAL_HIGH_CAP_0P:
+		val = range->xtal_cap0p | 0x00;
+		break;
+	default:
+	case XTAL_LOW_CAP_0P:
+		val = range->xtal_cap0p | 0x08;
+		break;
+	}
+	rc = r820t_write_reg_mask(priv, 0x10, val, 0x0b);
+	if (rc < 0)
+		return rc;
+
+	if (priv->imr_done) {
+		reg08 = priv->imr_data[range->imr_mem].gain_x;
+		reg09 = priv->imr_data[range->imr_mem].phase_y;
+	} else {
+		reg08 = 0;
+		reg09 = 0;
+	}
+	rc = r820t_write_reg_mask(priv, 0x08, reg08, 0x3f);
+	if (rc < 0)
+		return rc;
+
+	rc = r820t_write_reg_mask(priv, 0x09, reg09, 0x3f);
+
+	return rc;
+}
+
+static int r820t_set_pll(struct r820t_priv *priv, enum v4l2_tuner_type type,
+			 u32 freq)
+{
+	u32 vco_freq;
+	int rc, i;
+	unsigned sleep_time = 10000;
+	u32 vco_fra;		/* VCO contribution by SDM (kHz) */
+	u32 vco_min  = 1770000;
+	u32 vco_max  = vco_min * 2;
+	u32 pll_ref;
+	u16 n_sdm = 2;
+	u16 sdm = 0;
+	u8 mix_div = 2;
+	u8 div_buf = 0;
+	u8 div_num = 0;
+	u8 refdiv2 = 0;
+	u8 ni, si, nint, vco_fine_tune, val;
+	u8 data[5];
+
+	/* Frequency in kHz */
+	freq = freq / 1000;
+	pll_ref = priv->cfg->xtal / 1000;
+
+#if 0
+	/* Doesn't exist on rtl-sdk, and on field tests, caused troubles */
+	if ((priv->cfg->rafael_chip == CHIP_R620D) ||
+	   (priv->cfg->rafael_chip == CHIP_R828D) ||
+	   (priv->cfg->rafael_chip == CHIP_R828)) {
+		/* ref set refdiv2, reffreq = Xtal/2 on ATV application */
+		if (type != V4L2_TUNER_DIGITAL_TV) {
+			pll_ref /= 2;
+			refdiv2 = 0x10;
+			sleep_time = 20000;
+		}
+	} else {
+		if (priv->cfg->xtal > 24000000) {
+			pll_ref /= 2;
+			refdiv2 = 0x10;
+		}
+	}
+#endif
+
+	rc = r820t_write_reg_mask(priv, 0x10, refdiv2, 0x10);
+	if (rc < 0)
+		return rc;
+
+	/* set pll autotune = 128kHz */
+	rc = r820t_write_reg_mask(priv, 0x1a, 0x00, 0x0c);
+	if (rc < 0)
+		return rc;
+
+	/* set VCO current = 100 */
+	rc = r820t_write_reg_mask(priv, 0x12, 0x80, 0xe0);
+	if (rc < 0)
+		return rc;
+
+	/* Calculate divider */
+	while (mix_div <= 64) {
+		if (((freq * mix_div) >= vco_min) &&
+		   ((freq * mix_div) < vco_max)) {
+			div_buf = mix_div;
+			while (div_buf > 2) {
+				div_buf = div_buf >> 1;
+				div_num++;
+			}
+			break;
+		}
+		mix_div = mix_div << 1;
+	}
+
+	rc = r820t_read(priv, 0x00, data, sizeof(data));
+	if (rc < 0)
+		return rc;
+
+	vco_fine_tune = (data[4] & 0x30) >> 4;
+
+	if (vco_fine_tune > VCO_POWER_REF)
+		div_num = div_num - 1;
+	else if (vco_fine_tune < VCO_POWER_REF)
+		div_num = div_num + 1;
+
+	rc = r820t_write_reg_mask(priv, 0x10, div_num << 5, 0xe0);
+	if (rc < 0)
+		return rc;
+
+	vco_freq = freq * mix_div;
+	nint = vco_freq / (2 * pll_ref);
+	vco_fra = vco_freq - 2 * pll_ref * nint;
+
+	/* boundary spur prevention */
+	if (vco_fra < pll_ref / 64) {
+		vco_fra = 0;
+	} else if (vco_fra > pll_ref * 127 / 64) {
+		vco_fra = 0;
+		nint++;
+	} else if ((vco_fra > pll_ref * 127 / 128) && (vco_fra < pll_ref)) {
+		vco_fra = pll_ref * 127 / 128;
+	} else if ((vco_fra > pll_ref) && (vco_fra < pll_ref * 129 / 128)) {
+		vco_fra = pll_ref * 129 / 128;
+	}
+
+	if (nint > 63) {
+		tuner_info("No valid PLL values for %u kHz!\n", freq);
+		return -EINVAL;
+	}
+
+	ni = (nint - 13) / 4;
+	si = nint - 4 * ni - 13;
+
+	rc = r820t_write_reg(priv, 0x14, ni + (si << 6));
+	if (rc < 0)
+		return rc;
+
+	/* pw_sdm */
+	if (!vco_fra)
+		val = 0x08;
+	else
+		val = 0x00;
+
+	rc = r820t_write_reg_mask(priv, 0x12, val, 0x08);
+	if (rc < 0)
+		return rc;
+
+	/* sdm calculator */
+	while (vco_fra > 1) {
+		if (vco_fra > (2 * pll_ref / n_sdm)) {
+			sdm = sdm + 32768 / (n_sdm / 2);
+			vco_fra = vco_fra - 2 * pll_ref / n_sdm;
+			if (n_sdm >= 0x8000)
+				break;
+		}
+		n_sdm = n_sdm << 1;
+	}
+
+	tuner_dbg("freq %d kHz, pll ref %d%s, sdm=0x%04x\n",
+		  freq, pll_ref, refdiv2 ? " / 2" : "", sdm);
+
+	rc = r820t_write_reg(priv, 0x16, sdm >> 8);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg(priv, 0x15, sdm & 0xff);
+	if (rc < 0)
+		return rc;
+
+	for (i = 0; i < 2; i++) {
+		usleep_range(sleep_time, sleep_time + 1000);
+
+		/* Check if PLL has locked */
+		rc = r820t_read(priv, 0x00, data, 3);
+		if (rc < 0)
+			return rc;
+		if (data[2] & 0x40)
+			break;
+
+		if (!i) {
+			/* Didn't lock. Increase VCO current */
+			rc = r820t_write_reg_mask(priv, 0x12, 0x60, 0xe0);
+			if (rc < 0)
+				return rc;
+		}
+	}
+
+	if (!(data[2] & 0x40)) {
+		priv->has_lock = false;
+		return 0;
+	}
+
+	priv->has_lock = true;
+	tuner_dbg("tuner has lock at frequency %d kHz\n", freq);
+
+	/* set pll autotune = 8kHz */
+	rc = r820t_write_reg_mask(priv, 0x1a, 0x08, 0x08);
+
+	return rc;
+}
+
+static int r820t_sysfreq_sel(struct r820t_priv *priv, u32 freq,
+			     enum v4l2_tuner_type type,
+			     v4l2_std_id std,
+			     u32 delsys)
+{
+	int rc;
+	u8 mixer_top, lna_top, cp_cur, div_buf_cur, lna_vth_l, mixer_vth_l;
+	u8 air_cable1_in, cable2_in, pre_dect, lna_discharge, filter_cur;
+
+	tuner_dbg("adjusting tuner parameters for the standard\n");
+
+	switch (delsys) {
+	case SYS_DVBT:
+		if ((freq == 506000000) || (freq == 666000000) ||
+		   (freq == 818000000)) {
+			mixer_top = 0x14;	/* mixer top:14 , top-1, low-discharge */
+			lna_top = 0xe5;		/* detect bw 3, lna top:4, predet top:2 */
+			cp_cur = 0x28;		/* 101, 0.2 */
+			div_buf_cur = 0x20;	/* 10, 200u */
+		} else {
+			mixer_top = 0x24;	/* mixer top:13 , top-1, low-discharge */
+			lna_top = 0xe5;		/* detect bw 3, lna top:4, predet top:2 */
+			cp_cur = 0x38;		/* 111, auto */
+			div_buf_cur = 0x30;	/* 11, 150u */
+		}
+		lna_vth_l = 0x53;		/* lna vth 0.84	,  vtl 0.64 */
+		mixer_vth_l = 0x75;		/* mixer vth 1.04, vtl 0.84 */
+		air_cable1_in = 0x00;
+		cable2_in = 0x00;
+		pre_dect = 0x40;
+		lna_discharge = 14;
+		filter_cur = 0x40;		/* 10, low */
+		break;
+	case SYS_DVBT2:
+		mixer_top = 0x24;	/* mixer top:13 , top-1, low-discharge */
+		lna_top = 0xe5;		/* detect bw 3, lna top:4, predet top:2 */
+		lna_vth_l = 0x53;	/* lna vth 0.84	,  vtl 0.64 */
+		mixer_vth_l = 0x75;	/* mixer vth 1.04, vtl 0.84 */
+		air_cable1_in = 0x00;
+		cable2_in = 0x00;
+		pre_dect = 0x40;
+		lna_discharge = 14;
+		cp_cur = 0x38;		/* 111, auto */
+		div_buf_cur = 0x30;	/* 11, 150u */
+		filter_cur = 0x40;	/* 10, low */
+		break;
+	case SYS_ISDBT:
+		mixer_top = 0x24;	/* mixer top:13 , top-1, low-discharge */
+		lna_top = 0xe5;		/* detect bw 3, lna top:4, predet top:2 */
+		lna_vth_l = 0x75;	/* lna vth 1.04	,  vtl 0.84 */
+		mixer_vth_l = 0x75;	/* mixer vth 1.04, vtl 0.84 */
+		air_cable1_in = 0x00;
+		cable2_in = 0x00;
+		pre_dect = 0x40;
+		lna_discharge = 14;
+		cp_cur = 0x38;		/* 111, auto */
+		div_buf_cur = 0x30;	/* 11, 150u */
+		filter_cur = 0x40;	/* 10, low */
+		break;
+	default: /* DVB-T 8M */
+		mixer_top = 0x24;	/* mixer top:13 , top-1, low-discharge */
+		lna_top = 0xe5;		/* detect bw 3, lna top:4, predet top:2 */
+		lna_vth_l = 0x53;	/* lna vth 0.84	,  vtl 0.64 */
+		mixer_vth_l = 0x75;	/* mixer vth 1.04, vtl 0.84 */
+		air_cable1_in = 0x00;
+		cable2_in = 0x00;
+		pre_dect = 0x40;
+		lna_discharge = 14;
+		cp_cur = 0x38;		/* 111, auto */
+		div_buf_cur = 0x30;	/* 11, 150u */
+		filter_cur = 0x40;	/* 10, low */
+		break;
+	}
+
+	if (priv->cfg->use_diplexer &&
+	   ((priv->cfg->rafael_chip == CHIP_R820T) ||
+	   (priv->cfg->rafael_chip == CHIP_R828S) ||
+	   (priv->cfg->rafael_chip == CHIP_R820C))) {
+		if (freq > DIP_FREQ)
+			air_cable1_in = 0x00;
+		else
+			air_cable1_in = 0x60;
+		cable2_in = 0x00;
+	}
+
+
+	if (priv->cfg->use_predetect) {
+		rc = r820t_write_reg_mask(priv, 0x06, pre_dect, 0x40);
+		if (rc < 0)
+			return rc;
+	}
+
+	rc = r820t_write_reg_mask(priv, 0x1d, lna_top, 0xc7);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg_mask(priv, 0x1c, mixer_top, 0xf8);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg(priv, 0x0d, lna_vth_l);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg(priv, 0x0e, mixer_vth_l);
+	if (rc < 0)
+		return rc;
+
+	/* Air-IN only for Astrometa */
+	rc = r820t_write_reg_mask(priv, 0x05, air_cable1_in, 0x60);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg_mask(priv, 0x06, cable2_in, 0x08);
+	if (rc < 0)
+		return rc;
+
+	rc = r820t_write_reg_mask(priv, 0x11, cp_cur, 0x38);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg_mask(priv, 0x17, div_buf_cur, 0x30);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg_mask(priv, 0x0a, filter_cur, 0x60);
+	if (rc < 0)
+		return rc;
+	/*
+	 * Original driver initializes regs 0x05 and 0x06 with the
+	 * same value again on this point. Probably, it is just an
+	 * error there
+	 */
+
+	/*
+	 * Set LNA
+	 */
+
+	tuner_dbg("adjusting LNA parameters\n");
+	if (type != V4L2_TUNER_ANALOG_TV) {
+		/* LNA TOP: lowest */
+		rc = r820t_write_reg_mask(priv, 0x1d, 0, 0x38);
+		if (rc < 0)
+			return rc;
+
+		/* 0: normal mode */
+		rc = r820t_write_reg_mask(priv, 0x1c, 0, 0x04);
+		if (rc < 0)
+			return rc;
+
+		/* 0: PRE_DECT off */
+		rc = r820t_write_reg_mask(priv, 0x06, 0, 0x40);
+		if (rc < 0)
+			return rc;
+
+		/* agc clk 250hz */
+		rc = r820t_write_reg_mask(priv, 0x1a, 0x30, 0x30);
+		if (rc < 0)
+			return rc;
+
+		msleep(250);
+
+		/* write LNA TOP = 3 */
+		rc = r820t_write_reg_mask(priv, 0x1d, 0x18, 0x38);
+		if (rc < 0)
+			return rc;
+
+		/*
+		 * write discharge mode
+		 * FIXME: IMHO, the mask here is wrong, but it matches
+		 * what's there at the original driver
+		 */
+		rc = r820t_write_reg_mask(priv, 0x1c, mixer_top, 0x04);
+		if (rc < 0)
+			return rc;
+
+		/* LNA discharge current */
+		rc = r820t_write_reg_mask(priv, 0x1e, lna_discharge, 0x1f);
+		if (rc < 0)
+			return rc;
+
+		/* agc clk 60hz */
+		rc = r820t_write_reg_mask(priv, 0x1a, 0x20, 0x30);
+		if (rc < 0)
+			return rc;
+	} else {
+		/* PRE_DECT off */
+		rc = r820t_write_reg_mask(priv, 0x06, 0, 0x40);
+		if (rc < 0)
+			return rc;
+
+		/* write LNA TOP */
+		rc = r820t_write_reg_mask(priv, 0x1d, lna_top, 0x38);
+		if (rc < 0)
+			return rc;
+
+		/*
+		 * write discharge mode
+		 * FIXME: IMHO, the mask here is wrong, but it matches
+		 * what's there at the original driver
+		 */
+		rc = r820t_write_reg_mask(priv, 0x1c, mixer_top, 0x04);
+		if (rc < 0)
+			return rc;
+
+		/* LNA discharge current */
+		rc = r820t_write_reg_mask(priv, 0x1e, lna_discharge, 0x1f);
+		if (rc < 0)
+			return rc;
+
+		/* agc clk 1Khz, external det1 cap 1u */
+		rc = r820t_write_reg_mask(priv, 0x1a, 0x00, 0x30);
+		if (rc < 0)
+			return rc;
+
+		rc = r820t_write_reg_mask(priv, 0x10, 0x00, 0x04);
+		if (rc < 0)
+			return rc;
+	 }
+	 return 0;
+}
+
+static int r820t_set_tv_standard(struct r820t_priv *priv,
+				 unsigned bw,
+				 enum v4l2_tuner_type type,
+				 v4l2_std_id std, u32 delsys)
+
+{
+	int rc, i;
+	u32 if_khz, filt_cal_lo;
+	u8 data[5], val;
+	u8 filt_gain, img_r, filt_q, hp_cor, ext_enable, loop_through;
+	u8 lt_att, flt_ext_widest, polyfil_cur;
+	bool need_calibration;
+
+	tuner_dbg("selecting the delivery system\n");
+
+	if (delsys == SYS_ISDBT) {
+		if_khz = 4063;
+		filt_cal_lo = 59000;
+		filt_gain = 0x10;	/* +3db, 6mhz on */
+		img_r = 0x00;		/* image negative */
+		filt_q = 0x10;		/* r10[4]:low q(1'b1) */
+		hp_cor = 0x6a;		/* 1.7m disable, +2cap, 1.25mhz */
+		ext_enable = 0x40;	/* r30[6], ext enable; r30[5]:0 ext at lna max */
+		loop_through = 0x00;	/* r5[7], lt on */
+		lt_att = 0x00;		/* r31[7], lt att enable */
+		flt_ext_widest = 0x00;	/* r15[7]: flt_ext_wide off */
+		polyfil_cur = 0x60;	/* r25[6:5]:min */
+	} else {
+		if (bw <= 6) {
+			if_khz = 3570;
+			filt_cal_lo = 56000;	/* 52000->56000 */
+			filt_gain = 0x10;	/* +3db, 6mhz on */
+			img_r = 0x00;		/* image negative */
+			filt_q = 0x10;		/* r10[4]:low q(1'b1) */
+			hp_cor = 0x6b;		/* 1.7m disable, +2cap, 1.0mhz */
+			ext_enable = 0x60;	/* r30[6]=1 ext enable; r30[5]:1 ext at lna max-1 */
+			loop_through = 0x00;	/* r5[7], lt on */
+			lt_att = 0x00;		/* r31[7], lt att enable */
+			flt_ext_widest = 0x00;	/* r15[7]: flt_ext_wide off */
+			polyfil_cur = 0x60;	/* r25[6:5]:min */
+		} else if (bw == 7) {
+#if 0
+			/*
+			 * There are two 7 MHz tables defined on the original
+			 * driver, but just the second one seems to be visible
+			 * by rtl2832. Keep this one here commented, as it
+			 * might be needed in the future
+			 */
+
+			if_khz = 4070;
+			filt_cal_lo = 60000;
+			filt_gain = 0x10;	/* +3db, 6mhz on */
+			img_r = 0x00;		/* image negative */
+			filt_q = 0x10;		/* r10[4]:low q(1'b1) */
+			hp_cor = 0x2b;		/* 1.7m disable, +1cap, 1.0mhz */
+			ext_enable = 0x60;	/* r30[6]=1 ext enable; r30[5]:1 ext at lna max-1 */
+			loop_through = 0x00;	/* r5[7], lt on */
+			lt_att = 0x00;		/* r31[7], lt att enable */
+			flt_ext_widest = 0x00;	/* r15[7]: flt_ext_wide off */
+			polyfil_cur = 0x60;	/* r25[6:5]:min */
+#endif
+			/* 7 MHz, second table */
+			if_khz = 4570;
+			filt_cal_lo = 63000;
+			filt_gain = 0x10;	/* +3db, 6mhz on */
+			img_r = 0x00;		/* image negative */
+			filt_q = 0x10;		/* r10[4]:low q(1'b1) */
+			hp_cor = 0x2a;		/* 1.7m disable, +1cap, 1.25mhz */
+			ext_enable = 0x60;	/* r30[6]=1 ext enable; r30[5]:1 ext at lna max-1 */
+			loop_through = 0x00;	/* r5[7], lt on */
+			lt_att = 0x00;		/* r31[7], lt att enable */
+			flt_ext_widest = 0x00;	/* r15[7]: flt_ext_wide off */
+			polyfil_cur = 0x60;	/* r25[6:5]:min */
+		} else {
+			if_khz = 4570;
+			filt_cal_lo = 68500;
+			filt_gain = 0x10;	/* +3db, 6mhz on */
+			img_r = 0x00;		/* image negative */
+			filt_q = 0x10;		/* r10[4]:low q(1'b1) */
+			hp_cor = 0x0b;		/* 1.7m disable, +0cap, 1.0mhz */
+			ext_enable = 0x60;	/* r30[6]=1 ext enable; r30[5]:1 ext at lna max-1 */
+			loop_through = 0x00;	/* r5[7], lt on */
+			lt_att = 0x00;		/* r31[7], lt att enable */
+			flt_ext_widest = 0x00;	/* r15[7]: flt_ext_wide off */
+			polyfil_cur = 0x60;	/* r25[6:5]:min */
+		}
+	}
+
+	/* Initialize the shadow registers */
+	memcpy(priv->regs, r820t_init_array, sizeof(r820t_init_array));
+
+	/* Init Flag & Xtal_check Result */
+	if (priv->imr_done)
+		val = 1 | priv->xtal_cap_sel << 1;
+	else
+		val = 0;
+	rc = r820t_write_reg_mask(priv, 0x0c, val, 0x0f);
+	if (rc < 0)
+		return rc;
+
+	/* version */
+	rc = r820t_write_reg_mask(priv, 0x13, VER_NUM, 0x3f);
+	if (rc < 0)
+		return rc;
+
+	/* for LT Gain test */
+	if (type != V4L2_TUNER_ANALOG_TV) {
+		rc = r820t_write_reg_mask(priv, 0x1d, 0x00, 0x38);
+		if (rc < 0)
+			return rc;
+		usleep_range(1000, 2000);
+	}
+	priv->int_freq = if_khz * 1000;
+
+	/* Check if standard changed. If so, filter calibration is needed */
+	if (type != priv->type)
+		need_calibration = true;
+	else if ((type == V4L2_TUNER_ANALOG_TV) && (std != priv->std))
+		need_calibration = true;
+	else if ((type == V4L2_TUNER_DIGITAL_TV) &&
+		 ((delsys != priv->delsys) || bw != priv->bw))
+		need_calibration = true;
+	else
+		need_calibration = false;
+
+	if (need_calibration) {
+		tuner_dbg("calibrating the tuner\n");
+		for (i = 0; i < 2; i++) {
+			/* Set filt_cap */
+			rc = r820t_write_reg_mask(priv, 0x0b, hp_cor, 0x60);
+			if (rc < 0)
+				return rc;
+
+			/* set cali clk =on */
+			rc = r820t_write_reg_mask(priv, 0x0f, 0x04, 0x04);
+			if (rc < 0)
+				return rc;
+
+			/* X'tal cap 0pF for PLL */
+			rc = r820t_write_reg_mask(priv, 0x10, 0x00, 0x03);
+			if (rc < 0)
+				return rc;
+
+			rc = r820t_set_pll(priv, type, filt_cal_lo * 1000);
+			if (rc < 0 || !priv->has_lock)
+				return rc;
+
+			/* Start Trigger */
+			rc = r820t_write_reg_mask(priv, 0x0b, 0x10, 0x10);
+			if (rc < 0)
+				return rc;
+
+			usleep_range(1000, 2000);
+
+			/* Stop Trigger */
+			rc = r820t_write_reg_mask(priv, 0x0b, 0x00, 0x10);
+			if (rc < 0)
+				return rc;
+
+			/* set cali clk =off */
+			rc = r820t_write_reg_mask(priv, 0x0f, 0x00, 0x04);
+			if (rc < 0)
+				return rc;
+
+			/* Check if calibration worked */
+			rc = r820t_read(priv, 0x00, data, sizeof(data));
+			if (rc < 0)
+				return rc;
+
+			priv->fil_cal_code = data[4] & 0x0f;
+			if (priv->fil_cal_code && priv->fil_cal_code != 0x0f)
+				break;
+		}
+		/* narrowest */
+		if (priv->fil_cal_code == 0x0f)
+			priv->fil_cal_code = 0;
+	}
+
+	rc = r820t_write_reg_mask(priv, 0x0a,
+				  filt_q | priv->fil_cal_code, 0x1f);
+	if (rc < 0)
+		return rc;
+
+	/* Set BW, Filter_gain, & HP corner */
+	rc = r820t_write_reg_mask(priv, 0x0b, hp_cor, 0xef);
+	if (rc < 0)
+		return rc;
+
+
+	/* Set Img_R */
+	rc = r820t_write_reg_mask(priv, 0x07, img_r, 0x80);
+	if (rc < 0)
+		return rc;
+
+	/* Set filt_3dB, V6MHz */
+	rc = r820t_write_reg_mask(priv, 0x06, filt_gain, 0x30);
+	if (rc < 0)
+		return rc;
+
+	/* channel filter extension */
+	rc = r820t_write_reg_mask(priv, 0x1e, ext_enable, 0x60);
+	if (rc < 0)
+		return rc;
+
+	/* Loop through */
+	rc = r820t_write_reg_mask(priv, 0x05, loop_through, 0x80);
+	if (rc < 0)
+		return rc;
+
+	/* Loop through attenuation */
+	rc = r820t_write_reg_mask(priv, 0x1f, lt_att, 0x80);
+	if (rc < 0)
+		return rc;
+
+	/* filter extension widest */
+	rc = r820t_write_reg_mask(priv, 0x0f, flt_ext_widest, 0x80);
+	if (rc < 0)
+		return rc;
+
+	/* RF poly filter current */
+	rc = r820t_write_reg_mask(priv, 0x19, polyfil_cur, 0x60);
+	if (rc < 0)
+		return rc;
+
+	/* Store current standard. If it changes, re-calibrate the tuner */
+	priv->delsys = delsys;
+	priv->type = type;
+	priv->std = std;
+	priv->bw = bw;
+
+	return 0;
+}
+
+static int r820t_read_gain(struct r820t_priv *priv)
+{
+	u8 data[4];
+	int rc;
+
+	rc = r820t_read(priv, 0x00, data, sizeof(data));
+	if (rc < 0)
+		return rc;
+
+	return ((data[3] & 0x0f) << 1) + ((data[3] & 0xf0) >> 4);
+}
+
+#if 0
+/* FIXME: This routine requires more testing */
+static int r820t_set_gain_mode(struct r820t_priv *priv,
+			       bool set_manual_gain,
+			       int gain)
+{
+	int rc;
+
+	if (set_manual_gain) {
+		int i, total_gain = 0;
+		uint8_t mix_index = 0, lna_index = 0;
+		u8 data[4];
+
+		/* LNA auto off */
+		rc = r820t_write_reg_mask(priv, 0x05, 0x10, 0x10);
+		if (rc < 0)
+			return rc;
+
+		 /* Mixer auto off */
+		rc = r820t_write_reg_mask(priv, 0x07, 0, 0x10);
+		if (rc < 0)
+			return rc;
+
+		rc = r820t_read(priv, 0x00, data, sizeof(data));
+		if (rc < 0)
+			return rc;
+
+		/* set fixed VGA gain for now (16.3 dB) */
+		rc = r820t_write_reg_mask(priv, 0x0c, 0x08, 0x9f);
+		if (rc < 0)
+			return rc;
+
+		for (i = 0; i < 15; i++) {
+			if (total_gain >= gain)
+				break;
+
+			total_gain += r820t_lna_gain_steps[++lna_index];
+
+			if (total_gain >= gain)
+				break;
+
+			total_gain += r820t_mixer_gain_steps[++mix_index];
+		}
+
+		/* set LNA gain */
+		rc = r820t_write_reg_mask(priv, 0x05, lna_index, 0x0f);
+		if (rc < 0)
+			return rc;
+
+		/* set Mixer gain */
+		rc = r820t_write_reg_mask(priv, 0x07, mix_index, 0x0f);
+		if (rc < 0)
+			return rc;
+	} else {
+		/* LNA */
+		rc = r820t_write_reg_mask(priv, 0x05, 0, 0x10);
+		if (rc < 0)
+			return rc;
+
+		/* Mixer */
+		rc = r820t_write_reg_mask(priv, 0x07, 0x10, 0x10);
+		if (rc < 0)
+			return rc;
+
+		/* set fixed VGA gain for now (26.5 dB) */
+		rc = r820t_write_reg_mask(priv, 0x0c, 0x0b, 0x9f);
+		if (rc < 0)
+			return rc;
+	}
+
+	return 0;
+}
+#endif
+
+static int generic_set_freq(struct dvb_frontend *fe,
+			    u32 freq /* in HZ */,
+			    unsigned bw,
+			    enum v4l2_tuner_type type,
+			    v4l2_std_id std, u32 delsys)
+{
+	struct r820t_priv		*priv = fe->tuner_priv;
+	int				rc = -EINVAL;
+	u32				lo_freq;
+
+	tuner_dbg("should set frequency to %d kHz, bw %d MHz\n",
+		  freq / 1000, bw);
+
+	rc = r820t_set_tv_standard(priv, bw, type, std, delsys);
+	if (rc < 0)
+		goto err;
+
+	if ((type == V4L2_TUNER_ANALOG_TV) && (std == V4L2_STD_SECAM_LC))
+		lo_freq = freq - priv->int_freq;
+	 else
+		lo_freq = freq + priv->int_freq;
+
+	rc = r820t_set_mux(priv, lo_freq);
+	if (rc < 0)
+		goto err;
+
+	rc = r820t_set_pll(priv, type, lo_freq);
+	if (rc < 0 || !priv->has_lock)
+		goto err;
+
+	rc = r820t_sysfreq_sel(priv, freq, type, std, delsys);
+	if (rc < 0)
+		goto err;
+
+	tuner_dbg("%s: PLL locked on frequency %d Hz, gain=%d\n",
+		  __func__, freq, r820t_read_gain(priv));
+
+err:
+
+	if (rc < 0)
+		tuner_dbg("%s: failed=%d\n", __func__, rc);
+	return rc;
+}
+
+/*
+ * r820t standby logic
+ */
+
+static int r820t_standby(struct r820t_priv *priv)
+{
+	int rc;
+
+	/* If device was not initialized yet, don't need to standby */
+	if (!priv->init_done)
+		return 0;
+
+	rc = r820t_write_reg(priv, 0x06, 0xb1);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg(priv, 0x05, 0x03);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg(priv, 0x07, 0x3a);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg(priv, 0x08, 0x40);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg(priv, 0x09, 0xc0);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg(priv, 0x0a, 0x36);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg(priv, 0x0c, 0x35);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg(priv, 0x0f, 0x68);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg(priv, 0x11, 0x03);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg(priv, 0x17, 0xf4);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg(priv, 0x19, 0x0c);
+
+	/* Force initial calibration */
+	priv->type = -1;
+
+	return rc;
+}
+
+/*
+ * r820t device init logic
+ */
+
+static int r820t_xtal_check(struct r820t_priv *priv)
+{
+	int rc, i;
+	u8 data[3], val;
+
+	/* Initialize the shadow registers */
+	memcpy(priv->regs, r820t_init_array, sizeof(r820t_init_array));
+
+	/* cap 30pF & Drive Low */
+	rc = r820t_write_reg_mask(priv, 0x10, 0x0b, 0x0b);
+	if (rc < 0)
+		return rc;
+
+	/* set pll autotune = 128kHz */
+	rc = r820t_write_reg_mask(priv, 0x1a, 0x00, 0x0c);
+	if (rc < 0)
+		return rc;
+
+	/* set manual initial reg = 111111;  */
+	rc = r820t_write_reg_mask(priv, 0x13, 0x7f, 0x7f);
+	if (rc < 0)
+		return rc;
+
+	/* set auto */
+	rc = r820t_write_reg_mask(priv, 0x13, 0x00, 0x40);
+	if (rc < 0)
+		return rc;
+
+	/* Try several xtal capacitor alternatives */
+	for (i = 0; i < ARRAY_SIZE(r820t_xtal_capacitor); i++) {
+		rc = r820t_write_reg_mask(priv, 0x10,
+					  r820t_xtal_capacitor[i][0], 0x1b);
+		if (rc < 0)
+			return rc;
+
+		usleep_range(5000, 6000);
+
+		rc = r820t_read(priv, 0x00, data, sizeof(data));
+		if (rc < 0)
+			return rc;
+		if (!(data[2] & 0x40))
+			continue;
+
+		val = data[2] & 0x3f;
+
+		if (priv->cfg->xtal == 16000000 && (val > 29 || val < 23))
+			break;
+
+		if (val != 0x3f)
+			break;
+	}
+
+	if (i == ARRAY_SIZE(r820t_xtal_capacitor))
+		return -EINVAL;
+
+	return r820t_xtal_capacitor[i][1];
+}
+
+static int r820t_imr_prepare(struct r820t_priv *priv)
+{
+	int rc;
+
+	/* Initialize the shadow registers */
+	memcpy(priv->regs, r820t_init_array, sizeof(r820t_init_array));
+
+	/* lna off (air-in off) */
+	rc = r820t_write_reg_mask(priv, 0x05, 0x20, 0x20);
+	if (rc < 0)
+		return rc;
+
+	/* mixer gain mode = manual */
+	rc = r820t_write_reg_mask(priv, 0x07, 0, 0x10);
+	if (rc < 0)
+		return rc;
+
+	/* filter corner = lowest */
+	rc = r820t_write_reg_mask(priv, 0x0a, 0x0f, 0x0f);
+	if (rc < 0)
+		return rc;
+
+	/* filter bw=+2cap, hp=5M */
+	rc = r820t_write_reg_mask(priv, 0x0b, 0x60, 0x6f);
+	if (rc < 0)
+		return rc;
+
+	/* adc=on, vga code mode, gain = 26.5dB   */
+	rc = r820t_write_reg_mask(priv, 0x0c, 0x0b, 0x9f);
+	if (rc < 0)
+		return rc;
+
+	/* ring clk = on */
+	rc = r820t_write_reg_mask(priv, 0x0f, 0, 0x08);
+	if (rc < 0)
+		return rc;
+
+	/* ring power = on */
+	rc = r820t_write_reg_mask(priv, 0x18, 0x10, 0x10);
+	if (rc < 0)
+		return rc;
+
+	/* from ring = ring pll in */
+	rc = r820t_write_reg_mask(priv, 0x1c, 0x02, 0x02);
+	if (rc < 0)
+		return rc;
+
+	/* sw_pdect = det3 */
+	rc = r820t_write_reg_mask(priv, 0x1e, 0x80, 0x80);
+	if (rc < 0)
+		return rc;
+
+	/* Set filt_3dB */
+	rc = r820t_write_reg_mask(priv, 0x06, 0x20, 0x20);
+
+	return rc;
+}
+
+static int r820t_multi_read(struct r820t_priv *priv)
+{
+	int rc, i;
+	u8 data[2], min = 0, max = 255, sum = 0;
+
+	usleep_range(5000, 6000);
+
+	for (i = 0; i < 6; i++) {
+		rc = r820t_read(priv, 0x00, data, sizeof(data));
+		if (rc < 0)
+			return rc;
+
+		sum += data[1];
+
+		if (data[1] < min)
+			min = data[1];
+
+		if (data[1] > max)
+			max = data[1];
+	}
+	rc = sum - max - min;
+
+	return rc;
+}
+
+static int r820t_imr_cross(struct r820t_priv *priv,
+			   struct r820t_sect_type iq_point[3],
+			   u8 *x_direct)
+{
+	struct r820t_sect_type cross[5]; /* (0,0)(0,Q-1)(0,I-1)(Q-1,0)(I-1,0) */
+	struct r820t_sect_type tmp;
+	int i, rc;
+	u8 reg08, reg09;
+
+	reg08 = r820t_read_cache_reg(priv, 8) & 0xc0;
+	reg09 = r820t_read_cache_reg(priv, 9) & 0xc0;
+
+	tmp.gain_x = 0;
+	tmp.phase_y = 0;
+	tmp.value = 255;
+
+	for (i = 0; i < 5; i++) {
+		switch (i) {
+		case 0:
+			cross[i].gain_x  = reg08;
+			cross[i].phase_y = reg09;
+			break;
+		case 1:
+			cross[i].gain_x  = reg08;		/* 0 */
+			cross[i].phase_y = reg09 + 1;		/* Q-1 */
+			break;
+		case 2:
+			cross[i].gain_x  = reg08;		/* 0 */
+			cross[i].phase_y = (reg09 | 0x20) + 1;	/* I-1 */
+			break;
+		case 3:
+			cross[i].gain_x  = reg08 + 1;		/* Q-1 */
+			cross[i].phase_y = reg09;
+			break;
+		default:
+			cross[i].gain_x  = (reg08 | 0x20) + 1;	/* I-1 */
+			cross[i].phase_y = reg09;
+		}
+
+		rc = r820t_write_reg(priv, 0x08, cross[i].gain_x);
+		if (rc < 0)
+			return rc;
+
+		rc = r820t_write_reg(priv, 0x09, cross[i].phase_y);
+		if (rc < 0)
+			return rc;
+
+		rc = r820t_multi_read(priv);
+		if (rc < 0)
+			return rc;
+
+		cross[i].value = rc;
+
+		if (cross[i].value < tmp.value)
+			memcpy(&tmp, &cross[i], sizeof(tmp));
+	}
+
+	if ((tmp.phase_y & 0x1f) == 1) {	/* y-direction */
+		*x_direct = 0;
+
+		iq_point[0] = cross[0];
+		iq_point[1] = cross[1];
+		iq_point[2] = cross[2];
+	} else {				/* (0,0) or x-direction */
+		*x_direct = 1;
+
+		iq_point[0] = cross[0];
+		iq_point[1] = cross[3];
+		iq_point[2] = cross[4];
+	}
+	return 0;
+}
+
+static void r820t_compre_cor(struct r820t_sect_type iq[3])
+{
+	int i;
+
+	for (i = 3; i > 0; i--) {
+		if (iq[0].value > iq[i - 1].value)
+			swap(iq[0], iq[i - 1]);
+	}
+}
+
+static int r820t_compre_step(struct r820t_priv *priv,
+			     struct r820t_sect_type iq[3], u8 reg)
+{
+	int rc;
+	struct r820t_sect_type tmp;
+
+	/*
+	 * Purpose: if (Gain<9 or Phase<9), Gain+1 or Phase+1 and compare
+	 * with min value:
+	 *  new < min => update to min and continue
+	 *  new > min => Exit
+	 */
+
+	/* min value already saved in iq[0] */
+	tmp.phase_y = iq[0].phase_y;
+	tmp.gain_x  = iq[0].gain_x;
+
+	while (((tmp.gain_x & 0x1f) < IMR_TRIAL) &&
+	      ((tmp.phase_y & 0x1f) < IMR_TRIAL)) {
+		if (reg == 0x08)
+			tmp.gain_x++;
+		else
+			tmp.phase_y++;
+
+		rc = r820t_write_reg(priv, 0x08, tmp.gain_x);
+		if (rc < 0)
+			return rc;
+
+		rc = r820t_write_reg(priv, 0x09, tmp.phase_y);
+		if (rc < 0)
+			return rc;
+
+		rc = r820t_multi_read(priv);
+		if (rc < 0)
+			return rc;
+		tmp.value = rc;
+
+		if (tmp.value <= iq[0].value) {
+			iq[0].gain_x  = tmp.gain_x;
+			iq[0].phase_y = tmp.phase_y;
+			iq[0].value   = tmp.value;
+		} else {
+			return 0;
+		}
+
+	}
+
+	return 0;
+}
+
+static int r820t_iq_tree(struct r820t_priv *priv,
+			 struct r820t_sect_type iq[3],
+			 u8 fix_val, u8 var_val, u8 fix_reg)
+{
+	int rc, i;
+	u8 tmp, var_reg;
+
+	/*
+	 * record IMC results by input gain/phase location then adjust
+	 * gain or phase positive 1 step and negtive 1 step,
+	 * both record results
+	 */
+
+	if (fix_reg == 0x08)
+		var_reg = 0x09;
+	else
+		var_reg = 0x08;
+
+	for (i = 0; i < 3; i++) {
+		rc = r820t_write_reg(priv, fix_reg, fix_val);
+		if (rc < 0)
+			return rc;
+
+		rc = r820t_write_reg(priv, var_reg, var_val);
+		if (rc < 0)
+			return rc;
+
+		rc = r820t_multi_read(priv);
+		if (rc < 0)
+			return rc;
+		iq[i].value = rc;
+
+		if (fix_reg == 0x08) {
+			iq[i].gain_x  = fix_val;
+			iq[i].phase_y = var_val;
+		} else {
+			iq[i].phase_y = fix_val;
+			iq[i].gain_x  = var_val;
+		}
+
+		if (i == 0) {  /* try right-side point */
+			var_val++;
+		} else if (i == 1) { /* try left-side point */
+			 /* if absolute location is 1, change I/Q direction */
+			if ((var_val & 0x1f) < 0x02) {
+				tmp = 2 - (var_val & 0x1f);
+
+				/* b[5]:I/Q selection. 0:Q-path, 1:I-path */
+				if (var_val & 0x20) {
+					var_val &= 0xc0;
+					var_val |= tmp;
+				} else {
+					var_val |= 0x20 | tmp;
+				}
+			} else {
+				var_val -= 2;
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int r820t_section(struct r820t_priv *priv,
+			 struct r820t_sect_type *iq_point)
+{
+	int rc;
+	struct r820t_sect_type compare_iq[3], compare_bet[3];
+
+	/* Try X-1 column and save min result to compare_bet[0] */
+	if (!(iq_point->gain_x & 0x1f))
+		compare_iq[0].gain_x = ((iq_point->gain_x) & 0xdf) + 1;  /* Q-path, Gain=1 */
+	else
+		compare_iq[0].gain_x  = iq_point->gain_x - 1;  /* left point */
+	compare_iq[0].phase_y = iq_point->phase_y;
+
+	/* y-direction */
+	rc = r820t_iq_tree(priv, compare_iq,  compare_iq[0].gain_x,
+			compare_iq[0].phase_y, 0x08);
+	if (rc < 0)
+		return rc;
+
+	r820t_compre_cor(compare_iq);
+
+	compare_bet[0] = compare_iq[0];
+
+	/* Try X column and save min result to compare_bet[1] */
+	compare_iq[0].gain_x  = iq_point->gain_x;
+	compare_iq[0].phase_y = iq_point->phase_y;
+
+	rc = r820t_iq_tree(priv, compare_iq,  compare_iq[0].gain_x,
+			   compare_iq[0].phase_y, 0x08);
+	if (rc < 0)
+		return rc;
+
+	r820t_compre_cor(compare_iq);
+
+	compare_bet[1] = compare_iq[0];
+
+	/* Try X+1 column and save min result to compare_bet[2] */
+	if ((iq_point->gain_x & 0x1f) == 0x00)
+		compare_iq[0].gain_x = ((iq_point->gain_x) | 0x20) + 1;  /* I-path, Gain=1 */
+	else
+		compare_iq[0].gain_x = iq_point->gain_x + 1;
+	compare_iq[0].phase_y = iq_point->phase_y;
+
+	rc = r820t_iq_tree(priv, compare_iq,  compare_iq[0].gain_x,
+			   compare_iq[0].phase_y, 0x08);
+	if (rc < 0)
+		return rc;
+
+	r820t_compre_cor(compare_iq);
+
+	compare_bet[2] = compare_iq[0];
+
+	r820t_compre_cor(compare_bet);
+
+	*iq_point = compare_bet[0];
+
+	return 0;
+}
+
+static int r820t_vga_adjust(struct r820t_priv *priv)
+{
+	int rc;
+	u8 vga_count;
+
+	/* increase vga power to let image significant */
+	for (vga_count = 12; vga_count < 16; vga_count++) {
+		rc = r820t_write_reg_mask(priv, 0x0c, vga_count, 0x0f);
+		if (rc < 0)
+			return rc;
+
+		usleep_range(10000, 11000);
+
+		rc = r820t_multi_read(priv);
+		if (rc < 0)
+			return rc;
+
+		if (rc > 40 * 4)
+			break;
+	}
+
+	return 0;
+}
+
+static int r820t_iq(struct r820t_priv *priv, struct r820t_sect_type *iq_pont)
+{
+	struct r820t_sect_type compare_iq[3];
+	int rc;
+	u8 x_direction = 0;  /* 1:x, 0:y */
+	u8 dir_reg, other_reg;
+
+	r820t_vga_adjust(priv);
+
+	rc = r820t_imr_cross(priv, compare_iq, &x_direction);
+	if (rc < 0)
+		return rc;
+
+	if (x_direction == 1) {
+		dir_reg   = 0x08;
+		other_reg = 0x09;
+	} else {
+		dir_reg   = 0x09;
+		other_reg = 0x08;
+	}
+
+	/* compare and find min of 3 points. determine i/q direction */
+	r820t_compre_cor(compare_iq);
+
+	/* increase step to find min value of this direction */
+	rc = r820t_compre_step(priv, compare_iq, dir_reg);
+	if (rc < 0)
+		return rc;
+
+	/* the other direction */
+	rc = r820t_iq_tree(priv, compare_iq,  compare_iq[0].gain_x,
+				compare_iq[0].phase_y, dir_reg);
+	if (rc < 0)
+		return rc;
+
+	/* compare and find min of 3 points. determine i/q direction */
+	r820t_compre_cor(compare_iq);
+
+	/* increase step to find min value on this direction */
+	rc = r820t_compre_step(priv, compare_iq, other_reg);
+	if (rc < 0)
+		return rc;
+
+	/* check 3 points again */
+	rc = r820t_iq_tree(priv, compare_iq,  compare_iq[0].gain_x,
+				compare_iq[0].phase_y, other_reg);
+	if (rc < 0)
+		return rc;
+
+	r820t_compre_cor(compare_iq);
+
+	/* section-9 check */
+	rc = r820t_section(priv, compare_iq);
+
+	*iq_pont = compare_iq[0];
+
+	/* reset gain/phase control setting */
+	rc = r820t_write_reg_mask(priv, 0x08, 0, 0x3f);
+	if (rc < 0)
+		return rc;
+
+	rc = r820t_write_reg_mask(priv, 0x09, 0, 0x3f);
+
+	return rc;
+}
+
+static int r820t_f_imr(struct r820t_priv *priv, struct r820t_sect_type *iq_pont)
+{
+	int rc;
+
+	r820t_vga_adjust(priv);
+
+	/*
+	 * search surrounding points from previous point
+	 * try (x-1), (x), (x+1) columns, and find min IMR result point
+	 */
+	rc = r820t_section(priv, iq_pont);
+	if (rc < 0)
+		return rc;
+
+	return 0;
+}
+
+static int r820t_imr(struct r820t_priv *priv, unsigned imr_mem, bool im_flag)
+{
+	struct r820t_sect_type imr_point;
+	int rc;
+	u32 ring_vco, ring_freq, ring_ref;
+	u8 n_ring, n;
+	int reg18, reg19, reg1f;
+
+	if (priv->cfg->xtal > 24000000)
+		ring_ref = priv->cfg->xtal / 2;
+	else
+		ring_ref = priv->cfg->xtal;
+
+	n_ring = 15;
+	for (n = 0; n < 16; n++) {
+		if ((16 + n) * 8 * ring_ref >= 3100000) {
+			n_ring = n;
+			break;
+		}
+	}
+
+	reg18 = r820t_read_cache_reg(priv, 0x18);
+	reg19 = r820t_read_cache_reg(priv, 0x19);
+	reg1f = r820t_read_cache_reg(priv, 0x1f);
+
+	reg18 &= 0xf0;      /* set ring[3:0] */
+	reg18 |= n_ring;
+
+	ring_vco = (16 + n_ring) * 8 * ring_ref;
+
+	reg18 &= 0xdf;   /* clear ring_se23 */
+	reg19 &= 0xfc;   /* clear ring_seldiv */
+	reg1f &= 0xfc;   /* clear ring_att */
+
+	switch (imr_mem) {
+	case 0:
+		ring_freq = ring_vco / 48;
+		reg18 |= 0x20;  /* ring_se23 = 1 */
+		reg19 |= 0x03;  /* ring_seldiv = 3 */
+		reg1f |= 0x02;  /* ring_att 10 */
+		break;
+	case 1:
+		ring_freq = ring_vco / 16;
+		reg18 |= 0x00;  /* ring_se23 = 0 */
+		reg19 |= 0x02;  /* ring_seldiv = 2 */
+		reg1f |= 0x00;  /* pw_ring 00 */
+		break;
+	case 2:
+		ring_freq = ring_vco / 8;
+		reg18 |= 0x00;  /* ring_se23 = 0 */
+		reg19 |= 0x01;  /* ring_seldiv = 1 */
+		reg1f |= 0x03;  /* pw_ring 11 */
+		break;
+	case 3:
+		ring_freq = ring_vco / 6;
+		reg18 |= 0x20;  /* ring_se23 = 1 */
+		reg19 |= 0x00;  /* ring_seldiv = 0 */
+		reg1f |= 0x03;  /* pw_ring 11 */
+		break;
+	case 4:
+		ring_freq = ring_vco / 4;
+		reg18 |= 0x00;  /* ring_se23 = 0 */
+		reg19 |= 0x00;  /* ring_seldiv = 0 */
+		reg1f |= 0x01;  /* pw_ring 01 */
+		break;
+	default:
+		ring_freq = ring_vco / 4;
+		reg18 |= 0x00;  /* ring_se23 = 0 */
+		reg19 |= 0x00;  /* ring_seldiv = 0 */
+		reg1f |= 0x01;  /* pw_ring 01 */
+		break;
+	}
+
+
+	/* write pw_ring, n_ring, ringdiv2 registers */
+
+	/* n_ring, ring_se23 */
+	rc = r820t_write_reg(priv, 0x18, reg18);
+	if (rc < 0)
+		return rc;
+
+	/* ring_sediv */
+	rc = r820t_write_reg(priv, 0x19, reg19);
+	if (rc < 0)
+		return rc;
+
+	/* pw_ring */
+	rc = r820t_write_reg(priv, 0x1f, reg1f);
+	if (rc < 0)
+		return rc;
+
+	/* mux input freq ~ rf_in freq */
+	rc = r820t_set_mux(priv, (ring_freq - 5300) * 1000);
+	if (rc < 0)
+		return rc;
+
+	rc = r820t_set_pll(priv, V4L2_TUNER_DIGITAL_TV,
+			   (ring_freq - 5300) * 1000);
+	if (!priv->has_lock)
+		rc = -EINVAL;
+	if (rc < 0)
+		return rc;
+
+	if (im_flag) {
+		rc = r820t_iq(priv, &imr_point);
+	} else {
+		imr_point.gain_x  = priv->imr_data[3].gain_x;
+		imr_point.phase_y = priv->imr_data[3].phase_y;
+		imr_point.value   = priv->imr_data[3].value;
+
+		rc = r820t_f_imr(priv, &imr_point);
+	}
+	if (rc < 0)
+		return rc;
+
+	/* save IMR value */
+	switch (imr_mem) {
+	case 0:
+		priv->imr_data[0].gain_x  = imr_point.gain_x;
+		priv->imr_data[0].phase_y = imr_point.phase_y;
+		priv->imr_data[0].value   = imr_point.value;
+		break;
+	case 1:
+		priv->imr_data[1].gain_x  = imr_point.gain_x;
+		priv->imr_data[1].phase_y = imr_point.phase_y;
+		priv->imr_data[1].value   = imr_point.value;
+		break;
+	case 2:
+		priv->imr_data[2].gain_x  = imr_point.gain_x;
+		priv->imr_data[2].phase_y = imr_point.phase_y;
+		priv->imr_data[2].value   = imr_point.value;
+		break;
+	case 3:
+		priv->imr_data[3].gain_x  = imr_point.gain_x;
+		priv->imr_data[3].phase_y = imr_point.phase_y;
+		priv->imr_data[3].value   = imr_point.value;
+		break;
+	case 4:
+		priv->imr_data[4].gain_x  = imr_point.gain_x;
+		priv->imr_data[4].phase_y = imr_point.phase_y;
+		priv->imr_data[4].value   = imr_point.value;
+		break;
+	default:
+		priv->imr_data[4].gain_x  = imr_point.gain_x;
+		priv->imr_data[4].phase_y = imr_point.phase_y;
+		priv->imr_data[4].value   = imr_point.value;
+		break;
+	}
+
+	return 0;
+}
+
+static int r820t_imr_callibrate(struct r820t_priv *priv)
+{
+	int rc, i;
+	int xtal_cap = 0;
+
+	if (priv->init_done)
+		return 0;
+
+	/* Detect Xtal capacitance */
+	if ((priv->cfg->rafael_chip == CHIP_R820T) ||
+	    (priv->cfg->rafael_chip == CHIP_R828S) ||
+	    (priv->cfg->rafael_chip == CHIP_R820C)) {
+		priv->xtal_cap_sel = XTAL_HIGH_CAP_0P;
+	} else {
+		/* Initialize registers */
+		rc = r820t_write(priv, 0x05,
+				r820t_init_array, sizeof(r820t_init_array));
+		if (rc < 0)
+			return rc;
+		for (i = 0; i < 3; i++) {
+			rc = r820t_xtal_check(priv);
+			if (rc < 0)
+				return rc;
+			if (!i || rc > xtal_cap)
+				xtal_cap = rc;
+		}
+		priv->xtal_cap_sel = xtal_cap;
+	}
+
+	/*
+	 * Disables IMR callibration. That emulates the same behaviour
+	 * as what is done by rtl-sdr userspace library. Useful for testing
+	 */
+	if (no_imr_cal) {
+		priv->init_done = true;
+
+		return 0;
+	}
+
+	/* Initialize registers */
+	rc = r820t_write(priv, 0x05,
+			 r820t_init_array, sizeof(r820t_init_array));
+	if (rc < 0)
+		return rc;
+
+	rc = r820t_imr_prepare(priv);
+	if (rc < 0)
+		return rc;
+
+	rc = r820t_imr(priv, 3, true);
+	if (rc < 0)
+		return rc;
+	rc = r820t_imr(priv, 1, false);
+	if (rc < 0)
+		return rc;
+	rc = r820t_imr(priv, 0, false);
+	if (rc < 0)
+		return rc;
+	rc = r820t_imr(priv, 2, false);
+	if (rc < 0)
+		return rc;
+	rc = r820t_imr(priv, 4, false);
+	if (rc < 0)
+		return rc;
+
+	priv->init_done = true;
+	priv->imr_done = true;
+
+	return 0;
+}
+
+#if 0
+/* Not used, for now */
+static int r820t_gpio(struct r820t_priv *priv, bool enable)
+{
+	return r820t_write_reg_mask(priv, 0x0f, enable ? 1 : 0, 0x01);
+}
+#endif
+
+/*
+ *  r820t frontend operations and tuner attach code
+ *
+ * All driver locks and i2c control are only in this part of the code
+ */
+
+static int r820t_init(struct dvb_frontend *fe)
+{
+	struct r820t_priv *priv = fe->tuner_priv;
+	int rc;
+
+	tuner_dbg("%s:\n", __func__);
+
+	mutex_lock(&priv->lock);
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+
+	rc = r820t_imr_callibrate(priv);
+	if (rc < 0)
+		goto err;
+
+	/* Initialize registers */
+	rc = r820t_write(priv, 0x05,
+			 r820t_init_array, sizeof(r820t_init_array));
+
+err:
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0);
+	mutex_unlock(&priv->lock);
+
+	if (rc < 0)
+		tuner_dbg("%s: failed=%d\n", __func__, rc);
+	return rc;
+}
+
+static int r820t_sleep(struct dvb_frontend *fe)
+{
+	struct r820t_priv *priv = fe->tuner_priv;
+	int rc;
+
+	tuner_dbg("%s:\n", __func__);
+
+	mutex_lock(&priv->lock);
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+
+	rc = r820t_standby(priv);
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0);
+	mutex_unlock(&priv->lock);
+
+	tuner_dbg("%s: failed=%d\n", __func__, rc);
+	return rc;
+}
+
+static int r820t_set_analog_freq(struct dvb_frontend *fe,
+				 struct analog_parameters *p)
+{
+	struct r820t_priv *priv = fe->tuner_priv;
+	unsigned bw;
+	int rc;
+
+	tuner_dbg("%s called\n", __func__);
+
+	/* if std is not defined, choose one */
+	if (!p->std)
+		p->std = V4L2_STD_MN;
+
+	if ((p->std == V4L2_STD_PAL_M) || (p->std == V4L2_STD_NTSC))
+		bw = 6;
+	else
+		bw = 8;
+
+	mutex_lock(&priv->lock);
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+
+	rc = generic_set_freq(fe, 62500l * p->frequency, bw,
+			      V4L2_TUNER_ANALOG_TV, p->std, SYS_UNDEFINED);
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0);
+	mutex_unlock(&priv->lock);
+
+	return rc;
+}
+
+static int r820t_set_params(struct dvb_frontend *fe)
+{
+	struct r820t_priv *priv = fe->tuner_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int rc;
+	unsigned bw;
+
+	tuner_dbg("%s: delivery_system=%d frequency=%d bandwidth_hz=%d\n",
+		__func__, c->delivery_system, c->frequency, c->bandwidth_hz);
+
+	mutex_lock(&priv->lock);
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+
+	bw = (c->bandwidth_hz + 500000) / 1000000;
+	if (!bw)
+		bw = 8;
+
+	rc = generic_set_freq(fe, c->frequency, bw,
+			      V4L2_TUNER_DIGITAL_TV, 0, c->delivery_system);
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0);
+	mutex_unlock(&priv->lock);
+
+	if (rc)
+		tuner_dbg("%s: failed=%d\n", __func__, rc);
+	return rc;
+}
+
+static int r820t_signal(struct dvb_frontend *fe, u16 *strength)
+{
+	struct r820t_priv *priv = fe->tuner_priv;
+	int rc = 0;
+
+	mutex_lock(&priv->lock);
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+
+	if (priv->has_lock) {
+		rc = r820t_read_gain(priv);
+		if (rc < 0)
+			goto err;
+
+		/* A higher gain at LNA means a lower signal strength */
+		*strength = (45 - rc) << 4 | 0xff;
+		if (*strength == 0xff)
+			*strength = 0;
+	} else {
+		*strength = 0;
+	}
+
+err:
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0);
+	mutex_unlock(&priv->lock);
+
+	tuner_dbg("%s: %s, gain=%d strength=%d\n",
+		  __func__,
+		  priv->has_lock ? "PLL locked" : "no signal",
+		  rc, *strength);
+
+	return 0;
+}
+
+static int r820t_get_if_frequency(struct dvb_frontend *fe, u32 *frequency)
+{
+	struct r820t_priv *priv = fe->tuner_priv;
+
+	tuner_dbg("%s:\n", __func__);
+
+	*frequency = priv->int_freq;
+
+	return 0;
+}
+
+static int r820t_release(struct dvb_frontend *fe)
+{
+	struct r820t_priv *priv = fe->tuner_priv;
+
+	tuner_dbg("%s:\n", __func__);
+
+	mutex_lock(&r820t_list_mutex);
+
+	if (priv)
+		hybrid_tuner_release_state(priv);
+
+	mutex_unlock(&r820t_list_mutex);
+
+	kfree(fe->tuner_priv);
+	fe->tuner_priv = NULL;
+
+	return 0;
+}
+
+static const struct dvb_tuner_ops r820t_tuner_ops = {
+	.info = {
+		.name           = "Rafael Micro R820T",
+		.frequency_min  =   42000000,
+		.frequency_max  = 1002000000,
+	},
+	.init = r820t_init,
+	.release = r820t_release,
+	.sleep = r820t_sleep,
+	.set_params = r820t_set_params,
+	.set_analog_params = r820t_set_analog_freq,
+	.get_if_frequency = r820t_get_if_frequency,
+	.get_rf_strength = r820t_signal,
+};
+
+struct dvb_frontend *r820t_attach(struct dvb_frontend *fe,
+				  struct i2c_adapter *i2c,
+				  const struct r820t_config *cfg)
+{
+	struct r820t_priv *priv;
+	int rc = -ENODEV;
+	u8 data[5];
+	int instance;
+
+	mutex_lock(&r820t_list_mutex);
+
+	instance = hybrid_tuner_request_state(struct r820t_priv, priv,
+					      hybrid_tuner_instance_list,
+					      i2c, cfg->i2c_addr,
+					      "r820t");
+	switch (instance) {
+	case 0:
+		/* memory allocation failure */
+		goto err_no_gate;
+		break;
+	case 1:
+		/* new tuner instance */
+		priv->cfg = cfg;
+
+		mutex_init(&priv->lock);
+
+		fe->tuner_priv = priv;
+		break;
+	case 2:
+		/* existing tuner instance */
+		fe->tuner_priv = priv;
+		break;
+	}
+
+	memcpy(&fe->ops.tuner_ops, &r820t_tuner_ops, sizeof(r820t_tuner_ops));
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+
+	/* check if the tuner is there */
+	rc = r820t_read(priv, 0x00, data, sizeof(data));
+	if (rc < 0)
+		goto err;
+
+	rc = r820t_sleep(fe);
+	if (rc < 0)
+		goto err;
+
+	tuner_info("Rafael Micro r820t successfully identified\n");
+
+	fe->tuner_priv = priv;
+	memcpy(&fe->ops.tuner_ops, &r820t_tuner_ops,
+			sizeof(struct dvb_tuner_ops));
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0);
+
+	mutex_unlock(&r820t_list_mutex);
+
+	return fe;
+err:
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0);
+
+err_no_gate:
+	mutex_unlock(&r820t_list_mutex);
+
+	tuner_info("%s: failed=%d\n", __func__, rc);
+	r820t_release(fe);
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(r820t_attach);
+
+MODULE_DESCRIPTION("Rafael Micro r820t silicon tuner driver");
+MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@redhat.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/tuners/r820t.h b/drivers/media/tuners/r820t.h
new file mode 100644
index 00000000000..48af3548027
--- /dev/null
+++ b/drivers/media/tuners/r820t.h
@@ -0,0 +1,59 @@
+/*
+ * Elonics R820T silicon tuner driver
+ *
+ * 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 R820T_H
+#define R820T_H
+
+#include <linux/kconfig.h>
+#include "dvb_frontend.h"
+
+enum r820t_chip {
+	CHIP_R820T,
+	CHIP_R620D,
+	CHIP_R828D,
+	CHIP_R828,
+	CHIP_R828S,
+	CHIP_R820C,
+};
+
+struct r820t_config {
+	u8 i2c_addr;		/* 0x34 */
+	u32 xtal;
+	enum r820t_chip rafael_chip;
+	unsigned max_i2c_msg_len;
+	bool use_diplexer;
+	bool use_predetect;
+};
+
+#if IS_ENABLED(CONFIG_MEDIA_TUNER_R820T)
+struct dvb_frontend *r820t_attach(struct dvb_frontend *fe,
+				  struct i2c_adapter *i2c,
+				  const struct r820t_config *cfg);
+#else
+static inline struct dvb_frontend *r820t_attach(struct dvb_frontend *fe,
+						struct i2c_adapter *i2c,
+						const struct r820t_config *cfg)
+{
+	pr_warn("%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+#endif
+
+#endif
diff --git a/drivers/media/tuners/tda18212.h b/drivers/media/tuners/tda18212.h
index 9bd5da4aabb..7e0d503baf0 100644
--- a/drivers/media/tuners/tda18212.h
+++ b/drivers/media/tuners/tda18212.h
@@ -21,6 +21,7 @@
 #ifndef TDA18212_H
 #define TDA18212_H
 
+#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 
 struct tda18212_config {
@@ -36,8 +37,7 @@ struct tda18212_config {
 	u16 if_dvbc;
 };
 
-#if defined(CONFIG_MEDIA_TUNER_TDA18212) || \
-	(defined(CONFIG_MEDIA_TUNER_TDA18212_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_MEDIA_TUNER_TDA18212)
 extern struct dvb_frontend *tda18212_attach(struct dvb_frontend *fe,
 	struct i2c_adapter *i2c, struct tda18212_config *cfg);
 #else
diff --git a/drivers/media/tuners/tda18218.h b/drivers/media/tuners/tda18218.h
index b4180d18002..366410e0cc9 100644
--- a/drivers/media/tuners/tda18218.h
+++ b/drivers/media/tuners/tda18218.h
@@ -21,6 +21,7 @@
 #ifndef TDA18218_H
 #define TDA18218_H
 
+#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 
 struct tda18218_config {
@@ -29,8 +30,7 @@ struct tda18218_config {
 	u8 loop_through:1;
 };
 
-#if defined(CONFIG_MEDIA_TUNER_TDA18218) || \
-	(defined(CONFIG_MEDIA_TUNER_TDA18218_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_MEDIA_TUNER_TDA18218)
 extern struct dvb_frontend *tda18218_attach(struct dvb_frontend *fe,
 	struct i2c_adapter *i2c, struct tda18218_config *cfg);
 #else
diff --git a/drivers/media/tuners/tda18271-fe.c b/drivers/media/tuners/tda18271-fe.c
index e7786862dab..4995b890c16 100644
--- a/drivers/media/tuners/tda18271-fe.c
+++ b/drivers/media/tuners/tda18271-fe.c
@@ -21,6 +21,7 @@
 #include <linux/delay.h>
 #include <linux/videodev2.h>
 #include "tda18271-priv.h"
+#include "tda8290.h"
 
 int tda18271_debug;
 module_param_named(debug, tda18271_debug, int, 0644);
@@ -867,12 +868,12 @@ static int tda18271_agc(struct dvb_frontend *fe)
 	int ret = 0;
 
 	switch (priv->config) {
-	case 0:
+	case TDA8290_LNA_OFF:
 		/* no external agc configuration required */
 		if (tda18271_debug & DBG_ADV)
 			tda_dbg("no agc configuration provided\n");
 		break;
-	case 3:
+	case TDA8290_LNA_ON_BRIDGE:
 		/* switch with GPIO of saa713x */
 		tda_dbg("invoking callback\n");
 		if (fe->callback)
@@ -881,8 +882,8 @@ static int tda18271_agc(struct dvb_frontend *fe)
 					   TDA18271_CALLBACK_CMD_AGC_ENABLE,
 					   priv->mode);
 		break;
-	case 1:
-	case 2:
+	case TDA8290_LNA_GP0_HIGH_ON:
+	case TDA8290_LNA_GP0_HIGH_OFF:
 	default:
 		/* n/a - currently not supported */
 		tda_err("unsupported configuration: %d\n", priv->config);
diff --git a/drivers/media/tuners/tda827x.c b/drivers/media/tuners/tda827x.c
index a0d17626747..73453a255cd 100644
--- a/drivers/media/tuners/tda827x.c
+++ b/drivers/media/tuners/tda827x.c
@@ -479,10 +479,10 @@ static void tda827xa_lna_gain(struct dvb_frontend *fe, int high,
 			dprintk("setting LNA to low gain\n");
 	}
 	switch (priv->cfg->config) {
-	case 0: /* no LNA */
+	case TDA8290_LNA_OFF: /* no LNA */
 		break;
-	case 1: /* switch is GPIO 0 of tda8290 */
-	case 2:
+	case TDA8290_LNA_GP0_HIGH_ON: /* switch is GPIO 0 of tda8290 */
+	case TDA8290_LNA_GP0_HIGH_OFF:
 		if (params == NULL) {
 			gp_func = 0;
 			arg  = 0;
@@ -499,11 +499,11 @@ static void tda827xa_lna_gain(struct dvb_frontend *fe, int high,
 				     DVB_FRONTEND_COMPONENT_TUNER,
 				     gp_func, arg);
 		buf[1] = high ? 0 : 1;
-		if (priv->cfg->config == 2)
+		if (priv->cfg->config == TDA8290_LNA_GP0_HIGH_OFF)
 			buf[1] = high ? 1 : 0;
 		tuner_transfer(fe, &msg, 1);
 		break;
-	case 3: /* switch with GPIO of saa713x */
+	case TDA8290_LNA_ON_BRIDGE: /* switch with GPIO of saa713x */
 		if (fe->callback)
 			fe->callback(priv->i2c_adap->algo_data,
 				     DVB_FRONTEND_COMPONENT_TUNER, 0, high);
diff --git a/drivers/media/tuners/tda827x.h b/drivers/media/tuners/tda827x.h
index 9432b5b6121..b64292152ba 100644
--- a/drivers/media/tuners/tda827x.h
+++ b/drivers/media/tuners/tda827x.h
@@ -26,6 +26,7 @@
 
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
+#include "tda8290.h"
 
 struct tda827x_config
 {
@@ -34,7 +35,7 @@ struct tda827x_config
 	int (*sleep) (struct dvb_frontend *fe);
 
 	/* interface to tda829x driver */
-	unsigned int config;
+	enum tda8290_lna config;
 	int 	     switch_addr;
 
 	void (*agcf)(struct dvb_frontend *fe);
diff --git a/drivers/media/tuners/tda8290.c b/drivers/media/tuners/tda8290.c
index 8c4852114ee..ab4106c17b4 100644
--- a/drivers/media/tuners/tda8290.c
+++ b/drivers/media/tuners/tda8290.c
@@ -54,6 +54,7 @@ struct tda8290_priv {
 #define TDA18271 16
 
 	struct tda827x_config cfg;
+	struct tda18271_std_map *tda18271_std_map;
 };
 
 /*---------------------------------------------------------------------*/
@@ -233,7 +234,8 @@ static void tda8290_set_params(struct dvb_frontend *fe,
 	}
 
 
-	tda8290_i2c_bridge(fe, 1);
+	if (fe->ops.analog_ops.i2c_gate_ctrl)
+		fe->ops.analog_ops.i2c_gate_ctrl(fe, 1);
 
 	if (fe->ops.tuner_ops.set_analog_params)
 		fe->ops.tuner_ops.set_analog_params(fe, params);
@@ -302,7 +304,8 @@ static void tda8290_set_params(struct dvb_frontend *fe,
 		}
 	}
 
-	tda8290_i2c_bridge(fe, 0);
+	if (fe->ops.analog_ops.i2c_gate_ctrl)
+		fe->ops.analog_ops.i2c_gate_ctrl(fe, 0);
 	tuner_i2c_xfer_send(&priv->i2c_props, if_agc_set, 2);
 }
 
@@ -388,7 +391,7 @@ static void tda8295_agc2_out(struct dvb_frontend *fe, int enable)
 	tuner_i2c_xfer_send(&priv->i2c_props, set_gpio_val, 2);
 }
 
-static int tda8295_has_signal(struct dvb_frontend *fe)
+static int tda8295_has_signal(struct dvb_frontend *fe, u16 *signal)
 {
 	struct tda8290_priv *priv = fe->analog_demod_priv;
 
@@ -396,7 +399,8 @@ static int tda8295_has_signal(struct dvb_frontend *fe)
 	unsigned char ret;
 
 	tuner_i2c_xfer_send_recv(&priv->i2c_props, &hvpll_stat, 1, &ret, 1);
-	return (ret & 0x01) ? 65535 : 0;
+	*signal = (ret & 0x01) ? 65535 : 0;
+	return 0;
 }
 
 /*---------------------------------------------------------------------*/
@@ -405,7 +409,7 @@ static void tda8295_set_params(struct dvb_frontend *fe,
 			       struct analog_parameters *params)
 {
 	struct tda8290_priv *priv = fe->analog_demod_priv;
-
+	u16 signal = 0;
 	unsigned char blanking_mode[]     = { 0x1d, 0x00 };
 
 	set_audio(fe, params);
@@ -424,7 +428,8 @@ static void tda8295_set_params(struct dvb_frontend *fe,
 	tuner_i2c_xfer_send(&priv->i2c_props, blanking_mode, 2);
 	msleep(20);
 
-	tda8295_i2c_bridge(fe, 1);
+	if (fe->ops.analog_ops.i2c_gate_ctrl)
+		fe->ops.analog_ops.i2c_gate_ctrl(fe, 1);
 
 	if (fe->ops.tuner_ops.set_analog_params)
 		fe->ops.tuner_ops.set_analog_params(fe, params);
@@ -432,17 +437,19 @@ static void tda8295_set_params(struct dvb_frontend *fe,
 	if (priv->cfg.agcf)
 		priv->cfg.agcf(fe);
 
-	if (tda8295_has_signal(fe))
+	tda8295_has_signal(fe, &signal);
+	if (signal)
 		tuner_dbg("tda8295 is locked\n");
 	else
 		tuner_dbg("tda8295 not locked, no signal?\n");
 
-	tda8295_i2c_bridge(fe, 0);
+	if (fe->ops.analog_ops.i2c_gate_ctrl)
+		fe->ops.analog_ops.i2c_gate_ctrl(fe, 0);
 }
 
 /*---------------------------------------------------------------------*/
 
-static int tda8290_has_signal(struct dvb_frontend *fe)
+static int tda8290_has_signal(struct dvb_frontend *fe, u16 *signal)
 {
 	struct tda8290_priv *priv = fe->analog_demod_priv;
 
@@ -451,7 +458,8 @@ static int tda8290_has_signal(struct dvb_frontend *fe)
 
 	tuner_i2c_xfer_send_recv(&priv->i2c_props,
 				 i2c_get_afc, ARRAY_SIZE(i2c_get_afc), &afc, 1);
-	return (afc & 0x80)? 65535:0;
+	*signal = (afc & 0x80) ? 65535 : 0;
+	return 0;
 }
 
 /*---------------------------------------------------------------------*/
@@ -465,11 +473,13 @@ static void tda8290_standby(struct dvb_frontend *fe)
 	unsigned char tda8290_agc_tri[] = { 0x02, 0x20 };
 	struct i2c_msg msg = {.addr = priv->tda827x_addr, .flags=0, .buf=cb1, .len = 2};
 
-	tda8290_i2c_bridge(fe, 1);
+	if (fe->ops.analog_ops.i2c_gate_ctrl)
+		fe->ops.analog_ops.i2c_gate_ctrl(fe, 1);
 	if (priv->ver & TDA8275A)
 		cb1[1] = 0x90;
 	i2c_transfer(priv->i2c_props.adap, &msg, 1);
-	tda8290_i2c_bridge(fe, 0);
+	if (fe->ops.analog_ops.i2c_gate_ctrl)
+		fe->ops.analog_ops.i2c_gate_ctrl(fe, 0);
 	tuner_i2c_xfer_send(&priv->i2c_props, tda8290_agc_tri, 2);
 	tuner_i2c_xfer_send(&priv->i2c_props, tda8290_standby, 2);
 }
@@ -489,7 +499,8 @@ static void tda8290_init_if(struct dvb_frontend *fe)
 	unsigned char set_GP00_CF[] = { 0x20, 0x01 };
 	unsigned char set_GP01_CF[] = { 0x20, 0x0B };
 
-	if ((priv->cfg.config == 1) || (priv->cfg.config == 2))
+	if ((priv->cfg.config == TDA8290_LNA_GP0_HIGH_ON) ||
+	    (priv->cfg.config == TDA8290_LNA_GP0_HIGH_OFF))
 		tuner_i2c_xfer_send(&priv->i2c_props, set_GP00_CF, 2);
 	else
 		tuner_i2c_xfer_send(&priv->i2c_props, set_GP01_CF, 2);
@@ -537,9 +548,11 @@ static void tda8290_init_tuner(struct dvb_frontend *fe)
 	if (priv->ver & TDA8275A)
 		msg.buf = tda8275a_init;
 
-	tda8290_i2c_bridge(fe, 1);
+	if (fe->ops.analog_ops.i2c_gate_ctrl)
+		fe->ops.analog_ops.i2c_gate_ctrl(fe, 1);
 	i2c_transfer(priv->i2c_props.adap, &msg, 1);
-	tda8290_i2c_bridge(fe, 0);
+	if (fe->ops.analog_ops.i2c_gate_ctrl)
+		fe->ops.analog_ops.i2c_gate_ctrl(fe, 0);
 }
 
 /*---------------------------------------------------------------------*/
@@ -565,19 +578,13 @@ static struct tda18271_config tda829x_tda18271_config = {
 static int tda829x_find_tuner(struct dvb_frontend *fe)
 {
 	struct tda8290_priv *priv = fe->analog_demod_priv;
-	struct analog_demod_ops *analog_ops = &fe->ops.analog_ops;
 	int i, ret, tuners_found;
 	u32 tuner_addrs;
 	u8 data;
 	struct i2c_msg msg = { .flags = I2C_M_RD, .buf = &data, .len = 1 };
 
-	if (!analog_ops->i2c_gate_ctrl) {
-		printk(KERN_ERR "tda8290: no gate control were provided!\n");
-
-		return -EINVAL;
-	}
-
-	analog_ops->i2c_gate_ctrl(fe, 1);
+	if (fe->ops.analog_ops.i2c_gate_ctrl)
+		fe->ops.analog_ops.i2c_gate_ctrl(fe, 1);
 
 	/* probe for tuner chip */
 	tuners_found = 0;
@@ -595,7 +602,8 @@ static int tda829x_find_tuner(struct dvb_frontend *fe)
 	   give a response now
 	 */
 
-	analog_ops->i2c_gate_ctrl(fe, 0);
+	if (fe->ops.analog_ops.i2c_gate_ctrl)
+		fe->ops.analog_ops.i2c_gate_ctrl(fe, 0);
 
 	if (tuners_found > 1)
 		for (i = 0; i < tuners_found; i++) {
@@ -618,18 +626,21 @@ static int tda829x_find_tuner(struct dvb_frontend *fe)
 	priv->tda827x_addr = tuner_addrs;
 	msg.addr = tuner_addrs;
 
-	analog_ops->i2c_gate_ctrl(fe, 1);
+	if (fe->ops.analog_ops.i2c_gate_ctrl)
+		fe->ops.analog_ops.i2c_gate_ctrl(fe, 1);
 	ret = i2c_transfer(priv->i2c_props.adap, &msg, 1);
 
 	if (ret != 1) {
 		tuner_warn("tuner access failed!\n");
-		analog_ops->i2c_gate_ctrl(fe, 0);
+		if (fe->ops.analog_ops.i2c_gate_ctrl)
+			fe->ops.analog_ops.i2c_gate_ctrl(fe, 0);
 		return -EREMOTEIO;
 	}
 
 	if ((data == 0x83) || (data == 0x84)) {
 		priv->ver |= TDA18271;
 		tda829x_tda18271_config.config = priv->cfg.config;
+		tda829x_tda18271_config.std_map = priv->tda18271_std_map;
 		dvb_attach(tda18271_attach, fe, priv->tda827x_addr,
 			   priv->i2c_props.adap, &tda829x_tda18271_config);
 	} else {
@@ -648,7 +659,8 @@ static int tda829x_find_tuner(struct dvb_frontend *fe)
 	if (fe->ops.tuner_ops.sleep)
 		fe->ops.tuner_ops.sleep(fe);
 
-	analog_ops->i2c_gate_ctrl(fe, 0);
+	if (fe->ops.analog_ops.i2c_gate_ctrl)
+		fe->ops.analog_ops.i2c_gate_ctrl(fe, 0);
 
 	return 0;
 }
@@ -740,8 +752,10 @@ struct dvb_frontend *tda829x_attach(struct dvb_frontend *fe,
 	priv->i2c_props.addr     = i2c_addr;
 	priv->i2c_props.adap     = i2c_adap;
 	priv->i2c_props.name     = "tda829x";
-	if (cfg)
-		priv->cfg.config         = cfg->lna_cfg;
+	if (cfg) {
+		priv->cfg.config = cfg->lna_cfg;
+		priv->tda18271_std_map = cfg->tda18271_std_map;
+	}
 
 	if (tda8290_probe(&priv->i2c_props) == 0) {
 		priv->ver = TDA8290;
@@ -755,6 +769,9 @@ struct dvb_frontend *tda829x_attach(struct dvb_frontend *fe,
 		       sizeof(struct analog_demod_ops));
 	}
 
+	if (cfg && cfg->no_i2c_gate)
+		fe->ops.analog_ops.i2c_gate_ctrl = NULL;
+
 	if (!(cfg) || (TDA829X_PROBE_TUNER == cfg->probe_tuner)) {
 		tda8295_power(fe, 1);
 		if (tda829x_find_tuner(fe) < 0)
diff --git a/drivers/media/tuners/tda8290.h b/drivers/media/tuners/tda8290.h
index e12ecbaa35a..cf96e585785 100644
--- a/drivers/media/tuners/tda8290.h
+++ b/drivers/media/tuners/tda8290.h
@@ -19,13 +19,23 @@
 
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
+#include "tda18271.h"
+
+enum tda8290_lna {
+	TDA8290_LNA_OFF = 0,
+	TDA8290_LNA_GP0_HIGH_ON = 1,
+	TDA8290_LNA_GP0_HIGH_OFF = 2,
+	TDA8290_LNA_ON_BRIDGE = 3,
+};
 
 struct tda829x_config {
-	unsigned int lna_cfg;
+	enum tda8290_lna lna_cfg;
 
 	unsigned int probe_tuner:1;
 #define TDA829X_PROBE_TUNER 0
 #define TDA829X_DONT_PROBE  1
+	unsigned int no_i2c_gate:1;
+	struct tda18271_std_map *tda18271_std_map;
 };
 
 #if IS_ENABLED(CONFIG_MEDIA_TUNER_TDA8290)
diff --git a/drivers/media/tuners/tda9887.c b/drivers/media/tuners/tda9887.c
index cdb645d5743..300005c535b 100644
--- a/drivers/media/tuners/tda9887.c
+++ b/drivers/media/tuners/tda9887.c
@@ -596,22 +596,22 @@ static void tda9887_tuner_status(struct dvb_frontend *fe)
 		   priv->data[1], priv->data[2], priv->data[3]);
 }
 
-static int tda9887_get_afc(struct dvb_frontend *fe)
+static int tda9887_get_afc(struct dvb_frontend *fe, s32 *afc)
 {
 	struct tda9887_priv *priv = fe->analog_demod_priv;
-	static int AFC_BITS_2_kHz[] = {
+	static const int AFC_BITS_2_kHz[] = {
 		-12500,  -37500,  -62500,  -97500,
 		-112500, -137500, -162500, -187500,
 		187500,  162500,  137500,  112500,
 		97500 ,  62500,   37500 ,  12500
 	};
-	int afc=0;
 	__u8 reg = 0;
 
-	if (1 == tuner_i2c_xfer_recv(&priv->i2c_props,&reg,1))
-		afc = AFC_BITS_2_kHz[(reg>>1)&0x0f];
-
-	return afc;
+	if (priv->mode != V4L2_TUNER_RADIO)
+		return 0;
+	if (1 == tuner_i2c_xfer_recv(&priv->i2c_props, &reg, 1))
+		*afc = AFC_BITS_2_kHz[(reg >> 1) & 0x0f];
+	return 0;
 }
 
 static void tda9887_standby(struct dvb_frontend *fe)
diff --git a/drivers/media/tuners/tua9001.h b/drivers/media/tuners/tua9001.h
index cf5b815feff..26358da1c10 100644
--- a/drivers/media/tuners/tua9001.h
+++ b/drivers/media/tuners/tua9001.h
@@ -21,6 +21,7 @@
 #ifndef TUA9001_H
 #define TUA9001_H
 
+#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 
 struct tua9001_config {
@@ -50,8 +51,7 @@ struct tua9001_config {
 #define TUA9001_CMD_RESETN  1
 #define TUA9001_CMD_RXEN    2
 
-#if defined(CONFIG_MEDIA_TUNER_TUA9001) || \
-	(defined(CONFIG_MEDIA_TUNER_TUA9001_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_MEDIA_TUNER_TUA9001)
 extern struct dvb_frontend *tua9001_attach(struct dvb_frontend *fe,
 		struct i2c_adapter *i2c, struct tua9001_config *cfg);
 #else
diff --git a/drivers/media/tuners/tuner-simple.c b/drivers/media/tuners/tuner-simple.c
index 39e7e583c8c..ca274c2d8c7 100644
--- a/drivers/media/tuners/tuner-simple.c
+++ b/drivers/media/tuners/tuner-simple.c
@@ -115,6 +115,7 @@ struct tuner_simple_priv {
 
 	u32 frequency;
 	u32 bandwidth;
+	bool radio_mode;
 };
 
 /* ---------------------------------------------------------------------- */
@@ -189,7 +190,7 @@ static int simple_get_rf_strength(struct dvb_frontend *fe, u16 *strength)
 	struct tuner_simple_priv *priv = fe->tuner_priv;
 	int signal;
 
-	if (priv->i2c_props.adap == NULL)
+	if (priv->i2c_props.adap == NULL || !priv->radio_mode)
 		return -EINVAL;
 
 	signal = tuner_signal(tuner_read_status(fe));
@@ -776,11 +777,13 @@ static int simple_set_params(struct dvb_frontend *fe,
 
 	switch (params->mode) {
 	case V4L2_TUNER_RADIO:
+		priv->radio_mode = true;
 		ret = simple_set_radio_freq(fe, params);
 		priv->frequency = params->frequency * 125 / 2;
 		break;
 	case V4L2_TUNER_ANALOG_TV:
 	case V4L2_TUNER_DIGITAL_TV:
+		priv->radio_mode = false;
 		ret = simple_set_tv_freq(fe, params);
 		priv->frequency = params->frequency * 62500;
 		break;
diff --git a/drivers/media/tuners/tuner-types.c b/drivers/media/tuners/tuner-types.c
index 2da4440c16e..98bc15a388b 100644
--- a/drivers/media/tuners/tuner-types.c
+++ b/drivers/media/tuners/tuner-types.c
@@ -1381,6 +1381,58 @@ static struct tuner_params tuner_philips_fq1236_mk5_params[] = {
 	},
 };
 
+/* --------- Sony BTF-PG472Z PAL/SECAM ------- */
+
+static struct tuner_range tuner_sony_btf_pg472z_ranges[] = {
+	{ 16 * 144.25 /*MHz*/, 0xc6, 0x01, },
+	{ 16 * 427.25 /*MHz*/, 0xc6, 0x02, },
+	{ 16 * 999.99        , 0xc6, 0x04, },
+};
+
+static struct tuner_params tuner_sony_btf_pg472z_params[] = {
+	{
+		.type   = TUNER_PARAM_TYPE_PAL,
+		.ranges = tuner_sony_btf_pg472z_ranges,
+		.count  = ARRAY_SIZE(tuner_sony_btf_pg472z_ranges),
+		.has_tda9887 = 1,
+		.port1_active = 1,
+		.port2_invert_for_secam_lc = 1,
+	},
+};
+
+/* 90-99 */
+/* --------- Sony BTF-PG467Z NTSC-M-JP ------- */
+
+static struct tuner_range tuner_sony_btf_pg467z_ranges[] = {
+	{ 16 * 220.25 /*MHz*/, 0xc6, 0x01, },
+	{ 16 * 467.25 /*MHz*/, 0xc6, 0x02, },
+	{ 16 * 999.99        , 0xc6, 0x04, },
+};
+
+static struct tuner_params tuner_sony_btf_pg467z_params[] = {
+	{
+		.type   = TUNER_PARAM_TYPE_NTSC,
+		.ranges = tuner_sony_btf_pg467z_ranges,
+		.count  = ARRAY_SIZE(tuner_sony_btf_pg467z_ranges),
+	},
+};
+
+/* --------- Sony BTF-PG463Z NTSC-M ------- */
+
+static struct tuner_range tuner_sony_btf_pg463z_ranges[] = {
+	{ 16 * 130.25 /*MHz*/, 0xc6, 0x01, },
+	{ 16 * 364.25 /*MHz*/, 0xc6, 0x02, },
+	{ 16 * 999.99        , 0xc6, 0x04, },
+};
+
+static struct tuner_params tuner_sony_btf_pg463z_params[] = {
+	{
+		.type   = TUNER_PARAM_TYPE_NTSC,
+		.ranges = tuner_sony_btf_pg463z_ranges,
+		.count  = ARRAY_SIZE(tuner_sony_btf_pg463z_ranges),
+	},
+};
+
 /* --------------------------------------------------------------------- */
 
 struct tunertype tuners[] = {
@@ -1872,6 +1924,23 @@ struct tunertype tuners[] = {
 		.name   = "Xceive 5000C tuner",
 		/* see xc5000.c for details */
 	},
+	[TUNER_SONY_BTF_PG472Z] = {
+		.name   = "Sony BTF-PG472Z PAL/SECAM",
+		.params = tuner_sony_btf_pg472z_params,
+		.count  = ARRAY_SIZE(tuner_sony_btf_pg472z_params),
+	},
+
+	/* 90-99 */
+	[TUNER_SONY_BTF_PK467Z] = {
+		.name   = "Sony BTF-PK467Z NTSC-M-JP",
+		.params = tuner_sony_btf_pg467z_params,
+		.count  = ARRAY_SIZE(tuner_sony_btf_pg467z_params),
+	},
+	[TUNER_SONY_BTF_PB463Z] = {
+		.name   = "Sony BTF-PB463Z NTSC-M",
+		.params = tuner_sony_btf_pg463z_params,
+		.count  = ARRAY_SIZE(tuner_sony_btf_pg463z_params),
+	},
 };
 EXPORT_SYMBOL(tuners);
 
diff --git a/drivers/media/tuners/tuner-xc2028.c b/drivers/media/tuners/tuner-xc2028.c
index 09451737c77..878d2c4d9e8 100644
--- a/drivers/media/tuners/tuner-xc2028.c
+++ b/drivers/media/tuners/tuner-xc2028.c
@@ -1378,8 +1378,7 @@ static int xc2028_set_config(struct dvb_frontend *fe, void *priv_cfg)
 	 * For the firmware name, keep a local copy of the string,
 	 * in order to avoid troubles during device release.
 	 */
-	if (priv->ctrl.fname)
-		kfree(priv->ctrl.fname);
+	kfree(priv->ctrl.fname);
 	memcpy(&priv->ctrl, p, sizeof(priv->ctrl));
 	if (p->fname) {
 		priv->ctrl.fname = kstrdup(p->fname, GFP_KERNEL);
diff --git a/drivers/media/tuners/tuner_it913x.c b/drivers/media/tuners/tuner_it913x.c
new file mode 100644
index 00000000000..6f30d7e535b
--- /dev/null
+++ b/drivers/media/tuners/tuner_it913x.c
@@ -0,0 +1,447 @@
+/*
+ * ITE Tech IT9137 silicon tuner driver
+ *
+ *  Copyright (C) 2011 Malcolm Priestley (tvboxspy@gmail.com)
+ *  IT9137 Copyright (C) ITE Tech 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 "tuner_it913x_priv.h"
+
+struct it913x_state {
+	struct i2c_adapter *i2c_adap;
+	u8 i2c_addr;
+	u8 chip_ver;
+	u8 tuner_type;
+	u8 firmware_ver;
+	u16 tun_xtal;
+	u8 tun_fdiv;
+	u8 tun_clk_mode;
+	u32 tun_fn_min;
+};
+
+/* read multiple registers */
+static int it913x_rd_regs(struct it913x_state *state,
+		u32 reg, u8 *data, u8 count)
+{
+	int ret;
+	u8 b[3];
+	struct i2c_msg msg[2] = {
+		{ .addr = state->i2c_addr, .flags = 0,
+			.buf = b, .len = sizeof(b) },
+		{ .addr = state->i2c_addr, .flags = I2C_M_RD,
+			.buf = data, .len = count }
+	};
+	b[0] = (u8)(reg >> 16) & 0xff;
+	b[1] = (u8)(reg >> 8) & 0xff;
+	b[2] = (u8) reg & 0xff;
+	b[0] |= 0x80; /* All reads from demodulator */
+
+	ret = i2c_transfer(state->i2c_adap, msg, 2);
+
+	return ret;
+}
+
+/* read single register */
+static int it913x_rd_reg(struct it913x_state *state, u32 reg)
+{
+	int ret;
+	u8 b[1];
+	ret = it913x_rd_regs(state, reg, &b[0], sizeof(b));
+	return (ret < 0) ? -ENODEV : b[0];
+}
+
+/* write multiple registers */
+static int it913x_wr_regs(struct it913x_state *state,
+		u8 pro, u32 reg, u8 buf[], u8 count)
+{
+	u8 b[256];
+	struct i2c_msg msg[1] = {
+		{ .addr = state->i2c_addr, .flags = 0,
+		  .buf = b, .len = 3 + count }
+	};
+	int ret;
+	b[0] = (u8)(reg >> 16) & 0xff;
+	b[1] = (u8)(reg >> 8) & 0xff;
+	b[2] = (u8) reg & 0xff;
+	memcpy(&b[3], buf, count);
+
+	if (pro == PRO_DMOD)
+		b[0] |= 0x80;
+
+	ret = i2c_transfer(state->i2c_adap, msg, 1);
+
+	if (ret < 0)
+		return -EIO;
+
+	return 0;
+}
+
+/* write single register */
+static int it913x_wr_reg(struct it913x_state *state,
+		u8 pro, u32 reg, u32 data)
+{
+	int ret;
+	u8 b[4];
+	u8 s;
+
+	b[0] = data >> 24;
+	b[1] = (data >> 16) & 0xff;
+	b[2] = (data >> 8) & 0xff;
+	b[3] = data & 0xff;
+	/* expand write as needed */
+	if (data < 0x100)
+		s = 3;
+	else if (data < 0x1000)
+		s = 2;
+	else if (data < 0x100000)
+		s = 1;
+	else
+		s = 0;
+
+	ret = it913x_wr_regs(state, pro, reg, &b[s], sizeof(b) - s);
+
+	return ret;
+}
+
+static int it913x_script_loader(struct it913x_state *state,
+		struct it913xset *loadscript)
+{
+	int ret, i;
+	if (loadscript == NULL)
+		return -EINVAL;
+
+	for (i = 0; i < 1000; ++i) {
+		if (loadscript[i].pro == 0xff)
+			break;
+		ret = it913x_wr_regs(state, loadscript[i].pro,
+			loadscript[i].address,
+			loadscript[i].reg, loadscript[i].count);
+		if (ret < 0)
+			return -ENODEV;
+	}
+	return 0;
+}
+
+static int it913x_init(struct dvb_frontend *fe)
+{
+	struct it913x_state *state = fe->tuner_priv;
+	int ret, i, reg;
+	u8 val, nv_val;
+	u8 nv[] = {48, 32, 24, 16, 12, 8, 6, 4, 2};
+	u8 b[2];
+
+	reg = it913x_rd_reg(state, 0xec86);
+	switch (reg) {
+	case 0:
+		state->tun_clk_mode = reg;
+		state->tun_xtal = 2000;
+		state->tun_fdiv = 3;
+		val = 16;
+		break;
+	case -ENODEV:
+		return -ENODEV;
+	case 1:
+	default:
+		state->tun_clk_mode = reg;
+		state->tun_xtal = 640;
+		state->tun_fdiv = 1;
+		val = 6;
+		break;
+	}
+
+	reg = it913x_rd_reg(state, 0xed03);
+
+	if (reg < 0)
+		return -ENODEV;
+	else if (reg < ARRAY_SIZE(nv))
+		nv_val = nv[reg];
+	else
+		nv_val = 2;
+
+	for (i = 0; i < 50; i++) {
+		ret = it913x_rd_regs(state, 0xed23, &b[0], sizeof(b));
+		reg = (b[1] << 8) + b[0];
+		if (reg > 0)
+			break;
+		if (ret < 0)
+			return -ENODEV;
+		udelay(2000);
+	}
+	state->tun_fn_min = state->tun_xtal * reg;
+	state->tun_fn_min /= (state->tun_fdiv * nv_val);
+	dev_dbg(&state->i2c_adap->dev, "%s: Tuner fn_min %d\n", __func__,
+			state->tun_fn_min);
+
+	if (state->chip_ver > 1)
+		msleep(50);
+	else {
+		for (i = 0; i < 50; i++) {
+			reg = it913x_rd_reg(state, 0xec82);
+			if (reg > 0)
+				break;
+			if (reg < 0)
+				return -ENODEV;
+			udelay(2000);
+		}
+	}
+
+	/* Power Up Tuner - common all versions */
+	ret = it913x_wr_reg(state, PRO_DMOD, 0xec40, 0x1);
+	ret |= it913x_wr_reg(state, PRO_DMOD, 0xfba8, 0x0);
+	ret |= it913x_wr_reg(state, PRO_DMOD, 0xec57, 0x0);
+	ret |= it913x_wr_reg(state, PRO_DMOD, 0xec58, 0x0);
+
+	return it913x_wr_reg(state, PRO_DMOD, 0xed81, val);
+}
+
+static int it9137_set_params(struct dvb_frontend *fe)
+{
+	struct it913x_state *state = fe->tuner_priv;
+	struct it913xset *set_tuner = set_it9137_template;
+	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
+	u32 bandwidth = p->bandwidth_hz;
+	u32 frequency_m = p->frequency;
+	int ret, reg;
+	u32 frequency = frequency_m / 1000;
+	u32 freq, temp_f, tmp;
+	u16 iqik_m_cal;
+	u16 n_div;
+	u8 n;
+	u8 l_band;
+	u8 lna_band;
+	u8 bw;
+
+	if (state->firmware_ver == 1)
+		set_tuner = set_it9135_template;
+	else
+		set_tuner = set_it9137_template;
+
+	dev_dbg(&state->i2c_adap->dev, "%s: Tuner Frequency %d Bandwidth %d\n",
+			__func__, frequency, bandwidth);
+
+	if (frequency >= 51000 && frequency <= 440000) {
+		l_band = 0;
+		lna_band = 0;
+	} else if (frequency > 440000 && frequency <= 484000) {
+		l_band = 1;
+		lna_band = 1;
+	} else if (frequency > 484000 && frequency <= 533000) {
+		l_band = 1;
+		lna_band = 2;
+	} else if (frequency > 533000 && frequency <= 587000) {
+		l_band = 1;
+		lna_band = 3;
+	} else if (frequency > 587000 && frequency <= 645000) {
+		l_band = 1;
+		lna_band = 4;
+	} else if (frequency > 645000 && frequency <= 710000) {
+		l_band = 1;
+		lna_band = 5;
+	} else if (frequency > 710000 && frequency <= 782000) {
+		l_band = 1;
+		lna_band = 6;
+	} else if (frequency > 782000 && frequency <= 860000) {
+		l_band = 1;
+		lna_band = 7;
+	} else if (frequency > 1450000 && frequency <= 1492000) {
+		l_band = 1;
+		lna_band = 0;
+	} else if (frequency > 1660000 && frequency <= 1685000) {
+		l_band = 1;
+		lna_band = 1;
+	} else
+		return -EINVAL;
+	set_tuner[0].reg[0] = lna_band;
+
+	switch (bandwidth) {
+	case 5000000:
+		bw = 0;
+		break;
+	case 6000000:
+		bw = 2;
+		break;
+	case 7000000:
+		bw = 4;
+		break;
+	default:
+	case 8000000:
+		bw = 6;
+		break;
+	}
+
+	set_tuner[1].reg[0] = bw;
+	set_tuner[2].reg[0] = 0xa0 | (l_band << 3);
+
+	if (frequency > 53000 && frequency <= 74000) {
+		n_div = 48;
+		n = 0;
+	} else if (frequency > 74000 && frequency <= 111000) {
+		n_div = 32;
+		n = 1;
+	} else if (frequency > 111000 && frequency <= 148000) {
+		n_div = 24;
+		n = 2;
+	} else if (frequency > 148000 && frequency <= 222000) {
+		n_div = 16;
+		n = 3;
+	} else if (frequency > 222000 && frequency <= 296000) {
+		n_div = 12;
+		n = 4;
+	} else if (frequency > 296000 && frequency <= 445000) {
+		n_div = 8;
+		n = 5;
+	} else if (frequency > 445000 && frequency <= state->tun_fn_min) {
+		n_div = 6;
+		n = 6;
+	} else if (frequency > state->tun_fn_min && frequency <= 950000) {
+		n_div = 4;
+		n = 7;
+	} else if (frequency > 1450000 && frequency <= 1680000) {
+		n_div = 2;
+		n = 0;
+	} else
+		return -EINVAL;
+
+	reg = it913x_rd_reg(state, 0xed81);
+	iqik_m_cal = (u16)reg * n_div;
+
+	if (reg < 0x20) {
+		if (state->tun_clk_mode == 0)
+			iqik_m_cal = (iqik_m_cal * 9) >> 5;
+		else
+			iqik_m_cal >>= 1;
+	} else {
+		iqik_m_cal = 0x40 - iqik_m_cal;
+		if (state->tun_clk_mode == 0)
+			iqik_m_cal = ~((iqik_m_cal * 9) >> 5);
+		else
+			iqik_m_cal = ~(iqik_m_cal >> 1);
+	}
+
+	temp_f = frequency * (u32)n_div * (u32)state->tun_fdiv;
+	freq = temp_f / state->tun_xtal;
+	tmp = freq * state->tun_xtal;
+
+	if ((temp_f - tmp) >= (state->tun_xtal >> 1))
+		freq++;
+
+	freq += (u32) n << 13;
+	/* Frequency OMEGA_IQIK_M_CAL_MID*/
+	temp_f = freq + (u32)iqik_m_cal;
+
+	set_tuner[3].reg[0] =  temp_f & 0xff;
+	set_tuner[4].reg[0] =  (temp_f >> 8) & 0xff;
+
+	dev_dbg(&state->i2c_adap->dev, "%s: High Frequency = %04x\n",
+			__func__, temp_f);
+
+	/* Lower frequency */
+	set_tuner[5].reg[0] =  freq & 0xff;
+	set_tuner[6].reg[0] =  (freq >> 8) & 0xff;
+
+	dev_dbg(&state->i2c_adap->dev, "%s: low Frequency = %04x\n",
+			__func__, freq);
+
+	ret = it913x_script_loader(state, set_tuner);
+
+	return (ret < 0) ? -ENODEV : 0;
+}
+
+/* Power sequence */
+/* Power Up	Tuner on -> Frontend suspend off -> Tuner clk on */
+/* Power Down	Frontend suspend on -> Tuner clk off -> Tuner off */
+
+static int it913x_sleep(struct dvb_frontend *fe)
+{
+	struct it913x_state *state = fe->tuner_priv;
+	return it913x_script_loader(state, it9137_tuner_off);
+}
+
+static int it913x_release(struct dvb_frontend *fe)
+{
+	kfree(fe->tuner_priv);
+	return 0;
+}
+
+static const struct dvb_tuner_ops it913x_tuner_ops = {
+	.info = {
+		.name           = "ITE Tech IT913X",
+		.frequency_min  = 174000000,
+		.frequency_max  = 862000000,
+	},
+
+	.release = it913x_release,
+
+	.init = it913x_init,
+	.sleep = it913x_sleep,
+	.set_params = it9137_set_params,
+};
+
+struct dvb_frontend *it913x_attach(struct dvb_frontend *fe,
+		struct i2c_adapter *i2c_adap, u8 i2c_addr, u8 config)
+{
+	struct it913x_state *state = NULL;
+
+	/* allocate memory for the internal state */
+	state = kzalloc(sizeof(struct it913x_state), GFP_KERNEL);
+	if (state == NULL)
+		return NULL;
+
+	state->i2c_adap = i2c_adap;
+	state->i2c_addr = i2c_addr;
+
+	switch (config) {
+	case AF9033_TUNER_IT9135_38:
+	case AF9033_TUNER_IT9135_51:
+	case AF9033_TUNER_IT9135_52:
+		state->chip_ver = 0x01;
+		break;
+	case AF9033_TUNER_IT9135_60:
+	case AF9033_TUNER_IT9135_61:
+	case AF9033_TUNER_IT9135_62:
+		state->chip_ver = 0x02;
+		break;
+	default:
+		dev_dbg(&i2c_adap->dev,
+				"%s: invalid config=%02x\n", __func__, config);
+		goto error;
+	}
+
+	state->tuner_type = config;
+	state->firmware_ver = 1;
+
+	fe->tuner_priv = state;
+	memcpy(&fe->ops.tuner_ops, &it913x_tuner_ops,
+			sizeof(struct dvb_tuner_ops));
+
+	dev_info(&i2c_adap->dev,
+			"%s: ITE Tech IT913X successfully attached\n",
+			KBUILD_MODNAME);
+	dev_dbg(&i2c_adap->dev, "%s: config=%02x chip_ver=%02x\n",
+			__func__, config, state->chip_ver);
+
+	return fe;
+error:
+	kfree(state);
+	return NULL;
+}
+EXPORT_SYMBOL(it913x_attach);
+
+MODULE_DESCRIPTION("ITE Tech IT913X silicon tuner driver");
+MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/tuners/tuner_it913x.h b/drivers/media/tuners/tuner_it913x.h
new file mode 100644
index 00000000000..12dd36bd9e7
--- /dev/null
+++ b/drivers/media/tuners/tuner_it913x.h
@@ -0,0 +1,45 @@
+/*
+ * ITE Tech IT9137 silicon tuner driver
+ *
+ *  Copyright (C) 2011 Malcolm Priestley (tvboxspy@gmail.com)
+ *  IT9137 Copyright (C) ITE Tech 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.=
+ */
+
+#ifndef IT913X_H
+#define IT913X_H
+
+#include "dvb_frontend.h"
+
+#if defined(CONFIG_MEDIA_TUNER_IT913X) || \
+	(defined(CONFIG_MEDIA_TUNER_IT913X_MODULE) && defined(MODULE))
+extern struct dvb_frontend *it913x_attach(struct dvb_frontend *fe,
+	struct i2c_adapter *i2c_adap,
+	u8 i2c_addr,
+	u8 config);
+#else
+static inline struct dvb_frontend *it913x_attach(struct dvb_frontend *fe,
+	struct i2c_adapter *i2c_adap,
+	u8 i2c_addr,
+	u8 config)
+{
+	pr_warn("%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+#endif
+
+#endif
diff --git a/drivers/media/tuners/tuner_it913x_priv.h b/drivers/media/tuners/tuner_it913x_priv.h
new file mode 100644
index 00000000000..ce652108aa5
--- /dev/null
+++ b/drivers/media/tuners/tuner_it913x_priv.h
@@ -0,0 +1,78 @@
+/*
+ * ITE Tech IT9137 silicon tuner driver
+ *
+ *  Copyright (C) 2011 Malcolm Priestley (tvboxspy@gmail.com)
+ *  IT9137 Copyright (C) ITE Tech 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.=
+ */
+
+#ifndef IT913X_PRIV_H
+#define IT913X_PRIV_H
+
+#include "tuner_it913x.h"
+#include "af9033.h"
+
+#define PRO_LINK		0x0
+#define PRO_DMOD		0x1
+#define TRIGGER_OFSM		0x0000
+
+struct it913xset {	u32 pro;
+			u32 address;
+			u8 reg[15];
+			u8 count;
+};
+
+/* Tuner setting scripts (still keeping it9137) */
+static struct it913xset it9137_tuner_off[] = {
+	{PRO_DMOD, 0xfba8, {0x01}, 0x01}, /* Tuner Clock Off  */
+	{PRO_DMOD, 0xec40, {0x00}, 0x01}, /* Power Down Tuner */
+	{PRO_DMOD, 0xec02, {0x3f, 0x1f, 0x3f, 0x3f}, 0x04},
+	{PRO_DMOD, 0xec06, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+				0x00, 0x00, 0x00, 0x00}, 0x0c},
+	{PRO_DMOD, 0xec12, {0x00, 0x00, 0x00, 0x00}, 0x04},
+	{PRO_DMOD, 0xec17, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+				0x00}, 0x09},
+	{PRO_DMOD, 0xec22, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+				0x00, 0x00}, 0x0a},
+	{PRO_DMOD, 0xec20, {0x00}, 0x01},
+	{PRO_DMOD, 0xec3f, {0x01}, 0x01},
+	{0xff, 0x0000, {0x00}, 0x00}, /* Terminating Entry */
+};
+
+static struct it913xset set_it9135_template[] = {
+	{PRO_DMOD, 0xee06, {0x00}, 0x01},
+	{PRO_DMOD, 0xec56, {0x00}, 0x01},
+	{PRO_DMOD, 0xec4c, {0x00}, 0x01},
+	{PRO_DMOD, 0xec4d, {0x00}, 0x01},
+	{PRO_DMOD, 0xec4e, {0x00}, 0x01},
+	{PRO_DMOD, 0x011e, {0x00}, 0x01}, /* Older Devices */
+	{PRO_DMOD, 0x011f, {0x00}, 0x01},
+	{0xff, 0x0000, {0x00}, 0x00}, /* Terminating Entry */
+};
+
+static struct it913xset set_it9137_template[] = {
+	{PRO_DMOD, 0xee06, {0x00}, 0x01},
+	{PRO_DMOD, 0xec56, {0x00}, 0x01},
+	{PRO_DMOD, 0xec4c, {0x00}, 0x01},
+	{PRO_DMOD, 0xec4d, {0x00}, 0x01},
+	{PRO_DMOD, 0xec4e, {0x00}, 0x01},
+	{PRO_DMOD, 0xec4f, {0x00}, 0x01},
+	{PRO_DMOD, 0xec50, {0x00}, 0x01},
+	{0xff, 0x0000, {0x00}, 0x00}, /* Terminating Entry */
+};
+
+#endif
diff --git a/drivers/media/tuners/xc5000.c b/drivers/media/tuners/xc5000.c
index d6be1b613c5..5cd09a681b6 100644
--- a/drivers/media/tuners/xc5000.c
+++ b/drivers/media/tuners/xc5000.c
@@ -422,13 +422,19 @@ static int xc_initialize(struct xc5000_priv *priv)
 }
 
 static int xc_SetTVStandard(struct xc5000_priv *priv,
-	u16 VideoMode, u16 AudioMode)
+	u16 VideoMode, u16 AudioMode, u8 RadioMode)
 {
 	int ret;
 	dprintk(1, "%s(0x%04x,0x%04x)\n", __func__, VideoMode, AudioMode);
-	dprintk(1, "%s() Standard = %s\n",
-		__func__,
-		XC5000_Standard[priv->video_standard].Name);
+	if (RadioMode) {
+		dprintk(1, "%s() Standard = %s\n",
+			__func__,
+			XC5000_Standard[RadioMode].Name);
+	} else {
+		dprintk(1, "%s() Standard = %s\n",
+			__func__,
+			XC5000_Standard[priv->video_standard].Name);
+	}
 
 	ret = xc_write_reg(priv, XREG_VIDEO_MODE, VideoMode);
 	if (ret == XC_RESULT_SUCCESS)
@@ -824,7 +830,7 @@ static int xc5000_set_params(struct dvb_frontend *fe)
 
 	ret = xc_SetTVStandard(priv,
 		XC5000_Standard[priv->video_standard].VideoMode,
-		XC5000_Standard[priv->video_standard].AudioMode);
+		XC5000_Standard[priv->video_standard].AudioMode, 0);
 	if (ret != XC_RESULT_SUCCESS) {
 		printk(KERN_ERR "xc5000: xc_SetTVStandard failed\n");
 		return -EREMOTEIO;
@@ -940,7 +946,7 @@ tune_channel:
 
 	ret = xc_SetTVStandard(priv,
 		XC5000_Standard[priv->video_standard].VideoMode,
-		XC5000_Standard[priv->video_standard].AudioMode);
+		XC5000_Standard[priv->video_standard].AudioMode, 0);
 	if (ret != XC_RESULT_SUCCESS) {
 		printk(KERN_ERR "xc5000: xc_SetTVStandard failed\n");
 		return -EREMOTEIO;
@@ -1003,7 +1009,7 @@ static int xc5000_set_radio_freq(struct dvb_frontend *fe,
 	priv->rf_mode = XC_RF_MODE_AIR;
 
 	ret = xc_SetTVStandard(priv, XC5000_Standard[radio_input].VideoMode,
-			       XC5000_Standard[radio_input].AudioMode);
+			       XC5000_Standard[radio_input].AudioMode, radio_input);
 
 	if (ret != XC_RESULT_SUCCESS) {
 		printk(KERN_ERR "xc5000: xc_SetTVStandard failed\n");
diff --git a/drivers/media/tuners/xc5000.h b/drivers/media/tuners/xc5000.h
index b1a54749462..7245cae19f0 100644
--- a/drivers/media/tuners/xc5000.h
+++ b/drivers/media/tuners/xc5000.h
@@ -22,6 +22,7 @@
 #ifndef __XC5000_H__
 #define __XC5000_H__
 
+#include <linux/kconfig.h>
 #include <linux/firmware.h>
 
 struct dvb_frontend;
@@ -56,8 +57,7 @@ struct xc5000_config {
  * it's passed back to a bridge during tuner_callback().
  */
 
-#if defined(CONFIG_MEDIA_TUNER_XC5000) || \
-    (defined(CONFIG_MEDIA_TUNER_XC5000_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_MEDIA_TUNER_XC5000)
 extern struct dvb_frontend *xc5000_attach(struct dvb_frontend *fe,
 					  struct i2c_adapter *i2c,
 					  const struct xc5000_config *cfg);
diff --git a/drivers/media/usb/au0828/au0828-core.c b/drivers/media/usb/au0828/au0828-core.c
index 1e6f40ef1c6..bd9d19a73ef 100644
--- a/drivers/media/usb/au0828/au0828-core.c
+++ b/drivers/media/usb/au0828/au0828-core.c
@@ -125,6 +125,26 @@ static int recv_control_msg(struct au0828_dev *dev, u16 request, u32 value,
 	return status;
 }
 
+static void au0828_usb_release(struct au0828_dev *dev)
+{
+	/* I2C */
+	au0828_i2c_unregister(dev);
+
+	kfree(dev);
+}
+
+#ifdef CONFIG_VIDEO_AU0828_V4L2
+static void au0828_usb_v4l2_release(struct v4l2_device *v4l2_dev)
+{
+	struct au0828_dev *dev =
+		container_of(v4l2_dev, struct au0828_dev, v4l2_dev);
+
+	v4l2_ctrl_handler_free(&dev->v4l2_ctrl_hdl);
+	v4l2_device_unregister(&dev->v4l2_dev);
+	au0828_usb_release(dev);
+}
+#endif
+
 static void au0828_usb_disconnect(struct usb_interface *interface)
 {
 	struct au0828_dev *dev = usb_get_intfdata(interface);
@@ -134,26 +154,19 @@ static void au0828_usb_disconnect(struct usb_interface *interface)
 	/* Digital TV */
 	au0828_dvb_unregister(dev);
 
-#ifdef CONFIG_VIDEO_AU0828_V4L2
-	if (AUVI_INPUT(0).type != AU0828_VMUX_UNDEFINED)
-		au0828_analog_unregister(dev);
-#endif
-
-	/* I2C */
-	au0828_i2c_unregister(dev);
-
-#ifdef CONFIG_VIDEO_AU0828_V4L2
-	v4l2_device_unregister(&dev->v4l2_dev);
-#endif
-
 	usb_set_intfdata(interface, NULL);
-
 	mutex_lock(&dev->mutex);
 	dev->usbdev = NULL;
 	mutex_unlock(&dev->mutex);
-
-	kfree(dev);
-
+#ifdef CONFIG_VIDEO_AU0828_V4L2
+	if (AUVI_INPUT(0).type != AU0828_VMUX_UNDEFINED) {
+		au0828_analog_unregister(dev);
+		v4l2_device_disconnect(&dev->v4l2_dev);
+		v4l2_device_put(&dev->v4l2_dev);
+		return;
+	}
+#endif
+	au0828_usb_release(dev);
 }
 
 static int au0828_usb_probe(struct usb_interface *interface,
@@ -202,15 +215,27 @@ static int au0828_usb_probe(struct usb_interface *interface,
 	dev->boardnr = id->driver_info;
 
 #ifdef CONFIG_VIDEO_AU0828_V4L2
+	dev->v4l2_dev.release = au0828_usb_v4l2_release;
+
 	/* Create the v4l2_device */
 	retval = v4l2_device_register(&interface->dev, &dev->v4l2_dev);
 	if (retval) {
-		printk(KERN_ERR "%s() v4l2_device_register failed\n",
+		pr_err("%s() v4l2_device_register failed\n",
+		       __func__);
+		mutex_unlock(&dev->lock);
+		kfree(dev);
+		return retval;
+	}
+	/* This control handler will inherit the controls from au8522 */
+	retval = v4l2_ctrl_handler_init(&dev->v4l2_ctrl_hdl, 4);
+	if (retval) {
+		pr_err("%s() v4l2_ctrl_handler_init failed\n",
 		       __func__);
 		mutex_unlock(&dev->lock);
 		kfree(dev);
-		return -EIO;
+		return retval;
 	}
+	dev->v4l2_dev.ctrl_handler = &dev->v4l2_ctrl_hdl;
 #endif
 
 	/* Power Up the bridge */
diff --git a/drivers/media/usb/au0828/au0828-video.c b/drivers/media/usb/au0828/au0828-video.c
index 8b9e8268e91..75ac9947cda 100644
--- a/drivers/media/usb/au0828/au0828-video.c
+++ b/drivers/media/usb/au0828/au0828-video.c
@@ -35,6 +35,7 @@
 #include <linux/suspend.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
 #include <media/v4l2-chip-ident.h>
 #include <media/tuner.h>
 #include "au0828.h"
@@ -58,6 +59,12 @@ do {\
 	} \
   } while (0)
 
+static inline void i2c_gate_ctrl(struct au0828_dev *dev, int val)
+{
+	if (dev->dvb.frontend && dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl)
+		dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl(dev->dvb.frontend, val);
+}
+
 static inline void print_err_status(struct au0828_dev *dev,
 				    int packet, int status)
 {
@@ -988,20 +995,22 @@ static int au0828_v4l2_open(struct file *filp)
 
 	fh->type = type;
 	fh->dev = dev;
+	v4l2_fh_init(&fh->fh, vdev);
 	filp->private_data = fh;
 
-	if (fh->type == V4L2_BUF_TYPE_VIDEO_CAPTURE && dev->users == 0) {
+	if (mutex_lock_interruptible(&dev->lock)) {
+		kfree(fh);
+		return -ERESTARTSYS;
+	}
+	if (dev->users == 0) {
 		/* set au0828 interface0 to AS5 here again */
 		ret = usb_set_interface(dev->usbdev, 0, 5);
 		if (ret < 0) {
+			mutex_unlock(&dev->lock);
 			printk(KERN_INFO "Au0828 can't set alternate to 5!\n");
+			kfree(fh);
 			return -EBUSY;
 		}
-		dev->width = NTSC_STD_W;
-		dev->height = NTSC_STD_H;
-		dev->frame_size = dev->width * dev->height * 2;
-		dev->field_size = dev->width * dev->height;
-		dev->bytesperline = dev->width * 2;
 
 		au0828_analog_stream_enable(dev);
 		au0828_analog_stream_reset(dev);
@@ -1014,6 +1023,7 @@ static int au0828_v4l2_open(struct file *filp)
 	}
 
 	dev->users++;
+	mutex_unlock(&dev->lock);
 
 	videobuf_queue_vmalloc_init(&fh->vb_vidq, &au0828_video_qops,
 				    NULL, &dev->slock,
@@ -1023,14 +1033,13 @@ static int au0828_v4l2_open(struct file *filp)
 				    &dev->lock);
 
 	/* VBI Setup */
-	dev->vbi_width = 720;
-	dev->vbi_height = 1;
 	videobuf_queue_vmalloc_init(&fh->vb_vbiq, &au0828_vbi_qops,
 				    NULL, &dev->slock,
 				    V4L2_BUF_TYPE_VBI_CAPTURE,
 				    V4L2_FIELD_SEQ_TB,
 				    sizeof(struct au0828_buffer), fh,
 				    &dev->lock);
+	v4l2_fh_add(&fh->fh);
 	return ret;
 }
 
@@ -1040,6 +1049,9 @@ static int au0828_v4l2_close(struct file *filp)
 	struct au0828_fh *fh = filp->private_data;
 	struct au0828_dev *dev = fh->dev;
 
+	v4l2_fh_del(&fh->fh);
+	v4l2_fh_exit(&fh->fh);
+	mutex_lock(&dev->lock);
 	if (res_check(fh, AU0828_RESOURCE_VIDEO)) {
 		/* Cancel timeout thread in case they didn't call streamoff */
 		dev->vid_timeout_running = 0;
@@ -1058,19 +1070,14 @@ static int au0828_v4l2_close(struct file *filp)
 		res_free(fh, AU0828_RESOURCE_VBI);
 	}
 
-	if (dev->users == 1) {
-		if (dev->dev_state & DEV_DISCONNECTED) {
-			au0828_analog_unregister(dev);
-			kfree(dev);
-			return 0;
-		}
-
+	if (dev->users == 1 && video_is_registered(video_devdata(filp))) {
 		au0828_analog_stream_disable(dev);
 
 		au0828_uninit_isoc(dev);
 
 		/* Save some power by putting tuner to sleep */
 		v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_power, 0);
+		dev->std_set_in_tuner_core = 0;
 
 		/* When close the device, set the usb intf0 into alt0 to free
 		   USB bandwidth */
@@ -1078,6 +1085,7 @@ static int au0828_v4l2_close(struct file *filp)
 		if (ret < 0)
 			printk(KERN_INFO "Au0828 can't set alternate to 0!\n");
 	}
+	mutex_unlock(&dev->lock);
 
 	videobuf_mmap_free(&fh->vb_vidq);
 	videobuf_mmap_free(&fh->vb_vbiq);
@@ -1087,6 +1095,26 @@ static int au0828_v4l2_close(struct file *filp)
 	return 0;
 }
 
+/* Must be called with dev->lock held */
+static void au0828_init_tuner(struct au0828_dev *dev)
+{
+	struct v4l2_frequency f = {
+		.frequency = dev->ctrl_freq,
+		.type = V4L2_TUNER_ANALOG_TV,
+	};
+
+	if (dev->std_set_in_tuner_core)
+		return;
+	dev->std_set_in_tuner_core = 1;
+	i2c_gate_ctrl(dev, 1);
+	/* If we've never sent the standard in tuner core, do so now.
+	   We don't do this at device probe because we don't want to
+	   incur the cost of a firmware load */
+	v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_std, dev->std);
+	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_frequency, &f);
+	i2c_gate_ctrl(dev, 0);
+}
+
 static ssize_t au0828_v4l2_read(struct file *filp, char __user *buf,
 				size_t count, loff_t *pos)
 {
@@ -1098,6 +1126,11 @@ static ssize_t au0828_v4l2_read(struct file *filp, char __user *buf,
 	if (rc < 0)
 		return rc;
 
+	if (mutex_lock_interruptible(&dev->lock))
+		return -ERESTARTSYS;
+	au0828_init_tuner(dev);
+	mutex_unlock(&dev->lock);
+
 	if (fh->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
 		if (res_locked(dev, AU0828_RESOURCE_VIDEO))
 			return -EBUSY;
@@ -1128,23 +1161,32 @@ static unsigned int au0828_v4l2_poll(struct file *filp, poll_table *wait)
 {
 	struct au0828_fh *fh = filp->private_data;
 	struct au0828_dev *dev = fh->dev;
-	int rc;
+	unsigned long req_events = poll_requested_events(wait);
+	unsigned int res;
 
-	rc = check_dev(dev);
-	if (rc < 0)
-		return rc;
+	if (check_dev(dev) < 0)
+		return POLLERR;
+
+	res = v4l2_ctrl_poll(filp, wait);
+	if (!(req_events & (POLLIN | POLLRDNORM)))
+		return res;
+
+	if (mutex_lock_interruptible(&dev->lock))
+		return -ERESTARTSYS;
+	au0828_init_tuner(dev);
+	mutex_unlock(&dev->lock);
 
 	if (fh->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
 		if (!res_get(fh, AU0828_RESOURCE_VIDEO))
 			return POLLERR;
-		return videobuf_poll_stream(filp, &fh->vb_vidq, wait);
-	} else if (fh->type == V4L2_BUF_TYPE_VBI_CAPTURE) {
+		return res | videobuf_poll_stream(filp, &fh->vb_vidq, wait);
+	}
+	if (fh->type == V4L2_BUF_TYPE_VBI_CAPTURE) {
 		if (!res_get(fh, AU0828_RESOURCE_VBI))
 			return POLLERR;
-		return videobuf_poll_stream(filp, &fh->vb_vbiq, wait);
-	} else {
-		return POLLERR;
+		return res | videobuf_poll_stream(filp, &fh->vb_vbiq, wait);
 	}
+	return POLLERR;
 }
 
 static int au0828_v4l2_mmap(struct file *filp, struct vm_area_struct *vma)
@@ -1172,9 +1214,6 @@ static int au0828_set_format(struct au0828_dev *dev, unsigned int cmd,
 	int width = format->fmt.pix.width;
 	int height = format->fmt.pix.height;
 
-	if (format->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
 	/* If they are demanding a format other than the one we support,
 	   bail out (tvtime asks for UYVY and then retries with YUYV) */
 	if (format->fmt.pix.pixelformat != V4L2_PIX_FMT_UYVY)
@@ -1193,6 +1232,7 @@ static int au0828_set_format(struct au0828_dev *dev, unsigned int cmd,
 	format->fmt.pix.sizeimage = width * height * 2;
 	format->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 	format->fmt.pix.field = V4L2_FIELD_INTERLACED;
+	format->fmt.pix.priv = 0;
 
 	if (cmd == VIDIOC_TRY_FMT)
 		return 0;
@@ -1226,35 +1266,28 @@ static int au0828_set_format(struct au0828_dev *dev, unsigned int cmd,
 }
 
 
-static int vidioc_queryctrl(struct file *file, void *priv,
-			    struct v4l2_queryctrl *qc)
-{
-	struct au0828_fh *fh = priv;
-	struct au0828_dev *dev = fh->dev;
-	v4l2_device_call_all(&dev->v4l2_dev, 0, core, queryctrl, qc);
-	if (qc->type)
-		return 0;
-	else
-		return -EINVAL;
-}
-
 static int vidioc_querycap(struct file *file, void  *priv,
 			   struct v4l2_capability *cap)
 {
-	struct au0828_fh *fh  = priv;
+	struct video_device *vdev = video_devdata(file);
+	struct au0828_fh *fh = priv;
 	struct au0828_dev *dev = fh->dev;
 
 	strlcpy(cap->driver, "au0828", sizeof(cap->driver));
 	strlcpy(cap->card, dev->board.name, sizeof(cap->card));
-	strlcpy(cap->bus_info, dev->v4l2_dev.name, sizeof(cap->bus_info));
+	usb_make_path(dev->usbdev, cap->bus_info, sizeof(cap->bus_info));
 
-	/*set the device capabilities */
-	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE |
-		V4L2_CAP_VBI_CAPTURE |
-		V4L2_CAP_AUDIO |
+	/* set the device capabilities */
+	cap->device_caps = V4L2_CAP_AUDIO |
 		V4L2_CAP_READWRITE |
 		V4L2_CAP_STREAMING |
 		V4L2_CAP_TUNER;
+	if (vdev->vfl_type == VFL_TYPE_GRABBER)
+		cap->device_caps |= V4L2_CAP_VIDEO_CAPTURE;
+	else
+		cap->device_caps |= V4L2_CAP_VBI_CAPTURE;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS |
+		V4L2_CAP_VBI_CAPTURE | V4L2_CAP_VIDEO_CAPTURE;
 	return 0;
 }
 
@@ -1286,6 +1319,7 @@ static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 	f->fmt.pix.sizeimage = dev->frame_size;
 	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M; /* NTSC/PAL */
 	f->fmt.pix.field = V4L2_FIELD_INTERLACED;
+	f->fmt.pix.priv = 0;
 	return 0;
 }
 
@@ -1320,24 +1354,34 @@ out:
 	return rc;
 }
 
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id * norm)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id norm)
 {
 	struct au0828_fh *fh = priv;
 	struct au0828_dev *dev = fh->dev;
 
-	if (dev->dvb.frontend && dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl)
-		dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl(dev->dvb.frontend, 1);
+	dev->std = norm;
+
+	au0828_init_tuner(dev);
+
+	i2c_gate_ctrl(dev, 1);
 
 	/* FIXME: when we support something other than NTSC, we are going to
 	   have to make the au0828 bridge adjust the size of its capture
 	   buffer, which is currently hardcoded at 720x480 */
 
-	v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_std, *norm);
-	dev->std_set_in_tuner_core = 1;
+	v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_std, norm);
 
-	if (dev->dvb.frontend && dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl)
-		dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl(dev->dvb.frontend, 0);
+	i2c_gate_ctrl(dev, 0);
+
+	return 0;
+}
+
+static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *norm)
+{
+	struct au0828_fh *fh = priv;
+	struct au0828_dev *dev = fh->dev;
 
+	*norm = dev->std;
 	return 0;
 }
 
@@ -1368,10 +1412,13 @@ static int vidioc_enum_input(struct file *file, void *priv,
 	input->index = tmp;
 	strcpy(input->name, inames[AUVI_INPUT(tmp).type]);
 	if ((AUVI_INPUT(tmp).type == AU0828_VMUX_TELEVISION) ||
-	    (AUVI_INPUT(tmp).type == AU0828_VMUX_CABLE))
+	    (AUVI_INPUT(tmp).type == AU0828_VMUX_CABLE)) {
 		input->type |= V4L2_INPUT_TYPE_TUNER;
-	else
+		input->audioset = 1;
+	} else {
 		input->type |= V4L2_INPUT_TYPE_CAMERA;
+		input->audioset = 2;
+	}
 
 	input->std = dev->vdev->tvnorms;
 
@@ -1386,32 +1433,25 @@ static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
 	return 0;
 }
 
-static int vidioc_s_input(struct file *file, void *priv, unsigned int index)
+static void au0828_s_input(struct au0828_dev *dev, int index)
 {
-	struct au0828_fh *fh = priv;
-	struct au0828_dev *dev = fh->dev;
 	int i;
 
-	dprintk(1, "VIDIOC_S_INPUT in function %s, input=%d\n", __func__,
-		index);
-	if (index >= AU0828_MAX_INPUT)
-		return -EINVAL;
-	if (AUVI_INPUT(index).type == 0)
-		return -EINVAL;
-	dev->ctrl_input = index;
-
 	switch (AUVI_INPUT(index).type) {
 	case AU0828_VMUX_SVIDEO:
 		dev->input_type = AU0828_VMUX_SVIDEO;
+		dev->ctrl_ainput = 1;
 		break;
 	case AU0828_VMUX_COMPOSITE:
 		dev->input_type = AU0828_VMUX_COMPOSITE;
+		dev->ctrl_ainput = 1;
 		break;
 	case AU0828_VMUX_TELEVISION:
 		dev->input_type = AU0828_VMUX_TELEVISION;
+		dev->ctrl_ainput = 0;
 		break;
 	default:
-		dprintk(1, "VIDIOC_S_INPUT unknown input type set [%d]\n",
+		dprintk(1, "unknown input type set [%d]\n",
 			AUVI_INPUT(index).type);
 		break;
 	}
@@ -1442,55 +1482,60 @@ static int vidioc_s_input(struct file *file, void *priv, unsigned int index)
 
 	v4l2_device_call_all(&dev->v4l2_dev, 0, audio, s_routing,
 			AUVI_INPUT(index).amux, 0, 0);
-	return 0;
 }
 
-static int vidioc_g_audio(struct file *file, void *priv, struct v4l2_audio *a)
+static int vidioc_s_input(struct file *file, void *priv, unsigned int index)
 {
 	struct au0828_fh *fh = priv;
 	struct au0828_dev *dev = fh->dev;
-	unsigned int index = a->index;
 
+	dprintk(1, "VIDIOC_S_INPUT in function %s, input=%d\n", __func__,
+		index);
+	if (index >= AU0828_MAX_INPUT)
+		return -EINVAL;
+	if (AUVI_INPUT(index).type == 0)
+		return -EINVAL;
+	dev->ctrl_input = index;
+	au0828_s_input(dev, index);
+	return 0;
+}
+
+static int vidioc_enumaudio(struct file *file, void *priv, struct v4l2_audio *a)
+{
 	if (a->index > 1)
 		return -EINVAL;
 
-	index = dev->ctrl_ainput;
-	if (index == 0)
+	if (a->index == 0)
 		strcpy(a->name, "Television");
 	else
 		strcpy(a->name, "Line in");
 
 	a->capability = V4L2_AUDCAP_STEREO;
-	a->index = index;
 	return 0;
 }
 
-static int vidioc_s_audio(struct file *file, void *priv, const struct v4l2_audio *a)
+static int vidioc_g_audio(struct file *file, void *priv, struct v4l2_audio *a)
 {
 	struct au0828_fh *fh = priv;
 	struct au0828_dev *dev = fh->dev;
-	if (a->index != dev->ctrl_ainput)
-		return -EINVAL;
-	return 0;
-}
 
-static int vidioc_g_ctrl(struct file *file, void *priv,
-			 struct v4l2_control *ctrl)
-{
-	struct au0828_fh *fh = priv;
-	struct au0828_dev *dev = fh->dev;
+	a->index = dev->ctrl_ainput;
+	if (a->index == 0)
+		strcpy(a->name, "Television");
+	else
+		strcpy(a->name, "Line in");
 
-	v4l2_device_call_all(&dev->v4l2_dev, 0, core, g_ctrl, ctrl);
+	a->capability = V4L2_AUDCAP_STEREO;
 	return 0;
-
 }
 
-static int vidioc_s_ctrl(struct file *file, void *priv,
-				struct v4l2_control *ctrl)
+static int vidioc_s_audio(struct file *file, void *priv, const struct v4l2_audio *a)
 {
 	struct au0828_fh *fh = priv;
 	struct au0828_dev *dev = fh->dev;
-	v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_ctrl, ctrl);
+
+	if (a->index != dev->ctrl_ainput)
+		return -EINVAL;
 	return 0;
 }
 
@@ -1503,12 +1548,16 @@ static int vidioc_g_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
 		return -EINVAL;
 
 	strcpy(t->name, "Auvitek tuner");
+
+	au0828_init_tuner(dev);
+	i2c_gate_ctrl(dev, 1);
 	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, g_tuner, t);
+	i2c_gate_ctrl(dev, 0);
 	return 0;
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *t)
+				const struct v4l2_tuner *t)
 {
 	struct au0828_fh *fh = priv;
 	struct au0828_dev *dev = fh->dev;
@@ -1516,15 +1565,10 @@ static int vidioc_s_tuner(struct file *file, void *priv,
 	if (t->index != 0)
 		return -EINVAL;
 
-	t->type = V4L2_TUNER_ANALOG_TV;
-
-	if (dev->dvb.frontend && dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl)
-		dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl(dev->dvb.frontend, 1);
-
+	au0828_init_tuner(dev);
+	i2c_gate_ctrl(dev, 1);
 	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_tuner, t);
-
-	if (dev->dvb.frontend && dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl)
-		dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl(dev->dvb.frontend, 0);
+	i2c_gate_ctrl(dev, 0);
 
 	dprintk(1, "VIDIOC_S_TUNER: signal = %x, afc = %x\n", t->signal,
 		t->afc);
@@ -1539,40 +1583,31 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 	struct au0828_fh *fh = priv;
 	struct au0828_dev *dev = fh->dev;
 
-	freq->type = V4L2_TUNER_ANALOG_TV;
+	if (freq->tuner != 0)
+		return -EINVAL;
 	freq->frequency = dev->ctrl_freq;
 	return 0;
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *freq)
+				const struct v4l2_frequency *freq)
 {
 	struct au0828_fh *fh = priv;
 	struct au0828_dev *dev = fh->dev;
+	struct v4l2_frequency new_freq = *freq;
 
 	if (freq->tuner != 0)
 		return -EINVAL;
-	if (freq->type != V4L2_TUNER_ANALOG_TV)
-		return -EINVAL;
-
-	dev->ctrl_freq = freq->frequency;
 
-	if (dev->dvb.frontend && dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl)
-		dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl(dev->dvb.frontend, 1);
-
-	if (dev->std_set_in_tuner_core == 0) {
-	  /* If we've never sent the standard in tuner core, do so now.  We
-	     don't do this at device probe because we don't want to incur
-	     the cost of a firmware load */
-	  v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_std,
-			       dev->vdev->tvnorms);
-	  dev->std_set_in_tuner_core = 1;
-	}
+	au0828_init_tuner(dev);
+	i2c_gate_ctrl(dev, 1);
 
 	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_frequency, freq);
+	/* Get the actual set (and possibly clamped) frequency */
+	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, g_frequency, &new_freq);
+	dev->ctrl_freq = new_freq.frequency;
 
-	if (dev->dvb.frontend && dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl)
-		dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl(dev->dvb.frontend, 0);
+	i2c_gate_ctrl(dev, 0);
 
 	au0828_analog_stream_reset(dev);
 
@@ -1598,6 +1633,7 @@ static int vidioc_g_fmt_vbi_cap(struct file *file, void *priv,
 	format->fmt.vbi.count[1] = dev->vbi_height;
 	format->fmt.vbi.start[0] = 21;
 	format->fmt.vbi.start[1] = 284;
+	memset(format->fmt.vbi.reserved, 0, sizeof(format->fmt.vbi.reserved));
 
 	return 0;
 }
@@ -1664,6 +1700,7 @@ static int vidioc_streamon(struct file *file, void *priv,
 	if (unlikely(!res_get(fh, get_ressource(fh))))
 		return -EBUSY;
 
+	au0828_init_tuner(dev);
 	if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
 		au0828_analog_stream_enable(dev);
 		v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_stream, 1);
@@ -1756,7 +1793,7 @@ static int vidioc_g_register(struct file *file, void *priv,
 }
 
 static int vidioc_s_register(struct file *file, void *priv,
-			     struct v4l2_dbg_register *reg)
+			     const struct v4l2_dbg_register *reg)
 {
 	struct au0828_fh *fh = priv;
 	struct au0828_dev *dev = fh->dev;
@@ -1773,6 +1810,15 @@ static int vidioc_s_register(struct file *file, void *priv,
 }
 #endif
 
+static int vidioc_log_status(struct file *file, void *fh)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	v4l2_ctrl_log_status(file, fh);
+	v4l2_device_call_all(vdev->v4l2_dev, 0, core, log_status);
+	return 0;
+}
+
 static int vidioc_reqbufs(struct file *file, void *priv,
 			  struct v4l2_requestbuffers *rb)
 {
@@ -1872,7 +1918,9 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_try_fmt_vid_cap     = vidioc_try_fmt_vid_cap,
 	.vidioc_s_fmt_vid_cap       = vidioc_s_fmt_vid_cap,
 	.vidioc_g_fmt_vbi_cap       = vidioc_g_fmt_vbi_cap,
+	.vidioc_try_fmt_vbi_cap     = vidioc_g_fmt_vbi_cap,
 	.vidioc_s_fmt_vbi_cap       = vidioc_g_fmt_vbi_cap,
+	.vidioc_enumaudio           = vidioc_enumaudio,
 	.vidioc_g_audio             = vidioc_g_audio,
 	.vidioc_s_audio             = vidioc_s_audio,
 	.vidioc_cropcap             = vidioc_cropcap,
@@ -1881,12 +1929,10 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_qbuf                = vidioc_qbuf,
 	.vidioc_dqbuf               = vidioc_dqbuf,
 	.vidioc_s_std               = vidioc_s_std,
+	.vidioc_g_std               = vidioc_g_std,
 	.vidioc_enum_input          = vidioc_enum_input,
 	.vidioc_g_input             = vidioc_g_input,
 	.vidioc_s_input             = vidioc_s_input,
-	.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_tuner             = vidioc_g_tuner,
@@ -1898,6 +1944,9 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_s_register          = vidioc_s_register,
 #endif
 	.vidioc_g_chip_ident        = vidioc_g_chip_ident,
+	.vidioc_log_status	    = vidioc_log_status,
+	.vidioc_subscribe_event     = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event   = v4l2_event_unsubscribe,
 };
 
 static const struct video_device au0828_video_template = {
@@ -1905,7 +1954,6 @@ static const struct video_device au0828_video_template = {
 	.release                    = video_device_release,
 	.ioctl_ops 		    = &video_ioctl_ops,
 	.tvnorms                    = V4L2_STD_NTSC_M,
-	.current_norm               = V4L2_STD_NTSC_M,
 };
 
 /**************************************************************************/
@@ -1972,7 +2020,12 @@ int au0828_analog_register(struct au0828_dev *dev,
 	dev->field_size = dev->width * dev->height;
 	dev->frame_size = dev->field_size << 1;
 	dev->bytesperline = dev->width << 1;
+	dev->vbi_width = 720;
+	dev->vbi_height = 1;
 	dev->ctrl_ainput = 0;
+	dev->ctrl_freq = 960;
+	dev->std = V4L2_STD_NTSC_M;
+	au0828_s_input(dev, 0);
 
 	/* allocate and fill v4l2 video struct */
 	dev->vdev = video_device_alloc();
@@ -1991,14 +2044,16 @@ int au0828_analog_register(struct au0828_dev *dev,
 
 	/* Fill the video capture device struct */
 	*dev->vdev = au0828_video_template;
-	dev->vdev->parent = &dev->usbdev->dev;
+	dev->vdev->v4l2_dev = &dev->v4l2_dev;
 	dev->vdev->lock = &dev->lock;
+	set_bit(V4L2_FL_USE_FH_PRIO, &dev->vdev->flags);
 	strcpy(dev->vdev->name, "au0828a video");
 
 	/* Setup the VBI device */
 	*dev->vbi_dev = au0828_video_template;
-	dev->vbi_dev->parent = &dev->usbdev->dev;
+	dev->vbi_dev->v4l2_dev = &dev->v4l2_dev;
 	dev->vbi_dev->lock = &dev->lock;
+	set_bit(V4L2_FL_USE_FH_PRIO, &dev->vbi_dev->flags);
 	strcpy(dev->vbi_dev->name, "au0828a vbi");
 
 	/* Register the v4l2 device */
diff --git a/drivers/media/usb/au0828/au0828.h b/drivers/media/usb/au0828/au0828.h
index e579ff69ca4..ef1f57f22be 100644
--- a/drivers/media/usb/au0828/au0828.h
+++ b/drivers/media/usb/au0828/au0828.h
@@ -28,6 +28,8 @@
 #include <linux/videodev2.h>
 #include <media/videobuf-vmalloc.h>
 #include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fh.h>
 
 /* DVB */
 #include "demux.h"
@@ -118,6 +120,9 @@ enum au0828_dev_state {
 };
 
 struct au0828_fh {
+	/* must be the first field of this struct! */
+	struct v4l2_fh fh;
+
 	struct au0828_dev *dev;
 	unsigned int  resources;
 
@@ -202,6 +207,7 @@ struct au0828_dev {
 #ifdef CONFIG_VIDEO_AU0828_V4L2
 	/* Analog */
 	struct v4l2_device v4l2_dev;
+	struct v4l2_ctrl_handler v4l2_ctrl_hdl;
 #endif
 	int users;
 	unsigned int resources;	/* resources in use */
@@ -216,6 +222,7 @@ struct au0828_dev {
 	int vbi_width;
 	int vbi_height;
 	u32 vbi_read;
+	v4l2_std_id std;
 	u32 field_size;
 	u32 frame_size;
 	u32 bytesperline;
diff --git a/drivers/media/usb/cx231xx/cx231xx-417.c b/drivers/media/usb/cx231xx/cx231xx-417.c
index 28688dbcb60..f548db8043d 100644
--- a/drivers/media/usb/cx231xx/cx231xx-417.c
+++ b/drivers/media/usb/cx231xx/cx231xx-417.c
@@ -34,11 +34,12 @@
 #include <linux/vmalloc.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
 #include <media/cx2341x.h>
+#include <media/tuner.h>
 #include <linux/usb.h>
 
 #include "cx231xx.h"
-/*#include "cx23885-ioctl.h"*/
 
 #define CX231xx_FIRM_IMAGE_SIZE 376836
 #define CX231xx_FIRM_IMAGE_NAME "v4l-cx23885-enc.fw"
@@ -75,9 +76,11 @@
 static unsigned int mpegbufs = 8;
 module_param(mpegbufs, int, 0644);
 MODULE_PARM_DESC(mpegbufs, "number of mpeg buffers, range 2-32");
+
 static unsigned int mpeglines = 128;
 module_param(mpeglines, int, 0644);
 MODULE_PARM_DESC(mpeglines, "number of lines in an MPEG buffer, range 2-32");
+
 static unsigned int mpeglinesize = 512;
 module_param(mpeglinesize, int, 0644);
 MODULE_PARM_DESC(mpeglinesize,
@@ -86,10 +89,10 @@ MODULE_PARM_DESC(mpeglinesize,
 static unsigned int v4l_debug = 1;
 module_param(v4l_debug, int, 0644);
 MODULE_PARM_DESC(v4l_debug, "enable V4L debug messages");
-struct cx231xx_dmaqueue *dma_qq;
+
 #define dprintk(level, fmt, arg...)\
 	do { if (v4l_debug >= level) \
-		printk(KERN_INFO "%s: " fmt, \
+		pr_info("%s: " fmt, \
 		(dev) ? dev->name : "cx231xx[?]", ## arg); \
 	} while (0)
 
@@ -131,11 +134,13 @@ static struct cx231xx_tvnorm cx231xx_tvnorms[] = {
 };
 
 /* ------------------------------------------------------------------ */
+
 enum cx231xx_capture_type {
 	CX231xx_MPEG_CAPTURE,
 	CX231xx_RAW_CAPTURE,
 	CX231xx_RAW_PASSTHRU_CAPTURE
 };
+
 enum cx231xx_capture_bits {
 	CX231xx_RAW_BITS_NONE             = 0x00,
 	CX231xx_RAW_BITS_YUV_CAPTURE      = 0x01,
@@ -144,33 +149,40 @@ enum cx231xx_capture_bits {
 	CX231xx_RAW_BITS_PASSTHRU_CAPTURE = 0x08,
 	CX231xx_RAW_BITS_TO_HOST_CAPTURE  = 0x10
 };
+
 enum cx231xx_capture_end {
 	CX231xx_END_AT_GOP, /* stop at the end of gop, generate irq */
 	CX231xx_END_NOW, /* stop immediately, no irq */
 };
+
 enum cx231xx_framerate {
 	CX231xx_FRAMERATE_NTSC_30, /* NTSC: 30fps */
 	CX231xx_FRAMERATE_PAL_25   /* PAL: 25fps */
 };
+
 enum cx231xx_stream_port {
 	CX231xx_OUTPUT_PORT_MEMORY,
 	CX231xx_OUTPUT_PORT_STREAMING,
 	CX231xx_OUTPUT_PORT_SERIAL
 };
+
 enum cx231xx_data_xfer_status {
 	CX231xx_MORE_BUFFERS_FOLLOW,
 	CX231xx_LAST_BUFFER,
 };
+
 enum cx231xx_picture_mask {
 	CX231xx_PICTURE_MASK_NONE,
 	CX231xx_PICTURE_MASK_I_FRAMES,
 	CX231xx_PICTURE_MASK_I_P_FRAMES = 0x3,
 	CX231xx_PICTURE_MASK_ALL_FRAMES = 0x7,
 };
+
 enum cx231xx_vbi_mode_bits {
 	CX231xx_VBI_BITS_SLICED,
 	CX231xx_VBI_BITS_RAW,
 };
+
 enum cx231xx_vbi_insertion_bits {
 	CX231xx_VBI_BITS_INSERT_IN_XTENSION_USR_DATA,
 	CX231xx_VBI_BITS_INSERT_IN_PRIVATE_PACKETS = 0x1 << 1,
@@ -178,56 +190,69 @@ enum cx231xx_vbi_insertion_bits {
 	CX231xx_VBI_BITS_SEPARATE_STREAM_USR_DATA = 0x4 << 1,
 	CX231xx_VBI_BITS_SEPARATE_STREAM_PRV_DATA = 0x5 << 1,
 };
+
 enum cx231xx_dma_unit {
 	CX231xx_DMA_BYTES,
 	CX231xx_DMA_FRAMES,
 };
+
 enum cx231xx_dma_transfer_status_bits {
 	CX231xx_DMA_TRANSFER_BITS_DONE = 0x01,
 	CX231xx_DMA_TRANSFER_BITS_ERROR = 0x04,
 	CX231xx_DMA_TRANSFER_BITS_LL_ERROR = 0x10,
 };
+
 enum cx231xx_pause {
 	CX231xx_PAUSE_ENCODING,
 	CX231xx_RESUME_ENCODING,
 };
+
 enum cx231xx_copyright {
 	CX231xx_COPYRIGHT_OFF,
 	CX231xx_COPYRIGHT_ON,
 };
+
 enum cx231xx_notification_type {
 	CX231xx_NOTIFICATION_REFRESH,
 };
+
 enum cx231xx_notification_status {
 	CX231xx_NOTIFICATION_OFF,
 	CX231xx_NOTIFICATION_ON,
 };
+
 enum cx231xx_notification_mailbox {
 	CX231xx_NOTIFICATION_NO_MAILBOX = -1,
 };
+
 enum cx231xx_field1_lines {
 	CX231xx_FIELD1_SAA7114 = 0x00EF, /* 239 */
 	CX231xx_FIELD1_SAA7115 = 0x00F0, /* 240 */
 	CX231xx_FIELD1_MICRONAS = 0x0105, /* 261 */
 };
+
 enum cx231xx_field2_lines {
 	CX231xx_FIELD2_SAA7114 = 0x00EF, /* 239 */
 	CX231xx_FIELD2_SAA7115 = 0x00F0, /* 240 */
 	CX231xx_FIELD2_MICRONAS = 0x0106, /* 262 */
 };
+
 enum cx231xx_custom_data_type {
 	CX231xx_CUSTOM_EXTENSION_USR_DATA,
 	CX231xx_CUSTOM_PRIVATE_PACKET,
 };
+
 enum cx231xx_mute {
 	CX231xx_UNMUTE,
 	CX231xx_MUTE,
 };
+
 enum cx231xx_mute_video_mask {
 	CX231xx_MUTE_VIDEO_V_MASK = 0x0000FF00,
 	CX231xx_MUTE_VIDEO_U_MASK = 0x00FF0000,
 	CX231xx_MUTE_VIDEO_Y_MASK = 0xFF000000,
 };
+
 enum cx231xx_mute_video_shift {
 	CX231xx_MUTE_VIDEO_V_SHIFT = 8,
 	CX231xx_MUTE_VIDEO_U_SHIFT = 16,
@@ -296,41 +321,43 @@ enum cx231xx_mute_video_shift {
 
 
 #define CX23417_GPIO_MASK 0xFC0003FF
-static int setITVCReg(struct cx231xx *dev, u32 gpio_direction, u32 value)
+
+static int set_itvc_reg(struct cx231xx *dev, u32 gpio_direction, u32 value)
 {
 	int status = 0;
 	u32 _gpio_direction = 0;
 
 	_gpio_direction = _gpio_direction & CX23417_GPIO_MASK;
-	_gpio_direction = _gpio_direction|gpio_direction;
+	_gpio_direction = _gpio_direction | gpio_direction;
 	status = cx231xx_send_gpio_cmd(dev, _gpio_direction,
 			 (u8 *)&value, 4, 0, 0);
 	return status;
 }
-static int getITVCReg(struct cx231xx *dev, u32 gpio_direction, u32 *pValue)
+
+static int get_itvc_reg(struct cx231xx *dev, u32 gpio_direction, u32 *val_ptr)
 {
 	int status = 0;
 	u32 _gpio_direction = 0;
 
 	_gpio_direction = _gpio_direction & CX23417_GPIO_MASK;
-	_gpio_direction = _gpio_direction|gpio_direction;
+	_gpio_direction = _gpio_direction | gpio_direction;
 
 	status = cx231xx_send_gpio_cmd(dev, _gpio_direction,
-		 (u8 *)pValue, 4, 0, 1);
+		 (u8 *)val_ptr, 4, 0, 1);
 	return status;
 }
 
-static int waitForMciComplete(struct cx231xx *dev)
+static int wait_for_mci_complete(struct cx231xx *dev)
 {
 	u32 gpio;
-	u32 gpio_driection = 0;
+	u32 gpio_direction = 0;
 	u8 count = 0;
-	getITVCReg(dev, gpio_driection, &gpio);
+	get_itvc_reg(dev, gpio_direction, &gpio);
 
 	while (!(gpio&0x020000)) {
 		msleep(10);
 
-		getITVCReg(dev, gpio_driection, &gpio);
+		get_itvc_reg(dev, gpio_direction, &gpio);
 
 		if (count++ > 100) {
 			dprintk(3, "ERROR: Timeout - gpio=%x\n", gpio);
@@ -345,57 +372,57 @@ static int mc417_register_write(struct cx231xx *dev, u16 address, u32 value)
 	u32 temp;
 	int status = 0;
 
-	temp = 0x82|MCI_REGISTER_DATA_BYTE0|((value&0x000000FF)<<8);
-	temp = temp<<10;
-	status = setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_REGISTER_DATA_BYTE0 | ((value & 0x000000FF) << 8);
+	temp = temp << 10;
+	status = set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 	if (status < 0)
 		return status;
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*write data byte 1;*/
-	temp = 0x82|MCI_REGISTER_DATA_BYTE1|(value&0x0000FF00);
-	temp = temp<<10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_REGISTER_DATA_BYTE1 | (value & 0x0000FF00);
+	temp = temp << 10;
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*write data byte 2;*/
-	temp = 0x82|MCI_REGISTER_DATA_BYTE2|((value&0x00FF0000)>>8);
-	temp = temp<<10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_REGISTER_DATA_BYTE2 | ((value & 0x00FF0000) >> 8);
+	temp = temp << 10;
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*write data byte 3;*/
-	temp = 0x82|MCI_REGISTER_DATA_BYTE3|((value&0xFF000000)>>16);
-	temp = temp<<10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_REGISTER_DATA_BYTE3 | ((value & 0xFF000000) >> 16);
+	temp = temp << 10;
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*write address byte 0;*/
-	temp = 0x82|MCI_REGISTER_ADDRESS_BYTE0|((address&0x000000FF)<<8);
-	temp = temp<<10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_REGISTER_ADDRESS_BYTE0 | ((address & 0x000000FF) << 8);
+	temp = temp << 10;
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*write address byte 1;*/
-	temp = 0x82|MCI_REGISTER_ADDRESS_BYTE1|(address&0x0000FF00);
-	temp = temp<<10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_REGISTER_ADDRESS_BYTE1 | (address & 0x0000FF00);
+	temp = temp << 10;
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*Write that the mode is write.*/
 	temp = 0x82 | MCI_REGISTER_MODE | MCI_MODE_REGISTER_WRITE;
-	temp = temp<<10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp << 10;
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
-	return waitForMciComplete(dev);
+	return wait_for_mci_complete(dev);
 }
 
 static int mc417_register_read(struct cx231xx *dev, u16 address, u32 *value)
@@ -407,70 +434,68 @@ static int mc417_register_read(struct cx231xx *dev, u16 address, u32 *value)
 
 	temp = 0x82 | MCI_REGISTER_ADDRESS_BYTE0 | ((address & 0x00FF) << 8);
 	temp = temp << 10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 	temp = temp | ((0x05) << 10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*write address byte 1;*/
 	temp = 0x82 | MCI_REGISTER_ADDRESS_BYTE1 | (address & 0xFF00);
 	temp = temp << 10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 	temp = temp | ((0x05) << 10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*write that the mode is read;*/
 	temp = 0x82 | MCI_REGISTER_MODE | MCI_MODE_REGISTER_READ;
 	temp = temp << 10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 	temp = temp | ((0x05) << 10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*wait for the MIRDY line to be asserted ,
 	signalling that the read is done;*/
-	ret = waitForMciComplete(dev);
+	ret = wait_for_mci_complete(dev);
 
 	/*switch the DATA- GPIO to input mode;*/
 
 	/*Read data byte 0;*/
 	temp = (0x82 | MCI_REGISTER_DATA_BYTE0) << 10;
-	setITVCReg(dev, ITVC_READ_DIR, temp);
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
 	temp = ((0x81 | MCI_REGISTER_DATA_BYTE0) << 10);
-	setITVCReg(dev, ITVC_READ_DIR, temp);
-	getITVCReg(dev, ITVC_READ_DIR, &temp);
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
+	get_itvc_reg(dev, ITVC_READ_DIR, &temp);
 	return_value |= ((temp & 0x03FC0000) >> 18);
-	setITVCReg(dev, ITVC_READ_DIR, (0x87 << 10));
+	set_itvc_reg(dev, ITVC_READ_DIR, (0x87 << 10));
 
 	/* Read data byte 1;*/
 	temp = (0x82 | MCI_REGISTER_DATA_BYTE1) << 10;
-	setITVCReg(dev, ITVC_READ_DIR, temp);
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
 	temp = ((0x81 | MCI_REGISTER_DATA_BYTE1) << 10);
-	setITVCReg(dev, ITVC_READ_DIR, temp);
-	getITVCReg(dev, ITVC_READ_DIR, &temp);
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
+	get_itvc_reg(dev, ITVC_READ_DIR, &temp);
 
 	return_value |= ((temp & 0x03FC0000) >> 10);
-	setITVCReg(dev, ITVC_READ_DIR, (0x87 << 10));
+	set_itvc_reg(dev, ITVC_READ_DIR, (0x87 << 10));
 
 	/*Read data byte 2;*/
 	temp = (0x82 | MCI_REGISTER_DATA_BYTE2) << 10;
-	setITVCReg(dev, ITVC_READ_DIR, temp);
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
 	temp = ((0x81 | MCI_REGISTER_DATA_BYTE2) << 10);
-	setITVCReg(dev, ITVC_READ_DIR, temp);
-	getITVCReg(dev, ITVC_READ_DIR, &temp);
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
+	get_itvc_reg(dev, ITVC_READ_DIR, &temp);
 	return_value |= ((temp & 0x03FC0000) >> 2);
-	setITVCReg(dev, ITVC_READ_DIR, (0x87 << 10));
+	set_itvc_reg(dev, ITVC_READ_DIR, (0x87 << 10));
 
 	/*Read data byte 3;*/
 	temp = (0x82 | MCI_REGISTER_DATA_BYTE3) << 10;
-	setITVCReg(dev, ITVC_READ_DIR, temp);
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
 	temp = ((0x81 | MCI_REGISTER_DATA_BYTE3) << 10);
-	setITVCReg(dev, ITVC_READ_DIR, temp);
-	getITVCReg(dev, ITVC_READ_DIR, &temp);
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
+	get_itvc_reg(dev, ITVC_READ_DIR, &temp);
 	return_value |= ((temp & 0x03FC0000) << 6);
-	setITVCReg(dev, ITVC_READ_DIR, (0x87 << 10));
+	set_itvc_reg(dev, ITVC_READ_DIR, (0x87 << 10));
 
 	*value  = return_value;
-
-
 	return ret;
 }
 
@@ -481,59 +506,59 @@ static int mc417_memory_write(struct cx231xx *dev, u32 address, u32 value)
 	u32 temp;
 	int ret = 0;
 
-	temp = 0x82 | MCI_MEMORY_DATA_BYTE0|((value & 0x000000FF) << 8);
+	temp = 0x82 | MCI_MEMORY_DATA_BYTE0 | ((value & 0x000000FF) << 8);
 	temp = temp << 10;
-	ret = setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	ret = set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 	if (ret < 0)
 		return ret;
-	temp = temp | ((0x05) << 10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*write data byte 1;*/
 	temp = 0x82 | MCI_MEMORY_DATA_BYTE1 | (value & 0x0000FF00);
 	temp = temp << 10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp | ((0x05) << 10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*write data byte 2;*/
-	temp = 0x82|MCI_MEMORY_DATA_BYTE2|((value&0x00FF0000)>>8);
-	temp = temp<<10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_MEMORY_DATA_BYTE2 | ((value & 0x00FF0000) >> 8);
+	temp = temp << 10;
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*write data byte 3;*/
-	temp = 0x82|MCI_MEMORY_DATA_BYTE3|((value&0xFF000000)>>16);
-	temp = temp<<10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_MEMORY_DATA_BYTE3 | ((value & 0xFF000000) >> 16);
+	temp = temp << 10;
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/* write address byte 2;*/
-	temp = 0x82|MCI_MEMORY_ADDRESS_BYTE2 | MCI_MODE_MEMORY_WRITE |
-		((address & 0x003F0000)>>8);
-	temp = temp<<10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_MEMORY_ADDRESS_BYTE2 | MCI_MODE_MEMORY_WRITE |
+		((address & 0x003F0000) >> 8);
+	temp = temp << 10;
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/* write address byte 1;*/
-	temp = 0x82|MCI_MEMORY_ADDRESS_BYTE1 | (address & 0xFF00);
-	temp = temp<<10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_MEMORY_ADDRESS_BYTE1 | (address & 0xFF00);
+	temp = temp << 10;
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/* write address byte 0;*/
-	temp = 0x82|MCI_MEMORY_ADDRESS_BYTE0|((address & 0x00FF)<<8);
-	temp = temp<<10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_MEMORY_ADDRESS_BYTE0 | ((address & 0x00FF) << 8);
+	temp = temp << 10;
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*wait for MIRDY line;*/
-	waitForMciComplete(dev);
+	wait_for_mci_complete(dev);
 
 	return 0;
 }
@@ -545,68 +570,68 @@ static int mc417_memory_read(struct cx231xx *dev, u32 address, u32 *value)
 	int ret = 0;
 
 	/*write address byte 2;*/
-	temp = 0x82|MCI_MEMORY_ADDRESS_BYTE2 | MCI_MODE_MEMORY_READ |
-		((address & 0x003F0000)>>8);
-	temp = temp<<10;
-	ret = setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_MEMORY_ADDRESS_BYTE2 | MCI_MODE_MEMORY_READ |
+		((address & 0x003F0000) >> 8);
+	temp = temp << 10;
+	ret = set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 	if (ret < 0)
 		return ret;
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*write address byte 1*/
-	temp = 0x82|MCI_MEMORY_ADDRESS_BYTE1 | (address & 0xFF00);
-	temp = temp<<10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_MEMORY_ADDRESS_BYTE1 | (address & 0xFF00);
+	temp = temp << 10;
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*write address byte 0*/
-	temp = 0x82|MCI_MEMORY_ADDRESS_BYTE0 | ((address & 0x00FF)<<8);
-	temp = temp<<10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_MEMORY_ADDRESS_BYTE0 | ((address & 0x00FF) << 8);
+	temp = temp << 10;
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*Wait for MIRDY line*/
-	ret = waitForMciComplete(dev);
+	ret = wait_for_mci_complete(dev);
 
 
 	/*Read data byte 3;*/
-	temp = (0x82|MCI_MEMORY_DATA_BYTE3)<<10;
-	setITVCReg(dev, ITVC_READ_DIR, temp);
-	temp = ((0x81|MCI_MEMORY_DATA_BYTE3)<<10);
-	setITVCReg(dev, ITVC_READ_DIR, temp);
-	getITVCReg(dev, ITVC_READ_DIR, &temp);
-	return_value |= ((temp&0x03FC0000)<<6);
-	setITVCReg(dev, ITVC_READ_DIR, (0x87<<10));
+	temp = (0x82 | MCI_MEMORY_DATA_BYTE3) << 10;
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
+	temp = ((0x81 | MCI_MEMORY_DATA_BYTE3) << 10);
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
+	get_itvc_reg(dev, ITVC_READ_DIR, &temp);
+	return_value |= ((temp & 0x03FC0000) << 6);
+	set_itvc_reg(dev, ITVC_READ_DIR, (0x87 << 10));
 
 	/*Read data byte 2;*/
-	temp = (0x82|MCI_MEMORY_DATA_BYTE2)<<10;
-	setITVCReg(dev, ITVC_READ_DIR, temp);
-	temp = ((0x81|MCI_MEMORY_DATA_BYTE2)<<10);
-	setITVCReg(dev, ITVC_READ_DIR, temp);
-	getITVCReg(dev, ITVC_READ_DIR, &temp);
-	return_value |= ((temp&0x03FC0000)>>2);
-	setITVCReg(dev, ITVC_READ_DIR, (0x87<<10));
+	temp = (0x82 | MCI_MEMORY_DATA_BYTE2) << 10;
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
+	temp = ((0x81 | MCI_MEMORY_DATA_BYTE2) << 10);
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
+	get_itvc_reg(dev, ITVC_READ_DIR, &temp);
+	return_value |= ((temp & 0x03FC0000) >> 2);
+	set_itvc_reg(dev, ITVC_READ_DIR, (0x87 << 10));
 
 	/* Read data byte 1;*/
-	temp = (0x82|MCI_MEMORY_DATA_BYTE1)<<10;
-	setITVCReg(dev, ITVC_READ_DIR, temp);
-	temp = ((0x81|MCI_MEMORY_DATA_BYTE1)<<10);
-	setITVCReg(dev, ITVC_READ_DIR, temp);
-	getITVCReg(dev, ITVC_READ_DIR, &temp);
-	return_value |= ((temp&0x03FC0000)>>10);
-	setITVCReg(dev, ITVC_READ_DIR, (0x87<<10));
+	temp = (0x82 | MCI_MEMORY_DATA_BYTE1) << 10;
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
+	temp = ((0x81 | MCI_MEMORY_DATA_BYTE1) << 10);
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
+	get_itvc_reg(dev, ITVC_READ_DIR, &temp);
+	return_value |= ((temp & 0x03FC0000) >> 10);
+	set_itvc_reg(dev, ITVC_READ_DIR, (0x87 << 10));
 
 	/*Read data byte 0;*/
-	temp = (0x82|MCI_MEMORY_DATA_BYTE0)<<10;
-	setITVCReg(dev, ITVC_READ_DIR, temp);
-	temp = ((0x81|MCI_MEMORY_DATA_BYTE0)<<10);
-	setITVCReg(dev, ITVC_READ_DIR, temp);
-	getITVCReg(dev, ITVC_READ_DIR, &temp);
-	return_value |= ((temp&0x03FC0000)>>18);
-	setITVCReg(dev, ITVC_READ_DIR, (0x87<<10));
+	temp = (0x82 | MCI_MEMORY_DATA_BYTE0) << 10;
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
+	temp = ((0x81 | MCI_MEMORY_DATA_BYTE0) << 10);
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
+	get_itvc_reg(dev, ITVC_READ_DIR, &temp);
+	return_value |= ((temp & 0x03FC0000) >> 18);
+	set_itvc_reg(dev, ITVC_READ_DIR, (0x87 << 10));
 
 	*value  = return_value;
 	return ret;
@@ -619,94 +644,91 @@ static char *cmd_to_str(int cmd)
 {
 	switch (cmd) {
 	case CX2341X_ENC_PING_FW:
-		return  "PING_FW";
+		return "PING_FW";
 	case CX2341X_ENC_START_CAPTURE:
-		return  "START_CAPTURE";
+		return "START_CAPTURE";
 	case CX2341X_ENC_STOP_CAPTURE:
-		return  "STOP_CAPTURE";
+		return "STOP_CAPTURE";
 	case CX2341X_ENC_SET_AUDIO_ID:
-		return  "SET_AUDIO_ID";
+		return "SET_AUDIO_ID";
 	case CX2341X_ENC_SET_VIDEO_ID:
-		return  "SET_VIDEO_ID";
+		return "SET_VIDEO_ID";
 	case CX2341X_ENC_SET_PCR_ID:
-		return  "SET_PCR_PID";
+		return "SET_PCR_PID";
 	case CX2341X_ENC_SET_FRAME_RATE:
-		return  "SET_FRAME_RATE";
+		return "SET_FRAME_RATE";
 	case CX2341X_ENC_SET_FRAME_SIZE:
-		return  "SET_FRAME_SIZE";
+		return "SET_FRAME_SIZE";
 	case CX2341X_ENC_SET_BIT_RATE:
-		return  "SET_BIT_RATE";
+		return "SET_BIT_RATE";
 	case CX2341X_ENC_SET_GOP_PROPERTIES:
-		return  "SET_GOP_PROPERTIES";
+		return "SET_GOP_PROPERTIES";
 	case CX2341X_ENC_SET_ASPECT_RATIO:
-		return  "SET_ASPECT_RATIO";
+		return "SET_ASPECT_RATIO";
 	case CX2341X_ENC_SET_DNR_FILTER_MODE:
-		return  "SET_DNR_FILTER_PROPS";
+		return "SET_DNR_FILTER_PROPS";
 	case CX2341X_ENC_SET_DNR_FILTER_PROPS:
-		return  "SET_DNR_FILTER_PROPS";
+		return "SET_DNR_FILTER_PROPS";
 	case CX2341X_ENC_SET_CORING_LEVELS:
-		return  "SET_CORING_LEVELS";
+		return "SET_CORING_LEVELS";
 	case CX2341X_ENC_SET_SPATIAL_FILTER_TYPE:
-		return  "SET_SPATIAL_FILTER_TYPE";
+		return "SET_SPATIAL_FILTER_TYPE";
 	case CX2341X_ENC_SET_VBI_LINE:
-		return  "SET_VBI_LINE";
+		return "SET_VBI_LINE";
 	case CX2341X_ENC_SET_STREAM_TYPE:
-		return  "SET_STREAM_TYPE";
+		return "SET_STREAM_TYPE";
 	case CX2341X_ENC_SET_OUTPUT_PORT:
-		return  "SET_OUTPUT_PORT";
+		return "SET_OUTPUT_PORT";
 	case CX2341X_ENC_SET_AUDIO_PROPERTIES:
-		return  "SET_AUDIO_PROPERTIES";
+		return "SET_AUDIO_PROPERTIES";
 	case CX2341X_ENC_HALT_FW:
-		return  "HALT_FW";
+		return "HALT_FW";
 	case CX2341X_ENC_GET_VERSION:
-		return  "GET_VERSION";
+		return "GET_VERSION";
 	case CX2341X_ENC_SET_GOP_CLOSURE:
-		return  "SET_GOP_CLOSURE";
+		return "SET_GOP_CLOSURE";
 	case CX2341X_ENC_GET_SEQ_END:
-		return  "GET_SEQ_END";
+		return "GET_SEQ_END";
 	case CX2341X_ENC_SET_PGM_INDEX_INFO:
-		return  "SET_PGM_INDEX_INFO";
+		return "SET_PGM_INDEX_INFO";
 	case CX2341X_ENC_SET_VBI_CONFIG:
-		return  "SET_VBI_CONFIG";
+		return "SET_VBI_CONFIG";
 	case CX2341X_ENC_SET_DMA_BLOCK_SIZE:
-		return  "SET_DMA_BLOCK_SIZE";
+		return "SET_DMA_BLOCK_SIZE";
 	case CX2341X_ENC_GET_PREV_DMA_INFO_MB_10:
-		return  "GET_PREV_DMA_INFO_MB_10";
+		return "GET_PREV_DMA_INFO_MB_10";
 	case CX2341X_ENC_GET_PREV_DMA_INFO_MB_9:
-		return  "GET_PREV_DMA_INFO_MB_9";
+		return "GET_PREV_DMA_INFO_MB_9";
 	case CX2341X_ENC_SCHED_DMA_TO_HOST:
-		return  "SCHED_DMA_TO_HOST";
+		return "SCHED_DMA_TO_HOST";
 	case CX2341X_ENC_INITIALIZE_INPUT:
-		return  "INITIALIZE_INPUT";
+		return "INITIALIZE_INPUT";
 	case CX2341X_ENC_SET_FRAME_DROP_RATE:
-		return  "SET_FRAME_DROP_RATE";
+		return "SET_FRAME_DROP_RATE";
 	case CX2341X_ENC_PAUSE_ENCODER:
-		return  "PAUSE_ENCODER";
+		return "PAUSE_ENCODER";
 	case CX2341X_ENC_REFRESH_INPUT:
-		return  "REFRESH_INPUT";
+		return "REFRESH_INPUT";
 	case CX2341X_ENC_SET_COPYRIGHT:
-		return  "SET_COPYRIGHT";
+		return "SET_COPYRIGHT";
 	case CX2341X_ENC_SET_EVENT_NOTIFICATION:
-		return  "SET_EVENT_NOTIFICATION";
+		return "SET_EVENT_NOTIFICATION";
 	case CX2341X_ENC_SET_NUM_VSYNC_LINES:
-		return  "SET_NUM_VSYNC_LINES";
+		return "SET_NUM_VSYNC_LINES";
 	case CX2341X_ENC_SET_PLACEHOLDER:
-		return  "SET_PLACEHOLDER";
+		return "SET_PLACEHOLDER";
 	case CX2341X_ENC_MUTE_VIDEO:
-		return  "MUTE_VIDEO";
+		return "MUTE_VIDEO";
 	case CX2341X_ENC_MUTE_AUDIO:
-		return  "MUTE_AUDIO";
+		return "MUTE_AUDIO";
 	case CX2341X_ENC_MISC:
-		return  "MISC";
+		return "MISC";
 	default:
 		return "UNKNOWN";
 	}
 }
 
-static int cx231xx_mbox_func(void *priv,
-			     u32 command,
-			     int in,
-			     int out,
+static int cx231xx_mbox_func(void *priv, u32 command, int in, int out,
 			     u32 data[CX2341X_MBOX_MAX_DATA])
 {
 	struct cx231xx *dev = priv;
@@ -721,11 +743,9 @@ static int cx231xx_mbox_func(void *priv,
 	   without side effects */
 	mc417_memory_read(dev, dev->cx23417_mailbox - 4, &value);
 	if (value != 0x12345678) {
-		dprintk(3,
-			"Firmware and/or mailbox pointer not initialized "
-			"or corrupted, signature = 0x%x, cmd = %s\n", value,
-			cmd_to_str(command));
-		return -1;
+		dprintk(3, "Firmware and/or mailbox pointer not initialized or corrupted, signature = 0x%x, cmd = %s\n",
+			value, cmd_to_str(command));
+		return -EIO;
 	}
 
 	/* This read looks at 32 bits, but flag is only 8 bits.
@@ -733,9 +753,9 @@ static int cx231xx_mbox_func(void *priv,
 	 */
 	mc417_memory_read(dev, dev->cx23417_mailbox, &flag);
 	if (flag) {
-		dprintk(3, "ERROR: Mailbox appears to be in use "
-			"(%x), cmd = %s\n", flag, cmd_to_str(command));
-		return -1;
+		dprintk(3, "ERROR: Mailbox appears to be in use (%x), cmd = %s\n",
+				flag, cmd_to_str(command));
+		return -EBUSY;
 	}
 
 	flag |= 1; /* tell 'em we're working on it */
@@ -764,7 +784,7 @@ static int cx231xx_mbox_func(void *priv,
 			break;
 		if (time_after(jiffies, timeout)) {
 			dprintk(3, "ERROR: API Mailbox timeout\n");
-			return -1;
+			return -EIO;
 		}
 		udelay(10);
 	}
@@ -781,17 +801,14 @@ static int cx231xx_mbox_func(void *priv,
 	flag = 0;
 	mc417_memory_write(dev, dev->cx23417_mailbox, flag);
 
-	return retval;
+	return 0;
 }
 
 /* We don't need to call the API often, so using just one
  * mailbox will probably suffice
  */
-static int cx231xx_api_cmd(struct cx231xx *dev,
-			   u32 command,
-			   u32 inputcnt,
-			   u32 outputcnt,
-			   ...)
+static int cx231xx_api_cmd(struct cx231xx *dev, u32 command,
+		u32 inputcnt, u32 outputcnt, ...)
 {
 	u32 data[CX2341X_MBOX_MAX_DATA];
 	va_list vargs;
@@ -813,6 +830,7 @@ static int cx231xx_api_cmd(struct cx231xx *dev,
 	return err;
 }
 
+
 static int cx231xx_find_mailbox(struct cx231xx *dev)
 {
 	u32 signature[4] = {
@@ -834,81 +852,80 @@ static int cx231xx_find_mailbox(struct cx231xx *dev)
 		else
 			signaturecnt = 0;
 		if (4 == signaturecnt) {
-			dprintk(1, "Mailbox signature found at 0x%x\n", i+1);
-			return i+1;
+			dprintk(1, "Mailbox signature found at 0x%x\n", i + 1);
+			return i + 1;
 		}
 	}
 	dprintk(3, "Mailbox signature values not found!\n");
 	return -1;
 }
 
-static void mciWriteMemoryToGPIO(struct cx231xx *dev, u32 address, u32 value,
+static void mci_write_memory_to_gpio(struct cx231xx *dev, u32 address, u32 value,
 		u32 *p_fw_image)
 {
-
 	u32 temp = 0;
 	int i = 0;
 
-	temp = 0x82|MCI_MEMORY_DATA_BYTE0|((value&0x000000FF)<<8);
-	temp = temp<<10;
+	temp = 0x82 | MCI_MEMORY_DATA_BYTE0 | ((value & 0x000000FF) << 8);
+	temp = temp << 10;
 	*p_fw_image = temp;
 	p_fw_image++;
-	temp = temp|((0x05)<<10);
+	temp = temp | (0x05 << 10);
 	*p_fw_image = temp;
 	p_fw_image++;
 
 	/*write data byte 1;*/
-	temp = 0x82|MCI_MEMORY_DATA_BYTE1|(value&0x0000FF00);
-	temp = temp<<10;
+	temp = 0x82 | MCI_MEMORY_DATA_BYTE1 | (value & 0x0000FF00);
+	temp = temp << 10;
 	*p_fw_image = temp;
 	p_fw_image++;
-	temp = temp|((0x05)<<10);
+	temp = temp | (0x05 << 10);
 	*p_fw_image = temp;
 	p_fw_image++;
 
 	/*write data byte 2;*/
-	temp = 0x82|MCI_MEMORY_DATA_BYTE2|((value&0x00FF0000)>>8);
-	temp = temp<<10;
+	temp = 0x82 | MCI_MEMORY_DATA_BYTE2 | ((value & 0x00FF0000) >> 8);
+	temp = temp << 10;
 	*p_fw_image = temp;
 	p_fw_image++;
-	temp = temp|((0x05)<<10);
+	temp = temp | (0x05 << 10);
 	*p_fw_image = temp;
 	p_fw_image++;
 
 	/*write data byte 3;*/
-	temp = 0x82|MCI_MEMORY_DATA_BYTE3|((value&0xFF000000)>>16);
-	temp = temp<<10;
+	temp = 0x82 | MCI_MEMORY_DATA_BYTE3 | ((value & 0xFF000000) >> 16);
+	temp = temp << 10;
 	*p_fw_image = temp;
 	p_fw_image++;
-	temp = temp|((0x05)<<10);
+	temp = temp | (0x05 << 10);
 	*p_fw_image = temp;
 	p_fw_image++;
 
 	/* write address byte 2;*/
-	temp = 0x82|MCI_MEMORY_ADDRESS_BYTE2 | MCI_MODE_MEMORY_WRITE |
-		((address & 0x003F0000)>>8);
-	temp = temp<<10;
+	temp = 0x82 | MCI_MEMORY_ADDRESS_BYTE2 | MCI_MODE_MEMORY_WRITE |
+		((address & 0x003F0000) >> 8);
+	temp = temp << 10;
 	*p_fw_image = temp;
 	p_fw_image++;
-	temp = temp|((0x05)<<10);
+	temp = temp | (0x05 << 10);
 	*p_fw_image = temp;
 	p_fw_image++;
 
 	/* write address byte 1;*/
-	temp = 0x82|MCI_MEMORY_ADDRESS_BYTE1 | (address & 0xFF00);
-	temp = temp<<10;
+	temp = 0x82 | MCI_MEMORY_ADDRESS_BYTE1 | (address & 0xFF00);
+	temp = temp << 10;
 	*p_fw_image = temp;
 	p_fw_image++;
-	temp = temp|((0x05)<<10);
+	temp = temp | (0x05 << 10);
 	*p_fw_image = temp;
 	p_fw_image++;
 
 	/* write address byte 0;*/
-	temp = 0x82|MCI_MEMORY_ADDRESS_BYTE0|((address & 0x00FF)<<8);
-	temp = temp<<10;
+	temp = 0x82 | MCI_MEMORY_ADDRESS_BYTE0 | ((address & 0x00FF) << 8);
+	temp = temp << 10;
 	*p_fw_image = temp;
 	p_fw_image++;
-	temp = temp|((0x05)<<10);
+	temp = temp | (0x05 << 10);
 	*p_fw_image = temp;
 	p_fw_image++;
 
@@ -971,8 +988,7 @@ static int cx231xx_load_firmware(struct cx231xx *dev)
 		IVTV_REG_APU, 0);
 
 	if (retval != 0) {
-		printk(KERN_ERR "%s: Error with mc417_register_write\n",
-			__func__);
+		pr_err("%s: Error with mc417_register_write\n", __func__);
 		return -1;
 	}
 
@@ -980,25 +996,21 @@ static int cx231xx_load_firmware(struct cx231xx *dev)
 				  &dev->udev->dev);
 
 	if (retval != 0) {
-		printk(KERN_ERR
-			"ERROR: Hotplug firmware request failed (%s).\n",
+		pr_err("ERROR: Hotplug firmware request failed (%s).\n",
 			CX231xx_FIRM_IMAGE_NAME);
-		printk(KERN_ERR "Please fix your hotplug setup, the board will "
-			"not work without firmware loaded!\n");
+		pr_err("Please fix your hotplug setup, the board will not work without firmware loaded!\n");
 		return -1;
 	}
 
 	if (firmware->size != CX231xx_FIRM_IMAGE_SIZE) {
-		printk(KERN_ERR "ERROR: Firmware size mismatch "
-			"(have %zd, expected %d)\n",
+		pr_err("ERROR: Firmware size mismatch (have %zd, expected %d)\n",
 			firmware->size, CX231xx_FIRM_IMAGE_SIZE);
 		release_firmware(firmware);
 		return -1;
 	}
 
 	if (0 != memcmp(firmware->data, magic, 8)) {
-		printk(KERN_ERR
-			"ERROR: Firmware magic mismatch, wrong file?\n");
+		pr_err("ERROR: Firmware magic mismatch, wrong file?\n");
 		release_firmware(firmware);
 		return -1;
 	}
@@ -1013,7 +1025,7 @@ static int cx231xx_load_firmware(struct cx231xx *dev)
 		 transfer_size += 4) {
 		fw_data = *p_fw_data;
 
-		 mciWriteMemoryToGPIO(dev, address, fw_data, p_current_fw);
+		mci_write_memory_to_gpio(dev, address, fw_data, p_current_fw);
 		address = address + 1;
 		p_current_fw += 20;
 		p_fw_data += 1;
@@ -1045,7 +1057,7 @@ static int cx231xx_load_firmware(struct cx231xx *dev)
 	retval |= mc417_register_write(dev, IVTV_REG_HW_BLOCKS,
 		IVTV_CMD_HW_BLOCKS_RST);
 	if (retval < 0) {
-		printk(KERN_ERR "%s: Error with mc417_register_write\n",
+		pr_err("%s: Error with mc417_register_write\n",
 			__func__);
 		return retval;
 	}
@@ -1057,7 +1069,7 @@ static int cx231xx_load_firmware(struct cx231xx *dev)
 	retval |= mc417_register_write(dev, IVTV_REG_VPU, value & 0xFFFFFFE8);
 
 	if (retval < 0) {
-		printk(KERN_ERR "%s: Error with mc417_register_write\n",
+		pr_err("%s: Error with mc417_register_write\n",
 			__func__);
 		return retval;
 	}
@@ -1082,10 +1094,10 @@ static void cx231xx_codec_settings(struct cx231xx *dev)
 	cx231xx_api_cmd(dev, CX2341X_ENC_SET_FRAME_SIZE, 2, 0,
 				dev->ts1.height, dev->ts1.width);
 
-	dev->mpeg_params.width = dev->ts1.width;
-	dev->mpeg_params.height = dev->ts1.height;
+	dev->mpeg_ctrl_handler.width = dev->ts1.width;
+	dev->mpeg_ctrl_handler.height = dev->ts1.height;
 
-	cx2341x_update(dev, cx231xx_mbox_func, NULL, &dev->mpeg_params);
+	cx2341x_handler_setup(&dev->mpeg_ctrl_handler);
 
 	cx231xx_api_cmd(dev, CX2341X_ENC_MISC, 2, 0, 3, 1);
 	cx231xx_api_cmd(dev, CX2341X_ENC_MISC, 2, 0, 4, 1);
@@ -1105,27 +1117,25 @@ static int cx231xx_initialize_codec(struct cx231xx *dev)
 		dprintk(2, "%s() PING OK\n", __func__);
 		retval = cx231xx_load_firmware(dev);
 		if (retval < 0) {
-			printk(KERN_ERR "%s() f/w load failed\n", __func__);
+			pr_err("%s() f/w load failed\n", __func__);
 			return retval;
 		}
 		retval = cx231xx_find_mailbox(dev);
 		if (retval < 0) {
-			printk(KERN_ERR "%s() mailbox < 0, error\n",
+			pr_err("%s() mailbox < 0, error\n",
 				__func__);
 			return -1;
 		}
 		dev->cx23417_mailbox = retval;
 		retval = cx231xx_api_cmd(dev, CX2341X_ENC_PING_FW, 0, 0);
 		if (retval < 0) {
-			printk(KERN_ERR
-				"ERROR: cx23417 firmware ping failed!\n");
+			pr_err("ERROR: cx23417 firmware ping failed!\n");
 			return -1;
 		}
 		retval = cx231xx_api_cmd(dev, CX2341X_ENC_GET_VERSION, 0, 1,
 			&version);
 		if (retval < 0) {
-			printk(KERN_ERR "ERROR: cx23417 firmware get encoder :"
-				"version failed!\n");
+			pr_err("ERROR: cx23417 firmware get encoder: version failed!\n");
 			return -1;
 		}
 		dprintk(1, "cx23417 firmware version is 0x%08x\n", version);
@@ -1134,7 +1144,7 @@ static int cx231xx_initialize_codec(struct cx231xx *dev)
 
 	for (i = 0; i < 1; i++) {
 		retval = mc417_register_read(dev, 0x20f8, &val);
-		dprintk(3, "***before enable656() VIM Capture Lines =%d ***\n",
+		dprintk(3, "***before enable656() VIM Capture Lines = %d ***\n",
 				 val);
 		if (retval < 0)
 			return retval;
@@ -1202,7 +1212,7 @@ static int cx231xx_initialize_codec(struct cx231xx *dev)
 
 	for (i = 0; i < 1; i++) {
 		mc417_register_read(dev, 0x20f8, &val);
-	dprintk(3, "***VIM Capture Lines =%d ***\n", val);
+		dprintk(3, "***VIM Capture Lines =%d ***\n", val);
 	}
 
 	return 0;
@@ -1223,6 +1233,7 @@ static int bb_buf_setup(struct videobuf_queue *q,
 
 	return 0;
 }
+
 static void free_buffer(struct videobuf_queue *vq, struct cx231xx_buffer *buf)
 {
 	struct cx231xx_fh *fh = vq->priv_data;
@@ -1249,91 +1260,85 @@ static void free_buffer(struct videobuf_queue *vq, struct cx231xx_buffer *buf)
 static void buffer_copy(struct cx231xx *dev, char *data, int len, struct urb *urb,
 		struct cx231xx_dmaqueue *dma_q)
 {
-		void *vbuf;
-		struct cx231xx_buffer *buf;
-		u32 tail_data = 0;
-		char *p_data;
-
-		if (dma_q->mpeg_buffer_done == 0) {
-			if (list_empty(&dma_q->active))
-				return;
-
-			buf = list_entry(dma_q->active.next,
-					struct cx231xx_buffer, vb.queue);
-			dev->video_mode.isoc_ctl.buf = buf;
-			dma_q->mpeg_buffer_done = 1;
-		}
-		/* Fill buffer */
-		buf = dev->video_mode.isoc_ctl.buf;
-		vbuf = videobuf_to_vmalloc(&buf->vb);
-
-		if ((dma_q->mpeg_buffer_completed+len) <
-		   mpeglines*mpeglinesize) {
-			if (dma_q->add_ps_package_head ==
-			   CX231XX_NEED_ADD_PS_PACKAGE_HEAD) {
-				memcpy(vbuf+dma_q->mpeg_buffer_completed,
-				       dma_q->ps_head, 3);
-				dma_q->mpeg_buffer_completed =
-				  dma_q->mpeg_buffer_completed + 3;
-				dma_q->add_ps_package_head =
-				  CX231XX_NONEED_PS_PACKAGE_HEAD;
-			}
-			memcpy(vbuf+dma_q->mpeg_buffer_completed, data, len);
-			dma_q->mpeg_buffer_completed =
-			  dma_q->mpeg_buffer_completed + len;
-		} else {
-			dma_q->mpeg_buffer_done = 0;
-
-			tail_data =
-			  mpeglines*mpeglinesize - dma_q->mpeg_buffer_completed;
-			memcpy(vbuf+dma_q->mpeg_buffer_completed,
-			       data, tail_data);
-
-			buf->vb.state = VIDEOBUF_DONE;
-			buf->vb.field_count++;
-			v4l2_get_timestamp(&buf->vb.ts);
-			list_del(&buf->vb.queue);
-			wake_up(&buf->vb.done);
-			dma_q->mpeg_buffer_completed = 0;
-
-			if (len - tail_data > 0) {
-				p_data = data + tail_data;
-				dma_q->left_data_count = len - tail_data;
-				memcpy(dma_q->p_left_data,
-				       p_data, len - tail_data);
-			}
-
-		}
-
-	    return;
-}
-
-static void buffer_filled(char *data, int len, struct urb *urb,
-		struct cx231xx_dmaqueue *dma_q)
-{
-		void *vbuf;
-		struct cx231xx_buffer *buf;
+	void *vbuf;
+	struct cx231xx_buffer *buf;
+	u32 tail_data = 0;
+	char *p_data;
 
+	if (dma_q->mpeg_buffer_done == 0) {
 		if (list_empty(&dma_q->active))
 			return;
 
-
 		buf = list_entry(dma_q->active.next,
-				 struct cx231xx_buffer, vb.queue);
+				struct cx231xx_buffer, vb.queue);
+		dev->video_mode.isoc_ctl.buf = buf;
+		dma_q->mpeg_buffer_done = 1;
+	}
+	/* Fill buffer */
+	buf = dev->video_mode.isoc_ctl.buf;
+	vbuf = videobuf_to_vmalloc(&buf->vb);
+
+	if ((dma_q->mpeg_buffer_completed+len) <
+			mpeglines*mpeglinesize) {
+		if (dma_q->add_ps_package_head ==
+				CX231XX_NEED_ADD_PS_PACKAGE_HEAD) {
+			memcpy(vbuf+dma_q->mpeg_buffer_completed,
+					dma_q->ps_head, 3);
+			dma_q->mpeg_buffer_completed =
+				dma_q->mpeg_buffer_completed + 3;
+			dma_q->add_ps_package_head =
+				CX231XX_NONEED_PS_PACKAGE_HEAD;
+		}
+		memcpy(vbuf+dma_q->mpeg_buffer_completed, data, len);
+		dma_q->mpeg_buffer_completed =
+			dma_q->mpeg_buffer_completed + len;
+	} else {
+		dma_q->mpeg_buffer_done = 0;
 
+		tail_data =
+			mpeglines*mpeglinesize - dma_q->mpeg_buffer_completed;
+		memcpy(vbuf+dma_q->mpeg_buffer_completed,
+				data, tail_data);
 
-		/* Fill buffer */
-		vbuf = videobuf_to_vmalloc(&buf->vb);
-		memcpy(vbuf, data, len);
 		buf->vb.state = VIDEOBUF_DONE;
 		buf->vb.field_count++;
 		v4l2_get_timestamp(&buf->vb.ts);
 		list_del(&buf->vb.queue);
 		wake_up(&buf->vb.done);
+		dma_q->mpeg_buffer_completed = 0;
 
-	    return;
+		if (len - tail_data > 0) {
+			p_data = data + tail_data;
+			dma_q->left_data_count = len - tail_data;
+			memcpy(dma_q->p_left_data,
+					p_data, len - tail_data);
+		}
+	}
 }
-static inline int cx231xx_isoc_copy(struct cx231xx *dev, struct urb *urb)
+
+static void buffer_filled(char *data, int len, struct urb *urb,
+		struct cx231xx_dmaqueue *dma_q)
+{
+	void *vbuf;
+	struct cx231xx_buffer *buf;
+
+	if (list_empty(&dma_q->active))
+		return;
+
+	buf = list_entry(dma_q->active.next,
+			struct cx231xx_buffer, vb.queue);
+
+	/* Fill buffer */
+	vbuf = videobuf_to_vmalloc(&buf->vb);
+	memcpy(vbuf, data, len);
+	buf->vb.state = VIDEOBUF_DONE;
+	buf->vb.field_count++;
+	v4l2_get_timestamp(&buf->vb.ts);
+	list_del(&buf->vb.queue);
+	wake_up(&buf->vb.done);
+}
+
+static int cx231xx_isoc_copy(struct cx231xx *dev, struct urb *urb)
 {
 	struct cx231xx_dmaqueue *dma_q = urb->context;
 	unsigned char *p_buffer;
@@ -1358,11 +1363,9 @@ static inline int cx231xx_isoc_copy(struct cx231xx *dev, struct urb *urb)
 
 	return 0;
 }
-static inline int cx231xx_bulk_copy(struct cx231xx *dev, struct urb *urb)
-{
 
-	/*char *outp;*/
-	/*struct cx231xx_buffer *buf;*/
+static int cx231xx_bulk_copy(struct cx231xx *dev, struct urb *urb)
+{
 	struct cx231xx_dmaqueue *dma_q = urb->context;
 	unsigned char *p_buffer, *buffer;
 	u32 buffer_size = 0;
@@ -1393,8 +1396,6 @@ static int bb_buf_prepare(struct videobuf_queue *q,
 	int rc = 0, urb_init = 0;
 	int size = fh->dev->ts1.ts_packet_size * fh->dev->ts1.ts_packet_count;
 
-	dma_qq = &dev->video_mode.vidq;
-
 	if (0 != buf->vb.baddr  &&  buf->vb.bsize < size)
 		return -EINVAL;
 	buf->vb.width = fh->dev->ts1.ts_packet_size;
@@ -1482,36 +1483,6 @@ static struct videobuf_queue_ops cx231xx_qops = {
 
 /* ------------------------------------------------------------------ */
 
-static const u32 *ctrl_classes[] = {
-	cx2341x_mpeg_ctrls,
-	NULL
-};
-
-static int cx231xx_queryctrl(struct cx231xx *dev,
-	struct v4l2_queryctrl *qctrl)
-{
-	qctrl->id = v4l2_ctrl_next(ctrl_classes, qctrl->id);
-	if (qctrl->id == 0)
-		return -EINVAL;
-
-	/* MPEG V4L2 controls */
-	if (cx2341x_ctrl_query(&dev->mpeg_params, qctrl))
-		qctrl->flags |= V4L2_CTRL_FLAG_DISABLED;
-
-	return 0;
-}
-
-static int cx231xx_querymenu(struct cx231xx *dev,
-	struct v4l2_querymenu *qmenu)
-{
-	struct v4l2_queryctrl qctrl;
-
-	qctrl.id = qmenu->id;
-	cx231xx_queryctrl(dev, &qctrl);
-	return v4l2_ctrl_query_menu(qmenu, &qctrl,
-		cx2341x_ctrl_get_menu(&dev->mpeg_params, qmenu->id));
-}
-
 static int vidioc_g_std(struct file *file, void *fh0, v4l2_std_id *norm)
 {
 	struct cx231xx_fh  *fh  = file->private_data;
@@ -1520,14 +1491,15 @@ static int vidioc_g_std(struct file *file, void *fh0, v4l2_std_id *norm)
 	*norm = dev->encodernorm.id;
 	return 0;
 }
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *id)
+
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id id)
 {
 	struct cx231xx_fh  *fh  = file->private_data;
 	struct cx231xx *dev = fh->dev;
 	unsigned int i;
 
 	for (i = 0; i < ARRAY_SIZE(cx231xx_tvnorms); i++)
-		if (*id & cx231xx_tvnorms[i].id)
+		if (id & cx231xx_tvnorms[i].id)
 			break;
 	if (i == ARRAY_SIZE(cx231xx_tvnorms))
 		return -EINVAL;
@@ -1537,12 +1509,12 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *id)
 		dprintk(3, "encodernorm set to NTSC\n");
 		dev->norm = V4L2_STD_NTSC;
 		dev->ts1.height = 480;
-		dev->mpeg_params.is_50hz = 0;
+		cx2341x_handler_set_50hz(&dev->mpeg_ctrl_handler, false);
 	} else {
 		dprintk(3, "encodernorm set to PAL\n");
 		dev->norm = V4L2_STD_PAL_B;
 		dev->ts1.height = 576;
-		dev->mpeg_params.is_50hz = 1;
+		cx2341x_handler_set_50hz(&dev->mpeg_ctrl_handler, true);
 	}
 	call_all(dev, core, s_std, dev->norm);
 	/* do mode control overrides */
@@ -1551,161 +1523,23 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *id)
 	dprintk(3, "exit vidioc_s_std() i=0x%x\n", i);
 	return 0;
 }
-static int vidioc_g_audio(struct file *file, void *fh,
-					struct v4l2_audio *a)
-{
-		struct v4l2_audio *vin = a;
-
-		int ret = -EINVAL;
-		if (vin->index > 0)
-			return ret;
-		strncpy(vin->name, "VideoGrabber Audio", 14);
-		vin->capability = V4L2_AUDCAP_STEREO;
-return 0;
-}
-static int vidioc_enumaudio(struct file *file, void *fh,
-					struct v4l2_audio *a)
-{
-		struct v4l2_audio *vin = a;
-
-		int ret = -EINVAL;
-
-		if (vin->index > 0)
-			return ret;
-		strncpy(vin->name, "VideoGrabber Audio", 14);
-		vin->capability = V4L2_AUDCAP_STEREO;
-
-
-return 0;
-}
-static const char *iname[] = {
-	[CX231XX_VMUX_COMPOSITE1] = "Composite1",
-	[CX231XX_VMUX_SVIDEO]     = "S-Video",
-	[CX231XX_VMUX_TELEVISION] = "Television",
-	[CX231XX_VMUX_CABLE]      = "Cable TV",
-	[CX231XX_VMUX_DVB]        = "DVB",
-	[CX231XX_VMUX_DEBUG]      = "for debug only",
-};
-static int vidioc_enum_input(struct file *file, void *priv,
-				struct v4l2_input *i)
-{
-	struct cx231xx_fh  *fh  = file->private_data;
-	struct cx231xx *dev = fh->dev;
-	struct cx231xx_input *input;
-	int n;
-	dprintk(3, "enter vidioc_enum_input()i->index=%d\n", i->index);
-
-	if (i->index >= 4)
-		return -EINVAL;
-
-
-	input = &cx231xx_boards[dev->model].input[i->index];
-
-	if (input->type == 0)
-		return -EINVAL;
-
-	/* FIXME
-	 * strcpy(i->name, input->name); */
-
-	n = i->index;
-	strcpy(i->name, iname[INPUT(n)->type]);
-
-	if (input->type == CX231XX_VMUX_TELEVISION ||
-	    input->type == CX231XX_VMUX_CABLE)
-		i->type = V4L2_INPUT_TYPE_TUNER;
-	else
-		i->type  = V4L2_INPUT_TYPE_CAMERA;
-
-
-	return 0;
-}
-
-static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
-{
-	*i = 0;
-	return  0;
-}
-
-static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
-{
-	struct cx231xx_fh  *fh  = file->private_data;
-	struct cx231xx *dev = fh->dev;
-
-	dprintk(3, "enter vidioc_s_input() i=%d\n", i);
-
-	mutex_lock(&dev->lock);
-
-	video_mux(dev, i);
-
-	mutex_unlock(&dev->lock);
-
-	if (i >= 4)
-		return -EINVAL;
-	dev->input = i;
-	dprintk(3, "exit vidioc_s_input()\n");
-	return 0;
-}
-
-static int vidioc_g_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *t)
-{
-	return 0;
-}
-
-static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *t)
-{
-	return 0;
-}
-
-static int vidioc_g_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
-{
-	return 0;
-}
-
-static int vidioc_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
-{
-
-
-	return 0;
-}
 
 static int vidioc_s_ctrl(struct file *file, void *priv,
 				struct v4l2_control *ctl)
 {
 	struct cx231xx_fh  *fh  = file->private_data;
 	struct cx231xx *dev = fh->dev;
+
 	dprintk(3, "enter vidioc_s_ctrl()\n");
 	/* Update the A/V core */
 	call_all(dev, core, s_ctrl, ctl);
 	dprintk(3, "exit vidioc_s_ctrl()\n");
 	return 0;
 }
-static struct v4l2_capability pvr_capability = {
-	.driver         = "cx231xx",
-	.card           = "VideoGrabber",
-	.bus_info       = "usb",
-	.version        = 1,
-	.capabilities   = (V4L2_CAP_VIDEO_CAPTURE |
-			   V4L2_CAP_TUNER | V4L2_CAP_AUDIO | V4L2_CAP_RADIO |
-			 V4L2_CAP_STREAMING | V4L2_CAP_READWRITE),
-};
-static int vidioc_querycap(struct file *file, void  *priv,
-				struct v4l2_capability *cap)
-{
-
-
-
-		memcpy(cap, &pvr_capability, sizeof(struct v4l2_capability));
-	return 0;
-}
 
 static int vidioc_enum_fmt_vid_cap(struct file *file, void  *priv,
 					struct v4l2_fmtdesc *f)
 {
-
 	if (f->index != 0)
 		return -EINVAL;
 
@@ -1720,17 +1554,18 @@ static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 {
 	struct cx231xx_fh  *fh  = file->private_data;
 	struct cx231xx *dev = fh->dev;
+
 	dprintk(3, "enter vidioc_g_fmt_vid_cap()\n");
-	f->fmt.pix.pixelformat  = V4L2_PIX_FMT_MPEG;
+	f->fmt.pix.pixelformat = V4L2_PIX_FMT_MPEG;
 	f->fmt.pix.bytesperline = 0;
-	f->fmt.pix.sizeimage    =
-		dev->ts1.ts_packet_size * dev->ts1.ts_packet_count;
-	f->fmt.pix.colorspace   = 0;
-	f->fmt.pix.width        = dev->ts1.width;
-	f->fmt.pix.height       = dev->ts1.height;
-	f->fmt.pix.field        = fh->vidq.field;
-	dprintk(1, "VIDIOC_G_FMT: w: %d, h: %d, f: %d\n",
-		dev->ts1.width, dev->ts1.height, fh->vidq.field);
+	f->fmt.pix.sizeimage = mpeglines * mpeglinesize;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	f->fmt.pix.width = dev->ts1.width;
+	f->fmt.pix.height = dev->ts1.height;
+	f->fmt.pix.field = V4L2_FIELD_INTERLACED;
+	f->fmt.pix.priv = 0;
+	dprintk(1, "VIDIOC_G_FMT: w: %d, h: %d\n",
+		dev->ts1.width, dev->ts1.height);
 	dprintk(3, "exit vidioc_g_fmt_vid_cap()\n");
 	return 0;
 }
@@ -1740,25 +1575,20 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 {
 	struct cx231xx_fh  *fh  = file->private_data;
 	struct cx231xx *dev = fh->dev;
+
 	dprintk(3, "enter vidioc_try_fmt_vid_cap()\n");
-	f->fmt.pix.pixelformat  = V4L2_PIX_FMT_MPEG;
+	f->fmt.pix.pixelformat = V4L2_PIX_FMT_MPEG;
 	f->fmt.pix.bytesperline = 0;
-	f->fmt.pix.sizeimage    =
-		dev->ts1.ts_packet_size * dev->ts1.ts_packet_count;
-	f->fmt.pix.colorspace   = 0;
-	dprintk(1, "VIDIOC_TRY_FMT: w: %d, h: %d, f: %d\n",
-		dev->ts1.width, dev->ts1.height, fh->vidq.field);
+	f->fmt.pix.sizeimage = mpeglines * mpeglinesize;
+	f->fmt.pix.field = V4L2_FIELD_INTERLACED;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	f->fmt.pix.priv = 0;
+	dprintk(1, "VIDIOC_TRY_FMT: w: %d, h: %d\n",
+		dev->ts1.width, dev->ts1.height);
 	dprintk(3, "exit vidioc_try_fmt_vid_cap()\n");
 	return 0;
 }
 
-static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
-				struct v4l2_format *f)
-{
-
-	return 0;
-}
-
 static int vidioc_reqbufs(struct file *file, void *priv,
 				struct v4l2_requestbuffers *p)
 {
@@ -1795,22 +1625,22 @@ static int vidioc_streamon(struct file *file, void *priv,
 				enum v4l2_buf_type i)
 {
 	struct cx231xx_fh  *fh  = file->private_data;
-
 	struct cx231xx *dev = fh->dev;
+
 	dprintk(3, "enter vidioc_streamon()\n");
-		cx231xx_set_alt_setting(dev, INDEX_TS1, 0);
-		cx231xx_set_mode(dev, CX231XX_DIGITAL_MODE);
-		if (dev->USE_ISO)
-			cx231xx_init_isoc(dev, CX231XX_NUM_PACKETS,
-				       CX231XX_NUM_BUFS,
-				       dev->video_mode.max_pkt_size,
-				       cx231xx_isoc_copy);
-		else {
-			cx231xx_init_bulk(dev, 320,
-				       5,
-				       dev->ts1_mode.max_pkt_size,
-				       cx231xx_bulk_copy);
-		}
+	cx231xx_set_alt_setting(dev, INDEX_TS1, 0);
+	cx231xx_set_mode(dev, CX231XX_DIGITAL_MODE);
+	if (dev->USE_ISO)
+		cx231xx_init_isoc(dev, CX231XX_NUM_PACKETS,
+				CX231XX_NUM_BUFS,
+				dev->video_mode.max_pkt_size,
+				cx231xx_isoc_copy);
+	else {
+		cx231xx_init_bulk(dev, 320,
+				5,
+				dev->ts1_mode.max_pkt_size,
+				cx231xx_bulk_copy);
+	}
 	dprintk(3, "exit vidioc_streamon()\n");
 	return videobuf_streamon(&fh->vidq);
 }
@@ -1822,117 +1652,25 @@ static int vidioc_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
 	return videobuf_streamoff(&fh->vidq);
 }
 
-static int vidioc_g_ext_ctrls(struct file *file, void *priv,
-				struct v4l2_ext_controls *f)
-{
-	struct cx231xx_fh  *fh  = priv;
-	struct cx231xx *dev = fh->dev;
-	dprintk(3, "enter vidioc_g_ext_ctrls()\n");
-	if (f->ctrl_class != V4L2_CTRL_CLASS_MPEG)
-		return -EINVAL;
-	dprintk(3, "exit vidioc_g_ext_ctrls()\n");
-	return cx2341x_ext_ctrls(&dev->mpeg_params, 0, f, VIDIOC_G_EXT_CTRLS);
-}
-
-static int vidioc_s_ext_ctrls(struct file *file, void *priv,
-				struct v4l2_ext_controls *f)
-{
-	struct cx231xx_fh  *fh  = priv;
-	struct cx231xx *dev = fh->dev;
-	struct cx2341x_mpeg_params p;
-	int err;
-	dprintk(3, "enter vidioc_s_ext_ctrls()\n");
-	if (f->ctrl_class != V4L2_CTRL_CLASS_MPEG)
-		return -EINVAL;
-
-	p = dev->mpeg_params;
-	err = cx2341x_ext_ctrls(&p, 0, f, VIDIOC_TRY_EXT_CTRLS);
-	if (err == 0) {
-		err = cx2341x_update(dev, cx231xx_mbox_func,
-			&dev->mpeg_params, &p);
-		dev->mpeg_params = p;
-	}
-
-	return err;
-
-
-return 0;
-}
-
-static int vidioc_try_ext_ctrls(struct file *file, void *priv,
-				struct v4l2_ext_controls *f)
-{
-	struct cx231xx_fh  *fh  = priv;
-	struct cx231xx *dev = fh->dev;
-	struct cx2341x_mpeg_params p;
-	int err;
-	dprintk(3, "enter vidioc_try_ext_ctrls()\n");
-	if (f->ctrl_class != V4L2_CTRL_CLASS_MPEG)
-		return -EINVAL;
-
-	p = dev->mpeg_params;
-	err = cx2341x_ext_ctrls(&p, 0, f, VIDIOC_TRY_EXT_CTRLS);
-	dprintk(3, "exit vidioc_try_ext_ctrls() err=%d\n", err);
-	return err;
-}
-
 static int vidioc_log_status(struct file *file, void *priv)
 {
 	struct cx231xx_fh  *fh  = priv;
 	struct cx231xx *dev = fh->dev;
-	char name[32 + 2];
 
-	snprintf(name, sizeof(name), "%s/2", dev->name);
-	dprintk(3,
-		"%s/2: ============  START LOG STATUS  ============\n",
-	       dev->name);
 	call_all(dev, core, log_status);
-	cx2341x_log_status(&dev->mpeg_params, name);
-	dprintk(3,
-		"%s/2: =============  END LOG STATUS  =============\n",
-	       dev->name);
-	return 0;
-}
-
-static int vidioc_querymenu(struct file *file, void *priv,
-				struct v4l2_querymenu *a)
-{
-	struct cx231xx_fh  *fh  = priv;
-	struct cx231xx *dev = fh->dev;
-	dprintk(3, "enter vidioc_querymenu()\n");
-	dprintk(3, "exit vidioc_querymenu()\n");
-	return cx231xx_querymenu(dev, a);
-}
-
-static int vidioc_queryctrl(struct file *file, void *priv,
-				struct v4l2_queryctrl *c)
-{
-	struct cx231xx_fh  *fh  = priv;
-	struct cx231xx *dev = fh->dev;
-	dprintk(3, "enter vidioc_queryctrl()\n");
-	dprintk(3, "exit vidioc_queryctrl()\n");
-	return cx231xx_queryctrl(dev, c);
+	return v4l2_ctrl_log_status(file, priv);
 }
 
 static int mpeg_open(struct file *file)
 {
-	int minor = video_devdata(file)->minor;
-	struct cx231xx *h, *dev = NULL;
-	/*struct list_head *list;*/
+	struct video_device *vdev = video_devdata(file);
+	struct cx231xx *dev = video_drvdata(file);
 	struct cx231xx_fh *fh;
-	/*u32 value = 0;*/
 
 	dprintk(2, "%s()\n", __func__);
 
-	list_for_each_entry(h, &cx231xx_devlist, devlist) {
-		if (h->v4l_device->minor == minor)
-			dev = h;
-	}
-
-	if (dev == NULL)
-		return -ENODEV;
-
-	mutex_lock(&dev->lock);
+	if (mutex_lock_interruptible(&dev->lock))
+		return -ERESTARTSYS;
 
 	/* allocate + initialize per filehandle data */
 	fh = kzalloc(sizeof(*fh), GFP_KERNEL);
@@ -1942,29 +1680,30 @@ static int mpeg_open(struct file *file)
 	}
 
 	file->private_data = fh;
-	fh->dev      = dev;
+	v4l2_fh_init(&fh->fh, vdev);
+	fh->dev = dev;
 
 
 	videobuf_queue_vmalloc_init(&fh->vidq, &cx231xx_qops,
 			    NULL, &dev->video_mode.slock,
 			    V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_FIELD_INTERLACED,
-			    sizeof(struct cx231xx_buffer), fh, NULL);
+			    sizeof(struct cx231xx_buffer), fh, &dev->lock);
 /*
 	videobuf_queue_sg_init(&fh->vidq, &cx231xx_qops,
 			    &dev->udev->dev, &dev->ts1.slock,
 			    V4L2_BUF_TYPE_VIDEO_CAPTURE,
 			    V4L2_FIELD_INTERLACED,
 			    sizeof(struct cx231xx_buffer),
-			    fh, NULL);
+			    fh, &dev->lock);
 */
 
-
 	cx231xx_set_alt_setting(dev, INDEX_VANC, 1);
 	cx231xx_set_gpio_value(dev, 2, 0);
 
 	cx231xx_initialize_codec(dev);
 
 	mutex_unlock(&dev->lock);
+	v4l2_fh_add(&fh->fh);
 	cx231xx_start_TS1(dev);
 
 	return 0;
@@ -1977,25 +1716,20 @@ static int mpeg_release(struct file *file)
 
 	dprintk(3, "mpeg_release()! dev=0x%p\n", dev);
 
-	if (!dev) {
-		dprintk(3, "abort!!!\n");
-		return 0;
-	}
-
 	mutex_lock(&dev->lock);
 
 	cx231xx_stop_TS1(dev);
 
-		/* do this before setting alternate! */
-		if (dev->USE_ISO)
-			cx231xx_uninit_isoc(dev);
-		else
-			cx231xx_uninit_bulk(dev);
-		cx231xx_set_mode(dev, CX231XX_SUSPEND);
+	/* do this before setting alternate! */
+	if (dev->USE_ISO)
+		cx231xx_uninit_isoc(dev);
+	else
+		cx231xx_uninit_bulk(dev);
+	cx231xx_set_mode(dev, CX231XX_SUSPEND);
 
-		cx231xx_api_cmd(fh->dev, CX2341X_ENC_STOP_CAPTURE, 3, 0,
-				CX231xx_END_NOW, CX231xx_MPEG_CAPTURE,
-				CX231xx_RAW_BITS_NONE);
+	cx231xx_api_cmd(fh->dev, CX2341X_ENC_STOP_CAPTURE, 3, 0,
+			CX231xx_END_NOW, CX231xx_MPEG_CAPTURE,
+			CX231xx_RAW_BITS_NONE);
 
 	/* FIXME: Review this crap */
 	/* Shut device down on last close */
@@ -2015,7 +1749,8 @@ static int mpeg_release(struct file *file)
 		videobuf_read_stop(&fh->vidq);
 
 	videobuf_mmap_free(&fh->vidq);
-	file->private_data = NULL;
+	v4l2_fh_del(&fh->fh);
+	v4l2_fh_exit(&fh->fh);
 	kfree(fh);
 	mutex_unlock(&dev->lock);
 	return 0;
@@ -2027,7 +1762,6 @@ static ssize_t mpeg_read(struct file *file, char __user *data,
 	struct cx231xx_fh *fh = file->private_data;
 	struct cx231xx *dev = fh->dev;
 
-
 	/* Deal w/ A/V decoder * and mpeg encoder sync issues. */
 	/* Start mpeg encoder on first read. */
 	if (atomic_cmpxchg(&fh->v4l_reading, 0, 1) == 0) {
@@ -2044,12 +1778,23 @@ static ssize_t mpeg_read(struct file *file, char __user *data,
 static unsigned int mpeg_poll(struct file *file,
 	struct poll_table_struct *wait)
 {
+	unsigned long req_events = poll_requested_events(wait);
 	struct cx231xx_fh *fh = file->private_data;
-	/*struct cx231xx *dev = fh->dev;*/
+	struct cx231xx *dev = fh->dev;
+	unsigned int res = 0;
+
+	if (v4l2_event_pending(&fh->fh))
+		res |= POLLPRI;
+	else
+		poll_wait(file, &fh->fh.wait, wait);
 
-	/*dprintk(2, "%s\n", __func__);*/
+	if (!(req_events & (POLLIN | POLLRDNORM)))
+		return res;
 
-	return videobuf_poll_stream(file, &fh->vidq, wait);
+	mutex_lock(&dev->lock);
+	res |= videobuf_poll_stream(file, &fh->vidq, wait);
+	mutex_unlock(&dev->lock);
+	return res;
 }
 
 static int mpeg_mmap(struct file *file, struct vm_area_struct *vma)
@@ -2069,44 +1814,39 @@ static struct v4l2_file_operations mpeg_fops = {
 	.read	       = mpeg_read,
 	.poll          = mpeg_poll,
 	.mmap	       = mpeg_mmap,
-	.ioctl	       = video_ioctl2,
+	.unlocked_ioctl = video_ioctl2,
 };
 
 static const struct v4l2_ioctl_ops mpeg_ioctl_ops = {
 	.vidioc_s_std		 = vidioc_s_std,
 	.vidioc_g_std		 = vidioc_g_std,
-	.vidioc_enum_input	 = vidioc_enum_input,
-	.vidioc_enumaudio	 = vidioc_enumaudio,
-	.vidioc_g_audio		 = vidioc_g_audio,
-	.vidioc_g_input		 = vidioc_g_input,
-	.vidioc_s_input		 = vidioc_s_input,
-	.vidioc_g_tuner		 = vidioc_g_tuner,
-	.vidioc_s_tuner		 = vidioc_s_tuner,
-	.vidioc_g_frequency	 = vidioc_g_frequency,
-	.vidioc_s_frequency	 = vidioc_s_frequency,
+	.vidioc_g_tuner          = cx231xx_g_tuner,
+	.vidioc_s_tuner          = cx231xx_s_tuner,
+	.vidioc_g_frequency      = cx231xx_g_frequency,
+	.vidioc_s_frequency      = cx231xx_s_frequency,
+	.vidioc_enum_input	 = cx231xx_enum_input,
+	.vidioc_g_input		 = cx231xx_g_input,
+	.vidioc_s_input		 = cx231xx_s_input,
 	.vidioc_s_ctrl		 = vidioc_s_ctrl,
-	.vidioc_querycap	 = vidioc_querycap,
+	.vidioc_querycap	 = cx231xx_querycap,
 	.vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
 	.vidioc_g_fmt_vid_cap	 = vidioc_g_fmt_vid_cap,
 	.vidioc_try_fmt_vid_cap	 = vidioc_try_fmt_vid_cap,
-	.vidioc_s_fmt_vid_cap	 = vidioc_s_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap	 = vidioc_try_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,
-	.vidioc_g_ext_ctrls	 = vidioc_g_ext_ctrls,
-	.vidioc_s_ext_ctrls	 = vidioc_s_ext_ctrls,
-	.vidioc_try_ext_ctrls	 = vidioc_try_ext_ctrls,
 	.vidioc_log_status	 = vidioc_log_status,
-	.vidioc_querymenu	 = vidioc_querymenu,
-	.vidioc_queryctrl	 = vidioc_queryctrl,
-/*	.vidioc_g_chip_ident	 = cx231xx_g_chip_ident,*/
+	.vidioc_g_chip_ident	 = cx231xx_g_chip_ident,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
-/*	.vidioc_g_register	 = cx231xx_g_register,*/
-/*	.vidioc_s_register	 = cx231xx_s_register,*/
+	.vidioc_g_register	 = cx231xx_g_register,
+	.vidioc_s_register	 = cx231xx_s_register,
 #endif
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 static struct video_device cx231xx_mpeg_template = {
@@ -2114,8 +1854,7 @@ static struct video_device cx231xx_mpeg_template = {
 	.fops          = &mpeg_fops,
 	.ioctl_ops     = &mpeg_ioctl_ops,
 	.minor         = -1,
-	.tvnorms       = CX231xx_NORMS,
-	.current_norm  = V4L2_STD_NTSC_M,
+	.tvnorms       = V4L2_STD_ALL,
 };
 
 void cx231xx_417_unregister(struct cx231xx *dev)
@@ -2128,10 +1867,44 @@ void cx231xx_417_unregister(struct cx231xx *dev)
 			video_unregister_device(dev->v4l_device);
 		else
 			video_device_release(dev->v4l_device);
+		v4l2_ctrl_handler_free(&dev->mpeg_ctrl_handler.hdl);
 		dev->v4l_device = NULL;
 	}
 }
 
+static int cx231xx_s_video_encoding(struct cx2341x_handler *cxhdl, u32 val)
+{
+	struct cx231xx *dev = container_of(cxhdl, struct cx231xx, mpeg_ctrl_handler);
+	int is_mpeg1 = val == V4L2_MPEG_VIDEO_ENCODING_MPEG_1;
+	struct v4l2_mbus_framefmt fmt;
+
+	/* fix videodecoder resolution */
+	fmt.width = cxhdl->width / (is_mpeg1 ? 2 : 1);
+	fmt.height = cxhdl->height;
+	fmt.code = V4L2_MBUS_FMT_FIXED;
+	v4l2_subdev_call(dev->sd_cx25840, video, s_mbus_fmt, &fmt);
+	return 0;
+}
+
+static int cx231xx_s_audio_sampling_freq(struct cx2341x_handler *cxhdl, u32 idx)
+{
+	static const u32 freqs[3] = { 44100, 48000, 32000 };
+	struct cx231xx *dev = container_of(cxhdl, struct cx231xx, mpeg_ctrl_handler);
+
+	/* The audio clock of the digitizer must match the codec sample
+	   rate otherwise you get some very strange effects. */
+	if (idx < ARRAY_SIZE(freqs))
+		call_all(dev, audio, s_clock_freq, freqs[idx]);
+	return 0;
+}
+
+static struct cx2341x_handler_ops cx231xx_ops = {
+	/* needed for the video clock freq */
+	.s_audio_sampling_freq = cx231xx_s_audio_sampling_freq,
+	/* needed for setting up the video resolution */
+	.s_video_encoding = cx231xx_s_video_encoding,
+};
+
 static struct video_device *cx231xx_video_dev_alloc(
 	struct cx231xx *dev,
 	struct usb_device *usbdev,
@@ -2145,12 +1918,21 @@ static struct video_device *cx231xx_video_dev_alloc(
 	if (NULL == vfd)
 		return NULL;
 	*vfd = *template;
-	vfd->minor = -1;
 	snprintf(vfd->name, sizeof(vfd->name), "%s %s (%s)", dev->name,
 		type, cx231xx_boards[dev->model].name);
 
 	vfd->v4l2_dev = &dev->v4l2_dev;
+	vfd->lock = &dev->lock;
 	vfd->release = video_device_release;
+	set_bit(V4L2_FL_USE_FH_PRIO, &vfd->flags);
+	vfd->ctrl_handler = &dev->mpeg_ctrl_handler.hdl;
+	video_set_drvdata(vfd, dev);
+	if (dev->tuner_type == TUNER_ABSENT) {
+		v4l2_disable_ioctl(vfd, VIDIOC_G_FREQUENCY);
+		v4l2_disable_ioctl(vfd, VIDIOC_S_FREQUENCY);
+		v4l2_disable_ioctl(vfd, VIDIOC_G_TUNER);
+		v4l2_disable_ioctl(vfd, VIDIOC_S_TUNER);
+	}
 
 	return vfd;
 
@@ -2173,10 +1955,27 @@ int cx231xx_417_register(struct cx231xx *dev)
 		tsport->height = 576;
 
 	tsport->width = 720;
-	cx2341x_fill_defaults(&dev->mpeg_params);
+	err = cx2341x_handler_init(&dev->mpeg_ctrl_handler, 50);
+	if (err) {
+		dprintk(3, "%s: can't init cx2341x controls\n", dev->name);
+		return err;
+	}
+	dev->mpeg_ctrl_handler.func = cx231xx_mbox_func;
+	dev->mpeg_ctrl_handler.priv = dev;
+	dev->mpeg_ctrl_handler.ops = &cx231xx_ops;
+	if (dev->sd_cx25840)
+		v4l2_ctrl_add_handler(&dev->mpeg_ctrl_handler.hdl,
+				dev->sd_cx25840->ctrl_handler, NULL);
+	if (dev->mpeg_ctrl_handler.hdl.error) {
+		err = dev->mpeg_ctrl_handler.hdl.error;
+		dprintk(3, "%s: can't add cx25840 controls\n", dev->name);
+		v4l2_ctrl_handler_free(&dev->mpeg_ctrl_handler.hdl);
+		return err;
+	}
 	dev->norm = V4L2_STD_NTSC;
 
-	dev->mpeg_params.port = CX2341X_PORT_SERIAL;
+	dev->mpeg_ctrl_handler.port = CX2341X_PORT_SERIAL;
+	cx2341x_handler_set_50hz(&dev->mpeg_ctrl_handler, false);
 
 	/* Allocate and initialize V4L video device */
 	dev->v4l_device = cx231xx_video_dev_alloc(dev,
@@ -2185,6 +1984,7 @@ int cx231xx_417_register(struct cx231xx *dev)
 		VFL_TYPE_GRABBER, -1);
 	if (err < 0) {
 		dprintk(3, "%s: can't register mpeg device\n", dev->name);
+		v4l2_ctrl_handler_free(&dev->mpeg_ctrl_handler.hdl);
 		return err;
 	}
 
diff --git a/drivers/media/usb/cx231xx/cx231xx-audio.c b/drivers/media/usb/cx231xx/cx231xx-audio.c
index b4c99c7270c..81a1d971d79 100644
--- a/drivers/media/usb/cx231xx/cx231xx-audio.c
+++ b/drivers/media/usb/cx231xx/cx231xx-audio.c
@@ -449,9 +449,6 @@ static int snd_cx231xx_capture_open(struct snd_pcm_substream *substream)
 		return -ENODEV;
 	}
 
-	/* Sets volume, mute, etc */
-	dev->mute = 0;
-
 	/* set alternate setting for audio interface */
 	/* 1 - 48000 samples per sec */
 	mutex_lock(&dev->lock);
@@ -503,7 +500,6 @@ static int snd_cx231xx_pcm_close(struct snd_pcm_substream *substream)
 		return ret;
 	}
 
-	dev->mute = 1;
 	dev->adev.users--;
 	mutex_unlock(&dev->lock);
 
@@ -708,8 +704,8 @@ static int cx231xx_audio_init(struct cx231xx *dev)
 					    audio_index + 1];
 
 	adev->end_point_addr =
-	    le16_to_cpu(uif->altsetting[0].endpoint[isoc_pipe].desc.
-			bEndpointAddress);
+	    uif->altsetting[0].endpoint[isoc_pipe].desc.
+			bEndpointAddress;
 
 	adev->num_alt = uif->num_altsetting;
 	cx231xx_info("EndPoint Addr 0x%x, Alternate settings: %i\n",
diff --git a/drivers/media/usb/cx231xx/cx231xx-avcore.c b/drivers/media/usb/cx231xx/cx231xx-avcore.c
index 72220791374..235ba657d52 100644
--- a/drivers/media/usb/cx231xx/cx231xx-avcore.c
+++ b/drivers/media/usb/cx231xx/cx231xx-avcore.c
@@ -357,6 +357,7 @@ int cx231xx_afe_update_power_control(struct cx231xx *dev,
 	case CX231XX_BOARD_PV_PLAYTV_USB_HYBRID:
 	case CX231XX_BOARD_HAUPPAUGE_USB2_FM_PAL:
 	case CX231XX_BOARD_HAUPPAUGE_USB2_FM_NTSC:
+	case CX231XX_BOARD_OTG102:
 		if (avmode == POLARIS_AVMODE_ANALOGT_TV) {
 			while (afe_power_status != (FLD_PWRDN_TUNING_BIAS |
 						FLD_PWRDN_ENABLE_PLL)) {
@@ -1720,6 +1721,7 @@ int cx231xx_dif_set_standard(struct cx231xx *dev, u32 standard)
 	case CX231XX_BOARD_CNXT_RDU_250:
 	case CX231XX_BOARD_CNXT_VIDEO_GRABBER:
 	case CX231XX_BOARD_HAUPPAUGE_EXETER:
+	case CX231XX_BOARD_OTG102:
 		func_mode = 0x03;
 		break;
 	case CX231XX_BOARD_CNXT_RDE_253S:
@@ -2133,7 +2135,7 @@ int cx231xx_tuner_post_channel_change(struct cx231xx *dev)
 
 	status = vid_blk_write_word(dev, DIF_AGC_IF_REF, dwval);
 
-	return status;
+	return status == sizeof(dwval) ? 0 : -EIO;
 }
 
 /******************************************************************************
@@ -2221,7 +2223,7 @@ int cx231xx_set_power_mode(struct cx231xx *dev, enum AV_MODE mode)
 	if (status < 0)
 		return status;
 
-	tmp = *((u32 *) value);
+	tmp = le32_to_cpu(*((u32 *) value));
 
 	switch (mode) {
 	case POLARIS_AVMODE_ENXTERNAL_AV:
@@ -2442,7 +2444,7 @@ int cx231xx_power_suspend(struct cx231xx *dev)
 	if (status > 0)
 		return status;
 
-	tmp = *((u32 *) value);
+	tmp = le32_to_cpu(*((u32 *) value));
 	tmp &= (~PWR_MODE_MASK);
 
 	value[0] = (u8) tmp;
@@ -2470,7 +2472,7 @@ int cx231xx_start_stream(struct cx231xx *dev, u32 ep_mask)
 	if (status < 0)
 		return status;
 
-	tmp = *((u32 *) value);
+	tmp = le32_to_cpu(*((u32 *) value));
 	tmp |= ep_mask;
 	value[0] = (u8) tmp;
 	value[1] = (u8) (tmp >> 8);
@@ -2495,7 +2497,7 @@ int cx231xx_stop_stream(struct cx231xx *dev, u32 ep_mask)
 	if (status < 0)
 		return status;
 
-	tmp = *((u32 *) value);
+	tmp = le32_to_cpu(*((u32 *) value));
 	tmp &= (~ep_mask);
 	value[0] = (u8) tmp;
 	value[1] = (u8) (tmp >> 8);
@@ -2638,20 +2640,23 @@ EXPORT_SYMBOL_GPL(cx231xx_capture_start);
 /*****************************************************************************
 *                   G P I O   B I T control functions                        *
 ******************************************************************************/
-int cx231xx_set_gpio_bit(struct cx231xx *dev, u32 gpio_bit, u8 *gpio_val)
+static int cx231xx_set_gpio_bit(struct cx231xx *dev, u32 gpio_bit, u32 gpio_val)
 {
 	int status = 0;
 
-	status = cx231xx_send_gpio_cmd(dev, gpio_bit, gpio_val, 4, 0, 0);
+	gpio_val = cpu_to_le32(gpio_val);
+	status = cx231xx_send_gpio_cmd(dev, gpio_bit, (u8 *)&gpio_val, 4, 0, 0);
 
 	return status;
 }
 
-int cx231xx_get_gpio_bit(struct cx231xx *dev, u32 gpio_bit, u8 *gpio_val)
+static int cx231xx_get_gpio_bit(struct cx231xx *dev, u32 gpio_bit, u32 *gpio_val)
 {
+	u32 tmp;
 	int status = 0;
 
-	status = cx231xx_send_gpio_cmd(dev, gpio_bit, gpio_val, 4, 0, 1);
+	status = cx231xx_send_gpio_cmd(dev, gpio_bit, (u8 *)&tmp, 4, 0, 1);
+	*gpio_val = le32_to_cpu(tmp);
 
 	return status;
 }
@@ -2683,7 +2688,7 @@ int cx231xx_set_gpio_direction(struct cx231xx *dev,
 	else
 		value = dev->gpio_dir | (1 << pin_number);
 
-	status = cx231xx_set_gpio_bit(dev, value, (u8 *) &dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, value, dev->gpio_val);
 
 	/* cache the value for future */
 	dev->gpio_dir = value;
@@ -2717,7 +2722,7 @@ int cx231xx_set_gpio_value(struct cx231xx *dev, int pin_number, int pin_value)
 		value = dev->gpio_dir | (1 << pin_number);
 		dev->gpio_dir = value;
 		status = cx231xx_set_gpio_bit(dev, dev->gpio_dir,
-					      (u8 *) &dev->gpio_val);
+					      dev->gpio_val);
 		value = 0;
 	}
 
@@ -2730,7 +2735,7 @@ int cx231xx_set_gpio_value(struct cx231xx *dev, int pin_number, int pin_value)
 	dev->gpio_val = value;
 
 	/* toggle bit0 of GP_IO */
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 
 	return status;
 }
@@ -2748,7 +2753,7 @@ int cx231xx_gpio_i2c_start(struct cx231xx *dev)
 	dev->gpio_val |= 1 << dev->board.tuner_scl_gpio;
 	dev->gpio_val |= 1 << dev->board.tuner_sda_gpio;
 
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 	if (status < 0)
 		return -EINVAL;
 
@@ -2756,7 +2761,7 @@ int cx231xx_gpio_i2c_start(struct cx231xx *dev)
 	dev->gpio_val |= 1 << dev->board.tuner_scl_gpio;
 	dev->gpio_val &= ~(1 << dev->board.tuner_sda_gpio);
 
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 	if (status < 0)
 		return -EINVAL;
 
@@ -2764,7 +2769,7 @@ int cx231xx_gpio_i2c_start(struct cx231xx *dev)
 	dev->gpio_val &= ~(1 << dev->board.tuner_scl_gpio);
 	dev->gpio_val &= ~(1 << dev->board.tuner_sda_gpio);
 
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 	if (status < 0)
 		return -EINVAL;
 
@@ -2782,7 +2787,7 @@ int cx231xx_gpio_i2c_end(struct cx231xx *dev)
 	dev->gpio_val &= ~(1 << dev->board.tuner_scl_gpio);
 	dev->gpio_val &= ~(1 << dev->board.tuner_sda_gpio);
 
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 	if (status < 0)
 		return -EINVAL;
 
@@ -2790,7 +2795,7 @@ int cx231xx_gpio_i2c_end(struct cx231xx *dev)
 	dev->gpio_val |= 1 << dev->board.tuner_scl_gpio;
 	dev->gpio_val &= ~(1 << dev->board.tuner_sda_gpio);
 
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 	if (status < 0)
 		return -EINVAL;
 
@@ -2800,7 +2805,7 @@ int cx231xx_gpio_i2c_end(struct cx231xx *dev)
 	dev->gpio_dir &= ~(1 << dev->board.tuner_sda_gpio);
 
 	status =
-	    cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	    cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 	if (status < 0)
 		return -EINVAL;
 
@@ -2822,33 +2827,33 @@ int cx231xx_gpio_i2c_write_byte(struct cx231xx *dev, u8 data)
 			dev->gpio_val &= ~(1 << dev->board.tuner_scl_gpio);
 			dev->gpio_val &= ~(1 << dev->board.tuner_sda_gpio);
 			status = cx231xx_set_gpio_bit(dev, dev->gpio_dir,
-						      (u8 *)&dev->gpio_val);
+						      dev->gpio_val);
 
 			/* set SCL to output 1; set SDA to output 0     */
 			dev->gpio_val |= 1 << dev->board.tuner_scl_gpio;
 			status = cx231xx_set_gpio_bit(dev, dev->gpio_dir,
-						      (u8 *)&dev->gpio_val);
+						      dev->gpio_val);
 
 			/* set SCL to output 0; set SDA to output 0     */
 			dev->gpio_val &= ~(1 << dev->board.tuner_scl_gpio);
 			status = cx231xx_set_gpio_bit(dev, dev->gpio_dir,
-						      (u8 *)&dev->gpio_val);
+						      dev->gpio_val);
 		} else {
 			/* set SCL to output 0; set SDA to output 1     */
 			dev->gpio_val &= ~(1 << dev->board.tuner_scl_gpio);
 			dev->gpio_val |= 1 << dev->board.tuner_sda_gpio;
 			status = cx231xx_set_gpio_bit(dev, dev->gpio_dir,
-						      (u8 *)&dev->gpio_val);
+						      dev->gpio_val);
 
 			/* set SCL to output 1; set SDA to output 1     */
 			dev->gpio_val |= 1 << dev->board.tuner_scl_gpio;
 			status = cx231xx_set_gpio_bit(dev, dev->gpio_dir,
-						      (u8 *)&dev->gpio_val);
+						      dev->gpio_val);
 
 			/* set SCL to output 0; set SDA to output 1     */
 			dev->gpio_val &= ~(1 << dev->board.tuner_scl_gpio);
 			status = cx231xx_set_gpio_bit(dev, dev->gpio_dir,
-						      (u8 *)&dev->gpio_val);
+						      dev->gpio_val);
 		}
 	}
 	return status;
@@ -2867,17 +2872,17 @@ int cx231xx_gpio_i2c_read_byte(struct cx231xx *dev, u8 *buf)
 		/* set SCL to output 0; set SDA to input */
 		dev->gpio_val &= ~(1 << dev->board.tuner_scl_gpio);
 		status = cx231xx_set_gpio_bit(dev, dev->gpio_dir,
-					      (u8 *)&dev->gpio_val);
+					      dev->gpio_val);
 
 		/* set SCL to output 1; set SDA to input */
 		dev->gpio_val |= 1 << dev->board.tuner_scl_gpio;
 		status = cx231xx_set_gpio_bit(dev, dev->gpio_dir,
-					      (u8 *)&dev->gpio_val);
+					      dev->gpio_val);
 
 		/* get SDA data bit */
 		gpio_logic_value = dev->gpio_val;
 		status = cx231xx_get_gpio_bit(dev, dev->gpio_dir,
-					      (u8 *)&dev->gpio_val);
+					      &dev->gpio_val);
 		if ((dev->gpio_val & (1 << dev->board.tuner_sda_gpio)) != 0)
 			value |= (1 << (8 - i - 1));
 
@@ -2888,7 +2893,7 @@ int cx231xx_gpio_i2c_read_byte(struct cx231xx *dev, u8 *buf)
 	   !!!set SDA to input, never to modify SDA direction at
 	   the same times */
 	dev->gpio_val &= ~(1 << dev->board.tuner_scl_gpio);
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 
 	/* store the value */
 	*buf = value & 0xff;
@@ -2909,12 +2914,12 @@ int cx231xx_gpio_i2c_read_ack(struct cx231xx *dev)
 	dev->gpio_dir &= ~(1 << dev->board.tuner_scl_gpio);
 
 	gpio_logic_value = dev->gpio_val;
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 
 	do {
 		msleep(2);
 		status = cx231xx_get_gpio_bit(dev, dev->gpio_dir,
-					      (u8 *)&dev->gpio_val);
+					      &dev->gpio_val);
 		nCnt--;
 	} while (((dev->gpio_val &
 			  (1 << dev->board.tuner_scl_gpio)) == 0) &&
@@ -2929,7 +2934,7 @@ int cx231xx_gpio_i2c_read_ack(struct cx231xx *dev)
 	 * through clock stretch, slave has given a SCL signal,
 	 * so the SDA data can be directly read.
 	 */
-	status = cx231xx_get_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_get_gpio_bit(dev, dev->gpio_dir, &dev->gpio_val);
 
 	if ((dev->gpio_val & 1 << dev->board.tuner_sda_gpio) == 0) {
 		dev->gpio_val = gpio_logic_value;
@@ -2945,7 +2950,7 @@ int cx231xx_gpio_i2c_read_ack(struct cx231xx *dev)
 	dev->gpio_val = gpio_logic_value;
 	dev->gpio_dir |= (1 << dev->board.tuner_scl_gpio);
 	dev->gpio_val &= ~(1 << dev->board.tuner_scl_gpio);
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 
 	return status;
 }
@@ -2956,24 +2961,24 @@ int cx231xx_gpio_i2c_write_ack(struct cx231xx *dev)
 
 	/* set SDA to ouput */
 	dev->gpio_dir |= 1 << dev->board.tuner_sda_gpio;
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 
 	/* set SCL = 0 (output); set SDA = 0 (output) */
 	dev->gpio_val &= ~(1 << dev->board.tuner_sda_gpio);
 	dev->gpio_val &= ~(1 << dev->board.tuner_scl_gpio);
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 
 	/* set SCL = 1 (output); set SDA = 0 (output) */
 	dev->gpio_val |= 1 << dev->board.tuner_scl_gpio;
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 
 	/* set SCL = 0 (output); set SDA = 0 (output) */
 	dev->gpio_val &= ~(1 << dev->board.tuner_scl_gpio);
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 
 	/* set SDA to input,and then the slave will read data from SDA. */
 	dev->gpio_dir &= ~(1 << dev->board.tuner_sda_gpio);
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 
 	return status;
 }
@@ -2985,15 +2990,15 @@ int cx231xx_gpio_i2c_write_nak(struct cx231xx *dev)
 	/* set scl to output ; set sda to input */
 	dev->gpio_dir |= 1 << dev->board.tuner_scl_gpio;
 	dev->gpio_dir &= ~(1 << dev->board.tuner_sda_gpio);
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 
 	/* set scl to output 0; set sda to input */
 	dev->gpio_val &= ~(1 << dev->board.tuner_scl_gpio);
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 
 	/* set scl to output 1; set sda to input */
 	dev->gpio_val |= 1 << dev->board.tuner_scl_gpio;
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 
 	return status;
 }
diff --git a/drivers/media/usb/cx231xx/cx231xx-cards.c b/drivers/media/usb/cx231xx/cx231xx-cards.c
index 8d529565f16..13249e5a789 100644
--- a/drivers/media/usb/cx231xx/cx231xx-cards.c
+++ b/drivers/media/usb/cx231xx/cx231xx-cards.c
@@ -263,7 +263,11 @@ struct cx231xx_board cx231xx_boards[] = {
 		.norm = V4L2_STD_PAL,
 		.no_alt_vanc = 1,
 		.external_av = 1,
-		.has_417 = 1,
+		.dont_use_port_3 = 1,
+		/* Actually, it has a 417, but it isn't working correctly.
+		 * So set to 0 for now until someone can manage to get this
+		 * to work reliably. */
+		.has_417 = 0,
 
 		.input = {{
 				.type = CX231XX_VMUX_COMPOSITE1,
@@ -630,6 +634,39 @@ struct cx231xx_board cx231xx_boards[] = {
 			.gpio = NULL,
 		} },
 	},
+	[CX231XX_BOARD_OTG102] = {
+		.name = "Geniatech OTG102",
+		.tuner_type = TUNER_ABSENT,
+		.decoder = CX231XX_AVDECODER,
+		.output_mode = OUT_MODE_VIP11,
+		.ctl_pin_status_mask = 0xFFFFFFC4,
+		.agc_analog_digital_select_gpio = 0x0c, 
+			/* According with PV CxPlrCAP.inf file */
+		.gpio_pin_status_mask = 0x4001000,
+		.norm = V4L2_STD_NTSC,
+		.no_alt_vanc = 1,
+		.external_av = 1,
+		.dont_use_port_3 = 1,
+		/*.has_417 = 1, */
+		/* This board is believed to have a hardware encoding chip
+		 * supporting mpeg1/2/4, but as the 417 is apparently not
+		 * working for the reference board it is not here either. */
+
+		.input = {{
+				.type = CX231XX_VMUX_COMPOSITE1,
+				.vmux = CX231XX_VIN_2_1,
+				.amux = CX231XX_AMUX_LINE_IN,
+				.gpio = NULL,
+			}, {
+				.type = CX231XX_VMUX_SVIDEO,
+				.vmux = CX231XX_VIN_1_1 |
+					(CX231XX_VIN_1_2 << 8) |
+					CX25840_SVIDEO_ON,
+				.amux = CX231XX_AMUX_LINE_IN,
+				.gpio = NULL,
+			}
+		},
+	},
 };
 const unsigned int cx231xx_bcount = ARRAY_SIZE(cx231xx_boards);
 
@@ -671,6 +708,8 @@ struct usb_device_id cx231xx_id_table[] = {
 	 .driver_info = CX231XX_BOARD_ICONBIT_U100},
 	{USB_DEVICE(0x0fd9, 0x0037),
 	 .driver_info = CX231XX_BOARD_ELGATO_VIDEO_CAPTURE_V2},
+	{USB_DEVICE(0x1f4d, 0x0102),
+	 .driver_info = CX231XX_BOARD_OTG102},
 	{},
 };
 
@@ -846,8 +885,6 @@ void cx231xx_card_setup(struct cx231xx *dev)
 int cx231xx_config(struct cx231xx *dev)
 {
 	/* TBD need to add cx231xx specific code */
-	dev->mute = 1;		/* maybe not the right place... */
-	dev->volume = 0x1f;
 
 	return 0;
 }
@@ -1187,8 +1224,8 @@ static int cx231xx_usb_probe(struct usb_interface *interface,
 	uif = udev->actconfig->interface[dev->current_pcb_config.
 		       hs_config_info[0].interface_info.video_index + 1];
 
-	dev->video_mode.end_point_addr = le16_to_cpu(uif->altsetting[0].
-			endpoint[isoc_pipe].desc.bEndpointAddress);
+	dev->video_mode.end_point_addr = uif->altsetting[0].
+			endpoint[isoc_pipe].desc.bEndpointAddress;
 
 	dev->video_mode.num_alt = uif->num_altsetting;
 	cx231xx_info("EndPoint Addr 0x%x, Alternate settings: %i\n",
@@ -1221,8 +1258,8 @@ static int cx231xx_usb_probe(struct usb_interface *interface,
 				       vanc_index + 1];
 
 	dev->vbi_mode.end_point_addr =
-	    le16_to_cpu(uif->altsetting[0].endpoint[isoc_pipe].desc.
-			bEndpointAddress);
+	    uif->altsetting[0].endpoint[isoc_pipe].desc.
+			bEndpointAddress;
 
 	dev->vbi_mode.num_alt = uif->num_altsetting;
 	cx231xx_info("EndPoint Addr 0x%x, Alternate settings: %i\n",
@@ -1256,8 +1293,8 @@ static int cx231xx_usb_probe(struct usb_interface *interface,
 				       hanc_index + 1];
 
 	dev->sliced_cc_mode.end_point_addr =
-	    le16_to_cpu(uif->altsetting[0].endpoint[isoc_pipe].desc.
-			bEndpointAddress);
+	    uif->altsetting[0].endpoint[isoc_pipe].desc.
+			bEndpointAddress;
 
 	dev->sliced_cc_mode.num_alt = uif->num_altsetting;
 	cx231xx_info("EndPoint Addr 0x%x, Alternate settings: %i\n",
@@ -1292,8 +1329,8 @@ static int cx231xx_usb_probe(struct usb_interface *interface,
 					       ts1_index + 1];
 
 		dev->ts1_mode.end_point_addr =
-		    le16_to_cpu(uif->altsetting[0].endpoint[isoc_pipe].
-				desc.bEndpointAddress);
+		    uif->altsetting[0].endpoint[isoc_pipe].
+				desc.bEndpointAddress;
 
 		dev->ts1_mode.num_alt = uif->num_altsetting;
 		cx231xx_info("EndPoint Addr 0x%x, Alternate settings: %i\n",
diff --git a/drivers/media/usb/cx231xx/cx231xx-core.c b/drivers/media/usb/cx231xx/cx231xx-core.c
index 05358d48613..4ba3ce09b71 100644
--- a/drivers/media/usb/cx231xx/cx231xx-core.c
+++ b/drivers/media/usb/cx231xx/cx231xx-core.c
@@ -1488,7 +1488,7 @@ int cx231xx_mode_register(struct cx231xx *dev, u16 address, u32 mode)
 	if (status < 0)
 		return status;
 
-	tmp = *((u32 *) value);
+	tmp = le32_to_cpu(*((u32 *) value));
 	tmp |= mode;
 
 	value[0] = (u8) tmp;
diff --git a/drivers/media/usb/cx231xx/cx231xx-dvb.c b/drivers/media/usb/cx231xx/cx231xx-dvb.c
index 7c4e360ba9b..14e26106fd7 100644
--- a/drivers/media/usb/cx231xx/cx231xx-dvb.c
+++ b/drivers/media/usb/cx231xx/cx231xx-dvb.c
@@ -89,8 +89,8 @@ static struct tda18271_std_map cnxt_rde253s_tda18271_std_map = {
 };
 
 static struct tda18271_std_map mb86a20s_tda18271_config = {
-	.dvbt_6   = { .if_freq = 3300, .agc_mode = 3, .std = 4,
-		      .if_lvl = 7, .rfagc_top = 0x37, },
+	.dvbt_6   = { .if_freq = 4000, .agc_mode = 3, .std = 4,
+		      .if_lvl = 0, .rfagc_top = 0x37, },
 };
 
 static struct tda18271_config cnxt_rde253s_tunerconfig = {
diff --git a/drivers/media/usb/cx231xx/cx231xx-pcb-cfg.c b/drivers/media/usb/cx231xx/cx231xx-pcb-cfg.c
index 7473c33e823..d7308ab7a90 100644
--- a/drivers/media/usb/cx231xx/cx231xx-pcb-cfg.c
+++ b/drivers/media/usb/cx231xx/cx231xx-pcb-cfg.c
@@ -672,7 +672,7 @@ u32 initialize_cx231xx(struct cx231xx *dev)
 	pcb config it is related to */
 	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, BOARD_CFG_STAT, data, 4);
 
-	config_info = *((u32 *) data);
+	config_info = le32_to_cpu(*((u32 *) data));
 	usb_speed = (u8) (config_info & 0x1);
 
 	/* Verify this device belongs to Bus power or Self power device */
diff --git a/drivers/media/usb/cx231xx/cx231xx-pcb-cfg.h b/drivers/media/usb/cx231xx/cx231xx-pcb-cfg.h
index f5e46e89f3a..b3c6190e0c6 100644
--- a/drivers/media/usb/cx231xx/cx231xx-pcb-cfg.h
+++ b/drivers/media/usb/cx231xx/cx231xx-pcb-cfg.h
@@ -68,11 +68,6 @@ enum USB_SPEED{
 	HIGH_SPEED = 0x1	/* 1: high speed */
 };
 
-enum _true_false{
-	FALSE = 0,
-	TRUE = 1
-};
-
 #define TS_MASK         0x6
 enum TS_PORT{
 	NO_TS_PORT = 0x0,	/* 2'b00: Neither port used. PCB not a Hybrid,
diff --git a/drivers/media/usb/cx231xx/cx231xx-vbi.c b/drivers/media/usb/cx231xx/cx231xx-vbi.c
index 46e3892557c..1340ff26881 100644
--- a/drivers/media/usb/cx231xx/cx231xx-vbi.c
+++ b/drivers/media/usb/cx231xx/cx231xx-vbi.c
@@ -70,10 +70,10 @@ static inline void print_err_status(struct cx231xx *dev, int packet, int status)
 		break;
 	}
 	if (packet < 0) {
-		cx231xx_err(DRIVER_NAME "URB status %d [%s].\n", status,
+		cx231xx_err("URB status %d [%s].\n", status,
 			    errmsg);
 	} else {
-		cx231xx_err(DRIVER_NAME "URB packet %d, status %d [%s].\n",
+		cx231xx_err("URB packet %d, status %d [%s].\n",
 			    packet, status, errmsg);
 	}
 }
@@ -317,7 +317,7 @@ static void cx231xx_irq_vbi_callback(struct urb *urb)
 	case -ESHUTDOWN:
 		return;
 	default:		/* error */
-		cx231xx_err(DRIVER_NAME "urb completition error %d.\n",
+		cx231xx_err("urb completition error %d.\n",
 			    urb->status);
 		break;
 	}
@@ -332,7 +332,7 @@ static void cx231xx_irq_vbi_callback(struct urb *urb)
 
 	urb->status = usb_submit_urb(urb, GFP_ATOMIC);
 	if (urb->status) {
-		cx231xx_err(DRIVER_NAME "urb resubmit failed (error=%i)\n",
+		cx231xx_err("urb resubmit failed (error=%i)\n",
 			    urb->status);
 	}
 }
@@ -345,7 +345,7 @@ void cx231xx_uninit_vbi_isoc(struct cx231xx *dev)
 	struct urb *urb;
 	int i;
 
-	cx231xx_info(DRIVER_NAME "cx231xx: called cx231xx_uninit_vbi_isoc\n");
+	cx231xx_info("called cx231xx_uninit_vbi_isoc\n");
 
 	dev->vbi_mode.bulk_ctl.nfields = -1;
 	for (i = 0; i < dev->vbi_mode.bulk_ctl.num_bufs; i++) {
@@ -394,7 +394,7 @@ int cx231xx_init_vbi_isoc(struct cx231xx *dev, int max_packets,
 	struct urb *urb;
 	int rc;
 
-	cx231xx_info(DRIVER_NAME "cx231xx: called cx231xx_prepare_isoc\n");
+	cx231xx_info("called cx231xx_vbi_isoc\n");
 
 	/* De-allocates all pending stuff */
 	cx231xx_uninit_vbi_isoc(dev);
@@ -442,8 +442,7 @@ int cx231xx_init_vbi_isoc(struct cx231xx *dev, int max_packets,
 
 		urb = usb_alloc_urb(0, GFP_KERNEL);
 		if (!urb) {
-			cx231xx_err(DRIVER_NAME
-				    ": cannot alloc bulk_ctl.urb %i\n", i);
+			cx231xx_err("cannot alloc bulk_ctl.urb %i\n", i);
 			cx231xx_uninit_vbi_isoc(dev);
 			return -ENOMEM;
 		}
@@ -453,8 +452,7 @@ int cx231xx_init_vbi_isoc(struct cx231xx *dev, int max_packets,
 		dev->vbi_mode.bulk_ctl.transfer_buffer[i] =
 		    kzalloc(sb_size, GFP_KERNEL);
 		if (!dev->vbi_mode.bulk_ctl.transfer_buffer[i]) {
-			cx231xx_err(DRIVER_NAME
-				    ": unable to allocate %i bytes for transfer"
+			cx231xx_err("unable to allocate %i bytes for transfer"
 				    " buffer %i%s\n", sb_size, i,
 				    in_interrupt() ? " while in int" : "");
 			cx231xx_uninit_vbi_isoc(dev);
@@ -473,8 +471,7 @@ int cx231xx_init_vbi_isoc(struct cx231xx *dev, int max_packets,
 	for (i = 0; i < dev->vbi_mode.bulk_ctl.num_bufs; i++) {
 		rc = usb_submit_urb(dev->vbi_mode.bulk_ctl.urb[i], GFP_ATOMIC);
 		if (rc) {
-			cx231xx_err(DRIVER_NAME
-				    ": submit of urb %i failed (error=%i)\n", i,
+			cx231xx_err("submit of urb %i failed (error=%i)\n", i,
 				    rc);
 			cx231xx_uninit_vbi_isoc(dev);
 			return rc;
@@ -526,7 +523,7 @@ static inline void vbi_buffer_filled(struct cx231xx *dev,
 				     struct cx231xx_buffer *buf)
 {
 	/* Advice that buffer was filled */
-	/* cx231xx_info(DRIVER_NAME "[%p/%d] wakeup\n", buf, buf->vb.i); */
+	/* cx231xx_info("[%p/%d] wakeup\n", buf, buf->vb.i); */
 
 	buf->vb.state = VIDEOBUF_DONE;
 	buf->vb.field_count++;
@@ -618,7 +615,7 @@ static inline void get_next_vbi_buf(struct cx231xx_dmaqueue *dma_q,
 	char *outp;
 
 	if (list_empty(&dma_q->active)) {
-		cx231xx_err(DRIVER_NAME ": No active queue to serve\n");
+		cx231xx_err("No active queue to serve\n");
 		dev->vbi_mode.bulk_ctl.buf = NULL;
 		*buf = NULL;
 		return;
diff --git a/drivers/media/usb/cx231xx/cx231xx-video.c b/drivers/media/usb/cx231xx/cx231xx-video.c
index 06376d904c9..cd221474e1b 100644
--- a/drivers/media/usb/cx231xx/cx231xx-video.c
+++ b/drivers/media/usb/cx231xx/cx231xx-video.c
@@ -35,6 +35,7 @@
 
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
 #include <media/v4l2-chip-ident.h>
 #include <media/msp3400.h>
 #include <media/tuner.h>
@@ -100,125 +101,6 @@ static struct cx231xx_fmt format[] = {
 	 },
 };
 
-/* supported controls */
-/* Common to all boards */
-
-/* ------------------------------------------------------------------- */
-
-static const struct v4l2_queryctrl no_ctl = {
-	.name = "42",
-	.flags = V4L2_CTRL_FLAG_DISABLED,
-};
-
-static struct cx231xx_ctrl cx231xx_ctls[] = {
-	/* --- video --- */
-	{
-		.v = {
-			.id = V4L2_CID_BRIGHTNESS,
-			.name = "Brightness",
-			.minimum = 0x00,
-			.maximum = 0xff,
-			.step = 1,
-			.default_value = 0x7f,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-		},
-		.off = 128,
-		.reg = LUMA_CTRL,
-		.mask = 0x00ff,
-		.shift = 0,
-	}, {
-		.v = {
-			.id = V4L2_CID_CONTRAST,
-			.name = "Contrast",
-			.minimum = 0,
-			.maximum = 0xff,
-			.step = 1,
-			.default_value = 0x3f,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-		},
-		.off = 0,
-		.reg = LUMA_CTRL,
-		.mask = 0xff00,
-		.shift = 8,
-	}, {
-		.v = {
-			.id = V4L2_CID_HUE,
-			.name = "Hue",
-			.minimum = 0,
-			.maximum = 0xff,
-			.step = 1,
-			.default_value = 0x7f,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-		},
-		.off = 128,
-		.reg = CHROMA_CTRL,
-		.mask = 0xff0000,
-		.shift = 16,
-	}, {
-	/* strictly, this only describes only U saturation.
-	* V saturation is handled specially through code.
-	*/
-		.v = {
-			.id = V4L2_CID_SATURATION,
-			.name = "Saturation",
-			.minimum = 0,
-			.maximum = 0xff,
-			.step = 1,
-			.default_value = 0x7f,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-		},
-		.off = 0,
-		.reg = CHROMA_CTRL,
-		.mask = 0x00ff,
-		.shift = 0,
-	}, {
-		/* --- audio --- */
-		.v = {
-			.id = V4L2_CID_AUDIO_MUTE,
-			.name = "Mute",
-			.minimum = 0,
-			.maximum = 1,
-			.default_value = 1,
-			.type = V4L2_CTRL_TYPE_BOOLEAN,
-		},
-		.reg = PATH1_CTL1,
-		.mask = (0x1f << 24),
-		.shift = 24,
-	}, {
-		.v = {
-			.id = V4L2_CID_AUDIO_VOLUME,
-			.name = "Volume",
-			.minimum = 0,
-			.maximum = 0x3f,
-			.step = 1,
-			.default_value = 0x3f,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-		},
-		.reg = PATH1_VOL_CTL,
-		.mask = 0xff,
-		.shift = 0,
-	}
-};
-static const int CX231XX_CTLS = ARRAY_SIZE(cx231xx_ctls);
-
-static const u32 cx231xx_user_ctrls[] = {
-	V4L2_CID_USER_CLASS,
-	V4L2_CID_BRIGHTNESS,
-	V4L2_CID_CONTRAST,
-	V4L2_CID_SATURATION,
-	V4L2_CID_HUE,
-	V4L2_CID_AUDIO_VOLUME,
-#if 0
-	V4L2_CID_AUDIO_BALANCE,
-#endif
-	V4L2_CID_AUDIO_MUTE,
-	0
-};
-
-static const u32 *ctrl_classes[] = {
-	cx231xx_user_ctrls,
-	NULL
-};
 
 /* ------------------------------------------------------------------
 	Video buffer and parser functions
@@ -1005,6 +887,7 @@ static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 
 	f->fmt.pix.field = V4L2_FIELD_INTERLACED;
+	f->fmt.pix.priv = 0;
 
 	return 0;
 }
@@ -1045,10 +928,11 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 	f->fmt.pix.width = width;
 	f->fmt.pix.height = height;
 	f->fmt.pix.pixelformat = fmt->fourcc;
-	f->fmt.pix.bytesperline = (dev->width * fmt->depth + 7) >> 3;
+	f->fmt.pix.bytesperline = (width * fmt->depth + 7) >> 3;
 	f->fmt.pix.sizeimage = f->fmt.pix.bytesperline * height;
 	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 	f->fmt.pix.field = V4L2_FIELD_INTERLACED;
+	f->fmt.pix.priv = 0;
 
 	return 0;
 }
@@ -1103,39 +987,39 @@ static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *id)
 	return 0;
 }
 
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *norm)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id norm)
 {
 	struct cx231xx_fh *fh = priv;
 	struct cx231xx *dev = fh->dev;
 	struct v4l2_mbus_framefmt mbus_fmt;
-	struct v4l2_format f;
 	int rc;
 
 	rc = check_dev(dev);
 	if (rc < 0)
 		return rc;
 
-	cx231xx_info("vidioc_s_std : 0x%x\n", (unsigned int)*norm);
+	if (dev->norm == norm)
+		return 0;
 
-	dev->norm = *norm;
+	if (videobuf_queue_is_busy(&fh->vb_vidq))
+		return -EBUSY;
+
+	dev->norm = norm;
 
 	/* Adjusts width/height, if needed */
-	f.fmt.pix.width = dev->width;
-	f.fmt.pix.height = dev->height;
-	vidioc_try_fmt_vid_cap(file, priv, &f);
+	dev->width = 720;
+	dev->height = (dev->norm & V4L2_STD_625_50) ? 576 : 480;
 
 	call_all(dev, core, s_std, dev->norm);
 
 	/* We need to reset basic properties in the decoder related to
 	   resolution (since a standard change effects things like the number
 	   of lines in VACT, etc) */
-	v4l2_fill_mbus_format(&mbus_fmt, &f.fmt.pix, V4L2_MBUS_FMT_FIXED);
+	memset(&mbus_fmt, 0, sizeof(mbus_fmt));
+	mbus_fmt.code = V4L2_MBUS_FMT_FIXED;
+	mbus_fmt.width = dev->width;
+	mbus_fmt.height = dev->height;
 	call_all(dev, video, s_mbus_fmt, &mbus_fmt);
-	v4l2_fill_pix_format(&f.fmt.pix, &mbus_fmt);
-
-	/* set new image size */
-	dev->width = f.fmt.pix.width;
-	dev->height = f.fmt.pix.height;
 
 	/* do mode control overrides */
 	cx231xx_do_mode_ctrl_overrides(dev);
@@ -1152,7 +1036,7 @@ static const char *iname[] = {
 	[CX231XX_VMUX_DEBUG]      = "for debug only",
 };
 
-static int vidioc_enum_input(struct file *file, void *priv,
+int cx231xx_enum_input(struct file *file, void *priv,
 			     struct v4l2_input *i)
 {
 	struct cx231xx_fh *fh = priv;
@@ -1192,7 +1076,7 @@ static int vidioc_enum_input(struct file *file, void *priv,
 	return 0;
 }
 
-static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
+int cx231xx_g_input(struct file *file, void *priv, unsigned int *i)
 {
 	struct cx231xx_fh *fh = priv;
 	struct cx231xx *dev = fh->dev;
@@ -1202,7 +1086,7 @@ static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
 	return 0;
 }
 
-static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
+int cx231xx_s_input(struct file *file, void *priv, unsigned int i)
 {
 	struct cx231xx_fh *fh = priv;
 	struct cx231xx *dev = fh->dev;
@@ -1231,117 +1115,7 @@ static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
 	return 0;
 }
 
-static int vidioc_g_audio(struct file *file, void *priv, struct v4l2_audio *a)
-{
-	struct cx231xx_fh *fh = priv;
-	struct cx231xx *dev = fh->dev;
-
-	switch (a->index) {
-	case CX231XX_AMUX_VIDEO:
-		strcpy(a->name, "Television");
-		break;
-	case CX231XX_AMUX_LINE_IN:
-		strcpy(a->name, "Line In");
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	a->index = dev->ctl_ainput;
-	a->capability = V4L2_AUDCAP_STEREO;
-
-	return 0;
-}
-
-static int vidioc_s_audio(struct file *file, void *priv, const struct v4l2_audio *a)
-{
-	struct cx231xx_fh *fh = priv;
-	struct cx231xx *dev = fh->dev;
-	int status = 0;
-
-	/* Doesn't allow manual routing */
-	if (a->index != dev->ctl_ainput)
-		return -EINVAL;
-
-	dev->ctl_ainput = INPUT(a->index)->amux;
-	status = cx231xx_set_audio_input(dev, dev->ctl_ainput);
-
-	return status;
-}
-
-static int vidioc_queryctrl(struct file *file, void *priv,
-			    struct v4l2_queryctrl *qc)
-{
-	struct cx231xx_fh *fh = priv;
-	struct cx231xx *dev = fh->dev;
-	int id = qc->id;
-	int i;
-	int rc;
-
-	rc = check_dev(dev);
-	if (rc < 0)
-		return rc;
-
-	qc->id = v4l2_ctrl_next(ctrl_classes, qc->id);
-	if (unlikely(qc->id == 0))
-		return -EINVAL;
-
-	memset(qc, 0, sizeof(*qc));
-
-	qc->id = id;
-
-	if (qc->id < V4L2_CID_BASE || qc->id >= V4L2_CID_LASTP1)
-		return -EINVAL;
-
-	for (i = 0; i < CX231XX_CTLS; i++)
-		if (cx231xx_ctls[i].v.id == qc->id)
-			break;
-
-	if (i == CX231XX_CTLS) {
-		*qc = no_ctl;
-		return 0;
-	}
-	*qc = cx231xx_ctls[i].v;
-
-	call_all(dev, core, queryctrl, qc);
-
-	if (qc->type)
-		return 0;
-	else
-		return -EINVAL;
-}
-
-static int vidioc_g_ctrl(struct file *file, void *priv,
-			 struct v4l2_control *ctrl)
-{
-	struct cx231xx_fh *fh = priv;
-	struct cx231xx *dev = fh->dev;
-	int rc;
-
-	rc = check_dev(dev);
-	if (rc < 0)
-		return rc;
-
-	call_all(dev, core, g_ctrl, ctrl);
-	return rc;
-}
-
-static int vidioc_s_ctrl(struct file *file, void *priv,
-			 struct v4l2_control *ctrl)
-{
-	struct cx231xx_fh *fh = priv;
-	struct cx231xx *dev = fh->dev;
-	int rc;
-
-	rc = check_dev(dev);
-	if (rc < 0)
-		return rc;
-
-	call_all(dev, core, s_ctrl, ctrl);
-	return rc;
-}
-
-static int vidioc_g_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
+int cx231xx_g_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
 {
 	struct cx231xx_fh *fh = priv;
 	struct cx231xx *dev = fh->dev;
@@ -1360,11 +1134,12 @@ static int vidioc_g_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
 	t->capability = V4L2_TUNER_CAP_NORM;
 	t->rangehigh = 0xffffffffUL;
 	t->signal = 0xffff;	/* LOCKED */
+	call_all(dev, tuner, g_tuner, t);
 
 	return 0;
 }
 
-static int vidioc_s_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
+int cx231xx_s_tuner(struct file *file, void *priv, const struct v4l2_tuner *t)
 {
 	struct cx231xx_fh *fh = priv;
 	struct cx231xx *dev = fh->dev;
@@ -1382,25 +1157,26 @@ static int vidioc_s_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
 	return 0;
 }
 
-static int vidioc_g_frequency(struct file *file, void *priv,
+int cx231xx_g_frequency(struct file *file, void *priv,
 			      struct v4l2_frequency *f)
 {
 	struct cx231xx_fh *fh = priv;
 	struct cx231xx *dev = fh->dev;
 
-	f->type = fh->radio ? V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
-	f->frequency = dev->ctl_freq;
+	if (f->tuner)
+		return -EINVAL;
 
-	call_all(dev, tuner, g_frequency, f);
+	f->frequency = dev->ctl_freq;
 
 	return 0;
 }
 
-static int vidioc_s_frequency(struct file *file, void *priv,
-			      struct v4l2_frequency *f)
+int cx231xx_s_frequency(struct file *file, void *priv,
+			      const struct v4l2_frequency *f)
 {
 	struct cx231xx_fh *fh = priv;
 	struct cx231xx *dev = fh->dev;
+	struct v4l2_frequency new_freq = *f;
 	int rc;
 	u32 if_frequency = 5400000;
 
@@ -1415,16 +1191,12 @@ static int vidioc_s_frequency(struct file *file, void *priv,
 	if (0 != f->tuner)
 		return -EINVAL;
 
-	if (unlikely(0 == fh->radio && f->type != V4L2_TUNER_ANALOG_TV))
-		return -EINVAL;
-	if (unlikely(1 == fh->radio && f->type != V4L2_TUNER_RADIO))
-		return -EINVAL;
-
 	/* set pre channel change settings in DIF first */
 	rc = cx231xx_tuner_pre_channel_change(dev);
 
-	dev->ctl_freq = f->frequency;
 	call_all(dev, tuner, s_frequency, f);
+	call_all(dev, tuner, g_frequency, &new_freq);
+	dev->ctl_freq = new_freq.frequency;
 
 	/* set post channel change settings in DIF first */
 	rc = cx231xx_tuner_post_channel_change(dev);
@@ -1456,6 +1228,19 @@ static int vidioc_s_frequency(struct file *file, void *priv,
 	return rc;
 }
 
+int cx231xx_g_chip_ident(struct file *file, void *fh,
+			struct v4l2_dbg_chip_ident *chip)
+{
+	chip->ident = V4L2_IDENT_NONE;
+	chip->revision = 0;
+	if (chip->match.type == V4L2_CHIP_MATCH_HOST) {
+		if (v4l2_chip_match_host(&chip->match))
+			chip->ident = V4L2_IDENT_CX23100;
+		return 0;
+	}
+	return -EINVAL;
+}
+
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 
 /*
@@ -1471,7 +1256,7 @@ static int vidioc_s_frequency(struct file *file, void *priv,
   if type == i2caddr, then <chip> is the 7-bit I2C address
 */
 
-static int vidioc_g_register(struct file *file, void *priv,
+int cx231xx_g_register(struct file *file, void *priv,
 			     struct v4l2_dbg_register *reg)
 {
 	struct cx231xx_fh *fh = priv;
@@ -1618,8 +1403,8 @@ static int vidioc_g_register(struct file *file, void *priv,
 	return ret;
 }
 
-static int vidioc_s_register(struct file *file, void *priv,
-			     struct v4l2_dbg_register *reg)
+int cx231xx_s_register(struct file *file, void *priv,
+			     const struct v4l2_dbg_register *reg)
 {
 	struct cx231xx_fh *fh = priv;
 	struct cx231xx *dev = fh->dev;
@@ -1837,9 +1622,6 @@ static int vidioc_streamoff(struct file *file, void *priv,
 	if (rc < 0)
 		return rc;
 
-	if ((fh->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) &&
-	    (fh->type != V4L2_BUF_TYPE_VBI_CAPTURE))
-		return -EINVAL;
 	if (type != fh->type)
 		return -EINVAL;
 
@@ -1851,9 +1633,10 @@ static int vidioc_streamoff(struct file *file, void *priv,
 	return 0;
 }
 
-static int vidioc_querycap(struct file *file, void *priv,
+int cx231xx_querycap(struct file *file, void *priv,
 			   struct v4l2_capability *cap)
 {
+	struct video_device *vdev = video_devdata(file);
 	struct cx231xx_fh *fh = priv;
 	struct cx231xx *dev = fh->dev;
 
@@ -1861,17 +1644,22 @@ static int vidioc_querycap(struct file *file, void *priv,
 	strlcpy(cap->card, cx231xx_boards[dev->model].name, sizeof(cap->card));
 	usb_make_path(dev->udev, cap->bus_info, sizeof(cap->bus_info));
 
-	cap->capabilities = V4L2_CAP_VBI_CAPTURE |
-#if 0
-		V4L2_CAP_SLICED_VBI_CAPTURE |
-#endif
-		V4L2_CAP_VIDEO_CAPTURE	|
-		V4L2_CAP_AUDIO		|
-		V4L2_CAP_READWRITE	|
-		V4L2_CAP_STREAMING;
-
+	if (vdev->vfl_type == VFL_TYPE_RADIO)
+		cap->device_caps = V4L2_CAP_RADIO;
+	else {
+		cap->device_caps = V4L2_CAP_READWRITE | V4L2_CAP_STREAMING;
+		if (vdev->vfl_type == VFL_TYPE_VBI)
+			cap->device_caps |= V4L2_CAP_VBI_CAPTURE;
+		else
+			cap->device_caps |= V4L2_CAP_VIDEO_CAPTURE;
+	}
 	if (dev->tuner_type != TUNER_ABSENT)
-		cap->capabilities |= V4L2_CAP_TUNER;
+		cap->device_caps |= V4L2_CAP_TUNER;
+	cap->capabilities = cap->device_caps | V4L2_CAP_READWRITE |
+		V4L2_CAP_VBI_CAPTURE | V4L2_CAP_VIDEO_CAPTURE |
+		V4L2_CAP_STREAMING | V4L2_CAP_DEVICE_CAPS;
+	if (dev->radio_dev)
+		cap->capabilities |= V4L2_CAP_RADIO;
 
 	return 0;
 }
@@ -1888,47 +1676,6 @@ static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
 	return 0;
 }
 
-/* Sliced VBI ioctls */
-static int vidioc_g_fmt_sliced_vbi_cap(struct file *file, void *priv,
-				       struct v4l2_format *f)
-{
-	struct cx231xx_fh *fh = priv;
-	struct cx231xx *dev = fh->dev;
-	int rc;
-
-	rc = check_dev(dev);
-	if (rc < 0)
-		return rc;
-
-	f->fmt.sliced.service_set = 0;
-
-	call_all(dev, vbi, g_sliced_fmt, &f->fmt.sliced);
-
-	if (f->fmt.sliced.service_set == 0)
-		rc = -EINVAL;
-
-	return rc;
-}
-
-static int vidioc_try_set_sliced_vbi_cap(struct file *file, void *priv,
-					 struct v4l2_format *f)
-{
-	struct cx231xx_fh *fh = priv;
-	struct cx231xx *dev = fh->dev;
-	int rc;
-
-	rc = check_dev(dev);
-	if (rc < 0)
-		return rc;
-
-	call_all(dev, vbi, g_sliced_fmt, &f->fmt.sliced);
-
-	if (f->fmt.sliced.service_set == 0)
-		return -EINVAL;
-
-	return 0;
-}
-
 /* RAW VBI ioctls */
 
 static int vidioc_g_fmt_vbi_cap(struct file *file, void *priv,
@@ -1936,6 +1683,7 @@ static int vidioc_g_fmt_vbi_cap(struct file *file, void *priv,
 {
 	struct cx231xx_fh *fh = priv;
 	struct cx231xx *dev = fh->dev;
+
 	f->fmt.vbi.sampling_rate = 6750000 * 4;
 	f->fmt.vbi.samples_per_line = VBI_LINE_LENGTH;
 	f->fmt.vbi.sample_format = V4L2_PIX_FMT_GREY;
@@ -1947,6 +1695,7 @@ static int vidioc_g_fmt_vbi_cap(struct file *file, void *priv,
 	f->fmt.vbi.start[1] = (dev->norm & V4L2_STD_625_50) ?
 	    PAL_VBI_START_LINE + 312 : NTSC_VBI_START_LINE + 263;
 	f->fmt.vbi.count[1] = f->fmt.vbi.count[0];
+	memset(f->fmt.vbi.reserved, 0, sizeof(f->fmt.vbi.reserved));
 
 	return 0;
 
@@ -1958,12 +1707,6 @@ static int vidioc_try_fmt_vbi_cap(struct file *file, void *priv,
 	struct cx231xx_fh *fh = priv;
 	struct cx231xx *dev = fh->dev;
 
-	if (dev->vbi_stream_on && !fh->stream_on) {
-		cx231xx_errdev("%s device in use by another fh\n", __func__);
-		return -EBUSY;
-	}
-
-	f->type = V4L2_BUF_TYPE_VBI_CAPTURE;
 	f->fmt.vbi.sampling_rate = 6750000 * 4;
 	f->fmt.vbi.samples_per_line = VBI_LINE_LENGTH;
 	f->fmt.vbi.sample_format = V4L2_PIX_FMT_GREY;
@@ -1976,11 +1719,25 @@ static int vidioc_try_fmt_vbi_cap(struct file *file, void *priv,
 	f->fmt.vbi.start[1] = (dev->norm & V4L2_STD_625_50) ?
 	    PAL_VBI_START_LINE + 312 : NTSC_VBI_START_LINE + 263;
 	f->fmt.vbi.count[1] = f->fmt.vbi.count[0];
+	memset(f->fmt.vbi.reserved, 0, sizeof(f->fmt.vbi.reserved));
 
 	return 0;
 
 }
 
+static int vidioc_s_fmt_vbi_cap(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct cx231xx_fh *fh = priv;
+	struct cx231xx *dev = fh->dev;
+
+	if (dev->vbi_stream_on && !fh->stream_on) {
+		cx231xx_errdev("%s device in use by another fh\n", __func__);
+		return -EBUSY;
+	}
+	return vidioc_try_fmt_vbi_cap(file, priv, f);
+}
+
 static int vidioc_reqbufs(struct file *file, void *priv,
 			  struct v4l2_requestbuffers *rb)
 {
@@ -2038,58 +1795,24 @@ static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *b)
 /* RADIO ESPECIFIC IOCTLS                                      */
 /* ----------------------------------------------------------- */
 
-static int radio_querycap(struct file *file, void *priv,
-			  struct v4l2_capability *cap)
-{
-	struct cx231xx *dev = ((struct cx231xx_fh *)priv)->dev;
-
-	strlcpy(cap->driver, "cx231xx", sizeof(cap->driver));
-	strlcpy(cap->card, cx231xx_boards[dev->model].name, sizeof(cap->card));
-	usb_make_path(dev->udev, cap->bus_info, sizeof(cap->bus_info));
-
-	cap->capabilities = V4L2_CAP_TUNER;
-	return 0;
-}
-
 static int radio_g_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
 {
 	struct cx231xx *dev = ((struct cx231xx_fh *)priv)->dev;
 
-	if (unlikely(t->index > 0))
+	if (t->index)
 		return -EINVAL;
 
 	strcpy(t->name, "Radio");
-	t->type = V4L2_TUNER_RADIO;
-
-	call_all(dev, tuner, s_tuner, t);
-
-	return 0;
-}
 
-static int radio_enum_input(struct file *file, void *priv, struct v4l2_input *i)
-{
-	if (i->index != 0)
-		return -EINVAL;
-	strcpy(i->name, "Radio");
-	i->type = V4L2_INPUT_TYPE_TUNER;
+	call_all(dev, tuner, g_tuner, t);
 
 	return 0;
 }
-
-static int radio_g_audio(struct file *file, void *priv, struct v4l2_audio *a)
-{
-	if (unlikely(a->index))
-		return -EINVAL;
-
-	strcpy(a->name, "Radio");
-	return 0;
-}
-
-static int radio_s_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
+static int radio_s_tuner(struct file *file, void *priv, const struct v4l2_tuner *t)
 {
 	struct cx231xx *dev = ((struct cx231xx_fh *)priv)->dev;
 
-	if (0 != t->index)
+	if (t->index)
 		return -EINVAL;
 
 	call_all(dev, tuner, s_tuner, t);
@@ -2097,36 +1820,6 @@ static int radio_s_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
 	return 0;
 }
 
-static int radio_s_audio(struct file *file, void *fh, const struct v4l2_audio *a)
-{
-	return 0;
-}
-
-static int radio_s_input(struct file *file, void *fh, unsigned int i)
-{
-	return 0;
-}
-
-static int radio_queryctrl(struct file *file, void *priv,
-			   struct v4l2_queryctrl *c)
-{
-	int i;
-
-	if (c->id < V4L2_CID_BASE || c->id >= V4L2_CID_LASTP1)
-		return -EINVAL;
-	if (c->id == V4L2_CID_AUDIO_MUTE) {
-		for (i = 0; i < CX231XX_CTLS; i++) {
-			if (cx231xx_ctls[i].v.id == c->id)
-				break;
-		}
-		if (i == CX231XX_CTLS)
-			return -EINVAL;
-		*c = cx231xx_ctls[i].v;
-	} else
-		*c = no_ctl;
-	return 0;
-}
-
 /*
  * cx231xx_v4l2_open()
  * inits the device and starts isoc transfer
@@ -2174,14 +1867,11 @@ static int cx231xx_v4l2_open(struct file *filp)
 		return -ERESTARTSYS;
 	}
 	fh->dev = dev;
-	fh->radio = radio;
 	fh->type = fh_type;
 	filp->private_data = fh;
+	v4l2_fh_init(&fh->fh, vdev);
 
 	if (fh->type == V4L2_BUF_TYPE_VIDEO_CAPTURE && dev->users == 0) {
-		dev->width = norm_maxw(dev);
-		dev->height = norm_maxh(dev);
-
 		/* Power up in Analog TV mode */
 		if (dev->board.external_av)
 			cx231xx_set_power_mode(dev,
@@ -2204,7 +1894,7 @@ static int cx231xx_v4l2_open(struct file *filp)
 		dev->video_input = dev->video_input > 2 ? 2 : dev->video_input;
 
 	}
-	if (fh->radio) {
+	if (radio) {
 		cx231xx_videodbg("video_open: setting radio device\n");
 
 		/* cx231xx_start_radio(dev); */
@@ -2232,6 +1922,7 @@ static int cx231xx_v4l2_open(struct file *filp)
 					    fh, &dev->lock);
 	}
 	mutex_unlock(&dev->lock);
+	v4l2_fh_add(&fh->fh);
 
 	return errCode;
 }
@@ -2275,6 +1966,8 @@ void cx231xx_release_analog_resources(struct cx231xx *dev)
 			video_device_release(dev->vdev);
 		dev->vdev = NULL;
 	}
+	v4l2_ctrl_handler_free(&dev->ctrl_handler);
+	v4l2_ctrl_handler_free(&dev->radio_ctrl_handler);
 }
 
 /*
@@ -2324,12 +2017,15 @@ static int cx231xx_close(struct file *filp)
 			else
 				cx231xx_set_alt_setting(dev, INDEX_HANC, 0);
 
+			v4l2_fh_del(&fh->fh);
+			v4l2_fh_exit(&fh->fh);
 			kfree(fh);
 			dev->users--;
 			wake_up_interruptible_nr(&dev->open, 1);
 			return 0;
 		}
 
+	v4l2_fh_del(&fh->fh);
 	dev->users--;
 	if (!dev->users) {
 		videobuf_stop(&fh->vb_vidq);
@@ -2356,6 +2052,7 @@ static int cx231xx_close(struct file *filp)
 		/* set alternate 0 */
 		cx231xx_set_alt_setting(dev, INDEX_VIDEO, 0);
 	}
+	v4l2_fh_exit(&fh->fh);
 	kfree(fh);
 	wake_up_interruptible_nr(&dev->open, 1);
 	return 0;
@@ -2412,29 +2109,37 @@ cx231xx_v4l2_read(struct file *filp, char __user *buf, size_t count,
  */
 static unsigned int cx231xx_v4l2_poll(struct file *filp, poll_table *wait)
 {
+	unsigned long req_events = poll_requested_events(wait);
 	struct cx231xx_fh *fh = filp->private_data;
 	struct cx231xx *dev = fh->dev;
+	unsigned res = 0;
 	int rc;
 
 	rc = check_dev(dev);
 	if (rc < 0)
-		return rc;
+		return POLLERR;
 
 	rc = res_get(fh);
 
 	if (unlikely(rc < 0))
 		return POLLERR;
 
+	if (v4l2_event_pending(&fh->fh))
+		res |= POLLPRI;
+	else
+		poll_wait(filp, &fh->fh.wait, wait);
+
+	if (!(req_events & (POLLIN | POLLRDNORM)))
+		return res;
+
 	if ((V4L2_BUF_TYPE_VIDEO_CAPTURE == fh->type) ||
 	    (V4L2_BUF_TYPE_VBI_CAPTURE == fh->type)) {
-		unsigned int res;
-
 		mutex_lock(&dev->lock);
-		res = videobuf_poll_stream(filp, &fh->vb_vidq, wait);
+		res |= videobuf_poll_stream(filp, &fh->vb_vidq, wait);
 		mutex_unlock(&dev->lock);
 		return res;
 	}
-	return POLLERR;
+	return res | POLLERR;
 }
 
 /*
@@ -2479,41 +2184,37 @@ static const struct v4l2_file_operations cx231xx_v4l_fops = {
 };
 
 static const struct v4l2_ioctl_ops video_ioctl_ops = {
-	.vidioc_querycap               = vidioc_querycap,
+	.vidioc_querycap               = cx231xx_querycap,
 	.vidioc_enum_fmt_vid_cap       = vidioc_enum_fmt_vid_cap,
 	.vidioc_g_fmt_vid_cap          = vidioc_g_fmt_vid_cap,
 	.vidioc_try_fmt_vid_cap        = vidioc_try_fmt_vid_cap,
 	.vidioc_s_fmt_vid_cap          = vidioc_s_fmt_vid_cap,
 	.vidioc_g_fmt_vbi_cap          = vidioc_g_fmt_vbi_cap,
 	.vidioc_try_fmt_vbi_cap        = vidioc_try_fmt_vbi_cap,
-	.vidioc_s_fmt_vbi_cap          = vidioc_try_fmt_vbi_cap,
-	.vidioc_g_audio                =  vidioc_g_audio,
-	.vidioc_s_audio                = vidioc_s_audio,
+	.vidioc_s_fmt_vbi_cap          = vidioc_s_fmt_vbi_cap,
 	.vidioc_cropcap                = vidioc_cropcap,
-	.vidioc_g_fmt_sliced_vbi_cap   = vidioc_g_fmt_sliced_vbi_cap,
-	.vidioc_try_fmt_sliced_vbi_cap = vidioc_try_set_sliced_vbi_cap,
 	.vidioc_reqbufs                = vidioc_reqbufs,
 	.vidioc_querybuf               = vidioc_querybuf,
 	.vidioc_qbuf                   = vidioc_qbuf,
 	.vidioc_dqbuf                  = vidioc_dqbuf,
 	.vidioc_s_std                  = vidioc_s_std,
 	.vidioc_g_std                  = vidioc_g_std,
-	.vidioc_enum_input             = vidioc_enum_input,
-	.vidioc_g_input                = vidioc_g_input,
-	.vidioc_s_input                = vidioc_s_input,
-	.vidioc_queryctrl              = vidioc_queryctrl,
-	.vidioc_g_ctrl                 = vidioc_g_ctrl,
-	.vidioc_s_ctrl                 = vidioc_s_ctrl,
+	.vidioc_enum_input             = cx231xx_enum_input,
+	.vidioc_g_input                = cx231xx_g_input,
+	.vidioc_s_input                = cx231xx_s_input,
 	.vidioc_streamon               = vidioc_streamon,
 	.vidioc_streamoff              = vidioc_streamoff,
-	.vidioc_g_tuner                = vidioc_g_tuner,
-	.vidioc_s_tuner                = vidioc_s_tuner,
-	.vidioc_g_frequency            = vidioc_g_frequency,
-	.vidioc_s_frequency            = vidioc_s_frequency,
+	.vidioc_g_tuner                = cx231xx_g_tuner,
+	.vidioc_s_tuner                = cx231xx_s_tuner,
+	.vidioc_g_frequency            = cx231xx_g_frequency,
+	.vidioc_s_frequency            = cx231xx_s_frequency,
+	.vidioc_g_chip_ident           = cx231xx_g_chip_ident,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
-	.vidioc_g_register             = vidioc_g_register,
-	.vidioc_s_register             = vidioc_s_register,
+	.vidioc_g_register             = cx231xx_g_register,
+	.vidioc_s_register             = cx231xx_s_register,
 #endif
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 static struct video_device cx231xx_vbi_template;
@@ -2523,33 +2224,29 @@ static const struct video_device cx231xx_video_template = {
 	.release      = video_device_release,
 	.ioctl_ops    = &video_ioctl_ops,
 	.tvnorms      = V4L2_STD_ALL,
-	.current_norm = V4L2_STD_PAL,
 };
 
 static const struct v4l2_file_operations radio_fops = {
 	.owner   = THIS_MODULE,
 	.open   = cx231xx_v4l2_open,
 	.release = cx231xx_v4l2_close,
-	.ioctl   = video_ioctl2,
+	.poll = v4l2_ctrl_poll,
+	.unlocked_ioctl = video_ioctl2,
 };
 
 static const struct v4l2_ioctl_ops radio_ioctl_ops = {
-	.vidioc_querycap    = radio_querycap,
+	.vidioc_querycap    = cx231xx_querycap,
 	.vidioc_g_tuner     = radio_g_tuner,
-	.vidioc_enum_input  = radio_enum_input,
-	.vidioc_g_audio     = radio_g_audio,
 	.vidioc_s_tuner     = radio_s_tuner,
-	.vidioc_s_audio     = radio_s_audio,
-	.vidioc_s_input     = radio_s_input,
-	.vidioc_queryctrl   = radio_queryctrl,
-	.vidioc_g_ctrl      = vidioc_g_ctrl,
-	.vidioc_s_ctrl      = vidioc_s_ctrl,
-	.vidioc_g_frequency = vidioc_g_frequency,
-	.vidioc_s_frequency = vidioc_s_frequency,
+	.vidioc_g_frequency = cx231xx_g_frequency,
+	.vidioc_s_frequency = cx231xx_s_frequency,
+	.vidioc_g_chip_ident = cx231xx_g_chip_ident,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
-	.vidioc_g_register  = vidioc_g_register,
-	.vidioc_s_register  = vidioc_s_register,
+	.vidioc_g_register  = cx231xx_g_register,
+	.vidioc_s_register  = cx231xx_s_register,
 #endif
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 static struct video_device cx231xx_radio_template = {
@@ -2575,10 +2272,17 @@ static struct video_device *cx231xx_vdev_init(struct cx231xx *dev,
 	vfd->release = video_device_release;
 	vfd->debug = video_debug;
 	vfd->lock = &dev->lock;
+	set_bit(V4L2_FL_USE_FH_PRIO, &vfd->flags);
 
 	snprintf(vfd->name, sizeof(vfd->name), "%s %s", dev->name, type_name);
 
 	video_set_drvdata(vfd, dev);
+	if (dev->tuner_type == TUNER_ABSENT) {
+		v4l2_disable_ioctl(vfd, VIDIOC_G_FREQUENCY);
+		v4l2_disable_ioctl(vfd, VIDIOC_S_FREQUENCY);
+		v4l2_disable_ioctl(vfd, VIDIOC_G_TUNER);
+		v4l2_disable_ioctl(vfd, VIDIOC_S_TUNER);
+	}
 	return vfd;
 }
 
@@ -2590,7 +2294,7 @@ int cx231xx_register_analog_devices(struct cx231xx *dev)
 		     dev->name, CX231XX_VERSION);
 
 	/* set default norm */
-	/*dev->norm = cx231xx_video_template.current_norm; */
+	dev->norm = V4L2_STD_PAL;
 	dev->width = norm_maxw(dev);
 	dev->height = norm_maxh(dev);
 	dev->interlaced = 0;
@@ -2601,9 +2305,23 @@ int cx231xx_register_analog_devices(struct cx231xx *dev)
 	/* Set the initial input */
 	video_mux(dev, dev->video_input);
 
-	/* Audio defaults */
-	dev->mute = 1;
-	dev->volume = 0x1f;
+	call_all(dev, core, s_std, dev->norm);
+
+	v4l2_ctrl_handler_init(&dev->ctrl_handler, 10);
+	v4l2_ctrl_handler_init(&dev->radio_ctrl_handler, 5);
+
+	if (dev->sd_cx25840) {
+		v4l2_ctrl_add_handler(&dev->ctrl_handler,
+				dev->sd_cx25840->ctrl_handler, NULL);
+		v4l2_ctrl_add_handler(&dev->radio_ctrl_handler,
+				dev->sd_cx25840->ctrl_handler,
+				v4l2_ctrl_radio_filter);
+	}
+
+	if (dev->ctrl_handler.error)
+		return dev->ctrl_handler.error;
+	if (dev->radio_ctrl_handler.error)
+		return dev->radio_ctrl_handler.error;
 
 	/* enable vbi capturing */
 	/* write code here...  */
@@ -2615,6 +2333,7 @@ int cx231xx_register_analog_devices(struct cx231xx *dev)
 		return -ENODEV;
 	}
 
+	dev->vdev->ctrl_handler = &dev->ctrl_handler;
 	/* register v4l2 video video_device */
 	ret = video_register_device(dev->vdev, VFL_TYPE_GRABBER,
 				    video_nr[dev->devno]);
@@ -2634,6 +2353,11 @@ int cx231xx_register_analog_devices(struct cx231xx *dev)
 	/* Allocate and fill vbi video_device struct */
 	dev->vbi_dev = cx231xx_vdev_init(dev, &cx231xx_vbi_template, "vbi");
 
+	if (!dev->vbi_dev) {
+		cx231xx_errdev("cannot allocate video_device.\n");
+		return -ENODEV;
+	}
+	dev->vbi_dev->ctrl_handler = &dev->ctrl_handler;
 	/* register v4l2 vbi video_device */
 	ret = video_register_device(dev->vbi_dev, VFL_TYPE_VBI,
 				    vbi_nr[dev->devno]);
@@ -2652,6 +2376,7 @@ int cx231xx_register_analog_devices(struct cx231xx *dev)
 			cx231xx_errdev("cannot allocate video_device.\n");
 			return -ENODEV;
 		}
+		dev->radio_dev->ctrl_handler = &dev->radio_ctrl_handler;
 		ret = video_register_device(dev->radio_dev, VFL_TYPE_RADIO,
 					    radio_nr[dev->devno]);
 		if (ret < 0) {
diff --git a/drivers/media/usb/cx231xx/cx231xx.h b/drivers/media/usb/cx231xx/cx231xx.h
index 3e11462be0d..5ad9fd61d3c 100644
--- a/drivers/media/usb/cx231xx/cx231xx.h
+++ b/drivers/media/usb/cx231xx/cx231xx.h
@@ -33,6 +33,8 @@
 
 #include <media/videobuf-vmalloc.h>
 #include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fh.h>
 #include <media/rc-core.h>
 #include <media/ir-kbd-i2c.h>
 #include <media/videobuf-dvb.h>
@@ -69,6 +71,7 @@
 #define CX231XX_BOARD_HAUPPAUGE_USB2_FM_PAL 14
 #define CX231XX_BOARD_HAUPPAUGE_USB2_FM_NTSC 15
 #define CX231XX_BOARD_ELGATO_VIDEO_CAPTURE_V2 16
+#define CX231XX_BOARD_OTG102 17
 
 /* Limits minimum and default number of buffers */
 #define CX231XX_MIN_BUF                 4
@@ -428,27 +431,12 @@ struct cx231xx_audio {
 struct cx231xx;
 
 struct cx231xx_fh {
+	struct v4l2_fh fh;
 	struct cx231xx *dev;
 	unsigned int stream_on:1;	/* Locks streams */
-	int radio;
-
-	struct videobuf_queue vb_vidq;
-
 	enum v4l2_buf_type type;
 
-
-
-/*following is copyed from cx23885.h*/
-	u32                        resources;
-
-	/* video overlay */
-	struct v4l2_window         win;
-	struct v4l2_clip           *clips;
-	unsigned int               nclips;
-
-	/* video capture */
-	struct cx23417_fmt         *fmt;
-	unsigned int               width, height;
+	struct videobuf_queue vb_vidq;
 
 	/* vbi capture */
 	struct videobuf_queue      vidq;
@@ -516,14 +504,6 @@ struct cx231xx_tvnorm {
 	u32		cxoformat;
 };
 
-struct cx231xx_ctrl {
-	struct v4l2_queryctrl v;
-	u32 off;
-	u32 reg;
-	u32 mask;
-	u32 shift;
-};
-
 enum TRANSFER_TYPE {
 	Raw_Video = 0,
 	Audio,
@@ -631,6 +611,9 @@ struct cx231xx {
 	struct v4l2_device v4l2_dev;
 	struct v4l2_subdev *sd_cx25840;
 	struct v4l2_subdev *sd_tuner;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl_handler radio_ctrl_handler;
+	struct cx2341x_handler mpeg_ctrl_handler;
 
 	struct work_struct wq_trigger;		/* Trigger to start/stop audio for alsa module */
 	atomic_t	   stream_started;	/* stream should be running if true */
@@ -653,8 +636,6 @@ struct cx231xx {
 	v4l2_std_id norm;	/* selected tv norm */
 	int ctl_freq;		/* selected frequency */
 	unsigned int ctl_ainput;	/* selected audio input */
-	int mute;
-	int volume;
 
 	/* frame properties */
 	int width;		/* current frame width */
@@ -736,7 +717,6 @@ struct cx231xx {
 	u8 USE_ISO;
 	struct cx231xx_tvnorm      encodernorm;
 	struct cx231xx_tsport      ts1, ts2;
-	struct cx2341x_mpeg_params mpeg_params;
 	struct video_device        *v4l_device;
 	atomic_t                   v4l_reader_count;
 	u32                        freq;
@@ -866,8 +846,6 @@ int cx231xx_send_usb_command(struct cx231xx_i2c *i2c_bus,
 /* Gpio related functions */
 int cx231xx_send_gpio_cmd(struct cx231xx *dev, u32 gpio_bit, u8 *gpio_val,
 			  u8 len, u8 request, u8 direction);
-int cx231xx_set_gpio_bit(struct cx231xx *dev, u32 gpio_bit, u8 *gpio_val);
-int cx231xx_get_gpio_bit(struct cx231xx *dev, u32 gpio_bit, u8 *gpio_val);
 int cx231xx_set_gpio_value(struct cx231xx *dev, int pin_number, int pin_value);
 int cx231xx_set_gpio_direction(struct cx231xx *dev, int pin_number,
 			       int pin_value);
@@ -955,6 +933,23 @@ int cx231xx_register_extension(struct cx231xx_ops *dev);
 void cx231xx_unregister_extension(struct cx231xx_ops *dev);
 void cx231xx_init_extension(struct cx231xx *dev);
 void cx231xx_close_extension(struct cx231xx *dev);
+int cx231xx_querycap(struct file *file, void *priv,
+			   struct v4l2_capability *cap);
+int cx231xx_g_tuner(struct file *file, void *priv, struct v4l2_tuner *t);
+int cx231xx_s_tuner(struct file *file, void *priv, const struct v4l2_tuner *t);
+int cx231xx_g_frequency(struct file *file, void *priv,
+			      struct v4l2_frequency *f);
+int cx231xx_s_frequency(struct file *file, void *priv,
+			      const struct v4l2_frequency *f);
+int cx231xx_enum_input(struct file *file, void *priv,
+			     struct v4l2_input *i);
+int cx231xx_g_input(struct file *file, void *priv, unsigned int *i);
+int cx231xx_s_input(struct file *file, void *priv, unsigned int i);
+int cx231xx_g_chip_ident(struct file *file, void *fh, struct v4l2_dbg_chip_ident *chip);
+int cx231xx_g_register(struct file *file, void *priv,
+			     struct v4l2_dbg_register *reg);
+int cx231xx_s_register(struct file *file, void *priv,
+			     const struct v4l2_dbg_register *reg);
 
 /* Provided by cx231xx-cards.c */
 extern void cx231xx_pre_card_setup(struct cx231xx *dev);
diff --git a/drivers/media/usb/dvb-usb-v2/Kconfig b/drivers/media/usb/dvb-usb-v2/Kconfig
index 692224d97d0..a3c8ecf2207 100644
--- a/drivers/media/usb/dvb-usb-v2/Kconfig
+++ b/drivers/media/usb/dvb-usb-v2/Kconfig
@@ -13,10 +13,6 @@ config DVB_USB_V2
 
 	  Say Y if you own a USB DVB device.
 
-config DVB_USB_CYPRESS_FIRMWARE
-	tristate "Cypress firmware helper routines"
-	depends on DVB_USB_V2
-
 config DVB_USB_AF9015
 	tristate "Afatech AF9015 DVB-T USB2.0 support"
 	depends on DVB_USB_V2
@@ -41,6 +37,7 @@ config DVB_USB_AF9035
 	select MEDIA_TUNER_MXL5007T if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_TDA18218 if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_FC2580 if MEDIA_SUBDRV_AUTOSELECT
+	select MEDIA_TUNER_IT913X if MEDIA_SUBDRV_AUTOSELECT
 	help
 	  Say Y here to support the Afatech AF9035 based DVB USB receiver.
 
@@ -72,7 +69,7 @@ config DVB_USB_AU6610
 config DVB_USB_AZ6007
 	tristate "AzureWave 6007 and clones DVB-T/C USB2.0 support"
 	depends on DVB_USB_V2
-	select DVB_USB_CYPRESS_FIRMWARE
+	select CYPRESS_FIRMWARE
 	select DVB_DRXK if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_MT2063 if MEDIA_SUBDRV_AUTOSELECT
 	help
@@ -146,6 +143,7 @@ config DVB_USB_RTL28XXU
 	select MEDIA_TUNER_FC0013 if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_E4000 if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_FC2580 if MEDIA_SUBDRV_AUTOSELECT
+	select MEDIA_TUNER_R820T if MEDIA_SUBDRV_AUTOSELECT
 	help
 	  Say Y here to support the Realtek RTL28xxU DVB USB receiver.
 
diff --git a/drivers/media/usb/dvb-usb-v2/Makefile b/drivers/media/usb/dvb-usb-v2/Makefile
index b76f58e6c64..2c06714b9ef 100644
--- a/drivers/media/usb/dvb-usb-v2/Makefile
+++ b/drivers/media/usb/dvb-usb-v2/Makefile
@@ -1,9 +1,6 @@
 dvb_usb_v2-objs := dvb_usb_core.o dvb_usb_urb.o usb_urb.o
 obj-$(CONFIG_DVB_USB_V2) += dvb_usb_v2.o
 
-dvb_usb_cypress_firmware-objs := cypress_firmware.o
-obj-$(CONFIG_DVB_USB_CYPRESS_FIRMWARE) += dvb_usb_cypress_firmware.o
-
 dvb-usb-af9015-objs := af9015.o
 obj-$(CONFIG_DVB_USB_AF9015) += dvb-usb-af9015.o
 
@@ -46,4 +43,4 @@ obj-$(CONFIG_DVB_USB_RTL28XXU) += dvb-usb-rtl28xxu.o
 ccflags-y += -I$(srctree)/drivers/media/dvb-core
 ccflags-y += -I$(srctree)/drivers/media/dvb-frontends
 ccflags-y += -I$(srctree)/drivers/media/tuners
-
+ccflags-y += -I$(srctree)/drivers/media/common
diff --git a/drivers/media/usb/dvb-usb-v2/af9015.c b/drivers/media/usb/dvb-usb-v2/af9015.c
index b86d0f27a39..d556042cf31 100644
--- a/drivers/media/usb/dvb-usb-v2/af9015.c
+++ b/drivers/media/usb/dvb-usb-v2/af9015.c
@@ -30,22 +30,22 @@ DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
 
 static int af9015_ctrl_msg(struct dvb_usb_device *d, struct req_t *req)
 {
-#define BUF_LEN 63
 #define REQ_HDR_LEN 8 /* send header size */
 #define ACK_HDR_LEN 2 /* rece header size */
 	struct af9015_state *state = d_to_priv(d);
 	int ret, wlen, rlen;
-	u8 buf[BUF_LEN];
 	u8 write = 1;
 
-	buf[0] = req->cmd;
-	buf[1] = state->seq++;
-	buf[2] = req->i2c_addr;
-	buf[3] = req->addr >> 8;
-	buf[4] = req->addr & 0xff;
-	buf[5] = req->mbox;
-	buf[6] = req->addr_len;
-	buf[7] = req->data_len;
+	mutex_lock(&d->usb_mutex);
+
+	state->buf[0] = req->cmd;
+	state->buf[1] = state->seq++;
+	state->buf[2] = req->i2c_addr;
+	state->buf[3] = req->addr >> 8;
+	state->buf[4] = req->addr & 0xff;
+	state->buf[5] = req->mbox;
+	state->buf[6] = req->addr_len;
+	state->buf[7] = req->data_len;
 
 	switch (req->cmd) {
 	case GET_CONFIG:
@@ -55,14 +55,14 @@ static int af9015_ctrl_msg(struct dvb_usb_device *d, struct req_t *req)
 		break;
 	case READ_I2C:
 		write = 0;
-		buf[2] |= 0x01; /* set I2C direction */
+		state->buf[2] |= 0x01; /* set I2C direction */
 	case WRITE_I2C:
-		buf[0] = READ_WRITE_I2C;
+		state->buf[0] = READ_WRITE_I2C;
 		break;
 	case WRITE_MEMORY:
 		if (((req->addr & 0xff00) == 0xff00) ||
 		    ((req->addr & 0xff00) == 0xae00))
-			buf[0] = WRITE_VIRTUAL_MEMORY;
+			state->buf[0] = WRITE_VIRTUAL_MEMORY;
 	case WRITE_VIRTUAL_MEMORY:
 	case COPY_FIRMWARE:
 	case DOWNLOAD_FIRMWARE:
@@ -90,7 +90,7 @@ static int af9015_ctrl_msg(struct dvb_usb_device *d, struct req_t *req)
 	rlen = ACK_HDR_LEN;
 	if (write) {
 		wlen += req->data_len;
-		memcpy(&buf[REQ_HDR_LEN], req->data, req->data_len);
+		memcpy(&state->buf[REQ_HDR_LEN], req->data, req->data_len);
 	} else {
 		rlen += req->data_len;
 	}
@@ -99,22 +99,25 @@ static int af9015_ctrl_msg(struct dvb_usb_device *d, struct req_t *req)
 	if (req->cmd == DOWNLOAD_FIRMWARE || req->cmd == RECONNECT_USB)
 		rlen = 0;
 
-	ret = dvb_usbv2_generic_rw(d, buf, wlen, buf, rlen);
+	ret = dvb_usbv2_generic_rw_locked(d,
+			state->buf, wlen, state->buf, rlen);
 	if (ret)
 		goto error;
 
 	/* check status */
-	if (rlen && buf[1]) {
+	if (rlen && state->buf[1]) {
 		dev_err(&d->udev->dev, "%s: command failed=%d\n",
-				KBUILD_MODNAME, buf[1]);
+				KBUILD_MODNAME, state->buf[1]);
 		ret = -EIO;
 		goto error;
 	}
 
 	/* read request, copy returned data to return buf */
 	if (!write)
-		memcpy(req->data, &buf[ACK_HDR_LEN], req->data_len);
+		memcpy(req->data, &state->buf[ACK_HDR_LEN], req->data_len);
 error:
+	mutex_unlock(&d->usb_mutex);
+
 	return ret;
 }
 
@@ -1317,6 +1320,43 @@ static int af9015_get_rc_config(struct dvb_usb_device *d, struct dvb_usb_rc *rc)
 	#define af9015_get_rc_config NULL
 #endif
 
+static int af9015_probe(struct usb_interface *intf,
+		const struct usb_device_id *id)
+{
+	struct usb_device *udev = interface_to_usbdev(intf);
+	char manufacturer[sizeof("ITE Technologies, Inc.")];
+
+	memset(manufacturer, 0, sizeof(manufacturer));
+	usb_string(udev, udev->descriptor.iManufacturer,
+			manufacturer, sizeof(manufacturer));
+	/*
+	 * There is two devices having same ID but different chipset. One uses
+	 * AF9015 and the other IT9135 chipset. Only difference seen on lsusb
+	 * is iManufacturer string.
+	 *
+	 * idVendor           0x0ccd TerraTec Electronic GmbH
+	 * idProduct          0x0099
+	 * bcdDevice            2.00
+	 * iManufacturer           1 Afatech
+	 * iProduct                2 DVB-T 2
+	 *
+	 * idVendor           0x0ccd TerraTec Electronic GmbH
+	 * idProduct          0x0099
+	 * bcdDevice            2.00
+	 * iManufacturer           1 ITE Technologies, Inc.
+	 * iProduct                2 DVB-T TV Stick
+	 */
+	if ((le16_to_cpu(udev->descriptor.idVendor) == USB_VID_TERRATEC) &&
+			(le16_to_cpu(udev->descriptor.idProduct) == 0x0099)) {
+		if (!strcmp("ITE Technologies, Inc.", manufacturer)) {
+			dev_dbg(&udev->dev, "%s: rejecting device\n", __func__);
+			return -ENODEV;
+		}
+	}
+
+	return dvb_usbv2_probe(intf, id);
+}
+
 /* interface 0 is used by DVB-T receiver and
    interface 1 is for remote controller (HID) */
 static struct dvb_usb_device_properties af9015_props = {
@@ -1425,6 +1465,7 @@ static const struct usb_device_id af9015_id_table[] = {
 		&af9015_props, "AverMedia AVerTV Volar M (A815Mac)", NULL) },
 	{ DVB_USB_DEVICE(USB_VID_TERRATEC, USB_PID_TERRATEC_CINERGY_T_STICK_RC,
 		&af9015_props, "TerraTec Cinergy T Stick RC", RC_MAP_TERRATEC_SLIM_2) },
+	/* XXX: that same ID [0ccd:0099] is used by af9035 driver too */
 	{ DVB_USB_DEVICE(USB_VID_TERRATEC, USB_PID_TERRATEC_CINERGY_T_STICK_DUAL_RC,
 		&af9015_props, "TerraTec Cinergy T Stick Dual RC", RC_MAP_TERRATEC_SLIM) },
 	{ DVB_USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_A850T,
@@ -1441,7 +1482,7 @@ MODULE_DEVICE_TABLE(usb, af9015_id_table);
 static struct usb_driver af9015_usb_driver = {
 	.name = KBUILD_MODNAME,
 	.id_table = af9015_id_table,
-	.probe = dvb_usbv2_probe,
+	.probe = af9015_probe,
 	.disconnect = dvb_usbv2_disconnect,
 	.suspend = dvb_usbv2_suspend,
 	.resume = dvb_usbv2_resume,
diff --git a/drivers/media/usb/dvb-usb-v2/af9015.h b/drivers/media/usb/dvb-usb-v2/af9015.h
index 533637dedd2..3a6f3ad1ead 100644
--- a/drivers/media/usb/dvb-usb-v2/af9015.h
+++ b/drivers/media/usb/dvb-usb-v2/af9015.h
@@ -115,7 +115,9 @@ enum af9015_ir_mode {
 	AF9015_IR_MODE_POLLING, /* just guess */
 };
 
+#define BUF_LEN 63
 struct af9015_state {
+	u8 buf[BUF_LEN]; /* bulk USB control message */
 	u8 ir_mode;
 	u8 rc_repeat;
 	u32 rc_keycode;
diff --git a/drivers/media/usb/dvb-usb-v2/af9035.c b/drivers/media/usb/dvb-usb-v2/af9035.c
index f11cc42454f..b638fc1cd57 100644
--- a/drivers/media/usb/dvb-usb-v2/af9035.c
+++ b/drivers/media/usb/dvb-usb-v2/af9035.c
@@ -41,79 +41,84 @@ static u16 af9035_checksum(const u8 *buf, size_t len)
 
 static int af9035_ctrl_msg(struct dvb_usb_device *d, 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
 	struct state *state = d_to_priv(d);
 	int ret, wlen, rlen;
-	u8 buf[BUF_LEN];
 	u16 checksum, tmp_checksum;
 
+	mutex_lock(&d->usb_mutex);
+
 	/* buffer overflow check */
 	if (req->wlen > (BUF_LEN - REQ_HDR_LEN - CHECKSUM_LEN) ||
 			req->rlen > (BUF_LEN - ACK_HDR_LEN - CHECKSUM_LEN)) {
 		dev_err(&d->udev->dev, "%s: too much data wlen=%d rlen=%d\n",
 				__func__, req->wlen, req->rlen);
-		return -EINVAL;
+		ret = -EINVAL;
+		goto exit;
 	}
 
-	buf[0] = REQ_HDR_LEN + req->wlen + CHECKSUM_LEN - 1;
-	buf[1] = req->mbox;
-	buf[2] = req->cmd;
-	buf[3] = state->seq++;
-	memcpy(&buf[REQ_HDR_LEN], req->wbuf, req->wlen);
+	state->buf[0] = REQ_HDR_LEN + req->wlen + CHECKSUM_LEN - 1;
+	state->buf[1] = req->mbox;
+	state->buf[2] = req->cmd;
+	state->buf[3] = state->seq++;
+	memcpy(&state->buf[REQ_HDR_LEN], req->wbuf, req->wlen);
 
 	wlen = REQ_HDR_LEN + req->wlen + CHECKSUM_LEN;
 	rlen = ACK_HDR_LEN + req->rlen + CHECKSUM_LEN;
 
 	/* calc and add checksum */
-	checksum = af9035_checksum(buf, buf[0] - 1);
-	buf[buf[0] - 1] = (checksum >> 8);
-	buf[buf[0] - 0] = (checksum & 0xff);
+	checksum = af9035_checksum(state->buf, state->buf[0] - 1);
+	state->buf[state->buf[0] - 1] = (checksum >> 8);
+	state->buf[state->buf[0] - 0] = (checksum & 0xff);
 
 	/* no ack for these packets */
 	if (req->cmd == CMD_FW_DL)
 		rlen = 0;
 
-	ret = dvb_usbv2_generic_rw(d, buf, wlen, buf, rlen);
+	ret = dvb_usbv2_generic_rw_locked(d,
+			state->buf, wlen, state->buf, rlen);
 	if (ret)
-		goto err;
+		goto exit;
 
 	/* no ack for those packets */
 	if (req->cmd == CMD_FW_DL)
 		goto exit;
 
 	/* verify checksum */
-	checksum = af9035_checksum(buf, rlen - 2);
-	tmp_checksum = (buf[rlen - 2] << 8) | buf[rlen - 1];
+	checksum = af9035_checksum(state->buf, rlen - 2);
+	tmp_checksum = (state->buf[rlen - 2] << 8) | state->buf[rlen - 1];
 	if (tmp_checksum != checksum) {
 		dev_err(&d->udev->dev, "%s: command=%02x checksum mismatch " \
 				"(%04x != %04x)\n", KBUILD_MODNAME, req->cmd,
 				tmp_checksum, checksum);
 		ret = -EIO;
-		goto err;
+		goto exit;
 	}
 
 	/* check status */
-	if (buf[2]) {
+	if (state->buf[2]) {
+		/* fw returns status 1 when IR code was not received */
+		if (req->cmd == CMD_IR_GET || state->buf[2] == 1) {
+			ret = 1;
+			goto exit;
+		}
+
 		dev_dbg(&d->udev->dev, "%s: command=%02x failed fw error=%d\n",
-				__func__, req->cmd, buf[2]);
+				__func__, req->cmd, state->buf[2]);
 		ret = -EIO;
-		goto err;
+		goto exit;
 	}
 
 	/* read request, copy returned data to return buf */
 	if (req->rlen)
-		memcpy(req->rbuf, &buf[ACK_HDR_LEN], req->rlen);
-
+		memcpy(req->rbuf, &state->buf[ACK_HDR_LEN], req->rlen);
 exit:
-	return 0;
-
-err:
-	dev_dbg(&d->udev->dev, "%s: failed=%d\n", __func__, ret);
-
+	mutex_unlock(&d->usb_mutex);
+	if (ret < 0)
+		dev_dbg(&d->udev->dev, "%s: failed=%d\n", __func__, ret);
 	return ret;
 }
 
@@ -292,12 +297,40 @@ static struct i2c_algorithm af9035_i2c_algo = {
 
 static int af9035_identify_state(struct dvb_usb_device *d, const char **name)
 {
+	struct state *state = d_to_priv(d);
 	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_rd_regs(d, 0x1222, rbuf, 3);
+	if (ret < 0)
+		goto err;
+
+	state->chip_version = rbuf[0];
+	state->chip_type = rbuf[2] << 8 | rbuf[1] << 0;
+
+	ret = af9035_rd_reg(d, 0x384f, &state->prechip_version);
+	if (ret < 0)
+		goto err;
+
+	dev_info(&d->udev->dev,
+			"%s: prechip_version=%02x chip_version=%02x chip_type=%04x\n",
+			__func__, state->prechip_version, state->chip_version,
+			state->chip_type);
+
+	if (state->chip_type == 0x9135) {
+		if (state->chip_version == 0x02)
+			*name = AF9035_FIRMWARE_IT9135_V2;
+		else
+			*name = AF9035_FIRMWARE_IT9135_V1;
+		state->eeprom_addr = EEPROM_BASE_IT9135;
+	} else {
+		*name = AF9035_FIRMWARE_AF9035;
+		state->eeprom_addr = EEPROM_BASE_AF9035;
+	}
+
 	ret = af9035_ctrl_msg(d, &req);
 	if (ret < 0)
 		goto err;
@@ -316,66 +349,19 @@ err:
 	return ret;
 }
 
-static int af9035_download_firmware(struct dvb_usb_device *d,
+static int af9035_download_firmware_old(struct dvb_usb_device *d,
 		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, tmp;
+	u8 hdr_core;
 	u16 hdr_addr, hdr_data_len, hdr_checksum;
 	#define MAX_DATA 58
 	#define HDR_SIZE 7
 
 	/*
-	 * In case of dual tuner configuration we need to do some extra
-	 * initialization in order to download firmware to slave demod too,
-	 * which is done by master demod.
-	 * Master feeds also clock and controls power via GPIO.
-	 */
-	ret = af9035_rd_reg(d, EEPROM_DUAL_MODE, &tmp);
-	if (ret < 0)
-		goto err;
-
-	if (tmp) {
-		/* configure gpioh1, reset & power slave demod */
-		ret = af9035_wr_reg_mask(d, 0x00d8b0, 0x01, 0x01);
-		if (ret < 0)
-			goto err;
-
-		ret = af9035_wr_reg_mask(d, 0x00d8b1, 0x01, 0x01);
-		if (ret < 0)
-			goto err;
-
-		ret = af9035_wr_reg_mask(d, 0x00d8af, 0x00, 0x01);
-		if (ret < 0)
-			goto err;
-
-		usleep_range(10000, 50000);
-
-		ret = af9035_wr_reg_mask(d, 0x00d8af, 0x01, 0x01);
-		if (ret < 0)
-			goto err;
-
-		/* tell the slave I2C address */
-		ret = af9035_rd_reg(d, EEPROM_2ND_DEMOD_ADDR, &tmp);
-		if (ret < 0)
-			goto err;
-
-		ret = af9035_wr_reg(d, 0x00417f, tmp);
-		if (ret < 0)
-			goto err;
-
-		/* enable clock out */
-		ret = af9035_wr_reg_mask(d, 0x00d81a, 0x01, 0x01);
-		if (ret < 0)
-			goto err;
-	}
-
-	/*
 	 * Thanks to Daniel Glöckner <daniel-gl@gmx.net> about that info!
 	 *
 	 * byte 0: MCS 51 core
@@ -441,28 +427,6 @@ static int af9035_download_firmware(struct dvb_usb_device *d,
 	if (i)
 		dev_warn(&d->udev->dev, "%s: bad firmware\n", KBUILD_MODNAME);
 
-	/* firmware loaded, request boot */
-	req.cmd = CMD_FW_BOOT;
-	ret = af9035_ctrl_msg(d, &req);
-	if (ret < 0)
-		goto err;
-
-	/* ensure firmware starts */
-	wbuf[0] = 1;
-	ret = af9035_ctrl_msg(d, &req_fw_ver);
-	if (ret < 0)
-		goto err;
-
-	if (!(rbuf[0] || rbuf[1] || rbuf[2] || rbuf[3])) {
-		dev_err(&d->udev->dev, "%s: firmware did not run\n",
-				KBUILD_MODNAME);
-		ret = -ENODEV;
-		goto err;
-	}
-
-	dev_info(&d->udev->dev, "%s: firmware version=%d.%d.%d.%d",
-			KBUILD_MODNAME, rbuf[0], rbuf[1], rbuf[2], rbuf[3]);
-
 	return 0;
 
 err:
@@ -471,15 +435,11 @@ err:
 	return ret;
 }
 
-static int af9035_download_firmware_it9135(struct dvb_usb_device *d,
+static int af9035_download_firmware_new(struct dvb_usb_device *d,
 		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
 
 	/*
@@ -494,7 +454,6 @@ static int af9035_download_firmware_it9135(struct dvb_usb_device *d,
 	 * 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 &&
@@ -513,6 +472,86 @@ static int af9035_download_firmware_it9135(struct dvb_usb_device *d,
 		}
 	}
 
+	return 0;
+
+err:
+	dev_dbg(&d->udev->dev, "%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9035_download_firmware(struct dvb_usb_device *d,
+		const struct firmware *fw)
+{
+	struct state *state = d_to_priv(d);
+	int ret;
+	u8 wbuf[1];
+	u8 rbuf[4];
+	u8 tmp;
+	struct usb_req req = { 0, 0, 0, NULL, 0, NULL };
+	struct usb_req req_fw_ver = { CMD_FW_QUERYINFO, 0, 1, wbuf, 4, rbuf } ;
+	dev_dbg(&d->udev->dev, "%s:\n", __func__);
+
+	/*
+	 * In case of dual tuner configuration we need to do some extra
+	 * initialization in order to download firmware to slave demod too,
+	 * which is done by master demod.
+	 * Master feeds also clock and controls power via GPIO.
+	 */
+	ret = af9035_rd_reg(d, state->eeprom_addr + EEPROM_DUAL_MODE, &tmp);
+	if (ret < 0)
+		goto err;
+
+	if (tmp) {
+		/* configure gpioh1, reset & power slave demod */
+		ret = af9035_wr_reg_mask(d, 0x00d8b0, 0x01, 0x01);
+		if (ret < 0)
+			goto err;
+
+		ret = af9035_wr_reg_mask(d, 0x00d8b1, 0x01, 0x01);
+		if (ret < 0)
+			goto err;
+
+		ret = af9035_wr_reg_mask(d, 0x00d8af, 0x00, 0x01);
+		if (ret < 0)
+			goto err;
+
+		usleep_range(10000, 50000);
+
+		ret = af9035_wr_reg_mask(d, 0x00d8af, 0x01, 0x01);
+		if (ret < 0)
+			goto err;
+
+		/* tell the slave I2C address */
+		ret = af9035_rd_reg(d,
+				state->eeprom_addr + EEPROM_2ND_DEMOD_ADDR,
+				&tmp);
+		if (ret < 0)
+			goto err;
+
+		if (state->chip_type == 0x9135) {
+			ret = af9035_wr_reg(d, 0x004bfb, tmp);
+			if (ret < 0)
+				goto err;
+		} else {
+			ret = af9035_wr_reg(d, 0x00417f, tmp);
+			if (ret < 0)
+				goto err;
+
+			/* enable clock out */
+			ret = af9035_wr_reg_mask(d, 0x00d81a, 0x01, 0x01);
+			if (ret < 0)
+				goto err;
+		}
+	}
+
+	if (fw->data[0] == 0x01)
+		ret = af9035_download_firmware_old(d, fw);
+	else
+		ret = af9035_download_firmware_new(d, fw);
+	if (ret < 0)
+		goto err;
+
 	/* firmware loaded, request boot */
 	req.cmd = CMD_FW_BOOT;
 	ret = af9035_ctrl_msg(d, &req);
@@ -546,15 +585,42 @@ err:
 static int af9035_read_config(struct dvb_usb_device *d)
 {
 	struct state *state = d_to_priv(d);
-	int ret, i, eeprom_shift = 0;
+	int ret, i;
 	u8 tmp;
-	u16 tmp16;
+	u16 tmp16, addr;
 
 	/* demod I2C "address" */
 	state->af9033_config[0].i2c_addr = 0x38;
+	state->af9033_config[0].adc_multiplier = AF9033_ADC_MULTIPLIER_2X;
+	state->af9033_config[1].adc_multiplier = AF9033_ADC_MULTIPLIER_2X;
+	state->af9033_config[0].ts_mode = AF9033_TS_MODE_USB;
+	state->af9033_config[1].ts_mode = AF9033_TS_MODE_SERIAL;
+
+	/* eeprom memory mapped location */
+	if (state->chip_type == 0x9135) {
+		if (state->chip_version == 0x02) {
+			state->af9033_config[0].tuner = AF9033_TUNER_IT9135_60;
+			state->af9033_config[1].tuner = AF9033_TUNER_IT9135_60;
+			tmp16 = 0x00461d;
+		} else {
+			state->af9033_config[0].tuner = AF9033_TUNER_IT9135_38;
+			state->af9033_config[1].tuner = AF9033_TUNER_IT9135_38;
+			tmp16 = 0x00461b;
+		}
+
+		/* check if eeprom exists */
+		ret = af9035_rd_reg(d, tmp16, &tmp);
+		if (ret < 0)
+			goto err;
+
+		if (tmp == 0x00) {
+			dev_dbg(&d->udev->dev, "%s: no eeprom\n", __func__);
+			goto skip_eeprom;
+		}
+	}
 
 	/* check if there is dual tuners */
-	ret = af9035_rd_reg(d, EEPROM_DUAL_MODE, &tmp);
+	ret = af9035_rd_reg(d, state->eeprom_addr + EEPROM_DUAL_MODE, &tmp);
 	if (ret < 0)
 		goto err;
 
@@ -564,7 +630,9 @@ static int af9035_read_config(struct dvb_usb_device *d)
 
 	if (state->dual_mode) {
 		/* read 2nd demodulator I2C address */
-		ret = af9035_rd_reg(d, EEPROM_2ND_DEMOD_ADDR, &tmp);
+		ret = af9035_rd_reg(d,
+				state->eeprom_addr + EEPROM_2ND_DEMOD_ADDR,
+				&tmp);
 		if (ret < 0)
 			goto err;
 
@@ -573,17 +641,25 @@ static int af9035_read_config(struct dvb_usb_device *d)
 				__func__, tmp);
 	}
 
+	addr = state->eeprom_addr;
+
 	for (i = 0; i < state->dual_mode + 1; i++) {
 		/* tuner */
-		ret = af9035_rd_reg(d, EEPROM_1_TUNER_ID + eeprom_shift, &tmp);
+		ret = af9035_rd_reg(d, addr + EEPROM_1_TUNER_ID, &tmp);
 		if (ret < 0)
 			goto err;
 
-		state->af9033_config[i].tuner = tmp;
+		if (tmp == 0x00)
+			dev_dbg(&d->udev->dev,
+					"%s: [%d]tuner not set, using default\n",
+					__func__, i);
+		else
+			state->af9033_config[i].tuner = tmp;
+
 		dev_dbg(&d->udev->dev, "%s: [%d]tuner=%02x\n",
-				__func__, i, tmp);
+				__func__, i, state->af9033_config[i].tuner);
 
-		switch (tmp) {
+		switch (state->af9033_config[i].tuner) {
 		case AF9033_TUNER_TUA9001:
 		case AF9033_TUNER_FC0011:
 		case AF9033_TUNER_MXL5007T:
@@ -592,32 +668,46 @@ static int af9035_read_config(struct dvb_usb_device *d)
 		case AF9033_TUNER_FC0012:
 			state->af9033_config[i].spec_inv = 1;
 			break;
+		case AF9033_TUNER_IT9135_38:
+		case AF9033_TUNER_IT9135_51:
+		case AF9033_TUNER_IT9135_52:
+		case AF9033_TUNER_IT9135_60:
+		case AF9033_TUNER_IT9135_61:
+		case AF9033_TUNER_IT9135_62:
+			break;
 		default:
-			dev_warn(&d->udev->dev, "%s: tuner id=%02x not " \
-					"supported, please report!",
+			dev_warn(&d->udev->dev,
+					"%s: tuner id=%02x not supported, please report!",
 					KBUILD_MODNAME, tmp);
 		}
 
 		/* disable dual mode if driver does not support it */
 		if (i == 1)
-			switch (tmp) {
+			switch (state->af9033_config[i].tuner) {
 			case AF9033_TUNER_FC0012:
+			case AF9033_TUNER_IT9135_38:
+			case AF9033_TUNER_IT9135_51:
+			case AF9033_TUNER_IT9135_52:
+			case AF9033_TUNER_IT9135_60:
+			case AF9033_TUNER_IT9135_61:
+			case AF9033_TUNER_IT9135_62:
+			case AF9033_TUNER_MXL5007T:
 				break;
 			default:
 				state->dual_mode = false;
-				dev_info(&d->udev->dev, "%s: driver does not " \
-						"support 2nd tuner and will " \
-						"disable it", KBUILD_MODNAME);
+				dev_info(&d->udev->dev,
+						"%s: driver does not support 2nd tuner and will disable it",
+						KBUILD_MODNAME);
 		}
 
 		/* tuner IF frequency */
-		ret = af9035_rd_reg(d, EEPROM_1_IFFREQ_L + eeprom_shift, &tmp);
+		ret = af9035_rd_reg(d, addr + EEPROM_1_IF_L, &tmp);
 		if (ret < 0)
 			goto err;
 
 		tmp16 = tmp;
 
-		ret = af9035_rd_reg(d, EEPROM_1_IFFREQ_H + eeprom_shift, &tmp);
+		ret = af9035_rd_reg(d, addr + EEPROM_1_IF_H, &tmp);
 		if (ret < 0)
 			goto err;
 
@@ -625,9 +715,10 @@ static int af9035_read_config(struct dvb_usb_device *d)
 
 		dev_dbg(&d->udev->dev, "%s: [%d]IF=%d\n", __func__, i, tmp16);
 
-		eeprom_shift = 0x10; /* shift for the 2nd tuner params */
+		addr += 0x10; /* shift for the 2nd tuner params */
 	}
 
+skip_eeprom:
 	/* get demod clock */
 	ret = af9035_rd_reg(d, 0x00d800, &tmp);
 	if (ret < 0)
@@ -635,34 +726,12 @@ static int af9035_read_config(struct dvb_usb_device *d)
 
 	tmp = (tmp >> 0) & 0x0f;
 
-	for (i = 0; i < ARRAY_SIZE(state->af9033_config); i++)
-		state->af9033_config[i].clock = clock_lut[tmp];
-
-	return 0;
-
-err:
-	dev_dbg(&d->udev->dev, "%s: failed=%d\n", __func__, ret);
-
-	return ret;
-}
-
-static int af9035_read_config_it9135(struct dvb_usb_device *d)
-{
-	struct state *state = d_to_priv(d);
-	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 < ARRAY_SIZE(state->af9033_config); i++)
-		state->af9033_config[i].clock = clock_lut_it9135[tmp];
+	for (i = 0; i < ARRAY_SIZE(state->af9033_config); i++) {
+		if (state->chip_type == 0x9135)
+			state->af9033_config[i].clock = clock_lut_it9135[tmp];
+		else
+			state->af9033_config[i].clock = clock_lut_af9035[tmp];
+	}
 
 	return 0;
 
@@ -821,7 +890,12 @@ static int af9035_frontend_callback(void *adapter_priv, int component,
 static int af9035_get_adapter_count(struct dvb_usb_device *d)
 {
 	struct state *state = d_to_priv(d);
-	return state->dual_mode + 1;
+
+	/* disable 2nd adapter as we don't have PID filters implemented */
+	if (d->udev->speed == USB_SPEED_FULL)
+		return 1;
+	else
+		return state->dual_mode + 1;
 }
 
 static int af9035_frontend_attach(struct dvb_usb_adapter *adap)
@@ -829,6 +903,7 @@ static int af9035_frontend_attach(struct dvb_usb_adapter *adap)
 	struct state *state = adap_to_priv(adap);
 	struct dvb_usb_device *d = adap_to_d(adap);
 	int ret;
+	dev_dbg(&d->udev->dev, "%s:\n", __func__);
 
 	if (!state->af9033_config[adap->id].tuner) {
 		/* unsupported tuner */
@@ -836,20 +911,6 @@ static int af9035_frontend_attach(struct dvb_usb_adapter *adap)
 		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(d, 0x00417f,
-				state->af9033_config[1].i2c_addr);
-		if (ret < 0)
-			goto err;
-
-		ret = af9035_wr_reg(d, 0x00d81a, state->dual_mode);
-		if (ret < 0)
-			goto err;
-	}
-
 	/* attach demodulator */
 	adap->fe[0] = dvb_attach(af9033_attach, &state->af9033_config[adap->id],
 			&d->i2c_adap);
@@ -928,6 +989,8 @@ static int af9035_tuner_attach(struct dvb_usb_adapter *adap)
 	struct dvb_frontend *fe;
 	struct i2c_msg msg[1];
 	u8 tuner_addr;
+	dev_dbg(&d->udev->dev, "%s:\n", __func__);
+
 	/*
 	 * XXX: Hack used in that function: we abuse unused I2C address bit [7]
 	 * to carry info about used I2C bus for dual tuner configuration.
@@ -1082,6 +1145,17 @@ static int af9035_tuner_attach(struct dvb_usb_adapter *adap)
 		fe = dvb_attach(fc0012_attach, adap->fe[0], &d->i2c_adap,
 				&af9035_fc0012_config[adap->id]);
 		break;
+	case AF9033_TUNER_IT9135_38:
+	case AF9033_TUNER_IT9135_51:
+	case AF9033_TUNER_IT9135_52:
+	case AF9033_TUNER_IT9135_60:
+	case AF9033_TUNER_IT9135_61:
+	case AF9033_TUNER_IT9135_62:
+		/* attach tuner */
+		fe = dvb_attach(it913x_attach, adap->fe[0], &d->i2c_adap,
+				state->af9033_config[adap->id].i2c_addr,
+				state->af9033_config[0].tuner);
+		break;
 	default:
 		fe = NULL;
 	}
@@ -1103,8 +1177,8 @@ static int af9035_init(struct dvb_usb_device *d)
 {
 	struct state *state = d_to_priv(d);
 	int ret, i;
-	u16 frame_size = 87 * 188 / 4;
-	u8  packet_size = 512 / 4;
+	u16 frame_size = (d->udev->speed == USB_SPEED_FULL ? 5 : 87) * 188 / 4;
+	u8 packet_size = (d->udev->speed == USB_SPEED_FULL ? 64 : 512) / 4;
 	struct reg_val_mask tab[] = {
 		{ 0x80f99d, 0x01, 0x01 },
 		{ 0x80f9a4, 0x01, 0x01 },
@@ -1149,40 +1223,49 @@ err:
 #if IS_ENABLED(CONFIG_RC_CORE)
 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 };
+	u32 key;
+	u8 buf[4];
+	struct usb_req req = { CMD_IR_GET, 0, 0, NULL, 4, buf };
 
 	ret = af9035_ctrl_msg(d, &req);
-	if (ret < 0)
+	if (ret == 1)
+		return 0;
+	else if (ret < 0)
 		goto err;
 
-	if ((b[2] + b[3]) == 0xff) {
-		if ((b[0] + b[1]) == 0xff) {
-			/* NEC */
-			key = b[0] << 8 | b[2];
+	if ((buf[2] + buf[3]) == 0xff) {
+		if ((buf[0] + buf[1]) == 0xff) {
+			/* NEC standard 16bit */
+			key = buf[0] << 8 | buf[2];
 		} else {
-			/* ext. NEC */
-			key = b[0] << 16 | b[1] << 8 | b[2];
+			/* NEC extended 24bit */
+			key = buf[0] << 16 | buf[1] << 8 | buf[2];
 		}
 	} else {
-		key = b[0] << 24 | b[1] << 16 | b[2] << 8 | b[3];
+		/* NEC full code 32bit */
+		key = buf[0] << 24 | buf[1] << 16 | buf[2] << 8 | buf[3];
 	}
 
+	dev_dbg(&d->udev->dev, "%s: %*ph\n", __func__, 4, buf);
+
 	rc_keydown(d->rc_dev, key, 0);
 
-err:
-	/* ignore errors */
 	return 0;
+
+err:
+	dev_dbg(&d->udev->dev, "%s: failed=%d\n", __func__, ret);
+
+	return ret;
 }
 
 static int af9035_get_rc_config(struct dvb_usb_device *d, struct dvb_usb_rc *rc)
 {
+	struct state *state = d_to_priv(d);
 	int ret;
 	u8 tmp;
 
-	ret = af9035_rd_reg(d, EEPROM_IR_MODE, &tmp);
+	ret = af9035_rd_reg(d, state->eeprom_addr + EEPROM_IR_MODE, &tmp);
 	if (ret < 0)
 		goto err;
 
@@ -1190,7 +1273,8 @@ static int af9035_get_rc_config(struct dvb_usb_device *d, struct dvb_usb_rc *rc)
 
 	/* don't activate rc if in HID mode or if not available */
 	if (tmp == 5) {
-		ret = af9035_rd_reg(d, EEPROM_IR_TYPE, &tmp);
+		ret = af9035_rd_reg(d, state->eeprom_addr + EEPROM_IR_TYPE,
+				&tmp);
 		if (ret < 0)
 			goto err;
 
@@ -1225,6 +1309,109 @@ err:
 	#define af9035_get_rc_config NULL
 #endif
 
+static int af9035_get_stream_config(struct dvb_frontend *fe, u8 *ts_type,
+		struct usb_data_stream_properties *stream)
+{
+	struct dvb_usb_device *d = fe_to_d(fe);
+	dev_dbg(&d->udev->dev, "%s: adap=%d\n", __func__, fe_to_adap(fe)->id);
+
+	if (d->udev->speed == USB_SPEED_FULL)
+		stream->u.bulk.buffersize = 5 * 188;
+
+	return 0;
+}
+
+/*
+ * FIXME: PID filter is property of demodulator and should be moved to the
+ * correct driver. Also we support only adapter #0 PID filter and will
+ * disable adapter #1 if USB1.1 is used.
+ */
+static int af9035_pid_filter_ctrl(struct dvb_usb_adapter *adap, int onoff)
+{
+	struct dvb_usb_device *d = adap_to_d(adap);
+	int ret;
+
+	dev_dbg(&d->udev->dev, "%s: onoff=%d\n", __func__, onoff);
+
+	ret = af9035_wr_reg_mask(d, 0x80f993, onoff, 0x01);
+	if (ret < 0)
+		goto err;
+
+	return 0;
+
+err:
+	dev_dbg(&d->udev->dev, "%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9035_pid_filter(struct dvb_usb_adapter *adap, int index, u16 pid,
+		int onoff)
+{
+	struct dvb_usb_device *d = adap_to_d(adap);
+	int ret;
+	u8 wbuf[2] = {(pid >> 0) & 0xff, (pid >> 8) & 0xff};
+
+	dev_dbg(&d->udev->dev, "%s: index=%d pid=%04x onoff=%d\n",
+			__func__, index, pid, onoff);
+
+	ret = af9035_wr_regs(d, 0x80f996, wbuf, 2);
+	if (ret < 0)
+		goto err;
+
+	ret = af9035_wr_reg(d, 0x80f994, onoff);
+	if (ret < 0)
+		goto err;
+
+	ret = af9035_wr_reg(d, 0x80f995, index);
+	if (ret < 0)
+		goto err;
+
+	return 0;
+
+err:
+	dev_dbg(&d->udev->dev, "%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9035_probe(struct usb_interface *intf,
+		const struct usb_device_id *id)
+{
+	struct usb_device *udev = interface_to_usbdev(intf);
+	char manufacturer[sizeof("Afatech")];
+
+	memset(manufacturer, 0, sizeof(manufacturer));
+	usb_string(udev, udev->descriptor.iManufacturer,
+			manufacturer, sizeof(manufacturer));
+	/*
+	 * There is two devices having same ID but different chipset. One uses
+	 * AF9015 and the other IT9135 chipset. Only difference seen on lsusb
+	 * is iManufacturer string.
+	 *
+	 * idVendor           0x0ccd TerraTec Electronic GmbH
+	 * idProduct          0x0099
+	 * bcdDevice            2.00
+	 * iManufacturer           1 Afatech
+	 * iProduct                2 DVB-T 2
+	 *
+	 * idVendor           0x0ccd TerraTec Electronic GmbH
+	 * idProduct          0x0099
+	 * bcdDevice            2.00
+	 * iManufacturer           1 ITE Technologies, Inc.
+	 * iProduct                2 DVB-T TV Stick
+	 */
+	if ((le16_to_cpu(udev->descriptor.idVendor) == USB_VID_TERRATEC) &&
+			(le16_to_cpu(udev->descriptor.idProduct) == 0x0099)) {
+		if (!strcmp("Afatech", manufacturer)) {
+			dev_dbg(&udev->dev, "%s: rejecting device\n", __func__);
+			return -ENODEV;
+		}
+	}
+
+	return dvb_usbv2_probe(intf, id);
+}
+
 /* interface 0 is used by DVB-T receiver and
    interface 1 is for remote controller (HID) */
 static const struct dvb_usb_device_properties af9035_props = {
@@ -1237,7 +1424,6 @@ static const struct dvb_usb_device_properties af9035_props = {
 	.generic_bulk_ctrl_endpoint_response = 0x81,
 
 	.identify_state = af9035_identify_state,
-	.firmware = AF9035_FIRMWARE_AF9035,
 	.download_firmware = af9035_download_firmware,
 
 	.i2c_algo = &af9035_i2c_algo,
@@ -1246,40 +1432,18 @@ static const struct dvb_usb_device_properties af9035_props = {
 	.tuner_attach = af9035_tuner_attach,
 	.init = af9035_init,
 	.get_rc_config = af9035_get_rc_config,
+	.get_stream_config = af9035_get_stream_config,
 
 	.get_adapter_count = af9035_get_adapter_count,
 	.adapter = {
 		{
-			.stream = DVB_USB_STREAM_BULK(0x84, 6, 87 * 188),
-		}, {
-			.stream = DVB_USB_STREAM_BULK(0x85, 6, 87 * 188),
-		},
-	},
-};
-
-static const struct dvb_usb_device_properties it9135_props = {
-	.driver_name = KBUILD_MODNAME,
-	.owner = THIS_MODULE,
-	.adapter_nr = adapter_nr,
-	.size_of_priv = sizeof(struct state),
+			.caps = DVB_USB_ADAP_HAS_PID_FILTER |
+				DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
 
-	.generic_bulk_ctrl_endpoint = 0x02,
-	.generic_bulk_ctrl_endpoint_response = 0x81,
-
-	.identify_state = af9035_identify_state,
-	.firmware = AF9035_FIRMWARE_IT9135,
-	.download_firmware = af9035_download_firmware_it9135,
+			.pid_filter_count = 32,
+			.pid_filter_ctrl = af9035_pid_filter_ctrl,
+			.pid_filter = af9035_pid_filter,
 
-	.i2c_algo = &af9035_i2c_algo,
-	.read_config = af9035_read_config_it9135,
-	.frontend_attach = af9035_frontend_attach,
-	.tuner_attach = af9035_tuner_attach,
-	.init = af9035_init,
-	.get_rc_config = af9035_get_rc_config,
-
-	.num_adapters = 1,
-	.adapter = {
-		{
 			.stream = DVB_USB_STREAM_BULK(0x84, 6, 87 * 188),
 		}, {
 			.stream = DVB_USB_STREAM_BULK(0x85, 6, 87 * 188),
@@ -1288,6 +1452,7 @@ static const struct dvb_usb_device_properties it9135_props = {
 };
 
 static const struct usb_device_id af9035_id_table[] = {
+	/* AF9035 devices */
 	{ DVB_USB_DEVICE(USB_VID_AFATECH, USB_PID_AFATECH_AF9035_9035,
 		&af9035_props, "Afatech AF9035 reference design", NULL) },
 	{ DVB_USB_DEVICE(USB_VID_AFATECH, USB_PID_AFATECH_AF9035_1000,
@@ -1312,6 +1477,18 @@ static const struct usb_device_id af9035_id_table[] = {
 		&af9035_props, "AVerMedia Twinstar (A825)", NULL) },
 	{ DVB_USB_DEVICE(USB_VID_ASUS, USB_PID_ASUS_U3100MINI_PLUS,
 		&af9035_props, "Asus U3100Mini Plus", NULL) },
+        { DVB_USB_DEVICE(USB_VID_TERRATEC, 0x00aa,
+		&af9035_props, "TerraTec Cinergy T Stick (rev. 2)", NULL) },
+	/* IT9135 devices */
+#if 0
+	{ DVB_USB_DEVICE(0x048d, 0x9135,
+		&af9035_props, "IT9135 reference design", NULL) },
+	{ DVB_USB_DEVICE(0x048d, 0x9006,
+		&af9035_props, "IT9135 reference design", NULL) },
+#endif
+	/* XXX: that same ID [0ccd:0099] is used by af9015 driver too */
+	{ DVB_USB_DEVICE(USB_VID_TERRATEC, 0x0099,
+		&af9035_props, "TerraTec Cinergy T Stick Dual RC (rev. 2)", NULL) },
 	{ }
 };
 MODULE_DEVICE_TABLE(usb, af9035_id_table);
@@ -1319,7 +1496,7 @@ MODULE_DEVICE_TABLE(usb, af9035_id_table);
 static struct usb_driver af9035_usb_driver = {
 	.name = KBUILD_MODNAME,
 	.id_table = af9035_id_table,
-	.probe = dvb_usbv2_probe,
+	.probe = af9035_probe,
 	.disconnect = dvb_usbv2_disconnect,
 	.suspend = dvb_usbv2_suspend,
 	.resume = dvb_usbv2_resume,
@@ -1334,4 +1511,5 @@ MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
 MODULE_DESCRIPTION("Afatech AF9035 driver");
 MODULE_LICENSE("GPL");
 MODULE_FIRMWARE(AF9035_FIRMWARE_AF9035);
-MODULE_FIRMWARE(AF9035_FIRMWARE_IT9135);
+MODULE_FIRMWARE(AF9035_FIRMWARE_IT9135_V1);
+MODULE_FIRMWARE(AF9035_FIRMWARE_IT9135_V2);
diff --git a/drivers/media/usb/dvb-usb-v2/af9035.h b/drivers/media/usb/dvb-usb-v2/af9035.h
index 29f3eec22c2..b5827ca3a01 100644
--- a/drivers/media/usb/dvb-usb-v2/af9035.h
+++ b/drivers/media/usb/dvb-usb-v2/af9035.h
@@ -30,6 +30,7 @@
 #include "mxl5007t.h"
 #include "tda18218.h"
 #include "fc2580.h"
+#include "tuner_it913x.h"
 
 struct reg_val {
 	u32 reg;
@@ -52,12 +53,18 @@ struct usb_req {
 };
 
 struct state {
+#define BUF_LEN 64
+	u8 buf[BUF_LEN];
 	u8 seq; /* packet sequence number */
-	bool dual_mode;
+	u8 prechip_version;
+	u8 chip_version;
+	u16 chip_type;
+	u8 dual_mode:1;
+	u16 eeprom_addr;
 	struct af9033_config af9033_config[2];
 };
 
-u32 clock_lut[] = {
+static const u32 clock_lut_af9035[] = {
 	20480000, /*      FPGA */
 	16384000, /* 16.38 MHz */
 	20480000, /* 20.48 MHz */
@@ -72,7 +79,7 @@ u32 clock_lut[] = {
 	12000000, /* 12.00 MHz */
 };
 
-u32 clock_lut_it9135[] = {
+static const u32 clock_lut_it9135[] = {
 	12000000, /* 12.00 MHz */
 	20480000, /* 20.48 MHz */
 	36000000, /* 36.00 MHz */
@@ -86,19 +93,31 @@ u32 clock_lut_it9135[] = {
 };
 
 #define AF9035_FIRMWARE_AF9035 "dvb-usb-af9035-02.fw"
-#define AF9035_FIRMWARE_IT9135 "dvb-usb-it9135-01.fw"
+#define AF9035_FIRMWARE_IT9135_V1 "dvb-usb-it9135-01.fw"
+#define AF9035_FIRMWARE_IT9135_V2 "dvb-usb-it9135-02.fw"
 
-/* EEPROM locations */
-#define EEPROM_IR_MODE            0x430d
-#define EEPROM_DUAL_MODE          0x4326
-#define EEPROM_2ND_DEMOD_ADDR     0x4327
-#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
+/*
+ * eeprom is memory mapped as read only. Writing that memory mapped address
+ * will not corrupt eeprom.
+ *
+ * eeprom has value 0x00 single mode and 0x03 for dual mode as far as I have
+ * seen to this day.
+ */
+
+#define EEPROM_BASE_AF9035        0x42fd
+#define EEPROM_BASE_IT9135        0x499c
+#define EEPROM_SHIFT                0x10
+
+#define EEPROM_IR_MODE              0x10
+#define EEPROM_DUAL_MODE            0x29
+#define EEPROM_2ND_DEMOD_ADDR       0x2a
+#define EEPROM_IR_TYPE              0x2c
+#define EEPROM_1_IF_L               0x30
+#define EEPROM_1_IF_H               0x31
+#define EEPROM_1_TUNER_ID           0x34
+#define EEPROM_2_IF_L               0x40
+#define EEPROM_2_IF_H               0x41
+#define EEPROM_2_TUNER_ID           0x44
 
 /* USB commands */
 #define CMD_MEM_RD                  0x00
diff --git a/drivers/media/usb/dvb-usb-v2/anysee.c b/drivers/media/usb/dvb-usb-v2/anysee.c
index a20d691d0b6..90cfa35ef6e 100644
--- a/drivers/media/usb/dvb-usb-v2/anysee.c
+++ b/drivers/media/usb/dvb-usb-v2/anysee.c
@@ -45,25 +45,24 @@
 #include "cxd2820r.h"
 
 DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
-static DEFINE_MUTEX(anysee_usb_mutex);
 
-static int anysee_ctrl_msg(struct dvb_usb_device *d, u8 *sbuf, u8 slen,
-	u8 *rbuf, u8 rlen)
+static int anysee_ctrl_msg(struct dvb_usb_device *d,
+		u8 *sbuf, u8 slen, u8 *rbuf, u8 rlen)
 {
 	struct anysee_state *state = d_to_priv(d);
 	int act_len, ret, i;
-	u8 buf[64];
 
-	memcpy(&buf[0], sbuf, slen);
-	buf[60] = state->seq++;
+	mutex_lock(&d->usb_mutex);
 
-	mutex_lock(&anysee_usb_mutex);
+	memcpy(&state->buf[0], sbuf, slen);
+	state->buf[60] = state->seq++;
 
-	dev_dbg(&d->udev->dev, "%s: >>> %*ph\n", __func__, slen, buf);
+	dev_dbg(&d->udev->dev, "%s: >>> %*ph\n", __func__, slen, state->buf);
 
 	/* We need receive one message more after dvb_usb_generic_rw due
 	   to weird transaction flow, which is 1 x send + 2 x receive. */
-	ret = dvb_usbv2_generic_rw(d, buf, sizeof(buf), buf, sizeof(buf));
+	ret = dvb_usbv2_generic_rw_locked(d, state->buf, sizeof(state->buf),
+			state->buf, sizeof(state->buf));
 	if (ret)
 		goto error_unlock;
 
@@ -82,20 +81,19 @@ static int anysee_ctrl_msg(struct dvb_usb_device *d, u8 *sbuf, u8 slen,
 	for (i = 0; i < 3; i++) {
 		/* receive 2nd answer */
 		ret = usb_bulk_msg(d->udev, usb_rcvbulkpipe(d->udev,
-			d->props->generic_bulk_ctrl_endpoint), buf, sizeof(buf),
-			&act_len, 2000);
-
+				d->props->generic_bulk_ctrl_endpoint),
+				state->buf, sizeof(state->buf), &act_len, 2000);
 		if (ret) {
-			dev_dbg(&d->udev->dev, "%s: recv bulk message " \
-					"failed=%d\n", __func__, ret);
+			dev_dbg(&d->udev->dev,
+					"%s: recv bulk message failed=%d\n",
+					__func__, ret);
 		} else {
 			dev_dbg(&d->udev->dev, "%s: <<< %*ph\n", __func__,
-					rlen, buf);
-
-			if (buf[63] != 0x4f)
-				dev_dbg(&d->udev->dev, "%s: cmd failed\n",
-						__func__);
+					rlen, state->buf);
 
+			if (state->buf[63] != 0x4f)
+				dev_dbg(&d->udev->dev,
+						"%s: cmd failed\n", __func__);
 			break;
 		}
 	}
@@ -109,11 +107,10 @@ static int anysee_ctrl_msg(struct dvb_usb_device *d, u8 *sbuf, u8 slen,
 
 	/* read request, copy returned data to return buf */
 	if (rbuf && rlen)
-		memcpy(rbuf, buf, rlen);
+		memcpy(rbuf, state->buf, rlen);
 
 error_unlock:
-	mutex_unlock(&anysee_usb_mutex);
-
+	mutex_unlock(&d->usb_mutex);
 	return ret;
 }
 
@@ -638,7 +635,7 @@ static int anysee_frontend_attach(struct dvb_usb_adapter *adap)
 {
 	struct anysee_state *state = adap_to_priv(adap);
 	struct dvb_usb_device *d = adap_to_d(adap);
-	int ret;
+	int ret = 0;
 	u8 tmp;
 	struct i2c_msg msg[2] = {
 		{
@@ -884,9 +881,8 @@ static int anysee_frontend_attach(struct dvb_usb_adapter *adap)
 	if (!adap->fe[0]) {
 		/* we have no frontend :-( */
 		ret = -ENODEV;
-		dev_err(&d->udev->dev, "%s: Unsupported Anysee version. " \
-				"Please report the " \
-				"<linux-media@vger.kernel.org>.\n",
+		dev_err(&d->udev->dev,
+				"%s: Unsupported Anysee version. Please report to <linux-media@vger.kernel.org>.\n",
 				KBUILD_MODNAME);
 	}
 error:
diff --git a/drivers/media/usb/dvb-usb-v2/anysee.h b/drivers/media/usb/dvb-usb-v2/anysee.h
index c1a4273f14f..8f426d9fc6e 100644
--- a/drivers/media/usb/dvb-usb-v2/anysee.h
+++ b/drivers/media/usb/dvb-usb-v2/anysee.h
@@ -52,8 +52,9 @@ enum cmd {
 };
 
 struct anysee_state {
-	u8 hw; /* PCB ID */
+	u8 buf[64];
 	u8 seq;
+	u8 hw; /* PCB ID */
 	u8 fe_id:1; /* frondend ID */
 	u8 has_ci:1;
 	u8 ci_attached:1;
diff --git a/drivers/media/usb/dvb-usb-v2/az6007.c b/drivers/media/usb/dvb-usb-v2/az6007.c
index 70ec80d8be7..44c64ef361b 100644
--- a/drivers/media/usb/dvb-usb-v2/az6007.c
+++ b/drivers/media/usb/dvb-usb-v2/az6007.c
@@ -842,7 +842,7 @@ static int az6007_download_firmware(struct dvb_usb_device *d,
 {
 	pr_debug("Loading az6007 firmware\n");
 
-	return usbv2_cypress_load_firmware(d->udev, fw, CYPRESS_FX2);
+	return cypress_load_firmware(d->udev, fw, CYPRESS_FX2);
 }
 
 /* DVB USB Driver stuff */
diff --git a/drivers/media/usb/dvb-usb-v2/dvb_usb.h b/drivers/media/usb/dvb-usb-v2/dvb_usb.h
index 3cac8bd0b11..658c6d47fdf 100644
--- a/drivers/media/usb/dvb-usb-v2/dvb_usb.h
+++ b/drivers/media/usb/dvb-usb-v2/dvb_usb.h
@@ -329,13 +329,16 @@ struct dvb_usb_adapter {
 	u8 feed_count;
 	u8 max_feed_count;
 	s8 active_fe;
+#define ADAP_INIT                0
+#define ADAP_SLEEP               1
+#define ADAP_STREAMING           2
+	unsigned long state_bits;
 
 	/* dvb */
 	struct dvb_adapter   dvb_adap;
 	struct dmxdev        dmxdev;
 	struct dvb_demux     demux;
 	struct dvb_net       dvb_net;
-	struct mutex         sync_mutex;
 
 	struct dvb_frontend *fe[MAX_NO_OF_FE_PER_ADAP];
 	int (*fe_init[MAX_NO_OF_FE_PER_ADAP]) (struct dvb_frontend *);
@@ -400,5 +403,9 @@ extern int dvb_usbv2_reset_resume(struct usb_interface *);
 /* the generic read/write method for device control */
 extern int dvb_usbv2_generic_rw(struct dvb_usb_device *, u8 *, u16, u8 *, u16);
 extern int dvb_usbv2_generic_write(struct dvb_usb_device *, u8 *, u16);
+/* caller must hold lock when locked versions are called */
+extern int dvb_usbv2_generic_rw_locked(struct dvb_usb_device *,
+		u8 *, u16, u8 *, u16);
+extern int dvb_usbv2_generic_write_locked(struct dvb_usb_device *, u8 *, u16);
 
 #endif
diff --git a/drivers/media/usb/dvb-usb-v2/dvb_usb_core.c b/drivers/media/usb/dvb-usb-v2/dvb_usb_core.c
index 08679205591..19f6737d981 100644
--- a/drivers/media/usb/dvb-usb-v2/dvb_usb_core.c
+++ b/drivers/media/usb/dvb-usb-v2/dvb_usb_core.c
@@ -28,10 +28,11 @@ MODULE_PARM_DESC(disable_rc_polling,
 static int dvb_usb_force_pid_filter_usage;
 module_param_named(force_pid_filter_usage, dvb_usb_force_pid_filter_usage,
 		int, 0444);
-MODULE_PARM_DESC(force_pid_filter_usage, "force all DVB USB devices to use a " \
-		"PID filter, if any (default: 0)");
+MODULE_PARM_DESC(force_pid_filter_usage,
+		"force all DVB USB devices to use a PID filter, if any (default: 0)");
 
-static int dvb_usbv2_download_firmware(struct dvb_usb_device *d, const char *name)
+static int dvb_usbv2_download_firmware(struct dvb_usb_device *d,
+		const char *name)
 {
 	int ret;
 	const struct firmware *fw;
@@ -44,10 +45,9 @@ static int dvb_usbv2_download_firmware(struct dvb_usb_device *d, const char *nam
 
 	ret = request_firmware(&fw, name, &d->udev->dev);
 	if (ret < 0) {
-		dev_err(&d->udev->dev, "%s: Did not find the firmware file "\
-				"'%s'. Please see linux/Documentation/dvb/ " \
-				"for more details on firmware-problems. " \
-				"Status %d\n", KBUILD_MODNAME, name, ret);
+		dev_err(&d->udev->dev,
+				"%s: Did not find the firmware file '%s'. Please see linux/Documentation/dvb/ for more details on firmware-problems. Status %d\n",
+				KBUILD_MODNAME, name, ret);
 		goto err;
 	}
 
@@ -181,9 +181,9 @@ static int dvb_usbv2_remote_init(struct dvb_usb_device *d)
 		/* initialize a work queue for handling polling */
 		INIT_DELAYED_WORK(&d->rc_query_work,
 				dvb_usb_read_remote_control);
-		dev_info(&d->udev->dev, "%s: schedule remote query interval " \
-				"to %d msecs\n", KBUILD_MODNAME,
-				d->rc.interval);
+		dev_info(&d->udev->dev,
+				"%s: schedule remote query interval to %d msecs\n",
+				KBUILD_MODNAME, d->rc.interval);
 		schedule_delayed_work(&d->rc_query_work,
 				msecs_to_jiffies(d->rc.interval));
 		d->rc_polling_active = true;
@@ -253,128 +253,159 @@ static int dvb_usbv2_adapter_stream_exit(struct dvb_usb_adapter *adap)
 	return usb_urb_exitv2(&adap->stream);
 }
 
-static inline int dvb_usb_ctrl_feed(struct dvb_demux_feed *dvbdmxfeed,
-		int count)
+static int wait_schedule(void *ptr)
+{
+	schedule();
+
+	return 0;
+}
+
+static int dvb_usb_start_feed(struct dvb_demux_feed *dvbdmxfeed)
 {
 	struct dvb_usb_adapter *adap = dvbdmxfeed->demux->priv;
 	struct dvb_usb_device *d = adap_to_d(adap);
-	int ret;
-	dev_dbg(&d->udev->dev, "%s: adap=%d active_fe=%d feed_type=%d " \
-			"setting pid [%s]: %04x (%04d) at index %d '%s'\n",
+	int ret = 0;
+	struct usb_data_stream_properties stream_props;
+	dev_dbg(&d->udev->dev,
+			"%s: adap=%d active_fe=%d feed_type=%d setting pid [%s]: %04x (%04d) at index %d\n",
 			__func__, adap->id, adap->active_fe, dvbdmxfeed->type,
 			adap->pid_filtering ? "yes" : "no", dvbdmxfeed->pid,
-			dvbdmxfeed->pid, dvbdmxfeed->index,
-			(count == 1) ? "on" : "off");
+			dvbdmxfeed->pid, dvbdmxfeed->index);
+
+	/* wait init is done */
+	wait_on_bit(&adap->state_bits, ADAP_INIT, wait_schedule,
+			TASK_UNINTERRUPTIBLE);
 
 	if (adap->active_fe == -1)
 		return -EINVAL;
 
-	adap->feed_count += count;
-
-	/* stop feeding if it is last pid */
-	if (adap->feed_count == 0) {
-		dev_dbg(&d->udev->dev, "%s: stop feeding\n", __func__);
-
-		if (d->props->streaming_ctrl) {
-			ret = d->props->streaming_ctrl(
-					adap->fe[adap->active_fe], 0);
-			if (ret < 0) {
-				dev_err(&d->udev->dev, "%s: streaming_ctrl() " \
-						"failed=%d\n", KBUILD_MODNAME,
-						ret);
-				usb_urb_killv2(&adap->stream);
-				goto err_mutex_unlock;
-			}
-		}
-		usb_urb_killv2(&adap->stream);
-		mutex_unlock(&adap->sync_mutex);
-	}
+	/* skip feed setup if we are already feeding */
+	if (adap->feed_count++ > 0)
+		goto skip_feed_start;
 
-	/* activate the pid on the device pid filter */
-	if (adap->props->caps & DVB_USB_ADAP_HAS_PID_FILTER &&
-			adap->pid_filtering && adap->props->pid_filter) {
-		ret = adap->props->pid_filter(adap, dvbdmxfeed->index,
-				dvbdmxfeed->pid, (count == 1) ? 1 : 0);
-		if (ret < 0)
-			dev_err(&d->udev->dev, "%s: pid_filter() failed=%d\n",
+	/* set 'streaming' status bit */
+	set_bit(ADAP_STREAMING, &adap->state_bits);
+
+	/* resolve input and output streaming parameters */
+	if (d->props->get_stream_config) {
+		memcpy(&stream_props, &adap->props->stream,
+				sizeof(struct usb_data_stream_properties));
+		ret = d->props->get_stream_config(adap->fe[adap->active_fe],
+				&adap->ts_type, &stream_props);
+		if (ret)
+			dev_err(&d->udev->dev,
+					"%s: get_stream_config() failed=%d\n",
 					KBUILD_MODNAME, ret);
+	} else {
+		stream_props = adap->props->stream;
 	}
 
-	/* start feeding if it is first pid */
-	if (adap->feed_count == 1 && count == 1) {
-		struct usb_data_stream_properties stream_props;
-		mutex_lock(&adap->sync_mutex);
-		dev_dbg(&d->udev->dev, "%s: start feeding\n", __func__);
+	switch (adap->ts_type) {
+	case DVB_USB_FE_TS_TYPE_204:
+		adap->stream.complete = dvb_usb_data_complete_204;
+		break;
+	case DVB_USB_FE_TS_TYPE_RAW:
+		adap->stream.complete = dvb_usb_data_complete_raw;
+		break;
+	case DVB_USB_FE_TS_TYPE_188:
+	default:
+		adap->stream.complete = dvb_usb_data_complete;
+		break;
+	}
 
-		/* resolve input and output streaming paramters */
-		if (d->props->get_stream_config) {
-			memcpy(&stream_props, &adap->props->stream,
-				sizeof(struct usb_data_stream_properties));
-			ret = d->props->get_stream_config(
-					adap->fe[adap->active_fe],
-					&adap->ts_type, &stream_props);
-			if (ret < 0)
-				goto err_mutex_unlock;
-		} else {
-			stream_props = adap->props->stream;
-		}
+	/* submit USB streaming packets */
+	usb_urb_submitv2(&adap->stream, &stream_props);
 
-		switch (adap->ts_type) {
-		case DVB_USB_FE_TS_TYPE_204:
-			adap->stream.complete = dvb_usb_data_complete_204;
-			break;
-		case DVB_USB_FE_TS_TYPE_RAW:
-			adap->stream.complete = dvb_usb_data_complete_raw;
-			break;
-		case DVB_USB_FE_TS_TYPE_188:
-		default:
-			adap->stream.complete = dvb_usb_data_complete;
-			break;
-		}
+	/* enable HW PID filter */
+	if (adap->pid_filtering && adap->props->pid_filter_ctrl) {
+		ret = adap->props->pid_filter_ctrl(adap, 1);
+		if (ret)
+			dev_err(&d->udev->dev,
+					"%s: pid_filter_ctrl() failed=%d\n",
+					KBUILD_MODNAME, ret);
+	}
 
-		usb_urb_submitv2(&adap->stream, &stream_props);
-
-		if (adap->props->caps & DVB_USB_ADAP_HAS_PID_FILTER &&
-				adap->props->caps &
-				DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF &&
-				adap->props->pid_filter_ctrl) {
-			ret = adap->props->pid_filter_ctrl(adap,
-					adap->pid_filtering);
-			if (ret < 0) {
-				dev_err(&d->udev->dev, "%s: " \
-						"pid_filter_ctrl() failed=%d\n",
-						KBUILD_MODNAME, ret);
-				goto err_mutex_unlock;
-			}
-		}
+	/* ask device to start streaming */
+	if (d->props->streaming_ctrl) {
+		ret = d->props->streaming_ctrl(adap->fe[adap->active_fe], 1);
+		if (ret)
+			dev_err(&d->udev->dev,
+					"%s: streaming_ctrl() failed=%d\n",
+					KBUILD_MODNAME, ret);
+	}
+skip_feed_start:
 
-		if (d->props->streaming_ctrl) {
-			ret = d->props->streaming_ctrl(
-					adap->fe[adap->active_fe], 1);
-			if (ret < 0) {
-				dev_err(&d->udev->dev, "%s: streaming_ctrl() " \
-						"failed=%d\n", KBUILD_MODNAME,
-						ret);
-				goto err_mutex_unlock;
-			}
-		}
+	/* add PID to device HW PID filter */
+	if (adap->pid_filtering && adap->props->pid_filter) {
+		ret = adap->props->pid_filter(adap, dvbdmxfeed->index,
+				dvbdmxfeed->pid, 1);
+		if (ret)
+			dev_err(&d->udev->dev, "%s: pid_filter() failed=%d\n",
+					KBUILD_MODNAME, ret);
 	}
 
-	return 0;
-err_mutex_unlock:
-	mutex_unlock(&adap->sync_mutex);
-	dev_dbg(&d->udev->dev, "%s: failed=%d\n", __func__, ret);
+	if (ret)
+		dev_dbg(&d->udev->dev, "%s: failed=%d\n", __func__, ret);
 	return ret;
 }
 
-static int dvb_usb_start_feed(struct dvb_demux_feed *dvbdmxfeed)
-{
-	return dvb_usb_ctrl_feed(dvbdmxfeed, 1);
-}
-
 static int dvb_usb_stop_feed(struct dvb_demux_feed *dvbdmxfeed)
 {
-	return dvb_usb_ctrl_feed(dvbdmxfeed, -1);
+	struct dvb_usb_adapter *adap = dvbdmxfeed->demux->priv;
+	struct dvb_usb_device *d = adap_to_d(adap);
+	int ret = 0;
+	dev_dbg(&d->udev->dev,
+			"%s: adap=%d active_fe=%d feed_type=%d setting pid [%s]: %04x (%04d) at index %d\n",
+			__func__, adap->id, adap->active_fe, dvbdmxfeed->type,
+			adap->pid_filtering ? "yes" : "no", dvbdmxfeed->pid,
+			dvbdmxfeed->pid, dvbdmxfeed->index);
+
+	if (adap->active_fe == -1)
+		return -EINVAL;
+
+	/* remove PID from device HW PID filter */
+	if (adap->pid_filtering && adap->props->pid_filter) {
+		ret = adap->props->pid_filter(adap, dvbdmxfeed->index,
+				dvbdmxfeed->pid, 0);
+		if (ret)
+			dev_err(&d->udev->dev, "%s: pid_filter() failed=%d\n",
+					KBUILD_MODNAME, ret);
+	}
+
+	/* we cannot stop streaming until last PID is removed */
+	if (--adap->feed_count > 0)
+		goto skip_feed_stop;
+
+	/* ask device to stop streaming */
+	if (d->props->streaming_ctrl) {
+		ret = d->props->streaming_ctrl(adap->fe[adap->active_fe], 0);
+		if (ret)
+			dev_err(&d->udev->dev,
+					"%s: streaming_ctrl() failed=%d\n",
+					KBUILD_MODNAME, ret);
+	}
+
+	/* disable HW PID filter */
+	if (adap->pid_filtering && adap->props->pid_filter_ctrl) {
+		ret = adap->props->pid_filter_ctrl(adap, 0);
+		if (ret)
+			dev_err(&d->udev->dev,
+					"%s: pid_filter_ctrl() failed=%d\n",
+					KBUILD_MODNAME, ret);
+	}
+
+	/* kill USB streaming packets */
+	usb_urb_killv2(&adap->stream);
+
+	/* clear 'streaming' status bit */
+	clear_bit(ADAP_STREAMING, &adap->state_bits);
+	smp_mb__after_clear_bit();
+	wake_up_bit(&adap->state_bits, ADAP_STREAMING);
+skip_feed_stop:
+
+	if (ret)
+		dev_dbg(&d->udev->dev, "%s: failed=%d\n", __func__, ret);
+	return ret;
 }
 
 static int dvb_usbv2_adapter_dvb_init(struct dvb_usb_adapter *adap)
@@ -435,8 +466,6 @@ static int dvb_usbv2_adapter_dvb_init(struct dvb_usb_adapter *adap)
 		goto err_dvb_net_init;
 	}
 
-	mutex_init(&adap->sync_mutex);
-
 	return 0;
 err_dvb_net_init:
 	dvb_dmxdev_release(&adap->dmxdev);
@@ -500,7 +529,7 @@ static int dvb_usb_fe_init(struct dvb_frontend *fe)
 
 	if (!adap->suspend_resume_active) {
 		adap->active_fe = fe->id;
-		mutex_lock(&adap->sync_mutex);
+		set_bit(ADAP_INIT, &adap->state_bits);
 	}
 
 	ret = dvb_usbv2_device_power_ctrl(d, 1);
@@ -519,8 +548,11 @@ static int dvb_usb_fe_init(struct dvb_frontend *fe)
 			goto err;
 	}
 err:
-	if (!adap->suspend_resume_active)
-		mutex_unlock(&adap->sync_mutex);
+	if (!adap->suspend_resume_active) {
+		clear_bit(ADAP_INIT, &adap->state_bits);
+		smp_mb__after_clear_bit();
+		wake_up_bit(&adap->state_bits, ADAP_INIT);
+	}
 
 	dev_dbg(&d->udev->dev, "%s: ret=%d\n", __func__, ret);
 	return ret;
@@ -534,8 +566,11 @@ static int dvb_usb_fe_sleep(struct dvb_frontend *fe)
 	dev_dbg(&d->udev->dev, "%s: adap=%d fe=%d\n", __func__, adap->id,
 			fe->id);
 
-	if (!adap->suspend_resume_active)
-		mutex_lock(&adap->sync_mutex);
+	if (!adap->suspend_resume_active) {
+		set_bit(ADAP_SLEEP, &adap->state_bits);
+		wait_on_bit(&adap->state_bits, ADAP_STREAMING, wait_schedule,
+				TASK_UNINTERRUPTIBLE);
+	}
 
 	if (adap->fe_sleep[fe->id]) {
 		ret = adap->fe_sleep[fe->id](fe);
@@ -555,7 +590,9 @@ static int dvb_usb_fe_sleep(struct dvb_frontend *fe)
 err:
 	if (!adap->suspend_resume_active) {
 		adap->active_fe = -1;
-		mutex_unlock(&adap->sync_mutex);
+		clear_bit(ADAP_SLEEP, &adap->state_bits);
+		smp_mb__after_clear_bit();
+		wake_up_bit(&adap->state_bits, ADAP_SLEEP);
 	}
 
 	dev_dbg(&d->udev->dev, "%s: ret=%d\n", __func__, ret);
@@ -574,8 +611,9 @@ static int dvb_usbv2_adapter_frontend_init(struct dvb_usb_adapter *adap)
 	if (d->props->frontend_attach) {
 		ret = d->props->frontend_attach(adap);
 		if (ret < 0) {
-			dev_dbg(&d->udev->dev, "%s: frontend_attach() " \
-					"failed=%d\n", __func__, ret);
+			dev_dbg(&d->udev->dev,
+					"%s: frontend_attach() failed=%d\n",
+					__func__, ret);
 			goto err_dvb_frontend_detach;
 		}
 	} else {
@@ -595,8 +633,9 @@ static int dvb_usbv2_adapter_frontend_init(struct dvb_usb_adapter *adap)
 
 		ret = dvb_register_frontend(&adap->dvb_adap, adap->fe[i]);
 		if (ret < 0) {
-			dev_err(&d->udev->dev, "%s: frontend%d registration " \
-					"failed\n", KBUILD_MODNAME, i);
+			dev_err(&d->udev->dev,
+					"%s: frontend%d registration failed\n",
+					KBUILD_MODNAME, i);
 			goto err_dvb_unregister_frontend;
 		}
 
@@ -670,33 +709,33 @@ static int dvb_usbv2_adapter_init(struct dvb_usb_device *d)
 		/* speed - when running at FULL speed we need a HW PID filter */
 		if (d->udev->speed == USB_SPEED_FULL &&
 				!(adap->props->caps & DVB_USB_ADAP_HAS_PID_FILTER)) {
-			dev_err(&d->udev->dev, "%s: this USB2.0 device " \
-					"cannot be run on a USB1.1 port (it " \
-					"lacks a hardware PID filter)\n",
+			dev_err(&d->udev->dev,
+					"%s: this USB2.0 device cannot be run on a USB1.1 port (it lacks a hardware PID filter)\n",
 					KBUILD_MODNAME);
 			ret = -ENODEV;
 			goto err;
 		} else if ((d->udev->speed == USB_SPEED_FULL &&
 				adap->props->caps & DVB_USB_ADAP_HAS_PID_FILTER) ||
 				(adap->props->caps & DVB_USB_ADAP_NEED_PID_FILTERING)) {
-			dev_info(&d->udev->dev, "%s: will use the device's " \
-					"hardware PID filter " \
-					"(table count: %d)\n", KBUILD_MODNAME,
+			dev_info(&d->udev->dev,
+					"%s: will use the device's hardware PID filter (table count: %d)\n",
+					KBUILD_MODNAME,
 					adap->props->pid_filter_count);
 			adap->pid_filtering  = 1;
 			adap->max_feed_count = adap->props->pid_filter_count;
 		} else {
-			dev_info(&d->udev->dev, "%s: will pass the complete " \
-					"MPEG2 transport stream to the " \
-					"software demuxer\n", KBUILD_MODNAME);
+			dev_info(&d->udev->dev,
+					"%s: will pass the complete MPEG2 transport stream to the software demuxer\n",
+					KBUILD_MODNAME);
 			adap->pid_filtering  = 0;
 			adap->max_feed_count = 255;
 		}
 
 		if (!adap->pid_filtering && dvb_usb_force_pid_filter_usage &&
 				adap->props->caps & DVB_USB_ADAP_HAS_PID_FILTER) {
-			dev_info(&d->udev->dev, "%s: PID filter enabled by " \
-					"module option\n", KBUILD_MODNAME);
+			dev_info(&d->udev->dev,
+					"%s: PID filter enabled by module option\n",
+					KBUILD_MODNAME);
 			adap->pid_filtering  = 1;
 			adap->max_feed_count = adap->props->pid_filter_count;
 		}
@@ -825,8 +864,9 @@ static void dvb_usbv2_init_work(struct work_struct *work)
 		if (ret == 0) {
 			;
 		} else if (ret == COLD) {
-			dev_info(&d->udev->dev, "%s: found a '%s' in cold " \
-					"state\n", KBUILD_MODNAME, d->name);
+			dev_info(&d->udev->dev,
+					"%s: found a '%s' in cold state\n",
+					KBUILD_MODNAME, d->name);
 
 			if (!name)
 				name = d->props->firmware;
@@ -868,8 +908,9 @@ static void dvb_usbv2_init_work(struct work_struct *work)
 	if (ret < 0)
 		goto err_usb_driver_release_interface;
 
-	dev_info(&d->udev->dev, "%s: '%s' successfully initialized and " \
-			"connected\n", KBUILD_MODNAME, d->name);
+	dev_info(&d->udev->dev,
+			"%s: '%s' successfully initialized and connected\n",
+			KBUILD_MODNAME, d->name);
 
 	return;
 err_usb_driver_release_interface:
diff --git a/drivers/media/usb/dvb-usb-v2/dvb_usb_urb.c b/drivers/media/usb/dvb-usb-v2/dvb_usb_urb.c
index 5716662b483..33ff97e708e 100644
--- a/drivers/media/usb/dvb-usb-v2/dvb_usb_urb.c
+++ b/drivers/media/usb/dvb-usb-v2/dvb_usb_urb.c
@@ -21,8 +21,8 @@
 
 #include "dvb_usb_common.h"
 
-int dvb_usbv2_generic_rw(struct dvb_usb_device *d, u8 *wbuf, u16 wlen, u8 *rbuf,
-		u16 rlen)
+static int dvb_usb_v2_generic_io(struct dvb_usb_device *d,
+		u8 *wbuf, u16 wlen, u8 *rbuf, u16 rlen)
 {
 	int ret, actual_length;
 
@@ -32,8 +32,6 @@ int dvb_usbv2_generic_rw(struct dvb_usb_device *d, u8 *wbuf, u16 wlen, u8 *rbuf,
 		return -EINVAL;
 	}
 
-	mutex_lock(&d->usb_mutex);
-
 	dev_dbg(&d->udev->dev, "%s: >>> %*ph\n", __func__, wlen, wbuf);
 
 	ret = usb_bulk_msg(d->udev, usb_sndbulkpipe(d->udev,
@@ -56,20 +54,51 @@ int dvb_usbv2_generic_rw(struct dvb_usb_device *d, u8 *wbuf, u16 wlen, u8 *rbuf,
 				d->props->generic_bulk_ctrl_endpoint_response),
 				rbuf, rlen, &actual_length, 2000);
 		if (ret)
-			dev_err(&d->udev->dev, "%s: 2nd usb_bulk_msg() " \
-					"failed=%d\n", KBUILD_MODNAME, ret);
+			dev_err(&d->udev->dev,
+					"%s: 2nd usb_bulk_msg() failed=%d\n",
+					KBUILD_MODNAME, ret);
 
 		dev_dbg(&d->udev->dev, "%s: <<< %*ph\n", __func__,
 				actual_length, rbuf);
 	}
 
+	return ret;
+}
+
+int dvb_usbv2_generic_rw(struct dvb_usb_device *d,
+		u8 *wbuf, u16 wlen, u8 *rbuf, u16 rlen)
+{
+	int ret;
+
+	mutex_lock(&d->usb_mutex);
+	ret = dvb_usb_v2_generic_io(d, wbuf, wlen, rbuf, rlen);
 	mutex_unlock(&d->usb_mutex);
+
 	return ret;
 }
 EXPORT_SYMBOL(dvb_usbv2_generic_rw);
 
 int dvb_usbv2_generic_write(struct dvb_usb_device *d, u8 *buf, u16 len)
 {
-	return dvb_usbv2_generic_rw(d, buf, len, NULL, 0);
+	int ret;
+
+	mutex_lock(&d->usb_mutex);
+	ret = dvb_usb_v2_generic_io(d, buf, len, NULL, 0);
+	mutex_unlock(&d->usb_mutex);
+
+	return ret;
 }
 EXPORT_SYMBOL(dvb_usbv2_generic_write);
+
+int dvb_usbv2_generic_rw_locked(struct dvb_usb_device *d,
+		u8 *wbuf, u16 wlen, u8 *rbuf, u16 rlen)
+{
+	return dvb_usb_v2_generic_io(d, wbuf, wlen, rbuf, rlen);
+}
+EXPORT_SYMBOL(dvb_usbv2_generic_rw_locked);
+
+int dvb_usbv2_generic_write_locked(struct dvb_usb_device *d, u8 *buf, u16 len)
+{
+	return dvb_usb_v2_generic_io(d, buf, len, NULL, 0);
+}
+EXPORT_SYMBOL(dvb_usbv2_generic_write_locked);
diff --git a/drivers/media/usb/dvb-usb-v2/it913x.c b/drivers/media/usb/dvb-usb-v2/it913x.c
index 833847995c6..e48cdeb9df4 100644
--- a/drivers/media/usb/dvb-usb-v2/it913x.c
+++ b/drivers/media/usb/dvb-usb-v2/it913x.c
@@ -218,6 +218,7 @@ static int it913x_pid_filter_ctrl(struct dvb_usb_adapter *adap, int onoff)
 
 	deb_info(1, "PID_C  (%02x)", onoff);
 
+	st->pid_filter_onoff = adap->pid_filtering;
 	ret = it913x_wr_reg(d, pro, PID_EN, st->pid_filter_onoff);
 
 	mutex_unlock(&d->i2c_mutex);
diff --git a/drivers/media/usb/dvb-usb-v2/lmedm04.c b/drivers/media/usb/dvb-usb-v2/lmedm04.c
index f30c58cecbb..b3fd0ffa3c3 100644
--- a/drivers/media/usb/dvb-usb-v2/lmedm04.c
+++ b/drivers/media/usb/dvb-usb-v2/lmedm04.c
@@ -1241,10 +1241,13 @@ static int lme2510_get_stream_config(struct dvb_frontend *fe, u8 *ts_type,
 		struct usb_data_stream_properties *stream)
 {
 	struct dvb_usb_adapter *adap = fe_to_adap(fe);
-	struct dvb_usb_device *d = adap_to_d(adap);
+	struct dvb_usb_device *d;
 
 	if (adap == NULL)
 		return 0;
+
+	d = adap_to_d(adap);
+
 	/* Turn PID filter on the fly by module option */
 	if (pid_filter == 2) {
 		adap->pid_filtering  = 1;
@@ -1299,8 +1302,7 @@ static void lme2510_exit(struct dvb_usb_device *d)
 
 	if (d != NULL) {
 		usb_buffer = lme2510_exit_int(d);
-		if (usb_buffer != NULL)
-			kfree(usb_buffer);
+		kfree(usb_buffer);
 	}
 }
 
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.h b/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.h
index 432706ae527..3f3f8bfd190 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.h
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.h
@@ -21,6 +21,7 @@
 #ifndef __MXL111SF_DEMOD_H__
 #define __MXL111SF_DEMOD_H__
 
+#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 #include "mxl111sf.h"
 
@@ -31,8 +32,7 @@ struct mxl111sf_demod_config {
 			    struct mxl111sf_reg_ctrl_info *ctrl_reg_info);
 };
 
-#if defined(CONFIG_DVB_USB_MXL111SF) || \
-	(defined(CONFIG_DVB_USB_MXL111SF_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_USB_MXL111SF)
 extern
 struct dvb_frontend *mxl111sf_demod_attach(struct mxl111sf_state *mxl_state,
 					   struct mxl111sf_demod_config *cfg);
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf-tuner.h b/drivers/media/usb/dvb-usb-v2/mxl111sf-tuner.h
index ff333960b18..90f583e5d6a 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf-tuner.h
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf-tuner.h
@@ -21,8 +21,8 @@
 #ifndef __MXL111SF_TUNER_H__
 #define __MXL111SF_TUNER_H__
 
+#include <linux/kconfig.h>
 #include "dvb_frontend.h"
-
 #include "mxl111sf.h"
 
 enum mxl_if_freq {
@@ -60,8 +60,7 @@ struct mxl111sf_tuner_config {
 
 /* ------------------------------------------------------------------------ */
 
-#if defined(CONFIG_DVB_USB_MXL111SF) || \
-	(defined(CONFIG_DVB_USB_MXL111SF_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_USB_MXL111SF)
 extern
 struct dvb_frontend *mxl111sf_tuner_attach(struct dvb_frontend *fe,
 					   struct mxl111sf_state *mxl_state,
diff --git a/drivers/media/usb/dvb-usb-v2/rtl28xxu.c b/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
index d98387a3c95..22015fe1a0f 100644
--- a/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
+++ b/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
@@ -33,6 +33,7 @@
 #include "e4000.h"
 #include "fc2580.h"
 #include "tua9001.h"
+#include "r820t.h"
 
 DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
 
@@ -375,6 +376,7 @@ static int rtl2832u_read_config(struct dvb_usb_device *d)
 	struct rtl28xxu_req req_mxl5007t = {0xd9c0, CMD_I2C_RD, 1, buf};
 	struct rtl28xxu_req req_e4000 = {0x02c8, CMD_I2C_RD, 1, buf};
 	struct rtl28xxu_req req_tda18272 = {0x00c0, CMD_I2C_RD, 2, buf};
+	struct rtl28xxu_req req_r820t = {0x0034, CMD_I2C_RD, 5, buf};
 
 	dev_dbg(&d->udev->dev, "%s:\n", __func__);
 
@@ -479,6 +481,14 @@ static int rtl2832u_read_config(struct dvb_usb_device *d)
 		goto found;
 	}
 
+	/* check R820T by reading tuner stats at I2C addr 0x1a */
+	ret = rtl28xxu_ctrl_msg(d, &req_r820t);
+	if (ret == 0) {
+		priv->tuner = TUNER_RTL2832_R820T;
+		priv->tuner_name = "R820T";
+		goto found;
+	}
+
 found:
 	dev_dbg(&d->udev->dev, "%s: tuner=%s\n", __func__, priv->tuner_name);
 
@@ -589,6 +599,12 @@ static struct rtl2832_config rtl28xxu_rtl2832_e4000_config = {
 	.tuner = TUNER_RTL2832_E4000,
 };
 
+static struct rtl2832_config rtl28xxu_rtl2832_r820t_config = {
+	.i2c_addr = 0x10,
+	.xtal = 28800000,
+	.tuner = TUNER_RTL2832_R820T,
+};
+
 static int rtl2832u_fc0012_tuner_callback(struct dvb_usb_device *d,
 		int cmd, int arg)
 {
@@ -728,6 +744,9 @@ static int rtl2832u_frontend_attach(struct dvb_usb_adapter *adap)
 	case TUNER_RTL2832_E4000:
 		rtl2832_config = &rtl28xxu_rtl2832_e4000_config;
 		break;
+	case TUNER_RTL2832_R820T:
+		rtl2832_config = &rtl28xxu_rtl2832_r820t_config;
+		break;
 	default:
 		dev_err(&d->udev->dev, "%s: unknown tuner=%s\n",
 				KBUILD_MODNAME, priv->tuner_name);
@@ -840,6 +859,13 @@ static const struct fc0012_config rtl2832u_fc0012_config = {
 	.xtal_freq = FC_XTAL_28_8_MHZ,
 };
 
+static const struct r820t_config rtl2832u_r820t_config = {
+	.i2c_addr = 0x1a,
+	.xtal = 28800000,
+	.max_i2c_msg_len = 2,
+	.rafael_chip = CHIP_R820T,
+};
+
 static int rtl2832u_tuner_attach(struct dvb_usb_adapter *adap)
 {
 	int ret;
@@ -889,6 +915,14 @@ static int rtl2832u_tuner_attach(struct dvb_usb_adapter *adap)
 		fe = dvb_attach(tua9001_attach, adap->fe[0], &d->i2c_adap,
 				&rtl2832u_tua9001_config);
 		break;
+	case TUNER_RTL2832_R820T:
+		fe = dvb_attach(r820t_attach, adap->fe[0], &d->i2c_adap,
+				&rtl2832u_r820t_config);
+
+		/* Use tuner to get the signal strength */
+		adap->fe[0]->ops.read_signal_strength =
+				adap->fe[0]->ops.tuner_ops.get_rf_strength;
+		break;
 	default:
 		fe = NULL;
 		dev_err(&d->udev->dev, "%s: unknown tuner=%d\n", KBUILD_MODNAME,
@@ -1372,6 +1406,8 @@ static const struct usb_device_id rtl28xxu_id_table[] = {
 		&rtl2832u_props, "Digivox Micro Hd", NULL) },
 	{ DVB_USB_DEVICE(USB_VID_COMPRO, 0x0620,
 		&rtl2832u_props, "Compro VideoMate U620F", NULL) },
+	{ DVB_USB_DEVICE(USB_VID_KWORLD_2, 0xd394,
+		&rtl2832u_props, "MaxMedia HU394-T", NULL) },
 	{ }
 };
 MODULE_DEVICE_TABLE(usb, rtl28xxu_id_table);
diff --git a/drivers/media/usb/dvb-usb-v2/rtl28xxu.h b/drivers/media/usb/dvb-usb-v2/rtl28xxu.h
index 2f3af2d3b6c..533a3312728 100644
--- a/drivers/media/usb/dvb-usb-v2/rtl28xxu.h
+++ b/drivers/media/usb/dvb-usb-v2/rtl28xxu.h
@@ -82,6 +82,7 @@ enum rtl28xxu_tuner {
 	TUNER_RTL2832_E4000,
 	TUNER_RTL2832_TDA18272,
 	TUNER_RTL2832_FC0013,
+	TUNER_RTL2832_R820T,
 };
 
 struct rtl28xxu_req {
diff --git a/drivers/media/usb/dvb-usb-v2/usb_urb.c b/drivers/media/usb/dvb-usb-v2/usb_urb.c
index 7346f85f3f2..ca8f3c2b108 100644
--- a/drivers/media/usb/dvb-usb-v2/usb_urb.c
+++ b/drivers/media/usb/dvb-usb-v2/usb_urb.c
@@ -22,8 +22,8 @@ static void usb_urb_complete(struct urb *urb)
 	int i;
 	u8 *b;
 
-	dev_dbg_ratelimited(&stream->udev->dev, "%s: %s urb completed " \
-			"status=%d length=%d/%d pack_num=%d errors=%d\n",
+	dev_dbg_ratelimited(&stream->udev->dev,
+			"%s: %s urb completed status=%d length=%d/%d pack_num=%d errors=%d\n",
 			__func__, ptype == PIPE_ISOCHRONOUS ? "isoc" : "bulk",
 			urb->status, urb->actual_length,
 			urb->transfer_buffer_length,
@@ -49,8 +49,8 @@ static void usb_urb_complete(struct urb *urb)
 	case PIPE_ISOCHRONOUS:
 		for (i = 0; i < urb->number_of_packets; i++) {
 			if (urb->iso_frame_desc[i].status != 0)
-				dev_dbg(&stream->udev->dev, "%s: iso frame " \
-						"descriptor has an error=%d\n",
+				dev_dbg(&stream->udev->dev,
+						"%s: iso frame descriptor has an error=%d\n",
 						__func__,
 						urb->iso_frame_desc[i].status);
 			else if (urb->iso_frame_desc[i].actual_length > 0)
@@ -67,8 +67,9 @@ static void usb_urb_complete(struct urb *urb)
 			stream->complete(stream, b, urb->actual_length);
 		break;
 	default:
-		dev_err(&stream->udev->dev, "%s: unknown endpoint type in " \
-				"completition handler\n", KBUILD_MODNAME);
+		dev_err(&stream->udev->dev,
+				"%s: unknown endpoint type in completition handler\n",
+				KBUILD_MODNAME);
 		return;
 	}
 	usb_submit_urb(urb, GFP_ATOMIC);
@@ -101,8 +102,8 @@ int usb_urb_submitv2(struct usb_data_stream *stream,
 		dev_dbg(&stream->udev->dev, "%s: submit urb=%d\n", __func__, i);
 		ret = usb_submit_urb(stream->urb_list[i], GFP_ATOMIC);
 		if (ret) {
-			dev_err(&stream->udev->dev, "%s: could not submit " \
-					"urb no. %d - get them all back\n",
+			dev_err(&stream->udev->dev,
+					"%s: could not submit urb no. %d - get them all back\n",
 					KBUILD_MODNAME, i);
 			usb_urb_killv2(stream);
 			return ret;
@@ -229,8 +230,9 @@ static int usb_alloc_stream_buffers(struct usb_data_stream *stream, int num,
 	stream->buf_num = 0;
 	stream->buf_size = size;
 
-	dev_dbg(&stream->udev->dev, "%s: all in all I will use %lu bytes for " \
-			"streaming\n", __func__,  num * size);
+	dev_dbg(&stream->udev->dev,
+			"%s: all in all I will use %lu bytes for streaming\n",
+			__func__,  num * size);
 
 	for (stream->buf_num = 0; stream->buf_num < num; stream->buf_num++) {
 		stream->buf_list[stream->buf_num] = usb_alloc_coherent(
@@ -274,8 +276,8 @@ int usb_urb_reconfig(struct usb_data_stream *stream,
 	}
 
 	if (stream->buf_num < props->count || stream->buf_size < buf_size) {
-		dev_err(&stream->udev->dev, "%s: cannot reconfigure as " \
-				"allocated buffers are too small\n",
+		dev_err(&stream->udev->dev,
+				"%s: cannot reconfigure as allocated buffers are too small\n",
 				KBUILD_MODNAME);
 		return -EINVAL;
 	}
@@ -321,8 +323,9 @@ int usb_urb_initv2(struct usb_data_stream *stream,
 	memcpy(&stream->props, props, sizeof(*props));
 
 	if (!stream->complete) {
-		dev_err(&stream->udev->dev, "%s: there is no data callback - " \
-				"this doesn't make sense\n", KBUILD_MODNAME);
+		dev_err(&stream->udev->dev,
+				"%s: there is no data callback - this doesn't make sense\n",
+				KBUILD_MODNAME);
 		return -EINVAL;
 	}
 
@@ -343,8 +346,9 @@ int usb_urb_initv2(struct usb_data_stream *stream,
 
 		return usb_urb_alloc_isoc_urbs(stream);
 	default:
-		dev_err(&stream->udev->dev, "%s: unknown urb-type for data " \
-				"transfer\n", KBUILD_MODNAME);
+		dev_err(&stream->udev->dev,
+				"%s: unknown urb-type for data transfer\n",
+				KBUILD_MODNAME);
 		return -EINVAL;
 	}
 }
diff --git a/drivers/media/usb/dvb-usb/cinergyT2-fe.c b/drivers/media/usb/dvb-usb/cinergyT2-fe.c
index 1efc028a76c..c890fe46acd 100644
--- a/drivers/media/usb/dvb-usb/cinergyT2-fe.c
+++ b/drivers/media/usb/dvb-usb/cinergyT2-fe.c
@@ -300,8 +300,7 @@ static int cinergyt2_fe_set_frontend(struct dvb_frontend *fe)
 static void cinergyt2_fe_release(struct dvb_frontend *fe)
 {
 	struct cinergyt2_fe_state *state = fe->demodulator_priv;
-	if (state != NULL)
-		kfree(state);
+	kfree(state);
 }
 
 static struct dvb_frontend_ops cinergyt2_fe_ops;
diff --git a/drivers/media/usb/dvb-usb/dib0700_devices.c b/drivers/media/usb/dvb-usb/dib0700_devices.c
index 11798426fa8..f08136052f9 100644
--- a/drivers/media/usb/dvb-usb/dib0700_devices.c
+++ b/drivers/media/usb/dvb-usb/dib0700_devices.c
@@ -1431,13 +1431,22 @@ static int dib8090_get_adc_power(struct dvb_frontend *fe)
 	return dib8000_get_adc_power(fe, 1);
 }
 
+static void dib8090_agc_control(struct dvb_frontend *fe, u8 restart)
+{
+	deb_info("AGC control callback: %i\n", restart);
+	dib0090_dcc_freq(fe, restart);
+
+	if (restart == 0) /* before AGC startup */
+		dib0090_set_dc_servo(fe, 1);
+}
+
 static struct dib8000_config dib809x_dib8000_config[2] = {
 	{
 	.output_mpeg2_in_188_bytes = 1,
 
 	.agc_config_count = 2,
 	.agc = dib8090_agc_config,
-	.agc_control = dib0090_dcc_freq,
+	.agc_control = dib8090_agc_control,
 	.pll = &dib8090_pll_config_12mhz,
 	.tuner_is_baseband = 1,
 
@@ -1456,7 +1465,7 @@ static struct dib8000_config dib809x_dib8000_config[2] = {
 
 	.agc_config_count = 2,
 	.agc = dib8090_agc_config,
-	.agc_control = dib0090_dcc_freq,
+	.agc_control = dib8090_agc_control,
 	.pll = &dib8090_pll_config_12mhz,
 	.tuner_is_baseband = 1,
 
@@ -1504,28 +1513,89 @@ static struct dib0090_config dib809x_dib0090_config = {
 	.fref_clock_ratio = 6,
 };
 
+static u8 dib8090_compute_pll_parameters(struct dvb_frontend *fe)
+{
+	u8 optimal_pll_ratio = 20;
+	u32 freq_adc, ratio, rest, max = 0;
+	u8 pll_ratio;
+
+	for (pll_ratio = 17; pll_ratio <= 20; pll_ratio++) {
+		freq_adc = 12 * pll_ratio * (1 << 8) / 16;
+		ratio = ((fe->dtv_property_cache.frequency / 1000) * (1 << 8) / 1000) / freq_adc;
+		rest = ((fe->dtv_property_cache.frequency / 1000) * (1 << 8) / 1000) - ratio * freq_adc;
+
+		if (rest > freq_adc / 2)
+			rest = freq_adc - rest;
+		deb_info("PLL ratio=%i rest=%i\n", pll_ratio, rest);
+		if ((rest > max) && (rest > 717)) {
+			optimal_pll_ratio = pll_ratio;
+			max = rest;
+		}
+	}
+	deb_info("optimal PLL ratio=%i\n", optimal_pll_ratio);
+
+	return optimal_pll_ratio;
+}
+
 static int dib8096_set_param_override(struct dvb_frontend *fe)
 {
-	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
 	struct dvb_usb_adapter *adap = fe->dvb->priv;
 	struct dib0700_adapter_state *state = adap->priv;
-	u8 band = BAND_OF_FREQUENCY(p->frequency/1000);
-	u16 target;
+	u8 pll_ratio, band = BAND_OF_FREQUENCY(fe->dtv_property_cache.frequency / 1000);
+	u16 target, ltgain, rf_gain_limit;
+	u32 timf;
 	int ret = 0;
 	enum frontend_tune_state tune_state = CT_SHUTDOWN;
-	u16 ltgain, rf_gain_limit;
+
+	switch (band) {
+	default:
+			deb_info("Warning : Rf frequency  (%iHz) is not in the supported range, using VHF switch ", fe->dtv_property_cache.frequency);
+	case BAND_VHF:
+			dib8000_set_gpio(fe, 3, 0, 1);
+			break;
+	case BAND_UHF:
+			dib8000_set_gpio(fe, 3, 0, 0);
+			break;
+	}
 
 	ret = state->set_param_save(fe);
 	if (ret < 0)
 		return ret;
 
-	target = (dib0090_get_wbd_target(fe) * 8 * 18 / 33 + 1) / 2;
-	dib8000_set_wbd_ref(fe, target);
+	if (fe->dtv_property_cache.bandwidth_hz != 6000000) {
+		deb_info("only 6MHz bandwidth is supported\n");
+		return -EINVAL;
+	}
+
+	/** Update PLL if needed ratio **/
+	dib8000_update_pll(fe, &dib8090_pll_config_12mhz, fe->dtv_property_cache.bandwidth_hz / 1000, 0);
+
+	/** Get optimize PLL ratio to remove spurious **/
+	pll_ratio = dib8090_compute_pll_parameters(fe);
+	if (pll_ratio == 17)
+		timf = 21387946;
+	else if (pll_ratio == 18)
+		timf = 20199727;
+	else if (pll_ratio == 19)
+		timf = 19136583;
+	else
+		timf = 18179756;
+
+	/** Update ratio **/
+	dib8000_update_pll(fe, &dib8090_pll_config_12mhz, fe->dtv_property_cache.bandwidth_hz / 1000, pll_ratio);
 
+	dib8000_ctrl_timf(fe, DEMOD_TIMF_SET, timf);
+
+	if (band != BAND_CBAND) {
+		/* dib0090_get_wbd_target is returning any possible temperature compensated wbd-target */
+		target = (dib0090_get_wbd_target(fe) * 8 * 18 / 33 + 1) / 2;
+		dib8000_set_wbd_ref(fe, target);
+	}
 
 	if (band == BAND_CBAND) {
 		deb_info("tuning in CBAND - soft-AGC startup\n");
 		dib0090_set_tune_state(fe, CT_AGC_START);
+
 		do {
 			ret = dib0090_gain_control(fe);
 			msleep(ret);
@@ -1534,14 +1604,17 @@ static int dib8096_set_param_override(struct dvb_frontend *fe)
 				dib8000_set_gpio(fe, 6, 0, 1);
 			else if (tune_state == CT_AGC_STEP_1) {
 				dib0090_get_current_gain(fe, NULL, NULL, &rf_gain_limit, &ltgain);
-				if (rf_gain_limit == 0)
+				if (rf_gain_limit < 2000) /* activate the external attenuator in case of very high input power */
 					dib8000_set_gpio(fe, 6, 0, 0);
 			}
 		} while (tune_state < CT_AGC_STOP);
+
+		deb_info("switching to PWM AGC\n");
 		dib0090_pwm_gain_reset(fe);
 		dib8000_pwm_agc_reset(fe);
 		dib8000_set_tune_state(fe, CT_DEMOD_START);
 	} else {
+		/* for everything else than CBAND we are using standard AGC */
 		deb_info("not tuning in CBAND - standard AGC startup\n");
 		dib0090_pwm_gain_reset(fe);
 	}
@@ -1814,21 +1887,92 @@ struct dibx090p_adc {
 	u32 pll_prediv;		/* New loopdiv */
 };
 
-struct dibx090p_adc dib8090p_adc_tab[] = {
-	{ 50000, 17043521, 16, 3}, /* 64 MHz */
-	{878000, 20199729, 9, 1}, /* 60 MHz */
-	{0xffffffff, 0, 0, 0}, /* 60 MHz */
+struct dibx090p_best_adc {
+	u32 timf;
+	u32 pll_loopdiv;
+	u32 pll_prediv;
 };
 
+static int dib8096p_get_best_sampling(struct dvb_frontend *fe, struct dibx090p_best_adc *adc)
+{
+	u8 spur = 0, prediv = 0, loopdiv = 0, min_prediv = 1, max_prediv = 1;
+	u16 xtal = 12000;
+	u16 fcp_min = 1900;  /* PLL, Minimum Frequency of phase comparator (KHz) */
+	u16 fcp_max = 20000; /* PLL, Maximum Frequency of phase comparator (KHz) */
+	u32 fmem_max = 140000; /* 140MHz max SDRAM freq */
+	u32 fdem_min = 66000;
+	u32 fcp = 0, fs = 0, fdem = 0, fmem = 0;
+	u32 harmonic_id = 0;
+
+	adc->timf = 0;
+	adc->pll_loopdiv = loopdiv;
+	adc->pll_prediv = prediv;
+
+	deb_info("bandwidth = %d", fe->dtv_property_cache.bandwidth_hz);
+
+	/* Find Min and Max prediv */
+	while ((xtal / max_prediv) >= fcp_min)
+		max_prediv++;
+
+	max_prediv--;
+	min_prediv = max_prediv;
+	while ((xtal / min_prediv) <= fcp_max) {
+		min_prediv--;
+		if (min_prediv == 1)
+			break;
+	}
+	deb_info("MIN prediv = %d : MAX prediv = %d", min_prediv, max_prediv);
+
+	min_prediv = 1;
+
+	for (prediv = min_prediv; prediv < max_prediv; prediv++) {
+		fcp = xtal / prediv;
+		if (fcp > fcp_min && fcp < fcp_max) {
+			for (loopdiv = 1; loopdiv < 64; loopdiv++) {
+				fmem = ((xtal/prediv) * loopdiv);
+				fdem = fmem / 2;
+				fs   = fdem / 4;
+
+				/* test min/max system restrictions */
+				if ((fdem >= fdem_min) && (fmem <= fmem_max) && (fs >= fe->dtv_property_cache.bandwidth_hz / 1000)) {
+					spur = 0;
+					/* test fs harmonics positions */
+					for (harmonic_id = (fe->dtv_property_cache.frequency / (1000 * fs));  harmonic_id <= ((fe->dtv_property_cache.frequency / (1000 * fs)) + 1); harmonic_id++) {
+						if (((fs * harmonic_id) >= (fe->dtv_property_cache.frequency / 1000 - (fe->dtv_property_cache.bandwidth_hz / 2000))) &&  ((fs * harmonic_id) <= (fe->dtv_property_cache.frequency / 1000 + (fe->dtv_property_cache.bandwidth_hz / 2000)))) {
+							spur = 1;
+							break;
+						}
+					}
+
+					if (!spur) {
+						adc->pll_loopdiv = loopdiv;
+						adc->pll_prediv = prediv;
+						adc->timf = (4260880253U / fdem) * (1 << 8);
+						adc->timf += ((4260880253U % fdem) << 8) / fdem;
+
+						deb_info("RF %6d; BW %6d; Xtal %6d; Fmem %6d; Fdem %6d; Fs %6d; Prediv %2d; Loopdiv %2d; Timf %8d;", fe->dtv_property_cache.frequency, fe->dtv_property_cache.bandwidth_hz, xtal, fmem, fdem, fs, prediv, loopdiv, adc->timf);
+						break;
+					}
+				}
+			}
+		}
+		if (!spur)
+			break;
+	}
+
+	if (adc->pll_loopdiv == 0 && adc->pll_prediv == 0)
+		return -EINVAL;
+	return 0;
+}
+
 static int dib8096p_agc_startup(struct dvb_frontend *fe)
 {
-	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
 	struct dvb_usb_adapter *adap = fe->dvb->priv;
 	struct dib0700_adapter_state *state = adap->priv;
 	struct dibx000_bandwidth_config pll;
+	struct dibx090p_best_adc adc;
 	u16 target;
-	int better_sampling_freq = 0, ret;
-	struct dibx090p_adc *adc_table = &dib8090p_adc_tab[0];
+	int ret;
 
 	ret = state->set_param_save(fe);
 	if (ret < 0)
@@ -1841,23 +1985,27 @@ static int dib8096p_agc_startup(struct dvb_frontend *fe)
 	target = (dib0090_get_wbd_target(fe) * 8  + 1) / 2;
 	dib8000_set_wbd_ref(fe, target);
 
+	if (dib8096p_get_best_sampling(fe, &adc) == 0) {
+		pll.pll_ratio  = adc.pll_loopdiv;
+		pll.pll_prediv = adc.pll_prediv;
 
-	while (p->frequency / 1000 > adc_table->freq) {
-		better_sampling_freq = 1;
-		adc_table++;
-	}
-
-	if ((adc_table->freq != 0xffffffff) && better_sampling_freq) {
-		pll.pll_ratio  = adc_table->pll_loopdiv;
-		pll.pll_prediv = adc_table->pll_prediv;
-		dib8000_update_pll(fe, &pll);
-		dib8000_ctrl_timf(fe, DEMOD_TIMF_SET, adc_table->timf);
+		dib0700_set_i2c_speed(adap->dev, 200);
+		dib8000_update_pll(fe, &pll, fe->dtv_property_cache.bandwidth_hz / 1000, 0);
+		dib8000_ctrl_timf(fe, DEMOD_TIMF_SET, adc.timf);
+		dib0700_set_i2c_speed(adap->dev, 1000);
 	}
 	return 0;
 }
 
 static int tfe8096p_frontend_attach(struct dvb_usb_adapter *adap)
 {
+	struct dib0700_state *st = adap->dev->priv;
+	u32 fw_version;
+
+	dib0700_get_version(adap->dev, NULL, NULL, &fw_version, NULL);
+	if (fw_version >= 0x10200)
+		st->fw_use_new_i2c_api = 1;
+
 	dib0700_set_gpio(adap->dev, GPIO6, GPIO_OUT, 1);
 	msleep(20);
 	dib0700_set_gpio(adap->dev, GPIO9, GPIO_OUT, 1);
@@ -2242,13 +2390,7 @@ static int nim9090md_tuner_attach(struct dvb_usb_adapter *adap)
 }
 
 /* NIM7090 */
-struct dib7090p_best_adc {
-	u32 timf;
-	u32 pll_loopdiv;
-	u32 pll_prediv;
-};
-
-static int dib7090p_get_best_sampling(struct dvb_frontend *fe , struct dib7090p_best_adc *adc)
+static int dib7090p_get_best_sampling(struct dvb_frontend *fe , struct dibx090p_best_adc *adc)
 {
 	u8 spur = 0, prediv = 0, loopdiv = 0, min_prediv = 1, max_prediv = 1;
 
@@ -2327,7 +2469,7 @@ static int dib7090_agc_startup(struct dvb_frontend *fe)
 	struct dib0700_adapter_state *state = adap->priv;
 	struct dibx000_bandwidth_config pll;
 	u16 target;
-	struct dib7090p_best_adc adc;
+	struct dibx090p_best_adc adc;
 	int ret;
 
 	ret = state->set_param_save(fe);
@@ -2357,36 +2499,16 @@ static int dib7090_agc_restart(struct dvb_frontend *fe, u8 restart)
 	return 0;
 }
 
-static int dib7090e_update_lna(struct dvb_frontend *fe, u16 agc_global)
+static int tfe7790p_update_lna(struct dvb_frontend *fe, u16 agc_global)
 {
-	u16 agc1 = 0, agc2, wbd = 0, wbd_target, wbd_offset, threshold_agc1;
-	s16 wbd_delta;
+	deb_info("update LNA: agc global=%i", agc_global);
 
-	if ((fe->dtv_property_cache.frequency) < 400000000)
-		threshold_agc1 = 25000;
-	else
-		threshold_agc1 = 30000;
-
-	wbd_target = (dib0090_get_wbd_target(fe)*8+1)/2;
-	wbd_offset = dib0090_get_wbd_offset(fe);
-	dib7000p_get_agc_values(fe, NULL, &agc1, &agc2, &wbd);
-	wbd_delta = (s16)wbd - (((s16)wbd_offset+10)*4) ;
-
-	deb_info("update lna, agc_global=%d agc1=%d agc2=%d",
-			agc_global, agc1, agc2);
-	deb_info("update lna, wbd=%d wbd target=%d wbd offset=%d wbd delta=%d",
-			wbd, wbd_target, wbd_offset, wbd_delta);
-
-	if ((agc1 < threshold_agc1) && (wbd_delta > 0)) {
-		dib0090_set_switch(fe, 1, 1, 1);
-		dib0090_set_vga(fe, 0);
-		dib0090_update_rframp_7090(fe, 0);
-		dib0090_update_tuning_table_7090(fe, 0);
+	if (agc_global < 25000) {
+		dib7000p_set_gpio(fe, 8, 0, 0);
+		dib7000p_set_agc1_min(fe, 0);
 	} else {
-		dib0090_set_vga(fe, 1);
-		dib0090_update_rframp_7090(fe, 1);
-		dib0090_update_tuning_table_7090(fe, 1);
-		dib0090_set_switch(fe, 0, 0, 0);
+		dib7000p_set_gpio(fe, 8, 0, 1);
+		dib7000p_set_agc1_min(fe, 32768);
 	}
 
 	return 0;
@@ -2400,15 +2522,6 @@ static struct dib0090_wbd_slope dib7090_wbd_table[] = {
 	{ 0xFFFF, 0,   0, 0,   0,   0},
 };
 
-static struct dib0090_wbd_slope dib7090e_wbd_table[] = {
-	{ 380,   81, 850, 64, 540,	4},
-	{ 700,   51, 866, 21,  320,	4},
-	{ 860,   48, 666, 18,  330,	6},
-	{1700,    0, 250, 0,   100, 6},
-	{2600,    0, 250, 0,   100, 6},
-	{ 0xFFFF, 0,   0, 0,   0,	0},
-};
-
 static struct dibx000_agc_config dib7090_agc_config[2] = {
 	{
 		.band_caps      = BAND_UHF,
@@ -2428,7 +2541,7 @@ static struct dibx000_agc_config dib7090_agc_config[2] = {
 		.wbd_alpha      = 5,
 
 		.agc1_max       = 65535,
-		.agc1_min       = 0,
+		.agc1_min       = 32768,
 
 		.agc2_max       = 65535,
 		.agc2_min       = 0,
@@ -2505,7 +2618,7 @@ static struct dib7000p_config nim7090_dib7000p_config = {
 	.output_mpeg2_in_188_bytes  = 1,
 	.hostbus_diversity			= 1,
 	.tuner_is_baseband			= 1,
-	.update_lna					= NULL,
+	.update_lna					= tfe7790p_update_lna, /* GPIO used is the same as TFE7790 */
 
 	.agc_config_count			= 2,
 	.agc						= dib7090_agc_config,
@@ -2529,12 +2642,26 @@ static struct dib7000p_config nim7090_dib7000p_config = {
 	.enMpegOutput				= 1,
 };
 
+static int tfe7090p_pvr_update_lna(struct dvb_frontend *fe, u16 agc_global)
+{
+	deb_info("TFE7090P-PVR update LNA: agc global=%i", agc_global);
+	if (agc_global < 25000) {
+		dib7000p_set_gpio(fe, 5, 0, 0);
+		dib7000p_set_agc1_min(fe, 0);
+	} else {
+		dib7000p_set_gpio(fe, 5, 0, 1);
+		dib7000p_set_agc1_min(fe, 32768);
+	}
+
+	return 0;
+}
+
 static struct dib7000p_config tfe7090pvr_dib7000p_config[2] = {
 	{
 		.output_mpeg2_in_188_bytes  = 1,
 		.hostbus_diversity			= 1,
 		.tuner_is_baseband			= 1,
-		.update_lna					= NULL,
+		.update_lna					= tfe7090p_pvr_update_lna,
 
 		.agc_config_count			= 2,
 		.agc						= dib7090_agc_config,
@@ -2561,7 +2688,7 @@ static struct dib7000p_config tfe7090pvr_dib7000p_config[2] = {
 		.output_mpeg2_in_188_bytes  = 1,
 		.hostbus_diversity			= 1,
 		.tuner_is_baseband			= 1,
-		.update_lna					= NULL,
+		.update_lna					= tfe7090p_pvr_update_lna,
 
 		.agc_config_count			= 2,
 		.agc						= dib7090_agc_config,
@@ -2587,34 +2714,6 @@ static struct dib7000p_config tfe7090pvr_dib7000p_config[2] = {
 	}
 };
 
-static struct dib7000p_config tfe7090e_dib7000p_config = {
-	.output_mpeg2_in_188_bytes  = 1,
-	.hostbus_diversity			= 1,
-	.tuner_is_baseband			= 1,
-	.update_lna					= dib7090e_update_lna,
-
-	.agc_config_count			= 2,
-	.agc						= dib7090_agc_config,
-
-	.bw							= &dib7090_clock_config_12_mhz,
-
-	.gpio_dir					= DIB7000P_GPIO_DEFAULT_DIRECTIONS,
-	.gpio_val					= DIB7000P_GPIO_DEFAULT_VALUES,
-	.gpio_pwm_pos				= DIB7000P_GPIO_DEFAULT_PWM_POS,
-
-	.pwm_freq_div				= 0,
-
-	.agc_control				= dib7090_agc_restart,
-
-	.spur_protect				= 0,
-	.disable_sample_and_hold	= 0,
-	.enable_current_mirror		= 0,
-	.diversity_delay			= 0,
-
-	.output_mode				= OUTMODE_MPEG2_FIFO,
-	.enMpegOutput				= 1,
-};
-
 static const struct dib0090_config nim7090_dib0090_config = {
 	.io.clock_khz = 12000,
 	.io.pll_bypass = 0,
@@ -2649,47 +2748,11 @@ static const struct dib0090_config nim7090_dib0090_config = {
 	.in_soc = 1,
 };
 
-static const struct dib0090_config tfe7090e_dib0090_config = {
-	.io.clock_khz = 12000,
-	.io.pll_bypass = 0,
-	.io.pll_range = 0,
-	.io.pll_prediv = 3,
-	.io.pll_loopdiv = 6,
-	.io.adc_clock_ratio = 0,
-	.io.pll_int_loop_filt = 0,
-	.reset = dib7090_tuner_sleep,
-	.sleep = dib7090_tuner_sleep,
-
-	.freq_offset_khz_uhf = 0,
-	.freq_offset_khz_vhf = 0,
-
-	.get_adc_power = dib7090_get_adc_power,
-
-	.clkouttobamse = 1,
-	.analog_output = 0,
-
-	.wbd_vhf_offset = 0,
-	.wbd_cband_offset = 0,
-	.use_pwm_agc = 1,
-	.clkoutdrive = 0,
-
-	.fref_clock_ratio = 0,
-
-	.wbd = dib7090e_wbd_table,
-
-	.ls_cfg_pad_drv = 0,
-	.data_tx_drv = 0,
-	.low_if = NULL,
-	.in_soc = 1,
-	.force_cband_input = 1,
-	.is_dib7090e = 1,
-};
-
-static struct dib7000p_config tfe7790e_dib7000p_config = {
+static struct dib7000p_config tfe7790p_dib7000p_config = {
 	.output_mpeg2_in_188_bytes  = 1,
 	.hostbus_diversity			= 1,
 	.tuner_is_baseband			= 1,
-	.update_lna					= dib7090e_update_lna,
+	.update_lna					= tfe7790p_update_lna,
 
 	.agc_config_count			= 2,
 	.agc						= dib7090_agc_config,
@@ -2713,7 +2776,7 @@ static struct dib7000p_config tfe7790e_dib7000p_config = {
 	.enMpegOutput				= 1,
 };
 
-static const struct dib0090_config tfe7790e_dib0090_config = {
+static const struct dib0090_config tfe7790p_dib0090_config = {
 	.io.clock_khz = 12000,
 	.io.pll_bypass = 0,
 	.io.pll_range = 0,
@@ -2739,14 +2802,14 @@ static const struct dib0090_config tfe7790e_dib0090_config = {
 
 	.fref_clock_ratio = 0,
 
-	.wbd = dib7090e_wbd_table,
+	.wbd = dib7090_wbd_table,
 
 	.ls_cfg_pad_drv = 0,
 	.data_tx_drv = 0,
 	.low_if = NULL,
 	.in_soc = 1,
-	.force_cband_input = 1,
-	.is_dib7090e = 1,
+	.force_cband_input = 0,
+	.is_dib7090e = 0,
 	.force_crystal_mode = 1,
 };
 
@@ -2942,37 +3005,11 @@ static int tfe7090pvr_tuner1_attach(struct dvb_usb_adapter *adap)
 	return 0;
 }
 
-static int tfe7090e_frontend_attach(struct dvb_usb_adapter *adap)
-{
-	dib0700_set_gpio(adap->dev, GPIO6, GPIO_OUT, 1);
-	msleep(20);
-	dib0700_set_gpio(adap->dev, GPIO9, GPIO_OUT, 1);
-	dib0700_set_gpio(adap->dev, GPIO4, GPIO_OUT, 1);
-	dib0700_set_gpio(adap->dev, GPIO7, GPIO_OUT, 1);
-	dib0700_set_gpio(adap->dev, GPIO10, GPIO_OUT, 0);
-
-	msleep(20);
-	dib0700_set_gpio(adap->dev, GPIO10, GPIO_OUT, 1);
-	msleep(20);
-	dib0700_set_gpio(adap->dev, GPIO0, GPIO_OUT, 1);
-
-	if (dib7000p_i2c_enumeration(&adap->dev->i2c_adap,
-				1, 0x10, &tfe7090e_dib7000p_config) != 0) {
-		err("%s: dib7000p_i2c_enumeration failed.  Cannot continue\n",
-				__func__);
-		return -ENODEV;
-	}
-	adap->fe_adap[0].fe = dvb_attach(dib7000p_attach, &adap->dev->i2c_adap,
-			0x80, &tfe7090e_dib7000p_config);
-
-	return adap->fe_adap[0].fe == NULL ?  -ENODEV : 0;
-}
-
-static int tfe7790e_frontend_attach(struct dvb_usb_adapter *adap)
+static int tfe7790p_frontend_attach(struct dvb_usb_adapter *adap)
 {
 	struct dib0700_state *st = adap->dev->priv;
 
-	/* The TFE7790E requires the dib0700 to not be in master mode */
+	/* The TFE7790P requires the dib0700 to not be in master mode */
 	st->disable_streaming_master_mode = 1;
 
 	dib0700_set_gpio(adap->dev, GPIO6, GPIO_OUT, 1);
@@ -2988,42 +3025,25 @@ static int tfe7790e_frontend_attach(struct dvb_usb_adapter *adap)
 	dib0700_set_gpio(adap->dev, GPIO0, GPIO_OUT, 1);
 
 	if (dib7000p_i2c_enumeration(&adap->dev->i2c_adap,
-				1, 0x10, &tfe7790e_dib7000p_config) != 0) {
+				1, 0x10, &tfe7790p_dib7000p_config) != 0) {
 		err("%s: dib7000p_i2c_enumeration failed.  Cannot continue\n",
 				__func__);
 		return -ENODEV;
 	}
 	adap->fe_adap[0].fe = dvb_attach(dib7000p_attach, &adap->dev->i2c_adap,
-			0x80, &tfe7790e_dib7000p_config);
+			0x80, &tfe7790p_dib7000p_config);
 
 	return adap->fe_adap[0].fe == NULL ?  -ENODEV : 0;
 }
 
-static int tfe7790e_tuner_attach(struct dvb_usb_adapter *adap)
-{
-	struct dib0700_adapter_state *st = adap->priv;
-	struct i2c_adapter *tun_i2c =
-		dib7090_get_i2c_tuner(adap->fe_adap[0].fe);
-
-	if (dvb_attach(dib0090_register, adap->fe_adap[0].fe, tun_i2c,
-				&tfe7790e_dib0090_config) == NULL)
-		return -ENODEV;
-
-	dib7000p_set_gpio(adap->fe_adap[0].fe, 8, 0, 1);
-
-	st->set_param_save = adap->fe_adap[0].fe->ops.tuner_ops.set_params;
-	adap->fe_adap[0].fe->ops.tuner_ops.set_params = dib7090_agc_startup;
-	return 0;
-}
-
-static int tfe7090e_tuner_attach(struct dvb_usb_adapter *adap)
+static int tfe7790p_tuner_attach(struct dvb_usb_adapter *adap)
 {
 	struct dib0700_adapter_state *st = adap->priv;
 	struct i2c_adapter *tun_i2c =
 		dib7090_get_i2c_tuner(adap->fe_adap[0].fe);
 
 	if (dvb_attach(dib0090_register, adap->fe_adap[0].fe, tun_i2c,
-				&tfe7090e_dib0090_config) == NULL)
+				&tfe7790p_dib0090_config) == NULL)
 		return -ENODEV;
 
 	dib7000p_set_gpio(adap->fe_adap[0].fe, 8, 0, 1);
@@ -3566,10 +3586,9 @@ struct usb_device_id dib0700_usb_id_table[] = {
 /* 75 */{ USB_DEVICE(USB_VID_MEDION,    USB_PID_CREATIX_CTX1921) },
 	{ USB_DEVICE(USB_VID_PINNACLE,  USB_PID_PINNACLE_PCTV340E) },
 	{ USB_DEVICE(USB_VID_PINNACLE,  USB_PID_PINNACLE_PCTV340E_SE) },
-	{ 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) },
+	{ USB_DEVICE(USB_VID_DIBCOM,    USB_PID_DIBCOM_TFE7790P) },
+	{ USB_DEVICE(USB_VID_DIBCOM,    USB_PID_DIBCOM_TFE8096P) },
+/* 80 */{ USB_DEVICE(USB_VID_ELGATO,	USB_PID_ELGATO_EYETV_DTT_2) },
 	{ 0 }		/* Terminating entry */
 };
 MODULE_DEVICE_TABLE(usb, dib0700_usb_id_table);
@@ -3880,7 +3899,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 				{ NULL },
 			},
 			{   "Elgato EyeTV DTT rev. 2",
-				{ &dib0700_usb_id_table[81], NULL },
+				{ &dib0700_usb_id_table[80], NULL },
 				{ NULL },
 			},
 		},
@@ -4697,48 +4716,8 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 					.pid_filter_count = 32,
 					.pid_filter = stk70x0p_pid_filter,
 					.pid_filter_ctrl = stk70x0p_pid_filter_ctrl,
-					.frontend_attach  = tfe7090e_frontend_attach,
-					.tuner_attach     = tfe7090e_tuner_attach,
-
-					DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
-				} },
-
-				.size_of_priv =
-					sizeof(struct dib0700_adapter_state),
-			},
-		},
-
-		.num_device_descs = 1,
-		.devices = {
-			{   "DiBcom TFE7090E reference design",
-				{ &dib0700_usb_id_table[78], NULL },
-				{ NULL },
-			},
-		},
-
-		.rc.core = {
-			.rc_interval      = DEFAULT_RC_INTERVAL,
-			.rc_codes         = RC_MAP_DIB0700_RC5_TABLE,
-			.module_name	  = "dib0700",
-			.rc_query         = dib0700_rc_query_old_firmware,
-			.allowed_protos   = RC_BIT_RC5 |
-					    RC_BIT_RC6_MCE |
-					    RC_BIT_NEC,
-			.change_protocol  = dib0700_change_protocol,
-		},
-	}, { DIB0700_DEFAULT_DEVICE_PROPERTIES,
-		.num_adapters = 1,
-		.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 = stk70x0p_pid_filter,
-					.pid_filter_ctrl = stk70x0p_pid_filter_ctrl,
-					.frontend_attach  = tfe7790e_frontend_attach,
-					.tuner_attach     = tfe7790e_tuner_attach,
+					.frontend_attach  = tfe7790p_frontend_attach,
+					.tuner_attach     = tfe7790p_tuner_attach,
 
 					DIB0700_DEFAULT_STREAMING_CONFIG(0x03),
 				} },
@@ -4750,8 +4729,8 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 
 		.num_device_descs = 1,
 		.devices = {
-			{   "DiBcom TFE7790E reference design",
-				{ &dib0700_usb_id_table[79], NULL },
+			{   "DiBcom TFE7790P reference design",
+				{ &dib0700_usb_id_table[78], NULL },
 				{ NULL },
 			},
 		},
@@ -4792,7 +4771,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 		.num_device_descs = 1,
 		.devices = {
 			{   "DiBcom TFE8096P reference design",
-				{ &dib0700_usb_id_table[80], NULL },
+				{ &dib0700_usb_id_table[79], NULL },
 				{ NULL },
 			},
 		},
diff --git a/drivers/media/usb/dvb-usb/dibusb-common.c b/drivers/media/usb/dvb-usb/dibusb-common.c
index af0d4321845..c2dded92f1d 100644
--- a/drivers/media/usb/dvb-usb/dibusb-common.c
+++ b/drivers/media/usb/dvb-usb/dibusb-common.c
@@ -8,6 +8,8 @@
  *
  * see Documentation/dvb/README.dvb-usb for more information
  */
+
+#include <linux/kconfig.h>
 #include "dibusb.h"
 
 static int debug;
@@ -232,8 +234,7 @@ static struct dibx000_agc_config dib3000p_panasonic_agc_config = {
 	.agc2_slope2 = 0x1e,
 };
 
-#if defined(CONFIG_DVB_DIB3000MC) || 					\
-	(defined(CONFIG_DVB_DIB3000MC_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_DIB3000MC)
 
 static struct dib3000mc_config mod3000p_dib3000p_config = {
 	&dib3000p_panasonic_agc_config,
diff --git a/drivers/media/usb/dvb-usb/dw2102.c b/drivers/media/usb/dvb-usb/dw2102.c
index 9578a6761f1..6e237b6dd0a 100644
--- a/drivers/media/usb/dvb-usb/dw2102.c
+++ b/drivers/media/usb/dvb-usb/dw2102.c
@@ -29,7 +29,6 @@
 #include "stb6100.h"
 #include "stb6100_proc.h"
 #include "m88rs2000.h"
-#include "ts2020.h"
 
 #ifndef USB_PID_DW2102
 #define USB_PID_DW2102 0x2102
@@ -79,6 +78,10 @@
 #define USB_PID_TEVII_S632 0xd632
 #endif
 
+#ifndef USB_PID_GOTVIEW_SAT_HD
+#define USB_PID_GOTVIEW_SAT_HD 0x5456
+#endif
+
 #define DW210X_READ_MSG 0
 #define DW210X_WRITE_MSG 1
 
@@ -1548,6 +1551,8 @@ enum dw2102_table_entry {
 	X3M_SPC1400HD,
 	TEVII_S421,
 	TEVII_S632,
+	TERRATEC_CINERGY_S2_R2,
+	GOTVIEW_SAT_HD,
 };
 
 static struct usb_device_id dw2102_table[] = {
@@ -1568,6 +1573,8 @@ static struct usb_device_id dw2102_table[] = {
 	[X3M_SPC1400HD] = {USB_DEVICE(0x1f4d, 0x3100)},
 	[TEVII_S421] = {USB_DEVICE(0x9022, USB_PID_TEVII_S421)},
 	[TEVII_S632] = {USB_DEVICE(0x9022, USB_PID_TEVII_S632)},
+	[TERRATEC_CINERGY_S2_R2] = {USB_DEVICE(USB_VID_TERRATEC, 0x00b0)},
+	[GOTVIEW_SAT_HD] = {USB_DEVICE(0x1FE1, USB_PID_GOTVIEW_SAT_HD)},
 	{ }
 };
 
@@ -1968,7 +1975,7 @@ static struct dvb_usb_device_properties su3000_properties = {
 		}},
 		}
 	},
-	.num_device_descs = 3,
+	.num_device_descs = 5,
 	.devices = {
 		{ "SU3000HD DVB-S USB2.0",
 			{ &dw2102_table[GENIATECH_SU3000], NULL },
@@ -1982,6 +1989,14 @@ static struct dvb_usb_device_properties su3000_properties = {
 			{ &dw2102_table[X3M_SPC1400HD], NULL },
 			{ NULL },
 		},
+		{ "Terratec Cinergy S2 USB HD Rev.2",
+			{ &dw2102_table[TERRATEC_CINERGY_S2_R2], NULL },
+			{ NULL },
+		},
+		{ "GOTVIEW Satellite HD",
+			{ &dw2102_table[GOTVIEW_SAT_HD], NULL },
+			{ NULL },
+		},
 	}
 };
 
diff --git a/drivers/media/usb/dvb-usb/m920x.c b/drivers/media/usb/dvb-usb/m920x.c
index 92afeb20650..c2b635d6a17 100644
--- a/drivers/media/usb/dvb-usb/m920x.c
+++ b/drivers/media/usb/dvb-usb/m920x.c
@@ -68,20 +68,20 @@ static inline int m920x_write_seq(struct usb_device *udev, u8 request,
 				  struct m920x_inits *seq)
 {
 	int ret;
-	while (seq->address) {
+	do {
 		ret = m920x_write(udev, request, seq->data, seq->address);
 		if (ret != 0)
 			return ret;
 
 		seq++;
-	}
+	} while (seq->address);
 
-	return ret;
+	return 0;
 }
 
 static int m920x_init(struct dvb_usb_device *d, struct m920x_inits *rc_seq)
 {
-	int ret = 0, i, epi, flags = 0;
+	int ret, i, epi, flags = 0;
 	int adap_enabled[M9206_MAX_ADAPTERS] = { 0 };
 
 	/* Remote controller init. */
@@ -124,7 +124,7 @@ static int m920x_init(struct dvb_usb_device *d, struct m920x_inits *rc_seq)
 		}
 	}
 
-	return ret;
+	return 0;
 }
 
 static int m920x_init_ep(struct usb_interface *intf)
diff --git a/drivers/media/usb/dvb-usb/opera1.c b/drivers/media/usb/dvb-usb/opera1.c
index c8a95042dfb..16ba90acf53 100644
--- a/drivers/media/usb/dvb-usb/opera1.c
+++ b/drivers/media/usb/dvb-usb/opera1.c
@@ -151,7 +151,7 @@ static int opera1_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 			break;
 		}
 		if (dvb_usb_opera1_debug & 0x10)
-			info("sending i2c mesage %d %d", tmp, msg[i].len);
+			info("sending i2c message %d %d", tmp, msg[i].len);
 	}
 	mutex_unlock(&d->i2c_mutex);
 	return num;
diff --git a/drivers/media/usb/em28xx/Kconfig b/drivers/media/usb/em28xx/Kconfig
index c754a80a8d8..ca5ee6aceb6 100644
--- a/drivers/media/usb/em28xx/Kconfig
+++ b/drivers/media/usb/em28xx/Kconfig
@@ -46,6 +46,7 @@ config VIDEO_EM28XX_DVB
 	select DVB_A8293 if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_MT352 if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_S5H1409 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_MB86A20S if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_QT1010 if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_TDA18271 if MEDIA_SUBDRV_AUTOSELECT
 	---help---
diff --git a/drivers/media/usb/em28xx/Makefile b/drivers/media/usb/em28xx/Makefile
index 634fb920dd3..ad6d4855794 100644
--- a/drivers/media/usb/em28xx/Makefile
+++ b/drivers/media/usb/em28xx/Makefile
@@ -1,5 +1,5 @@
 em28xx-y +=	em28xx-video.o em28xx-i2c.o em28xx-cards.o
-em28xx-y +=	em28xx-core.o  em28xx-vbi.o
+em28xx-y +=	em28xx-core.o  em28xx-vbi.o em28xx-camera.o
 
 em28xx-alsa-objs := em28xx-audio.o
 em28xx-rc-objs := em28xx-input.o
diff --git a/drivers/media/usb/em28xx/em28xx-camera.c b/drivers/media/usb/em28xx/em28xx-camera.c
new file mode 100644
index 00000000000..73cc50afa5e
--- /dev/null
+++ b/drivers/media/usb/em28xx/em28xx-camera.c
@@ -0,0 +1,434 @@
+/*
+   em28xx-camera.c - driver for Empia EM25xx/27xx/28xx USB video capture devices
+
+   Copyright (C) 2009 Mauro Carvalho Chehab <mchehab@infradead.org>
+   Copyright (C) 2013 Frank Schäfer <fschaefer.oss@googlemail.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., 675 Mass Ave, Cambridge, MA 02139, USA.
+*/
+
+#include <linux/i2c.h>
+#include <media/soc_camera.h>
+#include <media/mt9v011.h>
+#include <media/v4l2-common.h>
+
+#include "em28xx.h"
+
+
+/* Possible i2c addresses of Micron sensors */
+static unsigned short micron_sensor_addrs[] = {
+	0xb8 >> 1,   /* MT9V111, MT9V403 */
+	0xba >> 1,   /* MT9M001/011/111/112, MT9V011/012/112, MT9D011 */
+	0x90 >> 1,   /* MT9V012/112, MT9D011 (alternative address) */
+	I2C_CLIENT_END
+};
+
+/* Possible i2c addresses of Omnivision sensors */
+static unsigned short omnivision_sensor_addrs[] = {
+	0x42 >> 1,   /* OV7725, OV7670/60/48 */
+	0x60 >> 1,   /* OV2640, OV9650/53/55 */
+	I2C_CLIENT_END
+};
+
+
+static struct soc_camera_link camlink = {
+	.bus_id = 0,
+	.flags = 0,
+	.module_name = "em28xx",
+};
+
+
+/* FIXME: Should be replaced by a proper mt9m111 driver */
+static int em28xx_initialize_mt9m111(struct em28xx *dev)
+{
+	int i;
+	unsigned char regs[][3] = {
+		{ 0x0d, 0x00, 0x01, },  /* reset and use defaults */
+		{ 0x0d, 0x00, 0x00, },
+		{ 0x0a, 0x00, 0x21, },
+		{ 0x21, 0x04, 0x00, },  /* full readout speed, no row/col skipping */
+	};
+
+	for (i = 0; i < ARRAY_SIZE(regs); i++)
+		i2c_master_send(&dev->i2c_client[dev->def_i2c_bus],
+				&regs[i][0], 3);
+
+	return 0;
+}
+
+
+/* FIXME: Should be replaced by a proper mt9m001 driver */
+static int em28xx_initialize_mt9m001(struct em28xx *dev)
+{
+	int i;
+	unsigned char regs[][3] = {
+		{ 0x0d, 0x00, 0x01, },
+		{ 0x0d, 0x00, 0x00, },
+		{ 0x04, 0x05, 0x00, },	/* hres = 1280 */
+		{ 0x03, 0x04, 0x00, },  /* vres = 1024 */
+		{ 0x20, 0x11, 0x00, },
+		{ 0x06, 0x00, 0x10, },
+		{ 0x2b, 0x00, 0x24, },
+		{ 0x2e, 0x00, 0x24, },
+		{ 0x35, 0x00, 0x24, },
+		{ 0x2d, 0x00, 0x20, },
+		{ 0x2c, 0x00, 0x20, },
+		{ 0x09, 0x0a, 0xd4, },
+		{ 0x35, 0x00, 0x57, },
+	};
+
+	for (i = 0; i < ARRAY_SIZE(regs); i++)
+		i2c_master_send(&dev->i2c_client[dev->def_i2c_bus],
+				&regs[i][0], 3);
+
+	return 0;
+}
+
+
+/*
+ * Probes Micron sensors with 8 bit address and 16 bit register width
+ */
+static int em28xx_probe_sensor_micron(struct em28xx *dev)
+{
+	int ret, i;
+	char *name;
+	u8 reg;
+	__be16 id_be;
+	u16 id;
+
+	struct i2c_client client = dev->i2c_client[dev->def_i2c_bus];
+
+	dev->em28xx_sensor = EM28XX_NOSENSOR;
+	for (i = 0; micron_sensor_addrs[i] != I2C_CLIENT_END; i++) {
+		client.addr = micron_sensor_addrs[i];
+		/* NOTE: i2c_smbus_read_word_data() doesn't work with BE data */
+		/* Read chip ID from register 0x00 */
+		reg = 0x00;
+		ret = i2c_master_send(&client, &reg, 1);
+		if (ret < 0) {
+			if (ret != -ENODEV)
+				em28xx_errdev("couldn't read from i2c device 0x%02x: error %i\n",
+					      client.addr << 1, ret);
+			continue;
+		}
+		ret = i2c_master_recv(&client, (u8 *)&id_be, 2);
+		if (ret < 0) {
+			em28xx_errdev("couldn't read from i2c device 0x%02x: error %i\n",
+				      client.addr << 1, ret);
+			continue;
+		}
+		id = be16_to_cpu(id_be);
+		/* Read chip ID from register 0xff */
+		reg = 0xff;
+		ret = i2c_master_send(&client, &reg, 1);
+		if (ret < 0) {
+			em28xx_errdev("couldn't read from i2c device 0x%02x: error %i\n",
+				      client.addr << 1, ret);
+			continue;
+		}
+		ret = i2c_master_recv(&client, (u8 *)&id_be, 2);
+		if (ret < 0) {
+			em28xx_errdev("couldn't read from i2c device 0x%02x: error %i\n",
+				      client.addr << 1, ret);
+			continue;
+		}
+		/* Validate chip ID to be sure we have a Micron device */
+		if (id != be16_to_cpu(id_be))
+			continue;
+		/* Check chip ID */
+		id = be16_to_cpu(id_be);
+		switch (id) {
+		case 0x1222:
+			name = "MT9V012"; /* MI370 */ /* 640x480 */
+			break;
+		case 0x1229:
+			name = "MT9V112"; /* 640x480 */
+			break;
+		case 0x1433:
+			name = "MT9M011"; /* 1280x1024 */
+			break;
+		case 0x143a:    /* found in the ECS G200 */
+			name = "MT9M111"; /* MI1310 */ /* 1280x1024 */
+			dev->em28xx_sensor = EM28XX_MT9M111;
+			break;
+		case 0x148c:
+			name = "MT9M112"; /* MI1320 */ /* 1280x1024 */
+			break;
+		case 0x1511:
+			name = "MT9D011"; /* MI2010 */ /* 1600x1200 */
+			break;
+		case 0x8232:
+		case 0x8243:	/* rev B */
+			name = "MT9V011"; /* MI360 */ /* 640x480 */
+			dev->em28xx_sensor = EM28XX_MT9V011;
+			break;
+		case 0x8431:
+			name = "MT9M001"; /* 1280x1024 */
+			dev->em28xx_sensor = EM28XX_MT9M001;
+			break;
+		default:
+			em28xx_info("unknown Micron sensor detected: 0x%04x\n",
+				    id);
+			return 0;
+		}
+
+		if (dev->em28xx_sensor == EM28XX_NOSENSOR)
+			em28xx_info("unsupported sensor detected: %s\n", name);
+		else
+			em28xx_info("sensor %s detected\n", name);
+
+		dev->i2c_client[dev->def_i2c_bus].addr = client.addr;
+		return 0;
+	}
+
+	return -ENODEV;
+}
+
+/*
+ * Probes Omnivision sensors with 8 bit address and register width
+ */
+static int em28xx_probe_sensor_omnivision(struct em28xx *dev)
+{
+	int ret, i;
+	char *name;
+	u8 reg;
+	u16 id;
+	struct i2c_client client = dev->i2c_client[dev->def_i2c_bus];
+
+	dev->em28xx_sensor = EM28XX_NOSENSOR;
+	/* NOTE: these devices have the register auto incrementation disabled
+	 * by default, so we have to use single byte reads !              */
+	for (i = 0; omnivision_sensor_addrs[i] != I2C_CLIENT_END; i++) {
+		client.addr = omnivision_sensor_addrs[i];
+		/* Read manufacturer ID from registers 0x1c-0x1d (BE) */
+		reg = 0x1c;
+		ret = i2c_smbus_read_byte_data(&client, reg);
+		if (ret < 0) {
+			if (ret != -ENODEV)
+				em28xx_errdev("couldn't read from i2c device 0x%02x: error %i\n",
+					      client.addr << 1, ret);
+			continue;
+		}
+		id = ret << 8;
+		reg = 0x1d;
+		ret = i2c_smbus_read_byte_data(&client, reg);
+		if (ret < 0) {
+			em28xx_errdev("couldn't read from i2c device 0x%02x: error %i\n",
+				      client.addr << 1, ret);
+			continue;
+		}
+		id += ret;
+		/* Check manufacturer ID */
+		if (id != 0x7fa2)
+			continue;
+		/* Read product ID from registers 0x0a-0x0b (BE) */
+		reg = 0x0a;
+		ret = i2c_smbus_read_byte_data(&client, reg);
+		if (ret < 0) {
+			em28xx_errdev("couldn't read from i2c device 0x%02x: error %i\n",
+				      client.addr << 1, ret);
+			continue;
+		}
+		id = ret << 8;
+		reg = 0x0b;
+		ret = i2c_smbus_read_byte_data(&client, reg);
+		if (ret < 0) {
+			em28xx_errdev("couldn't read from i2c device 0x%02x: error %i\n",
+				      client.addr << 1, ret);
+			continue;
+		}
+		id += ret;
+		/* Check product ID */
+		switch (id) {
+		case 0x2642:
+			name = "OV2640";
+			dev->em28xx_sensor = EM28XX_OV2640;
+			break;
+		case 0x7648:
+			name = "OV7648";
+			break;
+		case 0x7660:
+			name = "OV7660";
+			break;
+		case 0x7673:
+			name = "OV7670";
+			break;
+		case 0x7720:
+			name = "OV7720";
+			break;
+		case 0x7721:
+			name = "OV7725";
+			break;
+		case 0x9648: /* Rev 2 */
+		case 0x9649: /* Rev 3 */
+			name = "OV9640";
+			break;
+		case 0x9650:
+		case 0x9652: /* OV9653 */
+			name = "OV9650";
+			break;
+		case 0x9656: /* Rev 4 */
+		case 0x9657: /* Rev 5 */
+			name = "OV9655";
+			break;
+		default:
+			em28xx_info("unknown OmniVision sensor detected: 0x%04x\n",
+				    id);
+			return 0;
+		}
+
+		if (dev->em28xx_sensor == EM28XX_NOSENSOR)
+			em28xx_info("unsupported sensor detected: %s\n", name);
+		else
+			em28xx_info("sensor %s detected\n", name);
+
+		dev->i2c_client[dev->def_i2c_bus].addr = client.addr;
+		return 0;
+	}
+
+	return -ENODEV;
+}
+
+int em28xx_detect_sensor(struct em28xx *dev)
+{
+	int ret;
+
+	ret = em28xx_probe_sensor_micron(dev);
+
+	if (dev->em28xx_sensor == EM28XX_NOSENSOR && ret < 0)
+		ret = em28xx_probe_sensor_omnivision(dev);
+
+	/*
+	 * NOTE: the Windows driver also probes i2c addresses
+	 *       0x22 (Samsung ?) and 0x66 (Kodak ?)
+	 */
+
+	if (dev->em28xx_sensor == EM28XX_NOSENSOR && ret < 0) {
+		em28xx_info("No sensor detected\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+int em28xx_init_camera(struct em28xx *dev)
+{
+	switch (dev->em28xx_sensor) {
+	case EM28XX_MT9V011:
+	{
+		struct mt9v011_platform_data pdata;
+		struct i2c_board_info mt9v011_info = {
+			.type = "mt9v011",
+			.addr = dev->i2c_client[dev->def_i2c_bus].addr,
+			.platform_data = &pdata,
+		};
+
+		dev->sensor_xres = 640;
+		dev->sensor_yres = 480;
+
+		/*
+		 * FIXME: mt9v011 uses I2S speed as xtal clk - at least with
+		 * the Silvercrest cam I have here for testing - for higher
+		 * resolutions, a high clock cause horizontal artifacts, so we
+		 * need to use a lower xclk frequency.
+		 * Yet, it would be possible to adjust xclk depending on the
+		 * desired resolution, since this affects directly the
+		 * frame rate.
+		 */
+		dev->board.xclk = EM28XX_XCLK_FREQUENCY_4_3MHZ;
+		em28xx_write_reg(dev, EM28XX_R0F_XCLK, dev->board.xclk);
+		dev->sensor_xtal = 4300000;
+		pdata.xtal = dev->sensor_xtal;
+		if (NULL ==
+		    v4l2_i2c_new_subdev_board(&dev->v4l2_dev,
+					      &dev->i2c_adap[dev->def_i2c_bus],
+					      &mt9v011_info, NULL))
+			return -ENODEV;
+		/* probably means GRGB 16 bit bayer */
+		dev->vinmode = 0x0d;
+		dev->vinctl = 0x00;
+
+		break;
+	}
+	case EM28XX_MT9M001:
+		dev->sensor_xres = 1280;
+		dev->sensor_yres = 1024;
+
+		em28xx_initialize_mt9m001(dev);
+
+		/* probably means BGGR 16 bit bayer */
+		dev->vinmode = 0x0c;
+		dev->vinctl = 0x00;
+
+		break;
+	case EM28XX_MT9M111:
+		dev->sensor_xres = 640;
+		dev->sensor_yres = 512;
+
+		dev->board.xclk = EM28XX_XCLK_FREQUENCY_48MHZ;
+		em28xx_write_reg(dev, EM28XX_R0F_XCLK, dev->board.xclk);
+		em28xx_initialize_mt9m111(dev);
+
+		dev->vinmode = 0x0a;
+		dev->vinctl = 0x00;
+
+		break;
+	case EM28XX_OV2640:
+	{
+		struct v4l2_subdev *subdev;
+		struct i2c_board_info ov2640_info = {
+			.type = "ov2640",
+			.flags = I2C_CLIENT_SCCB,
+			.addr = dev->i2c_client[dev->def_i2c_bus].addr,
+			.platform_data = &camlink,
+		};
+		struct v4l2_mbus_framefmt fmt;
+
+		/*
+		 * FIXME: sensor supports resolutions up to 1600x1200, but
+		 * resolution setting/switching needs to be modified to
+		 * - switch sensor output resolution (including further
+		 *   configuration changes)
+		 * - adjust bridge xclk
+		 * - disable 16 bit (12 bit) output formats on high resolutions
+		 */
+		dev->sensor_xres = 640;
+		dev->sensor_yres = 480;
+
+		subdev =
+		     v4l2_i2c_new_subdev_board(&dev->v4l2_dev,
+					       &dev->i2c_adap[dev->def_i2c_bus],
+					       &ov2640_info, NULL);
+
+		fmt.code = V4L2_MBUS_FMT_YUYV8_2X8;
+		fmt.width = 640;
+		fmt.height = 480;
+		v4l2_subdev_call(subdev, video, s_mbus_fmt, &fmt);
+
+		/* NOTE: for UXGA=1600x1200 switch to 12MHz */
+		dev->board.xclk = EM28XX_XCLK_FREQUENCY_24MHZ;
+		em28xx_write_reg(dev, EM28XX_R0F_XCLK, dev->board.xclk);
+		dev->vinmode = 0x08;
+		dev->vinctl = 0x00;
+
+		break;
+	}
+	case EM28XX_NOSENSOR:
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
diff --git a/drivers/media/usb/em28xx/em28xx-cards.c b/drivers/media/usb/em28xx/em28xx-cards.c
index 54a03b20de6..83bfbe4c980 100644
--- a/drivers/media/usb/em28xx/em28xx-cards.c
+++ b/drivers/media/usb/em28xx/em28xx-cards.c
@@ -34,7 +34,6 @@
 #include <media/saa7115.h>
 #include <media/tvp5150.h>
 #include <media/tvaudio.h>
-#include <media/mt9v011.h>
 #include <media/i2c-addr.h>
 #include <media/tveeprom.h>
 #include <media/v4l2-common.h>
@@ -345,6 +344,18 @@ static struct em28xx_reg_seq pctv_460e[] = {
 	{             -1,   -1,   -1,  -1},
 };
 
+static struct em28xx_reg_seq c3tech_digital_duo_digital[] = {
+	{EM2874_R80_GPIO,	0xff,	0xff,	10},
+	{EM2874_R80_GPIO,	0xfd,	0xff,	10}, /* xc5000 reset */
+	{EM2874_R80_GPIO,	0xf9,	0xff,	35},
+	{EM2874_R80_GPIO,	0xfd,	0xff,	10},
+	{EM2874_R80_GPIO,	0xff,	0xff,	10},
+	{EM2874_R80_GPIO,	0xfe,	0xff,	10},
+	{EM2874_R80_GPIO,	0xbe,	0xff,	10},
+	{EM2874_R80_GPIO,	0xfe,	0xff,	20},
+	{ -1,			-1,	-1,	-1},
+};
+
 #if 0
 static struct em28xx_reg_seq hauppauge_930c_gpio[] = {
 	{EM2874_R80_GPIO,	0x6f,	0xff,	10},
@@ -958,8 +969,8 @@ struct em28xx_board em28xx_boards[] = {
 #else
 		.tuner_type   = TUNER_ABSENT,
 #endif
-		.i2c_speed    = EM2874_I2C_SECONDARY_BUS_SELECT |
-				EM28XX_I2C_CLK_WAIT_ENABLE |
+		.def_i2c_bus  = 1,
+		.i2c_speed    = EM28XX_I2C_CLK_WAIT_ENABLE |
 				EM28XX_I2C_FREQ_400_KHZ,
 	},
 	[EM2884_BOARD_HAUPPAUGE_WINTV_HVR_930C] = {
@@ -974,17 +985,27 @@ struct em28xx_board em28xx_boards[] = {
 		.tuner_type   = TUNER_ABSENT,
 #endif
 		.ir_codes     = RC_MAP_HAUPPAUGE,
-		.i2c_speed    = EM2874_I2C_SECONDARY_BUS_SELECT |
-				EM28XX_I2C_CLK_WAIT_ENABLE |
+		.def_i2c_bus  = 1,
+		.i2c_speed    = EM28XX_I2C_CLK_WAIT_ENABLE |
 				EM28XX_I2C_FREQ_400_KHZ,
 	},
+	[EM2884_BOARD_C3TECH_DIGITAL_DUO] = {
+		.name         = "C3 Tech Digital Duo HDTV/SDTV USB",
+		.has_dvb      = 1,
+		/* FIXME: Add analog support - need a saa7136 driver */
+		.tuner_type = TUNER_ABSENT,	/* Digital-only TDA18271HD */
+		.ir_codes     = RC_MAP_EMPTY,
+		.def_i2c_bus  = 1,
+		.i2c_speed    = EM28XX_I2C_CLK_WAIT_ENABLE,
+		.dvb_gpio     = c3tech_digital_duo_digital,
+	},
 	[EM2884_BOARD_CINERGY_HTC_STICK] = {
 		.name         = "Terratec Cinergy HTC Stick",
 		.has_dvb      = 1,
 		.ir_codes     = RC_MAP_NEC_TERRATEC_CINERGY_XS,
 		.tuner_type   = TUNER_ABSENT,
-		.i2c_speed    = EM2874_I2C_SECONDARY_BUS_SELECT |
-				EM28XX_I2C_CLK_WAIT_ENABLE |
+		.def_i2c_bus  = 1,
+		.i2c_speed    = EM28XX_I2C_CLK_WAIT_ENABLE |
 				EM28XX_I2C_FREQ_400_KHZ,
 	},
 	[EM2880_BOARD_HAUPPAUGE_WINTV_HVR_900] = {
@@ -1404,8 +1425,8 @@ struct em28xx_board em28xx_boards[] = {
 	},
 
 	[EM2874_BOARD_LEADERSHIP_ISDBT] = {
-		.i2c_speed      = EM2874_I2C_SECONDARY_BUS_SELECT |
-				  EM28XX_I2C_CLK_WAIT_ENABLE |
+		.def_i2c_bus	= 1,
+		.i2c_speed      = EM28XX_I2C_CLK_WAIT_ENABLE |
 				  EM28XX_I2C_FREQ_100_KHZ,
 		.xclk		= EM28XX_XCLK_FREQUENCY_10MHZ,
 		.name		= "EM2874 Leadership ISDBT",
@@ -1917,8 +1938,8 @@ struct em28xx_board em28xx_boards[] = {
 	 * Empia EM28174, Sony CXD2820R and NXP TDA18271HD/C2 */
 	[EM28174_BOARD_PCTV_290E] = {
 		.name          = "PCTV nanoStick T2 290e",
-		.i2c_speed      = EM2874_I2C_SECONDARY_BUS_SELECT |
-			EM28XX_I2C_CLK_WAIT_ENABLE | EM28XX_I2C_FREQ_100_KHZ,
+		.def_i2c_bus   = 1,
+		.i2c_speed     = EM28XX_I2C_CLK_WAIT_ENABLE | EM28XX_I2C_FREQ_100_KHZ,
 		.tuner_type    = TUNER_ABSENT,
 		.tuner_gpio    = pctv_290e,
 		.has_dvb       = 1,
@@ -1927,8 +1948,8 @@ struct em28xx_board em28xx_boards[] = {
 	/* 2013:024f PCTV DVB-S2 Stick 460e
 	 * Empia EM28174, NXP TDA10071, Conexant CX24118A and Allegro A8293 */
 	[EM28174_BOARD_PCTV_460E] = {
-		.i2c_speed     = EM2874_I2C_SECONDARY_BUS_SELECT |
-			EM28XX_I2C_CLK_WAIT_ENABLE | EM28XX_I2C_FREQ_400_KHZ,
+		.def_i2c_bus   = 1,
+		.i2c_speed     = EM28XX_I2C_CLK_WAIT_ENABLE | EM28XX_I2C_FREQ_400_KHZ,
 		.name          = "PCTV DVB-S2 Stick (460e)",
 		.tuner_type    = TUNER_ABSENT,
 		.tuner_gpio    = pctv_460e,
@@ -1958,8 +1979,9 @@ struct em28xx_board em28xx_boards[] = {
 		.tuner_type    = TUNER_ABSENT,
 		.tuner_gpio    = maxmedia_ub425_tc,
 		.has_dvb       = 1,
-		.i2c_speed     = EM2874_I2C_SECONDARY_BUS_SELECT |
-				EM28XX_I2C_CLK_WAIT_ENABLE |
+		.ir_codes      = RC_MAP_REDDO,
+		.def_i2c_bus   = 1,
+		.i2c_speed     = EM28XX_I2C_CLK_WAIT_ENABLE |
 				EM28XX_I2C_FREQ_400_KHZ,
 	},
 	/* 2304:0242 PCTV QuatroStick (510e)
@@ -1970,8 +1992,8 @@ struct em28xx_board em28xx_boards[] = {
 		.tuner_gpio    = pctv_510e,
 		.has_dvb       = 1,
 		.ir_codes      = RC_MAP_PINNACLE_PCTV_HD,
-		.i2c_speed     = EM2874_I2C_SECONDARY_BUS_SELECT |
-				EM28XX_I2C_CLK_WAIT_ENABLE |
+		.def_i2c_bus   = 1,
+		.i2c_speed     = EM28XX_I2C_CLK_WAIT_ENABLE |
 				EM28XX_I2C_FREQ_400_KHZ,
 	},
 	/* 2013:0251 PCTV QuatroStick nano (520e)
@@ -1982,8 +2004,8 @@ struct em28xx_board em28xx_boards[] = {
 		.tuner_gpio    = pctv_520e,
 		.has_dvb       = 1,
 		.ir_codes      = RC_MAP_PINNACLE_PCTV_HD,
-		.i2c_speed     = EM2874_I2C_SECONDARY_BUS_SELECT |
-				EM28XX_I2C_CLK_WAIT_ENABLE |
+		.def_i2c_bus   = 1,
+		.i2c_speed     = EM28XX_I2C_CLK_WAIT_ENABLE |
 				EM28XX_I2C_FREQ_400_KHZ,
 	},
 	[EM2884_BOARD_TERRATEC_HTC_USB_XS] = {
@@ -1991,8 +2013,8 @@ struct em28xx_board em28xx_boards[] = {
 		.has_dvb      = 1,
 		.ir_codes     = RC_MAP_NEC_TERRATEC_CINERGY_XS,
 		.tuner_type   = TUNER_ABSENT,
-		.i2c_speed    = EM2874_I2C_SECONDARY_BUS_SELECT |
-				EM28XX_I2C_CLK_WAIT_ENABLE |
+		.def_i2c_bus  = 1,
+		.i2c_speed    = EM28XX_I2C_CLK_WAIT_ENABLE |
 				EM28XX_I2C_FREQ_400_KHZ,
 	},
 };
@@ -2144,6 +2166,8 @@ struct usb_device_id em28xx_id_table[] = {
 			.driver_info = EM28174_BOARD_PCTV_460E },
 	{ USB_DEVICE(0x2040, 0x1605),
 			.driver_info = EM2884_BOARD_HAUPPAUGE_WINTV_HVR_930C },
+	{ USB_DEVICE(0x1b80, 0xe755),
+			.driver_info = EM2884_BOARD_C3TECH_DIGITAL_DUO },
 	{ USB_DEVICE(0xeb1a, 0x5006),
 			.driver_info = EM2860_BOARD_HT_VIDBOX_NW03 },
 	{ USB_DEVICE(0x1b80, 0xe309), /* Sveon STV40 */
@@ -2183,6 +2207,7 @@ static struct em28xx_hash_table em28xx_i2c_hash[] = {
 	{0x4ba50080, EM2861_BOARD_GADMEI_UTV330PLUS, TUNER_TNF_5335MF},
 	{0x6b800080, EM2874_BOARD_LEADERSHIP_ISDBT, TUNER_ABSENT},
 };
+/* NOTE: introduce a separate hash table for devices with 16 bit eeproms */
 
 /* I2C possible address to saa7115, tvp5150, msp3400, tvaudio */
 static unsigned short saa711x_addrs[] = {
@@ -2204,8 +2229,9 @@ static unsigned short msp3400_addrs[] = {
 
 int em28xx_tuner_callback(void *ptr, int component, int command, int arg)
 {
+	struct em28xx_i2c_bus *i2c_bus = ptr;
+	struct em28xx *dev = i2c_bus->dev;
 	int rc = 0;
-	struct em28xx *dev = ptr;
 
 	if (dev->tuner_type != TUNER_XC2028 && dev->tuner_type != TUNER_XC5000)
 		return 0;
@@ -2233,145 +2259,9 @@ static inline void em28xx_set_model(struct em28xx *dev)
 	if (!dev->board.i2c_speed)
 		dev->board.i2c_speed = EM28XX_I2C_CLK_WAIT_ENABLE |
 				       EM28XX_I2C_FREQ_100_KHZ;
-}
-
-
-/* FIXME: Should be replaced by a proper mt9m111 driver */
-static int em28xx_initialize_mt9m111(struct em28xx *dev)
-{
-	int i;
-	unsigned char regs[][3] = {
-		{ 0x0d, 0x00, 0x01, },  /* reset and use defaults */
-		{ 0x0d, 0x00, 0x00, },
-		{ 0x0a, 0x00, 0x21, },
-		{ 0x21, 0x04, 0x00, },  /* full readout speed, no row/col skipping */
-	};
-
-	for (i = 0; i < ARRAY_SIZE(regs); i++)
-		i2c_master_send(&dev->i2c_client, &regs[i][0], 3);
-
-	return 0;
-}
-
-
-/* FIXME: Should be replaced by a proper mt9m001 driver */
-static int em28xx_initialize_mt9m001(struct em28xx *dev)
-{
-	int i;
-	unsigned char regs[][3] = {
-		{ 0x0d, 0x00, 0x01, },
-		{ 0x0d, 0x00, 0x00, },
-		{ 0x04, 0x05, 0x00, },	/* hres = 1280 */
-		{ 0x03, 0x04, 0x00, },  /* vres = 1024 */
-		{ 0x20, 0x11, 0x00, },
-		{ 0x06, 0x00, 0x10, },
-		{ 0x2b, 0x00, 0x24, },
-		{ 0x2e, 0x00, 0x24, },
-		{ 0x35, 0x00, 0x24, },
-		{ 0x2d, 0x00, 0x20, },
-		{ 0x2c, 0x00, 0x20, },
-		{ 0x09, 0x0a, 0xd4, },
-		{ 0x35, 0x00, 0x57, },
-	};
-
-	for (i = 0; i < ARRAY_SIZE(regs); i++)
-		i2c_master_send(&dev->i2c_client, &regs[i][0], 3);
-
-	return 0;
-}
-
-/* HINT method: webcam I2C chips
- *
- * This method works for webcams with Micron sensors
- */
-static int em28xx_hint_sensor(struct em28xx *dev)
-{
-	int rc;
-	char *sensor_name;
-	unsigned char cmd;
-	__be16 version_be;
-	u16 version;
-
-	/* Micron sensor detection */
-	dev->i2c_client.addr = 0xba >> 1;
-	cmd = 0;
-	i2c_master_send(&dev->i2c_client, &cmd, 1);
-	rc = i2c_master_recv(&dev->i2c_client, (char *)&version_be, 2);
-	if (rc != 2)
-		return -EINVAL;
-
-	version = be16_to_cpu(version_be);
-	switch (version) {
-	case 0x8232:		/* mt9v011 640x480 1.3 Mpix sensor */
-	case 0x8243:		/* mt9v011 rev B 640x480 1.3 Mpix sensor */
-		dev->model = EM2820_BOARD_SILVERCREST_WEBCAM;
-		em28xx_set_model(dev);
-
-		sensor_name = "mt9v011";
-		dev->em28xx_sensor = EM28XX_MT9V011;
-		dev->sensor_xres = 640;
-		dev->sensor_yres = 480;
-		/*
-		 * FIXME: mt9v011 uses I2S speed as xtal clk - at least with
-		 * the Silvercrest cam I have here for testing - for higher
-		 * resolutions, a high clock cause horizontal artifacts, so we
-		 * need to use a lower xclk frequency.
-		 * Yet, it would be possible to adjust xclk depending on the
-		 * desired resolution, since this affects directly the
-		 * frame rate.
-		 */
-		dev->board.xclk = EM28XX_XCLK_FREQUENCY_4_3MHZ;
-		dev->sensor_xtal = 4300000;
-
-		/* probably means GRGB 16 bit bayer */
-		dev->vinmode = 0x0d;
-		dev->vinctl = 0x00;
-
-		break;
-
-	case 0x143a:    /* MT9M111 as found in the ECS G200 */
-		dev->model = EM2750_BOARD_UNKNOWN;
-		em28xx_set_model(dev);
-
-		sensor_name = "mt9m111";
-		dev->board.xclk = EM28XX_XCLK_FREQUENCY_48MHZ;
-		dev->em28xx_sensor = EM28XX_MT9M111;
-		em28xx_initialize_mt9m111(dev);
-		dev->sensor_xres = 640;
-		dev->sensor_yres = 512;
-
-		dev->vinmode = 0x0a;
-		dev->vinctl = 0x00;
-
-		break;
-
-	case 0x8431:
-		dev->model = EM2750_BOARD_UNKNOWN;
-		em28xx_set_model(dev);
-
-		sensor_name = "mt9m001";
-		dev->em28xx_sensor = EM28XX_MT9M001;
-		em28xx_initialize_mt9m001(dev);
-		dev->sensor_xres = 1280;
-		dev->sensor_yres = 1024;
-
-		/* probably means BGGR 16 bit bayer */
-		dev->vinmode = 0x0c;
-		dev->vinctl = 0x00;
-
-		break;
-	default:
-		printk("Unknown Micron Sensor 0x%04x\n", version);
-		return -EINVAL;
-	}
-
-	/* Setup webcam defaults */
-	em28xx_pre_card_setup(dev);
 
-	em28xx_errdev("Sensor is %s, using model %s entry.\n",
-		      sensor_name, em28xx_boards[dev->model].name);
-
-	return 0;
+	/* Should be initialized early, for I2C to work */
+	dev->def_i2c_bus = dev->board.def_i2c_bus;
 }
 
 /* Since em28xx_pre_card_setup() requires a proper dev->model,
@@ -2599,6 +2489,18 @@ static int em28xx_hint_board(struct em28xx *dev)
 {
 	int i;
 
+	if (dev->board.is_webcam) {
+		if (dev->em28xx_sensor == EM28XX_MT9V011) {
+			dev->model = EM2820_BOARD_SILVERCREST_WEBCAM;
+		} else if (dev->em28xx_sensor == EM28XX_MT9M001 ||
+			   dev->em28xx_sensor == EM28XX_MT9M111) {
+			dev->model = EM2750_BOARD_UNKNOWN;
+		}
+		/* FIXME: IMPROVE ! */
+
+		return 0;
+	}
+
 	/* HINT method: EEPROM
 	 *
 	 * This method works only for boards with eeprom.
@@ -2638,7 +2540,7 @@ static int em28xx_hint_board(struct em28xx *dev)
 
 	/* user did not request i2c scanning => do it now */
 	if (!dev->i2c_hash)
-		em28xx_do_i2c_scan(dev);
+		em28xx_do_i2c_scan(dev, dev->def_i2c_bus);
 
 	for (i = 0; i < ARRAY_SIZE(em28xx_i2c_hash); i++) {
 		if (dev->i2c_hash == em28xx_i2c_hash[i].hash) {
@@ -2684,16 +2586,16 @@ static void em28xx_card_setup(struct em28xx *dev)
 	 * If sensor is not found, then it isn't a webcam.
 	 */
 	if (dev->board.is_webcam) {
-		if (em28xx_hint_sensor(dev) < 0)
+		if (em28xx_detect_sensor(dev) < 0)
 			dev->board.is_webcam = 0;
 		else
 			dev->progressive = 1;
 	}
 
-	if (!dev->board.is_webcam) {
-		switch (dev->model) {
-		case EM2820_BOARD_UNKNOWN:
-		case EM2800_BOARD_UNKNOWN:
+	switch (dev->model) {
+	case EM2750_BOARD_UNKNOWN:
+	case EM2820_BOARD_UNKNOWN:
+	case EM2800_BOARD_UNKNOWN:
 		/*
 		 * The K-WORLD DVB-T 310U is detected as an MSI Digivox AD.
 		 *
@@ -2714,9 +2616,8 @@ static void em28xx_card_setup(struct em28xx *dev)
 			em28xx_pre_card_setup(dev);
 		}
 		break;
-		default:
-			em28xx_set_model(dev);
-		}
+	default:
+		em28xx_set_model(dev);
 	}
 
 	em28xx_info("Identified as %s (card=%d)\n",
@@ -2736,15 +2637,19 @@ static void em28xx_card_setup(struct em28xx *dev)
 	case EM2880_BOARD_HAUPPAUGE_WINTV_HVR_900_R2:
 	case EM2883_BOARD_HAUPPAUGE_WINTV_HVR_850:
 	case EM2883_BOARD_HAUPPAUGE_WINTV_HVR_950:
+	case EM2884_BOARD_HAUPPAUGE_WINTV_HVR_930C:
 	{
 		struct tveeprom tv;
+
+		if (dev->eedata == NULL)
+			break;
 #if defined(CONFIG_MODULES) && defined(MODULE)
 		request_module("tveeprom");
 #endif
 		/* Call first TVeeprom */
 
-		dev->i2c_client.addr = 0xa0 >> 1;
-		tveeprom_hauppauge_analog(&dev->i2c_client, &tv, dev->eedata);
+		dev->i2c_client[dev->def_i2c_bus].addr = 0xa0 >> 1;
+		tveeprom_hauppauge_analog(&dev->i2c_client[dev->def_i2c_bus], &tv, dev->eedata);
 
 		dev->tuner_type = tv.tuner_type;
 
@@ -2791,7 +2696,7 @@ static void em28xx_card_setup(struct em28xx *dev)
 		em28xx_set_mode(dev, EM28XX_ANALOG_MODE);
 		break;
 
-/*
+		/*
 		 * The Dikom DK300 is detected as an Kworld VS-DVB-T 323UR.
 		 *
 		 * This occurs because they share identical USB vendor and
@@ -2826,51 +2731,41 @@ static void em28xx_card_setup(struct em28xx *dev)
 				"addresses)\n\n");
 	}
 
+	/* Free eeprom data memory */
+	kfree(dev->eedata);
+	dev->eedata = NULL;
+
 	/* Allow override tuner type by a module parameter */
 	if (tuner >= 0)
 		dev->tuner_type = tuner;
 
 	/* request some modules */
 	if (dev->board.has_msp34xx)
-		v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap,
+		v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap[dev->def_i2c_bus],
 			"msp3400", 0, msp3400_addrs);
 
 	if (dev->board.decoder == EM28XX_SAA711X)
-		v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap,
+		v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap[dev->def_i2c_bus],
 			"saa7115_auto", 0, saa711x_addrs);
 
 	if (dev->board.decoder == EM28XX_TVP5150)
-		v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap,
+		v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap[dev->def_i2c_bus],
 			"tvp5150", 0, tvp5150_addrs);
 
-	if (dev->em28xx_sensor == EM28XX_MT9V011) {
-		struct mt9v011_platform_data pdata;
-		struct i2c_board_info mt9v011_info = {
-			.type = "mt9v011",
-			.addr = 0xba >> 1,
-			.platform_data = &pdata,
-		};
-
-		pdata.xtal = dev->sensor_xtal;
-		v4l2_i2c_new_subdev_board(&dev->v4l2_dev, &dev->i2c_adap,
-				&mt9v011_info, NULL);
-	}
-
-
 	if (dev->board.adecoder == EM28XX_TVAUDIO)
-		v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap,
+		v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap[dev->def_i2c_bus],
 			"tvaudio", dev->board.tvaudio_addr, NULL);
 
 	if (dev->board.tuner_type != TUNER_ABSENT) {
 		int has_demod = (dev->tda9887_conf & TDA9887_PRESENT);
 
 		if (dev->board.radio.type)
-			v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap,
+			v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap[dev->def_i2c_bus],
 				"tuner", dev->board.radio_addr, NULL);
 
 		if (has_demod)
 			v4l2_i2c_new_subdev(&dev->v4l2_dev,
-				&dev->i2c_adap, "tuner",
+				&dev->i2c_adap[dev->def_i2c_bus], "tuner",
 				0, v4l2_i2c_tuner_addrs(ADDRS_DEMOD));
 		if (dev->tuner_addr == 0) {
 			enum v4l2_i2c_tuner_type type =
@@ -2878,18 +2773,20 @@ static void em28xx_card_setup(struct em28xx *dev)
 			struct v4l2_subdev *sd;
 
 			sd = v4l2_i2c_new_subdev(&dev->v4l2_dev,
-				&dev->i2c_adap, "tuner",
+				&dev->i2c_adap[dev->def_i2c_bus], "tuner",
 				0, v4l2_i2c_tuner_addrs(type));
 
 			if (sd)
 				dev->tuner_addr = v4l2_i2c_subdev_addr(sd);
 		} else {
-			v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap,
+			v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap[dev->def_i2c_bus],
 				"tuner", dev->tuner_addr, NULL);
 		}
 	}
 
 	em28xx_tuner_setup(dev);
+
+	em28xx_init_camera(dev);
 }
 
 
@@ -2914,7 +2811,8 @@ static void request_module_async(struct work_struct *work)
 
 	if (dev->board.has_dvb)
 		request_module("em28xx-dvb");
-	if ((dev->board.ir_codes || dev->board.has_ir_i2c) && !disable_ir)
+	if (dev->board.has_snapshot_button ||
+	    ((dev->board.ir_codes || dev->board.has_ir_i2c) && !disable_ir))
 		request_module("em28xx-rc");
 #endif /* CONFIG_MODULES */
 }
@@ -2941,7 +2839,9 @@ void em28xx_release_resources(struct em28xx *dev)
 
 	em28xx_release_analog_resources(dev);
 
-	em28xx_i2c_unregister(dev);
+	if (dev->def_i2c_bus)
+		em28xx_i2c_unregister(dev, 1);
+	em28xx_i2c_unregister(dev, 0);
 
 	v4l2_ctrl_handler_free(&dev->ctrl_handler);
 
@@ -3002,8 +2902,23 @@ static int em28xx_init_dev(struct em28xx *dev, struct usb_device *udev,
 		case CHIP_ID_EM2750:
 			chip_name = "em2750";
 			break;
+		case CHIP_ID_EM2765:
+			chip_name = "em2765";
+			dev->wait_after_write = 0;
+			dev->is_em25xx = 1;
+			dev->eeprom_addrwidth_16bit = 1;
+			break;
 		case CHIP_ID_EM2820:
 			chip_name = "em2710/2820";
+			if (le16_to_cpu(dev->udev->descriptor.idVendor)
+								    == 0xeb1a) {
+				__le16 idProd = dev->udev->descriptor.idProduct;
+				if (le16_to_cpu(idProd) == 0x2710)
+					chip_name = "em2710";
+				else if (le16_to_cpu(idProd) == 0x2820)
+					chip_name = "em2820";
+			}
+			/* NOTE: the em2820 is used in webcams, too ! */
 			break;
 		case CHIP_ID_EM2840:
 			chip_name = "em2840";
@@ -3019,11 +2934,13 @@ static int em28xx_init_dev(struct em28xx *dev, struct usb_device *udev,
 			chip_name = "em2874";
 			dev->reg_gpio_num = EM2874_R80_GPIO;
 			dev->wait_after_write = 0;
+			dev->eeprom_addrwidth_16bit = 1;
 			break;
 		case CHIP_ID_EM28174:
 			chip_name = "em28174";
 			dev->reg_gpio_num = EM2874_R80_GPIO;
 			dev->wait_after_write = 0;
+			dev->eeprom_addrwidth_16bit = 1;
 			break;
 		case CHIP_ID_EM2883:
 			chip_name = "em2882/3";
@@ -3033,6 +2950,7 @@ static int em28xx_init_dev(struct em28xx *dev, struct usb_device *udev,
 			chip_name = "em2884";
 			dev->reg_gpio_num = EM2874_R80_GPIO;
 			dev->wait_after_write = 0;
+			dev->eeprom_addrwidth_16bit = 1;
 			break;
 		default:
 			printk(KERN_INFO DRIVER_NAME
@@ -3066,14 +2984,6 @@ static int em28xx_init_dev(struct em28xx *dev, struct usb_device *udev,
 
 	em28xx_pre_card_setup(dev);
 
-	if (dev->chip_id == CHIP_ID_EM2820) {
-		if (dev->board.is_webcam)
-			chip_name = "em2710";
-		else
-			chip_name = "em2820";
-		snprintf(dev->name, sizeof(dev->name), "%s #%d", chip_name, dev->devno);
-	}
-
 	if (!dev->board.is_em2800) {
 		/* Resets I2C speed */
 		retval = em28xx_write_reg(dev, EM28XX_R06_I2C_CLK, dev->board.i2c_speed);
@@ -3091,17 +3001,37 @@ static int em28xx_init_dev(struct em28xx *dev, struct usb_device *udev,
 		return retval;
 	}
 
-	v4l2_ctrl_handler_init(hdl, 4);
+	v4l2_ctrl_handler_init(hdl, 8);
 	dev->v4l2_dev.ctrl_handler = hdl;
 
-	/* register i2c bus */
-	retval = em28xx_i2c_register(dev);
+	rt_mutex_init(&dev->i2c_bus_lock);
+
+	/* register i2c bus 0 */
+	if (dev->board.is_em2800)
+		retval = em28xx_i2c_register(dev, 0, EM28XX_I2C_ALGO_EM2800);
+	else
+		retval = em28xx_i2c_register(dev, 0, EM28XX_I2C_ALGO_EM28XX);
 	if (retval < 0) {
-		em28xx_errdev("%s: em28xx_i2c_register - error [%d]!\n",
+		em28xx_errdev("%s: em28xx_i2c_register bus 0 - error [%d]!\n",
 			__func__, retval);
 		goto unregister_dev;
 	}
 
+	/* register i2c bus 1 */
+	if (dev->def_i2c_bus) {
+		if (dev->is_em25xx)
+			retval = em28xx_i2c_register(dev, 1,
+						  EM28XX_I2C_ALGO_EM25XX_BUS_B);
+		else
+			retval = em28xx_i2c_register(dev, 1,
+							EM28XX_I2C_ALGO_EM28XX);
+		if (retval < 0) {
+			em28xx_errdev("%s: em28xx_i2c_register bus 1 - error [%d]!\n",
+				__func__, retval);
+			goto unregister_dev;
+		}
+	}
+
 	/*
 	 * Default format, used for tvp5150 or saa711x output formats
 	 */
@@ -3160,11 +3090,6 @@ static int em28xx_init_dev(struct em28xx *dev, struct usb_device *udev,
 		msleep(3);
 	}
 
-	v4l2_ctrl_handler_setup(&dev->ctrl_handler);
-	retval = dev->ctrl_handler.error;
-	if (retval)
-		goto fail;
-
 	retval = em28xx_register_analog_devices(dev);
 	if (retval < 0) {
 		goto fail;
@@ -3176,7 +3101,9 @@ static int em28xx_init_dev(struct em28xx *dev, struct usb_device *udev,
 	return 0;
 
 fail:
-	em28xx_i2c_unregister(dev);
+	if (dev->def_i2c_bus)
+		em28xx_i2c_unregister(dev, 1);
+	em28xx_i2c_unregister(dev, 0);
 	v4l2_ctrl_handler_free(&dev->ctrl_handler);
 
 unregister_dev:
@@ -3292,14 +3219,15 @@ static int em28xx_usb_probe(struct usb_interface *interface,
 						dev->analog_ep_bulk =
 							    e->bEndpointAddress;
 					} else {
-						has_dvb = true;
 						if (usb_endpoint_xfer_isoc(e)) {
-							dev->dvb_ep_isoc = e->bEndpointAddress;
 							if (size > dev->dvb_max_pkt_size_isoc) {
+								has_dvb = true; /* see NOTE (~) */
+								dev->dvb_ep_isoc = e->bEndpointAddress;
 								dev->dvb_max_pkt_size_isoc = size;
 								dev->dvb_alt_isoc = i;
 							}
 						} else {
+							has_dvb = true;
 							dev->dvb_ep_bulk = e->bEndpointAddress;
 						}
 					}
@@ -3326,6 +3254,12 @@ static int em28xx_usb_probe(struct usb_interface *interface,
 			 * so far. But there might be devices for which this
 			 * logic is not sufficient...
 			 */
+			/*
+			 * NOTE (~): some manufacturers (e.g. Terratec) disable
+			 * endpoints by setting wMaxPacketSize to 0 bytes for
+			 * all alt settings. So far, we've seen this for
+			 * DVB isoc endpoints only.
+			 */
 		}
 	}
 
diff --git a/drivers/media/usb/em28xx/em28xx-core.c b/drivers/media/usb/em28xx/em28xx-core.c
index aaedd11791f..a802128ce9c 100644
--- a/drivers/media/usb/em28xx/em28xx-core.c
+++ b/drivers/media/usb/em28xx/em28xx-core.c
@@ -607,12 +607,12 @@ EXPORT_SYMBOL_GPL(em28xx_audio_setup);
 
 int em28xx_colorlevels_set_default(struct em28xx *dev)
 {
-	em28xx_write_reg(dev, EM28XX_R20_YGAIN, 0x10);	/* contrast */
-	em28xx_write_reg(dev, EM28XX_R21_YOFFSET, 0x00);	/* brightness */
-	em28xx_write_reg(dev, EM28XX_R22_UVGAIN, 0x10);	/* saturation */
-	em28xx_write_reg(dev, EM28XX_R23_UOFFSET, 0x00);
-	em28xx_write_reg(dev, EM28XX_R24_VOFFSET, 0x00);
-	em28xx_write_reg(dev, EM28XX_R25_SHARPNESS, 0x00);
+	em28xx_write_reg(dev, EM28XX_R20_YGAIN, CONTRAST_DEFAULT);
+	em28xx_write_reg(dev, EM28XX_R21_YOFFSET, BRIGHTNESS_DEFAULT);
+	em28xx_write_reg(dev, EM28XX_R22_UVGAIN, SATURATION_DEFAULT);
+	em28xx_write_reg(dev, EM28XX_R23_UOFFSET, BLUE_BALANCE_DEFAULT);
+	em28xx_write_reg(dev, EM28XX_R24_VOFFSET, RED_BALANCE_DEFAULT);
+	em28xx_write_reg(dev, EM28XX_R25_SHARPNESS, SHARPNESS_DEFAULT);
 
 	em28xx_write_reg(dev, EM28XX_R14_GAMMA, 0x20);
 	em28xx_write_reg(dev, EM28XX_R15_RGAIN, 0x20);
@@ -681,6 +681,11 @@ int em28xx_vbi_supported(struct em28xx *dev)
 	if (disable_vbi == 1)
 		return 0;
 
+	if (dev->board.is_webcam)
+		return 0;
+
+	/* FIXME: check subdevices for VBI support */
+
 	if (dev->chip_id == CHIP_ID_EM2860 ||
 	    dev->chip_id == CHIP_ID_EM2883)
 		return 1;
@@ -692,12 +697,23 @@ int em28xx_vbi_supported(struct em28xx *dev)
 int em28xx_set_outfmt(struct em28xx *dev)
 {
 	int ret;
-	u8 vinctrl;
-
-	ret = em28xx_write_reg_bits(dev, EM28XX_R27_OUTFMT,
-				dev->format->reg | 0x20, 0xff);
+	u8 fmt, vinctrl;
+
+	fmt = dev->format->reg;
+	if (!dev->is_em25xx)
+		fmt |= 0x20;
+	/*
+	 * NOTE: it's not clear if this is really needed !
+	 * The datasheets say bit 5 is a reserved bit and devices seem to work
+	 * fine without it. But the Windows driver sets it for em2710/50+em28xx
+	 * devices and we've always been setting it, too.
+	 *
+	 * em2765 (em25xx, em276x/7x/8x) devices do NOT work with this bit set,
+	 * it's likely used for an additional (compressed ?) format there.
+	 */
+	ret = em28xx_write_reg(dev, EM28XX_R27_OUTFMT, fmt);
 	if (ret < 0)
-			return ret;
+		return ret;
 
 	ret = em28xx_write_reg(dev, EM28XX_R10_VINMODE, dev->vinmode);
 	if (ret < 0)
@@ -751,6 +767,13 @@ static void em28xx_capture_area_set(struct em28xx *dev, u8 hstart, u8 vstart,
 	em28xx_write_regs(dev, EM28XX_R1E_CWIDTH, &cwidth, 1);
 	em28xx_write_regs(dev, EM28XX_R1F_CHEIGHT, &cheight, 1);
 	em28xx_write_regs(dev, EM28XX_R1B_OFLOW, &overflow, 1);
+
+	/* FIXME: function/meaning of these registers ? */
+	/* FIXME: align width+height to multiples of 4 ?! */
+	if (dev->is_em25xx) {
+		em28xx_write_reg(dev, 0x34, width >> 4);
+		em28xx_write_reg(dev, 0x35, height >> 4);
+	}
 }
 
 static int em28xx_scaler_set(struct em28xx *dev, u16 h, u16 v)
diff --git a/drivers/media/usb/em28xx/em28xx-dvb.c b/drivers/media/usb/em28xx/em28xx-dvb.c
index a81ec2e8cc9..b22f8fed812 100644
--- a/drivers/media/usb/em28xx/em28xx-dvb.c
+++ b/drivers/media/usb/em28xx/em28xx-dvb.c
@@ -50,6 +50,7 @@
 #include "tda10071.h"
 #include "a8293.h"
 #include "qt1010.h"
+#include "mb86a20s.h"
 
 MODULE_DESCRIPTION("driver for em28xx based DVB cards");
 MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@infradead.org>");
@@ -177,7 +178,8 @@ static inline int em28xx_dvb_urb_data_copy(struct em28xx *dev, struct urb *urb)
 static int em28xx_start_streaming(struct em28xx_dvb *dvb)
 {
 	int rc;
-	struct em28xx *dev = dvb->adapter.priv;
+	struct em28xx_i2c_bus *i2c_bus = dvb->adapter.priv;
+	struct em28xx *dev = i2c_bus->dev;
 	int dvb_max_packet_size, packet_multiplier, dvb_alt;
 
 	if (dev->dvb_xfer_bulk) {
@@ -216,12 +218,11 @@ static int em28xx_start_streaming(struct em28xx_dvb *dvb)
 
 static int em28xx_stop_streaming(struct em28xx_dvb *dvb)
 {
-	struct em28xx *dev = dvb->adapter.priv;
+	struct em28xx_i2c_bus *i2c_bus = dvb->adapter.priv;
+	struct em28xx *dev = i2c_bus->dev;
 
 	em28xx_stop_urbs(dev);
 
-	em28xx_set_mode(dev, EM28XX_SUSPEND);
-
 	return 0;
 }
 
@@ -269,7 +270,8 @@ static int em28xx_stop_feed(struct dvb_demux_feed *feed)
 /* ------------------------------------------------------------------ */
 static int em28xx_dvb_bus_ctrl(struct dvb_frontend *fe, int acquire)
 {
-	struct em28xx *dev = fe->dvb->priv;
+	struct em28xx_i2c_bus *i2c_bus = fe->dvb->priv;
+        struct em28xx *dev = i2c_bus->dev;
 
 	if (acquire)
 		return em28xx_set_mode(dev, EM28XX_DIGITAL_MODE);
@@ -465,10 +467,10 @@ static void hauppauge_hvr930c_init(struct em28xx *dev)
 	em28xx_write_reg(dev, EM28XX_R06_I2C_CLK, 0x44);
 	msleep(10);
 
-	dev->i2c_client.addr = 0x82 >> 1;
+	dev->i2c_client[dev->def_i2c_bus].addr = 0x82 >> 1;
 
 	for (i = 0; i < ARRAY_SIZE(regs); i++)
-		i2c_master_send(&dev->i2c_client, regs[i].r, regs[i].len);
+		i2c_master_send(&dev->i2c_client[dev->def_i2c_bus], regs[i].r, regs[i].len);
 	em28xx_gpio_set(dev, hauppauge_hvr930c_end);
 
 	msleep(100);
@@ -522,10 +524,10 @@ static void terratec_h5_init(struct em28xx *dev)
 	em28xx_write_reg(dev, EM28XX_R06_I2C_CLK, 0x45);
 	msleep(10);
 
-	dev->i2c_client.addr = 0x82 >> 1;
+	dev->i2c_client[dev->def_i2c_bus].addr = 0x82 >> 1;
 
 	for (i = 0; i < ARRAY_SIZE(regs); i++)
-		i2c_master_send(&dev->i2c_client, regs[i].r, regs[i].len);
+		i2c_master_send(&dev->i2c_client[dev->def_i2c_bus], regs[i].r, regs[i].len);
 	em28xx_gpio_set(dev, terratec_h5_end);
 };
 
@@ -575,10 +577,10 @@ static void terratec_htc_stick_init(struct em28xx *dev)
 	em28xx_write_reg(dev, EM28XX_R06_I2C_CLK, 0x44);
 	msleep(10);
 
-	dev->i2c_client.addr = 0x82 >> 1;
+	dev->i2c_client[dev->def_i2c_bus].addr = 0x82 >> 1;
 
 	for (i = 0; i < ARRAY_SIZE(regs); i++)
-		i2c_master_send(&dev->i2c_client, regs[i].r, regs[i].len);
+		i2c_master_send(&dev->i2c_client[dev->def_i2c_bus], regs[i].r, regs[i].len);
 
 	em28xx_gpio_set(dev, terratec_htc_stick_end);
 };
@@ -633,10 +635,10 @@ static void terratec_htc_usb_xs_init(struct em28xx *dev)
 	em28xx_write_reg(dev, EM28XX_R06_I2C_CLK, 0x44);
 	msleep(10);
 
-	dev->i2c_client.addr = 0x82 >> 1;
+	dev->i2c_client[dev->def_i2c_bus].addr = 0x82 >> 1;
 
 	for (i = 0; i < ARRAY_SIZE(regs); i++)
-		i2c_master_send(&dev->i2c_client, regs[i].r, regs[i].len);
+		i2c_master_send(&dev->i2c_client[dev->def_i2c_bus], regs[i].r, regs[i].len);
 
 	em28xx_gpio_set(dev, terratec_htc_usb_xs_end);
 };
@@ -662,10 +664,10 @@ static void pctv_520e_init(struct em28xx *dev)
 		{{ 0x01, 0x00, 0x73, 0xaf }, 4},
 	};
 
-	dev->i2c_client.addr = 0x82 >> 1; /* 0x41 */
+	dev->i2c_client[dev->def_i2c_bus].addr = 0x82 >> 1; /* 0x41 */
 
 	for (i = 0; i < ARRAY_SIZE(regs); i++)
-		i2c_master_send(&dev->i2c_client, regs[i].r, regs[i].len);
+		i2c_master_send(&dev->i2c_client[dev->def_i2c_bus], regs[i].r, regs[i].len);
 };
 
 static int em28xx_pctv_290e_set_lna(struct dvb_frontend *fe)
@@ -768,9 +770,25 @@ static struct zl10353_config em28xx_zl10353_no_i2c_gate_dev = {
 };
 static struct qt1010_config em28xx_qt1010_config = {
 	.i2c_address = 0x62
+};
 
+static const struct mb86a20s_config c3tech_duo_mb86a20s_config = {
+	.demod_address = 0x10,
+	.is_serial = true,
+};
+
+static struct tda18271_std_map mb86a20s_tda18271_config = {
+	.dvbt_6   = { .if_freq = 4000, .agc_mode = 3, .std = 4,
+		      .if_lvl = 1, .rfagc_top = 0x37, },
 };
 
+static struct tda18271_config c3tech_duo_tda18271_config = {
+	.std_map = &mb86a20s_tda18271_config,
+	.gate    = TDA18271_GATE_DIGITAL,
+	.small_i2c = TDA18271_03_BYTE_CHUNK_INIT,
+};
+
+
 /* ------------------------------------------------------------------ */
 
 static int em28xx_attach_xc3028(u8 addr, struct em28xx *dev)
@@ -779,7 +797,7 @@ static int em28xx_attach_xc3028(u8 addr, struct em28xx *dev)
 	struct xc2028_config cfg;
 
 	memset(&cfg, 0, sizeof(cfg));
-	cfg.i2c_adap  = &dev->i2c_adap;
+	cfg.i2c_adap  = &dev->i2c_adap[dev->def_i2c_bus];
 	cfg.i2c_addr  = addr;
 
 	if (!dev->dvb->fe[0]) {
@@ -824,7 +842,7 @@ static int em28xx_register_dvb(struct em28xx_dvb *dvb, struct module *module,
 	if (dvb->fe[1])
 		dvb->fe[1]->ops.ts_bus_ctrl = em28xx_dvb_bus_ctrl;
 
-	dvb->adapter.priv = dev;
+	dvb->adapter.priv = &dev->i2c_bus[dev->def_i2c_bus];
 
 	/* register frontend */
 	result = dvb_register_frontend(&dvb->adapter, dvb->fe[0]);
@@ -962,7 +980,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 	switch (dev->model) {
 	case EM2874_BOARD_LEADERSHIP_ISDBT:
 		dvb->fe[0] = dvb_attach(s921_attach,
-				&sharp_isdbt, &dev->i2c_adap);
+				&sharp_isdbt, &dev->i2c_adap[dev->def_i2c_bus]);
 
 		if (!dvb->fe[0]) {
 			result = -EINVAL;
@@ -976,7 +994,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 	case EM2880_BOARD_AMD_ATI_TV_WONDER_HD_600:
 		dvb->fe[0] = dvb_attach(lgdt330x_attach,
 					   &em2880_lgdt3303_dev,
-					   &dev->i2c_adap);
+					   &dev->i2c_adap[dev->def_i2c_bus]);
 		if (em28xx_attach_xc3028(0x61, dev) < 0) {
 			result = -EINVAL;
 			goto out_free;
@@ -985,7 +1003,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 	case EM2880_BOARD_KWORLD_DVB_310U:
 		dvb->fe[0] = dvb_attach(zl10353_attach,
 					   &em28xx_zl10353_with_xc3028,
-					   &dev->i2c_adap);
+					   &dev->i2c_adap[dev->def_i2c_bus]);
 		if (em28xx_attach_xc3028(0x61, dev) < 0) {
 			result = -EINVAL;
 			goto out_free;
@@ -996,7 +1014,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 	case EM2880_BOARD_EMPIRE_DUAL_TV:
 		dvb->fe[0] = dvb_attach(zl10353_attach,
 					   &em28xx_zl10353_xc3028_no_i2c_gate,
-					   &dev->i2c_adap);
+					   &dev->i2c_adap[dev->def_i2c_bus]);
 		if (em28xx_attach_xc3028(0x61, dev) < 0) {
 			result = -EINVAL;
 			goto out_free;
@@ -1009,13 +1027,13 @@ static int em28xx_dvb_init(struct em28xx *dev)
 	case EM2882_BOARD_KWORLD_VS_DVBT:
 		dvb->fe[0] = dvb_attach(zl10353_attach,
 					   &em28xx_zl10353_xc3028_no_i2c_gate,
-					   &dev->i2c_adap);
+					   &dev->i2c_adap[dev->def_i2c_bus]);
 		if (dvb->fe[0] == NULL) {
 			/* This board could have either a zl10353 or a mt352.
 			   If the chip id isn't for zl10353, try mt352 */
 			dvb->fe[0] = dvb_attach(mt352_attach,
 						   &terratec_xs_mt352_cfg,
-						   &dev->i2c_adap);
+						   &dev->i2c_adap[dev->def_i2c_bus]);
 		}
 
 		if (em28xx_attach_xc3028(0x61, dev) < 0) {
@@ -1026,16 +1044,16 @@ static int em28xx_dvb_init(struct em28xx *dev)
 	case EM2870_BOARD_KWORLD_355U:
 		dvb->fe[0] = dvb_attach(zl10353_attach,
 					   &em28xx_zl10353_no_i2c_gate_dev,
-					   &dev->i2c_adap);
+					   &dev->i2c_adap[dev->def_i2c_bus]);
 		if (dvb->fe[0] != NULL)
 			dvb_attach(qt1010_attach, dvb->fe[0],
-				   &dev->i2c_adap, &em28xx_qt1010_config);
+				   &dev->i2c_adap[dev->def_i2c_bus], &em28xx_qt1010_config);
 		break;
 	case EM2883_BOARD_KWORLD_HYBRID_330U:
 	case EM2882_BOARD_EVGA_INDTUBE:
 		dvb->fe[0] = dvb_attach(s5h1409_attach,
 					   &em28xx_s5h1409_with_xc3028,
-					   &dev->i2c_adap);
+					   &dev->i2c_adap[dev->def_i2c_bus]);
 		if (em28xx_attach_xc3028(0x61, dev) < 0) {
 			result = -EINVAL;
 			goto out_free;
@@ -1044,10 +1062,10 @@ static int em28xx_dvb_init(struct em28xx *dev)
 	case EM2882_BOARD_KWORLD_ATSC_315U:
 		dvb->fe[0] = dvb_attach(lgdt330x_attach,
 					   &em2880_lgdt3303_dev,
-					   &dev->i2c_adap);
+					   &dev->i2c_adap[dev->def_i2c_bus]);
 		if (dvb->fe[0] != NULL) {
 			if (!dvb_attach(simple_tuner_attach, dvb->fe[0],
-				&dev->i2c_adap, 0x61, TUNER_THOMSON_DTT761X)) {
+				&dev->i2c_adap[dev->def_i2c_bus], 0x61, TUNER_THOMSON_DTT761X)) {
 				result = -EINVAL;
 				goto out_free;
 			}
@@ -1056,7 +1074,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 	case EM2880_BOARD_HAUPPAUGE_WINTV_HVR_900_R2:
 	case EM2882_BOARD_PINNACLE_HYBRID_PRO_330E:
 		dvb->fe[0] = dvb_attach(drxd_attach, &em28xx_drxd, NULL,
-					   &dev->i2c_adap, &dev->udev->dev);
+					   &dev->i2c_adap[dev->def_i2c_bus], &dev->udev->dev);
 		if (em28xx_attach_xc3028(0x61, dev) < 0) {
 			result = -EINVAL;
 			goto out_free;
@@ -1066,10 +1084,10 @@ static int em28xx_dvb_init(struct em28xx *dev)
 		/* Philips CU1216L NIM (Philips TDA10023 + Infineon TUA6034) */
 		dvb->fe[0] = dvb_attach(tda10023_attach,
 			&em28xx_tda10023_config,
-			&dev->i2c_adap, 0x48);
+			&dev->i2c_adap[dev->def_i2c_bus], 0x48);
 		if (dvb->fe[0]) {
 			if (!dvb_attach(simple_tuner_attach, dvb->fe[0],
-				&dev->i2c_adap, 0x60, TUNER_PHILIPS_CU1216L)) {
+				&dev->i2c_adap[dev->def_i2c_bus], 0x60, TUNER_PHILIPS_CU1216L)) {
 				result = -EINVAL;
 				goto out_free;
 			}
@@ -1078,10 +1096,10 @@ static int em28xx_dvb_init(struct em28xx *dev)
 	case EM2870_BOARD_KWORLD_A340:
 		dvb->fe[0] = dvb_attach(lgdt3305_attach,
 					   &em2870_lgdt3304_dev,
-					   &dev->i2c_adap);
+					   &dev->i2c_adap[dev->def_i2c_bus]);
 		if (dvb->fe[0] != NULL)
 			dvb_attach(tda18271_attach, dvb->fe[0], 0x60,
-				   &dev->i2c_adap, &kworld_a340_config);
+				   &dev->i2c_adap[dev->def_i2c_bus], &kworld_a340_config);
 		break;
 	case EM28174_BOARD_PCTV_290E:
 		/* set default GPIO0 for LNA, used if GPIOLIB is undefined */
@@ -1089,14 +1107,14 @@ static int em28xx_dvb_init(struct em28xx *dev)
 				CXD2820R_GPIO_L;
 		dvb->fe[0] = dvb_attach(cxd2820r_attach,
 					&em28xx_cxd2820r_config,
-					&dev->i2c_adap,
+					&dev->i2c_adap[dev->def_i2c_bus],
 					&dvb->lna_gpio);
 		if (dvb->fe[0]) {
 			/* FE 0 attach tuner */
 			if (!dvb_attach(tda18271_attach,
 					dvb->fe[0],
 					0x60,
-					&dev->i2c_adap,
+					&dev->i2c_adap[dev->def_i2c_bus],
 					&em28xx_cxd2820r_tda18271_config)) {
 
 				dvb_frontend_detach(dvb->fe[0]);
@@ -1126,7 +1144,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 		hauppauge_hvr930c_init(dev);
 
 		dvb->fe[0] = dvb_attach(drxk_attach,
-					&hauppauge_930c_drxk, &dev->i2c_adap);
+					&hauppauge_930c_drxk, &dev->i2c_adap[dev->def_i2c_bus]);
 		if (!dvb->fe[0]) {
 			result = -EINVAL;
 			goto out_free;
@@ -1144,7 +1162,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 
 		if (dvb->fe[0]->ops.i2c_gate_ctrl)
 			dvb->fe[0]->ops.i2c_gate_ctrl(dvb->fe[0], 1);
-		if (!dvb_attach(xc5000_attach, dvb->fe[0], &dev->i2c_adap,
+		if (!dvb_attach(xc5000_attach, dvb->fe[0], &dev->i2c_adap[dev->def_i2c_bus],
 				&cfg)) {
 			result = -EINVAL;
 			goto out_free;
@@ -1157,7 +1175,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 	case EM2884_BOARD_TERRATEC_H5:
 		terratec_h5_init(dev);
 
-		dvb->fe[0] = dvb_attach(drxk_attach, &terratec_h5_drxk, &dev->i2c_adap);
+		dvb->fe[0] = dvb_attach(drxk_attach, &terratec_h5_drxk, &dev->i2c_adap[dev->def_i2c_bus]);
 		if (!dvb->fe[0]) {
 			result = -EINVAL;
 			goto out_free;
@@ -1171,7 +1189,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 		/* Attach tda18271 to DVB-C frontend */
 		if (dvb->fe[0]->ops.i2c_gate_ctrl)
 			dvb->fe[0]->ops.i2c_gate_ctrl(dvb->fe[0], 1);
-		if (!dvb_attach(tda18271c2dd_attach, dvb->fe[0], &dev->i2c_adap, 0x60)) {
+		if (!dvb_attach(tda18271c2dd_attach, dvb->fe[0], &dev->i2c_adap[dev->def_i2c_bus], 0x60)) {
 			result = -EINVAL;
 			goto out_free;
 		}
@@ -1179,20 +1197,29 @@ static int em28xx_dvb_init(struct em28xx *dev)
 			dvb->fe[0]->ops.i2c_gate_ctrl(dvb->fe[0], 0);
 
 		break;
+	case EM2884_BOARD_C3TECH_DIGITAL_DUO:
+		dvb->fe[0] = dvb_attach(mb86a20s_attach,
+					   &c3tech_duo_mb86a20s_config,
+					   &dev->i2c_adap[dev->def_i2c_bus]);
+		if (dvb->fe[0] != NULL)
+			dvb_attach(tda18271_attach, dvb->fe[0], 0x60,
+				   &dev->i2c_adap[dev->def_i2c_bus],
+				   &c3tech_duo_tda18271_config);
+		break;
 	case EM28174_BOARD_PCTV_460E:
 		/* attach demod */
 		dvb->fe[0] = dvb_attach(tda10071_attach,
-			&em28xx_tda10071_config, &dev->i2c_adap);
+			&em28xx_tda10071_config, &dev->i2c_adap[dev->def_i2c_bus]);
 
 		/* attach SEC */
 		if (dvb->fe[0])
-			dvb_attach(a8293_attach, dvb->fe[0], &dev->i2c_adap,
+			dvb_attach(a8293_attach, dvb->fe[0], &dev->i2c_adap[dev->def_i2c_bus],
 				&em28xx_a8293_config);
 		break;
 	case EM2874_BOARD_MAXMEDIA_UB425_TC:
 		/* attach demodulator */
 		dvb->fe[0] = dvb_attach(drxk_attach, &maxmedia_ub425_tc_drxk,
-				&dev->i2c_adap);
+				&dev->i2c_adap[dev->def_i2c_bus]);
 
 		if (dvb->fe[0]) {
 			/* disable I2C-gate */
@@ -1200,7 +1227,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 
 			/* attach tuner */
 			if (!dvb_attach(tda18271c2dd_attach, dvb->fe[0],
-					&dev->i2c_adap, 0x60)) {
+					&dev->i2c_adap[dev->def_i2c_bus], 0x60)) {
 				dvb_frontend_detach(dvb->fe[0]);
 				result = -EINVAL;
 				goto out_free;
@@ -1218,12 +1245,12 @@ static int em28xx_dvb_init(struct em28xx *dev)
 
 		/* attach demodulator */
 		dvb->fe[0] = dvb_attach(drxk_attach, &pctv_520e_drxk,
-				&dev->i2c_adap);
+				&dev->i2c_adap[dev->def_i2c_bus]);
 
 		if (dvb->fe[0]) {
 			/* attach tuner */
 			if (!dvb_attach(tda18271_attach, dvb->fe[0], 0x60,
-					&dev->i2c_adap,
+					&dev->i2c_adap[dev->def_i2c_bus],
 					&em28xx_cxd2820r_tda18271_config)) {
 				dvb_frontend_detach(dvb->fe[0]);
 				result = -EINVAL;
@@ -1236,7 +1263,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 
 		/* attach demodulator */
 		dvb->fe[0] = dvb_attach(drxk_attach, &terratec_htc_stick_drxk,
-					&dev->i2c_adap);
+					&dev->i2c_adap[dev->def_i2c_bus]);
 		if (!dvb->fe[0]) {
 			result = -EINVAL;
 			goto out_free;
@@ -1244,7 +1271,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 
 		/* Attach the demodulator. */
 		if (!dvb_attach(tda18271_attach, dvb->fe[0], 0x60,
-				&dev->i2c_adap,
+				&dev->i2c_adap[dev->def_i2c_bus],
 				&em28xx_cxd2820r_tda18271_config)) {
 			result = -EINVAL;
 			goto out_free;
@@ -1255,7 +1282,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 
 		/* attach demodulator */
 		dvb->fe[0] = dvb_attach(drxk_attach, &terratec_htc_stick_drxk,
-					&dev->i2c_adap);
+					&dev->i2c_adap[dev->def_i2c_bus]);
 		if (!dvb->fe[0]) {
 			result = -EINVAL;
 			goto out_free;
@@ -1263,7 +1290,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 
 		/* Attach the demodulator. */
 		if (!dvb_attach(tda18271_attach, dvb->fe[0], 0x60,
-				&dev->i2c_adap,
+				&dev->i2c_adap[dev->def_i2c_bus],
 				&em28xx_cxd2820r_tda18271_config)) {
 			result = -EINVAL;
 			goto out_free;
diff --git a/drivers/media/usb/em28xx/em28xx-i2c.c b/drivers/media/usb/em28xx/em28xx-i2c.c
index 8532c1d4fd4..4851cc2e4a4 100644
--- a/drivers/media/usb/em28xx/em28xx-i2c.c
+++ b/drivers/media/usb/em28xx/em28xx-i2c.c
@@ -5,6 +5,7 @@
 		      Markus Rechberger <mrechberger@gmail.com>
 		      Mauro Carvalho Chehab <mchehab@infradead.org>
 		      Sascha Sommer <saschasommer@freenet.de>
+   Copyright (C) 2013 Frank Schäfer <fschaefer.oss@googlemail.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
@@ -41,14 +42,6 @@ static unsigned int i2c_debug;
 module_param(i2c_debug, int, 0644);
 MODULE_PARM_DESC(i2c_debug, "enable debug messages [i2c]");
 
-#define dprintk2(lvl, fmt, args...)			\
-do {							\
-	if (i2c_debug >= lvl) {				\
-		printk(KERN_DEBUG "%s at %s: " fmt,	\
-		       dev->name, __func__ , ##args);	\
-      } 						\
-} while (0)
-
 /*
  * em2800_i2c_send_bytes()
  * send up to 4 bytes to the em2800 i2c device
@@ -76,8 +69,8 @@ static int em2800_i2c_send_bytes(struct em28xx *dev, u8 addr, u8 *buf, u16 len)
 	/* trigger write */
 	ret = dev->em28xx_write_regs(dev, 4 - len, &b2[4 - len], 2 + len);
 	if (ret != 2 + len) {
-		em28xx_warn("failed to trigger write to i2c address 0x%x "
-			    "(error=%i)\n", addr, ret);
+		em28xx_warn("failed to trigger write to i2c address 0x%x (error=%i)\n",
+			    addr, ret);
 		return (ret < 0) ? ret : -EIO;
 	}
 	/* wait for completion */
@@ -89,8 +82,8 @@ static int em2800_i2c_send_bytes(struct em28xx *dev, u8 addr, u8 *buf, u16 len)
 		} else if (ret == 0x94 + len - 1) {
 			return -ENODEV;
 		} else if (ret < 0) {
-			em28xx_warn("failed to get i2c transfer status from "
-				    "bridge register (error=%i)\n", ret);
+			em28xx_warn("failed to get i2c transfer status from bridge register (error=%i)\n",
+				    ret);
 			return ret;
 		}
 		msleep(5);
@@ -118,8 +111,8 @@ static int em2800_i2c_recv_bytes(struct em28xx *dev, u8 addr, u8 *buf, u16 len)
 	buf2[0] = addr;
 	ret = dev->em28xx_write_regs(dev, 0x04, buf2, 2);
 	if (ret != 2) {
-		em28xx_warn("failed to trigger read from i2c address 0x%x "
-			    "(error=%i)\n", addr, ret);
+		em28xx_warn("failed to trigger read from i2c address 0x%x (error=%i)\n",
+			    addr, ret);
 		return (ret < 0) ? ret : -EIO;
 	}
 
@@ -132,8 +125,8 @@ static int em2800_i2c_recv_bytes(struct em28xx *dev, u8 addr, u8 *buf, u16 len)
 		} else if (ret == 0x94 + len - 1) {
 			return -ENODEV;
 		} else if (ret < 0) {
-			em28xx_warn("failed to get i2c transfer status from "
-				    "bridge register (error=%i)\n", ret);
+			em28xx_warn("failed to get i2c transfer status from bridge register (error=%i)\n",
+				    ret);
 			return ret;
 		}
 		msleep(5);
@@ -144,9 +137,8 @@ static int em2800_i2c_recv_bytes(struct em28xx *dev, u8 addr, u8 *buf, u16 len)
 	/* get the received message */
 	ret = dev->em28xx_read_reg_req_len(dev, 0x00, 4-len, buf2, len);
 	if (ret != len) {
-		em28xx_warn("reading from i2c device at 0x%x failed: "
-			    "couldn't get the received message from the bridge "
-			    "(error=%i)\n", addr, ret);
+		em28xx_warn("reading from i2c device at 0x%x failed: couldn't get the received message from the bridge (error=%i)\n",
+			    addr, ret);
 		return (ret < 0) ? ret : -EIO;
 	}
 	for (i = 0; i < len; i++)
@@ -180,19 +172,20 @@ static int em28xx_i2c_send_bytes(struct em28xx *dev, u16 addr, u8 *buf,
 
 	if (len < 1 || len > 64)
 		return -EOPNOTSUPP;
-	/* NOTE: limited by the USB ctrl message constraints
-	 * Zero length reads always succeed, even if no device is connected */
+	/*
+	 * NOTE: limited by the USB ctrl message constraints
+	 * Zero length reads always succeed, even if no device is connected
+	 */
 
 	/* Write to i2c device */
 	ret = dev->em28xx_write_regs_req(dev, stop ? 2 : 3, addr, buf, len);
 	if (ret != len) {
 		if (ret < 0) {
-			em28xx_warn("writing to i2c device at 0x%x failed "
-				    "(error=%i)\n", addr, ret);
+			em28xx_warn("writing to i2c device at 0x%x failed (error=%i)\n",
+				    addr, ret);
 			return ret;
 		} else {
-			em28xx_warn("%i bytes write to i2c device at 0x%x "
-				    "requested, but %i bytes written\n",
+			em28xx_warn("%i bytes write to i2c device at 0x%x requested, but %i bytes written\n",
 				    len, addr, ret);
 			return -EIO;
 		}
@@ -207,14 +200,16 @@ static int em28xx_i2c_send_bytes(struct em28xx *dev, u16 addr, u8 *buf,
 		} else if (ret == 0x10) {
 			return -ENODEV;
 		} else if (ret < 0) {
-			em28xx_warn("failed to read i2c transfer status from "
-				    "bridge (error=%i)\n", ret);
+			em28xx_warn("failed to read i2c transfer status from bridge (error=%i)\n",
+				    ret);
 			return ret;
 		}
 		msleep(5);
-		/* NOTE: do we really have to wait for success ?
-		   Never seen anything else than 0x00 or 0x10
-		   (even with high payload) ...			*/
+		/*
+		 * NOTE: do we really have to wait for success ?
+		 * Never seen anything else than 0x00 or 0x10
+		 * (even with high payload) ...
+		 */
 	}
 	em28xx_warn("write to i2c device at 0x%x timed out\n", addr);
 	return -EIO;
@@ -230,29 +225,32 @@ static int em28xx_i2c_recv_bytes(struct em28xx *dev, u16 addr, u8 *buf, u16 len)
 
 	if (len < 1 || len > 64)
 		return -EOPNOTSUPP;
-	/* NOTE: limited by the USB ctrl message constraints
-	 * Zero length reads always succeed, even if no device is connected */
+	/*
+	 * NOTE: limited by the USB ctrl message constraints
+	 * Zero length reads always succeed, even if no device is connected
+	 */
 
 	/* Read data from i2c device */
 	ret = dev->em28xx_read_reg_req_len(dev, 2, addr, buf, len);
-	if (ret != len) {
-		if (ret < 0) {
-			em28xx_warn("reading from i2c device at 0x%x failed "
-				    "(error=%i)\n", addr, ret);
-			return ret;
-		} else {
-			em28xx_warn("%i bytes requested from i2c device at "
-				    "0x%x, but %i bytes received\n",
-				    len, addr, ret);
-			return -EIO;
-		}
+	if (ret < 0) {
+		em28xx_warn("reading from i2c device at 0x%x failed (error=%i)\n",
+			    addr, ret);
+		return ret;
 	}
+	/*
+	 * NOTE: some devices with two i2c busses have the bad habit to return 0
+	 * bytes if we are on bus B AND there was no write attempt to the
+	 * specified slave address before AND no device is present at the
+	 * requested slave address.
+	 * Anyway, the next check will fail with -ENODEV in this case, so avoid
+	 * spamming the system log on device probing and do nothing here.
+	 */
 
 	/* Check success of the i2c operation */
 	ret = dev->em28xx_read_reg(dev, 0x05);
 	if (ret < 0) {
-		em28xx_warn("failed to read i2c transfer status from "
-			    "bridge (error=%i)\n", ret);
+		em28xx_warn("failed to read i2c transfer status from bridge (error=%i)\n",
+			    ret);
 		return ret;
 	}
 	if (ret > 0) {
@@ -282,77 +280,254 @@ static int em28xx_i2c_check_for_device(struct em28xx *dev, u16 addr)
 }
 
 /*
+ * em25xx_bus_B_send_bytes
+ * write bytes to the i2c device
+ */
+static int em25xx_bus_B_send_bytes(struct em28xx *dev, u16 addr, u8 *buf,
+				   u16 len)
+{
+	int ret;
+
+	if (len < 1 || len > 64)
+		return -EOPNOTSUPP;
+	/*
+	 * NOTE: limited by the USB ctrl message constraints
+	 * Zero length reads always succeed, even if no device is connected
+	 */
+
+	/* Set register and write value */
+	ret = dev->em28xx_write_regs_req(dev, 0x06, addr, buf, len);
+	if (ret != len) {
+		if (ret < 0) {
+			em28xx_warn("writing to i2c device at 0x%x failed (error=%i)\n",
+				    addr, ret);
+			return ret;
+		} else {
+			em28xx_warn("%i bytes write to i2c device at 0x%x requested, but %i bytes written\n",
+				    len, addr, ret);
+			return -EIO;
+		}
+	}
+	/* Check success */
+	ret = dev->em28xx_read_reg_req(dev, 0x08, 0x0000);
+	/*
+	 * NOTE: the only error we've seen so far is
+	 * 0x01 when the slave device is not present
+	 */
+	if (!ret)
+		return len;
+	else if (ret > 0)
+		return -ENODEV;
+
+	return ret;
+	/*
+	 * NOTE: With chip types (other chip IDs) which actually don't support
+	 * this operation, it seems to succeed ALWAYS ! (even if there is no
+	 * slave device or even no second i2c bus provided)
+	 */
+}
+
+/*
+ * em25xx_bus_B_recv_bytes
+ * read bytes from the i2c device
+ */
+static int em25xx_bus_B_recv_bytes(struct em28xx *dev, u16 addr, u8 *buf,
+				   u16 len)
+{
+	int ret;
+
+	if (len < 1 || len > 64)
+		return -EOPNOTSUPP;
+	/*
+	 * NOTE: limited by the USB ctrl message constraints
+	 * Zero length reads always succeed, even if no device is connected
+	 */
+
+	/* Read value */
+	ret = dev->em28xx_read_reg_req_len(dev, 0x06, addr, buf, len);
+	if (ret < 0) {
+		em28xx_warn("reading from i2c device at 0x%x failed (error=%i)\n",
+			    addr, ret);
+		return ret;
+	}
+	/*
+	 * NOTE: some devices with two i2c busses have the bad habit to return 0
+	 * bytes if we are on bus B AND there was no write attempt to the
+	 * specified slave address before AND no device is present at the
+	 * requested slave address.
+	 * Anyway, the next check will fail with -ENODEV in this case, so avoid
+	 * spamming the system log on device probing and do nothing here.
+	 */
+
+	/* Check success */
+	ret = dev->em28xx_read_reg_req(dev, 0x08, 0x0000);
+	/*
+	 * NOTE: the only error we've seen so far is
+	 * 0x01 when the slave device is not present
+	 */
+	if (!ret)
+		return len;
+	else if (ret > 0)
+		return -ENODEV;
+
+	return ret;
+	/*
+	 * NOTE: With chip types (other chip IDs) which actually don't support
+	 * this operation, it seems to succeed ALWAYS ! (even if there is no
+	 * slave device or even no second i2c bus provided)
+	 */
+}
+
+/*
+ * em25xx_bus_B_check_for_device()
+ * check if there is a i2c device at the supplied address
+ */
+static int em25xx_bus_B_check_for_device(struct em28xx *dev, u16 addr)
+{
+	u8 buf;
+	int ret;
+
+	ret = em25xx_bus_B_recv_bytes(dev, addr, &buf, 1);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+	/*
+	 * NOTE: With chips which do not support this operation,
+	 * it seems to succeed ALWAYS ! (even if no device connected)
+	 */
+}
+
+static inline int i2c_check_for_device(struct em28xx_i2c_bus *i2c_bus, u16 addr)
+{
+	struct em28xx *dev = i2c_bus->dev;
+	int rc = -EOPNOTSUPP;
+
+	if (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM28XX)
+		rc = em28xx_i2c_check_for_device(dev, addr);
+	else if (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM2800)
+		rc = em2800_i2c_check_for_device(dev, addr);
+	else if (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM25XX_BUS_B)
+		rc = em25xx_bus_B_check_for_device(dev, addr);
+	if (rc == -ENODEV) {
+		if (i2c_debug)
+			printk(" no device\n");
+	}
+	return rc;
+}
+
+static inline int i2c_recv_bytes(struct em28xx_i2c_bus *i2c_bus,
+				 struct i2c_msg msg)
+{
+	struct em28xx *dev = i2c_bus->dev;
+	u16 addr = msg.addr << 1;
+	int byte, rc = -EOPNOTSUPP;
+
+	if (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM28XX)
+		rc = em28xx_i2c_recv_bytes(dev, addr, msg.buf, msg.len);
+	else if (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM2800)
+		rc = em2800_i2c_recv_bytes(dev, addr, msg.buf, msg.len);
+	else if (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM25XX_BUS_B)
+		rc = em25xx_bus_B_recv_bytes(dev, addr, msg.buf, msg.len);
+	if (i2c_debug) {
+		for (byte = 0; byte < msg.len; byte++)
+			printk(" %02x", msg.buf[byte]);
+	}
+	return rc;
+}
+
+static inline int i2c_send_bytes(struct em28xx_i2c_bus *i2c_bus,
+				 struct i2c_msg msg, int stop)
+{
+	struct em28xx *dev = i2c_bus->dev;
+	u16 addr = msg.addr << 1;
+	int byte, rc = -EOPNOTSUPP;
+
+	if (i2c_debug) {
+		for (byte = 0; byte < msg.len; byte++)
+			printk(" %02x", msg.buf[byte]);
+	}
+	if (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM28XX)
+		rc = em28xx_i2c_send_bytes(dev, addr, msg.buf, msg.len, stop);
+	else if (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM2800)
+		rc = em2800_i2c_send_bytes(dev, addr, msg.buf, msg.len);
+	else if (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM25XX_BUS_B)
+		rc = em25xx_bus_B_send_bytes(dev, addr, msg.buf, msg.len);
+	return rc;
+}
+
+/*
  * em28xx_i2c_xfer()
  * the main i2c transfer function
  */
 static int em28xx_i2c_xfer(struct i2c_adapter *i2c_adap,
 			   struct i2c_msg msgs[], int num)
 {
-	struct em28xx *dev = i2c_adap->algo_data;
-	int addr, rc, i, byte;
+	struct em28xx_i2c_bus *i2c_bus = i2c_adap->algo_data;
+	struct em28xx *dev = i2c_bus->dev;
+	unsigned bus = i2c_bus->bus;
+	int addr, rc, i;
+	u8 reg;
+
+	rc = rt_mutex_trylock(&dev->i2c_bus_lock);
+	if (rc < 0)
+		return rc;
+
+	/* Switch I2C bus if needed */
+	if (bus != dev->cur_i2c_bus &&
+	    i2c_bus->algo_type == EM28XX_I2C_ALGO_EM28XX) {
+		if (bus == 1)
+			reg = EM2874_I2C_SECONDARY_BUS_SELECT;
+		else
+			reg = 0;
+		em28xx_write_reg_bits(dev, EM28XX_R06_I2C_CLK, reg,
+				      EM2874_I2C_SECONDARY_BUS_SELECT);
+		dev->cur_i2c_bus = bus;
+	}
 
-	if (num <= 0)
+	if (num <= 0) {
+		rt_mutex_unlock(&dev->i2c_bus_lock);
 		return 0;
+	}
 	for (i = 0; i < num; i++) {
 		addr = msgs[i].addr << 1;
-		dprintk2(2, "%s %s addr=%x len=%d:",
-			 (msgs[i].flags & I2C_M_RD) ? "read" : "write",
-			 i == num - 1 ? "stop" : "nonstop", addr, msgs[i].len);
+		if (i2c_debug)
+			printk(KERN_DEBUG "%s at %s: %s %s addr=%02x len=%d:",
+			       dev->name, __func__ ,
+			       (msgs[i].flags & I2C_M_RD) ? "read" : "write",
+			       i == num - 1 ? "stop" : "nonstop",
+			       addr, msgs[i].len);
 		if (!msgs[i].len) { /* no len: check only for device presence */
-			if (dev->board.is_em2800)
-				rc = em2800_i2c_check_for_device(dev, addr);
-			else
-				rc = em28xx_i2c_check_for_device(dev, addr);
+			rc = i2c_check_for_device(i2c_bus, addr);
 			if (rc == -ENODEV) {
-				if (i2c_debug >= 2)
-					printk(" no device\n");
+				rt_mutex_unlock(&dev->i2c_bus_lock);
 				return rc;
 			}
 		} else if (msgs[i].flags & I2C_M_RD) {
 			/* read bytes */
-			if (dev->board.is_em2800)
-				rc = em2800_i2c_recv_bytes(dev, addr,
-							   msgs[i].buf,
-							   msgs[i].len);
-			else
-				rc = em28xx_i2c_recv_bytes(dev, addr,
-							   msgs[i].buf,
-							   msgs[i].len);
-			if (i2c_debug >= 2) {
-				for (byte = 0; byte < msgs[i].len; byte++)
-					printk(" %02x", msgs[i].buf[byte]);
-			}
+			rc = i2c_recv_bytes(i2c_bus, msgs[i]);
 		} else {
 			/* write bytes */
-			if (i2c_debug >= 2) {
-				for (byte = 0; byte < msgs[i].len; byte++)
-					printk(" %02x", msgs[i].buf[byte]);
-			}
-			if (dev->board.is_em2800)
-				rc = em2800_i2c_send_bytes(dev, addr,
-							   msgs[i].buf,
-							   msgs[i].len);
-			else
-				rc = em28xx_i2c_send_bytes(dev, addr,
-							   msgs[i].buf,
-							   msgs[i].len,
-							   i == num - 1);
+			rc = i2c_send_bytes(i2c_bus, msgs[i], i == num - 1);
 		}
 		if (rc < 0) {
-			if (i2c_debug >= 2)
+			if (i2c_debug)
 				printk(" ERROR: %i\n", rc);
+			rt_mutex_unlock(&dev->i2c_bus_lock);
 			return rc;
 		}
-		if (i2c_debug >= 2)
+		if (i2c_debug)
 			printk("\n");
 	}
 
+	rt_mutex_unlock(&dev->i2c_bus_lock);
 	return num;
 }
 
-/* based on linux/sunrpc/svcauth.h and linux/hash.h
+/*
+ * based on linux/sunrpc/svcauth.h and linux/hash.h
  * The original hash function returns a different value, if arch is x86_64
- *  or i386.
+ * or i386.
  */
 static inline unsigned long em28xx_hash_mem(char *buf, int length, int bits)
 {
@@ -375,127 +550,230 @@ static inline unsigned long em28xx_hash_mem(char *buf, int length, int bits)
 	return (hash >> (32 - bits)) & 0xffffffffUL;
 }
 
-static int em28xx_i2c_eeprom(struct em28xx *dev, unsigned char *eedata, int len)
+/*
+ * Helper function to read data blocks from i2c clients with 8 or 16 bit
+ * address width, 8 bit register width and auto incrementation been activated
+ */
+static int em28xx_i2c_read_block(struct em28xx *dev, unsigned bus, u16 addr,
+				 bool addr_w16, u16 len, u8 *data)
 {
-	unsigned char buf, *p = eedata;
-	struct em28xx_eeprom *em_eeprom = (void *)eedata;
-	int i, err, size = len, block, block_max;
-
-	if (dev->chip_id == CHIP_ID_EM2874 ||
-	    dev->chip_id == CHIP_ID_EM28174 ||
-	    dev->chip_id == CHIP_ID_EM2884) {
-		/* Empia switched to a 16-bit addressable eeprom in newer
-		   devices.  While we could certainly write a routine to read
-		   the eeprom, there is nothing of use in there that cannot be
-		   accessed through registers, and there is the risk that we
-		   could corrupt the eeprom (since a 16-bit read call is
-		   interpreted as a write call by 8-bit eeproms).
-		*/
-		return 0;
+	int remain = len, rsize, rsize_max, ret;
+	u8 buf[2];
+
+	/* Sanity check */
+	if (addr + remain > (addr_w16 * 0xff00 + 0xff + 1))
+		return -EINVAL;
+	/* Select address */
+	buf[0] = addr >> 8;
+	buf[1] = addr & 0xff;
+	ret = i2c_master_send(&dev->i2c_client[bus], buf + !addr_w16, 1 + addr_w16);
+	if (ret < 0)
+		return ret;
+	/* Read data */
+	if (dev->board.is_em2800)
+		rsize_max = 4;
+	else
+		rsize_max = 64;
+	while (remain > 0) {
+		if (remain > rsize_max)
+			rsize = rsize_max;
+		else
+			rsize = remain;
+
+		ret = i2c_master_recv(&dev->i2c_client[bus], data, rsize);
+		if (ret < 0)
+			return ret;
+
+		remain -= rsize;
+		data += rsize;
 	}
 
-	dev->i2c_client.addr = 0xa0 >> 1;
+	return len;
+}
+
+static int em28xx_i2c_eeprom(struct em28xx *dev, unsigned bus,
+			     u8 **eedata, u16 *eedata_len)
+{
+	const u16 len = 256;
+	/*
+	 * FIXME common length/size for bytes to read, to display, hash
+	 * calculation and returned device dataset. Simplifies the code a lot,
+	 * but we might have to deal with multiple sizes in the future !
+	 */
+	int i, err;
+	struct em28xx_eeprom *dev_config;
+	u8 buf, *data;
+
+	*eedata = NULL;
+	*eedata_len = 0;
+
+	/* EEPROM is always on i2c bus 0 on all known devices. */
+
+	dev->i2c_client[bus].addr = 0xa0 >> 1;
 
 	/* Check if board has eeprom */
-	err = i2c_master_recv(&dev->i2c_client, &buf, 0);
+	err = i2c_master_recv(&dev->i2c_client[bus], &buf, 0);
 	if (err < 0) {
-		em28xx_errdev("board has no eeprom\n");
-		memset(eedata, 0, len);
+		em28xx_info("board has no eeprom\n");
 		return -ENODEV;
 	}
 
-	buf = 0;
-
-	err = i2c_master_send(&dev->i2c_client, &buf, 1);
-	if (err != 1) {
-		printk(KERN_INFO "%s: Huh, no eeprom present (err=%d)?\n",
-		       dev->name, err);
-		return err;
+	data = kzalloc(len, GFP_KERNEL);
+	if (data == NULL)
+		return -ENOMEM;
+
+	/* Read EEPROM content */
+	err = em28xx_i2c_read_block(dev, bus, 0x0000,
+				    dev->eeprom_addrwidth_16bit,
+				    len, data);
+	if (err != len) {
+		em28xx_errdev("failed to read eeprom (err=%d)\n", err);
+		goto error;
 	}
 
-	if (dev->board.is_em2800)
-		block_max = 4;
-	else
-		block_max = 64;
-
-	while (size > 0) {
-		if (size > block_max)
-			block = block_max;
-		else
-			block = size;
-
-		if (block !=
-		    (err = i2c_master_recv(&dev->i2c_client, p, block))) {
-			printk(KERN_WARNING
-			       "%s: i2c eeprom read error (err=%d)\n",
-			       dev->name, err);
-			return err;
-		}
-		size -= block;
-		p += block;
-	}
+	/* Display eeprom content */
 	for (i = 0; i < len; i++) {
-		if (0 == (i % 16))
-			printk(KERN_INFO "%s: i2c eeprom %02x:", dev->name, i);
-		printk(" %02x", eedata[i]);
+		if (0 == (i % 16)) {
+			if (dev->eeprom_addrwidth_16bit)
+				em28xx_info("i2c eeprom %04x:", i);
+			else
+				em28xx_info("i2c eeprom %02x:", i);
+		}
+		printk(" %02x", data[i]);
 		if (15 == (i % 16))
 			printk("\n");
 	}
+	if (dev->eeprom_addrwidth_16bit)
+		em28xx_info("i2c eeprom %04x: ... (skipped)\n", i);
+
+	if (dev->eeprom_addrwidth_16bit &&
+	    data[0] == 0x26 && data[3] == 0x00) {
+		/* new eeprom format; size 4-64kb */
+		u16 mc_start;
+		u16 hwconf_offset;
+
+		dev->hash = em28xx_hash_mem(data, len, 32);
+		mc_start = (data[1] << 8) + 4;	/* usually 0x0004 */
+
+		em28xx_info("EEPROM ID = %02x %02x %02x %02x, EEPROM hash = 0x%08lx\n",
+			    data[0], data[1], data[2], data[3], dev->hash);
+		em28xx_info("EEPROM info:\n");
+		em28xx_info("\tmicrocode start address = 0x%04x, boot configuration = 0x%02x\n",
+			    mc_start, data[2]);
+		/*
+		 * boot configuration (address 0x0002):
+		 * [0]   microcode download speed: 1 = 400 kHz; 0 = 100 kHz
+		 * [1]   always selects 12 kb RAM
+		 * [2]   USB device speed: 1 = force Full Speed; 0 = auto detect
+		 * [4]   1 = force fast mode and no suspend for device testing
+		 * [5:7] USB PHY tuning registers; determined by device
+		 *       characterization
+		 */
+
+		/*
+		 * Read hardware config dataset offset from address
+		 * (microcode start + 46)
+		 */
+		err = em28xx_i2c_read_block(dev, bus, mc_start + 46, 1, 2,
+					    data);
+		if (err != 2) {
+			em28xx_errdev("failed to read hardware configuration data from eeprom (err=%d)\n",
+				      err);
+			goto error;
+		}
 
-	if (em_eeprom->id == 0x9567eb1a)
-		dev->hash = em28xx_hash_mem(eedata, len, 32);
+		/* Calculate hardware config dataset start address */
+		hwconf_offset = mc_start + data[0] + (data[1] << 8);
+
+		/* Read hardware config dataset */
+		/*
+		 * NOTE: the microcode copy can be multiple pages long, but
+		 * we assume the hardware config dataset is the same as in
+		 * the old eeprom and not longer than 256 bytes.
+		 * tveeprom is currently also limited to 256 bytes.
+		 */
+		err = em28xx_i2c_read_block(dev, bus, hwconf_offset, 1, len,
+					    data);
+		if (err != len) {
+			em28xx_errdev("failed to read hardware configuration data from eeprom (err=%d)\n",
+				      err);
+			goto error;
+		}
 
-	printk(KERN_INFO "%s: EEPROM ID= 0x%08x, EEPROM hash = 0x%08lx\n",
-	       dev->name, em_eeprom->id, dev->hash);
+		/* Verify hardware config dataset */
+		/* NOTE: not all devices provide this type of dataset */
+		if (data[0] != 0x1a || data[1] != 0xeb ||
+		    data[2] != 0x67 || data[3] != 0x95) {
+			em28xx_info("\tno hardware configuration dataset found in eeprom\n");
+			kfree(data);
+			return 0;
+		}
 
-	printk(KERN_INFO "%s: EEPROM info:\n", dev->name);
+		/* TODO: decrypt eeprom data for camera bridges (em25xx, em276x+) */
+
+	} else if (!dev->eeprom_addrwidth_16bit &&
+		   data[0] == 0x1a && data[1] == 0xeb &&
+		   data[2] == 0x67 && data[3] == 0x95) {
+		dev->hash = em28xx_hash_mem(data, len, 32);
+		em28xx_info("EEPROM ID = %02x %02x %02x %02x, EEPROM hash = 0x%08lx\n",
+			    data[0], data[1], data[2], data[3], dev->hash);
+		em28xx_info("EEPROM info:\n");
+	} else {
+		em28xx_info("unknown eeprom format or eeprom corrupted !\n");
+		err = -ENODEV;
+		goto error;
+	}
 
-	switch (em_eeprom->chip_conf >> 4 & 0x3) {
+	*eedata = data;
+	*eedata_len = len;
+	dev_config = (void *)eedata;
+
+	switch (le16_to_cpu(dev_config->chip_conf) >> 4 & 0x3) {
 	case 0:
-		printk(KERN_INFO "%s:\tNo audio on board.\n", dev->name);
+		em28xx_info("\tNo audio on board.\n");
 		break;
 	case 1:
-		printk(KERN_INFO "%s:\tAC97 audio (5 sample rates)\n",
-				 dev->name);
+		em28xx_info("\tAC97 audio (5 sample rates)\n");
 		break;
 	case 2:
-		printk(KERN_INFO "%s:\tI2S audio, sample rate=32k\n",
-				 dev->name);
+		em28xx_info("\tI2S audio, sample rate=32k\n");
 		break;
 	case 3:
-		printk(KERN_INFO "%s:\tI2S audio, 3 sample rates\n",
-				 dev->name);
+		em28xx_info("\tI2S audio, 3 sample rates\n");
 		break;
 	}
 
-	if (em_eeprom->chip_conf & 1 << 3)
-		printk(KERN_INFO "%s:\tUSB Remote wakeup capable\n", dev->name);
+	if (le16_to_cpu(dev_config->chip_conf) & 1 << 3)
+		em28xx_info("\tUSB Remote wakeup capable\n");
 
-	if (em_eeprom->chip_conf & 1 << 2)
-		printk(KERN_INFO "%s:\tUSB Self power capable\n", dev->name);
+	if (le16_to_cpu(dev_config->chip_conf) & 1 << 2)
+		em28xx_info("\tUSB Self power capable\n");
 
-	switch (em_eeprom->chip_conf & 0x3) {
+	switch (le16_to_cpu(dev_config->chip_conf) & 0x3) {
 	case 0:
-		printk(KERN_INFO "%s:\t500mA max power\n", dev->name);
+		em28xx_info("\t500mA max power\n");
 		break;
 	case 1:
-		printk(KERN_INFO "%s:\t400mA max power\n", dev->name);
+		em28xx_info("\t400mA max power\n");
 		break;
 	case 2:
-		printk(KERN_INFO "%s:\t300mA max power\n", dev->name);
+		em28xx_info("\t300mA max power\n");
 		break;
 	case 3:
-		printk(KERN_INFO "%s:\t200mA max power\n", dev->name);
+		em28xx_info("\t200mA max power\n");
 		break;
 	}
-	printk(KERN_INFO "%s:\tTable at 0x%02x, strings=0x%04x, 0x%04x, 0x%04x\n",
-				dev->name,
-				em_eeprom->string_idx_table,
-				em_eeprom->string1,
-				em_eeprom->string2,
-				em_eeprom->string3);
+	em28xx_info("\tTable at offset 0x%02x, strings=0x%04x, 0x%04x, 0x%04x\n",
+		    dev_config->string_idx_table,
+		    le16_to_cpu(dev_config->string1),
+		    le16_to_cpu(dev_config->string2),
+		    le16_to_cpu(dev_config->string3));
 
 	return 0;
+
+error:
+	kfree(data);
+	return err;
 }
 
 /* ----------------------------------------------------------- */
@@ -503,13 +781,20 @@ static int em28xx_i2c_eeprom(struct em28xx *dev, unsigned char *eedata, int len)
 /*
  * functionality()
  */
-static u32 functionality(struct i2c_adapter *adap)
+static u32 functionality(struct i2c_adapter *i2c_adap)
 {
-	struct em28xx *dev = adap->algo_data;
-	u32 func_flags = I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
-	if (dev->board.is_em2800)
-		func_flags &= ~I2C_FUNC_SMBUS_WRITE_BLOCK_DATA;
-	return func_flags;
+	struct em28xx_i2c_bus *i2c_bus = i2c_adap->algo_data;
+
+	if ((i2c_bus->algo_type == EM28XX_I2C_ALGO_EM28XX) ||
+	    (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM25XX_BUS_B)) {
+		return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+	} else if (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM2800)  {
+		return (I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL) &
+			~I2C_FUNC_SMBUS_WRITE_BLOCK_DATA;
+	}
+
+	WARN(1, "Unknown i2c bus algorithm.\n");
+	return 0;
 }
 
 static struct i2c_algorithm em28xx_algo = {
@@ -556,7 +841,7 @@ static char *i2c_devs[128] = {
  * do_i2c_scan()
  * check i2c address range for devices
  */
-void em28xx_do_i2c_scan(struct em28xx *dev)
+void em28xx_do_i2c_scan(struct em28xx *dev, unsigned bus)
 {
 	u8 i2c_devicelist[128];
 	unsigned char buf;
@@ -565,55 +850,68 @@ void em28xx_do_i2c_scan(struct em28xx *dev)
 	memset(i2c_devicelist, 0, ARRAY_SIZE(i2c_devicelist));
 
 	for (i = 0; i < ARRAY_SIZE(i2c_devs); i++) {
-		dev->i2c_client.addr = i;
-		rc = i2c_master_recv(&dev->i2c_client, &buf, 0);
+		dev->i2c_client[bus].addr = i;
+		rc = i2c_master_recv(&dev->i2c_client[bus], &buf, 0);
 		if (rc < 0)
 			continue;
 		i2c_devicelist[i] = i;
-		printk(KERN_INFO "%s: found i2c device @ 0x%x [%s]\n",
-		       dev->name, i << 1, i2c_devs[i] ? i2c_devs[i] : "???");
+		em28xx_info("found i2c device @ 0x%x on bus %d [%s]\n",
+			    i << 1, bus, i2c_devs[i] ? i2c_devs[i] : "???");
 	}
 
-	dev->i2c_hash = em28xx_hash_mem(i2c_devicelist,
-					ARRAY_SIZE(i2c_devicelist), 32);
+	if (bus == dev->def_i2c_bus)
+		dev->i2c_hash = em28xx_hash_mem(i2c_devicelist,
+						ARRAY_SIZE(i2c_devicelist), 32);
 }
 
 /*
  * em28xx_i2c_register()
  * register i2c bus
  */
-int em28xx_i2c_register(struct em28xx *dev)
+int em28xx_i2c_register(struct em28xx *dev, unsigned bus,
+			enum em28xx_i2c_algo_type algo_type)
 {
 	int retval;
 
 	BUG_ON(!dev->em28xx_write_regs || !dev->em28xx_read_reg);
 	BUG_ON(!dev->em28xx_write_regs_req || !dev->em28xx_read_reg_req);
-	dev->i2c_adap = em28xx_adap_template;
-	dev->i2c_adap.dev.parent = &dev->udev->dev;
-	strcpy(dev->i2c_adap.name, dev->name);
-	dev->i2c_adap.algo_data = dev;
-	i2c_set_adapdata(&dev->i2c_adap, &dev->v4l2_dev);
 
-	retval = i2c_add_adapter(&dev->i2c_adap);
+	if (bus >= NUM_I2C_BUSES)
+		return -ENODEV;
+
+	dev->i2c_adap[bus] = em28xx_adap_template;
+	dev->i2c_adap[bus].dev.parent = &dev->udev->dev;
+	strcpy(dev->i2c_adap[bus].name, dev->name);
+
+	dev->i2c_bus[bus].bus = bus;
+	dev->i2c_bus[bus].algo_type = algo_type;
+	dev->i2c_bus[bus].dev = dev;
+	dev->i2c_adap[bus].algo_data = &dev->i2c_bus[bus];
+	i2c_set_adapdata(&dev->i2c_adap[bus], &dev->v4l2_dev);
+
+	retval = i2c_add_adapter(&dev->i2c_adap[bus]);
 	if (retval < 0) {
 		em28xx_errdev("%s: i2c_add_adapter failed! retval [%d]\n",
 			__func__, retval);
 		return retval;
 	}
 
-	dev->i2c_client = em28xx_client_template;
-	dev->i2c_client.adapter = &dev->i2c_adap;
+	dev->i2c_client[bus] = em28xx_client_template;
+	dev->i2c_client[bus].adapter = &dev->i2c_adap[bus];
 
-	retval = em28xx_i2c_eeprom(dev, dev->eedata, sizeof(dev->eedata));
-	if ((retval < 0) && (retval != -ENODEV)) {
-		em28xx_errdev("%s: em28xx_i2_eeprom failed! retval [%d]\n",
-			__func__, retval);
+	/* Up to now, all eeproms are at bus 0 */
+	if (!bus) {
+		retval = em28xx_i2c_eeprom(dev, bus, &dev->eedata, &dev->eedata_len);
+		if ((retval < 0) && (retval != -ENODEV)) {
+			em28xx_errdev("%s: em28xx_i2_eeprom failed! retval [%d]\n",
+				__func__, retval);
 
-		return retval;
+			return retval;
+		}
 	}
 
 	if (i2c_scan)
-		em28xx_do_i2c_scan(dev);
+		em28xx_do_i2c_scan(dev, bus);
 
 	return 0;
 }
@@ -622,8 +920,11 @@ int em28xx_i2c_register(struct em28xx *dev)
  * em28xx_i2c_unregister()
  * unregister i2c_bus
  */
-int em28xx_i2c_unregister(struct em28xx *dev)
+int em28xx_i2c_unregister(struct em28xx *dev, unsigned bus)
 {
-	i2c_del_adapter(&dev->i2c_adap);
+	if (bus >= NUM_I2C_BUSES)
+		return -ENODEV;
+
+	i2c_del_adapter(&dev->i2c_adap[bus]);
 	return 0;
 }
diff --git a/drivers/media/usb/em28xx/em28xx-input.c b/drivers/media/usb/em28xx/em28xx-input.c
index 1bef990b3f1..466b19d0d76 100644
--- a/drivers/media/usb/em28xx/em28xx-input.c
+++ b/drivers/media/usb/em28xx/em28xx-input.c
@@ -280,11 +280,12 @@ static int em2874_polling_getkey(struct em28xx_IR *ir,
 
 static int em28xx_i2c_ir_handle_key(struct em28xx_IR *ir)
 {
+	struct em28xx *dev = ir->dev;
 	static u32 ir_key;
 	int rc;
 	struct i2c_client client;
 
-	client.adapter = &ir->dev->i2c_adap;
+	client.adapter = &ir->dev->i2c_adap[dev->def_i2c_bus];
 	client.addr = ir->i2c_dev_addr;
 
 	rc = ir->get_key_i2c(&client, &ir_key);
@@ -461,7 +462,7 @@ static int em28xx_probe_i2c_ir(struct em28xx *dev)
 	};
 
 	while (addr_list[i] != I2C_CLIENT_END) {
-		if (i2c_probe_func_quick_read(&dev->i2c_adap, addr_list[i]) == 1)
+		if (i2c_probe_func_quick_read(&dev->i2c_adap[dev->def_i2c_bus], addr_list[i]) == 1)
 			return addr_list[i];
 		i++;
 	}
diff --git a/drivers/media/usb/em28xx/em28xx-reg.h b/drivers/media/usb/em28xx/em28xx-reg.h
index 885089e22bc..622871db04a 100644
--- a/drivers/media/usb/em28xx/em28xx-reg.h
+++ b/drivers/media/usb/em28xx/em28xx-reg.h
@@ -48,7 +48,7 @@
 #define EM28XX_CHIPCFG2_TS_PACKETSIZE_752	0x03
 
 
-	/* GPIO/GPO registers */
+/* GPIO/GPO registers */
 #define EM2880_R04_GPO	0x04    /* em2880-em2883 only */
 #define EM28XX_R08_GPIO	0x08	/* em2820 or upper */
 
@@ -120,12 +120,23 @@
 #define EM28XX_R1E_CWIDTH	0x1e
 #define EM28XX_R1F_CHEIGHT	0x1f
 
-#define EM28XX_R20_YGAIN	0x20
-#define EM28XX_R21_YOFFSET	0x21
-#define EM28XX_R22_UVGAIN	0x22
-#define EM28XX_R23_UOFFSET	0x23
-#define EM28XX_R24_VOFFSET	0x24
-#define EM28XX_R25_SHARPNESS	0x25
+#define EM28XX_R20_YGAIN	0x20 /* contrast [0:4]   */
+#define   CONTRAST_DEFAULT	0x10
+
+#define EM28XX_R21_YOFFSET	0x21 /* brightness       */	/* signed */
+#define   BRIGHTNESS_DEFAULT	0x00
+
+#define EM28XX_R22_UVGAIN	0x22 /* saturation [0:4] */
+#define   SATURATION_DEFAULT	0x10
+
+#define EM28XX_R23_UOFFSET	0x23 /* blue balance     */	/* signed */
+#define   BLUE_BALANCE_DEFAULT	0x00
+
+#define EM28XX_R24_VOFFSET	0x24 /* red balance      */	/* signed */
+#define   RED_BALANCE_DEFAULT	0x00
+
+#define EM28XX_R25_SHARPNESS	0x25 /* sharpness [0:4]  */
+#define   SHARPNESS_DEFAULT	0x00
 
 #define EM28XX_R26_COMPR	0x26
 #define EM28XX_R27_OUTFMT	0x27
@@ -152,8 +163,17 @@
 #define EM28XX_R31_HSCALEHIGH	0x31
 #define EM28XX_R32_VSCALELOW	0x32
 #define EM28XX_R33_VSCALEHIGH	0x33
+#define   EM28XX_HVSCALE_MAX	0x3fff /* => 20% */
+
 #define EM28XX_R34_VBI_START_H	0x34
 #define EM28XX_R35_VBI_START_V	0x35
+/*
+ * NOTE: the EM276x (and EM25xx, EM277x/8x ?) (camera bridges) use these
+ * registers for a different unknown purpose.
+ *   => register 0x34 is set to capture width / 16
+ *   => register 0x35 is set to capture height / 16
+ */
+
 #define EM28XX_R36_VBI_WIDTH	0x36
 #define EM28XX_R37_VBI_HEIGHT	0x37
 
@@ -206,6 +226,7 @@ enum em28xx_chip_id {
 	CHIP_ID_EM2860 = 34,
 	CHIP_ID_EM2870 = 35,
 	CHIP_ID_EM2883 = 36,
+	CHIP_ID_EM2765 = 54,
 	CHIP_ID_EM2874 = 65,
 	CHIP_ID_EM2884 = 68,
 	CHIP_ID_EM28174 = 113,
diff --git a/drivers/media/usb/em28xx/em28xx-video.c b/drivers/media/usb/em28xx/em28xx-video.c
index 32bd7de5dec..32d60e5546b 100644
--- a/drivers/media/usb/em28xx/em28xx-video.c
+++ b/drivers/media/usb/em28xx/em28xx-video.c
@@ -52,7 +52,7 @@
 
 #define DRIVER_DESC         "Empia em28xx based USB video device driver"
 
-#define EM28XX_VERSION "0.1.3"
+#define EM28XX_VERSION "0.2.0"
 
 #define em28xx_videodbg(fmt, arg...) do {\
 	if (video_debug) \
@@ -76,6 +76,16 @@ MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL");
 MODULE_VERSION(EM28XX_VERSION);
 
+
+#define EM25XX_FRMDATAHDR_BYTE1			0x02
+#define EM25XX_FRMDATAHDR_BYTE2_STILL_IMAGE	0x20
+#define EM25XX_FRMDATAHDR_BYTE2_FRAME_END	0x02
+#define EM25XX_FRMDATAHDR_BYTE2_FRAME_ID	0x01
+#define EM25XX_FRMDATAHDR_BYTE2_MASK	(EM25XX_FRMDATAHDR_BYTE2_STILL_IMAGE | \
+					 EM25XX_FRMDATAHDR_BYTE2_FRAME_END |   \
+					 EM25XX_FRMDATAHDR_BYTE2_FRAME_ID)
+
+
 static unsigned int video_nr[] = {[0 ... (EM28XX_MAXBOARDS - 1)] = -1U };
 static unsigned int vbi_nr[]   = {[0 ... (EM28XX_MAXBOARDS - 1)] = -1U };
 static unsigned int radio_nr[] = {[0 ... (EM28XX_MAXBOARDS - 1)] = -1U };
@@ -408,6 +418,62 @@ static inline void process_frame_data_em28xx(struct em28xx *dev,
 		em28xx_copy_video(dev, buf, data_pkt, data_len);
 }
 
+/*
+ * Process data packet according to the em25xx/em276x/7x/8x frame data format
+ */
+static inline void process_frame_data_em25xx(struct em28xx *dev,
+					     unsigned char *data_pkt,
+					     unsigned int  data_len)
+{
+	struct em28xx_buffer    *buf = dev->usb_ctl.vid_buf;
+	struct em28xx_dmaqueue  *dmaq = &dev->vidq;
+	bool frame_end = 0;
+
+	/* Check for header */
+	/* NOTE: at least with bulk transfers, only the first packet
+	 * has a header and has always set the FRAME_END bit         */
+	if (data_len >= 2) {	/* em25xx header is only 2 bytes long */
+		if ((data_pkt[0] == EM25XX_FRMDATAHDR_BYTE1) &&
+		    ((data_pkt[1] & ~EM25XX_FRMDATAHDR_BYTE2_MASK) == 0x00)) {
+			dev->top_field = !(data_pkt[1] &
+					   EM25XX_FRMDATAHDR_BYTE2_FRAME_ID);
+			frame_end = data_pkt[1] &
+				    EM25XX_FRMDATAHDR_BYTE2_FRAME_END;
+			data_pkt += 2;
+			data_len -= 2;
+		}
+
+		/* Finish field and prepare next (BULK only) */
+		if (dev->analog_xfer_bulk && frame_end) {
+			buf = finish_field_prepare_next(dev, buf, dmaq);
+			dev->usb_ctl.vid_buf = buf;
+		}
+		/* NOTE: in ISOC mode when a new frame starts and buf==NULL,
+		 * we COULD already prepare a buffer here to avoid skipping the
+		 * first frame.
+		 */
+	}
+
+	/* Copy data */
+	if (buf != NULL && data_len > 0)
+		em28xx_copy_video(dev, buf, data_pkt, data_len);
+
+	/* Finish frame (ISOC only) => avoids lag of 1 frame */
+	if (!dev->analog_xfer_bulk && frame_end) {
+		buf = finish_field_prepare_next(dev, buf, dmaq);
+		dev->usb_ctl.vid_buf = buf;
+	}
+
+	/* NOTE: Tested with USB bulk transfers only !
+	 * The wording in the datasheet suggests that isoc might work different.
+	 * The current code assumes that with isoc transfers each packet has a
+	 * header like with the other em28xx devices.
+	 */
+	/* NOTE: Support for interlaced mode is pure theory. It has not been
+	 * tested and it is unknown if these devices actually support it. */
+	/* NOTE: No VBI support yet (these chips likely do not support VBI). */
+}
+
 /* Processes and copies the URB data content (video and VBI data) */
 static inline int em28xx_urb_data_copy(struct em28xx *dev, struct urb *urb)
 {
@@ -460,7 +526,13 @@ static inline int em28xx_urb_data_copy(struct em28xx *dev, struct urb *urb)
 			continue;
 		}
 
-		process_frame_data_em28xx(dev, usb_data_pkt, usb_data_len);
+		if (dev->is_em25xx)
+			process_frame_data_em25xx(dev,
+						  usb_data_pkt, usb_data_len);
+		else
+			process_frame_data_em28xx(dev,
+						  usb_data_pkt, usb_data_len);
+
 	}
 	return 1;
 }
@@ -700,6 +772,7 @@ int em28xx_vb2_setup(struct em28xx *dev)
 	q = &dev->vb_vidq;
 	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 	q->io_modes = VB2_READ | VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 	q->drv_priv = dev;
 	q->buf_struct_size = sizeof(struct em28xx_buffer);
 	q->ops = &em28xx_video_qops;
@@ -713,6 +786,7 @@ int em28xx_vb2_setup(struct em28xx *dev)
 	q = &dev->vb_vbiq;
 	q->type = V4L2_BUF_TYPE_VBI_CAPTURE;
 	q->io_modes = VB2_READ | VB2_MMAP | VB2_USERPTR;
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 	q->drv_priv = dev;
 	q->buf_struct_size = sizeof(struct em28xx_buffer);
 	q->ops = &em28xx_vbi_qops;
@@ -782,33 +856,45 @@ void em28xx_ctrl_notify(struct v4l2_ctrl *ctrl, void *priv)
 static int em28xx_s_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct em28xx *dev = container_of(ctrl->handler, struct em28xx, ctrl_handler);
+	int ret = -EINVAL;
 
 	switch (ctrl->id) {
 	case V4L2_CID_AUDIO_MUTE:
 		dev->mute = ctrl->val;
+		ret = em28xx_audio_analog_set(dev);
 		break;
 	case V4L2_CID_AUDIO_VOLUME:
 		dev->volume = ctrl->val;
+		ret = em28xx_audio_analog_set(dev);
+		break;
+	case V4L2_CID_CONTRAST:
+		ret = em28xx_write_reg(dev, EM28XX_R20_YGAIN, ctrl->val);
+		break;
+	case V4L2_CID_BRIGHTNESS:
+		ret = em28xx_write_reg(dev, EM28XX_R21_YOFFSET, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		ret = em28xx_write_reg(dev, EM28XX_R22_UVGAIN, ctrl->val);
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		ret = em28xx_write_reg(dev, EM28XX_R23_UOFFSET, ctrl->val);
+		break;
+	case V4L2_CID_RED_BALANCE:
+		ret = em28xx_write_reg(dev, EM28XX_R24_VOFFSET, ctrl->val);
+		break;
+	case V4L2_CID_SHARPNESS:
+		ret = em28xx_write_reg(dev, EM28XX_R25_SHARPNESS, ctrl->val);
 		break;
 	}
 
-	return em28xx_audio_analog_set(dev);
+	return (ret < 0) ? ret : 0;
 }
 
 const struct v4l2_ctrl_ops em28xx_ctrl_ops = {
 	.s_ctrl = em28xx_s_ctrl,
 };
 
-static int check_dev(struct em28xx *dev)
-{
-	if (dev->disconnected) {
-		em28xx_errdev("v4l2 ioctl: device not present\n");
-		return -ENODEV;
-	}
-	return 0;
-}
-
-static void get_scale(struct em28xx *dev,
+static void size_to_scale(struct em28xx *dev,
 			unsigned int width, unsigned int height,
 			unsigned int *hscale, unsigned int *vscale)
 {
@@ -816,12 +902,23 @@ static void get_scale(struct em28xx *dev,
 	unsigned int          maxh = norm_maxh(dev);
 
 	*hscale = (((unsigned long)maxw) << 12) / width - 4096L;
-	if (*hscale >= 0x4000)
-		*hscale = 0x3fff;
+	if (*hscale > EM28XX_HVSCALE_MAX)
+		*hscale = EM28XX_HVSCALE_MAX;
 
 	*vscale = (((unsigned long)maxh) << 12) / height - 4096L;
-	if (*vscale >= 0x4000)
-		*vscale = 0x3fff;
+	if (*vscale > EM28XX_HVSCALE_MAX)
+		*vscale = EM28XX_HVSCALE_MAX;
+}
+
+static void scale_to_size(struct em28xx *dev,
+			  unsigned int hscale, unsigned int vscale,
+			  unsigned int *width, unsigned int *height)
+{
+	unsigned int          maxw = norm_maxw(dev);
+	unsigned int          maxh = norm_maxh(dev);
+
+	*width = (((unsigned long)maxw) << 12) / (hscale + 4096L);
+	*height = (((unsigned long)maxh) << 12) / (vscale + 4096L);
 }
 
 /* ------------------------------------------------------------------
@@ -898,10 +995,8 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 				      1, 0);
 	}
 
-	get_scale(dev, width, height, &hscale, &vscale);
-
-	width = (((unsigned long)maxw) << 12) / (hscale + 4096L);
-	height = (((unsigned long)maxh) << 12) / (vscale + 4096L);
+	size_to_scale(dev, width, height, &hscale, &vscale);
+	scale_to_size(dev, hscale, vscale, &width, &height);
 
 	f->fmt.pix.width = width;
 	f->fmt.pix.height = height;
@@ -932,7 +1027,7 @@ static int em28xx_set_video_format(struct em28xx *dev, unsigned int fourcc,
 	dev->height = height;
 
 	/* set new image size */
-	get_scale(dev, dev->width, dev->height, &dev->hscale, &dev->vscale);
+	size_to_scale(dev, dev->width, dev->height, &dev->hscale, &dev->vscale);
 
 	em28xx_resolution_set(dev);
 
@@ -957,13 +1052,6 @@ static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *norm)
 {
 	struct em28xx_fh   *fh  = priv;
 	struct em28xx      *dev = fh->dev;
-	int                rc;
-
-	if (dev->board.is_webcam)
-		return -ENOTTY;
-	rc = check_dev(dev);
-	if (rc < 0)
-		return rc;
 
 	*norm = dev->norm;
 
@@ -974,48 +1062,35 @@ static int vidioc_querystd(struct file *file, void *priv, v4l2_std_id *norm)
 {
 	struct em28xx_fh   *fh  = priv;
 	struct em28xx      *dev = fh->dev;
-	int                rc;
-
-	if (dev->board.is_webcam)
-		return -ENOTTY;
-	rc = check_dev(dev);
-	if (rc < 0)
-		return rc;
 
 	v4l2_device_call_all(&dev->v4l2_dev, 0, video, querystd, norm);
 
 	return 0;
 }
 
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *norm)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id norm)
 {
 	struct em28xx_fh   *fh  = priv;
 	struct em28xx      *dev = fh->dev;
 	struct v4l2_format f;
-	int                rc;
 
-	if (dev->board.is_webcam)
-		return -ENOTTY;
-	if (*norm == dev->norm)
+	if (norm == dev->norm)
 		return 0;
-	rc = check_dev(dev);
-	if (rc < 0)
-		return rc;
 
 	if (dev->streaming_users > 0)
 		return -EBUSY;
 
-	dev->norm = *norm;
+	dev->norm = norm;
 
 	/* Adjusts width/height, if needed */
 	f.fmt.pix.width = 720;
-	f.fmt.pix.height = (*norm & V4L2_STD_525_60) ? 480 : 576;
+	f.fmt.pix.height = (norm & V4L2_STD_525_60) ? 480 : 576;
 	vidioc_try_fmt_vid_cap(file, priv, &f);
 
 	/* set new image size */
 	dev->width = f.fmt.pix.width;
 	dev->height = f.fmt.pix.height;
-	get_scale(dev, dev->width, dev->height, &dev->hscale, &dev->vscale);
+	size_to_scale(dev, dev->width, dev->height, &dev->hscale, &dev->vscale);
 
 	em28xx_resolution_set(dev);
 	v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_std, dev->norm);
@@ -1030,9 +1105,6 @@ static int vidioc_g_parm(struct file *file, void *priv,
 	struct em28xx      *dev = fh->dev;
 	int rc = 0;
 
-	if (p->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
 	p->parm.capture.readbuffers = EM28XX_MIN_BUF;
 	if (dev->board.is_webcam)
 		rc = v4l2_device_call_until_err(&dev->v4l2_dev, 0,
@@ -1050,12 +1122,6 @@ static int vidioc_s_parm(struct file *file, void *priv,
 	struct em28xx_fh   *fh  = priv;
 	struct em28xx      *dev = fh->dev;
 
-	if (!dev->board.is_webcam)
-		return -ENOTTY;
-
-	if (p->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
 	p->parm.capture.readbuffers = EM28XX_MIN_BUF;
 	return v4l2_device_call_until_err(&dev->v4l2_dev, 0, video, s_parm, p);
 }
@@ -1116,11 +1182,6 @@ static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
 {
 	struct em28xx_fh   *fh  = priv;
 	struct em28xx      *dev = fh->dev;
-	int                rc;
-
-	rc = check_dev(dev);
-	if (rc < 0)
-		return rc;
 
 	if (i >= MAX_EM28XX_INPUT)
 		return -EINVAL;
@@ -1136,9 +1197,6 @@ static int vidioc_g_audio(struct file *file, void *priv, struct v4l2_audio *a)
 	struct em28xx_fh   *fh    = priv;
 	struct em28xx      *dev   = fh->dev;
 
-	if (!dev->audio_mode.has_audio)
-		return -EINVAL;
-
 	switch (a->index) {
 	case EM28XX_AMUX_VIDEO:
 		strcpy(a->name, "Television");
@@ -1179,10 +1237,6 @@ static int vidioc_s_audio(struct file *file, void *priv, const struct v4l2_audio
 	struct em28xx_fh   *fh  = priv;
 	struct em28xx      *dev = fh->dev;
 
-
-	if (!dev->audio_mode.has_audio)
-		return -EINVAL;
-
 	if (a->index >= MAX_EM28XX_INPUT)
 		return -EINVAL;
 	if (0 == INPUT(a->index)->type)
@@ -1202,11 +1256,6 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 {
 	struct em28xx_fh      *fh  = priv;
 	struct em28xx         *dev = fh->dev;
-	int                   rc;
-
-	rc = check_dev(dev);
-	if (rc < 0)
-		return rc;
 
 	if (0 != t->index)
 		return -EINVAL;
@@ -1218,15 +1267,10 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *t)
+				const struct v4l2_tuner *t)
 {
 	struct em28xx_fh      *fh  = priv;
 	struct em28xx         *dev = fh->dev;
-	int                   rc;
-
-	rc = check_dev(dev);
-	if (rc < 0)
-		return rc;
 
 	if (0 != t->index)
 		return -EINVAL;
@@ -1249,39 +1293,22 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
+				const struct v4l2_frequency *f)
 {
+	struct v4l2_frequency new_freq = *f;
 	struct em28xx_fh      *fh  = priv;
 	struct em28xx         *dev = fh->dev;
-	int                   rc;
-
-	rc = check_dev(dev);
-	if (rc < 0)
-		return rc;
 
 	if (0 != f->tuner)
 		return -EINVAL;
 
 	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_frequency, f);
-	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, g_frequency, f);
-	dev->ctl_freq = f->frequency;
+	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, g_frequency, &new_freq);
+	dev->ctl_freq = new_freq.frequency;
 
 	return 0;
 }
 
-#ifdef CONFIG_VIDEO_ADV_DEBUG
-static int em28xx_reg_len(int reg)
-{
-	switch (reg) {
-	case EM28XX_R40_AC97LSB:
-	case EM28XX_R30_HSCALELOW:
-	case EM28XX_R32_VSCALELOW:
-		return 2;
-	default:
-		return 1;
-	}
-}
-
 static int vidioc_g_chip_ident(struct file *file, void *priv,
 	       struct v4l2_dbg_chip_ident *chip)
 {
@@ -1290,9 +1317,9 @@ static int vidioc_g_chip_ident(struct file *file, void *priv,
 
 	chip->ident = V4L2_IDENT_NONE;
 	chip->revision = 0;
-	if (chip->match.type == V4L2_CHIP_MATCH_HOST) {
-		if (v4l2_chip_match_host(&chip->match))
-			chip->ident = V4L2_IDENT_NONE;
+	if (chip->match.type == V4L2_CHIP_MATCH_BRIDGE) {
+		if (chip->match.addr > 1)
+			return -EINVAL;
 		return 0;
 	}
 	if (chip->match.type != V4L2_CHIP_MATCH_I2C_DRIVER &&
@@ -1304,6 +1331,33 @@ static int vidioc_g_chip_ident(struct file *file, void *priv,
 	return 0;
 }
 
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int vidioc_g_chip_info(struct file *file, void *priv,
+	       struct v4l2_dbg_chip_info *chip)
+{
+	struct em28xx_fh      *fh  = priv;
+	struct em28xx         *dev = fh->dev;
+
+	if (chip->match.addr > 1)
+		return -EINVAL;
+	if (chip->match.addr == 1)
+		strlcpy(chip->name, "ac97", sizeof(chip->name));
+	else
+		strlcpy(chip->name, dev->v4l2_dev.name, sizeof(chip->name));
+	return 0;
+}
+
+static int em28xx_reg_len(int reg)
+{
+	switch (reg) {
+	case EM28XX_R40_AC97LSB:
+	case EM28XX_R30_HSCALELOW:
+	case EM28XX_R32_VSCALELOW:
+		return 2;
+	default:
+		return 1;
+	}
+}
 
 static int vidioc_g_register(struct file *file, void *priv,
 			     struct v4l2_dbg_register *reg)
@@ -1313,6 +1367,12 @@ static int vidioc_g_register(struct file *file, void *priv,
 	int ret;
 
 	switch (reg->match.type) {
+	case V4L2_CHIP_MATCH_BRIDGE:
+		if (reg->match.addr > 1)
+			return -EINVAL;
+		if (!reg->match.addr)
+			break;
+		/* fall-through */
 	case V4L2_CHIP_MATCH_AC97:
 		ret = em28xx_read_ac97(dev, reg->reg);
 		if (ret < 0)
@@ -1329,8 +1389,7 @@ static int vidioc_g_register(struct file *file, void *priv,
 		v4l2_device_call_all(&dev->v4l2_dev, 0, core, g_register, reg);
 		return 0;
 	default:
-		if (!v4l2_chip_match_host(&reg->match))
-			return -EINVAL;
+		return -EINVAL;
 	}
 
 	/* Match host */
@@ -1356,13 +1415,19 @@ static int vidioc_g_register(struct file *file, void *priv,
 }
 
 static int vidioc_s_register(struct file *file, void *priv,
-			     struct v4l2_dbg_register *reg)
+			     const struct v4l2_dbg_register *reg)
 {
 	struct em28xx_fh      *fh  = priv;
 	struct em28xx         *dev = fh->dev;
 	__le16 buf;
 
 	switch (reg->match.type) {
+	case V4L2_CHIP_MATCH_BRIDGE:
+		if (reg->match.addr > 1)
+			return -EINVAL;
+		if (!reg->match.addr)
+			break;
+		/* fall-through */
 	case V4L2_CHIP_MATCH_AC97:
 		return em28xx_write_ac97(dev, reg->reg, reg->val);
 	case V4L2_CHIP_MATCH_I2C_DRIVER:
@@ -1373,8 +1438,7 @@ static int vidioc_s_register(struct file *file, void *priv,
 		v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_register, reg);
 		return 0;
 	default:
-		if (!v4l2_chip_match_host(&reg->match))
-			return -EINVAL;
+		return -EINVAL;
 	}
 
 	/* Match host */
@@ -1386,26 +1450,6 @@ static int vidioc_s_register(struct file *file, void *priv,
 #endif
 
 
-static int vidioc_cropcap(struct file *file, void *priv,
-					struct v4l2_cropcap *cc)
-{
-	struct em28xx_fh      *fh  = priv;
-	struct em28xx         *dev = fh->dev;
-
-	if (cc->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	cc->bounds.left = 0;
-	cc->bounds.top = 0;
-	cc->bounds.width = dev->width;
-	cc->bounds.height = dev->height;
-	cc->defrect = cc->bounds;
-	cc->pixelaspect.numerator = 54;	/* 4:3 FIXME: remove magic numbers */
-	cc->pixelaspect.denominator = 59;
-
-	return 0;
-}
-
 static int vidioc_querycap(struct file *file, void  *priv,
 					struct v4l2_capability *cap)
 {
@@ -1482,8 +1526,12 @@ static int vidioc_enum_framesizes(struct file *file, void *priv,
 
 	/* Report a continuous range */
 	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
-	fsize->stepwise.min_width = 48;
-	fsize->stepwise.min_height = 32;
+	scale_to_size(dev, EM28XX_HVSCALE_MAX, EM28XX_HVSCALE_MAX,
+		      &fsize->stepwise.min_width, &fsize->stepwise.min_height);
+	if (fsize->stepwise.min_width < 48)
+		fsize->stepwise.min_width = 48;
+	if (fsize->stepwise.min_height < 38)
+		fsize->stepwise.min_height = 38;
 	fsize->stepwise.max_width = maxw;
 	fsize->stepwise.max_height = maxh;
 	fsize->stepwise.step_width = 1;
@@ -1522,35 +1570,6 @@ static int vidioc_g_fmt_vbi_cap(struct file *file, void *priv,
 	return 0;
 }
 
-static int vidioc_s_fmt_vbi_cap(struct file *file, void *priv,
-				struct v4l2_format *format)
-{
-	struct em28xx_fh      *fh  = priv;
-	struct em28xx         *dev = fh->dev;
-
-	format->fmt.vbi.samples_per_line = dev->vbi_width;
-	format->fmt.vbi.sample_format = V4L2_PIX_FMT_GREY;
-	format->fmt.vbi.offset = 0;
-	format->fmt.vbi.flags = 0;
-	format->fmt.vbi.sampling_rate = 6750000 * 4 / 2;
-	format->fmt.vbi.count[0] = dev->vbi_height;
-	format->fmt.vbi.count[1] = dev->vbi_height;
-	memset(format->fmt.vbi.reserved, 0, sizeof(format->fmt.vbi.reserved));
-
-	/* Varies by video standard (NTSC, PAL, etc.) */
-	if (dev->norm & V4L2_STD_525_60) {
-		/* NTSC */
-		format->fmt.vbi.start[0] = 10;
-		format->fmt.vbi.start[1] = 273;
-	} else if (dev->norm & V4L2_STD_625_50) {
-		/* PAL */
-		format->fmt.vbi.start[0] = 6;
-		format->fmt.vbi.start[1] = 318;
-	}
-
-	return 0;
-}
-
 /* ----------------------------------------------------------- */
 /* RADIO ESPECIFIC IOCTLS                                      */
 /* ----------------------------------------------------------- */
@@ -1564,7 +1583,6 @@ static int radio_g_tuner(struct file *file, void *priv,
 		return -EINVAL;
 
 	strcpy(t->name, "Radio");
-	t->type = V4L2_TUNER_RADIO;
 
 	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, g_tuner, t);
 
@@ -1572,7 +1590,7 @@ static int radio_g_tuner(struct file *file, void *priv,
 }
 
 static int radio_s_tuner(struct file *file, void *priv,
-			 struct v4l2_tuner *t)
+			 const struct v4l2_tuner *t)
 {
 	struct em28xx *dev = ((struct em28xx_fh *)priv)->dev;
 
@@ -1749,11 +1767,10 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_s_fmt_vid_cap       = vidioc_s_fmt_vid_cap,
 	.vidioc_g_fmt_vbi_cap       = vidioc_g_fmt_vbi_cap,
 	.vidioc_try_fmt_vbi_cap     = vidioc_g_fmt_vbi_cap,
-	.vidioc_s_fmt_vbi_cap       = vidioc_s_fmt_vbi_cap,
+	.vidioc_s_fmt_vbi_cap       = vidioc_g_fmt_vbi_cap,
 	.vidioc_enum_framesizes     = vidioc_enum_framesizes,
 	.vidioc_g_audio             = vidioc_g_audio,
 	.vidioc_s_audio             = vidioc_s_audio,
-	.vidioc_cropcap             = vidioc_cropcap,
 
 	.vidioc_reqbufs             = vb2_ioctl_reqbufs,
 	.vidioc_create_bufs         = vb2_ioctl_create_bufs,
@@ -1778,10 +1795,11 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_s_frequency         = vidioc_s_frequency,
 	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
 	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+	.vidioc_g_chip_ident        = vidioc_g_chip_ident,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
+	.vidioc_g_chip_info         = vidioc_g_chip_info,
 	.vidioc_g_register          = vidioc_g_register,
 	.vidioc_s_register          = vidioc_s_register,
-	.vidioc_g_chip_ident        = vidioc_g_chip_ident,
 #endif
 };
 
@@ -1808,7 +1826,9 @@ static const struct v4l2_ioctl_ops radio_ioctl_ops = {
 	.vidioc_s_frequency   = vidioc_s_frequency,
 	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
 	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+	.vidioc_g_chip_ident  = vidioc_g_chip_ident,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
+	.vidioc_g_chip_info   = vidioc_g_chip_info,
 	.vidioc_g_register    = vidioc_g_register,
 	.vidioc_s_register    = vidioc_s_register,
 #endif
@@ -1887,9 +1907,42 @@ int em28xx_register_analog_devices(struct em28xx *dev)
 			 (EM28XX_XCLK_AUDIO_UNMUTE | val));
 
 	em28xx_set_outfmt(dev);
-	em28xx_colorlevels_set_default(dev);
 	em28xx_compression_disable(dev);
 
+	/* Add image controls */
+	/* NOTE: at this point, the subdevices are already registered, so bridge
+	 * controls are only added/enabled when no subdevice provides them */
+	if (NULL == v4l2_ctrl_find(&dev->ctrl_handler, V4L2_CID_CONTRAST))
+		v4l2_ctrl_new_std(&dev->ctrl_handler, &em28xx_ctrl_ops,
+				  V4L2_CID_CONTRAST,
+				  0, 0x1f, 1, CONTRAST_DEFAULT);
+	if (NULL == v4l2_ctrl_find(&dev->ctrl_handler, V4L2_CID_BRIGHTNESS))
+		v4l2_ctrl_new_std(&dev->ctrl_handler, &em28xx_ctrl_ops,
+				  V4L2_CID_BRIGHTNESS,
+				  -0x80, 0x7f, 1, BRIGHTNESS_DEFAULT);
+	if (NULL == v4l2_ctrl_find(&dev->ctrl_handler, V4L2_CID_SATURATION))
+		v4l2_ctrl_new_std(&dev->ctrl_handler, &em28xx_ctrl_ops,
+				  V4L2_CID_SATURATION,
+				  0, 0x1f, 1, SATURATION_DEFAULT);
+	if (NULL == v4l2_ctrl_find(&dev->ctrl_handler, V4L2_CID_BLUE_BALANCE))
+		v4l2_ctrl_new_std(&dev->ctrl_handler, &em28xx_ctrl_ops,
+				  V4L2_CID_BLUE_BALANCE,
+				  -0x30, 0x30, 1, BLUE_BALANCE_DEFAULT);
+	if (NULL == v4l2_ctrl_find(&dev->ctrl_handler, V4L2_CID_RED_BALANCE))
+		v4l2_ctrl_new_std(&dev->ctrl_handler, &em28xx_ctrl_ops,
+				  V4L2_CID_RED_BALANCE,
+				  -0x30, 0x30, 1, RED_BALANCE_DEFAULT);
+	if (NULL == v4l2_ctrl_find(&dev->ctrl_handler, V4L2_CID_SHARPNESS))
+		v4l2_ctrl_new_std(&dev->ctrl_handler, &em28xx_ctrl_ops,
+				  V4L2_CID_SHARPNESS,
+				  0, 0x0f, 1, SHARPNESS_DEFAULT);
+
+	/* Reset image controls */
+	em28xx_colorlevels_set_default(dev);
+	v4l2_ctrl_handler_setup(&dev->ctrl_handler);
+	if (dev->ctrl_handler.error)
+		return dev->ctrl_handler.error;
+
 	/* allocate and fill video video_device struct */
 	dev->vdev = em28xx_vdev_init(dev, &em28xx_video_template, "video");
 	if (!dev->vdev) {
@@ -1899,6 +1952,25 @@ int em28xx_register_analog_devices(struct em28xx *dev)
 	dev->vdev->queue = &dev->vb_vidq;
 	dev->vdev->queue->lock = &dev->vb_queue_lock;
 
+	/* disable inapplicable ioctls */
+	if (dev->board.is_webcam) {
+		v4l2_disable_ioctl(dev->vdev, VIDIOC_QUERYSTD);
+		v4l2_disable_ioctl(dev->vdev, VIDIOC_G_STD);
+		v4l2_disable_ioctl(dev->vdev, VIDIOC_S_STD);
+	} else {
+		v4l2_disable_ioctl(dev->vdev, VIDIOC_S_PARM);
+	}
+	if (dev->tuner_type == TUNER_ABSENT) {
+		v4l2_disable_ioctl(dev->vdev, VIDIOC_G_TUNER);
+		v4l2_disable_ioctl(dev->vdev, VIDIOC_S_TUNER);
+		v4l2_disable_ioctl(dev->vdev, VIDIOC_G_FREQUENCY);
+		v4l2_disable_ioctl(dev->vdev, VIDIOC_S_FREQUENCY);
+	}
+	if (!dev->audio_mode.has_audio) {
+		v4l2_disable_ioctl(dev->vdev, VIDIOC_G_AUDIO);
+		v4l2_disable_ioctl(dev->vdev, VIDIOC_S_AUDIO);
+	}
+
 	/* register v4l2 video video_device */
 	ret = video_register_device(dev->vdev, VFL_TYPE_GRABBER,
 				       video_nr[dev->devno]);
@@ -1916,6 +1988,19 @@ int em28xx_register_analog_devices(struct em28xx *dev)
 		dev->vbi_dev->queue = &dev->vb_vbiq;
 		dev->vbi_dev->queue->lock = &dev->vb_vbi_queue_lock;
 
+		/* disable inapplicable ioctls */
+		v4l2_disable_ioctl(dev->vdev, VIDIOC_S_PARM);
+		if (dev->tuner_type == TUNER_ABSENT) {
+			v4l2_disable_ioctl(dev->vbi_dev, VIDIOC_G_TUNER);
+			v4l2_disable_ioctl(dev->vbi_dev, VIDIOC_S_TUNER);
+			v4l2_disable_ioctl(dev->vbi_dev, VIDIOC_G_FREQUENCY);
+			v4l2_disable_ioctl(dev->vbi_dev, VIDIOC_S_FREQUENCY);
+		}
+		if (!dev->audio_mode.has_audio) {
+			v4l2_disable_ioctl(dev->vbi_dev, VIDIOC_G_AUDIO);
+			v4l2_disable_ioctl(dev->vbi_dev, VIDIOC_S_AUDIO);
+		}
+
 		/* register v4l2 vbi video_device */
 		ret = video_register_device(dev->vbi_dev, VFL_TYPE_VBI,
 					    vbi_nr[dev->devno]);
diff --git a/drivers/media/usb/em28xx/em28xx.h b/drivers/media/usb/em28xx/em28xx.h
index 5f0b2c59e84..a9323b63d8e 100644
--- a/drivers/media/usb/em28xx/em28xx.h
+++ b/drivers/media/usb/em28xx/em28xx.h
@@ -42,28 +42,28 @@
 #include "em28xx-reg.h"
 
 /* Boards supported by driver */
-#define EM2800_BOARD_UNKNOWN			0
-#define EM2820_BOARD_UNKNOWN			1
-#define EM2820_BOARD_TERRATEC_CINERGY_250	2
-#define EM2820_BOARD_PINNACLE_USB_2		3
-#define EM2820_BOARD_HAUPPAUGE_WINTV_USB_2      4
-#define EM2820_BOARD_MSI_VOX_USB_2              5
-#define EM2800_BOARD_TERRATEC_CINERGY_200       6
-#define EM2800_BOARD_LEADTEK_WINFAST_USBII      7
-#define EM2800_BOARD_KWORLD_USB2800             8
-#define EM2820_BOARD_PINNACLE_DVC_90		9
-#define EM2880_BOARD_HAUPPAUGE_WINTV_HVR_900	10
-#define EM2880_BOARD_TERRATEC_HYBRID_XS		11
-#define EM2820_BOARD_KWORLD_PVRTV2800RF		12
-#define EM2880_BOARD_TERRATEC_PRODIGY_XS	13
-#define EM2820_BOARD_PROLINK_PLAYTV_USB2	14
-#define EM2800_BOARD_VGEAR_POCKETTV             15
-#define EM2883_BOARD_HAUPPAUGE_WINTV_HVR_950	16
-#define EM2880_BOARD_PINNACLE_PCTV_HD_PRO	17
-#define EM2880_BOARD_HAUPPAUGE_WINTV_HVR_900_R2	18
-#define EM2860_BOARD_SAA711X_REFERENCE_DESIGN	19
-#define EM2880_BOARD_AMD_ATI_TV_WONDER_HD_600   20
-#define EM2800_BOARD_GRABBEEX_USB2800           21
+#define EM2800_BOARD_UNKNOWN			  0
+#define EM2820_BOARD_UNKNOWN			  1
+#define EM2820_BOARD_TERRATEC_CINERGY_250	  2
+#define EM2820_BOARD_PINNACLE_USB_2		  3
+#define EM2820_BOARD_HAUPPAUGE_WINTV_USB_2	  4
+#define EM2820_BOARD_MSI_VOX_USB_2		  5
+#define EM2800_BOARD_TERRATEC_CINERGY_200	  6
+#define EM2800_BOARD_LEADTEK_WINFAST_USBII	  7
+#define EM2800_BOARD_KWORLD_USB2800		  8
+#define EM2820_BOARD_PINNACLE_DVC_90		  9
+#define EM2880_BOARD_HAUPPAUGE_WINTV_HVR_900	  10
+#define EM2880_BOARD_TERRATEC_HYBRID_XS		  11
+#define EM2820_BOARD_KWORLD_PVRTV2800RF		  12
+#define EM2880_BOARD_TERRATEC_PRODIGY_XS	  13
+#define EM2820_BOARD_PROLINK_PLAYTV_USB2	  14
+#define EM2800_BOARD_VGEAR_POCKETTV		  15
+#define EM2883_BOARD_HAUPPAUGE_WINTV_HVR_950	  16
+#define EM2880_BOARD_PINNACLE_PCTV_HD_PRO	  17
+#define EM2880_BOARD_HAUPPAUGE_WINTV_HVR_900_R2	  18
+#define EM2860_BOARD_SAA711X_REFERENCE_DESIGN	  19
+#define EM2880_BOARD_AMD_ATI_TV_WONDER_HD_600	  20
+#define EM2800_BOARD_GRABBEEX_USB2800		  21
 #define EM2750_BOARD_UNKNOWN			  22
 #define EM2750_BOARD_DLCW_130			  23
 #define EM2820_BOARD_DLINK_USB_TV		  24
@@ -99,36 +99,37 @@
 #define EM2882_BOARD_KWORLD_VS_DVBT		  54
 #define EM2882_BOARD_TERRATEC_HYBRID_XS		  55
 #define EM2882_BOARD_PINNACLE_HYBRID_PRO_330E	  56
-#define EM2883_BOARD_KWORLD_HYBRID_330U                  57
+#define EM2883_BOARD_KWORLD_HYBRID_330U		  57
 #define EM2820_BOARD_COMPRO_VIDEOMATE_FORYOU	  58
 #define EM2883_BOARD_HAUPPAUGE_WINTV_HVR_850	  60
 #define EM2820_BOARD_PROLINK_PLAYTV_BOX4_USB2	  61
 #define EM2820_BOARD_GADMEI_TVR200		  62
-#define EM2860_BOARD_KAIOMY_TVNPC_U2              63
-#define EM2860_BOARD_EASYCAP                      64
+#define EM2860_BOARD_KAIOMY_TVNPC_U2		  63
+#define EM2860_BOARD_EASYCAP			  64
 #define EM2820_BOARD_IODATA_GVMVP_SZ		  65
 #define EM2880_BOARD_EMPIRE_DUAL_TV		  66
 #define EM2860_BOARD_TERRATEC_GRABBY		  67
 #define EM2860_BOARD_TERRATEC_AV350		  68
 #define EM2882_BOARD_KWORLD_ATSC_315U		  69
 #define EM2882_BOARD_EVGA_INDTUBE		  70
-#define EM2820_BOARD_SILVERCREST_WEBCAM           71
-#define EM2861_BOARD_GADMEI_UTV330PLUS           72
-#define EM2870_BOARD_REDDO_DVB_C_USB_BOX          73
+#define EM2820_BOARD_SILVERCREST_WEBCAM		  71
+#define EM2861_BOARD_GADMEI_UTV330PLUS		  72
+#define EM2870_BOARD_REDDO_DVB_C_USB_BOX	  73
 #define EM2800_BOARD_VC211A			  74
 #define EM2882_BOARD_DIKOM_DK300		  75
 #define EM2870_BOARD_KWORLD_A340		  76
 #define EM2874_BOARD_LEADERSHIP_ISDBT		  77
-#define EM28174_BOARD_PCTV_290E                   78
+#define EM28174_BOARD_PCTV_290E			  78
 #define EM2884_BOARD_TERRATEC_H5		  79
-#define EM28174_BOARD_PCTV_460E                   80
+#define EM28174_BOARD_PCTV_460E			  80
 #define EM2884_BOARD_HAUPPAUGE_WINTV_HVR_930C	  81
 #define EM2884_BOARD_CINERGY_HTC_STICK		  82
-#define EM2860_BOARD_HT_VIDBOX_NW03 		  83
-#define EM2874_BOARD_MAXMEDIA_UB425_TC            84
-#define EM2884_BOARD_PCTV_510E                    85
-#define EM2884_BOARD_PCTV_520E                    86
+#define EM2860_BOARD_HT_VIDBOX_NW03		  83
+#define EM2874_BOARD_MAXMEDIA_UB425_TC		  84
+#define EM2884_BOARD_PCTV_510E			  85
+#define EM2884_BOARD_PCTV_520E			  86
 #define EM2884_BOARD_TERRATEC_HTC_USB_XS	  87
+#define EM2884_BOARD_C3TECH_DIGITAL_DUO		  88
 
 /* Limits minimum and default number of buffers */
 #define EM28XX_MIN_BUF 4
@@ -157,6 +158,9 @@
 #define EM28XX_NUM_BUFS 5
 #define EM28XX_DVB_NUM_BUFS 5
 
+/* max number of I2C buses on em28xx devices */
+#define NUM_I2C_BUSES	2
+
 /* isoc transfers: number of packets for each buffer
    windows requests only 64 packets .. so we better do the same
    this is what I found out for all alternate numbers there!
@@ -172,27 +176,6 @@
 
 #define EM28XX_INTERLACED_DEFAULT 1
 
-/*
-#define (use usbview if you want to get the other alternate number infos)
-#define
-#define alternate number 2
-#define 			Endpoint Address: 82
-			Direction: in
-			Attribute: 1
-			Type: Isoc
-			Max Packet Size: 1448
-			Interval: 125us
-
-  alternate number 7
-
-			Endpoint Address: 82
-			Direction: in
-			Attribute: 1
-			Type: Isoc
-			Max Packet Size: 3072
-			Interval: 125us
-*/
-
 /* time in msecs to wait for i2c writes to finish */
 #define EM2800_I2C_XFER_TIMEOUT		20
 
@@ -381,6 +364,7 @@ enum em28xx_sensor {
 	EM28XX_MT9V011,
 	EM28XX_MT9M001,
 	EM28XX_MT9M111,
+	EM28XX_OV2640,
 };
 
 enum em28xx_adecoder {
@@ -393,6 +377,7 @@ struct em28xx_board {
 	int vchannels;
 	int tuner_type;
 	int tuner_addr;
+	unsigned def_i2c_bus;	/* Default I2C bus */
 
 	/* i2c flags */
 	unsigned int tda9887_conf;
@@ -426,15 +411,15 @@ struct em28xx_board {
 };
 
 struct em28xx_eeprom {
-	u32 id;			/* 0x9567eb1a */
-	u16 vendor_ID;
-	u16 product_ID;
+	u8 id[4];			/* 1a eb 67 95 */
+	__le16 vendor_ID;
+	__le16 product_ID;
 
-	u16 chip_conf;
+	__le16 chip_conf;
 
-	u16 board_conf;
+	__le16 board_conf;
 
-	u16 string1, string2, string3;
+	__le16 string1, string2, string3;
 
 	u8 string_idx_table;
 };
@@ -477,6 +462,20 @@ struct em28xx_fh {
 	enum v4l2_buf_type           type;
 };
 
+enum em28xx_i2c_algo_type {
+	EM28XX_I2C_ALGO_EM28XX = 0,
+	EM28XX_I2C_ALGO_EM2800,
+	EM28XX_I2C_ALGO_EM25XX_BUS_B,
+};
+
+struct em28xx_i2c_bus {
+	struct em28xx *dev;
+
+	unsigned bus;
+	enum em28xx_i2c_algo_type algo_type;
+};
+
+
 /* main device struct */
 struct em28xx {
 	/* generic device properties */
@@ -484,6 +483,7 @@ struct em28xx {
 	int model;		/* index in the device_data struct */
 	int devno;		/* marks the number of this device */
 	enum em28xx_chip_id chip_id;
+	unsigned int is_em25xx:1;	/* em25xx/em276x/7x/8x family bridge */
 
 	unsigned char disconnected:1;	/* device has been diconnected */
 
@@ -491,8 +491,6 @@ struct em28xx {
 
 	struct v4l2_device v4l2_dev;
 	struct v4l2_ctrl_handler ctrl_handler;
-	/* provides ac97 mute and volume overrides */
-	struct v4l2_ctrl_handler ac97_ctrl_handler;
 	struct em28xx_board board;
 
 	/* Webcam specific fields */
@@ -511,8 +509,8 @@ struct em28xx {
 	unsigned int is_audio_only:1;
 
 	/* Controls audio streaming */
-	struct work_struct wq_trigger;              /* Trigger to start/stop audio for alsa module */
-	 atomic_t       stream_started;      /* stream should be running if true */
+	struct work_struct wq_trigger;	/* Trigger to start/stop audio for alsa module */
+	atomic_t       stream_started;	/* stream should be running if true */
 
 	struct em28xx_fmt *format;
 
@@ -530,9 +528,17 @@ struct em28xx {
 	int tuner_type;		/* type of the tuner */
 	int tuner_addr;		/* tuner address */
 	int tda9887_conf;
+
 	/* i2c i/o */
-	struct i2c_adapter i2c_adap;
-	struct i2c_client i2c_client;
+	struct i2c_adapter i2c_adap[NUM_I2C_BUSES];
+	struct i2c_client i2c_client[NUM_I2C_BUSES];
+	struct em28xx_i2c_bus i2c_bus[NUM_I2C_BUSES];
+
+	unsigned char eeprom_addrwidth_16bit:1;
+	unsigned def_i2c_bus;	/* Default I2C bus */
+	unsigned cur_i2c_bus;	/* Current I2C bus */
+	struct rt_mutex i2c_bus_lock;
+
 	/* video for linux */
 	int users;		/* user count for exclusive use */
 	int streaming_users;    /* Number of actively streaming users */
@@ -584,7 +590,9 @@ struct em28xx {
 	/* resources in use */
 	unsigned int resources;
 
-	unsigned char eedata[256];
+	/* eeprom content */
+	u8 *eedata;
+	u16 eedata_len;
 
 	/* Isoc control struct */
 	struct em28xx_dmaqueue vidq;
@@ -600,7 +608,7 @@ struct em28xx {
 	u8 analog_ep_isoc;	/* address of isoc endpoint for analog */
 	u8 analog_ep_bulk;	/* address of bulk endpoint for analog */
 	u8 dvb_ep_isoc;		/* address of isoc endpoint for DVB */
-	u8 dvb_ep_bulk;		/* address of bulk endpoint for DVC */
+	u8 dvb_ep_bulk;		/* address of bulk endpoint for DVB */
 	int alt;		/* alternate setting */
 	int max_pkt_size;	/* max packet size of the selected ep at alt */
 	int packet_multiplier;	/* multiplier for wMaxPacketSize, used for
@@ -651,16 +659,12 @@ struct em28xx_ops {
 };
 
 /* Provided by em28xx-i2c.c */
-void em28xx_do_i2c_scan(struct em28xx *dev);
-int  em28xx_i2c_register(struct em28xx *dev);
-int  em28xx_i2c_unregister(struct em28xx *dev);
+void em28xx_do_i2c_scan(struct em28xx *dev, unsigned bus);
+int  em28xx_i2c_register(struct em28xx *dev, unsigned bus,
+			 enum em28xx_i2c_algo_type algo_type);
+int  em28xx_i2c_unregister(struct em28xx *dev, unsigned bus);
 
 /* Provided by em28xx-core.c */
-
-u32 em28xx_request_buffers(struct em28xx *dev, u32 count);
-void em28xx_queue_unusedframes(struct em28xx *dev);
-void em28xx_release_buffers(struct em28xx *dev);
-
 int em28xx_read_reg_req_len(struct em28xx *dev, u8 req, u16 reg,
 			    char *buf, int len);
 int em28xx_read_reg_req(struct em28xx *dev, u8 req, u16 reg);
@@ -693,7 +697,6 @@ int em28xx_init_usb_xfer(struct em28xx *dev, enum em28xx_mode mode,
 					(struct em28xx *dev, struct urb *urb));
 void em28xx_uninit_usb_xfer(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);
 void em28xx_wake_i2c(struct em28xx *dev);
@@ -712,16 +715,18 @@ int em28xx_stop_vbi_streaming(struct vb2_queue *vq);
 extern const struct v4l2_ctrl_ops em28xx_ctrl_ops;
 
 /* Provided by em28xx-cards.c */
-extern int em2800_variant_detect(struct usb_device *udev, int model);
 extern struct em28xx_board em28xx_boards[];
 extern struct usb_device_id em28xx_id_table[];
-extern const unsigned int em28xx_bcount;
 int em28xx_tuner_callback(void *ptr, int component, int command, int arg);
 void em28xx_release_resources(struct em28xx *dev);
 
 /* Provided by em28xx-vbi.c */
 extern struct vb2_ops em28xx_vbi_qops;
 
+/* Provided by em28xx-camera.c */
+int em28xx_detect_sensor(struct em28xx *dev);
+int em28xx_init_camera(struct em28xx *dev);
+
 /* printk macros */
 
 #define em28xx_err(fmt, arg...) do {\
@@ -744,72 +749,6 @@ static inline int em28xx_compression_disable(struct em28xx *dev)
 	return em28xx_write_reg(dev, EM28XX_R26_COMPR, 0x00);
 }
 
-static inline int em28xx_contrast_get(struct em28xx *dev)
-{
-	return em28xx_read_reg(dev, EM28XX_R20_YGAIN) & 0x1f;
-}
-
-static inline int em28xx_brightness_get(struct em28xx *dev)
-{
-	return em28xx_read_reg(dev, EM28XX_R21_YOFFSET);
-}
-
-static inline int em28xx_saturation_get(struct em28xx *dev)
-{
-	return em28xx_read_reg(dev, EM28XX_R22_UVGAIN) & 0x1f;
-}
-
-static inline int em28xx_u_balance_get(struct em28xx *dev)
-{
-	return em28xx_read_reg(dev, EM28XX_R23_UOFFSET);
-}
-
-static inline int em28xx_v_balance_get(struct em28xx *dev)
-{
-	return em28xx_read_reg(dev, EM28XX_R24_VOFFSET);
-}
-
-static inline int em28xx_gamma_get(struct em28xx *dev)
-{
-	return em28xx_read_reg(dev, EM28XX_R14_GAMMA) & 0x3f;
-}
-
-static inline int em28xx_contrast_set(struct em28xx *dev, s32 val)
-{
-	u8 tmp = (u8) val;
-	return em28xx_write_regs(dev, EM28XX_R20_YGAIN, &tmp, 1);
-}
-
-static inline int em28xx_brightness_set(struct em28xx *dev, s32 val)
-{
-	u8 tmp = (u8) val;
-	return em28xx_write_regs(dev, EM28XX_R21_YOFFSET, &tmp, 1);
-}
-
-static inline int em28xx_saturation_set(struct em28xx *dev, s32 val)
-{
-	u8 tmp = (u8) val;
-	return em28xx_write_regs(dev, EM28XX_R22_UVGAIN, &tmp, 1);
-}
-
-static inline int em28xx_u_balance_set(struct em28xx *dev, s32 val)
-{
-	u8 tmp = (u8) val;
-	return em28xx_write_regs(dev, EM28XX_R23_UOFFSET, &tmp, 1);
-}
-
-static inline int em28xx_v_balance_set(struct em28xx *dev, s32 val)
-{
-	u8 tmp = (u8) val;
-	return em28xx_write_regs(dev, EM28XX_R24_VOFFSET, &tmp, 1);
-}
-
-static inline int em28xx_gamma_set(struct em28xx *dev, s32 val)
-{
-	u8 tmp = (u8) val;
-	return em28xx_write_regs(dev, EM28XX_R14_GAMMA, &tmp, 1);
-}
-
 /*FIXME: maxw should be dependent of alt mode */
 static inline unsigned int norm_maxw(struct em28xx *dev)
 {
diff --git a/drivers/media/usb/gspca/autogain_functions.h b/drivers/media/usb/gspca/autogain_functions.h
deleted file mode 100644
index d625eafe63e..00000000000
--- a/drivers/media/usb/gspca/autogain_functions.h
+++ /dev/null
@@ -1,183 +0,0 @@
-/*
- * Functions for auto gain.
- *
- * Copyright (C) 2010-2011 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
- */
-
-#ifdef WANT_REGULAR_AUTOGAIN
-/* 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. */
-static inline int auto_gain_n_exposure(
-			struct gspca_dev *gspca_dev,
-			int avg_lum,
-			int desired_avg_lum,
-			int deadzone,
-			int gain_knee,
-			int exposure_knee)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	int i, steps, gain, orig_gain, exposure, orig_exposure;
-	int retval = 0;
-
-	orig_gain = gain = sd->ctrls[GAIN].val;
-	orig_exposure = exposure = sd->ctrls[EXPOSURE].val;
-
-	/* 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 > sd->ctrls[GAIN].def)
-				gain--;
-			else if (exposure > sd->ctrls[EXPOSURE].min)
-				exposure--;
-			else if (gain > sd->ctrls[GAIN].min)
-				gain--;
-			else
-				break;
-		} else {
-			if (gain < sd->ctrls[GAIN].def)
-				gain++;
-			else if (exposure < exposure_knee)
-				exposure++;
-			else if (gain < gain_knee)
-				gain++;
-			else if (exposure < sd->ctrls[EXPOSURE].max)
-				exposure++;
-			else if (gain < sd->ctrls[GAIN].max)
-				gain++;
-			else
-				break;
-		}
-	}
-
-	if (gain != orig_gain) {
-		sd->ctrls[GAIN].val = gain;
-		setgain(gspca_dev);
-		retval = 1;
-	}
-	if (exposure != orig_exposure) {
-		sd->ctrls[EXPOSURE].val = exposure;
-		setexposure(gspca_dev);
-		retval = 1;
-	}
-
-	if (retval)
-		PDEBUG(D_FRAM, "autogain: changed gain: %d, expo: %d",
-			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
-   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.
-
-   Note this assumes that the sd struct for the cam in question has
-   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. */
-static inline int coarse_grained_expo_autogain(
-			struct gspca_dev *gspca_dev,
-			int avg_lum,
-			int desired_avg_lum,
-			int deadzone)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	int steps, gain, orig_gain, exposure, orig_exposure;
-	int gain_low, gain_high;
-	int retval = 0;
-
-	orig_gain = gain = sd->ctrls[GAIN].val;
-	orig_exposure = exposure = sd->ctrls[EXPOSURE].val;
-
-	gain_low = (sd->ctrls[GAIN].max - sd->ctrls[GAIN].min) / 5 * 2;
-	gain_low += sd->ctrls[GAIN].min;
-	gain_high = (sd->ctrls[GAIN].max - sd->ctrls[GAIN].min) / 5 * 4;
-	gain_high += sd->ctrls[GAIN].min;
-
-	/* 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 < sd->ctrls[EXPOSURE].max) {
-		gain = gain_high;
-		sd->exp_too_low_cnt++;
-		sd->exp_too_high_cnt = 0;
-	} else if ((gain + steps) < gain_low &&
-		   exposure > sd->ctrls[EXPOSURE].min) {
-		gain = gain_low;
-		sd->exp_too_high_cnt++;
-		sd->exp_too_low_cnt = 0;
-	} else {
-		gain += steps;
-		if (gain > sd->ctrls[GAIN].max)
-			gain = sd->ctrls[GAIN].max;
-		else if (gain < sd->ctrls[GAIN].min)
-			gain = sd->ctrls[GAIN].min;
-		sd->exp_too_high_cnt = 0;
-		sd->exp_too_low_cnt = 0;
-	}
-
-	if (sd->exp_too_high_cnt > 3) {
-		exposure--;
-		sd->exp_too_high_cnt = 0;
-	} else if (sd->exp_too_low_cnt > 3) {
-		exposure++;
-		sd->exp_too_low_cnt = 0;
-	}
-
-	if (gain != orig_gain) {
-		sd->ctrls[GAIN].val = gain;
-		setgain(gspca_dev);
-		retval = 1;
-	}
-	if (exposure != orig_exposure) {
-		sd->ctrls[EXPOSURE].val = exposure;
-		setexposure(gspca_dev);
-		retval = 1;
-	}
-
-	if (retval)
-		PDEBUG(D_FRAM, "autogain: changed gain: %d, expo: %d",
-			gain, exposure);
-	return retval;
-}
-#endif
diff --git a/drivers/media/usb/gspca/benq.c b/drivers/media/usb/gspca/benq.c
index 352f32190e6..05f406deae1 100644
--- a/drivers/media/usb/gspca/benq.c
+++ b/drivers/media/usb/gspca/benq.c
@@ -186,7 +186,7 @@ static void sd_isoc_irq(struct urb *urb)
 		/* check the packet status and length */
 		if (urb0->iso_frame_desc[i].actual_length != SD_PKT_SZ
 		    || urb->iso_frame_desc[i].actual_length != SD_PKT_SZ) {
-			PDEBUG(D_ERR, "ISOC bad lengths %d / %d",
+			PERR("ISOC bad lengths %d / %d",
 				urb0->iso_frame_desc[i].actual_length,
 				urb->iso_frame_desc[i].actual_length);
 			gspca_dev->last_packet_type = DISCARD_PACKET;
diff --git a/drivers/media/usb/gspca/conex.c b/drivers/media/usb/gspca/conex.c
index c9052f20435..38714df31ac 100644
--- a/drivers/media/usb/gspca/conex.c
+++ b/drivers/media/usb/gspca/conex.c
@@ -73,12 +73,11 @@ static void reg_r(struct gspca_dev *gspca_dev,
 {
 	struct usb_device *dev = gspca_dev->dev;
 
-#ifdef GSPCA_DEBUG
 	if (len > USB_BUF_SZ) {
-		pr_err("reg_r: buffer overflow\n");
+		PERR("reg_r: buffer overflow\n");
 		return;
 	}
-#endif
+
 	usb_control_msg(dev,
 			usb_rcvctrlpipe(dev, 0),
 			0,
@@ -113,13 +112,12 @@ static void reg_w(struct gspca_dev *gspca_dev,
 {
 	struct usb_device *dev = gspca_dev->dev;
 
-#ifdef GSPCA_DEBUG
 	if (len > USB_BUF_SZ) {
-		pr_err("reg_w: buffer overflow\n");
+		PERR("reg_w: buffer overflow\n");
 		return;
 	}
 	PDEBUG(D_USBO, "reg write [%02x] = %02x..", index, *buffer);
-#endif
+
 	memcpy(gspca_dev->usb_buf, buffer, len);
 	usb_control_msg(dev,
 			usb_sndctrlpipe(dev, 0),
@@ -689,7 +687,7 @@ static void cx11646_jpeg(struct gspca_dev*gspca_dev)
 		reg_w_val(gspca_dev, 0x0053, 0x00);
 	} while (--retry);
 	if (retry == 0)
-		PDEBUG(D_ERR, "Damned Errors sending jpeg Table");
+		PERR("Damned Errors sending jpeg Table");
 	/* send the qtable now */
 	reg_r(gspca_dev, 0x0001, 1);		/* -> 0x18 */
 	length = 8;
diff --git a/drivers/media/usb/gspca/cpia1.c b/drivers/media/usb/gspca/cpia1.c
index 1dcdd9f95f1..064b53043b1 100644
--- a/drivers/media/usb/gspca/cpia1.c
+++ b/drivers/media/usb/gspca/cpia1.c
@@ -421,8 +421,7 @@ static int cpia_usb_transferCmd(struct gspca_dev *gspca_dev, u8 *command)
 		pipe = usb_sndctrlpipe(gspca_dev->dev, 0);
 		requesttype = USB_TYPE_VENDOR | USB_RECIP_DEVICE;
 	} else {
-		PDEBUG(D_ERR, "Unexpected first byte of command: %x",
-		       command[0]);
+		PERR("Unexpected first byte of command: %x", command[0]);
 		return -EINVAL;
 	}
 
@@ -701,7 +700,7 @@ static void reset_camera_params(struct gspca_dev *gspca_dev)
 	params->qx3.cradled = 0;
 }
 
-static void printstatus(struct cam_params *params)
+static void printstatus(struct gspca_dev *gspca_dev, struct cam_params *params)
 {
 	PDEBUG(D_PROBE, "status: %02x %02x %02x %02x %02x %02x %02x %02x",
 	       params->status.systemState, params->status.grabState,
@@ -725,10 +724,9 @@ static int goto_low_power(struct gspca_dev *gspca_dev)
 
 	if (sd->params.status.systemState != LO_POWER_STATE) {
 		if (sd->params.status.systemState != WARM_BOOT_STATE) {
-			PDEBUG(D_ERR,
-			       "unexpected state after lo power cmd: %02x",
-			       sd->params.status.systemState);
-			printstatus(&sd->params);
+			PERR("unexpected state after lo power cmd: %02x",
+			     sd->params.status.systemState);
+			printstatus(gspca_dev, &sd->params);
 		}
 		return -EIO;
 	}
@@ -756,9 +754,9 @@ static int goto_high_power(struct gspca_dev *gspca_dev)
 		return ret;
 
 	if (sd->params.status.systemState != HI_POWER_STATE) {
-		PDEBUG(D_ERR, "unexpected state after hi power cmd: %02x",
-			       sd->params.status.systemState);
-		printstatus(&sd->params);
+		PERR("unexpected state after hi power cmd: %02x",
+		     sd->params.status.systemState);
+		printstatus(gspca_dev, &sd->params);
 		return -EIO;
 	}
 
@@ -1449,8 +1447,8 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	sd->params.version.firmwareVersion = 0;
 	get_version_information(gspca_dev);
 	if (sd->params.version.firmwareVersion != 1) {
-		PDEBUG(D_ERR, "only firmware version 1 is supported (got: %d)",
-		       sd->params.version.firmwareVersion);
+		PERR("only firmware version 1 is supported (got: %d)",
+		     sd->params.version.firmwareVersion);
 		return -ENODEV;
 	}
 
@@ -1475,9 +1473,9 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	/* Start the camera in low power mode */
 	if (goto_low_power(gspca_dev)) {
 		if (sd->params.status.systemState != WARM_BOOT_STATE) {
-			PDEBUG(D_ERR, "unexpected systemstate: %02x",
-			       sd->params.status.systemState);
-			printstatus(&sd->params);
+			PERR("unexpected systemstate: %02x",
+			     sd->params.status.systemState);
+			printstatus(gspca_dev, &sd->params);
 			return -ENODEV;
 		}
 
@@ -1523,9 +1521,8 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		return ret;
 
 	if (sd->params.status.fatalError) {
-		PDEBUG(D_ERR, "fatal_error: %04x, vp_status: %04x",
-		       sd->params.status.fatalError,
-		       sd->params.status.vpStatus);
+		PERR("fatal_error: %04x, vp_status: %04x",
+		     sd->params.status.fatalError, sd->params.status.vpStatus);
 		return -EIO;
 	}
 
diff --git a/drivers/media/usb/gspca/etoms.c b/drivers/media/usb/gspca/etoms.c
index 38f68e11c3a..26c9ee1f104 100644
--- a/drivers/media/usb/gspca/etoms.c
+++ b/drivers/media/usb/gspca/etoms.c
@@ -163,12 +163,11 @@ static void reg_r(struct gspca_dev *gspca_dev,
 {
 	struct usb_device *dev = gspca_dev->dev;
 
-#ifdef GSPCA_DEBUG
 	if (len > USB_BUF_SZ) {
-		pr_err("reg_r: buffer overflow\n");
+		PERR("reg_r: buffer overflow\n");
 		return;
 	}
-#endif
+
 	usb_control_msg(dev,
 			usb_rcvctrlpipe(dev, 0),
 			0,
@@ -201,13 +200,12 @@ static void reg_w(struct gspca_dev *gspca_dev,
 {
 	struct usb_device *dev = gspca_dev->dev;
 
-#ifdef GSPCA_DEBUG
 	if (len > USB_BUF_SZ) {
 		pr_err("reg_w: buffer overflow\n");
 		return;
 	}
 	PDEBUG(D_USBO, "reg write [%02x] = %02x..", index, *buffer);
-#endif
+
 	memcpy(gspca_dev->usb_buf, buffer, len);
 	usb_control_msg(dev,
 			usb_sndctrlpipe(dev, 0),
@@ -274,7 +272,7 @@ static int et_video(struct gspca_dev *gspca_dev,
 		     : 0);		/* stopvideo */
 	ret = Et_WaitStatus(gspca_dev);
 	if (ret != 0)
-		PDEBUG(D_ERR, "timeout video on/off");
+		PERR("timeout video on/off");
 	return ret;
 }
 
@@ -768,9 +766,7 @@ static const struct sd_desc sd_desc = {
 /* -- module initialisation -- */
 static const struct usb_device_id device_table[] = {
 	{USB_DEVICE(0x102c, 0x6151), .driver_info = SENSOR_PAS106},
-#if !defined CONFIG_USB_ET61X251 && !defined CONFIG_USB_ET61X251_MODULE
 	{USB_DEVICE(0x102c, 0x6251), .driver_info = SENSOR_TAS5130CXX},
-#endif
 	{}
 };
 
diff --git a/drivers/media/usb/gspca/gl860/gl860.c b/drivers/media/usb/gspca/gl860/gl860.c
index ced3b71f14e..cb1e64ca59c 100644
--- a/drivers/media/usb/gspca/gl860/gl860.c
+++ b/drivers/media/usb/gspca/gl860/gl860.c
@@ -58,115 +58,135 @@ MODULE_PARM_DESC(sensor,
 
 /*============================ webcam controls =============================*/
 
-/* Functions to get and set a control value */
-#define SD_SETGET(thename) \
-static int sd_set_##thename(struct gspca_dev *gspca_dev, s32 val)\
-{\
-	struct sd *sd = (struct sd *) gspca_dev;\
-\
-	sd->vcur.thename = val;\
-	if (gspca_dev->streaming)\
-		sd->waitSet = 1;\
-	return 0;\
-} \
-static int sd_get_##thename(struct gspca_dev *gspca_dev, s32 *val)\
-{\
-	struct sd *sd = (struct sd *) gspca_dev;\
-\
-	*val = sd->vcur.thename;\
-	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;
 
-SD_SETGET(mirror)
-SD_SETGET(flip)
-SD_SETGET(AC50Hz)
-SD_SETGET(backlight)
-SD_SETGET(brightness)
-SD_SETGET(gamma)
-SD_SETGET(hue)
-SD_SETGET(saturation)
-SD_SETGET(sharpness)
-SD_SETGET(whitebal)
-SD_SETGET(contrast)
-
-#define GL860_NCTRLS 11
-
-/* control table */
-static struct ctrl sd_ctrls_mi1320[GL860_NCTRLS];
-static struct ctrl sd_ctrls_mi2020[GL860_NCTRLS];
-static struct ctrl sd_ctrls_ov2640[GL860_NCTRLS];
-static struct ctrl sd_ctrls_ov9655[GL860_NCTRLS];
-
-#define SET_MY_CTRL(theid, \
-	thetype, thelabel, thename) \
-	if (sd->vmax.thename != 0) {\
-		sd_ctrls[nCtrls].qctrl.id   = theid;\
-		sd_ctrls[nCtrls].qctrl.type = thetype;\
-		strcpy(sd_ctrls[nCtrls].qctrl.name, thelabel);\
-		sd_ctrls[nCtrls].qctrl.minimum = 0;\
-		sd_ctrls[nCtrls].qctrl.maximum = sd->vmax.thename;\
-		sd_ctrls[nCtrls].qctrl.default_value = sd->vcur.thename;\
-		sd_ctrls[nCtrls].qctrl.step = \
-			(sd->vmax.thename < 16) ? 1 : sd->vmax.thename/16;\
-		sd_ctrls[nCtrls].set = sd_set_##thename;\
-		sd_ctrls[nCtrls].get = sd_get_##thename;\
-		nCtrls++;\
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		sd->vcur.brightness = ctrl->val;
+		break;
+	case V4L2_CID_CONTRAST:
+		sd->vcur.contrast = ctrl->val;
+		break;
+	case V4L2_CID_SATURATION:
+		sd->vcur.saturation = ctrl->val;
+		break;
+	case V4L2_CID_HUE:
+		sd->vcur.hue = ctrl->val;
+		break;
+	case V4L2_CID_GAMMA:
+		sd->vcur.gamma = ctrl->val;
+		break;
+	case V4L2_CID_HFLIP:
+		sd->vcur.mirror = ctrl->val;
+		break;
+	case V4L2_CID_VFLIP:
+		sd->vcur.flip = ctrl->val;
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		sd->vcur.AC50Hz = ctrl->val;
+		break;
+	case V4L2_CID_WHITE_BALANCE_TEMPERATURE:
+		sd->vcur.whitebal = ctrl->val;
+		break;
+	case V4L2_CID_SHARPNESS:
+		sd->vcur.sharpness = ctrl->val;
+		break;
+	case V4L2_CID_BACKLIGHT_COMPENSATION:
+		sd->vcur.backlight = ctrl->val;
+		break;
+	default:
+		return -EINVAL;
 	}
 
-static int gl860_build_control_table(struct gspca_dev *gspca_dev)
+	if (gspca_dev->streaming)
+		sd->waitSet = 1;
+
+	return 0;
+}
+
+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 ctrl *sd_ctrls;
-	int nCtrls = 0;
-
-	if (_MI1320_)
-		sd_ctrls = sd_ctrls_mi1320;
-	else if (_MI2020_)
-		sd_ctrls = sd_ctrls_mi2020;
-	else if (_OV2640_)
-		sd_ctrls = sd_ctrls_ov2640;
-	else if (_OV9655_)
-		sd_ctrls = sd_ctrls_ov9655;
-	else
-		return 0;
-
-	memset(sd_ctrls, 0, GL860_NCTRLS * sizeof(struct ctrl));
-
-	SET_MY_CTRL(V4L2_CID_BRIGHTNESS,
-		V4L2_CTRL_TYPE_INTEGER, "Brightness", brightness)
-	SET_MY_CTRL(V4L2_CID_SHARPNESS,
-		V4L2_CTRL_TYPE_INTEGER, "Sharpness", sharpness)
-	SET_MY_CTRL(V4L2_CID_CONTRAST,
-		V4L2_CTRL_TYPE_INTEGER, "Contrast", contrast)
-	SET_MY_CTRL(V4L2_CID_GAMMA,
-		V4L2_CTRL_TYPE_INTEGER, "Gamma", gamma)
-	SET_MY_CTRL(V4L2_CID_HUE,
-		V4L2_CTRL_TYPE_INTEGER, "Palette", hue)
-	SET_MY_CTRL(V4L2_CID_SATURATION,
-		V4L2_CTRL_TYPE_INTEGER, "Saturation", saturation)
-	SET_MY_CTRL(V4L2_CID_WHITE_BALANCE_TEMPERATURE,
-		V4L2_CTRL_TYPE_INTEGER, "White Bal.", whitebal)
-	SET_MY_CTRL(V4L2_CID_BACKLIGHT_COMPENSATION,
-		V4L2_CTRL_TYPE_INTEGER, "Backlight" , backlight)
-
-	SET_MY_CTRL(V4L2_CID_HFLIP,
-		V4L2_CTRL_TYPE_BOOLEAN, "Mirror", mirror)
-	SET_MY_CTRL(V4L2_CID_VFLIP,
-		V4L2_CTRL_TYPE_BOOLEAN, "Flip", flip)
-	SET_MY_CTRL(V4L2_CID_POWER_LINE_FREQUENCY,
-		V4L2_CTRL_TYPE_BOOLEAN, "AC power 50Hz", AC50Hz)
-
-	return nCtrls;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 11);
+
+	if (sd->vmax.brightness)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops, V4L2_CID_BRIGHTNESS,
+				  0, sd->vmax.brightness, 1,
+				  sd->vcur.brightness);
+
+	if (sd->vmax.contrast)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops, V4L2_CID_CONTRAST,
+				  0, sd->vmax.contrast, 1,
+				  sd->vcur.contrast);
+
+	if (sd->vmax.saturation)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops, V4L2_CID_SATURATION,
+				  0, sd->vmax.saturation, 1,
+				  sd->vcur.saturation);
+
+	if (sd->vmax.hue)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops, V4L2_CID_HUE,
+				  0, sd->vmax.hue, 1, sd->vcur.hue);
+
+	if (sd->vmax.gamma)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops, V4L2_CID_GAMMA,
+				  0, sd->vmax.gamma, 1, sd->vcur.gamma);
+
+	if (sd->vmax.mirror)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops, V4L2_CID_HFLIP,
+				  0, sd->vmax.mirror, 1, sd->vcur.mirror);
+
+	if (sd->vmax.flip)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops, V4L2_CID_VFLIP,
+				  0, sd->vmax.flip, 1, sd->vcur.flip);
+
+	if (sd->vmax.AC50Hz)
+		v4l2_ctrl_new_std_menu(hdl, &sd_ctrl_ops,
+				  V4L2_CID_POWER_LINE_FREQUENCY,
+				  sd->vmax.AC50Hz, 0, sd->vcur.AC50Hz);
+
+	if (sd->vmax.whitebal)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+				  V4L2_CID_WHITE_BALANCE_TEMPERATURE,
+				  0, sd->vmax.whitebal, 1, sd->vcur.whitebal);
+
+	if (sd->vmax.sharpness)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops, V4L2_CID_SHARPNESS,
+				  0, sd->vmax.sharpness, 1,
+				  sd->vcur.sharpness);
+
+	if (sd->vmax.backlight)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+				  V4L2_CID_BACKLIGHT_COMPENSATION,
+				  0, sd->vmax.backlight, 1,
+				  sd->vcur.backlight);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+
+	return 0;
 }
 
 /*==================== sud-driver structure initialisation =================*/
 
 static const struct sd_desc sd_desc_mi1320 = {
 	.name        = MODULE_NAME,
-	.ctrls       = sd_ctrls_mi1320,
-	.nctrls      = GL860_NCTRLS,
 	.config      = sd_config,
 	.init        = sd_init,
+	.init_controls = sd_init_controls,
 	.isoc_init   = sd_isoc_init,
 	.start       = sd_start,
 	.stop0       = sd_stop0,
@@ -176,10 +196,9 @@ static const struct sd_desc sd_desc_mi1320 = {
 
 static const struct sd_desc sd_desc_mi2020 = {
 	.name        = MODULE_NAME,
-	.ctrls       = sd_ctrls_mi2020,
-	.nctrls      = GL860_NCTRLS,
 	.config      = sd_config,
 	.init        = sd_init,
+	.init_controls = sd_init_controls,
 	.isoc_init   = sd_isoc_init,
 	.start       = sd_start,
 	.stop0       = sd_stop0,
@@ -189,10 +208,9 @@ static const struct sd_desc sd_desc_mi2020 = {
 
 static const struct sd_desc sd_desc_ov2640 = {
 	.name        = MODULE_NAME,
-	.ctrls       = sd_ctrls_ov2640,
-	.nctrls      = GL860_NCTRLS,
 	.config      = sd_config,
 	.init        = sd_init,
+	.init_controls = sd_init_controls,
 	.isoc_init   = sd_isoc_init,
 	.start       = sd_start,
 	.stop0       = sd_stop0,
@@ -202,10 +220,9 @@ static const struct sd_desc sd_desc_ov2640 = {
 
 static const struct sd_desc sd_desc_ov9655 = {
 	.name        = MODULE_NAME,
-	.ctrls       = sd_ctrls_ov9655,
-	.nctrls      = GL860_NCTRLS,
 	.config      = sd_config,
 	.init        = sd_init,
+	.init_controls = sd_init_controls,
 	.isoc_init   = sd_isoc_init,
 	.start       = sd_start,
 	.stop0       = sd_stop0,
@@ -371,7 +388,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	dev_init_settings(gspca_dev);
 	if (AC50Hz != 0xff)
 		((struct sd *) gspca_dev)->vcur.AC50Hz = AC50Hz;
-	gl860_build_control_table(gspca_dev);
 
 	return 0;
 }
@@ -566,7 +582,7 @@ int gl860_RTx(struct gspca_dev *gspca_dev,
 		pr_err("ctrl transfer failed %4d [p%02x r%d v%04x i%04x len%d]\n",
 		       r, pref, req, val, index, len);
 	else if (len > 1 && r < len)
-		PDEBUG(D_ERR, "short ctrl transfer %d/%d", r, len);
+		PERR("short ctrl transfer %d/%d", r, len);
 
 	msleep(1);
 
diff --git a/drivers/media/usb/gspca/gspca.c b/drivers/media/usb/gspca/gspca.c
index 3564bdbb2ea..5995ec4de6b 100644
--- a/drivers/media/usb/gspca/gspca.c
+++ b/drivers/media/usb/gspca/gspca.c
@@ -60,14 +60,14 @@ MODULE_DESCRIPTION("GSPCA USB Camera Driver");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(GSPCA_VERSION);
 
-#ifdef GSPCA_DEBUG
-int gspca_debug = D_ERR | D_PROBE;
+int gspca_debug;
 EXPORT_SYMBOL(gspca_debug);
 
-static void PDEBUG_MODE(char *txt, __u32 pixfmt, int w, int h)
+static void PDEBUG_MODE(struct gspca_dev *gspca_dev, int debug, char *txt,
+			__u32 pixfmt, int w, int h)
 {
 	if ((pixfmt >> 24) >= '0' && (pixfmt >> 24) <= 'z') {
-		PDEBUG(D_CONF|D_STREAM, "%s %c%c%c%c %dx%d",
+		PDEBUG(debug, "%s %c%c%c%c %dx%d",
 			txt,
 			pixfmt & 0xff,
 			(pixfmt >> 8) & 0xff,
@@ -75,15 +75,12 @@ static void PDEBUG_MODE(char *txt, __u32 pixfmt, int w, int h)
 			pixfmt >> 24,
 			w, h);
 	} else {
-		PDEBUG(D_CONF|D_STREAM, "%s 0x%08x %dx%d",
+		PDEBUG(debug, "%s 0x%08x %dx%d",
 			txt,
 			pixfmt,
 			w, h);
 	}
 }
-#else
-#define PDEBUG_MODE(txt, pixfmt, w, h)
-#endif
 
 /* specific memory types - !! should be different from V4L2_MEMORY_xxx */
 #define GSPCA_MEMORY_NO 0	/* V4L2_MEMORY_xxx starts from 1 */
@@ -129,7 +126,7 @@ static void int_irq(struct urb *urb)
 	case 0:
 		if (gspca_dev->sd_desc->int_pkt_scan(gspca_dev,
 		    urb->transfer_buffer, urb->actual_length) < 0) {
-			PDEBUG(D_ERR, "Unknown packet received");
+			PERR("Unknown packet received");
 		}
 		break;
 
@@ -143,7 +140,7 @@ static void int_irq(struct urb *urb)
 		break;
 
 	default:
-		PDEBUG(D_ERR, "URB error %i, resubmitting", urb->status);
+		PERR("URB error %i, resubmitting", urb->status);
 		urb->status = 0;
 		ret = 0;
 	}
@@ -229,7 +226,7 @@ static int alloc_and_submit_int_urb(struct gspca_dev *gspca_dev,
 	urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
 	ret = usb_submit_urb(urb, GFP_KERNEL);
 	if (ret < 0) {
-		PDEBUG(D_ERR, "submit int URB failed with error %i", ret);
+		PERR("submit int URB failed with error %i", ret);
 		goto error_submit;
 	}
 	gspca_dev->int_urb = urb;
@@ -315,7 +312,7 @@ static void fill_frame(struct gspca_dev *gspca_dev,
 		if (gspca_dev->frozen)
 			return;
 #endif
-		PDEBUG(D_ERR|D_PACK, "urb status: %d", urb->status);
+		PERR("urb status: %d", urb->status);
 		urb->status = 0;
 		goto resubmit;
 	}
@@ -388,7 +385,7 @@ static void bulk_irq(struct urb *urb)
 		if (gspca_dev->frozen)
 			return;
 #endif
-		PDEBUG(D_ERR|D_PACK, "urb status: %d", urb->status);
+		PERR("urb status: %d", urb->status);
 		urb->status = 0;
 		goto resubmit;
 	}
@@ -460,7 +457,7 @@ void gspca_frame_add(struct gspca_dev *gspca_dev,
 	/* append the packet to the frame buffer */
 	if (len > 0) {
 		if (gspca_dev->image_len + len > gspca_dev->frsz) {
-			PDEBUG(D_ERR|D_PACK, "frame overflow %d > %d",
+			PERR("frame overflow %d > %d",
 				gspca_dev->image_len + len,
 				gspca_dev->frsz);
 			packet_type = DISCARD_PACKET;
@@ -570,11 +567,10 @@ static void destroy_urbs(struct gspca_dev *gspca_dev)
 
 		gspca_dev->urb[i] = NULL;
 		usb_kill_urb(urb);
-		if (urb->transfer_buffer != NULL)
-			usb_free_coherent(gspca_dev->dev,
-					  urb->transfer_buffer_length,
-					  urb->transfer_buffer,
-					  urb->transfer_dma);
+		usb_free_coherent(gspca_dev->dev,
+				  urb->transfer_buffer_length,
+				  urb->transfer_buffer,
+				  urb->transfer_dma);
 		usb_free_urb(urb);
 	}
 }
@@ -960,9 +956,7 @@ static int gspca_init_transfer(struct gspca_dev *gspca_dev)
 		/* the bandwidth is not wide enough
 		 * negotiate or try a lower alternate setting */
 retry:
-		PDEBUG(D_ERR|D_STREAM,
-			"alt %d - bandwidth not wide enough - trying again",
-			alt);
+		PERR("alt %d - bandwidth not wide enough, trying again", alt);
 		msleep(20);	/* wait for kill complete */
 		if (gspca_dev->sd_desc->isoc_nego) {
 			ret = gspca_dev->sd_desc->isoc_nego(gspca_dev);
@@ -984,7 +978,6 @@ out:
 
 static void gspca_set_default_mode(struct gspca_dev *gspca_dev)
 {
-	struct gspca_ctrl *ctrl;
 	int i;
 
 	i = gspca_dev->cam.nmodes - 1;	/* take the highest mode */
@@ -993,17 +986,8 @@ static void gspca_set_default_mode(struct gspca_dev *gspca_dev)
 	gspca_dev->height = gspca_dev->cam.cam_mode[i].height;
 	gspca_dev->pixfmt = gspca_dev->cam.cam_mode[i].pixelformat;
 
-	/* 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;
-		     i < gspca_dev->sd_desc->nctrls;
-		     i++, ctrl++)
-			ctrl->val = ctrl->def;
-	}
 }
 
 static int wxh_to_mode(struct gspca_dev *gspca_dev,
@@ -1055,7 +1039,7 @@ static int vidioc_g_register(struct file *file, void *priv,
 }
 
 static int vidioc_s_register(struct file *file, void *priv,
-			struct v4l2_dbg_register *reg)
+			const struct v4l2_dbg_register *reg)
 {
 	struct gspca_dev *gspca_dev = video_drvdata(file);
 
@@ -1137,10 +1121,9 @@ static int try_fmt_vid_cap(struct gspca_dev *gspca_dev,
 	w = fmt->fmt.pix.width;
 	h = fmt->fmt.pix.height;
 
-#ifdef GSPCA_DEBUG
-	if (gspca_debug & D_CONF)
-		PDEBUG_MODE("try fmt cap", fmt->fmt.pix.pixelformat, w, h);
-#endif
+	PDEBUG_MODE(gspca_dev, D_CONF, "try fmt cap",
+		    fmt->fmt.pix.pixelformat, w, h);
+
 	/* search the closest mode for width and height */
 	mode = wxh_to_mode(gspca_dev, w, h);
 
@@ -1153,8 +1136,6 @@ static int try_fmt_vid_cap(struct gspca_dev *gspca_dev,
 					fmt->fmt.pix.pixelformat);
 		if (mode2 >= 0)
 			mode = mode2;
-/*		else
-			;		 * no chance, return this mode */
 	}
 	fmt->fmt.pix = gspca_dev->cam.cam_mode[mode];
 	/* some drivers use priv internally, zero it before giving it to
@@ -1290,15 +1271,6 @@ static int dev_open(struct file *file)
 	if (!try_module_get(gspca_dev->module))
 		return -ENODEV;
 
-#ifdef GSPCA_DEBUG
-	/* activate the v4l2 debug */
-	if (gspca_debug & D_V4L2)
-		gspca_dev->vdev.debug |= V4L2_DEBUG_IOCTL
-					| V4L2_DEBUG_IOCTL_ARG;
-	else
-		gspca_dev->vdev.debug &= ~(V4L2_DEBUG_IOCTL
-					| V4L2_DEBUG_IOCTL_ARG);
-#endif
 	return v4l2_fh_open(file);
 }
 
@@ -1357,134 +1329,6 @@ static int vidioc_querycap(struct file *file, void  *priv,
 	return 0;
 }
 
-static int get_ctrl(struct gspca_dev *gspca_dev,
-				   int id)
-{
-	const struct ctrl *ctrls;
-	int i;
-
-	for (i = 0, ctrls = gspca_dev->sd_desc->ctrls;
-	     i < gspca_dev->sd_desc->nctrls;
-	     i++, ctrls++) {
-		if (gspca_dev->ctrl_dis & (1 << i))
-			continue;
-		if (id == ctrls->qctrl.id)
-			return i;
-	}
-	return -1;
-}
-
-static int vidioc_queryctrl(struct file *file, void *priv,
-			   struct v4l2_queryctrl *q_ctrl)
-{
-	struct gspca_dev *gspca_dev = video_drvdata(file);
-	const struct ctrl *ctrls;
-	struct gspca_ctrl *gspca_ctrl;
-	int i, idx;
-	u32 id;
-
-	id = q_ctrl->id;
-	if (id & V4L2_CTRL_FLAG_NEXT_CTRL) {
-		id &= V4L2_CTRL_ID_MASK;
-		id++;
-		idx = -1;
-		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 < id)
-				continue;
-			if (idx >= 0
-			 && gspca_dev->sd_desc->ctrls[i].qctrl.id
-				    > gspca_dev->sd_desc->ctrls[idx].qctrl.id)
-				continue;
-			idx = i;
-		}
-	} else {
-		idx = get_ctrl(gspca_dev, id);
-	}
-	if (idx < 0)
-		return -EINVAL;
-	ctrls = &gspca_dev->sd_desc->ctrls[idx];
-	memcpy(q_ctrl, &ctrls->qctrl, sizeof *q_ctrl);
-	if (gspca_dev->cam.ctrls != NULL) {
-		gspca_ctrl = &gspca_dev->cam.ctrls[idx];
-		q_ctrl->default_value = gspca_ctrl->def;
-		q_ctrl->minimum = gspca_ctrl->min;
-		q_ctrl->maximum = gspca_ctrl->max;
-	}
-	if (gspca_dev->ctrl_inac & (1 << idx))
-		q_ctrl->flags |= V4L2_CTRL_FLAG_INACTIVE;
-	return 0;
-}
-
-static int vidioc_s_ctrl(struct file *file, void *priv,
-			 struct v4l2_control *ctrl)
-{
-	struct gspca_dev *gspca_dev = video_drvdata(file);
-	const struct ctrl *ctrls;
-	struct gspca_ctrl *gspca_ctrl;
-	int idx;
-
-	idx = get_ctrl(gspca_dev, ctrl->id);
-	if (idx < 0)
-		return -EINVAL;
-	if (gspca_dev->ctrl_inac & (1 << idx))
-		return -EINVAL;
-	ctrls = &gspca_dev->sd_desc->ctrls[idx];
-	if (gspca_dev->cam.ctrls != NULL) {
-		gspca_ctrl = &gspca_dev->cam.ctrls[idx];
-		if (ctrl->value < gspca_ctrl->min
-		    || ctrl->value > gspca_ctrl->max)
-			return -ERANGE;
-	} else {
-		gspca_ctrl = NULL;
-		if (ctrl->value < ctrls->qctrl.minimum
-		    || ctrl->value > ctrls->qctrl.maximum)
-			return -ERANGE;
-	}
-	PDEBUG(D_CONF, "set ctrl [%08x] = %d", ctrl->id, ctrl->value);
-	gspca_dev->usb_err = 0;
-	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);
-	}
-	return gspca_dev->usb_err;
-}
-
-static int vidioc_g_ctrl(struct file *file, void *priv,
-			 struct v4l2_control *ctrl)
-{
-	struct gspca_dev *gspca_dev = video_drvdata(file);
-	const struct ctrl *ctrls;
-	int idx;
-
-	idx = get_ctrl(gspca_dev, ctrl->id);
-	if (idx < 0)
-		return -EINVAL;
-	ctrls = &gspca_dev->sd_desc->ctrls[idx];
-
-	gspca_dev->usb_err = 0;
-	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;
-	return 0;
-}
-
-static int vidioc_querymenu(struct file *file, void *priv,
-			    struct v4l2_querymenu *qmenu)
-{
-	struct gspca_dev *gspca_dev = video_drvdata(file);
-
-	if (!gspca_dev->sd_desc->querymenu)
-		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)
 {
@@ -1621,14 +1465,8 @@ static int vidioc_streamon(struct file *file, void *priv,
 		if (ret < 0)
 			goto out;
 	}
-#ifdef GSPCA_DEBUG
-	if (gspca_debug & D_STREAM) {
-		PDEBUG_MODE("stream on OK",
-			gspca_dev->pixfmt,
-			gspca_dev->width,
-			gspca_dev->height);
-	}
-#endif
+	PDEBUG_MODE(gspca_dev, D_STREAM, "stream on OK", gspca_dev->pixfmt,
+		    gspca_dev->width, gspca_dev->height);
 	ret = 0;
 out:
 	mutex_unlock(&gspca_dev->queue_lock);
@@ -1879,8 +1717,7 @@ static int vidioc_dqbuf(struct file *file, void *priv,
 		if (copy_to_user((__u8 __user *) frame->v4l2_buf.m.userptr,
 				 frame->data,
 				 frame->v4l2_buf.bytesused)) {
-			PDEBUG(D_ERR|D_STREAM,
-				"dqbuf cp to user failed");
+			PERR("dqbuf cp to user failed");
 			ret = -EFAULT;
 		}
 	}
@@ -2092,8 +1929,7 @@ static ssize_t dev_read(struct file *file, char __user *data,
 		count = frame->v4l2_buf.bytesused;
 	ret = copy_to_user(data, frame->data, count);
 	if (ret != 0) {
-		PDEBUG(D_ERR|D_STREAM,
-			"read cp to user lack %d / %zd", ret, count);
+		PERR("read cp to user lack %d / %zd", ret, count);
 		ret = -EFAULT;
 		goto out;
 	}
@@ -2125,10 +1961,6 @@ static const struct v4l2_ioctl_ops dev_ioctl_ops = {
 	.vidioc_g_fmt_vid_cap	= vidioc_g_fmt_vid_cap,
 	.vidioc_s_fmt_vid_cap	= vidioc_s_fmt_vid_cap,
 	.vidioc_streamon	= vidioc_streamon,
-	.vidioc_queryctrl	= vidioc_queryctrl,
-	.vidioc_g_ctrl		= vidioc_g_ctrl,
-	.vidioc_s_ctrl		= vidioc_s_ctrl,
-	.vidioc_querymenu	= vidioc_querymenu,
 	.vidioc_enum_input	= vidioc_enum_input,
 	.vidioc_g_input		= vidioc_g_input,
 	.vidioc_s_input		= vidioc_s_input,
@@ -2157,22 +1989,6 @@ static const struct video_device gspca_template = {
 	.release = video_device_release_empty, /* We use v4l2_dev.release */
 };
 
-/* initialize the controls */
-static void ctrls_init(struct gspca_dev *gspca_dev)
-{
-	struct gspca_ctrl *ctrl;
-	int i;
-
-	for (i = 0, ctrl = gspca_dev->cam.ctrls;
-	     i < gspca_dev->sd_desc->nctrls;
-	     i++, ctrl++) {
-		ctrl->def = gspca_dev->sd_desc->ctrls[i].qctrl.default_value;
-		ctrl->val = ctrl->def;
-		ctrl->min = gspca_dev->sd_desc->ctrls[i].qctrl.minimum;
-		ctrl->max = gspca_dev->sd_desc->ctrls[i].qctrl.maximum;
-	}
-}
-
 /*
  * probe and create a new gspca device
  *
@@ -2249,8 +2065,6 @@ int gspca_dev_probe2(struct usb_interface *intf,
 	ret = sd_desc->config(gspca_dev, id);
 	if (ret < 0)
 		goto out;
-	if (gspca_dev->cam.ctrls != NULL)
-		ctrls_init(gspca_dev);
 	ret = sd_desc->init(gspca_dev);
 	if (ret < 0)
 		goto out;
@@ -2450,10 +2264,6 @@ static void __exit gspca_exit(void)
 module_init(gspca_init);
 module_exit(gspca_exit);
 
-#ifdef GSPCA_DEBUG
 module_param_named(debug, gspca_debug, int, 0644);
 MODULE_PARM_DESC(debug,
-		"Debug (bit) 0x01:error 0x02:probe 0x04:config"
-		" 0x08:stream 0x10:frame 0x20:packet"
-		" 0x0100: v4l2");
-#endif
+		"1:probe 2:config 3:stream 4:frame 5:packet 6:usbi 7:usbo");
diff --git a/drivers/media/usb/gspca/gspca.h b/drivers/media/usb/gspca/gspca.h
index 5559932bf2f..ef8efeb8007 100644
--- a/drivers/media/usb/gspca/gspca.h
+++ b/drivers/media/usb/gspca/gspca.h
@@ -10,30 +10,26 @@
 #include <media/v4l2-device.h>
 #include <linux/mutex.h>
 
-/* compilation option */
-/*#define GSPCA_DEBUG 1*/
 
-#ifdef GSPCA_DEBUG
-/* GSPCA our debug messages */
+
+/* GSPCA debug codes */
+
+#define D_PROBE  1
+#define D_CONF   2
+#define D_STREAM 3
+#define D_FRAM   4
+#define D_PACK   5
+#define D_USBI   6
+#define D_USBO   7
+
 extern int gspca_debug;
-#define PDEBUG(level, fmt, ...)					\
-do {								\
-	if (gspca_debug & (level))				\
-		pr_info(fmt, ##__VA_ARGS__);			\
-} while (0)
 
-#define D_ERR  0x01
-#define D_PROBE 0x02
-#define D_CONF 0x04
-#define D_STREAM 0x08
-#define D_FRAM 0x10
-#define D_PACK 0x20
-#define D_USBI 0x00
-#define D_USBO 0x00
-#define D_V4L2 0x0100
-#else
-#define PDEBUG(level, fmt, ...) do {} while(0)
-#endif
+
+#define PDEBUG(level, fmt, ...) \
+	v4l2_dbg(level, gspca_debug, &gspca_dev->v4l2_dev, fmt, ##__VA_ARGS__)
+
+#define PERR(fmt, ...) \
+	v4l2_err(&gspca_dev->v4l2_dev, fmt, ##__VA_ARGS__)
 
 #define GSPCA_MAX_FRAMES 16	/* maximum number of video frame buffers */
 /* image transfers */
@@ -46,20 +42,11 @@ struct framerates {
 	int nrates;
 };
 
-/* control definition */
-struct gspca_ctrl {
-	s16 val;	/* current value */
-	s16 def;	/* default value */
-	s16 min, max;	/* minimum and maximum values */
-};
-
 /* device information - set at probe time */
 struct cam {
 	const struct v4l2_pix_format *cam_mode;	/* size nmodes */
 	const struct framerates *mode_framerates; /* must have size nmodes,
 						   * just like cam_mode */
-	struct gspca_ctrl *ctrls;	/* control table - size nctrls */
-					/* may be NULL */
 	u32 bulk_size;		/* buffer size when image transfer by bulk */
 	u32 input_flags;	/* value for ENUM_INPUT status flags */
 	u8 nmodes;		/* size of cam_mode */
@@ -87,14 +74,14 @@ typedef int (*cam_get_jpg_op) (struct gspca_dev *,
 				struct v4l2_jpegcompression *);
 typedef int (*cam_set_jpg_op) (struct gspca_dev *,
 				const struct v4l2_jpegcompression *);
-typedef int (*cam_reg_op) (struct gspca_dev *,
+typedef int (*cam_get_reg_op) (struct gspca_dev *,
 				struct v4l2_dbg_register *);
+typedef int (*cam_set_reg_op) (struct gspca_dev *,
+				const struct v4l2_dbg_register *);
 typedef int (*cam_ident_op) (struct gspca_dev *,
 				struct v4l2_dbg_chip_ident *);
 typedef void (*cam_streamparm_op) (struct gspca_dev *,
 				  struct v4l2_streamparm *);
-typedef int (*cam_qmnu_op) (struct gspca_dev *,
-			struct v4l2_querymenu *);
 typedef void (*cam_pkt_op) (struct gspca_dev *gspca_dev,
 				u8 *data,
 				int len);
@@ -102,20 +89,10 @@ typedef int (*cam_int_pkt_op) (struct gspca_dev *gspca_dev,
 				u8 *data,
 				int len);
 
-struct ctrl {
-	struct v4l2_queryctrl qctrl;
-	int (*set)(struct gspca_dev *, __s32);
-	int (*get)(struct gspca_dev *, __s32 *);
-	cam_v_op set_control;
-};
-
 /* subdriver description */
 struct sd_desc {
 /* information */
 	const char *name;	/* sub-driver name */
-/* controls */
-	const struct ctrl *ctrls;	/* static control definition */
-	int nctrls;
 /* mandatory operations */
 	cam_cf_op config;	/* called on probe */
 	cam_op init;		/* called on probe and resume */
@@ -130,12 +107,11 @@ struct sd_desc {
 	cam_v_op dq_callback;	/* called when a frame has been dequeued */
 	cam_get_jpg_op get_jcomp;
 	cam_set_jpg_op set_jcomp;
-	cam_qmnu_op querymenu;
 	cam_streamparm_op get_streamparm;
 	cam_streamparm_op set_streamparm;
 #ifdef CONFIG_VIDEO_ADV_DEBUG
-	cam_reg_op set_register;
-	cam_reg_op get_register;
+	cam_set_reg_op set_register;
+	cam_get_reg_op get_register;
 #endif
 	cam_ident_op get_chip_ident;
 #if IS_ENABLED(CONFIG_INPUT)
@@ -174,8 +150,6 @@ struct gspca_dev {
 
 	struct cam cam;				/* device information */
 	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
diff --git a/drivers/media/usb/gspca/jeilinj.c b/drivers/media/usb/gspca/jeilinj.c
index 1ba29fe7fad..8da3dde3838 100644
--- a/drivers/media/usb/gspca/jeilinj.c
+++ b/drivers/media/usb/gspca/jeilinj.c
@@ -266,7 +266,7 @@ static int jlj_start(struct gspca_dev *gspca_dev)
 	msleep(2);
 	setfreq(gspca_dev, v4l2_ctrl_g_ctrl(sd->freq));
 	if (gspca_dev->usb_err < 0)
-		PDEBUG(D_ERR, "Start streaming command failed");
+		PERR("Start streaming command failed");
 	return gspca_dev->usb_err;
 }
 
diff --git a/drivers/media/usb/gspca/konica.c b/drivers/media/usb/gspca/konica.c
index 61e25dbf244..39c96bb4c98 100644
--- a/drivers/media/usb/gspca/konica.c
+++ b/drivers/media/usb/gspca/konica.c
@@ -277,7 +277,7 @@ static void sd_isoc_irq(struct urb *urb)
 		if (gspca_dev->frozen)
 			return;
 #endif
-		PDEBUG(D_ERR, "urb status: %d", urb->status);
+		PERR("urb status: %d", urb->status);
 		st = usb_submit_urb(urb, GFP_ATOMIC);
 		if (st < 0)
 			pr_err("resubmit urb error %d\n", st);
@@ -295,33 +295,30 @@ static void sd_isoc_irq(struct urb *urb)
 	sd->last_data_urb = NULL;
 
 	if (!data_urb || data_urb->start_frame != status_urb->start_frame) {
-		PDEBUG(D_ERR|D_PACK, "lost sync on frames");
+		PERR("lost sync on frames");
 		goto resubmit;
 	}
 
 	if (data_urb->number_of_packets != status_urb->number_of_packets) {
-		PDEBUG(D_ERR|D_PACK,
-		       "no packets does not match, data: %d, status: %d",
-		       data_urb->number_of_packets,
-		       status_urb->number_of_packets);
+		PERR("no packets does not match, data: %d, status: %d",
+		     data_urb->number_of_packets,
+		     status_urb->number_of_packets);
 		goto resubmit;
 	}
 
 	for (i = 0; i < status_urb->number_of_packets; i++) {
 		if (data_urb->iso_frame_desc[i].status ||
 		    status_urb->iso_frame_desc[i].status) {
-			PDEBUG(D_ERR|D_PACK,
-			       "pkt %d data-status %d, status-status %d", i,
-			       data_urb->iso_frame_desc[i].status,
-			       status_urb->iso_frame_desc[i].status);
+			PERR("pkt %d data-status %d, status-status %d", i,
+			     data_urb->iso_frame_desc[i].status,
+			     status_urb->iso_frame_desc[i].status);
 			gspca_dev->last_packet_type = DISCARD_PACKET;
 			continue;
 		}
 
 		if (status_urb->iso_frame_desc[i].actual_length != 1) {
-			PDEBUG(D_ERR|D_PACK,
-			       "bad status packet length %d",
-			       status_urb->iso_frame_desc[i].actual_length);
+			PERR("bad status packet length %d",
+			     status_urb->iso_frame_desc[i].actual_length);
 			gspca_dev->last_packet_type = DISCARD_PACKET;
 			continue;
 		}
@@ -366,12 +363,11 @@ resubmit:
 	if (data_urb) {
 		st = usb_submit_urb(data_urb, GFP_ATOMIC);
 		if (st < 0)
-			PDEBUG(D_ERR|D_PACK,
-			       "usb_submit_urb(data_urb) ret %d", st);
+			PERR("usb_submit_urb(data_urb) ret %d", st);
 	}
 	st = usb_submit_urb(status_urb, GFP_ATOMIC);
 	if (st < 0)
-		pr_err("usb_submit_urb(status_urb) ret %d\n", st);
+		PERR("usb_submit_urb(status_urb) ret %d\n", st);
 }
 
 static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
diff --git a/drivers/media/usb/gspca/m5602/m5602_bridge.h b/drivers/media/usb/gspca/m5602/m5602_bridge.h
index 51af3ee3ab8..19eb1a64f9d 100644
--- a/drivers/media/usb/gspca/m5602/m5602_bridge.h
+++ b/drivers/media/usb/gspca/m5602/m5602_bridge.h
@@ -136,16 +136,33 @@ struct sd {
 	/* A pointer to the currently connected sensor */
 	const struct m5602_sensor *sensor;
 
-	struct sd_desc *desc;
-
-	/* Sensor private data */
-	void *sensor_priv;
-
 	/* The current frame's id, used to detect frame boundaries */
 	u8 frame_id;
 
 	/* The current frame count */
 	u32 frame_count;
+
+	/* Camera rotation polling thread for "flipable" cams */
+	struct task_struct *rotation_thread;
+
+	struct { /* auto-white-bal + green/red/blue balance control cluster */
+		struct v4l2_ctrl *auto_white_bal;
+		struct v4l2_ctrl *red_bal;
+		struct v4l2_ctrl *blue_bal;
+		struct v4l2_ctrl *green_bal;
+	};
+	struct { /* autoexpo / expo cluster */
+		struct v4l2_ctrl *autoexpo;
+		struct v4l2_ctrl *expo;
+	};
+	struct { /* autogain / gain cluster */
+		struct v4l2_ctrl *autogain;
+		struct v4l2_ctrl *gain;
+	};
+	struct { /* hflip/vflip cluster */
+		struct v4l2_ctrl *hflip;
+		struct v4l2_ctrl *vflip;
+	};
 };
 
 int m5602_read_bridge(
diff --git a/drivers/media/usb/gspca/m5602/m5602_core.c b/drivers/media/usb/gspca/m5602/m5602_core.c
index ed22638978c..d926e62cb80 100644
--- a/drivers/media/usb/gspca/m5602/m5602_core.c
+++ b/drivers/media/usb/gspca/m5602/m5602_core.c
@@ -41,6 +41,7 @@ MODULE_DEVICE_TABLE(usb, m5602_table);
 int m5602_read_bridge(struct sd *sd, const u8 address, u8 *i2c_data)
 {
 	int err;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *) sd;
 	struct usb_device *udev = sd->gspca_dev.dev;
 	__u8 *buf = sd->gspca_dev.usb_buf;
 
@@ -62,6 +63,7 @@ int m5602_read_bridge(struct sd *sd, const u8 address, u8 *i2c_data)
 int m5602_write_bridge(struct sd *sd, const u8 address, const u8 i2c_data)
 {
 	int err;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *) sd;
 	struct usb_device *udev = sd->gspca_dev.dev;
 	__u8 *buf = sd->gspca_dev.usb_buf;
 
@@ -98,6 +100,7 @@ int m5602_read_sensor(struct sd *sd, const u8 address,
 		       u8 *i2c_data, const u8 len)
 {
 	int err, i;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *) sd;
 
 	if (!len || len > sd->sensor->i2c_regW)
 		return -EINVAL;
@@ -147,6 +150,7 @@ int m5602_write_sensor(struct sd *sd, const u8 address,
 {
 	int err, i;
 	u8 *p;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *) sd;
 	struct usb_device *udev = sd->gspca_dev.dev;
 	__u8 *buf = sd->gspca_dev.usb_buf;
 
@@ -252,6 +256,16 @@ static int m5602_init(struct gspca_dev *gspca_dev)
 	return err;
 }
 
+static int m5602_init_controls(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	if (!sd->sensor->init_controls)
+		return 0;
+
+	return sd->sensor->init_controls(sd);
+}
+
 static int m5602_start_transfer(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -336,11 +350,12 @@ static void m5602_stop_transfer(struct gspca_dev *gspca_dev)
 		sd->sensor->stop(sd);
 }
 
-/* sub-driver description, the ctrl and nctrl is filled at probe time */
-static struct sd_desc sd_desc = {
+/* sub-driver description */
+static const struct sd_desc sd_desc = {
 	.name		= MODULE_NAME,
 	.config		= m5602_configure,
 	.init		= m5602_init,
+	.init_controls	= m5602_init_controls,
 	.start		= m5602_start_transfer,
 	.stopN		= m5602_stop_transfer,
 	.pkt_scan	= m5602_urb_complete
@@ -355,7 +370,6 @@ static int m5602_configure(struct gspca_dev *gspca_dev,
 	int err;
 
 	cam = &gspca_dev->cam;
-	sd->desc = &sd_desc;
 
 	if (dump_bridge)
 		m5602_dump_bridge(sd);
@@ -368,7 +382,7 @@ static int m5602_configure(struct gspca_dev *gspca_dev,
 	return 0;
 
 fail:
-	PDEBUG(D_ERR, "ALi m5602 webcam failed");
+	PERR("ALi m5602 webcam failed");
 	cam->cam_mode = NULL;
 	cam->nmodes = 0;
 
diff --git a/drivers/media/usb/gspca/m5602/m5602_mt9m111.c b/drivers/media/usb/gspca/m5602/m5602_mt9m111.c
index 6268aa24ec5..cfa4663f893 100644
--- a/drivers/media/usb/gspca/m5602/m5602_mt9m111.c
+++ b/drivers/media/usb/gspca/m5602/m5602_mt9m111.c
@@ -20,22 +20,8 @@
 
 #include "m5602_mt9m111.h"
 
-static int mt9m111_set_vflip(struct gspca_dev *gspca_dev, __s32 val);
-static int mt9m111_get_vflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int mt9m111_get_hflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int mt9m111_set_hflip(struct gspca_dev *gspca_dev, __s32 val);
-static int mt9m111_get_gain(struct gspca_dev *gspca_dev, __s32 *val);
-static int mt9m111_set_gain(struct gspca_dev *gspca_dev, __s32 val);
-static int mt9m111_set_auto_white_balance(struct gspca_dev *gspca_dev,
-					 __s32 val);
-static int mt9m111_get_auto_white_balance(struct gspca_dev *gspca_dev,
-					  __s32 *val);
-static int mt9m111_get_green_balance(struct gspca_dev *gspca_dev, __s32 *val);
-static int mt9m111_set_green_balance(struct gspca_dev *gspca_dev, __s32 val);
-static int mt9m111_get_blue_balance(struct gspca_dev *gspca_dev, __s32 *val);
-static int mt9m111_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val);
-static int mt9m111_get_red_balance(struct gspca_dev *gspca_dev, __s32 *val);
-static int mt9m111_set_red_balance(struct gspca_dev *gspca_dev, __s32 val);
+static int mt9m111_s_ctrl(struct v4l2_ctrl *ctrl);
+static void mt9m111_dump_registers(struct sd *sd);
 
 static struct v4l2_pix_format mt9m111_modes[] = {
 	{
@@ -50,118 +36,27 @@ static struct v4l2_pix_format mt9m111_modes[] = {
 	}
 };
 
-static const struct ctrl mt9m111_ctrls[] = {
-#define VFLIP_IDX 0
-	{
-		{
-			.id		= V4L2_CID_VFLIP,
-			.type           = V4L2_CTRL_TYPE_BOOLEAN,
-			.name           = "vertical flip",
-			.minimum        = 0,
-			.maximum        = 1,
-			.step           = 1,
-			.default_value  = 0
-		},
-		.set = mt9m111_set_vflip,
-		.get = mt9m111_get_vflip
-	},
-#define HFLIP_IDX 1
-	{
-		{
-			.id             = V4L2_CID_HFLIP,
-			.type           = V4L2_CTRL_TYPE_BOOLEAN,
-			.name           = "horizontal flip",
-			.minimum        = 0,
-			.maximum        = 1,
-			.step           = 1,
-			.default_value  = 0
-		},
-		.set = mt9m111_set_hflip,
-		.get = mt9m111_get_hflip
-	},
-#define GAIN_IDX 2
-	{
-		{
-			.id             = V4L2_CID_GAIN,
-			.type           = V4L2_CTRL_TYPE_INTEGER,
-			.name           = "gain",
-			.minimum        = 0,
-			.maximum        = (INITIAL_MAX_GAIN - 1) * 2 * 2 * 2,
-			.step           = 1,
-			.default_value  = MT9M111_DEFAULT_GAIN,
-			.flags          = V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = mt9m111_set_gain,
-		.get = mt9m111_get_gain
-	},
-#define AUTO_WHITE_BALANCE_IDX 3
-	{
-		{
-			.id             = V4L2_CID_AUTO_WHITE_BALANCE,
-			.type           = V4L2_CTRL_TYPE_BOOLEAN,
-			.name           = "auto white balance",
-			.minimum        = 0,
-			.maximum        = 1,
-			.step           = 1,
-			.default_value  = 0,
-		},
-		.set = mt9m111_set_auto_white_balance,
-		.get = mt9m111_get_auto_white_balance
-	},
-#define GREEN_BALANCE_IDX 4
-	{
-		{
-			.id		= M5602_V4L2_CID_GREEN_BALANCE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "green balance",
-			.minimum	= 0x00,
-			.maximum	= 0x7ff,
-			.step		= 0x1,
-			.default_value	= MT9M111_GREEN_GAIN_DEFAULT,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = mt9m111_set_green_balance,
-		.get = mt9m111_get_green_balance
-	},
-#define BLUE_BALANCE_IDX 5
-	{
-		{
-			.id		= V4L2_CID_BLUE_BALANCE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "blue balance",
-			.minimum	= 0x00,
-			.maximum	= 0x7ff,
-			.step		= 0x1,
-			.default_value	= MT9M111_BLUE_GAIN_DEFAULT,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = mt9m111_set_blue_balance,
-		.get = mt9m111_get_blue_balance
-	},
-#define RED_BALANCE_IDX 5
-	{
-		{
-			.id		= V4L2_CID_RED_BALANCE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "red balance",
-			.minimum	= 0x00,
-			.maximum	= 0x7ff,
-			.step		= 0x1,
-			.default_value	= MT9M111_RED_GAIN_DEFAULT,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = mt9m111_set_red_balance,
-		.get = mt9m111_get_red_balance
-	},
+static const struct v4l2_ctrl_ops mt9m111_ctrl_ops = {
+	.s_ctrl = mt9m111_s_ctrl,
 };
 
-static void mt9m111_dump_registers(struct sd *sd);
+static const struct v4l2_ctrl_config mt9m111_greenbal_cfg = {
+	.ops	= &mt9m111_ctrl_ops,
+	.id	= M5602_V4L2_CID_GREEN_BALANCE,
+	.name	= "Green Balance",
+	.type	= V4L2_CTRL_TYPE_INTEGER,
+	.min	= 0,
+	.max	= 0x7ff,
+	.step	= 1,
+	.def	= MT9M111_GREEN_GAIN_DEFAULT,
+	.flags	= V4L2_CTRL_FLAG_SLIDER,
+};
 
 int mt9m111_probe(struct sd *sd)
 {
 	u8 data[2] = {0x00, 0x00};
 	int i;
-	s32 *sensor_settings;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 
 	if (force_sensor) {
 		if (force_sensor == MT9M111_SENSOR) {
@@ -200,19 +95,8 @@ int mt9m111_probe(struct sd *sd)
 	return -ENODEV;
 
 sensor_found:
-	sensor_settings = kmalloc(ARRAY_SIZE(mt9m111_ctrls) * sizeof(s32),
-				  GFP_KERNEL);
-	if (!sensor_settings)
-		return -ENOMEM;
-
 	sd->gspca_dev.cam.cam_mode = mt9m111_modes;
 	sd->gspca_dev.cam.nmodes = ARRAY_SIZE(mt9m111_modes);
-	sd->desc->ctrls = mt9m111_ctrls;
-	sd->desc->nctrls = ARRAY_SIZE(mt9m111_ctrls);
-
-	for (i = 0; i < ARRAY_SIZE(mt9m111_ctrls); i++)
-		sensor_settings[i] = mt9m111_ctrls[i].qctrl.default_value;
-	sd->sensor_priv = sensor_settings;
 
 	return 0;
 }
@@ -220,7 +104,6 @@ sensor_found:
 int mt9m111_init(struct sd *sd)
 {
 	int i, err = 0;
-	s32 *sensor_settings = sd->sensor_priv;
 
 	/* Init the sensor */
 	for (i = 0; i < ARRAY_SIZE(init_mt9m111) && !err; i++) {
@@ -241,30 +124,45 @@ int mt9m111_init(struct sd *sd)
 	if (dump_sensor)
 		mt9m111_dump_registers(sd);
 
-	err = mt9m111_set_vflip(&sd->gspca_dev, sensor_settings[VFLIP_IDX]);
-	if (err < 0)
-		return err;
-
-	err = mt9m111_set_hflip(&sd->gspca_dev, sensor_settings[HFLIP_IDX]);
-	if (err < 0)
-		return err;
-
-	err = mt9m111_set_green_balance(&sd->gspca_dev,
-					 sensor_settings[GREEN_BALANCE_IDX]);
-	if (err < 0)
-		return err;
+	return 0;
+}
 
-	err = mt9m111_set_blue_balance(&sd->gspca_dev,
-					 sensor_settings[BLUE_BALANCE_IDX]);
-	if (err < 0)
-		return err;
+int mt9m111_init_controls(struct sd *sd)
+{
+	struct v4l2_ctrl_handler *hdl = &sd->gspca_dev.ctrl_handler;
+
+	sd->gspca_dev.vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 7);
+
+	sd->auto_white_bal = v4l2_ctrl_new_std(hdl, &mt9m111_ctrl_ops,
+					       V4L2_CID_AUTO_WHITE_BALANCE,
+					       0, 1, 1, 0);
+	sd->green_bal = v4l2_ctrl_new_custom(hdl, &mt9m111_greenbal_cfg, NULL);
+	sd->red_bal = v4l2_ctrl_new_std(hdl, &mt9m111_ctrl_ops,
+					V4L2_CID_RED_BALANCE, 0, 0x7ff, 1,
+					MT9M111_RED_GAIN_DEFAULT);
+	sd->blue_bal = v4l2_ctrl_new_std(hdl, &mt9m111_ctrl_ops,
+					V4L2_CID_BLUE_BALANCE, 0, 0x7ff, 1,
+					MT9M111_BLUE_GAIN_DEFAULT);
+
+	v4l2_ctrl_new_std(hdl, &mt9m111_ctrl_ops, V4L2_CID_GAIN, 0,
+			  (INITIAL_MAX_GAIN - 1) * 2 * 2 * 2, 1,
+			  MT9M111_DEFAULT_GAIN);
+
+	sd->hflip = v4l2_ctrl_new_std(hdl, &mt9m111_ctrl_ops, V4L2_CID_HFLIP,
+				      0, 1, 1, 0);
+	sd->vflip = v4l2_ctrl_new_std(hdl, &mt9m111_ctrl_ops, V4L2_CID_VFLIP,
+				      0, 1, 1, 0);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
 
-	err = mt9m111_set_red_balance(&sd->gspca_dev,
-					sensor_settings[RED_BALANCE_IDX]);
-	if (err < 0)
-		return err;
+	v4l2_ctrl_auto_cluster(4, &sd->auto_white_bal, 0, false);
+	v4l2_ctrl_cluster(2, &sd->hflip);
 
-	return mt9m111_set_gain(&sd->gspca_dev, sensor_settings[GAIN_IDX]);
+	return 0;
 }
 
 int mt9m111_start(struct sd *sd)
@@ -272,7 +170,7 @@ int mt9m111_start(struct sd *sd)
 	int i, err = 0;
 	u8 data[2];
 	struct cam *cam = &sd->gspca_dev.cam;
-	s32 *sensor_settings = sd->sensor_priv;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 
 	int width = cam->cam_mode[sd->gspca_dev.curr_mode].width - 1;
 	int height = cam->cam_mode[sd->gspca_dev.curr_mode].height;
@@ -333,26 +231,11 @@ int mt9m111_start(struct sd *sd)
 
 	switch (width) {
 	case 640:
-		PDEBUG(D_V4L2, "Configuring camera for VGA mode");
-		data[0] = MT9M111_RMB_OVER_SIZED;
-		data[1] = MT9M111_RMB_ROW_SKIP_2X |
-			  MT9M111_RMB_COLUMN_SKIP_2X |
-			  (sensor_settings[VFLIP_IDX] << 0) |
-			  (sensor_settings[HFLIP_IDX] << 1);
-
-		err = m5602_write_sensor(sd,
-					 MT9M111_SC_R_MODE_CONTEXT_B, data, 2);
+		PDEBUG(D_CONF, "Configuring camera for VGA mode");
 		break;
 
 	case 320:
-		PDEBUG(D_V4L2, "Configuring camera for QVGA mode");
-		data[0] = MT9M111_RMB_OVER_SIZED;
-		data[1] = MT9M111_RMB_ROW_SKIP_4X |
-				MT9M111_RMB_COLUMN_SKIP_4X |
-				(sensor_settings[VFLIP_IDX] << 0) |
-				(sensor_settings[HFLIP_IDX] << 1);
-		err = m5602_write_sensor(sd,
-					 MT9M111_SC_R_MODE_CONTEXT_B, data, 2);
+		PDEBUG(D_CONF, "Configuring camera for QVGA mode");
 		break;
 	}
 	return err;
@@ -361,105 +244,46 @@ int mt9m111_start(struct sd *sd)
 void mt9m111_disconnect(struct sd *sd)
 {
 	sd->sensor = NULL;
-	kfree(sd->sensor_priv);
-}
-
-static int mt9m111_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 mt9m111_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
-{
-	int err;
-	u8 data[2] = {0x00, 0x00};
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	PDEBUG(D_V4L2, "Set vertical flip to %d", val);
-
-	sensor_settings[VFLIP_IDX] = val;
-
-	/* The mt9m111 is flipped by default */
-	val = !val;
-
-	/* Set the correct page map */
-	err = m5602_write_sensor(sd, MT9M111_PAGE_MAP, data, 2);
-	if (err < 0)
-		return err;
-
-	err = m5602_read_sensor(sd, MT9M111_SC_R_MODE_CONTEXT_B, data, 2);
-	if (err < 0)
-		return err;
-
-	data[1] = (data[1] & 0xfe) | val;
-	err = m5602_write_sensor(sd, MT9M111_SC_R_MODE_CONTEXT_B,
-				   data, 2);
-	return err;
-}
-
-static int mt9m111_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 mt9m111_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
+static int mt9m111_set_hvflip(struct gspca_dev *gspca_dev)
 {
 	int err;
 	u8 data[2] = {0x00, 0x00};
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
+	int hflip;
+	int vflip;
 
-	PDEBUG(D_V4L2, "Set horizontal flip to %d", val);
-
-	sensor_settings[HFLIP_IDX] = val;
+	PDEBUG(D_CONF, "Set hvflip to %d %d", sd->hflip->val, sd->vflip->val);
 
 	/* The mt9m111 is flipped by default */
-	val = !val;
+	hflip = !sd->hflip->val;
+	vflip = !sd->vflip->val;
 
 	/* Set the correct page map */
 	err = m5602_write_sensor(sd, MT9M111_PAGE_MAP, data, 2);
 	if (err < 0)
 		return err;
 
-	err = m5602_read_sensor(sd, MT9M111_SC_R_MODE_CONTEXT_B, data, 2);
-	if (err < 0)
-		return err;
-
-	data[1] = (data[1] & 0xfd) | ((val << 1) & 0x02);
+	data[0] = MT9M111_RMB_OVER_SIZED;
+	if (gspca_dev->width == 640) {
+		data[1] = MT9M111_RMB_ROW_SKIP_2X |
+			  MT9M111_RMB_COLUMN_SKIP_2X |
+			  (hflip << 1) | vflip;
+	} else {
+		data[1] = MT9M111_RMB_ROW_SKIP_4X |
+			  MT9M111_RMB_COLUMN_SKIP_4X |
+			  (hflip << 1) | vflip;
+	}
 	err = m5602_write_sensor(sd, MT9M111_SC_R_MODE_CONTEXT_B,
 					data, 2);
 	return err;
 }
 
-static int mt9m111_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];
-	PDEBUG(D_V4L2, "Read gain %d", *val);
-
-	return 0;
-}
-
 static int mt9m111_set_auto_white_balance(struct gspca_dev *gspca_dev,
 					  __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	int err;
 	u8 data[2];
 
@@ -467,33 +291,19 @@ static int mt9m111_set_auto_white_balance(struct gspca_dev *gspca_dev,
 	if (err < 0)
 		return err;
 
-	sensor_settings[AUTO_WHITE_BALANCE_IDX] = val & 0x01;
 	data[1] = ((data[1] & 0xfd) | ((val & 0x01) << 1));
 
 	err = m5602_write_sensor(sd, MT9M111_CP_OPERATING_MODE_CTL, data, 2);
 
-	PDEBUG(D_V4L2, "Set auto white balance %d", val);
+	PDEBUG(D_CONF, "Set auto white balance %d", val);
 	return err;
 }
 
-static int mt9m111_get_auto_white_balance(struct gspca_dev *gspca_dev,
-					  __s32 *val) {
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[AUTO_WHITE_BALANCE_IDX];
-	PDEBUG(D_V4L2, "Read auto white balance %d", *val);
-	return 0;
-}
-
 static int mt9m111_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err, tmp;
 	u8 data[2] = {0x00, 0x00};
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	sensor_settings[GAIN_IDX] = val;
 
 	/* Set the correct page map */
 	err = m5602_write_sensor(sd, MT9M111_PAGE_MAP, data, 2);
@@ -518,7 +328,7 @@ static int mt9m111_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 
 	data[1] = (tmp & 0xff);
 	data[0] = (tmp & 0xff00) >> 8;
-	PDEBUG(D_V4L2, "tmp=%d, data[1]=%d, data[0]=%d", tmp,
+	PDEBUG(D_CONF, "tmp=%d, data[1]=%d, data[0]=%d", tmp,
 	       data[1], data[0]);
 
 	err = m5602_write_sensor(sd, MT9M111_SC_GLOBAL_GAIN,
@@ -532,13 +342,11 @@ static int mt9m111_set_green_balance(struct gspca_dev *gspca_dev, __s32 val)
 	int err;
 	u8 data[2];
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	sensor_settings[GREEN_BALANCE_IDX] = val;
 	data[1] = (val & 0xff);
 	data[0] = (val & 0xff00) >> 8;
 
-	PDEBUG(D_V4L2, "Set green balance %d", val);
+	PDEBUG(D_CONF, "Set green balance %d", val);
 	err = m5602_write_sensor(sd, MT9M111_SC_GREEN_1_GAIN,
 				 data, 2);
 	if (err < 0)
@@ -548,66 +356,68 @@ static int mt9m111_set_green_balance(struct gspca_dev *gspca_dev, __s32 val)
 				  data, 2);
 }
 
-static int mt9m111_get_green_balance(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[GREEN_BALANCE_IDX];
-	PDEBUG(D_V4L2, "Read green balance %d", *val);
-	return 0;
-}
-
 static int mt9m111_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val)
 {
 	u8 data[2];
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	sensor_settings[BLUE_BALANCE_IDX] = val;
 	data[1] = (val & 0xff);
 	data[0] = (val & 0xff00) >> 8;
 
-	PDEBUG(D_V4L2, "Set blue balance %d", val);
+	PDEBUG(D_CONF, "Set blue balance %d", val);
 
 	return m5602_write_sensor(sd, MT9M111_SC_BLUE_GAIN,
 				  data, 2);
 }
 
-static int mt9m111_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];
-	PDEBUG(D_V4L2, "Read blue balance %d", *val);
-	return 0;
-}
-
 static int mt9m111_set_red_balance(struct gspca_dev *gspca_dev, __s32 val)
 {
 	u8 data[2];
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	sensor_settings[RED_BALANCE_IDX] = val;
 	data[1] = (val & 0xff);
 	data[0] = (val & 0xff00) >> 8;
 
-	PDEBUG(D_V4L2, "Set red balance %d", val);
+	PDEBUG(D_CONF, "Set red balance %d", val);
 
 	return m5602_write_sensor(sd, MT9M111_SC_RED_GAIN,
 				  data, 2);
 }
 
-static int mt9m111_get_red_balance(struct gspca_dev *gspca_dev, __s32 *val)
+static int mt9m111_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;
-	s32 *sensor_settings = sd->sensor_priv;
+	int err;
 
-	*val = sensor_settings[RED_BALANCE_IDX];
-	PDEBUG(D_V4L2, "Read red balance %d", *val);
-	return 0;
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		err = mt9m111_set_auto_white_balance(gspca_dev, ctrl->val);
+		if (err || ctrl->val)
+			return err;
+		err = mt9m111_set_green_balance(gspca_dev, sd->green_bal->val);
+		if (err)
+			return err;
+		err = mt9m111_set_red_balance(gspca_dev, sd->red_bal->val);
+		if (err)
+			return err;
+		err = mt9m111_set_blue_balance(gspca_dev, sd->blue_bal->val);
+		break;
+	case V4L2_CID_GAIN:
+		err = mt9m111_set_gain(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		err = mt9m111_set_hvflip(gspca_dev);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return err;
 }
 
 static void mt9m111_dump_registers(struct sd *sd)
diff --git a/drivers/media/usb/gspca/m5602/m5602_mt9m111.h b/drivers/media/usb/gspca/m5602/m5602_mt9m111.h
index 8c672b5c8c6..07448d35e3c 100644
--- a/drivers/media/usb/gspca/m5602/m5602_mt9m111.h
+++ b/drivers/media/usb/gspca/m5602/m5602_mt9m111.h
@@ -110,6 +110,7 @@ extern bool dump_sensor;
 
 int mt9m111_probe(struct sd *sd);
 int mt9m111_init(struct sd *sd);
+int mt9m111_init_controls(struct sd *sd);
 int mt9m111_start(struct sd *sd);
 void mt9m111_disconnect(struct sd *sd);
 
@@ -121,6 +122,7 @@ static const struct m5602_sensor mt9m111 = {
 
 	.probe = mt9m111_probe,
 	.init = mt9m111_init,
+	.init_controls = mt9m111_init_controls,
 	.disconnect = mt9m111_disconnect,
 	.start = mt9m111_start,
 };
diff --git a/drivers/media/usb/gspca/m5602/m5602_ov7660.c b/drivers/media/usb/gspca/m5602/m5602_ov7660.c
index 9a14835c128..64b3b03a914 100644
--- a/drivers/media/usb/gspca/m5602/m5602_ov7660.c
+++ b/drivers/media/usb/gspca/m5602/m5602_ov7660.c
@@ -20,111 +20,8 @@
 
 #include "m5602_ov7660.h"
 
-static int ov7660_get_gain(struct gspca_dev *gspca_dev, __s32 *val);
-static int ov7660_set_gain(struct gspca_dev *gspca_dev, __s32 val);
-static int ov7660_get_auto_white_balance(struct gspca_dev *gspca_dev,
-					 __s32 *val);
-static int ov7660_set_auto_white_balance(struct gspca_dev *gspca_dev,
-					 __s32 val);
-static int ov7660_get_auto_gain(struct gspca_dev *gspca_dev, __s32 *val);
-static int ov7660_set_auto_gain(struct gspca_dev *gspca_dev, __s32 val);
-static int ov7660_get_auto_exposure(struct gspca_dev *gspca_dev, __s32 *val);
-static int ov7660_set_auto_exposure(struct gspca_dev *gspca_dev, __s32 val);
-static int ov7660_get_hflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int ov7660_set_hflip(struct gspca_dev *gspca_dev, __s32 val);
-static int ov7660_get_vflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int ov7660_set_vflip(struct gspca_dev *gspca_dev, __s32 val);
-
-static const struct ctrl ov7660_ctrls[] = {
-#define GAIN_IDX 1
-	{
-		{
-			.id		= V4L2_CID_GAIN,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "gain",
-			.minimum	= 0x00,
-			.maximum	= 0xff,
-			.step		= 0x1,
-			.default_value	= OV7660_DEFAULT_GAIN,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = ov7660_set_gain,
-		.get = ov7660_get_gain
-	},
-#define BLUE_BALANCE_IDX 2
-#define RED_BALANCE_IDX 3
-#define AUTO_WHITE_BALANCE_IDX 4
-	{
-		{
-			.id		= V4L2_CID_AUTO_WHITE_BALANCE,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "auto white balance",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 1
-		},
-		.set = ov7660_set_auto_white_balance,
-		.get = ov7660_get_auto_white_balance
-	},
-#define AUTO_GAIN_CTRL_IDX 5
-	{
-		{
-			.id		= V4L2_CID_AUTOGAIN,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "auto gain control",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 1
-		},
-		.set = ov7660_set_auto_gain,
-		.get = ov7660_get_auto_gain
-	},
-#define AUTO_EXPOSURE_IDX 6
-	{
-		{
-			.id		= V4L2_CID_EXPOSURE_AUTO,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "auto exposure",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 1
-		},
-		.set = ov7660_set_auto_exposure,
-		.get = ov7660_get_auto_exposure
-	},
-#define HFLIP_IDX 7
-	{
-		{
-			.id		= V4L2_CID_HFLIP,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "horizontal flip",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 0
-		},
-		.set = ov7660_set_hflip,
-		.get = ov7660_get_hflip
-	},
-#define VFLIP_IDX 8
-	{
-		{
-			.id		= V4L2_CID_VFLIP,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "vertical flip",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 0
-		},
-		.set = ov7660_set_vflip,
-		.get = ov7660_get_vflip
-	},
-
-};
+static int ov7660_s_ctrl(struct v4l2_ctrl *ctrl);
+static void ov7660_dump_registers(struct sd *sd);
 
 static struct v4l2_pix_format ov7660_modes[] = {
 	{
@@ -140,15 +37,15 @@ static struct v4l2_pix_format ov7660_modes[] = {
 	}
 };
 
-static void ov7660_dump_registers(struct sd *sd);
+static const struct v4l2_ctrl_ops ov7660_ctrl_ops = {
+	.s_ctrl = ov7660_s_ctrl,
+};
 
 int ov7660_probe(struct sd *sd)
 {
 	int err = 0, i;
 	u8 prod_id = 0, ver_id = 0;
 
-	s32 *sensor_settings;
-
 	if (force_sensor) {
 		if (force_sensor == OV7660_SENSOR) {
 			pr_info("Forcing an %s sensor\n", ov7660.name);
@@ -191,27 +88,15 @@ int ov7660_probe(struct sd *sd)
 	return -ENODEV;
 
 sensor_found:
-	sensor_settings = kmalloc(
-		ARRAY_SIZE(ov7660_ctrls) * sizeof(s32), GFP_KERNEL);
-	if (!sensor_settings)
-		return -ENOMEM;
-
 	sd->gspca_dev.cam.cam_mode = ov7660_modes;
 	sd->gspca_dev.cam.nmodes = ARRAY_SIZE(ov7660_modes);
-	sd->desc->ctrls = ov7660_ctrls;
-	sd->desc->nctrls = ARRAY_SIZE(ov7660_ctrls);
-
-	for (i = 0; i < ARRAY_SIZE(ov7660_ctrls); i++)
-		sensor_settings[i] = ov7660_ctrls[i].qctrl.default_value;
-	sd->sensor_priv = sensor_settings;
 
 	return 0;
 }
 
 int ov7660_init(struct sd *sd)
 {
-	int i, err = 0;
-	s32 *sensor_settings = sd->sensor_priv;
+	int i, err;
 
 	/* Init the sensor */
 	for (i = 0; i < ARRAY_SIZE(init_ov7660); i++) {
@@ -226,38 +111,47 @@ int ov7660_init(struct sd *sd)
 			err = m5602_write_sensor(sd,
 				init_ov7660[i][1], data, 1);
 		}
+		if (err < 0)
+			return err;
 	}
 
 	if (dump_sensor)
 		ov7660_dump_registers(sd);
 
-	err = ov7660_set_gain(&sd->gspca_dev, sensor_settings[GAIN_IDX]);
-	if (err < 0)
-		return err;
+	return 0;
+}
 
-	err = ov7660_set_auto_white_balance(&sd->gspca_dev,
-		sensor_settings[AUTO_WHITE_BALANCE_IDX]);
-	if (err < 0)
-		return err;
+int ov7660_init_controls(struct sd *sd)
+{
+	struct v4l2_ctrl_handler *hdl = &sd->gspca_dev.ctrl_handler;
 
-	err = ov7660_set_auto_gain(&sd->gspca_dev,
-		sensor_settings[AUTO_GAIN_CTRL_IDX]);
-	if (err < 0)
-		return err;
+	sd->gspca_dev.vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 6);
 
-	err = ov7660_set_auto_exposure(&sd->gspca_dev,
-		sensor_settings[AUTO_EXPOSURE_IDX]);
-	if (err < 0)
-		return err;
-	err = ov7660_set_hflip(&sd->gspca_dev,
-		sensor_settings[HFLIP_IDX]);
-	if (err < 0)
-		return err;
+	v4l2_ctrl_new_std(hdl, &ov7660_ctrl_ops, V4L2_CID_AUTO_WHITE_BALANCE,
+			  0, 1, 1, 1);
+	v4l2_ctrl_new_std_menu(hdl, &ov7660_ctrl_ops,
+			  V4L2_CID_EXPOSURE_AUTO, 1, 0, V4L2_EXPOSURE_AUTO);
 
-	err = ov7660_set_vflip(&sd->gspca_dev,
-		sensor_settings[VFLIP_IDX]);
+	sd->autogain = v4l2_ctrl_new_std(hdl, &ov7660_ctrl_ops,
+					 V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+	sd->gain = v4l2_ctrl_new_std(hdl, &ov7660_ctrl_ops, V4L2_CID_GAIN, 0,
+				     255, 1, OV7660_DEFAULT_GAIN);
 
-	return err;
+	sd->hflip = v4l2_ctrl_new_std(hdl, &ov7660_ctrl_ops, V4L2_CID_HFLIP,
+				      0, 1, 1, 0);
+	sd->vflip = v4l2_ctrl_new_std(hdl, &ov7660_ctrl_ops, V4L2_CID_VFLIP,
+				      0, 1, 1, 0);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+
+	v4l2_ctrl_auto_cluster(2, &sd->autogain, 0, false);
+	v4l2_ctrl_cluster(2, &sd->hflip);
+
+	return 0;
 }
 
 int ov7660_start(struct sd *sd)
@@ -275,56 +169,29 @@ void ov7660_disconnect(struct sd *sd)
 	ov7660_stop(sd);
 
 	sd->sensor = NULL;
-	kfree(sd->sensor_priv);
-}
-
-static int ov7660_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];
-	PDEBUG(D_V4L2, "Read gain %d", *val);
-	return 0;
 }
 
 static int ov7660_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
-	u8 i2c_data;
+	u8 i2c_data = val;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	PDEBUG(D_V4L2, "Setting gain to %d", val);
-
-	sensor_settings[GAIN_IDX] = val;
+	PDEBUG(D_CONF, "Setting gain to %d", val);
 
 	err = m5602_write_sensor(sd, OV7660_GAIN, &i2c_data, 1);
 	return err;
 }
 
-
-static int ov7660_get_auto_white_balance(struct gspca_dev *gspca_dev,
-					 __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[AUTO_WHITE_BALANCE_IDX];
-	return 0;
-}
-
 static int ov7660_set_auto_white_balance(struct gspca_dev *gspca_dev,
 					 __s32 val)
 {
 	int err;
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	PDEBUG(D_V4L2, "Set auto white balance to %d", val);
+	PDEBUG(D_CONF, "Set auto white balance to %d", val);
 
-	sensor_settings[AUTO_WHITE_BALANCE_IDX] = val;
 	err = m5602_read_sensor(sd, OV7660_COM8, &i2c_data, 1);
 	if (err < 0)
 		return err;
@@ -335,26 +202,14 @@ static int ov7660_set_auto_white_balance(struct gspca_dev *gspca_dev,
 	return err;
 }
 
-static int ov7660_get_auto_gain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[AUTO_GAIN_CTRL_IDX];
-	PDEBUG(D_V4L2, "Read auto gain control %d", *val);
-	return 0;
-}
-
 static int ov7660_set_auto_gain(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	PDEBUG(D_V4L2, "Set auto gain control to %d", val);
+	PDEBUG(D_CONF, "Set auto gain control to %d", val);
 
-	sensor_settings[AUTO_GAIN_CTRL_IDX] = val;
 	err = m5602_read_sensor(sd, OV7660_COM8, &i2c_data, 1);
 	if (err < 0)
 		return err;
@@ -364,94 +219,69 @@ static int ov7660_set_auto_gain(struct gspca_dev *gspca_dev, __s32 val)
 	return m5602_write_sensor(sd, OV7660_COM8, &i2c_data, 1);
 }
 
-static int ov7660_get_auto_exposure(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[AUTO_EXPOSURE_IDX];
-	PDEBUG(D_V4L2, "Read auto exposure control %d", *val);
-	return 0;
-}
-
 static int ov7660_set_auto_exposure(struct gspca_dev *gspca_dev,
 				    __s32 val)
 {
 	int err;
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	PDEBUG(D_V4L2, "Set auto exposure control to %d", val);
+	PDEBUG(D_CONF, "Set auto exposure control to %d", val);
 
-	sensor_settings[AUTO_EXPOSURE_IDX] = val;
 	err = m5602_read_sensor(sd, OV7660_COM8, &i2c_data, 1);
 	if (err < 0)
 		return err;
 
+	val = (val == V4L2_EXPOSURE_AUTO);
 	i2c_data = ((i2c_data & 0xfe) | ((val & 0x01) << 0));
 
 	return m5602_write_sensor(sd, OV7660_COM8, &i2c_data, 1);
 }
 
-static int ov7660_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 ov7660_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
+static int ov7660_set_hvflip(struct gspca_dev *gspca_dev)
 {
 	int err;
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	PDEBUG(D_V4L2, "Set horizontal flip to %d", val);
+	PDEBUG(D_CONF, "Set hvflip to %d, %d", sd->hflip->val, sd->vflip->val);
 
-	sensor_settings[HFLIP_IDX] = val;
-
-	i2c_data = ((val & 0x01) << 5) |
-		(sensor_settings[VFLIP_IDX] << 4);
+	i2c_data = (sd->hflip->val << 5) | (sd->vflip->val << 4);
 
 	err = m5602_write_sensor(sd, OV7660_MVFP, &i2c_data, 1);
 
 	return err;
 }
 
-static int ov7660_get_vflip(struct gspca_dev *gspca_dev, __s32 *val)
+static int ov7660_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;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[VFLIP_IDX];
-	PDEBUG(D_V4L2, "Read vertical flip %d", *val);
-
-	return 0;
-}
-
-static int ov7660_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
-{
 	int err;
-	u8 i2c_data;
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	PDEBUG(D_V4L2, "Set vertical flip to %d", val);
-	sensor_settings[VFLIP_IDX] = val;
 
-	i2c_data = ((val & 0x01) << 4) | (sensor_settings[VFLIP_IDX] << 5);
-	err = m5602_write_sensor(sd, OV7660_MVFP, &i2c_data, 1);
-	if (err < 0)
-		return err;
-
-	/* When vflip is toggled we need to readjust the bridge hsync/vsync */
-	if (gspca_dev->streaming)
-		err = ov7660_start(sd);
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		err = ov7660_set_auto_white_balance(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		err = ov7660_set_auto_exposure(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_AUTOGAIN:
+		err = ov7660_set_auto_gain(gspca_dev, ctrl->val);
+		if (err || ctrl->val)
+			return err;
+		err = ov7660_set_gain(gspca_dev, sd->gain->val);
+		break;
+	case V4L2_CID_HFLIP:
+		err = ov7660_set_hvflip(gspca_dev);
+		break;
+	default:
+		return -EINVAL;
+	}
 
 	return err;
 }
diff --git a/drivers/media/usb/gspca/m5602/m5602_ov7660.h b/drivers/media/usb/gspca/m5602/m5602_ov7660.h
index 2b6a13b508f..6fece1ce123 100644
--- a/drivers/media/usb/gspca/m5602/m5602_ov7660.h
+++ b/drivers/media/usb/gspca/m5602/m5602_ov7660.h
@@ -90,6 +90,8 @@ extern bool dump_sensor;
 
 int ov7660_probe(struct sd *sd);
 int ov7660_init(struct sd *sd);
+int ov7660_init(struct sd *sd);
+int ov7660_init_controls(struct sd *sd);
 int ov7660_start(struct sd *sd);
 int ov7660_stop(struct sd *sd);
 void ov7660_disconnect(struct sd *sd);
@@ -100,6 +102,7 @@ static const struct m5602_sensor ov7660 = {
 	.i2c_regW = 1,
 	.probe = ov7660_probe,
 	.init = ov7660_init,
+	.init_controls = ov7660_init_controls,
 	.start = ov7660_start,
 	.stop = ov7660_stop,
 	.disconnect = ov7660_disconnect,
diff --git a/drivers/media/usb/gspca/m5602/m5602_ov9650.c b/drivers/media/usb/gspca/m5602/m5602_ov9650.c
index 2114a8b90ec..59bc62bfae2 100644
--- a/drivers/media/usb/gspca/m5602/m5602_ov9650.c
+++ b/drivers/media/usb/gspca/m5602/m5602_ov9650.c
@@ -20,26 +20,8 @@
 
 #include "m5602_ov9650.h"
 
-static int ov9650_set_exposure(struct gspca_dev *gspca_dev, __s32 val);
-static int ov9650_get_exposure(struct gspca_dev *gspca_dev, __s32 *val);
-static int ov9650_get_gain(struct gspca_dev *gspca_dev, __s32 *val);
-static int ov9650_set_gain(struct gspca_dev *gspca_dev, __s32 val);
-static int ov9650_get_red_balance(struct gspca_dev *gspca_dev, __s32 *val);
-static int ov9650_set_red_balance(struct gspca_dev *gspca_dev, __s32 val);
-static int ov9650_get_blue_balance(struct gspca_dev *gspca_dev, __s32 *val);
-static int ov9650_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val);
-static int ov9650_get_hflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int ov9650_set_hflip(struct gspca_dev *gspca_dev, __s32 val);
-static int ov9650_get_vflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int ov9650_set_vflip(struct gspca_dev *gspca_dev, __s32 val);
-static int ov9650_get_auto_white_balance(struct gspca_dev *gspca_dev,
-					 __s32 *val);
-static int ov9650_set_auto_white_balance(struct gspca_dev *gspca_dev,
-					 __s32 val);
-static int ov9650_get_auto_gain(struct gspca_dev *gspca_dev, __s32 *val);
-static int ov9650_set_auto_gain(struct gspca_dev *gspca_dev, __s32 val);
-static int ov9650_get_auto_exposure(struct gspca_dev *gspca_dev, __s32 *val);
-static int ov9650_set_auto_exposure(struct gspca_dev *gspca_dev, __s32 val);
+static int ov9650_s_ctrl(struct v4l2_ctrl *ctrl);
+static void ov9650_dump_registers(struct sd *sd);
 
 /* Vertically and horizontally flips the image if matched, needed for machines
    where the sensor is mounted upside down */
@@ -113,140 +95,6 @@ static
 	{}
 };
 
-static const struct ctrl ov9650_ctrls[] = {
-#define EXPOSURE_IDX 0
-	{
-		{
-			.id		= V4L2_CID_EXPOSURE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "exposure",
-			.minimum	= 0x00,
-			.maximum	= 0x1ff,
-			.step		= 0x4,
-			.default_value	= EXPOSURE_DEFAULT,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = ov9650_set_exposure,
-		.get = ov9650_get_exposure
-	},
-#define GAIN_IDX 1
-	{
-		{
-			.id		= V4L2_CID_GAIN,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "gain",
-			.minimum	= 0x00,
-			.maximum	= 0x3ff,
-			.step		= 0x1,
-			.default_value	= GAIN_DEFAULT,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = ov9650_set_gain,
-		.get = ov9650_get_gain
-	},
-#define RED_BALANCE_IDX 2
-	{
-		{
-			.id		= V4L2_CID_RED_BALANCE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "red balance",
-			.minimum	= 0x00,
-			.maximum	= 0xff,
-			.step		= 0x1,
-			.default_value	= RED_GAIN_DEFAULT,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = ov9650_set_red_balance,
-		.get = ov9650_get_red_balance
-	},
-#define BLUE_BALANCE_IDX 3
-	{
-		{
-			.id		= V4L2_CID_BLUE_BALANCE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "blue balance",
-			.minimum	= 0x00,
-			.maximum	= 0xff,
-			.step		= 0x1,
-			.default_value	= BLUE_GAIN_DEFAULT,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = ov9650_set_blue_balance,
-		.get = ov9650_get_blue_balance
-	},
-#define HFLIP_IDX 4
-	{
-		{
-			.id		= V4L2_CID_HFLIP,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "horizontal flip",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 0
-		},
-		.set = ov9650_set_hflip,
-		.get = ov9650_get_hflip
-	},
-#define VFLIP_IDX 5
-	{
-		{
-			.id		= V4L2_CID_VFLIP,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "vertical flip",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 0
-		},
-		.set = ov9650_set_vflip,
-		.get = ov9650_get_vflip
-	},
-#define AUTO_WHITE_BALANCE_IDX 6
-	{
-		{
-			.id		= V4L2_CID_AUTO_WHITE_BALANCE,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "auto white balance",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 1
-		},
-		.set = ov9650_set_auto_white_balance,
-		.get = ov9650_get_auto_white_balance
-	},
-#define AUTO_GAIN_CTRL_IDX 7
-	{
-		{
-			.id		= V4L2_CID_AUTOGAIN,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "auto gain control",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 1
-		},
-		.set = ov9650_set_auto_gain,
-		.get = ov9650_get_auto_gain
-	},
-#define AUTO_EXPOSURE_IDX 8
-	{
-		{
-			.id		= V4L2_CID_EXPOSURE_AUTO,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "auto exposure",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 1
-		},
-		.set = ov9650_set_auto_exposure,
-		.get = ov9650_get_auto_exposure
-	}
-
-};
-
 static struct v4l2_pix_format ov9650_modes[] = {
 	{
 		176,
@@ -291,13 +139,15 @@ static struct v4l2_pix_format ov9650_modes[] = {
 	}
 };
 
-static void ov9650_dump_registers(struct sd *sd);
+static const struct v4l2_ctrl_ops ov9650_ctrl_ops = {
+	.s_ctrl = ov9650_s_ctrl,
+};
 
 int ov9650_probe(struct sd *sd)
 {
 	int err = 0;
 	u8 prod_id = 0, ver_id = 0, i;
-	s32 *sensor_settings;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 
 	if (force_sensor) {
 		if (force_sensor == OV9650_SENSOR) {
@@ -338,19 +188,9 @@ int ov9650_probe(struct sd *sd)
 	return -ENODEV;
 
 sensor_found:
-	sensor_settings = kmalloc(
-		ARRAY_SIZE(ov9650_ctrls) * sizeof(s32), GFP_KERNEL);
-	if (!sensor_settings)
-		return -ENOMEM;
-
 	sd->gspca_dev.cam.cam_mode = ov9650_modes;
 	sd->gspca_dev.cam.nmodes = ARRAY_SIZE(ov9650_modes);
-	sd->desc->ctrls = ov9650_ctrls;
-	sd->desc->nctrls = ARRAY_SIZE(ov9650_ctrls);
 
-	for (i = 0; i < ARRAY_SIZE(ov9650_ctrls); i++)
-		sensor_settings[i] = ov9650_ctrls[i].qctrl.default_value;
-	sd->sensor_priv = sensor_settings;
 	return 0;
 }
 
@@ -358,7 +198,6 @@ int ov9650_init(struct sd *sd)
 {
 	int i, err = 0;
 	u8 data;
-	s32 *sensor_settings = sd->sensor_priv;
 
 	if (dump_sensor)
 		ov9650_dump_registers(sd);
@@ -372,46 +211,52 @@ int ov9650_init(struct sd *sd)
 			err = m5602_write_bridge(sd, init_ov9650[i][1], data);
 	}
 
-	err = ov9650_set_exposure(&sd->gspca_dev,
-				   sensor_settings[EXPOSURE_IDX]);
-	if (err < 0)
-		return err;
-
-	err = ov9650_set_gain(&sd->gspca_dev, sensor_settings[GAIN_IDX]);
-	if (err < 0)
-		return err;
-
-	err = ov9650_set_red_balance(&sd->gspca_dev,
-				      sensor_settings[RED_BALANCE_IDX]);
-	if (err < 0)
-		return err;
-
-	err = ov9650_set_blue_balance(&sd->gspca_dev,
-				       sensor_settings[BLUE_BALANCE_IDX]);
-	if (err < 0)
-		return err;
-
-	err = ov9650_set_hflip(&sd->gspca_dev, sensor_settings[HFLIP_IDX]);
-	if (err < 0)
-		return err;
-
-	err = ov9650_set_vflip(&sd->gspca_dev, sensor_settings[VFLIP_IDX]);
-	if (err < 0)
-		return err;
+	return 0;
+}
 
-	err = ov9650_set_auto_exposure(&sd->gspca_dev,
-				sensor_settings[AUTO_EXPOSURE_IDX]);
-	if (err < 0)
-		return err;
+int ov9650_init_controls(struct sd *sd)
+{
+	struct v4l2_ctrl_handler *hdl = &sd->gspca_dev.ctrl_handler;
+
+	sd->gspca_dev.vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 9);
+
+	sd->auto_white_bal = v4l2_ctrl_new_std(hdl, &ov9650_ctrl_ops,
+					       V4L2_CID_AUTO_WHITE_BALANCE,
+					       0, 1, 1, 1);
+	sd->red_bal = v4l2_ctrl_new_std(hdl, &ov9650_ctrl_ops,
+					V4L2_CID_RED_BALANCE, 0, 255, 1,
+					RED_GAIN_DEFAULT);
+	sd->blue_bal = v4l2_ctrl_new_std(hdl, &ov9650_ctrl_ops,
+					V4L2_CID_BLUE_BALANCE, 0, 255, 1,
+					BLUE_GAIN_DEFAULT);
+
+	sd->autoexpo = v4l2_ctrl_new_std_menu(hdl, &ov9650_ctrl_ops,
+			  V4L2_CID_EXPOSURE_AUTO, 1, 0, V4L2_EXPOSURE_AUTO);
+	sd->expo = v4l2_ctrl_new_std(hdl, &ov9650_ctrl_ops, V4L2_CID_EXPOSURE,
+			  0, 0x1ff, 4, EXPOSURE_DEFAULT);
+
+	sd->autogain = v4l2_ctrl_new_std(hdl, &ov9650_ctrl_ops,
+					 V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+	sd->gain = v4l2_ctrl_new_std(hdl, &ov9650_ctrl_ops, V4L2_CID_GAIN, 0,
+				     0x3ff, 1, GAIN_DEFAULT);
+
+	sd->hflip = v4l2_ctrl_new_std(hdl, &ov9650_ctrl_ops, V4L2_CID_HFLIP,
+				      0, 1, 1, 0);
+	sd->vflip = v4l2_ctrl_new_std(hdl, &ov9650_ctrl_ops, V4L2_CID_VFLIP,
+				      0, 1, 1, 0);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
 
-	err = ov9650_set_auto_white_balance(&sd->gspca_dev,
-				sensor_settings[AUTO_WHITE_BALANCE_IDX]);
-	if (err < 0)
-		return err;
+	v4l2_ctrl_auto_cluster(3, &sd->auto_white_bal, 0, false);
+	v4l2_ctrl_auto_cluster(2, &sd->autoexpo, 0, false);
+	v4l2_ctrl_auto_cluster(2, &sd->autogain, 0, false);
+	v4l2_ctrl_cluster(2, &sd->hflip);
 
-	err = ov9650_set_auto_gain(&sd->gspca_dev,
-				sensor_settings[AUTO_GAIN_CTRL_IDX]);
-	return err;
+	return 0;
 }
 
 int ov9650_start(struct sd *sd)
@@ -419,17 +264,17 @@ int ov9650_start(struct sd *sd)
 	u8 data;
 	int i, err = 0;
 	struct cam *cam = &sd->gspca_dev.cam;
-	s32 *sensor_settings = sd->sensor_priv;
 
 	int width = cam->cam_mode[sd->gspca_dev.curr_mode].width;
 	int height = cam->cam_mode[sd->gspca_dev.curr_mode].height;
 	int ver_offs = cam->cam_mode[sd->gspca_dev.curr_mode].priv;
 	int hor_offs = OV9650_LEFT_OFFSET;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 
 	if ((!dmi_check_system(ov9650_flip_dmi_table) &&
-		sensor_settings[VFLIP_IDX]) ||
+		sd->vflip->val) ||
 		(dmi_check_system(ov9650_flip_dmi_table) &&
-		!sensor_settings[VFLIP_IDX]))
+		!sd->vflip->val))
 		ver_offs--;
 
 	if (width <= 320)
@@ -508,7 +353,7 @@ int ov9650_start(struct sd *sd)
 
 	switch (width) {
 	case 640:
-		PDEBUG(D_V4L2, "Configuring camera for VGA mode");
+		PDEBUG(D_CONF, "Configuring camera for VGA mode");
 
 		data = OV9650_VGA_SELECT | OV9650_RGB_SELECT |
 		       OV9650_RAW_RGB_SELECT;
@@ -516,7 +361,7 @@ int ov9650_start(struct sd *sd)
 		break;
 
 	case 352:
-		PDEBUG(D_V4L2, "Configuring camera for CIF mode");
+		PDEBUG(D_CONF, "Configuring camera for CIF mode");
 
 		data = OV9650_CIF_SELECT | OV9650_RGB_SELECT |
 				OV9650_RAW_RGB_SELECT;
@@ -524,7 +369,7 @@ int ov9650_start(struct sd *sd)
 		break;
 
 	case 320:
-		PDEBUG(D_V4L2, "Configuring camera for QVGA mode");
+		PDEBUG(D_CONF, "Configuring camera for QVGA mode");
 
 		data = OV9650_QVGA_SELECT | OV9650_RGB_SELECT |
 				OV9650_RAW_RGB_SELECT;
@@ -532,7 +377,7 @@ int ov9650_start(struct sd *sd)
 		break;
 
 	case 176:
-		PDEBUG(D_V4L2, "Configuring camera for QCIF mode");
+		PDEBUG(D_CONF, "Configuring camera for QCIF mode");
 
 		data = OV9650_QCIF_SELECT | OV9650_RGB_SELECT |
 			OV9650_RAW_RGB_SELECT;
@@ -553,29 +398,16 @@ void ov9650_disconnect(struct sd *sd)
 	ov9650_stop(sd);
 
 	sd->sensor = NULL;
-	kfree(sd->sensor_priv);
-}
-
-static int ov9650_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 ov9650_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 i2c_data;
 	int err;
 
-	PDEBUG(D_V4L2, "Set exposure to %d", val);
+	PDEBUG(D_CONF, "Set exposure to %d", val);
 
-	sensor_settings[EXPOSURE_IDX] = val;
 	/* The 6 MSBs */
 	i2c_data = (val >> 10) & 0x3f;
 	err = m5602_write_sensor(sd, OV9650_AECHM,
@@ -596,26 +428,13 @@ static int ov9650_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 	return err;
 }
 
-static int ov9650_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];
-	PDEBUG(D_V4L2, "Read gain %d", *val);
-	return 0;
-}
-
 static int ov9650_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	PDEBUG(D_V4L2, "Setting gain to %d", val);
-
-	sensor_settings[GAIN_IDX] = val;
+	PDEBUG(D_CONF, "Setting gain to %d", val);
 
 	/* The 2 MSB */
 	/* Read the OV9650_VREF register first to avoid
@@ -637,117 +456,46 @@ static int ov9650_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 	return err;
 }
 
-static int ov9650_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];
-	PDEBUG(D_V4L2, "Read red gain %d", *val);
-	return 0;
-}
-
 static int ov9650_set_red_balance(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	PDEBUG(D_V4L2, "Set red gain to %d", val);
-
-	sensor_settings[RED_BALANCE_IDX] = val;
+	PDEBUG(D_CONF, "Set red gain to %d", val);
 
 	i2c_data = val & 0xff;
 	err = m5602_write_sensor(sd, OV9650_RED, &i2c_data, 1);
 	return err;
 }
 
-static int ov9650_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];
-	PDEBUG(D_V4L2, "Read blue gain %d", *val);
-
-	return 0;
-}
-
 static int ov9650_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	PDEBUG(D_V4L2, "Set blue gain to %d", val);
 
-	sensor_settings[BLUE_BALANCE_IDX] = val;
+	PDEBUG(D_CONF, "Set blue gain to %d", val);
 
 	i2c_data = val & 0xff;
 	err = m5602_write_sensor(sd, OV9650_BLUE, &i2c_data, 1);
 	return err;
 }
 
-static int ov9650_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 ov9650_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
+static int ov9650_set_hvflip(struct gspca_dev *gspca_dev)
 {
 	int err;
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
+	int hflip = sd->hflip->val;
+	int vflip = sd->vflip->val;
 
-	PDEBUG(D_V4L2, "Set horizontal flip to %d", val);
-
-	sensor_settings[HFLIP_IDX] = val;
-
-	if (!dmi_check_system(ov9650_flip_dmi_table))
-		i2c_data = ((val & 0x01) << 5) |
-				(sensor_settings[VFLIP_IDX] << 4);
-	else
-		i2c_data = ((val & 0x01) << 5) |
-				(!sensor_settings[VFLIP_IDX] << 4);
-
-	err = m5602_write_sensor(sd, OV9650_MVFP, &i2c_data, 1);
-
-	return err;
-}
-
-static int ov9650_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 ov9650_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
-{
-	int err;
-	u8 i2c_data;
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	PDEBUG(D_V4L2, "Set vertical flip to %d", val);
-	sensor_settings[VFLIP_IDX] = val;
+	PDEBUG(D_CONF, "Set hvflip to %d %d", hflip, vflip);
 
 	if (dmi_check_system(ov9650_flip_dmi_table))
-		val = !val;
+		vflip = !vflip;
 
-	i2c_data = ((val & 0x01) << 4) | (sensor_settings[VFLIP_IDX] << 5);
+	i2c_data = (hflip << 5) | (vflip << 4);
 	err = m5602_write_sensor(sd, OV9650_MVFP, &i2c_data, 1);
 	if (err < 0)
 		return err;
@@ -759,57 +507,34 @@ static int ov9650_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
 	return err;
 }
 
-static int ov9650_get_auto_exposure(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[AUTO_EXPOSURE_IDX];
-	PDEBUG(D_V4L2, "Read auto exposure control %d", *val);
-	return 0;
-}
-
 static int ov9650_set_auto_exposure(struct gspca_dev *gspca_dev,
 				    __s32 val)
 {
 	int err;
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	PDEBUG(D_V4L2, "Set auto exposure control to %d", val);
+	PDEBUG(D_CONF, "Set auto exposure control to %d", val);
 
-	sensor_settings[AUTO_EXPOSURE_IDX] = val;
 	err = m5602_read_sensor(sd, OV9650_COM8, &i2c_data, 1);
 	if (err < 0)
 		return err;
 
+	val = (val == V4L2_EXPOSURE_AUTO);
 	i2c_data = ((i2c_data & 0xfe) | ((val & 0x01) << 0));
 
 	return m5602_write_sensor(sd, OV9650_COM8, &i2c_data, 1);
 }
 
-static int ov9650_get_auto_white_balance(struct gspca_dev *gspca_dev,
-					 __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[AUTO_WHITE_BALANCE_IDX];
-	return 0;
-}
-
 static int ov9650_set_auto_white_balance(struct gspca_dev *gspca_dev,
 					 __s32 val)
 {
 	int err;
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	PDEBUG(D_V4L2, "Set auto white balance to %d", val);
+	PDEBUG(D_CONF, "Set auto white balance to %d", val);
 
-	sensor_settings[AUTO_WHITE_BALANCE_IDX] = val;
 	err = m5602_read_sensor(sd, OV9650_COM8, &i2c_data, 1);
 	if (err < 0)
 		return err;
@@ -820,26 +545,14 @@ static int ov9650_set_auto_white_balance(struct gspca_dev *gspca_dev,
 	return err;
 }
 
-static int ov9650_get_auto_gain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[AUTO_GAIN_CTRL_IDX];
-	PDEBUG(D_V4L2, "Read auto gain control %d", *val);
-	return 0;
-}
-
 static int ov9650_set_auto_gain(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	PDEBUG(D_V4L2, "Set auto gain control to %d", val);
+	PDEBUG(D_CONF, "Set auto gain control to %d", val);
 
-	sensor_settings[AUTO_GAIN_CTRL_IDX] = val;
 	err = m5602_read_sensor(sd, OV9650_COM8, &i2c_data, 1);
 	if (err < 0)
 		return err;
@@ -849,6 +562,48 @@ static int ov9650_set_auto_gain(struct gspca_dev *gspca_dev, __s32 val)
 	return m5602_write_sensor(sd, OV9650_COM8, &i2c_data, 1);
 }
 
+static int ov9650_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;
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		err = ov9650_set_auto_white_balance(gspca_dev, ctrl->val);
+		if (err || ctrl->val)
+			return err;
+		err = ov9650_set_red_balance(gspca_dev, sd->red_bal->val);
+		if (err)
+			return err;
+		err = ov9650_set_blue_balance(gspca_dev, sd->blue_bal->val);
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		err = ov9650_set_auto_exposure(gspca_dev, ctrl->val);
+		if (err || ctrl->val == V4L2_EXPOSURE_AUTO)
+			return err;
+		err = ov9650_set_exposure(gspca_dev, sd->expo->val);
+		break;
+	case V4L2_CID_AUTOGAIN:
+		err = ov9650_set_auto_gain(gspca_dev, ctrl->val);
+		if (err || ctrl->val)
+			return err;
+		err = ov9650_set_gain(gspca_dev, sd->gain->val);
+		break;
+	case V4L2_CID_HFLIP:
+		err = ov9650_set_hvflip(gspca_dev);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return err;
+}
+
 static void ov9650_dump_registers(struct sd *sd)
 {
 	int address;
diff --git a/drivers/media/usb/gspca/m5602/m5602_ov9650.h b/drivers/media/usb/gspca/m5602/m5602_ov9650.h
index f7aa5bf6898..f9f5870da60 100644
--- a/drivers/media/usb/gspca/m5602/m5602_ov9650.h
+++ b/drivers/media/usb/gspca/m5602/m5602_ov9650.h
@@ -139,6 +139,7 @@ extern bool dump_sensor;
 
 int ov9650_probe(struct sd *sd);
 int ov9650_init(struct sd *sd);
+int ov9650_init_controls(struct sd *sd);
 int ov9650_start(struct sd *sd);
 int ov9650_stop(struct sd *sd);
 void ov9650_disconnect(struct sd *sd);
@@ -149,6 +150,7 @@ static const struct m5602_sensor ov9650 = {
 	.i2c_regW = 1,
 	.probe = ov9650_probe,
 	.init = ov9650_init,
+	.init_controls = ov9650_init_controls,
 	.start = ov9650_start,
 	.stop = ov9650_stop,
 	.disconnect = ov9650_disconnect,
diff --git a/drivers/media/usb/gspca/m5602/m5602_po1030.c b/drivers/media/usb/gspca/m5602/m5602_po1030.c
index b8771698cbc..4bf5c43424b 100644
--- a/drivers/media/usb/gspca/m5602/m5602_po1030.c
+++ b/drivers/media/usb/gspca/m5602/m5602_po1030.c
@@ -20,28 +20,8 @@
 
 #include "m5602_po1030.h"
 
-static int po1030_get_exposure(struct gspca_dev *gspca_dev, __s32 *val);
-static int po1030_set_exposure(struct gspca_dev *gspca_dev, __s32 val);
-static int po1030_get_gain(struct gspca_dev *gspca_dev, __s32 *val);
-static int po1030_set_gain(struct gspca_dev *gspca_dev, __s32 val);
-static int po1030_get_red_balance(struct gspca_dev *gspca_dev, __s32 *val);
-static int po1030_set_red_balance(struct gspca_dev *gspca_dev, __s32 val);
-static int po1030_get_blue_balance(struct gspca_dev *gspca_dev, __s32 *val);
-static int po1030_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val);
-static int po1030_get_green_balance(struct gspca_dev *gspca_dev, __s32 *val);
-static int po1030_set_green_balance(struct gspca_dev *gspca_dev, __s32 val);
-static int po1030_get_hflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int po1030_set_hflip(struct gspca_dev *gspca_dev, __s32 val);
-static int po1030_get_vflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int po1030_set_vflip(struct gspca_dev *gspca_dev, __s32 val);
-static int po1030_set_auto_white_balance(struct gspca_dev *gspca_dev,
-					 __s32 val);
-static int po1030_get_auto_white_balance(struct gspca_dev *gspca_dev,
-					 __s32 *val);
-static int po1030_set_auto_exposure(struct gspca_dev *gspca_dev,
-					 __s32 val);
-static int po1030_get_auto_exposure(struct gspca_dev *gspca_dev,
-					 __s32 *val);
+static int po1030_s_ctrl(struct v4l2_ctrl *ctrl);
+static void po1030_dump_registers(struct sd *sd);
 
 static struct v4l2_pix_format po1030_modes[] = {
 	{
@@ -56,146 +36,26 @@ static struct v4l2_pix_format po1030_modes[] = {
 	}
 };
 
-static const struct ctrl po1030_ctrls[] = {
-#define GAIN_IDX 0
-	{
-		{
-			.id		= V4L2_CID_GAIN,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "gain",
-			.minimum	= 0x00,
-			.maximum	= 0x4f,
-			.step		= 0x1,
-			.default_value	= PO1030_GLOBAL_GAIN_DEFAULT,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = po1030_set_gain,
-		.get = po1030_get_gain
-	},
-#define EXPOSURE_IDX 1
-	{
-		{
-			.id		= V4L2_CID_EXPOSURE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "exposure",
-			.minimum	= 0x00,
-			.maximum	= 0x02ff,
-			.step		= 0x1,
-			.default_value	= PO1030_EXPOSURE_DEFAULT,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = po1030_set_exposure,
-		.get = po1030_get_exposure
-	},
-#define RED_BALANCE_IDX 2
-	{
-		{
-			.id		= V4L2_CID_RED_BALANCE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "red balance",
-			.minimum	= 0x00,
-			.maximum	= 0xff,
-			.step		= 0x1,
-			.default_value	= PO1030_RED_GAIN_DEFAULT,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = po1030_set_red_balance,
-		.get = po1030_get_red_balance
-	},
-#define BLUE_BALANCE_IDX 3
-	{
-		{
-			.id		= V4L2_CID_BLUE_BALANCE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "blue balance",
-			.minimum	= 0x00,
-			.maximum	= 0xff,
-			.step		= 0x1,
-			.default_value	= PO1030_BLUE_GAIN_DEFAULT,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = po1030_set_blue_balance,
-		.get = po1030_get_blue_balance
-	},
-#define HFLIP_IDX 4
-	{
-		{
-			.id		= V4L2_CID_HFLIP,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "horizontal flip",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 0,
-		},
-		.set = po1030_set_hflip,
-		.get = po1030_get_hflip
-	},
-#define VFLIP_IDX 5
-	{
-		{
-			.id		= V4L2_CID_VFLIP,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "vertical flip",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 0,
-		},
-		.set = po1030_set_vflip,
-		.get = po1030_get_vflip
-	},
-#define AUTO_WHITE_BALANCE_IDX 6
-	{
-		{
-			.id		= V4L2_CID_AUTO_WHITE_BALANCE,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "auto white balance",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 0,
-		},
-		.set = po1030_set_auto_white_balance,
-		.get = po1030_get_auto_white_balance
-	},
-#define AUTO_EXPOSURE_IDX 7
-	{
-		{
-			.id		= V4L2_CID_EXPOSURE_AUTO,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "auto exposure",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 0,
-		},
-		.set = po1030_set_auto_exposure,
-		.get = po1030_get_auto_exposure
-	},
-#define GREEN_BALANCE_IDX 8
-	{
-		{
-			.id		= M5602_V4L2_CID_GREEN_BALANCE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "green balance",
-			.minimum	= 0x00,
-			.maximum	= 0xff,
-			.step		= 0x1,
-			.default_value	= PO1030_GREEN_GAIN_DEFAULT,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = po1030_set_green_balance,
-		.get = po1030_get_green_balance
-	},
+static const struct v4l2_ctrl_ops po1030_ctrl_ops = {
+	.s_ctrl = po1030_s_ctrl,
 };
 
-static void po1030_dump_registers(struct sd *sd);
+static const struct v4l2_ctrl_config po1030_greenbal_cfg = {
+	.ops	= &po1030_ctrl_ops,
+	.id	= M5602_V4L2_CID_GREEN_BALANCE,
+	.name	= "Green Balance",
+	.type	= V4L2_CTRL_TYPE_INTEGER,
+	.min	= 0,
+	.max	= 255,
+	.step	= 1,
+	.def	= PO1030_GREEN_GAIN_DEFAULT,
+	.flags	= V4L2_CTRL_FLAG_SLIDER,
+};
 
 int po1030_probe(struct sd *sd)
 {
 	u8 dev_id_h = 0, i;
-	s32 *sensor_settings;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 
 	if (force_sensor) {
 		if (force_sensor == PO1030_SENSOR) {
@@ -229,26 +89,14 @@ int po1030_probe(struct sd *sd)
 	return -ENODEV;
 
 sensor_found:
-	sensor_settings = kmalloc(
-		ARRAY_SIZE(po1030_ctrls) * sizeof(s32), GFP_KERNEL);
-	if (!sensor_settings)
-		return -ENOMEM;
-
 	sd->gspca_dev.cam.cam_mode = po1030_modes;
 	sd->gspca_dev.cam.nmodes = ARRAY_SIZE(po1030_modes);
-	sd->desc->ctrls = po1030_ctrls;
-	sd->desc->nctrls = ARRAY_SIZE(po1030_ctrls);
-
-	for (i = 0; i < ARRAY_SIZE(po1030_ctrls); i++)
-		sensor_settings[i] = po1030_ctrls[i].qctrl.default_value;
-	sd->sensor_priv = sensor_settings;
 
 	return 0;
 }
 
 int po1030_init(struct sd *sd)
 {
-	s32 *sensor_settings = sd->sensor_priv;
 	int i, err = 0;
 
 	/* Init the sensor */
@@ -279,46 +127,50 @@ int po1030_init(struct sd *sd)
 	if (dump_sensor)
 		po1030_dump_registers(sd);
 
-	err = po1030_set_exposure(&sd->gspca_dev,
-				   sensor_settings[EXPOSURE_IDX]);
-	if (err < 0)
-		return err;
-
-	err = po1030_set_gain(&sd->gspca_dev, sensor_settings[GAIN_IDX]);
-	if (err < 0)
-		return err;
-
-	err = po1030_set_hflip(&sd->gspca_dev, sensor_settings[HFLIP_IDX]);
-	if (err < 0)
-		return err;
-
-	err = po1030_set_vflip(&sd->gspca_dev, sensor_settings[VFLIP_IDX]);
-	if (err < 0)
-		return err;
-
-	err = po1030_set_red_balance(&sd->gspca_dev,
-				      sensor_settings[RED_BALANCE_IDX]);
-	if (err < 0)
-		return err;
-
-	err = po1030_set_blue_balance(&sd->gspca_dev,
-				      sensor_settings[BLUE_BALANCE_IDX]);
-	if (err < 0)
-		return err;
+	return 0;
+}
 
-	err = po1030_set_green_balance(&sd->gspca_dev,
-				       sensor_settings[GREEN_BALANCE_IDX]);
-	if (err < 0)
-		return err;
+int po1030_init_controls(struct sd *sd)
+{
+	struct v4l2_ctrl_handler *hdl = &sd->gspca_dev.ctrl_handler;
+
+	sd->gspca_dev.vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 9);
+
+	sd->auto_white_bal = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops,
+					       V4L2_CID_AUTO_WHITE_BALANCE,
+					       0, 1, 1, 0);
+	sd->green_bal = v4l2_ctrl_new_custom(hdl, &po1030_greenbal_cfg, NULL);
+	sd->red_bal = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops,
+					V4L2_CID_RED_BALANCE, 0, 255, 1,
+					PO1030_RED_GAIN_DEFAULT);
+	sd->blue_bal = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops,
+					V4L2_CID_BLUE_BALANCE, 0, 255, 1,
+					PO1030_BLUE_GAIN_DEFAULT);
+
+	sd->autoexpo = v4l2_ctrl_new_std_menu(hdl, &po1030_ctrl_ops,
+			  V4L2_CID_EXPOSURE_AUTO, 1, 0, V4L2_EXPOSURE_MANUAL);
+	sd->expo = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops, V4L2_CID_EXPOSURE,
+			  0, 0x2ff, 1, PO1030_EXPOSURE_DEFAULT);
+
+	sd->gain = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops, V4L2_CID_GAIN, 0,
+				     0x4f, 1, PO1030_GLOBAL_GAIN_DEFAULT);
+
+	sd->hflip = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops, V4L2_CID_HFLIP,
+				      0, 1, 1, 0);
+	sd->vflip = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops, V4L2_CID_VFLIP,
+				      0, 1, 1, 0);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
 
-	err = po1030_set_auto_white_balance(&sd->gspca_dev,
-				sensor_settings[AUTO_WHITE_BALANCE_IDX]);
-	if (err < 0)
-		return err;
+	v4l2_ctrl_auto_cluster(4, &sd->auto_white_bal, 0, false);
+	v4l2_ctrl_auto_cluster(2, &sd->autoexpo, 0, false);
+	v4l2_ctrl_cluster(2, &sd->hflip);
 
-	err = po1030_set_auto_exposure(&sd->gspca_dev,
-				sensor_settings[AUTO_EXPOSURE_IDX]);
-	return err;
+	return 0;
 }
 
 int po1030_start(struct sd *sd)
@@ -448,28 +300,16 @@ int po1030_start(struct sd *sd)
 	return err;
 }
 
-static int po1030_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, "Exposure read as %d", *val);
-	return 0;
-}
-
 static int po1030_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 i2c_data;
 	int err;
 
-	sensor_settings[EXPOSURE_IDX] = val;
-	PDEBUG(D_V4L2, "Set exposure to %d", val & 0xffff);
+	PDEBUG(D_CONF, "Set exposure to %d", val & 0xffff);
 
 	i2c_data = ((val & 0xff00) >> 8);
-	PDEBUG(D_V4L2, "Set exposure to high byte to 0x%x",
+	PDEBUG(D_CONF, "Set exposure to high byte to 0x%x",
 	       i2c_data);
 
 	err = m5602_write_sensor(sd, PO1030_INTEGLINES_H,
@@ -478,7 +318,7 @@ static int po1030_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 		return err;
 
 	i2c_data = (val & 0xff);
-	PDEBUG(D_V4L2, "Set exposure to low byte to 0x%x",
+	PDEBUG(D_CONF, "Set exposure to low byte to 0x%x",
 	       i2c_data);
 	err = m5602_write_sensor(sd, PO1030_INTEGLINES_M,
 				  &i2c_data, 1);
@@ -486,91 +326,32 @@ static int po1030_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 	return err;
 }
 
-static int po1030_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];
-	PDEBUG(D_V4L2, "Read global gain %d", *val);
-	return 0;
-}
-
 static int po1030_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 i2c_data;
 	int err;
 
-	sensor_settings[GAIN_IDX] = val;
-
 	i2c_data = val & 0xff;
-	PDEBUG(D_V4L2, "Set global gain to %d", i2c_data);
+	PDEBUG(D_CONF, "Set global gain to %d", i2c_data);
 	err = m5602_write_sensor(sd, PO1030_GLOBALGAIN,
 				 &i2c_data, 1);
 	return err;
 }
 
-static int po1030_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 hflip %d", *val);
-
-	return 0;
-}
-
-static int po1030_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-	u8 i2c_data;
-	int err;
-
-	sensor_settings[HFLIP_IDX] = val;
-
-	PDEBUG(D_V4L2, "Set hflip %d", val);
-	err = m5602_read_sensor(sd, PO1030_CONTROL2, &i2c_data, 1);
-	if (err < 0)
-		return err;
-
-	i2c_data = (0x7f & i2c_data) | ((val & 0x01) << 7);
-
-	err = m5602_write_sensor(sd, PO1030_CONTROL2,
-				 &i2c_data, 1);
-
-	return err;
-}
-
-static int po1030_get_vflip(struct gspca_dev *gspca_dev, __s32 *val)
+static int po1030_set_hvflip(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[VFLIP_IDX];
-	PDEBUG(D_V4L2, "Read vflip %d", *val);
-
-	return 0;
-}
-
-static int po1030_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 i2c_data;
 	int err;
 
-	sensor_settings[VFLIP_IDX] = val;
-
-	PDEBUG(D_V4L2, "Set vflip %d", val);
+	PDEBUG(D_CONF, "Set hvflip %d %d", sd->hflip->val, sd->vflip->val);
 	err = m5602_read_sensor(sd, PO1030_CONTROL2, &i2c_data, 1);
 	if (err < 0)
 		return err;
 
-	i2c_data = (i2c_data & 0xbf) | ((val & 0x01) << 6);
+	i2c_data = (0x3f & i2c_data) | (sd->hflip->val << 7) |
+		   (sd->vflip->val << 6);
 
 	err = m5602_write_sensor(sd, PO1030_CONTROL2,
 				 &i2c_data, 1);
@@ -578,81 +359,41 @@ static int po1030_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
 	return err;
 }
 
-static int po1030_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];
-	PDEBUG(D_V4L2, "Read red gain %d", *val);
-	return 0;
-}
-
 static int po1030_set_red_balance(struct gspca_dev *gspca_dev, __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 i2c_data;
 	int err;
 
-	sensor_settings[RED_BALANCE_IDX] = val;
-
 	i2c_data = val & 0xff;
-	PDEBUG(D_V4L2, "Set red gain to %d", i2c_data);
+	PDEBUG(D_CONF, "Set red gain to %d", i2c_data);
 	err = m5602_write_sensor(sd, PO1030_RED_GAIN,
 				  &i2c_data, 1);
 	return err;
 }
 
-static int po1030_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];
-	PDEBUG(D_V4L2, "Read blue gain %d", *val);
-
-	return 0;
-}
-
 static int po1030_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 i2c_data;
 	int err;
 
-	sensor_settings[BLUE_BALANCE_IDX] = val;
-
 	i2c_data = val & 0xff;
-	PDEBUG(D_V4L2, "Set blue gain to %d", i2c_data);
+	PDEBUG(D_CONF, "Set blue gain to %d", i2c_data);
 	err = m5602_write_sensor(sd, PO1030_BLUE_GAIN,
 				  &i2c_data, 1);
 
 	return err;
 }
 
-static int po1030_get_green_balance(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[GREEN_BALANCE_IDX];
-	PDEBUG(D_V4L2, "Read green gain %d", *val);
-
-	return 0;
-}
-
 static int po1030_set_green_balance(struct gspca_dev *gspca_dev, __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 i2c_data;
 	int err;
 
-	sensor_settings[GREEN_BALANCE_IDX] = val;
 	i2c_data = val & 0xff;
-	PDEBUG(D_V4L2, "Set green gain to %d", i2c_data);
+	PDEBUG(D_CONF, "Set green gain to %d", i2c_data);
 
 	err = m5602_write_sensor(sd, PO1030_GREEN_1_GAIN,
 			   &i2c_data, 1);
@@ -663,63 +404,36 @@ static int po1030_set_green_balance(struct gspca_dev *gspca_dev, __s32 val)
 				 &i2c_data, 1);
 }
 
-static int po1030_get_auto_white_balance(struct gspca_dev *gspca_dev,
-					 __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[AUTO_WHITE_BALANCE_IDX];
-	PDEBUG(D_V4L2, "Auto white balancing is %d", *val);
-
-	return 0;
-}
-
 static int po1030_set_auto_white_balance(struct gspca_dev *gspca_dev,
 					 __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 i2c_data;
 	int err;
 
-	sensor_settings[AUTO_WHITE_BALANCE_IDX] = val;
-
 	err = m5602_read_sensor(sd, PO1030_AUTOCTRL1, &i2c_data, 1);
 	if (err < 0)
 		return err;
 
-	PDEBUG(D_V4L2, "Set auto white balance to %d", val);
+	PDEBUG(D_CONF, "Set auto white balance to %d", val);
 	i2c_data = (i2c_data & 0xfe) | (val & 0x01);
 	err = m5602_write_sensor(sd, PO1030_AUTOCTRL1, &i2c_data, 1);
 	return err;
 }
 
-static int po1030_get_auto_exposure(struct gspca_dev *gspca_dev,
-				    __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[AUTO_EXPOSURE_IDX];
-	PDEBUG(D_V4L2, "Auto exposure is %d", *val);
-	return 0;
-}
-
 static int po1030_set_auto_exposure(struct gspca_dev *gspca_dev,
 				    __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 i2c_data;
 	int err;
 
-	sensor_settings[AUTO_EXPOSURE_IDX] = val;
 	err = m5602_read_sensor(sd, PO1030_AUTOCTRL1, &i2c_data, 1);
 	if (err < 0)
 		return err;
 
-	PDEBUG(D_V4L2, "Set auto exposure to %d", val);
+	PDEBUG(D_CONF, "Set auto exposure to %d", val);
+	val = (val == V4L2_EXPOSURE_AUTO);
 	i2c_data = (i2c_data & 0xfd) | ((val & 0x01) << 1);
 	return m5602_write_sensor(sd, PO1030_AUTOCTRL1, &i2c_data, 1);
 }
@@ -727,7 +441,48 @@ static int po1030_set_auto_exposure(struct gspca_dev *gspca_dev,
 void po1030_disconnect(struct sd *sd)
 {
 	sd->sensor = NULL;
-	kfree(sd->sensor_priv);
+}
+
+static int po1030_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;
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		err = po1030_set_auto_white_balance(gspca_dev, ctrl->val);
+		if (err || ctrl->val)
+			return err;
+		err = po1030_set_green_balance(gspca_dev, sd->green_bal->val);
+		if (err)
+			return err;
+		err = po1030_set_red_balance(gspca_dev, sd->red_bal->val);
+		if (err)
+			return err;
+		err = po1030_set_blue_balance(gspca_dev, sd->blue_bal->val);
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		err = po1030_set_auto_exposure(gspca_dev, ctrl->val);
+		if (err || ctrl->val == V4L2_EXPOSURE_AUTO)
+			return err;
+		err = po1030_set_exposure(gspca_dev, sd->expo->val);
+		break;
+	case V4L2_CID_GAIN:
+		err = po1030_set_gain(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		err = po1030_set_hvflip(gspca_dev);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return err;
 }
 
 static void po1030_dump_registers(struct sd *sd)
diff --git a/drivers/media/usb/gspca/m5602/m5602_po1030.h b/drivers/media/usb/gspca/m5602/m5602_po1030.h
index 81a2bcb88fe..a6ab76149bd 100644
--- a/drivers/media/usb/gspca/m5602/m5602_po1030.h
+++ b/drivers/media/usb/gspca/m5602/m5602_po1030.h
@@ -151,6 +151,7 @@ extern bool dump_sensor;
 
 int po1030_probe(struct sd *sd);
 int po1030_init(struct sd *sd);
+int po1030_init_controls(struct sd *sd);
 int po1030_start(struct sd *sd);
 void po1030_disconnect(struct sd *sd);
 
@@ -162,6 +163,7 @@ static const struct m5602_sensor po1030 = {
 
 	.probe = po1030_probe,
 	.init = po1030_init,
+	.init_controls = po1030_init_controls,
 	.start = po1030_start,
 	.disconnect = po1030_disconnect,
 };
diff --git a/drivers/media/usb/gspca/m5602/m5602_s5k4aa.c b/drivers/media/usb/gspca/m5602/m5602_s5k4aa.c
index c8e1572eb50..7d12599458e 100644
--- a/drivers/media/usb/gspca/m5602/m5602_s5k4aa.c
+++ b/drivers/media/usb/gspca/m5602/m5602_s5k4aa.c
@@ -20,18 +20,12 @@
 
 #include "m5602_s5k4aa.h"
 
-static int s5k4aa_get_exposure(struct gspca_dev *gspca_dev, __s32 *val);
-static int s5k4aa_set_exposure(struct gspca_dev *gspca_dev, __s32 val);
-static int s5k4aa_get_vflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int s5k4aa_set_vflip(struct gspca_dev *gspca_dev, __s32 val);
-static int s5k4aa_get_hflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int s5k4aa_set_hflip(struct gspca_dev *gspca_dev, __s32 val);
-static int s5k4aa_get_gain(struct gspca_dev *gspca_dev, __s32 *val);
-static int s5k4aa_set_gain(struct gspca_dev *gspca_dev, __s32 val);
-static int s5k4aa_get_noise(struct gspca_dev *gspca_dev, __s32 *val);
-static int s5k4aa_set_noise(struct gspca_dev *gspca_dev, __s32 val);
-static int s5k4aa_get_brightness(struct gspca_dev *gspca_dev, __s32 *val);
-static int s5k4aa_set_brightness(struct gspca_dev *gspca_dev, __s32 val);
+static int s5k4aa_s_ctrl(struct v4l2_ctrl *ctrl);
+static void s5k4aa_dump_registers(struct sd *sd);
+
+static const struct v4l2_ctrl_ops s5k4aa_ctrl_ops = {
+	.s_ctrl = s5k4aa_s_ctrl,
+};
 
 static
     const
@@ -147,104 +141,12 @@ static struct v4l2_pix_format s5k4aa_modes[] = {
 	}
 };
 
-static const struct ctrl s5k4aa_ctrls[] = {
-#define VFLIP_IDX 0
-	{
-		{
-			.id		= V4L2_CID_VFLIP,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "vertical flip",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 0
-		},
-		.set = s5k4aa_set_vflip,
-		.get = s5k4aa_get_vflip
-	},
-#define HFLIP_IDX 1
-	{
-		{
-			.id		= V4L2_CID_HFLIP,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "horizontal flip",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 0
-		},
-		.set = s5k4aa_set_hflip,
-		.get = s5k4aa_get_hflip
-	},
-#define GAIN_IDX 2
-	{
-		{
-			.id		= V4L2_CID_GAIN,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "Gain",
-			.minimum	= 0,
-			.maximum	= 127,
-			.step		= 1,
-			.default_value	= S5K4AA_DEFAULT_GAIN,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = s5k4aa_set_gain,
-		.get = s5k4aa_get_gain
-	},
-#define EXPOSURE_IDX 3
-	{
-		{
-			.id		= V4L2_CID_EXPOSURE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "Exposure",
-			.minimum	= 13,
-			.maximum	= 0xfff,
-			.step		= 1,
-			.default_value	= 0x100,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = s5k4aa_set_exposure,
-		.get = s5k4aa_get_exposure
-	},
-#define NOISE_SUPP_IDX 4
-	{
-		{
-			.id		= V4L2_CID_PRIVATE_BASE,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "Noise suppression (smoothing)",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 1,
-		},
-			.set = s5k4aa_set_noise,
-			.get = s5k4aa_get_noise
-	},
-#define BRIGHTNESS_IDX 5
-	{
-		{
-			.id		= V4L2_CID_BRIGHTNESS,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "Brightness",
-			.minimum	= 0,
-			.maximum	= 0x1f,
-			.step		= 1,
-			.default_value	= S5K4AA_DEFAULT_BRIGHTNESS,
-		},
-			.set = s5k4aa_set_brightness,
-			.get = s5k4aa_get_brightness
-	},
-
-};
-
-static void s5k4aa_dump_registers(struct sd *sd);
-
 int s5k4aa_probe(struct sd *sd)
 {
 	u8 prod_id[6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
 	const u8 expected_prod_id[6] = {0x00, 0x10, 0x00, 0x4b, 0x33, 0x75};
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int i, err = 0;
-	s32 *sensor_settings;
 
 	if (force_sensor) {
 		if (force_sensor == S5K4AA_SENSOR) {
@@ -303,19 +205,8 @@ int s5k4aa_probe(struct sd *sd)
 		pr_info("Detected a s5k4aa sensor\n");
 
 sensor_found:
-	sensor_settings = kmalloc(
-		ARRAY_SIZE(s5k4aa_ctrls) * sizeof(s32), GFP_KERNEL);
-	if (!sensor_settings)
-		return -ENOMEM;
-
 	sd->gspca_dev.cam.cam_mode = s5k4aa_modes;
 	sd->gspca_dev.cam.nmodes = ARRAY_SIZE(s5k4aa_modes);
-	sd->desc->ctrls = s5k4aa_ctrls;
-	sd->desc->nctrls = ARRAY_SIZE(s5k4aa_ctrls);
-
-	for (i = 0; i < ARRAY_SIZE(s5k4aa_ctrls); i++)
-		sensor_settings[i] = s5k4aa_ctrls[i].qctrl.default_value;
-	sd->sensor_priv = sensor_settings;
 
 	return 0;
 }
@@ -325,11 +216,11 @@ int s5k4aa_start(struct sd *sd)
 	int i, err = 0;
 	u8 data[2];
 	struct cam *cam = &sd->gspca_dev.cam;
-	s32 *sensor_settings = sd->sensor_priv;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 
 	switch (cam->cam_mode[sd->gspca_dev.curr_mode].width) {
 	case 1280:
-		PDEBUG(D_V4L2, "Configuring camera for SXGA mode");
+		PDEBUG(D_CONF, "Configuring camera for SXGA mode");
 
 		for (i = 0; i < ARRAY_SIZE(SXGA_s5k4aa); i++) {
 			switch (SXGA_s5k4aa[i][0]) {
@@ -359,13 +250,10 @@ int s5k4aa_start(struct sd *sd)
 				return -EINVAL;
 			}
 		}
-		err = s5k4aa_set_noise(&sd->gspca_dev, 0);
-		if (err < 0)
-			return err;
 		break;
 
 	case 640:
-		PDEBUG(D_V4L2, "Configuring camera for VGA mode");
+		PDEBUG(D_CONF, "Configuring camera for VGA mode");
 
 		for (i = 0; i < ARRAY_SIZE(VGA_s5k4aa); i++) {
 			switch (VGA_s5k4aa[i][0]) {
@@ -395,37 +283,12 @@ int s5k4aa_start(struct sd *sd)
 				return -EINVAL;
 			}
 		}
-		err = s5k4aa_set_noise(&sd->gspca_dev, 1);
-		if (err < 0)
-			return err;
 		break;
 	}
 	if (err < 0)
 		return err;
 
-	err = s5k4aa_set_exposure(&sd->gspca_dev,
-				   sensor_settings[EXPOSURE_IDX]);
-	if (err < 0)
-		return err;
-
-	err = s5k4aa_set_gain(&sd->gspca_dev, sensor_settings[GAIN_IDX]);
-	if (err < 0)
-		return err;
-
-	err = s5k4aa_set_brightness(&sd->gspca_dev,
-				     sensor_settings[BRIGHTNESS_IDX]);
-	if (err < 0)
-		return err;
-
-	err = s5k4aa_set_noise(&sd->gspca_dev, sensor_settings[NOISE_SUPP_IDX]);
-	if (err < 0)
-		return err;
-
-	err = s5k4aa_set_vflip(&sd->gspca_dev, sensor_settings[VFLIP_IDX]);
-	if (err < 0)
-		return err;
-
-	return s5k4aa_set_hflip(&sd->gspca_dev, sensor_settings[HFLIP_IDX]);
+	return 0;
 }
 
 int s5k4aa_init(struct sd *sd)
@@ -466,13 +329,36 @@ int s5k4aa_init(struct sd *sd)
 	return err;
 }
 
-static int s5k4aa_get_exposure(struct gspca_dev *gspca_dev, __s32 *val)
+int s5k4aa_init_controls(struct sd *sd)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
+	struct v4l2_ctrl_handler *hdl = &sd->gspca_dev.ctrl_handler;
+
+	sd->gspca_dev.vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 6);
 
-	*val = sensor_settings[EXPOSURE_IDX];
-	PDEBUG(D_V4L2, "Read exposure %d", *val);
+	v4l2_ctrl_new_std(hdl, &s5k4aa_ctrl_ops, V4L2_CID_BRIGHTNESS,
+			  0, 0x1f, 1, S5K4AA_DEFAULT_BRIGHTNESS);
+
+	v4l2_ctrl_new_std(hdl, &s5k4aa_ctrl_ops, V4L2_CID_EXPOSURE,
+			  13, 0xfff, 1, 0x100);
+
+	v4l2_ctrl_new_std(hdl, &s5k4aa_ctrl_ops, V4L2_CID_GAIN,
+			  0, 127, 1, S5K4AA_DEFAULT_GAIN);
+
+	v4l2_ctrl_new_std(hdl, &s5k4aa_ctrl_ops, V4L2_CID_SHARPNESS,
+			  0, 1, 1, 1);
+
+	sd->hflip = v4l2_ctrl_new_std(hdl, &s5k4aa_ctrl_ops, V4L2_CID_HFLIP,
+				      0, 1, 1, 0);
+	sd->vflip = v4l2_ctrl_new_std(hdl, &s5k4aa_ctrl_ops, V4L2_CID_VFLIP,
+				      0, 1, 1, 0);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+
+	v4l2_ctrl_cluster(2, &sd->hflip);
 
 	return 0;
 }
@@ -480,12 +366,10 @@ static int s5k4aa_get_exposure(struct gspca_dev *gspca_dev, __s32 *val)
 static int s5k4aa_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 data = S5K4AA_PAGE_MAP_2;
 	int err;
 
-	sensor_settings[EXPOSURE_IDX] = val;
-	PDEBUG(D_V4L2, "Set exposure to %d", val);
+	PDEBUG(D_CONF, "Set exposure to %d", val);
 	err = m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
 	if (err < 0)
 		return err;
@@ -499,27 +383,15 @@ static int s5k4aa_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 	return err;
 }
 
-static int s5k4aa_get_vflip(struct gspca_dev *gspca_dev, __s32 *val)
+static int s5k4aa_set_hvflip(struct gspca_dev *gspca_dev)
 {
 	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 s5k4aa_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 data = S5K4AA_PAGE_MAP_2;
 	int err;
+	int hflip = sd->hflip->val;
+	int vflip = sd->vflip->val;
 
-	sensor_settings[VFLIP_IDX] = val;
-
-	PDEBUG(D_V4L2, "Set vertical flip to %d", val);
+	PDEBUG(D_CONF, "Set hvflip %d %d", hflip, vflip);
 	err = m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
 	if (err < 0)
 		return err;
@@ -528,93 +400,48 @@ static int s5k4aa_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
 	if (err < 0)
 		return err;
 
-	if (dmi_check_system(s5k4aa_vflip_dmi_table))
-		val = !val;
+	if (dmi_check_system(s5k4aa_vflip_dmi_table)) {
+		hflip = !hflip;
+		vflip = !vflip;
+	}
 
-	data = ((data & ~S5K4AA_RM_V_FLIP) | ((val & 0x01) << 7));
+	data = (data & 0x7f) | (vflip << 7) | (hflip << 6);
 	err = m5602_write_sensor(sd, S5K4AA_READ_MODE, &data, 1);
 	if (err < 0)
 		return err;
 
-	err = m5602_read_sensor(sd, S5K4AA_ROWSTART_LO, &data, 1);
+	err = m5602_read_sensor(sd, S5K4AA_COLSTART_LO, &data, 1);
 	if (err < 0)
 		return err;
-	if (val)
+	if (hflip)
 		data &= 0xfe;
 	else
 		data |= 0x01;
-	err = m5602_write_sensor(sd, S5K4AA_ROWSTART_LO, &data, 1);
-	return err;
-}
-
-static int s5k4aa_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 s5k4aa_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-	u8 data = S5K4AA_PAGE_MAP_2;
-	int err;
-
-	sensor_settings[HFLIP_IDX] = val;
-
-	PDEBUG(D_V4L2, "Set horizontal flip to %d", val);
-	err = m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
-	if (err < 0)
-		return err;
-
-	err = m5602_read_sensor(sd, S5K4AA_READ_MODE, &data, 1);
-	if (err < 0)
-		return err;
-
-	if (dmi_check_system(s5k4aa_vflip_dmi_table))
-		val = !val;
-
-	data = ((data & ~S5K4AA_RM_H_FLIP) | ((val & 0x01) << 6));
-	err = m5602_write_sensor(sd, S5K4AA_READ_MODE, &data, 1);
+	err = m5602_write_sensor(sd, S5K4AA_COLSTART_LO, &data, 1);
 	if (err < 0)
 		return err;
 
-	err = m5602_read_sensor(sd, S5K4AA_COLSTART_LO, &data, 1);
+	err = m5602_read_sensor(sd, S5K4AA_ROWSTART_LO, &data, 1);
 	if (err < 0)
 		return err;
-	if (val)
+	if (vflip)
 		data &= 0xfe;
 	else
 		data |= 0x01;
-	err = m5602_write_sensor(sd, S5K4AA_COLSTART_LO, &data, 1);
-	return err;
-}
-
-static int s5k4aa_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
+	err = m5602_write_sensor(sd, S5K4AA_ROWSTART_LO, &data, 1);
+	if (err < 0)
+		return err;
 
-	*val = sensor_settings[GAIN_IDX];
-	PDEBUG(D_V4L2, "Read gain %d", *val);
 	return 0;
 }
 
 static int s5k4aa_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 data = S5K4AA_PAGE_MAP_2;
 	int err;
 
-	sensor_settings[GAIN_IDX] = val;
-
-	PDEBUG(D_V4L2, "Set gain to %d", val);
+	PDEBUG(D_CONF, "Set gain to %d", val);
 	err = m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
 	if (err < 0)
 		return err;
@@ -625,26 +452,13 @@ static int s5k4aa_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 	return err;
 }
 
-static int s5k4aa_get_brightness(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[BRIGHTNESS_IDX];
-	PDEBUG(D_V4L2, "Read brightness %d", *val);
-	return 0;
-}
-
 static int s5k4aa_set_brightness(struct gspca_dev *gspca_dev, __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 data = S5K4AA_PAGE_MAP_2;
 	int err;
 
-	sensor_settings[BRIGHTNESS_IDX] = val;
-
-	PDEBUG(D_V4L2, "Set brightness to %d", val);
+	PDEBUG(D_CONF, "Set brightness to %d", val);
 	err = m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
 	if (err < 0)
 		return err;
@@ -653,26 +467,13 @@ static int s5k4aa_set_brightness(struct gspca_dev *gspca_dev, __s32 val)
 	return m5602_write_sensor(sd, S5K4AA_BRIGHTNESS, &data, 1);
 }
 
-static int s5k4aa_get_noise(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[NOISE_SUPP_IDX];
-	PDEBUG(D_V4L2, "Read noise %d", *val);
-	return 0;
-}
-
 static int s5k4aa_set_noise(struct gspca_dev *gspca_dev, __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 data = S5K4AA_PAGE_MAP_2;
 	int err;
 
-	sensor_settings[NOISE_SUPP_IDX] = val;
-
-	PDEBUG(D_V4L2, "Set noise to %d", val);
+	PDEBUG(D_CONF, "Set noise to %d", val);
 	err = m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
 	if (err < 0)
 		return err;
@@ -681,10 +482,41 @@ static int s5k4aa_set_noise(struct gspca_dev *gspca_dev, __s32 val)
 	return m5602_write_sensor(sd, S5K4AA_NOISE_SUPP, &data, 1);
 }
 
+static int s5k4aa_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	int err;
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		err = s5k4aa_set_brightness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		err = s5k4aa_set_exposure(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_GAIN:
+		err = s5k4aa_set_gain(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_SHARPNESS:
+		err = s5k4aa_set_noise(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		err = s5k4aa_set_hvflip(gspca_dev);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return err;
+}
+
 void s5k4aa_disconnect(struct sd *sd)
 {
 	sd->sensor = NULL;
-	kfree(sd->sensor_priv);
 }
 
 static void s5k4aa_dump_registers(struct sd *sd)
diff --git a/drivers/media/usb/gspca/m5602/m5602_s5k4aa.h b/drivers/media/usb/gspca/m5602/m5602_s5k4aa.h
index 8e0035e731c..9953e976695 100644
--- a/drivers/media/usb/gspca/m5602/m5602_s5k4aa.h
+++ b/drivers/media/usb/gspca/m5602/m5602_s5k4aa.h
@@ -69,6 +69,7 @@ extern bool dump_sensor;
 
 int s5k4aa_probe(struct sd *sd);
 int s5k4aa_init(struct sd *sd);
+int s5k4aa_init_controls(struct sd *sd);
 int s5k4aa_start(struct sd *sd);
 void s5k4aa_disconnect(struct sd *sd);
 
@@ -79,6 +80,7 @@ static const struct m5602_sensor s5k4aa = {
 
 	.probe = s5k4aa_probe,
 	.init = s5k4aa_init,
+	.init_controls = s5k4aa_init_controls,
 	.start = s5k4aa_start,
 	.disconnect = s5k4aa_disconnect,
 };
diff --git a/drivers/media/usb/gspca/m5602/m5602_s5k83a.c b/drivers/media/usb/gspca/m5602/m5602_s5k83a.c
index 1de743a02b0..7cbc3a00bda 100644
--- a/drivers/media/usb/gspca/m5602/m5602_s5k83a.c
+++ b/drivers/media/usb/gspca/m5602/m5602_s5k83a.c
@@ -21,16 +21,11 @@
 #include <linux/kthread.h>
 #include "m5602_s5k83a.h"
 
-static int s5k83a_set_gain(struct gspca_dev *gspca_dev, __s32 val);
-static int s5k83a_get_gain(struct gspca_dev *gspca_dev, __s32 *val);
-static int s5k83a_set_brightness(struct gspca_dev *gspca_dev, __s32 val);
-static int s5k83a_get_brightness(struct gspca_dev *gspca_dev, __s32 *val);
-static int s5k83a_set_exposure(struct gspca_dev *gspca_dev, __s32 val);
-static int s5k83a_get_exposure(struct gspca_dev *gspca_dev, __s32 *val);
-static int s5k83a_get_vflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int s5k83a_set_vflip(struct gspca_dev *gspca_dev, __s32 val);
-static int s5k83a_get_hflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int s5k83a_set_hflip(struct gspca_dev *gspca_dev, __s32 val);
+static int s5k83a_s_ctrl(struct v4l2_ctrl *ctrl);
+
+static const struct v4l2_ctrl_ops s5k83a_ctrl_ops = {
+	.s_ctrl = s5k83a_s_ctrl,
+};
 
 static struct v4l2_pix_format s5k83a_modes[] = {
 	{
@@ -46,83 +41,6 @@ static struct v4l2_pix_format s5k83a_modes[] = {
 	}
 };
 
-static const struct ctrl s5k83a_ctrls[] = {
-#define GAIN_IDX 0
-	{
-		{
-			.id = V4L2_CID_GAIN,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-			.name = "gain",
-			.minimum = 0x00,
-			.maximum = 0xff,
-			.step = 0x01,
-			.default_value = S5K83A_DEFAULT_GAIN,
-			.flags = V4L2_CTRL_FLAG_SLIDER
-		},
-			.set = s5k83a_set_gain,
-			.get = s5k83a_get_gain
-
-	},
-#define BRIGHTNESS_IDX 1
-	{
-		{
-			.id = V4L2_CID_BRIGHTNESS,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-			.name = "brightness",
-			.minimum = 0x00,
-			.maximum = 0xff,
-			.step = 0x01,
-			.default_value = S5K83A_DEFAULT_BRIGHTNESS,
-			.flags = V4L2_CTRL_FLAG_SLIDER
-		},
-			.set = s5k83a_set_brightness,
-			.get = s5k83a_get_brightness,
-	},
-#define EXPOSURE_IDX 2
-	{
-		{
-			.id = V4L2_CID_EXPOSURE,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-			.name = "exposure",
-			.minimum = 0x00,
-			.maximum = S5K83A_MAXIMUM_EXPOSURE,
-			.step = 0x01,
-			.default_value = S5K83A_DEFAULT_EXPOSURE,
-			.flags = V4L2_CTRL_FLAG_SLIDER
-		},
-			.set = s5k83a_set_exposure,
-			.get = s5k83a_get_exposure
-	},
-#define HFLIP_IDX 3
-	{
-		{
-			.id = V4L2_CID_HFLIP,
-			.type = V4L2_CTRL_TYPE_BOOLEAN,
-			.name = "horizontal flip",
-			.minimum = 0,
-			.maximum = 1,
-			.step = 1,
-			.default_value = 0
-		},
-			.set = s5k83a_set_hflip,
-			.get = s5k83a_get_hflip
-	},
-#define VFLIP_IDX 4
-	{
-		{
-			.id = V4L2_CID_VFLIP,
-			.type = V4L2_CTRL_TYPE_BOOLEAN,
-			.name = "vertical flip",
-			.minimum = 0,
-			.maximum = 1,
-			.step = 1,
-			.default_value = 0
-		},
-		.set = s5k83a_set_vflip,
-		.get = s5k83a_get_vflip
-	}
-};
-
 static void s5k83a_dump_registers(struct sd *sd);
 static int s5k83a_get_rotation(struct sd *sd, u8 *reg_data);
 static int s5k83a_set_led_indication(struct sd *sd, u8 val);
@@ -131,9 +49,9 @@ static int s5k83a_set_flip_real(struct gspca_dev *gspca_dev,
 
 int s5k83a_probe(struct sd *sd)
 {
-	struct s5k83a_priv *sens_priv;
 	u8 prod_id = 0, ver_id = 0;
 	int i, err = 0;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 
 	if (force_sensor) {
 		if (force_sensor == S5K83A_SENSOR) {
@@ -173,38 +91,18 @@ int s5k83a_probe(struct sd *sd)
 		pr_info("Detected a s5k83a sensor\n");
 
 sensor_found:
-	sens_priv = kmalloc(
-		sizeof(struct s5k83a_priv), GFP_KERNEL);
-	if (!sens_priv)
-		return -ENOMEM;
-
-	sens_priv->settings =
-	kmalloc(sizeof(s32)*ARRAY_SIZE(s5k83a_ctrls), GFP_KERNEL);
-	if (!sens_priv->settings) {
-		kfree(sens_priv);
-		return -ENOMEM;
-	}
-
 	sd->gspca_dev.cam.cam_mode = s5k83a_modes;
 	sd->gspca_dev.cam.nmodes = ARRAY_SIZE(s5k83a_modes);
-	sd->desc->ctrls = s5k83a_ctrls;
-	sd->desc->nctrls = ARRAY_SIZE(s5k83a_ctrls);
 
 	/* null the pointer! thread is't running now */
-	sens_priv->rotation_thread = NULL;
-
-	for (i = 0; i < ARRAY_SIZE(s5k83a_ctrls); i++)
-		sens_priv->settings[i] = s5k83a_ctrls[i].qctrl.default_value;
+	sd->rotation_thread = NULL;
 
-	sd->sensor_priv = sens_priv;
 	return 0;
 }
 
 int s5k83a_init(struct sd *sd)
 {
 	int i, err = 0;
-	s32 *sensor_settings =
-			((struct s5k83a_priv *) sd->sensor_priv)->settings;
 
 	for (i = 0; i < ARRAY_SIZE(init_s5k83a) && !err; i++) {
 		u8 data[2] = {0x00, 0x00};
@@ -237,33 +135,44 @@ int s5k83a_init(struct sd *sd)
 	if (dump_sensor)
 		s5k83a_dump_registers(sd);
 
-	err = s5k83a_set_gain(&sd->gspca_dev, sensor_settings[GAIN_IDX]);
-	if (err < 0)
-		return err;
+	return err;
+}
 
-	err = s5k83a_set_brightness(&sd->gspca_dev,
-				     sensor_settings[BRIGHTNESS_IDX]);
-	if (err < 0)
-		return err;
+int s5k83a_init_controls(struct sd *sd)
+{
+	struct v4l2_ctrl_handler *hdl = &sd->gspca_dev.ctrl_handler;
 
-	err = s5k83a_set_exposure(&sd->gspca_dev,
-				   sensor_settings[EXPOSURE_IDX]);
-	if (err < 0)
-		return err;
+	sd->gspca_dev.vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 6);
 
-	err = s5k83a_set_hflip(&sd->gspca_dev, sensor_settings[HFLIP_IDX]);
-	if (err < 0)
-		return err;
+	v4l2_ctrl_new_std(hdl, &s5k83a_ctrl_ops, V4L2_CID_BRIGHTNESS,
+			  0, 255, 1, S5K83A_DEFAULT_BRIGHTNESS);
 
-	err = s5k83a_set_vflip(&sd->gspca_dev, sensor_settings[VFLIP_IDX]);
+	v4l2_ctrl_new_std(hdl, &s5k83a_ctrl_ops, V4L2_CID_EXPOSURE,
+			  0, S5K83A_MAXIMUM_EXPOSURE, 1,
+			  S5K83A_DEFAULT_EXPOSURE);
 
-	return err;
+	v4l2_ctrl_new_std(hdl, &s5k83a_ctrl_ops, V4L2_CID_GAIN,
+			  0, 255, 1, S5K83A_DEFAULT_GAIN);
+
+	sd->hflip = v4l2_ctrl_new_std(hdl, &s5k83a_ctrl_ops, V4L2_CID_HFLIP,
+				      0, 1, 1, 0);
+	sd->vflip = v4l2_ctrl_new_std(hdl, &s5k83a_ctrl_ops, V4L2_CID_VFLIP,
+				      0, 1, 1, 0);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+
+	v4l2_ctrl_cluster(2, &sd->hflip);
+
+	return 0;
 }
 
 static int rotation_thread_function(void *data)
 {
 	struct sd *sd = (struct sd *) data;
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
 	u8 reg, previous_rotation = 0;
 	__s32 vflip, hflip;
 
@@ -277,8 +186,8 @@ static int rotation_thread_function(void *data)
 			previous_rotation = reg;
 			pr_info("Camera was flipped\n");
 
-			s5k83a_get_vflip((struct gspca_dev *) sd, &vflip);
-			s5k83a_get_hflip((struct gspca_dev *) sd, &hflip);
+			hflip = sd->hflip->val;
+			vflip = sd->vflip->val;
 
 			if (reg) {
 				vflip = !vflip;
@@ -294,26 +203,25 @@ static int rotation_thread_function(void *data)
 
 	/* return to "front" flip */
 	if (previous_rotation) {
-		s5k83a_get_vflip((struct gspca_dev *) sd, &vflip);
-		s5k83a_get_hflip((struct gspca_dev *) sd, &hflip);
+		hflip = sd->hflip->val;
+		vflip = sd->vflip->val;
 		s5k83a_set_flip_real((struct gspca_dev *) sd, vflip, hflip);
 	}
 
-	sens_priv->rotation_thread = NULL;
+	sd->rotation_thread = NULL;
 	return 0;
 }
 
 int s5k83a_start(struct sd *sd)
 {
 	int i, err = 0;
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
 
 	/* Create another thread, polling the GPIO ports of the camera to check
 	   if it got rotated. This is how the windows driver does it so we have
 	   to assume that there is no better way of accomplishing this */
-	sens_priv->rotation_thread = kthread_create(rotation_thread_function,
-						    sd, "rotation thread");
-	wake_up_process(sens_priv->rotation_thread);
+	sd->rotation_thread = kthread_create(rotation_thread_function,
+					     sd, "rotation thread");
+	wake_up_process(sd->rotation_thread);
 
 	/* Preinit the sensor */
 	for (i = 0; i < ARRAY_SIZE(start_s5k83a) && !err; i++) {
@@ -333,32 +241,17 @@ int s5k83a_start(struct sd *sd)
 
 int s5k83a_stop(struct sd *sd)
 {
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
-
-	if (sens_priv->rotation_thread)
-		kthread_stop(sens_priv->rotation_thread);
+	if (sd->rotation_thread)
+		kthread_stop(sd->rotation_thread);
 
 	return s5k83a_set_led_indication(sd, 0);
 }
 
 void s5k83a_disconnect(struct sd *sd)
 {
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
-
 	s5k83a_stop(sd);
 
 	sd->sensor = NULL;
-	kfree(sens_priv->settings);
-	kfree(sens_priv);
-}
-
-static int s5k83a_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
-
-	*val = sens_priv->settings[GAIN_IDX];
-	return 0;
 }
 
 static int s5k83a_set_gain(struct gspca_dev *gspca_dev, __s32 val)
@@ -366,9 +259,6 @@ static int s5k83a_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 	int err;
 	u8 data[2];
 	struct sd *sd = (struct sd *) gspca_dev;
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
-
-	sens_priv->settings[GAIN_IDX] = val;
 
 	data[0] = 0x00;
 	data[1] = 0x20;
@@ -391,60 +281,29 @@ static int s5k83a_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 	return err;
 }
 
-static int s5k83a_get_brightness(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
-
-	*val = sens_priv->settings[BRIGHTNESS_IDX];
-	return 0;
-}
-
 static int s5k83a_set_brightness(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	u8 data[1];
 	struct sd *sd = (struct sd *) gspca_dev;
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
 
-	sens_priv->settings[BRIGHTNESS_IDX] = val;
 	data[0] = val;
 	err = m5602_write_sensor(sd, S5K83A_BRIGHTNESS, data, 1);
 	return err;
 }
 
-static int s5k83a_get_exposure(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
-
-	*val = sens_priv->settings[EXPOSURE_IDX];
-	return 0;
-}
-
 static int s5k83a_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	u8 data[2];
 	struct sd *sd = (struct sd *) gspca_dev;
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
 
-	sens_priv->settings[EXPOSURE_IDX] = val;
 	data[0] = 0;
 	data[1] = val;
 	err = m5602_write_sensor(sd, S5K83A_EXPOSURE, data, 2);
 	return err;
 }
 
-static int s5k83a_get_vflip(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
-
-	*val = sens_priv->settings[VFLIP_IDX];
-	return 0;
-}
-
 static int s5k83a_set_flip_real(struct gspca_dev *gspca_dev,
 				__s32 vflip, __s32 hflip)
 {
@@ -476,60 +335,52 @@ static int s5k83a_set_flip_real(struct gspca_dev *gspca_dev,
 	return err;
 }
 
-static int s5k83a_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
+static int s5k83a_set_hvflip(struct gspca_dev *gspca_dev)
 {
 	int err;
 	u8 reg;
-	__s32 hflip;
 	struct sd *sd = (struct sd *) gspca_dev;
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
-
-	sens_priv->settings[VFLIP_IDX] = val;
-
-	s5k83a_get_hflip(gspca_dev, &hflip);
+	int hflip = sd->hflip->val;
+	int vflip = sd->vflip->val;
 
 	err = s5k83a_get_rotation(sd, &reg);
 	if (err < 0)
 		return err;
 	if (reg) {
-		val = !val;
 		hflip = !hflip;
+		vflip = !vflip;
 	}
 
-	err = s5k83a_set_flip_real(gspca_dev, val, hflip);
+	err = s5k83a_set_flip_real(gspca_dev, vflip, hflip);
 	return err;
 }
 
-static int s5k83a_get_hflip(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
-
-	*val = sens_priv->settings[HFLIP_IDX];
-	return 0;
-}
-
-static int s5k83a_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
+static int s5k83a_s_ctrl(struct v4l2_ctrl *ctrl)
 {
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
 	int err;
-	u8 reg;
-	__s32 vflip;
-	struct sd *sd = (struct sd *) gspca_dev;
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
-
-	sens_priv->settings[HFLIP_IDX] = val;
 
-	s5k83a_get_vflip(gspca_dev, &vflip);
-
-	err = s5k83a_get_rotation(sd, &reg);
-	if (err < 0)
-		return err;
-	if (reg) {
-		val = !val;
-		vflip = !vflip;
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		err = s5k83a_set_brightness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		err = s5k83a_set_exposure(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_GAIN:
+		err = s5k83a_set_gain(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		err = s5k83a_set_hvflip(gspca_dev);
+		break;
+	default:
+		return -EINVAL;
 	}
 
-	err = s5k83a_set_flip_real(gspca_dev, vflip, val);
 	return err;
 }
 
diff --git a/drivers/media/usb/gspca/m5602/m5602_s5k83a.h b/drivers/media/usb/gspca/m5602/m5602_s5k83a.h
index 79952247b53..d61b918228d 100644
--- a/drivers/media/usb/gspca/m5602/m5602_s5k83a.h
+++ b/drivers/media/usb/gspca/m5602/m5602_s5k83a.h
@@ -45,6 +45,7 @@ extern bool dump_sensor;
 
 int s5k83a_probe(struct sd *sd);
 int s5k83a_init(struct sd *sd);
+int s5k83a_init_controls(struct sd *sd);
 int s5k83a_start(struct sd *sd);
 int s5k83a_stop(struct sd *sd);
 void s5k83a_disconnect(struct sd *sd);
@@ -53,6 +54,7 @@ static const struct m5602_sensor s5k83a = {
 	.name = "S5K83A",
 	.probe = s5k83a_probe,
 	.init = s5k83a_init,
+	.init_controls = s5k83a_init_controls,
 	.start = s5k83a_start,
 	.stop = s5k83a_stop,
 	.disconnect = s5k83a_disconnect,
@@ -60,13 +62,6 @@ static const struct m5602_sensor s5k83a = {
 	.i2c_regW = 2,
 };
 
-struct s5k83a_priv {
-	/* We use another thread periodically
-	   probing the orientation of the camera */
-	struct task_struct *rotation_thread;
-	s32 *settings;
-};
-
 static const unsigned char preinit_s5k83a[][4] = {
 	{BRIDGE, M5602_XB_MCU_CLK_DIV, 0x02, 0x00},
 	{BRIDGE, M5602_XB_MCU_CLK_CTRL, 0xb0, 0x00},
diff --git a/drivers/media/usb/gspca/m5602/m5602_sensor.h b/drivers/media/usb/gspca/m5602/m5602_sensor.h
index edff4f1f586..48341b4d607 100644
--- a/drivers/media/usb/gspca/m5602/m5602_sensor.h
+++ b/drivers/media/usb/gspca/m5602/m5602_sensor.h
@@ -57,6 +57,9 @@ struct m5602_sensor {
 	/* Performs a initialization sequence */
 	int (*init)(struct sd *sd);
 
+	/* Controls initialization, maybe NULL */
+	int (*init_controls)(struct sd *sd);
+
 	/* Executed when the camera starts to send data */
 	int (*start)(struct sd *sd);
 
diff --git a/drivers/media/usb/gspca/mr97310a.c b/drivers/media/usb/gspca/mr97310a.c
index 8f4714df599..68bb2f35966 100644
--- a/drivers/media/usb/gspca/mr97310a.c
+++ b/drivers/media/usb/gspca/mr97310a.c
@@ -289,7 +289,7 @@ static int zero_the_pointer(struct gspca_dev *gspca_dev)
 			return err_code;
 	}
 	if (status != 0x0a)
-		PDEBUG(D_ERR, "status is %02x", status);
+		PERR("status is %02x", status);
 
 	tries = 0;
 	while (tries < 4) {
@@ -330,7 +330,7 @@ static void stream_stop(struct gspca_dev *gspca_dev)
 	gspca_dev->usb_buf[0] = 0x01;
 	gspca_dev->usb_buf[1] = 0x00;
 	if (mr_write(gspca_dev, 2) < 0)
-		PDEBUG(D_ERR, "Stream Stop failed");
+		PERR("Stream Stop failed");
 }
 
 static void lcd_stop(struct gspca_dev *gspca_dev)
@@ -338,7 +338,7 @@ static void lcd_stop(struct gspca_dev *gspca_dev)
 	gspca_dev->usb_buf[0] = 0x19;
 	gspca_dev->usb_buf[1] = 0x54;
 	if (mr_write(gspca_dev, 2) < 0)
-		PDEBUG(D_ERR, "LCD Stop failed");
+		PERR("LCD Stop failed");
 }
 
 static int isoc_enable(struct gspca_dev *gspca_dev)
@@ -1026,7 +1026,7 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	struct sd *sd = (struct sd *) gspca_dev;
 	unsigned char *sof;
 
-	sof = pac_find_sof(&sd->sof_read, data, len);
+	sof = pac_find_sof(gspca_dev, &sd->sof_read, data, len);
 	if (sof) {
 		int n;
 
diff --git a/drivers/media/usb/gspca/ov519.c b/drivers/media/usb/gspca/ov519.c
index 9ad19a7ef81..a3958ee8681 100644
--- a/drivers/media/usb/gspca/ov519.c
+++ b/drivers/media/usb/gspca/ov519.c
@@ -2034,6 +2034,7 @@ static unsigned char ov7670_abs_to_sm(unsigned char v)
 /* Write a OV519 register */
 static void reg_w(struct sd *sd, u16 index, u16 value)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int ret, req = 0;
 
 	if (sd->gspca_dev.usb_err < 0)
@@ -2071,7 +2072,7 @@ static void reg_w(struct sd *sd, u16 index, u16 value)
 			sd->gspca_dev.usb_buf, 1, 500);
 leave:
 	if (ret < 0) {
-		pr_err("reg_w %02x failed %d\n", index, ret);
+		PERR("reg_w %02x failed %d\n", index, ret);
 		sd->gspca_dev.usb_err = ret;
 		return;
 	}
@@ -2081,6 +2082,7 @@ leave:
 /* returns: negative is error, pos or zero is data */
 static int reg_r(struct sd *sd, u16 index)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int ret;
 	int req;
 
@@ -2110,7 +2112,7 @@ static int reg_r(struct sd *sd, u16 index)
 		PDEBUG(D_USBI, "GET %02x 0000 %04x %02x",
 			req, index, ret);
 	} else {
-		pr_err("reg_r %02x failed %d\n", index, ret);
+		PERR("reg_r %02x failed %d\n", index, ret);
 		sd->gspca_dev.usb_err = ret;
 	}
 
@@ -2121,6 +2123,7 @@ static int reg_r(struct sd *sd, u16 index)
 static int reg_r8(struct sd *sd,
 		  u16 index)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int ret;
 
 	if (sd->gspca_dev.usb_err < 0)
@@ -2135,7 +2138,7 @@ static int reg_r8(struct sd *sd,
 	if (ret >= 0) {
 		ret = sd->gspca_dev.usb_buf[0];
 	} else {
-		pr_err("reg_r8 %02x failed %d\n", index, ret);
+		PERR("reg_r8 %02x failed %d\n", index, ret);
 		sd->gspca_dev.usb_err = ret;
 	}
 
@@ -2174,6 +2177,7 @@ static void reg_w_mask(struct sd *sd,
  */
 static void ov518_reg_w32(struct sd *sd, u16 index, u32 value, int n)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int ret;
 
 	if (sd->gspca_dev.usb_err < 0)
@@ -2188,13 +2192,14 @@ static void ov518_reg_w32(struct sd *sd, u16 index, u32 value, int n)
 			0, index,
 			sd->gspca_dev.usb_buf, n, 500);
 	if (ret < 0) {
-		pr_err("reg_w32 %02x failed %d\n", index, ret);
+		PERR("reg_w32 %02x failed %d\n", index, ret);
 		sd->gspca_dev.usb_err = ret;
 	}
 }
 
 static void ov511_i2c_w(struct sd *sd, u8 reg, u8 value)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int rc, retries;
 
 	PDEBUG(D_USBO, "ov511_i2c_w %02x %02x", reg, value);
@@ -2228,6 +2233,7 @@ static void ov511_i2c_w(struct sd *sd, u8 reg, u8 value)
 
 static int ov511_i2c_r(struct sd *sd, u8 reg)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int rc, value, retries;
 
 	/* Two byte write cycle */
@@ -2300,6 +2306,8 @@ static void ov518_i2c_w(struct sd *sd,
 		u8 reg,
 		u8 value)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
+
 	PDEBUG(D_USBO, "ov518_i2c_w %02x %02x", reg, value);
 
 	/* Select camera register */
@@ -2325,6 +2333,7 @@ static void ov518_i2c_w(struct sd *sd,
  */
 static int ov518_i2c_r(struct sd *sd, u8 reg)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int value;
 
 	/* Select camera register */
@@ -2345,6 +2354,7 @@ static int ov518_i2c_r(struct sd *sd, u8 reg)
 
 static void ovfx2_i2c_w(struct sd *sd, u8 reg, u8 value)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int ret;
 
 	if (sd->gspca_dev.usb_err < 0)
@@ -2357,7 +2367,7 @@ static void ovfx2_i2c_w(struct sd *sd, u8 reg, u8 value)
 			(u16) value, (u16) reg, NULL, 0, 500);
 
 	if (ret < 0) {
-		pr_err("ovfx2_i2c_w %02x failed %d\n", reg, ret);
+		PERR("ovfx2_i2c_w %02x failed %d\n", reg, ret);
 		sd->gspca_dev.usb_err = ret;
 	}
 
@@ -2366,6 +2376,7 @@ static void ovfx2_i2c_w(struct sd *sd, u8 reg, u8 value)
 
 static int ovfx2_i2c_r(struct sd *sd, u8 reg)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int ret;
 
 	if (sd->gspca_dev.usb_err < 0)
@@ -2381,7 +2392,7 @@ static int ovfx2_i2c_r(struct sd *sd, u8 reg)
 		ret = sd->gspca_dev.usb_buf[0];
 		PDEBUG(D_USBI, "ovfx2_i2c_r %02x %02x", reg, ret);
 	} else {
-		pr_err("ovfx2_i2c_r %02x failed %d\n", reg, ret);
+		PERR("ovfx2_i2c_r %02x failed %d\n", reg, ret);
 		sd->gspca_dev.usb_err = ret;
 	}
 
@@ -2478,6 +2489,8 @@ static void i2c_w_mask(struct sd *sd,
  * registers while the camera is streaming */
 static inline void ov51x_stop(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
+
 	PDEBUG(D_STREAM, "stopping");
 	sd->stopped = 1;
 	switch (sd->bridge) {
@@ -2507,6 +2520,8 @@ static inline void ov51x_stop(struct sd *sd)
  * actually stopped (for performance). */
 static inline void ov51x_restart(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
+
 	PDEBUG(D_STREAM, "restarting");
 	if (!sd->stopped)
 		return;
@@ -2545,6 +2560,7 @@ static void ov51x_set_slave_ids(struct sd *sd, u8 slave);
 static int init_ov_sensor(struct sd *sd, u8 slave)
 {
 	int i;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 
 	ov51x_set_slave_ids(sd, slave);
 
@@ -2624,10 +2640,11 @@ static void write_i2c_regvals(struct sd *sd,
 /* This initializes the OV2x10 / OV3610 / OV3620 / OV9600 */
 static void ov_hires_configure(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int high, low;
 
 	if (sd->bridge != BRIDGE_OVFX2) {
-		pr_err("error hires sensors only supported with ovfx2\n");
+		PERR("error hires sensors only supported with ovfx2\n");
 		return;
 	}
 
@@ -2662,7 +2679,7 @@ static void ov_hires_configure(struct sd *sd)
 		}
 		break;
 	}
-	pr_err("Error unknown sensor type: %02x%02x\n", high, low);
+	PERR("Error unknown sensor type: %02x%02x\n", high, low);
 }
 
 /* This initializes the OV8110, OV8610 sensor. The OV8110 uses
@@ -2670,6 +2687,7 @@ static void ov_hires_configure(struct sd *sd)
  */
 static void ov8xx0_configure(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int rc;
 
 	PDEBUG(D_PROBE, "starting ov8xx0 configuration");
@@ -2677,13 +2695,13 @@ static void ov8xx0_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");
+		PERR("Error detecting sensor type");
 		return;
 	}
 	if ((rc & 3) == 1)
 		sd->sensor = SEN_OV8610;
 	else
-		pr_err("Unknown image sensor version: %d\n", rc & 3);
+		PERR("Unknown image sensor version: %d\n", rc & 3);
 }
 
 /* This initializes the OV7610, OV7620, or OV76BE sensor. The OV76BE uses
@@ -2691,6 +2709,7 @@ static void ov8xx0_configure(struct sd *sd)
  */
 static void ov7xx0_configure(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int rc, high, low;
 
 	PDEBUG(D_PROBE, "starting OV7xx0 configuration");
@@ -2701,7 +2720,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) {
-		pr_err("Error detecting sensor type\n");
+		PERR("Error detecting sensor type\n");
 		return;
 	}
 	if ((rc & 3) == 3) {
@@ -2729,19 +2748,19 @@ static void ov7xx0_configure(struct sd *sd)
 		/* try to read product id registers */
 		high = i2c_r(sd, 0x0a);
 		if (high < 0) {
-			pr_err("Error detecting camera chip PID\n");
+			PERR("Error detecting camera chip PID\n");
 			return;
 		}
 		low = i2c_r(sd, 0x0b);
 		if (low < 0) {
-			pr_err("Error detecting camera chip VER\n");
+			PERR("Error detecting camera chip VER\n");
 			return;
 		}
 		if (high == 0x76) {
 			switch (low) {
 			case 0x30:
-				pr_err("Sensor is an OV7630/OV7635\n");
-				pr_err("7630 is not supported by this driver\n");
+				PERR("Sensor is an OV7630/OV7635\n");
+				PERR("7630 is not supported by this driver\n");
 				return;
 			case 0x40:
 				PDEBUG(D_PROBE, "Sensor is an OV7645");
@@ -2760,7 +2779,7 @@ static void ov7xx0_configure(struct sd *sd)
 				sd->sensor = SEN_OV7660;
 				break;
 			default:
-				pr_err("Unknown sensor: 0x76%02x\n", low);
+				PERR("Unknown sensor: 0x76%02x\n", low);
 				return;
 			}
 		} else {
@@ -2768,20 +2787,22 @@ static void ov7xx0_configure(struct sd *sd)
 			sd->sensor = SEN_OV7620;
 		}
 	} else {
-		pr_err("Unknown image sensor version: %d\n", rc & 3);
+		PERR("Unknown image sensor version: %d\n", rc & 3);
 	}
 }
 
 /* This initializes the OV6620, OV6630, OV6630AE, or OV6630AF sensor. */
 static void ov6xx0_configure(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int rc;
+
 	PDEBUG(D_PROBE, "starting OV6xx0 configuration");
 
 	/* Detect sensor (sub)type */
 	rc = i2c_r(sd, OV7610_REG_COM_I);
 	if (rc < 0) {
-		pr_err("Error detecting sensor type\n");
+		PERR("Error detecting sensor type\n");
 		return;
 	}
 
@@ -2810,7 +2831,7 @@ static void ov6xx0_configure(struct sd *sd)
 		pr_warn("WARNING: Sensor is an OV66307. Your camera may have been misdetected in previous driver versions.\n");
 		break;
 	default:
-		pr_err("FATAL: Unknown sensor version: 0x%02x\n", rc);
+		PERR("FATAL: Unknown sensor version: 0x%02x\n", rc);
 		return;
 	}
 
@@ -2907,6 +2928,7 @@ static void ov51x_upload_quan_tables(struct sd *sd)
 		7, 7, 7, 7, 7, 7, 8, 8
 	};
 
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	const unsigned char *pYTable, *pUVTable;
 	unsigned char val0, val1;
 	int i, size, reg = R51x_COMP_LUT_BEGIN;
@@ -3300,7 +3322,7 @@ static int sd_init(struct gspca_dev *gspca_dev)
 	} else if (init_ov_sensor(sd, OV_HIRES_SID) >= 0) {
 		ov_hires_configure(sd);
 	} else {
-		pr_err("Can't determine sensor slave IDs\n");
+		PERR("Can't determine sensor slave IDs\n");
 		goto error;
 	}
 
@@ -3433,7 +3455,7 @@ static int sd_init(struct gspca_dev *gspca_dev)
 	}
 	return gspca_dev->usb_err;
 error:
-	PDEBUG(D_ERR, "OV519 Config failed");
+	PERR("OV519 Config failed");
 	return -EINVAL;
 }
 
@@ -3459,6 +3481,7 @@ static int sd_isoc_init(struct gspca_dev *gspca_dev)
  */
 static void ov511_mode_init_regs(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int hsegs, vsegs, packet_size, fps, needed;
 	int interlaced = 0;
 	struct usb_host_interface *alt;
@@ -3467,7 +3490,7 @@ static void ov511_mode_init_regs(struct sd *sd)
 	intf = usb_ifnum_to_if(sd->gspca_dev.dev, sd->gspca_dev.iface);
 	alt = usb_altnum_to_altsetting(intf, sd->gspca_dev.alt);
 	if (!alt) {
-		pr_err("Couldn't get altsetting\n");
+		PERR("Couldn't get altsetting\n");
 		sd->gspca_dev.usb_err = -EIO;
 		return;
 	}
@@ -3583,6 +3606,7 @@ static void ov511_mode_init_regs(struct sd *sd)
  */
 static void ov518_mode_init_regs(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int hsegs, vsegs, packet_size;
 	struct usb_host_interface *alt;
 	struct usb_interface *intf;
@@ -3590,7 +3614,7 @@ static void ov518_mode_init_regs(struct sd *sd)
 	intf = usb_ifnum_to_if(sd->gspca_dev.dev, sd->gspca_dev.iface);
 	alt = usb_altnum_to_altsetting(intf, sd->gspca_dev.alt);
 	if (!alt) {
-		pr_err("Couldn't get altsetting\n");
+		PERR("Couldn't get altsetting\n");
 		sd->gspca_dev.usb_err = -EIO;
 		return;
 	}
@@ -3750,6 +3774,8 @@ static void ov519_mode_init_regs(struct sd *sd)
 		/* windows reads 0x55 at this point, why? */
 	};
 
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
+
 	/******** Set the mode ********/
 	switch (sd->sensor) {
 	default:
@@ -3865,11 +3891,10 @@ static void ov519_mode_init_regs(struct sd *sd)
 
 static void mode_init_ov_sensor_regs(struct sd *sd)
 {
-	struct gspca_dev *gspca_dev;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int qvga, xstart, xend, ystart, yend;
 	u8 v;
 
-	gspca_dev = &sd->gspca_dev;
 	qvga = gspca_dev->cam.cam_mode[gspca_dev->curr_mode].priv & 1;
 
 	/******** Mode (VGA/QVGA) and sensor specific regs ********/
@@ -4304,7 +4329,7 @@ static void ov511_pkt_scan(struct gspca_dev *gspca_dev,
 			/* Frame end */
 			if ((in[9] + 1) * 8 != gspca_dev->width ||
 			    (in[10] + 1) * 8 != gspca_dev->height) {
-				PDEBUG(D_ERR, "Invalid frame size, got: %dx%d,"
+				PERR("Invalid frame size, got: %dx%d,"
 					" requested: %dx%d\n",
 					(in[9] + 1) * 8, (in[10] + 1) * 8,
 					gspca_dev->width, gspca_dev->height);
@@ -4355,7 +4380,7 @@ static void ov518_pkt_scan(struct gspca_dev *gspca_dev,
 		   except that they may contain part of the footer), are
 		   numbered 0 */
 		else if (sd->packet_nr == 0 || data[len]) {
-			PDEBUG(D_ERR, "Invalid packet nr: %d (expect: %d)",
+			PERR("Invalid packet nr: %d (expect: %d)",
 				(int)data[len], (int)sd->packet_nr);
 			gspca_dev->last_packet_type = DISCARD_PACKET;
 			return;
@@ -4898,7 +4923,7 @@ static int sd_init_controls(struct gspca_dev *gspca_dev)
 			QUALITY_MIN, QUALITY_MAX, 1, QUALITY_DEF);
 
 	if (hdl->error) {
-		pr_err("Could not initialize controls\n");
+		PERR("Could not initialize controls\n");
 		return hdl->error;
 	}
 	if (gspca_dev->autogain)
diff --git a/drivers/media/usb/gspca/ov534.c b/drivers/media/usb/gspca/ov534.c
index bb09d7884b8..2e28c81a03a 100644
--- a/drivers/media/usb/gspca/ov534.c
+++ b/drivers/media/usb/gspca/ov534.c
@@ -690,7 +690,7 @@ static int sccb_check_status(struct gspca_dev *gspca_dev)
 		case 0x03:
 			break;
 		default:
-			PDEBUG(D_ERR, "sccb status 0x%02x, attempt %d/5",
+			PERR("sccb status 0x%02x, attempt %d/5",
 			       data, i + 1);
 		}
 	}
diff --git a/drivers/media/usb/gspca/pac207.c b/drivers/media/usb/gspca/pac207.c
index 3b75097dd34..83519be94e5 100644
--- a/drivers/media/usb/gspca/pac207.c
+++ b/drivers/media/usb/gspca/pac207.c
@@ -373,7 +373,7 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	struct sd *sd = (struct sd *) gspca_dev;
 	unsigned char *sof;
 
-	sof = pac_find_sof(&sd->sof_read, data, len);
+	sof = pac_find_sof(gspca_dev, &sd->sof_read, data, len);
 	if (sof) {
 		int n;
 
diff --git a/drivers/media/usb/gspca/pac7302.c b/drivers/media/usb/gspca/pac7302.c
index add6f725ba5..6008c8d546a 100644
--- a/drivers/media/usb/gspca/pac7302.c
+++ b/drivers/media/usb/gspca/pac7302.c
@@ -344,13 +344,10 @@ static void reg_w_var(struct gspca_dev *gspca_dev,
 			reg_w_page(gspca_dev, page3, page3_len);
 			break;
 		default:
-#ifdef GSPCA_DEBUG
 			if (len > USB_BUF_SZ) {
-				PDEBUG(D_ERR|D_STREAM,
-					"Incorrect variable sequence");
+				PERR("Incorrect variable sequence");
 				return;
 			}
-#endif
 			while (len > 0) {
 				if (len < 8) {
 					reg_w_buf(gspca_dev,
@@ -795,7 +792,7 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	u8 *image;
 	u8 *sof;
 
-	sof = pac_find_sof(&sd->sof_read, data, len);
+	sof = pac_find_sof(gspca_dev, &sd->sof_read, data, len);
 	if (sof) {
 		int n, lum_offset, footer_length;
 
@@ -843,7 +840,7 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 static int sd_dbg_s_register(struct gspca_dev *gspca_dev,
-			struct v4l2_dbg_register *reg)
+			const struct v4l2_dbg_register *reg)
 {
 	u8 index;
 	u8 value;
diff --git a/drivers/media/usb/gspca/pac7311.c b/drivers/media/usb/gspca/pac7311.c
index a12dfbf6e05..1a5bdc853a8 100644
--- a/drivers/media/usb/gspca/pac7311.c
+++ b/drivers/media/usb/gspca/pac7311.c
@@ -262,8 +262,7 @@ static void reg_w_var(struct gspca_dev *gspca_dev,
 			break;
 		default:
 			if (len > USB_BUF_SZ) {
-				PDEBUG(D_ERR|D_STREAM,
-					"Incorrect variable sequence");
+				PERR("Incorrect variable sequence");
 				return;
 			}
 			while (len > 0) {
@@ -575,7 +574,7 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	u8 *image;
 	unsigned char *sof;
 
-	sof = pac_find_sof(&sd->sof_read, data, len);
+	sof = pac_find_sof(gspca_dev, &sd->sof_read, data, len);
 	if (sof) {
 		int n, lum_offset, footer_length;
 
diff --git a/drivers/media/usb/gspca/pac_common.h b/drivers/media/usb/gspca/pac_common.h
index 8462a7c1a33..fbc5e226c3e 100644
--- a/drivers/media/usb/gspca/pac_common.h
+++ b/drivers/media/usb/gspca/pac_common.h
@@ -71,7 +71,7 @@ static const unsigned char pac_sof_marker[5] =
 	   +----------+
 */
 
-static unsigned char *pac_find_sof(u8 *sof_read,
+static unsigned char *pac_find_sof(struct gspca_dev *gspca_dev, u8 *sof_read,
 					unsigned char *m, int len)
 {
 	int i;
diff --git a/drivers/media/usb/gspca/sn9c2028.c b/drivers/media/usb/gspca/sn9c2028.c
index 03fa3fd940b..39b6b2e0296 100644
--- a/drivers/media/usb/gspca/sn9c2028.c
+++ b/drivers/media/usb/gspca/sn9c2028.c
@@ -650,13 +650,13 @@ static void sd_stopN(struct gspca_dev *gspca_dev)
 
 	result = sn9c2028_read1(gspca_dev);
 	if (result < 0)
-		PDEBUG(D_ERR, "Camera Stop read failed");
+		PERR("Camera Stop read failed");
 
 	memset(data, 0, 6);
 	data[0] = 0x14;
 	result = sn9c2028_command(gspca_dev, data);
 	if (result < 0)
-		PDEBUG(D_ERR, "Camera Stop command failed");
+		PERR("Camera Stop command failed");
 }
 
 /* Include sn9c2028 sof detection functions */
diff --git a/drivers/media/usb/gspca/sn9c20x.c b/drivers/media/usb/gspca/sn9c20x.c
index 4ec544f4a84..ead9a1f5851 100644
--- a/drivers/media/usb/gspca/sn9c20x.c
+++ b/drivers/media/usb/gspca/sn9c20x.c
@@ -1598,7 +1598,7 @@ static int sd_dbg_g_register(struct gspca_dev *gspca_dev,
 }
 
 static int sd_dbg_s_register(struct gspca_dev *gspca_dev,
-			struct v4l2_dbg_register *reg)
+			const struct v4l2_dbg_register *reg)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
diff --git a/drivers/media/usb/gspca/sonixb.c b/drivers/media/usb/gspca/sonixb.c
index 104ae25275b..3fe207e038c 100644
--- a/drivers/media/usb/gspca/sonixb.c
+++ b/drivers/media/usb/gspca/sonixb.c
@@ -1379,27 +1379,6 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	}
 }
 
-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;
-}
-
 #if IS_ENABLED(CONFIG_INPUT)
 static int sd_int_pkt_scan(struct gspca_dev *gspca_dev,
 			u8 *data,		/* interrupt packet data */
@@ -1428,7 +1407,6 @@ static const struct sd_desc sd_desc = {
 	.start = sd_start,
 	.stopN = sd_stopN,
 	.pkt_scan = sd_pkt_scan,
-	.querymenu = sd_querymenu,
 	.dq_callback = do_autogain,
 #if IS_ENABLED(CONFIG_INPUT)
 	.int_pkt_scan = sd_int_pkt_scan,
diff --git a/drivers/media/usb/gspca/sonixj.c b/drivers/media/usb/gspca/sonixj.c
index 671d0c6dece..3b5ccb1c4cd 100644
--- a/drivers/media/usb/gspca/sonixj.c
+++ b/drivers/media/usb/gspca/sonixj.c
@@ -31,32 +31,26 @@ MODULE_AUTHOR("Jean-François Moine <http://moinejf.free.fr>");
 MODULE_DESCRIPTION("GSPCA/SONIX JPEG USB Camera Driver");
 MODULE_LICENSE("GPL");
 
-/* controls */
-enum e_ctrl {
-	BRIGHTNESS,
-	CONTRAST,
-	COLORS,
-	BLUE,
-	RED,
-	GAMMA,
-	EXPOSURE,
-	AUTOGAIN,
-	GAIN,
-	HFLIP,
-	VFLIP,
-	SHARPNESS,
-	ILLUM,
-	FREQ,
-	NCTRLS		/* number of controls */
-};
-
 /* specific webcam descriptor */
 struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
 
-	struct gspca_ctrl ctrls[NCTRLS];
-
 	atomic_t avg_lum;
+	struct v4l2_ctrl *brightness;
+	struct v4l2_ctrl *contrast;
+	struct v4l2_ctrl *saturation;
+	struct { /* red/blue balance control cluster */
+		struct v4l2_ctrl *red_bal;
+		struct v4l2_ctrl *blue_bal;
+	};
+	struct { /* hflip/vflip control cluster */
+		struct v4l2_ctrl *vflip;
+		struct v4l2_ctrl *hflip;
+	};
+	struct v4l2_ctrl *gamma;
+	struct v4l2_ctrl *illum;
+	struct v4l2_ctrl *sharpness;
+	struct v4l2_ctrl *freq;
 	u32 exposure;
 
 	struct work_struct work;
@@ -127,283 +121,6 @@ static void qual_upd(struct work_struct *work);
 #define SEN_CLK_EN	0x20	/* enable sensor clock */
 #define DEF_EN		0x80	/* defect pixel by 0: soft, 1: hard */
 
-/* V4L2 controls supported by the driver */
-static void setbrightness(struct gspca_dev *gspca_dev);
-static void setcontrast(struct gspca_dev *gspca_dev);
-static void setcolors(struct gspca_dev *gspca_dev);
-static void setredblue(struct gspca_dev *gspca_dev);
-static void setgamma(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 setgain(struct gspca_dev *gspca_dev);
-static void sethvflip(struct gspca_dev *gspca_dev);
-static void setsharpness(struct gspca_dev *gspca_dev);
-static void setillum(struct gspca_dev *gspca_dev);
-static void setfreq(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 = 0x80,
-	    },
-	    .set_control = setbrightness
-	},
-[CONTRAST] = {
-	    {
-		.id      = V4L2_CID_CONTRAST,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Contrast",
-		.minimum = 0,
-#define CONTRAST_MAX 127
-		.maximum = CONTRAST_MAX,
-		.step    = 1,
-		.default_value = 20,
-	    },
-	    .set_control = setcontrast
-	},
-[COLORS] = {
-	    {
-		.id      = V4L2_CID_SATURATION,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Saturation",
-		.minimum = 0,
-		.maximum = 40,
-		.step    = 1,
-#define COLORS_DEF 25
-		.default_value = COLORS_DEF,
-	    },
-	    .set_control = setcolors
-	},
-[BLUE] = {
-	    {
-		.id      = V4L2_CID_BLUE_BALANCE,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Blue Balance",
-		.minimum = 24,
-		.maximum = 40,
-		.step    = 1,
-		.default_value = 32,
-	    },
-	    .set_control = setredblue
-	},
-[RED] = {
-	    {
-		.id      = V4L2_CID_RED_BALANCE,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Red Balance",
-		.minimum = 24,
-		.maximum = 40,
-		.step    = 1,
-		.default_value = 32,
-	    },
-	    .set_control = setredblue
-	},
-[GAMMA] = {
-	    {
-		.id      = V4L2_CID_GAMMA,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Gamma",
-		.minimum = 0,
-		.maximum = 40,
-		.step    = 1,
-#define GAMMA_DEF 20
-		.default_value = GAMMA_DEF,
-	    },
-	    .set_control = setgamma
-	},
-[EXPOSURE] = {
-	    {
-		.id      = V4L2_CID_EXPOSURE,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Exposure",
-		.minimum = 500,
-		.maximum = 1500,
-		.step    = 1,
-		.default_value = 1024
-	    },
-	    .set_control = setexposure
-	},
-[AUTOGAIN] = {
-	    {
-		.id      = V4L2_CID_AUTOGAIN,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Auto Gain",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 1
-	    },
-	    .set = sd_setautogain,
-	},
-[GAIN] = {
-	    {
-		.id      = V4L2_CID_GAIN,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Gain",
-		.minimum = 4,
-		.maximum = 49,
-		.step    = 1,
-		.default_value = 15
-	    },
-	    .set_control = setgain
-	},
-[HFLIP] = {
-	    {
-		.id      = V4L2_CID_HFLIP,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Mirror",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 0,
-	    },
-	    .set_control = sethvflip
-	},
-[VFLIP] = {
-	    {
-		.id      = V4L2_CID_VFLIP,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Vflip",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 0,
-	    },
-	    .set_control = sethvflip
-	},
-[SHARPNESS] = {
-	    {
-		.id	 = V4L2_CID_SHARPNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Sharpness",
-		.minimum = 0,
-		.maximum = 255,
-		.step    = 1,
-		.default_value = 90,
-	    },
-	    .set_control = setsharpness
-	},
-[ILLUM] = {
-	    {
-		.id      = V4L2_CID_ILLUMINATORS_1,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Illuminator / infrared",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 0,
-	    },
-	    .set_control = setillum
-	},
-/* ov7630/ov7648/ov7660 only */
-[FREQ] = {
-	    {
-		.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 = 1,
-	    },
-	    .set_control = setfreq
-	},
-};
-
-/* table of the disabled controls */
-static const __u32 ctrl_dis[] = {
-[SENSOR_ADCM1700] =	(1 << EXPOSURE) |
-			(1 << AUTOGAIN) |
-			(1 << GAIN) |
-			(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << FREQ),
-
-[SENSOR_GC0307] =	(1 << EXPOSURE) |
-			(1 << GAIN) |
-			(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << FREQ),
-
-[SENSOR_HV7131R] =	(1 << EXPOSURE) |
-			(1 << GAIN) |
-			(1 << HFLIP) |
-			(1 << FREQ),
-
-[SENSOR_MI0360] =	(1 << EXPOSURE) |
-			(1 << GAIN) |
-			(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << FREQ),
-
-[SENSOR_MI0360B] =	(1 << EXPOSURE) |
-			(1 << GAIN) |
-			(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << FREQ),
-
-[SENSOR_MO4000] =	(1 << EXPOSURE) |
-			(1 << GAIN) |
-			(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << FREQ),
-
-[SENSOR_MT9V111] =	(1 << EXPOSURE) |
-			(1 << GAIN) |
-			(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << FREQ),
-
-[SENSOR_OM6802] =	(1 << EXPOSURE) |
-			(1 << GAIN) |
-			(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << FREQ),
-
-[SENSOR_OV7630] =	(1 << EXPOSURE) |
-			(1 << GAIN) |
-			(1 << HFLIP),
-
-[SENSOR_OV7648] =	(1 << EXPOSURE) |
-			(1 << GAIN) |
-			(1 << HFLIP),
-
-[SENSOR_OV7660] =	(1 << EXPOSURE) |
-			(1 << AUTOGAIN) |
-			(1 << GAIN) |
-			(1 << HFLIP) |
-			(1 << VFLIP),
-
-[SENSOR_PO1030] =	(1 << EXPOSURE) |
-			(1 << AUTOGAIN) |
-			(1 << GAIN) |
-			(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << FREQ),
-
-[SENSOR_PO2030N] =	(1 << FREQ),
-
-[SENSOR_SOI768] =	(1 << EXPOSURE) |
-			(1 << AUTOGAIN) |
-			(1 << GAIN) |
-			(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << FREQ),
-
-[SENSOR_SP80708] =	(1 << EXPOSURE) |
-			(1 << AUTOGAIN) |
-			(1 << GAIN) |
-			(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << FREQ),
-};
-
 static const struct v4l2_pix_format cif_mode[] = {
 	{352, 288, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 352,
@@ -1442,12 +1159,11 @@ static void reg_r(struct gspca_dev *gspca_dev,
 
 	if (gspca_dev->usb_err < 0)
 		return;
-#ifdef GSPCA_DEBUG
 	if (len > USB_BUF_SZ) {
-		pr_err("reg_r: buffer overflow\n");
+		PERR("reg_r: buffer overflow\n");
 		return;
 	}
-#endif
+
 	ret = usb_control_msg(gspca_dev->dev,
 			usb_rcvctrlpipe(gspca_dev->dev, 0),
 			0,
@@ -1496,12 +1212,12 @@ static void reg_w(struct gspca_dev *gspca_dev,
 		return;
 	PDEBUG(D_USBO, "reg_w [%04x] = %02x %02x ..",
 		value, buffer[0], buffer[1]);
-#ifdef GSPCA_DEBUG
+
 	if (len > USB_BUF_SZ) {
-		pr_err("reg_w: buffer overflow\n");
+		PERR("reg_w: buffer overflow\n");
 		return;
 	}
-#endif
+
 	memcpy(gspca_dev->usb_buf, buffer, len);
 	ret = usb_control_msg(gspca_dev->dev,
 			usb_sndctrlpipe(gspca_dev->dev, 0),
@@ -1822,7 +1538,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 		cam->nmodes = ARRAY_SIZE(vga_mode);
 	}
 	cam->npkt = 24;			/* 24 packets per ISOC message */
-	cam->ctrls = sd->ctrls;
 
 	sd->ag_cnt = -1;
 	sd->quality = QUALITY_DEF;
@@ -1888,9 +1603,6 @@ static int sd_init(struct gspca_dev *gspca_dev)
 		break;
 	}
 
-	if (sd->sensor == SENSOR_OM6802)
-		sd->ctrls[SHARPNESS].def = 0x10;
-
 	/* Note we do not disable the sensor clock here (power saving mode),
 	   as that also disables the button on the cam. */
 	reg_w1(gspca_dev, 0xf1, 0x00);
@@ -1899,13 +1611,92 @@ static int sd_init(struct gspca_dev *gspca_dev)
 	sn9c1xx = sn_tb[sd->sensor];
 	sd->i2c_addr = sn9c1xx[9];
 
-	gspca_dev->ctrl_dis = ctrl_dis[sd->sensor];
-	if (!(sd->flags & F_ILLUM))
-		gspca_dev->ctrl_dis |= (1 << ILLUM);
-
 	return gspca_dev->usb_err;
 }
 
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl);
+
+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, 14);
+
+	sd->brightness = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+#define CONTRAST_MAX 127
+	sd->contrast = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, CONTRAST_MAX, 1, 20);
+#define COLORS_DEF 25
+	sd->saturation = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 40, 1, COLORS_DEF);
+	sd->red_bal = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_RED_BALANCE, 24, 40, 1, 32);
+	sd->blue_bal = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BLUE_BALANCE, 24, 40, 1, 32);
+#define GAMMA_DEF 20
+	sd->gamma = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_GAMMA, 0, 40, 1, GAMMA_DEF);
+
+	if (sd->sensor == SENSOR_OM6802)
+		sd->sharpness = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SHARPNESS, 0, 255, 1, 16);
+	else
+		sd->sharpness = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SHARPNESS, 0, 255, 1, 90);
+
+	if (sd->flags & F_ILLUM)
+		sd->illum = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_ILLUMINATORS_1, 0, 1, 1, 0);
+
+	if (sd->sensor == SENSOR_PO2030N) {
+		gspca_dev->exposure = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_EXPOSURE, 500, 1500, 1, 1024);
+		gspca_dev->gain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_GAIN, 4, 49, 1, 15);
+		sd->hflip = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_HFLIP, 0, 1, 1, 0);
+	}
+
+	if (sd->sensor != SENSOR_ADCM1700 && sd->sensor != SENSOR_OV7660 &&
+	    sd->sensor != SENSOR_PO1030 && sd->sensor != SENSOR_SOI768 &&
+	    sd->sensor != SENSOR_SP80708)
+		gspca_dev->autogain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+
+	if (sd->sensor == SENSOR_HV7131R || sd->sensor == SENSOR_OV7630 ||
+	    sd->sensor == SENSOR_OV7648 || sd->sensor == SENSOR_PO2030N)
+		sd->vflip = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+	if (sd->sensor == SENSOR_OV7630 || sd->sensor == SENSOR_OV7648 ||
+	    sd->sensor == SENSOR_OV7660)
+		sd->freq = v4l2_ctrl_new_std_menu(hdl, &sd_ctrl_ops,
+			V4L2_CID_POWER_LINE_FREQUENCY,
+			V4L2_CID_POWER_LINE_FREQUENCY_60HZ, 0,
+			V4L2_CID_POWER_LINE_FREQUENCY_50HZ);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+
+	v4l2_ctrl_cluster(2, &sd->red_bal);
+	if (sd->sensor == SENSOR_PO2030N) {
+		v4l2_ctrl_cluster(2, &sd->vflip);
+		v4l2_ctrl_auto_cluster(3, &gspca_dev->autogain, 0, false);
+	}
+
+	return 0;
+}
+
 static u32 expo_adjust(struct gspca_dev *gspca_dev,
 			u32 expo)
 {
@@ -2014,10 +1805,9 @@ static void setbrightness(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	unsigned int expo;
-	int brightness;
+	int brightness = sd->brightness->val;
 	u8 k2;
 
-	brightness = sd->ctrls[BRIGHTNESS].val;
 	k2 = (brightness - 0x80) >> 2;
 	switch (sd->sensor) {
 	case SENSOR_ADCM1700:
@@ -2064,7 +1854,7 @@ static void setcontrast(struct gspca_dev *gspca_dev)
 	u8 k2;
 	u8 contrast[6];
 
-	k2 = sd->ctrls[CONTRAST].val * 37 / (CONTRAST_MAX + 1)
+	k2 = sd->contrast->val * 37 / (CONTRAST_MAX + 1)
 				+ 37;		/* 37..73 */
 	contrast[0] = (k2 + 1) / 2;		/* red */
 	contrast[1] = 0;
@@ -2090,7 +1880,7 @@ static void setcolors(struct gspca_dev *gspca_dev)
 		 60, -51, -9		/* VR VG VB */
 	};
 
-	colors = sd->ctrls[COLORS].val;
+	colors = sd->saturation->val;
 	if (sd->sensor == SENSOR_MI0360B)
 		uv = uv_mi0360b;
 	else
@@ -2112,14 +1902,14 @@ static void setredblue(struct gspca_dev *gspca_dev)
 			{0xc1, 0x6e, 0x16, 0x00, 0x40, 0x00, 0x00, 0x10};
 
 		/* 0x40 = normal value = gain x 1 */
-		rg1b[3] = sd->ctrls[RED].val * 2;
-		rg1b[5] = sd->ctrls[BLUE].val * 2;
+		rg1b[3] = sd->red_bal->val * 2;
+		rg1b[5] = sd->blue_bal->val * 2;
 		i2c_w8(gspca_dev, rg1b);
 		return;
 	}
-	reg_w1(gspca_dev, 0x05, sd->ctrls[RED].val);
+	reg_w1(gspca_dev, 0x05, sd->red_bal->val);
 /*	reg_w1(gspca_dev, 0x07, 32); */
-	reg_w1(gspca_dev, 0x06, sd->ctrls[BLUE].val);
+	reg_w1(gspca_dev, 0x06, sd->blue_bal->val);
 }
 
 static void setgamma(struct gspca_dev *gspca_dev)
@@ -2153,7 +1943,7 @@ static void setgamma(struct gspca_dev *gspca_dev)
 		break;
 	}
 
-	val = sd->ctrls[GAMMA].val;
+	val = sd->gamma->val;
 	for (i = 0; i < sizeof gamma; i++)
 		gamma[i] = gamma_base[i]
 			+ delta[i] * (val - GAMMA_DEF) / 32;
@@ -2168,11 +1958,11 @@ static void setexposure(struct gspca_dev *gspca_dev)
 		u8 rexpo[] =		/* 1a: expo H, 1b: expo M */
 			{0xa1, 0x6e, 0x1a, 0x00, 0x40, 0x00, 0x00, 0x10};
 
-		rexpo[3] = sd->ctrls[EXPOSURE].val >> 8;
+		rexpo[3] = gspca_dev->exposure->val >> 8;
 		i2c_w8(gspca_dev, rexpo);
 		msleep(6);
 		rexpo[2] = 0x1b;
-		rexpo[3] = sd->ctrls[EXPOSURE].val;
+		rexpo[3] = gspca_dev->exposure->val;
 		i2c_w8(gspca_dev, rexpo);
 	}
 }
@@ -2181,8 +1971,6 @@ static void setautogain(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	if (gspca_dev->ctrl_dis & (1 << AUTOGAIN))
-		return;
 	switch (sd->sensor) {
 	case SENSOR_OV7630:
 	case SENSOR_OV7648: {
@@ -2192,13 +1980,13 @@ static void setautogain(struct gspca_dev *gspca_dev)
 			comb = 0xc0;
 		else
 			comb = 0xa0;
-		if (sd->ctrls[AUTOGAIN].val)
+		if (gspca_dev->autogain->val)
 			comb |= 0x03;
 		i2c_w1(&sd->gspca_dev, 0x13, comb);
 		return;
 	    }
 	}
-	if (sd->ctrls[AUTOGAIN].val)
+	if (gspca_dev->autogain->val)
 		sd->ag_cnt = AG_CNT_START;
 	else
 		sd->ag_cnt = -1;
@@ -2212,7 +2000,7 @@ static void setgain(struct gspca_dev *gspca_dev)
 		u8 rgain[] =		/* 15: gain */
 			{0xa1, 0x6e, 0x15, 0x00, 0x40, 0x00, 0x00, 0x15};
 
-		rgain[3] = sd->ctrls[GAIN].val;
+		rgain[3] = gspca_dev->gain->val;
 		i2c_w8(gspca_dev, rgain);
 	}
 }
@@ -2225,19 +2013,19 @@ static void sethvflip(struct gspca_dev *gspca_dev)
 	switch (sd->sensor) {
 	case SENSOR_HV7131R:
 		comn = 0x18;			/* clkdiv = 1, ablcen = 1 */
-		if (sd->ctrls[VFLIP].val)
+		if (sd->vflip->val)
 			comn |= 0x01;
 		i2c_w1(gspca_dev, 0x01, comn);	/* sctra */
 		break;
 	case SENSOR_OV7630:
 		comn = 0x02;
-		if (!sd->ctrls[VFLIP].val)
+		if (!sd->vflip->val)
 			comn |= 0x80;
 		i2c_w1(gspca_dev, 0x75, comn);
 		break;
 	case SENSOR_OV7648:
 		comn = 0x06;
-		if (sd->ctrls[VFLIP].val)
+		if (sd->vflip->val)
 			comn |= 0x80;
 		i2c_w1(gspca_dev, 0x75, comn);
 		break;
@@ -2251,9 +2039,9 @@ static void sethvflip(struct gspca_dev *gspca_dev)
 		 * bit3-0: X
 		 */
 		comn = 0x0a;
-		if (sd->ctrls[HFLIP].val)
+		if (sd->hflip->val)
 			comn |= 0x80;
-		if (sd->ctrls[VFLIP].val)
+		if (sd->vflip->val)
 			comn |= 0x40;
 		i2c_w1(&sd->gspca_dev, 0x1e, comn);
 		break;
@@ -2264,23 +2052,21 @@ static void setsharpness(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	reg_w1(gspca_dev, 0x99, sd->ctrls[SHARPNESS].val);
+	reg_w1(gspca_dev, 0x99, sd->sharpness->val);
 }
 
 static void setillum(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	if (gspca_dev->ctrl_dis & (1 << ILLUM))
-		return;
 	switch (sd->sensor) {
 	case SENSOR_ADCM1700:
 		reg_w1(gspca_dev, 0x02,				/* gpio */
-			sd->ctrls[ILLUM].val ? 0x64 : 0x60);
+			sd->illum->val ? 0x64 : 0x60);
 		break;
 	case SENSOR_MT9V111:
 		reg_w1(gspca_dev, 0x02,
-			sd->ctrls[ILLUM].val ? 0x77 : 0x74);
+			sd->illum->val ? 0x77 : 0x74);
 /* should have been: */
 /*						0x55 : 0x54);	* 370i */
 /*						0x66 : 0x64);	* Clip */
@@ -2292,13 +2078,11 @@ static void setfreq(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	if (gspca_dev->ctrl_dis & (1 << FREQ))
-		return;
 	if (sd->sensor == SENSOR_OV7660) {
 		u8 com8;
 
 		com8 = 0xdf;		/* auto gain/wb/expo */
-		switch (sd->ctrls[FREQ].val) {
+		switch (sd->freq->val) {
 		case 0: /* Banding filter disabled */
 			i2c_w1(gspca_dev, 0x13, com8 | 0x20);
 			break;
@@ -2326,7 +2110,7 @@ static void setfreq(struct gspca_dev *gspca_dev)
 			break;
 		}
 
-		switch (sd->ctrls[FREQ].val) {
+		switch (sd->freq->val) {
 		case 0: /* Banding filter disabled */
 			break;
 		case 1: /* 50 hz (filter on and framerate adj) */
@@ -2698,17 +2482,6 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	sd->reg01 = reg01;
 	sd->reg17 = reg17;
 
-	sethvflip(gspca_dev);
-	setbrightness(gspca_dev);
-	setcontrast(gspca_dev);
-	setcolors(gspca_dev);
-	setautogain(gspca_dev);
-	if (!(gspca_dev->ctrl_inac & ((1 << EXPOSURE) | (1 << GAIN)))) {
-		setexposure(gspca_dev);
-		setgain(gspca_dev);
-	}
-	setfreq(gspca_dev);
-
 	sd->pktsz = sd->npkt = 0;
 	sd->nchg = sd->short_mark = 0;
 	sd->work_thread = create_singlethread_workqueue(MODULE_NAME);
@@ -2803,9 +2576,6 @@ static void sd_stop0(struct gspca_dev *gspca_dev)
 	}
 }
 
-#define WANT_REGULAR_AUTOGAIN
-#include "autogain_functions.h"
-
 static void do_autogain(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -2825,7 +2595,7 @@ static void do_autogain(struct gspca_dev *gspca_dev)
 	PDEBUG(D_FRAM, "mean lum %d", delta);
 
 	if (sd->sensor == SENSOR_PO2030N) {
-		auto_gain_n_exposure(gspca_dev, delta, luma_mean, luma_delta,
+		gspca_expo_autogain(gspca_dev, delta, luma_mean, luma_delta,
 					15, 1024);
 		return;
 	}
@@ -3042,39 +2812,53 @@ marker_found:
 	}
 }
 
-static int sd_setautogain(struct gspca_dev *gspca_dev, __s32 val)
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
 
-	sd->ctrls[AUTOGAIN].val = val;
-	if (val)
-		gspca_dev->ctrl_inac |= (1 << EXPOSURE) | (1 << GAIN);
-	else
-		gspca_dev->ctrl_inac &= ~(1 << EXPOSURE) & ~(1 << GAIN);
-	if (gspca_dev->streaming)
-		setautogain(gspca_dev);
-	return gspca_dev->usb_err;
-}
+	gspca_dev->usb_err = 0;
 
-static int sd_querymenu(struct gspca_dev *gspca_dev,
-			struct v4l2_querymenu *menu)
-{
-	switch (menu->id) {
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		setbrightness(gspca_dev);
+		break;
+	case V4L2_CID_CONTRAST:
+		setcontrast(gspca_dev);
+		break;
+	case V4L2_CID_SATURATION:
+		setcolors(gspca_dev);
+		break;
+	case V4L2_CID_RED_BALANCE:
+		setredblue(gspca_dev);
+		break;
+	case V4L2_CID_GAMMA:
+		setgamma(gspca_dev);
+		break;
+	case V4L2_CID_AUTOGAIN:
+		setautogain(gspca_dev);
+		setexposure(gspca_dev);
+		setgain(gspca_dev);
+		break;
+	case V4L2_CID_VFLIP:
+		sethvflip(gspca_dev);
+		break;
+	case V4L2_CID_SHARPNESS:
+		setsharpness(gspca_dev);
+		break;
+	case V4L2_CID_ILLUMINATORS_1:
+		setillum(gspca_dev);
+		break;
 	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;
-		}
+		setfreq(gspca_dev);
 		break;
+	default:
+		return -EINVAL;
 	}
-	return -EINVAL;
+	return gspca_dev->usb_err;
 }
 
 #if IS_ENABLED(CONFIG_INPUT)
@@ -3099,16 +2883,14 @@ 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 = NCTRLS,
 	.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,
-	.querymenu = sd_querymenu,
 #if IS_ENABLED(CONFIG_INPUT)
 	.int_pkt_scan = sd_int_pkt_scan,
 #endif
diff --git a/drivers/media/usb/gspca/spca1528.c b/drivers/media/usb/gspca/spca1528.c
index 14d635277d7..688592b289e 100644
--- a/drivers/media/usb/gspca/spca1528.c
+++ b/drivers/media/usb/gspca/spca1528.c
@@ -146,7 +146,7 @@ static void wait_status_0(struct gspca_dev *gspca_dev)
 		w += 15;
 		msleep(w);
 	} while (--i > 0);
-	PDEBUG(D_ERR, "wait_status_0 timeout");
+	PERR("wait_status_0 timeout");
 	gspca_dev->usb_err = -ETIME;
 }
 
@@ -164,7 +164,7 @@ static void wait_status_1(struct gspca_dev *gspca_dev)
 			return;
 		}
 	} while (--i > 0);
-	PDEBUG(D_ERR, "wait_status_1 timeout");
+	PERR("wait_status_1 timeout");
 	gspca_dev->usb_err = -ETIME;
 }
 
diff --git a/drivers/media/usb/gspca/spca500.c b/drivers/media/usb/gspca/spca500.c
index 25cb68d0556..9f8bf51fd64 100644
--- a/drivers/media/usb/gspca/spca500.c
+++ b/drivers/media/usb/gspca/spca500.c
@@ -489,7 +489,7 @@ static int spca500_full_reset(struct gspca_dev *gspca_dev)
 		return err;
 	err = reg_r_wait(gspca_dev, 0x06, 0, 0);
 	if (err < 0) {
-		PDEBUG(D_ERR, "reg_r_wait() failed");
+		PERR("reg_r_wait() failed");
 		return err;
 	}
 	/* all ok */
@@ -505,7 +505,7 @@ static int spca500_full_reset(struct gspca_dev *gspca_dev)
 static int spca500_synch310(struct gspca_dev *gspca_dev)
 {
 	if (usb_set_interface(gspca_dev->dev, gspca_dev->iface, 0) < 0) {
-		PDEBUG(D_ERR, "Set packet size: set interface error");
+		PERR("Set packet size: set interface error");
 		goto error;
 	}
 	spca500_ping310(gspca_dev);
@@ -519,7 +519,7 @@ static int spca500_synch310(struct gspca_dev *gspca_dev)
 	if (usb_set_interface(gspca_dev->dev,
 				gspca_dev->iface,
 				gspca_dev->alt) < 0) {
-		PDEBUG(D_ERR, "Set packet size: set interface error");
+		PERR("Set packet size: set interface error");
 		goto error;
 	}
 	return 0;
@@ -544,7 +544,7 @@ static void spca500_reinit(struct gspca_dev *gspca_dev)
 	err = spca50x_setup_qtable(gspca_dev, 0x00, 0x8800, 0x8840,
 				 qtable_pocketdv);
 	if (err < 0)
-		PDEBUG(D_ERR|D_STREAM, "spca50x_setup_qtable failed on init");
+		PERR("spca50x_setup_qtable failed on init");
 
 	/* set qtable index */
 	reg_w(gspca_dev, 0x00, 0x8880, 2);
@@ -639,7 +639,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 					   0x00, 0x8800, 0x8840,
 					   qtable_creative_pccam);
 		if (err < 0)
-			PDEBUG(D_ERR, "spca50x_setup_qtable failed");
+			PERR("spca50x_setup_qtable failed");
 		/* Init SDRAM - needed for SDRAM access */
 		reg_w(gspca_dev, 0x00, 0x870a, 0x04);
 
@@ -647,7 +647,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		reg_w(gspca_dev, 0x00, 0x8000, 0x0004);
 		msleep(500);
 		if (reg_r_wait(gspca_dev, 0, 0x8000, 0x44) != 0)
-			PDEBUG(D_ERR, "reg_r_wait() failed");
+			PERR("reg_r_wait() failed");
 
 		reg_r(gspca_dev, 0x816b, 1);
 		Data = gspca_dev->usb_buf[0];
@@ -660,13 +660,13 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		/* enable drop packet */
 		err = reg_w(gspca_dev, 0x00, 0x850a, 0x0001);
 		if (err < 0)
-			PDEBUG(D_ERR, "failed to enable drop packet");
+			PERR("failed to enable drop packet");
 		reg_w(gspca_dev, 0x00, 0x8880, 3);
 		err = spca50x_setup_qtable(gspca_dev,
 					   0x00, 0x8800, 0x8840,
 					   qtable_creative_pccam);
 		if (err < 0)
-			PDEBUG(D_ERR, "spca50x_setup_qtable failed");
+			PERR("spca50x_setup_qtable failed");
 
 		/* Init SDRAM - needed for SDRAM access */
 		reg_w(gspca_dev, 0x00, 0x870a, 0x04);
@@ -675,7 +675,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		reg_w(gspca_dev, 0x00, 0x8000, 0x0004);
 
 		if (reg_r_wait(gspca_dev, 0, 0x8000, 0x44) != 0)
-			PDEBUG(D_ERR, "reg_r_wait() failed");
+			PERR("reg_r_wait() failed");
 
 		reg_r(gspca_dev, 0x816b, 1);
 		Data = gspca_dev->usb_buf[0];
@@ -689,18 +689,18 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		/* do a full reset */
 		err = spca500_full_reset(gspca_dev);
 		if (err < 0)
-			PDEBUG(D_ERR, "spca500_full_reset failed");
+			PERR("spca500_full_reset failed");
 
 		/* enable drop packet */
 		err = reg_w(gspca_dev, 0x00, 0x850a, 0x0001);
 		if (err < 0)
-			PDEBUG(D_ERR, "failed to enable drop packet");
+			PERR("failed to enable drop packet");
 		reg_w(gspca_dev, 0x00, 0x8880, 3);
 		err = spca50x_setup_qtable(gspca_dev,
 					   0x00, 0x8800, 0x8840,
 					   qtable_creative_pccam);
 		if (err < 0)
-			PDEBUG(D_ERR, "spca50x_setup_qtable failed");
+			PERR("spca50x_setup_qtable failed");
 
 		spca500_setmode(gspca_dev, xmult, ymult);
 		reg_w(gspca_dev, 0x20, 0x0001, 0x0004);
@@ -709,7 +709,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		reg_w(gspca_dev, 0x00, 0x8000, 0x0004);
 
 		if (reg_r_wait(gspca_dev, 0, 0x8000, 0x44) != 0)
-			PDEBUG(D_ERR, "reg_r_wait() failed");
+			PERR("reg_r_wait() failed");
 
 		reg_r(gspca_dev, 0x816b, 1);
 		Data = gspca_dev->usb_buf[0];
@@ -722,7 +722,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		/* do a full reset */
 		err = spca500_full_reset(gspca_dev);
 		if (err < 0)
-			PDEBUG(D_ERR, "spca500_full_reset failed");
+			PERR("spca500_full_reset failed");
 		/* enable drop packet */
 		reg_w(gspca_dev, 0x00, 0x850a, 0x0001);
 		reg_w(gspca_dev, 0x00, 0x8880, 0);
@@ -730,7 +730,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 					   0x00, 0x8800, 0x8840,
 					   qtable_kodak_ez200);
 		if (err < 0)
-			PDEBUG(D_ERR, "spca50x_setup_qtable failed");
+			PERR("spca50x_setup_qtable failed");
 		spca500_setmode(gspca_dev, xmult, ymult);
 
 		reg_w(gspca_dev, 0x20, 0x0001, 0x0004);
@@ -739,7 +739,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		reg_w(gspca_dev, 0x00, 0x8000, 0x0004);
 
 		if (reg_r_wait(gspca_dev, 0, 0x8000, 0x44) != 0)
-			PDEBUG(D_ERR, "reg_r_wait() failed");
+			PERR("reg_r_wait() failed");
 
 		reg_r(gspca_dev, 0x816b, 1);
 		Data = gspca_dev->usb_buf[0];
@@ -765,7 +765,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		err = spca50x_setup_qtable(gspca_dev,
 				   0x00, 0x8800, 0x8840, qtable_pocketdv);
 		if (err < 0)
-			PDEBUG(D_ERR, "spca50x_setup_qtable failed");
+			PERR("spca50x_setup_qtable failed");
 		reg_w(gspca_dev, 0x00, 0x8880, 2);
 
 		/* familycam Quicksmart pocketDV stuff */
@@ -795,7 +795,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 					0x00, 0x8800,
 					0x8840, qtable_creative_pccam);
 		if (err < 0)
-			PDEBUG(D_ERR, "spca50x_setup_qtable failed");
+			PERR("spca50x_setup_qtable failed");
 		reg_w(gspca_dev, 0x00, 0x8880, 3);
 		reg_w(gspca_dev, 0x00, 0x800a, 0x00);
 		/* Init SDRAM - needed for SDRAM access */
diff --git a/drivers/media/usb/gspca/spca501.c b/drivers/media/usb/gspca/spca501.c
index 3b7f777785b..d92fd17d670 100644
--- a/drivers/media/usb/gspca/spca501.c
+++ b/drivers/media/usb/gspca/spca501.c
@@ -1756,10 +1756,11 @@ static const __u16 spca501c_mysterious_init_data[][3] = {
 	{}
 };
 
-static int reg_write(struct usb_device *dev,
-		     __u16 req, __u16 index, __u16 value)
+static int reg_write(struct gspca_dev *gspca_dev,
+					__u16 req, __u16 index, __u16 value)
 {
 	int ret;
+	struct usb_device *dev = gspca_dev->dev;
 
 	ret = usb_control_msg(dev,
 			usb_sndctrlpipe(dev, 0),
@@ -1774,17 +1775,15 @@ static int reg_write(struct usb_device *dev,
 }
 
 
-static int write_vector(struct gspca_dev *gspca_dev,
-			const __u16 data[][3])
+static int write_vector(struct gspca_dev *gspca_dev, const __u16 data[][3])
 {
-	struct usb_device *dev = gspca_dev->dev;
 	int ret, i = 0;
 
 	while (data[i][0] != 0 || data[i][1] != 0 || data[i][2] != 0) {
-		ret = reg_write(dev, data[i][0], data[i][2], data[i][1]);
+		ret = reg_write(gspca_dev, data[i][0], data[i][2],
+								data[i][1]);
 		if (ret < 0) {
-			PDEBUG(D_ERR,
-				"Reg write failed for 0x%02x,0x%02x,0x%02x",
+			PERR("Reg write failed for 0x%02x,0x%02x,0x%02x",
 				data[i][0], data[i][1], data[i][2]);
 			return ret;
 		}
@@ -1795,30 +1794,28 @@ static int write_vector(struct gspca_dev *gspca_dev,
 
 static void setbrightness(struct gspca_dev *gspca_dev, s32 val)
 {
-	reg_write(gspca_dev->dev, SPCA501_REG_CCDSP, 0x12, val);
+	reg_write(gspca_dev, SPCA501_REG_CCDSP, 0x12, val);
 }
 
 static void setcontrast(struct gspca_dev *gspca_dev, s32 val)
 {
-	reg_write(gspca_dev->dev, 0x00, 0x00,
-				  (val >> 8) & 0xff);
-	reg_write(gspca_dev->dev, 0x00, 0x01,
-				  val & 0xff);
+	reg_write(gspca_dev, 0x00, 0x00, (val >> 8) & 0xff);
+	reg_write(gspca_dev, 0x00, 0x01, val & 0xff);
 }
 
 static void setcolors(struct gspca_dev *gspca_dev, s32 val)
 {
-	reg_write(gspca_dev->dev, SPCA501_REG_CCDSP, 0x0c, val);
+	reg_write(gspca_dev, SPCA501_REG_CCDSP, 0x0c, val);
 }
 
 static void setblue_balance(struct gspca_dev *gspca_dev, s32 val)
 {
-	reg_write(gspca_dev->dev, SPCA501_REG_CCDSP, 0x11, val);
+	reg_write(gspca_dev, SPCA501_REG_CCDSP, 0x11, val);
 }
 
 static void setred_balance(struct gspca_dev *gspca_dev, s32 val)
 {
-	reg_write(gspca_dev->dev, SPCA501_REG_CCDSP, 0x13, val);
+	reg_write(gspca_dev, SPCA501_REG_CCDSP, 0x13, val);
 }
 
 /* this function is called at probe time */
@@ -1868,7 +1865,6 @@ error:
 static int sd_start(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	struct usb_device *dev = gspca_dev->dev;
 	int mode;
 
 	switch (sd->subtype) {
@@ -1895,20 +1891,20 @@ static int sd_start(struct gspca_dev *gspca_dev)
 
 	/* Enable ISO packet machine CTRL reg=2,
 	 * index=1 bitmask=0x2 (bit ordinal 1) */
-	reg_write(dev, SPCA50X_REG_USB, 0x6, 0x94);
+	reg_write(gspca_dev, SPCA50X_REG_USB, 0x6, 0x94);
 	switch (mode) {
 	case 0: /* 640x480 */
-		reg_write(dev, SPCA50X_REG_USB, 0x07, 0x004a);
+		reg_write(gspca_dev, SPCA50X_REG_USB, 0x07, 0x004a);
 		break;
 	case 1: /* 320x240 */
-		reg_write(dev, SPCA50X_REG_USB, 0x07, 0x104a);
+		reg_write(gspca_dev, SPCA50X_REG_USB, 0x07, 0x104a);
 		break;
 	default:
 /*	case 2:  * 160x120 */
-		reg_write(dev, SPCA50X_REG_USB, 0x07, 0x204a);
+		reg_write(gspca_dev, SPCA50X_REG_USB, 0x07, 0x204a);
 		break;
 	}
-	reg_write(dev, SPCA501_REG_CTLRL, 0x01, 0x02);
+	reg_write(gspca_dev, SPCA501_REG_CTLRL, 0x01, 0x02);
 
 	return 0;
 }
@@ -1917,7 +1913,7 @@ static void sd_stopN(struct gspca_dev *gspca_dev)
 {
 	/* Disable ISO packet
 	 * machine CTRL reg=2, index=1 bitmask=0x0 (bit ordinal 1) */
-	reg_write(gspca_dev->dev, SPCA501_REG_CTLRL, 0x01, 0x00);
+	reg_write(gspca_dev, SPCA501_REG_CTLRL, 0x01, 0x00);
 }
 
 /* called on streamoff with alt 0 and on disconnect */
@@ -1925,7 +1921,7 @@ static void sd_stop0(struct gspca_dev *gspca_dev)
 {
 	if (!gspca_dev->present)
 		return;
-	reg_write(gspca_dev->dev, SPCA501_REG_CTLRL, 0x05, 0x00);
+	reg_write(gspca_dev, SPCA501_REG_CTLRL, 0x05, 0x00);
 }
 
 static void sd_pkt_scan(struct gspca_dev *gspca_dev,
diff --git a/drivers/media/usb/gspca/spca505.c b/drivers/media/usb/gspca/spca505.c
index bc7d67c3cb0..232b330d2dd 100644
--- a/drivers/media/usb/gspca/spca505.c
+++ b/drivers/media/usb/gspca/spca505.c
@@ -544,10 +544,11 @@ static const u8 spca505b_open_data_ccd[][3] = {
 	{}
 };
 
-static int reg_write(struct usb_device *dev,
+static int reg_write(struct gspca_dev *gspca_dev,
 		     u16 req, u16 index, u16 value)
 {
 	int ret;
+	struct usb_device *dev = gspca_dev->dev;
 
 	ret = usb_control_msg(dev,
 			usb_sndctrlpipe(dev, 0),
@@ -584,11 +585,11 @@ static int reg_read(struct gspca_dev *gspca_dev,
 static int write_vector(struct gspca_dev *gspca_dev,
 			const u8 data[][3])
 {
-	struct usb_device *dev = gspca_dev->dev;
 	int ret, i = 0;
 
 	while (data[i][0] != 0) {
-		ret = reg_write(dev, data[i][0], data[i][2], data[i][1]);
+		ret = reg_write(gspca_dev, data[i][0], data[i][2],
+								data[i][1]);
 		if (ret < 0)
 			return ret;
 		i++;
@@ -629,14 +630,13 @@ static int sd_init(struct gspca_dev *gspca_dev)
 
 static void setbrightness(struct gspca_dev *gspca_dev, s32 brightness)
 {
-	reg_write(gspca_dev->dev, 0x05, 0x00, (255 - brightness) >> 6);
-	reg_write(gspca_dev->dev, 0x05, 0x01, (255 - brightness) << 2);
+	reg_write(gspca_dev, 0x05, 0x00, (255 - brightness) >> 6);
+	reg_write(gspca_dev, 0x05, 0x01, (255 - brightness) << 2);
 }
 
 static int sd_start(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	struct usb_device *dev = gspca_dev->dev;
 	int ret, mode;
 	static u8 mode_tb[][3] = {
 	/*	  r00   r06   r07	*/
@@ -654,9 +654,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	ret = reg_read(gspca_dev, 0x06, 0x16);
 
 	if (ret < 0) {
-		PDEBUG(D_ERR|D_CONF,
-		       "register read failed err: %d",
-		       ret);
+		PERR("register read failed err: %d", ret);
 		return ret;
 	}
 	if (ret != 0x0101) {
@@ -664,22 +662,22 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		       ret);
 	}
 
-	ret = reg_write(gspca_dev->dev, 0x06, 0x16, 0x0a);
+	ret = reg_write(gspca_dev, 0x06, 0x16, 0x0a);
 	if (ret < 0)
 		return ret;
-	reg_write(gspca_dev->dev, 0x05, 0xc2, 0x12);
+	reg_write(gspca_dev, 0x05, 0xc2, 0x12);
 
 	/* necessary because without it we can see stream
 	 * only once after loading module */
 	/* stopping usb registers Tomasz change */
-	reg_write(dev, 0x02, 0x00, 0x00);
+	reg_write(gspca_dev, 0x02, 0x00, 0x00);
 
 	mode = gspca_dev->cam.cam_mode[(int) gspca_dev->curr_mode].priv;
-	reg_write(dev, SPCA50X_REG_COMPRESS, 0x00, mode_tb[mode][0]);
-	reg_write(dev, SPCA50X_REG_COMPRESS, 0x06, mode_tb[mode][1]);
-	reg_write(dev, SPCA50X_REG_COMPRESS, 0x07, mode_tb[mode][2]);
+	reg_write(gspca_dev, SPCA50X_REG_COMPRESS, 0x00, mode_tb[mode][0]);
+	reg_write(gspca_dev, SPCA50X_REG_COMPRESS, 0x06, mode_tb[mode][1]);
+	reg_write(gspca_dev, SPCA50X_REG_COMPRESS, 0x07, mode_tb[mode][2]);
 
-	return reg_write(dev, SPCA50X_REG_USB,
+	return reg_write(gspca_dev, SPCA50X_REG_USB,
 			 SPCA50X_USB_CTRL,
 			 SPCA50X_CUSB_ENABLE);
 }
@@ -687,7 +685,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 static void sd_stopN(struct gspca_dev *gspca_dev)
 {
 	/* Disable ISO packet machine */
-	reg_write(gspca_dev->dev, 0x02, 0x00, 0x00);
+	reg_write(gspca_dev, 0x02, 0x00, 0x00);
 }
 
 /* called on streamoff with alt 0 and on disconnect */
@@ -697,11 +695,11 @@ static void sd_stop0(struct gspca_dev *gspca_dev)
 		return;
 
 	/* This maybe reset or power control */
-	reg_write(gspca_dev->dev, 0x03, 0x03, 0x20);
-	reg_write(gspca_dev->dev, 0x03, 0x01, 0x00);
-	reg_write(gspca_dev->dev, 0x03, 0x00, 0x01);
-	reg_write(gspca_dev->dev, 0x05, 0x10, 0x01);
-	reg_write(gspca_dev->dev, 0x05, 0x11, 0x0f);
+	reg_write(gspca_dev, 0x03, 0x03, 0x20);
+	reg_write(gspca_dev, 0x03, 0x01, 0x00);
+	reg_write(gspca_dev, 0x03, 0x00, 0x01);
+	reg_write(gspca_dev, 0x05, 0x10, 0x01);
+	reg_write(gspca_dev, 0x05, 0x11, 0x0f);
 }
 
 static void sd_pkt_scan(struct gspca_dev *gspca_dev,
diff --git a/drivers/media/usb/gspca/spca508.c b/drivers/media/usb/gspca/spca508.c
index 1286b4170b8..75f2beb2ea5 100644
--- a/drivers/media/usb/gspca/spca508.c
+++ b/drivers/media/usb/gspca/spca508.c
@@ -1241,10 +1241,10 @@ static const u16 spca508_vista_init_data[][2] = {
 	{}
 };
 
-static int reg_write(struct usb_device *dev,
-			u16 index, u16 value)
+static int reg_write(struct gspca_dev *gspca_dev, u16 index, u16 value)
 {
 	int ret;
+	struct usb_device *dev = gspca_dev->dev;
 
 	ret = usb_control_msg(dev,
 			usb_sndctrlpipe(dev, 0),
@@ -1286,22 +1286,21 @@ static int reg_read(struct gspca_dev *gspca_dev,
 static int ssi_w(struct gspca_dev *gspca_dev,
 		u16 reg, u16 val)
 {
-	struct usb_device *dev = gspca_dev->dev;
 	int ret, retry;
 
-	ret = reg_write(dev, 0x8802, reg >> 8);
+	ret = reg_write(gspca_dev, 0x8802, reg >> 8);
 	if (ret < 0)
 		goto out;
-	ret = reg_write(dev, 0x8801, reg & 0x00ff);
+	ret = reg_write(gspca_dev, 0x8801, reg & 0x00ff);
 	if (ret < 0)
 		goto out;
 	if ((reg & 0xff00) == 0x1000) {		/* if 2 bytes */
-		ret = reg_write(dev, 0x8805, val & 0x00ff);
+		ret = reg_write(gspca_dev, 0x8805, val & 0x00ff);
 		if (ret < 0)
 			goto out;
 		val >>= 8;
 	}
-	ret = reg_write(dev, 0x8800, val);
+	ret = reg_write(gspca_dev, 0x8800, val);
 	if (ret < 0)
 		goto out;
 
@@ -1314,8 +1313,7 @@ static int ssi_w(struct gspca_dev *gspca_dev,
 		if (gspca_dev->usb_buf[0] == 0)
 			break;
 		if (--retry <= 0) {
-			PDEBUG(D_ERR, "ssi_w busy %02x",
-					gspca_dev->usb_buf[0]);
+			PERR("ssi_w busy %02x", gspca_dev->usb_buf[0]);
 			ret = -1;
 			break;
 		}
@@ -1329,7 +1327,6 @@ out:
 static int write_vector(struct gspca_dev *gspca_dev,
 			const u16 (*data)[2])
 {
-	struct usb_device *dev = gspca_dev->dev;
 	int ret = 0;
 
 	while ((*data)[1] != 0) {
@@ -1337,7 +1334,8 @@ static int write_vector(struct gspca_dev *gspca_dev,
 			if ((*data)[1] == 0xdd00)	/* delay */
 				msleep((*data)[0]);
 			else
-				ret = reg_write(dev, (*data)[1], (*data)[0]);
+				ret = reg_write(gspca_dev, (*data)[1],
+								(*data)[0]);
 		} else {
 			ret = ssi_w(gspca_dev, (*data)[1], (*data)[0]);
 		}
@@ -1363,8 +1361,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 		spca508cs110_init_data,		/* MicroInnovationIC200 4 */
 		spca508_init_data,		/* ViewQuestVQ110 5 */
 	};
-
-#ifdef GSPCA_DEBUG
 	int data1, data2;
 
 	/* Read from global register the USB product and vendor IDs, just to
@@ -1381,7 +1377,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 
 	data1 = reg_read(gspca_dev, 0x8621);
 	PDEBUG(D_PROBE, "Window 1 average luminance: %d", data1);
-#endif
 
 	cam = &gspca_dev->cam;
 	cam->cam_mode = sif_mode;
@@ -1404,26 +1399,26 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	int mode;
 
 	mode = gspca_dev->cam.cam_mode[gspca_dev->curr_mode].priv;
-	reg_write(gspca_dev->dev, 0x8500, mode);
+	reg_write(gspca_dev, 0x8500, mode);
 	switch (mode) {
 	case 0:
 	case 1:
-		reg_write(gspca_dev->dev, 0x8700, 0x28);	/* clock */
+		reg_write(gspca_dev, 0x8700, 0x28); /* clock */
 		break;
 	default:
 /*	case 2: */
 /*	case 3: */
-		reg_write(gspca_dev->dev, 0x8700, 0x23);	/* clock */
+		reg_write(gspca_dev, 0x8700, 0x23); /* clock */
 		break;
 	}
-	reg_write(gspca_dev->dev, 0x8112, 0x10 | 0x20);
+	reg_write(gspca_dev, 0x8112, 0x10 | 0x20);
 	return 0;
 }
 
 static void sd_stopN(struct gspca_dev *gspca_dev)
 {
 	/* Video ISO disable, Video Drop Packet enable: */
-	reg_write(gspca_dev->dev, 0x8112, 0x20);
+	reg_write(gspca_dev, 0x8112, 0x20);
 }
 
 static void sd_pkt_scan(struct gspca_dev *gspca_dev,
@@ -1450,10 +1445,10 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 static void setbrightness(struct gspca_dev *gspca_dev, s32 brightness)
 {
 	/* MX seem contrast */
-	reg_write(gspca_dev->dev, 0x8651, brightness);
-	reg_write(gspca_dev->dev, 0x8652, brightness);
-	reg_write(gspca_dev->dev, 0x8653, brightness);
-	reg_write(gspca_dev->dev, 0x8654, brightness);
+	reg_write(gspca_dev, 0x8651, brightness);
+	reg_write(gspca_dev, 0x8652, brightness);
+	reg_write(gspca_dev, 0x8653, brightness);
+	reg_write(gspca_dev, 0x8654, brightness);
 }
 
 static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
diff --git a/drivers/media/usb/gspca/spca561.c b/drivers/media/usb/gspca/spca561.c
index d1db3d8f652..403d71cd65d 100644
--- a/drivers/media/usb/gspca/spca561.c
+++ b/drivers/media/usb/gspca/spca561.c
@@ -285,9 +285,10 @@ static const __u16 spca561_161rev12A_data2[][2] = {
 	{}
 };
 
-static void reg_w_val(struct usb_device *dev, __u16 index, __u8 value)
+static void reg_w_val(struct gspca_dev *gspca_dev, __u16 index, __u8 value)
 {
 	int ret;
+	struct usb_device *dev = gspca_dev->dev;
 
 	ret = usb_control_msg(dev, usb_sndctrlpipe(dev, 0),
 			      0,		/* request */
@@ -301,12 +302,11 @@ static void reg_w_val(struct usb_device *dev, __u16 index, __u8 value)
 static void write_vector(struct gspca_dev *gspca_dev,
 			const __u16 data[][2])
 {
-	struct usb_device *dev = gspca_dev->dev;
 	int i;
 
 	i = 0;
 	while (data[i][1] != 0) {
-		reg_w_val(dev, data[i][1], data[i][0]);
+		reg_w_val(gspca_dev, data[i][1], data[i][0]);
 		i++;
 	}
 }
@@ -339,9 +339,9 @@ static void i2c_write(struct gspca_dev *gspca_dev, __u16 value, __u16 reg)
 {
 	int retry = 60;
 
-	reg_w_val(gspca_dev->dev, 0x8801, reg);
-	reg_w_val(gspca_dev->dev, 0x8805, value);
-	reg_w_val(gspca_dev->dev, 0x8800, value >> 8);
+	reg_w_val(gspca_dev, 0x8801, reg);
+	reg_w_val(gspca_dev, 0x8805, value);
+	reg_w_val(gspca_dev, 0x8800, value >> 8);
 	do {
 		reg_r(gspca_dev, 0x8803, 1);
 		if (!gspca_dev->usb_buf[0])
@@ -355,9 +355,9 @@ static int i2c_read(struct gspca_dev *gspca_dev, __u16 reg, __u8 mode)
 	int retry = 60;
 	__u8 value;
 
-	reg_w_val(gspca_dev->dev, 0x8804, 0x92);
-	reg_w_val(gspca_dev->dev, 0x8801, reg);
-	reg_w_val(gspca_dev->dev, 0x8802, mode | 0x01);
+	reg_w_val(gspca_dev, 0x8804, 0x92);
+	reg_w_val(gspca_dev, 0x8801, reg);
+	reg_w_val(gspca_dev, 0x8802, mode | 0x01);
 	do {
 		reg_r(gspca_dev, 0x8803, 1);
 		if (!gspca_dev->usb_buf[0]) {
@@ -459,14 +459,13 @@ static int sd_init_72a(struct gspca_dev *gspca_dev)
 	write_sensor_72a(gspca_dev, rev72a_init_sensor1);
 	write_vector(gspca_dev, rev72a_init_data2);
 	write_sensor_72a(gspca_dev, rev72a_init_sensor2);
-	reg_w_val(gspca_dev->dev, 0x8112, 0x30);
+	reg_w_val(gspca_dev, 0x8112, 0x30);
 	return 0;
 }
 
 static void setbrightness(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	struct usb_device *dev = gspca_dev->dev;
 	__u16 reg;
 
 	if (sd->chip_revision == Rev012A)
@@ -474,16 +473,15 @@ static void setbrightness(struct gspca_dev *gspca_dev, s32 val)
 	else
 		reg = 0x8611;
 
-	reg_w_val(dev, reg + 0, val);		/* R */
-	reg_w_val(dev, reg + 1, val);		/* Gr */
-	reg_w_val(dev, reg + 2, val);		/* B */
-	reg_w_val(dev, reg + 3, val);		/* Gb */
+	reg_w_val(gspca_dev, reg + 0, val);		/* R */
+	reg_w_val(gspca_dev, reg + 1, val);		/* Gr */
+	reg_w_val(gspca_dev, reg + 2, val);		/* B */
+	reg_w_val(gspca_dev, reg + 3, val);		/* Gb */
 }
 
 static void setwhite(struct gspca_dev *gspca_dev, s32 white, s32 contrast)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	struct usb_device *dev = gspca_dev->dev;
 	__u8 blue, red;
 	__u16 reg;
 
@@ -496,11 +494,11 @@ static void setwhite(struct gspca_dev *gspca_dev, s32 white, s32 contrast)
 		reg = 0x8651;
 		red += contrast - 0x20;
 		blue += contrast - 0x20;
-		reg_w_val(dev, 0x8652, contrast + 0x20); /* Gr */
-		reg_w_val(dev, 0x8654, contrast + 0x20); /* Gb */
+		reg_w_val(gspca_dev, 0x8652, contrast + 0x20); /* Gr */
+		reg_w_val(gspca_dev, 0x8654, contrast + 0x20); /* Gb */
 	}
-	reg_w_val(dev, reg, red);
-	reg_w_val(dev, reg + 2, blue);
+	reg_w_val(gspca_dev, reg, red);
+	reg_w_val(gspca_dev, reg + 2, blue);
 }
 
 /* rev 12a only */
@@ -570,7 +568,6 @@ static void setautogain(struct gspca_dev *gspca_dev, s32 val)
 
 static int sd_start_12a(struct gspca_dev *gspca_dev)
 {
-	struct usb_device *dev = gspca_dev->dev;
 	int mode;
 	static const __u8 Reg8391[8] =
 		{0x92, 0x30, 0x20, 0x00, 0x0c, 0x00, 0x00, 0x00};
@@ -578,34 +575,33 @@ static int sd_start_12a(struct gspca_dev *gspca_dev)
 	mode = gspca_dev->cam.cam_mode[(int) gspca_dev->curr_mode].priv;
 	if (mode <= 1) {
 		/* Use compression on 320x240 and above */
-		reg_w_val(dev, 0x8500, 0x10 | mode);
+		reg_w_val(gspca_dev, 0x8500, 0x10 | mode);
 	} else {
 		/* I couldn't get the compression to work below 320x240
 		 * Fortunately at these resolutions the bandwidth
 		 * is sufficient to push raw frames at ~20fps */
-		reg_w_val(dev, 0x8500, mode);
+		reg_w_val(gspca_dev, 0x8500, mode);
 	}		/* -- qq@kuku.eu.org */
 
 	gspca_dev->usb_buf[0] = 0xaa;
 	gspca_dev->usb_buf[1] = 0x00;
 	reg_w_buf(gspca_dev, 0x8307, 2);
 	/* clock - lower 0x8X values lead to fps > 30 */
-	reg_w_val(gspca_dev->dev, 0x8700, 0x8a);
+	reg_w_val(gspca_dev, 0x8700, 0x8a);
 					/* 0x8f 0x85 0x27 clock */
-	reg_w_val(gspca_dev->dev, 0x8112, 0x1e | 0x20);
-	reg_w_val(gspca_dev->dev, 0x850b, 0x03);
+	reg_w_val(gspca_dev, 0x8112, 0x1e | 0x20);
+	reg_w_val(gspca_dev, 0x850b, 0x03);
 	memcpy(gspca_dev->usb_buf, Reg8391, 8);
 	reg_w_buf(gspca_dev, 0x8391, 8);
 	reg_w_buf(gspca_dev, 0x8390, 8);
 
 	/* Led ON (bit 3 -> 0 */
-	reg_w_val(gspca_dev->dev, 0x8114, 0x00);
+	reg_w_val(gspca_dev, 0x8114, 0x00);
 	return 0;
 }
 static int sd_start_72a(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	struct usb_device *dev = gspca_dev->dev;
 	int Clck;
 	int mode;
 
@@ -630,15 +626,15 @@ static int sd_start_72a(struct gspca_dev *gspca_dev)
 		Clck = 0x21;
 		break;
 	}
-	reg_w_val(dev, 0x8700, Clck);	/* 0x27 clock */
-	reg_w_val(dev, 0x8702, 0x81);
-	reg_w_val(dev, 0x8500, mode);	/* mode */
+	reg_w_val(gspca_dev, 0x8700, Clck);	/* 0x27 clock */
+	reg_w_val(gspca_dev, 0x8702, 0x81);
+	reg_w_val(gspca_dev, 0x8500, mode);	/* mode */
 	write_sensor_72a(gspca_dev, rev72a_init_sensor2);
 	setwhite(gspca_dev, v4l2_ctrl_g_ctrl(sd->hue),
 			v4l2_ctrl_g_ctrl(sd->contrast));
 /*	setbrightness(gspca_dev);	 * fixme: bad values */
 	setautogain(gspca_dev, v4l2_ctrl_g_ctrl(sd->autogain));
-	reg_w_val(dev, 0x8112, 0x10 | 0x20);
+	reg_w_val(gspca_dev, 0x8112, 0x10 | 0x20);
 	return 0;
 }
 
@@ -647,12 +643,12 @@ static void sd_stopN(struct gspca_dev *gspca_dev)
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	if (sd->chip_revision == Rev012A) {
-		reg_w_val(gspca_dev->dev, 0x8112, 0x0e);
+		reg_w_val(gspca_dev, 0x8112, 0x0e);
 		/* Led Off (bit 3 -> 1 */
-		reg_w_val(gspca_dev->dev, 0x8114, 0x08);
+		reg_w_val(gspca_dev, 0x8114, 0x08);
 	} else {
-		reg_w_val(gspca_dev->dev, 0x8112, 0x20);
-/*		reg_w_val(gspca_dev->dev, 0x8102, 0x00); ?? */
+		reg_w_val(gspca_dev, 0x8112, 0x20);
+/*		reg_w_val(gspca_dev, 0x8102, 0x00); ?? */
 	}
 }
 
@@ -736,7 +732,7 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 
 		/* This should never happen */
 		if (len < 2) {
-			PDEBUG(D_ERR, "Short SOF packet, ignoring");
+			PERR("Short SOF packet, ignoring");
 			gspca_dev->last_packet_type = DISCARD_PACKET;
 			return;
 		}
diff --git a/drivers/media/usb/gspca/sq905.c b/drivers/media/usb/gspca/sq905.c
index 1d99f10a3e1..a7ae0ec9fa9 100644
--- a/drivers/media/usb/gspca/sq905.c
+++ b/drivers/media/usb/gspca/sq905.c
@@ -387,7 +387,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	}
 
 	if (ret < 0) {
-		PDEBUG(D_ERR, "Start streaming command failed");
+		PERR("Start streaming command failed");
 		return ret;
 	}
 	/* Start the workqueue function to do the streaming */
diff --git a/drivers/media/usb/gspca/sq905c.c b/drivers/media/usb/gspca/sq905c.c
index 410cdcbb55d..acb19fb9a3d 100644
--- a/drivers/media/usb/gspca/sq905c.c
+++ b/drivers/media/usb/gspca/sq905c.c
@@ -215,13 +215,13 @@ static int sd_config(struct gspca_dev *gspca_dev,
 
 	ret = sq905c_command(gspca_dev, SQ905C_GET_ID, 0);
 	if (ret < 0) {
-		PDEBUG(D_ERR, "Get version command failed");
+		PERR("Get version command failed");
 		return ret;
 	}
 
 	ret = sq905c_read(gspca_dev, 0xf5, 0, 20);
 	if (ret < 0) {
-		PDEBUG(D_ERR, "Reading version command failed");
+		PERR("Reading version command failed");
 		return ret;
 	}
 	/* Note we leave out the usb id and the manufacturing date */
@@ -286,7 +286,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	}
 
 	if (ret < 0) {
-		PDEBUG(D_ERR, "Start streaming command failed");
+		PERR("Start streaming command failed");
 		return ret;
 	}
 	/* Start the workqueue function to do the streaming */
diff --git a/drivers/media/usb/gspca/sq930x.c b/drivers/media/usb/gspca/sq930x.c
index 7e8748b31e8..b10d0821111 100644
--- a/drivers/media/usb/gspca/sq930x.c
+++ b/drivers/media/usb/gspca/sq930x.c
@@ -541,13 +541,11 @@ static void ucbus_write(struct gspca_dev *gspca_dev,
 	if (gspca_dev->usb_err < 0)
 		return;
 
-#ifdef GSPCA_DEBUG
 	if ((batchsize - 1) * 3 > USB_BUF_SZ) {
-		pr_err("Bug: usb_buf overflow\n");
+		PERR("Bug: usb_buf overflow\n");
 		gspca_dev->usb_err = -ENOMEM;
 		return;
 	}
-#endif
 
 	for (;;) {
 		len = ncmds;
diff --git a/drivers/media/usb/gspca/stv0680.c b/drivers/media/usb/gspca/stv0680.c
index 67605272aaa..9c0827631b9 100644
--- a/drivers/media/usb/gspca/stv0680.c
+++ b/drivers/media/usb/gspca/stv0680.c
@@ -86,7 +86,7 @@ static int stv_sndctrl(struct gspca_dev *gspca_dev, int set, u8 req, u16 val,
 static int stv0680_handle_error(struct gspca_dev *gspca_dev, int ret)
 {
 	stv_sndctrl(gspca_dev, 0, 0x80, 0, 0x02); /* Get Last Error */
-	PDEBUG(D_ERR, "last error: %i,  command = 0x%x",
+	PERR("last error: %i,  command = 0x%x",
 	       gspca_dev->usb_buf[0], gspca_dev->usb_buf[1]);
 	return ret;
 }
@@ -98,7 +98,7 @@ static int stv0680_get_video_mode(struct gspca_dev *gspca_dev)
 	gspca_dev->usb_buf[0] = 0x0f;
 
 	if (stv_sndctrl(gspca_dev, 0, 0x87, 0, 0x08) != 0x08) {
-		PDEBUG(D_ERR, "Get_Camera_Mode failed");
+		PERR("Get_Camera_Mode failed");
 		return stv0680_handle_error(gspca_dev, -EIO);
 	}
 
@@ -116,13 +116,13 @@ static int stv0680_set_video_mode(struct gspca_dev *gspca_dev, u8 mode)
 	gspca_dev->usb_buf[0] = mode;
 
 	if (stv_sndctrl(gspca_dev, 3, 0x07, 0x0100, 0x08) != 0x08) {
-		PDEBUG(D_ERR, "Set_Camera_Mode failed");
+		PERR("Set_Camera_Mode failed");
 		return stv0680_handle_error(gspca_dev, -EIO);
 	}
 
 	/* Verify we got what we've asked for */
 	if (stv0680_get_video_mode(gspca_dev) != mode) {
-		PDEBUG(D_ERR, "Error setting camera video mode!");
+		PERR("Error setting camera video mode!");
 		return -EIO;
 	}
 
@@ -146,7 +146,7 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	/* ping camera to be sure STV0680 is present */
 	if (stv_sndctrl(gspca_dev, 0, 0x88, 0x5678, 0x02) != 0x02 ||
 	    gspca_dev->usb_buf[0] != 0x56 || gspca_dev->usb_buf[1] != 0x78) {
-		PDEBUG(D_ERR, "STV(e): camera ping failed!!");
+		PERR("STV(e): camera ping failed!!");
 		return stv0680_handle_error(gspca_dev, -ENODEV);
 	}
 
@@ -156,7 +156,7 @@ static int sd_config(struct gspca_dev *gspca_dev,
 
 	if (stv_sndctrl(gspca_dev, 2, 0x06, 0x0200, 0x22) != 0x22 ||
 	    gspca_dev->usb_buf[7] != 0xa0 || gspca_dev->usb_buf[8] != 0x23) {
-		PDEBUG(D_ERR, "Could not get descriptor 0200.");
+		PERR("Could not get descriptor 0200.");
 		return stv0680_handle_error(gspca_dev, -ENODEV);
 	}
 	if (stv_sndctrl(gspca_dev, 0, 0x8a, 0, 0x02) != 0x02)
@@ -167,7 +167,7 @@ static int sd_config(struct gspca_dev *gspca_dev,
 		return stv0680_handle_error(gspca_dev, -ENODEV);
 
 	if (!(gspca_dev->usb_buf[7] & 0x09)) {
-		PDEBUG(D_ERR, "Camera supports neither CIF nor QVGA mode");
+		PERR("Camera supports neither CIF nor QVGA mode");
 		return -ENODEV;
 	}
 	if (gspca_dev->usb_buf[7] & 0x01)
diff --git a/drivers/media/usb/gspca/stv06xx/stv06xx.c b/drivers/media/usb/gspca/stv06xx/stv06xx.c
index 657160b4a1f..55ee7a61c67 100644
--- a/drivers/media/usb/gspca/stv06xx/stv06xx.c
+++ b/drivers/media/usb/gspca/stv06xx/stv06xx.c
@@ -42,8 +42,10 @@ static bool dump_sensor;
 int stv06xx_write_bridge(struct sd *sd, u16 address, u16 i2c_data)
 {
 	int err;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	struct usb_device *udev = sd->gspca_dev.dev;
 	__u8 *buf = sd->gspca_dev.usb_buf;
+
 	u8 len = (i2c_data > 0xff) ? 2 : 1;
 
 	buf[0] = i2c_data & 0xff;
@@ -62,6 +64,7 @@ int stv06xx_write_bridge(struct sd *sd, u16 address, u16 i2c_data)
 int stv06xx_read_bridge(struct sd *sd, u16 address, u8 *i2c_data)
 {
 	int err;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	struct usb_device *udev = sd->gspca_dev.dev;
 	__u8 *buf = sd->gspca_dev.usb_buf;
 
@@ -110,6 +113,7 @@ static int stv06xx_write_sensor_finish(struct sd *sd)
 int stv06xx_write_sensor_bytes(struct sd *sd, const u8 *data, u8 len)
 {
 	int err, i, j;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	struct usb_device *udev = sd->gspca_dev.dev;
 	__u8 *buf = sd->gspca_dev.usb_buf;
 
@@ -139,6 +143,7 @@ int stv06xx_write_sensor_bytes(struct sd *sd, const u8 *data, u8 len)
 int stv06xx_write_sensor_words(struct sd *sd, const u16 *data, u8 len)
 {
 	int err, i, j;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	struct usb_device *udev = sd->gspca_dev.dev;
 	__u8 *buf = sd->gspca_dev.usb_buf;
 
@@ -170,6 +175,7 @@ int stv06xx_write_sensor_words(struct sd *sd, const u16 *data, u8 len)
 int stv06xx_read_sensor(struct sd *sd, const u8 address, u16 *value)
 {
 	int err;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	struct usb_device *udev = sd->gspca_dev.dev;
 	__u8 *buf = sd->gspca_dev.usb_buf;
 
@@ -283,7 +289,7 @@ static int stv06xx_start(struct gspca_dev *gspca_dev)
 	intf = usb_ifnum_to_if(sd->gspca_dev.dev, sd->gspca_dev.iface);
 	alt = usb_altnum_to_altsetting(intf, sd->gspca_dev.alt);
 	if (!alt) {
-		PDEBUG(D_ERR, "Couldn't get altsetting");
+		PERR("Couldn't get altsetting");
 		return -EIO;
 	}
 
@@ -341,7 +347,7 @@ static int stv06xx_isoc_nego(struct gspca_dev *gspca_dev)
 
 	ret = usb_set_interface(gspca_dev->dev, gspca_dev->iface, 1);
 	if (ret < 0)
-		PDEBUG(D_ERR|D_STREAM, "set alt 1 err %d", ret);
+		PERR("set alt 1 err %d", ret);
 
 	return ret;
 }
@@ -406,7 +412,7 @@ static void stv06xx_pkt_scan(struct gspca_dev *gspca_dev,
 		len -= 4;
 
 		if (len < chunk_len) {
-			PDEBUG(D_ERR, "URB packet length is smaller"
+			PERR("URB packet length is smaller"
 				" than the specified chunk length");
 			gspca_dev->last_packet_type = DISCARD_PACKET;
 			return;
@@ -449,7 +455,7 @@ frame_data:
 				sd->to_skip = gspca_dev->width * 4;
 
 			if (chunk_len)
-				PDEBUG(D_ERR, "Chunk length is "
+				PERR("Chunk length is "
 					      "non-zero on a SOF");
 			break;
 
@@ -463,7 +469,7 @@ frame_data:
 					NULL, 0);
 
 			if (chunk_len)
-				PDEBUG(D_ERR, "Chunk length is "
+				PERR("Chunk length is "
 					      "non-zero on a EOF");
 			break;
 
@@ -596,7 +602,6 @@ MODULE_DEVICE_TABLE(usb, device_table);
 static int sd_probe(struct usb_interface *intf,
 			const struct usb_device_id *id)
 {
-	PDEBUG(D_PROBE, "Probing for a stv06xx device");
 	return gspca_dev_probe(intf, id, &sd_desc, sizeof(struct sd),
 			       THIS_MODULE);
 }
diff --git a/drivers/media/usb/gspca/stv06xx/stv06xx_hdcs.c b/drivers/media/usb/gspca/stv06xx/stv06xx_hdcs.c
index 06fa54c5efb..2220b70d47e 100644
--- a/drivers/media/usb/gspca/stv06xx/stv06xx_hdcs.c
+++ b/drivers/media/usb/gspca/stv06xx/stv06xx_hdcs.c
@@ -255,7 +255,7 @@ static int hdcs_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 		if (err < 0)
 			return err;
 	}
-	PDEBUG(D_V4L2, "Writing exposure %d, rowexp %d, srowexp %d",
+	PDEBUG(D_CONF, "Writing exposure %d, rowexp %d, srowexp %d",
 	       val, rowexp, srowexp);
 	return err;
 }
@@ -280,7 +280,7 @@ static int hdcs_set_gains(struct sd *sd, u8 g)
 
 static int hdcs_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 {
-	PDEBUG(D_V4L2, "Writing gain %d", val);
+	PDEBUG(D_CONF, "Writing gain %d", val);
 	return hdcs_set_gains((struct sd *) gspca_dev,
 			       val & 0xff);
 }
@@ -467,6 +467,8 @@ static int hdcs_probe_1020(struct sd *sd)
 
 static int hdcs_start(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
+
 	PDEBUG(D_STREAM, "Starting stream");
 
 	return hdcs_set_state(sd, HDCS_STATE_RUN);
@@ -474,6 +476,8 @@ static int hdcs_start(struct sd *sd)
 
 static int hdcs_stop(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
+
 	PDEBUG(D_STREAM, "Halting stream");
 
 	return hdcs_set_state(sd, HDCS_STATE_SLEEP);
diff --git a/drivers/media/usb/gspca/stv06xx/stv06xx_pb0100.c b/drivers/media/usb/gspca/stv06xx/stv06xx_pb0100.c
index cdfc3d05ab6..8206b774330 100644
--- a/drivers/media/usb/gspca/stv06xx/stv06xx_pb0100.c
+++ b/drivers/media/usb/gspca/stv06xx/stv06xx_pb0100.c
@@ -190,6 +190,7 @@ static int pb0100_start(struct sd *sd)
 	int err, packet_size, max_packet_size;
 	struct usb_host_interface *alt;
 	struct usb_interface *intf;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	struct cam *cam = &sd->gspca_dev.cam;
 	u32 mode = cam->cam_mode[sd->gspca_dev.curr_mode].priv;
 
@@ -239,6 +240,7 @@ static int pb0100_start(struct sd *sd)
 
 static int pb0100_stop(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int err;
 
 	err = stv06xx_write_sensor(sd, PB_ABORTFRAME, 1);
@@ -334,7 +336,7 @@ static int pb0100_set_gain(struct gspca_dev *gspca_dev, __s32 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);
+	PDEBUG(D_CONF, "Set green gain to %d, status: %d", val, err);
 
 	if (!err)
 		err = pb0100_set_red_balance(gspca_dev, ctrls->red->val);
@@ -357,7 +359,7 @@ static int pb0100_set_red_balance(struct gspca_dev *gspca_dev, __s32 val)
 		val = 255;
 
 	err = stv06xx_write_sensor(sd, PB_RGAIN, val);
-	PDEBUG(D_V4L2, "Set red gain to %d, status: %d", val, err);
+	PDEBUG(D_CONF, "Set red gain to %d, status: %d", val, err);
 
 	return err;
 }
@@ -375,7 +377,7 @@ static int pb0100_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val)
 		val = 255;
 
 	err = stv06xx_write_sensor(sd, PB_BGAIN, val);
-	PDEBUG(D_V4L2, "Set blue gain to %d, status: %d", val, err);
+	PDEBUG(D_CONF, "Set blue gain to %d, status: %d", val, err);
 
 	return err;
 }
@@ -386,7 +388,7 @@ static int pb0100_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 	int err;
 
 	err = stv06xx_write_sensor(sd, PB_RINTTIME, val);
-	PDEBUG(D_V4L2, "Set exposure to %d, status: %d", val, err);
+	PDEBUG(D_CONF, "Set exposure to %d, status: %d", val, err);
 
 	return err;
 }
@@ -406,7 +408,7 @@ static int pb0100_set_autogain(struct gspca_dev *gspca_dev, __s32 val)
 		val = 0;
 
 	err = stv06xx_write_sensor(sd, PB_EXPGAIN, val);
-	PDEBUG(D_V4L2, "Set autogain to %d (natural: %d), status: %d",
+	PDEBUG(D_CONF, "Set autogain to %d (natural: %d), status: %d",
 	       val, ctrls->natural->val, err);
 
 	return err;
@@ -428,7 +430,7 @@ static int pb0100_set_autogain_target(struct gspca_dev *gspca_dev, __s32 val)
 	if (!err)
 		err = stv06xx_write_sensor(sd, PB_R22, darkpixels);
 
-	PDEBUG(D_V4L2, "Set autogain target to %d, status: %d", val, err);
+	PDEBUG(D_CONF, "Set autogain target to %d, status: %d", val, err);
 
 	return err;
 }
diff --git a/drivers/media/usb/gspca/stv06xx/stv06xx_st6422.c b/drivers/media/usb/gspca/stv06xx/stv06xx_st6422.c
index 8a57990dfe0..515a9e12165 100644
--- a/drivers/media/usb/gspca/stv06xx/stv06xx_st6422.c
+++ b/drivers/media/usb/gspca/stv06xx/stv06xx_st6422.c
@@ -279,6 +279,8 @@ static int st6422_start(struct sd *sd)
 
 static int st6422_stop(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
+
 	PDEBUG(D_STREAM, "Halting stream");
 
 	return 0;
diff --git a/drivers/media/usb/gspca/stv06xx/stv06xx_vv6410.c b/drivers/media/usb/gspca/stv06xx/stv06xx_vv6410.c
index e95fa8997d2..bf3e5c317a2 100644
--- a/drivers/media/usb/gspca/stv06xx/stv06xx_vv6410.c
+++ b/drivers/media/usb/gspca/stv06xx/stv06xx_vv6410.c
@@ -131,6 +131,7 @@ static int vv6410_init(struct sd *sd)
 static int vv6410_start(struct sd *sd)
 {
 	int err;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	struct cam *cam = &sd->gspca_dev.cam;
 	u32 priv = cam->cam_mode[sd->gspca_dev.curr_mode].priv;
 
@@ -163,6 +164,7 @@ static int vv6410_start(struct sd *sd)
 
 static int vv6410_stop(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int err;
 
 	/* Turn off LED */
@@ -208,7 +210,7 @@ static int vv6410_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
 	else
 		i2c_data &= ~VV6410_HFLIP;
 
-	PDEBUG(D_V4L2, "Set horizontal flip to %d", val);
+	PDEBUG(D_CONF, "Set horizontal flip to %d", val);
 	err = stv06xx_write_sensor(sd, VV6410_DATAFORMAT, i2c_data);
 
 	return (err < 0) ? err : 0;
@@ -229,7 +231,7 @@ static int vv6410_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
 	else
 		i2c_data &= ~VV6410_VFLIP;
 
-	PDEBUG(D_V4L2, "Set vertical flip to %d", val);
+	PDEBUG(D_CONF, "Set vertical flip to %d", val);
 	err = stv06xx_write_sensor(sd, VV6410_DATAFORMAT, i2c_data);
 
 	return (err < 0) ? err : 0;
@@ -240,7 +242,7 @@ static int vv6410_set_analog_gain(struct gspca_dev *gspca_dev, __s32 val)
 	int err;
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	PDEBUG(D_V4L2, "Set analog gain to %d", val);
+	PDEBUG(D_CONF, "Set analog gain to %d", val);
 	err = stv06xx_write_sensor(sd, VV6410_ANALOGGAIN, 0xf0 | (val & 0xf));
 
 	return (err < 0) ? err : 0;
@@ -257,7 +259,7 @@ static int vv6410_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 	fine = val % VV6410_CIF_LINELENGTH;
 	coarse = min(512, val / VV6410_CIF_LINELENGTH);
 
-	PDEBUG(D_V4L2, "Set coarse exposure to %d, fine expsure to %d",
+	PDEBUG(D_CONF, "Set coarse exposure to %d, fine expsure to %d",
 	       coarse, fine);
 
 	err = stv06xx_write_sensor(sd, VV6410_FINEH, fine >> 8);
diff --git a/drivers/media/usb/gspca/sunplus.c b/drivers/media/usb/gspca/sunplus.c
index 9ccfcb1c647..af8767a9bd4 100644
--- a/drivers/media/usb/gspca/sunplus.c
+++ b/drivers/media/usb/gspca/sunplus.c
@@ -251,12 +251,10 @@ static void reg_r(struct gspca_dev *gspca_dev,
 {
 	int ret;
 
-#ifdef GSPCA_DEBUG
 	if (len > USB_BUF_SZ) {
-		pr_err("reg_r: buffer overflow\n");
+		PERR("reg_r: buffer overflow\n");
 		return;
 	}
-#endif
 	if (gspca_dev->usb_err < 0)
 		return;
 	ret = usb_control_msg(gspca_dev->dev,
@@ -357,12 +355,14 @@ static void spca504_acknowledged_command(struct gspca_dev *gspca_dev,
 	PDEBUG(D_FRAM, "after wait 0x%04x", gspca_dev->usb_buf[0]);
 }
 
-#ifdef GSPCA_DEBUG
 static void spca504_read_info(struct gspca_dev *gspca_dev)
 {
 	int i;
 	u8 info[6];
 
+	if (gspca_debug < D_STREAM)
+		return;
+
 	for (i = 0; i < 6; i++) {
 		reg_r(gspca_dev, 0, i, 1);
 		info[i] = gspca_dev->usb_buf[0];
@@ -373,7 +373,6 @@ static void spca504_read_info(struct gspca_dev *gspca_dev)
 		info[0], info[1], info[2],
 		info[3], info[4], info[5]);
 }
-#endif
 
 static void spca504A_acknowledged_command(struct gspca_dev *gspca_dev,
 			u8 req,
@@ -432,11 +431,13 @@ static void spca504B_WaitCmdStatus(struct gspca_dev *gspca_dev)
 	}
 }
 
-#ifdef GSPCA_DEBUG
 static void spca50x_GetFirmware(struct gspca_dev *gspca_dev)
 {
 	u8 *data;
 
+	if (gspca_debug < D_STREAM)
+		return;
+
 	data = gspca_dev->usb_buf;
 	reg_r(gspca_dev, 0x20, 0, 5);
 	PDEBUG(D_STREAM, "FirmWare: %d %d %d %d %d",
@@ -444,7 +445,6 @@ static void spca50x_GetFirmware(struct gspca_dev *gspca_dev)
 	reg_r(gspca_dev, 0x23, 0, 64);
 	reg_r(gspca_dev, 0x23, 1, 64);
 }
-#endif
 
 static void spca504B_SetSizeType(struct gspca_dev *gspca_dev)
 {
@@ -457,9 +457,8 @@ static void spca504B_SetSizeType(struct gspca_dev *gspca_dev)
 		reg_w_riv(gspca_dev, 0x31, 0, 0);
 		spca504B_WaitCmdStatus(gspca_dev);
 		spca504B_PollingDataReady(gspca_dev);
-#ifdef GSPCA_DEBUG
 		spca50x_GetFirmware(gspca_dev);
-#endif
+
 		reg_w_1(gspca_dev, 0x24, 0, 8, 2);		/* type */
 		reg_r(gspca_dev, 0x24, 8, 1);
 
@@ -645,14 +644,10 @@ static int sd_init(struct gspca_dev *gspca_dev)
 		/* fall thru */
 	case BRIDGE_SPCA533:
 		spca504B_PollingDataReady(gspca_dev);
-#ifdef GSPCA_DEBUG
 		spca50x_GetFirmware(gspca_dev);
-#endif
 		break;
 	case BRIDGE_SPCA536:
-#ifdef GSPCA_DEBUG
 		spca50x_GetFirmware(gspca_dev);
-#endif
 		reg_r(gspca_dev, 0x00, 0x5002, 1);
 		reg_w_1(gspca_dev, 0x24, 0, 0, 0);
 		reg_r(gspca_dev, 0x24, 0, 1);
@@ -678,9 +673,7 @@ static int sd_init(struct gspca_dev *gspca_dev)
 /*	case BRIDGE_SPCA504: */
 		PDEBUG(D_STREAM, "Opening SPCA504");
 		if (sd->subtype == AiptekMiniPenCam13) {
-#ifdef GSPCA_DEBUG
 			spca504_read_info(gspca_dev);
-#endif
 
 			/* Set AE AWB Banding Type 3-> 50Hz 2-> 60Hz */
 			spca504A_acknowledged_command(gspca_dev, 0x24,
@@ -752,9 +745,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		break;
 	case BRIDGE_SPCA504:
 		if (sd->subtype == AiptekMiniPenCam13) {
-#ifdef GSPCA_DEBUG
 			spca504_read_info(gspca_dev);
-#endif
 
 			/* Set AE AWB Banding Type 3-> 50Hz 2-> 60Hz */
 			spca504A_acknowledged_command(gspca_dev, 0x24,
@@ -766,9 +757,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 							0, 0, 0x9d, 1);
 		} else {
 			spca504_acknowledged_command(gspca_dev, 0x24, 8, 3);
-#ifdef GSPCA_DEBUG
 			spca504_read_info(gspca_dev);
-#endif
 			spca504_acknowledged_command(gspca_dev, 0x24, 8, 3);
 			spca504_acknowledged_command(gspca_dev, 0x24, 0, 0);
 		}
diff --git a/drivers/media/usb/gspca/vc032x.c b/drivers/media/usb/gspca/vc032x.c
index e50079503d9..c00ac57de51 100644
--- a/drivers/media/usb/gspca/vc032x.c
+++ b/drivers/media/usb/gspca/vc032x.c
@@ -2927,7 +2927,6 @@ static void reg_r(struct gspca_dev *gspca_dev,
 		  u16 len)
 {
 	reg_r_i(gspca_dev, req, index, len);
-#ifdef GSPCA_DEBUG
 	if (gspca_dev->usb_err < 0)
 		return;
 	if (len == 1)
@@ -2936,7 +2935,6 @@ static void reg_r(struct gspca_dev *gspca_dev,
 	else
 		PDEBUG(D_USBI, "GET %02x 0001 %04x %*ph",
 				req, index, 3, gspca_dev->usb_buf);
-#endif
 }
 
 static void reg_w_i(struct gspca_dev *gspca_dev,
@@ -2964,11 +2962,9 @@ static void reg_w(struct gspca_dev *gspca_dev,
 			    u16 value,
 			    u16 index)
 {
-#ifdef GSPCA_DEBUG
 	if (gspca_dev->usb_err < 0)
 		return;
 	PDEBUG(D_USBO, "SET %02x %04x %04x", req, value, index);
-#endif
 	reg_w_i(gspca_dev, req, value, index);
 }
 
@@ -3044,8 +3040,7 @@ static int vc032x_probe_sensor(struct gspca_dev *gspca_dev)
 		if (value == 0 && ptsensor_info->IdAdd == 0x82)
 			value = read_sensor_register(gspca_dev, 0x83);
 		if (value != 0) {
-			PDEBUG(D_ERR|D_PROBE, "Sensor ID %04x (%d)",
-				value, i);
+			PDEBUG(D_PROBE, "Sensor ID %04x (%d)", value, i);
 			if (value == ptsensor_info->VpId)
 				return ptsensor_info->sensorId;
 
@@ -3069,14 +3064,12 @@ static void i2c_write(struct gspca_dev *gspca_dev,
 {
 	int retry;
 
-#ifdef GSPCA_DEBUG
 	if (gspca_dev->usb_err < 0)
 		return;
 	if (size == 1)
 		PDEBUG(D_USBO, "i2c_w %02x %02x", reg, *val);
 	else
 		PDEBUG(D_USBO, "i2c_w %02x %02x%02x", reg, *val, val[1]);
-#endif
 	reg_r_i(gspca_dev, 0xa1, 0xb33f, 1);
 /*fixme:should check if (!(gspca_dev->usb_buf[0] & 0x02)) error*/
 	reg_w_i(gspca_dev, 0xa0, size, 0xb334);
diff --git a/drivers/media/usb/gspca/w996Xcf.c b/drivers/media/usb/gspca/w996Xcf.c
index 9e3a909e0a0..2165da0c7ce 100644
--- a/drivers/media/usb/gspca/w996Xcf.c
+++ b/drivers/media/usb/gspca/w996Xcf.c
@@ -232,6 +232,7 @@ static void w9968cf_smbus_write_nack(struct sd *sd)
 
 static void w9968cf_smbus_read_ack(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int sda;
 
 	/* Ensure SDA is high before raising clock to avoid a spurious stop */
@@ -248,6 +249,7 @@ static void w9968cf_smbus_read_ack(struct sd *sd)
 /* SMBus protocol: S Addr Wr [A] Subaddr [A] Value [A] P */
 static void w9968cf_i2c_w(struct sd *sd, u8 reg, u8 value)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	u16* data = (u16 *)sd->gspca_dev.usb_buf;
 
 	data[0] = 0x082f | ((sd->sensor_addr & 0x80) ? 0x1500 : 0x0);
@@ -297,6 +299,7 @@ static void w9968cf_i2c_w(struct sd *sd, u8 reg, u8 value)
 /* SMBus protocol: S Addr Wr [A] Subaddr [A] P S Addr+1 Rd [A] [Value] NA P */
 static int w9968cf_i2c_r(struct sd *sd, u8 reg)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int ret = 0;
 	u8 value;
 
@@ -326,7 +329,7 @@ static int w9968cf_i2c_r(struct sd *sd, u8 reg)
 		ret = value;
 		PDEBUG(D_USBI, "i2c [0x%02X] -> 0x%02X", reg, value);
 	} else
-		PDEBUG(D_ERR, "i2c read [0x%02x] failed", reg);
+		PERR("i2c read [0x%02x] failed", reg);
 
 	return ret;
 }
diff --git a/drivers/media/usb/gspca/zc3xx.c b/drivers/media/usb/gspca/zc3xx.c
index a8dc421f9f1..cbfc2f92142 100644
--- a/drivers/media/usb/gspca/zc3xx.c
+++ b/drivers/media/usb/gspca/zc3xx.c
@@ -6259,12 +6259,11 @@ static int vga_3wr_probe(struct gspca_dev *gspca_dev)
 	retword |= i2c_read(gspca_dev, 0x01);		/* ID 1 */
 	PDEBUG(D_PROBE, "probe 3wr vga 2 0x%04x", retword);
 	if (retword == 0x2030) {
-#ifdef GSPCA_DEBUG
 		u8 retbyte;
 
 		retbyte = i2c_read(gspca_dev, 0x02);	/* revision number */
 		PDEBUG(D_PROBE, "sensor PO2030 rev 0x%02x", retbyte);
-#endif
+
 		send_unknown(gspca_dev, SENSOR_PO2030);
 		return retword;
 	}
diff --git a/drivers/media/usb/hdpvr/hdpvr-core.c b/drivers/media/usb/hdpvr/hdpvr-core.c
index 5c619353639..8247c19d626 100644
--- a/drivers/media/usb/hdpvr/hdpvr-core.c
+++ b/drivers/media/usb/hdpvr/hdpvr-core.c
@@ -174,6 +174,7 @@ static int device_authorization(struct hdpvr_device *dev)
 	case HDPVR_FIRMWARE_VERSION_AC3:
 	case HDPVR_FIRMWARE_VERSION_0X12:
 	case HDPVR_FIRMWARE_VERSION_0X15:
+	case HDPVR_FIRMWARE_VERSION_0X1E:
 		dev->flags |= HDPVR_FLAG_AC3_CAP;
 		break;
 	default:
@@ -196,6 +197,7 @@ static int device_authorization(struct hdpvr_device *dev)
 	hex_dump_to_buffer(response, 8, 16, 1, print_buf, 5*buf_size+1, 0);
 	v4l2_dbg(MSG_INFO, hdpvr_debug, &dev->v4l2_dev, " response: %s\n",
 		 print_buf);
+	kfree(print_buf);
 #endif
 
 	msleep(100);
@@ -385,12 +387,6 @@ static int hdpvr_probe(struct usb_interface *interface,
 	}
 	mutex_unlock(&dev->io_mutex);
 
-	if (hdpvr_register_videodev(dev, &interface->dev,
-				    video_nr[atomic_inc_return(&dev_nr)])) {
-		v4l2_err(&dev->v4l2_dev, "registering videodev failed\n");
-		goto error;
-	}
-
 #if IS_ENABLED(CONFIG_I2C)
 	retval = hdpvr_register_i2c_adapter(dev);
 	if (retval < 0) {
@@ -413,6 +409,13 @@ static int hdpvr_probe(struct usb_interface *interface,
 	}
 #endif
 
+	retval = hdpvr_register_videodev(dev, &interface->dev,
+				    video_nr[atomic_inc_return(&dev_nr)]);
+	if (retval < 0) {
+		v4l2_err(&dev->v4l2_dev, "registering videodev failed\n");
+		goto error;
+	}
+
 	/* let the user know what node this device is now attached to */
 	v4l2_info(&dev->v4l2_dev, "device now attached to %s\n",
 		  video_device_node_name(dev->video_dev));
diff --git a/drivers/media/usb/hdpvr/hdpvr-video.c b/drivers/media/usb/hdpvr/hdpvr-video.c
index da6b7791222..774ba0e820b 100644
--- a/drivers/media/usb/hdpvr/hdpvr-video.c
+++ b/drivers/media/usb/hdpvr/hdpvr-video.c
@@ -10,6 +10,7 @@
  */
 
 #include <linux/kernel.h>
+#include <linux/kconfig.h>
 #include <linux/errno.h>
 #include <linux/init.h>
 #include <linux/slab.h>
@@ -20,9 +21,11 @@
 #include <linux/workqueue.h>
 
 #include <linux/videodev2.h>
+#include <linux/v4l2-dv-timings.h>
 #include <media/v4l2-dev.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
 #include "hdpvr.h"
 
 #define BULK_URB_TIMEOUT   90 /* 0.09 seconds */
@@ -34,8 +37,23 @@
 			 list_size(&dev->free_buff_list),		\
 			 list_size(&dev->rec_buff_list)); }
 
+static const struct v4l2_dv_timings hdpvr_dv_timings[] = {
+	V4L2_DV_BT_CEA_720X480I59_94,
+	V4L2_DV_BT_CEA_720X576I50,
+	V4L2_DV_BT_CEA_720X480P59_94,
+	V4L2_DV_BT_CEA_720X576P50,
+	V4L2_DV_BT_CEA_1280X720P50,
+	V4L2_DV_BT_CEA_1280X720P60,
+	V4L2_DV_BT_CEA_1920X1080I50,
+	V4L2_DV_BT_CEA_1920X1080I60,
+};
+
+/* Use 480i59 as the default timings */
+#define HDPVR_DEF_DV_TIMINGS_IDX (0)
+
 struct hdpvr_fh {
-	struct hdpvr_device	*dev;
+	struct v4l2_fh fh;
+	bool legacy_mode;
 };
 
 static uint list_size(struct list_head *list)
@@ -359,53 +377,29 @@ static int hdpvr_stop_streaming(struct hdpvr_device *dev)
 
 static int hdpvr_open(struct file *file)
 {
-	struct hdpvr_device *dev;
-	struct hdpvr_fh *fh;
-	int retval = -ENOMEM;
-
-	dev = (struct hdpvr_device *)video_get_drvdata(video_devdata(file));
-	if (!dev) {
-		pr_err("open failing with with ENODEV\n");
-		retval = -ENODEV;
-		goto err;
-	}
-
-	fh = kzalloc(sizeof(struct hdpvr_fh), GFP_KERNEL);
-	if (!fh) {
-		v4l2_err(&dev->v4l2_dev, "Out of memory\n");
-		goto err;
-	}
-	/* lock the device to allow correctly handling errors
-	 * in resumption */
-	mutex_lock(&dev->io_mutex);
-	dev->open_count++;
-	mutex_unlock(&dev->io_mutex);
+	struct hdpvr_fh *fh = kzalloc(sizeof(*fh), GFP_KERNEL);
 
-	fh->dev = dev;
-
-	/* save our object in the file's private structure */
+	if (fh == NULL)
+		return -ENOMEM;
+	fh->legacy_mode = true;
+	v4l2_fh_init(&fh->fh, video_devdata(file));
+	v4l2_fh_add(&fh->fh);
 	file->private_data = fh;
-
-	retval = 0;
-err:
-	return retval;
+	return 0;
 }
 
 static int hdpvr_release(struct file *file)
 {
-	struct hdpvr_fh		*fh  = file->private_data;
-	struct hdpvr_device	*dev = fh->dev;
-
-	if (!dev)
-		return -ENODEV;
+	struct hdpvr_device *dev = video_drvdata(file);
 
 	mutex_lock(&dev->io_mutex);
-	if (!(--dev->open_count) && dev->status == STATUS_STREAMING)
+	if (file->private_data == dev->owner) {
 		hdpvr_stop_streaming(dev);
-
+		dev->owner = NULL;
+	}
 	mutex_unlock(&dev->io_mutex);
 
-	return 0;
+	return v4l2_fh_release(file);
 }
 
 /*
@@ -415,8 +409,7 @@ static int hdpvr_release(struct file *file)
 static ssize_t hdpvr_read(struct file *file, char __user *buffer, size_t count,
 			  loff_t *pos)
 {
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
+	struct hdpvr_device *dev = video_drvdata(file);
 	struct hdpvr_buffer *buf = NULL;
 	struct urb *urb;
 	unsigned int ret = 0;
@@ -425,9 +418,6 @@ static ssize_t hdpvr_read(struct file *file, char __user *buffer, size_t count,
 	if (*pos)
 		return -ESPIPE;
 
-	if (!dev)
-		return -ENODEV;
-
 	mutex_lock(&dev->io_mutex);
 	if (dev->status == STATUS_IDLE) {
 		if (hdpvr_start_streaming(dev)) {
@@ -439,6 +429,7 @@ static ssize_t hdpvr_read(struct file *file, char __user *buffer, size_t count,
 			mutex_unlock(&dev->io_mutex);
 			goto err;
 		}
+		dev->owner = file->private_data;
 		print_buffer_status();
 	}
 	mutex_unlock(&dev->io_mutex);
@@ -516,23 +507,23 @@ err:
 
 static unsigned int hdpvr_poll(struct file *filp, poll_table *wait)
 {
+	unsigned long req_events = poll_requested_events(wait);
 	struct hdpvr_buffer *buf = NULL;
-	struct hdpvr_fh *fh = filp->private_data;
-	struct hdpvr_device *dev = fh->dev;
-	unsigned int mask = 0;
+	struct hdpvr_device *dev = video_drvdata(filp);
+	unsigned int mask = v4l2_ctrl_poll(filp, wait);
 
-	mutex_lock(&dev->io_mutex);
+	if (!(req_events & (POLLIN | POLLRDNORM)))
+		return mask;
 
-	if (!video_is_registered(dev->video_dev)) {
-		mutex_unlock(&dev->io_mutex);
-		return -EIO;
-	}
+	mutex_lock(&dev->io_mutex);
 
 	if (dev->status == STATUS_IDLE) {
 		if (hdpvr_start_streaming(dev)) {
 			v4l2_dbg(MSG_BUFFER, hdpvr_debug, &dev->v4l2_dev,
 				 "start_streaming failed\n");
 			dev->status = STATUS_IDLE;
+		} else {
+			dev->owner = filp->private_data;
 		}
 
 		print_buffer_status();
@@ -574,36 +565,188 @@ static int vidioc_querycap(struct file *file, void  *priv,
 	strcpy(cap->driver, "hdpvr");
 	strcpy(cap->card, "Hauppauge HD PVR");
 	usb_make_path(dev->udev, cap->bus_info, sizeof(cap->bus_info));
-	cap->capabilities =     V4L2_CAP_VIDEO_CAPTURE |
-				V4L2_CAP_AUDIO         |
-				V4L2_CAP_READWRITE;
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_AUDIO |
+			    V4L2_CAP_READWRITE;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
-static int vidioc_s_std(struct file *file, void *private_data,
-			v4l2_std_id *std)
+static int vidioc_s_std(struct file *file, void *_fh,
+			v4l2_std_id std)
 {
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
+	struct hdpvr_device *dev = video_drvdata(file);
+	struct hdpvr_fh *fh = _fh;
 	u8 std_type = 1;
 
-	if (*std & (V4L2_STD_NTSC | V4L2_STD_PAL_60))
+	if (!fh->legacy_mode && dev->options.video_input == HDPVR_COMPONENT)
+		return -ENODATA;
+	if (dev->status != STATUS_IDLE)
+		return -EBUSY;
+	if (std & V4L2_STD_525_60)
 		std_type = 0;
+	dev->cur_std = std;
+	dev->width = 720;
+	dev->height = std_type ? 576 : 480;
 
 	return hdpvr_config_call(dev, CTRL_VIDEO_STD_TYPE, std_type);
 }
 
+static int vidioc_g_std(struct file *file, void *_fh,
+			v4l2_std_id *std)
+{
+	struct hdpvr_device *dev = video_drvdata(file);
+	struct hdpvr_fh *fh = _fh;
+
+	if (!fh->legacy_mode && dev->options.video_input == HDPVR_COMPONENT)
+		return -ENODATA;
+	*std = dev->cur_std;
+	return 0;
+}
+
+static int vidioc_querystd(struct file *file, void *_fh, v4l2_std_id *a)
+{
+	struct hdpvr_device *dev = video_drvdata(file);
+	struct hdpvr_video_info *vid_info;
+	struct hdpvr_fh *fh = _fh;
+
+	*a = V4L2_STD_ALL;
+	if (dev->options.video_input == HDPVR_COMPONENT)
+		return fh->legacy_mode ? 0 : -ENODATA;
+	vid_info = get_video_info(dev);
+	if (vid_info == NULL)
+		return 0;
+	if (vid_info->width == 720 &&
+	    (vid_info->height == 480 || vid_info->height == 576)) {
+		*a = (vid_info->height == 480) ?
+			V4L2_STD_525_60 : V4L2_STD_625_50;
+	}
+	kfree(vid_info);
+	return 0;
+}
+
+static int vidioc_s_dv_timings(struct file *file, void *_fh,
+				    struct v4l2_dv_timings *timings)
+{
+	struct hdpvr_device *dev = video_drvdata(file);
+	struct hdpvr_fh *fh = _fh;
+	int i;
+
+	fh->legacy_mode = false;
+	if (dev->options.video_input)
+		return -ENODATA;
+	if (dev->status != STATUS_IDLE)
+		return -EBUSY;
+	for (i = 0; i < ARRAY_SIZE(hdpvr_dv_timings); i++)
+		if (v4l_match_dv_timings(timings, hdpvr_dv_timings + i, 0))
+			break;
+	if (i == ARRAY_SIZE(hdpvr_dv_timings))
+		return -EINVAL;
+	dev->cur_dv_timings = hdpvr_dv_timings[i];
+	dev->width = hdpvr_dv_timings[i].bt.width;
+	dev->height = hdpvr_dv_timings[i].bt.height;
+	return 0;
+}
+
+static int vidioc_g_dv_timings(struct file *file, void *_fh,
+				    struct v4l2_dv_timings *timings)
+{
+	struct hdpvr_device *dev = video_drvdata(file);
+	struct hdpvr_fh *fh = _fh;
+
+	fh->legacy_mode = false;
+	if (dev->options.video_input)
+		return -ENODATA;
+	*timings = dev->cur_dv_timings;
+	return 0;
+}
+
+static int vidioc_query_dv_timings(struct file *file, void *_fh,
+				    struct v4l2_dv_timings *timings)
+{
+	struct hdpvr_device *dev = video_drvdata(file);
+	struct hdpvr_fh *fh = _fh;
+	struct hdpvr_video_info *vid_info;
+	bool interlaced;
+	int ret = 0;
+	int i;
+
+	fh->legacy_mode = false;
+	if (dev->options.video_input)
+		return -ENODATA;
+	vid_info = get_video_info(dev);
+	if (vid_info == NULL)
+		return -ENOLCK;
+	interlaced = vid_info->fps <= 30;
+	for (i = 0; i < ARRAY_SIZE(hdpvr_dv_timings); i++) {
+		const struct v4l2_bt_timings *bt = &hdpvr_dv_timings[i].bt;
+		unsigned hsize;
+		unsigned vsize;
+		unsigned fps;
+
+		hsize = bt->hfrontporch + bt->hsync + bt->hbackporch + bt->width;
+		vsize = bt->vfrontporch + bt->vsync + bt->vbackporch +
+			bt->il_vfrontporch + bt->il_vsync + bt->il_vbackporch +
+			bt->height;
+		fps = (unsigned)bt->pixelclock / (hsize * vsize);
+		if (bt->width != vid_info->width ||
+		    bt->height != vid_info->height ||
+		    bt->interlaced != interlaced ||
+		    (fps != vid_info->fps && fps + 1 != vid_info->fps))
+			continue;
+		*timings = hdpvr_dv_timings[i];
+		break;
+	}
+	if (i == ARRAY_SIZE(hdpvr_dv_timings))
+		ret = -ERANGE;
+	kfree(vid_info);
+	return ret;
+}
+
+static int vidioc_enum_dv_timings(struct file *file, void *_fh,
+				    struct v4l2_enum_dv_timings *timings)
+{
+	struct hdpvr_device *dev = video_drvdata(file);
+	struct hdpvr_fh *fh = _fh;
+
+	fh->legacy_mode = false;
+	memset(timings->reserved, 0, sizeof(timings->reserved));
+	if (dev->options.video_input)
+		return -ENODATA;
+	if (timings->index >= ARRAY_SIZE(hdpvr_dv_timings))
+		return -EINVAL;
+	timings->timings = hdpvr_dv_timings[timings->index];
+	return 0;
+}
+
+static int vidioc_dv_timings_cap(struct file *file, void *_fh,
+				    struct v4l2_dv_timings_cap *cap)
+{
+	struct hdpvr_device *dev = video_drvdata(file);
+	struct hdpvr_fh *fh = _fh;
+
+	fh->legacy_mode = false;
+	if (dev->options.video_input)
+		return -ENODATA;
+	cap->type = V4L2_DV_BT_656_1120;
+	cap->bt.min_width = 720;
+	cap->bt.max_width = 1920;
+	cap->bt.min_height = 480;
+	cap->bt.max_height = 1080;
+	cap->bt.min_pixelclock = 27000000;
+	cap->bt.max_pixelclock = 74250000;
+	cap->bt.standards = V4L2_DV_BT_STD_CEA861;
+	cap->bt.capabilities = V4L2_DV_BT_CAP_INTERLACED | V4L2_DV_BT_CAP_PROGRESSIVE;
+	return 0;
+}
+
 static const char *iname[] = {
 	[HDPVR_COMPONENT] = "Component",
 	[HDPVR_SVIDEO]    = "S-Video",
 	[HDPVR_COMPOSITE] = "Composite",
 };
 
-static int vidioc_enum_input(struct file *file, void *priv,
-				struct v4l2_input *i)
+static int vidioc_enum_input(struct file *file, void *_fh, struct v4l2_input *i)
 {
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
 	unsigned int n;
 
 	n = i->index;
@@ -617,27 +760,42 @@ static int vidioc_enum_input(struct file *file, void *priv,
 
 	i->audioset = 1<<HDPVR_RCA_FRONT | 1<<HDPVR_RCA_BACK | 1<<HDPVR_SPDIF;
 
-	i->std = dev->video_dev->tvnorms;
+	i->capabilities = n ? V4L2_IN_CAP_STD : V4L2_IN_CAP_DV_TIMINGS;
+	i->std = n ? V4L2_STD_ALL : 0;
 
 	return 0;
 }
 
-static int vidioc_s_input(struct file *file, void *private_data,
+static int vidioc_s_input(struct file *file, void *_fh,
 			  unsigned int index)
 {
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
+	struct hdpvr_device *dev = video_drvdata(file);
 	int retval;
 
 	if (index >= HDPVR_VIDEO_INPUTS)
 		return -EINVAL;
 
 	if (dev->status != STATUS_IDLE)
-		return -EAGAIN;
+		return -EBUSY;
 
 	retval = hdpvr_config_call(dev, CTRL_VIDEO_INPUT_VALUE, index+1);
-	if (!retval)
+	if (!retval) {
 		dev->options.video_input = index;
+		/*
+		 * Unfortunately gstreamer calls ENUMSTD and bails out if it
+		 * won't find any formats, even though component input is
+		 * selected. This means that we have to leave tvnorms at
+		 * V4L2_STD_ALL. We cannot use the 'legacy' trick since
+		 * tvnorms is set at the device node level and not at the
+		 * filehandle level.
+		 *
+		 * Comment this out for now, but if the legacy mode can be
+		 * removed in the future, then this code should be enabled
+		 * again.
+		dev->video_dev->tvnorms =
+			(index != HDPVR_COMPONENT) ? V4L2_STD_ALL : 0;
+		 */
+	}
 
 	return retval;
 }
@@ -645,8 +803,7 @@ static int vidioc_s_input(struct file *file, void *private_data,
 static int vidioc_g_input(struct file *file, void *private_data,
 			  unsigned int *index)
 {
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
+	struct hdpvr_device *dev = video_drvdata(file);
 
 	*index = dev->options.video_input;
 	return 0;
@@ -679,15 +836,14 @@ static int vidioc_enumaudio(struct file *file, void *priv,
 static int vidioc_s_audio(struct file *file, void *private_data,
 			  const struct v4l2_audio *audio)
 {
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
+	struct hdpvr_device *dev = video_drvdata(file);
 	int retval;
 
 	if (audio->index >= HDPVR_AUDIO_INPUTS)
 		return -EINVAL;
 
 	if (dev->status != STATUS_IDLE)
-		return -EAGAIN;
+		return -EBUSY;
 
 	retval = hdpvr_set_audio(dev, audio->index+1, dev->options.audio_codec);
 	if (!retval)
@@ -699,8 +855,7 @@ static int vidioc_s_audio(struct file *file, void *private_data,
 static int vidioc_g_audio(struct file *file, void *private_data,
 			  struct v4l2_audio *audio)
 {
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
+	struct hdpvr_device *dev = video_drvdata(file);
 
 	audio->index = dev->options.audio_input;
 	audio->capability = V4L2_AUDCAP_STEREO;
@@ -709,335 +864,69 @@ static int vidioc_g_audio(struct file *file, void *private_data,
 	return 0;
 }
 
-static const s32 supported_v4l2_ctrls[] = {
-	V4L2_CID_BRIGHTNESS,
-	V4L2_CID_CONTRAST,
-	V4L2_CID_SATURATION,
-	V4L2_CID_HUE,
-	V4L2_CID_SHARPNESS,
-	V4L2_CID_MPEG_AUDIO_ENCODING,
-	V4L2_CID_MPEG_VIDEO_ENCODING,
-	V4L2_CID_MPEG_VIDEO_BITRATE_MODE,
-	V4L2_CID_MPEG_VIDEO_BITRATE,
-	V4L2_CID_MPEG_VIDEO_BITRATE_PEAK,
-};
-
-static int fill_queryctrl(struct hdpvr_options *opt, struct v4l2_queryctrl *qc,
-			  int ac3, int fw_ver)
-{
-	int err;
-
-	if (fw_ver > 0x15) {
-		switch (qc->id) {
-		case V4L2_CID_BRIGHTNESS:
-			return v4l2_ctrl_query_fill(qc, 0x0, 0xff, 1, 0x80);
-		case V4L2_CID_CONTRAST:
-			return v4l2_ctrl_query_fill(qc, 0x0, 0xff, 1, 0x40);
-		case V4L2_CID_SATURATION:
-			return v4l2_ctrl_query_fill(qc, 0x0, 0xff, 1, 0x40);
-		case V4L2_CID_HUE:
-			return v4l2_ctrl_query_fill(qc, 0x0, 0x1e, 1, 0xf);
-		case V4L2_CID_SHARPNESS:
-			return v4l2_ctrl_query_fill(qc, 0x0, 0xff, 1, 0x80);
-		}
-	} else {
-		switch (qc->id) {
-		case V4L2_CID_BRIGHTNESS:
-			return v4l2_ctrl_query_fill(qc, 0x0, 0xff, 1, 0x86);
-		case V4L2_CID_CONTRAST:
-			return v4l2_ctrl_query_fill(qc, 0x0, 0xff, 1, 0x80);
-		case V4L2_CID_SATURATION:
-			return v4l2_ctrl_query_fill(qc, 0x0, 0xff, 1, 0x80);
-		case V4L2_CID_HUE:
-			return v4l2_ctrl_query_fill(qc, 0x0, 0xff, 1, 0x80);
-		case V4L2_CID_SHARPNESS:
-			return v4l2_ctrl_query_fill(qc, 0x0, 0xff, 1, 0x80);
-		}
-	}
-
-	switch (qc->id) {
-	case V4L2_CID_MPEG_AUDIO_ENCODING:
-		return v4l2_ctrl_query_fill(
-			qc, V4L2_MPEG_AUDIO_ENCODING_AAC,
-			ac3 ? V4L2_MPEG_AUDIO_ENCODING_AC3
-			: V4L2_MPEG_AUDIO_ENCODING_AAC,
-			1, V4L2_MPEG_AUDIO_ENCODING_AAC);
-	case V4L2_CID_MPEG_VIDEO_ENCODING:
-		return v4l2_ctrl_query_fill(
-			qc, V4L2_MPEG_VIDEO_ENCODING_MPEG_4_AVC,
-			V4L2_MPEG_VIDEO_ENCODING_MPEG_4_AVC, 1,
-			V4L2_MPEG_VIDEO_ENCODING_MPEG_4_AVC);
-
-/* 	case V4L2_CID_MPEG_VIDEO_? maybe keyframe interval: */
-/* 		return v4l2_ctrl_query_fill(qc, 0, 128, 128, 0); */
-	case V4L2_CID_MPEG_VIDEO_BITRATE_MODE:
-		return v4l2_ctrl_query_fill(
-			qc, V4L2_MPEG_VIDEO_BITRATE_MODE_VBR,
-			V4L2_MPEG_VIDEO_BITRATE_MODE_CBR, 1,
-			V4L2_MPEG_VIDEO_BITRATE_MODE_CBR);
-
-	case V4L2_CID_MPEG_VIDEO_BITRATE:
-		return v4l2_ctrl_query_fill(qc, 1000000, 13500000, 100000,
-					    6500000);
-	case V4L2_CID_MPEG_VIDEO_BITRATE_PEAK:
-		err = v4l2_ctrl_query_fill(qc, 1100000, 20200000, 100000,
-					   9000000);
-		if (!err && opt->bitrate_mode == HDPVR_CONSTANT)
-			qc->flags |= V4L2_CTRL_FLAG_INACTIVE;
-		return err;
-	default:
-		return -EINVAL;
-	}
-}
-
-static int vidioc_queryctrl(struct file *file, void *private_data,
-			    struct v4l2_queryctrl *qc)
-{
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
-	int i, next;
-	u32 id = qc->id;
-
-	memset(qc, 0, sizeof(*qc));
-
-	next = !!(id &  V4L2_CTRL_FLAG_NEXT_CTRL);
-	qc->id = id & ~V4L2_CTRL_FLAG_NEXT_CTRL;
-
-	for (i = 0; i < ARRAY_SIZE(supported_v4l2_ctrls); i++) {
-		if (next) {
-			if (qc->id < supported_v4l2_ctrls[i])
-				qc->id = supported_v4l2_ctrls[i];
-			else
-				continue;
-		}
-
-		if (qc->id == supported_v4l2_ctrls[i])
-			return fill_queryctrl(&dev->options, qc,
-					      dev->flags & HDPVR_FLAG_AC3_CAP,
-					      dev->fw_ver);
-
-		if (qc->id < supported_v4l2_ctrls[i])
-			break;
-	}
-
-	return -EINVAL;
-}
-
-static int vidioc_g_ctrl(struct file *file, void *private_data,
-			 struct v4l2_control *ctrl)
+static int hdpvr_try_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
+	struct hdpvr_device *dev =
+		container_of(ctrl->handler, struct hdpvr_device, hdl);
 
 	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		ctrl->value = dev->options.brightness;
-		break;
-	case V4L2_CID_CONTRAST:
-		ctrl->value = dev->options.contrast;
-		break;
-	case V4L2_CID_SATURATION:
-		ctrl->value = dev->options.saturation;
-		break;
-	case V4L2_CID_HUE:
-		ctrl->value = dev->options.hue;
-		break;
-	case V4L2_CID_SHARPNESS:
-		ctrl->value = dev->options.sharpness;
+	case V4L2_CID_MPEG_VIDEO_BITRATE_MODE:
+		if (ctrl->val == V4L2_MPEG_VIDEO_BITRATE_MODE_VBR &&
+		    dev->video_bitrate->val >= dev->video_bitrate_peak->val)
+			dev->video_bitrate_peak->val =
+					dev->video_bitrate->val + 100000;
 		break;
-	default:
-		return -EINVAL;
 	}
 	return 0;
 }
 
-static int vidioc_s_ctrl(struct file *file, void *private_data,
-			 struct v4l2_control *ctrl)
+static int hdpvr_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
-	int retval;
+	struct hdpvr_device *dev =
+		container_of(ctrl->handler, struct hdpvr_device, hdl);
+	struct hdpvr_options *opt = &dev->options;
+	int ret = -EINVAL;
 
 	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		retval = hdpvr_config_call(dev, CTRL_BRIGHTNESS, ctrl->value);
-		if (!retval)
-			dev->options.brightness = ctrl->value;
-		break;
+		ret = hdpvr_config_call(dev, CTRL_BRIGHTNESS, ctrl->val);
+		if (ret)
+			break;
+		dev->options.brightness = ctrl->val;
+		return 0;
 	case V4L2_CID_CONTRAST:
-		retval = hdpvr_config_call(dev, CTRL_CONTRAST, ctrl->value);
-		if (!retval)
-			dev->options.contrast = ctrl->value;
-		break;
+		ret = hdpvr_config_call(dev, CTRL_CONTRAST, ctrl->val);
+		if (ret)
+			break;
+		dev->options.contrast = ctrl->val;
+		return 0;
 	case V4L2_CID_SATURATION:
-		retval = hdpvr_config_call(dev, CTRL_SATURATION, ctrl->value);
-		if (!retval)
-			dev->options.saturation = ctrl->value;
-		break;
+		ret = hdpvr_config_call(dev, CTRL_SATURATION, ctrl->val);
+		if (ret)
+			break;
+		dev->options.saturation = ctrl->val;
+		return 0;
 	case V4L2_CID_HUE:
-		retval = hdpvr_config_call(dev, CTRL_HUE, ctrl->value);
-		if (!retval)
-			dev->options.hue = ctrl->value;
-		break;
+		ret = hdpvr_config_call(dev, CTRL_HUE, ctrl->val);
+		if (ret)
+			break;
+		dev->options.hue = ctrl->val;
+		return 0;
 	case V4L2_CID_SHARPNESS:
-		retval = hdpvr_config_call(dev, CTRL_SHARPNESS, ctrl->value);
-		if (!retval)
-			dev->options.sharpness = ctrl->value;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	return retval;
-}
-
-
-static int hdpvr_get_ctrl(struct hdpvr_options *opt,
-			  struct v4l2_ext_control *ctrl)
-{
-	switch (ctrl->id) {
-	case V4L2_CID_MPEG_AUDIO_ENCODING:
-		ctrl->value = opt->audio_codec;
-		break;
-	case V4L2_CID_MPEG_VIDEO_ENCODING:
-		ctrl->value = V4L2_MPEG_VIDEO_ENCODING_MPEG_4_AVC;
-		break;
-/* 	case V4L2_CID_MPEG_VIDEO_B_FRAMES: */
-/* 		ctrl->value = (opt->gop_mode & 0x2) ? 0 : 128; */
-/* 		break; */
-	case V4L2_CID_MPEG_VIDEO_BITRATE_MODE:
-		ctrl->value = opt->bitrate_mode == HDPVR_CONSTANT
-			? V4L2_MPEG_VIDEO_BITRATE_MODE_CBR
-			: V4L2_MPEG_VIDEO_BITRATE_MODE_VBR;
-		break;
-	case V4L2_CID_MPEG_VIDEO_BITRATE:
-		ctrl->value = opt->bitrate * 100000;
-		break;
-	case V4L2_CID_MPEG_VIDEO_BITRATE_PEAK:
-		ctrl->value = opt->peak_bitrate * 100000;
-		break;
-	case V4L2_CID_MPEG_STREAM_TYPE:
-		ctrl->value = V4L2_MPEG_STREAM_TYPE_MPEG2_TS;
-		break;
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static int vidioc_g_ext_ctrls(struct file *file, void *priv,
-			      struct v4l2_ext_controls *ctrls)
-{
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
-	int i, err = 0;
-
-	if (ctrls->ctrl_class == V4L2_CTRL_CLASS_MPEG) {
-		for (i = 0; i < ctrls->count; i++) {
-			struct v4l2_ext_control *ctrl = ctrls->controls + i;
-
-			err = hdpvr_get_ctrl(&dev->options, ctrl);
-			if (err) {
-				ctrls->error_idx = i;
-				break;
-			}
-		}
-		return err;
-
-	}
-
-	return -EINVAL;
-}
-
-
-static int hdpvr_try_ctrl(struct v4l2_ext_control *ctrl, int ac3)
-{
-	int ret = -EINVAL;
-
-	switch (ctrl->id) {
-	case V4L2_CID_MPEG_AUDIO_ENCODING:
-		if (ctrl->value == V4L2_MPEG_AUDIO_ENCODING_AAC ||
-		    (ac3 && ctrl->value == V4L2_MPEG_AUDIO_ENCODING_AC3))
-			ret = 0;
-		break;
-	case V4L2_CID_MPEG_VIDEO_ENCODING:
-		if (ctrl->value == V4L2_MPEG_VIDEO_ENCODING_MPEG_4_AVC)
-			ret = 0;
-		break;
-/* 	case V4L2_CID_MPEG_VIDEO_B_FRAMES: */
-/* 		if (ctrl->value == 0 || ctrl->value == 128) */
-/* 			ret = 0; */
-/* 		break; */
-	case V4L2_CID_MPEG_VIDEO_BITRATE_MODE:
-		if (ctrl->value == V4L2_MPEG_VIDEO_BITRATE_MODE_CBR ||
-		    ctrl->value == V4L2_MPEG_VIDEO_BITRATE_MODE_VBR)
-			ret = 0;
-		break;
-	case V4L2_CID_MPEG_VIDEO_BITRATE:
-	{
-		uint bitrate = ctrl->value / 100000;
-		if (bitrate >= 10 && bitrate <= 135)
-			ret = 0;
-		break;
-	}
-	case V4L2_CID_MPEG_VIDEO_BITRATE_PEAK:
-	{
-		uint peak_bitrate = ctrl->value / 100000;
-		if (peak_bitrate >= 10 && peak_bitrate <= 202)
-			ret = 0;
-		break;
-	}
-	case V4L2_CID_MPEG_STREAM_TYPE:
-		if (ctrl->value == V4L2_MPEG_STREAM_TYPE_MPEG2_TS)
-			ret = 0;
-		break;
-	default:
-		return -EINVAL;
-	}
-	return ret;
-}
-
-static int vidioc_try_ext_ctrls(struct file *file, void *priv,
-				struct v4l2_ext_controls *ctrls)
-{
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
-	int i, err = 0;
-
-	if (ctrls->ctrl_class == V4L2_CTRL_CLASS_MPEG) {
-		for (i = 0; i < ctrls->count; i++) {
-			struct v4l2_ext_control *ctrl = ctrls->controls + i;
-
-			err = hdpvr_try_ctrl(ctrl,
-					     dev->flags & HDPVR_FLAG_AC3_CAP);
-			if (err) {
-				ctrls->error_idx = i;
-				break;
-			}
-		}
-		return err;
-	}
-
-	return -EINVAL;
-}
-
-
-static int hdpvr_set_ctrl(struct hdpvr_device *dev,
-			  struct v4l2_ext_control *ctrl)
-{
-	struct hdpvr_options *opt = &dev->options;
-	int ret = 0;
-
-	switch (ctrl->id) {
+		ret = hdpvr_config_call(dev, CTRL_SHARPNESS, ctrl->val);
+		if (ret)
+			break;
+		dev->options.sharpness = ctrl->val;
+		return 0;
 	case V4L2_CID_MPEG_AUDIO_ENCODING:
 		if (dev->flags & HDPVR_FLAG_AC3_CAP) {
-			opt->audio_codec = ctrl->value;
-			ret = hdpvr_set_audio(dev, opt->audio_input,
+			opt->audio_codec = ctrl->val;
+			return hdpvr_set_audio(dev, opt->audio_input,
 					      opt->audio_codec);
 		}
-		break;
+		return 0;
 	case V4L2_CID_MPEG_VIDEO_ENCODING:
-		break;
+		return 0;
 /* 	case V4L2_CID_MPEG_VIDEO_B_FRAMES: */
 /* 		if (ctrl->value == 0 && !(opt->gop_mode & 0x2)) { */
 /* 			opt->gop_mode |= 0x2; */
@@ -1050,86 +939,41 @@ static int hdpvr_set_ctrl(struct hdpvr_device *dev,
 /* 					  opt->gop_mode); */
 /* 		} */
 /* 		break; */
-	case V4L2_CID_MPEG_VIDEO_BITRATE_MODE:
-		if (ctrl->value == V4L2_MPEG_VIDEO_BITRATE_MODE_CBR &&
-		    opt->bitrate_mode != HDPVR_CONSTANT) {
-			opt->bitrate_mode = HDPVR_CONSTANT;
-			hdpvr_config_call(dev, CTRL_BITRATE_MODE_VALUE,
-					  opt->bitrate_mode);
-		}
-		if (ctrl->value == V4L2_MPEG_VIDEO_BITRATE_MODE_VBR &&
-		    opt->bitrate_mode == HDPVR_CONSTANT) {
-			opt->bitrate_mode = HDPVR_VARIABLE_AVERAGE;
+	case V4L2_CID_MPEG_VIDEO_BITRATE_MODE: {
+		uint peak_bitrate = dev->video_bitrate_peak->val / 100000;
+		uint bitrate = dev->video_bitrate->val / 100000;
+
+		if (ctrl->is_new) {
+			if (ctrl->val == V4L2_MPEG_VIDEO_BITRATE_MODE_CBR)
+				opt->bitrate_mode = HDPVR_CONSTANT;
+			else
+				opt->bitrate_mode = HDPVR_VARIABLE_AVERAGE;
 			hdpvr_config_call(dev, CTRL_BITRATE_MODE_VALUE,
 					  opt->bitrate_mode);
+			v4l2_ctrl_activate(dev->video_bitrate_peak,
+				ctrl->val != V4L2_MPEG_VIDEO_BITRATE_MODE_CBR);
 		}
-		break;
-	case V4L2_CID_MPEG_VIDEO_BITRATE: {
-		uint bitrate = ctrl->value / 100000;
-
-		opt->bitrate = bitrate;
-		if (bitrate >= opt->peak_bitrate)
-			opt->peak_bitrate = bitrate+1;
-
-		hdpvr_set_bitrate(dev);
-		break;
-	}
-	case V4L2_CID_MPEG_VIDEO_BITRATE_PEAK: {
-		uint peak_bitrate = ctrl->value / 100000;
-
-		if (opt->bitrate_mode == HDPVR_CONSTANT)
-			break;
 
-		if (opt->bitrate < peak_bitrate) {
+		if (dev->video_bitrate_peak->is_new ||
+		    dev->video_bitrate->is_new) {
+			opt->bitrate = bitrate;
 			opt->peak_bitrate = peak_bitrate;
 			hdpvr_set_bitrate(dev);
-		} else
-			ret = -EINVAL;
-		break;
+		}
+		return 0;
 	}
 	case V4L2_CID_MPEG_STREAM_TYPE:
-		break;
+		return 0;
 	default:
-		return -EINVAL;
+		break;
 	}
 	return ret;
 }
 
-static int vidioc_s_ext_ctrls(struct file *file, void *priv,
-			      struct v4l2_ext_controls *ctrls)
-{
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
-	int i, err = 0;
-
-	if (ctrls->ctrl_class == V4L2_CTRL_CLASS_MPEG) {
-		for (i = 0; i < ctrls->count; i++) {
-			struct v4l2_ext_control *ctrl = ctrls->controls + i;
-
-			err = hdpvr_try_ctrl(ctrl,
-					     dev->flags & HDPVR_FLAG_AC3_CAP);
-			if (err) {
-				ctrls->error_idx = i;
-				break;
-			}
-			err = hdpvr_set_ctrl(dev, ctrl);
-			if (err) {
-				ctrls->error_idx = i;
-				break;
-			}
-		}
-		return err;
-
-	}
-
-	return -EINVAL;
-}
-
 static int vidioc_enum_fmt_vid_cap(struct file *file, void *private_data,
 				    struct v4l2_fmtdesc *f)
 {
-
-	if (f->index != 0 || f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+	if (f->index != 0)
 		return -EINVAL;
 
 	f->flags = V4L2_FMT_FLAG_COMPRESSED;
@@ -1139,56 +983,92 @@ static int vidioc_enum_fmt_vid_cap(struct file *file, void *private_data,
 	return 0;
 }
 
-static int vidioc_g_fmt_vid_cap(struct file *file, void *private_data,
+static int vidioc_g_fmt_vid_cap(struct file *file, void *_fh,
 				struct v4l2_format *f)
 {
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
-	struct hdpvr_video_info *vid_info;
-
-	if (!dev)
-		return -ENODEV;
-
-	vid_info = get_video_info(dev);
-	if (!vid_info)
-		return -EFAULT;
-
-	f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	struct hdpvr_device *dev = video_drvdata(file);
+	struct hdpvr_fh *fh = _fh;
+
+	/*
+	 * The original driver would always returns the current detected
+	 * resolution as the format (and EFAULT if it couldn't be detected).
+	 * With the introduction of VIDIOC_QUERY_DV_TIMINGS there is now a
+	 * better way of doing this, but to stay compatible with existing
+	 * applications we assume legacy mode every time an application opens
+	 * the device. Only if one of the new DV_TIMINGS ioctls is called
+	 * will the filehandle go into 'normal' mode where g_fmt returns the
+	 * last set format.
+	 */
+	if (fh->legacy_mode) {
+		struct hdpvr_video_info *vid_info;
+
+		vid_info = get_video_info(dev);
+		if (!vid_info)
+			return -EFAULT;
+		f->fmt.pix.width = vid_info->width;
+		f->fmt.pix.height = vid_info->height;
+		kfree(vid_info);
+	} else {
+		f->fmt.pix.width = dev->width;
+		f->fmt.pix.height = dev->height;
+	}
 	f->fmt.pix.pixelformat	= V4L2_PIX_FMT_MPEG;
-	f->fmt.pix.width	= vid_info->width;
-	f->fmt.pix.height	= vid_info->height;
 	f->fmt.pix.sizeimage	= dev->bulk_in_size;
-	f->fmt.pix.colorspace	= 0;
 	f->fmt.pix.bytesperline	= 0;
-	f->fmt.pix.field	= V4L2_FIELD_ANY;
-
-	kfree(vid_info);
+	f->fmt.pix.priv		= 0;
+	if (f->fmt.pix.width == 720) {
+		/* SDTV formats */
+		f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+		f->fmt.pix.field = V4L2_FIELD_INTERLACED;
+	} else {
+		/* HDTV formats */
+		f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE240M;
+		f->fmt.pix.field = V4L2_FIELD_NONE;
+	}
 	return 0;
 }
 
 static int vidioc_encoder_cmd(struct file *filp, void *priv,
 			       struct v4l2_encoder_cmd *a)
 {
-	struct hdpvr_fh *fh = filp->private_data;
-	struct hdpvr_device *dev = fh->dev;
-	int res;
+	struct hdpvr_device *dev = video_drvdata(filp);
+	int res = 0;
 
 	mutex_lock(&dev->io_mutex);
+	a->flags = 0;
 
-	memset(&a->raw, 0, sizeof(a->raw));
 	switch (a->cmd) {
 	case V4L2_ENC_CMD_START:
-		a->flags = 0;
+		if (dev->owner && filp->private_data != dev->owner) {
+			res = -EBUSY;
+			break;
+		}
+		if (dev->status == STATUS_STREAMING)
+			break;
 		res = hdpvr_start_streaming(dev);
+		if (!res)
+			dev->owner = filp->private_data;
+		else
+			dev->status = STATUS_IDLE;
 		break;
 	case V4L2_ENC_CMD_STOP:
+		if (dev->owner && filp->private_data != dev->owner) {
+			res = -EBUSY;
+			break;
+		}
+		if (dev->status == STATUS_IDLE)
+			break;
 		res = hdpvr_stop_streaming(dev);
+		if (!res)
+			dev->owner = NULL;
 		break;
 	default:
 		v4l2_dbg(MSG_INFO, hdpvr_debug, &dev->v4l2_dev,
 			 "Unsupported encoder cmd %d\n", a->cmd);
 		res = -EINVAL;
+		break;
 	}
+
 	mutex_unlock(&dev->io_mutex);
 	return res;
 }
@@ -1196,6 +1076,7 @@ static int vidioc_encoder_cmd(struct file *filp, void *priv,
 static int vidioc_try_encoder_cmd(struct file *filp, void *priv,
 					struct v4l2_encoder_cmd *a)
 {
+	a->flags = 0;
 	switch (a->cmd) {
 	case V4L2_ENC_CMD_START:
 	case V4L2_ENC_CMD_STOP:
@@ -1208,22 +1089,28 @@ static int vidioc_try_encoder_cmd(struct file *filp, void *priv,
 static const struct v4l2_ioctl_ops hdpvr_ioctl_ops = {
 	.vidioc_querycap	= vidioc_querycap,
 	.vidioc_s_std		= vidioc_s_std,
+	.vidioc_g_std		= vidioc_g_std,
+	.vidioc_querystd	= vidioc_querystd,
+	.vidioc_s_dv_timings	= vidioc_s_dv_timings,
+	.vidioc_g_dv_timings	= vidioc_g_dv_timings,
+	.vidioc_query_dv_timings= vidioc_query_dv_timings,
+	.vidioc_enum_dv_timings	= vidioc_enum_dv_timings,
+	.vidioc_dv_timings_cap	= vidioc_dv_timings_cap,
 	.vidioc_enum_input	= vidioc_enum_input,
 	.vidioc_g_input		= vidioc_g_input,
 	.vidioc_s_input		= vidioc_s_input,
 	.vidioc_enumaudio	= vidioc_enumaudio,
 	.vidioc_g_audio		= vidioc_g_audio,
 	.vidioc_s_audio		= vidioc_s_audio,
-	.vidioc_queryctrl	= vidioc_queryctrl,
-	.vidioc_g_ctrl		= vidioc_g_ctrl,
-	.vidioc_s_ctrl		= vidioc_s_ctrl,
-	.vidioc_g_ext_ctrls	= vidioc_g_ext_ctrls,
-	.vidioc_s_ext_ctrls	= vidioc_s_ext_ctrls,
-	.vidioc_try_ext_ctrls	= vidioc_try_ext_ctrls,
-	.vidioc_enum_fmt_vid_cap	= vidioc_enum_fmt_vid_cap,
-	.vidioc_g_fmt_vid_cap		= vidioc_g_fmt_vid_cap,
+	.vidioc_enum_fmt_vid_cap= vidioc_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap	= vidioc_g_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap	= vidioc_g_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap	= vidioc_g_fmt_vid_cap,
 	.vidioc_encoder_cmd	= vidioc_encoder_cmd,
 	.vidioc_try_encoder_cmd	= vidioc_try_encoder_cmd,
+	.vidioc_log_status	= v4l2_ctrl_log_status,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 static void hdpvr_device_release(struct video_device *vdev)
@@ -1236,9 +1123,10 @@ static void hdpvr_device_release(struct video_device *vdev)
 	mutex_unlock(&dev->io_mutex);
 
 	v4l2_device_unregister(&dev->v4l2_dev);
+	v4l2_ctrl_handler_free(&dev->hdl);
 
 	/* deregister I2C adapter */
-#if defined(CONFIG_I2C) || (CONFIG_I2C_MODULE)
+#if IS_ENABLED(CONFIG_I2C)
 	mutex_lock(&dev->i2c_mutex);
 	i2c_del_adapter(&dev->i2c_adapter);
 	mutex_unlock(&dev->i2c_mutex);
@@ -1249,41 +1137,112 @@ static void hdpvr_device_release(struct video_device *vdev)
 }
 
 static const struct video_device hdpvr_video_template = {
-/* 	.type			= VFL_TYPE_GRABBER, */
-/* 	.type2			= VID_TYPE_CAPTURE | VID_TYPE_MPEG_ENCODER, */
 	.fops			= &hdpvr_fops,
 	.release		= hdpvr_device_release,
 	.ioctl_ops 		= &hdpvr_ioctl_ops,
-	.tvnorms 		=
-		V4L2_STD_NTSC  | V4L2_STD_SECAM | V4L2_STD_PAL_B |
-		V4L2_STD_PAL_G | V4L2_STD_PAL_H | V4L2_STD_PAL_I |
-		V4L2_STD_PAL_D | V4L2_STD_PAL_M | V4L2_STD_PAL_N |
-		V4L2_STD_PAL_60,
-	.current_norm 		= V4L2_STD_NTSC | V4L2_STD_PAL_M |
-		V4L2_STD_PAL_60,
+	.tvnorms		= V4L2_STD_ALL,
+};
+
+static const struct v4l2_ctrl_ops hdpvr_ctrl_ops = {
+	.try_ctrl = hdpvr_try_ctrl,
+	.s_ctrl = hdpvr_s_ctrl,
 };
 
 int hdpvr_register_videodev(struct hdpvr_device *dev, struct device *parent,
 			    int devnum)
 {
+	struct v4l2_ctrl_handler *hdl = &dev->hdl;
+	bool ac3 = dev->flags & HDPVR_FLAG_AC3_CAP;
+	int res;
+
+	dev->cur_std = V4L2_STD_525_60;
+	dev->width = 720;
+	dev->height = 480;
+	dev->cur_dv_timings = hdpvr_dv_timings[HDPVR_DEF_DV_TIMINGS_IDX];
+	v4l2_ctrl_handler_init(hdl, 11);
+	if (dev->fw_ver > 0x15) {
+		v4l2_ctrl_new_std(hdl, &hdpvr_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0x0, 0xff, 1, 0x80);
+		v4l2_ctrl_new_std(hdl, &hdpvr_ctrl_ops,
+			V4L2_CID_CONTRAST, 0x0, 0xff, 1, 0x40);
+		v4l2_ctrl_new_std(hdl, &hdpvr_ctrl_ops,
+			V4L2_CID_SATURATION, 0x0, 0xff, 1, 0x40);
+		v4l2_ctrl_new_std(hdl, &hdpvr_ctrl_ops,
+			V4L2_CID_HUE, 0x0, 0x1e, 1, 0xf);
+		v4l2_ctrl_new_std(hdl, &hdpvr_ctrl_ops,
+			V4L2_CID_SHARPNESS, 0x0, 0xff, 1, 0x80);
+	} else {
+		v4l2_ctrl_new_std(hdl, &hdpvr_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0x0, 0xff, 1, 0x86);
+		v4l2_ctrl_new_std(hdl, &hdpvr_ctrl_ops,
+			V4L2_CID_CONTRAST, 0x0, 0xff, 1, 0x80);
+		v4l2_ctrl_new_std(hdl, &hdpvr_ctrl_ops,
+			V4L2_CID_SATURATION, 0x0, 0xff, 1, 0x80);
+		v4l2_ctrl_new_std(hdl, &hdpvr_ctrl_ops,
+			V4L2_CID_HUE, 0x0, 0xff, 1, 0x80);
+		v4l2_ctrl_new_std(hdl, &hdpvr_ctrl_ops,
+			V4L2_CID_SHARPNESS, 0x0, 0xff, 1, 0x80);
+	}
+
+	v4l2_ctrl_new_std_menu(hdl, &hdpvr_ctrl_ops,
+		V4L2_CID_MPEG_STREAM_TYPE,
+		V4L2_MPEG_STREAM_TYPE_MPEG2_TS,
+		0x1, V4L2_MPEG_STREAM_TYPE_MPEG2_TS);
+	v4l2_ctrl_new_std_menu(hdl, &hdpvr_ctrl_ops,
+		V4L2_CID_MPEG_AUDIO_ENCODING,
+		ac3 ? V4L2_MPEG_AUDIO_ENCODING_AC3 : V4L2_MPEG_AUDIO_ENCODING_AAC,
+		0x7, V4L2_MPEG_AUDIO_ENCODING_AAC);
+	v4l2_ctrl_new_std_menu(hdl, &hdpvr_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_ENCODING,
+		V4L2_MPEG_VIDEO_ENCODING_MPEG_4_AVC, 0x3,
+		V4L2_MPEG_VIDEO_ENCODING_MPEG_4_AVC);
+
+	dev->video_mode = v4l2_ctrl_new_std_menu(hdl, &hdpvr_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_BITRATE_MODE,
+		V4L2_MPEG_VIDEO_BITRATE_MODE_CBR, 0,
+		V4L2_MPEG_VIDEO_BITRATE_MODE_CBR);
+
+	dev->video_bitrate = v4l2_ctrl_new_std(hdl, &hdpvr_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_BITRATE,
+		1000000, 13500000, 100000, 6500000);
+	dev->video_bitrate_peak = v4l2_ctrl_new_std(hdl, &hdpvr_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_BITRATE_PEAK,
+		1100000, 20200000, 100000, 9000000);
+	dev->v4l2_dev.ctrl_handler = hdl;
+	if (hdl->error) {
+		res = hdl->error;
+		v4l2_err(&dev->v4l2_dev, "Could not register controls\n");
+		goto error;
+	}
+	v4l2_ctrl_cluster(3, &dev->video_mode);
+	res = v4l2_ctrl_handler_setup(hdl);
+	if (res < 0) {
+		v4l2_err(&dev->v4l2_dev, "Could not setup controls\n");
+		goto error;
+	}
+
 	/* setup and register video device */
 	dev->video_dev = video_device_alloc();
 	if (!dev->video_dev) {
 		v4l2_err(&dev->v4l2_dev, "video_device_alloc() failed\n");
+		res = -ENOMEM;
 		goto error;
 	}
 
-	*(dev->video_dev) = hdpvr_video_template;
+	*dev->video_dev = hdpvr_video_template;
 	strcpy(dev->video_dev->name, "Hauppauge HD PVR");
-	dev->video_dev->parent = parent;
+	dev->video_dev->v4l2_dev = &dev->v4l2_dev;
 	video_set_drvdata(dev->video_dev, dev);
+	set_bit(V4L2_FL_USE_FH_PRIO, &dev->video_dev->flags);
 
-	if (video_register_device(dev->video_dev, VFL_TYPE_GRABBER, devnum)) {
+	res = video_register_device(dev->video_dev, VFL_TYPE_GRABBER, devnum);
+	if (res < 0) {
 		v4l2_err(&dev->v4l2_dev, "video_device registration failed\n");
 		goto error;
 	}
 
 	return 0;
 error:
-	return -ENOMEM;
+	v4l2_ctrl_handler_free(hdl);
+	return res;
 }
diff --git a/drivers/media/usb/hdpvr/hdpvr.h b/drivers/media/usb/hdpvr/hdpvr.h
index fea3c692699..1478f3d5763 100644
--- a/drivers/media/usb/hdpvr/hdpvr.h
+++ b/drivers/media/usb/hdpvr/hdpvr.h
@@ -16,6 +16,7 @@
 #include <linux/videodev2.h>
 
 #include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
 #include <media/ir-kbd-i2c.h>
 
 #define HDPVR_MAX 8
@@ -37,6 +38,7 @@
 #define HDPVR_FIRMWARE_VERSION_AC3	0x0d
 #define HDPVR_FIRMWARE_VERSION_0X12	0x12
 #define HDPVR_FIRMWARE_VERSION_0X15	0x15
+#define HDPVR_FIRMWARE_VERSION_0X1E	0x1e
 
 /* #define HDPVR_DEBUG */
 
@@ -65,10 +67,19 @@ struct hdpvr_options {
 struct hdpvr_device {
 	/* the v4l device for this device */
 	struct video_device	*video_dev;
+	/* the control handler for this device */
+	struct v4l2_ctrl_handler hdl;
 	/* the usb device for this device */
 	struct usb_device	*udev;
 	/* v4l2-device unused */
 	struct v4l2_device	v4l2_dev;
+	struct { /* video mode/bitrate control cluster */
+		struct v4l2_ctrl *video_mode;
+		struct v4l2_ctrl *video_bitrate;
+		struct v4l2_ctrl *video_bitrate_peak;
+	};
+	/* v4l2 format */
+	uint width, height;
 
 	/* the max packet size of the bulk endpoint */
 	size_t			bulk_in_size;
@@ -77,11 +88,11 @@ struct hdpvr_device {
 
 	/* holds the current device status */
 	__u8			status;
-	/* count the number of openers */
-	uint			open_count;
 
-	/* holds the cureent set options */
+	/* holds the current set options */
 	struct hdpvr_options	options;
+	v4l2_std_id		cur_std;
+	struct v4l2_dv_timings	cur_dv_timings;
 
 	uint			flags;
 
@@ -99,6 +110,8 @@ struct hdpvr_device {
 	struct workqueue_struct	*workqueue;
 	/**/
 	struct work_struct	worker;
+	/* current stream owner */
+	struct v4l2_fh		*owner;
 
 	/* I2C adapter */
 	struct i2c_adapter	i2c_adapter;
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-hdw.c b/drivers/media/usb/pvrusb2/pvrusb2-hdw.c
index 299751a8b06..e11267f35d8 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-hdw.c
+++ b/drivers/media/usb/pvrusb2/pvrusb2-hdw.c
@@ -5165,7 +5165,7 @@ static int pvr2_hdw_get_eeprom_addr(struct pvr2_hdw *hdw)
 
 
 int pvr2_hdw_register_access(struct pvr2_hdw *hdw,
-			     struct v4l2_dbg_match *match, u64 reg_id,
+			     const struct v4l2_dbg_match *match, u64 reg_id,
 			     int setFl, u64 *val_ptr)
 {
 #ifdef CONFIG_VIDEO_ADV_DEBUG
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-hdw.h b/drivers/media/usb/pvrusb2/pvrusb2-hdw.h
index 8060fc666ee..91bae930cd7 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-hdw.h
+++ b/drivers/media/usb/pvrusb2/pvrusb2-hdw.h
@@ -240,7 +240,7 @@ void pvr2_hdw_v4l_store_minor_number(struct pvr2_hdw *,
    setFl   - true to set the register, false to read it
    val_ptr - storage location for source / result. */
 int pvr2_hdw_register_access(struct pvr2_hdw *,
-			     struct v4l2_dbg_match *match, u64 reg_id,
+			     const struct v4l2_dbg_match *match, u64 reg_id,
 			     int setFl, u64 *val_ptr);
 
 /* The following entry points are all lower level things you normally don't
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-v4l2.c b/drivers/media/usb/pvrusb2/pvrusb2-v4l2.c
index 34c3b6e80e8..a8a65fa5793 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-v4l2.c
+++ b/drivers/media/usb/pvrusb2/pvrusb2-v4l2.c
@@ -196,13 +196,13 @@ static int pvr2_g_std(struct file *file, void *priv, v4l2_std_id *std)
 	return ret;
 }
 
-static int pvr2_s_std(struct file *file, void *priv, v4l2_std_id *std)
+static 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;
 
 	return pvr2_ctrl_set_value(
-		pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_STDCUR), *std);
+		pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_STDCUR), std);
 }
 
 static int pvr2_querystd(struct file *file, void *priv, v4l2_std_id *std)
@@ -352,7 +352,7 @@ static int pvr2_g_tuner(struct file *file, void *priv, struct v4l2_tuner *vt)
 	return pvr2_hdw_get_tuner_status(hdw, vt);
 }
 
-static int pvr2_s_tuner(struct file *file, void *priv, struct v4l2_tuner *vt)
+static int pvr2_s_tuner(struct file *file, void *priv, const struct v4l2_tuner *vt)
 {
 	struct pvr2_v4l2_fh *fh = file->private_data;
 	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
@@ -365,7 +365,7 @@ static int pvr2_s_tuner(struct file *file, void *priv, struct v4l2_tuner *vt)
 			vt->audmode);
 }
 
-static int pvr2_s_frequency(struct file *file, void *priv, struct v4l2_frequency *vf)
+static int pvr2_s_frequency(struct file *file, void *priv, const struct v4l2_frequency *vf)
 {
 	struct pvr2_v4l2_fh *fh = file->private_data;
 	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
@@ -815,7 +815,7 @@ static int pvr2_g_register(struct file *file, void *priv, struct v4l2_dbg_regist
 	return ret;
 }
 
-static int pvr2_s_register(struct file *file, void *priv, struct v4l2_dbg_register *req)
+static int pvr2_s_register(struct file *file, void *priv, const struct v4l2_dbg_register *req)
 {
 	struct pvr2_v4l2_fh *fh = file->private_data;
 	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
diff --git a/drivers/media/usb/pwc/pwc-if.c b/drivers/media/usb/pwc/pwc-if.c
index 5ec15cb1ed2..77bbf788965 100644
--- a/drivers/media/usb/pwc/pwc-if.c
+++ b/drivers/media/usb/pwc/pwc-if.c
@@ -1001,6 +1001,7 @@ static int usb_pwc_probe(struct usb_interface *intf, const struct usb_device_id
 	pdev->vb_queue.buf_struct_size = sizeof(struct pwc_frame_buf);
 	pdev->vb_queue.ops = &pwc_vb_queue_ops;
 	pdev->vb_queue.mem_ops = &vb2_vmalloc_memops;
+	pdev->vb_queue.timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 	rc = vb2_queue_init(&pdev->vb_queue);
 	if (rc < 0) {
 		PWC_ERROR("Oops, could not initialize vb2 queue.\n");
diff --git a/drivers/media/usb/s2255/s2255drv.c b/drivers/media/usb/s2255/s2255drv.c
index 498c57ea5d3..ab97e7d0b4f 100644
--- a/drivers/media/usb/s2255/s2255drv.c
+++ b/drivers/media/usb/s2255/s2255drv.c
@@ -43,12 +43,14 @@
 #include <linux/slab.h>
 #include <linux/videodev2.h>
 #include <linux/mm.h>
+#include <linux/vmalloc.h>
+#include <linux/usb.h>
 #include <media/videobuf-vmalloc.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
-#include <linux/vmalloc.h>
-#include <linux/usb.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
 
 #define S2255_VERSION		"1.22.1"
 #define FIRMWARE_FILE_NAME "f2255usb.bin"
@@ -217,12 +219,15 @@ struct s2255_dev;
 
 struct s2255_channel {
 	struct video_device	vdev;
+	struct v4l2_ctrl_handler hdl;
+	struct v4l2_ctrl	*jpegqual_ctrl;
 	int			resources;
 	struct s2255_dmaqueue	vidq;
 	struct s2255_bufferi	buffer;
 	struct s2255_mode	mode;
+	v4l2_std_id		std;
 	/* jpeg compression */
-	struct v4l2_jpegcompression jc;
+	unsigned		jpegqual;
 	/* capture parameters (for high quality mode full size) */
 	struct v4l2_captureparm cap_parm;
 	int			cur_frame;
@@ -292,6 +297,8 @@ struct s2255_buffer {
 };
 
 struct s2255_fh {
+	/* this must be the first field in this struct */
+	struct v4l2_fh		fh;
 	struct s2255_dev	*dev;
 	struct videobuf_queue	vb_vidq;
 	enum v4l2_buf_type	type;
@@ -306,7 +313,7 @@ struct s2255_fh {
 /* Need DSP version 5+ for video status feature */
 #define S2255_MIN_DSP_STATUS      5
 #define S2255_MIN_DSP_COLORFILTER 8
-#define S2255_NORMS		(V4L2_STD_PAL | V4L2_STD_NTSC)
+#define S2255_NORMS		(V4L2_STD_ALL)
 
 /* private V4L2 controls */
 
@@ -336,7 +343,7 @@ struct s2255_fh {
  */
 #define S2255_V4L2_YC_ON  1
 #define S2255_V4L2_YC_OFF 0
-#define V4L2_CID_PRIVATE_COLORFILTER (V4L2_CID_PRIVATE_BASE + 0)
+#define V4L2_CID_S2255_COLORFILTER (V4L2_CID_USER_S2255_BASE + 0)
 
 /* frame prefix size (sent once every frame) */
 #define PREFIX_SIZE		512
@@ -409,11 +416,6 @@ MODULE_DEVICE_TABLE(usb, s2255_table);
 /* JPEG formats must be defined last to support jpeg_enable parameter */
 static const struct s2255_fmt formats[] = {
 	{
-		.name = "4:2:2, planar, YUV422P",
-		.fourcc = V4L2_PIX_FMT_YUV422P,
-		.depth = 16
-
-	}, {
 		.name = "4:2:2, packed, YUYV",
 		.fourcc = V4L2_PIX_FMT_YUYV,
 		.depth = 16
@@ -423,6 +425,11 @@ static const struct s2255_fmt formats[] = {
 		.fourcc = V4L2_PIX_FMT_UYVY,
 		.depth = 16
 	}, {
+		.name = "4:2:2, planar, YUV422P",
+		.fourcc = V4L2_PIX_FMT_YUV422P,
+		.depth = 16
+
+	}, {
 		.name = "8bpp GREY",
 		.fourcc = V4L2_PIX_FMT_GREY,
 		.depth = 8
@@ -437,27 +444,27 @@ static const struct s2255_fmt formats[] = {
 	}
 };
 
-static int norm_maxw(struct video_device *vdev)
+static int norm_maxw(struct s2255_channel *channel)
 {
-	return (vdev->current_norm & V4L2_STD_NTSC) ?
+	return (channel->std & V4L2_STD_525_60) ?
 	    LINE_SZ_4CIFS_NTSC : LINE_SZ_4CIFS_PAL;
 }
 
-static int norm_maxh(struct video_device *vdev)
+static int norm_maxh(struct s2255_channel *channel)
 {
-	return (vdev->current_norm & V4L2_STD_NTSC) ?
+	return (channel->std & V4L2_STD_525_60) ?
 	    (NUM_LINES_1CIFS_NTSC * 2) : (NUM_LINES_1CIFS_PAL * 2);
 }
 
-static int norm_minw(struct video_device *vdev)
+static int norm_minw(struct s2255_channel *channel)
 {
-	return (vdev->current_norm & V4L2_STD_NTSC) ?
+	return (channel->std & V4L2_STD_525_60) ?
 	    LINE_SZ_1CIFS_NTSC : LINE_SZ_1CIFS_PAL;
 }
 
-static int norm_minh(struct video_device *vdev)
+static int norm_minh(struct s2255_channel *channel)
 {
-	return (vdev->current_norm & V4L2_STD_NTSC) ?
+	return (channel->std & V4L2_STD_525_60) ?
 	    (NUM_LINES_1CIFS_NTSC) : (NUM_LINES_1CIFS_PAL);
 }
 
@@ -515,7 +522,7 @@ static void s2255_timer(unsigned long user_data)
 
 
 /* this loads the firmware asynchronously.
-   Originally this was done synchroously in probe.
+   Originally this was done synchronously in probe.
    But it is better to load it asynchronously here than block
    inside the probe function. Blocking inside probe affects boot time.
    FW loading is triggered by the timer in the probe function
@@ -719,10 +726,10 @@ static int buffer_prepare(struct videobuf_queue *vq, struct videobuf_buffer *vb,
 	if (channel->fmt == NULL)
 		return -EINVAL;
 
-	if ((w < norm_minw(&channel->vdev)) ||
-	    (w > norm_maxw(&channel->vdev)) ||
-	    (h < norm_minh(&channel->vdev)) ||
-	    (h > norm_maxh(&channel->vdev))) {
+	if ((w < norm_minw(channel)) ||
+	    (w > norm_maxw(channel)) ||
+	    (h < norm_minh(channel)) ||
+	    (h > norm_maxh(channel))) {
 		dprintk(4, "invalid buffer prepare\n");
 		return -EINVAL;
 	}
@@ -810,37 +817,17 @@ static void res_free(struct s2255_fh *fh)
 	dprintk(1, "res: put\n");
 }
 
-static int vidioc_querymenu(struct file *file, void *priv,
-			    struct v4l2_querymenu *qmenu)
-{
-	static const char *colorfilter[] = {
-		"Off",
-		"On",
-		NULL
-	};
-	if (qmenu->id == V4L2_CID_PRIVATE_COLORFILTER) {
-		int i;
-		const char **menu_items = colorfilter;
-		for (i = 0; i < qmenu->index && menu_items[i]; i++)
-			; /* do nothing (from v4l2-common.c) */
-		if (menu_items[i] == NULL || menu_items[i][0] == '\0')
-			return -EINVAL;
-		strlcpy(qmenu->name, menu_items[qmenu->index],
-			sizeof(qmenu->name));
-		return 0;
-	}
-	return v4l2_ctrl_query_menu(qmenu, NULL, NULL);
-}
-
 static int vidioc_querycap(struct file *file, void *priv,
 			   struct v4l2_capability *cap)
 {
 	struct s2255_fh *fh = file->private_data;
 	struct s2255_dev *dev = fh->dev;
+
 	strlcpy(cap->driver, "s2255", sizeof(cap->driver));
 	strlcpy(cap->card, "s2255", sizeof(cap->card));
 	usb_make_path(dev->udev, cap->bus_info, sizeof(cap->bus_info));
-	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
@@ -865,13 +852,20 @@ static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 {
 	struct s2255_fh *fh = priv;
 	struct s2255_channel *channel = fh->channel;
+	int is_ntsc = channel->std & V4L2_STD_525_60;
 
 	f->fmt.pix.width = channel->width;
 	f->fmt.pix.height = channel->height;
-	f->fmt.pix.field = fh->vb_vidq.field;
+	if (f->fmt.pix.height >=
+	    (is_ntsc ? NUM_LINES_1CIFS_NTSC : NUM_LINES_1CIFS_PAL) * 2)
+		f->fmt.pix.field = V4L2_FIELD_INTERLACED;
+	else
+		f->fmt.pix.field = V4L2_FIELD_TOP;
 	f->fmt.pix.pixelformat = channel->fmt->fourcc;
 	f->fmt.pix.bytesperline = f->fmt.pix.width * (channel->fmt->depth >> 3);
 	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	f->fmt.pix.priv = 0;
 	return 0;
 }
 
@@ -880,12 +874,9 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 {
 	const struct s2255_fmt *fmt;
 	enum v4l2_field field;
-	int  b_any_field = 0;
 	struct s2255_fh *fh = priv;
 	struct s2255_channel *channel = fh->channel;
-	int is_ntsc;
-	is_ntsc =
-		(channel->vdev.current_norm & V4L2_STD_NTSC) ? 1 : 0;
+	int is_ntsc = channel->std & V4L2_STD_525_60;
 
 	fmt = format_by_fourcc(f->fmt.pix.pixelformat);
 
@@ -893,8 +884,6 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 		return -EINVAL;
 
 	field = f->fmt.pix.field;
-	if (field == V4L2_FIELD_ANY)
-		b_any_field = 1;
 
 	dprintk(50, "%s NTSC: %d suggested width: %d, height: %d\n",
 		__func__, is_ntsc, f->fmt.pix.width, f->fmt.pix.height);
@@ -902,24 +891,10 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 		/* NTSC */
 		if (f->fmt.pix.height >= NUM_LINES_1CIFS_NTSC * 2) {
 			f->fmt.pix.height = NUM_LINES_1CIFS_NTSC * 2;
-			if (b_any_field) {
-				field = V4L2_FIELD_SEQ_TB;
-			} else if (!((field == V4L2_FIELD_INTERLACED) ||
-				      (field == V4L2_FIELD_SEQ_TB) ||
-				      (field == V4L2_FIELD_INTERLACED_TB))) {
-				dprintk(1, "unsupported field setting\n");
-				return -EINVAL;
-			}
+			field = V4L2_FIELD_INTERLACED;
 		} else {
 			f->fmt.pix.height = NUM_LINES_1CIFS_NTSC;
-			if (b_any_field) {
-				field = V4L2_FIELD_TOP;
-			} else if (!((field == V4L2_FIELD_TOP) ||
-				      (field == V4L2_FIELD_BOTTOM))) {
-				dprintk(1, "unsupported field setting\n");
-				return -EINVAL;
-			}
-
+			field = V4L2_FIELD_TOP;
 		}
 		if (f->fmt.pix.width >= LINE_SZ_4CIFS_NTSC)
 			f->fmt.pix.width = LINE_SZ_4CIFS_NTSC;
@@ -933,41 +908,25 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 		/* PAL */
 		if (f->fmt.pix.height >= NUM_LINES_1CIFS_PAL * 2) {
 			f->fmt.pix.height = NUM_LINES_1CIFS_PAL * 2;
-			if (b_any_field) {
-				field = V4L2_FIELD_SEQ_TB;
-			} else if (!((field == V4L2_FIELD_INTERLACED) ||
-				      (field == V4L2_FIELD_SEQ_TB) ||
-				      (field == V4L2_FIELD_INTERLACED_TB))) {
-				dprintk(1, "unsupported field setting\n");
-				return -EINVAL;
-			}
+			field = V4L2_FIELD_INTERLACED;
 		} else {
 			f->fmt.pix.height = NUM_LINES_1CIFS_PAL;
-			if (b_any_field) {
-				field = V4L2_FIELD_TOP;
-			} else if (!((field == V4L2_FIELD_TOP) ||
-				     (field == V4L2_FIELD_BOTTOM))) {
-				dprintk(1, "unsupported field setting\n");
-				return -EINVAL;
-			}
+			field = V4L2_FIELD_TOP;
 		}
-		if (f->fmt.pix.width >= LINE_SZ_4CIFS_PAL) {
+		if (f->fmt.pix.width >= LINE_SZ_4CIFS_PAL)
 			f->fmt.pix.width = LINE_SZ_4CIFS_PAL;
-			field = V4L2_FIELD_SEQ_TB;
-		} else if (f->fmt.pix.width >= LINE_SZ_2CIFS_PAL) {
+		else if (f->fmt.pix.width >= LINE_SZ_2CIFS_PAL)
 			f->fmt.pix.width = LINE_SZ_2CIFS_PAL;
-			field = V4L2_FIELD_TOP;
-		} else if (f->fmt.pix.width >= LINE_SZ_1CIFS_PAL) {
+		else if (f->fmt.pix.width >= LINE_SZ_1CIFS_PAL)
 			f->fmt.pix.width = LINE_SZ_1CIFS_PAL;
-			field = V4L2_FIELD_TOP;
-		} else {
+		else
 			f->fmt.pix.width = LINE_SZ_1CIFS_PAL;
-			field = V4L2_FIELD_TOP;
-		}
 	}
 	f->fmt.pix.field = field;
 	f->fmt.pix.bytesperline = (f->fmt.pix.width * fmt->depth) >> 3;
 	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	f->fmt.pix.priv = 0;
 	dprintk(50, "%s: set width %d height %d field %d\n", __func__,
 		f->fmt.pix.width, f->fmt.pix.height, f->fmt.pix.field);
 	return 0;
@@ -1012,8 +971,8 @@ 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;
-	if (channel->width > norm_minw(&channel->vdev)) {
-		if (channel->height > norm_minh(&channel->vdev)) {
+	if (channel->width > norm_minw(channel)) {
+		if (channel->height > norm_minh(channel)) {
 			if (channel->cap_parm.capturemode &
 			    V4L2_MODE_HIGHQUALITY)
 				mode.scale = SCALE_4CIFSI;
@@ -1035,7 +994,7 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 	case V4L2_PIX_FMT_MJPEG:
 		mode.color &= ~MASK_COLOR;
 		mode.color |= COLOR_JPG;
-		mode.color |= (channel->jc.quality << 8);
+		mode.color |= (channel->jpegqual << 8);
 		break;
 	case V4L2_PIX_FMT_YUV422P:
 		mode.color &= ~MASK_COLOR;
@@ -1198,6 +1157,8 @@ static int s2255_set_mode(struct s2255_channel *channel,
 	__le32 *buffer;
 	unsigned long chn_rev;
 	struct s2255_dev *dev = to_s2255_dev(channel->vdev.v4l2_dev);
+	int i;
+
 	chn_rev = G_chnmap[channel->idx];
 	dprintk(3, "%s channel: %d\n", __func__, channel->idx);
 	/* if JPEG, set the quality */
@@ -1205,7 +1166,7 @@ static int s2255_set_mode(struct s2255_channel *channel,
 		mode->color &= ~MASK_COLOR;
 		mode->color |= COLOR_JPG;
 		mode->color &= ~MASK_JPG_QUALITY;
-		mode->color |= (channel->jc.quality << 8);
+		mode->color |= (channel->jpegqual << 8);
 	}
 	/* save the mode */
 	channel->mode = *mode;
@@ -1220,7 +1181,8 @@ static int s2255_set_mode(struct s2255_channel *channel,
 	buffer[0] = IN_DATA_TOKEN;
 	buffer[1] = (__le32) cpu_to_le32(chn_rev);
 	buffer[2] = CMD_SET_MODE;
-	memcpy(&buffer[3], &channel->mode, sizeof(struct s2255_mode));
+	for (i = 0; i < sizeof(struct s2255_mode) / sizeof(u32); i++)
+		buffer[3 + i] = cpu_to_le32(((u32 *)&channel->mode)[i]);
 	channel->setmode_ready = 0;
 	res = s2255_write_config(dev->udev, (unsigned char *)buffer, 512);
 	if (debug)
@@ -1332,12 +1294,14 @@ static int vidioc_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
 	return 0;
 }
 
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *i)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id i)
 {
 	struct s2255_fh *fh = priv;
 	struct s2255_mode mode;
 	struct videobuf_queue *q = &fh->vb_vidq;
+	struct s2255_channel *channel = fh->channel;
 	int ret = 0;
+
 	mutex_lock(&q->vb_lock);
 	if (videobuf_queue_is_busy(q)) {
 		dprintk(1, "queue busy\n");
@@ -1350,24 +1314,30 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *i)
 		goto out_s_std;
 	}
 	mode = fh->channel->mode;
-	if (*i & V4L2_STD_NTSC) {
-		dprintk(4, "%s NTSC\n", __func__);
+	if (i & V4L2_STD_525_60) {
+		dprintk(4, "%s 60 Hz\n", __func__);
 		/* if changing format, reset frame decimation/intervals */
 		if (mode.format != FORMAT_NTSC) {
 			mode.restart = 1;
 			mode.format = FORMAT_NTSC;
 			mode.fdec = FDEC_1;
+			channel->width = LINE_SZ_4CIFS_NTSC;
+			channel->height = NUM_LINES_4CIFS_NTSC * 2;
 		}
-	} else if (*i & V4L2_STD_PAL) {
-		dprintk(4, "%s PAL\n", __func__);
+	} else if (i & V4L2_STD_625_50) {
+		dprintk(4, "%s 50 Hz\n", __func__);
 		if (mode.format != FORMAT_PAL) {
 			mode.restart = 1;
 			mode.format = FORMAT_PAL;
 			mode.fdec = FDEC_1;
+			channel->width = LINE_SZ_4CIFS_PAL;
+			channel->height = NUM_LINES_4CIFS_PAL * 2;
 		}
 	} else {
 		ret = -EINVAL;
+		goto out_s_std;
 	}
+	fh->channel->std = i;
 	if (mode.restart)
 		s2255_set_mode(fh->channel, &mode);
 out_s_std:
@@ -1375,6 +1345,14 @@ out_s_std:
 	return ret;
 }
 
+static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *i)
+{
+	struct s2255_fh *fh = priv;
+
+	*i = fh->channel->std;
+	return 0;
+}
+
 /* Sensoray 2255 is a multiple channel capture device.
    It does not have a "crossbar" of inputs.
    We use one V4L device per channel. The user must
@@ -1427,110 +1405,36 @@ static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
 	return 0;
 }
 
-/* --- controls ---------------------------------------------- */
-static int vidioc_queryctrl(struct file *file, void *priv,
-			    struct v4l2_queryctrl *qc)
-{
-	struct s2255_fh *fh = priv;
-	struct s2255_channel *channel = fh->channel;
-	struct s2255_dev *dev = fh->dev;
-	switch (qc->id) {
-	case V4L2_CID_BRIGHTNESS:
-		v4l2_ctrl_query_fill(qc, -127, 127, 1, DEF_BRIGHT);
-		break;
-	case V4L2_CID_CONTRAST:
-		v4l2_ctrl_query_fill(qc, 0, 255, 1, DEF_CONTRAST);
-		break;
-	case V4L2_CID_SATURATION:
-		v4l2_ctrl_query_fill(qc, 0, 255, 1, DEF_SATURATION);
-		break;
-	case V4L2_CID_HUE:
-		v4l2_ctrl_query_fill(qc, 0, 255, 1, DEF_HUE);
-		break;
-	case V4L2_CID_PRIVATE_COLORFILTER:
-		if (dev->dsp_fw_ver < S2255_MIN_DSP_COLORFILTER)
-			return -EINVAL;
-		if ((dev->pid == 0x2257) && (channel->idx > 1))
-			return -EINVAL;
-		strlcpy(qc->name, "Color Filter", sizeof(qc->name));
-		qc->type = V4L2_CTRL_TYPE_MENU;
-		qc->minimum = 0;
-		qc->maximum = 1;
-		qc->step = 1;
-		qc->default_value = 1;
-		qc->flags = 0;
-		break;
-	default:
-		return -EINVAL;
-	}
-	dprintk(4, "%s, id %d\n", __func__, qc->id);
-	return 0;
-}
-
-static int vidioc_g_ctrl(struct file *file, void *priv,
-			 struct v4l2_control *ctrl)
-{
-	struct s2255_fh *fh = priv;
-	struct s2255_dev *dev = fh->dev;
-	struct s2255_channel *channel = fh->channel;
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		ctrl->value = channel->mode.bright;
-		break;
-	case V4L2_CID_CONTRAST:
-		ctrl->value = channel->mode.contrast;
-		break;
-	case V4L2_CID_SATURATION:
-		ctrl->value = channel->mode.saturation;
-		break;
-	case V4L2_CID_HUE:
-		ctrl->value = channel->mode.hue;
-		break;
-	case V4L2_CID_PRIVATE_COLORFILTER:
-		if (dev->dsp_fw_ver < S2255_MIN_DSP_COLORFILTER)
-			return -EINVAL;
-		if ((dev->pid == 0x2257) && (channel->idx > 1))
-			return -EINVAL;
-		ctrl->value = !((channel->mode.color & MASK_INPUT_TYPE) >> 16);
-		break;
-	default:
-		return -EINVAL;
-	}
-	dprintk(4, "%s, id %d val %d\n", __func__, ctrl->id, ctrl->value);
-	return 0;
-}
-
-static int vidioc_s_ctrl(struct file *file, void *priv,
-			 struct v4l2_control *ctrl)
+static int s2255_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct s2255_fh *fh = priv;
-	struct s2255_channel *channel = fh->channel;
-	struct s2255_dev *dev = to_s2255_dev(channel->vdev.v4l2_dev);
+	struct s2255_channel *channel =
+		container_of(ctrl->handler, struct s2255_channel, hdl);
 	struct s2255_mode mode;
+
 	mode = channel->mode;
 	dprintk(4, "%s\n", __func__);
+
 	/* update the mode to the corresponding value */
 	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		mode.bright = ctrl->value;
+		mode.bright = ctrl->val;
 		break;
 	case V4L2_CID_CONTRAST:
-		mode.contrast = ctrl->value;
+		mode.contrast = ctrl->val;
 		break;
 	case V4L2_CID_HUE:
-		mode.hue = ctrl->value;
+		mode.hue = ctrl->val;
 		break;
 	case V4L2_CID_SATURATION:
-		mode.saturation = ctrl->value;
+		mode.saturation = ctrl->val;
 		break;
-	case V4L2_CID_PRIVATE_COLORFILTER:
-		if (dev->dsp_fw_ver < S2255_MIN_DSP_COLORFILTER)
-			return -EINVAL;
-		if ((dev->pid == 0x2257) && (channel->idx > 1))
-			return -EINVAL;
+	case V4L2_CID_S2255_COLORFILTER:
 		mode.color &= ~MASK_INPUT_TYPE;
-		mode.color |= ((ctrl->value ? 0 : 1) << 16);
+		mode.color |= !ctrl->val << 16;
 		break;
+	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
+		channel->jpegqual = ctrl->val;
+		return 0;
 	default:
 		return -EINVAL;
 	}
@@ -1539,7 +1443,7 @@ static int vidioc_s_ctrl(struct file *file, void *priv,
 	   some V4L programs restart stream unnecessarily
 	   after a s_crtl.
 	*/
-	s2255_set_mode(fh->channel, &mode);
+	s2255_set_mode(channel, &mode);
 	return 0;
 }
 
@@ -1548,7 +1452,9 @@ static int vidioc_g_jpegcomp(struct file *file, void *priv,
 {
 	struct s2255_fh *fh = priv;
 	struct s2255_channel *channel = fh->channel;
-	*jc = channel->jc;
+
+	memset(jc, 0, sizeof(*jc));
+	jc->quality = channel->jpegqual;
 	dprintk(2, "%s: quality %d\n", __func__, jc->quality);
 	return 0;
 }
@@ -1560,7 +1466,7 @@ static int vidioc_s_jpegcomp(struct file *file, void *priv,
 	struct s2255_channel *channel = fh->channel;
 	if (jc->quality < 0 || jc->quality > 100)
 		return -EINVAL;
-	channel->jc.quality = jc->quality;
+	v4l2_ctrl_s_ctrl(channel->jpegqual_ctrl, jc->quality);
 	dprintk(2, "%s: quality %d\n", __func__, jc->quality);
 	return 0;
 }
@@ -1573,7 +1479,6 @@ static int vidioc_g_parm(struct file *file, void *priv,
 	struct s2255_channel *channel = fh->channel;
 	if (sp->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
-	memset(sp, 0, sizeof(struct v4l2_streamparm));
 	sp->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
 	sp->parm.capture.capturemode = channel->cap_parm.capturemode;
 	def_num = (channel->mode.format == FORMAT_NTSC) ? 1001 : 1000;
@@ -1643,36 +1548,64 @@ static int vidioc_s_parm(struct file *file, void *priv,
 	return 0;
 }
 
+#define NUM_SIZE_ENUMS 3
+static const struct v4l2_frmsize_discrete ntsc_sizes[] = {
+	{ 640, 480 },
+	{ 640, 240 },
+	{ 320, 240 },
+};
+static const struct v4l2_frmsize_discrete pal_sizes[] = {
+	{ 704, 576 },
+	{ 704, 288 },
+	{ 352, 288 },
+};
+
+static int vidioc_enum_framesizes(struct file *file, void *priv,
+			    struct v4l2_frmsizeenum *fe)
+{
+	struct s2255_fh *fh = priv;
+	struct s2255_channel *channel = fh->channel;
+	int is_ntsc = channel->std & V4L2_STD_525_60;
+	const struct s2255_fmt *fmt;
+
+	if (fe->index >= NUM_SIZE_ENUMS)
+		return -EINVAL;
+
+	fmt = format_by_fourcc(fe->pixel_format);
+	if (fmt == NULL)
+		return -EINVAL;
+	fe->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+	fe->discrete = is_ntsc ?  ntsc_sizes[fe->index] : pal_sizes[fe->index];
+	return 0;
+}
+
 static int vidioc_enum_frameintervals(struct file *file, void *priv,
 			    struct v4l2_frmivalenum *fe)
 {
-	int is_ntsc = 0;
+	struct s2255_fh *fh = priv;
+	struct s2255_channel *channel = fh->channel;
+	const struct s2255_fmt *fmt;
+	const struct v4l2_frmsize_discrete *sizes;
+	int is_ntsc = channel->std & V4L2_STD_525_60;
 #define NUM_FRAME_ENUMS 4
 	int frm_dec[NUM_FRAME_ENUMS] = {1, 2, 3, 5};
+	int i;
+
 	if (fe->index >= NUM_FRAME_ENUMS)
 		return -EINVAL;
-	switch (fe->width) {
-	case 640:
-		if (fe->height != 240 && fe->height != 480)
-			return -EINVAL;
-		is_ntsc = 1;
-		break;
-	case 320:
-		if (fe->height != 240)
-			return -EINVAL;
-		is_ntsc = 1;
-		break;
-	case 704:
-		if (fe->height != 288 && fe->height != 576)
-			return -EINVAL;
-		break;
-	case 352:
-		if (fe->height != 288)
-			return -EINVAL;
-		break;
-	default:
+
+	fmt = format_by_fourcc(fe->pixel_format);
+	if (fmt == NULL)
 		return -EINVAL;
-	}
+
+	sizes = is_ntsc ? ntsc_sizes : pal_sizes;
+	for (i = 0; i < NUM_SIZE_ENUMS; i++, sizes++)
+		if (fe->width == sizes->width &&
+		    fe->height == sizes->height)
+			break;
+	if (i == NUM_SIZE_ENUMS)
+		return -EINVAL;
+
 	fe->type = V4L2_FRMIVAL_TYPE_DISCRETE;
 	fe->discrete.denominator = is_ntsc ? 30000 : 25000;
 	fe->discrete.numerator = (is_ntsc ? 1001 : 1000) * frm_dec[fe->index];
@@ -1757,7 +1690,9 @@ static int __s2255_open(struct file *file)
 	fh = kzalloc(sizeof(*fh), GFP_KERNEL);
 	if (NULL == fh)
 		return -ENOMEM;
-	file->private_data = fh;
+	v4l2_fh_init(&fh->fh, vdev);
+	v4l2_fh_add(&fh->fh);
+	file->private_data = &fh->fh;
 	fh->dev = dev;
 	fh->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 	fh->channel = channel;
@@ -1800,12 +1735,13 @@ static unsigned int s2255_poll(struct file *file,
 {
 	struct s2255_fh *fh = file->private_data;
 	struct s2255_dev *dev = fh->dev;
-	int rc;
+	int rc = v4l2_ctrl_poll(file, wait);
+
 	dprintk(100, "%s\n", __func__);
 	if (V4L2_BUF_TYPE_VIDEO_CAPTURE != fh->type)
 		return POLLERR;
 	mutex_lock(&dev->lock);
-	rc = videobuf_poll_stream(file, &fh->vb_vidq, wait);
+	rc |= videobuf_poll_stream(file, &fh->vb_vidq, wait);
 	mutex_unlock(&dev->lock);
 	return rc;
 }
@@ -1852,6 +1788,8 @@ static int s2255_release(struct file *file)
 	videobuf_mmap_free(&fh->vb_vidq);
 	mutex_unlock(&dev->lock);
 	dprintk(1, "%s (dev=%s)\n", __func__, video_device_node_name(vdev));
+	v4l2_fh_del(&fh->fh);
+	v4l2_fh_exit(&fh->fh);
 	kfree(fh);
 	return 0;
 }
@@ -1886,7 +1824,6 @@ static const struct v4l2_file_operations s2255_fops_v4l = {
 };
 
 static const struct v4l2_ioctl_ops s2255_ioctl_ops = {
-	.vidioc_querymenu = vidioc_querymenu,
 	.vidioc_querycap = vidioc_querycap,
 	.vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
 	.vidioc_g_fmt_vid_cap = vidioc_g_fmt_vid_cap,
@@ -1897,26 +1834,33 @@ static const struct v4l2_ioctl_ops s2255_ioctl_ops = {
 	.vidioc_qbuf = vidioc_qbuf,
 	.vidioc_dqbuf = vidioc_dqbuf,
 	.vidioc_s_std = vidioc_s_std,
+	.vidioc_g_std = vidioc_g_std,
 	.vidioc_enum_input = vidioc_enum_input,
 	.vidioc_g_input = vidioc_g_input,
 	.vidioc_s_input = vidioc_s_input,
-	.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_s_jpegcomp = vidioc_s_jpegcomp,
 	.vidioc_g_jpegcomp = vidioc_g_jpegcomp,
 	.vidioc_s_parm = vidioc_s_parm,
 	.vidioc_g_parm = vidioc_g_parm,
+	.vidioc_enum_framesizes = vidioc_enum_framesizes,
 	.vidioc_enum_frameintervals = vidioc_enum_frameintervals,
+	.vidioc_log_status  = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 static void s2255_video_device_release(struct video_device *vdev)
 {
 	struct s2255_dev *dev = to_s2255_dev(vdev->v4l2_dev);
-	dprintk(4, "%s, chnls: %d \n", __func__,
+	struct s2255_channel *channel =
+		container_of(vdev, struct s2255_channel, vdev);
+
+	v4l2_ctrl_handler_free(&channel->hdl);
+	dprintk(4, "%s, chnls: %d\n", __func__,
 		atomic_read(&dev->num_channels));
+
 	if (atomic_dec_and_test(&dev->num_channels))
 		s2255_destroy(dev);
 	return;
@@ -1928,7 +1872,20 @@ static struct video_device template = {
 	.ioctl_ops = &s2255_ioctl_ops,
 	.release = s2255_video_device_release,
 	.tvnorms = S2255_NORMS,
-	.current_norm = V4L2_STD_NTSC_M,
+};
+
+static const struct v4l2_ctrl_ops s2255_ctrl_ops = {
+	.s_ctrl = s2255_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config color_filter_ctrl = {
+	.ops = &s2255_ctrl_ops,
+	.name = "Color Filter",
+	.id = V4L2_CID_S2255_COLORFILTER,
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+	.def = 1,
 };
 
 static int s2255_probe_v4l(struct s2255_dev *dev)
@@ -1945,11 +1902,36 @@ static int s2255_probe_v4l(struct s2255_dev *dev)
 	for (i = 0; i < MAX_CHANNELS; i++) {
 		channel = &dev->channel[i];
 		INIT_LIST_HEAD(&channel->vidq.active);
+
+		v4l2_ctrl_handler_init(&channel->hdl, 6);
+		v4l2_ctrl_new_std(&channel->hdl, &s2255_ctrl_ops,
+				V4L2_CID_BRIGHTNESS, -127, 127, 1, DEF_BRIGHT);
+		v4l2_ctrl_new_std(&channel->hdl, &s2255_ctrl_ops,
+				V4L2_CID_CONTRAST, 0, 255, 1, DEF_CONTRAST);
+		v4l2_ctrl_new_std(&channel->hdl, &s2255_ctrl_ops,
+				V4L2_CID_SATURATION, 0, 255, 1, DEF_SATURATION);
+		v4l2_ctrl_new_std(&channel->hdl, &s2255_ctrl_ops,
+				V4L2_CID_HUE, 0, 255, 1, DEF_HUE);
+		channel->jpegqual_ctrl = v4l2_ctrl_new_std(&channel->hdl,
+				&s2255_ctrl_ops,
+				V4L2_CID_JPEG_COMPRESSION_QUALITY,
+				0, 100, 1, S2255_DEF_JPEG_QUAL);
+		if (dev->dsp_fw_ver >= S2255_MIN_DSP_COLORFILTER &&
+		    (dev->pid != 0x2257 || channel->idx <= 1))
+			v4l2_ctrl_new_custom(&channel->hdl, &color_filter_ctrl, NULL);
+		if (channel->hdl.error) {
+			ret = channel->hdl.error;
+			v4l2_ctrl_handler_free(&channel->hdl);
+			dev_err(&dev->udev->dev, "couldn't register control\n");
+			break;
+		}
 		channel->vidq.dev = dev;
 		/* register 4 video devices */
 		channel->vdev = template;
+		channel->vdev.ctrl_handler = &channel->hdl;
 		channel->vdev.lock = &dev->lock;
 		channel->vdev.v4l2_dev = &dev->v4l2_dev;
+		set_bit(V4L2_FL_USE_FH_PRIO, &channel->vdev.flags);
 		video_set_drvdata(&channel->vdev, channel);
 		if (video_nr == -1)
 			ret = video_register_device(&channel->vdev,
@@ -2300,9 +2282,10 @@ static int s2255_board_init(struct s2255_dev *dev)
 		channel->mode = mode_def;
 		if (dev->pid == 0x2257 && j > 1)
 			channel->mode.color |= (1 << 16);
-		channel->jc.quality = S2255_DEF_JPEG_QUAL;
+		channel->jpegqual = S2255_DEF_JPEG_QUAL;
 		channel->width = LINE_SZ_4CIFS_NTSC;
 		channel->height = NUM_LINES_4CIFS_NTSC * 2;
+		channel->std = V4L2_STD_NTSC_M;
 		channel->fmt = &formats[0];
 		channel->mode.restart = 1;
 		channel->req_image_size = get_transfer_size(&mode_def);
@@ -2531,7 +2514,7 @@ static int s2255_probe(struct usb_interface *interface,
 		return -ENOMEM;
 	}
 	atomic_set(&dev->num_channels, 0);
-	dev->pid = id->idProduct;
+	dev->pid = le16_to_cpu(id->idProduct);
 	dev->fw_data = kzalloc(sizeof(struct s2255_fw), GFP_KERNEL);
 	if (!dev->fw_data)
 		goto errorFWDATA1;
@@ -2601,7 +2584,7 @@ static int s2255_probe(struct usb_interface *interface,
 		/* make sure firmware is the latest */
 		__le32 *pRel;
 		pRel = (__le32 *) &dev->fw_data->fw->data[fw_size - 4];
-		printk(KERN_INFO "s2255 dsp fw version %x\n", *pRel);
+		printk(KERN_INFO "s2255 dsp fw version %x\n", le32_to_cpu(*pRel));
 		dev->dsp_fw_ver = le32_to_cpu(*pRel);
 		if (dev->dsp_fw_ver < S2255_CUR_DSP_FWVER)
 			printk(KERN_INFO "s2255: f2255usb.bin out of date.\n");
diff --git a/drivers/media/usb/siano/smsusb.c b/drivers/media/usb/siano/smsusb.c
index de2c10289ee..03761c6f472 100644
--- a/drivers/media/usb/siano/smsusb.c
+++ b/drivers/media/usb/siano/smsusb.c
@@ -35,16 +35,23 @@ module_param_named(debug, sms_dbg, int, 0644);
 MODULE_PARM_DESC(debug, "set debug level (info=1, adv=2 (or-able))");
 
 #define USB1_BUFFER_SIZE		0x1000
-#define USB2_BUFFER_SIZE		0x4000
+#define USB2_BUFFER_SIZE		0x2000
 
 #define MAX_BUFFERS		50
 #define MAX_URBS		10
 
 struct smsusb_device_t;
 
+enum smsusb_state {
+	SMSUSB_DISCONNECTED,
+	SMSUSB_SUSPENDED,
+	SMSUSB_ACTIVE
+};
+
 struct smsusb_urb_t {
+	struct list_head entry;
 	struct smscore_buffer_t *cb;
-	struct smsusb_device_t	*dev;
+	struct smsusb_device_t *dev;
 
 	struct urb urb;
 };
@@ -57,11 +64,23 @@ struct smsusb_device_t {
 
 	int		response_alignment;
 	int		buffer_size;
+
+	unsigned char in_ep;
+	unsigned char out_ep;
+	enum smsusb_state state;
 };
 
 static int smsusb_submit_urb(struct smsusb_device_t *dev,
 			     struct smsusb_urb_t *surb);
 
+/**
+ * Completing URB's callback handler - top half (interrupt context)
+ * adds completing sms urb to the global surbs list and activtes the worker
+ * thread the surb
+ * IMPORTANT - blocking functions must not be called from here !!!
+
+ * @param urb pointer to a completing urb object
+ */
 static void smsusb_onresponse(struct urb *urb)
 {
 	struct smsusb_urb_t *surb = (struct smsusb_urb_t *) urb->context;
@@ -74,26 +93,26 @@ static void smsusb_onresponse(struct urb *urb)
 	}
 
 	if ((urb->actual_length > 0) && (urb->status == 0)) {
-		struct SmsMsgHdr_ST *phdr = (struct SmsMsgHdr_ST *)surb->cb->p;
+		struct sms_msg_hdr *phdr = (struct sms_msg_hdr *)surb->cb->p;
 
 		smsendian_handle_message_header(phdr);
-		if (urb->actual_length >= phdr->msgLength) {
-			surb->cb->size = phdr->msgLength;
+		if (urb->actual_length >= phdr->msg_length) {
+			surb->cb->size = phdr->msg_length;
 
 			if (dev->response_alignment &&
-			    (phdr->msgFlags & MSG_HDR_FLAG_SPLIT_MSG)) {
+			    (phdr->msg_flags & MSG_HDR_FLAG_SPLIT_MSG)) {
 
 				surb->cb->offset =
 					dev->response_alignment +
-					((phdr->msgFlags >> 8) & 3);
+					((phdr->msg_flags >> 8) & 3);
 
 				/* sanity check */
-				if (((int) phdr->msgLength +
+				if (((int) phdr->msg_length +
 				     surb->cb->offset) > urb->actual_length) {
 					sms_err("invalid response "
 						"msglen %d offset %d "
 						"size %d",
-						phdr->msgLength,
+						phdr->msg_length,
 						surb->cb->offset,
 						urb->actual_length);
 					goto exit_and_resubmit;
@@ -102,16 +121,22 @@ static void smsusb_onresponse(struct urb *urb)
 				/* move buffer pointer and
 				 * copy header to its new location */
 				memcpy((char *) phdr + surb->cb->offset,
-				       phdr, sizeof(struct SmsMsgHdr_ST));
+				       phdr, sizeof(struct sms_msg_hdr));
 			} else
 				surb->cb->offset = 0;
 
+			sms_debug("received %s(%d) size: %d",
+				  smscore_translate_msg(phdr->msg_type),
+				  phdr->msg_type, phdr->msg_length);
+
+			smsendian_handle_rx_message((struct sms_msg_data *) phdr);
+
 			smscore_onresponse(dev->coredev, surb->cb);
 			surb->cb = NULL;
 		} else {
 			sms_err("invalid response "
 				"msglen %d actual %d",
-				phdr->msgLength, urb->actual_length);
+				phdr->msg_length, urb->actual_length);
 		}
 	} else
 		sms_err("error, urb status %d, %d bytes",
@@ -136,7 +161,7 @@ static int smsusb_submit_urb(struct smsusb_device_t *dev,
 	usb_fill_bulk_urb(
 		&surb->urb,
 		dev->udev,
-		usb_rcvbulkpipe(dev->udev, 0x81),
+		usb_rcvbulkpipe(dev->udev, dev->in_ep),
 		surb->cb->p,
 		dev->buffer_size,
 		smsusb_onresponse,
@@ -181,9 +206,18 @@ static int smsusb_start_streaming(struct smsusb_device_t *dev)
 static int smsusb_sendrequest(void *context, void *buffer, size_t size)
 {
 	struct smsusb_device_t *dev = (struct smsusb_device_t *) context;
+	struct sms_msg_hdr *phdr = (struct sms_msg_hdr *) buffer;
 	int dummy;
 
-	smsendian_handle_message_header((struct SmsMsgHdr_ST *)buffer);
+	if (dev->state != SMSUSB_ACTIVE)
+		return -ENOENT;
+
+	sms_debug("sending %s(%d) size: %d",
+		  smscore_translate_msg(phdr->msg_type), phdr->msg_type,
+		  phdr->msg_length);
+
+	smsendian_handle_tx_message((struct sms_msg_data *) phdr);
+	smsendian_handle_message_header((struct sms_msg_hdr *)buffer);
 	return usb_bulk_msg(dev->udev, usb_sndbulkpipe(dev->udev, 2),
 			    buffer, size, &dummy, 1000);
 }
@@ -276,15 +310,15 @@ static void smsusb1_detectmode(void *context, int *mode)
 
 static int smsusb1_setmode(void *context, int mode)
 {
-	struct SmsMsgHdr_ST Msg = { MSG_SW_RELOAD_REQ, 0, HIF_TASK,
-			     sizeof(struct SmsMsgHdr_ST), 0 };
+	struct sms_msg_hdr msg = { MSG_SW_RELOAD_REQ, 0, HIF_TASK,
+			     sizeof(struct sms_msg_hdr), 0 };
 
 	if (mode < DEVICE_MODE_DVBT || mode > DEVICE_MODE_DVBT_BDA) {
 		sms_err("invalid firmware id specified %d", mode);
 		return -EINVAL;
 	}
 
-	return smsusb_sendrequest(context, &Msg, sizeof(Msg));
+	return smsusb_sendrequest(context, &msg, sizeof(msg));
 }
 
 static void smsusb_term_device(struct usb_interface *intf)
@@ -292,13 +326,15 @@ static void smsusb_term_device(struct usb_interface *intf)
 	struct smsusb_device_t *dev = usb_get_intfdata(intf);
 
 	if (dev) {
+		dev->state = SMSUSB_DISCONNECTED;
+
 		smsusb_stop_streaming(dev);
 
 		/* unregister from smscore */
 		if (dev->coredev)
 			smscore_unregister_device(dev->coredev);
 
-		sms_info("device %p destroyed", dev);
+		sms_info("device 0x%p destroyed", dev);
 		kfree(dev);
 	}
 
@@ -321,6 +357,7 @@ static int smsusb_init_device(struct usb_interface *intf, int board_id)
 	memset(&params, 0, sizeof(params));
 	usb_set_intfdata(intf, dev);
 	dev->udev = interface_to_usbdev(intf);
+	dev->state = SMSUSB_DISCONNECTED;
 
 	params.device_type = sms_get_board(board_id)->type;
 
@@ -331,21 +368,29 @@ static int smsusb_init_device(struct usb_interface *intf, int board_id)
 		params.setmode_handler = smsusb1_setmode;
 		params.detectmode_handler = smsusb1_detectmode;
 		break;
-	default:
+	case SMS_UNKNOWN_TYPE:
 		sms_err("Unspecified sms device type!");
 		/* fall-thru */
-	case SMS_NOVA_A0:
-	case SMS_NOVA_B0:
-	case SMS_VEGA:
+	default:
 		dev->buffer_size = USB2_BUFFER_SIZE;
 		dev->response_alignment =
 		    le16_to_cpu(dev->udev->ep_in[1]->desc.wMaxPacketSize) -
-		    sizeof(struct SmsMsgHdr_ST);
+		    sizeof(struct sms_msg_hdr);
 
 		params.flags |= SMS_DEVICE_FAMILY2;
 		break;
 	}
 
+	for (i = 0; i < intf->cur_altsetting->desc.bNumEndpoints; i++) {
+		if (intf->cur_altsetting->endpoint[i].desc. bEndpointAddress & USB_DIR_IN)
+			dev->in_ep = intf->cur_altsetting->endpoint[i].desc.bEndpointAddress;
+		else
+			dev->out_ep = intf->cur_altsetting->endpoint[i].desc.bEndpointAddress;
+	}
+
+	sms_info("in_ep = %02x, out_ep = %02x",
+		dev->in_ep, dev->out_ep);
+
 	params.device = &dev->udev->dev;
 	params.buffer_size = dev->buffer_size;
 	params.num_buffers = MAX_BUFFERS;
@@ -363,6 +408,8 @@ static int smsusb_init_device(struct usb_interface *intf, int board_id)
 
 	smscore_set_board_id(dev->coredev, board_id);
 
+	dev->coredev->is_usb_device = true;
+
 	/* initialize urbs */
 	for (i = 0; i < MAX_URBS; i++) {
 		dev->surbs[i].dev = dev;
@@ -377,6 +424,8 @@ static int smsusb_init_device(struct usb_interface *intf, int board_id)
 		return rc;
 	}
 
+	dev->state = SMSUSB_ACTIVE;
+
 	rc = smscore_start_device(dev->coredev);
 	if (rc < 0) {
 		sms_err("smscore_start_device(...) failed");
@@ -384,7 +433,7 @@ static int smsusb_init_device(struct usb_interface *intf, int board_id)
 		return rc;
 	}
 
-	sms_info("device %p created", dev);
+	sms_info("device 0x%p created", dev);
 
 	return rc;
 }
@@ -396,12 +445,21 @@ static int smsusb_probe(struct usb_interface *intf,
 	char devpath[32];
 	int i, rc;
 
-	rc = usb_clear_halt(udev, usb_rcvbulkpipe(udev, 0x81));
-	rc = usb_clear_halt(udev, usb_rcvbulkpipe(udev, 0x02));
+	sms_info("interface number %d",
+		 intf->cur_altsetting->desc.bInterfaceNumber);
 
-	if (intf->num_altsetting > 0) {
-		rc = usb_set_interface(
-			udev, intf->cur_altsetting->desc.bInterfaceNumber, 0);
+	if (sms_get_board(id->driver_info)->intf_num !=
+	    intf->cur_altsetting->desc.bInterfaceNumber) {
+		sms_err("interface number is %d expecting %d",
+			sms_get_board(id->driver_info)->intf_num,
+			intf->cur_altsetting->desc.bInterfaceNumber);
+		return -ENODEV;
+	}
+
+	if (intf->num_altsetting > 1) {
+		rc = usb_set_interface(udev,
+				       intf->cur_altsetting->desc.bInterfaceNumber,
+				       0);
 		if (rc < 0) {
 			sms_err("usb_set_interface failed, rc %d", rc);
 			return rc;
@@ -410,19 +468,27 @@ static int smsusb_probe(struct usb_interface *intf,
 
 	sms_info("smsusb_probe %d",
 	       intf->cur_altsetting->desc.bInterfaceNumber);
-	for (i = 0; i < intf->cur_altsetting->desc.bNumEndpoints; i++)
+	for (i = 0; i < intf->cur_altsetting->desc.bNumEndpoints; i++) {
 		sms_info("endpoint %d %02x %02x %d", i,
 		       intf->cur_altsetting->endpoint[i].desc.bEndpointAddress,
 		       intf->cur_altsetting->endpoint[i].desc.bmAttributes,
 		       intf->cur_altsetting->endpoint[i].desc.wMaxPacketSize);
-
+		if (intf->cur_altsetting->endpoint[i].desc.bEndpointAddress &
+		    USB_DIR_IN)
+			rc = usb_clear_halt(udev, usb_rcvbulkpipe(udev,
+				intf->cur_altsetting->endpoint[i].desc.bEndpointAddress));
+		else
+			rc = usb_clear_halt(udev, usb_sndbulkpipe(udev,
+				intf->cur_altsetting->endpoint[i].desc.bEndpointAddress));
+	}
 	if ((udev->actconfig->desc.bNumInterfaces == 2) &&
 	    (intf->cur_altsetting->desc.bInterfaceNumber == 0)) {
 		sms_err("rom interface 0 is not used");
 		return -ENODEV;
 	}
 
-	if (intf->cur_altsetting->desc.bInterfaceNumber == 1) {
+	if (id->driver_info == SMS1XXX_BOARD_SIANO_STELLAR_ROM) {
+		sms_info("stellar device was found.");
 		snprintf(devpath, sizeof(devpath), "usb\\%d-%s",
 			 udev->bus->busnum, udev->devpath);
 		sms_info("stellar device was found.");
@@ -445,7 +511,9 @@ static void smsusb_disconnect(struct usb_interface *intf)
 static int smsusb_suspend(struct usb_interface *intf, pm_message_t msg)
 {
 	struct smsusb_device_t *dev = usb_get_intfdata(intf);
-	printk(KERN_INFO "%s: Entering status %d.\n", __func__, msg.event);
+	printk(KERN_INFO "%s  Entering status %d.\n", __func__, msg.event);
+	dev->state = SMSUSB_SUSPENDED;
+	/*smscore_set_power_mode(dev, SMS_POWER_MODE_SUSPENDED);*/
 	smsusb_stop_streaming(dev);
 	return 0;
 }
@@ -456,9 +524,9 @@ static int smsusb_resume(struct usb_interface *intf)
 	struct smsusb_device_t *dev = usb_get_intfdata(intf);
 	struct usb_device *udev = interface_to_usbdev(intf);
 
-	printk(KERN_INFO "%s: Entering.\n", __func__);
-	usb_clear_halt(udev, usb_rcvbulkpipe(udev, 0x81));
-	usb_clear_halt(udev, usb_rcvbulkpipe(udev, 0x02));
+	printk(KERN_INFO "%s  Entering.\n", __func__);
+	usb_clear_halt(udev, usb_rcvbulkpipe(udev, dev->in_ep));
+	usb_clear_halt(udev, usb_sndbulkpipe(udev, dev->out_ep));
 
 	for (i = 0; i < intf->cur_altsetting->desc.bNumEndpoints; i++)
 		printk(KERN_INFO "endpoint %d %02x %02x %d\n", i,
@@ -546,6 +614,26 @@ static const struct usb_device_id smsusb_id_table[] = {
 		.driver_info = SMS1XXX_BOARD_HAUPPAUGE_WINDHAM },
 	{ USB_DEVICE(0x2040, 0xf5a0),
 		.driver_info = SMS1XXX_BOARD_HAUPPAUGE_WINDHAM },
+	{ USB_DEVICE(0x187f, 0x0202),
+		.driver_info = SMS1XXX_BOARD_SIANO_NICE },
+	{ USB_DEVICE(0x187f, 0x0301),
+		.driver_info = SMS1XXX_BOARD_SIANO_VENICE },
+	{ USB_DEVICE(0x187f, 0x0302),
+		.driver_info = SMS1XXX_BOARD_SIANO_VENICE },
+	{ USB_DEVICE(0x187f, 0x0310),
+		.driver_info = SMS1XXX_BOARD_SIANO_MING },
+	{ USB_DEVICE(0x187f, 0x0500),
+		.driver_info = SMS1XXX_BOARD_SIANO_PELE },
+	{ USB_DEVICE(0x187f, 0x0600),
+		.driver_info = SMS1XXX_BOARD_SIANO_RIO },
+	{ USB_DEVICE(0x187f, 0x0700),
+		.driver_info = SMS1XXX_BOARD_SIANO_DENVER_2160 },
+	{ USB_DEVICE(0x187f, 0x0800),
+		.driver_info = SMS1XXX_BOARD_SIANO_DENVER_1530 },
+	{ USB_DEVICE(0x19D2, 0x0086),
+		.driver_info = SMS1XXX_BOARD_ZTE_DVB_DATA_CARD },
+	{ USB_DEVICE(0x19D2, 0x0078),
+		.driver_info = SMS1XXX_BOARD_ONDA_MDTV_DATA_CARD },
 	{ } /* Terminating entry */
 	};
 
diff --git a/drivers/media/usb/stk1160/stk1160-v4l.c b/drivers/media/usb/stk1160/stk1160-v4l.c
index 6694f9e2ca5..a59153d2f8b 100644
--- a/drivers/media/usb/stk1160/stk1160-v4l.c
+++ b/drivers/media/usb/stk1160/stk1160-v4l.c
@@ -375,7 +375,7 @@ static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *norm)
 	return 0;
 }
 
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *norm)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id norm)
 {
 	struct stk1160 *dev = video_drvdata(file);
 	struct vb2_queue *q = &dev->vb_vidq;
@@ -388,7 +388,7 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *norm)
 		return -ENODEV;
 
 	/* We need to set this now, before we call stk1160_set_std */
-	dev->norm = *norm;
+	dev->norm = norm;
 
 	/* This is taken from saa7115 video decoder */
 	if (dev->norm & V4L2_STD_525_60) {
@@ -458,7 +458,7 @@ static int vidioc_g_chip_ident(struct file *file, void *priv,
 	       struct v4l2_dbg_chip_ident *chip)
 {
 	switch (chip->match.type) {
-	case V4L2_CHIP_MATCH_HOST:
+	case V4L2_CHIP_MATCH_BRIDGE:
 		chip->ident = V4L2_IDENT_NONE;
 		chip->revision = 0;
 		return 0;
@@ -476,9 +476,6 @@ static int vidioc_g_register(struct file *file, void *priv,
 	u8 val;
 
 	switch (reg->match.type) {
-	case V4L2_CHIP_MATCH_AC97:
-		/* TODO: Support me please :-( */
-		return -EINVAL;
 	case V4L2_CHIP_MATCH_I2C_DRIVER:
 		v4l2_device_call_all(&dev->v4l2_dev, 0, core, g_register, reg);
 		return 0;
@@ -500,13 +497,11 @@ static int vidioc_g_register(struct file *file, void *priv,
 }
 
 static int vidioc_s_register(struct file *file, void *priv,
-			     struct v4l2_dbg_register *reg)
+			     const struct v4l2_dbg_register *reg)
 {
 	struct stk1160 *dev = video_drvdata(file);
 
 	switch (reg->match.type) {
-	case V4L2_CHIP_MATCH_AC97:
-		return -EINVAL;
 	case V4L2_CHIP_MATCH_I2C_DRIVER:
 		v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_register, reg);
 		return 0;
@@ -687,6 +682,7 @@ int stk1160_vb2_setup(struct stk1160 *dev)
 	q->buf_struct_size = sizeof(struct stk1160_buffer);
 	q->ops = &stk1160_video_qops;
 	q->mem_ops = &vb2_vmalloc_memops;
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 
 	rc = vb2_queue_init(q);
 	if (rc < 0)
diff --git a/drivers/media/usb/stkwebcam/stk-webcam.c b/drivers/media/usb/stkwebcam/stk-webcam.c
index 4cbab085e34..c43c8d32be4 100644
--- a/drivers/media/usb/stkwebcam/stk-webcam.c
+++ b/drivers/media/usb/stkwebcam/stk-webcam.c
@@ -35,6 +35,7 @@
 #include <linux/videodev2.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
 
 #include "stk-webcam.h"
 
@@ -63,7 +64,39 @@ static struct usb_device_id stkwebcam_table[] = {
 };
 MODULE_DEVICE_TABLE(usb, stkwebcam_table);
 
-/* The stk webcam laptop module is mounted upside down in some laptops :( */
+/*
+ * The stk webcam laptop module is mounted upside down in some laptops :(
+ *
+ * Some background information (thanks to Hans de Goede for providing this):
+ *
+ * 1) Once upon a time the stkwebcam driver was written
+ *
+ * 2) The webcam in question was used mostly in Asus laptop models, including
+ * the laptop of the original author of the driver, and in these models, in
+ * typical Asus fashion (see the long long list for uvc cams inside v4l-utils),
+ * they mounted the webcam-module the wrong way up. So the hflip and vflip
+ * module options were given a default value of 1 (the correct value for
+ * upside down mounted models)
+ *
+ * 3) Years later I got a bug report from a user with a laptop with stkwebcam,
+ * where the module was actually mounted the right way up, and thus showed
+ * upside down under Linux. So now I was facing the choice of 2 options:
+ *
+ * a) Add a not-upside-down list to stkwebcam, which overrules the default.
+ *
+ * b) Do it like all the other drivers do, and make the default right for
+ *    cams mounted the proper way and add an upside-down model list, with
+ *    models where we need to flip-by-default.
+ *
+ * Despite knowing that going b) would cause a period of pain where we were
+ * building the table I opted to go for option b), since a) is just too ugly,
+ * and worse different from how every other driver does it leading to
+ * confusion in the long run. This change was made in kernel 3.6.
+ *
+ * So for any user report about upside-down images since kernel 3.6 ask them
+ * to provide the output of 'sudo dmidecode' so the laptop can be added in
+ * the table below.
+ */
 static const struct dmi_system_id stk_upside_down_dmi_table[] = {
 	{
 		.ident = "ASUS G1",
@@ -71,6 +104,12 @@ static const struct dmi_system_id stk_upside_down_dmi_table[] = {
 			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK Computer Inc."),
 			DMI_MATCH(DMI_PRODUCT_NAME, "G1")
 		}
+	}, {
+		.ident = "ASUS F3JC",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK Computer Inc."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "F3JC")
+		}
 	},
 	{}
 };
@@ -565,31 +604,31 @@ static void stk_free_buffers(struct stk_camera *dev)
 
 static int v4l_stk_open(struct file *fp)
 {
-	static int first_init = 1; /* webcam LED management */
-	struct stk_camera *dev;
-	struct video_device *vdev;
-
-	vdev = video_devdata(fp);
-	dev = vdev_to_camera(vdev);
+	struct stk_camera *dev = video_drvdata(fp);
+	int err;
 
 	if (dev == NULL || !is_present(dev))
 		return -ENXIO;
 
-	if (!first_init)
+	if (mutex_lock_interruptible(&dev->lock))
+		return -ERESTARTSYS;
+	if (!dev->first_init)
 		stk_camera_write_reg(dev, 0x0, 0x24);
 	else
-		first_init = 0;
-
-	fp->private_data = dev;
-	usb_autopm_get_interface(dev->interface);
+		dev->first_init = 0;
 
-	return 0;
+	err = v4l2_fh_open(fp);
+	if (!err)
+		usb_autopm_get_interface(dev->interface);
+	mutex_unlock(&dev->lock);
+	return err;
 }
 
 static int v4l_stk_release(struct file *fp)
 {
-	struct stk_camera *dev = fp->private_data;
+	struct stk_camera *dev = video_drvdata(fp);
 
+	mutex_lock(&dev->lock);
 	if (dev->owner == fp) {
 		stk_stop_stream(dev);
 		stk_free_buffers(dev);
@@ -600,22 +639,22 @@ static int v4l_stk_release(struct file *fp)
 
 	if (is_present(dev))
 		usb_autopm_put_interface(dev->interface);
-
-	return 0;
+	mutex_unlock(&dev->lock);
+	return v4l2_fh_release(fp);
 }
 
-static ssize_t v4l_stk_read(struct file *fp, char __user *buf,
+static ssize_t stk_read(struct file *fp, char __user *buf,
 		size_t count, loff_t *f_pos)
 {
 	int i;
 	int ret;
 	unsigned long flags;
 	struct stk_sio_buffer *sbuf;
-	struct stk_camera *dev = fp->private_data;
+	struct stk_camera *dev = video_drvdata(fp);
 
 	if (!is_present(dev))
 		return -EIO;
-	if (dev->owner && dev->owner != fp)
+	if (dev->owner && (!dev->reading || dev->owner != fp))
 		return -EBUSY;
 	dev->owner = fp;
 	if (!is_streaming(dev)) {
@@ -623,6 +662,7 @@ static ssize_t v4l_stk_read(struct file *fp, char __user *buf,
 			|| stk_allocate_buffers(dev, 3)
 			|| stk_start_stream(dev))
 			return -ENOMEM;
+		dev->reading = 1;
 		spin_lock_irqsave(&dev->spinlock, flags);
 		for (i = 0; i < dev->n_sbufs; i++) {
 			list_add_tail(&dev->sio_bufs[i].list, &dev->sio_avail);
@@ -665,9 +705,23 @@ static ssize_t v4l_stk_read(struct file *fp, char __user *buf,
 	return count;
 }
 
+static ssize_t v4l_stk_read(struct file *fp, char __user *buf,
+		size_t count, loff_t *f_pos)
+{
+	struct stk_camera *dev = video_drvdata(fp);
+	int ret;
+
+	if (mutex_lock_interruptible(&dev->lock))
+		return -ERESTARTSYS;
+	ret = stk_read(fp, buf, count, f_pos);
+	mutex_unlock(&dev->lock);
+	return ret;
+}
+
 static unsigned int v4l_stk_poll(struct file *fp, poll_table *wait)
 {
-	struct stk_camera *dev = fp->private_data;
+	struct stk_camera *dev = video_drvdata(fp);
+	unsigned res = v4l2_ctrl_poll(fp, wait);
 
 	poll_wait(fp, &dev->wait_frame, wait);
 
@@ -675,9 +729,9 @@ static unsigned int v4l_stk_poll(struct file *fp, poll_table *wait)
 		return POLLERR;
 
 	if (!list_empty(&dev->sio_full))
-		return POLLIN | POLLRDNORM;
+		return res | POLLIN | POLLRDNORM;
 
-	return 0;
+	return res;
 }
 
 
@@ -703,7 +757,7 @@ static int v4l_stk_mmap(struct file *fp, struct vm_area_struct *vma)
 	unsigned int i;
 	int ret;
 	unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
-	struct stk_camera *dev = fp->private_data;
+	struct stk_camera *dev = video_drvdata(fp);
 	struct stk_sio_buffer *sbuf = NULL;
 
 	if (!(vma->vm_flags & VM_WRITE) || !(vma->vm_flags & VM_SHARED))
@@ -733,12 +787,15 @@ static int v4l_stk_mmap(struct file *fp, struct vm_area_struct *vma)
 static int stk_vidioc_querycap(struct file *filp,
 		void *priv, struct v4l2_capability *cap)
 {
+	struct stk_camera *dev = video_drvdata(filp);
+
 	strcpy(cap->driver, "stk");
 	strcpy(cap->card, "stk");
-	cap->version = DRIVER_VERSION_NUM;
+	usb_make_path(dev->udev, cap->bus_info, sizeof(cap->bus_info));
 
-	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE
 		| V4L2_CAP_READWRITE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
@@ -762,111 +819,28 @@ static int stk_vidioc_g_input(struct file *filp, void *priv, unsigned int *i)
 
 static int stk_vidioc_s_input(struct file *filp, void *priv, unsigned int i)
 {
-	if (i != 0)
-		return -EINVAL;
-	else
-		return 0;
-}
-
-/* from vivi.c */
-static int stk_vidioc_s_std(struct file *filp, void *priv, v4l2_std_id *a)
-{
-	return 0;
-}
-
-/* List of all V4Lv2 controls supported by the driver */
-static struct v4l2_queryctrl stk_controls[] = {
-	{
-		.id      = V4L2_CID_BRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Brightness",
-		.minimum = 0,
-		.maximum = 0xffff,
-		.step    = 0x0100,
-		.default_value = 0x6000,
-	},
-	{
-		.id      = V4L2_CID_HFLIP,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Horizontal Flip",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 1,
-	},
-	{
-		.id      = V4L2_CID_VFLIP,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Vertical Flip",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 1,
-	},
-};
-
-static int stk_vidioc_queryctrl(struct file *filp,
-		void *priv, struct v4l2_queryctrl *c)
-{
-	int i;
-	int nbr;
-	nbr = ARRAY_SIZE(stk_controls);
-
-	for (i = 0; i < nbr; i++) {
-		if (stk_controls[i].id == c->id) {
-			memcpy(c, &stk_controls[i],
-				sizeof(struct v4l2_queryctrl));
-			return 0;
-		}
-	}
-	return -EINVAL;
+	return i ? -EINVAL : 0;
 }
 
-static int stk_vidioc_g_ctrl(struct file *filp,
-		void *priv, struct v4l2_control *c)
+static int stk_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct stk_camera *dev = priv;
-	switch (c->id) {
-	case V4L2_CID_BRIGHTNESS:
-		c->value = dev->vsettings.brightness;
-		break;
-	case V4L2_CID_HFLIP:
-		if (dmi_check_system(stk_upside_down_dmi_table))
-			c->value = !dev->vsettings.hflip;
-		else
-			c->value = dev->vsettings.hflip;
-		break;
-	case V4L2_CID_VFLIP:
-		if (dmi_check_system(stk_upside_down_dmi_table))
-			c->value = !dev->vsettings.vflip;
-		else
-			c->value = dev->vsettings.vflip;
-		break;
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
+	struct stk_camera *dev =
+		container_of(ctrl->handler, struct stk_camera, hdl);
 
-static int stk_vidioc_s_ctrl(struct file *filp,
-		void *priv, struct v4l2_control *c)
-{
-	struct stk_camera *dev = priv;
-	switch (c->id) {
+	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		dev->vsettings.brightness = c->value;
-		return stk_sensor_set_brightness(dev, c->value >> 8);
+		return stk_sensor_set_brightness(dev, ctrl->val);
 	case V4L2_CID_HFLIP:
 		if (dmi_check_system(stk_upside_down_dmi_table))
-			dev->vsettings.hflip = !c->value;
+			dev->vsettings.hflip = !ctrl->val;
 		else
-			dev->vsettings.hflip = c->value;
+			dev->vsettings.hflip = ctrl->val;
 		return 0;
 	case V4L2_CID_VFLIP:
 		if (dmi_check_system(stk_upside_down_dmi_table))
-			dev->vsettings.vflip = !c->value;
+			dev->vsettings.vflip = !ctrl->val;
 		else
-			dev->vsettings.vflip = c->value;
+			dev->vsettings.vflip = ctrl->val;
 		return 0;
 	default:
 		return -EINVAL;
@@ -921,7 +895,7 @@ static int stk_vidioc_g_fmt_vid_cap(struct file *filp,
 		void *priv, struct v4l2_format *f)
 {
 	struct v4l2_pix_format *pix_format = &f->fmt.pix;
-	struct stk_camera *dev = priv;
+	struct stk_camera *dev = video_drvdata(filp);
 	int i;
 
 	for (i = 0; i < ARRAY_SIZE(stk_sizes) &&
@@ -942,11 +916,12 @@ static int stk_vidioc_g_fmt_vid_cap(struct file *filp,
 		pix_format->bytesperline = 2 * pix_format->width;
 	pix_format->sizeimage = pix_format->bytesperline
 				* pix_format->height;
+	pix_format->priv = 0;
 	return 0;
 }
 
-static int stk_vidioc_try_fmt_vid_cap(struct file *filp,
-		void *priv, struct v4l2_format *fmtd)
+static int stk_try_fmt_vid_cap(struct file *filp,
+		struct v4l2_format *fmtd, int *idx)
 {
 	int i;
 	switch (fmtd->fmt.pix.pixelformat) {
@@ -968,11 +943,13 @@ static int stk_vidioc_try_fmt_vid_cap(struct file *filp,
 			< abs(fmtd->fmt.pix.width - stk_sizes[i].w))) {
 		fmtd->fmt.pix.height = stk_sizes[i-1].h;
 		fmtd->fmt.pix.width = stk_sizes[i-1].w;
-		fmtd->fmt.pix.priv = i - 1;
+		if (idx)
+			*idx = i - 1;
 	} else {
 		fmtd->fmt.pix.height = stk_sizes[i].h;
 		fmtd->fmt.pix.width = stk_sizes[i].w;
-		fmtd->fmt.pix.priv = i;
+		if (idx)
+			*idx = i;
 	}
 
 	fmtd->fmt.pix.field = V4L2_FIELD_NONE;
@@ -983,9 +960,16 @@ static int stk_vidioc_try_fmt_vid_cap(struct file *filp,
 		fmtd->fmt.pix.bytesperline = 2 * fmtd->fmt.pix.width;
 	fmtd->fmt.pix.sizeimage = fmtd->fmt.pix.bytesperline
 		* fmtd->fmt.pix.height;
+	fmtd->fmt.pix.priv = 0;
 	return 0;
 }
 
+static int stk_vidioc_try_fmt_vid_cap(struct file *filp,
+		void *priv, struct v4l2_format *fmtd)
+{
+	return stk_try_fmt_vid_cap(filp, fmtd, NULL);
+}
+
 static int stk_setup_format(struct stk_camera *dev)
 {
 	int i = 0;
@@ -1026,7 +1010,8 @@ static int stk_vidioc_s_fmt_vid_cap(struct file *filp,
 		void *priv, struct v4l2_format *fmtd)
 {
 	int ret;
-	struct stk_camera *dev = priv;
+	int idx;
+	struct stk_camera *dev = video_drvdata(filp);
 
 	if (dev == NULL)
 		return -ENODEV;
@@ -1034,17 +1019,16 @@ static int stk_vidioc_s_fmt_vid_cap(struct file *filp,
 		return -ENODEV;
 	if (is_streaming(dev))
 		return -EBUSY;
-	if (dev->owner && dev->owner != filp)
+	if (dev->owner)
 		return -EBUSY;
-	ret = stk_vidioc_try_fmt_vid_cap(filp, priv, fmtd);
+	ret = stk_try_fmt_vid_cap(filp, fmtd, &idx);
 	if (ret)
 		return ret;
-	dev->owner = filp;
 
 	dev->vsettings.palette = fmtd->fmt.pix.pixelformat;
 	stk_free_buffers(dev);
 	dev->frame_size = fmtd->fmt.pix.sizeimage;
-	dev->vsettings.mode = stk_sizes[fmtd->fmt.pix.priv].m;
+	dev->vsettings.mode = stk_sizes[idx].m;
 
 	stk_initialise(dev);
 	return stk_setup_format(dev);
@@ -1053,7 +1037,7 @@ static int stk_vidioc_s_fmt_vid_cap(struct file *filp,
 static int stk_vidioc_reqbufs(struct file *filp,
 		void *priv, struct v4l2_requestbuffers *rb)
 {
-	struct stk_camera *dev = priv;
+	struct stk_camera *dev = video_drvdata(filp);
 
 	if (dev == NULL)
 		return -ENODEV;
@@ -1062,6 +1046,13 @@ static int stk_vidioc_reqbufs(struct file *filp,
 	if (is_streaming(dev)
 		|| (dev->owner && dev->owner != filp))
 		return -EBUSY;
+	stk_free_buffers(dev);
+	if (rb->count == 0) {
+		stk_camera_write_reg(dev, 0x0, 0x49); /* turn off the LED */
+		unset_initialised(dev);
+		dev->owner = NULL;
+		return 0;
+	}
 	dev->owner = filp;
 
 	/*FIXME If they ask for zero, we must stop streaming and free */
@@ -1079,7 +1070,7 @@ static int stk_vidioc_reqbufs(struct file *filp,
 static int stk_vidioc_querybuf(struct file *filp,
 		void *priv, struct v4l2_buffer *buf)
 {
-	struct stk_camera *dev = priv;
+	struct stk_camera *dev = video_drvdata(filp);
 	struct stk_sio_buffer *sbuf;
 
 	if (buf->index >= dev->n_sbufs)
@@ -1092,7 +1083,7 @@ static int stk_vidioc_querybuf(struct file *filp,
 static int stk_vidioc_qbuf(struct file *filp,
 		void *priv, struct v4l2_buffer *buf)
 {
-	struct stk_camera *dev = priv;
+	struct stk_camera *dev = video_drvdata(filp);
 	struct stk_sio_buffer *sbuf;
 	unsigned long flags;
 
@@ -1116,7 +1107,7 @@ static int stk_vidioc_qbuf(struct file *filp,
 static int stk_vidioc_dqbuf(struct file *filp,
 		void *priv, struct v4l2_buffer *buf)
 {
-	struct stk_camera *dev = priv;
+	struct stk_camera *dev = video_drvdata(filp);
 	struct stk_sio_buffer *sbuf;
 	unsigned long flags;
 	int ret;
@@ -1149,7 +1140,7 @@ static int stk_vidioc_dqbuf(struct file *filp,
 static int stk_vidioc_streamon(struct file *filp,
 		void *priv, enum v4l2_buf_type type)
 {
-	struct stk_camera *dev = priv;
+	struct stk_camera *dev = video_drvdata(filp);
 	if (is_streaming(dev))
 		return 0;
 	if (dev->sio_bufs == NULL)
@@ -1161,7 +1152,7 @@ static int stk_vidioc_streamon(struct file *filp,
 static int stk_vidioc_streamoff(struct file *filp,
 		void *priv, enum v4l2_buf_type type)
 {
-	struct stk_camera *dev = priv;
+	struct stk_camera *dev = video_drvdata(filp);
 	unsigned long flags;
 	int i;
 	stk_stop_stream(dev);
@@ -1206,6 +1197,10 @@ static int stk_vidioc_enum_framesizes(struct file *filp,
 	}
 }
 
+static const struct v4l2_ctrl_ops stk_ctrl_ops = {
+	.s_ctrl = stk_s_ctrl,
+};
+
 static struct v4l2_file_operations v4l_stk_fops = {
 	.owner = THIS_MODULE,
 	.open = v4l_stk_open,
@@ -1213,7 +1208,7 @@ static struct v4l2_file_operations v4l_stk_fops = {
 	.read = v4l_stk_read,
 	.poll = v4l_stk_poll,
 	.mmap = v4l_stk_mmap,
-	.ioctl = video_ioctl2,
+	.unlocked_ioctl = video_ioctl2,
 };
 
 static const struct v4l2_ioctl_ops v4l_stk_ioctl_ops = {
@@ -1225,18 +1220,17 @@ static const struct v4l2_ioctl_ops v4l_stk_ioctl_ops = {
 	.vidioc_enum_input = stk_vidioc_enum_input,
 	.vidioc_s_input = stk_vidioc_s_input,
 	.vidioc_g_input = stk_vidioc_g_input,
-	.vidioc_s_std = stk_vidioc_s_std,
 	.vidioc_reqbufs = stk_vidioc_reqbufs,
 	.vidioc_querybuf = stk_vidioc_querybuf,
 	.vidioc_qbuf = stk_vidioc_qbuf,
 	.vidioc_dqbuf = stk_vidioc_dqbuf,
 	.vidioc_streamon = stk_vidioc_streamon,
 	.vidioc_streamoff = stk_vidioc_streamoff,
-	.vidioc_queryctrl = stk_vidioc_queryctrl,
-	.vidioc_g_ctrl = stk_vidioc_g_ctrl,
-	.vidioc_s_ctrl = stk_vidioc_s_ctrl,
 	.vidioc_g_parm = stk_vidioc_g_parm,
 	.vidioc_enum_framesizes = stk_vidioc_enum_framesizes,
+	.vidioc_log_status = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 static void stk_v4l_dev_release(struct video_device *vd)
@@ -1251,8 +1245,6 @@ static void stk_v4l_dev_release(struct video_device *vd)
 
 static struct video_device stk_v4l_data = {
 	.name = "stkwebcam",
-	.tvnorms = V4L2_STD_UNKNOWN,
-	.current_norm = V4L2_STD_UNKNOWN,
 	.fops = &v4l_stk_fops,
 	.ioctl_ops = &v4l_stk_ioctl_ops,
 	.release = stk_v4l_dev_release,
@@ -1264,8 +1256,11 @@ static int stk_register_video_device(struct stk_camera *dev)
 	int err;
 
 	dev->vdev = stk_v4l_data;
+	dev->vdev.lock = &dev->lock;
 	dev->vdev.debug = debug;
-	dev->vdev.parent = &dev->interface->dev;
+	dev->vdev.v4l2_dev = &dev->v4l2_dev;
+	set_bit(V4L2_FL_USE_FH_PRIO, &dev->vdev.flags);
+	video_set_drvdata(&dev->vdev, dev);
 	err = video_register_device(&dev->vdev, VFL_TYPE_GRABBER, -1);
 	if (err)
 		STK_ERROR("v4l registration failed\n");
@@ -1281,8 +1276,9 @@ static int stk_register_video_device(struct stk_camera *dev)
 static int stk_camera_probe(struct usb_interface *interface,
 		const struct usb_device_id *id)
 {
-	int i;
+	struct v4l2_ctrl_handler *hdl;
 	int err = 0;
+	int i;
 
 	struct stk_camera *dev = NULL;
 	struct usb_device *udev = interface_to_usbdev(interface);
@@ -1294,9 +1290,31 @@ static int stk_camera_probe(struct usb_interface *interface,
 		STK_ERROR("Out of memory !\n");
 		return -ENOMEM;
 	}
+	err = v4l2_device_register(&interface->dev, &dev->v4l2_dev);
+	if (err < 0) {
+		dev_err(&udev->dev, "couldn't register v4l2_device\n");
+		kfree(dev);
+		return err;
+	}
+	hdl = &dev->hdl;
+	v4l2_ctrl_handler_init(hdl, 3);
+	v4l2_ctrl_new_std(hdl, &stk_ctrl_ops,
+			  V4L2_CID_BRIGHTNESS, 0, 0xff, 0x1, 0x60);
+	v4l2_ctrl_new_std(hdl, &stk_ctrl_ops,
+			  V4L2_CID_HFLIP, 0, 1, 1, 1);
+	v4l2_ctrl_new_std(hdl, &stk_ctrl_ops,
+			  V4L2_CID_VFLIP, 0, 1, 1, 1);
+	if (hdl->error) {
+		err = hdl->error;
+		dev_err(&udev->dev, "couldn't register control\n");
+		goto error;
+	}
+	dev->v4l2_dev.ctrl_handler = hdl;
 
 	spin_lock_init(&dev->spinlock);
+	mutex_init(&dev->lock);
 	init_waitqueue_head(&dev->wait_frame);
+	dev->first_init = 1; /* webcam LED management */
 
 	dev->udev = udev;
 	dev->interface = interface;
@@ -1337,7 +1355,6 @@ static int stk_camera_probe(struct usb_interface *interface,
 		err = -ENODEV;
 		goto error;
 	}
-	dev->vsettings.brightness = 0x7fff;
 	dev->vsettings.palette = V4L2_PIX_FMT_RGB565;
 	dev->vsettings.mode = MODE_VGA;
 	dev->frame_size = 640 * 480 * 2;
@@ -1354,6 +1371,8 @@ static int stk_camera_probe(struct usb_interface *interface,
 	return 0;
 
 error:
+	v4l2_ctrl_handler_free(hdl);
+	v4l2_device_unregister(&dev->v4l2_dev);
 	kfree(dev);
 	return err;
 }
@@ -1371,6 +1390,8 @@ static void stk_camera_disconnect(struct usb_interface *interface)
 		 video_device_node_name(&dev->vdev));
 
 	video_unregister_device(&dev->vdev);
+	v4l2_ctrl_handler_free(&dev->hdl);
+	v4l2_device_unregister(&dev->v4l2_dev);
 }
 
 #ifdef CONFIG_PM
diff --git a/drivers/media/usb/stkwebcam/stk-webcam.h b/drivers/media/usb/stkwebcam/stk-webcam.h
index 9f673663757..9bbfa3d9bfd 100644
--- a/drivers/media/usb/stkwebcam/stk-webcam.h
+++ b/drivers/media/usb/stkwebcam/stk-webcam.h
@@ -23,6 +23,8 @@
 #define STKWEBCAM_H
 
 #include <linux/usb.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
 #include <media/v4l2-common.h>
 
 #define DRIVER_VERSION		"v0.0.1"
@@ -59,7 +61,6 @@ enum stk_mode {MODE_VGA, MODE_SXGA, MODE_CIF, MODE_QVGA, MODE_QCIF};
 
 struct stk_video {
 	enum stk_mode mode;
-	int brightness;
 	__u32 palette;
 	int hflip;
 	int vflip;
@@ -91,11 +92,15 @@ struct regval {
 };
 
 struct stk_camera {
+	struct v4l2_device v4l2_dev;
+	struct v4l2_ctrl_handler hdl;
 	struct video_device vdev;
 	struct usb_device *udev;
 	struct usb_interface *interface;
 	int webcam_model;
 	struct file *owner;
+	struct mutex lock;
+	int first_init;
 
 	u8 isoc_ep;
 
@@ -113,6 +118,7 @@ struct stk_camera {
 
 	int frame_size;
 	/* Streaming buffers */
+	int reading;
 	unsigned int n_sbufs;
 	struct stk_sio_buffer *sio_bufs;
 	struct list_head sio_avail;
diff --git a/drivers/media/usb/tlg2300/pd-common.h b/drivers/media/usb/tlg2300/pd-common.h
index 5dd73b7857d..9e23ad32d2f 100644
--- a/drivers/media/usb/tlg2300/pd-common.h
+++ b/drivers/media/usb/tlg2300/pd-common.h
@@ -10,6 +10,7 @@
 #include <linux/poll.h>
 #include <media/videobuf-vmalloc.h>
 #include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
 
 #include "dvb_frontend.h"
 #include "dvbdev.h"
@@ -25,7 +26,6 @@
 #define POSEIDON_STATE_ANALOG		(0x0001)
 #define POSEIDON_STATE_FM		(0x0002)
 #define POSEIDON_STATE_DVBT		(0x0004)
-#define POSEIDON_STATE_VBI		(0x0008)
 #define POSEIDON_STATE_DISCONNECT	(0x0080)
 
 #define PM_SUSPEND_DELAY	3
@@ -35,11 +35,11 @@
 #define V4L_PAL_VBI_FRAMESIZE	(V4L_PAL_VBI_LINES * 1440 * 2)
 #define V4L_NTSC_VBI_FRAMESIZE	(V4L_NTSC_VBI_LINES * 1440 * 2)
 
-#define TUNER_FREQ_MIN		(45000000)
-#define TUNER_FREQ_MAX		(862000000)
+#define TUNER_FREQ_MIN		(45000000U)
+#define TUNER_FREQ_MAX		(862000000U)
 
 struct vbi_data {
-	struct video_device	*v_dev;
+	struct video_device	v_dev;
 	struct video_data	*video;
 	struct front_face	*front;
 
@@ -62,7 +62,8 @@ struct running_context {
 
 struct video_data {
 	/* v4l2 video device */
-	struct video_device	*v_dev;
+	struct video_device	v_dev;
+	struct v4l2_ctrl_handler ctrl_handler;
 
 	/* the working context */
 	struct running_context	context;
@@ -115,10 +116,10 @@ struct poseidon_audio {
 
 struct radio_data {
 	__u32		fm_freq;
-	int		users;
 	unsigned int	is_radio_streaming;
 	int		pre_emphasis;
-	struct video_device *fm_dev;
+	struct video_device fm_dev;
+	struct v4l2_ctrl_handler ctrl_handler;
 };
 
 #define DVB_SBUF_NUM		4
@@ -233,7 +234,6 @@ void dvb_stop_streaming(struct pd_dvb_adapter *);
 /* FM */
 int poseidon_fm_init(struct poseidon *);
 int poseidon_fm_exit(struct poseidon *);
-struct video_device *vdev_init(struct poseidon *, struct video_device *);
 
 /* vendor command ops */
 int send_set_req(struct poseidon*, u8, s32, s32*);
@@ -249,7 +249,6 @@ void free_all_urb_generic(struct urb **urb_array, int num);
 
 /* misc */
 void poseidon_delete(struct kref *kref);
-void destroy_video_device(struct video_device **v_dev);
 extern int debug_mode;
 void set_debug_mode(struct video_device *vfd, int debug_mode);
 
@@ -269,13 +268,4 @@ void set_debug_mode(struct video_device *vfd, int debug_mode);
 				log();\
 		} while (0)
 
-#define logs(f) do { \
-			if ((debug_mode & 0x4) && \
-				(f)->type == V4L2_BUF_TYPE_VBI_CAPTURE) \
-					log("type : VBI");\
-								\
-			if ((debug_mode & 0x8) && \
-				(f)->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) \
-					log("type : VIDEO");\
-		} while (0)
 #endif
diff --git a/drivers/media/usb/tlg2300/pd-main.c b/drivers/media/usb/tlg2300/pd-main.c
index 7b1f6ebd0e2..e07e4c699cc 100644
--- a/drivers/media/usb/tlg2300/pd-main.c
+++ b/drivers/media/usb/tlg2300/pd-main.c
@@ -55,7 +55,6 @@ MODULE_PARM_DESC(debug_mode, "0 = disable, 1 = enable, 2 = verbose");
 
 #define TLG2300_FIRMWARE "tlg2300_firmware.bin"
 static const char *firmware_name = TLG2300_FIRMWARE;
-static struct usb_driver poseidon_driver;
 static LIST_HEAD(pd_device_list);
 
 /*
@@ -268,7 +267,8 @@ static inline void set_map_flags(struct poseidon *pd, struct usb_device *udev)
 static inline int get_autopm_ref(struct poseidon *pd)
 {
 	return  pd->video_data.users + pd->vbi_data.users + pd->audio.users
-		+ atomic_read(&pd->dvb_data.users) + pd->radio_data.users;
+		+ atomic_read(&pd->dvb_data.users) +
+		!list_empty(&pd->radio_data.fm_dev.fh_list);
 }
 
 /* fixup something for poseidon */
@@ -316,7 +316,7 @@ static int poseidon_suspend(struct usb_interface *intf, pm_message_t msg)
 		if (get_pm_count(pd) <= 0 && !in_hibernation(pd)) {
 			pd->msg.event = PM_EVENT_AUTO_SUSPEND;
 			pd->pm_resume = NULL; /*  a good guard */
-			printk(KERN_DEBUG "\n\t+ TLG2300 auto suspend +\n\n");
+			printk(KERN_DEBUG "TLG2300 auto suspend\n");
 		}
 		return 0;
 	}
@@ -331,7 +331,7 @@ static int poseidon_resume(struct usb_interface *intf)
 
 	if (!pd)
 		return 0;
-	printk(KERN_DEBUG "\n\t ++ TLG2300 resume ++\n\n");
+	printk(KERN_DEBUG "TLG2300 resume\n");
 
 	if (!is_working(pd)) {
 		if (PM_EVENT_AUTO_SUSPEND == pd->msg.event)
@@ -431,15 +431,11 @@ static int poseidon_probe(struct usb_interface *interface,
 	usb_set_intfdata(interface, pd);
 
 	if (new_one) {
-		struct device *dev = &interface->dev;
-
 		logpm(pd);
 		mutex_init(&pd->lock);
 
 		/* register v4l2 device */
-		snprintf(pd->v4l2_dev.name, sizeof(pd->v4l2_dev.name), "%s %s",
-			dev->driver->name, dev_name(dev));
-		ret = v4l2_device_register(NULL, &pd->v4l2_dev);
+		ret = v4l2_device_register(&interface->dev, &pd->v4l2_dev);
 
 		/* register devices in directory /dev */
 		ret = pd_video_init(pd);
@@ -530,7 +526,7 @@ module_init(poseidon_init);
 module_exit(poseidon_exit);
 
 MODULE_AUTHOR("Telegent Systems");
-MODULE_DESCRIPTION("For tlg2300-based USB device ");
+MODULE_DESCRIPTION("For tlg2300-based USB device");
 MODULE_LICENSE("GPL");
 MODULE_VERSION("0.0.2");
 MODULE_FIRMWARE(TLG2300_FIRMWARE);
diff --git a/drivers/media/usb/tlg2300/pd-radio.c b/drivers/media/usb/tlg2300/pd-radio.c
index 25eeb166aa0..ea6070ba835 100644
--- a/drivers/media/usb/tlg2300/pd-radio.c
+++ b/drivers/media/usb/tlg2300/pd-radio.c
@@ -9,6 +9,8 @@
 #include <linux/mm.h>
 #include <linux/mutex.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fh.h>
 #include <linux/sched.h>
 
 #include "pd-common.h"
@@ -18,8 +20,8 @@ static int set_frequency(struct poseidon *p, __u32 frequency);
 static int poseidon_fm_close(struct file *filp);
 static int poseidon_fm_open(struct file *filp);
 
-#define TUNER_FREQ_MIN_FM 76000000
-#define TUNER_FREQ_MAX_FM 108000000
+#define TUNER_FREQ_MIN_FM 76000000U
+#define TUNER_FREQ_MAX_FM 108000000U
 
 #define MAX_PREEMPHASIS (V4L2_PREEMPHASIS_75_uS + 1)
 static int preemphasis[MAX_PREEMPHASIS] = {
@@ -77,13 +79,9 @@ static int pm_fm_resume(struct poseidon *p)
 
 static int poseidon_fm_open(struct file *filp)
 {
-	struct video_device *vfd = video_devdata(filp);
-	struct poseidon *p = video_get_drvdata(vfd);
+	struct poseidon *p = video_drvdata(filp);
 	int ret = 0;
 
-	if (!p)
-		return -1;
-
 	mutex_lock(&p->lock);
 	if (p->state & POSEIDON_STATE_DISCONNECT) {
 		ret = -ENODEV;
@@ -94,9 +92,14 @@ static int poseidon_fm_open(struct file *filp)
 		ret = -EBUSY;
 		goto out;
 	}
+	ret = v4l2_fh_open(filp);
+	if (ret)
+		goto out;
 
 	usb_autopm_get_interface(p->interface);
 	if (0 == p->state) {
+		struct video_device *vfd = &p->radio_data.fm_dev;
+
 		/* default pre-emphasis */
 		if (p->radio_data.pre_emphasis == 0)
 			p->radio_data.pre_emphasis = TLG_TUNE_ASTD_FM_EUR;
@@ -109,9 +112,7 @@ static int poseidon_fm_open(struct file *filp)
 		}
 		p->state |= POSEIDON_STATE_FM;
 	}
-	p->radio_data.users++;
 	kref_get(&p->kref);
-	filp->private_data = p;
 out:
 	mutex_unlock(&p->lock);
 	return ret;
@@ -119,13 +120,12 @@ out:
 
 static int poseidon_fm_close(struct file *filp)
 {
-	struct poseidon *p = filp->private_data;
+	struct poseidon *p = video_drvdata(filp);
 	struct radio_data *fm = &p->radio_data;
 	uint32_t status;
 
 	mutex_lock(&p->lock);
-	fm->users--;
-	if (0 == fm->users)
+	if (v4l2_fh_is_singular_file(filp))
 		p->state &= ~POSEIDON_STATE_FM;
 
 	if (fm->is_radio_streaming && filp == p->file_for_stream) {
@@ -136,19 +136,23 @@ static int poseidon_fm_close(struct file *filp)
 	mutex_unlock(&p->lock);
 
 	kref_put(&p->kref, poseidon_delete);
-	filp->private_data = NULL;
-	return 0;
+	return v4l2_fh_release(filp);
 }
 
 static int vidioc_querycap(struct file *file, void *priv,
 			struct v4l2_capability *v)
 {
-	struct poseidon *p = file->private_data;
+	struct poseidon *p = video_drvdata(file);
 
 	strlcpy(v->driver, "tele-radio", sizeof(v->driver));
 	strlcpy(v->card, "Telegent Poseidon", sizeof(v->card));
 	usb_make_path(p->udev, v->bus_info, sizeof(v->bus_info));
-	v->capabilities = V4L2_CAP_TUNER | V4L2_CAP_RADIO;
+	v->device_caps = V4L2_CAP_TUNER | V4L2_CAP_RADIO;
+	/* Report all capabilities of the USB device */
+	v->capabilities = v->device_caps | V4L2_CAP_DEVICE_CAPS |
+			V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VBI_CAPTURE |
+			V4L2_CAP_AUDIO | V4L2_CAP_STREAMING |
+			V4L2_CAP_READWRITE;
 	return 0;
 }
 
@@ -156,27 +160,29 @@ static const struct v4l2_file_operations poseidon_fm_fops = {
 	.owner         = THIS_MODULE,
 	.open          = poseidon_fm_open,
 	.release       = poseidon_fm_close,
-	.ioctl	       = video_ioctl2,
+	.poll		= v4l2_ctrl_poll,
+	.unlocked_ioctl = video_ioctl2,
 };
 
 static int tlg_fm_vidioc_g_tuner(struct file *file, void *priv,
 				 struct v4l2_tuner *vt)
 {
+	struct poseidon *p = video_drvdata(file);
 	struct tuner_fm_sig_stat_s fm_stat = {};
 	int ret, status, count = 5;
-	struct poseidon *p = file->private_data;
 
 	if (vt->index != 0)
 		return -EINVAL;
 
 	vt->type	= V4L2_TUNER_RADIO;
-	vt->capability	= V4L2_TUNER_CAP_STEREO;
-	vt->rangelow	= TUNER_FREQ_MIN_FM / 62500;
-	vt->rangehigh	= TUNER_FREQ_MAX_FM / 62500;
+	vt->capability	= V4L2_TUNER_CAP_STEREO | V4L2_TUNER_CAP_LOW;
+	vt->rangelow	= TUNER_FREQ_MIN_FM * 2 / 125;
+	vt->rangehigh	= TUNER_FREQ_MAX_FM * 2 / 125;
 	vt->rxsubchans	= V4L2_TUNER_SUB_STEREO;
 	vt->audmode	= V4L2_TUNER_MODE_STEREO;
 	vt->signal	= 0;
 	vt->afc 	= 0;
+	strlcpy(vt->name, "Radio", sizeof(vt->name));
 
 	mutex_lock(&p->lock);
 	ret = send_get_req(p, TUNER_STATUS, TLG_MODE_FM_RADIO,
@@ -205,8 +211,10 @@ static int tlg_fm_vidioc_g_tuner(struct file *file, void *priv,
 static int fm_get_freq(struct file *file, void *priv,
 		       struct v4l2_frequency *argp)
 {
-	struct poseidon *p = file->private_data;
+	struct poseidon *p = video_drvdata(file);
 
+	if (argp->tuner)
+		return -EINVAL;
 	argp->frequency = p->radio_data.fm_freq;
 	return 0;
 }
@@ -221,11 +229,8 @@ static int set_frequency(struct poseidon *p, __u32 frequency)
 	ret = send_set_req(p, TUNER_AUD_ANA_STD,
 				p->radio_data.pre_emphasis, &status);
 
-	freq =  (frequency * 125) * 500 / 1000;/* kHZ */
-	if (freq < TUNER_FREQ_MIN_FM/1000 || freq > TUNER_FREQ_MAX_FM/1000) {
-		ret = -EINVAL;
-		goto error;
-	}
+	freq = (frequency * 125) / 2; /* Hz */
+	freq = clamp(freq, TUNER_FREQ_MIN_FM, TUNER_FREQ_MAX_FM);
 
 	ret = send_set_req(p, TUNE_FREQ_SELECT, freq, &status);
 	if (ret < 0)
@@ -240,18 +245,20 @@ static int set_frequency(struct poseidon *p, __u32 frequency)
 				TLG_TUNE_PLAY_SVC_START, &status);
 		p->radio_data.is_radio_streaming = 1;
 	}
-	p->radio_data.fm_freq = frequency;
+	p->radio_data.fm_freq = freq * 2 / 125;
 error:
 	mutex_unlock(&p->lock);
 	return ret;
 }
 
 static int fm_set_freq(struct file *file, void *priv,
-		       struct v4l2_frequency *argp)
+		       const struct v4l2_frequency *argp)
 {
-	struct poseidon *p = file->private_data;
+	struct poseidon *p = video_drvdata(file);
 
-	p->file_for_stream  = file;
+	if (argp->tuner)
+		return -EINVAL;
+	p->file_for_stream = file;
 #ifdef CONFIG_PM
 	p->pm_suspend = pm_fm_suspend;
 	p->pm_resume  = pm_fm_resume;
@@ -259,163 +266,75 @@ static int fm_set_freq(struct file *file, void *priv,
 	return set_frequency(p, argp->frequency);
 }
 
-static int tlg_fm_vidioc_g_ctrl(struct file *file, void *priv,
-		struct v4l2_control *arg)
-{
-	return 0;
-}
-
-static int tlg_fm_vidioc_g_exts_ctrl(struct file *file, void *fh,
-				struct v4l2_ext_controls *ctrls)
-{
-	struct poseidon *p = file->private_data;
-	int i;
-
-	if (ctrls->ctrl_class != V4L2_CTRL_CLASS_FM_TX)
-		return -EINVAL;
-
-	for (i = 0; i < ctrls->count; i++) {
-		struct v4l2_ext_control *ctrl = ctrls->controls + i;
-
-		if (ctrl->id != V4L2_CID_TUNE_PREEMPHASIS)
-			continue;
-
-		if (i < MAX_PREEMPHASIS)
-			ctrl->value = p->radio_data.pre_emphasis;
-	}
-	return 0;
-}
-
-static int tlg_fm_vidioc_s_exts_ctrl(struct file *file, void *fh,
-			struct v4l2_ext_controls *ctrls)
-{
-	int i;
-
-	if (ctrls->ctrl_class != V4L2_CTRL_CLASS_FM_TX)
-		return -EINVAL;
-
-	for (i = 0; i < ctrls->count; i++) {
-		struct v4l2_ext_control *ctrl = ctrls->controls + i;
-
-		if (ctrl->id != V4L2_CID_TUNE_PREEMPHASIS)
-			continue;
-
-		if (ctrl->value >= 0 && ctrl->value < MAX_PREEMPHASIS) {
-			struct poseidon *p = file->private_data;
-			int pre_emphasis = preemphasis[ctrl->value];
-			u32 status;
-
-			send_set_req(p, TUNER_AUD_ANA_STD,
-						pre_emphasis, &status);
-			p->radio_data.pre_emphasis = pre_emphasis;
-		}
-	}
-	return 0;
-}
-
-static int tlg_fm_vidioc_s_ctrl(struct file *file, void *priv,
-		struct v4l2_control *ctrl)
+static int tlg_fm_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	return 0;
-}
-
-static int tlg_fm_vidioc_queryctrl(struct file *file, void *priv,
-		struct v4l2_queryctrl *ctrl)
-{
-	if (!(ctrl->id & V4L2_CTRL_FLAG_NEXT_CTRL))
-		return -EINVAL;
+	struct poseidon *p = container_of(ctrl->handler, struct poseidon,
+						radio_data.ctrl_handler);
+	int pre_emphasis;
+	u32 status;
 
-	ctrl->id &= ~V4L2_CTRL_FLAG_NEXT_CTRL;
-	if (ctrl->id != V4L2_CID_TUNE_PREEMPHASIS) {
-		/* return the next supported control */
-		ctrl->id = V4L2_CID_TUNE_PREEMPHASIS;
-		v4l2_ctrl_query_fill(ctrl, V4L2_PREEMPHASIS_DISABLED,
-					V4L2_PREEMPHASIS_75_uS, 1,
-					V4L2_PREEMPHASIS_50_uS);
-		ctrl->flags = V4L2_CTRL_FLAG_UPDATE;
+	switch (ctrl->id) {
+	case V4L2_CID_TUNE_PREEMPHASIS:
+		pre_emphasis = preemphasis[ctrl->val];
+		send_set_req(p, TUNER_AUD_ANA_STD, pre_emphasis, &status);
+		p->radio_data.pre_emphasis = pre_emphasis;
 		return 0;
 	}
 	return -EINVAL;
 }
 
-static int tlg_fm_vidioc_querymenu(struct file *file, void *fh,
-				struct v4l2_querymenu *qmenu)
-{
-	return v4l2_ctrl_query_menu(qmenu, NULL, NULL);
-}
-
-static int vidioc_s_tuner(struct file *file, void *priv, struct v4l2_tuner *vt)
+static int vidioc_s_tuner(struct file *file, void *priv, const struct v4l2_tuner *vt)
 {
 	return vt->index > 0 ? -EINVAL : 0;
 }
-static int vidioc_s_audio(struct file *file, void *priv, const struct v4l2_audio *va)
-{
-	return (va->index != 0) ? -EINVAL : 0;
-}
-
-static int vidioc_g_audio(struct file *file, void *priv, struct v4l2_audio *a)
-{
-	a->index    = 0;
-	a->mode    = 0;
-	a->capability = V4L2_AUDCAP_STEREO;
-	strcpy(a->name, "Radio");
-	return 0;
-}
-
-static int vidioc_s_input(struct file *filp, void *priv, u32 i)
-{
-	return (i != 0) ? -EINVAL : 0;
-}
 
-static int vidioc_g_input(struct file *filp, void *priv, u32 *i)
-{
-	return (*i != 0) ? -EINVAL : 0;
-}
+static const struct v4l2_ctrl_ops tlg_fm_ctrl_ops = {
+	.s_ctrl = tlg_fm_s_ctrl,
+};
 
 static const struct v4l2_ioctl_ops poseidon_fm_ioctl_ops = {
 	.vidioc_querycap    = vidioc_querycap,
-	.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_queryctrl   = tlg_fm_vidioc_queryctrl,
-	.vidioc_querymenu   = tlg_fm_vidioc_querymenu,
-	.vidioc_g_ctrl      = tlg_fm_vidioc_g_ctrl,
-	.vidioc_s_ctrl      = tlg_fm_vidioc_s_ctrl,
-	.vidioc_s_ext_ctrls = tlg_fm_vidioc_s_exts_ctrl,
-	.vidioc_g_ext_ctrls = tlg_fm_vidioc_g_exts_ctrl,
 	.vidioc_s_tuner     = vidioc_s_tuner,
 	.vidioc_g_tuner     = tlg_fm_vidioc_g_tuner,
 	.vidioc_g_frequency = fm_get_freq,
 	.vidioc_s_frequency = fm_set_freq,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 static struct video_device poseidon_fm_template = {
 	.name       = "Telegent-Radio",
 	.fops       = &poseidon_fm_fops,
 	.minor      = -1,
-	.release    = video_device_release,
+	.release    = video_device_release_empty,
 	.ioctl_ops  = &poseidon_fm_ioctl_ops,
 };
 
 int poseidon_fm_init(struct poseidon *p)
 {
-	struct video_device *fm_dev;
-
-	fm_dev = vdev_init(p, &poseidon_fm_template);
-	if (fm_dev == NULL)
-		return -1;
-
-	if (video_register_device(fm_dev, VFL_TYPE_RADIO, -1) < 0) {
-		video_device_release(fm_dev);
-		return -1;
+	struct video_device *vfd = &p->radio_data.fm_dev;
+	struct v4l2_ctrl_handler *hdl = &p->radio_data.ctrl_handler;
+
+	*vfd = poseidon_fm_template;
+
+	set_frequency(p, TUNER_FREQ_MIN_FM);
+	v4l2_ctrl_handler_init(hdl, 1);
+	v4l2_ctrl_new_std_menu(hdl, &tlg_fm_ctrl_ops, V4L2_CID_TUNE_PREEMPHASIS,
+			V4L2_PREEMPHASIS_75_uS, 0, V4L2_PREEMPHASIS_50_uS);
+	if (hdl->error) {
+		v4l2_ctrl_handler_free(hdl);
+		return hdl->error;
 	}
-	p->radio_data.fm_dev = fm_dev;
-	return 0;
+	vfd->v4l2_dev = &p->v4l2_dev;
+	vfd->ctrl_handler = hdl;
+	set_bit(V4L2_FL_USE_FH_PRIO, &vfd->flags);
+	video_set_drvdata(vfd, p);
+	return video_register_device(vfd, VFL_TYPE_RADIO, -1);
 }
 
 int poseidon_fm_exit(struct poseidon *p)
 {
-	destroy_video_device(&p->radio_data.fm_dev);
+	video_unregister_device(&p->radio_data.fm_dev);
+	v4l2_ctrl_handler_free(&p->radio_data.ctrl_handler);
 	return 0;
 }
diff --git a/drivers/media/usb/tlg2300/pd-video.c b/drivers/media/usb/tlg2300/pd-video.c
index 21723378bb8..8df668d0655 100644
--- a/drivers/media/usb/tlg2300/pd-video.c
+++ b/drivers/media/usb/tlg2300/pd-video.c
@@ -8,6 +8,7 @@
 
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-dev.h>
+#include <media/v4l2-ctrls.h>
 
 #include "pd-common.h"
 #include "vendorcmds.h"
@@ -82,31 +83,6 @@ static const struct pd_input pd_inputs[] = {
 };
 static const unsigned int POSEIDON_INPUTS = ARRAY_SIZE(pd_inputs);
 
-struct poseidon_control {
-	struct v4l2_queryctrl v4l2_ctrl;
-	enum cmd_custom_param_id vc_id;
-};
-
-static struct poseidon_control controls[] = {
-	{
-		{ V4L2_CID_BRIGHTNESS, V4L2_CTRL_TYPE_INTEGER,
-			"brightness", 0, 10000, 1, 100, 0, },
-		CUST_PARM_ID_BRIGHTNESS_CTRL
-	}, {
-		{ V4L2_CID_CONTRAST, V4L2_CTRL_TYPE_INTEGER,
-			"contrast", 0, 10000, 1, 100, 0, },
-		CUST_PARM_ID_CONTRAST_CTRL,
-	}, {
-		{ V4L2_CID_HUE, V4L2_CTRL_TYPE_INTEGER,
-			"hue", 0, 10000, 1, 100, 0, },
-		CUST_PARM_ID_HUE_CTRL,
-	}, {
-		{ V4L2_CID_SATURATION, V4L2_CTRL_TYPE_INTEGER,
-			"saturation", 0, 10000, 1, 100, 0, },
-		CUST_PARM_ID_SATURATION_CTRL,
-	},
-};
-
 struct video_std_to_audio_std {
 	v4l2_std_id	video_std;
 	int 		audio_std;
@@ -142,17 +118,20 @@ static int get_audio_std(v4l2_std_id v4l2_std)
 static int vidioc_querycap(struct file *file, void *fh,
 			struct v4l2_capability *cap)
 {
-	struct front_face *front = fh;
-	struct poseidon *p = front->pd;
-
-	logs(front);
+	struct video_device *vdev = video_devdata(file);
+	struct poseidon *p = video_get_drvdata(vdev);
 
 	strcpy(cap->driver, "tele-video");
 	strcpy(cap->card, "Telegent Poseidon");
 	usb_make_path(p->udev, cap->bus_info, sizeof(cap->bus_info));
-	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_TUNER |
-				V4L2_CAP_AUDIO | V4L2_CAP_STREAMING |
-				V4L2_CAP_READWRITE | V4L2_CAP_VBI_CAPTURE;
+	cap->device_caps = V4L2_CAP_TUNER | V4L2_CAP_AUDIO |
+			V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
+	if (vdev->vfl_type == VFL_TYPE_VBI)
+		cap->device_caps |= V4L2_CAP_VBI_CAPTURE;
+	else
+		cap->device_caps |= V4L2_CAP_VIDEO_CAPTURE;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS |
+		V4L2_CAP_RADIO | V4L2_CAP_VBI_CAPTURE | V4L2_CAP_VIDEO_CAPTURE;
 	return 0;
 }
 
@@ -223,7 +202,6 @@ static void submit_frame(struct front_face *front)
  */
 static void end_field(struct video_data *video)
 {
-	/* logs(video->front); */
 	if (1 == video->field_count)
 		submit_frame(video->front);
 	else
@@ -718,17 +696,10 @@ static int vidioc_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
 	struct front_face *front = fh;
 	struct poseidon *pd = front->pd;
 
-	logs(front);
 	f->fmt.pix = pd->video_data.context.pix;
 	return 0;
 }
 
-static int vidioc_try_fmt(struct file *file, void *fh,
-		struct v4l2_format *f)
-{
-	return 0;
-}
-
 /*
  * VLC calls VIDIOC_S_STD before VIDIOC_S_FMT, while
  * Mplayer calls them in the reverse order.
@@ -787,7 +758,6 @@ static int vidioc_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
 	struct front_face *front	= fh;
 	struct poseidon *pd		= front->pd;
 
-	logs(front);
 	/* stop VBI here */
 	if (V4L2_BUF_TYPE_VIDEO_CAPTURE != f->type)
 		return -EINVAL;
@@ -828,11 +798,10 @@ static int vidioc_g_fmt_vbi(struct file *file, void *fh,
 		vbi_fmt->count[1] = V4L_PAL_VBI_LINES;
 	}
 	vbi_fmt->flags = V4L2_VBI_UNSYNC;
-	logs(front);
 	return 0;
 }
 
-static int set_std(struct poseidon *pd, v4l2_std_id *norm)
+static int set_std(struct poseidon *pd, v4l2_std_id norm)
 {
 	struct video_data *video = &pd->video_data;
 	struct vbi_data *vbi	= &pd->vbi_data;
@@ -842,7 +811,7 @@ static int set_std(struct poseidon *pd, v4l2_std_id *norm)
 	int height;
 
 	for (i = 0; i < POSEIDON_TVNORMS; i++) {
-		if (*norm & poseidon_tvnorms[i].v4l2_id) {
+		if (norm & poseidon_tvnorms[i].v4l2_id) {
 			param = poseidon_tvnorms[i].tlg_tvnorm;
 			log("name : %s", poseidon_tvnorms[i].name);
 			goto found;
@@ -877,17 +846,23 @@ out:
 	return ret;
 }
 
-static int vidioc_s_std(struct file *file, void *fh, v4l2_std_id *norm)
+static int vidioc_s_std(struct file *file, void *fh, v4l2_std_id norm)
 {
 	struct front_face *front = fh;
-	logs(front);
+
 	return set_std(front->pd, norm);
 }
 
-static int vidioc_enum_input(struct file *file, void *fh, struct v4l2_input *in)
+static int vidioc_g_std(struct file *file, void *fh, v4l2_std_id *norm)
 {
 	struct front_face *front = fh;
 
+	*norm = front->pd->video_data.context.tvnormid;
+	return 0;
+}
+
+static int vidioc_enum_input(struct file *file, void *fh, struct v4l2_input *in)
+{
 	if (in->index >= POSEIDON_INPUTS)
 		return -EINVAL;
 	strcpy(in->name, pd_inputs[in->index].name);
@@ -897,11 +872,10 @@ static int vidioc_enum_input(struct file *file, void *fh, struct v4l2_input *in)
 	 * the audio input index mixed with this video input,
 	 * Poseidon only have one audio/video, set to "0"
 	 */
-	in->audioset	= 0;
+	in->audioset	= 1;
 	in->tuner	= 0;
 	in->std		= V4L2_STD_ALL;
 	in->status	= 0;
-	logs(front);
 	return 0;
 }
 
@@ -911,7 +885,6 @@ static int vidioc_g_input(struct file *file, void *fh, unsigned int *i)
 	struct poseidon *pd = front->pd;
 	struct running_context *context = &pd->video_data.context;
 
-	logs(front);
 	*i = context->sig_index;
 	return 0;
 }
@@ -934,68 +907,28 @@ static int vidioc_s_input(struct file *file, void *fh, unsigned int i)
 	return 0;
 }
 
-static struct poseidon_control *check_control_id(__u32 id)
-{
-	struct poseidon_control *control = &controls[0];
-	int array_size = ARRAY_SIZE(controls);
-
-	for (; control < &controls[array_size]; control++)
-		if (control->v4l2_ctrl.id  == id)
-			return control;
-	return NULL;
-}
-
-static int vidioc_queryctrl(struct file *file, void *fh,
-			struct v4l2_queryctrl *a)
-{
-	struct poseidon_control *control = NULL;
-
-	control = check_control_id(a->id);
-	if (!control)
-		return -EINVAL;
-
-	*a = control->v4l2_ctrl;
-	return 0;
-}
-
-static int vidioc_g_ctrl(struct file *file, void *fh, struct v4l2_control *ctrl)
-{
-	struct front_face *front = fh;
-	struct poseidon *pd = front->pd;
-	struct poseidon_control *control = NULL;
-	struct tuner_custom_parameter_s tuner_param;
-	s32 ret = 0, cmd_status;
-
-	control = check_control_id(ctrl->id);
-	if (!control)
-		return -EINVAL;
-
-	mutex_lock(&pd->lock);
-	ret = send_get_req(pd, TUNER_CUSTOM_PARAMETER, control->vc_id,
-			&tuner_param, &cmd_status, sizeof(tuner_param));
-	mutex_unlock(&pd->lock);
-
-	if (ret || cmd_status)
-		return -1;
-
-	ctrl->value = tuner_param.param_value;
-	return 0;
-}
-
-static int vidioc_s_ctrl(struct file *file, void *fh, struct v4l2_control *a)
+static int tlg_s_ctrl(struct v4l2_ctrl *c)
 {
+	struct poseidon *pd = container_of(c->handler, struct poseidon,
+						video_data.ctrl_handler);
 	struct tuner_custom_parameter_s param = {0};
-	struct poseidon_control *control = NULL;
-	struct front_face *front	= fh;
-	struct poseidon *pd		= front->pd;
 	s32 ret = 0, cmd_status, params;
 
-	control = check_control_id(a->id);
-	if (!control)
-		return -EINVAL;
-
-	param.param_value = a->value;
-	param.param_id	= control->vc_id;
+	switch (c->id) {
+	case V4L2_CID_BRIGHTNESS:
+		param.param_id = CUST_PARM_ID_BRIGHTNESS_CTRL;
+		break;
+	case V4L2_CID_CONTRAST:
+		param.param_id = CUST_PARM_ID_CONTRAST_CTRL;
+		break;
+	case V4L2_CID_HUE:
+		param.param_id = CUST_PARM_ID_HUE_CTRL;
+		break;
+	case V4L2_CID_SATURATION:
+		param.param_id = CUST_PARM_ID_SATURATION_CTRL;
+		break;
+	}
+	param.param_value = c->val;
 	params = *(s32 *)&param; /* temp code */
 
 	mutex_lock(&pd->lock);
@@ -1079,7 +1012,6 @@ static int vidioc_g_tuner(struct file *file, void *fh, struct v4l2_tuner *tuner)
 	tuner->rxsubchans = pd_audio_modes[index].v4l2_audio_sub;
 	tuner->audmode = pd_audio_modes[index].v4l2_audio_mode;
 	tuner->afc = 0;
-	logs(front);
 	return 0;
 }
 
@@ -1099,7 +1031,7 @@ static int pd_vidioc_s_tuner(struct poseidon *pd, int index)
 	return ret;
 }
 
-static int vidioc_s_tuner(struct file *file, void *fh, struct v4l2_tuner *a)
+static int vidioc_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *a)
 {
 	struct front_face *front	= fh;
 	struct poseidon *pd		= front->pd;
@@ -1107,7 +1039,6 @@ static int vidioc_s_tuner(struct file *file, void *fh, struct v4l2_tuner *a)
 
 	if (0 != a->index)
 		return -EINVAL;
-	logs(front);
 	for (index = 0; index < POSEIDON_AUDIOMODS; index++)
 		if (a->audmode == pd_audio_modes[index].v4l2_audio_mode)
 			return pd_vidioc_s_tuner(pd, index);
@@ -1128,51 +1059,51 @@ static int vidioc_g_frequency(struct file *file, void *fh,
 	return 0;
 }
 
-static int set_frequency(struct poseidon *pd, __u32 frequency)
+static int set_frequency(struct poseidon *pd, u32 *frequency)
 {
 	s32 ret = 0, param, cmd_status;
 	struct running_context *context = &pd->video_data.context;
 
-	param = frequency * 62500 / 1000;
-	if (param < TUNER_FREQ_MIN/1000 || param > TUNER_FREQ_MAX / 1000)
-		return -EINVAL;
+	*frequency = clamp(*frequency,
+			TUNER_FREQ_MIN / 62500, TUNER_FREQ_MAX / 62500);
+	param = (*frequency) * 62500 / 1000;
 
 	mutex_lock(&pd->lock);
 	ret = send_set_req(pd, TUNE_FREQ_SELECT, param, &cmd_status);
 	ret = send_set_req(pd, TAKE_REQUEST, 0, &cmd_status);
 
 	msleep(250); /* wait for a while until the hardware is ready. */
-	context->freq = frequency;
+	context->freq = *frequency;
 	mutex_unlock(&pd->lock);
 	return ret;
 }
 
 static int vidioc_s_frequency(struct file *file, void *fh,
-				struct v4l2_frequency *freq)
+				const struct v4l2_frequency *freq)
 {
 	struct front_face *front = fh;
 	struct poseidon *pd = front->pd;
+	u32 frequency = freq->frequency;
 
-	logs(front);
+	if (freq->tuner)
+		return -EINVAL;
 #ifdef CONFIG_PM
 	pd->pm_suspend = pm_video_suspend;
 	pd->pm_resume = pm_video_resume;
 #endif
-	return set_frequency(pd, freq->frequency);
+	return set_frequency(pd, &frequency);
 }
 
 static int vidioc_reqbufs(struct file *file, void *fh,
 				struct v4l2_requestbuffers *b)
 {
 	struct front_face *front = file->private_data;
-	logs(front);
 	return videobuf_reqbufs(&front->q, b);
 }
 
 static int vidioc_querybuf(struct file *file, void *fh, struct v4l2_buffer *b)
 {
 	struct front_face *front = file->private_data;
-	logs(front);
 	return videobuf_querybuf(&front->q, b);
 }
 
@@ -1261,7 +1192,6 @@ static int vidioc_streamon(struct file *file, void *fh,
 {
 	struct front_face *front = fh;
 
-	logs(front);
 	if (unlikely(type != front->type))
 		return -EINVAL;
 	return videobuf_streamon(&front->q);
@@ -1272,7 +1202,6 @@ static int vidioc_streamoff(struct file *file, void *fh,
 {
 	struct front_face *front = file->private_data;
 
-	logs(front);
 	if (unlikely(type != front->type))
 		return -EINVAL;
 	return videobuf_streamoff(&front->q);
@@ -1341,11 +1270,11 @@ static int restore_v4l2_context(struct poseidon *pd,
 
 	pd_video_checkmode(pd);
 
-	set_std(pd, &context->tvnormid);
+	set_std(pd, context->tvnormid);
 	vidioc_s_input(NULL, front, context->sig_index);
 	pd_vidioc_s_tuner(pd, context->audio_idx);
 	pd_vidioc_s_fmt(pd, &context->pix);
-	set_frequency(pd, context->freq);
+	set_frequency(pd, &context->freq);
 	return 0;
 }
 
@@ -1406,12 +1335,14 @@ static int pd_video_open(struct file *file)
 	mutex_lock(&pd->lock);
 	usb_autopm_get_interface(pd->interface);
 
-	if (vfd->vfl_type == VFL_TYPE_GRABBER
-		&& !(pd->state & POSEIDON_STATE_ANALOG)) {
-		front = kzalloc(sizeof(struct front_face), GFP_KERNEL);
-		if (!front)
-			goto out;
-
+	if (pd->state && !(pd->state & POSEIDON_STATE_ANALOG)) {
+		ret = -EBUSY;
+		goto out;
+	}
+	front = kzalloc(sizeof(struct front_face), GFP_KERNEL);
+	if (!front)
+		goto out;
+	if (vfd->vfl_type == VFL_TYPE_GRABBER) {
 		pd->cur_transfer_mode	= usb_transfer_mode;/* bulk or iso */
 		init_video_context(&pd->video_data.context);
 
@@ -1422,7 +1353,6 @@ static int pd_video_open(struct file *file)
 			goto out;
 		}
 
-		pd->state		|= POSEIDON_STATE_ANALOG;
 		front->type		= V4L2_BUF_TYPE_VIDEO_CAPTURE;
 		pd->video_data.users++;
 		set_debug_mode(vfd, debug_mode);
@@ -1433,13 +1363,7 @@ static int pd_video_open(struct file *file)
 				V4L2_FIELD_INTERLACED,/* video is interlacd */
 				sizeof(struct videobuf_buffer),/*it's enough*/
 				front, NULL);
-	} else if (vfd->vfl_type == VFL_TYPE_VBI
-		&& !(pd->state & POSEIDON_STATE_VBI)) {
-		front = kzalloc(sizeof(struct front_face), GFP_KERNEL);
-		if (!front)
-			goto out;
-
-		pd->state	|= POSEIDON_STATE_VBI;
+	} else {
 		front->type	= V4L2_BUF_TYPE_VBI_CAPTURE;
 		pd->vbi_data.front = front;
 		pd->vbi_data.users++;
@@ -1450,19 +1374,15 @@ static int pd_video_open(struct file *file)
 				V4L2_FIELD_NONE, /* vbi is NONE mode */
 				sizeof(struct videobuf_buffer),
 				front, NULL);
-	} else {
-		/* maybe add FM support here */
-		log("other ");
-		ret = -EINVAL;
-		goto out;
 	}
 
-	front->pd		= pd;
-	front->curr_frame	= NULL;
+	pd->state |= POSEIDON_STATE_ANALOG;
+	front->pd = pd;
+	front->curr_frame = NULL;
 	INIT_LIST_HEAD(&front->active);
 	spin_lock_init(&front->queue_lock);
 
-	file->private_data	= front;
+	file->private_data = front;
 	kref_get(&pd->kref);
 
 	mutex_unlock(&pd->lock);
@@ -1479,12 +1399,9 @@ static int pd_video_release(struct file *file)
 	struct poseidon *pd = front->pd;
 	s32 cmd_status = 0;
 
-	logs(front);
 	mutex_lock(&pd->lock);
 
 	if (front->type	== V4L2_BUF_TYPE_VIDEO_CAPTURE) {
-		pd->state &= ~POSEIDON_STATE_ANALOG;
-
 		/* stop the device, and free the URBs */
 		usb_transfer_stop(&pd->video_data);
 		free_all_urb(&pd->video_data);
@@ -1496,10 +1413,11 @@ static int pd_video_release(struct file *file)
 		pd->file_for_stream = NULL;
 		pd->video_data.users--;
 	} else if (front->type	== V4L2_BUF_TYPE_VBI_CAPTURE) {
-		pd->state &= ~POSEIDON_STATE_VBI;
 		pd->vbi_data.front = NULL;
 		pd->vbi_data.users--;
 	}
+	if (!pd->vbi_data.users && !pd->video_data.users)
+		pd->state &= ~POSEIDON_STATE_ANALOG;
 	videobuf_stop(&front->q);
 	videobuf_mmap_free(&front->q);
 
@@ -1551,7 +1469,6 @@ static const struct v4l2_ioctl_ops pd_video_ioctl_ops = {
 	.vidioc_enum_fmt_vid_cap	= vidioc_enum_fmt,
 	.vidioc_s_fmt_vid_cap	= vidioc_s_fmt,
 	.vidioc_g_fmt_vbi_cap	= vidioc_g_fmt_vbi, /* VBI */
-	.vidioc_try_fmt_vid_cap = vidioc_try_fmt,
 
 	/* Input */
 	.vidioc_g_input		= vidioc_g_input,
@@ -1566,6 +1483,7 @@ static const struct v4l2_ioctl_ops pd_video_ioctl_ops = {
 	/* Tuner ioctls */
 	.vidioc_g_tuner		= vidioc_g_tuner,
 	.vidioc_s_tuner		= vidioc_s_tuner,
+	.vidioc_g_std		= vidioc_g_std,
 	.vidioc_s_std		= vidioc_s_std,
 	.vidioc_g_frequency	= vidioc_g_frequency,
 	.vidioc_s_frequency	= vidioc_s_frequency,
@@ -1579,59 +1497,29 @@ static const struct v4l2_ioctl_ops pd_video_ioctl_ops = {
 	/* Stream on/off */
 	.vidioc_streamon	= vidioc_streamon,
 	.vidioc_streamoff	= vidioc_streamoff,
-
-	/* Control handling */
-	.vidioc_queryctrl	= vidioc_queryctrl,
-	.vidioc_g_ctrl		= vidioc_g_ctrl,
-	.vidioc_s_ctrl		= vidioc_s_ctrl,
 };
 
 static struct video_device pd_video_template = {
 	.name = "Telegent-Video",
 	.fops = &pd_video_fops,
 	.minor = -1,
-	.release = video_device_release,
+	.release = video_device_release_empty,
 	.tvnorms = V4L2_STD_ALL,
 	.ioctl_ops = &pd_video_ioctl_ops,
 };
 
-struct video_device *vdev_init(struct poseidon *pd, struct video_device *tmp)
-{
-	struct video_device *vfd;
-
-	vfd = video_device_alloc();
-	if (vfd == NULL)
-		return NULL;
-	*vfd		= *tmp;
-	vfd->minor	= -1;
-	vfd->v4l2_dev	= &pd->v4l2_dev;
-	/*vfd->parent	= &(pd->udev->dev); */
-	vfd->release	= video_device_release;
-	video_set_drvdata(vfd, pd);
-	return vfd;
-}
-
-void destroy_video_device(struct video_device **v_dev)
-{
-	struct video_device *dev = *v_dev;
-
-	if (dev == NULL)
-		return;
-
-	if (video_is_registered(dev))
-		video_unregister_device(dev);
-	else
-		video_device_release(dev);
-	*v_dev = NULL;
-}
+static const struct v4l2_ctrl_ops tlg_ctrl_ops = {
+	.s_ctrl = tlg_s_ctrl,
+};
 
 void pd_video_exit(struct poseidon *pd)
 {
 	struct video_data *video = &pd->video_data;
 	struct vbi_data *vbi = &pd->vbi_data;
 
-	destroy_video_device(&video->v_dev);
-	destroy_video_device(&vbi->v_dev);
+	video_unregister_device(&video->v_dev);
+	video_unregister_device(&vbi->v_dev);
+	v4l2_ctrl_handler_free(&video->ctrl_handler);
 	log();
 }
 
@@ -1639,23 +1527,39 @@ int pd_video_init(struct poseidon *pd)
 {
 	struct video_data *video = &pd->video_data;
 	struct vbi_data *vbi	= &pd->vbi_data;
+	struct v4l2_ctrl_handler *hdl = &video->ctrl_handler;
+	u32 freq = TUNER_FREQ_MIN / 62500;
 	int ret = -ENOMEM;
 
-	video->v_dev = vdev_init(pd, &pd_video_template);
-	if (video->v_dev == NULL)
-		goto out;
+	v4l2_ctrl_handler_init(hdl, 4);
+	v4l2_ctrl_new_std(hdl, &tlg_ctrl_ops, V4L2_CID_BRIGHTNESS,
+			0, 10000, 1, 100);
+	v4l2_ctrl_new_std(hdl, &tlg_ctrl_ops, V4L2_CID_CONTRAST,
+			0, 10000, 1, 100);
+	v4l2_ctrl_new_std(hdl, &tlg_ctrl_ops, V4L2_CID_HUE,
+			0, 10000, 1, 100);
+	v4l2_ctrl_new_std(hdl, &tlg_ctrl_ops, V4L2_CID_SATURATION,
+			0, 10000, 1, 100);
+	if (hdl->error) {
+		v4l2_ctrl_handler_free(hdl);
+		return hdl->error;
+	}
+	set_frequency(pd, &freq);
+	video->v_dev = pd_video_template;
+	video->v_dev.v4l2_dev = &pd->v4l2_dev;
+	video->v_dev.ctrl_handler = hdl;
+	video_set_drvdata(&video->v_dev, pd);
 
-	ret = video_register_device(video->v_dev, VFL_TYPE_GRABBER, -1);
+	ret = video_register_device(&video->v_dev, VFL_TYPE_GRABBER, -1);
 	if (ret != 0)
 		goto out;
 
 	/* VBI uses the same template as video */
-	vbi->v_dev = vdev_init(pd, &pd_video_template);
-	if (vbi->v_dev == NULL) {
-		ret = -ENOMEM;
-		goto out;
-	}
-	ret = video_register_device(vbi->v_dev, VFL_TYPE_VBI, -1);
+	vbi->v_dev = pd_video_template;
+	vbi->v_dev.v4l2_dev = &pd->v4l2_dev;
+	vbi->v_dev.ctrl_handler = hdl;
+	video_set_drvdata(&vbi->v_dev, pd);
+	ret = video_register_device(&vbi->v_dev, VFL_TYPE_VBI, -1);
 	if (ret != 0)
 		goto out;
 	log("register VIDEO/VBI devices");
@@ -1665,4 +1569,3 @@ out:
 	pd_video_exit(pd);
 	return ret;
 }
-
diff --git a/drivers/media/usb/tm6000/tm6000-video.c b/drivers/media/usb/tm6000/tm6000-video.c
index 1a6857929c1..a78de1d1bc9 100644
--- a/drivers/media/usb/tm6000/tm6000-video.c
+++ b/drivers/media/usb/tm6000/tm6000-video.c
@@ -1056,13 +1056,13 @@ static int vidioc_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
 	return 0;
 }
 
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *norm)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id norm)
 {
 	int rc = 0;
 	struct tm6000_fh *fh = priv;
 	struct tm6000_core *dev = fh->dev;
 
-	dev->norm = *norm;
+	dev->norm = norm;
 	rc = tm6000_init_analog_mode(dev);
 
 	fh->width  = dev->width;
@@ -1134,7 +1134,7 @@ static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
 
 	dev->input = i;
 
-	rc = vidioc_s_std(file, priv, &dev->vfd->current_norm);
+	rc = vidioc_s_std(file, priv, dev->vfd->current_norm);
 
 	return rc;
 }
@@ -1215,7 +1215,7 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *t)
+				const struct v4l2_tuner *t)
 {
 	struct tm6000_fh   *fh  = priv;
 	struct tm6000_core *dev = fh->dev;
@@ -1255,7 +1255,7 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
+				const struct v4l2_frequency *f)
 {
 	struct tm6000_fh   *fh  = priv;
 	struct tm6000_core *dev = fh->dev;
@@ -1293,18 +1293,14 @@ static int radio_g_tuner(struct file *file, void *priv,
 }
 
 static int radio_s_tuner(struct file *file, void *priv,
-					struct v4l2_tuner *t)
+					const struct v4l2_tuner *t)
 {
 	struct tm6000_fh *fh = file->private_data;
 	struct tm6000_core *dev = fh->dev;
 
 	if (0 != t->index)
 		return -EINVAL;
-	if (t->audmode > V4L2_TUNER_MODE_STEREO)
-		t->audmode = V4L2_TUNER_MODE_STEREO;
-
 	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_tuner, t);
-
 	return 0;
 }
 
diff --git a/drivers/media/usb/ttusb-budget/dvb-ttusb-budget.c b/drivers/media/usb/ttusb-budget/dvb-ttusb-budget.c
index e4071855285..21b9049c7b3 100644
--- a/drivers/media/usb/ttusb-budget/dvb-ttusb-budget.c
+++ b/drivers/media/usb/ttusb-budget/dvb-ttusb-budget.c
@@ -930,11 +930,11 @@ static int ttusb_start_feed(struct dvb_demux_feed *dvbdmxfeed)
 
 	if (dvbdmxfeed->type == DMX_TYPE_TS) {
 		switch (dvbdmxfeed->pes_type) {
-		case DMX_TS_PES_VIDEO:
-		case DMX_TS_PES_AUDIO:
-		case DMX_TS_PES_TELETEXT:
-		case DMX_TS_PES_PCR:
-		case DMX_TS_PES_OTHER:
+		case DMX_PES_VIDEO:
+		case DMX_PES_AUDIO:
+		case DMX_PES_TELETEXT:
+		case DMX_PES_PCR:
+		case DMX_PES_OTHER:
 			break;
 		default:
 			return -EINVAL;
diff --git a/drivers/media/usb/ttusb-dec/ttusb_dec.c b/drivers/media/usb/ttusb-dec/ttusb_dec.c
index 504c8123033..e52c3b97f30 100644
--- a/drivers/media/usb/ttusb-dec/ttusb_dec.c
+++ b/drivers/media/usb/ttusb-dec/ttusb_dec.c
@@ -951,34 +951,34 @@ static int ttusb_dec_start_ts_feed(struct dvb_demux_feed *dvbdmxfeed)
 
 	switch (dvbdmxfeed->pes_type) {
 
-	case DMX_TS_PES_VIDEO:
-		dprintk("  pes_type: DMX_TS_PES_VIDEO\n");
+	case DMX_PES_VIDEO:
+		dprintk("  pes_type: DMX_PES_VIDEO\n");
 		dec->pid[DMX_PES_PCR] = dvbdmxfeed->pid;
 		dec->pid[DMX_PES_VIDEO] = dvbdmxfeed->pid;
 		dec->video_filter = dvbdmxfeed->filter;
 		ttusb_dec_set_pids(dec);
 		break;
 
-	case DMX_TS_PES_AUDIO:
-		dprintk("  pes_type: DMX_TS_PES_AUDIO\n");
+	case DMX_PES_AUDIO:
+		dprintk("  pes_type: DMX_PES_AUDIO\n");
 		dec->pid[DMX_PES_AUDIO] = dvbdmxfeed->pid;
 		dec->audio_filter = dvbdmxfeed->filter;
 		ttusb_dec_set_pids(dec);
 		break;
 
-	case DMX_TS_PES_TELETEXT:
+	case DMX_PES_TELETEXT:
 		dec->pid[DMX_PES_TELETEXT] = dvbdmxfeed->pid;
-		dprintk("  pes_type: DMX_TS_PES_TELETEXT(not supported)\n");
+		dprintk("  pes_type: DMX_PES_TELETEXT(not supported)\n");
 		return -ENOSYS;
 
-	case DMX_TS_PES_PCR:
-		dprintk("  pes_type: DMX_TS_PES_PCR\n");
+	case DMX_PES_PCR:
+		dprintk("  pes_type: DMX_PES_PCR\n");
 		dec->pid[DMX_PES_PCR] = dvbdmxfeed->pid;
 		ttusb_dec_set_pids(dec);
 		break;
 
-	case DMX_TS_PES_OTHER:
-		dprintk("  pes_type: DMX_TS_PES_OTHER(not supported)\n");
+	case DMX_PES_OTHER:
+		dprintk("  pes_type: DMX_PES_OTHER(not supported)\n");
 		return -ENOSYS;
 
 	default:
diff --git a/drivers/media/usb/usbvision/usbvision-video.c b/drivers/media/usb/usbvision/usbvision-video.c
index cd1fe78a553..d34c2afe2c2 100644
--- a/drivers/media/usb/usbvision/usbvision-video.c
+++ b/drivers/media/usb/usbvision/usbvision-video.c
@@ -483,7 +483,7 @@ static int vidioc_g_register(struct file *file, void *priv,
 }
 
 static int vidioc_s_register(struct file *file, void *priv,
-				struct v4l2_dbg_register *reg)
+				const struct v4l2_dbg_register *reg)
 {
 	struct usb_usbvision *usbvision = video_drvdata(file);
 	int err_code;
@@ -595,11 +595,11 @@ static int vidioc_s_input(struct file *file, void *priv, unsigned int input)
 	return 0;
 }
 
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *id)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id id)
 {
 	struct usb_usbvision *usbvision = video_drvdata(file);
 
-	usbvision->tvnorm_id = *id;
+	usbvision->tvnorm_id = id;
 
 	call_all(usbvision, core, s_std, usbvision->tvnorm_id);
 	/* propagate the change to the decoder */
@@ -628,7 +628,7 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *vt)
+				const struct v4l2_tuner *vt)
 {
 	struct usb_usbvision *usbvision = video_drvdata(file);
 
@@ -657,7 +657,7 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *freq)
+				const struct v4l2_frequency *freq)
 {
 	struct usb_usbvision *usbvision = video_drvdata(file);
 
diff --git a/drivers/media/usb/uvc/uvc_ctrl.c b/drivers/media/usb/uvc/uvc_ctrl.c
index 61e28dec991..a2f4501c23c 100644
--- a/drivers/media/usb/uvc/uvc_ctrl.c
+++ b/drivers/media/usb/uvc/uvc_ctrl.c
@@ -1487,7 +1487,7 @@ int uvc_ctrl_set(struct uvc_video_chain *chain,
 			step = mapping->get(mapping, UVC_GET_RES,
 					uvc_ctrl_data(ctrl, UVC_CTRL_DATA_RES));
 			if (!(step & value))
-				return -ERANGE;
+				return -EINVAL;
 		}
 
 		break;
diff --git a/drivers/media/usb/uvc/uvc_queue.c b/drivers/media/usb/uvc/uvc_queue.c
index 6c233a54ce4..cd962be860c 100644
--- a/drivers/media/usb/uvc/uvc_queue.c
+++ b/drivers/media/usb/uvc/uvc_queue.c
@@ -149,6 +149,7 @@ int uvc_queue_init(struct uvc_video_queue *queue, enum v4l2_buf_type type,
 	queue->queue.buf_struct_size = sizeof(struct uvc_buffer);
 	queue->queue.ops = &uvc_queue_qops;
 	queue->queue.mem_ops = &vb2_vmalloc_memops;
+	queue->queue.timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 	ret = vb2_queue_init(&queue->queue);
 	if (ret)
 		return ret;
diff --git a/drivers/media/v4l2-core/Kconfig b/drivers/media/v4l2-core/Kconfig
index 976d029e992..8c05565a240 100644
--- a/drivers/media/v4l2-core/Kconfig
+++ b/drivers/media/v4l2-core/Kconfig
@@ -67,6 +67,7 @@ config VIDEOBUF2_MEMOPS
 
 config VIDEOBUF2_DMA_CONTIG
 	tristate
+	depends on HAS_DMA
 	select VIDEOBUF2_CORE
 	select VIDEOBUF2_MEMOPS
 	select DMA_SHARED_BUFFER
diff --git a/drivers/media/v4l2-core/Makefile b/drivers/media/v4l2-core/Makefile
index a9d355230e8..aa50c46314b 100644
--- a/drivers/media/v4l2-core/Makefile
+++ b/drivers/media/v4l2-core/Makefile
@@ -9,8 +9,11 @@ videodev-objs	:=	v4l2-dev.o v4l2-ioctl.o v4l2-device.o v4l2-fh.o \
 ifeq ($(CONFIG_COMPAT),y)
   videodev-objs += v4l2-compat-ioctl32.o
 endif
+ifeq ($(CONFIG_OF),y)
+  videodev-objs += v4l2-of.o
+endif
 
-obj-$(CONFIG_VIDEO_DEV) += videodev.o
+obj-$(CONFIG_VIDEO_V4L2) += videodev.o
 obj-$(CONFIG_VIDEO_V4L2_INT_DEVICE) += v4l2-int-device.o
 obj-$(CONFIG_VIDEO_V4L2) += v4l2-common.o
 
diff --git a/drivers/media/v4l2-core/tuner-core.c b/drivers/media/v4l2-core/tuner-core.c
index b5a8aac2e12..ddc9379eb27 100644
--- a/drivers/media/v4l2-core/tuner-core.c
+++ b/drivers/media/v4l2-core/tuner-core.c
@@ -132,7 +132,7 @@ struct tuner {
 	bool                standby;	/* Standby mode */
 
 	unsigned int        type; /* chip type id */
-	unsigned int        config;
+	void                *config;
 	const char          *name;
 };
 
@@ -218,26 +218,6 @@ static void fe_standby(struct dvb_frontend *fe)
 		fe_tuner_ops->sleep(fe);
 }
 
-static int fe_has_signal(struct dvb_frontend *fe)
-{
-	u16 strength = 0;
-
-	if (fe->ops.tuner_ops.get_rf_strength)
-		fe->ops.tuner_ops.get_rf_strength(fe, &strength);
-
-	return strength;
-}
-
-static int fe_get_afc(struct dvb_frontend *fe)
-{
-	s32 afc = 0;
-
-	if (fe->ops.tuner_ops.get_afc)
-		fe->ops.tuner_ops.get_afc(fe, &afc);
-
-	return 0;
-}
-
 static int fe_set_config(struct dvb_frontend *fe, void *priv_cfg)
 {
 	struct dvb_tuner_ops *fe_tuner_ops = &fe->ops.tuner_ops;
@@ -253,11 +233,9 @@ static int fe_set_config(struct dvb_frontend *fe, void *priv_cfg)
 
 static void tuner_status(struct dvb_frontend *fe);
 
-static struct analog_demod_ops tuner_analog_ops = {
+static const struct analog_demod_ops tuner_analog_ops = {
 	.set_params     = fe_set_params,
 	.standby        = fe_standby,
-	.has_signal     = fe_has_signal,
-	.get_afc        = fe_get_afc,
 	.set_config     = fe_set_config,
 	.tuner_status   = tuner_status
 };
@@ -272,9 +250,8 @@ static struct analog_demod_ops tuner_analog_ops = {
  * @c:			i2c_client descriptoy
  * @type:		type of the tuner (e. g. tuner number)
  * @new_mode_mask:	Indicates if tuner supports TV and/or Radio
- * @new_config:		an optional parameter ranging from 0-255 used by
-			a few tuners to adjust an internal parameter,
-			like LNA mode
+ * @new_config:		an optional parameter used by a few tuners to adjust
+			internal parameters, like LNA mode
  * @tuner_callback:	an optional function to be called when switching
  *			to analog mode
  *
@@ -282,7 +259,7 @@ static struct analog_demod_ops tuner_analog_ops = {
  * by tun_setup structure. It contains several per-tuner initialization "magic"
  */
 static void set_type(struct i2c_client *c, unsigned int type,
-		     unsigned int new_mode_mask, unsigned int new_config,
+		     unsigned int new_mode_mask, void *new_config,
 		     int (*tuner_callback) (void *dev, int component, int cmd, int arg))
 {
 	struct tuner *t = to_tuner(i2c_get_clientdata(c));
@@ -297,8 +274,7 @@ static void set_type(struct i2c_client *c, unsigned int type,
 	}
 
 	t->type = type;
-	/* prevent invalid config values */
-	t->config = new_config < 256 ? new_config : 0;
+	t->config = new_config;
 	if (tuner_callback != NULL) {
 		tuner_dbg("defining GPIO callback\n");
 		t->fe.callback = tuner_callback;
@@ -316,11 +292,8 @@ static void set_type(struct i2c_client *c, unsigned int type,
 		break;
 	case TUNER_PHILIPS_TDA8290:
 	{
-		struct tda829x_config cfg = {
-			.lna_cfg        = t->config,
-		};
 		if (!dvb_attach(tda829x_attach, &t->fe, t->i2c->adapter,
-				t->i2c->addr, &cfg))
+				t->i2c->addr, t->config))
 			goto attach_failed;
 		break;
 	}
@@ -409,7 +382,6 @@ static void set_type(struct i2c_client *c, unsigned int type,
 	case TUNER_NXP_TDA18271:
 	{
 		struct tda18271_config cfg = {
-			.config = t->config,
 			.small_i2c = TDA18271_03_BYTE_CHUNK_INIT,
 		};
 
@@ -453,6 +425,11 @@ static void set_type(struct i2c_client *c, unsigned int type,
 		memcpy(analog_ops, &tuner_analog_ops,
 		       sizeof(struct analog_demod_ops));
 
+		if (fe_tuner_ops->get_rf_strength)
+			analog_ops->has_signal = fe_tuner_ops->get_rf_strength;
+		if (fe_tuner_ops->get_afc)
+			analog_ops->get_afc = fe_tuner_ops->get_afc;
+
 	} else {
 		t->name = analog_ops->info.name;
 	}
@@ -506,7 +483,7 @@ static int tuner_s_type_addr(struct v4l2_subdev *sd,
 	struct tuner *t = to_tuner(sd);
 	struct i2c_client *c = v4l2_get_subdevdata(sd);
 
-	tuner_dbg("Calling set_type_addr for type=%d, addr=0x%02x, mode=0x%02x, config=0x%02x\n",
+	tuner_dbg("Calling set_type_addr for type=%d, addr=0x%02x, mode=0x%02x, config=%p\n",
 			tun_setup->type,
 			tun_setup->addr,
 			tun_setup->mode_mask,
@@ -1073,9 +1050,12 @@ static void tuner_status(struct dvb_frontend *fe)
 		if (tuner_status & TUNER_STATUS_STEREO)
 			tuner_info("Stereo:          yes\n");
 	}
-	if (analog_ops->has_signal)
-		tuner_info("Signal strength: %d\n",
-			   analog_ops->has_signal(fe));
+	if (analog_ops->has_signal) {
+		u16 signal;
+
+		if (!analog_ops->has_signal(fe, &signal))
+			tuner_info("Signal strength: %hu\n", signal);
+	}
 }
 
 /*
@@ -1134,7 +1114,7 @@ static int tuner_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
 	return 0;
 }
 
-static int tuner_s_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *f)
+static int tuner_s_frequency(struct v4l2_subdev *sd, const struct v4l2_frequency *f)
 {
 	struct tuner *t = to_tuner(sd);
 
@@ -1193,9 +1173,13 @@ static int tuner_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
 	if (check_mode(t, vt->type) == -EINVAL)
 		return 0;
 	if (vt->type == t->mode && analog_ops->get_afc)
-		vt->afc = analog_ops->get_afc(&t->fe);
-	if (analog_ops->has_signal)
-		vt->signal = analog_ops->has_signal(&t->fe);
+		analog_ops->get_afc(&t->fe, &vt->afc);
+	if (vt->type == t->mode && analog_ops->has_signal) {
+		u16 signal = (u16)vt->signal;
+
+		if (!analog_ops->has_signal(&t->fe, &signal))
+			vt->signal = signal;
+	}
 	if (vt->type != V4L2_TUNER_RADIO) {
 		vt->capability |= V4L2_TUNER_CAP_NORM;
 		vt->rangelow = tv_range[0] * 16;
@@ -1233,7 +1217,7 @@ static int tuner_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
  * Note: vt->type should be initialized before calling it.
  * This is done by either video_ioctl2 or by the bridge driver.
  */
-static int tuner_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+static int tuner_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
 {
 	struct tuner *t = to_tuner(sd);
 
diff --git a/drivers/media/v4l2-core/v4l2-common.c b/drivers/media/v4l2-core/v4l2-common.c
index aa044f49166..3fed63f4e02 100644
--- a/drivers/media/v4l2-core/v4l2-common.c
+++ b/drivers/media/v4l2-core/v4l2-common.c
@@ -230,7 +230,7 @@ EXPORT_SYMBOL(v4l2_ctrl_next);
 int v4l2_chip_match_host(const struct v4l2_dbg_match *match)
 {
 	switch (match->type) {
-	case V4L2_CHIP_MATCH_HOST:
+	case V4L2_CHIP_MATCH_BRIDGE:
 		return match->addr == 0;
 	default:
 		return 0;
@@ -251,12 +251,11 @@ int v4l2_chip_match_i2c_client(struct i2c_client *c, const struct v4l2_dbg_match
 		if (c->driver == NULL || c->driver->driver.name == NULL)
 			return 0;
 		len = strlen(c->driver->driver.name);
-		/* legacy drivers have a ' suffix, don't try to match that */
-		if (len && c->driver->driver.name[len - 1] == '\'')
-			len--;
 		return len && !strncmp(c->driver->driver.name, match->name, len);
 	case V4L2_CHIP_MATCH_I2C_ADDR:
 		return c->addr == match->addr;
+	case V4L2_CHIP_MATCH_SUBDEV:
+		return 1;
 	default:
 		return 0;
 	}
@@ -551,53 +550,6 @@ void v4l_bound_align_image(u32 *w, unsigned int wmin, unsigned int wmax,
 EXPORT_SYMBOL_GPL(v4l_bound_align_image);
 
 /**
- * v4l_fill_dv_preset_info - fill description of a digital video preset
- * @preset - preset value
- * @info - pointer to struct v4l2_dv_enum_preset
- *
- * drivers can use this helper function to fill description of dv preset
- * in info.
- */
-int v4l_fill_dv_preset_info(u32 preset, struct v4l2_dv_enum_preset *info)
-{
-	static const struct v4l2_dv_preset_info {
-		u16 width;
-		u16 height;
-		const char *name;
-	} dv_presets[] = {
-		{ 0, 0, "Invalid" },		/* V4L2_DV_INVALID */
-		{ 720,  480, "480p@59.94" },	/* V4L2_DV_480P59_94 */
-		{ 720,  576, "576p@50" },	/* V4L2_DV_576P50 */
-		{ 1280, 720, "720p@24" },	/* V4L2_DV_720P24 */
-		{ 1280, 720, "720p@25" },	/* V4L2_DV_720P25 */
-		{ 1280, 720, "720p@30" },	/* V4L2_DV_720P30 */
-		{ 1280, 720, "720p@50" },	/* V4L2_DV_720P50 */
-		{ 1280, 720, "720p@59.94" },	/* V4L2_DV_720P59_94 */
-		{ 1280, 720, "720p@60" },	/* V4L2_DV_720P60 */
-		{ 1920, 1080, "1080i@29.97" },	/* V4L2_DV_1080I29_97 */
-		{ 1920, 1080, "1080i@30" },	/* V4L2_DV_1080I30 */
-		{ 1920, 1080, "1080i@25" },	/* V4L2_DV_1080I25 */
-		{ 1920, 1080, "1080i@50" },	/* V4L2_DV_1080I50 */
-		{ 1920, 1080, "1080i@60" },	/* V4L2_DV_1080I60 */
-		{ 1920, 1080, "1080p@24" },	/* V4L2_DV_1080P24 */
-		{ 1920, 1080, "1080p@25" },	/* V4L2_DV_1080P25 */
-		{ 1920, 1080, "1080p@30" },	/* V4L2_DV_1080P30 */
-		{ 1920, 1080, "1080p@50" },	/* V4L2_DV_1080P50 */
-		{ 1920, 1080, "1080p@60" },	/* V4L2_DV_1080P60 */
-	};
-
-	if (info == NULL || preset >= ARRAY_SIZE(dv_presets))
-		return -EINVAL;
-
-	info->preset = preset;
-	info->width = dv_presets[preset].width;
-	info->height = dv_presets[preset].height;
-	strlcpy(info->name, dv_presets[preset].name, sizeof(info->name));
-	return 0;
-}
-EXPORT_SYMBOL_GPL(v4l_fill_dv_preset_info);
-
-/**
  * v4l_match_dv_timings - check if two timings match
  * @t1 - compare this v4l2_dv_timings struct...
  * @t2 - with this struct.
diff --git a/drivers/media/v4l2-core/v4l2-compat-ioctl32.c b/drivers/media/v4l2-core/v4l2-compat-ioctl32.c
index 7157af301b1..f1295519f28 100644
--- a/drivers/media/v4l2-core/v4l2-compat-ioctl32.c
+++ b/drivers/media/v4l2-core/v4l2-compat-ioctl32.c
@@ -1076,10 +1076,6 @@ long v4l2_compat_ioctl32(struct file *file, unsigned int cmd, unsigned long arg)
 	case VIDIOC_DBG_G_REGISTER:
 	case VIDIOC_DBG_G_CHIP_IDENT:
 	case VIDIOC_S_HW_FREQ_SEEK:
-	case VIDIOC_ENUM_DV_PRESETS:
-	case VIDIOC_S_DV_PRESET:
-	case VIDIOC_G_DV_PRESET:
-	case VIDIOC_QUERY_DV_PRESET:
 	case VIDIOC_S_DV_TIMINGS:
 	case VIDIOC_G_DV_TIMINGS:
 	case VIDIOC_DQEVENT:
diff --git a/drivers/media/v4l2-core/v4l2-ctrls.c b/drivers/media/v4l2-core/v4l2-ctrls.c
index 6b28b580050..ebb8e48619a 100644
--- a/drivers/media/v4l2-core/v4l2-ctrls.c
+++ b/drivers/media/v4l2-core/v4l2-ctrls.c
@@ -234,6 +234,7 @@ const char * const *v4l2_ctrl_get_menu(u32 id)
 		"Average",
 		"Center Weighted",
 		"Spot",
+		"Matrix",
 		NULL
 	};
 	static const char * const camera_auto_focus_range[] = {
@@ -297,8 +298,8 @@ const char * const *v4l2_ctrl_get_menu(u32 id)
 		"Text",
 		NULL
 	};
-	static const char * const tune_preemphasis[] = {
-		"No Preemphasis",
+	static const char * const tune_emphasis[] = {
+		"None",
 		"50 Microseconds",
 		"75 Microseconds",
 		NULL,
@@ -508,7 +509,9 @@ const char * const *v4l2_ctrl_get_menu(u32 id)
 	case V4L2_CID_SCENE_MODE:
 		return scene_mode;
 	case V4L2_CID_TUNE_PREEMPHASIS:
-		return tune_preemphasis;
+		return tune_emphasis;
+	case V4L2_CID_TUNE_DEEMPHASIS:
+		return tune_emphasis;
 	case V4L2_CID_FLASH_LED_MODE:
 		return flash_led_mode;
 	case V4L2_CID_FLASH_STROBE_SOURCE:
@@ -695,6 +698,7 @@ const char *v4l2_ctrl_get_name(u32 id)
 	case V4L2_CID_MPEG_VIDEO_DEC_PTS:			return "Video Decoder PTS";
 	case V4L2_CID_MPEG_VIDEO_DEC_FRAME:			return "Video Decoder Frame Count";
 	case V4L2_CID_MPEG_VIDEO_VBV_DELAY:			return "Initial Delay for VBV Control";
+	case V4L2_CID_MPEG_VIDEO_REPEAT_SEQ_HEADER:		return "Repeat Sequence Header";
 
 	/* CAMERA controls */
 	/* Keep the order of the 'case's the same as in videodev2.h! */
@@ -799,6 +803,9 @@ const char *v4l2_ctrl_get_name(u32 id)
 	case V4L2_CID_DV_RX_POWER_PRESENT:	return "Power Present";
 	case V4L2_CID_DV_RX_RGB_RANGE:		return "Rx RGB Quantization Range";
 
+	case V4L2_CID_FM_RX_CLASS:		return "FM Radio Receiver Controls";
+	case V4L2_CID_TUNE_DEEMPHASIS:		return "De-Emphasis";
+	case V4L2_CID_RDS_RECEPTION:		return "RDS Reception";
 	default:
 		return NULL;
 	}
@@ -844,8 +851,10 @@ 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_MPEG_VIDEO_REPEAT_SEQ_HEADER:
 	case V4L2_CID_WIDE_DYNAMIC_RANGE:
 	case V4L2_CID_IMAGE_STABILIZATION:
+	case V4L2_CID_RDS_RECEPTION:
 		*type = V4L2_CTRL_TYPE_BOOLEAN;
 		*min = 0;
 		*max = *step = 1;
@@ -904,6 +913,7 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
 	case V4L2_CID_DV_TX_RGB_RANGE:
 	case V4L2_CID_DV_RX_RGB_RANGE:
 	case V4L2_CID_TEST_PATTERN:
+	case V4L2_CID_TUNE_DEEMPHASIS:
 		*type = V4L2_CTRL_TYPE_MENU;
 		break;
 	case V4L2_CID_LINK_FREQ:
@@ -926,6 +936,7 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
 	case V4L2_CID_IMAGE_SOURCE_CLASS:
 	case V4L2_CID_IMAGE_PROC_CLASS:
 	case V4L2_CID_DV_CLASS:
+	case V4L2_CID_FM_RX_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;
@@ -1362,11 +1373,13 @@ static inline int handler_set_err(struct v4l2_ctrl_handler *hdl, int err)
 }
 
 /* Initialize the handler */
-int v4l2_ctrl_handler_init(struct v4l2_ctrl_handler *hdl,
-			   unsigned nr_of_controls_hint)
+int v4l2_ctrl_handler_init_class(struct v4l2_ctrl_handler *hdl,
+				 unsigned nr_of_controls_hint,
+				 struct lock_class_key *key, const char *name)
 {
 	hdl->lock = &hdl->_lock;
 	mutex_init(hdl->lock);
+	lockdep_set_class_and_name(hdl->lock, key, name);
 	INIT_LIST_HEAD(&hdl->ctrls);
 	INIT_LIST_HEAD(&hdl->ctrl_refs);
 	hdl->nr_of_buckets = 1 + nr_of_controls_hint / 8;
@@ -1375,7 +1388,7 @@ int v4l2_ctrl_handler_init(struct v4l2_ctrl_handler *hdl,
 	hdl->error = hdl->buckets ? 0 : -ENOMEM;
 	return hdl->error;
 }
-EXPORT_SYMBOL(v4l2_ctrl_handler_init);
+EXPORT_SYMBOL(v4l2_ctrl_handler_init_class);
 
 /* Free all controls and control refs */
 void v4l2_ctrl_handler_free(struct v4l2_ctrl_handler *hdl)
diff --git a/drivers/media/v4l2-core/v4l2-dev.c b/drivers/media/v4l2-core/v4l2-dev.c
index de1e9ab7db9..5923c5dfacd 100644
--- a/drivers/media/v4l2-core/v4l2-dev.c
+++ b/drivers/media/v4l2-core/v4l2-dev.c
@@ -592,8 +592,9 @@ static void determine_valid_ioctls(struct video_device *vdev)
 	SET_VALID_IOCTL(ops, VIDIOC_S_FREQUENCY, vidioc_s_frequency);
 	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);
+	set_bit(_IOC_NR(VIDIOC_DBG_G_CHIP_INFO), valid_ioctls);
+	set_bit(_IOC_NR(VIDIOC_DBG_G_REGISTER), valid_ioctls);
+	set_bit(_IOC_NR(VIDIOC_DBG_S_REGISTER), valid_ioctls);
 #endif
 	SET_VALID_IOCTL(ops, VIDIOC_DBG_G_CHIP_IDENT, vidioc_g_chip_ident);
 	/* yes, really vidioc_subscribe_event */
@@ -685,7 +686,6 @@ static void determine_valid_ioctls(struct video_device *vdev)
 			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_QUERY_DV_PRESET, vidioc_query_dv_preset);
 			SET_VALID_IOCTL(ops, VIDIOC_QUERY_DV_TIMINGS, vidioc_query_dv_timings);
 		}
 		if (is_tx) {
@@ -708,9 +708,6 @@ static void determine_valid_ioctls(struct video_device *vdev)
 					(ops->vidioc_g_std || 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_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_S_DV_TIMINGS, vidioc_s_dv_timings);
 		SET_VALID_IOCTL(ops, VIDIOC_G_DV_TIMINGS, vidioc_g_dv_timings);
 		SET_VALID_IOCTL(ops, VIDIOC_ENUM_DV_TIMINGS, vidioc_enum_dv_timings);
diff --git a/drivers/media/v4l2-core/v4l2-ioctl.c b/drivers/media/v4l2-core/v4l2-ioctl.c
index aa6e7c788db..f81bda1a48e 100644
--- a/drivers/media/v4l2-core/v4l2-ioctl.c
+++ b/drivers/media/v4l2-core/v4l2-ioctl.c
@@ -35,6 +35,8 @@
 	memset((u8 *)(p) + offsetof(typeof(*(p)), field) + sizeof((p)->field), \
 	0, sizeof(*(p)) - offsetof(typeof(*(p)), field) - sizeof((p)->field))
 
+#define is_valid_ioctl(vfd, cmd) test_bit(_IOC_NR(cmd), (vfd)->valid_ioctls)
+
 struct std_descr {
 	v4l2_std_id std;
 	const char *descr;
@@ -167,9 +169,11 @@ static void v4l_print_querycap(const void *arg, bool write_only)
 {
 	const struct v4l2_capability *p = arg;
 
-	pr_cont("driver=%s, card=%s, bus=%s, version=0x%08x, "
+	pr_cont("driver=%.*s, card=%.*s, bus=%.*s, version=0x%08x, "
 		"capabilities=0x%08x, device_caps=0x%08x\n",
-		p->driver, p->card, p->bus_info,
+		(int)sizeof(p->driver), p->driver,
+		(int)sizeof(p->card), p->card,
+		(int)sizeof(p->bus_info), p->bus_info,
 		p->version, p->capabilities, p->device_caps);
 }
 
@@ -177,20 +181,21 @@ static void v4l_print_enuminput(const void *arg, bool write_only)
 {
 	const struct v4l2_input *p = arg;
 
-	pr_cont("index=%u, name=%s, type=%u, audioset=0x%x, tuner=%u, "
+	pr_cont("index=%u, name=%.*s, type=%u, audioset=0x%x, tuner=%u, "
 		"std=0x%08Lx, status=0x%x, capabilities=0x%x\n",
-		p->index, p->name, p->type, p->audioset, p->tuner,
-		(unsigned long long)p->std, p->status, p->capabilities);
+		p->index, (int)sizeof(p->name), p->name, p->type, p->audioset,
+		p->tuner, (unsigned long long)p->std, p->status,
+		p->capabilities);
 }
 
 static void v4l_print_enumoutput(const void *arg, bool write_only)
 {
 	const struct v4l2_output *p = arg;
 
-	pr_cont("index=%u, name=%s, type=%u, audioset=0x%x, "
+	pr_cont("index=%u, name=%.*s, type=%u, audioset=0x%x, "
 		"modulator=%u, std=0x%08Lx, capabilities=0x%x\n",
-		p->index, p->name, p->type, p->audioset, p->modulator,
-		(unsigned long long)p->std, p->capabilities);
+		p->index, (int)sizeof(p->name), p->name, p->type, p->audioset,
+		p->modulator, (unsigned long long)p->std, p->capabilities);
 }
 
 static void v4l_print_audio(const void *arg, bool write_only)
@@ -200,8 +205,9 @@ static void v4l_print_audio(const void *arg, bool write_only)
 	if (write_only)
 		pr_cont("index=%u, mode=0x%x\n", p->index, p->mode);
 	else
-		pr_cont("index=%u, name=%s, capability=0x%x, mode=0x%x\n",
-			p->index, p->name, p->capability, p->mode);
+		pr_cont("index=%u, name=%.*s, capability=0x%x, mode=0x%x\n",
+			p->index, (int)sizeof(p->name), p->name,
+			p->capability, p->mode);
 }
 
 static void v4l_print_audioout(const void *arg, bool write_only)
@@ -211,21 +217,22 @@ static void v4l_print_audioout(const void *arg, bool write_only)
 	if (write_only)
 		pr_cont("index=%u\n", p->index);
 	else
-		pr_cont("index=%u, name=%s, capability=0x%x, mode=0x%x\n",
-			p->index, p->name, p->capability, p->mode);
+		pr_cont("index=%u, name=%.*s, capability=0x%x, mode=0x%x\n",
+			p->index, (int)sizeof(p->name), p->name,
+			p->capability, p->mode);
 }
 
 static void v4l_print_fmtdesc(const void *arg, bool write_only)
 {
 	const struct v4l2_fmtdesc *p = arg;
 
-	pr_cont("index=%u, type=%s, flags=0x%x, pixelformat=%c%c%c%c, description='%s'\n",
+	pr_cont("index=%u, type=%s, flags=0x%x, pixelformat=%c%c%c%c, description='%.*s'\n",
 		p->index, prt_names(p->type, v4l2_type_names),
 		p->flags, (p->pixelformat & 0xff),
 		(p->pixelformat >>  8) & 0xff,
 		(p->pixelformat >> 16) & 0xff,
 		(p->pixelformat >> 24) & 0xff,
-		p->description);
+		(int)sizeof(p->description), p->description);
 }
 
 static void v4l_print_format(const void *arg, bool write_only)
@@ -348,9 +355,9 @@ static void v4l_print_modulator(const void *arg, bool write_only)
 	if (write_only)
 		pr_cont("index=%u, txsubchans=0x%x", p->index, p->txsubchans);
 	else
-		pr_cont("index=%u, name=%s, capability=0x%x, "
+		pr_cont("index=%u, name=%.*s, capability=0x%x, "
 			"rangelow=%u, rangehigh=%u, txsubchans=0x%x\n",
-			p->index, p->name, p->capability,
+			p->index, (int)sizeof(p->name), p->name, p->capability,
 			p->rangelow, p->rangehigh, p->txsubchans);
 }
 
@@ -361,10 +368,10 @@ static void v4l_print_tuner(const void *arg, bool write_only)
 	if (write_only)
 		pr_cont("index=%u, audmode=%u\n", p->index, p->audmode);
 	else
-		pr_cont("index=%u, name=%s, type=%u, capability=0x%x, "
+		pr_cont("index=%u, name=%.*s, type=%u, capability=0x%x, "
 			"rangelow=%u, rangehigh=%u, signal=%u, afc=%d, "
 			"rxsubchans=0x%x, audmode=%u\n",
-			p->index, p->name, p->type,
+			p->index, (int)sizeof(p->name), p->name, p->type,
 			p->capability, p->rangelow,
 			p->rangehigh, p->signal, p->afc,
 			p->rxsubchans, p->audmode);
@@ -382,9 +389,9 @@ static void v4l_print_standard(const void *arg, bool write_only)
 {
 	const struct v4l2_standard *p = arg;
 
-	pr_cont("index=%u, id=0x%Lx, name=%s, fps=%u/%u, "
+	pr_cont("index=%u, id=0x%Lx, name=%.*s, fps=%u/%u, "
 		"framelines=%u\n", p->index,
-		(unsigned long long)p->id, p->name,
+		(unsigned long long)p->id, (int)sizeof(p->name), p->name,
 		p->frameperiod.numerator,
 		p->frameperiod.denominator,
 		p->framelines);
@@ -504,9 +511,9 @@ static void v4l_print_queryctrl(const void *arg, bool write_only)
 {
 	const struct v4l2_queryctrl *p = arg;
 
-	pr_cont("id=0x%x, type=%d, name=%s, min/max=%d/%d, "
+	pr_cont("id=0x%x, type=%d, name=%.*s, min/max=%d/%d, "
 		"step=%d, default=%d, flags=0x%08x\n",
-			p->id, p->type, p->name,
+			p->id, p->type, (int)sizeof(p->name), p->name,
 			p->minimum, p->maximum,
 			p->step, p->default_value, p->flags);
 }
@@ -623,39 +630,39 @@ static void v4l_print_dbg_chip_ident(const void *arg, bool write_only)
 
 	pr_cont("type=%u, ", p->match.type);
 	if (p->match.type == V4L2_CHIP_MATCH_I2C_DRIVER)
-		pr_cont("name=%s, ", p->match.name);
+		pr_cont("name=%.*s, ",
+				(int)sizeof(p->match.name), p->match.name);
 	else
 		pr_cont("addr=%u, ", p->match.addr);
 	pr_cont("chip_ident=%u, revision=0x%x\n",
 			p->ident, p->revision);
 }
 
-static void v4l_print_dbg_register(const void *arg, bool write_only)
+static void v4l_print_dbg_chip_info(const void *arg, bool write_only)
 {
-	const struct v4l2_dbg_register *p = arg;
+	const struct v4l2_dbg_chip_info *p = arg;
 
 	pr_cont("type=%u, ", p->match.type);
 	if (p->match.type == V4L2_CHIP_MATCH_I2C_DRIVER)
-		pr_cont("name=%s, ", p->match.name);
+		pr_cont("name=%.*s, ",
+				(int)sizeof(p->match.name), p->match.name);
 	else
 		pr_cont("addr=%u, ", p->match.addr);
-	pr_cont("reg=0x%llx, val=0x%llx\n",
-			p->reg, p->val);
+	pr_cont("name=%.*s\n", (int)sizeof(p->name), p->name);
 }
 
-static void v4l_print_dv_enum_presets(const void *arg, bool write_only)
-{
-	const struct v4l2_dv_enum_preset *p = arg;
-
-	pr_cont("index=%u, preset=%u, name=%s, width=%u, height=%u\n",
-			p->index, p->preset, p->name, p->width, p->height);
-}
-
-static void v4l_print_dv_preset(const void *arg, bool write_only)
+static void v4l_print_dbg_register(const void *arg, bool write_only)
 {
-	const struct v4l2_dv_preset *p = arg;
+	const struct v4l2_dbg_register *p = arg;
 
-	pr_cont("preset=%u\n", p->preset);
+	pr_cont("type=%u, ", p->match.type);
+	if (p->match.type == V4L2_CHIP_MATCH_I2C_DRIVER)
+		pr_cont("name=%.*s, ",
+				(int)sizeof(p->match.name), p->match.name);
+	else
+		pr_cont("addr=%u, ", p->match.addr);
+	pr_cont("reg=0x%llx, val=0x%llx\n",
+			p->reg, p->val);
 }
 
 static void v4l_print_dv_timings(const void *arg, bool write_only)
@@ -997,20 +1004,17 @@ static int v4l_s_priority(const struct v4l2_ioctl_ops *ops,
 static int v4l_enuminput(const struct v4l2_ioctl_ops *ops,
 				struct file *file, void *fh, void *arg)
 {
+	struct video_device *vfd = video_devdata(file);
 	struct v4l2_input *p = arg;
 
 	/*
-	 * We set the flags for CAP_PRESETS, CAP_DV_TIMINGS &
+	 * We set the flags for CAP_DV_TIMINGS &
 	 * CAP_STD here based on ioctl handler provided by the
 	 * driver. If the driver doesn't support these
 	 * for a specific input, it must override these flags.
 	 */
-	if (ops->vidioc_s_std)
+	if (is_valid_ioctl(vfd, VIDIOC_S_STD))
 		p->capabilities |= V4L2_IN_CAP_STD;
-	if (ops->vidioc_s_dv_preset)
-		p->capabilities |= V4L2_IN_CAP_PRESETS;
-	if (ops->vidioc_s_dv_timings)
-		p->capabilities |= V4L2_IN_CAP_DV_TIMINGS;
 
 	return ops->vidioc_enum_input(file, fh, p);
 }
@@ -1018,20 +1022,17 @@ static int v4l_enuminput(const struct v4l2_ioctl_ops *ops,
 static int v4l_enumoutput(const struct v4l2_ioctl_ops *ops,
 				struct file *file, void *fh, void *arg)
 {
+	struct video_device *vfd = video_devdata(file);
 	struct v4l2_output *p = arg;
 
 	/*
-	 * We set the flags for CAP_PRESETS, CAP_DV_TIMINGS &
+	 * We set the flags for CAP_DV_TIMINGS &
 	 * CAP_STD here based on ioctl handler provided by the
 	 * driver. If the driver doesn't support these
 	 * for a specific output, it must override these flags.
 	 */
-	if (ops->vidioc_s_std)
+	if (is_valid_ioctl(vfd, VIDIOC_S_STD))
 		p->capabilities |= V4L2_OUT_CAP_STD;
-	if (ops->vidioc_s_dv_preset)
-		p->capabilities |= V4L2_OUT_CAP_PRESETS;
-	if (ops->vidioc_s_dv_timings)
-		p->capabilities |= V4L2_OUT_CAP_DV_TIMINGS;
 
 	return ops->vidioc_enum_output(file, fh, p);
 }
@@ -1316,7 +1317,7 @@ static int v4l_s_frequency(const struct v4l2_ioctl_ops *ops,
 				struct file *file, void *fh, void *arg)
 {
 	struct video_device *vfd = video_devdata(file);
-	struct v4l2_frequency *p = arg;
+	const struct v4l2_frequency *p = arg;
 	enum v4l2_tuner_type type;
 
 	type = (vfd->vfl_type == VFL_TYPE_RADIO) ?
@@ -1383,15 +1384,15 @@ static int v4l_s_std(const struct v4l2_ioctl_ops *ops,
 				struct file *file, void *fh, void *arg)
 {
 	struct video_device *vfd = video_devdata(file);
-	v4l2_std_id *id = arg, norm;
+	v4l2_std_id id = *(v4l2_std_id *)arg, norm;
 	int ret;
 
-	norm = (*id) & vfd->tvnorms;
+	norm = id & vfd->tvnorms;
 	if (vfd->tvnorms && !norm)	/* Check if std is supported */
 		return -EINVAL;
 
 	/* Calls the specific handler */
-	ret = ops->vidioc_s_std(file, fh, &norm);
+	ret = ops->vidioc_s_std(file, fh, norm);
 
 	/* Updates standard information */
 	if (ret >= 0)
@@ -1513,7 +1514,7 @@ static int v4l_g_parm(const struct v4l2_ioctl_ops *ops,
 	    p->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
 		return -EINVAL;
 	p->parm.capture.readbuffers = 2;
-	if (ops->vidioc_g_std)
+	if (is_valid_ioctl(vfd, VIDIOC_G_STD) && ops->vidioc_g_std)
 		ret = ops->vidioc_g_std(file, fh, &std);
 	if (ret == 0)
 		v4l2_video_std_frame_period(std,
@@ -1792,10 +1793,23 @@ static int v4l_dbg_g_register(const struct v4l2_ioctl_ops *ops,
 {
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 	struct v4l2_dbg_register *p = arg;
+	struct video_device *vfd = video_devdata(file);
+	struct v4l2_subdev *sd;
+	int idx = 0;
 
 	if (!capable(CAP_SYS_ADMIN))
 		return -EPERM;
-	return ops->vidioc_g_register(file, fh, p);
+	if (p->match.type == V4L2_CHIP_MATCH_SUBDEV) {
+		if (vfd->v4l2_dev == NULL)
+			return -EINVAL;
+		v4l2_device_for_each_subdev(sd, vfd->v4l2_dev)
+			if (p->match.addr == idx++)
+				return v4l2_subdev_call(sd, core, g_register, p);
+		return -EINVAL;
+	}
+	if (ops->vidioc_g_register)
+		return ops->vidioc_g_register(file, fh, p);
+	return -EINVAL;
 #else
 	return -ENOTTY;
 #endif
@@ -1805,11 +1819,24 @@ static int v4l_dbg_s_register(const struct v4l2_ioctl_ops *ops,
 				struct file *file, void *fh, void *arg)
 {
 #ifdef CONFIG_VIDEO_ADV_DEBUG
-	struct v4l2_dbg_register *p = arg;
+	const struct v4l2_dbg_register *p = arg;
+	struct video_device *vfd = video_devdata(file);
+	struct v4l2_subdev *sd;
+	int idx = 0;
 
 	if (!capable(CAP_SYS_ADMIN))
 		return -EPERM;
-	return ops->vidioc_s_register(file, fh, p);
+	if (p->match.type == V4L2_CHIP_MATCH_SUBDEV) {
+		if (vfd->v4l2_dev == NULL)
+			return -EINVAL;
+		v4l2_device_for_each_subdev(sd, vfd->v4l2_dev)
+			if (p->match.addr == idx++)
+				return v4l2_subdev_call(sd, core, s_register, p);
+		return -EINVAL;
+	}
+	if (ops->vidioc_s_register)
+		return ops->vidioc_s_register(file, fh, p);
+	return -EINVAL;
 #else
 	return -ENOTTY;
 #endif
@@ -1822,9 +1849,59 @@ static int v4l_dbg_g_chip_ident(const struct v4l2_ioctl_ops *ops,
 
 	p->ident = V4L2_IDENT_NONE;
 	p->revision = 0;
+	if (p->match.type == V4L2_CHIP_MATCH_SUBDEV)
+		return -EINVAL;
 	return ops->vidioc_g_chip_ident(file, fh, p);
 }
 
+static int v4l_dbg_g_chip_info(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	struct video_device *vfd = video_devdata(file);
+	struct v4l2_dbg_chip_info *p = arg;
+	struct v4l2_subdev *sd;
+	int idx = 0;
+
+	switch (p->match.type) {
+	case V4L2_CHIP_MATCH_BRIDGE:
+		if (ops->vidioc_s_register)
+			p->flags |= V4L2_CHIP_FL_WRITABLE;
+		if (ops->vidioc_g_register)
+			p->flags |= V4L2_CHIP_FL_READABLE;
+		if (vfd->v4l2_dev)
+			strlcpy(p->name, vfd->v4l2_dev->name, sizeof(p->name));
+		else if (vfd->parent)
+			strlcpy(p->name, vfd->parent->driver->name, sizeof(p->name));
+		else
+			strlcpy(p->name, "bridge", sizeof(p->name));
+		if (ops->vidioc_g_chip_info)
+			return ops->vidioc_g_chip_info(file, fh, arg);
+		if (p->match.addr)
+			return -EINVAL;
+		return 0;
+
+	case V4L2_CHIP_MATCH_SUBDEV:
+		if (vfd->v4l2_dev == NULL)
+			break;
+		v4l2_device_for_each_subdev(sd, vfd->v4l2_dev) {
+			if (p->match.addr != idx++)
+				continue;
+			if (sd->ops->core && sd->ops->core->s_register)
+				p->flags |= V4L2_CHIP_FL_WRITABLE;
+			if (sd->ops->core && sd->ops->core->g_register)
+				p->flags |= V4L2_CHIP_FL_READABLE;
+			strlcpy(p->name, sd->name, sizeof(p->name));
+			return 0;
+		}
+		break;
+	}
+	return -EINVAL;
+#else
+	return -ENOTTY;
+#endif
+}
+
 static int v4l_dqevent(const struct v4l2_ioctl_ops *ops,
 				struct file *file, void *fh, void *arg)
 {
@@ -1873,7 +1950,7 @@ static int v4l_enum_freq_bands(const struct v4l2_ioctl_ops *ops,
 		return -EINVAL;
 	if (ops->vidioc_enum_freq_bands)
 		return ops->vidioc_enum_freq_bands(file, fh, p);
-	if (ops->vidioc_g_tuner) {
+	if (is_valid_ioctl(vfd, VIDIOC_G_TUNER)) {
 		struct v4l2_tuner t = {
 			.index = p->tuner,
 			.type = type,
@@ -1891,7 +1968,7 @@ static int v4l_enum_freq_bands(const struct v4l2_ioctl_ops *ops,
 			V4L2_BAND_MODULATION_FM : V4L2_BAND_MODULATION_VSB;
 		return 0;
 	}
-	if (ops->vidioc_g_modulator) {
+	if (is_valid_ioctl(vfd, VIDIOC_G_MODULATOR)) {
 		struct v4l2_modulator m = {
 			.index = p->tuner,
 		};
@@ -2028,10 +2105,6 @@ static struct v4l2_ioctl_info v4l2_ioctls[] = {
 	IOCTL_INFO_FNC(VIDIOC_DBG_G_REGISTER, v4l_dbg_g_register, v4l_print_dbg_register, 0),
 	IOCTL_INFO_FNC(VIDIOC_DBG_G_CHIP_IDENT, v4l_dbg_g_chip_ident, v4l_print_dbg_chip_ident, 0),
 	IOCTL_INFO_FNC(VIDIOC_S_HW_FREQ_SEEK, v4l_s_hw_freq_seek, v4l_print_hw_freq_seek, INFO_FL_PRIO),
-	IOCTL_INFO_STD(VIDIOC_ENUM_DV_PRESETS, vidioc_enum_dv_presets, v4l_print_dv_enum_presets, 0),
-	IOCTL_INFO_STD(VIDIOC_S_DV_PRESET, vidioc_s_dv_preset, v4l_print_dv_preset, INFO_FL_PRIO),
-	IOCTL_INFO_STD(VIDIOC_G_DV_PRESET, vidioc_g_dv_preset, v4l_print_dv_preset, 0),
-	IOCTL_INFO_STD(VIDIOC_QUERY_DV_PRESET, vidioc_query_dv_preset, v4l_print_dv_preset, 0),
 	IOCTL_INFO_STD(VIDIOC_S_DV_TIMINGS, vidioc_s_dv_timings, v4l_print_dv_timings, INFO_FL_PRIO),
 	IOCTL_INFO_STD(VIDIOC_G_DV_TIMINGS, vidioc_g_dv_timings, v4l_print_dv_timings, 0),
 	IOCTL_INFO_FNC(VIDIOC_DQEVENT, v4l_dqevent, v4l_print_event, 0),
@@ -2043,6 +2116,7 @@ static struct v4l2_ioctl_info v4l2_ioctls[] = {
 	IOCTL_INFO_STD(VIDIOC_QUERY_DV_TIMINGS, vidioc_query_dv_timings, v4l_print_dv_timings, 0),
 	IOCTL_INFO_STD(VIDIOC_DV_TIMINGS_CAP, vidioc_dv_timings_cap, v4l_print_dv_timings_cap, INFO_FL_CLEAR(v4l2_dv_timings_cap, type)),
 	IOCTL_INFO_FNC(VIDIOC_ENUM_FREQ_BANDS, v4l_enum_freq_bands, v4l_print_freq_band, 0),
+	IOCTL_INFO_FNC(VIDIOC_DBG_G_CHIP_INFO, v4l_dbg_g_chip_info, v4l_print_dbg_chip_info, INFO_FL_CLEAR(v4l2_dbg_chip_info, match)),
 };
 #define V4L2_IOCTLS ARRAY_SIZE(v4l2_ioctls)
 
@@ -2147,11 +2221,6 @@ static long __video_do_ioctl(struct file *file,
 	}
 
 	write_only = _IOC_DIR(cmd) == _IOC_WRITE;
-	if (write_only && debug > V4L2_DEBUG_IOCTL) {
-		v4l_printk_ioctl(video_device_node_name(vfd), cmd);
-		pr_cont(": ");
-		info->debug(arg, write_only);
-	}
 	if (info->flags & INFO_FL_STD) {
 		typedef int (*vidioc_op)(struct file *file, void *fh, void *p);
 		const void *p = vfd->ioctl_ops;
@@ -2170,16 +2239,10 @@ static long __video_do_ioctl(struct file *file,
 
 done:
 	if (debug) {
-		if (write_only && debug > V4L2_DEBUG_IOCTL) {
-			if (ret < 0)
-				printk(KERN_DEBUG "%s: error %ld\n",
-					video_device_node_name(vfd), ret);
-			return ret;
-		}
 		v4l_printk_ioctl(video_device_node_name(vfd), cmd);
 		if (ret < 0)
-			pr_cont(": error %ld\n", ret);
-		else if (debug == V4L2_DEBUG_IOCTL)
+			pr_cont(": error %ld", ret);
+		if (debug == V4L2_DEBUG_IOCTL)
 			pr_cont("\n");
 		else if (_IOC_DIR(cmd) == _IOC_NONE)
 			info->debug(arg, write_only);
diff --git a/drivers/media/v4l2-core/v4l2-mem2mem.c b/drivers/media/v4l2-core/v4l2-mem2mem.c
index da99cf72716..66f599fcb82 100644
--- a/drivers/media/v4l2-core/v4l2-mem2mem.c
+++ b/drivers/media/v4l2-core/v4l2-mem2mem.c
@@ -230,12 +230,15 @@ static void v4l2_m2m_try_schedule(struct v4l2_m2m_ctx *m2m_ctx)
 		dprintk("No input buffers available\n");
 		return;
 	}
+	spin_lock_irqsave(&m2m_ctx->cap_q_ctx.rdy_spinlock, flags);
 	if (list_empty(&m2m_ctx->cap_q_ctx.rdy_queue)) {
+		spin_unlock_irqrestore(&m2m_ctx->cap_q_ctx.rdy_spinlock, flags);
 		spin_unlock_irqrestore(&m2m_ctx->out_q_ctx.rdy_spinlock, flags);
 		spin_unlock_irqrestore(&m2m_dev->job_spinlock, flags_job);
 		dprintk("No output buffers available\n");
 		return;
 	}
+	spin_unlock_irqrestore(&m2m_ctx->cap_q_ctx.rdy_spinlock, flags);
 	spin_unlock_irqrestore(&m2m_ctx->out_q_ctx.rdy_spinlock, flags);
 
 	if (m2m_dev->m2m_ops->job_ready
@@ -405,10 +408,35 @@ EXPORT_SYMBOL_GPL(v4l2_m2m_streamon);
 int v4l2_m2m_streamoff(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 		       enum v4l2_buf_type type)
 {
-	struct vb2_queue *vq;
+	struct v4l2_m2m_dev *m2m_dev;
+	struct v4l2_m2m_queue_ctx *q_ctx;
+	unsigned long flags_job, flags;
+	int ret;
 
-	vq = v4l2_m2m_get_vq(m2m_ctx, type);
-	return vb2_streamoff(vq, type);
+	q_ctx = get_queue_ctx(m2m_ctx, type);
+	ret = vb2_streamoff(&q_ctx->q, type);
+	if (ret)
+		return ret;
+
+	m2m_dev = m2m_ctx->m2m_dev;
+	spin_lock_irqsave(&m2m_dev->job_spinlock, flags_job);
+	/* We should not be scheduled anymore, since we're dropping a queue. */
+	INIT_LIST_HEAD(&m2m_ctx->queue);
+	m2m_ctx->job_flags = 0;
+
+	spin_lock_irqsave(&q_ctx->rdy_spinlock, flags);
+	/* Drop queue, since streamoff returns device to the same state as after
+	 * calling reqbufs. */
+	INIT_LIST_HEAD(&q_ctx->rdy_queue);
+	spin_unlock_irqrestore(&q_ctx->rdy_spinlock, flags);
+
+	if (m2m_dev->curr_ctx == m2m_ctx) {
+		m2m_dev->curr_ctx = NULL;
+		wake_up(&m2m_ctx->finished);
+	}
+	spin_unlock_irqrestore(&m2m_dev->job_spinlock, flags_job);
+
+	return 0;
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_streamoff);
 
diff --git a/drivers/media/v4l2-core/v4l2-of.c b/drivers/media/v4l2-core/v4l2-of.c
new file mode 100644
index 00000000000..aa59639d013
--- /dev/null
+++ b/drivers/media/v4l2-core/v4l2-of.c
@@ -0,0 +1,266 @@
+/*
+ * V4L2 OF binding parsing library
+ *
+ * Copyright (C) 2012 - 2013 Samsung Electronics Co., Ltd.
+ * Author: Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * Copyright (C) 2012 Renesas Electronics Corp.
+ * Author: Guennadi Liakhovetski <g.liakhovetski@gmx.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/string.h>
+#include <linux/types.h>
+
+#include <media/v4l2-of.h>
+
+static void v4l2_of_parse_csi_bus(const struct device_node *node,
+				  struct v4l2_of_endpoint *endpoint)
+{
+	struct v4l2_of_bus_mipi_csi2 *bus = &endpoint->bus.mipi_csi2;
+	u32 data_lanes[ARRAY_SIZE(bus->data_lanes)];
+	struct property *prop;
+	bool have_clk_lane = false;
+	unsigned int flags = 0;
+	u32 v;
+
+	prop = of_find_property(node, "data-lanes", NULL);
+	if (prop) {
+		const __be32 *lane = NULL;
+		int i;
+
+		for (i = 0; i < ARRAY_SIZE(data_lanes); i++) {
+			lane = of_prop_next_u32(prop, lane, &data_lanes[i]);
+			if (!lane)
+				break;
+		}
+		bus->num_data_lanes = i;
+		while (i--)
+			bus->data_lanes[i] = data_lanes[i];
+	}
+
+	if (!of_property_read_u32(node, "clock-lanes", &v)) {
+		bus->clock_lane = v;
+		have_clk_lane = true;
+	}
+
+	if (of_get_property(node, "clock-noncontinuous", &v))
+		flags |= V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK;
+	else if (have_clk_lane || bus->num_data_lanes > 0)
+		flags |= V4L2_MBUS_CSI2_CONTINUOUS_CLOCK;
+
+	bus->flags = flags;
+	endpoint->bus_type = V4L2_MBUS_CSI2;
+}
+
+static void v4l2_of_parse_parallel_bus(const struct device_node *node,
+				       struct v4l2_of_endpoint *endpoint)
+{
+	struct v4l2_of_bus_parallel *bus = &endpoint->bus.parallel;
+	unsigned int flags = 0;
+	u32 v;
+
+	if (!of_property_read_u32(node, "hsync-active", &v))
+		flags |= v ? V4L2_MBUS_HSYNC_ACTIVE_HIGH :
+			V4L2_MBUS_HSYNC_ACTIVE_LOW;
+
+	if (!of_property_read_u32(node, "vsync-active", &v))
+		flags |= v ? V4L2_MBUS_VSYNC_ACTIVE_HIGH :
+			V4L2_MBUS_VSYNC_ACTIVE_LOW;
+
+	if (!of_property_read_u32(node, "pclk-sample", &v))
+		flags |= v ? V4L2_MBUS_PCLK_SAMPLE_RISING :
+			V4L2_MBUS_PCLK_SAMPLE_FALLING;
+
+	if (!of_property_read_u32(node, "field-even-active", &v))
+		flags |= v ? V4L2_MBUS_FIELD_EVEN_HIGH :
+			V4L2_MBUS_FIELD_EVEN_LOW;
+	if (flags)
+		endpoint->bus_type = V4L2_MBUS_PARALLEL;
+	else
+		endpoint->bus_type = V4L2_MBUS_BT656;
+
+	if (!of_property_read_u32(node, "data-active", &v))
+		flags |= v ? V4L2_MBUS_DATA_ACTIVE_HIGH :
+			V4L2_MBUS_DATA_ACTIVE_LOW;
+
+	if (of_get_property(node, "slave-mode", &v))
+		flags |= V4L2_MBUS_SLAVE;
+	else
+		flags |= V4L2_MBUS_MASTER;
+
+	if (!of_property_read_u32(node, "bus-width", &v))
+		bus->bus_width = v;
+
+	if (!of_property_read_u32(node, "data-shift", &v))
+		bus->data_shift = v;
+
+	bus->flags = flags;
+
+}
+
+/**
+ * v4l2_of_parse_endpoint() - parse all endpoint node properties
+ * @node: pointer to endpoint device_node
+ * @endpoint: pointer to the V4L2 OF endpoint data structure
+ *
+ * All properties are optional. If none are found, we don't set any flags.
+ * This means the port has a static configuration and no properties have
+ * to be specified explicitly.
+ * If any properties that identify the bus as parallel are found and
+ * slave-mode isn't set, we set V4L2_MBUS_MASTER. Similarly, if we recognise
+ * the bus as serial CSI-2 and clock-noncontinuous isn't set, we set the
+ * V4L2_MBUS_CSI2_CONTINUOUS_CLOCK flag.
+ * The caller should hold a reference to @node.
+ */
+void v4l2_of_parse_endpoint(const struct device_node *node,
+			    struct v4l2_of_endpoint *endpoint)
+{
+	struct device_node *port_node = of_get_parent(node);
+
+	memset(endpoint, 0, offsetof(struct v4l2_of_endpoint, head));
+
+	endpoint->local_node = node;
+	/*
+	 * It doesn't matter whether the two calls below succeed.
+	 * If they don't then the default value 0 is used.
+	 */
+	of_property_read_u32(port_node, "reg", &endpoint->port);
+	of_property_read_u32(node, "reg", &endpoint->id);
+
+	v4l2_of_parse_csi_bus(node, endpoint);
+	/*
+	 * Parse the parallel video bus properties only if none
+	 * of the MIPI CSI-2 specific properties were found.
+	 */
+	if (endpoint->bus.mipi_csi2.flags == 0)
+		v4l2_of_parse_parallel_bus(node, endpoint);
+
+	of_node_put(port_node);
+}
+EXPORT_SYMBOL(v4l2_of_parse_endpoint);
+
+/**
+ * v4l2_of_get_next_endpoint() - get next endpoint node
+ * @parent: pointer to the parent device node
+ * @prev: previous endpoint node, or NULL to get first
+ *
+ * Return: An 'endpoint' node pointer with refcount incremented. Refcount
+ * of the passed @prev node is not decremented, the caller have to use
+ * of_node_put() on it when done.
+ */
+struct device_node *v4l2_of_get_next_endpoint(const struct device_node *parent,
+					struct device_node *prev)
+{
+	struct device_node *endpoint;
+	struct device_node *port = NULL;
+
+	if (!parent)
+		return NULL;
+
+	if (!prev) {
+		struct device_node *node;
+		/*
+		 * It's the first call, we have to find a port subnode
+		 * within this node or within an optional 'ports' node.
+		 */
+		node = of_get_child_by_name(parent, "ports");
+		if (node)
+			parent = node;
+
+		for_each_child_of_node(parent, node) {
+			if (!of_node_cmp(node->name, "port")) {
+				port = node;
+				break;
+			}
+		}
+		if (port) {
+			/* Found a port, get an endpoint. */
+			endpoint = of_get_next_child(port, NULL);
+			of_node_put(port);
+		} else {
+			endpoint = NULL;
+		}
+
+		if (!endpoint)
+			pr_err("%s(): no endpoint nodes specified for %s\n",
+			       __func__, parent->full_name);
+	} else {
+		port = of_get_parent(prev);
+		if (!port)
+			/* Hm, has someone given us the root node ?... */
+			return NULL;
+
+		/* Avoid dropping prev node refcount to 0. */
+		of_node_get(prev);
+		endpoint = of_get_next_child(port, prev);
+		if (endpoint) {
+			of_node_put(port);
+			return endpoint;
+		}
+
+		/* No more endpoints under this port, try the next one. */
+		do {
+			port = of_get_next_child(parent, port);
+			if (!port)
+				return NULL;
+		} while (of_node_cmp(port->name, "port"));
+
+		/* Pick up the first endpoint in this port. */
+		endpoint = of_get_next_child(port, NULL);
+		of_node_put(port);
+	}
+
+	return endpoint;
+}
+EXPORT_SYMBOL(v4l2_of_get_next_endpoint);
+
+/**
+ * v4l2_of_get_remote_port_parent() - get remote port's parent node
+ * @node: pointer to a local endpoint device_node
+ *
+ * Return: Remote device node associated with remote endpoint node linked
+ *	   to @node. Use of_node_put() on it when done.
+ */
+struct device_node *v4l2_of_get_remote_port_parent(
+			       const struct device_node *node)
+{
+	struct device_node *np;
+	unsigned int depth;
+
+	/* Get remote endpoint node. */
+	np = of_parse_phandle(node, "remote-endpoint", 0);
+
+	/* Walk 3 levels up only if there is 'ports' node. */
+	for (depth = 3; depth && np; depth--) {
+		np = of_get_next_parent(np);
+		if (depth == 2 && of_node_cmp(np->name, "ports"))
+			break;
+	}
+	return np;
+}
+EXPORT_SYMBOL(v4l2_of_get_remote_port_parent);
+
+/**
+ * v4l2_of_get_remote_port() - get remote port node
+ * @node: pointer to a local endpoint device_node
+ *
+ * Return: Remote port node associated with remote endpoint node linked
+ *	   to @node. Use of_node_put() on it when done.
+ */
+struct device_node *v4l2_of_get_remote_port(const struct device_node *node)
+{
+	struct device_node *np;
+
+	/* Get remote endpoint node. */
+	np = of_parse_phandle(node, "remote-endpoint", 0);
+	if (!np)
+		return NULL;
+	return of_get_parent(np);
+}
+EXPORT_SYMBOL(v4l2_of_get_remote_port);
diff --git a/drivers/media/v4l2-core/videobuf-dma-contig.c b/drivers/media/v4l2-core/videobuf-dma-contig.c
index 3a43ba0959b..67f572c3fba 100644
--- a/drivers/media/v4l2-core/videobuf-dma-contig.c
+++ b/drivers/media/v4l2-core/videobuf-dma-contig.c
@@ -27,7 +27,6 @@ struct videobuf_dma_contig_memory {
 	u32 magic;
 	void *vaddr;
 	dma_addr_t dma_handle;
-	bool cached;
 	unsigned long size;
 };
 
@@ -43,26 +42,8 @@ static int __videobuf_dc_alloc(struct device *dev,
 			       unsigned long size, gfp_t 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 = NULL;
-				return err;
-			}
-		}
-	} else
-		mem->vaddr = dma_alloc_coherent(dev, mem->size,
-						&mem->dma_handle, flags);
+	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);
@@ -77,14 +58,7 @@ static int __videobuf_dc_alloc(struct device *dev,
 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);
+	dma_free_coherent(dev, mem->size, mem->vaddr, mem->dma_handle);
 
 	mem->vaddr = NULL;
 }
@@ -234,7 +208,7 @@ out_up:
 	return ret;
 }
 
-static struct videobuf_buffer *__videobuf_alloc_vb(size_t size, bool cached)
+static struct videobuf_buffer *__videobuf_alloc(size_t size)
 {
 	struct videobuf_dma_contig_memory *mem;
 	struct videobuf_buffer *vb;
@@ -244,22 +218,11 @@ static struct videobuf_buffer *__videobuf_alloc_vb(size_t size, bool cached)
 		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;
@@ -310,19 +273,6 @@ static int __videobuf_iolock(struct videobuf_queue *q,
 	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)
@@ -331,8 +281,6 @@ 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__);
 
@@ -359,43 +307,16 @@ static int __videobuf_mmap_mapper(struct videobuf_queue *q,
 	size = vma->vm_end - vma->vm_start;
 	size = (size < mem->size) ? size : mem->size;
 
-	if (!mem->cached) {
-		vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
-		retval = remap_pfn_range(vma, vma->vm_start,
-			 mem->dma_handle >> PAGE_SHIFT,
+	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;
-		}
-	} else {
-		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;
-		}
+	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;
 	}
 
 	vma->vm_ops = &videobuf_vm_ops;
@@ -417,17 +338,8 @@ error:
 
 static struct videobuf_qtype_ops qops = {
 	.magic		= MAGIC_QTYPE_OPS,
-	.alloc_vb	= __videobuf_alloc_uncached,
-	.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,
+	.alloc_vb	= __videobuf_alloc,
 	.iolock		= __videobuf_iolock,
-	.sync		= __videobuf_sync,
 	.mmap_mapper	= __videobuf_mmap_mapper,
 	.vaddr		= __videobuf_to_vaddr,
 };
@@ -447,20 +359,6 @@ 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;
diff --git a/drivers/media/v4l2-core/videobuf2-core.c b/drivers/media/v4l2-core/videobuf2-core.c
index db1235dcb32..7d833eefaf4 100644
--- a/drivers/media/v4l2-core/videobuf2-core.c
+++ b/drivers/media/v4l2-core/videobuf2-core.c
@@ -54,10 +54,15 @@ static int __vb2_buf_mem_alloc(struct vb2_buffer *vb)
 	void *mem_priv;
 	int plane;
 
-	/* Allocate memory for all planes in this buffer */
+	/*
+	 * Allocate memory for all planes in this buffer
+	 * NOTE: mmapped areas should be page aligned
+	 */
 	for (plane = 0; plane < vb->num_planes; ++plane) {
+		unsigned long size = PAGE_ALIGN(q->plane_sizes[plane]);
+
 		mem_priv = call_memop(q, alloc, q->alloc_ctx[plane],
-				      q->plane_sizes[plane]);
+				      size, q->gfp_flags);
 		if (IS_ERR_OR_NULL(mem_priv))
 			goto free;
 
@@ -403,7 +408,7 @@ static void __fill_v4l2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b)
 	 * Clear any buffer state related flags.
 	 */
 	b->flags &= ~V4L2_BUFFER_MASK_FLAGS;
-	b->flags |= V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	b->flags |= q->timestamp_type;
 
 	switch (vb->state) {
 	case VB2_BUF_STATE_QUEUED:
@@ -855,7 +860,7 @@ void vb2_buffer_done(struct vb2_buffer *vb, enum vb2_buffer_state state)
 		return;
 
 	dprintk(4, "Done processing on buffer %d, state: %d\n",
-			vb->v4l2_buf.index, vb->state);
+			vb->v4l2_buf.index, state);
 
 	/* sync buffers */
 	for (plane = 0; plane < vb->num_planes; ++plane)
@@ -1852,6 +1857,7 @@ int vb2_mmap(struct vb2_queue *q, struct vm_area_struct *vma)
 	struct vb2_buffer *vb;
 	unsigned int buffer, plane;
 	int ret;
+	unsigned long length;
 
 	if (q->memory != V4L2_MEMORY_MMAP) {
 		dprintk(1, "Queue is not currently set up for mmap\n");
@@ -1886,6 +1892,18 @@ int vb2_mmap(struct vb2_queue *q, struct vm_area_struct *vma)
 
 	vb = q->bufs[buffer];
 
+	/*
+	 * MMAP requires page_aligned buffers.
+	 * The buffer length was page_aligned at __vb2_buf_mem_alloc(),
+	 * so, we need to do the same here.
+	 */
+	length = PAGE_ALIGN(vb->v4l2_planes[plane].length);
+	if (length < (vma->vm_end - vma->vm_start)) {
+		dprintk(1,
+			"MMAP invalid, as it would overflow buffer length\n");
+		return -EINVAL;
+	}
+
 	ret = call_memop(q, mmap, vb->planes[plane].mem_priv, vma);
 	if (ret)
 		return ret;
@@ -2039,9 +2057,13 @@ int vb2_queue_init(struct vb2_queue *q)
 	    WARN_ON(!q->type)		  ||
 	    WARN_ON(!q->io_modes)	  ||
 	    WARN_ON(!q->ops->queue_setup) ||
-	    WARN_ON(!q->ops->buf_queue))
+	    WARN_ON(!q->ops->buf_queue)   ||
+	    WARN_ON(q->timestamp_type & ~V4L2_BUF_FLAG_TIMESTAMP_MASK))
 		return -EINVAL;
 
+	/* Warn that the driver should choose an appropriate timestamp type */
+	WARN_ON(q->timestamp_type == V4L2_BUF_FLAG_TIMESTAMP_UNKNOWN);
+
 	INIT_LIST_HEAD(&q->queued_list);
 	INIT_LIST_HEAD(&q->done_list);
 	spin_lock_init(&q->done_lock);
diff --git a/drivers/media/v4l2-core/videobuf2-dma-contig.c b/drivers/media/v4l2-core/videobuf2-dma-contig.c
index 10beaee7f0a..fd56f256320 100644
--- a/drivers/media/v4l2-core/videobuf2-dma-contig.c
+++ b/drivers/media/v4l2-core/videobuf2-dma-contig.c
@@ -152,7 +152,7 @@ static void vb2_dc_put(void *buf_priv)
 	kfree(buf);
 }
 
-static void *vb2_dc_alloc(void *alloc_ctx, unsigned long size)
+static void *vb2_dc_alloc(void *alloc_ctx, unsigned long size, gfp_t gfp_flags)
 {
 	struct vb2_dc_conf *conf = alloc_ctx;
 	struct device *dev = conf->dev;
@@ -162,10 +162,8 @@ static void *vb2_dc_alloc(void *alloc_ctx, unsigned long size)
 	if (!buf)
 		return ERR_PTR(-ENOMEM);
 
-	/* align image size to PAGE_SIZE */
-	size = PAGE_ALIGN(size);
-
-	buf->vaddr = dma_alloc_coherent(dev, size, &buf->dma_addr, GFP_KERNEL);
+	buf->vaddr = dma_alloc_coherent(dev, size, &buf->dma_addr,
+						GFP_KERNEL | gfp_flags);
 	if (!buf->vaddr) {
 		dev_err(dev, "dma_alloc_coherent of size %ld failed\n", size);
 		kfree(buf);
diff --git a/drivers/media/v4l2-core/videobuf2-dma-sg.c b/drivers/media/v4l2-core/videobuf2-dma-sg.c
index 25c3b360e1a..16ae3dcc7e2 100644
--- a/drivers/media/v4l2-core/videobuf2-dma-sg.c
+++ b/drivers/media/v4l2-core/videobuf2-dma-sg.c
@@ -21,6 +21,15 @@
 #include <media/videobuf2-memops.h>
 #include <media/videobuf2-dma-sg.h>
 
+static int debug;
+module_param(debug, int, 0644);
+
+#define dprintk(level, fmt, arg...)					\
+	do {								\
+		if (debug >= level)					\
+			printk(KERN_DEBUG "vb2-dma-sg: " fmt, ## arg);	\
+	} while (0)
+
 struct vb2_dma_sg_buf {
 	void				*vaddr;
 	struct page			**pages;
@@ -33,7 +42,7 @@ struct vb2_dma_sg_buf {
 
 static void vb2_dma_sg_put(void *buf_priv);
 
-static void *vb2_dma_sg_alloc(void *alloc_ctx, unsigned long size)
+static void *vb2_dma_sg_alloc(void *alloc_ctx, unsigned long size, gfp_t gfp_flags)
 {
 	struct vb2_dma_sg_buf *buf;
 	int i;
@@ -46,7 +55,8 @@ static void *vb2_dma_sg_alloc(void *alloc_ctx, unsigned long size)
 	buf->write = 0;
 	buf->offset = 0;
 	buf->sg_desc.size = size;
-	buf->sg_desc.num_pages = (size + PAGE_SIZE - 1) >> PAGE_SHIFT;
+	/* size is already page aligned */
+	buf->sg_desc.num_pages = size >> PAGE_SHIFT;
 
 	buf->sg_desc.sglist = vzalloc(buf->sg_desc.num_pages *
 				      sizeof(*buf->sg_desc.sglist));
@@ -60,7 +70,8 @@ static void *vb2_dma_sg_alloc(void *alloc_ctx, unsigned long size)
 		goto fail_pages_array_alloc;
 
 	for (i = 0; i < buf->sg_desc.num_pages; ++i) {
-		buf->pages[i] = alloc_page(GFP_KERNEL | __GFP_ZERO | __GFP_NOWARN);
+		buf->pages[i] = alloc_page(GFP_KERNEL | __GFP_ZERO |
+					   __GFP_NOWARN | gfp_flags);
 		if (NULL == buf->pages[i])
 			goto fail_pages_alloc;
 		sg_set_page(&buf->sg_desc.sglist[i],
@@ -73,7 +84,7 @@ static void *vb2_dma_sg_alloc(void *alloc_ctx, unsigned long size)
 
 	atomic_inc(&buf->refcount);
 
-	printk(KERN_DEBUG "%s: Allocated buffer of %d pages\n",
+	dprintk(1, "%s: Allocated buffer of %d pages\n",
 		__func__, buf->sg_desc.num_pages);
 	return buf;
 
@@ -96,7 +107,7 @@ static void vb2_dma_sg_put(void *buf_priv)
 	int i = buf->sg_desc.num_pages;
 
 	if (atomic_dec_and_test(&buf->refcount)) {
-		printk(KERN_DEBUG "%s: Freeing buffer of %d pages\n", __func__,
+		dprintk(1, "%s: Freeing buffer of %d pages\n", __func__,
 			buf->sg_desc.num_pages);
 		if (buf->vaddr)
 			vm_unmap_ram(buf->vaddr, buf->sg_desc.num_pages);
@@ -162,7 +173,7 @@ static void *vb2_dma_sg_get_userptr(void *alloc_ctx, unsigned long vaddr,
 	return buf;
 
 userptr_fail_get_user_pages:
-	printk(KERN_DEBUG "get_user_pages requested/got: %d/%d]\n",
+	dprintk(1, "get_user_pages requested/got: %d/%d]\n",
 	       num_pages_from_user, buf->sg_desc.num_pages);
 	while (--num_pages_from_user >= 0)
 		put_page(buf->pages[num_pages_from_user]);
@@ -185,7 +196,7 @@ static void vb2_dma_sg_put_userptr(void *buf_priv)
 	struct vb2_dma_sg_buf *buf = buf_priv;
 	int i = buf->sg_desc.num_pages;
 
-	printk(KERN_DEBUG "%s: Releasing userspace buffer of %d pages\n",
+	dprintk(1, "%s: Releasing userspace buffer of %d pages\n",
 	       __func__, buf->sg_desc.num_pages);
 	if (buf->vaddr)
 		vm_unmap_ram(buf->vaddr, buf->sg_desc.num_pages);
diff --git a/drivers/media/v4l2-core/videobuf2-vmalloc.c b/drivers/media/v4l2-core/videobuf2-vmalloc.c
index a47fd4f589a..313d9771b2b 100644
--- a/drivers/media/v4l2-core/videobuf2-vmalloc.c
+++ b/drivers/media/v4l2-core/videobuf2-vmalloc.c
@@ -35,11 +35,11 @@ struct vb2_vmalloc_buf {
 
 static void vb2_vmalloc_put(void *buf_priv);
 
-static void *vb2_vmalloc_alloc(void *alloc_ctx, unsigned long size)
+static void *vb2_vmalloc_alloc(void *alloc_ctx, unsigned long size, gfp_t gfp_flags)
 {
 	struct vb2_vmalloc_buf *buf;
 
-	buf = kzalloc(sizeof(*buf), GFP_KERNEL);
+	buf = kzalloc(sizeof(*buf), GFP_KERNEL | gfp_flags);
 	if (!buf)
 		return NULL;
 
diff --git a/drivers/memory/emif.c b/drivers/memory/emif.c
index df087369485..cadf1cc19aa 100644
--- a/drivers/memory/emif.c
+++ b/drivers/memory/emif.c
@@ -25,6 +25,7 @@
 #include <linux/module.h>
 #include <linux/list.h>
 #include <linux/spinlock.h>
+#include <linux/pm.h>
 #include <memory/jedec_ddr.h>
 #include "emif.h"
 #include "of_memory.h"
@@ -256,6 +257,41 @@ static void set_lpmode(struct emif_data *emif, u8 lpmode)
 	u32 temp;
 	void __iomem *base = emif->base;
 
+	/*
+	 * Workaround for errata i743 - LPDDR2 Power-Down State is Not
+	 * Efficient
+	 *
+	 * i743 DESCRIPTION:
+	 * The EMIF supports power-down state for low power. The EMIF
+	 * automatically puts the SDRAM into power-down after the memory is
+	 * not accessed for a defined number of cycles and the
+	 * EMIF_PWR_MGMT_CTRL[10:8] REG_LP_MODE bit field is set to 0x4.
+	 * As the EMIF supports automatic output impedance calibration, a ZQ
+	 * calibration long command is issued every time it exits active
+	 * power-down and precharge power-down modes. The EMIF waits and
+	 * blocks any other command during this calibration.
+	 * The EMIF does not allow selective disabling of ZQ calibration upon
+	 * exit of power-down mode. Due to very short periods of power-down
+	 * cycles, ZQ calibration overhead creates bandwidth issues and
+	 * increases overall system power consumption. On the other hand,
+	 * issuing ZQ calibration long commands when exiting self-refresh is
+	 * still required.
+	 *
+	 * WORKAROUND
+	 * Because there is no power consumption benefit of the power-down due
+	 * to the calibration and there is a performance risk, the guideline
+	 * is to not allow power-down state and, therefore, to not have set
+	 * the EMIF_PWR_MGMT_CTRL[10:8] REG_LP_MODE bit field to 0x4.
+	 */
+	if ((emif->plat_data->ip_rev == EMIF_4D) &&
+	    (EMIF_LP_MODE_PWR_DN == lpmode)) {
+		WARN_ONCE(1,
+			  "REG_LP_MODE = LP_MODE_PWR_DN(4) is prohibited by"
+			  "erratum i743 switch to LP_MODE_SELF_REFRESH(2)\n");
+		/* rollback LP_MODE to Self-refresh mode */
+		lpmode = EMIF_LP_MODE_SELF_REFRESH;
+	}
+
 	temp = readl(base + EMIF_POWER_MANAGEMENT_CONTROL);
 	temp &= ~LP_MODE_MASK;
 	temp |= (lpmode << LP_MODE_SHIFT);
@@ -715,6 +751,8 @@ static u32 get_pwr_mgmt_ctrl(u32 freq, struct emif_data *emif, u32 ip_rev)
 	u32 timeout_perf	= EMIF_LP_MODE_TIMEOUT_PERFORMANCE;
 	u32 timeout_pwr		= EMIF_LP_MODE_TIMEOUT_POWER;
 	u32 freq_threshold	= EMIF_LP_MODE_FREQ_THRESHOLD;
+	u32 mask;
+	u8 shift;
 
 	struct emif_custom_configs *cust_cfgs = emif->plat_data->custom_configs;
 
@@ -728,37 +766,59 @@ static u32 get_pwr_mgmt_ctrl(u32 freq, struct emif_data *emif, u32 ip_rev)
 	/* Timeout based on DDR frequency */
 	timeout = freq >= freq_threshold ? timeout_perf : timeout_pwr;
 
-	/* The value to be set in register is "log2(timeout) - 3" */
+	/*
+	 * The value to be set in register is "log2(timeout) - 3"
+	 * if timeout < 16 load 0 in register
+	 * if timeout is not a power of 2, round to next highest power of 2
+	 */
 	if (timeout < 16) {
 		timeout = 0;
 	} else {
-		timeout = __fls(timeout) - 3;
 		if (timeout & (timeout - 1))
-			timeout++;
+			timeout <<= 1;
+		timeout = __fls(timeout) - 3;
 	}
 
 	switch (lpmode) {
 	case EMIF_LP_MODE_CLOCK_STOP:
-		pwr_mgmt_ctrl = (timeout << CS_TIM_SHIFT) |
-					SR_TIM_MASK | PD_TIM_MASK;
+		shift = CS_TIM_SHIFT;
+		mask = CS_TIM_MASK;
 		break;
 	case EMIF_LP_MODE_SELF_REFRESH:
 		/* Workaround for errata i735 */
 		if (timeout < 6)
 			timeout = 6;
 
-		pwr_mgmt_ctrl = (timeout << SR_TIM_SHIFT) |
-					CS_TIM_MASK | PD_TIM_MASK;
+		shift = SR_TIM_SHIFT;
+		mask = SR_TIM_MASK;
 		break;
 	case EMIF_LP_MODE_PWR_DN:
-		pwr_mgmt_ctrl = (timeout << PD_TIM_SHIFT) |
-					CS_TIM_MASK | SR_TIM_MASK;
+		shift = PD_TIM_SHIFT;
+		mask = PD_TIM_MASK;
 		break;
 	case EMIF_LP_MODE_DISABLE:
 	default:
-		pwr_mgmt_ctrl = CS_TIM_MASK |
-					PD_TIM_MASK | SR_TIM_MASK;
+		mask = 0;
+		shift = 0;
+		break;
 	}
+	/* Round to maximum in case of overflow, BUT warn! */
+	if (lpmode != EMIF_LP_MODE_DISABLE && timeout > mask >> shift) {
+		pr_err("TIMEOUT Overflow - lpmode=%d perf=%d pwr=%d freq=%d\n",
+		       lpmode,
+		       timeout_perf,
+		       timeout_pwr,
+		       freq_threshold);
+		WARN(1, "timeout=0x%02x greater than 0x%02x. Using max\n",
+		     timeout, mask >> shift);
+		timeout = mask >> shift;
+	}
+
+	/* Setup required timing */
+	pwr_mgmt_ctrl = (timeout << shift) & mask;
+	/* setup a default mask for rest of the modes */
+	pwr_mgmt_ctrl |= (SR_TIM_MASK | CS_TIM_MASK | PD_TIM_MASK) &
+			  ~mask;
 
 	/* No CS_TIM in EMIF_4D5 */
 	if (ip_rev == EMIF_4D5)
@@ -815,6 +875,8 @@ static void setup_registers(struct emif_data *emif, struct emif_regs *regs)
 
 	writel(regs->sdram_tim2_shdw, base + EMIF_SDRAM_TIMING_2_SHDW);
 	writel(regs->phy_ctrl_1_shdw, base + EMIF_DDR_PHY_CTRL_1_SHDW);
+	writel(regs->pwr_mgmt_ctrl_shdw,
+	       base + EMIF_POWER_MANAGEMENT_CTRL_SHDW);
 
 	/* Settings specific for EMIF4D5 */
 	if (emif->plat_data->ip_rev != EMIF_4D5)
@@ -892,6 +954,7 @@ static irqreturn_t handle_temp_alert(void __iomem *base, struct emif_data *emif)
 {
 	u32		old_temp_level;
 	irqreturn_t	ret = IRQ_HANDLED;
+	struct emif_custom_configs *custom_configs;
 
 	spin_lock_irqsave(&emif_lock, irq_state);
 	old_temp_level = emif->temperature_level;
@@ -904,6 +967,29 @@ static irqreturn_t handle_temp_alert(void __iomem *base, struct emif_data *emif)
 		goto out;
 	}
 
+	custom_configs = emif->plat_data->custom_configs;
+
+	/*
+	 * IF we detect higher than "nominal rating" from DDR sensor
+	 * on an unsupported DDR part, shutdown system
+	 */
+	if (custom_configs && !(custom_configs->mask &
+				EMIF_CUSTOM_CONFIG_EXTENDED_TEMP_PART)) {
+		if (emif->temperature_level >= SDRAM_TEMP_HIGH_DERATE_REFRESH) {
+			dev_err(emif->dev,
+				"%s:NOT Extended temperature capable memory."
+				"Converting MR4=0x%02x as shutdown event\n",
+				__func__, emif->temperature_level);
+			/*
+			 * Temperature far too high - do kernel_power_off()
+			 * from thread context
+			 */
+			emif->temperature_level = SDRAM_TEMP_VERY_HIGH_SHUTDOWN;
+			ret = IRQ_WAKE_THREAD;
+			goto out;
+		}
+	}
+
 	if (emif->temperature_level < old_temp_level ||
 		emif->temperature_level == SDRAM_TEMP_VERY_HIGH_SHUTDOWN) {
 		/*
@@ -965,7 +1051,14 @@ static irqreturn_t emif_threaded_isr(int irq, void *dev_id)
 
 	if (emif->temperature_level == SDRAM_TEMP_VERY_HIGH_SHUTDOWN) {
 		dev_emerg(emif->dev, "SDRAM temperature exceeds operating limit.. Needs shut down!!!\n");
-		kernel_power_off();
+
+		/* If we have Power OFF ability, use it, else try restarting */
+		if (pm_power_off) {
+			kernel_power_off();
+		} else {
+			WARN(1, "FIXME: NO pm_power_off!!! trying restart\n");
+			kernel_restart("SDRAM Over-temp Emergency restart");
+		}
 		return IRQ_HANDLED;
 	}
 
@@ -1170,7 +1263,7 @@ static void __init_or_module of_get_custom_configs(struct device_node *np_emif,
 {
 	struct emif_custom_configs	*cust_cfgs = NULL;
 	int				len;
-	const int			*lpmode, *poll_intvl;
+	const __be32			*lpmode, *poll_intvl;
 
 	lpmode = of_get_property(np_emif, "low-power-mode", &len);
 	poll_intvl = of_get_property(np_emif, "temp-alert-poll-interval", &len);
@@ -1184,7 +1277,7 @@ static void __init_or_module of_get_custom_configs(struct device_node *np_emif,
 
 	if (lpmode) {
 		cust_cfgs->mask |= EMIF_CUSTOM_CONFIG_LPMODE;
-		cust_cfgs->lpmode = *lpmode;
+		cust_cfgs->lpmode = be32_to_cpup(lpmode);
 		of_property_read_u32(np_emif,
 				"low-power-mode-timeout-performance",
 				&cust_cfgs->lpmode_timeout_performance);
@@ -1199,9 +1292,13 @@ static void __init_or_module of_get_custom_configs(struct device_node *np_emif,
 	if (poll_intvl) {
 		cust_cfgs->mask |=
 				EMIF_CUSTOM_CONFIG_TEMP_ALERT_POLL_INTERVAL;
-		cust_cfgs->temp_alert_poll_interval_ms = *poll_intvl;
+		cust_cfgs->temp_alert_poll_interval_ms =
+						be32_to_cpup(poll_intvl);
 	}
 
+	if (of_find_property(np_emif, "extended-temp-part", &len))
+		cust_cfgs->mask |= EMIF_CUSTOM_CONFIG_EXTENDED_TEMP_PART;
+
 	if (!is_custom_config_valid(cust_cfgs, emif->dev)) {
 		devm_kfree(emif->dev, cust_cfgs);
 		return;
@@ -1407,7 +1504,7 @@ static struct emif_data *__init_or_module get_device_details(
 	if (pd->timings) {
 		temp = devm_kzalloc(dev, size, GFP_KERNEL);
 		if (temp) {
-			memcpy(temp, pd->timings, sizeof(*pd->timings));
+			memcpy(temp, pd->timings, size);
 			pd->timings = temp;
 		} else {
 			dev_warn(dev, "%s:%d: allocation error\n", __func__,
@@ -1841,18 +1938,8 @@ static struct platform_driver emif_driver = {
 	},
 };
 
-static int __init_or_module emif_register(void)
-{
-	return platform_driver_probe(&emif_driver, emif_probe);
-}
-
-static void __exit emif_unregister(void)
-{
-	platform_driver_unregister(&emif_driver);
-}
+module_platform_driver_probe(emif_driver, emif_probe);
 
-module_init(emif_register);
-module_exit(emif_unregister);
 MODULE_DESCRIPTION("TI EMIF SDRAM Controller Driver");
 MODULE_LICENSE("GPL");
 MODULE_ALIAS("platform:emif");
diff --git a/drivers/memory/tegra30-mc.c b/drivers/memory/tegra30-mc.c
index 0b975986777..f4ae074badc 100644
--- a/drivers/memory/tegra30-mc.c
+++ b/drivers/memory/tegra30-mc.c
@@ -268,6 +268,7 @@ static const u32 tegra30_mc_ctx[] = {
 	MC_INTMASK,
 };
 
+#ifdef CONFIG_PM
 static int tegra30_mc_suspend(struct device *dev)
 {
 	int i;
@@ -291,6 +292,7 @@ static int tegra30_mc_resume(struct device *dev)
 	mc_readl(mc, MC_TIMING_CONTROL);
 	return 0;
 }
+#endif
 
 static UNIVERSAL_DEV_PM_OPS(tegra30_mc_pm,
 			    tegra30_mc_suspend,
diff --git a/drivers/mfd/Kconfig b/drivers/mfd/Kconfig
index c346941a251..ca86581d02c 100644
--- a/drivers/mfd/Kconfig
+++ b/drivers/mfd/Kconfig
@@ -991,7 +991,7 @@ config MFD_PM8XXX
 
 config MFD_PM8921_CORE
 	tristate "Qualcomm PM8921 PMIC chip"
-	depends on MSM_SSBI
+	depends on SSBI && BROKEN
 	select MFD_CORE
 	select MFD_PM8XXX
 	help
diff --git a/drivers/mfd/ab3100-core.c b/drivers/mfd/ab3100-core.c
index 2ec7725f4a0..a9bb140bc86 100644
--- a/drivers/mfd/ab3100-core.c
+++ b/drivers/mfd/ab3100-core.c
@@ -753,6 +753,7 @@ static struct mfd_cell ab3100_devs[] = {
 	},
 	{
 		.name = "ab3100-regulators",
+		.of_compatible = "stericsson,ab3100-regulators",
 		.id = -1,
 	},
 	{
diff --git a/drivers/mfd/pm8921-core.c b/drivers/mfd/pm8921-core.c
index d4b297cbd80..ecc137ffa8c 100644
--- a/drivers/mfd/pm8921-core.c
+++ b/drivers/mfd/pm8921-core.c
@@ -17,7 +17,7 @@
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 #include <linux/err.h>
-#include <linux/msm_ssbi.h>
+#include <linux/ssbi.h>
 #include <linux/mfd/core.h>
 #include <linux/mfd/pm8xxx/pm8921.h>
 #include <linux/mfd/pm8xxx/core.h>
@@ -35,7 +35,7 @@ static int pm8921_readb(const struct device *dev, u16 addr, u8 *val)
 	const struct pm8xxx_drvdata *pm8921_drvdata = dev_get_drvdata(dev);
 	const struct pm8921 *pmic = pm8921_drvdata->pm_chip_data;
 
-	return msm_ssbi_read(pmic->dev->parent, addr, val, 1);
+	return ssbi_read(pmic->dev->parent, addr, val, 1);
 }
 
 static int pm8921_writeb(const struct device *dev, u16 addr, u8 val)
@@ -43,7 +43,7 @@ static int pm8921_writeb(const struct device *dev, u16 addr, u8 val)
 	const struct pm8xxx_drvdata *pm8921_drvdata = dev_get_drvdata(dev);
 	const struct pm8921 *pmic = pm8921_drvdata->pm_chip_data;
 
-	return msm_ssbi_write(pmic->dev->parent, addr, &val, 1);
+	return ssbi_write(pmic->dev->parent, addr, &val, 1);
 }
 
 static int pm8921_read_buf(const struct device *dev, u16 addr, u8 *buf,
@@ -52,7 +52,7 @@ static int pm8921_read_buf(const struct device *dev, u16 addr, u8 *buf,
 	const struct pm8xxx_drvdata *pm8921_drvdata = dev_get_drvdata(dev);
 	const struct pm8921 *pmic = pm8921_drvdata->pm_chip_data;
 
-	return msm_ssbi_read(pmic->dev->parent, addr, buf, cnt);
+	return ssbi_read(pmic->dev->parent, addr, buf, cnt);
 }
 
 static int pm8921_write_buf(const struct device *dev, u16 addr, u8 *buf,
@@ -61,7 +61,7 @@ static int pm8921_write_buf(const struct device *dev, u16 addr, u8 *buf,
 	const struct pm8xxx_drvdata *pm8921_drvdata = dev_get_drvdata(dev);
 	const struct pm8921 *pmic = pm8921_drvdata->pm_chip_data;
 
-	return msm_ssbi_write(pmic->dev->parent, addr, buf, cnt);
+	return ssbi_write(pmic->dev->parent, addr, buf, cnt);
 }
 
 static int pm8921_read_irq_stat(const struct device *dev, int irq)
@@ -124,7 +124,7 @@ static int pm8921_probe(struct platform_device *pdev)
 	}
 
 	/* Read PMIC chip revision */
-	rc = msm_ssbi_read(pdev->dev.parent, REG_HWREV, &val, sizeof(val));
+	rc = ssbi_read(pdev->dev.parent, REG_HWREV, &val, sizeof(val));
 	if (rc) {
 		pr_err("Failed to read hw rev reg %d:rc=%d\n", REG_HWREV, rc);
 		goto err_read_rev;
@@ -133,7 +133,7 @@ static int pm8921_probe(struct platform_device *pdev)
 	rev = val;
 
 	/* Read PMIC chip revision 2 */
-	rc = msm_ssbi_read(pdev->dev.parent, REG_HWREV_2, &val, sizeof(val));
+	rc = ssbi_read(pdev->dev.parent, REG_HWREV_2, &val, sizeof(val));
 	if (rc) {
 		pr_err("Failed to read hw rev 2 reg %d:rc=%d\n",
 			REG_HWREV_2, rc);
diff --git a/drivers/mfd/wm5102-tables.c b/drivers/mfd/wm5102-tables.c
index a433f580aa4..ca2aed6bc83 100644
--- a/drivers/mfd/wm5102-tables.c
+++ b/drivers/mfd/wm5102-tables.c
@@ -331,6 +331,10 @@ static const struct reg_default wm5102_reg_default[] = {
 	{ 0x000002A3, 0x1102 },   /* R675   - Mic Detect 1 */ 
 	{ 0x000002A4, 0x009F },   /* R676   - Mic Detect 2 */ 
 	{ 0x000002A5, 0x0000 },   /* R677   - Mic Detect 3 */ 
+	{ 0x000002A6, 0x3737 },   /* R678   - Mic Detect Level 1 */
+	{ 0x000002A7, 0x372C },   /* R679   - Mic Detect Level 2 */
+	{ 0x000002A8, 0x1422 },   /* R680   - Mic Detect Level 3 */
+	{ 0x000002A9, 0x030A },   /* R681   - Mic Detect Level 4 */
 	{ 0x000002C3, 0x0000 },   /* R707   - Mic noise mix control 1 */ 
 	{ 0x000002CB, 0x0000 },   /* R715   - Isolation control */ 
 	{ 0x000002D3, 0x0000 },   /* R723   - Jack detect analogue */ 
@@ -1090,6 +1094,10 @@ static bool wm5102_readable_register(struct device *dev, unsigned int reg)
 	case ARIZONA_MIC_DETECT_1:
 	case ARIZONA_MIC_DETECT_2:
 	case ARIZONA_MIC_DETECT_3:
+	case ARIZONA_MIC_DETECT_LEVEL_1:
+	case ARIZONA_MIC_DETECT_LEVEL_2:
+	case ARIZONA_MIC_DETECT_LEVEL_3:
+	case ARIZONA_MIC_DETECT_LEVEL_4:
 	case ARIZONA_MIC_NOISE_MIX_CONTROL_1:
 	case ARIZONA_ISOLATION_CONTROL:
 	case ARIZONA_JACK_DETECT_ANALOGUE:
diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index e83fdfe0c8c..c002d8660e3 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -93,6 +93,14 @@ config ATMEL_TCB_CLKSRC_BLOCK
 	  TC can be used for other purposes, such as PWM generation and
 	  interval timing.
 
+config DUMMY_IRQ
+	tristate "Dummy IRQ handler"
+	default n
+	---help---
+	  This module accepts a single 'irq' parameter, which it should register for.
+	  The sole purpose of this module is to help with debugging of systems on
+	  which spurious IRQs would happen on disabled IRQ vector.
+
 config IBM_ASM
 	tristate "Device driver for IBM RSA service processor"
 	depends on X86 && PCI && INPUT
@@ -398,7 +406,7 @@ config DS1682
 
 config SPEAR13XX_PCIE_GADGET
 	bool "PCIe gadget support for SPEAr13XX platform"
-	depends on ARCH_SPEAR13XX
+	depends on ARCH_SPEAR13XX && BROKEN
 	default n
 	help
 	 This option enables gadget support for PCIe controller. If
@@ -418,7 +426,7 @@ config TI_DAC7512
 
 config VMWARE_BALLOON
 	tristate "VMware Balloon Driver"
-	depends on X86
+	depends on X86 && HYPERVISOR_GUEST
 	help
 	  This is VMware physical memory management driver which acts
 	  like a "balloon" that can be inflated to reclaim physical pages
@@ -510,6 +518,15 @@ config LATTICE_ECP3_CONFIG
 
 	  If unsure, say N.
 
+config SRAM
+	bool "Generic on-chip SRAM driver"
+	depends on HAS_IOMEM
+	select GENERIC_ALLOCATOR
+	help
+	  This driver allows you to declare a memory region to be managed by
+	  the genalloc API. It is supposed to be used for small on-chip SRAM
+	  areas found on many SoCs.
+
 source "drivers/misc/c2port/Kconfig"
 source "drivers/misc/eeprom/Kconfig"
 source "drivers/misc/cb710/Kconfig"
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index 35a1463c72d..c235d5b6831 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -13,6 +13,7 @@ obj-$(CONFIG_ATMEL_TCLIB)	+= atmel_tclib.o
 obj-$(CONFIG_BMP085)		+= bmp085.o
 obj-$(CONFIG_BMP085_I2C)	+= bmp085-i2c.o
 obj-$(CONFIG_BMP085_SPI)	+= bmp085-spi.o
+obj-$(CONFIG_DUMMY_IRQ)		+= dummy-irq.o
 obj-$(CONFIG_ICS932S401)	+= ics932s401.o
 obj-$(CONFIG_LKDTM)		+= lkdtm.o
 obj-$(CONFIG_TIFM_CORE)       	+= tifm_core.o
@@ -49,6 +50,6 @@ obj-y				+= carma/
 obj-$(CONFIG_USB_SWITCH_FSA9480) += fsa9480.o
 obj-$(CONFIG_ALTERA_STAPL)	+=altera-stapl/
 obj-$(CONFIG_INTEL_MEI)		+= mei/
-obj-$(CONFIG_MAX8997_MUIC)	+= max8997-muic.o
 obj-$(CONFIG_VMWARE_VMCI)	+= vmw_vmci/
 obj-$(CONFIG_LATTICE_ECP3_CONFIG)	+= lattice-ecp3-config.o
+obj-$(CONFIG_SRAM)		+= sram.o
diff --git a/drivers/misc/apds9802als.c b/drivers/misc/apds9802als.c
index d648b089302..5b5fd8416b3 100644
--- a/drivers/misc/apds9802als.c
+++ b/drivers/misc/apds9802als.c
@@ -272,19 +272,8 @@ static int apds9802als_remove(struct i2c_client *client)
 }
 
 #ifdef CONFIG_PM
-static int apds9802als_suspend(struct i2c_client *client, pm_message_t mesg)
-{
-	als_set_power_state(client, false);
-	return 0;
-}
-
-static int apds9802als_resume(struct i2c_client *client)
-{
-	als_set_default_config(client);
-	return 0;
-}
 
-static int apds9802als_runtime_suspend(struct device *dev)
+static int apds9802als_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 
@@ -292,7 +281,7 @@ static int apds9802als_runtime_suspend(struct device *dev)
 	return 0;
 }
 
-static int apds9802als_runtime_resume(struct device *dev)
+static int apds9802als_resume(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 
@@ -300,16 +289,12 @@ static int apds9802als_runtime_resume(struct device *dev)
 	return 0;
 }
 
-static const struct dev_pm_ops apds9802als_pm_ops = {
-	.runtime_suspend = apds9802als_runtime_suspend,
-	.runtime_resume = apds9802als_runtime_resume,
-};
+static UNIVERSAL_DEV_PM_OPS(apds9802als_pm_ops, apds9802als_suspend,
+	apds9802als_resume, NULL);
 
 #define APDS9802ALS_PM_OPS (&apds9802als_pm_ops)
 
 #else	/* CONFIG_PM */
-#define apds9802als_suspend NULL
-#define apds9802als_resume NULL
 #define APDS9802ALS_PM_OPS NULL
 #endif	/* CONFIG_PM */
 
@@ -327,8 +312,6 @@ static struct i2c_driver apds9802als_driver = {
 	},
 	.probe = apds9802als_probe,
 	.remove = apds9802als_remove,
-	.suspend = apds9802als_suspend,
-	.resume = apds9802als_resume,
 	.id_table = apds9802als_id,
 };
 
diff --git a/drivers/misc/apds990x.c b/drivers/misc/apds990x.c
index 0e67f8263cd..98f9bb26492 100644
--- a/drivers/misc/apds990x.c
+++ b/drivers/misc/apds990x.c
@@ -700,9 +700,6 @@ static ssize_t apds990x_lux_calib_store(struct device *dev,
 	if (strict_strtoul(buf, 0, &value))
 		return -EINVAL;
 
-	if (chip->lux_calib > APDS_RANGE)
-		return -EINVAL;
-
 	chip->lux_calib = value;
 
 	return len;
@@ -1204,7 +1201,7 @@ static int apds990x_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int apds990x_suspend(struct device *dev)
 {
 	struct i2c_client *client = container_of(dev, struct i2c_client, dev);
@@ -1227,10 +1224,6 @@ static int apds990x_resume(struct device *dev)
 
 	return 0;
 }
-#else
-#define apds990x_suspend  NULL
-#define apds990x_resume	  NULL
-#define apds990x_shutdown NULL
 #endif
 
 #ifdef CONFIG_PM_RUNTIME
diff --git a/drivers/misc/arm-charlcd.c b/drivers/misc/arm-charlcd.c
index fe8616a8d28..48651ef0028 100644
--- a/drivers/misc/arm-charlcd.c
+++ b/drivers/misc/arm-charlcd.c
@@ -378,18 +378,7 @@ static struct platform_driver charlcd_driver = {
 	.remove = __exit_p(charlcd_remove),
 };
 
-static int __init charlcd_init(void)
-{
-	return platform_driver_probe(&charlcd_driver, charlcd_probe);
-}
-
-static void __exit charlcd_exit(void)
-{
-	platform_driver_unregister(&charlcd_driver);
-}
-
-module_init(charlcd_init);
-module_exit(charlcd_exit);
+module_platform_driver_probe(charlcd_driver, charlcd_probe);
 
 MODULE_AUTHOR("Linus Walleij <triad@df.lth.se>");
 MODULE_DESCRIPTION("ARM Character LCD Driver");
diff --git a/drivers/misc/atmel_pwm.c b/drivers/misc/atmel_pwm.c
index 28f5aaa19d4..494d0500bda 100644
--- a/drivers/misc/atmel_pwm.c
+++ b/drivers/misc/atmel_pwm.c
@@ -393,17 +393,7 @@ static struct platform_driver atmel_pwm_driver = {
 	 */
 };
 
-static int __init pwm_init(void)
-{
-	return platform_driver_probe(&atmel_pwm_driver, pwm_probe);
-}
-module_init(pwm_init);
-
-static void __exit pwm_exit(void)
-{
-	platform_driver_unregister(&atmel_pwm_driver);
-}
-module_exit(pwm_exit);
+module_platform_driver_probe(atmel_pwm_driver, pwm_probe);
 
 MODULE_DESCRIPTION("Driver for AT32/AT91 PWM module");
 MODULE_LICENSE("GPL");
diff --git a/drivers/misc/bh1770glc.c b/drivers/misc/bh1770glc.c
index 2ed8fc3be7e..f4975f7d0d5 100644
--- a/drivers/misc/bh1770glc.c
+++ b/drivers/misc/bh1770glc.c
@@ -1310,7 +1310,7 @@ static int bh1770_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int bh1770_suspend(struct device *dev)
 {
 	struct i2c_client *client = container_of(dev, struct i2c_client, dev);
@@ -1346,11 +1346,6 @@ static int bh1770_resume(struct device *dev)
 	}
 	return ret;
 }
-
-#else
-#define bh1770_suspend	NULL
-#define bh1770_shutdown NULL
-#define bh1770_resume	NULL
 #endif
 
 #ifdef CONFIG_PM_RUNTIME
diff --git a/drivers/misc/bh1780gli.c b/drivers/misc/bh1780gli.c
index cf03d0abf33..818f3a0e62b 100644
--- a/drivers/misc/bh1780gli.c
+++ b/drivers/misc/bh1780gli.c
@@ -196,7 +196,7 @@ static int bh1780_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int bh1780_suspend(struct device *dev)
 {
 	struct bh1780_data *ddata;
@@ -235,11 +235,9 @@ static int bh1780_resume(struct device *dev)
 
 	return 0;
 }
+#endif /* CONFIG_PM_SLEEP */
+
 static SIMPLE_DEV_PM_OPS(bh1780_pm, bh1780_suspend, bh1780_resume);
-#define BH1780_PMOPS (&bh1780_pm)
-#else
-#define BH1780_PMOPS NULL
-#endif /* CONFIG_PM */
 
 static const struct i2c_device_id bh1780_id[] = {
 	{ "bh1780", 0 },
@@ -252,7 +250,7 @@ static struct i2c_driver bh1780_driver = {
 	.id_table	= bh1780_id,
 	.driver = {
 		.name = "bh1780",
-		.pm	= BH1780_PMOPS,
+		.pm	= &bh1780_pm,
 	},
 };
 
diff --git a/drivers/misc/cs5535-mfgpt.c b/drivers/misc/cs5535-mfgpt.c
index 9858f36dad8..effd8c6b2b9 100644
--- a/drivers/misc/cs5535-mfgpt.c
+++ b/drivers/misc/cs5535-mfgpt.c
@@ -24,8 +24,11 @@
 
 static int mfgpt_reset_timers;
 module_param_named(mfgptfix, mfgpt_reset_timers, int, 0644);
-MODULE_PARM_DESC(mfgptfix, "Reset the MFGPT timers during init; "
-		"required by some broken BIOSes (ie, TinyBIOS < 0.99).");
+MODULE_PARM_DESC(mfgptfix, "Try to reset the MFGPT timers during init; "
+		"required by some broken BIOSes (ie, TinyBIOS < 0.99) or kexec "
+		"(1 = reset the MFGPT using an undocumented bit, "
+		"2 = perform a soft reset by unconfiguring all timers); "
+		"use what works best for you.");
 
 struct cs5535_mfgpt_timer {
 	struct cs5535_mfgpt_chip *chip;
@@ -256,6 +259,28 @@ static void reset_all_timers(void)
 }
 
 /*
+ * This is another sledgehammer to reset all MFGPT timers.
+ * Instead of using the undocumented bit method it clears
+ * IRQ, NMI and RESET settings.
+ */
+static void soft_reset(void)
+{
+	int i;
+	struct cs5535_mfgpt_timer t;
+
+	for (i = 0; i < MFGPT_MAX_TIMERS; i++) {
+		t.nr = i;
+
+		cs5535_mfgpt_toggle_event(&t, MFGPT_CMP1, MFGPT_EVENT_RESET, 0);
+		cs5535_mfgpt_toggle_event(&t, MFGPT_CMP2, MFGPT_EVENT_RESET, 0);
+		cs5535_mfgpt_toggle_event(&t, MFGPT_CMP1, MFGPT_EVENT_NMI, 0);
+		cs5535_mfgpt_toggle_event(&t, MFGPT_CMP2, MFGPT_EVENT_NMI, 0);
+		cs5535_mfgpt_toggle_event(&t, MFGPT_CMP1, MFGPT_EVENT_IRQ, 0);
+		cs5535_mfgpt_toggle_event(&t, MFGPT_CMP2, MFGPT_EVENT_IRQ, 0);
+	}
+}
+
+/*
  * Check whether any MFGPTs are available for the kernel to use.  In most
  * cases, firmware that uses AMD's VSA code will claim all timers during
  * bootup; we certainly don't want to take them if they're already in use.
@@ -271,15 +296,17 @@ static int scan_timers(struct cs5535_mfgpt_chip *mfgpt)
 	int i;
 
 	/* bios workaround */
-	if (mfgpt_reset_timers)
+	if (mfgpt_reset_timers == 1)
 		reset_all_timers();
+	else if (mfgpt_reset_timers == 2)
+		soft_reset();
 
 	/* just to be safe, protect this section w/ lock */
 	spin_lock_irqsave(&mfgpt->lock, flags);
 	for (i = 0; i < MFGPT_MAX_TIMERS; i++) {
 		timer.nr = i;
 		val = cs5535_mfgpt_read(&timer, MFGPT_REG_SETUP);
-		if (!(val & MFGPT_SETUP_SETUP)) {
+		if (!(val & MFGPT_SETUP_SETUP) || mfgpt_reset_timers == 2) {
 			__set_bit(i, mfgpt->avail);
 			timers++;
 		}
@@ -294,6 +321,12 @@ static int cs5535_mfgpt_probe(struct platform_device *pdev)
 	struct resource *res;
 	int err = -EIO, t;
 
+	if (mfgpt_reset_timers < 0 || mfgpt_reset_timers > 2) {
+		dev_err(&pdev->dev, "Bad mfgpt_reset_timers value: %i\n",
+			mfgpt_reset_timers);
+		goto done;
+	}
+
 	/* There are two ways to get the MFGPT base address; one is by
 	 * fetching it from MSR_LBAR_MFGPT, the other is by reading the
 	 * PCI BAR info.  The latter method is easier (especially across
diff --git a/drivers/misc/dummy-irq.c b/drivers/misc/dummy-irq.c
new file mode 100644
index 00000000000..7014167e2c6
--- /dev/null
+++ b/drivers/misc/dummy-irq.c
@@ -0,0 +1,59 @@
+/*
+ * Dummy IRQ handler driver.
+ *
+ * This module only registers itself as a handler that is specified to it
+ * by the 'irq' parameter.
+ *
+ * The sole purpose of this module is to help with debugging of systems on
+ * which spurious IRQs would happen on disabled IRQ vector.
+ *
+ * Copyright (C) 2013 Jiri Kosina
+ */
+
+/*
+ * 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/module.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+
+static int irq;
+
+static irqreturn_t dummy_interrupt(int irq, void *dev_id)
+{
+	static int count = 0;
+
+	if (count == 0) {
+		printk(KERN_INFO "dummy-irq: interrupt occured on IRQ %d\n",
+				irq);
+		count++;
+	}
+
+	return IRQ_NONE;
+}
+
+static int __init dummy_irq_init(void)
+{
+	if (request_irq(irq, &dummy_interrupt, IRQF_SHARED, "dummy_irq", &irq)) {
+		printk(KERN_ERR "dummy-irq: cannot register IRQ %d\n", irq);
+		return -EIO;
+	}
+	printk(KERN_INFO "dummy-irq: registered for IRQ %d\n", irq);
+	return 0;
+}
+
+static void __exit dummy_irq_exit(void)
+{
+	printk(KERN_INFO "dummy-irq unloaded\n");
+	free_irq(irq, &irq);
+}
+
+module_init(dummy_irq_init);
+module_exit(dummy_irq_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Jiri Kosina");
+module_param(irq, uint, 0444);
+MODULE_PARM_DESC(irq, "The IRQ to register for");
diff --git a/drivers/misc/eeprom/at25.c b/drivers/misc/eeprom/at25.c
index b08cf8a0878..ad8fd8e6493 100644
--- a/drivers/misc/eeprom/at25.c
+++ b/drivers/misc/eeprom/at25.c
@@ -412,7 +412,7 @@ static int at25_probe(struct spi_device *spi)
 	mutex_init(&at25->lock);
 	at25->chip = chip;
 	at25->spi = spi_dev_get(spi);
-	dev_set_drvdata(&spi->dev, at25);
+	spi_set_drvdata(spi, at25);
 	at25->addrlen = addrlen;
 
 	/* Export the EEPROM bytes through sysfs, since that's convenient.
@@ -463,7 +463,7 @@ static int at25_remove(struct spi_device *spi)
 {
 	struct at25_data	*at25;
 
-	at25 = dev_get_drvdata(&spi->dev);
+	at25 = spi_get_drvdata(spi);
 	sysfs_remove_bin_file(&spi->dev.kobj, &at25->bin);
 	kfree(at25);
 	return 0;
diff --git a/drivers/misc/eeprom/eeprom_93xx46.c b/drivers/misc/eeprom/eeprom_93xx46.c
index a6b5d5e7348..94cfc121257 100644
--- a/drivers/misc/eeprom/eeprom_93xx46.c
+++ b/drivers/misc/eeprom/eeprom_93xx46.c
@@ -363,7 +363,7 @@ static int eeprom_93xx46_probe(struct spi_device *spi)
 			dev_err(&spi->dev, "can't create erase interface\n");
 	}
 
-	dev_set_drvdata(&spi->dev, edev);
+	spi_set_drvdata(spi, edev);
 	return 0;
 fail:
 	kfree(edev);
@@ -372,13 +372,13 @@ fail:
 
 static int eeprom_93xx46_remove(struct spi_device *spi)
 {
-	struct eeprom_93xx46_dev *edev = dev_get_drvdata(&spi->dev);
+	struct eeprom_93xx46_dev *edev = spi_get_drvdata(spi);
 
 	if (!(edev->pdata->flags & EE_READONLY))
 		device_remove_file(&spi->dev, &dev_attr_erase);
 
 	sysfs_remove_bin_file(&spi->dev.kobj, &edev->bin);
-	dev_set_drvdata(&spi->dev, NULL);
+	spi_set_drvdata(spi, NULL);
 	kfree(edev);
 	return 0;
 }
diff --git a/drivers/misc/ep93xx_pwm.c b/drivers/misc/ep93xx_pwm.c
index 16d7179e2f9..96787ec15ca 100644
--- a/drivers/misc/ep93xx_pwm.c
+++ b/drivers/misc/ep93xx_pwm.c
@@ -365,18 +365,7 @@ static struct platform_driver ep93xx_pwm_driver = {
 	.remove		= __exit_p(ep93xx_pwm_remove),
 };
 
-static int __init ep93xx_pwm_init(void)
-{
-	return platform_driver_probe(&ep93xx_pwm_driver, ep93xx_pwm_probe);
-}
-
-static void __exit ep93xx_pwm_exit(void)
-{
-	platform_driver_unregister(&ep93xx_pwm_driver);
-}
-
-module_init(ep93xx_pwm_init);
-module_exit(ep93xx_pwm_exit);
+module_platform_driver_probe(ep93xx_pwm_driver, ep93xx_pwm_probe);
 
 MODULE_AUTHOR("Matthieu Crapet <mcrapet@gmail.com>, "
 	      "H Hartley Sweeten <hsweeten@visionengravers.com>");
diff --git a/drivers/misc/fsa9480.c b/drivers/misc/fsa9480.c
index e8cbb1c59f4..a725c79c35f 100644
--- a/drivers/misc/fsa9480.c
+++ b/drivers/misc/fsa9480.c
@@ -474,10 +474,11 @@ static int fsa9480_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 
-static int fsa9480_suspend(struct i2c_client *client, pm_message_t state)
+static int fsa9480_suspend(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
 	struct fsa9480_usbsw *usbsw = i2c_get_clientdata(client);
 	struct fsa9480_platform_data *pdata = usbsw->pdata;
 
@@ -490,8 +491,9 @@ static int fsa9480_suspend(struct i2c_client *client, pm_message_t state)
 	return 0;
 }
 
-static int fsa9480_resume(struct i2c_client *client)
+static int fsa9480_resume(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
 	struct fsa9480_usbsw *usbsw = i2c_get_clientdata(client);
 	int dev1, dev2;
 
@@ -515,12 +517,14 @@ static int fsa9480_resume(struct i2c_client *client)
 	return 0;
 }
 
+static SIMPLE_DEV_PM_OPS(fsa9480_pm_ops, fsa9480_suspend, fsa9480_resume);
+#define FSA9480_PM_OPS (&fsa9480_pm_ops)
+
 #else
 
-#define fsa9480_suspend NULL
-#define fsa9480_resume NULL
+#define FSA9480_PM_OPS NULL
 
-#endif /* CONFIG_PM */
+#endif /* CONFIG_PM_SLEEP */
 
 static const struct i2c_device_id fsa9480_id[] = {
 	{"fsa9480", 0},
@@ -531,11 +535,10 @@ MODULE_DEVICE_TABLE(i2c, fsa9480_id);
 static struct i2c_driver fsa9480_i2c_driver = {
 	.driver = {
 		.name = "fsa9480",
+		.pm = FSA9480_PM_OPS,
 	},
 	.probe = fsa9480_probe,
 	.remove = fsa9480_remove,
-	.resume = fsa9480_resume,
-	.suspend = fsa9480_suspend,
 	.id_table = fsa9480_id,
 };
 
diff --git a/drivers/misc/isl29003.c b/drivers/misc/isl29003.c
index 29b306c6bdb..c5145b3fcce 100644
--- a/drivers/misc/isl29003.c
+++ b/drivers/misc/isl29003.c
@@ -409,18 +409,20 @@ static int isl29003_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM
-static int isl29003_suspend(struct i2c_client *client, pm_message_t mesg)
+#ifdef CONFIG_PM_SLEEP
+static int isl29003_suspend(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
 	struct isl29003_data *data = i2c_get_clientdata(client);
 
 	data->power_state_before_suspend = isl29003_get_power_state(client);
 	return isl29003_set_power_state(client, 0);
 }
 
-static int isl29003_resume(struct i2c_client *client)
+static int isl29003_resume(struct device *dev)
 {
 	int i;
+	struct i2c_client *client = to_i2c_client(dev);
 	struct isl29003_data *data = i2c_get_clientdata(client);
 
 	/* restore registers from cache */
@@ -432,10 +434,12 @@ static int isl29003_resume(struct i2c_client *client)
 		data->power_state_before_suspend);
 }
 
+static SIMPLE_DEV_PM_OPS(isl29003_pm_ops, isl29003_suspend, isl29003_resume);
+#define ISL29003_PM_OPS (&isl29003_pm_ops)
+
 #else
-#define isl29003_suspend	NULL
-#define isl29003_resume		NULL
-#endif /* CONFIG_PM */
+#define ISL29003_PM_OPS NULL
+#endif /* CONFIG_PM_SLEEP */
 
 static const struct i2c_device_id isl29003_id[] = {
 	{ "isl29003", 0 },
@@ -447,9 +451,8 @@ static struct i2c_driver isl29003_driver = {
 	.driver = {
 		.name	= ISL29003_DRV_NAME,
 		.owner	= THIS_MODULE,
+		.pm	= ISL29003_PM_OPS,
 	},
-	.suspend = isl29003_suspend,
-	.resume	= isl29003_resume,
 	.probe	= isl29003_probe,
 	.remove	= isl29003_remove,
 	.id_table = isl29003_id,
diff --git a/drivers/misc/lattice-ecp3-config.c b/drivers/misc/lattice-ecp3-config.c
index 155700bfd2b..bb26f086bd8 100644
--- a/drivers/misc/lattice-ecp3-config.c
+++ b/drivers/misc/lattice-ecp3-config.c
@@ -69,7 +69,7 @@ static const struct ecp3_dev ecp3_dev[] = {
 static void firmware_load(const struct firmware *fw, void *context)
 {
 	struct spi_device *spi = (struct spi_device *)context;
-	struct fpga_data *data = dev_get_drvdata(&spi->dev);
+	struct fpga_data *data = spi_get_drvdata(spi);
 	u8 *buffer;
 	int ret;
 	u8 txbuf[8];
diff --git a/drivers/misc/mei/Kconfig b/drivers/misc/mei/Kconfig
index d21b4d006a5..c76fa31e9bf 100644
--- a/drivers/misc/mei/Kconfig
+++ b/drivers/misc/mei/Kconfig
@@ -10,10 +10,9 @@ config INTEL_MEI
 	  <http://software.intel.com/en-us/manageability/>
 
 config INTEL_MEI_ME
-	bool "ME Enabled Intel Chipsets"
-	depends on INTEL_MEI
+	tristate "ME Enabled Intel Chipsets"
+	select INTEL_MEI
 	depends on X86 && PCI && WATCHDOG_CORE
-	default y
 	help
 	  MEI support for ME Enabled Intel chipsets.
 
diff --git a/drivers/misc/mei/Makefile b/drivers/misc/mei/Makefile
index 040af6c7b14..08698a46626 100644
--- a/drivers/misc/mei/Makefile
+++ b/drivers/misc/mei/Makefile
@@ -10,5 +10,10 @@ mei-objs += client.o
 mei-objs += main.o
 mei-objs += amthif.o
 mei-objs += wd.o
-mei-$(CONFIG_INTEL_MEI_ME) += pci-me.o
-mei-$(CONFIG_INTEL_MEI_ME) += hw-me.o
+mei-objs += bus.o
+mei-objs += nfc.o
+mei-$(CONFIG_DEBUG_FS) += debugfs.o
+
+obj-$(CONFIG_INTEL_MEI_ME) += mei-me.o
+mei-me-objs := pci-me.o
+mei-me-objs += hw-me.o
diff --git a/drivers/misc/mei/amthif.c b/drivers/misc/mei/amthif.c
index c86d7e3839a..b3e50984d2c 100644
--- a/drivers/misc/mei/amthif.c
+++ b/drivers/misc/mei/amthif.c
@@ -60,7 +60,7 @@ void mei_amthif_reset_params(struct mei_device *dev)
 }
 
 /**
- * mei_amthif_host_init_ - mei initialization amthif client.
+ * mei_amthif_host_init - mei initialization amthif client.
  *
  * @dev: the device structure
  *
@@ -433,7 +433,7 @@ unsigned int mei_amthif_poll(struct mei_device *dev,
 
 
 /**
- * mei_amthif_irq_process_completed - processes completed iamthif operation.
+ * mei_amthif_irq_write_completed - processes completed iamthif operation.
  *
  * @dev: the device structure.
  * @slots: free slots.
@@ -449,7 +449,7 @@ int mei_amthif_irq_write_complete(struct mei_device *dev, s32 *slots,
 	struct mei_msg_hdr mei_hdr;
 	struct mei_cl *cl = cb->cl;
 	size_t len = dev->iamthif_msg_buf_size - dev->iamthif_msg_buf_index;
-	size_t msg_slots = mei_data2slots(len);
+	u32 msg_slots = mei_data2slots(len);
 
 	mei_hdr.host_addr = cl->host_client_id;
 	mei_hdr.me_addr = cl->me_client_id;
@@ -505,14 +505,15 @@ int mei_amthif_irq_write_complete(struct mei_device *dev, s32 *slots,
  * mei_amthif_irq_read_message - read routine after ISR to
  *			handle the read amthif message
  *
- * @complete_list: An instance of our list structure
  * @dev: the device structure
  * @mei_hdr: header of amthif message
+ * @complete_list: An instance of our list structure
  *
  * returns 0 on success, <0 on failure.
  */
-int mei_amthif_irq_read_message(struct mei_cl_cb *complete_list,
-		struct mei_device *dev, struct mei_msg_hdr *mei_hdr)
+int mei_amthif_irq_read_msg(struct mei_device *dev,
+			    struct mei_msg_hdr *mei_hdr,
+			    struct mei_cl_cb *complete_list)
 {
 	struct mei_cl_cb *cb;
 	unsigned char *buffer;
@@ -530,8 +531,7 @@ int mei_amthif_irq_read_message(struct mei_cl_cb *complete_list,
 	if (!mei_hdr->msg_complete)
 		return 0;
 
-	dev_dbg(&dev->pdev->dev,
-			"amthif_message_buffer_index =%d\n",
+	dev_dbg(&dev->pdev->dev, "amthif_message_buffer_index =%d\n",
 			mei_hdr->length);
 
 	dev_dbg(&dev->pdev->dev, "completed amthif read.\n ");
@@ -566,12 +566,13 @@ int mei_amthif_irq_read_message(struct mei_cl_cb *complete_list,
  */
 int mei_amthif_irq_read(struct mei_device *dev, s32 *slots)
 {
+	u32 msg_slots = mei_data2slots(sizeof(struct hbm_flow_control));
 
-	if (((*slots) * sizeof(u32)) < (sizeof(struct mei_msg_hdr)
-			+ sizeof(struct hbm_flow_control))) {
+	if (*slots < msg_slots)
 		return -EMSGSIZE;
-	}
-	*slots -= mei_data2slots(sizeof(struct hbm_flow_control));
+
+	*slots -= msg_slots;
+
 	if (mei_hbm_cl_flow_control_req(dev, &dev->iamthif_cl)) {
 		dev_dbg(&dev->pdev->dev, "iamthif flow control failed\n");
 		return -EIO;
@@ -703,7 +704,7 @@ static bool mei_clear_lists(struct mei_device *dev, struct file *file)
 /**
 * mei_amthif_release - the release function
 *
-*  @inode: pointer to inode structure
+*  @dev: device structure
 *  @file: pointer to file structure
 *
 *  returns 0 on success, <0 on error
diff --git a/drivers/misc/mei/bus.c b/drivers/misc/mei/bus.c
new file mode 100644
index 00000000000..1e935eacaa7
--- /dev/null
+++ b/drivers/misc/mei/bus.c
@@ -0,0 +1,528 @@
+/*
+ * Intel Management Engine Interface (Intel MEI) Linux driver
+ * Copyright (c) 2012-2013, Intel Corporation.
+ *
+ * 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.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/mutex.h>
+#include <linux/interrupt.h>
+#include <linux/pci.h>
+#include <linux/mei_cl_bus.h>
+
+#include "mei_dev.h"
+#include "hw-me.h"
+#include "client.h"
+
+#define to_mei_cl_driver(d) container_of(d, struct mei_cl_driver, driver)
+#define to_mei_cl_device(d) container_of(d, struct mei_cl_device, dev)
+
+static int mei_cl_device_match(struct device *dev, struct device_driver *drv)
+{
+	struct mei_cl_device *device = to_mei_cl_device(dev);
+	struct mei_cl_driver *driver = to_mei_cl_driver(drv);
+	const struct mei_cl_device_id *id;
+
+	if (!device)
+		return 0;
+
+	if (!driver || !driver->id_table)
+		return 0;
+
+	id = driver->id_table;
+
+	while (id->name[0]) {
+		if (!strcmp(dev_name(dev), id->name))
+			return 1;
+
+		id++;
+	}
+
+	return 0;
+}
+
+static int mei_cl_device_probe(struct device *dev)
+{
+	struct mei_cl_device *device = to_mei_cl_device(dev);
+	struct mei_cl_driver *driver;
+	struct mei_cl_device_id id;
+
+	if (!device)
+		return 0;
+
+	driver = to_mei_cl_driver(dev->driver);
+	if (!driver || !driver->probe)
+		return -ENODEV;
+
+	dev_dbg(dev, "Device probe\n");
+
+	strncpy(id.name, dev_name(dev), MEI_CL_NAME_SIZE);
+
+	return driver->probe(device, &id);
+}
+
+static int mei_cl_device_remove(struct device *dev)
+{
+	struct mei_cl_device *device = to_mei_cl_device(dev);
+	struct mei_cl_driver *driver;
+
+	if (!device || !dev->driver)
+		return 0;
+
+	if (device->event_cb) {
+		device->event_cb = NULL;
+		cancel_work_sync(&device->event_work);
+	}
+
+	driver = to_mei_cl_driver(dev->driver);
+	if (!driver->remove) {
+		dev->driver = NULL;
+
+		return 0;
+	}
+
+	return driver->remove(device);
+}
+
+static ssize_t modalias_show(struct device *dev, struct device_attribute *a,
+			     char *buf)
+{
+	int len;
+
+	len = snprintf(buf, PAGE_SIZE, "mei:%s\n", dev_name(dev));
+
+	return (len >= PAGE_SIZE) ? (PAGE_SIZE - 1) : len;
+}
+
+static struct device_attribute mei_cl_dev_attrs[] = {
+	__ATTR_RO(modalias),
+	__ATTR_NULL,
+};
+
+static int mei_cl_uevent(struct device *dev, struct kobj_uevent_env *env)
+{
+	if (add_uevent_var(env, "MODALIAS=mei:%s", dev_name(dev)))
+		return -ENOMEM;
+
+	return 0;
+}
+
+static struct bus_type mei_cl_bus_type = {
+	.name		= "mei",
+	.dev_attrs	= mei_cl_dev_attrs,
+	.match		= mei_cl_device_match,
+	.probe		= mei_cl_device_probe,
+	.remove		= mei_cl_device_remove,
+	.uevent		= mei_cl_uevent,
+};
+
+static void mei_cl_dev_release(struct device *dev)
+{
+	kfree(to_mei_cl_device(dev));
+}
+
+static struct device_type mei_cl_device_type = {
+	.release	= mei_cl_dev_release,
+};
+
+static struct mei_cl *mei_bus_find_mei_cl_by_uuid(struct mei_device *dev,
+						uuid_le uuid)
+{
+	struct mei_cl *cl, *next;
+
+	list_for_each_entry_safe(cl, next, &dev->device_list, device_link) {
+		if (!uuid_le_cmp(uuid, cl->device_uuid))
+			return cl;
+	}
+
+	return NULL;
+}
+struct mei_cl_device *mei_cl_add_device(struct mei_device *dev,
+					uuid_le uuid, char *name,
+					struct mei_cl_ops *ops)
+{
+	struct mei_cl_device *device;
+	struct mei_cl *cl;
+	int status;
+
+	cl = mei_bus_find_mei_cl_by_uuid(dev, uuid);
+	if (cl == NULL)
+		return NULL;
+
+	device = kzalloc(sizeof(struct mei_cl_device), GFP_KERNEL);
+	if (!device)
+		return NULL;
+
+	device->cl = cl;
+	device->ops = ops;
+
+	device->dev.parent = &dev->pdev->dev;
+	device->dev.bus = &mei_cl_bus_type;
+	device->dev.type = &mei_cl_device_type;
+
+	dev_set_name(&device->dev, "%s", name);
+
+	status = device_register(&device->dev);
+	if (status) {
+		dev_err(&dev->pdev->dev, "Failed to register MEI device\n");
+		kfree(device);
+		return NULL;
+	}
+
+	cl->device = device;
+
+	dev_dbg(&device->dev, "client %s registered\n", name);
+
+	return device;
+}
+EXPORT_SYMBOL_GPL(mei_cl_add_device);
+
+void mei_cl_remove_device(struct mei_cl_device *device)
+{
+	device_unregister(&device->dev);
+}
+EXPORT_SYMBOL_GPL(mei_cl_remove_device);
+
+int __mei_cl_driver_register(struct mei_cl_driver *driver, struct module *owner)
+{
+	int err;
+
+	driver->driver.name = driver->name;
+	driver->driver.owner = owner;
+	driver->driver.bus = &mei_cl_bus_type;
+
+	err = driver_register(&driver->driver);
+	if (err)
+		return err;
+
+	pr_debug("mei: driver [%s] registered\n", driver->driver.name);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(__mei_cl_driver_register);
+
+void mei_cl_driver_unregister(struct mei_cl_driver *driver)
+{
+	driver_unregister(&driver->driver);
+
+	pr_debug("mei: driver [%s] unregistered\n", driver->driver.name);
+}
+EXPORT_SYMBOL_GPL(mei_cl_driver_unregister);
+
+static int ___mei_cl_send(struct mei_cl *cl, u8 *buf, size_t length,
+			bool blocking)
+{
+	struct mei_device *dev;
+	struct mei_cl_cb *cb;
+	int id;
+	int rets;
+
+	if (WARN_ON(!cl || !cl->dev))
+		return -ENODEV;
+
+	dev = cl->dev;
+
+	if (cl->state != MEI_FILE_CONNECTED)
+		return -ENODEV;
+
+	/* Check if we have an ME client device */
+	id = mei_me_cl_by_id(dev, cl->me_client_id);
+	if (id < 0)
+		return -ENODEV;
+
+	if (length > dev->me_clients[id].props.max_msg_length)
+		return -EINVAL;
+
+	cb = mei_io_cb_init(cl, NULL);
+	if (!cb)
+		return -ENOMEM;
+
+	rets = mei_io_cb_alloc_req_buf(cb, length);
+	if (rets < 0) {
+		mei_io_cb_free(cb);
+		return rets;
+	}
+
+	memcpy(cb->request_buffer.data, buf, length);
+
+	mutex_lock(&dev->device_lock);
+
+	rets = mei_cl_write(cl, cb, blocking);
+
+	mutex_unlock(&dev->device_lock);
+	if (rets < 0)
+		mei_io_cb_free(cb);
+
+	return rets;
+}
+
+int __mei_cl_recv(struct mei_cl *cl, u8 *buf, size_t length)
+{
+	struct mei_device *dev;
+	struct mei_cl_cb *cb;
+	size_t r_length;
+	int err;
+
+	if (WARN_ON(!cl || !cl->dev))
+		return -ENODEV;
+
+	dev = cl->dev;
+
+	mutex_lock(&dev->device_lock);
+
+	if (!cl->read_cb) {
+		err = mei_cl_read_start(cl, length);
+		if (err < 0) {
+			mutex_unlock(&dev->device_lock);
+			return err;
+		}
+	}
+
+	if (cl->reading_state != MEI_READ_COMPLETE &&
+	    !waitqueue_active(&cl->rx_wait)) {
+		mutex_unlock(&dev->device_lock);
+
+		if (wait_event_interruptible(cl->rx_wait,
+				(MEI_READ_COMPLETE == cl->reading_state))) {
+			if (signal_pending(current))
+				return -EINTR;
+			return -ERESTARTSYS;
+		}
+
+		mutex_lock(&dev->device_lock);
+	}
+
+	cb = cl->read_cb;
+
+	if (cl->reading_state != MEI_READ_COMPLETE) {
+		r_length = 0;
+		goto out;
+	}
+
+	r_length = min_t(size_t, length, cb->buf_idx);
+
+	memcpy(buf, cb->response_buffer.data, r_length);
+
+	mei_io_cb_free(cb);
+	cl->reading_state = MEI_IDLE;
+	cl->read_cb = NULL;
+
+out:
+	mutex_unlock(&dev->device_lock);
+
+	return r_length;
+}
+
+inline int __mei_cl_async_send(struct mei_cl *cl, u8 *buf, size_t length)
+{
+	return ___mei_cl_send(cl, buf, length, 0);
+}
+
+inline int __mei_cl_send(struct mei_cl *cl, u8 *buf, size_t length)
+{
+	return ___mei_cl_send(cl, buf, length, 1);
+}
+
+int mei_cl_send(struct mei_cl_device *device, u8 *buf, size_t length)
+{
+	struct mei_cl *cl = device->cl;
+
+	if (cl == NULL)
+		return -ENODEV;
+
+	if (device->ops && device->ops->send)
+		return device->ops->send(device, buf, length);
+
+	return __mei_cl_send(cl, buf, length);
+}
+EXPORT_SYMBOL_GPL(mei_cl_send);
+
+int mei_cl_recv(struct mei_cl_device *device, u8 *buf, size_t length)
+{
+	struct mei_cl *cl =  device->cl;
+
+	if (cl == NULL)
+		return -ENODEV;
+
+	if (device->ops && device->ops->recv)
+		return device->ops->recv(device, buf, length);
+
+	return __mei_cl_recv(cl, buf, length);
+}
+EXPORT_SYMBOL_GPL(mei_cl_recv);
+
+static void mei_bus_event_work(struct work_struct *work)
+{
+	struct mei_cl_device *device;
+
+	device = container_of(work, struct mei_cl_device, event_work);
+
+	if (device->event_cb)
+		device->event_cb(device, device->events, device->event_context);
+
+	device->events = 0;
+
+	/* Prepare for the next read */
+	mei_cl_read_start(device->cl, 0);
+}
+
+int mei_cl_register_event_cb(struct mei_cl_device *device,
+			  mei_cl_event_cb_t event_cb, void *context)
+{
+	if (device->event_cb)
+		return -EALREADY;
+
+	device->events = 0;
+	device->event_cb = event_cb;
+	device->event_context = context;
+	INIT_WORK(&device->event_work, mei_bus_event_work);
+
+	mei_cl_read_start(device->cl, 0);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mei_cl_register_event_cb);
+
+void *mei_cl_get_drvdata(const struct mei_cl_device *device)
+{
+	return dev_get_drvdata(&device->dev);
+}
+EXPORT_SYMBOL_GPL(mei_cl_get_drvdata);
+
+void mei_cl_set_drvdata(struct mei_cl_device *device, void *data)
+{
+	dev_set_drvdata(&device->dev, data);
+}
+EXPORT_SYMBOL_GPL(mei_cl_set_drvdata);
+
+int mei_cl_enable_device(struct mei_cl_device *device)
+{
+	int err;
+	struct mei_device *dev;
+	struct mei_cl *cl = device->cl;
+
+	if (cl == NULL)
+		return -ENODEV;
+
+	dev = cl->dev;
+
+	mutex_lock(&dev->device_lock);
+
+	cl->state = MEI_FILE_CONNECTING;
+
+	err = mei_cl_connect(cl, NULL);
+	if (err < 0) {
+		mutex_unlock(&dev->device_lock);
+		dev_err(&dev->pdev->dev, "Could not connect to the ME client");
+
+		return err;
+	}
+
+	mutex_unlock(&dev->device_lock);
+
+	if (device->event_cb && !cl->read_cb)
+		mei_cl_read_start(device->cl, 0);
+
+	if (!device->ops || !device->ops->enable)
+		return 0;
+
+	return device->ops->enable(device);
+}
+EXPORT_SYMBOL_GPL(mei_cl_enable_device);
+
+int mei_cl_disable_device(struct mei_cl_device *device)
+{
+	int err;
+	struct mei_device *dev;
+	struct mei_cl *cl = device->cl;
+
+	if (cl == NULL)
+		return -ENODEV;
+
+	dev = cl->dev;
+
+	mutex_lock(&dev->device_lock);
+
+	if (cl->state != MEI_FILE_CONNECTED) {
+		mutex_unlock(&dev->device_lock);
+		dev_err(&dev->pdev->dev, "Already disconnected");
+
+		return 0;
+	}
+
+	cl->state = MEI_FILE_DISCONNECTING;
+
+	err = mei_cl_disconnect(cl);
+	if (err < 0) {
+		mutex_unlock(&dev->device_lock);
+		dev_err(&dev->pdev->dev,
+			"Could not disconnect from the ME client");
+
+		return err;
+	}
+
+	/* Flush queues and remove any pending read */
+	mei_cl_flush_queues(cl);
+
+	if (cl->read_cb) {
+		struct mei_cl_cb *cb = NULL;
+
+		cb = mei_cl_find_read_cb(cl);
+		/* Remove entry from read list */
+		if (cb)
+			list_del(&cb->list);
+
+		cb = cl->read_cb;
+		cl->read_cb = NULL;
+
+		if (cb) {
+			mei_io_cb_free(cb);
+			cb = NULL;
+		}
+	}
+
+	mutex_unlock(&dev->device_lock);
+
+	if (!device->ops || !device->ops->disable)
+		return 0;
+
+	return device->ops->disable(device);
+}
+EXPORT_SYMBOL_GPL(mei_cl_disable_device);
+
+void mei_cl_bus_rx_event(struct mei_cl *cl)
+{
+	struct mei_cl_device *device = cl->device;
+
+	if (!device || !device->event_cb)
+		return;
+
+	set_bit(MEI_CL_EVENT_RX, &device->events);
+
+	schedule_work(&device->event_work);
+}
+
+int __init mei_cl_bus_init(void)
+{
+	return bus_register(&mei_cl_bus_type);
+}
+
+void __exit mei_cl_bus_exit(void)
+{
+	bus_unregister(&mei_cl_bus_type);
+}
diff --git a/drivers/misc/mei/client.c b/drivers/misc/mei/client.c
index 1569afe935d..e310ca6ed1a 100644
--- a/drivers/misc/mei/client.c
+++ b/drivers/misc/mei/client.c
@@ -109,7 +109,7 @@ void mei_io_cb_free(struct mei_cl_cb *cb)
  * mei_io_cb_init - allocate and initialize io callback
  *
  * @cl - mei client
- * @file: pointer to file structure
+ * @fp: pointer to file structure
  *
  * returns mei_cl_cb pointer or NULL;
  */
@@ -132,8 +132,8 @@ struct mei_cl_cb *mei_io_cb_init(struct mei_cl *cl, struct file *fp)
 /**
  * mei_io_cb_alloc_req_buf - allocate request buffer
  *
- * @cb -  io callback structure
- * @size: size of the buffer
+ * @cb: io callback structure
+ * @length: size of the buffer
  *
  * returns 0 on success
  *         -EINVAL if cb is NULL
@@ -154,10 +154,10 @@ int mei_io_cb_alloc_req_buf(struct mei_cl_cb *cb, size_t length)
 	return 0;
 }
 /**
- * mei_io_cb_alloc_req_buf - allocate respose buffer
+ * mei_io_cb_alloc_resp_buf - allocate respose buffer
  *
- * @cb -  io callback structure
- * @size: size of the buffer
+ * @cb: io callback structure
+ * @length: size of the buffer
  *
  * returns 0 on success
  *         -EINVAL if cb is NULL
@@ -183,7 +183,6 @@ int mei_io_cb_alloc_resp_buf(struct mei_cl_cb *cb, size_t length)
 /**
  * mei_cl_flush_queues - flushes queue lists belonging to cl.
  *
- * @dev: the device structure
  * @cl: host client
  */
 int mei_cl_flush_queues(struct mei_cl *cl)
@@ -216,6 +215,7 @@ void mei_cl_init(struct mei_cl *cl, struct mei_device *dev)
 	init_waitqueue_head(&cl->rx_wait);
 	init_waitqueue_head(&cl->tx_wait);
 	INIT_LIST_HEAD(&cl->link);
+	INIT_LIST_HEAD(&cl->device_link);
 	cl->reading_state = MEI_IDLE;
 	cl->writing_state = MEI_IDLE;
 	cl->dev = dev;
@@ -243,7 +243,8 @@ struct mei_cl *mei_cl_allocate(struct mei_device *dev)
 /**
  * mei_cl_find_read_cb - find this cl's callback in the read list
  *
- * @dev: device structure
+ * @cl: host client
+ *
  * returns cb on success, NULL on error
  */
 struct mei_cl_cb *mei_cl_find_read_cb(struct mei_cl *cl)
@@ -262,6 +263,7 @@ struct mei_cl_cb *mei_cl_find_read_cb(struct mei_cl *cl)
  *
  * @cl - host client
  * @id - fixed host id or -1 for genereting one
+ *
  * returns 0 on success
  *	-EINVAL on incorrect values
  *	-ENONET if client not found
@@ -301,7 +303,7 @@ int mei_cl_link(struct mei_cl *cl, int id)
 /**
  * mei_cl_unlink - remove me_cl from the list
  *
- * @dev: the device structure
+ * @cl: host client
  */
 int mei_cl_unlink(struct mei_cl *cl)
 {
@@ -357,6 +359,9 @@ void mei_host_client_init(struct work_struct *work)
 			mei_amthif_host_init(dev);
 		else if (!uuid_le_cmp(client_props->protocol_name, mei_wd_guid))
 			mei_wd_host_init(dev);
+		else if (!uuid_le_cmp(client_props->protocol_name, mei_nfc_guid))
+			mei_nfc_host_init(dev);
+
 	}
 
 	dev->dev_state = MEI_DEV_ENABLED;
@@ -534,7 +539,6 @@ out:
 /**
  * mei_cl_flow_ctrl_creds - checks flow_control credits for cl.
  *
- * @dev: the device structure
  * @cl: private data of the file object
  *
  * returns 1 if mei_flow_ctrl_creds >0, 0 - otherwise.
@@ -575,8 +579,8 @@ int mei_cl_flow_ctrl_creds(struct mei_cl *cl)
 /**
  * mei_cl_flow_ctrl_reduce - reduces flow_control.
  *
- * @dev: the device structure
  * @cl: private data of the file object
+ *
  * @returns
  *	0 on success
  *	-ENOENT when me client is not found
@@ -614,13 +618,13 @@ int mei_cl_flow_ctrl_reduce(struct mei_cl *cl)
 }
 
 /**
- * mei_cl_start_read - the start read client message function.
+ * mei_cl_read_start - the start read client message function.
  *
  * @cl: host client
  *
  * returns 0 on success, <0 on failure.
  */
-int mei_cl_read_start(struct mei_cl *cl)
+int mei_cl_read_start(struct mei_cl *cl, size_t length)
 {
 	struct mei_device *dev;
 	struct mei_cl_cb *cb;
@@ -653,8 +657,9 @@ int mei_cl_read_start(struct mei_cl *cl)
 	if (!cb)
 		return -ENOMEM;
 
-	rets = mei_io_cb_alloc_resp_buf(cb,
-			dev->me_clients[i].props.max_msg_length);
+	/* always allocate at least client max message */
+	length = max_t(size_t, length, dev->me_clients[i].props.max_msg_length);
+	rets = mei_io_cb_alloc_resp_buf(cb, length);
 	if (rets)
 		goto err;
 
@@ -677,6 +682,111 @@ err:
 }
 
 /**
+ * mei_cl_write - submit a write cb to mei device
+	assumes device_lock is locked
+ *
+ * @cl: host client
+ * @cl: write callback with filled data
+ *
+ * returns numbe of bytes sent on success, <0 on failure.
+ */
+int mei_cl_write(struct mei_cl *cl, struct mei_cl_cb *cb, bool blocking)
+{
+	struct mei_device *dev;
+	struct mei_msg_data *buf;
+	struct mei_msg_hdr mei_hdr;
+	int rets;
+
+
+	if (WARN_ON(!cl || !cl->dev))
+		return -ENODEV;
+
+	if (WARN_ON(!cb))
+		return -EINVAL;
+
+	dev = cl->dev;
+
+
+	buf = &cb->request_buffer;
+
+	dev_dbg(&dev->pdev->dev, "mei_cl_write %d\n", buf->size);
+
+
+	cb->fop_type = MEI_FOP_WRITE;
+
+	rets = mei_cl_flow_ctrl_creds(cl);
+	if (rets < 0)
+		goto err;
+
+	/* Host buffer is not ready, we queue the request */
+	if (rets == 0 || !dev->hbuf_is_ready) {
+		cb->buf_idx = 0;
+		/* unseting complete will enqueue the cb for write */
+		mei_hdr.msg_complete = 0;
+		cl->writing_state = MEI_WRITING;
+		rets = buf->size;
+		goto out;
+	}
+
+	dev->hbuf_is_ready = false;
+
+	/* Check for a maximum length */
+	if (buf->size > mei_hbuf_max_len(dev)) {
+		mei_hdr.length = mei_hbuf_max_len(dev);
+		mei_hdr.msg_complete = 0;
+	} else {
+		mei_hdr.length = buf->size;
+		mei_hdr.msg_complete = 1;
+	}
+
+	mei_hdr.host_addr = cl->host_client_id;
+	mei_hdr.me_addr = cl->me_client_id;
+	mei_hdr.reserved = 0;
+
+	dev_dbg(&dev->pdev->dev, "write " MEI_HDR_FMT "\n",
+		MEI_HDR_PRM(&mei_hdr));
+
+
+	if (mei_write_message(dev, &mei_hdr, buf->data)) {
+		rets = -EIO;
+		goto err;
+	}
+
+	cl->writing_state = MEI_WRITING;
+	cb->buf_idx = mei_hdr.length;
+
+	rets = buf->size;
+out:
+	if (mei_hdr.msg_complete) {
+		if (mei_cl_flow_ctrl_reduce(cl)) {
+			rets = -ENODEV;
+			goto err;
+		}
+		list_add_tail(&cb->list, &dev->write_waiting_list.list);
+	} else {
+		list_add_tail(&cb->list, &dev->write_list.list);
+	}
+
+
+	if (blocking && cl->writing_state != MEI_WRITE_COMPLETE) {
+
+		mutex_unlock(&dev->device_lock);
+		if (wait_event_interruptible(cl->tx_wait,
+			cl->writing_state == MEI_WRITE_COMPLETE)) {
+				if (signal_pending(current))
+					rets = -EINTR;
+				else
+					rets = -ERESTARTSYS;
+		}
+		mutex_lock(&dev->device_lock);
+	}
+err:
+	return rets;
+}
+
+
+
+/**
  * mei_cl_all_disconnect - disconnect forcefully all connected clients
  *
  * @dev - mei device
diff --git a/drivers/misc/mei/client.h b/drivers/misc/mei/client.h
index 214b2397ec3..cfdb144526a 100644
--- a/drivers/misc/mei/client.h
+++ b/drivers/misc/mei/client.h
@@ -86,17 +86,16 @@ int mei_cl_flow_ctrl_reduce(struct mei_cl *cl);
  */
 bool mei_cl_is_other_connecting(struct mei_cl *cl);
 int mei_cl_disconnect(struct mei_cl *cl);
-
-int mei_cl_read_start(struct mei_cl *cl);
-
 int mei_cl_connect(struct mei_cl *cl, struct file *file);
+int mei_cl_read_start(struct mei_cl *cl, size_t length);
+int mei_cl_write(struct mei_cl *cl, struct mei_cl_cb *cb, bool blocking);
 
 void mei_host_client_init(struct work_struct *work);
 
 
+
 void mei_cl_all_disconnect(struct mei_device *dev);
 void mei_cl_all_read_wakeup(struct mei_device *dev);
 void mei_cl_all_write_clear(struct mei_device *dev);
 
-
 #endif /* _MEI_CLIENT_H_ */
diff --git a/drivers/misc/mei/debugfs.c b/drivers/misc/mei/debugfs.c
new file mode 100644
index 00000000000..e3870f22d23
--- /dev/null
+++ b/drivers/misc/mei/debugfs.c
@@ -0,0 +1,143 @@
+/*
+ *
+ * Intel Management Engine Interface (Intel MEI) Linux driver
+ * Copyright (c) 2012-2013, Intel Corporation.
+ *
+ * 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.
+ *
+ */
+#include <linux/slab.h>
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/debugfs.h>
+#include <linux/pci.h>
+
+#include <linux/mei.h>
+
+#include "mei_dev.h"
+#include "hw.h"
+
+static ssize_t mei_dbgfs_read_meclients(struct file *fp, char __user *ubuf,
+					size_t cnt, loff_t *ppos)
+{
+	struct mei_device *dev = fp->private_data;
+	struct mei_me_client *cl;
+	const size_t bufsz = 1024;
+	char *buf = kzalloc(bufsz, GFP_KERNEL);
+	int i;
+	int pos = 0;
+	int ret;
+
+	if  (!buf)
+		return -ENOMEM;
+
+	pos += scnprintf(buf + pos, bufsz - pos,
+			"  |id|addr|         UUID                       |con|msg len|\n");
+
+	mutex_lock(&dev->device_lock);
+
+	/*  if the driver is not enabled the list won't b consitent */
+	if (dev->dev_state != MEI_DEV_ENABLED)
+		goto out;
+
+	for (i = 0; i < dev->me_clients_num; i++) {
+		cl = &dev->me_clients[i];
+
+		/* skip me clients that cannot be connected */
+		if (cl->props.max_number_of_connections == 0)
+			continue;
+
+		pos += scnprintf(buf + pos, bufsz - pos,
+			"%2d|%2d|%4d|%pUl|%3d|%7d|\n",
+			i, cl->client_id,
+			cl->props.fixed_address,
+			&cl->props.protocol_name,
+			cl->props.max_number_of_connections,
+			cl->props.max_msg_length);
+	}
+out:
+	mutex_unlock(&dev->device_lock);
+	ret = simple_read_from_buffer(ubuf, cnt, ppos, buf, pos);
+	kfree(buf);
+	return ret;
+}
+
+static const struct file_operations mei_dbgfs_fops_meclients = {
+	.open = simple_open,
+	.read = mei_dbgfs_read_meclients,
+	.llseek = generic_file_llseek,
+};
+
+static ssize_t mei_dbgfs_read_devstate(struct file *fp, char __user *ubuf,
+					size_t cnt, loff_t *ppos)
+{
+	struct mei_device *dev = fp->private_data;
+	const size_t bufsz = 1024;
+	char *buf = kzalloc(bufsz, GFP_KERNEL);
+	int pos = 0;
+	int ret;
+
+	if  (!buf)
+		return -ENOMEM;
+
+	pos += scnprintf(buf + pos, bufsz - pos, "%s\n",
+			mei_dev_state_str(dev->dev_state));
+	ret = simple_read_from_buffer(ubuf, cnt, ppos, buf, pos);
+	kfree(buf);
+	return ret;
+}
+static const struct file_operations mei_dbgfs_fops_devstate = {
+	.open = simple_open,
+	.read = mei_dbgfs_read_devstate,
+	.llseek = generic_file_llseek,
+};
+
+/**
+ * mei_dbgfs_deregister - Remove the debugfs files and directories
+ * @mei - pointer to mei device private dat
+ */
+void mei_dbgfs_deregister(struct mei_device *dev)
+{
+	if (!dev->dbgfs_dir)
+		return;
+	debugfs_remove_recursive(dev->dbgfs_dir);
+	dev->dbgfs_dir = NULL;
+}
+
+/**
+ * Add the debugfs files
+ *
+ */
+int mei_dbgfs_register(struct mei_device *dev, const char *name)
+{
+	struct dentry *dir, *f;
+	dir = debugfs_create_dir(name, NULL);
+	if (!dir)
+		return -ENOMEM;
+
+	f = debugfs_create_file("meclients", S_IRUSR, dir,
+				dev, &mei_dbgfs_fops_meclients);
+	if (!f) {
+		dev_err(&dev->pdev->dev, "meclients: registration failed\n");
+		goto err;
+	}
+	f = debugfs_create_file("devstate", S_IRUSR, dir,
+				dev, &mei_dbgfs_fops_devstate);
+	if (!f) {
+		dev_err(&dev->pdev->dev, "devstate: registration failed\n");
+		goto err;
+	}
+	dev->dbgfs_dir = dir;
+	return 0;
+err:
+	mei_dbgfs_deregister(dev);
+	return -ENODEV;
+}
+
diff --git a/drivers/misc/mei/hbm.c b/drivers/misc/mei/hbm.c
index fb9e63ba3bb..6916045166e 100644
--- a/drivers/misc/mei/hbm.c
+++ b/drivers/misc/mei/hbm.c
@@ -52,7 +52,7 @@ static void mei_hbm_me_cl_allocate(struct mei_device *dev)
 			sizeof(struct mei_me_client), GFP_KERNEL);
 	if (!clients) {
 		dev_err(&dev->pdev->dev, "memory allocation for ME clients failed.\n");
-		dev->dev_state = MEI_DEV_RESETING;
+		dev->dev_state = MEI_DEV_RESETTING;
 		mei_reset(dev, 1);
 		return;
 	}
@@ -62,6 +62,7 @@ static void mei_hbm_me_cl_allocate(struct mei_device *dev)
 
 /**
  * mei_hbm_cl_hdr - construct client hbm header
+ *
  * @cl: - client
  * @hbm_cmd: host bus message command
  * @buf: buffer for cl header
@@ -123,12 +124,33 @@ static bool is_treat_specially_client(struct mei_cl *cl,
 	return false;
 }
 
+int mei_hbm_start_wait(struct mei_device *dev)
+{
+	int ret;
+	if (dev->hbm_state > MEI_HBM_START)
+		return 0;
+
+	mutex_unlock(&dev->device_lock);
+	ret = wait_event_interruptible_timeout(dev->wait_recvd_msg,
+			dev->hbm_state == MEI_HBM_IDLE ||
+			dev->hbm_state > MEI_HBM_START,
+			mei_secs_to_jiffies(MEI_INTEROP_TIMEOUT));
+	mutex_lock(&dev->device_lock);
+
+	if (ret <= 0 && (dev->hbm_state <= MEI_HBM_START)) {
+		dev->hbm_state = MEI_HBM_IDLE;
+		dev_err(&dev->pdev->dev, "wating for mei start failed\n");
+		return -ETIMEDOUT;
+	}
+	return 0;
+}
+
 /**
  * mei_hbm_start_req - sends start request message.
  *
  * @dev: the device structure
  */
-void mei_hbm_start_req(struct mei_device *dev)
+int mei_hbm_start_req(struct mei_device *dev)
 {
 	struct mei_msg_hdr *mei_hdr = &dev->wr_msg.hdr;
 	struct hbm_host_version_request *start_req;
@@ -143,18 +165,19 @@ void mei_hbm_start_req(struct mei_device *dev)
 	start_req->host_version.major_version = HBM_MAJOR_VERSION;
 	start_req->host_version.minor_version = HBM_MINOR_VERSION;
 
-	dev->recvd_msg = false;
+	dev->hbm_state = MEI_HBM_IDLE;
 	if (mei_write_message(dev, mei_hdr, dev->wr_msg.data)) {
-		dev_dbg(&dev->pdev->dev, "write send version message to FW fail.\n");
-		dev->dev_state = MEI_DEV_RESETING;
+		dev_err(&dev->pdev->dev, "version message writet failed\n");
+		dev->dev_state = MEI_DEV_RESETTING;
 		mei_reset(dev, 1);
+		return -ENODEV;
 	}
-	dev->init_clients_state = MEI_START_MESSAGE;
+	dev->hbm_state = MEI_HBM_START;
 	dev->init_clients_timer = MEI_CLIENTS_INIT_TIMEOUT;
-	return ;
+	return 0;
 }
 
-/**
+/*
  * mei_hbm_enum_clients_req - sends enumeration client request message.
  *
  * @dev: the device structure
@@ -174,17 +197,17 @@ static void mei_hbm_enum_clients_req(struct mei_device *dev)
 	enum_req->hbm_cmd = HOST_ENUM_REQ_CMD;
 
 	if (mei_write_message(dev, mei_hdr, dev->wr_msg.data)) {
-		dev->dev_state = MEI_DEV_RESETING;
-		dev_dbg(&dev->pdev->dev, "write send enumeration request message to FW fail.\n");
+		dev->dev_state = MEI_DEV_RESETTING;
+		dev_err(&dev->pdev->dev, "enumeration request write failed.\n");
 		mei_reset(dev, 1);
 	}
-	dev->init_clients_state = MEI_ENUM_CLIENTS_MESSAGE;
+	dev->hbm_state = MEI_HBM_ENUM_CLIENTS;
 	dev->init_clients_timer = MEI_CLIENTS_INIT_TIMEOUT;
 	return;
 }
 
 /**
- * mei_hbm_prop_requsest - request property for a single client
+ * mei_hbm_prop_req - request property for a single client
  *
  * @dev: the device structure
  *
@@ -208,6 +231,7 @@ static int mei_hbm_prop_req(struct mei_device *dev)
 
 	/* We got all client properties */
 	if (next_client_index == MEI_CLIENTS_MAX) {
+		dev->hbm_state = MEI_HBM_STARTED;
 		schedule_work(&dev->init_work);
 
 		return 0;
@@ -226,8 +250,8 @@ static int mei_hbm_prop_req(struct mei_device *dev)
 	prop_req->address = next_client_index;
 
 	if (mei_write_message(dev, mei_hdr, dev->wr_msg.data)) {
-		dev->dev_state = MEI_DEV_RESETING;
-		dev_err(&dev->pdev->dev, "Properties request command failed\n");
+		dev->dev_state = MEI_DEV_RESETTING;
+		dev_err(&dev->pdev->dev, "properties request write failed\n");
 		mei_reset(dev, 1);
 
 		return -EIO;
@@ -283,9 +307,9 @@ int mei_hbm_cl_flow_control_req(struct mei_device *dev, struct mei_cl *cl)
 }
 
 /**
- * add_single_flow_creds - adds single buffer credentials.
+ * mei_hbm_add_single_flow_creds - adds single buffer credentials.
  *
- * @file: private data ot the file object.
+ * @dev: the device structure
  * @flow: flow control.
  */
 static void mei_hbm_add_single_flow_creds(struct mei_device *dev,
@@ -477,7 +501,7 @@ static void mei_hbm_cl_connect_res(struct mei_device *dev,
 
 
 /**
- * mei_client_disconnect_request - disconnect request initiated by me
+ * mei_hbm_fw_disconnect_req - disconnect request initiated by me
  *  host sends disoconnect response
  *
  * @dev: the device structure.
@@ -542,27 +566,28 @@ void mei_hbm_dispatch(struct mei_device *dev, struct mei_msg_hdr *hdr)
 			dev->version = version_res->me_max_version;
 			dev_dbg(&dev->pdev->dev, "version mismatch.\n");
 
+			dev->hbm_state = MEI_HBM_STOP;
 			mei_hbm_stop_req_prepare(dev, &dev->wr_msg.hdr,
 						dev->wr_msg.data);
 			mei_write_message(dev, &dev->wr_msg.hdr,
 					dev->wr_msg.data);
+
 			return;
 		}
 
 		dev->version.major_version = HBM_MAJOR_VERSION;
 		dev->version.minor_version = HBM_MINOR_VERSION;
 		if (dev->dev_state == MEI_DEV_INIT_CLIENTS &&
-		    dev->init_clients_state == MEI_START_MESSAGE) {
+		    dev->hbm_state == MEI_HBM_START) {
 			dev->init_clients_timer = 0;
 			mei_hbm_enum_clients_req(dev);
 		} else {
-			dev->recvd_msg = false;
-			dev_dbg(&dev->pdev->dev, "reset due to received hbm: host start\n");
+			dev_err(&dev->pdev->dev, "reset: wrong host start response\n");
 			mei_reset(dev, 1);
 			return;
 		}
 
-		dev->recvd_msg = true;
+		wake_up_interruptible(&dev->wait_recvd_msg);
 		dev_dbg(&dev->pdev->dev, "host start response message received.\n");
 		break;
 
@@ -591,23 +616,20 @@ void mei_hbm_dispatch(struct mei_device *dev, struct mei_msg_hdr *hdr)
 		me_client = &dev->me_clients[dev->me_client_presentation_num];
 
 		if (props_res->status || !dev->me_clients) {
-			dev_dbg(&dev->pdev->dev, "reset due to received host client properties response bus message wrong status.\n");
+			dev_err(&dev->pdev->dev, "reset: properties response hbm wrong status.\n");
 			mei_reset(dev, 1);
 			return;
 		}
 
 		if (me_client->client_id != props_res->address) {
-			dev_err(&dev->pdev->dev,
-				"Host client properties reply mismatch\n");
+			dev_err(&dev->pdev->dev, "reset: host properties response address mismatch\n");
 			mei_reset(dev, 1);
-
 			return;
 		}
 
 		if (dev->dev_state != MEI_DEV_INIT_CLIENTS ||
-		    dev->init_clients_state != MEI_CLIENT_PROPERTIES_MESSAGE) {
-			dev_err(&dev->pdev->dev,
-				"Unexpected client properties reply\n");
+		    dev->hbm_state != MEI_HBM_CLIENT_PROPERTIES) {
+			dev_err(&dev->pdev->dev, "reset: unexpected properties response\n");
 			mei_reset(dev, 1);
 
 			return;
@@ -626,26 +648,28 @@ void mei_hbm_dispatch(struct mei_device *dev, struct mei_msg_hdr *hdr)
 		enum_res = (struct hbm_host_enum_response *) mei_msg;
 		memcpy(dev->me_clients_map, enum_res->valid_addresses, 32);
 		if (dev->dev_state == MEI_DEV_INIT_CLIENTS &&
-		    dev->init_clients_state == MEI_ENUM_CLIENTS_MESSAGE) {
+		    dev->hbm_state == MEI_HBM_ENUM_CLIENTS) {
 				dev->init_clients_timer = 0;
 				dev->me_client_presentation_num = 0;
 				dev->me_client_index = 0;
 				mei_hbm_me_cl_allocate(dev);
-				dev->init_clients_state =
-					MEI_CLIENT_PROPERTIES_MESSAGE;
+				dev->hbm_state = MEI_HBM_CLIENT_PROPERTIES;
 
 				/* first property reqeust */
 				mei_hbm_prop_req(dev);
 		} else {
-			dev_dbg(&dev->pdev->dev, "reset due to received host enumeration clients response bus message.\n");
+			dev_err(&dev->pdev->dev, "reset: unexpected enumeration response hbm.\n");
 			mei_reset(dev, 1);
 			return;
 		}
 		break;
 
 	case HOST_STOP_RES_CMD:
+
+		if (dev->hbm_state != MEI_HBM_STOP)
+			dev_err(&dev->pdev->dev, "unexpected stop response hbm.\n");
 		dev->dev_state = MEI_DEV_DISABLED;
-		dev_dbg(&dev->pdev->dev, "resetting because of FW stop response.\n");
+		dev_info(&dev->pdev->dev, "reset: FW stop response.\n");
 		mei_reset(dev, 1);
 		break;
 
@@ -657,6 +681,7 @@ void mei_hbm_dispatch(struct mei_device *dev, struct mei_msg_hdr *hdr)
 
 	case ME_STOP_REQ_CMD:
 
+		dev->hbm_state = MEI_HBM_STOP;
 		mei_hbm_stop_req_prepare(dev, &dev->wr_ext_msg.hdr,
 					dev->wr_ext_msg.data);
 		break;
diff --git a/drivers/misc/mei/hbm.h b/drivers/misc/mei/hbm.h
index b552afbaf85..e80dc24ef3e 100644
--- a/drivers/misc/mei/hbm.h
+++ b/drivers/misc/mei/hbm.h
@@ -17,6 +17,27 @@
 #ifndef _MEI_HBM_H_
 #define _MEI_HBM_H_
 
+struct mei_device;
+struct mei_msg_hdr;
+struct mei_cl;
+
+/**
+ * enum mei_hbm_state - host bus message protocol state
+ *
+ * @MEI_HBM_IDLE : protocol not started
+ * @MEI_HBM_START : start request message was sent
+ * @MEI_HBM_ENUM_CLIENTS : enumeration request was sent
+ * @MEI_HBM_CLIENT_PROPERTIES : acquiring clients properties
+ */
+enum mei_hbm_state {
+	MEI_HBM_IDLE = 0,
+	MEI_HBM_START,
+	MEI_HBM_ENUM_CLIENTS,
+	MEI_HBM_CLIENT_PROPERTIES,
+	MEI_HBM_STARTED,
+	MEI_HBM_STOP,
+};
+
 void mei_hbm_dispatch(struct mei_device *dev, struct mei_msg_hdr *hdr);
 
 static inline void mei_hbm_hdr(struct mei_msg_hdr *hdr, size_t length)
@@ -28,8 +49,8 @@ static inline void mei_hbm_hdr(struct mei_msg_hdr *hdr, size_t length)
 	hdr->reserved = 0;
 }
 
-void mei_hbm_start_req(struct mei_device *dev);
-
+int mei_hbm_start_req(struct mei_device *dev);
+int mei_hbm_start_wait(struct mei_device *dev);
 int mei_hbm_cl_flow_control_req(struct mei_device *dev, struct mei_cl *cl);
 int mei_hbm_cl_disconnect_req(struct mei_device *dev, struct mei_cl *cl);
 int mei_hbm_cl_connect_req(struct mei_device *dev, struct mei_cl *cl);
diff --git a/drivers/misc/mei/hw-me.c b/drivers/misc/mei/hw-me.c
index 45ea7185c00..822170f0034 100644
--- a/drivers/misc/mei/hw-me.c
+++ b/drivers/misc/mei/hw-me.c
@@ -26,14 +26,14 @@
 
 
 /**
- * mei_reg_read - Reads 32bit data from the mei device
+ * mei_me_reg_read - Reads 32bit data from the mei device
  *
  * @dev: the device structure
  * @offset: offset from which to read the data
  *
  * returns register value (u32)
  */
-static inline u32 mei_reg_read(const struct mei_me_hw *hw,
+static inline u32 mei_me_reg_read(const struct mei_me_hw *hw,
 			       unsigned long offset)
 {
 	return ioread32(hw->mem_addr + offset);
@@ -41,20 +41,20 @@ static inline u32 mei_reg_read(const struct mei_me_hw *hw,
 
 
 /**
- * mei_reg_write - Writes 32bit data to the mei device
+ * mei_me_reg_write - Writes 32bit data to the mei device
  *
  * @dev: the device structure
  * @offset: offset from which to write the data
  * @value: register value to write (u32)
  */
-static inline void mei_reg_write(const struct mei_me_hw *hw,
+static inline void mei_me_reg_write(const struct mei_me_hw *hw,
 				 unsigned long offset, u32 value)
 {
 	iowrite32(value, hw->mem_addr + offset);
 }
 
 /**
- * mei_mecbrw_read - Reads 32bit data from ME circular buffer
+ * mei_me_mecbrw_read - Reads 32bit data from ME circular buffer
  *  read window register
  *
  * @dev: the device structure
@@ -63,18 +63,18 @@ static inline void mei_reg_write(const struct mei_me_hw *hw,
  */
 static u32 mei_me_mecbrw_read(const struct mei_device *dev)
 {
-	return mei_reg_read(to_me_hw(dev), ME_CB_RW);
+	return mei_me_reg_read(to_me_hw(dev), ME_CB_RW);
 }
 /**
- * mei_mecsr_read - Reads 32bit data from the ME CSR
+ * mei_me_mecsr_read - Reads 32bit data from the ME CSR
  *
  * @dev: the device structure
  *
  * returns ME_CSR_HA register value (u32)
  */
-static inline u32 mei_mecsr_read(const struct mei_me_hw *hw)
+static inline u32 mei_me_mecsr_read(const struct mei_me_hw *hw)
 {
-	return mei_reg_read(hw, ME_CSR_HA);
+	return mei_me_reg_read(hw, ME_CSR_HA);
 }
 
 /**
@@ -86,7 +86,7 @@ static inline u32 mei_mecsr_read(const struct mei_me_hw *hw)
  */
 static inline u32 mei_hcsr_read(const struct mei_me_hw *hw)
 {
-	return mei_reg_read(hw, H_CSR);
+	return mei_me_reg_read(hw, H_CSR);
 }
 
 /**
@@ -98,12 +98,12 @@ static inline u32 mei_hcsr_read(const struct mei_me_hw *hw)
 static inline void mei_hcsr_set(struct mei_me_hw *hw, u32 hcsr)
 {
 	hcsr &= ~H_IS;
-	mei_reg_write(hw, H_CSR, hcsr);
+	mei_me_reg_write(hw, H_CSR, hcsr);
 }
 
 
 /**
- * me_hw_config - configure hw dependent settings
+ * mei_me_hw_config - configure hw dependent settings
  *
  * @dev: mei device
  */
@@ -123,7 +123,7 @@ static void mei_me_intr_clear(struct mei_device *dev)
 	struct mei_me_hw *hw = to_me_hw(dev);
 	u32 hcsr = mei_hcsr_read(hw);
 	if ((hcsr & H_IS) == H_IS)
-		mei_reg_write(hw, H_CSR, hcsr);
+		mei_me_reg_write(hw, H_CSR, hcsr);
 }
 /**
  * mei_me_intr_enable - enables mei device interrupts
@@ -152,10 +152,24 @@ static void mei_me_intr_disable(struct mei_device *dev)
 }
 
 /**
+ * mei_me_hw_reset_release - release device from the reset
+ *
+ * @dev: the device structure
+ */
+static void mei_me_hw_reset_release(struct mei_device *dev)
+{
+	struct mei_me_hw *hw = to_me_hw(dev);
+	u32 hcsr = mei_hcsr_read(hw);
+
+	hcsr |= H_IG;
+	hcsr &= ~H_RST;
+	mei_hcsr_set(hw, hcsr);
+}
+/**
  * mei_me_hw_reset - resets fw via mei csr register.
  *
  * @dev: the device structure
- * @interrupts_enabled: if interrupt should be enabled after reset.
+ * @intr_enable: if interrupt should be enabled after reset.
  */
 static void mei_me_hw_reset(struct mei_device *dev, bool intr_enable)
 {
@@ -169,18 +183,14 @@ static void mei_me_hw_reset(struct mei_device *dev, bool intr_enable)
 	if (intr_enable)
 		hcsr |= H_IE;
 	else
-		hcsr &= ~H_IE;
-
-	mei_hcsr_set(hw, hcsr);
-
-	hcsr = mei_hcsr_read(hw) | H_IG;
-	hcsr &= ~H_RST;
+		hcsr |= ~H_IE;
 
 	mei_hcsr_set(hw, hcsr);
 
-	hcsr = mei_hcsr_read(hw);
+	if (dev->dev_state == MEI_DEV_POWER_DOWN)
+		mei_me_hw_reset_release(dev);
 
-	dev_dbg(&dev->pdev->dev, "current HCSR = 0x%08x.\n", hcsr);
+	dev_dbg(&dev->pdev->dev, "current HCSR = 0x%08x.\n", mei_hcsr_read(hw));
 }
 
 /**
@@ -218,10 +228,42 @@ static bool mei_me_host_is_ready(struct mei_device *dev)
 static bool mei_me_hw_is_ready(struct mei_device *dev)
 {
 	struct mei_me_hw *hw = to_me_hw(dev);
-	hw->me_hw_state = mei_mecsr_read(hw);
+	hw->me_hw_state = mei_me_mecsr_read(hw);
 	return (hw->me_hw_state & ME_RDY_HRA) == ME_RDY_HRA;
 }
 
+static int mei_me_hw_ready_wait(struct mei_device *dev)
+{
+	int err;
+	if (mei_me_hw_is_ready(dev))
+		return 0;
+
+	mutex_unlock(&dev->device_lock);
+	err = wait_event_interruptible_timeout(dev->wait_hw_ready,
+			dev->recvd_hw_ready, MEI_INTEROP_TIMEOUT);
+	mutex_lock(&dev->device_lock);
+	if (!err && !dev->recvd_hw_ready) {
+		dev_err(&dev->pdev->dev,
+			"wait hw ready failed. status = 0x%x\n", err);
+		return -ETIMEDOUT;
+	}
+
+	dev->recvd_hw_ready = false;
+	return 0;
+}
+
+static int mei_me_hw_start(struct mei_device *dev)
+{
+	int ret = mei_me_hw_ready_wait(dev);
+	if (ret)
+		return ret;
+	dev_dbg(&dev->pdev->dev, "hw is ready\n");
+
+	mei_me_host_set_ready(dev);
+	return ret;
+}
+
+
 /**
  * mei_hbuf_filled_slots - gets number of device filled buffer slots
  *
@@ -243,7 +285,7 @@ static unsigned char mei_hbuf_filled_slots(struct mei_device *dev)
 }
 
 /**
- * mei_hbuf_is_empty - checks if host buffer is empty.
+ * mei_me_hbuf_is_empty - checks if host buffer is empty.
  *
  * @dev: the device structure
  *
@@ -295,10 +337,11 @@ static int mei_me_write_message(struct mei_device *dev,
 			unsigned char *buf)
 {
 	struct mei_me_hw *hw = to_me_hw(dev);
-	unsigned long rem, dw_cnt;
+	unsigned long rem;
 	unsigned long length = header->length;
 	u32 *reg_buf = (u32 *)buf;
 	u32 hcsr;
+	u32 dw_cnt;
 	int i;
 	int empty_slots;
 
@@ -311,16 +354,16 @@ static int mei_me_write_message(struct mei_device *dev,
 	if (empty_slots < 0 || dw_cnt > empty_slots)
 		return -EIO;
 
-	mei_reg_write(hw, H_CB_WW, *((u32 *) header));
+	mei_me_reg_write(hw, H_CB_WW, *((u32 *) header));
 
 	for (i = 0; i < length / 4; i++)
-		mei_reg_write(hw, H_CB_WW, reg_buf[i]);
+		mei_me_reg_write(hw, H_CB_WW, reg_buf[i]);
 
 	rem = length & 0x3;
 	if (rem > 0) {
 		u32 reg = 0;
 		memcpy(&reg, &buf[length - rem], rem);
-		mei_reg_write(hw, H_CB_WW, reg);
+		mei_me_reg_write(hw, H_CB_WW, reg);
 	}
 
 	hcsr = mei_hcsr_read(hw) | H_IG;
@@ -344,7 +387,7 @@ static int mei_me_count_full_read_slots(struct mei_device *dev)
 	char read_ptr, write_ptr;
 	unsigned char buffer_depth, filled_slots;
 
-	hw->me_hw_state = mei_mecsr_read(hw);
+	hw->me_hw_state = mei_me_mecsr_read(hw);
 	buffer_depth = (unsigned char)((hw->me_hw_state & ME_CBD_HRA) >> 24);
 	read_ptr = (char) ((hw->me_hw_state & ME_CBRP_HRA) >> 8);
 	write_ptr = (char) ((hw->me_hw_state & ME_CBWP_HRA) >> 16);
@@ -404,7 +447,7 @@ irqreturn_t mei_me_irq_quick_handler(int irq, void *dev_id)
 		return IRQ_NONE;
 
 	/* clear H_IS bit in H_CSR */
-	mei_reg_write(hw, H_CSR, csr_reg);
+	mei_me_reg_write(hw, H_CSR, csr_reg);
 
 	return IRQ_WAKE_THREAD;
 }
@@ -423,12 +466,8 @@ irqreturn_t mei_me_irq_thread_handler(int irq, void *dev_id)
 {
 	struct mei_device *dev = (struct mei_device *) dev_id;
 	struct mei_cl_cb complete_list;
-	struct mei_cl_cb *cb_pos = NULL, *cb_next = NULL;
-	struct mei_cl *cl;
 	s32 slots;
 	int rets;
-	bool  bus_message_received;
-
 
 	dev_dbg(&dev->pdev->dev, "function called after ISR to handle the interrupt processing.\n");
 	/* initialize our complete list */
@@ -442,7 +481,7 @@ irqreturn_t mei_me_irq_thread_handler(int irq, void *dev_id)
 
 	/* check if ME wants a reset */
 	if (!mei_hw_is_ready(dev) &&
-	    dev->dev_state != MEI_DEV_RESETING &&
+	    dev->dev_state != MEI_DEV_RESETTING &&
 	    dev->dev_state != MEI_DEV_INITIALIZING) {
 		dev_dbg(&dev->pdev->dev, "FW not ready.\n");
 		mei_reset(dev, 1);
@@ -455,18 +494,14 @@ irqreturn_t mei_me_irq_thread_handler(int irq, void *dev_id)
 		if (mei_hw_is_ready(dev)) {
 			dev_dbg(&dev->pdev->dev, "we need to start the dev.\n");
 
-			mei_host_set_ready(dev);
-
-			dev_dbg(&dev->pdev->dev, "link is established start sending messages.\n");
-			/* link is established * start sending messages.  */
-
-			dev->dev_state = MEI_DEV_INIT_CLIENTS;
+			dev->recvd_hw_ready = true;
+			wake_up_interruptible(&dev->wait_hw_ready);
 
-			mei_hbm_start_req(dev);
 			mutex_unlock(&dev->device_lock);
 			return IRQ_HANDLED;
 		} else {
-			dev_dbg(&dev->pdev->dev, "FW not ready.\n");
+			dev_dbg(&dev->pdev->dev, "Reset Completed.\n");
+			mei_me_hw_reset_release(dev);
 			mutex_unlock(&dev->device_lock);
 			return IRQ_HANDLED;
 		}
@@ -488,44 +523,20 @@ end:
 	dev_dbg(&dev->pdev->dev, "end of bottom half function.\n");
 	dev->hbuf_is_ready = mei_hbuf_is_ready(dev);
 
-	bus_message_received = false;
-	if (dev->recvd_msg && waitqueue_active(&dev->wait_recvd_msg)) {
-		dev_dbg(&dev->pdev->dev, "received waiting bus message\n");
-		bus_message_received = true;
-	}
 	mutex_unlock(&dev->device_lock);
-	if (bus_message_received) {
-		dev_dbg(&dev->pdev->dev, "wake up dev->wait_recvd_msg\n");
-		wake_up_interruptible(&dev->wait_recvd_msg);
-		bus_message_received = false;
-	}
-	if (list_empty(&complete_list.list))
-		return IRQ_HANDLED;
 
+	mei_irq_compl_handler(dev, &complete_list);
 
-	list_for_each_entry_safe(cb_pos, cb_next, &complete_list.list, list) {
-		cl = cb_pos->cl;
-		list_del(&cb_pos->list);
-		if (cl) {
-			if (cl != &dev->iamthif_cl) {
-				dev_dbg(&dev->pdev->dev, "completing call back.\n");
-				mei_irq_complete_handler(cl, cb_pos);
-				cb_pos = NULL;
-			} else if (cl == &dev->iamthif_cl) {
-				mei_amthif_complete(dev, cb_pos);
-			}
-		}
-	}
 	return IRQ_HANDLED;
 }
 static const struct mei_hw_ops mei_me_hw_ops = {
 
-	.host_set_ready = mei_me_host_set_ready,
 	.host_is_ready = mei_me_host_is_ready,
 
 	.hw_is_ready = mei_me_hw_is_ready,
 	.hw_reset = mei_me_hw_reset,
-	.hw_config  = mei_me_hw_config,
+	.hw_config = mei_me_hw_config,
+	.hw_start = mei_me_hw_start,
 
 	.intr_clear = mei_me_intr_clear,
 	.intr_enable = mei_me_intr_enable,
@@ -543,7 +554,7 @@ static const struct mei_hw_ops mei_me_hw_ops = {
 };
 
 /**
- * init_mei_device - allocates and initializes the mei device structure
+ * mei_me_dev_init - allocates and initializes the mei device structure
  *
  * @pdev: The pci device structure
  *
@@ -560,14 +571,6 @@ struct mei_device *mei_me_dev_init(struct pci_dev *pdev)
 
 	mei_device_init(dev);
 
-	INIT_LIST_HEAD(&dev->wd_cl.link);
-	INIT_LIST_HEAD(&dev->iamthif_cl.link);
-	mei_io_list_init(&dev->amthif_cmd_list);
-	mei_io_list_init(&dev->amthif_rd_complete_list);
-
-	INIT_DELAYED_WORK(&dev->timer_work, mei_timer);
-	INIT_WORK(&dev->init_work, mei_host_client_init);
-
 	dev->ops = &mei_me_hw_ops;
 
 	dev->pdev = pdev;
diff --git a/drivers/misc/mei/hw-me.h b/drivers/misc/mei/hw-me.h
index 8518d3eeb83..80bd829fbd9 100644
--- a/drivers/misc/mei/hw-me.h
+++ b/drivers/misc/mei/hw-me.h
@@ -36,12 +36,6 @@ struct mei_me_hw {
 
 struct mei_device *mei_me_dev_init(struct pci_dev *pdev);
 
-/* get slots (dwords) from a message length + header (bytes) */
-static inline unsigned char mei_data2slots(size_t length)
-{
-	return DIV_ROUND_UP(sizeof(struct mei_msg_hdr) + length, 4);
-}
-
 irqreturn_t mei_me_irq_quick_handler(int irq, void *dev_id);
 irqreturn_t mei_me_irq_thread_handler(int irq, void *dev_id);
 
diff --git a/drivers/misc/mei/init.c b/drivers/misc/mei/init.c
index 6ec530168af..713d89fedc4 100644
--- a/drivers/misc/mei/init.c
+++ b/drivers/misc/mei/init.c
@@ -14,6 +14,7 @@
  *
  */
 
+#include <linux/export.h>
 #include <linux/pci.h>
 #include <linux/sched.h>
 #include <linux/wait.h>
@@ -22,6 +23,7 @@
 #include <linux/mei.h>
 
 #include "mei_dev.h"
+#include "hbm.h"
 #include "client.h"
 
 const char *mei_dev_state_str(int state)
@@ -31,9 +33,8 @@ const char *mei_dev_state_str(int state)
 	MEI_DEV_STATE(INITIALIZING);
 	MEI_DEV_STATE(INIT_CLIENTS);
 	MEI_DEV_STATE(ENABLED);
-	MEI_DEV_STATE(RESETING);
+	MEI_DEV_STATE(RESETTING);
 	MEI_DEV_STATE(DISABLED);
-	MEI_DEV_STATE(RECOVERING_FROM_RESET);
 	MEI_DEV_STATE(POWER_DOWN);
 	MEI_DEV_STATE(POWER_UP);
 	default:
@@ -46,7 +47,9 @@ void mei_device_init(struct mei_device *dev)
 {
 	/* setup our list array */
 	INIT_LIST_HEAD(&dev->file_list);
+	INIT_LIST_HEAD(&dev->device_list);
 	mutex_init(&dev->device_lock);
+	init_waitqueue_head(&dev->wait_hw_ready);
 	init_waitqueue_head(&dev->wait_recvd_msg);
 	init_waitqueue_head(&dev->wait_stop_wd);
 	dev->dev_state = MEI_DEV_INITIALIZING;
@@ -56,19 +59,27 @@ void mei_device_init(struct mei_device *dev)
 	mei_io_list_init(&dev->write_waiting_list);
 	mei_io_list_init(&dev->ctrl_wr_list);
 	mei_io_list_init(&dev->ctrl_rd_list);
+
+	INIT_DELAYED_WORK(&dev->timer_work, mei_timer);
+	INIT_WORK(&dev->init_work, mei_host_client_init);
+
+	INIT_LIST_HEAD(&dev->wd_cl.link);
+	INIT_LIST_HEAD(&dev->iamthif_cl.link);
+	mei_io_list_init(&dev->amthif_cmd_list);
+	mei_io_list_init(&dev->amthif_rd_complete_list);
+
 }
+EXPORT_SYMBOL_GPL(mei_device_init);
 
 /**
- * mei_hw_init - initializes host and fw to start work.
+ * mei_start - initializes host and fw to start work.
  *
  * @dev: the device structure
  *
  * returns 0 on success, <0 on failure.
  */
-int mei_hw_init(struct mei_device *dev)
+int mei_start(struct mei_device *dev)
 {
-	int ret = 0;
-
 	mutex_lock(&dev->device_lock);
 
 	/* acknowledge interrupt and stop interupts */
@@ -76,29 +87,15 @@ int mei_hw_init(struct mei_device *dev)
 
 	mei_hw_config(dev);
 
-	dev->recvd_msg = false;
 	dev_dbg(&dev->pdev->dev, "reset in start the mei device.\n");
 
 	mei_reset(dev, 1);
 
-	/* wait for ME to turn on ME_RDY */
-	if (!dev->recvd_msg) {
-		mutex_unlock(&dev->device_lock);
-		ret = wait_event_interruptible_timeout(dev->wait_recvd_msg,
-			dev->recvd_msg,
-			mei_secs_to_jiffies(MEI_INTEROP_TIMEOUT));
-		mutex_lock(&dev->device_lock);
-	}
-
-	if (ret <= 0 && !dev->recvd_msg) {
-		dev->dev_state = MEI_DEV_DISABLED;
-		dev_dbg(&dev->pdev->dev,
-			"wait_event_interruptible_timeout failed"
-			"on wait for ME to turn on ME_RDY.\n");
+	if (mei_hbm_start_wait(dev)) {
+		dev_err(&dev->pdev->dev, "HBM haven't started");
 		goto err;
 	}
 
-
 	if (!mei_host_is_ready(dev)) {
 		dev_err(&dev->pdev->dev, "host is not ready.\n");
 		goto err;
@@ -115,7 +112,6 @@ int mei_hw_init(struct mei_device *dev)
 		goto err;
 	}
 
-	dev->recvd_msg = false;
 	dev_dbg(&dev->pdev->dev, "link layer has been established.\n");
 
 	mutex_unlock(&dev->device_lock);
@@ -126,6 +122,7 @@ err:
 	mutex_unlock(&dev->device_lock);
 	return -ENODEV;
 }
+EXPORT_SYMBOL_GPL(mei_start);
 
 /**
  * mei_reset - resets host and fw.
@@ -137,9 +134,6 @@ void mei_reset(struct mei_device *dev, int interrupts_enabled)
 {
 	bool unexpected;
 
-	if (dev->dev_state == MEI_DEV_RECOVERING_FROM_RESET)
-		return;
-
 	unexpected = (dev->dev_state != MEI_DEV_INITIALIZING &&
 			dev->dev_state != MEI_DEV_DISABLED &&
 			dev->dev_state != MEI_DEV_POWER_DOWN &&
@@ -147,11 +141,12 @@ void mei_reset(struct mei_device *dev, int interrupts_enabled)
 
 	mei_hw_reset(dev, interrupts_enabled);
 
+	dev->hbm_state = MEI_HBM_IDLE;
 
 	if (dev->dev_state != MEI_DEV_INITIALIZING) {
 		if (dev->dev_state != MEI_DEV_DISABLED &&
 		    dev->dev_state != MEI_DEV_POWER_DOWN)
-			dev->dev_state = MEI_DEV_RESETING;
+			dev->dev_state = MEI_DEV_RESETTING;
 
 		mei_cl_all_disconnect(dev);
 
@@ -176,13 +171,50 @@ void mei_reset(struct mei_device *dev, int interrupts_enabled)
 		dev_warn(&dev->pdev->dev, "unexpected reset: dev_state = %s\n",
 			 mei_dev_state_str(dev->dev_state));
 
+	if (!interrupts_enabled) {
+		dev_dbg(&dev->pdev->dev, "intr not enabled end of reset\n");
+		return;
+	}
+
+	mei_hw_start(dev);
+
+	dev_dbg(&dev->pdev->dev, "link is established start sending messages.\n");
+	/* link is established * start sending messages.  */
+
+	dev->dev_state = MEI_DEV_INIT_CLIENTS;
+
+	mei_hbm_start_req(dev);
+
 	/* wake up all readings so they can be interrupted */
 	mei_cl_all_read_wakeup(dev);
 
 	/* remove all waiting requests */
 	mei_cl_all_write_clear(dev);
 }
+EXPORT_SYMBOL_GPL(mei_reset);
+
+void mei_stop(struct mei_device *dev)
+{
+	dev_dbg(&dev->pdev->dev, "stopping the device.\n");
+
+	mutex_lock(&dev->device_lock);
+
+	cancel_delayed_work(&dev->timer_work);
 
+	mei_wd_stop(dev);
+
+	mei_nfc_host_exit();
+
+	dev->dev_state = MEI_DEV_POWER_DOWN;
+	mei_reset(dev, 0);
+
+	mutex_unlock(&dev->device_lock);
+
+	flush_scheduled_work();
+
+	mei_watchdog_unregister(dev);
+}
+EXPORT_SYMBOL_GPL(mei_stop);
 
 
 
diff --git a/drivers/misc/mei/interrupt.c b/drivers/misc/mei/interrupt.c
index 3535b2676c9..2ad73698941 100644
--- a/drivers/misc/mei/interrupt.c
+++ b/drivers/misc/mei/interrupt.c
@@ -15,6 +15,7 @@
  */
 
 
+#include <linux/export.h>
 #include <linux/pci.h>
 #include <linux/kthread.h>
 #include <linux/interrupt.h>
@@ -30,103 +31,153 @@
 
 
 /**
- * mei_complete_handler - processes completed operation.
+ * mei_cl_complete_handler - processes completed operation for a client
  *
  * @cl: private data of the file object.
- * @cb_pos: callback block.
+ * @cb: callback block.
  */
-void mei_irq_complete_handler(struct mei_cl *cl, struct mei_cl_cb *cb_pos)
+static void mei_cl_complete_handler(struct mei_cl *cl, struct mei_cl_cb *cb)
 {
-	if (cb_pos->fop_type == MEI_FOP_WRITE) {
-		mei_io_cb_free(cb_pos);
-		cb_pos = NULL;
+	if (cb->fop_type == MEI_FOP_WRITE) {
+		mei_io_cb_free(cb);
+		cb = NULL;
 		cl->writing_state = MEI_WRITE_COMPLETE;
 		if (waitqueue_active(&cl->tx_wait))
 			wake_up_interruptible(&cl->tx_wait);
 
-	} else if (cb_pos->fop_type == MEI_FOP_READ &&
+	} else if (cb->fop_type == MEI_FOP_READ &&
 			MEI_READING == cl->reading_state) {
 		cl->reading_state = MEI_READ_COMPLETE;
 		if (waitqueue_active(&cl->rx_wait))
 			wake_up_interruptible(&cl->rx_wait);
+		else
+			mei_cl_bus_rx_event(cl);
+
+	}
+}
+
+/**
+ * mei_irq_compl_handler - dispatch complete handelers
+ *	for the completed callbacks
+ *
+ * @dev - mei device
+ * @compl_list - list of completed cbs
+ */
+void mei_irq_compl_handler(struct mei_device *dev, struct mei_cl_cb *compl_list)
+{
+	struct mei_cl_cb *cb, *next;
+	struct mei_cl *cl;
+
+	list_for_each_entry_safe(cb, next, &compl_list->list, list) {
+		cl = cb->cl;
+		list_del(&cb->list);
+		if (!cl)
+			continue;
 
+		dev_dbg(&dev->pdev->dev, "completing call back.\n");
+		if (cl == &dev->iamthif_cl)
+			mei_amthif_complete(dev, cb);
+		else
+			mei_cl_complete_handler(cl, cb);
 	}
 }
+EXPORT_SYMBOL_GPL(mei_irq_compl_handler);
 
 /**
- * _mei_irq_thread_state_ok - checks if mei header matches file private data
+ * mei_cl_hbm_equal - check if hbm is addressed to the client
  *
- * @cl: private data of the file object
+ * @cl: host client
  * @mei_hdr: header of mei client message
  *
- * returns !=0 if matches, 0 if no match.
+ * returns true if matches, false otherwise
+ */
+static inline int mei_cl_hbm_equal(struct mei_cl *cl,
+			struct mei_msg_hdr *mei_hdr)
+{
+	return cl->host_client_id == mei_hdr->host_addr &&
+		cl->me_client_id == mei_hdr->me_addr;
+}
+/**
+ * mei_cl_is_reading - checks if the client
+		is the one to read this message
+ *
+ * @cl: mei client
+ * @mei_hdr: header of mei message
+ *
+ * returns true on match and false otherwise
  */
-static int _mei_irq_thread_state_ok(struct mei_cl *cl,
-				struct mei_msg_hdr *mei_hdr)
+static bool mei_cl_is_reading(struct mei_cl *cl, struct mei_msg_hdr *mei_hdr)
 {
-	return (cl->host_client_id == mei_hdr->host_addr &&
-		cl->me_client_id == mei_hdr->me_addr &&
+	return mei_cl_hbm_equal(cl, mei_hdr) &&
 		cl->state == MEI_FILE_CONNECTED &&
-		MEI_READ_COMPLETE != cl->reading_state);
+		cl->reading_state != MEI_READ_COMPLETE;
 }
 
 /**
- * mei_irq_thread_read_client_message - bottom half read routine after ISR to
- * handle the read mei client message data processing.
+ * mei_irq_read_client_message - process client message
  *
- * @complete_list: An instance of our list structure
  * @dev: the device structure
  * @mei_hdr: header of mei client message
+ * @complete_list: An instance of our list structure
  *
  * returns 0 on success, <0 on failure.
  */
-static int mei_irq_thread_read_client_message(struct mei_cl_cb *complete_list,
-		struct mei_device *dev,
-		struct mei_msg_hdr *mei_hdr)
+static int mei_cl_irq_read_msg(struct mei_device *dev,
+			       struct mei_msg_hdr *mei_hdr,
+			       struct mei_cl_cb *complete_list)
 {
 	struct mei_cl *cl;
-	struct mei_cl_cb *cb_pos = NULL, *cb_next = NULL;
+	struct mei_cl_cb *cb, *next;
 	unsigned char *buffer = NULL;
 
-	dev_dbg(&dev->pdev->dev, "start client msg\n");
-	if (list_empty(&dev->read_list.list))
-		goto quit;
+	list_for_each_entry_safe(cb, next, &dev->read_list.list, list) {
+		cl = cb->cl;
+		if (!cl || !mei_cl_is_reading(cl, mei_hdr))
+			continue;
 
-	list_for_each_entry_safe(cb_pos, cb_next, &dev->read_list.list, list) {
-		cl = cb_pos->cl;
-		if (cl && _mei_irq_thread_state_ok(cl, mei_hdr)) {
-			cl->reading_state = MEI_READING;
-			buffer = cb_pos->response_buffer.data + cb_pos->buf_idx;
+		cl->reading_state = MEI_READING;
 
-			if (cb_pos->response_buffer.size <
-					mei_hdr->length + cb_pos->buf_idx) {
-				dev_dbg(&dev->pdev->dev, "message overflow.\n");
-				list_del(&cb_pos->list);
+		if (cb->response_buffer.size == 0 ||
+		    cb->response_buffer.data == NULL) {
+			dev_err(&dev->pdev->dev, "response buffer is not allocated.\n");
+			list_del(&cb->list);
+			return -ENOMEM;
+		}
+
+		if (cb->response_buffer.size < mei_hdr->length + cb->buf_idx) {
+			dev_dbg(&dev->pdev->dev, "message overflow. size %d len %d idx %ld\n",
+				cb->response_buffer.size,
+				mei_hdr->length, cb->buf_idx);
+			buffer = krealloc(cb->response_buffer.data,
+					  mei_hdr->length + cb->buf_idx,
+					  GFP_KERNEL);
+
+			if (!buffer) {
+				dev_err(&dev->pdev->dev, "allocation failed.\n");
+				list_del(&cb->list);
 				return -ENOMEM;
 			}
-			if (buffer)
-				mei_read_slots(dev, buffer, mei_hdr->length);
-
-			cb_pos->buf_idx += mei_hdr->length;
-			if (mei_hdr->msg_complete) {
-				cl->status = 0;
-				list_del(&cb_pos->list);
-				dev_dbg(&dev->pdev->dev,
-					"completed read H cl = %d, ME cl = %d, length = %lu\n",
-					cl->host_client_id,
-					cl->me_client_id,
-					cb_pos->buf_idx);
-
-				list_add_tail(&cb_pos->list,
-						&complete_list->list);
-			}
-
-			break;
+			cb->response_buffer.data = buffer;
+			cb->response_buffer.size =
+				mei_hdr->length + cb->buf_idx;
 		}
 
+		buffer = cb->response_buffer.data + cb->buf_idx;
+		mei_read_slots(dev, buffer, mei_hdr->length);
+
+		cb->buf_idx += mei_hdr->length;
+		if (mei_hdr->msg_complete) {
+			cl->status = 0;
+			list_del(&cb->list);
+			dev_dbg(&dev->pdev->dev, "completed read H cl = %d, ME cl = %d, length = %lu\n",
+				cl->host_client_id,
+				cl->me_client_id,
+				cb->buf_idx);
+			list_add_tail(&cb->list, &complete_list->list);
+		}
+		break;
 	}
 
-quit:
 	dev_dbg(&dev->pdev->dev, "message read\n");
 	if (!buffer) {
 		mei_read_slots(dev, dev->rd_msg_buf, mei_hdr->length);
@@ -153,31 +204,33 @@ static int _mei_irq_thread_close(struct mei_device *dev, s32 *slots,
 				struct mei_cl *cl,
 				struct mei_cl_cb *cmpl_list)
 {
-	if ((*slots * sizeof(u32)) < (sizeof(struct mei_msg_hdr) +
-			sizeof(struct hbm_client_connect_request)))
-		return -EBADMSG;
+	u32 msg_slots =
+		mei_data2slots(sizeof(struct hbm_client_connect_request));
 
-	*slots -= mei_data2slots(sizeof(struct hbm_client_connect_request));
+	if (*slots < msg_slots)
+		return -EMSGSIZE;
+
+	*slots -= msg_slots;
 
 	if (mei_hbm_cl_disconnect_req(dev, cl)) {
 		cl->status = 0;
 		cb_pos->buf_idx = 0;
 		list_move_tail(&cb_pos->list, &cmpl_list->list);
-		return -EMSGSIZE;
-	} else {
-		cl->state = MEI_FILE_DISCONNECTING;
-		cl->status = 0;
-		cb_pos->buf_idx = 0;
-		list_move_tail(&cb_pos->list, &dev->ctrl_rd_list.list);
-		cl->timer_count = MEI_CONNECT_TIMEOUT;
+		return -EIO;
 	}
 
+	cl->state = MEI_FILE_DISCONNECTING;
+	cl->status = 0;
+	cb_pos->buf_idx = 0;
+	list_move_tail(&cb_pos->list, &dev->ctrl_rd_list.list);
+	cl->timer_count = MEI_CONNECT_TIMEOUT;
+
 	return 0;
 }
 
 
 /**
- * _mei_hb_read - processes read related operation.
+ * _mei_irq_thread_read - processes read related operation.
  *
  * @dev: the device structure.
  * @slots: free slots.
@@ -192,14 +245,15 @@ static int _mei_irq_thread_read(struct mei_device *dev,	s32 *slots,
 			struct mei_cl *cl,
 			struct mei_cl_cb *cmpl_list)
 {
-	if ((*slots * sizeof(u32)) < (sizeof(struct mei_msg_hdr) +
-			sizeof(struct hbm_flow_control))) {
+	u32 msg_slots = mei_data2slots(sizeof(struct hbm_flow_control));
+
+	if (*slots < msg_slots) {
 		/* return the cancel routine */
 		list_del(&cb_pos->list);
-		return -EBADMSG;
+		return -EMSGSIZE;
 	}
 
-	*slots -= mei_data2slots(sizeof(struct hbm_flow_control));
+	*slots -= msg_slots;
 
 	if (mei_hbm_cl_flow_control_req(dev, cl)) {
 		cl->status = -ENODEV;
@@ -229,15 +283,19 @@ static int _mei_irq_thread_ioctl(struct mei_device *dev, s32 *slots,
 			struct mei_cl *cl,
 			struct mei_cl_cb *cmpl_list)
 {
-	if ((*slots * sizeof(u32)) < (sizeof(struct mei_msg_hdr) +
-			sizeof(struct hbm_client_connect_request))) {
+	u32 msg_slots =
+		mei_data2slots(sizeof(struct hbm_client_connect_request));
+
+	if (*slots < msg_slots) {
 		/* return the cancel routine */
 		list_del(&cb_pos->list);
-		return -EBADMSG;
+		return -EMSGSIZE;
 	}
 
+	*slots -=  msg_slots;
+
 	cl->state = MEI_FILE_CONNECTING;
-	*slots -= mei_data2slots(sizeof(struct hbm_client_connect_request));
+
 	if (mei_hbm_cl_connect_req(dev, cl)) {
 		cl->status = -ENODEV;
 		cb_pos->buf_idx = 0;
@@ -266,7 +324,7 @@ static int mei_irq_thread_write_complete(struct mei_device *dev, s32 *slots,
 	struct mei_msg_hdr mei_hdr;
 	struct mei_cl *cl = cb->cl;
 	size_t len = cb->request_buffer.size - cb->buf_idx;
-	size_t msg_slots = mei_data2slots(len);
+	u32 msg_slots = mei_data2slots(len);
 
 	mei_hdr.host_addr = cl->host_client_id;
 	mei_hdr.me_addr = cl->me_client_id;
@@ -298,19 +356,20 @@ static int mei_irq_thread_write_complete(struct mei_device *dev, s32 *slots,
 		return -ENODEV;
 	}
 
-	if (mei_cl_flow_ctrl_reduce(cl))
-		return -ENODEV;
 
 	cl->status = 0;
 	cb->buf_idx += mei_hdr.length;
-	if (mei_hdr.msg_complete)
+	if (mei_hdr.msg_complete) {
+		if (mei_cl_flow_ctrl_reduce(cl))
+			return -ENODEV;
 		list_move_tail(&cb->list, &dev->write_waiting_list.list);
+	}
 
 	return 0;
 }
 
 /**
- * mei_irq_thread_read_handler - bottom half read routine after ISR to
+ * mei_irq_read_handler - bottom half read routine after ISR to
  * handle the read processing.
  *
  * @dev: the device structure
@@ -350,8 +409,7 @@ int mei_irq_read_handler(struct mei_device *dev,
 					" client = %d, ME client = %d\n",
 					cl_pos->host_client_id,
 					cl_pos->me_client_id);
-			if (cl_pos->host_client_id == mei_hdr->host_addr &&
-			    cl_pos->me_client_id == mei_hdr->me_addr)
+			if (mei_cl_hbm_equal(cl_pos, mei_hdr))
 				break;
 		}
 
@@ -362,7 +420,7 @@ int mei_irq_read_handler(struct mei_device *dev,
 		}
 	}
 	if (((*slots) * sizeof(u32)) < mei_hdr->length) {
-		dev_dbg(&dev->pdev->dev,
+		dev_err(&dev->pdev->dev,
 				"we can't read the message slots =%08x.\n",
 				*slots);
 		/* we can't read the message */
@@ -378,20 +436,19 @@ int mei_irq_read_handler(struct mei_device *dev,
 	} else if (mei_hdr->host_addr == dev->iamthif_cl.host_client_id &&
 		   (MEI_FILE_CONNECTED == dev->iamthif_cl.state) &&
 		   (dev->iamthif_state == MEI_IAMTHIF_READING)) {
-		dev_dbg(&dev->pdev->dev, "call mei_irq_thread_read_iamthif_message.\n");
 
+		dev_dbg(&dev->pdev->dev, "call mei_irq_thread_read_iamthif_message.\n");
 		dev_dbg(&dev->pdev->dev, MEI_HDR_FMT, MEI_HDR_PRM(mei_hdr));
 
-		ret = mei_amthif_irq_read_message(cmpl_list, dev, mei_hdr);
+		ret = mei_amthif_irq_read_msg(dev, mei_hdr, cmpl_list);
 		if (ret)
 			goto end;
 	} else {
-		dev_dbg(&dev->pdev->dev, "call mei_irq_thread_read_client_message.\n");
-		ret = mei_irq_thread_read_client_message(cmpl_list,
-							 dev, mei_hdr);
+		dev_dbg(&dev->pdev->dev, "call mei_cl_irq_read_msg.\n");
+		dev_dbg(&dev->pdev->dev, MEI_HDR_FMT, MEI_HDR_PRM(mei_hdr));
+		ret = mei_cl_irq_read_msg(dev, mei_hdr, cmpl_list);
 		if (ret)
 			goto end;
-
 	}
 
 	/* reset the number of slots and header */
@@ -400,7 +457,7 @@ int mei_irq_read_handler(struct mei_device *dev,
 
 	if (*slots == -EOVERFLOW) {
 		/* overflow - reset */
-		dev_dbg(&dev->pdev->dev, "resetting due to slots overflow.\n");
+		dev_err(&dev->pdev->dev, "resetting due to slots overflow.\n");
 		/* set the event since message has been read */
 		ret = -ERANGE;
 		goto end;
@@ -408,6 +465,7 @@ int mei_irq_read_handler(struct mei_device *dev,
 end:
 	return ret;
 }
+EXPORT_SYMBOL_GPL(mei_irq_read_handler);
 
 
 /**
@@ -419,8 +477,7 @@ end:
  *
  * returns 0 on success, <0 on failure.
  */
-int mei_irq_write_handler(struct mei_device *dev,
-				struct mei_cl_cb *cmpl_list)
+int mei_irq_write_handler(struct mei_device *dev, struct mei_cl_cb *cmpl_list)
 {
 
 	struct mei_cl *cl;
@@ -559,6 +616,7 @@ int mei_irq_write_handler(struct mei_device *dev,
 	}
 	return 0;
 }
+EXPORT_SYMBOL_GPL(mei_irq_write_handler);
 
 
 
@@ -586,8 +644,8 @@ void mei_timer(struct work_struct *work)
 		if (dev->dev_state == MEI_DEV_INIT_CLIENTS) {
 			if (dev->init_clients_timer) {
 				if (--dev->init_clients_timer == 0) {
-					dev_dbg(&dev->pdev->dev, "IMEI reset due to init clients timeout ,init clients state = %d.\n",
-						dev->init_clients_state);
+					dev_err(&dev->pdev->dev, "reset: init clients timeout hbm_state = %d.\n",
+						dev->hbm_state);
 					mei_reset(dev, 1);
 				}
 			}
@@ -598,7 +656,7 @@ void mei_timer(struct work_struct *work)
 	list_for_each_entry_safe(cl_pos, cl_next, &dev->file_list, link) {
 		if (cl_pos->timer_count) {
 			if (--cl_pos->timer_count == 0) {
-				dev_dbg(&dev->pdev->dev, "HECI reset due to connect/disconnect timeout.\n");
+				dev_err(&dev->pdev->dev, "reset: connect/disconnect timeout.\n");
 				mei_reset(dev, 1);
 				goto out;
 			}
@@ -607,7 +665,7 @@ void mei_timer(struct work_struct *work)
 
 	if (dev->iamthif_stall_timer) {
 		if (--dev->iamthif_stall_timer == 0) {
-			dev_dbg(&dev->pdev->dev, "resetting because of hang to amthi.\n");
+			dev_err(&dev->pdev->dev, "reset: amthif  hanged.\n");
 			mei_reset(dev, 1);
 			dev->iamthif_msg_buf_size = 0;
 			dev->iamthif_msg_buf_index = 0;
diff --git a/drivers/misc/mei/main.c b/drivers/misc/mei/main.c
index 903f809b21f..7c44c8dbae4 100644
--- a/drivers/misc/mei/main.c
+++ b/drivers/misc/mei/main.c
@@ -48,7 +48,7 @@
  *
  * @inode: pointer to inode structure
  * @file: pointer to file structure
- *
+ e
  * returns 0 on success, <0 on error
  */
 static int mei_open(struct inode *inode, struct file *file)
@@ -244,7 +244,7 @@ static ssize_t mei_read(struct file *file, char __user *ubuf,
 		goto out;
 	}
 
-	err = mei_cl_read_start(cl);
+	err = mei_cl_read_start(cl, length);
 	if (err && err != -EBUSY) {
 		dev_dbg(&dev->pdev->dev,
 			"mei start read failure with status = %d\n", err);
@@ -292,9 +292,8 @@ static ssize_t mei_read(struct file *file, char __user *ubuf,
 	}
 	/* now copy the data to user space */
 copy_buffer:
-	dev_dbg(&dev->pdev->dev, "cb->response_buffer size - %d\n",
-	    cb->response_buffer.size);
-	dev_dbg(&dev->pdev->dev, "cb->buf_idx - %lu\n", cb->buf_idx);
+	dev_dbg(&dev->pdev->dev, "buf.size = %d buf.idx= %ld\n",
+	    cb->response_buffer.size, cb->buf_idx);
 	if (length == 0 || ubuf == NULL || *offset > cb->buf_idx) {
 		rets = -EMSGSIZE;
 		goto free;
@@ -342,11 +341,10 @@ static ssize_t mei_write(struct file *file, const char __user *ubuf,
 {
 	struct mei_cl *cl = file->private_data;
 	struct mei_cl_cb *write_cb = NULL;
-	struct mei_msg_hdr mei_hdr;
 	struct mei_device *dev;
 	unsigned long timeout = 0;
 	int rets;
-	int i;
+	int id;
 
 	if (WARN_ON(!cl || !cl->dev))
 		return -ENODEV;
@@ -357,24 +355,24 @@ static ssize_t mei_write(struct file *file, const char __user *ubuf,
 
 	if (dev->dev_state != MEI_DEV_ENABLED) {
 		rets = -ENODEV;
-		goto err;
+		goto out;
 	}
 
-	i = mei_me_cl_by_id(dev, cl->me_client_id);
-	if (i < 0) {
+	id = mei_me_cl_by_id(dev, cl->me_client_id);
+	if (id < 0) {
 		rets = -ENODEV;
-		goto err;
+		goto out;
 	}
-	if (length > dev->me_clients[i].props.max_msg_length || length <= 0) {
+	if (length > dev->me_clients[id].props.max_msg_length || length <= 0) {
 		rets = -EMSGSIZE;
-		goto err;
+		goto out;
 	}
 
 	if (cl->state != MEI_FILE_CONNECTED) {
-		rets = -ENODEV;
 		dev_err(&dev->pdev->dev, "host client = %d,  is not connected to ME client = %d",
 			cl->host_client_id, cl->me_client_id);
-		goto err;
+		rets = -ENODEV;
+		goto out;
 	}
 	if (cl == &dev->iamthif_cl) {
 		write_cb = mei_amthif_find_read_list_entry(dev, file);
@@ -412,17 +410,15 @@ static ssize_t mei_write(struct file *file, const char __user *ubuf,
 	if (!write_cb) {
 		dev_err(&dev->pdev->dev, "write cb allocation failed\n");
 		rets = -ENOMEM;
-		goto err;
+		goto out;
 	}
 	rets = mei_io_cb_alloc_req_buf(write_cb, length);
 	if (rets)
-		goto err;
-
-	dev_dbg(&dev->pdev->dev, "cb request size = %zd\n", length);
+		goto out;
 
 	rets = copy_from_user(write_cb->request_buffer.data, ubuf, length);
 	if (rets)
-		goto err;
+		goto out;
 
 	cl->sm_state = 0;
 	if (length == 4 &&
@@ -440,65 +436,17 @@ static ssize_t mei_write(struct file *file, const char __user *ubuf,
 		if (rets) {
 			dev_err(&dev->pdev->dev,
 				"amthif write failed with status = %d\n", rets);
-			goto err;
+			goto out;
 		}
 		mutex_unlock(&dev->device_lock);
 		return length;
 	}
 
-	write_cb->fop_type = MEI_FOP_WRITE;
-
-	dev_dbg(&dev->pdev->dev, "host client = %d, ME client = %d\n",
-	    cl->host_client_id, cl->me_client_id);
-	rets = mei_cl_flow_ctrl_creds(cl);
-	if (rets < 0)
-		goto err;
-
-	if (rets == 0 || !dev->hbuf_is_ready) {
-		write_cb->buf_idx = 0;
-		mei_hdr.msg_complete = 0;
-		cl->writing_state = MEI_WRITING;
-		goto out;
-	}
-
-	dev->hbuf_is_ready = false;
-	if (length >  mei_hbuf_max_len(dev)) {
-		mei_hdr.length = mei_hbuf_max_len(dev);
-		mei_hdr.msg_complete = 0;
-	} else {
-		mei_hdr.length = length;
-		mei_hdr.msg_complete = 1;
-	}
-	mei_hdr.host_addr = cl->host_client_id;
-	mei_hdr.me_addr = cl->me_client_id;
-	mei_hdr.reserved = 0;
-
-	dev_dbg(&dev->pdev->dev, "write " MEI_HDR_FMT "\n",
-		MEI_HDR_PRM(&mei_hdr));
-	if (mei_write_message(dev, &mei_hdr, write_cb->request_buffer.data)) {
-		rets = -ENODEV;
-		goto err;
-	}
-	cl->writing_state = MEI_WRITING;
-	write_cb->buf_idx = mei_hdr.length;
-
+	rets = mei_cl_write(cl, write_cb, false);
 out:
-	if (mei_hdr.msg_complete) {
-		if (mei_cl_flow_ctrl_reduce(cl)) {
-			rets = -ENODEV;
-			goto err;
-		}
-		list_add_tail(&write_cb->list, &dev->write_waiting_list.list);
-	} else {
-		list_add_tail(&write_cb->list, &dev->write_list.list);
-	}
-
 	mutex_unlock(&dev->device_lock);
-	return length;
-
-err:
-	mutex_unlock(&dev->device_lock);
-	mei_io_cb_free(write_cb);
+	if (rets < 0)
+		mei_io_cb_free(write_cb);
 	return rets;
 }
 
@@ -753,17 +701,44 @@ static struct miscdevice  mei_misc_device = {
 		.minor = MISC_DYNAMIC_MINOR,
 };
 
-int mei_register(struct device *dev)
+
+int mei_register(struct mei_device *dev)
 {
-	mei_misc_device.parent = dev;
-	return misc_register(&mei_misc_device);
+	int ret;
+	mei_misc_device.parent = &dev->pdev->dev;
+	ret = misc_register(&mei_misc_device);
+	if (ret)
+		return ret;
+
+	if (mei_dbgfs_register(dev, mei_misc_device.name))
+		dev_err(&dev->pdev->dev, "cannot register debugfs\n");
+
+	return 0;
 }
+EXPORT_SYMBOL_GPL(mei_register);
 
-void mei_deregister(void)
+void mei_deregister(struct mei_device *dev)
 {
+	mei_dbgfs_deregister(dev);
 	misc_deregister(&mei_misc_device);
 	mei_misc_device.parent = NULL;
 }
+EXPORT_SYMBOL_GPL(mei_deregister);
+
+static int __init mei_init(void)
+{
+	return mei_cl_bus_init();
+}
+
+static void __exit mei_exit(void)
+{
+	mei_cl_bus_exit();
+}
+
+module_init(mei_init);
+module_exit(mei_exit);
 
+MODULE_AUTHOR("Intel Corporation");
+MODULE_DESCRIPTION("Intel(R) Management Engine Interface");
 MODULE_LICENSE("GPL v2");
 
diff --git a/drivers/misc/mei/mei_dev.h b/drivers/misc/mei/mei_dev.h
index cb80166161f..4de5140e737 100644
--- a/drivers/misc/mei/mei_dev.h
+++ b/drivers/misc/mei/mei_dev.h
@@ -21,9 +21,11 @@
 #include <linux/watchdog.h>
 #include <linux/poll.h>
 #include <linux/mei.h>
+#include <linux/mei_cl_bus.h>
 
 #include "hw.h"
 #include "hw-me-regs.h"
+#include "hbm.h"
 
 /*
  * watch dog definition
@@ -95,22 +97,14 @@ enum mei_dev_state {
 	MEI_DEV_INITIALIZING = 0,
 	MEI_DEV_INIT_CLIENTS,
 	MEI_DEV_ENABLED,
-	MEI_DEV_RESETING,
+	MEI_DEV_RESETTING,
 	MEI_DEV_DISABLED,
-	MEI_DEV_RECOVERING_FROM_RESET,
 	MEI_DEV_POWER_DOWN,
 	MEI_DEV_POWER_UP
 };
 
 const char *mei_dev_state_str(int state);
 
-/* init clients states*/
-enum mei_init_clients_states {
-	MEI_START_MESSAGE = 0,
-	MEI_ENUM_CLIENTS_MESSAGE,
-	MEI_CLIENT_PROPERTIES_MESSAGE
-};
-
 enum iamthif_states {
 	MEI_IAMTHIF_IDLE,
 	MEI_IAMTHIF_WRITING,
@@ -153,7 +147,7 @@ enum mei_cb_file_ops {
 /*
  * Intel MEI message data struct
  */
-struct mei_message_data {
+struct mei_msg_data {
 	u32 size;
 	unsigned char *data;
 };
@@ -184,8 +178,8 @@ struct mei_cl_cb {
 	struct list_head list;
 	struct mei_cl *cl;
 	enum mei_cb_file_ops fop_type;
-	struct mei_message_data request_buffer;
-	struct mei_message_data response_buffer;
+	struct mei_msg_data request_buffer;
+	struct mei_msg_data response_buffer;
 	unsigned long buf_idx;
 	unsigned long read_time;
 	struct file *file_object;
@@ -209,15 +203,20 @@ struct mei_cl {
 	enum mei_file_transaction_states writing_state;
 	int sm_state;
 	struct mei_cl_cb *read_cb;
+
+	/* MEI CL bus data */
+	struct mei_cl_device *device;
+	struct list_head device_link;
+	uuid_le device_uuid;
 };
 
 /** struct mei_hw_ops
  *
- * @host_set_ready   - notify FW that host side is ready
  * @host_is_ready    - query for host readiness
 
  * @hw_is_ready      - query if hw is ready
  * @hw_reset         - reset hw
+ * @hw_start         - start hw after reset
  * @hw_config        - configure hw
 
  * @intr_clear       - clear pending interrupts
@@ -237,11 +236,11 @@ struct mei_cl {
  */
 struct mei_hw_ops {
 
-	void (*host_set_ready) (struct mei_device *dev);
 	bool (*host_is_ready) (struct mei_device *dev);
 
 	bool (*hw_is_ready) (struct mei_device *dev);
 	void (*hw_reset) (struct mei_device *dev, bool enable);
+	int  (*hw_start) (struct mei_device *dev);
 	void (*hw_config) (struct mei_device *dev);
 
 	void (*intr_clear) (struct mei_device *dev);
@@ -263,9 +262,77 @@ struct mei_hw_ops {
 		     unsigned char *buf, unsigned long len);
 };
 
+/* MEI bus API*/
+
+/**
+ * struct mei_cl_ops - MEI CL device ops
+ * This structure allows ME host clients to implement technology
+ * specific operations.
+ *
+ * @enable: Enable an MEI CL device. Some devices require specific
+ *	HECI commands to initialize completely.
+ * @disable: Disable an MEI CL device.
+ * @send: Tx hook for the device. This allows ME host clients to trap
+ *	the device driver buffers before actually physically
+ *	pushing it to the ME.
+ * @recv: Rx hook for the device. This allows ME host clients to trap the
+ *	ME buffers before forwarding them to the device driver.
+ */
+struct mei_cl_ops {
+	int (*enable)(struct mei_cl_device *device);
+	int (*disable)(struct mei_cl_device *device);
+	int (*send)(struct mei_cl_device *device, u8 *buf, size_t length);
+	int (*recv)(struct mei_cl_device *device, u8 *buf, size_t length);
+};
+
+struct mei_cl_device *mei_cl_add_device(struct mei_device *dev,
+					uuid_le uuid, char *name,
+					struct mei_cl_ops *ops);
+void mei_cl_remove_device(struct mei_cl_device *device);
+
+int __mei_cl_async_send(struct mei_cl *cl, u8 *buf, size_t length);
+int __mei_cl_send(struct mei_cl *cl, u8 *buf, size_t length);
+int __mei_cl_recv(struct mei_cl *cl, u8 *buf, size_t length);
+void mei_cl_bus_rx_event(struct mei_cl *cl);
+int mei_cl_bus_init(void);
+void mei_cl_bus_exit(void);
+
+
+/**
+ * struct mei_cl_device - MEI device handle
+ * An mei_cl_device pointer is returned from mei_add_device()
+ * and links MEI bus clients to their actual ME host client pointer.
+ * Drivers for MEI devices will get an mei_cl_device pointer
+ * when being probed and shall use it for doing ME bus I/O.
+ *
+ * @dev: linux driver model device pointer
+ * @uuid: me client uuid
+ * @cl: mei client
+ * @ops: ME transport ops
+ * @event_cb: Drivers register this callback to get asynchronous ME
+ *	events (e.g. Rx buffer pending) notifications.
+ * @events: Events bitmask sent to the driver.
+ * @priv_data: client private data
+ */
+struct mei_cl_device {
+	struct device dev;
+
+	struct mei_cl *cl;
+
+	const struct mei_cl_ops *ops;
+
+	struct work_struct event_work;
+	mei_cl_event_cb_t event_cb;
+	void *event_context;
+	unsigned long events;
+
+	void *priv_data;
+};
+
 /**
  * struct mei_device -  MEI private device struct
 
+ * @hbm_state - state of host bus message protocol
  * @mem_addr - mem mapped base register address
 
  * @hbuf_depth - depth of hardware host/write buffer is slots
@@ -296,11 +363,12 @@ struct mei_device {
 	 */
 	struct mutex device_lock; /* device lock */
 	struct delayed_work timer_work;	/* MEI timer delayed work (timeouts) */
-	bool recvd_msg;
 
+	bool recvd_hw_ready;
 	/*
 	 * waiting queue for receive message from FW
 	 */
+	wait_queue_head_t wait_hw_ready;
 	wait_queue_head_t wait_recvd_msg;
 	wait_queue_head_t wait_stop_wd;
 
@@ -308,7 +376,7 @@ struct mei_device {
 	 * mei device  states
 	 */
 	enum mei_dev_state dev_state;
-	enum mei_init_clients_states init_clients_state;
+	enum mei_hbm_state hbm_state;
 	u16 init_clients_timer;
 
 	unsigned char rd_msg_buf[MEI_RD_MSG_BUF_SIZE];	/* control messages */
@@ -365,6 +433,14 @@ struct mei_device {
 
 	struct work_struct init_work;
 
+	/* List of bus devices */
+	struct list_head device_list;
+
+#if IS_ENABLED(CONFIG_DEBUG_FS)
+	struct dentry *dbgfs_dir;
+#endif /* CONFIG_DEBUG_FS */
+
+
 	const struct mei_hw_ops *ops;
 	char hw[0] __aligned(sizeof(void *));
 };
@@ -374,13 +450,24 @@ static inline unsigned long mei_secs_to_jiffies(unsigned long sec)
 	return msecs_to_jiffies(sec * MSEC_PER_SEC);
 }
 
+/**
+ * mei_data2slots - get slots - number of (dwords) from a message length
+ *	+ size of the mei header
+ * @length - size of the messages in bytes
+ * returns  - number of slots
+ */
+static inline u32 mei_data2slots(size_t length)
+{
+	return DIV_ROUND_UP(sizeof(struct mei_msg_hdr) + length, 4);
+}
 
 /*
  * mei init function prototypes
  */
 void mei_device_init(struct mei_device *dev);
 void mei_reset(struct mei_device *dev, int interrupts);
-int mei_hw_init(struct mei_device *dev);
+int mei_start(struct mei_device *dev);
+void mei_stop(struct mei_device *dev);
 
 /*
  *  MEI interrupt functions prototype
@@ -391,8 +478,7 @@ int mei_irq_read_handler(struct mei_device *dev,
 		struct mei_cl_cb *cmpl_list, s32 *slots);
 
 int mei_irq_write_handler(struct mei_device *dev, struct mei_cl_cb *cmpl_list);
-
-void mei_irq_complete_handler(struct mei_cl *cl, struct mei_cl_cb *cb_pos);
+void mei_irq_compl_handler(struct mei_device *dev, struct mei_cl_cb *cmpl_list);
 
 /*
  * AMTHIF - AMT Host Interface Functions
@@ -416,6 +502,25 @@ struct mei_cl_cb *mei_amthif_find_read_list_entry(struct mei_device *dev,
 
 void mei_amthif_run_next_cmd(struct mei_device *dev);
 
+int mei_amthif_irq_write_complete(struct mei_device *dev, s32 *slots,
+			struct mei_cl_cb *cb, struct mei_cl_cb *cmpl_list);
+
+void mei_amthif_complete(struct mei_device *dev, struct mei_cl_cb *cb);
+int mei_amthif_irq_read_msg(struct mei_device *dev,
+			    struct mei_msg_hdr *mei_hdr,
+			    struct mei_cl_cb *complete_list);
+int mei_amthif_irq_read(struct mei_device *dev, s32 *slots);
+
+/*
+ * NFC functions
+ */
+int mei_nfc_host_init(struct mei_device *dev);
+void mei_nfc_host_exit(void);
+
+/*
+ * NFC Client UUID
+ */
+extern const uuid_le mei_nfc_guid;
 
 int mei_amthif_irq_write_complete(struct mei_device *dev, s32 *slots,
 			struct mei_cl_cb *cb, struct mei_cl_cb *cmpl_list);
@@ -454,6 +559,11 @@ static inline void mei_hw_reset(struct mei_device *dev, bool enable)
 	dev->ops->hw_reset(dev, enable);
 }
 
+static inline void mei_hw_start(struct mei_device *dev)
+{
+	dev->ops->hw_start(dev);
+}
+
 static inline void mei_clear_interrupts(struct mei_device *dev)
 {
 	dev->ops->intr_clear(dev);
@@ -469,10 +579,6 @@ static inline void mei_disable_interrupts(struct mei_device *dev)
 	dev->ops->intr_disable(dev);
 }
 
-static inline void mei_host_set_ready(struct mei_device *dev)
-{
-	dev->ops->host_set_ready(dev);
-}
 static inline bool mei_host_is_ready(struct mei_device *dev)
 {
 	return dev->ops->host_is_ready(dev);
@@ -520,8 +626,19 @@ static inline int mei_count_full_read_slots(struct mei_device *dev)
 	return dev->ops->rdbuf_full_slots(dev);
 }
 
-int mei_register(struct device *dev);
-void mei_deregister(void);
+#if IS_ENABLED(CONFIG_DEBUG_FS)
+int mei_dbgfs_register(struct mei_device *dev, const char *name);
+void mei_dbgfs_deregister(struct mei_device *dev);
+#else
+static inline int mei_dbgfs_register(struct mei_device *dev, const char *name)
+{
+	return 0;
+}
+static inline void mei_dbgfs_deregister(struct mei_device *dev) {}
+#endif /* CONFIG_DEBUG_FS */
+
+int mei_register(struct mei_device *dev);
+void mei_deregister(struct mei_device *dev);
 
 #define MEI_HDR_FMT "hdr:host=%02d me=%02d len=%d comp=%1d"
 #define MEI_HDR_PRM(hdr)                  \
diff --git a/drivers/misc/mei/nfc.c b/drivers/misc/mei/nfc.c
new file mode 100644
index 00000000000..3adf8a70f26
--- /dev/null
+++ b/drivers/misc/mei/nfc.c
@@ -0,0 +1,554 @@
+/*
+ *
+ * Intel Management Engine Interface (Intel MEI) Linux driver
+ * Copyright (c) 2003-2013, Intel Corporation.
+ *
+ * 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.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/device.h>
+#include <linux/pci.h>
+#include <linux/mei_cl_bus.h>
+
+#include "mei_dev.h"
+#include "client.h"
+
+struct mei_nfc_cmd {
+	u8 command;
+	u8 status;
+	u16 req_id;
+	u32 reserved;
+	u16 data_size;
+	u8 sub_command;
+	u8 data[];
+} __packed;
+
+struct mei_nfc_reply {
+	u8 command;
+	u8 status;
+	u16 req_id;
+	u32 reserved;
+	u16 data_size;
+	u8 sub_command;
+	u8 reply_status;
+	u8 data[];
+} __packed;
+
+struct mei_nfc_if_version {
+	u8 radio_version_sw[3];
+	u8 reserved[3];
+	u8 radio_version_hw[3];
+	u8 i2c_addr;
+	u8 fw_ivn;
+	u8 vendor_id;
+	u8 radio_type;
+} __packed;
+
+struct mei_nfc_connect {
+	u8 fw_ivn;
+	u8 vendor_id;
+} __packed;
+
+struct mei_nfc_connect_resp {
+	u8 fw_ivn;
+	u8 vendor_id;
+	u16 me_major;
+	u16 me_minor;
+	u16 me_hotfix;
+	u16 me_build;
+} __packed;
+
+struct mei_nfc_hci_hdr {
+	u8 cmd;
+	u8 status;
+	u16 req_id;
+	u32 reserved;
+	u16 data_size;
+} __packed;
+
+#define MEI_NFC_CMD_MAINTENANCE 0x00
+#define MEI_NFC_CMD_HCI_SEND 0x01
+#define MEI_NFC_CMD_HCI_RECV 0x02
+
+#define MEI_NFC_SUBCMD_CONNECT    0x00
+#define MEI_NFC_SUBCMD_IF_VERSION 0x01
+
+#define MEI_NFC_HEADER_SIZE 10
+
+/** mei_nfc_dev - NFC mei device
+ *
+ * @cl: NFC host client
+ * @cl_info: NFC info host client
+ * @init_work: perform connection to the info client
+ * @fw_ivn: NFC Intervace Version Number
+ * @vendor_id: NFC manufacturer ID
+ * @radio_type: NFC radio type
+ */
+struct mei_nfc_dev {
+	struct mei_cl *cl;
+	struct mei_cl *cl_info;
+	struct work_struct init_work;
+	wait_queue_head_t send_wq;
+	u8 fw_ivn;
+	u8 vendor_id;
+	u8 radio_type;
+	char *bus_name;
+
+	u16 req_id;
+	u16 recv_req_id;
+};
+
+static struct mei_nfc_dev nfc_dev;
+
+/* UUIDs for NFC F/W clients */
+const uuid_le mei_nfc_guid = UUID_LE(0x0bb17a78, 0x2a8e, 0x4c50,
+				     0x94, 0xd4, 0x50, 0x26,
+				     0x67, 0x23, 0x77, 0x5c);
+
+static const uuid_le mei_nfc_info_guid = UUID_LE(0xd2de1625, 0x382d, 0x417d,
+					0x48, 0xa4, 0xef, 0xab,
+					0xba, 0x8a, 0x12, 0x06);
+
+/* Vendors */
+#define MEI_NFC_VENDOR_INSIDE 0x00
+#define MEI_NFC_VENDOR_NXP    0x01
+
+/* Radio types */
+#define MEI_NFC_VENDOR_INSIDE_UREAD 0x00
+#define MEI_NFC_VENDOR_NXP_PN544    0x01
+
+static void mei_nfc_free(struct mei_nfc_dev *ndev)
+{
+	if (ndev->cl) {
+		list_del(&ndev->cl->device_link);
+		mei_cl_unlink(ndev->cl);
+		kfree(ndev->cl);
+	}
+
+	if (ndev->cl_info) {
+		list_del(&ndev->cl_info->device_link);
+		mei_cl_unlink(ndev->cl_info);
+		kfree(ndev->cl_info);
+	}
+}
+
+static int mei_nfc_build_bus_name(struct mei_nfc_dev *ndev)
+{
+	struct mei_device *dev;
+
+	if (!ndev->cl)
+		return -ENODEV;
+
+	dev = ndev->cl->dev;
+
+	switch (ndev->vendor_id) {
+	case MEI_NFC_VENDOR_INSIDE:
+		switch (ndev->radio_type) {
+		case MEI_NFC_VENDOR_INSIDE_UREAD:
+			ndev->bus_name = "microread";
+			return 0;
+
+		default:
+			dev_err(&dev->pdev->dev, "Unknow radio type 0x%x\n",
+				ndev->radio_type);
+
+			return -EINVAL;
+		}
+
+	case MEI_NFC_VENDOR_NXP:
+		switch (ndev->radio_type) {
+		case MEI_NFC_VENDOR_NXP_PN544:
+			ndev->bus_name = "pn544";
+			return 0;
+		default:
+			dev_err(&dev->pdev->dev, "Unknow radio type 0x%x\n",
+				ndev->radio_type);
+
+			return -EINVAL;
+		}
+
+	default:
+		dev_err(&dev->pdev->dev, "Unknow vendor ID 0x%x\n",
+			ndev->vendor_id);
+
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int mei_nfc_connect(struct mei_nfc_dev *ndev)
+{
+	struct mei_device *dev;
+	struct mei_cl *cl;
+	struct mei_nfc_cmd *cmd, *reply;
+	struct mei_nfc_connect *connect;
+	struct mei_nfc_connect_resp *connect_resp;
+	size_t connect_length, connect_resp_length;
+	int bytes_recv, ret;
+
+	cl = ndev->cl;
+	dev = cl->dev;
+
+	connect_length = sizeof(struct mei_nfc_cmd) +
+			sizeof(struct mei_nfc_connect);
+
+	connect_resp_length = sizeof(struct mei_nfc_cmd) +
+			sizeof(struct mei_nfc_connect_resp);
+
+	cmd = kzalloc(connect_length, GFP_KERNEL);
+	if (!cmd)
+		return -ENOMEM;
+	connect = (struct mei_nfc_connect *)cmd->data;
+
+	reply = kzalloc(connect_resp_length, GFP_KERNEL);
+	if (!reply) {
+		kfree(cmd);
+		return -ENOMEM;
+	}
+
+	connect_resp = (struct mei_nfc_connect_resp *)reply->data;
+
+	cmd->command = MEI_NFC_CMD_MAINTENANCE;
+	cmd->data_size = 3;
+	cmd->sub_command = MEI_NFC_SUBCMD_CONNECT;
+	connect->fw_ivn = ndev->fw_ivn;
+	connect->vendor_id = ndev->vendor_id;
+
+	ret = __mei_cl_send(cl, (u8 *)cmd, connect_length);
+	if (ret < 0) {
+		dev_err(&dev->pdev->dev, "Could not send connect cmd\n");
+		goto err;
+	}
+
+	bytes_recv = __mei_cl_recv(cl, (u8 *)reply, connect_resp_length);
+	if (bytes_recv < 0) {
+		dev_err(&dev->pdev->dev, "Could not read connect response\n");
+		ret = bytes_recv;
+		goto err;
+	}
+
+	dev_info(&dev->pdev->dev, "IVN 0x%x Vendor ID 0x%x\n",
+		 connect_resp->fw_ivn, connect_resp->vendor_id);
+
+	dev_info(&dev->pdev->dev, "ME FW %d.%d.%d.%d\n",
+		connect_resp->me_major, connect_resp->me_minor,
+		connect_resp->me_hotfix, connect_resp->me_build);
+
+	ret = 0;
+
+err:
+	kfree(reply);
+	kfree(cmd);
+
+	return ret;
+}
+
+static int mei_nfc_if_version(struct mei_nfc_dev *ndev)
+{
+	struct mei_device *dev;
+	struct mei_cl *cl;
+
+	struct mei_nfc_cmd cmd;
+	struct mei_nfc_reply *reply = NULL;
+	struct mei_nfc_if_version *version;
+	size_t if_version_length;
+	int bytes_recv, ret;
+
+	cl = ndev->cl_info;
+	dev = cl->dev;
+
+	memset(&cmd, 0, sizeof(struct mei_nfc_cmd));
+	cmd.command = MEI_NFC_CMD_MAINTENANCE;
+	cmd.data_size = 1;
+	cmd.sub_command = MEI_NFC_SUBCMD_IF_VERSION;
+
+	ret = __mei_cl_send(cl, (u8 *)&cmd, sizeof(struct mei_nfc_cmd));
+	if (ret < 0) {
+		dev_err(&dev->pdev->dev, "Could not send IF version cmd\n");
+		return ret;
+	}
+
+	/* to be sure on the stack we alloc memory */
+	if_version_length = sizeof(struct mei_nfc_reply) +
+		sizeof(struct mei_nfc_if_version);
+
+	reply = kzalloc(if_version_length, GFP_KERNEL);
+	if (!reply)
+		return -ENOMEM;
+
+	bytes_recv = __mei_cl_recv(cl, (u8 *)reply, if_version_length);
+	if (bytes_recv < 0 || bytes_recv < sizeof(struct mei_nfc_reply)) {
+		dev_err(&dev->pdev->dev, "Could not read IF version\n");
+		ret = -EIO;
+		goto err;
+	}
+
+	version = (struct mei_nfc_if_version *)reply->data;
+
+	ndev->fw_ivn = version->fw_ivn;
+	ndev->vendor_id = version->vendor_id;
+	ndev->radio_type = version->radio_type;
+
+err:
+	kfree(reply);
+	return ret;
+}
+
+static int mei_nfc_enable(struct mei_cl_device *cldev)
+{
+	struct mei_device *dev;
+	struct mei_nfc_dev *ndev = &nfc_dev;
+	int ret;
+
+	dev = ndev->cl->dev;
+
+	ret = mei_nfc_connect(ndev);
+	if (ret < 0) {
+		dev_err(&dev->pdev->dev, "Could not connect to NFC");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int mei_nfc_disable(struct mei_cl_device *cldev)
+{
+	return 0;
+}
+
+static int mei_nfc_send(struct mei_cl_device *cldev, u8 *buf, size_t length)
+{
+	struct mei_device *dev;
+	struct mei_nfc_dev *ndev;
+	struct mei_nfc_hci_hdr *hdr;
+	u8 *mei_buf;
+	int err;
+
+	ndev = (struct mei_nfc_dev *) cldev->priv_data;
+	dev = ndev->cl->dev;
+
+	mei_buf = kzalloc(length + MEI_NFC_HEADER_SIZE, GFP_KERNEL);
+	if (!mei_buf)
+		return -ENOMEM;
+
+	hdr = (struct mei_nfc_hci_hdr *) mei_buf;
+	hdr->cmd = MEI_NFC_CMD_HCI_SEND;
+	hdr->status = 0;
+	hdr->req_id = ndev->req_id;
+	hdr->reserved = 0;
+	hdr->data_size = length;
+
+	memcpy(mei_buf + MEI_NFC_HEADER_SIZE, buf, length);
+
+	err = __mei_cl_send(ndev->cl, mei_buf, length + MEI_NFC_HEADER_SIZE);
+	if (err < 0)
+		return err;
+
+	kfree(mei_buf);
+
+	if (!wait_event_interruptible_timeout(ndev->send_wq,
+				ndev->recv_req_id == ndev->req_id, HZ)) {
+		dev_err(&dev->pdev->dev, "NFC MEI command timeout\n");
+		err = -ETIMEDOUT;
+	} else {
+		ndev->req_id++;
+	}
+
+	return err;
+}
+
+static int mei_nfc_recv(struct mei_cl_device *cldev, u8 *buf, size_t length)
+{
+	struct mei_nfc_dev *ndev;
+	struct mei_nfc_hci_hdr *hci_hdr;
+	int received_length;
+
+	ndev = (struct mei_nfc_dev *)cldev->priv_data;
+
+	received_length = __mei_cl_recv(ndev->cl, buf, length);
+	if (received_length < 0)
+		return received_length;
+
+	hci_hdr = (struct mei_nfc_hci_hdr *) buf;
+
+	if (hci_hdr->cmd == MEI_NFC_CMD_HCI_SEND) {
+		ndev->recv_req_id = hci_hdr->req_id;
+		wake_up(&ndev->send_wq);
+
+		return 0;
+	}
+
+	return received_length;
+}
+
+static struct mei_cl_ops nfc_ops = {
+	.enable = mei_nfc_enable,
+	.disable = mei_nfc_disable,
+	.send = mei_nfc_send,
+	.recv = mei_nfc_recv,
+};
+
+static void mei_nfc_init(struct work_struct *work)
+{
+	struct mei_device *dev;
+	struct mei_cl_device *cldev;
+	struct mei_nfc_dev *ndev;
+	struct mei_cl *cl_info;
+
+	ndev = container_of(work, struct mei_nfc_dev, init_work);
+
+	cl_info = ndev->cl_info;
+	dev = cl_info->dev;
+
+	mutex_lock(&dev->device_lock);
+
+	if (mei_cl_connect(cl_info, NULL) < 0) {
+		mutex_unlock(&dev->device_lock);
+		dev_err(&dev->pdev->dev,
+			"Could not connect to the NFC INFO ME client");
+
+		goto err;
+	}
+
+	mutex_unlock(&dev->device_lock);
+
+	if (mei_nfc_if_version(ndev) < 0) {
+		dev_err(&dev->pdev->dev, "Could not get the NFC interfave version");
+
+		goto err;
+	}
+
+	dev_info(&dev->pdev->dev,
+		"NFC MEI VERSION: IVN 0x%x Vendor ID 0x%x Type 0x%x\n",
+		ndev->fw_ivn, ndev->vendor_id, ndev->radio_type);
+
+	mutex_lock(&dev->device_lock);
+
+	if (mei_cl_disconnect(cl_info) < 0) {
+		mutex_unlock(&dev->device_lock);
+		dev_err(&dev->pdev->dev,
+			"Could not disconnect the NFC INFO ME client");
+
+		goto err;
+	}
+
+	mutex_unlock(&dev->device_lock);
+
+	if (mei_nfc_build_bus_name(ndev) < 0) {
+		dev_err(&dev->pdev->dev,
+			"Could not build the bus ID name\n");
+		return;
+	}
+
+	cldev = mei_cl_add_device(dev, mei_nfc_guid, ndev->bus_name, &nfc_ops);
+	if (!cldev) {
+		dev_err(&dev->pdev->dev,
+			"Could not add the NFC device to the MEI bus\n");
+
+		goto err;
+	}
+
+	cldev->priv_data = ndev;
+
+
+	return;
+
+err:
+	mei_nfc_free(ndev);
+
+	return;
+}
+
+
+int mei_nfc_host_init(struct mei_device *dev)
+{
+	struct mei_nfc_dev *ndev = &nfc_dev;
+	struct mei_cl *cl_info, *cl = NULL;
+	int i, ret;
+
+	/* already initialzed */
+	if (ndev->cl_info)
+		return 0;
+
+	cl_info = mei_cl_allocate(dev);
+	cl = mei_cl_allocate(dev);
+
+	if (!cl || !cl_info) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	/* check for valid client id */
+	i = mei_me_cl_by_uuid(dev, &mei_nfc_info_guid);
+	if (i < 0) {
+		dev_info(&dev->pdev->dev, "nfc: failed to find the client\n");
+		ret = -ENOENT;
+		goto err;
+	}
+
+	cl_info->me_client_id = dev->me_clients[i].client_id;
+
+	ret = mei_cl_link(cl_info, MEI_HOST_CLIENT_ID_ANY);
+	if (ret)
+		goto err;
+
+	cl_info->device_uuid = mei_nfc_info_guid;
+
+	list_add_tail(&cl_info->device_link, &dev->device_list);
+
+	/* check for valid client id */
+	i = mei_me_cl_by_uuid(dev, &mei_nfc_guid);
+	if (i < 0) {
+		dev_info(&dev->pdev->dev, "nfc: failed to find the client\n");
+		ret = -ENOENT;
+		goto err;
+	}
+
+	cl->me_client_id = dev->me_clients[i].client_id;
+
+	ret = mei_cl_link(cl, MEI_HOST_CLIENT_ID_ANY);
+	if (ret)
+		goto err;
+
+	cl->device_uuid = mei_nfc_guid;
+
+	list_add_tail(&cl->device_link, &dev->device_list);
+
+	ndev->cl_info = cl_info;
+	ndev->cl = cl;
+	ndev->req_id = 1;
+
+	INIT_WORK(&ndev->init_work, mei_nfc_init);
+	init_waitqueue_head(&ndev->send_wq);
+	schedule_work(&ndev->init_work);
+
+	return 0;
+
+err:
+	mei_nfc_free(ndev);
+
+	return ret;
+}
+
+void mei_nfc_host_exit(void)
+{
+	struct mei_nfc_dev *ndev = &nfc_dev;
+
+	if (ndev->cl && ndev->cl->device)
+		mei_cl_remove_device(ndev->cl->device);
+
+	mei_nfc_free(ndev);
+}
diff --git a/drivers/misc/mei/pci-me.c b/drivers/misc/mei/pci-me.c
index b40ec0601ab..a727464e9c3 100644
--- a/drivers/misc/mei/pci-me.c
+++ b/drivers/misc/mei/pci-me.c
@@ -47,7 +47,7 @@
 static struct pci_dev *mei_pdev;
 
 /* mei_pci_tbl - PCI Device ID Table */
-static DEFINE_PCI_DEVICE_TABLE(mei_pci_tbl) = {
+static DEFINE_PCI_DEVICE_TABLE(mei_me_pci_tbl) = {
 	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_82946GZ)},
 	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_82G35)},
 	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_82Q965)},
@@ -86,18 +86,19 @@ static DEFINE_PCI_DEVICE_TABLE(mei_pci_tbl) = {
 	{0, }
 };
 
-MODULE_DEVICE_TABLE(pci, mei_pci_tbl);
+MODULE_DEVICE_TABLE(pci, mei_me_pci_tbl);
 
 static DEFINE_MUTEX(mei_mutex);
 
 /**
  * mei_quirk_probe - probe for devices that doesn't valid ME interface
+ *
  * @pdev: PCI device structure
  * @ent: entry into pci_device_table
  *
  * returns true if ME Interface is valid, false otherwise
  */
-static bool mei_quirk_probe(struct pci_dev *pdev,
+static bool mei_me_quirk_probe(struct pci_dev *pdev,
 				const struct pci_device_id *ent)
 {
 	u32 reg;
@@ -119,7 +120,7 @@ static bool mei_quirk_probe(struct pci_dev *pdev,
  *
  * returns 0 on success, <0 on failure.
  */
-static int mei_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+static int mei_me_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
 	struct mei_device *dev;
 	struct mei_me_hw *hw;
@@ -127,7 +128,7 @@ static int mei_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	mutex_lock(&mei_mutex);
 
-	if (!mei_quirk_probe(pdev, ent)) {
+	if (!mei_me_quirk_probe(pdev, ent)) {
 		err = -ENODEV;
 		goto end;
 	}
@@ -184,20 +185,19 @@ static int mei_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		goto disable_msi;
 	}
 
-	if (mei_hw_init(dev)) {
+	if (mei_start(dev)) {
 		dev_err(&pdev->dev, "init hw failure.\n");
 		err = -ENODEV;
 		goto release_irq;
 	}
 
-	err = mei_register(&pdev->dev);
+	err = mei_register(dev);
 	if (err)
 		goto release_irq;
 
 	mei_pdev = pdev;
 	pci_set_drvdata(pdev, dev);
 
-
 	schedule_delayed_work(&dev->timer_work, HZ);
 
 	mutex_unlock(&mei_mutex);
@@ -233,7 +233,7 @@ end:
  * mei_remove is called by the PCI subsystem to alert the driver
  * that it should release a PCI device.
  */
-static void mei_remove(struct pci_dev *pdev)
+static void mei_me_remove(struct pci_dev *pdev)
 {
 	struct mei_device *dev;
 	struct mei_me_hw *hw;
@@ -247,44 +247,12 @@ static void mei_remove(struct pci_dev *pdev)
 
 	hw = to_me_hw(dev);
 
-	mutex_lock(&dev->device_lock);
-
-	cancel_delayed_work(&dev->timer_work);
 
-	mei_wd_stop(dev);
+	dev_err(&pdev->dev, "stop\n");
+	mei_stop(dev);
 
 	mei_pdev = NULL;
 
-	if (dev->iamthif_cl.state == MEI_FILE_CONNECTED) {
-		dev->iamthif_cl.state = MEI_FILE_DISCONNECTING;
-		mei_cl_disconnect(&dev->iamthif_cl);
-	}
-	if (dev->wd_cl.state == MEI_FILE_CONNECTED) {
-		dev->wd_cl.state = MEI_FILE_DISCONNECTING;
-		mei_cl_disconnect(&dev->wd_cl);
-	}
-
-	/* Unregistering watchdog device */
-	mei_watchdog_unregister(dev);
-
-	/* remove entry if already in list */
-	dev_dbg(&pdev->dev, "list del iamthif and wd file list.\n");
-
-	if (dev->open_handle_count > 0)
-		dev->open_handle_count--;
-	mei_cl_unlink(&dev->wd_cl);
-
-	if (dev->open_handle_count > 0)
-		dev->open_handle_count--;
-	mei_cl_unlink(&dev->iamthif_cl);
-
-	dev->iamthif_current_cb = NULL;
-	dev->me_clients_num = 0;
-
-	mutex_unlock(&dev->device_lock);
-
-	flush_scheduled_work();
-
 	/* disable interrupts */
 	mei_disable_interrupts(dev);
 
@@ -295,44 +263,37 @@ static void mei_remove(struct pci_dev *pdev)
 	if (hw->mem_addr)
 		pci_iounmap(pdev, hw->mem_addr);
 
+	mei_deregister(dev);
+
 	kfree(dev);
 
 	pci_release_regions(pdev);
 	pci_disable_device(pdev);
 
-	mei_deregister();
 
 }
 #ifdef CONFIG_PM
-static int mei_pci_suspend(struct device *device)
+static int mei_me_pci_suspend(struct device *device)
 {
 	struct pci_dev *pdev = to_pci_dev(device);
 	struct mei_device *dev = pci_get_drvdata(pdev);
-	int err;
 
 	if (!dev)
 		return -ENODEV;
-	mutex_lock(&dev->device_lock);
 
-	cancel_delayed_work(&dev->timer_work);
+	dev_err(&pdev->dev, "suspend\n");
 
-	/* Stop watchdog if exists */
-	err = mei_wd_stop(dev);
-	/* Set new mei state */
-	if (dev->dev_state == MEI_DEV_ENABLED ||
-	    dev->dev_state == MEI_DEV_RECOVERING_FROM_RESET) {
-		dev->dev_state = MEI_DEV_POWER_DOWN;
-		mei_reset(dev, 0);
-	}
-	mutex_unlock(&dev->device_lock);
+	mei_stop(dev);
+
+	mei_disable_interrupts(dev);
 
 	free_irq(pdev->irq, dev);
 	pci_disable_msi(pdev);
 
-	return err;
+	return 0;
 }
 
-static int mei_pci_resume(struct device *device)
+static int mei_me_pci_resume(struct device *device)
 {
 	struct pci_dev *pdev = to_pci_dev(device);
 	struct mei_device *dev;
@@ -372,24 +333,24 @@ static int mei_pci_resume(struct device *device)
 
 	return err;
 }
-static SIMPLE_DEV_PM_OPS(mei_pm_ops, mei_pci_suspend, mei_pci_resume);
-#define MEI_PM_OPS	(&mei_pm_ops)
+static SIMPLE_DEV_PM_OPS(mei_me_pm_ops, mei_me_pci_suspend, mei_me_pci_resume);
+#define MEI_ME_PM_OPS	(&mei_me_pm_ops)
 #else
-#define MEI_PM_OPS	NULL
+#define MEI_ME_PM_OPS	NULL
 #endif /* CONFIG_PM */
 /*
  *  PCI driver structure
  */
-static struct pci_driver mei_driver = {
+static struct pci_driver mei_me_driver = {
 	.name = KBUILD_MODNAME,
-	.id_table = mei_pci_tbl,
-	.probe = mei_probe,
-	.remove = mei_remove,
-	.shutdown = mei_remove,
-	.driver.pm = MEI_PM_OPS,
+	.id_table = mei_me_pci_tbl,
+	.probe = mei_me_probe,
+	.remove = mei_me_remove,
+	.shutdown = mei_me_remove,
+	.driver.pm = MEI_ME_PM_OPS,
 };
 
-module_pci_driver(mei_driver);
+module_pci_driver(mei_me_driver);
 
 MODULE_AUTHOR("Intel Corporation");
 MODULE_DESCRIPTION("Intel(R) Management Engine Interface");
diff --git a/drivers/misc/mei/wd.c b/drivers/misc/mei/wd.c
index 2413247fc39..6251a4ee706 100644
--- a/drivers/misc/mei/wd.c
+++ b/drivers/misc/mei/wd.c
@@ -58,6 +58,7 @@ static void mei_wd_set_start_timeout(struct mei_device *dev, u16 timeout)
  * mei_wd_host_init - connect to the watchdog client
  *
  * @dev: the device structure
+ *
  * returns -ENENT if wd client cannot be found
  *         -EIO if write has failed
  *         0 on success
@@ -317,7 +318,8 @@ end:
  *
  * returns 0 if success, negative errno code for failure
  */
-static int mei_wd_ops_set_timeout(struct watchdog_device *wd_dev, unsigned int timeout)
+static int mei_wd_ops_set_timeout(struct watchdog_device *wd_dev,
+		unsigned int timeout)
 {
 	struct mei_device *dev;
 
diff --git a/drivers/misc/sram.c b/drivers/misc/sram.c
new file mode 100644
index 00000000000..437192e4300
--- /dev/null
+++ b/drivers/misc/sram.c
@@ -0,0 +1,121 @@
+/*
+ * Generic on-chip SRAM allocation driver
+ *
+ * Copyright (C) 2012 Philipp Zabel, Pengutronix
+ *
+ * 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/kernel.h>
+#include <linux/init.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/genalloc.h>
+
+#define SRAM_GRANULARITY	32
+
+struct sram_dev {
+	struct gen_pool *pool;
+	struct clk *clk;
+};
+
+static int sram_probe(struct platform_device *pdev)
+{
+	void __iomem *virt_base;
+	struct sram_dev *sram;
+	struct resource *res;
+	unsigned long size;
+	int ret;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -EINVAL;
+
+	size = resource_size(res);
+
+	virt_base = devm_request_and_ioremap(&pdev->dev, res);
+	if (!virt_base)
+		return -EADDRNOTAVAIL;
+
+	sram = devm_kzalloc(&pdev->dev, sizeof(*sram), GFP_KERNEL);
+	if (!sram)
+		return -ENOMEM;
+
+	sram->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(sram->clk))
+		sram->clk = NULL;
+	else
+		clk_prepare_enable(sram->clk);
+
+	sram->pool = devm_gen_pool_create(&pdev->dev, ilog2(SRAM_GRANULARITY), -1);
+	if (!sram->pool)
+		return -ENOMEM;
+
+	ret = gen_pool_add_virt(sram->pool, (unsigned long)virt_base,
+				res->start, size, -1);
+	if (ret < 0) {
+		gen_pool_destroy(sram->pool);
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, sram);
+
+	dev_dbg(&pdev->dev, "SRAM pool: %ld KiB @ 0x%p\n", size / 1024, virt_base);
+
+	return 0;
+}
+
+static int sram_remove(struct platform_device *pdev)
+{
+	struct sram_dev *sram = platform_get_drvdata(pdev);
+
+	if (gen_pool_avail(sram->pool) < gen_pool_size(sram->pool))
+		dev_dbg(&pdev->dev, "removed while SRAM allocated\n");
+
+	gen_pool_destroy(sram->pool);
+
+	if (sram->clk)
+		clk_disable_unprepare(sram->clk);
+
+	return 0;
+}
+
+#ifdef CONFIG_OF
+static struct of_device_id sram_dt_ids[] = {
+	{ .compatible = "mmio-sram" },
+	{}
+};
+#endif
+
+static struct platform_driver sram_driver = {
+	.driver = {
+		.name = "sram",
+		.of_match_table = of_match_ptr(sram_dt_ids),
+	},
+	.probe = sram_probe,
+	.remove = sram_remove,
+};
+
+static int __init sram_init(void)
+{
+	return platform_driver_register(&sram_driver);
+}
+
+postcore_initcall(sram_init);
diff --git a/drivers/misc/tsl2550.c b/drivers/misc/tsl2550.c
index 1e7bc0eb081..1dfde4d543d 100644
--- a/drivers/misc/tsl2550.c
+++ b/drivers/misc/tsl2550.c
@@ -417,24 +417,26 @@ static int tsl2550_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 
-static int tsl2550_suspend(struct i2c_client *client, pm_message_t mesg)
+static int tsl2550_suspend(struct device *dev)
 {
-	return tsl2550_set_power_state(client, 0);
+	return tsl2550_set_power_state(to_i2c_client(dev), 0);
 }
 
-static int tsl2550_resume(struct i2c_client *client)
+static int tsl2550_resume(struct device *dev)
 {
-	return tsl2550_set_power_state(client, 1);
+	return tsl2550_set_power_state(to_i2c_client(dev), 1);
 }
 
+static SIMPLE_DEV_PM_OPS(tsl2550_pm_ops, tsl2550_suspend, tsl2550_resume);
+#define TSL2550_PM_OPS (&tsl2550_pm_ops)
+
 #else
 
-#define tsl2550_suspend		NULL
-#define tsl2550_resume		NULL
+#define TSL2550_PM_OPS NULL
 
-#endif /* CONFIG_PM */
+#endif /* CONFIG_PM_SLEEP */
 
 static const struct i2c_device_id tsl2550_id[] = {
 	{ "tsl2550", 0 },
@@ -446,9 +448,8 @@ static struct i2c_driver tsl2550_driver = {
 	.driver = {
 		.name	= TSL2550_DRV_NAME,
 		.owner	= THIS_MODULE,
+		.pm	= TSL2550_PM_OPS,
 	},
-	.suspend = tsl2550_suspend,
-	.resume	= tsl2550_resume,
 	.probe	= tsl2550_probe,
 	.remove	= tsl2550_remove,
 	.id_table = tsl2550_id,
diff --git a/drivers/misc/vmw_vmci/Kconfig b/drivers/misc/vmw_vmci/Kconfig
index 39c2ecadb27..ea98f7e9ccd 100644
--- a/drivers/misc/vmw_vmci/Kconfig
+++ b/drivers/misc/vmw_vmci/Kconfig
@@ -4,7 +4,7 @@
 
 config VMWARE_VMCI
 	tristate "VMware VMCI Driver"
-	depends on X86 && PCI
+	depends on X86 && PCI && NET
 	help
 	  This is VMware's Virtual Machine Communication Interface.  It enables
 	  high-speed communication between host and guest in a virtual
diff --git a/drivers/misc/vmw_vmci/vmci_datagram.c b/drivers/misc/vmw_vmci/vmci_datagram.c
index ed5c433cd49..f3cdd904fe4 100644
--- a/drivers/misc/vmw_vmci/vmci_datagram.c
+++ b/drivers/misc/vmw_vmci/vmci_datagram.c
@@ -42,9 +42,11 @@ struct datagram_entry {
 
 struct delayed_datagram_info {
 	struct datagram_entry *entry;
-	struct vmci_datagram msg;
 	struct work_struct work;
 	bool in_dg_host_queue;
+	/* msg and msg_payload must be together. */
+	struct vmci_datagram msg;
+	u8 msg_payload[];
 };
 
 /* Number of in-flight host->host datagrams */
diff --git a/drivers/mmc/card/sdio_uart.c b/drivers/mmc/card/sdio_uart.c
index c931dfe6a59..f093cea0d06 100644
--- a/drivers/mmc/card/sdio_uart.c
+++ b/drivers/mmc/card/sdio_uart.c
@@ -134,7 +134,6 @@ static void sdio_uart_port_put(struct sdio_uart_port *port)
 static void sdio_uart_port_remove(struct sdio_uart_port *port)
 {
 	struct sdio_func *func;
-	struct tty_struct *tty;
 
 	BUG_ON(sdio_uart_table[port->index] != port);
 
@@ -155,12 +154,8 @@ static void sdio_uart_port_remove(struct sdio_uart_port *port)
 	sdio_claim_host(func);
 	port->func = NULL;
 	mutex_unlock(&port->func_lock);
-	tty = tty_port_tty_get(&port->port);
 	/* tty_hangup is async so is this safe as is ?? */
-	if (tty) {
-		tty_hangup(tty);
-		tty_kref_put(tty);
-	}
+	tty_port_tty_hangup(&port->port, false);
 	mutex_unlock(&port->port.mutex);
 	sdio_release_irq(func);
 	sdio_disable_func(func);
@@ -492,11 +487,7 @@ static void sdio_uart_check_modem_status(struct sdio_uart_port *port)
 			wake_up_interruptible(&port->port.open_wait);
 		else {
 			/* DCD drop - hang up if tty attached */
-			tty = tty_port_tty_get(&port->port);
-			if (tty) {
-				tty_hangup(tty);
-				tty_kref_put(tty);
-			}
+			tty_port_tty_hangup(&port->port, false);
 		}
 	}
 	if (status & UART_MSR_DCTS) {
diff --git a/drivers/mmc/core/core.c b/drivers/mmc/core/core.c
index 08a3cf2a761..9290bb51a06 100644
--- a/drivers/mmc/core/core.c
+++ b/drivers/mmc/core/core.c
@@ -120,8 +120,8 @@ static void mmc_should_fail_request(struct mmc_host *host,
 	    !should_fail(&host->fail_mmc_request, data->blksz * data->blocks))
 		return;
 
-	data->error = data_errors[random32() % ARRAY_SIZE(data_errors)];
-	data->bytes_xfered = (random32() % (data->bytes_xfered >> 9)) << 9;
+	data->error = data_errors[prandom_u32() % ARRAY_SIZE(data_errors)];
+	data->bytes_xfered = (prandom_u32() % (data->bytes_xfered >> 9)) << 9;
 }
 
 #else /* CONFIG_FAIL_MMC_REQUEST */
diff --git a/drivers/mmc/host/sdricoh_cs.c b/drivers/mmc/host/sdricoh_cs.c
index 7009f17ad6c..50adbd155f3 100644
--- a/drivers/mmc/host/sdricoh_cs.c
+++ b/drivers/mmc/host/sdricoh_cs.c
@@ -543,25 +543,7 @@ static struct pcmcia_driver sdricoh_driver = {
 	.suspend = sdricoh_pcmcia_suspend,
 	.resume = sdricoh_pcmcia_resume,
 };
-
-/*****************************************************************************\
- *                                                                           *
- * Driver init/exit                                                          *
- *                                                                           *
-\*****************************************************************************/
-
-static int __init sdricoh_drv_init(void)
-{
-	return pcmcia_register_driver(&sdricoh_driver);
-}
-
-static void __exit sdricoh_drv_exit(void)
-{
-	pcmcia_unregister_driver(&sdricoh_driver);
-}
-
-module_init(sdricoh_drv_init);
-module_exit(sdricoh_drv_exit);
+module_pcmcia_driver(sdricoh_driver);
 
 module_param(switchlocked, uint, 0444);
 
diff --git a/drivers/mtd/mtdchar.c b/drivers/mtd/mtdchar.c
index 92ab30ab00d..dc571ebc1aa 100644
--- a/drivers/mtd/mtdchar.c
+++ b/drivers/mtd/mtdchar.c
@@ -1123,33 +1123,6 @@ static unsigned long mtdchar_get_unmapped_area(struct file *file,
 }
 #endif
 
-static inline unsigned long get_vm_size(struct vm_area_struct *vma)
-{
-	return vma->vm_end - vma->vm_start;
-}
-
-static inline resource_size_t get_vm_offset(struct vm_area_struct *vma)
-{
-	return (resource_size_t) vma->vm_pgoff << PAGE_SHIFT;
-}
-
-/*
- * Set a new vm offset.
- *
- * Verify that the incoming offset really works as a page offset,
- * and that the offset and size fit in a resource_size_t.
- */
-static inline int set_vm_offset(struct vm_area_struct *vma, resource_size_t off)
-{
-	pgoff_t pgoff = off >> PAGE_SHIFT;
-	if (off != (resource_size_t) pgoff << PAGE_SHIFT)
-		return -EINVAL;
-	if (off + get_vm_size(vma) - 1 < off)
-		return -EINVAL;
-	vma->vm_pgoff = pgoff;
-	return 0;
-}
-
 /*
  * set up a mapping for shared memory segments
  */
@@ -1159,45 +1132,17 @@ static int mtdchar_mmap(struct file *file, struct vm_area_struct *vma)
 	struct mtd_file_info *mfi = file->private_data;
 	struct mtd_info *mtd = mfi->mtd;
 	struct map_info *map = mtd->priv;
-	resource_size_t start, off;
-	unsigned long len, vma_len;
 
         /* This is broken because it assumes the MTD device is map-based
 	   and that mtd->priv is a valid struct map_info.  It should be
 	   replaced with something that uses the mtd_get_unmapped_area()
 	   operation properly. */
 	if (0 /*mtd->type == MTD_RAM || mtd->type == MTD_ROM*/) {
-		off = get_vm_offset(vma);
-		start = map->phys;
-		len = PAGE_ALIGN((start & ~PAGE_MASK) + map->size);
-		start &= PAGE_MASK;
-		vma_len = get_vm_size(vma);
-
-		/* Overflow in off+len? */
-		if (vma_len + off < off)
-			return -EINVAL;
-		/* Does it fit in the mapping? */
-		if (vma_len + off > len)
-			return -EINVAL;
-
-		off += start;
-		/* Did that overflow? */
-		if (off < start)
-			return -EINVAL;
-		if (set_vm_offset(vma, off) < 0)
-			return -EINVAL;
-		vma->vm_flags |= VM_IO | VM_DONTEXPAND | VM_DONTDUMP;
-
 #ifdef pgprot_noncached
-		if (file->f_flags & O_DSYNC || off >= __pa(high_memory))
+		if (file->f_flags & O_DSYNC || map->phys >= __pa(high_memory))
 			vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
 #endif
-		if (io_remap_pfn_range(vma, vma->vm_start, off >> PAGE_SHIFT,
-				       vma->vm_end - vma->vm_start,
-				       vma->vm_page_prot))
-			return -EAGAIN;
-
-		return 0;
+		return vm_iomap_memory(vma, map->phys, map->size);
 	}
 	return -ENOSYS;
 #else
diff --git a/drivers/net/arcnet/com20020_cs.c b/drivers/net/arcnet/com20020_cs.c
index 5bed4c4e250..74dc1875f9c 100644
--- a/drivers/net/arcnet/com20020_cs.c
+++ b/drivers/net/arcnet/com20020_cs.c
@@ -333,16 +333,4 @@ static struct pcmcia_driver com20020_cs_driver = {
 	.suspend	= com20020_suspend,
 	.resume		= com20020_resume,
 };
-
-static int __init init_com20020_cs(void)
-{
-	return pcmcia_register_driver(&com20020_cs_driver);
-}
-
-static void __exit exit_com20020_cs(void)
-{
-	pcmcia_unregister_driver(&com20020_cs_driver);
-}
-
-module_init(init_com20020_cs);
-module_exit(exit_com20020_cs);
+module_pcmcia_driver(com20020_cs_driver);
diff --git a/drivers/net/bonding/bond_main.c b/drivers/net/bonding/bond_main.c
index 6bbd90e1123..dbbea0eec13 100644
--- a/drivers/net/bonding/bond_main.c
+++ b/drivers/net/bonding/bond_main.c
@@ -846,8 +846,10 @@ static void bond_mc_swap(struct bonding *bond, struct slave *new_active,
 		if (bond->dev->flags & IFF_ALLMULTI)
 			dev_set_allmulti(old_active->dev, -1);
 
+		netif_addr_lock_bh(bond->dev);
 		netdev_for_each_mc_addr(ha, bond->dev)
 			dev_mc_del(old_active->dev, ha->addr);
+		netif_addr_unlock_bh(bond->dev);
 	}
 
 	if (new_active) {
@@ -858,8 +860,10 @@ static void bond_mc_swap(struct bonding *bond, struct slave *new_active,
 		if (bond->dev->flags & IFF_ALLMULTI)
 			dev_set_allmulti(new_active->dev, 1);
 
+		netif_addr_lock_bh(bond->dev);
 		netdev_for_each_mc_addr(ha, bond->dev)
 			dev_mc_add(new_active->dev, ha->addr);
+		netif_addr_unlock_bh(bond->dev);
 	}
 }
 
@@ -1901,11 +1905,29 @@ err_dest_symlinks:
 	bond_destroy_slave_symlinks(bond_dev, slave_dev);
 
 err_detach:
+	if (!USES_PRIMARY(bond->params.mode)) {
+		netif_addr_lock_bh(bond_dev);
+		bond_mc_list_flush(bond_dev, slave_dev);
+		netif_addr_unlock_bh(bond_dev);
+	}
+	bond_del_vlans_from_slave(bond, slave_dev);
 	write_lock_bh(&bond->lock);
 	bond_detach_slave(bond, new_slave);
+	if (bond->primary_slave == new_slave)
+		bond->primary_slave = NULL;
 	write_unlock_bh(&bond->lock);
+	if (bond->curr_active_slave == new_slave) {
+		read_lock(&bond->lock);
+		write_lock_bh(&bond->curr_slave_lock);
+		bond_change_active_slave(bond, NULL);
+		bond_select_active_slave(bond);
+		write_unlock_bh(&bond->curr_slave_lock);
+		read_unlock(&bond->lock);
+	}
+	slave_disable_netpoll(new_slave);
 
 err_close:
+	slave_dev->priv_flags &= ~IFF_BONDING;
 	dev_close(slave_dev);
 
 err_unset_master:
@@ -1976,12 +1998,11 @@ static int __bond_release_one(struct net_device *bond_dev,
 		return -EINVAL;
 	}
 
+	write_unlock_bh(&bond->lock);
 	/* unregister rx_handler early so bond_handle_frame wouldn't be called
 	 * for this slave anymore.
 	 */
 	netdev_rx_handler_unregister(slave_dev);
-	write_unlock_bh(&bond->lock);
-	synchronize_net();
 	write_lock_bh(&bond->lock);
 
 	if (!all && !bond->params.fail_over_mac) {
@@ -3169,11 +3190,20 @@ static int bond_slave_netdev_event(unsigned long event,
 				   struct net_device *slave_dev)
 {
 	struct slave *slave = bond_slave_get_rtnl(slave_dev);
-	struct bonding *bond = slave->bond;
-	struct net_device *bond_dev = slave->bond->dev;
+	struct bonding *bond;
+	struct net_device *bond_dev;
 	u32 old_speed;
 	u8 old_duplex;
 
+	/* A netdev event can be generated while enslaving a device
+	 * before netdev_rx_handler_register is called in which case
+	 * slave will be NULL
+	 */
+	if (!slave)
+		return NOTIFY_DONE;
+	bond_dev = slave->bond->dev;
+	bond = slave->bond;
+
 	switch (event) {
 	case NETDEV_UNREGISTER:
 		if (bond->setup_by_slave)
@@ -3287,20 +3317,22 @@ static int bond_xmit_hash_policy_l2(struct sk_buff *skb, int count)
  */
 static int bond_xmit_hash_policy_l23(struct sk_buff *skb, int count)
 {
-	struct ethhdr *data = (struct ethhdr *)skb->data;
-	struct iphdr *iph;
-	struct ipv6hdr *ipv6h;
+	const struct ethhdr *data;
+	const struct iphdr *iph;
+	const struct ipv6hdr *ipv6h;
 	u32 v6hash;
-	__be32 *s, *d;
+	const __be32 *s, *d;
 
 	if (skb->protocol == htons(ETH_P_IP) &&
-	    skb_network_header_len(skb) >= sizeof(*iph)) {
+	    pskb_network_may_pull(skb, sizeof(*iph))) {
 		iph = ip_hdr(skb);
+		data = (struct ethhdr *)skb->data;
 		return ((ntohl(iph->saddr ^ iph->daddr) & 0xffff) ^
 			(data->h_dest[5] ^ data->h_source[5])) % count;
 	} else if (skb->protocol == htons(ETH_P_IPV6) &&
-		   skb_network_header_len(skb) >= sizeof(*ipv6h)) {
+		   pskb_network_may_pull(skb, sizeof(*ipv6h))) {
 		ipv6h = ipv6_hdr(skb);
+		data = (struct ethhdr *)skb->data;
 		s = &ipv6h->saddr.s6_addr32[0];
 		d = &ipv6h->daddr.s6_addr32[0];
 		v6hash = (s[1] ^ d[1]) ^ (s[2] ^ d[2]) ^ (s[3] ^ d[3]);
@@ -3319,33 +3351,36 @@ static int bond_xmit_hash_policy_l23(struct sk_buff *skb, int count)
 static int bond_xmit_hash_policy_l34(struct sk_buff *skb, int count)
 {
 	u32 layer4_xor = 0;
-	struct iphdr *iph;
-	struct ipv6hdr *ipv6h;
-	__be32 *s, *d;
-	__be16 *layer4hdr;
+	const struct iphdr *iph;
+	const struct ipv6hdr *ipv6h;
+	const __be32 *s, *d;
+	const __be16 *l4 = NULL;
+	__be16 _l4[2];
+	int noff = skb_network_offset(skb);
+	int poff;
 
 	if (skb->protocol == htons(ETH_P_IP) &&
-	    skb_network_header_len(skb) >= sizeof(*iph)) {
+	    pskb_may_pull(skb, noff + sizeof(*iph))) {
 		iph = ip_hdr(skb);
-		if (!ip_is_fragment(iph) &&
-		    (iph->protocol == IPPROTO_TCP ||
-		     iph->protocol == IPPROTO_UDP) &&
-		    (skb_headlen(skb) - skb_network_offset(skb) >=
-		     iph->ihl * sizeof(u32) + sizeof(*layer4hdr) * 2)) {
-			layer4hdr = (__be16 *)((u32 *)iph + iph->ihl);
-			layer4_xor = ntohs(*layer4hdr ^ *(layer4hdr + 1));
+		poff = proto_ports_offset(iph->protocol);
+
+		if (!ip_is_fragment(iph) && poff >= 0) {
+			l4 = skb_header_pointer(skb, noff + (iph->ihl << 2) + poff,
+						sizeof(_l4), &_l4);
+			if (l4)
+				layer4_xor = ntohs(l4[0] ^ l4[1]);
 		}
 		return (layer4_xor ^
 			((ntohl(iph->saddr ^ iph->daddr)) & 0xffff)) % count;
 	} else if (skb->protocol == htons(ETH_P_IPV6) &&
-		   skb_network_header_len(skb) >= sizeof(*ipv6h)) {
+		   pskb_may_pull(skb, noff + sizeof(*ipv6h))) {
 		ipv6h = ipv6_hdr(skb);
-		if ((ipv6h->nexthdr == IPPROTO_TCP ||
-		     ipv6h->nexthdr == IPPROTO_UDP) &&
-		    (skb_headlen(skb) - skb_network_offset(skb) >=
-		     sizeof(*ipv6h) + sizeof(*layer4hdr) * 2)) {
-			layer4hdr = (__be16 *)(ipv6h + 1);
-			layer4_xor = ntohs(*layer4hdr ^ *(layer4hdr + 1));
+		poff = proto_ports_offset(ipv6h->nexthdr);
+		if (poff >= 0) {
+			l4 = skb_header_pointer(skb, noff + sizeof(*ipv6h) + poff,
+						sizeof(_l4), &_l4);
+			if (l4)
+				layer4_xor = ntohs(l4[0] ^ l4[1]);
 		}
 		s = &ipv6h->saddr.s6_addr32[0];
 		d = &ipv6h->daddr.s6_addr32[0];
@@ -4847,9 +4882,18 @@ static int __net_init bond_net_init(struct net *net)
 static void __net_exit bond_net_exit(struct net *net)
 {
 	struct bond_net *bn = net_generic(net, bond_net_id);
+	struct bonding *bond, *tmp_bond;
+	LIST_HEAD(list);
 
 	bond_destroy_sysfs(bn);
 	bond_destroy_proc_dir(bn);
+
+	/* Kill off any bonds created after unregistering bond rtnl ops */
+	rtnl_lock();
+	list_for_each_entry_safe(bond, tmp_bond, &bn->dev_list, bond_list)
+		unregister_netdevice_queue(bond->dev, &list);
+	unregister_netdevice_many(&list);
+	rtnl_unlock();
 }
 
 static struct pernet_operations bond_net_ops = {
diff --git a/drivers/net/bonding/bond_sysfs.c b/drivers/net/bonding/bond_sysfs.c
index 1c9e09fbdff..ea7a388f484 100644
--- a/drivers/net/bonding/bond_sysfs.c
+++ b/drivers/net/bonding/bond_sysfs.c
@@ -183,6 +183,11 @@ int bond_create_slave_symlinks(struct net_device *master,
 	sprintf(linkname, "slave_%s", slave->name);
 	ret = sysfs_create_link(&(master->dev.kobj), &(slave->dev.kobj),
 				linkname);
+
+	/* free the master link created earlier in case of error */
+	if (ret)
+		sysfs_remove_link(&(slave->dev.kobj), "master");
+
 	return ret;
 
 }
@@ -522,7 +527,7 @@ static ssize_t bonding_store_arp_interval(struct device *d,
 		goto out;
 	}
 	if (new_value < 0) {
-		pr_err("%s: Invalid arp_interval value %d not in range 1-%d; rejected.\n",
+		pr_err("%s: Invalid arp_interval value %d not in range 0-%d; rejected.\n",
 		       bond->dev->name, new_value, INT_MAX);
 		ret = -EINVAL;
 		goto out;
@@ -537,14 +542,15 @@ static ssize_t bonding_store_arp_interval(struct device *d,
 	pr_info("%s: Setting ARP monitoring interval to %d.\n",
 		bond->dev->name, new_value);
 	bond->params.arp_interval = new_value;
-	if (bond->params.miimon) {
-		pr_info("%s: ARP monitoring cannot be used with MII monitoring. %s Disabling MII monitoring.\n",
-			bond->dev->name, bond->dev->name);
-		bond->params.miimon = 0;
-	}
-	if (!bond->params.arp_targets[0]) {
-		pr_info("%s: ARP monitoring has been set up, but no ARP targets have been specified.\n",
-			bond->dev->name);
+	if (new_value) {
+		if (bond->params.miimon) {
+			pr_info("%s: ARP monitoring cannot be used with MII monitoring. %s Disabling MII monitoring.\n",
+				bond->dev->name, bond->dev->name);
+			bond->params.miimon = 0;
+		}
+		if (!bond->params.arp_targets[0])
+			pr_info("%s: ARP monitoring has been set up, but no ARP targets have been specified.\n",
+				bond->dev->name);
 	}
 	if (bond->dev->flags & IFF_UP) {
 		/* If the interface is up, we may need to fire off
@@ -552,10 +558,13 @@ static ssize_t bonding_store_arp_interval(struct device *d,
 		 * timer will get fired off when the open function
 		 * is called.
 		 */
-		cancel_delayed_work_sync(&bond->mii_work);
-		queue_delayed_work(bond->wq, &bond->arp_work, 0);
+		if (!new_value) {
+			cancel_delayed_work_sync(&bond->arp_work);
+		} else {
+			cancel_delayed_work_sync(&bond->mii_work);
+			queue_delayed_work(bond->wq, &bond->arp_work, 0);
+		}
 	}
-
 out:
 	rtnl_unlock();
 	return ret;
@@ -697,7 +706,7 @@ static ssize_t bonding_store_downdelay(struct device *d,
 	}
 	if (new_value < 0) {
 		pr_err("%s: Invalid down delay value %d not in range %d-%d; rejected.\n",
-		       bond->dev->name, new_value, 1, INT_MAX);
+		       bond->dev->name, new_value, 0, INT_MAX);
 		ret = -EINVAL;
 		goto out;
 	} else {
@@ -752,8 +761,8 @@ static ssize_t bonding_store_updelay(struct device *d,
 		goto out;
 	}
 	if (new_value < 0) {
-		pr_err("%s: Invalid down delay value %d not in range %d-%d; rejected.\n",
-		       bond->dev->name, new_value, 1, INT_MAX);
+		pr_err("%s: Invalid up delay value %d not in range %d-%d; rejected.\n",
+		       bond->dev->name, new_value, 0, INT_MAX);
 		ret = -EINVAL;
 		goto out;
 	} else {
@@ -963,37 +972,37 @@ static ssize_t bonding_store_miimon(struct device *d,
 	}
 	if (new_value < 0) {
 		pr_err("%s: Invalid miimon value %d not in range %d-%d; rejected.\n",
-		       bond->dev->name, new_value, 1, INT_MAX);
+		       bond->dev->name, new_value, 0, INT_MAX);
 		ret = -EINVAL;
 		goto out;
-	} else {
-		pr_info("%s: Setting MII monitoring interval to %d.\n",
-			bond->dev->name, new_value);
-		bond->params.miimon = new_value;
-		if (bond->params.updelay)
-			pr_info("%s: Note: Updating updelay (to %d) since it is a multiple of the miimon value.\n",
-				bond->dev->name,
-				bond->params.updelay * bond->params.miimon);
-		if (bond->params.downdelay)
-			pr_info("%s: Note: Updating downdelay (to %d) since it is a multiple of the miimon value.\n",
-				bond->dev->name,
-				bond->params.downdelay * bond->params.miimon);
-		if (bond->params.arp_interval) {
-			pr_info("%s: MII monitoring cannot be used with ARP monitoring. Disabling ARP monitoring...\n",
-				bond->dev->name);
-			bond->params.arp_interval = 0;
-			if (bond->params.arp_validate) {
-				bond->params.arp_validate =
-					BOND_ARP_VALIDATE_NONE;
-			}
-		}
-
-		if (bond->dev->flags & IFF_UP) {
-			/* If the interface is up, we may need to fire off
-			 * the MII timer. If the interface is down, the
-			 * timer will get fired off when the open function
-			 * is called.
-			 */
+	}
+	pr_info("%s: Setting MII monitoring interval to %d.\n",
+		bond->dev->name, new_value);
+	bond->params.miimon = new_value;
+	if (bond->params.updelay)
+		pr_info("%s: Note: Updating updelay (to %d) since it is a multiple of the miimon value.\n",
+			bond->dev->name,
+			bond->params.updelay * bond->params.miimon);
+	if (bond->params.downdelay)
+		pr_info("%s: Note: Updating downdelay (to %d) since it is a multiple of the miimon value.\n",
+			bond->dev->name,
+			bond->params.downdelay * bond->params.miimon);
+	if (new_value && bond->params.arp_interval) {
+		pr_info("%s: MII monitoring cannot be used with ARP monitoring. Disabling ARP monitoring...\n",
+			bond->dev->name);
+		bond->params.arp_interval = 0;
+		if (bond->params.arp_validate)
+			bond->params.arp_validate = BOND_ARP_VALIDATE_NONE;
+	}
+	if (bond->dev->flags & IFF_UP) {
+		/* If the interface is up, we may need to fire off
+		 * the MII timer. If the interface is down, the
+		 * timer will get fired off when the open function
+		 * is called.
+		 */
+		if (!new_value) {
+			cancel_delayed_work_sync(&bond->mii_work);
+		} else {
 			cancel_delayed_work_sync(&bond->arp_work);
 			queue_delayed_work(bond->wq, &bond->mii_work, 0);
 		}
diff --git a/drivers/net/caif/caif_serial.c b/drivers/net/caif/caif_serial.c
index 666891a9a24..be90debc7cd 100644
--- a/drivers/net/caif/caif_serial.c
+++ b/drivers/net/caif/caif_serial.c
@@ -88,11 +88,9 @@ static inline void update_tty_status(struct ser_device *ser)
 {
 	ser->tty_status =
 		ser->tty->stopped << 5 |
-		ser->tty->hw_stopped << 4 |
 		ser->tty->flow_stopped << 3 |
 		ser->tty->packet << 2 |
-		ser->tty->port->low_latency << 1 |
-		ser->tty->warned;
+		ser->tty->port->low_latency << 1;
 }
 static inline void debugfs_init(struct ser_device *ser, struct tty_struct *tty)
 {
diff --git a/drivers/net/can/mcp251x.c b/drivers/net/can/mcp251x.c
index f32b9fc6a98..9aa0c64c33c 100644
--- a/drivers/net/can/mcp251x.c
+++ b/drivers/net/can/mcp251x.c
@@ -929,6 +929,7 @@ static int mcp251x_open(struct net_device *net)
 	struct mcp251x_priv *priv = netdev_priv(net);
 	struct spi_device *spi = priv->spi;
 	struct mcp251x_platform_data *pdata = spi->dev.platform_data;
+	unsigned long flags;
 	int ret;
 
 	ret = open_candev(net);
@@ -945,9 +946,14 @@ static int mcp251x_open(struct net_device *net)
 	priv->tx_skb = NULL;
 	priv->tx_len = 0;
 
+	flags = IRQF_ONESHOT;
+	if (pdata->irq_flags)
+		flags |= pdata->irq_flags;
+	else
+		flags |= IRQF_TRIGGER_FALLING;
+
 	ret = request_threaded_irq(spi->irq, NULL, mcp251x_can_ist,
-		  pdata->irq_flags ? pdata->irq_flags : IRQF_TRIGGER_FALLING,
-		  DEVICE_NAME, priv);
+				   flags, DEVICE_NAME, priv);
 	if (ret) {
 		dev_err(&spi->dev, "failed to acquire irq %d\n", spi->irq);
 		if (pdata->transceiver_enable)
diff --git a/drivers/net/can/sja1000/Kconfig b/drivers/net/can/sja1000/Kconfig
index b39ca5b3ea7..ff2ba86cd4a 100644
--- a/drivers/net/can/sja1000/Kconfig
+++ b/drivers/net/can/sja1000/Kconfig
@@ -46,6 +46,7 @@ config CAN_EMS_PCI
 config CAN_PEAK_PCMCIA
 	tristate "PEAK PCAN-PC Card"
 	depends on PCMCIA
+	depends on HAS_IOPORT
 	---help---
 	  This driver is for the PCAN-PC Card PCMCIA adapter (1 or 2 channels)
 	  from PEAK-System (http://www.peak-system.com). To compile this
diff --git a/drivers/net/can/sja1000/ems_pcmcia.c b/drivers/net/can/sja1000/ems_pcmcia.c
index 5c2f3fbbf5a..321c27e1c7f 100644
--- a/drivers/net/can/sja1000/ems_pcmcia.c
+++ b/drivers/net/can/sja1000/ems_pcmcia.c
@@ -316,15 +316,4 @@ static struct pcmcia_driver ems_pcmcia_driver = {
 	.remove = ems_pcmcia_remove,
 	.id_table = ems_pcmcia_tbl,
 };
-
-static int __init ems_pcmcia_init(void)
-{
-	return pcmcia_register_driver(&ems_pcmcia_driver);
-}
-module_init(ems_pcmcia_init);
-
-static void __exit ems_pcmcia_exit(void)
-{
-	pcmcia_unregister_driver(&ems_pcmcia_driver);
-}
-module_exit(ems_pcmcia_exit);
+module_pcmcia_driver(ems_pcmcia_driver);
diff --git a/drivers/net/can/sja1000/peak_pcmcia.c b/drivers/net/can/sja1000/peak_pcmcia.c
index 1a7020ba37f..0a707f70661 100644
--- a/drivers/net/can/sja1000/peak_pcmcia.c
+++ b/drivers/net/can/sja1000/peak_pcmcia.c
@@ -740,15 +740,4 @@ static struct pcmcia_driver pcan_driver = {
 	.remove = pcan_remove,
 	.id_table = pcan_table,
 };
-
-static int __init pcan_init(void)
-{
-	return pcmcia_register_driver(&pcan_driver);
-}
-module_init(pcan_init);
-
-static void __exit pcan_exit(void)
-{
-	pcmcia_unregister_driver(&pcan_driver);
-}
-module_exit(pcan_exit);
+module_pcmcia_driver(pcan_driver);
diff --git a/drivers/net/can/sja1000/plx_pci.c b/drivers/net/can/sja1000/plx_pci.c
index a042cdc260d..3c18d7d000e 100644
--- a/drivers/net/can/sja1000/plx_pci.c
+++ b/drivers/net/can/sja1000/plx_pci.c
@@ -348,7 +348,7 @@ static inline int plx_pci_check_sja1000(const struct sja1000_priv *priv)
 	 */
 	if ((priv->read_reg(priv, REG_CR) & REG_CR_BASICCAN_INITIAL_MASK) ==
 	    REG_CR_BASICCAN_INITIAL &&
-	    (priv->read_reg(priv, REG_SR) == REG_SR_BASICCAN_INITIAL) &&
+	    (priv->read_reg(priv, SJA1000_REG_SR) == REG_SR_BASICCAN_INITIAL) &&
 	    (priv->read_reg(priv, REG_IR) == REG_IR_BASICCAN_INITIAL))
 		flag = 1;
 
@@ -360,7 +360,7 @@ static inline int plx_pci_check_sja1000(const struct sja1000_priv *priv)
 	 * See states on p. 23 of the Datasheet.
 	 */
 	if (priv->read_reg(priv, REG_MOD) == REG_MOD_PELICAN_INITIAL &&
-	    priv->read_reg(priv, REG_SR) == REG_SR_PELICAN_INITIAL &&
+	    priv->read_reg(priv, SJA1000_REG_SR) == REG_SR_PELICAN_INITIAL &&
 	    priv->read_reg(priv, REG_IR) == REG_IR_PELICAN_INITIAL)
 		return flag;
 
diff --git a/drivers/net/can/sja1000/sja1000.c b/drivers/net/can/sja1000/sja1000.c
index daf4013a8fc..e4df307eaa9 100644
--- a/drivers/net/can/sja1000/sja1000.c
+++ b/drivers/net/can/sja1000/sja1000.c
@@ -92,7 +92,7 @@ static void sja1000_write_cmdreg(struct sja1000_priv *priv, u8 val)
 	 */
 	spin_lock_irqsave(&priv->cmdreg_lock, flags);
 	priv->write_reg(priv, REG_CMR, val);
-	priv->read_reg(priv, REG_SR);
+	priv->read_reg(priv, SJA1000_REG_SR);
 	spin_unlock_irqrestore(&priv->cmdreg_lock, flags);
 }
 
@@ -502,7 +502,7 @@ irqreturn_t sja1000_interrupt(int irq, void *dev_id)
 
 	while ((isrc = priv->read_reg(priv, REG_IR)) && (n < SJA1000_MAX_IRQ)) {
 		n++;
-		status = priv->read_reg(priv, REG_SR);
+		status = priv->read_reg(priv, SJA1000_REG_SR);
 		/* check for absent controller due to hw unplug */
 		if (status == 0xFF && sja1000_is_absent(priv))
 			return IRQ_NONE;
@@ -530,7 +530,7 @@ irqreturn_t sja1000_interrupt(int irq, void *dev_id)
 			/* receive interrupt */
 			while (status & SR_RBS) {
 				sja1000_rx(dev);
-				status = priv->read_reg(priv, REG_SR);
+				status = priv->read_reg(priv, SJA1000_REG_SR);
 				/* check for absent controller */
 				if (status == 0xFF && sja1000_is_absent(priv))
 					return IRQ_NONE;
diff --git a/drivers/net/can/sja1000/sja1000.h b/drivers/net/can/sja1000/sja1000.h
index afa99847a51..aa48e053da2 100644
--- a/drivers/net/can/sja1000/sja1000.h
+++ b/drivers/net/can/sja1000/sja1000.h
@@ -56,7 +56,7 @@
 /* SJA1000 registers - manual section 6.4 (Pelican Mode) */
 #define REG_MOD		0x00
 #define REG_CMR		0x01
-#define REG_SR		0x02
+#define SJA1000_REG_SR		0x02
 #define REG_IR		0x03
 #define REG_IER		0x04
 #define REG_ALC		0x0B
diff --git a/drivers/net/can/sja1000/sja1000_of_platform.c b/drivers/net/can/sja1000/sja1000_of_platform.c
index 6433b81256c..8e0c4a00193 100644
--- a/drivers/net/can/sja1000/sja1000_of_platform.c
+++ b/drivers/net/can/sja1000/sja1000_of_platform.c
@@ -96,8 +96,8 @@ static int sja1000_ofp_probe(struct platform_device *ofdev)
 	struct net_device *dev;
 	struct sja1000_priv *priv;
 	struct resource res;
-	const u32 *prop;
-	int err, irq, res_size, prop_size;
+	u32 prop;
+	int err, irq, res_size;
 	void __iomem *base;
 
 	err = of_address_to_resource(np, 0, &res);
@@ -138,27 +138,27 @@ static int sja1000_ofp_probe(struct platform_device *ofdev)
 	priv->read_reg = sja1000_ofp_read_reg;
 	priv->write_reg = sja1000_ofp_write_reg;
 
-	prop = of_get_property(np, "nxp,external-clock-frequency", &prop_size);
-	if (prop && (prop_size ==  sizeof(u32)))
-		priv->can.clock.freq = *prop / 2;
+	err = of_property_read_u32(np, "nxp,external-clock-frequency", &prop);
+	if (!err)
+		priv->can.clock.freq = prop / 2;
 	else
 		priv->can.clock.freq = SJA1000_OFP_CAN_CLOCK; /* default */
 
-	prop = of_get_property(np, "nxp,tx-output-mode", &prop_size);
-	if (prop && (prop_size == sizeof(u32)))
-		priv->ocr |= *prop & OCR_MODE_MASK;
+	err = of_property_read_u32(np, "nxp,tx-output-mode", &prop);
+	if (!err)
+		priv->ocr |= prop & OCR_MODE_MASK;
 	else
 		priv->ocr |= OCR_MODE_NORMAL; /* default */
 
-	prop = of_get_property(np, "nxp,tx-output-config", &prop_size);
-	if (prop && (prop_size == sizeof(u32)))
-		priv->ocr |= (*prop << OCR_TX_SHIFT) & OCR_TX_MASK;
+	err = of_property_read_u32(np, "nxp,tx-output-config", &prop);
+	if (!err)
+		priv->ocr |= (prop << OCR_TX_SHIFT) & OCR_TX_MASK;
 	else
 		priv->ocr |= OCR_TX0_PULLDOWN; /* default */
 
-	prop = of_get_property(np, "nxp,clock-out-frequency", &prop_size);
-	if (prop && (prop_size == sizeof(u32)) && *prop) {
-		u32 divider = priv->can.clock.freq * 2 / *prop;
+	err = of_property_read_u32(np, "nxp,clock-out-frequency", &prop);
+	if (!err && prop) {
+		u32 divider = priv->can.clock.freq * 2 / prop;
 
 		if (divider > 1)
 			priv->cdr |= divider / 2 - 1;
@@ -168,8 +168,7 @@ static int sja1000_ofp_probe(struct platform_device *ofdev)
 		priv->cdr |= CDR_CLK_OFF; /* default */
 	}
 
-	prop = of_get_property(np, "nxp,no-comparator-bypass", NULL);
-	if (!prop)
+	if (!of_property_read_bool(np, "nxp,no-comparator-bypass"))
 		priv->cdr |= CDR_CBP; /* default */
 
 	priv->irq_flags = IRQF_SHARED;
diff --git a/drivers/net/can/softing/softing_cs.c b/drivers/net/can/softing/softing_cs.c
index c2c0a5bb0b2..498605f833d 100644
--- a/drivers/net/can/softing/softing_cs.c
+++ b/drivers/net/can/softing/softing_cs.c
@@ -27,7 +27,7 @@
 #include "softing_platform.h"
 
 static int softingcs_index;
-static spinlock_t softingcs_index_lock;
+static DEFINE_SPINLOCK(softingcs_index_lock);
 
 static int softingcs_reset(struct platform_device *pdev, int v);
 static int softingcs_enable_irq(struct platform_device *pdev, int v);
@@ -340,19 +340,7 @@ static struct pcmcia_driver softingcs_driver = {
 	.remove		= softingcs_remove,
 };
 
-static int __init softingcs_start(void)
-{
-	spin_lock_init(&softingcs_index_lock);
-	return pcmcia_register_driver(&softingcs_driver);
-}
-
-static void __exit softingcs_stop(void)
-{
-	pcmcia_unregister_driver(&softingcs_driver);
-}
-
-module_init(softingcs_start);
-module_exit(softingcs_stop);
+module_pcmcia_driver(softingcs_driver);
 
 MODULE_DESCRIPTION("softing CANcard driver"
 		", links PCMCIA card to softing driver");
diff --git a/drivers/net/ethernet/3com/3c574_cs.c b/drivers/net/ethernet/3com/3c574_cs.c
index ffd8de28a76..6fc994fa4ab 100644
--- a/drivers/net/ethernet/3com/3c574_cs.c
+++ b/drivers/net/ethernet/3com/3c574_cs.c
@@ -1165,16 +1165,4 @@ static struct pcmcia_driver tc574_driver = {
 	.suspend	= tc574_suspend,
 	.resume		= tc574_resume,
 };
-
-static int __init init_tc574(void)
-{
-	return pcmcia_register_driver(&tc574_driver);
-}
-
-static void __exit exit_tc574(void)
-{
-	pcmcia_unregister_driver(&tc574_driver);
-}
-
-module_init(init_tc574);
-module_exit(exit_tc574);
+module_pcmcia_driver(tc574_driver);
diff --git a/drivers/net/ethernet/3com/3c589_cs.c b/drivers/net/ethernet/3com/3c589_cs.c
index a556c01e011..078480aaa16 100644
--- a/drivers/net/ethernet/3com/3c589_cs.c
+++ b/drivers/net/ethernet/3com/3c589_cs.c
@@ -928,16 +928,4 @@ static struct pcmcia_driver tc589_driver = {
 	.suspend	= tc589_suspend,
 	.resume		= tc589_resume,
 };
-
-static int __init init_tc589(void)
-{
-	return pcmcia_register_driver(&tc589_driver);
-}
-
-static void __exit exit_tc589(void)
-{
-	pcmcia_unregister_driver(&tc589_driver);
-}
-
-module_init(init_tc589);
-module_exit(exit_tc589);
+module_pcmcia_driver(tc589_driver);
diff --git a/drivers/net/ethernet/8390/ax88796.c b/drivers/net/ethernet/8390/ax88796.c
index cab306a9888..e1d26433d61 100644
--- a/drivers/net/ethernet/8390/ax88796.c
+++ b/drivers/net/ethernet/8390/ax88796.c
@@ -828,7 +828,7 @@ static int ax_probe(struct platform_device *pdev)
 	struct ei_device *ei_local;
 	struct ax_device *ax;
 	struct resource *irq, *mem, *mem2;
-	resource_size_t mem_size, mem2_size = 0;
+	unsigned long mem_size, mem2_size = 0;
 	int ret = 0;
 
 	dev = ax__alloc_ei_netdev(sizeof(struct ax_device));
diff --git a/drivers/net/ethernet/8390/axnet_cs.c b/drivers/net/ethernet/8390/axnet_cs.c
index e1b3941bd14..d801c1410fb 100644
--- a/drivers/net/ethernet/8390/axnet_cs.c
+++ b/drivers/net/ethernet/8390/axnet_cs.c
@@ -728,19 +728,7 @@ static struct pcmcia_driver axnet_cs_driver = {
 	.suspend	= axnet_suspend,
 	.resume		= axnet_resume,
 };
-
-static int __init init_axnet_cs(void)
-{
-	return pcmcia_register_driver(&axnet_cs_driver);
-}
-
-static void __exit exit_axnet_cs(void)
-{
-	pcmcia_unregister_driver(&axnet_cs_driver);
-}
-
-module_init(init_axnet_cs);
-module_exit(exit_axnet_cs);
+module_pcmcia_driver(axnet_cs_driver);
 
 /*====================================================================*/
 
diff --git a/drivers/net/ethernet/8390/pcnet_cs.c b/drivers/net/ethernet/8390/pcnet_cs.c
index de1af0bfed4..46c5aadaca8 100644
--- a/drivers/net/ethernet/8390/pcnet_cs.c
+++ b/drivers/net/ethernet/8390/pcnet_cs.c
@@ -1694,16 +1694,4 @@ static struct pcmcia_driver pcnet_driver = {
 	.suspend	= pcnet_suspend,
 	.resume		= pcnet_resume,
 };
-
-static int __init init_pcnet_cs(void)
-{
-    return pcmcia_register_driver(&pcnet_driver);
-}
-
-static void __exit exit_pcnet_cs(void)
-{
-    pcmcia_unregister_driver(&pcnet_driver);
-}
-
-module_init(init_pcnet_cs);
-module_exit(exit_pcnet_cs);
+module_pcmcia_driver(pcnet_driver);
diff --git a/drivers/net/ethernet/amd/nmclan_cs.c b/drivers/net/ethernet/amd/nmclan_cs.c
index 9f59bf63514..d4ed89130c5 100644
--- a/drivers/net/ethernet/amd/nmclan_cs.c
+++ b/drivers/net/ethernet/amd/nmclan_cs.c
@@ -1508,16 +1508,4 @@ static struct pcmcia_driver nmclan_cs_driver = {
 	.suspend	= nmclan_suspend,
 	.resume		= nmclan_resume,
 };
-
-static int __init init_nmclan_cs(void)
-{
-	return pcmcia_register_driver(&nmclan_cs_driver);
-}
-
-static void __exit exit_nmclan_cs(void)
-{
-	pcmcia_unregister_driver(&nmclan_cs_driver);
-}
-
-module_init(init_nmclan_cs);
-module_exit(exit_nmclan_cs);
+module_pcmcia_driver(nmclan_cs_driver);
diff --git a/drivers/net/ethernet/atheros/atl1c/atl1c_hw.c b/drivers/net/ethernet/atheros/atl1c/atl1c_hw.c
index 21e261ffbe1..3ef7092e3f1 100644
--- a/drivers/net/ethernet/atheros/atl1c/atl1c_hw.c
+++ b/drivers/net/ethernet/atheros/atl1c/atl1c_hw.c
@@ -810,7 +810,7 @@ int atl1c_power_saving(struct atl1c_hw *hw, u32 wufc)
 	if (wufc & AT_WUFC_LNKC) {
 		wol_ctrl |= WOL_LINK_CHG_EN | WOL_LINK_CHG_PME_EN;
 		if (atl1c_write_phy_reg(hw, MII_IER, IER_LINK_UP) != 0) {
-			dev_dbg(&pdev->dev, "%s: write phy MII_IER faild.\n",
+			dev_dbg(&pdev->dev, "%s: write phy MII_IER failed.\n",
 				atl1c_driver_name);
 		}
 	}
diff --git a/drivers/net/ethernet/atheros/atl1e/atl1e.h b/drivers/net/ethernet/atheros/atl1e/atl1e.h
index 829b5ad71d0..b5fd934585e 100644
--- a/drivers/net/ethernet/atheros/atl1e/atl1e.h
+++ b/drivers/net/ethernet/atheros/atl1e/atl1e.h
@@ -186,7 +186,7 @@ struct atl1e_tpd_desc {
 /* how about 0x2000 */
 #define MAX_TX_BUF_LEN      0x2000
 #define MAX_TX_BUF_SHIFT    13
-/*#define MAX_TX_BUF_LEN  0x3000 */
+#define MAX_TSO_SEG_SIZE    0x3c00
 
 /* rrs word 1 bit 0:31 */
 #define RRS_RX_CSUM_MASK	0xFFFF
@@ -438,7 +438,6 @@ struct atl1e_adapter {
 	struct atl1e_hw        hw;
 	struct atl1e_hw_stats  hw_stats;
 
-	bool have_msi;
 	u32 wol;
 	u16 link_speed;
 	u16 link_duplex;
diff --git a/drivers/net/ethernet/atheros/atl1e/atl1e_main.c b/drivers/net/ethernet/atheros/atl1e/atl1e_main.c
index 92f4734f860..ac25f05ff68 100644
--- a/drivers/net/ethernet/atheros/atl1e/atl1e_main.c
+++ b/drivers/net/ethernet/atheros/atl1e/atl1e_main.c
@@ -1849,34 +1849,19 @@ static void atl1e_free_irq(struct atl1e_adapter *adapter)
 	struct net_device *netdev = adapter->netdev;
 
 	free_irq(adapter->pdev->irq, netdev);
-
-	if (adapter->have_msi)
-		pci_disable_msi(adapter->pdev);
 }
 
 static int atl1e_request_irq(struct atl1e_adapter *adapter)
 {
 	struct pci_dev    *pdev   = adapter->pdev;
 	struct net_device *netdev = adapter->netdev;
-	int flags = 0;
 	int err = 0;
 
-	adapter->have_msi = true;
-	err = pci_enable_msi(pdev);
-	if (err) {
-		netdev_dbg(netdev,
-			   "Unable to allocate MSI interrupt Error: %d\n", err);
-		adapter->have_msi = false;
-	}
-
-	if (!adapter->have_msi)
-		flags |= IRQF_SHARED;
-	err = request_irq(pdev->irq, atl1e_intr, flags, netdev->name, netdev);
+	err = request_irq(pdev->irq, atl1e_intr, IRQF_SHARED, netdev->name,
+			  netdev);
 	if (err) {
 		netdev_dbg(adapter->netdev,
 			   "Unable to allocate interrupt Error: %d\n", err);
-		if (adapter->have_msi)
-			pci_disable_msi(pdev);
 		return err;
 	}
 	netdev_dbg(netdev, "atl1e_request_irq OK\n");
@@ -2344,6 +2329,7 @@ static int atl1e_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	INIT_WORK(&adapter->reset_task, atl1e_reset_task);
 	INIT_WORK(&adapter->link_chg_task, atl1e_link_chg_task);
+	netif_set_gso_max_size(netdev, MAX_TSO_SEG_SIZE);
 	err = register_netdev(netdev);
 	if (err) {
 		netdev_err(netdev, "register netdevice failed\n");
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
index 4046f97378c..57619dd4a92 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
@@ -2614,6 +2614,9 @@ int bnx2x_nic_load(struct bnx2x *bp, int load_mode)
 			}
 		}
 
+		/* initialize FW coalescing state machines in RAM */
+		bnx2x_update_coalesce(bp);
+
 		/* setup the leading queue */
 		rc = bnx2x_setup_leading(bp);
 		if (rc) {
@@ -4580,11 +4583,11 @@ static void storm_memset_hc_disable(struct bnx2x *bp, u8 port,
 	u32 enable_flag = disable ? 0 : (1 << HC_INDEX_DATA_HC_ENABLED_SHIFT);
 	u32 addr = BAR_CSTRORM_INTMEM +
 		   CSTORM_STATUS_BLOCK_DATA_FLAGS_OFFSET(fw_sb_id, sb_index);
-	u16 flags = REG_RD16(bp, addr);
+	u8 flags = REG_RD8(bp, addr);
 	/* clear and set */
 	flags &= ~HC_INDEX_DATA_HC_ENABLED;
 	flags |= enable_flag;
-	REG_WR16(bp, addr, flags);
+	REG_WR8(bp, addr, flags);
 	DP(NETIF_MSG_IFUP,
 	   "port %x fw_sb_id %d sb_index %d disable %d\n",
 	   port, fw_sb_id, sb_index, disable);
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_dcb.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_dcb.c
index 568205436a1..4b077a7f16a 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_dcb.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_dcb.c
@@ -591,7 +591,7 @@ static int bnx2x_dcbx_read_shmem_remote_mib(struct bnx2x *bp)
 				 DCBX_READ_REMOTE_MIB);
 
 	if (rc) {
-		BNX2X_ERR("Faild to read remote mib from FW\n");
+		BNX2X_ERR("Failed to read remote mib from FW\n");
 		return rc;
 	}
 
@@ -619,7 +619,7 @@ static int bnx2x_dcbx_read_shmem_neg_results(struct bnx2x *bp)
 				 DCBX_READ_LOCAL_MIB);
 
 	if (rc) {
-		BNX2X_ERR("Faild to read local mib from FW\n");
+		BNX2X_ERR("Failed to read local mib from FW\n");
 		return rc;
 	}
 
@@ -2139,12 +2139,12 @@ static u8 bnx2x_dcbnl_get_cap(struct net_device *netdev, int capid, u8 *cap)
 			break;
 		default:
 			BNX2X_ERR("Non valid capability ID\n");
-			rval = -EINVAL;
+			rval = 1;
 			break;
 		}
 	} else {
 		DP(BNX2X_MSG_DCB, "DCB disabled\n");
-		rval = -EINVAL;
+		rval = 1;
 	}
 
 	DP(BNX2X_MSG_DCB, "capid %d:%x\n", capid, *cap);
@@ -2170,12 +2170,12 @@ static int bnx2x_dcbnl_get_numtcs(struct net_device *netdev, int tcid, u8 *num)
 			break;
 		default:
 			BNX2X_ERR("Non valid TC-ID\n");
-			rval = -EINVAL;
+			rval = 1;
 			break;
 		}
 	} else {
 		DP(BNX2X_MSG_DCB, "DCB disabled\n");
-		rval = -EINVAL;
+		rval = 1;
 	}
 
 	return rval;
@@ -2188,7 +2188,7 @@ static int bnx2x_dcbnl_set_numtcs(struct net_device *netdev, int tcid, u8 num)
 	return -EINVAL;
 }
 
-static u8  bnx2x_dcbnl_get_pfc_state(struct net_device *netdev)
+static u8 bnx2x_dcbnl_get_pfc_state(struct net_device *netdev)
 {
 	struct bnx2x *bp = netdev_priv(netdev);
 	DP(BNX2X_MSG_DCB, "state = %d\n", bp->dcbx_local_feat.pfc.enabled);
@@ -2390,12 +2390,12 @@ static u8 bnx2x_dcbnl_get_featcfg(struct net_device *netdev, int featid,
 			break;
 		default:
 			BNX2X_ERR("Non valid featrue-ID\n");
-			rval = -EINVAL;
+			rval = 1;
 			break;
 		}
 	} else {
 		DP(BNX2X_MSG_DCB, "DCB disabled\n");
-		rval = -EINVAL;
+		rval = 1;
 	}
 
 	return rval;
@@ -2431,12 +2431,12 @@ static u8 bnx2x_dcbnl_set_featcfg(struct net_device *netdev, int featid,
 			break;
 		default:
 			BNX2X_ERR("Non valid featrue-ID\n");
-			rval = -EINVAL;
+			rval = 1;
 			break;
 		}
 	} else {
 		DP(BNX2X_MSG_DCB, "dcbnl call not valid\n");
-		rval = -EINVAL;
+		rval = 1;
 	}
 
 	return rval;
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
index 77ebae0ac64..0283f343b0d 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
@@ -13437,13 +13437,7 @@ static void bnx2x_check_kr2_wa(struct link_params *params,
 {
 	struct bnx2x *bp = params->bp;
 	u16 base_page, next_page, not_kr2_device, lane;
-	int sigdet = bnx2x_warpcore_get_sigdet(phy, params);
-
-	if (!sigdet) {
-		if (!(vars->link_attr_sync & LINK_ATTR_SYNC_KR2_ENABLE))
-			bnx2x_kr2_recovery(params, vars, phy);
-		return;
-	}
+	int sigdet;
 
 	/* Once KR2 was disabled, wait 5 seconds before checking KR2 recovery
 	 * since some switches tend to reinit the AN process and clear the
@@ -13454,6 +13448,16 @@ static void bnx2x_check_kr2_wa(struct link_params *params,
 		vars->check_kr2_recovery_cnt--;
 		return;
 	}
+
+	sigdet = bnx2x_warpcore_get_sigdet(phy, params);
+	if (!sigdet) {
+		if (!(vars->link_attr_sync & LINK_ATTR_SYNC_KR2_ENABLE)) {
+			bnx2x_kr2_recovery(params, vars, phy);
+			DP(NETIF_MSG_LINK, "No sigdet\n");
+		}
+		return;
+	}
+
 	lane = bnx2x_get_warpcore_lane(phy, params);
 	CL22_WR_OVER_CL45(bp, phy, MDIO_REG_BANK_AER_BLOCK,
 			  MDIO_AER_BLOCK_AER_REG, lane);
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
index e81a747ea8c..c50696b396f 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
@@ -4947,7 +4947,7 @@ static void bnx2x_after_function_update(struct bnx2x *bp)
 				  q);
 	}
 
-	if (!NO_FCOE(bp)) {
+	if (!NO_FCOE(bp) && CNIC_ENABLED(bp)) {
 		fp = &bp->fp[FCOE_IDX(bp)];
 		queue_params.q_obj = &bnx2x_sp_obj(bp, fp).q_obj;
 
@@ -9878,6 +9878,10 @@ static int bnx2x_prev_unload_common(struct bnx2x *bp)
 				REG_RD(bp, NIG_REG_NIG_INT_STS_CLR_0);
 			}
 		}
+		if (!CHIP_IS_E1x(bp))
+			/* block FW from writing to host */
+			REG_WR(bp, PGLUE_B_REG_INTERNAL_PFID_ENABLE_MASTER, 0);
+
 		/* wait until BRB is empty */
 		tmp_reg = REG_RD(bp, BRB1_REG_NUM_OF_FULL_BLOCKS);
 		while (timer_count) {
@@ -13354,6 +13358,7 @@ static int bnx2x_unregister_cnic(struct net_device *dev)
 	RCU_INIT_POINTER(bp->cnic_ops, NULL);
 	mutex_unlock(&bp->cnic_mutex);
 	synchronize_rcu();
+	bp->cnic_enabled = false;
 	kfree(bp->cnic_kwq);
 	bp->cnic_kwq = NULL;
 
diff --git a/drivers/net/ethernet/broadcom/tg3.c b/drivers/net/ethernet/broadcom/tg3.c
index 67d2663b397..17a972734ba 100644
--- a/drivers/net/ethernet/broadcom/tg3.c
+++ b/drivers/net/ethernet/broadcom/tg3.c
@@ -14604,8 +14604,11 @@ static void tg3_read_vpd(struct tg3 *tp)
 		if (j + len > block_end)
 			goto partno;
 
-		memcpy(tp->fw_ver, &vpd_data[j], len);
-		strncat(tp->fw_ver, " bc ", vpdlen - len - 1);
+		if (len >= sizeof(tp->fw_ver))
+			len = sizeof(tp->fw_ver) - 1;
+		memset(tp->fw_ver, 0, sizeof(tp->fw_ver));
+		snprintf(tp->fw_ver, sizeof(tp->fw_ver), "%.*s bc ", len,
+			 &vpd_data[j]);
 	}
 
 partno:
diff --git a/drivers/net/ethernet/calxeda/xgmac.c b/drivers/net/ethernet/calxeda/xgmac.c
index a170065b597..b0ebc9f6d55 100644
--- a/drivers/net/ethernet/calxeda/xgmac.c
+++ b/drivers/net/ethernet/calxeda/xgmac.c
@@ -163,6 +163,7 @@
 #define XGMAC_FLOW_CTRL_FCB_BPA	0x00000001	/* Flow Control Busy ... */
 
 /* XGMAC_INT_STAT reg */
+#define XGMAC_INT_STAT_PMTIM	0x00800000	/* PMT Interrupt Mask */
 #define XGMAC_INT_STAT_PMT	0x0080		/* PMT Interrupt Status */
 #define XGMAC_INT_STAT_LPI	0x0040		/* LPI Interrupt Status */
 
@@ -960,6 +961,9 @@ static int xgmac_hw_init(struct net_device *dev)
 	writel(DMA_INTR_DEFAULT_MASK, ioaddr + XGMAC_DMA_STATUS);
 	writel(DMA_INTR_DEFAULT_MASK, ioaddr + XGMAC_DMA_INTR_ENA);
 
+	/* Mask power mgt interrupt */
+	writel(XGMAC_INT_STAT_PMTIM, ioaddr + XGMAC_INT_STAT);
+
 	/* XGMAC requires AXI bus init. This is a 'magic number' for now */
 	writel(0x0077000E, ioaddr + XGMAC_DMA_AXI_BUS);
 
@@ -1141,6 +1145,9 @@ static int xgmac_rx(struct xgmac_priv *priv, int limit)
 		struct sk_buff *skb;
 		int frame_len;
 
+		if (!dma_ring_cnt(priv->rx_head, priv->rx_tail, DMA_RX_RING_SZ))
+			break;
+
 		entry = priv->rx_tail;
 		p = priv->dma_rx + entry;
 		if (desc_get_owner(p))
@@ -1825,7 +1832,7 @@ static void xgmac_pmt(void __iomem *ioaddr, unsigned long mode)
 	unsigned int pmt = 0;
 
 	if (mode & WAKE_MAGIC)
-		pmt |= XGMAC_PMT_POWERDOWN | XGMAC_PMT_MAGIC_PKT;
+		pmt |= XGMAC_PMT_POWERDOWN | XGMAC_PMT_MAGIC_PKT_EN;
 	if (mode & WAKE_UCAST)
 		pmt |= XGMAC_PMT_POWERDOWN | XGMAC_PMT_GLBL_UNICAST;
 
diff --git a/drivers/net/ethernet/davicom/dm9000.c b/drivers/net/ethernet/davicom/dm9000.c
index 8cdf02503d1..9eada8e8607 100644
--- a/drivers/net/ethernet/davicom/dm9000.c
+++ b/drivers/net/ethernet/davicom/dm9000.c
@@ -257,6 +257,107 @@ static void dm9000_dumpblk_32bit(void __iomem *reg, int count)
 		tmp = readl(reg);
 }
 
+/*
+ * Sleep, either by using msleep() or if we are suspending, then
+ * use mdelay() to sleep.
+ */
+static void dm9000_msleep(board_info_t *db, unsigned int ms)
+{
+	if (db->in_suspend)
+		mdelay(ms);
+	else
+		msleep(ms);
+}
+
+/* Read a word from phyxcer */
+static int
+dm9000_phy_read(struct net_device *dev, int phy_reg_unused, int reg)
+{
+	board_info_t *db = netdev_priv(dev);
+	unsigned long flags;
+	unsigned int reg_save;
+	int ret;
+
+	mutex_lock(&db->addr_lock);
+
+	spin_lock_irqsave(&db->lock, flags);
+
+	/* Save previous register address */
+	reg_save = readb(db->io_addr);
+
+	/* Fill the phyxcer register into REG_0C */
+	iow(db, DM9000_EPAR, DM9000_PHY | reg);
+
+	/* Issue phyxcer read command */
+	iow(db, DM9000_EPCR, EPCR_ERPRR | EPCR_EPOS);
+
+	writeb(reg_save, db->io_addr);
+	spin_unlock_irqrestore(&db->lock, flags);
+
+	dm9000_msleep(db, 1);		/* Wait read complete */
+
+	spin_lock_irqsave(&db->lock, flags);
+	reg_save = readb(db->io_addr);
+
+	iow(db, DM9000_EPCR, 0x0);	/* Clear phyxcer read command */
+
+	/* The read data keeps on REG_0D & REG_0E */
+	ret = (ior(db, DM9000_EPDRH) << 8) | ior(db, DM9000_EPDRL);
+
+	/* restore the previous address */
+	writeb(reg_save, db->io_addr);
+	spin_unlock_irqrestore(&db->lock, flags);
+
+	mutex_unlock(&db->addr_lock);
+
+	dm9000_dbg(db, 5, "phy_read[%02x] -> %04x\n", reg, ret);
+	return ret;
+}
+
+/* Write a word to phyxcer */
+static void
+dm9000_phy_write(struct net_device *dev,
+		 int phyaddr_unused, int reg, int value)
+{
+	board_info_t *db = netdev_priv(dev);
+	unsigned long flags;
+	unsigned long reg_save;
+
+	dm9000_dbg(db, 5, "phy_write[%02x] = %04x\n", reg, value);
+	mutex_lock(&db->addr_lock);
+
+	spin_lock_irqsave(&db->lock, flags);
+
+	/* Save previous register address */
+	reg_save = readb(db->io_addr);
+
+	/* Fill the phyxcer register into REG_0C */
+	iow(db, DM9000_EPAR, DM9000_PHY | reg);
+
+	/* Fill the written data into REG_0D & REG_0E */
+	iow(db, DM9000_EPDRL, value);
+	iow(db, DM9000_EPDRH, value >> 8);
+
+	/* Issue phyxcer write command */
+	iow(db, DM9000_EPCR, EPCR_EPOS | EPCR_ERPRW);
+
+	writeb(reg_save, db->io_addr);
+	spin_unlock_irqrestore(&db->lock, flags);
+
+	dm9000_msleep(db, 1);		/* Wait write complete */
+
+	spin_lock_irqsave(&db->lock, flags);
+	reg_save = readb(db->io_addr);
+
+	iow(db, DM9000_EPCR, 0x0);	/* Clear phyxcer write command */
+
+	/* restore the previous address */
+	writeb(reg_save, db->io_addr);
+
+	spin_unlock_irqrestore(&db->lock, flags);
+	mutex_unlock(&db->addr_lock);
+}
+
 /* dm9000_set_io
  *
  * select the specified set of io routines to use with the
@@ -795,6 +896,9 @@ dm9000_init_dm9000(struct net_device *dev)
 
 	iow(db, DM9000_GPCR, GPCR_GEP_CNTL);	/* Let GPIO0 output */
 
+	dm9000_phy_write(dev, 0, MII_BMCR, BMCR_RESET); /* PHY RESET */
+	dm9000_phy_write(dev, 0, MII_DM_DSPCR, DSPCR_INIT_PARAM); /* Init */
+
 	ncr = (db->flags & DM9000_PLATF_EXT_PHY) ? NCR_EXT_PHY : 0;
 
 	/* if wol is needed, then always set NCR_WAKEEN otherwise we end
@@ -1201,109 +1305,6 @@ dm9000_open(struct net_device *dev)
 	return 0;
 }
 
-/*
- * Sleep, either by using msleep() or if we are suspending, then
- * use mdelay() to sleep.
- */
-static void dm9000_msleep(board_info_t *db, unsigned int ms)
-{
-	if (db->in_suspend)
-		mdelay(ms);
-	else
-		msleep(ms);
-}
-
-/*
- *   Read a word from phyxcer
- */
-static int
-dm9000_phy_read(struct net_device *dev, int phy_reg_unused, int reg)
-{
-	board_info_t *db = netdev_priv(dev);
-	unsigned long flags;
-	unsigned int reg_save;
-	int ret;
-
-	mutex_lock(&db->addr_lock);
-
-	spin_lock_irqsave(&db->lock,flags);
-
-	/* Save previous register address */
-	reg_save = readb(db->io_addr);
-
-	/* Fill the phyxcer register into REG_0C */
-	iow(db, DM9000_EPAR, DM9000_PHY | reg);
-
-	iow(db, DM9000_EPCR, EPCR_ERPRR | EPCR_EPOS);	/* Issue phyxcer read command */
-
-	writeb(reg_save, db->io_addr);
-	spin_unlock_irqrestore(&db->lock,flags);
-
-	dm9000_msleep(db, 1);		/* Wait read complete */
-
-	spin_lock_irqsave(&db->lock,flags);
-	reg_save = readb(db->io_addr);
-
-	iow(db, DM9000_EPCR, 0x0);	/* Clear phyxcer read command */
-
-	/* The read data keeps on REG_0D & REG_0E */
-	ret = (ior(db, DM9000_EPDRH) << 8) | ior(db, DM9000_EPDRL);
-
-	/* restore the previous address */
-	writeb(reg_save, db->io_addr);
-	spin_unlock_irqrestore(&db->lock,flags);
-
-	mutex_unlock(&db->addr_lock);
-
-	dm9000_dbg(db, 5, "phy_read[%02x] -> %04x\n", reg, ret);
-	return ret;
-}
-
-/*
- *   Write a word to phyxcer
- */
-static void
-dm9000_phy_write(struct net_device *dev,
-		 int phyaddr_unused, int reg, int value)
-{
-	board_info_t *db = netdev_priv(dev);
-	unsigned long flags;
-	unsigned long reg_save;
-
-	dm9000_dbg(db, 5, "phy_write[%02x] = %04x\n", reg, value);
-	mutex_lock(&db->addr_lock);
-
-	spin_lock_irqsave(&db->lock,flags);
-
-	/* Save previous register address */
-	reg_save = readb(db->io_addr);
-
-	/* Fill the phyxcer register into REG_0C */
-	iow(db, DM9000_EPAR, DM9000_PHY | reg);
-
-	/* Fill the written data into REG_0D & REG_0E */
-	iow(db, DM9000_EPDRL, value);
-	iow(db, DM9000_EPDRH, value >> 8);
-
-	iow(db, DM9000_EPCR, EPCR_EPOS | EPCR_ERPRW);	/* Issue phyxcer write command */
-
-	writeb(reg_save, db->io_addr);
-	spin_unlock_irqrestore(&db->lock, flags);
-
-	dm9000_msleep(db, 1);		/* Wait write complete */
-
-	spin_lock_irqsave(&db->lock,flags);
-	reg_save = readb(db->io_addr);
-
-	iow(db, DM9000_EPCR, 0x0);	/* Clear phyxcer write command */
-
-	/* restore the previous address */
-	writeb(reg_save, db->io_addr);
-
-	spin_unlock_irqrestore(&db->lock, flags);
-	mutex_unlock(&db->addr_lock);
-}
-
 static void
 dm9000_shutdown(struct net_device *dev)
 {
@@ -1502,7 +1503,12 @@ dm9000_probe(struct platform_device *pdev)
 	db->flags |= DM9000_PLATF_SIMPLE_PHY;
 #endif
 
-	dm9000_reset(db);
+	/* Fixing bug on dm9000_probe, takeover dm9000_reset(db),
+	 * Need 'NCR_MAC_LBK' bit to indeed stable our DM9000 fifo
+	 * while probe stage.
+	 */
+
+	iow(db, DM9000_NCR, NCR_MAC_LBK | NCR_RST);
 
 	/* try multiple times, DM9000 sometimes gets the read wrong */
 	for (i = 0; i < 8; i++) {
diff --git a/drivers/net/ethernet/davicom/dm9000.h b/drivers/net/ethernet/davicom/dm9000.h
index 55688bd1a3e..9ce058adaba 100644
--- a/drivers/net/ethernet/davicom/dm9000.h
+++ b/drivers/net/ethernet/davicom/dm9000.h
@@ -69,7 +69,9 @@
 #define NCR_WAKEEN          (1<<6)
 #define NCR_FCOL            (1<<4)
 #define NCR_FDX             (1<<3)
-#define NCR_LBK             (3<<1)
+
+#define NCR_RESERVED        (3<<1)
+#define NCR_MAC_LBK         (1<<1)
 #define NCR_RST	            (1<<0)
 
 #define NSR_SPEED           (1<<7)
@@ -167,5 +169,12 @@
 #define ISR_LNKCHNG		(1<<5)
 #define ISR_UNDERRUN		(1<<4)
 
+/* Davicom MII registers.
+ */
+
+#define MII_DM_DSPCR		0x1b    /* DSP Control Register */
+
+#define DSPCR_INIT_PARAM	0xE100	/* DSP init parameter */
+
 #endif /* _DM9000X_H_ */
 
diff --git a/drivers/net/ethernet/emulex/benet/be_main.c b/drivers/net/ethernet/emulex/benet/be_main.c
index 08e54f3d288..2886c9b63f9 100644
--- a/drivers/net/ethernet/emulex/benet/be_main.c
+++ b/drivers/net/ethernet/emulex/benet/be_main.c
@@ -759,8 +759,9 @@ static struct sk_buff *be_insert_vlan_in_pkt(struct be_adapter *adapter,
 
 	if (vlan_tx_tag_present(skb)) {
 		vlan_tag = be_get_tx_vlan_tag(adapter, skb);
-		__vlan_put_tag(skb, vlan_tag);
-		skb->vlan_tci = 0;
+		skb = __vlan_put_tag(skb, vlan_tag);
+		if (skb)
+			skb->vlan_tci = 0;
 	}
 
 	return skb;
diff --git a/drivers/net/ethernet/freescale/fec.c b/drivers/net/ethernet/freescale/fec.c
index e3f39372ce2..73195f643c9 100644
--- a/drivers/net/ethernet/freescale/fec.c
+++ b/drivers/net/ethernet/freescale/fec.c
@@ -345,6 +345,53 @@ fec_enet_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 	return NETDEV_TX_OK;
 }
 
+/* Init RX & TX buffer descriptors
+ */
+static void fec_enet_bd_init(struct net_device *dev)
+{
+	struct fec_enet_private *fep = netdev_priv(dev);
+	struct bufdesc *bdp;
+	unsigned int i;
+
+	/* Initialize the receive buffer descriptors. */
+	bdp = fep->rx_bd_base;
+	for (i = 0; i < RX_RING_SIZE; i++) {
+
+		/* Initialize the BD for every fragment in the page. */
+		if (bdp->cbd_bufaddr)
+			bdp->cbd_sc = BD_ENET_RX_EMPTY;
+		else
+			bdp->cbd_sc = 0;
+		bdp = fec_enet_get_nextdesc(bdp, fep->bufdesc_ex);
+	}
+
+	/* Set the last buffer to wrap */
+	bdp = fec_enet_get_prevdesc(bdp, fep->bufdesc_ex);
+	bdp->cbd_sc |= BD_SC_WRAP;
+
+	fep->cur_rx = fep->rx_bd_base;
+
+	/* ...and the same for transmit */
+	bdp = fep->tx_bd_base;
+	fep->cur_tx = bdp;
+	for (i = 0; i < TX_RING_SIZE; i++) {
+
+		/* Initialize the BD for every fragment in the page. */
+		bdp->cbd_sc = 0;
+		if (bdp->cbd_bufaddr && fep->tx_skbuff[i]) {
+			dev_kfree_skb_any(fep->tx_skbuff[i]);
+			fep->tx_skbuff[i] = NULL;
+		}
+		bdp->cbd_bufaddr = 0;
+		bdp = fec_enet_get_nextdesc(bdp, fep->bufdesc_ex);
+	}
+
+	/* Set the last buffer to wrap */
+	bdp = fec_enet_get_prevdesc(bdp, fep->bufdesc_ex);
+	bdp->cbd_sc |= BD_SC_WRAP;
+	fep->dirty_tx = bdp;
+}
+
 /* This function is called to start or restart the FEC during a link
  * change.  This only happens when switching between half and full
  * duplex.
@@ -388,6 +435,8 @@ fec_restart(struct net_device *ndev, int duplex)
 	/* Set maximum receive buffer size. */
 	writel(PKT_MAXBLR_SIZE, fep->hwp + FEC_R_BUFF_SIZE);
 
+	fec_enet_bd_init(ndev);
+
 	/* Set receive and transmit descriptor base. */
 	writel(fep->bd_dma, fep->hwp + FEC_R_DES_START);
 	if (fep->bufdesc_ex)
@@ -397,7 +446,6 @@ fec_restart(struct net_device *ndev, int duplex)
 		writel((unsigned long)fep->bd_dma + sizeof(struct bufdesc)
 			* RX_RING_SIZE,	fep->hwp + FEC_X_DES_START);
 
-	fep->cur_rx = fep->rx_bd_base;
 
 	for (i = 0; i <= TX_RING_MOD_MASK; i++) {
 		if (fep->tx_skbuff[i]) {
@@ -954,6 +1002,7 @@ static void fec_enet_adjust_link(struct net_device *ndev)
 	} else {
 		if (fep->link) {
 			fec_stop(ndev);
+			fep->link = phy_dev->link;
 			status_change = 1;
 		}
 	}
@@ -1332,7 +1381,7 @@ static int fec_enet_ioctl(struct net_device *ndev, struct ifreq *rq, int cmd)
 static void fec_enet_free_buffers(struct net_device *ndev)
 {
 	struct fec_enet_private *fep = netdev_priv(ndev);
-	int i;
+	unsigned int i;
 	struct sk_buff *skb;
 	struct bufdesc	*bdp;
 
@@ -1356,7 +1405,7 @@ static void fec_enet_free_buffers(struct net_device *ndev)
 static int fec_enet_alloc_buffers(struct net_device *ndev)
 {
 	struct fec_enet_private *fep = netdev_priv(ndev);
-	int i;
+	unsigned int i;
 	struct sk_buff *skb;
 	struct bufdesc	*bdp;
 
@@ -1597,8 +1646,6 @@ static int fec_enet_init(struct net_device *ndev)
 {
 	struct fec_enet_private *fep = netdev_priv(ndev);
 	struct bufdesc *cbd_base;
-	struct bufdesc *bdp;
-	int i;
 
 	/* Allocate memory for buffer descriptors. */
 	cbd_base = dma_alloc_coherent(NULL, PAGE_SIZE, &fep->bd_dma,
@@ -1608,6 +1655,7 @@ static int fec_enet_init(struct net_device *ndev)
 		return -ENOMEM;
 	}
 
+	memset(cbd_base, 0, PAGE_SIZE);
 	spin_lock_init(&fep->hw_lock);
 
 	fep->netdev = ndev;
@@ -1631,35 +1679,6 @@ static int fec_enet_init(struct net_device *ndev)
 	writel(FEC_RX_DISABLED_IMASK, fep->hwp + FEC_IMASK);
 	netif_napi_add(ndev, &fep->napi, fec_enet_rx_napi, FEC_NAPI_WEIGHT);
 
-	/* Initialize the receive buffer descriptors. */
-	bdp = fep->rx_bd_base;
-	for (i = 0; i < RX_RING_SIZE; i++) {
-
-		/* Initialize the BD for every fragment in the page. */
-		bdp->cbd_sc = 0;
-		bdp = fec_enet_get_nextdesc(bdp, fep->bufdesc_ex);
-	}
-
-	/* Set the last buffer to wrap */
-	bdp = fec_enet_get_prevdesc(bdp, fep->bufdesc_ex);
-	bdp->cbd_sc |= BD_SC_WRAP;
-
-	/* ...and the same for transmit */
-	bdp = fep->tx_bd_base;
-	fep->cur_tx = bdp;
-	for (i = 0; i < TX_RING_SIZE; i++) {
-
-		/* Initialize the BD for every fragment in the page. */
-		bdp->cbd_sc = 0;
-		bdp->cbd_bufaddr = 0;
-		bdp = fec_enet_get_nextdesc(bdp, fep->bufdesc_ex);
-	}
-
-	/* Set the last buffer to wrap */
-	bdp = fec_enet_get_prevdesc(bdp, fep->bufdesc_ex);
-	bdp->cbd_sc |= BD_SC_WRAP;
-	fep->dirty_tx = bdp;
-
 	fec_restart(ndev, 0);
 
 	return 0;
diff --git a/drivers/net/ethernet/freescale/fec_ptp.c b/drivers/net/ethernet/freescale/fec_ptp.c
index 1f17ca0f220..0d8df400a47 100644
--- a/drivers/net/ethernet/freescale/fec_ptp.c
+++ b/drivers/net/ethernet/freescale/fec_ptp.c
@@ -128,6 +128,7 @@ void fec_ptp_start_cyclecounter(struct net_device *ndev)
 
 	spin_unlock_irqrestore(&fep->tmreg_lock, flags);
 }
+EXPORT_SYMBOL(fec_ptp_start_cyclecounter);
 
 /**
  * fec_ptp_adjfreq - adjust ptp cycle frequency
@@ -318,6 +319,7 @@ int fec_ptp_ioctl(struct net_device *ndev, struct ifreq *ifr, int cmd)
 	return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
 	    -EFAULT : 0;
 }
+EXPORT_SYMBOL(fec_ptp_ioctl);
 
 /**
  * fec_time_keep - call timecounter_read every second to avoid timer overrun
@@ -383,3 +385,4 @@ void fec_ptp_init(struct net_device *ndev, struct platform_device *pdev)
 		pr_info("registered PHC device on %s\n", ndev->name);
 	}
 }
+EXPORT_SYMBOL(fec_ptp_init);
diff --git a/drivers/net/ethernet/fujitsu/fmvj18x_cs.c b/drivers/net/ethernet/fujitsu/fmvj18x_cs.c
index 2418faf2251..ab98b77df30 100644
--- a/drivers/net/ethernet/fujitsu/fmvj18x_cs.c
+++ b/drivers/net/ethernet/fujitsu/fmvj18x_cs.c
@@ -705,19 +705,7 @@ static struct pcmcia_driver fmvj18x_cs_driver = {
 	.suspend	= fmvj18x_suspend,
 	.resume		= fmvj18x_resume,
 };
-
-static int __init init_fmvj18x_cs(void)
-{
-	return pcmcia_register_driver(&fmvj18x_cs_driver);
-}
-
-static void __exit exit_fmvj18x_cs(void)
-{
-	pcmcia_unregister_driver(&fmvj18x_cs_driver);
-}
-
-module_init(init_fmvj18x_cs);
-module_exit(exit_fmvj18x_cs);
+module_pcmcia_driver(fmvj18x_cs_driver);
 
 /*====================================================================*/
 
diff --git a/drivers/net/ethernet/intel/e100.c b/drivers/net/ethernet/intel/e100.c
index ec800b093e7..d2bea3f07c7 100644
--- a/drivers/net/ethernet/intel/e100.c
+++ b/drivers/net/ethernet/intel/e100.c
@@ -870,7 +870,7 @@ err_unlock:
 }
 
 static int e100_exec_cb(struct nic *nic, struct sk_buff *skb,
-	void (*cb_prepare)(struct nic *, struct cb *, struct sk_buff *))
+	int (*cb_prepare)(struct nic *, struct cb *, struct sk_buff *))
 {
 	struct cb *cb;
 	unsigned long flags;
@@ -888,10 +888,13 @@ static int e100_exec_cb(struct nic *nic, struct sk_buff *skb,
 	nic->cbs_avail--;
 	cb->skb = skb;
 
+	err = cb_prepare(nic, cb, skb);
+	if (err)
+		goto err_unlock;
+
 	if (unlikely(!nic->cbs_avail))
 		err = -ENOSPC;
 
-	cb_prepare(nic, cb, skb);
 
 	/* Order is important otherwise we'll be in a race with h/w:
 	 * set S-bit in current first, then clear S-bit in previous. */
@@ -1091,7 +1094,7 @@ static void e100_get_defaults(struct nic *nic)
 	nic->mii.mdio_write = mdio_write;
 }
 
-static void e100_configure(struct nic *nic, struct cb *cb, struct sk_buff *skb)
+static int e100_configure(struct nic *nic, struct cb *cb, struct sk_buff *skb)
 {
 	struct config *config = &cb->u.config;
 	u8 *c = (u8 *)config;
@@ -1181,6 +1184,7 @@ static void e100_configure(struct nic *nic, struct cb *cb, struct sk_buff *skb)
 	netif_printk(nic, hw, KERN_DEBUG, nic->netdev,
 		     "[16-23]=%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X\n",
 		     c[16], c[17], c[18], c[19], c[20], c[21], c[22], c[23]);
+	return 0;
 }
 
 /*************************************************************************
@@ -1331,7 +1335,7 @@ static const struct firmware *e100_request_firmware(struct nic *nic)
 	return fw;
 }
 
-static void e100_setup_ucode(struct nic *nic, struct cb *cb,
+static int e100_setup_ucode(struct nic *nic, struct cb *cb,
 			     struct sk_buff *skb)
 {
 	const struct firmware *fw = (void *)skb;
@@ -1358,6 +1362,7 @@ static void e100_setup_ucode(struct nic *nic, struct cb *cb,
 	cb->u.ucode[min_size] |= cpu_to_le32((BUNDLESMALL) ? 0xFFFF : 0xFF80);
 
 	cb->command = cpu_to_le16(cb_ucode | cb_el);
+	return 0;
 }
 
 static inline int e100_load_ucode_wait(struct nic *nic)
@@ -1400,18 +1405,20 @@ static inline int e100_load_ucode_wait(struct nic *nic)
 	return err;
 }
 
-static void e100_setup_iaaddr(struct nic *nic, struct cb *cb,
+static int e100_setup_iaaddr(struct nic *nic, struct cb *cb,
 	struct sk_buff *skb)
 {
 	cb->command = cpu_to_le16(cb_iaaddr);
 	memcpy(cb->u.iaaddr, nic->netdev->dev_addr, ETH_ALEN);
+	return 0;
 }
 
-static void e100_dump(struct nic *nic, struct cb *cb, struct sk_buff *skb)
+static int e100_dump(struct nic *nic, struct cb *cb, struct sk_buff *skb)
 {
 	cb->command = cpu_to_le16(cb_dump);
 	cb->u.dump_buffer_addr = cpu_to_le32(nic->dma_addr +
 		offsetof(struct mem, dump_buf));
+	return 0;
 }
 
 static int e100_phy_check_without_mii(struct nic *nic)
@@ -1581,7 +1588,7 @@ static int e100_hw_init(struct nic *nic)
 	return 0;
 }
 
-static void e100_multi(struct nic *nic, struct cb *cb, struct sk_buff *skb)
+static int e100_multi(struct nic *nic, struct cb *cb, struct sk_buff *skb)
 {
 	struct net_device *netdev = nic->netdev;
 	struct netdev_hw_addr *ha;
@@ -1596,6 +1603,7 @@ static void e100_multi(struct nic *nic, struct cb *cb, struct sk_buff *skb)
 		memcpy(&cb->u.multi.addr[i++ * ETH_ALEN], &ha->addr,
 			ETH_ALEN);
 	}
+	return 0;
 }
 
 static void e100_set_multicast_list(struct net_device *netdev)
@@ -1756,11 +1764,18 @@ static void e100_watchdog(unsigned long data)
 		  round_jiffies(jiffies + E100_WATCHDOG_PERIOD));
 }
 
-static void e100_xmit_prepare(struct nic *nic, struct cb *cb,
+static int e100_xmit_prepare(struct nic *nic, struct cb *cb,
 	struct sk_buff *skb)
 {
+	dma_addr_t dma_addr;
 	cb->command = nic->tx_command;
 
+	dma_addr = pci_map_single(nic->pdev,
+				  skb->data, skb->len, PCI_DMA_TODEVICE);
+	/* If we can't map the skb, have the upper layer try later */
+	if (pci_dma_mapping_error(nic->pdev, dma_addr))
+		return -ENOMEM;
+
 	/*
 	 * Use the last 4 bytes of the SKB payload packet as the CRC, used for
 	 * testing, ie sending frames with bad CRC.
@@ -1777,11 +1792,10 @@ static void e100_xmit_prepare(struct nic *nic, struct cb *cb,
 	cb->u.tcb.tcb_byte_count = 0;
 	cb->u.tcb.threshold = nic->tx_threshold;
 	cb->u.tcb.tbd_count = 1;
-	cb->u.tcb.tbd.buf_addr = cpu_to_le32(pci_map_single(nic->pdev,
-		skb->data, skb->len, PCI_DMA_TODEVICE));
-	/* check for mapping failure? */
+	cb->u.tcb.tbd.buf_addr = cpu_to_le32(dma_addr);
 	cb->u.tcb.tbd.size = cpu_to_le16(skb->len);
 	skb_tx_timestamp(skb);
+	return 0;
 }
 
 static netdev_tx_t e100_xmit_frame(struct sk_buff *skb,
diff --git a/drivers/net/ethernet/intel/e1000/e1000_ethtool.c b/drivers/net/ethernet/intel/e1000/e1000_ethtool.c
index 43462d596a4..ffd287196bf 100644
--- a/drivers/net/ethernet/intel/e1000/e1000_ethtool.c
+++ b/drivers/net/ethernet/intel/e1000/e1000_ethtool.c
@@ -1053,6 +1053,10 @@ static int e1000_setup_desc_rings(struct e1000_adapter *adapter)
 		txdr->buffer_info[i].dma =
 			dma_map_single(&pdev->dev, skb->data, skb->len,
 				       DMA_TO_DEVICE);
+		if (dma_mapping_error(&pdev->dev, txdr->buffer_info[i].dma)) {
+			ret_val = 4;
+			goto err_nomem;
+		}
 		tx_desc->buffer_addr = cpu_to_le64(txdr->buffer_info[i].dma);
 		tx_desc->lower.data = cpu_to_le32(skb->len);
 		tx_desc->lower.data |= cpu_to_le32(E1000_TXD_CMD_EOP |
@@ -1069,7 +1073,7 @@ static int e1000_setup_desc_rings(struct e1000_adapter *adapter)
 	rxdr->buffer_info = kcalloc(rxdr->count, sizeof(struct e1000_buffer),
 				    GFP_KERNEL);
 	if (!rxdr->buffer_info) {
-		ret_val = 4;
+		ret_val = 5;
 		goto err_nomem;
 	}
 
@@ -1077,7 +1081,7 @@ static int e1000_setup_desc_rings(struct e1000_adapter *adapter)
 	rxdr->desc = dma_alloc_coherent(&pdev->dev, rxdr->size, &rxdr->dma,
 					GFP_KERNEL);
 	if (!rxdr->desc) {
-		ret_val = 5;
+		ret_val = 6;
 		goto err_nomem;
 	}
 	memset(rxdr->desc, 0, rxdr->size);
@@ -1101,7 +1105,7 @@ static int e1000_setup_desc_rings(struct e1000_adapter *adapter)
 
 		skb = alloc_skb(E1000_RXBUFFER_2048 + NET_IP_ALIGN, GFP_KERNEL);
 		if (!skb) {
-			ret_val = 6;
+			ret_val = 7;
 			goto err_nomem;
 		}
 		skb_reserve(skb, NET_IP_ALIGN);
@@ -1110,6 +1114,10 @@ static int e1000_setup_desc_rings(struct e1000_adapter *adapter)
 		rxdr->buffer_info[i].dma =
 			dma_map_single(&pdev->dev, skb->data,
 				       E1000_RXBUFFER_2048, DMA_FROM_DEVICE);
+		if (dma_mapping_error(&pdev->dev, rxdr->buffer_info[i].dma)) {
+			ret_val = 8;
+			goto err_nomem;
+		}
 		rx_desc->buffer_addr = cpu_to_le64(rxdr->buffer_info[i].dma);
 		memset(skb->data, 0x00, skb->len);
 	}
diff --git a/drivers/net/ethernet/intel/e1000e/netdev.c b/drivers/net/ethernet/intel/e1000e/netdev.c
index 948b86ffa4f..7e615e2bf7e 100644
--- a/drivers/net/ethernet/intel/e1000e/netdev.c
+++ b/drivers/net/ethernet/intel/e1000e/netdev.c
@@ -848,11 +848,16 @@ check_page:
 			}
 		}
 
-		if (!buffer_info->dma)
+		if (!buffer_info->dma) {
 			buffer_info->dma = dma_map_page(&pdev->dev,
 			                                buffer_info->page, 0,
 			                                PAGE_SIZE,
 							DMA_FROM_DEVICE);
+			if (dma_mapping_error(&pdev->dev, buffer_info->dma)) {
+				adapter->alloc_rx_buff_failed++;
+				break;
+			}
+		}
 
 		rx_desc = E1000_RX_DESC_EXT(*rx_ring, i);
 		rx_desc->read.buffer_addr = cpu_to_le64(buffer_info->dma);
diff --git a/drivers/net/ethernet/intel/igb/e1000_82575.c b/drivers/net/ethernet/intel/igb/e1000_82575.c
index b64542acfa3..12b1d848080 100644
--- a/drivers/net/ethernet/intel/igb/e1000_82575.c
+++ b/drivers/net/ethernet/intel/igb/e1000_82575.c
@@ -1818,27 +1818,32 @@ out:
  **/
 void igb_vmdq_set_anti_spoofing_pf(struct e1000_hw *hw, bool enable, int pf)
 {
-	u32 dtxswc;
+	u32 reg_val, reg_offset;
 
 	switch (hw->mac.type) {
 	case e1000_82576:
+		reg_offset = E1000_DTXSWC;
+		break;
 	case e1000_i350:
-		dtxswc = rd32(E1000_DTXSWC);
-		if (enable) {
-			dtxswc |= (E1000_DTXSWC_MAC_SPOOF_MASK |
-				   E1000_DTXSWC_VLAN_SPOOF_MASK);
-			/* The PF can spoof - it has to in order to
-			 * support emulation mode NICs */
-			dtxswc ^= (1 << pf | 1 << (pf + MAX_NUM_VFS));
-		} else {
-			dtxswc &= ~(E1000_DTXSWC_MAC_SPOOF_MASK |
-				    E1000_DTXSWC_VLAN_SPOOF_MASK);
-		}
-		wr32(E1000_DTXSWC, dtxswc);
+		reg_offset = E1000_TXSWC;
 		break;
 	default:
-		break;
+		return;
+	}
+
+	reg_val = rd32(reg_offset);
+	if (enable) {
+		reg_val |= (E1000_DTXSWC_MAC_SPOOF_MASK |
+			     E1000_DTXSWC_VLAN_SPOOF_MASK);
+		/* The PF can spoof - it has to in order to
+		 * support emulation mode NICs
+		 */
+		reg_val ^= (1 << pf | 1 << (pf + MAX_NUM_VFS));
+	} else {
+		reg_val &= ~(E1000_DTXSWC_MAC_SPOOF_MASK |
+			     E1000_DTXSWC_VLAN_SPOOF_MASK);
 	}
+	wr32(reg_offset, reg_val);
 }
 
 /**
diff --git a/drivers/net/ethernet/intel/igb/igb.h b/drivers/net/ethernet/intel/igb/igb.h
index 25151401c2a..ab577a763a2 100644
--- a/drivers/net/ethernet/intel/igb/igb.h
+++ b/drivers/net/ethernet/intel/igb/igb.h
@@ -284,18 +284,10 @@ struct igb_q_vector {
 enum e1000_ring_flags_t {
 	IGB_RING_FLAG_RX_SCTP_CSUM,
 	IGB_RING_FLAG_RX_LB_VLAN_BSWAP,
-	IGB_RING_FLAG_RX_BUILD_SKB_ENABLED,
 	IGB_RING_FLAG_TX_CTX_IDX,
 	IGB_RING_FLAG_TX_DETECT_HANG
 };
 
-#define ring_uses_build_skb(ring) \
-	test_bit(IGB_RING_FLAG_RX_BUILD_SKB_ENABLED, &(ring)->flags)
-#define set_ring_build_skb_enabled(ring) \
-	set_bit(IGB_RING_FLAG_RX_BUILD_SKB_ENABLED, &(ring)->flags)
-#define clear_ring_build_skb_enabled(ring) \
-	clear_bit(IGB_RING_FLAG_RX_BUILD_SKB_ENABLED, &(ring)->flags)
-
 #define IGB_TXD_DCMD (E1000_ADVTXD_DCMD_EOP | E1000_ADVTXD_DCMD_RS)
 
 #define IGB_RX_DESC(R, i)	    \
diff --git a/drivers/net/ethernet/intel/igb/igb_hwmon.c b/drivers/net/ethernet/intel/igb/igb_hwmon.c
index 4623502054d..0478a1abe54 100644
--- a/drivers/net/ethernet/intel/igb/igb_hwmon.c
+++ b/drivers/net/ethernet/intel/igb/igb_hwmon.c
@@ -39,7 +39,7 @@
 #include <linux/pci.h>
 
 #ifdef CONFIG_IGB_HWMON
-struct i2c_board_info i350_sensor_info = {
+static struct i2c_board_info i350_sensor_info = {
 	I2C_BOARD_INFO("i350bb", (0Xf8 >> 1)),
 };
 
diff --git a/drivers/net/ethernet/intel/igb/igb_main.c b/drivers/net/ethernet/intel/igb/igb_main.c
index 4dbd62968c7..64f75291e3a 100644
--- a/drivers/net/ethernet/intel/igb/igb_main.c
+++ b/drivers/net/ethernet/intel/igb/igb_main.c
@@ -2542,8 +2542,8 @@ static void igb_probe_vfs(struct igb_adapter *adapter)
 	if ((hw->mac.type == e1000_i210) || (hw->mac.type == e1000_i211))
 		return;
 
-	igb_enable_sriov(pdev, max_vfs);
 	pci_sriov_set_totalvfs(pdev, 7);
+	igb_enable_sriov(pdev, max_vfs);
 
 #endif /* CONFIG_PCI_IOV */
 }
@@ -2652,7 +2652,7 @@ static int igb_sw_init(struct igb_adapter *adapter)
 		if (max_vfs > 7) {
 			dev_warn(&pdev->dev,
 				 "Maximum of 7 VFs per PF, using max\n");
-			adapter->vfs_allocated_count = 7;
+			max_vfs = adapter->vfs_allocated_count = 7;
 		} else
 			adapter->vfs_allocated_count = max_vfs;
 		if (adapter->vfs_allocated_count)
@@ -3350,20 +3350,6 @@ void igb_configure_rx_ring(struct igb_adapter *adapter,
 	wr32(E1000_RXDCTL(reg_idx), rxdctl);
 }
 
-static void igb_set_rx_buffer_len(struct igb_adapter *adapter,
-				  struct igb_ring *rx_ring)
-{
-#define IGB_MAX_BUILD_SKB_SIZE \
-	(SKB_WITH_OVERHEAD(IGB_RX_BUFSZ) - \
-	 (NET_SKB_PAD + NET_IP_ALIGN + IGB_TS_HDR_LEN))
-
-	/* set build_skb flag */
-	if (adapter->max_frame_size <= IGB_MAX_BUILD_SKB_SIZE)
-		set_ring_build_skb_enabled(rx_ring);
-	else
-		clear_ring_build_skb_enabled(rx_ring);
-}
-
 /**
  * igb_configure_rx - Configure receive Unit after Reset
  * @adapter: board private structure
@@ -3383,11 +3369,8 @@ static void igb_configure_rx(struct igb_adapter *adapter)
 
 	/* Setup the HW Rx Head and Tail Descriptor Pointers and
 	 * the Base and Length of the Rx Descriptor Ring */
-	for (i = 0; i < adapter->num_rx_queues; i++) {
-		struct igb_ring *rx_ring = adapter->rx_ring[i];
-		igb_set_rx_buffer_len(adapter, rx_ring);
-		igb_configure_rx_ring(adapter, rx_ring);
-	}
+	for (i = 0; i < adapter->num_rx_queues; i++)
+		igb_configure_rx_ring(adapter, adapter->rx_ring[i]);
 }
 
 /**
@@ -6203,78 +6186,6 @@ static bool igb_add_rx_frag(struct igb_ring *rx_ring,
 	return igb_can_reuse_rx_page(rx_buffer, page, truesize);
 }
 
-static struct sk_buff *igb_build_rx_buffer(struct igb_ring *rx_ring,
-					   union e1000_adv_rx_desc *rx_desc)
-{
-	struct igb_rx_buffer *rx_buffer;
-	struct sk_buff *skb;
-	struct page *page;
-	void *page_addr;
-	unsigned int size = le16_to_cpu(rx_desc->wb.upper.length);
-#if (PAGE_SIZE < 8192)
-	unsigned int truesize = IGB_RX_BUFSZ;
-#else
-	unsigned int truesize = SKB_DATA_ALIGN(sizeof(struct skb_shared_info)) +
-				SKB_DATA_ALIGN(NET_SKB_PAD +
-					       NET_IP_ALIGN +
-					       size);
-#endif
-
-	/* If we spanned a buffer we have a huge mess so test for it */
-	BUG_ON(unlikely(!igb_test_staterr(rx_desc, E1000_RXD_STAT_EOP)));
-
-	rx_buffer = &rx_ring->rx_buffer_info[rx_ring->next_to_clean];
-	page = rx_buffer->page;
-	prefetchw(page);
-
-	page_addr = page_address(page) + rx_buffer->page_offset;
-
-	/* prefetch first cache line of first page */
-	prefetch(page_addr + NET_SKB_PAD + NET_IP_ALIGN);
-#if L1_CACHE_BYTES < 128
-	prefetch(page_addr + L1_CACHE_BYTES + NET_SKB_PAD + NET_IP_ALIGN);
-#endif
-
-	/* build an skb to around the page buffer */
-	skb = build_skb(page_addr, truesize);
-	if (unlikely(!skb)) {
-		rx_ring->rx_stats.alloc_failed++;
-		return NULL;
-	}
-
-	/* we are reusing so sync this buffer for CPU use */
-	dma_sync_single_range_for_cpu(rx_ring->dev,
-				      rx_buffer->dma,
-				      rx_buffer->page_offset,
-				      IGB_RX_BUFSZ,
-				      DMA_FROM_DEVICE);
-
-	/* update pointers within the skb to store the data */
-	skb_reserve(skb, NET_IP_ALIGN + NET_SKB_PAD);
-	__skb_put(skb, size);
-
-	/* pull timestamp out of packet data */
-	if (igb_test_staterr(rx_desc, E1000_RXDADV_STAT_TSIP)) {
-		igb_ptp_rx_pktstamp(rx_ring->q_vector, skb->data, skb);
-		__skb_pull(skb, IGB_TS_HDR_LEN);
-	}
-
-	if (igb_can_reuse_rx_page(rx_buffer, page, truesize)) {
-		/* hand second half of page back to the ring */
-		igb_reuse_rx_page(rx_ring, rx_buffer);
-	} else {
-		/* we are not reusing the buffer so unmap it */
-		dma_unmap_page(rx_ring->dev, rx_buffer->dma,
-			       PAGE_SIZE, DMA_FROM_DEVICE);
-	}
-
-	/* clear contents of buffer_info */
-	rx_buffer->dma = 0;
-	rx_buffer->page = NULL;
-
-	return skb;
-}
-
 static struct sk_buff *igb_fetch_rx_buffer(struct igb_ring *rx_ring,
 					   union e1000_adv_rx_desc *rx_desc,
 					   struct sk_buff *skb)
@@ -6690,10 +6601,7 @@ static bool igb_clean_rx_irq(struct igb_q_vector *q_vector, const int budget)
 		rmb();
 
 		/* retrieve a buffer from the ring */
-		if (ring_uses_build_skb(rx_ring))
-			skb = igb_build_rx_buffer(rx_ring, rx_desc);
-		else
-			skb = igb_fetch_rx_buffer(rx_ring, rx_desc, skb);
+		skb = igb_fetch_rx_buffer(rx_ring, rx_desc, skb);
 
 		/* exit if we failed to retrieve a buffer */
 		if (!skb)
@@ -6780,14 +6688,6 @@ static bool igb_alloc_mapped_page(struct igb_ring *rx_ring,
 	return true;
 }
 
-static inline unsigned int igb_rx_offset(struct igb_ring *rx_ring)
-{
-	if (ring_uses_build_skb(rx_ring))
-		return NET_SKB_PAD + NET_IP_ALIGN;
-	else
-		return 0;
-}
-
 /**
  * igb_alloc_rx_buffers - Replace used receive buffers; packet split
  * @adapter: address of board private structure
@@ -6814,9 +6714,7 @@ void igb_alloc_rx_buffers(struct igb_ring *rx_ring, u16 cleaned_count)
 		 * Refresh the desc even if buffer_addrs didn't change
 		 * because each write-back erases this info.
 		 */
-		rx_desc->read.pkt_addr = cpu_to_le64(bi->dma +
-						     bi->page_offset +
-						     igb_rx_offset(rx_ring));
+		rx_desc->read.pkt_addr = cpu_to_le64(bi->dma + bi->page_offset);
 
 		rx_desc++;
 		bi++;
diff --git a/drivers/net/ethernet/intel/igb/igb_ptp.c b/drivers/net/ethernet/intel/igb/igb_ptp.c
index 0987822359f..0a237507ee8 100644
--- a/drivers/net/ethernet/intel/igb/igb_ptp.c
+++ b/drivers/net/ethernet/intel/igb/igb_ptp.c
@@ -740,7 +740,7 @@ void igb_ptp_init(struct igb_adapter *adapter)
 	case e1000_82576:
 		snprintf(adapter->ptp_caps.name, 16, "%pm", netdev->dev_addr);
 		adapter->ptp_caps.owner = THIS_MODULE;
-		adapter->ptp_caps.max_adj = 1000000000;
+		adapter->ptp_caps.max_adj = 999999881;
 		adapter->ptp_caps.n_ext_ts = 0;
 		adapter->ptp_caps.pps = 0;
 		adapter->ptp_caps.adjfreq = igb_ptp_adjfreq_82576;
diff --git a/drivers/net/ethernet/intel/ixgb/ixgb_main.c b/drivers/net/ethernet/intel/ixgb/ixgb_main.c
index ea480837343..b5f94abe3cf 100644
--- a/drivers/net/ethernet/intel/ixgb/ixgb_main.c
+++ b/drivers/net/ethernet/intel/ixgb/ixgb_main.c
@@ -2159,6 +2159,10 @@ map_skb:
 		                                  skb->data,
 		                                  adapter->rx_buffer_len,
 						  DMA_FROM_DEVICE);
+		if (dma_mapping_error(&pdev->dev, buffer_info->dma)) {
+			adapter->alloc_rx_buff_failed++;
+			break;
+		}
 
 		rx_desc = IXGB_RX_DESC(*rx_ring, i);
 		rx_desc->buff_addr = cpu_to_le64(buffer_info->dma);
@@ -2168,7 +2172,8 @@ map_skb:
 		rx_desc->status = 0;
 
 
-		if (++i == rx_ring->count) i = 0;
+		if (++i == rx_ring->count)
+			i = 0;
 		buffer_info = &rx_ring->buffer_info[i];
 	}
 
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
index db5611ae407..79f4a26ea6c 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
@@ -7922,12 +7922,19 @@ static int __init ixgbe_init_module(void)
 	ixgbe_dbg_init();
 #endif /* CONFIG_DEBUG_FS */
 
+	ret = pci_register_driver(&ixgbe_driver);
+	if (ret) {
+#ifdef CONFIG_DEBUG_FS
+		ixgbe_dbg_exit();
+#endif /* CONFIG_DEBUG_FS */
+		return ret;
+	}
+
 #ifdef CONFIG_IXGBE_DCA
 	dca_register_notify(&dca_notifier);
 #endif
 
-	ret = pci_register_driver(&ixgbe_driver);
-	return ret;
+	return 0;
 }
 
 module_init(ixgbe_init_module);
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
index d44b4d21268..97e33669c0b 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
@@ -1049,6 +1049,12 @@ int ixgbe_ndo_set_vf_vlan(struct net_device *netdev, int vf, u16 vlan, u8 qos)
 	if ((vf >= adapter->num_vfs) || (vlan > 4095) || (qos > 7))
 		return -EINVAL;
 	if (vlan || qos) {
+		if (adapter->vfinfo[vf].pf_vlan)
+			err = ixgbe_set_vf_vlan(adapter, false,
+						adapter->vfinfo[vf].pf_vlan,
+						vf);
+		if (err)
+			goto out;
 		err = ixgbe_set_vf_vlan(adapter, true, vlan, vf);
 		if (err)
 			goto out;
diff --git a/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c b/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
index c3db6cd69b6..2b6cb5ca48e 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
+++ b/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
@@ -944,9 +944,17 @@ free_queue_irqs:
 		free_irq(adapter->msix_entries[vector].vector,
 			 adapter->q_vector[vector]);
 	}
-	pci_disable_msix(adapter->pdev);
-	kfree(adapter->msix_entries);
-	adapter->msix_entries = NULL;
+	/* This failure is non-recoverable - it indicates the system is
+	 * out of MSIX vector resources and the VF driver cannot run
+	 * without them.  Set the number of msix vectors to zero
+	 * indicating that not enough can be allocated.  The error
+	 * will be returned to the user indicating device open failed.
+	 * Any further attempts to force the driver to open will also
+	 * fail.  The only way to recover is to unload the driver and
+	 * reload it again.  If the system has recovered some MSIX
+	 * vectors then it may succeed.
+	 */
+	adapter->num_msix_vectors = 0;
 	return err;
 }
 
@@ -2572,6 +2580,15 @@ static int ixgbevf_open(struct net_device *netdev)
 	struct ixgbe_hw *hw = &adapter->hw;
 	int err;
 
+	/* A previous failure to open the device because of a lack of
+	 * available MSIX vector resources may have reset the number
+	 * of msix vectors variable to zero.  The only way to recover
+	 * is to unload/reload the driver and hope that the system has
+	 * been able to recover some MSIX vector resources.
+	 */
+	if (!adapter->num_msix_vectors)
+		return -ENOMEM;
+
 	/* disallow open during test */
 	if (test_bit(__IXGBEVF_TESTING, &adapter->state))
 		return -EBUSY;
@@ -2628,7 +2645,6 @@ static int ixgbevf_open(struct net_device *netdev)
 
 err_req_irq:
 	ixgbevf_down(adapter);
-	ixgbevf_free_irq(adapter);
 err_setup_rx:
 	ixgbevf_free_all_rx_resources(adapter);
 err_setup_tx:
diff --git a/drivers/net/ethernet/lantiq_etop.c b/drivers/net/ethernet/lantiq_etop.c
index 6a2127489af..bfdb0686039 100644
--- a/drivers/net/ethernet/lantiq_etop.c
+++ b/drivers/net/ethernet/lantiq_etop.c
@@ -769,7 +769,7 @@ ltq_etop_probe(struct platform_device *pdev)
 	return 0;
 
 err_free:
-	kfree(dev);
+	free_netdev(dev);
 err_out:
 	return err;
 }
diff --git a/drivers/net/ethernet/marvell/Kconfig b/drivers/net/ethernet/marvell/Kconfig
index edfba937092..434e33c527d 100644
--- a/drivers/net/ethernet/marvell/Kconfig
+++ b/drivers/net/ethernet/marvell/Kconfig
@@ -33,6 +33,7 @@ config MV643XX_ETH
 
 config MVMDIO
 	tristate "Marvell MDIO interface support"
+	select PHYLIB
 	---help---
 	  This driver supports the MDIO interface found in the network
 	  interface units of the Marvell EBU SoCs (Kirkwood, Orion5x,
@@ -45,7 +46,6 @@ config MVMDIO
 config MVNETA
 	tristate "Marvell Armada 370/XP network interface support"
 	depends on MACH_ARMADA_370_XP
-	select PHYLIB
 	select MVMDIO
 	---help---
 	  This driver supports the network interface units in the
diff --git a/drivers/net/ethernet/marvell/mvneta.c b/drivers/net/ethernet/marvell/mvneta.c
index cd345b8969b..a47a097c21e 100644
--- a/drivers/net/ethernet/marvell/mvneta.c
+++ b/drivers/net/ethernet/marvell/mvneta.c
@@ -374,7 +374,6 @@ static int rxq_number = 8;
 static int txq_number = 8;
 
 static int rxq_def;
-static int txq_def;
 
 #define MVNETA_DRIVER_NAME "mvneta"
 #define MVNETA_DRIVER_VERSION "1.0"
@@ -1475,7 +1474,8 @@ error:
 static int mvneta_tx(struct sk_buff *skb, struct net_device *dev)
 {
 	struct mvneta_port *pp = netdev_priv(dev);
-	struct mvneta_tx_queue *txq = &pp->txqs[txq_def];
+	u16 txq_id = skb_get_queue_mapping(skb);
+	struct mvneta_tx_queue *txq = &pp->txqs[txq_id];
 	struct mvneta_tx_desc *tx_desc;
 	struct netdev_queue *nq;
 	int frags = 0;
@@ -1485,7 +1485,7 @@ static int mvneta_tx(struct sk_buff *skb, struct net_device *dev)
 		goto out;
 
 	frags = skb_shinfo(skb)->nr_frags + 1;
-	nq    = netdev_get_tx_queue(dev, txq_def);
+	nq    = netdev_get_tx_queue(dev, txq_id);
 
 	/* Get a descriptor for the first part of the packet */
 	tx_desc = mvneta_txq_next_desc_get(txq);
@@ -2689,7 +2689,7 @@ static int mvneta_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	dev = alloc_etherdev_mq(sizeof(struct mvneta_port), 8);
+	dev = alloc_etherdev_mqs(sizeof(struct mvneta_port), txq_number, rxq_number);
 	if (!dev)
 		return -ENOMEM;
 
@@ -2771,16 +2771,17 @@ static int mvneta_probe(struct platform_device *pdev)
 
 	netif_napi_add(dev, &pp->napi, mvneta_poll, pp->weight);
 
+	dev->features = NETIF_F_SG | NETIF_F_IP_CSUM;
+	dev->hw_features |= NETIF_F_SG | NETIF_F_IP_CSUM;
+	dev->vlan_features |= NETIF_F_SG | NETIF_F_IP_CSUM;
+	dev->priv_flags |= IFF_UNICAST_FLT;
+
 	err = register_netdev(dev);
 	if (err < 0) {
 		dev_err(&pdev->dev, "failed to register\n");
 		goto err_deinit;
 	}
 
-	dev->features = NETIF_F_SG | NETIF_F_IP_CSUM;
-	dev->hw_features = NETIF_F_SG | NETIF_F_IP_CSUM;
-	dev->priv_flags |= IFF_UNICAST_FLT;
-
 	netdev_info(dev, "mac: %pM\n", dev->dev_addr);
 
 	platform_set_drvdata(pdev, pp->dev);
@@ -2843,4 +2844,3 @@ module_param(rxq_number, int, S_IRUGO);
 module_param(txq_number, int, S_IRUGO);
 
 module_param(rxq_def, int, S_IRUGO);
-module_param(txq_def, int, S_IRUGO);
diff --git a/drivers/net/ethernet/marvell/sky2.c b/drivers/net/ethernet/marvell/sky2.c
index fc07ca35721..6a0e671fcec 100644
--- a/drivers/net/ethernet/marvell/sky2.c
+++ b/drivers/net/ethernet/marvell/sky2.c
@@ -1067,7 +1067,7 @@ static void sky2_ramset(struct sky2_hw *hw, u16 q, u32 start, u32 space)
 		sky2_write32(hw, RB_ADDR(q, RB_RX_UTHP), tp);
 		sky2_write32(hw, RB_ADDR(q, RB_RX_LTHP), space/2);
 
-		tp = space - 2048/8;
+		tp = space - 8192/8;
 		sky2_write32(hw, RB_ADDR(q, RB_RX_UTPP), tp);
 		sky2_write32(hw, RB_ADDR(q, RB_RX_LTPP), space/4);
 	} else {
diff --git a/drivers/net/ethernet/marvell/sky2.h b/drivers/net/ethernet/marvell/sky2.h
index 615ac63ea86..ec6dcd80152 100644
--- a/drivers/net/ethernet/marvell/sky2.h
+++ b/drivers/net/ethernet/marvell/sky2.h
@@ -2074,7 +2074,7 @@ enum {
 	GM_IS_RX_FF_OR	= 1<<1,	/* Receive FIFO Overrun */
 	GM_IS_RX_COMPL	= 1<<0,	/* Frame Reception Complete */
 
-#define GMAC_DEF_MSK     GM_IS_TX_FF_UR
+#define GMAC_DEF_MSK     (GM_IS_TX_FF_UR | GM_IS_RX_FF_OR)
 };
 
 /*	GMAC_LINK_CTRL	16 bit	GMAC Link Control Reg (YUKON only) */
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
index 995d4b6d5c1..30d78f806dc 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
@@ -411,8 +411,8 @@ static int mlx4_en_vlan_rx_kill_vid(struct net_device *dev, unsigned short vid)
 
 static void mlx4_en_u64_to_mac(unsigned char dst_mac[ETH_ALEN + 2], u64 src_mac)
 {
-	unsigned int i;
-	for (i = ETH_ALEN - 1; i; --i) {
+	int i;
+	for (i = ETH_ALEN - 1; i >= 0; --i) {
 		dst_mac[i] = src_mac & 0xff;
 		src_mac >>= 8;
 	}
@@ -1637,6 +1637,17 @@ void mlx4_en_stop_port(struct net_device *dev, int detach)
 	/* Flush multicast filter */
 	mlx4_SET_MCAST_FLTR(mdev->dev, priv->port, 0, 1, MLX4_MCAST_CONFIG);
 
+	/* Remove flow steering rules for the port*/
+	if (mdev->dev->caps.steering_mode ==
+	    MLX4_STEERING_MODE_DEVICE_MANAGED) {
+		ASSERT_RTNL();
+		list_for_each_entry_safe(flow, tmp_flow,
+					 &priv->ethtool_list, list) {
+			mlx4_flow_detach(mdev->dev, flow->id);
+			list_del(&flow->list);
+		}
+	}
+
 	mlx4_en_destroy_drop_qp(priv);
 
 	/* Free TX Rings */
@@ -1657,17 +1668,6 @@ void mlx4_en_stop_port(struct net_device *dev, int detach)
 	if (!(mdev->dev->caps.flags2 & MLX4_DEV_CAP_FLAGS2_REASSIGN_MAC_EN))
 		mdev->mac_removed[priv->port] = 1;
 
-	/* Remove flow steering rules for the port*/
-	if (mdev->dev->caps.steering_mode ==
-	    MLX4_STEERING_MODE_DEVICE_MANAGED) {
-		ASSERT_RTNL();
-		list_for_each_entry_safe(flow, tmp_flow,
-					 &priv->ethtool_list, list) {
-			mlx4_flow_detach(mdev->dev, flow->id);
-			list_del(&flow->list);
-		}
-	}
-
 	/* Free RX Rings */
 	for (i = 0; i < priv->rx_ring_num; i++) {
 		mlx4_en_deactivate_rx_ring(priv, &priv->rx_ring[i]);
diff --git a/drivers/net/ethernet/mellanox/mlx4/eq.c b/drivers/net/ethernet/mellanox/mlx4/eq.c
index 251ae2f9311..8e3123a1df8 100644
--- a/drivers/net/ethernet/mellanox/mlx4/eq.c
+++ b/drivers/net/ethernet/mellanox/mlx4/eq.c
@@ -771,7 +771,7 @@ int mlx4_MAP_EQ_wrapper(struct mlx4_dev *dev, int slave,
 	struct mlx4_slave_event_eq_info *event_eq =
 		priv->mfunc.master.slave_state[slave].event_eq;
 	u32 in_modifier = vhcr->in_modifier;
-	u32 eqn = in_modifier & 0x1FF;
+	u32 eqn = in_modifier & 0x3FF;
 	u64 in_param =  vhcr->in_param;
 	int err = 0;
 	int i;
diff --git a/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c b/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
index 2995687f1ae..1391b52f443 100644
--- a/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
+++ b/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
@@ -99,6 +99,7 @@ struct res_qp {
 	struct list_head	mcg_list;
 	spinlock_t		mcg_spl;
 	int			local_qpn;
+	atomic_t		ref_count;
 };
 
 enum res_mtt_states {
@@ -197,6 +198,7 @@ enum res_fs_rule_states {
 
 struct res_fs_rule {
 	struct res_common	com;
+	int			qpn;
 };
 
 static void *res_tracker_lookup(struct rb_root *root, u64 res_id)
@@ -355,7 +357,7 @@ static int mpt_mask(struct mlx4_dev *dev)
 	return dev->caps.num_mpts - 1;
 }
 
-static void *find_res(struct mlx4_dev *dev, int res_id,
+static void *find_res(struct mlx4_dev *dev, u64 res_id,
 		      enum mlx4_resource type)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
@@ -447,6 +449,7 @@ static struct res_common *alloc_qp_tr(int id)
 	ret->local_qpn = id;
 	INIT_LIST_HEAD(&ret->mcg_list);
 	spin_lock_init(&ret->mcg_spl);
+	atomic_set(&ret->ref_count, 0);
 
 	return &ret->com;
 }
@@ -554,7 +557,7 @@ static struct res_common *alloc_xrcdn_tr(int id)
 	return &ret->com;
 }
 
-static struct res_common *alloc_fs_rule_tr(u64 id)
+static struct res_common *alloc_fs_rule_tr(u64 id, int qpn)
 {
 	struct res_fs_rule *ret;
 
@@ -564,7 +567,7 @@ static struct res_common *alloc_fs_rule_tr(u64 id)
 
 	ret->com.res_id = id;
 	ret->com.state = RES_FS_RULE_ALLOCATED;
-
+	ret->qpn = qpn;
 	return &ret->com;
 }
 
@@ -602,7 +605,7 @@ static struct res_common *alloc_tr(u64 id, enum mlx4_resource type, int slave,
 		ret = alloc_xrcdn_tr(id);
 		break;
 	case RES_FS_RULE:
-		ret = alloc_fs_rule_tr(id);
+		ret = alloc_fs_rule_tr(id, extra);
 		break;
 	default:
 		return NULL;
@@ -671,10 +674,14 @@ undo:
 
 static int remove_qp_ok(struct res_qp *res)
 {
-	if (res->com.state == RES_QP_BUSY)
+	if (res->com.state == RES_QP_BUSY || atomic_read(&res->ref_count) ||
+	    !list_empty(&res->mcg_list)) {
+		pr_err("resource tracker: fail to remove qp, state %d, ref_count %d\n",
+		       res->com.state, atomic_read(&res->ref_count));
 		return -EBUSY;
-	else if (res->com.state != RES_QP_RESERVED)
+	} else if (res->com.state != RES_QP_RESERVED) {
 		return -EPERM;
+	}
 
 	return 0;
 }
@@ -3124,6 +3131,7 @@ int mlx4_QP_FLOW_STEERING_ATTACH_wrapper(struct mlx4_dev *dev, int slave,
 	struct list_head *rlist = &tracker->slave_list[slave].res_list[RES_MAC];
 	int err;
 	int qpn;
+	struct res_qp *rqp;
 	struct mlx4_net_trans_rule_hw_ctrl *ctrl;
 	struct _rule_hw  *rule_header;
 	int header_id;
@@ -3134,7 +3142,7 @@ int mlx4_QP_FLOW_STEERING_ATTACH_wrapper(struct mlx4_dev *dev, int slave,
 
 	ctrl = (struct mlx4_net_trans_rule_hw_ctrl *)inbox->buf;
 	qpn = be32_to_cpu(ctrl->qpn) & 0xffffff;
-	err = get_res(dev, slave, qpn, RES_QP, NULL);
+	err = get_res(dev, slave, qpn, RES_QP, &rqp);
 	if (err) {
 		pr_err("Steering rule with qpn 0x%x rejected.\n", qpn);
 		return err;
@@ -3175,14 +3183,16 @@ int mlx4_QP_FLOW_STEERING_ATTACH_wrapper(struct mlx4_dev *dev, int slave,
 	if (err)
 		goto err_put;
 
-	err = add_res_range(dev, slave, vhcr->out_param, 1, RES_FS_RULE, 0);
+	err = add_res_range(dev, slave, vhcr->out_param, 1, RES_FS_RULE, qpn);
 	if (err) {
 		mlx4_err(dev, "Fail to add flow steering resources.\n ");
 		/* detach rule*/
 		mlx4_cmd(dev, vhcr->out_param, 0, 0,
 			 MLX4_QP_FLOW_STEERING_DETACH, MLX4_CMD_TIME_CLASS_A,
 			 MLX4_CMD_NATIVE);
+		goto err_put;
 	}
+	atomic_inc(&rqp->ref_count);
 err_put:
 	put_res(dev, slave, qpn, RES_QP);
 	return err;
@@ -3195,20 +3205,35 @@ int mlx4_QP_FLOW_STEERING_DETACH_wrapper(struct mlx4_dev *dev, int slave,
 					 struct mlx4_cmd_info *cmd)
 {
 	int err;
+	struct res_qp *rqp;
+	struct res_fs_rule *rrule;
 
 	if (dev->caps.steering_mode !=
 	    MLX4_STEERING_MODE_DEVICE_MANAGED)
 		return -EOPNOTSUPP;
 
+	err = get_res(dev, slave, vhcr->in_param, RES_FS_RULE, &rrule);
+	if (err)
+		return err;
+	/* Release the rule form busy state before removal */
+	put_res(dev, slave, vhcr->in_param, RES_FS_RULE);
+	err = get_res(dev, slave, rrule->qpn, RES_QP, &rqp);
+	if (err)
+		return err;
+
 	err = rem_res_range(dev, slave, vhcr->in_param, 1, RES_FS_RULE, 0);
 	if (err) {
 		mlx4_err(dev, "Fail to remove flow steering resources.\n ");
-		return err;
+		goto out;
 	}
 
 	err = mlx4_cmd(dev, vhcr->in_param, 0, 0,
 		       MLX4_QP_FLOW_STEERING_DETACH, MLX4_CMD_TIME_CLASS_A,
 		       MLX4_CMD_NATIVE);
+	if (!err)
+		atomic_dec(&rqp->ref_count);
+out:
+	put_res(dev, slave, rrule->qpn, RES_QP);
 	return err;
 }
 
@@ -3806,6 +3831,7 @@ void mlx4_delete_all_resources_for_slave(struct mlx4_dev *dev, int slave)
 	mutex_lock(&priv->mfunc.master.res_tracker.slave_list[slave].mutex);
 	/*VLAN*/
 	rem_slave_macs(dev, slave);
+	rem_slave_fs_rule(dev, slave);
 	rem_slave_qps(dev, slave);
 	rem_slave_srqs(dev, slave);
 	rem_slave_cqs(dev, slave);
@@ -3814,6 +3840,5 @@ void mlx4_delete_all_resources_for_slave(struct mlx4_dev *dev, int slave)
 	rem_slave_mtts(dev, slave);
 	rem_slave_counters(dev, slave);
 	rem_slave_xrcdns(dev, slave);
-	rem_slave_fs_rule(dev, slave);
 	mutex_unlock(&priv->mfunc.master.res_tracker.slave_list[slave].mutex);
 }
diff --git a/drivers/net/ethernet/micrel/ks8851.c b/drivers/net/ethernet/micrel/ks8851.c
index 33bcb63d56a..8fb481252e2 100644
--- a/drivers/net/ethernet/micrel/ks8851.c
+++ b/drivers/net/ethernet/micrel/ks8851.c
@@ -528,7 +528,7 @@ static void ks8851_rx_pkts(struct ks8851_net *ks)
 	for (; rxfc != 0; rxfc--) {
 		rxh = ks8851_rdreg32(ks, KS_RXFHSR);
 		rxstat = rxh & 0xffff;
-		rxlen = rxh >> 16;
+		rxlen = (rxh >> 16) & 0xfff;
 
 		netif_dbg(ks, rx_status, ks->netdev,
 			  "rx: stat 0x%04x, len 0x%04x\n", rxstat, rxlen);
diff --git a/drivers/net/ethernet/nxp/lpc_eth.c b/drivers/net/ethernet/nxp/lpc_eth.c
index c4122c86f82..efa29b712d5 100644
--- a/drivers/net/ethernet/nxp/lpc_eth.c
+++ b/drivers/net/ethernet/nxp/lpc_eth.c
@@ -1472,7 +1472,8 @@ static int lpc_eth_drv_probe(struct platform_device *pdev)
 	}
 	platform_set_drvdata(pdev, ndev);
 
-	if (lpc_mii_init(pldat) != 0)
+	ret = lpc_mii_init(pldat);
+	if (ret)
 		goto err_out_unregister_netdev;
 
 	netdev_info(ndev, "LPC mac at 0x%08x irq %d\n",
diff --git a/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_main.c b/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_main.c
index 39ab4d09faa..73ce7dd6b95 100644
--- a/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_main.c
+++ b/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_main.c
@@ -1726,9 +1726,9 @@ pch_gbe_clean_rx(struct pch_gbe_adapter *adapter,
 
 			skb->protocol = eth_type_trans(skb, netdev);
 			if (tcp_ip_status & PCH_GBE_RXD_ACC_STAT_TCPIPOK)
-				skb->ip_summed = CHECKSUM_NONE;
-			else
 				skb->ip_summed = CHECKSUM_UNNECESSARY;
+			else
+				skb->ip_summed = CHECKSUM_NONE;
 
 			napi_gro_receive(&adapter->napi, skb);
 			(*work_done)++;
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.c
index cd5ae8813cb..edd63f1230f 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.c
@@ -1500,6 +1500,12 @@ int qlcnic_83xx_loopback_test(struct net_device *netdev, u8 mode)
 		}
 	} while ((adapter->ahw->linkup && ahw->has_link_events) != 1);
 
+	/* Make sure carrier is off and queue is stopped during loopback */
+	if (netif_running(netdev)) {
+		netif_carrier_off(netdev);
+		netif_stop_queue(netdev);
+	}
+
 	ret = qlcnic_do_lb_test(adapter, mode);
 
 	qlcnic_83xx_clear_lb_mode(adapter, mode);
@@ -2780,6 +2786,7 @@ static u64 *qlcnic_83xx_fill_stats(struct qlcnic_adapter *adapter,
 void qlcnic_83xx_get_stats(struct qlcnic_adapter *adapter, u64 *data)
 {
 	struct qlcnic_cmd_args cmd;
+	struct net_device *netdev = adapter->netdev;
 	int ret = 0;
 
 	qlcnic_alloc_mbx_args(&cmd, adapter, QLCNIC_CMD_GET_STATISTICS);
@@ -2789,7 +2796,7 @@ void qlcnic_83xx_get_stats(struct qlcnic_adapter *adapter, u64 *data)
 	data = qlcnic_83xx_fill_stats(adapter, &cmd, data,
 				      QLC_83XX_STAT_TX, &ret);
 	if (ret) {
-		dev_info(&adapter->pdev->dev, "Error getting MAC stats\n");
+		netdev_err(netdev, "Error getting Tx stats\n");
 		goto out;
 	}
 	/* Get MAC stats */
@@ -2799,8 +2806,7 @@ void qlcnic_83xx_get_stats(struct qlcnic_adapter *adapter, u64 *data)
 	data = qlcnic_83xx_fill_stats(adapter, &cmd, data,
 				      QLC_83XX_STAT_MAC, &ret);
 	if (ret) {
-		dev_info(&adapter->pdev->dev,
-			 "Error getting Rx stats\n");
+		netdev_err(netdev, "Error getting MAC stats\n");
 		goto out;
 	}
 	/* Get Rx stats */
@@ -2810,8 +2816,7 @@ void qlcnic_83xx_get_stats(struct qlcnic_adapter *adapter, u64 *data)
 	data = qlcnic_83xx_fill_stats(adapter, &cmd, data,
 				      QLC_83XX_STAT_RX, &ret);
 	if (ret)
-		dev_info(&adapter->pdev->dev,
-			 "Error getting Tx stats\n");
+		netdev_err(netdev, "Error getting Rx stats\n");
 out:
 	qlcnic_free_mbx_args(&cmd);
 }
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_io.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_io.c
index 0e630061bff..5fa847fe388 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_io.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_io.c
@@ -358,8 +358,7 @@ set_flags:
 		memcpy(&first_desc->eth_addr, skb->data, ETH_ALEN);
 	}
 	opcode = TX_ETHER_PKT;
-	if ((adapter->netdev->features & (NETIF_F_TSO | NETIF_F_TSO6)) &&
-	    skb_shinfo(skb)->gso_size > 0) {
+	if (skb_is_gso(skb)) {
 		hdr_len = skb_transport_offset(skb) + tcp_hdrlen(skb);
 		first_desc->mss = cpu_to_le16(skb_shinfo(skb)->gso_size);
 		first_desc->total_hdr_length = hdr_len;
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sysfs.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sysfs.c
index 987fb6f8adc..5ef328af61d 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sysfs.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sysfs.c
@@ -200,10 +200,10 @@ beacon_err:
 	}
 
 	err = qlcnic_config_led(adapter, b_state, b_rate);
-	if (!err)
+	if (!err) {
 		err = len;
-	else
 		ahw->beacon_state = b_state;
+	}
 
 	if (test_and_clear_bit(__QLCNIC_DIAG_RES_ALLOC, &adapter->state))
 		qlcnic_diag_free_res(adapter->netdev, max_sds_rings);
diff --git a/drivers/net/ethernet/qlogic/qlge/qlge.h b/drivers/net/ethernet/qlogic/qlge/qlge.h
index a131d7b5d2f..7e8d6826396 100644
--- a/drivers/net/ethernet/qlogic/qlge/qlge.h
+++ b/drivers/net/ethernet/qlogic/qlge/qlge.h
@@ -18,7 +18,7 @@
  */
 #define DRV_NAME  	"qlge"
 #define DRV_STRING 	"QLogic 10 Gigabit PCI-E Ethernet Driver "
-#define DRV_VERSION	"v1.00.00.31"
+#define DRV_VERSION	"v1.00.00.32"
 
 #define WQ_ADDR_ALIGN	0x3	/* 4 byte alignment */
 
diff --git a/drivers/net/ethernet/qlogic/qlge/qlge_ethtool.c b/drivers/net/ethernet/qlogic/qlge/qlge_ethtool.c
index 6f316ab2325..0780e039b27 100644
--- a/drivers/net/ethernet/qlogic/qlge/qlge_ethtool.c
+++ b/drivers/net/ethernet/qlogic/qlge/qlge_ethtool.c
@@ -379,13 +379,13 @@ static int ql_get_settings(struct net_device *ndev,
 
 	ecmd->supported = SUPPORTED_10000baseT_Full;
 	ecmd->advertising = ADVERTISED_10000baseT_Full;
-	ecmd->autoneg = AUTONEG_ENABLE;
 	ecmd->transceiver = XCVR_EXTERNAL;
 	if ((qdev->link_status & STS_LINK_TYPE_MASK) ==
 				STS_LINK_TYPE_10GBASET) {
 		ecmd->supported |= (SUPPORTED_TP | SUPPORTED_Autoneg);
 		ecmd->advertising |= (ADVERTISED_TP | ADVERTISED_Autoneg);
 		ecmd->port = PORT_TP;
+		ecmd->autoneg = AUTONEG_ENABLE;
 	} else {
 		ecmd->supported |= SUPPORTED_FIBRE;
 		ecmd->advertising |= ADVERTISED_FIBRE;
diff --git a/drivers/net/ethernet/qlogic/qlge/qlge_main.c b/drivers/net/ethernet/qlogic/qlge/qlge_main.c
index b13ab544a7e..8033555e53c 100644
--- a/drivers/net/ethernet/qlogic/qlge/qlge_main.c
+++ b/drivers/net/ethernet/qlogic/qlge/qlge_main.c
@@ -1434,11 +1434,13 @@ map_error:
 }
 
 /* Categorizing receive firmware frame errors */
-static void ql_categorize_rx_err(struct ql_adapter *qdev, u8 rx_err)
+static void ql_categorize_rx_err(struct ql_adapter *qdev, u8 rx_err,
+				 struct rx_ring *rx_ring)
 {
 	struct nic_stats *stats = &qdev->nic_stats;
 
 	stats->rx_err_count++;
+	rx_ring->rx_errors++;
 
 	switch (rx_err & IB_MAC_IOCB_RSP_ERR_MASK) {
 	case IB_MAC_IOCB_RSP_ERR_CODE_ERR:
@@ -1474,6 +1476,12 @@ static void ql_process_mac_rx_gro_page(struct ql_adapter *qdev,
 	struct bq_desc *lbq_desc = ql_get_curr_lchunk(qdev, rx_ring);
 	struct napi_struct *napi = &rx_ring->napi;
 
+	/* Frame error, so drop the packet. */
+	if (ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_ERR_MASK) {
+		ql_categorize_rx_err(qdev, ib_mac_rsp->flags2, rx_ring);
+		put_page(lbq_desc->p.pg_chunk.page);
+		return;
+	}
 	napi->dev = qdev->ndev;
 
 	skb = napi_get_frags(napi);
@@ -1529,6 +1537,12 @@ static void ql_process_mac_rx_page(struct ql_adapter *qdev,
 	addr = lbq_desc->p.pg_chunk.va;
 	prefetch(addr);
 
+	/* Frame error, so drop the packet. */
+	if (ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_ERR_MASK) {
+		ql_categorize_rx_err(qdev, ib_mac_rsp->flags2, rx_ring);
+		goto err_out;
+	}
+
 	/* The max framesize filter on this chip is set higher than
 	 * MTU since FCoE uses 2k frames.
 	 */
@@ -1614,6 +1628,13 @@ static void ql_process_mac_rx_skb(struct ql_adapter *qdev,
 	memcpy(skb_put(new_skb, length), skb->data, length);
 	skb = new_skb;
 
+	/* Frame error, so drop the packet. */
+	if (ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_ERR_MASK) {
+		ql_categorize_rx_err(qdev, ib_mac_rsp->flags2, rx_ring);
+		dev_kfree_skb_any(skb);
+		return;
+	}
+
 	/* loopback self test for ethtool */
 	if (test_bit(QL_SELFTEST, &qdev->flags)) {
 		ql_check_lb_frame(qdev, skb);
@@ -1919,6 +1940,13 @@ static void ql_process_mac_split_rx_intr(struct ql_adapter *qdev,
 		return;
 	}
 
+	/* Frame error, so drop the packet. */
+	if (ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_ERR_MASK) {
+		ql_categorize_rx_err(qdev, ib_mac_rsp->flags2, rx_ring);
+		dev_kfree_skb_any(skb);
+		return;
+	}
+
 	/* The max framesize filter on this chip is set higher than
 	 * MTU since FCoE uses 2k frames.
 	 */
@@ -2000,12 +2028,6 @@ static unsigned long ql_process_mac_rx_intr(struct ql_adapter *qdev,
 
 	QL_DUMP_IB_MAC_RSP(ib_mac_rsp);
 
-	/* Frame error, so drop the packet. */
-	if (ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_ERR_MASK) {
-		ql_categorize_rx_err(qdev, ib_mac_rsp->flags2);
-		return (unsigned long)length;
-	}
-
 	if (ib_mac_rsp->flags4 & IB_MAC_IOCB_RSP_HV) {
 		/* The data and headers are split into
 		 * separate buffers.
diff --git a/drivers/net/ethernet/realtek/r8169.c b/drivers/net/ethernet/realtek/r8169.c
index 28fb50a1e9c..4ecbe64a758 100644
--- a/drivers/net/ethernet/realtek/r8169.c
+++ b/drivers/net/ethernet/realtek/r8169.c
@@ -3818,6 +3818,30 @@ static void rtl_init_mdio_ops(struct rtl8169_private *tp)
 	}
 }
 
+static void rtl_speed_down(struct rtl8169_private *tp)
+{
+	u32 adv;
+	int lpa;
+
+	rtl_writephy(tp, 0x1f, 0x0000);
+	lpa = rtl_readphy(tp, MII_LPA);
+
+	if (lpa & (LPA_10HALF | LPA_10FULL))
+		adv = ADVERTISED_10baseT_Half | ADVERTISED_10baseT_Full;
+	else if (lpa & (LPA_100HALF | LPA_100FULL))
+		adv = ADVERTISED_10baseT_Half | ADVERTISED_10baseT_Full |
+		      ADVERTISED_100baseT_Half | ADVERTISED_100baseT_Full;
+	else
+		adv = ADVERTISED_10baseT_Half | ADVERTISED_10baseT_Full |
+		      ADVERTISED_100baseT_Half | ADVERTISED_100baseT_Full |
+		      (tp->mii.supports_gmii ?
+		       ADVERTISED_1000baseT_Half |
+		       ADVERTISED_1000baseT_Full : 0);
+
+	rtl8169_set_speed(tp->dev, AUTONEG_ENABLE, SPEED_1000, DUPLEX_FULL,
+			  adv);
+}
+
 static void rtl_wol_suspend_quirk(struct rtl8169_private *tp)
 {
 	void __iomem *ioaddr = tp->mmio_addr;
@@ -3848,9 +3872,7 @@ static bool rtl_wol_pll_power_down(struct rtl8169_private *tp)
 	if (!(__rtl8169_get_wol(tp) & WAKE_ANY))
 		return false;
 
-	rtl_writephy(tp, 0x1f, 0x0000);
-	rtl_writephy(tp, MII_BMCR, 0x0000);
-
+	rtl_speed_down(tp);
 	rtl_wol_suspend_quirk(tp);
 
 	return true;
diff --git a/drivers/net/ethernet/renesas/sh_eth.c b/drivers/net/ethernet/renesas/sh_eth.c
index 33e96176e4d..6ed333fe5c0 100644
--- a/drivers/net/ethernet/renesas/sh_eth.c
+++ b/drivers/net/ethernet/renesas/sh_eth.c
@@ -1216,10 +1216,7 @@ static void sh_eth_error(struct net_device *ndev, int intr_status)
 		if (felic_stat & ECSR_LCHNG) {
 			/* Link Changed */
 			if (mdp->cd->no_psr || mdp->no_ether_link) {
-				if (mdp->link == PHY_DOWN)
-					link_stat = 0;
-				else
-					link_stat = PHY_ST_LINK;
+				goto ignore_link;
 			} else {
 				link_stat = (sh_eth_read(ndev, PSR));
 				if (mdp->ether_link_active_low)
@@ -1242,6 +1239,7 @@ static void sh_eth_error(struct net_device *ndev, int intr_status)
 		}
 	}
 
+ignore_link:
 	if (intr_status & EESR_TWB) {
 		/* Write buck end. unused write back interrupt */
 		if (intr_status & EESR_TABT)	/* Transmit Abort int */
@@ -1326,12 +1324,18 @@ static irqreturn_t sh_eth_interrupt(int irq, void *netdev)
 	struct sh_eth_private *mdp = netdev_priv(ndev);
 	struct sh_eth_cpu_data *cd = mdp->cd;
 	irqreturn_t ret = IRQ_NONE;
-	u32 intr_status = 0;
+	unsigned long intr_status;
 
 	spin_lock(&mdp->lock);
 
-	/* Get interrpt stat */
+	/* Get interrupt status */
 	intr_status = sh_eth_read(ndev, EESR);
+	/* Mask it with the interrupt mask, forcing ECI interrupt to be always
+	 * enabled since it's the one that  comes thru regardless of the mask,
+	 * and we need to fully handle it in sh_eth_error() in order to quench
+	 * it as it doesn't get cleared by just writing 1 to the ECI bit...
+	 */
+	intr_status &= sh_eth_read(ndev, EESIPR) | DMAC_M_ECI;
 	/* Clear interrupt */
 	if (intr_status & (EESR_FRC | EESR_RMAF | EESR_RRF |
 			EESR_RTLF | EESR_RTSF | EESR_PRE | EESR_CERF |
@@ -1373,7 +1377,7 @@ static void sh_eth_adjust_link(struct net_device *ndev)
 	struct phy_device *phydev = mdp->phydev;
 	int new_state = 0;
 
-	if (phydev->link != PHY_DOWN) {
+	if (phydev->link) {
 		if (phydev->duplex != mdp->duplex) {
 			new_state = 1;
 			mdp->duplex = phydev->duplex;
@@ -1387,17 +1391,21 @@ static void sh_eth_adjust_link(struct net_device *ndev)
 			if (mdp->cd->set_rate)
 				mdp->cd->set_rate(ndev);
 		}
-		if (mdp->link == PHY_DOWN) {
+		if (!mdp->link) {
 			sh_eth_write(ndev,
 				(sh_eth_read(ndev, ECMR) & ~ECMR_TXF), ECMR);
 			new_state = 1;
 			mdp->link = phydev->link;
+			if (mdp->cd->no_psr || mdp->no_ether_link)
+				sh_eth_rcv_snd_enable(ndev);
 		}
 	} else if (mdp->link) {
 		new_state = 1;
-		mdp->link = PHY_DOWN;
+		mdp->link = 0;
 		mdp->speed = 0;
 		mdp->duplex = -1;
+		if (mdp->cd->no_psr || mdp->no_ether_link)
+			sh_eth_rcv_snd_disable(ndev);
 	}
 
 	if (new_state && netif_msg_link(mdp))
@@ -1414,7 +1422,7 @@ static int sh_eth_phy_init(struct net_device *ndev)
 	snprintf(phy_id, sizeof(phy_id), PHY_ID_FMT,
 		mdp->mii_bus->id , mdp->phy_id);
 
-	mdp->link = PHY_DOWN;
+	mdp->link = 0;
 	mdp->speed = 0;
 	mdp->duplex = -1;
 
@@ -2220,6 +2228,7 @@ static void sh_eth_tsu_init(struct sh_eth_private *mdp)
 /* MDIO bus release function */
 static int sh_mdio_release(struct net_device *ndev)
 {
+	struct sh_eth_private *mdp = netdev_priv(ndev);
 	struct mii_bus *bus = dev_get_drvdata(&ndev->dev);
 
 	/* unregister mdio bus */
@@ -2234,6 +2243,9 @@ static int sh_mdio_release(struct net_device *ndev)
 	/* free bitbang info */
 	free_mdio_bitbang(bus);
 
+	/* free bitbang memory */
+	kfree(mdp->bitbang);
+
 	return 0;
 }
 
@@ -2262,6 +2274,7 @@ static int sh_mdio_init(struct net_device *ndev, int id,
 	bitbang->ctrl.ops = &bb_ops;
 
 	/* MII controller setting */
+	mdp->bitbang = bitbang;
 	mdp->mii_bus = alloc_mdio_bitbang(&bitbang->ctrl);
 	if (!mdp->mii_bus) {
 		ret = -ENOMEM;
@@ -2441,6 +2454,11 @@ static int sh_eth_drv_probe(struct platform_device *pdev)
 		}
 		mdp->tsu_addr = ioremap(rtsu->start,
 					resource_size(rtsu));
+		if (mdp->tsu_addr == NULL) {
+			ret = -ENOMEM;
+			dev_err(&pdev->dev, "TSU ioremap failed.\n");
+			goto out_release;
+		}
 		mdp->port = devno % 2;
 		ndev->features = NETIF_F_HW_VLAN_FILTER;
 	}
diff --git a/drivers/net/ethernet/renesas/sh_eth.h b/drivers/net/ethernet/renesas/sh_eth.h
index bae84fd2e73..828be451500 100644
--- a/drivers/net/ethernet/renesas/sh_eth.h
+++ b/drivers/net/ethernet/renesas/sh_eth.h
@@ -705,6 +705,7 @@ struct sh_eth_private {
 	const u16 *reg_offset;
 	void __iomem *addr;
 	void __iomem *tsu_addr;
+	struct bb_info *bitbang;
 	u32 num_rx_ring;
 	u32 num_tx_ring;
 	dma_addr_t rx_desc_dma;
@@ -722,7 +723,7 @@ struct sh_eth_private {
 	u32 phy_id;					/* PHY ID */
 	struct mii_bus *mii_bus;	/* MDIO bus control */
 	struct phy_device *phydev;	/* PHY device control */
-	enum phy_state link;
+	int link;
 	phy_interface_t phy_interface;
 	int msg_enable;
 	int speed;
diff --git a/drivers/net/ethernet/sfc/Kconfig b/drivers/net/ethernet/sfc/Kconfig
index 435b4f1e348..4136ccc4a95 100644
--- a/drivers/net/ethernet/sfc/Kconfig
+++ b/drivers/net/ethernet/sfc/Kconfig
@@ -24,7 +24,7 @@ config SFC_MCDI_MON
 	bool "Solarflare SFC9000-family hwmon support"
 	depends on SFC && HWMON && !(SFC=y && HWMON=m)
 	default y
-	----help---
+	---help---
 	  This exposes the on-board firmware-managed sensors as a
 	  hardware monitor device.
 config SFC_SRIOV
diff --git a/drivers/net/ethernet/smsc/Kconfig b/drivers/net/ethernet/smsc/Kconfig
index 5a689af516e..bb4c1674ff9 100644
--- a/drivers/net/ethernet/smsc/Kconfig
+++ b/drivers/net/ethernet/smsc/Kconfig
@@ -5,7 +5,7 @@
 config NET_VENDOR_SMSC
 	bool "SMC (SMSC)/Western Digital devices"
 	default y
-	depends on ARM || ISA || MAC || ARM || MIPS || M32R || SUPERH || \
+	depends on ARM || ISA || MAC || ARM64 || MIPS || M32R || SUPERH || \
 		BLACKFIN || MN10300 || COLDFIRE || PCI || PCMCIA
 	---help---
 	  If you have a network (Ethernet) card belonging to this class, say Y
@@ -40,7 +40,7 @@ config SMC91X
 	select NET_CORE
 	select MII
 	depends on (ARM || M32R || SUPERH || MIPS || BLACKFIN || \
-		    MN10300 || COLDFIRE)
+		    MN10300 || COLDFIRE || ARM64)
 	---help---
 	  This is a driver for SMC's 91x series of Ethernet chipsets,
 	  including the SMC91C94 and the SMC91C111. Say Y if you want it
diff --git a/drivers/net/ethernet/smsc/smc91c92_cs.c b/drivers/net/ethernet/smsc/smc91c92_cs.c
index 04393b5fef7..656d2e2ebfc 100644
--- a/drivers/net/ethernet/smsc/smc91c92_cs.c
+++ b/drivers/net/ethernet/smsc/smc91c92_cs.c
@@ -2054,16 +2054,4 @@ static struct pcmcia_driver smc91c92_cs_driver = {
 	.suspend	= smc91c92_suspend,
 	.resume		= smc91c92_resume,
 };
-
-static int __init init_smc91c92_cs(void)
-{
-	return pcmcia_register_driver(&smc91c92_cs_driver);
-}
-
-static void __exit exit_smc91c92_cs(void)
-{
-	pcmcia_unregister_driver(&smc91c92_cs_driver);
-}
-
-module_init(init_smc91c92_cs);
-module_exit(exit_smc91c92_cs);
+module_pcmcia_driver(smc91c92_cs_driver);
diff --git a/drivers/net/ethernet/stmicro/stmmac/mmc_core.c b/drivers/net/ethernet/stmicro/stmmac/mmc_core.c
index 0c74a702d46..50617c5a0bd 100644
--- a/drivers/net/ethernet/stmicro/stmmac/mmc_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/mmc_core.c
@@ -149,6 +149,7 @@ void dwmac_mmc_intr_all_mask(void __iomem *ioaddr)
 {
 	writel(MMC_DEFAULT_MASK, ioaddr + MMC_RX_INTR_MASK);
 	writel(MMC_DEFAULT_MASK, ioaddr + MMC_TX_INTR_MASK);
+	writel(MMC_DEFAULT_MASK, ioaddr + MMC_RX_IPC_INTR_MASK);
 }
 
 /* This reads the MAC core counters (if actaully supported).
diff --git a/drivers/net/ethernet/ti/cpsw.c b/drivers/net/ethernet/ti/cpsw.c
index 75c48558e6f..4781d3d8e18 100644
--- a/drivers/net/ethernet/ti/cpsw.c
+++ b/drivers/net/ethernet/ti/cpsw.c
@@ -436,7 +436,7 @@ void cpsw_tx_handler(void *token, int len, int status)
 	 * queue is stopped then start the queue as we have free desc for tx
 	 */
 	if (unlikely(netif_queue_stopped(ndev)))
-		netif_start_queue(ndev);
+		netif_wake_queue(ndev);
 	cpts_tx_timestamp(priv->cpts, skb);
 	priv->stats.tx_packets++;
 	priv->stats.tx_bytes += len;
@@ -1364,7 +1364,7 @@ static int cpsw_probe_dt(struct cpsw_platform_data *data,
 		struct platform_device *mdio;
 
 		parp = of_get_property(slave_node, "phy_id", &lenp);
-		if ((parp == NULL) && (lenp != (sizeof(void *) * 2))) {
+		if ((parp == NULL) || (lenp != (sizeof(void *) * 2))) {
 			pr_err("Missing slave[%d] phy_id property\n", i);
 			ret = -EINVAL;
 			goto error_ret;
@@ -1380,7 +1380,7 @@ static int cpsw_probe_dt(struct cpsw_platform_data *data,
 			memcpy(slave_data->mac_addr, mac_addr, ETH_ALEN);
 
 		if (data->dual_emac) {
-			if (of_property_read_u32(node, "dual_emac_res_vlan",
+			if (of_property_read_u32(slave_node, "dual_emac_res_vlan",
 						 &prop)) {
 				pr_err("Missing dual_emac_res_vlan in DT.\n");
 				slave_data->dual_emac_res_vlan = i+1;
diff --git a/drivers/net/ethernet/ti/cpts.c b/drivers/net/ethernet/ti/cpts.c
index 463597f919f..8c351f100ac 100644
--- a/drivers/net/ethernet/ti/cpts.c
+++ b/drivers/net/ethernet/ti/cpts.c
@@ -94,7 +94,7 @@ static int cpts_fifo_read(struct cpts *cpts, int match)
 		case CPTS_EV_HW:
 			break;
 		default:
-			pr_err("cpts: unkown event type\n");
+			pr_err("cpts: unknown event type\n");
 			break;
 		}
 		if (type == match)
diff --git a/drivers/net/ethernet/ti/davinci_emac.c b/drivers/net/ethernet/ti/davinci_emac.c
index ae1b77aa199..72300bc9e37 100644
--- a/drivers/net/ethernet/ti/davinci_emac.c
+++ b/drivers/net/ethernet/ti/davinci_emac.c
@@ -1053,7 +1053,7 @@ static void emac_tx_handler(void *token, int len, int status)
 	 * queue is stopped then start the queue as we have free desc for tx
 	 */
 	if (unlikely(netif_queue_stopped(ndev)))
-		netif_start_queue(ndev);
+		netif_wake_queue(ndev);
 	ndev->stats.tx_packets++;
 	ndev->stats.tx_bytes += len;
 	dev_kfree_skb_any(skb);
diff --git a/drivers/net/ethernet/xircom/xirc2ps_cs.c b/drivers/net/ethernet/xircom/xirc2ps_cs.c
index 98e09d0d3ce..1025b4e937d 100644
--- a/drivers/net/ethernet/xircom/xirc2ps_cs.c
+++ b/drivers/net/ethernet/xircom/xirc2ps_cs.c
@@ -1775,21 +1775,7 @@ static struct pcmcia_driver xirc2ps_cs_driver = {
 	.suspend	= xirc2ps_suspend,
 	.resume		= xirc2ps_resume,
 };
-
-static int __init
-init_xirc2ps_cs(void)
-{
-	return pcmcia_register_driver(&xirc2ps_cs_driver);
-}
-
-static void __exit
-exit_xirc2ps_cs(void)
-{
-	pcmcia_unregister_driver(&xirc2ps_cs_driver);
-}
-
-module_init(init_xirc2ps_cs);
-module_exit(exit_xirc2ps_cs);
+module_pcmcia_driver(xirc2ps_cs_driver);
 
 #ifndef MODULE
 static int __init setup_xirc2ps_cs(char *str)
diff --git a/drivers/net/hyperv/netvsc.c b/drivers/net/hyperv/netvsc.c
index 1cd77483da5..f5f0f09e4cc 100644
--- a/drivers/net/hyperv/netvsc.c
+++ b/drivers/net/hyperv/netvsc.c
@@ -470,8 +470,10 @@ static void netvsc_send_completion(struct hv_device *device,
 			packet->trans_id;
 
 		/* Notify the layer above us */
-		nvsc_packet->completion.send.send_completion(
-			nvsc_packet->completion.send.send_completion_ctx);
+		if (nvsc_packet)
+			nvsc_packet->completion.send.send_completion(
+				nvsc_packet->completion.send.
+				send_completion_ctx);
 
 		num_outstanding_sends =
 			atomic_dec_return(&net_device->num_outstanding_sends);
@@ -498,6 +500,7 @@ int netvsc_send(struct hv_device *device,
 	int ret = 0;
 	struct nvsp_message sendMessage;
 	struct net_device *ndev;
+	u64 req_id;
 
 	net_device = get_outbound_net_device(device);
 	if (!net_device)
@@ -518,20 +521,24 @@ int netvsc_send(struct hv_device *device,
 		0xFFFFFFFF;
 	sendMessage.msg.v1_msg.send_rndis_pkt.send_buf_section_size = 0;
 
+	if (packet->completion.send.send_completion)
+		req_id = (u64)packet;
+	else
+		req_id = 0;
+
 	if (packet->page_buf_cnt) {
 		ret = vmbus_sendpacket_pagebuffer(device->channel,
 						  packet->page_buf,
 						  packet->page_buf_cnt,
 						  &sendMessage,
 						  sizeof(struct nvsp_message),
-						  (unsigned long)packet);
+						  req_id);
 	} else {
 		ret = vmbus_sendpacket(device->channel, &sendMessage,
 				sizeof(struct nvsp_message),
-				(unsigned long)packet,
+				req_id,
 				VM_PKT_DATA_INBAND,
 				VMBUS_DATA_PACKET_FLAG_COMPLETION_REQUESTED);
-
 	}
 
 	if (ret == 0) {
diff --git a/drivers/net/hyperv/netvsc_drv.c b/drivers/net/hyperv/netvsc_drv.c
index 5f85205cd12..8341b62e552 100644
--- a/drivers/net/hyperv/netvsc_drv.c
+++ b/drivers/net/hyperv/netvsc_drv.c
@@ -241,13 +241,11 @@ void netvsc_linkstatus_callback(struct hv_device *device_obj,
 
 	if (status == 1) {
 		netif_carrier_on(net);
-		netif_wake_queue(net);
 		ndev_ctx = netdev_priv(net);
 		schedule_delayed_work(&ndev_ctx->dwork, 0);
 		schedule_delayed_work(&ndev_ctx->dwork, msecs_to_jiffies(20));
 	} else {
 		netif_carrier_off(net);
-		netif_tx_disable(net);
 	}
 }
 
diff --git a/drivers/net/hyperv/rndis_filter.c b/drivers/net/hyperv/rndis_filter.c
index 2b657d4d63a..0775f0aefd1 100644
--- a/drivers/net/hyperv/rndis_filter.c
+++ b/drivers/net/hyperv/rndis_filter.c
@@ -61,9 +61,6 @@ struct rndis_request {
 
 static void rndis_filter_send_completion(void *ctx);
 
-static void rndis_filter_send_request_completion(void *ctx);
-
-
 
 static struct rndis_device *get_rndis_device(void)
 {
@@ -241,10 +238,7 @@ static int rndis_filter_send_request(struct rndis_device *dev,
 			packet->page_buf[0].len;
 	}
 
-	packet->completion.send.send_completion_ctx = req;/* packet; */
-	packet->completion.send.send_completion =
-		rndis_filter_send_request_completion;
-	packet->completion.send.send_completion_tid = (unsigned long)dev;
+	packet->completion.send.send_completion = NULL;
 
 	ret = netvsc_send(dev->net_dev->dev, packet);
 	return ret;
@@ -999,9 +993,3 @@ static void rndis_filter_send_completion(void *ctx)
 	/* Pass it back to the original handler */
 	filter_pkt->completion(filter_pkt->completion_ctx);
 }
-
-
-static void rndis_filter_send_request_completion(void *ctx)
-{
-	/* Noop */
-}
diff --git a/drivers/net/ppp/ppp_async.c b/drivers/net/ppp/ppp_async.c
index a031f6b456b..9c889e0303d 100644
--- a/drivers/net/ppp/ppp_async.c
+++ b/drivers/net/ppp/ppp_async.c
@@ -314,7 +314,7 @@ ppp_asynctty_ioctl(struct tty_struct *tty, struct file *file,
 		/* flush our buffers and the serial port's buffer */
 		if (arg == TCIOFLUSH || arg == TCOFLUSH)
 			ppp_async_flush_output(ap);
-		err = tty_perform_flush(tty, arg);
+		err = n_tty_ioctl_helper(tty, file, cmd, arg);
 		break;
 
 	case FIONREAD:
diff --git a/drivers/net/ppp/ppp_synctty.c b/drivers/net/ppp/ppp_synctty.c
index 1a12033d2ef..bdf3b13a71a 100644
--- a/drivers/net/ppp/ppp_synctty.c
+++ b/drivers/net/ppp/ppp_synctty.c
@@ -355,7 +355,7 @@ ppp_synctty_ioctl(struct tty_struct *tty, struct file *file,
 		/* flush our buffers and the serial port's buffer */
 		if (arg == TCIOFLUSH || arg == TCOFLUSH)
 			ppp_sync_flush_output(ap);
-		err = tty_perform_flush(tty, arg);
+		err = n_tty_ioctl_helper(tty, file, cmd, arg);
 		break;
 
 	case FIONREAD:
diff --git a/drivers/net/tun.c b/drivers/net/tun.c
index b7c457adc0d..729ed533bb3 100644
--- a/drivers/net/tun.c
+++ b/drivers/net/tun.c
@@ -1594,7 +1594,7 @@ static int tun_set_iff(struct net *net, struct file *file, struct ifreq *ifr)
 
 		if (tun->flags & TUN_TAP_MQ &&
 		    (tun->numqueues + tun->numdisabled > 1))
-			return err;
+			return -EBUSY;
 	}
 	else {
 		char *name;
diff --git a/drivers/net/usb/cdc_mbim.c b/drivers/net/usb/cdc_mbim.c
index 16c84299729..32a76059e7d 100644
--- a/drivers/net/usb/cdc_mbim.c
+++ b/drivers/net/usb/cdc_mbim.c
@@ -134,7 +134,7 @@ static struct sk_buff *cdc_mbim_tx_fixup(struct usbnet *dev, struct sk_buff *skb
 		goto error;
 
 	if (skb) {
-		if (skb->len <= sizeof(ETH_HLEN))
+		if (skb->len <= ETH_HLEN)
 			goto error;
 
 		/* mapping VLANs to MBIM sessions:
@@ -323,6 +323,11 @@ static int cdc_mbim_suspend(struct usb_interface *intf, pm_message_t message)
 		goto error;
 	}
 
+	/*
+	 * Both usbnet_suspend() and subdriver->suspend() MUST return 0
+	 * in system sleep context, otherwise, the resume callback has
+	 * to recover device from previous suspend failure.
+	 */
 	ret = usbnet_suspend(intf, message);
 	if (ret < 0)
 		goto error;
diff --git a/drivers/net/usb/hso.c b/drivers/net/usb/hso.c
index e2dd3249b6b..cba1d46e672 100644
--- a/drivers/net/usb/hso.c
+++ b/drivers/net/usb/hso.c
@@ -1925,7 +1925,6 @@ static void hso_std_serial_write_bulk_callback(struct urb *urb)
 {
 	struct hso_serial *serial = urb->context;
 	int status = urb->status;
-	struct tty_struct *tty;
 
 	/* sanity check */
 	if (!serial) {
@@ -1941,11 +1940,7 @@ static void hso_std_serial_write_bulk_callback(struct urb *urb)
 		return;
 	}
 	hso_put_activity(serial->parent);
-	tty = tty_port_tty_get(&serial->port);
-	if (tty) {
-		tty_wakeup(tty);
-		tty_kref_put(tty);
-	}
+	tty_port_tty_wakeup(&serial->port);
 	hso_kick_transmit(serial);
 
 	D1(" ");
@@ -2008,12 +2003,8 @@ static void ctrl_callback(struct urb *urb)
 		put_rxbuf_data_and_resubmit_ctrl_urb(serial);
 		spin_unlock(&serial->serial_lock);
 	} else {
-		struct tty_struct *tty = tty_port_tty_get(&serial->port);
 		hso_put_activity(serial->parent);
-		if (tty) {
-			tty_wakeup(tty);
-			tty_kref_put(tty);
-		}
+		tty_port_tty_wakeup(&serial->port);
 		/* response to a write command */
 		hso_kick_transmit(serial);
 	}
@@ -3133,18 +3124,13 @@ static void hso_serial_ref_free(struct kref *ref)
 static void hso_free_interface(struct usb_interface *interface)
 {
 	struct hso_serial *hso_dev;
-	struct tty_struct *tty;
 	int i;
 
 	for (i = 0; i < HSO_SERIAL_TTY_MINORS; i++) {
 		if (serial_table[i] &&
 		    (serial_table[i]->interface == interface)) {
 			hso_dev = dev2ser(serial_table[i]);
-			tty = tty_port_tty_get(&hso_dev->port);
-			if (tty) {
-				tty_hangup(tty);
-				tty_kref_put(tty);
-			}
+			tty_port_tty_hangup(&hso_dev->port, false);
 			mutex_lock(&hso_dev->parent->mutex);
 			hso_dev->parent->usb_gone = 1;
 			mutex_unlock(&hso_dev->parent->mutex);
diff --git a/drivers/net/usb/qmi_wwan.c b/drivers/net/usb/qmi_wwan.c
index 968d5d50751..5a88e72090c 100644
--- a/drivers/net/usb/qmi_wwan.c
+++ b/drivers/net/usb/qmi_wwan.c
@@ -13,6 +13,7 @@
 #include <linux/module.h>
 #include <linux/netdevice.h>
 #include <linux/ethtool.h>
+#include <linux/etherdevice.h>
 #include <linux/mii.h>
 #include <linux/usb.h>
 #include <linux/usb/cdc.h>
@@ -52,6 +53,96 @@ struct qmi_wwan_state {
 	struct usb_interface *data;
 };
 
+/* default ethernet address used by the modem */
+static const u8 default_modem_addr[ETH_ALEN] = {0x02, 0x50, 0xf3};
+
+/* Make up an ethernet header if the packet doesn't have one.
+ *
+ * A firmware bug common among several devices cause them to send raw
+ * IP packets under some circumstances.  There is no way for the
+ * driver/host to know when this will happen.  And even when the bug
+ * hits, some packets will still arrive with an intact header.
+ *
+ * The supported devices are only capably of sending IPv4, IPv6 and
+ * ARP packets on a point-to-point link. Any packet with an ethernet
+ * header will have either our address or a broadcast/multicast
+ * address as destination.  ARP packets will always have a header.
+ *
+ * This means that this function will reliably add the appropriate
+ * header iff necessary, provided our hardware address does not start
+ * with 4 or 6.
+ *
+ * Another common firmware bug results in all packets being addressed
+ * to 00:a0:c6:00:00:00 despite the host address being different.
+ * This function will also fixup such packets.
+ */
+static int qmi_wwan_rx_fixup(struct usbnet *dev, struct sk_buff *skb)
+{
+	__be16 proto;
+
+	/* usbnet rx_complete guarantees that skb->len is at least
+	 * hard_header_len, so we can inspect the dest address without
+	 * checking skb->len
+	 */
+	switch (skb->data[0] & 0xf0) {
+	case 0x40:
+		proto = htons(ETH_P_IP);
+		break;
+	case 0x60:
+		proto = htons(ETH_P_IPV6);
+		break;
+	case 0x00:
+		if (is_multicast_ether_addr(skb->data))
+			return 1;
+		/* possibly bogus destination - rewrite just in case */
+		skb_reset_mac_header(skb);
+		goto fix_dest;
+	default:
+		/* pass along other packets without modifications */
+		return 1;
+	}
+	if (skb_headroom(skb) < ETH_HLEN)
+		return 0;
+	skb_push(skb, ETH_HLEN);
+	skb_reset_mac_header(skb);
+	eth_hdr(skb)->h_proto = proto;
+	memset(eth_hdr(skb)->h_source, 0, ETH_ALEN);
+fix_dest:
+	memcpy(eth_hdr(skb)->h_dest, dev->net->dev_addr, ETH_ALEN);
+	return 1;
+}
+
+/* very simplistic detection of IPv4 or IPv6 headers */
+static bool possibly_iphdr(const char *data)
+{
+	return (data[0] & 0xd0) == 0x40;
+}
+
+/* disallow addresses which may be confused with IP headers */
+static int qmi_wwan_mac_addr(struct net_device *dev, void *p)
+{
+	int ret;
+	struct sockaddr *addr = p;
+
+	ret = eth_prepare_mac_addr_change(dev, p);
+	if (ret < 0)
+		return ret;
+	if (possibly_iphdr(addr->sa_data))
+		return -EADDRNOTAVAIL;
+	eth_commit_mac_addr_change(dev, p);
+	return 0;
+}
+
+static const struct net_device_ops qmi_wwan_netdev_ops = {
+	.ndo_open		= usbnet_open,
+	.ndo_stop		= usbnet_stop,
+	.ndo_start_xmit		= usbnet_start_xmit,
+	.ndo_tx_timeout		= usbnet_tx_timeout,
+	.ndo_change_mtu		= usbnet_change_mtu,
+	.ndo_set_mac_address	= qmi_wwan_mac_addr,
+	.ndo_validate_addr	= eth_validate_addr,
+};
+
 /* using a counter to merge subdriver requests with our own into a combined state */
 static int qmi_wwan_manage_power(struct usbnet *dev, int on)
 {
@@ -229,6 +320,18 @@ next_desc:
 		usb_driver_release_interface(driver, info->data);
 	}
 
+	/* Never use the same address on both ends of the link, even
+	 * if the buggy firmware told us to.
+	 */
+	if (!compare_ether_addr(dev->net->dev_addr, default_modem_addr))
+		eth_hw_addr_random(dev->net);
+
+	/* make MAC addr easily distinguishable from an IP header */
+	if (possibly_iphdr(dev->net->dev_addr)) {
+		dev->net->dev_addr[0] |= 0x02;	/* set local assignment bit */
+		dev->net->dev_addr[0] &= 0xbf;	/* clear "IP" bit */
+	}
+	dev->net->netdev_ops = &qmi_wwan_netdev_ops;
 err:
 	return status;
 }
@@ -271,6 +374,11 @@ static int qmi_wwan_suspend(struct usb_interface *intf, pm_message_t message)
 	struct qmi_wwan_state *info = (void *)&dev->data;
 	int ret;
 
+	/*
+	 * Both usbnet_suspend() and subdriver->suspend() MUST return 0
+	 * in system sleep context, otherwise, the resume callback has
+	 * to recover device from previous suspend failure.
+	 */
 	ret = usbnet_suspend(intf, message);
 	if (ret < 0)
 		goto err;
@@ -307,6 +415,7 @@ static const struct driver_info	qmi_wwan_info = {
 	.bind		= qmi_wwan_bind,
 	.unbind		= qmi_wwan_unbind,
 	.manage_power	= qmi_wwan_manage_power,
+	.rx_fixup       = qmi_wwan_rx_fixup,
 };
 
 #define HUAWEI_VENDOR_ID	0x12D1
diff --git a/drivers/net/usb/smsc75xx.c b/drivers/net/usb/smsc75xx.c
index 9abe51710f2..75409748c77 100644
--- a/drivers/net/usb/smsc75xx.c
+++ b/drivers/net/usb/smsc75xx.c
@@ -914,8 +914,12 @@ static int smsc75xx_set_rx_max_frame_length(struct usbnet *dev, int size)
 static int smsc75xx_change_mtu(struct net_device *netdev, int new_mtu)
 {
 	struct usbnet *dev = netdev_priv(netdev);
+	int ret;
+
+	if (new_mtu > MAX_SINGLE_PACKET_SIZE)
+		return -EINVAL;
 
-	int ret = smsc75xx_set_rx_max_frame_length(dev, new_mtu);
+	ret = smsc75xx_set_rx_max_frame_length(dev, new_mtu + ETH_HLEN);
 	if (ret < 0) {
 		netdev_warn(dev->net, "Failed to set mac rx frame length\n");
 		return ret;
@@ -1324,7 +1328,7 @@ static int smsc75xx_reset(struct usbnet *dev)
 
 	netif_dbg(dev, ifup, dev->net, "FCT_TX_CTL set to 0x%08x\n", buf);
 
-	ret = smsc75xx_set_rx_max_frame_length(dev, 1514);
+	ret = smsc75xx_set_rx_max_frame_length(dev, dev->net->mtu + ETH_HLEN);
 	if (ret < 0) {
 		netdev_warn(dev->net, "Failed to set max rx frame length\n");
 		return ret;
@@ -2011,7 +2015,11 @@ static int smsc75xx_suspend(struct usb_interface *intf, pm_message_t message)
 	ret = smsc75xx_enter_suspend0(dev);
 
 done:
-	if (ret)
+	/*
+	 * TODO: resume() might need to handle the suspend failure
+	 * in system sleep
+	 */
+	if (ret && PMSG_IS_AUTO(message))
 		usbnet_resume(intf);
 	return ret;
 }
@@ -2134,8 +2142,8 @@ static int smsc75xx_rx_fixup(struct usbnet *dev, struct sk_buff *skb)
 			else if (rx_cmd_a & (RX_CMD_A_LONG | RX_CMD_A_RUNT))
 				dev->net->stats.rx_frame_errors++;
 		} else {
-			/* ETH_FRAME_LEN + 4(CRC) + 2(COE) + 4(Vlan) */
-			if (unlikely(size > (ETH_FRAME_LEN + 12))) {
+			/* MAX_SINGLE_PACKET_SIZE + 4(CRC) + 2(COE) + 4(Vlan) */
+			if (unlikely(size > (MAX_SINGLE_PACKET_SIZE + ETH_HLEN + 12))) {
 				netif_dbg(dev, rx_err, dev->net,
 					  "size err rx_cmd_a=0x%08x\n",
 					  rx_cmd_a);
diff --git a/drivers/net/usb/smsc95xx.c b/drivers/net/usb/smsc95xx.c
index e6d2dea1373..3f38ba868f6 100644
--- a/drivers/net/usb/smsc95xx.c
+++ b/drivers/net/usb/smsc95xx.c
@@ -1660,7 +1660,11 @@ static int smsc95xx_suspend(struct usb_interface *intf, pm_message_t message)
 	ret = smsc95xx_enter_suspend0(dev);
 
 done:
-	if (ret)
+	/*
+	 * TODO: resume() might need to handle the suspend failure
+	 * in system sleep
+	 */
+	if (ret && PMSG_IS_AUTO(message))
 		usbnet_resume(intf);
 	return ret;
 }
diff --git a/drivers/net/wireless/airo_cs.c b/drivers/net/wireless/airo_cs.c
index 956024a636e..14128fd265a 100644
--- a/drivers/net/wireless/airo_cs.c
+++ b/drivers/net/wireless/airo_cs.c
@@ -180,16 +180,7 @@ static struct pcmcia_driver airo_driver = {
 	.suspend	= airo_suspend,
 	.resume		= airo_resume,
 };
-
-static int __init airo_cs_init(void)
-{
-	return pcmcia_register_driver(&airo_driver);
-}
-
-static void __exit airo_cs_cleanup(void)
-{
-	pcmcia_unregister_driver(&airo_driver);
-}
+module_pcmcia_driver(airo_driver);
 
 /*
     This program is free software; you can redistribute it and/or
@@ -229,6 +220,3 @@ static void __exit airo_cs_cleanup(void)
     IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
     POSSIBILITY OF SUCH DAMAGE.
 */
-
-module_init(airo_cs_init);
-module_exit(airo_cs_cleanup);
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_calib.c b/drivers/net/wireless/ath/ath9k/ar9003_calib.c
index 4cc13940c89..f76c3ca07a4 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_calib.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_calib.c
@@ -1023,6 +1023,7 @@ static bool ar9003_hw_init_cal(struct ath_hw *ah,
 					  AR_PHY_AGC_CONTROL_FLTR_CAL   |
 					  AR_PHY_AGC_CONTROL_PKDET_CAL;
 
+	/* Use chip chainmask only for calibration */
 	ar9003_hw_set_chain_masks(ah, ah->caps.rx_chainmask, ah->caps.tx_chainmask);
 
 	if (rtt) {
@@ -1150,6 +1151,9 @@ skip_tx_iqcal:
 		ar9003_hw_rtt_disable(ah);
 	}
 
+	/* Revert chainmask to runtime parameters */
+	ar9003_hw_set_chain_masks(ah, ah->rxchainmask, ah->txchainmask);
+
 	/* Initialize list pointers */
 	ah->cal_list = ah->cal_list_last = ah->cal_list_curr = NULL;
 
diff --git a/drivers/net/wireless/ath/ath9k/ar9580_1p0_initvals.h b/drivers/net/wireless/ath/ath9k/ar9580_1p0_initvals.h
index 28fd99203f6..bdee2ed6721 100644
--- a/drivers/net/wireless/ath/ath9k/ar9580_1p0_initvals.h
+++ b/drivers/net/wireless/ath/ath9k/ar9580_1p0_initvals.h
@@ -519,7 +519,7 @@ static const u32 ar9580_1p0_mac_core[][2] = {
 	{0x00008258, 0x00000000},
 	{0x0000825c, 0x40000000},
 	{0x00008260, 0x00080922},
-	{0x00008264, 0x9bc00010},
+	{0x00008264, 0x9d400010},
 	{0x00008268, 0xffffffff},
 	{0x0000826c, 0x0000ffff},
 	{0x00008270, 0x00000000},
diff --git a/drivers/net/wireless/ath/ath9k/dfs_pattern_detector.c b/drivers/net/wireless/ath/ath9k/dfs_pattern_detector.c
index 467b60014b7..73fe8d6db56 100644
--- a/drivers/net/wireless/ath/ath9k/dfs_pattern_detector.c
+++ b/drivers/net/wireless/ath/ath9k/dfs_pattern_detector.c
@@ -143,14 +143,14 @@ channel_detector_create(struct dfs_pattern_detector *dpd, u16 freq)
 	u32 sz, i;
 	struct channel_detector *cd;
 
-	cd = kmalloc(sizeof(*cd), GFP_KERNEL);
+	cd = kmalloc(sizeof(*cd), GFP_ATOMIC);
 	if (cd == NULL)
 		goto fail;
 
 	INIT_LIST_HEAD(&cd->head);
 	cd->freq = freq;
 	sz = sizeof(cd->detectors) * dpd->num_radar_types;
-	cd->detectors = kzalloc(sz, GFP_KERNEL);
+	cd->detectors = kzalloc(sz, GFP_ATOMIC);
 	if (cd->detectors == NULL)
 		goto fail;
 
diff --git a/drivers/net/wireless/ath/ath9k/dfs_pri_detector.c b/drivers/net/wireless/ath/ath9k/dfs_pri_detector.c
index 91b8dceeadb..5e48c5515b8 100644
--- a/drivers/net/wireless/ath/ath9k/dfs_pri_detector.c
+++ b/drivers/net/wireless/ath/ath9k/dfs_pri_detector.c
@@ -218,7 +218,7 @@ static bool pulse_queue_enqueue(struct pri_detector *pde, u64 ts)
 {
 	struct pulse_elem *p = pool_get_pulse_elem();
 	if (p == NULL) {
-		p = kmalloc(sizeof(*p), GFP_KERNEL);
+		p = kmalloc(sizeof(*p), GFP_ATOMIC);
 		if (p == NULL) {
 			DFS_POOL_STAT_INC(pulse_alloc_error);
 			return false;
@@ -299,7 +299,7 @@ static bool pseq_handler_create_sequences(struct pri_detector *pde,
 		ps.deadline_ts = ps.first_ts + ps.dur;
 		new_ps = pool_get_pseq_elem();
 		if (new_ps == NULL) {
-			new_ps = kmalloc(sizeof(*new_ps), GFP_KERNEL);
+			new_ps = kmalloc(sizeof(*new_ps), GFP_ATOMIC);
 			if (new_ps == NULL) {
 				DFS_POOL_STAT_INC(pseq_alloc_error);
 				return false;
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_init.c b/drivers/net/wireless/ath/ath9k/htc_drv_init.c
index 716058b6755..a47f5e05fc0 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_init.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_init.c
@@ -796,7 +796,7 @@ static int ath9k_init_firmware_version(struct ath9k_htc_priv *priv)
 	 * required version.
 	 */
 	if (priv->fw_version_major != MAJOR_VERSION_REQ ||
-	    priv->fw_version_minor != MINOR_VERSION_REQ) {
+	    priv->fw_version_minor < MINOR_VERSION_REQ) {
 		dev_err(priv->dev, "ath9k_htc: Please upgrade to FW version %d.%d\n",
 			MAJOR_VERSION_REQ, MINOR_VERSION_REQ);
 		return -EINVAL;
diff --git a/drivers/net/wireless/ath/ath9k/link.c b/drivers/net/wireless/ath/ath9k/link.c
index ade3afb21f9..7fdac6c7b3e 100644
--- a/drivers/net/wireless/ath/ath9k/link.c
+++ b/drivers/net/wireless/ath/ath9k/link.c
@@ -28,21 +28,21 @@ void ath_tx_complete_poll_work(struct work_struct *work)
 	int i;
 	bool needreset = false;
 
-	for (i = 0; i < ATH9K_NUM_TX_QUEUES; i++)
-		if (ATH_TXQ_SETUP(sc, i)) {
-			txq = &sc->tx.txq[i];
-			ath_txq_lock(sc, txq);
-			if (txq->axq_depth) {
-				if (txq->axq_tx_inprogress) {
-					needreset = true;
-					ath_txq_unlock(sc, txq);
-					break;
-				} else {
-					txq->axq_tx_inprogress = true;
-				}
+	for (i = 0; i < IEEE80211_NUM_ACS; i++) {
+		txq = sc->tx.txq_map[i];
+
+		ath_txq_lock(sc, txq);
+		if (txq->axq_depth) {
+			if (txq->axq_tx_inprogress) {
+				needreset = true;
+				ath_txq_unlock(sc, txq);
+				break;
+			} else {
+				txq->axq_tx_inprogress = true;
 			}
-			ath_txq_unlock_complete(sc, txq);
 		}
+		ath_txq_unlock_complete(sc, txq);
+	}
 
 	if (needreset) {
 		ath_dbg(ath9k_hw_common(sc->sc_ah), RESET,
@@ -170,7 +170,8 @@ void ath_rx_poll(unsigned long data)
 {
 	struct ath_softc *sc = (struct ath_softc *)data;
 
-	ieee80211_queue_work(sc->hw, &sc->hw_check_work);
+	if (!test_bit(SC_OP_INVALID, &sc->sc_flags))
+		ieee80211_queue_work(sc->hw, &sc->hw_check_work);
 }
 
 /*
diff --git a/drivers/net/wireless/ath/ath9k/main.c b/drivers/net/wireless/ath/ath9k/main.c
index 6e66f9c6782..988372d218a 100644
--- a/drivers/net/wireless/ath/ath9k/main.c
+++ b/drivers/net/wireless/ath/ath9k/main.c
@@ -280,6 +280,10 @@ static int ath_reset_internal(struct ath_softc *sc, struct ath9k_channel *hchan)
 	if (r) {
 		ath_err(common,
 			"Unable to reset channel, reset status %d\n", r);
+
+		ath9k_hw_enable_interrupts(ah);
+		ath9k_queue_reset(sc, RESET_TYPE_BB_HANG);
+
 		goto out;
 	}
 
diff --git a/drivers/net/wireless/atmel_cs.c b/drivers/net/wireless/atmel_cs.c
index b42930f457c..52257221921 100644
--- a/drivers/net/wireless/atmel_cs.c
+++ b/drivers/net/wireless/atmel_cs.c
@@ -245,16 +245,7 @@ static struct pcmcia_driver atmel_driver = {
 	.suspend	= atmel_suspend,
 	.resume		= atmel_resume,
 };
-
-static int __init atmel_cs_init(void)
-{
-        return pcmcia_register_driver(&atmel_driver);
-}
-
-static void __exit atmel_cs_cleanup(void)
-{
-        pcmcia_unregister_driver(&atmel_driver);
-}
+module_pcmcia_driver(atmel_driver);
 
 /*
     This program is free software; you can redistribute it and/or
@@ -294,6 +285,3 @@ static void __exit atmel_cs_cleanup(void)
     IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
     POSSIBILITY OF SUCH DAMAGE.
 */
-
-module_init(atmel_cs_init);
-module_exit(atmel_cs_cleanup);
diff --git a/drivers/net/wireless/b43/dma.c b/drivers/net/wireless/b43/dma.c
index 38bc5a7997f..122146943bf 100644
--- a/drivers/net/wireless/b43/dma.c
+++ b/drivers/net/wireless/b43/dma.c
@@ -1487,8 +1487,12 @@ void b43_dma_handle_txstatus(struct b43_wldev *dev,
 	const struct b43_dma_ops *ops;
 	struct b43_dmaring *ring;
 	struct b43_dmadesc_meta *meta;
+	static const struct b43_txstatus fake; /* filled with 0 */
+	const struct b43_txstatus *txstat;
 	int slot, firstused;
 	bool frame_succeed;
+	int skip;
+	static u8 err_out1, err_out2;
 
 	ring = parse_cookie(dev, status->cookie, &slot);
 	if (unlikely(!ring))
@@ -1501,13 +1505,36 @@ void b43_dma_handle_txstatus(struct b43_wldev *dev,
 	firstused = ring->current_slot - ring->used_slots + 1;
 	if (firstused < 0)
 		firstused = ring->nr_slots + firstused;
+
+	skip = 0;
 	if (unlikely(slot != firstused)) {
 		/* This possibly is a firmware bug and will result in
-		 * malfunction, memory leaks and/or stall of DMA functionality. */
-		b43dbg(dev->wl, "Out of order TX status report on DMA ring %d. "
-		       "Expected %d, but got %d\n",
-		       ring->index, firstused, slot);
-		return;
+		 * malfunction, memory leaks and/or stall of DMA functionality.
+		 */
+		if (slot == next_slot(ring, next_slot(ring, firstused))) {
+			/* If a single header/data pair was missed, skip over
+			 * the first two slots in an attempt to recover.
+			 */
+			slot = firstused;
+			skip = 2;
+			if (!err_out1) {
+				/* Report the error once. */
+				b43dbg(dev->wl,
+				       "Skip on DMA ring %d slot %d.\n",
+				       ring->index, slot);
+				err_out1 = 1;
+			}
+		} else {
+			/* More than a single header/data pair were missed.
+			 * Report this error once.
+			 */
+			if (!err_out2)
+				b43dbg(dev->wl,
+				       "Out of order TX status report on DMA ring %d. Expected %d, but got %d\n",
+				       ring->index, firstused, slot);
+			err_out2 = 1;
+			return;
+		}
 	}
 
 	ops = ring->ops;
@@ -1522,11 +1549,13 @@ void b43_dma_handle_txstatus(struct b43_wldev *dev,
 			       slot, firstused, ring->index);
 			break;
 		}
+
 		if (meta->skb) {
 			struct b43_private_tx_info *priv_info =
-				b43_get_priv_tx_info(IEEE80211_SKB_CB(meta->skb));
+			     b43_get_priv_tx_info(IEEE80211_SKB_CB(meta->skb));
 
-			unmap_descbuffer(ring, meta->dmaaddr, meta->skb->len, 1);
+			unmap_descbuffer(ring, meta->dmaaddr,
+					 meta->skb->len, 1);
 			kfree(priv_info->bouncebuffer);
 			priv_info->bouncebuffer = NULL;
 		} else {
@@ -1538,8 +1567,9 @@ void b43_dma_handle_txstatus(struct b43_wldev *dev,
 			struct ieee80211_tx_info *info;
 
 			if (unlikely(!meta->skb)) {
-				/* This is a scatter-gather fragment of a frame, so
-				 * the skb pointer must not be NULL. */
+				/* This is a scatter-gather fragment of a frame,
+				 * so the skb pointer must not be NULL.
+				 */
 				b43dbg(dev->wl, "TX status unexpected NULL skb "
 				       "at slot %d (first=%d) on ring %d\n",
 				       slot, firstused, ring->index);
@@ -1550,9 +1580,18 @@ void b43_dma_handle_txstatus(struct b43_wldev *dev,
 
 			/*
 			 * Call back to inform the ieee80211 subsystem about
-			 * the status of the transmission.
+			 * the status of the transmission. When skipping over
+			 * a missed TX status report, use a status structure
+			 * filled with zeros to indicate that the frame was not
+			 * sent (frame_count 0) and not acknowledged
 			 */
-			frame_succeed = b43_fill_txstatus_report(dev, info, status);
+			if (unlikely(skip))
+				txstat = &fake;
+			else
+				txstat = status;
+
+			frame_succeed = b43_fill_txstatus_report(dev, info,
+								 txstat);
 #ifdef CONFIG_B43_DEBUG
 			if (frame_succeed)
 				ring->nr_succeed_tx_packets++;
@@ -1580,12 +1619,14 @@ void b43_dma_handle_txstatus(struct b43_wldev *dev,
 		/* Everything unmapped and free'd. So it's not used anymore. */
 		ring->used_slots--;
 
-		if (meta->is_last_fragment) {
+		if (meta->is_last_fragment && !skip) {
 			/* This is the last scatter-gather
 			 * fragment of the frame. We are done. */
 			break;
 		}
 		slot = next_slot(ring, slot);
+		if (skip > 0)
+			--skip;
 	}
 	if (ring->stopped) {
 		B43_WARN_ON(free_slots(ring) < TX_SLOTS_PER_FRAME);
diff --git a/drivers/net/wireless/b43/pcmcia.c b/drivers/net/wireless/b43/pcmcia.c
index f2ea2ceec8a..55f2bd7f8f7 100644
--- a/drivers/net/wireless/b43/pcmcia.c
+++ b/drivers/net/wireless/b43/pcmcia.c
@@ -130,6 +130,10 @@ static struct pcmcia_driver b43_pcmcia_driver = {
 	.resume		= b43_pcmcia_resume,
 };
 
+/*
+ * These are not module init/exit functions!
+ * The module_pcmcia_driver() helper cannot be used here.
+ */
 int b43_pcmcia_init(void)
 {
 	return pcmcia_register_driver(&b43_pcmcia_driver);
diff --git a/drivers/net/wireless/b43/phy_n.c b/drivers/net/wireless/b43/phy_n.c
index 3c35382ee6c..b70f220bc4b 100644
--- a/drivers/net/wireless/b43/phy_n.c
+++ b/drivers/net/wireless/b43/phy_n.c
@@ -1564,7 +1564,7 @@ static void b43_nphy_rev3_rssi_cal(struct b43_wldev *dev)
 	u16 clip_off[2] = { 0xFFFF, 0xFFFF };
 
 	u8 vcm_final = 0;
-	s8 offset[4];
+	s32 offset[4];
 	s32 results[8][4] = { };
 	s32 results_min[4] = { };
 	s32 poll_results[4] = { };
@@ -1615,7 +1615,7 @@ static void b43_nphy_rev3_rssi_cal(struct b43_wldev *dev)
 		}
 		for (i = 0; i < 4; i += 2) {
 			s32 curr;
-			s32 mind = 40;
+			s32 mind = 0x100000;
 			s32 minpoll = 249;
 			u8 minvcm = 0;
 			if (2 * core != i)
@@ -1732,7 +1732,7 @@ static void b43_nphy_rev2_rssi_cal(struct b43_wldev *dev, u8 type)
 	u8 regs_save_radio[2];
 	u16 regs_save_phy[2];
 
-	s8 offset[4];
+	s32 offset[4];
 	u8 core;
 	u8 rail;
 
@@ -1799,7 +1799,7 @@ static void b43_nphy_rev2_rssi_cal(struct b43_wldev *dev, u8 type)
 	}
 
 	for (i = 0; i < 4; i++) {
-		s32 mind = 40;
+		s32 mind = 0x100000;
 		u8 minvcm = 0;
 		s32 minpoll = 249;
 		s32 curr;
@@ -5165,7 +5165,8 @@ static void b43_nphy_pmu_spur_avoid(struct b43_wldev *dev, bool avoid)
 #endif
 #ifdef CONFIG_B43_SSB
 	case B43_BUS_SSB:
-		/* FIXME */
+		ssb_pmu_spuravoid_pllupdate(&dev->dev->sdev->bus->chipco,
+					    avoid);
 		break;
 #endif
 	}
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
index 4469321c0eb..35fc68be158 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
@@ -3317,15 +3317,15 @@ static int _brcmf_sdbrcm_download_firmware(struct brcmf_sdio *bus)
 		goto err;
 	}
 
-	/* External image takes precedence if specified */
 	if (brcmf_sdbrcm_download_code_file(bus)) {
 		brcmf_err("dongle image file download failed\n");
 		goto err;
 	}
 
-	/* External nvram takes precedence if specified */
-	if (brcmf_sdbrcm_download_nvram(bus))
+	if (brcmf_sdbrcm_download_nvram(bus)) {
 		brcmf_err("dongle nvram file download failed\n");
+		goto err;
+	}
 
 	/* Take arm out of reset */
 	if (brcmf_sdbrcm_download_state(bus, false)) {
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
index 2af9c0f0798..78da3eff75e 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
@@ -1891,8 +1891,10 @@ static s32
 brcmf_add_keyext(struct wiphy *wiphy, struct net_device *ndev,
 	      u8 key_idx, const u8 *mac_addr, struct key_params *params)
 {
+	struct brcmf_if *ifp = netdev_priv(ndev);
 	struct brcmf_wsec_key key;
 	s32 err = 0;
+	u8 keybuf[8];
 
 	memset(&key, 0, sizeof(key));
 	key.index = (u32) key_idx;
@@ -1916,8 +1918,9 @@ brcmf_add_keyext(struct wiphy *wiphy, struct net_device *ndev,
 		brcmf_dbg(CONN, "Setting the key index %d\n", key.index);
 		memcpy(key.data, params->key, key.len);
 
-		if (params->cipher == WLAN_CIPHER_SUITE_TKIP) {
-			u8 keybuf[8];
+		if ((ifp->vif->mode != WL_MODE_AP) &&
+		    (params->cipher == WLAN_CIPHER_SUITE_TKIP)) {
+			brcmf_dbg(CONN, "Swapping RX/TX MIC key\n");
 			memcpy(keybuf, &key.data[24], sizeof(keybuf));
 			memcpy(&key.data[24], &key.data[16], sizeof(keybuf));
 			memcpy(&key.data[16], keybuf, sizeof(keybuf));
@@ -2013,7 +2016,7 @@ brcmf_cfg80211_add_key(struct wiphy *wiphy, struct net_device *ndev,
 		break;
 	case WLAN_CIPHER_SUITE_TKIP:
 		if (ifp->vif->mode != WL_MODE_AP) {
-			brcmf_dbg(CONN, "Swapping key\n");
+			brcmf_dbg(CONN, "Swapping RX/TX MIC key\n");
 			memcpy(keybuf, &key.data[24], sizeof(keybuf));
 			memcpy(&key.data[24], &key.data[16], sizeof(keybuf));
 			memcpy(&key.data[16], keybuf, sizeof(keybuf));
@@ -2118,8 +2121,7 @@ brcmf_cfg80211_get_key(struct wiphy *wiphy, struct net_device *ndev,
 		err = -EAGAIN;
 		goto done;
 	}
-	switch (wsec & ~SES_OW_ENABLED) {
-	case WEP_ENABLED:
+	if (wsec & WEP_ENABLED) {
 		sec = &profile->sec;
 		if (sec->cipher_pairwise & WLAN_CIPHER_SUITE_WEP40) {
 			params.cipher = WLAN_CIPHER_SUITE_WEP40;
@@ -2128,16 +2130,13 @@ brcmf_cfg80211_get_key(struct wiphy *wiphy, struct net_device *ndev,
 			params.cipher = WLAN_CIPHER_SUITE_WEP104;
 			brcmf_dbg(CONN, "WLAN_CIPHER_SUITE_WEP104\n");
 		}
-		break;
-	case TKIP_ENABLED:
+	} else if (wsec & TKIP_ENABLED) {
 		params.cipher = WLAN_CIPHER_SUITE_TKIP;
 		brcmf_dbg(CONN, "WLAN_CIPHER_SUITE_TKIP\n");
-		break;
-	case AES_ENABLED:
+	} else if (wsec & AES_ENABLED) {
 		params.cipher = WLAN_CIPHER_SUITE_AES_CMAC;
 		brcmf_dbg(CONN, "WLAN_CIPHER_SUITE_AES_CMAC\n");
-		break;
-	default:
+	} else  {
 		brcmf_err("Invalid algo (0x%x)\n", wsec);
 		err = -EINVAL;
 		goto done;
@@ -3824,8 +3823,9 @@ exit:
 static int brcmf_cfg80211_stop_ap(struct wiphy *wiphy, struct net_device *ndev)
 {
 	struct brcmf_if *ifp = netdev_priv(ndev);
-	s32 err = -EPERM;
+	s32 err;
 	struct brcmf_fil_bss_enable_le bss_enable;
+	struct brcmf_join_params join_params;
 
 	brcmf_dbg(TRACE, "Enter\n");
 
@@ -3833,16 +3833,21 @@ static int brcmf_cfg80211_stop_ap(struct wiphy *wiphy, struct net_device *ndev)
 		/* Due to most likely deauths outstanding we sleep */
 		/* first to make sure they get processed by fw. */
 		msleep(400);
-		err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_AP, 0);
-		if (err < 0) {
-			brcmf_err("setting AP mode failed %d\n", err);
-			goto exit;
-		}
+
+		memset(&join_params, 0, sizeof(join_params));
+		err = brcmf_fil_cmd_data_set(ifp, BRCMF_C_SET_SSID,
+					     &join_params, sizeof(join_params));
+		if (err < 0)
+			brcmf_err("SET SSID error (%d)\n", err);
 		err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_UP, 0);
-		if (err < 0) {
+		if (err < 0)
 			brcmf_err("BRCMF_C_UP error %d\n", err);
-			goto exit;
-		}
+		err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_AP, 0);
+		if (err < 0)
+			brcmf_err("setting AP mode failed %d\n", err);
+		err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_INFRA, 0);
+		if (err < 0)
+			brcmf_err("setting INFRA mode failed %d\n", err);
 	} else {
 		bss_enable.bsscfg_idx = cpu_to_le32(ifp->bssidx);
 		bss_enable.enable = cpu_to_le32(0);
@@ -3855,7 +3860,6 @@ static int brcmf_cfg80211_stop_ap(struct wiphy *wiphy, struct net_device *ndev)
 	set_bit(BRCMF_VIF_STATUS_AP_CREATING, &ifp->vif->sme_state);
 	clear_bit(BRCMF_VIF_STATUS_AP_CREATED, &ifp->vif->sme_state);
 
-exit:
 	return err;
 }
 
@@ -4124,10 +4128,6 @@ static const struct ieee80211_iface_limit brcmf_iface_limits[] = {
 	},
 	{
 		.max = 1,
-		.types = BIT(NL80211_IFTYPE_P2P_DEVICE)
-	},
-	{
-		.max = 1,
 		.types = BIT(NL80211_IFTYPE_P2P_CLIENT) |
 			 BIT(NL80211_IFTYPE_P2P_GO)
 	},
@@ -4183,8 +4183,7 @@ static struct wiphy *brcmf_setup_wiphy(struct device *phydev)
 				 BIT(NL80211_IFTYPE_ADHOC) |
 				 BIT(NL80211_IFTYPE_AP) |
 				 BIT(NL80211_IFTYPE_P2P_CLIENT) |
-				 BIT(NL80211_IFTYPE_P2P_GO) |
-				 BIT(NL80211_IFTYPE_P2P_DEVICE);
+				 BIT(NL80211_IFTYPE_P2P_GO);
 	wiphy->iface_combinations = brcmf_iface_combos;
 	wiphy->n_iface_combinations = ARRAY_SIZE(brcmf_iface_combos);
 	wiphy->bands[IEEE80211_BAND_2GHZ] = &__wl_band_2ghz;
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c b/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
index c6451c61407..e2340b231aa 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
@@ -274,6 +274,130 @@ static void brcms_set_basic_rate(struct brcm_rateset *rs, u16 rate, bool is_br)
 	}
 }
 
+/**
+ * This function frees the WL per-device resources.
+ *
+ * This function frees resources owned by the WL device pointed to
+ * by the wl parameter.
+ *
+ * precondition: can both be called locked and unlocked
+ *
+ */
+static void brcms_free(struct brcms_info *wl)
+{
+	struct brcms_timer *t, *next;
+
+	/* free ucode data */
+	if (wl->fw.fw_cnt)
+		brcms_ucode_data_free(&wl->ucode);
+	if (wl->irq)
+		free_irq(wl->irq, wl);
+
+	/* kill dpc */
+	tasklet_kill(&wl->tasklet);
+
+	if (wl->pub) {
+		brcms_debugfs_detach(wl->pub);
+		brcms_c_module_unregister(wl->pub, "linux", wl);
+	}
+
+	/* free common resources */
+	if (wl->wlc) {
+		brcms_c_detach(wl->wlc);
+		wl->wlc = NULL;
+		wl->pub = NULL;
+	}
+
+	/* virtual interface deletion is deferred so we cannot spinwait */
+
+	/* wait for all pending callbacks to complete */
+	while (atomic_read(&wl->callbacks) > 0)
+		schedule();
+
+	/* free timers */
+	for (t = wl->timers; t; t = next) {
+		next = t->next;
+#ifdef DEBUG
+		kfree(t->name);
+#endif
+		kfree(t);
+	}
+}
+
+/*
+* called from both kernel as from this kernel module (error flow on attach)
+* precondition: perimeter lock is not acquired.
+*/
+static void brcms_remove(struct bcma_device *pdev)
+{
+	struct ieee80211_hw *hw = bcma_get_drvdata(pdev);
+	struct brcms_info *wl = hw->priv;
+
+	if (wl->wlc) {
+		wiphy_rfkill_set_hw_state(wl->pub->ieee_hw->wiphy, false);
+		wiphy_rfkill_stop_polling(wl->pub->ieee_hw->wiphy);
+		ieee80211_unregister_hw(hw);
+	}
+
+	brcms_free(wl);
+
+	bcma_set_drvdata(pdev, NULL);
+	ieee80211_free_hw(hw);
+}
+
+/*
+ * Precondition: Since this function is called in brcms_pci_probe() context,
+ * no locking is required.
+ */
+static void brcms_release_fw(struct brcms_info *wl)
+{
+	int i;
+	for (i = 0; i < MAX_FW_IMAGES; i++) {
+		release_firmware(wl->fw.fw_bin[i]);
+		release_firmware(wl->fw.fw_hdr[i]);
+	}
+}
+
+/*
+ * 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 bcma_device *pdev)
+{
+	int status;
+	struct device *device = &pdev->dev;
+	char fw_name[100];
+	int i;
+
+	memset(&wl->fw, 0, sizeof(struct brcms_firmware));
+	for (i = 0; i < MAX_FW_IMAGES; i++) {
+		if (brcms_firmwares[i] == NULL)
+			break;
+		sprintf(fw_name, "%s-%d.fw", brcms_firmwares[i],
+			UCODE_LOADER_API_VER);
+		status = request_firmware(&wl->fw.fw_bin[i], fw_name, device);
+		if (status) {
+			wiphy_err(wl->wiphy, "%s: fail to load firmware %s\n",
+				  KBUILD_MODNAME, fw_name);
+			return status;
+		}
+		sprintf(fw_name, "%s_hdr-%d.fw", brcms_firmwares[i],
+			UCODE_LOADER_API_VER);
+		status = request_firmware(&wl->fw.fw_hdr[i], fw_name, device);
+		if (status) {
+			wiphy_err(wl->wiphy, "%s: fail to load firmware %s\n",
+				  KBUILD_MODNAME, fw_name);
+			return status;
+		}
+		wl->fw.hdr_num_entries[i] =
+		    wl->fw.fw_hdr[i]->size / (sizeof(struct firmware_hdr));
+	}
+	wl->fw.fw_cnt = i;
+	status = brcms_ucode_data_init(wl, &wl->ucode);
+	brcms_release_fw(wl);
+	return status;
+}
+
 static void brcms_ops_tx(struct ieee80211_hw *hw,
 			 struct ieee80211_tx_control *control,
 			 struct sk_buff *skb)
@@ -306,6 +430,14 @@ static int brcms_ops_start(struct ieee80211_hw *hw)
 	if (!blocked)
 		wiphy_rfkill_stop_polling(wl->pub->ieee_hw->wiphy);
 
+	if (!wl->ucode.bcm43xx_bomminor) {
+		err = brcms_request_fw(wl, wl->wlc->hw->d11core);
+		if (err) {
+			brcms_remove(wl->wlc->hw->d11core);
+			return -ENOENT;
+		}
+	}
+
 	spin_lock_bh(&wl->lock);
 	/* avoid acknowledging frames before a non-monitor device is added */
 	wl->mute_tx = true;
@@ -793,128 +925,6 @@ void brcms_dpc(unsigned long data)
 	wake_up(&wl->tx_flush_wq);
 }
 
-/*
- * 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 bcma_device *pdev)
-{
-	int status;
-	struct device *device = &pdev->dev;
-	char fw_name[100];
-	int i;
-
-	memset(&wl->fw, 0, sizeof(struct brcms_firmware));
-	for (i = 0; i < MAX_FW_IMAGES; i++) {
-		if (brcms_firmwares[i] == NULL)
-			break;
-		sprintf(fw_name, "%s-%d.fw", brcms_firmwares[i],
-			UCODE_LOADER_API_VER);
-		status = request_firmware(&wl->fw.fw_bin[i], fw_name, device);
-		if (status) {
-			wiphy_err(wl->wiphy, "%s: fail to load firmware %s\n",
-				  KBUILD_MODNAME, fw_name);
-			return status;
-		}
-		sprintf(fw_name, "%s_hdr-%d.fw", brcms_firmwares[i],
-			UCODE_LOADER_API_VER);
-		status = request_firmware(&wl->fw.fw_hdr[i], fw_name, device);
-		if (status) {
-			wiphy_err(wl->wiphy, "%s: fail to load firmware %s\n",
-				  KBUILD_MODNAME, fw_name);
-			return status;
-		}
-		wl->fw.hdr_num_entries[i] =
-		    wl->fw.fw_hdr[i]->size / (sizeof(struct firmware_hdr));
-	}
-	wl->fw.fw_cnt = i;
-	return brcms_ucode_data_init(wl, &wl->ucode);
-}
-
-/*
- * Precondition: Since this function is called in brcms_pci_probe() context,
- * no locking is required.
- */
-static void brcms_release_fw(struct brcms_info *wl)
-{
-	int i;
-	for (i = 0; i < MAX_FW_IMAGES; i++) {
-		release_firmware(wl->fw.fw_bin[i]);
-		release_firmware(wl->fw.fw_hdr[i]);
-	}
-}
-
-/**
- * This function frees the WL per-device resources.
- *
- * This function frees resources owned by the WL device pointed to
- * by the wl parameter.
- *
- * precondition: can both be called locked and unlocked
- *
- */
-static void brcms_free(struct brcms_info *wl)
-{
-	struct brcms_timer *t, *next;
-
-	/* free ucode data */
-	if (wl->fw.fw_cnt)
-		brcms_ucode_data_free(&wl->ucode);
-	if (wl->irq)
-		free_irq(wl->irq, wl);
-
-	/* kill dpc */
-	tasklet_kill(&wl->tasklet);
-
-	if (wl->pub) {
-		brcms_debugfs_detach(wl->pub);
-		brcms_c_module_unregister(wl->pub, "linux", wl);
-	}
-
-	/* free common resources */
-	if (wl->wlc) {
-		brcms_c_detach(wl->wlc);
-		wl->wlc = NULL;
-		wl->pub = NULL;
-	}
-
-	/* virtual interface deletion is deferred so we cannot spinwait */
-
-	/* wait for all pending callbacks to complete */
-	while (atomic_read(&wl->callbacks) > 0)
-		schedule();
-
-	/* free timers */
-	for (t = wl->timers; t; t = next) {
-		next = t->next;
-#ifdef DEBUG
-		kfree(t->name);
-#endif
-		kfree(t);
-	}
-}
-
-/*
-* called from both kernel as from this kernel module (error flow on attach)
-* precondition: perimeter lock is not acquired.
-*/
-static void brcms_remove(struct bcma_device *pdev)
-{
-	struct ieee80211_hw *hw = bcma_get_drvdata(pdev);
-	struct brcms_info *wl = hw->priv;
-
-	if (wl->wlc) {
-		wiphy_rfkill_set_hw_state(wl->pub->ieee_hw->wiphy, false);
-		wiphy_rfkill_stop_polling(wl->pub->ieee_hw->wiphy);
-		ieee80211_unregister_hw(hw);
-	}
-
-	brcms_free(wl);
-
-	bcma_set_drvdata(pdev, NULL);
-	ieee80211_free_hw(hw);
-}
-
 static irqreturn_t brcms_isr(int irq, void *dev_id)
 {
 	struct brcms_info *wl;
@@ -1047,18 +1057,8 @@ static struct brcms_info *brcms_attach(struct bcma_device *pdev)
 	spin_lock_init(&wl->lock);
 	spin_lock_init(&wl->isr_lock);
 
-	/* prepare ucode */
-	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);
-		brcms_remove(pdev);
-		return NULL;
-	}
-
 	/* common load-time initialization */
 	wl->wlc = brcms_c_attach((void *)wl, pdev, unit, false, &err);
-	brcms_release_fw(wl);
 	if (!wl->wlc) {
 		wiphy_err(wl->wiphy, "%s: attach() failed with code %d\n",
 			  KBUILD_MODNAME, err);
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.c b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.c
index 21a82423247..18d37645e2c 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.c
@@ -1137,9 +1137,8 @@ wlc_lcnphy_set_rx_gain_by_distribution(struct brcms_phy *pi,
 	gain0_15 = ((biq1 & 0xf) << 12) |
 		   ((tia & 0xf) << 8) |
 		   ((lna2 & 0x3) << 6) |
-		   ((lna2 & 0x3) << 4) |
-		   ((lna1 & 0x3) << 2) |
-		   ((lna1 & 0x3) << 0);
+		   ((lna2 &
+		     0x3) << 4) | ((lna1 & 0x3) << 2) | ((lna1 & 0x3) << 0);
 
 	mod_phy_reg(pi, 0x4b6, (0xffff << 0), gain0_15 << 0);
 	mod_phy_reg(pi, 0x4b7, (0xf << 0), gain16_19 << 0);
@@ -1157,8 +1156,6 @@ wlc_lcnphy_set_rx_gain_by_distribution(struct brcms_phy *pi,
 	}
 
 	mod_phy_reg(pi, 0x44d, (0x1 << 0), (!trsw) << 0);
-	mod_phy_reg(pi, 0x4b1, (0x3 << 11), lna1 << 11);
-	mod_phy_reg(pi, 0x4e6, (0x3 << 3), lna1 << 3);
 
 }
 
@@ -1331,43 +1328,6 @@ static u32 wlc_lcnphy_measure_digital_power(struct brcms_phy *pi, u16 nsamples)
 	return (iq_est.i_pwr + iq_est.q_pwr) / nsamples;
 }
 
-static bool wlc_lcnphy_rx_iq_cal_gain(struct brcms_phy *pi, u16 biq1_gain,
-				      u16 tia_gain, u16 lna2_gain)
-{
-	u32 i_thresh_l, q_thresh_l;
-	u32 i_thresh_h, q_thresh_h;
-	struct lcnphy_iq_est iq_est_h, iq_est_l;
-
-	wlc_lcnphy_set_rx_gain_by_distribution(pi, 0, 0, 0, biq1_gain, tia_gain,
-					       lna2_gain, 0);
-
-	wlc_lcnphy_rx_gain_override_enable(pi, true);
-	wlc_lcnphy_start_tx_tone(pi, 2000, (40 >> 1), 0);
-	udelay(500);
-	write_radio_reg(pi, RADIO_2064_REG112, 0);
-	if (!wlc_lcnphy_rx_iq_est(pi, 1024, 32, &iq_est_l))
-		return false;
-
-	wlc_lcnphy_start_tx_tone(pi, 2000, 40, 0);
-	udelay(500);
-	write_radio_reg(pi, RADIO_2064_REG112, 0);
-	if (!wlc_lcnphy_rx_iq_est(pi, 1024, 32, &iq_est_h))
-		return false;
-
-	i_thresh_l = (iq_est_l.i_pwr << 1);
-	i_thresh_h = (iq_est_l.i_pwr << 2) + iq_est_l.i_pwr;
-
-	q_thresh_l = (iq_est_l.q_pwr << 1);
-	q_thresh_h = (iq_est_l.q_pwr << 2) + iq_est_l.q_pwr;
-	if ((iq_est_h.i_pwr > i_thresh_l) &&
-	    (iq_est_h.i_pwr < i_thresh_h) &&
-	    (iq_est_h.q_pwr > q_thresh_l) &&
-	    (iq_est_h.q_pwr < q_thresh_h))
-		return true;
-
-	return false;
-}
-
 static bool
 wlc_lcnphy_rx_iq_cal(struct brcms_phy *pi,
 		     const struct lcnphy_rx_iqcomp *iqcomp,
@@ -1382,8 +1342,8 @@ wlc_lcnphy_rx_iq_cal(struct brcms_phy *pi,
 	    RFOverrideVal0_old, rfoverride2_old, rfoverride2val_old,
 	    rfoverride3_old, rfoverride3val_old, rfoverride4_old,
 	    rfoverride4val_old, afectrlovr_old, afectrlovrval_old;
-	int tia_gain, lna2_gain, biq1_gain;
-	bool set_gain;
+	int tia_gain;
+	u32 received_power, rx_pwr_threshold;
 	u16 old_sslpnCalibClkEnCtrl, old_sslpnRxFeClkEnCtrl;
 	u16 values_to_save[11];
 	s16 *ptr;
@@ -1408,134 +1368,126 @@ wlc_lcnphy_rx_iq_cal(struct brcms_phy *pi,
 		goto cal_done;
 	}
 
-	WARN_ON(module != 1);
-	tx_pwr_ctrl = wlc_lcnphy_get_tx_pwr_ctrl(pi);
-	wlc_lcnphy_set_tx_pwr_ctrl(pi, LCNPHY_TX_PWR_CTRL_OFF);
-
-	for (i = 0; i < 11; i++)
-		values_to_save[i] =
-			read_radio_reg(pi, rxiq_cal_rf_reg[i]);
-	Core1TxControl_old = read_phy_reg(pi, 0x631);
-
-	or_phy_reg(pi, 0x631, 0x0015);
-
-	RFOverride0_old = read_phy_reg(pi, 0x44c);
-	RFOverrideVal0_old = read_phy_reg(pi, 0x44d);
-	rfoverride2_old = read_phy_reg(pi, 0x4b0);
-	rfoverride2val_old = read_phy_reg(pi, 0x4b1);
-	rfoverride3_old = read_phy_reg(pi, 0x4f9);
-	rfoverride3val_old = read_phy_reg(pi, 0x4fa);
-	rfoverride4_old = read_phy_reg(pi, 0x938);
-	rfoverride4val_old = read_phy_reg(pi, 0x939);
-	afectrlovr_old = read_phy_reg(pi, 0x43b);
-	afectrlovrval_old = read_phy_reg(pi, 0x43c);
-	old_sslpnCalibClkEnCtrl = read_phy_reg(pi, 0x6da);
-	old_sslpnRxFeClkEnCtrl = read_phy_reg(pi, 0x6db);
-
-	tx_gain_override_old = wlc_lcnphy_tx_gain_override_enabled(pi);
-	if (tx_gain_override_old) {
-		wlc_lcnphy_get_tx_gain(pi, &old_gains);
-		tx_gain_index_old = pi_lcn->lcnphy_current_index;
-	}
-
-	wlc_lcnphy_set_tx_pwr_by_index(pi, tx_gain_idx);
+	if (module == 1) {
 
-	mod_phy_reg(pi, 0x4f9, (0x1 << 0), 1 << 0);
-	mod_phy_reg(pi, 0x4fa, (0x1 << 0), 0 << 0);
+		tx_pwr_ctrl = wlc_lcnphy_get_tx_pwr_ctrl(pi);
+		wlc_lcnphy_set_tx_pwr_ctrl(pi, LCNPHY_TX_PWR_CTRL_OFF);
 
-	mod_phy_reg(pi, 0x43b, (0x1 << 1), 1 << 1);
-	mod_phy_reg(pi, 0x43c, (0x1 << 1), 0 << 1);
+		for (i = 0; i < 11; i++)
+			values_to_save[i] =
+				read_radio_reg(pi, rxiq_cal_rf_reg[i]);
+		Core1TxControl_old = read_phy_reg(pi, 0x631);
+
+		or_phy_reg(pi, 0x631, 0x0015);
+
+		RFOverride0_old = read_phy_reg(pi, 0x44c);
+		RFOverrideVal0_old = read_phy_reg(pi, 0x44d);
+		rfoverride2_old = read_phy_reg(pi, 0x4b0);
+		rfoverride2val_old = read_phy_reg(pi, 0x4b1);
+		rfoverride3_old = read_phy_reg(pi, 0x4f9);
+		rfoverride3val_old = read_phy_reg(pi, 0x4fa);
+		rfoverride4_old = read_phy_reg(pi, 0x938);
+		rfoverride4val_old = read_phy_reg(pi, 0x939);
+		afectrlovr_old = read_phy_reg(pi, 0x43b);
+		afectrlovrval_old = read_phy_reg(pi, 0x43c);
+		old_sslpnCalibClkEnCtrl = read_phy_reg(pi, 0x6da);
+		old_sslpnRxFeClkEnCtrl = read_phy_reg(pi, 0x6db);
+
+		tx_gain_override_old = wlc_lcnphy_tx_gain_override_enabled(pi);
+		if (tx_gain_override_old) {
+			wlc_lcnphy_get_tx_gain(pi, &old_gains);
+			tx_gain_index_old = pi_lcn->lcnphy_current_index;
+		}
 
-	write_radio_reg(pi, RADIO_2064_REG116, 0x06);
-	write_radio_reg(pi, RADIO_2064_REG12C, 0x07);
-	write_radio_reg(pi, RADIO_2064_REG06A, 0xd3);
-	write_radio_reg(pi, RADIO_2064_REG098, 0x03);
-	write_radio_reg(pi, RADIO_2064_REG00B, 0x7);
-	mod_radio_reg(pi, RADIO_2064_REG113, 1 << 4, 1 << 4);
-	write_radio_reg(pi, RADIO_2064_REG01D, 0x01);
-	write_radio_reg(pi, RADIO_2064_REG114, 0x01);
-	write_radio_reg(pi, RADIO_2064_REG02E, 0x10);
-	write_radio_reg(pi, RADIO_2064_REG12A, 0x08);
-
-	mod_phy_reg(pi, 0x938, (0x1 << 0), 1 << 0);
-	mod_phy_reg(pi, 0x939, (0x1 << 0), 0 << 0);
-	mod_phy_reg(pi, 0x938, (0x1 << 1), 1 << 1);
-	mod_phy_reg(pi, 0x939, (0x1 << 1), 1 << 1);
-	mod_phy_reg(pi, 0x938, (0x1 << 2), 1 << 2);
-	mod_phy_reg(pi, 0x939, (0x1 << 2), 1 << 2);
-	mod_phy_reg(pi, 0x938, (0x1 << 3), 1 << 3);
-	mod_phy_reg(pi, 0x939, (0x1 << 3), 1 << 3);
-	mod_phy_reg(pi, 0x938, (0x1 << 5), 1 << 5);
-	mod_phy_reg(pi, 0x939, (0x1 << 5), 0 << 5);
+		wlc_lcnphy_set_tx_pwr_by_index(pi, tx_gain_idx);
 
-	mod_phy_reg(pi, 0x43b, (0x1 << 0), 1 << 0);
-	mod_phy_reg(pi, 0x43c, (0x1 << 0), 0 << 0);
+		mod_phy_reg(pi, 0x4f9, (0x1 << 0), 1 << 0);
+		mod_phy_reg(pi, 0x4fa, (0x1 << 0), 0 << 0);
 
-	write_phy_reg(pi, 0x6da, 0xffff);
-	or_phy_reg(pi, 0x6db, 0x3);
+		mod_phy_reg(pi, 0x43b, (0x1 << 1), 1 << 1);
+		mod_phy_reg(pi, 0x43c, (0x1 << 1), 0 << 1);
 
-	wlc_lcnphy_set_trsw_override(pi, tx_switch, rx_switch);
-	set_gain = false;
-
-	lna2_gain = 3;
-	while ((lna2_gain >= 0) && !set_gain) {
-		tia_gain = 4;
-
-		while ((tia_gain >= 0) && !set_gain) {
-			biq1_gain = 6;
-
-			while ((biq1_gain >= 0) && !set_gain) {
-				set_gain = wlc_lcnphy_rx_iq_cal_gain(pi,
-								     (u16)
-								     biq1_gain,
-								     (u16)
-								     tia_gain,
-								     (u16)
-								     lna2_gain);
-				biq1_gain -= 1;
-			}
+		write_radio_reg(pi, RADIO_2064_REG116, 0x06);
+		write_radio_reg(pi, RADIO_2064_REG12C, 0x07);
+		write_radio_reg(pi, RADIO_2064_REG06A, 0xd3);
+		write_radio_reg(pi, RADIO_2064_REG098, 0x03);
+		write_radio_reg(pi, RADIO_2064_REG00B, 0x7);
+		mod_radio_reg(pi, RADIO_2064_REG113, 1 << 4, 1 << 4);
+		write_radio_reg(pi, RADIO_2064_REG01D, 0x01);
+		write_radio_reg(pi, RADIO_2064_REG114, 0x01);
+		write_radio_reg(pi, RADIO_2064_REG02E, 0x10);
+		write_radio_reg(pi, RADIO_2064_REG12A, 0x08);
+
+		mod_phy_reg(pi, 0x938, (0x1 << 0), 1 << 0);
+		mod_phy_reg(pi, 0x939, (0x1 << 0), 0 << 0);
+		mod_phy_reg(pi, 0x938, (0x1 << 1), 1 << 1);
+		mod_phy_reg(pi, 0x939, (0x1 << 1), 1 << 1);
+		mod_phy_reg(pi, 0x938, (0x1 << 2), 1 << 2);
+		mod_phy_reg(pi, 0x939, (0x1 << 2), 1 << 2);
+		mod_phy_reg(pi, 0x938, (0x1 << 3), 1 << 3);
+		mod_phy_reg(pi, 0x939, (0x1 << 3), 1 << 3);
+		mod_phy_reg(pi, 0x938, (0x1 << 5), 1 << 5);
+		mod_phy_reg(pi, 0x939, (0x1 << 5), 0 << 5);
+
+		mod_phy_reg(pi, 0x43b, (0x1 << 0), 1 << 0);
+		mod_phy_reg(pi, 0x43c, (0x1 << 0), 0 << 0);
+
+		wlc_lcnphy_start_tx_tone(pi, 2000, 120, 0);
+		write_phy_reg(pi, 0x6da, 0xffff);
+		or_phy_reg(pi, 0x6db, 0x3);
+		wlc_lcnphy_set_trsw_override(pi, tx_switch, rx_switch);
+		wlc_lcnphy_rx_gain_override_enable(pi, true);
+
+		tia_gain = 8;
+		rx_pwr_threshold = 950;
+		while (tia_gain > 0) {
 			tia_gain -= 1;
+			wlc_lcnphy_set_rx_gain_by_distribution(pi,
+							       0, 0, 2, 2,
+							       (u16)
+							       tia_gain, 1, 0);
+			udelay(500);
+
+			received_power =
+				wlc_lcnphy_measure_digital_power(pi, 2000);
+			if (received_power < rx_pwr_threshold)
+				break;
 		}
-		lna2_gain -= 1;
-	}
+		result = wlc_lcnphy_calc_rx_iq_comp(pi, 0xffff);
 
-	if (set_gain)
-		result = wlc_lcnphy_calc_rx_iq_comp(pi, 1024);
-	else
-		result = false;
+		wlc_lcnphy_stop_tx_tone(pi);
 
-	wlc_lcnphy_stop_tx_tone(pi);
+		write_phy_reg(pi, 0x631, Core1TxControl_old);
 
-	write_phy_reg(pi, 0x631, Core1TxControl_old);
-
-	write_phy_reg(pi, 0x44c, RFOverrideVal0_old);
-	write_phy_reg(pi, 0x44d, RFOverrideVal0_old);
-	write_phy_reg(pi, 0x4b0, rfoverride2_old);
-	write_phy_reg(pi, 0x4b1, rfoverride2val_old);
-	write_phy_reg(pi, 0x4f9, rfoverride3_old);
-	write_phy_reg(pi, 0x4fa, rfoverride3val_old);
-	write_phy_reg(pi, 0x938, rfoverride4_old);
-	write_phy_reg(pi, 0x939, rfoverride4val_old);
-	write_phy_reg(pi, 0x43b, afectrlovr_old);
-	write_phy_reg(pi, 0x43c, afectrlovrval_old);
-	write_phy_reg(pi, 0x6da, old_sslpnCalibClkEnCtrl);
-	write_phy_reg(pi, 0x6db, old_sslpnRxFeClkEnCtrl);
+		write_phy_reg(pi, 0x44c, RFOverrideVal0_old);
+		write_phy_reg(pi, 0x44d, RFOverrideVal0_old);
+		write_phy_reg(pi, 0x4b0, rfoverride2_old);
+		write_phy_reg(pi, 0x4b1, rfoverride2val_old);
+		write_phy_reg(pi, 0x4f9, rfoverride3_old);
+		write_phy_reg(pi, 0x4fa, rfoverride3val_old);
+		write_phy_reg(pi, 0x938, rfoverride4_old);
+		write_phy_reg(pi, 0x939, rfoverride4val_old);
+		write_phy_reg(pi, 0x43b, afectrlovr_old);
+		write_phy_reg(pi, 0x43c, afectrlovrval_old);
+		write_phy_reg(pi, 0x6da, old_sslpnCalibClkEnCtrl);
+		write_phy_reg(pi, 0x6db, old_sslpnRxFeClkEnCtrl);
 
-	wlc_lcnphy_clear_trsw_override(pi);
+		wlc_lcnphy_clear_trsw_override(pi);
 
-	mod_phy_reg(pi, 0x44c, (0x1 << 2), 0 << 2);
+		mod_phy_reg(pi, 0x44c, (0x1 << 2), 0 << 2);
 
-	for (i = 0; i < 11; i++)
-		write_radio_reg(pi, rxiq_cal_rf_reg[i],
-				values_to_save[i]);
+		for (i = 0; i < 11; i++)
+			write_radio_reg(pi, rxiq_cal_rf_reg[i],
+					values_to_save[i]);
 
-	if (tx_gain_override_old)
-		wlc_lcnphy_set_tx_pwr_by_index(pi, tx_gain_index_old);
-	else
-		wlc_lcnphy_disable_tx_gain_override(pi);
+		if (tx_gain_override_old)
+			wlc_lcnphy_set_tx_pwr_by_index(pi, tx_gain_index_old);
+		else
+			wlc_lcnphy_disable_tx_gain_override(pi);
 
-	wlc_lcnphy_set_tx_pwr_ctrl(pi, tx_pwr_ctrl);
-	wlc_lcnphy_rx_gain_override_enable(pi, false);
+		wlc_lcnphy_set_tx_pwr_ctrl(pi, tx_pwr_ctrl);
+		wlc_lcnphy_rx_gain_override_enable(pi, false);
+	}
 
 cal_done:
 	kfree(ptr);
@@ -1829,17 +1781,6 @@ wlc_lcnphy_radio_2064_channel_tune_4313(struct brcms_phy *pi, u8 channel)
 		write_radio_reg(pi, RADIO_2064_REG038, 3);
 		write_radio_reg(pi, RADIO_2064_REG091, 7);
 	}
-
-	if (!(pi->sh->boardflags & BFL_FEM)) {
-		u8 reg038[14] = {0xd, 0xe, 0xd, 0xd, 0xd, 0xc,
-			0xa, 0xb, 0xb, 0x3, 0x3, 0x2, 0x0, 0x0};
-
-		write_radio_reg(pi, RADIO_2064_REG02A, 0xf);
-		write_radio_reg(pi, RADIO_2064_REG091, 0x3);
-		write_radio_reg(pi, RADIO_2064_REG038, 0x3);
-
-		write_radio_reg(pi, RADIO_2064_REG038, reg038[channel - 1]);
-	}
 }
 
 static int
@@ -2034,16 +1975,6 @@ wlc_lcnphy_set_tssi_mux(struct brcms_phy *pi, enum lcnphy_tssi_mode pos)
 		} else {
 			mod_radio_reg(pi, RADIO_2064_REG03A, 1, 0x1);
 			mod_radio_reg(pi, RADIO_2064_REG11A, 0x8, 0x8);
-			mod_radio_reg(pi, RADIO_2064_REG028, 0x1, 0x0);
-			mod_radio_reg(pi, RADIO_2064_REG11A, 0x4, 1<<2);
-			mod_radio_reg(pi, RADIO_2064_REG036, 0x10, 0x0);
-			mod_radio_reg(pi, RADIO_2064_REG11A, 0x10, 1<<4);
-			mod_radio_reg(pi, RADIO_2064_REG036, 0x3, 0x0);
-			mod_radio_reg(pi, RADIO_2064_REG035, 0xff, 0x77);
-			mod_radio_reg(pi, RADIO_2064_REG028, 0x1e, 0xe<<1);
-			mod_radio_reg(pi, RADIO_2064_REG112, 0x80, 1<<7);
-			mod_radio_reg(pi, RADIO_2064_REG005, 0x7, 1<<1);
-			mod_radio_reg(pi, RADIO_2064_REG029, 0xf0, 0<<4);
 		}
 	} else {
 		mod_phy_reg(pi, 0x4d9, (0x1 << 2), (0x1) << 2);
@@ -2130,14 +2061,12 @@ static void wlc_lcnphy_pwrctrl_rssiparams(struct brcms_phy *pi)
 		    (auxpga_vmid_temp << 0) | (auxpga_gain_temp << 12));
 
 	mod_radio_reg(pi, RADIO_2064_REG082, (1 << 5), (1 << 5));
-	mod_radio_reg(pi, RADIO_2064_REG07C, (1 << 0), (1 << 0));
 }
 
 static void wlc_lcnphy_tssi_setup(struct brcms_phy *pi)
 {
 	struct phytbl_info tab;
 	u32 rfseq, ind;
-	u8 tssi_sel;
 
 	tab.tbl_id = LCNPHY_TBL_ID_TXPWRCTL;
 	tab.tbl_width = 32;
@@ -2159,13 +2088,7 @@ static void wlc_lcnphy_tssi_setup(struct brcms_phy *pi)
 
 	mod_phy_reg(pi, 0x503, (0x1 << 4), (1) << 4);
 
-	if (pi->sh->boardflags & BFL_FEM) {
-		tssi_sel = 0x1;
-		wlc_lcnphy_set_tssi_mux(pi, LCNPHY_TSSI_EXT);
-	} else {
-		tssi_sel = 0xe;
-		wlc_lcnphy_set_tssi_mux(pi, LCNPHY_TSSI_POST_PA);
-	}
+	wlc_lcnphy_set_tssi_mux(pi, LCNPHY_TSSI_EXT);
 	mod_phy_reg(pi, 0x4a4, (0x1 << 14), (0) << 14);
 
 	mod_phy_reg(pi, 0x4a4, (0x1 << 15), (1) << 15);
@@ -2201,10 +2124,9 @@ static void wlc_lcnphy_tssi_setup(struct brcms_phy *pi)
 	mod_phy_reg(pi, 0x49a, (0x1ff << 0), (0xff) << 0);
 
 	if (LCNREV_IS(pi->pubpi.phy_rev, 2)) {
-		mod_radio_reg(pi, RADIO_2064_REG028, 0xf, tssi_sel);
+		mod_radio_reg(pi, RADIO_2064_REG028, 0xf, 0xe);
 		mod_radio_reg(pi, RADIO_2064_REG086, 0x4, 0x4);
 	} else {
-		mod_radio_reg(pi, RADIO_2064_REG028, 0x1e, tssi_sel << 1);
 		mod_radio_reg(pi, RADIO_2064_REG03A, 0x1, 1);
 		mod_radio_reg(pi, RADIO_2064_REG11A, 0x8, 1 << 3);
 	}
@@ -2251,10 +2173,6 @@ static void wlc_lcnphy_tssi_setup(struct brcms_phy *pi)
 
 	mod_phy_reg(pi, 0x4d7, (0xf << 8), (0) << 8);
 
-	mod_radio_reg(pi, RADIO_2064_REG035, 0xff, 0x0);
-	mod_radio_reg(pi, RADIO_2064_REG036, 0x3, 0x0);
-	mod_radio_reg(pi, RADIO_2064_REG11A, 0x8, 0x8);
-
 	wlc_lcnphy_pwrctrl_rssiparams(pi);
 }
 
@@ -2873,8 +2791,6 @@ static void wlc_lcnphy_idle_tssi_est(struct brcms_phy_pub *ppi)
 		read_radio_reg(pi, RADIO_2064_REG007) & 1;
 	u16 SAVE_jtag_auxpga = read_radio_reg(pi, RADIO_2064_REG0FF) & 0x10;
 	u16 SAVE_iqadc_aux_en = read_radio_reg(pi, RADIO_2064_REG11F) & 4;
-	u8 SAVE_bbmult = wlc_lcnphy_get_bbmult(pi);
-
 	idleTssi = read_phy_reg(pi, 0x4ab);
 	suspend = (0 == (bcma_read32(pi->d11core, D11REGOFFS(maccontrol)) &
 			 MCTL_EN_MAC));
@@ -2892,12 +2808,6 @@ static void wlc_lcnphy_idle_tssi_est(struct brcms_phy_pub *ppi)
 	mod_radio_reg(pi, RADIO_2064_REG0FF, 0x10, 1 << 4);
 	mod_radio_reg(pi, RADIO_2064_REG11F, 0x4, 1 << 2);
 	wlc_lcnphy_tssi_setup(pi);
-
-	mod_phy_reg(pi, 0x4d7, (0x1 << 0), (1 << 0));
-	mod_phy_reg(pi, 0x4d7, (0x1 << 6), (1 << 6));
-
-	wlc_lcnphy_set_bbmult(pi, 0x0);
-
 	wlc_phy_do_dummy_tx(pi, true, OFF);
 	idleTssi = ((read_phy_reg(pi, 0x4ab) & (0x1ff << 0))
 		    >> 0);
@@ -2919,7 +2829,6 @@ static void wlc_lcnphy_idle_tssi_est(struct brcms_phy_pub *ppi)
 
 	mod_phy_reg(pi, 0x44c, (0x1 << 12), (0) << 12);
 
-	wlc_lcnphy_set_bbmult(pi, SAVE_bbmult);
 	wlc_lcnphy_set_tx_gain_override(pi, tx_gain_override_old);
 	wlc_lcnphy_set_tx_gain(pi, &old_gains);
 	wlc_lcnphy_set_tx_pwr_ctrl(pi, SAVE_txpwrctrl);
@@ -3133,11 +3042,6 @@ static void wlc_lcnphy_tx_pwr_ctrl_init(struct brcms_phy_pub *ppi)
 			wlc_lcnphy_write_table(pi, &tab);
 			tab.tbl_offset++;
 		}
-		mod_phy_reg(pi, 0x4d0, (0x1 << 0), (0) << 0);
-		mod_phy_reg(pi, 0x4d3, (0xff << 0), (0) << 0);
-		mod_phy_reg(pi, 0x4d3, (0xff << 8), (0) << 8);
-		mod_phy_reg(pi, 0x4d0, (0x1 << 4), (0) << 4);
-		mod_phy_reg(pi, 0x4d0, (0x1 << 2), (0) << 2);
 
 		mod_phy_reg(pi, 0x410, (0x1 << 7), (0) << 7);
 
@@ -3939,6 +3843,7 @@ static void wlc_lcnphy_txpwrtbl_iqlo_cal(struct brcms_phy *pi)
 	target_gains.pad_gain = 21;
 	target_gains.dac_gain = 0;
 	wlc_lcnphy_set_tx_gain(pi, &target_gains);
+	wlc_lcnphy_set_tx_pwr_by_index(pi, 16);
 
 	if (LCNREV_IS(pi->pubpi.phy_rev, 1) || pi_lcn->lcnphy_hw_iqcal_en) {
 
@@ -3949,7 +3854,6 @@ static void wlc_lcnphy_txpwrtbl_iqlo_cal(struct brcms_phy *pi)
 					lcnphy_recal ? LCNPHY_CAL_RECAL :
 					LCNPHY_CAL_FULL), false);
 	} else {
-		wlc_lcnphy_set_tx_pwr_by_index(pi, 16);
 		wlc_lcnphy_tx_iqlo_soft_cal_full(pi);
 	}
 
@@ -4374,22 +4278,17 @@ wlc_lcnphy_load_tx_gain_table(struct brcms_phy *pi,
 	if (CHSPEC_IS5G(pi->radio_chanspec))
 		pa_gain = 0x70;
 	else
-		pa_gain = 0x60;
+		pa_gain = 0x70;
 
 	if (pi->sh->boardflags & BFL_FEM)
 		pa_gain = 0x10;
-
 	tab.tbl_id = LCNPHY_TBL_ID_TXPWRCTL;
 	tab.tbl_width = 32;
 	tab.tbl_len = 1;
 	tab.tbl_ptr = &val;
 
 	for (j = 0; j < 128; j++) {
-		if (pi->sh->boardflags & BFL_FEM)
-			gm_gain = gain_table[j].gm;
-		else
-			gm_gain = 15;
-
+		gm_gain = gain_table[j].gm;
 		val = (((u32) pa_gain << 24) |
 		       (gain_table[j].pad << 16) |
 		       (gain_table[j].pga << 8) | gm_gain);
@@ -4600,10 +4499,7 @@ static void wlc_radio_2064_init(struct brcms_phy *pi)
 
 	write_phy_reg(pi, 0x4ea, 0x4688);
 
-	if (pi->sh->boardflags & BFL_FEM)
-		mod_phy_reg(pi, 0x4eb, (0x7 << 0), 2 << 0);
-	else
-		mod_phy_reg(pi, 0x4eb, (0x7 << 0), 3 << 0);
+	mod_phy_reg(pi, 0x4eb, (0x7 << 0), 2 << 0);
 
 	mod_phy_reg(pi, 0x4eb, (0x7 << 6), 0 << 6);
 
@@ -4614,13 +4510,6 @@ static void wlc_radio_2064_init(struct brcms_phy *pi)
 	wlc_lcnphy_rcal(pi);
 
 	wlc_lcnphy_rc_cal(pi);
-
-	if (!(pi->sh->boardflags & BFL_FEM)) {
-		write_radio_reg(pi, RADIO_2064_REG032, 0x6f);
-		write_radio_reg(pi, RADIO_2064_REG033, 0x19);
-		write_radio_reg(pi, RADIO_2064_REG039, 0xe);
-	}
-
 }
 
 static void wlc_lcnphy_radio_init(struct brcms_phy *pi)
@@ -4650,20 +4539,22 @@ static void wlc_lcnphy_tbl_init(struct brcms_phy *pi)
 		wlc_lcnphy_write_table(pi, &tab);
 	}
 
-	if (!(pi->sh->boardflags & BFL_FEM)) {
-		tab.tbl_id = LCNPHY_TBL_ID_RFSEQ;
-		tab.tbl_width = 16;
-		tab.tbl_ptr = &val;
-		tab.tbl_len = 1;
+	tab.tbl_id = LCNPHY_TBL_ID_RFSEQ;
+	tab.tbl_width = 16;
+	tab.tbl_ptr = &val;
+	tab.tbl_len = 1;
 
-		val = 150;
-		tab.tbl_offset = 0;
-		wlc_lcnphy_write_table(pi, &tab);
+	val = 114;
+	tab.tbl_offset = 0;
+	wlc_lcnphy_write_table(pi, &tab);
 
-		val = 220;
-		tab.tbl_offset = 1;
-		wlc_lcnphy_write_table(pi, &tab);
-	}
+	val = 130;
+	tab.tbl_offset = 1;
+	wlc_lcnphy_write_table(pi, &tab);
+
+	val = 6;
+	tab.tbl_offset = 8;
+	wlc_lcnphy_write_table(pi, &tab);
 
 	if (CHSPEC_IS2G(pi->radio_chanspec)) {
 		if (pi->sh->boardflags & BFL_FEM)
@@ -5055,7 +4946,6 @@ void wlc_phy_chanspec_set_lcnphy(struct brcms_phy *pi, u16 chanspec)
 		wlc_lcnphy_load_tx_iir_filter(pi, true, 3);
 
 	mod_phy_reg(pi, 0x4eb, (0x7 << 3), (1) << 3);
-	wlc_lcnphy_tssi_setup(pi);
 }
 
 void wlc_phy_detach_lcnphy(struct brcms_phy *pi)
@@ -5094,7 +4984,8 @@ bool wlc_phy_attach_lcnphy(struct brcms_phy *pi)
 	if (!wlc_phy_txpwr_srom_read_lcnphy(pi))
 		return false;
 
-	if (LCNREV_IS(pi->pubpi.phy_rev, 1)) {
+	if ((pi->sh->boardflags & BFL_FEM) &&
+	    (LCNREV_IS(pi->pubpi.phy_rev, 1))) {
 		if (pi_lcn->lcnphy_tempsense_option == 3) {
 			pi->hwpwrctrl = true;
 			pi->hwpwrctrl_capable = true;
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phytbl_lcn.c b/drivers/net/wireless/brcm80211/brcmsmac/phy/phytbl_lcn.c
index b7e95acc208..622c01ca72c 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phytbl_lcn.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/phy/phytbl_lcn.c
@@ -1992,70 +1992,70 @@ static const u16 dot11lcn_sw_ctrl_tbl_4313_epa_rev0[] = {
 };
 
 static const u16 dot11lcn_sw_ctrl_tbl_4313_rev0[] = {
-	0x0009,
 	0x000a,
-	0x0005,
-	0x0006,
 	0x0009,
-	0x000a,
-	0x0005,
 	0x0006,
-	0x0009,
-	0x000a,
 	0x0005,
-	0x0006,
-	0x0009,
 	0x000a,
-	0x0005,
-	0x0006,
 	0x0009,
-	0x000a,
-	0x0005,
 	0x0006,
-	0x0009,
-	0x000a,
 	0x0005,
-	0x0006,
-	0x0009,
 	0x000a,
-	0x0005,
-	0x0006,
 	0x0009,
-	0x000a,
-	0x0005,
 	0x0006,
-	0x0009,
-	0x000a,
 	0x0005,
-	0x0006,
-	0x0009,
 	0x000a,
-	0x0005,
-	0x0006,
 	0x0009,
-	0x000a,
-	0x0005,
 	0x0006,
-	0x0009,
-	0x000a,
 	0x0005,
-	0x0006,
+	0x000a,
 	0x0009,
+	0x0006,
+	0x0005,
 	0x000a,
+	0x0009,
+	0x0006,
 	0x0005,
+	0x000a,
+	0x0009,
 	0x0006,
+	0x0005,
+	0x000a,
 	0x0009,
+	0x0006,
+	0x0005,
 	0x000a,
+	0x0009,
+	0x0006,
 	0x0005,
+	0x000a,
+	0x0009,
 	0x0006,
+	0x0005,
+	0x000a,
 	0x0009,
+	0x0006,
+	0x0005,
 	0x000a,
+	0x0009,
+	0x0006,
 	0x0005,
+	0x000a,
+	0x0009,
 	0x0006,
+	0x0005,
+	0x000a,
 	0x0009,
+	0x0006,
+	0x0005,
 	0x000a,
+	0x0009,
+	0x0006,
 	0x0005,
+	0x000a,
+	0x0009,
 	0x0006,
+	0x0005,
 };
 
 static const u16 dot11lcn_sw_ctrl_tbl_rev0[] = {
diff --git a/drivers/net/wireless/hostap/hostap_cs.c b/drivers/net/wireless/hostap/hostap_cs.c
index 89e9d3a78c3..56cd01ca8ad 100644
--- a/drivers/net/wireless/hostap/hostap_cs.c
+++ b/drivers/net/wireless/hostap/hostap_cs.c
@@ -709,17 +709,4 @@ static struct pcmcia_driver hostap_driver = {
 	.suspend	= hostap_cs_suspend,
 	.resume		= hostap_cs_resume,
 };
-
-static int __init init_prism2_pccard(void)
-{
-	return pcmcia_register_driver(&hostap_driver);
-}
-
-static void __exit exit_prism2_pccard(void)
-{
-	pcmcia_unregister_driver(&hostap_driver);
-}
-
-
-module_init(init_prism2_pccard);
-module_exit(exit_prism2_pccard);
+module_pcmcia_driver(hostap_driver);
diff --git a/drivers/net/wireless/iwlegacy/3945-mac.c b/drivers/net/wireless/iwlegacy/3945-mac.c
index 3630a41df50..c353b5f19c8 100644
--- a/drivers/net/wireless/iwlegacy/3945-mac.c
+++ b/drivers/net/wireless/iwlegacy/3945-mac.c
@@ -475,6 +475,7 @@ il3945_tx_skb(struct il_priv *il,
 	dma_addr_t txcmd_phys;
 	int txq_id = skb_get_queue_mapping(skb);
 	u16 len, idx, hdr_len;
+	u16 firstlen, secondlen;
 	u8 id;
 	u8 unicast;
 	u8 sta_id;
@@ -589,21 +590,22 @@ il3945_tx_skb(struct il_priv *il,
 	len =
 	    sizeof(struct il3945_tx_cmd) + sizeof(struct il_cmd_header) +
 	    hdr_len;
-	len = (len + 3) & ~3;
+	firstlen = (len + 3) & ~3;
 
 	/* Physical address of this Tx command's header (not MAC header!),
 	 * within command buffer array. */
 	txcmd_phys =
-	    pci_map_single(il->pci_dev, &out_cmd->hdr, len, PCI_DMA_TODEVICE);
+	    pci_map_single(il->pci_dev, &out_cmd->hdr, firstlen,
+			   PCI_DMA_TODEVICE);
 	if (unlikely(pci_dma_mapping_error(il->pci_dev, txcmd_phys)))
 		goto drop_unlock;
 
 	/* Set up TFD's 2nd entry to point directly to remainder of skb,
 	 * if any (802.11 null frames have no payload). */
-	len = skb->len - hdr_len;
-	if (len) {
+	secondlen = skb->len - hdr_len;
+	if (secondlen > 0) {
 		phys_addr =
-		    pci_map_single(il->pci_dev, skb->data + hdr_len, len,
+		    pci_map_single(il->pci_dev, skb->data + hdr_len, secondlen,
 				   PCI_DMA_TODEVICE);
 		if (unlikely(pci_dma_mapping_error(il->pci_dev, phys_addr)))
 			goto drop_unlock;
@@ -611,12 +613,12 @@ il3945_tx_skb(struct il_priv *il,
 
 	/* Add buffer containing Tx command and MAC(!) header to TFD's
 	 * first entry */
-	il->ops->txq_attach_buf_to_tfd(il, txq, txcmd_phys, len, 1, 0);
+	il->ops->txq_attach_buf_to_tfd(il, txq, txcmd_phys, firstlen, 1, 0);
 	dma_unmap_addr_set(out_meta, mapping, txcmd_phys);
-	dma_unmap_len_set(out_meta, len, len);
-	if (len)
-		il->ops->txq_attach_buf_to_tfd(il, txq, phys_addr, len, 0,
-					       U32_PAD(len));
+	dma_unmap_len_set(out_meta, len, firstlen);
+	if (secondlen > 0)
+		il->ops->txq_attach_buf_to_tfd(il, txq, phys_addr, secondlen, 0,
+					       U32_PAD(secondlen));
 
 	if (!ieee80211_has_morefrags(hdr->frame_control)) {
 		txq->need_update = 1;
diff --git a/drivers/net/wireless/iwlegacy/4965-rs.c b/drivers/net/wireless/iwlegacy/4965-rs.c
index e8324b5e5bf..6c7493c2d69 100644
--- a/drivers/net/wireless/iwlegacy/4965-rs.c
+++ b/drivers/net/wireless/iwlegacy/4965-rs.c
@@ -2152,7 +2152,7 @@ il4965_rs_initialize_lq(struct il_priv *il, struct ieee80211_conf *conf,
 	int rate_idx;
 	int i;
 	u32 rate;
-	u8 use_green = il4965_rs_use_green(il, sta);
+	u8 use_green;
 	u8 active_tbl = 0;
 	u8 valid_tx_ant;
 	struct il_station_priv *sta_priv;
@@ -2160,6 +2160,7 @@ il4965_rs_initialize_lq(struct il_priv *il, struct ieee80211_conf *conf,
 	if (!sta || !lq_sta)
 		return;
 
+	use_green = il4965_rs_use_green(il, sta);
 	sta_priv = (void *)sta->drv_priv;
 
 	i = lq_sta->last_txrate_idx;
diff --git a/drivers/net/wireless/iwlwifi/dvm/lib.c b/drivers/net/wireless/iwlwifi/dvm/lib.c
index 86ea5f4c393..44ca0e57f9f 100644
--- a/drivers/net/wireless/iwlwifi/dvm/lib.c
+++ b/drivers/net/wireless/iwlwifi/dvm/lib.c
@@ -1262,6 +1262,15 @@ int iwl_dvm_send_cmd(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
 	}
 
 	/*
+	 * This can happen upon FW ASSERT: we clear the STATUS_FW_ERROR flag
+	 * in iwl_down but cancel the workers only later.
+	 */
+	if (!priv->ucode_loaded) {
+		IWL_ERR(priv, "Fw not loaded - dropping CMD: %x\n", cmd->id);
+		return -EIO;
+	}
+
+	/*
 	 * Synchronous commands from this op-mode must hold
 	 * the mutex, this ensures we don't try to send two
 	 * (or more) synchronous commands at a time.
diff --git a/drivers/net/wireless/iwlwifi/dvm/rxon.c b/drivers/net/wireless/iwlwifi/dvm/rxon.c
index 23be948cf16..a82b6b39d4f 100644
--- a/drivers/net/wireless/iwlwifi/dvm/rxon.c
+++ b/drivers/net/wireless/iwlwifi/dvm/rxon.c
@@ -1419,6 +1419,14 @@ void iwlagn_bss_info_changed(struct ieee80211_hw *hw,
 
 	mutex_lock(&priv->mutex);
 
+	if (changes & BSS_CHANGED_IDLE && bss_conf->idle) {
+		/*
+		 * If we go idle, then clearly no "passive-no-rx"
+		 * workaround is needed any more, this is a reset.
+		 */
+		iwlagn_lift_passive_no_rx(priv);
+	}
+
 	if (unlikely(!iwl_is_ready(priv))) {
 		IWL_DEBUG_MAC80211(priv, "leave - not ready\n");
 		mutex_unlock(&priv->mutex);
@@ -1450,16 +1458,6 @@ void iwlagn_bss_info_changed(struct ieee80211_hw *hw,
 			priv->timestamp = bss_conf->sync_tsf;
 			ctx->staging.filter_flags |= RXON_FILTER_ASSOC_MSK;
 		} else {
-			/*
-			 * If we disassociate while there are pending
-			 * frames, just wake up the queues and let the
-			 * frames "escape" ... This shouldn't really
-			 * be happening to start with, but we should
-			 * not get stuck in this case either since it
-			 * can happen if userspace gets confused.
-			 */
-			iwlagn_lift_passive_no_rx(priv);
-
 			ctx->staging.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
 
 			if (ctx->ctxid == IWL_RXON_CTX_BSS)
diff --git a/drivers/net/wireless/iwlwifi/dvm/tx.c b/drivers/net/wireless/iwlwifi/dvm/tx.c
index 6aec2df3bb2..d1a670d7b10 100644
--- a/drivers/net/wireless/iwlwifi/dvm/tx.c
+++ b/drivers/net/wireless/iwlwifi/dvm/tx.c
@@ -1192,7 +1192,7 @@ int iwlagn_rx_reply_tx(struct iwl_priv *priv, struct iwl_rx_cmd_buffer *rxb,
 			memset(&info->status, 0, sizeof(info->status));
 
 			if (status == TX_STATUS_FAIL_PASSIVE_NO_RX &&
-			    iwl_is_associated_ctx(ctx) && ctx->vif &&
+			    ctx->vif &&
 			    ctx->vif->type == NL80211_IFTYPE_STATION) {
 				/* block and stop all queues */
 				priv->passive_no_rx = true;
diff --git a/drivers/net/wireless/iwlwifi/dvm/ucode.c b/drivers/net/wireless/iwlwifi/dvm/ucode.c
index 736fe9bb140..1a4ac9236a4 100644
--- a/drivers/net/wireless/iwlwifi/dvm/ucode.c
+++ b/drivers/net/wireless/iwlwifi/dvm/ucode.c
@@ -367,6 +367,8 @@ int iwl_load_ucode_wait_alive(struct iwl_priv *priv,
 		return -EIO;
 	}
 
+	priv->ucode_loaded = true;
+
 	if (ucode_type != IWL_UCODE_WOWLAN) {
 		/* delay a bit to give rfkill time to run */
 		msleep(5);
@@ -380,8 +382,6 @@ int iwl_load_ucode_wait_alive(struct iwl_priv *priv,
 		return ret;
 	}
 
-	priv->ucode_loaded = true;
-
 	return 0;
 }
 
diff --git a/drivers/net/wireless/iwlwifi/pcie/trans.c b/drivers/net/wireless/iwlwifi/pcie/trans.c
index 17bedc50e75..12c4f31ca8f 100644
--- a/drivers/net/wireless/iwlwifi/pcie/trans.c
+++ b/drivers/net/wireless/iwlwifi/pcie/trans.c
@@ -475,6 +475,10 @@ static int iwl_trans_pcie_start_fw(struct iwl_trans *trans,
 
 	/* If platform's RF_KILL switch is NOT set to KILL */
 	hw_rfkill = iwl_is_rfkill_set(trans);
+	if (hw_rfkill)
+		set_bit(STATUS_RFKILL, &trans_pcie->status);
+	else
+		clear_bit(STATUS_RFKILL, &trans_pcie->status);
 	iwl_op_mode_hw_rf_kill(trans->op_mode, hw_rfkill);
 	if (hw_rfkill && !run_in_rfkill)
 		return -ERFKILL;
@@ -641,6 +645,7 @@ static int iwl_trans_pcie_d3_resume(struct iwl_trans *trans,
 
 static int iwl_trans_pcie_start_hw(struct iwl_trans *trans)
 {
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	bool hw_rfkill;
 	int err;
 
@@ -656,6 +661,10 @@ static int iwl_trans_pcie_start_hw(struct iwl_trans *trans)
 	iwl_enable_rfkill_int(trans);
 
 	hw_rfkill = iwl_is_rfkill_set(trans);
+	if (hw_rfkill)
+		set_bit(STATUS_RFKILL, &trans_pcie->status);
+	else
+		clear_bit(STATUS_RFKILL, &trans_pcie->status);
 	iwl_op_mode_hw_rf_kill(trans->op_mode, hw_rfkill);
 
 	return 0;
@@ -694,6 +703,10 @@ static void iwl_trans_pcie_stop_hw(struct iwl_trans *trans,
 		 * op_mode.
 		 */
 		hw_rfkill = iwl_is_rfkill_set(trans);
+		if (hw_rfkill)
+			set_bit(STATUS_RFKILL, &trans_pcie->status);
+		else
+			clear_bit(STATUS_RFKILL, &trans_pcie->status);
 		iwl_op_mode_hw_rf_kill(trans->op_mode, hw_rfkill);
 	}
 }
diff --git a/drivers/net/wireless/iwlwifi/pcie/tx.c b/drivers/net/wireless/iwlwifi/pcie/tx.c
index 8595c16f74d..cb5c6792e3a 100644
--- a/drivers/net/wireless/iwlwifi/pcie/tx.c
+++ b/drivers/net/wireless/iwlwifi/pcie/tx.c
@@ -1264,7 +1264,7 @@ static int iwl_pcie_enqueue_hcmd(struct iwl_trans *trans,
 	for (i = 0; i < IWL_MAX_CMD_TBS_PER_TFD; i++) {
 		int copy = 0;
 
-		if (!cmd->len)
+		if (!cmd->len[i])
 			continue;
 
 		/* need at least IWL_HCMD_SCRATCHBUF_SIZE copied */
diff --git a/drivers/net/wireless/libertas/if_cs.c b/drivers/net/wireless/libertas/if_cs.c
index 16beaf39dc5..c94dd680267 100644
--- a/drivers/net/wireless/libertas/if_cs.c
+++ b/drivers/net/wireless/libertas/if_cs.c
@@ -999,7 +999,6 @@ static const struct pcmcia_device_id if_cs_ids[] = {
 };
 MODULE_DEVICE_TABLE(pcmcia, if_cs_ids);
 
-
 static struct pcmcia_driver lbs_driver = {
 	.owner		= THIS_MODULE,
 	.name		= DRV_NAME,
@@ -1007,26 +1006,4 @@ static struct pcmcia_driver lbs_driver = {
 	.remove		= if_cs_detach,
 	.id_table       = if_cs_ids,
 };
-
-
-static int __init if_cs_init(void)
-{
-	int ret;
-
-	lbs_deb_enter(LBS_DEB_CS);
-	ret = pcmcia_register_driver(&lbs_driver);
-	lbs_deb_leave(LBS_DEB_CS);
-	return ret;
-}
-
-
-static void __exit if_cs_exit(void)
-{
-	lbs_deb_enter(LBS_DEB_CS);
-	pcmcia_unregister_driver(&lbs_driver);
-	lbs_deb_leave(LBS_DEB_CS);
-}
-
-
-module_init(if_cs_init);
-module_exit(if_cs_exit);
+module_pcmcia_driver(lbs_driver);
diff --git a/drivers/net/wireless/mwifiex/cfg80211.c b/drivers/net/wireless/mwifiex/cfg80211.c
index a44023a7bd5..8aaf56ade4d 100644
--- a/drivers/net/wireless/mwifiex/cfg80211.c
+++ b/drivers/net/wireless/mwifiex/cfg80211.c
@@ -1892,7 +1892,8 @@ mwifiex_cfg80211_scan(struct wiphy *wiphy,
 		}
 	}
 
-	for (i = 0; i < request->n_channels; i++) {
+	for (i = 0; i < min_t(u32, request->n_channels,
+			      MWIFIEX_USER_SCAN_CHAN_MAX); i++) {
 		chan = request->channels[i];
 		priv->user_scan_cfg->chan_list[i].chan_number = chan->hw_value;
 		priv->user_scan_cfg->chan_list[i].radio_type = chan->band;
diff --git a/drivers/net/wireless/mwifiex/cmdevt.c b/drivers/net/wireless/mwifiex/cmdevt.c
index 20a6c555587..b5c8b962ce1 100644
--- a/drivers/net/wireless/mwifiex/cmdevt.c
+++ b/drivers/net/wireless/mwifiex/cmdevt.c
@@ -157,6 +157,20 @@ static int mwifiex_dnld_cmd_to_fw(struct mwifiex_private *priv,
 		return -1;
 	}
 
+	cmd_code = le16_to_cpu(host_cmd->command);
+	cmd_size = le16_to_cpu(host_cmd->size);
+
+	if (adapter->hw_status == MWIFIEX_HW_STATUS_RESET &&
+	    cmd_code != HostCmd_CMD_FUNC_SHUTDOWN &&
+	    cmd_code != HostCmd_CMD_FUNC_INIT) {
+		dev_err(adapter->dev,
+			"DNLD_CMD: FW in reset state, ignore cmd %#x\n",
+			cmd_code);
+		mwifiex_complete_cmd(adapter, cmd_node);
+		mwifiex_insert_cmd_to_free_q(adapter, cmd_node);
+		return -1;
+	}
+
 	/* Set command sequence number */
 	adapter->seq_num++;
 	host_cmd->seq_num = cpu_to_le16(HostCmd_SET_SEQ_NO_BSS_INFO
@@ -168,9 +182,6 @@ static int mwifiex_dnld_cmd_to_fw(struct mwifiex_private *priv,
 	adapter->curr_cmd = cmd_node;
 	spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock, flags);
 
-	cmd_code = le16_to_cpu(host_cmd->command);
-	cmd_size = le16_to_cpu(host_cmd->size);
-
 	/* Adjust skb length */
 	if (cmd_node->cmd_skb->len > cmd_size)
 		/*
@@ -484,8 +495,6 @@ int mwifiex_send_cmd_sync(struct mwifiex_private *priv, uint16_t cmd_no,
 
 	ret = mwifiex_send_cmd_async(priv, cmd_no, cmd_action, cmd_oid,
 				     data_buf);
-	if (!ret)
-		ret = mwifiex_wait_queue_complete(adapter);
 
 	return ret;
 }
@@ -588,9 +597,10 @@ int mwifiex_send_cmd_async(struct mwifiex_private *priv, uint16_t cmd_no,
 	if (cmd_no == HostCmd_CMD_802_11_SCAN) {
 		mwifiex_queue_scan_cmd(priv, cmd_node);
 	} else {
-		adapter->cmd_queued = cmd_node;
 		mwifiex_insert_cmd_to_pending_q(adapter, cmd_node, true);
 		queue_work(adapter->workqueue, &adapter->main_work);
+		if (cmd_node->wait_q_enabled)
+			ret = mwifiex_wait_queue_complete(adapter, cmd_node);
 	}
 
 	return ret;
diff --git a/drivers/net/wireless/mwifiex/init.c b/drivers/net/wireless/mwifiex/init.c
index e38aa9b3663..0ff4c37ab42 100644
--- a/drivers/net/wireless/mwifiex/init.c
+++ b/drivers/net/wireless/mwifiex/init.c
@@ -709,6 +709,14 @@ mwifiex_shutdown_drv(struct mwifiex_adapter *adapter)
 		return ret;
 	}
 
+	/* cancel current command */
+	if (adapter->curr_cmd) {
+		dev_warn(adapter->dev, "curr_cmd is still in processing\n");
+		del_timer(&adapter->cmd_timer);
+		mwifiex_insert_cmd_to_free_q(adapter, adapter->curr_cmd);
+		adapter->curr_cmd = NULL;
+	}
+
 	/* shut down mwifiex */
 	dev_dbg(adapter->dev, "info: shutdown mwifiex...\n");
 
diff --git a/drivers/net/wireless/mwifiex/main.h b/drivers/net/wireless/mwifiex/main.h
index 553adfb0aa8..7035ade9af7 100644
--- a/drivers/net/wireless/mwifiex/main.h
+++ b/drivers/net/wireless/mwifiex/main.h
@@ -723,7 +723,6 @@ struct mwifiex_adapter {
 	u16 cmd_wait_q_required;
 	struct mwifiex_wait_queue cmd_wait_q;
 	u8 scan_wait_q_woken;
-	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];
@@ -1018,7 +1017,8 @@ int mwifiex_request_set_multicast_list(struct mwifiex_private *priv,
 			struct mwifiex_multicast_list *mcast_list);
 int mwifiex_copy_mcast_addr(struct mwifiex_multicast_list *mlist,
 			    struct net_device *dev);
-int mwifiex_wait_queue_complete(struct mwifiex_adapter *adapter);
+int mwifiex_wait_queue_complete(struct mwifiex_adapter *adapter,
+				struct cmd_ctrl_node *cmd_queued);
 int mwifiex_bss_start(struct mwifiex_private *priv, struct cfg80211_bss *bss,
 		      struct cfg80211_ssid *req_ssid);
 int mwifiex_cancel_hs(struct mwifiex_private *priv, int cmd_type);
diff --git a/drivers/net/wireless/mwifiex/pcie.c b/drivers/net/wireless/mwifiex/pcie.c
index 5c395e2e6a2..feb20461339 100644
--- a/drivers/net/wireless/mwifiex/pcie.c
+++ b/drivers/net/wireless/mwifiex/pcie.c
@@ -1508,6 +1508,7 @@ static int mwifiex_pcie_process_cmd_complete(struct mwifiex_adapter *adapter)
 		}
 		memcpy(adapter->upld_buf, skb->data,
 		       min_t(u32, MWIFIEX_SIZE_OF_CMD_BUFFER, skb->len));
+		skb_push(skb, INTF_HEADER_LEN);
 		if (mwifiex_map_pci_memory(adapter, skb, MWIFIEX_UPLD_SIZE,
 					   PCI_DMA_FROMDEVICE))
 			return -1;
diff --git a/drivers/net/wireless/mwifiex/scan.c b/drivers/net/wireless/mwifiex/scan.c
index bb60c2754a9..e7f6deaf715 100644
--- a/drivers/net/wireless/mwifiex/scan.c
+++ b/drivers/net/wireless/mwifiex/scan.c
@@ -1388,10 +1388,15 @@ int mwifiex_scan_networks(struct mwifiex_private *priv,
 			list_del(&cmd_node->list);
 			spin_unlock_irqrestore(&adapter->scan_pending_q_lock,
 					       flags);
-			adapter->cmd_queued = cmd_node;
 			mwifiex_insert_cmd_to_pending_q(adapter, cmd_node,
 							true);
 			queue_work(adapter->workqueue, &adapter->main_work);
+
+			/* Perform internal scan synchronously */
+			if (!priv->scan_request) {
+				dev_dbg(adapter->dev, "wait internal scan\n");
+				mwifiex_wait_queue_complete(adapter, cmd_node);
+			}
 		} else {
 			spin_unlock_irqrestore(&adapter->scan_pending_q_lock,
 					       flags);
@@ -1790,7 +1795,12 @@ check_next_scan:
 		/* Need to indicate IOCTL complete */
 		if (adapter->curr_cmd->wait_q_enabled) {
 			adapter->cmd_wait_q.status = 0;
-			mwifiex_complete_cmd(adapter, adapter->curr_cmd);
+			if (!priv->scan_request) {
+				dev_dbg(adapter->dev,
+					"complete internal scan\n");
+				mwifiex_complete_cmd(adapter,
+						     adapter->curr_cmd);
+			}
 		}
 		if (priv->report_scan_result)
 			priv->report_scan_result = false;
@@ -1946,9 +1956,6 @@ int mwifiex_request_scan(struct mwifiex_private *priv,
 		/* Normal scan */
 		ret = mwifiex_scan_networks(priv, NULL);
 
-	if (!ret)
-		ret = mwifiex_wait_queue_complete(priv->adapter);
-
 	up(&priv->async_sem);
 
 	return ret;
diff --git a/drivers/net/wireless/mwifiex/sta_ioctl.c b/drivers/net/wireless/mwifiex/sta_ioctl.c
index 9f33c92c90f..13100f8de3d 100644
--- a/drivers/net/wireless/mwifiex/sta_ioctl.c
+++ b/drivers/net/wireless/mwifiex/sta_ioctl.c
@@ -54,16 +54,10 @@ int mwifiex_copy_mcast_addr(struct mwifiex_multicast_list *mlist,
  * This function waits on a cmd wait queue. It also cancels the pending
  * request after waking up, in case of errors.
  */
-int mwifiex_wait_queue_complete(struct mwifiex_adapter *adapter)
+int mwifiex_wait_queue_complete(struct mwifiex_adapter *adapter,
+				struct cmd_ctrl_node *cmd_queued)
 {
 	int status;
-	struct cmd_ctrl_node *cmd_queued;
-
-	if (!adapter->cmd_queued)
-		return 0;
-
-	cmd_queued = adapter->cmd_queued;
-	adapter->cmd_queued = NULL;
 
 	dev_dbg(adapter->dev, "cmd pending\n");
 	atomic_inc(&adapter->cmd_pending);
diff --git a/drivers/net/wireless/orinoco/orinoco_cs.c b/drivers/net/wireless/orinoco/orinoco_cs.c
index d7dbc00bcfb..d21d9593931 100644
--- a/drivers/net/wireless/orinoco/orinoco_cs.c
+++ b/drivers/net/wireless/orinoco/orinoco_cs.c
@@ -338,18 +338,4 @@ static struct pcmcia_driver orinoco_driver = {
 	.suspend	= orinoco_cs_suspend,
 	.resume		= orinoco_cs_resume,
 };
-
-static int __init
-init_orinoco_cs(void)
-{
-	return pcmcia_register_driver(&orinoco_driver);
-}
-
-static void __exit
-exit_orinoco_cs(void)
-{
-	pcmcia_unregister_driver(&orinoco_driver);
-}
-
-module_init(init_orinoco_cs);
-module_exit(exit_orinoco_cs);
+module_pcmcia_driver(orinoco_driver);
diff --git a/drivers/net/wireless/orinoco/spectrum_cs.c b/drivers/net/wireless/orinoco/spectrum_cs.c
index 6e28ee4e9c5..e2264bc12eb 100644
--- a/drivers/net/wireless/orinoco/spectrum_cs.c
+++ b/drivers/net/wireless/orinoco/spectrum_cs.c
@@ -318,18 +318,4 @@ static struct pcmcia_driver orinoco_driver = {
 	.resume		= spectrum_cs_resume,
 	.id_table       = spectrum_cs_ids,
 };
-
-static int __init
-init_spectrum_cs(void)
-{
-	return pcmcia_register_driver(&orinoco_driver);
-}
-
-static void __exit
-exit_spectrum_cs(void)
-{
-	pcmcia_unregister_driver(&orinoco_driver);
-}
-
-module_init(init_spectrum_cs);
-module_exit(exit_spectrum_cs);
+module_pcmcia_driver(orinoco_driver);
diff --git a/drivers/net/wireless/rt2x00/Kconfig b/drivers/net/wireless/rt2x00/Kconfig
index 2bf4efa3318..76cd47eb901 100644
--- a/drivers/net/wireless/rt2x00/Kconfig
+++ b/drivers/net/wireless/rt2x00/Kconfig
@@ -20,6 +20,7 @@ if RT2X00
 config RT2400PCI
 	tristate "Ralink rt2400 (PCI/PCMCIA) support"
 	depends on PCI
+	select RT2X00_LIB_MMIO
 	select RT2X00_LIB_PCI
 	select EEPROM_93CX6
 	---help---
@@ -31,6 +32,7 @@ config RT2400PCI
 config RT2500PCI
 	tristate "Ralink rt2500 (PCI/PCMCIA) support"
 	depends on PCI
+	select RT2X00_LIB_MMIO
 	select RT2X00_LIB_PCI
 	select EEPROM_93CX6
 	---help---
@@ -43,6 +45,7 @@ config RT61PCI
 	tristate "Ralink rt2501/rt61 (PCI/PCMCIA) support"
 	depends on PCI
 	select RT2X00_LIB_PCI
+	select RT2X00_LIB_MMIO
 	select RT2X00_LIB_FIRMWARE
 	select RT2X00_LIB_CRYPTO
 	select CRC_ITU_T
@@ -57,6 +60,7 @@ config RT2800PCI
 	tristate "Ralink rt27xx/rt28xx/rt30xx (PCI/PCIe/PCMCIA) support"
 	depends on PCI || SOC_RT288X || SOC_RT305X
 	select RT2800_LIB
+	select RT2X00_LIB_MMIO
 	select RT2X00_LIB_PCI if PCI
 	select RT2X00_LIB_SOC if SOC_RT288X || SOC_RT305X
 	select RT2X00_LIB_FIRMWARE
@@ -185,6 +189,9 @@ endif
 config RT2800_LIB
 	tristate
 
+config RT2X00_LIB_MMIO
+	tristate
+
 config RT2X00_LIB_PCI
 	tristate
 	select RT2X00_LIB
diff --git a/drivers/net/wireless/rt2x00/Makefile b/drivers/net/wireless/rt2x00/Makefile
index 349d5b8284a..f069d8bc5b6 100644
--- a/drivers/net/wireless/rt2x00/Makefile
+++ b/drivers/net/wireless/rt2x00/Makefile
@@ -9,6 +9,7 @@ rt2x00lib-$(CONFIG_RT2X00_LIB_FIRMWARE)	+= rt2x00firmware.o
 rt2x00lib-$(CONFIG_RT2X00_LIB_LEDS)	+= rt2x00leds.o
 
 obj-$(CONFIG_RT2X00_LIB)		+= rt2x00lib.o
+obj-$(CONFIG_RT2X00_LIB_MMIO)		+= rt2x00mmio.o
 obj-$(CONFIG_RT2X00_LIB_PCI)		+= rt2x00pci.o
 obj-$(CONFIG_RT2X00_LIB_SOC)		+= rt2x00soc.o
 obj-$(CONFIG_RT2X00_LIB_USB)		+= rt2x00usb.o
diff --git a/drivers/net/wireless/rt2x00/rt2400pci.c b/drivers/net/wireless/rt2x00/rt2400pci.c
index 221beaaa83f..dcfb54e0c51 100644
--- a/drivers/net/wireless/rt2x00/rt2400pci.c
+++ b/drivers/net/wireless/rt2x00/rt2400pci.c
@@ -34,6 +34,7 @@
 #include <linux/slab.h>
 
 #include "rt2x00.h"
+#include "rt2x00mmio.h"
 #include "rt2x00pci.h"
 #include "rt2400pci.h"
 
diff --git a/drivers/net/wireless/rt2x00/rt2500pci.c b/drivers/net/wireless/rt2x00/rt2500pci.c
index 39edc59e8d0..e1d2dc9ed28 100644
--- a/drivers/net/wireless/rt2x00/rt2500pci.c
+++ b/drivers/net/wireless/rt2x00/rt2500pci.c
@@ -34,6 +34,7 @@
 #include <linux/slab.h>
 
 #include "rt2x00.h"
+#include "rt2x00mmio.h"
 #include "rt2x00pci.h"
 #include "rt2500pci.h"
 
diff --git a/drivers/net/wireless/rt2x00/rt2800pci.c b/drivers/net/wireless/rt2x00/rt2800pci.c
index ded73da4de0..ba5a05625aa 100644
--- a/drivers/net/wireless/rt2x00/rt2800pci.c
+++ b/drivers/net/wireless/rt2x00/rt2800pci.c
@@ -41,6 +41,7 @@
 #include <linux/eeprom_93cx6.h>
 
 #include "rt2x00.h"
+#include "rt2x00mmio.h"
 #include "rt2x00pci.h"
 #include "rt2x00soc.h"
 #include "rt2800lib.h"
diff --git a/drivers/net/wireless/rt2x00/rt2x00mmio.c b/drivers/net/wireless/rt2x00/rt2x00mmio.c
new file mode 100644
index 00000000000..d84a680ba0c
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2x00mmio.c
@@ -0,0 +1,216 @@
+/*
+	Copyright (C) 2004 - 2009 Ivo van Doorn <IvDoorn@gmail.com>
+	<http://rt2x00.serialmonkey.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.
+ */
+
+/*
+	Module: rt2x00mmio
+	Abstract: rt2x00 generic mmio device routines.
+ */
+
+#include <linux/dma-mapping.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+
+#include "rt2x00.h"
+#include "rt2x00mmio.h"
+
+/*
+ * Register access.
+ */
+int rt2x00pci_regbusy_read(struct rt2x00_dev *rt2x00dev,
+			   const unsigned int offset,
+			   const struct rt2x00_field32 field,
+			   u32 *reg)
+{
+	unsigned int i;
+
+	if (!test_bit(DEVICE_STATE_PRESENT, &rt2x00dev->flags))
+		return 0;
+
+	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
+		rt2x00pci_register_read(rt2x00dev, offset, reg);
+		if (!rt2x00_get_field32(*reg, field))
+			return 1;
+		udelay(REGISTER_BUSY_DELAY);
+	}
+
+	printk_once(KERN_ERR "%s() Indirect register access failed: "
+	      "offset=0x%.08x, value=0x%.08x\n", __func__, offset, *reg);
+	*reg = ~0;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(rt2x00pci_regbusy_read);
+
+bool rt2x00pci_rxdone(struct rt2x00_dev *rt2x00dev)
+{
+	struct data_queue *queue = rt2x00dev->rx;
+	struct queue_entry *entry;
+	struct queue_entry_priv_pci *entry_priv;
+	struct skb_frame_desc *skbdesc;
+	int max_rx = 16;
+
+	while (--max_rx) {
+		entry = rt2x00queue_get_entry(queue, Q_INDEX);
+		entry_priv = entry->priv_data;
+
+		if (rt2x00dev->ops->lib->get_entry_state(entry))
+			break;
+
+		/*
+		 * Fill in desc fields of the skb descriptor
+		 */
+		skbdesc = get_skb_frame_desc(entry->skb);
+		skbdesc->desc = entry_priv->desc;
+		skbdesc->desc_len = entry->queue->desc_size;
+
+		/*
+		 * DMA is already done, notify rt2x00lib that
+		 * it finished successfully.
+		 */
+		rt2x00lib_dmastart(entry);
+		rt2x00lib_dmadone(entry);
+
+		/*
+		 * Send the frame to rt2x00lib for further processing.
+		 */
+		rt2x00lib_rxdone(entry, GFP_ATOMIC);
+	}
+
+	return !max_rx;
+}
+EXPORT_SYMBOL_GPL(rt2x00pci_rxdone);
+
+void rt2x00pci_flush_queue(struct data_queue *queue, bool drop)
+{
+	unsigned int i;
+
+	for (i = 0; !rt2x00queue_empty(queue) && i < 10; i++)
+		msleep(10);
+}
+EXPORT_SYMBOL_GPL(rt2x00pci_flush_queue);
+
+/*
+ * Device initialization handlers.
+ */
+static int rt2x00pci_alloc_queue_dma(struct rt2x00_dev *rt2x00dev,
+				     struct data_queue *queue)
+{
+	struct queue_entry_priv_pci *entry_priv;
+	void *addr;
+	dma_addr_t dma;
+	unsigned int i;
+
+	/*
+	 * Allocate DMA memory for descriptor and buffer.
+	 */
+	addr = dma_alloc_coherent(rt2x00dev->dev,
+				  queue->limit * queue->desc_size,
+				  &dma, GFP_KERNEL);
+	if (!addr)
+		return -ENOMEM;
+
+	memset(addr, 0, queue->limit * queue->desc_size);
+
+	/*
+	 * Initialize all queue entries to contain valid addresses.
+	 */
+	for (i = 0; i < queue->limit; i++) {
+		entry_priv = queue->entries[i].priv_data;
+		entry_priv->desc = addr + i * queue->desc_size;
+		entry_priv->desc_dma = dma + i * queue->desc_size;
+	}
+
+	return 0;
+}
+
+static void rt2x00pci_free_queue_dma(struct rt2x00_dev *rt2x00dev,
+				     struct data_queue *queue)
+{
+	struct queue_entry_priv_pci *entry_priv =
+	    queue->entries[0].priv_data;
+
+	if (entry_priv->desc)
+		dma_free_coherent(rt2x00dev->dev,
+				  queue->limit * queue->desc_size,
+				  entry_priv->desc, entry_priv->desc_dma);
+	entry_priv->desc = NULL;
+}
+
+int rt2x00pci_initialize(struct rt2x00_dev *rt2x00dev)
+{
+	struct data_queue *queue;
+	int status;
+
+	/*
+	 * Allocate DMA
+	 */
+	queue_for_each(rt2x00dev, queue) {
+		status = rt2x00pci_alloc_queue_dma(rt2x00dev, queue);
+		if (status)
+			goto exit;
+	}
+
+	/*
+	 * Register interrupt handler.
+	 */
+	status = request_irq(rt2x00dev->irq,
+			     rt2x00dev->ops->lib->irq_handler,
+			     IRQF_SHARED, rt2x00dev->name, rt2x00dev);
+	if (status) {
+		ERROR(rt2x00dev, "IRQ %d allocation failed (error %d).\n",
+		      rt2x00dev->irq, status);
+		goto exit;
+	}
+
+	return 0;
+
+exit:
+	queue_for_each(rt2x00dev, queue)
+		rt2x00pci_free_queue_dma(rt2x00dev, queue);
+
+	return status;
+}
+EXPORT_SYMBOL_GPL(rt2x00pci_initialize);
+
+void rt2x00pci_uninitialize(struct rt2x00_dev *rt2x00dev)
+{
+	struct data_queue *queue;
+
+	/*
+	 * Free irq line.
+	 */
+	free_irq(rt2x00dev->irq, rt2x00dev);
+
+	/*
+	 * Free DMA
+	 */
+	queue_for_each(rt2x00dev, queue)
+		rt2x00pci_free_queue_dma(rt2x00dev, queue);
+}
+EXPORT_SYMBOL_GPL(rt2x00pci_uninitialize);
+
+/*
+ * rt2x00mmio module information.
+ */
+MODULE_AUTHOR(DRV_PROJECT);
+MODULE_VERSION(DRV_VERSION);
+MODULE_DESCRIPTION("rt2x00 mmio library");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/wireless/rt2x00/rt2x00mmio.h b/drivers/net/wireless/rt2x00/rt2x00mmio.h
new file mode 100644
index 00000000000..4ecaf60175b
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2x00mmio.h
@@ -0,0 +1,119 @@
+/*
+	Copyright (C) 2004 - 2009 Ivo van Doorn <IvDoorn@gmail.com>
+	<http://rt2x00.serialmonkey.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.
+ */
+
+/*
+	Module: rt2x00mmio
+	Abstract: Data structures for the rt2x00mmio module.
+ */
+
+#ifndef RT2X00MMIO_H
+#define RT2X00MMIO_H
+
+#include <linux/io.h>
+
+/*
+ * Register access.
+ */
+static inline void rt2x00pci_register_read(struct rt2x00_dev *rt2x00dev,
+					   const unsigned int offset,
+					   u32 *value)
+{
+	*value = readl(rt2x00dev->csr.base + offset);
+}
+
+static inline void rt2x00pci_register_multiread(struct rt2x00_dev *rt2x00dev,
+						const unsigned int offset,
+						void *value, const u32 length)
+{
+	memcpy_fromio(value, rt2x00dev->csr.base + offset, length);
+}
+
+static inline void rt2x00pci_register_write(struct rt2x00_dev *rt2x00dev,
+					    const unsigned int offset,
+					    u32 value)
+{
+	writel(value, rt2x00dev->csr.base + offset);
+}
+
+static inline void rt2x00pci_register_multiwrite(struct rt2x00_dev *rt2x00dev,
+						 const unsigned int offset,
+						 const void *value,
+						 const u32 length)
+{
+	__iowrite32_copy(rt2x00dev->csr.base + offset, value, length >> 2);
+}
+
+/**
+ * rt2x00pci_regbusy_read - Read from register with busy check
+ * @rt2x00dev: Device pointer, see &struct rt2x00_dev.
+ * @offset: Register offset
+ * @field: Field to check if register is busy
+ * @reg: Pointer to where register contents should be stored
+ *
+ * This function will read the given register, and checks if the
+ * register is busy. If it is, it will sleep for a couple of
+ * microseconds before reading the register again. If the register
+ * is not read after a certain timeout, this function will return
+ * FALSE.
+ */
+int rt2x00pci_regbusy_read(struct rt2x00_dev *rt2x00dev,
+			   const unsigned int offset,
+			   const struct rt2x00_field32 field,
+			   u32 *reg);
+
+/**
+ * struct queue_entry_priv_pci: Per entry PCI specific information
+ *
+ * @desc: Pointer to device descriptor
+ * @desc_dma: DMA pointer to &desc.
+ * @data: Pointer to device's entry memory.
+ * @data_dma: DMA pointer to &data.
+ */
+struct queue_entry_priv_pci {
+	__le32 *desc;
+	dma_addr_t desc_dma;
+};
+
+/**
+ * rt2x00pci_rxdone - Handle RX done events
+ * @rt2x00dev: Device pointer, see &struct rt2x00_dev.
+ *
+ * Returns true if there are still rx frames pending and false if all
+ * pending rx frames were processed.
+ */
+bool rt2x00pci_rxdone(struct rt2x00_dev *rt2x00dev);
+
+/**
+ * rt2x00pci_flush_queue - Flush data queue
+ * @queue: Data queue to stop
+ * @drop: True to drop all pending frames.
+ *
+ * This will wait for a maximum of 100ms, waiting for the queues
+ * to become empty.
+ */
+void rt2x00pci_flush_queue(struct data_queue *queue, bool drop);
+
+/*
+ * Device initialization handlers.
+ */
+int rt2x00pci_initialize(struct rt2x00_dev *rt2x00dev);
+void rt2x00pci_uninitialize(struct rt2x00_dev *rt2x00dev);
+
+#endif /* RT2X00MMIO_H */
diff --git a/drivers/net/wireless/rt2x00/rt2x00pci.c b/drivers/net/wireless/rt2x00/rt2x00pci.c
index a0c8caef3b0..e87865e3311 100644
--- a/drivers/net/wireless/rt2x00/rt2x00pci.c
+++ b/drivers/net/wireless/rt2x00/rt2x00pci.c
@@ -33,182 +33,6 @@
 #include "rt2x00pci.h"
 
 /*
- * Register access.
- */
-int rt2x00pci_regbusy_read(struct rt2x00_dev *rt2x00dev,
-			   const unsigned int offset,
-			   const struct rt2x00_field32 field,
-			   u32 *reg)
-{
-	unsigned int i;
-
-	if (!test_bit(DEVICE_STATE_PRESENT, &rt2x00dev->flags))
-		return 0;
-
-	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-		rt2x00pci_register_read(rt2x00dev, offset, reg);
-		if (!rt2x00_get_field32(*reg, field))
-			return 1;
-		udelay(REGISTER_BUSY_DELAY);
-	}
-
-	ERROR(rt2x00dev, "Indirect register access failed: "
-	      "offset=0x%.08x, value=0x%.08x\n", offset, *reg);
-	*reg = ~0;
-
-	return 0;
-}
-EXPORT_SYMBOL_GPL(rt2x00pci_regbusy_read);
-
-bool rt2x00pci_rxdone(struct rt2x00_dev *rt2x00dev)
-{
-	struct data_queue *queue = rt2x00dev->rx;
-	struct queue_entry *entry;
-	struct queue_entry_priv_pci *entry_priv;
-	struct skb_frame_desc *skbdesc;
-	int max_rx = 16;
-
-	while (--max_rx) {
-		entry = rt2x00queue_get_entry(queue, Q_INDEX);
-		entry_priv = entry->priv_data;
-
-		if (rt2x00dev->ops->lib->get_entry_state(entry))
-			break;
-
-		/*
-		 * Fill in desc fields of the skb descriptor
-		 */
-		skbdesc = get_skb_frame_desc(entry->skb);
-		skbdesc->desc = entry_priv->desc;
-		skbdesc->desc_len = entry->queue->desc_size;
-
-		/*
-		 * DMA is already done, notify rt2x00lib that
-		 * it finished successfully.
-		 */
-		rt2x00lib_dmastart(entry);
-		rt2x00lib_dmadone(entry);
-
-		/*
-		 * Send the frame to rt2x00lib for further processing.
-		 */
-		rt2x00lib_rxdone(entry, GFP_ATOMIC);
-	}
-
-	return !max_rx;
-}
-EXPORT_SYMBOL_GPL(rt2x00pci_rxdone);
-
-void rt2x00pci_flush_queue(struct data_queue *queue, bool drop)
-{
-	unsigned int i;
-
-	for (i = 0; !rt2x00queue_empty(queue) && i < 10; i++)
-		msleep(10);
-}
-EXPORT_SYMBOL_GPL(rt2x00pci_flush_queue);
-
-/*
- * Device initialization handlers.
- */
-static int rt2x00pci_alloc_queue_dma(struct rt2x00_dev *rt2x00dev,
-				     struct data_queue *queue)
-{
-	struct queue_entry_priv_pci *entry_priv;
-	void *addr;
-	dma_addr_t dma;
-	unsigned int i;
-
-	/*
-	 * Allocate DMA memory for descriptor and buffer.
-	 */
-	addr = dma_alloc_coherent(rt2x00dev->dev,
-				  queue->limit * queue->desc_size,
-				  &dma, GFP_KERNEL);
-	if (!addr)
-		return -ENOMEM;
-
-	memset(addr, 0, queue->limit * queue->desc_size);
-
-	/*
-	 * Initialize all queue entries to contain valid addresses.
-	 */
-	for (i = 0; i < queue->limit; i++) {
-		entry_priv = queue->entries[i].priv_data;
-		entry_priv->desc = addr + i * queue->desc_size;
-		entry_priv->desc_dma = dma + i * queue->desc_size;
-	}
-
-	return 0;
-}
-
-static void rt2x00pci_free_queue_dma(struct rt2x00_dev *rt2x00dev,
-				     struct data_queue *queue)
-{
-	struct queue_entry_priv_pci *entry_priv =
-	    queue->entries[0].priv_data;
-
-	if (entry_priv->desc)
-		dma_free_coherent(rt2x00dev->dev,
-				  queue->limit * queue->desc_size,
-				  entry_priv->desc, entry_priv->desc_dma);
-	entry_priv->desc = NULL;
-}
-
-int rt2x00pci_initialize(struct rt2x00_dev *rt2x00dev)
-{
-	struct data_queue *queue;
-	int status;
-
-	/*
-	 * Allocate DMA
-	 */
-	queue_for_each(rt2x00dev, queue) {
-		status = rt2x00pci_alloc_queue_dma(rt2x00dev, queue);
-		if (status)
-			goto exit;
-	}
-
-	/*
-	 * Register interrupt handler.
-	 */
-	status = request_irq(rt2x00dev->irq,
-			     rt2x00dev->ops->lib->irq_handler,
-			     IRQF_SHARED, rt2x00dev->name, rt2x00dev);
-	if (status) {
-		ERROR(rt2x00dev, "IRQ %d allocation failed (error %d).\n",
-		      rt2x00dev->irq, status);
-		goto exit;
-	}
-
-	return 0;
-
-exit:
-	queue_for_each(rt2x00dev, queue)
-		rt2x00pci_free_queue_dma(rt2x00dev, queue);
-
-	return status;
-}
-EXPORT_SYMBOL_GPL(rt2x00pci_initialize);
-
-void rt2x00pci_uninitialize(struct rt2x00_dev *rt2x00dev)
-{
-	struct data_queue *queue;
-
-	/*
-	 * Free irq line.
-	 */
-	free_irq(rt2x00dev->irq, rt2x00dev);
-
-	/*
-	 * Free DMA
-	 */
-	queue_for_each(rt2x00dev, queue)
-		rt2x00pci_free_queue_dma(rt2x00dev, queue);
-}
-EXPORT_SYMBOL_GPL(rt2x00pci_uninitialize);
-
-/*
  * PCI driver handlers.
  */
 static void rt2x00pci_free_reg(struct rt2x00_dev *rt2x00dev)
diff --git a/drivers/net/wireless/rt2x00/rt2x00pci.h b/drivers/net/wireless/rt2x00/rt2x00pci.h
index e2c99f2b9a1..60d90b20f8b 100644
--- a/drivers/net/wireless/rt2x00/rt2x00pci.h
+++ b/drivers/net/wireless/rt2x00/rt2x00pci.h
@@ -36,94 +36,6 @@
 #define PCI_DEVICE_DATA(__ops)	.driver_data = (kernel_ulong_t)(__ops)
 
 /*
- * Register access.
- */
-static inline void rt2x00pci_register_read(struct rt2x00_dev *rt2x00dev,
-					   const unsigned int offset,
-					   u32 *value)
-{
-	*value = readl(rt2x00dev->csr.base + offset);
-}
-
-static inline void rt2x00pci_register_multiread(struct rt2x00_dev *rt2x00dev,
-						const unsigned int offset,
-						void *value, const u32 length)
-{
-	memcpy_fromio(value, rt2x00dev->csr.base + offset, length);
-}
-
-static inline void rt2x00pci_register_write(struct rt2x00_dev *rt2x00dev,
-					    const unsigned int offset,
-					    u32 value)
-{
-	writel(value, rt2x00dev->csr.base + offset);
-}
-
-static inline void rt2x00pci_register_multiwrite(struct rt2x00_dev *rt2x00dev,
-						 const unsigned int offset,
-						 const void *value,
-						 const u32 length)
-{
-	__iowrite32_copy(rt2x00dev->csr.base + offset, value, length >> 2);
-}
-
-/**
- * rt2x00pci_regbusy_read - Read from register with busy check
- * @rt2x00dev: Device pointer, see &struct rt2x00_dev.
- * @offset: Register offset
- * @field: Field to check if register is busy
- * @reg: Pointer to where register contents should be stored
- *
- * This function will read the given register, and checks if the
- * register is busy. If it is, it will sleep for a couple of
- * microseconds before reading the register again. If the register
- * is not read after a certain timeout, this function will return
- * FALSE.
- */
-int rt2x00pci_regbusy_read(struct rt2x00_dev *rt2x00dev,
-			   const unsigned int offset,
-			   const struct rt2x00_field32 field,
-			   u32 *reg);
-
-/**
- * struct queue_entry_priv_pci: Per entry PCI specific information
- *
- * @desc: Pointer to device descriptor
- * @desc_dma: DMA pointer to &desc.
- * @data: Pointer to device's entry memory.
- * @data_dma: DMA pointer to &data.
- */
-struct queue_entry_priv_pci {
-	__le32 *desc;
-	dma_addr_t desc_dma;
-};
-
-/**
- * rt2x00pci_rxdone - Handle RX done events
- * @rt2x00dev: Device pointer, see &struct rt2x00_dev.
- *
- * Returns true if there are still rx frames pending and false if all
- * pending rx frames were processed.
- */
-bool rt2x00pci_rxdone(struct rt2x00_dev *rt2x00dev);
-
-/**
- * rt2x00pci_flush_queue - Flush data queue
- * @queue: Data queue to stop
- * @drop: True to drop all pending frames.
- *
- * This will wait for a maximum of 100ms, waiting for the queues
- * to become empty.
- */
-void rt2x00pci_flush_queue(struct data_queue *queue, bool drop);
-
-/*
- * Device initialization handlers.
- */
-int rt2x00pci_initialize(struct rt2x00_dev *rt2x00dev);
-void rt2x00pci_uninitialize(struct rt2x00_dev *rt2x00dev);
-
-/*
  * PCI driver handlers.
  */
 int rt2x00pci_probe(struct pci_dev *pci_dev, const struct rt2x00_ops *ops);
diff --git a/drivers/net/wireless/rt2x00/rt61pci.c b/drivers/net/wireless/rt2x00/rt61pci.c
index f95792cfcf8..9e3c8ff53e3 100644
--- a/drivers/net/wireless/rt2x00/rt61pci.c
+++ b/drivers/net/wireless/rt2x00/rt61pci.c
@@ -35,6 +35,7 @@
 #include <linux/eeprom_93cx6.h>
 
 #include "rt2x00.h"
+#include "rt2x00mmio.h"
 #include "rt2x00pci.h"
 #include "rt61pci.h"
 
diff --git a/drivers/net/wireless/rtlwifi/usb.c b/drivers/net/wireless/rtlwifi/usb.c
index 156b52732f3..5847d6d0881 100644
--- a/drivers/net/wireless/rtlwifi/usb.c
+++ b/drivers/net/wireless/rtlwifi/usb.c
@@ -851,6 +851,7 @@ static void _rtl_usb_transmit(struct ieee80211_hw *hw, struct sk_buff *skb,
 	if (unlikely(!_urb)) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 			 "Can't allocate urb. Drop skb!\n");
+		kfree_skb(skb);
 		return;
 	}
 	_rtl_submit_tx_urb(hw, _urb);
diff --git a/drivers/net/wireless/wl3501_cs.c b/drivers/net/wireless/wl3501_cs.c
index 730186d0449..38d2089f338 100644
--- a/drivers/net/wireless/wl3501_cs.c
+++ b/drivers/net/wireless/wl3501_cs.c
@@ -2013,19 +2013,7 @@ static struct pcmcia_driver wl3501_driver = {
 	.suspend	= wl3501_suspend,
 	.resume		= wl3501_resume,
 };
-
-static int __init wl3501_init_module(void)
-{
-	return pcmcia_register_driver(&wl3501_driver);
-}
-
-static void __exit wl3501_exit_module(void)
-{
-	pcmcia_unregister_driver(&wl3501_driver);
-}
-
-module_init(wl3501_init_module);
-module_exit(wl3501_exit_module);
+module_pcmcia_driver(wl3501_driver);
 
 MODULE_AUTHOR("Fox Chen <mhchen@golf.ccl.itri.org.tw>, "
 	      "Arnaldo Carvalho de Melo <acme@conectiva.com.br>,"
diff --git a/drivers/nfc/microread/mei.c b/drivers/nfc/microread/mei.c
index eef38cfd812..ca33ae19393 100644
--- a/drivers/nfc/microread/mei.c
+++ b/drivers/nfc/microread/mei.c
@@ -22,7 +22,7 @@
 #include <linux/slab.h>
 #include <linux/interrupt.h>
 #include <linux/gpio.h>
-#include <linux/mei_bus.h>
+#include <linux/mei_cl_bus.h>
 
 #include <linux/nfc.h>
 #include <net/nfc/hci.h>
@@ -32,9 +32,6 @@
 
 #define MICROREAD_DRIVER_NAME "microread"
 
-#define MICROREAD_UUID UUID_LE(0x0bb17a78, 0x2a8e, 0x4c50, 0x94, \
-			       0xd4, 0x50, 0x26, 0x67, 0x23, 0x77, 0x5c)
-
 struct mei_nfc_hdr {
 	u8 cmd;
 	u8 status;
@@ -48,7 +45,7 @@ struct mei_nfc_hdr {
 #define MEI_NFC_MAX_READ (MEI_NFC_HEADER_SIZE + MEI_NFC_MAX_HCI_PAYLOAD)
 
 struct microread_mei_phy {
-	struct mei_device *mei_device;
+	struct mei_cl_device *device;
 	struct nfc_hci_dev *hdev;
 
 	int powered;
@@ -105,14 +102,14 @@ static int microread_mei_write(void *phy_id, struct sk_buff *skb)
 
 	MEI_DUMP_SKB_OUT("mei frame sent", skb);
 
-	r = mei_send(phy->device, skb->data, skb->len);
+	r = mei_cl_send(phy->device, skb->data, skb->len);
 	if (r > 0)
 		r = 0;
 
 	return r;
 }
 
-static void microread_event_cb(struct mei_device *device, u32 events,
+static void microread_event_cb(struct mei_cl_device *device, u32 events,
 			       void *context)
 {
 	struct microread_mei_phy *phy = context;
@@ -120,7 +117,7 @@ static void microread_event_cb(struct mei_device *device, u32 events,
 	if (phy->hard_fault != 0)
 		return;
 
-	if (events & BIT(MEI_EVENT_RX)) {
+	if (events & BIT(MEI_CL_EVENT_RX)) {
 		struct sk_buff *skb;
 		int reply_size;
 
@@ -128,7 +125,7 @@ static void microread_event_cb(struct mei_device *device, u32 events,
 		if (!skb)
 			return;
 
-		reply_size = mei_recv(device, skb->data, MEI_NFC_MAX_READ);
+		reply_size = mei_cl_recv(device, skb->data, MEI_NFC_MAX_READ);
 		if (reply_size < MEI_NFC_HEADER_SIZE) {
 			kfree(skb);
 			return;
@@ -149,8 +146,8 @@ static struct nfc_phy_ops mei_phy_ops = {
 	.disable = microread_mei_disable,
 };
 
-static int microread_mei_probe(struct mei_device *device,
-			       const struct mei_id *id)
+static int microread_mei_probe(struct mei_cl_device *device,
+			       const struct mei_cl_device_id *id)
 {
 	struct microread_mei_phy *phy;
 	int r;
@@ -164,9 +161,9 @@ static int microread_mei_probe(struct mei_device *device,
 	}
 
 	phy->device = device;
-	mei_set_clientdata(device, phy);
+	mei_cl_set_drvdata(device, phy);
 
-	r = mei_register_event_cb(device, microread_event_cb, phy);
+	r = mei_cl_register_event_cb(device, microread_event_cb, phy);
 	if (r) {
 		pr_err(MICROREAD_DRIVER_NAME ": event cb registration failed\n");
 		goto err_out;
@@ -186,9 +183,9 @@ err_out:
 	return r;
 }
 
-static int microread_mei_remove(struct mei_device *device)
+static int microread_mei_remove(struct mei_cl_device *device)
 {
-	struct microread_mei_phy *phy = mei_get_clientdata(device);
+	struct microread_mei_phy *phy = mei_cl_get_drvdata(device);
 
 	pr_info("Removing microread\n");
 
@@ -202,16 +199,15 @@ static int microread_mei_remove(struct mei_device *device)
 	return 0;
 }
 
-static struct mei_id microread_mei_tbl[] = {
-	{ MICROREAD_DRIVER_NAME, MICROREAD_UUID },
+static struct mei_cl_device_id microread_mei_tbl[] = {
+	{ MICROREAD_DRIVER_NAME },
 
 	/* required last entry */
 	{ }
 };
-
 MODULE_DEVICE_TABLE(mei, microread_mei_tbl);
 
-static struct mei_driver microread_driver = {
+static struct mei_cl_driver microread_driver = {
 	.id_table = microread_mei_tbl,
 	.name = MICROREAD_DRIVER_NAME,
 
@@ -225,7 +221,7 @@ static int microread_mei_init(void)
 
 	pr_debug(DRIVER_DESC ": %s\n", __func__);
 
-	r = mei_driver_register(&microread_driver);
+	r = mei_cl_driver_register(&microread_driver);
 	if (r) {
 		pr_err(MICROREAD_DRIVER_NAME ": driver registration failed\n");
 		return r;
@@ -236,7 +232,7 @@ static int microread_mei_init(void)
 
 static void microread_mei_exit(void)
 {
-	mei_driver_unregister(&microread_driver);
+	mei_cl_driver_unregister(&microread_driver);
 }
 
 module_init(microread_mei_init);
diff --git a/drivers/of/base.c b/drivers/of/base.c
index 321d3ef0500..1733081eb87 100644
--- a/drivers/of/base.c
+++ b/drivers/of/base.c
@@ -382,6 +382,7 @@ struct device_node *of_get_next_parent(struct device_node *node)
 	raw_spin_unlock_irqrestore(&devtree_lock, flags);
 	return parent;
 }
+EXPORT_SYMBOL(of_get_next_parent);
 
 /**
  *	of_get_next_child - Iterate a node childs
diff --git a/drivers/parport/parport_amiga.c b/drivers/parport/parport_amiga.c
index ee78e0ee6e0..09503b8d12e 100644
--- a/drivers/parport/parport_amiga.c
+++ b/drivers/parport/parport_amiga.c
@@ -244,20 +244,7 @@ static struct platform_driver amiga_parallel_driver = {
 	},
 };
 
-static int __init amiga_parallel_init(void)
-{
-	return platform_driver_probe(&amiga_parallel_driver,
-				     amiga_parallel_probe);
-}
-
-module_init(amiga_parallel_init);
-
-static void __exit amiga_parallel_exit(void)
-{
-	platform_driver_unregister(&amiga_parallel_driver);
-}
-
-module_exit(amiga_parallel_exit);
+module_platform_driver_probe(amiga_parallel_driver, amiga_parallel_probe);
 
 MODULE_AUTHOR("Joerg Dorchain <joerg@dorchain.net>");
 MODULE_DESCRIPTION("Parport Driver for Amiga builtin Port");
diff --git a/drivers/parport/parport_cs.c b/drivers/parport/parport_cs.c
index 067ad517c1f..e9b52e4a464 100644
--- a/drivers/parport/parport_cs.c
+++ b/drivers/parport/parport_cs.c
@@ -193,16 +193,4 @@ static struct pcmcia_driver parport_cs_driver = {
 	.remove		= parport_detach,
 	.id_table	= parport_ids,
 };
-
-static int __init init_parport_cs(void)
-{
-	return pcmcia_register_driver(&parport_cs_driver);
-}
-
-static void __exit exit_parport_cs(void)
-{
-	pcmcia_unregister_driver(&parport_cs_driver);
-}
-
-module_init(init_parport_cs);
-module_exit(exit_parport_cs);
+module_pcmcia_driver(parport_cs_driver);
diff --git a/drivers/parport/parport_gsc.c b/drivers/parport/parport_gsc.c
index 050773c3682..a5251cb5fb0 100644
--- a/drivers/parport/parport_gsc.c
+++ b/drivers/parport/parport_gsc.c
@@ -246,14 +246,14 @@ struct parport *parport_gsc_probe_port(unsigned long base,
 		printk (KERN_DEBUG "parport (0x%lx): no memory!\n", base);
 		return NULL;
 	}
-	ops = kmalloc (sizeof (struct parport_operations), GFP_KERNEL);
+	ops = kmemdup(&parport_gsc_ops, sizeof(struct parport_operations),
+		      GFP_KERNEL);
 	if (!ops) {
 		printk (KERN_DEBUG "parport (0x%lx): no memory for ops!\n",
 			base);
 		kfree (priv);
 		return NULL;
 	}
-	memcpy (ops, &parport_gsc_ops, sizeof (struct parport_operations));
 	priv->ctr = 0xc;
 	priv->ctr_writable = 0xff;
 	priv->dma_buf = 0;
diff --git a/drivers/parport/parport_sunbpp.c b/drivers/parport/parport_sunbpp.c
index 5c4b6a1db6c..dffd6d0bd15 100644
--- a/drivers/parport/parport_sunbpp.c
+++ b/drivers/parport/parport_sunbpp.c
@@ -284,12 +284,11 @@ static int bpp_probe(struct platform_device *op)
 	size = resource_size(&op->resource[0]);
 	dma = PARPORT_DMA_NONE;
 
-	ops = kmalloc(sizeof(struct parport_operations), GFP_KERNEL);
+	ops = kmemdup(&parport_sunbpp_ops, sizeof(struct parport_operations),
+		      GFP_KERNEL);
         if (!ops)
 		goto out_unmap;
 
-        memcpy (ops, &parport_sunbpp_ops, sizeof(struct parport_operations));
-
 	dprintk(("register_port\n"));
 	if (!(p = parport_register_port((unsigned long)base, irq, dma, ops)))
 		goto out_free_ops;
diff --git a/drivers/parport/procfs.c b/drivers/parport/procfs.c
index 3f56bc086cb..92ed045a5f9 100644
--- a/drivers/parport/procfs.c
+++ b/drivers/parport/procfs.c
@@ -476,10 +476,9 @@ int parport_proc_register(struct parport *port)
 	struct parport_sysctl_table *t;
 	int i;
 
-	t = kmalloc(sizeof(*t), GFP_KERNEL);
+	t = kmemdup(&parport_sysctl_template, sizeof(*t), GFP_KERNEL);
 	if (t == NULL)
 		return -ENOMEM;
-	memcpy(t, &parport_sysctl_template, sizeof(*t));
 
 	t->device_dir[0].extra1 = port;
 
@@ -523,10 +522,9 @@ int parport_device_proc_register(struct pardevice *device)
 	struct parport_device_sysctl_table *t;
 	struct parport * port = device->port;
 	
-	t = kmalloc(sizeof(*t), GFP_KERNEL);
+	t = kmemdup(&parport_device_sysctl_template, sizeof(*t), GFP_KERNEL);
 	if (t == NULL)
 		return -ENOMEM;
-	memcpy(t, &parport_device_sysctl_template, sizeof(*t));
 
 	t->dev_dir[0].child = t->parport_dir;
 	t->parport_dir[0].child = t->port_dir;
diff --git a/drivers/pci/bus.c b/drivers/pci/bus.c
index 8647dc6f52d..748f8f3e9ff 100644
--- a/drivers/pci/bus.c
+++ b/drivers/pci/bus.c
@@ -202,20 +202,16 @@ void pci_bus_add_devices(const struct pci_bus *bus)
 		if (dev->is_added)
 			continue;
 		retval = pci_bus_add_device(dev);
+		if (retval)
+			dev_err(&dev->dev, "Error adding device (%d)\n",
+				retval);
 	}
 
 	list_for_each_entry(dev, &bus->devices, bus_list) {
 		BUG_ON(!dev->is_added);
-
 		child = dev->subordinate;
-
-		if (!child)
-			continue;
-		pci_bus_add_devices(child);
-
-		if (child->is_added)
-			continue;
-		child->is_added = 1;
+		if (child)
+			pci_bus_add_devices(child);
 	}
 }
 
diff --git a/drivers/pci/hotplug/Kconfig b/drivers/pci/hotplug/Kconfig
index 13e9e63a726..9fcb87f353d 100644
--- a/drivers/pci/hotplug/Kconfig
+++ b/drivers/pci/hotplug/Kconfig
@@ -52,15 +52,12 @@ config HOTPLUG_PCI_IBM
 	  When in doubt, say N.
 
 config HOTPLUG_PCI_ACPI
-	tristate "ACPI PCI Hotplug driver"
-	depends on (!ACPI_DOCK && ACPI) || (ACPI_DOCK)
+	bool "ACPI PCI Hotplug driver"
+	depends on HOTPLUG_PCI=y && ((!ACPI_DOCK && ACPI) || (ACPI_DOCK))
 	help
 	  Say Y here if you have a system that supports PCI Hotplug using
 	  ACPI.
 
-	  To compile this driver as a module, choose M here: the
-	  module will be called acpiphp.
-
 	  When in doubt, say N.
 
 config HOTPLUG_PCI_ACPI_IBM
diff --git a/drivers/pci/hotplug/acpiphp.h b/drivers/pci/hotplug/acpiphp.h
index b70ac00a117..6fdd49c6f0b 100644
--- a/drivers/pci/hotplug/acpiphp.h
+++ b/drivers/pci/hotplug/acpiphp.h
@@ -73,8 +73,9 @@ static inline const char *slot_name(struct slot *slot)
  */
 struct acpiphp_bridge {
 	struct list_head list;
+	struct list_head slots;
+	struct kref ref;
 	acpi_handle handle;
-	struct acpiphp_slot *slots;
 
 	/* Ejectable PCI-to-PCI bridge (PCI bridge and PCI function) */
 	struct acpiphp_func *func;
@@ -97,7 +98,7 @@ struct acpiphp_bridge {
  * PCI slot information for each *physical* PCI slot
  */
 struct acpiphp_slot {
-	struct acpiphp_slot *next;
+	struct list_head node;
 	struct acpiphp_bridge *bridge;	/* parent */
 	struct list_head funcs;		/* one slot may have different
 					   objects (i.e. for each function) */
@@ -119,7 +120,6 @@ struct acpiphp_slot {
  */
 struct acpiphp_func {
 	struct acpiphp_slot *slot;	/* parent */
-	struct acpiphp_bridge *bridge;	/* Ejectable PCI-to-PCI bridge */
 
 	struct list_head sibling;
 	struct notifier_block nb;
@@ -146,10 +146,6 @@ struct acpiphp_attention_info
 #define ACPI_PCI_HOST_HID		"PNP0A03"
 
 /* ACPI _STA method value (ignore bit 4; battery present) */
-#define ACPI_STA_PRESENT		(0x00000001)
-#define ACPI_STA_ENABLED		(0x00000002)
-#define ACPI_STA_SHOW_IN_UI		(0x00000004)
-#define ACPI_STA_FUNCTIONING		(0x00000008)
 #define ACPI_STA_ALL			(0x0000000f)
 
 /* bridge flags */
@@ -174,25 +170,24 @@ struct acpiphp_attention_info
 /* function prototypes */
 
 /* acpiphp_core.c */
-extern int acpiphp_register_attention(struct acpiphp_attention_info*info);
-extern int acpiphp_unregister_attention(struct acpiphp_attention_info *info);
-extern int acpiphp_register_hotplug_slot(struct acpiphp_slot *slot);
-extern void acpiphp_unregister_hotplug_slot(struct acpiphp_slot *slot);
+int acpiphp_register_attention(struct acpiphp_attention_info*info);
+int acpiphp_unregister_attention(struct acpiphp_attention_info *info);
+int acpiphp_register_hotplug_slot(struct acpiphp_slot *slot);
+void acpiphp_unregister_hotplug_slot(struct acpiphp_slot *slot);
 
 /* acpiphp_glue.c */
-extern int acpiphp_glue_init (void);
-extern void acpiphp_glue_exit (void);
 typedef int (*acpiphp_callback)(struct acpiphp_slot *slot, void *data);
 
-extern int acpiphp_enable_slot (struct acpiphp_slot *slot);
-extern int acpiphp_disable_slot (struct acpiphp_slot *slot);
-extern int acpiphp_eject_slot (struct acpiphp_slot *slot);
-extern u8 acpiphp_get_power_status (struct acpiphp_slot *slot);
-extern u8 acpiphp_get_attention_status (struct acpiphp_slot *slot);
-extern u8 acpiphp_get_latch_status (struct acpiphp_slot *slot);
-extern u8 acpiphp_get_adapter_status (struct acpiphp_slot *slot);
+int acpiphp_enable_slot(struct acpiphp_slot *slot);
+int acpiphp_disable_slot(struct acpiphp_slot *slot);
+int acpiphp_eject_slot(struct acpiphp_slot *slot);
+u8 acpiphp_get_power_status(struct acpiphp_slot *slot);
+u8 acpiphp_get_attention_status(struct acpiphp_slot *slot);
+u8 acpiphp_get_latch_status(struct acpiphp_slot *slot);
+u8 acpiphp_get_adapter_status(struct acpiphp_slot *slot);
 
 /* variables */
 extern bool acpiphp_debug;
+extern bool acpiphp_disabled;
 
 #endif /* _ACPIPHP_H */
diff --git a/drivers/pci/hotplug/acpiphp_core.c b/drivers/pci/hotplug/acpiphp_core.c
index c2fd3095701..ca8127950fc 100644
--- a/drivers/pci/hotplug/acpiphp_core.c
+++ b/drivers/pci/hotplug/acpiphp_core.c
@@ -37,6 +37,7 @@
 
 #include <linux/kernel.h>
 #include <linux/pci.h>
+#include <linux/pci-acpi.h>
 #include <linux/pci_hotplug.h>
 #include <linux/slab.h>
 #include <linux/smp.h>
@@ -48,6 +49,7 @@
 #define SLOT_NAME_SIZE  21              /* {_SUN} */
 
 bool acpiphp_debug;
+bool acpiphp_disabled;
 
 /* local variables */
 static struct acpiphp_attention_info *attention_info;
@@ -60,7 +62,9 @@ MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL");
 MODULE_PARM_DESC(debug, "Debugging mode enabled or not");
+MODULE_PARM_DESC(disable, "disable acpiphp driver");
 module_param_named(debug, acpiphp_debug, bool, 0644);
+module_param_named(disable, acpiphp_disabled, bool, 0444);
 
 /* export the attention callback registration methods */
 EXPORT_SYMBOL_GPL(acpiphp_register_attention);
@@ -351,27 +355,9 @@ void acpiphp_unregister_hotplug_slot(struct acpiphp_slot *acpiphp_slot)
 }
 
 
-static int __init acpiphp_init(void)
+void __init acpiphp_init(void)
 {
-	info(DRIVER_DESC " version: " DRIVER_VERSION "\n");
-
-	if (acpi_pci_disabled)
-		return 0;
-
-	/* read all the ACPI info from the system */
-	/* initialize internal data structure etc. */
-	return acpiphp_glue_init();
-}
-
-
-static void __exit acpiphp_exit(void)
-{
-	if (acpi_pci_disabled)
-		return;
-
-	/* deallocate internal data structures etc. */
-	acpiphp_glue_exit();
+	info(DRIVER_DESC " version: " DRIVER_VERSION "%s\n",
+		acpiphp_disabled ? ", disabled by user; please report a bug"
+				 : "");
 }
-
-module_init(acpiphp_init);
-module_exit(acpiphp_exit);
diff --git a/drivers/pci/hotplug/acpiphp_glue.c b/drivers/pci/hotplug/acpiphp_glue.c
index 270fdbadc19..96fed19c6d9 100644
--- a/drivers/pci/hotplug/acpiphp_glue.c
+++ b/drivers/pci/hotplug/acpiphp_glue.c
@@ -54,6 +54,7 @@
 #include "acpiphp.h"
 
 static LIST_HEAD(bridge_list);
+static DEFINE_MUTEX(bridge_mutex);
 
 #define MY_NAME "acpiphp_glue"
 
@@ -61,6 +62,7 @@ static void handle_hotplug_event_bridge (acpi_handle, u32, void *);
 static void acpiphp_sanitize_bus(struct pci_bus *bus);
 static void acpiphp_set_hpp_values(struct pci_bus *bus);
 static void handle_hotplug_event_func(acpi_handle handle, u32 type, void *context);
+static void free_bridge(struct kref *kref);
 
 /* callback routine to check for the existence of a pci dock device */
 static acpi_status
@@ -76,6 +78,39 @@ is_pci_dock_device(acpi_handle handle, u32 lvl, void *context, void **rv)
 	}
 }
 
+static inline void get_bridge(struct acpiphp_bridge *bridge)
+{
+	kref_get(&bridge->ref);
+}
+
+static inline void put_bridge(struct acpiphp_bridge *bridge)
+{
+	kref_put(&bridge->ref, free_bridge);
+}
+
+static void free_bridge(struct kref *kref)
+{
+	struct acpiphp_bridge *bridge;
+	struct acpiphp_slot *slot, *next;
+	struct acpiphp_func *func, *tmp;
+
+	bridge = container_of(kref, struct acpiphp_bridge, ref);
+
+	list_for_each_entry_safe(slot, next, &bridge->slots, node) {
+		list_for_each_entry_safe(func, tmp, &slot->funcs, sibling) {
+			kfree(func);
+		}
+		kfree(slot);
+	}
+
+	/* Release reference acquired by acpiphp_bridge_handle_to_function() */
+	if ((bridge->flags & BRIDGE_HAS_EJ0) && bridge->func)
+		put_bridge(bridge->func->slot->bridge);
+	put_device(&bridge->pci_bus->dev);
+	pci_dev_put(bridge->pci_dev);
+	kfree(bridge);
+}
+
 /*
  * the _DCK method can do funny things... and sometimes not
  * hah-hah funny.
@@ -154,9 +189,10 @@ register_slot(acpi_handle handle, u32 lvl, void *context, void **rv)
 	acpi_handle tmp;
 	acpi_status status = AE_OK;
 	unsigned long long adr, sun;
-	int device, function, retval;
+	int device, function, retval, found = 0;
 	struct pci_bus *pbus = bridge->pci_bus;
 	struct pci_dev *pdev;
+	u32 val;
 
 	if (!acpi_pci_check_ejectable(pbus, handle) && !is_dock_device(handle))
 		return AE_OK;
@@ -170,7 +206,7 @@ register_slot(acpi_handle handle, u32 lvl, void *context, void **rv)
 	device = (adr >> 16) & 0xffff;
 	function = adr & 0xffff;
 
-	pdev = pbus->self;
+	pdev = bridge->pci_dev;
 	if (pdev && device_is_managed_by_native_pciehp(pdev))
 		return AE_OK;
 
@@ -178,7 +214,6 @@ register_slot(acpi_handle handle, u32 lvl, void *context, void **rv)
 	if (!newfunc)
 		return AE_NO_MEMORY;
 
-	INIT_LIST_HEAD(&newfunc->sibling);
 	newfunc->handle = handle;
 	newfunc->function = function;
 
@@ -207,14 +242,15 @@ register_slot(acpi_handle handle, u32 lvl, void *context, void **rv)
 	}
 
 	/* search for objects that share the same slot */
-	for (slot = bridge->slots; slot; slot = slot->next)
+	list_for_each_entry(slot, &bridge->slots, node)
 		if (slot->device == device) {
 			if (slot->sun != sun)
 				warn("sibling found, but _SUN doesn't match!\n");
+			found = 1;
 			break;
 		}
 
-	if (!slot) {
+	if (!found) {
 		slot = kzalloc(sizeof(struct acpiphp_slot), GFP_KERNEL);
 		if (!slot) {
 			kfree(newfunc);
@@ -227,9 +263,9 @@ register_slot(acpi_handle handle, u32 lvl, void *context, void **rv)
 		INIT_LIST_HEAD(&slot->funcs);
 		mutex_init(&slot->crit_sect);
 
-		slot->next = bridge->slots;
-		bridge->slots = slot;
-
+		mutex_lock(&bridge_mutex);
+		list_add_tail(&slot->node, &bridge->slots);
+		mutex_unlock(&bridge_mutex);
 		bridge->nr_slots++;
 
 		dbg("found ACPI PCI Hotplug slot %llu at PCI %04x:%02x:%02x\n",
@@ -247,13 +283,13 @@ register_slot(acpi_handle handle, u32 lvl, void *context, void **rv)
 	}
 
 	newfunc->slot = slot;
+	mutex_lock(&bridge_mutex);
 	list_add_tail(&newfunc->sibling, &slot->funcs);
+	mutex_unlock(&bridge_mutex);
 
-	pdev = pci_get_slot(pbus, PCI_DEVFN(device, function));
-	if (pdev) {
+	if (pci_bus_read_dev_vendor_id(pbus, PCI_DEVFN(device, function),
+				       &val, 60*1000))
 		slot->flags |= (SLOT_ENABLED | SLOT_POWEREDON);
-		pci_dev_put(pdev);
-	}
 
 	if (is_dock_device(handle)) {
 		/* we don't want to call this device's _EJ0
@@ -290,7 +326,9 @@ register_slot(acpi_handle handle, u32 lvl, void *context, void **rv)
 
  err_exit:
 	bridge->nr_slots--;
-	bridge->slots = slot->next;
+	mutex_lock(&bridge_mutex);
+	list_del(&slot->node);
+	mutex_unlock(&bridge_mutex);
 	kfree(slot);
 	kfree(newfunc);
 
@@ -315,13 +353,17 @@ static void init_bridge_misc(struct acpiphp_bridge *bridge)
 	acpi_status status;
 
 	/* must be added to the list prior to calling register_slot */
+	mutex_lock(&bridge_mutex);
 	list_add(&bridge->list, &bridge_list);
+	mutex_unlock(&bridge_mutex);
 
 	/* register all slot objects under this bridge */
 	status = acpi_walk_namespace(ACPI_TYPE_DEVICE, bridge->handle, (u32)1,
 				     register_slot, NULL, bridge, NULL);
 	if (ACPI_FAILURE(status)) {
+		mutex_lock(&bridge_mutex);
 		list_del(&bridge->list);
+		mutex_unlock(&bridge_mutex);
 		return;
 	}
 
@@ -351,178 +393,46 @@ static struct acpiphp_func *acpiphp_bridge_handle_to_function(acpi_handle handle
 {
 	struct acpiphp_bridge *bridge;
 	struct acpiphp_slot *slot;
-	struct acpiphp_func *func;
+	struct acpiphp_func *func = NULL;
 
+	mutex_lock(&bridge_mutex);
 	list_for_each_entry(bridge, &bridge_list, list) {
-		for (slot = bridge->slots; slot; slot = slot->next) {
+		list_for_each_entry(slot, &bridge->slots, node) {
 			list_for_each_entry(func, &slot->funcs, sibling) {
-				if (func->handle == handle)
+				if (func->handle == handle) {
+					get_bridge(func->slot->bridge);
+					mutex_unlock(&bridge_mutex);
 					return func;
+				}
 			}
 		}
 	}
+	mutex_unlock(&bridge_mutex);
 
 	return NULL;
 }
 
 
-static inline void config_p2p_bridge_flags(struct acpiphp_bridge *bridge)
-{
-	acpi_handle dummy_handle;
-	struct acpiphp_func *func;
-
-	if (ACPI_SUCCESS(acpi_get_handle(bridge->handle,
-					"_EJ0", &dummy_handle))) {
-		bridge->flags |= BRIDGE_HAS_EJ0;
-
-		dbg("found ejectable p2p bridge\n");
-
-		/* make link between PCI bridge and PCI function */
-		func = acpiphp_bridge_handle_to_function(bridge->handle);
-		if (!func)
-			return;
-		bridge->func = func;
-		func->bridge = bridge;
-	}
-}
-
-
-/* allocate and initialize host bridge data structure */
-static void add_host_bridge(struct acpi_pci_root *root)
-{
-	struct acpiphp_bridge *bridge;
-	acpi_handle handle = root->device->handle;
-
-	bridge = kzalloc(sizeof(struct acpiphp_bridge), GFP_KERNEL);
-	if (bridge == NULL)
-		return;
-
-	bridge->handle = handle;
-
-	bridge->pci_bus = root->bus;
-
-	init_bridge_misc(bridge);
-}
-
-
-/* allocate and initialize PCI-to-PCI bridge data structure */
-static void add_p2p_bridge(acpi_handle *handle)
-{
-	struct acpiphp_bridge *bridge;
-
-	bridge = kzalloc(sizeof(struct acpiphp_bridge), GFP_KERNEL);
-	if (bridge == NULL) {
-		err("out of memory\n");
-		return;
-	}
-
-	bridge->handle = handle;
-	config_p2p_bridge_flags(bridge);
-
-	bridge->pci_dev = acpi_get_pci_dev(handle);
-	bridge->pci_bus = bridge->pci_dev->subordinate;
-	if (!bridge->pci_bus) {
-		err("This is not a PCI-to-PCI bridge!\n");
-		goto err;
-	}
-
-	/*
-	 * Grab a ref to the subordinate PCI bus in case the bus is
-	 * removed via PCI core logical hotplug. The ref pins the bus
-	 * (which we access during module unload).
-	 */
-	get_device(&bridge->pci_bus->dev);
-
-	init_bridge_misc(bridge);
-	return;
- err:
-	pci_dev_put(bridge->pci_dev);
-	kfree(bridge);
-	return;
-}
-
-
-/* callback routine to find P2P bridges */
-static acpi_status
-find_p2p_bridge(acpi_handle handle, u32 lvl, void *context, void **rv)
-{
-	acpi_status status;
-	struct pci_dev *dev;
-
-	dev = acpi_get_pci_dev(handle);
-	if (!dev || !dev->subordinate)
-		goto out;
-
-	/* check if this bridge has ejectable slots */
-	if ((detect_ejectable_slots(handle) > 0)) {
-		dbg("found PCI-to-PCI bridge at PCI %s\n", pci_name(dev));
-		add_p2p_bridge(handle);
-	}
-
-	/* search P2P bridges under this p2p bridge */
-	status = acpi_walk_namespace(ACPI_TYPE_DEVICE, handle, (u32)1,
-				     find_p2p_bridge, NULL, NULL, NULL);
-	if (ACPI_FAILURE(status))
-		warn("find_p2p_bridge failed (error code = 0x%x)\n", status);
-
- out:
-	pci_dev_put(dev);
-	return AE_OK;
-}
-
-
-/* find hot-pluggable slots, and then find P2P bridge */
-static int add_bridge(struct acpi_pci_root *root)
-{
-	acpi_status status;
-	unsigned long long tmp;
-	acpi_handle dummy_handle;
-	acpi_handle handle = root->device->handle;
-
-	/* if the bridge doesn't have _STA, we assume it is always there */
-	status = acpi_get_handle(handle, "_STA", &dummy_handle);
-	if (ACPI_SUCCESS(status)) {
-		status = acpi_evaluate_integer(handle, "_STA", NULL, &tmp);
-		if (ACPI_FAILURE(status)) {
-			dbg("%s: _STA evaluation failure\n", __func__);
-			return 0;
-		}
-		if ((tmp & ACPI_STA_FUNCTIONING) == 0)
-			/* don't register this object */
-			return 0;
-	}
-
-	/* check if this bridge has ejectable slots */
-	if (detect_ejectable_slots(handle) > 0) {
-		dbg("found PCI host-bus bridge with hot-pluggable slots\n");
-		add_host_bridge(root);
-	}
-
-	/* search P2P bridges under this host bridge */
-	status = acpi_walk_namespace(ACPI_TYPE_DEVICE, handle, (u32)1,
-				     find_p2p_bridge, NULL, NULL, NULL);
-
-	if (ACPI_FAILURE(status))
-		warn("find_p2p_bridge failed (error code = 0x%x)\n", status);
-
-	return 0;
-}
-
 static struct acpiphp_bridge *acpiphp_handle_to_bridge(acpi_handle handle)
 {
 	struct acpiphp_bridge *bridge;
 
+	mutex_lock(&bridge_mutex);
 	list_for_each_entry(bridge, &bridge_list, list)
-		if (bridge->handle == handle)
+		if (bridge->handle == handle) {
+			get_bridge(bridge);
+			mutex_unlock(&bridge_mutex);
 			return bridge;
+		}
+	mutex_unlock(&bridge_mutex);
 
 	return NULL;
 }
 
 static void cleanup_bridge(struct acpiphp_bridge *bridge)
 {
-	struct acpiphp_slot *slot, *next;
-	struct acpiphp_func *func, *tmp;
+	struct acpiphp_slot *slot;
+	struct acpiphp_func *func;
 	acpi_status status;
 	acpi_handle handle = bridge->handle;
 
@@ -543,10 +453,8 @@ static void cleanup_bridge(struct acpiphp_bridge *bridge)
 			err("failed to install interrupt notify handler\n");
 	}
 
-	slot = bridge->slots;
-	while (slot) {
-		next = slot->next;
-		list_for_each_entry_safe(func, tmp, &slot->funcs, sibling) {
+	list_for_each_entry(slot, &bridge->slots, node) {
+		list_for_each_entry(func, &slot->funcs, sibling) {
 			if (is_dock_device(func->handle)) {
 				unregister_hotplug_dock_device(func->handle);
 				unregister_dock_notifier(&func->nb);
@@ -558,63 +466,13 @@ static void cleanup_bridge(struct acpiphp_bridge *bridge)
 				if (ACPI_FAILURE(status))
 					err("failed to remove notify handler\n");
 			}
-			list_del(&func->sibling);
-			kfree(func);
 		}
 		acpiphp_unregister_hotplug_slot(slot);
-		list_del(&slot->funcs);
-		kfree(slot);
-		slot = next;
 	}
 
-	/*
-	 * Only P2P bridges have a pci_dev
-	 */
-	if (bridge->pci_dev)
-		put_device(&bridge->pci_bus->dev);
-
-	pci_dev_put(bridge->pci_dev);
+	mutex_lock(&bridge_mutex);
 	list_del(&bridge->list);
-	kfree(bridge);
-}
-
-static acpi_status
-cleanup_p2p_bridge(acpi_handle handle, u32 lvl, void *context, void **rv)
-{
-	struct acpiphp_bridge *bridge;
-
-	/* cleanup p2p bridges under this P2P bridge
-	   in a depth-first manner */
-	acpi_walk_namespace(ACPI_TYPE_DEVICE, handle, (u32)1,
-				cleanup_p2p_bridge, NULL, NULL, NULL);
-
-	bridge = acpiphp_handle_to_bridge(handle);
-	if (bridge)
-		cleanup_bridge(bridge);
-
-	return AE_OK;
-}
-
-static void remove_bridge(struct acpi_pci_root *root)
-{
-	struct acpiphp_bridge *bridge;
-	acpi_handle handle = root->device->handle;
-
-	/* cleanup p2p bridges under this host bridge
-	   in a depth-first manner */
-	acpi_walk_namespace(ACPI_TYPE_DEVICE, handle,
-				(u32)1, cleanup_p2p_bridge, NULL, NULL, NULL);
-
-	/*
-	 * On root bridges with hotplug slots directly underneath (ie,
-	 * no p2p bridge between), we call cleanup_bridge(). 
-	 *
-	 * The else clause cleans up root bridges that either had no
-	 * hotplug slots at all, or had a p2p bridge underneath.
-	 */
-	bridge = acpiphp_handle_to_bridge(handle);
-	if (bridge)
-		cleanup_bridge(bridge);
+	mutex_unlock(&bridge_mutex);
 }
 
 static int power_on_slot(struct acpiphp_slot *slot)
@@ -798,6 +656,7 @@ static void check_hotplug_bridge(struct acpiphp_slot *slot, struct pci_dev *dev)
 		}
 	}
 }
+
 /**
  * enable_device - enable, configure a slot
  * @slot: slot to be enabled
@@ -810,9 +669,7 @@ static int __ref enable_device(struct acpiphp_slot *slot)
 	struct pci_dev *dev;
 	struct pci_bus *bus = slot->bridge->pci_bus;
 	struct acpiphp_func *func;
-	int retval = 0;
 	int num, max, pass;
-	acpi_status status;
 
 	if (slot->flags & SLOT_ENABLED)
 		goto err_exit;
@@ -867,23 +724,11 @@ static int __ref enable_device(struct acpiphp_slot *slot)
 			slot->flags &= (~SLOT_ENABLED);
 			continue;
 		}
-
-		if (dev->hdr_type != PCI_HEADER_TYPE_BRIDGE &&
-		    dev->hdr_type != PCI_HEADER_TYPE_CARDBUS) {
-			pci_dev_put(dev);
-			continue;
-		}
-
-		status = find_p2p_bridge(func->handle, (u32)1, bus, NULL);
-		if (ACPI_FAILURE(status))
-			warn("find_p2p_bridge failed (error code = 0x%x)\n",
-				status);
-		pci_dev_put(dev);
 	}
 
 
  err_exit:
-	return retval;
+	return 0;
 }
 
 /* return first device in slot, acquiring a reference on it */
@@ -912,23 +757,6 @@ static int disable_device(struct acpiphp_slot *slot)
 {
 	struct acpiphp_func *func;
 	struct pci_dev *pdev;
-	struct pci_bus *bus = slot->bridge->pci_bus;
-
-	/* The slot will be enabled when func 0 is added, so check
-	   func 0 before disable the slot. */
-	pdev = pci_get_slot(bus, PCI_DEVFN(slot->device, 0));
-	if (!pdev)
-		goto err_exit;
-	pci_dev_put(pdev);
-
-	list_for_each_entry(func, &slot->funcs, sibling) {
-		if (func->bridge) {
-			/* cleanup p2p bridges under this P2P bridge */
-			cleanup_p2p_bridge(func->bridge->handle,
-						(u32)1, NULL, NULL);
-			func->bridge = NULL;
-		}
-	}
 
 	/*
 	 * enable_device() enumerates all functions in this device via
@@ -947,7 +775,6 @@ static int disable_device(struct acpiphp_slot *slot)
 
 	slot->flags &= (~SLOT_ENABLED);
 
-err_exit:
 	return 0;
 }
 
@@ -1037,7 +864,7 @@ static int acpiphp_check_bridge(struct acpiphp_bridge *bridge)
 
 	enabled = disabled = 0;
 
-	for (slot = bridge->slots; slot; slot = slot->next) {
+	list_for_each_entry(slot, &bridge->slots, node) {
 		unsigned int status = get_slot_status(slot);
 		if (slot->flags & SLOT_ENABLED) {
 			if (status == ACPI_STA_ALL)
@@ -1082,11 +909,11 @@ static void acpiphp_set_hpp_values(struct pci_bus *bus)
  */
 static void acpiphp_sanitize_bus(struct pci_bus *bus)
 {
-	struct pci_dev *dev;
+	struct pci_dev *dev, *tmp;
 	int i;
 	unsigned long type_mask = IORESOURCE_IO | IORESOURCE_MEM;
 
-	list_for_each_entry(dev, &bus->devices, bus_list) {
+	list_for_each_entry_safe(dev, tmp, &bus->devices, bus_list) {
 		for (i=0; i<PCI_BRIDGE_RESOURCES; i++) {
 			struct resource *res = &dev->resource[i];
 			if ((res->flags & type_mask) && !res->start &&
@@ -1118,6 +945,7 @@ check_sub_bridges(acpi_handle handle, u32 lvl, void *context, void **rv)
 		dbg("%s: re-enumerating slots under %s\n",
 			__func__, objname);
 		acpiphp_check_bridge(bridge);
+		put_bridge(bridge);
 	}
 	return AE_OK ;
 }
@@ -1195,6 +1023,7 @@ static void _handle_hotplug_event_bridge(struct work_struct *work)
 
 	acpi_scan_lock_release();
 	kfree(hp_work); /* allocated in handle_hotplug_event_bridge */
+	put_bridge(bridge);
 }
 
 /**
@@ -1208,6 +1037,8 @@ static void _handle_hotplug_event_bridge(struct work_struct *work)
 static void handle_hotplug_event_bridge(acpi_handle handle, u32 type,
 					void *context)
 {
+	struct acpiphp_bridge *bridge = context;
+
 	/*
 	 * Currently the code adds all hotplug events to the kacpid_wq
 	 * queue when it should add hotplug events to the kacpi_hotplug_wq.
@@ -1216,6 +1047,7 @@ static void handle_hotplug_event_bridge(acpi_handle handle, u32 type,
 	 * For now just re-add this work to the kacpi_hotplug_wq so we
 	 * don't deadlock on hotplug actions.
 	 */
+	get_bridge(bridge);
 	alloc_acpi_hp_work(handle, type, context, _handle_hotplug_event_bridge);
 }
 
@@ -1270,6 +1102,7 @@ static void _handle_hotplug_event_func(struct work_struct *work)
 
 	acpi_scan_lock_release();
 	kfree(hp_work); /* allocated in handle_hotplug_event_func */
+	put_bridge(func->slot->bridge);
 }
 
 /**
@@ -1283,6 +1116,8 @@ static void _handle_hotplug_event_func(struct work_struct *work)
 static void handle_hotplug_event_func(acpi_handle handle, u32 type,
 				      void *context)
 {
+	struct acpiphp_func *func = context;
+
 	/*
 	 * Currently the code adds all hotplug events to the kacpid_wq
 	 * queue when it should add hotplug events to the kacpi_hotplug_wq.
@@ -1291,33 +1126,69 @@ static void handle_hotplug_event_func(acpi_handle handle, u32 type,
 	 * For now just re-add this work to the kacpi_hotplug_wq so we
 	 * don't deadlock on hotplug actions.
 	 */
+	get_bridge(func->slot->bridge);
 	alloc_acpi_hp_work(handle, type, context, _handle_hotplug_event_func);
 }
 
-static struct acpi_pci_driver acpi_pci_hp_driver = {
-	.add =		add_bridge,
-	.remove =	remove_bridge,
-};
-
-/**
- * acpiphp_glue_init - initializes all PCI hotplug - ACPI glue data structures
+/*
+ * Create hotplug slots for the PCI bus.
+ * It should always return 0 to avoid skipping following notifiers.
  */
-int __init acpiphp_glue_init(void)
+void acpiphp_enumerate_slots(struct pci_bus *bus, acpi_handle handle)
 {
-	acpi_pci_register_driver(&acpi_pci_hp_driver);
+	acpi_handle dummy_handle;
+	struct acpiphp_bridge *bridge;
 
-	return 0;
-}
+	if (acpiphp_disabled)
+		return;
 
+	if (detect_ejectable_slots(handle) <= 0)
+		return;
 
-/**
- * acpiphp_glue_exit - terminates all PCI hotplug - ACPI glue data structures
- *
- * This function frees all data allocated in acpiphp_glue_init().
- */
-void  acpiphp_glue_exit(void)
+	bridge = kzalloc(sizeof(struct acpiphp_bridge), GFP_KERNEL);
+	if (bridge == NULL) {
+		err("out of memory\n");
+		return;
+	}
+
+	INIT_LIST_HEAD(&bridge->slots);
+	kref_init(&bridge->ref);
+	bridge->handle = handle;
+	bridge->pci_dev = pci_dev_get(bus->self);
+	bridge->pci_bus = bus;
+
+	/*
+	 * Grab a ref to the subordinate PCI bus in case the bus is
+	 * removed via PCI core logical hotplug. The ref pins the bus
+	 * (which we access during module unload).
+	 */
+	get_device(&bus->dev);
+
+	if (!pci_is_root_bus(bridge->pci_bus) &&
+	    ACPI_SUCCESS(acpi_get_handle(bridge->handle,
+					"_EJ0", &dummy_handle))) {
+		dbg("found ejectable p2p bridge\n");
+		bridge->flags |= BRIDGE_HAS_EJ0;
+		bridge->func = acpiphp_bridge_handle_to_function(handle);
+	}
+
+	init_bridge_misc(bridge);
+}
+
+/* Destroy hotplug slots associated with the PCI bus */
+void acpiphp_remove_slots(struct pci_bus *bus)
 {
-	acpi_pci_unregister_driver(&acpi_pci_hp_driver);
+	struct acpiphp_bridge *bridge, *tmp;
+
+	if (acpiphp_disabled)
+		return;
+
+	list_for_each_entry_safe(bridge, tmp, &bridge_list, list)
+		if (bridge->pci_bus == bus) {
+			cleanup_bridge(bridge);
+			put_bridge(bridge);
+			break;
+		}
 }
 
 /**
@@ -1396,7 +1267,7 @@ u8 acpiphp_get_latch_status(struct acpiphp_slot *slot)
 
 	sta = get_slot_status(slot);
 
-	return (sta & ACPI_STA_SHOW_IN_UI) ? 0 : 1;
+	return (sta & ACPI_STA_DEVICE_UI) ? 0 : 1;
 }
 
 
diff --git a/drivers/pci/hotplug/cpci_hotplug.h b/drivers/pci/hotplug/cpci_hotplug.h
index 9fff878cf02..1356211431d 100644
--- a/drivers/pci/hotplug/cpci_hotplug.h
+++ b/drivers/pci/hotplug/cpci_hotplug.h
@@ -75,28 +75,36 @@ static inline const char *slot_name(struct slot *slot)
 	return hotplug_slot_name(slot->hotplug_slot);
 }
 
-extern int cpci_hp_register_controller(struct cpci_hp_controller *controller);
-extern int cpci_hp_unregister_controller(struct cpci_hp_controller *controller);
-extern int cpci_hp_register_bus(struct pci_bus *bus, u8 first, u8 last);
-extern int cpci_hp_unregister_bus(struct pci_bus *bus);
-extern int cpci_hp_start(void);
-extern int cpci_hp_stop(void);
+int cpci_hp_register_controller(struct cpci_hp_controller *controller);
+int cpci_hp_unregister_controller(struct cpci_hp_controller *controller);
+int cpci_hp_register_bus(struct pci_bus *bus, u8 first, u8 last);
+int cpci_hp_unregister_bus(struct pci_bus *bus);
+int cpci_hp_start(void);
+int cpci_hp_stop(void);
 
 /*
  * Internal function prototypes, these functions should not be used by
  * board/chassis drivers.
  */
-extern u8 cpci_get_attention_status(struct slot *slot);
-extern u8 cpci_get_latch_status(struct slot *slot);
-extern u8 cpci_get_adapter_status(struct slot *slot);
-extern u16 cpci_get_hs_csr(struct slot * slot);
-extern int cpci_set_attention_status(struct slot *slot, int status);
-extern int cpci_check_and_clear_ins(struct slot * slot);
-extern int cpci_check_ext(struct slot * slot);
-extern int cpci_clear_ext(struct slot * slot);
-extern int cpci_led_on(struct slot * slot);
-extern int cpci_led_off(struct slot * slot);
-extern int cpci_configure_slot(struct slot *slot);
-extern int cpci_unconfigure_slot(struct slot *slot);
+u8 cpci_get_attention_status(struct slot *slot);
+u8 cpci_get_latch_status(struct slot *slot);
+u8 cpci_get_adapter_status(struct slot *slot);
+u16 cpci_get_hs_csr(struct slot * slot);
+int cpci_set_attention_status(struct slot *slot, int status);
+int cpci_check_and_clear_ins(struct slot * slot);
+int cpci_check_ext(struct slot * slot);
+int cpci_clear_ext(struct slot * slot);
+int cpci_led_on(struct slot * slot);
+int cpci_led_off(struct slot * slot);
+int cpci_configure_slot(struct slot *slot);
+int cpci_unconfigure_slot(struct slot *slot);
+
+#ifdef CONFIG_HOTPLUG_PCI_CPCI
+int cpci_hotplug_init(int debug);
+void cpci_hotplug_exit(void);
+#else
+static inline int cpci_hotplug_init(int debug) { return 0; }
+static inline void cpci_hotplug_exit(void) { }
+#endif
 
 #endif	/* _CPCI_HOTPLUG_H */
diff --git a/drivers/pci/hotplug/cpqphp.h b/drivers/pci/hotplug/cpqphp.h
index d8ffc736680..516b87738b6 100644
--- a/drivers/pci/hotplug/cpqphp.h
+++ b/drivers/pci/hotplug/cpqphp.h
@@ -404,50 +404,44 @@ struct resource_lists {
 
 
 /* debugfs functions for the hotplug controller info */
-extern void cpqhp_initialize_debugfs(void);
-extern void cpqhp_shutdown_debugfs(void);
-extern void cpqhp_create_debugfs_files(struct controller *ctrl);
-extern void cpqhp_remove_debugfs_files(struct controller *ctrl);
+void cpqhp_initialize_debugfs(void);
+void cpqhp_shutdown_debugfs(void);
+void cpqhp_create_debugfs_files(struct controller *ctrl);
+void cpqhp_remove_debugfs_files(struct controller *ctrl);
 
 /* controller functions */
-extern void cpqhp_pushbutton_thread(unsigned long event_pointer);
-extern irqreturn_t cpqhp_ctrl_intr(int IRQ, void *data);
-extern int cpqhp_find_available_resources(struct controller *ctrl,
-					  void __iomem *rom_start);
-extern int cpqhp_event_start_thread(void);
-extern void cpqhp_event_stop_thread(void);
-extern struct pci_func *cpqhp_slot_create(unsigned char busnumber);
-extern struct pci_func *cpqhp_slot_find(unsigned char bus, unsigned char device,
-					unsigned char index);
-extern int cpqhp_process_SI(struct controller *ctrl, struct pci_func *func);
-extern int cpqhp_process_SS(struct controller *ctrl, struct pci_func *func);
-extern int cpqhp_hardware_test(struct controller *ctrl, int test_num);
+void cpqhp_pushbutton_thread(unsigned long event_pointer);
+irqreturn_t cpqhp_ctrl_intr(int IRQ, void *data);
+int cpqhp_find_available_resources(struct controller *ctrl,
+				   void __iomem *rom_start);
+int cpqhp_event_start_thread(void);
+void cpqhp_event_stop_thread(void);
+struct pci_func *cpqhp_slot_create(unsigned char busnumber);
+struct pci_func *cpqhp_slot_find(unsigned char bus, unsigned char device,
+				 unsigned char index);
+int cpqhp_process_SI(struct controller *ctrl, struct pci_func *func);
+int cpqhp_process_SS(struct controller *ctrl, struct pci_func *func);
+int cpqhp_hardware_test(struct controller *ctrl, int test_num);
 
 /* resource functions */
-extern int	cpqhp_resource_sort_and_combine	(struct pci_resource **head);
+int	cpqhp_resource_sort_and_combine	(struct pci_resource **head);
 
 /* pci functions */
-extern int cpqhp_set_irq(u8 bus_num, u8 dev_num, u8 int_pin, u8 irq_num);
-extern int cpqhp_get_bus_dev(struct controller *ctrl, u8 *bus_num, u8 *dev_num,
-			     u8 slot);
-extern int cpqhp_save_config(struct controller *ctrl, int busnumber,
-			     int is_hot_plug);
-extern int cpqhp_save_base_addr_length(struct controller *ctrl,
-				       struct pci_func *func);
-extern int cpqhp_save_used_resources(struct controller *ctrl,
-				     struct pci_func *func);
-extern int cpqhp_configure_board(struct controller *ctrl,
-				 struct pci_func *func);
-extern int cpqhp_save_slot_config(struct controller *ctrl,
-				  struct pci_func *new_slot);
-extern int cpqhp_valid_replace(struct controller *ctrl, struct pci_func *func);
-extern void cpqhp_destroy_board_resources(struct pci_func *func);
-extern int cpqhp_return_board_resources	(struct pci_func *func,
-					 struct resource_lists *resources);
-extern void cpqhp_destroy_resource_list(struct resource_lists *resources);
-extern int cpqhp_configure_device(struct controller *ctrl,
-				  struct pci_func *func);
-extern int cpqhp_unconfigure_device(struct pci_func *func);
+int cpqhp_set_irq(u8 bus_num, u8 dev_num, u8 int_pin, u8 irq_num);
+int cpqhp_get_bus_dev(struct controller *ctrl, u8 *bus_num, u8 *dev_num,
+		      u8 slot);
+int cpqhp_save_config(struct controller *ctrl, int busnumber, int is_hot_plug);
+int cpqhp_save_base_addr_length(struct controller *ctrl, struct pci_func *func);
+int cpqhp_save_used_resources(struct controller *ctrl, struct pci_func *func);
+int cpqhp_configure_board(struct controller *ctrl, struct pci_func *func);
+int cpqhp_save_slot_config(struct controller *ctrl, struct pci_func *new_slot);
+int cpqhp_valid_replace(struct controller *ctrl, struct pci_func *func);
+void cpqhp_destroy_board_resources(struct pci_func *func);
+int cpqhp_return_board_resources(struct pci_func *func,
+				 struct resource_lists *resources);
+void cpqhp_destroy_resource_list(struct resource_lists *resources);
+int cpqhp_configure_device(struct controller *ctrl, struct pci_func *func);
+int cpqhp_unconfigure_device(struct pci_func *func);
 
 /* Global variables */
 extern int cpqhp_debug;
diff --git a/drivers/pci/hotplug/cpqphp_nvram.h b/drivers/pci/hotplug/cpqphp_nvram.h
index e89c0702119..34e4e54fcf1 100644
--- a/drivers/pci/hotplug/cpqphp_nvram.h
+++ b/drivers/pci/hotplug/cpqphp_nvram.h
@@ -30,26 +30,26 @@
 
 #ifndef CONFIG_HOTPLUG_PCI_COMPAQ_NVRAM
 
-static inline void compaq_nvram_init (void __iomem *rom_start)
+static inline void compaq_nvram_init(void __iomem *rom_start)
 {
 	return;
 }
 
-static inline int compaq_nvram_load (void __iomem *rom_start, struct controller *ctrl)
+static inline int compaq_nvram_load(void __iomem *rom_start, struct controller *ctrl)
 {
 	return 0;
 }
 
-static inline int compaq_nvram_store (void __iomem *rom_start)
+static inline int compaq_nvram_store(void __iomem *rom_start)
 {
 	return 0;
 }
 
 #else
 
-extern void compaq_nvram_init	(void __iomem *rom_start);
-extern int compaq_nvram_load	(void __iomem *rom_start, struct controller *ctrl);
-extern int compaq_nvram_store	(void __iomem *rom_start);
+void compaq_nvram_init(void __iomem *rom_start);
+int compaq_nvram_load(void __iomem *rom_start, struct controller *ctrl);
+int compaq_nvram_store(void __iomem *rom_start);
 
 #endif
 
diff --git a/drivers/pci/hotplug/ibmphp.h b/drivers/pci/hotplug/ibmphp.h
index a8d391a4957..8c5b25871d0 100644
--- a/drivers/pci/hotplug/ibmphp.h
+++ b/drivers/pci/hotplug/ibmphp.h
@@ -275,17 +275,17 @@ extern struct list_head ibmphp_slot_head;
 * FUNCTION PROTOTYPES                                      *
 ***********************************************************/
 
-extern void ibmphp_free_ebda_hpc_queue (void);
-extern int ibmphp_access_ebda (void);
-extern struct slot *ibmphp_get_slot_from_physical_num (u8);
-extern int ibmphp_get_total_hp_slots (void);
-extern void ibmphp_free_ibm_slot (struct slot *);
-extern void ibmphp_free_bus_info_queue (void);
-extern void ibmphp_free_ebda_pci_rsrc_queue (void);
-extern struct bus_info *ibmphp_find_same_bus_num (u32);
-extern int ibmphp_get_bus_index (u8);
-extern u16 ibmphp_get_total_controllers (void);
-extern int ibmphp_register_pci (void);
+void ibmphp_free_ebda_hpc_queue(void);
+int ibmphp_access_ebda(void);
+struct slot *ibmphp_get_slot_from_physical_num(u8);
+int ibmphp_get_total_hp_slots(void);
+void ibmphp_free_ibm_slot(struct slot *);
+void ibmphp_free_bus_info_queue(void);
+void ibmphp_free_ebda_pci_rsrc_queue(void);
+struct bus_info *ibmphp_find_same_bus_num(u32);
+int ibmphp_get_bus_index(u8);
+u16 ibmphp_get_total_controllers(void);
+int ibmphp_register_pci(void);
 
 /* passed parameters */
 #define MEM		0
@@ -381,24 +381,24 @@ struct res_needed {
 
 /* functions */
 
-extern int ibmphp_rsrc_init (void);
-extern int ibmphp_add_resource (struct resource_node *);
-extern int ibmphp_remove_resource (struct resource_node *);
-extern int ibmphp_find_resource (struct bus_node *, u32, struct resource_node **, int);
-extern int ibmphp_check_resource (struct resource_node *, u8);
-extern int ibmphp_remove_bus (struct bus_node *, u8);
-extern void ibmphp_free_resources (void);
-extern int ibmphp_add_pfmem_from_mem (struct resource_node *);
-extern struct bus_node *ibmphp_find_res_bus (u8);
-extern void ibmphp_print_test (void);	/* for debugging purposes */
+int ibmphp_rsrc_init(void);
+int ibmphp_add_resource(struct resource_node *);
+int ibmphp_remove_resource(struct resource_node *);
+int ibmphp_find_resource(struct bus_node *, u32, struct resource_node **, int);
+int ibmphp_check_resource(struct resource_node *, u8);
+int ibmphp_remove_bus(struct bus_node *, u8);
+void ibmphp_free_resources(void);
+int ibmphp_add_pfmem_from_mem(struct resource_node *);
+struct bus_node *ibmphp_find_res_bus(u8);
+void ibmphp_print_test(void);	/* for debugging purposes */
 
-extern void ibmphp_hpc_initvars (void);
-extern int ibmphp_hpc_readslot (struct slot *, u8, u8 *);
-extern int ibmphp_hpc_writeslot (struct slot *, u8);
-extern void ibmphp_lock_operations (void);
-extern void ibmphp_unlock_operations (void);
-extern int ibmphp_hpc_start_poll_thread (void);
-extern void ibmphp_hpc_stop_poll_thread (void);
+void ibmphp_hpc_initvars(void);
+int ibmphp_hpc_readslot(struct slot *, u8, u8 *);
+int ibmphp_hpc_writeslot(struct slot *, u8);
+void ibmphp_lock_operations(void);
+void ibmphp_unlock_operations(void);
+int ibmphp_hpc_start_poll_thread(void);
+void ibmphp_hpc_stop_poll_thread(void);
 
 //----------------------------------------------------------------------------
 
@@ -749,11 +749,11 @@ struct controller {
 
 /* Functions */
 
-extern int ibmphp_init_devno (struct slot **);	/* This function is called from EBDA, so we need it not be static */
-extern int ibmphp_do_disable_slot (struct slot *slot_cur);
-extern int ibmphp_update_slot_info (struct slot *);	/* This function is called from HPC, so we need it to not be be static */
-extern int ibmphp_configure_card (struct pci_func *, u8);
-extern int ibmphp_unconfigure_card (struct slot **, int);
+int ibmphp_init_devno(struct slot **);	/* This function is called from EBDA, so we need it not be static */
+int ibmphp_do_disable_slot(struct slot *slot_cur);
+int ibmphp_update_slot_info(struct slot *);	/* This function is called from HPC, so we need it to not be be static */
+int ibmphp_configure_card(struct pci_func *, u8);
+int ibmphp_unconfigure_card(struct slot **, int);
 extern struct hotplug_slot_ops ibmphp_hotplug_slot_ops;
 
 #endif				//__IBMPHP_H
diff --git a/drivers/pci/hotplug/pci_hotplug_core.c b/drivers/pci/hotplug/pci_hotplug_core.c
index 202f4a969eb..ec20f74c898 100644
--- a/drivers/pci/hotplug/pci_hotplug_core.c
+++ b/drivers/pci/hotplug/pci_hotplug_core.c
@@ -41,6 +41,7 @@
 #include <linux/pci_hotplug.h>
 #include <asm/uaccess.h>
 #include "../pci.h"
+#include "cpci_hotplug.h"
 
 #define MY_NAME	"pci_hotplug"
 
@@ -63,14 +64,6 @@ static bool debug;
 static LIST_HEAD(pci_hotplug_slot_list);
 static DEFINE_MUTEX(pci_hp_mutex);
 
-#ifdef CONFIG_HOTPLUG_PCI_CPCI
-extern int cpci_hotplug_init(int debug);
-extern void cpci_hotplug_exit(void);
-#else
-static inline int cpci_hotplug_init(int debug) { return 0; }
-static inline void cpci_hotplug_exit(void) { }
-#endif
-
 /* Weee, fun with macros... */
 #define GET_STATUS(name,type)	\
 static int get_##name (struct hotplug_slot *slot, type *value)		\
@@ -524,13 +517,11 @@ int pci_hp_deregister(struct hotplug_slot *hotplug)
  *
  * Returns 0 if successful, anything else for an error.
  */
-int __must_check pci_hp_change_slot_info(struct hotplug_slot *hotplug,
-					 struct hotplug_slot_info *info)
+int pci_hp_change_slot_info(struct hotplug_slot *hotplug,
+			    struct hotplug_slot_info *info)
 {
-	struct pci_slot *slot;
 	if (!hotplug || !info)
 		return -ENODEV;
-	slot = hotplug->pci_slot;
 
 	memcpy(hotplug->info, info, sizeof(struct hotplug_slot_info));
 
diff --git a/drivers/pci/hotplug/pciehp.h b/drivers/pci/hotplug/pciehp.h
index 2c113de9432..7fb326983ed 100644
--- a/drivers/pci/hotplug/pciehp.h
+++ b/drivers/pci/hotplug/pciehp.h
@@ -127,15 +127,15 @@ struct controller {
 #define NO_CMD_CMPL(ctrl)	((ctrl)->slot_cap & PCI_EXP_SLTCAP_NCCS)
 #define PSN(ctrl)		((ctrl)->slot_cap >> 19)
 
-extern int pciehp_sysfs_enable_slot(struct slot *slot);
-extern int pciehp_sysfs_disable_slot(struct slot *slot);
-extern u8 pciehp_handle_attention_button(struct slot *p_slot);
-extern u8 pciehp_handle_switch_change(struct slot *p_slot);
-extern u8 pciehp_handle_presence_change(struct slot *p_slot);
-extern u8 pciehp_handle_power_fault(struct slot *p_slot);
-extern int pciehp_configure_device(struct slot *p_slot);
-extern int pciehp_unconfigure_device(struct slot *p_slot);
-extern void pciehp_queue_pushbutton_work(struct work_struct *work);
+int pciehp_sysfs_enable_slot(struct slot *slot);
+int pciehp_sysfs_disable_slot(struct slot *slot);
+u8 pciehp_handle_attention_button(struct slot *p_slot);
+u8 pciehp_handle_switch_change(struct slot *p_slot);
+u8 pciehp_handle_presence_change(struct slot *p_slot);
+u8 pciehp_handle_power_fault(struct slot *p_slot);
+int pciehp_configure_device(struct slot *p_slot);
+int pciehp_unconfigure_device(struct slot *p_slot);
+void pciehp_queue_pushbutton_work(struct work_struct *work);
 struct controller *pcie_init(struct pcie_device *dev);
 int pcie_init_notification(struct controller *ctrl);
 int pciehp_enable_slot(struct slot *p_slot);
@@ -166,8 +166,8 @@ static inline const char *slot_name(struct slot *slot)
 #include <acpi/acpi_bus.h>
 #include <linux/pci-acpi.h>
 
-extern void __init pciehp_acpi_slot_detection_init(void);
-extern int pciehp_acpi_slot_detection_check(struct pci_dev *dev);
+void __init pciehp_acpi_slot_detection_init(void);
+int pciehp_acpi_slot_detection_check(struct pci_dev *dev);
 
 static inline void pciehp_firmware_init(void)
 {
diff --git a/drivers/pci/hotplug/pciehp_acpi.c b/drivers/pci/hotplug/pciehp_acpi.c
index 24d709b7388..ead7c534095 100644
--- a/drivers/pci/hotplug/pciehp_acpi.c
+++ b/drivers/pci/hotplug/pciehp_acpi.c
@@ -90,7 +90,7 @@ static int __init dummy_probe(struct pcie_device *dev)
 	slot = kzalloc(sizeof(*slot), GFP_KERNEL);
 	if (!slot)
 		return -ENOMEM;
-	slot->number = slot_cap >> 19;
+	slot->number = (slot_cap & PCI_EXP_SLTCAP_PSN) >> 19;
 	list_for_each_entry(tmp, &dummy_slots, list) {
 		if (tmp->number == slot->number)
 			dup_slot_id++;
diff --git a/drivers/pci/hotplug/rpadlpar.h b/drivers/pci/hotplug/rpadlpar.h
index 4a0a59b82ea..81df93931ad 100644
--- a/drivers/pci/hotplug/rpadlpar.h
+++ b/drivers/pci/hotplug/rpadlpar.h
@@ -15,10 +15,10 @@
 #ifndef _RPADLPAR_IO_H_
 #define _RPADLPAR_IO_H_
 
-extern int dlpar_sysfs_init(void);
-extern void dlpar_sysfs_exit(void);
+int dlpar_sysfs_init(void);
+void dlpar_sysfs_exit(void);
 
-extern int dlpar_add_slot(char *drc_name);
-extern int dlpar_remove_slot(char *drc_name);
+int dlpar_add_slot(char *drc_name);
+int dlpar_remove_slot(char *drc_name);
 
 #endif
diff --git a/drivers/pci/hotplug/rpaphp.h b/drivers/pci/hotplug/rpaphp.h
index df5677440a0..3135856e5e1 100644
--- a/drivers/pci/hotplug/rpaphp.h
+++ b/drivers/pci/hotplug/rpaphp.h
@@ -86,18 +86,18 @@ extern struct list_head rpaphp_slot_head;
 /* function prototypes */
 
 /* rpaphp_pci.c */
-extern int rpaphp_enable_slot(struct slot *slot);
-extern int rpaphp_get_sensor_state(struct slot *slot, int *state);
+int rpaphp_enable_slot(struct slot *slot);
+int rpaphp_get_sensor_state(struct slot *slot, int *state);
 
 /* rpaphp_core.c */
-extern int rpaphp_add_slot(struct device_node *dn);
-extern int rpaphp_get_drc_props(struct device_node *dn, int *drc_index,
+int rpaphp_add_slot(struct device_node *dn);
+int rpaphp_get_drc_props(struct device_node *dn, int *drc_index,
 		char **drc_name, char **drc_type, int *drc_power_domain);
 
 /* rpaphp_slot.c */
-extern void dealloc_slot_struct(struct slot *slot);
-extern struct slot *alloc_slot_struct(struct device_node *dn, int drc_index, char *drc_name, int power_domain);
-extern int rpaphp_register_slot(struct slot *slot);
-extern int rpaphp_deregister_slot(struct slot *slot);
+void dealloc_slot_struct(struct slot *slot);
+struct slot *alloc_slot_struct(struct device_node *dn, int drc_index, char *drc_name, int power_domain);
+int rpaphp_register_slot(struct slot *slot);
+int rpaphp_deregister_slot(struct slot *slot);
 	
 #endif				/* _PPC64PHP_H */
diff --git a/drivers/pci/hotplug/s390_pci_hpc.c b/drivers/pci/hotplug/s390_pci_hpc.c
index 7db249a2501..46a7b738f61 100644
--- a/drivers/pci/hotplug/s390_pci_hpc.c
+++ b/drivers/pci/hotplug/s390_pci_hpc.c
@@ -16,6 +16,7 @@
 #include <linux/pci.h>
 #include <linux/pci_hotplug.h>
 #include <linux/init.h>
+#include <asm/pci_debug.h>
 #include <asm/sclp.h>
 
 #define SLOT_NAME_SIZE	10
@@ -49,6 +50,7 @@ static int enable_slot(struct hotplug_slot *hotplug_slot)
 		return -EIO;
 
 	rc = sclp_pci_configure(slot->zdev->fid);
+	zpci_dbg(3, "conf fid:%x, rc:%d\n", slot->zdev->fid, rc);
 	if (!rc) {
 		slot->zdev->state = ZPCI_FN_STATE_CONFIGURED;
 		/* automatically scan the device after is was configured */
@@ -66,16 +68,16 @@ static int disable_slot(struct hotplug_slot *hotplug_slot)
 	if (!zpci_fn_configured(slot->zdev->state))
 		return -EIO;
 
+	rc = zpci_disable_device(slot->zdev);
+	if (rc)
+		return rc;
 	/* TODO: we rely on the user to unbind/remove the device, is that plausible
 	 *	 or do we need to trigger that here?
 	 */
 	rc = sclp_pci_deconfigure(slot->zdev->fid);
-	if (!rc) {
-		/* Fixme: better call List-PCI to find the disabled FH
-		   for the FID since the FH should be opaque... */
-		slot->zdev->fh &= 0x7fffffff;
+	zpci_dbg(3, "deconf fid:%x, rc:%d\n", slot->zdev->fid, rc);
+	if (!rc)
 		slot->zdev->state = ZPCI_FN_STATE_STANDBY;
-	}
 	return rc;
 }
 
diff --git a/drivers/pci/hotplug/shpchp.h b/drivers/pci/hotplug/shpchp.h
index b849f995075..e260f207a90 100644
--- a/drivers/pci/hotplug/shpchp.h
+++ b/drivers/pci/hotplug/shpchp.h
@@ -168,19 +168,19 @@ struct controller {
 #define WRONG_BUS_FREQUENCY		0x0000000D
 #define POWER_FAILURE			0x0000000E
 
-extern int __must_check shpchp_create_ctrl_files(struct controller *ctrl);
-extern void shpchp_remove_ctrl_files(struct controller *ctrl);
-extern int shpchp_sysfs_enable_slot(struct slot *slot);
-extern int shpchp_sysfs_disable_slot(struct slot *slot);
-extern u8 shpchp_handle_attention_button(u8 hp_slot, struct controller *ctrl);
-extern u8 shpchp_handle_switch_change(u8 hp_slot, struct controller *ctrl);
-extern u8 shpchp_handle_presence_change(u8 hp_slot, struct controller *ctrl);
-extern u8 shpchp_handle_power_fault(u8 hp_slot, struct controller *ctrl);
-extern int shpchp_configure_device(struct slot *p_slot);
-extern int shpchp_unconfigure_device(struct slot *p_slot);
-extern void cleanup_slots(struct controller *ctrl);
-extern void shpchp_queue_pushbutton_work(struct work_struct *work);
-extern int shpc_init( struct controller *ctrl, struct pci_dev *pdev);
+int __must_check shpchp_create_ctrl_files(struct controller *ctrl);
+void shpchp_remove_ctrl_files(struct controller *ctrl);
+int shpchp_sysfs_enable_slot(struct slot *slot);
+int shpchp_sysfs_disable_slot(struct slot *slot);
+u8 shpchp_handle_attention_button(u8 hp_slot, struct controller *ctrl);
+u8 shpchp_handle_switch_change(u8 hp_slot, struct controller *ctrl);
+u8 shpchp_handle_presence_change(u8 hp_slot, struct controller *ctrl);
+u8 shpchp_handle_power_fault(u8 hp_slot, struct controller *ctrl);
+int shpchp_configure_device(struct slot *p_slot);
+int shpchp_unconfigure_device(struct slot *p_slot);
+void cleanup_slots(struct controller *ctrl);
+void shpchp_queue_pushbutton_work(struct work_struct *work);
+int shpc_init( struct controller *ctrl, struct pci_dev *pdev);
 
 static inline const char *slot_name(struct slot *slot)
 {
diff --git a/drivers/pci/hotplug/shpchp_sysfs.c b/drivers/pci/hotplug/shpchp_sysfs.c
index eeb23ceae4a..e8c31fe2056 100644
--- a/drivers/pci/hotplug/shpchp_sysfs.c
+++ b/drivers/pci/hotplug/shpchp_sysfs.c
@@ -85,7 +85,7 @@ static ssize_t show_ctrl (struct device *dev, struct device_attribute *attr, cha
 }
 static DEVICE_ATTR (ctrl, S_IRUGO, show_ctrl, NULL);
 
-int __must_check shpchp_create_ctrl_files (struct controller *ctrl)
+int shpchp_create_ctrl_files (struct controller *ctrl)
 {
 	return device_create_file (&ctrl->pci_dev->dev, &dev_attr_ctrl);
 }
diff --git a/drivers/pci/msi.c b/drivers/pci/msi.c
index 00cc78c7aa0..d40bed72676 100644
--- a/drivers/pci/msi.c
+++ b/drivers/pci/msi.c
@@ -22,10 +22,12 @@
 #include <linux/slab.h>
 
 #include "pci.h"
-#include "msi.h"
 
 static int pci_msi_enable = 1;
 
+#define msix_table_size(flags)	((flags & PCI_MSIX_FLAGS_QSIZE) + 1)
+
+
 /* Arch hooks */
 
 #ifndef arch_msi_check_device
@@ -111,32 +113,26 @@ void default_restore_msi_irqs(struct pci_dev *dev, int irq)
 }
 #endif
 
-static void msi_set_enable(struct pci_dev *dev, int pos, int enable)
+static void msi_set_enable(struct pci_dev *dev, int enable)
 {
 	u16 control;
 
-	BUG_ON(!pos);
-
-	pci_read_config_word(dev, pos + PCI_MSI_FLAGS, &control);
+	pci_read_config_word(dev, dev->msi_cap + PCI_MSI_FLAGS, &control);
 	control &= ~PCI_MSI_FLAGS_ENABLE;
 	if (enable)
 		control |= PCI_MSI_FLAGS_ENABLE;
-	pci_write_config_word(dev, pos + PCI_MSI_FLAGS, control);
+	pci_write_config_word(dev, dev->msi_cap + PCI_MSI_FLAGS, control);
 }
 
 static void msix_set_enable(struct pci_dev *dev, int enable)
 {
-	int pos;
 	u16 control;
 
-	pos = pci_find_capability(dev, PCI_CAP_ID_MSIX);
-	if (pos) {
-		pci_read_config_word(dev, pos + PCI_MSIX_FLAGS, &control);
-		control &= ~PCI_MSIX_FLAGS_ENABLE;
-		if (enable)
-			control |= PCI_MSIX_FLAGS_ENABLE;
-		pci_write_config_word(dev, pos + PCI_MSIX_FLAGS, control);
-	}
+	pci_read_config_word(dev, dev->msix_cap + PCI_MSIX_FLAGS, &control);
+	control &= ~PCI_MSIX_FLAGS_ENABLE;
+	if (enable)
+		control |= PCI_MSIX_FLAGS_ENABLE;
+	pci_write_config_word(dev, dev->msix_cap + PCI_MSIX_FLAGS, control);
 }
 
 static inline __attribute_const__ u32 msi_mask(unsigned x)
@@ -247,18 +243,18 @@ void __read_msi_msg(struct msi_desc *entry, struct msi_msg *msg)
 		msg->data = readl(base + PCI_MSIX_ENTRY_DATA);
 	} else {
 		struct pci_dev *dev = entry->dev;
-		int pos = entry->msi_attrib.pos;
+		int pos = dev->msi_cap;
 		u16 data;
 
-		pci_read_config_dword(dev, msi_lower_address_reg(pos),
-					&msg->address_lo);
+		pci_read_config_dword(dev, pos + PCI_MSI_ADDRESS_LO,
+				      &msg->address_lo);
 		if (entry->msi_attrib.is_64) {
-			pci_read_config_dword(dev, msi_upper_address_reg(pos),
-						&msg->address_hi);
-			pci_read_config_word(dev, msi_data_reg(pos, 1), &data);
+			pci_read_config_dword(dev, pos + PCI_MSI_ADDRESS_HI,
+					      &msg->address_hi);
+			pci_read_config_word(dev, pos + PCI_MSI_DATA_64, &data);
 		} else {
 			msg->address_hi = 0;
-			pci_read_config_word(dev, msi_data_reg(pos, 0), &data);
+			pci_read_config_word(dev, pos + PCI_MSI_DATA_32, &data);
 		}
 		msg->data = data;
 	}
@@ -302,24 +298,24 @@ void __write_msi_msg(struct msi_desc *entry, struct msi_msg *msg)
 		writel(msg->data, base + PCI_MSIX_ENTRY_DATA);
 	} else {
 		struct pci_dev *dev = entry->dev;
-		int pos = entry->msi_attrib.pos;
+		int pos = dev->msi_cap;
 		u16 msgctl;
 
-		pci_read_config_word(dev, msi_control_reg(pos), &msgctl);
+		pci_read_config_word(dev, pos + PCI_MSI_FLAGS, &msgctl);
 		msgctl &= ~PCI_MSI_FLAGS_QSIZE;
 		msgctl |= entry->msi_attrib.multiple << 4;
-		pci_write_config_word(dev, msi_control_reg(pos), msgctl);
+		pci_write_config_word(dev, pos + PCI_MSI_FLAGS, msgctl);
 
-		pci_write_config_dword(dev, msi_lower_address_reg(pos),
-					msg->address_lo);
+		pci_write_config_dword(dev, pos + PCI_MSI_ADDRESS_LO,
+				       msg->address_lo);
 		if (entry->msi_attrib.is_64) {
-			pci_write_config_dword(dev, msi_upper_address_reg(pos),
-						msg->address_hi);
-			pci_write_config_word(dev, msi_data_reg(pos, 1),
-						msg->data);
+			pci_write_config_dword(dev, pos + PCI_MSI_ADDRESS_HI,
+					       msg->address_hi);
+			pci_write_config_word(dev, pos + PCI_MSI_DATA_64,
+					      msg->data);
 		} else {
-			pci_write_config_word(dev, msi_data_reg(pos, 0),
-						msg->data);
+			pci_write_config_word(dev, pos + PCI_MSI_DATA_32,
+					      msg->data);
 		}
 	}
 	entry->msg = *msg;
@@ -391,7 +387,6 @@ static void pci_intx_for_msi(struct pci_dev *dev, int enable)
 
 static void __pci_restore_msi_state(struct pci_dev *dev)
 {
-	int pos;
 	u16 control;
 	struct msi_desc *entry;
 
@@ -399,22 +394,20 @@ static void __pci_restore_msi_state(struct pci_dev *dev)
 		return;
 
 	entry = irq_get_msi_desc(dev->irq);
-	pos = entry->msi_attrib.pos;
 
 	pci_intx_for_msi(dev, 0);
-	msi_set_enable(dev, pos, 0);
+	msi_set_enable(dev, 0);
 	arch_restore_msi_irqs(dev, dev->irq);
 
-	pci_read_config_word(dev, pos + PCI_MSI_FLAGS, &control);
+	pci_read_config_word(dev, dev->msi_cap + PCI_MSI_FLAGS, &control);
 	msi_mask_irq(entry, msi_capable_mask(control), entry->masked);
 	control &= ~PCI_MSI_FLAGS_QSIZE;
 	control |= (entry->msi_attrib.multiple << 4) | PCI_MSI_FLAGS_ENABLE;
-	pci_write_config_word(dev, pos + PCI_MSI_FLAGS, control);
+	pci_write_config_word(dev, dev->msi_cap + PCI_MSI_FLAGS, control);
 }
 
 static void __pci_restore_msix_state(struct pci_dev *dev)
 {
-	int pos;
 	struct msi_desc *entry;
 	u16 control;
 
@@ -422,13 +415,12 @@ static void __pci_restore_msix_state(struct pci_dev *dev)
 		return;
 	BUG_ON(list_empty(&dev->msi_list));
 	entry = list_first_entry(&dev->msi_list, struct msi_desc, list);
-	pos = entry->msi_attrib.pos;
-	pci_read_config_word(dev, pos + PCI_MSIX_FLAGS, &control);
+	pci_read_config_word(dev, dev->msix_cap + PCI_MSIX_FLAGS, &control);
 
 	/* route the table */
 	pci_intx_for_msi(dev, 0);
 	control |= PCI_MSIX_FLAGS_ENABLE | PCI_MSIX_FLAGS_MASKALL;
-	pci_write_config_word(dev, pos + PCI_MSIX_FLAGS, control);
+	pci_write_config_word(dev, dev->msix_cap + PCI_MSIX_FLAGS, control);
 
 	list_for_each_entry(entry, &dev->msi_list, list) {
 		arch_restore_msi_irqs(dev, entry->irq);
@@ -436,7 +428,7 @@ static void __pci_restore_msix_state(struct pci_dev *dev)
 	}
 
 	control &= ~PCI_MSIX_FLAGS_MASKALL;
-	pci_write_config_word(dev, pos + PCI_MSIX_FLAGS, control);
+	pci_write_config_word(dev, dev->msix_cap + PCI_MSIX_FLAGS, control);
 }
 
 void pci_restore_msi_state(struct pci_dev *dev)
@@ -484,12 +476,12 @@ static struct msi_attribute mode_attribute =
 	__ATTR(mode, S_IRUGO, show_msi_mode, NULL);
 
 
-struct attribute *msi_irq_default_attrs[] = {
+static struct attribute *msi_irq_default_attrs[] = {
 	&mode_attribute.attr,
 	NULL
 };
 
-void msi_kobj_release(struct kobject *kobj)
+static void msi_kobj_release(struct kobject *kobj)
 {
 	struct msi_desc *entry = to_msi_desc(kobj);
 
@@ -552,27 +544,27 @@ out_unroll:
 static int msi_capability_init(struct pci_dev *dev, int nvec)
 {
 	struct msi_desc *entry;
-	int pos, ret;
+	int ret;
 	u16 control;
 	unsigned mask;
 
-	pos = pci_find_capability(dev, PCI_CAP_ID_MSI);
-	msi_set_enable(dev, pos, 0);	/* Disable MSI during set up */
+	msi_set_enable(dev, 0);	/* Disable MSI during set up */
 
-	pci_read_config_word(dev, msi_control_reg(pos), &control);
+	pci_read_config_word(dev, dev->msi_cap + PCI_MSI_FLAGS, &control);
 	/* MSI Entry Initialization */
 	entry = alloc_msi_entry(dev);
 	if (!entry)
 		return -ENOMEM;
 
 	entry->msi_attrib.is_msix	= 0;
-	entry->msi_attrib.is_64		= is_64bit_address(control);
+	entry->msi_attrib.is_64		= !!(control & PCI_MSI_FLAGS_64BIT);
 	entry->msi_attrib.entry_nr	= 0;
-	entry->msi_attrib.maskbit	= is_mask_bit_support(control);
+	entry->msi_attrib.maskbit	= !!(control & PCI_MSI_FLAGS_MASKBIT);
 	entry->msi_attrib.default_irq	= dev->irq;	/* Save IOAPIC IRQ */
-	entry->msi_attrib.pos		= pos;
+	entry->msi_attrib.pos		= dev->msi_cap;
 
-	entry->mask_pos = msi_mask_reg(pos, entry->msi_attrib.is_64);
+	entry->mask_pos = dev->msi_cap + (control & PCI_MSI_FLAGS_64BIT) ?
+		PCI_MSI_MASK_64 : PCI_MSI_MASK_32;
 	/* All MSIs are unmasked by default, Mask them all */
 	if (entry->msi_attrib.maskbit)
 		pci_read_config_dword(dev, entry->mask_pos, &entry->masked);
@@ -598,31 +590,30 @@ static int msi_capability_init(struct pci_dev *dev, int nvec)
 
 	/* Set MSI enabled bits	 */
 	pci_intx_for_msi(dev, 0);
-	msi_set_enable(dev, pos, 1);
+	msi_set_enable(dev, 1);
 	dev->msi_enabled = 1;
 
 	dev->irq = entry->irq;
 	return 0;
 }
 
-static void __iomem *msix_map_region(struct pci_dev *dev, unsigned pos,
-							unsigned nr_entries)
+static void __iomem *msix_map_region(struct pci_dev *dev, unsigned nr_entries)
 {
 	resource_size_t phys_addr;
 	u32 table_offset;
 	u8 bir;
 
-	pci_read_config_dword(dev, msix_table_offset_reg(pos), &table_offset);
-	bir = (u8)(table_offset & PCI_MSIX_FLAGS_BIRMASK);
-	table_offset &= ~PCI_MSIX_FLAGS_BIRMASK;
+	pci_read_config_dword(dev, dev->msix_cap + PCI_MSIX_TABLE,
+			      &table_offset);
+	bir = (u8)(table_offset & PCI_MSIX_TABLE_BIR);
+	table_offset &= PCI_MSIX_TABLE_OFFSET;
 	phys_addr = pci_resource_start(dev, bir) + table_offset;
 
 	return ioremap_nocache(phys_addr, nr_entries * PCI_MSIX_ENTRY_SIZE);
 }
 
-static int msix_setup_entries(struct pci_dev *dev, unsigned pos,
-				void __iomem *base, struct msix_entry *entries,
-				int nvec)
+static int msix_setup_entries(struct pci_dev *dev, void __iomem *base,
+			      struct msix_entry *entries, int nvec)
 {
 	struct msi_desc *entry;
 	int i;
@@ -642,7 +633,7 @@ static int msix_setup_entries(struct pci_dev *dev, unsigned pos,
 		entry->msi_attrib.is_64		= 1;
 		entry->msi_attrib.entry_nr	= entries[i].entry;
 		entry->msi_attrib.default_irq	= dev->irq;
-		entry->msi_attrib.pos		= pos;
+		entry->msi_attrib.pos		= dev->msix_cap;
 		entry->mask_base		= base;
 
 		list_add_tail(&entry->list, &dev->msi_list);
@@ -652,7 +643,7 @@ static int msix_setup_entries(struct pci_dev *dev, unsigned pos,
 }
 
 static void msix_program_entries(struct pci_dev *dev,
-					struct msix_entry *entries)
+				 struct msix_entry *entries)
 {
 	struct msi_desc *entry;
 	int i = 0;
@@ -682,23 +673,22 @@ static void msix_program_entries(struct pci_dev *dev,
 static int msix_capability_init(struct pci_dev *dev,
 				struct msix_entry *entries, int nvec)
 {
-	int pos, ret;
+	int ret;
 	u16 control;
 	void __iomem *base;
 
-	pos = pci_find_capability(dev, PCI_CAP_ID_MSIX);
-	pci_read_config_word(dev, pos + PCI_MSIX_FLAGS, &control);
+	pci_read_config_word(dev, dev->msix_cap + PCI_MSIX_FLAGS, &control);
 
 	/* Ensure MSI-X is disabled while it is set up */
 	control &= ~PCI_MSIX_FLAGS_ENABLE;
-	pci_write_config_word(dev, pos + PCI_MSIX_FLAGS, control);
+	pci_write_config_word(dev, dev->msix_cap + PCI_MSIX_FLAGS, control);
 
 	/* Request & Map MSI-X table region */
-	base = msix_map_region(dev, pos, multi_msix_capable(control));
+	base = msix_map_region(dev, msix_table_size(control));
 	if (!base)
 		return -ENOMEM;
 
-	ret = msix_setup_entries(dev, pos, base, entries, nvec);
+	ret = msix_setup_entries(dev, base, entries, nvec);
 	if (ret)
 		return ret;
 
@@ -712,7 +702,7 @@ static int msix_capability_init(struct pci_dev *dev,
 	 * interrupts coming in before they're fully set up.
 	 */
 	control |= PCI_MSIX_FLAGS_MASKALL | PCI_MSIX_FLAGS_ENABLE;
-	pci_write_config_word(dev, pos + PCI_MSIX_FLAGS, control);
+	pci_write_config_word(dev, dev->msix_cap + PCI_MSIX_FLAGS, control);
 
 	msix_program_entries(dev, entries);
 
@@ -727,7 +717,7 @@ static int msix_capability_init(struct pci_dev *dev,
 	dev->msix_enabled = 1;
 
 	control &= ~PCI_MSIX_FLAGS_MASKALL;
-	pci_write_config_word(dev, pos + PCI_MSIX_FLAGS, control);
+	pci_write_config_word(dev, dev->msix_cap + PCI_MSIX_FLAGS, control);
 
 	return 0;
 
@@ -795,9 +785,6 @@ static int pci_msi_check_device(struct pci_dev *dev, int nvec, int type)
 	if (ret)
 		return ret;
 
-	if (!pci_find_capability(dev, type))
-		return -EINVAL;
-
 	return 0;
 }
 
@@ -816,13 +803,13 @@ static int pci_msi_check_device(struct pci_dev *dev, int nvec, int type)
  */
 int pci_enable_msi_block(struct pci_dev *dev, unsigned int nvec)
 {
-	int status, pos, maxvec;
+	int status, maxvec;
 	u16 msgctl;
 
-	pos = pci_find_capability(dev, PCI_CAP_ID_MSI);
-	if (!pos)
+	if (!dev->msi_cap)
 		return -EINVAL;
-	pci_read_config_word(dev, pos + PCI_MSI_FLAGS, &msgctl);
+
+	pci_read_config_word(dev, dev->msi_cap + PCI_MSI_FLAGS, &msgctl);
 	maxvec = 1 << ((msgctl & PCI_MSI_FLAGS_QMASK) >> 1);
 	if (nvec > maxvec)
 		return maxvec;
@@ -847,14 +834,13 @@ EXPORT_SYMBOL(pci_enable_msi_block);
 
 int pci_enable_msi_block_auto(struct pci_dev *dev, unsigned int *maxvec)
 {
-	int ret, pos, nvec;
+	int ret, nvec;
 	u16 msgctl;
 
-	pos = pci_find_capability(dev, PCI_CAP_ID_MSI);
-	if (!pos)
+	if (!dev->msi_cap)
 		return -EINVAL;
 
-	pci_read_config_word(dev, pos + PCI_MSI_FLAGS, &msgctl);
+	pci_read_config_word(dev, dev->msi_cap + PCI_MSI_FLAGS, &msgctl);
 	ret = 1 << ((msgctl & PCI_MSI_FLAGS_QMASK) >> 1);
 
 	if (maxvec)
@@ -876,21 +862,19 @@ void pci_msi_shutdown(struct pci_dev *dev)
 	struct msi_desc *desc;
 	u32 mask;
 	u16 ctrl;
-	unsigned pos;
 
 	if (!pci_msi_enable || !dev || !dev->msi_enabled)
 		return;
 
 	BUG_ON(list_empty(&dev->msi_list));
 	desc = list_first_entry(&dev->msi_list, struct msi_desc, list);
-	pos = desc->msi_attrib.pos;
 
-	msi_set_enable(dev, pos, 0);
+	msi_set_enable(dev, 0);
 	pci_intx_for_msi(dev, 1);
 	dev->msi_enabled = 0;
 
 	/* Return the device with MSI unmasked as initial states */
-	pci_read_config_word(dev, pos + PCI_MSI_FLAGS, &ctrl);
+	pci_read_config_word(dev, dev->msi_cap + PCI_MSI_FLAGS, &ctrl);
 	mask = msi_capable_mask(ctrl);
 	/* Keep cached state to be restored */
 	__msi_mask_irq(desc, mask, ~mask);
@@ -917,15 +901,13 @@ EXPORT_SYMBOL(pci_disable_msi);
  */
 int pci_msix_table_size(struct pci_dev *dev)
 {
-	int pos;
 	u16 control;
 
-	pos = pci_find_capability(dev, PCI_CAP_ID_MSIX);
-	if (!pos)
+	if (!dev->msix_cap)
 		return 0;
 
-	pci_read_config_word(dev, msi_control_reg(pos), &control);
-	return multi_msix_capable(control);
+	pci_read_config_word(dev, dev->msix_cap + PCI_MSIX_FLAGS, &control);
+	return msix_table_size(control);
 }
 
 /**
@@ -948,7 +930,7 @@ int pci_enable_msix(struct pci_dev *dev, struct msix_entry *entries, int nvec)
 	int status, nr_entries;
 	int i, j;
 
-	if (!entries)
+	if (!entries || !dev->msix_cap)
 		return -EINVAL;
 
 	status = pci_msi_check_device(dev, nvec, PCI_CAP_ID_MSIX);
@@ -1048,15 +1030,17 @@ EXPORT_SYMBOL(pci_msi_enabled);
 
 void pci_msi_init_pci_dev(struct pci_dev *dev)
 {
-	int pos;
 	INIT_LIST_HEAD(&dev->msi_list);
 
 	/* Disable the msi hardware to avoid screaming interrupts
 	 * during boot.  This is the power on reset default so
 	 * usually this should be a noop.
 	 */
-	pos = pci_find_capability(dev, PCI_CAP_ID_MSI);
-	if (pos)
-		msi_set_enable(dev, pos, 0);
-	msix_set_enable(dev, 0);
+	dev->msi_cap = pci_find_capability(dev, PCI_CAP_ID_MSI);
+	if (dev->msi_cap)
+		msi_set_enable(dev, 0);
+
+	dev->msix_cap = pci_find_capability(dev, PCI_CAP_ID_MSIX);
+	if (dev->msix_cap)
+		msix_set_enable(dev, 0);
 }
diff --git a/drivers/pci/msi.h b/drivers/pci/msi.h
deleted file mode 100644
index 65c42f80f23..00000000000
--- a/drivers/pci/msi.h
+++ /dev/null
@@ -1,24 +0,0 @@
-/*
- * Copyright (C) 2003-2004 Intel
- * Copyright (C) Tom Long Nguyen (tom.l.nguyen@intel.com)
- */
-
-#ifndef MSI_H
-#define MSI_H
-
-#define msi_control_reg(base)		(base + PCI_MSI_FLAGS)
-#define msi_lower_address_reg(base)	(base + PCI_MSI_ADDRESS_LO)
-#define msi_upper_address_reg(base)	(base + PCI_MSI_ADDRESS_HI)
-#define msi_data_reg(base, is64bit)	\
-	(base + ((is64bit == 1) ? PCI_MSI_DATA_64 : PCI_MSI_DATA_32))
-#define msi_mask_reg(base, is64bit)	\
-	(base + ((is64bit == 1) ? PCI_MSI_MASK_64 : PCI_MSI_MASK_32))
-#define is_64bit_address(control)	(!!(control & PCI_MSI_FLAGS_64BIT))
-#define is_mask_bit_support(control)	(!!(control & PCI_MSI_FLAGS_MASKBIT))
-
-#define msix_table_offset_reg(base)	(base + PCI_MSIX_TABLE)
-#define msix_pba_offset_reg(base)	(base + PCI_MSIX_PBA)
-#define msix_table_size(control) 	((control & PCI_MSIX_FLAGS_QSIZE)+1)
-#define multi_msix_capable(control)	msix_table_size((control))
-
-#endif /* MSI_H */
diff --git a/drivers/pci/pci-acpi.c b/drivers/pci/pci-acpi.c
index dee5dddaa29..e4b1fb2c0f5 100644
--- a/drivers/pci/pci-acpi.c
+++ b/drivers/pci/pci-acpi.c
@@ -53,14 +53,15 @@ static void pci_acpi_wake_dev(acpi_handle handle, u32 event, void *context)
 		return;
 	}
 
-	if (!pci_dev->pm_cap || !pci_dev->pme_support
-	     || pci_check_pme_status(pci_dev)) {
-		if (pci_dev->pme_poll)
-			pci_dev->pme_poll = false;
+	/* Clear PME Status if set. */
+	if (pci_dev->pme_support)
+		pci_check_pme_status(pci_dev);
 
-		pci_wakeup_event(pci_dev);
-		pm_runtime_resume(&pci_dev->dev);
-	}
+	if (pci_dev->pme_poll)
+		pci_dev->pme_poll = false;
+
+	pci_wakeup_event(pci_dev);
+	pm_runtime_resume(&pci_dev->dev);
 
 	if (pci_dev->subordinate)
 		pci_pme_wakeup_bus(pci_dev->subordinate);
@@ -287,6 +288,32 @@ static struct pci_platform_pm_ops acpi_pci_platform_pm = {
 	.run_wake = acpi_pci_run_wake,
 };
 
+void acpi_pci_add_bus(struct pci_bus *bus)
+{
+	acpi_handle handle = NULL;
+
+	if (bus->bridge)
+		handle = ACPI_HANDLE(bus->bridge);
+	if (acpi_pci_disabled || handle == NULL)
+		return;
+
+	acpi_pci_slot_enumerate(bus, handle);
+	acpiphp_enumerate_slots(bus, handle);
+}
+
+void acpi_pci_remove_bus(struct pci_bus *bus)
+{
+	/*
+	 * bus->bridge->acpi_node.handle has already been reset to NULL
+	 * when acpi_pci_remove_bus() is called, so don't check ACPI handle.
+	 */
+	if (acpi_pci_disabled)
+		return;
+
+	acpiphp_remove_slots(bus);
+	acpi_pci_slot_remove(bus);
+}
+
 /* ACPI bus type */
 static int acpi_pci_find_device(struct device *dev, acpi_handle *handle)
 {
@@ -361,7 +388,11 @@ static int __init acpi_pci_init(void)
 	ret = register_acpi_bus_type(&acpi_pci_bus);
 	if (ret)
 		return 0;
+
 	pci_set_platform_pm(&acpi_pci_platform_pm);
+	acpi_pci_slot_init();
+	acpiphp_init();
+
 	return 0;
 }
 arch_initcall(acpi_pci_init);
diff --git a/drivers/pci/pci-driver.c b/drivers/pci/pci-driver.c
index 1fa1e482a99..79277fb36c6 100644
--- a/drivers/pci/pci-driver.c
+++ b/drivers/pci/pci-driver.c
@@ -390,9 +390,10 @@ static void pci_device_shutdown(struct device *dev)
 
 	/*
 	 * Turn off Bus Master bit on the device to tell it to not
-	 * continue to do DMA
+	 * continue to do DMA. Don't touch devices in D3cold or unknown states.
 	 */
-	pci_clear_master(pci_dev);
+	if (pci_dev->current_state <= PCI_D3hot)
+		pci_clear_master(pci_dev);
 }
 
 #ifdef CONFIG_PM
diff --git a/drivers/pci/pci-sysfs.c b/drivers/pci/pci-sysfs.c
index 9c6e9bb674e..5b4a9d9cd20 100644
--- a/drivers/pci/pci-sysfs.c
+++ b/drivers/pci/pci-sysfs.c
@@ -897,7 +897,7 @@ int pci_mmap_fits(struct pci_dev *pdev, int resno, struct vm_area_struct *vma,
 
 	if (pci_resource_len(pdev, resno) == 0)
 		return 0;
-	nr = (vma->vm_end - vma->vm_start) >> PAGE_SHIFT;
+	nr = vma_pages(vma);
 	start = vma->vm_pgoff;
 	size = ((pci_resource_len(pdev, resno) - 1) >> PAGE_SHIFT) + 1;
 	pci_start = (mmap_api == PCI_MMAP_PROCFS) ?
diff --git a/drivers/pci/pci.c b/drivers/pci/pci.c
index b099e0025d2..a899d8bb190 100644
--- a/drivers/pci/pci.c
+++ b/drivers/pci/pci.c
@@ -646,15 +646,11 @@ static int pci_platform_power_transition(struct pci_dev *dev, pci_power_t state)
 		error = platform_pci_set_power_state(dev, state);
 		if (!error)
 			pci_update_current_state(dev, state);
-		/* Fall back to PCI_D0 if native PM is not supported */
-		if (!dev->pm_cap)
-			dev->current_state = PCI_D0;
-	} else {
+	} else
 		error = -ENODEV;
-		/* Fall back to PCI_D0 if native PM is not supported */
-		if (!dev->pm_cap)
-			dev->current_state = PCI_D0;
-	}
+
+	if (error && !dev->pm_cap) /* Fall back to PCI_D0 */
+		dev->current_state = PCI_D0;
 
 	return error;
 }
@@ -1575,7 +1571,7 @@ void pci_pme_active(struct pci_dev *dev, bool enable)
 {
 	u16 pmcsr;
 
-	if (!dev->pm_cap)
+	if (!dev->pme_support)
 		return;
 
 	pci_read_config_word(dev, dev->pm_cap + PCI_PM_CTRL, &pmcsr);
@@ -1924,6 +1920,7 @@ void pci_pm_init(struct pci_dev *dev)
 	dev->wakeup_prepared = false;
 
 	dev->pm_cap = 0;
+	dev->pme_support = 0;
 
 	/* find PCI PM capability in list */
 	pm = pci_find_capability(dev, PCI_CAP_ID_PM);
@@ -1975,8 +1972,6 @@ void pci_pm_init(struct pci_dev *dev)
 		device_set_wakeup_capable(&dev->dev, true);
 		/* Disable the PME# generation functionality */
 		pci_pme_active(dev, false);
-	} else {
-		dev->pme_support = 0;
 	}
 }
 
@@ -2619,7 +2614,7 @@ void pci_release_selected_regions(struct pci_dev *pdev, int bars)
 			pci_release_region(pdev, i);
 }
 
-int __pci_request_selected_regions(struct pci_dev *pdev, int bars,
+static int __pci_request_selected_regions(struct pci_dev *pdev, int bars,
 				 const char *res_name, int excl)
 {
 	int i;
@@ -3699,7 +3694,7 @@ static DEFINE_SPINLOCK(resource_alignment_lock);
  * RETURNS: Resource alignment if it is specified.
  *          Zero if it is not specified.
  */
-resource_size_t pci_specified_resource_alignment(struct pci_dev *dev)
+static resource_size_t pci_specified_resource_alignment(struct pci_dev *dev)
 {
 	int seg, bus, slot, func, align_order, count;
 	resource_size_t align = 0;
@@ -3812,7 +3807,7 @@ void pci_reassigndev_resource_alignment(struct pci_dev *dev)
 	}
 }
 
-ssize_t pci_set_resource_alignment_param(const char *buf, size_t count)
+static ssize_t pci_set_resource_alignment_param(const char *buf, size_t count)
 {
 	if (count > RESOURCE_ALIGNMENT_PARAM_SIZE - 1)
 		count = RESOURCE_ALIGNMENT_PARAM_SIZE - 1;
@@ -3823,7 +3818,7 @@ ssize_t pci_set_resource_alignment_param(const char *buf, size_t count)
 	return count;
 }
 
-ssize_t pci_get_resource_alignment_param(char *buf, size_t size)
+static ssize_t pci_get_resource_alignment_param(char *buf, size_t size)
 {
 	size_t count;
 	spin_lock(&resource_alignment_lock);
diff --git a/drivers/pci/pci.h b/drivers/pci/pci.h
index 7346ee68f47..68678ed76b0 100644
--- a/drivers/pci/pci.h
+++ b/drivers/pci/pci.h
@@ -8,26 +8,25 @@
 
 /* Functions internal to the PCI core code */
 
-extern int pci_create_sysfs_dev_files(struct pci_dev *pdev);
-extern void pci_remove_sysfs_dev_files(struct pci_dev *pdev);
+int pci_create_sysfs_dev_files(struct pci_dev *pdev);
+void pci_remove_sysfs_dev_files(struct pci_dev *pdev);
 #if !defined(CONFIG_DMI) && !defined(CONFIG_ACPI)
 static inline void pci_create_firmware_label_files(struct pci_dev *pdev)
 { return; }
 static inline void pci_remove_firmware_label_files(struct pci_dev *pdev)
 { return; }
 #else
-extern void pci_create_firmware_label_files(struct pci_dev *pdev);
-extern void pci_remove_firmware_label_files(struct pci_dev *pdev);
+void pci_create_firmware_label_files(struct pci_dev *pdev);
+void pci_remove_firmware_label_files(struct pci_dev *pdev);
 #endif
-extern void pci_cleanup_rom(struct pci_dev *dev);
+void pci_cleanup_rom(struct pci_dev *dev);
 #ifdef HAVE_PCI_MMAP
 enum pci_mmap_api {
 	PCI_MMAP_SYSFS,	/* mmap on /sys/bus/pci/devices/<BDF>/resource<N> */
 	PCI_MMAP_PROCFS	/* mmap on /proc/bus/pci/<BDF> */
 };
-extern int pci_mmap_fits(struct pci_dev *pdev, int resno,
-			 struct vm_area_struct *vmai,
-			 enum pci_mmap_api mmap_api);
+int pci_mmap_fits(struct pci_dev *pdev, int resno, struct vm_area_struct *vmai,
+		  enum pci_mmap_api mmap_api);
 #endif
 int pci_probe_reset_function(struct pci_dev *dev);
 
@@ -60,17 +59,17 @@ struct pci_platform_pm_ops {
 	int (*run_wake)(struct pci_dev *dev, bool enable);
 };
 
-extern int pci_set_platform_pm(struct pci_platform_pm_ops *ops);
-extern void pci_update_current_state(struct pci_dev *dev, pci_power_t state);
-extern void pci_power_up(struct pci_dev *dev);
-extern void pci_disable_enabled_device(struct pci_dev *dev);
-extern int pci_finish_runtime_suspend(struct pci_dev *dev);
-extern int __pci_pme_wakeup(struct pci_dev *dev, void *ign);
-extern void pci_wakeup_bus(struct pci_bus *bus);
-extern void pci_config_pm_runtime_get(struct pci_dev *dev);
-extern void pci_config_pm_runtime_put(struct pci_dev *dev);
-extern void pci_pm_init(struct pci_dev *dev);
-extern void pci_allocate_cap_save_buffers(struct pci_dev *dev);
+int pci_set_platform_pm(struct pci_platform_pm_ops *ops);
+void pci_update_current_state(struct pci_dev *dev, pci_power_t state);
+void pci_power_up(struct pci_dev *dev);
+void pci_disable_enabled_device(struct pci_dev *dev);
+int pci_finish_runtime_suspend(struct pci_dev *dev);
+int __pci_pme_wakeup(struct pci_dev *dev, void *ign);
+void pci_wakeup_bus(struct pci_bus *bus);
+void pci_config_pm_runtime_get(struct pci_dev *dev);
+void pci_config_pm_runtime_put(struct pci_dev *dev);
+void pci_pm_init(struct pci_dev *dev);
+void pci_allocate_cap_save_buffers(struct pci_dev *dev);
 void pci_free_cap_save_buffers(struct pci_dev *dev);
 
 static inline void pci_wakeup_event(struct pci_dev *dev)
@@ -96,7 +95,7 @@ struct pci_vpd {
 	struct bin_attribute *attr; /* descriptor for sysfs VPD entry */
 };
 
-extern int pci_vpd_pci22_init(struct pci_dev *dev);
+int pci_vpd_pci22_init(struct pci_dev *dev);
 static inline void pci_vpd_release(struct pci_dev *dev)
 {
 	if (dev->vpd)
@@ -105,9 +104,9 @@ static inline void pci_vpd_release(struct pci_dev *dev)
 
 /* PCI /proc functions */
 #ifdef CONFIG_PROC_FS
-extern int pci_proc_attach_device(struct pci_dev *dev);
-extern int pci_proc_detach_device(struct pci_dev *dev);
-extern int pci_proc_detach_bus(struct pci_bus *bus);
+int pci_proc_attach_device(struct pci_dev *dev);
+int pci_proc_detach_device(struct pci_dev *dev);
+int pci_proc_detach_bus(struct pci_bus *bus);
 #else
 static inline int pci_proc_attach_device(struct pci_dev *dev) { return 0; }
 static inline int pci_proc_detach_device(struct pci_dev *dev) { return 0; }
@@ -118,8 +117,8 @@ static inline int pci_proc_detach_bus(struct pci_bus *bus) { return 0; }
 int pci_hp_add_bridge(struct pci_dev *dev);
 
 #ifdef HAVE_PCI_LEGACY
-extern void pci_create_legacy_files(struct pci_bus *bus);
-extern void pci_remove_legacy_files(struct pci_bus *bus);
+void pci_create_legacy_files(struct pci_bus *bus);
+void pci_remove_legacy_files(struct pci_bus *bus);
 #else
 static inline void pci_create_legacy_files(struct pci_bus *bus) { return; }
 static inline void pci_remove_legacy_files(struct pci_bus *bus) { return; }
@@ -134,7 +133,7 @@ extern unsigned int pci_pm_d3_delay;
 
 #ifdef CONFIG_PCI_MSI
 void pci_no_msi(void);
-extern void pci_msi_init_pci_dev(struct pci_dev *dev);
+void pci_msi_init_pci_dev(struct pci_dev *dev);
 #else
 static inline void pci_no_msi(void) { }
 static inline void pci_msi_init_pci_dev(struct pci_dev *dev) { }
@@ -198,12 +197,11 @@ enum pci_bar_type {
 
 bool pci_bus_read_dev_vendor_id(struct pci_bus *bus, int devfn, u32 *pl,
 				int crs_timeout);
-extern int pci_setup_device(struct pci_dev *dev);
-extern int __pci_read_base(struct pci_dev *dev, enum pci_bar_type type,
-				struct resource *res, unsigned int reg);
-extern int pci_resource_bar(struct pci_dev *dev, int resno,
-			    enum pci_bar_type *type);
-extern void pci_configure_ari(struct pci_dev *dev);
+int pci_setup_device(struct pci_dev *dev);
+int __pci_read_base(struct pci_dev *dev, enum pci_bar_type type,
+		    struct resource *res, unsigned int reg);
+int pci_resource_bar(struct pci_dev *dev, int resno, enum pci_bar_type *type);
+void pci_configure_ari(struct pci_dev *dev);
 
 /**
  * pci_ari_enabled - query ARI forwarding status
@@ -217,7 +215,7 @@ static inline int pci_ari_enabled(struct pci_bus *bus)
 }
 
 void pci_reassigndev_resource_alignment(struct pci_dev *dev);
-extern void pci_disable_bridge_window(struct pci_dev *dev);
+void pci_disable_bridge_window(struct pci_dev *dev);
 
 /* Single Root I/O Virtualization */
 struct pci_sriov {
@@ -241,7 +239,7 @@ struct pci_sriov {
 };
 
 #ifdef CONFIG_PCI_ATS
-extern void pci_restore_ats_state(struct pci_dev *dev);
+void pci_restore_ats_state(struct pci_dev *dev);
 #else
 static inline void pci_restore_ats_state(struct pci_dev *dev)
 {
@@ -249,14 +247,13 @@ static inline void pci_restore_ats_state(struct pci_dev *dev)
 #endif /* CONFIG_PCI_ATS */
 
 #ifdef CONFIG_PCI_IOV
-extern int pci_iov_init(struct pci_dev *dev);
-extern void pci_iov_release(struct pci_dev *dev);
-extern int pci_iov_resource_bar(struct pci_dev *dev, int resno,
-				enum pci_bar_type *type);
-extern resource_size_t pci_sriov_resource_alignment(struct pci_dev *dev,
-						    int resno);
-extern void pci_restore_iov_state(struct pci_dev *dev);
-extern int pci_iov_bus_range(struct pci_bus *bus);
+int pci_iov_init(struct pci_dev *dev);
+void pci_iov_release(struct pci_dev *dev);
+int pci_iov_resource_bar(struct pci_dev *dev, int resno,
+			 enum pci_bar_type *type);
+resource_size_t pci_sriov_resource_alignment(struct pci_dev *dev, int resno);
+void pci_restore_iov_state(struct pci_dev *dev);
+int pci_iov_bus_range(struct pci_bus *bus);
 
 #else
 static inline int pci_iov_init(struct pci_dev *dev)
@@ -282,10 +279,10 @@ static inline int pci_iov_bus_range(struct pci_bus *bus)
 
 #endif /* CONFIG_PCI_IOV */
 
-extern unsigned long pci_cardbus_resource_alignment(struct resource *);
+unsigned long pci_cardbus_resource_alignment(struct resource *);
 
 static inline resource_size_t pci_resource_alignment(struct pci_dev *dev,
-					 struct resource *res)
+						     struct resource *res)
 {
 #ifdef CONFIG_PCI_IOV
 	int resno = res - dev->resource;
@@ -298,7 +295,7 @@ static inline resource_size_t pci_resource_alignment(struct pci_dev *dev,
 	return resource_alignment(res);
 }
 
-extern void pci_enable_acs(struct pci_dev *dev);
+void pci_enable_acs(struct pci_dev *dev);
 
 struct pci_dev_reset_methods {
 	u16 vendor;
@@ -307,7 +304,7 @@ struct pci_dev_reset_methods {
 };
 
 #ifdef CONFIG_PCI_QUIRKS
-extern int pci_dev_specific_reset(struct pci_dev *dev, int probe);
+int pci_dev_specific_reset(struct pci_dev *dev, int probe);
 #else
 static inline int pci_dev_specific_reset(struct pci_dev *dev, int probe)
 {
diff --git a/drivers/pci/pcie/Kconfig b/drivers/pci/pcie/Kconfig
index fde4a32a029..569f82fc9e2 100644
--- a/drivers/pci/pcie/Kconfig
+++ b/drivers/pci/pcie/Kconfig
@@ -82,4 +82,4 @@ endchoice
 
 config PCIE_PME
 	def_bool y
-	depends on PCIEPORTBUS && PM_RUNTIME && ACPI
+	depends on PCIEPORTBUS && PM_RUNTIME
diff --git a/drivers/pci/pcie/aer/aer_inject.c b/drivers/pci/pcie/aer/aer_inject.c
index 4e24cb8a94a..587e7e85310 100644
--- a/drivers/pci/pcie/aer/aer_inject.c
+++ b/drivers/pci/pcie/aer/aer_inject.c
@@ -212,8 +212,8 @@ out:
 	return ops->read(bus, devfn, where, size, val);
 }
 
-int pci_write_aer(struct pci_bus *bus, unsigned int devfn, int where, int size,
-		  u32 val)
+static int pci_write_aer(struct pci_bus *bus, unsigned int devfn, int where,
+			 int size, u32 val)
 {
 	u32 *sim;
 	struct aer_error *err;
@@ -334,13 +334,13 @@ static int aer_inject(struct aer_error_inj *einj)
 		return -ENODEV;
 	rpdev = pcie_find_root_port(dev);
 	if (!rpdev) {
-		ret = -ENOTTY;
+		ret = -ENODEV;
 		goto out_put;
 	}
 
 	pos_cap_err = pci_find_ext_capability(dev, PCI_EXT_CAP_ID_ERR);
 	if (!pos_cap_err) {
-		ret = -ENOTTY;
+		ret = -EPERM;
 		goto out_put;
 	}
 	pci_read_config_dword(dev, pos_cap_err + PCI_ERR_UNCOR_SEVER, &sever);
@@ -350,7 +350,7 @@ static int aer_inject(struct aer_error_inj *einj)
 
 	rp_pos_cap_err = pci_find_ext_capability(rpdev, PCI_EXT_CAP_ID_ERR);
 	if (!rp_pos_cap_err) {
-		ret = -ENOTTY;
+		ret = -EPERM;
 		goto out_put;
 	}
 
diff --git a/drivers/pci/pcie/aer/aerdrv.h b/drivers/pci/pcie/aer/aerdrv.h
index 22f840f4dda..d12c77cd699 100644
--- a/drivers/pci/pcie/aer/aerdrv.h
+++ b/drivers/pci/pcie/aer/aerdrv.h
@@ -110,15 +110,15 @@ static inline pci_ers_result_t merge_result(enum pci_ers_result orig,
 }
 
 extern struct bus_type pcie_port_bus_type;
-extern void aer_do_secondary_bus_reset(struct pci_dev *dev);
-extern int aer_init(struct pcie_device *dev);
-extern void aer_isr(struct work_struct *work);
-extern void aer_print_error(struct pci_dev *dev, struct aer_err_info *info);
-extern void aer_print_port_info(struct pci_dev *dev, struct aer_err_info *info);
-extern irqreturn_t aer_irq(int irq, void *context);
+void aer_do_secondary_bus_reset(struct pci_dev *dev);
+int aer_init(struct pcie_device *dev);
+void aer_isr(struct work_struct *work);
+void aer_print_error(struct pci_dev *dev, struct aer_err_info *info);
+void aer_print_port_info(struct pci_dev *dev, struct aer_err_info *info);
+irqreturn_t aer_irq(int irq, void *context);
 
 #ifdef CONFIG_ACPI_APEI
-extern int pcie_aer_get_firmware_first(struct pci_dev *pci_dev);
+int pcie_aer_get_firmware_first(struct pci_dev *pci_dev);
 #else
 static inline int pcie_aer_get_firmware_first(struct pci_dev *pci_dev)
 {
diff --git a/drivers/pci/pcie/aer/aerdrv_core.c b/drivers/pci/pcie/aer/aerdrv_core.c
index 564d97f94b6..8ec8b4f4856 100644
--- a/drivers/pci/pcie/aer/aerdrv_core.c
+++ b/drivers/pci/pcie/aer/aerdrv_core.c
@@ -89,8 +89,6 @@ static int add_error_device(struct aer_err_info *e_info, struct pci_dev *dev)
 	return -ENOSPC;
 }
 
-#define	PCI_BUS(x)	(((x) >> 8) & 0xff)
-
 /**
  * is_error_source - check whether the device is source of reported error
  * @dev: pointer to pci_dev to be checked
@@ -106,7 +104,7 @@ static bool is_error_source(struct pci_dev *dev, struct aer_err_info *e_info)
 	 * When bus id is equal to 0, it might be a bad id
 	 * reported by root port.
 	 */
-	if (!nosourceid && (PCI_BUS(e_info->id) != 0)) {
+	if (!nosourceid && (PCI_BUS_NUM(e_info->id) != 0)) {
 		/* Device ID match? */
 		if (e_info->id == ((dev->bus->number << 8) | dev->devfn))
 			return true;
diff --git a/drivers/pci/pcie/pme.c b/drivers/pci/pcie/pme.c
index 9ca0dc9ffd8..795db1f9d50 100644
--- a/drivers/pci/pcie/pme.c
+++ b/drivers/pci/pcie/pme.c
@@ -19,8 +19,6 @@
 #include <linux/interrupt.h>
 #include <linux/device.h>
 #include <linux/pcieport_if.h>
-#include <linux/acpi.h>
-#include <linux/pci-acpi.h>
 #include <linux/pm_runtime.h>
 
 #include "../pci.h"
diff --git a/drivers/pci/pcie/portdrv.h b/drivers/pci/pcie/portdrv.h
index eea2ca2375e..d2eb80aab56 100644
--- a/drivers/pci/pcie/portdrv.h
+++ b/drivers/pci/pcie/portdrv.h
@@ -21,18 +21,18 @@
 #define get_descriptor_id(type, service) (((type - 4) << 4) | service)
 
 extern struct bus_type pcie_port_bus_type;
-extern int pcie_port_device_register(struct pci_dev *dev);
+int pcie_port_device_register(struct pci_dev *dev);
 #ifdef CONFIG_PM
-extern int pcie_port_device_suspend(struct device *dev);
-extern int pcie_port_device_resume(struct device *dev);
+int pcie_port_device_suspend(struct device *dev);
+int pcie_port_device_resume(struct device *dev);
 #endif
-extern void pcie_port_device_remove(struct pci_dev *dev);
-extern int __must_check pcie_port_bus_register(void);
-extern void pcie_port_bus_unregister(void);
+void pcie_port_device_remove(struct pci_dev *dev);
+int __must_check pcie_port_bus_register(void);
+void pcie_port_bus_unregister(void);
 
 struct pci_dev;
 
-extern void pcie_clear_root_pme_status(struct pci_dev *dev);
+void pcie_clear_root_pme_status(struct pci_dev *dev);
 
 #ifdef CONFIG_HOTPLUG_PCI_PCIE
 extern bool pciehp_msi_disabled;
@@ -59,7 +59,7 @@ static inline bool pcie_pme_no_msi(void)
 	return pcie_pme_msi_disabled;
 }
 
-extern void pcie_pme_interrupt_enable(struct pci_dev *dev, bool enable);
+void pcie_pme_interrupt_enable(struct pci_dev *dev, bool enable);
 #else /* !CONFIG_PCIE_PME */
 static inline void pcie_pme_disable_msi(void) {}
 static inline bool pcie_pme_no_msi(void) { return false; }
@@ -67,7 +67,7 @@ static inline void pcie_pme_interrupt_enable(struct pci_dev *dev, bool en) {}
 #endif /* !CONFIG_PCIE_PME */
 
 #ifdef CONFIG_ACPI
-extern int pcie_port_acpi_setup(struct pci_dev *port, int *mask);
+int pcie_port_acpi_setup(struct pci_dev *port, int *mask);
 
 static inline int pcie_port_platform_notify(struct pci_dev *port, int *mask)
 {
diff --git a/drivers/pci/pcie/portdrv_acpi.c b/drivers/pci/pcie/portdrv_acpi.c
index a86b56e5f2f..b4d2894ee3f 100644
--- a/drivers/pci/pcie/portdrv_acpi.c
+++ b/drivers/pci/pcie/portdrv_acpi.c
@@ -17,6 +17,7 @@
 
 #include "aer/aerdrv.h"
 #include "../pci.h"
+#include "portdrv.h"
 
 /**
  * pcie_port_acpi_setup - Request the BIOS to release control of PCIe services.
diff --git a/drivers/pci/pcie/portdrv_pci.c b/drivers/pci/pcie/portdrv_pci.c
index 08c243ab034..696caed5fdf 100644
--- a/drivers/pci/pcie/portdrv_pci.c
+++ b/drivers/pci/pcie/portdrv_pci.c
@@ -185,14 +185,6 @@ static const struct dev_pm_ops pcie_portdrv_pm_ops = {
 #endif /* !PM */
 
 /*
- * PCIe port runtime suspend is broken for some chipsets, so use a
- * black list to disable runtime PM for these chipsets.
- */
-static const struct pci_device_id port_runtime_pm_black_list[] = {
-	{ /* end: all zeroes */ }
-};
-
-/*
  * pcie_portdrv_probe - Probe PCI-Express port devices
  * @dev: PCI-Express port device being probed
  *
@@ -225,16 +217,11 @@ static int pcie_portdrv_probe(struct pci_dev *dev,
 	 * it by default.
 	 */
 	dev->d3cold_allowed = false;
-	if (!pci_match_id(port_runtime_pm_black_list, dev))
-		pm_runtime_put_noidle(&dev->dev);
-
 	return 0;
 }
 
 static void pcie_portdrv_remove(struct pci_dev *dev)
 {
-	if (!pci_match_id(port_runtime_pm_black_list, dev))
-		pm_runtime_get_noresume(&dev->dev);
 	pcie_port_device_remove(dev);
 	pci_disable_device(dev);
 }
@@ -272,11 +259,9 @@ static pci_ers_result_t pcie_portdrv_error_detected(struct pci_dev *dev,
 					enum pci_channel_state error)
 {
 	struct aer_broadcast_data data = {error, PCI_ERS_RESULT_CAN_RECOVER};
-	int ret;
-
-	/* can not fail */
-	ret = device_for_each_child(&dev->dev, &data, error_detected_iter);
 
+	/* get true return value from &data */
+	device_for_each_child(&dev->dev, &data, error_detected_iter);
 	return data.result;
 }
 
@@ -308,10 +293,9 @@ static int mmio_enabled_iter(struct device *device, void *data)
 static pci_ers_result_t pcie_portdrv_mmio_enabled(struct pci_dev *dev)
 {
 	pci_ers_result_t status = PCI_ERS_RESULT_RECOVERED;
-	int retval;
 
 	/* get true return value from &status */
-	retval = device_for_each_child(&dev->dev, &status, mmio_enabled_iter);
+	device_for_each_child(&dev->dev, &status, mmio_enabled_iter);
 	return status;
 }
 
@@ -343,7 +327,6 @@ static int slot_reset_iter(struct device *device, void *data)
 static pci_ers_result_t pcie_portdrv_slot_reset(struct pci_dev *dev)
 {
 	pci_ers_result_t status = PCI_ERS_RESULT_RECOVERED;
-	int retval;
 
 	/* If fatal, restore cfg space for possible link reset at upstream */
 	if (dev->error_state == pci_channel_io_frozen) {
@@ -354,8 +337,7 @@ static pci_ers_result_t pcie_portdrv_slot_reset(struct pci_dev *dev)
 	}
 
 	/* get true return value from &status */
-	retval = device_for_each_child(&dev->dev, &status, slot_reset_iter);
-
+	device_for_each_child(&dev->dev, &status, slot_reset_iter);
 	return status;
 }
 
@@ -381,9 +363,7 @@ static int resume_iter(struct device *device, void *data)
 
 static void pcie_portdrv_err_resume(struct pci_dev *dev)
 {
-	int retval;
-	/* nothing to do with error value, if it ever happens */
-	retval = device_for_each_child(&dev->dev, NULL, resume_iter);
+	device_for_each_child(&dev->dev, NULL, resume_iter);
 }
 
 /*
diff --git a/drivers/pci/probe.c b/drivers/pci/probe.c
index b494066ef32..43ece5d41d3 100644
--- a/drivers/pci/probe.c
+++ b/drivers/pci/probe.c
@@ -673,6 +673,8 @@ add_dev:
 	ret = device_register(&child->dev);
 	WARN_ON(ret < 0);
 
+	pcibios_add_bus(child);
+
 	/* Create legacy_io and legacy_mem files for this bus */
 	pci_create_legacy_files(child);
 
@@ -1627,8 +1629,7 @@ unsigned int pci_scan_child_bus(struct pci_bus *bus)
 	if (!bus->is_added) {
 		dev_dbg(&bus->dev, "fixups for bus\n");
 		pcibios_fixup_bus(bus);
-		if (pci_is_root_bus(bus))
-			bus->is_added = 1;
+		bus->is_added = 1;
 	}
 
 	for (pass=0; pass < 2; pass++)
@@ -1661,6 +1662,14 @@ int __weak pcibios_root_bridge_prepare(struct pci_host_bridge *bridge)
 	return 0;
 }
 
+void __weak pcibios_add_bus(struct pci_bus *bus)
+{
+}
+
+void __weak pcibios_remove_bus(struct pci_bus *bus)
+{
+}
+
 struct pci_bus *pci_create_root_bus(struct device *parent, int bus,
 		struct pci_ops *ops, void *sysdata, struct list_head *resources)
 {
@@ -1715,6 +1724,8 @@ struct pci_bus *pci_create_root_bus(struct device *parent, int bus,
 	if (error)
 		goto class_dev_reg_err;
 
+	pcibios_add_bus(b);
+
 	/* Create legacy_io and legacy_mem files for this bus */
 	pci_create_legacy_files(b);
 
diff --git a/drivers/pci/quirks.c b/drivers/pci/quirks.c
index 0369fb6fc1d..7d68aeebf56 100644
--- a/drivers/pci/quirks.c
+++ b/drivers/pci/quirks.c
@@ -324,29 +324,30 @@ static void quirk_cs5536_vsa(struct pci_dev *dev)
 }
 DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_AMD, PCI_DEVICE_ID_AMD_CS5536_ISA, quirk_cs5536_vsa);
 
-static void quirk_io_region(struct pci_dev *dev, unsigned region,
-	unsigned size, int nr, const char *name)
+static void quirk_io_region(struct pci_dev *dev, int port,
+				unsigned size, int nr, const char *name)
 {
-	region &= ~(size-1);
-	if (region) {
-		struct pci_bus_region bus_region;
-		struct resource *res = dev->resource + nr;
+	u16 region;
+	struct pci_bus_region bus_region;
+	struct resource *res = dev->resource + nr;
 
-		res->name = pci_name(dev);
-		res->start = region;
-		res->end = region + size - 1;
-		res->flags = IORESOURCE_IO;
+	pci_read_config_word(dev, port, &region);
+	region &= ~(size - 1);
 
-		/* Convert from PCI bus to resource space.  */
-		bus_region.start = res->start;
-		bus_region.end = res->end;
-		pcibios_bus_to_resource(dev, res, &bus_region);
+	if (!region)
+		return;
 
-		if (pci_claim_resource(dev, nr) == 0)
-			dev_info(&dev->dev, "quirk: %pR claimed by %s\n",
-				 res, name);
-	}
-}	
+	res->name = pci_name(dev);
+	res->flags = IORESOURCE_IO;
+
+	/* Convert from PCI bus to resource space */
+	bus_region.start = region;
+	bus_region.end = region + size - 1;
+	pcibios_bus_to_resource(dev, res, &bus_region);
+
+	if (!pci_claim_resource(dev, nr))
+		dev_info(&dev->dev, "quirk: %pR claimed by %s\n", res, name);
+}
 
 /*
  *	ATI Northbridge setups MCE the processor if you even
@@ -374,12 +375,8 @@ DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_ATI,	PCI_DEVICE_ID_ATI_RS100,   quirk_ati_
  */
 static void quirk_ali7101_acpi(struct pci_dev *dev)
 {
-	u16 region;
-
-	pci_read_config_word(dev, 0xE0, &region);
-	quirk_io_region(dev, region, 64, PCI_BRIDGE_RESOURCES, "ali7101 ACPI");
-	pci_read_config_word(dev, 0xE2, &region);
-	quirk_io_region(dev, region, 32, PCI_BRIDGE_RESOURCES+1, "ali7101 SMB");
+	quirk_io_region(dev, 0xE0, 64, PCI_BRIDGE_RESOURCES, "ali7101 ACPI");
+	quirk_io_region(dev, 0xE2, 32, PCI_BRIDGE_RESOURCES+1, "ali7101 SMB");
 }
 DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_AL,	PCI_DEVICE_ID_AL_M7101,		quirk_ali7101_acpi);
 
@@ -442,12 +439,10 @@ static void piix4_mem_quirk(struct pci_dev *dev, const char *name, unsigned int
  */
 static void quirk_piix4_acpi(struct pci_dev *dev)
 {
-	u32 region, res_a;
+	u32 res_a;
 
-	pci_read_config_dword(dev, 0x40, &region);
-	quirk_io_region(dev, region, 64, PCI_BRIDGE_RESOURCES, "PIIX4 ACPI");
-	pci_read_config_dword(dev, 0x90, &region);
-	quirk_io_region(dev, region, 16, PCI_BRIDGE_RESOURCES+1, "PIIX4 SMB");
+	quirk_io_region(dev, 0x40, 64, PCI_BRIDGE_RESOURCES, "PIIX4 ACPI");
+	quirk_io_region(dev, 0x90, 16, PCI_BRIDGE_RESOURCES+1, "PIIX4 SMB");
 
 	/* Device resource A has enables for some of the other ones */
 	pci_read_config_dword(dev, 0x5c, &res_a);
@@ -491,7 +486,6 @@ DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_82443MX_3,	qui
  */
 static void quirk_ich4_lpc_acpi(struct pci_dev *dev)
 {
-	u32 region;
 	u8 enable;
 
 	/*
@@ -503,22 +497,14 @@ static void quirk_ich4_lpc_acpi(struct pci_dev *dev)
 	*/
 
 	pci_read_config_byte(dev, ICH_ACPI_CNTL, &enable);
-	if (enable & ICH4_ACPI_EN) {
-		pci_read_config_dword(dev, ICH_PMBASE, &region);
-		region &= PCI_BASE_ADDRESS_IO_MASK;
-		if (region >= PCIBIOS_MIN_IO)
-			quirk_io_region(dev, region, 128, PCI_BRIDGE_RESOURCES,
-					"ICH4 ACPI/GPIO/TCO");
-	}
+	if (enable & ICH4_ACPI_EN)
+		quirk_io_region(dev, ICH_PMBASE, 128, PCI_BRIDGE_RESOURCES,
+				 "ICH4 ACPI/GPIO/TCO");
 
 	pci_read_config_byte(dev, ICH4_GPIO_CNTL, &enable);
-	if (enable & ICH4_GPIO_EN) {
-		pci_read_config_dword(dev, ICH4_GPIOBASE, &region);
-		region &= PCI_BASE_ADDRESS_IO_MASK;
-		if (region >= PCIBIOS_MIN_IO)
-			quirk_io_region(dev, region, 64,
-					PCI_BRIDGE_RESOURCES + 1, "ICH4 GPIO");
-	}
+	if (enable & ICH4_GPIO_EN)
+		quirk_io_region(dev, ICH4_GPIOBASE, 64, PCI_BRIDGE_RESOURCES+1,
+				"ICH4 GPIO");
 }
 DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,    PCI_DEVICE_ID_INTEL_82801AA_0,		quirk_ich4_lpc_acpi);
 DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,    PCI_DEVICE_ID_INTEL_82801AB_0,		quirk_ich4_lpc_acpi);
@@ -533,26 +519,17 @@ DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,    PCI_DEVICE_ID_INTEL_ESB_1,		qui
 
 static void ich6_lpc_acpi_gpio(struct pci_dev *dev)
 {
-	u32 region;
 	u8 enable;
 
 	pci_read_config_byte(dev, ICH_ACPI_CNTL, &enable);
-	if (enable & ICH6_ACPI_EN) {
-		pci_read_config_dword(dev, ICH_PMBASE, &region);
-		region &= PCI_BASE_ADDRESS_IO_MASK;
-		if (region >= PCIBIOS_MIN_IO)
-			quirk_io_region(dev, region, 128, PCI_BRIDGE_RESOURCES,
-					"ICH6 ACPI/GPIO/TCO");
-	}
+	if (enable & ICH6_ACPI_EN)
+		quirk_io_region(dev, ICH_PMBASE, 128, PCI_BRIDGE_RESOURCES,
+				 "ICH6 ACPI/GPIO/TCO");
 
 	pci_read_config_byte(dev, ICH6_GPIO_CNTL, &enable);
-	if (enable & ICH6_GPIO_EN) {
-		pci_read_config_dword(dev, ICH6_GPIOBASE, &region);
-		region &= PCI_BASE_ADDRESS_IO_MASK;
-		if (region >= PCIBIOS_MIN_IO)
-			quirk_io_region(dev, region, 64,
-					PCI_BRIDGE_RESOURCES + 1, "ICH6 GPIO");
-	}
+	if (enable & ICH6_GPIO_EN)
+		quirk_io_region(dev, ICH6_GPIOBASE, 64, PCI_BRIDGE_RESOURCES+1,
+				"ICH6 GPIO");
 }
 
 static void ich6_lpc_generic_decode(struct pci_dev *dev, unsigned reg, const char *name, int dynsize)
@@ -650,13 +627,9 @@ DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,   PCI_DEVICE_ID_INTEL_ICH10_1, qui
  */
 static void quirk_vt82c586_acpi(struct pci_dev *dev)
 {
-	u32 region;
-
-	if (dev->revision & 0x10) {
-		pci_read_config_dword(dev, 0x48, &region);
-		region &= PCI_BASE_ADDRESS_IO_MASK;
-		quirk_io_region(dev, region, 256, PCI_BRIDGE_RESOURCES, "vt82c586 ACPI");
-	}
+	if (dev->revision & 0x10)
+		quirk_io_region(dev, 0x48, 256, PCI_BRIDGE_RESOURCES,
+				"vt82c586 ACPI");
 }
 DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_VIA,	PCI_DEVICE_ID_VIA_82C586_3,	quirk_vt82c586_acpi);
 
@@ -668,18 +641,12 @@ DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_VIA,	PCI_DEVICE_ID_VIA_82C586_3,	quirk_vt
  */
 static void quirk_vt82c686_acpi(struct pci_dev *dev)
 {
-	u16 hm;
-	u32 smb;
-
 	quirk_vt82c586_acpi(dev);
 
-	pci_read_config_word(dev, 0x70, &hm);
-	hm &= PCI_BASE_ADDRESS_IO_MASK;
-	quirk_io_region(dev, hm, 128, PCI_BRIDGE_RESOURCES + 1, "vt82c686 HW-mon");
+	quirk_io_region(dev, 0x70, 128, PCI_BRIDGE_RESOURCES+1,
+				 "vt82c686 HW-mon");
 
-	pci_read_config_dword(dev, 0x90, &smb);
-	smb &= PCI_BASE_ADDRESS_IO_MASK;
-	quirk_io_region(dev, smb, 16, PCI_BRIDGE_RESOURCES + 2, "vt82c686 SMB");
+	quirk_io_region(dev, 0x90, 16, PCI_BRIDGE_RESOURCES+2, "vt82c686 SMB");
 }
 DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_VIA,	PCI_DEVICE_ID_VIA_82C686_4,	quirk_vt82c686_acpi);
 
@@ -690,15 +657,8 @@ DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_VIA,	PCI_DEVICE_ID_VIA_82C686_4,	quirk_vt
  */
 static void quirk_vt8235_acpi(struct pci_dev *dev)
 {
-	u16 pm, smb;
-
-	pci_read_config_word(dev, 0x88, &pm);
-	pm &= PCI_BASE_ADDRESS_IO_MASK;
-	quirk_io_region(dev, pm, 128, PCI_BRIDGE_RESOURCES, "vt8235 PM");
-
-	pci_read_config_word(dev, 0xd0, &smb);
-	smb &= PCI_BASE_ADDRESS_IO_MASK;
-	quirk_io_region(dev, smb, 16, PCI_BRIDGE_RESOURCES + 1, "vt8235 SMB");
+	quirk_io_region(dev, 0x88, 128, PCI_BRIDGE_RESOURCES, "vt8235 PM");
+	quirk_io_region(dev, 0xd0, 16, PCI_BRIDGE_RESOURCES+1, "vt8235 SMB");
 }
 DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_VIA,	PCI_DEVICE_ID_VIA_8235,	quirk_vt8235_acpi);
 
@@ -2594,6 +2554,14 @@ static void quirk_msi_intx_disable_ati_bug(struct pci_dev *dev)
 		dev->dev_flags |= PCI_DEV_FLAGS_MSI_INTX_DISABLE_BUG;
 	pci_dev_put(p);
 }
+static void quirk_msi_intx_disable_qca_bug(struct pci_dev *dev)
+{
+	/* AR816X/AR817X/E210X MSI is fixed at HW level from revision 0x18 */
+	if (dev->revision < 0x18) {
+		dev_info(&dev->dev, "set MSI_INTX_DISABLE_BUG flag\n");
+		dev->dev_flags |= PCI_DEV_FLAGS_MSI_INTX_DISABLE_BUG;
+	}
+}
 DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_BROADCOM,
 			PCI_DEVICE_ID_TIGON3_5780,
 			quirk_msi_intx_disable_bug);
@@ -2643,6 +2611,16 @@ DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_ATTANSIC, 0x1073,
 			quirk_msi_intx_disable_bug);
 DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_ATTANSIC, 0x1083,
 			quirk_msi_intx_disable_bug);
+DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_ATTANSIC, 0x1090,
+			quirk_msi_intx_disable_qca_bug);
+DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_ATTANSIC, 0x1091,
+			quirk_msi_intx_disable_qca_bug);
+DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_ATTANSIC, 0x10a0,
+			quirk_msi_intx_disable_qca_bug);
+DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_ATTANSIC, 0x10a1,
+			quirk_msi_intx_disable_qca_bug);
+DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_ATTANSIC, 0xe091,
+			quirk_msi_intx_disable_qca_bug);
 #endif /* CONFIG_PCI_MSI */
 
 /* Allow manual resource allocation for PCI hotplug bridges
diff --git a/drivers/pci/remove.c b/drivers/pci/remove.c
index cc875e6ed15..8fc54b7327b 100644
--- a/drivers/pci/remove.c
+++ b/drivers/pci/remove.c
@@ -50,10 +50,8 @@ void pci_remove_bus(struct pci_bus *bus)
 	list_del(&bus->node);
 	pci_bus_release_busn_res(bus);
 	up_write(&pci_bus_sem);
-	if (!bus->is_added)
-		return;
-
 	pci_remove_legacy_files(bus);
+	pcibios_remove_bus(bus);
 	device_unregister(&bus->dev);
 }
 EXPORT_SYMBOL(pci_remove_bus);
diff --git a/drivers/pci/rom.c b/drivers/pci/rom.c
index b41ac7756a4..c5d0a08a874 100644
--- a/drivers/pci/rom.c
+++ b/drivers/pci/rom.c
@@ -100,27 +100,6 @@ size_t pci_get_rom_size(struct pci_dev *pdev, void __iomem *rom, size_t size)
 	return min((size_t)(image - rom), size);
 }
 
-static loff_t pci_find_rom(struct pci_dev *pdev, size_t *size)
-{
-	struct resource *res = &pdev->resource[PCI_ROM_RESOURCE];
-	loff_t start;
-
-	/* assign the ROM an address if it doesn't have one */
-	if (res->parent == NULL && pci_assign_resource(pdev, PCI_ROM_RESOURCE))
-		return 0;
-	start = pci_resource_start(pdev, PCI_ROM_RESOURCE);
-	*size = pci_resource_len(pdev, PCI_ROM_RESOURCE);
-
-	if (*size == 0)
-		return 0;
-
-	/* Enable ROM space decodes */
-	if (pci_enable_rom(pdev))
-		return 0;
-
-	return start;
-}
-
 /**
  * pci_map_rom - map a PCI ROM to kernel space
  * @pdev: pointer to pci device struct
@@ -135,7 +114,7 @@ static loff_t pci_find_rom(struct pci_dev *pdev, size_t *size)
 void __iomem *pci_map_rom(struct pci_dev *pdev, size_t *size)
 {
 	struct resource *res = &pdev->resource[PCI_ROM_RESOURCE];
-	loff_t start = 0;
+	loff_t start;
 	void __iomem *rom;
 
 	/*
@@ -154,21 +133,21 @@ void __iomem *pci_map_rom(struct pci_dev *pdev, size_t *size)
 			return (void __iomem *)(unsigned long)
 				pci_resource_start(pdev, PCI_ROM_RESOURCE);
 		} else {
-			start = pci_find_rom(pdev, size);
-		}
-	}
+			/* assign the ROM an address if it doesn't have one */
+			if (res->parent == NULL &&
+			    pci_assign_resource(pdev,PCI_ROM_RESOURCE))
+				return NULL;
+			start = pci_resource_start(pdev, PCI_ROM_RESOURCE);
+			*size = pci_resource_len(pdev, PCI_ROM_RESOURCE);
+			if (*size == 0)
+				return NULL;
 
-	/*
-	 * Some devices may provide ROMs via a source other than the BAR
-	 */
-	if (!start && pdev->rom && pdev->romlen) {
-		*size = pdev->romlen;
-		return phys_to_virt(pdev->rom);
+			/* Enable ROM space decodes */
+			if (pci_enable_rom(pdev))
+				return NULL;
+		}
 	}
 
-	if (!start)
-		return NULL;
-
 	rom = ioremap(start, *size);
 	if (!rom) {
 		/* restore enable if ioremap fails */
@@ -202,8 +181,7 @@ void pci_unmap_rom(struct pci_dev *pdev, void __iomem *rom)
 	if (res->flags & (IORESOURCE_ROM_COPY | IORESOURCE_ROM_BIOS_COPY))
 		return;
 
-	if (!pdev->rom || !pdev->romlen)
-		iounmap(rom);
+	iounmap(rom);
 
 	/* Disable again before continuing, leave enabled if pci=rom */
 	if (!(res->flags & (IORESOURCE_ROM_ENABLE | IORESOURCE_ROM_SHADOW)))
@@ -227,7 +205,24 @@ void pci_cleanup_rom(struct pci_dev *pdev)
 	}
 }
 
+/**
+ * pci_platform_rom - provides a pointer to any ROM image provided by the
+ * platform
+ * @pdev: pointer to pci device struct
+ * @size: pointer to receive size of pci window over ROM
+ */
+void __iomem *pci_platform_rom(struct pci_dev *pdev, size_t *size)
+{
+	if (pdev->rom && pdev->romlen) {
+		*size = pdev->romlen;
+		return phys_to_virt((phys_addr_t)pdev->rom);
+	}
+
+	return NULL;
+}
+
 EXPORT_SYMBOL(pci_map_rom);
 EXPORT_SYMBOL(pci_unmap_rom);
 EXPORT_SYMBOL_GPL(pci_enable_rom);
 EXPORT_SYMBOL_GPL(pci_disable_rom);
+EXPORT_SYMBOL(pci_platform_rom);
diff --git a/drivers/pci/setup-bus.c b/drivers/pci/setup-bus.c
index 7e8739e25b9..16abaaa1f83 100644
--- a/drivers/pci/setup-bus.c
+++ b/drivers/pci/setup-bus.c
@@ -1044,7 +1044,7 @@ handle_done:
 	;
 }
 
-void __ref __pci_bus_size_bridges(struct pci_bus *bus,
+static void __ref __pci_bus_size_bridges(struct pci_bus *bus,
 			struct list_head *realloc_head)
 {
 	struct pci_dev *dev;
@@ -1545,6 +1545,8 @@ again:
 
 enable_all:
 	retval = pci_reenable_device(bridge);
+	if (retval)
+		dev_err(&bridge->dev, "Error reenabling bridge (%d)\n", retval);
 	pci_set_master(bridge);
 	pci_enable_bridges(parent);
 }
diff --git a/drivers/pci/setup-res.c b/drivers/pci/setup-res.c
index 81b88bda793..07f2eddc09c 100644
--- a/drivers/pci/setup-res.c
+++ b/drivers/pci/setup-res.c
@@ -261,7 +261,6 @@ int pci_assign_resource(struct pci_dev *dev, int resno)
 {
 	struct resource *res = dev->resource + resno;
 	resource_size_t align, size;
-	struct pci_bus *bus;
 	int ret;
 
 	align = pci_resource_alignment(dev, res);
@@ -271,7 +270,6 @@ int pci_assign_resource(struct pci_dev *dev, int resno)
 		return -EINVAL;
 	}
 
-	bus = dev->bus;
 	size = resource_size(res);
 	ret = _pci_assign_resource(dev, resno, size, align);
 
diff --git a/drivers/pci/slot.c b/drivers/pci/slot.c
index ac6412fb8d6..c1e9284a677 100644
--- a/drivers/pci/slot.c
+++ b/drivers/pci/slot.c
@@ -377,14 +377,17 @@ void pci_hp_create_module_link(struct pci_slot *pci_slot)
 {
 	struct hotplug_slot *slot = pci_slot->hotplug;
 	struct kobject *kobj = NULL;
-	int no_warn;
+	int ret;
 
 	if (!slot || !slot->ops)
 		return;
 	kobj = kset_find_obj(module_kset, slot->ops->mod_name);
 	if (!kobj)
 		return;
-	no_warn = sysfs_create_link(&pci_slot->kobj, kobj, "module");
+	ret = sysfs_create_link(&pci_slot->kobj, kobj, "module");
+	if (ret)
+		dev_err(&pci_slot->bus->dev, "Error creating sysfs link (%d)\n",
+			ret);
 	kobject_put(kobj);
 }
 EXPORT_SYMBOL_GPL(pci_hp_create_module_link);
diff --git a/drivers/pinctrl/Kconfig b/drivers/pinctrl/Kconfig
index 34f51d2d90d..f910962baaa 100644
--- a/drivers/pinctrl/Kconfig
+++ b/drivers/pinctrl/Kconfig
@@ -106,20 +106,11 @@ config PINCTRL_LANTIQ
 	select PINMUX
 	select PINCONF
 
-config PINCTRL_PXA3xx
-	bool
-	select PINMUX
-
 config PINCTRL_FALCON
 	bool
 	depends on SOC_FALCON
 	depends on PINCTRL_LANTIQ
 
-config PINCTRL_MMP2
-	bool "MMP2 pin controller driver"
-	depends on ARCH_MMP
-	select PINCTRL_PXA3xx
-
 config PINCTRL_MXS
 	bool
 	select PINMUX
@@ -151,21 +142,12 @@ config PINCTRL_DB8540
 	bool "DB8540 pin controller driver"
 	depends on PINCTRL_NOMADIK && ARCH_U8500
 
-config PINCTRL_PXA168
-	bool "PXA168 pin controller driver"
-	depends on ARCH_MMP
-	select PINCTRL_PXA3xx
-
-config PINCTRL_PXA910
-	bool "PXA910 pin controller driver"
-	depends on ARCH_MMP
-	select PINCTRL_PXA3xx
-
 config PINCTRL_SINGLE
 	tristate "One-register-per-pin type device tree based pinctrl driver"
 	depends on OF
 	select PINMUX
 	select PINCONF
+	select GENERIC_PINCONF
 	help
 	  This selects the device tree based generic pinctrl driver.
 
@@ -226,6 +208,11 @@ config PINCTRL_EXYNOS5440
 	select PINMUX
 	select PINCONF
 
+config PINCTRL_S3C64XX
+	bool "Samsung S3C64XX SoC pinctrl driver"
+	depends on ARCH_S3C64XX
+	select PINCTRL_SAMSUNG
+
 source "drivers/pinctrl/mvebu/Kconfig"
 source "drivers/pinctrl/sh-pfc/Kconfig"
 source "drivers/pinctrl/spear/Kconfig"
diff --git a/drivers/pinctrl/Makefile b/drivers/pinctrl/Makefile
index f82cc5baf76..988279ae23c 100644
--- a/drivers/pinctrl/Makefile
+++ b/drivers/pinctrl/Makefile
@@ -21,9 +21,7 @@ obj-$(CONFIG_PINCTRL_IMX35)	+= pinctrl-imx35.o
 obj-$(CONFIG_PINCTRL_IMX51)	+= pinctrl-imx51.o
 obj-$(CONFIG_PINCTRL_IMX53)	+= pinctrl-imx53.o
 obj-$(CONFIG_PINCTRL_IMX6Q)	+= pinctrl-imx6q.o
-obj-$(CONFIG_PINCTRL_PXA3xx)	+= pinctrl-pxa3xx.o
 obj-$(CONFIG_PINCTRL_FALCON)	+= pinctrl-falcon.o
-obj-$(CONFIG_PINCTRL_MMP2)	+= pinctrl-mmp2.o
 obj-$(CONFIG_PINCTRL_MXS)	+= pinctrl-mxs.o
 obj-$(CONFIG_PINCTRL_IMX23)	+= pinctrl-imx23.o
 obj-$(CONFIG_PINCTRL_IMX28)	+= pinctrl-imx28.o
@@ -31,8 +29,6 @@ obj-$(CONFIG_PINCTRL_NOMADIK)	+= pinctrl-nomadik.o
 obj-$(CONFIG_PINCTRL_STN8815)	+= pinctrl-nomadik-stn8815.o
 obj-$(CONFIG_PINCTRL_DB8500)	+= pinctrl-nomadik-db8500.o
 obj-$(CONFIG_PINCTRL_DB8540)	+= pinctrl-nomadik-db8540.o
-obj-$(CONFIG_PINCTRL_PXA168)	+= pinctrl-pxa168.o
-obj-$(CONFIG_PINCTRL_PXA910)	+= pinctrl-pxa910.o
 obj-$(CONFIG_PINCTRL_SINGLE)	+= pinctrl-single.o
 obj-$(CONFIG_PINCTRL_SIRF)	+= pinctrl-sirf.o
 obj-$(CONFIG_PINCTRL_SUNXI)	+= pinctrl-sunxi.o
@@ -45,6 +41,7 @@ obj-$(CONFIG_PINCTRL_COH901)	+= pinctrl-coh901.o
 obj-$(CONFIG_PINCTRL_SAMSUNG)	+= pinctrl-samsung.o
 obj-$(CONFIG_PINCTRL_EXYNOS)	+= pinctrl-exynos.o
 obj-$(CONFIG_PINCTRL_EXYNOS5440)	+= pinctrl-exynos5440.o
+obj-$(CONFIG_PINCTRL_S3C64XX)	+= pinctrl-s3c64xx.o
 obj-$(CONFIG_PINCTRL_XWAY)	+= pinctrl-xway.o
 obj-$(CONFIG_PINCTRL_LANTIQ)	+= pinctrl-lantiq.o
 
diff --git a/drivers/pinctrl/core.c b/drivers/pinctrl/core.c
index b0de6e7f1fd..c3d222ed39a 100644
--- a/drivers/pinctrl/core.c
+++ b/drivers/pinctrl/core.c
@@ -27,6 +27,11 @@
 #include <linux/pinctrl/consumer.h>
 #include <linux/pinctrl/pinctrl.h>
 #include <linux/pinctrl/machine.h>
+
+#ifdef CONFIG_GPIOLIB
+#include <asm-generic/gpio.h>
+#endif
+
 #include "core.h"
 #include "devicetree.h"
 #include "pinmux.h"
@@ -35,11 +40,17 @@
 
 static bool pinctrl_dummy_state;
 
-/* Mutex taken by all entry points */
-DEFINE_MUTEX(pinctrl_mutex);
+/* Mutex taken to protect pinctrl_list */
+DEFINE_MUTEX(pinctrl_list_mutex);
+
+/* Mutex taken to protect pinctrl_maps */
+DEFINE_MUTEX(pinctrl_maps_mutex);
+
+/* Mutex taken to protect pinctrldev_list */
+DEFINE_MUTEX(pinctrldev_list_mutex);
 
 /* Global list of pin control devices (struct pinctrl_dev) */
-LIST_HEAD(pinctrldev_list);
+static LIST_HEAD(pinctrldev_list);
 
 /* List of pin controller handles (struct pinctrl) */
 static LIST_HEAD(pinctrl_list);
@@ -106,6 +117,23 @@ struct pinctrl_dev *get_pinctrl_dev_from_devname(const char *devname)
 	return found ? pctldev : NULL;
 }
 
+struct pinctrl_dev *get_pinctrl_dev_from_of_node(struct device_node *np)
+{
+	struct pinctrl_dev *pctldev;
+
+	mutex_lock(&pinctrldev_list_mutex);
+
+	list_for_each_entry(pctldev, &pinctrldev_list, node)
+		if (pctldev->dev->of_node == np) {
+			mutex_unlock(&pinctrldev_list_mutex);
+			return pctldev;
+		}
+
+	mutex_lock(&pinctrldev_list_mutex);
+
+	return NULL;
+}
+
 /**
  * pin_get_from_name() - look up a pin number from a name
  * @pctldev: the pin control device to lookup the pin on
@@ -165,9 +193,9 @@ bool pin_is_valid(struct pinctrl_dev *pctldev, int pin)
 	if (pin < 0)
 		return false;
 
-	mutex_lock(&pinctrl_mutex);
+	mutex_lock(&pctldev->mutex);
 	pindesc = pin_desc_get(pctldev, pin);
-	mutex_unlock(&pinctrl_mutex);
+	mutex_unlock(&pctldev->mutex);
 
 	return pindesc != NULL;
 }
@@ -264,19 +292,58 @@ pinctrl_match_gpio_range(struct pinctrl_dev *pctldev, unsigned gpio)
 {
 	struct pinctrl_gpio_range *range = NULL;
 
+	mutex_lock(&pctldev->mutex);
 	/* Loop over the ranges */
 	list_for_each_entry(range, &pctldev->gpio_ranges, node) {
 		/* Check if we're in the valid range */
 		if (gpio >= range->base &&
 		    gpio < range->base + range->npins) {
+			mutex_unlock(&pctldev->mutex);
 			return range;
 		}
 	}
-
+	mutex_unlock(&pctldev->mutex);
 	return NULL;
 }
 
 /**
+ * pinctrl_ready_for_gpio_range() - check if other GPIO pins of
+ * the same GPIO chip are in range
+ * @gpio: gpio pin to check taken from the global GPIO pin space
+ *
+ * This function is complement of pinctrl_match_gpio_range(). If the return
+ * value of pinctrl_match_gpio_range() is NULL, this function could be used
+ * to check whether pinctrl device is ready or not. Maybe some GPIO pins
+ * of the same GPIO chip don't have back-end pinctrl interface.
+ * If the return value is true, it means that pinctrl device is ready & the
+ * certain GPIO pin doesn't have back-end pinctrl device. If the return value
+ * is false, it means that pinctrl device may not be ready.
+ */
+#ifdef CONFIG_GPIOLIB
+static bool pinctrl_ready_for_gpio_range(unsigned gpio)
+{
+	struct pinctrl_dev *pctldev;
+	struct pinctrl_gpio_range *range = NULL;
+	struct gpio_chip *chip = gpio_to_chip(gpio);
+
+	/* Loop over the pin controllers */
+	list_for_each_entry(pctldev, &pinctrldev_list, node) {
+		/* Loop over the ranges */
+		list_for_each_entry(range, &pctldev->gpio_ranges, node) {
+			/* Check if any gpio range overlapped with gpio chip */
+			if (range->base + range->npins - 1 < chip->base ||
+			    range->base > chip->base + chip->ngpio - 1)
+				continue;
+			return true;
+		}
+	}
+	return false;
+}
+#else
+static bool pinctrl_ready_for_gpio_range(unsigned gpio) { return true; }
+#endif
+
+/**
  * pinctrl_get_device_gpio_range() - find device for GPIO range
  * @gpio: the pin to locate the pin controller for
  * @outdev: the pin control device if found
@@ -319,9 +386,9 @@ static int pinctrl_get_device_gpio_range(unsigned gpio,
 void pinctrl_add_gpio_range(struct pinctrl_dev *pctldev,
 			    struct pinctrl_gpio_range *range)
 {
-	mutex_lock(&pinctrl_mutex);
+	mutex_lock(&pctldev->mutex);
 	list_add_tail(&range->node, &pctldev->gpio_ranges);
-	mutex_unlock(&pinctrl_mutex);
+	mutex_unlock(&pctldev->mutex);
 }
 EXPORT_SYMBOL_GPL(pinctrl_add_gpio_range);
 
@@ -339,17 +406,25 @@ EXPORT_SYMBOL_GPL(pinctrl_add_gpio_ranges);
 struct pinctrl_dev *pinctrl_find_and_add_gpio_range(const char *devname,
 		struct pinctrl_gpio_range *range)
 {
-	struct pinctrl_dev *pctldev = get_pinctrl_dev_from_devname(devname);
+	struct pinctrl_dev *pctldev;
+
+	mutex_lock(&pinctrldev_list_mutex);
+
+	pctldev = get_pinctrl_dev_from_devname(devname);
 
 	/*
 	 * If we can't find this device, let's assume that is because
 	 * it has not probed yet, so the driver trying to register this
 	 * range need to defer probing.
 	 */
-	if (!pctldev)
+	if (!pctldev) {
+		mutex_unlock(&pinctrldev_list_mutex);
 		return ERR_PTR(-EPROBE_DEFER);
-
+	}
 	pinctrl_add_gpio_range(pctldev, range);
+
+	mutex_unlock(&pinctrldev_list_mutex);
+
 	return pctldev;
 }
 EXPORT_SYMBOL_GPL(pinctrl_find_and_add_gpio_range);
@@ -365,14 +440,17 @@ pinctrl_find_gpio_range_from_pin(struct pinctrl_dev *pctldev,
 {
 	struct pinctrl_gpio_range *range = NULL;
 
+	mutex_lock(&pctldev->mutex);
 	/* Loop over the ranges */
 	list_for_each_entry(range, &pctldev->gpio_ranges, node) {
 		/* Check if we're in the valid range */
 		if (pin >= range->pin_base &&
 		    pin < range->pin_base + range->npins) {
+			mutex_unlock(&pctldev->mutex);
 			return range;
 		}
 	}
+	mutex_unlock(&pctldev->mutex);
 
 	return NULL;
 }
@@ -386,9 +464,9 @@ EXPORT_SYMBOL_GPL(pinctrl_find_gpio_range_from_pin);
 void pinctrl_remove_gpio_range(struct pinctrl_dev *pctldev,
 			       struct pinctrl_gpio_range *range)
 {
-	mutex_lock(&pinctrl_mutex);
+	mutex_lock(&pctldev->mutex);
 	list_del(&range->node);
-	mutex_unlock(&pinctrl_mutex);
+	mutex_unlock(&pctldev->mutex);
 }
 EXPORT_SYMBOL_GPL(pinctrl_remove_gpio_range);
 
@@ -439,11 +517,13 @@ int pinctrl_request_gpio(unsigned gpio)
 	int ret;
 	int pin;
 
-	mutex_lock(&pinctrl_mutex);
+	mutex_lock(&pinctrldev_list_mutex);
 
 	ret = pinctrl_get_device_gpio_range(gpio, &pctldev, &range);
 	if (ret) {
-		mutex_unlock(&pinctrl_mutex);
+		if (pinctrl_ready_for_gpio_range(gpio))
+			ret = 0;
+		mutex_unlock(&pinctrldev_list_mutex);
 		return ret;
 	}
 
@@ -452,7 +532,7 @@ int pinctrl_request_gpio(unsigned gpio)
 
 	ret = pinmux_request_gpio(pctldev, range, pin, gpio);
 
-	mutex_unlock(&pinctrl_mutex);
+	mutex_unlock(&pinctrldev_list_mutex);
 	return ret;
 }
 EXPORT_SYMBOL_GPL(pinctrl_request_gpio);
@@ -472,20 +552,22 @@ void pinctrl_free_gpio(unsigned gpio)
 	int ret;
 	int pin;
 
-	mutex_lock(&pinctrl_mutex);
+	mutex_lock(&pinctrldev_list_mutex);
 
 	ret = pinctrl_get_device_gpio_range(gpio, &pctldev, &range);
 	if (ret) {
-		mutex_unlock(&pinctrl_mutex);
+		mutex_unlock(&pinctrldev_list_mutex);
 		return;
 	}
+	mutex_lock(&pctldev->mutex);
 
 	/* Convert to the pin controllers number space */
 	pin = gpio - range->base + range->pin_base;
 
 	pinmux_free_gpio(pctldev, pin, range);
 
-	mutex_unlock(&pinctrl_mutex);
+	mutex_unlock(&pctldev->mutex);
+	mutex_unlock(&pinctrldev_list_mutex);
 }
 EXPORT_SYMBOL_GPL(pinctrl_free_gpio);
 
@@ -496,14 +578,24 @@ static int pinctrl_gpio_direction(unsigned gpio, bool input)
 	int ret;
 	int pin;
 
+	mutex_lock(&pinctrldev_list_mutex);
+
 	ret = pinctrl_get_device_gpio_range(gpio, &pctldev, &range);
-	if (ret)
+	if (ret) {
+		mutex_unlock(&pinctrldev_list_mutex);
 		return ret;
+	}
+
+	mutex_lock(&pctldev->mutex);
 
 	/* Convert to the pin controllers number space */
 	pin = gpio - range->base + range->pin_base;
+	ret = pinmux_gpio_direction(pctldev, range, pin, input);
+
+	mutex_unlock(&pctldev->mutex);
+	mutex_unlock(&pinctrldev_list_mutex);
 
-	return pinmux_gpio_direction(pctldev, range, pin, input);
+	return ret;
 }
 
 /**
@@ -516,11 +608,7 @@ static int pinctrl_gpio_direction(unsigned gpio, bool input)
  */
 int pinctrl_gpio_direction_input(unsigned gpio)
 {
-	int ret;
-	mutex_lock(&pinctrl_mutex);
-	ret = pinctrl_gpio_direction(gpio, true);
-	mutex_unlock(&pinctrl_mutex);
-	return ret;
+	return pinctrl_gpio_direction(gpio, true);
 }
 EXPORT_SYMBOL_GPL(pinctrl_gpio_direction_input);
 
@@ -534,11 +622,7 @@ EXPORT_SYMBOL_GPL(pinctrl_gpio_direction_input);
  */
 int pinctrl_gpio_direction_output(unsigned gpio)
 {
-	int ret;
-	mutex_lock(&pinctrl_mutex);
-	ret = pinctrl_gpio_direction(gpio, false);
-	mutex_unlock(&pinctrl_mutex);
-	return ret;
+	return pinctrl_gpio_direction(gpio, false);
 }
 EXPORT_SYMBOL_GPL(pinctrl_gpio_direction_output);
 
@@ -641,14 +725,18 @@ static struct pinctrl *find_pinctrl(struct device *dev)
 {
 	struct pinctrl *p;
 
+	mutex_lock(&pinctrl_list_mutex);
 	list_for_each_entry(p, &pinctrl_list, node)
-		if (p->dev == dev)
+		if (p->dev == dev) {
+			mutex_unlock(&pinctrl_list_mutex);
 			return p;
+		}
 
+	mutex_unlock(&pinctrl_list_mutex);
 	return NULL;
 }
 
-static void pinctrl_put_locked(struct pinctrl *p, bool inlist);
+static void pinctrl_free(struct pinctrl *p, bool inlist);
 
 static struct pinctrl *create_pinctrl(struct device *dev)
 {
@@ -681,6 +769,7 @@ static struct pinctrl *create_pinctrl(struct device *dev)
 
 	devname = dev_name(dev);
 
+	mutex_lock(&pinctrl_maps_mutex);
 	/* Iterate over the pin control maps to locate the right ones */
 	for_each_maps(maps_node, i, map) {
 		/* Map must be for this device */
@@ -702,13 +791,16 @@ static struct pinctrl *create_pinctrl(struct device *dev)
 		 * an -EPROBE_DEFER later, as that is the worst case.
 		 */
 		if (ret == -EPROBE_DEFER) {
-			pinctrl_put_locked(p, false);
+			pinctrl_free(p, false);
+			mutex_unlock(&pinctrl_maps_mutex);
 			return ERR_PTR(ret);
 		}
 	}
+	mutex_unlock(&pinctrl_maps_mutex);
+
 	if (ret < 0) {
 		/* If some other error than deferral occured, return here */
-		pinctrl_put_locked(p, false);
+		pinctrl_free(p, false);
 		return ERR_PTR(ret);
 	}
 
@@ -720,7 +812,11 @@ static struct pinctrl *create_pinctrl(struct device *dev)
 	return p;
 }
 
-static struct pinctrl *pinctrl_get_locked(struct device *dev)
+/**
+ * pinctrl_get() - retrieves the pinctrl handle for a device
+ * @dev: the device to obtain the handle for
+ */
+struct pinctrl *pinctrl_get(struct device *dev)
 {
 	struct pinctrl *p;
 
@@ -741,43 +837,35 @@ static struct pinctrl *pinctrl_get_locked(struct device *dev)
 
 	return create_pinctrl(dev);
 }
+EXPORT_SYMBOL_GPL(pinctrl_get);
 
-/**
- * pinctrl_get() - retrieves the pinctrl handle for a device
- * @dev: the device to obtain the handle for
- */
-struct pinctrl *pinctrl_get(struct device *dev)
+static void pinctrl_free_setting(bool disable_setting,
+				 struct pinctrl_setting *setting)
 {
-	struct pinctrl *p;
-
-	mutex_lock(&pinctrl_mutex);
-	p = pinctrl_get_locked(dev);
-	mutex_unlock(&pinctrl_mutex);
-
-	return p;
+	switch (setting->type) {
+	case PIN_MAP_TYPE_MUX_GROUP:
+		if (disable_setting)
+			pinmux_disable_setting(setting);
+		pinmux_free_setting(setting);
+		break;
+	case PIN_MAP_TYPE_CONFIGS_PIN:
+	case PIN_MAP_TYPE_CONFIGS_GROUP:
+		pinconf_free_setting(setting);
+		break;
+	default:
+		break;
+	}
 }
-EXPORT_SYMBOL_GPL(pinctrl_get);
 
-static void pinctrl_put_locked(struct pinctrl *p, bool inlist)
+static void pinctrl_free(struct pinctrl *p, bool inlist)
 {
 	struct pinctrl_state *state, *n1;
 	struct pinctrl_setting *setting, *n2;
 
+	mutex_lock(&pinctrl_list_mutex);
 	list_for_each_entry_safe(state, n1, &p->states, node) {
 		list_for_each_entry_safe(setting, n2, &state->settings, node) {
-			switch (setting->type) {
-			case PIN_MAP_TYPE_MUX_GROUP:
-				if (state == p->state)
-					pinmux_disable_setting(setting);
-				pinmux_free_setting(setting);
-				break;
-			case PIN_MAP_TYPE_CONFIGS_PIN:
-			case PIN_MAP_TYPE_CONFIGS_GROUP:
-				pinconf_free_setting(setting);
-				break;
-			default:
-				break;
-			}
+			pinctrl_free_setting(state == p->state, setting);
 			list_del(&setting->node);
 			kfree(setting);
 		}
@@ -790,6 +878,7 @@ static void pinctrl_put_locked(struct pinctrl *p, bool inlist)
 	if (inlist)
 		list_del(&p->node);
 	kfree(p);
+	mutex_unlock(&pinctrl_list_mutex);
 }
 
 /**
@@ -800,7 +889,7 @@ static void pinctrl_release(struct kref *kref)
 {
 	struct pinctrl *p = container_of(kref, struct pinctrl, users);
 
-	pinctrl_put_locked(p, true);
+	pinctrl_free(p, true);
 }
 
 /**
@@ -809,14 +898,17 @@ static void pinctrl_release(struct kref *kref)
  */
 void pinctrl_put(struct pinctrl *p)
 {
-	mutex_lock(&pinctrl_mutex);
 	kref_put(&p->users, pinctrl_release);
-	mutex_unlock(&pinctrl_mutex);
 }
 EXPORT_SYMBOL_GPL(pinctrl_put);
 
-static struct pinctrl_state *pinctrl_lookup_state_locked(struct pinctrl *p,
-							 const char *name)
+/**
+ * pinctrl_lookup_state() - retrieves a state handle from a pinctrl handle
+ * @p: the pinctrl handle to retrieve the state from
+ * @name: the state name to retrieve
+ */
+struct pinctrl_state *pinctrl_lookup_state(struct pinctrl *p,
+						 const char *name)
 {
 	struct pinctrl_state *state;
 
@@ -833,28 +925,17 @@ static struct pinctrl_state *pinctrl_lookup_state_locked(struct pinctrl *p,
 
 	return state;
 }
+EXPORT_SYMBOL_GPL(pinctrl_lookup_state);
 
 /**
- * pinctrl_lookup_state() - retrieves a state handle from a pinctrl handle
- * @p: the pinctrl handle to retrieve the state from
- * @name: the state name to retrieve
+ * pinctrl_select_state() - select/activate/program a pinctrl state to HW
+ * @p: the pinctrl handle for the device that requests configuration
+ * @state: the state handle to select/activate/program
  */
-struct pinctrl_state *pinctrl_lookup_state(struct pinctrl *p, const char *name)
-{
-	struct pinctrl_state *s;
-
-	mutex_lock(&pinctrl_mutex);
-	s = pinctrl_lookup_state_locked(p, name);
-	mutex_unlock(&pinctrl_mutex);
-
-	return s;
-}
-EXPORT_SYMBOL_GPL(pinctrl_lookup_state);
-
-static int pinctrl_select_state_locked(struct pinctrl *p,
-				       struct pinctrl_state *state)
+int pinctrl_select_state(struct pinctrl *p, struct pinctrl_state *state)
 {
 	struct pinctrl_setting *setting, *setting2;
+	struct pinctrl_state *old_state = p->state;
 	int ret;
 
 	if (p->state == state)
@@ -888,7 +969,7 @@ static int pinctrl_select_state_locked(struct pinctrl *p,
 		}
 	}
 
-	p->state = state;
+	p->state = NULL;
 
 	/* Apply all the settings for the new state */
 	list_for_each_entry(setting, &state->settings, node) {
@@ -904,27 +985,36 @@ static int pinctrl_select_state_locked(struct pinctrl *p,
 			ret = -EINVAL;
 			break;
 		}
+
 		if (ret < 0) {
-			/* FIXME: Difficult to return to prev state */
-			return ret;
+			goto unapply_new_state;
 		}
 	}
 
+	p->state = state;
+
 	return 0;
-}
 
-/**
- * pinctrl_select() - select/activate/program a pinctrl state to HW
- * @p: the pinctrl handle for the device that requests configuratio
- * @state: the state handle to select/activate/program
- */
-int pinctrl_select_state(struct pinctrl *p, struct pinctrl_state *state)
-{
-	int ret;
+unapply_new_state:
+	dev_err(p->dev, "Error applying setting, reverse things back\n");
 
-	mutex_lock(&pinctrl_mutex);
-	ret = pinctrl_select_state_locked(p, state);
-	mutex_unlock(&pinctrl_mutex);
+	list_for_each_entry(setting2, &state->settings, node) {
+		if (&setting2->node == &setting->node)
+			break;
+		/*
+		 * All we can do here is pinmux_disable_setting.
+		 * That means that some pins are muxed differently now
+		 * than they were before applying the setting (We can't
+		 * "unmux a pin"!), but it's not a big deal since the pins
+		 * are free to be muxed by another apply_setting.
+		 */
+		if (setting2->type == PIN_MAP_TYPE_MUX_GROUP)
+			pinmux_disable_setting(setting2);
+	}
+
+	/* There's no infinite recursive loop here because p->state is NULL */
+	if (old_state)
+		pinctrl_select_state(p, old_state);
 
 	return ret;
 }
@@ -979,9 +1069,8 @@ static int devm_pinctrl_match(struct device *dev, void *res, void *data)
  */
 void devm_pinctrl_put(struct pinctrl *p)
 {
-	WARN_ON(devres_destroy(p->dev, devm_pinctrl_release,
+	WARN_ON(devres_release(p->dev, devm_pinctrl_release,
 			       devm_pinctrl_match, p));
-	pinctrl_put(p);
 }
 EXPORT_SYMBOL_GPL(devm_pinctrl_put);
 
@@ -1055,10 +1144,10 @@ int pinctrl_register_map(struct pinctrl_map const *maps, unsigned num_maps,
 	}
 
 	if (!locked)
-		mutex_lock(&pinctrl_mutex);
+		mutex_lock(&pinctrl_maps_mutex);
 	list_add_tail(&maps_node->node, &pinctrl_maps);
 	if (!locked)
-		mutex_unlock(&pinctrl_mutex);
+		mutex_unlock(&pinctrl_maps_mutex);
 
 	return 0;
 }
@@ -1080,12 +1169,15 @@ void pinctrl_unregister_map(struct pinctrl_map const *map)
 {
 	struct pinctrl_maps *maps_node;
 
+	mutex_lock(&pinctrl_maps_mutex);
 	list_for_each_entry(maps_node, &pinctrl_maps, node) {
 		if (maps_node->maps == map) {
 			list_del(&maps_node->node);
+			mutex_unlock(&pinctrl_maps_mutex);
 			return;
 		}
 	}
+	mutex_unlock(&pinctrl_maps_mutex);
 }
 
 /**
@@ -1122,7 +1214,7 @@ static int pinctrl_pins_show(struct seq_file *s, void *what)
 
 	seq_printf(s, "registered pins: %d\n", pctldev->desc->npins);
 
-	mutex_lock(&pinctrl_mutex);
+	mutex_lock(&pctldev->mutex);
 
 	/* The pin number can be retrived from the pin controller descriptor */
 	for (i = 0; i < pctldev->desc->npins; i++) {
@@ -1144,7 +1236,7 @@ static int pinctrl_pins_show(struct seq_file *s, void *what)
 		seq_puts(s, "\n");
 	}
 
-	mutex_unlock(&pinctrl_mutex);
+	mutex_unlock(&pctldev->mutex);
 
 	return 0;
 }
@@ -1155,8 +1247,9 @@ static int pinctrl_groups_show(struct seq_file *s, void *what)
 	const struct pinctrl_ops *ops = pctldev->desc->pctlops;
 	unsigned ngroups, selector = 0;
 
+	mutex_lock(&pctldev->mutex);
+
 	ngroups = ops->get_groups_count(pctldev);
-	mutex_lock(&pinctrl_mutex);
 
 	seq_puts(s, "registered pin groups:\n");
 	while (selector < ngroups) {
@@ -1177,7 +1270,7 @@ static int pinctrl_groups_show(struct seq_file *s, void *what)
 			for (i = 0; i < num_pins; i++) {
 				pname = pin_get_name(pctldev, pins[i]);
 				if (WARN_ON(!pname)) {
-					mutex_unlock(&pinctrl_mutex);
+					mutex_unlock(&pctldev->mutex);
 					return -EINVAL;
 				}
 				seq_printf(s, "pin %d (%s)\n", pins[i], pname);
@@ -1187,7 +1280,7 @@ static int pinctrl_groups_show(struct seq_file *s, void *what)
 		selector++;
 	}
 
-	mutex_unlock(&pinctrl_mutex);
+	mutex_unlock(&pctldev->mutex);
 
 	return 0;
 }
@@ -1199,7 +1292,7 @@ static int pinctrl_gpioranges_show(struct seq_file *s, void *what)
 
 	seq_puts(s, "GPIO ranges handled:\n");
 
-	mutex_lock(&pinctrl_mutex);
+	mutex_lock(&pctldev->mutex);
 
 	/* Loop over the ranges */
 	list_for_each_entry(range, &pctldev->gpio_ranges, node) {
@@ -1210,7 +1303,7 @@ static int pinctrl_gpioranges_show(struct seq_file *s, void *what)
 			   (range->pin_base + range->npins - 1));
 	}
 
-	mutex_unlock(&pinctrl_mutex);
+	mutex_unlock(&pctldev->mutex);
 
 	return 0;
 }
@@ -1221,7 +1314,7 @@ static int pinctrl_devices_show(struct seq_file *s, void *what)
 
 	seq_puts(s, "name [pinmux] [pinconf]\n");
 
-	mutex_lock(&pinctrl_mutex);
+	mutex_lock(&pinctrldev_list_mutex);
 
 	list_for_each_entry(pctldev, &pinctrldev_list, node) {
 		seq_printf(s, "%s ", pctldev->desc->name);
@@ -1236,7 +1329,7 @@ static int pinctrl_devices_show(struct seq_file *s, void *what)
 		seq_puts(s, "\n");
 	}
 
-	mutex_unlock(&pinctrl_mutex);
+	mutex_unlock(&pinctrldev_list_mutex);
 
 	return 0;
 }
@@ -1265,8 +1358,7 @@ static int pinctrl_maps_show(struct seq_file *s, void *what)
 
 	seq_puts(s, "Pinctrl maps:\n");
 
-	mutex_lock(&pinctrl_mutex);
-
+	mutex_lock(&pinctrl_maps_mutex);
 	for_each_maps(maps_node, i, map) {
 		seq_printf(s, "device %s\nstate %s\ntype %s (%d)\n",
 			   map->dev_name, map->name, map_type(map->type),
@@ -1290,8 +1382,7 @@ static int pinctrl_maps_show(struct seq_file *s, void *what)
 
 		seq_printf(s, "\n");
 	}
-
-	mutex_unlock(&pinctrl_mutex);
+	mutex_unlock(&pinctrl_maps_mutex);
 
 	return 0;
 }
@@ -1304,7 +1395,7 @@ static int pinctrl_show(struct seq_file *s, void *what)
 
 	seq_puts(s, "Requested pin control handlers their pinmux maps:\n");
 
-	mutex_lock(&pinctrl_mutex);
+	mutex_lock(&pinctrl_list_mutex);
 
 	list_for_each_entry(p, &pinctrl_list, node) {
 		seq_printf(s, "device: %s current state: %s\n",
@@ -1336,7 +1427,7 @@ static int pinctrl_show(struct seq_file *s, void *what)
 		}
 	}
 
-	mutex_unlock(&pinctrl_mutex);
+	mutex_unlock(&pinctrl_list_mutex);
 
 	return 0;
 }
@@ -1522,6 +1613,7 @@ struct pinctrl_dev *pinctrl_register(struct pinctrl_desc *pctldesc,
 	INIT_RADIX_TREE(&pctldev->pin_desc_tree, GFP_KERNEL);
 	INIT_LIST_HEAD(&pctldev->gpio_ranges);
 	pctldev->dev = dev;
+	mutex_init(&pctldev->mutex);
 
 	/* check core ops for sanity */
 	if (pinctrl_check_ops(pctldev)) {
@@ -1551,38 +1643,37 @@ struct pinctrl_dev *pinctrl_register(struct pinctrl_desc *pctldesc,
 		goto out_err;
 	}
 
-	mutex_lock(&pinctrl_mutex);
-
+	mutex_lock(&pinctrldev_list_mutex);
 	list_add_tail(&pctldev->node, &pinctrldev_list);
+	mutex_unlock(&pinctrldev_list_mutex);
+
+	pctldev->p = pinctrl_get(pctldev->dev);
 
-	pctldev->p = pinctrl_get_locked(pctldev->dev);
 	if (!IS_ERR(pctldev->p)) {
 		pctldev->hog_default =
-			pinctrl_lookup_state_locked(pctldev->p,
-						    PINCTRL_STATE_DEFAULT);
+			pinctrl_lookup_state(pctldev->p, PINCTRL_STATE_DEFAULT);
 		if (IS_ERR(pctldev->hog_default)) {
 			dev_dbg(dev, "failed to lookup the default state\n");
 		} else {
-			if (pinctrl_select_state_locked(pctldev->p,
+			if (pinctrl_select_state(pctldev->p,
 						pctldev->hog_default))
 				dev_err(dev,
 					"failed to select default state\n");
 		}
 
 		pctldev->hog_sleep =
-			pinctrl_lookup_state_locked(pctldev->p,
+			pinctrl_lookup_state(pctldev->p,
 						    PINCTRL_STATE_SLEEP);
 		if (IS_ERR(pctldev->hog_sleep))
 			dev_dbg(dev, "failed to lookup the sleep state\n");
 	}
 
-	mutex_unlock(&pinctrl_mutex);
-
 	pinctrl_init_device_debugfs(pctldev);
 
 	return pctldev;
 
 out_err:
+	mutex_destroy(&pctldev->mutex);
 	kfree(pctldev);
 	return NULL;
 }
@@ -1600,12 +1691,13 @@ void pinctrl_unregister(struct pinctrl_dev *pctldev)
 	if (pctldev == NULL)
 		return;
 
-	pinctrl_remove_device_debugfs(pctldev);
+	mutex_lock(&pinctrldev_list_mutex);
+	mutex_lock(&pctldev->mutex);
 
-	mutex_lock(&pinctrl_mutex);
+	pinctrl_remove_device_debugfs(pctldev);
 
 	if (!IS_ERR(pctldev->p))
-		pinctrl_put_locked(pctldev->p, true);
+		pinctrl_put(pctldev->p);
 
 	/* TODO: check that no pinmuxes are still active? */
 	list_del(&pctldev->node);
@@ -1616,9 +1708,10 @@ void pinctrl_unregister(struct pinctrl_dev *pctldev)
 	list_for_each_entry_safe(range, n, &pctldev->gpio_ranges, node)
 		list_del(&range->node);
 
+	mutex_unlock(&pctldev->mutex);
+	mutex_destroy(&pctldev->mutex);
 	kfree(pctldev);
-
-	mutex_unlock(&pinctrl_mutex);
+	mutex_unlock(&pinctrldev_list_mutex);
 }
 EXPORT_SYMBOL_GPL(pinctrl_unregister);
 
diff --git a/drivers/pinctrl/core.h b/drivers/pinctrl/core.h
index ee72f1f6d86..75476b3d87d 100644
--- a/drivers/pinctrl/core.h
+++ b/drivers/pinctrl/core.h
@@ -33,6 +33,7 @@ struct pinctrl_gpio_range;
  * @p: result of pinctrl_get() for this device
  * @hog_default: default state for pins hogged by this device
  * @hog_sleep: sleep state for pins hogged by this device
+ * @mutex: mutex taken on each pin controller specific action
  * @device_root: debugfs root for this device
  */
 struct pinctrl_dev {
@@ -46,6 +47,7 @@ struct pinctrl_dev {
 	struct pinctrl *p;
 	struct pinctrl_state *hog_default;
 	struct pinctrl_state *hog_sleep;
+	struct mutex mutex;
 #ifdef CONFIG_DEBUG_FS
 	struct dentry *device_root;
 #endif
@@ -72,7 +74,7 @@ struct pinctrl {
 
 /**
  * struct pinctrl_state - a pinctrl state for a device
- * @node: list not for struct pinctrl's @states field
+ * @node: list node for struct pinctrl's @states field
  * @name: the name of this state
  * @settings: a list of settings for this state
  */
@@ -168,6 +170,7 @@ struct pinctrl_maps {
 };
 
 struct pinctrl_dev *get_pinctrl_dev_from_devname(const char *dev_name);
+struct pinctrl_dev *get_pinctrl_dev_from_of_node(struct device_node *np);
 int pin_get_from_name(struct pinctrl_dev *pctldev, const char *name);
 const char *pin_get_name(struct pinctrl_dev *pctldev, const unsigned pin);
 int pinctrl_get_group_selector(struct pinctrl_dev *pctldev,
@@ -186,8 +189,7 @@ void pinctrl_unregister_map(struct pinctrl_map const *map);
 extern int pinctrl_force_sleep(struct pinctrl_dev *pctldev);
 extern int pinctrl_force_default(struct pinctrl_dev *pctldev);
 
-extern struct mutex pinctrl_mutex;
-extern struct list_head pinctrldev_list;
+extern struct mutex pinctrl_maps_mutex;
 extern struct list_head pinctrl_maps;
 
 #define for_each_maps(_maps_node_, _i_, _map_) \
diff --git a/drivers/pinctrl/devicetree.c b/drivers/pinctrl/devicetree.c
index fd40a11ad64..340fb4e6c60 100644
--- a/drivers/pinctrl/devicetree.c
+++ b/drivers/pinctrl/devicetree.c
@@ -41,7 +41,7 @@ static void dt_free_map(struct pinctrl_dev *pctldev,
 		     struct pinctrl_map *map, unsigned num_maps)
 {
 	if (pctldev) {
-		struct pinctrl_ops *ops = pctldev->desc->pctlops;
+		const struct pinctrl_ops *ops = pctldev->desc->pctlops;
 		ops->dt_free_map(pctldev, map, num_maps);
 	} else {
 		/* There is no pctldev for PIN_MAP_TYPE_DUMMY_STATE */
@@ -95,22 +95,11 @@ static int dt_remember_or_free_map(struct pinctrl *p, const char *statename,
 	return pinctrl_register_map(map, num_maps, false, true);
 }
 
-static struct pinctrl_dev *find_pinctrl_by_of_node(struct device_node *np)
-{
-	struct pinctrl_dev *pctldev;
-
-	list_for_each_entry(pctldev, &pinctrldev_list, node)
-		if (pctldev->dev->of_node == np)
-			return pctldev;
-
-	return NULL;
-}
-
 struct pinctrl_dev *of_pinctrl_get(struct device_node *np)
 {
 	struct pinctrl_dev *pctldev;
 
-	pctldev = find_pinctrl_by_of_node(np);
+	pctldev = get_pinctrl_dev_from_of_node(np);
 	if (!pctldev)
 		return NULL;
 
@@ -122,7 +111,7 @@ static int dt_to_map_one_config(struct pinctrl *p, const char *statename,
 {
 	struct device_node *np_pctldev;
 	struct pinctrl_dev *pctldev;
-	struct pinctrl_ops *ops;
+	const struct pinctrl_ops *ops;
 	int ret;
 	struct pinctrl_map *map;
 	unsigned num_maps;
@@ -138,7 +127,7 @@ static int dt_to_map_one_config(struct pinctrl *p, const char *statename,
 			/* OK let's just assume this will appear later then */
 			return -EPROBE_DEFER;
 		}
-		pctldev = find_pinctrl_by_of_node(np_pctldev);
+		pctldev = get_pinctrl_dev_from_of_node(np_pctldev);
 		if (pctldev)
 			break;
 		/* Do not defer probing of hogs (circular loop) */
diff --git a/drivers/pinctrl/mvebu/pinctrl-mvebu.c b/drivers/pinctrl/mvebu/pinctrl-mvebu.c
index c689c04a4f5..bb7ddb1bc89 100644
--- a/drivers/pinctrl/mvebu/pinctrl-mvebu.c
+++ b/drivers/pinctrl/mvebu/pinctrl-mvebu.c
@@ -263,7 +263,7 @@ static void mvebu_pinconf_group_dbg_show(struct pinctrl_dev *pctldev,
 	return;
 }
 
-static struct pinconf_ops mvebu_pinconf_ops = {
+static const struct pinconf_ops mvebu_pinconf_ops = {
 	.pin_config_group_get = mvebu_pinconf_group_get,
 	.pin_config_group_set = mvebu_pinconf_group_set,
 	.pin_config_group_dbg_show = mvebu_pinconf_group_dbg_show,
@@ -369,7 +369,7 @@ static int mvebu_pinmux_gpio_set_direction(struct pinctrl_dev *pctldev,
 	return -ENOTSUPP;
 }
 
-static struct pinmux_ops mvebu_pinmux_ops = {
+static const struct pinmux_ops mvebu_pinmux_ops = {
 	.get_functions_count = mvebu_pinmux_get_funcs_count,
 	.get_function_name = mvebu_pinmux_get_func_name,
 	.get_function_groups = mvebu_pinmux_get_groups,
@@ -470,7 +470,7 @@ static void mvebu_pinctrl_dt_free_map(struct pinctrl_dev *pctldev,
 	kfree(map);
 }
 
-static struct pinctrl_ops mvebu_pinctrl_ops = {
+static const struct pinctrl_ops mvebu_pinctrl_ops = {
 	.get_groups_count = mvebu_pinctrl_get_groups_count,
 	.get_group_name = mvebu_pinctrl_get_group_name,
 	.get_group_pins = mvebu_pinctrl_get_group_pins,
@@ -478,8 +478,12 @@ static struct pinctrl_ops mvebu_pinctrl_ops = {
 	.dt_free_map = mvebu_pinctrl_dt_free_map,
 };
 
-static int _add_function(struct mvebu_pinctrl_function *funcs, const char *name)
+static int _add_function(struct mvebu_pinctrl_function *funcs, int *funcsize,
+			const char *name)
 {
+	if (*funcsize <= 0)
+		return -EOVERFLOW;
+
 	while (funcs->num_groups) {
 		/* function already there */
 		if (strcmp(funcs->name, name) == 0) {
@@ -488,8 +492,12 @@ static int _add_function(struct mvebu_pinctrl_function *funcs, const char *name)
 		}
 		funcs++;
 	}
+
+	/* append new unique function */
 	funcs->name = name;
 	funcs->num_groups = 1;
+	(*funcsize)--;
+
 	return 0;
 }
 
@@ -497,12 +505,12 @@ static int mvebu_pinctrl_build_functions(struct platform_device *pdev,
 					 struct mvebu_pinctrl *pctl)
 {
 	struct mvebu_pinctrl_function *funcs;
-	int num = 0;
+	int num = 0, funcsize = pctl->desc.npins;
 	int n, s;
 
 	/* we allocate functions for number of pins and hope
-	 * there are less unique functions than pins available */
-	funcs = devm_kzalloc(&pdev->dev, pctl->desc.npins *
+	 * there are fewer unique functions than pins available */
+	funcs = devm_kzalloc(&pdev->dev, funcsize *
 			     sizeof(struct mvebu_pinctrl_function), GFP_KERNEL);
 	if (!funcs)
 		return -ENOMEM;
@@ -510,26 +518,27 @@ static int mvebu_pinctrl_build_functions(struct platform_device *pdev,
 	for (n = 0; n < pctl->num_groups; n++) {
 		struct mvebu_pinctrl_group *grp = &pctl->groups[n];
 		for (s = 0; s < grp->num_settings; s++) {
+			int ret;
+
 			/* skip unsupported settings on this variant */
 			if (pctl->variant &&
 			    !(pctl->variant & grp->settings[s].variant))
 				continue;
 
 			/* check for unique functions and count groups */
-			if (_add_function(funcs, grp->settings[s].name))
+			ret = _add_function(funcs, &funcsize,
+					    grp->settings[s].name);
+			if (ret == -EOVERFLOW)
+				dev_err(&pdev->dev,
+					"More functions than pins(%d)\n",
+					pctl->desc.npins);
+			if (ret < 0)
 				continue;
 
 			num++;
 		}
 	}
 
-	/* with the number of unique functions and it's groups known,
-	   reallocate functions and assign group names */
-	funcs = krealloc(funcs, num * sizeof(struct mvebu_pinctrl_function),
-			 GFP_KERNEL);
-	if (!funcs)
-		return -ENOMEM;
-
 	pctl->num_functions = num;
 	pctl->functions = funcs;
 
@@ -620,7 +629,7 @@ int mvebu_pinctrl_probe(struct platform_device *pdev)
 
 		/* special soc specific control */
 		if (ctrl->mpp_get || ctrl->mpp_set) {
-			if (!ctrl->name || !ctrl->mpp_set || !ctrl->mpp_set) {
+			if (!ctrl->name || !ctrl->mpp_get || !ctrl->mpp_set) {
 				dev_err(&pdev->dev, "wrong soc control info\n");
 				return -EINVAL;
 			}
diff --git a/drivers/pinctrl/pinconf-generic.c b/drivers/pinctrl/pinconf-generic.c
index 06c304ac6f7..2ad5a8d337b 100644
--- a/drivers/pinctrl/pinconf-generic.c
+++ b/drivers/pinctrl/pinconf-generic.c
@@ -12,6 +12,7 @@
 #define pr_fmt(fmt) "generic pinconfig core: " fmt
 
 #include <linux/kernel.h>
+#include <linux/module.h>
 #include <linux/init.h>
 #include <linux/device.h>
 #include <linux/slab.h>
@@ -33,7 +34,7 @@ struct pin_config_item {
 
 #define PCONFDUMP(a, b, c) { .param = a, .display = b, .format = c }
 
-struct pin_config_item conf_items[] = {
+static struct pin_config_item conf_items[] = {
 	PCONFDUMP(PIN_CONFIG_BIAS_DISABLE, "input bias disabled", NULL),
 	PCONFDUMP(PIN_CONFIG_BIAS_HIGH_IMPEDANCE, "input bias high impedance", NULL),
 	PCONFDUMP(PIN_CONFIG_BIAS_PULL_UP, "input bias pull up", NULL),
@@ -59,7 +60,7 @@ void pinconf_generic_dump_pin(struct pinctrl_dev *pctldev,
 	if (!ops->is_generic)
 		return;
 
-	for(i = 0; i < ARRAY_SIZE(conf_items); i++) {
+	for (i = 0; i < ARRAY_SIZE(conf_items); i++) {
 		unsigned long config;
 		int ret;
 
@@ -94,7 +95,7 @@ void pinconf_generic_dump_group(struct pinctrl_dev *pctldev,
 	if (!ops->is_generic)
 		return;
 
-	for(i = 0; i < ARRAY_SIZE(conf_items); i++) {
+	for (i = 0; i < ARRAY_SIZE(conf_items); i++) {
 		unsigned long config;
 		int ret;
 
@@ -120,4 +121,17 @@ void pinconf_generic_dump_group(struct pinctrl_dev *pctldev,
 	}
 }
 
+void pinconf_generic_dump_config(struct pinctrl_dev *pctldev,
+				 struct seq_file *s, unsigned long config)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(conf_items); i++) {
+		if (pinconf_to_config_param(config) != conf_items[i].param)
+			continue;
+		seq_printf(s, "%s: 0x%x", conf_items[i].display,
+			   pinconf_to_config_argument(config));
+	}
+}
+EXPORT_SYMBOL_GPL(pinconf_generic_dump_config);
 #endif
diff --git a/drivers/pinctrl/pinconf.c b/drivers/pinctrl/pinconf.c
index ac8d382a79b..c67c37e23dd 100644
--- a/drivers/pinctrl/pinconf.c
+++ b/drivers/pinctrl/pinconf.c
@@ -17,6 +17,7 @@
 #include <linux/slab.h>
 #include <linux/debugfs.h>
 #include <linux/seq_file.h>
+#include <linux/uaccess.h>
 #include <linux/pinctrl/machine.h>
 #include <linux/pinctrl/pinctrl.h>
 #include <linux/pinctrl/pinconf.h>
@@ -88,14 +89,14 @@ int pin_config_get(const char *dev_name, const char *name,
 	struct pinctrl_dev *pctldev;
 	int pin;
 
-	mutex_lock(&pinctrl_mutex);
-
 	pctldev = get_pinctrl_dev_from_devname(dev_name);
 	if (!pctldev) {
 		pin = -EINVAL;
-		goto unlock;
+		return pin;
 	}
 
+	mutex_lock(&pctldev->mutex);
+
 	pin = pin_get_from_name(pctldev, name);
 	if (pin < 0)
 		goto unlock;
@@ -103,7 +104,7 @@ int pin_config_get(const char *dev_name, const char *name,
 	pin = pin_config_get_for_pin(pctldev, pin, config);
 
 unlock:
-	mutex_unlock(&pinctrl_mutex);
+	mutex_unlock(&pctldev->mutex);
 	return pin;
 }
 EXPORT_SYMBOL(pin_config_get);
@@ -144,14 +145,14 @@ int pin_config_set(const char *dev_name, const char *name,
 	struct pinctrl_dev *pctldev;
 	int pin, ret;
 
-	mutex_lock(&pinctrl_mutex);
-
 	pctldev = get_pinctrl_dev_from_devname(dev_name);
 	if (!pctldev) {
 		ret = -EINVAL;
-		goto unlock;
+		return ret;
 	}
 
+	mutex_lock(&pctldev->mutex);
+
 	pin = pin_get_from_name(pctldev, name);
 	if (pin < 0) {
 		ret = pin;
@@ -161,7 +162,7 @@ int pin_config_set(const char *dev_name, const char *name,
 	ret = pin_config_set_for_pin(pctldev, pin, config);
 
 unlock:
-	mutex_unlock(&pinctrl_mutex);
+	mutex_unlock(&pctldev->mutex);
 	return ret;
 }
 EXPORT_SYMBOL(pin_config_set);
@@ -173,13 +174,14 @@ int pin_config_group_get(const char *dev_name, const char *pin_group,
 	const struct pinconf_ops *ops;
 	int selector, ret;
 
-	mutex_lock(&pinctrl_mutex);
-
 	pctldev = get_pinctrl_dev_from_devname(dev_name);
 	if (!pctldev) {
 		ret = -EINVAL;
-		goto unlock;
+		return ret;
 	}
+
+	mutex_lock(&pctldev->mutex);
+
 	ops = pctldev->desc->confops;
 
 	if (!ops || !ops->pin_config_group_get) {
@@ -199,7 +201,7 @@ int pin_config_group_get(const char *dev_name, const char *pin_group,
 	ret = ops->pin_config_group_get(pctldev, selector, config);
 
 unlock:
-	mutex_unlock(&pinctrl_mutex);
+	mutex_unlock(&pctldev->mutex);
 	return ret;
 }
 EXPORT_SYMBOL(pin_config_group_get);
@@ -216,13 +218,14 @@ int pin_config_group_set(const char *dev_name, const char *pin_group,
 	int ret;
 	int i;
 
-	mutex_lock(&pinctrl_mutex);
-
 	pctldev = get_pinctrl_dev_from_devname(dev_name);
 	if (!pctldev) {
 		ret = -EINVAL;
-		goto unlock;
+		return ret;
 	}
+
+	mutex_lock(&pctldev->mutex);
+
 	ops = pctldev->desc->confops;
 	pctlops = pctldev->desc->pctlops;
 
@@ -278,7 +281,7 @@ int pin_config_group_set(const char *dev_name, const char *pin_group,
 	ret = 0;
 
 unlock:
-	mutex_unlock(&pinctrl_mutex);
+	mutex_unlock(&pctldev->mutex);
 
 	return ret;
 }
@@ -486,7 +489,7 @@ static int pinconf_pins_show(struct seq_file *s, void *what)
 	seq_puts(s, "Pin config settings per pin\n");
 	seq_puts(s, "Format: pin (name): configs\n");
 
-	mutex_lock(&pinctrl_mutex);
+	mutex_lock(&pctldev->mutex);
 
 	/* The pin number can be retrived from the pin controller descriptor */
 	for (i = 0; i < pctldev->desc->npins; i++) {
@@ -506,7 +509,7 @@ static int pinconf_pins_show(struct seq_file *s, void *what)
 		seq_printf(s, "\n");
 	}
 
-	mutex_unlock(&pinctrl_mutex);
+	mutex_unlock(&pctldev->mutex);
 
 	return 0;
 }
@@ -574,122 +577,58 @@ static const struct file_operations pinconf_groups_ops = {
 	.release	= single_release,
 };
 
-/* 32bit read/write ressources */
-#define MAX_NAME_LEN 16
-char dbg_pinname[MAX_NAME_LEN]; /* shared: name of the state of the pin*/
-char dbg_state_name[MAX_NAME_LEN]; /* shared: state of the pin*/
-static u32 dbg_config; /* shared: config to be read/set for the pin & state*/
-
-static int pinconf_dbg_pinname_print(struct seq_file *s, void *d)
-{
-	if (strlen(dbg_pinname))
-		seq_printf(s, "%s\n", dbg_pinname);
-	else
-		seq_printf(s, "No pin name set\n");
-	return 0;
-}
-
-static int pinconf_dbg_pinname_open(struct inode *inode, struct file *file)
-{
-	return single_open(file, pinconf_dbg_pinname_print, inode->i_private);
-}
-
-static int pinconf_dbg_pinname_write(struct file *file,
-	const char __user *user_buf, size_t count, loff_t *ppos)
-{
-	int err;
-
-	if (count > MAX_NAME_LEN)
-		return -EINVAL;
-
-	err = sscanf(user_buf, "%15s", dbg_pinname);
-
-	if (err != 1)
-		return -EINVAL;
-
-	return count;
-}
+#define MAX_NAME_LEN 15
 
-static const struct file_operations pinconf_dbg_pinname_fops = {
-	.open = pinconf_dbg_pinname_open,
-	.write = pinconf_dbg_pinname_write,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-	.owner = THIS_MODULE,
+struct dbg_cfg {
+	enum pinctrl_map_type map_type;
+	char dev_name[MAX_NAME_LEN+1];
+	char state_name[MAX_NAME_LEN+1];
+	char pin_name[MAX_NAME_LEN+1];
 };
 
-static int pinconf_dbg_state_print(struct seq_file *s, void *d)
-{
-	if (strlen(dbg_state_name))
-		seq_printf(s, "%s\n", dbg_pinname);
-	else
-		seq_printf(s, "No pin state set\n");
-	return 0;
-}
-
-static int pinconf_dbg_state_open(struct inode *inode, struct file *file)
-{
-	return single_open(file, pinconf_dbg_state_print, inode->i_private);
-}
-
-static int pinconf_dbg_state_write(struct file *file,
-	const char __user *user_buf, size_t count, loff_t *ppos)
-{
-	int err;
-
-	if (count > MAX_NAME_LEN)
-		return -EINVAL;
-
-	err = sscanf(user_buf, "%15s", dbg_state_name);
-
-	if (err != 1)
-		return -EINVAL;
-
-	return count;
-}
-
-static const struct file_operations pinconf_dbg_pinstate_fops = {
-	.open = pinconf_dbg_state_open,
-	.write = pinconf_dbg_state_write,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-	.owner = THIS_MODULE,
-};
+/*
+ * Goal is to keep this structure as global in order to simply read the
+ * pinconf-config file after a write to check config is as expected
+ */
+static struct dbg_cfg pinconf_dbg_conf;
 
 /**
  * pinconf_dbg_config_print() - display the pinctrl config from the pinctrl
- * map, of a pin/state pair based on pinname and state that have been
- * selected with the debugfs entries pinconf-name and pinconf-state
- * @s: contains the 32bits config to be written
+ * map, of the dev/pin/state that was last written to pinconf-config file.
+ * @s: string filled in  with config description
  * @d: not used
  */
 static int pinconf_dbg_config_print(struct seq_file *s, void *d)
 {
 	struct pinctrl_maps *maps_node;
-	struct pinctrl_map const *map;
+	const struct pinctrl_map *map;
 	struct pinctrl_dev *pctldev = NULL;
-	struct pinconf_ops *confops = NULL;
+	const struct pinconf_ops *confops = NULL;
+	const struct pinctrl_map_configs *configs;
+	struct dbg_cfg *dbg = &pinconf_dbg_conf;
 	int i, j;
 	bool found = false;
+	unsigned long config;
 
-	mutex_lock(&pinctrl_mutex);
+	mutex_lock(&pctldev->mutex);
 
 	/* Parse the pinctrl map and look for the elected pin/state */
 	for_each_maps(maps_node, i, map) {
-		if (map->type != PIN_MAP_TYPE_CONFIGS_PIN)
+		if (map->type != dbg->map_type)
 			continue;
-
-		if (strncmp(map->name, dbg_state_name, MAX_NAME_LEN) > 0)
+		if (strcmp(map->dev_name, dbg->dev_name))
+			continue;
+		if (strcmp(map->name, dbg->state_name))
 			continue;
 
 		for (j = 0; j < map->data.configs.num_configs; j++) {
-			if (0 == strncmp(map->data.configs.group_or_pin,
-						dbg_pinname, MAX_NAME_LEN)) {
-				/* We found the right pin / state, read the
-				 * config and store the pctldev */
-				dbg_config = map->data.configs.configs[j];
+			if (!strcmp(map->data.configs.group_or_pin,
+					dbg->pin_name)) {
+				/*
+				 * We found the right pin / state, read the
+				 * config and he pctldev for later use
+				 */
+				configs = &map->data.configs;
 				pctldev = get_pinctrl_dev_from_devname
 					(map->ctrl_dev_name);
 				found = true;
@@ -698,74 +637,166 @@ static int pinconf_dbg_config_print(struct seq_file *s, void *d)
 		}
 	}
 
-	mutex_unlock(&pinctrl_mutex);
+	if (!found) {
+		seq_printf(s, "No config found for dev/state/pin, expected:\n");
+		seq_printf(s, "Searched dev:%s\n", dbg->dev_name);
+		seq_printf(s, "Searched state:%s\n", dbg->state_name);
+		seq_printf(s, "Searched pin:%s\n", dbg->pin_name);
+		seq_printf(s, "Use: modify config_pin <devname> "\
+				"<state> <pinname> <value>\n");
+		goto exit;
+	}
 
-	if (found) {
-		seq_printf(s, "Config of %s in state %s: 0x%08X\n", dbg_pinname,
-				 dbg_state_name, dbg_config);
+	config = *(configs->configs);
+	seq_printf(s, "Dev %s has config of %s in state %s: 0x%08lX\n",
+			dbg->dev_name, dbg->pin_name,
+			dbg->state_name, config);
 
-		if (pctldev)
-			confops = pctldev->desc->confops;
+	if (pctldev)
+		confops = pctldev->desc->confops;
 
-		if (confops && confops->pin_config_config_dbg_show)
-			confops->pin_config_config_dbg_show(pctldev,
-					s, dbg_config);
-	} else {
-		seq_printf(s, "No pin found for defined name/state\n");
-	}
+	if (confops && confops->pin_config_config_dbg_show)
+		confops->pin_config_config_dbg_show(pctldev, s, config);
 
-	return 0;
-}
+exit:
+	mutex_unlock(&pctldev->mutex);
 
-static int pinconf_dbg_config_open(struct inode *inode, struct file *file)
-{
-	return single_open(file, pinconf_dbg_config_print, inode->i_private);
+	return 0;
 }
 
 /**
- * pinconf_dbg_config_write() - overwrite the pinctrl config in thepinctrl
- * map, of a pin/state pair based on pinname and state that have been
- * selected with the debugfs entries pinconf-name and pinconf-state
+ * pinconf_dbg_config_write() - modify the pinctrl config in the pinctrl
+ * map, of a dev/pin/state entry based on user entries to pinconf-config
+ * @user_buf: contains the modification request with expected format:
+ *     modify config_pin <devicename> <state> <pinname> <newvalue>
+ * modify is literal string, alternatives like add/delete not supported yet
+ * config_pin is literal, alternatives like config_mux not supported yet
+ * <devicename> <state> <pinname> are values that should match the pinctrl-maps
+ * <newvalue> reflects the new config and is driver dependant
  */
 static int pinconf_dbg_config_write(struct file *file,
 	const char __user *user_buf, size_t count, loff_t *ppos)
 {
-	int err;
-	unsigned long config;
 	struct pinctrl_maps *maps_node;
-	struct pinctrl_map const *map;
-	int i, j;
+	const struct pinctrl_map *map;
+	struct pinctrl_dev *pctldev = NULL;
+	const struct pinconf_ops *confops = NULL;
+	struct dbg_cfg *dbg = &pinconf_dbg_conf;
+	const struct pinctrl_map_configs *configs;
+	char config[MAX_NAME_LEN+1];
+	bool found = false;
+	char buf[128];
+	char *b = &buf[0];
+	int buf_size;
+	char *token;
+	int i;
+
+	/* Get userspace string and assure termination */
+	buf_size = min(count, (sizeof(buf)-1));
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	buf[buf_size] = 0;
+
+	/*
+	 * need to parse entry and extract parameters:
+	 * modify configs_pin devicename state pinname newvalue
+	 */
+
+	/* Get arg: 'modify' */
+	token = strsep(&b, " ");
+	if (!token)
+		return -EINVAL;
+	if (strcmp(token, "modify"))
+		return -EINVAL;
 
-	err = kstrtoul_from_user(user_buf, count, 0, &config);
+	/* Get arg type: "config_pin" type supported so far */
+	token = strsep(&b, " ");
+	if (!token)
+		return -EINVAL;
+	if (strcmp(token, "config_pin"))
+		return -EINVAL;
+	dbg->map_type = PIN_MAP_TYPE_CONFIGS_PIN;
 
-	if (err)
-		return err;
+	/* get arg 'device_name' */
+	token = strsep(&b, " ");
+	if (token == NULL)
+		return -EINVAL;
+	if (strlen(token) >= MAX_NAME_LEN)
+		return -EINVAL;
+	strncpy(dbg->dev_name, token, MAX_NAME_LEN);
 
-	dbg_config = config;
+	/* get arg 'state_name' */
+	token = strsep(&b, " ");
+	if (token == NULL)
+		return -EINVAL;
+	if (strlen(token) >= MAX_NAME_LEN)
+		return -EINVAL;
+	strncpy(dbg->state_name, token, MAX_NAME_LEN);
 
-	mutex_lock(&pinctrl_mutex);
+	/* get arg 'pin_name' */
+	token = strsep(&b, " ");
+	if (token == NULL)
+		return -EINVAL;
+	if (strlen(token) >= MAX_NAME_LEN)
+		return -EINVAL;
+	strncpy(dbg->pin_name, token, MAX_NAME_LEN);
 
-	/* Parse the pinctrl map and look for the selected pin/state */
+	/* get new_value of config' */
+	token = strsep(&b, " ");
+	if (token == NULL)
+		return -EINVAL;
+	if (strlen(token) >= MAX_NAME_LEN)
+		return -EINVAL;
+	strncpy(config, token, MAX_NAME_LEN);
+
+	mutex_lock(&pinctrl_maps_mutex);
+
+	/* Parse the pinctrl map and look for the selected dev/state/pin */
 	for_each_maps(maps_node, i, map) {
-		if (map->type != PIN_MAP_TYPE_CONFIGS_PIN)
+		if (strcmp(map->dev_name, dbg->dev_name))
 			continue;
-
-		if (strncmp(map->name, dbg_state_name, MAX_NAME_LEN) > 0)
+		if (map->type != dbg->map_type)
+			continue;
+		if (strcmp(map->name, dbg->state_name))
 			continue;
 
 		/*  we found the right pin / state, so overwrite config */
-		for (j = 0; j < map->data.configs.num_configs; j++) {
-			if (strncmp(map->data.configs.group_or_pin, dbg_pinname,
-						MAX_NAME_LEN) == 0)
-				map->data.configs.configs[j] = dbg_config;
+		if (!strcmp(map->data.configs.group_or_pin, dbg->pin_name)) {
+			found = true;
+			pctldev = get_pinctrl_dev_from_devname(
+					map->ctrl_dev_name);
+			configs = &map->data.configs;
+			break;
 		}
 	}
 
-	mutex_unlock(&pinctrl_mutex);
+	if (!found) {
+		count = -EINVAL;
+		goto exit;
+	}
+
+	if (pctldev)
+		confops = pctldev->desc->confops;
+
+	if (confops && confops->pin_config_dbg_parse_modify) {
+		for (i = 0; i < configs->num_configs; i++) {
+			confops->pin_config_dbg_parse_modify(pctldev,
+						     config,
+						     &configs->configs[i]);
+		}
+	}
+
+exit:
+	mutex_unlock(&pinctrl_maps_mutex);
 
 	return count;
 }
 
+static int pinconf_dbg_config_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, pinconf_dbg_config_print, inode->i_private);
+}
+
 static const struct file_operations pinconf_dbg_pinconfig_fops = {
 	.open = pinconf_dbg_config_open,
 	.write = pinconf_dbg_config_write,
@@ -782,10 +813,6 @@ void pinconf_init_device_debugfs(struct dentry *devroot,
 			    devroot, pctldev, &pinconf_pins_ops);
 	debugfs_create_file("pinconf-groups", S_IFREG | S_IRUGO,
 			    devroot, pctldev, &pinconf_groups_ops);
-	debugfs_create_file("pinconf-name", (S_IRUGO | S_IWUSR | S_IWGRP),
-			    devroot, pctldev, &pinconf_dbg_pinname_fops);
-	debugfs_create_file("pinconf-state",  (S_IRUGO | S_IWUSR | S_IWGRP),
-			    devroot, pctldev, &pinconf_dbg_pinstate_fops);
 	debugfs_create_file("pinconf-config",  (S_IRUGO | S_IWUSR | S_IWGRP),
 			    devroot, pctldev, &pinconf_dbg_pinconfig_fops);
 }
diff --git a/drivers/pinctrl/pinconf.h b/drivers/pinctrl/pinconf.h
index e3ed8cb072a..92c7267244d 100644
--- a/drivers/pinctrl/pinconf.h
+++ b/drivers/pinctrl/pinconf.h
@@ -90,7 +90,7 @@ static inline void pinconf_init_device_debugfs(struct dentry *devroot,
  * pin config.
  */
 
-#ifdef CONFIG_GENERIC_PINCONF
+#if defined(CONFIG_GENERIC_PINCONF) && defined(CONFIG_DEBUG_FS)
 
 void pinconf_generic_dump_pin(struct pinctrl_dev *pctldev,
 			      struct seq_file *s, unsigned pin);
@@ -98,6 +98,8 @@ void pinconf_generic_dump_pin(struct pinctrl_dev *pctldev,
 void pinconf_generic_dump_group(struct pinctrl_dev *pctldev,
 			      struct seq_file *s, const char *gname);
 
+void pinconf_generic_dump_config(struct pinctrl_dev *pctldev,
+				 struct seq_file *s, unsigned long config);
 #else
 
 static inline void pinconf_generic_dump_pin(struct pinctrl_dev *pctldev,
@@ -114,4 +116,10 @@ static inline void pinconf_generic_dump_group(struct pinctrl_dev *pctldev,
 	return;
 }
 
+static inline void pinconf_generic_dump_config(struct pinctrl_dev *pctldev,
+					       struct seq_file *s,
+					       unsigned long config)
+{
+	return;
+}
 #endif
diff --git a/drivers/pinctrl/pinctrl-ab8500.c b/drivers/pinctrl/pinctrl-ab8500.c
index 3b471d87c21..2ac2d0ad302 100644
--- a/drivers/pinctrl/pinctrl-ab8500.c
+++ b/drivers/pinctrl/pinctrl-ab8500.c
@@ -389,7 +389,8 @@ static const struct abx500_function ab8500_functions[] = {
  *	alt_A	|       1       |          0          |          0
  */
 
-struct alternate_functions ab8500_alternate_functions[AB8500_GPIO_MAX_NUMBER + 1] = {
+static struct
+alternate_functions ab8500_alternate_functions[AB8500_GPIO_MAX_NUMBER + 1] = {
 	ALTERNATE_FUNCTIONS(0, UNUSED, UNUSED, UNUSED, 0, 0, 0), /* no GPIO0 */
 	ALTERNATE_FUNCTIONS(1,	    0, UNUSED, UNUSED, 0, 0, 0), /* GPIO1, altA controlled by bit 0 */
 	ALTERNATE_FUNCTIONS(2,      1, UNUSED, UNUSED, 0, 0, 0), /* GPIO2, altA controlled by bit 1 */
@@ -455,7 +456,7 @@ struct alternate_functions ab8500_alternate_functions[AB8500_GPIO_MAX_NUMBER + 1
  *	GPIO24 and GPIO25
  *	GPIO36 to GPIO41
  */
-struct abx500_gpio_irq_cluster ab8500_gpio_irq_cluster[] = {
+static struct abx500_gpio_irq_cluster ab8500_gpio_irq_cluster[] = {
 	GPIO_IRQ_CLUSTER(6,  13, AB8500_INT_GPIO6R),
 	GPIO_IRQ_CLUSTER(24, 25, AB8500_INT_GPIO24R),
 	GPIO_IRQ_CLUSTER(36, 41, AB8500_INT_GPIO36R),
diff --git a/drivers/pinctrl/pinctrl-ab8505.c b/drivers/pinctrl/pinctrl-ab8505.c
index 3a4238e879e..bf0ef4ac376 100644
--- a/drivers/pinctrl/pinctrl-ab8505.c
+++ b/drivers/pinctrl/pinctrl-ab8505.c
@@ -271,7 +271,8 @@ static const struct abx500_function ab8505_functions[] = {
  *	alt_A	|       1       |          0          |          0
  */
 
-struct alternate_functions ab8505_alternate_functions[AB8505_GPIO_MAX_NUMBER + 1] = {
+static struct
+alternate_functions ab8505_alternate_functions[AB8505_GPIO_MAX_NUMBER + 1] = {
 	ALTERNATE_FUNCTIONS(0, UNUSED, UNUSED, UNUSED, 0, 0, 0), /* no GPIO0 */
 	ALTERNATE_FUNCTIONS(1,      0, UNUSED, UNUSED, 0, 0, 0), /* GPIO1, altA controlled by bit 0 */
 	ALTERNATE_FUNCTIONS(2,      1, UNUSED, UNUSED, 0, 0, 0), /* GPIO2, altA controlled by bit 1 */
@@ -284,7 +285,7 @@ struct alternate_functions ab8505_alternate_functions[AB8505_GPIO_MAX_NUMBER + 1
 
 	ALTERNATE_FUNCTIONS(9, UNUSED, UNUSED, UNUSED, 0, 0, 0), /* no GPIO9, bit 0 reserved */
 	ALTERNATE_FUNCTIONS(10,      1,      0, UNUSED, 1, 0, 0), /* GPIO10, altA and altB controlled by bit 0 */
-	ALTERNATE_FUNCTIONS(11,      2, UNUSED, UNUSED, 0, 0, 0), /* GPIO11, altA controlled by bit 2 */
+	ALTERNATE_FUNCTIONS(11,      2,      1, UNUSED, 0, 0, 0), /* GPIO11, altA controlled by bit 2 */
 	ALTERNATE_FUNCTIONS(12, UNUSED, UNUSED, UNUSED, 0, 0, 0), /* no GPIO12, bit3 reseved */
 	ALTERNATE_FUNCTIONS(13,      4,      3,      4, 1, 0, 2), /* GPIO13, altA altB and altC controlled by bit 3 and 4 */
 	ALTERNATE_FUNCTIONS(14,      5, UNUSED, UNUSED, 0, 0, 0), /* GPIO14, altA controlled by bit 5 */
@@ -348,7 +349,7 @@ struct alternate_functions ab8505_alternate_functions[AB8505_GPIO_MAX_NUMBER + 1
  *	GPIO50
  *	GPIO52 to GPIO53
  */
-struct abx500_gpio_irq_cluster ab8505_gpio_irq_cluster[] = {
+static struct abx500_gpio_irq_cluster ab8505_gpio_irq_cluster[] = {
 	GPIO_IRQ_CLUSTER(10, 11, AB8500_INT_GPIO10R),
 	GPIO_IRQ_CLUSTER(13, 13, AB8500_INT_GPIO13R),
 	GPIO_IRQ_CLUSTER(40, 41, AB8500_INT_GPIO40R),
diff --git a/drivers/pinctrl/pinctrl-ab8540.c b/drivers/pinctrl/pinctrl-ab8540.c
index 8ee1e8d95f6..9867535d49c 100644
--- a/drivers/pinctrl/pinctrl-ab8540.c
+++ b/drivers/pinctrl/pinctrl-ab8540.c
@@ -299,7 +299,8 @@ static const struct abx500_function ab8540_functions[] = {
  *
  */
 
-struct alternate_functions ab8540_alternate_functions[AB8540_GPIO_MAX_NUMBER + 1] = {
+static struct
+alternate_functions ab8540_alternate_functions[AB8540_GPIO_MAX_NUMBER + 1] = {
 	/* GPIOSEL1 - bit 4-7 reserved */
 	ALTERNATE_FUNCTIONS(0, UNUSED, UNUSED, UNUSED, 0, 0, 0), /* no GPIO0 */
 	ALTERNATE_FUNCTIONS(1,	    0, UNUSED, UNUSED, 0, 0, 0), /* GPIO1, altA controlled by bit 0 */
@@ -376,7 +377,7 @@ static struct pullud ab8540_pullud = {
  *	GPIO43 to GPIO44
  *	GPIO51 to GPIO54
  */
-struct abx500_gpio_irq_cluster ab8540_gpio_irq_cluster[] = {
+static struct abx500_gpio_irq_cluster ab8540_gpio_irq_cluster[] = {
 	GPIO_IRQ_CLUSTER(43, 43, AB8540_INT_GPIO43F),
 	GPIO_IRQ_CLUSTER(44, 44, AB8540_INT_GPIO44F),
 	GPIO_IRQ_CLUSTER(51, 54, AB9540_INT_GPIO51R),
diff --git a/drivers/pinctrl/pinctrl-ab9540.c b/drivers/pinctrl/pinctrl-ab9540.c
index 7610bd012b9..1a281ca95da 100644
--- a/drivers/pinctrl/pinctrl-ab9540.c
+++ b/drivers/pinctrl/pinctrl-ab9540.c
@@ -379,7 +379,8 @@ static const struct abx500_function ab9540_functions[] = {
  *	alt_A	|       1       |          0          |          0
  */
 
-struct alternate_functions ab9540alternate_functions[AB9540_GPIO_MAX_NUMBER + 1] = {
+static struct
+alternate_functions ab9540alternate_functions[AB9540_GPIO_MAX_NUMBER + 1] = {
 	/* GPIOSEL1 - bits 4-7 are reserved */
 	ALTERNATE_FUNCTIONS(0, UNUSED, UNUSED, UNUSED, 0, 0, 0), /* no GPIO0 */
 	ALTERNATE_FUNCTIONS(1,	    0, UNUSED, UNUSED, 0, 0, 0), /* GPIO1, altA controlled by bit 0 */
@@ -393,7 +394,7 @@ struct alternate_functions ab9540alternate_functions[AB9540_GPIO_MAX_NUMBER + 1]
 	/* GPIOSEL2 - bits 0 and 3 are reserved */
 	ALTERNATE_FUNCTIONS(9, UNUSED, UNUSED, UNUSED, 0, 0, 0), /* no GPIO9 */
 	ALTERNATE_FUNCTIONS(10,      1,      0, UNUSED, 1, 0, 0), /* GPIO10, altA and altB controlled by bit 0 */
-	ALTERNATE_FUNCTIONS(11,      2, UNUSED, UNUSED, 0, 0, 0), /* GPIO11, altA controlled by bit 1 */
+	ALTERNATE_FUNCTIONS(11,      2,	     1, UNUSED, 0, 0, 0), /* GPIO11, altA controlled by bit 1 */
 	ALTERNATE_FUNCTIONS(12, UNUSED, UNUSED, UNUSED, 0, 0, 0), /* no GPIO12 */
 	ALTERNATE_FUNCTIONS(13,      4,      3,      4, 1, 0, 2), /* GPIO13, altA altB and altC controlled by bit 3 and 4 */
 	ALTERNATE_FUNCTIONS(14,      5, UNUSED, UNUSED, 0, 0, 0), /* GPIO14, altA controlled by bit 5 */
@@ -454,7 +455,7 @@ struct alternate_functions ab9540alternate_functions[AB9540_GPIO_MAX_NUMBER + 1]
 	ALTERNATE_FUNCTIONS(54,	     5, UNUSED, UNUSED, 0, 0, 0), /* GPIO54 = GPIO60, altA controlled by bit 5 */
 };
 
-struct abx500_gpio_irq_cluster ab9540_gpio_irq_cluster[] = {
+static struct abx500_gpio_irq_cluster ab9540_gpio_irq_cluster[] = {
 	GPIO_IRQ_CLUSTER(10, 13, AB8500_INT_GPIO10R),
 	GPIO_IRQ_CLUSTER(24, 25, AB8500_INT_GPIO24R),
 	GPIO_IRQ_CLUSTER(40, 41, AB8500_INT_GPIO40R),
diff --git a/drivers/pinctrl/pinctrl-abx500.c b/drivers/pinctrl/pinctrl-abx500.c
index caecdd37306..aa17f7580f6 100644
--- a/drivers/pinctrl/pinctrl-abx500.c
+++ b/drivers/pinctrl/pinctrl-abx500.c
@@ -422,7 +422,7 @@ static u8 abx500_get_mode(struct pinctrl_dev *pctldev, struct gpio_chip *chip,
 	}
 
 	/* check if pin use AlternateFunction register */
-	if ((af.alt_bit1 == UNUSED) && (af.alt_bit1 == UNUSED))
+	if ((af.alt_bit1 == UNUSED) && (af.alt_bit2 == UNUSED))
 		return mode;
 	/*
 	 * if pin GPIOSEL bit is set and pin supports alternate function,
@@ -517,14 +517,14 @@ static inline void abx500_gpio_dbg_show_one(struct seq_file *s,
 #define abx500_gpio_dbg_show	NULL
 #endif
 
-int abx500_gpio_request(struct gpio_chip *chip, unsigned offset)
+static int abx500_gpio_request(struct gpio_chip *chip, unsigned offset)
 {
 	int gpio = chip->base + offset;
 
 	return pinctrl_request_gpio(gpio);
 }
 
-void abx500_gpio_free(struct gpio_chip *chip, unsigned offset)
+static void abx500_gpio_free(struct gpio_chip *chip, unsigned offset)
 {
 	int gpio = chip->base + offset;
 
@@ -611,7 +611,7 @@ static void abx500_pmx_disable(struct pinctrl_dev *pctldev,
 	dev_dbg(pct->dev, "disable group %s, %u pins\n", g->name, g->npins);
 }
 
-int abx500_gpio_request_enable(struct pinctrl_dev *pctldev,
+static int abx500_gpio_request_enable(struct pinctrl_dev *pctldev,
 			       struct pinctrl_gpio_range *range,
 			       unsigned offset)
 {
@@ -656,7 +656,7 @@ static void abx500_gpio_disable_free(struct pinctrl_dev *pctldev,
 {
 }
 
-static struct pinmux_ops abx500_pinmux_ops = {
+static const struct pinmux_ops abx500_pinmux_ops = {
 	.get_functions_count = abx500_pmx_get_funcs_cnt,
 	.get_function_name = abx500_pmx_get_func_name,
 	.get_function_groups = abx500_pmx_get_func_groups,
@@ -704,21 +704,21 @@ static void abx500_pin_dbg_show(struct pinctrl_dev *pctldev,
 				 chip->base + offset - 1);
 }
 
-static struct pinctrl_ops abx500_pinctrl_ops = {
+static const struct pinctrl_ops abx500_pinctrl_ops = {
 	.get_groups_count = abx500_get_groups_cnt,
 	.get_group_name = abx500_get_group_name,
 	.get_group_pins = abx500_get_group_pins,
 	.pin_dbg_show = abx500_pin_dbg_show,
 };
 
-int abx500_pin_config_get(struct pinctrl_dev *pctldev,
+static int abx500_pin_config_get(struct pinctrl_dev *pctldev,
 			  unsigned pin,
 			  unsigned long *config)
 {
 	return -ENOSYS;
 }
 
-int abx500_pin_config_set(struct pinctrl_dev *pctldev,
+static int abx500_pin_config_set(struct pinctrl_dev *pctldev,
 			  unsigned pin,
 			  unsigned long config)
 {
@@ -778,7 +778,7 @@ int abx500_pin_config_set(struct pinctrl_dev *pctldev,
 	return ret;
 }
 
-static struct pinconf_ops abx500_pinconf_ops = {
+static const struct pinconf_ops abx500_pinconf_ops = {
 	.pin_config_get = abx500_pin_config_get,
 	.pin_config_set = abx500_pin_config_set,
 };
@@ -834,6 +834,7 @@ static const struct of_device_id abx500_gpio_match[] = {
 	{ .compatible = "stericsson,ab8505-gpio", .data = (void *)PINCTRL_AB8505, },
 	{ .compatible = "stericsson,ab8540-gpio", .data = (void *)PINCTRL_AB8540, },
 	{ .compatible = "stericsson,ab9540-gpio", .data = (void *)PINCTRL_AB9540, },
+	{ }
 };
 
 static int abx500_gpio_probe(struct platform_device *pdev)
@@ -879,7 +880,6 @@ static int abx500_gpio_probe(struct platform_device *pdev)
 	pct->parent = dev_get_drvdata(pdev->dev.parent);
 	pct->chip = abx500gpio_chip;
 	pct->chip.dev = &pdev->dev;
-	pct->chip.base = pdata->gpio_base;
 	pct->chip.base = (np) ? -1 : pdata->gpio_base;
 
 	/* initialize the lock */
diff --git a/drivers/pinctrl/pinctrl-at91.c b/drivers/pinctrl/pinctrl-at91.c
index efb7f10e902..4d7f531e945 100644
--- a/drivers/pinctrl/pinctrl-at91.c
+++ b/drivers/pinctrl/pinctrl-at91.c
@@ -294,7 +294,7 @@ static void at91_dt_free_map(struct pinctrl_dev *pctldev,
 {
 }
 
-static struct pinctrl_ops at91_pctrl_ops = {
+static const struct pinctrl_ops at91_pctrl_ops = {
 	.get_groups_count	= at91_get_groups_count,
 	.get_group_name		= at91_get_group_name,
 	.get_group_pins		= at91_get_group_pins,
@@ -303,7 +303,7 @@ static struct pinctrl_ops at91_pctrl_ops = {
 	.dt_free_map		= at91_dt_free_map,
 };
 
-static void __iomem * pin_to_controller(struct at91_pinctrl *info,
+static void __iomem *pin_to_controller(struct at91_pinctrl *info,
 				 unsigned int bank)
 {
 	return gpio_chips[bank]->regbase;
@@ -501,7 +501,7 @@ static void at91_pin_dbg(const struct device *dev, const struct at91_pmx_pin *pi
 	}
 }
 
-static int pin_check_config(struct at91_pinctrl *info, const char* name,
+static int pin_check_config(struct at91_pinctrl *info, const char *name,
 			    int index, const struct at91_pmx_pin *pin)
 {
 	int mux;
@@ -579,7 +579,7 @@ static int at91_pmx_enable(struct pinctrl_dev *pctldev, unsigned selector,
 		pio = pin_to_controller(info, pin->bank);
 		mask = pin_to_mask(pin->pin);
 		at91_mux_disable_interrupt(pio, mask);
-		switch(pin->mux) {
+		switch (pin->mux) {
 		case AT91_MUX_GPIO:
 			at91_mux_gpio_enable(pio, mask, 1);
 			break;
@@ -696,7 +696,7 @@ static void at91_gpio_disable_free(struct pinctrl_dev *pctldev,
 	/* Set the pin to some default state, GPIO is usually default */
 }
 
-static struct pinmux_ops at91_pmx_ops = {
+static const struct pinmux_ops at91_pmx_ops = {
 	.get_functions_count	= at91_pmx_get_funcs_count,
 	.get_function_name	= at91_pmx_get_func_name,
 	.get_function_groups	= at91_pmx_get_groups,
@@ -776,7 +776,7 @@ static void at91_pinconf_group_dbg_show(struct pinctrl_dev *pctldev,
 {
 }
 
-static struct pinconf_ops at91_pinconf_ops = {
+static const struct pinconf_ops at91_pinconf_ops = {
 	.pin_config_get			= at91_pinconf_get,
 	.pin_config_set			= at91_pinconf_set,
 	.pin_config_dbg_show		= at91_pinconf_dbg_show,
@@ -812,7 +812,7 @@ static int at91_pinctrl_mux_mask(struct at91_pinctrl *info,
 {
 	int ret = 0;
 	int size;
-	const const __be32 *list;
+	const __be32 *list;
 
 	list = of_get_property(np, "atmel,mux-mask", &size);
 	if (!list) {
@@ -846,7 +846,7 @@ static int at91_pinctrl_parse_groups(struct device_node *np,
 {
 	struct at91_pmx_pin *pin;
 	int size;
-	const const __be32 *list;
+	const __be32 *list;
 	int i, j;
 
 	dev_dbg(info->dev, "group(%d): %s\n", index, np->name);
@@ -944,7 +944,7 @@ static int at91_pinctrl_probe_dt(struct platform_device *pdev,
 		return -ENODEV;
 
 	info->dev = &pdev->dev;
-	info->ops = (struct at91_pinctrl_mux_ops*)
+	info->ops = (struct at91_pinctrl_mux_ops *)
 		of_match_device(at91_pinctrl_of_match, &pdev->dev)->data;
 	at91_pinctrl_child_count(info, np);
 
@@ -1002,7 +1002,7 @@ static int at91_pinctrl_probe(struct platform_device *pdev)
 {
 	struct at91_pinctrl *info;
 	struct pinctrl_pin_desc *pdesc;
-	int ret, i, j ,k;
+	int ret, i, j, k;
 
 	info = devm_kzalloc(&pdev->dev, sizeof(*info), GFP_KERNEL);
 	if (!info)
@@ -1568,7 +1568,7 @@ static int at91_gpio_probe(struct platform_device *pdev)
 		goto err;
 	}
 
-	at91_chip->ops = (struct at91_pinctrl_mux_ops*)
+	at91_chip->ops = (struct at91_pinctrl_mux_ops *)
 		of_match_device(at91_gpio_of_match, &pdev->dev)->data;
 	at91_chip->pioc_virq = irq;
 	at91_chip->pioc_idx = alias_idx;
@@ -1605,7 +1605,8 @@ static int at91_gpio_probe(struct platform_device *pdev)
 			chip->ngpio = ngpio;
 	}
 
-	names = devm_kzalloc(&pdev->dev, sizeof(char*) * chip->ngpio, GFP_KERNEL);
+	names = devm_kzalloc(&pdev->dev, sizeof(char *) * chip->ngpio,
+			     GFP_KERNEL);
 
 	if (!names) {
 		ret = -ENOMEM;
@@ -1615,7 +1616,7 @@ static int at91_gpio_probe(struct platform_device *pdev)
 	for (i = 0; i < chip->ngpio; i++)
 		names[i] = kasprintf(GFP_KERNEL, "pio%c%d", alias_idx + 'A', i);
 
-	chip->names = (const char*const*)names;
+	chip->names = (const char *const *)names;
 
 	range = &at91_chip->range;
 	range->name = chip->label;
diff --git a/drivers/pinctrl/pinctrl-bcm2835.c b/drivers/pinctrl/pinctrl-bcm2835.c
index 4eb6d2c4e4d..f28d4b08771 100644
--- a/drivers/pinctrl/pinctrl-bcm2835.c
+++ b/drivers/pinctrl/pinctrl-bcm2835.c
@@ -795,7 +795,7 @@ out:
 	return err;
 }
 
-static struct pinctrl_ops bcm2835_pctl_ops = {
+static const struct pinctrl_ops bcm2835_pctl_ops = {
 	.get_groups_count = bcm2835_pctl_get_groups_count,
 	.get_group_name = bcm2835_pctl_get_group_name,
 	.get_group_pins = bcm2835_pctl_get_group_pins,
@@ -872,7 +872,7 @@ static int bcm2835_pmx_gpio_set_direction(struct pinctrl_dev *pctldev,
 	return 0;
 }
 
-static struct pinmux_ops bcm2835_pmx_ops = {
+static const struct pinmux_ops bcm2835_pmx_ops = {
 	.get_functions_count = bcm2835_pmx_get_functions_count,
 	.get_function_name = bcm2835_pmx_get_function_name,
 	.get_function_groups = bcm2835_pmx_get_function_groups,
@@ -916,7 +916,7 @@ static int bcm2835_pinconf_set(struct pinctrl_dev *pctldev,
 	return 0;
 }
 
-static struct pinconf_ops bcm2835_pinconf_ops = {
+static const struct pinconf_ops bcm2835_pinconf_ops = {
 	.pin_config_get = bcm2835_pinconf_get,
 	.pin_config_set = bcm2835_pinconf_set,
 };
diff --git a/drivers/pinctrl/pinctrl-coh901.c b/drivers/pinctrl/pinctrl-coh901.c
index 8b7e7bc2226..edde3acc418 100644
--- a/drivers/pinctrl/pinctrl-coh901.c
+++ b/drivers/pinctrl/pinctrl-coh901.c
@@ -318,13 +318,16 @@ static int u300_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 	struct u300_gpio_port *port = NULL;
 	struct list_head *p;
 	int retirq;
+	bool found = false;
 
 	list_for_each(p, &gpio->port_list) {
 		port = list_entry(p, struct u300_gpio_port, node);
-		if (port->number == portno)
+		if (port->number == portno) {
+			found = true;
 			break;
+		}
 	}
-	if (port == NULL) {
+	if (!found) {
 		dev_err(gpio->dev, "could not locate port for GPIO %d IRQ\n",
 			offset);
 		return -EINVAL;
@@ -359,7 +362,7 @@ int u300_gpio_config_get(struct gpio_chip *chip,
 	drmode &= (U300_GPIO_PXPCR_PIN_MODE_MASK << ((offset & 0x07) << 1));
 	drmode >>= ((offset & 0x07) << 1);
 
-	switch(param) {
+	switch (param) {
 	case PIN_CONFIG_BIAS_HIGH_IMPEDANCE:
 		*config = 0;
 		if (biasmode)
diff --git a/drivers/pinctrl/pinctrl-exynos.c b/drivers/pinctrl/pinctrl-exynos.c
index 538b9ddaadf..8b10b1ac907 100644
--- a/drivers/pinctrl/pinctrl-exynos.c
+++ b/drivers/pinctrl/pinctrl-exynos.c
@@ -26,6 +26,7 @@
 #include <linux/of_irq.h>
 #include <linux/io.h>
 #include <linux/slab.h>
+#include <linux/spinlock.h>
 #include <linux/err.h>
 
 #include <asm/mach/irq.h>
@@ -33,6 +34,17 @@
 #include "pinctrl-samsung.h"
 #include "pinctrl-exynos.h"
 
+
+static struct samsung_pin_bank_type bank_type_off = {
+	.fld_width = { 4, 1, 2, 2, 2, 2, },
+	.reg_offset = { 0x00, 0x04, 0x08, 0x0c, 0x10, 0x14, },
+};
+
+static struct samsung_pin_bank_type bank_type_alive = {
+	.fld_width = { 4, 1, 2, 2, },
+	.reg_offset = { 0x00, 0x04, 0x08, 0x0c, },
+};
+
 /* list of external wakeup controllers supported */
 static const struct of_device_id exynos_wkup_irq_ids[] = {
 	{ .compatible = "samsung,exynos4210-wakeup-eint", },
@@ -75,12 +87,14 @@ static void exynos_gpio_irq_ack(struct irq_data *irqd)
 static int exynos_gpio_irq_set_type(struct irq_data *irqd, unsigned int type)
 {
 	struct samsung_pin_bank *bank = irq_data_get_irq_chip_data(irqd);
+	struct samsung_pin_bank_type *bank_type = bank->type;
 	struct samsung_pinctrl_drv_data *d = bank->drvdata;
 	struct samsung_pin_ctrl *ctrl = d->ctrl;
 	unsigned int pin = irqd->hwirq;
 	unsigned int shift = EXYNOS_EINT_CON_LEN * pin;
 	unsigned int con, trig_type;
 	unsigned long reg_con = ctrl->geint_con + bank->eint_offset;
+	unsigned long flags;
 	unsigned int mask;
 
 	switch (type) {
@@ -114,15 +128,19 @@ static int exynos_gpio_irq_set_type(struct irq_data *irqd, unsigned int type)
 	con |= trig_type << shift;
 	writel(con, d->virt_base + reg_con);
 
-	reg_con = bank->pctl_offset;
-	shift = pin * bank->func_width;
-	mask = (1 << bank->func_width) - 1;
+	reg_con = bank->pctl_offset + bank_type->reg_offset[PINCFG_TYPE_FUNC];
+	shift = pin * bank_type->fld_width[PINCFG_TYPE_FUNC];
+	mask = (1 << bank_type->fld_width[PINCFG_TYPE_FUNC]) - 1;
+
+	spin_lock_irqsave(&bank->slock, flags);
 
 	con = readl(d->virt_base + reg_con);
 	con &= ~(mask << shift);
 	con |= EXYNOS_EINT_FUNC << shift;
 	writel(con, d->virt_base + reg_con);
 
+	spin_unlock_irqrestore(&bank->slock, flags);
+
 	return 0;
 }
 
@@ -253,11 +271,13 @@ static void exynos_wkup_irq_ack(struct irq_data *irqd)
 static int exynos_wkup_irq_set_type(struct irq_data *irqd, unsigned int type)
 {
 	struct samsung_pin_bank *bank = irq_data_get_irq_chip_data(irqd);
+	struct samsung_pin_bank_type *bank_type = bank->type;
 	struct samsung_pinctrl_drv_data *d = bank->drvdata;
 	unsigned int pin = irqd->hwirq;
 	unsigned long reg_con = d->ctrl->weint_con + bank->eint_offset;
 	unsigned long shift = EXYNOS_EINT_CON_LEN * pin;
 	unsigned long con, trig_type;
+	unsigned long flags;
 	unsigned int mask;
 
 	switch (type) {
@@ -291,15 +311,19 @@ static int exynos_wkup_irq_set_type(struct irq_data *irqd, unsigned int type)
 	con |= trig_type << shift;
 	writel(con, d->virt_base + reg_con);
 
-	reg_con = bank->pctl_offset;
-	shift = pin * bank->func_width;
-	mask = (1 << bank->func_width) - 1;
+	reg_con = bank->pctl_offset + bank_type->reg_offset[PINCFG_TYPE_FUNC];
+	shift = pin * bank_type->fld_width[PINCFG_TYPE_FUNC];
+	mask = (1 << bank_type->fld_width[PINCFG_TYPE_FUNC]) - 1;
+
+	spin_lock_irqsave(&bank->slock, flags);
 
 	con = readl(d->virt_base + reg_con);
 	con &= ~(mask << shift);
 	con |= EXYNOS_EINT_FUNC << shift;
 	writel(con, d->virt_base + reg_con);
 
+	spin_unlock_irqrestore(&bank->slock, flags);
+
 	return 0;
 }
 
diff --git a/drivers/pinctrl/pinctrl-exynos.h b/drivers/pinctrl/pinctrl-exynos.h
index 0a708890d8b..9b1f77a5bf0 100644
--- a/drivers/pinctrl/pinctrl-exynos.h
+++ b/drivers/pinctrl/pinctrl-exynos.h
@@ -48,26 +48,18 @@
 
 #define EXYNOS_PIN_BANK_EINTN(pins, reg, id)		\
 	{						\
+		.type		= &bank_type_off,	\
 		.pctl_offset	= reg,			\
 		.nr_pins	= pins,			\
-		.func_width	= 4,			\
-		.pud_width	= 2,			\
-		.drv_width	= 2,			\
-		.conpdn_width	= 2,			\
-		.pudpdn_width	= 2,			\
 		.eint_type	= EINT_TYPE_NONE,	\
 		.name		= id			\
 	}
 
 #define EXYNOS_PIN_BANK_EINTG(pins, reg, id, offs)	\
 	{						\
+		.type		= &bank_type_off,	\
 		.pctl_offset	= reg,			\
 		.nr_pins	= pins,			\
-		.func_width	= 4,			\
-		.pud_width	= 2,			\
-		.drv_width	= 2,			\
-		.conpdn_width	= 2,			\
-		.pudpdn_width	= 2,			\
 		.eint_type	= EINT_TYPE_GPIO,	\
 		.eint_offset	= offs,			\
 		.name		= id			\
@@ -75,11 +67,9 @@
 
 #define EXYNOS_PIN_BANK_EINTW(pins, reg, id, offs)	\
 	{						\
+		.type		= &bank_type_alive,	\
 		.pctl_offset	= reg,			\
 		.nr_pins	= pins,			\
-		.func_width	= 4,			\
-		.pud_width	= 2,			\
-		.drv_width	= 2,			\
 		.eint_type	= EINT_TYPE_WKUP,	\
 		.eint_offset	= offs,			\
 		.name		= id			\
diff --git a/drivers/pinctrl/pinctrl-exynos5440.c b/drivers/pinctrl/pinctrl-exynos5440.c
index 1376eb7305d..6038503ed92 100644
--- a/drivers/pinctrl/pinctrl-exynos5440.c
+++ b/drivers/pinctrl/pinctrl-exynos5440.c
@@ -20,6 +20,9 @@
 #include <linux/pinctrl/pinctrl.h>
 #include <linux/pinctrl/pinmux.h>
 #include <linux/pinctrl/pinconf.h>
+#include <linux/interrupt.h>
+#include <linux/irqdomain.h>
+#include <linux/of_irq.h>
 #include "core.h"
 
 /* EXYNOS5440 GPIO and Pinctrl register offsets */
@@ -37,6 +40,7 @@
 #define GPIO_DS1		0x2C
 
 #define EXYNOS5440_MAX_PINS		23
+#define EXYNOS5440_MAX_GPIO_INT	8
 #define PIN_NAME_LENGTH		10
 
 #define GROUP_SUFFIX		"-grp"
@@ -109,6 +113,7 @@ struct exynos5440_pmx_func {
 struct exynos5440_pinctrl_priv_data {
 	void __iomem			*reg_base;
 	struct gpio_chip		*gc;
+	struct irq_domain		*irq_domain;
 
 	const struct exynos5440_pin_group	*pin_groups;
 	unsigned int			nr_groups;
@@ -116,6 +121,16 @@ struct exynos5440_pinctrl_priv_data {
 	unsigned int			nr_functions;
 };
 
+/**
+ * struct exynos5440_gpio_intr_data: private data for gpio interrupts.
+ * @priv: driver's private runtime data.
+ * @gpio_int: gpio interrupt number.
+ */
+struct exynos5440_gpio_intr_data {
+	struct exynos5440_pinctrl_priv_data	*priv;
+	unsigned int				gpio_int;
+};
+
 /* list of all possible config options supported */
 static struct pin_config {
 	char		*prop_cfg;
@@ -286,7 +301,7 @@ static void exynos5440_dt_free_map(struct pinctrl_dev *pctldev,
 }
 
 /* list of pinctrl callbacks for the pinctrl core */
-static struct pinctrl_ops exynos5440_pctrl_ops = {
+static const struct pinctrl_ops exynos5440_pctrl_ops = {
 	.get_groups_count	= exynos5440_get_group_count,
 	.get_group_name		= exynos5440_get_group_name,
 	.get_group_pins		= exynos5440_get_group_pins,
@@ -374,7 +389,7 @@ static int exynos5440_pinmux_gpio_set_direction(struct pinctrl_dev *pctldev,
 }
 
 /* list of pinmux callbacks for the pinmux vertical in pinctrl core */
-static struct pinmux_ops exynos5440_pinmux_ops = {
+static const struct pinmux_ops exynos5440_pinmux_ops = {
 	.get_functions_count	= exynos5440_get_functions_count,
 	.get_function_name	= exynos5440_pinmux_get_fname,
 	.get_function_groups	= exynos5440_pinmux_get_groups,
@@ -523,7 +538,7 @@ static int exynos5440_pinconf_group_get(struct pinctrl_dev *pctldev,
 }
 
 /* list of pinconfig callbacks for pinconfig vertical in the pinctrl code */
-static struct pinconf_ops exynos5440_pinconf_ops = {
+static const struct pinconf_ops exynos5440_pinconf_ops = {
 	.pin_config_get		= exynos5440_pinconf_get,
 	.pin_config_set		= exynos5440_pinconf_set,
 	.pin_config_group_get	= exynos5440_pinconf_group_get,
@@ -598,6 +613,22 @@ static int exynos5440_gpio_direction_output(struct gpio_chip *gc, unsigned offse
 	return 0;
 }
 
+/* gpiolib gpio_to_irq callback function */
+static int exynos5440_gpio_to_irq(struct gpio_chip *gc, unsigned offset)
+{
+	struct exynos5440_pinctrl_priv_data *priv = dev_get_drvdata(gc->dev);
+	unsigned int virq;
+
+	if (offset < 16 || offset > 23)
+		return -ENXIO;
+
+	if (!priv->irq_domain)
+		return -ENXIO;
+
+	virq = irq_create_mapping(priv->irq_domain, offset - 16);
+	return virq ? : -ENXIO;
+}
+
 /* parse the pin numbers listed in the 'samsung,exynos5440-pins' property */
 static int exynos5440_pinctrl_parse_dt_pins(struct platform_device *pdev,
 			struct device_node *cfg_np, unsigned int **pin_list,
@@ -670,8 +701,10 @@ static int exynos5440_pinctrl_parse_dt(struct platform_device *pdev,
 
 		ret = exynos5440_pinctrl_parse_dt_pins(pdev, cfg_np,
 					&pin_list, &npins);
-		if (ret)
-			return ret;
+		if (ret) {
+			gname = NULL;
+			goto skip_to_pin_function;
+		}
 
 		/* derive pin group name from the node name */
 		gname = devm_kzalloc(dev, strlen(cfg_np->name) + GSUFFIX_LEN,
@@ -687,6 +720,7 @@ static int exynos5440_pinctrl_parse_dt(struct platform_device *pdev,
 		grp->num_pins = npins;
 		grp++;
 
+skip_to_pin_function:
 		ret = of_property_read_u32(cfg_np, "samsung,exynos5440-pin-function",
 						&function);
 		if (ret)
@@ -709,7 +743,7 @@ static int exynos5440_pinctrl_parse_dt(struct platform_device *pdev,
 			return -ENOMEM;
 		}
 		func->groups[0] = gname;
-		func->num_groups = 1;
+		func->num_groups = gname ? 1 : 0;
 		func->function = function;
 		func++;
 		func_idx++;
@@ -818,6 +852,7 @@ static int exynos5440_gpiolib_register(struct platform_device *pdev,
 	gc->get = exynos5440_gpio_get;
 	gc->direction_input = exynos5440_gpio_direction_input;
 	gc->direction_output = exynos5440_gpio_direction_output;
+	gc->to_irq = exynos5440_gpio_to_irq;
 	gc->label = "gpiolib-exynos5440";
 	gc->owner = THIS_MODULE;
 	ret = gpiochip_add(gc);
@@ -842,6 +877,110 @@ static int exynos5440_gpiolib_unregister(struct platform_device *pdev,
 	return 0;
 }
 
+static void exynos5440_gpio_irq_unmask(struct irq_data *irqd)
+{
+	struct exynos5440_pinctrl_priv_data *d;
+	unsigned long gpio_int;
+
+	d = irq_data_get_irq_chip_data(irqd);
+	gpio_int = readl(d->reg_base + GPIO_INT);
+	gpio_int |= 1 << irqd->hwirq;
+	writel(gpio_int, d->reg_base + GPIO_INT);
+}
+
+static void exynos5440_gpio_irq_mask(struct irq_data *irqd)
+{
+	struct exynos5440_pinctrl_priv_data *d;
+	unsigned long gpio_int;
+
+	d = irq_data_get_irq_chip_data(irqd);
+	gpio_int = readl(d->reg_base + GPIO_INT);
+	gpio_int &= ~(1 << irqd->hwirq);
+	writel(gpio_int, d->reg_base + GPIO_INT);
+}
+
+/* irq_chip for gpio interrupts */
+static struct irq_chip exynos5440_gpio_irq_chip = {
+	.name		= "exynos5440_gpio_irq_chip",
+	.irq_unmask	= exynos5440_gpio_irq_unmask,
+	.irq_mask	= exynos5440_gpio_irq_mask,
+};
+
+/* interrupt handler for GPIO interrupts 0..7 */
+static irqreturn_t exynos5440_gpio_irq(int irq, void *data)
+{
+	struct exynos5440_gpio_intr_data *intd = data;
+	struct exynos5440_pinctrl_priv_data *d = intd->priv;
+	int virq;
+
+	virq = irq_linear_revmap(d->irq_domain, intd->gpio_int);
+	if (!virq)
+		return IRQ_NONE;
+	generic_handle_irq(virq);
+	return IRQ_HANDLED;
+}
+
+static int exynos5440_gpio_irq_map(struct irq_domain *h, unsigned int virq,
+					irq_hw_number_t hw)
+{
+	struct exynos5440_pinctrl_priv_data *d = h->host_data;
+
+	irq_set_chip_data(virq, d);
+	irq_set_chip_and_handler(virq, &exynos5440_gpio_irq_chip,
+					handle_level_irq);
+	set_irq_flags(virq, IRQF_VALID);
+	return 0;
+}
+
+/* irq domain callbacks for gpio interrupt controller */
+static const struct irq_domain_ops exynos5440_gpio_irqd_ops = {
+	.map	= exynos5440_gpio_irq_map,
+	.xlate	= irq_domain_xlate_twocell,
+};
+
+/* setup handling of gpio interrupts */
+static int exynos5440_gpio_irq_init(struct platform_device *pdev,
+				struct exynos5440_pinctrl_priv_data *priv)
+{
+	struct device *dev = &pdev->dev;
+	struct exynos5440_gpio_intr_data *intd;
+	int i, irq, ret;
+
+	intd = devm_kzalloc(dev, sizeof(*intd) * EXYNOS5440_MAX_GPIO_INT,
+					GFP_KERNEL);
+	if (!intd) {
+		dev_err(dev, "failed to allocate memory for gpio intr data\n");
+		return -ENOMEM;
+	}
+
+	for (i = 0; i < EXYNOS5440_MAX_GPIO_INT; i++) {
+		irq = irq_of_parse_and_map(dev->of_node, i);
+		if (irq <= 0) {
+			dev_err(dev, "irq parsing failed\n");
+			return -EINVAL;
+		}
+
+		intd->gpio_int = i;
+		intd->priv = priv;
+		ret = devm_request_irq(dev, irq, exynos5440_gpio_irq,
+					0, dev_name(dev), intd++);
+		if (ret) {
+			dev_err(dev, "irq request failed\n");
+			return -ENXIO;
+		}
+	}
+
+	priv->irq_domain = irq_domain_add_linear(dev->of_node,
+				EXYNOS5440_MAX_GPIO_INT,
+				&exynos5440_gpio_irqd_ops, priv);
+	if (!priv->irq_domain) {
+		dev_err(dev, "failed to create irq domain\n");
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
 static int exynos5440_pinctrl_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
@@ -854,7 +993,7 @@ static int exynos5440_pinctrl_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	priv = devm_kzalloc(dev, sizeof(priv), GFP_KERNEL);
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
 	if (!priv) {
 		dev_err(dev, "could not allocate memory for private data\n");
 		return -ENOMEM;
@@ -880,6 +1019,12 @@ static int exynos5440_pinctrl_probe(struct platform_device *pdev)
 		return ret;
 	}
 
+	ret = exynos5440_gpio_irq_init(pdev, priv);
+	if (ret) {
+		dev_err(dev, "failed to setup gpio interrupts\n");
+		return ret;
+	}
+
 	platform_set_drvdata(pdev, priv);
 	dev_info(dev, "EXYNOS5440 pinctrl driver registered\n");
 	return 0;
diff --git a/drivers/pinctrl/pinctrl-falcon.c b/drivers/pinctrl/pinctrl-falcon.c
index af97a1f9000..f9b2a1d4854 100644
--- a/drivers/pinctrl/pinctrl-falcon.c
+++ b/drivers/pinctrl/pinctrl-falcon.c
@@ -353,7 +353,7 @@ static void falcon_pinconf_group_dbg_show(struct pinctrl_dev *pctrldev,
 {
 }
 
-static struct pinconf_ops falcon_pinconf_ops = {
+static const struct pinconf_ops falcon_pinconf_ops = {
 	.pin_config_get			= falcon_pinconf_get,
 	.pin_config_set			= falcon_pinconf_set,
 	.pin_config_group_get		= falcon_pinconf_group_get,
diff --git a/drivers/pinctrl/pinctrl-imx.c b/drivers/pinctrl/pinctrl-imx.c
index 4cebb9c6c5c..0ef190449ea 100644
--- a/drivers/pinctrl/pinctrl-imx.c
+++ b/drivers/pinctrl/pinctrl-imx.c
@@ -207,7 +207,7 @@ static void imx_dt_free_map(struct pinctrl_dev *pctldev,
 	kfree(map);
 }
 
-static struct pinctrl_ops imx_pctrl_ops = {
+static const struct pinctrl_ops imx_pctrl_ops = {
 	.get_groups_count = imx_get_groups_count,
 	.get_group_name = imx_get_group_name,
 	.get_group_pins = imx_get_group_pins,
@@ -299,7 +299,7 @@ static int imx_pmx_get_groups(struct pinctrl_dev *pctldev, unsigned selector,
 	return 0;
 }
 
-static struct pinmux_ops imx_pmx_ops = {
+static const struct pinmux_ops imx_pmx_ops = {
 	.get_functions_count = imx_pmx_get_funcs_count,
 	.get_function_name = imx_pmx_get_func_name,
 	.get_function_groups = imx_pmx_get_groups,
@@ -397,7 +397,7 @@ static void imx_pinconf_group_dbg_show(struct pinctrl_dev *pctldev,
 	}
 }
 
-static struct pinconf_ops imx_pinconf_ops = {
+static const struct pinconf_ops imx_pinconf_ops = {
 	.pin_config_get = imx_pinconf_get,
 	.pin_config_set = imx_pinconf_set,
 	.pin_config_dbg_show = imx_pinconf_dbg_show,
diff --git a/drivers/pinctrl/pinctrl-lantiq.c b/drivers/pinctrl/pinctrl-lantiq.c
index a7038461135..615c5002b75 100644
--- a/drivers/pinctrl/pinctrl-lantiq.c
+++ b/drivers/pinctrl/pinctrl-lantiq.c
@@ -169,7 +169,7 @@ static int ltq_pinctrl_dt_node_to_map(struct pinctrl_dev *pctldev,
 	return 0;
 }
 
-static struct pinctrl_ops ltq_pctrl_ops = {
+static const struct pinctrl_ops ltq_pctrl_ops = {
 	.get_groups_count	= ltq_get_group_count,
 	.get_group_name		= ltq_get_group_name,
 	.get_group_pins		= ltq_get_group_pins,
@@ -311,7 +311,7 @@ static int ltq_pmx_gpio_request_enable(struct pinctrl_dev *pctrldev,
 	return info->apply_mux(pctrldev, mfp, pin_func);
 }
 
-static struct pinmux_ops ltq_pmx_ops = {
+static const struct pinmux_ops ltq_pmx_ops = {
 	.get_functions_count	= ltq_pmx_func_count,
 	.get_function_name	= ltq_pmx_func_name,
 	.get_function_groups	= ltq_pmx_get_groups,
diff --git a/drivers/pinctrl/pinctrl-mmp2.c b/drivers/pinctrl/pinctrl-mmp2.c
deleted file mode 100644
index 4afa56a3a51..00000000000
--- a/drivers/pinctrl/pinctrl-mmp2.c
+++ /dev/null
@@ -1,722 +0,0 @@
-/*
- *  linux/drivers/pinctrl/pinmux-mmp2.c
- *
- *  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
- *  publishhed by the Free Software Foundation.
- *
- *  Copyright (C) 2011, Marvell Technology Group Ltd.
- *
- *  Author: Haojian Zhuang <haojian.zhuang@marvell.com>
- *
- */
-
-#include <linux/device.h>
-#include <linux/module.h>
-#include <linux/io.h>
-#include <linux/platform_device.h>
-#include "pinctrl-pxa3xx.h"
-
-#define MMP2_DS_MASK		0x1800
-#define MMP2_DS_SHIFT		11
-#define MMP2_SLEEP_MASK		0x38
-#define MMP2_SLEEP_SELECT	(1 << 9)
-#define MMP2_SLEEP_DATA		(1 << 8)
-#define MMP2_SLEEP_DIR		(1 << 7)
-
-#define MFPR_MMP2(a, r, f0, f1, f2, f3, f4, f5, f6, f7)		\
-	{							\
-		.name = #a,					\
-		.pin = a,					\
-		.mfpr = r,					\
-		.func = {					\
-			MMP2_MUX_##f0,				\
-			MMP2_MUX_##f1,				\
-			MMP2_MUX_##f2,				\
-			MMP2_MUX_##f3,				\
-			MMP2_MUX_##f4,				\
-			MMP2_MUX_##f5,				\
-			MMP2_MUX_##f6,				\
-			MMP2_MUX_##f7,				\
-		},						\
-	}
-
-#define GRP_MMP2(a, m, p)		\
-	{ .name = a, .mux = MMP2_MUX_##m, .pins = p, .npins = ARRAY_SIZE(p), }
-
-/* 174 pins */
-enum mmp2_pin_list {
-	/* 0~168: GPIO0~GPIO168 */
-	TWSI4_SCL = 169,
-	TWSI4_SDA, /* 170 */
-	G_CLKREQ,
-	VCXO_REQ,
-	VCXO_OUT,
-};
-
-enum mmp2_mux {
-	/* PXA3xx_MUX_GPIO = 0 (predefined in pinctrl-pxa3xx.h) */
-	MMP2_MUX_GPIO = 0,
-	MMP2_MUX_G_CLKREQ,
-	MMP2_MUX_VCXO_REQ,
-	MMP2_MUX_VCXO_OUT,
-	MMP2_MUX_KP_MK,
-	MMP2_MUX_KP_DK,
-	MMP2_MUX_CCIC1,
-	MMP2_MUX_CCIC2,
-	MMP2_MUX_SPI,
-	MMP2_MUX_SSPA2,
-	MMP2_MUX_ROT,
-	MMP2_MUX_I2S,
-	MMP2_MUX_TB,
-	MMP2_MUX_CAM2,
-	MMP2_MUX_HDMI,
-	MMP2_MUX_TWSI2,
-	MMP2_MUX_TWSI3,
-	MMP2_MUX_TWSI4,
-	MMP2_MUX_TWSI5,
-	MMP2_MUX_TWSI6,
-	MMP2_MUX_UART1,
-	MMP2_MUX_UART2,
-	MMP2_MUX_UART3,
-	MMP2_MUX_UART4,
-	MMP2_MUX_SSP1_RX,
-	MMP2_MUX_SSP1_FRM,
-	MMP2_MUX_SSP1_TXRX,
-	MMP2_MUX_SSP2_RX,
-	MMP2_MUX_SSP2_FRM,
-	MMP2_MUX_SSP1,
-	MMP2_MUX_SSP2,
-	MMP2_MUX_SSP3,
-	MMP2_MUX_SSP4,
-	MMP2_MUX_MMC1,
-	MMP2_MUX_MMC2,
-	MMP2_MUX_MMC3,
-	MMP2_MUX_MMC4,
-	MMP2_MUX_ULPI,
-	MMP2_MUX_AC,
-	MMP2_MUX_CA,
-	MMP2_MUX_PWM,
-	MMP2_MUX_USIM,
-	MMP2_MUX_TIPU,
-	MMP2_MUX_PLL,
-	MMP2_MUX_NAND,
-	MMP2_MUX_FSIC,
-	MMP2_MUX_SLEEP_IND,
-	MMP2_MUX_EXT_DMA,
-	MMP2_MUX_ONE_WIRE,
-	MMP2_MUX_LCD,
-	MMP2_MUX_SMC,
-	MMP2_MUX_SMC_INT,
-	MMP2_MUX_MSP,
-	MMP2_MUX_G_CLKOUT,
-	MMP2_MUX_32K_CLKOUT,
-	MMP2_MUX_PRI_JTAG,
-	MMP2_MUX_AAS_JTAG,
-	MMP2_MUX_AAS_GPIO,
-	MMP2_MUX_AAS_SPI,
-	MMP2_MUX_AAS_TWSI,
-	MMP2_MUX_AAS_DEU_EX,
-	MMP2_MUX_NONE = 0xffff,
-};
-
-static struct pinctrl_pin_desc mmp2_pads[] = {
-	/*
-	 * The name indicates function 0 of this pin.
-	 * After reset, function 0 is the default function of pin.
-	 */
-	PINCTRL_PIN(GPIO0, "GPIO0"),
-	PINCTRL_PIN(GPIO1, "GPIO1"),
-	PINCTRL_PIN(GPIO2, "GPIO2"),
-	PINCTRL_PIN(GPIO3, "GPIO3"),
-	PINCTRL_PIN(GPIO4, "GPIO4"),
-	PINCTRL_PIN(GPIO5, "GPIO5"),
-	PINCTRL_PIN(GPIO6, "GPIO6"),
-	PINCTRL_PIN(GPIO7, "GPIO7"),
-	PINCTRL_PIN(GPIO8, "GPIO8"),
-	PINCTRL_PIN(GPIO9, "GPIO9"),
-	PINCTRL_PIN(GPIO10, "GPIO10"),
-	PINCTRL_PIN(GPIO11, "GPIO11"),
-	PINCTRL_PIN(GPIO12, "GPIO12"),
-	PINCTRL_PIN(GPIO13, "GPIO13"),
-	PINCTRL_PIN(GPIO14, "GPIO14"),
-	PINCTRL_PIN(GPIO15, "GPIO15"),
-	PINCTRL_PIN(GPIO16, "GPIO16"),
-	PINCTRL_PIN(GPIO17, "GPIO17"),
-	PINCTRL_PIN(GPIO18, "GPIO18"),
-	PINCTRL_PIN(GPIO19, "GPIO19"),
-	PINCTRL_PIN(GPIO20, "GPIO20"),
-	PINCTRL_PIN(GPIO21, "GPIO21"),
-	PINCTRL_PIN(GPIO22, "GPIO22"),
-	PINCTRL_PIN(GPIO23, "GPIO23"),
-	PINCTRL_PIN(GPIO24, "GPIO24"),
-	PINCTRL_PIN(GPIO25, "GPIO25"),
-	PINCTRL_PIN(GPIO26, "GPIO26"),
-	PINCTRL_PIN(GPIO27, "GPIO27"),
-	PINCTRL_PIN(GPIO28, "GPIO28"),
-	PINCTRL_PIN(GPIO29, "GPIO29"),
-	PINCTRL_PIN(GPIO30, "GPIO30"),
-	PINCTRL_PIN(GPIO31, "GPIO31"),
-	PINCTRL_PIN(GPIO32, "GPIO32"),
-	PINCTRL_PIN(GPIO33, "GPIO33"),
-	PINCTRL_PIN(GPIO34, "GPIO34"),
-	PINCTRL_PIN(GPIO35, "GPIO35"),
-	PINCTRL_PIN(GPIO36, "GPIO36"),
-	PINCTRL_PIN(GPIO37, "GPIO37"),
-	PINCTRL_PIN(GPIO38, "GPIO38"),
-	PINCTRL_PIN(GPIO39, "GPIO39"),
-	PINCTRL_PIN(GPIO40, "GPIO40"),
-	PINCTRL_PIN(GPIO41, "GPIO41"),
-	PINCTRL_PIN(GPIO42, "GPIO42"),
-	PINCTRL_PIN(GPIO43, "GPIO43"),
-	PINCTRL_PIN(GPIO44, "GPIO44"),
-	PINCTRL_PIN(GPIO45, "GPIO45"),
-	PINCTRL_PIN(GPIO46, "GPIO46"),
-	PINCTRL_PIN(GPIO47, "GPIO47"),
-	PINCTRL_PIN(GPIO48, "GPIO48"),
-	PINCTRL_PIN(GPIO49, "GPIO49"),
-	PINCTRL_PIN(GPIO50, "GPIO50"),
-	PINCTRL_PIN(GPIO51, "GPIO51"),
-	PINCTRL_PIN(GPIO52, "GPIO52"),
-	PINCTRL_PIN(GPIO53, "GPIO53"),
-	PINCTRL_PIN(GPIO54, "GPIO54"),
-	PINCTRL_PIN(GPIO55, "GPIO55"),
-	PINCTRL_PIN(GPIO56, "GPIO56"),
-	PINCTRL_PIN(GPIO57, "GPIO57"),
-	PINCTRL_PIN(GPIO58, "GPIO58"),
-	PINCTRL_PIN(GPIO59, "GPIO59"),
-	PINCTRL_PIN(GPIO60, "GPIO60"),
-	PINCTRL_PIN(GPIO61, "GPIO61"),
-	PINCTRL_PIN(GPIO62, "GPIO62"),
-	PINCTRL_PIN(GPIO63, "GPIO63"),
-	PINCTRL_PIN(GPIO64, "GPIO64"),
-	PINCTRL_PIN(GPIO65, "GPIO65"),
-	PINCTRL_PIN(GPIO66, "GPIO66"),
-	PINCTRL_PIN(GPIO67, "GPIO67"),
-	PINCTRL_PIN(GPIO68, "GPIO68"),
-	PINCTRL_PIN(GPIO69, "GPIO69"),
-	PINCTRL_PIN(GPIO70, "GPIO70"),
-	PINCTRL_PIN(GPIO71, "GPIO71"),
-	PINCTRL_PIN(GPIO72, "GPIO72"),
-	PINCTRL_PIN(GPIO73, "GPIO73"),
-	PINCTRL_PIN(GPIO74, "GPIO74"),
-	PINCTRL_PIN(GPIO75, "GPIO75"),
-	PINCTRL_PIN(GPIO76, "GPIO76"),
-	PINCTRL_PIN(GPIO77, "GPIO77"),
-	PINCTRL_PIN(GPIO78, "GPIO78"),
-	PINCTRL_PIN(GPIO79, "GPIO79"),
-	PINCTRL_PIN(GPIO80, "GPIO80"),
-	PINCTRL_PIN(GPIO81, "GPIO81"),
-	PINCTRL_PIN(GPIO82, "GPIO82"),
-	PINCTRL_PIN(GPIO83, "GPIO83"),
-	PINCTRL_PIN(GPIO84, "GPIO84"),
-	PINCTRL_PIN(GPIO85, "GPIO85"),
-	PINCTRL_PIN(GPIO86, "GPIO86"),
-	PINCTRL_PIN(GPIO87, "GPIO87"),
-	PINCTRL_PIN(GPIO88, "GPIO88"),
-	PINCTRL_PIN(GPIO89, "GPIO89"),
-	PINCTRL_PIN(GPIO90, "GPIO90"),
-	PINCTRL_PIN(GPIO91, "GPIO91"),
-	PINCTRL_PIN(GPIO92, "GPIO92"),
-	PINCTRL_PIN(GPIO93, "GPIO93"),
-	PINCTRL_PIN(GPIO94, "GPIO94"),
-	PINCTRL_PIN(GPIO95, "GPIO95"),
-	PINCTRL_PIN(GPIO96, "GPIO96"),
-	PINCTRL_PIN(GPIO97, "GPIO97"),
-	PINCTRL_PIN(GPIO98, "GPIO98"),
-	PINCTRL_PIN(GPIO99, "GPIO99"),
-	PINCTRL_PIN(GPIO100, "GPIO100"),
-	PINCTRL_PIN(GPIO101, "GPIO101"),
-	PINCTRL_PIN(GPIO102, "GPIO102"),
-	PINCTRL_PIN(GPIO103, "GPIO103"),
-	PINCTRL_PIN(GPIO104, "GPIO104"),
-	PINCTRL_PIN(GPIO105, "GPIO105"),
-	PINCTRL_PIN(GPIO106, "GPIO106"),
-	PINCTRL_PIN(GPIO107, "GPIO107"),
-	PINCTRL_PIN(GPIO108, "GPIO108"),
-	PINCTRL_PIN(GPIO109, "GPIO109"),
-	PINCTRL_PIN(GPIO110, "GPIO110"),
-	PINCTRL_PIN(GPIO111, "GPIO111"),
-	PINCTRL_PIN(GPIO112, "GPIO112"),
-	PINCTRL_PIN(GPIO113, "GPIO113"),
-	PINCTRL_PIN(GPIO114, "GPIO114"),
-	PINCTRL_PIN(GPIO115, "GPIO115"),
-	PINCTRL_PIN(GPIO116, "GPIO116"),
-	PINCTRL_PIN(GPIO117, "GPIO117"),
-	PINCTRL_PIN(GPIO118, "GPIO118"),
-	PINCTRL_PIN(GPIO119, "GPIO119"),
-	PINCTRL_PIN(GPIO120, "GPIO120"),
-	PINCTRL_PIN(GPIO121, "GPIO121"),
-	PINCTRL_PIN(GPIO122, "GPIO122"),
-	PINCTRL_PIN(GPIO123, "GPIO123"),
-	PINCTRL_PIN(GPIO124, "GPIO124"),
-	PINCTRL_PIN(GPIO125, "GPIO125"),
-	PINCTRL_PIN(GPIO126, "GPIO126"),
-	PINCTRL_PIN(GPIO127, "GPIO127"),
-	PINCTRL_PIN(GPIO128, "GPIO128"),
-	PINCTRL_PIN(GPIO129, "GPIO129"),
-	PINCTRL_PIN(GPIO130, "GPIO130"),
-	PINCTRL_PIN(GPIO131, "GPIO131"),
-	PINCTRL_PIN(GPIO132, "GPIO132"),
-	PINCTRL_PIN(GPIO133, "GPIO133"),
-	PINCTRL_PIN(GPIO134, "GPIO134"),
-	PINCTRL_PIN(GPIO135, "GPIO135"),
-	PINCTRL_PIN(GPIO136, "GPIO136"),
-	PINCTRL_PIN(GPIO137, "GPIO137"),
-	PINCTRL_PIN(GPIO138, "GPIO138"),
-	PINCTRL_PIN(GPIO139, "GPIO139"),
-	PINCTRL_PIN(GPIO140, "GPIO140"),
-	PINCTRL_PIN(GPIO141, "GPIO141"),
-	PINCTRL_PIN(GPIO142, "GPIO142"),
-	PINCTRL_PIN(GPIO143, "GPIO143"),
-	PINCTRL_PIN(GPIO144, "GPIO144"),
-	PINCTRL_PIN(GPIO145, "GPIO145"),
-	PINCTRL_PIN(GPIO146, "GPIO146"),
-	PINCTRL_PIN(GPIO147, "GPIO147"),
-	PINCTRL_PIN(GPIO148, "GPIO148"),
-	PINCTRL_PIN(GPIO149, "GPIO149"),
-	PINCTRL_PIN(GPIO150, "GPIO150"),
-	PINCTRL_PIN(GPIO151, "GPIO151"),
-	PINCTRL_PIN(GPIO152, "GPIO152"),
-	PINCTRL_PIN(GPIO153, "GPIO153"),
-	PINCTRL_PIN(GPIO154, "GPIO154"),
-	PINCTRL_PIN(GPIO155, "GPIO155"),
-	PINCTRL_PIN(GPIO156, "GPIO156"),
-	PINCTRL_PIN(GPIO157, "GPIO157"),
-	PINCTRL_PIN(GPIO158, "GPIO158"),
-	PINCTRL_PIN(GPIO159, "GPIO159"),
-	PINCTRL_PIN(GPIO160, "GPIO160"),
-	PINCTRL_PIN(GPIO161, "GPIO161"),
-	PINCTRL_PIN(GPIO162, "GPIO162"),
-	PINCTRL_PIN(GPIO163, "GPIO163"),
-	PINCTRL_PIN(GPIO164, "GPIO164"),
-	PINCTRL_PIN(GPIO165, "GPIO165"),
-	PINCTRL_PIN(GPIO166, "GPIO166"),
-	PINCTRL_PIN(GPIO167, "GPIO167"),
-	PINCTRL_PIN(GPIO168, "GPIO168"),
-	PINCTRL_PIN(TWSI4_SCL, "TWSI4_SCL"),
-	PINCTRL_PIN(TWSI4_SDA, "TWSI4_SDA"),
-	PINCTRL_PIN(G_CLKREQ, "G_CLKREQ"),
-	PINCTRL_PIN(VCXO_REQ, "VCXO_REQ"),
-	PINCTRL_PIN(VCXO_OUT, "VCXO_OUT"),
-};
-
-struct pxa3xx_mfp_pin mmp2_mfp[] = {
-	/*       pin         offs   f0        f1          f2          f3          f4          f5        f6        f7  */
-	MFPR_MMP2(GPIO0,     0x054, GPIO,     KP_MK,      NONE,       SPI,        NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO1,     0x058, GPIO,     KP_MK,      NONE,       SPI,        NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO2,     0x05C, GPIO,     KP_MK,      NONE,       SPI,        NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO3,     0x060, GPIO,     KP_MK,      NONE,       SPI,        NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO4,     0x064, GPIO,     KP_MK,      NONE,       NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO5,     0x068, GPIO,     KP_MK,      NONE,       SPI,        NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO6,     0x06C, GPIO,     KP_MK,      NONE,       SPI,        NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO7,     0x070, GPIO,     KP_MK,      NONE,       SPI,        NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO8,     0x074, GPIO,     KP_MK,      NONE,       NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO9,     0x078, GPIO,     KP_MK,      NONE,       NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO10,    0x07C, GPIO,     KP_MK,      NONE,       NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO11,    0x080, GPIO,     KP_MK,      NONE,       NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO12,    0x084, GPIO,     KP_MK,      NONE,       CCIC1,      NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO13,    0x088, GPIO,     KP_MK,      NONE,       CCIC1,      NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO14,    0x08C, GPIO,     KP_MK,      NONE,       CCIC1,      NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO15,    0x090, GPIO,     KP_MK,      KP_DK,      CCIC1,      NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO16,    0x094, GPIO,     KP_DK,      ROT,        CCIC1,      NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO17,    0x098, GPIO,     KP_DK,      ROT,        CCIC1,      NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO18,    0x09C, GPIO,     KP_DK,      ROT,        CCIC1,      NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO19,    0x0A0, GPIO,     KP_DK,      ROT,        CCIC1,      NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO20,    0x0A4, GPIO,     KP_DK,      TB,         CCIC1,      NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO21,    0x0A8, GPIO,     KP_DK,      TB,         CCIC1,      NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO22,    0x0AC, GPIO,     KP_DK,      TB,         CCIC1,      NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO23,    0x0B0, GPIO,     KP_DK,      TB,         CCIC1,      NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO24,    0x0B4, GPIO,     I2S,        VCXO_OUT,   NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO25,    0x0B8, GPIO,     I2S,        HDMI,       SSPA2,      NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO26,    0x0BC, GPIO,     I2S,        HDMI,       SSPA2,      NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO27,    0x0C0, GPIO,     I2S,        HDMI,       SSPA2,      NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO28,    0x0C4, GPIO,     I2S,        NONE,       SSPA2,      NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO29,    0x0C8, GPIO,     UART1,      KP_MK,      NONE,       NONE,       NONE,     AAS_SPI,  NONE),
-	MFPR_MMP2(GPIO30,    0x0CC, GPIO,     UART1,      KP_MK,      NONE,       NONE,       NONE,     AAS_SPI,  NONE),
-	MFPR_MMP2(GPIO31,    0x0D0, GPIO,     UART1,      KP_MK,      NONE,       NONE,       NONE,     AAS_SPI,  NONE),
-	MFPR_MMP2(GPIO32,    0x0D4, GPIO,     UART1,      KP_MK,      NONE,       NONE,       NONE,     AAS_SPI,  NONE),
-	MFPR_MMP2(GPIO33,    0x0D8, GPIO,     SSPA2,      I2S,        NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO34,    0x0DC, GPIO,     SSPA2,      I2S,        NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO35,    0x0E0, GPIO,     SSPA2,      I2S,        NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO36,    0x0E4, GPIO,     SSPA2,      I2S,        NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO37,    0x0E8, GPIO,     MMC2,       SSP1,       TWSI2,      UART2,      UART3,    AAS_SPI,  AAS_TWSI),
-	MFPR_MMP2(GPIO38,    0x0EC, GPIO,     MMC2,       SSP1,       TWSI2,      UART2,      UART3,    AAS_SPI,  AAS_TWSI),
-	MFPR_MMP2(GPIO39,    0x0F0, GPIO,     MMC2,       SSP1,       TWSI2,      UART2,      UART3,    AAS_SPI,  AAS_TWSI),
-	MFPR_MMP2(GPIO40,    0x0F4, GPIO,     MMC2,       SSP1,       TWSI2,      UART2,      UART3,    AAS_SPI,  AAS_TWSI),
-	MFPR_MMP2(GPIO41,    0x0F8, GPIO,     MMC2,       TWSI5,      NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO42,    0x0FC, GPIO,     MMC2,       TWSI5,      NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO43,    0x100, GPIO,     TWSI2,      UART4,      SSP1,       UART2,      UART3,    NONE,     AAS_TWSI),
-	MFPR_MMP2(GPIO44,    0x104, GPIO,     TWSI2,      UART4,      SSP1,       UART2,      UART3,    NONE,     AAS_TWSI),
-	MFPR_MMP2(GPIO45,    0x108, GPIO,     UART1,      UART4,      SSP1,       UART2,      UART3,    NONE,     NONE),
-	MFPR_MMP2(GPIO46,    0x10C, GPIO,     UART1,      UART4,      SSP1,       UART2,      UART3,    NONE,     NONE),
-	MFPR_MMP2(GPIO47,    0x110, GPIO,     UART2,      SSP2,       TWSI6,      CAM2,       AAS_SPI,  AAS_GPIO, NONE),
-	MFPR_MMP2(GPIO48,    0x114, GPIO,     UART2,      SSP2,       TWSI6,      CAM2,       AAS_SPI,  AAS_GPIO, NONE),
-	MFPR_MMP2(GPIO49,    0x118, GPIO,     UART2,      SSP2,       PWM,        CCIC2,      AAS_SPI,  NONE,     NONE),
-	MFPR_MMP2(GPIO50,    0x11C, GPIO,     UART2,      SSP2,       PWM,        CCIC2,      AAS_SPI,  NONE,     NONE),
-	MFPR_MMP2(GPIO51,    0x120, GPIO,     UART3,      ROT,        AAS_GPIO,   PWM,        NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO52,    0x124, GPIO,     UART3,      ROT,        AAS_GPIO,   PWM,        NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO53,    0x128, GPIO,     UART3,      TWSI2,      VCXO_REQ,   NONE,       PWM,      NONE,     AAS_TWSI),
-	MFPR_MMP2(GPIO54,    0x12C, GPIO,     UART3,      TWSI2,      VCXO_OUT,   HDMI,       PWM,      NONE,     AAS_TWSI),
-	MFPR_MMP2(GPIO55,    0x130, GPIO,     SSP2,       SSP1,       UART2,      ROT,        TWSI2,    SSP3,     AAS_TWSI),
-	MFPR_MMP2(GPIO56,    0x134, GPIO,     SSP2,       SSP1,       UART2,      ROT,        TWSI2,    KP_DK,    AAS_TWSI),
-	MFPR_MMP2(GPIO57,    0x138, GPIO,     SSP2_RX,    SSP1_TXRX,  SSP2_FRM,   SSP1_RX,    VCXO_REQ, KP_DK,    NONE),
-	MFPR_MMP2(GPIO58,    0x13C, GPIO,     SSP2,       SSP1_RX,    SSP1_FRM,   SSP1_TXRX,  VCXO_REQ, KP_DK,    NONE),
-	MFPR_MMP2(GPIO59,    0x280, GPIO,     CCIC1,      ULPI,       MMC3,       CCIC2,      UART3,    UART4,    NONE),
-	MFPR_MMP2(GPIO60,    0x284, GPIO,     CCIC1,      ULPI,       MMC3,       CCIC2,      UART3,    UART4,    NONE),
-	MFPR_MMP2(GPIO61,    0x288, GPIO,     CCIC1,      ULPI,       MMC3,       CCIC2,      UART3,    HDMI,     NONE),
-	MFPR_MMP2(GPIO62,    0x28C, GPIO,     CCIC1,      ULPI,       MMC3,       CCIC2,      UART3,    NONE,     NONE),
-	MFPR_MMP2(GPIO63,    0x290, GPIO,     CCIC1,      ULPI,       MMC3,       CCIC2,      MSP,      UART4,    NONE),
-	MFPR_MMP2(GPIO64,    0x294, GPIO,     CCIC1,      ULPI,       MMC3,       CCIC2,      MSP,      UART4,    NONE),
-	MFPR_MMP2(GPIO65,    0x298, GPIO,     CCIC1,      ULPI,       MMC3,       CCIC2,      MSP,      UART4,    NONE),
-	MFPR_MMP2(GPIO66,    0x29C, GPIO,     CCIC1,      ULPI,       MMC3,       CCIC2,      MSP,      UART4,    NONE),
-	MFPR_MMP2(GPIO67,    0x2A0, GPIO,     CCIC1,      ULPI,       MMC3,       CCIC2,      MSP,      NONE,     NONE),
-	MFPR_MMP2(GPIO68,    0x2A4, GPIO,     CCIC1,      ULPI,       MMC3,       CCIC2,      MSP,      LCD,      NONE),
-	MFPR_MMP2(GPIO69,    0x2A8, GPIO,     CCIC1,      ULPI,       MMC3,       CCIC2,      NONE,     LCD,      NONE),
-	MFPR_MMP2(GPIO70,    0x2AC, GPIO,     CCIC1,      ULPI,       MMC3,       CCIC2,      MSP,      LCD,      NONE),
-	MFPR_MMP2(GPIO71,    0x2B0, GPIO,     TWSI3,      NONE,       PWM,        NONE,       NONE,     LCD,      AAS_TWSI),
-	MFPR_MMP2(GPIO72,    0x2B4, GPIO,     TWSI3,      HDMI,       PWM,        NONE,       NONE,     LCD,      AAS_TWSI),
-	MFPR_MMP2(GPIO73,    0x2B8, GPIO,     VCXO_REQ,   32K_CLKOUT, PWM,        VCXO_OUT,   NONE,     LCD,      NONE),
-	MFPR_MMP2(GPIO74,    0x170, GPIO,     LCD,        SMC,        MMC4,       SSP3,       UART2,    UART4,    TIPU),
-	MFPR_MMP2(GPIO75,    0x174, GPIO,     LCD,        SMC,        MMC4,       SSP3,       UART2,    UART4,    TIPU),
-	MFPR_MMP2(GPIO76,    0x178, GPIO,     LCD,        SMC,        MMC4,       SSP3,       UART2,    UART4,    TIPU),
-	MFPR_MMP2(GPIO77,    0x17C, GPIO,     LCD,        SMC,        MMC4,       SSP3,       UART2,    UART4,    TIPU),
-	MFPR_MMP2(GPIO78,    0x180, GPIO,     LCD,        HDMI,       MMC4,       NONE,       SSP4,     AAS_SPI,  TIPU),
-	MFPR_MMP2(GPIO79,    0x184, GPIO,     LCD,        AAS_GPIO,   MMC4,       NONE,       SSP4,     AAS_SPI,  TIPU),
-	MFPR_MMP2(GPIO80,    0x188, GPIO,     LCD,        AAS_GPIO,   MMC4,       NONE,       SSP4,     AAS_SPI,  TIPU),
-	MFPR_MMP2(GPIO81,    0x18C, GPIO,     LCD,        AAS_GPIO,   MMC4,       NONE,       SSP4,     AAS_SPI,  TIPU),
-	MFPR_MMP2(GPIO82,    0x190, GPIO,     LCD,        NONE,       MMC4,       NONE,       NONE,     CCIC2,    TIPU),
-	MFPR_MMP2(GPIO83,    0x194, GPIO,     LCD,        NONE,       MMC4,       NONE,       NONE,     CCIC2,    TIPU),
-	MFPR_MMP2(GPIO84,    0x198, GPIO,     LCD,        SMC,        MMC2,       NONE,       TWSI5,    AAS_TWSI, TIPU),
-	MFPR_MMP2(GPIO85,    0x19C, GPIO,     LCD,        SMC,        MMC2,       NONE,       TWSI5,    AAS_TWSI, TIPU),
-	MFPR_MMP2(GPIO86,    0x1A0, GPIO,     LCD,        SMC,        MMC2,       NONE,       TWSI6,    CCIC2,    TIPU),
-	MFPR_MMP2(GPIO87,    0x1A4, GPIO,     LCD,        SMC,        MMC2,       NONE,       TWSI6,    CCIC2,    TIPU),
-	MFPR_MMP2(GPIO88,    0x1A8, GPIO,     LCD,        AAS_GPIO,   MMC2,       NONE,       NONE,     CCIC2,    TIPU),
-	MFPR_MMP2(GPIO89,    0x1AC, GPIO,     LCD,        AAS_GPIO,   MMC2,       NONE,       NONE,     CCIC2,    TIPU),
-	MFPR_MMP2(GPIO90,    0x1B0, GPIO,     LCD,        AAS_GPIO,   MMC2,       NONE,       NONE,     CCIC2,    TIPU),
-	MFPR_MMP2(GPIO91,    0x1B4, GPIO,     LCD,        AAS_GPIO,   MMC2,       NONE,       NONE,     CCIC2,    TIPU),
-	MFPR_MMP2(GPIO92,    0x1B8, GPIO,     LCD,        AAS_GPIO,   MMC2,       NONE,       NONE,     CCIC2,    TIPU),
-	MFPR_MMP2(GPIO93,    0x1BC, GPIO,     LCD,        AAS_GPIO,   MMC2,       NONE,       NONE,     CCIC2,    TIPU),
-	MFPR_MMP2(GPIO94,    0x1C0, GPIO,     LCD,        AAS_GPIO,   SPI,        NONE,       AAS_SPI,  CCIC2,    TIPU),
-	MFPR_MMP2(GPIO95,    0x1C4, GPIO,     LCD,        TWSI3,      SPI,        AAS_DEU_EX, AAS_SPI,  CCIC2,    TIPU),
-	MFPR_MMP2(GPIO96,    0x1C8, GPIO,     LCD,        TWSI3,      SPI,        AAS_DEU_EX, AAS_SPI,  NONE,     TIPU),
-	MFPR_MMP2(GPIO97,    0x1CC, GPIO,     LCD,        TWSI6,      SPI,        AAS_DEU_EX, AAS_SPI,  NONE,     TIPU),
-	MFPR_MMP2(GPIO98,    0x1D0, GPIO,     LCD,        TWSI6,      SPI,        ONE_WIRE,   NONE,     NONE,     TIPU),
-	MFPR_MMP2(GPIO99,    0x1D4, GPIO,     LCD,        SMC,        SPI,        TWSI5,      NONE,     NONE,     TIPU),
-	MFPR_MMP2(GPIO100,   0x1D8, GPIO,     LCD,        SMC,        SPI,        TWSI5,      NONE,     NONE,     TIPU),
-	MFPR_MMP2(GPIO101,   0x1DC, GPIO,     LCD,        SMC,        SPI,        NONE,       NONE,     NONE,     TIPU),
-	MFPR_MMP2(GPIO102,   0x000, USIM,     GPIO,       FSIC,       KP_DK,      LCD,        NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO103,   0x004, USIM,     GPIO,       FSIC,       KP_DK,      LCD,        NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO104,   0x1FC, NAND,     GPIO,       NONE,       NONE,       NAND,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO105,   0x1F8, NAND,     GPIO,       NONE,       NONE,       NAND,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO106,   0x1F4, NAND,     GPIO,       NONE,       NONE,       NAND,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO107,   0x1F0, NAND,     GPIO,       NONE,       NONE,       NAND,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO108,   0x21C, NAND,     GPIO,       NONE,       NONE,       NAND,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO109,   0x218, NAND,     GPIO,       NONE,       NONE,       NAND,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO110,   0x214, NAND,     GPIO,       NONE,       NONE,       NAND,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO111,   0x200, NAND,     GPIO,       MMC3,       NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO112,   0x244, NAND,     GPIO,       MMC3,       SMC,        NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO113,   0x25C, SMC,      GPIO,       EXT_DMA,    MMC3,       SMC,        HDMI,     NONE,     NONE),
-	MFPR_MMP2(GPIO114,   0x164, G_CLKOUT, 32K_CLKOUT, HDMI,       NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO115,   0x260, GPIO,     NONE,       AC,         UART4,      UART3,      SSP1,     NONE,     NONE),
-	MFPR_MMP2(GPIO116,   0x264, GPIO,     NONE,       AC,         UART4,      UART3,      SSP1,     NONE,     NONE),
-	MFPR_MMP2(GPIO117,   0x268, GPIO,     NONE,       AC,         UART4,      UART3,      SSP1,     NONE,     NONE),
-	MFPR_MMP2(GPIO118,   0x26C, GPIO,     NONE,       AC,         UART4,      UART3,      SSP1,     NONE,     NONE),
-	MFPR_MMP2(GPIO119,   0x270, GPIO,     NONE,       CA,         SSP3,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO120,   0x274, GPIO,     NONE,       CA,         SSP3,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO121,   0x278, GPIO,     NONE,       CA,         SSP3,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO122,   0x27C, GPIO,     NONE,       CA,         SSP3,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO123,   0x148, GPIO,     SLEEP_IND,  ONE_WIRE,   32K_CLKOUT, NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO124,   0x00C, GPIO,     MMC1,       LCD,        MMC3,       NAND,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO125,   0x010, GPIO,     MMC1,       LCD,        MMC3,       NAND,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO126,   0x014, GPIO,     MMC1,       LCD,        MMC3,       NAND,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO127,   0x018, GPIO,     NONE,       LCD,        MMC3,       NAND,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO128,   0x01C, GPIO,     NONE,       LCD,        MMC3,       NAND,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO129,   0x020, GPIO,     MMC1,       LCD,        MMC3,       NAND,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO130,   0x024, GPIO,     MMC1,       LCD,        MMC3,       NAND,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO131,   0x028, GPIO,     MMC1,       NONE,       MSP,        NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO132,   0x02C, GPIO,     MMC1,       PRI_JTAG,   MSP,        SSP3,       AAS_JTAG, NONE,     NONE),
-	MFPR_MMP2(GPIO133,   0x030, GPIO,     MMC1,       PRI_JTAG,   MSP,        SSP3,       AAS_JTAG, NONE,     NONE),
-	MFPR_MMP2(GPIO134,   0x034, GPIO,     MMC1,       PRI_JTAG,   MSP,        SSP3,       AAS_JTAG, NONE,     NONE),
-	MFPR_MMP2(GPIO135,   0x038, GPIO,     NONE,       LCD,        MMC3,       NAND,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO136,   0x03C, GPIO,     MMC1,       PRI_JTAG,   MSP,        SSP3,       AAS_JTAG, NONE,     NONE),
-	MFPR_MMP2(GPIO137,   0x040, GPIO,     HDMI,       LCD,        MSP,        NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO138,   0x044, GPIO,     NONE,       LCD,        MMC3,       SMC,        NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO139,   0x048, GPIO,     MMC1,       PRI_JTAG,   MSP,        NONE,       AAS_JTAG, NONE,     NONE),
-	MFPR_MMP2(GPIO140,   0x04C, GPIO,     MMC1,       LCD,        NONE,       NONE,       UART2,    UART1,    NONE),
-	MFPR_MMP2(GPIO141,   0x050, GPIO,     MMC1,       LCD,        NONE,       NONE,       UART2,    UART1,    NONE),
-	MFPR_MMP2(GPIO142,   0x008, USIM,     GPIO,       FSIC,       KP_DK,      NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO143,   0x220, NAND,     GPIO,       SMC,        NONE,       NAND,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO144,   0x224, NAND,     GPIO,       SMC_INT,    SMC,        NAND,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO145,   0x228, SMC,      GPIO,       NONE,       NONE,       SMC,        NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO146,   0x22C, SMC,      GPIO,       NONE,       NONE,       SMC,        NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO147,   0x230, NAND,     GPIO,       NONE,       NONE,       NAND,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO148,   0x234, NAND,     GPIO,       NONE,       NONE,       NAND,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO149,   0x238, NAND,     GPIO,       NONE,       NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO150,   0x23C, NAND,     GPIO,       NONE,       NONE,       NAND,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO151,   0x240, SMC,      GPIO,       MMC3,       NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO152,   0x248, SMC,      GPIO,       NONE,       NONE,       SMC,        NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO153,   0x24C, SMC,      GPIO,       NONE,       NONE,       SMC,        NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO154,   0x254, SMC_INT,  GPIO,       SMC,        NONE,       NAND,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO155,   0x258, EXT_DMA,  GPIO,       SMC,        NONE,       EXT_DMA,    NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO156,   0x14C, PRI_JTAG, GPIO,       PWM,        NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO157,   0x150, PRI_JTAG, GPIO,       PWM,        NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO158,   0x154, PRI_JTAG, GPIO,       PWM,        NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO159,   0x158, PRI_JTAG, GPIO,       PWM,        NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO160,   0x250, NAND,     GPIO,       SMC,        NONE,       NAND,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO161,   0x210, NAND,     GPIO,       NONE,       NONE,       NAND,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO162,   0x20C, NAND,     GPIO,       MMC3,       NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO163,   0x208, NAND,     GPIO,       MMC3,       NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO164,   0x204, NAND,     GPIO,       MMC3,       NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO165,   0x1EC, NAND,     GPIO,       MMC3,       NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO166,   0x1E8, NAND,     GPIO,       MMC3,       NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO167,   0x1E4, NAND,     GPIO,       MMC3,       NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(GPIO168,   0x1E0, NAND,     GPIO,       MMC3,       NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(TWSI4_SCL, 0x2BC, TWSI4,    LCD,        NONE,       NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(TWSI4_SDA, 0x2C0, TWSI4,    LCD,        NONE,       NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(G_CLKREQ,  0x160, G_CLKREQ, ONE_WIRE,   NONE,       NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(VCXO_REQ,  0x168, VCXO_REQ, ONE_WIRE,   PLL,        NONE,       NONE,       NONE,     NONE,     NONE),
-	MFPR_MMP2(VCXO_OUT,  0x16C, VCXO_OUT, 32K_CLKOUT, NONE,       NONE,       NONE,       NONE,     NONE,     NONE),
-};
-
-static const unsigned mmp2_uart1_pin1[] = {GPIO29, GPIO30, GPIO31, GPIO32};
-static const unsigned mmp2_uart1_pin2[] = {GPIO45, GPIO46};
-static const unsigned mmp2_uart1_pin3[] = {GPIO140, GPIO141};
-static const unsigned mmp2_uart2_pin1[] = {GPIO37, GPIO38, GPIO39, GPIO40};
-static const unsigned mmp2_uart2_pin2[] = {GPIO43, GPIO44, GPIO45, GPIO46};
-static const unsigned mmp2_uart2_pin3[] = {GPIO47, GPIO48, GPIO49, GPIO50};
-static const unsigned mmp2_uart2_pin4[] = {GPIO74, GPIO75, GPIO76, GPIO77};
-static const unsigned mmp2_uart2_pin5[] = {GPIO55, GPIO56};
-static const unsigned mmp2_uart2_pin6[] = {GPIO140, GPIO141};
-static const unsigned mmp2_uart3_pin1[] = {GPIO37, GPIO38, GPIO39, GPIO40};
-static const unsigned mmp2_uart3_pin2[] = {GPIO43, GPIO44, GPIO45, GPIO46};
-static const unsigned mmp2_uart3_pin3[] = {GPIO51, GPIO52, GPIO53, GPIO54};
-static const unsigned mmp2_uart3_pin4[] = {GPIO59, GPIO60, GPIO61, GPIO62};
-static const unsigned mmp2_uart3_pin5[] = {GPIO115, GPIO116, GPIO117, GPIO118};
-static const unsigned mmp2_uart3_pin6[] = {GPIO51, GPIO52};
-static const unsigned mmp2_uart4_pin1[] = {GPIO43, GPIO44, GPIO45, GPIO46};
-static const unsigned mmp2_uart4_pin2[] = {GPIO63, GPIO64, GPIO65, GPIO66};
-static const unsigned mmp2_uart4_pin3[] = {GPIO74, GPIO75, GPIO76, GPIO77};
-static const unsigned mmp2_uart4_pin4[] = {GPIO115, GPIO116, GPIO117, GPIO118};
-static const unsigned mmp2_uart4_pin5[] = {GPIO59, GPIO60};
-static const unsigned mmp2_kpdk_pin1[] = {GPIO16, GPIO17, GPIO18, GPIO19};
-static const unsigned mmp2_kpdk_pin2[] = {GPIO16, GPIO17};
-static const unsigned mmp2_twsi2_pin1[] = {GPIO37, GPIO38};
-static const unsigned mmp2_twsi2_pin2[] = {GPIO39, GPIO40};
-static const unsigned mmp2_twsi2_pin3[] = {GPIO43, GPIO44};
-static const unsigned mmp2_twsi2_pin4[] = {GPIO53, GPIO54};
-static const unsigned mmp2_twsi2_pin5[] = {GPIO55, GPIO56};
-static const unsigned mmp2_twsi3_pin1[] = {GPIO71, GPIO72};
-static const unsigned mmp2_twsi3_pin2[] = {GPIO95, GPIO96};
-static const unsigned mmp2_twsi4_pin1[] = {TWSI4_SCL, TWSI4_SDA};
-static const unsigned mmp2_twsi5_pin1[] = {GPIO41, GPIO42};
-static const unsigned mmp2_twsi5_pin2[] = {GPIO84, GPIO85};
-static const unsigned mmp2_twsi5_pin3[] = {GPIO99, GPIO100};
-static const unsigned mmp2_twsi6_pin1[] = {GPIO47, GPIO48};
-static const unsigned mmp2_twsi6_pin2[] = {GPIO86, GPIO87};
-static const unsigned mmp2_twsi6_pin3[] = {GPIO97, GPIO98};
-static const unsigned mmp2_ccic1_pin1[] = {GPIO12, GPIO13, GPIO14, GPIO15,
-	GPIO16, GPIO17, GPIO18, GPIO19, GPIO20, GPIO21, GPIO22, GPIO23};
-static const unsigned mmp2_ccic1_pin2[] = {GPIO59, GPIO60, GPIO61, GPIO62,
-	GPIO63, GPIO64, GPIO65, GPIO66, GPIO67, GPIO68, GPIO69, GPIO70};
-static const unsigned mmp2_ccic2_pin1[] = {GPIO59, GPIO60, GPIO61, GPIO62,
-	GPIO63, GPIO64, GPIO65, GPIO66, GPIO67, GPIO68, GPIO69, GPIO70};
-static const unsigned mmp2_ccic2_pin2[] = {GPIO82, GPIO83, GPIO86, GPIO87,
-	GPIO88, GPIO89, GPIO90, GPIO91, GPIO92, GPIO93, GPIO94, GPIO95};
-static const unsigned mmp2_ulpi_pin1[] = {GPIO59, GPIO60, GPIO61, GPIO62,
-	GPIO63, GPIO64, GPIO65, GPIO66, GPIO67, GPIO68, GPIO69, GPIO70};
-static const unsigned mmp2_ro_pin1[] = {GPIO16, GPIO17};
-static const unsigned mmp2_ro_pin2[] = {GPIO18, GPIO19};
-static const unsigned mmp2_ro_pin3[] = {GPIO51, GPIO52};
-static const unsigned mmp2_ro_pin4[] = {GPIO55, GPIO56};
-static const unsigned mmp2_i2s_pin1[] = {GPIO24, GPIO25, GPIO26, GPIO27,
-	GPIO28};
-static const unsigned mmp2_i2s_pin2[] = {GPIO33, GPIO34, GPIO35, GPIO36};
-static const unsigned mmp2_ssp1_pin1[] = {GPIO37, GPIO38, GPIO39, GPIO40};
-static const unsigned mmp2_ssp1_pin2[] = {GPIO43, GPIO44, GPIO45, GPIO46};
-static const unsigned mmp2_ssp1_pin3[] = {GPIO115, GPIO116, GPIO117, GPIO118};
-static const unsigned mmp2_ssp2_pin1[] = {GPIO47, GPIO48, GPIO49, GPIO50};
-static const unsigned mmp2_ssp3_pin1[] = {GPIO119, GPIO120, GPIO121, GPIO122};
-static const unsigned mmp2_ssp3_pin2[] = {GPIO132, GPIO133, GPIO133, GPIO136};
-static const unsigned mmp2_sspa2_pin1[] = {GPIO25, GPIO26, GPIO27, GPIO28};
-static const unsigned mmp2_sspa2_pin2[] = {GPIO33, GPIO34, GPIO35, GPIO36};
-static const unsigned mmp2_mmc1_pin1[] = {GPIO131, GPIO132, GPIO133, GPIO134,
-	GPIO136, GPIO139, GPIO140, GPIO141};
-static const unsigned mmp2_mmc2_pin1[] = {GPIO37, GPIO38, GPIO39, GPIO40,
-	GPIO41, GPIO42};
-static const unsigned mmp2_mmc3_pin1[] = {GPIO111, GPIO112, GPIO151, GPIO162,
-	GPIO163, GPIO164, GPIO165, GPIO166, GPIO167, GPIO168};
-
-static struct pxa3xx_pin_group mmp2_grps[] = {
-	GRP_MMP2("uart1 4p1", UART1, mmp2_uart1_pin1),
-	GRP_MMP2("uart1 2p2", UART1, mmp2_uart1_pin2),
-	GRP_MMP2("uart1 2p3", UART1, mmp2_uart1_pin3),
-	GRP_MMP2("uart2 4p1", UART2, mmp2_uart2_pin1),
-	GRP_MMP2("uart2 4p2", UART2, mmp2_uart2_pin2),
-	GRP_MMP2("uart2 4p3", UART2, mmp2_uart2_pin3),
-	GRP_MMP2("uart2 4p4", UART2, mmp2_uart2_pin4),
-	GRP_MMP2("uart2 2p5", UART2, mmp2_uart2_pin5),
-	GRP_MMP2("uart2 2p6", UART2, mmp2_uart2_pin6),
-	GRP_MMP2("uart3 4p1", UART3, mmp2_uart3_pin1),
-	GRP_MMP2("uart3 4p2", UART3, mmp2_uart3_pin2),
-	GRP_MMP2("uart3 4p3", UART3, mmp2_uart3_pin3),
-	GRP_MMP2("uart3 4p4", UART3, mmp2_uart3_pin4),
-	GRP_MMP2("uart3 4p5", UART3, mmp2_uart3_pin5),
-	GRP_MMP2("uart3 2p6", UART3, mmp2_uart3_pin6),
-	GRP_MMP2("uart4 4p1", UART4, mmp2_uart4_pin1),
-	GRP_MMP2("uart4 4p2", UART4, mmp2_uart4_pin2),
-	GRP_MMP2("uart4 4p3", UART4, mmp2_uart4_pin3),
-	GRP_MMP2("uart4 4p4", UART4, mmp2_uart4_pin4),
-	GRP_MMP2("uart4 2p5", UART4, mmp2_uart4_pin5),
-	GRP_MMP2("kpdk 4p1", KP_DK, mmp2_kpdk_pin1),
-	GRP_MMP2("kpdk 4p2", KP_DK, mmp2_kpdk_pin2),
-	GRP_MMP2("twsi2-1", TWSI2, mmp2_twsi2_pin1),
-	GRP_MMP2("twsi2-2", TWSI2, mmp2_twsi2_pin2),
-	GRP_MMP2("twsi2-3", TWSI2, mmp2_twsi2_pin3),
-	GRP_MMP2("twsi2-4", TWSI2, mmp2_twsi2_pin4),
-	GRP_MMP2("twsi2-5", TWSI2, mmp2_twsi2_pin5),
-	GRP_MMP2("twsi3-1", TWSI3, mmp2_twsi3_pin1),
-	GRP_MMP2("twsi3-2", TWSI3, mmp2_twsi3_pin2),
-	GRP_MMP2("twsi4", TWSI4, mmp2_twsi4_pin1),
-	GRP_MMP2("twsi5-1", TWSI5, mmp2_twsi5_pin1),
-	GRP_MMP2("twsi5-2", TWSI5, mmp2_twsi5_pin2),
-	GRP_MMP2("twsi5-3", TWSI5, mmp2_twsi5_pin3),
-	GRP_MMP2("twsi6-1", TWSI6, mmp2_twsi6_pin1),
-	GRP_MMP2("twsi6-2", TWSI6, mmp2_twsi6_pin2),
-	GRP_MMP2("twsi6-3", TWSI6, mmp2_twsi6_pin3),
-	GRP_MMP2("ccic1-1", CCIC1, mmp2_ccic1_pin1),
-	GRP_MMP2("ccic1-2", CCIC1, mmp2_ccic1_pin2),
-	GRP_MMP2("ccic2-1", CCIC2, mmp2_ccic2_pin1),
-	GRP_MMP2("ccic2-1", CCIC2, mmp2_ccic2_pin2),
-	GRP_MMP2("ulpi", ULPI, mmp2_ulpi_pin1),
-	GRP_MMP2("ro-1", ROT, mmp2_ro_pin1),
-	GRP_MMP2("ro-2", ROT, mmp2_ro_pin2),
-	GRP_MMP2("ro-3", ROT, mmp2_ro_pin3),
-	GRP_MMP2("ro-4", ROT, mmp2_ro_pin4),
-	GRP_MMP2("i2s 5p1", I2S, mmp2_i2s_pin1),
-	GRP_MMP2("i2s 4p2", I2S, mmp2_i2s_pin2),
-	GRP_MMP2("ssp1 4p1", SSP1, mmp2_ssp1_pin1),
-	GRP_MMP2("ssp1 4p2", SSP1, mmp2_ssp1_pin2),
-	GRP_MMP2("ssp1 4p3", SSP1, mmp2_ssp1_pin3),
-	GRP_MMP2("ssp2 4p1", SSP2, mmp2_ssp2_pin1),
-	GRP_MMP2("ssp3 4p1", SSP3, mmp2_ssp3_pin1),
-	GRP_MMP2("ssp3 4p2", SSP3, mmp2_ssp3_pin2),
-	GRP_MMP2("sspa2 4p1", SSPA2, mmp2_sspa2_pin1),
-	GRP_MMP2("sspa2 4p2", SSPA2, mmp2_sspa2_pin2),
-	GRP_MMP2("mmc1 8p1", MMC1, mmp2_mmc1_pin1),
-	GRP_MMP2("mmc2 6p1", MMC2, mmp2_mmc2_pin1),
-	GRP_MMP2("mmc3 10p1", MMC3, mmp2_mmc3_pin1),
-};
-
-static const char * const mmp2_uart1_grps[] = {"uart1 4p1", "uart1 2p2",
-	"uart1 2p3"};
-static const char * const mmp2_uart2_grps[] = {"uart2 4p1", "uart2 4p2",
-	"uart2 4p3", "uart2 4p4", "uart2 4p5", "uart2 4p6"};
-static const char * const mmp2_uart3_grps[] = {"uart3 4p1", "uart3 4p2",
-	"uart3 4p3", "uart3 4p4", "uart3 4p5", "uart3 2p6"};
-static const char * const mmp2_uart4_grps[] = {"uart4 4p1", "uart4 4p2",
-	"uart4 4p3", "uart4 4p4", "uart4 2p5"};
-static const char * const mmp2_kpdk_grps[] = {"kpdk 4p1", "kpdk 4p2"};
-static const char * const mmp2_twsi2_grps[] = {"twsi2-1", "twsi2-2",
-	"twsi2-3", "twsi2-4", "twsi2-5"};
-static const char * const mmp2_twsi3_grps[] = {"twsi3-1", "twsi3-2"};
-static const char * const mmp2_twsi4_grps[] = {"twsi4"};
-static const char * const mmp2_twsi5_grps[] = {"twsi5-1", "twsi5-2",
-	"twsi5-3"};
-static const char * const mmp2_twsi6_grps[] = {"twsi6-1", "twsi6-2",
-	"twsi6-3"};
-static const char * const mmp2_ccic1_grps[] = {"ccic1-1", "ccic1-2"};
-static const char * const mmp2_ccic2_grps[] = {"ccic2-1", "ccic2-2"};
-static const char * const mmp2_ulpi_grps[] = {"ulpi"};
-static const char * const mmp2_ro_grps[] = {"ro-1", "ro-2", "ro-3", "ro-4"};
-static const char * const mmp2_i2s_grps[] = {"i2s 5p1", "i2s 4p2"};
-static const char * const mmp2_ssp1_grps[] = {"ssp1 4p1", "ssp1 4p2",
-	"ssp1 4p3"};
-static const char * const mmp2_ssp2_grps[] = {"ssp2 4p1"};
-static const char * const mmp2_ssp3_grps[] = {"ssp3 4p1", "ssp3 4p2"};
-static const char * const mmp2_sspa2_grps[] = {"sspa2 4p1", "sspa2 4p2"};
-static const char * const mmp2_mmc1_grps[] = {"mmc1 8p1"};
-static const char * const mmp2_mmc2_grps[] = {"mmc2 6p1"};
-static const char * const mmp2_mmc3_grps[] = {"mmc3 10p1"};
-
-static struct pxa3xx_pmx_func mmp2_funcs[] = {
-	{"uart1",	ARRAY_AND_SIZE(mmp2_uart1_grps)},
-	{"uart2",	ARRAY_AND_SIZE(mmp2_uart2_grps)},
-	{"uart3",	ARRAY_AND_SIZE(mmp2_uart3_grps)},
-	{"uart4",	ARRAY_AND_SIZE(mmp2_uart4_grps)},
-	{"kpdk",	ARRAY_AND_SIZE(mmp2_kpdk_grps)},
-	{"twsi2",	ARRAY_AND_SIZE(mmp2_twsi2_grps)},
-	{"twsi3",	ARRAY_AND_SIZE(mmp2_twsi3_grps)},
-	{"twsi4",	ARRAY_AND_SIZE(mmp2_twsi4_grps)},
-	{"twsi5",	ARRAY_AND_SIZE(mmp2_twsi5_grps)},
-	{"twsi6",	ARRAY_AND_SIZE(mmp2_twsi6_grps)},
-	{"ccic1",	ARRAY_AND_SIZE(mmp2_ccic1_grps)},
-	{"ccic2",	ARRAY_AND_SIZE(mmp2_ccic2_grps)},
-	{"ulpi",	ARRAY_AND_SIZE(mmp2_ulpi_grps)},
-	{"ro",		ARRAY_AND_SIZE(mmp2_ro_grps)},
-	{"i2s",		ARRAY_AND_SIZE(mmp2_i2s_grps)},
-	{"ssp1",	ARRAY_AND_SIZE(mmp2_ssp1_grps)},
-	{"ssp2",	ARRAY_AND_SIZE(mmp2_ssp2_grps)},
-	{"ssp3",	ARRAY_AND_SIZE(mmp2_ssp3_grps)},
-	{"sspa2",	ARRAY_AND_SIZE(mmp2_sspa2_grps)},
-	{"mmc1",	ARRAY_AND_SIZE(mmp2_mmc1_grps)},
-	{"mmc2",	ARRAY_AND_SIZE(mmp2_mmc2_grps)},
-	{"mmc3",	ARRAY_AND_SIZE(mmp2_mmc3_grps)},
-};
-
-static struct pinctrl_desc mmp2_pctrl_desc = {
-	.name		= "mmp2-pinctrl",
-	.owner		= THIS_MODULE,
-};
-
-static struct pxa3xx_pinmux_info mmp2_info = {
-	.mfp		= mmp2_mfp,
-	.num_mfp	= ARRAY_SIZE(mmp2_mfp),
-	.grps		= mmp2_grps,
-	.num_grps	= ARRAY_SIZE(mmp2_grps),
-	.funcs		= mmp2_funcs,
-	.num_funcs	= ARRAY_SIZE(mmp2_funcs),
-	.num_gpio	= 169,
-	.desc		= &mmp2_pctrl_desc,
-	.pads		= mmp2_pads,
-	.num_pads	= ARRAY_SIZE(mmp2_pads),
-
-	.cputype	= PINCTRL_MMP2,
-	.ds_mask	= MMP2_DS_MASK,
-	.ds_shift	= MMP2_DS_SHIFT,
-};
-
-static int mmp2_pinmux_probe(struct platform_device *pdev)
-{
-	return pxa3xx_pinctrl_register(pdev, &mmp2_info);
-}
-
-static int mmp2_pinmux_remove(struct platform_device *pdev)
-{
-	return pxa3xx_pinctrl_unregister(pdev);
-}
-
-static struct platform_driver mmp2_pinmux_driver = {
-	.driver = {
-		.name	= "mmp2-pinmux",
-		.owner	= THIS_MODULE,
-	},
-	.probe	= mmp2_pinmux_probe,
-	.remove	= mmp2_pinmux_remove,
-};
-
-static int __init mmp2_pinmux_init(void)
-{
-	return platform_driver_register(&mmp2_pinmux_driver);
-}
-core_initcall_sync(mmp2_pinmux_init);
-
-static void __exit mmp2_pinmux_exit(void)
-{
-	platform_driver_unregister(&mmp2_pinmux_driver);
-}
-module_exit(mmp2_pinmux_exit);
-
-MODULE_AUTHOR("Haojian Zhuang <haojian.zhuang@marvell.com>");
-MODULE_DESCRIPTION("PXA3xx pin control driver");
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/pinctrl-mxs.c b/drivers/pinctrl/pinctrl-mxs.c
index 23af9f1f9c3..b45c4eb3579 100644
--- a/drivers/pinctrl/pinctrl-mxs.c
+++ b/drivers/pinctrl/pinctrl-mxs.c
@@ -158,7 +158,7 @@ static void mxs_dt_free_map(struct pinctrl_dev *pctldev,
 	kfree(map);
 }
 
-static struct pinctrl_ops mxs_pinctrl_ops = {
+static const struct pinctrl_ops mxs_pinctrl_ops = {
 	.get_groups_count = mxs_get_groups_count,
 	.get_group_name = mxs_get_group_name,
 	.get_group_pins = mxs_get_group_pins,
@@ -219,7 +219,7 @@ static int mxs_pinctrl_enable(struct pinctrl_dev *pctldev, unsigned selector,
 	return 0;
 }
 
-static struct pinmux_ops mxs_pinmux_ops = {
+static const struct pinmux_ops mxs_pinmux_ops = {
 	.get_functions_count = mxs_pinctrl_get_funcs_count,
 	.get_function_name = mxs_pinctrl_get_func_name,
 	.get_function_groups = mxs_pinctrl_get_func_groups,
@@ -319,7 +319,7 @@ static void mxs_pinconf_group_dbg_show(struct pinctrl_dev *pctldev,
 		seq_printf(s, "0x%lx", config);
 }
 
-static struct pinconf_ops mxs_pinconf_ops = {
+static const struct pinconf_ops mxs_pinconf_ops = {
 	.pin_config_get = mxs_pinconf_get,
 	.pin_config_set = mxs_pinconf_set,
 	.pin_config_group_get = mxs_pinconf_group_get,
diff --git a/drivers/pinctrl/pinctrl-nomadik-db8500.c b/drivers/pinctrl/pinctrl-nomadik-db8500.c
index 30b4da91ef7..c7484072964 100644
--- a/drivers/pinctrl/pinctrl-nomadik-db8500.c
+++ b/drivers/pinctrl/pinctrl-nomadik-db8500.c
@@ -466,7 +466,7 @@ static const unsigned mc1_a_1_pins[] = { DB8500_PIN_AH16, DB8500_PIN_AG15,
 	DB8500_PIN_AJ15, DB8500_PIN_AG14, DB8500_PIN_AF13, DB8500_PIN_AG13,
 	DB8500_PIN_AH15 };
 static const unsigned mc1_a_2_pins[] = { DB8500_PIN_AH16, DB8500_PIN_AJ15,
-	DB8500_PIN_AG14, DB8500_PIN_AF13, DB8500_PIN_AG13,DB8500_PIN_AH15 };
+	DB8500_PIN_AG14, DB8500_PIN_AF13, DB8500_PIN_AG13, DB8500_PIN_AH15 };
 static const unsigned mc1dir_a_1_pins[] = { DB8500_PIN_AH13, DB8500_PIN_AG12,
 	DB8500_PIN_AH12, DB8500_PIN_AH11 };
 static const unsigned hsir_a_1_pins[] = { DB8500_PIN_AG10, DB8500_PIN_AH10,
@@ -663,7 +663,7 @@ static const unsigned hwobs_oc4_1_pins[] = { DB8500_PIN_D17, DB8500_PIN_D16,
 	DB8500_PIN_D21, DB8500_PIN_D20,	DB8500_PIN_C20, DB8500_PIN_B21,
 	DB8500_PIN_C21, DB8500_PIN_A22, DB8500_PIN_B24, DB8500_PIN_C22 };
 
-#define DB8500_PIN_GROUP(a,b) { .name = #a, .pins = a##_pins,		\
+#define DB8500_PIN_GROUP(a, b) { .name = #a, .pins = a##_pins,		\
 			.npins = ARRAY_SIZE(a##_pins), .altsetting = b }
 
 static const struct nmk_pingroup nmk_db8500_groups[] = {
diff --git a/drivers/pinctrl/pinctrl-nomadik-stn8815.c b/drivers/pinctrl/pinctrl-nomadik-stn8815.c
index 924a3393fa8..ed39dcafd4f 100644
--- a/drivers/pinctrl/pinctrl-nomadik-stn8815.c
+++ b/drivers/pinctrl/pinctrl-nomadik-stn8815.c
@@ -299,7 +299,7 @@ static const unsigned i2c0_a_1_pins[] = { STN8815_PIN_D3, STN8815_PIN_D2 };
 static const unsigned u1_b_1_pins[] = { STN8815_PIN_B16, STN8815_PIN_A16 };
 static const unsigned i2cusb_b_1_pins[] = { STN8815_PIN_C21, STN8815_PIN_C20 };
 
-#define STN8815_PIN_GROUP(a,b) { .name = #a, .pins = a##_pins,		\
+#define STN8815_PIN_GROUP(a, b) { .name = #a, .pins = a##_pins,		\
 			.npins = ARRAY_SIZE(a##_pins), .altsetting = b }
 
 static const struct nmk_pingroup nmk_stn8815_groups[] = {
diff --git a/drivers/pinctrl/pinctrl-nomadik.c b/drivers/pinctrl/pinctrl-nomadik.c
index 36d20293de5..435bf3078d2 100644
--- a/drivers/pinctrl/pinctrl-nomadik.c
+++ b/drivers/pinctrl/pinctrl-nomadik.c
@@ -1565,8 +1565,8 @@ static int nmk_dt_add_map_configs(struct pinctrl_map **map,
 	return 0;
 }
 
-#define NMK_CONFIG_PIN(x,y) { .property = x, .config = y, }
-#define NMK_CONFIG_PIN_ARRAY(x,y) { .property = x, .choice = y, \
+#define NMK_CONFIG_PIN(x, y) { .property = x, .config = y, }
+#define NMK_CONFIG_PIN_ARRAY(x, y) { .property = x, .choice = y, \
 	.size = ARRAY_SIZE(y), }
 
 static const unsigned long nmk_pin_input_modes[] = {
@@ -1764,7 +1764,7 @@ int nmk_pinctrl_dt_node_to_map(struct pinctrl_dev *pctldev,
 	return 0;
 }
 
-static struct pinctrl_ops nmk_pinctrl_ops = {
+static const struct pinctrl_ops nmk_pinctrl_ops = {
 	.get_groups_count = nmk_get_groups_cnt,
 	.get_group_name = nmk_get_group_name,
 	.get_group_pins = nmk_get_group_pins,
@@ -1975,7 +1975,7 @@ static void nmk_gpio_disable_free(struct pinctrl_dev *pctldev,
 	/* Set the pin to some default state, GPIO is usually default */
 }
 
-static struct pinmux_ops nmk_pinmux_ops = {
+static const struct pinmux_ops nmk_pinmux_ops = {
 	.get_functions_count = nmk_pmx_get_funcs_cnt,
 	.get_function_name = nmk_pmx_get_func_name,
 	.get_function_groups = nmk_pmx_get_func_groups,
@@ -2068,7 +2068,7 @@ static int nmk_pin_config_set(struct pinctrl_dev *pctldev, unsigned pin,
 		pin, cfg, pullnames[pull], slpmnames[slpm],
 		output ? "output " : "input",
 		output ? (val ? "high" : "low") : "",
-		lowemi ? "on" : "off" );
+		lowemi ? "on" : "off");
 
 	clk_enable(nmk_chip->clk);
 	bit = pin % NMK_GPIO_PER_CHIP;
@@ -2089,7 +2089,7 @@ static int nmk_pin_config_set(struct pinctrl_dev *pctldev, unsigned pin,
 	return 0;
 }
 
-static struct pinconf_ops nmk_pinconf_ops = {
+static const struct pinconf_ops nmk_pinconf_ops = {
 	.pin_config_get = nmk_pin_config_get,
 	.pin_config_set = nmk_pin_config_set,
 };
@@ -2111,6 +2111,10 @@ static const struct of_device_id nmk_pinctrl_match[] = {
 		.compatible = "stericsson,nmk-pinctrl",
 		.data = (void *)PINCTRL_NMK_DB8500,
 	},
+	{
+		.compatible = "stericsson,nmk-pinctrl-db8540",
+		.data = (void *)PINCTRL_NMK_DB8540,
+	},
 	{},
 };
 
diff --git a/drivers/pinctrl/pinctrl-pxa168.c b/drivers/pinctrl/pinctrl-pxa168.c
deleted file mode 100644
index d9cd2b45748..00000000000
--- a/drivers/pinctrl/pinctrl-pxa168.c
+++ /dev/null
@@ -1,651 +0,0 @@
-/*
- *  linux/drivers/pinctrl/pinmux-pxa168.c
- *
- *  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
- *  publishhed by the Free Software Foundation.
- *
- *  Copyright (C) 2011, Marvell Technology Group Ltd.
- *
- *  Author: Haojian Zhuang <haojian.zhuang@marvell.com>
- *
- */
-
-#include <linux/device.h>
-#include <linux/module.h>
-#include <linux/io.h>
-#include <linux/platform_device.h>
-#include "pinctrl-pxa3xx.h"
-
-#define PXA168_DS_MASK		0x1800
-#define PXA168_DS_SHIFT		11
-#define PXA168_SLEEP_MASK	0x38
-#define PXA168_SLEEP_SELECT	(1 << 9)
-#define PXA168_SLEEP_DATA	(1 << 8)
-#define PXA168_SLEEP_DIR	(1 << 7)
-
-#define MFPR_168(a, r, f0, f1, f2, f3, f4, f5, f6, f7)		\
-	{							\
-		.name = #a,					\
-		.pin = a,					\
-		.mfpr = r,					\
-		.func = {					\
-			PXA168_MUX_##f0,			\
-			PXA168_MUX_##f1,			\
-			PXA168_MUX_##f2,			\
-			PXA168_MUX_##f3,			\
-			PXA168_MUX_##f4,			\
-			PXA168_MUX_##f5,			\
-			PXA168_MUX_##f6,			\
-			PXA168_MUX_##f7,			\
-		},						\
-	}
-
-#define GRP_168(a, m, p)		\
-	{ .name = a, .mux = PXA168_MUX_##m, .pins = p, .npins = ARRAY_SIZE(p), }
-
-/* 131 pins */
-enum pxa168_pin_list {
-	/* 0~122: GPIO0~GPIO122 */
-	PWR_SCL = 123,
-	PWR_SDA,
-	TDI,
-	TMS,
-	TCK,
-	TDO,
-	TRST,
-	WAKEUP = 130,
-};
-
-enum pxa168_mux {
-	/* PXA3xx_MUX_GPIO = 0 (predefined in pinctrl-pxa3xx.h) */
-	PXA168_MUX_GPIO = 0,
-	PXA168_MUX_DFIO,
-	PXA168_MUX_NAND,
-	PXA168_MUX_SMC,
-	PXA168_MUX_SMC_CS0,
-	PXA168_MUX_SMC_CS1,
-	PXA168_MUX_SMC_INT,
-	PXA168_MUX_SMC_RDY,
-	PXA168_MUX_MMC1,
-	PXA168_MUX_MMC2,
-	PXA168_MUX_MMC2_CMD,
-	PXA168_MUX_MMC2_CLK,
-	PXA168_MUX_MMC3,
-	PXA168_MUX_MMC3_CMD,
-	PXA168_MUX_MMC3_CLK,
-	PXA168_MUX_MMC4,
-	PXA168_MUX_MSP,
-	PXA168_MUX_MSP_DAT3,
-	PXA168_MUX_MSP_INS,
-	PXA168_MUX_I2C,
-	PXA168_MUX_PWRI2C,
-	PXA168_MUX_AC97,
-	PXA168_MUX_AC97_SYSCLK,
-	PXA168_MUX_PWM,
-	PXA168_MUX_PWM1,
-	PXA168_MUX_XD,
-	PXA168_MUX_XP,
-	PXA168_MUX_LCD,
-	PXA168_MUX_CCIC,
-	PXA168_MUX_CF,
-	PXA168_MUX_CF_RDY,
-	PXA168_MUX_CF_nINPACK,
-	PXA168_MUX_CF_nWAIT,
-	PXA168_MUX_KP_MKOUT,
-	PXA168_MUX_KP_MKIN,
-	PXA168_MUX_KP_DK,
-	PXA168_MUX_ETH,
-	PXA168_MUX_ETH_TX,
-	PXA168_MUX_ETH_RX,
-	PXA168_MUX_ONE_WIRE,
-	PXA168_MUX_UART1,
-	PXA168_MUX_UART1_TX,
-	PXA168_MUX_UART1_CTS,
-	PXA168_MUX_UART1_nRI,
-	PXA168_MUX_UART1_DTR,
-	PXA168_MUX_UART2,
-	PXA168_MUX_UART2_TX,
-	PXA168_MUX_UART3,
-	PXA168_MUX_UART3_TX,
-	PXA168_MUX_UART3_CTS,
-	PXA168_MUX_SSP1,
-	PXA168_MUX_SSP1_TX,
-	PXA168_MUX_SSP2,
-	PXA168_MUX_SSP2_TX,
-	PXA168_MUX_SSP3,
-	PXA168_MUX_SSP3_TX,
-	PXA168_MUX_SSP4,
-	PXA168_MUX_SSP4_TX,
-	PXA168_MUX_SSP5,
-	PXA168_MUX_SSP5_TX,
-	PXA168_MUX_USB,
-	PXA168_MUX_JTAG,
-	PXA168_MUX_RESET,
-	PXA168_MUX_WAKEUP,
-	PXA168_MUX_EXT_32K_IN,
-	PXA168_MUX_NONE = 0xffff,
-};
-
-static struct pinctrl_pin_desc pxa168_pads[] = {
-	PINCTRL_PIN(GPIO0, "GPIO0"),
-	PINCTRL_PIN(GPIO1, "GPIO1"),
-	PINCTRL_PIN(GPIO2, "GPIO2"),
-	PINCTRL_PIN(GPIO3, "GPIO3"),
-	PINCTRL_PIN(GPIO4, "GPIO4"),
-	PINCTRL_PIN(GPIO5, "GPIO5"),
-	PINCTRL_PIN(GPIO6, "GPIO6"),
-	PINCTRL_PIN(GPIO7, "GPIO7"),
-	PINCTRL_PIN(GPIO8, "GPIO8"),
-	PINCTRL_PIN(GPIO9, "GPIO9"),
-	PINCTRL_PIN(GPIO10, "GPIO10"),
-	PINCTRL_PIN(GPIO11, "GPIO11"),
-	PINCTRL_PIN(GPIO12, "GPIO12"),
-	PINCTRL_PIN(GPIO13, "GPIO13"),
-	PINCTRL_PIN(GPIO14, "GPIO14"),
-	PINCTRL_PIN(GPIO15, "GPIO15"),
-	PINCTRL_PIN(GPIO16, "GPIO16"),
-	PINCTRL_PIN(GPIO17, "GPIO17"),
-	PINCTRL_PIN(GPIO18, "GPIO18"),
-	PINCTRL_PIN(GPIO19, "GPIO19"),
-	PINCTRL_PIN(GPIO20, "GPIO20"),
-	PINCTRL_PIN(GPIO21, "GPIO21"),
-	PINCTRL_PIN(GPIO22, "GPIO22"),
-	PINCTRL_PIN(GPIO23, "GPIO23"),
-	PINCTRL_PIN(GPIO24, "GPIO24"),
-	PINCTRL_PIN(GPIO25, "GPIO25"),
-	PINCTRL_PIN(GPIO26, "GPIO26"),
-	PINCTRL_PIN(GPIO27, "GPIO27"),
-	PINCTRL_PIN(GPIO28, "GPIO28"),
-	PINCTRL_PIN(GPIO29, "GPIO29"),
-	PINCTRL_PIN(GPIO30, "GPIO30"),
-	PINCTRL_PIN(GPIO31, "GPIO31"),
-	PINCTRL_PIN(GPIO32, "GPIO32"),
-	PINCTRL_PIN(GPIO33, "GPIO33"),
-	PINCTRL_PIN(GPIO34, "GPIO34"),
-	PINCTRL_PIN(GPIO35, "GPIO35"),
-	PINCTRL_PIN(GPIO36, "GPIO36"),
-	PINCTRL_PIN(GPIO37, "GPIO37"),
-	PINCTRL_PIN(GPIO38, "GPIO38"),
-	PINCTRL_PIN(GPIO39, "GPIO39"),
-	PINCTRL_PIN(GPIO40, "GPIO40"),
-	PINCTRL_PIN(GPIO41, "GPIO41"),
-	PINCTRL_PIN(GPIO42, "GPIO42"),
-	PINCTRL_PIN(GPIO43, "GPIO43"),
-	PINCTRL_PIN(GPIO44, "GPIO44"),
-	PINCTRL_PIN(GPIO45, "GPIO45"),
-	PINCTRL_PIN(GPIO46, "GPIO46"),
-	PINCTRL_PIN(GPIO47, "GPIO47"),
-	PINCTRL_PIN(GPIO48, "GPIO48"),
-	PINCTRL_PIN(GPIO49, "GPIO49"),
-	PINCTRL_PIN(GPIO50, "GPIO50"),
-	PINCTRL_PIN(GPIO51, "GPIO51"),
-	PINCTRL_PIN(GPIO52, "GPIO52"),
-	PINCTRL_PIN(GPIO53, "GPIO53"),
-	PINCTRL_PIN(GPIO54, "GPIO54"),
-	PINCTRL_PIN(GPIO55, "GPIO55"),
-	PINCTRL_PIN(GPIO56, "GPIO56"),
-	PINCTRL_PIN(GPIO57, "GPIO57"),
-	PINCTRL_PIN(GPIO58, "GPIO58"),
-	PINCTRL_PIN(GPIO59, "GPIO59"),
-	PINCTRL_PIN(GPIO60, "GPIO60"),
-	PINCTRL_PIN(GPIO61, "GPIO61"),
-	PINCTRL_PIN(GPIO62, "GPIO62"),
-	PINCTRL_PIN(GPIO63, "GPIO63"),
-	PINCTRL_PIN(GPIO64, "GPIO64"),
-	PINCTRL_PIN(GPIO65, "GPIO65"),
-	PINCTRL_PIN(GPIO66, "GPIO66"),
-	PINCTRL_PIN(GPIO67, "GPIO67"),
-	PINCTRL_PIN(GPIO68, "GPIO68"),
-	PINCTRL_PIN(GPIO69, "GPIO69"),
-	PINCTRL_PIN(GPIO70, "GPIO70"),
-	PINCTRL_PIN(GPIO71, "GPIO71"),
-	PINCTRL_PIN(GPIO72, "GPIO72"),
-	PINCTRL_PIN(GPIO73, "GPIO73"),
-	PINCTRL_PIN(GPIO74, "GPIO74"),
-	PINCTRL_PIN(GPIO75, "GPIO75"),
-	PINCTRL_PIN(GPIO76, "GPIO76"),
-	PINCTRL_PIN(GPIO77, "GPIO77"),
-	PINCTRL_PIN(GPIO78, "GPIO78"),
-	PINCTRL_PIN(GPIO79, "GPIO79"),
-	PINCTRL_PIN(GPIO80, "GPIO80"),
-	PINCTRL_PIN(GPIO81, "GPIO81"),
-	PINCTRL_PIN(GPIO82, "GPIO82"),
-	PINCTRL_PIN(GPIO83, "GPIO83"),
-	PINCTRL_PIN(GPIO84, "GPIO84"),
-	PINCTRL_PIN(GPIO85, "GPIO85"),
-	PINCTRL_PIN(GPIO86, "GPIO86"),
-	PINCTRL_PIN(GPIO87, "GPIO87"),
-	PINCTRL_PIN(GPIO88, "GPIO88"),
-	PINCTRL_PIN(GPIO89, "GPIO89"),
-	PINCTRL_PIN(GPIO90, "GPIO90"),
-	PINCTRL_PIN(GPIO91, "GPIO91"),
-	PINCTRL_PIN(GPIO92, "GPIO92"),
-	PINCTRL_PIN(GPIO93, "GPIO93"),
-	PINCTRL_PIN(GPIO94, "GPIO94"),
-	PINCTRL_PIN(GPIO95, "GPIO95"),
-	PINCTRL_PIN(GPIO96, "GPIO96"),
-	PINCTRL_PIN(GPIO97, "GPIO97"),
-	PINCTRL_PIN(GPIO98, "GPIO98"),
-	PINCTRL_PIN(GPIO99, "GPIO99"),
-	PINCTRL_PIN(GPIO100, "GPIO100"),
-	PINCTRL_PIN(GPIO101, "GPIO101"),
-	PINCTRL_PIN(GPIO102, "GPIO102"),
-	PINCTRL_PIN(GPIO103, "GPIO103"),
-	PINCTRL_PIN(GPIO104, "GPIO104"),
-	PINCTRL_PIN(GPIO105, "GPIO105"),
-	PINCTRL_PIN(GPIO106, "GPIO106"),
-	PINCTRL_PIN(GPIO107, "GPIO107"),
-	PINCTRL_PIN(GPIO108, "GPIO108"),
-	PINCTRL_PIN(GPIO109, "GPIO109"),
-	PINCTRL_PIN(GPIO110, "GPIO110"),
-	PINCTRL_PIN(GPIO111, "GPIO111"),
-	PINCTRL_PIN(GPIO112, "GPIO112"),
-	PINCTRL_PIN(GPIO113, "GPIO113"),
-	PINCTRL_PIN(GPIO114, "GPIO114"),
-	PINCTRL_PIN(GPIO115, "GPIO115"),
-	PINCTRL_PIN(GPIO116, "GPIO116"),
-	PINCTRL_PIN(GPIO117, "GPIO117"),
-	PINCTRL_PIN(GPIO118, "GPIO118"),
-	PINCTRL_PIN(GPIO119, "GPIO119"),
-	PINCTRL_PIN(GPIO120, "GPIO120"),
-	PINCTRL_PIN(GPIO121, "GPIO121"),
-	PINCTRL_PIN(GPIO122, "GPIO122"),
-	PINCTRL_PIN(PWR_SCL, "PWR_SCL"),
-	PINCTRL_PIN(PWR_SDA, "PWR_SDA"),
-	PINCTRL_PIN(TDI, "TDI"),
-	PINCTRL_PIN(TMS, "TMS"),
-	PINCTRL_PIN(TCK, "TCK"),
-	PINCTRL_PIN(TDO, "TDO"),
-	PINCTRL_PIN(TRST, "TRST"),
-	PINCTRL_PIN(WAKEUP, "WAKEUP"),
-};
-
-struct pxa3xx_mfp_pin pxa168_mfp[] = {
-	/*       pin      offs   f0       f1           f2         f3           f4           f5        f6           f7  */
-	MFPR_168(GPIO0,   0x04C, DFIO,    NONE,        NONE,      MSP,         MMC3_CMD,    GPIO,     MMC3,        NONE),
-	MFPR_168(GPIO1,   0x050, DFIO,    NONE,        NONE,      MSP,         MMC3_CLK,    GPIO,     MMC3,        NONE),
-	MFPR_168(GPIO2,   0x054, DFIO,    NONE,        NONE,      MSP,         NONE,        GPIO,     MMC3,        NONE),
-	MFPR_168(GPIO3,   0x058, DFIO,    NONE,        NONE,      NONE,        NONE,        GPIO,     MMC3,        NONE),
-	MFPR_168(GPIO4,   0x05C, DFIO,    NONE,        NONE,      MSP_DAT3,    NONE,        GPIO,     MMC3,        NONE),
-	MFPR_168(GPIO5,   0x060, DFIO,    NONE,        NONE,      MSP,         NONE,        GPIO,     MMC3,        NONE),
-	MFPR_168(GPIO6,   0x064, DFIO,    NONE,        NONE,      MSP,         NONE,        GPIO,     MMC3,        NONE),
-	MFPR_168(GPIO7,   0x068, DFIO,    NONE,        NONE,      MSP,         NONE,        GPIO,     MMC3,        NONE),
-	MFPR_168(GPIO8,   0x06C, DFIO,    MMC2,        UART3_TX,  NONE,        MMC2_CMD,    GPIO,     MMC3_CLK,    NONE),
-	MFPR_168(GPIO9,   0x070, DFIO,    MMC2,        UART3,     NONE,        MMC2_CLK,    GPIO,     MMC3_CMD,    NONE),
-	MFPR_168(GPIO10,  0x074, DFIO,    MMC2,        UART3,     NONE,        NONE,        GPIO,     MSP_DAT3,    NONE),
-	MFPR_168(GPIO11,  0x078, DFIO,    MMC2,        UART3,     NONE,        NONE,        GPIO,     MSP,         NONE),
-	MFPR_168(GPIO12,  0x07C, DFIO,    MMC2,        UART3,     NONE,        NONE,        GPIO,     MSP,         NONE),
-	MFPR_168(GPIO13,  0x080, DFIO,    MMC2,        UART3,     NONE,        NONE,        GPIO,     MSP,         NONE),
-	MFPR_168(GPIO14,  0x084, DFIO,    MMC2,        NONE,      NONE,        NONE,        GPIO,     MSP,         NONE),
-	MFPR_168(GPIO15,  0x088, DFIO,    MMC2,        NONE,      NONE,        NONE,        GPIO,     MSP,         NONE),
-	MFPR_168(GPIO16,  0x08C, GPIO,    NAND,        SMC_CS0,   SMC_CS1,     NONE,        NONE,     MMC3,        NONE),
-	MFPR_168(GPIO17,  0x090, NAND,    NONE,        NONE,      NONE,        NONE,        GPIO,     MSP,         NONE),
-	MFPR_168(GPIO18,  0x094, GPIO,    NAND,        SMC_CS1,   SMC_CS0,     NONE,        NONE,     NONE,        NONE),
-	MFPR_168(GPIO19,  0x098, SMC_CS0, NONE,        NONE,      CF,          NONE,        GPIO,     NONE,        NONE),
-	MFPR_168(GPIO20,  0x09C, GPIO,    NONE,        SMC_CS1,   CF,          CF_RDY,      NONE,     NONE,        NONE),
-	MFPR_168(GPIO21,  0x0A0, NAND,    MMC2_CLK,    NONE,      NONE,        NONE,        GPIO,     NONE,        NONE),
-	MFPR_168(GPIO22,  0x0A4, NAND,    MMC2_CMD,    NONE,      NONE,        NONE,        GPIO,     NONE,        NONE),
-	MFPR_168(GPIO23,  0x0A8, SMC,     NAND,        NONE,      CF,          NONE,        GPIO,     NONE,        NONE),
-	MFPR_168(GPIO24,  0x0AC, NAND,    NONE,        NONE,      NONE,        NONE,        GPIO,     NONE,        NONE),
-	MFPR_168(GPIO25,  0x0B0, SMC,     NAND,        NONE,      CF,          NONE,        GPIO,     NONE,        NONE),
-	MFPR_168(GPIO26,  0x0B4, GPIO,    NAND,        NONE,      NONE,        CF,          NONE,     NONE,        NONE),
-	MFPR_168(GPIO27,  0x0B8, SMC_INT, NAND,        SMC,       NONE,        SMC_RDY,     GPIO,     NONE,        NONE),
-	MFPR_168(GPIO28,  0x0BC, SMC_RDY, MMC4,        SMC,       CF_RDY,      NONE,        GPIO,     MMC2_CMD,    NONE),
-	MFPR_168(GPIO29,  0x0C0, SMC,     MMC4,        NONE,      CF,          NONE,        GPIO,     MMC2_CLK,    KP_DK),
-	MFPR_168(GPIO30,  0x0C4, SMC,     MMC4,        UART3_TX,  CF,          NONE,        GPIO,     MMC2,        KP_DK),
-	MFPR_168(GPIO31,  0x0C8, SMC,     MMC4,        UART3,     CF,          NONE,        GPIO,     MMC2,        KP_DK),
-	MFPR_168(GPIO32,  0x0CC, SMC,     MMC4,        UART3,     CF,          NONE,        GPIO,     MMC2,        KP_DK),
-	MFPR_168(GPIO33,  0x0D0, SMC,     MMC4,        UART3,     CF,          CF_nINPACK,  GPIO,     MMC2,        KP_DK),
-	MFPR_168(GPIO34,  0x0D4, GPIO,    NONE,        SMC_CS1,   CF,          CF_nWAIT,    NONE,     MMC3,        KP_DK),
-	MFPR_168(GPIO35,  0x0D8, GPIO,    NONE,        SMC,       CF_nINPACK,  NONE,        NONE,     MMC3_CMD,    KP_DK),
-	MFPR_168(GPIO36,  0x0DC, GPIO,    NONE,        SMC,       CF_nWAIT,    NONE,        NONE,     MMC3_CLK,    KP_DK),
-	MFPR_168(GPIO37,  0x000, GPIO,    MMC1,        NONE,      KP_MKOUT,    CCIC,        XP,       KP_MKIN,     KP_DK),
-	MFPR_168(GPIO38,  0x004, GPIO,    MMC1,        NONE,      KP_MKOUT,    CCIC,        XP,       KP_MKIN,     KP_DK),
-	MFPR_168(GPIO39,  0x008, GPIO,    NONE,        NONE,      KP_MKOUT,    CCIC,        XP,       KP_MKIN,     KP_DK),
-	MFPR_168(GPIO40,  0x00C, GPIO,    MMC1,        MSP,       KP_MKOUT,    CCIC,        XP,       KP_MKIN,     KP_DK),
-	MFPR_168(GPIO41,  0x010, GPIO,    MMC1,        MSP,       NONE,        CCIC,        XP,       KP_MKIN,     KP_DK),
-	MFPR_168(GPIO42,  0x014, GPIO,    I2C,         NONE,      MSP,         CCIC,        XP,       KP_MKIN,     KP_DK),
-	MFPR_168(GPIO43,  0x018, GPIO,    MMC1,        MSP,       MSP_INS,     NONE,        NONE,     KP_MKIN,     KP_DK),
-	MFPR_168(GPIO44,  0x01C, GPIO,    MMC1,        MSP_DAT3,  MSP,         CCIC,        XP,       KP_MKIN,     KP_DK),
-	MFPR_168(GPIO45,  0x020, GPIO,    NONE,        NONE,      MSP,         CCIC,        XP,       NONE,        KP_DK),
-	MFPR_168(GPIO46,  0x024, GPIO,    MMC1,        MSP_INS,   MSP,         CCIC,        NONE,     KP_MKOUT,    KP_DK),
-	MFPR_168(GPIO47,  0x028, GPIO,    NONE,        NONE,      MSP_INS,     NONE,        XP,       NONE,        KP_DK),
-	MFPR_168(GPIO48,  0x02C, GPIO,    MMC1,        NONE,      MSP_DAT3,    CCIC,        NONE,     NONE,        KP_DK),
-	MFPR_168(GPIO49,  0x030, GPIO,    MMC1,        NONE,      MSP,         NONE,        XD,       KP_MKOUT,    NONE),
-	MFPR_168(GPIO50,  0x034, GPIO,    I2C,         NONE,      MSP,         CCIC,        XD,       KP_MKOUT,    NONE),
-	MFPR_168(GPIO51,  0x038, GPIO,    MMC1,        NONE,      MSP,         NONE,        XD,       KP_MKOUT,    NONE),
-	MFPR_168(GPIO52,  0x03C, GPIO,    MMC1,        NONE,      MSP,         NONE,        XD,       KP_MKOUT,    NONE),
-	MFPR_168(GPIO53,  0x040, GPIO,    MMC1,        NONE,      NONE,        NONE,        XD,       KP_MKOUT,    NONE),
-	MFPR_168(GPIO54,  0x044, GPIO,    MMC1,        NONE,      NONE,        CCIC,        XD,       KP_MKOUT,    NONE),
-	MFPR_168(GPIO55,  0x048, GPIO,    NONE,        NONE,      MSP,         CCIC,        XD,       KP_MKOUT,    NONE),
-	MFPR_168(GPIO56,  0x0E0, GPIO,    LCD,         NONE,      NONE,        NONE,        NONE,     NONE,        NONE),
-	MFPR_168(GPIO57,  0x0E4, GPIO,    LCD,         NONE,      NONE,        NONE,        NONE,     NONE,        NONE),
-	MFPR_168(GPIO58,  0x0E8, GPIO,    LCD,         NONE,      NONE,        NONE,        NONE,     NONE,        NONE),
-	MFPR_168(GPIO59,  0x0EC, GPIO,    LCD,         NONE,      NONE,        NONE,        NONE,     NONE,        NONE),
-	MFPR_168(GPIO60,  0x0F0, GPIO,    LCD,         NONE,      NONE,        NONE,        NONE,     NONE,        NONE),
-	MFPR_168(GPIO61,  0x0F4, GPIO,    LCD,         NONE,      NONE,        NONE,        NONE,     NONE,        NONE),
-	MFPR_168(GPIO62,  0x0F8, GPIO,    LCD,         NONE,      NONE,        NONE,        NONE,     NONE,        NONE),
-	MFPR_168(GPIO63,  0x0FC, GPIO,    LCD,         NONE,      NONE,        NONE,        NONE,     NONE,        NONE),
-	MFPR_168(GPIO64,  0x100, GPIO,    LCD,         NONE,      NONE,        NONE,        NONE,     NONE,        NONE),
-	MFPR_168(GPIO65,  0x104, GPIO,    LCD,         NONE,      NONE,        NONE,        NONE,     NONE,        NONE),
-	MFPR_168(GPIO66,  0x108, GPIO,    LCD,         NONE,      NONE,        NONE,        NONE,     NONE,        NONE),
-	MFPR_168(GPIO67,  0x10C, GPIO,    LCD,         NONE,      NONE,        NONE,        NONE,     NONE,        NONE),
-	MFPR_168(GPIO68,  0x110, GPIO,    LCD,         NONE,      XD,          NONE,        NONE,     NONE,        NONE),
-	MFPR_168(GPIO69,  0x114, GPIO,    LCD,         NONE,      XD,          NONE,        NONE,     NONE,        NONE),
-	MFPR_168(GPIO70,  0x118, GPIO,    LCD,         NONE,      XD,          NONE,        NONE,     NONE,        NONE),
-	MFPR_168(GPIO71,  0x11C, GPIO,    LCD,         NONE,      XD,          NONE,        NONE,     NONE,        NONE),
-	MFPR_168(GPIO72,  0x120, GPIO,    LCD,         NONE,      XD,          NONE,        NONE,     NONE,        NONE),
-	MFPR_168(GPIO73,  0x124, GPIO,    LCD,         NONE,      XD,          NONE,        NONE,     NONE,        NONE),
-	MFPR_168(GPIO74,  0x128, GPIO,    LCD,         PWM,       XD,          NONE,        NONE,     NONE,        NONE),
-	MFPR_168(GPIO75,  0x12C, GPIO,    LCD,         PWM,       XD,          ONE_WIRE,    NONE,     NONE,        NONE),
-	MFPR_168(GPIO76,  0x130, GPIO,    LCD,         PWM,       I2C,         NONE,        NONE,     MSP_INS,     NONE),
-	MFPR_168(GPIO77,  0x134, GPIO,    LCD,         PWM1,      I2C,         ONE_WIRE,    NONE,     XD,          NONE),
-	MFPR_168(GPIO78,  0x138, GPIO,    LCD,         NONE,      NONE,        NONE,        MMC4,     NONE,        NONE),
-	MFPR_168(GPIO79,  0x13C, GPIO,    LCD,         NONE,      NONE,        ONE_WIRE,    MMC4,     NONE,        NONE),
-	MFPR_168(GPIO80,  0x140, GPIO,    LCD,         NONE,      I2C,         NONE,        MMC4,     NONE,        NONE),
-	MFPR_168(GPIO81,  0x144, GPIO,    LCD,         NONE,      I2C,         ONE_WIRE,    MMC4,     NONE,        NONE),
-	MFPR_168(GPIO82,  0x148, GPIO,    LCD,         PWM,       NONE,        NONE,        MMC4,     NONE,        NONE),
-	MFPR_168(GPIO83,  0x14C, GPIO,    LCD,         PWM,       NONE,        RESET,       MMC4,     NONE,        NONE),
-	MFPR_168(GPIO84,  0x150, GPIO,    NONE,        PWM,       ONE_WIRE,    PWM1,        NONE,     NONE,        EXT_32K_IN),
-	MFPR_168(GPIO85,  0x154, GPIO,    NONE,        PWM1,      NONE,        NONE,        NONE,     NONE,        USB),
-	MFPR_168(GPIO86,  0x158, GPIO,    MMC2,        UART2,     NONE,        JTAG,        ETH_TX,   SSP5_TX,     SSP5),
-	MFPR_168(GPIO87,  0x15C, GPIO,    MMC2,        UART2,     NONE,        JTAG,        ETH_TX,   SSP5,        SSP5_TX),
-	MFPR_168(GPIO88,  0x160, GPIO,    MMC2,        UART2,     UART2_TX,    JTAG,        ETH_TX,   ETH_RX,      SSP5),
-	MFPR_168(GPIO89,  0x164, GPIO,    MMC2,        UART2_TX,  UART2,       JTAG,        ETH_TX,   ETH_RX,      SSP5),
-	MFPR_168(GPIO90,  0x168, GPIO,    MMC2,        NONE,      SSP3,        JTAG,        ETH_TX,   ETH_RX,      NONE),
-	MFPR_168(GPIO91,  0x16C, GPIO,    MMC2,        NONE,      SSP3,        SSP4,        ETH_TX,   ETH_RX,      NONE),
-	MFPR_168(GPIO92,  0x170, GPIO,    MMC2,        NONE,      SSP3,        SSP3_TX,     ETH,      NONE,        NONE),
-	MFPR_168(GPIO93,  0x174, GPIO,    MMC2,        NONE,      SSP3_TX,     SSP3,        ETH,      NONE,        NONE),
-	MFPR_168(GPIO94,  0x178, GPIO,    MMC2_CMD,    SSP3,      AC97_SYSCLK, AC97,        ETH,      NONE,        NONE),
-	MFPR_168(GPIO95,  0x17C, GPIO,    MMC2_CLK,    NONE,      NONE,        AC97,        ETH,      NONE,        NONE),
-	MFPR_168(GPIO96,  0x180, GPIO,    PWM,         NONE,      MMC2,        NONE,        ETH_RX,   ETH_TX,      NONE),
-	MFPR_168(GPIO97,  0x184, GPIO,    PWM,         ONE_WIRE,  NONE,        NONE,        ETH_RX,   ETH_TX,      NONE),
-	MFPR_168(GPIO98,  0x188, GPIO,    PWM1,        UART3_TX,  UART3,       NONE,        ETH_RX,   ETH_TX,      NONE),
-	MFPR_168(GPIO99,  0x18C, GPIO,    ONE_WIRE,    UART3,     UART3_TX,    NONE,        ETH_RX,   ETH_TX,      NONE),
-	MFPR_168(GPIO100, 0x190, GPIO,    NONE,        UART3_CTS, UART3,       NONE,        ETH,      NONE,        NONE),
-	MFPR_168(GPIO101, 0x194, GPIO,    NONE,        UART3,     UART3_CTS,   NONE,        ETH,      NONE,        NONE),
-	MFPR_168(GPIO102, 0x198, GPIO,    I2C,         UART3,     SSP4,        NONE,        NONE,     NONE,        NONE),
-	MFPR_168(GPIO103, 0x19C, GPIO,    I2C,         UART3,     SSP4,        SSP2,        ETH,      NONE,        NONE),
-	MFPR_168(GPIO104, 0x1A0, GPIO,    PWM,         UART1,     SSP4,        SSP4_TX,     AC97,     KP_MKOUT,    NONE),
-	MFPR_168(GPIO105, 0x1A4, GPIO,    I2C,         UART1,     SSP4_TX,     SSP4,        AC97,     KP_MKOUT,    NONE),
-	MFPR_168(GPIO106, 0x1A8, GPIO,    I2C,         PWM1,      AC97_SYSCLK, MMC2,        NONE,     KP_MKOUT,    NONE),
-	MFPR_168(GPIO107, 0x1AC, GPIO,    UART1_TX,    UART1,     NONE,        SSP2,        MSP_DAT3, NONE,        KP_MKIN),
-	MFPR_168(GPIO108, 0x1B0, GPIO,    UART1,       UART1_TX,  NONE,        SSP2_TX,     MSP,      NONE,        KP_MKIN),
-	MFPR_168(GPIO109, 0x1B4, GPIO,    UART1_CTS,   UART1,     NONE,        AC97_SYSCLK, MSP,      NONE,        KP_MKIN),
-	MFPR_168(GPIO110, 0x1B8, GPIO,    UART1,       UART1_CTS, NONE,        SMC_RDY,     MSP,      NONE,        KP_MKIN),
-	MFPR_168(GPIO111, 0x1BC, GPIO,    UART1_nRI,   UART1,     SSP3,        SSP2,        MSP,      XD,          KP_MKOUT),
-	MFPR_168(GPIO112, 0x1C0, GPIO,    UART1_DTR,   UART1,     ONE_WIRE,    SSP2,        MSP,      XD,          KP_MKOUT),
-	MFPR_168(GPIO113, 0x1C4, GPIO,    NONE,        NONE,      NONE,        NONE,        NONE,     AC97_SYSCLK, NONE),
-	MFPR_168(GPIO114, 0x1C8, GPIO,    SSP1,        NONE,      NONE,        NONE,        NONE,     AC97,        NONE),
-	MFPR_168(GPIO115, 0x1CC, GPIO,    SSP1,        NONE,      NONE,        NONE,        NONE,     AC97,        NONE),
-	MFPR_168(GPIO116, 0x1D0, GPIO,    SSP1_TX,     SSP1,      NONE,        NONE,        NONE,     AC97,        NONE),
-	MFPR_168(GPIO117, 0x1D4, GPIO,    SSP1,        SSP1_TX,   NONE,        MMC2_CMD,    NONE,     AC97,        NONE),
-	MFPR_168(GPIO118, 0x1D8, GPIO,    SSP2,        NONE,      NONE,        MMC2_CLK,    NONE,     AC97,        KP_MKIN),
-	MFPR_168(GPIO119, 0x1DC, GPIO,    SSP2,        NONE,      NONE,        MMC2,        NONE,     AC97,        KP_MKIN),
-	MFPR_168(GPIO120, 0x1E0, GPIO,    SSP2,        SSP2_TX,   NONE,        MMC2,        NONE,     NONE,        KP_MKIN),
-	MFPR_168(GPIO121, 0x1E4, GPIO,    SSP2_TX,     SSP2,      NONE,        MMC2,        NONE,     NONE,        KP_MKIN),
-	MFPR_168(GPIO122, 0x1E8, GPIO,    AC97_SYSCLK, SSP2,      PWM,         MMC2,        NONE,     NONE,        NONE),
-	MFPR_168(PWR_SCL, 0x1EC, PWRI2C,  NONE,        NONE,      NONE,        NONE,        NONE,     GPIO,        MMC4),
-	MFPR_168(PWR_SDA, 0x1F0, PWRI2C,  NONE,        NONE,      NONE,        NONE,        NONE,     GPIO,        NONE),
-	MFPR_168(TDI,     0x1F4, JTAG,    PWM1,        UART2,     MMC4,        SSP5,        NONE,     XD,          MMC4),
-	MFPR_168(TMS,     0x1F8, JTAG,    PWM,         UART2,     NONE,        SSP5,        NONE,     XD,          MMC4),
-	MFPR_168(TCK,     0x1FC, JTAG,    PWM,         UART2,     UART2_TX,    SSP5,        NONE,     XD,          MMC4),
-	MFPR_168(TDO,     0x200, JTAG,    PWM,         UART2_TX,  UART2,       SSP5_TX,     NONE,     XD,          MMC4),
-	MFPR_168(TRST,    0x204, JTAG,    ONE_WIRE,    SSP2,      SSP3,        AC97_SYSCLK, NONE,     XD,          MMC4),
-	MFPR_168(WAKEUP,  0x208, WAKEUP,  ONE_WIRE,    PWM1,      PWM,         SSP2,        NONE,     GPIO,        MMC4),
-};
-
-static const unsigned p168_jtag_pin1[] = {TDI, TMS, TCK, TDO, TRST};
-static const unsigned p168_wakeup_pin1[] = {WAKEUP};
-static const unsigned p168_ssp1rx_pin1[] = {GPIO114, GPIO115, GPIO116};
-static const unsigned p168_ssp1tx_pin1[] = {GPIO117};
-static const unsigned p168_ssp4rx_pin1[] = {GPIO102, GPIO103, GPIO104};
-static const unsigned p168_ssp4tx_pin1[] = {GPIO105};
-static const unsigned p168_ssp5rx_pin1[] = {GPIO86, GPIO88, GPIO89};
-static const unsigned p168_ssp5tx_pin1[] = {GPIO87};
-static const unsigned p168_i2c_pin1[] = {GPIO105, GPIO106};
-static const unsigned p168_pwri2c_pin1[] = {PWR_SCL, PWR_SDA};
-static const unsigned p168_mmc1_pin1[] = {GPIO40, GPIO41, GPIO43, GPIO46,
-	GPIO49, GPIO51, GPIO52, GPIO53};
-static const unsigned p168_mmc2_data_pin1[] = {GPIO90, GPIO91, GPIO92, GPIO93};
-static const unsigned p168_mmc2_cmd_pin1[] = {GPIO94};
-static const unsigned p168_mmc2_clk_pin1[] = {GPIO95};
-static const unsigned p168_mmc3_data_pin1[] = {GPIO0, GPIO1, GPIO2, GPIO3,
-	GPIO4, GPIO5, GPIO6, GPIO7};
-static const unsigned p168_mmc3_cmd_pin1[] = {GPIO9};
-static const unsigned p168_mmc3_clk_pin1[] = {GPIO8};
-static const unsigned p168_eth_pin1[] = {GPIO92, GPIO93, GPIO100, GPIO101,
-	GPIO103};
-static const unsigned p168_ethtx_pin1[] = {GPIO86, GPIO87, GPIO88, GPIO89,
-	GPIO90, GPIO91};
-static const unsigned p168_ethrx_pin1[] = {GPIO94, GPIO95, GPIO96, GPIO97,
-	GPIO98, GPIO99};
-static const unsigned p168_uart1rx_pin1[] = {GPIO107};
-static const unsigned p168_uart1tx_pin1[] = {GPIO108};
-static const unsigned p168_uart3rx_pin1[] = {GPIO98, GPIO100, GPIO101};
-static const unsigned p168_uart3tx_pin1[] = {GPIO99};
-static const unsigned p168_msp_pin1[] = {GPIO40, GPIO41, GPIO42, GPIO43,
-	GPIO44, GPIO50};
-static const unsigned p168_ccic_pin1[] = {GPIO37, GPIO38, GPIO39, GPIO40,
-	GPIO41, GPIO42, GPIO44, GPIO45, GPIO46, GPIO48, GPIO54, GPIO55};
-static const unsigned p168_xd_pin1[] = {GPIO37, GPIO38, GPIO39, GPIO40,
-	GPIO41, GPIO42, GPIO44, GPIO45, GPIO47, GPIO48, GPIO49, GPIO50,
-	GPIO51, GPIO52};
-static const unsigned p168_lcd_pin1[] = {GPIO56, GPIO57, GPIO58, GPIO59,
-	GPIO60, GPIO61, GPIO62, GPIO63, GPIO64, GPIO65, GPIO66, GPIO67,
-	GPIO68, GPIO69, GPIO70, GPIO71, GPIO72, GPIO73, GPIO74, GPIO75,
-	GPIO76, GPIO77, GPIO78, GPIO79, GPIO80, GPIO81, GPIO82, GPIO83};
-static const unsigned p168_dfio_pin1[] = {GPIO0, GPIO1, GPIO2, GPIO3,
-	GPIO4, GPIO5, GPIO6, GPIO7, GPIO8, GPIO9, GPIO10, GPIO11, GPIO12,
-	GPIO13, GPIO14, GPIO15};
-static const unsigned p168_nand_pin1[] = {GPIO16, GPIO17, GPIO21, GPIO22,
-	GPIO24, GPIO26};
-static const unsigned p168_smc_pin1[] = {GPIO23, GPIO25, GPIO29, GPIO35,
-	GPIO36};
-static const unsigned p168_smccs0_pin1[] = {GPIO18};
-static const unsigned p168_smccs1_pin1[] = {GPIO34};
-static const unsigned p168_smcrdy_pin1[] = {GPIO28};
-static const unsigned p168_ac97sysclk_pin1[] = {GPIO113};
-static const unsigned p168_ac97_pin1[] = {GPIO114, GPIO115, GPIO117, GPIO118,
-	GPIO119};
-static const unsigned p168_cf_pin1[] = {GPIO19, GPIO20, GPIO23, GPIO25,
-	GPIO28, GPIO29, GPIO30, GPIO31, GPIO32, GPIO33, GPIO34, GPIO35,
-	GPIO36};
-static const unsigned p168_kpmkin_pin1[] = {GPIO109, GPIO110, GPIO121};
-static const unsigned p168_kpmkout_pin1[] = {GPIO111, GPIO112};
-static const unsigned p168_gpio86_pin1[] = {WAKEUP};
-static const unsigned p168_gpio86_pin2[] = {GPIO86};
-static const unsigned p168_gpio87_pin1[] = {GPIO87};
-static const unsigned p168_gpio87_pin2[] = {PWR_SDA};
-static const unsigned p168_gpio88_pin1[] = {GPIO88};
-static const unsigned p168_gpio88_pin2[] = {PWR_SCL};
-
-static struct pxa3xx_pin_group pxa168_grps[] = {
-	GRP_168("uart1rx-1", UART1, p168_uart1rx_pin1),
-	GRP_168("uart1tx-1", UART1_TX, p168_uart1tx_pin1),
-	GRP_168("uart3rx-1", UART3, p168_uart3rx_pin1),
-	GRP_168("uart3tx-1", UART3_TX, p168_uart3tx_pin1),
-	GRP_168("ssp1rx-1", SSP1, p168_ssp1rx_pin1),
-	GRP_168("ssp1tx-1", SSP1_TX, p168_ssp1tx_pin1),
-	GRP_168("ssp4rx-1", SSP4, p168_ssp4rx_pin1),
-	GRP_168("ssp4tx-1", SSP4_TX, p168_ssp4tx_pin1),
-	GRP_168("ssp5rx-1", SSP5, p168_ssp5rx_pin1),
-	GRP_168("ssp5tx-1", SSP5_TX, p168_ssp5tx_pin1),
-	GRP_168("jtag", JTAG, p168_jtag_pin1),
-	GRP_168("wakeup", WAKEUP, p168_wakeup_pin1),
-	GRP_168("i2c", I2C, p168_i2c_pin1),
-	GRP_168("pwri2c", PWRI2C, p168_pwri2c_pin1),
-	GRP_168("mmc1 8p1", MMC1, p168_mmc1_pin1),
-	GRP_168("mmc2 4p1", MMC2, p168_mmc2_data_pin1),
-	GRP_168("mmc2 cmd1", MMC2_CMD, p168_mmc2_cmd_pin1),
-	GRP_168("mmc2 clk1", MMC2_CLK, p168_mmc2_clk_pin1),
-	GRP_168("mmc3 8p1", MMC3, p168_mmc3_data_pin1),
-	GRP_168("mmc3 cmd1", MMC3_CMD, p168_mmc3_cmd_pin1),
-	GRP_168("mmc3 clk1", MMC3_CLK, p168_mmc3_clk_pin1),
-	GRP_168("eth", ETH, p168_eth_pin1),
-	GRP_168("eth rx", ETH_RX, p168_ethrx_pin1),
-	GRP_168("eth tx", ETH_TX, p168_ethtx_pin1),
-	GRP_168("msp", MSP, p168_msp_pin1),
-	GRP_168("ccic", CCIC, p168_ccic_pin1),
-	GRP_168("xd", XD, p168_xd_pin1),
-	GRP_168("lcd", LCD, p168_lcd_pin1),
-	GRP_168("dfio", DFIO, p168_dfio_pin1),
-	GRP_168("nand", NAND, p168_nand_pin1),
-	GRP_168("smc", SMC, p168_smc_pin1),
-	GRP_168("smc cs0", SMC_CS0, p168_smccs0_pin1),
-	GRP_168("smc cs1", SMC_CS1, p168_smccs1_pin1),
-	GRP_168("smc rdy", SMC_RDY, p168_smcrdy_pin1),
-	GRP_168("ac97 sysclk", AC97_SYSCLK, p168_ac97sysclk_pin1),
-	GRP_168("ac97", AC97, p168_ac97_pin1),
-	GRP_168("cf", CF, p168_cf_pin1),
-	GRP_168("kp mkin 3p1", KP_MKIN, p168_kpmkin_pin1),
-	GRP_168("kp mkout 2p1", KP_MKOUT, p168_kpmkout_pin1),
-	GRP_168("gpio86-1", GPIO, p168_gpio86_pin1),
-	GRP_168("gpio86-2", GPIO, p168_gpio86_pin2),
-	GRP_168("gpio87-1", GPIO, p168_gpio87_pin1),
-	GRP_168("gpio87-2", GPIO, p168_gpio87_pin2),
-	GRP_168("gpio88-1", GPIO, p168_gpio88_pin1),
-	GRP_168("gpio88-2", GPIO, p168_gpio88_pin2),
-};
-
-static const char * const p168_uart1rx_grps[] = {"uart1rx-1"};
-static const char * const p168_uart1tx_grps[] = {"uart1tx-1"};
-static const char * const p168_uart3rx_grps[] = {"uart3rx-1"};
-static const char * const p168_uart3tx_grps[] = {"uart3tx-1"};
-static const char * const p168_ssp1rx_grps[] = {"ssp1rx-1"};
-static const char * const p168_ssp1tx_grps[] = {"ssp1tx-1"};
-static const char * const p168_ssp4rx_grps[] = {"ssp4rx-1"};
-static const char * const p168_ssp4tx_grps[] = {"ssp4tx-1"};
-static const char * const p168_ssp5rx_grps[] = {"ssp5rx-1"};
-static const char * const p168_ssp5tx_grps[] = {"ssp5tx-1"};
-static const char * const p168_i2c_grps[] = {"i2c"};
-static const char * const p168_pwri2c_grps[] = {"pwri2c"};
-static const char * const p168_mmc1_grps[] = {"mmc1 8p1"};
-static const char * const p168_mmc2_data_grps[] = {"mmc2 4p1"};
-static const char * const p168_mmc2_cmd_grps[] = {"mmc2 cmd1"};
-static const char * const p168_mmc2_clk_grps[] = {"mmc2 clk1"};
-static const char * const p168_mmc3_data_grps[] = {"mmc3 8p1"};
-static const char * const p168_mmc3_cmd_grps[] = {"mmc3 cmd1"};
-static const char * const p168_mmc3_clk_grps[] = {"mmc3 clk1"};
-static const char * const p168_eth_grps[] = {"eth"};
-static const char * const p168_ethrx_grps[] = {"eth rx"};
-static const char * const p168_ethtx_grps[] = {"eth tx"};
-static const char * const p168_msp_grps[] = {"msp"};
-static const char * const p168_ccic_grps[] = {"ccic"};
-static const char * const p168_xd_grps[] = {"xd"};
-static const char * const p168_lcd_grps[] = {"lcd"};
-static const char * const p168_dfio_grps[] = {"dfio"};
-static const char * const p168_nand_grps[] = {"nand"};
-static const char * const p168_smc_grps[] = {"smc"};
-static const char * const p168_smccs0_grps[] = {"smc cs0"};
-static const char * const p168_smccs1_grps[] = {"smc cs1"};
-static const char * const p168_smcrdy_grps[] = {"smc rdy"};
-static const char * const p168_ac97sysclk_grps[] = {"ac97 sysclk"};
-static const char * const p168_ac97_grps[] = {"ac97"};
-static const char * const p168_cf_grps[] = {"cf"};
-static const char * const p168_kpmkin_grps[] = {"kp mkin 3p1"};
-static const char * const p168_kpmkout_grps[] = {"kp mkout 2p1"};
-static const char * const p168_gpio86_grps[] = {"gpio86-1", "gpio86-2"};
-static const char * const p168_gpio87_grps[] = {"gpio87-1", "gpio87-2"};
-static const char * const p168_gpio88_grps[] = {"gpio88-1", "gpio88-2"};
-
-static struct pxa3xx_pmx_func pxa168_funcs[] = {
-	{"uart1 rx",	ARRAY_AND_SIZE(p168_uart1rx_grps)},
-	{"uart1 tx",	ARRAY_AND_SIZE(p168_uart1tx_grps)},
-	{"uart3 rx",	ARRAY_AND_SIZE(p168_uart3rx_grps)},
-	{"uart3 tx",	ARRAY_AND_SIZE(p168_uart3tx_grps)},
-	{"ssp1 rx",	ARRAY_AND_SIZE(p168_ssp1rx_grps)},
-	{"ssp1 tx",	ARRAY_AND_SIZE(p168_ssp1tx_grps)},
-	{"ssp4 rx",	ARRAY_AND_SIZE(p168_ssp4rx_grps)},
-	{"ssp4 tx",	ARRAY_AND_SIZE(p168_ssp4tx_grps)},
-	{"ssp5 rx",	ARRAY_AND_SIZE(p168_ssp5rx_grps)},
-	{"ssp5 tx",	ARRAY_AND_SIZE(p168_ssp5tx_grps)},
-	{"i2c",		ARRAY_AND_SIZE(p168_i2c_grps)},
-	{"pwri2c",	ARRAY_AND_SIZE(p168_pwri2c_grps)},
-	{"mmc1",	ARRAY_AND_SIZE(p168_mmc1_grps)},
-	{"mmc2",	ARRAY_AND_SIZE(p168_mmc2_data_grps)},
-	{"mmc2 cmd",	ARRAY_AND_SIZE(p168_mmc2_cmd_grps)},
-	{"mmc2 clk",	ARRAY_AND_SIZE(p168_mmc2_clk_grps)},
-	{"mmc3",	ARRAY_AND_SIZE(p168_mmc3_data_grps)},
-	{"mmc3 cmd",	ARRAY_AND_SIZE(p168_mmc3_cmd_grps)},
-	{"mmc3 clk",	ARRAY_AND_SIZE(p168_mmc3_clk_grps)},
-	{"eth",		ARRAY_AND_SIZE(p168_eth_grps)},
-	{"eth rx",	ARRAY_AND_SIZE(p168_ethrx_grps)},
-	{"eth tx",	ARRAY_AND_SIZE(p168_ethtx_grps)},
-	{"msp",		ARRAY_AND_SIZE(p168_msp_grps)},
-	{"ccic",	ARRAY_AND_SIZE(p168_ccic_grps)},
-	{"xd",		ARRAY_AND_SIZE(p168_xd_grps)},
-	{"lcd",		ARRAY_AND_SIZE(p168_lcd_grps)},
-	{"dfio",	ARRAY_AND_SIZE(p168_dfio_grps)},
-	{"nand",	ARRAY_AND_SIZE(p168_nand_grps)},
-	{"smc",		ARRAY_AND_SIZE(p168_smc_grps)},
-	{"smc cs0",	ARRAY_AND_SIZE(p168_smccs0_grps)},
-	{"smc cs1",	ARRAY_AND_SIZE(p168_smccs1_grps)},
-	{"smc rdy",	ARRAY_AND_SIZE(p168_smcrdy_grps)},
-	{"ac97",	ARRAY_AND_SIZE(p168_ac97_grps)},
-	{"ac97 sysclk",	ARRAY_AND_SIZE(p168_ac97sysclk_grps)},
-	{"cf",		ARRAY_AND_SIZE(p168_cf_grps)},
-	{"kpmkin",	ARRAY_AND_SIZE(p168_kpmkin_grps)},
-	{"kpmkout",	ARRAY_AND_SIZE(p168_kpmkout_grps)},
-	{"gpio86",	ARRAY_AND_SIZE(p168_gpio86_grps)},
-	{"gpio87",	ARRAY_AND_SIZE(p168_gpio87_grps)},
-	{"gpio88",	ARRAY_AND_SIZE(p168_gpio88_grps)},
-};
-
-static struct pinctrl_desc pxa168_pctrl_desc = {
-	.name		= "pxa168-pinctrl",
-	.owner		= THIS_MODULE,
-};
-
-static struct pxa3xx_pinmux_info pxa168_info = {
-	.mfp		= pxa168_mfp,
-	.num_mfp	= ARRAY_SIZE(pxa168_mfp),
-	.grps		= pxa168_grps,
-	.num_grps	= ARRAY_SIZE(pxa168_grps),
-	.funcs		= pxa168_funcs,
-	.num_funcs	= ARRAY_SIZE(pxa168_funcs),
-	.num_gpio	= 128,
-	.desc		= &pxa168_pctrl_desc,
-	.pads		= pxa168_pads,
-	.num_pads	= ARRAY_SIZE(pxa168_pads),
-
-	.cputype	= PINCTRL_PXA168,
-	.ds_mask	= PXA168_DS_MASK,
-	.ds_shift	= PXA168_DS_SHIFT,
-};
-
-static int pxa168_pinmux_probe(struct platform_device *pdev)
-{
-	return pxa3xx_pinctrl_register(pdev, &pxa168_info);
-}
-
-static int pxa168_pinmux_remove(struct platform_device *pdev)
-{
-	return pxa3xx_pinctrl_unregister(pdev);
-}
-
-static struct platform_driver pxa168_pinmux_driver = {
-	.driver = {
-		.name	= "pxa168-pinmux",
-		.owner	= THIS_MODULE,
-	},
-	.probe	= pxa168_pinmux_probe,
-	.remove	= pxa168_pinmux_remove,
-};
-
-static int __init pxa168_pinmux_init(void)
-{
-	return platform_driver_register(&pxa168_pinmux_driver);
-}
-core_initcall_sync(pxa168_pinmux_init);
-
-static void __exit pxa168_pinmux_exit(void)
-{
-	platform_driver_unregister(&pxa168_pinmux_driver);
-}
-module_exit(pxa168_pinmux_exit);
-
-MODULE_AUTHOR("Haojian Zhuang <haojian.zhuang@marvell.com>");
-MODULE_DESCRIPTION("PXA3xx pin control driver");
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/pinctrl-pxa3xx.c b/drivers/pinctrl/pinctrl-pxa3xx.c
deleted file mode 100644
index 1f49bb02a6a..00000000000
--- a/drivers/pinctrl/pinctrl-pxa3xx.c
+++ /dev/null
@@ -1,227 +0,0 @@
-/*
- *  linux/drivers/pinctrl/pinctrl-pxa3xx.c
- *
- *  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
- *  publishhed by the Free Software Foundation.
- *
- *  Copyright (C) 2011, Marvell Technology Group Ltd.
- *
- *  Author: Haojian Zhuang <haojian.zhuang@marvell.com>
- *
- */
-
-#include <linux/err.h>
-#include <linux/module.h>
-#include <linux/device.h>
-#include <linux/io.h>
-#include <linux/platform_device.h>
-#include <linux/slab.h>
-#include "pinctrl-pxa3xx.h"
-
-static struct pinctrl_gpio_range pxa3xx_pinctrl_gpio_range = {
-	.name		= "PXA3xx GPIO",
-	.id		= 0,
-	.base		= 0,
-	.pin_base	= 0,
-};
-
-static int pxa3xx_get_groups_count(struct pinctrl_dev *pctrldev)
-{
-	struct pxa3xx_pinmux_info *info = pinctrl_dev_get_drvdata(pctrldev);
-
-	return info->num_grps;
-}
-
-static const char *pxa3xx_get_group_name(struct pinctrl_dev *pctrldev,
-					 unsigned selector)
-{
-	struct pxa3xx_pinmux_info *info = pinctrl_dev_get_drvdata(pctrldev);
-
-	return info->grps[selector].name;
-}
-
-static int pxa3xx_get_group_pins(struct pinctrl_dev *pctrldev,
-				 unsigned selector,
-				 const unsigned **pins,
-				 unsigned *num_pins)
-{
-	struct pxa3xx_pinmux_info *info = pinctrl_dev_get_drvdata(pctrldev);
-
-	*pins = info->grps[selector].pins;
-	*num_pins = info->grps[selector].npins;
-	return 0;
-}
-
-static struct pinctrl_ops pxa3xx_pctrl_ops = {
-	.get_groups_count = pxa3xx_get_groups_count,
-	.get_group_name	= pxa3xx_get_group_name,
-	.get_group_pins	= pxa3xx_get_group_pins,
-};
-
-static int pxa3xx_pmx_get_funcs_count(struct pinctrl_dev *pctrldev)
-{
-	struct pxa3xx_pinmux_info *info = pinctrl_dev_get_drvdata(pctrldev);
-
-	return info->num_funcs;
-}
-
-static const char *pxa3xx_pmx_get_func_name(struct pinctrl_dev *pctrldev,
-					    unsigned func)
-{
-	struct pxa3xx_pinmux_info *info = pinctrl_dev_get_drvdata(pctrldev);
-	return info->funcs[func].name;
-}
-
-static int pxa3xx_pmx_get_groups(struct pinctrl_dev *pctrldev, unsigned func,
-				 const char * const **groups,
-				 unsigned * const num_groups)
-{
-	struct pxa3xx_pinmux_info *info = pinctrl_dev_get_drvdata(pctrldev);
-	*groups = info->funcs[func].groups;
-	*num_groups = info->funcs[func].num_groups;
-	return 0;
-}
-
-/* Return function number. If failure, return negative value. */
-static int match_mux(struct pxa3xx_mfp_pin *mfp, unsigned mux)
-{
-	int i;
-	for (i = 0; i < PXA3xx_MAX_MUX; i++) {
-		if (mfp->func[i] == mux)
-			break;
-	}
-	if (i >= PXA3xx_MAX_MUX)
-		return -EINVAL;
-	return i;
-}
-
-/* check whether current pin configuration is valid. Negative for failure */
-static int match_group_mux(struct pxa3xx_pin_group *grp,
-			   struct pxa3xx_pinmux_info *info,
-			   unsigned mux)
-{
-	int i, pin, ret = 0;
-	for (i = 0; i < grp->npins; i++) {
-		pin = grp->pins[i];
-		ret = match_mux(&info->mfp[pin], mux);
-		if (ret < 0) {
-			dev_err(info->dev, "Can't find mux %d on pin%d\n",
-				mux, pin);
-			break;
-		}
-	}
-	return ret;
-}
-
-static int pxa3xx_pmx_enable(struct pinctrl_dev *pctrldev, unsigned func,
-			     unsigned group)
-{
-	struct pxa3xx_pinmux_info *info = pinctrl_dev_get_drvdata(pctrldev);
-	struct pxa3xx_pin_group *pin_grp = &info->grps[group];
-	unsigned int data;
-	int i, mfpr, pin, pin_func;
-
-	if (!pin_grp->npins ||
-		(match_group_mux(pin_grp, info, pin_grp->mux) < 0)) {
-		dev_err(info->dev, "Failed to set the pin group: %d\n", group);
-		return -EINVAL;
-	}
-	for (i = 0; i < pin_grp->npins; i++) {
-		pin = pin_grp->pins[i];
-		pin_func = match_mux(&info->mfp[pin], pin_grp->mux);
-		mfpr = info->mfp[pin].mfpr;
-		data = readl_relaxed(info->virt_base + mfpr);
-		data &= ~MFPR_FUNC_MASK;
-		data |= pin_func;
-		writel_relaxed(data, info->virt_base + mfpr);
-	}
-	return 0;
-}
-
-static int pxa3xx_pmx_request_gpio(struct pinctrl_dev *pctrldev,
-				   struct pinctrl_gpio_range *range,
-				   unsigned pin)
-{
-	struct pxa3xx_pinmux_info *info = pinctrl_dev_get_drvdata(pctrldev);
-	unsigned int data;
-	int pin_func, mfpr;
-
-	pin_func = match_mux(&info->mfp[pin], PXA3xx_MUX_GPIO);
-	if (pin_func < 0) {
-		dev_err(info->dev, "No GPIO function on pin%d (%s)\n",
-			pin, info->pads[pin].name);
-		return -EINVAL;
-	}
-	mfpr = info->mfp[pin].mfpr;
-	/* write gpio function into mfpr register */
-	data = readl_relaxed(info->virt_base + mfpr) & ~MFPR_FUNC_MASK;
-	data |= pin_func;
-	writel_relaxed(data, info->virt_base + mfpr);
-	return 0;
-}
-
-static struct pinmux_ops pxa3xx_pmx_ops = {
-	.get_functions_count	= pxa3xx_pmx_get_funcs_count,
-	.get_function_name	= pxa3xx_pmx_get_func_name,
-	.get_function_groups	= pxa3xx_pmx_get_groups,
-	.enable			= pxa3xx_pmx_enable,
-	.gpio_request_enable	= pxa3xx_pmx_request_gpio,
-};
-
-int pxa3xx_pinctrl_register(struct platform_device *pdev,
-			    struct pxa3xx_pinmux_info *info)
-{
-	struct pinctrl_desc *desc;
-	struct resource *res;
-
-	if (!info || !info->cputype)
-		return -EINVAL;
-	desc = info->desc;
-	desc->pins = info->pads;
-	desc->npins = info->num_pads;
-	desc->pctlops = &pxa3xx_pctrl_ops;
-	desc->pmxops = &pxa3xx_pmx_ops;
-	info->dev = &pdev->dev;
-	pxa3xx_pinctrl_gpio_range.npins = info->num_gpio;
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res)
-		return -ENOENT;
-	info->virt_base = devm_ioremap_resource(&pdev->dev, res);
-	if (IS_ERR(info->virt_base))
-		return PTR_ERR(info->virt_base);
-	info->pctrl = pinctrl_register(desc, &pdev->dev, info);
-	if (!info->pctrl) {
-		dev_err(&pdev->dev, "failed to register PXA pinmux driver\n");
-		return -EINVAL;
-	}
-	pinctrl_add_gpio_range(info->pctrl, &pxa3xx_pinctrl_gpio_range);
-	platform_set_drvdata(pdev, info);
-	return 0;
-}
-
-int pxa3xx_pinctrl_unregister(struct platform_device *pdev)
-{
-	struct pxa3xx_pinmux_info *info = platform_get_drvdata(pdev);
-
-	pinctrl_unregister(info->pctrl);
-	platform_set_drvdata(pdev, NULL);
-	return 0;
-}
-
-static int __init pxa3xx_pinctrl_init(void)
-{
-	pr_info("pxa3xx-pinctrl: PXA3xx pinctrl driver initializing\n");
-	return 0;
-}
-core_initcall_sync(pxa3xx_pinctrl_init);
-
-static void __exit pxa3xx_pinctrl_exit(void)
-{
-}
-module_exit(pxa3xx_pinctrl_exit);
-
-MODULE_AUTHOR("Haojian Zhuang <haojian.zhuang@marvell.com>");
-MODULE_DESCRIPTION("PXA3xx pin control driver");
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/pinctrl-pxa3xx.h b/drivers/pinctrl/pinctrl-pxa3xx.h
deleted file mode 100644
index 92fad088083..00000000000
--- a/drivers/pinctrl/pinctrl-pxa3xx.h
+++ /dev/null
@@ -1,262 +0,0 @@
-/*
- *  linux/drivers/pinctrl/pinctrl-pxa3xx.h
- *
- *  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
- *  publishhed by the Free Software Foundation.
- *
- *  Copyright (C) 2011, Marvell Technology Group Ltd.
- *
- *  Author: Haojian Zhuang <haojian.zhuang@marvell.com>
- *
- */
-
-#ifndef __PINCTRL_PXA3XX_H
-
-#include <linux/pinctrl/pinctrl.h>
-#include <linux/pinctrl/pinmux.h>
-
-#define ARRAY_AND_SIZE(x)	(x), ARRAY_SIZE(x)
-
-#define PXA3xx_MUX_GPIO		0
-
-#define PXA3xx_MAX_MUX		8
-#define MFPR_FUNC_MASK		0x7
-
-enum pxa_cpu_type {
-	PINCTRL_INVALID = 0,
-	PINCTRL_PXA300,
-	PINCTRL_PXA310,
-	PINCTRL_PXA320,
-	PINCTRL_PXA168,
-	PINCTRL_PXA910,
-	PINCTRL_PXA930,
-	PINCTRL_PXA955,
-	PINCTRL_MMP2,
-	PINCTRL_MAX,
-};
-
-struct pxa3xx_mfp_pin {
-	const char *name;
-	const unsigned int pin;
-	const unsigned int mfpr;	/* register offset */
-	const unsigned short func[8];
-};
-
-struct pxa3xx_pin_group {
-	const char *name;
-	const unsigned mux;
-	const unsigned *pins;
-	const unsigned npins;
-};
-
-struct pxa3xx_pmx_func {
-	const char *name;
-	const char * const * groups;
-	const unsigned num_groups;
-};
-
-struct pxa3xx_pinmux_info {
-	struct device *dev;
-	struct pinctrl_dev *pctrl;
-	enum pxa_cpu_type cputype;
-	void __iomem *virt_base;
-
-	struct pxa3xx_mfp_pin *mfp;
-	unsigned int num_mfp;
-	struct pxa3xx_pin_group *grps;
-	unsigned int num_grps;
-	struct pxa3xx_pmx_func *funcs;
-	unsigned int num_funcs;
-	unsigned int num_gpio;
-	struct pinctrl_desc *desc;
-	struct pinctrl_pin_desc *pads;
-	unsigned int num_pads;
-
-	unsigned ds_mask;	/* drive strength mask */
-	unsigned ds_shift;	/* drive strength shift */
-	unsigned slp_mask;	/* sleep mask */
-	unsigned slp_input_low;
-	unsigned slp_input_high;
-	unsigned slp_output_low;
-	unsigned slp_output_high;
-	unsigned slp_float;
-};
-
-enum pxa3xx_pin_list {
-	GPIO0 = 0,
-	GPIO1,
-	GPIO2,
-	GPIO3,
-	GPIO4,
-	GPIO5,
-	GPIO6,
-	GPIO7,
-	GPIO8,
-	GPIO9,
-	GPIO10, /* 10 */
-	GPIO11,
-	GPIO12,
-	GPIO13,
-	GPIO14,
-	GPIO15,
-	GPIO16,
-	GPIO17,
-	GPIO18,
-	GPIO19,
-	GPIO20, /* 20 */
-	GPIO21,
-	GPIO22,
-	GPIO23,
-	GPIO24,
-	GPIO25,
-	GPIO26,
-	GPIO27,
-	GPIO28,
-	GPIO29,
-	GPIO30, /* 30 */
-	GPIO31,
-	GPIO32,
-	GPIO33,
-	GPIO34,
-	GPIO35,
-	GPIO36,
-	GPIO37,
-	GPIO38,
-	GPIO39,
-	GPIO40, /* 40 */
-	GPIO41,
-	GPIO42,
-	GPIO43,
-	GPIO44,
-	GPIO45,
-	GPIO46,
-	GPIO47,
-	GPIO48,
-	GPIO49,
-	GPIO50, /* 50 */
-	GPIO51,
-	GPIO52,
-	GPIO53,
-	GPIO54,
-	GPIO55,
-	GPIO56,
-	GPIO57,
-	GPIO58,
-	GPIO59,
-	GPIO60, /* 60 */
-	GPIO61,
-	GPIO62,
-	GPIO63,
-	GPIO64,
-	GPIO65,
-	GPIO66,
-	GPIO67,
-	GPIO68,
-	GPIO69,
-	GPIO70, /* 70 */
-	GPIO71,
-	GPIO72,
-	GPIO73,
-	GPIO74,
-	GPIO75,
-	GPIO76,
-	GPIO77,
-	GPIO78,
-	GPIO79,
-	GPIO80, /* 80 */
-	GPIO81,
-	GPIO82,
-	GPIO83,
-	GPIO84,
-	GPIO85,
-	GPIO86,
-	GPIO87,
-	GPIO88,
-	GPIO89,
-	GPIO90, /* 90 */
-	GPIO91,
-	GPIO92,
-	GPIO93,
-	GPIO94,
-	GPIO95,
-	GPIO96,
-	GPIO97,
-	GPIO98,
-	GPIO99,
-	GPIO100, /* 100 */
-	GPIO101,
-	GPIO102,
-	GPIO103,
-	GPIO104,
-	GPIO105,
-	GPIO106,
-	GPIO107,
-	GPIO108,
-	GPIO109,
-	GPIO110, /* 110 */
-	GPIO111,
-	GPIO112,
-	GPIO113,
-	GPIO114,
-	GPIO115,
-	GPIO116,
-	GPIO117,
-	GPIO118,
-	GPIO119,
-	GPIO120, /* 120 */
-	GPIO121,
-	GPIO122,
-	GPIO123,
-	GPIO124,
-	GPIO125,
-	GPIO126,
-	GPIO127,
-	GPIO128,
-	GPIO129,
-	GPIO130, /* 130 */
-	GPIO131,
-	GPIO132,
-	GPIO133,
-	GPIO134,
-	GPIO135,
-	GPIO136,
-	GPIO137,
-	GPIO138,
-	GPIO139,
-	GPIO140, /* 140 */
-	GPIO141,
-	GPIO142,
-	GPIO143,
-	GPIO144,
-	GPIO145,
-	GPIO146,
-	GPIO147,
-	GPIO148,
-	GPIO149,
-	GPIO150, /* 150 */
-	GPIO151,
-	GPIO152,
-	GPIO153,
-	GPIO154,
-	GPIO155,
-	GPIO156,
-	GPIO157,
-	GPIO158,
-	GPIO159,
-	GPIO160, /* 160 */
-	GPIO161,
-	GPIO162,
-	GPIO163,
-	GPIO164,
-	GPIO165,
-	GPIO166,
-	GPIO167,
-	GPIO168,
-	GPIO169,
-};
-
-extern int pxa3xx_pinctrl_register(struct platform_device *pdev,
-				   struct pxa3xx_pinmux_info *info);
-extern int pxa3xx_pinctrl_unregister(struct platform_device *pdev);
-#endif	/* __PINCTRL_PXA3XX_H */
diff --git a/drivers/pinctrl/pinctrl-pxa910.c b/drivers/pinctrl/pinctrl-pxa910.c
deleted file mode 100644
index a2f917b847f..00000000000
--- a/drivers/pinctrl/pinctrl-pxa910.c
+++ /dev/null
@@ -1,1007 +0,0 @@
-/*
- *  linux/drivers/pinctrl/pinmux-pxa910.c
- *
- *  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
- *  publishhed by the Free Software Foundation.
- *
- *  Copyright (C) 2011, Marvell Technology Group Ltd.
- *
- *  Author: Haojian Zhuang <haojian.zhuang@marvell.com>
- *
- */
-
-#include <linux/device.h>
-#include <linux/module.h>
-#include <linux/io.h>
-#include <linux/platform_device.h>
-#include "pinctrl-pxa3xx.h"
-
-#define PXA910_DS_MASK		0x1800
-#define PXA910_DS_SHIFT		11
-#define PXA910_SLEEP_MASK	0x38
-#define PXA910_SLEEP_SELECT	(1 << 9)
-#define PXA910_SLEEP_DATA	(1 << 8)
-#define PXA910_SLEEP_DIR	(1 << 7)
-
-#define MFPR_910(a, r, f0, f1, f2, f3, f4, f5, f6, f7)		\
-	{							\
-		.name = #a,					\
-		.pin = a,					\
-		.mfpr = r,					\
-		.func = {					\
-			PXA910_MUX_##f0,			\
-			PXA910_MUX_##f1,			\
-			PXA910_MUX_##f2,			\
-			PXA910_MUX_##f3,			\
-			PXA910_MUX_##f4,			\
-			PXA910_MUX_##f5,			\
-			PXA910_MUX_##f6,			\
-			PXA910_MUX_##f7,			\
-		},						\
-	}
-
-#define GRP_910(a, m, p)		\
-	{ .name = a, .mux = PXA910_MUX_##m, .pins = p, .npins = ARRAY_SIZE(p), }
-
-/* 170 pins */
-enum pxa910_pin_list {
-	/* 0~127: GPIO0~GPIO127 */
-	ND_IO15 = 128,
-	ND_IO14,
-	ND_IO13, /* 130 */
-	ND_IO12,
-	ND_IO11,
-	ND_IO10,
-	ND_IO9,
-	ND_IO8,
-	ND_IO7,
-	ND_IO6,
-	ND_IO5,
-	ND_IO4,
-	ND_IO3, /* 140 */
-	ND_IO2,
-	ND_IO1,
-	ND_IO0,
-	ND_NCS0,
-	ND_NCS1,
-	SM_NCS0,
-	SM_NCS1,
-	ND_NWE,
-	ND_NRE,
-	ND_CLE, /* 150 */
-	ND_ALE,
-	SM_SCLK,
-	ND_RDY0,
-	SM_ADV,
-	ND_RDY1,
-	SM_ADVMUX,
-	SM_RDY,
-	MMC1_DAT7,
-	MMC1_DAT6,
-	MMC1_DAT5, /* 160 */
-	MMC1_DAT4,
-	MMC1_DAT3,
-	MMC1_DAT2,
-	MMC1_DAT1,
-	MMC1_DAT0,
-	MMC1_CMD,
-	MMC1_CLK,
-	MMC1_CD,
-	VCXO_OUT,
-};
-
-enum pxa910_mux {
-	/* PXA3xx_MUX_GPIO = 0 (predefined in pinctrl-pxa3xx.h) */
-	PXA910_MUX_GPIO = 0,
-	PXA910_MUX_NAND,
-	PXA910_MUX_USIM2,
-	PXA910_MUX_EXT_DMA,
-	PXA910_MUX_EXT_INT,
-	PXA910_MUX_MMC1,
-	PXA910_MUX_MMC2,
-	PXA910_MUX_MMC3,
-	PXA910_MUX_SM_INT,
-	PXA910_MUX_PRI_JTAG,
-	PXA910_MUX_SEC1_JTAG,
-	PXA910_MUX_SEC2_JTAG,
-	PXA910_MUX_RESET,	/* SLAVE RESET OUT */
-	PXA910_MUX_CLK_REQ,
-	PXA910_MUX_VCXO_REQ,
-	PXA910_MUX_VCXO_OUT,
-	PXA910_MUX_VCXO_REQ2,
-	PXA910_MUX_VCXO_OUT2,
-	PXA910_MUX_SPI,
-	PXA910_MUX_SPI2,
-	PXA910_MUX_GSSP,
-	PXA910_MUX_SSP0,
-	PXA910_MUX_SSP1,
-	PXA910_MUX_SSP2,
-	PXA910_MUX_DSSP2,
-	PXA910_MUX_DSSP3,
-	PXA910_MUX_UART0,
-	PXA910_MUX_UART1,
-	PXA910_MUX_UART2,
-	PXA910_MUX_TWSI,
-	PXA910_MUX_CCIC,
-	PXA910_MUX_PWM0,
-	PXA910_MUX_PWM1,
-	PXA910_MUX_PWM2,
-	PXA910_MUX_PWM3,
-	PXA910_MUX_HSL,
-	PXA910_MUX_ONE_WIRE,
-	PXA910_MUX_LCD,
-	PXA910_MUX_DAC_ST23,
-	PXA910_MUX_ULPI,
-	PXA910_MUX_TB,
-	PXA910_MUX_KP_MK,
-	PXA910_MUX_KP_DK,
-	PXA910_MUX_TCU_GPOA,
-	PXA910_MUX_TCU_GPOB,
-	PXA910_MUX_ROT,
-	PXA910_MUX_TDS,
-	PXA910_MUX_32K_CLK, /* 32KHz CLK OUT */
-	PXA910_MUX_MN_CLK, /* MN CLK OUT */
-	PXA910_MUX_SMC,
-	PXA910_MUX_SM_ADDR18,
-	PXA910_MUX_SM_ADDR19,
-	PXA910_MUX_SM_ADDR20,
-	PXA910_MUX_NONE = 0xffff,
-};
-
-
-static struct pinctrl_pin_desc pxa910_pads[] = {
-	PINCTRL_PIN(GPIO0, "GPIO0"),
-	PINCTRL_PIN(GPIO1, "GPIO1"),
-	PINCTRL_PIN(GPIO2, "GPIO2"),
-	PINCTRL_PIN(GPIO3, "GPIO3"),
-	PINCTRL_PIN(GPIO4, "GPIO4"),
-	PINCTRL_PIN(GPIO5, "GPIO5"),
-	PINCTRL_PIN(GPIO6, "GPIO6"),
-	PINCTRL_PIN(GPIO7, "GPIO7"),
-	PINCTRL_PIN(GPIO8, "GPIO8"),
-	PINCTRL_PIN(GPIO9, "GPIO9"),
-	PINCTRL_PIN(GPIO10, "GPIO10"),
-	PINCTRL_PIN(GPIO11, "GPIO11"),
-	PINCTRL_PIN(GPIO12, "GPIO12"),
-	PINCTRL_PIN(GPIO13, "GPIO13"),
-	PINCTRL_PIN(GPIO14, "GPIO14"),
-	PINCTRL_PIN(GPIO15, "GPIO15"),
-	PINCTRL_PIN(GPIO16, "GPIO16"),
-	PINCTRL_PIN(GPIO17, "GPIO17"),
-	PINCTRL_PIN(GPIO18, "GPIO18"),
-	PINCTRL_PIN(GPIO19, "GPIO19"),
-	PINCTRL_PIN(GPIO20, "GPIO20"),
-	PINCTRL_PIN(GPIO21, "GPIO21"),
-	PINCTRL_PIN(GPIO22, "GPIO22"),
-	PINCTRL_PIN(GPIO23, "GPIO23"),
-	PINCTRL_PIN(GPIO24, "GPIO24"),
-	PINCTRL_PIN(GPIO25, "GPIO25"),
-	PINCTRL_PIN(GPIO26, "GPIO26"),
-	PINCTRL_PIN(GPIO27, "GPIO27"),
-	PINCTRL_PIN(GPIO28, "GPIO28"),
-	PINCTRL_PIN(GPIO29, "GPIO29"),
-	PINCTRL_PIN(GPIO30, "GPIO30"),
-	PINCTRL_PIN(GPIO31, "GPIO31"),
-	PINCTRL_PIN(GPIO32, "GPIO32"),
-	PINCTRL_PIN(GPIO33, "GPIO33"),
-	PINCTRL_PIN(GPIO34, "GPIO34"),
-	PINCTRL_PIN(GPIO35, "GPIO35"),
-	PINCTRL_PIN(GPIO36, "GPIO36"),
-	PINCTRL_PIN(GPIO37, "GPIO37"),
-	PINCTRL_PIN(GPIO38, "GPIO38"),
-	PINCTRL_PIN(GPIO39, "GPIO39"),
-	PINCTRL_PIN(GPIO40, "GPIO40"),
-	PINCTRL_PIN(GPIO41, "GPIO41"),
-	PINCTRL_PIN(GPIO42, "GPIO42"),
-	PINCTRL_PIN(GPIO43, "GPIO43"),
-	PINCTRL_PIN(GPIO44, "GPIO44"),
-	PINCTRL_PIN(GPIO45, "GPIO45"),
-	PINCTRL_PIN(GPIO46, "GPIO46"),
-	PINCTRL_PIN(GPIO47, "GPIO47"),
-	PINCTRL_PIN(GPIO48, "GPIO48"),
-	PINCTRL_PIN(GPIO49, "GPIO49"),
-	PINCTRL_PIN(GPIO50, "GPIO50"),
-	PINCTRL_PIN(GPIO51, "GPIO51"),
-	PINCTRL_PIN(GPIO52, "GPIO52"),
-	PINCTRL_PIN(GPIO53, "GPIO53"),
-	PINCTRL_PIN(GPIO54, "GPIO54"),
-	PINCTRL_PIN(GPIO55, "GPIO55"),
-	PINCTRL_PIN(GPIO56, "GPIO56"),
-	PINCTRL_PIN(GPIO57, "GPIO57"),
-	PINCTRL_PIN(GPIO58, "GPIO58"),
-	PINCTRL_PIN(GPIO59, "GPIO59"),
-	PINCTRL_PIN(GPIO60, "GPIO60"),
-	PINCTRL_PIN(GPIO61, "GPIO61"),
-	PINCTRL_PIN(GPIO62, "GPIO62"),
-	PINCTRL_PIN(GPIO63, "GPIO63"),
-	PINCTRL_PIN(GPIO64, "GPIO64"),
-	PINCTRL_PIN(GPIO65, "GPIO65"),
-	PINCTRL_PIN(GPIO66, "GPIO66"),
-	PINCTRL_PIN(GPIO67, "GPIO67"),
-	PINCTRL_PIN(GPIO68, "GPIO68"),
-	PINCTRL_PIN(GPIO69, "GPIO69"),
-	PINCTRL_PIN(GPIO70, "GPIO70"),
-	PINCTRL_PIN(GPIO71, "GPIO71"),
-	PINCTRL_PIN(GPIO72, "GPIO72"),
-	PINCTRL_PIN(GPIO73, "GPIO73"),
-	PINCTRL_PIN(GPIO74, "GPIO74"),
-	PINCTRL_PIN(GPIO75, "GPIO75"),
-	PINCTRL_PIN(GPIO76, "GPIO76"),
-	PINCTRL_PIN(GPIO77, "GPIO77"),
-	PINCTRL_PIN(GPIO78, "GPIO78"),
-	PINCTRL_PIN(GPIO79, "GPIO79"),
-	PINCTRL_PIN(GPIO80, "GPIO80"),
-	PINCTRL_PIN(GPIO81, "GPIO81"),
-	PINCTRL_PIN(GPIO82, "GPIO82"),
-	PINCTRL_PIN(GPIO83, "GPIO83"),
-	PINCTRL_PIN(GPIO84, "GPIO84"),
-	PINCTRL_PIN(GPIO85, "GPIO85"),
-	PINCTRL_PIN(GPIO86, "GPIO86"),
-	PINCTRL_PIN(GPIO87, "GPIO87"),
-	PINCTRL_PIN(GPIO88, "GPIO88"),
-	PINCTRL_PIN(GPIO89, "GPIO89"),
-	PINCTRL_PIN(GPIO90, "GPIO90"),
-	PINCTRL_PIN(GPIO91, "GPIO91"),
-	PINCTRL_PIN(GPIO92, "GPIO92"),
-	PINCTRL_PIN(GPIO93, "GPIO93"),
-	PINCTRL_PIN(GPIO94, "GPIO94"),
-	PINCTRL_PIN(GPIO95, "GPIO95"),
-	PINCTRL_PIN(GPIO96, "GPIO96"),
-	PINCTRL_PIN(GPIO97, "GPIO97"),
-	PINCTRL_PIN(GPIO98, "GPIO98"),
-	PINCTRL_PIN(GPIO99, "GPIO99"),
-	PINCTRL_PIN(GPIO100, "GPIO100"),
-	PINCTRL_PIN(GPIO101, "GPIO101"),
-	PINCTRL_PIN(GPIO102, "GPIO102"),
-	PINCTRL_PIN(GPIO103, "GPIO103"),
-	PINCTRL_PIN(GPIO104, "GPIO104"),
-	PINCTRL_PIN(GPIO105, "GPIO105"),
-	PINCTRL_PIN(GPIO106, "GPIO106"),
-	PINCTRL_PIN(GPIO107, "GPIO107"),
-	PINCTRL_PIN(GPIO108, "GPIO108"),
-	PINCTRL_PIN(GPIO109, "GPIO109"),
-	PINCTRL_PIN(GPIO110, "GPIO110"),
-	PINCTRL_PIN(GPIO111, "GPIO111"),
-	PINCTRL_PIN(GPIO112, "GPIO112"),
-	PINCTRL_PIN(GPIO113, "GPIO113"),
-	PINCTRL_PIN(GPIO114, "GPIO114"),
-	PINCTRL_PIN(GPIO115, "GPIO115"),
-	PINCTRL_PIN(GPIO116, "GPIO116"),
-	PINCTRL_PIN(GPIO117, "GPIO117"),
-	PINCTRL_PIN(GPIO118, "GPIO118"),
-	PINCTRL_PIN(GPIO119, "GPIO119"),
-	PINCTRL_PIN(GPIO120, "GPIO120"),
-	PINCTRL_PIN(GPIO121, "GPIO121"),
-	PINCTRL_PIN(GPIO122, "GPIO122"),
-	PINCTRL_PIN(GPIO123, "GPIO123"),
-	PINCTRL_PIN(GPIO124, "GPIO124"),
-	PINCTRL_PIN(GPIO125, "GPIO125"),
-	PINCTRL_PIN(GPIO126, "GPIO126"),
-	PINCTRL_PIN(GPIO127, "GPIO127"),
-	PINCTRL_PIN(ND_IO15, "ND_IO15"),
-	PINCTRL_PIN(ND_IO14, "ND_IO14"),
-	PINCTRL_PIN(ND_IO13, "ND_IO13"),
-	PINCTRL_PIN(ND_IO12, "ND_IO12"),
-	PINCTRL_PIN(ND_IO11, "ND_IO11"),
-	PINCTRL_PIN(ND_IO10, "ND_IO10"),
-	PINCTRL_PIN(ND_IO9, "ND_IO9"),
-	PINCTRL_PIN(ND_IO8, "ND_IO8"),
-	PINCTRL_PIN(ND_IO7, "ND_IO7"),
-	PINCTRL_PIN(ND_IO6, "ND_IO6"),
-	PINCTRL_PIN(ND_IO5, "ND_IO5"),
-	PINCTRL_PIN(ND_IO4, "ND_IO4"),
-	PINCTRL_PIN(ND_IO3, "ND_IO3"),
-	PINCTRL_PIN(ND_IO2, "ND_IO2"),
-	PINCTRL_PIN(ND_IO1, "ND_IO1"),
-	PINCTRL_PIN(ND_IO0, "ND_IO0"),
-	PINCTRL_PIN(ND_NCS0, "ND_NCS0_SM_NCS2"),
-	PINCTRL_PIN(ND_NCS1, "ND_NCS1_SM_NCS3"),
-	PINCTRL_PIN(SM_NCS0, "SM_NCS0"),
-	PINCTRL_PIN(SM_NCS1, "SM_NCS1"),
-	PINCTRL_PIN(ND_NWE, "ND_NWE"),
-	PINCTRL_PIN(ND_NRE, "ND_NRE"),
-	PINCTRL_PIN(ND_CLE, "ND_CLE_SM_NOE"),
-	PINCTRL_PIN(ND_ALE, "ND_ALE_SM_NWE"),
-	PINCTRL_PIN(SM_SCLK, "SM_SCLK"),
-	PINCTRL_PIN(ND_RDY0, "ND_RDY0"),
-	PINCTRL_PIN(SM_ADV, "SM_ADV"),
-	PINCTRL_PIN(ND_RDY1, "ND_RDY1"),
-	PINCTRL_PIN(SM_RDY, "SM_RDY"),
-	PINCTRL_PIN(MMC1_DAT7, "MMC1_DAT7"),
-	PINCTRL_PIN(MMC1_DAT6, "MMC1_DAT6"),
-	PINCTRL_PIN(MMC1_DAT5, "MMC1_DAT5"),
-	PINCTRL_PIN(MMC1_DAT4, "MMC1_DAT4"),
-	PINCTRL_PIN(MMC1_DAT3, "MMC1_DAT3"),
-	PINCTRL_PIN(MMC1_DAT2, "MMC1_DAT2"),
-	PINCTRL_PIN(MMC1_DAT1, "MMC1_DAT1"),
-	PINCTRL_PIN(MMC1_DAT0, "MMC1_DAT0"),
-	PINCTRL_PIN(MMC1_CMD, "MMC1 CMD"),
-	PINCTRL_PIN(MMC1_CLK, "MMC1 CLK"),
-	PINCTRL_PIN(MMC1_CD, "MMC1 CD"),
-	PINCTRL_PIN(VCXO_OUT, "VCXO_OUT"),
-};
-
-struct pxa3xx_mfp_pin pxa910_mfp[] = {
-	/*       pin        offs   f0        f1      f2         f3         f4         f5        f6        f7  */
-	MFPR_910(GPIO0,     0x0DC, GPIO,     KP_MK,  NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO1,     0x0E0, GPIO,     KP_MK,  NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO2,     0x0E4, GPIO,     KP_MK,  NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO3,     0x0E8, GPIO,     KP_MK,  NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO4,     0x0EC, GPIO,     KP_MK,  NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO5,     0x0F0, GPIO,     KP_MK,  NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO6,     0x0F4, GPIO,     KP_MK,  NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO7,     0x0F8, GPIO,     KP_MK,  NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO8,     0x0FC, GPIO,     KP_MK,  NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO9,     0x100, GPIO,     KP_MK,  NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO10,    0x104, GPIO,     KP_MK,  NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO11,    0x108, GPIO,     KP_MK,  NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO12,    0x10C, GPIO,     KP_MK,  NONE,      NONE,      KP_DK,     NONE,     NONE,     NONE),
-	MFPR_910(GPIO13,    0x110, GPIO,     KP_MK,  NONE,      NONE,      KP_DK,     NONE,     NONE,     NONE),
-	MFPR_910(GPIO14,    0x114, GPIO,     KP_MK,  NONE,      NONE,      KP_DK,     TB,       NONE,     NONE),
-	MFPR_910(GPIO15,    0x118, GPIO,     KP_MK,  NONE,      NONE,      KP_DK,     TB,       NONE,     NONE),
-	MFPR_910(GPIO16,    0x11C, GPIO,     KP_DK,  NONE,      NONE,      NONE,      TB,       NONE,     NONE),
-	MFPR_910(GPIO17,    0x120, GPIO,     KP_DK,  NONE,      NONE,      NONE,      TB,       NONE,     NONE),
-	MFPR_910(GPIO18,    0x124, GPIO,     KP_DK,  NONE,      NONE,      ROT,       NONE,     NONE,     NONE),
-	MFPR_910(GPIO19,    0x128, GPIO,     KP_DK,  NONE,      NONE,      ROT,       NONE,     NONE,     NONE),
-	MFPR_910(GPIO20,    0x12C, GPIO,     SSP1,   NONE,      NONE,      VCXO_OUT,  NONE,     NONE,     NONE),
-	MFPR_910(GPIO21,    0x130, GPIO,     SSP1,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO22,    0x134, GPIO,     SSP1,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO23,    0x138, GPIO,     SSP1,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO24,    0x13C, GPIO,     SSP1,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO25,    0x140, GPIO,     GSSP,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO26,    0x144, GPIO,     GSSP,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO27,    0x148, GPIO,     GSSP,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO28,    0x14C, GPIO,     GSSP,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO29,    0x150, GPIO,     UART0,  NONE,      NONE,      UART1,     NONE,     NONE,     NONE),
-	MFPR_910(GPIO30,    0x154, GPIO,     UART0,  NONE,      NONE,      UART1,     NONE,     NONE,     NONE),
-	MFPR_910(GPIO31,    0x158, GPIO,     UART0,  NONE,      NONE,      UART1,     NONE,     NONE,     NONE),
-	MFPR_910(GPIO32,    0x15C, GPIO,     UART0,  DAC_ST23,  NONE,      UART1,     NONE,     NONE,     NONE),
-	MFPR_910(GPIO33,    0x160, GPIO,     MMC2,   SSP0,      SSP2,      NONE,      SPI,      NONE,     MMC3),
-	MFPR_910(GPIO34,    0x164, GPIO,     MMC2,   SSP0,      SSP2,      NONE,      SPI,      NONE,     MMC3),
-	MFPR_910(GPIO35,    0x168, GPIO,     MMC2,   SSP0,      SSP2,      NONE,      SPI,      NONE,     MMC3),
-	MFPR_910(GPIO36,    0x16C, GPIO,     MMC2,   SSP0,      SSP2,      NONE,      SPI,      NONE,     MMC3),
-	MFPR_910(GPIO37,    0x170, GPIO,     MMC2,   NONE,      NONE,      NONE,      SPI,      HSL,      NONE),
-	MFPR_910(GPIO38,    0x174, GPIO,     MMC2,   NONE,      NONE,      NONE,      NONE,     HSL,      NONE),
-	MFPR_910(GPIO39,    0x178, GPIO,     MMC2,   NONE,      NONE,      NONE,      NONE,     HSL,      NONE),
-	MFPR_910(GPIO40,    0x17C, GPIO,     MMC2,   NONE,      NONE,      NONE,      NONE,     HSL,      NONE),
-	MFPR_910(GPIO41,    0x180, GPIO,     MMC2,   NONE,      NONE,      NONE,      NONE,     HSL,      NONE),
-	MFPR_910(GPIO42,    0x184, GPIO,     MMC2,   NONE,      NONE,      NONE,      NONE,     HSL,      NONE),
-	MFPR_910(GPIO43,    0x188, GPIO,     UART1,  NONE,      DAC_ST23,  NONE,      DSSP2,    SPI,      UART2),
-	MFPR_910(GPIO44,    0x18C, GPIO,     UART1,  NONE,      EXT_INT,   NONE,      DSSP2,    SPI,      UART2),
-	MFPR_910(GPIO45,    0x190, GPIO,     UART1,  NONE,      EXT_INT,   NONE,      DSSP2,    SPI,      UART2),
-	MFPR_910(GPIO46,    0x194, GPIO,     UART1,  NONE,      EXT_INT,   NONE,      DSSP2,    SPI,      UART2),
-	MFPR_910(GPIO47,    0x198, GPIO,     SSP0,   NONE,      NONE,      NONE,      SSP2,     UART1,    NONE),
-	MFPR_910(GPIO48,    0x19C, GPIO,     SSP0,   NONE,      NONE,      NONE,      SSP2,     UART1,    NONE),
-	MFPR_910(GPIO49,    0x1A0, GPIO,     SSP0,   UART0,     VCXO_REQ,  NONE,      SSP2,     NONE,     MMC3),
-	MFPR_910(GPIO50,    0x1A4, GPIO,     SSP0,   UART0,     VCXO_OUT,  NONE,      SSP2,     NONE,     MMC3),
-	MFPR_910(GPIO51,    0x1A8, GPIO,     UART2,  PWM1,      TWSI,      SSP0,      NONE,     DSSP3,    NONE),
-	MFPR_910(GPIO52,    0x1AC, GPIO,     UART2,  DAC_ST23,  TWSI,      SSP0,      NONE,     DSSP3,    NONE),
-	MFPR_910(GPIO53,    0x1B0, GPIO,     UART2,  TWSI,      NONE,      SSP0,      NONE,     DSSP3,    NONE),
-	MFPR_910(GPIO54,    0x1B4, GPIO,     UART2,  TWSI,      SSP0,      NONE,      NONE,     DSSP3,    NONE),
-	MFPR_910(GPIO55,    0x2F0, TDS,      GPIO,   TB,        NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO56,    0x2F4, TDS,      GPIO,   TB,        NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO57,    0x2F8, TDS,      GPIO,   TB,        NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO58,    0x2FC, TDS,      GPIO,   TB,        NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO59,    0x300, TDS,      GPIO,   TCU_GPOA,  TCU_GPOB,  ONE_WIRE,  NONE,     NONE,     NONE),
-	MFPR_910(GPIO60,    0x304, GPIO,     NONE,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO61,    0x308, GPIO,     NONE,   NONE,      NONE,      NONE,      NONE,     NONE,     HSL),
-	MFPR_910(GPIO62,    0x30C, GPIO,     NONE,   NONE,      NONE,      NONE,      NONE,     NONE,     HSL),
-	MFPR_910(GPIO63,    0x310, GPIO,     NONE,   NONE,      NONE,      NONE,      NONE,     NONE,     HSL),
-	MFPR_910(GPIO64,    0x314, GPIO,     SPI2,   NONE,      NONE,      NONE,      NONE,     NONE,     HSL),
-	MFPR_910(GPIO65,    0x318, GPIO,     SPI2,   NONE,      NONE,      NONE,      NONE,     ONE_WIRE, HSL),
-	MFPR_910(GPIO66,    0x31C, GPIO,     NONE,   NONE,      NONE,      NONE,      NONE,     NONE,     HSL),
-	MFPR_910(GPIO67,    0x1B8, GPIO,     CCIC,   SPI,       NONE,      NONE,      ULPI,     NONE,     USIM2),
-	MFPR_910(GPIO68,    0x1BC, GPIO,     CCIC,   SPI,       NONE,      NONE,      ULPI,     NONE,     USIM2),
-	MFPR_910(GPIO69,    0x1C0, GPIO,     CCIC,   SPI,       NONE,      NONE,      ULPI,     NONE,     USIM2),
-	MFPR_910(GPIO70,    0x1C4, GPIO,     CCIC,   SPI,       NONE,      NONE,      ULPI,     NONE,     NONE),
-	MFPR_910(GPIO71,    0x1C8, GPIO,     CCIC,   SPI,       NONE,      NONE,      ULPI,     NONE,     NONE),
-	MFPR_910(GPIO72,    0x1CC, GPIO,     CCIC,   EXT_DMA,   NONE,      NONE,      ULPI,     NONE,     NONE),
-	MFPR_910(GPIO73,    0x1D0, GPIO,     CCIC,   EXT_DMA,   NONE,      NONE,      ULPI,     NONE,     NONE),
-	MFPR_910(GPIO74,    0x1D4, GPIO,     CCIC,   EXT_DMA,   NONE,      NONE,      ULPI,     NONE,     NONE),
-	MFPR_910(GPIO75,    0x1D8, GPIO,     CCIC,   NONE,      NONE,      NONE,      ULPI,     NONE,     NONE),
-	MFPR_910(GPIO76,    0x1DC, GPIO,     CCIC,   NONE,      NONE,      NONE,      ULPI,     NONE,     NONE),
-	MFPR_910(GPIO77,    0x1E0, GPIO,     CCIC,   NONE,      NONE,      NONE,      ULPI,     NONE,     NONE),
-	MFPR_910(GPIO78,    0x1E4, GPIO,     CCIC,   NONE,      NONE,      NONE,      ULPI,     NONE,     NONE),
-	MFPR_910(GPIO79,    0x1E8, GPIO,     TWSI,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO80,    0x1EC, GPIO,     TWSI,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO81,    0x1F0, GPIO,     LCD,    NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO82,    0x1F4, GPIO,     LCD,    NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO83,    0x1F8, GPIO,     LCD,    NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO84,    0x1FC, GPIO,     LCD,    VCXO_REQ2, NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO85,    0x200, GPIO,     LCD,    NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO86,    0x204, GPIO,     LCD,    VCXO_OUT2, NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO87,    0x208, GPIO,     LCD,    NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO88,    0x20C, GPIO,     LCD,    NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO89,    0x210, GPIO,     LCD,    NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO90,    0x214, GPIO,     LCD,    NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO91,    0x218, GPIO,     LCD,    NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO92,    0x21C, GPIO,     LCD,    NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO93,    0x220, GPIO,     LCD,    NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO94,    0x224, GPIO,     LCD,    NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO95,    0x228, GPIO,     LCD,    NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO96,    0x22C, GPIO,     LCD,    NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO97,    0x230, GPIO,     LCD,    NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO98,    0x234, GPIO,     LCD,    NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO99,    0x0B0, MMC1,     GPIO,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO100,   0x238, GPIO,     LCD,    NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO101,   0x23C, GPIO,     LCD,    NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO102,   0x240, GPIO,     LCD,    DSSP2,     SPI,       NONE,      NONE,     NONE,     SPI2),
-	MFPR_910(GPIO103,   0x244, GPIO,     LCD,    DSSP2,     SPI,       NONE,      NONE,     NONE,     SPI2),
-	MFPR_910(GPIO104,   0x248, GPIO,     LCD,    DSSP2,     SPI,       NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO105,   0x24C, GPIO,     LCD,    DSSP2,     SPI,       NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO106,   0x250, GPIO,     LCD,    DSSP3,     ONE_WIRE,  NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO107,   0x254, GPIO,     LCD,    DSSP3,     SPI,       NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO108,   0x258, GPIO,     LCD,    DSSP3,     SPI,       NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO109,   0x25C, GPIO,     LCD,    DSSP3,     SPI,       NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO110,   0x298, GPIO,     NONE,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO111,   0x29C, GPIO,     NONE,   DSSP2,     NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO112,   0x2A0, GPIO,     NONE,   DSSP2,     NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO113,   0x2A4, GPIO,     NONE,   DSSP2,     NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO114,   0x2A8, GPIO,     NONE,   DSSP3,     NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO115,   0x2AC, GPIO,     NONE,   DSSP3,     NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO116,   0x2B0, GPIO,     NONE,   DSSP3,     NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO117,   0x0B4, PRI_JTAG, GPIO,   PWM0,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO118,   0x0B8, PRI_JTAG, GPIO,   PWM1,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO119,   0x0BC, PRI_JTAG, GPIO,   PWM2,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO120,   0x0C0, PRI_JTAG, GPIO,   PWM3,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO121,   0x32C, GPIO,     NONE,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO122,   0x0C8, RESET,    GPIO,   32K_CLK,   NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO123,   0x0CC, CLK_REQ,  GPIO,   ONE_WIRE,  EXT_DMA,   NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO124,   0x0D0, GPIO,     MN_CLK, DAC_ST23,  NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO125,   0x0D4, VCXO_REQ, GPIO,   NONE,      EXT_INT,   NONE,      NONE,     NONE,     NONE),
-	MFPR_910(GPIO126,   0x06C, GPIO,     SMC,    NONE,      SM_ADDR18, NONE,      EXT_DMA,  NONE,     NONE),
-	MFPR_910(GPIO127,   0x070, GPIO,     SMC,    NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(ND_IO15,   0x004, NAND,     GPIO,   USIM2,     EXT_DMA,   NONE,      NONE,     NONE,     NONE),
-	MFPR_910(ND_IO14,   0x008, NAND,     GPIO,   USIM2,     NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(ND_IO13,   0x00C, NAND,     GPIO,   USIM2,     EXT_INT,   NONE,      NONE,     NONE,     NONE),
-	MFPR_910(ND_IO12,   0x010, NAND,     GPIO,   SSP2,      EXT_INT,   NONE,      NONE,     NONE,     NONE),
-	MFPR_910(ND_IO11,   0x014, NAND,     GPIO,   SSP2,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(ND_IO10,   0x018, NAND,     GPIO,   SSP2,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(ND_IO9,    0x01C, NAND,     GPIO,   SSP2,      NONE,      VCXO_OUT2, NONE,     NONE,     NONE),
-	MFPR_910(ND_IO8,    0x020, NAND,     GPIO,   NONE,      NONE,      PWM3,      NONE,     NONE,     NONE),
-	MFPR_910(ND_IO7,    0x024, NAND,     MMC3,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(ND_IO6,    0x028, NAND,     MMC3,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(ND_IO5,    0x02C, NAND,     MMC3,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(ND_IO4,    0x030, NAND,     MMC3,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(ND_IO3,    0x034, NAND,     MMC3,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(ND_IO2,    0x038, NAND,     MMC3,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(ND_IO1,    0x03C, NAND,     MMC3,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(ND_IO0,    0x040, NAND,     MMC3,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(ND_NCS0,   0x044, NAND,     GPIO,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(ND_NCS1,   0x048, NAND,     GPIO,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(SM_NCS0,   0x04C, SMC,      GPIO,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(SM_NCS1,   0x050, SMC,      GPIO,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(ND_NWE,    0x054, GPIO,     NAND,   NONE,      SM_ADDR20, NONE,      SMC,      NONE,     NONE),
-	MFPR_910(ND_NRE,    0x058, GPIO,     NAND,   NONE,      SMC,       NONE,      EXT_DMA,  NONE,     NONE),
-	MFPR_910(ND_CLE,    0x05C, NAND,     MMC3,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(ND_ALE,    0x060, GPIO,     NAND,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(SM_SCLK,   0x064, MMC3,     NONE,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(ND_RDY0,   0x068, NAND,     GPIO,   NONE,      SMC,       NONE,      NONE,     NONE,     NONE),
-	MFPR_910(SM_ADV,    0x074, SMC,      GPIO,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(ND_RDY1,   0x078, NAND,     GPIO,   NONE,      SMC,       NONE,      NONE,     NONE,     NONE),
-	MFPR_910(SM_ADVMUX, 0x07C, SMC,      GPIO,   NONE,      SM_ADDR19, NONE,      NONE,     NONE,     NONE),
-	MFPR_910(SM_RDY,    0x080, SMC,      GPIO,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(MMC1_DAT7, 0x084, MMC1,     GPIO,   SEC1_JTAG, TB,        NONE,      NONE,     NONE,     NONE),
-	MFPR_910(MMC1_DAT6, 0x088, MMC1,     GPIO,   SEC1_JTAG, TB,        NONE,      NONE,     NONE,     NONE),
-	MFPR_910(MMC1_DAT5, 0x08C, MMC1,     GPIO,   SEC1_JTAG, TB,        NONE,      NONE,     NONE,     NONE),
-	MFPR_910(MMC1_DAT4, 0x090, MMC1,     GPIO,   NONE,      TB,        NONE,      NONE,     NONE,     NONE),
-	MFPR_910(MMC1_DAT3, 0x094, MMC1,     HSL,    SEC2_JTAG, SSP0,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(MMC1_DAT2, 0x098, MMC1,     HSL,    SEC2_JTAG, SSP2,      SSP0,      NONE,     NONE,     NONE),
-	MFPR_910(MMC1_DAT1, 0x09C, MMC1,     HSL,    SEC2_JTAG, SSP2,      SSP0,      NONE,     NONE,     NONE),
-	MFPR_910(MMC1_DAT0, 0x0A0, MMC1,     HSL,    SEC2_JTAG, SSP2,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(MMC1_CMD,  0x0A4, MMC1,     HSL,    SEC1_JTAG, SSP2,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(MMC1_CLK,  0x0A8, MMC1,     HSL,    SEC2_JTAG, SSP0,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(MMC1_CD,   0x0AC, MMC1,     GPIO,   SEC1_JTAG, NONE,      NONE,      NONE,     NONE,     NONE),
-	MFPR_910(VCXO_OUT,  0x0D8, VCXO_OUT, PWM3,   NONE,      NONE,      NONE,      NONE,     NONE,     NONE),
-};
-
-
-static const unsigned p910_usim2_pin1[] = {GPIO67, GPIO68, GPIO69};
-static const unsigned p910_usim2_pin2[] = {ND_IO15, ND_IO14, ND_IO13};
-static const unsigned p910_mmc1_pin1[] = {MMC1_DAT7, MMC1_DAT6, MMC1_DAT5,
-	MMC1_DAT4, MMC1_DAT3, MMC1_DAT2, MMC1_DAT1, MMC1_DAT0, MMC1_CMD,
-	MMC1_CLK, MMC1_CD, GPIO99};
-static const unsigned p910_mmc2_pin1[] = {GPIO33, GPIO34, GPIO35, GPIO36,
-	GPIO37, GPIO38, GPIO39, GPIO40, GPIO41, GPIO42};
-static const unsigned p910_mmc3_pin1[] = {GPIO33, GPIO34, GPIO35, GPIO36,
-	GPIO49, GPIO50};
-static const unsigned p910_mmc3_pin2[] = {ND_IO7, ND_IO6, ND_IO5, ND_IO4,
-	ND_IO3, ND_IO2, ND_IO1, ND_IO0, ND_CLE, SM_SCLK};
-static const unsigned p910_uart0_pin1[] = {GPIO29, GPIO30, GPIO31, GPIO32};
-static const unsigned p910_uart1_pin1[] = {GPIO47, GPIO48};
-static const unsigned p910_uart1_pin2[] = {GPIO31, GPIO32};
-static const unsigned p910_uart1_pin3[] = {GPIO45, GPIO46};
-static const unsigned p910_uart1_pin4[] = {GPIO29, GPIO30, GPIO31, GPIO32};
-static const unsigned p910_uart1_pin5[] = {GPIO43, GPIO44, GPIO45, GPIO46};
-static const unsigned p910_uart2_pin1[] = {GPIO43, GPIO44};
-static const unsigned p910_uart2_pin2[] = {GPIO51, GPIO52};
-static const unsigned p910_uart2_pin3[] = {GPIO43, GPIO44, GPIO45, GPIO46};
-static const unsigned p910_uart2_pin4[] = {GPIO51, GPIO52, GPIO53, GPIO54};
-static const unsigned p910_twsi_pin1[] = {GPIO51, GPIO52};
-static const unsigned p910_twsi_pin2[] = {GPIO53, GPIO54};
-static const unsigned p910_twsi_pin3[] = {GPIO79, GPIO80};
-static const unsigned p910_ccic_pin1[] = {GPIO67, GPIO68, GPIO69, GPIO70,
-	GPIO71, GPIO72, GPIO73, GPIO74, GPIO75, GPIO76, GPIO77, GPIO78};
-static const unsigned p910_lcd_pin1[] = {GPIO81, GPIO82, GPIO83, GPIO84,
-	GPIO85, GPIO86, GPIO87, GPIO88, GPIO89, GPIO90, GPIO91, GPIO92,
-	GPIO93, GPIO94, GPIO95, GPIO96, GPIO97, GPIO98, GPIO100, GPIO101,
-	GPIO102, GPIO103};
-static const unsigned p910_spi_pin1[] = {GPIO104, GPIO105, GPIO107, GPIO108};
-static const unsigned p910_spi_pin2[] = {GPIO43, GPIO44, GPIO45, GPIO46};
-static const unsigned p910_spi_pin3[] = {GPIO33, GPIO34, GPIO35, GPIO36,
-	GPIO37};
-static const unsigned p910_spi_pin4[] = {GPIO67, GPIO68, GPIO69, GPIO70,
-	GPIO71};
-static const unsigned p910_spi2_pin1[] = {GPIO64, GPIO65};
-static const unsigned p910_spi2_pin2[] = {GPIO102, GPIO103};
-static const unsigned p910_dssp2_pin1[] = {GPIO102, GPIO103, GPIO104, GPIO105};
-static const unsigned p910_dssp2_pin2[] = {GPIO43, GPIO44, GPIO45, GPIO46};
-static const unsigned p910_dssp2_pin3[] = {GPIO111, GPIO112, GPIO113};
-static const unsigned p910_dssp3_pin1[] = {GPIO106, GPIO107, GPIO108, GPIO109};
-static const unsigned p910_dssp3_pin2[] = {GPIO51, GPIO52, GPIO53, GPIO54};
-static const unsigned p910_dssp3_pin3[] = {GPIO114, GPIO115, GPIO116};
-static const unsigned p910_ssp0_pin1[] = {MMC1_DAT3, MMC1_DAT2, MMC1_DAT1,
-	MMC1_CLK};
-static const unsigned p910_ssp0_pin2[] = {GPIO33, GPIO34, GPIO35, GPIO36};
-static const unsigned p910_ssp0_pin3[] = {GPIO47, GPIO48, GPIO49, GPIO50};
-static const unsigned p910_ssp0_pin4[] = {GPIO51, GPIO52, GPIO53, GPIO54};
-static const unsigned p910_ssp1_pin1[] = {GPIO21, GPIO22, GPIO23, GPIO24};
-static const unsigned p910_ssp1_pin2[] = {GPIO20, GPIO21, GPIO22, GPIO23,
-	GPIO24};
-static const unsigned p910_ssp2_pin1[] = {MMC1_DAT2, MMC1_DAT1, MMC1_DAT0,
-	MMC1_CMD};
-static const unsigned p910_ssp2_pin2[] = {GPIO33, GPIO34, GPIO35, GPIO36};
-static const unsigned p910_ssp2_pin3[] = {GPIO47, GPIO48, GPIO49, GPIO50};
-static const unsigned p910_ssp2_pin4[] = {ND_IO12, ND_IO11, ND_IO10, ND_IO9};
-static const unsigned p910_gssp_pin1[] = {GPIO25, GPIO26, GPIO27, GPIO28};
-static const unsigned p910_pwm0_pin1[] = {GPIO117};
-static const unsigned p910_pwm1_pin1[] = {GPIO118};
-static const unsigned p910_pwm1_pin2[] = {GPIO51};
-static const unsigned p910_pwm2_pin1[] = {GPIO119};
-static const unsigned p910_pwm3_pin1[] = {GPIO120};
-static const unsigned p910_pwm3_pin2[] = {ND_IO8};
-static const unsigned p910_pwm3_pin3[] = {VCXO_OUT};
-static const unsigned p910_pri_jtag_pin1[] = {GPIO117, GPIO118, GPIO119,
-	GPIO120};
-static const unsigned p910_sec1_jtag_pin1[] = {MMC1_DAT7, MMC1_DAT6, MMC1_DAT5,
-	MMC1_CMD, MMC1_CD};
-static const unsigned p910_sec2_jtag_pin1[] = {MMC1_DAT3, MMC1_DAT2, MMC1_DAT1,
-	MMC1_DAT0, MMC1_CLK};
-static const unsigned p910_hsl_pin1[] = {GPIO37, GPIO38, GPIO39, GPIO40,
-	GPIO41, GPIO42};
-static const unsigned p910_hsl_pin2[] = {GPIO61, GPIO62, GPIO63, GPIO64,
-	GPIO65, GPIO66};
-static const unsigned p910_hsl_pin3[] = {MMC1_DAT3, MMC1_DAT2, MMC1_DAT1,
-	MMC1_DAT0, MMC1_CMD, MMC1_CLK};
-static const unsigned p910_w1_pin1[] = {GPIO59};
-static const unsigned p910_w1_pin2[] = {GPIO65};
-static const unsigned p910_w1_pin3[] = {GPIO106};
-static const unsigned p910_w1_pin4[] = {GPIO123};
-static const unsigned p910_kpmk_pin1[] = {GPIO0, GPIO1, GPIO2, GPIO3, GPIO4,
-	GPIO5, GPIO6, GPIO7, GPIO8, GPIO9, GPIO10, GPIO11, GPIO12, GPIO13,
-	GPIO14, GPIO15};
-static const unsigned p910_kpmk_pin2[] = {GPIO0, GPIO1, GPIO2, GPIO3, GPIO4,
-	GPIO5, GPIO6, GPIO7, GPIO8, GPIO9, GPIO12};
-static const unsigned p910_kpdk_pin1[] = {GPIO12, GPIO13, GPIO14, GPIO15,
-	GPIO16, GPIO17, GPIO18, GPIO19};
-static const unsigned p910_tds_pin1[] = {GPIO55, GPIO56, GPIO57, GPIO58,
-	GPIO59};
-static const unsigned p910_tds_pin2[] = {GPIO55, GPIO57, GPIO58, GPIO59};
-static const unsigned p910_tb_pin1[] = {GPIO14, GPIO15, GPIO16, GPIO17};
-static const unsigned p910_tb_pin2[] = {GPIO55, GPIO56, GPIO57, GPIO58};
-static const unsigned p910_tb_pin3[] = {MMC1_DAT7, MMC1_DAT6, MMC1_DAT5,
-	MMC1_DAT4};
-static const unsigned p910_ext_dma0_pin1[] = {GPIO72};
-static const unsigned p910_ext_dma0_pin2[] = {ND_IO15};
-static const unsigned p910_ext_dma0_pin3[] = {ND_NRE};
-static const unsigned p910_ext_dma1_pin1[] = {GPIO73};
-static const unsigned p910_ext_dma1_pin2[] = {GPIO123};
-static const unsigned p910_ext_dma1_pin3[] = {GPIO126};
-static const unsigned p910_ext_dma2_pin1[] = {GPIO74};
-static const unsigned p910_ext0_int_pin1[] = {GPIO44};
-static const unsigned p910_ext0_int_pin2[] = {ND_IO13};
-static const unsigned p910_ext1_int_pin1[] = {GPIO45};
-static const unsigned p910_ext1_int_pin2[] = {ND_IO12};
-static const unsigned p910_ext2_int_pin1[] = {GPIO46};
-static const unsigned p910_ext2_int_pin2[] = {GPIO125};
-static const unsigned p910_dac_st23_pin1[] = {GPIO32};
-static const unsigned p910_dac_st23_pin2[] = {GPIO43};
-static const unsigned p910_dac_st23_pin3[] = {GPIO52};
-static const unsigned p910_dac_st23_pin4[] = {GPIO124};
-static const unsigned p910_vcxo_out_pin1[] = {GPIO50};
-static const unsigned p910_vcxo_out_pin2[] = {VCXO_OUT};
-static const unsigned p910_vcxo_out_pin3[] = {GPIO20};
-static const unsigned p910_vcxo_req_pin1[] = {GPIO49};
-static const unsigned p910_vcxo_req_pin2[] = {GPIO125};
-static const unsigned p910_vcxo_out2_pin1[] = {GPIO86};
-static const unsigned p910_vcxo_out2_pin2[] = {ND_IO9};
-static const unsigned p910_vcxo_req2_pin1[] = {GPIO84};
-static const unsigned p910_ulpi_pin1[] = {GPIO67, GPIO68, GPIO69, GPIO70,
-	GPIO71, GPIO72, GPIO73, GPIO74, GPIO75, GPIO76, GPIO77, GPIO78};
-static const unsigned p910_nand_pin1[] = {ND_IO15, ND_IO14, ND_IO13, ND_IO12,
-	ND_IO11, ND_IO10, ND_IO9, ND_IO8, ND_IO7, ND_IO6, ND_IO5, ND_IO4,
-	ND_IO3, ND_IO2, ND_IO1, ND_IO0, ND_NCS0, ND_NWE, ND_NRE, ND_CLE,
-	ND_ALE, ND_RDY0};
-static const unsigned p910_gpio0_pin1[] = {GPIO0};
-static const unsigned p910_gpio0_pin2[] = {SM_ADV};
-static const unsigned p910_gpio1_pin1[] = {GPIO1};
-static const unsigned p910_gpio1_pin2[] = {ND_RDY1};
-static const unsigned p910_gpio2_pin1[] = {GPIO2};
-static const unsigned p910_gpio2_pin2[] = {SM_ADVMUX};
-static const unsigned p910_gpio3_pin1[] = {GPIO3};
-static const unsigned p910_gpio3_pin2[] = {SM_RDY};
-static const unsigned p910_gpio20_pin1[] = {GPIO20};
-static const unsigned p910_gpio20_pin2[] = {ND_IO15};
-static const unsigned p910_gpio20_pin3[] = {MMC1_DAT6};
-static const unsigned p910_gpio21_pin1[] = {GPIO21};
-static const unsigned p910_gpio21_pin2[] = {ND_IO14};
-static const unsigned p910_gpio21_pin3[] = {MMC1_DAT5};
-static const unsigned p910_gpio22_pin1[] = {GPIO22};
-static const unsigned p910_gpio22_pin2[] = {ND_IO13};
-static const unsigned p910_gpio22_pin3[] = {MMC1_DAT4};
-static const unsigned p910_gpio23_pin1[] = {GPIO23};
-static const unsigned p910_gpio23_pin2[] = {ND_IO12};
-static const unsigned p910_gpio23_pin3[] = {MMC1_CD};
-static const unsigned p910_gpio24_pin1[] = {GPIO24};
-static const unsigned p910_gpio24_pin2[] = {ND_IO11};
-static const unsigned p910_gpio24_pin3[] = {MMC1_DAT7};
-static const unsigned p910_gpio25_pin1[] = {GPIO25};
-static const unsigned p910_gpio25_pin2[] = {ND_IO10};
-static const unsigned p910_gpio26_pin1[] = {GPIO26};
-static const unsigned p910_gpio26_pin2[] = {ND_IO9};
-static const unsigned p910_gpio27_pin1[] = {GPIO27};
-static const unsigned p910_gpio27_pin2[] = {ND_IO8};
-static const unsigned p910_gpio85_pin1[] = {GPIO85};
-static const unsigned p910_gpio85_pin2[] = {ND_NCS0};
-static const unsigned p910_gpio86_pin1[] = {GPIO86};
-static const unsigned p910_gpio86_pin2[] = {ND_NCS1};
-static const unsigned p910_gpio87_pin1[] = {GPIO87};
-static const unsigned p910_gpio87_pin2[] = {SM_NCS0};
-static const unsigned p910_gpio88_pin1[] = {GPIO88};
-static const unsigned p910_gpio88_pin2[] = {SM_NCS1};
-static const unsigned p910_gpio89_pin1[] = {GPIO89};
-static const unsigned p910_gpio89_pin2[] = {ND_NWE};
-static const unsigned p910_gpio90_pin1[] = {GPIO90};
-static const unsigned p910_gpio90_pin2[] = {ND_NRE};
-static const unsigned p910_gpio91_pin1[] = {GPIO91};
-static const unsigned p910_gpio91_pin2[] = {ND_ALE};
-static const unsigned p910_gpio92_pin1[] = {GPIO92};
-static const unsigned p910_gpio92_pin2[] = {ND_RDY0};
-
-static struct pxa3xx_pin_group pxa910_grps[] = {
-	GRP_910("usim2 3p1", USIM2, p910_usim2_pin1),
-	GRP_910("usim2 3p2", USIM2, p910_usim2_pin2),
-	GRP_910("mmc1 12p", MMC1, p910_mmc1_pin1),
-	GRP_910("mmc2 10p", MMC2, p910_mmc2_pin1),
-	GRP_910("mmc3 6p", MMC3, p910_mmc3_pin1),
-	GRP_910("mmc3 10p", MMC3, p910_mmc3_pin2),
-	GRP_910("uart0 4p", UART0, p910_uart0_pin1),
-	GRP_910("uart1 2p1", UART1, p910_uart1_pin1),
-	GRP_910("uart1 2p2", UART1, p910_uart1_pin2),
-	GRP_910("uart1 2p3", UART1, p910_uart1_pin3),
-	GRP_910("uart1 4p4", UART1, p910_uart1_pin4),
-	GRP_910("uart1 4p5", UART1, p910_uart1_pin5),
-	GRP_910("uart2 2p1", UART2, p910_uart2_pin1),
-	GRP_910("uart2 2p2", UART2, p910_uart2_pin2),
-	GRP_910("uart2 4p3", UART2, p910_uart2_pin3),
-	GRP_910("uart2 4p4", UART2, p910_uart2_pin4),
-	GRP_910("twsi 2p1", TWSI, p910_twsi_pin1),
-	GRP_910("twsi 2p2", TWSI, p910_twsi_pin2),
-	GRP_910("twsi 2p3", TWSI, p910_twsi_pin3),
-	GRP_910("ccic", CCIC, p910_ccic_pin1),
-	GRP_910("lcd", LCD, p910_lcd_pin1),
-	GRP_910("spi 4p1", SPI, p910_spi_pin1),
-	GRP_910("spi 4p2", SPI, p910_spi_pin2),
-	GRP_910("spi 5p3", SPI, p910_spi_pin3),
-	GRP_910("spi 5p4", SPI, p910_spi_pin4),
-	GRP_910("dssp2 4p1", DSSP2, p910_dssp2_pin1),
-	GRP_910("dssp2 4p2", DSSP2, p910_dssp2_pin2),
-	GRP_910("dssp2 3p3", DSSP2, p910_dssp2_pin3),
-	GRP_910("dssp3 4p1", DSSP3, p910_dssp3_pin1),
-	GRP_910("dssp3 4p2", DSSP3, p910_dssp3_pin2),
-	GRP_910("dssp3 3p3", DSSP3, p910_dssp3_pin3),
-	GRP_910("ssp0 4p1", SSP0, p910_ssp0_pin1),
-	GRP_910("ssp0 4p2", SSP0, p910_ssp0_pin2),
-	GRP_910("ssp0 4p3", SSP0, p910_ssp0_pin3),
-	GRP_910("ssp0 4p4", SSP0, p910_ssp0_pin4),
-	GRP_910("ssp1 4p1", SSP1, p910_ssp1_pin1),
-	GRP_910("ssp1 5p2", SSP1, p910_ssp1_pin2),
-	GRP_910("ssp2 4p1", SSP2, p910_ssp2_pin1),
-	GRP_910("ssp2 4p2", SSP2, p910_ssp2_pin2),
-	GRP_910("ssp2 4p3", SSP2, p910_ssp2_pin3),
-	GRP_910("ssp2 4p4", SSP2, p910_ssp2_pin4),
-	GRP_910("gssp", GSSP, p910_gssp_pin1),
-	GRP_910("pwm0", PWM0, p910_pwm0_pin1),
-	GRP_910("pwm1-1", PWM1, p910_pwm1_pin1),
-	GRP_910("pwm1-2", PWM1, p910_pwm1_pin2),
-	GRP_910("pwm2", PWM2, p910_pwm2_pin1),
-	GRP_910("pwm3-1", PWM3, p910_pwm3_pin1),
-	GRP_910("pwm3-2", PWM3, p910_pwm3_pin2),
-	GRP_910("pwm3-3", PWM3, p910_pwm3_pin3),
-	GRP_910("pri jtag", PRI_JTAG, p910_pri_jtag_pin1),
-	GRP_910("sec1 jtag", SEC1_JTAG, p910_sec1_jtag_pin1),
-	GRP_910("sec2 jtag", SEC2_JTAG, p910_sec2_jtag_pin1),
-	GRP_910("hsl 6p1", HSL, p910_hsl_pin1),
-	GRP_910("hsl 6p2", HSL, p910_hsl_pin2),
-	GRP_910("hsl 6p3", HSL, p910_hsl_pin3),
-	GRP_910("w1-1", ONE_WIRE, p910_w1_pin1),
-	GRP_910("w1-2", ONE_WIRE, p910_w1_pin2),
-	GRP_910("w1-3", ONE_WIRE, p910_w1_pin3),
-	GRP_910("w1-4", ONE_WIRE, p910_w1_pin4),
-	GRP_910("kpmk 16p1", KP_MK, p910_kpmk_pin1),
-	GRP_910("kpmk 11p2", KP_MK, p910_kpmk_pin2),
-	GRP_910("kpdk 8p1", KP_DK, p910_kpdk_pin1),
-	GRP_910("tds 5p1", TDS, p910_tds_pin1),
-	GRP_910("tds 4p2", TDS, p910_tds_pin2),
-	GRP_910("tb 4p1", TB, p910_tb_pin1),
-	GRP_910("tb 4p2", TB, p910_tb_pin2),
-	GRP_910("tb 4p3", TB, p910_tb_pin3),
-	GRP_910("ext dma0-1", EXT_DMA, p910_ext_dma0_pin1),
-	GRP_910("ext dma0-2", EXT_DMA, p910_ext_dma0_pin2),
-	GRP_910("ext dma0-3", EXT_DMA, p910_ext_dma0_pin3),
-	GRP_910("ext dma1-1", EXT_DMA, p910_ext_dma1_pin1),
-	GRP_910("ext dma1-2", EXT_DMA, p910_ext_dma1_pin2),
-	GRP_910("ext dma1-3", EXT_DMA, p910_ext_dma1_pin3),
-	GRP_910("ext dma2", EXT_DMA, p910_ext_dma2_pin1),
-	GRP_910("ext0 int-1", EXT_INT, p910_ext0_int_pin1),
-	GRP_910("ext0 int-2", EXT_INT, p910_ext0_int_pin2),
-	GRP_910("ext1 int-1", EXT_INT, p910_ext1_int_pin1),
-	GRP_910("ext1 int-2", EXT_INT, p910_ext1_int_pin2),
-	GRP_910("ext2 int-1", EXT_INT, p910_ext2_int_pin1),
-	GRP_910("ext2 int-2", EXT_INT, p910_ext2_int_pin2),
-	GRP_910("dac st23-1", DAC_ST23, p910_dac_st23_pin1),
-	GRP_910("dac st23-2", DAC_ST23, p910_dac_st23_pin2),
-	GRP_910("dac st23-3", DAC_ST23, p910_dac_st23_pin3),
-	GRP_910("dac st23-4", DAC_ST23, p910_dac_st23_pin4),
-	GRP_910("vcxo out-1", VCXO_OUT, p910_vcxo_out_pin1),
-	GRP_910("vcxo out-2", VCXO_OUT, p910_vcxo_out_pin2),
-	GRP_910("vcxo out-3", VCXO_OUT, p910_vcxo_out_pin3),
-	GRP_910("vcxo req-1", VCXO_REQ, p910_vcxo_req_pin1),
-	GRP_910("vcxo req-2", VCXO_REQ, p910_vcxo_req_pin2),
-	GRP_910("vcxo out2-1", VCXO_OUT2, p910_vcxo_out2_pin1),
-	GRP_910("vcxo out2-2", VCXO_OUT2, p910_vcxo_out2_pin2),
-	GRP_910("vcxo req2", VCXO_REQ2, p910_vcxo_req2_pin1),
-	GRP_910("ulpi", ULPI, p910_ulpi_pin1),
-	GRP_910("nand", NAND, p910_nand_pin1),
-	GRP_910("gpio0-1", GPIO, p910_gpio0_pin1),
-	GRP_910("gpio0-2", GPIO, p910_gpio0_pin2),
-	GRP_910("gpio1-1", GPIO, p910_gpio1_pin1),
-	GRP_910("gpio1-2", GPIO, p910_gpio1_pin2),
-	GRP_910("gpio2-1", GPIO, p910_gpio2_pin1),
-	GRP_910("gpio2-2", GPIO, p910_gpio2_pin2),
-	GRP_910("gpio3-1", GPIO, p910_gpio3_pin1),
-	GRP_910("gpio3-2", GPIO, p910_gpio3_pin2),
-	GRP_910("gpio20-1", GPIO, p910_gpio20_pin1),
-	GRP_910("gpio20-2", GPIO, p910_gpio20_pin2),
-	GRP_910("gpio21-1", GPIO, p910_gpio21_pin1),
-	GRP_910("gpio21-2", GPIO, p910_gpio21_pin2),
-	GRP_910("gpio22-1", GPIO, p910_gpio22_pin1),
-	GRP_910("gpio22-2", GPIO, p910_gpio22_pin2),
-	GRP_910("gpio23-1", GPIO, p910_gpio23_pin1),
-	GRP_910("gpio23-2", GPIO, p910_gpio23_pin2),
-	GRP_910("gpio24-1", GPIO, p910_gpio24_pin1),
-	GRP_910("gpio24-2", GPIO, p910_gpio24_pin2),
-	GRP_910("gpio25-1", GPIO, p910_gpio25_pin1),
-	GRP_910("gpio25-2", GPIO, p910_gpio25_pin2),
-	GRP_910("gpio26-1", GPIO, p910_gpio26_pin1),
-	GRP_910("gpio26-2", GPIO, p910_gpio26_pin2),
-	GRP_910("gpio27-1", GPIO, p910_gpio27_pin1),
-	GRP_910("gpio27-2", GPIO, p910_gpio27_pin2),
-	GRP_910("gpio85-1", GPIO, p910_gpio85_pin1),
-	GRP_910("gpio85-2", GPIO, p910_gpio85_pin2),
-	GRP_910("gpio86-1", GPIO, p910_gpio86_pin1),
-	GRP_910("gpio86-2", GPIO, p910_gpio86_pin2),
-	GRP_910("gpio87-1", GPIO, p910_gpio87_pin1),
-	GRP_910("gpio87-2", GPIO, p910_gpio87_pin2),
-	GRP_910("gpio88-1", GPIO, p910_gpio88_pin1),
-	GRP_910("gpio88-2", GPIO, p910_gpio88_pin2),
-	GRP_910("gpio89-1", GPIO, p910_gpio89_pin1),
-	GRP_910("gpio89-2", GPIO, p910_gpio89_pin2),
-	GRP_910("gpio90-1", GPIO, p910_gpio90_pin1),
-	GRP_910("gpio90-2", GPIO, p910_gpio90_pin2),
-	GRP_910("gpio91-1", GPIO, p910_gpio91_pin1),
-	GRP_910("gpio91-2", GPIO, p910_gpio91_pin2),
-	GRP_910("gpio92-1", GPIO, p910_gpio92_pin1),
-	GRP_910("gpio92-2", GPIO, p910_gpio92_pin2),
-};
-
-static const char * const p910_usim2_grps[] = {"usim2 3p1", "usim2 3p2"};
-static const char * const p910_mmc1_grps[] = {"mmc1 12p"};
-static const char * const p910_mmc2_grps[] = {"mmc2 10p"};
-static const char * const p910_mmc3_grps[] = {"mmc3 6p", "mmc3 10p"};
-static const char * const p910_uart0_grps[] = {"uart0 4p"};
-static const char * const p910_uart1_grps[] = {"uart1 2p1", "uart1 2p2",
-	"uart1 2p3", "uart1 4p4", "uart1 4p5"};
-static const char * const p910_uart2_grps[] = {"uart2 2p1", "uart2 2p2",
-	"uart2 4p3", "uart2 4p4"};
-static const char * const p910_twsi_grps[] = {"twsi 2p1", "twsi 2p2",
-	"twsi 2p3"};
-static const char * const p910_ccic_grps[] = {"ccic"};
-static const char * const p910_lcd_grps[] = {"lcd"};
-static const char * const p910_spi_grps[] = {"spi 4p1", "spi 4p2", "spi 5p3",
-	"spi 5p4"};
-static const char * const p910_dssp2_grps[] = {"dssp2 4p1", "dssp2 4p2",
-	"dssp2 3p3"};
-static const char * const p910_dssp3_grps[] = {"dssp3 4p1", "dssp3 4p2",
-	"dssp3 3p3"};
-static const char * const p910_ssp0_grps[] = {"ssp0 4p1", "ssp0 4p2",
-	"ssp0 4p3", "ssp0 4p4"};
-static const char * const p910_ssp1_grps[] = {"ssp1 4p1", "ssp1 5p2"};
-static const char * const p910_ssp2_grps[] = {"ssp2 4p1", "ssp2 4p2",
-	"ssp2 4p3", "ssp2 4p4"};
-static const char * const p910_gssp_grps[] = {"gssp"};
-static const char * const p910_pwm0_grps[] = {"pwm0"};
-static const char * const p910_pwm1_grps[] = {"pwm1-1", "pwm1-2"};
-static const char * const p910_pwm2_grps[] = {"pwm2"};
-static const char * const p910_pwm3_grps[] = {"pwm3-1", "pwm3-2", "pwm3-3"};
-static const char * const p910_pri_jtag_grps[] = {"pri jtag"};
-static const char * const p910_sec1_jtag_grps[] = {"sec1 jtag"};
-static const char * const p910_sec2_jtag_grps[] = {"sec2 jtag"};
-static const char * const p910_hsl_grps[] = {"hsl 6p1", "hsl 6p2", "hsl 6p3"};
-static const char * const p910_w1_grps[] = {"w1-1", "w1-2", "w1-3", "w1-4"};
-static const char * const p910_kpmk_grps[] = {"kpmk 16p1", "kpmk 11p2"};
-static const char * const p910_kpdk_grps[] = {"kpdk 8p1"};
-static const char * const p910_tds_grps[] = {"tds 5p1", "tds 4p2"};
-static const char * const p910_tb_grps[] = {"tb 4p1", "tb 4p2", "tb 4p3"};
-static const char * const p910_dma0_grps[] = {"ext dma0-1", "ext dma0-2",
-	"ext dma0-3"};
-static const char * const p910_dma1_grps[] = {"ext dma1-1", "ext dma1-2",
-	"ext dma1-3"};
-static const char * const p910_dma2_grps[] = {"ext dma2"};
-static const char * const p910_int0_grps[] = {"ext0 int-1", "ext0 int-2"};
-static const char * const p910_int1_grps[] = {"ext1 int-1", "ext1 int-2"};
-static const char * const p910_int2_grps[] = {"ext2 int-1", "ext2 int-2"};
-static const char * const p910_dac_st23_grps[] = {"dac st23-1", "dac st23-2",
-	"dac st23-3", "dac st23-4"};
-static const char * const p910_vcxo_out_grps[] = {"vcxo out-1", "vcxo out-2",
-	"vcxo out-3"};
-static const char * const p910_vcxo_req_grps[] = {"vcxo req-1", "vcxo req-2"};
-static const char * const p910_vcxo_out2_grps[] = {"vcxo out2-1",
-	"vcxo out2-2"};
-static const char * const p910_vcxo_req2_grps[] = {"vcxo req2"};
-static const char * const p910_ulpi_grps[] = {"ulpi"};
-static const char * const p910_nand_grps[] = {"nand"};
-static const char * const p910_gpio0_grps[] = {"gpio0-1", "gpio0-2"};
-static const char * const p910_gpio1_grps[] = {"gpio1-1", "gpio1-2"};
-static const char * const p910_gpio2_grps[] = {"gpio2-1", "gpio2-2"};
-static const char * const p910_gpio3_grps[] = {"gpio3-1", "gpio3-2"};
-static const char * const p910_gpio20_grps[] = {"gpio20-1", "gpio20-2"};
-static const char * const p910_gpio21_grps[] = {"gpio21-1", "gpio21-2"};
-static const char * const p910_gpio22_grps[] = {"gpio22-1", "gpio22-2"};
-static const char * const p910_gpio23_grps[] = {"gpio23-1", "gpio23-2"};
-static const char * const p910_gpio24_grps[] = {"gpio24-1", "gpio24-2"};
-static const char * const p910_gpio25_grps[] = {"gpio25-1", "gpio25-2"};
-static const char * const p910_gpio26_grps[] = {"gpio26-1", "gpio26-2"};
-static const char * const p910_gpio27_grps[] = {"gpio27-1", "gpio27-2"};
-static const char * const p910_gpio85_grps[] = {"gpio85-1", "gpio85-2"};
-static const char * const p910_gpio86_grps[] = {"gpio86-1", "gpio86-2"};
-static const char * const p910_gpio87_grps[] = {"gpio87-1", "gpio87-2"};
-static const char * const p910_gpio88_grps[] = {"gpio88-1", "gpio88-2"};
-static const char * const p910_gpio89_grps[] = {"gpio89-1", "gpio89-2"};
-static const char * const p910_gpio90_grps[] = {"gpio90-1", "gpio90-2"};
-static const char * const p910_gpio91_grps[] = {"gpio91-1", "gpio91-2"};
-static const char * const p910_gpio92_grps[] = {"gpio92-1", "gpio92-2"};
-
-static struct pxa3xx_pmx_func pxa910_funcs[] = {
-	{"usim2",	ARRAY_AND_SIZE(p910_usim2_grps)},
-	{"mmc1",	ARRAY_AND_SIZE(p910_mmc1_grps)},
-	{"mmc2",	ARRAY_AND_SIZE(p910_mmc2_grps)},
-	{"mmc3",	ARRAY_AND_SIZE(p910_mmc3_grps)},
-	{"uart0",	ARRAY_AND_SIZE(p910_uart0_grps)},
-	{"uart1",	ARRAY_AND_SIZE(p910_uart1_grps)},
-	{"uart2",	ARRAY_AND_SIZE(p910_uart2_grps)},
-	{"twsi",	ARRAY_AND_SIZE(p910_twsi_grps)},
-	{"ccic",	ARRAY_AND_SIZE(p910_ccic_grps)},
-	{"lcd",		ARRAY_AND_SIZE(p910_lcd_grps)},
-	{"spi",		ARRAY_AND_SIZE(p910_spi_grps)},
-	{"dssp2",	ARRAY_AND_SIZE(p910_dssp2_grps)},
-	{"dssp3",	ARRAY_AND_SIZE(p910_dssp3_grps)},
-	{"ssp0",	ARRAY_AND_SIZE(p910_ssp0_grps)},
-	{"ssp1",	ARRAY_AND_SIZE(p910_ssp1_grps)},
-	{"ssp2",	ARRAY_AND_SIZE(p910_ssp2_grps)},
-	{"gssp",	ARRAY_AND_SIZE(p910_gssp_grps)},
-	{"pwm0",	ARRAY_AND_SIZE(p910_pwm0_grps)},
-	{"pwm1",	ARRAY_AND_SIZE(p910_pwm1_grps)},
-	{"pwm2",	ARRAY_AND_SIZE(p910_pwm2_grps)},
-	{"pwm3",	ARRAY_AND_SIZE(p910_pwm3_grps)},
-	{"pri_jtag",	ARRAY_AND_SIZE(p910_pri_jtag_grps)},
-	{"sec1_jtag",	ARRAY_AND_SIZE(p910_sec1_jtag_grps)},
-	{"sec2_jtag",	ARRAY_AND_SIZE(p910_sec2_jtag_grps)},
-	{"hsl",		ARRAY_AND_SIZE(p910_hsl_grps)},
-	{"w1",		ARRAY_AND_SIZE(p910_w1_grps)},
-	{"kpmk",	ARRAY_AND_SIZE(p910_kpmk_grps)},
-	{"kpdk",	ARRAY_AND_SIZE(p910_kpdk_grps)},
-	{"tds",		ARRAY_AND_SIZE(p910_tds_grps)},
-	{"tb",		ARRAY_AND_SIZE(p910_tb_grps)},
-	{"dma0",	ARRAY_AND_SIZE(p910_dma0_grps)},
-	{"dma1",	ARRAY_AND_SIZE(p910_dma1_grps)},
-	{"dma2",	ARRAY_AND_SIZE(p910_dma2_grps)},
-	{"int0",	ARRAY_AND_SIZE(p910_int0_grps)},
-	{"int1",	ARRAY_AND_SIZE(p910_int1_grps)},
-	{"int2",	ARRAY_AND_SIZE(p910_int2_grps)},
-	{"dac_st23",	ARRAY_AND_SIZE(p910_dac_st23_grps)},
-	{"vcxo_out",	ARRAY_AND_SIZE(p910_vcxo_out_grps)},
-	{"vcxo_req",	ARRAY_AND_SIZE(p910_vcxo_req_grps)},
-	{"vcxo_out2",	ARRAY_AND_SIZE(p910_vcxo_out2_grps)},
-	{"vcxo_req2",	ARRAY_AND_SIZE(p910_vcxo_req2_grps)},
-	{"ulpi",	ARRAY_AND_SIZE(p910_ulpi_grps)},
-	{"nand",	ARRAY_AND_SIZE(p910_nand_grps)},
-	{"gpio0",	ARRAY_AND_SIZE(p910_gpio0_grps)},
-	{"gpio1",	ARRAY_AND_SIZE(p910_gpio1_grps)},
-	{"gpio2",	ARRAY_AND_SIZE(p910_gpio2_grps)},
-	{"gpio3",	ARRAY_AND_SIZE(p910_gpio3_grps)},
-	{"gpio20",	ARRAY_AND_SIZE(p910_gpio20_grps)},
-	{"gpio21",	ARRAY_AND_SIZE(p910_gpio21_grps)},
-	{"gpio22",	ARRAY_AND_SIZE(p910_gpio22_grps)},
-	{"gpio23",	ARRAY_AND_SIZE(p910_gpio23_grps)},
-	{"gpio24",	ARRAY_AND_SIZE(p910_gpio24_grps)},
-	{"gpio25",	ARRAY_AND_SIZE(p910_gpio25_grps)},
-	{"gpio26",	ARRAY_AND_SIZE(p910_gpio26_grps)},
-	{"gpio27",	ARRAY_AND_SIZE(p910_gpio27_grps)},
-	{"gpio85",	ARRAY_AND_SIZE(p910_gpio85_grps)},
-	{"gpio86",	ARRAY_AND_SIZE(p910_gpio86_grps)},
-	{"gpio87",	ARRAY_AND_SIZE(p910_gpio87_grps)},
-	{"gpio88",	ARRAY_AND_SIZE(p910_gpio88_grps)},
-	{"gpio89",	ARRAY_AND_SIZE(p910_gpio89_grps)},
-	{"gpio90",	ARRAY_AND_SIZE(p910_gpio90_grps)},
-	{"gpio91",	ARRAY_AND_SIZE(p910_gpio91_grps)},
-	{"gpio92",	ARRAY_AND_SIZE(p910_gpio92_grps)},
-};
-
-static struct pinctrl_desc pxa910_pctrl_desc = {
-	.name		= "pxa910-pinctrl",
-	.owner		= THIS_MODULE,
-};
-
-static struct pxa3xx_pinmux_info pxa910_info = {
-	.mfp		= pxa910_mfp,
-	.num_mfp	= ARRAY_SIZE(pxa910_mfp),
-	.grps		= pxa910_grps,
-	.num_grps	= ARRAY_SIZE(pxa910_grps),
-	.funcs		= pxa910_funcs,
-	.num_funcs	= ARRAY_SIZE(pxa910_funcs),
-	.num_gpio	= 128,
-	.desc		= &pxa910_pctrl_desc,
-	.pads		= pxa910_pads,
-	.num_pads	= ARRAY_SIZE(pxa910_pads),
-
-	.cputype	= PINCTRL_PXA910,
-	.ds_mask	= PXA910_DS_MASK,
-	.ds_shift	= PXA910_DS_SHIFT,
-};
-
-static int pxa910_pinmux_probe(struct platform_device *pdev)
-{
-	return pxa3xx_pinctrl_register(pdev, &pxa910_info);
-}
-
-static int pxa910_pinmux_remove(struct platform_device *pdev)
-{
-	return pxa3xx_pinctrl_unregister(pdev);
-}
-
-static struct platform_driver pxa910_pinmux_driver = {
-	.driver = {
-		.name	= "pxa910-pinmux",
-		.owner	= THIS_MODULE,
-	},
-	.probe	= pxa910_pinmux_probe,
-	.remove	= pxa910_pinmux_remove,
-};
-
-static int __init pxa910_pinmux_init(void)
-{
-	return platform_driver_register(&pxa910_pinmux_driver);
-}
-core_initcall_sync(pxa910_pinmux_init);
-
-static void __exit pxa910_pinmux_exit(void)
-{
-	platform_driver_unregister(&pxa910_pinmux_driver);
-}
-module_exit(pxa910_pinmux_exit);
-
-MODULE_AUTHOR("Haojian Zhuang <haojian.zhuang@marvell.com>");
-MODULE_DESCRIPTION("PXA3xx pin control driver");
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/pinctrl-s3c64xx.c b/drivers/pinctrl/pinctrl-s3c64xx.c
new file mode 100644
index 00000000000..89143c90300
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-s3c64xx.c
@@ -0,0 +1,816 @@
+/*
+ * S3C64xx specific support for pinctrl-samsung driver.
+ *
+ * Copyright (c) 2013 Tomasz Figa <tomasz.figa@gmail.com>
+ *
+ * Based on pinctrl-exynos.c, please see the file for original copyrights.
+ *
+ * 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 file contains the Samsung S3C64xx specific information required by the
+ * the Samsung pinctrl/gpiolib driver. It also includes the implementation of
+ * external gpio and wakeup interrupt support.
+ */
+
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/irqdomain.h>
+#include <linux/irq.h>
+#include <linux/of_irq.h>
+#include <linux/io.h>
+#include <linux/irqchip/chained_irq.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+
+#include "pinctrl-samsung.h"
+
+#define NUM_EINT0		28
+#define NUM_EINT0_IRQ		4
+#define EINT_MAX_PER_REG	16
+#define EINT_MAX_PER_GROUP	16
+
+/* External GPIO and wakeup interrupt related definitions */
+#define SVC_GROUP_SHIFT		4
+#define SVC_GROUP_MASK		0xf
+#define SVC_NUM_MASK		0xf
+#define SVC_GROUP(x)		((x >> SVC_GROUP_SHIFT) & \
+						SVC_GROUP_MASK)
+
+#define EINT12CON_REG		0x200
+#define EINT12MASK_REG		0x240
+#define EINT12PEND_REG		0x260
+
+#define EINT_OFFS(i)		((i) % (2 * EINT_MAX_PER_GROUP))
+#define EINT_GROUP(i)		((i) / EINT_MAX_PER_GROUP)
+#define EINT_REG(g)		(4 * ((g) / 2))
+
+#define EINTCON_REG(i)		(EINT12CON_REG + EINT_REG(EINT_GROUP(i)))
+#define EINTMASK_REG(i)		(EINT12MASK_REG + EINT_REG(EINT_GROUP(i)))
+#define EINTPEND_REG(i)		(EINT12PEND_REG + EINT_REG(EINT_GROUP(i)))
+
+#define SERVICE_REG		0x284
+#define SERVICEPEND_REG		0x288
+
+#define EINT0CON0_REG		0x900
+#define EINT0MASK_REG		0x920
+#define EINT0PEND_REG		0x924
+
+/* S3C64xx specific external interrupt trigger types */
+#define EINT_LEVEL_LOW		0
+#define EINT_LEVEL_HIGH		1
+#define EINT_EDGE_FALLING	2
+#define EINT_EDGE_RISING	4
+#define EINT_EDGE_BOTH		6
+#define EINT_CON_MASK		0xF
+#define EINT_CON_LEN		4
+
+static struct samsung_pin_bank_type bank_type_4bit_off = {
+	.fld_width = { 4, 1, 2, 0, 2, 2, },
+	.reg_offset = { 0x00, 0x04, 0x08, 0, 0x0c, 0x10, },
+};
+
+static struct samsung_pin_bank_type bank_type_4bit_alive = {
+	.fld_width = { 4, 1, 2, },
+	.reg_offset = { 0x00, 0x04, 0x08, },
+};
+
+static struct samsung_pin_bank_type bank_type_4bit2_off = {
+	.fld_width = { 4, 1, 2, 0, 2, 2, },
+	.reg_offset = { 0x00, 0x08, 0x0c, 0, 0x10, 0x14, },
+};
+
+static struct samsung_pin_bank_type bank_type_4bit2_alive = {
+	.fld_width = { 4, 1, 2, },
+	.reg_offset = { 0x00, 0x08, 0x0c, },
+};
+
+static struct samsung_pin_bank_type bank_type_2bit_off = {
+	.fld_width = { 2, 1, 2, 0, 2, 2, },
+	.reg_offset = { 0x00, 0x04, 0x08, 0, 0x0c, 0x10, },
+};
+
+static struct samsung_pin_bank_type bank_type_2bit_alive = {
+	.fld_width = { 2, 1, 2, },
+	.reg_offset = { 0x00, 0x04, 0x08, },
+};
+
+#define PIN_BANK_4BIT(pins, reg, id)			\
+	{						\
+		.type		= &bank_type_4bit_off,	\
+		.pctl_offset	= reg,			\
+		.nr_pins	= pins,			\
+		.eint_type	= EINT_TYPE_NONE,	\
+		.name		= id			\
+	}
+
+#define PIN_BANK_4BIT_EINTG(pins, reg, id, eoffs)	\
+	{						\
+		.type		= &bank_type_4bit_off,	\
+		.pctl_offset	= reg,			\
+		.nr_pins	= pins,			\
+		.eint_type	= EINT_TYPE_GPIO,	\
+		.eint_func	= 7,			\
+		.eint_mask	= (1 << (pins)) - 1,	\
+		.eint_offset	= eoffs,		\
+		.name		= id			\
+	}
+
+#define PIN_BANK_4BIT_EINTW(pins, reg, id, eoffs, emask) \
+	{						\
+		.type		= &bank_type_4bit_alive,\
+		.pctl_offset	= reg,			\
+		.nr_pins	= pins,			\
+		.eint_type	= EINT_TYPE_WKUP,	\
+		.eint_func	= 3,			\
+		.eint_mask	= emask,		\
+		.eint_offset	= eoffs,		\
+		.name		= id			\
+	}
+
+#define PIN_BANK_4BIT2_EINTG(pins, reg, id, eoffs)	\
+	{						\
+		.type		= &bank_type_4bit2_off,	\
+		.pctl_offset	= reg,			\
+		.nr_pins	= pins,			\
+		.eint_type	= EINT_TYPE_GPIO,	\
+		.eint_func	= 7,			\
+		.eint_mask	= (1 << (pins)) - 1,	\
+		.eint_offset	= eoffs,		\
+		.name		= id			\
+	}
+
+#define PIN_BANK_4BIT2_EINTW(pins, reg, id, eoffs, emask) \
+	{						\
+		.type		= &bank_type_4bit2_alive,\
+		.pctl_offset	= reg,			\
+		.nr_pins	= pins,			\
+		.eint_type	= EINT_TYPE_WKUP,	\
+		.eint_func	= 3,			\
+		.eint_mask	= emask,		\
+		.eint_offset	= eoffs,		\
+		.name		= id			\
+	}
+
+#define PIN_BANK_4BIT2_ALIVE(pins, reg, id)		\
+	{						\
+		.type		= &bank_type_4bit2_alive,\
+		.pctl_offset	= reg,			\
+		.nr_pins	= pins,			\
+		.eint_type	= EINT_TYPE_NONE,	\
+		.name		= id			\
+	}
+
+#define PIN_BANK_2BIT(pins, reg, id)			\
+	{						\
+		.type		= &bank_type_2bit_off,	\
+		.pctl_offset	= reg,			\
+		.nr_pins	= pins,			\
+		.eint_type	= EINT_TYPE_NONE,	\
+		.name		= id			\
+	}
+
+#define PIN_BANK_2BIT_EINTG(pins, reg, id, eoffs, emask) \
+	{						\
+		.type		= &bank_type_2bit_off,	\
+		.pctl_offset	= reg,			\
+		.nr_pins	= pins,			\
+		.eint_type	= EINT_TYPE_GPIO,	\
+		.eint_func	= 3,			\
+		.eint_mask	= emask,		\
+		.eint_offset	= eoffs,		\
+		.name		= id			\
+	}
+
+#define PIN_BANK_2BIT_EINTW(pins, reg, id, eoffs)	\
+	{						\
+		.type		= &bank_type_2bit_alive,\
+		.pctl_offset	= reg,			\
+		.nr_pins	= pins,			\
+		.eint_type	= EINT_TYPE_WKUP,	\
+		.eint_func	= 2,			\
+		.eint_mask	= (1 << (pins)) - 1,	\
+		.eint_offset	= eoffs,		\
+		.name		= id			\
+	}
+
+/**
+ * struct s3c64xx_eint0_data: EINT0 common data
+ * @drvdata: pin controller driver data
+ * @domains: IRQ domains of particular EINT0 interrupts
+ * @pins: pin offsets inside of banks of particular EINT0 interrupts
+ */
+struct s3c64xx_eint0_data {
+	struct samsung_pinctrl_drv_data *drvdata;
+	struct irq_domain *domains[NUM_EINT0];
+	u8 pins[NUM_EINT0];
+};
+
+/**
+ * struct s3c64xx_eint0_domain_data: EINT0 per-domain data
+ * @bank: pin bank related to the domain
+ * @eints: EINT0 interrupts related to the domain
+ */
+struct s3c64xx_eint0_domain_data {
+	struct samsung_pin_bank *bank;
+	u8 eints[];
+};
+
+/**
+ * struct s3c64xx_eint_gpio_data: GPIO EINT data
+ * @drvdata: pin controller driver data
+ * @domains: array of domains related to EINT interrupt groups
+ */
+struct s3c64xx_eint_gpio_data {
+	struct samsung_pinctrl_drv_data *drvdata;
+	struct irq_domain *domains[];
+};
+
+/*
+ * Common functions for S3C64xx EINT configuration
+ */
+
+static int s3c64xx_irq_get_trigger(unsigned int type)
+{
+	int trigger;
+
+	switch (type) {
+	case IRQ_TYPE_EDGE_RISING:
+		trigger = EINT_EDGE_RISING;
+		break;
+	case IRQ_TYPE_EDGE_FALLING:
+		trigger = EINT_EDGE_FALLING;
+		break;
+	case IRQ_TYPE_EDGE_BOTH:
+		trigger = EINT_EDGE_BOTH;
+		break;
+	case IRQ_TYPE_LEVEL_HIGH:
+		trigger = EINT_LEVEL_HIGH;
+		break;
+	case IRQ_TYPE_LEVEL_LOW:
+		trigger = EINT_LEVEL_LOW;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return trigger;
+}
+
+static void s3c64xx_irq_set_handler(unsigned int irq, unsigned int type)
+{
+	/* Edge- and level-triggered interrupts need different handlers */
+	if (type & IRQ_TYPE_EDGE_BOTH)
+		__irq_set_handler_locked(irq, handle_edge_irq);
+	else
+		__irq_set_handler_locked(irq, handle_level_irq);
+}
+
+static void s3c64xx_irq_set_function(struct samsung_pinctrl_drv_data *d,
+					struct samsung_pin_bank *bank, int pin)
+{
+	struct samsung_pin_bank_type *bank_type = bank->type;
+	unsigned long flags;
+	void __iomem *reg;
+	u8 shift;
+	u32 mask;
+	u32 val;
+
+	/* Make sure that pin is configured as interrupt */
+	reg = d->virt_base + bank->pctl_offset;
+	shift = pin;
+	if (bank_type->fld_width[PINCFG_TYPE_FUNC] * shift >= 32) {
+		/* 4-bit bank type with 2 con regs */
+		reg += 4;
+		shift -= 8;
+	}
+
+	shift = shift * bank_type->fld_width[PINCFG_TYPE_FUNC];
+	mask = (1 << bank_type->fld_width[PINCFG_TYPE_FUNC]) - 1;
+
+	spin_lock_irqsave(&bank->slock, flags);
+
+	val = readl(reg);
+	val &= ~(mask << shift);
+	val |= bank->eint_func << shift;
+	writel(val, reg);
+
+	spin_unlock_irqrestore(&bank->slock, flags);
+}
+
+/*
+ * Functions for EINT GPIO configuration (EINT groups 1-9)
+ */
+
+static inline void s3c64xx_gpio_irq_set_mask(struct irq_data *irqd, bool mask)
+{
+	struct samsung_pin_bank *bank = irq_data_get_irq_chip_data(irqd);
+	struct samsung_pinctrl_drv_data *d = bank->drvdata;
+	unsigned char index = EINT_OFFS(bank->eint_offset) + irqd->hwirq;
+	void __iomem *reg = d->virt_base + EINTMASK_REG(bank->eint_offset);
+	u32 val;
+
+	val = readl(reg);
+	if (mask)
+		val |= 1 << index;
+	else
+		val &= ~(1 << index);
+	writel(val, reg);
+}
+
+static void s3c64xx_gpio_irq_unmask(struct irq_data *irqd)
+{
+	s3c64xx_gpio_irq_set_mask(irqd, false);
+}
+
+static void s3c64xx_gpio_irq_mask(struct irq_data *irqd)
+{
+	s3c64xx_gpio_irq_set_mask(irqd, true);
+}
+
+static void s3c64xx_gpio_irq_ack(struct irq_data *irqd)
+{
+	struct samsung_pin_bank *bank = irq_data_get_irq_chip_data(irqd);
+	struct samsung_pinctrl_drv_data *d = bank->drvdata;
+	unsigned char index = EINT_OFFS(bank->eint_offset) + irqd->hwirq;
+	void __iomem *reg = d->virt_base + EINTPEND_REG(bank->eint_offset);
+
+	writel(1 << index, reg);
+}
+
+static int s3c64xx_gpio_irq_set_type(struct irq_data *irqd, unsigned int type)
+{
+	struct samsung_pin_bank *bank = irq_data_get_irq_chip_data(irqd);
+	struct samsung_pinctrl_drv_data *d = bank->drvdata;
+	void __iomem *reg;
+	int trigger;
+	u8 shift;
+	u32 val;
+
+	trigger = s3c64xx_irq_get_trigger(type);
+	if (trigger < 0) {
+		pr_err("unsupported external interrupt type\n");
+		return -EINVAL;
+	}
+
+	s3c64xx_irq_set_handler(irqd->irq, type);
+
+	/* Set up interrupt trigger */
+	reg = d->virt_base + EINTCON_REG(bank->eint_offset);
+	shift = EINT_OFFS(bank->eint_offset) + irqd->hwirq;
+	shift = 4 * (shift / 4); /* 4 EINTs per trigger selector */
+
+	val = readl(reg);
+	val &= ~(EINT_CON_MASK << shift);
+	val |= trigger << shift;
+	writel(val, reg);
+
+	s3c64xx_irq_set_function(d, bank, irqd->hwirq);
+
+	return 0;
+}
+
+/*
+ * irq_chip for gpio interrupts.
+ */
+static struct irq_chip s3c64xx_gpio_irq_chip = {
+	.name		= "GPIO",
+	.irq_unmask	= s3c64xx_gpio_irq_unmask,
+	.irq_mask	= s3c64xx_gpio_irq_mask,
+	.irq_ack	= s3c64xx_gpio_irq_ack,
+	.irq_set_type	= s3c64xx_gpio_irq_set_type,
+};
+
+static int s3c64xx_gpio_irq_map(struct irq_domain *h, unsigned int virq,
+					irq_hw_number_t hw)
+{
+	struct samsung_pin_bank *bank = h->host_data;
+
+	if (!(bank->eint_mask & (1 << hw)))
+		return -EINVAL;
+
+	irq_set_chip_and_handler(virq,
+				&s3c64xx_gpio_irq_chip, handle_level_irq);
+	irq_set_chip_data(virq, bank);
+	set_irq_flags(virq, IRQF_VALID);
+
+	return 0;
+}
+
+/*
+ * irq domain callbacks for external gpio interrupt controller.
+ */
+static const struct irq_domain_ops s3c64xx_gpio_irqd_ops = {
+	.map	= s3c64xx_gpio_irq_map,
+	.xlate	= irq_domain_xlate_twocell,
+};
+
+static void s3c64xx_eint_gpio_irq(unsigned int irq, struct irq_desc *desc)
+{
+	struct irq_chip *chip = irq_get_chip(irq);
+	struct s3c64xx_eint_gpio_data *data = irq_get_handler_data(irq);
+	struct samsung_pinctrl_drv_data *drvdata = data->drvdata;
+
+	chained_irq_enter(chip, desc);
+
+	do {
+		unsigned int svc;
+		unsigned int group;
+		unsigned int pin;
+		unsigned int virq;
+
+		svc = readl(drvdata->virt_base + SERVICE_REG);
+		group = SVC_GROUP(svc);
+		pin = svc & SVC_NUM_MASK;
+
+		if (!group)
+			break;
+
+		/* Group 1 is used for two pin banks */
+		if (group == 1) {
+			if (pin < 8)
+				group = 0;
+			else
+				pin -= 8;
+		}
+
+		virq = irq_linear_revmap(data->domains[group], pin);
+		/*
+		 * Something must be really wrong if an unmapped EINT
+		 * was unmasked...
+		 */
+		BUG_ON(!virq);
+
+		generic_handle_irq(virq);
+	} while (1);
+
+	chained_irq_exit(chip, desc);
+}
+
+/**
+ * s3c64xx_eint_gpio_init() - setup handling of external gpio interrupts.
+ * @d: driver data of samsung pinctrl driver.
+ */
+static int s3c64xx_eint_gpio_init(struct samsung_pinctrl_drv_data *d)
+{
+	struct s3c64xx_eint_gpio_data *data;
+	struct samsung_pin_bank *bank;
+	struct device *dev = d->dev;
+	unsigned int nr_domains;
+	unsigned int i;
+
+	if (!d->irq) {
+		dev_err(dev, "irq number not available\n");
+		return -EINVAL;
+	}
+
+	nr_domains = 0;
+	bank = d->ctrl->pin_banks;
+	for (i = 0; i < d->ctrl->nr_banks; ++i, ++bank) {
+		unsigned int nr_eints;
+		unsigned int mask;
+
+		if (bank->eint_type != EINT_TYPE_GPIO)
+			continue;
+
+		mask = bank->eint_mask;
+		nr_eints = fls(mask);
+
+		bank->irq_domain = irq_domain_add_linear(bank->of_node,
+					nr_eints, &s3c64xx_gpio_irqd_ops, bank);
+		if (!bank->irq_domain) {
+			dev_err(dev, "gpio irq domain add failed\n");
+			return -ENXIO;
+		}
+
+		++nr_domains;
+	}
+
+	data = devm_kzalloc(dev, sizeof(*data)
+			+ nr_domains * sizeof(*data->domains), GFP_KERNEL);
+	if (!data) {
+		dev_err(dev, "failed to allocate handler data\n");
+		return -ENOMEM;
+	}
+	data->drvdata = d;
+
+	bank = d->ctrl->pin_banks;
+	nr_domains = 0;
+	for (i = 0; i < d->ctrl->nr_banks; ++i, ++bank) {
+		if (bank->eint_type != EINT_TYPE_GPIO)
+			continue;
+
+		data->domains[nr_domains++] = bank->irq_domain;
+	}
+
+	irq_set_chained_handler(d->irq, s3c64xx_eint_gpio_irq);
+	irq_set_handler_data(d->irq, data);
+
+	return 0;
+}
+
+/*
+ * Functions for configuration of EINT0 wake-up interrupts
+ */
+
+static inline void s3c64xx_eint0_irq_set_mask(struct irq_data *irqd, bool mask)
+{
+	struct s3c64xx_eint0_domain_data *ddata =
+					irq_data_get_irq_chip_data(irqd);
+	struct samsung_pinctrl_drv_data *d = ddata->bank->drvdata;
+	u32 val;
+
+	val = readl(d->virt_base + EINT0MASK_REG);
+	if (mask)
+		val |= 1 << ddata->eints[irqd->hwirq];
+	else
+		val &= ~(1 << ddata->eints[irqd->hwirq]);
+	writel(val, d->virt_base + EINT0MASK_REG);
+}
+
+static void s3c64xx_eint0_irq_unmask(struct irq_data *irqd)
+{
+	s3c64xx_eint0_irq_set_mask(irqd, false);
+}
+
+static void s3c64xx_eint0_irq_mask(struct irq_data *irqd)
+{
+	s3c64xx_eint0_irq_set_mask(irqd, true);
+}
+
+static void s3c64xx_eint0_irq_ack(struct irq_data *irqd)
+{
+	struct s3c64xx_eint0_domain_data *ddata =
+					irq_data_get_irq_chip_data(irqd);
+	struct samsung_pinctrl_drv_data *d = ddata->bank->drvdata;
+
+	writel(1 << ddata->eints[irqd->hwirq],
+					d->virt_base + EINT0PEND_REG);
+}
+
+static int s3c64xx_eint0_irq_set_type(struct irq_data *irqd, unsigned int type)
+{
+	struct s3c64xx_eint0_domain_data *ddata =
+					irq_data_get_irq_chip_data(irqd);
+	struct samsung_pin_bank *bank = ddata->bank;
+	struct samsung_pinctrl_drv_data *d = bank->drvdata;
+	void __iomem *reg;
+	int trigger;
+	u8 shift;
+	u32 val;
+
+	trigger = s3c64xx_irq_get_trigger(type);
+	if (trigger < 0) {
+		pr_err("unsupported external interrupt type\n");
+		return -EINVAL;
+	}
+
+	s3c64xx_irq_set_handler(irqd->irq, type);
+
+	/* Set up interrupt trigger */
+	reg = d->virt_base + EINT0CON0_REG;
+	shift = ddata->eints[irqd->hwirq];
+	if (shift >= EINT_MAX_PER_REG) {
+		reg += 4;
+		shift -= EINT_MAX_PER_REG;
+	}
+	shift = EINT_CON_LEN * (shift / 2);
+
+	val = readl(reg);
+	val &= ~(EINT_CON_MASK << shift);
+	val |= trigger << shift;
+	writel(val, reg);
+
+	s3c64xx_irq_set_function(d, bank, irqd->hwirq);
+
+	return 0;
+}
+
+/*
+ * irq_chip for wakeup interrupts
+ */
+static struct irq_chip s3c64xx_eint0_irq_chip = {
+	.name		= "EINT0",
+	.irq_unmask	= s3c64xx_eint0_irq_unmask,
+	.irq_mask	= s3c64xx_eint0_irq_mask,
+	.irq_ack	= s3c64xx_eint0_irq_ack,
+	.irq_set_type	= s3c64xx_eint0_irq_set_type,
+};
+
+static inline void s3c64xx_irq_demux_eint(unsigned int irq,
+					struct irq_desc *desc, u32 range)
+{
+	struct irq_chip *chip = irq_get_chip(irq);
+	struct s3c64xx_eint0_data *data = irq_get_handler_data(irq);
+	struct samsung_pinctrl_drv_data *drvdata = data->drvdata;
+	unsigned int pend, mask;
+
+	chained_irq_enter(chip, desc);
+
+	pend = readl(drvdata->virt_base + EINT0PEND_REG);
+	mask = readl(drvdata->virt_base + EINT0MASK_REG);
+
+	pend = pend & range & ~mask;
+	pend &= range;
+
+	while (pend) {
+		unsigned int virq;
+
+		irq = fls(pend) - 1;
+		pend &= ~(1 << irq);
+
+		virq = irq_linear_revmap(data->domains[irq], data->pins[irq]);
+		/*
+		 * Something must be really wrong if an unmapped EINT
+		 * was unmasked...
+		 */
+		BUG_ON(!virq);
+
+		generic_handle_irq(virq);
+	}
+
+	chained_irq_exit(chip, desc);
+}
+
+static void s3c64xx_demux_eint0_3(unsigned int irq, struct irq_desc *desc)
+{
+	s3c64xx_irq_demux_eint(irq, desc, 0xf);
+}
+
+static void s3c64xx_demux_eint4_11(unsigned int irq, struct irq_desc *desc)
+{
+	s3c64xx_irq_demux_eint(irq, desc, 0xff0);
+}
+
+static void s3c64xx_demux_eint12_19(unsigned int irq, struct irq_desc *desc)
+{
+	s3c64xx_irq_demux_eint(irq, desc, 0xff000);
+}
+
+static void s3c64xx_demux_eint20_27(unsigned int irq, struct irq_desc *desc)
+{
+	s3c64xx_irq_demux_eint(irq, desc, 0xff00000);
+}
+
+static irq_flow_handler_t s3c64xx_eint0_handlers[NUM_EINT0_IRQ] = {
+	s3c64xx_demux_eint0_3,
+	s3c64xx_demux_eint4_11,
+	s3c64xx_demux_eint12_19,
+	s3c64xx_demux_eint20_27,
+};
+
+static int s3c64xx_eint0_irq_map(struct irq_domain *h, unsigned int virq,
+					irq_hw_number_t hw)
+{
+	struct s3c64xx_eint0_domain_data *ddata = h->host_data;
+	struct samsung_pin_bank *bank = ddata->bank;
+
+	if (!(bank->eint_mask & (1 << hw)))
+		return -EINVAL;
+
+	irq_set_chip_and_handler(virq,
+				&s3c64xx_eint0_irq_chip, handle_level_irq);
+	irq_set_chip_data(virq, ddata);
+	set_irq_flags(virq, IRQF_VALID);
+
+	return 0;
+}
+
+/*
+ * irq domain callbacks for external wakeup interrupt controller.
+ */
+static const struct irq_domain_ops s3c64xx_eint0_irqd_ops = {
+	.map	= s3c64xx_eint0_irq_map,
+	.xlate	= irq_domain_xlate_twocell,
+};
+
+/* list of external wakeup controllers supported */
+static const struct of_device_id s3c64xx_eint0_irq_ids[] = {
+	{ .compatible = "samsung,s3c64xx-wakeup-eint", },
+	{ }
+};
+
+/**
+ * s3c64xx_eint_eint0_init() - setup handling of external wakeup interrupts.
+ * @d: driver data of samsung pinctrl driver.
+ */
+static int s3c64xx_eint_eint0_init(struct samsung_pinctrl_drv_data *d)
+{
+	struct device *dev = d->dev;
+	struct device_node *eint0_np = NULL;
+	struct device_node *np;
+	struct samsung_pin_bank *bank;
+	struct s3c64xx_eint0_data *data;
+	unsigned int i;
+
+	for_each_child_of_node(dev->of_node, np) {
+		if (of_match_node(s3c64xx_eint0_irq_ids, np)) {
+			eint0_np = np;
+			break;
+		}
+	}
+	if (!eint0_np)
+		return -ENODEV;
+
+	data = devm_kzalloc(dev, sizeof(*data), GFP_KERNEL);
+	if (!data) {
+		dev_err(dev, "could not allocate memory for wkup eint data\n");
+		return -ENOMEM;
+	}
+	data->drvdata = d;
+
+	for (i = 0; i < NUM_EINT0_IRQ; ++i) {
+		unsigned int irq;
+
+		irq = irq_of_parse_and_map(eint0_np, i);
+		if (!irq) {
+			dev_err(dev, "failed to get wakeup EINT IRQ %d\n", i);
+			return -ENXIO;
+		}
+
+		irq_set_chained_handler(irq, s3c64xx_eint0_handlers[i]);
+		irq_set_handler_data(irq, data);
+	}
+
+	bank = d->ctrl->pin_banks;
+	for (i = 0; i < d->ctrl->nr_banks; ++i, ++bank) {
+		struct s3c64xx_eint0_domain_data *ddata;
+		unsigned int nr_eints;
+		unsigned int mask;
+		unsigned int irq;
+		unsigned int pin;
+
+		if (bank->eint_type != EINT_TYPE_WKUP)
+			continue;
+
+		mask = bank->eint_mask;
+		nr_eints = fls(mask);
+
+		ddata = devm_kzalloc(dev,
+				sizeof(*ddata) + nr_eints, GFP_KERNEL);
+		if (!ddata) {
+			dev_err(dev, "failed to allocate domain data\n");
+			return -ENOMEM;
+		}
+		ddata->bank = bank;
+
+		bank->irq_domain = irq_domain_add_linear(bank->of_node,
+				nr_eints, &s3c64xx_eint0_irqd_ops, ddata);
+		if (!bank->irq_domain) {
+			dev_err(dev, "wkup irq domain add failed\n");
+			return -ENXIO;
+		}
+
+		irq = bank->eint_offset;
+		mask = bank->eint_mask;
+		for (pin = 0; mask; ++pin, mask >>= 1) {
+			if (!(mask & 1))
+				continue;
+			data->domains[irq] = bank->irq_domain;
+			data->pins[irq] = pin;
+			ddata->eints[pin] = irq;
+			++irq;
+		}
+	}
+
+	return 0;
+}
+
+/* pin banks of s3c64xx pin-controller 0 */
+static struct samsung_pin_bank s3c64xx_pin_banks0[] = {
+	PIN_BANK_4BIT_EINTG(8, 0x000, "gpa", 0),
+	PIN_BANK_4BIT_EINTG(7, 0x020, "gpb", 8),
+	PIN_BANK_4BIT_EINTG(8, 0x040, "gpc", 16),
+	PIN_BANK_4BIT_EINTG(5, 0x060, "gpd", 32),
+	PIN_BANK_4BIT(5, 0x080, "gpe"),
+	PIN_BANK_2BIT_EINTG(16, 0x0a0, "gpf", 48, 0x3fff),
+	PIN_BANK_4BIT_EINTG(7, 0x0c0, "gpg", 64),
+	PIN_BANK_4BIT2_EINTG(10, 0x0e0, "gph", 80),
+	PIN_BANK_2BIT(16, 0x100, "gpi"),
+	PIN_BANK_2BIT(12, 0x120, "gpj"),
+	PIN_BANK_4BIT2_ALIVE(16, 0x800, "gpk"),
+	PIN_BANK_4BIT2_EINTW(15, 0x810, "gpl", 16, 0x7f00),
+	PIN_BANK_4BIT_EINTW(6, 0x820, "gpm", 23, 0x1f),
+	PIN_BANK_2BIT_EINTW(16, 0x830, "gpn", 0),
+	PIN_BANK_2BIT_EINTG(16, 0x140, "gpo", 96, 0xffff),
+	PIN_BANK_2BIT_EINTG(15, 0x160, "gpp", 112, 0x7fff),
+	PIN_BANK_2BIT_EINTG(9, 0x180, "gpq", 128, 0x1ff),
+};
+
+/*
+ * Samsung pinctrl driver data for S3C64xx SoC. S3C64xx SoC includes
+ * one gpio/pin-mux/pinconfig controller.
+ */
+struct samsung_pin_ctrl s3c64xx_pin_ctrl[] = {
+	{
+		/* pin-controller instance 1 data */
+		.pin_banks	= s3c64xx_pin_banks0,
+		.nr_banks	= ARRAY_SIZE(s3c64xx_pin_banks0),
+		.eint_gpio_init = s3c64xx_eint_gpio_init,
+		.eint_wkup_init = s3c64xx_eint_eint0_init,
+		.label		= "S3C64xx-GPIO",
+	},
+};
diff --git a/drivers/pinctrl/pinctrl-samsung.c b/drivers/pinctrl/pinctrl-samsung.c
index f206df17565..4f54faf2971 100644
--- a/drivers/pinctrl/pinctrl-samsung.c
+++ b/drivers/pinctrl/pinctrl-samsung.c
@@ -27,6 +27,7 @@
 #include <linux/err.h>
 #include <linux/gpio.h>
 #include <linux/irqdomain.h>
+#include <linux/spinlock.h>
 
 #include "core.h"
 #include "pinctrl-samsung.h"
@@ -214,7 +215,7 @@ static void samsung_dt_free_map(struct pinctrl_dev *pctldev,
 }
 
 /* list of pinctrl callbacks for the pinctrl core */
-static struct pinctrl_ops samsung_pctrl_ops = {
+static const struct pinctrl_ops samsung_pctrl_ops = {
 	.get_groups_count	= samsung_get_group_count,
 	.get_group_name		= samsung_get_group_name,
 	.get_group_pins		= samsung_get_group_pins,
@@ -274,10 +275,6 @@ static void pin_to_reg_bank(struct samsung_pinctrl_drv_data *drvdata,
 	*offset = pin - b->pin_base;
 	if (bank)
 		*bank = b;
-
-	/* some banks have two config registers in a single bank */
-	if (*offset * b->func_width > BITS_PER_LONG)
-		*reg += 4;
 }
 
 /* enable or disable a pinmux function */
@@ -289,6 +286,7 @@ static void samsung_pinmux_setup(struct pinctrl_dev *pctldev, unsigned selector,
 	struct samsung_pin_bank *bank;
 	void __iomem *reg;
 	u32 mask, shift, data, pin_offset, cnt;
+	unsigned long flags;
 
 	drvdata = pinctrl_dev_get_drvdata(pctldev);
 	pins = drvdata->pin_groups[group].pins;
@@ -298,16 +296,28 @@ static void samsung_pinmux_setup(struct pinctrl_dev *pctldev, unsigned selector,
 	 * pin function number in the config register.
 	 */
 	for (cnt = 0; cnt < drvdata->pin_groups[group].num_pins; cnt++) {
+		struct samsung_pin_bank_type *type;
+
 		pin_to_reg_bank(drvdata, pins[cnt] - drvdata->ctrl->base,
 				&reg, &pin_offset, &bank);
-		mask = (1 << bank->func_width) - 1;
-		shift = pin_offset * bank->func_width;
+		type = bank->type;
+		mask = (1 << type->fld_width[PINCFG_TYPE_FUNC]) - 1;
+		shift = pin_offset * type->fld_width[PINCFG_TYPE_FUNC];
+		if (shift >= 32) {
+			/* Some banks have two config registers */
+			shift -= 32;
+			reg += 4;
+		}
+
+		spin_lock_irqsave(&bank->slock, flags);
 
-		data = readl(reg);
+		data = readl(reg + type->reg_offset[PINCFG_TYPE_FUNC]);
 		data &= ~(mask << shift);
 		if (enable)
 			data |= drvdata->pin_groups[group].func << shift;
-		writel(data, reg);
+		writel(data, reg + type->reg_offset[PINCFG_TYPE_FUNC]);
+
+		spin_unlock_irqrestore(&bank->slock, flags);
 	}
 }
 
@@ -334,30 +344,44 @@ static void samsung_pinmux_disable(struct pinctrl_dev *pctldev,
 static int samsung_pinmux_gpio_set_direction(struct pinctrl_dev *pctldev,
 		struct pinctrl_gpio_range *range, unsigned offset, bool input)
 {
+	struct samsung_pin_bank_type *type;
 	struct samsung_pin_bank *bank;
 	struct samsung_pinctrl_drv_data *drvdata;
 	void __iomem *reg;
 	u32 data, pin_offset, mask, shift;
+	unsigned long flags;
 
 	bank = gc_to_pin_bank(range->gc);
+	type = bank->type;
 	drvdata = pinctrl_dev_get_drvdata(pctldev);
 
 	pin_offset = offset - bank->pin_base;
-	reg = drvdata->virt_base + bank->pctl_offset;
+	reg = drvdata->virt_base + bank->pctl_offset +
+					type->reg_offset[PINCFG_TYPE_FUNC];
+
+	mask = (1 << type->fld_width[PINCFG_TYPE_FUNC]) - 1;
+	shift = pin_offset * type->fld_width[PINCFG_TYPE_FUNC];
+	if (shift >= 32) {
+		/* Some banks have two config registers */
+		shift -= 32;
+		reg += 4;
+	}
 
-	mask = (1 << bank->func_width) - 1;
-	shift = pin_offset * bank->func_width;
+	spin_lock_irqsave(&bank->slock, flags);
 
 	data = readl(reg);
 	data &= ~(mask << shift);
 	if (!input)
 		data |= FUNC_OUTPUT << shift;
 	writel(data, reg);
+
+	spin_unlock_irqrestore(&bank->slock, flags);
+
 	return 0;
 }
 
 /* list of pinmux callbacks for the pinmux vertical in pinctrl core */
-static struct pinmux_ops samsung_pinmux_ops = {
+static const struct pinmux_ops samsung_pinmux_ops = {
 	.get_functions_count	= samsung_get_functions_count,
 	.get_function_name	= samsung_pinmux_get_fname,
 	.get_function_groups	= samsung_pinmux_get_groups,
@@ -371,40 +395,26 @@ static int samsung_pinconf_rw(struct pinctrl_dev *pctldev, unsigned int pin,
 				unsigned long *config, bool set)
 {
 	struct samsung_pinctrl_drv_data *drvdata;
+	struct samsung_pin_bank_type *type;
 	struct samsung_pin_bank *bank;
 	void __iomem *reg_base;
 	enum pincfg_type cfg_type = PINCFG_UNPACK_TYPE(*config);
 	u32 data, width, pin_offset, mask, shift;
 	u32 cfg_value, cfg_reg;
+	unsigned long flags;
 
 	drvdata = pinctrl_dev_get_drvdata(pctldev);
 	pin_to_reg_bank(drvdata, pin - drvdata->ctrl->base, &reg_base,
 					&pin_offset, &bank);
+	type = bank->type;
 
-	switch (cfg_type) {
-	case PINCFG_TYPE_PUD:
-		width = bank->pud_width;
-		cfg_reg = PUD_REG;
-		break;
-	case PINCFG_TYPE_DRV:
-		width = bank->drv_width;
-		cfg_reg = DRV_REG;
-		break;
-	case PINCFG_TYPE_CON_PDN:
-		width = bank->conpdn_width;
-		cfg_reg = CONPDN_REG;
-		break;
-	case PINCFG_TYPE_PUD_PDN:
-		width = bank->pudpdn_width;
-		cfg_reg = PUDPDN_REG;
-		break;
-	default:
-		WARN_ON(1);
+	if (cfg_type >= PINCFG_TYPE_NUM || !type->fld_width[cfg_type])
 		return -EINVAL;
-	}
 
-	if (!width)
-		return -EINVAL;
+	width = type->fld_width[cfg_type];
+	cfg_reg = type->reg_offset[cfg_type];
+
+	spin_lock_irqsave(&bank->slock, flags);
 
 	mask = (1 << width) - 1;
 	shift = pin_offset * width;
@@ -420,6 +430,9 @@ static int samsung_pinconf_rw(struct pinctrl_dev *pctldev, unsigned int pin,
 		data &= mask;
 		*config = PINCFG_PACK(cfg_type, data);
 	}
+
+	spin_unlock_irqrestore(&bank->slock, flags);
+
 	return 0;
 }
 
@@ -468,7 +481,7 @@ static int samsung_pinconf_group_get(struct pinctrl_dev *pctldev,
 }
 
 /* list of pinconfig callbacks for pinconfig vertical in the pinctrl code */
-static struct pinconf_ops samsung_pinconf_ops = {
+static const struct pinconf_ops samsung_pinconf_ops = {
 	.pin_config_get		= samsung_pinconf_get,
 	.pin_config_set		= samsung_pinconf_set,
 	.pin_config_group_get	= samsung_pinconf_group_get,
@@ -479,16 +492,22 @@ static struct pinconf_ops samsung_pinconf_ops = {
 static void samsung_gpio_set(struct gpio_chip *gc, unsigned offset, int value)
 {
 	struct samsung_pin_bank *bank = gc_to_pin_bank(gc);
+	struct samsung_pin_bank_type *type = bank->type;
+	unsigned long flags;
 	void __iomem *reg;
 	u32 data;
 
 	reg = bank->drvdata->virt_base + bank->pctl_offset;
 
-	data = readl(reg + DAT_REG);
+	spin_lock_irqsave(&bank->slock, flags);
+
+	data = readl(reg + type->reg_offset[PINCFG_TYPE_DAT]);
 	data &= ~(1 << offset);
 	if (value)
 		data |= 1 << offset;
-	writel(data, reg + DAT_REG);
+	writel(data, reg + type->reg_offset[PINCFG_TYPE_DAT]);
+
+	spin_unlock_irqrestore(&bank->slock, flags);
 }
 
 /* gpiolib gpio_get callback function */
@@ -497,10 +516,11 @@ static int samsung_gpio_get(struct gpio_chip *gc, unsigned offset)
 	void __iomem *reg;
 	u32 data;
 	struct samsung_pin_bank *bank = gc_to_pin_bank(gc);
+	struct samsung_pin_bank_type *type = bank->type;
 
 	reg = bank->drvdata->virt_base + bank->pctl_offset;
 
-	data = readl(reg + DAT_REG);
+	data = readl(reg + type->reg_offset[PINCFG_TYPE_DAT]);
 	data >>= offset;
 	data &= 1;
 	return data;
@@ -859,6 +879,7 @@ static struct samsung_pin_ctrl *samsung_pinctrl_get_soc_data(
 
 	bank = ctrl->pin_banks;
 	for (i = 0; i < ctrl->nr_banks; ++i, ++bank) {
+		spin_lock_init(&bank->slock);
 		bank->drvdata = d;
 		bank->pin_base = ctrl->nr_pins;
 		ctrl->nr_pins += bank->nr_pins;
@@ -944,10 +965,16 @@ static int samsung_pinctrl_probe(struct platform_device *pdev)
 }
 
 static const struct of_device_id samsung_pinctrl_dt_match[] = {
+#ifdef CONFIG_PINCTRL_EXYNOS
 	{ .compatible = "samsung,exynos4210-pinctrl",
 		.data = (void *)exynos4210_pin_ctrl },
 	{ .compatible = "samsung,exynos4x12-pinctrl",
 		.data = (void *)exynos4x12_pin_ctrl },
+#endif
+#ifdef CONFIG_PINCTRL_S3C64XX
+	{ .compatible = "samsung,s3c64xx-pinctrl",
+		.data = s3c64xx_pin_ctrl },
+#endif
 	{},
 };
 MODULE_DEVICE_TABLE(of, samsung_pinctrl_dt_match);
diff --git a/drivers/pinctrl/pinctrl-samsung.h b/drivers/pinctrl/pinctrl-samsung.h
index e2d4e67f7e8..45f27b41e30 100644
--- a/drivers/pinctrl/pinctrl-samsung.h
+++ b/drivers/pinctrl/pinctrl-samsung.h
@@ -25,28 +25,27 @@
 
 #include <linux/gpio.h>
 
-/* register offsets within a pin bank */
-#define DAT_REG		0x4
-#define PUD_REG		0x8
-#define DRV_REG		0xC
-#define CONPDN_REG	0x10
-#define PUDPDN_REG	0x14
-
 /* pinmux function number for pin as gpio output line */
 #define FUNC_OUTPUT	0x1
 
 /**
  * enum pincfg_type - possible pin configuration types supported.
+ * @PINCFG_TYPE_FUNC: Function configuration.
+ * @PINCFG_TYPE_DAT: Pin value configuration.
  * @PINCFG_TYPE_PUD: Pull up/down configuration.
  * @PINCFG_TYPE_DRV: Drive strength configuration.
  * @PINCFG_TYPE_CON_PDN: Pin function in power down mode.
  * @PINCFG_TYPE_PUD_PDN: Pull up/down configuration in power down mode.
  */
 enum pincfg_type {
+	PINCFG_TYPE_FUNC,
+	PINCFG_TYPE_DAT,
 	PINCFG_TYPE_PUD,
 	PINCFG_TYPE_DRV,
 	PINCFG_TYPE_CON_PDN,
 	PINCFG_TYPE_PUD_PDN,
+
+	PINCFG_TYPE_NUM
 };
 
 /*
@@ -103,33 +102,40 @@ enum eint_type {
 struct samsung_pinctrl_drv_data;
 
 /**
+ * struct samsung_pin_bank_type: pin bank type description
+ * @fld_width: widths of configuration bitfields (0 if unavailable)
+ * @reg_offset: offsets of configuration registers (don't care of width is 0)
+ */
+struct samsung_pin_bank_type {
+	u8 fld_width[PINCFG_TYPE_NUM];
+	u8 reg_offset[PINCFG_TYPE_NUM];
+};
+
+/**
  * struct samsung_pin_bank: represent a controller pin-bank.
+ * @type: type of the bank (register offsets and bitfield widths)
  * @pctl_offset: starting offset of the pin-bank registers.
  * @pin_base: starting pin number of the bank.
  * @nr_pins: number of pins included in this bank.
- * @func_width: width of the function selector bit field.
- * @pud_width: width of the pin pull up/down selector bit field.
- * @drv_width: width of the pin driver strength selector bit field.
- * @conpdn_width: width of the sleep mode function selector bin field.
- * @pudpdn_width: width of the sleep mode pull up/down selector bit field.
+ * @eint_func: function to set in CON register to configure pin as EINT.
  * @eint_type: type of the external interrupt supported by the bank.
+ * @eint_mask: bit mask of pins which support EINT function.
  * @name: name to be prefixed for each pin in this pin bank.
  * @of_node: OF node of the bank.
  * @drvdata: link to controller driver data
  * @irq_domain: IRQ domain of the bank.
  * @gpio_chip: GPIO chip of the bank.
  * @grange: linux gpio pin range supported by this bank.
+ * @slock: spinlock protecting bank registers
  */
 struct samsung_pin_bank {
+	struct samsung_pin_bank_type *type;
 	u32		pctl_offset;
 	u32		pin_base;
 	u8		nr_pins;
-	u8		func_width;
-	u8		pud_width;
-	u8		drv_width;
-	u8		conpdn_width;
-	u8		pudpdn_width;
+	u8		eint_func;
 	enum eint_type	eint_type;
+	u32		eint_mask;
 	u32		eint_offset;
 	char		*name;
 	struct device_node *of_node;
@@ -137,6 +143,7 @@ struct samsung_pin_bank {
 	struct irq_domain *irq_domain;
 	struct gpio_chip gpio_chip;
 	struct pinctrl_gpio_range grange;
+	spinlock_t slock;
 };
 
 /**
@@ -237,5 +244,6 @@ struct samsung_pmx_func {
 /* list of all exported SoC specific data */
 extern struct samsung_pin_ctrl exynos4210_pin_ctrl[];
 extern struct samsung_pin_ctrl exynos4x12_pin_ctrl[];
+extern struct samsung_pin_ctrl s3c64xx_pin_ctrl[];
 
 #endif /* __PINCTRL_SAMSUNG_H */
diff --git a/drivers/pinctrl/pinctrl-single.c b/drivers/pinctrl/pinctrl-single.c
index 5c32e880bcb..5f2d2bfd356 100644
--- a/drivers/pinctrl/pinctrl-single.c
+++ b/drivers/pinctrl/pinctrl-single.c
@@ -22,8 +22,10 @@
 
 #include <linux/pinctrl/pinctrl.h>
 #include <linux/pinctrl/pinmux.h>
+#include <linux/pinctrl/pinconf-generic.h>
 
 #include "core.h"
+#include "pinconf.h"
 
 #define DRIVER_NAME			"pinctrl-single"
 #define PCS_MUX_PINS_NAME		"pinctrl-single,pins"
@@ -59,6 +61,33 @@ struct pcs_func_vals {
 };
 
 /**
+ * struct pcs_conf_vals - pinconf parameter, pinconf register offset
+ * and value, enable, disable, mask
+ * @param:	config parameter
+ * @val:	user input bits in the pinconf register
+ * @enable:	enable bits in the pinconf register
+ * @disable:	disable bits in the pinconf register
+ * @mask:	mask bits in the register value
+ */
+struct pcs_conf_vals {
+	enum pin_config_param param;
+	unsigned val;
+	unsigned enable;
+	unsigned disable;
+	unsigned mask;
+};
+
+/**
+ * struct pcs_conf_type - pinconf property name, pinconf param pair
+ * @name:	property name in DTS file
+ * @param:	config parameter
+ */
+struct pcs_conf_type {
+	const char *name;
+	enum pin_config_param param;
+};
+
+/**
  * struct pcs_function - pinctrl function
  * @name:	pinctrl function name
  * @vals:	register and vals array
@@ -73,6 +102,22 @@ struct pcs_function {
 	unsigned nvals;
 	const char **pgnames;
 	int npgnames;
+	struct pcs_conf_vals *conf;
+	int nconfs;
+	struct list_head node;
+};
+
+/**
+ * struct pcs_gpiofunc_range - pin ranges with same mux value of gpio function
+ * @offset:	offset base of pins
+ * @npins:	number pins with the same mux value of gpio function
+ * @gpiofunc:	mux value of gpio function
+ * @node:	list node
+ */
+struct pcs_gpiofunc_range {
+	unsigned offset;
+	unsigned npins;
+	unsigned gpiofunc;
 	struct list_head node;
 };
 
@@ -117,12 +162,14 @@ struct pcs_name {
  * @fshift:	function register shift
  * @foff:	value to turn mux off
  * @fmax:	max number of functions in fmask
+ * @is_pinconf:	whether supports pinconf
  * @names:	array of register names for pins
  * @pins:	physical pins on the SoC
  * @pgtree:	pingroup index radix tree
  * @ftree:	function index radix tree
  * @pingroups:	list of pingroups
  * @functions:	list of functions
+ * @gpiofuncs:	list of gpio functions
  * @ngroups:	number of pingroups
  * @nfuncs:	number of functions
  * @desc:	pin controller descriptor
@@ -142,12 +189,14 @@ struct pcs_device {
 	unsigned foff;
 	unsigned fmax;
 	bool bits_per_mux;
+	bool is_pinconf;
 	struct pcs_name *names;
 	struct pcs_data pins;
 	struct radix_tree_root pgtree;
 	struct radix_tree_root ftree;
 	struct list_head pingroups;
 	struct list_head functions;
+	struct list_head gpiofuncs;
 	unsigned ngroups;
 	unsigned nfuncs;
 	struct pinctrl_desc desc;
@@ -155,6 +204,16 @@ struct pcs_device {
 	void (*write)(unsigned val, void __iomem *reg);
 };
 
+static int pcs_pinconf_get(struct pinctrl_dev *pctldev, unsigned pin,
+			   unsigned long *config);
+static int pcs_pinconf_set(struct pinctrl_dev *pctldev, unsigned pin,
+			   unsigned long config);
+
+static enum pin_config_param pcs_bias[] = {
+	PIN_CONFIG_BIAS_PULL_DOWN,
+	PIN_CONFIG_BIAS_PULL_UP,
+};
+
 /*
  * REVISIT: Reads and writes could eventually use regmap or something
  * generic. But at least on omaps, some mux registers are performance
@@ -270,7 +329,7 @@ static int pcs_dt_node_to_map(struct pinctrl_dev *pctldev,
 				struct device_node *np_config,
 				struct pinctrl_map **map, unsigned *num_maps);
 
-static struct pinctrl_ops pcs_pinctrl_ops = {
+static const struct pinctrl_ops pcs_pinctrl_ops = {
 	.get_groups_count = pcs_get_groups_count,
 	.get_group_name = pcs_get_group_name,
 	.get_group_pins = pcs_get_group_pins,
@@ -326,6 +385,28 @@ static int pcs_get_function_groups(struct pinctrl_dev *pctldev,
 	return 0;
 }
 
+static int pcs_get_function(struct pinctrl_dev *pctldev, unsigned pin,
+			    struct pcs_function **func)
+{
+	struct pcs_device *pcs = pinctrl_dev_get_drvdata(pctldev);
+	struct pin_desc *pdesc = pin_desc_get(pctldev, pin);
+	const struct pinctrl_setting_mux *setting;
+	unsigned fselector;
+
+	/* If pin is not described in DTS & enabled, mux_setting is NULL. */
+	setting = pdesc->mux_setting;
+	if (!setting)
+		return -ENOTSUPP;
+	fselector = setting->func;
+	*func = radix_tree_lookup(&pcs->ftree, fselector);
+	if (!(*func)) {
+		dev_err(pcs->dev, "%s could not find function%i\n",
+			__func__, fselector);
+		return -ENOTSUPP;
+	}
+	return 0;
+}
+
 static int pcs_enable(struct pinctrl_dev *pctldev, unsigned fselector,
 	unsigned group)
 {
@@ -334,6 +415,9 @@ static int pcs_enable(struct pinctrl_dev *pctldev, unsigned fselector,
 	int i;
 
 	pcs = pinctrl_dev_get_drvdata(pctldev);
+	/* If function mask is null, needn't enable it. */
+	if (!pcs->fmask)
+		return 0;
 	func = radix_tree_lookup(&pcs->ftree, fselector);
 	if (!func)
 		return -EINVAL;
@@ -368,6 +452,10 @@ static void pcs_disable(struct pinctrl_dev *pctldev, unsigned fselector,
 	int i;
 
 	pcs = pinctrl_dev_get_drvdata(pctldev);
+	/* If function mask is null, needn't disable it. */
+	if (!pcs->fmask)
+		return;
+
 	func = radix_tree_lookup(&pcs->ftree, fselector);
 	if (!func) {
 		dev_err(pcs->dev, "%s could not find function%i\n",
@@ -403,12 +491,33 @@ static void pcs_disable(struct pinctrl_dev *pctldev, unsigned fselector,
 }
 
 static int pcs_request_gpio(struct pinctrl_dev *pctldev,
-			struct pinctrl_gpio_range *range, unsigned offset)
+			    struct pinctrl_gpio_range *range, unsigned pin)
 {
-	return -ENOTSUPP;
+	struct pcs_device *pcs = pinctrl_dev_get_drvdata(pctldev);
+	struct pcs_gpiofunc_range *frange = NULL;
+	struct list_head *pos, *tmp;
+	int mux_bytes = 0;
+	unsigned data;
+
+	/* If function mask is null, return directly. */
+	if (!pcs->fmask)
+		return -ENOTSUPP;
+
+	list_for_each_safe(pos, tmp, &pcs->gpiofuncs) {
+		frange = list_entry(pos, struct pcs_gpiofunc_range, node);
+		if (pin >= frange->offset + frange->npins
+			|| pin < frange->offset)
+			continue;
+		mux_bytes = pcs->width / BITS_PER_BYTE;
+		data = pcs->read(pcs->base + pin * mux_bytes) & ~pcs->fmask;
+		data |= frange->gpiofunc;
+		pcs->write(data, pcs->base + pin * mux_bytes);
+		break;
+	}
+	return 0;
 }
 
-static struct pinmux_ops pcs_pinmux_ops = {
+static const struct pinmux_ops pcs_pinmux_ops = {
 	.get_functions_count = pcs_get_functions_count,
 	.get_function_name = pcs_get_function_name,
 	.get_function_groups = pcs_get_function_groups,
@@ -417,32 +526,190 @@ static struct pinmux_ops pcs_pinmux_ops = {
 	.gpio_request_enable = pcs_request_gpio,
 };
 
+/* Clear BIAS value */
+static void pcs_pinconf_clear_bias(struct pinctrl_dev *pctldev, unsigned pin)
+{
+	unsigned long config;
+	int i;
+	for (i = 0; i < ARRAY_SIZE(pcs_bias); i++) {
+		config = pinconf_to_config_packed(pcs_bias[i], 0);
+		pcs_pinconf_set(pctldev, pin, config);
+	}
+}
+
+/*
+ * Check whether PIN_CONFIG_BIAS_DISABLE is valid.
+ * It's depend on that PULL_DOWN & PULL_UP configs are all invalid.
+ */
+static bool pcs_pinconf_bias_disable(struct pinctrl_dev *pctldev, unsigned pin)
+{
+	unsigned long config;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(pcs_bias); i++) {
+		config = pinconf_to_config_packed(pcs_bias[i], 0);
+		if (!pcs_pinconf_get(pctldev, pin, &config))
+			goto out;
+	}
+	return true;
+out:
+	return false;
+}
+
 static int pcs_pinconf_get(struct pinctrl_dev *pctldev,
 				unsigned pin, unsigned long *config)
 {
+	struct pcs_device *pcs = pinctrl_dev_get_drvdata(pctldev);
+	struct pcs_function *func;
+	enum pin_config_param param;
+	unsigned offset = 0, data = 0, i, j, ret;
+
+	ret = pcs_get_function(pctldev, pin, &func);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < func->nconfs; i++) {
+		param = pinconf_to_config_param(*config);
+		if (param == PIN_CONFIG_BIAS_DISABLE) {
+			if (pcs_pinconf_bias_disable(pctldev, pin)) {
+				*config = 0;
+				return 0;
+			} else {
+				return -ENOTSUPP;
+			}
+		} else if (param != func->conf[i].param) {
+			continue;
+		}
+
+		offset = pin * (pcs->width / BITS_PER_BYTE);
+		data = pcs->read(pcs->base + offset) & func->conf[i].mask;
+		switch (func->conf[i].param) {
+		/* 4 parameters */
+		case PIN_CONFIG_BIAS_PULL_DOWN:
+		case PIN_CONFIG_BIAS_PULL_UP:
+		case PIN_CONFIG_INPUT_SCHMITT_ENABLE:
+			if ((data != func->conf[i].enable) ||
+			    (data == func->conf[i].disable))
+				return -ENOTSUPP;
+			*config = 0;
+			break;
+		/* 2 parameters */
+		case PIN_CONFIG_INPUT_SCHMITT:
+			for (j = 0; j < func->nconfs; j++) {
+				switch (func->conf[j].param) {
+				case PIN_CONFIG_INPUT_SCHMITT_ENABLE:
+					if (data != func->conf[j].enable)
+						return -ENOTSUPP;
+					break;
+				default:
+					break;
+				}
+			}
+			*config = data;
+			break;
+		case PIN_CONFIG_DRIVE_STRENGTH:
+		case PIN_CONFIG_SLEW_RATE:
+		default:
+			*config = data;
+			break;
+		}
+		return 0;
+	}
 	return -ENOTSUPP;
 }
 
 static int pcs_pinconf_set(struct pinctrl_dev *pctldev,
 				unsigned pin, unsigned long config)
 {
+	struct pcs_device *pcs = pinctrl_dev_get_drvdata(pctldev);
+	struct pcs_function *func;
+	unsigned offset = 0, shift = 0, i, data, ret;
+	u16 arg;
+
+	ret = pcs_get_function(pctldev, pin, &func);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < func->nconfs; i++) {
+		if (pinconf_to_config_param(config) == func->conf[i].param) {
+			offset = pin * (pcs->width / BITS_PER_BYTE);
+			data = pcs->read(pcs->base + offset);
+			arg = pinconf_to_config_argument(config);
+			switch (func->conf[i].param) {
+			/* 2 parameters */
+			case PIN_CONFIG_INPUT_SCHMITT:
+			case PIN_CONFIG_DRIVE_STRENGTH:
+			case PIN_CONFIG_SLEW_RATE:
+				shift = ffs(func->conf[i].mask) - 1;
+				data &= ~func->conf[i].mask;
+				data |= (arg << shift) & func->conf[i].mask;
+				break;
+			/* 4 parameters */
+			case PIN_CONFIG_BIAS_DISABLE:
+				pcs_pinconf_clear_bias(pctldev, pin);
+				break;
+			case PIN_CONFIG_BIAS_PULL_DOWN:
+			case PIN_CONFIG_BIAS_PULL_UP:
+				if (arg)
+					pcs_pinconf_clear_bias(pctldev, pin);
+				/* fall through */
+			case PIN_CONFIG_INPUT_SCHMITT_ENABLE:
+				data &= ~func->conf[i].mask;
+				if (arg)
+					data |= func->conf[i].enable;
+				else
+					data |= func->conf[i].disable;
+				break;
+			default:
+				return -ENOTSUPP;
+			}
+			pcs->write(data, pcs->base + offset);
+			return 0;
+		}
+	}
 	return -ENOTSUPP;
 }
 
 static int pcs_pinconf_group_get(struct pinctrl_dev *pctldev,
 				unsigned group, unsigned long *config)
 {
-	return -ENOTSUPP;
+	const unsigned *pins;
+	unsigned npins, old = 0;
+	int i, ret;
+
+	ret = pcs_get_group_pins(pctldev, group, &pins, &npins);
+	if (ret)
+		return ret;
+	for (i = 0; i < npins; i++) {
+		if (pcs_pinconf_get(pctldev, pins[i], config))
+			return -ENOTSUPP;
+		/* configs do not match between two pins */
+		if (i && (old != *config))
+			return -ENOTSUPP;
+		old = *config;
+	}
+	return 0;
 }
 
 static int pcs_pinconf_group_set(struct pinctrl_dev *pctldev,
 				unsigned group, unsigned long config)
 {
-	return -ENOTSUPP;
+	const unsigned *pins;
+	unsigned npins;
+	int i, ret;
+
+	ret = pcs_get_group_pins(pctldev, group, &pins, &npins);
+	if (ret)
+		return ret;
+	for (i = 0; i < npins; i++) {
+		if (pcs_pinconf_set(pctldev, pins[i], config))
+			return -ENOTSUPP;
+	}
+	return 0;
 }
 
 static void pcs_pinconf_dbg_show(struct pinctrl_dev *pctldev,
-				struct seq_file *s, unsigned offset)
+				struct seq_file *s, unsigned pin)
 {
 }
 
@@ -451,13 +718,22 @@ static void pcs_pinconf_group_dbg_show(struct pinctrl_dev *pctldev,
 {
 }
 
-static struct pinconf_ops pcs_pinconf_ops = {
+static void pcs_pinconf_config_dbg_show(struct pinctrl_dev *pctldev,
+					struct seq_file *s,
+					unsigned long config)
+{
+	pinconf_generic_dump_config(pctldev, s, config);
+}
+
+static const struct pinconf_ops pcs_pinconf_ops = {
 	.pin_config_get = pcs_pinconf_get,
 	.pin_config_set = pcs_pinconf_set,
 	.pin_config_group_get = pcs_pinconf_group_get,
 	.pin_config_group_set = pcs_pinconf_group_set,
 	.pin_config_dbg_show = pcs_pinconf_dbg_show,
 	.pin_config_group_dbg_show = pcs_pinconf_group_dbg_show,
+	.pin_config_config_dbg_show = pcs_pinconf_config_dbg_show,
+	.is_generic = true,
 };
 
 /**
@@ -648,11 +924,158 @@ static int pcs_get_pin_by_offset(struct pcs_device *pcs, unsigned offset)
 	return index;
 }
 
+/*
+ * check whether data matches enable bits or disable bits
+ * Return value: 1 for matching enable bits, 0 for matching disable bits,
+ *               and negative value for matching failure.
+ */
+static int pcs_config_match(unsigned data, unsigned enable, unsigned disable)
+{
+	int ret = -EINVAL;
+
+	if (data == enable)
+		ret = 1;
+	else if (data == disable)
+		ret = 0;
+	return ret;
+}
+
+static void add_config(struct pcs_conf_vals **conf, enum pin_config_param param,
+		       unsigned value, unsigned enable, unsigned disable,
+		       unsigned mask)
+{
+	(*conf)->param = param;
+	(*conf)->val = value;
+	(*conf)->enable = enable;
+	(*conf)->disable = disable;
+	(*conf)->mask = mask;
+	(*conf)++;
+}
+
+static void add_setting(unsigned long **setting, enum pin_config_param param,
+			unsigned arg)
+{
+	**setting = pinconf_to_config_packed(param, arg);
+	(*setting)++;
+}
+
+/* add pinconf setting with 2 parameters */
+static void pcs_add_conf2(struct pcs_device *pcs, struct device_node *np,
+			  const char *name, enum pin_config_param param,
+			  struct pcs_conf_vals **conf, unsigned long **settings)
+{
+	unsigned value[2], shift;
+	int ret;
+
+	ret = of_property_read_u32_array(np, name, value, 2);
+	if (ret)
+		return;
+	/* set value & mask */
+	value[0] &= value[1];
+	shift = ffs(value[1]) - 1;
+	/* skip enable & disable */
+	add_config(conf, param, value[0], 0, 0, value[1]);
+	add_setting(settings, param, value[0] >> shift);
+}
+
+/* add pinconf setting with 4 parameters */
+static void pcs_add_conf4(struct pcs_device *pcs, struct device_node *np,
+			  const char *name, enum pin_config_param param,
+			  struct pcs_conf_vals **conf, unsigned long **settings)
+{
+	unsigned value[4];
+	int ret;
+
+	/* value to set, enable, disable, mask */
+	ret = of_property_read_u32_array(np, name, value, 4);
+	if (ret)
+		return;
+	if (!value[3]) {
+		dev_err(pcs->dev, "mask field of the property can't be 0\n");
+		return;
+	}
+	value[0] &= value[3];
+	value[1] &= value[3];
+	value[2] &= value[3];
+	ret = pcs_config_match(value[0], value[1], value[2]);
+	if (ret < 0)
+		dev_dbg(pcs->dev, "failed to match enable or disable bits\n");
+	add_config(conf, param, value[0], value[1], value[2], value[3]);
+	add_setting(settings, param, ret);
+}
+
+static int pcs_parse_pinconf(struct pcs_device *pcs, struct device_node *np,
+			     struct pcs_function *func,
+			     struct pinctrl_map **map)
+
+{
+	struct pinctrl_map *m = *map;
+	int i = 0, nconfs = 0;
+	unsigned long *settings = NULL, *s = NULL;
+	struct pcs_conf_vals *conf = NULL;
+	struct pcs_conf_type prop2[] = {
+		{ "pinctrl-single,drive-strength", PIN_CONFIG_DRIVE_STRENGTH, },
+		{ "pinctrl-single,slew-rate", PIN_CONFIG_SLEW_RATE, },
+		{ "pinctrl-single,input-schmitt", PIN_CONFIG_INPUT_SCHMITT, },
+	};
+	struct pcs_conf_type prop4[] = {
+		{ "pinctrl-single,bias-pullup", PIN_CONFIG_BIAS_PULL_UP, },
+		{ "pinctrl-single,bias-pulldown", PIN_CONFIG_BIAS_PULL_DOWN, },
+		{ "pinctrl-single,input-schmitt-enable",
+			PIN_CONFIG_INPUT_SCHMITT_ENABLE, },
+	};
+
+	/* If pinconf isn't supported, don't parse properties in below. */
+	if (!pcs->is_pinconf)
+		return 0;
+
+	/* cacluate how much properties are supported in current node */
+	for (i = 0; i < ARRAY_SIZE(prop2); i++) {
+		if (of_find_property(np, prop2[i].name, NULL))
+			nconfs++;
+	}
+	for (i = 0; i < ARRAY_SIZE(prop4); i++) {
+		if (of_find_property(np, prop4[i].name, NULL))
+			nconfs++;
+	}
+	if (!nconfs)
+		return 0;
+
+	func->conf = devm_kzalloc(pcs->dev,
+				  sizeof(struct pcs_conf_vals) * nconfs,
+				  GFP_KERNEL);
+	if (!func->conf)
+		return -ENOMEM;
+	func->nconfs = nconfs;
+	conf = &(func->conf[0]);
+	m++;
+	settings = devm_kzalloc(pcs->dev, sizeof(unsigned long) * nconfs,
+				GFP_KERNEL);
+	if (!settings)
+		return -ENOMEM;
+	s = &settings[0];
+
+	for (i = 0; i < ARRAY_SIZE(prop2); i++)
+		pcs_add_conf2(pcs, np, prop2[i].name, prop2[i].param,
+			      &conf, &s);
+	for (i = 0; i < ARRAY_SIZE(prop4); i++)
+		pcs_add_conf4(pcs, np, prop4[i].name, prop4[i].param,
+			      &conf, &s);
+	m->type = PIN_MAP_TYPE_CONFIGS_GROUP;
+	m->data.configs.group_or_pin = np->name;
+	m->data.configs.configs = settings;
+	m->data.configs.num_configs = nconfs;
+	return 0;
+}
+
+static void pcs_free_pingroups(struct pcs_device *pcs);
+
 /**
  * smux_parse_one_pinctrl_entry() - parses a device tree mux entry
  * @pcs: pinctrl driver instance
  * @np: device node of the mux entry
  * @map: map entry
+ * @num_maps: number of map
  * @pgnames: pingroup names
  *
  * Note that this binding currently supports only sets of one register + value.
@@ -669,6 +1092,7 @@ static int pcs_get_pin_by_offset(struct pcs_device *pcs, unsigned offset)
 static int pcs_parse_one_pinctrl_entry(struct pcs_device *pcs,
 						struct device_node *np,
 						struct pinctrl_map **map,
+						unsigned *num_maps,
 						const char **pgnames)
 {
 	struct pcs_func_vals *vals;
@@ -741,8 +1165,18 @@ static int pcs_parse_one_pinctrl_entry(struct pcs_device *pcs,
 	(*map)->data.mux.group = np->name;
 	(*map)->data.mux.function = np->name;
 
+	if (pcs->is_pinconf) {
+		if (pcs_parse_pinconf(pcs, np, function, map))
+			goto free_pingroups;
+		*num_maps = 2;
+	} else {
+		*num_maps = 1;
+	}
 	return 0;
 
+free_pingroups:
+	pcs_free_pingroups(pcs);
+	*num_maps = 1;
 free_function:
 	pcs_remove_function(pcs, function);
 
@@ -771,7 +1205,8 @@ static int pcs_dt_node_to_map(struct pinctrl_dev *pctldev,
 
 	pcs = pinctrl_dev_get_drvdata(pctldev);
 
-	*map = devm_kzalloc(pcs->dev, sizeof(**map), GFP_KERNEL);
+	/* create 2 maps. One is for pinmux, and the other is for pinconf. */
+	*map = devm_kzalloc(pcs->dev, sizeof(**map) * 2, GFP_KERNEL);
 	if (!*map)
 		return -ENOMEM;
 
@@ -783,13 +1218,13 @@ static int pcs_dt_node_to_map(struct pinctrl_dev *pctldev,
 		goto free_map;
 	}
 
-	ret = pcs_parse_one_pinctrl_entry(pcs, np_config, map, pgnames);
+	ret = pcs_parse_one_pinctrl_entry(pcs, np_config, map, num_maps,
+					  pgnames);
 	if (ret < 0) {
 		dev_err(pcs->dev, "no pins entries for %s\n",
 			np_config->name);
 		goto free_pgnames;
 	}
-	*num_maps = 1;
 
 	return 0;
 
@@ -879,6 +1314,37 @@ static void pcs_free_resources(struct pcs_device *pcs)
 
 static struct of_device_id pcs_of_match[];
 
+static int pcs_add_gpio_func(struct device_node *node, struct pcs_device *pcs)
+{
+	const char *propname = "pinctrl-single,gpio-range";
+	const char *cellname = "#pinctrl-single,gpio-range-cells";
+	struct of_phandle_args gpiospec;
+	struct pcs_gpiofunc_range *range;
+	int ret, i;
+
+	for (i = 0; ; i++) {
+		ret = of_parse_phandle_with_args(node, propname, cellname,
+						 i, &gpiospec);
+		/* Do not treat it as error. Only treat it as end condition. */
+		if (ret) {
+			ret = 0;
+			break;
+		}
+		range = devm_kzalloc(pcs->dev, sizeof(*range), GFP_KERNEL);
+		if (!range) {
+			ret = -ENOMEM;
+			break;
+		}
+		range->offset = gpiospec.args[0];
+		range->npins = gpiospec.args[1];
+		range->gpiofunc = gpiospec.args[2];
+		mutex_lock(&pcs->mutex);
+		list_add_tail(&range->node, &pcs->gpiofuncs);
+		mutex_unlock(&pcs->mutex);
+	}
+	return ret;
+}
+
 static int pcs_probe(struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
@@ -900,14 +1366,23 @@ static int pcs_probe(struct platform_device *pdev)
 	mutex_init(&pcs->mutex);
 	INIT_LIST_HEAD(&pcs->pingroups);
 	INIT_LIST_HEAD(&pcs->functions);
+	INIT_LIST_HEAD(&pcs->gpiofuncs);
+	pcs->is_pinconf = match->data;
 
 	PCS_GET_PROP_U32("pinctrl-single,register-width", &pcs->width,
 			 "register width not specified\n");
 
-	PCS_GET_PROP_U32("pinctrl-single,function-mask", &pcs->fmask,
-			 "function register mask not specified\n");
-	pcs->fshift = ffs(pcs->fmask) - 1;
-	pcs->fmax = pcs->fmask >> pcs->fshift;
+	ret = of_property_read_u32(np, "pinctrl-single,function-mask",
+				   &pcs->fmask);
+	if (!ret) {
+		pcs->fshift = ffs(pcs->fmask) - 1;
+		pcs->fmax = pcs->fmask >> pcs->fshift;
+	} else {
+		/* If mask property doesn't exist, function mux is invalid. */
+		pcs->fmask = 0;
+		pcs->fshift = 0;
+		pcs->fmax = 0;
+	}
 
 	ret = of_property_read_u32(np, "pinctrl-single,function-off",
 					&pcs->foff);
@@ -961,7 +1436,8 @@ static int pcs_probe(struct platform_device *pdev)
 	pcs->desc.name = DRIVER_NAME;
 	pcs->desc.pctlops = &pcs_pinctrl_ops;
 	pcs->desc.pmxops = &pcs_pinmux_ops;
-	pcs->desc.confops = &pcs_pinconf_ops;
+	if (pcs->is_pinconf)
+		pcs->desc.confops = &pcs_pinconf_ops;
 	pcs->desc.owner = THIS_MODULE;
 
 	ret = pcs_allocate_pin_table(pcs);
@@ -975,6 +1451,10 @@ static int pcs_probe(struct platform_device *pdev)
 		goto free;
 	}
 
+	ret = pcs_add_gpio_func(np, pcs);
+	if (ret < 0)
+		goto free;
+
 	dev_info(pcs->dev, "%i pins at pa %p size %u\n",
 		 pcs->desc.npins, pcs->base, pcs->size);
 
@@ -999,7 +1479,8 @@ static int pcs_remove(struct platform_device *pdev)
 }
 
 static struct of_device_id pcs_of_match[] = {
-	{ .compatible = DRIVER_NAME, },
+	{ .compatible = "pinctrl-single", .data = (void *)false },
+	{ .compatible = "pinconf-single", .data = (void *)true },
 	{ },
 };
 MODULE_DEVICE_TABLE(of, pcs_of_match);
diff --git a/drivers/pinctrl/pinctrl-sirf.c b/drivers/pinctrl/pinctrl-sirf.c
index d02498b30c6..fb906257074 100644
--- a/drivers/pinctrl/pinctrl-sirf.c
+++ b/drivers/pinctrl/pinctrl-sirf.c
@@ -979,7 +979,7 @@ static void sirfsoc_dt_free_map(struct pinctrl_dev *pctldev,
 	kfree(map);
 }
 
-static struct pinctrl_ops sirfsoc_pctrl_ops = {
+static const struct pinctrl_ops sirfsoc_pctrl_ops = {
 	.get_groups_count = sirfsoc_get_groups_count,
 	.get_group_name = sirfsoc_get_group_name,
 	.get_group_pins = sirfsoc_get_group_pins,
@@ -1181,7 +1181,7 @@ static int sirfsoc_pinmux_request_gpio(struct pinctrl_dev *pmxdev,
 	return 0;
 }
 
-static struct pinmux_ops sirfsoc_pinmux_ops = {
+static const struct pinmux_ops sirfsoc_pinmux_ops = {
 	.enable = sirfsoc_pinmux_enable,
 	.disable = sirfsoc_pinmux_disable,
 	.get_functions_count = sirfsoc_pinmux_get_funcs_count,
@@ -1685,15 +1685,12 @@ static void sirfsoc_gpio_set_pullup(const u32 *pullups)
 	const unsigned long *p = (const unsigned long *)pullups;
 
 	for (i = 0; i < SIRFSOC_GPIO_NO_OF_BANKS; i++) {
-		n = find_first_bit(p + i, BITS_PER_LONG);
-		while (n < BITS_PER_LONG) {
+		for_each_set_bit(n, p + i, BITS_PER_LONG) {
 			u32 offset = SIRFSOC_GPIO_CTRL(i, n);
 			u32 val = readl(sgpio_bank[i].chip.regs + offset);
 			val |= SIRFSOC_GPIO_CTL_PULL_MASK;
 			val |= SIRFSOC_GPIO_CTL_PULL_HIGH;
 			writel(val, sgpio_bank[i].chip.regs + offset);
-
-			n = find_next_bit(p + i, BITS_PER_LONG, n + 1);
 		}
 	}
 }
@@ -1704,15 +1701,12 @@ static void sirfsoc_gpio_set_pulldown(const u32 *pulldowns)
 	const unsigned long *p = (const unsigned long *)pulldowns;
 
 	for (i = 0; i < SIRFSOC_GPIO_NO_OF_BANKS; i++) {
-		n = find_first_bit(p + i, BITS_PER_LONG);
-		while (n < BITS_PER_LONG) {
+		for_each_set_bit(n, p + i, BITS_PER_LONG) {
 			u32 offset = SIRFSOC_GPIO_CTRL(i, n);
 			u32 val = readl(sgpio_bank[i].chip.regs + offset);
 			val |= SIRFSOC_GPIO_CTL_PULL_MASK;
 			val &= ~SIRFSOC_GPIO_CTL_PULL_HIGH;
 			writel(val, sgpio_bank[i].chip.regs + offset);
-
-			n = find_next_bit(p + i, BITS_PER_LONG, n + 1);
 		}
 	}
 }
diff --git a/drivers/pinctrl/pinctrl-sunxi.c b/drivers/pinctrl/pinctrl-sunxi.c
index 80b11e3415b..c52fc2c0873 100644
--- a/drivers/pinctrl/pinctrl-sunxi.c
+++ b/drivers/pinctrl/pinctrl-sunxi.c
@@ -11,6 +11,7 @@
  */
 
 #include <linux/io.h>
+#include <linux/clk.h>
 #include <linux/gpio.h>
 #include <linux/module.h>
 #include <linux/of.h>
@@ -30,482 +31,856 @@
 static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA0,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "emac"),		/* ERXD3 */
+		SUNXI_FUNCTION(0x3, "spi1"),		/* CS0 */
+		SUNXI_FUNCTION(0x4, "uart2")),		/* RTS */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA1,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "emac"),		/* ERXD2 */
+		SUNXI_FUNCTION(0x3, "spi1"),		/* CLK */
+		SUNXI_FUNCTION(0x4, "uart2")),		/* CTS */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA2,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "emac"),		/* ERXD1 */
+		SUNXI_FUNCTION(0x3, "spi1"),		/* MOSI */
+		SUNXI_FUNCTION(0x4, "uart2")),		/* TX */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA3,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "emac"),		/* ERXD0 */
+		SUNXI_FUNCTION(0x3, "spi1"),		/* MISO */
+		SUNXI_FUNCTION(0x4, "uart2")),		/* RX */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA4,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "emac"),		/* ETXD3 */
+		SUNXI_FUNCTION(0x3, "spi1")),		/* CS1 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA5,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "emac"),		/* ETXD2 */
+		SUNXI_FUNCTION(0x3, "spi3")),		/* CS0 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA6,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "emac"),		/* ETXD1 */
+		SUNXI_FUNCTION(0x3, "spi3")),		/* CLK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA7,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "emac"),		/* ETXD0 */
+		SUNXI_FUNCTION(0x3, "spi3")),		/* MOSI */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA8,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "emac"),		/* ERXCK */
+		SUNXI_FUNCTION(0x3, "spi3")),		/* MISO */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA9,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "emac"),		/* ERXERR */
+		SUNXI_FUNCTION(0x3, "spi3")),		/* CS1 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA10,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
 		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "emac"),		/* ERXDV */
 		SUNXI_FUNCTION(0x4, "uart1")),		/* TX */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA11,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
 		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "emac"),		/* EMDC */
 		SUNXI_FUNCTION(0x4, "uart1")),		/* RX */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA12,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
 		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "emac"),		/* EMDIO */
+		SUNXI_FUNCTION(0x3, "uart6"),		/* TX */
 		SUNXI_FUNCTION(0x4, "uart1")),		/* RTS */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA13,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
 		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "emac"),		/* ETXEN */
+		SUNXI_FUNCTION(0x3, "uart6"),		/* RX */
 		SUNXI_FUNCTION(0x4, "uart1")),		/* CTS */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA14,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
 		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "emac"),		/* ETXCK */
+		SUNXI_FUNCTION(0x3, "uart7"),		/* TX */
 		SUNXI_FUNCTION(0x4, "uart1")),		/* DTR */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA15,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
 		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "emac"),		/* ECRS */
+		SUNXI_FUNCTION(0x3, "uart7"),		/* RX */
 		SUNXI_FUNCTION(0x4, "uart1")),		/* DSR */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA16,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
 		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "emac"),		/* ECOL */
+		SUNXI_FUNCTION(0x3, "can"),		/* TX */
 		SUNXI_FUNCTION(0x4, "uart1")),		/* DCD */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA17,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
 		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "emac"),		/* ETXERR */
+		SUNXI_FUNCTION(0x3, "can"),		/* RX */
 		SUNXI_FUNCTION(0x4, "uart1")),		/* RING */
 	/* Hole */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB0,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "i2c0")),		/* SCK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB1,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "i2c0")),		/* SDA */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB2,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "pwm")),		/* PWM0 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB3,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ir0")),		/* TX */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB4,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ir0")),		/* RX */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB5,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "i2s"),		/* MCLK */
+		SUNXI_FUNCTION(0x3, "ac97")),		/* MCLK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB6,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "i2s"),		/* BCLK */
+		SUNXI_FUNCTION(0x3, "ac97")),		/* BCLK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB7,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "i2s"),		/* LRCK */
+		SUNXI_FUNCTION(0x3, "ac97")),		/* SYNC */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB8,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "i2s"),		/* DO0 */
+		SUNXI_FUNCTION(0x3, "ac97")),		/* DO */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB9,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "i2s")),		/* DO1 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB10,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "i2s")),		/* DO2 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB11,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "i2s")),		/* DO3 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB12,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "i2s"),		/* DI */
+		SUNXI_FUNCTION(0x3, "ac97")),		/* DI */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB13,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "spi2")),		/* CS1 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB14,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "spi2"),		/* CS0 */
+		SUNXI_FUNCTION(0x3, "jtag")),		/* MS0 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB15,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "spi2"),		/* CLK */
+		SUNXI_FUNCTION(0x3, "jtag")),		/* CK0 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB16,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "spi2"),		/* MOSI */
+		SUNXI_FUNCTION(0x3, "jtag")),		/* DO0 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB17,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "spi2"),		/* MISO */
+		SUNXI_FUNCTION(0x3, "jtag")),		/* DI0 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB18,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "i2c1")),		/* SCK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB19,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "i2c1")),		/* SDA */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB20,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "i2c2")),		/* SCK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB21,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "i2c2")),		/* SDA */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB22,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
 		SUNXI_FUNCTION(0x1, "gpio_out"),
-		SUNXI_FUNCTION(0x2, "uart0")),		/* TX */
+		SUNXI_FUNCTION(0x2, "uart0"),		/* TX */
+		SUNXI_FUNCTION(0x3, "ir1")),		/* TX */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB23,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
 		SUNXI_FUNCTION(0x1, "gpio_out"),
-		SUNXI_FUNCTION(0x2, "uart0")),		/* RX */
+		SUNXI_FUNCTION(0x2, "uart0"),		/* RX */
+		SUNXI_FUNCTION(0x3, "ir1")),		/* RX */
 	/* Hole */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC0,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NWE */
+		SUNXI_FUNCTION(0x3, "spi0")),		/* MOSI */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC1,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NALE */
+		SUNXI_FUNCTION(0x3, "spi0")),		/* MISO */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC2,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NCLE */
+		SUNXI_FUNCTION(0x3, "spi0")),		/* SCK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC3,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0")),		/* NCE1 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC4,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0")),		/* NCE0 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC5,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0")),	/* NRE# */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC6,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NRB0 */
+		SUNXI_FUNCTION(0x3, "mmc2")),		/* CMD */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC7,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NRB1 */
+		SUNXI_FUNCTION(0x3, "mmc2")),		/* CLK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC8,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ0 */
+		SUNXI_FUNCTION(0x3, "mmc2")),		/* D0 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC9,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ1 */
+		SUNXI_FUNCTION(0x3, "mmc2")),		/* D1 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC10,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ2 */
+		SUNXI_FUNCTION(0x3, "mmc2")),		/* D2 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC11,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ3 */
+		SUNXI_FUNCTION(0x3, "mmc2")),		/* D3 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC12,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0")),		/* NDQ4 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC13,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0")),		/* NDQ5 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC14,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0")),		/* NDQ6 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC15,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0")),		/* NDQ7 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC16,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0")),		/* NWP */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC17,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0")),		/* NCE2 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC18,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0")),		/* NCE3 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC19,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NCE4 */
+		SUNXI_FUNCTION(0x3, "spi2")),		/* CS0 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC20,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NCE5 */
+		SUNXI_FUNCTION(0x3, "spi2")),		/* CLK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC21,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NCE6 */
+		SUNXI_FUNCTION(0x3, "spi2")),		/* MOSI */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC22,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NCE7 */
+		SUNXI_FUNCTION(0x3, "spi2")),		/* MISO */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC23,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x3, "spi0")),		/* CS0 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC24,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0")),		/* NDQS */
 	/* Hole */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD0,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* D0 */
+		SUNXI_FUNCTION(0x3, "lvds0")),		/* VP0 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD1,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* D1 */
+		SUNXI_FUNCTION(0x3, "lvds0")),		/* VN0 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD2,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* D2 */
+		SUNXI_FUNCTION(0x3, "lvds0")),		/* VP1 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD3,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* D3 */
+		SUNXI_FUNCTION(0x3, "lvds0")),		/* VN1 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD4,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* D4 */
+		SUNXI_FUNCTION(0x3, "lvds0")),		/* VP2 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD5,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* D5 */
+		SUNXI_FUNCTION(0x3, "lvds0")),		/* VN2 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD6,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* D6 */
+		SUNXI_FUNCTION(0x3, "lvds0")),		/* VPC */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD7,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* D7 */
+		SUNXI_FUNCTION(0x3, "lvds0")),		/* VNC */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD8,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* D8 */
+		SUNXI_FUNCTION(0x3, "lvds0")),		/* VP3 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD9,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* D9 */
+		SUNXI_FUNCTION(0x3, "lvds0")),		/* VM3 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD10,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* D10 */
+		SUNXI_FUNCTION(0x3, "lvds1")),		/* VP0 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD11,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* D11 */
+		SUNXI_FUNCTION(0x3, "lvds1")),		/* VN0 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD12,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* D12 */
+		SUNXI_FUNCTION(0x3, "lvds1")),		/* VP1 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD13,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* D13 */
+		SUNXI_FUNCTION(0x3, "lvds1")),		/* VN1 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD14,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* D14 */
+		SUNXI_FUNCTION(0x3, "lvds1")),		/* VP2 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD15,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* D15 */
+		SUNXI_FUNCTION(0x3, "lvds1")),		/* VN2 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD16,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* D16 */
+		SUNXI_FUNCTION(0x3, "lvds1")),		/* VPC */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD17,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* D17 */
+		SUNXI_FUNCTION(0x3, "lvds1")),		/* VNC */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD18,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* D18 */
+		SUNXI_FUNCTION(0x3, "lvds1")),		/* VP3 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD19,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* D19 */
+		SUNXI_FUNCTION(0x3, "lvds1")),		/* VN3 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD20,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* D20 */
+		SUNXI_FUNCTION(0x3, "csi1")),		/* MCLK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD21,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* D21 */
+		SUNXI_FUNCTION(0x3, "sim")),		/* VPPEN */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD22,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* D22 */
+		SUNXI_FUNCTION(0x3, "sim")),		/* VPPPP */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD23,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* D23 */
+		SUNXI_FUNCTION(0x3, "sim")),		/* DET */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD24,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* CLK */
+		SUNXI_FUNCTION(0x3, "sim")),		/* VCCEN */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD25,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* DE */
+		SUNXI_FUNCTION(0x3, "sim")),		/* RST */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD26,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* HSYNC */
+		SUNXI_FUNCTION(0x3, "sim")),		/* SCK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD27,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0"),		/* VSYNC */
+		SUNXI_FUNCTION(0x3, "sim")),		/* SDA */
 	/* Hole */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE0,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ts0"),		/* CLK */
+		SUNXI_FUNCTION(0x3, "csi0")),		/* PCK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE1,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ts0"),		/* ERR */
+		SUNXI_FUNCTION(0x3, "csi0")),		/* CK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE2,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ts0"),		/* SYNC */
+		SUNXI_FUNCTION(0x3, "csi0")),		/* HSYNC */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE3,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ts0"),		/* DVLD */
+		SUNXI_FUNCTION(0x3, "csi0")),		/* VSYNC */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE4,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ts0"),		/* D0 */
+		SUNXI_FUNCTION(0x3, "csi0")),		/* D0 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE5,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ts0"),		/* D1 */
+		SUNXI_FUNCTION(0x3, "csi0"),		/* D1 */
+		SUNXI_FUNCTION(0x4, "sim")),		/* VPPEN */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE6,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ts0"),		/* D2 */
+		SUNXI_FUNCTION(0x3, "csi0")),		/* D2 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE7,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ts0"),		/* D3 */
+		SUNXI_FUNCTION(0x3, "csi0")),		/* D3 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE8,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ts0"),		/* D4 */
+		SUNXI_FUNCTION(0x3, "csi0")),		/* D4 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE9,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ts0"),		/* D5 */
+		SUNXI_FUNCTION(0x3, "csi0")),		/* D5 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE10,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ts0"),		/* D6 */
+		SUNXI_FUNCTION(0x3, "csi0")),		/* D6 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE11,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ts0"),		/* D7 */
+		SUNXI_FUNCTION(0x3, "csi0")),		/* D7 */
 	/* Hole */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF0,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "mmc0"),		/* D1 */
+		SUNXI_FUNCTION(0x4, "jtag")),		/* MSI */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF1,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "mmc0"),		/* D0 */
+		SUNXI_FUNCTION(0x4, "jtag")),		/* DI1 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF2,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
 		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "mmc0"),		/* CLK */
 		SUNXI_FUNCTION(0x4, "uart0")),		/* TX */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF3,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "mmc0"),		/* CMD */
+		SUNXI_FUNCTION(0x4, "jtag")),		/* DO1 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF4,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
 		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "mmc0"),		/* D3 */
 		SUNXI_FUNCTION(0x4, "uart0")),		/* RX */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF5,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "mmc0"),		/* D2 */
+		SUNXI_FUNCTION(0x4, "jtag")),		/* CK1 */
 	/* Hole */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG0,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ts1"),		/* CLK */
+		SUNXI_FUNCTION(0x3, "csi1"),		/* PCK */
+		SUNXI_FUNCTION(0x4, "mmc1")),		/* CMD */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG1,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ts1"),		/* ERR */
+		SUNXI_FUNCTION(0x3, "csi1"),		/* CK */
+		SUNXI_FUNCTION(0x4, "mmc1")),		/* CLK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG2,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ts1"),		/* SYNC */
+		SUNXI_FUNCTION(0x3, "csi1"),		/* HSYNC */
+		SUNXI_FUNCTION(0x4, "mmc1")),		/* D0 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG3,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ts1"),		/* DVLD */
+		SUNXI_FUNCTION(0x3, "csi1"),		/* VSYNC */
+		SUNXI_FUNCTION(0x4, "mmc1")),		/* D1 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG4,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ts1"),		/* D0 */
+		SUNXI_FUNCTION(0x3, "csi1"),		/* D0 */
+		SUNXI_FUNCTION(0x4, "mmc1"),		/* D2 */
+		SUNXI_FUNCTION(0x5, "csi0")),		/* D8 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG5,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ts1"),		/* D1 */
+		SUNXI_FUNCTION(0x3, "csi1"),		/* D1 */
+		SUNXI_FUNCTION(0x4, "mmc1"),		/* D3 */
+		SUNXI_FUNCTION(0x5, "csi0")),		/* D9 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG6,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ts1"),		/* D2 */
+		SUNXI_FUNCTION(0x3, "csi1"),		/* D2 */
+		SUNXI_FUNCTION(0x4, "uart3"),		/* TX */
+		SUNXI_FUNCTION(0x5, "csi0")),		/* D10 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG7,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ts1"),		/* D3 */
+		SUNXI_FUNCTION(0x3, "csi1"),		/* D3 */
+		SUNXI_FUNCTION(0x4, "uart3"),		/* RX */
+		SUNXI_FUNCTION(0x5, "csi0")),		/* D11 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG8,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ts1"),		/* D4 */
+		SUNXI_FUNCTION(0x3, "csi1"),		/* D4 */
+		SUNXI_FUNCTION(0x4, "uart3"),		/* RTS */
+		SUNXI_FUNCTION(0x5, "csi0")),		/* D12 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG9,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ts1"),		/* D5 */
+		SUNXI_FUNCTION(0x3, "csi1"),		/* D5 */
+		SUNXI_FUNCTION(0x4, "uart3"),		/* CTS */
+		SUNXI_FUNCTION(0x5, "csi0")),		/* D13 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG10,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ts1"),		/* D6 */
+		SUNXI_FUNCTION(0x3, "csi1"),		/* D6 */
+		SUNXI_FUNCTION(0x4, "uart4"),		/* TX */
+		SUNXI_FUNCTION(0x5, "csi0")),		/* D14 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG11,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ts1"),		/* D7 */
+		SUNXI_FUNCTION(0x3, "csi1"),		/* D7 */
+		SUNXI_FUNCTION(0x4, "uart4"),		/* RX */
+		SUNXI_FUNCTION(0x5, "csi0")),		/* D15 */
 	/* Hole */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH0,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* D0 */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATAA0 */
+		SUNXI_FUNCTION(0x4, "uart3"),		/* TX */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* D0 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH1,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* D1 */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATAA1 */
+		SUNXI_FUNCTION(0x4, "uart3"),		/* RX */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* D1 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH2,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* D2 */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATAA2 */
+		SUNXI_FUNCTION(0x4, "uart3"),		/* RTS */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* D2 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH3,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* D3 */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATAIRQ */
+		SUNXI_FUNCTION(0x4, "uart3"),		/* CTS */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* D3 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH4,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* D4 */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATAD0 */
+		SUNXI_FUNCTION(0x4, "uart4"),		/* TX */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* D4 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH5,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* D5 */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATAD1 */
+		SUNXI_FUNCTION(0x4, "uart4"),		/* RX */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* D5 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH6,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* D6 */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATAD2 */
+		SUNXI_FUNCTION(0x4, "uart5"),		/* TX */
+		SUNXI_FUNCTION(0x5, "ms"),		/* BS */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* D6 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH7,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* D7 */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATAD3 */
+		SUNXI_FUNCTION(0x4, "uart5"),		/* RX */
+		SUNXI_FUNCTION(0x5, "ms"),		/* CLK */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* D7 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH8,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* D8 */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATAD4 */
+		SUNXI_FUNCTION(0x4, "keypad"),		/* IN0 */
+		SUNXI_FUNCTION(0x5, "ms"),		/* D0 */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* D8 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH9,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* D9 */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATAD5 */
+		SUNXI_FUNCTION(0x4, "keypad"),		/* IN1 */
+		SUNXI_FUNCTION(0x5, "ms"),		/* D1 */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* D9 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH10,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* D10 */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATAD6 */
+		SUNXI_FUNCTION(0x4, "keypad"),		/* IN2 */
+		SUNXI_FUNCTION(0x5, "ms"),		/* D2 */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* D10 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH11,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* D11 */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATAD7 */
+		SUNXI_FUNCTION(0x4, "keypad"),		/* IN3 */
+		SUNXI_FUNCTION(0x5, "ms"),		/* D3 */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* D11 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH12,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* D12 */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATAD8 */
+		SUNXI_FUNCTION(0x4, "ps2"),		/* SCK1 */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* D12 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH13,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* D13 */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATAD9 */
+		SUNXI_FUNCTION(0x4, "ps2"),		/* SDA1 */
+		SUNXI_FUNCTION(0x5, "sim"),		/* RST */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* D13 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH14,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* D14 */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATAD10 */
+		SUNXI_FUNCTION(0x4, "keypad"),		/* IN4 */
+		SUNXI_FUNCTION(0x5, "sim"),		/* VPPEN */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* D14 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH15,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* D15 */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATAD11 */
+		SUNXI_FUNCTION(0x4, "keypad"),		/* IN5 */
+		SUNXI_FUNCTION(0x5, "sim"),		/* VPPPP */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* D15 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH16,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* D16 */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATAD12 */
+		SUNXI_FUNCTION(0x4, "keypad"),		/* IN6 */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* D16 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH17,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* D17 */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATAD13 */
+		SUNXI_FUNCTION(0x4, "keypad"),		/* IN7 */
+		SUNXI_FUNCTION(0x5, "sim"),		/* VCCEN */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* D17 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH18,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* D18 */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATAD14 */
+		SUNXI_FUNCTION(0x4, "keypad"),		/* OUT0 */
+		SUNXI_FUNCTION(0x5, "sim"),		/* SCK */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* D18 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH19,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* D19 */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATAD15 */
+		SUNXI_FUNCTION(0x4, "keypad"),		/* OUT1 */
+		SUNXI_FUNCTION(0x5, "sim"),		/* SDA */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* D19 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH20,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* D20 */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATAOE */
+		SUNXI_FUNCTION(0x4, "can"),		/* TX */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* D20 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH21,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* D21 */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATADREQ */
+		SUNXI_FUNCTION(0x4, "can"),		/* RX */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* D21 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH22,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* D22 */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATADACK */
+		SUNXI_FUNCTION(0x4, "keypad"),		/* OUT2 */
+		SUNXI_FUNCTION(0x5, "mmc1"),		/* CMD */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* D22 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH23,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* D23 */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATACS0 */
+		SUNXI_FUNCTION(0x4, "keypad"),		/* OUT3 */
+		SUNXI_FUNCTION(0x5, "mmc1"),		/* CLK */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* D23 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH24,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* CLK */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATACS1 */
+		SUNXI_FUNCTION(0x4, "keypad"),		/* OUT4 */
+		SUNXI_FUNCTION(0x5, "mmc1"),		/* D0 */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* PCLK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH25,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* DE */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATAIORDY */
+		SUNXI_FUNCTION(0x4, "keypad"),		/* OUT5 */
+		SUNXI_FUNCTION(0x5, "mmc1"),		/* D1 */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* FIELD */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH26,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* HSYNC */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATAIOR */
+		SUNXI_FUNCTION(0x4, "keypad"),		/* OUT6 */
+		SUNXI_FUNCTION(0x5, "mmc1"),		/* D2 */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* HSYNC */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH27,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd1"),		/* VSYNC */
+		SUNXI_FUNCTION(0x3, "pata"),		/* ATAIOW */
+		SUNXI_FUNCTION(0x4, "keypad"),		/* OUT7 */
+		SUNXI_FUNCTION(0x5, "mmc1"),		/* D3 */
+		SUNXI_FUNCTION(0x7, "csi1")),		/* VSYNC */
 	/* Hole */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI0,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
@@ -518,277 +893,401 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		SUNXI_FUNCTION(0x1, "gpio_out")),
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI3,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "pwm")),		/* PWM1 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI4,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "mmc3")),		/* CMD */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI5,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "mmc3")),		/* CLK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI6,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "mmc3")),		/* D0 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI7,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "mmc3")),		/* D1 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI8,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "mmc3")),		/* D2 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI9,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "mmc3")),		/* D3 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI10,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "spi0"),		/* CS0 */
+		SUNXI_FUNCTION(0x3, "uart5")),		/* TX */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI11,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "spi0"),		/* CLK */
+		SUNXI_FUNCTION(0x3, "uart5")),		/* RX */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI12,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "spi0"),		/* MOSI */
+		SUNXI_FUNCTION(0x3, "uart6")),		/* TX */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI13,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "spi0"),		/* MISO */
+		SUNXI_FUNCTION(0x3, "uart6")),		/* RX */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI14,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "spi0"),		/* CS1 */
+		SUNXI_FUNCTION(0x3, "ps2"),		/* SCK1 */
+		SUNXI_FUNCTION(0x4, "timer4")),		/* TCLKIN0 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI15,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "spi1"),		/* CS1 */
+		SUNXI_FUNCTION(0x3, "ps2"),		/* SDA1 */
+		SUNXI_FUNCTION(0x4, "timer5")),		/* TCLKIN1 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI16,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "spi1"),		/* CS0 */
+		SUNXI_FUNCTION(0x3, "uart2")),		/* RTS */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI17,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "spi1"),		/* CLK */
+		SUNXI_FUNCTION(0x3, "uart2")),		/* CTS */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI18,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "spi1"),		/* MOSI */
+		SUNXI_FUNCTION(0x3, "uart2")),		/* TX */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI19,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "spi1"),		/* MISO */
+		SUNXI_FUNCTION(0x3, "uart2")),		/* RX */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI20,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ps2"),		/* SCK0 */
+		SUNXI_FUNCTION(0x3, "uart7"),		/* TX */
+		SUNXI_FUNCTION(0x4, "hdmi")),		/* HSCL */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI21,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ps2"),		/* SDA0 */
+		SUNXI_FUNCTION(0x3, "uart7"),		/* RX */
+		SUNXI_FUNCTION(0x4, "hdmi")),		/* HSDA */
 };
 
 static const struct sunxi_desc_pin sun5i_a13_pins[] = {
 	/* Hole */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB0,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "i2c0")),		/* SCK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB1,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "i2c0")),		/* SDA */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB2,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "pwm")),
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB3,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ir0")),		/* TX */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB4,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "ir0")),		/* RX */
 	/* Hole */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB10,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "spi2")),		/* CS1 */
 	/* Hole */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB15,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "i2c1")),		/* SCK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB16,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "i2c1")),		/* SDA */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB17,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "i2c2")),		/* SCK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB18,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "i2c2")),		/* SDA */
 	/* Hole */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC0,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NWE */
+		SUNXI_FUNCTION(0x3, "spi0")),		/* MOSI */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC1,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NALE */
+		SUNXI_FUNCTION(0x3, "spi0")),		/* MISO */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC2,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NCLE */
+		SUNXI_FUNCTION(0x3, "spi0")),		/* CLK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC3,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NCE1 */
+		SUNXI_FUNCTION(0x3, "spi0")),		/* CS0 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC4,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0")),		/* NCE0 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC5,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0")),		/* NRE */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC6,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NRB0 */
+		SUNXI_FUNCTION(0x3, "mmc2")),		/* CMD */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC7,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NRB1 */
+		SUNXI_FUNCTION(0x3, "mmc2")),		/* CLK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC8,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ0 */
+		SUNXI_FUNCTION(0x3, "mmc2")),		/* D0 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC9,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ1 */
+		SUNXI_FUNCTION(0x3, "mmc2")),		/* D1 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC10,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ2 */
+		SUNXI_FUNCTION(0x3, "mmc2")),		/* D2 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC11,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ3 */
+		SUNXI_FUNCTION(0x3, "mmc2")),		/* D3 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC12,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ4 */
+		SUNXI_FUNCTION(0x3, "mmc2")),		/* D4 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC13,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ5 */
+		SUNXI_FUNCTION(0x3, "mmc2")),		/* D5 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC14,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ6 */
+		SUNXI_FUNCTION(0x3, "mmc2")),		/* D6 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC15,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ7 */
+		SUNXI_FUNCTION(0x3, "mmc2")),		/* D7 */
 	/* Hole */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC19,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "nand0"),		/* NDQS */
+		SUNXI_FUNCTION(0x4, "uart3")),		/* RTS */
 	/* Hole */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD2,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0")),		/* D2 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD3,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0")),		/* D3 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD4,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0")),		/* D4 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD5,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0")),		/* D5 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD6,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0")),		/* D6 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD7,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0")),		/* D7 */
 	/* Hole */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD10,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0")),		/* D10 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD11,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0")),		/* D11 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD12,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0")),		/* D12 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD13,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0")),		/* D13 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD14,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0")),		/* D14 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD15,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0")),		/* D15 */
 	/* Hole */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD18,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0")),		/* D18 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD19,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0")),		/* D19 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD20,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0")),		/* D20 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD21,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0")),		/* D21 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD22,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0")),		/* D22 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD23,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0")),		/* D23 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD24,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0")),		/* CLK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD25,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0")),		/* DE */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD26,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0")),		/* HSYNC */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD27,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "lcd0")),		/* VSYNC */
 	/* Hole */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE0,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x3, "csi0"),		/* PCLK */
+		SUNXI_FUNCTION(0x4, "spi2")),		/* CS0 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE1,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x3, "csi0"),		/* MCLK */
+		SUNXI_FUNCTION(0x4, "spi2")),		/* CLK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE2,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x3, "csi0"),		/* HSYNC */
+		SUNXI_FUNCTION(0x4, "spi2")),		/* MOSI */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE3,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x3, "csi0"),		/* VSYNC */
+		SUNXI_FUNCTION(0x4, "spi2")),		/* MISO */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE4,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x3, "csi0"),		/* D0 */
+		SUNXI_FUNCTION(0x4, "mmc2")),		/* D0 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE5,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x3, "csi0"),		/* D1 */
+		SUNXI_FUNCTION(0x4, "mmc2")),		/* D1 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE6,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x3, "csi0"),		/* D2 */
+		SUNXI_FUNCTION(0x4, "mmc2")),		/* D2 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE7,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x3, "csi0"),		/* D3 */
+		SUNXI_FUNCTION(0x4, "mmc2")),		/* D3 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE8,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x3, "csi0"),		/* D4 */
+		SUNXI_FUNCTION(0x4, "mmc2")),		/* CMD */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE9,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x3, "csi0"),		/* D5 */
+		SUNXI_FUNCTION(0x4, "mmc2")),		/* CLK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE10,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
 		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x3, "csi0"),		/* D6 */
 		SUNXI_FUNCTION(0x4, "uart1")),		/* TX */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE11,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
 		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x3, "csi0"),		/* D7 */
 		SUNXI_FUNCTION(0x4, "uart1")),		/* RX */
 	/* Hole */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF0,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x4, "mmc0")),		/* D1 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF1,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x4, "mmc0")),		/* D0 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF2,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x4, "mmc0")),		/* CLK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF3,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x4, "mmc0")),		/* CMD */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF4,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x4, "mmc0")),		/* D3 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF5,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x4, "mmc0")),		/* D2 */
 	/* Hole */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG0,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
@@ -802,24 +1301,34 @@ static const struct sunxi_desc_pin sun5i_a13_pins[] = {
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG3,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
 		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "mmc1"),		/* CMD */
 		SUNXI_FUNCTION(0x4, "uart1")),		/* TX */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG4,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
 		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "mmc1"),		/* CLK */
 		SUNXI_FUNCTION(0x4, "uart1")),		/* RX */
-	/* Hole */
+/* Hole */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG9,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "spi1"),		/* CS0 */
+		SUNXI_FUNCTION(0x3, "uart3")),		/* TX */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG10,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "spi1"),		/* CLK */
+		SUNXI_FUNCTION(0x3, "uart3")),		/* RX */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG11,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "spi1"),		/* MOSI */
+		SUNXI_FUNCTION(0x3, "uart3")),		/* CTS */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG12,
 		SUNXI_FUNCTION(0x0, "gpio_in"),
-		SUNXI_FUNCTION(0x1, "gpio_out")),
+		SUNXI_FUNCTION(0x1, "gpio_out"),
+		SUNXI_FUNCTION(0x2, "spi1"),		/* MISO */
+		SUNXI_FUNCTION(0x3, "uart3")),		/* RTS */
 };
 
 static const struct sunxi_pinctrl_desc sun4i_a10_pinctrl_data = {
@@ -1029,7 +1538,7 @@ static void sunxi_pctrl_dt_free_map(struct pinctrl_dev *pctldev,
 	kfree(map);
 }
 
-static struct pinctrl_ops sunxi_pctrl_ops = {
+static const struct pinctrl_ops sunxi_pctrl_ops = {
 	.dt_node_to_map		= sunxi_pctrl_dt_node_to_map,
 	.dt_free_map		= sunxi_pctrl_dt_free_map,
 	.get_groups_count	= sunxi_pctrl_get_groups_count,
@@ -1098,7 +1607,7 @@ static int sunxi_pconf_group_set(struct pinctrl_dev *pctldev,
 	return 0;
 }
 
-static struct pinconf_ops sunxi_pconf_ops = {
+static const struct pinconf_ops sunxi_pconf_ops = {
 	.pin_config_group_get	= sunxi_pconf_group_get,
 	.pin_config_group_set	= sunxi_pconf_group_set,
 };
@@ -1204,7 +1713,7 @@ error:
 	return ret;
 }
 
-static struct pinmux_ops sunxi_pmx_ops = {
+static const struct pinmux_ops sunxi_pmx_ops = {
 	.get_functions_count	= sunxi_pmx_get_funcs_cnt,
 	.get_function_name	= sunxi_pmx_get_func_name,
 	.get_function_groups	= sunxi_pmx_get_func_groups,
@@ -1409,6 +1918,7 @@ static int sunxi_pinctrl_probe(struct platform_device *pdev)
 	struct pinctrl_pin_desc *pins;
 	struct sunxi_pinctrl *pctl;
 	int i, ret, last_pin;
+	struct clk *clk;
 
 	pctl = devm_kzalloc(&pdev->dev, sizeof(*pctl), GFP_KERNEL);
 	if (!pctl)
@@ -1479,6 +1989,12 @@ static int sunxi_pinctrl_probe(struct platform_device *pdev)
 			goto gpiochip_error;
 	}
 
+	clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(clk))
+		goto gpiochip_error;
+
+	clk_prepare_enable(clk);
+
 	dev_info(&pdev->dev, "initialized sunXi PIO driver\n");
 
 	return 0;
diff --git a/drivers/pinctrl/pinctrl-tegra.c b/drivers/pinctrl/pinctrl-tegra.c
index f195d77a357..2fa9bc6cd7a 100644
--- a/drivers/pinctrl/pinctrl-tegra.c
+++ b/drivers/pinctrl/pinctrl-tegra.c
@@ -316,7 +316,7 @@ static int tegra_pinctrl_dt_node_to_map(struct pinctrl_dev *pctldev,
 	return 0;
 }
 
-static struct pinctrl_ops tegra_pinctrl_ops = {
+static const struct pinctrl_ops tegra_pinctrl_ops = {
 	.get_groups_count = tegra_pinctrl_get_groups_count,
 	.get_group_name = tegra_pinctrl_get_group_name,
 	.get_group_pins = tegra_pinctrl_get_group_pins,
@@ -401,7 +401,7 @@ static void tegra_pinctrl_disable(struct pinctrl_dev *pctldev,
 	pmx_writel(pmx, val, g->mux_bank, g->mux_reg);
 }
 
-static struct pinmux_ops tegra_pinmux_ops = {
+static const struct pinmux_ops tegra_pinmux_ops = {
 	.get_functions_count = tegra_pinctrl_get_funcs_count,
 	.get_function_name = tegra_pinctrl_get_func_name,
 	.get_function_groups = tegra_pinctrl_get_func_groups,
@@ -676,7 +676,7 @@ static void tegra_pinconf_config_dbg_show(struct pinctrl_dev *pctldev,
 }
 #endif
 
-static struct pinconf_ops tegra_pinconf_ops = {
+static const struct pinconf_ops tegra_pinconf_ops = {
 	.pin_config_get = tegra_pinconf_get,
 	.pin_config_set = tegra_pinconf_set,
 	.pin_config_group_get = tegra_pinconf_group_get,
diff --git a/drivers/pinctrl/pinctrl-u300.c b/drivers/pinctrl/pinctrl-u300.c
index 2b577255083..6a3a7503e6a 100644
--- a/drivers/pinctrl/pinctrl-u300.c
+++ b/drivers/pinctrl/pinctrl-u300.c
@@ -860,7 +860,7 @@ static void u300_pin_dbg_show(struct pinctrl_dev *pctldev, struct seq_file *s,
 	seq_printf(s, " " DRIVER_NAME);
 }
 
-static struct pinctrl_ops u300_pctrl_ops = {
+static const struct pinctrl_ops u300_pctrl_ops = {
 	.get_groups_count = u300_get_groups_count,
 	.get_group_name = u300_get_group_name,
 	.get_group_pins = u300_get_group_pins,
@@ -1003,7 +1003,7 @@ static int u300_pmx_get_groups(struct pinctrl_dev *pctldev, unsigned selector,
 	return 0;
 }
 
-static struct pinmux_ops u300_pmx_ops = {
+static const struct pinmux_ops u300_pmx_ops = {
 	.get_functions_count = u300_pmx_get_funcs_count,
 	.get_function_name = u300_pmx_get_func_name,
 	.get_function_groups = u300_pmx_get_groups,
@@ -1046,7 +1046,7 @@ static int u300_pin_config_set(struct pinctrl_dev *pctldev, unsigned pin,
 	return 0;
 }
 
-static struct pinconf_ops u300_pconf_ops = {
+static const struct pinconf_ops u300_pconf_ops = {
 	.is_generic = true,
 	.pin_config_get = u300_pin_config_get,
 	.pin_config_set = u300_pin_config_set,
diff --git a/drivers/pinctrl/pinctrl-xway.c b/drivers/pinctrl/pinctrl-xway.c
index 068224efa6f..f2977cff836 100644
--- a/drivers/pinctrl/pinctrl-xway.c
+++ b/drivers/pinctrl/pinctrl-xway.c
@@ -553,7 +553,7 @@ int xway_pinconf_group_set(struct pinctrl_dev *pctldev,
 	return ret;
 }
 
-static struct pinconf_ops xway_pinconf_ops = {
+static const struct pinconf_ops xway_pinconf_ops = {
 	.pin_config_get	= xway_pinconf_get,
 	.pin_config_set	= xway_pinconf_set,
 	.pin_config_group_set = xway_pinconf_group_set,
diff --git a/drivers/pinctrl/pinmux.c b/drivers/pinctrl/pinmux.c
index 1a00658b3ea..88cc5095d0c 100644
--- a/drivers/pinctrl/pinmux.c
+++ b/drivers/pinctrl/pinmux.c
@@ -194,6 +194,11 @@ static const char *pin_free(struct pinctrl_dev *pctldev, int pin,
 	}
 
 	if (!gpio_range) {
+		/*
+		 * A pin should not be freed more times than allocated.
+		 */
+		if (WARN_ON(!desc->mux_usecount))
+			return NULL;
 		desc->mux_usecount--;
 		if (desc->mux_usecount)
 			return NULL;
@@ -501,7 +506,7 @@ static int pinmux_functions_show(struct seq_file *s, void *what)
 	if (!pmxops)
 		return 0;
 
-	mutex_lock(&pinctrl_mutex);
+	mutex_lock(&pctldev->mutex);
 	nfuncs = pmxops->get_functions_count(pctldev);
 	while (func_selector < nfuncs) {
 		const char *func = pmxops->get_function_name(pctldev,
@@ -525,7 +530,7 @@ static int pinmux_functions_show(struct seq_file *s, void *what)
 		func_selector++;
 	}
 
-	mutex_unlock(&pinctrl_mutex);
+	mutex_unlock(&pctldev->mutex);
 
 	return 0;
 }
@@ -543,7 +548,7 @@ static int pinmux_pins_show(struct seq_file *s, void *what)
 	seq_puts(s, "Pinmux settings per pin\n");
 	seq_puts(s, "Format: pin (name): mux_owner gpio_owner hog?\n");
 
-	mutex_lock(&pinctrl_mutex);
+	mutex_lock(&pctldev->mutex);
 
 	/* The pin number can be retrived from the pin controller descriptor */
 	for (i = 0; i < pctldev->desc->npins; i++) {
@@ -578,7 +583,7 @@ static int pinmux_pins_show(struct seq_file *s, void *what)
 			seq_printf(s, "\n");
 	}
 
-	mutex_unlock(&pinctrl_mutex);
+	mutex_unlock(&pctldev->mutex);
 
 	return 0;
 }
diff --git a/drivers/pinctrl/spear/pinctrl-spear.c b/drivers/pinctrl/spear/pinctrl-spear.c
index 6a7dae70db0..116da0412c4 100644
--- a/drivers/pinctrl/spear/pinctrl-spear.c
+++ b/drivers/pinctrl/spear/pinctrl-spear.c
@@ -198,7 +198,7 @@ static void spear_pinctrl_dt_free_map(struct pinctrl_dev *pctldev,
 	kfree(map);
 }
 
-static struct pinctrl_ops spear_pinctrl_ops = {
+static const struct pinctrl_ops spear_pinctrl_ops = {
 	.get_groups_count = spear_pinctrl_get_groups_cnt,
 	.get_group_name = spear_pinctrl_get_group_name,
 	.get_group_pins = spear_pinctrl_get_group_pins,
@@ -340,7 +340,7 @@ static void gpio_disable_free(struct pinctrl_dev *pctldev,
 	gpio_request_endisable(pctldev, range, offset, false);
 }
 
-static struct pinmux_ops spear_pinmux_ops = {
+static const struct pinmux_ops spear_pinmux_ops = {
 	.get_functions_count = spear_pinctrl_get_funcs_count,
 	.get_function_name = spear_pinctrl_get_func_name,
 	.get_function_groups = spear_pinctrl_get_func_groups,
diff --git a/drivers/platform/x86/hp-wmi.c b/drivers/platform/x86/hp-wmi.c
index 45cacf79f3a..1a779bbfb87 100644
--- a/drivers/platform/x86/hp-wmi.c
+++ b/drivers/platform/x86/hp-wmi.c
@@ -134,7 +134,6 @@ static const struct key_entry hp_wmi_keymap[] = {
 	{ KE_KEY, 0x2142, { KEY_MEDIA } },
 	{ KE_KEY, 0x213b, { KEY_INFO } },
 	{ KE_KEY, 0x2169, { KEY_DIRECTION } },
-	{ KE_KEY, 0x216a, { KEY_SETUP } },
 	{ KE_KEY, 0x231b, { KEY_HELP } },
 	{ KE_END, 0 }
 };
@@ -925,9 +924,6 @@ static int __init hp_wmi_init(void)
 		err = hp_wmi_input_setup();
 		if (err)
 			return err;
-		
-		//Enable magic for hotkeys that run on the SMBus
-		ec_write(0xe6,0x6e);
 	}
 
 	if (bios_capable) {
diff --git a/drivers/platform/x86/thinkpad_acpi.c b/drivers/platform/x86/thinkpad_acpi.c
index 9a907567f41..edec135b168 100644
--- a/drivers/platform/x86/thinkpad_acpi.c
+++ b/drivers/platform/x86/thinkpad_acpi.c
@@ -1964,9 +1964,6 @@ struct tp_nvram_state {
 /* kthread for the hotkey poller */
 static struct task_struct *tpacpi_hotkey_task;
 
-/* Acquired while the poller kthread is running, use to sync start/stop */
-static struct mutex hotkey_thread_mutex;
-
 /*
  * Acquire mutex to write poller control variables as an
  * atomic block.
@@ -2462,8 +2459,6 @@ static int hotkey_kthread(void *data)
 	unsigned int poll_freq;
 	bool was_frozen;
 
-	mutex_lock(&hotkey_thread_mutex);
-
 	if (tpacpi_lifecycle == TPACPI_LIFE_EXITING)
 		goto exit;
 
@@ -2523,7 +2518,6 @@ static int hotkey_kthread(void *data)
 	}
 
 exit:
-	mutex_unlock(&hotkey_thread_mutex);
 	return 0;
 }
 
@@ -2533,9 +2527,6 @@ static void hotkey_poll_stop_sync(void)
 	if (tpacpi_hotkey_task) {
 		kthread_stop(tpacpi_hotkey_task);
 		tpacpi_hotkey_task = NULL;
-		mutex_lock(&hotkey_thread_mutex);
-		/* at this point, the thread did exit */
-		mutex_unlock(&hotkey_thread_mutex);
 	}
 }
 
@@ -3234,7 +3225,6 @@ static int __init hotkey_init(struct ibm_init_struct *iibm)
 	mutex_init(&hotkey_mutex);
 
 #ifdef CONFIG_THINKPAD_ACPI_HOTKEY_POLL
-	mutex_init(&hotkey_thread_mutex);
 	mutex_init(&hotkey_thread_data_mutex);
 #endif
 
diff --git a/drivers/power/Kconfig b/drivers/power/Kconfig
index 339f802b91c..0d0b5d7d19d 100644
--- a/drivers/power/Kconfig
+++ b/drivers/power/Kconfig
@@ -254,7 +254,7 @@ config BATTERY_RX51
 
 config CHARGER_ISP1704
 	tristate "ISP1704 USB Charger Detection"
-	depends on USB_OTG_UTILS
+	depends on USB_PHY
 	help
 	  Say Y to enable support for USB Charger Detection with
 	  ISP1707/ISP1704 USB transceivers.
diff --git a/drivers/power/pda_power.c b/drivers/power/pda_power.c
index 7df7c5facc1..0c52e2a0d90 100644
--- a/drivers/power/pda_power.c
+++ b/drivers/power/pda_power.c
@@ -35,7 +35,7 @@ static struct timer_list supply_timer;
 static struct timer_list polling_timer;
 static int polling;
 
-#ifdef CONFIG_USB_OTG_UTILS
+#if IS_ENABLED(CONFIG_USB_PHY)
 static struct usb_phy *transceiver;
 static struct notifier_block otg_nb;
 #endif
@@ -218,7 +218,7 @@ static void polling_timer_func(unsigned long unused)
 		  jiffies + msecs_to_jiffies(pdata->polling_interval));
 }
 
-#ifdef CONFIG_USB_OTG_UTILS
+#if IS_ENABLED(CONFIG_USB_PHY)
 static int otg_is_usb_online(void)
 {
 	return (transceiver->last_event == USB_EVENT_VBUS ||
@@ -315,7 +315,7 @@ static int pda_power_probe(struct platform_device *pdev)
 		pda_psy_usb.num_supplicants = pdata->num_supplicants;
 	}
 
-#ifdef CONFIG_USB_OTG_UTILS
+#if IS_ENABLED(CONFIG_USB_PHY)
 	transceiver = usb_get_phy(USB_PHY_TYPE_USB2);
 	if (!IS_ERR_OR_NULL(transceiver)) {
 		if (!pdata->is_usb_online)
@@ -367,7 +367,7 @@ static int pda_power_probe(struct platform_device *pdev)
 		}
 	}
 
-#ifdef CONFIG_USB_OTG_UTILS
+#if IS_ENABLED(CONFIG_USB_PHY)
 	if (!IS_ERR_OR_NULL(transceiver) && pdata->use_otg_notifier) {
 		otg_nb.notifier_call = otg_handle_notification;
 		ret = usb_register_notifier(transceiver, &otg_nb);
@@ -391,7 +391,7 @@ static int pda_power_probe(struct platform_device *pdev)
 
 	return 0;
 
-#ifdef CONFIG_USB_OTG_UTILS
+#if IS_ENABLED(CONFIG_USB_PHY)
 otg_reg_notifier_failed:
 	if (pdata->is_usb_online && usb_irq)
 		free_irq(usb_irq->start, &pda_psy_usb);
@@ -402,7 +402,7 @@ usb_irq_failed:
 usb_supply_failed:
 	if (pdata->is_ac_online && ac_irq)
 		free_irq(ac_irq->start, &pda_psy_ac);
-#ifdef CONFIG_USB_OTG_UTILS
+#if IS_ENABLED(CONFIG_USB_PHY)
 	if (!IS_ERR_OR_NULL(transceiver))
 		usb_put_phy(transceiver);
 #endif
@@ -437,7 +437,7 @@ static int pda_power_remove(struct platform_device *pdev)
 		power_supply_unregister(&pda_psy_usb);
 	if (pdata->is_ac_online)
 		power_supply_unregister(&pda_psy_ac);
-#ifdef CONFIG_USB_OTG_UTILS
+#if IS_ENABLED(CONFIG_USB_PHY)
 	if (!IS_ERR_OR_NULL(transceiver))
 		usb_put_phy(transceiver);
 #endif
diff --git a/drivers/power/pm2301_charger.c b/drivers/power/pm2301_charger.c
index f123f3c219c..a44175139bb 100644
--- a/drivers/power/pm2301_charger.c
+++ b/drivers/power/pm2301_charger.c
@@ -226,7 +226,7 @@ static int pm2xxx_charger_ovv_mngt(struct pm2xxx_charger *pm2, int val)
 
 static int pm2xxx_charger_wd_exp_mngt(struct pm2xxx_charger *pm2, int val)
 {
-	dev_dbg(pm2->dev , "20 minutes watchdog occured\n");
+	dev_dbg(pm2->dev , "20 minutes watchdog expired\n");
 
 	pm2->ac.wd_expired = true;
 	power_supply_changed(&pm2->ac_chg.psy);
diff --git a/drivers/regulator/88pm8607.c b/drivers/regulator/88pm8607.c
index c79ab843333..493948a38fc 100644
--- a/drivers/regulator/88pm8607.c
+++ b/drivers/regulator/88pm8607.c
@@ -220,35 +220,6 @@ static int pm8607_list_voltage(struct regulator_dev *rdev, unsigned index)
 	return ret;
 }
 
-static int pm8606_preg_enable(struct regulator_dev *rdev)
-{
-	struct pm8607_regulator_info *info = rdev_get_drvdata(rdev);
-
-	return pm860x_set_bits(info->i2c, rdev->desc->enable_reg,
-			       1 << rdev->desc->enable_mask, 0);
-}
-
-static int pm8606_preg_disable(struct regulator_dev *rdev)
-{
-	struct pm8607_regulator_info *info = rdev_get_drvdata(rdev);
-
-	return pm860x_set_bits(info->i2c, rdev->desc->enable_reg,
-			       1 << rdev->desc->enable_mask,
-			       1 << rdev->desc->enable_mask);
-}
-
-static int pm8606_preg_is_enabled(struct regulator_dev *rdev)
-{
-	struct pm8607_regulator_info *info = rdev_get_drvdata(rdev);
-	int ret;
-
-	ret = pm860x_reg_read(info->i2c, rdev->desc->enable_reg);
-	if (ret < 0)
-		return ret;
-
-	return !((unsigned char)ret & (1 << rdev->desc->enable_mask));
-}
-
 static struct regulator_ops pm8607_regulator_ops = {
 	.list_voltage	= pm8607_list_voltage,
 	.set_voltage_sel = regulator_set_voltage_sel_regmap,
@@ -259,9 +230,9 @@ static struct regulator_ops pm8607_regulator_ops = {
 };
 
 static struct regulator_ops pm8606_preg_ops = {
-	.enable		= pm8606_preg_enable,
-	.disable	= pm8606_preg_disable,
-	.is_enabled	= pm8606_preg_is_enabled,
+	.enable		= regulator_enable_regmap,
+	.disable	= regulator_disable_regmap,
+	.is_enabled	= regulator_is_enabled_regmap,
 };
 
 #define PM8606_PREG(ereg, ebit)						\
@@ -274,6 +245,7 @@ static struct regulator_ops pm8606_preg_ops = {
 		.owner	= THIS_MODULE,					\
 		.enable_reg = PM8606_##ereg,				\
 		.enable_mask = (ebit),					\
+		.enable_is_inverted = true,				\
 	},								\
 }
 
diff --git a/drivers/regulator/Makefile b/drivers/regulator/Makefile
index 6e8250382de..47a34ff88f9 100644
--- a/drivers/regulator/Makefile
+++ b/drivers/regulator/Makefile
@@ -12,7 +12,7 @@ obj-$(CONFIG_REGULATOR_USERSPACE_CONSUMER) += userspace-consumer.o
 obj-$(CONFIG_REGULATOR_88PM8607) += 88pm8607.o
 obj-$(CONFIG_REGULATOR_AAT2870) += aat2870-regulator.o
 obj-$(CONFIG_REGULATOR_AB3100) += ab3100.o
-obj-$(CONFIG_REGULATOR_AB8500)	+= ab8500.o
+obj-$(CONFIG_REGULATOR_AB8500)	+= ab8500.o ab8500-ext.o
 obj-$(CONFIG_REGULATOR_AD5398) += ad5398.o
 obj-$(CONFIG_REGULATOR_ANATOP) += anatop-regulator.o
 obj-$(CONFIG_REGULATOR_ARIZONA) += arizona-micsupp.o arizona-ldo1.o
diff --git a/drivers/regulator/ab3100.c b/drivers/regulator/ab3100.c
index 111ec69a3e9..3be9e46594a 100644
--- a/drivers/regulator/ab3100.c
+++ b/drivers/regulator/ab3100.c
@@ -17,6 +17,8 @@
 #include <linux/regulator/driver.h>
 #include <linux/mfd/ab3100.h>
 #include <linux/mfd/abx500.h>
+#include <linux/of.h>
+#include <linux/regulator/of_regulator.h>
 
 /* LDO registers and some handy masking definitions for AB3100 */
 #define AB3100_LDO_A		0x40
@@ -345,7 +347,11 @@ static int ab3100_get_voltage_regulator_external(struct regulator_dev *reg)
 {
 	struct ab3100_regulator *abreg = rdev_get_drvdata(reg);
 
-	return abreg->plfdata->external_voltage;
+	if (abreg->plfdata)
+		return abreg->plfdata->external_voltage;
+	else
+		/* TODO: encode external voltage into device tree */
+		return 0;
 }
 
 static struct regulator_ops regulator_ops_fixed = {
@@ -488,16 +494,174 @@ ab3100_regulator_desc[AB3100_NUM_REGULATORS] = {
 	},
 };
 
+static int ab3100_regulator_register(struct platform_device *pdev,
+				     struct ab3100_platform_data *plfdata,
+				     struct regulator_init_data *init_data,
+				     struct device_node *np,
+				     int id)
+{
+	struct regulator_desc *desc;
+	struct ab3100_regulator *reg;
+	struct regulator_dev *rdev;
+	struct regulator_config config = { };
+	int err, i;
+
+	for (i = 0; i < AB3100_NUM_REGULATORS; i++) {
+		desc = &ab3100_regulator_desc[i];
+		if (desc->id == id)
+			break;
+	}
+	if (desc->id != id)
+		return -ENODEV;
+
+	/* Same index used for this array */
+	reg = &ab3100_regulators[i];
+
+	/*
+	 * Initialize per-regulator struct.
+	 * Inherit platform data, this comes down from the
+	 * i2c boarddata, from the machine. So if you want to
+	 * see what it looks like for a certain machine, go
+	 * into the machine I2C setup.
+	 */
+	reg->dev = &pdev->dev;
+	if (plfdata) {
+		reg->plfdata = plfdata;
+		config.init_data = &plfdata->reg_constraints[i];
+	} else if (np) {
+		config.of_node = np;
+		config.init_data = init_data;
+	}
+	config.dev = &pdev->dev;
+	config.driver_data = reg;
+
+	rdev = regulator_register(desc, &config);
+	if (IS_ERR(rdev)) {
+		err = PTR_ERR(rdev);
+		dev_err(&pdev->dev,
+			"%s: failed to register regulator %s err %d\n",
+			__func__, desc->name,
+			err);
+		return err;
+	}
+
+	/* Then set a pointer back to the registered regulator */
+	reg->rdev = rdev;
+	return 0;
+}
+
+static struct of_regulator_match ab3100_regulator_matches[] = {
+	{ .name = "ab3100_ldo_a", .driver_data = (void *) AB3100_LDO_A, },
+	{ .name = "ab3100_ldo_c", .driver_data = (void *) AB3100_LDO_C, },
+	{ .name = "ab3100_ldo_d", .driver_data = (void *) AB3100_LDO_D, },
+	{ .name = "ab3100_ldo_e", .driver_data = (void *) AB3100_LDO_E, },
+	{ .name = "ab3100_ldo_f", .driver_data = (void *) AB3100_LDO_F },
+	{ .name = "ab3100_ldo_g", .driver_data = (void *) AB3100_LDO_G },
+	{ .name = "ab3100_ldo_h", .driver_data = (void *) AB3100_LDO_H },
+	{ .name = "ab3100_ldo_k", .driver_data = (void *) AB3100_LDO_K },
+	{ .name = "ab3100_ext", .driver_data = (void *) AB3100_LDO_EXT },
+	{ .name = "ab3100_buck", .driver_data = (void *) AB3100_BUCK },
+};
+
 /*
- * NOTE: the following functions are regulators pluralis - it is the
- * binding to the AB3100 core driver and the parent platform device
- * for all the different regulators.
+ * Initial settings of ab3100 registers.
+ * Common for below LDO regulator settings are that
+ * bit 7-5 controls voltage. Bit 4 turns regulator ON(1) or OFF(0).
+ * Bit 3-2 controls sleep enable and bit 1-0 controls sleep mode.
  */
+/* LDO_A 0x16: 2.75V, ON, SLEEP_A, SLEEP OFF GND */
+#define LDO_A_SETTING		0x16
+/* LDO_C 0x10: 2.65V, ON, SLEEP_A or B, SLEEP full power */
+#define LDO_C_SETTING		0x10
+/* LDO_D 0x10: 2.65V, ON, sleep mode not used */
+#define LDO_D_SETTING		0x10
+/* LDO_E 0x10: 1.8V, ON, SLEEP_A or B, SLEEP full power */
+#define LDO_E_SETTING		0x10
+/* LDO_E SLEEP 0x00: 1.8V, not used, SLEEP_A or B, not used */
+#define LDO_E_SLEEP_SETTING	0x00
+/* LDO_F 0xD0: 2.5V, ON, SLEEP_A or B, SLEEP full power */
+#define LDO_F_SETTING		0xD0
+/* LDO_G 0x00: 2.85V, OFF, SLEEP_A or B, SLEEP full power */
+#define LDO_G_SETTING		0x00
+/* LDO_H 0x18: 2.75V, ON, SLEEP_B, SLEEP full power */
+#define LDO_H_SETTING		0x18
+/* LDO_K 0x00: 2.75V, OFF, SLEEP_A or B, SLEEP full power */
+#define LDO_K_SETTING		0x00
+/* LDO_EXT 0x00: Voltage not set, OFF, not used, not used */
+#define LDO_EXT_SETTING		0x00
+/* BUCK 0x7D: 1.2V, ON, SLEEP_A and B, SLEEP low power */
+#define BUCK_SETTING	0x7D
+/* BUCK SLEEP 0xAC: 1.05V, Not used, SLEEP_A and B, Not used */
+#define BUCK_SLEEP_SETTING	0xAC
+
+static const u8 ab3100_reg_initvals[] = {
+	LDO_A_SETTING,
+	LDO_C_SETTING,
+	LDO_E_SETTING,
+	LDO_E_SLEEP_SETTING,
+	LDO_F_SETTING,
+	LDO_G_SETTING,
+	LDO_H_SETTING,
+	LDO_K_SETTING,
+	LDO_EXT_SETTING,
+	BUCK_SETTING,
+	BUCK_SLEEP_SETTING,
+	LDO_D_SETTING,
+};
+
+static int ab3100_regulators_remove(struct platform_device *pdev)
+{
+	int i;
+
+	for (i = 0; i < AB3100_NUM_REGULATORS; i++) {
+		struct ab3100_regulator *reg = &ab3100_regulators[i];
+
+		regulator_unregister(reg->rdev);
+		reg->rdev = NULL;
+	}
+	return 0;
+}
+
+static int
+ab3100_regulator_of_probe(struct platform_device *pdev, struct device_node *np)
+{
+	int err, i;
+
+	/*
+	 * Set up the regulator registers, as was previously done with
+	 * platform data.
+	 */
+	/* Set up regulators */
+	for (i = 0; i < ARRAY_SIZE(ab3100_reg_init_order); i++) {
+		err = abx500_set_register_interruptible(&pdev->dev, 0,
+					ab3100_reg_init_order[i],
+					ab3100_reg_initvals[i]);
+		if (err) {
+			dev_err(&pdev->dev, "regulator initialization failed with error %d\n",
+				err);
+			return err;
+		}
+	}
+
+	for (i = 0; i < ARRAY_SIZE(ab3100_regulator_matches); i++) {
+		err = ab3100_regulator_register(
+			pdev, NULL, ab3100_regulator_matches[i].init_data,
+			ab3100_regulator_matches[i].of_node,
+			(int) ab3100_regulator_matches[i].driver_data);
+		if (err) {
+			ab3100_regulators_remove(pdev);
+			return err;
+		}
+	}
+
+	return 0;
+}
+
 
 static int ab3100_regulators_probe(struct platform_device *pdev)
 {
 	struct ab3100_platform_data *plfdata = pdev->dev.platform_data;
-	struct regulator_config config = { };
+	struct device_node *np = pdev->dev.of_node;
 	int err = 0;
 	u8 data;
 	int i;
@@ -516,6 +680,18 @@ static int ab3100_regulators_probe(struct platform_device *pdev)
 		dev_notice(&pdev->dev,
 			   "chip is in inactive mode (Cold start)\n");
 
+	if (np) {
+		err = of_regulator_match(&pdev->dev, np,
+					 ab3100_regulator_matches,
+					 ARRAY_SIZE(ab3100_regulator_matches));
+		if (err < 0) {
+			dev_err(&pdev->dev,
+				"Error parsing regulator init data: %d\n", err);
+			return err;
+		}
+		return ab3100_regulator_of_probe(pdev, np);
+	}
+
 	/* Set up regulators */
 	for (i = 0; i < ARRAY_SIZE(ab3100_reg_init_order); i++) {
 		err = abx500_set_register_interruptible(&pdev->dev, 0,
@@ -530,59 +706,19 @@ static int ab3100_regulators_probe(struct platform_device *pdev)
 
 	/* Register the regulators */
 	for (i = 0; i < AB3100_NUM_REGULATORS; i++) {
-		struct ab3100_regulator *reg = &ab3100_regulators[i];
-		struct regulator_dev *rdev;
-
-		/*
-		 * Initialize per-regulator struct.
-		 * Inherit platform data, this comes down from the
-		 * i2c boarddata, from the machine. So if you want to
-		 * see what it looks like for a certain machine, go
-		 * into the machine I2C setup.
-		 */
-		reg->dev = &pdev->dev;
-		reg->plfdata = plfdata;
+		struct regulator_desc *desc = &ab3100_regulator_desc[i];
 
-		config.dev = &pdev->dev;
-		config.driver_data = reg;
-		config.init_data = &plfdata->reg_constraints[i];
-
-		/*
-		 * Register the regulator, pass around
-		 * the ab3100_regulator struct
-		 */
-		rdev = regulator_register(&ab3100_regulator_desc[i], &config);
-		if (IS_ERR(rdev)) {
-			err = PTR_ERR(rdev);
-			dev_err(&pdev->dev,
-				"%s: failed to register regulator %s err %d\n",
-				__func__, ab3100_regulator_desc[i].name,
-				err);
-			/* remove the already registered regulators */
-			while (--i >= 0)
-				regulator_unregister(ab3100_regulators[i].rdev);
+		err = ab3100_regulator_register(pdev, plfdata, NULL, NULL,
+						desc->id);
+		if (err) {
+			ab3100_regulators_remove(pdev);
 			return err;
 		}
-
-		/* Then set a pointer back to the registered regulator */
-		reg->rdev = rdev;
 	}
 
 	return 0;
 }
 
-static int ab3100_regulators_remove(struct platform_device *pdev)
-{
-	int i;
-
-	for (i = 0; i < AB3100_NUM_REGULATORS; i++) {
-		struct ab3100_regulator *reg = &ab3100_regulators[i];
-
-		regulator_unregister(reg->rdev);
-	}
-	return 0;
-}
-
 static struct platform_driver ab3100_regulators_driver = {
 	.driver = {
 		.name  = "ab3100-regulators",
diff --git a/drivers/regulator/ab8500-ext.c b/drivers/regulator/ab8500-ext.c
new file mode 100644
index 00000000000..b4d45472aae
--- /dev/null
+++ b/drivers/regulator/ab8500-ext.c
@@ -0,0 +1,407 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License Terms: GNU General Public License v2
+ *
+ * Authors: Bengt Jonsson <bengt.g.jonsson@stericsson.com>
+ *
+ * This file is based on drivers/regulator/ab8500.c
+ *
+ * AB8500 external regulators
+ *
+ * ab8500-ext supports the following regulators:
+ * - VextSupply3
+ */
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/abx500/ab8500.h>
+#include <linux/regulator/ab8500.h>
+
+/**
+ * struct ab8500_ext_regulator_info - ab8500 regulator information
+ * @dev: device pointer
+ * @desc: regulator description
+ * @rdev: regulator device
+ * @cfg: regulator configuration (extension of regulator FW configuration)
+ * @update_bank: bank to control on/off
+ * @update_reg: register to control on/off
+ * @update_mask: mask to enable/disable and set mode of regulator
+ * @update_val: bits holding the regulator current mode
+ * @update_val_hp: bits to set EN pin active (LPn pin deactive)
+ *                 normally this means high power mode
+ * @update_val_lp: bits to set EN pin active and LPn pin active
+ *                 normally this means low power mode
+ * @update_val_hw: bits to set regulator pins in HW control
+ *                 SysClkReq pins and logic will choose mode
+ */
+struct ab8500_ext_regulator_info {
+	struct device *dev;
+	struct regulator_desc desc;
+	struct regulator_dev *rdev;
+	struct ab8500_ext_regulator_cfg *cfg;
+	u8 update_bank;
+	u8 update_reg;
+	u8 update_mask;
+	u8 update_val;
+	u8 update_val_hp;
+	u8 update_val_lp;
+	u8 update_val_hw;
+};
+
+static int ab8500_ext_regulator_enable(struct regulator_dev *rdev)
+{
+	int ret;
+	struct ab8500_ext_regulator_info *info = rdev_get_drvdata(rdev);
+	u8 regval;
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * To satisfy both HW high power request and SW request, the regulator
+	 * must be on in high power.
+	 */
+	if (info->cfg && info->cfg->hwreq)
+		regval = info->update_val_hp;
+	else
+		regval = info->update_val;
+
+	ret = abx500_mask_and_set_register_interruptible(info->dev,
+		info->update_bank, info->update_reg,
+		info->update_mask, regval);
+	if (ret < 0) {
+		dev_err(rdev_get_dev(info->rdev),
+			"couldn't set enable bits for regulator\n");
+		return ret;
+	}
+
+	dev_dbg(rdev_get_dev(rdev),
+		"%s-enable (bank, reg, mask, value): 0x%02x, 0x%02x, 0x%02x, 0x%02x\n",
+		info->desc.name, info->update_bank, info->update_reg,
+		info->update_mask, regval);
+
+	return 0;
+}
+
+static int ab8500_ext_regulator_disable(struct regulator_dev *rdev)
+{
+	int ret;
+	struct ab8500_ext_regulator_info *info = rdev_get_drvdata(rdev);
+	u8 regval;
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * Set the regulator in HW request mode if configured
+	 */
+	if (info->cfg && info->cfg->hwreq)
+		regval = info->update_val_hw;
+	else
+		regval = 0;
+
+	ret = abx500_mask_and_set_register_interruptible(info->dev,
+		info->update_bank, info->update_reg,
+		info->update_mask, regval);
+	if (ret < 0) {
+		dev_err(rdev_get_dev(info->rdev),
+			"couldn't set disable bits for regulator\n");
+		return ret;
+	}
+
+	dev_dbg(rdev_get_dev(rdev), "%s-disable (bank, reg, mask, value):"
+		" 0x%02x, 0x%02x, 0x%02x, 0x%02x\n",
+		info->desc.name, info->update_bank, info->update_reg,
+		info->update_mask, regval);
+
+	return 0;
+}
+
+static int ab8500_ext_regulator_is_enabled(struct regulator_dev *rdev)
+{
+	int ret;
+	struct ab8500_ext_regulator_info *info = rdev_get_drvdata(rdev);
+	u8 regval;
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	ret = abx500_get_register_interruptible(info->dev,
+		info->update_bank, info->update_reg, &regval);
+	if (ret < 0) {
+		dev_err(rdev_get_dev(rdev),
+			"couldn't read 0x%x register\n", info->update_reg);
+		return ret;
+	}
+
+	dev_dbg(rdev_get_dev(rdev), "%s-is_enabled (bank, reg, mask, value):"
+		" 0x%02x, 0x%02x, 0x%02x, 0x%02x\n",
+		info->desc.name, info->update_bank, info->update_reg,
+		info->update_mask, regval);
+
+	if (((regval & info->update_mask) == info->update_val_lp) ||
+	    ((regval & info->update_mask) == info->update_val_hp))
+		return 1;
+	else
+		return 0;
+}
+
+static int ab8500_ext_regulator_set_mode(struct regulator_dev *rdev,
+					 unsigned int mode)
+{
+	int ret = 0;
+	struct ab8500_ext_regulator_info *info = rdev_get_drvdata(rdev);
+	u8 regval;
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	switch (mode) {
+	case REGULATOR_MODE_NORMAL:
+		regval = info->update_val_hp;
+		break;
+	case REGULATOR_MODE_IDLE:
+		regval = info->update_val_lp;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	/* If regulator is enabled and info->cfg->hwreq is set, the regulator
+	   must be on in high power, so we don't need to write the register with
+	   the same value.
+	 */
+	if (ab8500_ext_regulator_is_enabled(rdev) &&
+	    !(info->cfg && info->cfg->hwreq)) {
+		ret = abx500_mask_and_set_register_interruptible(info->dev,
+					info->update_bank, info->update_reg,
+					info->update_mask, regval);
+		if (ret < 0) {
+			dev_err(rdev_get_dev(rdev),
+				"Could not set regulator mode.\n");
+			return ret;
+		}
+
+		dev_dbg(rdev_get_dev(rdev),
+			"%s-set_mode (bank, reg, mask, value): "
+			"0x%x, 0x%x, 0x%x, 0x%x\n",
+			info->desc.name, info->update_bank, info->update_reg,
+			info->update_mask, regval);
+	}
+
+	info->update_val = regval;
+
+	return 0;
+}
+
+static unsigned int ab8500_ext_regulator_get_mode(struct regulator_dev *rdev)
+{
+	struct ab8500_ext_regulator_info *info = rdev_get_drvdata(rdev);
+	int ret;
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	if (info->update_val == info->update_val_hp)
+		ret = REGULATOR_MODE_NORMAL;
+	else if (info->update_val == info->update_val_lp)
+		ret = REGULATOR_MODE_IDLE;
+	else
+		ret = -EINVAL;
+
+	return ret;
+}
+
+static int ab8500_ext_list_voltage(struct regulator_dev *rdev,
+				   unsigned selector)
+{
+	struct regulation_constraints *regu_constraints = rdev->constraints;
+
+	if (regu_constraints == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator constraints null pointer\n");
+		return -EINVAL;
+	}
+	/* return the uV for the fixed regulators */
+	if (regu_constraints->min_uV && regu_constraints->max_uV) {
+		if (regu_constraints->min_uV == regu_constraints->max_uV)
+			return regu_constraints->min_uV;
+	}
+	return -EINVAL;
+}
+
+static struct regulator_ops ab8500_ext_regulator_ops = {
+	.enable			= ab8500_ext_regulator_enable,
+	.disable		= ab8500_ext_regulator_disable,
+	.is_enabled		= ab8500_ext_regulator_is_enabled,
+	.set_mode		= ab8500_ext_regulator_set_mode,
+	.get_mode		= ab8500_ext_regulator_get_mode,
+	.list_voltage		= ab8500_ext_list_voltage,
+};
+
+static struct ab8500_ext_regulator_info
+		ab8500_ext_regulator_info[AB8500_NUM_EXT_REGULATORS] = {
+	[AB8500_EXT_SUPPLY1] = {
+		.desc = {
+			.name		= "VEXTSUPPLY1",
+			.ops		= &ab8500_ext_regulator_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_EXT_SUPPLY1,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+		},
+		.update_bank		= 0x04,
+		.update_reg		= 0x08,
+		.update_mask		= 0x03,
+		.update_val		= 0x01,
+		.update_val_hp		= 0x01,
+		.update_val_lp		= 0x03,
+		.update_val_hw		= 0x02,
+	},
+	[AB8500_EXT_SUPPLY2] = {
+		.desc = {
+			.name		= "VEXTSUPPLY2",
+			.ops		= &ab8500_ext_regulator_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_EXT_SUPPLY2,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+		},
+		.update_bank		= 0x04,
+		.update_reg		= 0x08,
+		.update_mask		= 0x0c,
+		.update_val		= 0x04,
+		.update_val_hp		= 0x04,
+		.update_val_lp		= 0x0c,
+		.update_val_hw		= 0x08,
+	},
+	[AB8500_EXT_SUPPLY3] = {
+		.desc = {
+			.name		= "VEXTSUPPLY3",
+			.ops		= &ab8500_ext_regulator_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_EXT_SUPPLY3,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+		},
+		.update_bank		= 0x04,
+		.update_reg		= 0x08,
+		.update_mask		= 0x30,
+		.update_val		= 0x10,
+		.update_val_hp		= 0x10,
+		.update_val_lp		= 0x30,
+		.update_val_hw		= 0x20,
+	},
+};
+
+int ab8500_ext_regulator_init(struct platform_device *pdev)
+{
+	struct ab8500 *ab8500 = dev_get_drvdata(pdev->dev.parent);
+	struct ab8500_platform_data *ppdata;
+	struct ab8500_regulator_platform_data *pdata;
+	struct regulator_config config = { };
+	int i, err;
+
+	if (!ab8500) {
+		dev_err(&pdev->dev, "null mfd parent\n");
+		return -EINVAL;
+	}
+	ppdata = dev_get_platdata(ab8500->dev);
+	if (!ppdata) {
+		dev_err(&pdev->dev, "null parent pdata\n");
+		return -EINVAL;
+	}
+
+	pdata = ppdata->regulator;
+	if (!pdata) {
+		dev_err(&pdev->dev, "null pdata\n");
+		return -EINVAL;
+	}
+
+	/* make sure the platform data has the correct size */
+	if (pdata->num_ext_regulator != ARRAY_SIZE(ab8500_ext_regulator_info)) {
+		dev_err(&pdev->dev, "Configuration error: size mismatch.\n");
+		return -EINVAL;
+	}
+
+	/* check for AB8500 2.x */
+	if (is_ab8500_2p0_or_earlier(ab8500)) {
+		struct ab8500_ext_regulator_info *info;
+
+		/* VextSupply3LPn is inverted on AB8500 2.x */
+		info = &ab8500_ext_regulator_info[AB8500_EXT_SUPPLY3];
+		info->update_val = 0x30;
+		info->update_val_hp = 0x30;
+		info->update_val_lp = 0x10;
+	}
+
+	/* register all regulators */
+	for (i = 0; i < ARRAY_SIZE(ab8500_ext_regulator_info); i++) {
+		struct ab8500_ext_regulator_info *info = NULL;
+
+		/* assign per-regulator data */
+		info = &ab8500_ext_regulator_info[i];
+		info->dev = &pdev->dev;
+		info->cfg = (struct ab8500_ext_regulator_cfg *)
+			pdata->ext_regulator[i].driver_data;
+
+		config.dev = &pdev->dev;
+		config.init_data = &pdata->ext_regulator[i];
+		config.driver_data = info;
+
+		/* register regulator with framework */
+		info->rdev = regulator_register(&info->desc, &config);
+		if (IS_ERR(info->rdev)) {
+			err = PTR_ERR(info->rdev);
+			dev_err(&pdev->dev, "failed to register regulator %s\n",
+					info->desc.name);
+			/* when we fail, un-register all earlier regulators */
+			while (--i >= 0) {
+				info = &ab8500_ext_regulator_info[i];
+				regulator_unregister(info->rdev);
+			}
+			return err;
+		}
+
+		dev_dbg(rdev_get_dev(info->rdev),
+			"%s-probed\n", info->desc.name);
+	}
+
+	return 0;
+}
+
+void ab8500_ext_regulator_exit(struct platform_device *pdev)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(ab8500_ext_regulator_info); i++) {
+		struct ab8500_ext_regulator_info *info = NULL;
+		info = &ab8500_ext_regulator_info[i];
+
+		dev_vdbg(rdev_get_dev(info->rdev),
+			"%s-remove\n", info->desc.name);
+
+		regulator_unregister(info->rdev);
+	}
+}
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Bengt Jonsson <bengt.g.jonsson@stericsson.com>");
+MODULE_DESCRIPTION("AB8500 external regulator driver");
+MODULE_ALIAS("platform:ab8500-ext-regulator");
diff --git a/drivers/regulator/ab8500.c b/drivers/regulator/ab8500.c
index 09014f38a94..f6656b8c28b 100644
--- a/drivers/regulator/ab8500.c
+++ b/drivers/regulator/ab8500.c
@@ -5,11 +5,15 @@
  *
  * Authors: Sundar Iyer <sundar.iyer@stericsson.com> for ST-Ericsson
  *          Bengt Jonsson <bengt.g.jonsson@stericsson.com> for ST-Ericsson
+ *          Daniel Willerud <daniel.willerud@stericsson.com> for ST-Ericsson
  *
  * AB8500 peripheral regulators
  *
  * AB8500 supports the following regulators:
  *   VAUX1/2/3, VINTCORE, VTVOUT, VUSB, VAUDIO, VAMIC1/2, VDMIC, VANA
+ *
+ * AB8505 supports the following regulators:
+ *   VAUX1/2/3/4/5/6, VINTCORE, VADC, VUSB, VAUDIO, VAMIC1/2, VDMIC, VANA
  */
 #include <linux/init.h>
 #include <linux/kernel.h>
@@ -26,33 +30,64 @@
 #include <linux/slab.h>
 
 /**
+ * struct ab8500_shared_mode - is used when mode is shared between
+ * two regulators.
+ * @shared_regulator: pointer to the other sharing regulator
+ * @lp_mode_req: low power mode requested by this regulator
+ */
+struct ab8500_shared_mode {
+	struct ab8500_regulator_info *shared_regulator;
+	bool lp_mode_req;
+};
+
+/**
  * struct ab8500_regulator_info - ab8500 regulator information
  * @dev: device pointer
  * @desc: regulator description
  * @regulator_dev: regulator device
+ * @shared_mode: used when mode is shared between two regulators
+ * @load_lp_uA: maximum load in idle (low power) mode
  * @update_bank: bank to control on/off
  * @update_reg: register to control on/off
- * @update_mask: mask to enable/disable regulator
- * @update_val_enable: bits to enable the regulator in normal (high power) mode
+ * @update_mask: mask to enable/disable and set mode of regulator
+ * @update_val: bits holding the regulator current mode
+ * @update_val_idle: bits to enable the regulator in idle (low power) mode
+ * @update_val_normal: bits to enable the regulator in normal (high power) mode
+ * @mode_bank: bank with location of mode register
+ * @mode_reg: mode register
+ * @mode_mask: mask for setting mode
+ * @mode_val_idle: mode setting for low power
+ * @mode_val_normal: mode setting for normal power
  * @voltage_bank: bank to control regulator voltage
  * @voltage_reg: register to control regulator voltage
  * @voltage_mask: mask to control regulator voltage
- * @voltage_shift: shift to control regulator voltage
- * @delay: startup/set voltage delay in us
  */
 struct ab8500_regulator_info {
 	struct device		*dev;
 	struct regulator_desc	desc;
 	struct regulator_dev	*regulator;
+	struct ab8500_shared_mode *shared_mode;
+	int load_lp_uA;
 	u8 update_bank;
 	u8 update_reg;
 	u8 update_mask;
-	u8 update_val_enable;
+	u8 update_val;
+	u8 update_val_idle;
+	u8 update_val_normal;
+	u8 mode_bank;
+	u8 mode_reg;
+	u8 mode_mask;
+	u8 mode_val_idle;
+	u8 mode_val_normal;
 	u8 voltage_bank;
 	u8 voltage_reg;
 	u8 voltage_mask;
-	u8 voltage_shift;
-	unsigned int delay;
+	struct {
+		u8 voltage_limit;
+		u8 voltage_bank;
+		u8 voltage_reg;
+		u8 voltage_mask;
+	} expand_register;
 };
 
 /* voltage tables for the vauxn/vintcore supplies */
@@ -86,6 +121,44 @@ static const unsigned int ldo_vaux3_voltages[] = {
 	2910000,
 };
 
+static const unsigned int ldo_vaux56_voltages[] = {
+	1800000,
+	1050000,
+	1100000,
+	1200000,
+	1500000,
+	2200000,
+	2500000,
+	2790000,
+};
+
+static const unsigned int ldo_vaux3_ab8540_voltages[] = {
+	1200000,
+	1500000,
+	1800000,
+	2100000,
+	2500000,
+	2750000,
+	2790000,
+	2910000,
+	3050000,
+};
+
+static const unsigned int ldo_vaux56_ab8540_voltages[] = {
+	750000, 760000, 770000, 780000, 790000, 800000,
+	810000, 820000, 830000, 840000, 850000, 860000,
+	870000, 880000, 890000, 900000, 910000, 920000,
+	930000, 940000, 950000, 960000, 970000, 980000,
+	990000, 1000000, 1010000, 1020000, 1030000,
+	1040000, 1050000, 1060000, 1070000, 1080000,
+	1090000, 1100000, 1110000, 1120000, 1130000,
+	1140000, 1150000, 1160000, 1170000, 1180000,
+	1190000, 1200000, 1210000, 1220000, 1230000,
+	1240000, 1250000, 1260000, 1270000, 1280000,
+	1290000, 1300000, 1310000, 1320000, 1330000,
+	1340000, 1350000, 1360000, 1800000, 2790000,
+};
+
 static const unsigned int ldo_vintcore_voltages[] = {
 	1200000,
 	1225000,
@@ -96,6 +169,72 @@ static const unsigned int ldo_vintcore_voltages[] = {
 	1350000,
 };
 
+static const unsigned int ldo_sdio_voltages[] = {
+	1160000,
+	1050000,
+	1100000,
+	1500000,
+	1800000,
+	2200000,
+	2910000,
+	3050000,
+};
+
+static const unsigned int fixed_1200000_voltage[] = {
+	1200000,
+};
+
+static const unsigned int fixed_1800000_voltage[] = {
+	1800000,
+};
+
+static const unsigned int fixed_2000000_voltage[] = {
+	2000000,
+};
+
+static const unsigned int fixed_2050000_voltage[] = {
+	2050000,
+};
+
+static const unsigned int fixed_3300000_voltage[] = {
+	3300000,
+};
+
+static const unsigned int ldo_vana_voltages[] = {
+	1050000,
+	1075000,
+	1100000,
+	1125000,
+	1150000,
+	1175000,
+	1200000,
+	1225000,
+};
+
+static const unsigned int ldo_vaudio_voltages[] = {
+	2000000,
+	2100000,
+	2200000,
+	2300000,
+	2400000,
+	2500000,
+	2600000,
+	2600000,	/* Duplicated in Vaudio and IsoUicc Control register. */
+};
+
+static const unsigned int ldo_vdmic_voltages[] = {
+	1800000,
+	1900000,
+	2000000,
+	2850000,
+};
+
+static DEFINE_MUTEX(shared_mode_mutex);
+static struct ab8500_shared_mode ldo_anamic1_shared;
+static struct ab8500_shared_mode ldo_anamic2_shared;
+static struct ab8500_shared_mode ab8540_ldo_anamic1_shared;
+static struct ab8500_shared_mode ab8540_ldo_anamic2_shared;
+
 static int ab8500_regulator_enable(struct regulator_dev *rdev)
 {
 	int ret;
@@ -108,15 +247,17 @@ static int ab8500_regulator_enable(struct regulator_dev *rdev)
 
 	ret = abx500_mask_and_set_register_interruptible(info->dev,
 		info->update_bank, info->update_reg,
-		info->update_mask, info->update_val_enable);
-	if (ret < 0)
+		info->update_mask, info->update_val);
+	if (ret < 0) {
 		dev_err(rdev_get_dev(rdev),
 			"couldn't set enable bits for regulator\n");
+		return ret;
+	}
 
 	dev_vdbg(rdev_get_dev(rdev),
 		"%s-enable (bank, reg, mask, value): 0x%x, 0x%x, 0x%x, 0x%x\n",
 		info->desc.name, info->update_bank, info->update_reg,
-		info->update_mask, info->update_val_enable);
+		info->update_mask, info->update_val);
 
 	return ret;
 }
@@ -134,9 +275,11 @@ static int ab8500_regulator_disable(struct regulator_dev *rdev)
 	ret = abx500_mask_and_set_register_interruptible(info->dev,
 		info->update_bank, info->update_reg,
 		info->update_mask, 0x0);
-	if (ret < 0)
+	if (ret < 0) {
 		dev_err(rdev_get_dev(rdev),
 			"couldn't set disable bits for regulator\n");
+		return ret;
+	}
 
 	dev_vdbg(rdev_get_dev(rdev),
 		"%s-disable (bank, reg, mask, value): 0x%x, 0x%x, 0x%x, 0x%x\n",
@@ -172,14 +315,170 @@ static int ab8500_regulator_is_enabled(struct regulator_dev *rdev)
 		info->update_mask, regval);
 
 	if (regval & info->update_mask)
-		return true;
+		return 1;
+	else
+		return 0;
+}
+
+static unsigned int ab8500_regulator_get_optimum_mode(
+		struct regulator_dev *rdev, int input_uV,
+		int output_uV, int load_uA)
+{
+	unsigned int mode;
+
+	struct ab8500_regulator_info *info = rdev_get_drvdata(rdev);
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	if (load_uA <= info->load_lp_uA)
+		mode = REGULATOR_MODE_IDLE;
+	else
+		mode = REGULATOR_MODE_NORMAL;
+
+	return mode;
+}
+
+static int ab8500_regulator_set_mode(struct regulator_dev *rdev,
+				     unsigned int mode)
+{
+	int ret = 0;
+	u8 bank, reg, mask, val;
+	bool lp_mode_req = false;
+	struct ab8500_regulator_info *info = rdev_get_drvdata(rdev);
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	if (info->mode_mask) {
+		bank = info->mode_bank;
+		reg = info->mode_reg;
+		mask = info->mode_mask;
+	} else {
+		bank = info->update_bank;
+		reg = info->update_reg;
+		mask = info->update_mask;
+	}
+
+	if (info->shared_mode)
+		mutex_lock(&shared_mode_mutex);
+
+	switch (mode) {
+	case REGULATOR_MODE_NORMAL:
+		if (info->shared_mode)
+			lp_mode_req = false;
+
+		if (info->mode_mask)
+			val = info->mode_val_normal;
+		else
+			val = info->update_val_normal;
+		break;
+	case REGULATOR_MODE_IDLE:
+		if (info->shared_mode) {
+			struct ab8500_regulator_info *shared_regulator;
+
+			shared_regulator = info->shared_mode->shared_regulator;
+			if (!shared_regulator->shared_mode->lp_mode_req) {
+				/* Other regulator prevent LP mode */
+				info->shared_mode->lp_mode_req = true;
+				goto out_unlock;
+			}
+
+			lp_mode_req = true;
+		}
+
+		if (info->mode_mask)
+			val = info->mode_val_idle;
+		else
+			val = info->update_val_idle;
+		break;
+	default:
+		ret = -EINVAL;
+		goto out_unlock;
+	}
+
+	if (info->mode_mask || ab8500_regulator_is_enabled(rdev)) {
+		ret = abx500_mask_and_set_register_interruptible(info->dev,
+			bank, reg, mask, val);
+		if (ret < 0) {
+			dev_err(rdev_get_dev(rdev),
+				"couldn't set regulator mode\n");
+			goto out_unlock;
+		}
+
+		dev_vdbg(rdev_get_dev(rdev),
+			"%s-set_mode (bank, reg, mask, value): "
+			"0x%x, 0x%x, 0x%x, 0x%x\n",
+			info->desc.name, bank, reg,
+			mask, val);
+	}
+
+	if (!info->mode_mask)
+		info->update_val = val;
+
+	if (info->shared_mode)
+		info->shared_mode->lp_mode_req = lp_mode_req;
+
+out_unlock:
+	if (info->shared_mode)
+		mutex_unlock(&shared_mode_mutex);
+
+	return ret;
+}
+
+static unsigned int ab8500_regulator_get_mode(struct regulator_dev *rdev)
+{
+	struct ab8500_regulator_info *info = rdev_get_drvdata(rdev);
+	int ret;
+	u8 val;
+	u8 val_normal;
+	u8 val_idle;
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	/* Need special handling for shared mode */
+	if (info->shared_mode) {
+		if (info->shared_mode->lp_mode_req)
+			return REGULATOR_MODE_IDLE;
+		else
+			return REGULATOR_MODE_NORMAL;
+	}
+
+	if (info->mode_mask) {
+		/* Dedicated register for handling mode */
+		ret = abx500_get_register_interruptible(info->dev,
+		info->mode_bank, info->mode_reg, &val);
+		val = val & info->mode_mask;
+
+		val_normal = info->mode_val_normal;
+		val_idle = info->mode_val_idle;
+	} else {
+		/* Mode register same as enable register */
+		val = info->update_val;
+		val_normal = info->update_val_normal;
+		val_idle = info->update_val_idle;
+	}
+
+	if (val == val_normal)
+		ret = REGULATOR_MODE_NORMAL;
+	else if (val == val_idle)
+		ret = REGULATOR_MODE_IDLE;
 	else
-		return false;
+		ret = -EINVAL;
+
+	return ret;
 }
 
 static int ab8500_regulator_get_voltage_sel(struct regulator_dev *rdev)
 {
-	int ret, val;
+	int ret, voltage_shift;
 	struct ab8500_regulator_info *info = rdev_get_drvdata(rdev);
 	u8 regval;
 
@@ -188,6 +487,8 @@ static int ab8500_regulator_get_voltage_sel(struct regulator_dev *rdev)
 		return -EINVAL;
 	}
 
+	voltage_shift = ffs(info->voltage_mask) - 1;
+
 	ret = abx500_get_register_interruptible(info->dev,
 			info->voltage_bank, info->voltage_reg, &regval);
 	if (ret < 0) {
@@ -201,16 +502,62 @@ static int ab8500_regulator_get_voltage_sel(struct regulator_dev *rdev)
 		"0x%x, 0x%x, 0x%x, 0x%x, 0x%x\n",
 		info->desc.name, info->voltage_bank,
 		info->voltage_reg, info->voltage_mask,
-		info->voltage_shift, regval);
+		voltage_shift, regval);
 
-	val = regval & info->voltage_mask;
-	return val >> info->voltage_shift;
+	return (regval & info->voltage_mask) >> voltage_shift;
+}
+
+static int ab8540_aux3_regulator_get_voltage_sel(struct regulator_dev *rdev)
+{
+	int ret, voltage_shift;
+	struct ab8500_regulator_info *info = rdev_get_drvdata(rdev);
+	u8 regval, regval_expand;
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	ret = abx500_get_register_interruptible(info->dev,
+			info->expand_register.voltage_bank,
+			info->expand_register.voltage_reg, &regval_expand);
+	if (ret < 0) {
+		dev_err(rdev_get_dev(rdev),
+			"couldn't read voltage expand reg for regulator\n");
+		return ret;
+	}
+
+	dev_vdbg(rdev_get_dev(rdev),
+		 "%s-get_voltage expand (bank, reg, mask, value): 0x%x, 0x%x, 0x%x, 0x%x\n",
+		 info->desc.name, info->expand_register.voltage_bank,
+		 info->expand_register.voltage_reg,
+		 info->expand_register.voltage_mask, regval_expand);
+
+	if (regval_expand & info->expand_register.voltage_mask)
+		return info->expand_register.voltage_limit;
+
+	ret = abx500_get_register_interruptible(info->dev,
+			info->voltage_bank, info->voltage_reg, &regval);
+	if (ret < 0) {
+		dev_err(rdev_get_dev(rdev),
+			"couldn't read voltage reg for regulator\n");
+		return ret;
+	}
+
+	dev_vdbg(rdev_get_dev(rdev),
+		 "%s-get_voltage (bank, reg, mask, value): 0x%x, 0x%x, 0x%x, 0x%x\n",
+		 info->desc.name, info->voltage_bank, info->voltage_reg,
+		 info->voltage_mask, regval);
+
+	voltage_shift = ffs(info->voltage_mask) - 1;
+
+	return (regval & info->voltage_mask) >> voltage_shift;
 }
 
 static int ab8500_regulator_set_voltage_sel(struct regulator_dev *rdev,
 					    unsigned selector)
 {
-	int ret;
+	int ret, voltage_shift;
 	struct ab8500_regulator_info *info = rdev_get_drvdata(rdev);
 	u8 regval;
 
@@ -219,8 +566,10 @@ static int ab8500_regulator_set_voltage_sel(struct regulator_dev *rdev,
 		return -EINVAL;
 	}
 
+	voltage_shift = ffs(info->voltage_mask) - 1;
+
 	/* set the registers for the request */
-	regval = (u8)selector << info->voltage_shift;
+	regval = (u8)selector << voltage_shift;
 	ret = abx500_mask_and_set_register_interruptible(info->dev,
 			info->voltage_bank, info->voltage_reg,
 			info->voltage_mask, regval);
@@ -237,32 +586,121 @@ static int ab8500_regulator_set_voltage_sel(struct regulator_dev *rdev,
 	return ret;
 }
 
-static int ab8500_regulator_set_voltage_time_sel(struct regulator_dev *rdev,
-					     unsigned int old_sel,
-					     unsigned int new_sel)
+static int ab8540_aux3_regulator_set_voltage_sel(struct regulator_dev *rdev,
+						unsigned selector)
 {
+	int ret;
 	struct ab8500_regulator_info *info = rdev_get_drvdata(rdev);
+	u8 regval, regval_expand;
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	if (selector < info->expand_register.voltage_limit) {
+		int voltage_shift = ffs(info->voltage_mask) - 1;
+
+		regval = (u8)selector << voltage_shift;
+		ret = abx500_mask_and_set_register_interruptible(info->dev,
+					info->voltage_bank, info->voltage_reg,
+					info->voltage_mask, regval);
+		if (ret < 0) {
+			dev_err(rdev_get_dev(rdev),
+				"couldn't set voltage reg for regulator\n");
+			return ret;
+		}
 
-	return info->delay;
+		dev_vdbg(rdev_get_dev(rdev),
+			 "%s-set_voltage (bank, reg, mask, value): 0x%x, 0x%x, 0x%x, 0x%x\n",
+			 info->desc.name, info->voltage_bank, info->voltage_reg,
+			 info->voltage_mask, regval);
+
+		regval_expand = 0;
+	} else {
+		regval_expand = info->expand_register.voltage_mask;
+	}
+
+	ret = abx500_mask_and_set_register_interruptible(info->dev,
+				info->expand_register.voltage_bank,
+				info->expand_register.voltage_reg,
+				info->expand_register.voltage_mask,
+				regval_expand);
+	if (ret < 0) {
+		dev_err(rdev_get_dev(rdev),
+			"couldn't set expand voltage reg for regulator\n");
+		return ret;
+	}
+
+	dev_vdbg(rdev_get_dev(rdev),
+		 "%s-set_voltage expand (bank, reg, mask, value): 0x%x, 0x%x, 0x%x, 0x%x\n",
+		 info->desc.name, info->expand_register.voltage_bank,
+		 info->expand_register.voltage_reg,
+		 info->expand_register.voltage_mask, regval_expand);
+
+	return 0;
 }
 
-static struct regulator_ops ab8500_regulator_ops = {
+static struct regulator_ops ab8500_regulator_volt_mode_ops = {
+	.enable			= ab8500_regulator_enable,
+	.disable		= ab8500_regulator_disable,
+	.is_enabled		= ab8500_regulator_is_enabled,
+	.get_optimum_mode	= ab8500_regulator_get_optimum_mode,
+	.set_mode		= ab8500_regulator_set_mode,
+	.get_mode		= ab8500_regulator_get_mode,
+	.get_voltage_sel 	= ab8500_regulator_get_voltage_sel,
+	.set_voltage_sel	= ab8500_regulator_set_voltage_sel,
+	.list_voltage		= regulator_list_voltage_table,
+};
+
+static struct regulator_ops ab8540_aux3_regulator_volt_mode_ops = {
+	.enable		= ab8500_regulator_enable,
+	.disable	= ab8500_regulator_disable,
+	.get_optimum_mode	= ab8500_regulator_get_optimum_mode,
+	.set_mode	= ab8500_regulator_set_mode,
+	.get_mode	= ab8500_regulator_get_mode,
+	.is_enabled	= ab8500_regulator_is_enabled,
+	.get_voltage_sel = ab8540_aux3_regulator_get_voltage_sel,
+	.set_voltage_sel = ab8540_aux3_regulator_set_voltage_sel,
+	.list_voltage	= regulator_list_voltage_table,
+};
+
+static struct regulator_ops ab8500_regulator_volt_ops = {
 	.enable		= ab8500_regulator_enable,
 	.disable	= ab8500_regulator_disable,
 	.is_enabled	= ab8500_regulator_is_enabled,
 	.get_voltage_sel = ab8500_regulator_get_voltage_sel,
 	.set_voltage_sel = ab8500_regulator_set_voltage_sel,
 	.list_voltage	= regulator_list_voltage_table,
-	.set_voltage_time_sel = ab8500_regulator_set_voltage_time_sel,
 };
 
-static struct regulator_ops ab8500_regulator_fixed_ops = {
+static struct regulator_ops ab8500_regulator_mode_ops = {
+	.enable			= ab8500_regulator_enable,
+	.disable		= ab8500_regulator_disable,
+	.is_enabled		= ab8500_regulator_is_enabled,
+	.get_optimum_mode	= ab8500_regulator_get_optimum_mode,
+	.set_mode		= ab8500_regulator_set_mode,
+	.get_mode		= ab8500_regulator_get_mode,
+	.list_voltage		= regulator_list_voltage_table,
+};
+
+static struct regulator_ops ab8500_regulator_ops = {
+	.enable			= ab8500_regulator_enable,
+	.disable		= ab8500_regulator_disable,
+	.is_enabled		= ab8500_regulator_is_enabled,
+	.list_voltage		= regulator_list_voltage_table,
+};
+
+static struct regulator_ops ab8500_regulator_anamic_mode_ops = {
 	.enable		= ab8500_regulator_enable,
 	.disable	= ab8500_regulator_disable,
 	.is_enabled	= ab8500_regulator_is_enabled,
-	.list_voltage	= regulator_list_voltage_linear,
+	.set_mode	= ab8500_regulator_set_mode,
+	.get_mode	= ab8500_regulator_get_mode,
+	.list_voltage	= regulator_list_voltage_table,
 };
 
+/* AB8500 regulator information */
 static struct ab8500_regulator_info
 		ab8500_regulator_info[AB8500_NUM_REGULATORS] = {
 	/*
@@ -274,17 +712,21 @@ static struct ab8500_regulator_info
 	[AB8500_LDO_AUX1] = {
 		.desc = {
 			.name		= "LDO-AUX1",
-			.ops		= &ab8500_regulator_ops,
+			.ops		= &ab8500_regulator_volt_mode_ops,
 			.type		= REGULATOR_VOLTAGE,
 			.id		= AB8500_LDO_AUX1,
 			.owner		= THIS_MODULE,
 			.n_voltages	= ARRAY_SIZE(ldo_vauxn_voltages),
 			.volt_table	= ldo_vauxn_voltages,
+			.enable_time	= 200,
 		},
+		.load_lp_uA		= 5000,
 		.update_bank		= 0x04,
 		.update_reg		= 0x09,
 		.update_mask		= 0x03,
-		.update_val_enable	= 0x01,
+		.update_val		= 0x01,
+		.update_val_idle	= 0x03,
+		.update_val_normal	= 0x01,
 		.voltage_bank		= 0x04,
 		.voltage_reg		= 0x1f,
 		.voltage_mask		= 0x0f,
@@ -292,17 +734,21 @@ static struct ab8500_regulator_info
 	[AB8500_LDO_AUX2] = {
 		.desc = {
 			.name		= "LDO-AUX2",
-			.ops		= &ab8500_regulator_ops,
+			.ops		= &ab8500_regulator_volt_mode_ops,
 			.type		= REGULATOR_VOLTAGE,
 			.id		= AB8500_LDO_AUX2,
 			.owner		= THIS_MODULE,
 			.n_voltages	= ARRAY_SIZE(ldo_vauxn_voltages),
 			.volt_table	= ldo_vauxn_voltages,
+			.enable_time	= 200,
 		},
+		.load_lp_uA		= 5000,
 		.update_bank		= 0x04,
 		.update_reg		= 0x09,
 		.update_mask		= 0x0c,
-		.update_val_enable	= 0x04,
+		.update_val		= 0x04,
+		.update_val_idle	= 0x0c,
+		.update_val_normal	= 0x04,
 		.voltage_bank		= 0x04,
 		.voltage_reg		= 0x20,
 		.voltage_mask		= 0x0f,
@@ -310,17 +756,21 @@ static struct ab8500_regulator_info
 	[AB8500_LDO_AUX3] = {
 		.desc = {
 			.name		= "LDO-AUX3",
-			.ops		= &ab8500_regulator_ops,
+			.ops		= &ab8500_regulator_volt_mode_ops,
 			.type		= REGULATOR_VOLTAGE,
 			.id		= AB8500_LDO_AUX3,
 			.owner		= THIS_MODULE,
 			.n_voltages	= ARRAY_SIZE(ldo_vaux3_voltages),
 			.volt_table	= ldo_vaux3_voltages,
+			.enable_time	= 450,
 		},
+		.load_lp_uA		= 5000,
 		.update_bank		= 0x04,
 		.update_reg		= 0x0a,
 		.update_mask		= 0x03,
-		.update_val_enable	= 0x01,
+		.update_val		= 0x01,
+		.update_val_idle	= 0x03,
+		.update_val_normal	= 0x01,
 		.voltage_bank		= 0x04,
 		.voltage_reg		= 0x21,
 		.voltage_mask		= 0x07,
@@ -328,21 +778,24 @@ static struct ab8500_regulator_info
 	[AB8500_LDO_INTCORE] = {
 		.desc = {
 			.name		= "LDO-INTCORE",
-			.ops		= &ab8500_regulator_ops,
+			.ops		= &ab8500_regulator_volt_mode_ops,
 			.type		= REGULATOR_VOLTAGE,
 			.id		= AB8500_LDO_INTCORE,
 			.owner		= THIS_MODULE,
 			.n_voltages	= ARRAY_SIZE(ldo_vintcore_voltages),
 			.volt_table	= ldo_vintcore_voltages,
+			.enable_time	= 750,
 		},
+		.load_lp_uA		= 5000,
 		.update_bank		= 0x03,
 		.update_reg		= 0x80,
 		.update_mask		= 0x44,
-		.update_val_enable	= 0x04,
+		.update_val		= 0x44,
+		.update_val_idle	= 0x44,
+		.update_val_normal	= 0x04,
 		.voltage_bank		= 0x03,
 		.voltage_reg		= 0x80,
 		.voltage_mask		= 0x38,
-		.voltage_shift		= 3,
 	},
 
 	/*
@@ -353,112 +806,984 @@ static struct ab8500_regulator_info
 	[AB8500_LDO_TVOUT] = {
 		.desc = {
 			.name		= "LDO-TVOUT",
-			.ops		= &ab8500_regulator_fixed_ops,
+			.ops		= &ab8500_regulator_mode_ops,
 			.type		= REGULATOR_VOLTAGE,
 			.id		= AB8500_LDO_TVOUT,
 			.owner		= THIS_MODULE,
 			.n_voltages	= 1,
-			.min_uV		= 2000000,
+			.volt_table	= fixed_2000000_voltage,
+			.enable_time	= 500,
+		},
+		.load_lp_uA		= 1000,
+		.update_bank		= 0x03,
+		.update_reg		= 0x80,
+		.update_mask		= 0x82,
+		.update_val		= 0x02,
+		.update_val_idle	= 0x82,
+		.update_val_normal	= 0x02,
+	},
+	[AB8500_LDO_AUDIO] = {
+		.desc = {
+			.name		= "LDO-AUDIO",
+			.ops		= &ab8500_regulator_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_LDO_AUDIO,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+			.enable_time	= 140,
+			.volt_table	= fixed_2000000_voltage,
+		},
+		.update_bank		= 0x03,
+		.update_reg		= 0x83,
+		.update_mask		= 0x02,
+		.update_val		= 0x02,
+	},
+	[AB8500_LDO_ANAMIC1] = {
+		.desc = {
+			.name		= "LDO-ANAMIC1",
+			.ops		= &ab8500_regulator_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_LDO_ANAMIC1,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+			.enable_time	= 500,
+			.volt_table	= fixed_2050000_voltage,
+		},
+		.update_bank		= 0x03,
+		.update_reg		= 0x83,
+		.update_mask		= 0x08,
+		.update_val		= 0x08,
+	},
+	[AB8500_LDO_ANAMIC2] = {
+		.desc = {
+			.name		= "LDO-ANAMIC2",
+			.ops		= &ab8500_regulator_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_LDO_ANAMIC2,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+			.enable_time	= 500,
+			.volt_table	= fixed_2050000_voltage,
+		},
+		.update_bank		= 0x03,
+		.update_reg		= 0x83,
+		.update_mask		= 0x10,
+		.update_val		= 0x10,
+	},
+	[AB8500_LDO_DMIC] = {
+		.desc = {
+			.name		= "LDO-DMIC",
+			.ops		= &ab8500_regulator_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_LDO_DMIC,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+			.enable_time	= 420,
+			.volt_table	= fixed_1800000_voltage,
+		},
+		.update_bank		= 0x03,
+		.update_reg		= 0x83,
+		.update_mask		= 0x04,
+		.update_val		= 0x04,
+	},
+
+	/*
+	 * Regulators with fixed voltage and normal/idle modes
+	 */
+	[AB8500_LDO_ANA] = {
+		.desc = {
+			.name		= "LDO-ANA",
+			.ops		= &ab8500_regulator_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_LDO_ANA,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+			.enable_time	= 140,
+			.volt_table	= fixed_1200000_voltage,
+		},
+		.load_lp_uA		= 1000,
+		.update_bank		= 0x04,
+		.update_reg		= 0x06,
+		.update_mask		= 0x0c,
+		.update_val		= 0x04,
+		.update_val_idle	= 0x0c,
+		.update_val_normal	= 0x04,
+	},
+};
+
+/* AB8505 regulator information */
+static struct ab8500_regulator_info
+		ab8505_regulator_info[AB8505_NUM_REGULATORS] = {
+	/*
+	 * Variable Voltage Regulators
+	 *   name, min mV, max mV,
+	 *   update bank, reg, mask, enable val
+	 *   volt bank, reg, mask
+	 */
+	[AB8505_LDO_AUX1] = {
+		.desc = {
+			.name		= "LDO-AUX1",
+			.ops		= &ab8500_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8505_LDO_AUX1,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vauxn_voltages),
+			.volt_table	= ldo_vauxn_voltages,
+		},
+		.load_lp_uA		= 5000,
+		.update_bank		= 0x04,
+		.update_reg		= 0x09,
+		.update_mask		= 0x03,
+		.update_val		= 0x01,
+		.update_val_idle	= 0x03,
+		.update_val_normal	= 0x01,
+		.voltage_bank		= 0x04,
+		.voltage_reg		= 0x1f,
+		.voltage_mask		= 0x0f,
+	},
+	[AB8505_LDO_AUX2] = {
+		.desc = {
+			.name		= "LDO-AUX2",
+			.ops		= &ab8500_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8505_LDO_AUX2,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vauxn_voltages),
+			.volt_table	= ldo_vauxn_voltages,
+		},
+		.load_lp_uA		= 5000,
+		.update_bank		= 0x04,
+		.update_reg		= 0x09,
+		.update_mask		= 0x0c,
+		.update_val		= 0x04,
+		.update_val_idle	= 0x0c,
+		.update_val_normal	= 0x04,
+		.voltage_bank		= 0x04,
+		.voltage_reg		= 0x20,
+		.voltage_mask		= 0x0f,
+	},
+	[AB8505_LDO_AUX3] = {
+		.desc = {
+			.name		= "LDO-AUX3",
+			.ops		= &ab8500_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8505_LDO_AUX3,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vaux3_voltages),
+			.volt_table	= ldo_vaux3_voltages,
+		},
+		.load_lp_uA		= 5000,
+		.update_bank		= 0x04,
+		.update_reg		= 0x0a,
+		.update_mask		= 0x03,
+		.update_val		= 0x01,
+		.update_val_idle	= 0x03,
+		.update_val_normal	= 0x01,
+		.voltage_bank		= 0x04,
+		.voltage_reg		= 0x21,
+		.voltage_mask		= 0x07,
+	},
+	[AB8505_LDO_AUX4] = {
+		.desc = {
+			.name		= "LDO-AUX4",
+			.ops		= &ab8500_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8505_LDO_AUX4,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vauxn_voltages),
+			.volt_table	= ldo_vauxn_voltages,
+		},
+		.load_lp_uA		= 5000,
+		/* values for Vaux4Regu register */
+		.update_bank		= 0x04,
+		.update_reg		= 0x2e,
+		.update_mask		= 0x03,
+		.update_val		= 0x01,
+		.update_val_idle	= 0x03,
+		.update_val_normal	= 0x01,
+		/* values for Vaux4SEL register */
+		.voltage_bank		= 0x04,
+		.voltage_reg		= 0x2f,
+		.voltage_mask		= 0x0f,
+	},
+	[AB8505_LDO_AUX5] = {
+		.desc = {
+			.name		= "LDO-AUX5",
+			.ops		= &ab8500_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8505_LDO_AUX5,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vaux56_voltages),
+			.volt_table	= ldo_vaux56_voltages,
+		},
+		.load_lp_uA		= 2000,
+		/* values for CtrlVaux5 register */
+		.update_bank		= 0x01,
+		.update_reg		= 0x55,
+		.update_mask		= 0x18,
+		.update_val		= 0x10,
+		.update_val_idle	= 0x18,
+		.update_val_normal	= 0x10,
+		.voltage_bank		= 0x01,
+		.voltage_reg		= 0x55,
+		.voltage_mask		= 0x07,
+	},
+	[AB8505_LDO_AUX6] = {
+		.desc = {
+			.name		= "LDO-AUX6",
+			.ops		= &ab8500_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8505_LDO_AUX6,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vaux56_voltages),
+			.volt_table	= ldo_vaux56_voltages,
+		},
+		.load_lp_uA		= 2000,
+		/* values for CtrlVaux6 register */
+		.update_bank		= 0x01,
+		.update_reg		= 0x56,
+		.update_mask		= 0x18,
+		.update_val		= 0x10,
+		.update_val_idle	= 0x18,
+		.update_val_normal	= 0x10,
+		.voltage_bank		= 0x01,
+		.voltage_reg		= 0x56,
+		.voltage_mask		= 0x07,
+	},
+	[AB8505_LDO_INTCORE] = {
+		.desc = {
+			.name		= "LDO-INTCORE",
+			.ops		= &ab8500_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8505_LDO_INTCORE,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vintcore_voltages),
+			.volt_table	= ldo_vintcore_voltages,
+		},
+		.load_lp_uA		= 5000,
+		.update_bank		= 0x03,
+		.update_reg		= 0x80,
+		.update_mask		= 0x44,
+		.update_val		= 0x04,
+		.update_val_idle	= 0x44,
+		.update_val_normal	= 0x04,
+		.voltage_bank		= 0x03,
+		.voltage_reg		= 0x80,
+		.voltage_mask		= 0x38,
+	},
+
+	/*
+	 * Fixed Voltage Regulators
+	 *   name, fixed mV,
+	 *   update bank, reg, mask, enable val
+	 */
+	[AB8505_LDO_ADC] = {
+		.desc = {
+			.name		= "LDO-ADC",
+			.ops		= &ab8500_regulator_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8505_LDO_ADC,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+			.volt_table	= fixed_2000000_voltage,
 			.enable_time	= 10000,
 		},
-		.delay			= 10000,
+		.load_lp_uA		= 1000,
 		.update_bank		= 0x03,
 		.update_reg		= 0x80,
 		.update_mask		= 0x82,
-		.update_val_enable	= 0x02,
+		.update_val		= 0x02,
+		.update_val_idle	= 0x82,
+		.update_val_normal	= 0x02,
 	},
-	[AB8500_LDO_USB] = {
+	[AB8505_LDO_USB] = {
 		.desc = {
 			.name           = "LDO-USB",
-			.ops            = &ab8500_regulator_fixed_ops,
+			.ops            = &ab8500_regulator_mode_ops,
 			.type           = REGULATOR_VOLTAGE,
-			.id             = AB8500_LDO_USB,
+			.id             = AB8505_LDO_USB,
 			.owner          = THIS_MODULE,
 			.n_voltages     = 1,
-			.min_uV		= 3300000,
+			.volt_table	= fixed_3300000_voltage,
 		},
 		.update_bank            = 0x03,
 		.update_reg             = 0x82,
 		.update_mask            = 0x03,
-		.update_val_enable      = 0x01,
+		.update_val		= 0x01,
+		.update_val_idle	= 0x03,
+		.update_val_normal	= 0x01,
 	},
-	[AB8500_LDO_AUDIO] = {
+	[AB8505_LDO_AUDIO] = {
 		.desc = {
 			.name		= "LDO-AUDIO",
-			.ops		= &ab8500_regulator_fixed_ops,
+			.ops		= &ab8500_regulator_volt_ops,
 			.type		= REGULATOR_VOLTAGE,
-			.id		= AB8500_LDO_AUDIO,
+			.id		= AB8505_LDO_AUDIO,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vaudio_voltages),
+			.volt_table	= ldo_vaudio_voltages,
+		},
+		.update_bank		= 0x03,
+		.update_reg		= 0x83,
+		.update_mask		= 0x02,
+		.update_val		= 0x02,
+		.voltage_bank		= 0x01,
+		.voltage_reg		= 0x57,
+		.voltage_mask		= 0x70,
+	},
+	[AB8505_LDO_ANAMIC1] = {
+		.desc = {
+			.name		= "LDO-ANAMIC1",
+			.ops		= &ab8500_regulator_anamic_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8505_LDO_ANAMIC1,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+			.volt_table	= fixed_2050000_voltage,
+		},
+		.shared_mode		= &ldo_anamic1_shared,
+		.update_bank		= 0x03,
+		.update_reg		= 0x83,
+		.update_mask		= 0x08,
+		.update_val		= 0x08,
+		.mode_bank		= 0x01,
+		.mode_reg		= 0x54,
+		.mode_mask		= 0x04,
+		.mode_val_idle		= 0x04,
+		.mode_val_normal	= 0x00,
+	},
+	[AB8505_LDO_ANAMIC2] = {
+		.desc = {
+			.name		= "LDO-ANAMIC2",
+			.ops		= &ab8500_regulator_anamic_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8505_LDO_ANAMIC2,
 			.owner		= THIS_MODULE,
 			.n_voltages	= 1,
-			.min_uV		= 2000000,
+			.volt_table	= fixed_2050000_voltage,
+		},
+		.shared_mode		= &ldo_anamic2_shared,
+		.update_bank		= 0x03,
+		.update_reg		= 0x83,
+		.update_mask		= 0x10,
+		.update_val		= 0x10,
+		.mode_bank		= 0x01,
+		.mode_reg		= 0x54,
+		.mode_mask		= 0x04,
+		.mode_val_idle		= 0x04,
+		.mode_val_normal	= 0x00,
+	},
+	[AB8505_LDO_AUX8] = {
+		.desc = {
+			.name		= "LDO-AUX8",
+			.ops		= &ab8500_regulator_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8505_LDO_AUX8,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+			.volt_table	= fixed_1800000_voltage,
+		},
+		.update_bank		= 0x03,
+		.update_reg		= 0x83,
+		.update_mask		= 0x04,
+		.update_val		= 0x04,
+	},
+	/*
+	 * Regulators with fixed voltage and normal/idle modes
+	 */
+	[AB8505_LDO_ANA] = {
+		.desc = {
+			.name		= "LDO-ANA",
+			.ops		= &ab8500_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8505_LDO_ANA,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vana_voltages),
+			.volt_table	= ldo_vana_voltages,
+		},
+		.load_lp_uA		= 1000,
+		.update_bank		= 0x04,
+		.update_reg		= 0x06,
+		.update_mask		= 0x0c,
+		.update_val		= 0x04,
+		.update_val_idle	= 0x0c,
+		.update_val_normal	= 0x04,
+		.voltage_bank		= 0x04,
+		.voltage_reg		= 0x29,
+		.voltage_mask		= 0x7,
+	},
+};
+
+/* AB9540 regulator information */
+static struct ab8500_regulator_info
+		ab9540_regulator_info[AB9540_NUM_REGULATORS] = {
+	/*
+	 * Variable Voltage Regulators
+	 *   name, min mV, max mV,
+	 *   update bank, reg, mask, enable val
+	 *   volt bank, reg, mask
+	 */
+	[AB9540_LDO_AUX1] = {
+		.desc = {
+			.name		= "LDO-AUX1",
+			.ops		= &ab8500_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB9540_LDO_AUX1,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vauxn_voltages),
+			.volt_table	= ldo_vauxn_voltages,
+		},
+		.load_lp_uA		= 5000,
+		.update_bank		= 0x04,
+		.update_reg		= 0x09,
+		.update_mask		= 0x03,
+		.update_val		= 0x01,
+		.update_val_idle	= 0x03,
+		.update_val_normal	= 0x01,
+		.voltage_bank		= 0x04,
+		.voltage_reg		= 0x1f,
+		.voltage_mask		= 0x0f,
+	},
+	[AB9540_LDO_AUX2] = {
+		.desc = {
+			.name		= "LDO-AUX2",
+			.ops		= &ab8500_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB9540_LDO_AUX2,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vauxn_voltages),
+			.volt_table	= ldo_vauxn_voltages,
+		},
+		.load_lp_uA		= 5000,
+		.update_bank		= 0x04,
+		.update_reg		= 0x09,
+		.update_mask		= 0x0c,
+		.update_val		= 0x04,
+		.update_val_idle	= 0x0c,
+		.update_val_normal	= 0x04,
+		.voltage_bank		= 0x04,
+		.voltage_reg		= 0x20,
+		.voltage_mask		= 0x0f,
+	},
+	[AB9540_LDO_AUX3] = {
+		.desc = {
+			.name		= "LDO-AUX3",
+			.ops		= &ab8500_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB9540_LDO_AUX3,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vaux3_voltages),
+			.volt_table	= ldo_vaux3_voltages,
+		},
+		.load_lp_uA		= 5000,
+		.update_bank		= 0x04,
+		.update_reg		= 0x0a,
+		.update_mask		= 0x03,
+		.update_val		= 0x01,
+		.update_val_idle	= 0x03,
+		.update_val_normal	= 0x01,
+		.voltage_bank		= 0x04,
+		.voltage_reg		= 0x21,
+		.voltage_mask		= 0x07,
+	},
+	[AB9540_LDO_AUX4] = {
+		.desc = {
+			.name		= "LDO-AUX4",
+			.ops		= &ab8500_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB9540_LDO_AUX4,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vauxn_voltages),
+			.volt_table	= ldo_vauxn_voltages,
+		},
+		.load_lp_uA		= 5000,
+		/* values for Vaux4Regu register */
+		.update_bank		= 0x04,
+		.update_reg		= 0x2e,
+		.update_mask		= 0x03,
+		.update_val		= 0x01,
+		.update_val_idle	= 0x03,
+		.update_val_normal	= 0x01,
+		/* values for Vaux4SEL register */
+		.voltage_bank		= 0x04,
+		.voltage_reg		= 0x2f,
+		.voltage_mask		= 0x0f,
+	},
+	[AB9540_LDO_INTCORE] = {
+		.desc = {
+			.name		= "LDO-INTCORE",
+			.ops		= &ab8500_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB9540_LDO_INTCORE,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vintcore_voltages),
+			.volt_table	= ldo_vintcore_voltages,
+		},
+		.load_lp_uA		= 5000,
+		.update_bank		= 0x03,
+		.update_reg		= 0x80,
+		.update_mask		= 0x44,
+		.update_val		= 0x44,
+		.update_val_idle	= 0x44,
+		.update_val_normal	= 0x04,
+		.voltage_bank		= 0x03,
+		.voltage_reg		= 0x80,
+		.voltage_mask		= 0x38,
+	},
+
+	/*
+	 * Fixed Voltage Regulators
+	 *   name, fixed mV,
+	 *   update bank, reg, mask, enable val
+	 */
+	[AB9540_LDO_TVOUT] = {
+		.desc = {
+			.name		= "LDO-TVOUT",
+			.ops		= &ab8500_regulator_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB9540_LDO_TVOUT,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+			.volt_table	= fixed_2000000_voltage,
+			.enable_time	= 10000,
+		},
+		.load_lp_uA		= 1000,
+		.update_bank		= 0x03,
+		.update_reg		= 0x80,
+		.update_mask		= 0x82,
+		.update_val		= 0x02,
+		.update_val_idle	= 0x82,
+		.update_val_normal	= 0x02,
+	},
+	[AB9540_LDO_USB] = {
+		.desc = {
+			.name           = "LDO-USB",
+			.ops            = &ab8500_regulator_ops,
+			.type           = REGULATOR_VOLTAGE,
+			.id             = AB9540_LDO_USB,
+			.owner          = THIS_MODULE,
+			.n_voltages     = 1,
+			.volt_table	= fixed_3300000_voltage,
+		},
+		.update_bank            = 0x03,
+		.update_reg             = 0x82,
+		.update_mask            = 0x03,
+		.update_val		= 0x01,
+		.update_val_idle	= 0x03,
+		.update_val_normal	= 0x01,
+	},
+	[AB9540_LDO_AUDIO] = {
+		.desc = {
+			.name		= "LDO-AUDIO",
+			.ops		= &ab8500_regulator_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB9540_LDO_AUDIO,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+			.volt_table	= fixed_2000000_voltage,
 		},
 		.update_bank		= 0x03,
 		.update_reg		= 0x83,
 		.update_mask		= 0x02,
-		.update_val_enable	= 0x02,
+		.update_val		= 0x02,
 	},
-	[AB8500_LDO_ANAMIC1] = {
+	[AB9540_LDO_ANAMIC1] = {
 		.desc = {
 			.name		= "LDO-ANAMIC1",
-			.ops		= &ab8500_regulator_fixed_ops,
+			.ops		= &ab8500_regulator_ops,
 			.type		= REGULATOR_VOLTAGE,
-			.id		= AB8500_LDO_ANAMIC1,
+			.id		= AB9540_LDO_ANAMIC1,
 			.owner		= THIS_MODULE,
 			.n_voltages	= 1,
-			.min_uV		= 2050000,
+			.volt_table	= fixed_2050000_voltage,
 		},
 		.update_bank		= 0x03,
 		.update_reg		= 0x83,
 		.update_mask		= 0x08,
-		.update_val_enable	= 0x08,
+		.update_val		= 0x08,
 	},
-	[AB8500_LDO_ANAMIC2] = {
+	[AB9540_LDO_ANAMIC2] = {
 		.desc = {
 			.name		= "LDO-ANAMIC2",
-			.ops		= &ab8500_regulator_fixed_ops,
+			.ops		= &ab8500_regulator_ops,
 			.type		= REGULATOR_VOLTAGE,
-			.id		= AB8500_LDO_ANAMIC2,
+			.id		= AB9540_LDO_ANAMIC2,
 			.owner		= THIS_MODULE,
 			.n_voltages	= 1,
-			.min_uV		= 2050000,
+			.volt_table	= fixed_2050000_voltage,
 		},
 		.update_bank		= 0x03,
 		.update_reg		= 0x83,
 		.update_mask		= 0x10,
-		.update_val_enable	= 0x10,
+		.update_val		= 0x10,
 	},
-	[AB8500_LDO_DMIC] = {
+	[AB9540_LDO_DMIC] = {
 		.desc = {
 			.name		= "LDO-DMIC",
-			.ops		= &ab8500_regulator_fixed_ops,
+			.ops		= &ab8500_regulator_ops,
 			.type		= REGULATOR_VOLTAGE,
-			.id		= AB8500_LDO_DMIC,
+			.id		= AB9540_LDO_DMIC,
 			.owner		= THIS_MODULE,
 			.n_voltages	= 1,
-			.min_uV		= 1800000,
+			.volt_table	= fixed_1800000_voltage,
 		},
 		.update_bank		= 0x03,
 		.update_reg		= 0x83,
 		.update_mask		= 0x04,
-		.update_val_enable	= 0x04,
+		.update_val		= 0x04,
 	},
-	[AB8500_LDO_ANA] = {
+
+	/*
+	 * Regulators with fixed voltage and normal/idle modes
+	 */
+	[AB9540_LDO_ANA] = {
 		.desc = {
 			.name		= "LDO-ANA",
-			.ops		= &ab8500_regulator_fixed_ops,
+			.ops		= &ab8500_regulator_mode_ops,
 			.type		= REGULATOR_VOLTAGE,
-			.id		= AB8500_LDO_ANA,
+			.id		= AB9540_LDO_ANA,
 			.owner		= THIS_MODULE,
 			.n_voltages	= 1,
-			.min_uV		= 1200000,
+			.volt_table	= fixed_1200000_voltage,
 		},
+		.load_lp_uA		= 1000,
 		.update_bank		= 0x04,
 		.update_reg		= 0x06,
 		.update_mask		= 0x0c,
-		.update_val_enable	= 0x04,
+		.update_val		= 0x08,
+		.update_val_idle	= 0x0c,
+		.update_val_normal	= 0x08,
 	},
+};
 
+/* AB8540 regulator information */
+static struct ab8500_regulator_info
+		ab8540_regulator_info[AB8540_NUM_REGULATORS] = {
+	/*
+	 * Variable Voltage Regulators
+	 *   name, min mV, max mV,
+	 *   update bank, reg, mask, enable val
+	 *   volt bank, reg, mask
+	 */
+	[AB8540_LDO_AUX1] = {
+		.desc = {
+			.name		= "LDO-AUX1",
+			.ops		= &ab8500_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8540_LDO_AUX1,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vauxn_voltages),
+			.volt_table	= ldo_vauxn_voltages,
+		},
+		.load_lp_uA		= 5000,
+		.update_bank		= 0x04,
+		.update_reg		= 0x09,
+		.update_mask		= 0x03,
+		.update_val		= 0x01,
+		.update_val_idle	= 0x03,
+		.update_val_normal	= 0x01,
+		.voltage_bank		= 0x04,
+		.voltage_reg		= 0x1f,
+		.voltage_mask		= 0x0f,
+	},
+	[AB8540_LDO_AUX2] = {
+		.desc = {
+			.name		= "LDO-AUX2",
+			.ops		= &ab8500_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8540_LDO_AUX2,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vauxn_voltages),
+			.volt_table	= ldo_vauxn_voltages,
+		},
+		.load_lp_uA		= 5000,
+		.update_bank		= 0x04,
+		.update_reg		= 0x09,
+		.update_mask		= 0x0c,
+		.update_val		= 0x04,
+		.update_val_idle	= 0x0c,
+		.update_val_normal	= 0x04,
+		.voltage_bank		= 0x04,
+		.voltage_reg		= 0x20,
+		.voltage_mask		= 0x0f,
+	},
+	[AB8540_LDO_AUX3] = {
+		.desc = {
+			.name		= "LDO-AUX3",
+			.ops		= &ab8540_aux3_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8540_LDO_AUX3,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vaux3_ab8540_voltages),
+			.volt_table	= ldo_vaux3_ab8540_voltages,
+		},
+		.load_lp_uA		= 5000,
+		.update_bank		= 0x04,
+		.update_reg		= 0x0a,
+		.update_mask		= 0x03,
+		.update_val		= 0x01,
+		.update_val_idle	= 0x03,
+		.update_val_normal	= 0x01,
+		.voltage_bank		= 0x04,
+		.voltage_reg		= 0x21,
+		.voltage_mask		= 0x07,
+		.expand_register = {
+			.voltage_limit		= 8,
+			.voltage_bank		= 0x04,
+			.voltage_reg		= 0x01,
+			.voltage_mask		= 0x10,
+		}
+	},
+	[AB8540_LDO_AUX4] = {
+		.desc = {
+			.name		= "LDO-AUX4",
+			.ops		= &ab8500_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8540_LDO_AUX4,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vauxn_voltages),
+			.volt_table	= ldo_vauxn_voltages,
+		},
+		.load_lp_uA		= 5000,
+		/* values for Vaux4Regu register */
+		.update_bank		= 0x04,
+		.update_reg		= 0x2e,
+		.update_mask		= 0x03,
+		.update_val		= 0x01,
+		.update_val_idle	= 0x03,
+		.update_val_normal	= 0x01,
+		/* values for Vaux4SEL register */
+		.voltage_bank		= 0x04,
+		.voltage_reg		= 0x2f,
+		.voltage_mask		= 0x0f,
+	},
+	[AB8540_LDO_AUX5] = {
+		.desc = {
+			.name		= "LDO-AUX5",
+			.ops		= &ab8500_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8540_LDO_AUX5,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vaux56_ab8540_voltages),
+			.volt_table	= ldo_vaux56_ab8540_voltages,
+		},
+		.load_lp_uA		= 20000,
+		/* values for Vaux5Regu register */
+		.update_bank		= 0x04,
+		.update_reg		= 0x32,
+		.update_mask		= 0x03,
+		.update_val		= 0x01,
+		.update_val_idle	= 0x03,
+		.update_val_normal	= 0x01,
+		/* values for Vaux5SEL register */
+		.voltage_bank		= 0x04,
+		.voltage_reg		= 0x33,
+		.voltage_mask		= 0x3f,
+	},
+	[AB8540_LDO_AUX6] = {
+		.desc = {
+			.name		= "LDO-AUX6",
+			.ops		= &ab8500_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8540_LDO_AUX6,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vaux56_ab8540_voltages),
+			.volt_table	= ldo_vaux56_ab8540_voltages,
+		},
+		.load_lp_uA		= 20000,
+		/* values for Vaux6Regu register */
+		.update_bank		= 0x04,
+		.update_reg		= 0x35,
+		.update_mask		= 0x03,
+		.update_val		= 0x01,
+		.update_val_idle	= 0x03,
+		.update_val_normal	= 0x01,
+		/* values for Vaux6SEL register */
+		.voltage_bank		= 0x04,
+		.voltage_reg		= 0x36,
+		.voltage_mask		= 0x3f,
+	},
+	[AB8540_LDO_INTCORE] = {
+		.desc = {
+			.name		= "LDO-INTCORE",
+			.ops		= &ab8500_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8540_LDO_INTCORE,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vintcore_voltages),
+			.volt_table	= ldo_vintcore_voltages,
+		},
+		.load_lp_uA		= 5000,
+		.update_bank		= 0x03,
+		.update_reg		= 0x80,
+		.update_mask		= 0x44,
+		.update_val		= 0x44,
+		.update_val_idle	= 0x44,
+		.update_val_normal	= 0x04,
+		.voltage_bank		= 0x03,
+		.voltage_reg		= 0x80,
+		.voltage_mask		= 0x38,
+	},
 
+	/*
+	 * Fixed Voltage Regulators
+	 *   name, fixed mV,
+	 *   update bank, reg, mask, enable val
+	 */
+	[AB8540_LDO_TVOUT] = {
+		.desc = {
+			.name		= "LDO-TVOUT",
+			.ops		= &ab8500_regulator_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8540_LDO_TVOUT,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+			.volt_table     = fixed_2000000_voltage,
+			.enable_time	= 10000,
+		},
+		.load_lp_uA		= 1000,
+		.update_bank		= 0x03,
+		.update_reg		= 0x80,
+		.update_mask		= 0x82,
+		.update_val		= 0x02,
+		.update_val_idle	= 0x82,
+		.update_val_normal	= 0x02,
+	},
+	[AB8540_LDO_AUDIO] = {
+		.desc = {
+			.name		= "LDO-AUDIO",
+			.ops		= &ab8500_regulator_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8540_LDO_AUDIO,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+			.volt_table	= fixed_2000000_voltage,
+		},
+		.update_bank		= 0x03,
+		.update_reg		= 0x83,
+		.update_mask		= 0x02,
+		.update_val		= 0x02,
+	},
+	[AB8540_LDO_ANAMIC1] = {
+		.desc = {
+			.name		= "LDO-ANAMIC1",
+			.ops		= &ab8500_regulator_anamic_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8540_LDO_ANAMIC1,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+			.volt_table	= fixed_2050000_voltage,
+		},
+		.shared_mode		= &ab8540_ldo_anamic1_shared,
+		.update_bank		= 0x03,
+		.update_reg		= 0x83,
+		.update_mask		= 0x08,
+		.update_val		= 0x08,
+		.mode_bank		= 0x03,
+		.mode_reg		= 0x83,
+		.mode_mask		= 0x20,
+		.mode_val_idle		= 0x20,
+		.mode_val_normal	= 0x00,
+	},
+	[AB8540_LDO_ANAMIC2] = {
+		.desc = {
+			.name		= "LDO-ANAMIC2",
+			.ops		= &ab8500_regulator_anamic_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8540_LDO_ANAMIC2,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+			.volt_table	= fixed_2050000_voltage,
+		},
+		.shared_mode		= &ab8540_ldo_anamic2_shared,
+		.update_bank		= 0x03,
+		.update_reg		= 0x83,
+		.update_mask		= 0x10,
+		.update_val		= 0x10,
+		.mode_bank		= 0x03,
+		.mode_reg		= 0x83,
+		.mode_mask		= 0x20,
+		.mode_val_idle		= 0x20,
+		.mode_val_normal	= 0x00,
+	},
+	[AB8540_LDO_DMIC] = {
+		.desc = {
+			.name		= "LDO-DMIC",
+			.ops		= &ab8500_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8540_LDO_DMIC,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vdmic_voltages),
+			.volt_table	= ldo_vdmic_voltages,
+		},
+		.load_lp_uA		= 1000,
+		.update_bank		= 0x03,
+		.update_reg		= 0x83,
+		.update_mask		= 0x04,
+		.update_val		= 0x04,
+		.voltage_bank		= 0x03,
+		.voltage_reg		= 0x83,
+		.voltage_mask		= 0xc0,
+	},
+
+	/*
+	 * Regulators with fixed voltage and normal/idle modes
+	 */
+	[AB8540_LDO_ANA] = {
+		.desc = {
+			.name		= "LDO-ANA",
+			.ops		= &ab8500_regulator_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8540_LDO_ANA,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+			.volt_table     = fixed_1200000_voltage,
+		},
+		.load_lp_uA		= 1000,
+		.update_bank		= 0x04,
+		.update_reg		= 0x06,
+		.update_mask		= 0x0c,
+		.update_val		= 0x04,
+		.update_val_idle	= 0x0c,
+		.update_val_normal	= 0x04,
+	},
+	[AB8540_LDO_SDIO] = {
+		.desc = {
+			.name		= "LDO-SDIO",
+			.ops		= &ab8500_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8540_LDO_SDIO,
+			.owner		= THIS_MODULE,
+			.n_voltages 	= ARRAY_SIZE(ldo_sdio_voltages),
+			.volt_table	= ldo_sdio_voltages,
+		},
+		.load_lp_uA		= 5000,
+		.update_bank		= 0x03,
+		.update_reg		= 0x88,
+		.update_mask		= 0x30,
+		.update_val		= 0x10,
+		.update_val_idle	= 0x30,
+		.update_val_normal	= 0x10,
+		.voltage_bank		= 0x03,
+		.voltage_reg		= 0x88,
+		.voltage_mask		= 0x07,
+	},
+};
+
+static struct ab8500_shared_mode ldo_anamic1_shared = {
+	.shared_regulator = &ab8505_regulator_info[AB8505_LDO_ANAMIC2],
+};
+
+static struct ab8500_shared_mode ldo_anamic2_shared = {
+	.shared_regulator = &ab8505_regulator_info[AB8505_LDO_ANAMIC1],
+};
+
+static struct ab8500_shared_mode ab8540_ldo_anamic1_shared = {
+	.shared_regulator = &ab8540_regulator_info[AB8540_LDO_ANAMIC2],
+};
+
+static struct ab8500_shared_mode ab8540_ldo_anamic2_shared = {
+	.shared_regulator = &ab8540_regulator_info[AB8540_LDO_ANAMIC1],
 };
 
 struct ab8500_reg_init {
@@ -474,13 +1799,13 @@ struct ab8500_reg_init {
 		.mask = _mask,			\
 	}
 
+/* AB8500 register init */
 static struct ab8500_reg_init ab8500_reg_init[] = {
 	/*
 	 * 0x30, VanaRequestCtrl
-	 * 0x0C, VpllRequestCtrl
 	 * 0xc0, VextSupply1RequestCtrl
 	 */
-	REG_INIT(AB8500_REGUREQUESTCTRL2,	0x03, 0x04, 0xfc),
+	REG_INIT(AB8500_REGUREQUESTCTRL2,	0x03, 0x04, 0xf0),
 	/*
 	 * 0x03, VextSupply2RequestCtrl
 	 * 0x0c, VextSupply3RequestCtrl
@@ -547,13 +1872,21 @@ static struct ab8500_reg_init ab8500_reg_init[] = {
 	REG_INIT(AB8500_REGUSWHPREQVALID2,	0x03, 0x0e, 0x1f),
 	/*
 	 * 0x02, SysClkReq2Valid1
-	 * ...
+	 * 0x04, SysClkReq3Valid1
+	 * 0x08, SysClkReq4Valid1
+	 * 0x10, SysClkReq5Valid1
+	 * 0x20, SysClkReq6Valid1
+	 * 0x40, SysClkReq7Valid1
 	 * 0x80, SysClkReq8Valid1
 	 */
 	REG_INIT(AB8500_REGUSYSCLKREQVALID1,	0x03, 0x0f, 0xfe),
 	/*
 	 * 0x02, SysClkReq2Valid2
-	 * ...
+	 * 0x04, SysClkReq3Valid2
+	 * 0x08, SysClkReq4Valid2
+	 * 0x10, SysClkReq5Valid2
+	 * 0x20, SysClkReq6Valid2
+	 * 0x40, SysClkReq7Valid2
 	 * 0x80, SysClkReq8Valid2
 	 */
 	REG_INIT(AB8500_REGUSYSCLKREQVALID2,	0x03, 0x10, 0xfe),
@@ -578,8 +1911,8 @@ static struct ab8500_reg_init ab8500_reg_init[] = {
 	 */
 	REG_INIT(AB8500_REGUCTRL1VAMIC,		0x03, 0x84, 0x03),
 	/*
+	 * 0x03, VpllRegu (NOTE! PRCMU register bits)
 	 * 0x0c, VanaRegu
-	 * 0x03, VpllRegu
 	 */
 	REG_INIT(AB8500_VPLLVANAREGU,		0x04, 0x06, 0x0f),
 	/*
@@ -605,10 +1938,6 @@ static struct ab8500_reg_init ab8500_reg_init[] = {
 	 */
 	REG_INIT(AB8500_VRF1VAUX3REGU,		0x04, 0x0a, 0x03),
 	/*
-	 * 0x3f, Vsmps1Sel1
-	 */
-	REG_INIT(AB8500_VSMPS1SEL1,		0x04, 0x13, 0x3f),
-	/*
 	 * 0x0f, Vaux1Sel
 	 */
 	REG_INIT(AB8500_VAUX1SEL,		0x04, 0x1f, 0x0f),
@@ -641,52 +1970,1073 @@ static struct ab8500_reg_init ab8500_reg_init[] = {
 	REG_INIT(AB8500_REGUCTRLDISCH2,		0x04, 0x44, 0x16),
 };
 
-static int
-ab8500_regulator_init_registers(struct platform_device *pdev, int id, int value)
+/* AB8505 register init */
+static struct ab8500_reg_init ab8505_reg_init[] = {
+	/*
+	 * 0x03, VarmRequestCtrl
+	 * 0x0c, VsmpsCRequestCtrl
+	 * 0x30, VsmpsARequestCtrl
+	 * 0xc0, VsmpsBRequestCtrl
+	 */
+	REG_INIT(AB8505_REGUREQUESTCTRL1,	0x03, 0x03, 0xff),
+	/*
+	 * 0x03, VsafeRequestCtrl
+	 * 0x0c, VpllRequestCtrl
+	 * 0x30, VanaRequestCtrl
+	 */
+	REG_INIT(AB8505_REGUREQUESTCTRL2,	0x03, 0x04, 0x3f),
+	/*
+	 * 0x30, Vaux1RequestCtrl
+	 * 0xc0, Vaux2RequestCtrl
+	 */
+	REG_INIT(AB8505_REGUREQUESTCTRL3,	0x03, 0x05, 0xf0),
+	/*
+	 * 0x03, Vaux3RequestCtrl
+	 * 0x04, SwHPReq
+	 */
+	REG_INIT(AB8505_REGUREQUESTCTRL4,	0x03, 0x06, 0x07),
+	/*
+	 * 0x01, VsmpsASysClkReq1HPValid
+	 * 0x02, VsmpsBSysClkReq1HPValid
+	 * 0x04, VsafeSysClkReq1HPValid
+	 * 0x08, VanaSysClkReq1HPValid
+	 * 0x10, VpllSysClkReq1HPValid
+	 * 0x20, Vaux1SysClkReq1HPValid
+	 * 0x40, Vaux2SysClkReq1HPValid
+	 * 0x80, Vaux3SysClkReq1HPValid
+	 */
+	REG_INIT(AB8505_REGUSYSCLKREQ1HPVALID1,	0x03, 0x07, 0xff),
+	/*
+	 * 0x01, VsmpsCSysClkReq1HPValid
+	 * 0x02, VarmSysClkReq1HPValid
+	 * 0x04, VbbSysClkReq1HPValid
+	 * 0x08, VsmpsMSysClkReq1HPValid
+	 */
+	REG_INIT(AB8505_REGUSYSCLKREQ1HPVALID2,	0x03, 0x08, 0x0f),
+	/*
+	 * 0x01, VsmpsAHwHPReq1Valid
+	 * 0x02, VsmpsBHwHPReq1Valid
+	 * 0x04, VsafeHwHPReq1Valid
+	 * 0x08, VanaHwHPReq1Valid
+	 * 0x10, VpllHwHPReq1Valid
+	 * 0x20, Vaux1HwHPReq1Valid
+	 * 0x40, Vaux2HwHPReq1Valid
+	 * 0x80, Vaux3HwHPReq1Valid
+	 */
+	REG_INIT(AB8505_REGUHWHPREQ1VALID1,	0x03, 0x09, 0xff),
+	/*
+	 * 0x08, VsmpsMHwHPReq1Valid
+	 */
+	REG_INIT(AB8505_REGUHWHPREQ1VALID2,	0x03, 0x0a, 0x08),
+	/*
+	 * 0x01, VsmpsAHwHPReq2Valid
+	 * 0x02, VsmpsBHwHPReq2Valid
+	 * 0x04, VsafeHwHPReq2Valid
+	 * 0x08, VanaHwHPReq2Valid
+	 * 0x10, VpllHwHPReq2Valid
+	 * 0x20, Vaux1HwHPReq2Valid
+	 * 0x40, Vaux2HwHPReq2Valid
+	 * 0x80, Vaux3HwHPReq2Valid
+	 */
+	REG_INIT(AB8505_REGUHWHPREQ2VALID1,	0x03, 0x0b, 0xff),
+	/*
+	 * 0x08, VsmpsMHwHPReq2Valid
+	 */
+	REG_INIT(AB8505_REGUHWHPREQ2VALID2,	0x03, 0x0c, 0x08),
+	/*
+	 * 0x01, VsmpsCSwHPReqValid
+	 * 0x02, VarmSwHPReqValid
+	 * 0x04, VsmpsASwHPReqValid
+	 * 0x08, VsmpsBSwHPReqValid
+	 * 0x10, VsafeSwHPReqValid
+	 * 0x20, VanaSwHPReqValid
+	 * 0x40, VpllSwHPReqValid
+	 * 0x80, Vaux1SwHPReqValid
+	 */
+	REG_INIT(AB8505_REGUSWHPREQVALID1,	0x03, 0x0d, 0xff),
+	/*
+	 * 0x01, Vaux2SwHPReqValid
+	 * 0x02, Vaux3SwHPReqValid
+	 * 0x20, VsmpsMSwHPReqValid
+	 */
+	REG_INIT(AB8505_REGUSWHPREQVALID2,	0x03, 0x0e, 0x23),
+	/*
+	 * 0x02, SysClkReq2Valid1
+	 * 0x04, SysClkReq3Valid1
+	 * 0x08, SysClkReq4Valid1
+	 */
+	REG_INIT(AB8505_REGUSYSCLKREQVALID1,	0x03, 0x0f, 0x0e),
+	/*
+	 * 0x02, SysClkReq2Valid2
+	 * 0x04, SysClkReq3Valid2
+	 * 0x08, SysClkReq4Valid2
+	 */
+	REG_INIT(AB8505_REGUSYSCLKREQVALID2,	0x03, 0x10, 0x0e),
+	/*
+	 * 0x01, Vaux4SwHPReqValid
+	 * 0x02, Vaux4HwHPReq2Valid
+	 * 0x04, Vaux4HwHPReq1Valid
+	 * 0x08, Vaux4SysClkReq1HPValid
+	 */
+	REG_INIT(AB8505_REGUVAUX4REQVALID,	0x03, 0x11, 0x0f),
+	/*
+	 * 0x02, VadcEna
+	 * 0x04, VintCore12Ena
+	 * 0x38, VintCore12Sel
+	 * 0x40, VintCore12LP
+	 * 0x80, VadcLP
+	 */
+	REG_INIT(AB8505_REGUMISC1,		0x03, 0x80, 0xfe),
+	/*
+	 * 0x02, VaudioEna
+	 * 0x04, VdmicEna
+	 * 0x08, Vamic1Ena
+	 * 0x10, Vamic2Ena
+	 */
+	REG_INIT(AB8505_VAUDIOSUPPLY,		0x03, 0x83, 0x1e),
+	/*
+	 * 0x01, Vamic1_dzout
+	 * 0x02, Vamic2_dzout
+	 */
+	REG_INIT(AB8505_REGUCTRL1VAMIC,		0x03, 0x84, 0x03),
+	/*
+	 * 0x03, VsmpsARegu
+	 * 0x0c, VsmpsASelCtrl
+	 * 0x10, VsmpsAAutoMode
+	 * 0x20, VsmpsAPWMMode
+	 */
+	REG_INIT(AB8505_VSMPSAREGU,		0x04, 0x03, 0x3f),
+	/*
+	 * 0x03, VsmpsBRegu
+	 * 0x0c, VsmpsBSelCtrl
+	 * 0x10, VsmpsBAutoMode
+	 * 0x20, VsmpsBPWMMode
+	 */
+	REG_INIT(AB8505_VSMPSBREGU,		0x04, 0x04, 0x3f),
+	/*
+	 * 0x03, VsafeRegu
+	 * 0x0c, VsafeSelCtrl
+	 * 0x10, VsafeAutoMode
+	 * 0x20, VsafePWMMode
+	 */
+	REG_INIT(AB8505_VSAFEREGU,		0x04, 0x05, 0x3f),
+	/*
+	 * 0x03, VpllRegu (NOTE! PRCMU register bits)
+	 * 0x0c, VanaRegu
+	 */
+	REG_INIT(AB8505_VPLLVANAREGU,		0x04, 0x06, 0x0f),
+	/*
+	 * 0x03, VextSupply1Regu
+	 * 0x0c, VextSupply2Regu
+	 * 0x30, VextSupply3Regu
+	 * 0x40, ExtSupply2Bypass
+	 * 0x80, ExtSupply3Bypass
+	 */
+	REG_INIT(AB8505_EXTSUPPLYREGU,		0x04, 0x08, 0xff),
+	/*
+	 * 0x03, Vaux1Regu
+	 * 0x0c, Vaux2Regu
+	 */
+	REG_INIT(AB8505_VAUX12REGU,		0x04, 0x09, 0x0f),
+	/*
+	 * 0x0f, Vaux3Regu
+	 */
+	REG_INIT(AB8505_VRF1VAUX3REGU,		0x04, 0x0a, 0x0f),
+	/*
+	 * 0x3f, VsmpsASel1
+	 */
+	REG_INIT(AB8505_VSMPSASEL1,		0x04, 0x13, 0x3f),
+	/*
+	 * 0x3f, VsmpsASel2
+	 */
+	REG_INIT(AB8505_VSMPSASEL2,		0x04, 0x14, 0x3f),
+	/*
+	 * 0x3f, VsmpsASel3
+	 */
+	REG_INIT(AB8505_VSMPSASEL3,		0x04, 0x15, 0x3f),
+	/*
+	 * 0x3f, VsmpsBSel1
+	 */
+	REG_INIT(AB8505_VSMPSBSEL1,		0x04, 0x17, 0x3f),
+	/*
+	 * 0x3f, VsmpsBSel2
+	 */
+	REG_INIT(AB8505_VSMPSBSEL2,		0x04, 0x18, 0x3f),
+	/*
+	 * 0x3f, VsmpsBSel3
+	 */
+	REG_INIT(AB8505_VSMPSBSEL3,		0x04, 0x19, 0x3f),
+	/*
+	 * 0x7f, VsafeSel1
+	 */
+	REG_INIT(AB8505_VSAFESEL1,		0x04, 0x1b, 0x7f),
+	/*
+	 * 0x3f, VsafeSel2
+	 */
+	REG_INIT(AB8505_VSAFESEL2,		0x04, 0x1c, 0x7f),
+	/*
+	 * 0x3f, VsafeSel3
+	 */
+	REG_INIT(AB8505_VSAFESEL3,		0x04, 0x1d, 0x7f),
+	/*
+	 * 0x0f, Vaux1Sel
+	 */
+	REG_INIT(AB8505_VAUX1SEL,		0x04, 0x1f, 0x0f),
+	/*
+	 * 0x0f, Vaux2Sel
+	 */
+	REG_INIT(AB8505_VAUX2SEL,		0x04, 0x20, 0x0f),
+	/*
+	 * 0x07, Vaux3Sel
+	 * 0x30, VRF1Sel
+	 */
+	REG_INIT(AB8505_VRF1VAUX3SEL,		0x04, 0x21, 0x37),
+	/*
+	 * 0x03, Vaux4RequestCtrl
+	 */
+	REG_INIT(AB8505_VAUX4REQCTRL,		0x04, 0x2d, 0x03),
+	/*
+	 * 0x03, Vaux4Regu
+	 */
+	REG_INIT(AB8505_VAUX4REGU,		0x04, 0x2e, 0x03),
+	/*
+	 * 0x0f, Vaux4Sel
+	 */
+	REG_INIT(AB8505_VAUX4SEL,		0x04, 0x2f, 0x0f),
+	/*
+	 * 0x04, Vaux1Disch
+	 * 0x08, Vaux2Disch
+	 * 0x10, Vaux3Disch
+	 * 0x20, Vintcore12Disch
+	 * 0x40, VTVoutDisch
+	 * 0x80, VaudioDisch
+	 */
+	REG_INIT(AB8505_REGUCTRLDISCH,		0x04, 0x43, 0xfc),
+	/*
+	 * 0x02, VanaDisch
+	 * 0x04, VdmicPullDownEna
+	 * 0x10, VdmicDisch
+	 */
+	REG_INIT(AB8505_REGUCTRLDISCH2,		0x04, 0x44, 0x16),
+	/*
+	 * 0x01, Vaux4Disch
+	 */
+	REG_INIT(AB8505_REGUCTRLDISCH3,		0x04, 0x48, 0x01),
+	/*
+	 * 0x07, Vaux5Sel
+	 * 0x08, Vaux5LP
+	 * 0x10, Vaux5Ena
+	 * 0x20, Vaux5Disch
+	 * 0x40, Vaux5DisSfst
+	 * 0x80, Vaux5DisPulld
+	 */
+	REG_INIT(AB8505_CTRLVAUX5,		0x01, 0x55, 0xff),
+	/*
+	 * 0x07, Vaux6Sel
+	 * 0x08, Vaux6LP
+	 * 0x10, Vaux6Ena
+	 * 0x80, Vaux6DisPulld
+	 */
+	REG_INIT(AB8505_CTRLVAUX6,		0x01, 0x56, 0x9f),
+};
+
+/* AB9540 register init */
+static struct ab8500_reg_init ab9540_reg_init[] = {
+	/*
+	 * 0x03, VarmRequestCtrl
+	 * 0x0c, VapeRequestCtrl
+	 * 0x30, Vsmps1RequestCtrl
+	 * 0xc0, Vsmps2RequestCtrl
+	 */
+	REG_INIT(AB9540_REGUREQUESTCTRL1,	0x03, 0x03, 0xff),
+	/*
+	 * 0x03, Vsmps3RequestCtrl
+	 * 0x0c, VpllRequestCtrl
+	 * 0x30, VanaRequestCtrl
+	 * 0xc0, VextSupply1RequestCtrl
+	 */
+	REG_INIT(AB9540_REGUREQUESTCTRL2,	0x03, 0x04, 0xff),
+	/*
+	 * 0x03, VextSupply2RequestCtrl
+	 * 0x0c, VextSupply3RequestCtrl
+	 * 0x30, Vaux1RequestCtrl
+	 * 0xc0, Vaux2RequestCtrl
+	 */
+	REG_INIT(AB9540_REGUREQUESTCTRL3,	0x03, 0x05, 0xff),
+	/*
+	 * 0x03, Vaux3RequestCtrl
+	 * 0x04, SwHPReq
+	 */
+	REG_INIT(AB9540_REGUREQUESTCTRL4,	0x03, 0x06, 0x07),
+	/*
+	 * 0x01, Vsmps1SysClkReq1HPValid
+	 * 0x02, Vsmps2SysClkReq1HPValid
+	 * 0x04, Vsmps3SysClkReq1HPValid
+	 * 0x08, VanaSysClkReq1HPValid
+	 * 0x10, VpllSysClkReq1HPValid
+	 * 0x20, Vaux1SysClkReq1HPValid
+	 * 0x40, Vaux2SysClkReq1HPValid
+	 * 0x80, Vaux3SysClkReq1HPValid
+	 */
+	REG_INIT(AB9540_REGUSYSCLKREQ1HPVALID1,	0x03, 0x07, 0xff),
+	/*
+	 * 0x01, VapeSysClkReq1HPValid
+	 * 0x02, VarmSysClkReq1HPValid
+	 * 0x04, VbbSysClkReq1HPValid
+	 * 0x08, VmodSysClkReq1HPValid
+	 * 0x10, VextSupply1SysClkReq1HPValid
+	 * 0x20, VextSupply2SysClkReq1HPValid
+	 * 0x40, VextSupply3SysClkReq1HPValid
+	 */
+	REG_INIT(AB9540_REGUSYSCLKREQ1HPVALID2,	0x03, 0x08, 0x7f),
+	/*
+	 * 0x01, Vsmps1HwHPReq1Valid
+	 * 0x02, Vsmps2HwHPReq1Valid
+	 * 0x04, Vsmps3HwHPReq1Valid
+	 * 0x08, VanaHwHPReq1Valid
+	 * 0x10, VpllHwHPReq1Valid
+	 * 0x20, Vaux1HwHPReq1Valid
+	 * 0x40, Vaux2HwHPReq1Valid
+	 * 0x80, Vaux3HwHPReq1Valid
+	 */
+	REG_INIT(AB9540_REGUHWHPREQ1VALID1,	0x03, 0x09, 0xff),
+	/*
+	 * 0x01, VextSupply1HwHPReq1Valid
+	 * 0x02, VextSupply2HwHPReq1Valid
+	 * 0x04, VextSupply3HwHPReq1Valid
+	 * 0x08, VmodHwHPReq1Valid
+	 */
+	REG_INIT(AB9540_REGUHWHPREQ1VALID2,	0x03, 0x0a, 0x0f),
+	/*
+	 * 0x01, Vsmps1HwHPReq2Valid
+	 * 0x02, Vsmps2HwHPReq2Valid
+	 * 0x03, Vsmps3HwHPReq2Valid
+	 * 0x08, VanaHwHPReq2Valid
+	 * 0x10, VpllHwHPReq2Valid
+	 * 0x20, Vaux1HwHPReq2Valid
+	 * 0x40, Vaux2HwHPReq2Valid
+	 * 0x80, Vaux3HwHPReq2Valid
+	 */
+	REG_INIT(AB9540_REGUHWHPREQ2VALID1,	0x03, 0x0b, 0xff),
+	/*
+	 * 0x01, VextSupply1HwHPReq2Valid
+	 * 0x02, VextSupply2HwHPReq2Valid
+	 * 0x04, VextSupply3HwHPReq2Valid
+	 * 0x08, VmodHwHPReq2Valid
+	 */
+	REG_INIT(AB9540_REGUHWHPREQ2VALID2,	0x03, 0x0c, 0x0f),
+	/*
+	 * 0x01, VapeSwHPReqValid
+	 * 0x02, VarmSwHPReqValid
+	 * 0x04, Vsmps1SwHPReqValid
+	 * 0x08, Vsmps2SwHPReqValid
+	 * 0x10, Vsmps3SwHPReqValid
+	 * 0x20, VanaSwHPReqValid
+	 * 0x40, VpllSwHPReqValid
+	 * 0x80, Vaux1SwHPReqValid
+	 */
+	REG_INIT(AB9540_REGUSWHPREQVALID1,	0x03, 0x0d, 0xff),
+	/*
+	 * 0x01, Vaux2SwHPReqValid
+	 * 0x02, Vaux3SwHPReqValid
+	 * 0x04, VextSupply1SwHPReqValid
+	 * 0x08, VextSupply2SwHPReqValid
+	 * 0x10, VextSupply3SwHPReqValid
+	 * 0x20, VmodSwHPReqValid
+	 */
+	REG_INIT(AB9540_REGUSWHPREQVALID2,	0x03, 0x0e, 0x3f),
+	/*
+	 * 0x02, SysClkReq2Valid1
+	 * ...
+	 * 0x80, SysClkReq8Valid1
+	 */
+	REG_INIT(AB9540_REGUSYSCLKREQVALID1,	0x03, 0x0f, 0xfe),
+	/*
+	 * 0x02, SysClkReq2Valid2
+	 * ...
+	 * 0x80, SysClkReq8Valid2
+	 */
+	REG_INIT(AB9540_REGUSYSCLKREQVALID2,	0x03, 0x10, 0xfe),
+	/*
+	 * 0x01, Vaux4SwHPReqValid
+	 * 0x02, Vaux4HwHPReq2Valid
+	 * 0x04, Vaux4HwHPReq1Valid
+	 * 0x08, Vaux4SysClkReq1HPValid
+	 */
+	REG_INIT(AB9540_REGUVAUX4REQVALID,	0x03, 0x11, 0x0f),
+	/*
+	 * 0x02, VTVoutEna
+	 * 0x04, Vintcore12Ena
+	 * 0x38, Vintcore12Sel
+	 * 0x40, Vintcore12LP
+	 * 0x80, VTVoutLP
+	 */
+	REG_INIT(AB9540_REGUMISC1,		0x03, 0x80, 0xfe),
+	/*
+	 * 0x02, VaudioEna
+	 * 0x04, VdmicEna
+	 * 0x08, Vamic1Ena
+	 * 0x10, Vamic2Ena
+	 */
+	REG_INIT(AB9540_VAUDIOSUPPLY,		0x03, 0x83, 0x1e),
+	/*
+	 * 0x01, Vamic1_dzout
+	 * 0x02, Vamic2_dzout
+	 */
+	REG_INIT(AB9540_REGUCTRL1VAMIC,		0x03, 0x84, 0x03),
+	/*
+	 * 0x03, Vsmps1Regu
+	 * 0x0c, Vsmps1SelCtrl
+	 * 0x10, Vsmps1AutoMode
+	 * 0x20, Vsmps1PWMMode
+	 */
+	REG_INIT(AB9540_VSMPS1REGU,		0x04, 0x03, 0x3f),
+	/*
+	 * 0x03, Vsmps2Regu
+	 * 0x0c, Vsmps2SelCtrl
+	 * 0x10, Vsmps2AutoMode
+	 * 0x20, Vsmps2PWMMode
+	 */
+	REG_INIT(AB9540_VSMPS2REGU,		0x04, 0x04, 0x3f),
+	/*
+	 * 0x03, Vsmps3Regu
+	 * 0x0c, Vsmps3SelCtrl
+	 * NOTE! PRCMU register
+	 */
+	REG_INIT(AB9540_VSMPS3REGU,		0x04, 0x05, 0x0f),
+	/*
+	 * 0x03, VpllRegu
+	 * 0x0c, VanaRegu
+	 */
+	REG_INIT(AB9540_VPLLVANAREGU,		0x04, 0x06, 0x0f),
+	/*
+	 * 0x03, VextSupply1Regu
+	 * 0x0c, VextSupply2Regu
+	 * 0x30, VextSupply3Regu
+	 * 0x40, ExtSupply2Bypass
+	 * 0x80, ExtSupply3Bypass
+	 */
+	REG_INIT(AB9540_EXTSUPPLYREGU,		0x04, 0x08, 0xff),
+	/*
+	 * 0x03, Vaux1Regu
+	 * 0x0c, Vaux2Regu
+	 */
+	REG_INIT(AB9540_VAUX12REGU,		0x04, 0x09, 0x0f),
+	/*
+	 * 0x0c, Vrf1Regu
+	 * 0x03, Vaux3Regu
+	 */
+	REG_INIT(AB9540_VRF1VAUX3REGU,		0x04, 0x0a, 0x0f),
+	/*
+	 * 0x3f, Vsmps1Sel1
+	 */
+	REG_INIT(AB9540_VSMPS1SEL1,		0x04, 0x13, 0x3f),
+	/*
+	 * 0x3f, Vsmps1Sel2
+	 */
+	REG_INIT(AB9540_VSMPS1SEL2,		0x04, 0x14, 0x3f),
+	/*
+	 * 0x3f, Vsmps1Sel3
+	 */
+	REG_INIT(AB9540_VSMPS1SEL3,		0x04, 0x15, 0x3f),
+	/*
+	 * 0x3f, Vsmps2Sel1
+	 */
+	REG_INIT(AB9540_VSMPS2SEL1,		0x04, 0x17, 0x3f),
+	/*
+	 * 0x3f, Vsmps2Sel2
+	 */
+	REG_INIT(AB9540_VSMPS2SEL2,		0x04, 0x18, 0x3f),
+	/*
+	 * 0x3f, Vsmps2Sel3
+	 */
+	REG_INIT(AB9540_VSMPS2SEL3,		0x04, 0x19, 0x3f),
+	/*
+	 * 0x7f, Vsmps3Sel1
+	 * NOTE! PRCMU register
+	 */
+	REG_INIT(AB9540_VSMPS3SEL1,             0x04, 0x1b, 0x7f),
+	/*
+	 * 0x7f, Vsmps3Sel2
+	 * NOTE! PRCMU register
+	 */
+	REG_INIT(AB9540_VSMPS3SEL2,             0x04, 0x1c, 0x7f),
+	/*
+	 * 0x0f, Vaux1Sel
+	 */
+	REG_INIT(AB9540_VAUX1SEL,		0x04, 0x1f, 0x0f),
+	/*
+	 * 0x0f, Vaux2Sel
+	 */
+	REG_INIT(AB9540_VAUX2SEL,		0x04, 0x20, 0x0f),
+	/*
+	 * 0x07, Vaux3Sel
+	 * 0x30, Vrf1Sel
+	 */
+	REG_INIT(AB9540_VRF1VAUX3SEL,		0x04, 0x21, 0x37),
+	/*
+	 * 0x01, VextSupply12LP
+	 */
+	REG_INIT(AB9540_REGUCTRL2SPARE,		0x04, 0x22, 0x01),
+	/*
+	 * 0x03, Vaux4RequestCtrl
+	 */
+	REG_INIT(AB9540_VAUX4REQCTRL,		0x04, 0x2d, 0x03),
+	/*
+	 * 0x03, Vaux4Regu
+	 */
+	REG_INIT(AB9540_VAUX4REGU,		0x04, 0x2e, 0x03),
+	/*
+	 * 0x08, Vaux4Sel
+	 */
+	REG_INIT(AB9540_VAUX4SEL,		0x04, 0x2f, 0x0f),
+	/*
+	 * 0x01, VpllDisch
+	 * 0x02, Vrf1Disch
+	 * 0x04, Vaux1Disch
+	 * 0x08, Vaux2Disch
+	 * 0x10, Vaux3Disch
+	 * 0x20, Vintcore12Disch
+	 * 0x40, VTVoutDisch
+	 * 0x80, VaudioDisch
+	 */
+	REG_INIT(AB9540_REGUCTRLDISCH,		0x04, 0x43, 0xff),
+	/*
+	 * 0x01, VsimDisch
+	 * 0x02, VanaDisch
+	 * 0x04, VdmicPullDownEna
+	 * 0x08, VpllPullDownEna
+	 * 0x10, VdmicDisch
+	 */
+	REG_INIT(AB9540_REGUCTRLDISCH2,		0x04, 0x44, 0x1f),
+	/*
+	 * 0x01, Vaux4Disch
+	 */
+	REG_INIT(AB9540_REGUCTRLDISCH3,		0x04, 0x48, 0x01),
+};
+
+/* AB8540 register init */
+static struct ab8500_reg_init ab8540_reg_init[] = {
+	/*
+	 * 0x01, VSimSycClkReq1Valid
+	 * 0x02, VSimSycClkReq2Valid
+	 * 0x04, VSimSycClkReq3Valid
+	 * 0x08, VSimSycClkReq4Valid
+	 * 0x10, VSimSycClkReq5Valid
+	 * 0x20, VSimSycClkReq6Valid
+	 * 0x40, VSimSycClkReq7Valid
+	 * 0x80, VSimSycClkReq8Valid
+	 */
+	REG_INIT(AB8540_VSIMSYSCLKCTRL,		0x02, 0x33, 0xff),
+	/*
+	 * 0x03, VarmRequestCtrl
+	 * 0x0c, VapeRequestCtrl
+	 * 0x30, Vsmps1RequestCtrl
+	 * 0xc0, Vsmps2RequestCtrl
+	 */
+	REG_INIT(AB8540_REGUREQUESTCTRL1,	0x03, 0x03, 0xff),
+	/*
+	 * 0x03, Vsmps3RequestCtrl
+	 * 0x0c, VpllRequestCtrl
+	 * 0x30, VanaRequestCtrl
+	 * 0xc0, VextSupply1RequestCtrl
+	 */
+	REG_INIT(AB8540_REGUREQUESTCTRL2,	0x03, 0x04, 0xff),
+	/*
+	 * 0x03, VextSupply2RequestCtrl
+	 * 0x0c, VextSupply3RequestCtrl
+	 * 0x30, Vaux1RequestCtrl
+	 * 0xc0, Vaux2RequestCtrl
+	 */
+	REG_INIT(AB8540_REGUREQUESTCTRL3,	0x03, 0x05, 0xff),
+	/*
+	 * 0x03, Vaux3RequestCtrl
+	 * 0x04, SwHPReq
+	 */
+	REG_INIT(AB8540_REGUREQUESTCTRL4,	0x03, 0x06, 0x07),
+	/*
+	 * 0x01, Vsmps1SysClkReq1HPValid
+	 * 0x02, Vsmps2SysClkReq1HPValid
+	 * 0x04, Vsmps3SysClkReq1HPValid
+	 * 0x08, VanaSysClkReq1HPValid
+	 * 0x10, VpllSysClkReq1HPValid
+	 * 0x20, Vaux1SysClkReq1HPValid
+	 * 0x40, Vaux2SysClkReq1HPValid
+	 * 0x80, Vaux3SysClkReq1HPValid
+	 */
+	REG_INIT(AB8540_REGUSYSCLKREQ1HPVALID1,	0x03, 0x07, 0xff),
+	/*
+	 * 0x01, VapeSysClkReq1HPValid
+	 * 0x02, VarmSysClkReq1HPValid
+	 * 0x04, VbbSysClkReq1HPValid
+	 * 0x10, VextSupply1SysClkReq1HPValid
+	 * 0x20, VextSupply2SysClkReq1HPValid
+	 * 0x40, VextSupply3SysClkReq1HPValid
+	 */
+	REG_INIT(AB8540_REGUSYSCLKREQ1HPVALID2,	0x03, 0x08, 0x77),
+	/*
+	 * 0x01, Vsmps1HwHPReq1Valid
+	 * 0x02, Vsmps2HwHPReq1Valid
+	 * 0x04, Vsmps3HwHPReq1Valid
+	 * 0x08, VanaHwHPReq1Valid
+	 * 0x10, VpllHwHPReq1Valid
+	 * 0x20, Vaux1HwHPReq1Valid
+	 * 0x40, Vaux2HwHPReq1Valid
+	 * 0x80, Vaux3HwHPReq1Valid
+	 */
+	REG_INIT(AB8540_REGUHWHPREQ1VALID1,	0x03, 0x09, 0xff),
+	/*
+	 * 0x01, VextSupply1HwHPReq1Valid
+	 * 0x02, VextSupply2HwHPReq1Valid
+	 * 0x04, VextSupply3HwHPReq1Valid
+	 */
+	REG_INIT(AB8540_REGUHWHPREQ1VALID2,	0x03, 0x0a, 0x07),
+	/*
+	 * 0x01, Vsmps1HwHPReq2Valid
+	 * 0x02, Vsmps2HwHPReq2Valid
+	 * 0x03, Vsmps3HwHPReq2Valid
+	 * 0x08, VanaHwHPReq2Valid
+	 * 0x10, VpllHwHPReq2Valid
+	 * 0x20, Vaux1HwHPReq2Valid
+	 * 0x40, Vaux2HwHPReq2Valid
+	 * 0x80, Vaux3HwHPReq2Valid
+	 */
+	REG_INIT(AB8540_REGUHWHPREQ2VALID1,	0x03, 0x0b, 0xff),
+	/*
+	 * 0x01, VextSupply1HwHPReq2Valid
+	 * 0x02, VextSupply2HwHPReq2Valid
+	 * 0x04, VextSupply3HwHPReq2Valid
+	 */
+	REG_INIT(AB8540_REGUHWHPREQ2VALID2,	0x03, 0x0c, 0x07),
+	/*
+	 * 0x01, VapeSwHPReqValid
+	 * 0x02, VarmSwHPReqValid
+	 * 0x04, Vsmps1SwHPReqValid
+	 * 0x08, Vsmps2SwHPReqValid
+	 * 0x10, Vsmps3SwHPReqValid
+	 * 0x20, VanaSwHPReqValid
+	 * 0x40, VpllSwHPReqValid
+	 * 0x80, Vaux1SwHPReqValid
+	 */
+	REG_INIT(AB8540_REGUSWHPREQVALID1,	0x03, 0x0d, 0xff),
+	/*
+	 * 0x01, Vaux2SwHPReqValid
+	 * 0x02, Vaux3SwHPReqValid
+	 * 0x04, VextSupply1SwHPReqValid
+	 * 0x08, VextSupply2SwHPReqValid
+	 * 0x10, VextSupply3SwHPReqValid
+	 */
+	REG_INIT(AB8540_REGUSWHPREQVALID2,	0x03, 0x0e, 0x1f),
+	/*
+	 * 0x02, SysClkReq2Valid1
+	 * ...
+	 * 0x80, SysClkReq8Valid1
+	 */
+	REG_INIT(AB8540_REGUSYSCLKREQVALID1,	0x03, 0x0f, 0xff),
+	/*
+	 * 0x02, SysClkReq2Valid2
+	 * ...
+	 * 0x80, SysClkReq8Valid2
+	 */
+	REG_INIT(AB8540_REGUSYSCLKREQVALID2,	0x03, 0x10, 0xff),
+	/*
+	 * 0x01, Vaux4SwHPReqValid
+	 * 0x02, Vaux4HwHPReq2Valid
+	 * 0x04, Vaux4HwHPReq1Valid
+	 * 0x08, Vaux4SysClkReq1HPValid
+	 */
+	REG_INIT(AB8540_REGUVAUX4REQVALID,	0x03, 0x11, 0x0f),
+	/*
+	 * 0x01, Vaux5SwHPReqValid
+	 * 0x02, Vaux5HwHPReq2Valid
+	 * 0x04, Vaux5HwHPReq1Valid
+	 * 0x08, Vaux5SysClkReq1HPValid
+	 */
+	REG_INIT(AB8540_REGUVAUX5REQVALID,	0x03, 0x12, 0x0f),
+	/*
+	 * 0x01, Vaux6SwHPReqValid
+	 * 0x02, Vaux6HwHPReq2Valid
+	 * 0x04, Vaux6HwHPReq1Valid
+	 * 0x08, Vaux6SysClkReq1HPValid
+	 */
+	REG_INIT(AB8540_REGUVAUX6REQVALID,	0x03, 0x13, 0x0f),
+	/*
+	 * 0x01, VclkbSwHPReqValid
+	 * 0x02, VclkbHwHPReq2Valid
+	 * 0x04, VclkbHwHPReq1Valid
+	 * 0x08, VclkbSysClkReq1HPValid
+	 */
+	REG_INIT(AB8540_REGUVCLKBREQVALID,	0x03, 0x14, 0x0f),
+	/*
+	 * 0x01, Vrf1SwHPReqValid
+	 * 0x02, Vrf1HwHPReq2Valid
+	 * 0x04, Vrf1HwHPReq1Valid
+	 * 0x08, Vrf1SysClkReq1HPValid
+	 */
+	REG_INIT(AB8540_REGUVRF1REQVALID,	0x03, 0x15, 0x0f),
+	/*
+	 * 0x02, VTVoutEna
+	 * 0x04, Vintcore12Ena
+	 * 0x38, Vintcore12Sel
+	 * 0x40, Vintcore12LP
+	 * 0x80, VTVoutLP
+	 */
+	REG_INIT(AB8540_REGUMISC1,		0x03, 0x80, 0xfe),
+	/*
+	 * 0x02, VaudioEna
+	 * 0x04, VdmicEna
+	 * 0x08, Vamic1Ena
+	 * 0x10, Vamic2Ena
+	 * 0x20, Vamic12LP
+	 * 0xC0, VdmicSel
+	 */
+	REG_INIT(AB8540_VAUDIOSUPPLY,		0x03, 0x83, 0xfe),
+	/*
+	 * 0x01, Vamic1_dzout
+	 * 0x02, Vamic2_dzout
+	 */
+	REG_INIT(AB8540_REGUCTRL1VAMIC,		0x03, 0x84, 0x03),
+	/*
+	 * 0x07, VHSICSel
+	 * 0x08, VHSICOffState
+	 * 0x10, VHSIEna
+	 * 0x20, VHSICLP
+	 */
+	REG_INIT(AB8540_VHSIC,			0x03, 0x87, 0x3f),
+	/*
+	 * 0x07, VSDIOSel
+	 * 0x08, VSDIOOffState
+	 * 0x10, VSDIOEna
+	 * 0x20, VSDIOLP
+	 */
+	REG_INIT(AB8540_VSDIO,			0x03, 0x88, 0x3f),
+	/*
+	 * 0x03, Vsmps1Regu
+	 * 0x0c, Vsmps1SelCtrl
+	 * 0x10, Vsmps1AutoMode
+	 * 0x20, Vsmps1PWMMode
+	 */
+	REG_INIT(AB8540_VSMPS1REGU,		0x04, 0x03, 0x3f),
+	/*
+	 * 0x03, Vsmps2Regu
+	 * 0x0c, Vsmps2SelCtrl
+	 * 0x10, Vsmps2AutoMode
+	 * 0x20, Vsmps2PWMMode
+	 */
+	REG_INIT(AB8540_VSMPS2REGU,		0x04, 0x04, 0x3f),
+	/*
+	 * 0x03, Vsmps3Regu
+	 * 0x0c, Vsmps3SelCtrl
+	 * 0x10, Vsmps3AutoMode
+	 * 0x20, Vsmps3PWMMode
+	 * NOTE! PRCMU register
+	 */
+	REG_INIT(AB8540_VSMPS3REGU,		0x04, 0x05, 0x0f),
+	/*
+	 * 0x03, VpllRegu
+	 * 0x0c, VanaRegu
+	 */
+	REG_INIT(AB8540_VPLLVANAREGU,		0x04, 0x06, 0x0f),
+	/*
+	 * 0x03, VextSupply1Regu
+	 * 0x0c, VextSupply2Regu
+	 * 0x30, VextSupply3Regu
+	 * 0x40, ExtSupply2Bypass
+	 * 0x80, ExtSupply3Bypass
+	 */
+	REG_INIT(AB8540_EXTSUPPLYREGU,		0x04, 0x08, 0xff),
+	/*
+	 * 0x03, Vaux1Regu
+	 * 0x0c, Vaux2Regu
+	 */
+	REG_INIT(AB8540_VAUX12REGU,		0x04, 0x09, 0x0f),
+	/*
+	 * 0x0c, VRF1Regu
+	 * 0x03, Vaux3Regu
+	 */
+	REG_INIT(AB8540_VRF1VAUX3REGU,		0x04, 0x0a, 0x0f),
+	/*
+	 * 0x3f, Vsmps1Sel1
+	 */
+	REG_INIT(AB8540_VSMPS1SEL1,		0x04, 0x13, 0x3f),
+	/*
+	 * 0x3f, Vsmps1Sel2
+	 */
+	REG_INIT(AB8540_VSMPS1SEL2,		0x04, 0x14, 0x3f),
+	/*
+	 * 0x3f, Vsmps1Sel3
+	 */
+	REG_INIT(AB8540_VSMPS1SEL3,		0x04, 0x15, 0x3f),
+	/*
+	 * 0x3f, Vsmps2Sel1
+	 */
+	REG_INIT(AB8540_VSMPS2SEL1,		0x04, 0x17, 0x3f),
+	/*
+	 * 0x3f, Vsmps2Sel2
+	 */
+	REG_INIT(AB8540_VSMPS2SEL2,		0x04, 0x18, 0x3f),
+	/*
+	 * 0x3f, Vsmps2Sel3
+	 */
+	REG_INIT(AB8540_VSMPS2SEL3,		0x04, 0x19, 0x3f),
+	/*
+	 * 0x7f, Vsmps3Sel1
+	 * NOTE! PRCMU register
+	 */
+	REG_INIT(AB8540_VSMPS3SEL1,             0x04, 0x1b, 0x7f),
+	/*
+	 * 0x7f, Vsmps3Sel2
+	 * NOTE! PRCMU register
+	 */
+	REG_INIT(AB8540_VSMPS3SEL2,             0x04, 0x1c, 0x7f),
+	/*
+	 * 0x0f, Vaux1Sel
+	 */
+	REG_INIT(AB8540_VAUX1SEL,		0x04, 0x1f, 0x0f),
+	/*
+	 * 0x0f, Vaux2Sel
+	 */
+	REG_INIT(AB8540_VAUX2SEL,		0x04, 0x20, 0x0f),
+	/*
+	 * 0x07, Vaux3Sel
+	 * 0x70, Vrf1Sel
+	 */
+	REG_INIT(AB8540_VRF1VAUX3SEL,		0x04, 0x21, 0x77),
+	/*
+	 * 0x01, VextSupply12LP
+	 */
+	REG_INIT(AB8540_REGUCTRL2SPARE,		0x04, 0x22, 0x01),
+	/*
+	 * 0x07, Vanasel
+	 * 0x30, Vpllsel
+	 */
+	REG_INIT(AB8540_VANAVPLLSEL,		0x04, 0x29, 0x37),
+	/*
+	 * 0x03, Vaux4RequestCtrl
+	 */
+	REG_INIT(AB8540_VAUX4REQCTRL,		0x04, 0x2d, 0x03),
+	/*
+	 * 0x03, Vaux4Regu
+	 */
+	REG_INIT(AB8540_VAUX4REGU,		0x04, 0x2e, 0x03),
+	/*
+	 * 0x0f, Vaux4Sel
+	 */
+	REG_INIT(AB8540_VAUX4SEL,		0x04, 0x2f, 0x0f),
+	/*
+	 * 0x03, Vaux5RequestCtrl
+	 */
+	REG_INIT(AB8540_VAUX5REQCTRL,		0x04, 0x31, 0x03),
+	/*
+	 * 0x03, Vaux5Regu
+	 */
+	REG_INIT(AB8540_VAUX5REGU,		0x04, 0x32, 0x03),
+	/*
+	 * 0x3f, Vaux5Sel
+	 */
+	REG_INIT(AB8540_VAUX5SEL,		0x04, 0x33, 0x3f),
+	/*
+	 * 0x03, Vaux6RequestCtrl
+	 */
+	REG_INIT(AB8540_VAUX6REQCTRL,		0x04, 0x34, 0x03),
+	/*
+	 * 0x03, Vaux6Regu
+	 */
+	REG_INIT(AB8540_VAUX6REGU,		0x04, 0x35, 0x03),
+	/*
+	 * 0x3f, Vaux6Sel
+	 */
+	REG_INIT(AB8540_VAUX6SEL,		0x04, 0x36, 0x3f),
+	/*
+	 * 0x03, VCLKBRequestCtrl
+	 */
+	REG_INIT(AB8540_VCLKBREQCTRL,		0x04, 0x37, 0x03),
+	/*
+	 * 0x03, VCLKBRegu
+	 */
+	REG_INIT(AB8540_VCLKBREGU,		0x04, 0x38, 0x03),
+	/*
+	 * 0x07, VCLKBSel
+	 */
+	REG_INIT(AB8540_VCLKBSEL,		0x04, 0x39, 0x07),
+	/*
+	 * 0x03, Vrf1RequestCtrl
+	 */
+	REG_INIT(AB8540_VRF1REQCTRL,		0x04, 0x3a, 0x03),
+	/*
+	 * 0x01, VpllDisch
+	 * 0x02, Vrf1Disch
+	 * 0x04, Vaux1Disch
+	 * 0x08, Vaux2Disch
+	 * 0x10, Vaux3Disch
+	 * 0x20, Vintcore12Disch
+	 * 0x40, VTVoutDisch
+	 * 0x80, VaudioDisch
+	 */
+	REG_INIT(AB8540_REGUCTRLDISCH,		0x04, 0x43, 0xff),
+	/*
+	 * 0x02, VanaDisch
+	 * 0x04, VdmicPullDownEna
+	 * 0x08, VpllPullDownEna
+	 * 0x10, VdmicDisch
+	 */
+	REG_INIT(AB8540_REGUCTRLDISCH2,		0x04, 0x44, 0x1e),
+	/*
+	 * 0x01, Vaux4Disch
+	 */
+	REG_INIT(AB8540_REGUCTRLDISCH3,		0x04, 0x48, 0x01),
+	/*
+	 * 0x01, Vaux5Disch
+	 * 0x02, Vaux6Disch
+	 * 0x04, VCLKBDisch
+	 */
+	REG_INIT(AB8540_REGUCTRLDISCH4,		0x04, 0x49, 0x07),
+};
+
+static struct of_regulator_match ab8500_regulator_match[] = {
+	{ .name	= "ab8500_ldo_aux1",    .driver_data = (void *) AB8500_LDO_AUX1, },
+	{ .name	= "ab8500_ldo_aux2",    .driver_data = (void *) AB8500_LDO_AUX2, },
+	{ .name	= "ab8500_ldo_aux3",    .driver_data = (void *) AB8500_LDO_AUX3, },
+	{ .name	= "ab8500_ldo_intcore", .driver_data = (void *) AB8500_LDO_INTCORE, },
+	{ .name	= "ab8500_ldo_tvout",   .driver_data = (void *) AB8500_LDO_TVOUT, },
+	{ .name = "ab8500_ldo_audio",   .driver_data = (void *) AB8500_LDO_AUDIO, },
+	{ .name	= "ab8500_ldo_anamic1", .driver_data = (void *) AB8500_LDO_ANAMIC1, },
+	{ .name	= "ab8500_ldo_amamic2", .driver_data = (void *) AB8500_LDO_ANAMIC2, },
+	{ .name	= "ab8500_ldo_dmic",    .driver_data = (void *) AB8500_LDO_DMIC, },
+	{ .name	= "ab8500_ldo_ana",     .driver_data = (void *) AB8500_LDO_ANA, },
+};
+
+static struct of_regulator_match ab8505_regulator_match[] = {
+	{ .name	= "ab8500_ldo_aux1",    .driver_data = (void *) AB8505_LDO_AUX1, },
+	{ .name	= "ab8500_ldo_aux2",    .driver_data = (void *) AB8505_LDO_AUX2, },
+	{ .name	= "ab8500_ldo_aux3",    .driver_data = (void *) AB8505_LDO_AUX3, },
+	{ .name	= "ab8500_ldo_aux4",    .driver_data = (void *) AB8505_LDO_AUX4, },
+	{ .name	= "ab8500_ldo_aux5",    .driver_data = (void *) AB8505_LDO_AUX5, },
+	{ .name	= "ab8500_ldo_aux6",    .driver_data = (void *) AB8505_LDO_AUX6, },
+	{ .name	= "ab8500_ldo_intcore", .driver_data = (void *) AB8505_LDO_INTCORE, },
+	{ .name	= "ab8500_ldo_adc",	.driver_data = (void *) AB8505_LDO_ADC, },
+	{ .name = "ab8500_ldo_audio",   .driver_data = (void *) AB8505_LDO_AUDIO, },
+	{ .name	= "ab8500_ldo_anamic1", .driver_data = (void *) AB8505_LDO_ANAMIC1, },
+	{ .name	= "ab8500_ldo_amamic2", .driver_data = (void *) AB8505_LDO_ANAMIC2, },
+	{ .name	= "ab8500_ldo_aux8",    .driver_data = (void *) AB8505_LDO_AUX8, },
+	{ .name	= "ab8500_ldo_ana",     .driver_data = (void *) AB8505_LDO_ANA, },
+};
+
+static struct of_regulator_match ab8540_regulator_match[] = {
+	{ .name	= "ab8500_ldo_aux1",    .driver_data = (void *) AB8540_LDO_AUX1, },
+	{ .name	= "ab8500_ldo_aux2",    .driver_data = (void *) AB8540_LDO_AUX2, },
+	{ .name	= "ab8500_ldo_aux3",    .driver_data = (void *) AB8540_LDO_AUX3, },
+	{ .name	= "ab8500_ldo_aux4",    .driver_data = (void *) AB8540_LDO_AUX4, },
+	{ .name	= "ab8500_ldo_aux5",    .driver_data = (void *) AB8540_LDO_AUX5, },
+	{ .name	= "ab8500_ldo_aux6",    .driver_data = (void *) AB8540_LDO_AUX6, },
+	{ .name	= "ab8500_ldo_intcore", .driver_data = (void *) AB8540_LDO_INTCORE, },
+	{ .name	= "ab8500_ldo_tvout",   .driver_data = (void *) AB8540_LDO_TVOUT, },
+	{ .name = "ab8500_ldo_audio",   .driver_data = (void *) AB8540_LDO_AUDIO, },
+	{ .name	= "ab8500_ldo_anamic1", .driver_data = (void *) AB8540_LDO_ANAMIC1, },
+	{ .name	= "ab8500_ldo_amamic2", .driver_data = (void *) AB8540_LDO_ANAMIC2, },
+	{ .name	= "ab8500_ldo_dmic",    .driver_data = (void *) AB8540_LDO_DMIC, },
+	{ .name	= "ab8500_ldo_ana",     .driver_data = (void *) AB8540_LDO_ANA, },
+	{ .name = "ab8500_ldo_sdio",    .driver_data = (void *) AB8540_LDO_SDIO, },
+};
+
+static struct of_regulator_match ab9540_regulator_match[] = {
+	{ .name	= "ab8500_ldo_aux1",    .driver_data = (void *) AB9540_LDO_AUX1, },
+	{ .name	= "ab8500_ldo_aux2",    .driver_data = (void *) AB9540_LDO_AUX2, },
+	{ .name	= "ab8500_ldo_aux3",    .driver_data = (void *) AB9540_LDO_AUX3, },
+	{ .name	= "ab8500_ldo_aux4",    .driver_data = (void *) AB9540_LDO_AUX4, },
+	{ .name	= "ab8500_ldo_intcore", .driver_data = (void *) AB9540_LDO_INTCORE, },
+	{ .name	= "ab8500_ldo_tvout",   .driver_data = (void *) AB9540_LDO_TVOUT, },
+	{ .name = "ab8500_ldo_audio",   .driver_data = (void *) AB9540_LDO_AUDIO, },
+	{ .name	= "ab8500_ldo_anamic1", .driver_data = (void *) AB9540_LDO_ANAMIC1, },
+	{ .name	= "ab8500_ldo_amamic2", .driver_data = (void *) AB9540_LDO_ANAMIC2, },
+	{ .name	= "ab8500_ldo_dmic",    .driver_data = (void *) AB9540_LDO_DMIC, },
+	{ .name	= "ab8500_ldo_ana",     .driver_data = (void *) AB9540_LDO_ANA, },
+};
+
+static struct {
+	struct ab8500_regulator_info *info;
+	int info_size;
+	struct ab8500_reg_init *init;
+	int init_size;
+	struct of_regulator_match *match;
+	int match_size;
+} abx500_regulator;
+
+static void abx500_get_regulator_info(struct ab8500 *ab8500)
+{
+	if (is_ab9540(ab8500)) {
+		abx500_regulator.info = ab9540_regulator_info;
+		abx500_regulator.info_size = ARRAY_SIZE(ab9540_regulator_info);
+		abx500_regulator.init = ab9540_reg_init;
+		abx500_regulator.init_size = AB9540_NUM_REGULATOR_REGISTERS;
+		abx500_regulator.match = ab9540_regulator_match;
+		abx500_regulator.match_size = ARRAY_SIZE(ab9540_regulator_match);
+	} else if (is_ab8505(ab8500)) {
+		abx500_regulator.info = ab8505_regulator_info;
+		abx500_regulator.info_size = ARRAY_SIZE(ab8505_regulator_info);
+		abx500_regulator.init = ab8505_reg_init;
+		abx500_regulator.init_size = AB8505_NUM_REGULATOR_REGISTERS;
+		abx500_regulator.match = ab8505_regulator_match;
+		abx500_regulator.match_size = ARRAY_SIZE(ab8505_regulator_match);
+	} else if (is_ab8540(ab8500)) {
+		abx500_regulator.info = ab8540_regulator_info;
+		abx500_regulator.info_size = ARRAY_SIZE(ab8540_regulator_info);
+		abx500_regulator.init = ab8540_reg_init;
+		abx500_regulator.init_size = AB8540_NUM_REGULATOR_REGISTERS;
+		abx500_regulator.match = ab8540_regulator_match;
+		abx500_regulator.match_size = ARRAY_SIZE(ab8540_regulator_match);
+	} else {
+		abx500_regulator.info = ab8500_regulator_info;
+		abx500_regulator.info_size = ARRAY_SIZE(ab8500_regulator_info);
+		abx500_regulator.init = ab8500_reg_init;
+		abx500_regulator.init_size = AB8500_NUM_REGULATOR_REGISTERS;
+		abx500_regulator.match = ab8500_regulator_match;
+		abx500_regulator.match_size = ARRAY_SIZE(ab8500_regulator_match);
+	}
+}
+
+static int ab8500_regulator_init_registers(struct platform_device *pdev,
+					   int id, int mask, int value)
 {
+	struct ab8500_reg_init *reg_init = abx500_regulator.init;
 	int err;
 
-	if (value & ~ab8500_reg_init[id].mask) {
-		dev_err(&pdev->dev,
-			"Configuration error: value outside mask.\n");
-		return -EINVAL;
-	}
+	BUG_ON(value & ~mask);
+	BUG_ON(mask & ~reg_init[id].mask);
 
+	/* initialize register */
 	err = abx500_mask_and_set_register_interruptible(
 		&pdev->dev,
-		ab8500_reg_init[id].bank,
-		ab8500_reg_init[id].addr,
-		ab8500_reg_init[id].mask,
-		value);
+		reg_init[id].bank,
+		reg_init[id].addr,
+		mask, value);
 	if (err < 0) {
 		dev_err(&pdev->dev,
 			"Failed to initialize 0x%02x, 0x%02x.\n",
-			ab8500_reg_init[id].bank,
-			ab8500_reg_init[id].addr);
+			reg_init[id].bank,
+			reg_init[id].addr);
 		return err;
 	}
-
 	dev_vdbg(&pdev->dev,
-		"init: 0x%02x, 0x%02x, 0x%02x, 0x%02x\n",
-		ab8500_reg_init[id].bank,
-		ab8500_reg_init[id].addr,
-		ab8500_reg_init[id].mask,
-		value);
+		 "  init: 0x%02x, 0x%02x, 0x%02x, 0x%02x\n",
+		 reg_init[id].bank,
+		 reg_init[id].addr,
+		 mask, value);
 
 	return 0;
 }
 
 static int ab8500_regulator_register(struct platform_device *pdev,
-					struct regulator_init_data *init_data,
-					int id,
-					struct device_node *np)
+				     struct regulator_init_data *init_data,
+				     int id, struct device_node *np)
 {
+	struct ab8500 *ab8500 = dev_get_drvdata(pdev->dev.parent);
 	struct ab8500_regulator_info *info = NULL;
 	struct regulator_config config = { };
 	int err;
 
 	/* assign per-regulator data */
-	info = &ab8500_regulator_info[id];
+	info = &abx500_regulator.info[id];
 	info->dev = &pdev->dev;
 
 	config.dev = &pdev->dev;
@@ -695,7 +3045,7 @@ static int ab8500_regulator_register(struct platform_device *pdev,
 	config.of_node = np;
 
 	/* fix for hardware before ab8500v2.0 */
-	if (abx500_get_chip_id(info->dev) < 0x20) {
+	if (is_ab8500_1p1_or_earlier(ab8500)) {
 		if (info->desc.id == AB8500_LDO_AUX3) {
 			info->desc.n_voltages =
 				ARRAY_SIZE(ldo_vauxn_voltages);
@@ -712,7 +3062,7 @@ static int ab8500_regulator_register(struct platform_device *pdev,
 			info->desc.name);
 		/* when we fail, un-register all earlier regulators */
 		while (--id >= 0) {
-			info = &ab8500_regulator_info[id];
+			info = &abx500_regulator.info[id];
 			regulator_unregister(info->regulator);
 		}
 		return err;
@@ -721,29 +3071,16 @@ static int ab8500_regulator_register(struct platform_device *pdev,
 	return 0;
 }
 
-static struct of_regulator_match ab8500_regulator_matches[] = {
-	{ .name	= "ab8500_ldo_aux1",    .driver_data = (void *) AB8500_LDO_AUX1, },
-	{ .name	= "ab8500_ldo_aux2",    .driver_data = (void *) AB8500_LDO_AUX2, },
-	{ .name	= "ab8500_ldo_aux3",    .driver_data = (void *) AB8500_LDO_AUX3, },
-	{ .name	= "ab8500_ldo_intcore", .driver_data = (void *) AB8500_LDO_INTCORE, },
-	{ .name	= "ab8500_ldo_tvout",   .driver_data = (void *) AB8500_LDO_TVOUT, },
-	{ .name = "ab8500_ldo_usb",     .driver_data = (void *) AB8500_LDO_USB, },
-	{ .name = "ab8500_ldo_audio",   .driver_data = (void *) AB8500_LDO_AUDIO, },
-	{ .name	= "ab8500_ldo_anamic1", .driver_data = (void *) AB8500_LDO_ANAMIC1, },
-	{ .name	= "ab8500_ldo_amamic2", .driver_data = (void *) AB8500_LDO_ANAMIC2, },
-	{ .name	= "ab8500_ldo_dmic",    .driver_data = (void *) AB8500_LDO_DMIC, },
-	{ .name	= "ab8500_ldo_ana",     .driver_data = (void *) AB8500_LDO_ANA, },
-};
-
 static int
-ab8500_regulator_of_probe(struct platform_device *pdev, struct device_node *np)
+ab8500_regulator_of_probe(struct platform_device *pdev,
+			  struct device_node *np)
 {
+	struct of_regulator_match *match = abx500_regulator.match;
 	int err, i;
 
-	for (i = 0; i < ARRAY_SIZE(ab8500_regulator_info); i++) {
+	for (i = 0; i < abx500_regulator.info_size; i++) {
 		err = ab8500_regulator_register(
-			pdev, ab8500_regulator_matches[i].init_data,
-			i, ab8500_regulator_matches[i].of_node);
+			pdev, match[i].init_data, i, match[i].of_node);
 		if (err)
 			return err;
 	}
@@ -754,14 +3091,22 @@ ab8500_regulator_of_probe(struct platform_device *pdev, struct device_node *np)
 static int ab8500_regulator_probe(struct platform_device *pdev)
 {
 	struct ab8500 *ab8500 = dev_get_drvdata(pdev->dev.parent);
-	struct ab8500_platform_data *pdata;
 	struct device_node *np = pdev->dev.of_node;
+	struct ab8500_platform_data *ppdata;
+	struct ab8500_regulator_platform_data *pdata;
 	int i, err;
 
+	if (!ab8500) {
+		dev_err(&pdev->dev, "null mfd parent\n");
+		return -EINVAL;
+	}
+
+	abx500_get_regulator_info(ab8500);
+
 	if (np) {
 		err = of_regulator_match(&pdev->dev, np,
-					ab8500_regulator_matches,
-					ARRAY_SIZE(ab8500_regulator_matches));
+					 abx500_regulator.match,
+					 abx500_regulator.match_size);
 		if (err < 0) {
 			dev_err(&pdev->dev,
 				"Error parsing regulator init data: %d\n", err);
@@ -772,46 +3117,61 @@ static int ab8500_regulator_probe(struct platform_device *pdev)
 		return err;
 	}
 
-	if (!ab8500) {
-		dev_err(&pdev->dev, "null mfd parent\n");
+	ppdata = dev_get_platdata(ab8500->dev);
+	if (!ppdata) {
+		dev_err(&pdev->dev, "null parent pdata\n");
 		return -EINVAL;
 	}
-	pdata = dev_get_platdata(ab8500->dev);
+
+	pdata = ppdata->regulator;
 	if (!pdata) {
 		dev_err(&pdev->dev, "null pdata\n");
 		return -EINVAL;
 	}
 
 	/* make sure the platform data has the correct size */
-	if (pdata->num_regulator != ARRAY_SIZE(ab8500_regulator_info)) {
+	if (pdata->num_regulator != abx500_regulator.info_size) {
 		dev_err(&pdev->dev, "Configuration error: size mismatch.\n");
 		return -EINVAL;
 	}
 
+	/* initialize debug (initial state is recorded with this call) */
+	err = ab8500_regulator_debug_init(pdev);
+	if (err)
+		return err;
+
 	/* initialize registers */
-	for (i = 0; i < pdata->num_regulator_reg_init; i++) {
-		int id, value;
+	for (i = 0; i < pdata->num_reg_init; i++) {
+		int id, mask, value;
 
-		id = pdata->regulator_reg_init[i].id;
-		value = pdata->regulator_reg_init[i].value;
+		id = pdata->reg_init[i].id;
+		mask = pdata->reg_init[i].mask;
+		value = pdata->reg_init[i].value;
 
 		/* check for configuration errors */
-		if (id >= AB8500_NUM_REGULATOR_REGISTERS) {
-			dev_err(&pdev->dev,
-				"Configuration error: id outside range.\n");
-			return -EINVAL;
-		}
+		BUG_ON(id >= abx500_regulator.init_size);
 
-		err = ab8500_regulator_init_registers(pdev, id, value);
+		err = ab8500_regulator_init_registers(pdev, id, mask, value);
 		if (err < 0)
 			return err;
 	}
 
+	if (!is_ab8505(ab8500)) {
+		/* register external regulators (before Vaux1, 2 and 3) */
+		err = ab8500_ext_regulator_init(pdev);
+		if (err)
+			return err;
+	}
+
 	/* register all regulators */
-	for (i = 0; i < ARRAY_SIZE(ab8500_regulator_info); i++) {
-		err = ab8500_regulator_register(pdev, &pdata->regulator[i], i, NULL);
-		if (err < 0)
+	for (i = 0; i < abx500_regulator.info_size; i++) {
+		err = ab8500_regulator_register(pdev, &pdata->regulator[i],
+						i, NULL);
+		if (err < 0) {
+			if (!is_ab8505(ab8500))
+				ab8500_ext_regulator_exit(pdev);
 			return err;
+		}
 	}
 
 	return 0;
@@ -819,11 +3179,12 @@ static int ab8500_regulator_probe(struct platform_device *pdev)
 
 static int ab8500_regulator_remove(struct platform_device *pdev)
 {
-	int i;
+	int i, err;
+	struct ab8500 *ab8500 = dev_get_drvdata(pdev->dev.parent);
 
-	for (i = 0; i < ARRAY_SIZE(ab8500_regulator_info); i++) {
+	for (i = 0; i < abx500_regulator.info_size; i++) {
 		struct ab8500_regulator_info *info = NULL;
-		info = &ab8500_regulator_info[i];
+		info = &abx500_regulator.info[i];
 
 		dev_vdbg(rdev_get_dev(info->regulator),
 			"%s-remove\n", info->desc.name);
@@ -831,6 +3192,15 @@ static int ab8500_regulator_remove(struct platform_device *pdev)
 		regulator_unregister(info->regulator);
 	}
 
+	/* remove external regulators (after Vaux1, 2 and 3) */
+	if (!is_ab8505(ab8500))
+		ab8500_ext_regulator_exit(pdev);
+
+	/* remove regulator debug */
+	err = ab8500_regulator_debug_exit(pdev);
+	if (err)
+		return err;
+
 	return 0;
 }
 
@@ -863,5 +3233,7 @@ module_exit(ab8500_regulator_exit);
 
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Sundar Iyer <sundar.iyer@stericsson.com>");
+MODULE_AUTHOR("Bengt Jonsson <bengt.g.jonsson@stericsson.com>");
+MODULE_AUTHOR("Daniel Willerud <daniel.willerud@stericsson.com>");
 MODULE_DESCRIPTION("Regulator Driver for ST-Ericsson AB8500 Mixed-Sig PMIC");
 MODULE_ALIAS("platform:ab8500-regulator");
diff --git a/drivers/regulator/arizona-ldo1.c b/drivers/regulator/arizona-ldo1.c
index ed7beec53af..81d8681c319 100644
--- a/drivers/regulator/arizona-ldo1.c
+++ b/drivers/regulator/arizona-ldo1.c
@@ -131,7 +131,7 @@ static const struct regulator_desc arizona_ldo1_hc = {
 	.min_uV = 900000,
 	.uV_step = 50000,
 	.n_voltages = 8,
-	.enable_time = 500,
+	.enable_time = 1500,
 
 	.owner = THIS_MODULE,
 };
diff --git a/drivers/regulator/as3711-regulator.c b/drivers/regulator/as3711-regulator.c
index f0ba8c4eefa..3da6bd6950c 100644
--- a/drivers/regulator/as3711-regulator.c
+++ b/drivers/regulator/as3711-regulator.c
@@ -13,9 +13,11 @@
 #include <linux/init.h>
 #include <linux/mfd/as3711.h>
 #include <linux/module.h>
+#include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/regmap.h>
 #include <linux/regulator/driver.h>
+#include <linux/regulator/of_regulator.h>
 #include <linux/slab.h>
 
 struct as3711_regulator_info {
@@ -276,6 +278,53 @@ static struct as3711_regulator_info as3711_reg_info[] = {
 
 #define AS3711_REGULATOR_NUM ARRAY_SIZE(as3711_reg_info)
 
+static struct of_regulator_match
+as3711_regulator_matches[AS3711_REGULATOR_NUM] = {
+	[AS3711_REGULATOR_SD_1] = { .name = "sd1" },
+	[AS3711_REGULATOR_SD_2] = { .name = "sd2" },
+	[AS3711_REGULATOR_SD_3] = { .name = "sd3" },
+	[AS3711_REGULATOR_SD_4] = { .name = "sd4" },
+	[AS3711_REGULATOR_LDO_1] = { .name = "ldo1" },
+	[AS3711_REGULATOR_LDO_2] = { .name = "ldo2" },
+	[AS3711_REGULATOR_LDO_3] = { .name = "ldo3" },
+	[AS3711_REGULATOR_LDO_4] = { .name = "ldo4" },
+	[AS3711_REGULATOR_LDO_5] = { .name = "ldo5" },
+	[AS3711_REGULATOR_LDO_6] = { .name = "ldo6" },
+	[AS3711_REGULATOR_LDO_7] = { .name = "ldo7" },
+	[AS3711_REGULATOR_LDO_8] = { .name = "ldo8" },
+};
+
+static int as3711_regulator_parse_dt(struct device *dev,
+				struct device_node **of_node, const int count)
+{
+	struct as3711_regulator_pdata *pdata = dev_get_platdata(dev);
+	struct device_node *regulators =
+		of_find_node_by_name(dev->parent->of_node, "regulators");
+	struct of_regulator_match *match;
+	int ret, i;
+
+	if (!regulators) {
+		dev_err(dev, "regulator node not found\n");
+		return -ENODEV;
+	}
+
+	ret = of_regulator_match(dev->parent, regulators,
+				 as3711_regulator_matches, count);
+	of_node_put(regulators);
+	if (ret < 0) {
+		dev_err(dev, "Error parsing regulator init data: %d\n", ret);
+		return ret;
+	}
+
+	for (i = 0, match = as3711_regulator_matches; i < count; i++, match++)
+		if (match->of_node) {
+			pdata->init_data[i] = match->init_data;
+			of_node[i] = match->of_node;
+		}
+
+	return 0;
+}
+
 static int as3711_regulator_probe(struct platform_device *pdev)
 {
 	struct as3711_regulator_pdata *pdata = dev_get_platdata(&pdev->dev);
@@ -284,13 +333,24 @@ static int as3711_regulator_probe(struct platform_device *pdev)
 	struct regulator_config config = {.dev = &pdev->dev,};
 	struct as3711_regulator *reg = NULL;
 	struct as3711_regulator *regs;
+	struct device_node *of_node[AS3711_REGULATOR_NUM] = {};
 	struct regulator_dev *rdev;
 	struct as3711_regulator_info *ri;
 	int ret;
 	int id;
 
-	if (!pdata)
-		dev_dbg(&pdev->dev, "No platform data...\n");
+	if (!pdata) {
+		dev_err(&pdev->dev, "No platform data...\n");
+		return -ENODEV;
+	}
+
+	if (pdev->dev.parent->of_node) {
+		ret = as3711_regulator_parse_dt(&pdev->dev, of_node, AS3711_REGULATOR_NUM);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "DT parsing failed: %d\n", ret);
+			return ret;
+		}
+	}
 
 	regs = devm_kzalloc(&pdev->dev, AS3711_REGULATOR_NUM *
 			sizeof(struct as3711_regulator), GFP_KERNEL);
@@ -300,7 +360,7 @@ static int as3711_regulator_probe(struct platform_device *pdev)
 	}
 
 	for (id = 0, ri = as3711_reg_info; id < AS3711_REGULATOR_NUM; ++id, ri++) {
-		reg_data = pdata ? pdata->init_data[id] : NULL;
+		reg_data = pdata->init_data[id];
 
 		/* No need to register if there is no regulator data */
 		if (!reg_data)
@@ -312,6 +372,7 @@ static int as3711_regulator_probe(struct platform_device *pdev)
 		config.init_data = reg_data;
 		config.driver_data = reg;
 		config.regmap = as3711->regmap;
+		config.of_node = of_node[id];
 
 		rdev = regulator_register(&ri->desc, &config);
 		if (IS_ERR(rdev)) {
diff --git a/drivers/regulator/core.c b/drivers/regulator/core.c
index e3661c20cf3..6e501784158 100644
--- a/drivers/regulator/core.c
+++ b/drivers/regulator/core.c
@@ -51,6 +51,7 @@
 static DEFINE_MUTEX(regulator_list_mutex);
 static LIST_HEAD(regulator_list);
 static LIST_HEAD(regulator_map_list);
+static LIST_HEAD(regulator_ena_gpio_list);
 static bool has_full_constraints;
 static bool board_wants_dummy_regulator;
 
@@ -69,6 +70,19 @@ struct regulator_map {
 };
 
 /*
+ * struct regulator_enable_gpio
+ *
+ * Management for shared enable GPIO pin
+ */
+struct regulator_enable_gpio {
+	struct list_head list;
+	int gpio;
+	u32 enable_count;	/* a number of enabled shared GPIO */
+	u32 request_count;	/* a number of requested shared GPIO */
+	unsigned int ena_gpio_invert:1;
+};
+
+/*
  * struct regulator
  *
  * One for each consumer device.
@@ -116,7 +130,7 @@ static const char *rdev_get_name(struct regulator_dev *rdev)
  * @supply: regulator supply name
  *
  * Extract the regulator device node corresponding to the supply name.
- * retruns the device node corresponding to the regulator if found, else
+ * returns the device node corresponding to the regulator if found, else
  * returns NULL.
  */
 static struct device_node *of_get_regulator(struct device *dev, const char *supply)
@@ -1229,7 +1243,7 @@ static struct regulator *_regulator_get(struct device *dev, const char *id,
 	struct regulator_dev *rdev;
 	struct regulator *regulator = ERR_PTR(-EPROBE_DEFER);
 	const char *devname = NULL;
-	int ret;
+	int ret = 0;
 
 	if (id == NULL) {
 		pr_err("get() with no identifier\n");
@@ -1245,6 +1259,15 @@ static struct regulator *_regulator_get(struct device *dev, const char *id,
 	if (rdev)
 		goto found;
 
+	/*
+	 * If we have return value from dev_lookup fail, we do not expect to
+	 * succeed, so, quit with appropriate error value
+	 */
+	if (ret) {
+		regulator = ERR_PTR(ret);
+		goto out;
+	}
+
 	if (board_wants_dummy_regulator) {
 		rdev = dummy_regulator_rdev;
 		goto found;
@@ -1456,6 +1479,101 @@ void devm_regulator_put(struct regulator *regulator)
 }
 EXPORT_SYMBOL_GPL(devm_regulator_put);
 
+/* Manage enable GPIO list. Same GPIO pin can be shared among regulators */
+static int regulator_ena_gpio_request(struct regulator_dev *rdev,
+				const struct regulator_config *config)
+{
+	struct regulator_enable_gpio *pin;
+	int ret;
+
+	list_for_each_entry(pin, &regulator_ena_gpio_list, list) {
+		if (pin->gpio == config->ena_gpio) {
+			rdev_dbg(rdev, "GPIO %d is already used\n",
+				config->ena_gpio);
+			goto update_ena_gpio_to_rdev;
+		}
+	}
+
+	ret = gpio_request_one(config->ena_gpio,
+				GPIOF_DIR_OUT | config->ena_gpio_flags,
+				rdev_get_name(rdev));
+	if (ret)
+		return ret;
+
+	pin = kzalloc(sizeof(struct regulator_enable_gpio), GFP_KERNEL);
+	if (pin == NULL) {
+		gpio_free(config->ena_gpio);
+		return -ENOMEM;
+	}
+
+	pin->gpio = config->ena_gpio;
+	pin->ena_gpio_invert = config->ena_gpio_invert;
+	list_add(&pin->list, &regulator_ena_gpio_list);
+
+update_ena_gpio_to_rdev:
+	pin->request_count++;
+	rdev->ena_pin = pin;
+	return 0;
+}
+
+static void regulator_ena_gpio_free(struct regulator_dev *rdev)
+{
+	struct regulator_enable_gpio *pin, *n;
+
+	if (!rdev->ena_pin)
+		return;
+
+	/* Free the GPIO only in case of no use */
+	list_for_each_entry_safe(pin, n, &regulator_ena_gpio_list, list) {
+		if (pin->gpio == rdev->ena_pin->gpio) {
+			if (pin->request_count <= 1) {
+				pin->request_count = 0;
+				gpio_free(pin->gpio);
+				list_del(&pin->list);
+				kfree(pin);
+			} else {
+				pin->request_count--;
+			}
+		}
+	}
+}
+
+/**
+ * Balance enable_count of each GPIO and actual GPIO pin control.
+ * GPIO is enabled in case of initial use. (enable_count is 0)
+ * GPIO is disabled when it is not shared any more. (enable_count <= 1)
+ */
+static int regulator_ena_gpio_ctrl(struct regulator_dev *rdev, bool enable)
+{
+	struct regulator_enable_gpio *pin = rdev->ena_pin;
+
+	if (!pin)
+		return -EINVAL;
+
+	if (enable) {
+		/* Enable GPIO at initial use */
+		if (pin->enable_count == 0)
+			gpio_set_value_cansleep(pin->gpio,
+						!pin->ena_gpio_invert);
+
+		pin->enable_count++;
+	} else {
+		if (pin->enable_count > 1) {
+			pin->enable_count--;
+			return 0;
+		}
+
+		/* Disable GPIO if not used */
+		if (pin->enable_count <= 1) {
+			gpio_set_value_cansleep(pin->gpio,
+						pin->ena_gpio_invert);
+			pin->enable_count = 0;
+		}
+	}
+
+	return 0;
+}
+
 static int _regulator_do_enable(struct regulator_dev *rdev)
 {
 	int ret, delay;
@@ -1471,9 +1589,10 @@ static int _regulator_do_enable(struct regulator_dev *rdev)
 
 	trace_regulator_enable(rdev_get_name(rdev));
 
-	if (rdev->ena_gpio) {
-		gpio_set_value_cansleep(rdev->ena_gpio,
-					!rdev->ena_gpio_invert);
+	if (rdev->ena_pin) {
+		ret = regulator_ena_gpio_ctrl(rdev, true);
+		if (ret < 0)
+			return ret;
 		rdev->ena_gpio_state = 1;
 	} else if (rdev->desc->ops->enable) {
 		ret = rdev->desc->ops->enable(rdev);
@@ -1575,9 +1694,10 @@ static int _regulator_do_disable(struct regulator_dev *rdev)
 
 	trace_regulator_disable(rdev_get_name(rdev));
 
-	if (rdev->ena_gpio) {
-		gpio_set_value_cansleep(rdev->ena_gpio,
-					rdev->ena_gpio_invert);
+	if (rdev->ena_pin) {
+		ret = regulator_ena_gpio_ctrl(rdev, false);
+		if (ret < 0)
+			return ret;
 		rdev->ena_gpio_state = 0;
 
 	} else if (rdev->desc->ops->disable) {
@@ -1794,7 +1914,10 @@ int regulator_is_enabled_regmap(struct regulator_dev *rdev)
 	if (ret != 0)
 		return ret;
 
-	return (val & rdev->desc->enable_mask) != 0;
+	if (rdev->desc->enable_is_inverted)
+		return (val & rdev->desc->enable_mask) == 0;
+	else
+		return (val & rdev->desc->enable_mask) != 0;
 }
 EXPORT_SYMBOL_GPL(regulator_is_enabled_regmap);
 
@@ -1809,9 +1932,15 @@ EXPORT_SYMBOL_GPL(regulator_is_enabled_regmap);
  */
 int regulator_enable_regmap(struct regulator_dev *rdev)
 {
+	unsigned int val;
+
+	if (rdev->desc->enable_is_inverted)
+		val = 0;
+	else
+		val = rdev->desc->enable_mask;
+
 	return regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
-				  rdev->desc->enable_mask,
-				  rdev->desc->enable_mask);
+				  rdev->desc->enable_mask, val);
 }
 EXPORT_SYMBOL_GPL(regulator_enable_regmap);
 
@@ -1826,15 +1955,22 @@ EXPORT_SYMBOL_GPL(regulator_enable_regmap);
  */
 int regulator_disable_regmap(struct regulator_dev *rdev)
 {
+	unsigned int val;
+
+	if (rdev->desc->enable_is_inverted)
+		val = rdev->desc->enable_mask;
+	else
+		val = 0;
+
 	return regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
-				  rdev->desc->enable_mask, 0);
+				  rdev->desc->enable_mask, val);
 }
 EXPORT_SYMBOL_GPL(regulator_disable_regmap);
 
 static int _regulator_is_enabled(struct regulator_dev *rdev)
 {
 	/* A GPIO control always takes precedence */
-	if (rdev->ena_gpio)
+	if (rdev->ena_pin)
 		return rdev->ena_gpio_state;
 
 	/* If we don't know then assume that the regulator is always on */
@@ -2138,6 +2274,37 @@ int regulator_map_voltage_iterate(struct regulator_dev *rdev,
 EXPORT_SYMBOL_GPL(regulator_map_voltage_iterate);
 
 /**
+ * regulator_map_voltage_ascend - map_voltage() for ascendant voltage list
+ *
+ * @rdev: Regulator to operate on
+ * @min_uV: Lower bound for voltage
+ * @max_uV: Upper bound for voltage
+ *
+ * Drivers that have ascendant voltage list can use this as their
+ * map_voltage() operation.
+ */
+int regulator_map_voltage_ascend(struct regulator_dev *rdev,
+				 int min_uV, int max_uV)
+{
+	int i, ret;
+
+	for (i = 0; i < rdev->desc->n_voltages; i++) {
+		ret = rdev->desc->ops->list_voltage(rdev, i);
+		if (ret < 0)
+			continue;
+
+		if (ret > max_uV)
+			break;
+
+		if (ret >= min_uV && ret <= max_uV)
+			return i;
+	}
+
+	return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(regulator_map_voltage_ascend);
+
+/**
  * regulator_map_voltage_linear - map_voltage() for simple linear mappings
  *
  * @rdev: Regulator to operate on
@@ -3237,7 +3404,7 @@ static int add_regulator_attributes(struct regulator_dev *rdev)
 		if (status < 0)
 			return status;
 	}
-	if (rdev->ena_gpio || ops->is_enabled) {
+	if (rdev->ena_pin || ops->is_enabled) {
 		status = device_create_file(dev, &dev_attr_state);
 		if (status < 0)
 			return status;
@@ -3439,22 +3606,17 @@ regulator_register(const struct regulator_desc *regulator_desc,
 	dev_set_drvdata(&rdev->dev, rdev);
 
 	if (config->ena_gpio && gpio_is_valid(config->ena_gpio)) {
-		ret = gpio_request_one(config->ena_gpio,
-				       GPIOF_DIR_OUT | config->ena_gpio_flags,
-				       rdev_get_name(rdev));
+		ret = regulator_ena_gpio_request(rdev, config);
 		if (ret != 0) {
 			rdev_err(rdev, "Failed to request enable GPIO%d: %d\n",
 				 config->ena_gpio, ret);
 			goto wash;
 		}
 
-		rdev->ena_gpio = config->ena_gpio;
-		rdev->ena_gpio_invert = config->ena_gpio_invert;
-
 		if (config->ena_gpio_flags & GPIOF_OUT_INIT_HIGH)
 			rdev->ena_gpio_state = 1;
 
-		if (rdev->ena_gpio_invert)
+		if (config->ena_gpio_invert)
 			rdev->ena_gpio_state = !rdev->ena_gpio_state;
 	}
 
@@ -3481,7 +3643,14 @@ regulator_register(const struct regulator_desc *regulator_desc,
 
 		r = regulator_dev_lookup(dev, supply, &ret);
 
-		if (!r) {
+		if (ret == -ENODEV) {
+			/*
+			 * No supply was specified for this regulator and
+			 * there will never be one.
+			 */
+			ret = 0;
+			goto add_dev;
+		} else if (!r) {
 			dev_err(dev, "Failed to find supply %s\n", supply);
 			ret = -EPROBE_DEFER;
 			goto scrub;
@@ -3499,6 +3668,7 @@ regulator_register(const struct regulator_desc *regulator_desc,
 		}
 	}
 
+add_dev:
 	/* add consumers devices */
 	if (init_data) {
 		for (i = 0; i < init_data->num_consumer_supplies; i++) {
@@ -3526,8 +3696,7 @@ unset_supplies:
 scrub:
 	if (rdev->supply)
 		_regulator_put(rdev->supply);
-	if (rdev->ena_gpio)
-		gpio_free(rdev->ena_gpio);
+	regulator_ena_gpio_free(rdev);
 	kfree(rdev->constraints);
 wash:
 	device_unregister(&rdev->dev);
@@ -3562,8 +3731,7 @@ void regulator_unregister(struct regulator_dev *rdev)
 	unset_regulator_supplies(rdev);
 	list_del(&rdev->list);
 	kfree(rdev->constraints);
-	if (rdev->ena_gpio)
-		gpio_free(rdev->ena_gpio);
+	regulator_ena_gpio_free(rdev);
 	device_unregister(&rdev->dev);
 	mutex_unlock(&regulator_list_mutex);
 }
diff --git a/drivers/regulator/dbx500-prcmu.h b/drivers/regulator/dbx500-prcmu.h
index e763883a44f..c8e51ace9f0 100644
--- a/drivers/regulator/dbx500-prcmu.h
+++ b/drivers/regulator/dbx500-prcmu.h
@@ -21,7 +21,6 @@
  * @is_enabled: status of the regulator
  * @epod_id: id for EPOD (power domain)
  * @is_ramret: RAM retention switch for EPOD (power domain)
- * @operating_point: operating point (only for vape, to be removed)
  *
  */
 struct dbx500_regulator_info {
@@ -32,7 +31,6 @@ struct dbx500_regulator_info {
 	u16 epod_id;
 	bool is_ramret;
 	bool exclude_from_power_state;
-	unsigned int operating_point;
 };
 
 void power_state_active_enable(void);
diff --git a/drivers/regulator/fan53555.c b/drivers/regulator/fan53555.c
index 9165b0c40ed..f0e1ae52bb0 100644
--- a/drivers/regulator/fan53555.c
+++ b/drivers/regulator/fan53555.c
@@ -219,9 +219,7 @@ static int fan53555_regulator_register(struct fan53555_device_info *di,
 	rdesc->owner = THIS_MODULE;
 
 	di->rdev = regulator_register(&di->desc, config);
-	if (IS_ERR(di->rdev))
-		return PTR_ERR(di->rdev);
-	return 0;
+	return PTR_RET(di->rdev);
 
 }
 
diff --git a/drivers/regulator/lp3971.c b/drivers/regulator/lp3971.c
index 9cb2c0f3451..d8af9e77331 100644
--- a/drivers/regulator/lp3971.c
+++ b/drivers/regulator/lp3971.c
@@ -163,6 +163,7 @@ static int lp3971_ldo_set_voltage_sel(struct regulator_dev *dev,
 
 static struct regulator_ops lp3971_ldo_ops = {
 	.list_voltage = regulator_list_voltage_table,
+	.map_voltage = regulator_map_voltage_ascend,
 	.is_enabled = lp3971_ldo_is_enabled,
 	.enable = lp3971_ldo_enable,
 	.disable = lp3971_ldo_disable,
@@ -236,6 +237,7 @@ static int lp3971_dcdc_set_voltage_sel(struct regulator_dev *dev,
 
 static struct regulator_ops lp3971_dcdc_ops = {
 	.list_voltage = regulator_list_voltage_table,
+	.map_voltage = regulator_map_voltage_ascend,
 	.is_enabled = lp3971_dcdc_is_enabled,
 	.enable = lp3971_dcdc_enable,
 	.disable = lp3971_dcdc_disable,
diff --git a/drivers/regulator/lp3972.c b/drivers/regulator/lp3972.c
index 0baabcfb578..61e4cf9edf6 100644
--- a/drivers/regulator/lp3972.c
+++ b/drivers/regulator/lp3972.c
@@ -309,6 +309,7 @@ static int lp3972_ldo_set_voltage_sel(struct regulator_dev *dev,
 
 static struct regulator_ops lp3972_ldo_ops = {
 	.list_voltage = regulator_list_voltage_table,
+	.map_voltage = regulator_map_voltage_ascend,
 	.is_enabled = lp3972_ldo_is_enabled,
 	.enable = lp3972_ldo_enable,
 	.disable = lp3972_ldo_disable,
@@ -389,6 +390,7 @@ static int lp3972_dcdc_set_voltage_sel(struct regulator_dev *dev,
 
 static struct regulator_ops lp3972_dcdc_ops = {
 	.list_voltage = regulator_list_voltage_table,
+	.map_voltage = regulator_map_voltage_ascend,
 	.is_enabled = lp3972_dcdc_is_enabled,
 	.enable = lp3972_dcdc_enable,
 	.disable = lp3972_dcdc_disable,
diff --git a/drivers/regulator/lp872x.c b/drivers/regulator/lp872x.c
index 8e3c7ae0047..f5fc4a142cd 100644
--- a/drivers/regulator/lp872x.c
+++ b/drivers/regulator/lp872x.c
@@ -478,6 +478,7 @@ static unsigned int lp872x_buck_get_mode(struct regulator_dev *rdev)
 
 static struct regulator_ops lp872x_ldo_ops = {
 	.list_voltage = regulator_list_voltage_table,
+	.map_voltage = regulator_map_voltage_ascend,
 	.set_voltage_sel = regulator_set_voltage_sel_regmap,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
 	.enable = regulator_enable_regmap,
@@ -488,6 +489,7 @@ static struct regulator_ops lp872x_ldo_ops = {
 
 static struct regulator_ops lp8720_buck_ops = {
 	.list_voltage = regulator_list_voltage_table,
+	.map_voltage = regulator_map_voltage_ascend,
 	.set_voltage_sel = lp872x_buck_set_voltage_sel,
 	.get_voltage_sel = lp872x_buck_get_voltage_sel,
 	.enable = regulator_enable_regmap,
@@ -500,6 +502,7 @@ static struct regulator_ops lp8720_buck_ops = {
 
 static struct regulator_ops lp8725_buck_ops = {
 	.list_voltage = regulator_list_voltage_table,
+	.map_voltage = regulator_map_voltage_ascend,
 	.set_voltage_sel = lp872x_buck_set_voltage_sel,
 	.get_voltage_sel = lp872x_buck_get_voltage_sel,
 	.enable = regulator_enable_regmap,
diff --git a/drivers/regulator/lp8788-buck.c b/drivers/regulator/lp8788-buck.c
index 97891a7ea7b..eb1e1e88ae5 100644
--- a/drivers/regulator/lp8788-buck.c
+++ b/drivers/regulator/lp8788-buck.c
@@ -346,6 +346,7 @@ static unsigned int lp8788_buck_get_mode(struct regulator_dev *rdev)
 
 static struct regulator_ops lp8788_buck12_ops = {
 	.list_voltage = regulator_list_voltage_table,
+	.map_voltage = regulator_map_voltage_ascend,
 	.set_voltage_sel = lp8788_buck12_set_voltage_sel,
 	.get_voltage_sel = lp8788_buck12_get_voltage_sel,
 	.enable = regulator_enable_regmap,
@@ -358,6 +359,7 @@ static struct regulator_ops lp8788_buck12_ops = {
 
 static struct regulator_ops lp8788_buck34_ops = {
 	.list_voltage = regulator_list_voltage_table,
+	.map_voltage = regulator_map_voltage_ascend,
 	.set_voltage_sel = regulator_set_voltage_sel_regmap,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
 	.enable = regulator_enable_regmap,
diff --git a/drivers/regulator/lp8788-ldo.c b/drivers/regulator/lp8788-ldo.c
index cd5a14ad926..0ce2c4c194b 100644
--- a/drivers/regulator/lp8788-ldo.c
+++ b/drivers/regulator/lp8788-ldo.c
@@ -156,68 +156,6 @@ static const int lp8788_aldo7_vtbl[] = {
 	1200000, 1300000, 1400000, 1500000, 1600000, 1700000, 1800000, 1800000,
 };
 
-static enum lp8788_ldo_id lp8788_dldo_id[] = {
-	DLDO1,
-	DLDO2,
-	DLDO3,
-	DLDO4,
-	DLDO5,
-	DLDO6,
-	DLDO7,
-	DLDO8,
-	DLDO9,
-	DLDO10,
-	DLDO11,
-	DLDO12,
-};
-
-static enum lp8788_ldo_id lp8788_aldo_id[] = {
-	ALDO1,
-	ALDO2,
-	ALDO3,
-	ALDO4,
-	ALDO5,
-	ALDO6,
-	ALDO7,
-	ALDO8,
-	ALDO9,
-	ALDO10,
-};
-
-static int lp8788_ldo_enable(struct regulator_dev *rdev)
-{
-	struct lp8788_ldo *ldo = rdev_get_drvdata(rdev);
-
-	if (ldo->en_pin) {
-		gpio_set_value(ldo->en_pin->gpio, ENABLE);
-		return 0;
-	} else {
-		return regulator_enable_regmap(rdev);
-	}
-}
-
-static int lp8788_ldo_disable(struct regulator_dev *rdev)
-{
-	struct lp8788_ldo *ldo = rdev_get_drvdata(rdev);
-
-	if (ldo->en_pin) {
-		gpio_set_value(ldo->en_pin->gpio, DISABLE);
-		return 0;
-	} else {
-		return regulator_disable_regmap(rdev);
-	}
-}
-
-static int lp8788_ldo_is_enabled(struct regulator_dev *rdev)
-{
-	struct lp8788_ldo *ldo = rdev_get_drvdata(rdev);
-
-	if (ldo->en_pin)
-		return gpio_get_value(ldo->en_pin->gpio) ? 1 : 0;
-	else
-		return regulator_is_enabled_regmap(rdev);
-}
-
 static int lp8788_ldo_enable_time(struct regulator_dev *rdev)
 {
 	struct lp8788_ldo *ldo = rdev_get_drvdata(rdev);
@@ -232,38 +170,21 @@ static int lp8788_ldo_enable_time(struct regulator_dev *rdev)
 	return ENABLE_TIME_USEC * val;
 }
 
-static int lp8788_ldo_fixed_get_voltage(struct regulator_dev *rdev)
-{
-	enum lp8788_ldo_id id = rdev_get_id(rdev);
-
-	switch (id) {
-	case ALDO2 ... ALDO5:
-		return 2850000;
-	case DLDO12:
-	case ALDO8 ... ALDO9:
-		return 2500000;
-	case ALDO10:
-		return 1100000;
-	default:
-		return -EINVAL;
-	}
-}
-
 static struct regulator_ops lp8788_ldo_voltage_table_ops = {
 	.list_voltage = regulator_list_voltage_table,
 	.set_voltage_sel = regulator_set_voltage_sel_regmap,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
-	.enable = lp8788_ldo_enable,
-	.disable = lp8788_ldo_disable,
-	.is_enabled = lp8788_ldo_is_enabled,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
 	.enable_time = lp8788_ldo_enable_time,
 };
 
 static struct regulator_ops lp8788_ldo_voltage_fixed_ops = {
-	.get_voltage = lp8788_ldo_fixed_get_voltage,
-	.enable = lp8788_ldo_enable,
-	.disable = lp8788_ldo_disable,
-	.is_enabled = lp8788_ldo_is_enabled,
+	.list_voltage = regulator_list_voltage_linear,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
 	.enable_time = lp8788_ldo_enable_time,
 };
 
@@ -420,6 +341,7 @@ static struct regulator_desc lp8788_dldo_desc[] = {
 		.owner = THIS_MODULE,
 		.enable_reg = LP8788_EN_LDO_B,
 		.enable_mask = LP8788_EN_DLDO12_M,
+		.min_uV = 2500000,
 	},
 };
 
@@ -446,6 +368,7 @@ static struct regulator_desc lp8788_aldo_desc[] = {
 		.owner = THIS_MODULE,
 		.enable_reg = LP8788_EN_LDO_B,
 		.enable_mask = LP8788_EN_ALDO2_M,
+		.min_uV = 2850000,
 	},
 	{
 		.name = "aldo3",
@@ -456,6 +379,7 @@ static struct regulator_desc lp8788_aldo_desc[] = {
 		.owner = THIS_MODULE,
 		.enable_reg = LP8788_EN_LDO_B,
 		.enable_mask = LP8788_EN_ALDO3_M,
+		.min_uV = 2850000,
 	},
 	{
 		.name = "aldo4",
@@ -466,6 +390,7 @@ static struct regulator_desc lp8788_aldo_desc[] = {
 		.owner = THIS_MODULE,
 		.enable_reg = LP8788_EN_LDO_B,
 		.enable_mask = LP8788_EN_ALDO4_M,
+		.min_uV = 2850000,
 	},
 	{
 		.name = "aldo5",
@@ -476,6 +401,7 @@ static struct regulator_desc lp8788_aldo_desc[] = {
 		.owner = THIS_MODULE,
 		.enable_reg = LP8788_EN_LDO_C,
 		.enable_mask = LP8788_EN_ALDO5_M,
+		.min_uV = 2850000,
 	},
 	{
 		.name = "aldo6",
@@ -512,6 +438,7 @@ static struct regulator_desc lp8788_aldo_desc[] = {
 		.owner = THIS_MODULE,
 		.enable_reg = LP8788_EN_LDO_C,
 		.enable_mask = LP8788_EN_ALDO8_M,
+		.min_uV = 2500000,
 	},
 	{
 		.name = "aldo9",
@@ -522,6 +449,7 @@ static struct regulator_desc lp8788_aldo_desc[] = {
 		.owner = THIS_MODULE,
 		.enable_reg = LP8788_EN_LDO_C,
 		.enable_mask = LP8788_EN_ALDO9_M,
+		.min_uV = 2500000,
 	},
 	{
 		.name = "aldo10",
@@ -532,46 +460,14 @@ static struct regulator_desc lp8788_aldo_desc[] = {
 		.owner = THIS_MODULE,
 		.enable_reg = LP8788_EN_LDO_C,
 		.enable_mask = LP8788_EN_ALDO10_M,
+		.min_uV = 1100000,
 	},
 };
 
-static int lp8788_gpio_request_ldo_en(struct platform_device *pdev,
-				struct lp8788_ldo *ldo,
-				enum lp8788_ext_ldo_en_id id)
-{
-	struct device *dev = &pdev->dev;
-	struct lp8788_ldo_enable_pin *pin = ldo->en_pin;
-	int ret, gpio, pinstate;
-	char *name[] = {
-		[EN_ALDO1]   = "LP8788_EN_ALDO1",
-		[EN_ALDO234] = "LP8788_EN_ALDO234",
-		[EN_ALDO5]   = "LP8788_EN_ALDO5",
-		[EN_ALDO7]   = "LP8788_EN_ALDO7",
-		[EN_DLDO7]   = "LP8788_EN_DLDO7",
-		[EN_DLDO911] = "LP8788_EN_DLDO911",
-	};
-
-	gpio = pin->gpio;
-	if (!gpio_is_valid(gpio)) {
-		dev_err(dev, "invalid gpio: %d\n", gpio);
-		return -EINVAL;
-	}
-
-	pinstate = pin->init_state;
-	ret = devm_gpio_request_one(dev, gpio, pinstate, name[id]);
-	if (ret == -EBUSY) {
-		dev_warn(dev, "gpio%d already used\n", gpio);
-		return 0;
-	}
-
-	return ret;
-}
-
 static int lp8788_config_ldo_enable_mode(struct platform_device *pdev,
 					struct lp8788_ldo *ldo,
 					enum lp8788_ldo_id id)
 {
-	int ret;
 	struct lp8788 *lp = ldo->lp;
 	struct lp8788_platform_data *pdata = lp->pdata;
 	enum lp8788_ext_ldo_en_id enable_id;
@@ -613,14 +509,7 @@ static int lp8788_config_ldo_enable_mode(struct platform_device *pdev,
 		goto set_default_ldo_enable_mode;
 
 	ldo->en_pin = pdata->ldo_pin[enable_id];
-
-	ret = lp8788_gpio_request_ldo_en(pdev, ldo, enable_id);
-	if (ret) {
-		ldo->en_pin = NULL;
-		goto set_default_ldo_enable_mode;
-	}
-
-	return ret;
+	return 0;
 
 set_default_ldo_enable_mode:
 	return lp8788_update_bits(lp, LP8788_EN_SEL, en_mask[enable_id], 0);
@@ -640,10 +529,15 @@ static int lp8788_dldo_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	ldo->lp = lp;
-	ret = lp8788_config_ldo_enable_mode(pdev, ldo, lp8788_dldo_id[id]);
+	ret = lp8788_config_ldo_enable_mode(pdev, ldo, id);
 	if (ret)
 		return ret;
 
+	if (ldo->en_pin) {
+		cfg.ena_gpio = ldo->en_pin->gpio;
+		cfg.ena_gpio_flags = ldo->en_pin->init_state;
+	}
+
 	cfg.dev = pdev->dev.parent;
 	cfg.init_data = lp->pdata ? lp->pdata->dldo_data[id] : NULL;
 	cfg.driver_data = ldo;
@@ -696,10 +590,15 @@ static int lp8788_aldo_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	ldo->lp = lp;
-	ret = lp8788_config_ldo_enable_mode(pdev, ldo, lp8788_aldo_id[id]);
+	ret = lp8788_config_ldo_enable_mode(pdev, ldo, id + ALDO1);
 	if (ret)
 		return ret;
 
+	if (ldo->en_pin) {
+		cfg.ena_gpio = ldo->en_pin->gpio;
+		cfg.ena_gpio_flags = ldo->en_pin->init_state;
+	}
+
 	cfg.dev = pdev->dev.parent;
 	cfg.init_data = lp->pdata ? lp->pdata->aldo_data[id] : NULL;
 	cfg.driver_data = ldo;
diff --git a/drivers/regulator/max1586.c b/drivers/regulator/max1586.c
index 8c5a54f541b..54af6101581 100644
--- a/drivers/regulator/max1586.c
+++ b/drivers/regulator/max1586.c
@@ -56,7 +56,7 @@ struct max1586_data {
  *   set V6 to either 0V, 1.8V, 2.5V, 3V depending on (x & 0x3)
  * As regulator framework doesn't accept voltages to be 0V, we use 1uV.
  */
-static int v6_voltages_uv[] = { 1, 1800000, 2500000, 3000000 };
+static const unsigned int v6_voltages_uv[] = { 1, 1800000, 2500000, 3000000 };
 
 /*
  * V3 voltage
@@ -232,8 +232,7 @@ static int max1586_pmic_remove(struct i2c_client *client)
 	int i;
 
 	for (i = 0; i <= MAX1586_V6; i++)
-		if (max1586->rdev[i])
-			regulator_unregister(max1586->rdev[i]);
+		regulator_unregister(max1586->rdev[i]);
 	return 0;
 }
 
diff --git a/drivers/regulator/max77686.c b/drivers/regulator/max77686.c
index e4586ee8858..20935b1a6ed 100644
--- a/drivers/regulator/max77686.c
+++ b/drivers/regulator/max77686.c
@@ -75,17 +75,20 @@ static int max77686_buck_set_suspend_disable(struct regulator_dev *rdev)
 {
 	unsigned int val;
 	struct max77686_data *max77686 = rdev_get_drvdata(rdev);
-        int id = rdev_get_id(rdev);
+	int ret, id = rdev_get_id(rdev);
 
 	if (id == MAX77686_BUCK1)
 		val = 0x1;
 	else
 		val = 0x1 << MAX77686_OPMODE_BUCK234_SHIFT;
 
+	ret = regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
+				 rdev->desc->enable_mask, val);
+	if (ret)
+		return ret;
+
 	max77686->opmode[id] = val;
-	return regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
-				  rdev->desc->enable_mask,
-				  val);
+	return 0;
 }
 
 /* Some LDOs supports [LPM/Normal]ON mode during suspend state */
@@ -94,7 +97,7 @@ static int max77686_set_suspend_mode(struct regulator_dev *rdev,
 {
 	struct max77686_data *max77686 = rdev_get_drvdata(rdev);
 	unsigned int val;
-        int id = rdev_get_id(rdev);
+	int ret, id = rdev_get_id(rdev);
 
 	/* BUCK[5-9] doesn't support this feature */
 	if (id >= MAX77686_BUCK5)
@@ -113,10 +116,13 @@ static int max77686_set_suspend_mode(struct regulator_dev *rdev,
 		return -EINVAL;
 	}
 
+	ret = regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
+				  rdev->desc->enable_mask, val);
+	if (ret)
+		return ret;
+
 	max77686->opmode[id] = val;
-	return regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
-				  rdev->desc->enable_mask,
-				  val);
+	return 0;
 }
 
 /* Some LDOs supports LPM-ON/OFF/Normal-ON mode during suspend state */
@@ -125,6 +131,7 @@ static int max77686_ldo_set_suspend_mode(struct regulator_dev *rdev,
 {
 	unsigned int val;
 	struct max77686_data *max77686 = rdev_get_drvdata(rdev);
+	int ret;
 
 	switch (mode) {
 	case REGULATOR_MODE_STANDBY:			/* switch off */
@@ -142,10 +149,13 @@ static int max77686_ldo_set_suspend_mode(struct regulator_dev *rdev,
 		return -EINVAL;
 	}
 
+	ret = regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
+				 rdev->desc->enable_mask, val);
+	if (ret)
+		return ret;
+
 	max77686->opmode[rdev_get_id(rdev)] = val;
-	return regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
-				  rdev->desc->enable_mask,
-				  val);
+	return 0;
 }
 
 static int max77686_enable(struct regulator_dev *rdev)
diff --git a/drivers/regulator/max8649.c b/drivers/regulator/max8649.c
index 3ca14380f22..db6c9be10f3 100644
--- a/drivers/regulator/max8649.c
+++ b/drivers/regulator/max8649.c
@@ -60,36 +60,6 @@ struct max8649_regulator_info {
 	unsigned	ramp_down:1;
 };
 
-/* EN_PD means pulldown on EN input */
-static int max8649_enable(struct regulator_dev *rdev)
-{
-	struct max8649_regulator_info *info = rdev_get_drvdata(rdev);
-	return regmap_update_bits(info->regmap, MAX8649_CONTROL, MAX8649_EN_PD, 0);
-}
-
-/*
- * Applied internal pulldown resistor on EN input pin.
- * If pulldown EN pin outside, it would be better.
- */
-static int max8649_disable(struct regulator_dev *rdev)
-{
-	struct max8649_regulator_info *info = rdev_get_drvdata(rdev);
-	return regmap_update_bits(info->regmap, MAX8649_CONTROL, MAX8649_EN_PD,
-				MAX8649_EN_PD);
-}
-
-static int max8649_is_enabled(struct regulator_dev *rdev)
-{
-	struct max8649_regulator_info *info = rdev_get_drvdata(rdev);
-	unsigned int val;
-	int ret;
-
-	ret = regmap_read(info->regmap, MAX8649_CONTROL, &val);
-	if (ret != 0)
-		return ret;
-	return !((unsigned char)val & MAX8649_EN_PD);
-}
-
 static int max8649_enable_time(struct regulator_dev *rdev)
 {
 	struct max8649_regulator_info *info = rdev_get_drvdata(rdev);
@@ -151,9 +121,9 @@ static struct regulator_ops max8649_dcdc_ops = {
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
 	.list_voltage	= regulator_list_voltage_linear,
 	.map_voltage	= regulator_map_voltage_linear,
-	.enable		= max8649_enable,
-	.disable	= max8649_disable,
-	.is_enabled	= max8649_is_enabled,
+	.enable		= regulator_enable_regmap,
+	.disable	= regulator_disable_regmap,
+	.is_enabled	= regulator_is_enabled_regmap,
 	.enable_time	= max8649_enable_time,
 	.set_mode	= max8649_set_mode,
 	.get_mode	= max8649_get_mode,
@@ -169,6 +139,9 @@ static struct regulator_desc dcdc_desc = {
 	.vsel_mask	= MAX8649_VOL_MASK,
 	.min_uV		= MAX8649_DCDC_VMIN,
 	.uV_step	= MAX8649_DCDC_STEP,
+	.enable_reg	= MAX8649_CONTROL,
+	.enable_mask	= MAX8649_EN_PD,
+	.enable_is_inverted = true,
 };
 
 static struct regmap_config max8649_regmap_config = {
@@ -275,10 +248,8 @@ static int max8649_regulator_remove(struct i2c_client *client)
 {
 	struct max8649_regulator_info *info = i2c_get_clientdata(client);
 
-	if (info) {
-		if (info->regulator)
-			regulator_unregister(info->regulator);
-	}
+	if (info)
+		regulator_unregister(info->regulator);
 
 	return 0;
 }
diff --git a/drivers/regulator/max8660.c b/drivers/regulator/max8660.c
index 4d7c635c36c..d428ef9a626 100644
--- a/drivers/regulator/max8660.c
+++ b/drivers/regulator/max8660.c
@@ -426,8 +426,7 @@ static int max8660_remove(struct i2c_client *client)
 	int i;
 
 	for (i = 0; i < MAX8660_V_END; i++)
-		if (max8660->rdev[i])
-			regulator_unregister(max8660->rdev[i]);
+		regulator_unregister(max8660->rdev[i]);
 	return 0;
 }
 
diff --git a/drivers/regulator/max8925-regulator.c b/drivers/regulator/max8925-regulator.c
index 0d5f64a805a..3597da8f0dc 100644
--- a/drivers/regulator/max8925-regulator.c
+++ b/drivers/regulator/max8925-regulator.c
@@ -246,7 +246,6 @@ static struct max8925_regulator_info max8925_regulator_info[] = {
 
 #ifdef CONFIG_OF
 static int max8925_regulator_dt_init(struct platform_device *pdev,
-				    struct max8925_regulator_info *info,
 				    struct regulator_config *config,
 				    int ridx)
 {
@@ -272,7 +271,7 @@ static int max8925_regulator_dt_init(struct platform_device *pdev,
 	return 0;
 }
 #else
-#define max8925_regulator_dt_init(w, x, y, z)	(-1)
+#define max8925_regulator_dt_init(x, y, z)	(-1)
 #endif
 
 static int max8925_regulator_probe(struct platform_device *pdev)
@@ -309,7 +308,7 @@ static int max8925_regulator_probe(struct platform_device *pdev)
 	config.dev = &pdev->dev;
 	config.driver_data = ri;
 
-	if (max8925_regulator_dt_init(pdev, ri, &config, regulator_idx))
+	if (max8925_regulator_dt_init(pdev, &config, regulator_idx))
 		if (pdata)
 			config.init_data = pdata;
 
diff --git a/drivers/regulator/max8952.c b/drivers/regulator/max8952.c
index fc7935a19e3..5259c2fea90 100644
--- a/drivers/regulator/max8952.c
+++ b/drivers/regulator/max8952.c
@@ -28,6 +28,9 @@
 #include <linux/regulator/max8952.h>
 #include <linux/gpio.h>
 #include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+#include <linux/regulator/of_regulator.h>
 #include <linux/slab.h>
 
 /* Registers */
@@ -126,6 +129,69 @@ static const struct regulator_desc regulator = {
 	.owner		= THIS_MODULE,
 };
 
+#ifdef CONFIG_OF
+static struct of_device_id max8952_dt_match[] = {
+	{ .compatible = "maxim,max8952" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, max8952_dt_match);
+
+static struct max8952_platform_data *max8952_parse_dt(struct device *dev)
+{
+	struct max8952_platform_data *pd;
+	struct device_node *np = dev->of_node;
+	int ret;
+	int i;
+
+	pd = devm_kzalloc(dev, sizeof(*pd), GFP_KERNEL);
+	if (!pd) {
+		dev_err(dev, "Failed to allocate platform data\n");
+		return NULL;
+	}
+
+	pd->gpio_vid0 = of_get_named_gpio(np, "max8952,vid-gpios", 0);
+	pd->gpio_vid1 = of_get_named_gpio(np, "max8952,vid-gpios", 1);
+	pd->gpio_en = of_get_named_gpio(np, "max8952,en-gpio", 0);
+
+	if (of_property_read_u32(np, "max8952,default-mode", &pd->default_mode))
+		dev_warn(dev, "Default mode not specified, assuming 0\n");
+
+	ret = of_property_read_u32_array(np, "max8952,dvs-mode-microvolt",
+					pd->dvs_mode, ARRAY_SIZE(pd->dvs_mode));
+	if (ret) {
+		dev_err(dev, "max8952,dvs-mode-microvolt property not specified");
+		return NULL;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(pd->dvs_mode); ++i) {
+		if (pd->dvs_mode[i] < 770000 || pd->dvs_mode[i] > 1400000) {
+			dev_err(dev, "DVS voltage %d out of range\n", i);
+			return NULL;
+		}
+		pd->dvs_mode[i] = (pd->dvs_mode[i] - 770000) / 10000;
+	}
+
+	if (of_property_read_u32(np, "max8952,sync-freq", &pd->sync_freq))
+		dev_warn(dev, "max8952,sync-freq property not specified, defaulting to 26MHz\n");
+
+	if (of_property_read_u32(np, "max8952,ramp-speed", &pd->ramp_speed))
+		dev_warn(dev, "max8952,ramp-speed property not specified, defaulting to 32mV/us\n");
+
+	pd->reg_data = of_get_regulator_init_data(dev, np);
+	if (!pd->reg_data) {
+		dev_err(dev, "Failed to parse regulator init data\n");
+		return NULL;
+	}
+
+	return pd;
+}
+#else
+static struct max8952_platform_data *max8952_parse_dt(struct device *dev)
+{
+	return NULL;
+}
+#endif
+
 static int max8952_pmic_probe(struct i2c_client *client,
 		const struct i2c_device_id *i2c_id)
 {
@@ -136,6 +202,9 @@ static int max8952_pmic_probe(struct i2c_client *client,
 
 	int ret = 0, err = 0;
 
+	if (client->dev.of_node)
+		pdata = max8952_parse_dt(&client->dev);
+
 	if (!pdata) {
 		dev_err(&client->dev, "Require the platform data\n");
 		return -EINVAL;
@@ -154,11 +223,12 @@ static int max8952_pmic_probe(struct i2c_client *client,
 	max8952->pdata = pdata;
 
 	config.dev = max8952->dev;
-	config.init_data = &pdata->reg_data;
+	config.init_data = pdata->reg_data;
 	config.driver_data = max8952;
+	config.of_node = client->dev.of_node;
 
 	config.ena_gpio = pdata->gpio_en;
-	if (pdata->reg_data.constraints.boot_on)
+	if (pdata->reg_data->constraints.boot_on)
 		config.ena_gpio_flags |= GPIOF_OUT_INIT_HIGH;
 
 	max8952->rdev = regulator_register(&regulator, &config);
@@ -271,6 +341,7 @@ static struct i2c_driver max8952_pmic_driver = {
 	.remove		= max8952_pmic_remove,
 	.driver		= {
 		.name	= "max8952",
+		.of_match_table = of_match_ptr(max8952_dt_match),
 	},
 	.id_table	= max8952_ids,
 };
diff --git a/drivers/regulator/max8973-regulator.c b/drivers/regulator/max8973-regulator.c
index 9a8ea916300..adb1414e5e3 100644
--- a/drivers/regulator/max8973-regulator.c
+++ b/drivers/regulator/max8973-regulator.c
@@ -274,15 +274,15 @@ static int max8973_init_dcdc(struct max8973_chip *max,
 	if (pdata->reg_init_data &&
 			pdata->reg_init_data->constraints.ramp_delay) {
 		if (pdata->reg_init_data->constraints.ramp_delay < 25000)
-			control1 = MAX8973_RAMP_12mV_PER_US;
+			control1 |= MAX8973_RAMP_12mV_PER_US;
 		else if (pdata->reg_init_data->constraints.ramp_delay < 50000)
-			control1 = MAX8973_RAMP_25mV_PER_US;
+			control1 |= MAX8973_RAMP_25mV_PER_US;
 		else if (pdata->reg_init_data->constraints.ramp_delay < 200000)
-			control1 = MAX8973_RAMP_50mV_PER_US;
+			control1 |= MAX8973_RAMP_50mV_PER_US;
 		else
-			control1 = MAX8973_RAMP_200mV_PER_US;
+			control1 |= MAX8973_RAMP_200mV_PER_US;
 	} else {
-		control1 = MAX8973_RAMP_12mV_PER_US;
+		control1 |= MAX8973_RAMP_12mV_PER_US;
 		max->desc.ramp_delay = 12500;
 	}
 
diff --git a/drivers/regulator/max8997.c b/drivers/regulator/max8997.c
index 0ac7a87519b..df20069f053 100644
--- a/drivers/regulator/max8997.c
+++ b/drivers/regulator/max8997.c
@@ -1035,8 +1035,8 @@ static int max8997_pmic_probe(struct platform_device *pdev)
 	int i, ret, size, nr_dvs;
 	u8 max_buck1 = 0, max_buck2 = 0, max_buck5 = 0;
 
-	if (IS_ERR_OR_NULL(pdata)) {
-		dev_err(pdev->dev.parent, "No platform init data supplied.\n");
+	if (!pdata) {
+		dev_err(&pdev->dev, "No platform init data supplied.\n");
 		return -ENODEV;
 	}
 
diff --git a/drivers/regulator/max8998.c b/drivers/regulator/max8998.c
index b588f07c7ca..a57a1b15cdb 100644
--- a/drivers/regulator/max8998.c
+++ b/drivers/regulator/max8998.c
@@ -665,14 +665,16 @@ static int max8998_pmic_probe(struct platform_device *pdev)
 	    gpio_is_valid(pdata->buck1_set2)) {
 		/* Check if SET1 is not equal to 0 */
 		if (!pdata->buck1_set1) {
-			printk(KERN_ERR "MAX8998 SET1 GPIO defined as 0 !\n");
+			dev_err(&pdev->dev,
+				"MAX8998 SET1 GPIO defined as 0 !\n");
 			WARN_ON(!pdata->buck1_set1);
 			ret = -EIO;
 			goto err_out;
 		}
 		/* Check if SET2 is not equal to 0 */
 		if (!pdata->buck1_set2) {
-			printk(KERN_ERR "MAX8998 SET2 GPIO defined as 0 !\n");
+			dev_err(&pdev->dev,
+				"MAX8998 SET2 GPIO defined as 0 !\n");
 			WARN_ON(!pdata->buck1_set2);
 			ret = -EIO;
 			goto err_out;
@@ -738,7 +740,8 @@ static int max8998_pmic_probe(struct platform_device *pdev)
 	if (gpio_is_valid(pdata->buck2_set3)) {
 		/* Check if SET3 is not equal to 0 */
 		if (!pdata->buck2_set3) {
-			printk(KERN_ERR "MAX8998 SET3 GPIO defined as 0 !\n");
+			dev_err(&pdev->dev,
+				"MAX8998 SET3 GPIO defined as 0 !\n");
 			WARN_ON(!pdata->buck2_set3);
 			ret = -EIO;
 			goto err_out;
diff --git a/drivers/regulator/mc13783-regulator.c b/drivers/regulator/mc13783-regulator.c
index c46c6705cd7..fdf7f0a0909 100644
--- a/drivers/regulator/mc13783-regulator.c
+++ b/drivers/regulator/mc13783-regulator.c
@@ -398,33 +398,51 @@ static int mc13783_regulator_probe(struct platform_device *pdev)
 	struct mc13xxx *mc13783 = dev_get_drvdata(pdev->dev.parent);
 	struct mc13xxx_regulator_platform_data *pdata =
 		dev_get_platdata(&pdev->dev);
-	struct mc13xxx_regulator_init_data *init_data;
+	struct mc13xxx_regulator_init_data *mc13xxx_data;
 	struct regulator_config config = { };
-	int i, ret;
+	int i, ret, num_regulators;
 
-	dev_dbg(&pdev->dev, "%s id %d\n", __func__, pdev->id);
+	num_regulators = mc13xxx_get_num_regulators_dt(pdev);
 
-	if (!pdata)
+	if (num_regulators <= 0 && pdata)
+		num_regulators = pdata->num_regulators;
+	if (num_regulators <= 0)
 		return -EINVAL;
 
 	priv = devm_kzalloc(&pdev->dev, sizeof(*priv) +
-			pdata->num_regulators * sizeof(priv->regulators[0]),
+			num_regulators * sizeof(priv->regulators[0]),
 			GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
 
+	priv->num_regulators = num_regulators;
 	priv->mc13xxx_regulators = mc13783_regulators;
 	priv->mc13xxx = mc13783;
+	platform_set_drvdata(pdev, priv);
 
-	for (i = 0; i < pdata->num_regulators; i++) {
-		struct regulator_desc *desc;
+	mc13xxx_data = mc13xxx_parse_regulators_dt(pdev, mc13783_regulators,
+					ARRAY_SIZE(mc13783_regulators));
 
-		init_data = &pdata->regulators[i];
-		desc = &mc13783_regulators[init_data->id].desc;
+	for (i = 0; i < priv->num_regulators; i++) {
+		struct regulator_init_data *init_data;
+		struct regulator_desc *desc;
+		struct device_node *node = NULL;
+		int id;
+
+		if (mc13xxx_data) {
+			id = mc13xxx_data[i].id;
+			init_data = mc13xxx_data[i].init_data;
+			node = mc13xxx_data[i].node;
+		} else {
+			id = pdata->regulators[i].id;
+			init_data = pdata->regulators[i].init_data;
+		}
+		desc = &mc13783_regulators[id].desc;
 
 		config.dev = &pdev->dev;
-		config.init_data = init_data->init_data;
+		config.init_data = init_data;
 		config.driver_data = priv;
+		config.of_node = node;
 
 		priv->regulators[i] = regulator_register(desc, &config);
 		if (IS_ERR(priv->regulators[i])) {
@@ -435,8 +453,6 @@ static int mc13783_regulator_probe(struct platform_device *pdev)
 		}
 	}
 
-	platform_set_drvdata(pdev, priv);
-
 	return 0;
 err:
 	while (--i >= 0)
@@ -448,13 +464,11 @@ err:
 static int mc13783_regulator_remove(struct platform_device *pdev)
 {
 	struct mc13xxx_regulator_priv *priv = platform_get_drvdata(pdev);
-	struct mc13xxx_regulator_platform_data *pdata =
-		dev_get_platdata(&pdev->dev);
 	int i;
 
 	platform_set_drvdata(pdev, NULL);
 
-	for (i = 0; i < pdata->num_regulators; i++)
+	for (i = 0; i < priv->num_regulators; i++)
 		regulator_unregister(priv->regulators[i]);
 
 	return 0;
diff --git a/drivers/regulator/mc13892-regulator.c b/drivers/regulator/mc13892-regulator.c
index 9891aec47b5..b716283a876 100644
--- a/drivers/regulator/mc13892-regulator.c
+++ b/drivers/regulator/mc13892-regulator.c
@@ -465,13 +465,13 @@ static int mc13892_sw_regulator_set_voltage_sel(struct regulator_dev *rdev,
 	 */
 
 	if (mc13892_regulators[id].vsel_reg != MC13892_SWITCHERS0) {
+		mask |= MC13892_SWITCHERS0_SWxHI;
+
 		if (volt > 1375000) {
 			reg_value -= MC13892_SWxHI_SEL_OFFSET;
 			reg_value |= MC13892_SWITCHERS0_SWxHI;
-			mask |= MC13892_SWITCHERS0_SWxHI;
-		} else if (volt < 1100000) {
+		} else {
 			reg_value &= ~MC13892_SWITCHERS0_SWxHI;
-			mask |= MC13892_SWITCHERS0_SWxHI;
 		}
 	}
 
@@ -485,6 +485,7 @@ static int mc13892_sw_regulator_set_voltage_sel(struct regulator_dev *rdev,
 
 static struct regulator_ops mc13892_sw_regulator_ops = {
 	.list_voltage = regulator_list_voltage_table,
+	.map_voltage = regulator_map_voltage_ascend,
 	.set_voltage_sel = mc13892_sw_regulator_set_voltage_sel,
 	.get_voltage_sel = mc13892_sw_regulator_get_voltage_sel,
 };
@@ -535,7 +536,7 @@ static int mc13892_regulator_probe(struct platform_device *pdev)
 	struct mc13xxx_regulator_init_data *mc13xxx_data;
 	struct regulator_config config = { };
 	int i, ret;
-	int num_regulators = 0, num_parsed;
+	int num_regulators = 0;
 	u32 val;
 
 	num_regulators = mc13xxx_get_num_regulators_dt(pdev);
@@ -545,8 +546,6 @@ static int mc13892_regulator_probe(struct platform_device *pdev)
 	if (num_regulators <= 0)
 		return -EINVAL;
 
-	num_parsed = num_regulators;
-
 	priv = devm_kzalloc(&pdev->dev, sizeof(*priv) +
 		num_regulators * sizeof(priv->regulators[0]),
 		GFP_KERNEL);
@@ -589,40 +588,9 @@ static int mc13892_regulator_probe(struct platform_device *pdev)
 		= mc13892_vcam_get_mode;
 
 	mc13xxx_data = mc13xxx_parse_regulators_dt(pdev, mc13892_regulators,
-					ARRAY_SIZE(mc13892_regulators),
-					&num_parsed);
-
-	/*
-	 * Perform a little sanity check on the regulator tree - if we found
-	 * a number of regulators from mc13xxx_get_num_regulators_dt and
-	 * then parsed a smaller number in mc13xxx_parse_regulators_dt then
-	 * there is a regulator defined in the regulators node which has
-	 * not matched any usable regulator in the driver. In this case,
-	 * there is one missing and what will happen is the first regulator
-	 * will get registered again.
-	 *
-	 * Fix this by basically making our number of registerable regulators
-	 * equal to the number of regulators we parsed. We are allocating
-	 * too much memory for priv, but this is unavoidable at this point.
-	 *
-	 * As an example of how this can happen, try making a typo in your
-	 * regulators node (vviohi {} instead of viohi {}) so that the name
-	 * does not match..
-	 *
-	 * The check will basically pass for platform data (non-DT) because
-	 * mc13xxx_parse_regulators_dt for !CONFIG_OF will not touch num_parsed.
-	 *
-	 */
-	if (num_parsed != num_regulators) {
-		dev_warn(&pdev->dev,
-		"parsed %d != regulators %d - check your device tree!\n",
-			num_parsed, num_regulators);
-
-		num_regulators = num_parsed;
-		priv->num_regulators = num_regulators;
-	}
+					ARRAY_SIZE(mc13892_regulators));
 
-	for (i = 0; i < num_regulators; i++) {
+	for (i = 0; i < priv->num_regulators; i++) {
 		struct regulator_init_data *init_data;
 		struct regulator_desc *desc;
 		struct device_node *node = NULL;
diff --git a/drivers/regulator/mc13xxx-regulator-core.c b/drivers/regulator/mc13xxx-regulator-core.c
index 23cf9f9c383..da485928230 100644
--- a/drivers/regulator/mc13xxx-regulator-core.c
+++ b/drivers/regulator/mc13xxx-regulator-core.c
@@ -180,15 +180,13 @@ EXPORT_SYMBOL_GPL(mc13xxx_get_num_regulators_dt);
 
 struct mc13xxx_regulator_init_data *mc13xxx_parse_regulators_dt(
 	struct platform_device *pdev, struct mc13xxx_regulator *regulators,
-	int num_regulators, int *num_parsed)
+	int num_regulators)
 {
 	struct mc13xxx_regulator_priv *priv = platform_get_drvdata(pdev);
 	struct mc13xxx_regulator_init_data *data, *p;
 	struct device_node *parent, *child;
 	int i, parsed = 0;
 
-	*num_parsed = 0;
-
 	of_node_get(pdev->dev.parent->of_node);
 	parent = of_find_node_by_name(pdev->dev.parent->of_node, "regulators");
 	if (!parent)
@@ -204,10 +202,13 @@ struct mc13xxx_regulator_init_data *mc13xxx_parse_regulators_dt(
 	p = data;
 
 	for_each_child_of_node(parent, child) {
+		int found = 0;
+
 		for (i = 0; i < num_regulators; i++) {
+			if (!regulators[i].desc.name)
+				continue;
 			if (!of_node_cmp(child->name,
 					 regulators[i].desc.name)) {
-
 				p->id = i;
 				p->init_data = of_get_regulator_init_data(
 							&pdev->dev, child);
@@ -215,13 +216,19 @@ struct mc13xxx_regulator_init_data *mc13xxx_parse_regulators_dt(
 				p++;
 
 				parsed++;
+				found = 1;
 				break;
 			}
 		}
+
+		if (!found)
+			dev_warn(&pdev->dev,
+				 "Unknown regulator: %s\n", child->name);
 	}
 	of_node_put(parent);
 
-	*num_parsed = parsed;
+	priv->num_regulators = parsed;
+
 	return data;
 }
 EXPORT_SYMBOL_GPL(mc13xxx_parse_regulators_dt);
diff --git a/drivers/regulator/mc13xxx.h b/drivers/regulator/mc13xxx.h
index 007f83387fd..06c8903f182 100644
--- a/drivers/regulator/mc13xxx.h
+++ b/drivers/regulator/mc13xxx.h
@@ -39,7 +39,7 @@ extern int mc13xxx_fixed_regulator_set_voltage(struct regulator_dev *rdev,
 extern int mc13xxx_get_num_regulators_dt(struct platform_device *pdev);
 extern struct mc13xxx_regulator_init_data *mc13xxx_parse_regulators_dt(
 	struct platform_device *pdev, struct mc13xxx_regulator *regulators,
-	int num_regulators, int *num_parsed);
+	int num_regulators);
 #else
 static inline int mc13xxx_get_num_regulators_dt(struct platform_device *pdev)
 {
@@ -48,7 +48,7 @@ static inline int mc13xxx_get_num_regulators_dt(struct platform_device *pdev)
 
 static inline struct mc13xxx_regulator_init_data *mc13xxx_parse_regulators_dt(
 	struct platform_device *pdev, struct mc13xxx_regulator *regulators,
-	int num_regulators, int *num_parsed)
+	int num_regulators)
 {
 	return NULL;
 }
diff --git a/drivers/regulator/palmas-regulator.c b/drivers/regulator/palmas-regulator.c
index 39cf1460678..92ceed0fc65 100644
--- a/drivers/regulator/palmas-regulator.c
+++ b/drivers/regulator/palmas-regulator.c
@@ -1,7 +1,7 @@
 /*
  * Driver for Regulator part of Palmas PMIC Chips
  *
- * Copyright 2011-2012 Texas Instruments Inc.
+ * Copyright 2011-2013 Texas Instruments Inc.
  *
  * Author: Graeme Gregory <gg@slimlogic.co.uk>
  * Author: Ian Lartey <ian@slimlogic.co.uk>
@@ -29,6 +29,7 @@
 
 struct regs_info {
 	char	*name;
+	char	*sname;
 	u8	vsel_addr;
 	u8	ctrl_addr;
 	u8	tstep_addr;
@@ -37,115 +38,159 @@ struct regs_info {
 static const struct regs_info palmas_regs_info[] = {
 	{
 		.name		= "SMPS12",
+		.sname		= "smps1-in",
 		.vsel_addr	= PALMAS_SMPS12_VOLTAGE,
 		.ctrl_addr	= PALMAS_SMPS12_CTRL,
 		.tstep_addr	= PALMAS_SMPS12_TSTEP,
 	},
 	{
 		.name		= "SMPS123",
+		.sname		= "smps1-in",
 		.vsel_addr	= PALMAS_SMPS12_VOLTAGE,
 		.ctrl_addr	= PALMAS_SMPS12_CTRL,
 		.tstep_addr	= PALMAS_SMPS12_TSTEP,
 	},
 	{
 		.name		= "SMPS3",
+		.sname		= "smps3-in",
 		.vsel_addr	= PALMAS_SMPS3_VOLTAGE,
 		.ctrl_addr	= PALMAS_SMPS3_CTRL,
 	},
 	{
 		.name		= "SMPS45",
+		.sname		= "smps4-in",
 		.vsel_addr	= PALMAS_SMPS45_VOLTAGE,
 		.ctrl_addr	= PALMAS_SMPS45_CTRL,
 		.tstep_addr	= PALMAS_SMPS45_TSTEP,
 	},
 	{
 		.name		= "SMPS457",
+		.sname		= "smps4-in",
 		.vsel_addr	= PALMAS_SMPS45_VOLTAGE,
 		.ctrl_addr	= PALMAS_SMPS45_CTRL,
 		.tstep_addr	= PALMAS_SMPS45_TSTEP,
 	},
 	{
 		.name		= "SMPS6",
+		.sname		= "smps6-in",
 		.vsel_addr	= PALMAS_SMPS6_VOLTAGE,
 		.ctrl_addr	= PALMAS_SMPS6_CTRL,
 		.tstep_addr	= PALMAS_SMPS6_TSTEP,
 	},
 	{
 		.name		= "SMPS7",
+		.sname		= "smps7-in",
 		.vsel_addr	= PALMAS_SMPS7_VOLTAGE,
 		.ctrl_addr	= PALMAS_SMPS7_CTRL,
 	},
 	{
 		.name		= "SMPS8",
+		.sname		= "smps8-in",
 		.vsel_addr	= PALMAS_SMPS8_VOLTAGE,
 		.ctrl_addr	= PALMAS_SMPS8_CTRL,
 		.tstep_addr	= PALMAS_SMPS8_TSTEP,
 	},
 	{
 		.name		= "SMPS9",
+		.sname		= "smps9-in",
 		.vsel_addr	= PALMAS_SMPS9_VOLTAGE,
 		.ctrl_addr	= PALMAS_SMPS9_CTRL,
 	},
 	{
 		.name		= "SMPS10",
+		.sname		= "smps10-in",
+		.ctrl_addr	= PALMAS_SMPS10_CTRL,
 	},
 	{
 		.name		= "LDO1",
+		.sname		= "ldo1-in",
 		.vsel_addr	= PALMAS_LDO1_VOLTAGE,
 		.ctrl_addr	= PALMAS_LDO1_CTRL,
 	},
 	{
 		.name		= "LDO2",
+		.sname		= "ldo2-in",
 		.vsel_addr	= PALMAS_LDO2_VOLTAGE,
 		.ctrl_addr	= PALMAS_LDO2_CTRL,
 	},
 	{
 		.name		= "LDO3",
+		.sname		= "ldo3-in",
 		.vsel_addr	= PALMAS_LDO3_VOLTAGE,
 		.ctrl_addr	= PALMAS_LDO3_CTRL,
 	},
 	{
 		.name		= "LDO4",
+		.sname		= "ldo4-in",
 		.vsel_addr	= PALMAS_LDO4_VOLTAGE,
 		.ctrl_addr	= PALMAS_LDO4_CTRL,
 	},
 	{
 		.name		= "LDO5",
+		.sname		= "ldo5-in",
 		.vsel_addr	= PALMAS_LDO5_VOLTAGE,
 		.ctrl_addr	= PALMAS_LDO5_CTRL,
 	},
 	{
 		.name		= "LDO6",
+		.sname		= "ldo6-in",
 		.vsel_addr	= PALMAS_LDO6_VOLTAGE,
 		.ctrl_addr	= PALMAS_LDO6_CTRL,
 	},
 	{
 		.name		= "LDO7",
+		.sname		= "ldo7-in",
 		.vsel_addr	= PALMAS_LDO7_VOLTAGE,
 		.ctrl_addr	= PALMAS_LDO7_CTRL,
 	},
 	{
 		.name		= "LDO8",
+		.sname		= "ldo8-in",
 		.vsel_addr	= PALMAS_LDO8_VOLTAGE,
 		.ctrl_addr	= PALMAS_LDO8_CTRL,
 	},
 	{
 		.name		= "LDO9",
+		.sname		= "ldo9-in",
 		.vsel_addr	= PALMAS_LDO9_VOLTAGE,
 		.ctrl_addr	= PALMAS_LDO9_CTRL,
 	},
 	{
 		.name		= "LDOLN",
+		.sname		= "ldoln-in",
 		.vsel_addr	= PALMAS_LDOLN_VOLTAGE,
 		.ctrl_addr	= PALMAS_LDOLN_CTRL,
 	},
 	{
 		.name		= "LDOUSB",
+		.sname		= "ldousb-in",
 		.vsel_addr	= PALMAS_LDOUSB_VOLTAGE,
 		.ctrl_addr	= PALMAS_LDOUSB_CTRL,
 	},
+	{
+		.name		= "REGEN1",
+		.ctrl_addr	= PALMAS_REGEN1_CTRL,
+	},
+	{
+		.name		= "REGEN2",
+		.ctrl_addr	= PALMAS_REGEN2_CTRL,
+	},
+	{
+		.name		= "REGEN3",
+		.ctrl_addr	= PALMAS_REGEN3_CTRL,
+	},
+	{
+		.name		= "SYSEN1",
+		.ctrl_addr	= PALMAS_SYSEN1_CTRL,
+	},
+	{
+		.name		= "SYSEN2",
+		.ctrl_addr	= PALMAS_SYSEN2_CTRL,
+	},
 };
 
+static unsigned int palmas_smps_ramp_delay[4] = {0, 10000, 5000, 2500};
+
 #define SMPS_CTRL_MODE_OFF		0x00
 #define SMPS_CTRL_MODE_ON		0x01
 #define SMPS_CTRL_MODE_ECO		0x02
@@ -231,7 +276,10 @@ static int palmas_enable_smps(struct regulator_dev *dev)
 	palmas_smps_read(pmic->palmas, palmas_regs_info[id].ctrl_addr, &reg);
 
 	reg &= ~PALMAS_SMPS12_CTRL_MODE_ACTIVE_MASK;
-	reg |= SMPS_CTRL_MODE_ON;
+	if (pmic->current_reg_mode[id])
+		reg |= pmic->current_reg_mode[id];
+	else
+		reg |= SMPS_CTRL_MODE_ON;
 
 	palmas_smps_write(pmic->palmas, palmas_regs_info[id].ctrl_addr, reg);
 
@@ -253,16 +301,19 @@ static int palmas_disable_smps(struct regulator_dev *dev)
 	return 0;
 }
 
-
 static int palmas_set_mode_smps(struct regulator_dev *dev, unsigned int mode)
 {
 	struct palmas_pmic *pmic = rdev_get_drvdata(dev);
 	int id = rdev_get_id(dev);
 	unsigned int reg;
+	bool rail_enable = true;
 
 	palmas_smps_read(pmic->palmas, palmas_regs_info[id].ctrl_addr, &reg);
 	reg &= ~PALMAS_SMPS12_CTRL_MODE_ACTIVE_MASK;
 
+	if (reg == SMPS_CTRL_MODE_OFF)
+		rail_enable = false;
+
 	switch (mode) {
 	case REGULATOR_MODE_NORMAL:
 		reg |= SMPS_CTRL_MODE_ON;
@@ -276,8 +327,11 @@ static int palmas_set_mode_smps(struct regulator_dev *dev, unsigned int mode)
 	default:
 		return -EINVAL;
 	}
-	palmas_smps_write(pmic->palmas, palmas_regs_info[id].ctrl_addr, reg);
 
+	pmic->current_reg_mode[id] = reg & PALMAS_SMPS12_CTRL_MODE_ACTIVE_MASK;
+	if (rail_enable)
+		palmas_smps_write(pmic->palmas,
+			palmas_regs_info[id].ctrl_addr, reg);
 	return 0;
 }
 
@@ -287,9 +341,7 @@ static unsigned int palmas_get_mode_smps(struct regulator_dev *dev)
 	int id = rdev_get_id(dev);
 	unsigned int reg;
 
-	palmas_smps_read(pmic->palmas, palmas_regs_info[id].ctrl_addr, &reg);
-	reg &= PALMAS_SMPS12_CTRL_STATUS_MASK;
-	reg >>= PALMAS_SMPS12_CTRL_STATUS_SHIFT;
+	reg = pmic->current_reg_mode[id] & PALMAS_SMPS12_CTRL_MODE_ACTIVE_MASK;
 
 	switch (reg) {
 	case SMPS_CTRL_MODE_ON:
@@ -356,6 +408,63 @@ static int palmas_map_voltage_smps(struct regulator_dev *rdev,
 	return ret;
 }
 
+static int palma_smps_set_voltage_smps_time_sel(struct regulator_dev *rdev,
+	unsigned int old_selector, unsigned int new_selector)
+{
+	struct palmas_pmic *pmic = rdev_get_drvdata(rdev);
+	int id = rdev_get_id(rdev);
+	int old_uv, new_uv;
+	unsigned int ramp_delay = pmic->ramp_delay[id];
+
+	if (!ramp_delay)
+		return 0;
+
+	old_uv = palmas_list_voltage_smps(rdev, old_selector);
+	if (old_uv < 0)
+		return old_uv;
+
+	new_uv = palmas_list_voltage_smps(rdev, new_selector);
+	if (new_uv < 0)
+		return new_uv;
+
+	return DIV_ROUND_UP(abs(old_uv - new_uv), ramp_delay);
+}
+
+static int palmas_smps_set_ramp_delay(struct regulator_dev *rdev,
+		 int ramp_delay)
+{
+	struct palmas_pmic *pmic = rdev_get_drvdata(rdev);
+	int id = rdev_get_id(rdev);
+	unsigned int reg = 0;
+	unsigned int addr = palmas_regs_info[id].tstep_addr;
+	int ret;
+
+	/* SMPS3 and SMPS7 do not have tstep_addr setting */
+	switch (id) {
+	case PALMAS_REG_SMPS3:
+	case PALMAS_REG_SMPS7:
+		return 0;
+	}
+
+	if (ramp_delay <= 0)
+		reg = 0;
+	else if (ramp_delay <= 2500)
+		reg = 3;
+	else if (ramp_delay <= 5000)
+		reg = 2;
+	else
+		reg = 1;
+
+	ret = palmas_smps_write(pmic->palmas, addr, reg);
+	if (ret < 0) {
+		dev_err(pmic->palmas->dev, "TSTEP write failed: %d\n", ret);
+		return ret;
+	}
+
+	pmic->ramp_delay[id] = palmas_smps_ramp_delay[reg];
+	return ret;
+}
+
 static struct regulator_ops palmas_ops_smps = {
 	.is_enabled		= palmas_is_enabled_smps,
 	.enable			= palmas_enable_smps,
@@ -366,6 +475,8 @@ static struct regulator_ops palmas_ops_smps = {
 	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
 	.list_voltage		= palmas_list_voltage_smps,
 	.map_voltage		= palmas_map_voltage_smps,
+	.set_voltage_time_sel	= palma_smps_set_voltage_smps_time_sel,
+	.set_ramp_delay		= palmas_smps_set_ramp_delay,
 };
 
 static struct regulator_ops palmas_ops_smps10 = {
@@ -401,6 +512,12 @@ static struct regulator_ops palmas_ops_ldo = {
 	.map_voltage		= regulator_map_voltage_linear,
 };
 
+static struct regulator_ops palmas_ops_extreg = {
+	.is_enabled		= regulator_is_enabled_regmap,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+};
+
 /*
  * setup the hardware based sleep configuration of the SMPS/LDO regulators
  * from the platform data. This is different to the software based control
@@ -422,40 +539,32 @@ static int palmas_smps_init(struct palmas *palmas, int id,
 
 	switch (id) {
 	case PALMAS_REG_SMPS10:
-		if (reg_init->mode_sleep) {
-			reg &= ~PALMAS_SMPS10_CTRL_MODE_SLEEP_MASK;
+		reg &= ~PALMAS_SMPS10_CTRL_MODE_SLEEP_MASK;
+		if (reg_init->mode_sleep)
 			reg |= reg_init->mode_sleep <<
 					PALMAS_SMPS10_CTRL_MODE_SLEEP_SHIFT;
-		}
 		break;
 	default:
 		if (reg_init->warm_reset)
 			reg |= PALMAS_SMPS12_CTRL_WR_S;
+		else
+			reg &= ~PALMAS_SMPS12_CTRL_WR_S;
 
 		if (reg_init->roof_floor)
 			reg |= PALMAS_SMPS12_CTRL_ROOF_FLOOR_EN;
+		else
+			reg &= ~PALMAS_SMPS12_CTRL_ROOF_FLOOR_EN;
 
-		if (reg_init->mode_sleep) {
-			reg &= ~PALMAS_SMPS12_CTRL_MODE_SLEEP_MASK;
+		reg &= ~PALMAS_SMPS12_CTRL_MODE_SLEEP_MASK;
+		if (reg_init->mode_sleep)
 			reg |= reg_init->mode_sleep <<
 					PALMAS_SMPS12_CTRL_MODE_SLEEP_SHIFT;
-		}
 	}
 
 	ret = palmas_smps_write(palmas, addr, reg);
 	if (ret)
 		return ret;
 
-	if (palmas_regs_info[id].tstep_addr && reg_init->tstep) {
-		addr = palmas_regs_info[id].tstep_addr;
-
-		reg = reg_init->tstep & PALMAS_SMPS12_TSTEP_TSTEP_MASK;
-
-		ret = palmas_smps_write(palmas, addr, reg);
-		if (ret)
-			return ret;
-	}
-
 	if (palmas_regs_info[id].vsel_addr && reg_init->vsel) {
 		addr = palmas_regs_info[id].vsel_addr;
 
@@ -485,9 +594,13 @@ static int palmas_ldo_init(struct palmas *palmas, int id,
 
 	if (reg_init->warm_reset)
 		reg |= PALMAS_LDO1_CTRL_WR_S;
+	else
+		reg &= ~PALMAS_LDO1_CTRL_WR_S;
 
 	if (reg_init->mode_sleep)
 		reg |= PALMAS_LDO1_CTRL_MODE_SLEEP;
+	else
+		reg &= ~PALMAS_LDO1_CTRL_MODE_SLEEP;
 
 	ret = palmas_ldo_write(palmas, addr, reg);
 	if (ret)
@@ -496,6 +609,68 @@ static int palmas_ldo_init(struct palmas *palmas, int id,
 	return 0;
 }
 
+static int palmas_extreg_init(struct palmas *palmas, int id,
+		struct palmas_reg_init *reg_init)
+{
+	unsigned int addr;
+	int ret;
+	unsigned int val = 0;
+
+	addr = palmas_regs_info[id].ctrl_addr;
+
+	if (reg_init->mode_sleep)
+		val = PALMAS_REGEN1_CTRL_MODE_SLEEP;
+
+	ret = palmas_update_bits(palmas, PALMAS_RESOURCE_BASE,
+			addr, PALMAS_REGEN1_CTRL_MODE_SLEEP, val);
+	if (ret < 0) {
+		dev_err(palmas->dev, "Resource reg 0x%02x update failed %d\n",
+			addr, ret);
+		return ret;
+	}
+	return 0;
+}
+
+static void palmas_enable_ldo8_track(struct palmas *palmas)
+{
+	unsigned int reg;
+	unsigned int addr;
+	int ret;
+
+	addr = palmas_regs_info[PALMAS_REG_LDO8].ctrl_addr;
+
+	ret = palmas_ldo_read(palmas, addr, &reg);
+	if (ret) {
+		dev_err(palmas->dev, "Error in reading ldo8 control reg\n");
+		return;
+	}
+
+	reg |= PALMAS_LDO8_CTRL_LDO_TRACKING_EN;
+	ret = palmas_ldo_write(palmas, addr, reg);
+	if (ret < 0) {
+		dev_err(palmas->dev, "Error in enabling tracking mode\n");
+		return;
+	}
+	/*
+	 * When SMPS45 is set to off and LDO8 tracking is enabled, the LDO8
+	 * output is defined by the LDO8_VOLTAGE.VSEL register divided by two,
+	 * and can be set from 0.45 to 1.65 V.
+	 */
+	addr = palmas_regs_info[PALMAS_REG_LDO8].vsel_addr;
+	ret = palmas_ldo_read(palmas, addr, &reg);
+	if (ret) {
+		dev_err(palmas->dev, "Error in reading ldo8 voltage reg\n");
+		return;
+	}
+
+	reg = (reg << 1) & PALMAS_LDO8_VOLTAGE_VSEL_MASK;
+	ret = palmas_ldo_write(palmas, addr, reg);
+	if (ret < 0)
+		dev_err(palmas->dev, "Error in setting ldo8 voltage reg\n");
+
+	return;
+}
+
 static struct of_regulator_match palmas_matches[] = {
 	{ .name = "smps12", },
 	{ .name = "smps123", },
@@ -518,6 +693,11 @@ static struct of_regulator_match palmas_matches[] = {
 	{ .name = "ldo9", },
 	{ .name = "ldoln", },
 	{ .name = "ldousb", },
+	{ .name = "regen1", },
+	{ .name = "regen2", },
+	{ .name = "regen3", },
+	{ .name = "sysen1", },
+	{ .name = "sysen2", },
 };
 
 static void palmas_dt_to_pdata(struct device *dev,
@@ -553,39 +733,36 @@ static void palmas_dt_to_pdata(struct device *dev,
 		pdata->reg_init[idx] = devm_kzalloc(dev,
 				sizeof(struct palmas_reg_init), GFP_KERNEL);
 
-		ret = of_property_read_u32(palmas_matches[idx].of_node,
-				"ti,warm_reset", &prop);
-		if (!ret)
-			pdata->reg_init[idx]->warm_reset = prop;
+		pdata->reg_init[idx]->warm_reset =
+			of_property_read_bool(palmas_matches[idx].of_node,
+					     "ti,warm-reset");
 
-		ret = of_property_read_u32(palmas_matches[idx].of_node,
-				"ti,roof_floor", &prop);
-		if (!ret)
-			pdata->reg_init[idx]->roof_floor = prop;
+		pdata->reg_init[idx]->roof_floor =
+			of_property_read_bool(palmas_matches[idx].of_node,
+					      "ti,roof-floor");
 
 		ret = of_property_read_u32(palmas_matches[idx].of_node,
-				"ti,mode_sleep", &prop);
+				"ti,mode-sleep", &prop);
 		if (!ret)
 			pdata->reg_init[idx]->mode_sleep = prop;
 
-		ret = of_property_read_u32(palmas_matches[idx].of_node,
-				"ti,tstep", &prop);
-		if (!ret)
-			pdata->reg_init[idx]->tstep = prop;
+		ret = of_property_read_bool(palmas_matches[idx].of_node,
+					    "ti,smps-range");
+		if (ret)
+			pdata->reg_init[idx]->vsel =
+				PALMAS_SMPS12_VOLTAGE_RANGE;
 
-		ret = of_property_read_u32(palmas_matches[idx].of_node,
-				"ti,vsel", &prop);
-		if (!ret)
-			pdata->reg_init[idx]->vsel = prop;
+		if (idx == PALMAS_REG_LDO8)
+			pdata->enable_ldo8_tracking = of_property_read_bool(
+						palmas_matches[idx].of_node,
+						"ti,enable-ldo8-tracking");
 	}
 
-	ret = of_property_read_u32(node, "ti,ldo6_vibrator", &prop);
-	if (!ret)
-		pdata->ldo6_vibrator = prop;
+	pdata->ldo6_vibrator = of_property_read_bool(node, "ti,ldo6-vibrator");
 }
 
 
-static int palmas_probe(struct platform_device *pdev)
+static int palmas_regulators_probe(struct platform_device *pdev)
 {
 	struct palmas *palmas = dev_get_drvdata(pdev->dev.parent);
 	struct palmas_pmic_platform_data *pdata = pdev->dev.platform_data;
@@ -630,6 +807,7 @@ static int palmas_probe(struct platform_device *pdev)
 	config.driver_data = pmic;
 
 	for (id = 0; id < PALMAS_REG_LDO1; id++) {
+		bool ramp_delay_support = false;
 
 		/*
 		 * Miss out regulators which are not available due
@@ -640,19 +818,42 @@ static int palmas_probe(struct platform_device *pdev)
 		case PALMAS_REG_SMPS3:
 			if (pmic->smps123)
 				continue;
+			if (id == PALMAS_REG_SMPS12)
+				ramp_delay_support = true;
 			break;
 		case PALMAS_REG_SMPS123:
 			if (!pmic->smps123)
 				continue;
+			ramp_delay_support = true;
 			break;
 		case PALMAS_REG_SMPS45:
 		case PALMAS_REG_SMPS7:
 			if (pmic->smps457)
 				continue;
+			if (id == PALMAS_REG_SMPS45)
+				ramp_delay_support = true;
 			break;
 		case PALMAS_REG_SMPS457:
 			if (!pmic->smps457)
 				continue;
+			ramp_delay_support = true;
+			break;
+		}
+
+		if ((id == PALMAS_REG_SMPS6) && (id == PALMAS_REG_SMPS8))
+			ramp_delay_support = true;
+
+		if (ramp_delay_support) {
+			addr = palmas_regs_info[id].tstep_addr;
+			ret = palmas_smps_read(pmic->palmas, addr, &reg);
+			if (ret < 0) {
+				dev_err(&pdev->dev,
+					"reading TSTEP reg failed: %d\n", ret);
+				goto err_unregister_regulator;
+			}
+			pmic->desc[id].ramp_delay =
+					palmas_smps_ramp_delay[reg & 0x3];
+			pmic->ramp_delay[id] = pmic->desc[id].ramp_delay;
 		}
 
 		/* Initialise sleep/init values from platform data */
@@ -686,7 +887,8 @@ static int palmas_probe(struct platform_device *pdev)
 			/*
 			 * Read and store the RANGE bit for later use
 			 * This must be done before regulator is probed,
-			 * otherwise we error in probe with unsupportable ranges.
+			 * otherwise we error in probe with unsupportable
+			 * ranges. Read the current smps mode for later use.
 			 */
 			addr = palmas_regs_info[id].vsel_addr;
 
@@ -703,6 +905,14 @@ static int palmas_probe(struct platform_device *pdev)
 						palmas_regs_info[id].vsel_addr);
 			pmic->desc[id].vsel_mask =
 					PALMAS_SMPS12_VOLTAGE_VSEL_MASK;
+
+			/* Read the smps mode for later use. */
+			addr = palmas_regs_info[id].ctrl_addr;
+			ret = palmas_smps_read(pmic->palmas, addr, &reg);
+			if (ret)
+				goto err_unregister_regulator;
+			pmic->current_reg_mode[id] = reg &
+					PALMAS_SMPS12_CTRL_MODE_ACTIVE_MASK;
 		}
 
 		pmic->desc[id].type = REGULATOR_VOLTAGE;
@@ -713,6 +923,7 @@ static int palmas_probe(struct platform_device *pdev)
 		else
 			config.init_data = NULL;
 
+		pmic->desc[id].supply_name = palmas_regs_info[id].sname;
 		config.of_node = palmas_matches[id].of_node;
 
 		rdev = regulator_register(&pmic->desc[id], &config);
@@ -738,27 +949,49 @@ static int palmas_probe(struct platform_device *pdev)
 		/* Register the regulators */
 		pmic->desc[id].name = palmas_regs_info[id].name;
 		pmic->desc[id].id = id;
-		pmic->desc[id].n_voltages = PALMAS_LDO_NUM_VOLTAGES;
-
-		pmic->desc[id].ops = &palmas_ops_ldo;
-
 		pmic->desc[id].type = REGULATOR_VOLTAGE;
 		pmic->desc[id].owner = THIS_MODULE;
-		pmic->desc[id].min_uV = 900000;
-		pmic->desc[id].uV_step = 50000;
-		pmic->desc[id].linear_min_sel = 1;
-		pmic->desc[id].vsel_reg = PALMAS_BASE_TO_REG(PALMAS_LDO_BASE,
+
+		if (id < PALMAS_REG_REGEN1) {
+			pmic->desc[id].n_voltages = PALMAS_LDO_NUM_VOLTAGES;
+			pmic->desc[id].ops = &palmas_ops_ldo;
+			pmic->desc[id].min_uV = 900000;
+			pmic->desc[id].uV_step = 50000;
+			pmic->desc[id].linear_min_sel = 1;
+			pmic->desc[id].vsel_reg =
+					PALMAS_BASE_TO_REG(PALMAS_LDO_BASE,
 						palmas_regs_info[id].vsel_addr);
-		pmic->desc[id].vsel_mask = PALMAS_LDO1_VOLTAGE_VSEL_MASK;
-		pmic->desc[id].enable_reg = PALMAS_BASE_TO_REG(PALMAS_LDO_BASE,
+			pmic->desc[id].vsel_mask =
+					PALMAS_LDO1_VOLTAGE_VSEL_MASK;
+			pmic->desc[id].enable_reg =
+					PALMAS_BASE_TO_REG(PALMAS_LDO_BASE,
+						palmas_regs_info[id].ctrl_addr);
+			pmic->desc[id].enable_mask =
+					PALMAS_LDO1_CTRL_MODE_ACTIVE;
+
+			/* Check if LDO8 is in tracking mode or not */
+			if (pdata && (id == PALMAS_REG_LDO8) &&
+					pdata->enable_ldo8_tracking) {
+				palmas_enable_ldo8_track(palmas);
+				pmic->desc[id].min_uV = 450000;
+				pmic->desc[id].uV_step = 25000;
+			}
+		} else {
+			pmic->desc[id].n_voltages = 1;
+			pmic->desc[id].ops = &palmas_ops_extreg;
+			pmic->desc[id].enable_reg =
+					PALMAS_BASE_TO_REG(PALMAS_RESOURCE_BASE,
 						palmas_regs_info[id].ctrl_addr);
-		pmic->desc[id].enable_mask = PALMAS_LDO1_CTRL_MODE_ACTIVE;
+			pmic->desc[id].enable_mask =
+					PALMAS_REGEN1_CTRL_MODE_ACTIVE;
+		}
 
 		if (pdata)
 			config.init_data = pdata->reg_data[id];
 		else
 			config.init_data = NULL;
 
+		pmic->desc[id].supply_name = palmas_regs_info[id].sname;
 		config.of_node = palmas_matches[id].of_node;
 
 		rdev = regulator_register(&pmic->desc[id], &config);
@@ -777,7 +1010,12 @@ static int palmas_probe(struct platform_device *pdev)
 		if (pdata) {
 			reg_init = pdata->reg_init[id];
 			if (reg_init) {
-				ret = palmas_ldo_init(palmas, id, reg_init);
+				if (id < PALMAS_REG_REGEN1)
+					ret = palmas_ldo_init(palmas,
+							id, reg_init);
+				else
+					ret = palmas_extreg_init(palmas,
+							id, reg_init);
 				if (ret) {
 					regulator_unregister(pmic->rdev[id]);
 					goto err_unregister_regulator;
@@ -786,6 +1024,7 @@ static int palmas_probe(struct platform_device *pdev)
 		}
 	}
 
+
 	return 0;
 
 err_unregister_regulator:
@@ -794,7 +1033,7 @@ err_unregister_regulator:
 	return ret;
 }
 
-static int palmas_remove(struct platform_device *pdev)
+static int palmas_regulators_remove(struct platform_device *pdev)
 {
 	struct palmas_pmic *pmic = platform_get_drvdata(pdev);
 	int id;
@@ -806,6 +1045,12 @@ static int palmas_remove(struct platform_device *pdev)
 
 static struct of_device_id of_palmas_match_tbl[] = {
 	{ .compatible = "ti,palmas-pmic", },
+	{ .compatible = "ti,twl6035-pmic", },
+	{ .compatible = "ti,twl6036-pmic", },
+	{ .compatible = "ti,twl6037-pmic", },
+	{ .compatible = "ti,tps65913-pmic", },
+	{ .compatible = "ti,tps65914-pmic", },
+	{ .compatible = "ti,tps80036-pmic", },
 	{ /* end */ }
 };
 
@@ -815,8 +1060,8 @@ static struct platform_driver palmas_driver = {
 		.of_match_table = of_palmas_match_tbl,
 		.owner = THIS_MODULE,
 	},
-	.probe = palmas_probe,
-	.remove = palmas_remove,
+	.probe = palmas_regulators_probe,
+	.remove = palmas_regulators_remove,
 };
 
 static int __init palmas_init(void)
diff --git a/drivers/regulator/rc5t583-regulator.c b/drivers/regulator/rc5t583-regulator.c
index 9e6f78694bf..5885b450459 100644
--- a/drivers/regulator/rc5t583-regulator.c
+++ b/drivers/regulator/rc5t583-regulator.c
@@ -49,10 +49,6 @@ struct rc5t583_regulator_info {
 
 struct rc5t583_regulator {
 	struct rc5t583_regulator_info *reg_info;
-
-	/* Devices */
-	struct device		*dev;
-	struct rc5t583		*mfd;
 	struct regulator_dev	*rdev;
 };
 
@@ -155,8 +151,6 @@ static int rc5t583_regulator_probe(struct platform_device *pdev)
 		reg = &regs[id];
 		ri = &rc5t583_reg_info[id];
 		reg->reg_info = ri;
-		reg->mfd = rc5t583;
-		reg->dev = &pdev->dev;
 
 		if (ri->deepsleep_id == RC5T583_DS_NONE)
 			goto skip_ext_pwr_config;
diff --git a/drivers/regulator/s5m8767.c b/drivers/regulator/s5m8767.c
index 8a831947c35..c24448bc43c 100644
--- a/drivers/regulator/s5m8767.c
+++ b/drivers/regulator/s5m8767.c
@@ -549,7 +549,7 @@ static int s5m8767_pmic_dt_parse_pdata(struct platform_device *pdev,
 
 	rmode = devm_kzalloc(&pdev->dev, sizeof(*rmode) *
 				pdata->num_regulators, GFP_KERNEL);
-	if (!rdata) {
+	if (!rmode) {
 		dev_err(iodev->dev,
 			"could not allocate memory for regulator mode\n");
 		return -ENOMEM;
@@ -923,8 +923,7 @@ static int s5m8767_pmic_probe(struct platform_device *pdev)
 	return 0;
 err:
 	for (i = 0; i < s5m8767->num_regulators; i++)
-		if (rdev[i])
-			regulator_unregister(rdev[i]);
+		regulator_unregister(rdev[i]);
 
 	return ret;
 }
@@ -936,8 +935,7 @@ static int s5m8767_pmic_remove(struct platform_device *pdev)
 	int i;
 
 	for (i = 0; i < s5m8767->num_regulators; i++)
-		if (rdev[i])
-			regulator_unregister(rdev[i]);
+		regulator_unregister(rdev[i]);
 
 	return 0;
 }
diff --git a/drivers/regulator/tps62360-regulator.c b/drivers/regulator/tps62360-regulator.c
index acbd63fde41..612919c3081 100644
--- a/drivers/regulator/tps62360-regulator.c
+++ b/drivers/regulator/tps62360-regulator.c
@@ -278,7 +278,7 @@ static int tps62360_init_dcdc(struct tps62360_chip *tps,
 			__func__, REG_RAMPCTRL, ret);
 		return ret;
 	}
-	ramp_ctrl = (ramp_ctrl >> 4) & 0x7;
+	ramp_ctrl = (ramp_ctrl >> 5) & 0x7;
 
 	/* ramp mV/us = 32/(2^ramp_ctrl) */
 	tps->desc.ramp_delay = DIV_ROUND_UP(32000, BIT(ramp_ctrl));
diff --git a/drivers/regulator/tps65023-regulator.c b/drivers/regulator/tps65023-regulator.c
index 9b9af6d889c..9d053e23e9e 100644
--- a/drivers/regulator/tps65023-regulator.c
+++ b/drivers/regulator/tps65023-regulator.c
@@ -107,12 +107,7 @@ static const unsigned int DCDC_FIXED_1800000_VSEL_table[] = {
 };
 
 /* Supported voltage values for LDO regulators for tps65020 */
-static const unsigned int TPS65020_LDO1_VSEL_table[] = {
-	1000000, 1050000, 1100000, 1300000,
-	1800000, 2500000, 3000000, 3300000,
-};
-
-static const unsigned int TPS65020_LDO2_VSEL_table[] = {
+static const unsigned int TPS65020_LDO_VSEL_table[] = {
 	1000000, 1050000, 1100000, 1300000,
 	1800000, 2500000, 3000000, 3300000,
 };
@@ -154,20 +149,15 @@ struct tps_driver_data {
 static int tps65023_dcdc_get_voltage_sel(struct regulator_dev *dev)
 {
 	struct tps_pmic *tps = rdev_get_drvdata(dev);
-	int ret;
-	int data, dcdc = rdev_get_id(dev);
+	int dcdc = rdev_get_id(dev);
 
 	if (dcdc < TPS65023_DCDC_1 || dcdc > TPS65023_DCDC_3)
 		return -EINVAL;
 
-	if (dcdc == tps->core_regulator) {
-		ret = regmap_read(tps->regmap, TPS65023_REG_DEF_CORE, &data);
-		if (ret != 0)
-			return ret;
-		data &= (tps->info[dcdc]->table_len - 1);
-		return data;
-	} else
+	if (dcdc != tps->core_regulator)
 		return 0;
+
+	return regulator_get_voltage_sel_regmap(dev);
 }
 
 static int tps65023_dcdc_set_voltage_sel(struct regulator_dev *dev,
@@ -175,23 +165,11 @@ static int tps65023_dcdc_set_voltage_sel(struct regulator_dev *dev,
 {
 	struct tps_pmic *tps = rdev_get_drvdata(dev);
 	int dcdc = rdev_get_id(dev);
-	int ret;
 
 	if (dcdc != tps->core_regulator)
 		return -EINVAL;
 
-	ret = regmap_write(tps->regmap, TPS65023_REG_DEF_CORE, selector);
-	if (ret)
-		goto out;
-
-	/* Tell the chip that we have changed the value in DEFCORE
-	 * and its time to update the core voltage
-	 */
-	ret = regmap_update_bits(tps->regmap, TPS65023_REG_CON_CTRL2,
-				 TPS65023_REG_CTRL2_GO, TPS65023_REG_CTRL2_GO);
-
-out:
-	return ret;
+	return regulator_set_voltage_sel_regmap(dev, selector);
 }
 
 /* Operations permitted on VDCDCx */
@@ -202,6 +180,7 @@ static struct regulator_ops tps65023_dcdc_ops = {
 	.get_voltage_sel = tps65023_dcdc_get_voltage_sel,
 	.set_voltage_sel = tps65023_dcdc_set_voltage_sel,
 	.list_voltage = regulator_list_voltage_table,
+	.map_voltage = regulator_map_voltage_ascend,
 };
 
 /* Operations permitted on LDOx */
@@ -212,6 +191,7 @@ static struct regulator_ops tps65023_ldo_ops = {
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
 	.set_voltage_sel = regulator_set_voltage_sel_regmap,
 	.list_voltage = regulator_list_voltage_table,
+	.map_voltage = regulator_map_voltage_ascend,
 };
 
 static struct regmap_config tps65023_regmap_config = {
@@ -285,6 +265,10 @@ static int tps_65023_probe(struct i2c_client *client,
 		default: /* DCDCx */
 			tps->desc[i].enable_mask =
 					1 << (TPS65023_NUM_REGULATOR - i);
+			tps->desc[i].vsel_reg = TPS65023_REG_DEF_CORE;
+			tps->desc[i].vsel_mask = info->table_len - 1;
+			tps->desc[i].apply_reg = TPS65023_REG_CON_CTRL2;
+			tps->desc[i].apply_bit = TPS65023_REG_CTRL2_GO;
 		}
 
 		config.dev = &client->dev;
@@ -347,13 +331,13 @@ static const struct tps_info tps65020_regs[] = {
 	},
 	{
 		.name = "LDO1",
-		.table_len = ARRAY_SIZE(TPS65020_LDO1_VSEL_table),
-		.table = TPS65020_LDO1_VSEL_table,
+		.table_len = ARRAY_SIZE(TPS65020_LDO_VSEL_table),
+		.table = TPS65020_LDO_VSEL_table,
 	},
 	{
 		.name = "LDO2",
-		.table_len = ARRAY_SIZE(TPS65020_LDO2_VSEL_table),
-		.table = TPS65020_LDO2_VSEL_table,
+		.table_len = ARRAY_SIZE(TPS65020_LDO_VSEL_table),
+		.table = TPS65020_LDO_VSEL_table,
 	},
 };
 
diff --git a/drivers/regulator/tps6507x-regulator.c b/drivers/regulator/tps6507x-regulator.c
index 54aa2da7283..4117ff52dba 100644
--- a/drivers/regulator/tps6507x-regulator.c
+++ b/drivers/regulator/tps6507x-regulator.c
@@ -356,6 +356,7 @@ static struct regulator_ops tps6507x_pmic_ops = {
 	.get_voltage_sel = tps6507x_pmic_get_voltage_sel,
 	.set_voltage_sel = tps6507x_pmic_set_voltage_sel,
 	.list_voltage = regulator_list_voltage_table,
+	.map_voltage = regulator_map_voltage_ascend,
 };
 
 #ifdef CONFIG_OF
diff --git a/drivers/regulator/tps6524x-regulator.c b/drivers/regulator/tps6524x-regulator.c
index 843ee0a9bb9..1094393155e 100644
--- a/drivers/regulator/tps6524x-regulator.c
+++ b/drivers/regulator/tps6524x-regulator.c
@@ -572,6 +572,7 @@ static struct regulator_ops regulator_ops = {
 	.get_voltage_sel	= get_voltage_sel,
 	.set_voltage_sel	= set_voltage_sel,
 	.list_voltage		= regulator_list_voltage_table,
+	.map_voltage		= regulator_map_voltage_ascend,
 	.set_current_limit	= set_current_limit,
 	.get_current_limit	= get_current_limit,
 };
@@ -584,8 +585,7 @@ static int pmic_remove(struct spi_device *spi)
 	if (!hw)
 		return 0;
 	for (i = 0; i < N_REGULATORS; i++) {
-		if (hw->rdev[i])
-			regulator_unregister(hw->rdev[i]);
+		regulator_unregister(hw->rdev[i]);
 		hw->rdev[i] = NULL;
 	}
 	spi_set_drvdata(spi, NULL);
diff --git a/drivers/regulator/tps6586x-regulator.c b/drivers/regulator/tps6586x-regulator.c
index e68382d0e1e..d8fa37d5c73 100644
--- a/drivers/regulator/tps6586x-regulator.c
+++ b/drivers/regulator/tps6586x-regulator.c
@@ -70,6 +70,7 @@ static inline struct device *to_tps6586x_dev(struct regulator_dev *rdev)
 
 static struct regulator_ops tps6586x_regulator_ops = {
 	.list_voltage = regulator_list_voltage_table,
+	.map_voltage = regulator_map_voltage_ascend,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
 	.set_voltage_sel = regulator_set_voltage_sel_regmap,
 
@@ -245,7 +246,7 @@ static int tps6586x_regulator_set_slew_rate(struct platform_device *pdev,
 		reg = TPS6586X_SM1SL;
 		break;
 	default:
-		dev_warn(&pdev->dev, "Only SM0/SM1 can set slew rate\n");
+		dev_err(&pdev->dev, "Only SM0/SM1 can set slew rate\n");
 		return -EINVAL;
 	}
 
@@ -304,14 +305,12 @@ static struct tps6586x_platform_data *tps6586x_parse_regulator_dt(
 	}
 
 	err = of_regulator_match(&pdev->dev, regs, tps6586x_matches, num);
+	of_node_put(regs);
 	if (err < 0) {
 		dev_err(&pdev->dev, "Regulator match failed, e %d\n", err);
-		of_node_put(regs);
 		return NULL;
 	}
 
-	of_node_put(regs);
-
 	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
 	if (!pdata) {
 		dev_err(&pdev->dev, "Memory alloction failed\n");
diff --git a/drivers/regulator/tps65910-regulator.c b/drivers/regulator/tps65910-regulator.c
index 6ba6931ac85..45c16447744 100644
--- a/drivers/regulator/tps65910-regulator.c
+++ b/drivers/regulator/tps65910-regulator.c
@@ -748,6 +748,7 @@ static struct regulator_ops tps65910_ops_dcdc = {
 	.set_voltage_sel	= tps65910_set_voltage_dcdc_sel,
 	.set_voltage_time_sel	= regulator_set_voltage_time_sel,
 	.list_voltage		= tps65910_list_voltage_dcdc,
+	.map_voltage		= regulator_map_voltage_ascend,
 };
 
 static struct regulator_ops tps65910_ops_vdd3 = {
@@ -758,6 +759,7 @@ static struct regulator_ops tps65910_ops_vdd3 = {
 	.get_mode		= tps65910_get_mode,
 	.get_voltage		= tps65910_get_voltage_vdd3,
 	.list_voltage		= regulator_list_voltage_table,
+	.map_voltage		= regulator_map_voltage_ascend,
 };
 
 static struct regulator_ops tps65910_ops = {
@@ -769,6 +771,7 @@ static struct regulator_ops tps65910_ops = {
 	.get_voltage_sel	= tps65910_get_voltage_sel,
 	.set_voltage_sel	= tps65910_set_voltage_sel,
 	.list_voltage		= regulator_list_voltage_table,
+	.map_voltage		= regulator_map_voltage_ascend,
 };
 
 static struct regulator_ops tps65911_ops = {
@@ -780,6 +783,7 @@ static struct regulator_ops tps65911_ops = {
 	.get_voltage_sel	= tps65911_get_voltage_sel,
 	.set_voltage_sel	= tps65911_set_voltage_sel,
 	.list_voltage		= tps65911_list_voltage,
+	.map_voltage		= regulator_map_voltage_ascend,
 };
 
 static int tps65910_set_ext_sleep_config(struct tps65910_reg *pmic,
diff --git a/drivers/regulator/tps80031-regulator.c b/drivers/regulator/tps80031-regulator.c
index 9019d0e7ecb..6511d0bfd89 100644
--- a/drivers/regulator/tps80031-regulator.c
+++ b/drivers/regulator/tps80031-regulator.c
@@ -238,12 +238,11 @@ static int tps80031_dcdc_get_voltage_sel(struct regulator_dev *rdev)
 	return vsel & SMPS_VSEL_MASK;
 }
 
-static int tps80031_ldo_set_voltage_sel(struct regulator_dev *rdev,
-		unsigned sel)
+static int tps80031_ldo_list_voltage(struct regulator_dev *rdev,
+				     unsigned int sel)
 {
 	struct tps80031_regulator *ri = rdev_get_drvdata(rdev);
 	struct device *parent = to_tps80031_dev(rdev);
-	int ret;
 
 	/* Check for valid setting for TPS80031 or TPS80032-ES1.0 */
 	if ((ri->rinfo->desc.id == TPS80031_REGULATOR_LDO2) &&
@@ -260,28 +259,27 @@ static int tps80031_ldo_set_voltage_sel(struct regulator_dev *rdev,
 		}
 	}
 
-	ret = tps80031_write(parent, ri->rinfo->volt_id,
-			ri->rinfo->volt_reg, sel);
-	if (ret < 0)
-		dev_err(ri->dev, "Error in writing reg 0x%02x, e = %d\n",
-			ri->rinfo->volt_reg, ret);
-	return ret;
+	return regulator_list_voltage_linear(rdev, sel);
 }
 
-static int tps80031_ldo_get_voltage_sel(struct regulator_dev *rdev)
+static int tps80031_ldo_map_voltage(struct regulator_dev *rdev,
+				    int min_uV, int max_uV)
 {
 	struct tps80031_regulator *ri = rdev_get_drvdata(rdev);
 	struct device *parent = to_tps80031_dev(rdev);
-	uint8_t vsel;
-	int ret;
 
-	ret = tps80031_read(parent, ri->rinfo->volt_id,
-				ri->rinfo->volt_reg, &vsel);
-	if (ret < 0) {
-		dev_err(ri->dev, "Error in writing the Voltage register\n");
-		return ret;
+	/* Check for valid setting for TPS80031 or TPS80032-ES1.0 */
+	if ((ri->rinfo->desc.id == TPS80031_REGULATOR_LDO2) &&
+			(ri->device_flags & TRACK_MODE_ENABLE)) {
+		if (((tps80031_get_chip_info(parent) == TPS80031) ||
+			((tps80031_get_chip_info(parent) == TPS80032) &&
+			(tps80031_get_pmu_version(parent) == 0x0)))) {
+			return regulator_map_voltage_iterate(rdev, min_uV,
+							     max_uV);
+		}
 	}
-	return vsel & rdev->desc->vsel_mask;
+
+	return regulator_map_voltage_linear(rdev, min_uV, max_uV);
 }
 
 static int tps80031_vbus_is_enabled(struct regulator_dev *rdev)
@@ -390,9 +388,10 @@ static struct regulator_ops tps80031_dcdc_ops = {
 };
 
 static struct regulator_ops tps80031_ldo_ops = {
-	.list_voltage		= regulator_list_voltage_linear,
-	.set_voltage_sel	= tps80031_ldo_set_voltage_sel,
-	.get_voltage_sel	= tps80031_ldo_get_voltage_sel,
+	.list_voltage		= tps80031_ldo_list_voltage,
+	.map_voltage		= tps80031_ldo_map_voltage,
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
 	.enable			= tps80031_reg_enable,
 	.disable		= tps80031_reg_disable,
 	.is_enabled		= tps80031_reg_is_enabled,
@@ -459,6 +458,7 @@ static struct regulator_ops tps80031_ext_reg_ops = {
 		.uV_step = 100000,				\
 		.linear_min_sel = 1,				\
 		.n_voltages = 25,				\
+		.vsel_reg = TPS80031_##_id##_CFG_VOLTAGE,	\
 		.vsel_mask = LDO_VSEL_MASK,			\
 		.enable_time = 500,				\
 	},							\
@@ -680,6 +680,7 @@ static int tps80031_regulator_probe(struct platform_device *pdev)
 	struct tps80031_regulator *pmic;
 	struct regulator_dev *rdev;
 	struct regulator_config config = { };
+	struct tps80031 *tps80031_mfd = dev_get_drvdata(pdev->dev.parent);
 	int ret;
 	int num;
 
@@ -707,6 +708,8 @@ static int tps80031_regulator_probe(struct platform_device *pdev)
 		config.dev = &pdev->dev;
 		config.init_data = NULL;
 		config.driver_data = ri;
+		config.regmap = tps80031_mfd->regmap[ri->rinfo->volt_id];
+
 		if (tps_pdata) {
 			config.init_data = tps_pdata->reg_init_data;
 			ri->config_flags = tps_pdata->config_flags;
diff --git a/drivers/regulator/twl-regulator.c b/drivers/regulator/twl-regulator.c
index f705d25b437..fb6e67d74ff 100644
--- a/drivers/regulator/twl-regulator.c
+++ b/drivers/regulator/twl-regulator.c
@@ -441,12 +441,6 @@ static const u16 VSIM_VSEL_table[] = {
 static const u16 VDAC_VSEL_table[] = {
 	1200, 1300, 1800, 1800,
 };
-static const u16 VDD1_VSEL_table[] = {
-	800, 1450,
-};
-static const u16 VDD2_VSEL_table[] = {
-	800, 1450, 1500,
-};
 static const u16 VIO_VSEL_table[] = {
 	1800, 1850,
 };
@@ -615,18 +609,8 @@ static struct regulator_ops twl6030ldo_ops = {
 
 /*----------------------------------------------------------------------*/
 
-/*
- * Fixed voltage LDOs don't have a VSEL field to update.
- */
-static int twlfixed_list_voltage(struct regulator_dev *rdev, unsigned index)
-{
-	struct twlreg_info	*info = rdev_get_drvdata(rdev);
-
-	return info->min_mV * 1000;
-}
-
 static struct regulator_ops twl4030fixed_ops = {
-	.list_voltage	= twlfixed_list_voltage,
+	.list_voltage	= regulator_list_voltage_linear,
 
 	.enable		= twl4030reg_enable,
 	.disable	= twl4030reg_disable,
@@ -638,7 +622,7 @@ static struct regulator_ops twl4030fixed_ops = {
 };
 
 static struct regulator_ops twl6030fixed_ops = {
-	.list_voltage	= twlfixed_list_voltage,
+	.list_voltage	= regulator_list_voltage_linear,
 
 	.enable		= twl6030reg_enable,
 	.disable	= twl6030reg_disable,
@@ -944,19 +928,7 @@ static const struct twlreg_info TWLFIXED_INFO_##label = { \
 		.ops = &operations, \
 		.type = REGULATOR_VOLTAGE, \
 		.owner = THIS_MODULE, \
-		.enable_time = turnon_delay, \
-		}, \
-	}
-
-#define TWL6030_FIXED_RESOURCE(label, offset, turnon_delay) \
-static struct twlreg_info TWLRES_INFO_##label = { \
-	.base = offset, \
-	.desc = { \
-		.name = #label, \
-		.id = TWL6030_REG_##label, \
-		.ops = &twl6030_fixed_resource, \
-		.type = REGULATOR_VOLTAGE, \
-		.owner = THIS_MODULE, \
+		.min_uV = mVolts * 1000, \
 		.enable_time = turnon_delay, \
 		}, \
 	}
diff --git a/drivers/regulator/wm8994-regulator.c b/drivers/regulator/wm8994-regulator.c
index 6ff87234264..a612c356a69 100644
--- a/drivers/regulator/wm8994-regulator.c
+++ b/drivers/regulator/wm8994-regulator.c
@@ -18,6 +18,7 @@
 #include <linux/err.h>
 #include <linux/platform_device.h>
 #include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
 #include <linux/gpio.h>
 #include <linux/slab.h>
 
@@ -28,6 +29,8 @@
 struct wm8994_ldo {
 	struct regulator_dev *regulator;
 	struct wm8994 *wm8994;
+	struct regulator_consumer_supply supply;
+	struct regulator_init_data init_data;
 };
 
 #define WM8994_LDO1_MAX_SELECTOR 0x7
@@ -99,6 +102,26 @@ static const struct regulator_desc wm8994_ldo_desc[] = {
 	},
 };
 
+static const struct regulator_consumer_supply wm8994_ldo_consumer[] = {
+	{ .supply = "AVDD1" },
+	{ .supply = "DCVDD" },
+};
+
+static const struct regulator_init_data wm8994_ldo_default[] = {
+	{
+		.constraints = {
+			.valid_ops_mask = REGULATOR_CHANGE_STATUS,
+		},
+		.num_consumer_supplies = 1,
+	},
+	{
+		.constraints = {
+			.valid_ops_mask = REGULATOR_CHANGE_STATUS,
+		},
+		.num_consumer_supplies = 1,
+	},
+};
+
 static int wm8994_ldo_probe(struct platform_device *pdev)
 {
 	struct wm8994 *wm8994 = dev_get_drvdata(pdev->dev.parent);
@@ -117,13 +140,29 @@ static int wm8994_ldo_probe(struct platform_device *pdev)
 	}
 
 	ldo->wm8994 = wm8994;
+	ldo->supply = wm8994_ldo_consumer[id];
+	ldo->supply.dev_name = dev_name(wm8994->dev);
 
 	config.dev = wm8994->dev;
 	config.driver_data = ldo;
 	config.regmap = wm8994->regmap;
-	if (pdata) {
-		config.init_data = pdata->ldo[id].init_data;
+	config.init_data = &ldo->init_data;
+	if (pdata)
 		config.ena_gpio = pdata->ldo[id].enable;
+	else if (wm8994->dev->of_node)
+		config.ena_gpio = wm8994->pdata.ldo[id].enable;
+
+	/* Use default constraints if none set up */
+	if (!pdata || !pdata->ldo[id].init_data || wm8994->dev->of_node) {
+		dev_dbg(wm8994->dev, "Using default init data, supply %s %s\n",
+			ldo->supply.dev_name, ldo->supply.supply);
+
+		ldo->init_data = wm8994_ldo_default[id];
+		ldo->init_data.consumer_supplies = &ldo->supply;
+		if (!config.ena_gpio)
+			ldo->init_data.constraints.valid_ops_mask = 0;
+	} else {
+		ldo->init_data = *pdata->ldo[id].init_data;
 	}
 
 	ldo->regulator = regulator_register(&wm8994_ldo_desc[id], &config);
@@ -162,23 +201,7 @@ static struct platform_driver wm8994_ldo_driver = {
 	},
 };
 
-static int __init wm8994_ldo_init(void)
-{
-	int ret;
-
-	ret = platform_driver_register(&wm8994_ldo_driver);
-	if (ret != 0)
-		pr_err("Failed to register Wm8994 GP LDO driver: %d\n", ret);
-
-	return ret;
-}
-subsys_initcall(wm8994_ldo_init);
-
-static void __exit wm8994_ldo_exit(void)
-{
-	platform_driver_unregister(&wm8994_ldo_driver);
-}
-module_exit(wm8994_ldo_exit);
+module_platform_driver(wm8994_ldo_driver);
 
 /* Module information */
 MODULE_AUTHOR("Mark Brown <broonie@opensource.wolfsonmicro.com>");
diff --git a/drivers/remoteproc/Kconfig b/drivers/remoteproc/Kconfig
index cc1f7bf53fd..c6d77e20622 100644
--- a/drivers/remoteproc/Kconfig
+++ b/drivers/remoteproc/Kconfig
@@ -4,7 +4,7 @@ menu "Remoteproc drivers"
 config REMOTEPROC
 	tristate
 	depends on HAS_DMA
-	select FW_CONFIG
+	select FW_LOADER
 	select VIRTIO
 
 config OMAP_REMOTEPROC
diff --git a/drivers/remoteproc/remoteproc_core.c b/drivers/remoteproc/remoteproc_core.c
index 29387df4bfc..814af5ab8a7 100644
--- a/drivers/remoteproc/remoteproc_core.c
+++ b/drivers/remoteproc/remoteproc_core.c
@@ -59,7 +59,7 @@ static const char *rproc_crash_to_string(enum rproc_crash_type type)
 {
 	if (type < ARRAY_SIZE(rproc_crash_names))
 		return rproc_crash_names[type];
-	return "unkown";
+	return "unknown";
 }
 
 /*
@@ -217,7 +217,7 @@ int rproc_alloc_vring(struct rproc_vdev *rvdev, int i)
 	 * TODO: support predefined notifyids (via resource table)
 	 */
 	ret = idr_alloc(&rproc->notifyids, rvring, 0, 0, GFP_KERNEL);
-	if (ret) {
+	if (ret < 0) {
 		dev_err(dev, "idr_alloc failed: %d\n", ret);
 		dma_free_coherent(dev->parent, size, va, dma);
 		return ret;
@@ -366,10 +366,12 @@ static int rproc_handle_vdev(struct rproc *rproc, struct fw_rsc_vdev *rsc,
 	/* it is now safe to add the virtio device */
 	ret = rproc_add_virtio_dev(rvdev, rsc->id);
 	if (ret)
-		goto free_rvdev;
+		goto remove_rvdev;
 
 	return 0;
 
+remove_rvdev:
+	list_del(&rvdev->node);
 free_rvdev:
 	kfree(rvdev);
 	return ret;
diff --git a/drivers/remoteproc/ste_modem_rproc.c b/drivers/remoteproc/ste_modem_rproc.c
index a7743c06933..fb95c422005 100644
--- a/drivers/remoteproc/ste_modem_rproc.c
+++ b/drivers/remoteproc/ste_modem_rproc.c
@@ -240,6 +240,8 @@ static int sproc_drv_remove(struct platform_device *pdev)
 
 	/* Unregister as remoteproc device */
 	rproc_del(sproc->rproc);
+	dma_free_coherent(sproc->rproc->dev.parent, SPROC_FW_SIZE,
+			  sproc->fw_addr, sproc->fw_dma_addr);
 	rproc_put(sproc->rproc);
 
 	mdev->drv_data = NULL;
@@ -297,10 +299,13 @@ static int sproc_probe(struct platform_device *pdev)
 	/* Register as a remoteproc device */
 	err = rproc_add(rproc);
 	if (err)
-		goto free_rproc;
+		goto free_mem;
 
 	return 0;
 
+free_mem:
+	dma_free_coherent(rproc->dev.parent, SPROC_FW_SIZE,
+			  sproc->fw_addr, sproc->fw_dma_addr);
 free_rproc:
 	/* Reset device data upon error */
 	mdev->drv_data = NULL;
diff --git a/drivers/rpmsg/virtio_rpmsg_bus.c b/drivers/rpmsg/virtio_rpmsg_bus.c
index a59684b5fc6..7861f1119b7 100644
--- a/drivers/rpmsg/virtio_rpmsg_bus.c
+++ b/drivers/rpmsg/virtio_rpmsg_bus.c
@@ -951,8 +951,10 @@ static int rpmsg_probe(struct virtio_device *vdev)
 	bufs_va = dma_alloc_coherent(vdev->dev.parent->parent,
 				RPMSG_TOTAL_BUF_SPACE,
 				&vrp->bufs_dma, GFP_KERNEL);
-	if (!bufs_va)
+	if (!bufs_va) {
+		err = -ENOMEM;
 		goto vqs_del;
+	}
 
 	dev_dbg(&vdev->dev, "buffers: va %p, dma 0x%llx\n", bufs_va,
 					(unsigned long long)vrp->bufs_dma);
diff --git a/drivers/rtc/Kconfig b/drivers/rtc/Kconfig
index 79fbe3832df..0c81915b199 100644
--- a/drivers/rtc/Kconfig
+++ b/drivers/rtc/Kconfig
@@ -402,7 +402,7 @@ config RTC_DRV_TPS6586X
 	tristate "TI TPS6586X RTC driver"
 	depends on MFD_TPS6586X
 	help
-	  TI Power Managment IC TPS6586X supports RTC functionality
+	  TI Power Management IC TPS6586X supports RTC functionality
 	  along with alarm. This driver supports the RTC driver for
 	  the TPS6586X RTC module.
 
@@ -420,7 +420,7 @@ config RTC_DRV_TPS80031
 	tristate "TI TPS80031/TPS80032 RTC driver"
 	depends on MFD_TPS80031
 	help
-	  TI Power Managment IC TPS80031 supports RTC functionality
+	  TI Power Management IC TPS80031 supports RTC functionality
 	  along with alarm. This driver supports the RTC driver for
 	  the TPS80031 RTC module.
 
diff --git a/drivers/rtc/class.c b/drivers/rtc/class.c
index 9b742d3ffb9..66385402d20 100644
--- a/drivers/rtc/class.c
+++ b/drivers/rtc/class.c
@@ -259,6 +259,76 @@ void rtc_device_unregister(struct rtc_device *rtc)
 }
 EXPORT_SYMBOL_GPL(rtc_device_unregister);
 
+static void devm_rtc_device_release(struct device *dev, void *res)
+{
+	struct rtc_device *rtc = *(struct rtc_device **)res;
+
+	rtc_device_unregister(rtc);
+}
+
+static int devm_rtc_device_match(struct device *dev, void *res, void *data)
+{
+	struct rtc **r = res;
+
+	return *r == data;
+}
+
+/**
+ * devm_rtc_device_register - resource managed rtc_device_register()
+ * @dev: the device to register
+ * @name: the name of the device
+ * @ops: the rtc operations structure
+ * @owner: the module owner
+ *
+ * @return a struct rtc on success, or an ERR_PTR on error
+ *
+ * Managed rtc_device_register(). The rtc_device returned from this function
+ * are automatically freed on driver detach. See rtc_device_register()
+ * for more information.
+ */
+
+struct rtc_device *devm_rtc_device_register(struct device *dev,
+					const char *name,
+					const struct rtc_class_ops *ops,
+					struct module *owner)
+{
+	struct rtc_device **ptr, *rtc;
+
+	ptr = devres_alloc(devm_rtc_device_release, sizeof(*ptr), GFP_KERNEL);
+	if (!ptr)
+		return ERR_PTR(-ENOMEM);
+
+	rtc = rtc_device_register(name, dev, ops, owner);
+	if (!IS_ERR(rtc)) {
+		*ptr = rtc;
+		devres_add(dev, ptr);
+	} else {
+		devres_free(ptr);
+	}
+
+	return rtc;
+}
+EXPORT_SYMBOL_GPL(devm_rtc_device_register);
+
+/**
+ * devm_rtc_device_unregister - resource managed devm_rtc_device_unregister()
+ * @dev: the device to unregister
+ * @rtc: the RTC class device to unregister
+ *
+ * Deallocated a rtc allocated with devm_rtc_device_register(). Normally this
+ * function will not need to be called and the resource management code will
+ * ensure that the resource is freed.
+ */
+void devm_rtc_device_unregister(struct device *dev, struct rtc_device *rtc)
+{
+	int rc;
+
+	rc = devres_release(dev, devm_rtc_device_release,
+				devm_rtc_device_match, rtc);
+	WARN_ON(rc);
+}
+EXPORT_SYMBOL_GPL(devm_rtc_device_unregister);
+
 static int __init rtc_init(void)
 {
 	rtc_class = class_create(THIS_MODULE, "rtc");
diff --git a/drivers/rtc/rtc-88pm80x.c b/drivers/rtc/rtc-88pm80x.c
index 63b17ebe90e..f3742f364eb 100644
--- a/drivers/rtc/rtc-88pm80x.c
+++ b/drivers/rtc/rtc-88pm80x.c
@@ -234,7 +234,7 @@ static const struct rtc_class_ops pm80x_rtc_ops = {
 	.alarm_irq_enable = pm80x_rtc_alarm_irq_enable,
 };
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int pm80x_rtc_suspend(struct device *dev)
 {
 	return pm80x_dev_suspend(dev);
@@ -312,7 +312,7 @@ static int pm80x_rtc_probe(struct platform_device *pdev)
 	}
 	rtc_tm_to_time(&tm, &ticks);
 
-	info->rtc_dev = rtc_device_register("88pm80x-rtc", &pdev->dev,
+	info->rtc_dev = devm_rtc_device_register(&pdev->dev, "88pm80x-rtc",
 					    &pm80x_rtc_ops, THIS_MODULE);
 	if (IS_ERR(info->rtc_dev)) {
 		ret = PTR_ERR(info->rtc_dev);
@@ -346,7 +346,6 @@ static int pm80x_rtc_remove(struct platform_device *pdev)
 {
 	struct pm80x_rtc_info *info = platform_get_drvdata(pdev);
 	platform_set_drvdata(pdev, NULL);
-	rtc_device_unregister(info->rtc_dev);
 	pm80x_free_irq(info->chip, info->irq, info);
 	return 0;
 }
diff --git a/drivers/rtc/rtc-88pm860x.c b/drivers/rtc/rtc-88pm860x.c
index f663746f460..0f2b91bfee3 100644
--- a/drivers/rtc/rtc-88pm860x.c
+++ b/drivers/rtc/rtc-88pm860x.c
@@ -318,14 +318,14 @@ static int pm860x_rtc_probe(struct platform_device *pdev)
 
 	pdata = pdev->dev.platform_data;
 
-	info = kzalloc(sizeof(struct pm860x_rtc_info), GFP_KERNEL);
+	info = devm_kzalloc(&pdev->dev, sizeof(struct pm860x_rtc_info),
+			    GFP_KERNEL);
 	if (!info)
 		return -ENOMEM;
 	info->irq = platform_get_irq(pdev, 0);
 	if (info->irq < 0) {
 		dev_err(&pdev->dev, "No IRQ resource!\n");
-		ret = -EINVAL;
-		goto out;
+		return info->irq;
 	}
 
 	info->chip = chip;
@@ -333,12 +333,13 @@ static int pm860x_rtc_probe(struct platform_device *pdev)
 	info->dev = &pdev->dev;
 	dev_set_drvdata(&pdev->dev, info);
 
-	ret = request_threaded_irq(info->irq, NULL, rtc_update_handler,
-				   IRQF_ONESHOT, "rtc", info);
+	ret = devm_request_threaded_irq(&pdev->dev, info->irq, NULL,
+					rtc_update_handler, IRQF_ONESHOT, "rtc",
+					info);
 	if (ret < 0) {
 		dev_err(chip->dev, "Failed to request IRQ: #%d: %d\n",
 			info->irq, ret);
-		goto out;
+		return ret;
 	}
 
 	/* set addresses of 32-bit base value for RTC time */
@@ -350,7 +351,7 @@ static int pm860x_rtc_probe(struct platform_device *pdev)
 	ret = pm860x_rtc_read_time(&pdev->dev, &tm);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "Failed to read initial time.\n");
-		goto out_rtc;
+		return ret;
 	}
 	if ((tm.tm_year < 70) || (tm.tm_year > 138)) {
 		tm.tm_year = 70;
@@ -362,7 +363,7 @@ static int pm860x_rtc_probe(struct platform_device *pdev)
 		ret = pm860x_rtc_set_time(&pdev->dev, &tm);
 		if (ret < 0) {
 			dev_err(&pdev->dev, "Failed to set initial time.\n");
-			goto out_rtc;
+			return ret;
 		}
 	}
 	rtc_tm_to_time(&tm, &ticks);
@@ -373,12 +374,12 @@ static int pm860x_rtc_probe(struct platform_device *pdev)
 		}
 	}
 
-	info->rtc_dev = rtc_device_register("88pm860x-rtc", &pdev->dev,
+	info->rtc_dev = devm_rtc_device_register(&pdev->dev, "88pm860x-rtc",
 					    &pm860x_rtc_ops, THIS_MODULE);
 	ret = PTR_ERR(info->rtc_dev);
 	if (IS_ERR(info->rtc_dev)) {
 		dev_err(&pdev->dev, "Failed to register RTC device: %d\n", ret);
-		goto out_rtc;
+		return ret;
 	}
 
 	/*
@@ -405,11 +406,6 @@ static int pm860x_rtc_probe(struct platform_device *pdev)
 	device_init_wakeup(&pdev->dev, 1);
 
 	return 0;
-out_rtc:
-	free_irq(info->irq, info);
-out:
-	kfree(info);
-	return ret;
 }
 
 static int pm860x_rtc_remove(struct platform_device *pdev)
@@ -423,9 +419,6 @@ static int pm860x_rtc_remove(struct platform_device *pdev)
 #endif	/* VRTC_CALIBRATION */
 
 	platform_set_drvdata(pdev, NULL);
-	rtc_device_unregister(info->rtc_dev);
-	free_irq(info->irq, info);
-	kfree(info);
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-ab3100.c b/drivers/rtc/rtc-ab3100.c
index 261a07e0fb2..47a4f2c4d30 100644
--- a/drivers/rtc/rtc-ab3100.c
+++ b/drivers/rtc/rtc-ab3100.c
@@ -229,8 +229,8 @@ static int __init ab3100_rtc_probe(struct platform_device *pdev)
 		/* Ignore any error on this write */
 	}
 
-	rtc = rtc_device_register("ab3100-rtc", &pdev->dev, &ab3100_rtc_ops,
-				  THIS_MODULE);
+	rtc = devm_rtc_device_register(&pdev->dev, "ab3100-rtc",
+					&ab3100_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc)) {
 		err = PTR_ERR(rtc);
 		return err;
@@ -242,9 +242,6 @@ static int __init ab3100_rtc_probe(struct platform_device *pdev)
 
 static int __exit ab3100_rtc_remove(struct platform_device *pdev)
 {
-	struct rtc_device *rtc = platform_get_drvdata(pdev);
-
-	rtc_device_unregister(rtc);
 	platform_set_drvdata(pdev, NULL);
 	return 0;
 }
@@ -257,19 +254,7 @@ static struct platform_driver ab3100_rtc_driver = {
 	.remove	 = __exit_p(ab3100_rtc_remove),
 };
 
-static int __init ab3100_rtc_init(void)
-{
-	return platform_driver_probe(&ab3100_rtc_driver,
-				     ab3100_rtc_probe);
-}
-
-static void __exit ab3100_rtc_exit(void)
-{
-	platform_driver_unregister(&ab3100_rtc_driver);
-}
-
-module_init(ab3100_rtc_init);
-module_exit(ab3100_rtc_exit);
+module_platform_driver_probe(ab3100_rtc_driver, ab3100_rtc_probe);
 
 MODULE_AUTHOR("Linus Walleij <linus.walleij@stericsson.com>");
 MODULE_DESCRIPTION("AB3100 RTC Driver");
diff --git a/drivers/rtc/rtc-ab8500.c b/drivers/rtc/rtc-ab8500.c
index 57cde2b061e..63cfa314a39 100644
--- a/drivers/rtc/rtc-ab8500.c
+++ b/drivers/rtc/rtc-ab8500.c
@@ -422,20 +422,19 @@ static int ab8500_rtc_probe(struct platform_device *pdev)
 
 	device_init_wakeup(&pdev->dev, true);
 
-	rtc = rtc_device_register("ab8500-rtc", &pdev->dev, &ab8500_rtc_ops,
-			THIS_MODULE);
+	rtc = devm_rtc_device_register(&pdev->dev, "ab8500-rtc",
+					&ab8500_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc)) {
 		dev_err(&pdev->dev, "Registration failed\n");
 		err = PTR_ERR(rtc);
 		return err;
 	}
 
-	err = request_threaded_irq(irq, NULL, rtc_alarm_handler,
-		IRQF_NO_SUSPEND | IRQF_ONESHOT, "ab8500-rtc", rtc);
-	if (err < 0) {
-		rtc_device_unregister(rtc);
+	err = devm_request_threaded_irq(&pdev->dev, irq, NULL,
+			rtc_alarm_handler, IRQF_NO_SUSPEND | IRQF_ONESHOT,
+			"ab8500-rtc", rtc);
+	if (err < 0)
 		return err;
-	}
 
 	platform_set_drvdata(pdev, rtc);
 
@@ -450,13 +449,8 @@ static int ab8500_rtc_probe(struct platform_device *pdev)
 
 static int ab8500_rtc_remove(struct platform_device *pdev)
 {
-	struct rtc_device *rtc = platform_get_drvdata(pdev);
-	int irq = platform_get_irq_byname(pdev, "ALARM");
-
 	ab8500_sysfs_rtc_unregister(&pdev->dev);
 
-	free_irq(irq, rtc);
-	rtc_device_unregister(rtc);
 	platform_set_drvdata(pdev, NULL);
 
 	return 0;
diff --git a/drivers/rtc/rtc-at32ap700x.c b/drivers/rtc/rtc-at32ap700x.c
index 8dd08305aae..f47fbb5eee8 100644
--- a/drivers/rtc/rtc-at32ap700x.c
+++ b/drivers/rtc/rtc-at32ap700x.c
@@ -202,7 +202,8 @@ static int __init at32_rtc_probe(struct platform_device *pdev)
 	int irq;
 	int ret;
 
-	rtc = kzalloc(sizeof(struct rtc_at32ap700x), GFP_KERNEL);
+	rtc = devm_kzalloc(&pdev->dev, sizeof(struct rtc_at32ap700x),
+			   GFP_KERNEL);
 	if (!rtc) {
 		dev_dbg(&pdev->dev, "out of memory\n");
 		return -ENOMEM;
@@ -223,7 +224,7 @@ static int __init at32_rtc_probe(struct platform_device *pdev)
 	}
 
 	rtc->irq = irq;
-	rtc->regs = ioremap(regs->start, resource_size(regs));
+	rtc->regs = devm_ioremap(&pdev->dev, regs->start, resource_size(regs));
 	if (!rtc->regs) {
 		ret = -ENOMEM;
 		dev_dbg(&pdev->dev, "could not map I/O memory\n");
@@ -244,20 +245,21 @@ static int __init at32_rtc_probe(struct platform_device *pdev)
 				| RTC_BIT(CTRL_EN));
 	}
 
-	ret = request_irq(irq, at32_rtc_interrupt, IRQF_SHARED, "rtc", rtc);
+	ret = devm_request_irq(&pdev->dev, irq, at32_rtc_interrupt, IRQF_SHARED,
+				"rtc", rtc);
 	if (ret) {
 		dev_dbg(&pdev->dev, "could not request irq %d\n", irq);
-		goto out_iounmap;
+		goto out;
 	}
 
 	platform_set_drvdata(pdev, rtc);
 
-	rtc->rtc = rtc_device_register(pdev->name, &pdev->dev,
+	rtc->rtc = devm_rtc_device_register(&pdev->dev, pdev->name,
 				&at32_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc->rtc)) {
 		dev_dbg(&pdev->dev, "could not register rtc device\n");
 		ret = PTR_ERR(rtc->rtc);
-		goto out_free_irq;
+		goto out;
 	}
 
 	device_init_wakeup(&pdev->dev, 1);
@@ -267,26 +269,15 @@ static int __init at32_rtc_probe(struct platform_device *pdev)
 
 	return 0;
 
-out_free_irq:
-	platform_set_drvdata(pdev, NULL);
-	free_irq(irq, rtc);
-out_iounmap:
-	iounmap(rtc->regs);
 out:
-	kfree(rtc);
+	platform_set_drvdata(pdev, NULL);
 	return ret;
 }
 
 static int __exit at32_rtc_remove(struct platform_device *pdev)
 {
-	struct rtc_at32ap700x *rtc = platform_get_drvdata(pdev);
-
 	device_init_wakeup(&pdev->dev, 0);
 
-	free_irq(rtc->irq, rtc);
-	iounmap(rtc->regs);
-	rtc_device_unregister(rtc->rtc);
-	kfree(rtc);
 	platform_set_drvdata(pdev, NULL);
 
 	return 0;
@@ -302,17 +293,7 @@ static struct platform_driver at32_rtc_driver = {
 	},
 };
 
-static int __init at32_rtc_init(void)
-{
-	return platform_driver_probe(&at32_rtc_driver, at32_rtc_probe);
-}
-module_init(at32_rtc_init);
-
-static void __exit at32_rtc_exit(void)
-{
-	platform_driver_unregister(&at32_rtc_driver);
-}
-module_exit(at32_rtc_exit);
+module_platform_driver_probe(at32_rtc_driver, at32_rtc_probe);
 
 MODULE_AUTHOR("Hans-Christian Egtvedt <hcegtvedt@atmel.com>");
 MODULE_DESCRIPTION("Real time clock for AVR32 AT32AP700x");
diff --git a/drivers/rtc/rtc-at91rm9200.c b/drivers/rtc/rtc-at91rm9200.c
index 0a9f27e094e..0eab77b2234 100644
--- a/drivers/rtc/rtc-at91rm9200.c
+++ b/drivers/rtc/rtc-at91rm9200.c
@@ -28,6 +28,8 @@
 #include <linux/ioctl.h>
 #include <linux/completion.h>
 #include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
 
 #include <asm/uaccess.h>
 
@@ -44,7 +46,6 @@ static DECLARE_COMPLETION(at91_rtc_updated);
 static unsigned int at91_alarm_year = AT91_RTC_EPOCH;
 static void __iomem *at91_rtc_regs;
 static int irq;
-static u32 at91_rtc_imr;
 
 /*
  * Decode time/date into rtc_time structure
@@ -109,11 +110,9 @@ static int at91_rtc_settime(struct device *dev, struct rtc_time *tm)
 	cr = at91_rtc_read(AT91_RTC_CR);
 	at91_rtc_write(AT91_RTC_CR, cr | AT91_RTC_UPDCAL | AT91_RTC_UPDTIM);
 
-	at91_rtc_imr |= AT91_RTC_ACKUPD;
 	at91_rtc_write(AT91_RTC_IER, AT91_RTC_ACKUPD);
 	wait_for_completion(&at91_rtc_updated);	/* wait for ACKUPD interrupt */
 	at91_rtc_write(AT91_RTC_IDR, AT91_RTC_ACKUPD);
-	at91_rtc_imr &= ~AT91_RTC_ACKUPD;
 
 	at91_rtc_write(AT91_RTC_TIMR,
 			  bin2bcd(tm->tm_sec) << 0
@@ -145,7 +144,7 @@ static int at91_rtc_readalarm(struct device *dev, struct rtc_wkalrm *alrm)
 	tm->tm_yday = rtc_year_days(tm->tm_mday, tm->tm_mon, tm->tm_year);
 	tm->tm_year = at91_alarm_year - 1900;
 
-	alrm->enabled = (at91_rtc_imr & AT91_RTC_ALARM)
+	alrm->enabled = (at91_rtc_read(AT91_RTC_IMR) & AT91_RTC_ALARM)
 			? 1 : 0;
 
 	dev_dbg(dev, "%s(): %4d-%02d-%02d %02d:%02d:%02d\n", __func__,
@@ -171,7 +170,6 @@ static int at91_rtc_setalarm(struct device *dev, struct rtc_wkalrm *alrm)
 	tm.tm_sec = alrm->time.tm_sec;
 
 	at91_rtc_write(AT91_RTC_IDR, AT91_RTC_ALARM);
-	at91_rtc_imr &= ~AT91_RTC_ALARM;
 	at91_rtc_write(AT91_RTC_TIMALR,
 		  bin2bcd(tm.tm_sec) << 0
 		| bin2bcd(tm.tm_min) << 8
@@ -184,7 +182,6 @@ static int at91_rtc_setalarm(struct device *dev, struct rtc_wkalrm *alrm)
 
 	if (alrm->enabled) {
 		at91_rtc_write(AT91_RTC_SCCR, AT91_RTC_ALARM);
-		at91_rtc_imr |= AT91_RTC_ALARM;
 		at91_rtc_write(AT91_RTC_IER, AT91_RTC_ALARM);
 	}
 
@@ -201,12 +198,9 @@ static int at91_rtc_alarm_irq_enable(struct device *dev, unsigned int enabled)
 
 	if (enabled) {
 		at91_rtc_write(AT91_RTC_SCCR, AT91_RTC_ALARM);
-		at91_rtc_imr |= AT91_RTC_ALARM;
 		at91_rtc_write(AT91_RTC_IER, AT91_RTC_ALARM);
-	} else {
+	} else
 		at91_rtc_write(AT91_RTC_IDR, AT91_RTC_ALARM);
-		at91_rtc_imr &= ~AT91_RTC_ALARM;
-	}
 
 	return 0;
 }
@@ -215,10 +209,12 @@ static int at91_rtc_alarm_irq_enable(struct device *dev, unsigned int enabled)
  */
 static int at91_rtc_proc(struct device *dev, struct seq_file *seq)
 {
+	unsigned long imr = at91_rtc_read(AT91_RTC_IMR);
+
 	seq_printf(seq, "update_IRQ\t: %s\n",
-			(at91_rtc_imr & AT91_RTC_ACKUPD) ? "yes" : "no");
+			(imr & AT91_RTC_ACKUPD) ? "yes" : "no");
 	seq_printf(seq, "periodic_IRQ\t: %s\n",
-			(at91_rtc_imr & AT91_RTC_SECEV) ? "yes" : "no");
+			(imr & AT91_RTC_SECEV) ? "yes" : "no");
 
 	return 0;
 }
@@ -233,7 +229,7 @@ static irqreturn_t at91_rtc_interrupt(int irq, void *dev_id)
 	unsigned int rtsr;
 	unsigned long events = 0;
 
-	rtsr = at91_rtc_read(AT91_RTC_SR) & at91_rtc_imr;
+	rtsr = at91_rtc_read(AT91_RTC_SR) & at91_rtc_read(AT91_RTC_IMR);
 	if (rtsr) {		/* this interrupt is shared!  Is it ours? */
 		if (rtsr & AT91_RTC_ALARM)
 			events |= (RTC_AF | RTC_IRQF);
@@ -297,14 +293,13 @@ static int __init at91_rtc_probe(struct platform_device *pdev)
 	at91_rtc_write(AT91_RTC_IDR, AT91_RTC_ACKUPD | AT91_RTC_ALARM |
 					AT91_RTC_SECEV | AT91_RTC_TIMEV |
 					AT91_RTC_CALEV);
-	at91_rtc_imr = 0;
 
 	ret = request_irq(irq, at91_rtc_interrupt,
 				IRQF_SHARED,
 				"at91_rtc", pdev);
 	if (ret) {
 		dev_err(&pdev->dev, "IRQ %d already in use.\n", irq);
-		return ret;
+		goto err_unmap;
 	}
 
 	/* cpu init code should really have flagged this device as
@@ -316,13 +311,20 @@ static int __init at91_rtc_probe(struct platform_device *pdev)
 	rtc = rtc_device_register(pdev->name, &pdev->dev,
 				&at91_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc)) {
-		free_irq(irq, pdev);
-		return PTR_ERR(rtc);
+		ret = PTR_ERR(rtc);
+		goto err_free_irq;
 	}
 	platform_set_drvdata(pdev, rtc);
 
 	dev_info(&pdev->dev, "AT91 Real Time Clock driver.\n");
 	return 0;
+
+err_free_irq:
+	free_irq(irq, pdev);
+err_unmap:
+	iounmap(at91_rtc_regs);
+
+	return ret;
 }
 
 /*
@@ -336,84 +338,68 @@ static int __exit at91_rtc_remove(struct platform_device *pdev)
 	at91_rtc_write(AT91_RTC_IDR, AT91_RTC_ACKUPD | AT91_RTC_ALARM |
 					AT91_RTC_SECEV | AT91_RTC_TIMEV |
 					AT91_RTC_CALEV);
-	at91_rtc_imr = 0;
 	free_irq(irq, pdev);
 
 	rtc_device_unregister(rtc);
+	iounmap(at91_rtc_regs);
 	platform_set_drvdata(pdev, NULL);
 
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 
 /* AT91RM9200 RTC Power management control */
 
-static u32 at91_rtc_bkpimr;
-
+static u32 at91_rtc_imr;
 
 static int at91_rtc_suspend(struct device *dev)
 {
 	/* this IRQ is shared with DBGU and other hardware which isn't
 	 * necessarily doing PM like we are...
 	 */
-	at91_rtc_bkpimr = at91_rtc_imr & (AT91_RTC_ALARM|AT91_RTC_SECEV);
-	if (at91_rtc_bkpimr) {
-		if (device_may_wakeup(dev)) {
+	at91_rtc_imr = at91_rtc_read(AT91_RTC_IMR)
+			& (AT91_RTC_ALARM|AT91_RTC_SECEV);
+	if (at91_rtc_imr) {
+		if (device_may_wakeup(dev))
 			enable_irq_wake(irq);
-		} else {
-			at91_rtc_write(AT91_RTC_IDR, at91_rtc_bkpimr);
-			at91_rtc_imr &= ~at91_rtc_bkpimr;
-		}
-}
+		else
+			at91_rtc_write(AT91_RTC_IDR, at91_rtc_imr);
+	}
 	return 0;
 }
 
 static int at91_rtc_resume(struct device *dev)
 {
-	if (at91_rtc_bkpimr) {
-		if (device_may_wakeup(dev)) {
+	if (at91_rtc_imr) {
+		if (device_may_wakeup(dev))
 			disable_irq_wake(irq);
-		} else {
-			at91_rtc_imr |= at91_rtc_bkpimr;
-			at91_rtc_write(AT91_RTC_IER, at91_rtc_bkpimr);
-		}
+		else
+			at91_rtc_write(AT91_RTC_IER, at91_rtc_imr);
 	}
 	return 0;
 }
+#endif
 
-static const struct dev_pm_ops at91_rtc_pm = {
-	.suspend =	at91_rtc_suspend,
-	.resume =	at91_rtc_resume,
-};
-
-#define at91_rtc_pm_ptr	&at91_rtc_pm
+static SIMPLE_DEV_PM_OPS(at91_rtc_pm_ops, at91_rtc_suspend, at91_rtc_resume);
 
-#else
-#define at91_rtc_pm_ptr	NULL
-#endif
+static const struct of_device_id at91_rtc_dt_ids[] = {
+	{ .compatible = "atmel,at91rm9200-rtc" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, at91_rtc_dt_ids);
 
 static struct platform_driver at91_rtc_driver = {
 	.remove		= __exit_p(at91_rtc_remove),
 	.driver		= {
 		.name	= "at91_rtc",
 		.owner	= THIS_MODULE,
-		.pm	= at91_rtc_pm_ptr,
+		.pm	= &at91_rtc_pm_ops,
+		.of_match_table = of_match_ptr(at91_rtc_dt_ids),
 	},
 };
 
-static int __init at91_rtc_init(void)
-{
-	return platform_driver_probe(&at91_rtc_driver, at91_rtc_probe);
-}
-
-static void __exit at91_rtc_exit(void)
-{
-	platform_driver_unregister(&at91_rtc_driver);
-}
-
-module_init(at91_rtc_init);
-module_exit(at91_rtc_exit);
+module_platform_driver_probe(at91_rtc_driver, at91_rtc_probe);
 
 MODULE_AUTHOR("Rick Bronson");
 MODULE_DESCRIPTION("RTC driver for Atmel AT91RM9200");
diff --git a/drivers/rtc/rtc-at91rm9200.h b/drivers/rtc/rtc-at91rm9200.h
index 5f940b6844c..da1945e5f71 100644
--- a/drivers/rtc/rtc-at91rm9200.h
+++ b/drivers/rtc/rtc-at91rm9200.h
@@ -64,6 +64,7 @@
 #define	AT91_RTC_SCCR		0x1c			/* Status Clear Command Register */
 #define	AT91_RTC_IER		0x20			/* Interrupt Enable Register */
 #define	AT91_RTC_IDR		0x24			/* Interrupt Disable Register */
+#define	AT91_RTC_IMR		0x28			/* Interrupt Mask Register */
 
 #define	AT91_RTC_VER		0x2c			/* Valid Entry Register */
 #define		AT91_RTC_NVTIM		(1 <<  0)		/* Non valid Time */
diff --git a/drivers/rtc/rtc-at91sam9.c b/drivers/rtc/rtc-at91sam9.c
index 39cfd2ee004..b60a34cb145 100644
--- a/drivers/rtc/rtc-at91sam9.c
+++ b/drivers/rtc/rtc-at91sam9.c
@@ -20,6 +20,7 @@
 #include <linux/ioctl.h>
 #include <linux/slab.h>
 #include <linux/platform_data/atmel.h>
+#include <linux/io.h>
 
 #include <mach/at91_rtt.h>
 #include <mach/cpu.h>
@@ -309,7 +310,7 @@ static int at91_rtc_probe(struct platform_device *pdev)
 		return irq;
 	}
 
-	rtc = kzalloc(sizeof *rtc, GFP_KERNEL);
+	rtc = devm_kzalloc(&pdev->dev, sizeof(*rtc), GFP_KERNEL);
 	if (!rtc)
 		return -ENOMEM;
 
@@ -320,18 +321,19 @@ static int at91_rtc_probe(struct platform_device *pdev)
 		device_init_wakeup(&pdev->dev, 1);
 
 	platform_set_drvdata(pdev, rtc);
-	rtc->rtt = ioremap(r->start, resource_size(r));
+	rtc->rtt = devm_ioremap(&pdev->dev, r->start, resource_size(r));
 	if (!rtc->rtt) {
 		dev_err(&pdev->dev, "failed to map registers, aborting.\n");
 		ret = -ENOMEM;
 		goto fail;
 	}
 
-	rtc->gpbr = ioremap(r_gpbr->start, resource_size(r_gpbr));
+	rtc->gpbr = devm_ioremap(&pdev->dev, r_gpbr->start,
+				resource_size(r_gpbr));
 	if (!rtc->gpbr) {
 		dev_err(&pdev->dev, "failed to map gpbr registers, aborting.\n");
 		ret = -ENOMEM;
-		goto fail_gpbr;
+		goto fail;
 	}
 
 	mr = rtt_readl(rtc, MR);
@@ -346,20 +348,19 @@ static int at91_rtc_probe(struct platform_device *pdev)
 	mr &= ~(AT91_RTT_ALMIEN | AT91_RTT_RTTINCIEN);
 	rtt_writel(rtc, MR, mr);
 
-	rtc->rtcdev = rtc_device_register(pdev->name, &pdev->dev,
-				&at91_rtc_ops, THIS_MODULE);
+	rtc->rtcdev = devm_rtc_device_register(&pdev->dev, pdev->name,
+					&at91_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc->rtcdev)) {
 		ret = PTR_ERR(rtc->rtcdev);
-		goto fail_register;
+		goto fail;
 	}
 
 	/* register irq handler after we know what name we'll use */
-	ret = request_irq(rtc->irq, at91_rtc_interrupt, IRQF_SHARED,
-				dev_name(&rtc->rtcdev->dev), rtc);
+	ret = devm_request_irq(&pdev->dev, rtc->irq, at91_rtc_interrupt,
+				IRQF_SHARED, dev_name(&rtc->rtcdev->dev), rtc);
 	if (ret) {
 		dev_dbg(&pdev->dev, "can't share IRQ %d?\n", rtc->irq);
-		rtc_device_unregister(rtc->rtcdev);
-		goto fail_register;
+		goto fail;
 	}
 
 	/* NOTE:  sam9260 rev A silicon has a ROM bug which resets the
@@ -374,13 +375,8 @@ static int at91_rtc_probe(struct platform_device *pdev)
 
 	return 0;
 
-fail_register:
-	iounmap(rtc->gpbr);
-fail_gpbr:
-	iounmap(rtc->rtt);
 fail:
 	platform_set_drvdata(pdev, NULL);
-	kfree(rtc);
 	return ret;
 }
 
@@ -394,14 +390,8 @@ static int at91_rtc_remove(struct platform_device *pdev)
 
 	/* disable all interrupts */
 	rtt_writel(rtc, MR, mr & ~(AT91_RTT_ALMIEN | AT91_RTT_RTTINCIEN));
-	free_irq(rtc->irq, rtc);
-
-	rtc_device_unregister(rtc->rtcdev);
 
-	iounmap(rtc->gpbr);
-	iounmap(rtc->rtt);
 	platform_set_drvdata(pdev, NULL);
-	kfree(rtc);
 	return 0;
 }
 
@@ -414,14 +404,13 @@ static void at91_rtc_shutdown(struct platform_device *pdev)
 	rtt_writel(rtc, MR, mr & ~rtc->imr);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 
 /* AT91SAM9 RTC Power management control */
 
-static int at91_rtc_suspend(struct platform_device *pdev,
-					pm_message_t state)
+static int at91_rtc_suspend(struct device *dev)
 {
-	struct sam9_rtc	*rtc = platform_get_drvdata(pdev);
+	struct sam9_rtc	*rtc = dev_get_drvdata(dev);
 	u32		mr = rtt_readl(rtc, MR);
 
 	/*
@@ -430,7 +419,7 @@ static int at91_rtc_suspend(struct platform_device *pdev,
 	 */
 	rtc->imr = mr & (AT91_RTT_ALMIEN | AT91_RTT_RTTINCIEN);
 	if (rtc->imr) {
-		if (device_may_wakeup(&pdev->dev) && (mr & AT91_RTT_ALMIEN)) {
+		if (device_may_wakeup(dev) && (mr & AT91_RTT_ALMIEN)) {
 			enable_irq_wake(rtc->irq);
 			/* don't let RTTINC cause wakeups */
 			if (mr & AT91_RTT_RTTINCIEN)
@@ -442,13 +431,13 @@ static int at91_rtc_suspend(struct platform_device *pdev,
 	return 0;
 }
 
-static int at91_rtc_resume(struct platform_device *pdev)
+static int at91_rtc_resume(struct device *dev)
 {
-	struct sam9_rtc	*rtc = platform_get_drvdata(pdev);
+	struct sam9_rtc	*rtc = dev_get_drvdata(dev);
 	u32		mr;
 
 	if (rtc->imr) {
-		if (device_may_wakeup(&pdev->dev))
+		if (device_may_wakeup(dev))
 			disable_irq_wake(rtc->irq);
 		mr = rtt_readl(rtc, MR);
 		rtt_writel(rtc, MR, mr | rtc->imr);
@@ -456,20 +445,18 @@ static int at91_rtc_resume(struct platform_device *pdev)
 
 	return 0;
 }
-#else
-#define at91_rtc_suspend	NULL
-#define at91_rtc_resume		NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(at91_rtc_pm_ops, at91_rtc_suspend, at91_rtc_resume);
+
 static struct platform_driver at91_rtc_driver = {
 	.probe		= at91_rtc_probe,
 	.remove		= at91_rtc_remove,
 	.shutdown	= at91_rtc_shutdown,
-	.suspend	= at91_rtc_suspend,
-	.resume		= at91_rtc_resume,
 	.driver		= {
 		.name	= "rtc-at91sam9",
 		.owner	= THIS_MODULE,
+		.pm	= &at91_rtc_pm_ops,
 	},
 };
 
diff --git a/drivers/rtc/rtc-au1xxx.c b/drivers/rtc/rtc-au1xxx.c
index b309da4ec74..7995abc391f 100644
--- a/drivers/rtc/rtc-au1xxx.c
+++ b/drivers/rtc/rtc-au1xxx.c
@@ -101,7 +101,7 @@ static int au1xtoy_rtc_probe(struct platform_device *pdev)
 	while (au_readl(SYS_COUNTER_CNTRL) & SYS_CNTRL_C0S)
 		msleep(1);
 
-	rtcdev = rtc_device_register("rtc-au1xxx", &pdev->dev,
+	rtcdev = devm_rtc_device_register(&pdev->dev, "rtc-au1xxx",
 				     &au1xtoy_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtcdev)) {
 		ret = PTR_ERR(rtcdev);
@@ -118,9 +118,6 @@ out_err:
 
 static int au1xtoy_rtc_remove(struct platform_device *pdev)
 {
-	struct rtc_device *rtcdev = platform_get_drvdata(pdev);
-
-	rtc_device_unregister(rtcdev);
 	platform_set_drvdata(pdev, NULL);
 
 	return 0;
@@ -134,18 +131,7 @@ static struct platform_driver au1xrtc_driver = {
 	.remove		= au1xtoy_rtc_remove,
 };
 
-static int __init au1xtoy_rtc_init(void)
-{
-	return platform_driver_probe(&au1xrtc_driver, au1xtoy_rtc_probe);
-}
-
-static void __exit au1xtoy_rtc_exit(void)
-{
-	platform_driver_unregister(&au1xrtc_driver);
-}
-
-module_init(au1xtoy_rtc_init);
-module_exit(au1xtoy_rtc_exit);
+module_platform_driver_probe(au1xrtc_driver, au1xtoy_rtc_probe);
 
 MODULE_DESCRIPTION("Au1xxx TOY-counter-based RTC driver");
 MODULE_AUTHOR("Manuel Lauss <manuel.lauss@gmail.com>");
diff --git a/drivers/rtc/rtc-bfin.c b/drivers/rtc/rtc-bfin.c
index 4ec614b0954..ad44ec5dc29 100644
--- a/drivers/rtc/rtc-bfin.c
+++ b/drivers/rtc/rtc-bfin.c
@@ -352,14 +352,14 @@ static int bfin_rtc_probe(struct platform_device *pdev)
 	dev_dbg_stamp(dev);
 
 	/* Allocate memory for our RTC struct */
-	rtc = kzalloc(sizeof(*rtc), GFP_KERNEL);
+	rtc = devm_kzalloc(dev, sizeof(*rtc), GFP_KERNEL);
 	if (unlikely(!rtc))
 		return -ENOMEM;
 	platform_set_drvdata(pdev, rtc);
 	device_init_wakeup(dev, 1);
 
 	/* Register our RTC with the RTC framework */
-	rtc->rtc_dev = rtc_device_register(pdev->name, dev, &bfin_rtc_ops,
+	rtc->rtc_dev = devm_rtc_device_register(dev, pdev->name, &bfin_rtc_ops,
 						THIS_MODULE);
 	if (unlikely(IS_ERR(rtc->rtc_dev))) {
 		ret = PTR_ERR(rtc->rtc_dev);
@@ -367,9 +367,10 @@ static int bfin_rtc_probe(struct platform_device *pdev)
 	}
 
 	/* Grab the IRQ and init the hardware */
-	ret = request_irq(IRQ_RTC, bfin_rtc_interrupt, 0, pdev->name, dev);
+	ret = devm_request_irq(dev, IRQ_RTC, bfin_rtc_interrupt, 0,
+				pdev->name, dev);
 	if (unlikely(ret))
-		goto err_reg;
+		goto err;
 	/* sometimes the bootloader touched things, but the write complete was not
 	 * enabled, so let's just do a quick timeout here since the IRQ will not fire ...
 	 */
@@ -381,32 +382,23 @@ static int bfin_rtc_probe(struct platform_device *pdev)
 
 	return 0;
 
-err_reg:
-	rtc_device_unregister(rtc->rtc_dev);
 err:
-	kfree(rtc);
 	return ret;
 }
 
 static int bfin_rtc_remove(struct platform_device *pdev)
 {
-	struct bfin_rtc *rtc = platform_get_drvdata(pdev);
 	struct device *dev = &pdev->dev;
 
 	bfin_rtc_reset(dev, 0);
-	free_irq(IRQ_RTC, dev);
-	rtc_device_unregister(rtc->rtc_dev);
 	platform_set_drvdata(pdev, NULL);
-	kfree(rtc);
 
 	return 0;
 }
 
-#ifdef CONFIG_PM
-static int bfin_rtc_suspend(struct platform_device *pdev, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int bfin_rtc_suspend(struct device *dev)
 {
-	struct device *dev = &pdev->dev;
-
 	dev_dbg_stamp(dev);
 
 	if (device_may_wakeup(dev)) {
@@ -418,10 +410,8 @@ static int bfin_rtc_suspend(struct platform_device *pdev, pm_message_t state)
 	return 0;
 }
 
-static int bfin_rtc_resume(struct platform_device *pdev)
+static int bfin_rtc_resume(struct device *dev)
 {
-	struct device *dev = &pdev->dev;
-
 	dev_dbg_stamp(dev);
 
 	if (device_may_wakeup(dev))
@@ -440,20 +430,18 @@ static int bfin_rtc_resume(struct platform_device *pdev)
 
 	return 0;
 }
-#else
-# define bfin_rtc_suspend NULL
-# define bfin_rtc_resume  NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(bfin_rtc_pm_ops, bfin_rtc_suspend, bfin_rtc_resume);
+
 static struct platform_driver bfin_rtc_driver = {
 	.driver		= {
 		.name	= "rtc-bfin",
 		.owner	= THIS_MODULE,
+		.pm	= &bfin_rtc_pm_ops,
 	},
 	.probe		= bfin_rtc_probe,
 	.remove		= bfin_rtc_remove,
-	.suspend	= bfin_rtc_suspend,
-	.resume		= bfin_rtc_resume,
 };
 
 module_platform_driver(bfin_rtc_driver);
diff --git a/drivers/rtc/rtc-bq32k.c b/drivers/rtc/rtc-bq32k.c
index 036cb89f818..fea78bc713c 100644
--- a/drivers/rtc/rtc-bq32k.c
+++ b/drivers/rtc/rtc-bq32k.c
@@ -153,7 +153,7 @@ static int bq32k_probe(struct i2c_client *client,
 	if (error)
 		return error;
 
-	rtc = rtc_device_register(bq32k_driver.driver.name, &client->dev,
+	rtc = devm_rtc_device_register(&client->dev, bq32k_driver.driver.name,
 						&bq32k_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc))
 		return PTR_ERR(rtc);
@@ -165,9 +165,6 @@ static int bq32k_probe(struct i2c_client *client,
 
 static int bq32k_remove(struct i2c_client *client)
 {
-	struct rtc_device *rtc = i2c_get_clientdata(client);
-
-	rtc_device_unregister(rtc);
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-bq4802.c b/drivers/rtc/rtc-bq4802.c
index 693be71b5b1..af2886784a7 100644
--- a/drivers/rtc/rtc-bq4802.c
+++ b/drivers/rtc/rtc-bq4802.c
@@ -142,7 +142,7 @@ static const struct rtc_class_ops bq4802_ops = {
 
 static int bq4802_probe(struct platform_device *pdev)
 {
-	struct bq4802 *p = kzalloc(sizeof(*p), GFP_KERNEL);
+	struct bq4802 *p = devm_kzalloc(&pdev->dev, sizeof(*p), GFP_KERNEL);
 	int err = -ENOMEM;
 
 	if (!p)
@@ -155,54 +155,41 @@ static int bq4802_probe(struct platform_device *pdev)
 		p->r = platform_get_resource(pdev, IORESOURCE_IO, 0);
 		err = -EINVAL;
 		if (!p->r)
-			goto out_free;
+			goto out;
 	}
 	if (p->r->flags & IORESOURCE_IO) {
 		p->ioport = p->r->start;
 		p->read = bq4802_read_io;
 		p->write = bq4802_write_io;
 	} else if (p->r->flags & IORESOURCE_MEM) {
-		p->regs = ioremap(p->r->start, resource_size(p->r));
+		p->regs = devm_ioremap(&pdev->dev, p->r->start,
+					resource_size(p->r));
 		p->read = bq4802_read_mem;
 		p->write = bq4802_write_mem;
 	} else {
 		err = -EINVAL;
-		goto out_free;
+		goto out;
 	}
 
 	platform_set_drvdata(pdev, p);
 
-	p->rtc = rtc_device_register("bq4802", &pdev->dev,
-				     &bq4802_ops, THIS_MODULE);
+	p->rtc = devm_rtc_device_register(&pdev->dev, "bq4802",
+					&bq4802_ops, THIS_MODULE);
 	if (IS_ERR(p->rtc)) {
 		err = PTR_ERR(p->rtc);
-		goto out_iounmap;
+		goto out;
 	}
 
 	err = 0;
 out:
 	return err;
 
-out_iounmap:
-	if (p->r->flags & IORESOURCE_MEM)
-		iounmap(p->regs);
-out_free:
-	kfree(p);
-	goto out;
 }
 
 static int bq4802_remove(struct platform_device *pdev)
 {
-	struct bq4802 *p = platform_get_drvdata(pdev);
-
-	rtc_device_unregister(p->rtc);
-	if (p->r->flags & IORESOURCE_MEM)
-		iounmap(p->regs);
-
 	platform_set_drvdata(pdev, NULL);
 
-	kfree(p);
-
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-cmos.c b/drivers/rtc/rtc-cmos.c
index af97c94e8a3..cc5bea9c4b1 100644
--- a/drivers/rtc/rtc-cmos.c
+++ b/drivers/rtc/rtc-cmos.c
@@ -804,9 +804,8 @@ static int cmos_suspend(struct device *dev)
 			mask = RTC_IRQMASK;
 		tmp &= ~mask;
 		CMOS_WRITE(tmp, RTC_CONTROL);
+		hpet_mask_rtc_irq_bit(mask);
 
-		/* shut down hpet emulation - we don't need it for alarm */
-		hpet_mask_rtc_irq_bit(RTC_PIE|RTC_AIE|RTC_UIE);
 		cmos_checkintr(cmos, tmp);
 	}
 	spin_unlock_irq(&rtc_lock);
@@ -870,6 +869,7 @@ static int cmos_resume(struct device *dev)
 			rtc_update_irq(cmos->rtc, 1, mask);
 			tmp &= ~RTC_AIE;
 			hpet_mask_rtc_irq_bit(RTC_AIE);
+			hpet_rtc_timer_init();
 		} while (mask & RTC_AIE);
 		spin_unlock_irq(&rtc_lock);
 	}
diff --git a/drivers/rtc/rtc-coh901331.c b/drivers/rtc/rtc-coh901331.c
index 2d28ec1aa1c..93c06588ddc 100644
--- a/drivers/rtc/rtc-coh901331.c
+++ b/drivers/rtc/rtc-coh901331.c
@@ -47,7 +47,7 @@ struct coh901331_port {
 	u32 physize;
 	void __iomem *virtbase;
 	int irq;
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	u32 irqmaskstore;
 #endif
 };
@@ -155,7 +155,6 @@ static int __exit coh901331_remove(struct platform_device *pdev)
 	struct coh901331_port *rtap = dev_get_drvdata(&pdev->dev);
 
 	if (rtap) {
-		rtc_device_unregister(rtap->rtc);
 		clk_unprepare(rtap->clk);
 		platform_set_drvdata(pdev, NULL);
 	}
@@ -211,8 +210,8 @@ static int __init coh901331_probe(struct platform_device *pdev)
 	clk_disable(rtap->clk);
 
 	platform_set_drvdata(pdev, rtap);
-	rtap->rtc = rtc_device_register("coh901331", &pdev->dev, &coh901331_ops,
-					 THIS_MODULE);
+	rtap->rtc = devm_rtc_device_register(&pdev->dev, "coh901331",
+					&coh901331_ops, THIS_MODULE);
 	if (IS_ERR(rtap->rtc)) {
 		ret = PTR_ERR(rtap->rtc);
 		goto out_no_rtc;
@@ -226,17 +225,17 @@ static int __init coh901331_probe(struct platform_device *pdev)
 	return ret;
 }
 
-#ifdef CONFIG_PM
-static int coh901331_suspend(struct platform_device *pdev, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int coh901331_suspend(struct device *dev)
 {
-	struct coh901331_port *rtap = dev_get_drvdata(&pdev->dev);
+	struct coh901331_port *rtap = dev_get_drvdata(dev);
 
 	/*
 	 * If this RTC alarm will be used for waking the system up,
 	 * don't disable it of course. Else we just disable the alarm
 	 * and await suspension.
 	 */
-	if (device_may_wakeup(&pdev->dev)) {
+	if (device_may_wakeup(dev)) {
 		enable_irq_wake(rtap->irq);
 	} else {
 		clk_enable(rtap->clk);
@@ -248,12 +247,12 @@ static int coh901331_suspend(struct platform_device *pdev, pm_message_t state)
 	return 0;
 }
 
-static int coh901331_resume(struct platform_device *pdev)
+static int coh901331_resume(struct device *dev)
 {
-	struct coh901331_port *rtap = dev_get_drvdata(&pdev->dev);
+	struct coh901331_port *rtap = dev_get_drvdata(dev);
 
 	clk_prepare(rtap->clk);
-	if (device_may_wakeup(&pdev->dev)) {
+	if (device_may_wakeup(dev)) {
 		disable_irq_wake(rtap->irq);
 	} else {
 		clk_enable(rtap->clk);
@@ -262,11 +261,10 @@ static int coh901331_resume(struct platform_device *pdev)
 	}
 	return 0;
 }
-#else
-#define coh901331_suspend NULL
-#define coh901331_resume NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(coh901331_pm_ops, coh901331_suspend, coh901331_resume);
+
 static void coh901331_shutdown(struct platform_device *pdev)
 {
 	struct coh901331_port *rtap = dev_get_drvdata(&pdev->dev);
@@ -280,25 +278,13 @@ static struct platform_driver coh901331_driver = {
 	.driver = {
 		.name = "rtc-coh901331",
 		.owner = THIS_MODULE,
+		.pm = &coh901331_pm_ops,
 	},
 	.remove = __exit_p(coh901331_remove),
-	.suspend = coh901331_suspend,
-	.resume = coh901331_resume,
 	.shutdown = coh901331_shutdown,
 };
 
-static int __init coh901331_init(void)
-{
-	return platform_driver_probe(&coh901331_driver, coh901331_probe);
-}
-
-static void __exit coh901331_exit(void)
-{
-	platform_driver_unregister(&coh901331_driver);
-}
-
-module_init(coh901331_init);
-module_exit(coh901331_exit);
+module_platform_driver_probe(coh901331_driver, coh901331_probe);
 
 MODULE_AUTHOR("Linus Walleij <linus.walleij@stericsson.com>");
 MODULE_DESCRIPTION("ST-Ericsson AB COH 901 331 RTC Driver");
diff --git a/drivers/rtc/rtc-da9052.c b/drivers/rtc/rtc-da9052.c
index 969abbad7fe..7286b279cf2 100644
--- a/drivers/rtc/rtc-da9052.c
+++ b/drivers/rtc/rtc-da9052.c
@@ -247,7 +247,7 @@ static int da9052_rtc_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	rtc->rtc = rtc_device_register(pdev->name, &pdev->dev,
+	rtc->rtc = devm_rtc_device_register(&pdev->dev, pdev->name,
 				       &da9052_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc->rtc))
 		return PTR_ERR(rtc->rtc);
@@ -257,9 +257,6 @@ static int da9052_rtc_probe(struct platform_device *pdev)
 
 static int da9052_rtc_remove(struct platform_device *pdev)
 {
-	struct da9052_rtc *rtc = pdev->dev.platform_data;
-
-	rtc_device_unregister(rtc->rtc);
 	platform_set_drvdata(pdev, NULL);
 
 	return 0;
diff --git a/drivers/rtc/rtc-da9055.c b/drivers/rtc/rtc-da9055.c
index 8f0dcfedb83..73858ca9709 100644
--- a/drivers/rtc/rtc-da9055.c
+++ b/drivers/rtc/rtc-da9055.c
@@ -294,7 +294,7 @@ static int da9055_rtc_probe(struct platform_device *pdev)
 
 	device_init_wakeup(&pdev->dev, 1);
 
-	rtc->rtc = rtc_device_register(pdev->name, &pdev->dev,
+	rtc->rtc = devm_rtc_device_register(&pdev->dev, pdev->name,
 					&da9055_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc->rtc)) {
 		ret = PTR_ERR(rtc->rtc);
@@ -317,9 +317,6 @@ err_rtc:
 
 static int da9055_rtc_remove(struct platform_device *pdev)
 {
-	struct da9055_rtc *rtc = pdev->dev.platform_data;
-
-	rtc_device_unregister(rtc->rtc);
 	platform_set_drvdata(pdev, NULL);
 
 	return 0;
diff --git a/drivers/rtc/rtc-davinci.c b/drivers/rtc/rtc-davinci.c
index 56b73089bb2..a55048c3e26 100644
--- a/drivers/rtc/rtc-davinci.c
+++ b/drivers/rtc/rtc-davinci.c
@@ -523,7 +523,7 @@ static int __init davinci_rtc_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, davinci_rtc);
 
-	davinci_rtc->rtc = rtc_device_register(pdev->name, &pdev->dev,
+	davinci_rtc->rtc = devm_rtc_device_register(&pdev->dev, pdev->name,
 				    &davinci_rtc_ops, THIS_MODULE);
 	if (IS_ERR(davinci_rtc->rtc)) {
 		ret = PTR_ERR(davinci_rtc->rtc);
@@ -543,7 +543,7 @@ static int __init davinci_rtc_probe(struct platform_device *pdev)
 			  0, "davinci_rtc", davinci_rtc);
 	if (ret < 0) {
 		dev_err(dev, "unable to register davinci RTC interrupt\n");
-		goto fail2;
+		goto fail1;
 	}
 
 	/* Enable interrupts */
@@ -557,14 +557,12 @@ static int __init davinci_rtc_probe(struct platform_device *pdev)
 
 	return 0;
 
-fail2:
-	rtc_device_unregister(davinci_rtc->rtc);
 fail1:
 	platform_set_drvdata(pdev, NULL);
 	return ret;
 }
 
-static int davinci_rtc_remove(struct platform_device *pdev)
+static int __exit davinci_rtc_remove(struct platform_device *pdev)
 {
 	struct davinci_rtc *davinci_rtc = platform_get_drvdata(pdev);
 
@@ -572,8 +570,6 @@ static int davinci_rtc_remove(struct platform_device *pdev)
 
 	rtcif_write(davinci_rtc, 0, PRTCIF_INTEN);
 
-	rtc_device_unregister(davinci_rtc->rtc);
-
 	platform_set_drvdata(pdev, NULL);
 
 	return 0;
@@ -581,24 +577,14 @@ static int davinci_rtc_remove(struct platform_device *pdev)
 
 static struct platform_driver davinci_rtc_driver = {
 	.probe		= davinci_rtc_probe,
-	.remove		= davinci_rtc_remove,
+	.remove		= __exit_p(davinci_rtc_remove),
 	.driver		= {
 		.name = "rtc_davinci",
 		.owner = THIS_MODULE,
 	},
 };
 
-static int __init rtc_init(void)
-{
-	return platform_driver_probe(&davinci_rtc_driver, davinci_rtc_probe);
-}
-module_init(rtc_init);
-
-static void __exit rtc_exit(void)
-{
-	platform_driver_unregister(&davinci_rtc_driver);
-}
-module_exit(rtc_exit);
+module_platform_driver_probe(davinci_rtc_driver, davinci_rtc_probe);
 
 MODULE_AUTHOR("Miguel Aguilar <miguel.aguilar@ridgerun.com>");
 MODULE_DESCRIPTION("Texas Instruments DaVinci PRTC Driver");
diff --git a/drivers/rtc/rtc-dm355evm.c b/drivers/rtc/rtc-dm355evm.c
index b2ed2c94b08..1e1ca63d58a 100644
--- a/drivers/rtc/rtc-dm355evm.c
+++ b/drivers/rtc/rtc-dm355evm.c
@@ -127,8 +127,8 @@ static int dm355evm_rtc_probe(struct platform_device *pdev)
 {
 	struct rtc_device *rtc;
 
-	rtc = rtc_device_register(pdev->name,
-				  &pdev->dev, &dm355evm_rtc_ops, THIS_MODULE);
+	rtc = devm_rtc_device_register(&pdev->dev, pdev->name,
+					&dm355evm_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc)) {
 		dev_err(&pdev->dev, "can't register RTC device, err %ld\n",
 			PTR_ERR(rtc));
@@ -141,9 +141,6 @@ static int dm355evm_rtc_probe(struct platform_device *pdev)
 
 static int dm355evm_rtc_remove(struct platform_device *pdev)
 {
-	struct rtc_device *rtc = platform_get_drvdata(pdev);
-
-	rtc_device_unregister(rtc);
 	platform_set_drvdata(pdev, NULL);
 	return 0;
 }
diff --git a/drivers/rtc/rtc-ds1216.c b/drivers/rtc/rtc-ds1216.c
index 45cd8c9f5a3..c7702b7269f 100644
--- a/drivers/rtc/rtc-ds1216.c
+++ b/drivers/rtc/rtc-ds1216.c
@@ -30,8 +30,6 @@ struct ds1216_regs {
 struct ds1216_priv {
 	struct rtc_device *rtc;
 	void __iomem *ioaddr;
-	size_t size;
-	unsigned long baseaddr;
 };
 
 static const u8 magic[] = {
@@ -144,57 +142,33 @@ static int __init ds1216_rtc_probe(struct platform_device *pdev)
 {
 	struct resource *res;
 	struct ds1216_priv *priv;
-	int ret = 0;
 	u8 dummy[8];
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res)
 		return -ENODEV;
-	priv = kzalloc(sizeof *priv, GFP_KERNEL);
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
 
 	platform_set_drvdata(pdev, priv);
 
-	priv->size = resource_size(res);
-	if (!request_mem_region(res->start, priv->size, pdev->name)) {
-		ret = -EBUSY;
-		goto out;
-	}
-	priv->baseaddr = res->start;
-	priv->ioaddr = ioremap(priv->baseaddr, priv->size);
-	if (!priv->ioaddr) {
-		ret = -ENOMEM;
-		goto out;
-	}
-	priv->rtc = rtc_device_register("ds1216", &pdev->dev,
-				  &ds1216_rtc_ops, THIS_MODULE);
-	if (IS_ERR(priv->rtc)) {
-		ret = PTR_ERR(priv->rtc);
-		goto out;
-	}
+	priv->ioaddr = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(priv->ioaddr))
+		return PTR_ERR(priv->ioaddr);
+
+	priv->rtc = devm_rtc_device_register(&pdev->dev, "ds1216",
+					&ds1216_rtc_ops, THIS_MODULE);
+	if (IS_ERR(priv->rtc))
+		return PTR_ERR(priv->rtc);
 
 	/* dummy read to get clock into a known state */
 	ds1216_read(priv->ioaddr, dummy);
 	return 0;
-
-out:
-	if (priv->ioaddr)
-		iounmap(priv->ioaddr);
-	if (priv->baseaddr)
-		release_mem_region(priv->baseaddr, priv->size);
-	kfree(priv);
-	return ret;
 }
 
 static int __exit ds1216_rtc_remove(struct platform_device *pdev)
 {
-	struct ds1216_priv *priv = platform_get_drvdata(pdev);
-
-	rtc_device_unregister(priv->rtc);
-	iounmap(priv->ioaddr);
-	release_mem_region(priv->baseaddr, priv->size);
-	kfree(priv);
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-ds1286.c b/drivers/rtc/rtc-ds1286.c
index d989412a348..398c96a98fc 100644
--- a/drivers/rtc/rtc-ds1286.c
+++ b/drivers/rtc/rtc-ds1286.c
@@ -25,8 +25,6 @@
 struct ds1286_priv {
 	struct rtc_device *rtc;
 	u32 __iomem *rtcregs;
-	size_t size;
-	unsigned long baseaddr;
 	spinlock_t lock;
 };
 
@@ -270,7 +268,6 @@ static int ds1286_set_time(struct device *dev, struct rtc_time *tm)
 static int ds1286_read_alarm(struct device *dev, struct rtc_wkalrm *alm)
 {
 	struct ds1286_priv *priv = dev_get_drvdata(dev);
-	unsigned char cmd;
 	unsigned long flags;
 
 	/*
@@ -281,7 +278,7 @@ static int ds1286_read_alarm(struct device *dev, struct rtc_wkalrm *alm)
 	alm->time.tm_min = ds1286_rtc_read(priv, RTC_MINUTES_ALARM) & 0x7f;
 	alm->time.tm_hour = ds1286_rtc_read(priv, RTC_HOURS_ALARM)  & 0x1f;
 	alm->time.tm_wday = ds1286_rtc_read(priv, RTC_DAY_ALARM)    & 0x07;
-	cmd = ds1286_rtc_read(priv, RTC_CMD);
+	ds1286_rtc_read(priv, RTC_CMD);
 	spin_unlock_irqrestore(&priv->lock, flags);
 
 	alm->time.tm_min = bcd2bin(alm->time.tm_min);
@@ -334,56 +331,30 @@ static int ds1286_probe(struct platform_device *pdev)
 	struct rtc_device *rtc;
 	struct resource *res;
 	struct ds1286_priv *priv;
-	int ret = 0;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res)
 		return -ENODEV;
-	priv = kzalloc(sizeof(struct ds1286_priv), GFP_KERNEL);
+	priv = devm_kzalloc(&pdev->dev, sizeof(struct ds1286_priv), GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
 
-	priv->size = resource_size(res);
-	if (!request_mem_region(res->start, priv->size, pdev->name)) {
-		ret = -EBUSY;
-		goto out;
-	}
-	priv->baseaddr = res->start;
-	priv->rtcregs = ioremap(priv->baseaddr, priv->size);
-	if (!priv->rtcregs) {
-		ret = -ENOMEM;
-		goto out;
-	}
+	priv->rtcregs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(priv->rtcregs))
+		return PTR_ERR(priv->rtcregs);
+
 	spin_lock_init(&priv->lock);
 	platform_set_drvdata(pdev, priv);
-	rtc = rtc_device_register("ds1286", &pdev->dev,
-				  &ds1286_ops, THIS_MODULE);
-	if (IS_ERR(rtc)) {
-		ret = PTR_ERR(rtc);
-		goto out;
-	}
+	rtc = devm_rtc_device_register(&pdev->dev, "ds1286", &ds1286_ops,
+					THIS_MODULE);
+	if (IS_ERR(rtc))
+		return PTR_ERR(rtc);
 	priv->rtc = rtc;
 	return 0;
-
-out:
-	if (priv->rtc)
-		rtc_device_unregister(priv->rtc);
-	if (priv->rtcregs)
-		iounmap(priv->rtcregs);
-	if (priv->baseaddr)
-		release_mem_region(priv->baseaddr, priv->size);
-	kfree(priv);
-	return ret;
 }
 
 static int ds1286_remove(struct platform_device *pdev)
 {
-	struct ds1286_priv *priv = platform_get_drvdata(pdev);
-
-	rtc_device_unregister(priv->rtc);
-	iounmap(priv->rtcregs);
-	release_mem_region(priv->baseaddr, priv->size);
-	kfree(priv);
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-ds1302.c b/drivers/rtc/rtc-ds1302.c
index fdbcdb289d6..d1395434628 100644
--- a/drivers/rtc/rtc-ds1302.c
+++ b/drivers/rtc/rtc-ds1302.c
@@ -224,7 +224,7 @@ static int __init ds1302_rtc_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	rtc = rtc_device_register("ds1302", &pdev->dev,
+	rtc = devm_rtc_device_register(&pdev->dev, "ds1302",
 					   &ds1302_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc))
 		return PTR_ERR(rtc);
@@ -234,11 +234,8 @@ static int __init ds1302_rtc_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static int ds1302_rtc_remove(struct platform_device *pdev)
+static int __exit ds1302_rtc_remove(struct platform_device *pdev)
 {
-	struct rtc_device *rtc = platform_get_drvdata(pdev);
-
-	rtc_device_unregister(rtc);
 	platform_set_drvdata(pdev, NULL);
 
 	return 0;
@@ -249,21 +246,10 @@ static struct platform_driver ds1302_platform_driver = {
 		.name	= DRV_NAME,
 		.owner	= THIS_MODULE,
 	},
-	.remove		= ds1302_rtc_remove,
+	.remove		= __exit_p(ds1302_rtc_remove),
 };
 
-static int __init ds1302_rtc_init(void)
-{
-	return platform_driver_probe(&ds1302_platform_driver, ds1302_rtc_probe);
-}
-
-static void __exit ds1302_rtc_exit(void)
-{
-	platform_driver_unregister(&ds1302_platform_driver);
-}
-
-module_init(ds1302_rtc_init);
-module_exit(ds1302_rtc_exit);
+module_platform_driver_probe(ds1302_platform_driver, ds1302_rtc_probe);
 
 MODULE_DESCRIPTION("Dallas DS1302 RTC driver");
 MODULE_VERSION(DRV_VERSION);
diff --git a/drivers/rtc/rtc-ds1305.c b/drivers/rtc/rtc-ds1305.c
index b05a6dc9640..bb5f13f6363 100644
--- a/drivers/rtc/rtc-ds1305.c
+++ b/drivers/rtc/rtc-ds1305.c
@@ -619,7 +619,7 @@ static int ds1305_probe(struct spi_device *spi)
 		return -EINVAL;
 
 	/* set up driver data */
-	ds1305 = kzalloc(sizeof *ds1305, GFP_KERNEL);
+	ds1305 = devm_kzalloc(&spi->dev, sizeof(*ds1305), GFP_KERNEL);
 	if (!ds1305)
 		return -ENOMEM;
 	ds1305->spi = spi;
@@ -632,7 +632,7 @@ static int ds1305_probe(struct spi_device *spi)
 	if (status < 0) {
 		dev_dbg(&spi->dev, "can't %s, %d\n",
 				"read", status);
-		goto fail0;
+		return status;
 	}
 
 	dev_dbg(&spi->dev, "ctrl %s: %3ph\n", "read", ds1305->ctrl);
@@ -644,8 +644,7 @@ static int ds1305_probe(struct spi_device *spi)
 	 */
 	if ((ds1305->ctrl[0] & 0x38) != 0 || (ds1305->ctrl[1] & 0xfc) != 0) {
 		dev_dbg(&spi->dev, "RTC chip is not present\n");
-		status = -ENODEV;
-		goto fail0;
+		return -ENODEV;
 	}
 	if (ds1305->ctrl[2] == 0)
 		dev_dbg(&spi->dev, "chip may not be present\n");
@@ -664,7 +663,7 @@ static int ds1305_probe(struct spi_device *spi)
 
 		dev_dbg(&spi->dev, "clear WP --> %d\n", status);
 		if (status < 0)
-			goto fail0;
+			return status;
 	}
 
 	/* on DS1305, maybe start oscillator; like most low power
@@ -718,7 +717,7 @@ static int ds1305_probe(struct spi_device *spi)
 		if (status < 0) {
 			dev_dbg(&spi->dev, "can't %s, %d\n",
 					"write", status);
-			goto fail0;
+			return status;
 		}
 
 		dev_dbg(&spi->dev, "ctrl %s: %3ph\n", "write", ds1305->ctrl);
@@ -730,7 +729,7 @@ static int ds1305_probe(struct spi_device *spi)
 				&value, sizeof value);
 	if (status < 0) {
 		dev_dbg(&spi->dev, "read HOUR --> %d\n", status);
-		goto fail0;
+		return status;
 	}
 
 	ds1305->hr12 = (DS1305_HR_12 & value) != 0;
@@ -738,12 +737,12 @@ static int ds1305_probe(struct spi_device *spi)
 		dev_dbg(&spi->dev, "AM/PM\n");
 
 	/* register RTC ... from here on, ds1305->ctrl needs locking */
-	ds1305->rtc = rtc_device_register("ds1305", &spi->dev,
+	ds1305->rtc = devm_rtc_device_register(&spi->dev, "ds1305",
 			&ds1305_ops, THIS_MODULE);
 	if (IS_ERR(ds1305->rtc)) {
 		status = PTR_ERR(ds1305->rtc);
 		dev_dbg(&spi->dev, "register rtc --> %d\n", status);
-		goto fail0;
+		return status;
 	}
 
 	/* Maybe set up alarm IRQ; be ready to handle it triggering right
@@ -754,12 +753,12 @@ static int ds1305_probe(struct spi_device *spi)
 	 */
 	if (spi->irq) {
 		INIT_WORK(&ds1305->work, ds1305_work);
-		status = request_irq(spi->irq, ds1305_irq,
+		status = devm_request_irq(&spi->dev, spi->irq, ds1305_irq,
 				0, dev_name(&ds1305->rtc->dev), ds1305);
 		if (status < 0) {
 			dev_dbg(&spi->dev, "request_irq %d --> %d\n",
 					spi->irq, status);
-			goto fail1;
+			return status;
 		}
 
 		device_set_wakeup_capable(&spi->dev, 1);
@@ -769,18 +768,10 @@ static int ds1305_probe(struct spi_device *spi)
 	status = sysfs_create_bin_file(&spi->dev.kobj, &nvram);
 	if (status < 0) {
 		dev_dbg(&spi->dev, "register nvram --> %d\n", status);
-		goto fail2;
+		return status;
 	}
 
 	return 0;
-
-fail2:
-	free_irq(spi->irq, ds1305);
-fail1:
-	rtc_device_unregister(ds1305->rtc);
-fail0:
-	kfree(ds1305);
-	return status;
 }
 
 static int ds1305_remove(struct spi_device *spi)
@@ -792,13 +783,11 @@ static int ds1305_remove(struct spi_device *spi)
 	/* carefully shut down irq and workqueue, if present */
 	if (spi->irq) {
 		set_bit(FLAG_EXITING, &ds1305->flags);
-		free_irq(spi->irq, ds1305);
+		devm_free_irq(&spi->dev, spi->irq, ds1305);
 		cancel_work_sync(&ds1305->work);
 	}
 
-	rtc_device_unregister(ds1305->rtc);
 	spi_set_drvdata(spi, NULL);
-	kfree(ds1305);
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-ds1307.c b/drivers/rtc/rtc-ds1307.c
index 970a236b147..b53992ab309 100644
--- a/drivers/rtc/rtc-ds1307.c
+++ b/drivers/rtc/rtc-ds1307.c
@@ -4,6 +4,7 @@
  *  Copyright (C) 2005 James Chapman (ds1337 core)
  *  Copyright (C) 2006 David Brownell
  *  Copyright (C) 2009 Matthias Fuchs (rx8025 support)
+ *  Copyright (C) 2012 Bertrand Achard (nvram access fixes)
  *
  * 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
@@ -196,7 +197,7 @@ static s32 ds1307_read_block_data_once(const struct i2c_client *client,
 static s32 ds1307_read_block_data(const struct i2c_client *client, u8 command,
 				  u8 length, u8 *values)
 {
-	u8 oldvalues[I2C_SMBUS_BLOCK_MAX];
+	u8 oldvalues[255];
 	s32 ret;
 	int tries = 0;
 
@@ -222,7 +223,7 @@ static s32 ds1307_read_block_data(const struct i2c_client *client, u8 command,
 static s32 ds1307_write_block_data(const struct i2c_client *client, u8 command,
 				   u8 length, const u8 *values)
 {
-	u8 currvalues[I2C_SMBUS_BLOCK_MAX];
+	u8 currvalues[255];
 	int tries = 0;
 
 	dev_dbg(&client->dev, "ds1307_write_block_data (length=%d)\n", length);
@@ -250,6 +251,57 @@ static s32 ds1307_write_block_data(const struct i2c_client *client, u8 command,
 
 /*----------------------------------------------------------------------*/
 
+/* These RTC devices are not designed to be connected to a SMbus adapter.
+   SMbus limits block operations length to 32 bytes, whereas it's not
+   limited on I2C buses. As a result, accesses may exceed 32 bytes;
+   in that case, split them into smaller blocks */
+
+static s32 ds1307_native_smbus_write_block_data(const struct i2c_client *client,
+				u8 command, u8 length, const u8 *values)
+{
+	u8 suboffset = 0;
+
+	if (length <= I2C_SMBUS_BLOCK_MAX)
+		return i2c_smbus_write_i2c_block_data(client,
+					command, length, values);
+
+	while (suboffset < length) {
+		s32 retval = i2c_smbus_write_i2c_block_data(client,
+				command + suboffset,
+				min(I2C_SMBUS_BLOCK_MAX, length - suboffset),
+				values + suboffset);
+		if (retval < 0)
+			return retval;
+
+		suboffset += I2C_SMBUS_BLOCK_MAX;
+	}
+	return length;
+}
+
+static s32 ds1307_native_smbus_read_block_data(const struct i2c_client *client,
+				u8 command, u8 length, u8 *values)
+{
+	u8 suboffset = 0;
+
+	if (length <= I2C_SMBUS_BLOCK_MAX)
+		return i2c_smbus_read_i2c_block_data(client,
+					command, length, values);
+
+	while (suboffset < length) {
+		s32 retval = i2c_smbus_read_i2c_block_data(client,
+				command + suboffset,
+				min(I2C_SMBUS_BLOCK_MAX, length - suboffset),
+				values + suboffset);
+		if (retval < 0)
+			return retval;
+
+		suboffset += I2C_SMBUS_BLOCK_MAX;
+	}
+	return length;
+}
+
+/*----------------------------------------------------------------------*/
+
 /*
  * The IRQ logic includes a "real" handler running in IRQ context just
  * long enough to schedule this workqueue entry.   We need a task context
@@ -646,8 +698,8 @@ static int ds1307_probe(struct i2c_client *client,
 
 	buf = ds1307->regs;
 	if (i2c_check_functionality(adapter, I2C_FUNC_SMBUS_I2C_BLOCK)) {
-		ds1307->read_block_data = i2c_smbus_read_i2c_block_data;
-		ds1307->write_block_data = i2c_smbus_write_i2c_block_data;
+		ds1307->read_block_data = ds1307_native_smbus_read_block_data;
+		ds1307->write_block_data = ds1307_native_smbus_write_block_data;
 	} else {
 		ds1307->read_block_data = ds1307_read_block_data;
 		ds1307->write_block_data = ds1307_write_block_data;
@@ -661,7 +713,7 @@ static int ds1307_probe(struct i2c_client *client,
 		tmp = ds1307->read_block_data(ds1307->client,
 				DS1337_REG_CONTROL, 2, buf);
 		if (tmp != 2) {
-			pr_debug("read error %d\n", tmp);
+			dev_dbg(&client->dev, "read error %d\n", tmp);
 			err = -EIO;
 			goto exit_free;
 		}
@@ -700,7 +752,7 @@ static int ds1307_probe(struct i2c_client *client,
 		tmp = i2c_smbus_read_i2c_block_data(ds1307->client,
 				RX8025_REG_CTRL1 << 4 | 0x08, 2, buf);
 		if (tmp != 2) {
-			pr_debug("read error %d\n", tmp);
+			dev_dbg(&client->dev, "read error %d\n", tmp);
 			err = -EIO;
 			goto exit_free;
 		}
@@ -744,7 +796,7 @@ static int ds1307_probe(struct i2c_client *client,
 			tmp = i2c_smbus_read_i2c_block_data(ds1307->client,
 					RX8025_REG_CTRL1 << 4 | 0x08, 2, buf);
 			if (tmp != 2) {
-				pr_debug("read error %d\n", tmp);
+				dev_dbg(&client->dev, "read error %d\n", tmp);
 				err = -EIO;
 				goto exit_free;
 			}
@@ -772,7 +824,7 @@ read_rtc:
 	/* read RTC registers */
 	tmp = ds1307->read_block_data(ds1307->client, ds1307->offset, 8, buf);
 	if (tmp != 8) {
-		pr_debug("read error %d\n", tmp);
+		dev_dbg(&client->dev, "read error %d\n", tmp);
 		err = -EIO;
 		goto exit_free;
 	}
@@ -814,7 +866,7 @@ read_rtc:
 
 		tmp = i2c_smbus_read_byte_data(client, DS1340_REG_FLAG);
 		if (tmp < 0) {
-			pr_debug("read error %d\n", tmp);
+			dev_dbg(&client->dev, "read error %d\n", tmp);
 			err = -EIO;
 			goto exit_free;
 		}
@@ -908,8 +960,8 @@ read_rtc:
 		ds1307->nvram->attr.name = "nvram";
 		ds1307->nvram->attr.mode = S_IRUGO | S_IWUSR;
 		sysfs_bin_attr_init(ds1307->nvram);
-		ds1307->nvram->read = ds1307_nvram_read,
-		ds1307->nvram->write = ds1307_nvram_write,
+		ds1307->nvram->read = ds1307_nvram_read;
+		ds1307->nvram->write = ds1307_nvram_write;
 		ds1307->nvram->size = chip->nvram_size;
 		ds1307->nvram_offset = chip->nvram_offset;
 		err = sysfs_create_bin_file(&client->dev.kobj, ds1307->nvram);
diff --git a/drivers/rtc/rtc-ds1374.c b/drivers/rtc/rtc-ds1374.c
index fef76868aae..94366e12f40 100644
--- a/drivers/rtc/rtc-ds1374.c
+++ b/drivers/rtc/rtc-ds1374.c
@@ -347,7 +347,7 @@ static int ds1374_probe(struct i2c_client *client,
 	struct ds1374 *ds1374;
 	int ret;
 
-	ds1374 = kzalloc(sizeof(struct ds1374), GFP_KERNEL);
+	ds1374 = devm_kzalloc(&client->dev, sizeof(struct ds1374), GFP_KERNEL);
 	if (!ds1374)
 		return -ENOMEM;
 
@@ -359,36 +359,27 @@ static int ds1374_probe(struct i2c_client *client,
 
 	ret = ds1374_check_rtc_status(client);
 	if (ret)
-		goto out_free;
+		return ret;
 
 	if (client->irq > 0) {
-		ret = request_irq(client->irq, ds1374_irq, 0,
+		ret = devm_request_irq(&client->dev, client->irq, ds1374_irq, 0,
 		                  "ds1374", client);
 		if (ret) {
 			dev_err(&client->dev, "unable to request IRQ\n");
-			goto out_free;
+			return ret;
 		}
 
 		device_set_wakeup_capable(&client->dev, 1);
 	}
 
-	ds1374->rtc = rtc_device_register(client->name, &client->dev,
+	ds1374->rtc = devm_rtc_device_register(&client->dev, client->name,
 	                                  &ds1374_rtc_ops, THIS_MODULE);
 	if (IS_ERR(ds1374->rtc)) {
-		ret = PTR_ERR(ds1374->rtc);
 		dev_err(&client->dev, "unable to register the class device\n");
-		goto out_irq;
+		return PTR_ERR(ds1374->rtc);
 	}
 
 	return 0;
-
-out_irq:
-	if (client->irq > 0)
-		free_irq(client->irq, client);
-
-out_free:
-	kfree(ds1374);
-	return ret;
 }
 
 static int ds1374_remove(struct i2c_client *client)
@@ -400,16 +391,14 @@ static int ds1374_remove(struct i2c_client *client)
 		ds1374->exiting = 1;
 		mutex_unlock(&ds1374->mutex);
 
-		free_irq(client->irq, client);
+		devm_free_irq(&client->dev, client->irq, client);
 		cancel_work_sync(&ds1374->work);
 	}
 
-	rtc_device_unregister(ds1374->rtc);
-	kfree(ds1374);
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int ds1374_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
@@ -427,19 +416,15 @@ static int ds1374_resume(struct device *dev)
 		disable_irq_wake(client->irq);
 	return 0;
 }
+#endif
 
 static SIMPLE_DEV_PM_OPS(ds1374_pm, ds1374_suspend, ds1374_resume);
 
-#define DS1374_PM (&ds1374_pm)
-#else
-#define DS1374_PM NULL
-#endif
-
 static struct i2c_driver ds1374_driver = {
 	.driver = {
 		.name = "rtc-ds1374",
 		.owner = THIS_MODULE,
-		.pm = DS1374_PM,
+		.pm = &ds1374_pm,
 	},
 	.probe = ds1374_probe,
 	.remove = ds1374_remove,
diff --git a/drivers/rtc/rtc-ds1390.c b/drivers/rtc/rtc-ds1390.c
index f994257981a..289af419dff 100644
--- a/drivers/rtc/rtc-ds1390.c
+++ b/drivers/rtc/rtc-ds1390.c
@@ -131,26 +131,24 @@ static int ds1390_probe(struct spi_device *spi)
 	spi->bits_per_word = 8;
 	spi_setup(spi);
 
-	chip = kzalloc(sizeof *chip, GFP_KERNEL);
+	chip = devm_kzalloc(&spi->dev, sizeof(*chip), GFP_KERNEL);
 	if (!chip) {
 		dev_err(&spi->dev, "unable to allocate device memory\n");
 		return -ENOMEM;
 	}
-	dev_set_drvdata(&spi->dev, chip);
+	spi_set_drvdata(spi, chip);
 
 	res = ds1390_get_reg(&spi->dev, DS1390_REG_SECONDS, &tmp);
 	if (res != 0) {
 		dev_err(&spi->dev, "unable to read device\n");
-		kfree(chip);
 		return res;
 	}
 
-	chip->rtc = rtc_device_register("ds1390",
-				&spi->dev, &ds1390_rtc_ops, THIS_MODULE);
+	chip->rtc = devm_rtc_device_register(&spi->dev, "ds1390",
+					&ds1390_rtc_ops, THIS_MODULE);
 	if (IS_ERR(chip->rtc)) {
 		dev_err(&spi->dev, "unable to register device\n");
 		res = PTR_ERR(chip->rtc);
-		kfree(chip);
 	}
 
 	return res;
@@ -158,11 +156,6 @@ static int ds1390_probe(struct spi_device *spi)
 
 static int ds1390_remove(struct spi_device *spi)
 {
-	struct ds1390 *chip = spi_get_drvdata(spi);
-
-	rtc_device_unregister(chip->rtc);
-	kfree(chip);
-
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-ds1511.c b/drivers/rtc/rtc-ds1511.c
index 6a3fcfe3b0e..6ce8a997cf5 100644
--- a/drivers/rtc/rtc-ds1511.c
+++ b/drivers/rtc/rtc-ds1511.c
@@ -538,15 +538,14 @@ static int ds1511_rtc_probe(struct platform_device *pdev)
 		}
 	}
 
-	rtc = rtc_device_register(pdev->name, &pdev->dev, &ds1511_rtc_ops,
-		THIS_MODULE);
+	rtc = devm_rtc_device_register(&pdev->dev, pdev->name, &ds1511_rtc_ops,
+					THIS_MODULE);
 	if (IS_ERR(rtc))
 		return PTR_ERR(rtc);
 	pdata->rtc = rtc;
 
 	ret = sysfs_create_bin_file(&pdev->dev.kobj, &ds1511_nvram_attr);
-	if (ret)
-		rtc_device_unregister(pdata->rtc);
+
 	return ret;
 }
 
@@ -555,7 +554,6 @@ static int ds1511_rtc_remove(struct platform_device *pdev)
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 
 	sysfs_remove_bin_file(&pdev->dev.kobj, &ds1511_nvram_attr);
-	rtc_device_unregister(pdata->rtc);
 	if (pdata->irq > 0) {
 		/*
 		 * disable the alarm interrupt
diff --git a/drivers/rtc/rtc-ds1553.c b/drivers/rtc/rtc-ds1553.c
index 25ce0621ade..8c6c952e90b 100644
--- a/drivers/rtc/rtc-ds1553.c
+++ b/drivers/rtc/rtc-ds1553.c
@@ -326,15 +326,14 @@ static int ds1553_rtc_probe(struct platform_device *pdev)
 		}
 	}
 
-	rtc = rtc_device_register(pdev->name, &pdev->dev,
+	rtc = devm_rtc_device_register(&pdev->dev, pdev->name,
 				  &ds1553_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc))
 		return PTR_ERR(rtc);
 	pdata->rtc = rtc;
 
 	ret = sysfs_create_bin_file(&pdev->dev.kobj, &ds1553_nvram_attr);
-	if (ret)
-		rtc_device_unregister(rtc);
+
 	return ret;
 }
 
@@ -343,7 +342,6 @@ static int ds1553_rtc_remove(struct platform_device *pdev)
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 
 	sysfs_remove_bin_file(&pdev->dev.kobj, &ds1553_nvram_attr);
-	rtc_device_unregister(pdata->rtc);
 	if (pdata->irq > 0)
 		writeb(0, pdata->ioaddr + RTC_INTERRUPTS);
 	return 0;
diff --git a/drivers/rtc/rtc-ds1672.c b/drivers/rtc/rtc-ds1672.c
index 45d65c0b3a8..3fc2a473802 100644
--- a/drivers/rtc/rtc-ds1672.c
+++ b/drivers/rtc/rtc-ds1672.c
@@ -155,11 +155,6 @@ static const struct rtc_class_ops ds1672_rtc_ops = {
 
 static int ds1672_remove(struct i2c_client *client)
 {
-	struct rtc_device *rtc = i2c_get_clientdata(client);
-
-	if (rtc)
-		rtc_device_unregister(rtc);
-
 	return 0;
 }
 
@@ -177,7 +172,7 @@ static int ds1672_probe(struct i2c_client *client,
 
 	dev_info(&client->dev, "chip found, driver version " DRV_VERSION "\n");
 
-	rtc = rtc_device_register(ds1672_driver.driver.name, &client->dev,
+	rtc = devm_rtc_device_register(&client->dev, ds1672_driver.driver.name,
 				  &ds1672_rtc_ops, THIS_MODULE);
 
 	if (IS_ERR(rtc))
@@ -202,7 +197,6 @@ static int ds1672_probe(struct i2c_client *client,
 	return 0;
 
  exit_devreg:
-	rtc_device_unregister(rtc);
 	return err;
 }
 
diff --git a/drivers/rtc/rtc-ds1742.c b/drivers/rtc/rtc-ds1742.c
index 609c870e2cc..eccdc62ae1c 100644
--- a/drivers/rtc/rtc-ds1742.c
+++ b/drivers/rtc/rtc-ds1742.c
@@ -208,17 +208,14 @@ static int ds1742_rtc_probe(struct platform_device *pdev)
 
 	pdata->last_jiffies = jiffies;
 	platform_set_drvdata(pdev, pdata);
-	rtc = rtc_device_register(pdev->name, &pdev->dev,
+	rtc = devm_rtc_device_register(&pdev->dev, pdev->name,
 				  &ds1742_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc))
 		return PTR_ERR(rtc);
 	pdata->rtc = rtc;
 
 	ret = sysfs_create_bin_file(&pdev->dev.kobj, &pdata->nvram_attr);
-	if (ret) {
-		dev_err(&pdev->dev, "creating nvram file in sysfs failed\n");
-		rtc_device_unregister(rtc);
-	}
+
 	return ret;
 }
 
@@ -227,7 +224,6 @@ static int ds1742_rtc_remove(struct platform_device *pdev)
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 
 	sysfs_remove_bin_file(&pdev->dev.kobj, &pdata->nvram_attr);
-	rtc_device_unregister(pdata->rtc);
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-ds2404.c b/drivers/rtc/rtc-ds2404.c
index b04fc4272fb..2ca5a23aba8 100644
--- a/drivers/rtc/rtc-ds2404.c
+++ b/drivers/rtc/rtc-ds2404.c
@@ -228,7 +228,7 @@ static int rtc_probe(struct platform_device *pdev)
 	struct ds2404 *chip;
 	int retval = -EBUSY;
 
-	chip = kzalloc(sizeof(struct ds2404), GFP_KERNEL);
+	chip = devm_kzalloc(&pdev->dev, sizeof(struct ds2404), GFP_KERNEL);
 	if (!chip)
 		return -ENOMEM;
 
@@ -244,8 +244,8 @@ static int rtc_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, chip);
 
-	chip->rtc = rtc_device_register("ds2404",
-				&pdev->dev, &ds2404_rtc_ops, THIS_MODULE);
+	chip->rtc = devm_rtc_device_register(&pdev->dev, "ds2404",
+					&ds2404_rtc_ops, THIS_MODULE);
 	if (IS_ERR(chip->rtc)) {
 		retval = PTR_ERR(chip->rtc);
 		goto err_io;
@@ -257,20 +257,14 @@ static int rtc_probe(struct platform_device *pdev)
 err_io:
 	chip->ops->unmap_io(chip);
 err_chip:
-	kfree(chip);
 	return retval;
 }
 
 static int rtc_remove(struct platform_device *dev)
 {
 	struct ds2404 *chip = platform_get_drvdata(dev);
-	struct rtc_device *rtc = chip->rtc;
-
-	if (rtc)
-		rtc_device_unregister(rtc);
 
 	chip->ops->unmap_io(chip);
-	kfree(chip);
 
 	return 0;
 }
diff --git a/drivers/rtc/rtc-ds3232.c b/drivers/rtc/rtc-ds3232.c
index db0ca08db31..b83bb5a527f 100644
--- a/drivers/rtc/rtc-ds3232.c
+++ b/drivers/rtc/rtc-ds3232.c
@@ -397,7 +397,7 @@ static int ds3232_probe(struct i2c_client *client,
 	struct ds3232 *ds3232;
 	int ret;
 
-	ds3232 = kzalloc(sizeof(struct ds3232), GFP_KERNEL);
+	ds3232 = devm_kzalloc(&client->dev, sizeof(struct ds3232), GFP_KERNEL);
 	if (!ds3232)
 		return -ENOMEM;
 
@@ -409,34 +409,25 @@ static int ds3232_probe(struct i2c_client *client,
 
 	ret = ds3232_check_rtc_status(client);
 	if (ret)
-		goto out_free;
+		return ret;
 
-	ds3232->rtc = rtc_device_register(client->name, &client->dev,
+	ds3232->rtc = devm_rtc_device_register(&client->dev, client->name,
 					  &ds3232_rtc_ops, THIS_MODULE);
 	if (IS_ERR(ds3232->rtc)) {
-		ret = PTR_ERR(ds3232->rtc);
 		dev_err(&client->dev, "unable to register the class device\n");
-		goto out_irq;
+		return PTR_ERR(ds3232->rtc);
 	}
 
 	if (client->irq >= 0) {
-		ret = request_irq(client->irq, ds3232_irq, 0,
+		ret = devm_request_irq(&client->dev, client->irq, ds3232_irq, 0,
 				 "ds3232", client);
 		if (ret) {
 			dev_err(&client->dev, "unable to request IRQ\n");
-			goto out_free;
+			return ret;
 		}
 	}
 
 	return 0;
-
-out_irq:
-	if (client->irq >= 0)
-		free_irq(client->irq, client);
-
-out_free:
-	kfree(ds3232);
-	return ret;
 }
 
 static int ds3232_remove(struct i2c_client *client)
@@ -448,12 +439,10 @@ static int ds3232_remove(struct i2c_client *client)
 		ds3232->exiting = 1;
 		mutex_unlock(&ds3232->mutex);
 
-		free_irq(client->irq, client);
+		devm_free_irq(&client->dev, client->irq, client);
 		cancel_work_sync(&ds3232->work);
 	}
 
-	rtc_device_unregister(ds3232->rtc);
-	kfree(ds3232);
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-ds3234.c b/drivers/rtc/rtc-ds3234.c
index 7a4495ef1c3..ba98c0e9580 100644
--- a/drivers/rtc/rtc-ds3234.c
+++ b/drivers/rtc/rtc-ds3234.c
@@ -146,21 +146,18 @@ static int ds3234_probe(struct spi_device *spi)
 	ds3234_get_reg(&spi->dev, DS3234_REG_CONT_STAT, &tmp);
 	dev_info(&spi->dev, "Ctrl/Stat Reg: 0x%02x\n", tmp);
 
-	rtc = rtc_device_register("ds3234",
-				&spi->dev, &ds3234_rtc_ops, THIS_MODULE);
+	rtc = devm_rtc_device_register(&spi->dev, "ds3234",
+				&ds3234_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc))
 		return PTR_ERR(rtc);
 
-	dev_set_drvdata(&spi->dev, rtc);
+	spi_set_drvdata(spi, rtc);
 
 	return 0;
 }
 
 static int ds3234_remove(struct spi_device *spi)
 {
-	struct rtc_device *rtc = spi_get_drvdata(spi);
-
-	rtc_device_unregister(rtc);
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-efi.c b/drivers/rtc/rtc-efi.c
index 1a0c37c9152..b3c8c0b1709 100644
--- a/drivers/rtc/rtc-efi.c
+++ b/drivers/rtc/rtc-efi.c
@@ -191,7 +191,7 @@ static int __init efi_rtc_probe(struct platform_device *dev)
 {
 	struct rtc_device *rtc;
 
-	rtc = rtc_device_register("rtc-efi", &dev->dev, &efi_rtc_ops,
+	rtc = devm_rtc_device_register(&dev->dev, "rtc-efi", &efi_rtc_ops,
 					THIS_MODULE);
 	if (IS_ERR(rtc))
 		return PTR_ERR(rtc);
@@ -203,10 +203,6 @@ static int __init efi_rtc_probe(struct platform_device *dev)
 
 static int __exit efi_rtc_remove(struct platform_device *dev)
 {
-	struct rtc_device *rtc = platform_get_drvdata(dev);
-
-	rtc_device_unregister(rtc);
-
 	return 0;
 }
 
@@ -218,18 +214,7 @@ static struct platform_driver efi_rtc_driver = {
 	.remove = __exit_p(efi_rtc_remove),
 };
 
-static int __init efi_rtc_init(void)
-{
-	return platform_driver_probe(&efi_rtc_driver, efi_rtc_probe);
-}
-
-static void __exit efi_rtc_exit(void)
-{
-	platform_driver_unregister(&efi_rtc_driver);
-}
-
-module_init(efi_rtc_init);
-module_exit(efi_rtc_exit);
+module_platform_driver_probe(efi_rtc_driver, efi_rtc_probe);
 
 MODULE_AUTHOR("dann frazier <dannf@hp.com>");
 MODULE_LICENSE("GPL");
diff --git a/drivers/rtc/rtc-em3027.c b/drivers/rtc/rtc-em3027.c
index f6c24ce35d3..3f9eb57d048 100644
--- a/drivers/rtc/rtc-em3027.c
+++ b/drivers/rtc/rtc-em3027.c
@@ -121,7 +121,7 @@ static int em3027_probe(struct i2c_client *client,
 	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C))
 		return -ENODEV;
 
-	rtc = rtc_device_register(em3027_driver.driver.name, &client->dev,
+	rtc = devm_rtc_device_register(&client->dev, em3027_driver.driver.name,
 				  &em3027_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc))
 		return PTR_ERR(rtc);
@@ -133,11 +133,6 @@ static int em3027_probe(struct i2c_client *client,
 
 static int em3027_remove(struct i2c_client *client)
 {
-	struct rtc_device *rtc = i2c_get_clientdata(client);
-
-	if (rtc)
-		rtc_device_unregister(rtc);
-
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-ep93xx.c b/drivers/rtc/rtc-ep93xx.c
index 1a4e5e4a70c..5807b77c444 100644
--- a/drivers/rtc/rtc-ep93xx.c
+++ b/drivers/rtc/rtc-ep93xx.c
@@ -153,8 +153,8 @@ static int ep93xx_rtc_probe(struct platform_device *pdev)
 	pdev->dev.platform_data = ep93xx_rtc;
 	platform_set_drvdata(pdev, ep93xx_rtc);
 
-	ep93xx_rtc->rtc = rtc_device_register(pdev->name,
-				&pdev->dev, &ep93xx_rtc_ops, THIS_MODULE);
+	ep93xx_rtc->rtc = devm_rtc_device_register(&pdev->dev,
+				pdev->name, &ep93xx_rtc_ops, THIS_MODULE);
 	if (IS_ERR(ep93xx_rtc->rtc)) {
 		err = PTR_ERR(ep93xx_rtc->rtc);
 		goto exit;
@@ -162,12 +162,10 @@ static int ep93xx_rtc_probe(struct platform_device *pdev)
 
 	err = sysfs_create_group(&pdev->dev.kobj, &ep93xx_rtc_sysfs_files);
 	if (err)
-		goto fail;
+		goto exit;
 
 	return 0;
 
-fail:
-	rtc_device_unregister(ep93xx_rtc->rtc);
 exit:
 	platform_set_drvdata(pdev, NULL);
 	pdev->dev.platform_data = NULL;
@@ -176,11 +174,8 @@ exit:
 
 static int ep93xx_rtc_remove(struct platform_device *pdev)
 {
-	struct ep93xx_rtc *ep93xx_rtc = platform_get_drvdata(pdev);
-
 	sysfs_remove_group(&pdev->dev.kobj, &ep93xx_rtc_sysfs_files);
 	platform_set_drvdata(pdev, NULL);
-	rtc_device_unregister(ep93xx_rtc->rtc);
 	pdev->dev.platform_data = NULL;
 
 	return 0;
diff --git a/drivers/rtc/rtc-fm3130.c b/drivers/rtc/rtc-fm3130.c
index bff3cdc5140..2835fb6c196 100644
--- a/drivers/rtc/rtc-fm3130.c
+++ b/drivers/rtc/rtc-fm3130.c
@@ -358,7 +358,7 @@ static int fm3130_probe(struct i2c_client *client,
 			I2C_FUNC_I2C | I2C_FUNC_SMBUS_WRITE_BYTE_DATA))
 		return -EIO;
 
-	fm3130 = kzalloc(sizeof(struct fm3130), GFP_KERNEL);
+	fm3130 = devm_kzalloc(&client->dev, sizeof(struct fm3130), GFP_KERNEL);
 
 	if (!fm3130)
 		return -ENOMEM;
@@ -395,7 +395,7 @@ static int fm3130_probe(struct i2c_client *client,
 
 	tmp = i2c_transfer(adapter, fm3130->msg, 4);
 	if (tmp != 4) {
-		pr_debug("read error %d\n", tmp);
+		dev_dbg(&client->dev, "read error %d\n", tmp);
 		err = -EIO;
 		goto exit_free;
 	}
@@ -507,7 +507,7 @@ bad_clock:
 
 	/* We won't bail out here because we just got invalid data.
 	   Time setting from u-boot doesn't work anyway */
-	fm3130->rtc = rtc_device_register(client->name, &client->dev,
+	fm3130->rtc = devm_rtc_device_register(&client->dev, client->name,
 				&fm3130_rtc_ops, THIS_MODULE);
 	if (IS_ERR(fm3130->rtc)) {
 		err = PTR_ERR(fm3130->rtc);
@@ -517,16 +517,11 @@ bad_clock:
 	}
 	return 0;
 exit_free:
-	kfree(fm3130);
 	return err;
 }
 
 static int fm3130_remove(struct i2c_client *client)
 {
-	struct fm3130 *fm3130 = i2c_get_clientdata(client);
-
-	rtc_device_unregister(fm3130->rtc);
-	kfree(fm3130);
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-generic.c b/drivers/rtc/rtc-generic.c
index 98322004ad2..06279ce6bff 100644
--- a/drivers/rtc/rtc-generic.c
+++ b/drivers/rtc/rtc-generic.c
@@ -38,8 +38,8 @@ static int __init generic_rtc_probe(struct platform_device *dev)
 {
 	struct rtc_device *rtc;
 
-	rtc = rtc_device_register("rtc-generic", &dev->dev, &generic_rtc_ops,
-				  THIS_MODULE);
+	rtc = devm_rtc_device_register(&dev->dev, "rtc-generic",
+					&generic_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc))
 		return PTR_ERR(rtc);
 
@@ -50,10 +50,6 @@ static int __init generic_rtc_probe(struct platform_device *dev)
 
 static int __exit generic_rtc_remove(struct platform_device *dev)
 {
-	struct rtc_device *rtc = platform_get_drvdata(dev);
-
-	rtc_device_unregister(rtc);
-
 	return 0;
 }
 
@@ -65,18 +61,7 @@ static struct platform_driver generic_rtc_driver = {
 	.remove = __exit_p(generic_rtc_remove),
 };
 
-static int __init generic_rtc_init(void)
-{
-	return platform_driver_probe(&generic_rtc_driver, generic_rtc_probe);
-}
-
-static void __exit generic_rtc_fini(void)
-{
-	platform_driver_unregister(&generic_rtc_driver);
-}
-
-module_init(generic_rtc_init);
-module_exit(generic_rtc_fini);
+module_platform_driver_probe(generic_rtc_driver, generic_rtc_probe);
 
 MODULE_AUTHOR("Kyle McMartin <kyle@mcmartin.ca>");
 MODULE_LICENSE("GPL");
diff --git a/drivers/rtc/rtc-hid-sensor-time.c b/drivers/rtc/rtc-hid-sensor-time.c
index 31c5728ef62..63024505ddd 100644
--- a/drivers/rtc/rtc-hid-sensor-time.c
+++ b/drivers/rtc/rtc-hid-sensor-time.c
@@ -255,8 +255,9 @@ static int hid_time_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	time_state->rtc = rtc_device_register("hid-sensor-time",
-				&pdev->dev, &hid_time_rtc_ops, THIS_MODULE);
+	time_state->rtc = devm_rtc_device_register(&pdev->dev,
+					"hid-sensor-time", &hid_time_rtc_ops,
+					THIS_MODULE);
 
 	if (IS_ERR(time_state->rtc)) {
 		dev_err(&pdev->dev, "rtc device register failed!\n");
@@ -269,9 +270,7 @@ static int hid_time_probe(struct platform_device *pdev)
 static int hid_time_remove(struct platform_device *pdev)
 {
 	struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data;
-	struct hid_time_state *time_state = platform_get_drvdata(pdev);
 
-	rtc_device_unregister(time_state->rtc);
 	sensor_hub_remove_callback(hsdev, HID_USAGE_SENSOR_TIME);
 
 	return 0;
diff --git a/drivers/rtc/rtc-imxdi.c b/drivers/rtc/rtc-imxdi.c
index 82aad695979..d3a8c8e255d 100644
--- a/drivers/rtc/rtc-imxdi.c
+++ b/drivers/rtc/rtc-imxdi.c
@@ -369,7 +369,7 @@ static void dryice_work(struct work_struct *work)
 /*
  * probe for dryice rtc device
  */
-static int dryice_rtc_probe(struct platform_device *pdev)
+static int __init dryice_rtc_probe(struct platform_device *pdev)
 {
 	struct resource *res;
 	struct imxdi_dev *imxdi;
@@ -464,7 +464,7 @@ static int dryice_rtc_probe(struct platform_device *pdev)
 	}
 
 	platform_set_drvdata(pdev, imxdi);
-	imxdi->rtc = rtc_device_register(pdev->name, &pdev->dev,
+	imxdi->rtc = devm_rtc_device_register(&pdev->dev, pdev->name,
 				  &dryice_rtc_ops, THIS_MODULE);
 	if (IS_ERR(imxdi->rtc)) {
 		rc = PTR_ERR(imxdi->rtc);
@@ -479,7 +479,7 @@ err:
 	return rc;
 }
 
-static int dryice_rtc_remove(struct platform_device *pdev)
+static int __exit dryice_rtc_remove(struct platform_device *pdev)
 {
 	struct imxdi_dev *imxdi = platform_get_drvdata(pdev);
 
@@ -488,8 +488,6 @@ static int dryice_rtc_remove(struct platform_device *pdev)
 	/* mask all interrupts */
 	__raw_writel(0, imxdi->ioaddr + DIER);
 
-	rtc_device_unregister(imxdi->rtc);
-
 	clk_disable_unprepare(imxdi->clk);
 
 	return 0;
@@ -510,21 +508,10 @@ static struct platform_driver dryice_rtc_driver = {
 		   .owner = THIS_MODULE,
 		   .of_match_table = of_match_ptr(dryice_dt_ids),
 		   },
-	.remove = dryice_rtc_remove,
+	.remove = __exit_p(dryice_rtc_remove),
 };
 
-static int __init dryice_rtc_init(void)
-{
-	return platform_driver_probe(&dryice_rtc_driver, dryice_rtc_probe);
-}
-
-static void __exit dryice_rtc_exit(void)
-{
-	platform_driver_unregister(&dryice_rtc_driver);
-}
-
-module_init(dryice_rtc_init);
-module_exit(dryice_rtc_exit);
+module_platform_driver_probe(dryice_rtc_driver, dryice_rtc_probe);
 
 MODULE_AUTHOR("Freescale Semiconductor, Inc.");
 MODULE_AUTHOR("Baruch Siach <baruch@tkos.co.il>");
diff --git a/drivers/rtc/rtc-isl12022.c b/drivers/rtc/rtc-isl12022.c
index 6b4298ea683..a1bbbb8de02 100644
--- a/drivers/rtc/rtc-isl12022.c
+++ b/drivers/rtc/rtc-isl12022.c
@@ -252,12 +252,11 @@ static int isl12022_probe(struct i2c_client *client,
 {
 	struct isl12022 *isl12022;
 
-	int ret = 0;
-
 	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C))
 		return -ENODEV;
 
-	isl12022 = kzalloc(sizeof(struct isl12022), GFP_KERNEL);
+	isl12022 = devm_kzalloc(&client->dev, sizeof(struct isl12022),
+				GFP_KERNEL);
 	if (!isl12022)
 		return -ENOMEM;
 
@@ -265,37 +264,22 @@ static int isl12022_probe(struct i2c_client *client,
 
 	i2c_set_clientdata(client, isl12022);
 
-	isl12022->rtc = rtc_device_register(isl12022_driver.driver.name,
-					    &client->dev,
-					    &isl12022_rtc_ops,
-					    THIS_MODULE);
-
-	if (IS_ERR(isl12022->rtc)) {
-		ret = PTR_ERR(isl12022->rtc);
-		goto exit_kfree;
-	}
+	isl12022->rtc = devm_rtc_device_register(&client->dev,
+					isl12022_driver.driver.name,
+					&isl12022_rtc_ops, THIS_MODULE);
+	if (IS_ERR(isl12022->rtc))
+		return PTR_ERR(isl12022->rtc);
 
 	return 0;
-
-exit_kfree:
-	kfree(isl12022);
-
-	return ret;
 }
 
 static int isl12022_remove(struct i2c_client *client)
 {
-	struct isl12022 *isl12022 = i2c_get_clientdata(client);
-
-	rtc_device_unregister(isl12022->rtc);
-	kfree(isl12022);
-
 	return 0;
 }
 
 static const struct i2c_device_id isl12022_id[] = {
 	{ "isl12022", 0 },
-	{ "rtc8564", 0 },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, isl12022_id);
diff --git a/drivers/rtc/rtc-lp8788.c b/drivers/rtc/rtc-lp8788.c
index 9a4631218f4..9853ac15b29 100644
--- a/drivers/rtc/rtc-lp8788.c
+++ b/drivers/rtc/rtc-lp8788.c
@@ -299,7 +299,7 @@ static int lp8788_rtc_probe(struct platform_device *pdev)
 
 	device_init_wakeup(dev, 1);
 
-	rtc->rdev = rtc_device_register("lp8788_rtc", dev,
+	rtc->rdev = devm_rtc_device_register(dev, "lp8788_rtc",
 					&lp8788_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc->rdev)) {
 		dev_err(dev, "can not register rtc device\n");
@@ -314,9 +314,6 @@ static int lp8788_rtc_probe(struct platform_device *pdev)
 
 static int lp8788_rtc_remove(struct platform_device *pdev)
 {
-	struct lp8788_rtc *rtc = platform_get_drvdata(pdev);
-
-	rtc_device_unregister(rtc->rdev);
 	platform_set_drvdata(pdev, NULL);
 
 	return 0;
diff --git a/drivers/rtc/rtc-lpc32xx.c b/drivers/rtc/rtc-lpc32xx.c
index 40a598332ba..787550d756e 100644
--- a/drivers/rtc/rtc-lpc32xx.c
+++ b/drivers/rtc/rtc-lpc32xx.c
@@ -273,8 +273,8 @@ static int lpc32xx_rtc_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, rtc);
 
-	rtc->rtc = rtc_device_register(RTC_NAME, &pdev->dev, &lpc32xx_rtc_ops,
-		THIS_MODULE);
+	rtc->rtc = devm_rtc_device_register(&pdev->dev, RTC_NAME,
+					&lpc32xx_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc->rtc)) {
 		dev_err(&pdev->dev, "Can't get RTC\n");
 		platform_set_drvdata(pdev, NULL);
@@ -307,7 +307,6 @@ static int lpc32xx_rtc_remove(struct platform_device *pdev)
 		device_init_wakeup(&pdev->dev, 0);
 
 	platform_set_drvdata(pdev, NULL);
-	rtc_device_unregister(rtc->rtc);
 
 	return 0;
 }
diff --git a/drivers/rtc/rtc-ls1x.c b/drivers/rtc/rtc-ls1x.c
index f59b6349551..db82f91f456 100644
--- a/drivers/rtc/rtc-ls1x.c
+++ b/drivers/rtc/rtc-ls1x.c
@@ -172,7 +172,7 @@ static int ls1x_rtc_probe(struct platform_device *pdev)
 	while (readl(SYS_COUNTER_CNTRL) & SYS_CNTRL_TTS)
 		usleep_range(1000, 3000);
 
-	rtcdev = rtc_device_register("ls1x-rtc", &pdev->dev,
+	rtcdev = devm_rtc_device_register(&pdev->dev, "ls1x-rtc",
 					&ls1x_rtc_ops , THIS_MODULE);
 	if (IS_ERR(rtcdev)) {
 		ret = PTR_ERR(rtcdev);
@@ -187,9 +187,6 @@ err:
 
 static int ls1x_rtc_remove(struct platform_device *pdev)
 {
-	struct rtc_device *rtcdev = platform_get_drvdata(pdev);
-
-	rtc_device_unregister(rtcdev);
 	platform_set_drvdata(pdev, NULL);
 
 	return 0;
diff --git a/drivers/rtc/rtc-m41t80.c b/drivers/rtc/rtc-m41t80.c
index b885bcd0890..89674b5e6ef 100644
--- a/drivers/rtc/rtc-m41t80.c
+++ b/drivers/rtc/rtc-m41t80.c
@@ -637,7 +637,8 @@ static int m41t80_probe(struct i2c_client *client,
 	dev_info(&client->dev,
 		 "chip found, driver version " DRV_VERSION "\n");
 
-	clientdata = kzalloc(sizeof(*clientdata), GFP_KERNEL);
+	clientdata = devm_kzalloc(&client->dev, sizeof(*clientdata),
+				GFP_KERNEL);
 	if (!clientdata) {
 		rc = -ENOMEM;
 		goto exit;
@@ -646,8 +647,8 @@ static int m41t80_probe(struct i2c_client *client,
 	clientdata->features = id->driver_data;
 	i2c_set_clientdata(client, clientdata);
 
-	rtc = rtc_device_register(client->name, &client->dev,
-				  &m41t80_rtc_ops, THIS_MODULE);
+	rtc = devm_rtc_device_register(&client->dev, client->name,
+					&m41t80_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc)) {
 		rc = PTR_ERR(rtc);
 		rtc = NULL;
@@ -718,26 +719,19 @@ ht_err:
 	goto exit;
 
 exit:
-	if (rtc)
-		rtc_device_unregister(rtc);
-	kfree(clientdata);
 	return rc;
 }
 
 static int m41t80_remove(struct i2c_client *client)
 {
+#ifdef CONFIG_RTC_DRV_M41T80_WDT
 	struct m41t80_data *clientdata = i2c_get_clientdata(client);
-	struct rtc_device *rtc = clientdata->rtc;
 
-#ifdef CONFIG_RTC_DRV_M41T80_WDT
 	if (clientdata->features & M41T80_FEATURE_HT) {
 		misc_deregister(&wdt_dev);
 		unregister_reboot_notifier(&wdt_notifier);
 	}
 #endif
-	if (rtc)
-		rtc_device_unregister(rtc);
-	kfree(clientdata);
 
 	return 0;
 }
diff --git a/drivers/rtc/rtc-m41t93.c b/drivers/rtc/rtc-m41t93.c
index 49169680786..9707d36e8b1 100644
--- a/drivers/rtc/rtc-m41t93.c
+++ b/drivers/rtc/rtc-m41t93.c
@@ -184,12 +184,12 @@ static int m41t93_probe(struct spi_device *spi)
 		return -ENODEV;
 	}
 
-	rtc = rtc_device_register(m41t93_driver.driver.name,
-		&spi->dev, &m41t93_rtc_ops, THIS_MODULE);
+	rtc = devm_rtc_device_register(&spi->dev, m41t93_driver.driver.name,
+					&m41t93_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc))
 		return PTR_ERR(rtc);
 
-	dev_set_drvdata(&spi->dev, rtc);
+	spi_set_drvdata(spi, rtc);
 
 	return 0;
 }
@@ -197,11 +197,6 @@ static int m41t93_probe(struct spi_device *spi)
 
 static int m41t93_remove(struct spi_device *spi)
 {
-	struct rtc_device *rtc = spi_get_drvdata(spi);
-
-	if (rtc)
-		rtc_device_unregister(rtc);
-
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-m41t94.c b/drivers/rtc/rtc-m41t94.c
index 89266c6764b..7454ef0a4cf 100644
--- a/drivers/rtc/rtc-m41t94.c
+++ b/drivers/rtc/rtc-m41t94.c
@@ -124,23 +124,18 @@ static int m41t94_probe(struct spi_device *spi)
 		return res;
 	}
 
-	rtc = rtc_device_register(m41t94_driver.driver.name,
-		&spi->dev, &m41t94_rtc_ops, THIS_MODULE);
+	rtc = devm_rtc_device_register(&spi->dev, m41t94_driver.driver.name,
+					&m41t94_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc))
 		return PTR_ERR(rtc);
 
-	dev_set_drvdata(&spi->dev, rtc);
+	spi_set_drvdata(spi, rtc);
 
 	return 0;
 }
 
 static int m41t94_remove(struct spi_device *spi)
 {
-	struct rtc_device *rtc = spi_get_drvdata(spi);
-
-	if (rtc)
-		rtc_device_unregister(rtc);
-
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-m48t35.c b/drivers/rtc/rtc-m48t35.c
index 31c9190a1fc..37444246e5e 100644
--- a/drivers/rtc/rtc-m48t35.c
+++ b/drivers/rtc/rtc-m48t35.c
@@ -145,12 +145,11 @@ static int m48t35_probe(struct platform_device *pdev)
 {
 	struct resource *res;
 	struct m48t35_priv *priv;
-	int ret = 0;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res)
 		return -ENODEV;
-	priv = kzalloc(sizeof(struct m48t35_priv), GFP_KERNEL);
+	priv = devm_kzalloc(&pdev->dev, sizeof(struct m48t35_priv), GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
 
@@ -160,50 +159,29 @@ static int m48t35_probe(struct platform_device *pdev)
 	 * conflicts are resolved
 	 */
 #ifndef CONFIG_SGI_IP27
-	if (!request_mem_region(res->start, priv->size, pdev->name)) {
-		ret = -EBUSY;
-		goto out;
-	}
+	if (!devm_request_mem_region(&pdev->dev, res->start, priv->size,
+				     pdev->name))
+		return -EBUSY;
 #endif
 	priv->baseaddr = res->start;
-	priv->reg = ioremap(priv->baseaddr, priv->size);
-	if (!priv->reg) {
-		ret = -ENOMEM;
-		goto out;
-	}
+	priv->reg = devm_ioremap(&pdev->dev, priv->baseaddr, priv->size);
+	if (!priv->reg)
+		return -ENOMEM;
 
 	spin_lock_init(&priv->lock);
 
 	platform_set_drvdata(pdev, priv);
 
-	priv->rtc = rtc_device_register("m48t35", &pdev->dev,
+	priv->rtc = devm_rtc_device_register(&pdev->dev, "m48t35",
 				  &m48t35_ops, THIS_MODULE);
-	if (IS_ERR(priv->rtc)) {
-		ret = PTR_ERR(priv->rtc);
-		goto out;
-	}
+	if (IS_ERR(priv->rtc))
+		return PTR_ERR(priv->rtc);
 
 	return 0;
-
-out:
-	if (priv->reg)
-		iounmap(priv->reg);
-	if (priv->baseaddr)
-		release_mem_region(priv->baseaddr, priv->size);
-	kfree(priv);
-	return ret;
 }
 
 static int m48t35_remove(struct platform_device *pdev)
 {
-	struct m48t35_priv *priv = platform_get_drvdata(pdev);
-
-	rtc_device_unregister(priv->rtc);
-	iounmap(priv->reg);
-#ifndef CONFIG_SGI_IP27
-	release_mem_region(priv->baseaddr, priv->size);
-#endif
-	kfree(priv);
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-m48t86.c b/drivers/rtc/rtc-m48t86.c
index 2ffbcacd243..33a91c48453 100644
--- a/drivers/rtc/rtc-m48t86.c
+++ b/drivers/rtc/rtc-m48t86.c
@@ -148,8 +148,10 @@ static int m48t86_rtc_probe(struct platform_device *dev)
 {
 	unsigned char reg;
 	struct m48t86_ops *ops = dev->dev.platform_data;
-	struct rtc_device *rtc = rtc_device_register("m48t86",
-				&dev->dev, &m48t86_rtc_ops, THIS_MODULE);
+	struct rtc_device *rtc;
+
+	rtc = devm_rtc_device_register(&dev->dev, "m48t86",
+				&m48t86_rtc_ops, THIS_MODULE);
 
 	if (IS_ERR(rtc))
 		return PTR_ERR(rtc);
@@ -166,11 +168,6 @@ static int m48t86_rtc_probe(struct platform_device *dev)
 
 static int m48t86_rtc_remove(struct platform_device *dev)
 {
-	struct rtc_device *rtc = platform_get_drvdata(dev);
-
- 	if (rtc)
-		rtc_device_unregister(rtc);
-
 	platform_set_drvdata(dev, NULL);
 
 	return 0;
diff --git a/drivers/rtc/rtc-max6900.c b/drivers/rtc/rtc-max6900.c
index a00e33204b9..8669d6d09a0 100644
--- a/drivers/rtc/rtc-max6900.c
+++ b/drivers/rtc/rtc-max6900.c
@@ -214,11 +214,6 @@ static int max6900_rtc_set_time(struct device *dev, struct rtc_time *tm)
 
 static int max6900_remove(struct i2c_client *client)
 {
-	struct rtc_device *rtc = i2c_get_clientdata(client);
-
-	if (rtc)
-		rtc_device_unregister(rtc);
-
 	return 0;
 }
 
@@ -237,8 +232,8 @@ max6900_probe(struct i2c_client *client, const struct i2c_device_id *id)
 
 	dev_info(&client->dev, "chip found, driver version " DRV_VERSION "\n");
 
-	rtc = rtc_device_register(max6900_driver.driver.name,
-				  &client->dev, &max6900_rtc_ops, THIS_MODULE);
+	rtc = devm_rtc_device_register(&client->dev, max6900_driver.driver.name,
+					&max6900_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc))
 		return PTR_ERR(rtc);
 
diff --git a/drivers/rtc/rtc-max6902.c b/drivers/rtc/rtc-max6902.c
index 7d0bf698b79..e3aea00c314 100644
--- a/drivers/rtc/rtc-max6902.c
+++ b/drivers/rtc/rtc-max6902.c
@@ -93,24 +93,24 @@ static int max6902_set_time(struct device *dev, struct rtc_time *dt)
 	dt->tm_year = dt->tm_year + 1900;
 
 	/* Remove write protection */
-	max6902_set_reg(dev, 0xF, 0);
+	max6902_set_reg(dev, MAX6902_REG_CONTROL, 0);
 
-	max6902_set_reg(dev, 0x01, bin2bcd(dt->tm_sec));
-	max6902_set_reg(dev, 0x03, bin2bcd(dt->tm_min));
-	max6902_set_reg(dev, 0x05, bin2bcd(dt->tm_hour));
+	max6902_set_reg(dev, MAX6902_REG_SECONDS, bin2bcd(dt->tm_sec));
+	max6902_set_reg(dev, MAX6902_REG_MINUTES, bin2bcd(dt->tm_min));
+	max6902_set_reg(dev, MAX6902_REG_HOURS, bin2bcd(dt->tm_hour));
 
-	max6902_set_reg(dev, 0x07, bin2bcd(dt->tm_mday));
-	max6902_set_reg(dev, 0x09, bin2bcd(dt->tm_mon + 1));
-	max6902_set_reg(dev, 0x0B, bin2bcd(dt->tm_wday));
-	max6902_set_reg(dev, 0x0D, bin2bcd(dt->tm_year % 100));
-	max6902_set_reg(dev, 0x13, bin2bcd(dt->tm_year / 100));
+	max6902_set_reg(dev, MAX6902_REG_DATE, bin2bcd(dt->tm_mday));
+	max6902_set_reg(dev, MAX6902_REG_MONTH, bin2bcd(dt->tm_mon + 1));
+	max6902_set_reg(dev, MAX6902_REG_DAY, bin2bcd(dt->tm_wday));
+	max6902_set_reg(dev, MAX6902_REG_YEAR, bin2bcd(dt->tm_year % 100));
+	max6902_set_reg(dev, MAX6902_REG_CENTURY, bin2bcd(dt->tm_year / 100));
 
 	/* Compulab used a delay here. However, the datasheet
 	 * does not mention a delay being required anywhere... */
 	/* delay(2000); */
 
 	/* Write protect */
-	max6902_set_reg(dev, 0xF, 0x80);
+	max6902_set_reg(dev, MAX6902_REG_CONTROL, 0x80);
 
 	return 0;
 }
@@ -134,20 +134,17 @@ static int max6902_probe(struct spi_device *spi)
 	if (res != 0)
 		return res;
 
-	rtc = rtc_device_register("max6902",
-				&spi->dev, &max6902_rtc_ops, THIS_MODULE);
+	rtc = devm_rtc_device_register(&spi->dev, "max6902",
+				&max6902_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc))
 		return PTR_ERR(rtc);
 
-	dev_set_drvdata(&spi->dev, rtc);
+	spi_set_drvdata(spi, rtc);
 	return 0;
 }
 
 static int max6902_remove(struct spi_device *spi)
 {
-	struct rtc_device *rtc = dev_get_drvdata(&spi->dev);
-
-	rtc_device_unregister(rtc);
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-max77686.c b/drivers/rtc/rtc-max77686.c
index 6b1337f9baf..771812d62e6 100644
--- a/drivers/rtc/rtc-max77686.c
+++ b/drivers/rtc/rtc-max77686.c
@@ -24,7 +24,7 @@
 
 /* RTC Control Register */
 #define BCD_EN_SHIFT			0
-#define BCD_EN_MASK				(1 << BCD_EN_SHIFT)
+#define BCD_EN_MASK			(1 << BCD_EN_SHIFT)
 #define MODEL24_SHIFT			1
 #define MODEL24_MASK			(1 << MODEL24_SHIFT)
 /* RTC Update Register1 */
@@ -33,12 +33,12 @@
 #define RTC_RBUDR_SHIFT			4
 #define RTC_RBUDR_MASK			(1 << RTC_RBUDR_SHIFT)
 /* WTSR and SMPL Register */
-#define WTSRT_SHIFT				0
-#define SMPLT_SHIFT				2
+#define WTSRT_SHIFT			0
+#define SMPLT_SHIFT			2
 #define WTSR_EN_SHIFT			6
 #define SMPL_EN_SHIFT			7
-#define WTSRT_MASK				(3 << WTSRT_SHIFT)
-#define SMPLT_MASK				(3 << SMPLT_SHIFT)
+#define WTSRT_MASK			(3 << WTSRT_SHIFT)
+#define SMPLT_MASK			(3 << SMPLT_SHIFT)
 #define WTSR_EN_MASK			(1 << WTSR_EN_SHIFT)
 #define SMPL_EN_MASK			(1 << SMPL_EN_SHIFT)
 /* RTC Hour register */
@@ -466,7 +466,7 @@ static void max77686_rtc_enable_smpl(struct max77686_rtc_info *info, bool enable
 
 	val = 0;
 	regmap_read(info->max77686->rtc_regmap, MAX77686_WTSR_SMPL_CNTL, &val);
-	pr_info("%s: WTSR_SMPL(0x%02x)\n", __func__, val);
+	dev_info(info->dev, "%s: WTSR_SMPL(0x%02x)\n", __func__, val);
 }
 #endif /* MAX77686_RTC_WTSR_SMPL */
 
@@ -505,7 +505,8 @@ static int max77686_rtc_probe(struct platform_device *pdev)
 
 	dev_info(&pdev->dev, "%s\n", __func__);
 
-	info = kzalloc(sizeof(struct max77686_rtc_info), GFP_KERNEL);
+	info = devm_kzalloc(&pdev->dev, sizeof(struct max77686_rtc_info),
+				GFP_KERNEL);
 	if (!info)
 		return -ENOMEM;
 
@@ -513,13 +514,12 @@ static int max77686_rtc_probe(struct platform_device *pdev)
 	info->dev = &pdev->dev;
 	info->max77686 = max77686;
 	info->rtc = max77686->rtc;
-	info->max77686->rtc_regmap = regmap_init_i2c(info->max77686->rtc,
+	info->max77686->rtc_regmap = devm_regmap_init_i2c(info->max77686->rtc,
 					 &max77686_rtc_regmap_config);
 	if (IS_ERR(info->max77686->rtc_regmap)) {
 		ret = PTR_ERR(info->max77686->rtc_regmap);
 		dev_err(info->max77686->dev, "Failed to allocate register map: %d\n",
 				ret);
-		kfree(info);
 		return ret;
 	}
 	platform_set_drvdata(pdev, info);
@@ -538,8 +538,8 @@ static int max77686_rtc_probe(struct platform_device *pdev)
 
 	device_init_wakeup(&pdev->dev, 1);
 
-	info->rtc_dev = rtc_device_register("max77686-rtc", &pdev->dev,
-			&max77686_rtc_ops, THIS_MODULE);
+	info->rtc_dev = devm_rtc_device_register(&pdev->dev, "max77686-rtc",
+					&max77686_rtc_ops, THIS_MODULE);
 
 	if (IS_ERR(info->rtc_dev)) {
 		dev_info(&pdev->dev, "%s: fail\n", __func__);
@@ -551,36 +551,24 @@ static int max77686_rtc_probe(struct platform_device *pdev)
 		goto err_rtc;
 	}
 	virq = irq_create_mapping(max77686->irq_domain, MAX77686_RTCIRQ_RTCA1);
-	if (!virq)
+	if (!virq) {
+		ret = -ENXIO;
 		goto err_rtc;
+	}
 	info->virq = virq;
 
-	ret = request_threaded_irq(virq, NULL, max77686_rtc_alarm_irq, 0,
-			"rtc-alarm0", info);
-	if (ret < 0) {
+	ret = devm_request_threaded_irq(&pdev->dev, virq, NULL,
+				max77686_rtc_alarm_irq, 0, "rtc-alarm0", info);
+	if (ret < 0)
 		dev_err(&pdev->dev, "Failed to request alarm IRQ: %d: %d\n",
 			info->virq, ret);
-		goto err_rtc;
-	}
 
-	goto out;
 err_rtc:
-	kfree(info);
-	return ret;
-out:
 	return ret;
 }
 
 static int max77686_rtc_remove(struct platform_device *pdev)
 {
-	struct max77686_rtc_info *info = platform_get_drvdata(pdev);
-
-	if (info) {
-		free_irq(info->virq, info);
-		rtc_device_unregister(info->rtc_dev);
-		kfree(info);
-	}
-
 	return 0;
 }
 
@@ -594,11 +582,14 @@ static void max77686_rtc_shutdown(struct platform_device *pdev)
 	for (i = 0; i < 3; i++) {
 		max77686_rtc_enable_wtsr(info, false);
 		regmap_read(info->max77686->rtc_regmap, MAX77686_WTSR_SMPL_CNTL, &val);
-		pr_info("%s: WTSR_SMPL reg(0x%02x)\n", __func__, val);
-		if (val & WTSR_EN_MASK)
-			pr_emerg("%s: fail to disable WTSR\n", __func__);
-		else {
-			pr_info("%s: success to disable WTSR\n", __func__);
+		dev_info(info->dev, "%s: WTSR_SMPL reg(0x%02x)\n", __func__,
+				val);
+		if (val & WTSR_EN_MASK) {
+			dev_emerg(info->dev, "%s: fail to disable WTSR\n",
+					__func__);
+		} else {
+			dev_info(info->dev, "%s: success to disable WTSR\n",
+					__func__);
 			break;
 		}
 	}
@@ -624,18 +615,8 @@ static struct platform_driver max77686_rtc_driver = {
 	.id_table	= rtc_id,
 };
 
-static int __init max77686_rtc_init(void)
-{
-	return platform_driver_register(&max77686_rtc_driver);
-}
-module_init(max77686_rtc_init);
-
-static void __exit max77686_rtc_exit(void)
-{
-	platform_driver_unregister(&max77686_rtc_driver);
-}
-module_exit(max77686_rtc_exit);
+module_platform_driver(max77686_rtc_driver);
 
 MODULE_DESCRIPTION("Maxim MAX77686 RTC driver");
-MODULE_AUTHOR("<woong.byun@samsung.com>");
+MODULE_AUTHOR("Chiwoong Byun <woong.byun@samsung.com>");
 MODULE_LICENSE("GPL");
diff --git a/drivers/rtc/rtc-max8907.c b/drivers/rtc/rtc-max8907.c
index 31ca8faf9f0..86afb797125 100644
--- a/drivers/rtc/rtc-max8907.c
+++ b/drivers/rtc/rtc-max8907.c
@@ -190,7 +190,7 @@ static int max8907_rtc_probe(struct platform_device *pdev)
 	rtc->max8907 = max8907;
 	rtc->regmap = max8907->regmap_rtc;
 
-	rtc->rtc_dev = rtc_device_register("max8907-rtc", &pdev->dev,
+	rtc->rtc_dev = devm_rtc_device_register(&pdev->dev, "max8907-rtc",
 					&max8907_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc->rtc_dev)) {
 		ret = PTR_ERR(rtc->rtc_dev);
@@ -200,33 +200,21 @@ static int max8907_rtc_probe(struct platform_device *pdev)
 
 	rtc->irq = regmap_irq_get_virq(max8907->irqc_rtc,
 				       MAX8907_IRQ_RTC_ALARM0);
-	if (rtc->irq < 0) {
-		ret = rtc->irq;
-		goto err_unregister;
-	}
+	if (rtc->irq < 0)
+		return rtc->irq;
 
 	ret = devm_request_threaded_irq(&pdev->dev, rtc->irq, NULL,
 				max8907_irq_handler,
 				IRQF_ONESHOT, "max8907-alarm0", rtc);
-	if (ret < 0) {
+	if (ret < 0)
 		dev_err(&pdev->dev, "Failed to request IRQ%d: %d\n",
 			rtc->irq, ret);
-		goto err_unregister;
-	}
 
-	return 0;
-
-err_unregister:
-	rtc_device_unregister(rtc->rtc_dev);
 	return ret;
 }
 
 static int max8907_rtc_remove(struct platform_device *pdev)
 {
-	struct max8907_rtc *rtc = platform_get_drvdata(pdev);
-
-	rtc_device_unregister(rtc->rtc_dev);
-
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-max8925.c b/drivers/rtc/rtc-max8925.c
index a0c8265646d..7c90f4e45e2 100644
--- a/drivers/rtc/rtc-max8925.c
+++ b/drivers/rtc/rtc-max8925.c
@@ -253,7 +253,8 @@ static int max8925_rtc_probe(struct platform_device *pdev)
 	struct max8925_rtc_info *info;
 	int ret;
 
-	info = kzalloc(sizeof(struct max8925_rtc_info), GFP_KERNEL);
+	info = devm_kzalloc(&pdev->dev, sizeof(struct max8925_rtc_info),
+			    GFP_KERNEL);
 	if (!info)
 		return -ENOMEM;
 	info->chip = chip;
@@ -261,12 +262,13 @@ static int max8925_rtc_probe(struct platform_device *pdev)
 	info->dev = &pdev->dev;
 	info->irq = platform_get_irq(pdev, 0);
 
-	ret = request_threaded_irq(info->irq, NULL, rtc_update_handler,
-				   IRQF_ONESHOT, "rtc-alarm0", info);
+	ret = devm_request_threaded_irq(&pdev->dev, info->irq, NULL,
+					rtc_update_handler, IRQF_ONESHOT,
+					"rtc-alarm0", info);
 	if (ret < 0) {
 		dev_err(chip->dev, "Failed to request IRQ: #%d: %d\n",
 			info->irq, ret);
-		goto out_irq;
+		goto err;
 	}
 
 	dev_set_drvdata(&pdev->dev, info);
@@ -275,32 +277,22 @@ static int max8925_rtc_probe(struct platform_device *pdev)
 
 	device_init_wakeup(&pdev->dev, 1);
 
-	info->rtc_dev = rtc_device_register("max8925-rtc", &pdev->dev,
+	info->rtc_dev = devm_rtc_device_register(&pdev->dev, "max8925-rtc",
 					&max8925_rtc_ops, THIS_MODULE);
 	ret = PTR_ERR(info->rtc_dev);
 	if (IS_ERR(info->rtc_dev)) {
 		dev_err(&pdev->dev, "Failed to register RTC device: %d\n", ret);
-		goto out_rtc;
+		goto err;
 	}
 
 	return 0;
-out_rtc:
+err:
 	platform_set_drvdata(pdev, NULL);
-	free_irq(info->irq, info);
-out_irq:
-	kfree(info);
 	return ret;
 }
 
 static int max8925_rtc_remove(struct platform_device *pdev)
 {
-	struct max8925_rtc_info *info = platform_get_drvdata(pdev);
-
-	if (info) {
-		free_irq(info->irq, info);
-		rtc_device_unregister(info->rtc_dev);
-		kfree(info);
-	}
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-max8997.c b/drivers/rtc/rtc-max8997.c
index 00e505b6bee..dacf48db792 100644
--- a/drivers/rtc/rtc-max8997.c
+++ b/drivers/rtc/rtc-max8997.c
@@ -24,7 +24,7 @@
 /* Module parameter for WTSR function control */
 static int wtsr_en = 1;
 module_param(wtsr_en, int, 0444);
-MODULE_PARM_DESC(wtsr_en, "Wachdog Timeout & Sofware Reset (default=on)");
+MODULE_PARM_DESC(wtsr_en, "Watchdog Timeout & Software Reset (default=on)");
 /* Module parameter for SMPL function control */
 static int smpl_en = 1;
 module_param(smpl_en, int, 0444);
@@ -479,8 +479,8 @@ static int max8997_rtc_probe(struct platform_device *pdev)
 
 	device_init_wakeup(&pdev->dev, 1);
 
-	info->rtc_dev = rtc_device_register("max8997-rtc", &pdev->dev,
-			&max8997_rtc_ops, THIS_MODULE);
+	info->rtc_dev = devm_rtc_device_register(&pdev->dev, "max8997-rtc",
+					&max8997_rtc_ops, THIS_MODULE);
 
 	if (IS_ERR(info->rtc_dev)) {
 		ret = PTR_ERR(info->rtc_dev);
@@ -491,6 +491,7 @@ static int max8997_rtc_probe(struct platform_device *pdev)
 	virq = irq_create_mapping(max8997->irq_domain, MAX8997_PMICIRQ_RTCA1);
 	if (!virq) {
 		dev_err(&pdev->dev, "Failed to create mapping alarm IRQ\n");
+		ret = -ENXIO;
 		goto err_out;
 	}
 	info->virq = virq;
@@ -498,26 +499,16 @@ static int max8997_rtc_probe(struct platform_device *pdev)
 	ret = devm_request_threaded_irq(&pdev->dev, virq, NULL,
 				max8997_rtc_alarm_irq, 0,
 				"rtc-alarm0", info);
-	if (ret < 0) {
+	if (ret < 0)
 		dev_err(&pdev->dev, "Failed to request alarm IRQ: %d: %d\n",
 			info->virq, ret);
-		goto err_out;
-	}
-
-	return ret;
 
 err_out:
-	rtc_device_unregister(info->rtc_dev);
 	return ret;
 }
 
 static int max8997_rtc_remove(struct platform_device *pdev)
 {
-	struct max8997_rtc_info *info = platform_get_drvdata(pdev);
-
-	if (info)
-		rtc_device_unregister(info->rtc_dev);
-
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-max8998.c b/drivers/rtc/rtc-max8998.c
index 8f234a075e8..48b6612fae7 100644
--- a/drivers/rtc/rtc-max8998.c
+++ b/drivers/rtc/rtc-max8998.c
@@ -256,7 +256,8 @@ static int max8998_rtc_probe(struct platform_device *pdev)
 	struct max8998_rtc_info *info;
 	int ret;
 
-	info = kzalloc(sizeof(struct max8998_rtc_info), GFP_KERNEL);
+	info = devm_kzalloc(&pdev->dev, sizeof(struct max8998_rtc_info),
+			GFP_KERNEL);
 	if (!info)
 		return -ENOMEM;
 
@@ -267,7 +268,7 @@ static int max8998_rtc_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, info);
 
-	info->rtc_dev = rtc_device_register("max8998-rtc", &pdev->dev,
+	info->rtc_dev = devm_rtc_device_register(&pdev->dev, "max8998-rtc",
 			&max8998_rtc_ops, THIS_MODULE);
 
 	if (IS_ERR(info->rtc_dev)) {
@@ -276,8 +277,8 @@ static int max8998_rtc_probe(struct platform_device *pdev)
 		goto out_rtc;
 	}
 
-	ret = request_threaded_irq(info->irq, NULL, max8998_rtc_alarm_irq, 0,
-			"rtc-alarm0", info);
+	ret = devm_request_threaded_irq(&pdev->dev, info->irq, NULL,
+				max8998_rtc_alarm_irq, 0, "rtc-alarm0", info);
 
 	if (ret < 0)
 		dev_err(&pdev->dev, "Failed to request alarm IRQ: %d: %d\n",
@@ -294,20 +295,11 @@ static int max8998_rtc_probe(struct platform_device *pdev)
 
 out_rtc:
 	platform_set_drvdata(pdev, NULL);
-	kfree(info);
 	return ret;
 }
 
 static int max8998_rtc_remove(struct platform_device *pdev)
 {
-	struct max8998_rtc_info *info = platform_get_drvdata(pdev);
-
-	if (info) {
-		free_irq(info->irq, info);
-		rtc_device_unregister(info->rtc_dev);
-		kfree(info);
-	}
-
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-mc13xxx.c b/drivers/rtc/rtc-mc13xxx.c
index 2643d887492..7a8ed27a5f2 100644
--- a/drivers/rtc/rtc-mc13xxx.c
+++ b/drivers/rtc/rtc-mc13xxx.c
@@ -316,7 +316,7 @@ static int __init mc13xxx_rtc_probe(struct platform_device *pdev)
 	struct mc13xxx *mc13xxx;
 	int rtcrst_pending;
 
-	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
 
@@ -351,8 +351,8 @@ static int __init mc13xxx_rtc_probe(struct platform_device *pdev)
 
 	mc13xxx_unlock(mc13xxx);
 
-	priv->rtc = rtc_device_register(pdev->name,
-			&pdev->dev, &mc13xxx_rtc_ops, THIS_MODULE);
+	priv->rtc = devm_rtc_device_register(&pdev->dev, pdev->name,
+					&mc13xxx_rtc_ops, THIS_MODULE);
 	if (IS_ERR(priv->rtc)) {
 		ret = PTR_ERR(priv->rtc);
 
@@ -372,7 +372,6 @@ err_reset_irq_request:
 		mc13xxx_unlock(mc13xxx);
 
 		platform_set_drvdata(pdev, NULL);
-		kfree(priv);
 	}
 
 	return ret;
@@ -384,8 +383,6 @@ static int __exit mc13xxx_rtc_remove(struct platform_device *pdev)
 
 	mc13xxx_lock(priv->mc13xxx);
 
-	rtc_device_unregister(priv->rtc);
-
 	mc13xxx_irq_free(priv->mc13xxx, MC13XXX_IRQ_TODA, priv);
 	mc13xxx_irq_free(priv->mc13xxx, MC13XXX_IRQ_1HZ, priv);
 	mc13xxx_irq_free(priv->mc13xxx, MC13XXX_IRQ_RTCRST, priv);
@@ -394,8 +391,6 @@ static int __exit mc13xxx_rtc_remove(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, NULL);
 
-	kfree(priv);
-
 	return 0;
 }
 
@@ -420,17 +415,7 @@ static struct platform_driver mc13xxx_rtc_driver = {
 	},
 };
 
-static int __init mc13xxx_rtc_init(void)
-{
-	return platform_driver_probe(&mc13xxx_rtc_driver, &mc13xxx_rtc_probe);
-}
-module_init(mc13xxx_rtc_init);
-
-static void __exit mc13xxx_rtc_exit(void)
-{
-	platform_driver_unregister(&mc13xxx_rtc_driver);
-}
-module_exit(mc13xxx_rtc_exit);
+module_platform_driver_probe(mc13xxx_rtc_driver, &mc13xxx_rtc_probe);
 
 MODULE_AUTHOR("Sascha Hauer <s.hauer@pengutronix.de>");
 MODULE_DESCRIPTION("RTC driver for Freescale MC13XXX PMIC");
diff --git a/drivers/rtc/rtc-msm6242.c b/drivers/rtc/rtc-msm6242.c
index fcb113c1112..771f86a05d1 100644
--- a/drivers/rtc/rtc-msm6242.c
+++ b/drivers/rtc/rtc-msm6242.c
@@ -194,30 +194,28 @@ static const struct rtc_class_ops msm6242_rtc_ops = {
 	.set_time	= msm6242_set_time,
 };
 
-static int __init msm6242_rtc_probe(struct platform_device *dev)
+static int __init msm6242_rtc_probe(struct platform_device *pdev)
 {
 	struct resource *res;
 	struct msm6242_priv *priv;
 	struct rtc_device *rtc;
 	int error;
 
-	res = platform_get_resource(dev, IORESOURCE_MEM, 0);
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res)
 		return -ENODEV;
 
-	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
 
-	priv->regs = ioremap(res->start, resource_size(res));
-	if (!priv->regs) {
-		error = -ENOMEM;
-		goto out_free_priv;
-	}
-	platform_set_drvdata(dev, priv);
+	priv->regs = devm_ioremap(&pdev->dev, res->start, resource_size(res));
+	if (!priv->regs)
+		return -ENOMEM;
+	platform_set_drvdata(pdev, priv);
 
-	rtc = rtc_device_register("rtc-msm6242", &dev->dev, &msm6242_rtc_ops,
-				  THIS_MODULE);
+	rtc = devm_rtc_device_register(&pdev->dev, "rtc-msm6242",
+				&msm6242_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc)) {
 		error = PTR_ERR(rtc);
 		goto out_unmap;
@@ -227,20 +225,12 @@ static int __init msm6242_rtc_probe(struct platform_device *dev)
 	return 0;
 
 out_unmap:
-	platform_set_drvdata(dev, NULL);
-	iounmap(priv->regs);
-out_free_priv:
-	kfree(priv);
+	platform_set_drvdata(pdev, NULL);
 	return error;
 }
 
-static int __exit msm6242_rtc_remove(struct platform_device *dev)
+static int __exit msm6242_rtc_remove(struct platform_device *pdev)
 {
-	struct msm6242_priv *priv = platform_get_drvdata(dev);
-
-	rtc_device_unregister(priv->rtc);
-	iounmap(priv->regs);
-	kfree(priv);
 	return 0;
 }
 
@@ -252,18 +242,7 @@ static struct platform_driver msm6242_rtc_driver = {
 	.remove	= __exit_p(msm6242_rtc_remove),
 };
 
-static int __init msm6242_rtc_init(void)
-{
-	return platform_driver_probe(&msm6242_rtc_driver, msm6242_rtc_probe);
-}
-
-static void __exit msm6242_rtc_fini(void)
-{
-	platform_driver_unregister(&msm6242_rtc_driver);
-}
-
-module_init(msm6242_rtc_init);
-module_exit(msm6242_rtc_fini);
+module_platform_driver_probe(msm6242_rtc_driver, msm6242_rtc_probe);
 
 MODULE_AUTHOR("Geert Uytterhoeven <geert@linux-m68k.org>");
 MODULE_LICENSE("GPL");
diff --git a/drivers/rtc/rtc-mv.c b/drivers/rtc/rtc-mv.c
index 8f87fec27ce..baab802f215 100644
--- a/drivers/rtc/rtc-mv.c
+++ b/drivers/rtc/rtc-mv.c
@@ -217,7 +217,7 @@ static const struct rtc_class_ops mv_rtc_alarm_ops = {
 	.alarm_irq_enable = mv_rtc_alarm_irq_enable,
 };
 
-static int mv_rtc_probe(struct platform_device *pdev)
+static int __init mv_rtc_probe(struct platform_device *pdev)
 {
 	struct resource *res;
 	struct rtc_plat_data *pdata;
@@ -272,12 +272,13 @@ static int mv_rtc_probe(struct platform_device *pdev)
 
 	if (pdata->irq >= 0) {
 		device_init_wakeup(&pdev->dev, 1);
-		pdata->rtc = rtc_device_register(pdev->name, &pdev->dev,
+		pdata->rtc = devm_rtc_device_register(&pdev->dev, pdev->name,
 						 &mv_rtc_alarm_ops,
 						 THIS_MODULE);
-	} else
-		pdata->rtc = rtc_device_register(pdev->name, &pdev->dev,
+	} else {
+		pdata->rtc = devm_rtc_device_register(&pdev->dev, pdev->name,
 						 &mv_rtc_ops, THIS_MODULE);
+	}
 	if (IS_ERR(pdata->rtc)) {
 		ret = PTR_ERR(pdata->rtc);
 		goto out;
@@ -308,7 +309,6 @@ static int __exit mv_rtc_remove(struct platform_device *pdev)
 	if (pdata->irq >= 0)
 		device_init_wakeup(&pdev->dev, 0);
 
-	rtc_device_unregister(pdata->rtc);
 	if (!IS_ERR(pdata->clk))
 		clk_disable_unprepare(pdata->clk);
 
@@ -331,18 +331,7 @@ static struct platform_driver mv_rtc_driver = {
 	},
 };
 
-static __init int mv_init(void)
-{
-	return platform_driver_probe(&mv_rtc_driver, mv_rtc_probe);
-}
-
-static __exit void mv_exit(void)
-{
-	platform_driver_unregister(&mv_rtc_driver);
-}
-
-module_init(mv_init);
-module_exit(mv_exit);
+module_platform_driver_probe(mv_rtc_driver, mv_rtc_probe);
 
 MODULE_AUTHOR("Saeed Bishara <saeed@marvell.com>");
 MODULE_DESCRIPTION("Marvell RTC driver");
diff --git a/drivers/rtc/rtc-mxc.c b/drivers/rtc/rtc-mxc.c
index 1c3ef728956..9a3895bc4f4 100644
--- a/drivers/rtc/rtc-mxc.c
+++ b/drivers/rtc/rtc-mxc.c
@@ -439,7 +439,7 @@ static int mxc_rtc_probe(struct platform_device *pdev)
 	if (pdata->irq >=0)
 		device_init_wakeup(&pdev->dev, 1);
 
-	rtc = rtc_device_register(pdev->name, &pdev->dev, &mxc_rtc_ops,
+	rtc = devm_rtc_device_register(&pdev->dev, pdev->name, &mxc_rtc_ops,
 				  THIS_MODULE);
 	if (IS_ERR(rtc)) {
 		ret = PTR_ERR(rtc);
@@ -464,15 +464,13 @@ static int mxc_rtc_remove(struct platform_device *pdev)
 {
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 
-	rtc_device_unregister(pdata->rtc);
-
 	clk_disable_unprepare(pdata->clk);
 	platform_set_drvdata(pdev, NULL);
 
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int mxc_rtc_suspend(struct device *dev)
 {
 	struct rtc_plat_data *pdata = dev_get_drvdata(dev);
@@ -492,19 +490,14 @@ static int mxc_rtc_resume(struct device *dev)
 
 	return 0;
 }
-
-static struct dev_pm_ops mxc_rtc_pm_ops = {
-	.suspend	= mxc_rtc_suspend,
-	.resume		= mxc_rtc_resume,
-};
 #endif
 
+static SIMPLE_DEV_PM_OPS(mxc_rtc_pm_ops, mxc_rtc_suspend, mxc_rtc_resume);
+
 static struct platform_driver mxc_rtc_driver = {
 	.driver = {
 		   .name	= "mxc_rtc",
-#ifdef CONFIG_PM
 		   .pm		= &mxc_rtc_pm_ops,
-#endif
 		   .owner	= THIS_MODULE,
 	},
 	.id_table = imx_rtc_devtype,
diff --git a/drivers/rtc/rtc-nuc900.c b/drivers/rtc/rtc-nuc900.c
index a63680850fe..f5dfb6e5e7d 100644
--- a/drivers/rtc/rtc-nuc900.c
+++ b/drivers/rtc/rtc-nuc900.c
@@ -222,13 +222,13 @@ static struct rtc_class_ops nuc900_rtc_ops = {
 	.alarm_irq_enable = nuc900_alarm_irq_enable,
 };
 
-static int nuc900_rtc_probe(struct platform_device *pdev)
+static int __init nuc900_rtc_probe(struct platform_device *pdev)
 {
 	struct resource *res;
 	struct nuc900_rtc *nuc900_rtc;
-	int err = 0;
 
-	nuc900_rtc = kzalloc(sizeof(struct nuc900_rtc), GFP_KERNEL);
+	nuc900_rtc = devm_kzalloc(&pdev->dev, sizeof(struct nuc900_rtc),
+				GFP_KERNEL);
 	if (!nuc900_rtc) {
 		dev_err(&pdev->dev, "kzalloc nuc900_rtc failed\n");
 		return -ENOMEM;
@@ -236,93 +236,51 @@ static int nuc900_rtc_probe(struct platform_device *pdev)
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
 		dev_err(&pdev->dev, "platform_get_resource failed\n");
-		err = -ENXIO;
-		goto fail1;
+		return -ENXIO;
 	}
 
-	if (!request_mem_region(res->start, resource_size(res),
-				pdev->name)) {
-		dev_err(&pdev->dev, "request_mem_region failed\n");
-		err = -EBUSY;
-		goto fail1;
-	}
-
-	nuc900_rtc->rtc_reg = ioremap(res->start, resource_size(res));
-	if (!nuc900_rtc->rtc_reg) {
-		dev_err(&pdev->dev, "ioremap rtc_reg failed\n");
-		err = -ENOMEM;
-		goto fail2;
-	}
+	nuc900_rtc->rtc_reg = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(nuc900_rtc->rtc_reg))
+		return PTR_ERR(nuc900_rtc->rtc_reg);
 
 	platform_set_drvdata(pdev, nuc900_rtc);
 
-	nuc900_rtc->rtcdev = rtc_device_register(pdev->name, &pdev->dev,
+	nuc900_rtc->rtcdev = devm_rtc_device_register(&pdev->dev, pdev->name,
 						&nuc900_rtc_ops, THIS_MODULE);
 	if (IS_ERR(nuc900_rtc->rtcdev)) {
 		dev_err(&pdev->dev, "rtc device register failed\n");
-		err = PTR_ERR(nuc900_rtc->rtcdev);
-		goto fail3;
+		return PTR_ERR(nuc900_rtc->rtcdev);
 	}
 
 	__raw_writel(__raw_readl(nuc900_rtc->rtc_reg + REG_RTC_TSSR) | MODE24,
 					nuc900_rtc->rtc_reg + REG_RTC_TSSR);
 
 	nuc900_rtc->irq_num = platform_get_irq(pdev, 0);
-	if (request_irq(nuc900_rtc->irq_num, nuc900_rtc_interrupt,
-				0, "nuc900rtc", nuc900_rtc)) {
+	if (devm_request_irq(&pdev->dev, nuc900_rtc->irq_num,
+			nuc900_rtc_interrupt, 0, "nuc900rtc", nuc900_rtc)) {
 		dev_err(&pdev->dev, "NUC900 RTC request irq failed\n");
-		err = -EBUSY;
-		goto fail4;
+		return -EBUSY;
 	}
 
 	return 0;
-
-fail4:	rtc_device_unregister(nuc900_rtc->rtcdev);
-fail3:	iounmap(nuc900_rtc->rtc_reg);
-fail2:	release_mem_region(res->start, resource_size(res));
-fail1:	kfree(nuc900_rtc);
-	return err;
 }
 
-static int nuc900_rtc_remove(struct platform_device *pdev)
+static int __exit nuc900_rtc_remove(struct platform_device *pdev)
 {
-	struct nuc900_rtc *nuc900_rtc = platform_get_drvdata(pdev);
-	struct resource *res;
-
-	free_irq(nuc900_rtc->irq_num, nuc900_rtc);
-	rtc_device_unregister(nuc900_rtc->rtcdev);
-	iounmap(nuc900_rtc->rtc_reg);
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	release_mem_region(res->start, resource_size(res));
-
-	kfree(nuc900_rtc);
-
 	platform_set_drvdata(pdev, NULL);
 
 	return 0;
 }
 
 static struct platform_driver nuc900_rtc_driver = {
-	.remove		= nuc900_rtc_remove,
+	.remove		= __exit_p(nuc900_rtc_remove),
 	.driver		= {
 		.name	= "nuc900-rtc",
 		.owner	= THIS_MODULE,
 	},
 };
 
-static int __init nuc900_rtc_init(void)
-{
-	return platform_driver_probe(&nuc900_rtc_driver, nuc900_rtc_probe);
-}
-
-static void __exit nuc900_rtc_exit(void)
-{
-	platform_driver_unregister(&nuc900_rtc_driver);
-}
-
-module_init(nuc900_rtc_init);
-module_exit(nuc900_rtc_exit);
+module_platform_driver_probe(nuc900_rtc_driver, nuc900_rtc_probe);
 
 MODULE_AUTHOR("Wan ZongShun <mcuos.com@gmail.com>");
 MODULE_DESCRIPTION("nuc910/nuc920 RTC driver");
diff --git a/drivers/rtc/rtc-omap.c b/drivers/rtc/rtc-omap.c
index 600971407aa..4e1bdb832e3 100644
--- a/drivers/rtc/rtc-omap.c
+++ b/drivers/rtc/rtc-omap.c
@@ -324,7 +324,7 @@ MODULE_DEVICE_TABLE(of, omap_rtc_of_match);
 
 static int __init omap_rtc_probe(struct platform_device *pdev)
 {
-	struct resource		*res, *mem;
+	struct resource		*res;
 	struct rtc_device	*rtc;
 	u8			reg, new_ctrl;
 	const struct platform_device_id *id_entry;
@@ -352,18 +352,9 @@ static int __init omap_rtc_probe(struct platform_device *pdev)
 		return -ENOENT;
 	}
 
-	mem = request_mem_region(res->start, resource_size(res), pdev->name);
-	if (!mem) {
-		pr_debug("%s: RTC registers at %08x are not free\n",
-			pdev->name, res->start);
-		return -EBUSY;
-	}
-
-	rtc_base = ioremap(res->start, resource_size(res));
-	if (!rtc_base) {
-		pr_debug("%s: RTC registers can't be mapped\n", pdev->name);
-		goto fail;
-	}
+	rtc_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(rtc_base))
+		return PTR_ERR(rtc_base);
 
 	/* Enable the clock/module so that we can access the registers */
 	pm_runtime_enable(&pdev->dev);
@@ -375,7 +366,7 @@ static int __init omap_rtc_probe(struct platform_device *pdev)
 		rtc_writel(KICK1_VALUE, OMAP_RTC_KICK1_REG);
 	}
 
-	rtc = rtc_device_register(pdev->name, &pdev->dev,
+	rtc = devm_rtc_device_register(&pdev->dev, pdev->name,
 			&omap_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc)) {
 		pr_debug("%s: can't register RTC device, err %ld\n",
@@ -383,7 +374,6 @@ static int __init omap_rtc_probe(struct platform_device *pdev)
 		goto fail0;
 	}
 	platform_set_drvdata(pdev, rtc);
-	dev_set_drvdata(&rtc->dev, mem);
 
 	/* clear pending irqs, and set 1/second periodic,
 	 * which we'll use instead of update irqs
@@ -401,18 +391,18 @@ static int __init omap_rtc_probe(struct platform_device *pdev)
 		rtc_write(OMAP_RTC_STATUS_ALARM, OMAP_RTC_STATUS_REG);
 
 	/* handle periodic and alarm irqs */
-	if (request_irq(omap_rtc_timer, rtc_irq, 0,
+	if (devm_request_irq(&pdev->dev, omap_rtc_timer, rtc_irq, 0,
 			dev_name(&rtc->dev), rtc)) {
 		pr_debug("%s: RTC timer interrupt IRQ%d already claimed\n",
 			pdev->name, omap_rtc_timer);
-		goto fail1;
+		goto fail0;
 	}
 	if ((omap_rtc_timer != omap_rtc_alarm) &&
-		(request_irq(omap_rtc_alarm, rtc_irq, 0,
+		(devm_request_irq(&pdev->dev, omap_rtc_alarm, rtc_irq, 0,
 			dev_name(&rtc->dev), rtc))) {
 		pr_debug("%s: RTC alarm interrupt IRQ%d already claimed\n",
 			pdev->name, omap_rtc_alarm);
-		goto fail2;
+		goto fail0;
 	}
 
 	/* On boards with split power, RTC_ON_NOFF won't reset the RTC */
@@ -446,25 +436,16 @@ static int __init omap_rtc_probe(struct platform_device *pdev)
 
 	return 0;
 
-fail2:
-	free_irq(omap_rtc_timer, rtc);
-fail1:
-	rtc_device_unregister(rtc);
 fail0:
 	if (id_entry && (id_entry->driver_data & OMAP_RTC_HAS_KICKER))
 		rtc_writel(0, OMAP_RTC_KICK0_REG);
 	pm_runtime_put_sync(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
-	iounmap(rtc_base);
-fail:
-	release_mem_region(mem->start, resource_size(mem));
 	return -EIO;
 }
 
 static int __exit omap_rtc_remove(struct platform_device *pdev)
 {
-	struct rtc_device	*rtc = platform_get_drvdata(pdev);
-	struct resource		*mem = dev_get_drvdata(&rtc->dev);
 	const struct platform_device_id *id_entry =
 				platform_get_device_id(pdev);
 
@@ -473,12 +454,6 @@ static int __exit omap_rtc_remove(struct platform_device *pdev)
 	/* leave rtc running, but disable irqs */
 	rtc_write(0, OMAP_RTC_INTERRUPTS_REG);
 
-	free_irq(omap_rtc_timer, rtc);
-
-	if (omap_rtc_timer != omap_rtc_alarm)
-		free_irq(omap_rtc_alarm, rtc);
-
-	rtc_device_unregister(rtc);
 	if (id_entry && (id_entry->driver_data & OMAP_RTC_HAS_KICKER))
 		rtc_writel(0, OMAP_RTC_KICK0_REG);
 
@@ -486,16 +461,13 @@ static int __exit omap_rtc_remove(struct platform_device *pdev)
 	pm_runtime_put_sync(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
 
-	iounmap(rtc_base);
-	release_mem_region(mem->start, resource_size(mem));
 	return 0;
 }
 
-#ifdef CONFIG_PM
-
+#ifdef CONFIG_PM_SLEEP
 static u8 irqstat;
 
-static int omap_rtc_suspend(struct platform_device *pdev, pm_message_t state)
+static int omap_rtc_suspend(struct device *dev)
 {
 	irqstat = rtc_read(OMAP_RTC_INTERRUPTS_REG);
 
@@ -503,34 +475,32 @@ static int omap_rtc_suspend(struct platform_device *pdev, pm_message_t state)
 	 * source, and in fact this enable() call is just saving a flag
 	 * that's never used...
 	 */
-	if (device_may_wakeup(&pdev->dev))
+	if (device_may_wakeup(dev))
 		enable_irq_wake(omap_rtc_alarm);
 	else
 		rtc_write(0, OMAP_RTC_INTERRUPTS_REG);
 
 	/* Disable the clock/module */
-	pm_runtime_put_sync(&pdev->dev);
+	pm_runtime_put_sync(dev);
 
 	return 0;
 }
 
-static int omap_rtc_resume(struct platform_device *pdev)
+static int omap_rtc_resume(struct device *dev)
 {
 	/* Enable the clock/module so that we can access the registers */
-	pm_runtime_get_sync(&pdev->dev);
+	pm_runtime_get_sync(dev);
 
-	if (device_may_wakeup(&pdev->dev))
+	if (device_may_wakeup(dev))
 		disable_irq_wake(omap_rtc_alarm);
 	else
 		rtc_write(irqstat, OMAP_RTC_INTERRUPTS_REG);
 	return 0;
 }
-
-#else
-#define omap_rtc_suspend NULL
-#define omap_rtc_resume  NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(omap_rtc_pm_ops, omap_rtc_suspend, omap_rtc_resume);
+
 static void omap_rtc_shutdown(struct platform_device *pdev)
 {
 	rtc_write(0, OMAP_RTC_INTERRUPTS_REG);
@@ -539,28 +509,17 @@ static void omap_rtc_shutdown(struct platform_device *pdev)
 MODULE_ALIAS("platform:omap_rtc");
 static struct platform_driver omap_rtc_driver = {
 	.remove		= __exit_p(omap_rtc_remove),
-	.suspend	= omap_rtc_suspend,
-	.resume		= omap_rtc_resume,
 	.shutdown	= omap_rtc_shutdown,
 	.driver		= {
 		.name	= DRIVER_NAME,
 		.owner	= THIS_MODULE,
+		.pm	= &omap_rtc_pm_ops,
 		.of_match_table = of_match_ptr(omap_rtc_of_match),
 	},
 	.id_table	= omap_rtc_devtype,
 };
 
-static int __init rtc_init(void)
-{
-	return platform_driver_probe(&omap_rtc_driver, omap_rtc_probe);
-}
-module_init(rtc_init);
-
-static void __exit rtc_exit(void)
-{
-	platform_driver_unregister(&omap_rtc_driver);
-}
-module_exit(rtc_exit);
+module_platform_driver_probe(omap_rtc_driver, omap_rtc_probe);
 
 MODULE_AUTHOR("George G. Davis (and others)");
 MODULE_LICENSE("GPL");
diff --git a/drivers/rtc/rtc-palmas.c b/drivers/rtc/rtc-palmas.c
index 59c42986254..50204d474eb 100644
--- a/drivers/rtc/rtc-palmas.c
+++ b/drivers/rtc/rtc-palmas.c
@@ -30,6 +30,7 @@
 #include <linux/kernel.h>
 #include <linux/mfd/palmas.h>
 #include <linux/module.h>
+#include <linux/of.h>
 #include <linux/rtc.h>
 #include <linux/types.h>
 #include <linux/platform_device.h>
@@ -264,7 +265,7 @@ static int palmas_rtc_probe(struct platform_device *pdev)
 
 	palmas_rtc->irq = platform_get_irq(pdev, 0);
 
-	palmas_rtc->rtc = rtc_device_register(pdev->name, &pdev->dev,
+	palmas_rtc->rtc = devm_rtc_device_register(&pdev->dev, pdev->name,
 				&palmas_rtc_ops, THIS_MODULE);
 	if (IS_ERR(palmas_rtc->rtc)) {
 		ret = PTR_ERR(palmas_rtc->rtc);
@@ -272,14 +273,13 @@ static int palmas_rtc_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	ret = request_threaded_irq(palmas_rtc->irq, NULL,
+	ret = devm_request_threaded_irq(&pdev->dev, palmas_rtc->irq, NULL,
 			palmas_rtc_interrupt,
 			IRQF_TRIGGER_LOW | IRQF_ONESHOT |
 			IRQF_EARLY_RESUME,
 			dev_name(&pdev->dev), palmas_rtc);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "IRQ request failed, err = %d\n", ret);
-		rtc_device_unregister(palmas_rtc->rtc);
 		return ret;
 	}
 
@@ -289,11 +289,7 @@ static int palmas_rtc_probe(struct platform_device *pdev)
 
 static int palmas_rtc_remove(struct platform_device *pdev)
 {
-	struct palmas_rtc *palmas_rtc = platform_get_drvdata(pdev);
-
 	palmas_rtc_alarm_irq_enable(&pdev->dev, 0);
-	free_irq(palmas_rtc->irq, palmas_rtc);
-	rtc_device_unregister(palmas_rtc->rtc);
 	return 0;
 }
 
@@ -321,6 +317,14 @@ static const struct dev_pm_ops palmas_rtc_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(palmas_rtc_suspend, palmas_rtc_resume)
 };
 
+#ifdef CONFIG_OF
+static struct of_device_id of_palmas_rtc_match[] = {
+	{ .compatible = "ti,palmas-rtc"},
+	{ },
+};
+MODULE_DEVICE_TABLE(of, of_palmas_rtc_match);
+#endif
+
 static struct platform_driver palmas_rtc_driver = {
 	.probe		= palmas_rtc_probe,
 	.remove		= palmas_rtc_remove,
@@ -328,6 +332,7 @@ static struct platform_driver palmas_rtc_driver = {
 		.owner	= THIS_MODULE,
 		.name	= "palmas-rtc",
 		.pm	= &palmas_rtc_pm_ops,
+		.of_match_table = of_match_ptr(of_palmas_rtc_match),
 	},
 };
 
diff --git a/drivers/rtc/rtc-pcap.c b/drivers/rtc/rtc-pcap.c
index e0019cd0bf7..539a90b98bc 100644
--- a/drivers/rtc/rtc-pcap.c
+++ b/drivers/rtc/rtc-pcap.c
@@ -139,13 +139,14 @@ static const struct rtc_class_ops pcap_rtc_ops = {
 	.alarm_irq_enable = pcap_rtc_alarm_irq_enable,
 };
 
-static int pcap_rtc_probe(struct platform_device *pdev)
+static int __init pcap_rtc_probe(struct platform_device *pdev)
 {
 	struct pcap_rtc *pcap_rtc;
 	int timer_irq, alarm_irq;
 	int err = -ENOMEM;
 
-	pcap_rtc = kmalloc(sizeof(struct pcap_rtc), GFP_KERNEL);
+	pcap_rtc = devm_kzalloc(&pdev->dev, sizeof(struct pcap_rtc),
+				GFP_KERNEL);
 	if (!pcap_rtc)
 		return err;
 
@@ -153,68 +154,46 @@ static int pcap_rtc_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, pcap_rtc);
 
-	pcap_rtc->rtc = rtc_device_register("pcap", &pdev->dev,
-				  &pcap_rtc_ops, THIS_MODULE);
+	pcap_rtc->rtc = devm_rtc_device_register(&pdev->dev, "pcap",
+					&pcap_rtc_ops, THIS_MODULE);
 	if (IS_ERR(pcap_rtc->rtc)) {
 		err = PTR_ERR(pcap_rtc->rtc);
-		goto fail_rtc;
+		goto fail;
 	}
 
-
 	timer_irq = pcap_to_irq(pcap_rtc->pcap, PCAP_IRQ_1HZ);
 	alarm_irq = pcap_to_irq(pcap_rtc->pcap, PCAP_IRQ_TODA);
 
-	err = request_irq(timer_irq, pcap_rtc_irq, 0, "RTC Timer", pcap_rtc);
+	err = devm_request_irq(&pdev->dev, timer_irq, pcap_rtc_irq, 0,
+				"RTC Timer", pcap_rtc);
 	if (err)
-		goto fail_timer;
+		goto fail;
 
-	err = request_irq(alarm_irq, pcap_rtc_irq, 0, "RTC Alarm", pcap_rtc);
+	err = devm_request_irq(&pdev->dev, alarm_irq, pcap_rtc_irq, 0,
+				"RTC Alarm", pcap_rtc);
 	if (err)
-		goto fail_alarm;
+		goto fail;
 
 	return 0;
-fail_alarm:
-	free_irq(timer_irq, pcap_rtc);
-fail_timer:
-	rtc_device_unregister(pcap_rtc->rtc);
-fail_rtc:
+fail:
 	platform_set_drvdata(pdev, NULL);
-	kfree(pcap_rtc);
 	return err;
 }
 
-static int pcap_rtc_remove(struct platform_device *pdev)
+static int __exit pcap_rtc_remove(struct platform_device *pdev)
 {
-	struct pcap_rtc *pcap_rtc = platform_get_drvdata(pdev);
-
-	free_irq(pcap_to_irq(pcap_rtc->pcap, PCAP_IRQ_1HZ), pcap_rtc);
-	free_irq(pcap_to_irq(pcap_rtc->pcap, PCAP_IRQ_TODA), pcap_rtc);
-	rtc_device_unregister(pcap_rtc->rtc);
-	kfree(pcap_rtc);
-
 	return 0;
 }
 
 static struct platform_driver pcap_rtc_driver = {
-	.remove = pcap_rtc_remove,
+	.remove = __exit_p(pcap_rtc_remove),
 	.driver = {
 		.name  = "pcap-rtc",
 		.owner = THIS_MODULE,
 	},
 };
 
-static int __init rtc_pcap_init(void)
-{
-	return platform_driver_probe(&pcap_rtc_driver, pcap_rtc_probe);
-}
-
-static void __exit rtc_pcap_exit(void)
-{
-	platform_driver_unregister(&pcap_rtc_driver);
-}
-
-module_init(rtc_pcap_init);
-module_exit(rtc_pcap_exit);
+module_platform_driver_probe(pcap_rtc_driver, pcap_rtc_probe);
 
 MODULE_DESCRIPTION("Motorola pcap rtc driver");
 MODULE_AUTHOR("guiming zhuo <gmzhuo@gmail.com>");
diff --git a/drivers/rtc/rtc-pcf2123.c b/drivers/rtc/rtc-pcf2123.c
index 02b742afa76..796a6c5067d 100644
--- a/drivers/rtc/rtc-pcf2123.c
+++ b/drivers/rtc/rtc-pcf2123.c
@@ -226,7 +226,8 @@ static int pcf2123_probe(struct spi_device *spi)
 	u8 txbuf[2], rxbuf[2];
 	int ret, i;
 
-	pdata = kzalloc(sizeof(struct pcf2123_plat_data), GFP_KERNEL);
+	pdata = devm_kzalloc(&spi->dev, sizeof(struct pcf2123_plat_data),
+				GFP_KERNEL);
 	if (!pdata)
 		return -ENOMEM;
 	spi->dev.platform_data = pdata;
@@ -265,6 +266,7 @@ static int pcf2123_probe(struct spi_device *spi)
 
 	if (!(rxbuf[0] & 0x20)) {
 		dev_err(&spi->dev, "chip not found\n");
+		ret = -ENODEV;
 		goto kfree_exit;
 	}
 
@@ -281,7 +283,7 @@ static int pcf2123_probe(struct spi_device *spi)
 	pcf2123_delay_trec();
 
 	/* Finalize the initialization */
-	rtc = rtc_device_register(pcf2123_driver.driver.name, &spi->dev,
+	rtc = devm_rtc_device_register(&spi->dev, pcf2123_driver.driver.name,
 			&pcf2123_rtc_ops, THIS_MODULE);
 
 	if (IS_ERR(rtc)) {
@@ -314,7 +316,6 @@ sysfs_exit:
 		device_remove_file(&spi->dev, &pdata->regs[i].attr);
 
 kfree_exit:
-	kfree(pdata);
 	spi->dev.platform_data = NULL;
 	return ret;
 }
@@ -325,15 +326,10 @@ static int pcf2123_remove(struct spi_device *spi)
 	int i;
 
 	if (pdata) {
-		struct rtc_device *rtc = pdata->rtc;
-
-		if (rtc)
-			rtc_device_unregister(rtc);
 		for (i = 0; i < 16; i++)
 			if (pdata->regs[i].name[0])
 				device_remove_file(&spi->dev,
 						   &pdata->regs[i].attr);
-		kfree(pdata);
 	}
 
 	return 0;
diff --git a/drivers/rtc/rtc-pcf50633.c b/drivers/rtc/rtc-pcf50633.c
index e9f3135d305..e6b6911c8e0 100644
--- a/drivers/rtc/rtc-pcf50633.c
+++ b/drivers/rtc/rtc-pcf50633.c
@@ -252,20 +252,17 @@ static int pcf50633_rtc_probe(struct platform_device *pdev)
 {
 	struct pcf50633_rtc *rtc;
 
-	rtc = kzalloc(sizeof(*rtc), GFP_KERNEL);
+	rtc = devm_kzalloc(&pdev->dev, sizeof(*rtc), GFP_KERNEL);
 	if (!rtc)
 		return -ENOMEM;
 
 	rtc->pcf = dev_to_pcf50633(pdev->dev.parent);
 	platform_set_drvdata(pdev, rtc);
-	rtc->rtc_dev = rtc_device_register("pcf50633-rtc", &pdev->dev,
+	rtc->rtc_dev = devm_rtc_device_register(&pdev->dev, "pcf50633-rtc",
 				&pcf50633_rtc_ops, THIS_MODULE);
 
-	if (IS_ERR(rtc->rtc_dev)) {
-		int ret =  PTR_ERR(rtc->rtc_dev);
-		kfree(rtc);
-		return ret;
-	}
+	if (IS_ERR(rtc->rtc_dev))
+		return PTR_ERR(rtc->rtc_dev);
 
 	pcf50633_register_irq(rtc->pcf, PCF50633_IRQ_ALARM,
 					pcf50633_rtc_irq, rtc);
@@ -277,12 +274,8 @@ static int pcf50633_rtc_remove(struct platform_device *pdev)
 	struct pcf50633_rtc *rtc;
 
 	rtc = platform_get_drvdata(pdev);
-
 	pcf50633_free_irq(rtc->pcf, PCF50633_IRQ_ALARM);
 
-	rtc_device_unregister(rtc->rtc_dev);
-	kfree(rtc);
-
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-pcf8523.c b/drivers/rtc/rtc-pcf8523.c
index 889e3160e70..305c9515e5b 100644
--- a/drivers/rtc/rtc-pcf8523.c
+++ b/drivers/rtc/rtc-pcf8523.c
@@ -307,7 +307,7 @@ static int pcf8523_probe(struct i2c_client *client,
 	if (err < 0)
 		return err;
 
-	pcf->rtc = rtc_device_register(DRIVER_NAME, &client->dev,
+	pcf->rtc = devm_rtc_device_register(&client->dev, DRIVER_NAME,
 				       &pcf8523_rtc_ops, THIS_MODULE);
 	if (IS_ERR(pcf->rtc))
 		return PTR_ERR(pcf->rtc);
@@ -319,10 +319,6 @@ static int pcf8523_probe(struct i2c_client *client,
 
 static int pcf8523_remove(struct i2c_client *client)
 {
-	struct pcf8523 *pcf = i2c_get_clientdata(client);
-
-	rtc_device_unregister(pcf->rtc);
-
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-pcf8563.c b/drivers/rtc/rtc-pcf8563.c
index f7daf18a112..97b354a26a4 100644
--- a/drivers/rtc/rtc-pcf8563.c
+++ b/drivers/rtc/rtc-pcf8563.c
@@ -245,14 +245,13 @@ static int pcf8563_probe(struct i2c_client *client,
 {
 	struct pcf8563 *pcf8563;
 
-	int err = 0;
-
 	dev_dbg(&client->dev, "%s\n", __func__);
 
 	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C))
 		return -ENODEV;
 
-	pcf8563 = kzalloc(sizeof(struct pcf8563), GFP_KERNEL);
+	pcf8563 = devm_kzalloc(&client->dev, sizeof(struct pcf8563),
+				GFP_KERNEL);
 	if (!pcf8563)
 		return -ENOMEM;
 
@@ -260,31 +259,18 @@ static int pcf8563_probe(struct i2c_client *client,
 
 	i2c_set_clientdata(client, pcf8563);
 
-	pcf8563->rtc = rtc_device_register(pcf8563_driver.driver.name,
-				&client->dev, &pcf8563_rtc_ops, THIS_MODULE);
+	pcf8563->rtc = devm_rtc_device_register(&client->dev,
+				pcf8563_driver.driver.name,
+				&pcf8563_rtc_ops, THIS_MODULE);
 
-	if (IS_ERR(pcf8563->rtc)) {
-		err = PTR_ERR(pcf8563->rtc);
-		goto exit_kfree;
-	}
+	if (IS_ERR(pcf8563->rtc))
+		return PTR_ERR(pcf8563->rtc);
 
 	return 0;
-
-exit_kfree:
-	kfree(pcf8563);
-
-	return err;
 }
 
 static int pcf8563_remove(struct i2c_client *client)
 {
-	struct pcf8563 *pcf8563 = i2c_get_clientdata(client);
-
-	if (pcf8563->rtc)
-		rtc_device_unregister(pcf8563->rtc);
-
-	kfree(pcf8563);
-
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-pcf8583.c b/drivers/rtc/rtc-pcf8583.c
index 5f97c61247d..95886dcf4a3 100644
--- a/drivers/rtc/rtc-pcf8583.c
+++ b/drivers/rtc/rtc-pcf8583.c
@@ -268,39 +268,29 @@ static int pcf8583_probe(struct i2c_client *client,
 				const struct i2c_device_id *id)
 {
 	struct pcf8583 *pcf8583;
-	int err;
 
 	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C))
 		return -ENODEV;
 
-	pcf8583 = kzalloc(sizeof(struct pcf8583), GFP_KERNEL);
+	pcf8583 = devm_kzalloc(&client->dev, sizeof(struct pcf8583),
+				GFP_KERNEL);
 	if (!pcf8583)
 		return -ENOMEM;
 
 	i2c_set_clientdata(client, pcf8583);
 
-	pcf8583->rtc = rtc_device_register(pcf8583_driver.driver.name,
-			&client->dev, &pcf8583_rtc_ops, THIS_MODULE);
+	pcf8583->rtc = devm_rtc_device_register(&client->dev,
+				pcf8583_driver.driver.name,
+				&pcf8583_rtc_ops, THIS_MODULE);
 
-	if (IS_ERR(pcf8583->rtc)) {
-		err = PTR_ERR(pcf8583->rtc);
-		goto exit_kfree;
-	}
+	if (IS_ERR(pcf8583->rtc))
+		return PTR_ERR(pcf8583->rtc);
 
 	return 0;
-
-exit_kfree:
-	kfree(pcf8583);
-	return err;
 }
 
 static int pcf8583_remove(struct i2c_client *client)
 {
-	struct pcf8583 *pcf8583 = i2c_get_clientdata(client);
-
-	if (pcf8583->rtc)
-		rtc_device_unregister(pcf8583->rtc);
-	kfree(pcf8583);
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-ps3.c b/drivers/rtc/rtc-ps3.c
index 968133ce1ee..4bb825bb580 100644
--- a/drivers/rtc/rtc-ps3.c
+++ b/drivers/rtc/rtc-ps3.c
@@ -62,7 +62,7 @@ static int __init ps3_rtc_probe(struct platform_device *dev)
 {
 	struct rtc_device *rtc;
 
-	rtc = rtc_device_register("rtc-ps3", &dev->dev, &ps3_rtc_ops,
+	rtc = devm_rtc_device_register(&dev->dev, "rtc-ps3", &ps3_rtc_ops,
 				  THIS_MODULE);
 	if (IS_ERR(rtc))
 		return PTR_ERR(rtc);
@@ -73,7 +73,6 @@ static int __init ps3_rtc_probe(struct platform_device *dev)
 
 static int __exit ps3_rtc_remove(struct platform_device *dev)
 {
-	rtc_device_unregister(platform_get_drvdata(dev));
 	return 0;
 }
 
@@ -85,18 +84,7 @@ static struct platform_driver ps3_rtc_driver = {
 	.remove = __exit_p(ps3_rtc_remove),
 };
 
-static int __init ps3_rtc_init(void)
-{
-	return platform_driver_probe(&ps3_rtc_driver, ps3_rtc_probe);
-}
-
-static void __exit ps3_rtc_fini(void)
-{
-	platform_driver_unregister(&ps3_rtc_driver);
-}
-
-module_init(ps3_rtc_init);
-module_exit(ps3_rtc_fini);
+module_platform_driver_probe(ps3_rtc_driver, ps3_rtc_probe);
 
 MODULE_AUTHOR("Sony Corporation");
 MODULE_LICENSE("GPL");
diff --git a/drivers/rtc/rtc-puv3.c b/drivers/rtc/rtc-puv3.c
index 0407e13d4de..72f437170d2 100644
--- a/drivers/rtc/rtc-puv3.c
+++ b/drivers/rtc/rtc-puv3.c
@@ -207,14 +207,14 @@ static const struct rtc_class_ops puv3_rtcops = {
 	.proc	        = puv3_rtc_proc,
 };
 
-static void puv3_rtc_enable(struct platform_device *pdev, int en)
+static void puv3_rtc_enable(struct device *dev, int en)
 {
 	if (!en) {
 		writel(readl(RTC_RTSR) & ~RTC_RTSR_HZE, RTC_RTSR);
 	} else {
 		/* re-enable the device, and check it is ok */
 		if ((readl(RTC_RTSR) & RTC_RTSR_HZE) == 0) {
-			dev_info(&pdev->dev, "rtc disabled, re-enabling\n");
+			dev_info(dev, "rtc disabled, re-enabling\n");
 			writel(readl(RTC_RTSR) | RTC_RTSR_HZE, RTC_RTSR);
 		}
 	}
@@ -276,7 +276,7 @@ static int puv3_rtc_probe(struct platform_device *pdev)
 		goto err_nores;
 	}
 
-	puv3_rtc_enable(pdev, 1);
+	puv3_rtc_enable(&pdev->dev, 1);
 
 	/* register RTC and exit */
 	rtc = rtc_device_register("pkunity", &pdev->dev, &puv3_rtcops,
@@ -296,44 +296,41 @@ static int puv3_rtc_probe(struct platform_device *pdev)
 	return 0;
 
  err_nortc:
-	puv3_rtc_enable(pdev, 0);
+	puv3_rtc_enable(&pdev->dev, 0);
 	release_resource(puv3_rtc_mem);
 
  err_nores:
 	return ret;
 }
 
-#ifdef CONFIG_PM
-
+#ifdef CONFIG_PM_SLEEP
 static int ticnt_save;
 
-static int puv3_rtc_suspend(struct platform_device *pdev, pm_message_t state)
+static int puv3_rtc_suspend(struct device *dev)
 {
 	/* save RTAR for anyone using periodic interrupts */
 	ticnt_save = readl(RTC_RTAR);
-	puv3_rtc_enable(pdev, 0);
+	puv3_rtc_enable(dev, 0);
 	return 0;
 }
 
-static int puv3_rtc_resume(struct platform_device *pdev)
+static int puv3_rtc_resume(struct device *dev)
 {
-	puv3_rtc_enable(pdev, 1);
+	puv3_rtc_enable(dev, 1);
 	writel(ticnt_save, RTC_RTAR);
 	return 0;
 }
-#else
-#define puv3_rtc_suspend NULL
-#define puv3_rtc_resume  NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(puv3_rtc_pm_ops, puv3_rtc_suspend, puv3_rtc_resume);
+
 static struct platform_driver puv3_rtc_driver = {
 	.probe		= puv3_rtc_probe,
 	.remove		= puv3_rtc_remove,
-	.suspend	= puv3_rtc_suspend,
-	.resume		= puv3_rtc_resume,
 	.driver		= {
 		.name	= "PKUnity-v3-RTC",
 		.owner	= THIS_MODULE,
+		.pm	= &puv3_rtc_pm_ops,
 	}
 };
 
diff --git a/drivers/rtc/rtc-pxa.c b/drivers/rtc/rtc-pxa.c
index 03c85ee719a..ed037ae91c5 100644
--- a/drivers/rtc/rtc-pxa.c
+++ b/drivers/rtc/rtc-pxa.c
@@ -416,7 +416,7 @@ static struct of_device_id pxa_rtc_dt_ids[] = {
 MODULE_DEVICE_TABLE(of, pxa_rtc_dt_ids);
 #endif
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int pxa_rtc_suspend(struct device *dev)
 {
 	struct pxa_rtc *pxa_rtc = dev_get_drvdata(dev);
@@ -434,36 +434,20 @@ static int pxa_rtc_resume(struct device *dev)
 		disable_irq_wake(pxa_rtc->irq_Alrm);
 	return 0;
 }
-
-static const struct dev_pm_ops pxa_rtc_pm_ops = {
-	.suspend	= pxa_rtc_suspend,
-	.resume		= pxa_rtc_resume,
-};
 #endif
 
+static SIMPLE_DEV_PM_OPS(pxa_rtc_pm_ops, pxa_rtc_suspend, pxa_rtc_resume);
+
 static struct platform_driver pxa_rtc_driver = {
 	.remove		= __exit_p(pxa_rtc_remove),
 	.driver		= {
 		.name	= "pxa-rtc",
 		.of_match_table = of_match_ptr(pxa_rtc_dt_ids),
-#ifdef CONFIG_PM
 		.pm	= &pxa_rtc_pm_ops,
-#endif
 	},
 };
 
-static int __init pxa_rtc_init(void)
-{
-	return platform_driver_probe(&pxa_rtc_driver, pxa_rtc_probe);
-}
-
-static void __exit pxa_rtc_exit(void)
-{
-	platform_driver_unregister(&pxa_rtc_driver);
-}
-
-module_init(pxa_rtc_init);
-module_exit(pxa_rtc_exit);
+module_platform_driver_probe(pxa_rtc_driver, pxa_rtc_probe);
 
 MODULE_AUTHOR("Robert Jarzmik <robert.jarzmik@free.fr>");
 MODULE_DESCRIPTION("PXA27x/PXA3xx Realtime Clock Driver (RTC)");
diff --git a/drivers/rtc/rtc-r9701.c b/drivers/rtc/rtc-r9701.c
index 7726f4a4f2d..feeedbd8200 100644
--- a/drivers/rtc/rtc-r9701.c
+++ b/drivers/rtc/rtc-r9701.c
@@ -154,21 +154,18 @@ static int r9701_probe(struct spi_device *spi)
 		}
 	}
 
-	rtc = rtc_device_register("r9701",
-				&spi->dev, &r9701_rtc_ops, THIS_MODULE);
+	rtc = devm_rtc_device_register(&spi->dev, "r9701",
+				&r9701_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc))
 		return PTR_ERR(rtc);
 
-	dev_set_drvdata(&spi->dev, rtc);
+	spi_set_drvdata(spi, rtc);
 
 	return 0;
 }
 
 static int r9701_remove(struct spi_device *spi)
 {
-	struct rtc_device *rtc = dev_get_drvdata(&spi->dev);
-
-	rtc_device_unregister(rtc);
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-rc5t583.c b/drivers/rtc/rtc-rc5t583.c
index eb3194d664a..8eabcf51b35 100644
--- a/drivers/rtc/rtc-rc5t583.c
+++ b/drivers/rtc/rtc-rc5t583.c
@@ -256,7 +256,7 @@ static int rc5t583_rtc_probe(struct platform_device *pdev)
 	}
 	device_init_wakeup(&pdev->dev, 1);
 
-	ricoh_rtc->rtc = rtc_device_register(pdev->name, &pdev->dev,
+	ricoh_rtc->rtc = devm_rtc_device_register(&pdev->dev, pdev->name,
 		&rc5t583_rtc_ops, THIS_MODULE);
 	if (IS_ERR(ricoh_rtc->rtc)) {
 		ret = PTR_ERR(ricoh_rtc->rtc);
@@ -276,13 +276,10 @@ static int rc5t583_rtc_remove(struct platform_device *pdev)
 	struct rc5t583_rtc *rc5t583_rtc = dev_get_drvdata(&pdev->dev);
 
 	rc5t583_rtc_alarm_irq_enable(&rc5t583_rtc->rtc->dev, 0);
-
-	rtc_device_unregister(rc5t583_rtc->rtc);
 	return 0;
 }
 
 #ifdef CONFIG_PM_SLEEP
-
 static int rc5t583_rtc_suspend(struct device *dev)
 {
 	struct rc5t583 *rc5t583 = dev_get_drvdata(dev->parent);
@@ -304,24 +301,18 @@ static int rc5t583_rtc_resume(struct device *dev)
 	return regmap_write(rc5t583->regmap, RC5T583_RTC_CTL1,
 		rc5t583_rtc->irqen);
 }
-
-static const struct dev_pm_ops rc5t583_rtc_pm_ops = {
-	.suspend	= rc5t583_rtc_suspend,
-	.resume		= rc5t583_rtc_resume,
-};
-
-#define DEV_PM_OPS     (&rc5t583_rtc_pm_ops)
-#else
-#define DEV_PM_OPS     NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(rc5t583_rtc_pm_ops, rc5t583_rtc_suspend,
+			rc5t583_rtc_resume);
+
 static struct platform_driver rc5t583_rtc_driver = {
 	.probe		= rc5t583_rtc_probe,
 	.remove		= rc5t583_rtc_remove,
 	.driver		= {
 		.owner	= THIS_MODULE,
 		.name	= "rtc-rc5t583",
-		.pm	= DEV_PM_OPS,
+		.pm	= &rc5t583_rtc_pm_ops,
 	},
 };
 
diff --git a/drivers/rtc/rtc-rp5c01.c b/drivers/rtc/rtc-rp5c01.c
index 359da6d020b..873c689f01c 100644
--- a/drivers/rtc/rtc-rp5c01.c
+++ b/drivers/rtc/rtc-rp5c01.c
@@ -230,15 +230,13 @@ static int __init rp5c01_rtc_probe(struct platform_device *dev)
 	if (!res)
 		return -ENODEV;
 
-	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	priv = devm_kzalloc(&dev->dev, sizeof(*priv), GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
 
-	priv->regs = ioremap(res->start, resource_size(res));
-	if (!priv->regs) {
-		error = -ENOMEM;
-		goto out_free_priv;
-	}
+	priv->regs = devm_ioremap(&dev->dev, res->start, resource_size(res));
+	if (!priv->regs)
+		return -ENOMEM;
 
 	sysfs_bin_attr_init(&priv->nvram_attr);
 	priv->nvram_attr.attr.name = "nvram";
@@ -251,27 +249,22 @@ static int __init rp5c01_rtc_probe(struct platform_device *dev)
 
 	platform_set_drvdata(dev, priv);
 
-	rtc = rtc_device_register("rtc-rp5c01", &dev->dev, &rp5c01_rtc_ops,
+	rtc = devm_rtc_device_register(&dev->dev, "rtc-rp5c01", &rp5c01_rtc_ops,
 				  THIS_MODULE);
 	if (IS_ERR(rtc)) {
 		error = PTR_ERR(rtc);
-		goto out_unmap;
+		goto out;
 	}
 	priv->rtc = rtc;
 
 	error = sysfs_create_bin_file(&dev->dev.kobj, &priv->nvram_attr);
 	if (error)
-		goto out_unregister;
+		goto out;
 
 	return 0;
 
-out_unregister:
-	rtc_device_unregister(rtc);
-out_unmap:
+out:
 	platform_set_drvdata(dev, NULL);
-	iounmap(priv->regs);
-out_free_priv:
-	kfree(priv);
 	return error;
 }
 
@@ -280,9 +273,6 @@ static int __exit rp5c01_rtc_remove(struct platform_device *dev)
 	struct rp5c01_priv *priv = platform_get_drvdata(dev);
 
 	sysfs_remove_bin_file(&dev->dev.kobj, &priv->nvram_attr);
-	rtc_device_unregister(priv->rtc);
-	iounmap(priv->regs);
-	kfree(priv);
 	return 0;
 }
 
@@ -294,18 +284,7 @@ static struct platform_driver rp5c01_rtc_driver = {
 	.remove	= __exit_p(rp5c01_rtc_remove),
 };
 
-static int __init rp5c01_rtc_init(void)
-{
-	return platform_driver_probe(&rp5c01_rtc_driver, rp5c01_rtc_probe);
-}
-
-static void __exit rp5c01_rtc_fini(void)
-{
-	platform_driver_unregister(&rp5c01_rtc_driver);
-}
-
-module_init(rp5c01_rtc_init);
-module_exit(rp5c01_rtc_fini);
+module_platform_driver_probe(rp5c01_rtc_driver, rp5c01_rtc_probe);
 
 MODULE_AUTHOR("Geert Uytterhoeven <geert@linux-m68k.org>");
 MODULE_LICENSE("GPL");
diff --git a/drivers/rtc/rtc-rs5c313.c b/drivers/rtc/rtc-rs5c313.c
index d98ea5b759c..8089fc63e40 100644
--- a/drivers/rtc/rtc-rs5c313.c
+++ b/drivers/rtc/rtc-rs5c313.c
@@ -367,7 +367,7 @@ static const struct rtc_class_ops rs5c313_rtc_ops = {
 
 static int rs5c313_rtc_probe(struct platform_device *pdev)
 {
-	struct rtc_device *rtc = rtc_device_register("rs5c313", &pdev->dev,
+	struct rtc_device *rtc = devm_rtc_device_register(&pdev->dev, "rs5c313",
 				&rs5c313_rtc_ops, THIS_MODULE);
 
 	if (IS_ERR(rtc))
@@ -380,10 +380,6 @@ static int rs5c313_rtc_probe(struct platform_device *pdev)
 
 static int rs5c313_rtc_remove(struct platform_device *pdev)
 {
-	struct rtc_device *rtc = platform_get_drvdata( pdev );
-
-	rtc_device_unregister(rtc);
-
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-rs5c348.c b/drivers/rtc/rtc-rs5c348.c
index 72ef10be866..2c37df3586c 100644
--- a/drivers/rtc/rtc-rs5c348.c
+++ b/drivers/rtc/rtc-rs5c348.c
@@ -158,7 +158,8 @@ static int rs5c348_probe(struct spi_device *spi)
 	struct rtc_device *rtc;
 	struct rs5c348_plat_data *pdata;
 
-	pdata = kzalloc(sizeof(struct rs5c348_plat_data), GFP_KERNEL);
+	pdata = devm_kzalloc(&spi->dev, sizeof(struct rs5c348_plat_data),
+				GFP_KERNEL);
 	if (!pdata)
 		return -ENOMEM;
 	spi->dev.platform_data = pdata;
@@ -202,7 +203,7 @@ static int rs5c348_probe(struct spi_device *spi)
 	if (ret & RS5C348_BIT_24H)
 		pdata->rtc_24h = 1;
 
-	rtc = rtc_device_register(rs5c348_driver.driver.name, &spi->dev,
+	rtc = devm_rtc_device_register(&spi->dev, rs5c348_driver.driver.name,
 				  &rs5c348_rtc_ops, THIS_MODULE);
 
 	if (IS_ERR(rtc)) {
@@ -214,18 +215,11 @@ static int rs5c348_probe(struct spi_device *spi)
 
 	return 0;
  kfree_exit:
-	kfree(pdata);
 	return ret;
 }
 
 static int rs5c348_remove(struct spi_device *spi)
 {
-	struct rs5c348_plat_data *pdata = spi->dev.platform_data;
-	struct rtc_device *rtc = pdata->rtc;
-
-	if (rtc)
-		rtc_device_unregister(rtc);
-	kfree(pdata);
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-rs5c372.c b/drivers/rtc/rtc-rs5c372.c
index 581739f4009..224d634322b 100644
--- a/drivers/rtc/rtc-rs5c372.c
+++ b/drivers/rtc/rtc-rs5c372.c
@@ -579,7 +579,9 @@ static int rs5c372_probe(struct i2c_client *client,
 		}
 	}
 
-	if (!(rs5c372 = kzalloc(sizeof(struct rs5c372), GFP_KERNEL))) {
+	rs5c372 = devm_kzalloc(&client->dev, sizeof(struct rs5c372),
+				GFP_KERNEL);
+	if (!rs5c372) {
 		err = -ENOMEM;
 		goto exit;
 	}
@@ -594,7 +596,7 @@ static int rs5c372_probe(struct i2c_client *client,
 
 	err = rs5c_get_regs(rs5c372);
 	if (err < 0)
-		goto exit_kfree;
+		goto exit;
 
 	/* clock may be set for am/pm or 24 hr time */
 	switch (rs5c372->type) {
@@ -617,7 +619,7 @@ static int rs5c372_probe(struct i2c_client *client,
 		break;
 	default:
 		dev_err(&client->dev, "unknown RTC type\n");
-		goto exit_kfree;
+		goto exit;
 	}
 
 	/* if the oscillator lost power and no other software (like
@@ -629,7 +631,7 @@ static int rs5c372_probe(struct i2c_client *client,
 	err = rs5c_oscillator_setup(rs5c372);
 	if (unlikely(err < 0)) {
 		dev_err(&client->dev, "setup error\n");
-		goto exit_kfree;
+		goto exit;
 	}
 
 	if (rs5c372_get_datetime(client, &tm) < 0)
@@ -648,38 +650,28 @@ static int rs5c372_probe(struct i2c_client *client,
 			);
 
 	/* REVISIT use client->irq to register alarm irq ... */
-
-	rs5c372->rtc = rtc_device_register(rs5c372_driver.driver.name,
-				&client->dev, &rs5c372_rtc_ops, THIS_MODULE);
+	rs5c372->rtc = devm_rtc_device_register(&client->dev,
+					rs5c372_driver.driver.name,
+					&rs5c372_rtc_ops, THIS_MODULE);
 
 	if (IS_ERR(rs5c372->rtc)) {
 		err = PTR_ERR(rs5c372->rtc);
-		goto exit_kfree;
+		goto exit;
 	}
 
 	err = rs5c_sysfs_register(&client->dev);
 	if (err)
-		goto exit_devreg;
+		goto exit;
 
 	return 0;
 
-exit_devreg:
-	rtc_device_unregister(rs5c372->rtc);
-
-exit_kfree:
-	kfree(rs5c372);
-
 exit:
 	return err;
 }
 
 static int rs5c372_remove(struct i2c_client *client)
 {
-	struct rs5c372 *rs5c372 = i2c_get_clientdata(client);
-
-	rtc_device_unregister(rs5c372->rtc);
 	rs5c_sysfs_unregister(&client->dev);
-	kfree(rs5c372);
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-rv3029c2.c b/drivers/rtc/rtc-rv3029c2.c
index f8ee8ad7825..5032c24ec15 100644
--- a/drivers/rtc/rtc-rv3029c2.c
+++ b/drivers/rtc/rtc-rv3029c2.c
@@ -395,9 +395,8 @@ static int rv3029c2_probe(struct i2c_client *client,
 	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_EMUL))
 		return -ENODEV;
 
-	rtc = rtc_device_register(client->name,
-				&client->dev, &rv3029c2_rtc_ops,
-				THIS_MODULE);
+	rtc = devm_rtc_device_register(&client->dev, client->name,
+					&rv3029c2_rtc_ops, THIS_MODULE);
 
 	if (IS_ERR(rtc))
 		return PTR_ERR(rtc);
@@ -407,23 +406,14 @@ static int rv3029c2_probe(struct i2c_client *client,
 	rc = rv3029c2_i2c_get_sr(client, buf);
 	if (rc < 0) {
 		dev_err(&client->dev, "reading status failed\n");
-		goto exit_unregister;
+		return rc;
 	}
 
 	return 0;
-
-exit_unregister:
-	rtc_device_unregister(rtc);
-
-	return rc;
 }
 
 static int rv3029c2_remove(struct i2c_client *client)
 {
-	struct rtc_device *rtc = i2c_get_clientdata(client);
-
-	rtc_device_unregister(rtc);
-
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-rx4581.c b/drivers/rtc/rtc-rx4581.c
index 599ec73ec88..84eb08d65d3 100644
--- a/drivers/rtc/rtc-rx4581.c
+++ b/drivers/rtc/rtc-rx4581.c
@@ -273,20 +273,17 @@ static int rx4581_probe(struct spi_device *spi)
 	if (res != 0)
 		return res;
 
-	rtc = rtc_device_register("rx4581",
-				&spi->dev, &rx4581_rtc_ops, THIS_MODULE);
+	rtc = devm_rtc_device_register(&spi->dev, "rx4581",
+				&rx4581_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc))
 		return PTR_ERR(rtc);
 
-	dev_set_drvdata(&spi->dev, rtc);
+	spi_set_drvdata(spi, rtc);
 	return 0;
 }
 
 static int rx4581_remove(struct spi_device *spi)
 {
-	struct rtc_device *rtc = dev_get_drvdata(&spi->dev);
-
-	rtc_device_unregister(rtc);
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-rx8581.c b/drivers/rtc/rtc-rx8581.c
index b0c272658fa..07f3037b18f 100644
--- a/drivers/rtc/rtc-rx8581.c
+++ b/drivers/rtc/rtc-rx8581.c
@@ -240,8 +240,8 @@ static int rx8581_probe(struct i2c_client *client,
 
 	dev_info(&client->dev, "chip found, driver version " DRV_VERSION "\n");
 
-	rtc = rtc_device_register(rx8581_driver.driver.name,
-				&client->dev, &rx8581_rtc_ops, THIS_MODULE);
+	rtc = devm_rtc_device_register(&client->dev, rx8581_driver.driver.name,
+					&rx8581_rtc_ops, THIS_MODULE);
 
 	if (IS_ERR(rtc))
 		return PTR_ERR(rtc);
@@ -253,10 +253,6 @@ static int rx8581_probe(struct i2c_client *client,
 
 static int rx8581_remove(struct i2c_client *client)
 {
-	struct rtc_device *rtc = i2c_get_clientdata(client);
-
-	rtc_device_unregister(rtc);
-
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-s35390a.c b/drivers/rtc/rtc-s35390a.c
index 8a092325188..f40afdd0e5f 100644
--- a/drivers/rtc/rtc-s35390a.c
+++ b/drivers/rtc/rtc-s35390a.c
@@ -338,7 +338,8 @@ static int s35390a_probe(struct i2c_client *client,
 		goto exit;
 	}
 
-	s35390a = kzalloc(sizeof(struct s35390a), GFP_KERNEL);
+	s35390a = devm_kzalloc(&client->dev, sizeof(struct s35390a),
+				GFP_KERNEL);
 	if (!s35390a) {
 		err = -ENOMEM;
 		goto exit;
@@ -386,8 +387,9 @@ static int s35390a_probe(struct i2c_client *client,
 
 	device_set_wakeup_capable(&client->dev, 1);
 
-	s35390a->rtc = rtc_device_register(s35390a_driver.driver.name,
-				&client->dev, &s35390a_rtc_ops, THIS_MODULE);
+	s35390a->rtc = devm_rtc_device_register(&client->dev,
+					s35390a_driver.driver.name,
+					&s35390a_rtc_ops, THIS_MODULE);
 
 	if (IS_ERR(s35390a->rtc)) {
 		err = PTR_ERR(s35390a->rtc);
@@ -399,7 +401,6 @@ exit_dummy:
 	for (i = 1; i < 8; ++i)
 		if (s35390a->client[i])
 			i2c_unregister_device(s35390a->client[i]);
-	kfree(s35390a);
 
 exit:
 	return err;
@@ -408,15 +409,12 @@ exit:
 static int s35390a_remove(struct i2c_client *client)
 {
 	unsigned int i;
-
 	struct s35390a *s35390a = i2c_get_clientdata(client);
+
 	for (i = 1; i < 8; ++i)
 		if (s35390a->client[i])
 			i2c_unregister_device(s35390a->client[i]);
 
-	rtc_device_unregister(s35390a->rtc);
-	kfree(s35390a);
-
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-s3c.c b/drivers/rtc/rtc-s3c.c
index fb994e9ddc1..8e96c00936b 100644
--- a/drivers/rtc/rtc-s3c.c
+++ b/drivers/rtc/rtc-s3c.c
@@ -51,7 +51,6 @@ static struct clk *rtc_clk;
 static void __iomem *s3c_rtc_base;
 static int s3c_rtc_alarmno = NO_IRQ;
 static int s3c_rtc_tickno  = NO_IRQ;
-static bool wake_en;
 static enum s3c_cpu_type s3c_rtc_cpu_type;
 
 static DEFINE_SPINLOCK(s3c_rtc_pie_lock);
@@ -423,13 +422,11 @@ static void s3c_rtc_enable(struct platform_device *pdev, int en)
 
 static int s3c_rtc_remove(struct platform_device *dev)
 {
-	struct rtc_device *rtc = platform_get_drvdata(dev);
-
 	platform_set_drvdata(dev, NULL);
-	rtc_device_unregister(rtc);
 
 	s3c_rtc_setaie(&dev->dev, 0);
 
+	clk_unprepare(rtc_clk);
 	rtc_clk = NULL;
 
 	return 0;
@@ -498,7 +495,7 @@ static int s3c_rtc_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	clk_enable(rtc_clk);
+	clk_prepare_enable(rtc_clk);
 
 	/* check to see if everything is setup correctly */
 
@@ -511,7 +508,7 @@ static int s3c_rtc_probe(struct platform_device *pdev)
 
 	/* register RTC and exit */
 
-	rtc = rtc_device_register("s3c", &pdev->dev, &s3c_rtcops,
+	rtc = devm_rtc_device_register(&pdev->dev, "s3c", &s3c_rtcops,
 				  THIS_MODULE);
 
 	if (IS_ERR(rtc)) {
@@ -574,23 +571,24 @@ static int s3c_rtc_probe(struct platform_device *pdev)
 
  err_alarm_irq:
 	platform_set_drvdata(pdev, NULL);
-	rtc_device_unregister(rtc);
 
  err_nortc:
 	s3c_rtc_enable(pdev, 0);
-	clk_disable(rtc_clk);
+	clk_disable_unprepare(rtc_clk);
 
 	return ret;
 }
 
-#ifdef CONFIG_PM
-
+#ifdef CONFIG_PM_SLEEP
 /* RTC Power management control */
 
 static int ticnt_save, ticnt_en_save;
+static bool wake_en;
 
-static int s3c_rtc_suspend(struct platform_device *pdev, pm_message_t state)
+static int s3c_rtc_suspend(struct device *dev)
 {
+	struct platform_device *pdev = to_platform_device(dev);
+
 	clk_enable(rtc_clk);
 	/* save TICNT for anyone using periodic interrupts */
 	ticnt_save = readb(s3c_rtc_base + S3C2410_TICNT);
@@ -600,19 +598,20 @@ static int s3c_rtc_suspend(struct platform_device *pdev, pm_message_t state)
 	}
 	s3c_rtc_enable(pdev, 0);
 
-	if (device_may_wakeup(&pdev->dev) && !wake_en) {
+	if (device_may_wakeup(dev) && !wake_en) {
 		if (enable_irq_wake(s3c_rtc_alarmno) == 0)
 			wake_en = true;
 		else
-			dev_err(&pdev->dev, "enable_irq_wake failed\n");
+			dev_err(dev, "enable_irq_wake failed\n");
 	}
 	clk_disable(rtc_clk);
 
 	return 0;
 }
 
-static int s3c_rtc_resume(struct platform_device *pdev)
+static int s3c_rtc_resume(struct device *dev)
 {
+	struct platform_device *pdev = to_platform_device(dev);
 	unsigned int tmp;
 
 	clk_enable(rtc_clk);
@@ -623,7 +622,7 @@ static int s3c_rtc_resume(struct platform_device *pdev)
 		writew(tmp | ticnt_en_save, s3c_rtc_base + S3C2410_RTCCON);
 	}
 
-	if (device_may_wakeup(&pdev->dev) && wake_en) {
+	if (device_may_wakeup(dev) && wake_en) {
 		disable_irq_wake(s3c_rtc_alarmno);
 		wake_en = false;
 	}
@@ -631,11 +630,10 @@ static int s3c_rtc_resume(struct platform_device *pdev)
 
 	return 0;
 }
-#else
-#define s3c_rtc_suspend NULL
-#define s3c_rtc_resume  NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(s3c_rtc_pm_ops, s3c_rtc_suspend, s3c_rtc_resume);
+
 #ifdef CONFIG_OF
 static struct s3c_rtc_drv_data s3c_rtc_drv_data_array[] = {
 	[TYPE_S3C2410] = { TYPE_S3C2410 },
@@ -685,12 +683,11 @@ MODULE_DEVICE_TABLE(platform, s3c_rtc_driver_ids);
 static struct platform_driver s3c_rtc_driver = {
 	.probe		= s3c_rtc_probe,
 	.remove		= s3c_rtc_remove,
-	.suspend	= s3c_rtc_suspend,
-	.resume		= s3c_rtc_resume,
 	.id_table	= s3c_rtc_driver_ids,
 	.driver		= {
 		.name	= "s3c-rtc",
 		.owner	= THIS_MODULE,
+		.pm	= &s3c_rtc_pm_ops,
 		.of_match_table	= of_match_ptr(s3c_rtc_dt_match),
 	},
 };
diff --git a/drivers/rtc/rtc-sa1100.c b/drivers/rtc/rtc-sa1100.c
index 5ec5036df0b..00605601dbf 100644
--- a/drivers/rtc/rtc-sa1100.c
+++ b/drivers/rtc/rtc-sa1100.c
@@ -234,14 +234,13 @@ static int sa1100_rtc_probe(struct platform_device *pdev)
 	if (irq_1hz < 0 || irq_alarm < 0)
 		return -ENODEV;
 
-	info = kzalloc(sizeof(struct sa1100_rtc), GFP_KERNEL);
+	info = devm_kzalloc(&pdev->dev, sizeof(struct sa1100_rtc), GFP_KERNEL);
 	if (!info)
 		return -ENOMEM;
-	info->clk = clk_get(&pdev->dev, NULL);
+	info->clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(info->clk)) {
 		dev_err(&pdev->dev, "failed to find rtc clock source\n");
-		ret = PTR_ERR(info->clk);
-		goto err_clk;
+		return PTR_ERR(info->clk);
 	}
 	info->irq_1hz = irq_1hz;
 	info->irq_alarm = irq_alarm;
@@ -268,8 +267,8 @@ static int sa1100_rtc_probe(struct platform_device *pdev)
 
 	device_init_wakeup(&pdev->dev, 1);
 
-	rtc = rtc_device_register(pdev->name, &pdev->dev, &sa1100_rtc_ops,
-		THIS_MODULE);
+	rtc = devm_rtc_device_register(&pdev->dev, pdev->name, &sa1100_rtc_ops,
+					THIS_MODULE);
 
 	if (IS_ERR(rtc)) {
 		ret = PTR_ERR(rtc);
@@ -306,9 +305,6 @@ err_dev:
 	clk_disable_unprepare(info->clk);
 err_enable_clk:
 	platform_set_drvdata(pdev, NULL);
-	clk_put(info->clk);
-err_clk:
-	kfree(info);
 	return ret;
 }
 
@@ -317,17 +313,14 @@ static int sa1100_rtc_remove(struct platform_device *pdev)
 	struct sa1100_rtc *info = platform_get_drvdata(pdev);
 
 	if (info) {
-		rtc_device_unregister(info->rtc);
 		clk_disable_unprepare(info->clk);
-		clk_put(info->clk);
 		platform_set_drvdata(pdev, NULL);
-		kfree(info);
 	}
 
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int sa1100_rtc_suspend(struct device *dev)
 {
 	struct sa1100_rtc *info = dev_get_drvdata(dev);
@@ -343,13 +336,11 @@ static int sa1100_rtc_resume(struct device *dev)
 		disable_irq_wake(info->irq_alarm);
 	return 0;
 }
-
-static const struct dev_pm_ops sa1100_rtc_pm_ops = {
-	.suspend	= sa1100_rtc_suspend,
-	.resume		= sa1100_rtc_resume,
-};
 #endif
 
+static SIMPLE_DEV_PM_OPS(sa1100_rtc_pm_ops, sa1100_rtc_suspend,
+			sa1100_rtc_resume);
+
 #ifdef CONFIG_OF
 static struct of_device_id sa1100_rtc_dt_ids[] = {
 	{ .compatible = "mrvl,sa1100-rtc", },
@@ -364,9 +355,7 @@ static struct platform_driver sa1100_rtc_driver = {
 	.remove		= sa1100_rtc_remove,
 	.driver		= {
 		.name	= "sa1100-rtc",
-#ifdef CONFIG_PM
 		.pm	= &sa1100_rtc_pm_ops,
-#endif
 		.of_match_table = of_match_ptr(sa1100_rtc_dt_ids),
 	},
 };
diff --git a/drivers/rtc/rtc-sh.c b/drivers/rtc/rtc-sh.c
index e55a7635ae5..8d5bd2e3677 100644
--- a/drivers/rtc/rtc-sh.c
+++ b/drivers/rtc/rtc-sh.c
@@ -790,6 +790,7 @@ static void sh_rtc_set_irq_wake(struct device *dev, int enabled)
 	}
 }
 
+#ifdef CONFIG_PM_SLEEP
 static int sh_rtc_suspend(struct device *dev)
 {
 	if (device_may_wakeup(dev))
@@ -805,33 +806,20 @@ static int sh_rtc_resume(struct device *dev)
 
 	return 0;
 }
+#endif
 
-static const struct dev_pm_ops sh_rtc_dev_pm_ops = {
-	.suspend = sh_rtc_suspend,
-	.resume = sh_rtc_resume,
-};
+static SIMPLE_DEV_PM_OPS(sh_rtc_pm_ops, sh_rtc_suspend, sh_rtc_resume);
 
 static struct platform_driver sh_rtc_platform_driver = {
 	.driver		= {
 		.name	= DRV_NAME,
 		.owner	= THIS_MODULE,
-		.pm	= &sh_rtc_dev_pm_ops,
+		.pm	= &sh_rtc_pm_ops,
 	},
 	.remove		= __exit_p(sh_rtc_remove),
 };
 
-static int __init sh_rtc_init(void)
-{
-	return platform_driver_probe(&sh_rtc_platform_driver, sh_rtc_probe);
-}
-
-static void __exit sh_rtc_exit(void)
-{
-	platform_driver_unregister(&sh_rtc_platform_driver);
-}
-
-module_init(sh_rtc_init);
-module_exit(sh_rtc_exit);
+module_platform_driver_probe(sh_rtc_platform_driver, sh_rtc_probe);
 
 MODULE_DESCRIPTION("SuperH on-chip RTC driver");
 MODULE_VERSION(DRV_VERSION);
diff --git a/drivers/rtc/rtc-snvs.c b/drivers/rtc/rtc-snvs.c
index f7d90703db5..b04f09a1df2 100644
--- a/drivers/rtc/rtc-snvs.c
+++ b/drivers/rtc/rtc-snvs.c
@@ -283,7 +283,7 @@ static int snvs_rtc_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	data->rtc = rtc_device_register(pdev->name, &pdev->dev,
+	data->rtc = devm_rtc_device_register(&pdev->dev, pdev->name,
 					&snvs_rtc_ops, THIS_MODULE);
 	if (IS_ERR(data->rtc)) {
 		ret = PTR_ERR(data->rtc);
@@ -296,10 +296,6 @@ static int snvs_rtc_probe(struct platform_device *pdev)
 
 static int snvs_rtc_remove(struct platform_device *pdev)
 {
-	struct snvs_rtc_data *data = platform_get_drvdata(pdev);
-
-	rtc_device_unregister(data->rtc);
-
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-spear.c b/drivers/rtc/rtc-spear.c
index a18c3192ed4..574359c48f6 100644
--- a/drivers/rtc/rtc-spear.c
+++ b/drivers/rtc/rtc-spear.c
@@ -400,8 +400,8 @@ static int spear_rtc_probe(struct platform_device *pdev)
 	spin_lock_init(&config->lock);
 	platform_set_drvdata(pdev, config);
 
-	config->rtc = rtc_device_register(pdev->name, &pdev->dev,
-			&spear_rtc_ops, THIS_MODULE);
+	config->rtc = devm_rtc_device_register(&pdev->dev, pdev->name,
+					&spear_rtc_ops, THIS_MODULE);
 	if (IS_ERR(config->rtc)) {
 		dev_err(&pdev->dev, "can't register RTC device, err %ld\n",
 				PTR_ERR(config->rtc));
@@ -427,7 +427,6 @@ static int spear_rtc_remove(struct platform_device *pdev)
 {
 	struct spear_rtc_config *config = platform_get_drvdata(pdev);
 
-	rtc_device_unregister(config->rtc);
 	spear_rtc_disable_interrupt(config);
 	clk_disable_unprepare(config->clk);
 	device_init_wakeup(&pdev->dev, 0);
@@ -435,10 +434,10 @@ static int spear_rtc_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
-
-static int spear_rtc_suspend(struct platform_device *pdev, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int spear_rtc_suspend(struct device *dev)
 {
+	struct platform_device *pdev = to_platform_device(dev);
 	struct spear_rtc_config *config = platform_get_drvdata(pdev);
 	int irq;
 
@@ -454,8 +453,9 @@ static int spear_rtc_suspend(struct platform_device *pdev, pm_message_t state)
 	return 0;
 }
 
-static int spear_rtc_resume(struct platform_device *pdev)
+static int spear_rtc_resume(struct device *dev)
 {
+	struct platform_device *pdev = to_platform_device(dev);
 	struct spear_rtc_config *config = platform_get_drvdata(pdev);
 	int irq;
 
@@ -473,12 +473,10 @@ static int spear_rtc_resume(struct platform_device *pdev)
 
 	return 0;
 }
-
-#else
-#define spear_rtc_suspend	NULL
-#define spear_rtc_resume	NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(spear_rtc_pm_ops, spear_rtc_suspend, spear_rtc_resume);
+
 static void spear_rtc_shutdown(struct platform_device *pdev)
 {
 	struct spear_rtc_config *config = platform_get_drvdata(pdev);
@@ -498,11 +496,10 @@ MODULE_DEVICE_TABLE(of, spear_rtc_id_table);
 static struct platform_driver spear_rtc_driver = {
 	.probe = spear_rtc_probe,
 	.remove = spear_rtc_remove,
-	.suspend = spear_rtc_suspend,
-	.resume = spear_rtc_resume,
 	.shutdown = spear_rtc_shutdown,
 	.driver = {
 		.name = "rtc-spear",
+		.pm = &spear_rtc_pm_ops,
 		.of_match_table = of_match_ptr(spear_rtc_id_table),
 	},
 };
diff --git a/drivers/rtc/rtc-starfire.c b/drivers/rtc/rtc-starfire.c
index 5be98bfd7ed..987b5ec0ae5 100644
--- a/drivers/rtc/rtc-starfire.c
+++ b/drivers/rtc/rtc-starfire.c
@@ -39,8 +39,10 @@ static const struct rtc_class_ops starfire_rtc_ops = {
 
 static int __init starfire_rtc_probe(struct platform_device *pdev)
 {
-	struct rtc_device *rtc = rtc_device_register("starfire", &pdev->dev,
-				     &starfire_rtc_ops, THIS_MODULE);
+	struct rtc_device *rtc;
+
+	rtc = devm_rtc_device_register(&pdev->dev, "starfire",
+				&starfire_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc))
 		return PTR_ERR(rtc);
 
@@ -51,10 +53,6 @@ static int __init starfire_rtc_probe(struct platform_device *pdev)
 
 static int __exit starfire_rtc_remove(struct platform_device *pdev)
 {
-	struct rtc_device *rtc = platform_get_drvdata(pdev);
-
-	rtc_device_unregister(rtc);
-
 	return 0;
 }
 
@@ -66,15 +64,4 @@ static struct platform_driver starfire_rtc_driver = {
 	.remove		= __exit_p(starfire_rtc_remove),
 };
 
-static int __init starfire_rtc_init(void)
-{
-	return platform_driver_probe(&starfire_rtc_driver, starfire_rtc_probe);
-}
-
-static void __exit starfire_rtc_exit(void)
-{
-	platform_driver_unregister(&starfire_rtc_driver);
-}
-
-module_init(starfire_rtc_init);
-module_exit(starfire_rtc_exit);
+module_platform_driver_probe(starfire_rtc_driver, starfire_rtc_probe);
diff --git a/drivers/rtc/rtc-stk17ta8.c b/drivers/rtc/rtc-stk17ta8.c
index 7e4a6f65cb9..af5e97e3f27 100644
--- a/drivers/rtc/rtc-stk17ta8.c
+++ b/drivers/rtc/rtc-stk17ta8.c
@@ -336,14 +336,13 @@ static int stk17ta8_rtc_probe(struct platform_device *pdev)
 		}
 	}
 
-	pdata->rtc = rtc_device_register(pdev->name, &pdev->dev,
+	pdata->rtc = devm_rtc_device_register(&pdev->dev, pdev->name,
 				  &stk17ta8_rtc_ops, THIS_MODULE);
 	if (IS_ERR(pdata->rtc))
 		return PTR_ERR(pdata->rtc);
 
 	ret = sysfs_create_bin_file(&pdev->dev.kobj, &stk17ta8_nvram_attr);
-	if (ret)
-		rtc_device_unregister(pdata->rtc);
+
 	return ret;
 }
 
@@ -352,7 +351,6 @@ static int stk17ta8_rtc_remove(struct platform_device *pdev)
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 
 	sysfs_remove_bin_file(&pdev->dev.kobj, &stk17ta8_nvram_attr);
-	rtc_device_unregister(pdata->rtc);
 	if (pdata->irq > 0)
 		writeb(0, pdata->ioaddr + RTC_INTERRUPTS);
 	return 0;
diff --git a/drivers/rtc/rtc-stmp3xxx.c b/drivers/rtc/rtc-stmp3xxx.c
index 98f0d3c3073..a9cd26a26dc 100644
--- a/drivers/rtc/rtc-stmp3xxx.c
+++ b/drivers/rtc/rtc-stmp3xxx.c
@@ -227,11 +227,7 @@ static int stmp3xxx_rtc_remove(struct platform_device *pdev)
 
 	writel(STMP3XXX_RTC_CTRL_ALARM_IRQ_EN,
 			rtc_data->io + STMP3XXX_RTC_CTRL_CLR);
-	free_irq(rtc_data->irq_alarm, &pdev->dev);
-	rtc_device_unregister(rtc_data->rtc);
 	platform_set_drvdata(pdev, NULL);
-	iounmap(rtc_data->io);
-	kfree(rtc_data);
 
 	return 0;
 }
@@ -242,22 +238,20 @@ static int stmp3xxx_rtc_probe(struct platform_device *pdev)
 	struct resource *r;
 	int err;
 
-	rtc_data = kzalloc(sizeof *rtc_data, GFP_KERNEL);
+	rtc_data = devm_kzalloc(&pdev->dev, sizeof(*rtc_data), GFP_KERNEL);
 	if (!rtc_data)
 		return -ENOMEM;
 
 	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!r) {
 		dev_err(&pdev->dev, "failed to get resource\n");
-		err = -ENXIO;
-		goto out_free;
+		return -ENXIO;
 	}
 
-	rtc_data->io = ioremap(r->start, resource_size(r));
+	rtc_data->io = devm_ioremap(&pdev->dev, r->start, resource_size(r));
 	if (!rtc_data->io) {
 		dev_err(&pdev->dev, "ioremap failed\n");
-		err = -EIO;
-		goto out_free;
+		return -EIO;
 	}
 
 	rtc_data->irq_alarm = platform_get_irq(pdev, 0);
@@ -265,8 +259,7 @@ static int stmp3xxx_rtc_probe(struct platform_device *pdev)
 	if (!(readl(STMP3XXX_RTC_STAT + rtc_data->io) &
 			STMP3XXX_RTC_STAT_RTC_PRESENT)) {
 		dev_err(&pdev->dev, "no device onboard\n");
-		err = -ENODEV;
-		goto out_remap;
+		return -ENODEV;
 	}
 
 	platform_set_drvdata(pdev, rtc_data);
@@ -281,43 +274,38 @@ static int stmp3xxx_rtc_probe(struct platform_device *pdev)
 			STMP3XXX_RTC_CTRL_ALARM_IRQ_EN,
 			rtc_data->io + STMP3XXX_RTC_CTRL_CLR);
 
-	rtc_data->rtc = rtc_device_register(pdev->name, &pdev->dev,
+	rtc_data->rtc = devm_rtc_device_register(&pdev->dev, pdev->name,
 				&stmp3xxx_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc_data->rtc)) {
 		err = PTR_ERR(rtc_data->rtc);
-		goto out_remap;
+		goto out;
 	}
 
-	err = request_irq(rtc_data->irq_alarm, stmp3xxx_rtc_interrupt, 0,
-			"RTC alarm", &pdev->dev);
+	err = devm_request_irq(&pdev->dev, rtc_data->irq_alarm,
+			stmp3xxx_rtc_interrupt, 0, "RTC alarm", &pdev->dev);
 	if (err) {
 		dev_err(&pdev->dev, "Cannot claim IRQ%d\n",
 			rtc_data->irq_alarm);
-		goto out_irq_alarm;
+		goto out;
 	}
 
 	stmp3xxx_wdt_register(pdev);
 	return 0;
 
-out_irq_alarm:
-	rtc_device_unregister(rtc_data->rtc);
-out_remap:
+out:
 	platform_set_drvdata(pdev, NULL);
-	iounmap(rtc_data->io);
-out_free:
-	kfree(rtc_data);
 	return err;
 }
 
-#ifdef CONFIG_PM
-static int stmp3xxx_rtc_suspend(struct platform_device *dev, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int stmp3xxx_rtc_suspend(struct device *dev)
 {
 	return 0;
 }
 
-static int stmp3xxx_rtc_resume(struct platform_device *dev)
+static int stmp3xxx_rtc_resume(struct device *dev)
 {
-	struct stmp3xxx_rtc_data *rtc_data = platform_get_drvdata(dev);
+	struct stmp3xxx_rtc_data *rtc_data = dev_get_drvdata(dev);
 
 	mxs_reset_block(rtc_data->io);
 	writel(STMP3XXX_RTC_PERSISTENT0_ALARM_EN |
@@ -326,11 +314,11 @@ static int stmp3xxx_rtc_resume(struct platform_device *dev)
 			rtc_data->io + STMP3XXX_RTC_PERSISTENT0_CLR);
 	return 0;
 }
-#else
-#define stmp3xxx_rtc_suspend	NULL
-#define stmp3xxx_rtc_resume	NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(stmp3xxx_rtc_pm_ops, stmp3xxx_rtc_suspend,
+			stmp3xxx_rtc_resume);
+
 static const struct of_device_id rtc_dt_ids[] = {
 	{ .compatible = "fsl,stmp3xxx-rtc", },
 	{ /* sentinel */ }
@@ -340,11 +328,10 @@ MODULE_DEVICE_TABLE(of, rtc_dt_ids);
 static struct platform_driver stmp3xxx_rtcdrv = {
 	.probe		= stmp3xxx_rtc_probe,
 	.remove		= stmp3xxx_rtc_remove,
-	.suspend	= stmp3xxx_rtc_suspend,
-	.resume		= stmp3xxx_rtc_resume,
 	.driver		= {
 		.name	= "stmp3xxx-rtc",
 		.owner	= THIS_MODULE,
+		.pm	= &stmp3xxx_rtc_pm_ops,
 		.of_match_table = of_match_ptr(rtc_dt_ids),
 	},
 };
diff --git a/drivers/rtc/rtc-sun4v.c b/drivers/rtc/rtc-sun4v.c
index 59b5c2dcb58..ce42e5fa9e0 100644
--- a/drivers/rtc/rtc-sun4v.c
+++ b/drivers/rtc/rtc-sun4v.c
@@ -81,8 +81,10 @@ static const struct rtc_class_ops sun4v_rtc_ops = {
 
 static int __init sun4v_rtc_probe(struct platform_device *pdev)
 {
-	struct rtc_device *rtc = rtc_device_register("sun4v", &pdev->dev,
-				     &sun4v_rtc_ops, THIS_MODULE);
+	struct rtc_device *rtc;
+
+	rtc = devm_rtc_device_register(&pdev->dev, "sun4v",
+				&sun4v_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc))
 		return PTR_ERR(rtc);
 
@@ -92,9 +94,6 @@ static int __init sun4v_rtc_probe(struct platform_device *pdev)
 
 static int __exit sun4v_rtc_remove(struct platform_device *pdev)
 {
-	struct rtc_device *rtc = platform_get_drvdata(pdev);
-
-	rtc_device_unregister(rtc);
 	return 0;
 }
 
@@ -106,18 +105,7 @@ static struct platform_driver sun4v_rtc_driver = {
 	.remove		= __exit_p(sun4v_rtc_remove),
 };
 
-static int __init sun4v_rtc_init(void)
-{
-	return platform_driver_probe(&sun4v_rtc_driver, sun4v_rtc_probe);
-}
-
-static void __exit sun4v_rtc_exit(void)
-{
-	platform_driver_unregister(&sun4v_rtc_driver);
-}
-
-module_init(sun4v_rtc_init);
-module_exit(sun4v_rtc_exit);
+module_platform_driver_probe(sun4v_rtc_driver, sun4v_rtc_probe);
 
 MODULE_AUTHOR("David S. Miller <davem@davemloft.net>");
 MODULE_DESCRIPTION("SUN4V RTC driver");
diff --git a/drivers/rtc/rtc-tegra.c b/drivers/rtc/rtc-tegra.c
index 7c033756d6b..a34315d2547 100644
--- a/drivers/rtc/rtc-tegra.c
+++ b/drivers/rtc/rtc-tegra.c
@@ -26,6 +26,7 @@
 #include <linux/delay.h>
 #include <linux/rtc.h>
 #include <linux/platform_device.h>
+#include <linux/pm.h>
 
 /* set to 1 = busy every eight 32kHz clocks during copy of sec+msec to AHB */
 #define TEGRA_RTC_REG_BUSY			0x004
@@ -309,7 +310,7 @@ static const struct of_device_id tegra_rtc_dt_match[] = {
 };
 MODULE_DEVICE_TABLE(of, tegra_rtc_dt_match);
 
-static int tegra_rtc_probe(struct platform_device *pdev)
+static int __init tegra_rtc_probe(struct platform_device *pdev)
 {
 	struct tegra_rtc_info *info;
 	struct resource *res;
@@ -348,53 +349,35 @@ static int tegra_rtc_probe(struct platform_device *pdev)
 
 	device_init_wakeup(&pdev->dev, 1);
 
-	info->rtc_dev = rtc_device_register(
-		pdev->name, &pdev->dev, &tegra_rtc_ops, THIS_MODULE);
+	info->rtc_dev = devm_rtc_device_register(&pdev->dev,
+				dev_name(&pdev->dev), &tegra_rtc_ops,
+				THIS_MODULE);
 	if (IS_ERR(info->rtc_dev)) {
 		ret = PTR_ERR(info->rtc_dev);
-		info->rtc_dev = NULL;
-		dev_err(&pdev->dev,
-			"Unable to register device (err=%d).\n",
+		dev_err(&pdev->dev, "Unable to register device (err=%d).\n",
 			ret);
 		return ret;
 	}
 
 	ret = devm_request_irq(&pdev->dev, info->tegra_rtc_irq,
 			tegra_rtc_irq_handler, IRQF_TRIGGER_HIGH,
-			"rtc alarm", &pdev->dev);
+			dev_name(&pdev->dev), &pdev->dev);
 	if (ret) {
 		dev_err(&pdev->dev,
 			"Unable to request interrupt for device (err=%d).\n",
 			ret);
-		goto err_dev_unreg;
+		return ret;
 	}
 
 	dev_notice(&pdev->dev, "Tegra internal Real Time Clock\n");
 
 	return 0;
-
-err_dev_unreg:
-	rtc_device_unregister(info->rtc_dev);
-
-	return ret;
 }
 
-static int tegra_rtc_remove(struct platform_device *pdev)
+#ifdef CONFIG_PM_SLEEP
+static int tegra_rtc_suspend(struct device *dev)
 {
-	struct tegra_rtc_info *info = platform_get_drvdata(pdev);
-
-	rtc_device_unregister(info->rtc_dev);
-
-	platform_set_drvdata(pdev, NULL);
-
-	return 0;
-}
-
-#ifdef CONFIG_PM
-static int tegra_rtc_suspend(struct platform_device *pdev, pm_message_t state)
-{
-	struct device *dev = &pdev->dev;
-	struct tegra_rtc_info *info = platform_get_drvdata(pdev);
+	struct tegra_rtc_info *info = dev_get_drvdata(dev);
 
 	tegra_rtc_wait_while_busy(dev);
 
@@ -416,10 +399,9 @@ static int tegra_rtc_suspend(struct platform_device *pdev, pm_message_t state)
 	return 0;
 }
 
-static int tegra_rtc_resume(struct platform_device *pdev)
+static int tegra_rtc_resume(struct device *dev)
 {
-	struct device *dev = &pdev->dev;
-	struct tegra_rtc_info *info = platform_get_drvdata(pdev);
+	struct tegra_rtc_info *info = dev_get_drvdata(dev);
 
 	dev_vdbg(dev, "Resume (device_may_wakeup=%d)\n",
 		device_may_wakeup(dev));
@@ -431,6 +413,8 @@ static int tegra_rtc_resume(struct platform_device *pdev)
 }
 #endif
 
+static SIMPLE_DEV_PM_OPS(tegra_rtc_pm_ops, tegra_rtc_suspend, tegra_rtc_resume);
+
 static void tegra_rtc_shutdown(struct platform_device *pdev)
 {
 	dev_vdbg(&pdev->dev, "disabling interrupts.\n");
@@ -439,30 +423,16 @@ static void tegra_rtc_shutdown(struct platform_device *pdev)
 
 MODULE_ALIAS("platform:tegra_rtc");
 static struct platform_driver tegra_rtc_driver = {
-	.remove		= tegra_rtc_remove,
 	.shutdown	= tegra_rtc_shutdown,
 	.driver		= {
 		.name	= "tegra_rtc",
 		.owner	= THIS_MODULE,
 		.of_match_table = tegra_rtc_dt_match,
+		.pm	= &tegra_rtc_pm_ops,
 	},
-#ifdef CONFIG_PM
-	.suspend	= tegra_rtc_suspend,
-	.resume		= tegra_rtc_resume,
-#endif
 };
 
-static int __init tegra_rtc_init(void)
-{
-	return platform_driver_probe(&tegra_rtc_driver, tegra_rtc_probe);
-}
-module_init(tegra_rtc_init);
-
-static void __exit tegra_rtc_exit(void)
-{
-	platform_driver_unregister(&tegra_rtc_driver);
-}
-module_exit(tegra_rtc_exit);
+module_platform_driver_probe(tegra_rtc_driver, tegra_rtc_probe);
 
 MODULE_AUTHOR("Jon Mayo <jmayo@nvidia.com>");
 MODULE_DESCRIPTION("driver for Tegra internal RTC");
diff --git a/drivers/rtc/rtc-test.c b/drivers/rtc/rtc-test.c
index b92e0f6383e..7746e65b93f 100644
--- a/drivers/rtc/rtc-test.c
+++ b/drivers/rtc/rtc-test.c
@@ -99,8 +99,10 @@ static DEVICE_ATTR(irq, S_IRUGO | S_IWUSR, test_irq_show, test_irq_store);
 static int test_probe(struct platform_device *plat_dev)
 {
 	int err;
-	struct rtc_device *rtc = rtc_device_register("test", &plat_dev->dev,
-						&test_rtc_ops, THIS_MODULE);
+	struct rtc_device *rtc;
+
+	rtc = devm_rtc_device_register(&plat_dev->dev, "test",
+				&test_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc)) {
 		err = PTR_ERR(rtc);
 		return err;
@@ -115,15 +117,11 @@ static int test_probe(struct platform_device *plat_dev)
 	return 0;
 
 err:
-	rtc_device_unregister(rtc);
 	return err;
 }
 
 static int test_remove(struct platform_device *plat_dev)
 {
-	struct rtc_device *rtc = platform_get_drvdata(plat_dev);
-
-	rtc_device_unregister(rtc);
 	device_remove_file(&plat_dev->dev, &dev_attr_irq);
 
 	return 0;
diff --git a/drivers/rtc/rtc-tile.c b/drivers/rtc/rtc-tile.c
index 62db4841078..249b6531f11 100644
--- a/drivers/rtc/rtc-tile.c
+++ b/drivers/rtc/rtc-tile.c
@@ -80,8 +80,8 @@ static int tile_rtc_probe(struct platform_device *dev)
 {
 	struct rtc_device *rtc;
 
-	rtc = rtc_device_register("tile",
-				  &dev->dev, &tile_rtc_ops, THIS_MODULE);
+	rtc = devm_rtc_device_register(&dev->dev, "tile",
+				&tile_rtc_ops, THIS_MODULE);
 
 	if (IS_ERR(rtc))
 		return PTR_ERR(rtc);
@@ -96,11 +96,6 @@ static int tile_rtc_probe(struct platform_device *dev)
  */
 static int tile_rtc_remove(struct platform_device *dev)
 {
-	struct rtc_device *rtc = platform_get_drvdata(dev);
-
-	if (rtc)
-		rtc_device_unregister(rtc);
-
 	platform_set_drvdata(dev, NULL);
 
 	return 0;
diff --git a/drivers/rtc/rtc-tps6586x.c b/drivers/rtc/rtc-tps6586x.c
index aab4e8c9362..459c2ffc95a 100644
--- a/drivers/rtc/rtc-tps6586x.c
+++ b/drivers/rtc/rtc-tps6586x.c
@@ -274,7 +274,7 @@ static int tps6586x_rtc_probe(struct platform_device *pdev)
 	}
 
 	platform_set_drvdata(pdev, rtc);
-	rtc->rtc = rtc_device_register(dev_name(&pdev->dev), &pdev->dev,
+	rtc->rtc = devm_rtc_device_register(&pdev->dev, dev_name(&pdev->dev),
 				       &tps6586x_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc->rtc)) {
 		ret = PTR_ERR(rtc->rtc);
@@ -289,15 +289,12 @@ static int tps6586x_rtc_probe(struct platform_device *pdev)
 	if (ret < 0) {
 		dev_err(&pdev->dev, "request IRQ(%d) failed with ret %d\n",
 				rtc->irq, ret);
-		goto fail_req_irq;
+		goto fail_rtc_register;
 	}
 	disable_irq(rtc->irq);
 	device_set_wakeup_capable(&pdev->dev, 1);
 	return 0;
 
-fail_req_irq:
-	rtc_device_unregister(rtc->rtc);
-
 fail_rtc_register:
 	tps6586x_update(tps_dev, RTC_CTRL, 0,
 		RTC_ENABLE | OSC_SRC_SEL | PRE_BYPASS | CL_SEL_MASK);
@@ -306,12 +303,10 @@ fail_rtc_register:
 
 static int tps6586x_rtc_remove(struct platform_device *pdev)
 {
-	struct tps6586x_rtc *rtc = platform_get_drvdata(pdev);
 	struct device *tps_dev = to_tps6586x_dev(&pdev->dev);
 
 	tps6586x_update(tps_dev, RTC_CTRL, 0,
 		RTC_ENABLE | OSC_SRC_SEL | PRE_BYPASS | CL_SEL_MASK);
-	rtc_device_unregister(rtc->rtc);
 	return 0;
 }
 
@@ -335,9 +330,8 @@ static int tps6586x_rtc_resume(struct device *dev)
 }
 #endif
 
-static const struct dev_pm_ops tps6586x_pm_ops = {
-	SET_SYSTEM_SLEEP_PM_OPS(tps6586x_rtc_suspend, tps6586x_rtc_resume)
-};
+static SIMPLE_DEV_PM_OPS(tps6586x_pm_ops, tps6586x_rtc_suspend,
+			tps6586x_rtc_resume);
 
 static struct platform_driver tps6586x_rtc_driver = {
 	.driver	= {
diff --git a/drivers/rtc/rtc-tps65910.c b/drivers/rtc/rtc-tps65910.c
index 8bd8115329b..a9caf043b0c 100644
--- a/drivers/rtc/rtc-tps65910.c
+++ b/drivers/rtc/rtc-tps65910.c
@@ -263,7 +263,7 @@ static int tps65910_rtc_probe(struct platform_device *pdev)
 	if (irq <= 0) {
 		dev_warn(&pdev->dev, "Wake up is not possible as irq = %d\n",
 			irq);
-		return ret;
+		return -ENXIO;
 	}
 
 	ret = devm_request_threaded_irq(&pdev->dev, irq, NULL,
@@ -276,7 +276,7 @@ static int tps65910_rtc_probe(struct platform_device *pdev)
 	tps_rtc->irq = irq;
 	device_set_wakeup_capable(&pdev->dev, 1);
 
-	tps_rtc->rtc = rtc_device_register(pdev->name, &pdev->dev,
+	tps_rtc->rtc = devm_rtc_device_register(&pdev->dev, pdev->name,
 		&tps65910_rtc_ops, THIS_MODULE);
 	if (IS_ERR(tps_rtc->rtc)) {
 		ret = PTR_ERR(tps_rtc->rtc);
@@ -295,12 +295,8 @@ static int tps65910_rtc_probe(struct platform_device *pdev)
  */
 static int tps65910_rtc_remove(struct platform_device *pdev)
 {
-	/* leave rtc running, but disable irqs */
-	struct tps65910_rtc *tps_rtc = platform_get_drvdata(pdev);
-
 	tps65910_rtc_alarm_irq_enable(&pdev->dev, 0);
 
-	rtc_device_unregister(tps_rtc->rtc);
 	return 0;
 }
 
@@ -324,9 +320,8 @@ static int tps65910_rtc_resume(struct device *dev)
 }
 #endif
 
-static const struct dev_pm_ops tps65910_rtc_pm_ops = {
-	SET_SYSTEM_SLEEP_PM_OPS(tps65910_rtc_suspend, tps65910_rtc_resume)
-};
+static SIMPLE_DEV_PM_OPS(tps65910_rtc_pm_ops, tps65910_rtc_suspend,
+			tps65910_rtc_resume);
 
 static struct platform_driver tps65910_rtc_driver = {
 	.probe		= tps65910_rtc_probe,
diff --git a/drivers/rtc/rtc-tps80031.c b/drivers/rtc/rtc-tps80031.c
index 9aaf8aaebae..72662eafb93 100644
--- a/drivers/rtc/rtc-tps80031.c
+++ b/drivers/rtc/rtc-tps80031.c
@@ -277,7 +277,7 @@ static int tps80031_rtc_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	rtc->rtc = rtc_device_register(pdev->name, &pdev->dev,
+	rtc->rtc = devm_rtc_device_register(&pdev->dev, pdev->name,
 			       &tps80031_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc->rtc)) {
 		ret = PTR_ERR(rtc->rtc);
@@ -292,7 +292,6 @@ static int tps80031_rtc_probe(struct platform_device *pdev)
 	if (ret < 0) {
 		dev_err(&pdev->dev, "request IRQ:%d failed, err = %d\n",
 			 rtc->irq, ret);
-		rtc_device_unregister(rtc->rtc);
 		return ret;
 	}
 	device_set_wakeup_capable(&pdev->dev, 1);
@@ -301,9 +300,6 @@ static int tps80031_rtc_probe(struct platform_device *pdev)
 
 static int tps80031_rtc_remove(struct platform_device *pdev)
 {
-	struct tps80031_rtc *rtc = platform_get_drvdata(pdev);
-
-	rtc_device_unregister(rtc->rtc);
 	return 0;
 }
 
@@ -327,9 +323,8 @@ static int tps80031_rtc_resume(struct device *dev)
 };
 #endif
 
-static const struct dev_pm_ops tps80031_pm_ops = {
-	SET_SYSTEM_SLEEP_PM_OPS(tps80031_rtc_suspend, tps80031_rtc_resume)
-};
+static SIMPLE_DEV_PM_OPS(tps80031_pm_ops, tps80031_rtc_suspend,
+			tps80031_rtc_resume);
 
 static struct platform_driver tps80031_rtc_driver = {
 	.driver	= {
diff --git a/drivers/rtc/rtc-twl.c b/drivers/rtc/rtc-twl.c
index 8bc6c80b184..8751a5240c9 100644
--- a/drivers/rtc/rtc-twl.c
+++ b/drivers/rtc/rtc-twl.c
@@ -566,11 +566,10 @@ static void twl_rtc_shutdown(struct platform_device *pdev)
 	mask_rtc_irq_bit(BIT_RTC_INTERRUPTS_REG_IT_TIMER_M);
 }
 
-#ifdef CONFIG_PM
-
+#ifdef CONFIG_PM_SLEEP
 static unsigned char irqstat;
 
-static int twl_rtc_suspend(struct platform_device *pdev, pm_message_t state)
+static int twl_rtc_suspend(struct device *dev)
 {
 	irqstat = rtc_irq_bits;
 
@@ -578,17 +577,15 @@ static int twl_rtc_suspend(struct platform_device *pdev, pm_message_t state)
 	return 0;
 }
 
-static int twl_rtc_resume(struct platform_device *pdev)
+static int twl_rtc_resume(struct device *dev)
 {
 	set_rtc_irq_bit(irqstat);
 	return 0;
 }
-
-#else
-#define twl_rtc_suspend NULL
-#define twl_rtc_resume  NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(twl_rtc_pm_ops, twl_rtc_suspend, twl_rtc_resume);
+
 #ifdef CONFIG_OF
 static const struct of_device_id twl_rtc_of_match[] = {
 	{.compatible = "ti,twl4030-rtc", },
@@ -603,11 +600,10 @@ static struct platform_driver twl4030rtc_driver = {
 	.probe		= twl_rtc_probe,
 	.remove		= twl_rtc_remove,
 	.shutdown	= twl_rtc_shutdown,
-	.suspend	= twl_rtc_suspend,
-	.resume		= twl_rtc_resume,
 	.driver		= {
 		.owner		= THIS_MODULE,
 		.name		= "twl_rtc",
+		.pm		= &twl_rtc_pm_ops,
 		.of_match_table = of_match_ptr(twl_rtc_of_match),
 	},
 };
diff --git a/drivers/rtc/rtc-tx4939.c b/drivers/rtc/rtc-tx4939.c
index a12bfac49d3..f9a0677e4e3 100644
--- a/drivers/rtc/rtc-tx4939.c
+++ b/drivers/rtc/rtc-tx4939.c
@@ -268,14 +268,13 @@ static int __init tx4939_rtc_probe(struct platform_device *pdev)
 	if (devm_request_irq(&pdev->dev, irq, tx4939_rtc_interrupt,
 			     0, pdev->name, &pdev->dev) < 0)
 		return -EBUSY;
-	rtc = rtc_device_register(pdev->name, &pdev->dev,
+	rtc = devm_rtc_device_register(&pdev->dev, pdev->name,
 				  &tx4939_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc))
 		return PTR_ERR(rtc);
 	pdata->rtc = rtc;
 	ret = sysfs_create_bin_file(&pdev->dev.kobj, &tx4939_rtc_nvram_attr);
-	if (ret)
-		rtc_device_unregister(rtc);
+
 	return ret;
 }
 
@@ -284,7 +283,6 @@ static int __exit tx4939_rtc_remove(struct platform_device *pdev)
 	struct tx4939rtc_plat_data *pdata = platform_get_drvdata(pdev);
 
 	sysfs_remove_bin_file(&pdev->dev.kobj, &tx4939_rtc_nvram_attr);
-	rtc_device_unregister(pdata->rtc);
 	spin_lock_irq(&pdata->lock);
 	tx4939_rtc_cmd(pdata->rtcreg, TX4939_RTCCTL_COMMAND_NOP);
 	spin_unlock_irq(&pdata->lock);
@@ -299,18 +297,7 @@ static struct platform_driver tx4939_rtc_driver = {
 	},
 };
 
-static int __init tx4939rtc_init(void)
-{
-	return platform_driver_probe(&tx4939_rtc_driver, tx4939_rtc_probe);
-}
-
-static void __exit tx4939rtc_exit(void)
-{
-	platform_driver_unregister(&tx4939_rtc_driver);
-}
-
-module_init(tx4939rtc_init);
-module_exit(tx4939rtc_exit);
+module_platform_driver_probe(tx4939_rtc_driver, tx4939_rtc_probe);
 
 MODULE_AUTHOR("Atsushi Nemoto <anemo@mba.ocn.ne.jp>");
 MODULE_DESCRIPTION("TX4939 internal RTC driver");
diff --git a/drivers/rtc/rtc-v3020.c b/drivers/rtc/rtc-v3020.c
index bca5d677bc8..6e0cba8f47d 100644
--- a/drivers/rtc/rtc-v3020.c
+++ b/drivers/rtc/rtc-v3020.c
@@ -309,7 +309,7 @@ static int rtc_probe(struct platform_device *pdev)
 	int i;
 	int temp;
 
-	chip = kzalloc(sizeof *chip, GFP_KERNEL);
+	chip = devm_kzalloc(&pdev->dev, sizeof(*chip), GFP_KERNEL);
 	if (!chip)
 		return -ENOMEM;
 
@@ -353,8 +353,8 @@ static int rtc_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, chip);
 
-	chip->rtc = rtc_device_register("v3020",
-				&pdev->dev, &v3020_rtc_ops, THIS_MODULE);
+	chip->rtc = devm_rtc_device_register(&pdev->dev, "v3020",
+					&v3020_rtc_ops, THIS_MODULE);
 	if (IS_ERR(chip->rtc)) {
 		retval = PTR_ERR(chip->rtc);
 		goto err_io;
@@ -365,21 +365,14 @@ static int rtc_probe(struct platform_device *pdev)
 err_io:
 	chip->ops->unmap_io(chip);
 err_chip:
-	kfree(chip);
-
 	return retval;
 }
 
 static int rtc_remove(struct platform_device *dev)
 {
 	struct v3020 *chip = platform_get_drvdata(dev);
-	struct rtc_device *rtc = chip->rtc;
-
-	if (rtc)
-		rtc_device_unregister(rtc);
 
 	chip->ops->unmap_io(chip);
-	kfree(chip);
 
 	return 0;
 }
diff --git a/drivers/rtc/rtc-vt8500.c b/drivers/rtc/rtc-vt8500.c
index a000bc0a8bf..d89efee6d29 100644
--- a/drivers/rtc/rtc-vt8500.c
+++ b/drivers/rtc/rtc-vt8500.c
@@ -252,7 +252,7 @@ static int vt8500_rtc_probe(struct platform_device *pdev)
 	writel(VT8500_RTC_CR_ENABLE,
 	       vt8500_rtc->regbase + VT8500_RTC_CR);
 
-	vt8500_rtc->rtc = rtc_device_register("vt8500-rtc", &pdev->dev,
+	vt8500_rtc->rtc = devm_rtc_device_register(&pdev->dev, "vt8500-rtc",
 					      &vt8500_rtc_ops, THIS_MODULE);
 	if (IS_ERR(vt8500_rtc->rtc)) {
 		ret = PTR_ERR(vt8500_rtc->rtc);
@@ -266,13 +266,11 @@ static int vt8500_rtc_probe(struct platform_device *pdev)
 	if (ret < 0) {
 		dev_err(&pdev->dev, "can't get irq %i, err %d\n",
 			vt8500_rtc->irq_alarm, ret);
-		goto err_unreg;
+		goto err_return;
 	}
 
 	return 0;
 
-err_unreg:
-	rtc_device_unregister(vt8500_rtc->rtc);
 err_return:
 	return ret;
 }
@@ -281,8 +279,6 @@ static int vt8500_rtc_remove(struct platform_device *pdev)
 {
 	struct vt8500_rtc *vt8500_rtc = platform_get_drvdata(pdev);
 
-	rtc_device_unregister(vt8500_rtc->rtc);
-
 	/* Disable alarm matching */
 	writel(0, vt8500_rtc->regbase + VT8500_RTC_IS);
 
diff --git a/drivers/rtc/rtc-wm831x.c b/drivers/rtc/rtc-wm831x.c
index 2f0ac7b30a0..8d65b94e5a7 100644
--- a/drivers/rtc/rtc-wm831x.c
+++ b/drivers/rtc/rtc-wm831x.c
@@ -436,7 +436,7 @@ static int wm831x_rtc_probe(struct platform_device *pdev)
 
 	device_init_wakeup(&pdev->dev, 1);
 
-	wm831x_rtc->rtc = rtc_device_register("wm831x", &pdev->dev,
+	wm831x_rtc->rtc = devm_rtc_device_register(&pdev->dev, "wm831x",
 					      &wm831x_rtc_ops, THIS_MODULE);
 	if (IS_ERR(wm831x_rtc->rtc)) {
 		ret = PTR_ERR(wm831x_rtc->rtc);
@@ -462,10 +462,6 @@ err:
 
 static int wm831x_rtc_remove(struct platform_device *pdev)
 {
-	struct wm831x_rtc *wm831x_rtc = platform_get_drvdata(pdev);
-
-	rtc_device_unregister(wm831x_rtc->rtc);
-
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-wm8350.c b/drivers/rtc/rtc-wm8350.c
index 8ad86ae0d30..fa247deb9cf 100644
--- a/drivers/rtc/rtc-wm8350.c
+++ b/drivers/rtc/rtc-wm8350.c
@@ -339,7 +339,7 @@ static const struct rtc_class_ops wm8350_rtc_ops = {
 	.alarm_irq_enable = wm8350_rtc_alarm_irq_enable,
 };
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int wm8350_rtc_suspend(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
@@ -375,10 +375,6 @@ static int wm8350_rtc_resume(struct device *dev)
 
 	return 0;
 }
-
-#else
-#define wm8350_rtc_suspend NULL
-#define wm8350_rtc_resume NULL
 #endif
 
 static int wm8350_rtc_probe(struct platform_device *pdev)
@@ -439,8 +435,8 @@ static int wm8350_rtc_probe(struct platform_device *pdev)
 
 	device_init_wakeup(&pdev->dev, 1);
 
-	wm_rtc->rtc = rtc_device_register("wm8350", &pdev->dev,
-					  &wm8350_rtc_ops, THIS_MODULE);
+	wm_rtc->rtc = devm_rtc_device_register(&pdev->dev, "wm8350",
+					&wm8350_rtc_ops, THIS_MODULE);
 	if (IS_ERR(wm_rtc->rtc)) {
 		ret = PTR_ERR(wm_rtc->rtc);
 		dev_err(&pdev->dev, "failed to register RTC: %d\n", ret);
@@ -462,20 +458,15 @@ static int wm8350_rtc_probe(struct platform_device *pdev)
 static int wm8350_rtc_remove(struct platform_device *pdev)
 {
 	struct wm8350 *wm8350 = platform_get_drvdata(pdev);
-	struct wm8350_rtc *wm_rtc = &wm8350->rtc;
 
 	wm8350_free_irq(wm8350, WM8350_IRQ_RTC_SEC, wm8350);
 	wm8350_free_irq(wm8350, WM8350_IRQ_RTC_ALM, wm8350);
 
-	rtc_device_unregister(wm_rtc->rtc);
-
 	return 0;
 }
 
-static struct dev_pm_ops wm8350_rtc_pm_ops = {
-	.suspend = wm8350_rtc_suspend,
-	.resume = wm8350_rtc_resume,
-};
+static SIMPLE_DEV_PM_OPS(wm8350_rtc_pm_ops, wm8350_rtc_suspend,
+			wm8350_rtc_resume);
 
 static struct platform_driver wm8350_rtc_driver = {
 	.probe = wm8350_rtc_probe,
diff --git a/drivers/rtc/rtc-x1205.c b/drivers/rtc/rtc-x1205.c
index f36e59c6bc0..fa9b0679fb6 100644
--- a/drivers/rtc/rtc-x1205.c
+++ b/drivers/rtc/rtc-x1205.c
@@ -630,8 +630,8 @@ static int x1205_probe(struct i2c_client *client,
 
 	dev_info(&client->dev, "chip found, driver version " DRV_VERSION "\n");
 
-	rtc = rtc_device_register(x1205_driver.driver.name, &client->dev,
-				&x1205_rtc_ops, THIS_MODULE);
+	rtc = devm_rtc_device_register(&client->dev, x1205_driver.driver.name,
+					&x1205_rtc_ops, THIS_MODULE);
 
 	if (IS_ERR(rtc))
 		return PTR_ERR(rtc);
@@ -653,21 +653,13 @@ static int x1205_probe(struct i2c_client *client,
 
 	err = x1205_sysfs_register(&client->dev);
 	if (err)
-		goto exit_devreg;
+		return err;
 
 	return 0;
-
-exit_devreg:
-	rtc_device_unregister(rtc);
-
-	return err;
 }
 
 static int x1205_remove(struct i2c_client *client)
 {
-	struct rtc_device *rtc = i2c_get_clientdata(client);
-
-	rtc_device_unregister(rtc);
 	x1205_sysfs_unregister(&client->dev);
 	return 0;
 }
diff --git a/drivers/s390/block/dasd.c b/drivers/s390/block/dasd.c
index f1b7fdc58a5..82758cbb220 100644
--- a/drivers/s390/block/dasd.c
+++ b/drivers/s390/block/dasd.c
@@ -246,7 +246,7 @@ static struct dentry *dasd_debugfs_setup(const char *name,
 static int dasd_state_known_to_basic(struct dasd_device *device)
 {
 	struct dasd_block *block = device->block;
-	int rc;
+	int rc = 0;
 
 	/* Allocate and register gendisk structure. */
 	if (block) {
@@ -273,7 +273,8 @@ static int dasd_state_known_to_basic(struct dasd_device *device)
 	DBF_DEV_EVENT(DBF_EMERG, device, "%s", "debug area created");
 
 	device->state = DASD_STATE_BASIC;
-	return 0;
+
+	return rc;
 }
 
 /*
@@ -282,6 +283,7 @@ static int dasd_state_known_to_basic(struct dasd_device *device)
 static int dasd_state_basic_to_known(struct dasd_device *device)
 {
 	int rc;
+
 	if (device->block) {
 		dasd_profile_exit(&device->block->profile);
 		if (device->block->debugfs_dentry)
@@ -332,8 +334,10 @@ static int dasd_state_basic_to_ready(struct dasd_device *device)
 		if (block->base->discipline->do_analysis != NULL)
 			rc = block->base->discipline->do_analysis(block);
 		if (rc) {
-			if (rc != -EAGAIN)
+			if (rc != -EAGAIN) {
 				device->state = DASD_STATE_UNFMT;
+				goto out;
+			}
 			return rc;
 		}
 		dasd_setup_queue(block);
@@ -341,11 +345,16 @@ static int dasd_state_basic_to_ready(struct dasd_device *device)
 			     block->blocks << block->s2b_shift);
 		device->state = DASD_STATE_READY;
 		rc = dasd_scan_partitions(block);
-		if (rc)
+		if (rc) {
 			device->state = DASD_STATE_BASIC;
+			return rc;
+		}
 	} else {
 		device->state = DASD_STATE_READY;
 	}
+out:
+	if (device->discipline->basic_to_ready)
+		rc = device->discipline->basic_to_ready(device);
 	return rc;
 }
 
@@ -368,6 +377,11 @@ static int dasd_state_ready_to_basic(struct dasd_device *device)
 {
 	int rc;
 
+	if (device->discipline->ready_to_basic) {
+		rc = device->discipline->ready_to_basic(device);
+		if (rc)
+			return rc;
+	}
 	device->state = DASD_STATE_BASIC;
 	if (device->block) {
 		struct dasd_block *block = device->block;
@@ -402,16 +416,10 @@ static int dasd_state_unfmt_to_basic(struct dasd_device *device)
 static int
 dasd_state_ready_to_online(struct dasd_device * device)
 {
-	int rc;
 	struct gendisk *disk;
 	struct disk_part_iter piter;
 	struct hd_struct *part;
 
-	if (device->discipline->ready_to_online) {
-		rc = device->discipline->ready_to_online(device);
-		if (rc)
-			return rc;
-	}
 	device->state = DASD_STATE_ONLINE;
 	if (device->block) {
 		dasd_schedule_block_bh(device->block);
@@ -444,6 +452,7 @@ static int dasd_state_online_to_ready(struct dasd_device *device)
 		if (rc)
 			return rc;
 	}
+
 	device->state = DASD_STATE_READY;
 	if (device->block && !(device->features & DASD_FEATURE_USERAW)) {
 		disk = device->block->bdev->bd_disk;
@@ -2223,6 +2232,77 @@ static int _dasd_sleep_on(struct dasd_ccw_req *maincqr, int interruptible)
 	return rc;
 }
 
+static inline int _wait_for_wakeup_queue(struct list_head *ccw_queue)
+{
+	struct dasd_ccw_req *cqr;
+
+	list_for_each_entry(cqr, ccw_queue, blocklist) {
+		if (cqr->callback_data != DASD_SLEEPON_END_TAG)
+			return 0;
+	}
+
+	return 1;
+}
+
+static int _dasd_sleep_on_queue(struct list_head *ccw_queue, int interruptible)
+{
+	struct dasd_device *device;
+	int rc;
+	struct dasd_ccw_req *cqr, *n;
+
+retry:
+	list_for_each_entry_safe(cqr, n, ccw_queue, blocklist) {
+		device = cqr->startdev;
+		if (cqr->status != DASD_CQR_FILLED) /*could be failed*/
+			continue;
+
+		if (test_bit(DASD_FLAG_LOCK_STOLEN, &device->flags) &&
+		    !test_bit(DASD_CQR_ALLOW_SLOCK, &cqr->flags)) {
+			cqr->status = DASD_CQR_FAILED;
+			cqr->intrc = -EPERM;
+			continue;
+		}
+		/*Non-temporary stop condition will trigger fail fast*/
+		if (device->stopped & ~DASD_STOPPED_PENDING &&
+		    test_bit(DASD_CQR_FLAGS_FAILFAST, &cqr->flags) &&
+		    !dasd_eer_enabled(device)) {
+			cqr->status = DASD_CQR_FAILED;
+			cqr->intrc = -EAGAIN;
+			continue;
+		}
+
+		/*Don't try to start requests if device is stopped*/
+		if (interruptible) {
+			rc = wait_event_interruptible(
+				generic_waitq, !device->stopped);
+			if (rc == -ERESTARTSYS) {
+				cqr->status = DASD_CQR_FAILED;
+				cqr->intrc = rc;
+				continue;
+			}
+		} else
+			wait_event(generic_waitq, !(device->stopped));
+
+		if (!cqr->callback)
+			cqr->callback = dasd_wakeup_cb;
+		cqr->callback_data = DASD_SLEEPON_START_TAG;
+		dasd_add_request_tail(cqr);
+	}
+
+	wait_event(generic_waitq, _wait_for_wakeup_queue(ccw_queue));
+
+	rc = 0;
+	list_for_each_entry_safe(cqr, n, ccw_queue, blocklist) {
+		if (__dasd_sleep_on_erp(cqr))
+			rc = 1;
+	}
+	if (rc)
+		goto retry;
+
+
+	return 0;
+}
+
 /*
  * Queue a request to the tail of the device ccw_queue and wait for
  * it's completion.
@@ -2233,6 +2313,15 @@ int dasd_sleep_on(struct dasd_ccw_req *cqr)
 }
 
 /*
+ * Start requests from a ccw_queue and wait for their completion.
+ */
+int dasd_sleep_on_queue(struct list_head *ccw_queue)
+{
+	return _dasd_sleep_on_queue(ccw_queue, 0);
+}
+EXPORT_SYMBOL(dasd_sleep_on_queue);
+
+/*
  * Queue a request to the tail of the device ccw_queue and wait
  * interruptible for it's completion.
  */
@@ -2663,6 +2752,26 @@ static void _dasd_wake_block_flush_cb(struct dasd_ccw_req *cqr, void *data)
 }
 
 /*
+ * Requeue a request back to the block request queue
+ * only works for block requests
+ */
+static int _dasd_requeue_request(struct dasd_ccw_req *cqr)
+{
+	struct dasd_block *block = cqr->block;
+	struct request *req;
+	unsigned long flags;
+
+	if (!block)
+		return -EINVAL;
+	spin_lock_irqsave(&block->queue_lock, flags);
+	req = (struct request *) cqr->callback_data;
+	blk_requeue_request(block->request_queue, req);
+	spin_unlock_irqrestore(&block->queue_lock, flags);
+
+	return 0;
+}
+
+/*
  * Go through all request on the dasd_block request queue, cancel them
  * on the respective dasd_device, and return them to the generic
  * block layer.
@@ -3380,10 +3489,11 @@ EXPORT_SYMBOL_GPL(dasd_generic_verify_path);
 
 int dasd_generic_pm_freeze(struct ccw_device *cdev)
 {
+	struct dasd_device *device = dasd_device_from_cdev(cdev);
+	struct list_head freeze_queue;
 	struct dasd_ccw_req *cqr, *n;
+	struct dasd_ccw_req *refers;
 	int rc;
-	struct list_head freeze_queue;
-	struct dasd_device *device = dasd_device_from_cdev(cdev);
 
 	if (IS_ERR(device))
 		return PTR_ERR(device);
@@ -3396,7 +3506,8 @@ int dasd_generic_pm_freeze(struct ccw_device *cdev)
 
 	/* disallow new I/O  */
 	dasd_device_set_stop_bits(device, DASD_STOPPED_PM);
-	/* clear active requests */
+
+	/* clear active requests and requeue them to block layer if possible */
 	INIT_LIST_HEAD(&freeze_queue);
 	spin_lock_irq(get_ccwdev_lock(cdev));
 	rc = 0;
@@ -3416,7 +3527,6 @@ int dasd_generic_pm_freeze(struct ccw_device *cdev)
 		}
 		list_move_tail(&cqr->devlist, &freeze_queue);
 	}
-
 	spin_unlock_irq(get_ccwdev_lock(cdev));
 
 	list_for_each_entry_safe(cqr, n, &freeze_queue, devlist) {
@@ -3424,12 +3534,38 @@ int dasd_generic_pm_freeze(struct ccw_device *cdev)
 			   (cqr->status != DASD_CQR_CLEAR_PENDING));
 		if (cqr->status == DASD_CQR_CLEARED)
 			cqr->status = DASD_CQR_QUEUED;
+
+		/* requeue requests to blocklayer will only work for
+		   block device requests */
+		if (_dasd_requeue_request(cqr))
+			continue;
+
+		/* remove requests from device and block queue */
+		list_del_init(&cqr->devlist);
+		while (cqr->refers != NULL) {
+			refers = cqr->refers;
+			/* remove the request from the block queue */
+			list_del(&cqr->blocklist);
+			/* free the finished erp request */
+			dasd_free_erp_request(cqr, cqr->memdev);
+			cqr = refers;
+		}
+		if (cqr->block)
+			list_del_init(&cqr->blocklist);
+		cqr->block->base->discipline->free_cp(
+			cqr, (struct request *) cqr->callback_data);
 	}
-	/* move freeze_queue to start of the ccw_queue */
-	spin_lock_irq(get_ccwdev_lock(cdev));
-	list_splice_tail(&freeze_queue, &device->ccw_queue);
-	spin_unlock_irq(get_ccwdev_lock(cdev));
 
+	/*
+	 * if requests remain then they are internal request
+	 * and go back to the device queue
+	 */
+	if (!list_empty(&freeze_queue)) {
+		/* move freeze_queue to start of the ccw_queue */
+		spin_lock_irq(get_ccwdev_lock(cdev));
+		list_splice_tail(&freeze_queue, &device->ccw_queue);
+		spin_unlock_irq(get_ccwdev_lock(cdev));
+	}
 	dasd_put_device(device);
 	return rc;
 }
diff --git a/drivers/s390/block/dasd_devmap.c b/drivers/s390/block/dasd_devmap.c
index c196827c228..a71bb8aaca1 100644
--- a/drivers/s390/block/dasd_devmap.c
+++ b/drivers/s390/block/dasd_devmap.c
@@ -410,8 +410,7 @@ dasd_add_busid(const char *bus_id, int features)
 	struct dasd_devmap *devmap, *new, *tmp;
 	int hash;
 
-	new = (struct dasd_devmap *)
-		kzalloc(sizeof(struct dasd_devmap), GFP_KERNEL);
+	new = kzalloc(sizeof(struct dasd_devmap), GFP_KERNEL);
 	if (!new)
 		return ERR_PTR(-ENOMEM);
 	spin_lock(&dasd_devmap_lock);
diff --git a/drivers/s390/block/dasd_eckd.c b/drivers/s390/block/dasd_eckd.c
index 6999fd919e9..6a44b27623e 100644
--- a/drivers/s390/block/dasd_eckd.c
+++ b/drivers/s390/block/dasd_eckd.c
@@ -2022,7 +2022,7 @@ static int dasd_eckd_do_analysis(struct dasd_block *block)
 		return dasd_eckd_end_analysis(block);
 }
 
-static int dasd_eckd_ready_to_online(struct dasd_device *device)
+static int dasd_eckd_basic_to_ready(struct dasd_device *device)
 {
 	return dasd_alias_add_device(device);
 };
@@ -2031,6 +2031,11 @@ static int dasd_eckd_online_to_ready(struct dasd_device *device)
 {
 	cancel_work_sync(&device->reload_device);
 	cancel_work_sync(&device->kick_validate);
+	return 0;
+};
+
+static int dasd_eckd_ready_to_basic(struct dasd_device *device)
+{
 	return dasd_alias_remove_device(device);
 };
 
@@ -2050,45 +2055,34 @@ dasd_eckd_fill_geometry(struct dasd_block *block, struct hd_geometry *geo)
 }
 
 static struct dasd_ccw_req *
-dasd_eckd_format_device(struct dasd_device * device,
-			struct format_data_t * fdata)
+dasd_eckd_build_format(struct dasd_device *base,
+		       struct format_data_t *fdata)
 {
-	struct dasd_eckd_private *private;
+	struct dasd_eckd_private *base_priv;
+	struct dasd_eckd_private *start_priv;
+	struct dasd_device *startdev;
 	struct dasd_ccw_req *fcp;
 	struct eckd_count *ect;
+	struct ch_t address;
 	struct ccw1 *ccw;
 	void *data;
 	int rpt;
-	struct ch_t address;
 	int cplength, datasize;
-	int i;
+	int i, j;
 	int intensity = 0;
 	int r0_perm;
+	int nr_tracks;
 
-	private = (struct dasd_eckd_private *) device->private;
-	rpt = recs_per_track(&private->rdc_data, 0, fdata->blksize);
-	set_ch_t(&address,
-		 fdata->start_unit / private->rdc_data.trk_per_cyl,
-		 fdata->start_unit % private->rdc_data.trk_per_cyl);
+	startdev = dasd_alias_get_start_dev(base);
+	if (!startdev)
+		startdev = base;
 
-	/* Sanity checks. */
-	if (fdata->start_unit >=
-	    (private->real_cyl * private->rdc_data.trk_per_cyl)) {
-		dev_warn(&device->cdev->dev, "Start track number %d used in "
-			 "formatting is too big\n", fdata->start_unit);
-		return ERR_PTR(-EINVAL);
-	}
-	if (fdata->start_unit > fdata->stop_unit) {
-		dev_warn(&device->cdev->dev, "Start track %d used in "
-			 "formatting exceeds end track\n", fdata->start_unit);
-		return ERR_PTR(-EINVAL);
-	}
-	if (dasd_check_blocksize(fdata->blksize) != 0) {
-		dev_warn(&device->cdev->dev,
-			 "The DASD cannot be formatted with block size %d\n",
-			 fdata->blksize);
-		return ERR_PTR(-EINVAL);
-	}
+	start_priv = (struct dasd_eckd_private *) startdev->private;
+	base_priv = (struct dasd_eckd_private *) base->private;
+
+	rpt = recs_per_track(&base_priv->rdc_data, 0, fdata->blksize);
+
+	nr_tracks = fdata->stop_unit - fdata->start_unit + 1;
 
 	/*
 	 * fdata->intensity is a bit string that tells us what to do:
@@ -2106,149 +2100,282 @@ dasd_eckd_format_device(struct dasd_device * device,
 		r0_perm = 1;
 		intensity = fdata->intensity;
 	}
+
 	switch (intensity) {
 	case 0x00:	/* Normal format */
 	case 0x08:	/* Normal format, use cdl. */
-		cplength = 2 + rpt;
-		datasize = sizeof(struct DE_eckd_data) +
+		cplength = 2 + (rpt*nr_tracks);
+		datasize = sizeof(struct PFX_eckd_data) +
 			sizeof(struct LO_eckd_data) +
-			rpt * sizeof(struct eckd_count);
+			rpt * nr_tracks * sizeof(struct eckd_count);
 		break;
 	case 0x01:	/* Write record zero and format track. */
 	case 0x09:	/* Write record zero and format track, use cdl. */
-		cplength = 3 + rpt;
-		datasize = sizeof(struct DE_eckd_data) +
+		cplength = 2 + rpt * nr_tracks;
+		datasize = sizeof(struct PFX_eckd_data) +
 			sizeof(struct LO_eckd_data) +
 			sizeof(struct eckd_count) +
-			rpt * sizeof(struct eckd_count);
+			rpt * nr_tracks * sizeof(struct eckd_count);
 		break;
 	case 0x04:	/* Invalidate track. */
 	case 0x0c:	/* Invalidate track, use cdl. */
 		cplength = 3;
-		datasize = sizeof(struct DE_eckd_data) +
+		datasize = sizeof(struct PFX_eckd_data) +
 			sizeof(struct LO_eckd_data) +
 			sizeof(struct eckd_count);
 		break;
 	default:
-		dev_warn(&device->cdev->dev, "An I/O control call used "
-			 "incorrect flags 0x%x\n", fdata->intensity);
+		dev_warn(&startdev->cdev->dev,
+			 "An I/O control call used incorrect flags 0x%x\n",
+			 fdata->intensity);
 		return ERR_PTR(-EINVAL);
 	}
 	/* Allocate the format ccw request. */
-	fcp = dasd_smalloc_request(DASD_ECKD_MAGIC, cplength, datasize, device);
+	fcp = dasd_smalloc_request(DASD_ECKD_MAGIC, cplength,
+				   datasize, startdev);
 	if (IS_ERR(fcp))
 		return fcp;
 
+	start_priv->count++;
 	data = fcp->data;
 	ccw = fcp->cpaddr;
 
 	switch (intensity & ~0x08) {
 	case 0x00: /* Normal format. */
-		define_extent(ccw++, (struct DE_eckd_data *) data,
-			      fdata->start_unit, fdata->start_unit,
-			      DASD_ECKD_CCW_WRITE_CKD, device);
+		prefix(ccw++, (struct PFX_eckd_data *) data,
+		       fdata->start_unit, fdata->stop_unit,
+		       DASD_ECKD_CCW_WRITE_CKD, base, startdev);
 		/* grant subsystem permission to format R0 */
 		if (r0_perm)
-			((struct DE_eckd_data *)data)->ga_extended |= 0x04;
-		data += sizeof(struct DE_eckd_data);
+			((struct PFX_eckd_data *)data)
+				->define_extent.ga_extended |= 0x04;
+		data += sizeof(struct PFX_eckd_data);
 		ccw[-1].flags |= CCW_FLAG_CC;
 		locate_record(ccw++, (struct LO_eckd_data *) data,
-			      fdata->start_unit, 0, rpt,
-			      DASD_ECKD_CCW_WRITE_CKD, device,
+			      fdata->start_unit, 0, rpt*nr_tracks,
+			      DASD_ECKD_CCW_WRITE_CKD, base,
 			      fdata->blksize);
 		data += sizeof(struct LO_eckd_data);
 		break;
 	case 0x01: /* Write record zero + format track. */
-		define_extent(ccw++, (struct DE_eckd_data *) data,
-			      fdata->start_unit, fdata->start_unit,
-			      DASD_ECKD_CCW_WRITE_RECORD_ZERO,
-			      device);
-		data += sizeof(struct DE_eckd_data);
+		prefix(ccw++, (struct PFX_eckd_data *) data,
+		       fdata->start_unit, fdata->stop_unit,
+		       DASD_ECKD_CCW_WRITE_RECORD_ZERO,
+		       base, startdev);
+		data += sizeof(struct PFX_eckd_data);
 		ccw[-1].flags |= CCW_FLAG_CC;
 		locate_record(ccw++, (struct LO_eckd_data *) data,
-			      fdata->start_unit, 0, rpt + 1,
-			      DASD_ECKD_CCW_WRITE_RECORD_ZERO, device,
-			      device->block->bp_block);
+			      fdata->start_unit, 0, rpt * nr_tracks + 1,
+			      DASD_ECKD_CCW_WRITE_RECORD_ZERO, base,
+			      base->block->bp_block);
 		data += sizeof(struct LO_eckd_data);
 		break;
 	case 0x04: /* Invalidate track. */
-		define_extent(ccw++, (struct DE_eckd_data *) data,
-			      fdata->start_unit, fdata->start_unit,
-			      DASD_ECKD_CCW_WRITE_CKD, device);
-		data += sizeof(struct DE_eckd_data);
+		prefix(ccw++, (struct PFX_eckd_data *) data,
+		       fdata->start_unit, fdata->stop_unit,
+		       DASD_ECKD_CCW_WRITE_CKD, base, startdev);
+		data += sizeof(struct PFX_eckd_data);
 		ccw[-1].flags |= CCW_FLAG_CC;
 		locate_record(ccw++, (struct LO_eckd_data *) data,
 			      fdata->start_unit, 0, 1,
-			      DASD_ECKD_CCW_WRITE_CKD, device, 8);
+			      DASD_ECKD_CCW_WRITE_CKD, base, 8);
 		data += sizeof(struct LO_eckd_data);
 		break;
 	}
-	if (intensity & 0x01) {	/* write record zero */
-		ect = (struct eckd_count *) data;
-		data += sizeof(struct eckd_count);
-		ect->cyl = address.cyl;
-		ect->head = address.head;
-		ect->record = 0;
-		ect->kl = 0;
-		ect->dl = 8;
-		ccw[-1].flags |= CCW_FLAG_CC;
-		ccw->cmd_code = DASD_ECKD_CCW_WRITE_RECORD_ZERO;
-		ccw->flags = CCW_FLAG_SLI;
-		ccw->count = 8;
-		ccw->cda = (__u32)(addr_t) ect;
-		ccw++;
-	}
-	if ((intensity & ~0x08) & 0x04) {	/* erase track */
-		ect = (struct eckd_count *) data;
-		data += sizeof(struct eckd_count);
-		ect->cyl = address.cyl;
-		ect->head = address.head;
-		ect->record = 1;
-		ect->kl = 0;
-		ect->dl = 0;
-		ccw[-1].flags |= CCW_FLAG_CC;
-		ccw->cmd_code = DASD_ECKD_CCW_WRITE_CKD;
-		ccw->flags = CCW_FLAG_SLI;
-		ccw->count = 8;
-		ccw->cda = (__u32)(addr_t) ect;
-	} else {		/* write remaining records */
-		for (i = 0; i < rpt; i++) {
+
+	for (j = 0; j < nr_tracks; j++) {
+		/* calculate cylinder and head for the current track */
+		set_ch_t(&address,
+			 (fdata->start_unit + j) /
+			 base_priv->rdc_data.trk_per_cyl,
+			 (fdata->start_unit + j) %
+			 base_priv->rdc_data.trk_per_cyl);
+		if (intensity & 0x01) {	/* write record zero */
 			ect = (struct eckd_count *) data;
 			data += sizeof(struct eckd_count);
 			ect->cyl = address.cyl;
 			ect->head = address.head;
-			ect->record = i + 1;
+			ect->record = 0;
 			ect->kl = 0;
-			ect->dl = fdata->blksize;
-			/* Check for special tracks 0-1 when formatting CDL */
-			if ((intensity & 0x08) &&
-			    fdata->start_unit == 0) {
-				if (i < 3) {
-					ect->kl = 4;
-					ect->dl = sizes_trk0[i] - 4;
-				}
-			}
-			if ((intensity & 0x08) &&
-			    fdata->start_unit == 1) {
-				ect->kl = 44;
-				ect->dl = LABEL_SIZE - 44;
-			}
+			ect->dl = 8;
 			ccw[-1].flags |= CCW_FLAG_CC;
-			ccw->cmd_code = DASD_ECKD_CCW_WRITE_CKD;
+			ccw->cmd_code = DASD_ECKD_CCW_WRITE_RECORD_ZERO;
 			ccw->flags = CCW_FLAG_SLI;
 			ccw->count = 8;
 			ccw->cda = (__u32)(addr_t) ect;
 			ccw++;
 		}
+		if ((intensity & ~0x08) & 0x04) {	/* erase track */
+			ect = (struct eckd_count *) data;
+			data += sizeof(struct eckd_count);
+			ect->cyl = address.cyl;
+			ect->head = address.head;
+			ect->record = 1;
+			ect->kl = 0;
+			ect->dl = 0;
+			ccw[-1].flags |= CCW_FLAG_CC;
+			ccw->cmd_code = DASD_ECKD_CCW_WRITE_CKD;
+			ccw->flags = CCW_FLAG_SLI;
+			ccw->count = 8;
+			ccw->cda = (__u32)(addr_t) ect;
+		} else {		/* write remaining records */
+			for (i = 0; i < rpt; i++) {
+				ect = (struct eckd_count *) data;
+				data += sizeof(struct eckd_count);
+				ect->cyl = address.cyl;
+				ect->head = address.head;
+				ect->record = i + 1;
+				ect->kl = 0;
+				ect->dl = fdata->blksize;
+				/*
+				 * Check for special tracks 0-1
+				 * when formatting CDL
+				 */
+				if ((intensity & 0x08) &&
+				    fdata->start_unit == 0) {
+					if (i < 3) {
+						ect->kl = 4;
+						ect->dl = sizes_trk0[i] - 4;
+					}
+				}
+				if ((intensity & 0x08) &&
+				    fdata->start_unit == 1) {
+					ect->kl = 44;
+					ect->dl = LABEL_SIZE - 44;
+				}
+				ccw[-1].flags |= CCW_FLAG_CC;
+				if (i != 0 || j == 0)
+					ccw->cmd_code =
+						DASD_ECKD_CCW_WRITE_CKD;
+				else
+					ccw->cmd_code =
+						DASD_ECKD_CCW_WRITE_CKD_MT;
+				ccw->flags = CCW_FLAG_SLI;
+				ccw->count = 8;
+					ccw->cda = (__u32)(addr_t) ect;
+					ccw++;
+			}
+		}
 	}
-	fcp->startdev = device;
-	fcp->memdev = device;
+
+	fcp->startdev = startdev;
+	fcp->memdev = startdev;
 	fcp->retries = 256;
+	fcp->expires = startdev->default_expires * HZ;
 	fcp->buildclk = get_tod_clock();
 	fcp->status = DASD_CQR_FILLED;
+
 	return fcp;
 }
 
+static int
+dasd_eckd_format_device(struct dasd_device *base,
+			struct format_data_t *fdata)
+{
+	struct dasd_ccw_req *cqr, *n;
+	struct dasd_block *block;
+	struct dasd_eckd_private *private;
+	struct list_head format_queue;
+	struct dasd_device *device;
+	int old_stop, format_step;
+	int step, rc = 0;
+
+	block = base->block;
+	private = (struct dasd_eckd_private *) base->private;
+
+	/* Sanity checks. */
+	if (fdata->start_unit >=
+	    (private->real_cyl * private->rdc_data.trk_per_cyl)) {
+		dev_warn(&base->cdev->dev,
+			 "Start track number %u used in formatting is too big\n",
+			 fdata->start_unit);
+		return -EINVAL;
+	}
+	if (fdata->stop_unit >=
+	    (private->real_cyl * private->rdc_data.trk_per_cyl)) {
+		dev_warn(&base->cdev->dev,
+			 "Stop track number %u used in formatting is too big\n",
+			 fdata->stop_unit);
+		return -EINVAL;
+	}
+	if (fdata->start_unit > fdata->stop_unit) {
+		dev_warn(&base->cdev->dev,
+			 "Start track %u used in formatting exceeds end track\n",
+			 fdata->start_unit);
+		return -EINVAL;
+	}
+	if (dasd_check_blocksize(fdata->blksize) != 0) {
+		dev_warn(&base->cdev->dev,
+			 "The DASD cannot be formatted with block size %u\n",
+			 fdata->blksize);
+		return -EINVAL;
+	}
+
+	INIT_LIST_HEAD(&format_queue);
+	old_stop = fdata->stop_unit;
+
+	while (fdata->start_unit <= 1) {
+		fdata->stop_unit = fdata->start_unit;
+		cqr = dasd_eckd_build_format(base, fdata);
+		list_add(&cqr->blocklist, &format_queue);
+
+		fdata->stop_unit = old_stop;
+		fdata->start_unit++;
+
+		if (fdata->start_unit > fdata->stop_unit)
+			goto sleep;
+	}
+
+retry:
+	format_step = 255 / recs_per_track(&private->rdc_data, 0,
+					   fdata->blksize);
+	while (fdata->start_unit <= old_stop) {
+		step = fdata->stop_unit - fdata->start_unit + 1;
+		if (step > format_step)
+			fdata->stop_unit = fdata->start_unit + format_step - 1;
+
+		cqr = dasd_eckd_build_format(base, fdata);
+		if (IS_ERR(cqr)) {
+			if (PTR_ERR(cqr) == -ENOMEM) {
+				/*
+				 * not enough memory available
+				 * go to out and start requests
+				 * retry after first requests were finished
+				 */
+				fdata->stop_unit = old_stop;
+				goto sleep;
+			} else
+				return PTR_ERR(cqr);
+		}
+		list_add(&cqr->blocklist, &format_queue);
+
+		fdata->start_unit = fdata->stop_unit + 1;
+		fdata->stop_unit = old_stop;
+	}
+
+sleep:
+	dasd_sleep_on_queue(&format_queue);
+
+	list_for_each_entry_safe(cqr, n, &format_queue, blocklist) {
+		device = cqr->startdev;
+		private = (struct dasd_eckd_private *) device->private;
+		if (cqr->status == DASD_CQR_FAILED)
+			rc = -EIO;
+		list_del_init(&cqr->blocklist);
+		dasd_sfree_request(cqr, device);
+		private->count--;
+	}
+
+	/*
+	 * in case of ENOMEM we need to retry after
+	 * first requests are finished
+	 */
+	if (fdata->start_unit <= fdata->stop_unit)
+		goto retry;
+
+	return rc;
+}
+
 static void dasd_eckd_handle_terminated_request(struct dasd_ccw_req *cqr)
 {
 	cqr->status = DASD_CQR_FILLED;
@@ -4305,8 +4432,9 @@ static struct dasd_discipline dasd_eckd_discipline = {
 	.uncheck_device = dasd_eckd_uncheck_device,
 	.do_analysis = dasd_eckd_do_analysis,
 	.verify_path = dasd_eckd_verify_path,
-	.ready_to_online = dasd_eckd_ready_to_online,
+	.basic_to_ready = dasd_eckd_basic_to_ready,
 	.online_to_ready = dasd_eckd_online_to_ready,
+	.ready_to_basic = dasd_eckd_ready_to_basic,
 	.fill_geometry = dasd_eckd_fill_geometry,
 	.start_IO = dasd_start_IO,
 	.term_IO = dasd_term_IO,
diff --git a/drivers/s390/block/dasd_int.h b/drivers/s390/block/dasd_int.h
index 899e3f5a56e..0785bd9bd5b 100644
--- a/drivers/s390/block/dasd_int.h
+++ b/drivers/s390/block/dasd_int.h
@@ -300,10 +300,11 @@ struct dasd_discipline {
 	 * Last things to do when a device is set online, and first things
 	 * when it is set offline.
 	 */
-	int (*ready_to_online) (struct dasd_device *);
+	int (*basic_to_ready) (struct dasd_device *);
 	int (*online_to_ready) (struct dasd_device *);
+	int (*ready_to_basic)  (struct dasd_device *);
 
-	/*
+	/* (struct dasd_device *);
 	 * Device operation functions. build_cp creates a ccw chain for
 	 * a block device request, start_io starts the request and
 	 * term_IO cancels it (e.g. in case of a timeout). format_device
@@ -317,8 +318,8 @@ struct dasd_discipline {
 	int (*start_IO) (struct dasd_ccw_req *);
 	int (*term_IO) (struct dasd_ccw_req *);
 	void (*handle_terminated_request) (struct dasd_ccw_req *);
-	struct dasd_ccw_req *(*format_device) (struct dasd_device *,
-					       struct format_data_t *);
+	int (*format_device) (struct dasd_device *,
+			      struct format_data_t *);
 	int (*free_cp) (struct dasd_ccw_req *, struct request *);
 
 	/*
@@ -672,6 +673,7 @@ int  dasd_term_IO(struct dasd_ccw_req *);
 void dasd_schedule_device_bh(struct dasd_device *);
 void dasd_schedule_block_bh(struct dasd_block *);
 int  dasd_sleep_on(struct dasd_ccw_req *);
+int  dasd_sleep_on_queue(struct list_head *);
 int  dasd_sleep_on_immediatly(struct dasd_ccw_req *);
 int  dasd_sleep_on_interruptible(struct dasd_ccw_req *);
 void dasd_device_set_timer(struct dasd_device *, int);
diff --git a/drivers/s390/block/dasd_ioctl.c b/drivers/s390/block/dasd_ioctl.c
index 03c0e044455..8be1b51e931 100644
--- a/drivers/s390/block/dasd_ioctl.c
+++ b/drivers/s390/block/dasd_ioctl.c
@@ -143,12 +143,12 @@ static int dasd_ioctl_resume(struct dasd_block *block)
 /*
  * performs formatting of _device_ according to _fdata_
  * Note: The discipline's format_function is assumed to deliver formatting
- * commands to format a single unit of the device. In terms of the ECKD
- * devices this means CCWs are generated to format a single track.
+ * commands to format multiple units of the device. In terms of the ECKD
+ * devices this means CCWs are generated to format multiple tracks.
  */
-static int dasd_format(struct dasd_block *block, struct format_data_t *fdata)
+static int
+dasd_format(struct dasd_block *block, struct format_data_t *fdata)
 {
-	struct dasd_ccw_req *cqr;
 	struct dasd_device *base;
 	int rc;
 
@@ -157,8 +157,8 @@ static int dasd_format(struct dasd_block *block, struct format_data_t *fdata)
 		return -EPERM;
 
 	if (base->state != DASD_STATE_BASIC) {
-		pr_warning("%s: The DASD cannot be formatted while it is "
-			   "enabled\n",  dev_name(&base->cdev->dev));
+		pr_warn("%s: The DASD cannot be formatted while it is enabled\n",
+			dev_name(&base->cdev->dev));
 		return -EBUSY;
 	}
 
@@ -178,21 +178,10 @@ static int dasd_format(struct dasd_block *block, struct format_data_t *fdata)
 		bdput(bdev);
 	}
 
-	while (fdata->start_unit <= fdata->stop_unit) {
-		cqr = base->discipline->format_device(base, fdata);
-		if (IS_ERR(cqr))
-			return PTR_ERR(cqr);
-		rc = dasd_sleep_on_interruptible(cqr);
-		dasd_sfree_request(cqr, cqr->memdev);
-		if (rc) {
-			if (rc != -ERESTARTSYS)
-				pr_err("%s: Formatting unit %d failed with "
-				       "rc=%d\n", dev_name(&base->cdev->dev),
-				       fdata->start_unit, rc);
-			return rc;
-		}
-		fdata->start_unit++;
-	}
+	rc = base->discipline->format_device(base, fdata);
+	if (rc)
+		return rc;
+
 	return 0;
 }
 
diff --git a/drivers/s390/block/scm_blk.c b/drivers/s390/block/scm_blk.c
index 5ac9c935c15..b303cab76a7 100644
--- a/drivers/s390/block/scm_blk.c
+++ b/drivers/s390/block/scm_blk.c
@@ -307,7 +307,7 @@ static void scm_blk_handle_error(struct scm_request *scmrq)
 	case EQC_WR_PROHIBIT:
 		spin_lock_irqsave(&bdev->lock, flags);
 		if (bdev->state != SCM_WR_PROHIBIT)
-			pr_info("%lu: Write access to the SCM increment is suspended\n",
+			pr_info("%lx: Write access to the SCM increment is suspended\n",
 				(unsigned long) bdev->scmdev->address);
 		bdev->state = SCM_WR_PROHIBIT;
 		spin_unlock_irqrestore(&bdev->lock, flags);
@@ -445,7 +445,7 @@ void scm_blk_set_available(struct scm_blk_dev *bdev)
 
 	spin_lock_irqsave(&bdev->lock, flags);
 	if (bdev->state == SCM_WR_PROHIBIT)
-		pr_info("%lu: Write access to the SCM increment is restored\n",
+		pr_info("%lx: Write access to the SCM increment is restored\n",
 			(unsigned long) bdev->scmdev->address);
 	bdev->state = SCM_OPER;
 	spin_unlock_irqrestore(&bdev->lock, flags);
@@ -463,12 +463,15 @@ static int __init scm_blk_init(void)
 		goto out;
 
 	scm_major = ret;
-	if (scm_alloc_rqs(nr_requests))
-		goto out_unreg;
+	ret = scm_alloc_rqs(nr_requests);
+	if (ret)
+		goto out_free;
 
 	scm_debug = debug_register("scm_log", 16, 1, 16);
-	if (!scm_debug)
+	if (!scm_debug) {
+		ret = -ENOMEM;
 		goto out_free;
+	}
 
 	debug_register_view(scm_debug, &debug_hex_ascii_view);
 	debug_set_level(scm_debug, 2);
@@ -483,7 +486,6 @@ out_dbf:
 	debug_unregister(scm_debug);
 out_free:
 	scm_free_rqs();
-out_unreg:
 	unregister_blkdev(scm_major, "scm");
 out:
 	return ret;
diff --git a/drivers/s390/block/scm_blk_cluster.c b/drivers/s390/block/scm_blk_cluster.c
index f4bb61b0cea..c0d102e3a48 100644
--- a/drivers/s390/block/scm_blk_cluster.c
+++ b/drivers/s390/block/scm_blk_cluster.c
@@ -223,6 +223,8 @@ void scm_cluster_request_irq(struct scm_request *scmrq)
 
 bool scm_cluster_size_valid(void)
 {
-	return write_cluster_size == 0 || write_cluster_size == 32 ||
-		write_cluster_size == 64 || write_cluster_size == 128;
+	if (write_cluster_size == 1 || write_cluster_size > 128)
+		return false;
+
+	return !(write_cluster_size & (write_cluster_size - 1));
 }
diff --git a/drivers/s390/block/scm_drv.c b/drivers/s390/block/scm_drv.c
index 5f6180d6ff0..c98cf52d78d 100644
--- a/drivers/s390/block/scm_drv.c
+++ b/drivers/s390/block/scm_drv.c
@@ -19,7 +19,7 @@ static void scm_notify(struct scm_device *scmdev, enum scm_event event)
 
 	switch (event) {
 	case SCM_CHANGE:
-		pr_info("%lu: The capabilities of the SCM increment changed\n",
+		pr_info("%lx: The capabilities of the SCM increment changed\n",
 			(unsigned long) scmdev->address);
 		SCM_LOG(2, "State changed");
 		SCM_LOG_STATE(2, scmdev);
diff --git a/drivers/s390/char/con3215.c b/drivers/s390/char/con3215.c
index 7b00fa634d4..eb5d22795c4 100644
--- a/drivers/s390/char/con3215.c
+++ b/drivers/s390/char/con3215.c
@@ -502,7 +502,7 @@ static void raw3215_make_room(struct raw3215_info *raw, unsigned int length)
 		raw3215_try_io(raw);
 		raw->flags &= ~RAW3215_FLUSHING;
 #ifdef CONFIG_TN3215_CONSOLE
-		wait_cons_dev();
+		ccw_device_wait_idle(raw->cdev);
 #endif
 		/* Enough room freed up ? */
 		if (RAW3215_BUFFER_SIZE - raw->count >= length)
@@ -858,7 +858,7 @@ static void con3215_flush(void)
 	raw = raw3215[0];  /* console 3215 is the first one */
 	if (raw->port.flags & ASYNC_SUSPENDED)
 		/* The console is still frozen for suspend. */
-		if (ccw_device_force_console())
+		if (ccw_device_force_console(raw->cdev))
 			/* Forcing didn't work, no panic message .. */
 			return;
 	spin_lock_irqsave(get_ccwdev_lock(raw->cdev), flags);
diff --git a/drivers/s390/char/monreader.c b/drivers/s390/char/monreader.c
index f4ff515db25..0da3ae3cd63 100644
--- a/drivers/s390/char/monreader.c
+++ b/drivers/s390/char/monreader.c
@@ -174,8 +174,7 @@ static void mon_free_mem(struct mon_private *monpriv)
 	int i;
 
 	for (i = 0; i < MON_MSGLIM; i++)
-		if (monpriv->msg_array[i])
-			kfree(monpriv->msg_array[i]);
+		kfree(monpriv->msg_array[i]);
 	kfree(monpriv);
 }
 
diff --git a/drivers/s390/char/raw3270.c b/drivers/s390/char/raw3270.c
index 4c9030a5b9f..24a08e8f19e 100644
--- a/drivers/s390/char/raw3270.c
+++ b/drivers/s390/char/raw3270.c
@@ -796,7 +796,7 @@ struct raw3270 __init *raw3270_setup_console(struct ccw_device *cdev)
 	do {
 		__raw3270_reset_device(rp);
 		while (!raw3270_state_final(rp)) {
-			wait_cons_dev();
+			ccw_device_wait_idle(rp->cdev);
 			barrier();
 		}
 	} while (rp->state != RAW3270_STATE_READY);
@@ -810,7 +810,7 @@ raw3270_wait_cons_dev(struct raw3270 *rp)
 	unsigned long flags;
 
 	spin_lock_irqsave(get_ccwdev_lock(rp->cdev), flags);
-	wait_cons_dev();
+	ccw_device_wait_idle(rp->cdev);
 	spin_unlock_irqrestore(get_ccwdev_lock(rp->cdev), flags);
 }
 
@@ -1274,7 +1274,7 @@ void raw3270_pm_unfreeze(struct raw3270_view *view)
 
 	rp = view->dev;
 	if (rp && test_bit(RAW3270_FLAGS_FROZEN, &rp->flags))
-		ccw_device_force_console();
+		ccw_device_force_console(rp->cdev);
 #endif
 }
 
diff --git a/drivers/s390/char/sclp_cmd.c b/drivers/s390/char/sclp_cmd.c
index cd798386b62..178836ec252 100644
--- a/drivers/s390/char/sclp_cmd.c
+++ b/drivers/s390/char/sclp_cmd.c
@@ -561,6 +561,8 @@ static void __init sclp_add_standby_memory(void)
 	add_memory_merged(0);
 }
 
+#define MEM_SCT_SIZE (1UL << SECTION_SIZE_BITS)
+
 static void __init insert_increment(u16 rn, int standby, int assigned)
 {
 	struct memory_increment *incr, *new_incr;
@@ -573,7 +575,7 @@ static void __init insert_increment(u16 rn, int standby, int assigned)
 	new_incr->rn = rn;
 	new_incr->standby = standby;
 	if (!standby)
-		new_incr->usecount = 1;
+		new_incr->usecount = rzm > MEM_SCT_SIZE ? rzm/MEM_SCT_SIZE : 1;
 	last_rn = 0;
 	prev = &sclp_mem_list;
 	list_for_each_entry(incr, &sclp_mem_list, list) {
diff --git a/drivers/s390/char/sclp_tty.c b/drivers/s390/char/sclp_tty.c
index 14b4cb8abcc..7ed7a598781 100644
--- a/drivers/s390/char/sclp_tty.c
+++ b/drivers/s390/char/sclp_tty.c
@@ -107,7 +107,6 @@ sclp_tty_write_room (struct tty_struct *tty)
 static void
 sclp_ttybuf_callback(struct sclp_buffer *buffer, int rc)
 {
-	struct tty_struct *tty;
 	unsigned long flags;
 	void *page;
 
@@ -125,12 +124,8 @@ sclp_ttybuf_callback(struct sclp_buffer *buffer, int rc)
 					    struct sclp_buffer, list);
 		spin_unlock_irqrestore(&sclp_tty_lock, flags);
 	} while (buffer && sclp_emit_buffer(buffer, sclp_ttybuf_callback));
-	/* check if the tty needs a wake up call */
-	tty = tty_port_tty_get(&sclp_port);
-	if (tty != NULL) {
-		tty_wakeup(tty);
-		tty_kref_put(tty);
-	}
+
+	tty_port_tty_wakeup(&sclp_port);
 }
 
 static inline void
diff --git a/drivers/s390/char/sclp_vt220.c b/drivers/s390/char/sclp_vt220.c
index 6c92f62623b..5aaaa2ec8df 100644
--- a/drivers/s390/char/sclp_vt220.c
+++ b/drivers/s390/char/sclp_vt220.c
@@ -114,7 +114,6 @@ static struct sclp_register sclp_vt220_register = {
 static void
 sclp_vt220_process_queue(struct sclp_vt220_request *request)
 {
-	struct tty_struct *tty;
 	unsigned long flags;
 	void *page;
 
@@ -139,12 +138,7 @@ sclp_vt220_process_queue(struct sclp_vt220_request *request)
 	} while (__sclp_vt220_emit(request));
 	if (request == NULL && sclp_vt220_flush_later)
 		sclp_vt220_emit_current();
-	/* Check if the tty needs a wake up call */
-	tty = tty_port_tty_get(&sclp_vt220_port);
-	if (tty) {
-		tty_wakeup(tty);
-		tty_kref_put(tty);
-	}
+	tty_port_tty_wakeup(&sclp_vt220_port);
 }
 
 #define SCLP_BUFFER_MAX_RETRY		1
diff --git a/drivers/s390/char/tty3270.c b/drivers/s390/char/tty3270.c
index b907dba2402..cee69dac3e1 100644
--- a/drivers/s390/char/tty3270.c
+++ b/drivers/s390/char/tty3270.c
@@ -915,7 +915,7 @@ static int tty3270_install(struct tty_driver *driver, struct tty_struct *tty)
 	int i, rc;
 
 	/* Check if the tty3270 is already there. */
-	view = raw3270_find_view(&tty3270_fn, tty->index);
+	view = raw3270_find_view(&tty3270_fn, tty->index + RAW3270_FIRSTMINOR);
 	if (!IS_ERR(view)) {
 		tp = container_of(view, struct tty3270, view);
 		tty->driver_data = tp;
@@ -927,15 +927,16 @@ static int tty3270_install(struct tty_driver *driver, struct tty_struct *tty)
 		tp->inattr = TF_INPUT;
 		return tty_port_install(&tp->port, driver, tty);
 	}
-	if (tty3270_max_index < tty->index)
-		tty3270_max_index = tty->index;
+	if (tty3270_max_index < tty->index + 1)
+		tty3270_max_index = tty->index + 1;
 
 	/* Allocate tty3270 structure on first open. */
 	tp = tty3270_alloc_view();
 	if (IS_ERR(tp))
 		return PTR_ERR(tp);
 
-	rc = raw3270_add_view(&tp->view, &tty3270_fn, tty->index);
+	rc = raw3270_add_view(&tp->view, &tty3270_fn,
+			      tty->index + RAW3270_FIRSTMINOR);
 	if (rc) {
 		tty3270_free_view(tp);
 		return rc;
@@ -1846,12 +1847,12 @@ static const struct tty_operations tty3270_ops = {
 
 void tty3270_create_cb(int minor)
 {
-	tty_register_device(tty3270_driver, minor, NULL);
+	tty_register_device(tty3270_driver, minor - RAW3270_FIRSTMINOR, NULL);
 }
 
 void tty3270_destroy_cb(int minor)
 {
-	tty_unregister_device(tty3270_driver, minor);
+	tty_unregister_device(tty3270_driver, minor - RAW3270_FIRSTMINOR);
 }
 
 struct raw3270_notifier tty3270_notifier =
@@ -1884,7 +1885,8 @@ static int __init tty3270_init(void)
 	driver->driver_name = "tty3270";
 	driver->name = "3270/tty";
 	driver->major = IBM_TTY3270_MAJOR;
-	driver->minor_start = 0;
+	driver->minor_start = RAW3270_FIRSTMINOR;
+	driver->name_base = RAW3270_FIRSTMINOR;
 	driver->type = TTY_DRIVER_TYPE_SYSTEM;
 	driver->subtype = SYSTEM_TYPE_TTY;
 	driver->init_termios = tty_std_termios;
diff --git a/drivers/s390/char/zcore.c b/drivers/s390/char/zcore.c
index 1d61a01576d..22820610022 100644
--- a/drivers/s390/char/zcore.c
+++ b/drivers/s390/char/zcore.c
@@ -127,7 +127,7 @@ static int memcpy_hsa(void *dest, unsigned long src, size_t count, int mode)
 	}
 	if (mode == TO_USER) {
 		if (copy_to_user((__force __user void*) dest + offs, buf,
-				 PAGE_SIZE))
+				 count - offs))
 			return -EFAULT;
 	} else
 		memcpy(dest + offs, buf, count - offs);
diff --git a/drivers/s390/cio/chp.c b/drivers/s390/cio/chp.c
index 50ad5fdd815..21fabc6d5a9 100644
--- a/drivers/s390/cio/chp.c
+++ b/drivers/s390/cio/chp.c
@@ -377,6 +377,26 @@ static void chp_release(struct device *dev)
 }
 
 /**
+ * chp_update_desc - update channel-path description
+ * @chp - channel-path
+ *
+ * Update the channel-path description of the specified channel-path.
+ * Return zero on success, non-zero otherwise.
+ */
+int chp_update_desc(struct channel_path *chp)
+{
+	int rc;
+
+	rc = chsc_determine_base_channel_path_desc(chp->chpid, &chp->desc);
+	if (rc)
+		return rc;
+
+	rc = chsc_determine_fmt1_channel_path_desc(chp->chpid, &chp->desc_fmt1);
+
+	return rc;
+}
+
+/**
  * chp_new - register a new channel-path
  * @chpid - channel-path ID
  *
@@ -403,7 +423,7 @@ int chp_new(struct chp_id chpid)
 	mutex_init(&chp->lock);
 
 	/* Obtain channel path description and fill it in. */
-	ret = chsc_determine_base_channel_path_desc(chpid, &chp->desc);
+	ret = chp_update_desc(chp);
 	if (ret)
 		goto out_free;
 	if ((chp->desc.flags & 0x80) == 0) {
diff --git a/drivers/s390/cio/chp.h b/drivers/s390/cio/chp.h
index e1399dbee83..9284b785a06 100644
--- a/drivers/s390/cio/chp.h
+++ b/drivers/s390/cio/chp.h
@@ -44,6 +44,7 @@ struct channel_path {
 	struct mutex lock; /* Serialize access to below members. */
 	int state;
 	struct channel_path_desc desc;
+	struct channel_path_desc_fmt1 desc_fmt1;
 	/* Channel-measurement related stuff: */
 	int cmg;
 	int shared;
@@ -62,6 +63,7 @@ int chp_is_registered(struct chp_id chpid);
 void *chp_get_chp_desc(struct chp_id chpid);
 void chp_remove_cmg_attr(struct channel_path *chp);
 int chp_add_cmg_attr(struct channel_path *chp);
+int chp_update_desc(struct channel_path *chp);
 int chp_new(struct chp_id chpid);
 void chp_cfg_schedule(struct chp_id chpid, int configure);
 void chp_cfg_cancel_deconfigure(struct chp_id chpid);
diff --git a/drivers/s390/cio/chsc.c b/drivers/s390/cio/chsc.c
index e16c553f655..8ea7d9b2c67 100644
--- a/drivers/s390/cio/chsc.c
+++ b/drivers/s390/cio/chsc.c
@@ -376,7 +376,7 @@ static void chsc_process_sei_chp_avail(struct chsc_sei_nt0_area *sei_area)
 			continue;
 		}
 		mutex_lock(&chp->lock);
-		chsc_determine_base_channel_path_desc(chpid, &chp->desc);
+		chp_update_desc(chp);
 		mutex_unlock(&chp->lock);
 	}
 }
@@ -631,8 +631,8 @@ int chsc_chp_vary(struct chp_id chpid, int on)
 	 * Redo PathVerification on the devices the chpid connects to
 	 */
 	if (on) {
-		/* Try to update the channel path descritor. */
-		chsc_determine_base_channel_path_desc(chpid, &chp->desc);
+		/* Try to update the channel path description. */
+		chp_update_desc(chp);
 		for_each_subchannel_staged(s390_subchannel_vary_chpid_on,
 					   __s390_vary_chpid_on, &chpid);
 	} else
@@ -825,9 +825,10 @@ int chsc_determine_fmt1_channel_path_desc(struct chp_id chpid,
 {
 	struct chsc_response_struct *chsc_resp;
 	struct chsc_scpd *scpd_area;
+	unsigned long flags;
 	int ret;
 
-	spin_lock_irq(&chsc_page_lock);
+	spin_lock_irqsave(&chsc_page_lock, flags);
 	scpd_area = chsc_page;
 	ret = chsc_determine_channel_path_desc(chpid, 0, 0, 1, 0, scpd_area);
 	if (ret)
@@ -835,7 +836,7 @@ int chsc_determine_fmt1_channel_path_desc(struct chp_id chpid,
 	chsc_resp = (void *)&scpd_area->response;
 	memcpy(desc, &chsc_resp->data, sizeof(*desc));
 out:
-	spin_unlock_irq(&chsc_page_lock);
+	spin_unlock_irqrestore(&chsc_page_lock, flags);
 	return ret;
 }
 
diff --git a/drivers/s390/cio/cio.c b/drivers/s390/cio/cio.c
index 986ef6a92a4..935d80b4e9c 100644
--- a/drivers/s390/cio/cio.c
+++ b/drivers/s390/cio/cio.c
@@ -471,15 +471,6 @@ int cio_disable_subchannel(struct subchannel *sch)
 }
 EXPORT_SYMBOL_GPL(cio_disable_subchannel);
 
-int cio_create_sch_lock(struct subchannel *sch)
-{
-	sch->lock = kmalloc(sizeof(spinlock_t), GFP_KERNEL);
-	if (!sch->lock)
-		return -ENOMEM;
-	spin_lock_init(sch->lock);
-	return 0;
-}
-
 static int cio_check_devno_blacklisted(struct subchannel *sch)
 {
 	if (is_blacklisted(sch->schid.ssid, sch->schib.pmcw.dev)) {
@@ -536,32 +527,19 @@ int cio_validate_subchannel(struct subchannel *sch, struct subchannel_id schid)
 	sprintf(dbf_txt, "valsch%x", schid.sch_no);
 	CIO_TRACE_EVENT(4, dbf_txt);
 
-	/* Nuke all fields. */
-	memset(sch, 0, sizeof(struct subchannel));
-
-	sch->schid = schid;
-	if (cio_is_console(schid)) {
-		sch->lock = cio_get_console_lock();
-	} else {
-		err = cio_create_sch_lock(sch);
-		if (err)
-			goto out;
-	}
-	mutex_init(&sch->reg_mutex);
-
 	/*
 	 * The first subchannel that is not-operational (ccode==3)
-	 *  indicates that there aren't any more devices available.
+	 * indicates that there aren't any more devices available.
 	 * If stsch gets an exception, it means the current subchannel set
-	 *  is not valid.
+	 * is not valid.
 	 */
-	ccode = stsch_err (schid, &sch->schib);
+	ccode = stsch_err(schid, &sch->schib);
 	if (ccode) {
 		err = (ccode == 3) ? -ENXIO : ccode;
 		goto out;
 	}
-	/* Copy subchannel type from path management control word. */
 	sch->st = sch->schib.pmcw.st;
+	sch->schid = schid;
 
 	switch (sch->st) {
 	case SUBCHANNEL_TYPE_IO:
@@ -578,11 +556,7 @@ int cio_validate_subchannel(struct subchannel *sch, struct subchannel_id schid)
 
 	CIO_MSG_EVENT(4, "Subchannel 0.%x.%04x reports subchannel type %04X\n",
 		      sch->schid.ssid, sch->schid.sch_no, sch->st);
-	return 0;
 out:
-	if (!cio_is_console(schid))
-		kfree(sch->lock);
-	sch->lock = NULL;
 	return err;
 }
 
@@ -650,15 +624,13 @@ void __irq_entry do_IRQ(struct pt_regs *regs)
 }
 
 #ifdef CONFIG_CCW_CONSOLE
-static struct subchannel console_subchannel;
-static struct io_subchannel_private console_priv;
-static int console_subchannel_in_use;
+static struct subchannel *console_sch;
 
 /*
  * Use cio_tsch to update the subchannel status and call the interrupt handler
- * if status had been pending. Called with the console_subchannel lock.
+ * if status had been pending. Called with the subchannel's lock held.
  */
-static void cio_tsch(struct subchannel *sch)
+void cio_tsch(struct subchannel *sch)
 {
 	struct irb *irb;
 	int irq_context;
@@ -675,6 +647,7 @@ static void cio_tsch(struct subchannel *sch)
 		local_bh_disable();
 		irq_enter();
 	}
+	kstat_incr_irqs_this_cpu(IO_INTERRUPT, NULL);
 	if (sch->driver && sch->driver->irq)
 		sch->driver->irq(sch);
 	else
@@ -685,135 +658,90 @@ static void cio_tsch(struct subchannel *sch)
 	}
 }
 
-void *cio_get_console_priv(void)
-{
-	return &console_priv;
-}
-
-/*
- * busy wait for the next interrupt on the console
- */
-void wait_cons_dev(void)
+static int cio_test_for_console(struct subchannel_id schid, void *data)
 {
-	if (!console_subchannel_in_use)
-		return;
-
-	while (1) {
-		cio_tsch(&console_subchannel);
-		if (console_subchannel.schib.scsw.cmd.actl == 0)
-			break;
-		udelay_simple(100);
-	}
-}
+	struct schib schib;
 
-static int
-cio_test_for_console(struct subchannel_id schid, void *data)
-{
-	if (stsch_err(schid, &console_subchannel.schib) != 0)
+	if (stsch_err(schid, &schib) != 0)
 		return -ENXIO;
-	if ((console_subchannel.schib.pmcw.st == SUBCHANNEL_TYPE_IO) &&
-	    console_subchannel.schib.pmcw.dnv &&
-	    (console_subchannel.schib.pmcw.dev == console_devno)) {
+	if ((schib.pmcw.st == SUBCHANNEL_TYPE_IO) && schib.pmcw.dnv &&
+	    (schib.pmcw.dev == console_devno)) {
 		console_irq = schid.sch_no;
 		return 1; /* found */
 	}
 	return 0;
 }
 
-
-static int
-cio_get_console_sch_no(void)
+static int cio_get_console_sch_no(void)
 {
 	struct subchannel_id schid;
-	
+	struct schib schib;
+
 	init_subchannel_id(&schid);
 	if (console_irq != -1) {
 		/* VM provided us with the irq number of the console. */
 		schid.sch_no = console_irq;
-		if (stsch_err(schid, &console_subchannel.schib) != 0 ||
-		    (console_subchannel.schib.pmcw.st != SUBCHANNEL_TYPE_IO) ||
-		    !console_subchannel.schib.pmcw.dnv)
+		if (stsch_err(schid, &schib) != 0 ||
+		    (schib.pmcw.st != SUBCHANNEL_TYPE_IO) || !schib.pmcw.dnv)
 			return -1;
-		console_devno = console_subchannel.schib.pmcw.dev;
+		console_devno = schib.pmcw.dev;
 	} else if (console_devno != -1) {
 		/* At least the console device number is known. */
 		for_each_subchannel(cio_test_for_console, NULL);
-		if (console_irq == -1)
-			return -1;
-	} else {
-		/* unlike in 2.4, we cannot autoprobe here, since
-		 * the channel subsystem is not fully initialized.
-		 * With some luck, the HWC console can take over */
-		return -1;
 	}
 	return console_irq;
 }
 
-struct subchannel *
-cio_probe_console(void)
+struct subchannel *cio_probe_console(void)
 {
-	int sch_no, ret;
 	struct subchannel_id schid;
+	struct subchannel *sch;
+	int sch_no, ret;
 
-	if (xchg(&console_subchannel_in_use, 1) != 0)
-		return ERR_PTR(-EBUSY);
 	sch_no = cio_get_console_sch_no();
 	if (sch_no == -1) {
-		console_subchannel_in_use = 0;
 		pr_warning("No CCW console was found\n");
 		return ERR_PTR(-ENODEV);
 	}
-	memset(&console_subchannel, 0, sizeof(struct subchannel));
 	init_subchannel_id(&schid);
 	schid.sch_no = sch_no;
-	ret = cio_validate_subchannel(&console_subchannel, schid);
-	if (ret) {
-		console_subchannel_in_use = 0;
-		return ERR_PTR(-ENODEV);
-	}
+	sch = css_alloc_subchannel(schid);
+	if (IS_ERR(sch))
+		return sch;
 
-	/*
-	 * enable console I/O-interrupt subclass
-	 */
 	isc_register(CONSOLE_ISC);
-	console_subchannel.config.isc = CONSOLE_ISC;
-	console_subchannel.config.intparm = (u32)(addr_t)&console_subchannel;
-	ret = cio_commit_config(&console_subchannel);
+	sch->config.isc = CONSOLE_ISC;
+	sch->config.intparm = (u32)(addr_t)sch;
+	ret = cio_commit_config(sch);
 	if (ret) {
 		isc_unregister(CONSOLE_ISC);
-		console_subchannel_in_use = 0;
+		put_device(&sch->dev);
 		return ERR_PTR(ret);
 	}
-	return &console_subchannel;
-}
-
-void
-cio_release_console(void)
-{
-	console_subchannel.config.intparm = 0;
-	cio_commit_config(&console_subchannel);
-	isc_unregister(CONSOLE_ISC);
-	console_subchannel_in_use = 0;
+	console_sch = sch;
+	return sch;
 }
 
-/* Bah... hack to catch console special sausages. */
-int
-cio_is_console(struct subchannel_id schid)
+int cio_is_console(struct subchannel_id schid)
 {
-	if (!console_subchannel_in_use)
+	if (!console_sch)
 		return 0;
-	return schid_equal(&schid, &console_subchannel.schid);
+	return schid_equal(&schid, &console_sch->schid);
 }
 
-struct subchannel *
-cio_get_console_subchannel(void)
+void cio_register_early_subchannels(void)
 {
-	if (!console_subchannel_in_use)
-		return NULL;
-	return &console_subchannel;
+	int ret;
+
+	if (!console_sch)
+		return;
+
+	ret = css_register_subchannel(console_sch);
+	if (ret)
+		put_device(&console_sch->dev);
 }
+#endif /* CONFIG_CCW_CONSOLE */
 
-#endif
 static int
 __disable_subchannel_easy(struct subchannel_id schid, struct schib *schib)
 {
diff --git a/drivers/s390/cio/cio.h b/drivers/s390/cio/cio.h
index 4a1ff5c2eb8..d62f5e7f3cf 100644
--- a/drivers/s390/cio/cio.h
+++ b/drivers/s390/cio/cio.h
@@ -121,23 +121,18 @@ extern int cio_commit_config(struct subchannel *sch);
 int cio_tm_start_key(struct subchannel *sch, struct tcw *tcw, u8 lpm, u8 key);
 int cio_tm_intrg(struct subchannel *sch);
 
-int cio_create_sch_lock(struct subchannel *);
 void do_adapter_IO(u8 isc);
 void do_IRQ(struct pt_regs *);
 
 /* Use with care. */
 #ifdef CONFIG_CCW_CONSOLE
 extern struct subchannel *cio_probe_console(void);
-extern void cio_release_console(void);
 extern int cio_is_console(struct subchannel_id);
-extern struct subchannel *cio_get_console_subchannel(void);
-extern spinlock_t * cio_get_console_lock(void);
-extern void *cio_get_console_priv(void);
+extern void cio_register_early_subchannels(void);
+extern void cio_tsch(struct subchannel *sch);
 #else
 #define cio_is_console(schid) 0
-#define cio_get_console_subchannel() NULL
-#define cio_get_console_lock() NULL
-#define cio_get_console_priv() NULL
+static inline void cio_register_early_subchannels(void) {}
 #endif
 
 #endif
diff --git a/drivers/s390/cio/css.c b/drivers/s390/cio/css.c
index a239237d43f..1ebe5d3ddeb 100644
--- a/drivers/s390/cio/css.c
+++ b/drivers/s390/cio/css.c
@@ -137,37 +137,53 @@ out:
 
 static void css_sch_todo(struct work_struct *work);
 
-static struct subchannel *
-css_alloc_subchannel(struct subchannel_id schid)
+static int css_sch_create_locks(struct subchannel *sch)
+{
+	sch->lock = kmalloc(sizeof(*sch->lock), GFP_KERNEL);
+	if (!sch->lock)
+		return -ENOMEM;
+
+	spin_lock_init(sch->lock);
+	mutex_init(&sch->reg_mutex);
+
+	return 0;
+}
+
+static void css_subchannel_release(struct device *dev)
+{
+	struct subchannel *sch = to_subchannel(dev);
+
+	sch->config.intparm = 0;
+	cio_commit_config(sch);
+	kfree(sch->lock);
+	kfree(sch);
+}
+
+struct subchannel *css_alloc_subchannel(struct subchannel_id schid)
 {
 	struct subchannel *sch;
 	int ret;
 
-	sch = kmalloc (sizeof (*sch), GFP_KERNEL | GFP_DMA);
-	if (sch == NULL)
+	sch = kzalloc(sizeof(*sch), GFP_KERNEL | GFP_DMA);
+	if (!sch)
 		return ERR_PTR(-ENOMEM);
-	ret = cio_validate_subchannel (sch, schid);
-	if (ret < 0) {
-		kfree(sch);
-		return ERR_PTR(ret);
-	}
+
+	ret = cio_validate_subchannel(sch, schid);
+	if (ret < 0)
+		goto err;
+
+	ret = css_sch_create_locks(sch);
+	if (ret)
+		goto err;
+
 	INIT_WORK(&sch->todo_work, css_sch_todo);
+	sch->dev.release = &css_subchannel_release;
+	device_initialize(&sch->dev);
 	return sch;
-}
-
-static void
-css_subchannel_release(struct device *dev)
-{
-	struct subchannel *sch;
 
-	sch = to_subchannel(dev);
-	if (!cio_is_console(sch->schid)) {
-		/* Reset intparm to zeroes. */
-		sch->config.intparm = 0;
-		cio_commit_config(sch);
-		kfree(sch->lock);
-		kfree(sch);
-	}
+err:
+	kfree(sch);
+	return ERR_PTR(ret);
 }
 
 static int css_sch_device_register(struct subchannel *sch)
@@ -177,7 +193,7 @@ static int css_sch_device_register(struct subchannel *sch)
 	mutex_lock(&sch->reg_mutex);
 	dev_set_name(&sch->dev, "0.%x.%04x", sch->schid.ssid,
 		     sch->schid.sch_no);
-	ret = device_register(&sch->dev);
+	ret = device_add(&sch->dev);
 	mutex_unlock(&sch->reg_mutex);
 	return ret;
 }
@@ -228,16 +244,11 @@ void css_update_ssd_info(struct subchannel *sch)
 {
 	int ret;
 
-	if (cio_is_console(sch->schid)) {
-		/* Console is initialized too early for functions requiring
-		 * memory allocation. */
+	ret = chsc_get_ssd_info(sch->schid, &sch->ssd_info);
+	if (ret)
 		ssd_from_pmcw(&sch->ssd_info, &sch->schib.pmcw);
-	} else {
-		ret = chsc_get_ssd_info(sch->schid, &sch->ssd_info);
-		if (ret)
-			ssd_from_pmcw(&sch->ssd_info, &sch->schib.pmcw);
-		ssd_register_chpids(&sch->ssd_info);
-	}
+
+	ssd_register_chpids(&sch->ssd_info);
 }
 
 static ssize_t type_show(struct device *dev, struct device_attribute *attr,
@@ -275,14 +286,13 @@ static const struct attribute_group *default_subch_attr_groups[] = {
 	NULL,
 };
 
-static int css_register_subchannel(struct subchannel *sch)
+int css_register_subchannel(struct subchannel *sch)
 {
 	int ret;
 
 	/* Initialize the subchannel structure */
 	sch->dev.parent = &channel_subsystems[0]->device;
 	sch->dev.bus = &css_bus_type;
-	sch->dev.release = &css_subchannel_release;
 	sch->dev.groups = default_subch_attr_groups;
 	/*
 	 * We don't want to generate uevents for I/O subchannels that don't
@@ -314,23 +324,19 @@ static int css_register_subchannel(struct subchannel *sch)
 	return ret;
 }
 
-int css_probe_device(struct subchannel_id schid)
+static int css_probe_device(struct subchannel_id schid)
 {
-	int ret;
 	struct subchannel *sch;
+	int ret;
+
+	sch = css_alloc_subchannel(schid);
+	if (IS_ERR(sch))
+		return PTR_ERR(sch);
 
-	if (cio_is_console(schid))
-		sch = cio_get_console_subchannel();
-	else {
-		sch = css_alloc_subchannel(schid);
-		if (IS_ERR(sch))
-			return PTR_ERR(sch);
-	}
 	ret = css_register_subchannel(sch);
-	if (ret) {
-		if (!cio_is_console(schid))
-			put_device(&sch->dev);
-	}
+	if (ret)
+		put_device(&sch->dev);
+
 	return ret;
 }
 
@@ -770,7 +776,7 @@ static int __init setup_css(int nr)
 	css->pseudo_subchannel->dev.release = css_subchannel_release;
 	dev_set_name(&css->pseudo_subchannel->dev, "defunct");
 	mutex_init(&css->pseudo_subchannel->reg_mutex);
-	ret = cio_create_sch_lock(css->pseudo_subchannel);
+	ret = css_sch_create_locks(css->pseudo_subchannel);
 	if (ret) {
 		kfree(css->pseudo_subchannel);
 		return ret;
@@ -870,8 +876,7 @@ static struct notifier_block css_power_notifier = {
 
 /*
  * Now that the driver core is running, we can setup our channel subsystem.
- * The struct subchannel's are created during probing (except for the
- * static console subchannel).
+ * The struct subchannel's are created during probing.
  */
 static int __init css_bus_init(void)
 {
@@ -1050,6 +1055,8 @@ int css_complete_work(void)
  */
 static int __init channel_subsystem_init_sync(void)
 {
+	/* Register subchannels which are already in use. */
+	cio_register_early_subchannels();
 	/* Start initial subchannel evaluation. */
 	css_schedule_eval_all();
 	css_complete_work();
@@ -1065,9 +1072,8 @@ void channel_subsystem_reinit(void)
 	chsc_enable_facility(CHSC_SDA_OC_MSS);
 	chp_id_for_each(&chpid) {
 		chp = chpid_to_chp(chpid);
-		if (!chp)
-			continue;
-		chsc_determine_base_channel_path_desc(chpid, &chp->desc);
+		if (chp)
+			chp_update_desc(chp);
 	}
 }
 
diff --git a/drivers/s390/cio/css.h b/drivers/s390/cio/css.h
index 4af3dfe70ef..b1de6033523 100644
--- a/drivers/s390/cio/css.h
+++ b/drivers/s390/cio/css.h
@@ -101,7 +101,8 @@ extern int css_driver_register(struct css_driver *);
 extern void css_driver_unregister(struct css_driver *);
 
 extern void css_sch_device_unregister(struct subchannel *);
-extern int css_probe_device(struct subchannel_id);
+extern int css_register_subchannel(struct subchannel *);
+extern struct subchannel *css_alloc_subchannel(struct subchannel_id);
 extern struct subchannel *get_subchannel_by_schid(struct subchannel_id);
 extern int css_init_done;
 extern int max_ssid;
@@ -109,7 +110,6 @@ int for_each_subchannel_staged(int (*fn_known)(struct subchannel *, void *),
 			       int (*fn_unknown)(struct subchannel_id,
 			       void *), void *data);
 extern int for_each_subchannel(int(*fn)(struct subchannel_id, void *), void *);
-extern void css_reiterate_subchannels(void);
 void css_update_ssd_info(struct subchannel *sch);
 
 struct channel_subsystem {
diff --git a/drivers/s390/cio/device.c b/drivers/s390/cio/device.c
index c6767f5a58b..1ab5f6c36d9 100644
--- a/drivers/s390/cio/device.c
+++ b/drivers/s390/cio/device.c
@@ -19,6 +19,7 @@
 #include <linux/list.h>
 #include <linux/device.h>
 #include <linux/workqueue.h>
+#include <linux/delay.h>
 #include <linux/timer.h>
 #include <linux/kernel_stat.h>
 
@@ -43,6 +44,10 @@ static DEFINE_SPINLOCK(recovery_lock);
 static int recovery_phase;
 static const unsigned long recovery_delay[] = { 3, 30, 300 };
 
+static atomic_t ccw_device_init_count = ATOMIC_INIT(0);
+static DECLARE_WAIT_QUEUE_HEAD(ccw_device_init_wq);
+static struct bus_type ccw_bus_type;
+
 /******************* bus type handling ***********************/
 
 /* The Linux driver model distinguishes between a bus type and
@@ -127,8 +132,6 @@ static int ccw_uevent(struct device *dev, struct kobj_uevent_env *env)
 	return ret;
 }
 
-static struct bus_type ccw_bus_type;
-
 static void io_subchannel_irq(struct subchannel *);
 static int io_subchannel_probe(struct subchannel *);
 static int io_subchannel_remove(struct subchannel *);
@@ -137,8 +140,6 @@ static int io_subchannel_sch_event(struct subchannel *, int);
 static int io_subchannel_chp_event(struct subchannel *, struct chp_link *,
 				   int);
 static void recovery_func(unsigned long data);
-wait_queue_head_t ccw_device_init_wq;
-atomic_t ccw_device_init_count;
 
 static struct css_device_id io_subchannel_ids[] = {
 	{ .match_flags = 0x1, .type = SUBCHANNEL_TYPE_IO, },
@@ -191,10 +192,7 @@ int __init io_subchannel_init(void)
 {
 	int ret;
 
-	init_waitqueue_head(&ccw_device_init_wq);
-	atomic_set(&ccw_device_init_count, 0);
 	setup_timer(&recovery_timer, recovery_func, 0);
-
 	ret = bus_register(&ccw_bus_type);
 	if (ret)
 		return ret;
@@ -1086,19 +1084,14 @@ static int io_subchannel_probe(struct subchannel *sch)
 		dev_set_uevent_suppress(&sch->dev, 0);
 		kobject_uevent(&sch->dev.kobj, KOBJ_ADD);
 		cdev = sch_get_cdev(sch);
-		cdev->dev.groups = ccwdev_attr_groups;
-		device_initialize(&cdev->dev);
-		cdev->private->flags.initialized = 1;
-		ccw_device_register(cdev);
-		/*
-		 * Check if the device is already online. If it is
-		 * the reference count needs to be corrected since we
-		 * didn't obtain a reference in ccw_device_set_online.
-		 */
-		if (cdev->private->state != DEV_STATE_NOT_OPER &&
-		    cdev->private->state != DEV_STATE_OFFLINE &&
-		    cdev->private->state != DEV_STATE_BOXED)
-			get_device(&cdev->dev);
+		rc = ccw_device_register(cdev);
+		if (rc) {
+			/* Release online reference. */
+			put_device(&cdev->dev);
+			goto out_schedule;
+		}
+		if (atomic_dec_and_test(&ccw_device_init_count))
+			wake_up(&ccw_device_init_wq);
 		return 0;
 	}
 	io_subchannel_init_fields(sch);
@@ -1580,88 +1573,102 @@ out:
 }
 
 #ifdef CONFIG_CCW_CONSOLE
-static struct ccw_device console_cdev;
-static struct ccw_device_private console_private;
-static int console_cdev_in_use;
-
-static DEFINE_SPINLOCK(ccw_console_lock);
-
-spinlock_t * cio_get_console_lock(void)
-{
-	return &ccw_console_lock;
-}
-
 static int ccw_device_console_enable(struct ccw_device *cdev,
 				     struct subchannel *sch)
 {
-	struct io_subchannel_private *io_priv = cio_get_console_priv();
 	int rc;
 
-	/* Attach subchannel private data. */
-	memset(io_priv, 0, sizeof(*io_priv));
-	set_io_private(sch, io_priv);
 	io_subchannel_init_fields(sch);
 	rc = cio_commit_config(sch);
 	if (rc)
 		return rc;
 	sch->driver = &io_subchannel_driver;
-	/* Initialize the ccw_device structure. */
-	cdev->dev.parent= &sch->dev;
 	sch_set_cdev(sch, cdev);
 	io_subchannel_recog(cdev, sch);
 	/* Now wait for the async. recognition to come to an end. */
 	spin_lock_irq(cdev->ccwlock);
 	while (!dev_fsm_final_state(cdev))
-		wait_cons_dev();
-	rc = -EIO;
-	if (cdev->private->state != DEV_STATE_OFFLINE)
+		ccw_device_wait_idle(cdev);
+
+	/* Hold on to an extra reference while device is online. */
+	get_device(&cdev->dev);
+	rc = ccw_device_online(cdev);
+	if (rc)
 		goto out_unlock;
-	ccw_device_online(cdev);
+
 	while (!dev_fsm_final_state(cdev))
-		wait_cons_dev();
-	if (cdev->private->state != DEV_STATE_ONLINE)
-		goto out_unlock;
-	rc = 0;
+		ccw_device_wait_idle(cdev);
+
+	if (cdev->private->state == DEV_STATE_ONLINE)
+		cdev->online = 1;
+	else
+		rc = -EIO;
 out_unlock:
 	spin_unlock_irq(cdev->ccwlock);
+	if (rc) /* Give up online reference since onlining failed. */
+		put_device(&cdev->dev);
 	return rc;
 }
 
-struct ccw_device *
-ccw_device_probe_console(void)
+struct ccw_device *ccw_device_probe_console(void)
 {
+	struct io_subchannel_private *io_priv;
+	struct ccw_device *cdev;
 	struct subchannel *sch;
 	int ret;
 
-	if (xchg(&console_cdev_in_use, 1) != 0)
-		return ERR_PTR(-EBUSY);
 	sch = cio_probe_console();
-	if (IS_ERR(sch)) {
-		console_cdev_in_use = 0;
-		return (void *) sch;
+	if (IS_ERR(sch))
+		return ERR_CAST(sch);
+
+	io_priv = kzalloc(sizeof(*io_priv), GFP_KERNEL | GFP_DMA);
+	if (!io_priv) {
+		put_device(&sch->dev);
+		return ERR_PTR(-ENOMEM);
 	}
-	memset(&console_cdev, 0, sizeof(struct ccw_device));
-	memset(&console_private, 0, sizeof(struct ccw_device_private));
-	console_cdev.private = &console_private;
-	console_private.cdev = &console_cdev;
-	console_private.int_class = IRQIO_CIO;
-	ret = ccw_device_console_enable(&console_cdev, sch);
+	cdev = io_subchannel_create_ccwdev(sch);
+	if (IS_ERR(cdev)) {
+		put_device(&sch->dev);
+		kfree(io_priv);
+		return cdev;
+	}
+	set_io_private(sch, io_priv);
+	ret = ccw_device_console_enable(cdev, sch);
 	if (ret) {
-		cio_release_console();
-		console_cdev_in_use = 0;
+		set_io_private(sch, NULL);
+		put_device(&sch->dev);
+		put_device(&cdev->dev);
+		kfree(io_priv);
 		return ERR_PTR(ret);
 	}
-	console_cdev.online = 1;
-	return &console_cdev;
+	return cdev;
+}
+
+/**
+ * ccw_device_wait_idle() - busy wait for device to become idle
+ * @cdev: ccw device
+ *
+ * Poll until activity control is zero, that is, no function or data
+ * transfer is pending/active.
+ * Called with device lock being held.
+ */
+void ccw_device_wait_idle(struct ccw_device *cdev)
+{
+	struct subchannel *sch = to_subchannel(cdev->dev.parent);
+
+	while (1) {
+		cio_tsch(sch);
+		if (sch->schib.scsw.cmd.actl == 0)
+			break;
+		udelay_simple(100);
+	}
 }
 
 static int ccw_device_pm_restore(struct device *dev);
 
-int ccw_device_force_console(void)
+int ccw_device_force_console(struct ccw_device *cdev)
 {
-	if (!console_cdev_in_use)
-		return -ENODEV;
-	return ccw_device_pm_restore(&console_cdev.dev);
+	return ccw_device_pm_restore(&cdev->dev);
 }
 EXPORT_SYMBOL_GPL(ccw_device_force_console);
 #endif
diff --git a/drivers/s390/cio/device.h b/drivers/s390/cio/device.h
index 7d4ecb65db0..8d1d2987317 100644
--- a/drivers/s390/cio/device.h
+++ b/drivers/s390/cio/device.h
@@ -81,8 +81,6 @@ dev_fsm_final_state(struct ccw_device *cdev)
 		cdev->private->state == DEV_STATE_BOXED);
 }
 
-extern wait_queue_head_t ccw_device_init_wq;
-extern atomic_t ccw_device_init_count;
 int __init io_subchannel_init(void);
 
 void io_subchannel_recog_done(struct ccw_device *cdev);
diff --git a/drivers/s390/cio/device_ops.c b/drivers/s390/cio/device_ops.c
index c77b6e06bf6..4845d64f284 100644
--- a/drivers/s390/cio/device_ops.c
+++ b/drivers/s390/cio/device_ops.c
@@ -704,9 +704,9 @@ EXPORT_SYMBOL(ccw_device_tm_start_timeout);
 int ccw_device_get_mdc(struct ccw_device *cdev, u8 mask)
 {
 	struct subchannel *sch = to_subchannel(cdev->dev.parent);
-	struct channel_path_desc_fmt1 desc;
+	struct channel_path *chp;
 	struct chp_id chpid;
-	int mdc = 0, ret, i;
+	int mdc = 0, i;
 
 	/* Adjust requested path mask to excluded varied off paths. */
 	if (mask)
@@ -719,14 +719,20 @@ int ccw_device_get_mdc(struct ccw_device *cdev, u8 mask)
 		if (!(mask & (0x80 >> i)))
 			continue;
 		chpid.id = sch->schib.pmcw.chpid[i];
-		ret = chsc_determine_fmt1_channel_path_desc(chpid, &desc);
-		if (ret)
-			return ret;
-		if (!desc.f)
+		chp = chpid_to_chp(chpid);
+		if (!chp)
+			continue;
+
+		mutex_lock(&chp->lock);
+		if (!chp->desc_fmt1.f) {
+			mutex_unlock(&chp->lock);
 			return 0;
-		if (!desc.r)
+		}
+		if (!chp->desc_fmt1.r)
 			mdc = 1;
-		mdc = mdc ? min(mdc, (int)desc.mdc) : desc.mdc;
+		mdc = mdc ? min_t(int, mdc, chp->desc_fmt1.mdc) :
+			    chp->desc_fmt1.mdc;
+		mutex_unlock(&chp->lock);
 	}
 
 	return mdc;
diff --git a/drivers/s390/cio/idset.c b/drivers/s390/cio/idset.c
index 65d13e38803..5a999084a22 100644
--- a/drivers/s390/cio/idset.c
+++ b/drivers/s390/cio/idset.c
@@ -17,7 +17,7 @@ struct idset {
 
 static inline unsigned long bitmap_size(int num_ssid, int num_id)
 {
-	return __BITOPS_WORDS(num_ssid * num_id) * sizeof(unsigned long);
+	return BITS_TO_LONGS(num_ssid * num_id) * sizeof(unsigned long);
 }
 
 static struct idset *idset_new(int num_ssid, int num_id)
diff --git a/drivers/s390/net/qeth_core.h b/drivers/s390/net/qeth_core.h
index 8c0622399fc..6ccb7457746 100644
--- a/drivers/s390/net/qeth_core.h
+++ b/drivers/s390/net/qeth_core.h
@@ -769,6 +769,7 @@ struct qeth_card {
 	unsigned long thread_start_mask;
 	unsigned long thread_allowed_mask;
 	unsigned long thread_running_mask;
+	struct task_struct *recovery_task;
 	spinlock_t ip_lock;
 	struct list_head ip_list;
 	struct list_head *ip_tbd_list;
@@ -862,6 +863,8 @@ extern struct qeth_card_list_struct qeth_core_card_list;
 extern struct kmem_cache *qeth_core_header_cache;
 extern struct qeth_dbf_info qeth_dbf[QETH_DBF_INFOS];
 
+void qeth_set_recovery_task(struct qeth_card *);
+void qeth_clear_recovery_task(struct qeth_card *);
 void qeth_set_allowed_threads(struct qeth_card *, unsigned long , int);
 int qeth_threads_running(struct qeth_card *, unsigned long);
 int qeth_wait_for_threads(struct qeth_card *, unsigned long);
diff --git a/drivers/s390/net/qeth_core_main.c b/drivers/s390/net/qeth_core_main.c
index 0d73a999983..451f9202059 100644
--- a/drivers/s390/net/qeth_core_main.c
+++ b/drivers/s390/net/qeth_core_main.c
@@ -177,6 +177,23 @@ const char *qeth_get_cardname_short(struct qeth_card *card)
 	return "n/a";
 }
 
+void qeth_set_recovery_task(struct qeth_card *card)
+{
+	card->recovery_task = current;
+}
+EXPORT_SYMBOL_GPL(qeth_set_recovery_task);
+
+void qeth_clear_recovery_task(struct qeth_card *card)
+{
+	card->recovery_task = NULL;
+}
+EXPORT_SYMBOL_GPL(qeth_clear_recovery_task);
+
+static bool qeth_is_recovery_task(const struct qeth_card *card)
+{
+	return card->recovery_task == current;
+}
+
 void qeth_set_allowed_threads(struct qeth_card *card, unsigned long threads,
 			 int clear_start_mask)
 {
@@ -205,6 +222,8 @@ EXPORT_SYMBOL_GPL(qeth_threads_running);
 
 int qeth_wait_for_threads(struct qeth_card *card, unsigned long threads)
 {
+	if (qeth_is_recovery_task(card))
+		return 0;
 	return wait_event_interruptible(card->wait_q,
 			qeth_threads_running(card, threads) == 0);
 }
diff --git a/drivers/s390/net/qeth_l2_main.c b/drivers/s390/net/qeth_l2_main.c
index d690166efea..155b101bd73 100644
--- a/drivers/s390/net/qeth_l2_main.c
+++ b/drivers/s390/net/qeth_l2_main.c
@@ -1143,6 +1143,7 @@ static int qeth_l2_recover(void *ptr)
 	QETH_CARD_TEXT(card, 2, "recover2");
 	dev_warn(&card->gdev->dev,
 		"A recovery process has been started for the device\n");
+	qeth_set_recovery_task(card);
 	__qeth_l2_set_offline(card->gdev, 1);
 	rc = __qeth_l2_set_online(card->gdev, 1);
 	if (!rc)
@@ -1153,6 +1154,7 @@ static int qeth_l2_recover(void *ptr)
 		dev_warn(&card->gdev->dev, "The qeth device driver "
 				"failed to recover an error on the device\n");
 	}
+	qeth_clear_recovery_task(card);
 	qeth_clear_thread_start_bit(card, QETH_RECOVER_THREAD);
 	qeth_clear_thread_running_bit(card, QETH_RECOVER_THREAD);
 	return 0;
diff --git a/drivers/s390/net/qeth_l3_main.c b/drivers/s390/net/qeth_l3_main.c
index 8710337dab3..1f7edf1b26c 100644
--- a/drivers/s390/net/qeth_l3_main.c
+++ b/drivers/s390/net/qeth_l3_main.c
@@ -3515,6 +3515,7 @@ static int qeth_l3_recover(void *ptr)
 	QETH_CARD_TEXT(card, 2, "recover2");
 	dev_warn(&card->gdev->dev,
 		"A recovery process has been started for the device\n");
+	qeth_set_recovery_task(card);
 	__qeth_l3_set_offline(card->gdev, 1);
 	rc = __qeth_l3_set_online(card->gdev, 1);
 	if (!rc)
@@ -3525,6 +3526,7 @@ static int qeth_l3_recover(void *ptr)
 		dev_warn(&card->gdev->dev, "The qeth device driver "
 				"failed to recover an error on the device\n");
 	}
+	qeth_clear_recovery_task(card);
 	qeth_clear_thread_start_bit(card, QETH_RECOVER_THREAD);
 	qeth_clear_thread_running_bit(card, QETH_RECOVER_THREAD);
 	return 0;
diff --git a/drivers/sbus/char/bbc_i2c.c b/drivers/sbus/char/bbc_i2c.c
index 1a9d1e3ce64..c1441ed282e 100644
--- a/drivers/sbus/char/bbc_i2c.c
+++ b/drivers/sbus/char/bbc_i2c.c
@@ -282,7 +282,7 @@ static irqreturn_t bbc_i2c_interrupt(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static void __init reset_one_i2c(struct bbc_i2c_bus *bp)
+static void reset_one_i2c(struct bbc_i2c_bus *bp)
 {
 	writeb(I2C_PCF_PIN, bp->i2c_control_regs + 0x0);
 	writeb(bp->own, bp->i2c_control_regs + 0x1);
@@ -291,7 +291,7 @@ static void __init reset_one_i2c(struct bbc_i2c_bus *bp)
 	writeb(I2C_PCF_IDLE, bp->i2c_control_regs + 0x0);
 }
 
-static struct bbc_i2c_bus * __init attach_one_i2c(struct platform_device *op, int index)
+static struct bbc_i2c_bus * attach_one_i2c(struct platform_device *op, int index)
 {
 	struct bbc_i2c_bus *bp;
 	struct device_node *dp;
diff --git a/drivers/scsi/aacraid/aacraid.h b/drivers/scsi/aacraid/aacraid.h
index a6f7190c09a..9323d058706 100644
--- a/drivers/scsi/aacraid/aacraid.h
+++ b/drivers/scsi/aacraid/aacraid.h
@@ -12,7 +12,7 @@
  *----------------------------------------------------------------------------*/
 
 #ifndef AAC_DRIVER_BUILD
-# define AAC_DRIVER_BUILD 30000
+# define AAC_DRIVER_BUILD 30200
 # define AAC_DRIVER_BRANCH "-ms"
 #endif
 #define MAXIMUM_NUM_CONTAINERS	32
@@ -1918,6 +1918,10 @@ extern struct aac_common aac_config;
 #define	MONITOR_PANIC			0x00000020
 #define	KERNEL_UP_AND_RUNNING		0x00000080
 #define	KERNEL_PANIC			0x00000100
+#define	FLASH_UPD_PENDING		0x00002000
+#define	FLASH_UPD_SUCCESS		0x00004000
+#define	FLASH_UPD_FAILED		0x00008000
+#define	FWUPD_TIMEOUT			(5 * 60)
 
 /*
  *	Doorbell bit defines
diff --git a/drivers/scsi/aacraid/comminit.c b/drivers/scsi/aacraid/comminit.c
index 3f759957f4b..177b094c779 100644
--- a/drivers/scsi/aacraid/comminit.c
+++ b/drivers/scsi/aacraid/comminit.c
@@ -214,7 +214,7 @@ int aac_send_shutdown(struct aac_dev * dev)
 	cmd = (struct aac_close *) fib_data(fibctx);
 
 	cmd->command = cpu_to_le32(VM_CloseAll);
-	cmd->cid = cpu_to_le32(0xffffffff);
+	cmd->cid = cpu_to_le32(0xfffffffe);
 
 	status = aac_fib_send(ContainerCommand,
 			  fibctx,
diff --git a/drivers/scsi/aacraid/src.c b/drivers/scsi/aacraid/src.c
index e2e349204e7..0f56d8d7524 100644
--- a/drivers/scsi/aacraid/src.c
+++ b/drivers/scsi/aacraid/src.c
@@ -703,6 +703,28 @@ int aac_srcv_init(struct aac_dev *dev)
 		!aac_src_restart_adapter(dev, 0))
 		++restart;
 	/*
+	 *	Check to see if flash update is running.
+	 *	Wait for the adapter to be up and running. Wait up to 5 minutes
+	 */
+	status = src_readl(dev, MUnit.OMR);
+	if (status & FLASH_UPD_PENDING) {
+		start = jiffies;
+		do {
+			status = src_readl(dev, MUnit.OMR);
+			if (time_after(jiffies, start+HZ*FWUPD_TIMEOUT)) {
+				printk(KERN_ERR "%s%d: adapter flash update failed.\n",
+					dev->name, instance);
+				goto error_iounmap;
+			}
+		} while (!(status & FLASH_UPD_SUCCESS) &&
+			 !(status & FLASH_UPD_FAILED));
+		/* Delay 10 seconds.
+		 * Because right now FW is doing a soft reset,
+		 * do not read scratch pad register at this time
+		 */
+		ssleep(10);
+	}
+	/*
 	 *	Check to see if the board panic'd while booting.
 	 */
 	status = src_readl(dev, MUnit.OMR);
@@ -730,7 +752,9 @@ int aac_srcv_init(struct aac_dev *dev)
 	/*
 	 *	Wait for the adapter to be up and running. Wait up to 3 minutes
 	 */
-	while (!((status = src_readl(dev, MUnit.OMR)) & KERNEL_UP_AND_RUNNING)) {
+	while (!((status = src_readl(dev, MUnit.OMR)) &
+		KERNEL_UP_AND_RUNNING) ||
+		status == 0xffffffff) {
 		if ((restart &&
 		  (status & (KERNEL_PANIC|SELF_TEST_FAILED|MONITOR_PANIC))) ||
 		  time_after(jiffies, start+HZ*startup_timeout)) {
diff --git a/drivers/scsi/be2iscsi/be_iscsi.c b/drivers/scsi/be2iscsi/be_iscsi.c
index 214d691adb5..9014690fe84 100644
--- a/drivers/scsi/be2iscsi/be_iscsi.c
+++ b/drivers/scsi/be2iscsi/be_iscsi.c
@@ -369,7 +369,7 @@ beiscsi_set_vlan_tag(struct Scsi_Host *shost,
 		break;
 	default:
 		beiscsi_log(phba, KERN_WARNING, BEISCSI_LOG_CONFIG,
-			    "BS_%d : Unkown Param Type : %d\n",
+			    "BS_%d : Unknown Param Type : %d\n",
 			    iface_param->param);
 		return -ENOSYS;
 	}
diff --git a/drivers/scsi/be2iscsi/be_mgmt.c b/drivers/scsi/be2iscsi/be_mgmt.c
index a6c2fe4b4d6..55cc9902263 100644
--- a/drivers/scsi/be2iscsi/be_mgmt.c
+++ b/drivers/scsi/be2iscsi/be_mgmt.c
@@ -1292,7 +1292,7 @@ beiscsi_adap_family_disp(struct device *dev, struct device_attribute *attr,
 		break;
 	default:
 		return snprintf(buf, PAGE_SIZE,
-				"Unkown Adapter Family: 0x%x\n", dev_id);
+				"Unknown Adapter Family: 0x%x\n", dev_id);
 		break;
 	}
 }
diff --git a/drivers/scsi/bnx2fc/bnx2fc_fcoe.c b/drivers/scsi/bnx2fc/bnx2fc_fcoe.c
index 2daf4b0da43..90bc7bd0096 100644
--- a/drivers/scsi/bnx2fc/bnx2fc_fcoe.c
+++ b/drivers/scsi/bnx2fc/bnx2fc_fcoe.c
@@ -940,6 +940,7 @@ static int bnx2fc_libfc_config(struct fc_lport *lport)
 	fc_exch_init(lport);
 	fc_rport_init(lport);
 	fc_disc_init(lport);
+	fc_disc_config(lport, lport);
 	return 0;
 }
 
@@ -2133,6 +2134,7 @@ static int _bnx2fc_create(struct net_device *netdev,
 	}
 
 	ctlr = bnx2fc_to_ctlr(interface);
+	cdev = fcoe_ctlr_to_ctlr_dev(ctlr);
 	interface->vlan_id = vlan_id;
 
 	interface->timer_work_queue =
@@ -2143,7 +2145,7 @@ static int _bnx2fc_create(struct net_device *netdev,
 		goto ifput_err;
 	}
 
-	lport = bnx2fc_if_create(interface, &interface->hba->pcidev->dev, 0);
+	lport = bnx2fc_if_create(interface, &cdev->dev, 0);
 	if (!lport) {
 		printk(KERN_ERR PFX "Failed to create interface (%s)\n",
 			netdev->name);
@@ -2159,8 +2161,6 @@ static int _bnx2fc_create(struct net_device *netdev,
 	/* Make this master N_port */
 	ctlr->lp = lport;
 
-	cdev = fcoe_ctlr_to_ctlr_dev(ctlr);
-
 	if (link_state == BNX2FC_CREATE_LINK_UP)
 		cdev->enabled = FCOE_CTLR_ENABLED;
 	else
diff --git a/drivers/scsi/bnx2i/bnx2i.h b/drivers/scsi/bnx2i/bnx2i.h
index b44d04e41b0..f109e3b073c 100644
--- a/drivers/scsi/bnx2i/bnx2i.h
+++ b/drivers/scsi/bnx2i/bnx2i.h
@@ -580,8 +580,8 @@ struct bnx2i_5771x_dbell {
  * @sq_mem_size:        SQ size
  * @sq_prod_qe:         SQ producer entry pointer
  * @sq_cons_qe:         SQ consumer entry pointer
- * @sq_first_qe:        virtaul address of first entry in SQ
- * @sq_last_qe:         virtaul address of last entry in SQ
+ * @sq_first_qe:        virtual address of first entry in SQ
+ * @sq_last_qe:         virtual address of last entry in SQ
  * @sq_prod_idx:        SQ producer index
  * @sq_cons_idx:        SQ consumer index
  * @sqe_left:           number sq entry left
@@ -593,8 +593,8 @@ struct bnx2i_5771x_dbell {
  * @cq_mem_size:        CQ size
  * @cq_prod_qe:         CQ producer entry pointer
  * @cq_cons_qe:         CQ consumer entry pointer
- * @cq_first_qe:        virtaul address of first entry in CQ
- * @cq_last_qe:         virtaul address of last entry in CQ
+ * @cq_first_qe:        virtual address of first entry in CQ
+ * @cq_last_qe:         virtual address of last entry in CQ
  * @cq_prod_idx:        CQ producer index
  * @cq_cons_idx:        CQ consumer index
  * @cqe_left:           number cq entry left
@@ -608,8 +608,8 @@ struct bnx2i_5771x_dbell {
  * @rq_mem_size:        RQ size
  * @rq_prod_qe:         RQ producer entry pointer
  * @rq_cons_qe:         RQ consumer entry pointer
- * @rq_first_qe:        virtaul address of first entry in RQ
- * @rq_last_qe:         virtaul address of last entry in RQ
+ * @rq_first_qe:        virtual address of first entry in RQ
+ * @rq_last_qe:         virtual address of last entry in RQ
  * @rq_prod_idx:        RQ producer index
  * @rq_cons_idx:        RQ consumer index
  * @rqe_left:           number rq entry left
diff --git a/drivers/scsi/csiostor/csio_hw.c b/drivers/scsi/csiostor/csio_hw.c
index bdd78fb4fc7..7dbaf58fab9 100644
--- a/drivers/scsi/csiostor/csio_hw.c
+++ b/drivers/scsi/csiostor/csio_hw.c
@@ -3892,7 +3892,6 @@ csio_process_fwevtq_entry(struct csio_hw *hw, void *wr, uint32_t len,
 			  struct csio_fl_dma_buf *flb, void *priv)
 {
 	__u8 op;
-	__be64 *data;
 	void *msg = NULL;
 	uint32_t msg_len = 0;
 	bool msg_sg = 0;
@@ -3908,8 +3907,6 @@ csio_process_fwevtq_entry(struct csio_hw *hw, void *wr, uint32_t len,
 		msg = (void *) flb;
 		msg_len = flb->totlen;
 		msg_sg = 1;
-
-		data = (__be64 *) msg;
 	} else if (op == CPL_FW6_MSG || op == CPL_FW4_MSG) {
 
 		CSIO_INC_STATS(hw, n_cpl_fw6_msg);
@@ -3917,8 +3914,6 @@ csio_process_fwevtq_entry(struct csio_hw *hw, void *wr, uint32_t len,
 		msg = (void *)((uintptr_t)wr + sizeof(__be64));
 		msg_len = (op == CPL_FW6_MSG) ? sizeof(struct cpl_fw6_msg) :
 			   sizeof(struct cpl_fw4_msg);
-
-		data = (__be64 *) msg;
 	} else {
 		csio_warn(hw, "unexpected CPL %#x on FW event queue\n", op);
 		CSIO_INC_STATS(hw, n_cpl_unexp);
diff --git a/drivers/scsi/csiostor/csio_mb.c b/drivers/scsi/csiostor/csio_mb.c
index 5b27c48f683..f5d9ee1fda6 100644
--- a/drivers/scsi/csiostor/csio_mb.c
+++ b/drivers/scsi/csiostor/csio_mb.c
@@ -182,7 +182,7 @@ csio_mb_reset(struct csio_hw *hw, struct csio_mb *mbp, uint32_t tmo,
  * @tmo: Command timeout.
  * @pf: PF number.
  * @vf: VF number.
- * @nparams: Number of paramters
+ * @nparams: Number of parameters
  * @params: Parameter mnemonic array.
  * @val: Parameter value array.
  * @wr: Write/Read PARAMS.
@@ -721,7 +721,7 @@ csio_mb_iq_free(struct csio_hw *hw, struct csio_mb *mbp, void *priv,
  * @mbp: Mailbox structure to initialize
  * @priv: Private data
  * @mb_tmo: Mailbox time-out period (in ms).
- * @eq_ofld_params: (Offload) Egress queue paramters.
+ * @eq_ofld_params: (Offload) Egress queue parameters.
  * @cbfn: The call-back function
  *
  *
@@ -752,7 +752,7 @@ csio_mb_eq_ofld_alloc(struct csio_hw *hw, struct csio_mb *mbp, void *priv,
  * @priv: Private data
  * @mb_tmo: Mailbox time-out period (in ms).
  * @cascaded_req: TRUE - if this request is cascased with Eq-alloc request.
- * @eq_ofld_params: (Offload) Egress queue paramters.
+ * @eq_ofld_params: (Offload) Egress queue parameters.
  * @cbfn: The call-back function
  *
  *
@@ -817,7 +817,7 @@ csio_mb_eq_ofld_write(struct csio_hw *hw, struct csio_mb *mbp, void *priv,
  * @mbp: Mailbox structure to initialize
  * @priv: Private data.
  * @mb_tmo: Mailbox time-out period (in ms).
- * @eq_ofld_params: (Offload) Egress queue paramters.
+ * @eq_ofld_params: (Offload) Egress queue parameters.
  * @cbfn: The call-back function
  *
  *
@@ -840,7 +840,7 @@ csio_mb_eq_ofld_alloc_write(struct csio_hw *hw, struct csio_mb *mbp,
  * @hw: The HW structure.
  * @mbp: Mailbox structure to initialize.
  * @retval: Firmware return value.
- * @eq_ofld_params: (Offload) Egress queue paramters.
+ * @eq_ofld_params: (Offload) Egress queue parameters.
  *
  */
 void
@@ -870,7 +870,7 @@ csio_mb_eq_ofld_alloc_write_rsp(struct csio_hw *hw,
  * @mbp: Mailbox structure to initialize
  * @priv: Private data area.
  * @mb_tmo: Mailbox time-out period (in ms).
- * @eq_ofld_params: (Offload) Egress queue paramters, that is to be freed.
+ * @eq_ofld_params: (Offload) Egress queue parameters, that is to be freed.
  * @cbfn: The call-back function
  *
  *
diff --git a/drivers/scsi/device_handler/scsi_dh_alua.c b/drivers/scsi/device_handler/scsi_dh_alua.c
index 6f4d8e6f32f..68adb8955d2 100644
--- a/drivers/scsi/device_handler/scsi_dh_alua.c
+++ b/drivers/scsi/device_handler/scsi_dh_alua.c
@@ -232,13 +232,13 @@ static void stpg_endio(struct request *req, int error)
 	struct scsi_sense_hdr sense_hdr;
 	unsigned err = SCSI_DH_OK;
 
-	if (error || host_byte(req->errors) != DID_OK ||
-			msg_byte(req->errors) != COMMAND_COMPLETE) {
+	if (host_byte(req->errors) != DID_OK ||
+	    msg_byte(req->errors) != COMMAND_COMPLETE) {
 		err = SCSI_DH_IO;
 		goto done;
 	}
 
-	if (h->senselen > 0) {
+	if (req->sense_len > 0) {
 		err = scsi_normalize_sense(h->sense, SCSI_SENSE_BUFFERSIZE,
 					   &sense_hdr);
 		if (!err) {
@@ -255,7 +255,9 @@ static void stpg_endio(struct request *req, int error)
 			    ALUA_DH_NAME, sense_hdr.sense_key,
 			    sense_hdr.asc, sense_hdr.ascq);
 		err = SCSI_DH_IO;
-	}
+	} else if (error)
+		err = SCSI_DH_IO;
+
 	if (err == SCSI_DH_OK) {
 		h->state = TPGS_STATE_OPTIMIZED;
 		sdev_printk(KERN_INFO, h->sdev,
@@ -710,6 +712,10 @@ static int alua_set_params(struct scsi_device *sdev, const char *params)
 	return result;
 }
 
+static uint optimize_stpg;
+module_param(optimize_stpg, uint, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(optimize_stpg, "Allow use of a non-optimized path, rather than sending a STPG, when implicit TPGS is supported (0=No,1=Yes). Default is 0.");
+
 /*
  * alua_activate - activate a path
  * @sdev: device on the path to be activated
@@ -731,6 +737,9 @@ static int alua_activate(struct scsi_device *sdev,
 	if (err != SCSI_DH_OK)
 		goto out;
 
+	if (optimize_stpg)
+		h->flags |= ALUA_OPTIMIZE_STPG;
+
 	if (h->tpgs & TPGS_MODE_EXPLICIT) {
 		switch (h->state) {
 		case TPGS_STATE_NONOPTIMIZED:
diff --git a/drivers/scsi/fcoe/fcoe.c b/drivers/scsi/fcoe/fcoe.c
index b5d92fc93c7..9bfdc9a3f89 100644
--- a/drivers/scsi/fcoe/fcoe.c
+++ b/drivers/scsi/fcoe/fcoe.c
@@ -490,7 +490,6 @@ static void fcoe_interface_cleanup(struct fcoe_interface *fcoe)
 {
 	struct net_device *netdev = fcoe->netdev;
 	struct fcoe_ctlr *fip = fcoe_to_ctlr(fcoe);
-	struct fcoe_ctlr_device *ctlr_dev = fcoe_ctlr_to_ctlr_dev(fip);
 
 	rtnl_lock();
 	if (!fcoe->removed)
@@ -501,7 +500,6 @@ static void fcoe_interface_cleanup(struct fcoe_interface *fcoe)
 	/* tear-down the FCoE controller */
 	fcoe_ctlr_destroy(fip);
 	scsi_host_put(fip->lp->host);
-	fcoe_ctlr_device_delete(ctlr_dev);
 	dev_put(netdev);
 	module_put(THIS_MODULE);
 }
@@ -2194,6 +2192,8 @@ out_nodev:
  */
 static void fcoe_destroy_work(struct work_struct *work)
 {
+	struct fcoe_ctlr_device *cdev;
+	struct fcoe_ctlr *ctlr;
 	struct fcoe_port *port;
 	struct fcoe_interface *fcoe;
 	struct Scsi_Host *shost;
@@ -2224,10 +2224,15 @@ static void fcoe_destroy_work(struct work_struct *work)
 	mutex_lock(&fcoe_config_mutex);
 
 	fcoe = port->priv;
+	ctlr = fcoe_to_ctlr(fcoe);
+	cdev = fcoe_ctlr_to_ctlr_dev(ctlr);
+
 	fcoe_if_destroy(port->lport);
 	fcoe_interface_cleanup(fcoe);
 
 	mutex_unlock(&fcoe_config_mutex);
+
+	fcoe_ctlr_device_delete(cdev);
 }
 
 /**
@@ -2335,7 +2340,9 @@ static int _fcoe_create(struct net_device *netdev, enum fip_state fip_mode,
 		rc = -EIO;
 		rtnl_unlock();
 		fcoe_interface_cleanup(fcoe);
-		goto out_nortnl;
+		mutex_unlock(&fcoe_config_mutex);
+		fcoe_ctlr_device_delete(ctlr_dev);
+		goto out;
 	}
 
 	/* Make this the "master" N_Port */
@@ -2375,8 +2382,8 @@ static int _fcoe_create(struct net_device *netdev, enum fip_state fip_mode,
 
 out_nodev:
 	rtnl_unlock();
-out_nortnl:
 	mutex_unlock(&fcoe_config_mutex);
+out:
 	return rc;
 }
 
diff --git a/drivers/scsi/fcoe/fcoe_ctlr.c b/drivers/scsi/fcoe/fcoe_ctlr.c
index 08c3bc398da..cd743c545ce 100644
--- a/drivers/scsi/fcoe/fcoe_ctlr.c
+++ b/drivers/scsi/fcoe/fcoe_ctlr.c
@@ -2161,7 +2161,7 @@ static void fcoe_ctlr_vn_restart(struct fcoe_ctlr *fip)
 
 	if (fip->probe_tries < FIP_VN_RLIM_COUNT) {
 		fip->probe_tries++;
-		wait = random32() % FIP_VN_PROBE_WAIT;
+		wait = prandom_u32() % FIP_VN_PROBE_WAIT;
 	} else
 		wait = FIP_VN_RLIM_INT;
 	mod_timer(&fip->timer, jiffies + msecs_to_jiffies(wait));
@@ -2794,7 +2794,7 @@ static void fcoe_ctlr_vn_timeout(struct fcoe_ctlr *fip)
 					  fcoe_all_vn2vn, 0);
 			fip->port_ka_time = jiffies +
 				 msecs_to_jiffies(FIP_VN_BEACON_INT +
-					(random32() % FIP_VN_BEACON_FUZZ));
+					(prandom_u32() % FIP_VN_BEACON_FUZZ));
 		}
 		if (time_before(fip->port_ka_time, next_time))
 			next_time = fip->port_ka_time;
@@ -2815,6 +2815,47 @@ unlock:
 }
 
 /**
+ * fcoe_ctlr_mode_set() - Set or reset the ctlr's mode
+ * @lport: The local port to be (re)configured
+ * @fip:   The FCoE controller whose mode is changing
+ * @fip_mode: The new fip mode
+ *
+ * Note that the we shouldn't be changing the libfc discovery settings
+ * (fc_disc_config) while an lport is going through the libfc state
+ * machine. The mode can only be changed when a fcoe_ctlr device is
+ * disabled, so that should ensure that this routine is only called
+ * when nothing is happening.
+ */
+void fcoe_ctlr_mode_set(struct fc_lport *lport, struct fcoe_ctlr *fip,
+			enum fip_state fip_mode)
+{
+	void *priv;
+
+	WARN_ON(lport->state != LPORT_ST_RESET &&
+		lport->state != LPORT_ST_DISABLED);
+
+	if (fip_mode == FIP_MODE_VN2VN) {
+		lport->rport_priv_size = sizeof(struct fcoe_rport);
+		lport->point_to_multipoint = 1;
+		lport->tt.disc_recv_req = fcoe_ctlr_disc_recv;
+		lport->tt.disc_start = fcoe_ctlr_disc_start;
+		lport->tt.disc_stop = fcoe_ctlr_disc_stop;
+		lport->tt.disc_stop_final = fcoe_ctlr_disc_stop_final;
+		priv = fip;
+	} else {
+		lport->rport_priv_size = 0;
+		lport->point_to_multipoint = 0;
+		lport->tt.disc_recv_req = NULL;
+		lport->tt.disc_start = NULL;
+		lport->tt.disc_stop = NULL;
+		lport->tt.disc_stop_final = NULL;
+		priv = lport;
+	}
+
+	fc_disc_config(lport, priv);
+}
+
+/**
  * fcoe_libfc_config() - Sets up libfc related properties for local port
  * @lport:    The local port to configure libfc for
  * @fip:      The FCoE controller in use by the local port
@@ -2833,21 +2874,9 @@ int fcoe_libfc_config(struct fc_lport *lport, struct fcoe_ctlr *fip,
 	fc_exch_init(lport);
 	fc_elsct_init(lport);
 	fc_lport_init(lport);
-	if (fip->mode == FIP_MODE_VN2VN)
-		lport->rport_priv_size = sizeof(struct fcoe_rport);
 	fc_rport_init(lport);
-	if (fip->mode == FIP_MODE_VN2VN) {
-		lport->point_to_multipoint = 1;
-		lport->tt.disc_recv_req = fcoe_ctlr_disc_recv;
-		lport->tt.disc_start = fcoe_ctlr_disc_start;
-		lport->tt.disc_stop = fcoe_ctlr_disc_stop;
-		lport->tt.disc_stop_final = fcoe_ctlr_disc_stop_final;
-		mutex_init(&lport->disc.disc_mutex);
-		INIT_LIST_HEAD(&lport->disc.rports);
-		lport->disc.priv = fip;
-	} else {
-		fc_disc_init(lport);
-	}
+	fc_disc_init(lport);
+	fcoe_ctlr_mode_set(lport, fip, fip->mode);
 	return 0;
 }
 EXPORT_SYMBOL_GPL(fcoe_libfc_config);
@@ -2875,6 +2904,7 @@ EXPORT_SYMBOL(fcoe_fcf_get_selected);
 void fcoe_ctlr_set_fip_mode(struct fcoe_ctlr_device *ctlr_dev)
 {
 	struct fcoe_ctlr *ctlr = fcoe_ctlr_device_priv(ctlr_dev);
+	struct fc_lport *lport = ctlr->lp;
 
 	mutex_lock(&ctlr->ctlr_mutex);
 	switch (ctlr_dev->mode) {
@@ -2888,5 +2918,7 @@ void fcoe_ctlr_set_fip_mode(struct fcoe_ctlr_device *ctlr_dev)
 	}
 
 	mutex_unlock(&ctlr->ctlr_mutex);
+
+	fcoe_ctlr_mode_set(lport, ctlr, ctlr->mode);
 }
 EXPORT_SYMBOL(fcoe_ctlr_set_fip_mode);
diff --git a/drivers/scsi/ibmvscsi/ibmvscsi.c b/drivers/scsi/ibmvscsi/ibmvscsi.c
index a044f593e8b..d0fa4b6c551 100644
--- a/drivers/scsi/ibmvscsi/ibmvscsi.c
+++ b/drivers/scsi/ibmvscsi/ibmvscsi.c
@@ -1899,8 +1899,8 @@ static int ibmvscsi_slave_configure(struct scsi_device *sdev)
 		sdev->allow_restart = 1;
 		blk_queue_rq_timeout(sdev->request_queue, 120 * HZ);
 	}
-	scsi_adjust_queue_depth(sdev, 0, shost->cmd_per_lun);
 	spin_unlock_irqrestore(shost->host_lock, lock_flags);
+	scsi_adjust_queue_depth(sdev, 0, shost->cmd_per_lun);
 	return 0;
 }
 
diff --git a/drivers/scsi/ipr.c b/drivers/scsi/ipr.c
index f328089a106..2197b57fb22 100644
--- a/drivers/scsi/ipr.c
+++ b/drivers/scsi/ipr.c
@@ -5148,7 +5148,7 @@ static int ipr_cancel_op(struct scsi_cmnd *scsi_cmd)
 		ipr_trace;
 	}
 
-	list_add_tail(&ipr_cmd->queue, &hrrq->hrrq_free_q);
+	list_add_tail(&ipr_cmd->queue, &ipr_cmd->hrrq->hrrq_free_q);
 	if (!ipr_is_naca_model(res))
 		res->needs_sync_complete = 1;
 
@@ -9349,7 +9349,10 @@ static int ipr_test_msi(struct ipr_ioa_cfg *ioa_cfg, struct pci_dev *pdev)
 	int_reg = readl(ioa_cfg->regs.sense_interrupt_mask_reg);
 	spin_unlock_irqrestore(ioa_cfg->host->host_lock, lock_flags);
 
-	rc = request_irq(pdev->irq, ipr_test_intr, 0, IPR_NAME, ioa_cfg);
+	if (ioa_cfg->intr_flag == IPR_USE_MSIX)
+		rc = request_irq(ioa_cfg->vectors_info[0].vec, ipr_test_intr, 0, IPR_NAME, ioa_cfg);
+	else
+		rc = request_irq(pdev->irq, ipr_test_intr, 0, IPR_NAME, ioa_cfg);
 	if (rc) {
 		dev_err(&pdev->dev, "Can not assign irq %d\n", pdev->irq);
 		return rc;
@@ -9371,7 +9374,10 @@ static int ipr_test_msi(struct ipr_ioa_cfg *ioa_cfg, struct pci_dev *pdev)
 
 	spin_unlock_irqrestore(ioa_cfg->host->host_lock, lock_flags);
 
-	free_irq(pdev->irq, ioa_cfg);
+	if (ioa_cfg->intr_flag == IPR_USE_MSIX)
+		free_irq(ioa_cfg->vectors_info[0].vec, ioa_cfg);
+	else
+		free_irq(pdev->irq, ioa_cfg);
 
 	LEAVE;
 
@@ -9722,6 +9728,7 @@ static void __ipr_remove(struct pci_dev *pdev)
 	spin_unlock_irqrestore(ioa_cfg->host->host_lock, host_lock_flags);
 	wait_event(ioa_cfg->reset_wait_q, !ioa_cfg->in_reset_reload);
 	flush_work(&ioa_cfg->work_q);
+	INIT_LIST_HEAD(&ioa_cfg->used_res_q);
 	spin_lock_irqsave(ioa_cfg->host->host_lock, host_lock_flags);
 
 	spin_lock(&ipr_driver_lock);
diff --git a/drivers/scsi/isci/init.c b/drivers/scsi/isci/init.c
index 2839baa82a5..d25d0d859f0 100644
--- a/drivers/scsi/isci/init.c
+++ b/drivers/scsi/isci/init.c
@@ -721,7 +721,7 @@ static void isci_pci_remove(struct pci_dev *pdev)
 	}
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int isci_suspend(struct device *dev)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
@@ -770,18 +770,16 @@ static int isci_resume(struct device *dev)
 
 	return 0;
 }
+#endif
 
 static SIMPLE_DEV_PM_OPS(isci_pm_ops, isci_suspend, isci_resume);
-#endif
 
 static struct pci_driver isci_pci_driver = {
 	.name		= DRV_NAME,
 	.id_table	= isci_id_table,
 	.probe		= isci_pci_probe,
 	.remove		= isci_pci_remove,
-#ifdef CONFIG_PM
 	.driver.pm      = &isci_pm_ops,
-#endif
 };
 
 static __init int isci_init(void)
diff --git a/drivers/scsi/iscsi_tcp.c b/drivers/scsi/iscsi_tcp.c
index 1b91ca0dc1e..9e2588a6881 100644
--- a/drivers/scsi/iscsi_tcp.c
+++ b/drivers/scsi/iscsi_tcp.c
@@ -370,17 +370,24 @@ static inline int iscsi_sw_tcp_xmit_qlen(struct iscsi_conn *conn)
 static int iscsi_sw_tcp_pdu_xmit(struct iscsi_task *task)
 {
 	struct iscsi_conn *conn = task->conn;
-	int rc;
+	unsigned long pflags = current->flags;
+	int rc = 0;
+
+	current->flags |= PF_MEMALLOC;
 
 	while (iscsi_sw_tcp_xmit_qlen(conn)) {
 		rc = iscsi_sw_tcp_xmit(conn);
-		if (rc == 0)
-			return -EAGAIN;
+		if (rc == 0) {
+			rc = -EAGAIN;
+			break;
+		}
 		if (rc < 0)
-			return rc;
+			break;
+		rc = 0;
 	}
 
-	return 0;
+	tsk_restore_flags(current, pflags, PF_MEMALLOC);
+	return rc;
 }
 
 /*
@@ -665,6 +672,7 @@ iscsi_sw_tcp_conn_bind(struct iscsi_cls_session *cls_session,
 	sk->sk_reuse = SK_CAN_REUSE;
 	sk->sk_sndtimeo = 15 * HZ; /* FIXME: make it configurable */
 	sk->sk_allocation = GFP_ATOMIC;
+	sk_set_memalloc(sk);
 
 	iscsi_sw_tcp_conn_set_callbacks(conn);
 	tcp_sw_conn->sendpage = tcp_sw_conn->sock->ops->sendpage;
diff --git a/drivers/scsi/libfc/fc_disc.c b/drivers/scsi/libfc/fc_disc.c
index 8e561e6a557..880a9068ca1 100644
--- a/drivers/scsi/libfc/fc_disc.c
+++ b/drivers/scsi/libfc/fc_disc.c
@@ -712,12 +712,13 @@ static void fc_disc_stop_final(struct fc_lport *lport)
 }
 
 /**
- * fc_disc_init() - Initialize the discovery layer for a local port
- * @lport: The local port that needs the discovery layer to be initialized
+ * fc_disc_config() - Configure the discovery layer for a local port
+ * @lport: The local port that needs the discovery layer to be configured
+ * @priv: Private data structre for users of the discovery layer
  */
-int fc_disc_init(struct fc_lport *lport)
+void fc_disc_config(struct fc_lport *lport, void *priv)
 {
-	struct fc_disc *disc;
+	struct fc_disc *disc = &lport->disc;
 
 	if (!lport->tt.disc_start)
 		lport->tt.disc_start = fc_disc_start;
@@ -732,12 +733,21 @@ int fc_disc_init(struct fc_lport *lport)
 		lport->tt.disc_recv_req = fc_disc_recv_req;
 
 	disc = &lport->disc;
+
+	disc->priv = priv;
+}
+EXPORT_SYMBOL(fc_disc_config);
+
+/**
+ * fc_disc_init() - Initialize the discovery layer for a local port
+ * @lport: The local port that needs the discovery layer to be initialized
+ */
+void fc_disc_init(struct fc_lport *lport)
+{
+	struct fc_disc *disc = &lport->disc;
+
 	INIT_DELAYED_WORK(&disc->disc_work, fc_disc_timeout);
 	mutex_init(&disc->disc_mutex);
 	INIT_LIST_HEAD(&disc->rports);
-
-	disc->priv = lport;
-
-	return 0;
 }
 EXPORT_SYMBOL(fc_disc_init);
diff --git a/drivers/scsi/libiscsi.c b/drivers/scsi/libiscsi.c
index 82c3fd4bc93..5de94698450 100644
--- a/drivers/scsi/libiscsi.c
+++ b/drivers/scsi/libiscsi.c
@@ -507,7 +507,6 @@ static void iscsi_free_task(struct iscsi_task *task)
 	kfifo_in(&session->cmdpool.queue, (void*)&task, sizeof(void*));
 
 	if (sc) {
-		task->sc = NULL;
 		/* SCSI eh reuses commands to verify us */
 		sc->SCp.ptr = NULL;
 		/*
@@ -3142,7 +3141,7 @@ int iscsi_conn_bind(struct iscsi_cls_session *cls_session,
 }
 EXPORT_SYMBOL_GPL(iscsi_conn_bind);
 
-static int iscsi_switch_str_param(char **param, char *new_val_buf)
+int iscsi_switch_str_param(char **param, char *new_val_buf)
 {
 	char *new_val;
 
@@ -3159,6 +3158,7 @@ static int iscsi_switch_str_param(char **param, char *new_val_buf)
 	*param = new_val;
 	return 0;
 }
+EXPORT_SYMBOL_GPL(iscsi_switch_str_param);
 
 int iscsi_set_param(struct iscsi_cls_conn *cls_conn,
 		    enum iscsi_param param, char *buf, int buflen)
diff --git a/drivers/scsi/libsas/sas_expander.c b/drivers/scsi/libsas/sas_expander.c
index aec2e0da501..55cbd018015 100644
--- a/drivers/scsi/libsas/sas_expander.c
+++ b/drivers/scsi/libsas/sas_expander.c
@@ -235,6 +235,17 @@ static void sas_set_ex_phy(struct domain_device *dev, int phy_id, void *rsp)
 	linkrate  = phy->linkrate;
 	memcpy(sas_addr, phy->attached_sas_addr, SAS_ADDR_SIZE);
 
+	/* Handle vacant phy - rest of dr data is not valid so skip it */
+	if (phy->phy_state == PHY_VACANT) {
+		memset(phy->attached_sas_addr, 0, SAS_ADDR_SIZE);
+		phy->attached_dev_type = NO_DEVICE;
+		if (!test_bit(SAS_HA_ATA_EH_ACTIVE, &ha->state)) {
+			phy->phy_id = phy_id;
+			goto skip;
+		} else
+			goto out;
+	}
+
 	phy->attached_dev_type = to_dev_type(dr);
 	if (test_bit(SAS_HA_ATA_EH_ACTIVE, &ha->state))
 		goto out;
@@ -272,6 +283,7 @@ static void sas_set_ex_phy(struct domain_device *dev, int phy_id, void *rsp)
 	phy->phy->maximum_linkrate = dr->pmax_linkrate;
 	phy->phy->negotiated_linkrate = phy->linkrate;
 
+ skip:
 	if (new_phy)
 		if (sas_phy_add(phy->phy)) {
 			sas_phy_free(phy->phy);
@@ -388,7 +400,7 @@ int sas_ex_phy_discover(struct domain_device *dev, int single)
 	if (!disc_req)
 		return -ENOMEM;
 
-	disc_resp = alloc_smp_req(DISCOVER_RESP_SIZE);
+	disc_resp = alloc_smp_resp(DISCOVER_RESP_SIZE);
 	if (!disc_resp) {
 		kfree(disc_req);
 		return -ENOMEM;
diff --git a/drivers/scsi/lpfc/lpfc_attr.c b/drivers/scsi/lpfc/lpfc_attr.c
index a364cae9e98..9290713af25 100644
--- a/drivers/scsi/lpfc/lpfc_attr.c
+++ b/drivers/scsi/lpfc/lpfc_attr.c
@@ -692,7 +692,7 @@ lpfc_do_offline(struct lpfc_hba *phba, uint32_t type)
 	 */
 	for (i = 0; i < psli->num_rings; i++) {
 		pring = &psli->ring[i];
-		while (pring->txcmplq_cnt) {
+		while (!list_empty(&pring->txcmplq)) {
 			msleep(10);
 			if (cnt++ > 500) {  /* 5 secs */
 				lpfc_printf_log(phba,
@@ -2302,11 +2302,17 @@ static DEVICE_ATTR(lpfc_enable_npiv, S_IRUGO, lpfc_enable_npiv_show, NULL);
 LPFC_ATTR_R(fcf_failover_policy, 1, 1, 2,
 	"FCF Fast failover=1 Priority failover=2");
 
-int lpfc_enable_rrq;
+int lpfc_enable_rrq = 2;
 module_param(lpfc_enable_rrq, int, S_IRUGO);
 MODULE_PARM_DESC(lpfc_enable_rrq, "Enable RRQ functionality");
 lpfc_param_show(enable_rrq);
-lpfc_param_init(enable_rrq, 0, 0, 1);
+/*
+# lpfc_enable_rrq: Track XRI/OXID reuse after IO failures
+#	0x0 = disabled, XRI/OXID use not tracked.
+#	0x1 = XRI/OXID reuse is timed with ratov, RRQ sent.
+#	0x2 = XRI/OXID reuse is timed with ratov, No RRQ sent.
+*/
+lpfc_param_init(enable_rrq, 2, 0, 2);
 static DEVICE_ATTR(lpfc_enable_rrq, S_IRUGO, lpfc_enable_rrq_show, NULL);
 
 /*
diff --git a/drivers/scsi/lpfc/lpfc_bsg.c b/drivers/scsi/lpfc/lpfc_bsg.c
index 32d5683e618..88866689200 100644
--- a/drivers/scsi/lpfc/lpfc_bsg.c
+++ b/drivers/scsi/lpfc/lpfc_bsg.c
@@ -64,18 +64,14 @@ struct lpfc_bsg_event {
 	struct list_head events_to_get;
 	struct list_head events_to_see;
 
-	/* job waiting for this event to finish */
-	struct fc_bsg_job *set_job;
+	/* driver data associated with the job */
+	void *dd_data;
 };
 
 struct lpfc_bsg_iocb {
 	struct lpfc_iocbq *cmdiocbq;
-	struct lpfc_iocbq *rspiocbq;
-	struct lpfc_dmabuf *bmp;
+	struct lpfc_dmabuf *rmp;
 	struct lpfc_nodelist *ndlp;
-
-	/* job waiting for this iocb to finish */
-	struct fc_bsg_job *set_job;
 };
 
 struct lpfc_bsg_mbox {
@@ -86,20 +82,13 @@ struct lpfc_bsg_mbox {
 	uint32_t mbOffset; /* from app */
 	uint32_t inExtWLen; /* from app */
 	uint32_t outExtWLen; /* from app */
-
-	/* job waiting for this mbox command to finish */
-	struct fc_bsg_job *set_job;
 };
 
 #define MENLO_DID 0x0000FC0E
 
 struct lpfc_bsg_menlo {
 	struct lpfc_iocbq *cmdiocbq;
-	struct lpfc_iocbq *rspiocbq;
-	struct lpfc_dmabuf *bmp;
-
-	/* job waiting for this iocb to finish */
-	struct fc_bsg_job *set_job;
+	struct lpfc_dmabuf *rmp;
 };
 
 #define TYPE_EVT 	1
@@ -108,6 +97,7 @@ struct lpfc_bsg_menlo {
 #define TYPE_MENLO	4
 struct bsg_job_data {
 	uint32_t type;
+	struct fc_bsg_job *set_job; /* job waiting for this iocb to finish */
 	union {
 		struct lpfc_bsg_event *evt;
 		struct lpfc_bsg_iocb iocb;
@@ -141,6 +131,138 @@ struct lpfc_dmabufext {
 	uint32_t flag;
 };
 
+static void
+lpfc_free_bsg_buffers(struct lpfc_hba *phba, struct lpfc_dmabuf *mlist)
+{
+	struct lpfc_dmabuf *mlast, *next_mlast;
+
+	if (mlist) {
+		list_for_each_entry_safe(mlast, next_mlast, &mlist->list,
+					 list) {
+			lpfc_mbuf_free(phba, mlast->virt, mlast->phys);
+			list_del(&mlast->list);
+			kfree(mlast);
+		}
+		lpfc_mbuf_free(phba, mlist->virt, mlist->phys);
+		kfree(mlist);
+	}
+	return;
+}
+
+static struct lpfc_dmabuf *
+lpfc_alloc_bsg_buffers(struct lpfc_hba *phba, unsigned int size,
+		       int outbound_buffers, struct ulp_bde64 *bpl,
+		       int *bpl_entries)
+{
+	struct lpfc_dmabuf *mlist = NULL;
+	struct lpfc_dmabuf *mp;
+	unsigned int bytes_left = size;
+
+	/* Verify we can support the size specified */
+	if (!size || (size > (*bpl_entries * LPFC_BPL_SIZE)))
+		return NULL;
+
+	/* Determine the number of dma buffers to allocate */
+	*bpl_entries = (size % LPFC_BPL_SIZE ? size/LPFC_BPL_SIZE + 1 :
+			size/LPFC_BPL_SIZE);
+
+	/* Allocate dma buffer and place in BPL passed */
+	while (bytes_left) {
+		/* Allocate dma buffer  */
+		mp = kmalloc(sizeof(struct lpfc_dmabuf), GFP_KERNEL);
+		if (!mp) {
+			if (mlist)
+				lpfc_free_bsg_buffers(phba, mlist);
+			return NULL;
+		}
+
+		INIT_LIST_HEAD(&mp->list);
+		mp->virt = lpfc_mbuf_alloc(phba, MEM_PRI, &(mp->phys));
+
+		if (!mp->virt) {
+			kfree(mp);
+			if (mlist)
+				lpfc_free_bsg_buffers(phba, mlist);
+			return NULL;
+		}
+
+		/* Queue it to a linked list */
+		if (!mlist)
+			mlist = mp;
+		else
+			list_add_tail(&mp->list, &mlist->list);
+
+		/* Add buffer to buffer pointer list */
+		if (outbound_buffers)
+			bpl->tus.f.bdeFlags = BUFF_TYPE_BDE_64;
+		else
+			bpl->tus.f.bdeFlags = BUFF_TYPE_BDE_64I;
+		bpl->addrLow = le32_to_cpu(putPaddrLow(mp->phys));
+		bpl->addrHigh = le32_to_cpu(putPaddrHigh(mp->phys));
+		bpl->tus.f.bdeSize = (uint16_t)
+			(bytes_left >= LPFC_BPL_SIZE ? LPFC_BPL_SIZE :
+			 bytes_left);
+		bytes_left -= bpl->tus.f.bdeSize;
+		bpl->tus.w = le32_to_cpu(bpl->tus.w);
+		bpl++;
+	}
+	return mlist;
+}
+
+static unsigned int
+lpfc_bsg_copy_data(struct lpfc_dmabuf *dma_buffers,
+		   struct fc_bsg_buffer *bsg_buffers,
+		   unsigned int bytes_to_transfer, int to_buffers)
+{
+
+	struct lpfc_dmabuf *mp;
+	unsigned int transfer_bytes, bytes_copied = 0;
+	unsigned int sg_offset, dma_offset;
+	unsigned char *dma_address, *sg_address;
+	struct scatterlist *sgel;
+	LIST_HEAD(temp_list);
+
+
+	list_splice_init(&dma_buffers->list, &temp_list);
+	list_add(&dma_buffers->list, &temp_list);
+	sg_offset = 0;
+	sgel = bsg_buffers->sg_list;
+	list_for_each_entry(mp, &temp_list, list) {
+		dma_offset = 0;
+		while (bytes_to_transfer && sgel &&
+		       (dma_offset < LPFC_BPL_SIZE)) {
+			dma_address = mp->virt + dma_offset;
+			if (sg_offset) {
+				/* Continue previous partial transfer of sg */
+				sg_address = sg_virt(sgel) + sg_offset;
+				transfer_bytes = sgel->length - sg_offset;
+			} else {
+				sg_address = sg_virt(sgel);
+				transfer_bytes = sgel->length;
+			}
+			if (bytes_to_transfer < transfer_bytes)
+				transfer_bytes = bytes_to_transfer;
+			if (transfer_bytes > (LPFC_BPL_SIZE - dma_offset))
+				transfer_bytes = LPFC_BPL_SIZE - dma_offset;
+			if (to_buffers)
+				memcpy(dma_address, sg_address, transfer_bytes);
+			else
+				memcpy(sg_address, dma_address, transfer_bytes);
+			dma_offset += transfer_bytes;
+			sg_offset += transfer_bytes;
+			bytes_to_transfer -= transfer_bytes;
+			bytes_copied += transfer_bytes;
+			if (sg_offset >= sgel->length) {
+				sg_offset = 0;
+				sgel = sg_next(sgel);
+			}
+		}
+	}
+	list_del_init(&dma_buffers->list);
+	list_splice(&temp_list, &dma_buffers->list);
+	return bytes_copied;
+}
+
 /**
  * lpfc_bsg_send_mgmt_cmd_cmp - lpfc_bsg_send_mgmt_cmd's completion handler
  * @phba: Pointer to HBA context object.
@@ -166,62 +288,72 @@ lpfc_bsg_send_mgmt_cmd_cmp(struct lpfc_hba *phba,
 	struct bsg_job_data *dd_data;
 	struct fc_bsg_job *job;
 	IOCB_t *rsp;
-	struct lpfc_dmabuf *bmp;
+	struct lpfc_dmabuf *bmp, *cmp, *rmp;
 	struct lpfc_nodelist *ndlp;
 	struct lpfc_bsg_iocb *iocb;
 	unsigned long flags;
+	unsigned int rsp_size;
 	int rc = 0;
 
+	dd_data = cmdiocbq->context1;
+
+	/* Determine if job has been aborted */
 	spin_lock_irqsave(&phba->ct_ev_lock, flags);
-	dd_data = cmdiocbq->context2;
-	if (!dd_data) {
-		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
-		lpfc_sli_release_iocbq(phba, cmdiocbq);
-		return;
+	job = dd_data->set_job;
+	if (job) {
+		/* Prevent timeout handling from trying to abort job */
+		job->dd_data = NULL;
 	}
+	spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
 
 	iocb = &dd_data->context_un.iocb;
-	job = iocb->set_job;
-	job->dd_data = NULL; /* so timeout handler does not reply */
-
-	bmp = iocb->bmp;
+	ndlp = iocb->ndlp;
+	rmp = iocb->rmp;
+	cmp = cmdiocbq->context2;
+	bmp = cmdiocbq->context3;
 	rsp = &rspiocbq->iocb;
-	ndlp = cmdiocbq->context1;
 
-	pci_unmap_sg(phba->pcidev, job->request_payload.sg_list,
-		     job->request_payload.sg_cnt, DMA_TO_DEVICE);
-	pci_unmap_sg(phba->pcidev, job->reply_payload.sg_list,
-		     job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
+	/* Copy the completed data or set the error status */
 
-	if (rsp->ulpStatus) {
-		if (rsp->ulpStatus == IOSTAT_LOCAL_REJECT) {
-			switch (rsp->un.ulpWord[4] & IOERR_PARAM_MASK) {
-			case IOERR_SEQUENCE_TIMEOUT:
-				rc = -ETIMEDOUT;
-				break;
-			case IOERR_INVALID_RPI:
-				rc = -EFAULT;
-				break;
-			default:
+	if (job) {
+		if (rsp->ulpStatus) {
+			if (rsp->ulpStatus == IOSTAT_LOCAL_REJECT) {
+				switch (rsp->un.ulpWord[4] & IOERR_PARAM_MASK) {
+				case IOERR_SEQUENCE_TIMEOUT:
+					rc = -ETIMEDOUT;
+					break;
+				case IOERR_INVALID_RPI:
+					rc = -EFAULT;
+					break;
+				default:
+					rc = -EACCES;
+					break;
+				}
+			} else {
 				rc = -EACCES;
-				break;
 			}
-		} else
-			rc = -EACCES;
-	} else
-		job->reply->reply_payload_rcv_len =
-			rsp->un.genreq64.bdl.bdeSize;
+		} else {
+			rsp_size = rsp->un.genreq64.bdl.bdeSize;
+			job->reply->reply_payload_rcv_len =
+				lpfc_bsg_copy_data(rmp, &job->reply_payload,
+						   rsp_size, 0);
+		}
+	}
 
+	lpfc_free_bsg_buffers(phba, cmp);
+	lpfc_free_bsg_buffers(phba, rmp);
 	lpfc_mbuf_free(phba, bmp->virt, bmp->phys);
+	kfree(bmp);
 	lpfc_sli_release_iocbq(phba, cmdiocbq);
 	lpfc_nlp_put(ndlp);
-	kfree(bmp);
 	kfree(dd_data);
-	/* make error code available to userspace */
-	job->reply->result = rc;
-	/* complete the job back to userspace */
-	job->job_done(job);
-	spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
+
+	/* Complete the job if the job is still active */
+
+	if (job) {
+		job->reply->result = rc;
+		job->job_done(job);
+	}
 	return;
 }
 
@@ -240,12 +372,9 @@ lpfc_bsg_send_mgmt_cmd(struct fc_bsg_job *job)
 	uint32_t timeout;
 	struct lpfc_iocbq *cmdiocbq = NULL;
 	IOCB_t *cmd;
-	struct lpfc_dmabuf *bmp = NULL;
+	struct lpfc_dmabuf *bmp = NULL, *cmp = NULL, *rmp = NULL;
 	int request_nseg;
 	int reply_nseg;
-	struct scatterlist *sgel = NULL;
-	int numbde;
-	dma_addr_t busaddr;
 	struct bsg_job_data *dd_data;
 	uint32_t creg_val;
 	int rc = 0;
@@ -268,54 +397,50 @@ lpfc_bsg_send_mgmt_cmd(struct fc_bsg_job *job)
 		goto no_ndlp;
 	}
 
-	bmp = kmalloc(sizeof(struct lpfc_dmabuf), GFP_KERNEL);
-	if (!bmp) {
-		rc = -ENOMEM;
-		goto free_ndlp;
-	}
-
 	if (ndlp->nlp_flag & NLP_ELS_SND_MASK) {
 		rc = -ENODEV;
-		goto free_bmp;
+		goto free_ndlp;
 	}
 
 	cmdiocbq = lpfc_sli_get_iocbq(phba);
 	if (!cmdiocbq) {
 		rc = -ENOMEM;
-		goto free_bmp;
+		goto free_ndlp;
 	}
 
 	cmd = &cmdiocbq->iocb;
+
+	bmp = kmalloc(sizeof(struct lpfc_dmabuf), GFP_KERNEL);
+	if (!bmp) {
+		rc = -ENOMEM;
+		goto free_cmdiocbq;
+	}
 	bmp->virt = lpfc_mbuf_alloc(phba, 0, &bmp->phys);
 	if (!bmp->virt) {
 		rc = -ENOMEM;
-		goto free_cmdiocbq;
+		goto free_bmp;
 	}
 
 	INIT_LIST_HEAD(&bmp->list);
+
 	bpl = (struct ulp_bde64 *) bmp->virt;
-	request_nseg = pci_map_sg(phba->pcidev, job->request_payload.sg_list,
-				  job->request_payload.sg_cnt, DMA_TO_DEVICE);
-	for_each_sg(job->request_payload.sg_list, sgel, request_nseg, numbde) {
-		busaddr = sg_dma_address(sgel);
-		bpl->tus.f.bdeFlags = BUFF_TYPE_BDE_64;
-		bpl->tus.f.bdeSize = sg_dma_len(sgel);
-		bpl->tus.w = cpu_to_le32(bpl->tus.w);
-		bpl->addrLow = cpu_to_le32(putPaddrLow(busaddr));
-		bpl->addrHigh = cpu_to_le32(putPaddrHigh(busaddr));
-		bpl++;
+	request_nseg = LPFC_BPL_SIZE/sizeof(struct ulp_bde64);
+	cmp = lpfc_alloc_bsg_buffers(phba, job->request_payload.payload_len,
+				     1, bpl, &request_nseg);
+	if (!cmp) {
+		rc = -ENOMEM;
+		goto free_bmp;
 	}
+	lpfc_bsg_copy_data(cmp, &job->request_payload,
+			   job->request_payload.payload_len, 1);
 
-	reply_nseg = pci_map_sg(phba->pcidev, job->reply_payload.sg_list,
-				job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
-	for_each_sg(job->reply_payload.sg_list, sgel, reply_nseg, numbde) {
-		busaddr = sg_dma_address(sgel);
-		bpl->tus.f.bdeFlags = BUFF_TYPE_BDE_64I;
-		bpl->tus.f.bdeSize = sg_dma_len(sgel);
-		bpl->tus.w = cpu_to_le32(bpl->tus.w);
-		bpl->addrLow = cpu_to_le32(putPaddrLow(busaddr));
-		bpl->addrHigh = cpu_to_le32(putPaddrHigh(busaddr));
-		bpl++;
+	bpl += request_nseg;
+	reply_nseg = LPFC_BPL_SIZE/sizeof(struct ulp_bde64) - request_nseg;
+	rmp = lpfc_alloc_bsg_buffers(phba, job->reply_payload.payload_len, 0,
+				     bpl, &reply_nseg);
+	if (!rmp) {
+		rc = -ENOMEM;
+		goto free_cmp;
 	}
 
 	cmd->un.genreq64.bdl.ulpIoTag32 = 0;
@@ -343,17 +468,20 @@ lpfc_bsg_send_mgmt_cmd(struct fc_bsg_job *job)
 	cmd->ulpTimeout = timeout;
 
 	cmdiocbq->iocb_cmpl = lpfc_bsg_send_mgmt_cmd_cmp;
-	cmdiocbq->context1 = ndlp;
-	cmdiocbq->context2 = dd_data;
+	cmdiocbq->context1 = dd_data;
+	cmdiocbq->context2 = cmp;
+	cmdiocbq->context3 = bmp;
 	dd_data->type = TYPE_IOCB;
+	dd_data->set_job = job;
 	dd_data->context_un.iocb.cmdiocbq = cmdiocbq;
-	dd_data->context_un.iocb.set_job = job;
-	dd_data->context_un.iocb.bmp = bmp;
+	dd_data->context_un.iocb.ndlp = ndlp;
+	dd_data->context_un.iocb.rmp = rmp;
+	job->dd_data = dd_data;
 
 	if (phba->cfg_poll & DISABLE_FCP_RING_INT) {
 		if (lpfc_readl(phba->HCregaddr, &creg_val)) {
 			rc = -EIO ;
-			goto free_cmdiocbq;
+			goto free_rmp;
 		}
 		creg_val |= (HC_R0INT_ENA << LPFC_FCP_RING);
 		writel(creg_val, phba->HCregaddr);
@@ -368,19 +496,18 @@ lpfc_bsg_send_mgmt_cmd(struct fc_bsg_job *job)
 	else
 		rc = -EIO;
 
-
 	/* iocb failed so cleanup */
-	pci_unmap_sg(phba->pcidev, job->request_payload.sg_list,
-		     job->request_payload.sg_cnt, DMA_TO_DEVICE);
-	pci_unmap_sg(phba->pcidev, job->reply_payload.sg_list,
-		     job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
 
-	lpfc_mbuf_free(phba, bmp->virt, bmp->phys);
-
-free_cmdiocbq:
-	lpfc_sli_release_iocbq(phba, cmdiocbq);
+free_rmp:
+	lpfc_free_bsg_buffers(phba, rmp);
+free_cmp:
+	lpfc_free_bsg_buffers(phba, cmp);
 free_bmp:
+	if (bmp->virt)
+		lpfc_mbuf_free(phba, bmp->virt, bmp->phys);
 	kfree(bmp);
+free_cmdiocbq:
+	lpfc_sli_release_iocbq(phba, cmdiocbq);
 free_ndlp:
 	lpfc_nlp_put(ndlp);
 no_ndlp:
@@ -418,67 +545,68 @@ lpfc_bsg_rport_els_cmp(struct lpfc_hba *phba,
 	struct fc_bsg_job *job;
 	IOCB_t *rsp;
 	struct lpfc_nodelist *ndlp;
-	struct lpfc_dmabuf *pbuflist = NULL;
+	struct lpfc_dmabuf *pcmd = NULL, *prsp = NULL;
 	struct fc_bsg_ctels_reply *els_reply;
 	uint8_t *rjt_data;
 	unsigned long flags;
+	unsigned int rsp_size;
 	int rc = 0;
 
-	spin_lock_irqsave(&phba->ct_ev_lock, flags);
 	dd_data = cmdiocbq->context1;
-	/* normal completion and timeout crossed paths, already done */
-	if (!dd_data) {
-		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
-		return;
-	}
+	ndlp = dd_data->context_un.iocb.ndlp;
+	cmdiocbq->context1 = ndlp;
 
-	cmdiocbq->iocb_flag |= LPFC_IO_WAKE;
-	if (cmdiocbq->context2 && rspiocbq)
-		memcpy(&((struct lpfc_iocbq *)cmdiocbq->context2)->iocb,
-		       &rspiocbq->iocb, sizeof(IOCB_t));
+	/* Determine if job has been aborted */
+	spin_lock_irqsave(&phba->ct_ev_lock, flags);
+	job = dd_data->set_job;
+	if (job) {
+		/* Prevent timeout handling from trying to abort job  */
+		job->dd_data = NULL;
+	}
+	spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
 
-	job = dd_data->context_un.iocb.set_job;
-	cmdiocbq = dd_data->context_un.iocb.cmdiocbq;
-	rspiocbq = dd_data->context_un.iocb.rspiocbq;
 	rsp = &rspiocbq->iocb;
-	ndlp = dd_data->context_un.iocb.ndlp;
+	pcmd = (struct lpfc_dmabuf *)cmdiocbq->context2;
+	prsp = (struct lpfc_dmabuf *)pcmd->list.next;
 
-	pci_unmap_sg(phba->pcidev, job->request_payload.sg_list,
-		     job->request_payload.sg_cnt, DMA_TO_DEVICE);
-	pci_unmap_sg(phba->pcidev, job->reply_payload.sg_list,
-		     job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
+	/* Copy the completed job data or determine the job status if job is
+	 * still active
+	 */
 
-	if (job->reply->result == -EAGAIN)
-		rc = -EAGAIN;
-	else if (rsp->ulpStatus == IOSTAT_SUCCESS)
-		job->reply->reply_payload_rcv_len =
-			rsp->un.elsreq64.bdl.bdeSize;
-	else if (rsp->ulpStatus == IOSTAT_LS_RJT) {
-		job->reply->reply_payload_rcv_len =
-			sizeof(struct fc_bsg_ctels_reply);
-		/* LS_RJT data returned in word 4 */
-		rjt_data = (uint8_t *)&rsp->un.ulpWord[4];
-		els_reply = &job->reply->reply_data.ctels_reply;
-		els_reply->status = FC_CTELS_STATUS_REJECT;
-		els_reply->rjt_data.action = rjt_data[3];
-		els_reply->rjt_data.reason_code = rjt_data[2];
-		els_reply->rjt_data.reason_explanation = rjt_data[1];
-		els_reply->rjt_data.vendor_unique = rjt_data[0];
-	} else
-		rc = -EIO;
+	if (job) {
+		if (rsp->ulpStatus == IOSTAT_SUCCESS) {
+			rsp_size = rsp->un.elsreq64.bdl.bdeSize;
+			job->reply->reply_payload_rcv_len =
+				sg_copy_from_buffer(job->reply_payload.sg_list,
+						    job->reply_payload.sg_cnt,
+						    prsp->virt,
+						    rsp_size);
+		} else if (rsp->ulpStatus == IOSTAT_LS_RJT) {
+			job->reply->reply_payload_rcv_len =
+				sizeof(struct fc_bsg_ctels_reply);
+			/* LS_RJT data returned in word 4 */
+			rjt_data = (uint8_t *)&rsp->un.ulpWord[4];
+			els_reply = &job->reply->reply_data.ctels_reply;
+			els_reply->status = FC_CTELS_STATUS_REJECT;
+			els_reply->rjt_data.action = rjt_data[3];
+			els_reply->rjt_data.reason_code = rjt_data[2];
+			els_reply->rjt_data.reason_explanation = rjt_data[1];
+			els_reply->rjt_data.vendor_unique = rjt_data[0];
+		} else {
+			rc = -EIO;
+		}
+	}
 
-	pbuflist = (struct lpfc_dmabuf *) cmdiocbq->context3;
-	lpfc_mbuf_free(phba, pbuflist->virt, pbuflist->phys);
-	lpfc_sli_release_iocbq(phba, rspiocbq);
-	lpfc_sli_release_iocbq(phba, cmdiocbq);
 	lpfc_nlp_put(ndlp);
+	lpfc_els_free_iocb(phba, cmdiocbq);
 	kfree(dd_data);
-	/* make error code available to userspace */
-	job->reply->result = rc;
-	job->dd_data = NULL;
-	/* complete the job back to userspace */
-	job->job_done(job);
-	spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
+
+	/* Complete the job if the job is still active */
+
+	if (job) {
+		job->reply->result = rc;
+		job->job_done(job);
+	}
 	return;
 }
 
@@ -496,19 +624,8 @@ lpfc_bsg_rport_els(struct fc_bsg_job *job)
 	uint32_t elscmd;
 	uint32_t cmdsize;
 	uint32_t rspsize;
-	struct lpfc_iocbq *rspiocbq;
 	struct lpfc_iocbq *cmdiocbq;
-	IOCB_t *rsp;
 	uint16_t rpi = 0;
-	struct lpfc_dmabuf *pcmd;
-	struct lpfc_dmabuf *prsp;
-	struct lpfc_dmabuf *pbuflist = NULL;
-	struct ulp_bde64 *bpl;
-	int request_nseg;
-	int reply_nseg;
-	struct scatterlist *sgel = NULL;
-	int numbde;
-	dma_addr_t busaddr;
 	struct bsg_job_data *dd_data;
 	uint32_t creg_val;
 	int rc = 0;
@@ -516,6 +633,15 @@ lpfc_bsg_rport_els(struct fc_bsg_job *job)
 	/* in case no data is transferred */
 	job->reply->reply_payload_rcv_len = 0;
 
+	/* verify the els command is not greater than the
+	 * maximum ELS transfer size.
+	 */
+
+	if (job->request_payload.payload_len > FCELSSIZE) {
+		rc = -EINVAL;
+		goto no_dd_data;
+	}
+
 	/* allocate our bsg tracking structure */
 	dd_data = kmalloc(sizeof(struct bsg_job_data), GFP_KERNEL);
 	if (!dd_data) {
@@ -525,88 +651,51 @@ lpfc_bsg_rport_els(struct fc_bsg_job *job)
 		goto no_dd_data;
 	}
 
-	if (!lpfc_nlp_get(ndlp)) {
-		rc = -ENODEV;
-		goto free_dd_data;
-	}
-
 	elscmd = job->request->rqst_data.r_els.els_code;
 	cmdsize = job->request_payload.payload_len;
 	rspsize = job->reply_payload.payload_len;
-	rspiocbq = lpfc_sli_get_iocbq(phba);
-	if (!rspiocbq) {
-		lpfc_nlp_put(ndlp);
-		rc = -ENOMEM;
+
+	if (!lpfc_nlp_get(ndlp)) {
+		rc = -ENODEV;
 		goto free_dd_data;
 	}
 
-	rsp = &rspiocbq->iocb;
-	rpi = ndlp->nlp_rpi;
+	/* We will use the allocated dma buffers by prep els iocb for command
+	 * and response to ensure if the job times out and the request is freed,
+	 * we won't be dma into memory that is no longer allocated to for the
+	 * request.
+	 */
 
 	cmdiocbq = lpfc_prep_els_iocb(vport, 1, cmdsize, 0, ndlp,
 				      ndlp->nlp_DID, elscmd);
 	if (!cmdiocbq) {
 		rc = -EIO;
-		goto free_rspiocbq;
+		goto release_ndlp;
 	}
 
-	/* prep els iocb set context1 to the ndlp, context2 to the command
-	 * dmabuf, context3 holds the data dmabuf
-	 */
-	pcmd = (struct lpfc_dmabuf *) cmdiocbq->context2;
-	prsp = (struct lpfc_dmabuf *) pcmd->list.next;
-	lpfc_mbuf_free(phba, pcmd->virt, pcmd->phys);
-	kfree(pcmd);
-	lpfc_mbuf_free(phba, prsp->virt, prsp->phys);
-	kfree(prsp);
-	cmdiocbq->context2 = NULL;
-
-	pbuflist = (struct lpfc_dmabuf *) cmdiocbq->context3;
-	bpl = (struct ulp_bde64 *) pbuflist->virt;
-
-	request_nseg = pci_map_sg(phba->pcidev, job->request_payload.sg_list,
-				  job->request_payload.sg_cnt, DMA_TO_DEVICE);
-	for_each_sg(job->request_payload.sg_list, sgel, request_nseg, numbde) {
-		busaddr = sg_dma_address(sgel);
-		bpl->tus.f.bdeFlags = BUFF_TYPE_BDE_64;
-		bpl->tus.f.bdeSize = sg_dma_len(sgel);
-		bpl->tus.w = cpu_to_le32(bpl->tus.w);
-		bpl->addrLow = cpu_to_le32(putPaddrLow(busaddr));
-		bpl->addrHigh = cpu_to_le32(putPaddrHigh(busaddr));
-		bpl++;
-	}
+	rpi = ndlp->nlp_rpi;
+
+	/* Transfer the request payload to allocated command dma buffer */
+
+	sg_copy_to_buffer(job->request_payload.sg_list,
+			  job->request_payload.sg_cnt,
+			  ((struct lpfc_dmabuf *)cmdiocbq->context2)->virt,
+			  cmdsize);
 
-	reply_nseg = pci_map_sg(phba->pcidev, job->reply_payload.sg_list,
-				job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
-	for_each_sg(job->reply_payload.sg_list, sgel, reply_nseg, numbde) {
-		busaddr = sg_dma_address(sgel);
-		bpl->tus.f.bdeFlags = BUFF_TYPE_BDE_64I;
-		bpl->tus.f.bdeSize = sg_dma_len(sgel);
-		bpl->tus.w = cpu_to_le32(bpl->tus.w);
-		bpl->addrLow = cpu_to_le32(putPaddrLow(busaddr));
-		bpl->addrHigh = cpu_to_le32(putPaddrHigh(busaddr));
-		bpl++;
-	}
-	cmdiocbq->iocb.un.elsreq64.bdl.bdeSize =
-		(request_nseg + reply_nseg) * sizeof(struct ulp_bde64);
 	if (phba->sli_rev == LPFC_SLI_REV4)
 		cmdiocbq->iocb.ulpContext = phba->sli4_hba.rpi_ids[rpi];
 	else
 		cmdiocbq->iocb.ulpContext = rpi;
 	cmdiocbq->iocb_flag |= LPFC_IO_LIBDFC;
-	cmdiocbq->context1 = NULL;
-	cmdiocbq->context2 = NULL;
-
-	cmdiocbq->iocb_cmpl = lpfc_bsg_rport_els_cmp;
 	cmdiocbq->context1 = dd_data;
 	cmdiocbq->context_un.ndlp = ndlp;
-	cmdiocbq->context2 = rspiocbq;
+	cmdiocbq->iocb_cmpl = lpfc_bsg_rport_els_cmp;
 	dd_data->type = TYPE_IOCB;
+	dd_data->set_job = job;
 	dd_data->context_un.iocb.cmdiocbq = cmdiocbq;
-	dd_data->context_un.iocb.rspiocbq = rspiocbq;
-	dd_data->context_un.iocb.set_job = job;
-	dd_data->context_un.iocb.bmp = NULL;
 	dd_data->context_un.iocb.ndlp = ndlp;
+	dd_data->context_un.iocb.rmp = NULL;
+	job->dd_data = dd_data;
 
 	if (phba->cfg_poll & DISABLE_FCP_RING_INT) {
 		if (lpfc_readl(phba->HCregaddr, &creg_val)) {
@@ -617,8 +706,9 @@ lpfc_bsg_rport_els(struct fc_bsg_job *job)
 		writel(creg_val, phba->HCregaddr);
 		readl(phba->HCregaddr); /* flush */
 	}
+
 	rc = lpfc_sli_issue_iocb(phba, LPFC_ELS_RING, cmdiocbq, 0);
-	lpfc_nlp_put(ndlp);
+
 	if (rc == IOCB_SUCCESS)
 		return 0; /* done for now */
 	else if (rc == IOCB_BUSY)
@@ -627,17 +717,12 @@ lpfc_bsg_rport_els(struct fc_bsg_job *job)
 		rc = -EIO;
 
 linkdown_err:
-	pci_unmap_sg(phba->pcidev, job->request_payload.sg_list,
-		     job->request_payload.sg_cnt, DMA_TO_DEVICE);
-	pci_unmap_sg(phba->pcidev, job->reply_payload.sg_list,
-		     job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
 
-	lpfc_mbuf_free(phba, pbuflist->virt, pbuflist->phys);
-
-	lpfc_sli_release_iocbq(phba, cmdiocbq);
+	cmdiocbq->context1 = ndlp;
+	lpfc_els_free_iocb(phba, cmdiocbq);
 
-free_rspiocbq:
-	lpfc_sli_release_iocbq(phba, rspiocbq);
+release_ndlp:
+	lpfc_nlp_put(ndlp);
 
 free_dd_data:
 	kfree(dd_data);
@@ -680,6 +765,7 @@ lpfc_bsg_event_free(struct kref *kref)
 		kfree(ed);
 	}
 
+	kfree(evt->dd_data);
 	kfree(evt);
 }
 
@@ -723,6 +809,7 @@ lpfc_bsg_event_new(uint32_t ev_mask, int ev_reg_id, uint32_t ev_req_id)
 	evt->req_id = ev_req_id;
 	evt->reg_id = ev_reg_id;
 	evt->wait_time_stamp = jiffies;
+	evt->dd_data = NULL;
 	init_waitqueue_head(&evt->wq);
 	kref_init(&evt->kref);
 	return evt;
@@ -790,6 +877,7 @@ lpfc_bsg_ct_unsol_event(struct lpfc_hba *phba, struct lpfc_sli_ring *pring,
 	struct lpfc_hbq_entry *hbqe;
 	struct lpfc_sli_ct_request *ct_req;
 	struct fc_bsg_job *job = NULL;
+	struct bsg_job_data *dd_data = NULL;
 	unsigned long flags;
 	int size = 0;
 
@@ -986,10 +1074,11 @@ lpfc_bsg_ct_unsol_event(struct lpfc_hba *phba, struct lpfc_sli_ring *pring,
 		}
 
 		list_move(evt->events_to_see.prev, &evt->events_to_get);
-		lpfc_bsg_event_unref(evt);
 
-		job = evt->set_job;
-		evt->set_job = NULL;
+		dd_data = (struct bsg_job_data *)evt->dd_data;
+		job = dd_data->set_job;
+		dd_data->set_job = NULL;
+		lpfc_bsg_event_unref(evt);
 		if (job) {
 			job->reply->reply_payload_rcv_len = size;
 			/* make error code available to userspace */
@@ -1078,14 +1167,6 @@ lpfc_bsg_hba_set_event(struct fc_bsg_job *job)
 		goto job_error;
 	}
 
-	dd_data = kmalloc(sizeof(struct bsg_job_data), GFP_KERNEL);
-	if (dd_data == NULL) {
-		lpfc_printf_log(phba, KERN_WARNING, LOG_LIBDFC,
-				"2734 Failed allocation of dd_data\n");
-		rc = -ENOMEM;
-		goto job_error;
-	}
-
 	event_req = (struct set_ct_event *)
 		job->request->rqst_data.h_vendor.vendor_cmd;
 	ev_mask = ((uint32_t)(unsigned long)event_req->type_mask &
@@ -1095,6 +1176,7 @@ lpfc_bsg_hba_set_event(struct fc_bsg_job *job)
 		if (evt->reg_id == event_req->ev_reg_id) {
 			lpfc_bsg_event_ref(evt);
 			evt->wait_time_stamp = jiffies;
+			dd_data = (struct bsg_job_data *)evt->dd_data;
 			break;
 		}
 	}
@@ -1102,6 +1184,13 @@ lpfc_bsg_hba_set_event(struct fc_bsg_job *job)
 
 	if (&evt->node == &phba->ct_ev_waiters) {
 		/* no event waiting struct yet - first call */
+		dd_data = kmalloc(sizeof(struct bsg_job_data), GFP_KERNEL);
+		if (dd_data == NULL) {
+			lpfc_printf_log(phba, KERN_WARNING, LOG_LIBDFC,
+					"2734 Failed allocation of dd_data\n");
+			rc = -ENOMEM;
+			goto job_error;
+		}
 		evt = lpfc_bsg_event_new(ev_mask, event_req->ev_reg_id,
 					event_req->ev_req_id);
 		if (!evt) {
@@ -1111,7 +1200,10 @@ lpfc_bsg_hba_set_event(struct fc_bsg_job *job)
 			rc = -ENOMEM;
 			goto job_error;
 		}
-
+		dd_data->type = TYPE_EVT;
+		dd_data->set_job = NULL;
+		dd_data->context_un.evt = evt;
+		evt->dd_data = (void *)dd_data;
 		spin_lock_irqsave(&phba->ct_ev_lock, flags);
 		list_add(&evt->node, &phba->ct_ev_waiters);
 		lpfc_bsg_event_ref(evt);
@@ -1121,9 +1213,7 @@ lpfc_bsg_hba_set_event(struct fc_bsg_job *job)
 
 	spin_lock_irqsave(&phba->ct_ev_lock, flags);
 	evt->waiting = 1;
-	dd_data->type = TYPE_EVT;
-	dd_data->context_un.evt = evt;
-	evt->set_job = job; /* for unsolicited command */
+	dd_data->set_job = job; /* for unsolicited command */
 	job->dd_data = dd_data; /* for fc transport timeout callback*/
 	spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
 	return 0; /* call job done later */
@@ -1252,57 +1342,64 @@ lpfc_issue_ct_rsp_cmp(struct lpfc_hba *phba,
 	struct bsg_job_data *dd_data;
 	struct fc_bsg_job *job;
 	IOCB_t *rsp;
-	struct lpfc_dmabuf *bmp;
+	struct lpfc_dmabuf *bmp, *cmp;
 	struct lpfc_nodelist *ndlp;
 	unsigned long flags;
 	int rc = 0;
 
+	dd_data = cmdiocbq->context1;
+
+	/* Determine if job has been aborted */
 	spin_lock_irqsave(&phba->ct_ev_lock, flags);
-	dd_data = cmdiocbq->context2;
-	/* normal completion and timeout crossed paths, already done */
-	if (!dd_data) {
-		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
-		return;
+	job = dd_data->set_job;
+	if (job) {
+		/* Prevent timeout handling from trying to abort job  */
+		job->dd_data = NULL;
 	}
+	spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
 
-	job = dd_data->context_un.iocb.set_job;
-	bmp = dd_data->context_un.iocb.bmp;
-	rsp = &rspiocbq->iocb;
 	ndlp = dd_data->context_un.iocb.ndlp;
+	cmp = cmdiocbq->context2;
+	bmp = cmdiocbq->context3;
+	rsp = &rspiocbq->iocb;
 
-	pci_unmap_sg(phba->pcidev, job->request_payload.sg_list,
-		     job->request_payload.sg_cnt, DMA_TO_DEVICE);
+	/* Copy the completed job data or set the error status */
 
-	if (rsp->ulpStatus) {
-		if (rsp->ulpStatus == IOSTAT_LOCAL_REJECT) {
-			switch (rsp->un.ulpWord[4] & IOERR_PARAM_MASK) {
-			case IOERR_SEQUENCE_TIMEOUT:
-				rc = -ETIMEDOUT;
-				break;
-			case IOERR_INVALID_RPI:
-				rc = -EFAULT;
-				break;
-			default:
+	if (job) {
+		if (rsp->ulpStatus) {
+			if (rsp->ulpStatus == IOSTAT_LOCAL_REJECT) {
+				switch (rsp->un.ulpWord[4] & IOERR_PARAM_MASK) {
+				case IOERR_SEQUENCE_TIMEOUT:
+					rc = -ETIMEDOUT;
+					break;
+				case IOERR_INVALID_RPI:
+					rc = -EFAULT;
+					break;
+				default:
+					rc = -EACCES;
+					break;
+				}
+			} else {
 				rc = -EACCES;
-				break;
 			}
-		} else
-			rc = -EACCES;
-	} else
-		job->reply->reply_payload_rcv_len =
-			rsp->un.genreq64.bdl.bdeSize;
+		} else {
+			job->reply->reply_payload_rcv_len = 0;
+		}
+	}
 
+	lpfc_free_bsg_buffers(phba, cmp);
 	lpfc_mbuf_free(phba, bmp->virt, bmp->phys);
+	kfree(bmp);
 	lpfc_sli_release_iocbq(phba, cmdiocbq);
 	lpfc_nlp_put(ndlp);
-	kfree(bmp);
 	kfree(dd_data);
-	/* make error code available to userspace */
-	job->reply->result = rc;
-	job->dd_data = NULL;
-	/* complete the job back to userspace */
-	job->job_done(job);
-	spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
+
+	/* Complete the job if the job is still active */
+
+	if (job) {
+		job->reply->result = rc;
+		job->job_done(job);
+	}
 	return;
 }
 
@@ -1316,7 +1413,8 @@ lpfc_issue_ct_rsp_cmp(struct lpfc_hba *phba,
  **/
 static int
 lpfc_issue_ct_rsp(struct lpfc_hba *phba, struct fc_bsg_job *job, uint32_t tag,
-		  struct lpfc_dmabuf *bmp, int num_entry)
+		  struct lpfc_dmabuf *cmp, struct lpfc_dmabuf *bmp,
+		  int num_entry)
 {
 	IOCB_t *icmd;
 	struct lpfc_iocbq *ctiocb = NULL;
@@ -1377,7 +1475,7 @@ lpfc_issue_ct_rsp(struct lpfc_hba *phba, struct fc_bsg_job *job, uint32_t tag,
 
 		/* Check if the ndlp is active */
 		if (!ndlp || !NLP_CHK_NODE_ACT(ndlp)) {
-			rc = -IOCB_ERROR;
+			rc = IOCB_ERROR;
 			goto issue_ct_rsp_exit;
 		}
 
@@ -1385,7 +1483,7 @@ lpfc_issue_ct_rsp(struct lpfc_hba *phba, struct fc_bsg_job *job, uint32_t tag,
 		 * we respond
 		 */
 		if (!lpfc_nlp_get(ndlp)) {
-			rc = -IOCB_ERROR;
+			rc = IOCB_ERROR;
 			goto issue_ct_rsp_exit;
 		}
 
@@ -1407,17 +1505,17 @@ lpfc_issue_ct_rsp(struct lpfc_hba *phba, struct fc_bsg_job *job, uint32_t tag,
 	ctiocb->iocb_cmpl = NULL;
 	ctiocb->iocb_flag |= LPFC_IO_LIBDFC;
 	ctiocb->vport = phba->pport;
+	ctiocb->context1 = dd_data;
+	ctiocb->context2 = cmp;
 	ctiocb->context3 = bmp;
-
 	ctiocb->iocb_cmpl = lpfc_issue_ct_rsp_cmp;
-	ctiocb->context2 = dd_data;
-	ctiocb->context1 = ndlp;
+
 	dd_data->type = TYPE_IOCB;
+	dd_data->set_job = job;
 	dd_data->context_un.iocb.cmdiocbq = ctiocb;
-	dd_data->context_un.iocb.rspiocbq = NULL;
-	dd_data->context_un.iocb.set_job = job;
-	dd_data->context_un.iocb.bmp = bmp;
 	dd_data->context_un.iocb.ndlp = ndlp;
+	dd_data->context_un.iocb.rmp = NULL;
+	job->dd_data = dd_data;
 
 	if (phba->cfg_poll & DISABLE_FCP_RING_INT) {
 		if (lpfc_readl(phba->HCregaddr, &creg_val)) {
@@ -1454,11 +1552,8 @@ lpfc_bsg_send_mgmt_rsp(struct fc_bsg_job *job)
 	struct send_mgmt_resp *mgmt_resp = (struct send_mgmt_resp *)
 		job->request->rqst_data.h_vendor.vendor_cmd;
 	struct ulp_bde64 *bpl;
-	struct lpfc_dmabuf *bmp = NULL;
-	struct scatterlist *sgel = NULL;
-	int request_nseg;
-	int numbde;
-	dma_addr_t busaddr;
+	struct lpfc_dmabuf *bmp = NULL, *cmp = NULL;
+	int bpl_entries;
 	uint32_t tag = mgmt_resp->tag;
 	unsigned long reqbfrcnt =
 			(unsigned long)job->request_payload.payload_len;
@@ -1486,30 +1581,28 @@ lpfc_bsg_send_mgmt_rsp(struct fc_bsg_job *job)
 
 	INIT_LIST_HEAD(&bmp->list);
 	bpl = (struct ulp_bde64 *) bmp->virt;
-	request_nseg = pci_map_sg(phba->pcidev, job->request_payload.sg_list,
-				  job->request_payload.sg_cnt, DMA_TO_DEVICE);
-	for_each_sg(job->request_payload.sg_list, sgel, request_nseg, numbde) {
-		busaddr = sg_dma_address(sgel);
-		bpl->tus.f.bdeFlags = BUFF_TYPE_BDE_64;
-		bpl->tus.f.bdeSize = sg_dma_len(sgel);
-		bpl->tus.w = cpu_to_le32(bpl->tus.w);
-		bpl->addrLow = cpu_to_le32(putPaddrLow(busaddr));
-		bpl->addrHigh = cpu_to_le32(putPaddrHigh(busaddr));
-		bpl++;
+	bpl_entries = (LPFC_BPL_SIZE/sizeof(struct ulp_bde64));
+	cmp = lpfc_alloc_bsg_buffers(phba, job->request_payload.payload_len,
+				     1, bpl, &bpl_entries);
+	if (!cmp) {
+		rc = -ENOMEM;
+		goto send_mgmt_rsp_free_bmp;
 	}
+	lpfc_bsg_copy_data(cmp, &job->request_payload,
+			   job->request_payload.payload_len, 1);
 
-	rc = lpfc_issue_ct_rsp(phba, job, tag, bmp, request_nseg);
+	rc = lpfc_issue_ct_rsp(phba, job, tag, cmp, bmp, bpl_entries);
 
 	if (rc == IOCB_SUCCESS)
 		return 0; /* done for now */
 
-	/* TBD need to handle a timeout */
-	pci_unmap_sg(phba->pcidev, job->request_payload.sg_list,
-			  job->request_payload.sg_cnt, DMA_TO_DEVICE);
 	rc = -EACCES;
-	lpfc_mbuf_free(phba, bmp->virt, bmp->phys);
+
+	lpfc_free_bsg_buffers(phba, cmp);
 
 send_mgmt_rsp_free_bmp:
+	if (bmp->virt)
+		lpfc_mbuf_free(phba, bmp->virt, bmp->phys);
 	kfree(bmp);
 send_mgmt_rsp_exit:
 	/* make error code available to userspace */
@@ -1559,7 +1652,7 @@ lpfc_bsg_diag_mode_enter(struct lpfc_hba *phba)
 		scsi_block_requests(shost);
 	}
 
-	while (pring->txcmplq_cnt) {
+	while (!list_empty(&pring->txcmplq)) {
 		if (i++ > 500)  /* wait up to 5 seconds */
 			break;
 		msleep(10);
@@ -3193,13 +3286,7 @@ lpfc_bsg_issue_mbox_cmpl(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmboxq)
 	unsigned long flags;
 	uint8_t *pmb, *pmb_buf;
 
-	spin_lock_irqsave(&phba->ct_ev_lock, flags);
 	dd_data = pmboxq->context1;
-	/* job already timed out? */
-	if (!dd_data) {
-		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
-		return;
-	}
 
 	/*
 	 * The outgoing buffer is readily referred from the dma buffer,
@@ -3209,29 +3296,33 @@ lpfc_bsg_issue_mbox_cmpl(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmboxq)
 	pmb_buf = (uint8_t *)dd_data->context_un.mbox.mb;
 	memcpy(pmb_buf, pmb, sizeof(MAILBOX_t));
 
-	job = dd_data->context_un.mbox.set_job;
+	/* Determine if job has been aborted */
+
+	spin_lock_irqsave(&phba->ct_ev_lock, flags);
+	job = dd_data->set_job;
+	if (job) {
+		/* Prevent timeout handling from trying to abort job  */
+		job->dd_data = NULL;
+	}
+	spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
+
+	/* Copy the mailbox data to the job if it is still active */
+
 	if (job) {
 		size = job->reply_payload.payload_len;
 		job->reply->reply_payload_rcv_len =
 			sg_copy_from_buffer(job->reply_payload.sg_list,
 					    job->reply_payload.sg_cnt,
 					    pmb_buf, size);
-		/* need to hold the lock until we set job->dd_data to NULL
-		 * to hold off the timeout handler returning to the mid-layer
-		 * while we are still processing the job.
-		 */
-		job->dd_data = NULL;
-		dd_data->context_un.mbox.set_job = NULL;
-		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
-	} else {
-		dd_data->context_un.mbox.set_job = NULL;
-		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
 	}
 
+	dd_data->set_job = NULL;
 	mempool_free(dd_data->context_un.mbox.pmboxq, phba->mbox_mem_pool);
 	lpfc_bsg_dma_page_free(phba, dd_data->context_un.mbox.dmabuffers);
 	kfree(dd_data);
 
+	/* Complete the job if the job is still active */
+
 	if (job) {
 		job->reply->result = 0;
 		job->job_done(job);
@@ -3377,19 +3468,22 @@ lpfc_bsg_issue_mbox_ext_handle_job(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmboxq)
 	struct lpfc_sli_config_mbox *sli_cfg_mbx;
 	uint8_t *pmbx;
 
-	spin_lock_irqsave(&phba->ct_ev_lock, flags);
 	dd_data = pmboxq->context1;
-	/* has the job already timed out? */
-	if (!dd_data) {
-		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
-		job = NULL;
-		goto job_done_out;
+
+	/* Determine if job has been aborted */
+	spin_lock_irqsave(&phba->ct_ev_lock, flags);
+	job = dd_data->set_job;
+	if (job) {
+		/* Prevent timeout handling from trying to abort job  */
+		job->dd_data = NULL;
 	}
+	spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
 
 	/*
 	 * The outgoing buffer is readily referred from the dma buffer,
 	 * just need to get header part from mailboxq structure.
 	 */
+
 	pmb = (uint8_t *)&pmboxq->u.mb;
 	pmb_buf = (uint8_t *)dd_data->context_un.mbox.mb;
 	/* Copy the byte swapped response mailbox back to the user */
@@ -3406,21 +3500,18 @@ lpfc_bsg_issue_mbox_ext_handle_job(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmboxq)
 			sli_cfg_mbx->un.sli_config_emb0_subsys.mse[0].buf_len);
 	}
 
-	job = dd_data->context_un.mbox.set_job;
+	/* Complete the job if the job is still active */
+
 	if (job) {
 		size = job->reply_payload.payload_len;
 		job->reply->reply_payload_rcv_len =
 			sg_copy_from_buffer(job->reply_payload.sg_list,
 					    job->reply_payload.sg_cnt,
 					    pmb_buf, size);
+
 		/* result for successful */
 		job->reply->result = 0;
-		job->dd_data = NULL;
-		/* need to hold the lock util we set job->dd_data to NULL
-		 * to hold off the timeout handler from midlayer to take
-		 * any action.
-		 */
-		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
+
 		lpfc_printf_log(phba, KERN_INFO, LOG_LIBDFC,
 				"2937 SLI_CONFIG ext-buffer maibox command "
 				"(x%x/x%x) complete bsg job done, bsize:%d\n",
@@ -3431,20 +3522,18 @@ lpfc_bsg_issue_mbox_ext_handle_job(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmboxq)
 					phba->mbox_ext_buf_ctx.mboxType,
 					dma_ebuf, sta_pos_addr,
 					phba->mbox_ext_buf_ctx.mbx_dmabuf, 0);
-	} else
-		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
-
-job_done_out:
-	if (!job)
+	} else {
 		lpfc_printf_log(phba, KERN_ERR, LOG_LIBDFC,
 				"2938 SLI_CONFIG ext-buffer maibox "
 				"command (x%x/x%x) failure, rc:x%x\n",
 				phba->mbox_ext_buf_ctx.nembType,
 				phba->mbox_ext_buf_ctx.mboxType, rc);
+	}
+
+
 	/* state change */
 	phba->mbox_ext_buf_ctx.state = LPFC_BSG_MBOX_DONE;
 	kfree(dd_data);
-
 	return job;
 }
 
@@ -3461,8 +3550,10 @@ lpfc_bsg_issue_read_mbox_ext_cmpl(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmboxq)
 {
 	struct fc_bsg_job *job;
 
+	job = lpfc_bsg_issue_mbox_ext_handle_job(phba, pmboxq);
+
 	/* handle the BSG job with mailbox command */
-	if (phba->mbox_ext_buf_ctx.state == LPFC_BSG_MBOX_ABTS)
+	if (!job)
 		pmboxq->u.mb.mbxStatus = MBXERR_ERROR;
 
 	lpfc_printf_log(phba, KERN_INFO, LOG_LIBDFC,
@@ -3470,15 +3561,13 @@ lpfc_bsg_issue_read_mbox_ext_cmpl(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmboxq)
 			"complete, ctxState:x%x, mbxStatus:x%x\n",
 			phba->mbox_ext_buf_ctx.state, pmboxq->u.mb.mbxStatus);
 
-	job = lpfc_bsg_issue_mbox_ext_handle_job(phba, pmboxq);
-
 	if (pmboxq->u.mb.mbxStatus || phba->mbox_ext_buf_ctx.numBuf == 1)
 		lpfc_bsg_mbox_ext_session_reset(phba);
 
 	/* free base driver mailbox structure memory */
 	mempool_free(pmboxq, phba->mbox_mem_pool);
 
-	/* complete the bsg job if we have it */
+	/* if the job is still active, call job done */
 	if (job)
 		job->job_done(job);
 
@@ -3498,8 +3587,10 @@ lpfc_bsg_issue_write_mbox_ext_cmpl(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmboxq)
 {
 	struct fc_bsg_job *job;
 
+	job = lpfc_bsg_issue_mbox_ext_handle_job(phba, pmboxq);
+
 	/* handle the BSG job with the mailbox command */
-	if (phba->mbox_ext_buf_ctx.state == LPFC_BSG_MBOX_ABTS)
+	if (!job)
 		pmboxq->u.mb.mbxStatus = MBXERR_ERROR;
 
 	lpfc_printf_log(phba, KERN_INFO, LOG_LIBDFC,
@@ -3507,13 +3598,11 @@ lpfc_bsg_issue_write_mbox_ext_cmpl(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmboxq)
 			"complete, ctxState:x%x, mbxStatus:x%x\n",
 			phba->mbox_ext_buf_ctx.state, pmboxq->u.mb.mbxStatus);
 
-	job = lpfc_bsg_issue_mbox_ext_handle_job(phba, pmboxq);
-
 	/* free all memory, including dma buffers */
 	mempool_free(pmboxq, phba->mbox_mem_pool);
 	lpfc_bsg_mbox_ext_session_reset(phba);
 
-	/* complete the bsg job if we have it */
+	/* if the job is still active, call job done */
 	if (job)
 		job->job_done(job);
 
@@ -3759,9 +3848,9 @@ lpfc_bsg_sli_cfg_read_cmd_ext(struct lpfc_hba *phba, struct fc_bsg_job *job,
 	/* context fields to callback function */
 	pmboxq->context1 = dd_data;
 	dd_data->type = TYPE_MBOX;
+	dd_data->set_job = job;
 	dd_data->context_un.mbox.pmboxq = pmboxq;
 	dd_data->context_un.mbox.mb = (MAILBOX_t *)pmbx;
-	dd_data->context_un.mbox.set_job = job;
 	job->dd_data = dd_data;
 
 	/* state change */
@@ -3928,14 +4017,14 @@ lpfc_bsg_sli_cfg_write_cmd_ext(struct lpfc_hba *phba, struct fc_bsg_job *job,
 		/* context fields to callback function */
 		pmboxq->context1 = dd_data;
 		dd_data->type = TYPE_MBOX;
+		dd_data->set_job = job;
 		dd_data->context_un.mbox.pmboxq = pmboxq;
 		dd_data->context_un.mbox.mb = (MAILBOX_t *)mbx;
-		dd_data->context_un.mbox.set_job = job;
 		job->dd_data = dd_data;
 
 		/* state change */
-		phba->mbox_ext_buf_ctx.state = LPFC_BSG_MBOX_PORT;
 
+		phba->mbox_ext_buf_ctx.state = LPFC_BSG_MBOX_PORT;
 		rc = lpfc_sli_issue_mbox(phba, pmboxq, MBX_NOWAIT);
 		if ((rc == MBX_SUCCESS) || (rc == MBX_BUSY)) {
 			lpfc_printf_log(phba, KERN_INFO, LOG_LIBDFC,
@@ -3951,6 +4040,7 @@ lpfc_bsg_sli_cfg_write_cmd_ext(struct lpfc_hba *phba, struct fc_bsg_job *job,
 	}
 
 	/* wait for additoinal external buffers */
+
 	job->reply->result = 0;
 	job->job_done(job);
 	return SLI_CONFIG_HANDLED;
@@ -4268,9 +4358,9 @@ lpfc_bsg_write_ebuf_set(struct lpfc_hba *phba, struct fc_bsg_job *job,
 		/* context fields to callback function */
 		pmboxq->context1 = dd_data;
 		dd_data->type = TYPE_MBOX;
+		dd_data->set_job = job;
 		dd_data->context_un.mbox.pmboxq = pmboxq;
 		dd_data->context_un.mbox.mb = (MAILBOX_t *)pbuf;
-		dd_data->context_un.mbox.set_job = job;
 		job->dd_data = dd_data;
 
 		/* state change */
@@ -4455,7 +4545,6 @@ lpfc_bsg_issue_mbox(struct lpfc_hba *phba, struct fc_bsg_job *job,
 	uint8_t *from;
 	uint32_t size;
 
-
 	/* in case no data is transferred */
 	job->reply->reply_payload_rcv_len = 0;
 
@@ -4681,9 +4770,9 @@ lpfc_bsg_issue_mbox(struct lpfc_hba *phba, struct fc_bsg_job *job,
 	/* setup context field to pass wait_queue pointer to wake function */
 	pmboxq->context1 = dd_data;
 	dd_data->type = TYPE_MBOX;
+	dd_data->set_job = job;
 	dd_data->context_un.mbox.pmboxq = pmboxq;
 	dd_data->context_un.mbox.mb = (MAILBOX_t *)pmbx;
-	dd_data->context_un.mbox.set_job = job;
 	dd_data->context_un.mbox.ext = ext;
 	dd_data->context_un.mbox.mbOffset = mbox_req->mbOffset;
 	dd_data->context_un.mbox.inExtWLen = mbox_req->inExtWLen;
@@ -4741,7 +4830,7 @@ lpfc_bsg_mbox_cmd(struct fc_bsg_job *job)
 	if (job->request_len <
 	    sizeof(struct fc_bsg_request) + sizeof(struct dfc_mbox_req)) {
 		lpfc_printf_log(phba, KERN_INFO, LOG_LIBDFC,
-				"2737 Mix-and-match backward compability "
+				"2737 Mix-and-match backward compatibility "
 				"between MBOX_REQ old size:%d and "
 				"new request size:%d\n",
 				(int)(job->request_len -
@@ -4797,75 +4886,79 @@ lpfc_bsg_menlo_cmd_cmp(struct lpfc_hba *phba,
 	struct bsg_job_data *dd_data;
 	struct fc_bsg_job *job;
 	IOCB_t *rsp;
-	struct lpfc_dmabuf *bmp;
+	struct lpfc_dmabuf *bmp, *cmp, *rmp;
 	struct lpfc_bsg_menlo *menlo;
 	unsigned long flags;
 	struct menlo_response *menlo_resp;
+	unsigned int rsp_size;
 	int rc = 0;
 
-	spin_lock_irqsave(&phba->ct_ev_lock, flags);
 	dd_data = cmdiocbq->context1;
-	if (!dd_data) {
-		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
-		return;
-	}
-
+	cmp = cmdiocbq->context2;
+	bmp = cmdiocbq->context3;
 	menlo = &dd_data->context_un.menlo;
-	job = menlo->set_job;
-	job->dd_data = NULL; /* so timeout handler does not reply */
-
-	spin_lock(&phba->hbalock);
-	cmdiocbq->iocb_flag |= LPFC_IO_WAKE;
-	if (cmdiocbq->context2 && rspiocbq)
-		memcpy(&((struct lpfc_iocbq *)cmdiocbq->context2)->iocb,
-		       &rspiocbq->iocb, sizeof(IOCB_t));
-	spin_unlock(&phba->hbalock);
-
-	bmp = menlo->bmp;
-	rspiocbq = menlo->rspiocbq;
+	rmp = menlo->rmp;
 	rsp = &rspiocbq->iocb;
 
-	pci_unmap_sg(phba->pcidev, job->request_payload.sg_list,
-		     job->request_payload.sg_cnt, DMA_TO_DEVICE);
-	pci_unmap_sg(phba->pcidev, job->reply_payload.sg_list,
-		     job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
+	/* Determine if job has been aborted */
+	spin_lock_irqsave(&phba->ct_ev_lock, flags);
+	job = dd_data->set_job;
+	if (job) {
+		/* Prevent timeout handling from trying to abort job  */
+		job->dd_data = NULL;
+	}
+	spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
+
+	/* Copy the job data or set the failing status for the job */
 
-	/* always return the xri, this would be used in the case
-	 * of a menlo download to allow the data to be sent as a continuation
-	 * of the exchange.
-	 */
-	menlo_resp = (struct menlo_response *)
-		job->reply->reply_data.vendor_reply.vendor_rsp;
-	menlo_resp->xri = rsp->ulpContext;
-	if (rsp->ulpStatus) {
-		if (rsp->ulpStatus == IOSTAT_LOCAL_REJECT) {
-			switch (rsp->un.ulpWord[4] & IOERR_PARAM_MASK) {
-			case IOERR_SEQUENCE_TIMEOUT:
-				rc = -ETIMEDOUT;
-				break;
-			case IOERR_INVALID_RPI:
-				rc = -EFAULT;
-				break;
-			default:
+	if (job) {
+		/* always return the xri, this would be used in the case
+		 * of a menlo download to allow the data to be sent as a
+		 * continuation of the exchange.
+		 */
+
+		menlo_resp = (struct menlo_response *)
+			job->reply->reply_data.vendor_reply.vendor_rsp;
+		menlo_resp->xri = rsp->ulpContext;
+		if (rsp->ulpStatus) {
+			if (rsp->ulpStatus == IOSTAT_LOCAL_REJECT) {
+				switch (rsp->un.ulpWord[4] & IOERR_PARAM_MASK) {
+				case IOERR_SEQUENCE_TIMEOUT:
+					rc = -ETIMEDOUT;
+					break;
+				case IOERR_INVALID_RPI:
+					rc = -EFAULT;
+					break;
+				default:
+					rc = -EACCES;
+					break;
+				}
+			} else {
 				rc = -EACCES;
-				break;
 			}
-		} else
-			rc = -EACCES;
-	} else
-		job->reply->reply_payload_rcv_len =
-			rsp->un.genreq64.bdl.bdeSize;
+		} else {
+			rsp_size = rsp->un.genreq64.bdl.bdeSize;
+			job->reply->reply_payload_rcv_len =
+				lpfc_bsg_copy_data(rmp, &job->reply_payload,
+						   rsp_size, 0);
+		}
+
+	}
 
-	lpfc_mbuf_free(phba, bmp->virt, bmp->phys);
-	lpfc_sli_release_iocbq(phba, rspiocbq);
 	lpfc_sli_release_iocbq(phba, cmdiocbq);
+	lpfc_free_bsg_buffers(phba, cmp);
+	lpfc_free_bsg_buffers(phba, rmp);
+	lpfc_mbuf_free(phba, bmp->virt, bmp->phys);
 	kfree(bmp);
 	kfree(dd_data);
-	/* make error code available to userspace */
-	job->reply->result = rc;
-	/* complete the job back to userspace */
-	job->job_done(job);
-	spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
+
+	/* Complete the job if active */
+
+	if (job) {
+		job->reply->result = rc;
+		job->job_done(job);
+	}
+
 	return;
 }
 
@@ -4883,17 +4976,14 @@ lpfc_menlo_cmd(struct fc_bsg_job *job)
 {
 	struct lpfc_vport *vport = (struct lpfc_vport *)job->shost->hostdata;
 	struct lpfc_hba *phba = vport->phba;
-	struct lpfc_iocbq *cmdiocbq, *rspiocbq;
-	IOCB_t *cmd, *rsp;
+	struct lpfc_iocbq *cmdiocbq;
+	IOCB_t *cmd;
 	int rc = 0;
 	struct menlo_command *menlo_cmd;
 	struct menlo_response *menlo_resp;
-	struct lpfc_dmabuf *bmp = NULL;
+	struct lpfc_dmabuf *bmp = NULL, *cmp = NULL, *rmp = NULL;
 	int request_nseg;
 	int reply_nseg;
-	struct scatterlist *sgel = NULL;
-	int numbde;
-	dma_addr_t busaddr;
 	struct bsg_job_data *dd_data;
 	struct ulp_bde64 *bpl = NULL;
 
@@ -4948,50 +5038,38 @@ lpfc_menlo_cmd(struct fc_bsg_job *job)
 		goto free_dd;
 	}
 
-	cmdiocbq = lpfc_sli_get_iocbq(phba);
-	if (!cmdiocbq) {
+	bmp->virt = lpfc_mbuf_alloc(phba, 0, &bmp->phys);
+	if (!bmp->virt) {
 		rc = -ENOMEM;
 		goto free_bmp;
 	}
 
-	rspiocbq = lpfc_sli_get_iocbq(phba);
-	if (!rspiocbq) {
-		rc = -ENOMEM;
-		goto free_cmdiocbq;
-	}
-
-	rsp = &rspiocbq->iocb;
+	INIT_LIST_HEAD(&bmp->list);
 
-	bmp->virt = lpfc_mbuf_alloc(phba, 0, &bmp->phys);
-	if (!bmp->virt) {
+	bpl = (struct ulp_bde64 *)bmp->virt;
+	request_nseg = LPFC_BPL_SIZE/sizeof(struct ulp_bde64);
+	cmp = lpfc_alloc_bsg_buffers(phba, job->request_payload.payload_len,
+				     1, bpl, &request_nseg);
+	if (!cmp) {
 		rc = -ENOMEM;
-		goto free_rspiocbq;
+		goto free_bmp;
 	}
+	lpfc_bsg_copy_data(cmp, &job->request_payload,
+			   job->request_payload.payload_len, 1);
 
-	INIT_LIST_HEAD(&bmp->list);
-	bpl = (struct ulp_bde64 *) bmp->virt;
-	request_nseg = pci_map_sg(phba->pcidev, job->request_payload.sg_list,
-				  job->request_payload.sg_cnt, DMA_TO_DEVICE);
-	for_each_sg(job->request_payload.sg_list, sgel, request_nseg, numbde) {
-		busaddr = sg_dma_address(sgel);
-		bpl->tus.f.bdeFlags = BUFF_TYPE_BDE_64;
-		bpl->tus.f.bdeSize = sg_dma_len(sgel);
-		bpl->tus.w = cpu_to_le32(bpl->tus.w);
-		bpl->addrLow = cpu_to_le32(putPaddrLow(busaddr));
-		bpl->addrHigh = cpu_to_le32(putPaddrHigh(busaddr));
-		bpl++;
+	bpl += request_nseg;
+	reply_nseg = LPFC_BPL_SIZE/sizeof(struct ulp_bde64) - request_nseg;
+	rmp = lpfc_alloc_bsg_buffers(phba, job->reply_payload.payload_len, 0,
+				     bpl, &reply_nseg);
+	if (!rmp) {
+		rc = -ENOMEM;
+		goto free_cmp;
 	}
 
-	reply_nseg = pci_map_sg(phba->pcidev, job->reply_payload.sg_list,
-				job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
-	for_each_sg(job->reply_payload.sg_list, sgel, reply_nseg, numbde) {
-		busaddr = sg_dma_address(sgel);
-		bpl->tus.f.bdeFlags = BUFF_TYPE_BDE_64I;
-		bpl->tus.f.bdeSize = sg_dma_len(sgel);
-		bpl->tus.w = cpu_to_le32(bpl->tus.w);
-		bpl->addrLow = cpu_to_le32(putPaddrLow(busaddr));
-		bpl->addrHigh = cpu_to_le32(putPaddrHigh(busaddr));
-		bpl++;
+	cmdiocbq = lpfc_sli_get_iocbq(phba);
+	if (!cmdiocbq) {
+		rc = -ENOMEM;
+		goto free_rmp;
 	}
 
 	cmd = &cmdiocbq->iocb;
@@ -5013,11 +5091,10 @@ lpfc_menlo_cmd(struct fc_bsg_job *job)
 	cmdiocbq->vport = phba->pport;
 	/* We want the firmware to timeout before we do */
 	cmd->ulpTimeout = MENLO_TIMEOUT - 5;
-	cmdiocbq->context3 = bmp;
-	cmdiocbq->context2 = rspiocbq;
 	cmdiocbq->iocb_cmpl = lpfc_bsg_menlo_cmd_cmp;
 	cmdiocbq->context1 = dd_data;
-	cmdiocbq->context2 = rspiocbq;
+	cmdiocbq->context2 = cmp;
+	cmdiocbq->context3 = bmp;
 	if (menlo_cmd->cmd == LPFC_BSG_VENDOR_MENLO_CMD) {
 		cmd->ulpCommand = CMD_GEN_REQUEST64_CR;
 		cmd->ulpPU = MENLO_PU; /* 3 */
@@ -5031,29 +5108,25 @@ lpfc_menlo_cmd(struct fc_bsg_job *job)
 	}
 
 	dd_data->type = TYPE_MENLO;
+	dd_data->set_job = job;
 	dd_data->context_un.menlo.cmdiocbq = cmdiocbq;
-	dd_data->context_un.menlo.rspiocbq = rspiocbq;
-	dd_data->context_un.menlo.set_job = job;
-	dd_data->context_un.menlo.bmp = bmp;
+	dd_data->context_un.menlo.rmp = rmp;
+	job->dd_data = dd_data;
 
 	rc = lpfc_sli_issue_iocb(phba, LPFC_ELS_RING, cmdiocbq,
 		MENLO_TIMEOUT - 5);
 	if (rc == IOCB_SUCCESS)
 		return 0; /* done for now */
 
-	/* iocb failed so cleanup */
-	pci_unmap_sg(phba->pcidev, job->request_payload.sg_list,
-		     job->request_payload.sg_cnt, DMA_TO_DEVICE);
-	pci_unmap_sg(phba->pcidev, job->reply_payload.sg_list,
-		     job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
-
-	lpfc_mbuf_free(phba, bmp->virt, bmp->phys);
-
-free_rspiocbq:
-	lpfc_sli_release_iocbq(phba, rspiocbq);
-free_cmdiocbq:
 	lpfc_sli_release_iocbq(phba, cmdiocbq);
+
+free_rmp:
+	lpfc_free_bsg_buffers(phba, rmp);
+free_cmp:
+	lpfc_free_bsg_buffers(phba, cmp);
 free_bmp:
+	if (bmp->virt)
+		lpfc_mbuf_free(phba, bmp->virt, bmp->phys);
 	kfree(bmp);
 free_dd:
 	kfree(dd_data);
@@ -5162,70 +5235,94 @@ lpfc_bsg_timeout(struct fc_bsg_job *job)
 	struct lpfc_vport *vport = (struct lpfc_vport *)job->shost->hostdata;
 	struct lpfc_hba *phba = vport->phba;
 	struct lpfc_iocbq *cmdiocb;
-	struct lpfc_bsg_event *evt;
-	struct lpfc_bsg_iocb *iocb;
-	struct lpfc_bsg_mbox *mbox;
-	struct lpfc_bsg_menlo *menlo;
 	struct lpfc_sli_ring *pring = &phba->sli.ring[LPFC_ELS_RING];
 	struct bsg_job_data *dd_data;
 	unsigned long flags;
+	int rc = 0;
+	LIST_HEAD(completions);
+	struct lpfc_iocbq *check_iocb, *next_iocb;
+
+	/* if job's driver data is NULL, the command completed or is in the
+	 * the process of completing.  In this case, return status to request
+	 * so the timeout is retried.  This avoids double completion issues
+	 * and the request will be pulled off the timer queue when the
+	 * command's completion handler executes.  Otherwise, prevent the
+	 * command's completion handler from executing the job done callback
+	 * and continue processing to abort the outstanding the command.
+	 */
 
 	spin_lock_irqsave(&phba->ct_ev_lock, flags);
 	dd_data = (struct bsg_job_data *)job->dd_data;
-	/* timeout and completion crossed paths if no dd_data */
-	if (!dd_data) {
+	if (dd_data) {
+		dd_data->set_job = NULL;
+		job->dd_data = NULL;
+	} else {
 		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
-		return 0;
+		return -EAGAIN;
 	}
 
 	switch (dd_data->type) {
 	case TYPE_IOCB:
-		iocb = &dd_data->context_un.iocb;
-		cmdiocb = iocb->cmdiocbq;
-		/* hint to completion handler that the job timed out */
-		job->reply->result = -EAGAIN;
-		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
-		/* this will call our completion handler */
+		/* Check to see if IOCB was issued to the port or not. If not,
+		 * remove it from the txq queue and call cancel iocbs.
+		 * Otherwise, call abort iotag
+		 */
+
+		cmdiocb = dd_data->context_un.iocb.cmdiocbq;
 		spin_lock_irq(&phba->hbalock);
-		lpfc_sli_issue_abort_iotag(phba, pring, cmdiocb);
+		list_for_each_entry_safe(check_iocb, next_iocb, &pring->txq,
+					 list) {
+			if (check_iocb == cmdiocb) {
+				list_move_tail(&check_iocb->list, &completions);
+				break;
+			}
+		}
+		if (list_empty(&completions))
+			lpfc_sli_issue_abort_iotag(phba, pring, cmdiocb);
 		spin_unlock_irq(&phba->hbalock);
+		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
+		if (!list_empty(&completions)) {
+			lpfc_sli_cancel_iocbs(phba, &completions,
+					      IOSTAT_LOCAL_REJECT,
+					      IOERR_SLI_ABORTED);
+		}
 		break;
+
 	case TYPE_EVT:
-		evt = dd_data->context_un.evt;
-		/* this event has no job anymore */
-		evt->set_job = NULL;
-		job->dd_data = NULL;
-		job->reply->reply_payload_rcv_len = 0;
-		/* Return -EAGAIN which is our way of signallying the
-		 * app to retry.
-		 */
-		job->reply->result = -EAGAIN;
 		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
-		job->job_done(job);
 		break;
+
 	case TYPE_MBOX:
-		mbox = &dd_data->context_un.mbox;
-		/* this mbox has no job anymore */
-		mbox->set_job = NULL;
-		job->dd_data = NULL;
-		job->reply->reply_payload_rcv_len = 0;
-		job->reply->result = -EAGAIN;
-		/* the mbox completion handler can now be run */
-		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
-		job->job_done(job);
+		/* Update the ext buf ctx state if needed */
+
 		if (phba->mbox_ext_buf_ctx.state == LPFC_BSG_MBOX_PORT)
 			phba->mbox_ext_buf_ctx.state = LPFC_BSG_MBOX_ABTS;
+		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
 		break;
 	case TYPE_MENLO:
-		menlo = &dd_data->context_un.menlo;
-		cmdiocb = menlo->cmdiocbq;
-		/* hint to completion handler that the job timed out */
-		job->reply->result = -EAGAIN;
-		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
-		/* this will call our completion handler */
+		/* Check to see if IOCB was issued to the port or not. If not,
+		 * remove it from the txq queue and call cancel iocbs.
+		 * Otherwise, call abort iotag.
+		 */
+
+		cmdiocb = dd_data->context_un.menlo.cmdiocbq;
 		spin_lock_irq(&phba->hbalock);
-		lpfc_sli_issue_abort_iotag(phba, pring, cmdiocb);
+		list_for_each_entry_safe(check_iocb, next_iocb, &pring->txq,
+					 list) {
+			if (check_iocb == cmdiocb) {
+				list_move_tail(&check_iocb->list, &completions);
+				break;
+			}
+		}
+		if (list_empty(&completions))
+			lpfc_sli_issue_abort_iotag(phba, pring, cmdiocb);
 		spin_unlock_irq(&phba->hbalock);
+		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
+		if (!list_empty(&completions)) {
+			lpfc_sli_cancel_iocbs(phba, &completions,
+					      IOSTAT_LOCAL_REJECT,
+					      IOERR_SLI_ABORTED);
+		}
 		break;
 	default:
 		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
@@ -5236,5 +5333,5 @@ lpfc_bsg_timeout(struct fc_bsg_job *job)
 	 * otherwise an error message will be displayed on the console
 	 * so always return success (zero)
 	 */
-	return 0;
+	return rc;
 }
diff --git a/drivers/scsi/lpfc/lpfc_crtn.h b/drivers/scsi/lpfc/lpfc_crtn.h
index 76ca65dae78..7631893ae00 100644
--- a/drivers/scsi/lpfc/lpfc_crtn.h
+++ b/drivers/scsi/lpfc/lpfc_crtn.h
@@ -106,6 +106,7 @@ void lpfc_cleanup_discovery_resources(struct lpfc_vport *);
 void lpfc_cleanup(struct lpfc_vport *);
 void lpfc_disc_timeout(unsigned long);
 
+int lpfc_unregister_fcf_prep(struct lpfc_hba *);
 struct lpfc_nodelist *__lpfc_findnode_rpi(struct lpfc_vport *, uint16_t);
 struct lpfc_nodelist *lpfc_findnode_rpi(struct lpfc_vport *, uint16_t);
 void lpfc_worker_wake_up(struct lpfc_hba *);
diff --git a/drivers/scsi/lpfc/lpfc_els.c b/drivers/scsi/lpfc/lpfc_els.c
index 08d156a9094..bbed8471bf0 100644
--- a/drivers/scsi/lpfc/lpfc_els.c
+++ b/drivers/scsi/lpfc/lpfc_els.c
@@ -484,6 +484,7 @@ lpfc_issue_reg_vfi(struct lpfc_vport *vport)
 	vport->port_state = LPFC_FABRIC_CFG_LINK;
 	memcpy(dmabuf->virt, &phba->fc_fabparam, sizeof(vport->fc_sparam));
 	lpfc_reg_vfi(mboxq, vport, dmabuf->phys);
+
 	mboxq->mbox_cmpl = lpfc_mbx_cmpl_reg_vfi;
 	mboxq->vport = vport;
 	mboxq->context1 = dmabuf;
@@ -700,6 +701,20 @@ lpfc_cmpl_els_flogi_fabric(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 		}
 	}
 
+	/*
+	 * For FC we need to do some special processing because of the SLI
+	 * Port's default settings of the Common Service Parameters.
+	 */
+	if (phba->sli4_hba.lnk_info.lnk_tp == LPFC_LNK_TYPE_FC) {
+		/* If physical FC port changed, unreg VFI and ALL VPIs / RPIs */
+		if ((phba->sli_rev == LPFC_SLI_REV4) && fabric_param_changed)
+			lpfc_unregister_fcf_prep(phba);
+
+		/* This should just update the VFI CSPs*/
+		if (vport->fc_flag & FC_VFI_REGISTERED)
+			lpfc_issue_reg_vfi(vport);
+	}
+
 	if (fabric_param_changed &&
 		!(vport->fc_flag & FC_VPORT_NEEDS_REG_VPI)) {
 
@@ -6225,7 +6240,7 @@ lpfc_els_timeout_handler(struct lpfc_vport *vport)
 		spin_unlock_irq(&phba->hbalock);
 	}
 
-	if (phba->sli.ring[LPFC_ELS_RING].txcmplq_cnt)
+	if (!list_empty(&phba->sli.ring[LPFC_ELS_RING].txcmplq))
 		mod_timer(&vport->els_tmofunc, jiffies + HZ * timeout);
 }
 
@@ -6279,7 +6294,6 @@ lpfc_els_flush_cmd(struct lpfc_vport *vport)
 			continue;
 
 		list_move_tail(&piocb->list, &completions);
-		pring->txq_cnt--;
 	}
 
 	list_for_each_entry_safe(piocb, tmp_iocb, &pring->txcmplq, list) {
@@ -6339,7 +6353,6 @@ lpfc_els_flush_all_cmd(struct lpfc_hba  *phba)
 		    cmd->ulpCommand == CMD_ABORT_XRI_CN)
 			continue;
 		list_move_tail(&piocb->list, &completions);
-		pring->txq_cnt--;
 	}
 	list_for_each_entry_safe(piocb, tmp_iocb, &pring->txcmplq, list) {
 		if (piocb->iocb_flag & LPFC_IO_LIBDFC)
@@ -8065,7 +8078,7 @@ lpfc_sli4_els_xri_aborted(struct lpfc_hba *phba,
 				rxid, 1);
 
 			/* Check if TXQ queue needs to be serviced */
-			if (pring->txq_cnt)
+			if (!(list_empty(&pring->txq)))
 				lpfc_worker_wake_up(phba);
 			return;
 		}
diff --git a/drivers/scsi/lpfc/lpfc_hbadisc.c b/drivers/scsi/lpfc/lpfc_hbadisc.c
index d7096ad94d3..326e05a65a7 100644
--- a/drivers/scsi/lpfc/lpfc_hbadisc.c
+++ b/drivers/scsi/lpfc/lpfc_hbadisc.c
@@ -691,12 +691,15 @@ lpfc_work_done(struct lpfc_hba *phba)
 			/* Set the lpfc data pending flag */
 			set_bit(LPFC_DATA_READY, &phba->data_flags);
 		} else {
-			pring->flag &= ~LPFC_DEFERRED_RING_EVENT;
-			lpfc_sli_handle_slow_ring_event(phba, pring,
-							(status &
-							 HA_RXMASK));
+			if (phba->link_state >= LPFC_LINK_UP) {
+				pring->flag &= ~LPFC_DEFERRED_RING_EVENT;
+				lpfc_sli_handle_slow_ring_event(phba, pring,
+								(status &
+								HA_RXMASK));
+			}
 		}
-		if ((phba->sli_rev == LPFC_SLI_REV4) && pring->txq_cnt)
+		if ((phba->sli_rev == LPFC_SLI_REV4) &
+				 (!list_empty(&pring->txq)))
 			lpfc_drain_txq(phba);
 		/*
 		 * Turn on Ring interrupts
@@ -1732,7 +1735,7 @@ lpfc_check_pending_fcoe_event(struct lpfc_hba *phba, uint8_t unreg_fcf)
  * use through a sequence of @fcf_cnt eligible FCF records with equal
  * probability. To perform integer manunipulation of random numbers with
  * size unit32_t, the lower 16 bits of the 32-bit random number returned
- * from random32() are taken as the random random number generated.
+ * from prandom_u32() are taken as the random random number generated.
  *
  * Returns true when outcome is for the newly read FCF record should be
  * chosen; otherwise, return false when outcome is for keeping the previously
@@ -1744,7 +1747,7 @@ lpfc_sli4_new_fcf_random_select(struct lpfc_hba *phba, uint32_t fcf_cnt)
 	uint32_t rand_num;
 
 	/* Get 16-bit uniform random number */
-	rand_num = (0xFFFF & random32());
+	rand_num = 0xFFFF & prandom_u32();
 
 	/* Decision with probability 1/fcf_cnt */
 	if ((fcf_cnt * rand_num) < 0xFFFF)
@@ -1792,6 +1795,8 @@ lpfc_sli4_fcf_rec_mbox_parse(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq,
 	virt_addr = mboxq->sge_array->addr[0];
 
 	shdr = (union lpfc_sli4_cfg_shdr *)virt_addr;
+	lpfc_sli_pcimem_bcopy(shdr, shdr,
+			      sizeof(union lpfc_sli4_cfg_shdr));
 	shdr_status = bf_get(lpfc_mbox_hdr_status, &shdr->response);
 	shdr_add_status = bf_get(lpfc_mbox_hdr_add_status, &shdr->response);
 	if (shdr_status || shdr_add_status) {
@@ -2380,7 +2385,7 @@ lpfc_mbx_cmpl_fcf_scan_read_fcf_rec(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
 		phba->fcf.eligible_fcf_cnt = 1;
 		/* Seeding the random number generator for random selection */
 		seed = (uint32_t)(0xFFFFFFFF & jiffies);
-		srandom32(seed);
+		prandom_seed(seed);
 	}
 	spin_unlock_irq(&phba->hbalock);
 	goto read_next_fcf;
@@ -2888,6 +2893,11 @@ lpfc_mbx_cmpl_reg_vfi(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
 		lpfc_vport_set_state(vport, FC_VPORT_FAILED);
 		goto out_free_mem;
 	}
+
+	/* If the VFI is already registered, there is nothing else to do */
+	if (vport->fc_flag & FC_VFI_REGISTERED)
+		goto out_free_mem;
+
 	/* The VPI is implicitly registered when the VFI is registered */
 	spin_lock_irq(shost->host_lock);
 	vport->vpi_state |= LPFC_VPI_REGISTERED;
@@ -2980,6 +2990,7 @@ lpfc_mbx_process_link_up(struct lpfc_hba *phba, struct lpfc_mbx_read_top *la)
 	struct lpfc_dmabuf *mp;
 	int rc;
 	struct fcf_record *fcf_record;
+	uint32_t fc_flags = 0;
 
 	spin_lock_irq(&phba->hbalock);
 	switch (bf_get(lpfc_mbx_read_top_link_spd, la)) {
@@ -3011,11 +3022,8 @@ lpfc_mbx_process_link_up(struct lpfc_hba *phba, struct lpfc_mbx_read_top *la)
 				"1309 Link Up Event npiv not supported in loop "
 				"topology\n");
 				/* Get Loop Map information */
-		if (bf_get(lpfc_mbx_read_top_il, la)) {
-			spin_lock(shost->host_lock);
-			vport->fc_flag |= FC_LBIT;
-			spin_unlock(shost->host_lock);
-		}
+		if (bf_get(lpfc_mbx_read_top_il, la))
+			fc_flags |= FC_LBIT;
 
 		vport->fc_myDID = bf_get(lpfc_mbx_read_top_alpa_granted, la);
 		i = la->lilpBde64.tus.f.bdeSize;
@@ -3064,12 +3072,16 @@ lpfc_mbx_process_link_up(struct lpfc_hba *phba, struct lpfc_mbx_read_top *la)
 				phba->sli3_options |= LPFC_SLI3_NPIV_ENABLED;
 		}
 		vport->fc_myDID = phba->fc_pref_DID;
-		spin_lock(shost->host_lock);
-		vport->fc_flag |= FC_LBIT;
-		spin_unlock(shost->host_lock);
+		fc_flags |= FC_LBIT;
 	}
 	spin_unlock_irq(&phba->hbalock);
 
+	if (fc_flags) {
+		spin_lock_irq(shost->host_lock);
+		vport->fc_flag |= fc_flags;
+		spin_unlock_irq(shost->host_lock);
+	}
+
 	lpfc_linkup(phba);
 	sparam_mbox = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
 	if (!sparam_mbox)
@@ -3237,8 +3249,7 @@ lpfc_mbx_cmpl_read_topology(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
 		vport->fc_flag &= ~FC_BYPASSED_MODE;
 	spin_unlock_irq(shost->host_lock);
 
-	if ((phba->fc_eventTag  < la->eventTag) ||
-	    (phba->fc_eventTag == la->eventTag)) {
+	if (phba->fc_eventTag <= la->eventTag) {
 		phba->fc_stat.LinkMultiEvent++;
 		if (bf_get(lpfc_mbx_read_top_att_type, la) == LPFC_ATT_LINK_UP)
 			if (phba->fc_eventTag != 0)
@@ -3246,16 +3257,18 @@ lpfc_mbx_cmpl_read_topology(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
 	}
 
 	phba->fc_eventTag = la->eventTag;
-	spin_lock_irq(&phba->hbalock);
-	if (bf_get(lpfc_mbx_read_top_mm, la))
-		phba->sli.sli_flag |= LPFC_MENLO_MAINT;
-	else
-		phba->sli.sli_flag &= ~LPFC_MENLO_MAINT;
-	spin_unlock_irq(&phba->hbalock);
+	if (phba->sli_rev < LPFC_SLI_REV4) {
+		spin_lock_irq(&phba->hbalock);
+		if (bf_get(lpfc_mbx_read_top_mm, la))
+			phba->sli.sli_flag |= LPFC_MENLO_MAINT;
+		else
+			phba->sli.sli_flag &= ~LPFC_MENLO_MAINT;
+		spin_unlock_irq(&phba->hbalock);
+	}
 
 	phba->link_events++;
 	if ((bf_get(lpfc_mbx_read_top_att_type, la) == LPFC_ATT_LINK_UP) &&
-	    (!bf_get(lpfc_mbx_read_top_mm, la))) {
+	    !(phba->sli.sli_flag & LPFC_MENLO_MAINT)) {
 		phba->fc_stat.LinkUp++;
 		if (phba->link_flag & LS_LOOPBACK_MODE) {
 			lpfc_printf_log(phba, KERN_ERR, LOG_LINK_EVENT,
@@ -3300,8 +3313,8 @@ lpfc_mbx_cmpl_read_topology(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
 				bf_get(lpfc_mbx_read_top_fa, la));
 		lpfc_mbx_issue_link_down(phba);
 	}
-	if ((bf_get(lpfc_mbx_read_top_mm, la)) &&
-	    (bf_get(lpfc_mbx_read_top_att_type, la) == LPFC_ATT_LINK_UP)) {
+	if ((phba->sli.sli_flag & LPFC_MENLO_MAINT) &&
+	    ((bf_get(lpfc_mbx_read_top_att_type, la) == LPFC_ATT_LINK_UP))) {
 		if (phba->link_state != LPFC_LINK_DOWN) {
 			phba->fc_stat.LinkDown++;
 			lpfc_printf_log(phba, KERN_ERR, LOG_LINK_EVENT,
@@ -3329,8 +3342,9 @@ lpfc_mbx_cmpl_read_topology(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
 		}
 	}
 
-	if (bf_get(lpfc_mbx_read_top_fa, la)) {
-		if (bf_get(lpfc_mbx_read_top_mm, la))
+	if ((phba->sli_rev < LPFC_SLI_REV4) &&
+	    bf_get(lpfc_mbx_read_top_fa, la)) {
+		if (phba->sli.sli_flag & LPFC_MENLO_MAINT)
 			lpfc_issue_clear_la(phba, vport);
 		lpfc_printf_log(phba, KERN_INFO, LOG_LINK_EVENT,
 				"1311 fa %d\n",
@@ -4354,7 +4368,6 @@ lpfc_no_rpi(struct lpfc_hba *phba, struct lpfc_nodelist *ndlp)
 					   with an error */
 					list_move_tail(&iocb->list,
 						       &completions);
-					pring->txq_cnt--;
 				}
 			}
 			spin_unlock_irq(&phba->hbalock);
@@ -5055,7 +5068,6 @@ lpfc_free_tx(struct lpfc_hba *phba, struct lpfc_nodelist *ndlp)
 		    (icmd->ulpCommand == CMD_XMIT_ELS_RSP64_CX)) {
 
 			list_move_tail(&iocb->list, &completions);
-			pring->txq_cnt--;
 		}
 	}
 
diff --git a/drivers/scsi/lpfc/lpfc_hw4.h b/drivers/scsi/lpfc/lpfc_hw4.h
index 6e93b886cd4..1dd2f6f0a12 100644
--- a/drivers/scsi/lpfc/lpfc_hw4.h
+++ b/drivers/scsi/lpfc/lpfc_hw4.h
@@ -1958,6 +1958,9 @@ struct lpfc_mbx_init_vfi {
 
 struct lpfc_mbx_reg_vfi {
 	uint32_t word1;
+#define lpfc_reg_vfi_upd_SHIFT		29
+#define lpfc_reg_vfi_upd_MASK		0x00000001
+#define lpfc_reg_vfi_upd_WORD		word1
 #define lpfc_reg_vfi_vp_SHIFT		28
 #define lpfc_reg_vfi_vp_MASK		0x00000001
 #define lpfc_reg_vfi_vp_WORD		word1
diff --git a/drivers/scsi/lpfc/lpfc_init.c b/drivers/scsi/lpfc/lpfc_init.c
index 314b4f61b9e..5da29729026 100644
--- a/drivers/scsi/lpfc/lpfc_init.c
+++ b/drivers/scsi/lpfc/lpfc_init.c
@@ -839,7 +839,6 @@ lpfc_hba_down_post_s3(struct lpfc_hba *phba)
 		 * way, nothing should be on txcmplq as it will NEVER complete.
 		 */
 		list_splice_init(&pring->txcmplq, &completions);
-		pring->txcmplq_cnt = 0;
 		spin_unlock_irq(&phba->hbalock);
 
 		/* Cancel all the IOCBs from the completions list */
@@ -2915,9 +2914,9 @@ lpfc_sli4_xri_sgl_update(struct lpfc_hba *phba)
 			sglq_entry->state = SGL_FREED;
 			list_add_tail(&sglq_entry->list, &els_sgl_list);
 		}
-		spin_lock(&phba->hbalock);
+		spin_lock_irq(&phba->hbalock);
 		list_splice_init(&els_sgl_list, &phba->sli4_hba.lpfc_sgl_list);
-		spin_unlock(&phba->hbalock);
+		spin_unlock_irq(&phba->hbalock);
 	} else if (els_xri_cnt < phba->sli4_hba.els_xri_cnt) {
 		/* els xri-sgl shrinked */
 		xri_cnt = phba->sli4_hba.els_xri_cnt - els_xri_cnt;
@@ -3015,9 +3014,9 @@ lpfc_sli4_xri_sgl_update(struct lpfc_hba *phba)
 		psb->cur_iocbq.sli4_lxritag = lxri;
 		psb->cur_iocbq.sli4_xritag = phba->sli4_hba.xri_ids[lxri];
 	}
-	spin_lock(&phba->scsi_buf_list_lock);
+	spin_lock_irq(&phba->scsi_buf_list_lock);
 	list_splice_init(&scsi_sgl_list, &phba->lpfc_scsi_buf_list);
-	spin_unlock(&phba->scsi_buf_list_lock);
+	spin_unlock_irq(&phba->scsi_buf_list_lock);
 
 	return 0;
 
@@ -4004,6 +4003,52 @@ lpfc_sli4_perform_all_vport_cvl(struct lpfc_hba *phba)
 }
 
 /**
+ * lpfc_sli4_perform_inuse_fcf_recovery - Perform inuse fcf recovery
+ * @vport: pointer to lpfc hba data structure.
+ *
+ * This routine is to perform FCF recovery when the in-use FCF either dead or
+ * got modified.
+ **/
+static void
+lpfc_sli4_perform_inuse_fcf_recovery(struct lpfc_hba *phba,
+				     struct lpfc_acqe_fip *acqe_fip)
+{
+	int rc;
+
+	spin_lock_irq(&phba->hbalock);
+	/* Mark the fast failover process in progress */
+	phba->fcf.fcf_flag |= FCF_DEAD_DISC;
+	spin_unlock_irq(&phba->hbalock);
+
+	lpfc_printf_log(phba, KERN_INFO, LOG_FIP | LOG_DISCOVERY,
+			"2771 Start FCF fast failover process due to in-use "
+			"FCF DEAD/MODIFIED event: evt_tag:x%x, index:x%x\n",
+			acqe_fip->event_tag, acqe_fip->index);
+	rc = lpfc_sli4_redisc_fcf_table(phba);
+	if (rc) {
+		lpfc_printf_log(phba, KERN_ERR, LOG_FIP | LOG_DISCOVERY,
+				"2772 Issue FCF rediscover mabilbox command "
+				"failed, fail through to FCF dead event\n");
+		spin_lock_irq(&phba->hbalock);
+		phba->fcf.fcf_flag &= ~FCF_DEAD_DISC;
+		spin_unlock_irq(&phba->hbalock);
+		/*
+		 * Last resort will fail over by treating this as a link
+		 * down to FCF registration.
+		 */
+		lpfc_sli4_fcf_dead_failthrough(phba);
+	} else {
+		/* Reset FCF roundrobin bmask for new discovery */
+		lpfc_sli4_clear_fcf_rr_bmask(phba);
+		/*
+		 * Handling fast FCF failover to a DEAD FCF event is
+		 * considered equalivant to receiving CVL to all vports.
+		 */
+		lpfc_sli4_perform_all_vport_cvl(phba);
+	}
+}
+
+/**
  * lpfc_sli4_async_fip_evt - Process the asynchronous FCoE FIP event
  * @phba: pointer to lpfc hba data structure.
  * @acqe_link: pointer to the async fcoe completion queue entry.
@@ -4068,9 +4113,22 @@ lpfc_sli4_async_fip_evt(struct lpfc_hba *phba,
 			break;
 		}
 
-		/* If the FCF has been in discovered state, do nothing. */
-		if (phba->fcf.fcf_flag & FCF_SCAN_DONE) {
+		/* If FCF has been in discovered state, perform rediscovery
+		 * only if the FCF with the same index of the in-use FCF got
+		 * modified during normal operation. Otherwise, do nothing.
+		 */
+		if (phba->pport->port_state > LPFC_FLOGI) {
 			spin_unlock_irq(&phba->hbalock);
+			if (phba->fcf.current_rec.fcf_indx ==
+			    acqe_fip->index) {
+				lpfc_printf_log(phba, KERN_ERR, LOG_FIP,
+						"3300 In-use FCF (%d) "
+						"modified, perform FCF "
+						"rediscovery\n",
+						acqe_fip->index);
+				lpfc_sli4_perform_inuse_fcf_recovery(phba,
+								     acqe_fip);
+			}
 			break;
 		}
 		spin_unlock_irq(&phba->hbalock);
@@ -4123,39 +4181,7 @@ lpfc_sli4_async_fip_evt(struct lpfc_hba *phba,
 		 * is no longer valid as we are not in the middle of FCF
 		 * failover process already.
 		 */
-		spin_lock_irq(&phba->hbalock);
-		/* Mark the fast failover process in progress */
-		phba->fcf.fcf_flag |= FCF_DEAD_DISC;
-		spin_unlock_irq(&phba->hbalock);
-
-		lpfc_printf_log(phba, KERN_INFO, LOG_FIP | LOG_DISCOVERY,
-				"2771 Start FCF fast failover process due to "
-				"FCF DEAD event: evt_tag:x%x, fcf_index:x%x "
-				"\n", acqe_fip->event_tag, acqe_fip->index);
-		rc = lpfc_sli4_redisc_fcf_table(phba);
-		if (rc) {
-			lpfc_printf_log(phba, KERN_ERR, LOG_FIP |
-					LOG_DISCOVERY,
-					"2772 Issue FCF rediscover mabilbox "
-					"command failed, fail through to FCF "
-					"dead event\n");
-			spin_lock_irq(&phba->hbalock);
-			phba->fcf.fcf_flag &= ~FCF_DEAD_DISC;
-			spin_unlock_irq(&phba->hbalock);
-			/*
-			 * Last resort will fail over by treating this
-			 * as a link down to FCF registration.
-			 */
-			lpfc_sli4_fcf_dead_failthrough(phba);
-		} else {
-			/* Reset FCF roundrobin bmask for new discovery */
-			lpfc_sli4_clear_fcf_rr_bmask(phba);
-			/*
-			 * Handling fast FCF failover to a DEAD FCF event is
-			 * considered equalivant to receiving CVL to all vports.
-			 */
-			lpfc_sli4_perform_all_vport_cvl(phba);
-		}
+		lpfc_sli4_perform_inuse_fcf_recovery(phba, acqe_fip);
 		break;
 	case LPFC_FIP_EVENT_TYPE_CVL:
 		phba->fcoe_cvl_eventtag = acqe_fip->event_tag;
diff --git a/drivers/scsi/lpfc/lpfc_mbox.c b/drivers/scsi/lpfc/lpfc_mbox.c
index efc9cd9def8..a7a9fa46830 100644
--- a/drivers/scsi/lpfc/lpfc_mbox.c
+++ b/drivers/scsi/lpfc/lpfc_mbox.c
@@ -2126,32 +2126,40 @@ void
 lpfc_reg_vfi(struct lpfcMboxq *mbox, struct lpfc_vport *vport, dma_addr_t phys)
 {
 	struct lpfc_mbx_reg_vfi *reg_vfi;
+	struct lpfc_hba *phba = vport->phba;
 
 	memset(mbox, 0, sizeof(*mbox));
 	reg_vfi = &mbox->u.mqe.un.reg_vfi;
 	bf_set(lpfc_mqe_command, &mbox->u.mqe, MBX_REG_VFI);
 	bf_set(lpfc_reg_vfi_vp, reg_vfi, 1);
 	bf_set(lpfc_reg_vfi_vfi, reg_vfi,
-	       vport->phba->sli4_hba.vfi_ids[vport->vfi]);
-	bf_set(lpfc_reg_vfi_fcfi, reg_vfi, vport->phba->fcf.fcfi);
-	bf_set(lpfc_reg_vfi_vpi, reg_vfi, vport->phba->vpi_ids[vport->vpi]);
+	       phba->sli4_hba.vfi_ids[vport->vfi]);
+	bf_set(lpfc_reg_vfi_fcfi, reg_vfi, phba->fcf.fcfi);
+	bf_set(lpfc_reg_vfi_vpi, reg_vfi, phba->vpi_ids[vport->vpi]);
 	memcpy(reg_vfi->wwn, &vport->fc_portname, sizeof(struct lpfc_name));
 	reg_vfi->wwn[0] = cpu_to_le32(reg_vfi->wwn[0]);
 	reg_vfi->wwn[1] = cpu_to_le32(reg_vfi->wwn[1]);
-	reg_vfi->e_d_tov = vport->phba->fc_edtov;
-	reg_vfi->r_a_tov = vport->phba->fc_ratov;
+	reg_vfi->e_d_tov = phba->fc_edtov;
+	reg_vfi->r_a_tov = phba->fc_ratov;
 	reg_vfi->bde.addrHigh = putPaddrHigh(phys);
 	reg_vfi->bde.addrLow = putPaddrLow(phys);
 	reg_vfi->bde.tus.f.bdeSize = sizeof(vport->fc_sparam);
 	reg_vfi->bde.tus.f.bdeFlags = BUFF_TYPE_BDE_64;
 	bf_set(lpfc_reg_vfi_nport_id, reg_vfi, vport->fc_myDID);
+
+	/* Only FC supports upd bit */
+	if ((phba->sli4_hba.lnk_info.lnk_tp == LPFC_LNK_TYPE_FC) &&
+	    (vport->fc_flag & FC_VFI_REGISTERED)) {
+		bf_set(lpfc_reg_vfi_vp, reg_vfi, 0);
+		bf_set(lpfc_reg_vfi_upd, reg_vfi, 1);
+	}
 	lpfc_printf_vlog(vport, KERN_INFO, LOG_MBOX,
 			"3134 Register VFI, mydid:x%x, fcfi:%d, "
 			" vfi:%d, vpi:%d, fc_pname:%x%x\n",
 			vport->fc_myDID,
-			vport->phba->fcf.fcfi,
-			vport->phba->sli4_hba.vfi_ids[vport->vfi],
-			vport->phba->vpi_ids[vport->vpi],
+			phba->fcf.fcfi,
+			phba->sli4_hba.vfi_ids[vport->vfi],
+			phba->vpi_ids[vport->vpi],
 			reg_vfi->wwn[0], reg_vfi->wwn[1]);
 }
 
diff --git a/drivers/scsi/lpfc/lpfc_nportdisc.c b/drivers/scsi/lpfc/lpfc_nportdisc.c
index 46128c67920..82f4d354228 100644
--- a/drivers/scsi/lpfc/lpfc_nportdisc.c
+++ b/drivers/scsi/lpfc/lpfc_nportdisc.c
@@ -226,7 +226,6 @@ lpfc_els_abort(struct lpfc_hba *phba, struct lpfc_nodelist *ndlp)
 		if (lpfc_check_sli_ndlp(phba, pring, iocb, ndlp)) {
 			/* It matches, so deque and call compl with anp error */
 			list_move_tail(&iocb->list, &completions);
-			pring->txq_cnt--;
 		}
 	}
 
diff --git a/drivers/scsi/lpfc/lpfc_scsi.c b/drivers/scsi/lpfc/lpfc_scsi.c
index 98af07c6e30..74b8710e1e9 100644
--- a/drivers/scsi/lpfc/lpfc_scsi.c
+++ b/drivers/scsi/lpfc/lpfc_scsi.c
@@ -732,7 +732,7 @@ lpfc_sli4_fcp_xri_aborted(struct lpfc_hba *phba,
 		psb = container_of(iocbq, struct lpfc_scsi_buf, cur_iocbq);
 		psb->exch_busy = 0;
 		spin_unlock_irqrestore(&phba->hbalock, iflag);
-		if (pring->txq_cnt)
+		if (!list_empty(&pring->txq))
 			lpfc_worker_wake_up(phba);
 		return;
 
@@ -885,9 +885,9 @@ lpfc_sli4_repost_scsi_sgl_list(struct lpfc_hba *phba)
 	int num_posted, rc = 0;
 
 	/* get all SCSI buffers need to repost to a local list */
-	spin_lock(&phba->scsi_buf_list_lock);
+	spin_lock_irq(&phba->scsi_buf_list_lock);
 	list_splice_init(&phba->lpfc_scsi_buf_list, &post_sblist);
-	spin_unlock(&phba->scsi_buf_list_lock);
+	spin_unlock_irq(&phba->scsi_buf_list_lock);
 
 	/* post the list of scsi buffer sgls to port if available */
 	if (!list_empty(&post_sblist)) {
@@ -4246,7 +4246,7 @@ static __inline__ void lpfc_poll_rearm_timer(struct lpfc_hba * phba)
 	unsigned long  poll_tmo_expires =
 		(jiffies + msecs_to_jiffies(phba->cfg_poll_tmo));
 
-	if (phba->sli.ring[LPFC_FCP_RING].txcmplq_cnt)
+	if (!list_empty(&phba->sli.ring[LPFC_FCP_RING].txcmplq))
 		mod_timer(&phba->fcp_poll_timer,
 			  poll_tmo_expires);
 }
diff --git a/drivers/scsi/lpfc/lpfc_sli.c b/drivers/scsi/lpfc/lpfc_sli.c
index 74b67d98e95..35dd17eb0f2 100644
--- a/drivers/scsi/lpfc/lpfc_sli.c
+++ b/drivers/scsi/lpfc/lpfc_sli.c
@@ -438,11 +438,12 @@ lpfc_sli4_rq_put(struct lpfc_queue *hq, struct lpfc_queue *dq,
 	struct lpfc_rqe *temp_hrqe;
 	struct lpfc_rqe *temp_drqe;
 	struct lpfc_register doorbell;
-	int put_index = hq->host_index;
+	int put_index;
 
 	/* sanity check on queue memory */
 	if (unlikely(!hq) || unlikely(!dq))
 		return -ENOMEM;
+	put_index = hq->host_index;
 	temp_hrqe = hq->qe[hq->host_index].rqe;
 	temp_drqe = dq->qe[dq->host_index].rqe;
 
@@ -872,14 +873,16 @@ lpfc_set_rrq_active(struct lpfc_hba *phba, struct lpfc_nodelist *ndlp,
 				xritag, rxid, ndlp->nlp_DID, send_rrq);
 		return -EINVAL;
 	}
-	rrq->send_rrq = send_rrq;
+	if (phba->cfg_enable_rrq == 1)
+		rrq->send_rrq = send_rrq;
+	else
+		rrq->send_rrq = 0;
 	rrq->xritag = xritag;
 	rrq->rrq_stop_time = jiffies + HZ * (phba->fc_ratov + 1);
 	rrq->ndlp = ndlp;
 	rrq->nlp_DID = ndlp->nlp_DID;
 	rrq->vport = ndlp->vport;
 	rrq->rxid = rxid;
-	rrq->send_rrq = send_rrq;
 	spin_lock_irqsave(&phba->hbalock, iflags);
 	empty = list_empty(&phba->active_rrq_list);
 	list_add_tail(&rrq->list, &phba->active_rrq_list);
@@ -1008,6 +1011,18 @@ __lpfc_sli_release_iocbq_s4(struct lpfc_hba *phba, struct lpfc_iocbq *iocbq)
 	else
 		sglq = __lpfc_clear_active_sglq(phba, iocbq->sli4_lxritag);
 
+	/*
+	** This should have been removed from the txcmplq before calling
+	** iocbq_release. The normal completion
+	** path should have already done the list_del_init.
+	*/
+	if (unlikely(!list_empty(&iocbq->list))) {
+		if (iocbq->iocb_flag & LPFC_IO_ON_TXCMPLQ)
+			iocbq->iocb_flag &= ~LPFC_IO_ON_TXCMPLQ;
+		list_del_init(&iocbq->list);
+	}
+
+
 	if (sglq)  {
 		if ((iocbq->iocb_flag & LPFC_EXCHANGE_BUSY) &&
 			(sglq->state != SGL_XRI_ABORTED)) {
@@ -1024,7 +1039,7 @@ __lpfc_sli_release_iocbq_s4(struct lpfc_hba *phba, struct lpfc_iocbq *iocbq)
 				&phba->sli4_hba.lpfc_sgl_list);
 
 			/* Check if TXQ queue needs to be serviced */
-			if (pring->txq_cnt)
+			if (!list_empty(&pring->txq))
 				lpfc_worker_wake_up(phba);
 		}
 	}
@@ -1056,6 +1071,14 @@ __lpfc_sli_release_iocbq_s3(struct lpfc_hba *phba, struct lpfc_iocbq *iocbq)
 	size_t start_clean = offsetof(struct lpfc_iocbq, iocb);
 
 	/*
+	** This should have been removed from the txcmplq before calling
+	** iocbq_release. The normal completion
+	** path should have already done the list_del_init.
+	*/
+	if (unlikely(!list_empty(&iocbq->list)))
+		list_del_init(&iocbq->list);
+
+	/*
 	 * Clean all volatile data fields, preserve iotag and node struct.
 	 */
 	memset((char*)iocbq + start_clean, 0, sizeof(*iocbq) - start_clean);
@@ -1121,7 +1144,6 @@ lpfc_sli_cancel_iocbs(struct lpfc_hba *phba, struct list_head *iocblist,
 
 	while (!list_empty(iocblist)) {
 		list_remove_head(iocblist, piocb, struct lpfc_iocbq, list);
-
 		if (!piocb->iocb_cmpl)
 			lpfc_sli_release_iocbq(phba, piocb);
 		else {
@@ -1309,9 +1331,6 @@ lpfc_sli_ringtxcmpl_put(struct lpfc_hba *phba, struct lpfc_sli_ring *pring,
 {
 	list_add_tail(&piocb->list, &pring->txcmplq);
 	piocb->iocb_flag |= LPFC_IO_ON_TXCMPLQ;
-	pring->txcmplq_cnt++;
-	if (pring->txcmplq_cnt > pring->txcmplq_max)
-		pring->txcmplq_max = pring->txcmplq_cnt;
 
 	if ((unlikely(pring->ringno == LPFC_ELS_RING)) &&
 	   (piocb->iocb.ulpCommand != CMD_ABORT_XRI_CN) &&
@@ -1343,8 +1362,6 @@ lpfc_sli_ringtx_get(struct lpfc_hba *phba, struct lpfc_sli_ring *pring)
 	struct lpfc_iocbq *cmd_iocb;
 
 	list_remove_head((&pring->txq), cmd_iocb, struct lpfc_iocbq, list);
-	if (cmd_iocb != NULL)
-		pring->txq_cnt--;
 	return cmd_iocb;
 }
 
@@ -1613,8 +1630,9 @@ lpfc_sli_resume_iocb(struct lpfc_hba *phba, struct lpfc_sli_ring *pring)
 	 *  (c) link attention events can be processed (fcp ring only)
 	 *  (d) IOCB processing is not blocked by the outstanding mbox command.
 	 */
-	if (pring->txq_cnt &&
-	    lpfc_is_link_up(phba) &&
+
+	if (lpfc_is_link_up(phba) &&
+	    (!list_empty(&pring->txq)) &&
 	    (pring->ringno != phba->sli.fcp_ring ||
 	     phba->sli.sli_flag & LPFC_PROCESS_LA)) {
 
@@ -2611,7 +2629,6 @@ lpfc_sli_iocbq_lookup(struct lpfc_hba *phba,
 		cmd_iocb = phba->sli.iocbq_lookup[iotag];
 		list_del_init(&cmd_iocb->list);
 		if (cmd_iocb->iocb_flag & LPFC_IO_ON_TXCMPLQ) {
-			pring->txcmplq_cnt--;
 			cmd_iocb->iocb_flag &= ~LPFC_IO_ON_TXCMPLQ;
 		}
 		return cmd_iocb;
@@ -2649,7 +2666,6 @@ lpfc_sli_iocbq_lookup_by_tag(struct lpfc_hba *phba,
 			/* remove from txcmpl queue list */
 			list_del_init(&cmd_iocb->list);
 			cmd_iocb->iocb_flag &= ~LPFC_IO_ON_TXCMPLQ;
-			pring->txcmplq_cnt--;
 			return cmd_iocb;
 		}
 	}
@@ -3498,7 +3514,6 @@ lpfc_sli_abort_iocb_ring(struct lpfc_hba *phba, struct lpfc_sli_ring *pring)
 	 */
 	spin_lock_irq(&phba->hbalock);
 	list_splice_init(&pring->txq, &completions);
-	pring->txq_cnt = 0;
 
 	/* Next issue ABTS for everything on the txcmplq */
 	list_for_each_entry_safe(iocb, next_iocb, &pring->txcmplq, list)
@@ -3535,11 +3550,9 @@ lpfc_sli_flush_fcp_rings(struct lpfc_hba *phba)
 	spin_lock_irq(&phba->hbalock);
 	/* Retrieve everything on txq */
 	list_splice_init(&pring->txq, &txq);
-	pring->txq_cnt = 0;
 
 	/* Retrieve everything on the txcmplq */
 	list_splice_init(&pring->txcmplq, &txcmplq);
-	pring->txcmplq_cnt = 0;
 
 	/* Indicate the I/O queues are flushed */
 	phba->hba_flag |= HBA_FCP_IOQ_FLUSH;
@@ -5987,9 +6000,9 @@ lpfc_sli4_repost_els_sgl_list(struct lpfc_hba *phba)
 	LIST_HEAD(post_sgl_list);
 	LIST_HEAD(free_sgl_list);
 
-	spin_lock(&phba->hbalock);
+	spin_lock_irq(&phba->hbalock);
 	list_splice_init(&phba->sli4_hba.lpfc_sgl_list, &allc_sgl_list);
-	spin_unlock(&phba->hbalock);
+	spin_unlock_irq(&phba->hbalock);
 
 	list_for_each_entry_safe(sglq_entry, sglq_entry_next,
 				 &allc_sgl_list, list) {
@@ -6090,10 +6103,10 @@ lpfc_sli4_repost_els_sgl_list(struct lpfc_hba *phba)
 
 	/* push els sgls posted to the availble list */
 	if (!list_empty(&post_sgl_list)) {
-		spin_lock(&phba->hbalock);
+		spin_lock_irq(&phba->hbalock);
 		list_splice_init(&post_sgl_list,
 				 &phba->sli4_hba.lpfc_sgl_list);
-		spin_unlock(&phba->hbalock);
+		spin_unlock_irq(&phba->hbalock);
 	} else {
 		lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
 				"3161 Failure to post els sgl to port.\n");
@@ -7614,7 +7627,6 @@ __lpfc_sli_ringtx_put(struct lpfc_hba *phba, struct lpfc_sli_ring *pring,
 {
 	/* Insert the caller's iocb in the txq tail for later processing. */
 	list_add_tail(&piocb->list, &pring->txq);
-	pring->txq_cnt++;
 }
 
 /**
@@ -8386,7 +8398,7 @@ __lpfc_sli_issue_iocb_s4(struct lpfc_hba *phba, uint32_t ring_number,
 		    piocb->iocb.ulpCommand == CMD_CLOSE_XRI_CN)
 			sglq = NULL;
 		else {
-			if (pring->txq_cnt) {
+			if (!list_empty(&pring->txq)) {
 				if (!(flag & SLI_IOCB_RET_IOCB)) {
 					__lpfc_sli_ringtx_put(phba,
 						pring, piocb);
@@ -9054,7 +9066,6 @@ lpfc_sli_host_down(struct lpfc_vport *vport)
 			if (iocb->vport != vport)
 				continue;
 			list_move_tail(&iocb->list, &completions);
-			pring->txq_cnt--;
 		}
 
 		/* Next issue ABTS for everything on the txcmplq */
@@ -9123,8 +9134,6 @@ lpfc_sli_hba_down(struct lpfc_hba *phba)
 		 * given to the FW yet.
 		 */
 		list_splice_init(&pring->txq, &completions);
-		pring->txq_cnt = 0;
-
 	}
 	spin_unlock_irqrestore(&phba->hbalock, flags);
 
@@ -9965,6 +9974,9 @@ lpfc_sli_issue_iocb_wait(struct lpfc_hba *phba,
 	long timeleft, timeout_req = 0;
 	int retval = IOCB_SUCCESS;
 	uint32_t creg_val;
+	struct lpfc_iocbq *iocb;
+	int txq_cnt = 0;
+	int txcmplq_cnt = 0;
 	struct lpfc_sli_ring *pring = &phba->sli.ring[LPFC_ELS_RING];
 	/*
 	 * If the caller has provided a response iocbq buffer, then context2
@@ -10012,9 +10024,17 @@ lpfc_sli_issue_iocb_wait(struct lpfc_hba *phba,
 			retval = IOCB_TIMEDOUT;
 		}
 	} else if (retval == IOCB_BUSY) {
-		lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
-			"2818 Max IOCBs %d txq cnt %d txcmplq cnt %d\n",
-			phba->iocb_cnt, pring->txq_cnt, pring->txcmplq_cnt);
+		if (phba->cfg_log_verbose & LOG_SLI) {
+			list_for_each_entry(iocb, &pring->txq, list) {
+				txq_cnt++;
+			}
+			list_for_each_entry(iocb, &pring->txcmplq, list) {
+				txcmplq_cnt++;
+			}
+			lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
+				"2818 Max IOCBs %d txq cnt %d txcmplq cnt %d\n",
+				phba->iocb_cnt, txq_cnt, txcmplq_cnt);
+		}
 		return retval;
 	} else {
 		lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
@@ -11297,16 +11317,25 @@ lpfc_sli4_sp_handle_els_wcqe(struct lpfc_hba *phba, struct lpfc_queue *cq,
 	struct lpfc_iocbq *irspiocbq;
 	unsigned long iflags;
 	struct lpfc_sli_ring *pring = cq->pring;
+	int txq_cnt = 0;
+	int txcmplq_cnt = 0;
+	int fcp_txcmplq_cnt = 0;
 
 	/* Get an irspiocbq for later ELS response processing use */
 	irspiocbq = lpfc_sli_get_iocbq(phba);
 	if (!irspiocbq) {
+		if (!list_empty(&pring->txq))
+			txq_cnt++;
+		if (!list_empty(&pring->txcmplq))
+			txcmplq_cnt++;
+		if (!list_empty(&phba->sli.ring[LPFC_FCP_RING].txcmplq))
+			fcp_txcmplq_cnt++;
 		lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
 			"0387 NO IOCBQ data: txq_cnt=%d iocb_cnt=%d "
 			"fcp_txcmplq_cnt=%d, els_txcmplq_cnt=%d\n",
-			pring->txq_cnt, phba->iocb_cnt,
-			phba->sli.ring[LPFC_FCP_RING].txcmplq_cnt,
-			phba->sli.ring[LPFC_ELS_RING].txcmplq_cnt);
+			txq_cnt, phba->iocb_cnt,
+			fcp_txcmplq_cnt,
+			txcmplq_cnt);
 		return false;
 	}
 
@@ -15481,11 +15510,18 @@ lpfc_check_next_fcf_pri_level(struct lpfc_hba *phba)
 			LPFC_SLI4_FCF_TBL_INDX_MAX);
 	lpfc_printf_log(phba, KERN_INFO, LOG_FIP,
 			"3060 Last IDX %d\n", last_index);
-	if (list_empty(&phba->fcf.fcf_pri_list)) {
+
+	/* Verify the priority list has 2 or more entries */
+	spin_lock_irq(&phba->hbalock);
+	if (list_empty(&phba->fcf.fcf_pri_list) ||
+	    list_is_singular(&phba->fcf.fcf_pri_list)) {
+		spin_unlock_irq(&phba->hbalock);
 		lpfc_printf_log(phba, KERN_ERR, LOG_FIP,
 			"3061 Last IDX %d\n", last_index);
 		return 0; /* Empty rr list */
 	}
+	spin_unlock_irq(&phba->hbalock);
+
 	next_fcf_pri = 0;
 	/*
 	 * Clear the rr_bmask and set all of the bits that are at this
@@ -16244,14 +16280,19 @@ lpfc_drain_txq(struct lpfc_hba *phba)
 	char *fail_msg = NULL;
 	struct lpfc_sglq *sglq;
 	union lpfc_wqe wqe;
+	int txq_cnt = 0;
 
 	spin_lock_irqsave(&phba->hbalock, iflags);
-	if (pring->txq_cnt > pring->txq_max)
-		pring->txq_max = pring->txq_cnt;
+	list_for_each_entry(piocbq, &pring->txq, list) {
+		txq_cnt++;
+	}
+
+	if (txq_cnt > pring->txq_max)
+		pring->txq_max = txq_cnt;
 
 	spin_unlock_irqrestore(&phba->hbalock, iflags);
 
-	while (pring->txq_cnt) {
+	while (!list_empty(&pring->txq)) {
 		spin_lock_irqsave(&phba->hbalock, iflags);
 
 		piocbq = lpfc_sli_ringtx_get(phba, pring);
@@ -16259,7 +16300,7 @@ lpfc_drain_txq(struct lpfc_hba *phba)
 			spin_unlock_irqrestore(&phba->hbalock, iflags);
 			lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
 				"2823 txq empty and txq_cnt is %d\n ",
-				pring->txq_cnt);
+				txq_cnt);
 			break;
 		}
 		sglq = __lpfc_sli_get_sglq(phba, piocbq);
@@ -16268,6 +16309,7 @@ lpfc_drain_txq(struct lpfc_hba *phba)
 			spin_unlock_irqrestore(&phba->hbalock, iflags);
 			break;
 		}
+		txq_cnt--;
 
 		/* The xri and iocb resources secured,
 		 * attempt to issue request
@@ -16299,5 +16341,5 @@ lpfc_drain_txq(struct lpfc_hba *phba)
 	lpfc_sli_cancel_iocbs(phba, &completions, IOSTAT_LOCAL_REJECT,
 				IOERR_SLI_ABORTED);
 
-	return pring->txq_cnt;
+	return txq_cnt;
 }
diff --git a/drivers/scsi/lpfc/lpfc_version.h b/drivers/scsi/lpfc/lpfc_version.h
index f3b7795a296..664cd04f7cd 100644
--- a/drivers/scsi/lpfc/lpfc_version.h
+++ b/drivers/scsi/lpfc/lpfc_version.h
@@ -18,7 +18,7 @@
  * included with this package.                                     *
  *******************************************************************/
 
-#define LPFC_DRIVER_VERSION "8.3.37"
+#define LPFC_DRIVER_VERSION "8.3.38"
 #define LPFC_DRIVER_NAME		"lpfc"
 
 /* Used for SLI 2/3 */
diff --git a/drivers/scsi/megaraid/megaraid_sas.h b/drivers/scsi/megaraid/megaraid_sas.h
index 408d2548a74..684cc343cf0 100644
--- a/drivers/scsi/megaraid/megaraid_sas.h
+++ b/drivers/scsi/megaraid/megaraid_sas.h
@@ -1488,7 +1488,4 @@ struct megasas_mgmt_info {
 	int max_index;
 };
 
-#define msi_control_reg(base) (base + PCI_MSI_FLAGS)
-#define PCI_MSIX_FLAGS_ENABLE (1 << 15)
-
 #endif				/*LSI_MEGARAID_SAS_H */
diff --git a/drivers/scsi/megaraid/megaraid_sas_base.c b/drivers/scsi/megaraid/megaraid_sas_base.c
index 9d53540207e..7c90d57b867 100644
--- a/drivers/scsi/megaraid/megaraid_sas_base.c
+++ b/drivers/scsi/megaraid/megaraid_sas_base.c
@@ -3984,12 +3984,12 @@ static int megasas_probe_one(struct pci_dev *pdev,
 	if (reset_devices) {
 		pos = pci_find_capability(pdev, PCI_CAP_ID_MSIX);
 		if (pos) {
-			pci_read_config_word(pdev, msi_control_reg(pos),
+			pci_read_config_word(pdev, pos + PCI_MSIX_FLAGS,
 					     &control);
 			if (control & PCI_MSIX_FLAGS_ENABLE) {
 				dev_info(&pdev->dev, "resetting MSI-X\n");
 				pci_write_config_word(pdev,
-						      msi_control_reg(pos),
+						      pos + PCI_MSIX_FLAGS,
 						      control &
 						      ~PCI_MSIX_FLAGS_ENABLE);
 			}
diff --git a/drivers/scsi/mpt3sas/mpt3sas_config.c b/drivers/scsi/mpt3sas/mpt3sas_config.c
index 1df9ed4f371..4db0c7a18bd 100644
--- a/drivers/scsi/mpt3sas/mpt3sas_config.c
+++ b/drivers/scsi/mpt3sas/mpt3sas_config.c
@@ -148,7 +148,7 @@ _config_display_some_debug(struct MPT3SAS_ADAPTER *ioc, u16 smid,
 			desc = "raid_config";
 			break;
 		case MPI2_CONFIG_EXTPAGETYPE_DRIVER_MAPPING:
-			desc = "driver_mappping";
+			desc = "driver_mapping";
 			break;
 		}
 		break;
diff --git a/drivers/scsi/mvsas/mv_init.c b/drivers/scsi/mvsas/mv_init.c
index ce90d0546cd..74550922ad5 100644
--- a/drivers/scsi/mvsas/mv_init.c
+++ b/drivers/scsi/mvsas/mv_init.c
@@ -703,7 +703,7 @@ static struct pci_device_id mvs_pci_table[] = {
 	{ PCI_VDEVICE(TTI, 0x2744), chip_9480 },
 	{ PCI_VDEVICE(TTI, 0x2760), chip_9480 },
 	{
-		.vendor		= 0x1b4b,
+		.vendor		= PCI_VENDOR_ID_MARVELL_EXT,
 		.device		= 0x9480,
 		.subvendor	= PCI_ANY_ID,
 		.subdevice	= 0x9480,
@@ -712,7 +712,7 @@ static struct pci_device_id mvs_pci_table[] = {
 		.driver_data	= chip_9480,
 	},
 	{
-		.vendor		= 0x1b4b,
+		.vendor		= PCI_VENDOR_ID_MARVELL_EXT,
 		.device		= 0x9445,
 		.subvendor	= PCI_ANY_ID,
 		.subdevice	= 0x9480,
@@ -721,7 +721,7 @@ static struct pci_device_id mvs_pci_table[] = {
 		.driver_data	= chip_9445,
 	},
 	{
-		.vendor		= 0x1b4b,
+		.vendor		= PCI_VENDOR_ID_MARVELL_EXT,
 		.device		= 0x9485,
 		.subvendor	= PCI_ANY_ID,
 		.subdevice	= 0x9480,
diff --git a/drivers/scsi/mvumi.c b/drivers/scsi/mvumi.c
index 4594ccaaf49..c3601b57a80 100644
--- a/drivers/scsi/mvumi.c
+++ b/drivers/scsi/mvumi.c
@@ -49,8 +49,8 @@ MODULE_AUTHOR("jyli@marvell.com");
 MODULE_DESCRIPTION("Marvell UMI Driver");
 
 static DEFINE_PCI_DEVICE_TABLE(mvumi_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL_2, PCI_DEVICE_ID_MARVELL_MV9143) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL_2, PCI_DEVICE_ID_MARVELL_MV9580) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL_EXT, PCI_DEVICE_ID_MARVELL_MV9143) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL_EXT, PCI_DEVICE_ID_MARVELL_MV9580) },
 	{ 0 }
 };
 
diff --git a/drivers/scsi/mvumi.h b/drivers/scsi/mvumi.h
index e360135fd1b..41f168702ac 100644
--- a/drivers/scsi/mvumi.h
+++ b/drivers/scsi/mvumi.h
@@ -32,7 +32,6 @@
 #define VER_BUILD		1500
 
 #define MV_DRIVER_NAME			"mvumi"
-#define PCI_VENDOR_ID_MARVELL_2		0x1b4b
 #define PCI_DEVICE_ID_MARVELL_MV9143	0x9143
 #define PCI_DEVICE_ID_MARVELL_MV9580	0x9580
 
diff --git a/drivers/scsi/osd/osd_initiator.c b/drivers/scsi/osd/osd_initiator.c
index d8293f25ca3..aa66361ed44 100644
--- a/drivers/scsi/osd/osd_initiator.c
+++ b/drivers/scsi/osd/osd_initiator.c
@@ -1049,7 +1049,7 @@ static struct bio *_create_sg_bios(struct osd_request *or,
 
 	bio = bio_kmalloc(GFP_KERNEL, numentries);
 	if (unlikely(!bio)) {
-		OSD_DEBUG("Faild to allocate BIO size=%u\n", numentries);
+		OSD_DEBUG("Failed to allocate BIO size=%u\n", numentries);
 		return ERR_PTR(-ENOMEM);
 	}
 
diff --git a/drivers/scsi/qla2xxx/Makefile b/drivers/scsi/qla2xxx/Makefile
index dce7d788cdc..c37b244cf8a 100644
--- a/drivers/scsi/qla2xxx/Makefile
+++ b/drivers/scsi/qla2xxx/Makefile
@@ -1,6 +1,6 @@
 qla2xxx-y := qla_os.o qla_init.o qla_mbx.o qla_iocb.o qla_isr.o qla_gs.o \
 		qla_dbg.o qla_sup.o qla_attr.o qla_mid.o qla_dfs.o qla_bsg.o \
-        qla_nx.o qla_target.o
+        qla_nx.o qla_mr.o qla_target.o
 
 obj-$(CONFIG_SCSI_QLA_FC) += qla2xxx.o
 obj-$(CONFIG_TCM_QLA2XXX) += tcm_qla2xxx.o
diff --git a/drivers/scsi/qla2xxx/qla_attr.c b/drivers/scsi/qla2xxx/qla_attr.c
index 1d82eef4e1e..bf60c631abb 100644
--- a/drivers/scsi/qla2xxx/qla_attr.c
+++ b/drivers/scsi/qla2xxx/qla_attr.c
@@ -888,7 +888,10 @@ qla2x00_serial_num_show(struct device *dev, struct device_attribute *attr,
 	struct qla_hw_data *ha = vha->hw;
 	uint32_t sn;
 
-	if (IS_FWI2_CAPABLE(ha)) {
+	if (IS_QLAFX00(vha->hw)) {
+		return snprintf(buf, PAGE_SIZE, "%s\n",
+		    vha->hw->mr.serial_num);
+	} else if (IS_FWI2_CAPABLE(ha)) {
 		qla2xxx_get_vpd_field(vha, "SN", buf, PAGE_SIZE);
 		return snprintf(buf, PAGE_SIZE, "%s\n", buf);
 	}
@@ -912,6 +915,11 @@ qla2x00_isp_id_show(struct device *dev, struct device_attribute *attr,
 {
 	scsi_qla_host_t *vha = shost_priv(class_to_shost(dev));
 	struct qla_hw_data *ha = vha->hw;
+
+	if (IS_QLAFX00(vha->hw))
+		return snprintf(buf, PAGE_SIZE, "%s\n",
+		    vha->hw->mr.hw_version);
+
 	return snprintf(buf, PAGE_SIZE, "%04x %04x %04x %04x\n",
 	    ha->product_id[0], ha->product_id[1], ha->product_id[2],
 	    ha->product_id[3]);
@@ -922,6 +930,11 @@ qla2x00_model_name_show(struct device *dev, struct device_attribute *attr,
 			char *buf)
 {
 	scsi_qla_host_t *vha = shost_priv(class_to_shost(dev));
+
+	if (IS_QLAFX00(vha->hw))
+		return snprintf(buf, PAGE_SIZE, "%s\n",
+		    vha->hw->mr.product_name);
+
 	return snprintf(buf, PAGE_SIZE, "%s\n", vha->hw->model_number);
 }
 
@@ -1304,6 +1317,12 @@ qla2x00_fw_state_show(struct device *dev, struct device_attribute *attr,
 	scsi_qla_host_t *vha = shost_priv(class_to_shost(dev));
 	int rval = QLA_FUNCTION_FAILED;
 	uint16_t state[5];
+	uint32_t pstate;
+
+	if (IS_QLAFX00(vha->hw)) {
+		pstate = qlafx00_fw_state_show(dev, attr, buf);
+		return snprintf(buf, PAGE_SIZE, "0x%x\n", pstate);
+	}
 
 	if (qla2x00_reset_active(vha))
 		ql_log(ql_log_warn, vha, 0x707c,
@@ -1454,6 +1473,11 @@ qla2x00_get_host_speed(struct Scsi_Host *shost)
 					(shost_priv(shost)))->hw;
 	u32 speed = FC_PORTSPEED_UNKNOWN;
 
+	if (IS_QLAFX00(ha)) {
+		qlafx00_get_host_speed(shost);
+		return;
+	}
+
 	switch (ha->link_data_rate) {
 	case PORT_SPEED_1GB:
 		speed = FC_PORTSPEED_1GBIT;
@@ -1637,6 +1661,9 @@ qla2x00_issue_lip(struct Scsi_Host *shost)
 {
 	scsi_qla_host_t *vha = shost_priv(shost);
 
+	if (IS_QLAFX00(vha->hw))
+		return 0;
+
 	qla2x00_loop_reset(vha);
 	return 0;
 }
@@ -1655,6 +1682,9 @@ qla2x00_get_fc_host_stats(struct Scsi_Host *shost)
 	pfc_host_stat = &vha->fc_host_stat;
 	memset(pfc_host_stat, -1, sizeof(struct fc_host_statistics));
 
+	if (IS_QLAFX00(vha->hw))
+		goto done;
+
 	if (test_bit(UNLOADING, &vha->dpc_flags))
 		goto done;
 
@@ -1938,11 +1968,6 @@ qla24xx_vport_delete(struct fc_vport *fc_vport)
 		    "Timer for the VP[%d] has stopped\n", vha->vp_idx);
 	}
 
-	/* No pending activities shall be there on the vha now */
-	if (ql2xextended_error_logging & ql_dbg_user)
-		msleep(random32()%10);  /* Just to see if something falls on
-					* the net we have placed below */
-
 	BUG_ON(atomic_read(&vha->vref_count));
 
 	qla2x00_free_fcports(vha);
@@ -2092,6 +2117,9 @@ qla2x00_init_host_attr(scsi_qla_host_t *vha)
 		    FC_PORTSPEED_1GBIT;
 	else if (IS_QLA23XX(ha))
 		speed = FC_PORTSPEED_2GBIT | FC_PORTSPEED_1GBIT;
+	else if (IS_QLAFX00(ha))
+		speed = FC_PORTSPEED_8GBIT | FC_PORTSPEED_4GBIT |
+		    FC_PORTSPEED_2GBIT | FC_PORTSPEED_1GBIT;
 	else
 		speed = FC_PORTSPEED_1GBIT;
 	fc_host_supported_speeds(vha->host) = speed;
diff --git a/drivers/scsi/qla2xxx/qla_bsg.c b/drivers/scsi/qla2xxx/qla_bsg.c
index ad54099cb80..39719f89248 100644
--- a/drivers/scsi/qla2xxx/qla_bsg.c
+++ b/drivers/scsi/qla2xxx/qla_bsg.c
@@ -30,14 +30,31 @@ qla2x00_bsg_sp_free(void *data, void *ptr)
 	struct scsi_qla_host *vha = sp->fcport->vha;
 	struct fc_bsg_job *bsg_job = sp->u.bsg_job;
 	struct qla_hw_data *ha = vha->hw;
+	struct qla_mt_iocb_rqst_fx00 *piocb_rqst;
 
-	dma_unmap_sg(&ha->pdev->dev, bsg_job->request_payload.sg_list,
-	    bsg_job->request_payload.sg_cnt, DMA_TO_DEVICE);
+	if (sp->type == SRB_FXIOCB_BCMD) {
+		piocb_rqst = (struct qla_mt_iocb_rqst_fx00 *)
+		    &bsg_job->request->rqst_data.h_vendor.vendor_cmd[1];
 
-	dma_unmap_sg(&ha->pdev->dev, bsg_job->reply_payload.sg_list,
-	    bsg_job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
+		if (piocb_rqst->flags & SRB_FXDISC_REQ_DMA_VALID)
+			dma_unmap_sg(&ha->pdev->dev,
+			    bsg_job->request_payload.sg_list,
+			    bsg_job->request_payload.sg_cnt, DMA_TO_DEVICE);
+
+		if (piocb_rqst->flags & SRB_FXDISC_RESP_DMA_VALID)
+			dma_unmap_sg(&ha->pdev->dev,
+			    bsg_job->reply_payload.sg_list,
+			    bsg_job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
+	} else {
+		dma_unmap_sg(&ha->pdev->dev, bsg_job->request_payload.sg_list,
+		    bsg_job->request_payload.sg_cnt, DMA_TO_DEVICE);
+
+		dma_unmap_sg(&ha->pdev->dev, bsg_job->reply_payload.sg_list,
+		    bsg_job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
+	}
 
 	if (sp->type == SRB_CT_CMD ||
+	    sp->type == SRB_FXIOCB_BCMD ||
 	    sp->type == SRB_ELS_CMD_HST)
 		kfree(sp->fcport);
 	qla2x00_rel_sp(vha, sp);
@@ -751,6 +768,8 @@ qla2x00_process_loopback(struct fc_bsg_job *bsg_job)
 	elreq.transfer_size = req_data_len;
 
 	elreq.options = bsg_job->request->rqst_data.h_vendor.vendor_cmd[1];
+	elreq.iteration_count =
+	    bsg_job->request->rqst_data.h_vendor.vendor_cmd[2];
 
 	if (atomic_read(&vha->loop_state) == LOOP_READY &&
 	    (ha->current_topology == ISP_CFG_F ||
@@ -1883,6 +1902,128 @@ done:
 }
 
 static int
+qlafx00_mgmt_cmd(struct fc_bsg_job *bsg_job)
+{
+	struct Scsi_Host *host = bsg_job->shost;
+	scsi_qla_host_t *vha = shost_priv(host);
+	struct qla_hw_data *ha = vha->hw;
+	int rval = (DRIVER_ERROR << 16);
+	struct qla_mt_iocb_rqst_fx00 *piocb_rqst;
+	srb_t *sp;
+	int req_sg_cnt = 0, rsp_sg_cnt = 0;
+	struct fc_port *fcport;
+	char  *type = "FC_BSG_HST_FX_MGMT";
+
+	/* Copy the IOCB specific information */
+	piocb_rqst = (struct qla_mt_iocb_rqst_fx00 *)
+	    &bsg_job->request->rqst_data.h_vendor.vendor_cmd[1];
+
+	/* Dump the vendor information */
+	ql_dump_buffer(ql_dbg_user + ql_dbg_verbose , vha, 0x70cf,
+	    (uint8_t *)piocb_rqst, sizeof(struct qla_mt_iocb_rqst_fx00));
+
+	if (!vha->flags.online) {
+		ql_log(ql_log_warn, vha, 0x70d0,
+		    "Host is not online.\n");
+		rval = -EIO;
+		goto done;
+	}
+
+	if (piocb_rqst->flags & SRB_FXDISC_REQ_DMA_VALID) {
+		req_sg_cnt = dma_map_sg(&ha->pdev->dev,
+		    bsg_job->request_payload.sg_list,
+		    bsg_job->request_payload.sg_cnt, DMA_TO_DEVICE);
+		if (!req_sg_cnt) {
+			ql_log(ql_log_warn, vha, 0x70c7,
+			    "dma_map_sg return %d for request\n", req_sg_cnt);
+			rval = -ENOMEM;
+			goto done;
+		}
+	}
+
+	if (piocb_rqst->flags & SRB_FXDISC_RESP_DMA_VALID) {
+		rsp_sg_cnt = dma_map_sg(&ha->pdev->dev,
+		    bsg_job->reply_payload.sg_list,
+		    bsg_job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
+		if (!rsp_sg_cnt) {
+			ql_log(ql_log_warn, vha, 0x70c8,
+			    "dma_map_sg return %d for reply\n", rsp_sg_cnt);
+			rval = -ENOMEM;
+			goto done_unmap_req_sg;
+		}
+	}
+
+	ql_dbg(ql_dbg_user, vha, 0x70c9,
+	    "request_sg_cnt: %x dma_request_sg_cnt: %x reply_sg_cnt:%x "
+	    "dma_reply_sg_cnt: %x\n", bsg_job->request_payload.sg_cnt,
+	    req_sg_cnt, bsg_job->reply_payload.sg_cnt, rsp_sg_cnt);
+
+	/* Allocate a dummy fcport structure, since functions preparing the
+	 * IOCB and mailbox command retrieves port specific information
+	 * from fcport structure. For Host based ELS commands there will be
+	 * no fcport structure allocated
+	 */
+	fcport = qla2x00_alloc_fcport(vha, GFP_KERNEL);
+	if (!fcport) {
+		ql_log(ql_log_warn, vha, 0x70ca,
+		    "Failed to allocate fcport.\n");
+		rval = -ENOMEM;
+		goto done_unmap_rsp_sg;
+	}
+
+	/* Alloc SRB structure */
+	sp = qla2x00_get_sp(vha, fcport, GFP_KERNEL);
+	if (!sp) {
+		ql_log(ql_log_warn, vha, 0x70cb,
+		    "qla2x00_get_sp failed.\n");
+		rval = -ENOMEM;
+		goto done_free_fcport;
+	}
+
+	/* Initialize all required  fields of fcport */
+	fcport->vha = vha;
+	fcport->loop_id = piocb_rqst->dataword;
+
+	sp->type = SRB_FXIOCB_BCMD;
+	sp->name = "bsg_fx_mgmt";
+	sp->iocbs = qla24xx_calc_ct_iocbs(req_sg_cnt + rsp_sg_cnt);
+	sp->u.bsg_job = bsg_job;
+	sp->free = qla2x00_bsg_sp_free;
+	sp->done = qla2x00_bsg_job_done;
+
+	ql_dbg(ql_dbg_user, vha, 0x70cc,
+	    "bsg rqst type: %s fx_mgmt_type: %x id=%x\n",
+	    type, piocb_rqst->func_type, fcport->loop_id);
+
+	rval = qla2x00_start_sp(sp);
+	if (rval != QLA_SUCCESS) {
+		ql_log(ql_log_warn, vha, 0x70cd,
+		    "qla2x00_start_sp failed=%d.\n", rval);
+		mempool_free(sp, ha->srb_mempool);
+		rval = -EIO;
+		goto done_free_fcport;
+	}
+	return rval;
+
+done_free_fcport:
+	kfree(fcport);
+
+done_unmap_rsp_sg:
+	if (piocb_rqst->flags & SRB_FXDISC_RESP_DMA_VALID)
+		dma_unmap_sg(&ha->pdev->dev,
+		    bsg_job->reply_payload.sg_list,
+		    bsg_job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
+done_unmap_req_sg:
+	if (piocb_rqst->flags & SRB_FXDISC_REQ_DMA_VALID)
+		dma_unmap_sg(&ha->pdev->dev,
+		    bsg_job->request_payload.sg_list,
+		    bsg_job->request_payload.sg_cnt, DMA_TO_DEVICE);
+
+done:
+	return rval;
+}
+
+static int
 qla2x00_process_vendor_specific(struct fc_bsg_job *bsg_job)
 {
 	switch (bsg_job->request->rqst_data.h_vendor.vendor_cmd[0]) {
@@ -1928,6 +2069,8 @@ qla2x00_process_vendor_specific(struct fc_bsg_job *bsg_job)
 	case QL_VND_DIAG_IO_CMD:
 		return qla24xx_process_bidir_cmd(bsg_job);
 
+	case QL_VND_FX00_MGMT_CMD:
+		return qlafx00_mgmt_cmd(bsg_job);
 	default:
 		return -ENOSYS;
 	}
@@ -2007,7 +2150,8 @@ qla24xx_bsg_timeout(struct fc_bsg_job *bsg_job)
 			sp = req->outstanding_cmds[cnt];
 			if (sp) {
 				if (((sp->type == SRB_CT_CMD) ||
-					(sp->type == SRB_ELS_CMD_HST))
+					(sp->type == SRB_ELS_CMD_HST) ||
+					(sp->type == SRB_FXIOCB_BCMD))
 					&& (sp->u.bsg_job == bsg_job)) {
 					spin_unlock_irqrestore(&ha->hardware_lock, flags);
 					if (ha->isp_ops->abort_command(sp)) {
diff --git a/drivers/scsi/qla2xxx/qla_bsg.h b/drivers/scsi/qla2xxx/qla_bsg.h
index e9f6b9bbf29..04f770332c2 100644
--- a/drivers/scsi/qla2xxx/qla_bsg.h
+++ b/drivers/scsi/qla2xxx/qla_bsg.h
@@ -22,6 +22,7 @@
 #define QL_VND_DIAG_IO_CMD	0x0A
 #define QL_VND_WRITE_I2C	0x10
 #define QL_VND_READ_I2C		0x11
+#define QL_VND_FX00_MGMT_CMD	0x12
 
 /* BSG Vendor specific subcode returns */
 #define EXT_STATUS_OK			0
diff --git a/drivers/scsi/qla2xxx/qla_dbg.c b/drivers/scsi/qla2xxx/qla_dbg.c
index 1626de52e32..cfa2a20dee9 100644
--- a/drivers/scsi/qla2xxx/qla_dbg.c
+++ b/drivers/scsi/qla2xxx/qla_dbg.c
@@ -11,27 +11,31 @@
  * ----------------------------------------------------------------------
  * |             Level            |   Last Value Used  |     Holes	|
  * ----------------------------------------------------------------------
- * | Module Init and Probe        |       0x0126       | 0x4b,0xba,0xfa |
- * | Mailbox commands             |       0x115b       | 0x111a-0x111b  |
- * |                              |                    | 0x112c-0x112e  |
- * |                              |                    | 0x113a         |
- * | Device Discovery             |       0x2087       | 0x2020-0x2022, |
+ * | Module Init and Probe        |       0x014f       | 0x4b,0xba,0xfa |
+ * | Mailbox commands             |       0x1179       | 0x111a-0x111b  |
+ * |                              |                    | 0x1155-0x1158  |
+ * | Device Discovery             |       0x2095       | 0x2020-0x2022, |
  * |                              |                    | 0x2016         |
- * | Queue Command and IO tracing |       0x3031       | 0x3006-0x300b  |
+ * | Queue Command and IO tracing |       0x3058       | 0x3006-0x300b  |
  * |                              |                    | 0x3027-0x3028  |
- * |                              |                    | 0x302d-0x302e  |
- * | DPC Thread                   |       0x401d       | 0x4002,0x4013  |
- * | Async Events                 |       0x5071       | 0x502b-0x502f  |
+ * |                              |                    | 0x303d-0x3041  |
+ * |                              |                    | 0x302d,0x3033  |
+ * |                              |                    | 0x3036,0x3038  |
+ * |                              |                    | 0x303a		|
+ * | DPC Thread                   |       0x4022       | 0x4002,0x4013  |
+ * | Async Events                 |       0x5081       | 0x502b-0x502f  |
  * |                              |                    | 0x5047,0x5052  |
+ * |                              |                    | 0x5040,0x5075  |
  * | Timer Routines               |       0x6011       |                |
- * | User Space Interactions      |       0x70c4       | 0x7018,0x702e, |
+ * | User Space Interactions      |       0x70dd       | 0x7018,0x702e, |
  * |                              |                    | 0x7020,0x7024, |
  * |                              |                    | 0x7039,0x7045, |
  * |                              |                    | 0x7073-0x7075, |
- * |                              |                    | 0x708c,        |
+ * |                              |                    | 0x707b,0x708c, |
  * |                              |                    | 0x70a5,0x70a6, |
  * |                              |                    | 0x70a8,0x70ab, |
- * |                              |                    | 0x70ad-0x70ae  |
+ * |                              |                    | 0x70ad-0x70ae, |
+ * |                              |                    | 0x70d1-0x70da  |
  * | Task Management              |       0x803c       | 0x8025-0x8026  |
  * |                              |                    | 0x800b,0x8039  |
  * | AER/EEH                      |       0x9011       |		|
@@ -401,7 +405,7 @@ qla2xxx_copy_atioqueues(struct qla_hw_data *ha, void *ptr,
 		void *ring;
 	} aq, *aqp;
 
-	if (!ha->tgt.atio_q_length)
+	if (!ha->tgt.atio_ring)
 		return ptr;
 
 	num_queues = 1;
diff --git a/drivers/scsi/qla2xxx/qla_def.h b/drivers/scsi/qla2xxx/qla_def.h
index c6509911772..c32efc75322 100644
--- a/drivers/scsi/qla2xxx/qla_def.h
+++ b/drivers/scsi/qla2xxx/qla_def.h
@@ -245,7 +245,6 @@
 
 #define MAX_CMDSZ	16		/* SCSI maximum CDB size. */
 #include "qla_fw.h"
-
 /*
  * Timeout timer counts in seconds
  */
@@ -265,6 +264,7 @@
 #define RESPONSE_ENTRY_CNT_2300		512	/* Number of response entries.*/
 #define RESPONSE_ENTRY_CNT_MQ		128	/* Number of response entries.*/
 #define ATIO_ENTRY_CNT_24XX		4096	/* Number of ATIO entries. */
+#define RESPONSE_ENTRY_CNT_FX00		256     /* Number of response entries.*/
 
 struct req_que;
 
@@ -284,6 +284,7 @@ struct sd_dif_tuple {
 struct srb_cmd {
 	struct scsi_cmnd *cmd;		/* Linux SCSI command pkt */
 	uint32_t request_sense_length;
+	uint32_t fw_sense_length;
 	uint8_t *request_sense_ptr;
 	void *ctx;
 };
@@ -321,7 +322,39 @@ struct srb_iocb {
 			uint32_t flags;
 			uint32_t lun;
 			uint32_t data;
+			struct completion comp;
+			uint32_t comp_status;
 		} tmf;
+		struct {
+#define SRB_FXDISC_REQ_DMA_VALID	BIT_0
+#define SRB_FXDISC_RESP_DMA_VALID	BIT_1
+#define SRB_FXDISC_REQ_DWRD_VALID	BIT_2
+#define SRB_FXDISC_RSP_DWRD_VALID	BIT_3
+#define FXDISC_TIMEOUT 20
+			uint8_t flags;
+			uint32_t req_len;
+			uint32_t rsp_len;
+			void *req_addr;
+			void *rsp_addr;
+			dma_addr_t req_dma_handle;
+			dma_addr_t rsp_dma_handle;
+			uint32_t adapter_id;
+			uint32_t adapter_id_hi;
+			uint32_t req_func_type;
+			uint32_t req_data;
+			uint32_t req_data_extra;
+			uint32_t result;
+			uint32_t seq_number;
+			uint32_t fw_flags;
+			struct completion fxiocb_comp;
+			uint32_t reserved_0;
+			uint8_t reserved_1;
+		} fxiocb;
+		struct {
+			uint32_t cmd_hndl;
+			uint32_t comp_status;
+			struct completion comp;
+		} abt;
 	} u;
 
 	struct timer_list timer;
@@ -338,6 +371,10 @@ struct srb_iocb {
 #define SRB_TM_CMD	7
 #define SRB_SCSI_CMD	8
 #define SRB_BIDI_CMD	9
+#define SRB_FXIOCB_DCMD	10
+#define SRB_FXIOCB_BCMD	11
+#define SRB_ABT_CMD	12
+
 
 typedef struct srb {
 	atomic_t ref_count;
@@ -368,6 +405,10 @@ typedef struct srb {
 	(sp->u.scmd.request_sense_ptr)
 #define SET_CMD_SENSE_PTR(sp, ptr) \
 	(sp->u.scmd.request_sense_ptr = ptr)
+#define GET_FW_SENSE_LEN(sp) \
+	(sp->u.scmd.fw_sense_length)
+#define SET_FW_SENSE_LEN(sp, len) \
+	(sp->u.scmd.fw_sense_length = len)
 
 struct msg_echo_lb {
 	dma_addr_t send_dma;
@@ -376,6 +417,7 @@ struct msg_echo_lb {
 	uint16_t rsp_sg_cnt;
 	uint16_t options;
 	uint32_t transfer_size;
+	uint32_t iteration_count;
 };
 
 /*
@@ -542,11 +584,74 @@ struct device_reg_25xxmq {
 	uint32_t atio_q_out;
 };
 
+
+struct device_reg_fx00 {
+	uint32_t mailbox0;		/* 00 */
+	uint32_t mailbox1;		/* 04 */
+	uint32_t mailbox2;		/* 08 */
+	uint32_t mailbox3;		/* 0C */
+	uint32_t mailbox4;		/* 10 */
+	uint32_t mailbox5;		/* 14 */
+	uint32_t mailbox6;		/* 18 */
+	uint32_t mailbox7;		/* 1C */
+	uint32_t mailbox8;		/* 20 */
+	uint32_t mailbox9;		/* 24 */
+	uint32_t mailbox10;		/* 28 */
+	uint32_t mailbox11;
+	uint32_t mailbox12;
+	uint32_t mailbox13;
+	uint32_t mailbox14;
+	uint32_t mailbox15;
+	uint32_t mailbox16;
+	uint32_t mailbox17;
+	uint32_t mailbox18;
+	uint32_t mailbox19;
+	uint32_t mailbox20;
+	uint32_t mailbox21;
+	uint32_t mailbox22;
+	uint32_t mailbox23;
+	uint32_t mailbox24;
+	uint32_t mailbox25;
+	uint32_t mailbox26;
+	uint32_t mailbox27;
+	uint32_t mailbox28;
+	uint32_t mailbox29;
+	uint32_t mailbox30;
+	uint32_t mailbox31;
+	uint32_t aenmailbox0;
+	uint32_t aenmailbox1;
+	uint32_t aenmailbox2;
+	uint32_t aenmailbox3;
+	uint32_t aenmailbox4;
+	uint32_t aenmailbox5;
+	uint32_t aenmailbox6;
+	uint32_t aenmailbox7;
+	/* Request Queue. */
+	uint32_t req_q_in;		/* A0 - Request Queue In-Pointer */
+	uint32_t req_q_out;		/* A4 - Request Queue Out-Pointer */
+	/* Response Queue. */
+	uint32_t rsp_q_in;		/* A8 - Response Queue In-Pointer */
+	uint32_t rsp_q_out;		/* AC - Response Queue Out-Pointer */
+	/* Init values shadowed on FW Up Event */
+	uint32_t initval0;		/* B0 */
+	uint32_t initval1;		/* B4 */
+	uint32_t initval2;		/* B8 */
+	uint32_t initval3;		/* BC */
+	uint32_t initval4;		/* C0 */
+	uint32_t initval5;		/* C4 */
+	uint32_t initval6;		/* C8 */
+	uint32_t initval7;		/* CC */
+	uint32_t fwheartbeat;		/* D0 */
+};
+
+
+
 typedef union {
 		struct device_reg_2xxx isp;
 		struct device_reg_24xx isp24;
 		struct device_reg_25xxmq isp25mq;
 		struct device_reg_82xx isp82;
+		struct device_reg_fx00 ispfx00;
 } device_reg_t;
 
 #define ISP_REQ_Q_IN(ha, reg) \
@@ -602,6 +707,20 @@ typedef struct {
 #define IOCTL_CMD	BIT_2
 } mbx_cmd_t;
 
+struct mbx_cmd_32 {
+	uint32_t	out_mb;		/* outbound from driver */
+	uint32_t	in_mb;			/* Incoming from RISC */
+	uint32_t	mb[MAILBOX_REGISTER_COUNT];
+	long		buf_size;
+	void		*bufp;
+	uint32_t	tov;
+	uint8_t		flags;
+#define MBX_DMA_IN	BIT_0
+#define	MBX_DMA_OUT	BIT_1
+#define IOCTL_CMD	BIT_2
+};
+
+
 #define	MBX_TOV_SECONDS	30
 
 /*
@@ -677,6 +796,15 @@ typedef struct {
 #define MBA_BYPASS_NOTIFICATION	0x8043	/* Auto bypass notification. */
 #define MBA_DISCARD_RND_FRAME	0x8048	/* discard RND frame due to error. */
 #define MBA_REJECTED_FCP_CMD	0x8049	/* rejected FCP_CMD. */
+#define MBA_FW_NOT_STARTED	0x8050	/* Firmware not started */
+#define MBA_FW_STARTING		0x8051	/* Firmware starting */
+#define MBA_FW_RESTART_CMPLT	0x8060	/* Firmware restart complete */
+#define MBA_INIT_REQUIRED	0x8061	/* Initialization required */
+#define MBA_SHUTDOWN_REQUESTED	0x8062	/* Shutdown Requested */
+#define MBA_FW_INIT_FAILURE	0x8401	/* Firmware initialization failure */
+#define MBA_MIRROR_LUN_CHANGE	0x8402	/* Mirror LUN State Change
+					   Notification */
+#define MBA_FW_POLL_STATE	0x8600  /* Firmware in poll diagnostic state */
 
 /* 83XX FCoE specific */
 #define MBA_IDC_AEN		0x8200  /* FCoE: NIC Core state change AEN */
@@ -798,6 +926,12 @@ typedef struct {
 #define MBC_LUN_RESET			0x7E	/* Send LUN reset */
 
 /*
+ * all the Mt. Rainier mailbox command codes that clash with FC/FCoE ones
+ * should be defined with MBC_MR_*
+ */
+#define MBC_MR_DRV_SHUTDOWN		0x6A
+
+/*
  * ISP24xx mailbox commands
  */
 #define MBC_SERDES_PARAMS		0x10	/* Serdes Tx Parameters. */
@@ -863,7 +997,6 @@ typedef struct {
 #define	MBX_1		BIT_1
 #define	MBX_0		BIT_0
 
-#define RNID_TYPE_SET_VERSION	0x9
 #define RNID_TYPE_ASIC_TEMP	0xC
 
 /*
@@ -1059,6 +1192,30 @@ typedef struct {
 	uint8_t  reserved_3[26];
 } init_cb_t;
 
+
+struct init_cb_fx {
+	uint16_t	version;
+	uint16_t	reserved_1[13];
+	uint16_t	request_q_outpointer;
+	uint16_t	response_q_inpointer;
+	uint16_t	reserved_2[2];
+	uint16_t	response_q_length;
+	uint16_t	request_q_length;
+	uint16_t	reserved_3[2];
+	uint32_t	request_q_address[2];
+	uint32_t	response_q_address[2];
+	uint16_t	reserved_4[4];
+	uint8_t		response_q_msivec;
+	uint8_t		reserved_5[19];
+	uint16_t	interrupt_delay_timer;
+	uint16_t	reserved_6;
+	uint32_t	fwoptions1;
+	uint32_t	fwoptions2;
+	uint32_t	fwoptions3;
+	uint8_t		reserved_7[24];
+};
+
+
 /*
  * Get Link Status mailbox command return buffer.
  */
@@ -1832,6 +1989,9 @@ typedef struct fc_port {
 	uint16_t loop_id;
 	uint16_t old_loop_id;
 
+	uint16_t tgt_id;
+	uint16_t old_tgt_id;
+
 	uint8_t fcp_prio;
 
 	uint8_t fabric_port_name[WWN_SIZE];
@@ -1849,8 +2009,15 @@ typedef struct fc_port {
 
 	uint8_t fc4_type;
 	uint8_t scan_state;
+
+	unsigned long last_queue_full;
+	unsigned long last_ramp_up;
+
+	uint16_t port_id;
 } fc_port_t;
 
+#include "qla_mr.h"
+
 /*
  * Fibre channel port/lun states.
  */
@@ -2392,6 +2559,7 @@ struct isp_operations {
 	int (*start_scsi) (srb_t *);
 	int (*abort_isp) (struct scsi_qla_host *);
 	int (*iospace_config)(struct qla_hw_data*);
+	int (*initialize_adapter)(struct scsi_qla_host *);
 };
 
 /* MSI-X Support *************************************************************/
@@ -2430,6 +2598,7 @@ enum qla_work_type {
 	QLA_EVT_ASYNC_ADISC,
 	QLA_EVT_ASYNC_ADISC_DONE,
 	QLA_EVT_UEVENT,
+	QLA_EVT_AENFX,
 };
 
 
@@ -2457,7 +2626,15 @@ struct qla_work_evt {
 			u32 code;
 #define QLA_UEVENT_CODE_FW_DUMP	0
 		} uevent;
-	} u;
+		struct {
+			uint32_t        evtcode;
+			uint32_t        mbx[8];
+			uint32_t        count;
+		} aenfx;
+		struct {
+			srb_t *sp;
+		} iosb;
+	 } u;
 };
 
 struct qla_chip_state_84xx {
@@ -2521,6 +2698,11 @@ struct rsp_que {
 	struct req_que *req;
 	srb_t *status_srb; /* status continuation entry */
 	struct work_struct q_work;
+
+	dma_addr_t  dma_fx00;
+	response_t *ring_fx00;
+	uint16_t  length_fx00;
+	uint8_t rsp_pkt[REQUEST_ENTRY_SIZE];
 };
 
 /* Request queue data structure */
@@ -2545,6 +2727,11 @@ struct req_que {
 	uint16_t num_outstanding_cmds;
 #define	MAX_Q_DEPTH		32
 	int max_q_depth;
+
+	dma_addr_t  dma_fx00;
+	request_t *ring_fx00;
+	uint16_t  length_fx00;
+	uint8_t req_pkt[REQUEST_ENTRY_SIZE];
 };
 
 /* Place holder for FW buffer parameters */
@@ -2634,7 +2821,10 @@ struct qla_hw_data {
 		uint32_t	isp82xx_no_md_cap:1;
 		uint32_t	host_shutting_down:1;
 		uint32_t	idc_compl_status:1;
-		/* 32 bits */
+
+		uint32_t        mr_reset_hdlr_active:1;
+		uint32_t        mr_intr_valid:1;
+		/* 34 bits */
 	} flags;
 
 	/* This spinlock is used to protect "io transactions", you must
@@ -2651,7 +2841,21 @@ struct qla_hw_data {
 	resource_size_t pio_address;
 
 #define MIN_IOBASE_LEN          0x100
-/* Multi queue data structs */
+	dma_addr_t		bar0_hdl;
+
+	void __iomem *cregbase;
+	dma_addr_t		bar2_hdl;
+#define BAR0_LEN_FX00			(1024 * 1024)
+#define BAR2_LEN_FX00			(128 * 1024)
+
+	uint32_t		rqstq_intr_code;
+	uint32_t		mbx_intr_code;
+	uint32_t		req_que_len;
+	uint32_t		rsp_que_len;
+	uint32_t		req_que_off;
+	uint32_t		rsp_que_off;
+
+	/* Multi queue data structs */
 	device_reg_t __iomem *mqiobase;
 	device_reg_t __iomem *msixbase;
 	uint16_t        msix_count;
@@ -2730,7 +2934,8 @@ struct qla_hw_data {
 #define DT_ISP8021			BIT_14
 #define DT_ISP2031			BIT_15
 #define DT_ISP8031			BIT_16
-#define DT_ISP_LAST			(DT_ISP8031 << 1)
+#define DT_ISPFX00			BIT_17
+#define DT_ISP_LAST			(DT_ISPFX00 << 1)
 
 #define DT_T10_PI                       BIT_25
 #define DT_IIDMA                        BIT_26
@@ -2758,6 +2963,7 @@ struct qla_hw_data {
 #define IS_QLA82XX(ha)	(DT_MASK(ha) & DT_ISP8021)
 #define IS_QLA2031(ha)	(DT_MASK(ha) & DT_ISP2031)
 #define IS_QLA8031(ha)	(DT_MASK(ha) & DT_ISP8031)
+#define IS_QLAFX00(ha)	(DT_MASK(ha) & DT_ISPFX00)
 
 #define IS_QLA23XX(ha)  (IS_QLA2300(ha) || IS_QLA2312(ha) || IS_QLA2322(ha) || \
 			IS_QLA6312(ha) || IS_QLA6322(ha))
@@ -2822,6 +3028,7 @@ struct qla_hw_data {
 	uint16_t	r_a_tov;
 	int		port_down_retry_count;
 	uint8_t		mbx_count;
+	uint8_t		aen_mbx_count;
 
 	uint32_t	login_retry_count;
 	/* SNS command interfaces. */
@@ -2869,9 +3076,13 @@ struct qla_hw_data {
 	void		*swl;
 
 	/* These are used by mailbox operations. */
-	volatile uint16_t mailbox_out[MAILBOX_REGISTER_COUNT];
+	uint16_t mailbox_out[MAILBOX_REGISTER_COUNT];
+	uint32_t mailbox_out32[MAILBOX_REGISTER_COUNT];
+	uint32_t aenmb[AEN_MAILBOX_REGISTER_COUNT_FX00];
 
 	mbx_cmd_t	*mcp;
+	struct mbx_cmd_32	*mcp32;
+
 	unsigned long	mbx_cmd_flags;
 #define MBX_INTERRUPT		1
 #define MBX_INTR_WAIT		2
@@ -3015,6 +3226,7 @@ struct qla_hw_data {
 	int             cur_vport_count;
 
 	struct qla_chip_state_84xx *cs84xx;
+	struct qla_statistics qla_stats;
 	struct isp_operations *isp_ops;
 	struct workqueue_struct *wq;
 	struct qlfc_fw fw_buf;
@@ -3081,6 +3293,8 @@ struct qla_hw_data {
 	unsigned long   host_last_rampup_time;
 	int             cfg_lun_q_depth;
 
+	struct mr_data_fx00 mr;
+
 	struct qlt_hw_data tgt;
 	uint16_t	thermal_support;
 #define THERMAL_SUPPORT_I2C BIT_0
@@ -3110,6 +3324,8 @@ typedef struct scsi_qla_host {
 		uint32_t	process_response_queue	:1;
 		uint32_t	difdix_supported:1;
 		uint32_t	delete_progress:1;
+
+		uint32_t	fw_tgt_reported:1;
 	} flags;
 
 	atomic_t	loop_state;
@@ -3145,6 +3361,9 @@ typedef struct scsi_qla_host {
 #define SCR_PENDING		21	/* SCR in target mode */
 #define HOST_RAMP_DOWN_QUEUE_DEPTH     22
 #define HOST_RAMP_UP_QUEUE_DEPTH       23
+#define PORT_UPDATE_NEEDED	24
+#define FX00_RESET_RECOVERY	25
+#define FX00_TARGET_SCAN	26
 
 	uint32_t	device_flags;
 #define SWITCH_FOUND		BIT_0
@@ -3235,6 +3454,10 @@ struct qla_tgt_vp_map {
 	 test_bit(LOOP_RESYNC_NEEDED, &ha->dpc_flags) || \
 	 atomic_read(&ha->loop_state) == LOOP_DOWN)
 
+#define STATE_TRANSITION(ha) \
+		(test_bit(ISP_ABORT_NEEDED, &ha->dpc_flags) || \
+			 test_bit(LOOP_RESYNC_NEEDED, &ha->dpc_flags))
+
 #define QLA_VHA_MARK_BUSY(__vha, __bail) do {		     \
 	atomic_inc(&__vha->vref_count);			     \
 	mb();						     \
diff --git a/drivers/scsi/qla2xxx/qla_gbl.h b/drivers/scsi/qla2xxx/qla_gbl.h
index eb3ca21a7f1..026bfde33e6 100644
--- a/drivers/scsi/qla2xxx/qla_gbl.h
+++ b/drivers/scsi/qla2xxx/qla_gbl.h
@@ -86,6 +86,7 @@ extern int qla2xxx_mctp_dump(scsi_qla_host_t *);
 
 extern int
 qla2x00_alloc_outstanding_cmds(struct qla_hw_data *, struct req_que *);
+extern int qla2x00_init_rings(scsi_qla_host_t *);
 
 /*
  * Global Data in qla_os.c source file.
@@ -134,7 +135,6 @@ extern int qla2x00_post_async_adisc_work(struct scsi_qla_host *, fc_port_t *,
     uint16_t *);
 extern int qla2x00_post_async_adisc_done_work(struct scsi_qla_host *,
     fc_port_t *, uint16_t *);
-extern int qla2x00_post_uevent_work(struct scsi_qla_host *, u32);
 
 extern int qla81xx_restart_mpi_firmware(scsi_qla_host_t *);
 
@@ -158,6 +158,7 @@ extern int qla83xx_set_drv_presence(scsi_qla_host_t *vha);
 extern int __qla83xx_set_drv_presence(scsi_qla_host_t *vha);
 extern int qla83xx_clear_drv_presence(scsi_qla_host_t *vha);
 extern int __qla83xx_clear_drv_presence(scsi_qla_host_t *vha);
+extern int qla2x00_post_uevent_work(struct scsi_qla_host *, u32);
 
 /*
  * Global Functions in qla_mid.c source file.
@@ -211,8 +212,6 @@ extern int qla24xx_start_scsi(srb_t *sp);
 int qla2x00_marker(struct scsi_qla_host *, struct req_que *, struct rsp_que *,
 						uint16_t, uint16_t, uint8_t);
 extern int qla2x00_start_sp(srb_t *);
-extern uint16_t qla24xx_calc_iocbs(scsi_qla_host_t *, uint16_t);
-extern void qla24xx_build_scsi_iocbs(srb_t *, struct cmd_type_7 *, uint16_t);
 extern int qla24xx_dif_start_scsi(srb_t *);
 extern int qla2x00_start_bidir(srb_t *, struct scsi_qla_host *, uint32_t);
 extern unsigned long qla2x00_get_async_timeout(struct scsi_qla_host *);
@@ -358,9 +357,6 @@ extern int
 qla2x00_disable_fce_trace(scsi_qla_host_t *, uint64_t *, uint64_t *);
 
 extern int
-qla2x00_set_driver_version(scsi_qla_host_t *, char *);
-
-extern int
 qla2x00_read_sfp(scsi_qla_host_t *, dma_addr_t, uint8_t *,
 	uint16_t, uint16_t, uint16_t, uint16_t);
 
@@ -427,6 +423,12 @@ extern void qla2x00_free_irqs(scsi_qla_host_t *);
 
 extern int qla2x00_get_data_rate(scsi_qla_host_t *);
 extern const char *qla2x00_get_link_speed_str(struct qla_hw_data *, uint16_t);
+extern srb_t *
+qla2x00_get_sp_from_handle(scsi_qla_host_t *, const char *, struct req_que *,
+	void *);
+extern void
+qla2x00_process_completed_request(struct scsi_qla_host *, struct req_que *,
+	uint32_t);
 
 /*
  * Global Function Prototypes in qla_sup.c source file.
@@ -564,6 +566,42 @@ extern void qla25xx_wrt_req_reg(struct qla_hw_data *, uint16_t, uint16_t);
 extern void qla25xx_wrt_rsp_reg(struct qla_hw_data *, uint16_t, uint16_t);
 extern void qla24xx_wrt_rsp_reg(struct qla_hw_data *, uint16_t, uint16_t);
 
+/* qlafx00 related functions */
+extern int qlafx00_pci_config(struct scsi_qla_host *);
+extern int qlafx00_initialize_adapter(struct scsi_qla_host *);
+extern void qlafx00_soft_reset(scsi_qla_host_t *);
+extern int qlafx00_chip_diag(scsi_qla_host_t *);
+extern void qlafx00_config_rings(struct scsi_qla_host *);
+extern char *qlafx00_pci_info_str(struct scsi_qla_host *, char *);
+extern char *qlafx00_fw_version_str(struct scsi_qla_host *, char *);
+extern irqreturn_t qlafx00_intr_handler(int, void *);
+extern void qlafx00_enable_intrs(struct qla_hw_data *);
+extern void qlafx00_disable_intrs(struct qla_hw_data *);
+extern int qlafx00_abort_command(srb_t *);
+extern int qlafx00_abort_target(fc_port_t *, unsigned int, int);
+extern int qlafx00_lun_reset(fc_port_t *, unsigned int, int);
+extern int qlafx00_start_scsi(srb_t *);
+extern int qlafx00_abort_isp(scsi_qla_host_t *);
+extern int qlafx00_iospace_config(struct qla_hw_data *);
+extern int qlafx00_init_firmware(scsi_qla_host_t *, uint16_t);
+extern int qlafx00_fw_ready(scsi_qla_host_t *);
+extern int qlafx00_configure_devices(scsi_qla_host_t *);
+extern int qlafx00_reset_initialize(scsi_qla_host_t *);
+extern int qlafx00_fx_disc(scsi_qla_host_t *, fc_port_t *, uint8_t);
+extern int qlafx00_process_aen(struct scsi_qla_host *, struct qla_work_evt *);
+extern int qlafx00_post_aenfx_work(struct scsi_qla_host *,  uint32_t,
+				   uint32_t *, int);
+extern uint32_t qlafx00_fw_state_show(struct device *,
+				      struct device_attribute *, char *);
+extern void qlafx00_get_host_speed(struct Scsi_Host *);
+extern void qlafx00_init_response_q_entries(struct rsp_que *);
+
+extern void qlafx00_tm_iocb(srb_t *, struct tsk_mgmt_entry_fx00 *);
+extern void qlafx00_abort_iocb(srb_t *, struct abort_iocb_entry_fx00 *);
+extern void qlafx00_fxdisc_iocb(srb_t *, struct fxdisc_entry_fx00 *);
+extern void qlafx00_timer_routine(scsi_qla_host_t *);
+extern int qlafx00_rescan_isp(scsi_qla_host_t *);
+
 /* qla82xx related functions */
 
 /* PCI related functions */
diff --git a/drivers/scsi/qla2xxx/qla_gs.c b/drivers/scsi/qla2xxx/qla_gs.c
index 9b455250c10..d0ea8b92117 100644
--- a/drivers/scsi/qla2xxx/qla_gs.c
+++ b/drivers/scsi/qla2xxx/qla_gs.c
@@ -639,9 +639,14 @@ void
 qla2x00_get_sym_node_name(scsi_qla_host_t *vha, uint8_t *snn)
 {
 	struct qla_hw_data *ha = vha->hw;
-	sprintf(snn, "%s FW:v%d.%02d.%02d DVR:v%s",ha->model_number,
-	    ha->fw_major_version, ha->fw_minor_version,
-	    ha->fw_subminor_version, qla2x00_version_str);
+
+	if (IS_QLAFX00(ha))
+		sprintf(snn, "%s FW:v%s DVR:v%s", ha->model_number,
+		    ha->mr.fw_version, qla2x00_version_str);
+	else
+		sprintf(snn, "%s FW:v%d.%02d.%02d DVR:v%s", ha->model_number,
+		    ha->fw_major_version, ha->fw_minor_version,
+		    ha->fw_subminor_version, qla2x00_version_str);
 }
 
 /**
@@ -923,7 +928,7 @@ qla2x00_sns_gpn_id(scsi_qla_host_t *vha, sw_info_t *list)
 		    sns_cmd->p.gpn_data[9] != 0x02) {
 			ql_dbg(ql_dbg_disc + ql_dbg_buffer, vha, 0x207e,
 			    "GPN_ID failed, rejected request, gpn_rsp:\n");
-			ql_dump_buffer(ql_dbg_disc, vha, 0x207f,
+			ql_dump_buffer(ql_dbg_disc + ql_dbg_buffer, vha, 0x207f,
 			    sns_cmd->p.gpn_data, 16);
 			rval = QLA_FUNCTION_FAILED;
 		} else {
@@ -1718,7 +1723,8 @@ qla2x00_fdmi_register(scsi_qla_host_t *vha)
 	int rval;
        struct qla_hw_data *ha = vha->hw;
 
-	if (IS_QLA2100(ha) || IS_QLA2200(ha))
+	if (IS_QLA2100(ha) || IS_QLA2200(ha) ||
+	    IS_QLAFX00(ha))
 		return QLA_FUNCTION_FAILED;
 
 	rval = qla2x00_mgmt_svr_login(vha);
diff --git a/drivers/scsi/qla2xxx/qla_init.c b/drivers/scsi/qla2xxx/qla_init.c
index edf4d14a133..3565dfd8f37 100644
--- a/drivers/scsi/qla2xxx/qla_init.c
+++ b/drivers/scsi/qla2xxx/qla_init.c
@@ -25,7 +25,6 @@
 */
 static int qla2x00_isp_firmware(scsi_qla_host_t *);
 static int qla2x00_setup_chip(scsi_qla_host_t *);
-static int qla2x00_init_rings(scsi_qla_host_t *);
 static int qla2x00_fw_ready(scsi_qla_host_t *);
 static int qla2x00_configure_hba(scsi_qla_host_t *);
 static int qla2x00_configure_loop(scsi_qla_host_t *);
@@ -83,7 +82,9 @@ qla2x00_get_async_timeout(struct scsi_qla_host *vha)
 
 	/* Firmware should use switch negotiated r_a_tov for timeout. */
 	tmo = ha->r_a_tov / 10 * 2;
-	if (!IS_FWI2_CAPABLE(ha)) {
+	if (IS_QLAFX00(ha)) {
+		tmo = FX00_DEF_RATOV * 2;
+	} else if (!IS_FWI2_CAPABLE(ha)) {
 		/*
 		 * Except for earlier ISPs where the timeout is seeded from the
 		 * initialization control block.
@@ -619,8 +620,6 @@ qla2x00_initialize_adapter(scsi_qla_host_t *vha)
 	if (IS_QLA24XX_TYPE(ha) || IS_QLA25XX(ha))
 		qla24xx_read_fcp_prio_cfg(vha);
 
-	qla2x00_set_driver_version(vha, QLA2XXX_VERSION);
-
 	return (rval);
 }
 
@@ -1399,7 +1398,7 @@ qla2x00_alloc_fw_dump(scsi_qla_host_t *vha)
 			mq_size += ha->max_rsp_queues *
 			    (rsp->length * sizeof(response_t));
 		}
-		if (ha->tgt.atio_q_length)
+		if (ha->tgt.atio_ring)
 			mq_size += ha->tgt.atio_q_length * sizeof(request_t);
 		/* Allocate memory for Fibre Channel Event Buffer. */
 		if (!IS_QLA25XX(ha) && !IS_QLA81XX(ha) && !IS_QLA83XX(ha))
@@ -1979,7 +1978,7 @@ qla24xx_config_rings(struct scsi_qla_host *vha)
  *
  * Returns 0 on success.
  */
-static int
+int
 qla2x00_init_rings(scsi_qla_host_t *vha)
 {
 	int	rval;
@@ -2014,7 +2013,10 @@ qla2x00_init_rings(scsi_qla_host_t *vha)
 		if (!rsp)
 			continue;
 		/* Initialize response queue entries */
-		qla2x00_init_response_q_entries(rsp);
+		if (IS_QLAFX00(ha))
+			qlafx00_init_response_q_entries(rsp);
+		else
+			qla2x00_init_response_q_entries(rsp);
 	}
 
 	ha->tgt.atio_ring_ptr = ha->tgt.atio_ring;
@@ -2026,11 +2028,16 @@ qla2x00_init_rings(scsi_qla_host_t *vha)
 
 	spin_unlock_irqrestore(&ha->hardware_lock, flags);
 
+	ql_dbg(ql_dbg_init, vha, 0x00d1, "Issue init firmware.\n");
+
+	if (IS_QLAFX00(ha)) {
+		rval = qlafx00_init_firmware(vha, ha->init_cb_size);
+		goto next_check;
+	}
+
 	/* Update any ISP specific firmware options before initialization. */
 	ha->isp_ops->update_fw_options(vha);
 
-	ql_dbg(ql_dbg_init, vha, 0x00d1, "Issue init firmware.\n");
-
 	if (ha->flags.npiv_supported) {
 		if (ha->operating_mode == LOOP && !IS_CNA_CAPABLE(ha))
 			ha->max_npiv_vports = MIN_MULTI_ID_FABRIC - 1;
@@ -2044,6 +2051,7 @@ qla2x00_init_rings(scsi_qla_host_t *vha)
 	}
 
 	rval = qla2x00_init_firmware(vha, ha->init_cb_size);
+next_check:
 	if (rval) {
 		ql_log(ql_log_fatal, vha, 0x00d2,
 		    "Init Firmware **** FAILED ****.\n");
@@ -2071,6 +2079,9 @@ qla2x00_fw_ready(scsi_qla_host_t *vha)
 	uint16_t	state[5];
 	struct qla_hw_data *ha = vha->hw;
 
+	if (IS_QLAFX00(vha->hw))
+		return qlafx00_fw_ready(vha);
+
 	rval = QLA_SUCCESS;
 
 	/* 20 seconds for loop down. */
@@ -3136,6 +3147,12 @@ void
 qla2x00_update_fcport(scsi_qla_host_t *vha, fc_port_t *fcport)
 {
 	fcport->vha = vha;
+
+	if (IS_QLAFX00(vha->hw)) {
+		qla2x00_set_fcport_state(fcport, FCS_ONLINE);
+		qla2x00_reg_remote_port(vha, fcport);
+		return;
+	}
 	fcport->login_retry = 0;
 	fcport->flags &= ~(FCF_LOGIN_NEEDED | FCF_ASYNC_SENT);
 
@@ -3896,15 +3913,24 @@ qla2x00_loop_resync(scsi_qla_host_t *vha)
 			/* Wait at most MAX_TARGET RSCNs for a stable link. */
 			wait_time = 256;
 			do {
-				/* Issue a marker after FW becomes ready. */
-				qla2x00_marker(vha, req, rsp, 0, 0,
-					MK_SYNC_ALL);
-				vha->marker_needed = 0;
+				if (!IS_QLAFX00(vha->hw)) {
+					/*
+					 * Issue a marker after FW becomes
+					 * ready.
+					 */
+					qla2x00_marker(vha, req, rsp, 0, 0,
+						MK_SYNC_ALL);
+					vha->marker_needed = 0;
+				}
 
 				/* Remap devices on Loop. */
 				clear_bit(LOOP_RESYNC_NEEDED, &vha->dpc_flags);
 
-				qla2x00_configure_loop(vha);
+				if (IS_QLAFX00(vha->hw))
+					qlafx00_configure_devices(vha);
+				else
+					qla2x00_configure_loop(vha);
+
 				wait_time--;
 			} while (!atomic_read(&vha->loop_down_timer) &&
 				!(test_bit(ISP_ABORT_NEEDED, &vha->dpc_flags))
@@ -3970,9 +3996,7 @@ qla2x00_update_fcports(scsi_qla_host_t *base_vha)
 			if (fcport->drport &&
 			    atomic_read(&fcport->state) != FCS_UNCONFIGURED) {
 				spin_unlock_irqrestore(&ha->vport_slock, flags);
-
 				qla2x00_rport_del(fcport);
-
 				spin_lock_irqsave(&ha->vport_slock, flags);
 			}
 		}
diff --git a/drivers/scsi/qla2xxx/qla_inline.h b/drivers/scsi/qla2xxx/qla_inline.h
index 68e2c4afc13..98ab921070d 100644
--- a/drivers/scsi/qla2xxx/qla_inline.h
+++ b/drivers/scsi/qla2xxx/qla_inline.h
@@ -5,6 +5,28 @@
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
 
+/**
+ * qla24xx_calc_iocbs() - Determine number of Command Type 3 and
+ * Continuation Type 1 IOCBs to allocate.
+ *
+ * @dsds: number of data segment decriptors needed
+ *
+ * Returns the number of IOCB entries needed to store @dsds.
+ */
+static inline uint16_t
+qla24xx_calc_iocbs(scsi_qla_host_t *vha, uint16_t dsds)
+{
+	uint16_t iocbs;
+
+	iocbs = 1;
+	if (dsds > 1) {
+		iocbs += (dsds - 1) / 5;
+		if ((dsds - 1) % 5)
+			iocbs++;
+	}
+	return iocbs;
+}
+
 /*
  * qla2x00_debounce_register
  *      Debounce register.
@@ -58,6 +80,17 @@ host_to_fcp_swap(uint8_t *fcp, uint32_t bsize)
 }
 
 static inline void
+host_to_adap(uint8_t *src, uint8_t *dst, uint32_t bsize)
+{
+	uint32_t *isrc = (uint32_t *) src;
+	uint32_t *odest = (uint32_t *) dst;
+	uint32_t iter = bsize >> 2;
+
+	for (; iter ; iter--)
+		*odest++ = cpu_to_le32(*isrc++);
+}
+
+static inline void
 qla2x00_set_reserved_loop_ids(struct qla_hw_data *ha)
 {
 	int i;
@@ -213,12 +246,18 @@ qla2x00_init_timer(srb_t *sp, unsigned long tmo)
 	sp->u.iocb_cmd.timer.function = qla2x00_sp_timeout;
 	add_timer(&sp->u.iocb_cmd.timer);
 	sp->free = qla2x00_sp_free;
+	if ((IS_QLAFX00(sp->fcport->vha->hw)) &&
+	    (sp->type == SRB_FXIOCB_DCMD))
+		init_completion(&sp->u.iocb_cmd.u.fxiocb.fxiocb_comp);
 }
 
 static inline int
 qla2x00_gid_list_size(struct qla_hw_data *ha)
 {
-	return sizeof(struct gid_list_info) * ha->max_fibre_devices;
+	if (IS_QLAFX00(ha))
+		return sizeof(uint32_t) * 32;
+	else
+		return sizeof(struct gid_list_info) * ha->max_fibre_devices;
 }
 
 static inline void
diff --git a/drivers/scsi/qla2xxx/qla_iocb.c b/drivers/scsi/qla2xxx/qla_iocb.c
index d2630317cce..15e4080b347 100644
--- a/drivers/scsi/qla2xxx/qla_iocb.c
+++ b/drivers/scsi/qla2xxx/qla_iocb.c
@@ -135,7 +135,8 @@ qla2x00_prep_cont_type1_iocb(scsi_qla_host_t *vha, struct req_que *req)
 	cont_pkt = (cont_a64_entry_t *)req->ring_ptr;
 
 	/* Load packet defaults. */
-	*((uint32_t *)(&cont_pkt->entry_type)) =
+	*((uint32_t *)(&cont_pkt->entry_type)) = IS_QLAFX00(vha->hw) ?
+	    __constant_cpu_to_le32(CONTINUE_A64_TYPE_FX00) :
 	    __constant_cpu_to_le32(CONTINUE_A64_TYPE);
 
 	return (cont_pkt);
@@ -486,6 +487,10 @@ qla2x00_start_iocbs(struct scsi_qla_host *vha, struct req_que *req)
 		if (ha->mqenable || IS_QLA83XX(ha)) {
 			WRT_REG_DWORD(req->req_q_in, req->ring_index);
 			RD_REG_DWORD_RELAXED(&ha->iobase->isp24.hccr);
+		} else if (IS_QLAFX00(ha)) {
+			WRT_REG_DWORD(&reg->ispfx00.req_q_in, req->ring_index);
+			RD_REG_DWORD_RELAXED(&reg->ispfx00.req_q_in);
+			QLAFX00_SET_HST_INTR(ha, ha->rqstq_intr_code);
 		} else if (IS_FWI2_CAPABLE(ha)) {
 			WRT_REG_DWORD(&reg->isp24.req_q_in, req->ring_index);
 			RD_REG_DWORD_RELAXED(&reg->isp24.req_q_in);
@@ -514,11 +519,12 @@ __qla2x00_marker(struct scsi_qla_host *vha, struct req_que *req,
 			uint16_t lun, uint8_t type)
 {
 	mrk_entry_t *mrk;
-	struct mrk_entry_24xx *mrk24;
+	struct mrk_entry_24xx *mrk24 = NULL;
+	struct mrk_entry_fx00 *mrkfx = NULL;
+
 	struct qla_hw_data *ha = vha->hw;
 	scsi_qla_host_t *base_vha = pci_get_drvdata(ha->pdev);
 
-	mrk24 = NULL;
 	req = ha->req_q_map[0];
 	mrk = (mrk_entry_t *)qla2x00_alloc_iocbs(vha, NULL);
 	if (mrk == NULL) {
@@ -531,7 +537,15 @@ __qla2x00_marker(struct scsi_qla_host *vha, struct req_que *req,
 	mrk->entry_type = MARKER_TYPE;
 	mrk->modifier = type;
 	if (type != MK_SYNC_ALL) {
-		if (IS_FWI2_CAPABLE(ha)) {
+		if (IS_QLAFX00(ha)) {
+			mrkfx = (struct mrk_entry_fx00 *) mrk;
+			mrkfx->handle = MAKE_HANDLE(req->id, mrkfx->handle);
+			mrkfx->handle_hi = 0;
+			mrkfx->tgt_id = cpu_to_le16(loop_id);
+			mrkfx->lun[1] = LSB(lun);
+			mrkfx->lun[2] = MSB(lun);
+			host_to_fcp_swap(mrkfx->lun, sizeof(mrkfx->lun));
+		} else if (IS_FWI2_CAPABLE(ha)) {
 			mrk24 = (struct mrk_entry_24xx *) mrk;
 			mrk24->nport_handle = cpu_to_le16(loop_id);
 			mrk24->lun[1] = LSB(lun);
@@ -589,28 +603,6 @@ int qla2x00_issue_marker(scsi_qla_host_t *vha, int ha_locked)
 	return QLA_SUCCESS;
 }
 
-/**
- * qla24xx_calc_iocbs() - Determine number of Command Type 3 and
- * Continuation Type 1 IOCBs to allocate.
- *
- * @dsds: number of data segment decriptors needed
- *
- * Returns the number of IOCB entries needed to store @dsds.
- */
-inline uint16_t
-qla24xx_calc_iocbs(scsi_qla_host_t *vha, uint16_t dsds)
-{
-	uint16_t iocbs;
-
-	iocbs = 1;
-	if (dsds > 1) {
-		iocbs += (dsds - 1) / 5;
-		if ((dsds - 1) % 5)
-			iocbs++;
-	}
-	return iocbs;
-}
-
 static inline int
 qla24xx_build_scsi_type_6_iocbs(srb_t *sp, struct cmd_type_6 *cmd_pkt,
 	uint16_t tot_dsds)
@@ -1583,7 +1575,6 @@ queuing_error:
 	return QLA_FUNCTION_FAILED;
 }
 
-
 /**
  * qla24xx_dif_start_scsi() - Send a SCSI command to the ISP
  * @sp: command to send to the ISP
@@ -1852,6 +1843,8 @@ skip_cmd_array:
 			cnt = RD_REG_DWORD(&reg->isp82.req_q_out);
 		else if (IS_FWI2_CAPABLE(ha))
 			cnt = RD_REG_DWORD(&reg->isp24.req_q_out);
+		else if (IS_QLAFX00(ha))
+			cnt = RD_REG_DWORD(&reg->ispfx00.req_q_out);
 		else
 			cnt = qla2x00_debounce_register(
 			    ISP_REQ_Q_OUT(ha, &reg->isp));
@@ -1869,8 +1862,13 @@ skip_cmd_array:
 	req->cnt -= req_cnt;
 	pkt = req->ring_ptr;
 	memset(pkt, 0, REQUEST_ENTRY_SIZE);
-	pkt->entry_count = req_cnt;
-	pkt->handle = handle;
+	if (IS_QLAFX00(ha)) {
+		WRT_REG_BYTE(&pkt->entry_count, req_cnt);
+		WRT_REG_WORD(&pkt->handle, handle);
+	} else {
+		pkt->entry_count = req_cnt;
+		pkt->handle = handle;
+	}
 
 queuing_error:
 	return pkt;
@@ -2625,7 +2623,16 @@ qla2x00_start_sp(srb_t *sp)
 		    qla2x00_adisc_iocb(sp, pkt);
 		break;
 	case SRB_TM_CMD:
-		qla24xx_tm_iocb(sp, pkt);
+		IS_QLAFX00(ha) ?
+		    qlafx00_tm_iocb(sp, pkt) :
+		    qla24xx_tm_iocb(sp, pkt);
+		break;
+	case SRB_FXIOCB_DCMD:
+	case SRB_FXIOCB_BCMD:
+		qlafx00_fxdisc_iocb(sp, pkt);
+		break;
+	case SRB_ABT_CMD:
+		qlafx00_abort_iocb(sp, pkt);
 		break;
 	default:
 		break;
diff --git a/drivers/scsi/qla2xxx/qla_isr.c b/drivers/scsi/qla2xxx/qla_isr.c
index e9dbd74c20d..259d9205d87 100644
--- a/drivers/scsi/qla2xxx/qla_isr.c
+++ b/drivers/scsi/qla2xxx/qla_isr.c
@@ -13,11 +13,7 @@
 #include <scsi/scsi_bsg_fc.h>
 #include <scsi/scsi_eh.h>
 
-#include "qla_target.h"
-
 static void qla2x00_mbx_completion(scsi_qla_host_t *, uint16_t);
-static void qla2x00_process_completed_request(struct scsi_qla_host *,
-	struct req_que *, uint32_t);
 static void qla2x00_status_entry(scsi_qla_host_t *, struct rsp_que *, void *);
 static void qla2x00_status_cont_entry(struct rsp_que *, sts_cont_entry_t *);
 static void qla2x00_error_entry(scsi_qla_host_t *, struct rsp_que *,
@@ -1065,9 +1061,9 @@ skip_rio:
  * @ha: SCSI driver HA context
  * @index: SRB index
  */
-static void
+void
 qla2x00_process_completed_request(struct scsi_qla_host *vha,
-				struct req_que *req, uint32_t index)
+				  struct req_que *req, uint32_t index)
 {
 	srb_t *sp;
 	struct qla_hw_data *ha = vha->hw;
@@ -1101,7 +1097,7 @@ qla2x00_process_completed_request(struct scsi_qla_host *vha,
 	}
 }
 
-static srb_t *
+srb_t *
 qla2x00_get_sp_from_handle(scsi_qla_host_t *vha, const char *func,
     struct req_que *req, void *iocb)
 {
@@ -1994,7 +1990,7 @@ qla2x00_status_entry(scsi_qla_host_t *vha, struct rsp_que *rsp, void *pkt)
 		return;
 	}
 
-  	lscsi_status = scsi_status & STATUS_MASK;
+	lscsi_status = scsi_status & STATUS_MASK;
 
 	fcport = sp->fcport;
 
@@ -2939,7 +2935,7 @@ qla2x00_request_irqs(struct qla_hw_data *ha, struct rsp_que *rsp)
 
 	/* If possible, enable MSI-X. */
 	if (!IS_QLA2432(ha) && !IS_QLA2532(ha) && !IS_QLA8432(ha) &&
-		!IS_CNA_CAPABLE(ha) && !IS_QLA2031(ha))
+		!IS_CNA_CAPABLE(ha) && !IS_QLA2031(ha) && !IS_QLAFX00(ha))
 		goto skip_msi;
 
 	if (ha->pdev->subsystem_vendor == PCI_VENDOR_ID_HP &&
@@ -2972,7 +2968,7 @@ qla2x00_request_irqs(struct qla_hw_data *ha, struct rsp_que *rsp)
 skip_msix:
 
 	if (!IS_QLA24XX(ha) && !IS_QLA2532(ha) && !IS_QLA8432(ha) &&
-	    !IS_QLA8001(ha) && !IS_QLA82XX(ha))
+	    !IS_QLA8001(ha) && !IS_QLA82XX(ha) && !IS_QLAFX00(ha))
 		goto skip_msi;
 
 	ret = pci_enable_msi(ha->pdev);
@@ -2998,9 +2994,11 @@ skip_msi:
 		    "Failed to reserve interrupt %d already in use.\n",
 		    ha->pdev->irq);
 		goto fail;
-	} else if (!ha->flags.msi_enabled)
+	} else if (!ha->flags.msi_enabled) {
 		ql_dbg(ql_dbg_init, vha, 0x0125,
 		    "INTa mode: Enabled.\n");
+		ha->flags.mr_intr_valid = 1;
+	}
 
 clear_risc_ints:
 
diff --git a/drivers/scsi/qla2xxx/qla_mbx.c b/drivers/scsi/qla2xxx/qla_mbx.c
index 186dd59ce4f..9e5d89db727 100644
--- a/drivers/scsi/qla2xxx/qla_mbx.c
+++ b/drivers/scsi/qla2xxx/qla_mbx.c
@@ -3866,64 +3866,6 @@ qla81xx_restart_mpi_firmware(scsi_qla_host_t *vha)
 	return rval;
 }
 
-int
-qla2x00_set_driver_version(scsi_qla_host_t *vha, char *version)
-{
-	int rval;
-	mbx_cmd_t mc;
-	mbx_cmd_t *mcp = &mc;
-	int len;
-	uint16_t dwlen;
-	uint8_t *str;
-	dma_addr_t str_dma;
-	struct qla_hw_data *ha = vha->hw;
-
-	if (!IS_FWI2_CAPABLE(ha) || IS_QLA82XX(ha))
-		return QLA_FUNCTION_FAILED;
-
-	ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x1155,
-	    "Entered %s.\n", __func__);
-
-	str = dma_pool_alloc(ha->s_dma_pool, GFP_KERNEL, &str_dma);
-	if (!str) {
-		ql_log(ql_log_warn, vha, 0x1156,
-		    "Failed to allocate driver version param.\n");
-		return QLA_MEMORY_ALLOC_FAILED;
-	}
-
-	memcpy(str, "\x7\x3\x11\x0", 4);
-	dwlen = str[0];
-	len = dwlen * sizeof(uint32_t) - 4;
-	memset(str + 4, 0, len);
-	if (len > strlen(version))
-		len = strlen(version);
-	memcpy(str + 4, version, len);
-
-	mcp->mb[0] = MBC_SET_RNID_PARAMS;
-	mcp->mb[1] = RNID_TYPE_SET_VERSION << 8 | dwlen;
-	mcp->mb[2] = MSW(LSD(str_dma));
-	mcp->mb[3] = LSW(LSD(str_dma));
-	mcp->mb[6] = MSW(MSD(str_dma));
-	mcp->mb[7] = LSW(MSD(str_dma));
-	mcp->out_mb = MBX_7|MBX_6|MBX_3|MBX_2|MBX_1|MBX_0;
-	mcp->in_mb = MBX_0;
-	mcp->tov = MBX_TOV_SECONDS;
-	mcp->flags = 0;
-	rval = qla2x00_mailbox_command(vha, mcp);
-
-	if (rval != QLA_SUCCESS) {
-		ql_dbg(ql_dbg_mbx, vha, 0x1157,
-		    "Failed=%x mb[0]=%x.\n", rval, mcp->mb[0]);
-	} else {
-		ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x1158,
-		    "Done %s.\n", __func__);
-	}
-
-	dma_pool_free(ha->s_dma_pool, str, str_dma);
-
-	return rval;
-}
-
 static int
 qla2x00_read_asic_temperature(scsi_qla_host_t *vha, uint16_t *temp)
 {
@@ -4171,7 +4113,6 @@ qla2x00_loopback_test(scsi_qla_host_t *vha, struct msg_echo_lb *mreq,
 	int rval;
 	mbx_cmd_t mc;
 	mbx_cmd_t *mcp = &mc;
-	uint32_t iter_cnt = 0x1;
 
 	ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x10f7,
 	    "Entered %s.\n", __func__);
@@ -4197,8 +4138,8 @@ qla2x00_loopback_test(scsi_qla_host_t *vha, struct msg_echo_lb *mreq,
 	mcp->mb[7] = MSW(MSD(mreq->rcv_dma));
 
 	/* Iteration count */
-	mcp->mb[18] = LSW(iter_cnt);
-	mcp->mb[19] = MSW(iter_cnt);
+	mcp->mb[18] = LSW(mreq->iteration_count);
+	mcp->mb[19] = MSW(mreq->iteration_count);
 
 	mcp->out_mb = MBX_21|MBX_20|MBX_19|MBX_18|MBX_17|MBX_16|MBX_15|
 	    MBX_14|MBX_13|MBX_12|MBX_11|MBX_10|MBX_7|MBX_6|MBX_1|MBX_0;
@@ -4576,7 +4517,8 @@ qla2x00_get_thermal_temp(scsi_qla_host_t *vha, uint16_t *temp)
 			goto done;
 
 		ql_log(ql_log_warn, vha, 0x10c9,
-		    "Thermal not supported by I2C.\n");
+		    "Thermal not supported through I2C bus, trying alternate "
+		    "method (ISP access).\n");
 		ha->thermal_support &= ~THERMAL_SUPPORT_I2C;
 	}
 
@@ -4586,7 +4528,7 @@ qla2x00_get_thermal_temp(scsi_qla_host_t *vha, uint16_t *temp)
 			goto done;
 
 		ql_log(ql_log_warn, vha, 0x1019,
-		    "Thermal not supported by ISP.\n");
+		    "Thermal not supported through ISP.\n");
 		ha->thermal_support &= ~THERMAL_SUPPORT_ISP;
 	}
 
diff --git a/drivers/scsi/qla2xxx/qla_mr.c b/drivers/scsi/qla2xxx/qla_mr.c
new file mode 100644
index 00000000000..729b74389f8
--- /dev/null
+++ b/drivers/scsi/qla2xxx/qla_mr.c
@@ -0,0 +1,3476 @@
+/*
+ * QLogic Fibre Channel HBA Driver
+ * Copyright (c)  2003-2013 QLogic Corporation
+ *
+ * See LICENSE.qla2xxx for copyright and licensing details.
+ */
+#include "qla_def.h"
+#include <linux/delay.h>
+#include <linux/pci.h>
+#include <linux/ratelimit.h>
+#include <linux/vmalloc.h>
+#include <scsi/scsi_tcq.h>
+#include <linux/utsname.h>
+
+
+/* QLAFX00 specific Mailbox implementation functions */
+
+/*
+ * qlafx00_mailbox_command
+ *	Issue mailbox command and waits for completion.
+ *
+ * Input:
+ *	ha = adapter block pointer.
+ *	mcp = driver internal mbx struct pointer.
+ *
+ * Output:
+ *	mb[MAX_MAILBOX_REGISTER_COUNT] = returned mailbox data.
+ *
+ * Returns:
+ *	0 : QLA_SUCCESS = cmd performed success
+ *	1 : QLA_FUNCTION_FAILED   (error encountered)
+ *	6 : QLA_FUNCTION_TIMEOUT (timeout condition encountered)
+ *
+ * Context:
+ *	Kernel context.
+ */
+static int
+qlafx00_mailbox_command(scsi_qla_host_t *vha, struct mbx_cmd_32 *mcp)
+
+{
+	int		rval;
+	unsigned long    flags = 0;
+	device_reg_t __iomem *reg;
+	uint8_t		abort_active;
+	uint8_t		io_lock_on;
+	uint16_t	command = 0;
+	uint32_t	*iptr;
+	uint32_t __iomem *optr;
+	uint32_t	cnt;
+	uint32_t	mboxes;
+	unsigned long	wait_time;
+	struct qla_hw_data *ha = vha->hw;
+	scsi_qla_host_t *base_vha = pci_get_drvdata(ha->pdev);
+
+	if (ha->pdev->error_state > pci_channel_io_frozen) {
+		ql_log(ql_log_warn, vha, 0x115c,
+		    "error_state is greater than pci_channel_io_frozen, "
+		    "exiting.\n");
+		return QLA_FUNCTION_TIMEOUT;
+	}
+
+	if (vha->device_flags & DFLG_DEV_FAILED) {
+		ql_log(ql_log_warn, vha, 0x115f,
+		    "Device in failed state, exiting.\n");
+		return QLA_FUNCTION_TIMEOUT;
+	}
+
+	reg = ha->iobase;
+	io_lock_on = base_vha->flags.init_done;
+
+	rval = QLA_SUCCESS;
+	abort_active = test_bit(ABORT_ISP_ACTIVE, &base_vha->dpc_flags);
+
+	if (ha->flags.pci_channel_io_perm_failure) {
+		ql_log(ql_log_warn, vha, 0x1175,
+		    "Perm failure on EEH timeout MBX, exiting.\n");
+		return QLA_FUNCTION_TIMEOUT;
+	}
+
+	if (ha->flags.isp82xx_fw_hung) {
+		/* Setting Link-Down error */
+		mcp->mb[0] = MBS_LINK_DOWN_ERROR;
+		ql_log(ql_log_warn, vha, 0x1176,
+		    "FW hung = %d.\n", ha->flags.isp82xx_fw_hung);
+		rval = QLA_FUNCTION_FAILED;
+		goto premature_exit;
+	}
+
+	/*
+	 * Wait for active mailbox commands to finish by waiting at most tov
+	 * seconds. This is to serialize actual issuing of mailbox cmds during
+	 * non ISP abort time.
+	 */
+	if (!wait_for_completion_timeout(&ha->mbx_cmd_comp, mcp->tov * HZ)) {
+		/* Timeout occurred. Return error. */
+		ql_log(ql_log_warn, vha, 0x1177,
+		    "Cmd access timeout, cmd=0x%x, Exiting.\n",
+		    mcp->mb[0]);
+		return QLA_FUNCTION_TIMEOUT;
+	}
+
+	ha->flags.mbox_busy = 1;
+	/* Save mailbox command for debug */
+	ha->mcp32 = mcp;
+
+	ql_dbg(ql_dbg_mbx, vha, 0x1178,
+	    "Prepare to issue mbox cmd=0x%x.\n", mcp->mb[0]);
+
+	spin_lock_irqsave(&ha->hardware_lock, flags);
+
+	/* Load mailbox registers. */
+	optr = (uint32_t __iomem *)&reg->ispfx00.mailbox0;
+
+	iptr = mcp->mb;
+	command = mcp->mb[0];
+	mboxes = mcp->out_mb;
+
+	for (cnt = 0; cnt < ha->mbx_count; cnt++) {
+		if (mboxes & BIT_0)
+			WRT_REG_DWORD(optr, *iptr);
+
+		mboxes >>= 1;
+		optr++;
+		iptr++;
+	}
+
+	/* Issue set host interrupt command to send cmd out. */
+	ha->flags.mbox_int = 0;
+	clear_bit(MBX_INTERRUPT, &ha->mbx_cmd_flags);
+
+	ql_dump_buffer(ql_dbg_mbx + ql_dbg_buffer, vha, 0x1172,
+	    (uint8_t *)mcp->mb, 16);
+	ql_dump_buffer(ql_dbg_mbx + ql_dbg_buffer, vha, 0x1173,
+	    ((uint8_t *)mcp->mb + 0x10), 16);
+	ql_dump_buffer(ql_dbg_mbx + ql_dbg_buffer, vha, 0x1174,
+	    ((uint8_t *)mcp->mb + 0x20), 8);
+
+	/* Unlock mbx registers and wait for interrupt */
+	ql_dbg(ql_dbg_mbx, vha, 0x1179,
+	    "Going to unlock irq & waiting for interrupts. "
+	    "jiffies=%lx.\n", jiffies);
+
+	/* Wait for mbx cmd completion until timeout */
+	if ((!abort_active && io_lock_on) || IS_NOPOLLING_TYPE(ha)) {
+		set_bit(MBX_INTR_WAIT, &ha->mbx_cmd_flags);
+
+		QLAFX00_SET_HST_INTR(ha, ha->mbx_intr_code);
+		spin_unlock_irqrestore(&ha->hardware_lock, flags);
+
+		wait_for_completion_timeout(&ha->mbx_intr_comp, mcp->tov * HZ);
+
+		clear_bit(MBX_INTR_WAIT, &ha->mbx_cmd_flags);
+
+	} else {
+		ql_dbg(ql_dbg_mbx, vha, 0x112c,
+		    "Cmd=%x Polling Mode.\n", command);
+
+		QLAFX00_SET_HST_INTR(ha, ha->mbx_intr_code);
+		spin_unlock_irqrestore(&ha->hardware_lock, flags);
+
+		wait_time = jiffies + mcp->tov * HZ; /* wait at most tov secs */
+		while (!ha->flags.mbox_int) {
+			if (time_after(jiffies, wait_time))
+				break;
+
+			/* Check for pending interrupts. */
+			qla2x00_poll(ha->rsp_q_map[0]);
+
+			if (!ha->flags.mbox_int &&
+			    !(IS_QLA2200(ha) &&
+			    command == MBC_LOAD_RISC_RAM_EXTENDED))
+				usleep_range(10000, 11000);
+		} /* while */
+		ql_dbg(ql_dbg_mbx, vha, 0x112d,
+		    "Waited %d sec.\n",
+		    (uint)((jiffies - (wait_time - (mcp->tov * HZ)))/HZ));
+	}
+
+	/* Check whether we timed out */
+	if (ha->flags.mbox_int) {
+		uint32_t *iptr2;
+
+		ql_dbg(ql_dbg_mbx, vha, 0x112e,
+		    "Cmd=%x completed.\n", command);
+
+		/* Got interrupt. Clear the flag. */
+		ha->flags.mbox_int = 0;
+		clear_bit(MBX_INTERRUPT, &ha->mbx_cmd_flags);
+
+		if (ha->mailbox_out32[0] != MBS_COMMAND_COMPLETE)
+			rval = QLA_FUNCTION_FAILED;
+
+		/* Load return mailbox registers. */
+		iptr2 = mcp->mb;
+		iptr = (uint32_t *)&ha->mailbox_out32[0];
+		mboxes = mcp->in_mb;
+		for (cnt = 0; cnt < ha->mbx_count; cnt++) {
+			if (mboxes & BIT_0)
+				*iptr2 = *iptr;
+
+			mboxes >>= 1;
+			iptr2++;
+			iptr++;
+		}
+	} else {
+
+		rval = QLA_FUNCTION_TIMEOUT;
+	}
+
+	ha->flags.mbox_busy = 0;
+
+	/* Clean up */
+	ha->mcp32 = NULL;
+
+	if ((abort_active || !io_lock_on) && !IS_NOPOLLING_TYPE(ha)) {
+		ql_dbg(ql_dbg_mbx, vha, 0x113a,
+		    "checking for additional resp interrupt.\n");
+
+		/* polling mode for non isp_abort commands. */
+		qla2x00_poll(ha->rsp_q_map[0]);
+	}
+
+	if (rval == QLA_FUNCTION_TIMEOUT &&
+	    mcp->mb[0] != MBC_GEN_SYSTEM_ERROR) {
+		if (!io_lock_on || (mcp->flags & IOCTL_CMD) ||
+		    ha->flags.eeh_busy) {
+			/* not in dpc. schedule it for dpc to take over. */
+			ql_dbg(ql_dbg_mbx, vha, 0x115d,
+			    "Timeout, schedule isp_abort_needed.\n");
+
+			if (!test_bit(ISP_ABORT_NEEDED, &vha->dpc_flags) &&
+			    !test_bit(ABORT_ISP_ACTIVE, &vha->dpc_flags) &&
+			    !test_bit(ISP_ABORT_RETRY, &vha->dpc_flags)) {
+
+				ql_log(ql_log_info, base_vha, 0x115e,
+				    "Mailbox cmd timeout occurred, cmd=0x%x, "
+				    "mb[0]=0x%x, eeh_busy=0x%x. Scheduling ISP "
+				    "abort.\n", command, mcp->mb[0],
+				    ha->flags.eeh_busy);
+				set_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
+				qla2xxx_wake_dpc(vha);
+			}
+		} else if (!abort_active) {
+			/* call abort directly since we are in the DPC thread */
+			ql_dbg(ql_dbg_mbx, vha, 0x1160,
+			    "Timeout, calling abort_isp.\n");
+
+			if (!test_bit(ISP_ABORT_NEEDED, &vha->dpc_flags) &&
+			    !test_bit(ABORT_ISP_ACTIVE, &vha->dpc_flags) &&
+			    !test_bit(ISP_ABORT_RETRY, &vha->dpc_flags)) {
+
+				ql_log(ql_log_info, base_vha, 0x1161,
+				    "Mailbox cmd timeout occurred, cmd=0x%x, "
+				    "mb[0]=0x%x. Scheduling ISP abort ",
+				    command, mcp->mb[0]);
+
+				set_bit(ABORT_ISP_ACTIVE, &vha->dpc_flags);
+				clear_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
+				if (ha->isp_ops->abort_isp(vha)) {
+					/* Failed. retry later. */
+					set_bit(ISP_ABORT_NEEDED,
+					    &vha->dpc_flags);
+				}
+				clear_bit(ABORT_ISP_ACTIVE, &vha->dpc_flags);
+				ql_dbg(ql_dbg_mbx, vha, 0x1162,
+				    "Finished abort_isp.\n");
+			}
+		}
+	}
+
+premature_exit:
+	/* Allow next mbx cmd to come in. */
+	complete(&ha->mbx_cmd_comp);
+
+	if (rval) {
+		ql_log(ql_log_warn, base_vha, 0x1163,
+		    "**** Failed mbx[0]=%x, mb[1]=%x, mb[2]=%x, "
+		    "mb[3]=%x, cmd=%x ****.\n",
+		    mcp->mb[0], mcp->mb[1], mcp->mb[2], mcp->mb[3], command);
+	} else {
+		ql_dbg(ql_dbg_mbx, base_vha, 0x1164, "Done %s.\n", __func__);
+	}
+
+	return rval;
+}
+
+/*
+ * qlafx00_driver_shutdown
+ *	Indicate a driver shutdown to firmware.
+ *
+ * Input:
+ *	ha = adapter block pointer.
+ *
+ * Returns:
+ *	local function return status code.
+ *
+ * Context:
+ *	Kernel context.
+ */
+static int
+qlafx00_driver_shutdown(scsi_qla_host_t *vha, int tmo)
+{
+	int rval;
+	struct mbx_cmd_32 mc;
+	struct mbx_cmd_32 *mcp = &mc;
+
+	ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x1166,
+	    "Entered %s.\n", __func__);
+
+	mcp->mb[0] = MBC_MR_DRV_SHUTDOWN;
+	mcp->out_mb = MBX_0;
+	mcp->in_mb = MBX_0;
+	if (tmo)
+		mcp->tov = tmo;
+	else
+		mcp->tov = MBX_TOV_SECONDS;
+	mcp->flags = 0;
+	rval = qlafx00_mailbox_command(vha, mcp);
+
+	if (rval != QLA_SUCCESS) {
+		ql_dbg(ql_dbg_mbx, vha, 0x1167,
+		    "Failed=%x.\n", rval);
+	} else {
+		ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x1168,
+		    "Done %s.\n", __func__);
+	}
+
+	return rval;
+}
+
+/*
+ * qlafx00_get_firmware_state
+ *	Get adapter firmware state.
+ *
+ * Input:
+ *	ha = adapter block pointer.
+ *	TARGET_QUEUE_LOCK must be released.
+ *	ADAPTER_STATE_LOCK must be released.
+ *
+ * Returns:
+ *	qla7xxx local function return status code.
+ *
+ * Context:
+ *	Kernel context.
+ */
+static int
+qlafx00_get_firmware_state(scsi_qla_host_t *vha, uint32_t *states)
+{
+	int rval;
+	struct mbx_cmd_32 mc;
+	struct mbx_cmd_32 *mcp = &mc;
+
+	ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x1169,
+	    "Entered %s.\n", __func__);
+
+	mcp->mb[0] = MBC_GET_FIRMWARE_STATE;
+	mcp->out_mb = MBX_0;
+	mcp->in_mb = MBX_1|MBX_0;
+	mcp->tov = MBX_TOV_SECONDS;
+	mcp->flags = 0;
+	rval = qlafx00_mailbox_command(vha, mcp);
+
+	/* Return firmware states. */
+	states[0] = mcp->mb[1];
+
+	if (rval != QLA_SUCCESS) {
+		ql_dbg(ql_dbg_mbx, vha, 0x116a,
+		    "Failed=%x mb[0]=%x.\n", rval, mcp->mb[0]);
+	} else {
+		ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x116b,
+		    "Done %s.\n", __func__);
+	}
+	return rval;
+}
+
+/*
+ * qlafx00_init_firmware
+ *	Initialize adapter firmware.
+ *
+ * Input:
+ *	ha = adapter block pointer.
+ *	dptr = Initialization control block pointer.
+ *	size = size of initialization control block.
+ *	TARGET_QUEUE_LOCK must be released.
+ *	ADAPTER_STATE_LOCK must be released.
+ *
+ * Returns:
+ *	qlafx00 local function return status code.
+ *
+ * Context:
+ *	Kernel context.
+ */
+int
+qlafx00_init_firmware(scsi_qla_host_t *vha, uint16_t size)
+{
+	int rval;
+	struct mbx_cmd_32 mc;
+	struct mbx_cmd_32 *mcp = &mc;
+	struct qla_hw_data *ha = vha->hw;
+
+	ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x116c,
+	    "Entered %s.\n", __func__);
+
+	mcp->mb[0] = MBC_INITIALIZE_FIRMWARE;
+
+	mcp->mb[1] = 0;
+	mcp->mb[2] = MSD(ha->init_cb_dma);
+	mcp->mb[3] = LSD(ha->init_cb_dma);
+
+	mcp->out_mb = MBX_3|MBX_2|MBX_1|MBX_0;
+	mcp->in_mb = MBX_0;
+	mcp->buf_size = size;
+	mcp->flags = MBX_DMA_OUT;
+	mcp->tov = MBX_TOV_SECONDS;
+	rval = qlafx00_mailbox_command(vha, mcp);
+
+	if (rval != QLA_SUCCESS) {
+		ql_dbg(ql_dbg_mbx, vha, 0x116d,
+		    "Failed=%x mb[0]=%x.\n", rval, mcp->mb[0]);
+	} else {
+		ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x116e,
+		    "Done %s.\n", __func__);
+	}
+	return rval;
+}
+
+/*
+ * qlafx00_mbx_reg_test
+ */
+static int
+qlafx00_mbx_reg_test(scsi_qla_host_t *vha)
+{
+	int rval;
+	struct mbx_cmd_32 mc;
+	struct mbx_cmd_32 *mcp = &mc;
+
+	ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x116f,
+	    "Entered %s.\n", __func__);
+
+
+	mcp->mb[0] = MBC_MAILBOX_REGISTER_TEST;
+	mcp->mb[1] = 0xAAAA;
+	mcp->mb[2] = 0x5555;
+	mcp->mb[3] = 0xAA55;
+	mcp->mb[4] = 0x55AA;
+	mcp->mb[5] = 0xA5A5;
+	mcp->mb[6] = 0x5A5A;
+	mcp->mb[7] = 0x2525;
+	mcp->mb[8] = 0xBBBB;
+	mcp->mb[9] = 0x6666;
+	mcp->mb[10] = 0xBB66;
+	mcp->mb[11] = 0x66BB;
+	mcp->mb[12] = 0xB6B6;
+	mcp->mb[13] = 0x6B6B;
+	mcp->mb[14] = 0x3636;
+	mcp->mb[15] = 0xCCCC;
+
+
+	mcp->out_mb = MBX_15|MBX_14|MBX_13|MBX_12|MBX_11|MBX_10|MBX_9|MBX_8|
+			MBX_7|MBX_6|MBX_5|MBX_4|MBX_3|MBX_2|MBX_1|MBX_0;
+	mcp->in_mb = MBX_15|MBX_14|MBX_13|MBX_12|MBX_11|MBX_10|MBX_9|MBX_8|
+			MBX_7|MBX_6|MBX_5|MBX_4|MBX_3|MBX_2|MBX_1|MBX_0;
+	mcp->buf_size = 0;
+	mcp->flags = MBX_DMA_OUT;
+	mcp->tov = MBX_TOV_SECONDS;
+	rval = qlafx00_mailbox_command(vha, mcp);
+	if (rval == QLA_SUCCESS) {
+		if (mcp->mb[17] != 0xAAAA || mcp->mb[18] != 0x5555 ||
+		    mcp->mb[19] != 0xAA55 || mcp->mb[20] != 0x55AA)
+			rval = QLA_FUNCTION_FAILED;
+		if (mcp->mb[21] != 0xA5A5 || mcp->mb[22] != 0x5A5A ||
+		    mcp->mb[23] != 0x2525 || mcp->mb[24] != 0xBBBB)
+			rval = QLA_FUNCTION_FAILED;
+		if (mcp->mb[25] != 0x6666 || mcp->mb[26] != 0xBB66 ||
+		    mcp->mb[27] != 0x66BB || mcp->mb[28] != 0xB6B6)
+			rval = QLA_FUNCTION_FAILED;
+		if (mcp->mb[29] != 0x6B6B || mcp->mb[30] != 0x3636 ||
+		    mcp->mb[31] != 0xCCCC)
+			rval = QLA_FUNCTION_FAILED;
+	}
+
+	if (rval != QLA_SUCCESS) {
+		ql_dbg(ql_dbg_mbx, vha, 0x1170,
+		    "Failed=%x mb[0]=%x.\n", rval, mcp->mb[0]);
+	} else {
+		ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x1171,
+		    "Done %s.\n", __func__);
+	}
+	return rval;
+}
+
+/**
+ * qlafx00_pci_config() - Setup ISPFx00 PCI configuration registers.
+ * @ha: HA context
+ *
+ * Returns 0 on success.
+ */
+int
+qlafx00_pci_config(scsi_qla_host_t *vha)
+{
+	uint16_t w;
+	struct qla_hw_data *ha = vha->hw;
+
+	pci_set_master(ha->pdev);
+	pci_try_set_mwi(ha->pdev);
+
+	pci_read_config_word(ha->pdev, PCI_COMMAND, &w);
+	w |= (PCI_COMMAND_PARITY | PCI_COMMAND_SERR);
+	w &= ~PCI_COMMAND_INTX_DISABLE;
+	pci_write_config_word(ha->pdev, PCI_COMMAND, w);
+
+	/* PCIe -- adjust Maximum Read Request Size (2048). */
+	if (pci_find_capability(ha->pdev, PCI_CAP_ID_EXP))
+		pcie_set_readrq(ha->pdev, 2048);
+
+	ha->chip_revision = ha->pdev->revision;
+
+	return QLA_SUCCESS;
+}
+
+/**
+ * qlafx00_warm_reset() - Perform warm reset of iSA(CPUs being reset on SOC).
+ * @ha: HA context
+ *
+  */
+static inline void
+qlafx00_soc_cpu_reset(scsi_qla_host_t *vha)
+{
+	unsigned long flags = 0;
+	struct qla_hw_data *ha = vha->hw;
+	int i, core;
+	uint32_t cnt;
+
+	/* Set all 4 cores in reset */
+	for (i = 0; i < 4; i++) {
+		QLAFX00_SET_HBA_SOC_REG(ha,
+		    (SOC_SW_RST_CONTROL_REG_CORE0 + 8*i), (0xF01));
+	}
+
+	/* Set all 4 core Clock gating control */
+	for (i = 0; i < 4; i++) {
+		QLAFX00_SET_HBA_SOC_REG(ha,
+		    (SOC_SW_RST_CONTROL_REG_CORE0 + 4 + 8*i), (0x01010101));
+	}
+
+	/* Reset all units in Fabric */
+	QLAFX00_SET_HBA_SOC_REG(ha, SOC_FABRIC_RST_CONTROL_REG, (0x11F0101));
+
+	/* Reset all interrupt control registers */
+	for (i = 0; i < 115; i++) {
+		QLAFX00_SET_HBA_SOC_REG(ha,
+		    (SOC_INTERRUPT_SOURCE_I_CONTROL_REG + 4*i), (0x0));
+	}
+
+	/* Reset Timers control registers. per core */
+	for (core = 0; core < 4; core++)
+		for (i = 0; i < 8; i++)
+			QLAFX00_SET_HBA_SOC_REG(ha,
+			    (SOC_CORE_TIMER_REG + 0x100*core + 4*i), (0x0));
+
+	/* Reset per core IRQ ack register */
+	for (core = 0; core < 4; core++)
+		QLAFX00_SET_HBA_SOC_REG(ha,
+		    (SOC_IRQ_ACK_REG + 0x100*core), (0x3FF));
+
+	/* Set Fabric control and config to defaults */
+	QLAFX00_SET_HBA_SOC_REG(ha, SOC_FABRIC_CONTROL_REG, (0x2));
+	QLAFX00_SET_HBA_SOC_REG(ha, SOC_FABRIC_CONFIG_REG, (0x3));
+
+	spin_lock_irqsave(&ha->hardware_lock, flags);
+
+	/* Kick in Fabric units */
+	QLAFX00_SET_HBA_SOC_REG(ha, SOC_FABRIC_RST_CONTROL_REG, (0x0));
+
+	/* Kick in Core0 to start boot process */
+	QLAFX00_SET_HBA_SOC_REG(ha, SOC_SW_RST_CONTROL_REG_CORE0, (0xF00));
+
+	/* Wait 10secs for soft-reset to complete. */
+	for (cnt = 10; cnt; cnt--) {
+		msleep(1000);
+		barrier();
+	}
+	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+}
+
+/**
+ * qlafx00_soft_reset() - Soft Reset ISPFx00.
+ * @ha: HA context
+ *
+ * Returns 0 on success.
+ */
+void
+qlafx00_soft_reset(scsi_qla_host_t *vha)
+{
+	struct qla_hw_data *ha = vha->hw;
+
+	if (unlikely(pci_channel_offline(ha->pdev) &&
+	    ha->flags.pci_channel_io_perm_failure))
+		return;
+
+	ha->isp_ops->disable_intrs(ha);
+	qlafx00_soc_cpu_reset(vha);
+	ha->isp_ops->enable_intrs(ha);
+}
+
+/**
+ * qlafx00_chip_diag() - Test ISPFx00 for proper operation.
+ * @ha: HA context
+ *
+ * Returns 0 on success.
+ */
+int
+qlafx00_chip_diag(scsi_qla_host_t *vha)
+{
+	int rval = 0;
+	struct qla_hw_data *ha = vha->hw;
+	struct req_que *req = ha->req_q_map[0];
+
+	ha->fw_transfer_size = REQUEST_ENTRY_SIZE * req->length;
+
+	rval = qlafx00_mbx_reg_test(vha);
+	if (rval) {
+		ql_log(ql_log_warn, vha, 0x1165,
+		    "Failed mailbox send register test\n");
+	} else {
+		/* Flag a successful rval */
+		rval = QLA_SUCCESS;
+	}
+	return rval;
+}
+
+void
+qlafx00_config_rings(struct scsi_qla_host *vha)
+{
+	struct qla_hw_data *ha = vha->hw;
+	struct device_reg_fx00 __iomem *reg = &ha->iobase->ispfx00;
+	struct init_cb_fx *icb;
+	struct req_que *req = ha->req_q_map[0];
+	struct rsp_que *rsp = ha->rsp_q_map[0];
+
+	/* Setup ring parameters in initialization control block. */
+	icb = (struct init_cb_fx *)ha->init_cb;
+	icb->request_q_outpointer = __constant_cpu_to_le16(0);
+	icb->response_q_inpointer = __constant_cpu_to_le16(0);
+	icb->request_q_length = cpu_to_le16(req->length);
+	icb->response_q_length = cpu_to_le16(rsp->length);
+	icb->request_q_address[0] = cpu_to_le32(LSD(req->dma));
+	icb->request_q_address[1] = cpu_to_le32(MSD(req->dma));
+	icb->response_q_address[0] = cpu_to_le32(LSD(rsp->dma));
+	icb->response_q_address[1] = cpu_to_le32(MSD(rsp->dma));
+
+	WRT_REG_DWORD(&reg->req_q_in, 0);
+	WRT_REG_DWORD(&reg->req_q_out, 0);
+
+	WRT_REG_DWORD(&reg->rsp_q_in, 0);
+	WRT_REG_DWORD(&reg->rsp_q_out, 0);
+
+	/* PCI posting */
+	RD_REG_DWORD(&reg->rsp_q_out);
+}
+
+char *
+qlafx00_pci_info_str(struct scsi_qla_host *vha, char *str)
+{
+	struct qla_hw_data *ha = vha->hw;
+	int pcie_reg;
+
+	pcie_reg = pci_find_capability(ha->pdev, PCI_CAP_ID_EXP);
+	if (pcie_reg) {
+		strcpy(str, "PCIe iSA");
+		return str;
+	}
+	return str;
+}
+
+char *
+qlafx00_fw_version_str(struct scsi_qla_host *vha, char *str)
+{
+	struct qla_hw_data *ha = vha->hw;
+
+	sprintf(str, "%s", ha->mr.fw_version);
+	return str;
+}
+
+void
+qlafx00_enable_intrs(struct qla_hw_data *ha)
+{
+	unsigned long flags = 0;
+
+	spin_lock_irqsave(&ha->hardware_lock, flags);
+	ha->interrupts_on = 1;
+	QLAFX00_ENABLE_ICNTRL_REG(ha);
+	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+}
+
+void
+qlafx00_disable_intrs(struct qla_hw_data *ha)
+{
+	unsigned long flags = 0;
+
+	spin_lock_irqsave(&ha->hardware_lock, flags);
+	ha->interrupts_on = 0;
+	QLAFX00_DISABLE_ICNTRL_REG(ha);
+	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+}
+
+static void
+qlafx00_tmf_iocb_timeout(void *data)
+{
+	srb_t *sp = (srb_t *)data;
+	struct srb_iocb *tmf = &sp->u.iocb_cmd;
+
+	tmf->u.tmf.comp_status = CS_TIMEOUT;
+	complete(&tmf->u.tmf.comp);
+}
+
+static void
+qlafx00_tmf_sp_done(void *data, void *ptr, int res)
+{
+	srb_t *sp = (srb_t *)ptr;
+	struct srb_iocb *tmf = &sp->u.iocb_cmd;
+
+	complete(&tmf->u.tmf.comp);
+}
+
+static int
+qlafx00_async_tm_cmd(fc_port_t *fcport, uint32_t flags,
+		     uint32_t lun, uint32_t tag)
+{
+	scsi_qla_host_t *vha = fcport->vha;
+	struct srb_iocb *tm_iocb;
+	srb_t *sp;
+	int rval = QLA_FUNCTION_FAILED;
+
+	sp = qla2x00_get_sp(vha, fcport, GFP_KERNEL);
+	if (!sp)
+		goto done;
+
+	tm_iocb = &sp->u.iocb_cmd;
+	sp->type = SRB_TM_CMD;
+	sp->name = "tmf";
+	qla2x00_init_timer(sp, qla2x00_get_async_timeout(vha));
+	tm_iocb->u.tmf.flags = flags;
+	tm_iocb->u.tmf.lun = lun;
+	tm_iocb->u.tmf.data = tag;
+	sp->done = qlafx00_tmf_sp_done;
+	tm_iocb->timeout = qlafx00_tmf_iocb_timeout;
+	init_completion(&tm_iocb->u.tmf.comp);
+
+	rval = qla2x00_start_sp(sp);
+	if (rval != QLA_SUCCESS)
+		goto done_free_sp;
+
+	ql_dbg(ql_dbg_async, vha, 0x507b,
+	    "Task management command issued target_id=%x\n",
+	    fcport->tgt_id);
+
+	wait_for_completion(&tm_iocb->u.tmf.comp);
+
+	rval = tm_iocb->u.tmf.comp_status == CS_COMPLETE ?
+	    QLA_SUCCESS : QLA_FUNCTION_FAILED;
+
+done_free_sp:
+	sp->free(vha, sp);
+done:
+	return rval;
+}
+
+int
+qlafx00_abort_target(fc_port_t *fcport, unsigned int l, int tag)
+{
+	return qlafx00_async_tm_cmd(fcport, TCF_TARGET_RESET, l, tag);
+}
+
+int
+qlafx00_lun_reset(fc_port_t *fcport, unsigned int l, int tag)
+{
+	return qlafx00_async_tm_cmd(fcport, TCF_LUN_RESET, l, tag);
+}
+
+int
+qlafx00_iospace_config(struct qla_hw_data *ha)
+{
+	if (pci_request_selected_regions(ha->pdev, ha->bars,
+	    QLA2XXX_DRIVER_NAME)) {
+		ql_log_pci(ql_log_fatal, ha->pdev, 0x014e,
+		    "Failed to reserve PIO/MMIO regions (%s), aborting.\n",
+		    pci_name(ha->pdev));
+		goto iospace_error_exit;
+	}
+
+	/* Use MMIO operations for all accesses. */
+	if (!(pci_resource_flags(ha->pdev, 0) & IORESOURCE_MEM)) {
+		ql_log_pci(ql_log_warn, ha->pdev, 0x014f,
+		    "Invalid pci I/O region size (%s).\n",
+		    pci_name(ha->pdev));
+		goto iospace_error_exit;
+	}
+	if (pci_resource_len(ha->pdev, 0) < BAR0_LEN_FX00) {
+		ql_log_pci(ql_log_warn, ha->pdev, 0x0127,
+		    "Invalid PCI mem BAR0 region size (%s), aborting\n",
+			pci_name(ha->pdev));
+		goto iospace_error_exit;
+	}
+
+	ha->cregbase =
+	    ioremap_nocache(pci_resource_start(ha->pdev, 0), BAR0_LEN_FX00);
+	if (!ha->cregbase) {
+		ql_log_pci(ql_log_fatal, ha->pdev, 0x0128,
+		    "cannot remap MMIO (%s), aborting\n", pci_name(ha->pdev));
+		goto iospace_error_exit;
+	}
+
+	if (!(pci_resource_flags(ha->pdev, 2) & IORESOURCE_MEM)) {
+		ql_log_pci(ql_log_warn, ha->pdev, 0x0129,
+		    "region #2 not an MMIO resource (%s), aborting\n",
+		    pci_name(ha->pdev));
+		goto iospace_error_exit;
+	}
+	if (pci_resource_len(ha->pdev, 2) < BAR2_LEN_FX00) {
+		ql_log_pci(ql_log_warn, ha->pdev, 0x012a,
+		    "Invalid PCI mem BAR2 region size (%s), aborting\n",
+			pci_name(ha->pdev));
+		goto iospace_error_exit;
+	}
+
+	ha->iobase =
+	    ioremap_nocache(pci_resource_start(ha->pdev, 2), BAR2_LEN_FX00);
+	if (!ha->iobase) {
+		ql_log_pci(ql_log_fatal, ha->pdev, 0x012b,
+		    "cannot remap MMIO (%s), aborting\n", pci_name(ha->pdev));
+		goto iospace_error_exit;
+	}
+
+	/* Determine queue resources */
+	ha->max_req_queues = ha->max_rsp_queues = 1;
+
+	ql_log_pci(ql_log_info, ha->pdev, 0x012c,
+	    "Bars 0x%x, iobase0 0x%p, iobase2 0x%p\n",
+	    ha->bars, ha->cregbase, ha->iobase);
+
+	return 0;
+
+iospace_error_exit:
+	return -ENOMEM;
+}
+
+static void
+qlafx00_save_queue_ptrs(struct scsi_qla_host *vha)
+{
+	struct qla_hw_data *ha = vha->hw;
+	struct req_que *req = ha->req_q_map[0];
+	struct rsp_que *rsp = ha->rsp_q_map[0];
+
+	req->length_fx00 = req->length;
+	req->ring_fx00 = req->ring;
+	req->dma_fx00 = req->dma;
+
+	rsp->length_fx00 = rsp->length;
+	rsp->ring_fx00 = rsp->ring;
+	rsp->dma_fx00 = rsp->dma;
+
+	ql_dbg(ql_dbg_init, vha, 0x012d,
+	    "req: %p, ring_fx00: %p, length_fx00: 0x%x,"
+	    "req->dma_fx00: 0x%llx\n", req, req->ring_fx00,
+	    req->length_fx00, (u64)req->dma_fx00);
+
+	ql_dbg(ql_dbg_init, vha, 0x012e,
+	    "rsp: %p, ring_fx00: %p, length_fx00: 0x%x,"
+	    "rsp->dma_fx00: 0x%llx\n", rsp, rsp->ring_fx00,
+	    rsp->length_fx00, (u64)rsp->dma_fx00);
+}
+
+static int
+qlafx00_config_queues(struct scsi_qla_host *vha)
+{
+	struct qla_hw_data *ha = vha->hw;
+	struct req_que *req = ha->req_q_map[0];
+	struct rsp_que *rsp = ha->rsp_q_map[0];
+	dma_addr_t bar2_hdl = pci_resource_start(ha->pdev, 2);
+
+	req->length = ha->req_que_len;
+	req->ring = (void *)ha->iobase + ha->req_que_off;
+	req->dma = bar2_hdl + ha->req_que_off;
+	if ((!req->ring) || (req->length == 0)) {
+		ql_log_pci(ql_log_info, ha->pdev, 0x012f,
+		    "Unable to allocate memory for req_ring\n");
+		return QLA_FUNCTION_FAILED;
+	}
+
+	ql_dbg(ql_dbg_init, vha, 0x0130,
+	    "req: %p req_ring pointer %p req len 0x%x "
+	    "req off 0x%x\n, req->dma: 0x%llx",
+	    req, req->ring, req->length,
+	    ha->req_que_off, (u64)req->dma);
+
+	rsp->length = ha->rsp_que_len;
+	rsp->ring = (void *)ha->iobase + ha->rsp_que_off;
+	rsp->dma = bar2_hdl + ha->rsp_que_off;
+	if ((!rsp->ring) || (rsp->length == 0)) {
+		ql_log_pci(ql_log_info, ha->pdev, 0x0131,
+		    "Unable to allocate memory for rsp_ring\n");
+		return QLA_FUNCTION_FAILED;
+	}
+
+	ql_dbg(ql_dbg_init, vha, 0x0132,
+	    "rsp: %p rsp_ring pointer %p rsp len 0x%x "
+	    "rsp off 0x%x, rsp->dma: 0x%llx\n",
+	    rsp, rsp->ring, rsp->length,
+	    ha->rsp_que_off, (u64)rsp->dma);
+
+	return QLA_SUCCESS;
+}
+
+static int
+qlafx00_init_fw_ready(scsi_qla_host_t *vha)
+{
+	int rval = 0;
+	unsigned long wtime;
+	uint16_t wait_time;	/* Wait time */
+	struct qla_hw_data *ha = vha->hw;
+	struct device_reg_fx00 __iomem *reg = &ha->iobase->ispfx00;
+	uint32_t aenmbx, aenmbx7 = 0;
+	uint32_t state[5];
+	bool done = false;
+
+	/* 30 seconds wait - Adjust if required */
+	wait_time = 30;
+
+	/* wait time before firmware ready */
+	wtime = jiffies + (wait_time * HZ);
+	do {
+		aenmbx = RD_REG_DWORD(&reg->aenmailbox0);
+		barrier();
+		ql_dbg(ql_dbg_mbx, vha, 0x0133,
+		    "aenmbx: 0x%x\n", aenmbx);
+
+		switch (aenmbx) {
+		case MBA_FW_NOT_STARTED:
+		case MBA_FW_STARTING:
+			break;
+
+		case MBA_SYSTEM_ERR:
+		case MBA_REQ_TRANSFER_ERR:
+		case MBA_RSP_TRANSFER_ERR:
+		case MBA_FW_INIT_FAILURE:
+			qlafx00_soft_reset(vha);
+			break;
+
+		case MBA_FW_RESTART_CMPLT:
+			/* Set the mbx and rqstq intr code */
+			aenmbx7 = RD_REG_DWORD(&reg->aenmailbox7);
+			ha->mbx_intr_code = MSW(aenmbx7);
+			ha->rqstq_intr_code = LSW(aenmbx7);
+			ha->req_que_off = RD_REG_DWORD(&reg->aenmailbox1);
+			ha->rsp_que_off = RD_REG_DWORD(&reg->aenmailbox3);
+			ha->req_que_len = RD_REG_DWORD(&reg->aenmailbox5);
+			ha->rsp_que_len = RD_REG_DWORD(&reg->aenmailbox6);
+			WRT_REG_DWORD(&reg->aenmailbox0, 0);
+			RD_REG_DWORD_RELAXED(&reg->aenmailbox0);
+			ql_dbg(ql_dbg_init, vha, 0x0134,
+			    "f/w returned mbx_intr_code: 0x%x, "
+			    "rqstq_intr_code: 0x%x\n",
+			    ha->mbx_intr_code, ha->rqstq_intr_code);
+			QLAFX00_CLR_INTR_REG(ha, QLAFX00_HST_INT_STS_BITS);
+			rval = QLA_SUCCESS;
+			done = true;
+			break;
+
+		default:
+			/* If fw is apparently not ready. In order to continue,
+			 * we might need to issue Mbox cmd, but the problem is
+			 * that the DoorBell vector values that come with the
+			 * 8060 AEN are most likely gone by now (and thus no
+			 * bell would be rung on the fw side when mbox cmd is
+			 * issued). We have to therefore grab the 8060 AEN
+			 * shadow regs (filled in by FW when the last 8060
+			 * AEN was being posted).
+			 * Do the following to determine what is needed in
+			 * order to get the FW ready:
+			 * 1. reload the 8060 AEN values from the shadow regs
+			 * 2. clear int status to get rid of possible pending
+			 *    interrupts
+			 * 3. issue Get FW State Mbox cmd to determine fw state
+			 * Set the mbx and rqstq intr code from Shadow Regs
+			 */
+			aenmbx7 = RD_REG_DWORD(&reg->initval7);
+			ha->mbx_intr_code = MSW(aenmbx7);
+			ha->rqstq_intr_code = LSW(aenmbx7);
+			ha->req_que_off = RD_REG_DWORD(&reg->initval1);
+			ha->rsp_que_off = RD_REG_DWORD(&reg->initval3);
+			ha->req_que_len = RD_REG_DWORD(&reg->initval5);
+			ha->rsp_que_len = RD_REG_DWORD(&reg->initval6);
+			ql_dbg(ql_dbg_init, vha, 0x0135,
+			    "f/w returned mbx_intr_code: 0x%x, "
+			    "rqstq_intr_code: 0x%x\n",
+			    ha->mbx_intr_code, ha->rqstq_intr_code);
+			QLAFX00_CLR_INTR_REG(ha, QLAFX00_HST_INT_STS_BITS);
+
+			/* Get the FW state */
+			rval = qlafx00_get_firmware_state(vha, state);
+			if (rval != QLA_SUCCESS) {
+				/* Retry if timer has not expired */
+				break;
+			}
+
+			if (state[0] == FSTATE_FX00_CONFIG_WAIT) {
+				/* Firmware is waiting to be
+				 * initialized by driver
+				 */
+				rval = QLA_SUCCESS;
+				done = true;
+				break;
+			}
+
+			/* Issue driver shutdown and wait until f/w recovers.
+			 * Driver should continue to poll until 8060 AEN is
+			 * received indicating firmware recovery.
+			 */
+			ql_dbg(ql_dbg_init, vha, 0x0136,
+			    "Sending Driver shutdown fw_state 0x%x\n",
+			    state[0]);
+
+			rval = qlafx00_driver_shutdown(vha, 10);
+			if (rval != QLA_SUCCESS) {
+				rval = QLA_FUNCTION_FAILED;
+				break;
+			}
+			msleep(500);
+
+			wtime = jiffies + (wait_time * HZ);
+			break;
+		}
+
+		if (!done) {
+			if (time_after_eq(jiffies, wtime)) {
+				ql_dbg(ql_dbg_init, vha, 0x0137,
+				    "Init f/w failed: aen[7]: 0x%x\n",
+				    RD_REG_DWORD(&reg->aenmailbox7));
+				rval = QLA_FUNCTION_FAILED;
+				done = true;
+				break;
+			}
+			/* Delay for a while */
+			msleep(500);
+		}
+	} while (!done);
+
+	if (rval)
+		ql_dbg(ql_dbg_init, vha, 0x0138,
+		    "%s **** FAILED ****.\n", __func__);
+	else
+		ql_dbg(ql_dbg_init, vha, 0x0139,
+		    "%s **** SUCCESS ****.\n", __func__);
+
+	return rval;
+}
+
+/*
+ * qlafx00_fw_ready() - Waits for firmware ready.
+ * @ha: HA context
+ *
+ * Returns 0 on success.
+ */
+int
+qlafx00_fw_ready(scsi_qla_host_t *vha)
+{
+	int		rval;
+	unsigned long	wtime;
+	uint16_t	wait_time;	/* Wait time if loop is coming ready */
+	uint32_t	state[5];
+
+	rval = QLA_SUCCESS;
+
+	wait_time = 10;
+
+	/* wait time before firmware ready */
+	wtime = jiffies + (wait_time * HZ);
+
+	/* Wait for ISP to finish init */
+	if (!vha->flags.init_done)
+		ql_dbg(ql_dbg_init, vha, 0x013a,
+		    "Waiting for init to complete...\n");
+
+	do {
+		rval = qlafx00_get_firmware_state(vha, state);
+
+		if (rval == QLA_SUCCESS) {
+			if (state[0] == FSTATE_FX00_INITIALIZED) {
+				ql_dbg(ql_dbg_init, vha, 0x013b,
+				    "fw_state=%x\n", state[0]);
+				rval = QLA_SUCCESS;
+					break;
+			}
+		}
+		rval = QLA_FUNCTION_FAILED;
+
+		if (time_after_eq(jiffies, wtime))
+			break;
+
+		/* Delay for a while */
+		msleep(500);
+
+		ql_dbg(ql_dbg_init, vha, 0x013c,
+		    "fw_state=%x curr time=%lx.\n", state[0], jiffies);
+	} while (1);
+
+
+	if (rval)
+		ql_dbg(ql_dbg_init, vha, 0x013d,
+		    "Firmware ready **** FAILED ****.\n");
+	else
+		ql_dbg(ql_dbg_init, vha, 0x013e,
+		    "Firmware ready **** SUCCESS ****.\n");
+
+	return rval;
+}
+
+static int
+qlafx00_find_all_targets(scsi_qla_host_t *vha,
+	struct list_head *new_fcports)
+{
+	int		rval;
+	uint16_t	tgt_id;
+	fc_port_t	*fcport, *new_fcport;
+	int		found;
+	struct qla_hw_data *ha = vha->hw;
+
+	rval = QLA_SUCCESS;
+
+	if (!test_bit(LOOP_RESYNC_ACTIVE, &vha->dpc_flags))
+		return QLA_FUNCTION_FAILED;
+
+	if ((atomic_read(&vha->loop_down_timer) ||
+	     STATE_TRANSITION(vha))) {
+		atomic_set(&vha->loop_down_timer, 0);
+		set_bit(LOOP_RESYNC_NEEDED, &vha->dpc_flags);
+		return QLA_FUNCTION_FAILED;
+	}
+
+	ql_dbg(ql_dbg_disc + ql_dbg_init, vha, 0x2088,
+	    "Listing Target bit map...\n");
+	ql_dump_buffer(ql_dbg_disc + ql_dbg_init, vha,
+	    0x2089, (uint8_t *)ha->gid_list, 32);
+
+	/* Allocate temporary rmtport for any new rmtports discovered. */
+	new_fcport = qla2x00_alloc_fcport(vha, GFP_KERNEL);
+	if (new_fcport == NULL)
+		return QLA_MEMORY_ALLOC_FAILED;
+
+	for_each_set_bit(tgt_id, (void *)ha->gid_list,
+	    QLAFX00_TGT_NODE_LIST_SIZE) {
+
+		/* Send get target node info */
+		new_fcport->tgt_id = tgt_id;
+		rval = qlafx00_fx_disc(vha, new_fcport,
+		    FXDISC_GET_TGT_NODE_INFO);
+		if (rval != QLA_SUCCESS) {
+			ql_log(ql_log_warn, vha, 0x208a,
+			    "Target info scan failed -- assuming zero-entry "
+			    "result...\n");
+			continue;
+		}
+
+		/* Locate matching device in database. */
+		found = 0;
+		list_for_each_entry(fcport, &vha->vp_fcports, list) {
+			if (memcmp(new_fcport->port_name,
+			    fcport->port_name, WWN_SIZE))
+				continue;
+
+			found++;
+
+			/*
+			 * If tgt_id is same and state FCS_ONLINE, nothing
+			 * changed.
+			 */
+			if (fcport->tgt_id == new_fcport->tgt_id &&
+			    atomic_read(&fcport->state) == FCS_ONLINE)
+				break;
+
+			/*
+			 * Tgt ID changed or device was marked to be updated.
+			 */
+			ql_dbg(ql_dbg_disc + ql_dbg_init, vha, 0x208b,
+			    "TGT-ID Change(%s): Present tgt id: "
+			    "0x%x state: 0x%x "
+			    "wwnn = %llx wwpn = %llx.\n",
+			    __func__, fcport->tgt_id,
+			    atomic_read(&fcport->state),
+			    (unsigned long long)wwn_to_u64(fcport->node_name),
+			    (unsigned long long)wwn_to_u64(fcport->port_name));
+
+			ql_log(ql_log_info, vha, 0x208c,
+			    "TGT-ID Announce(%s): Discovered tgt "
+			    "id 0x%x wwnn = %llx "
+			    "wwpn = %llx.\n", __func__, new_fcport->tgt_id,
+			    (unsigned long long)
+			    wwn_to_u64(new_fcport->node_name),
+			    (unsigned long long)
+			    wwn_to_u64(new_fcport->port_name));
+
+			if (atomic_read(&fcport->state) != FCS_ONLINE) {
+				fcport->old_tgt_id = fcport->tgt_id;
+				fcport->tgt_id = new_fcport->tgt_id;
+				ql_log(ql_log_info, vha, 0x208d,
+				   "TGT-ID: New fcport Added: %p\n", fcport);
+				qla2x00_update_fcport(vha, fcport);
+			} else {
+				ql_log(ql_log_info, vha, 0x208e,
+				    " Existing TGT-ID %x did not get "
+				    " offline event from firmware.\n",
+				    fcport->old_tgt_id);
+				qla2x00_mark_device_lost(vha, fcport, 0, 0);
+				set_bit(LOOP_RESYNC_NEEDED, &vha->dpc_flags);
+				kfree(new_fcport);
+				return rval;
+			}
+			break;
+		}
+
+		if (found)
+			continue;
+
+		/* If device was not in our fcports list, then add it. */
+		list_add_tail(&new_fcport->list, new_fcports);
+
+		/* Allocate a new replacement fcport. */
+		new_fcport = qla2x00_alloc_fcport(vha, GFP_KERNEL);
+		if (new_fcport == NULL)
+			return QLA_MEMORY_ALLOC_FAILED;
+	}
+
+	kfree(new_fcport);
+	return rval;
+}
+
+/*
+ * qlafx00_configure_all_targets
+ *      Setup target devices with node ID's.
+ *
+ * Input:
+ *      ha = adapter block pointer.
+ *
+ * Returns:
+ *      0 = success.
+ *      BIT_0 = error
+ */
+static int
+qlafx00_configure_all_targets(scsi_qla_host_t *vha)
+{
+	int rval;
+	fc_port_t *fcport, *rmptemp;
+	LIST_HEAD(new_fcports);
+
+	rval = qlafx00_fx_disc(vha, &vha->hw->mr.fcport,
+	    FXDISC_GET_TGT_NODE_LIST);
+	if (rval != QLA_SUCCESS) {
+		set_bit(LOOP_RESYNC_NEEDED, &vha->dpc_flags);
+		return rval;
+	}
+
+	rval = qlafx00_find_all_targets(vha, &new_fcports);
+	if (rval != QLA_SUCCESS) {
+		set_bit(LOOP_RESYNC_NEEDED, &vha->dpc_flags);
+		return rval;
+	}
+
+	/*
+	 * Delete all previous devices marked lost.
+	 */
+	list_for_each_entry(fcport, &vha->vp_fcports, list) {
+		if (test_bit(LOOP_RESYNC_NEEDED, &vha->dpc_flags))
+			break;
+
+		if (atomic_read(&fcport->state) == FCS_DEVICE_LOST) {
+			if (fcport->port_type != FCT_INITIATOR)
+				qla2x00_mark_device_lost(vha, fcport, 0, 0);
+		}
+	}
+
+	/*
+	 * Add the new devices to our devices list.
+	 */
+	list_for_each_entry_safe(fcport, rmptemp, &new_fcports, list) {
+		if (test_bit(LOOP_RESYNC_NEEDED, &vha->dpc_flags))
+			break;
+
+		qla2x00_update_fcport(vha, fcport);
+		list_move_tail(&fcport->list, &vha->vp_fcports);
+		ql_log(ql_log_info, vha, 0x208f,
+		    "Attach new target id 0x%x wwnn = %llx "
+		    "wwpn = %llx.\n",
+		    fcport->tgt_id,
+		    (unsigned long long)wwn_to_u64(fcport->node_name),
+		    (unsigned long long)wwn_to_u64(fcport->port_name));
+	}
+
+	/* Free all new device structures not processed. */
+	list_for_each_entry_safe(fcport, rmptemp, &new_fcports, list) {
+		list_del(&fcport->list);
+		kfree(fcport);
+	}
+
+	return rval;
+}
+
+/*
+ * qlafx00_configure_devices
+ *      Updates Fibre Channel Device Database with what is actually on loop.
+ *
+ * Input:
+ *      ha                = adapter block pointer.
+ *
+ * Returns:
+ *      0 = success.
+ *      1 = error.
+ *      2 = database was full and device was not configured.
+ */
+int
+qlafx00_configure_devices(scsi_qla_host_t *vha)
+{
+	int  rval;
+	unsigned long flags, save_flags;
+	rval = QLA_SUCCESS;
+
+	save_flags = flags = vha->dpc_flags;
+
+	ql_dbg(ql_dbg_disc, vha, 0x2090,
+	    "Configure devices -- dpc flags =0x%lx\n", flags);
+
+	rval = qlafx00_configure_all_targets(vha);
+
+	if (rval == QLA_SUCCESS) {
+		if (test_bit(LOOP_RESYNC_NEEDED, &vha->dpc_flags)) {
+			rval = QLA_FUNCTION_FAILED;
+		} else {
+			atomic_set(&vha->loop_state, LOOP_READY);
+			ql_log(ql_log_info, vha, 0x2091,
+			    "Device Ready\n");
+		}
+	}
+
+	if (rval) {
+		ql_dbg(ql_dbg_disc, vha, 0x2092,
+		    "%s *** FAILED ***.\n", __func__);
+	} else {
+		ql_dbg(ql_dbg_disc, vha, 0x2093,
+		    "%s: exiting normally.\n", __func__);
+	}
+	return rval;
+}
+
+static void
+qlafx00_abort_isp_cleanup(scsi_qla_host_t *vha)
+{
+	struct qla_hw_data *ha = vha->hw;
+	fc_port_t *fcport;
+
+	vha->flags.online = 0;
+	ha->flags.chip_reset_done = 0;
+	ha->mr.fw_hbt_en = 0;
+	clear_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
+	vha->qla_stats.total_isp_aborts++;
+
+	ql_log(ql_log_info, vha, 0x013f,
+	    "Performing ISP error recovery - ha = %p.\n", ha);
+
+	ha->isp_ops->reset_chip(vha);
+
+	if (atomic_read(&vha->loop_state) != LOOP_DOWN) {
+		atomic_set(&vha->loop_state, LOOP_DOWN);
+		atomic_set(&vha->loop_down_timer,
+		    QLAFX00_LOOP_DOWN_TIME);
+	} else {
+		if (!atomic_read(&vha->loop_down_timer))
+			atomic_set(&vha->loop_down_timer,
+			    QLAFX00_LOOP_DOWN_TIME);
+	}
+
+	/* Clear all async request states across all VPs. */
+	list_for_each_entry(fcport, &vha->vp_fcports, list) {
+		fcport->flags = 0;
+		if (atomic_read(&fcport->state) == FCS_ONLINE)
+			qla2x00_set_fcport_state(fcport, FCS_DEVICE_LOST);
+	}
+
+	if (!ha->flags.eeh_busy) {
+		/* Requeue all commands in outstanding command list. */
+		qla2x00_abort_all_cmds(vha, DID_RESET << 16);
+	}
+
+	qla2x00_free_irqs(vha);
+	set_bit(FX00_RESET_RECOVERY, &vha->dpc_flags);
+
+	/* Clear the Interrupts */
+	QLAFX00_CLR_INTR_REG(ha, QLAFX00_HST_INT_STS_BITS);
+
+	ql_log(ql_log_info, vha, 0x0140,
+	    "%s Done done - ha=%p.\n", __func__, ha);
+}
+
+/**
+ * qlafx00_init_response_q_entries() - Initializes response queue entries.
+ * @ha: HA context
+ *
+ * Beginning of request ring has initialization control block already built
+ * by nvram config routine.
+ *
+ * Returns 0 on success.
+ */
+void
+qlafx00_init_response_q_entries(struct rsp_que *rsp)
+{
+	uint16_t cnt;
+	response_t *pkt;
+
+	rsp->ring_ptr = rsp->ring;
+	rsp->ring_index    = 0;
+	rsp->status_srb = NULL;
+	pkt = rsp->ring_ptr;
+	for (cnt = 0; cnt < rsp->length; cnt++) {
+		pkt->signature = RESPONSE_PROCESSED;
+		WRT_REG_DWORD(&pkt->signature, RESPONSE_PROCESSED);
+		pkt++;
+	}
+}
+
+int
+qlafx00_rescan_isp(scsi_qla_host_t *vha)
+{
+	uint32_t status = QLA_FUNCTION_FAILED;
+	struct qla_hw_data *ha = vha->hw;
+	struct device_reg_fx00 __iomem *reg = &ha->iobase->ispfx00;
+	uint32_t aenmbx7;
+
+	qla2x00_request_irqs(ha, ha->rsp_q_map[0]);
+
+	aenmbx7 = RD_REG_DWORD(&reg->aenmailbox7);
+	ha->mbx_intr_code = MSW(aenmbx7);
+	ha->rqstq_intr_code = LSW(aenmbx7);
+	ha->req_que_off = RD_REG_DWORD(&reg->aenmailbox1);
+	ha->rsp_que_off = RD_REG_DWORD(&reg->aenmailbox3);
+	ha->req_que_len = RD_REG_DWORD(&reg->aenmailbox5);
+	ha->rsp_que_len = RD_REG_DWORD(&reg->aenmailbox6);
+
+	ql_dbg(ql_dbg_disc, vha, 0x2094,
+	    "fw returned mbx_intr_code: 0x%x, rqstq_intr_code: 0x%x "
+	    " Req que offset 0x%x Rsp que offset 0x%x\n",
+	    ha->mbx_intr_code, ha->rqstq_intr_code,
+	    ha->req_que_off, ha->rsp_que_len);
+
+	/* Clear the Interrupts */
+	QLAFX00_CLR_INTR_REG(ha, QLAFX00_HST_INT_STS_BITS);
+
+	status = qla2x00_init_rings(vha);
+	if (!status) {
+		vha->flags.online = 1;
+
+		/* if no cable then assume it's good */
+		if ((vha->device_flags & DFLG_NO_CABLE))
+			status = 0;
+		/* Register system information */
+		if (qlafx00_fx_disc(vha,
+		    &vha->hw->mr.fcport, FXDISC_REG_HOST_INFO))
+			ql_dbg(ql_dbg_disc, vha, 0x2095,
+			    "failed to register host info\n");
+	}
+	scsi_unblock_requests(vha->host);
+	return status;
+}
+
+void
+qlafx00_timer_routine(scsi_qla_host_t *vha)
+{
+	struct qla_hw_data *ha = vha->hw;
+	uint32_t fw_heart_beat;
+	uint32_t aenmbx0;
+	struct device_reg_fx00 __iomem *reg = &ha->iobase->ispfx00;
+
+	/* Check firmware health */
+	if (ha->mr.fw_hbt_cnt)
+		ha->mr.fw_hbt_cnt--;
+	else {
+		if ((!ha->flags.mr_reset_hdlr_active) &&
+		    (!test_bit(UNLOADING, &vha->dpc_flags)) &&
+		    (!test_bit(ABORT_ISP_ACTIVE, &vha->dpc_flags)) &&
+		    (ha->mr.fw_hbt_en)) {
+			fw_heart_beat = RD_REG_DWORD(&reg->fwheartbeat);
+			if (fw_heart_beat != ha->mr.old_fw_hbt_cnt) {
+				ha->mr.old_fw_hbt_cnt = fw_heart_beat;
+				ha->mr.fw_hbt_miss_cnt = 0;
+			} else {
+				ha->mr.fw_hbt_miss_cnt++;
+				if (ha->mr.fw_hbt_miss_cnt ==
+				    QLAFX00_HEARTBEAT_MISS_CNT) {
+					set_bit(ISP_ABORT_NEEDED,
+					    &vha->dpc_flags);
+					qla2xxx_wake_dpc(vha);
+					ha->mr.fw_hbt_miss_cnt = 0;
+				}
+			}
+		}
+		ha->mr.fw_hbt_cnt = QLAFX00_HEARTBEAT_INTERVAL;
+	}
+
+	if (test_bit(FX00_RESET_RECOVERY, &vha->dpc_flags)) {
+		/* Reset recovery to be performed in timer routine */
+		aenmbx0 = RD_REG_DWORD(&reg->aenmailbox0);
+		if (ha->mr.fw_reset_timer_exp) {
+			set_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
+			qla2xxx_wake_dpc(vha);
+			ha->mr.fw_reset_timer_exp = 0;
+		} else if (aenmbx0 == MBA_FW_RESTART_CMPLT) {
+			/* Wake up DPC to rescan the targets */
+			set_bit(FX00_TARGET_SCAN, &vha->dpc_flags);
+			clear_bit(FX00_RESET_RECOVERY, &vha->dpc_flags);
+			qla2xxx_wake_dpc(vha);
+			ha->mr.fw_reset_timer_tick = QLAFX00_RESET_INTERVAL;
+		} else if ((aenmbx0 == MBA_FW_STARTING) &&
+		    (!ha->mr.fw_hbt_en)) {
+			ha->mr.fw_hbt_en = 1;
+		} else if (!ha->mr.fw_reset_timer_tick) {
+			if (aenmbx0 == ha->mr.old_aenmbx0_state)
+				ha->mr.fw_reset_timer_exp = 1;
+			ha->mr.fw_reset_timer_tick = QLAFX00_RESET_INTERVAL;
+		} else if (aenmbx0 == 0xFFFFFFFF) {
+			uint32_t data0, data1;
+
+			data0 = QLAFX00_RD_REG(ha,
+			    QLAFX00_BAR1_BASE_ADDR_REG);
+			data1 = QLAFX00_RD_REG(ha,
+			    QLAFX00_PEX0_WIN0_BASE_ADDR_REG);
+
+			data0 &= 0xffff0000;
+			data1 &= 0x0000ffff;
+
+			QLAFX00_WR_REG(ha,
+			    QLAFX00_PEX0_WIN0_BASE_ADDR_REG,
+			    (data0 | data1));
+		} else if ((aenmbx0 & 0xFF00) == MBA_FW_POLL_STATE) {
+			ha->mr.fw_reset_timer_tick =
+			    QLAFX00_MAX_RESET_INTERVAL;
+		}
+		ha->mr.old_aenmbx0_state = aenmbx0;
+		ha->mr.fw_reset_timer_tick--;
+	}
+}
+
+/*
+ *  qlfx00a_reset_initialize
+ *      Re-initialize after a iSA device reset.
+ *
+ * Input:
+ *      ha  = adapter block pointer.
+ *
+ * Returns:
+ *      0 = success
+ */
+int
+qlafx00_reset_initialize(scsi_qla_host_t *vha)
+{
+	struct qla_hw_data *ha = vha->hw;
+
+	if (vha->device_flags & DFLG_DEV_FAILED) {
+		ql_dbg(ql_dbg_init, vha, 0x0142,
+		    "Device in failed state\n");
+		return QLA_SUCCESS;
+	}
+
+	ha->flags.mr_reset_hdlr_active = 1;
+
+	if (vha->flags.online) {
+		scsi_block_requests(vha->host);
+		qlafx00_abort_isp_cleanup(vha);
+	}
+
+	ql_log(ql_log_info, vha, 0x0143,
+	    "(%s): succeeded.\n", __func__);
+	ha->flags.mr_reset_hdlr_active = 0;
+	return QLA_SUCCESS;
+}
+
+/*
+ *  qlafx00_abort_isp
+ *      Resets ISP and aborts all outstanding commands.
+ *
+ * Input:
+ *      ha  = adapter block pointer.
+ *
+ * Returns:
+ *      0 = success
+ */
+int
+qlafx00_abort_isp(scsi_qla_host_t *vha)
+{
+	struct qla_hw_data *ha = vha->hw;
+
+	if (vha->flags.online) {
+		if (unlikely(pci_channel_offline(ha->pdev) &&
+		    ha->flags.pci_channel_io_perm_failure)) {
+			clear_bit(ISP_ABORT_RETRY, &vha->dpc_flags);
+			return QLA_SUCCESS;
+		}
+
+		scsi_block_requests(vha->host);
+		qlafx00_abort_isp_cleanup(vha);
+	}
+
+	ql_log(ql_log_info, vha, 0x0145,
+	    "(%s): succeeded.\n", __func__);
+
+	return QLA_SUCCESS;
+}
+
+static inline fc_port_t*
+qlafx00_get_fcport(struct scsi_qla_host *vha, int tgt_id)
+{
+	fc_port_t	*fcport;
+
+	/* Check for matching device in remote port list. */
+	fcport = NULL;
+	list_for_each_entry(fcport, &vha->vp_fcports, list) {
+		if (fcport->tgt_id == tgt_id) {
+			ql_dbg(ql_dbg_async, vha, 0x5072,
+			    "Matching fcport(%p) found with TGT-ID: 0x%x "
+			    "and Remote TGT_ID: 0x%x\n",
+			    fcport, fcport->tgt_id, tgt_id);
+			break;
+		}
+	}
+	return fcport;
+}
+
+static void
+qlafx00_tgt_detach(struct scsi_qla_host *vha, int tgt_id)
+{
+	fc_port_t	*fcport;
+
+	ql_log(ql_log_info, vha, 0x5073,
+	    "Detach TGT-ID: 0x%x\n", tgt_id);
+
+	fcport = qlafx00_get_fcport(vha, tgt_id);
+	if (!fcport)
+		return;
+
+	qla2x00_mark_device_lost(vha, fcport, 0, 0);
+
+	return;
+}
+
+int
+qlafx00_process_aen(struct scsi_qla_host *vha, struct qla_work_evt *evt)
+{
+	int rval = 0;
+	uint32_t aen_code, aen_data;
+
+	aen_code = FCH_EVT_VENDOR_UNIQUE;
+	aen_data = evt->u.aenfx.evtcode;
+
+	switch (evt->u.aenfx.evtcode) {
+	case QLAFX00_MBA_PORT_UPDATE:		/* Port database update */
+		if (evt->u.aenfx.mbx[1] == 0) {
+			if (evt->u.aenfx.mbx[2] == 1) {
+				if (!vha->flags.fw_tgt_reported)
+					vha->flags.fw_tgt_reported = 1;
+				atomic_set(&vha->loop_down_timer, 0);
+				atomic_set(&vha->loop_state, LOOP_UP);
+				set_bit(LOOP_RESYNC_NEEDED, &vha->dpc_flags);
+				qla2xxx_wake_dpc(vha);
+			} else if (evt->u.aenfx.mbx[2] == 2) {
+				qlafx00_tgt_detach(vha, evt->u.aenfx.mbx[3]);
+			}
+		} else if (evt->u.aenfx.mbx[1] == 0xffff) {
+			if (evt->u.aenfx.mbx[2] == 1) {
+				if (!vha->flags.fw_tgt_reported)
+					vha->flags.fw_tgt_reported = 1;
+				set_bit(LOOP_RESYNC_NEEDED, &vha->dpc_flags);
+			} else if (evt->u.aenfx.mbx[2] == 2) {
+				vha->device_flags |= DFLG_NO_CABLE;
+				qla2x00_mark_all_devices_lost(vha, 1);
+			}
+		}
+		break;
+	case QLAFX00_MBA_LINK_UP:
+		aen_code = FCH_EVT_LINKUP;
+		aen_data = 0;
+		break;
+	case QLAFX00_MBA_LINK_DOWN:
+		aen_code = FCH_EVT_LINKDOWN;
+		aen_data = 0;
+		break;
+	}
+
+	fc_host_post_event(vha->host, fc_get_event_number(),
+	    aen_code, aen_data);
+
+	return rval;
+}
+
+static void
+qlafx00_update_host_attr(scsi_qla_host_t *vha, struct port_info_data *pinfo)
+{
+	u64 port_name = 0, node_name = 0;
+
+	port_name = (unsigned long long)wwn_to_u64(pinfo->port_name);
+	node_name = (unsigned long long)wwn_to_u64(pinfo->node_name);
+
+	fc_host_node_name(vha->host) = node_name;
+	fc_host_port_name(vha->host) = port_name;
+	if (!pinfo->port_type)
+		vha->hw->current_topology = ISP_CFG_F;
+	if (pinfo->link_status == QLAFX00_LINK_STATUS_UP)
+		atomic_set(&vha->loop_state, LOOP_READY);
+	else if (pinfo->link_status == QLAFX00_LINK_STATUS_DOWN)
+		atomic_set(&vha->loop_state, LOOP_DOWN);
+	vha->hw->link_data_rate = (uint16_t)pinfo->link_config;
+}
+
+static void
+qla2x00_fxdisc_iocb_timeout(void *data)
+{
+	srb_t *sp = (srb_t *)data;
+	struct srb_iocb *lio = &sp->u.iocb_cmd;
+
+	complete(&lio->u.fxiocb.fxiocb_comp);
+}
+
+static void
+qla2x00_fxdisc_sp_done(void *data, void *ptr, int res)
+{
+	srb_t *sp = (srb_t *)ptr;
+	struct srb_iocb *lio = &sp->u.iocb_cmd;
+
+	complete(&lio->u.fxiocb.fxiocb_comp);
+}
+
+int
+qlafx00_fx_disc(scsi_qla_host_t *vha, fc_port_t *fcport, uint8_t fx_type)
+{
+	srb_t *sp;
+	struct srb_iocb *fdisc;
+	int rval = QLA_FUNCTION_FAILED;
+	struct qla_hw_data *ha = vha->hw;
+	struct host_system_info *phost_info;
+	struct register_host_info *preg_hsi;
+	struct new_utsname *p_sysid = NULL;
+	struct timeval tv;
+
+	sp = qla2x00_get_sp(vha, fcport, GFP_KERNEL);
+	if (!sp)
+		goto done;
+
+	fdisc = &sp->u.iocb_cmd;
+	switch (fx_type) {
+	case FXDISC_GET_CONFIG_INFO:
+	fdisc->u.fxiocb.flags =
+		    SRB_FXDISC_RESP_DMA_VALID;
+		fdisc->u.fxiocb.rsp_len = sizeof(struct config_info_data);
+		break;
+	case FXDISC_GET_PORT_INFO:
+		fdisc->u.fxiocb.flags =
+		    SRB_FXDISC_RESP_DMA_VALID | SRB_FXDISC_REQ_DWRD_VALID;
+		fdisc->u.fxiocb.rsp_len = QLAFX00_PORT_DATA_INFO;
+		fdisc->u.fxiocb.req_data = fcport->port_id;
+		break;
+	case FXDISC_GET_TGT_NODE_INFO:
+		fdisc->u.fxiocb.flags =
+		    SRB_FXDISC_RESP_DMA_VALID | SRB_FXDISC_REQ_DWRD_VALID;
+		fdisc->u.fxiocb.rsp_len = QLAFX00_TGT_NODE_INFO;
+		fdisc->u.fxiocb.req_data = fcport->tgt_id;
+		break;
+	case FXDISC_GET_TGT_NODE_LIST:
+		fdisc->u.fxiocb.flags =
+		    SRB_FXDISC_RESP_DMA_VALID | SRB_FXDISC_REQ_DWRD_VALID;
+		fdisc->u.fxiocb.rsp_len = QLAFX00_TGT_NODE_LIST_SIZE;
+		break;
+	case FXDISC_REG_HOST_INFO:
+		fdisc->u.fxiocb.flags = SRB_FXDISC_REQ_DMA_VALID;
+		fdisc->u.fxiocb.req_len = sizeof(struct register_host_info);
+		p_sysid = utsname();
+		if (!p_sysid) {
+			ql_log(ql_log_warn, vha, 0x303c,
+			    "Not able to get the system informtion\n");
+			goto done_free_sp;
+		}
+		break;
+	default:
+		break;
+	}
+
+	if (fdisc->u.fxiocb.flags & SRB_FXDISC_REQ_DMA_VALID) {
+		fdisc->u.fxiocb.req_addr = dma_alloc_coherent(&ha->pdev->dev,
+		    fdisc->u.fxiocb.req_len,
+		    &fdisc->u.fxiocb.req_dma_handle, GFP_KERNEL);
+		if (!fdisc->u.fxiocb.req_addr)
+			goto done_free_sp;
+
+		if (fx_type == FXDISC_REG_HOST_INFO) {
+			preg_hsi = (struct register_host_info *)
+				fdisc->u.fxiocb.req_addr;
+			phost_info = &preg_hsi->hsi;
+			memset(preg_hsi, 0, sizeof(struct register_host_info));
+			phost_info->os_type = OS_TYPE_LINUX;
+			strncpy(phost_info->sysname,
+			    p_sysid->sysname, SYSNAME_LENGTH);
+			strncpy(phost_info->nodename,
+			    p_sysid->nodename, NODENAME_LENGTH);
+			strncpy(phost_info->release,
+			    p_sysid->release, RELEASE_LENGTH);
+			strncpy(phost_info->version,
+			    p_sysid->version, VERSION_LENGTH);
+			strncpy(phost_info->machine,
+			    p_sysid->machine, MACHINE_LENGTH);
+			strncpy(phost_info->domainname,
+			    p_sysid->domainname, DOMNAME_LENGTH);
+			strncpy(phost_info->hostdriver,
+			    QLA2XXX_VERSION, VERSION_LENGTH);
+			do_gettimeofday(&tv);
+			preg_hsi->utc = (uint64_t)tv.tv_sec;
+			ql_dbg(ql_dbg_init, vha, 0x0149,
+			    "ISP%04X: Host registration with firmware\n",
+			    ha->pdev->device);
+			ql_dbg(ql_dbg_init, vha, 0x014a,
+			    "os_type = '%d', sysname = '%s', nodname = '%s'\n",
+			    phost_info->os_type,
+			    phost_info->sysname,
+			    phost_info->nodename);
+			ql_dbg(ql_dbg_init, vha, 0x014b,
+			    "release = '%s', version = '%s'\n",
+			    phost_info->release,
+			    phost_info->version);
+			ql_dbg(ql_dbg_init, vha, 0x014c,
+			    "machine = '%s' "
+			    "domainname = '%s', hostdriver = '%s'\n",
+			    phost_info->machine,
+			    phost_info->domainname,
+			    phost_info->hostdriver);
+			ql_dump_buffer(ql_dbg_init + ql_dbg_disc, vha, 0x014d,
+			    (uint8_t *)phost_info,
+			    sizeof(struct host_system_info));
+		}
+	}
+
+	if (fdisc->u.fxiocb.flags & SRB_FXDISC_RESP_DMA_VALID) {
+		fdisc->u.fxiocb.rsp_addr = dma_alloc_coherent(&ha->pdev->dev,
+		    fdisc->u.fxiocb.rsp_len,
+		    &fdisc->u.fxiocb.rsp_dma_handle, GFP_KERNEL);
+		if (!fdisc->u.fxiocb.rsp_addr)
+			goto done_unmap_req;
+	}
+
+	sp->type = SRB_FXIOCB_DCMD;
+	sp->name = "fxdisc";
+	qla2x00_init_timer(sp, FXDISC_TIMEOUT);
+	fdisc->timeout = qla2x00_fxdisc_iocb_timeout;
+	fdisc->u.fxiocb.req_func_type = fx_type;
+	sp->done = qla2x00_fxdisc_sp_done;
+
+	rval = qla2x00_start_sp(sp);
+	if (rval != QLA_SUCCESS)
+		goto done_unmap_dma;
+
+	wait_for_completion(&fdisc->u.fxiocb.fxiocb_comp);
+
+	if (fx_type == FXDISC_GET_CONFIG_INFO) {
+		struct config_info_data *pinfo =
+		    (struct config_info_data *) fdisc->u.fxiocb.rsp_addr;
+		memcpy(&vha->hw->mr.product_name, pinfo->product_name,
+		    sizeof(vha->hw->mr.product_name));
+		memcpy(&vha->hw->mr.symbolic_name, pinfo->symbolic_name,
+		    sizeof(vha->hw->mr.symbolic_name));
+		memcpy(&vha->hw->mr.serial_num, pinfo->serial_num,
+		    sizeof(vha->hw->mr.serial_num));
+		memcpy(&vha->hw->mr.hw_version, pinfo->hw_version,
+		    sizeof(vha->hw->mr.hw_version));
+		memcpy(&vha->hw->mr.fw_version, pinfo->fw_version,
+		    sizeof(vha->hw->mr.fw_version));
+		strim(vha->hw->mr.fw_version);
+		memcpy(&vha->hw->mr.uboot_version, pinfo->uboot_version,
+		    sizeof(vha->hw->mr.uboot_version));
+		memcpy(&vha->hw->mr.fru_serial_num, pinfo->fru_serial_num,
+		    sizeof(vha->hw->mr.fru_serial_num));
+	} else if (fx_type == FXDISC_GET_PORT_INFO) {
+		struct port_info_data *pinfo =
+		    (struct port_info_data *) fdisc->u.fxiocb.rsp_addr;
+		memcpy(vha->node_name, pinfo->node_name, WWN_SIZE);
+		memcpy(vha->port_name, pinfo->port_name, WWN_SIZE);
+		vha->d_id.b.domain = pinfo->port_id[0];
+		vha->d_id.b.area = pinfo->port_id[1];
+		vha->d_id.b.al_pa = pinfo->port_id[2];
+		qlafx00_update_host_attr(vha, pinfo);
+		ql_dump_buffer(ql_dbg_init + ql_dbg_buffer, vha, 0x0141,
+		    (uint8_t *)pinfo, 16);
+	} else if (fx_type == FXDISC_GET_TGT_NODE_INFO) {
+		struct qlafx00_tgt_node_info *pinfo =
+		    (struct qlafx00_tgt_node_info *) fdisc->u.fxiocb.rsp_addr;
+		memcpy(fcport->node_name, pinfo->tgt_node_wwnn, WWN_SIZE);
+		memcpy(fcport->port_name, pinfo->tgt_node_wwpn, WWN_SIZE);
+		fcport->port_type = FCT_TARGET;
+		ql_dump_buffer(ql_dbg_init + ql_dbg_buffer, vha, 0x0144,
+		    (uint8_t *)pinfo, 16);
+	} else if (fx_type == FXDISC_GET_TGT_NODE_LIST) {
+		struct qlafx00_tgt_node_info *pinfo =
+		    (struct qlafx00_tgt_node_info *) fdisc->u.fxiocb.rsp_addr;
+		ql_dump_buffer(ql_dbg_init + ql_dbg_buffer, vha, 0x0146,
+		    (uint8_t *)pinfo, 16);
+		memcpy(vha->hw->gid_list, pinfo, QLAFX00_TGT_NODE_LIST_SIZE);
+	}
+	rval = fdisc->u.fxiocb.result;
+
+done_unmap_dma:
+	if (fdisc->u.fxiocb.rsp_addr)
+		dma_free_coherent(&ha->pdev->dev, fdisc->u.fxiocb.rsp_len,
+		    fdisc->u.fxiocb.rsp_addr, fdisc->u.fxiocb.rsp_dma_handle);
+
+done_unmap_req:
+	if (fdisc->u.fxiocb.req_addr)
+		dma_free_coherent(&ha->pdev->dev, fdisc->u.fxiocb.req_len,
+		    fdisc->u.fxiocb.req_addr, fdisc->u.fxiocb.req_dma_handle);
+done_free_sp:
+	sp->free(vha, sp);
+done:
+	return rval;
+}
+
+static void
+qlafx00_abort_iocb_timeout(void *data)
+{
+	srb_t *sp = (srb_t *)data;
+	struct srb_iocb *abt = &sp->u.iocb_cmd;
+
+	abt->u.abt.comp_status = CS_TIMEOUT;
+	complete(&abt->u.abt.comp);
+}
+
+static void
+qlafx00_abort_sp_done(void *data, void *ptr, int res)
+{
+	srb_t *sp = (srb_t *)ptr;
+	struct srb_iocb *abt = &sp->u.iocb_cmd;
+
+	complete(&abt->u.abt.comp);
+}
+
+static int
+qlafx00_async_abt_cmd(srb_t *cmd_sp)
+{
+	scsi_qla_host_t *vha = cmd_sp->fcport->vha;
+	fc_port_t *fcport = cmd_sp->fcport;
+	struct srb_iocb *abt_iocb;
+	srb_t *sp;
+	int rval = QLA_FUNCTION_FAILED;
+
+	sp = qla2x00_get_sp(vha, fcport, GFP_KERNEL);
+	if (!sp)
+		goto done;
+
+	abt_iocb = &sp->u.iocb_cmd;
+	sp->type = SRB_ABT_CMD;
+	sp->name = "abort";
+	qla2x00_init_timer(sp, FXDISC_TIMEOUT);
+	abt_iocb->u.abt.cmd_hndl = cmd_sp->handle;
+	sp->done = qlafx00_abort_sp_done;
+	abt_iocb->timeout = qlafx00_abort_iocb_timeout;
+	init_completion(&abt_iocb->u.abt.comp);
+
+	rval = qla2x00_start_sp(sp);
+	if (rval != QLA_SUCCESS)
+		goto done_free_sp;
+
+	ql_dbg(ql_dbg_async, vha, 0x507c,
+	    "Abort command issued - hdl=%x, target_id=%x\n",
+	    cmd_sp->handle, fcport->tgt_id);
+
+	wait_for_completion(&abt_iocb->u.abt.comp);
+
+	rval = abt_iocb->u.abt.comp_status == CS_COMPLETE ?
+	    QLA_SUCCESS : QLA_FUNCTION_FAILED;
+
+done_free_sp:
+	sp->free(vha, sp);
+done:
+	return rval;
+}
+
+int
+qlafx00_abort_command(srb_t *sp)
+{
+	unsigned long   flags = 0;
+
+	uint32_t	handle;
+	fc_port_t	*fcport = sp->fcport;
+	struct scsi_qla_host *vha = fcport->vha;
+	struct qla_hw_data *ha = vha->hw;
+	struct req_que *req = vha->req;
+
+	spin_lock_irqsave(&ha->hardware_lock, flags);
+	for (handle = 1; handle < DEFAULT_OUTSTANDING_COMMANDS; handle++) {
+		if (req->outstanding_cmds[handle] == sp)
+			break;
+	}
+	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+	if (handle == DEFAULT_OUTSTANDING_COMMANDS) {
+		/* Command not found. */
+		return QLA_FUNCTION_FAILED;
+	}
+	return qlafx00_async_abt_cmd(sp);
+}
+
+/*
+ * qlafx00_initialize_adapter
+ *      Initialize board.
+ *
+ * Input:
+ *      ha = adapter block pointer.
+ *
+ * Returns:
+ *      0 = success
+ */
+int
+qlafx00_initialize_adapter(scsi_qla_host_t *vha)
+{
+	int	rval;
+	struct qla_hw_data *ha = vha->hw;
+
+	/* Clear adapter flags. */
+	vha->flags.online = 0;
+	ha->flags.chip_reset_done = 0;
+	vha->flags.reset_active = 0;
+	ha->flags.pci_channel_io_perm_failure = 0;
+	ha->flags.eeh_busy = 0;
+	ha->thermal_support = 0;
+	atomic_set(&vha->loop_down_timer, LOOP_DOWN_TIME);
+	atomic_set(&vha->loop_state, LOOP_DOWN);
+	vha->device_flags = DFLG_NO_CABLE;
+	vha->dpc_flags = 0;
+	vha->flags.management_server_logged_in = 0;
+	vha->marker_needed = 0;
+	ha->isp_abort_cnt = 0;
+	ha->beacon_blink_led = 0;
+
+	set_bit(0, ha->req_qid_map);
+	set_bit(0, ha->rsp_qid_map);
+
+	ql_dbg(ql_dbg_init, vha, 0x0147,
+	    "Configuring PCI space...\n");
+
+	rval = ha->isp_ops->pci_config(vha);
+	if (rval) {
+		ql_log(ql_log_warn, vha, 0x0148,
+		    "Unable to configure PCI space.\n");
+		return rval;
+	}
+
+	rval = qlafx00_init_fw_ready(vha);
+	if (rval != QLA_SUCCESS)
+		return rval;
+
+	qlafx00_save_queue_ptrs(vha);
+
+	rval = qlafx00_config_queues(vha);
+	if (rval != QLA_SUCCESS)
+		return rval;
+
+	/*
+	 * Allocate the array of outstanding commands
+	 * now that we know the firmware resources.
+	 */
+	rval = qla2x00_alloc_outstanding_cmds(ha, vha->req);
+	if (rval != QLA_SUCCESS)
+		return rval;
+
+	rval = qla2x00_init_rings(vha);
+	ha->flags.chip_reset_done = 1;
+
+	return rval;
+}
+
+uint32_t
+qlafx00_fw_state_show(struct device *dev, struct device_attribute *attr,
+		      char *buf)
+{
+	scsi_qla_host_t *vha = shost_priv(class_to_shost(dev));
+	int rval = QLA_FUNCTION_FAILED;
+	uint32_t state[1];
+
+	if (qla2x00_reset_active(vha))
+		ql_log(ql_log_warn, vha, 0x70ce,
+		    "ISP reset active.\n");
+	else if (!vha->hw->flags.eeh_busy) {
+		rval = qlafx00_get_firmware_state(vha, state);
+	}
+	if (rval != QLA_SUCCESS)
+		memset(state, -1, sizeof(state));
+
+	return state[0];
+}
+
+void
+qlafx00_get_host_speed(struct Scsi_Host *shost)
+{
+	struct qla_hw_data *ha = ((struct scsi_qla_host *)
+					(shost_priv(shost)))->hw;
+	u32 speed = FC_PORTSPEED_UNKNOWN;
+
+	switch (ha->link_data_rate) {
+	case QLAFX00_PORT_SPEED_2G:
+		speed = FC_PORTSPEED_2GBIT;
+		break;
+	case QLAFX00_PORT_SPEED_4G:
+		speed = FC_PORTSPEED_4GBIT;
+		break;
+	case QLAFX00_PORT_SPEED_8G:
+		speed = FC_PORTSPEED_8GBIT;
+		break;
+	case QLAFX00_PORT_SPEED_10G:
+		speed = FC_PORTSPEED_10GBIT;
+		break;
+	}
+	fc_host_speed(shost) = speed;
+}
+
+/** QLAFX00 specific ISR implementation functions */
+
+static inline void
+qlafx00_handle_sense(srb_t *sp, uint8_t *sense_data, uint32_t par_sense_len,
+		     uint32_t sense_len, struct rsp_que *rsp, int res)
+{
+	struct scsi_qla_host *vha = sp->fcport->vha;
+	struct scsi_cmnd *cp = GET_CMD_SP(sp);
+	uint32_t track_sense_len;
+
+	SET_FW_SENSE_LEN(sp, sense_len);
+
+	if (sense_len >= SCSI_SENSE_BUFFERSIZE)
+		sense_len = SCSI_SENSE_BUFFERSIZE;
+
+	SET_CMD_SENSE_LEN(sp, sense_len);
+	SET_CMD_SENSE_PTR(sp, cp->sense_buffer);
+	track_sense_len = sense_len;
+
+	if (sense_len > par_sense_len)
+		sense_len = par_sense_len;
+
+	memcpy(cp->sense_buffer, sense_data, sense_len);
+
+	SET_FW_SENSE_LEN(sp, GET_FW_SENSE_LEN(sp) - sense_len);
+
+	SET_CMD_SENSE_PTR(sp, cp->sense_buffer + sense_len);
+	track_sense_len -= sense_len;
+	SET_CMD_SENSE_LEN(sp, track_sense_len);
+
+	ql_dbg(ql_dbg_io, vha, 0x304d,
+	    "sense_len=0x%x par_sense_len=0x%x track_sense_len=0x%x.\n",
+	    sense_len, par_sense_len, track_sense_len);
+	if (GET_FW_SENSE_LEN(sp) > 0) {
+		rsp->status_srb = sp;
+		cp->result = res;
+	}
+
+	if (sense_len) {
+		ql_dbg(ql_dbg_io + ql_dbg_buffer, vha, 0x3039,
+		    "Check condition Sense data, nexus%ld:%d:%d cmd=%p.\n",
+		    sp->fcport->vha->host_no, cp->device->id, cp->device->lun,
+		    cp);
+		ql_dump_buffer(ql_dbg_io + ql_dbg_buffer, vha, 0x3049,
+		    cp->sense_buffer, sense_len);
+	}
+}
+
+static void
+qlafx00_tm_iocb_entry(scsi_qla_host_t *vha, struct req_que *req,
+		      struct tsk_mgmt_entry_fx00 *pkt, srb_t *sp,
+		      uint16_t sstatus, uint16_t cpstatus)
+{
+	struct srb_iocb *tmf;
+
+	tmf = &sp->u.iocb_cmd;
+	if (cpstatus != CS_COMPLETE ||
+	    (sstatus & SS_RESPONSE_INFO_LEN_VALID))
+		cpstatus = CS_INCOMPLETE;
+	tmf->u.tmf.comp_status = cpstatus;
+	sp->done(vha, sp, 0);
+}
+
+static void
+qlafx00_abort_iocb_entry(scsi_qla_host_t *vha, struct req_que *req,
+			 struct abort_iocb_entry_fx00 *pkt)
+{
+	const char func[] = "ABT_IOCB";
+	srb_t *sp;
+	struct srb_iocb *abt;
+
+	sp = qla2x00_get_sp_from_handle(vha, func, req, pkt);
+	if (!sp)
+		return;
+
+	abt = &sp->u.iocb_cmd;
+	abt->u.abt.comp_status = le32_to_cpu(pkt->tgt_id_sts);
+	sp->done(vha, sp, 0);
+}
+
+static void
+qlafx00_ioctl_iosb_entry(scsi_qla_host_t *vha, struct req_que *req,
+			 struct ioctl_iocb_entry_fx00 *pkt)
+{
+	const char func[] = "IOSB_IOCB";
+	srb_t *sp;
+	struct fc_bsg_job *bsg_job;
+	struct srb_iocb *iocb_job;
+	int res;
+	struct qla_mt_iocb_rsp_fx00 fstatus;
+	uint8_t	*fw_sts_ptr;
+
+	sp = qla2x00_get_sp_from_handle(vha, func, req, pkt);
+	if (!sp)
+		return;
+
+	if (sp->type == SRB_FXIOCB_DCMD) {
+		iocb_job = &sp->u.iocb_cmd;
+		iocb_job->u.fxiocb.seq_number = le32_to_cpu(pkt->seq_no);
+		iocb_job->u.fxiocb.fw_flags = le32_to_cpu(pkt->fw_iotcl_flags);
+		iocb_job->u.fxiocb.result = le32_to_cpu(pkt->status);
+		if (iocb_job->u.fxiocb.flags & SRB_FXDISC_RSP_DWRD_VALID)
+			iocb_job->u.fxiocb.req_data =
+			    le32_to_cpu(pkt->dataword_r);
+	} else {
+		bsg_job = sp->u.bsg_job;
+
+		memset(&fstatus, 0, sizeof(struct qla_mt_iocb_rsp_fx00));
+
+		fstatus.reserved_1 = pkt->reserved_0;
+		fstatus.func_type = pkt->comp_func_num;
+		fstatus.ioctl_flags = pkt->fw_iotcl_flags;
+		fstatus.ioctl_data = pkt->dataword_r;
+		fstatus.adapid = pkt->adapid;
+		fstatus.adapid_hi = pkt->adapid_hi;
+		fstatus.reserved_2 = pkt->reserved_1;
+		fstatus.res_count = pkt->residuallen;
+		fstatus.status = pkt->status;
+		fstatus.seq_number = pkt->seq_no;
+		memcpy(fstatus.reserved_3,
+		    pkt->reserved_2, 20 * sizeof(uint8_t));
+
+		fw_sts_ptr = ((uint8_t *)bsg_job->req->sense) +
+		    sizeof(struct fc_bsg_reply);
+
+		memcpy(fw_sts_ptr, (uint8_t *)&fstatus,
+		    sizeof(struct qla_mt_iocb_rsp_fx00));
+		bsg_job->reply_len = sizeof(struct fc_bsg_reply) +
+			sizeof(struct qla_mt_iocb_rsp_fx00) + sizeof(uint8_t);
+
+		ql_dump_buffer(ql_dbg_user + ql_dbg_verbose,
+		    sp->fcport->vha, 0x5080,
+		    (uint8_t *)pkt, sizeof(struct ioctl_iocb_entry_fx00));
+
+		ql_dump_buffer(ql_dbg_user + ql_dbg_verbose,
+		    sp->fcport->vha, 0x5074,
+		    (uint8_t *)fw_sts_ptr, sizeof(struct qla_mt_iocb_rsp_fx00));
+
+		res = bsg_job->reply->result = DID_OK << 16;
+		bsg_job->reply->reply_payload_rcv_len =
+		    bsg_job->reply_payload.payload_len;
+	}
+	sp->done(vha, sp, res);
+}
+
+/**
+ * qlafx00_status_entry() - Process a Status IOCB entry.
+ * @ha: SCSI driver HA context
+ * @pkt: Entry pointer
+ */
+static void
+qlafx00_status_entry(scsi_qla_host_t *vha, struct rsp_que *rsp, void *pkt)
+{
+	srb_t		*sp;
+	fc_port_t	*fcport;
+	struct scsi_cmnd *cp;
+	struct sts_entry_fx00 *sts;
+	uint16_t	comp_status;
+	uint16_t	scsi_status;
+	uint16_t	ox_id;
+	uint8_t		lscsi_status;
+	int32_t		resid;
+	uint32_t	sense_len, par_sense_len, rsp_info_len, resid_len,
+	    fw_resid_len;
+	uint8_t		*rsp_info = NULL, *sense_data = NULL;
+	struct qla_hw_data *ha = vha->hw;
+	uint32_t hindex, handle;
+	uint16_t que;
+	struct req_que *req;
+	int logit = 1;
+	int res = 0;
+
+	sts = (struct sts_entry_fx00 *) pkt;
+
+	comp_status = le16_to_cpu(sts->comp_status);
+	scsi_status = le16_to_cpu(sts->scsi_status) & SS_MASK;
+	hindex = sts->handle;
+	handle = LSW(hindex);
+
+	que = MSW(hindex);
+	req = ha->req_q_map[que];
+
+	/* Validate handle. */
+	if (handle < req->num_outstanding_cmds)
+		sp = req->outstanding_cmds[handle];
+	else
+		sp = NULL;
+
+	if (sp == NULL) {
+		ql_dbg(ql_dbg_io, vha, 0x3034,
+		    "Invalid status handle (0x%x).\n", handle);
+
+		set_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
+		qla2xxx_wake_dpc(vha);
+		return;
+	}
+
+	if (sp->type == SRB_TM_CMD) {
+		req->outstanding_cmds[handle] = NULL;
+		qlafx00_tm_iocb_entry(vha, req, pkt, sp,
+		    scsi_status, comp_status);
+		return;
+	}
+
+	/* Fast path completion. */
+	if (comp_status == CS_COMPLETE && scsi_status == 0) {
+		qla2x00_do_host_ramp_up(vha);
+		qla2x00_process_completed_request(vha, req, handle);
+		return;
+	}
+
+	req->outstanding_cmds[handle] = NULL;
+	cp = GET_CMD_SP(sp);
+	if (cp == NULL) {
+		ql_dbg(ql_dbg_io, vha, 0x3048,
+		    "Command already returned (0x%x/%p).\n",
+		    handle, sp);
+
+		return;
+	}
+
+	lscsi_status = scsi_status & STATUS_MASK;
+
+	fcport = sp->fcport;
+
+	ox_id = 0;
+	sense_len = par_sense_len = rsp_info_len = resid_len =
+		fw_resid_len = 0;
+	if (scsi_status & SS_SENSE_LEN_VALID)
+		sense_len = le32_to_cpu(sts->sense_len);
+	if (scsi_status & (SS_RESIDUAL_UNDER | SS_RESIDUAL_OVER))
+		resid_len = le32_to_cpu(sts->residual_len);
+	if (comp_status == CS_DATA_UNDERRUN)
+		fw_resid_len = le32_to_cpu(sts->residual_len);
+	rsp_info = sense_data = sts->data;
+	par_sense_len = sizeof(sts->data);
+
+	/* Check for overrun. */
+	if (comp_status == CS_COMPLETE &&
+	    scsi_status & SS_RESIDUAL_OVER)
+		comp_status = CS_DATA_OVERRUN;
+
+	/*
+	 * Based on Host and scsi status generate status code for Linux
+	 */
+	switch (comp_status) {
+	case CS_COMPLETE:
+	case CS_QUEUE_FULL:
+		if (scsi_status == 0) {
+			res = DID_OK << 16;
+			break;
+		}
+		if (scsi_status & (SS_RESIDUAL_UNDER | SS_RESIDUAL_OVER)) {
+			resid = resid_len;
+			scsi_set_resid(cp, resid);
+
+			if (!lscsi_status &&
+			    ((unsigned)(scsi_bufflen(cp) - resid) <
+			     cp->underflow)) {
+				ql_dbg(ql_dbg_io, fcport->vha, 0x3050,
+				    "Mid-layer underflow "
+				    "detected (0x%x of 0x%x bytes).\n",
+				    resid, scsi_bufflen(cp));
+
+				res = DID_ERROR << 16;
+				break;
+			}
+		}
+		res = DID_OK << 16 | lscsi_status;
+
+		if (lscsi_status == SAM_STAT_TASK_SET_FULL) {
+			ql_dbg(ql_dbg_io, fcport->vha, 0x3051,
+			    "QUEUE FULL detected.\n");
+			break;
+		}
+		logit = 0;
+		if (lscsi_status != SS_CHECK_CONDITION)
+			break;
+
+		memset(cp->sense_buffer, 0, SCSI_SENSE_BUFFERSIZE);
+		if (!(scsi_status & SS_SENSE_LEN_VALID))
+			break;
+
+		qlafx00_handle_sense(sp, sense_data, par_sense_len, sense_len,
+		    rsp, res);
+		break;
+
+	case CS_DATA_UNDERRUN:
+		/* Use F/W calculated residual length. */
+		if (IS_FWI2_CAPABLE(ha) || IS_QLAFX00(ha))
+			resid = fw_resid_len;
+		else
+			resid = resid_len;
+		scsi_set_resid(cp, resid);
+		if (scsi_status & SS_RESIDUAL_UNDER) {
+			if ((IS_FWI2_CAPABLE(ha) || IS_QLAFX00(ha))
+			    && fw_resid_len != resid_len) {
+				ql_dbg(ql_dbg_io, fcport->vha, 0x3052,
+				    "Dropped frame(s) detected "
+				    "(0x%x of 0x%x bytes).\n",
+				    resid, scsi_bufflen(cp));
+
+				res = DID_ERROR << 16 | lscsi_status;
+				goto check_scsi_status;
+			}
+
+			if (!lscsi_status &&
+			    ((unsigned)(scsi_bufflen(cp) - resid) <
+			    cp->underflow)) {
+				ql_dbg(ql_dbg_io, fcport->vha, 0x3053,
+				    "Mid-layer underflow "
+				    "detected (0x%x of 0x%x bytes, "
+				    "cp->underflow: 0x%x).\n",
+				    resid, scsi_bufflen(cp), cp->underflow);
+
+				res = DID_ERROR << 16;
+				break;
+			}
+		} else if (lscsi_status != SAM_STAT_TASK_SET_FULL &&
+			    lscsi_status != SAM_STAT_BUSY) {
+			/*
+			 * scsi status of task set and busy are considered
+			 * to be task not completed.
+			 */
+
+			ql_dbg(ql_dbg_io, fcport->vha, 0x3054,
+			    "Dropped frame(s) detected (0x%x "
+			    "of 0x%x bytes).\n", resid,
+			    scsi_bufflen(cp));
+
+			res = DID_ERROR << 16 | lscsi_status;
+			goto check_scsi_status;
+		} else {
+			ql_dbg(ql_dbg_io, fcport->vha, 0x3055,
+			    "scsi_status: 0x%x, lscsi_status: 0x%x\n",
+			    scsi_status, lscsi_status);
+		}
+
+		res = DID_OK << 16 | lscsi_status;
+		logit = 0;
+
+check_scsi_status:
+		/*
+		 * Check to see if SCSI Status is non zero. If so report SCSI
+		 * Status.
+		 */
+		if (lscsi_status != 0) {
+			if (lscsi_status == SAM_STAT_TASK_SET_FULL) {
+				ql_dbg(ql_dbg_io, fcport->vha, 0x3056,
+				    "QUEUE FULL detected.\n");
+				logit = 1;
+				break;
+			}
+			if (lscsi_status != SS_CHECK_CONDITION)
+				break;
+
+			memset(cp->sense_buffer, 0, SCSI_SENSE_BUFFERSIZE);
+			if (!(scsi_status & SS_SENSE_LEN_VALID))
+				break;
+
+			qlafx00_handle_sense(sp, sense_data, par_sense_len,
+			    sense_len, rsp, res);
+		}
+		break;
+
+	case CS_PORT_LOGGED_OUT:
+	case CS_PORT_CONFIG_CHG:
+	case CS_PORT_BUSY:
+	case CS_INCOMPLETE:
+	case CS_PORT_UNAVAILABLE:
+	case CS_TIMEOUT:
+	case CS_RESET:
+
+		/*
+		 * We are going to have the fc class block the rport
+		 * while we try to recover so instruct the mid layer
+		 * to requeue until the class decides how to handle this.
+		 */
+		res = DID_TRANSPORT_DISRUPTED << 16;
+
+		ql_dbg(ql_dbg_io, fcport->vha, 0x3057,
+		    "Port down status: port-state=0x%x.\n",
+		    atomic_read(&fcport->state));
+
+		if (atomic_read(&fcport->state) == FCS_ONLINE)
+			qla2x00_mark_device_lost(fcport->vha, fcport, 1, 1);
+		break;
+
+	case CS_ABORTED:
+		res = DID_RESET << 16;
+		break;
+
+	default:
+		res = DID_ERROR << 16;
+		break;
+	}
+
+	if (logit)
+		ql_dbg(ql_dbg_io, fcport->vha, 0x3058,
+		    "FCP command status: 0x%x-0x%x (0x%x) "
+		    "nexus=%ld:%d:%d tgt_id: 0x%x lscsi_status: 0x%x"
+		    "cdb=%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x len=0x%x "
+		    "rsp_info=0x%x resid=0x%x fw_resid=0x%x "
+		    "sense_len=0x%x, par_sense_len=0x%x, rsp_info_len=0x%x\n",
+		    comp_status, scsi_status, res, vha->host_no,
+		    cp->device->id, cp->device->lun, fcport->tgt_id,
+		    lscsi_status, cp->cmnd[0], cp->cmnd[1], cp->cmnd[2],
+		    cp->cmnd[3], cp->cmnd[4], cp->cmnd[5], cp->cmnd[6],
+		    cp->cmnd[7], cp->cmnd[8], cp->cmnd[9], scsi_bufflen(cp),
+		    rsp_info_len, resid_len, fw_resid_len, sense_len,
+		    par_sense_len, rsp_info_len);
+
+	if (!res)
+		qla2x00_do_host_ramp_up(vha);
+
+	if (rsp->status_srb == NULL)
+		sp->done(ha, sp, res);
+}
+
+/**
+ * qlafx00_status_cont_entry() - Process a Status Continuations entry.
+ * @ha: SCSI driver HA context
+ * @pkt: Entry pointer
+ *
+ * Extended sense data.
+ */
+static void
+qlafx00_status_cont_entry(struct rsp_que *rsp, sts_cont_entry_t *pkt)
+{
+	uint8_t	sense_sz = 0;
+	struct qla_hw_data *ha = rsp->hw;
+	struct scsi_qla_host *vha = pci_get_drvdata(ha->pdev);
+	srb_t *sp = rsp->status_srb;
+	struct scsi_cmnd *cp;
+	uint32_t sense_len;
+	uint8_t *sense_ptr;
+
+	if (!sp) {
+		ql_dbg(ql_dbg_io, vha, 0x3037,
+		    "no SP, sp = %p\n", sp);
+		return;
+	}
+
+	if (!GET_FW_SENSE_LEN(sp)) {
+		ql_dbg(ql_dbg_io, vha, 0x304b,
+		    "no fw sense data, sp = %p\n", sp);
+		return;
+	}
+	cp = GET_CMD_SP(sp);
+	if (cp == NULL) {
+		ql_log(ql_log_warn, vha, 0x303b,
+		    "cmd is NULL: already returned to OS (sp=%p).\n", sp);
+
+		rsp->status_srb = NULL;
+		return;
+	}
+
+	if (!GET_CMD_SENSE_LEN(sp)) {
+		ql_dbg(ql_dbg_io, vha, 0x304c,
+		    "no sense data, sp = %p\n", sp);
+	} else {
+		sense_len = GET_CMD_SENSE_LEN(sp);
+		sense_ptr = GET_CMD_SENSE_PTR(sp);
+		ql_dbg(ql_dbg_io, vha, 0x304f,
+		    "sp=%p sense_len=0x%x sense_ptr=%p.\n",
+		    sp, sense_len, sense_ptr);
+
+		if (sense_len > sizeof(pkt->data))
+			sense_sz = sizeof(pkt->data);
+		else
+			sense_sz = sense_len;
+
+		/* Move sense data. */
+		ql_dump_buffer(ql_dbg_io + ql_dbg_buffer, vha, 0x304e,
+		    (uint8_t *)pkt, sizeof(sts_cont_entry_t));
+		memcpy(sense_ptr, pkt->data, sense_sz);
+		ql_dump_buffer(ql_dbg_io + ql_dbg_buffer, vha, 0x304a,
+		    sense_ptr, sense_sz);
+
+		sense_len -= sense_sz;
+		sense_ptr += sense_sz;
+
+		SET_CMD_SENSE_PTR(sp, sense_ptr);
+		SET_CMD_SENSE_LEN(sp, sense_len);
+	}
+	sense_len = GET_FW_SENSE_LEN(sp);
+	sense_len = (sense_len > sizeof(pkt->data)) ?
+	    (sense_len - sizeof(pkt->data)) : 0;
+	SET_FW_SENSE_LEN(sp, sense_len);
+
+	/* Place command on done queue. */
+	if (sense_len == 0) {
+		rsp->status_srb = NULL;
+		sp->done(ha, sp, cp->result);
+	}
+}
+
+/**
+ * qlafx00_multistatus_entry() - Process Multi response queue entries.
+ * @ha: SCSI driver HA context
+ */
+static void
+qlafx00_multistatus_entry(struct scsi_qla_host *vha,
+	struct rsp_que *rsp, void *pkt)
+{
+	srb_t		*sp;
+	struct multi_sts_entry_fx00 *stsmfx;
+	struct qla_hw_data *ha = vha->hw;
+	uint32_t handle, hindex, handle_count, i;
+	uint16_t que;
+	struct req_que *req;
+	uint32_t *handle_ptr;
+
+	stsmfx = (struct multi_sts_entry_fx00 *) pkt;
+
+	handle_count = stsmfx->handle_count;
+
+	if (handle_count > MAX_HANDLE_COUNT) {
+		ql_dbg(ql_dbg_io, vha, 0x3035,
+		    "Invalid handle count (0x%x).\n", handle_count);
+		set_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
+		qla2xxx_wake_dpc(vha);
+		return;
+	}
+
+	handle_ptr = (uint32_t *) &stsmfx->handles[0];
+
+	for (i = 0; i < handle_count; i++) {
+		hindex = le32_to_cpu(*handle_ptr);
+		handle = LSW(hindex);
+		que = MSW(hindex);
+		req = ha->req_q_map[que];
+
+		/* Validate handle. */
+		if (handle < req->num_outstanding_cmds)
+			sp = req->outstanding_cmds[handle];
+		else
+			sp = NULL;
+
+		if (sp == NULL) {
+			ql_dbg(ql_dbg_io, vha, 0x3044,
+			    "Invalid status handle (0x%x).\n", handle);
+			set_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
+			qla2xxx_wake_dpc(vha);
+			return;
+		}
+		qla2x00_process_completed_request(vha, req, handle);
+		handle_ptr++;
+	}
+}
+
+/**
+ * qlafx00_error_entry() - Process an error entry.
+ * @ha: SCSI driver HA context
+ * @pkt: Entry pointer
+ */
+static void
+qlafx00_error_entry(scsi_qla_host_t *vha, struct rsp_que *rsp,
+		    struct sts_entry_fx00 *pkt, uint8_t estatus, uint8_t etype)
+{
+	srb_t *sp;
+	struct qla_hw_data *ha = vha->hw;
+	const char func[] = "ERROR-IOCB";
+	uint16_t que = MSW(pkt->handle);
+	struct req_que *req = NULL;
+	int res = DID_ERROR << 16;
+
+	ql_dbg(ql_dbg_async, vha, 0x507f,
+	    "type of error status in response: 0x%x\n", estatus);
+
+	req = ha->req_q_map[que];
+
+	sp = qla2x00_get_sp_from_handle(vha, func, req, pkt);
+	if (sp) {
+		sp->done(ha, sp, res);
+		return;
+	}
+
+	set_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
+	qla2xxx_wake_dpc(vha);
+}
+
+/**
+ * qlafx00_process_response_queue() - Process response queue entries.
+ * @ha: SCSI driver HA context
+ */
+static void
+qlafx00_process_response_queue(struct scsi_qla_host *vha,
+	struct rsp_que *rsp)
+{
+	struct sts_entry_fx00 *pkt;
+	response_t *lptr;
+
+	if (!vha->flags.online)
+		return;
+
+	while (RD_REG_DWORD(&(rsp->ring_ptr->signature)) !=
+	    RESPONSE_PROCESSED) {
+		lptr = rsp->ring_ptr;
+		memcpy_fromio(rsp->rsp_pkt, lptr, sizeof(rsp->rsp_pkt));
+		pkt = (struct sts_entry_fx00 *)rsp->rsp_pkt;
+
+		rsp->ring_index++;
+		if (rsp->ring_index == rsp->length) {
+			rsp->ring_index = 0;
+			rsp->ring_ptr = rsp->ring;
+		} else {
+			rsp->ring_ptr++;
+		}
+
+		if (pkt->entry_status != 0 &&
+		    pkt->entry_type != IOCTL_IOSB_TYPE_FX00) {
+			qlafx00_error_entry(vha, rsp,
+			    (struct sts_entry_fx00 *)pkt, pkt->entry_status,
+			    pkt->entry_type);
+			goto next_iter;
+			continue;
+		}
+
+		switch (pkt->entry_type) {
+		case STATUS_TYPE_FX00:
+			qlafx00_status_entry(vha, rsp, pkt);
+			break;
+
+		case STATUS_CONT_TYPE_FX00:
+			qlafx00_status_cont_entry(rsp, (sts_cont_entry_t *)pkt);
+			break;
+
+		case MULTI_STATUS_TYPE_FX00:
+			qlafx00_multistatus_entry(vha, rsp, pkt);
+			break;
+
+		case ABORT_IOCB_TYPE_FX00:
+			qlafx00_abort_iocb_entry(vha, rsp->req,
+			   (struct abort_iocb_entry_fx00 *)pkt);
+			break;
+
+		case IOCTL_IOSB_TYPE_FX00:
+			qlafx00_ioctl_iosb_entry(vha, rsp->req,
+			    (struct ioctl_iocb_entry_fx00 *)pkt);
+			break;
+		default:
+			/* Type Not Supported. */
+			ql_dbg(ql_dbg_async, vha, 0x5081,
+			    "Received unknown response pkt type %x "
+			    "entry status=%x.\n",
+			    pkt->entry_type, pkt->entry_status);
+			break;
+		}
+next_iter:
+		WRT_REG_DWORD(&lptr->signature, RESPONSE_PROCESSED);
+		wmb();
+	}
+
+	/* Adjust ring index */
+	WRT_REG_DWORD(rsp->rsp_q_out, rsp->ring_index);
+}
+
+/**
+ * qlafx00_async_event() - Process aynchronous events.
+ * @ha: SCSI driver HA context
+ */
+static void
+qlafx00_async_event(scsi_qla_host_t *vha)
+{
+	struct qla_hw_data *ha = vha->hw;
+	struct device_reg_fx00 __iomem *reg;
+	int data_size = 1;
+
+	reg = &ha->iobase->ispfx00;
+	/* Setup to process RIO completion. */
+	switch (ha->aenmb[0]) {
+	case QLAFX00_MBA_SYSTEM_ERR:		/* System Error */
+		ql_log(ql_log_warn, vha, 0x5079,
+		    "ISP System Error - mbx1=%x\n", ha->aenmb[0]);
+		set_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
+		break;
+
+	case QLAFX00_MBA_SHUTDOWN_RQSTD:	/* Shutdown requested */
+		ql_dbg(ql_dbg_async, vha, 0x5076,
+		    "Asynchronous FW shutdown requested.\n");
+		set_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
+		qla2xxx_wake_dpc(vha);
+		break;
+
+	case QLAFX00_MBA_PORT_UPDATE:		/* Port database update */
+		ha->aenmb[1] = RD_REG_WORD(&reg->aenmailbox1);
+		ha->aenmb[2] = RD_REG_WORD(&reg->aenmailbox2);
+		ha->aenmb[3] = RD_REG_WORD(&reg->aenmailbox3);
+		ql_dbg(ql_dbg_async, vha, 0x5077,
+		    "Asynchronous port Update received "
+		    "aenmb[0]: %x, aenmb[1]: %x, aenmb[2]: %x, aenmb[3]: %x\n",
+		    ha->aenmb[0], ha->aenmb[1], ha->aenmb[2], ha->aenmb[3]);
+		data_size = 4;
+		break;
+	default:
+		ha->aenmb[1] = RD_REG_WORD(&reg->aenmailbox1);
+		ha->aenmb[2] = RD_REG_WORD(&reg->aenmailbox2);
+		ha->aenmb[3] = RD_REG_WORD(&reg->aenmailbox3);
+		ha->aenmb[4] = RD_REG_WORD(&reg->aenmailbox4);
+		ha->aenmb[5] = RD_REG_WORD(&reg->aenmailbox5);
+		ha->aenmb[6] = RD_REG_WORD(&reg->aenmailbox6);
+		ha->aenmb[7] = RD_REG_WORD(&reg->aenmailbox7);
+		ql_dbg(ql_dbg_async, vha, 0x5078,
+		    "AEN:%04x %04x %04x %04x :%04x %04x %04x %04x\n",
+		    ha->aenmb[0], ha->aenmb[1], ha->aenmb[2], ha->aenmb[3],
+		    ha->aenmb[4], ha->aenmb[5], ha->aenmb[6], ha->aenmb[7]);
+		break;
+	}
+	qlafx00_post_aenfx_work(vha, ha->aenmb[0],
+	    (uint32_t *)ha->aenmb, data_size);
+}
+
+/**
+ *
+ * qlafx00x_mbx_completion() - Process mailbox command completions.
+ * @ha: SCSI driver HA context
+ * @mb16: Mailbox16 register
+ */
+static void
+qlafx00_mbx_completion(scsi_qla_host_t *vha, uint32_t mb0)
+{
+	uint16_t	cnt;
+	uint16_t __iomem *wptr;
+	struct qla_hw_data *ha = vha->hw;
+	struct device_reg_fx00 __iomem *reg = &ha->iobase->ispfx00;
+
+	if (!ha->mcp32)
+		ql_dbg(ql_dbg_async, vha, 0x507e, "MBX pointer ERROR.\n");
+
+	/* Load return mailbox registers. */
+	ha->flags.mbox_int = 1;
+	ha->mailbox_out32[0] = mb0;
+	wptr = (uint16_t __iomem *)&reg->mailbox17;
+
+	for (cnt = 1; cnt < ha->mbx_count; cnt++) {
+		ha->mailbox_out32[cnt] = RD_REG_WORD(wptr);
+		wptr++;
+	}
+}
+
+/**
+ * qlafx00_intr_handler() - Process interrupts for the ISPFX00.
+ * @irq:
+ * @dev_id: SCSI driver HA context
+ *
+ * Called by system whenever the host adapter generates an interrupt.
+ *
+ * Returns handled flag.
+ */
+irqreturn_t
+qlafx00_intr_handler(int irq, void *dev_id)
+{
+	scsi_qla_host_t	*vha;
+	struct qla_hw_data *ha;
+	struct device_reg_fx00 __iomem *reg;
+	int		status;
+	unsigned long	iter;
+	uint32_t	stat;
+	uint32_t	mb[8];
+	struct rsp_que *rsp;
+	unsigned long	flags;
+	uint32_t clr_intr = 0;
+
+	rsp = (struct rsp_que *) dev_id;
+	if (!rsp) {
+		ql_log(ql_log_info, NULL, 0x507d,
+		    "%s: NULL response queue pointer.\n", __func__);
+		return IRQ_NONE;
+	}
+
+	ha = rsp->hw;
+	reg = &ha->iobase->ispfx00;
+	status = 0;
+
+	if (unlikely(pci_channel_offline(ha->pdev)))
+		return IRQ_HANDLED;
+
+	spin_lock_irqsave(&ha->hardware_lock, flags);
+	vha = pci_get_drvdata(ha->pdev);
+	for (iter = 50; iter--; clr_intr = 0) {
+		stat = QLAFX00_RD_INTR_REG(ha);
+		if ((stat & QLAFX00_HST_INT_STS_BITS) == 0)
+			break;
+
+		switch (stat & QLAFX00_HST_INT_STS_BITS) {
+		case QLAFX00_INTR_MB_CMPLT:
+		case QLAFX00_INTR_MB_RSP_CMPLT:
+		case QLAFX00_INTR_MB_ASYNC_CMPLT:
+		case QLAFX00_INTR_ALL_CMPLT:
+			mb[0] = RD_REG_WORD(&reg->mailbox16);
+			qlafx00_mbx_completion(vha, mb[0]);
+			status |= MBX_INTERRUPT;
+			clr_intr |= QLAFX00_INTR_MB_CMPLT;
+			break;
+		case QLAFX00_INTR_ASYNC_CMPLT:
+		case QLAFX00_INTR_RSP_ASYNC_CMPLT:
+			ha->aenmb[0] = RD_REG_WORD(&reg->aenmailbox0);
+			qlafx00_async_event(vha);
+			clr_intr |= QLAFX00_INTR_ASYNC_CMPLT;
+			break;
+		case QLAFX00_INTR_RSP_CMPLT:
+			qlafx00_process_response_queue(vha, rsp);
+			clr_intr |= QLAFX00_INTR_RSP_CMPLT;
+			break;
+		default:
+			ql_dbg(ql_dbg_async, vha, 0x507a,
+			    "Unrecognized interrupt type (%d).\n", stat);
+			break;
+		}
+		QLAFX00_CLR_INTR_REG(ha, clr_intr);
+		QLAFX00_RD_INTR_REG(ha);
+	}
+	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+
+	if (test_bit(MBX_INTR_WAIT, &ha->mbx_cmd_flags) &&
+	    (status & MBX_INTERRUPT) && ha->flags.mbox_int) {
+		set_bit(MBX_INTERRUPT, &ha->mbx_cmd_flags);
+		complete(&ha->mbx_intr_comp);
+	}
+	return IRQ_HANDLED;
+}
+
+/** QLAFX00 specific IOCB implementation functions */
+
+static inline cont_a64_entry_t *
+qlafx00_prep_cont_type1_iocb(struct req_que *req,
+			     cont_a64_entry_t *lcont_pkt)
+{
+	cont_a64_entry_t *cont_pkt;
+
+	/* Adjust ring index. */
+	req->ring_index++;
+	if (req->ring_index == req->length) {
+		req->ring_index = 0;
+		req->ring_ptr = req->ring;
+	} else {
+		req->ring_ptr++;
+	}
+
+	cont_pkt = (cont_a64_entry_t *)req->ring_ptr;
+
+	/* Load packet defaults. */
+	*((uint32_t *)(&lcont_pkt->entry_type)) =
+	    __constant_cpu_to_le32(CONTINUE_A64_TYPE_FX00);
+
+	return cont_pkt;
+}
+
+static inline void
+qlafx00_build_scsi_iocbs(srb_t *sp, struct cmd_type_7_fx00 *cmd_pkt,
+			 uint16_t tot_dsds, struct cmd_type_7_fx00 *lcmd_pkt)
+{
+	uint16_t	avail_dsds;
+	uint32_t	*cur_dsd;
+	scsi_qla_host_t	*vha;
+	struct scsi_cmnd *cmd;
+	struct scatterlist *sg;
+	int i, cont;
+	struct req_que *req;
+	cont_a64_entry_t lcont_pkt;
+	cont_a64_entry_t *cont_pkt;
+
+	vha = sp->fcport->vha;
+	req = vha->req;
+
+	cmd = GET_CMD_SP(sp);
+	cont = 0;
+	cont_pkt = NULL;
+
+	/* Update entry type to indicate Command Type 3 IOCB */
+	*((uint32_t *)(&lcmd_pkt->entry_type)) =
+	    __constant_cpu_to_le32(FX00_COMMAND_TYPE_7);
+
+	/* No data transfer */
+	if (!scsi_bufflen(cmd) || cmd->sc_data_direction == DMA_NONE) {
+		lcmd_pkt->byte_count = __constant_cpu_to_le32(0);
+		return;
+	}
+
+	/* Set transfer direction */
+	if (cmd->sc_data_direction == DMA_TO_DEVICE) {
+		lcmd_pkt->cntrl_flags =
+		    __constant_cpu_to_le16(TMF_WRITE_DATA);
+		vha->qla_stats.output_bytes += scsi_bufflen(cmd);
+	} else if (cmd->sc_data_direction == DMA_FROM_DEVICE) {
+		lcmd_pkt->cntrl_flags =
+		    __constant_cpu_to_le16(TMF_READ_DATA);
+		vha->qla_stats.input_bytes += scsi_bufflen(cmd);
+	}
+
+	/* One DSD is available in the Command Type 3 IOCB */
+	avail_dsds = 1;
+	cur_dsd = (uint32_t *)&lcmd_pkt->dseg_0_address;
+
+	/* Load data segments */
+	scsi_for_each_sg(cmd, sg, tot_dsds, i) {
+		dma_addr_t	sle_dma;
+
+		/* Allocate additional continuation packets? */
+		if (avail_dsds == 0) {
+			/*
+			 * Five DSDs are available in the Continuation
+			 * Type 1 IOCB.
+			 */
+			memset(&lcont_pkt, 0, REQUEST_ENTRY_SIZE);
+			cont_pkt =
+			    qlafx00_prep_cont_type1_iocb(req, &lcont_pkt);
+			cur_dsd = (uint32_t *)lcont_pkt.dseg_0_address;
+			avail_dsds = 5;
+			cont = 1;
+		}
+
+		sle_dma = sg_dma_address(sg);
+		*cur_dsd++ = cpu_to_le32(LSD(sle_dma));
+		*cur_dsd++ = cpu_to_le32(MSD(sle_dma));
+		*cur_dsd++ = cpu_to_le32(sg_dma_len(sg));
+		avail_dsds--;
+		if (avail_dsds == 0 && cont == 1) {
+			cont = 0;
+			memcpy_toio((void __iomem *)cont_pkt, &lcont_pkt,
+			    REQUEST_ENTRY_SIZE);
+		}
+
+	}
+	if (avail_dsds != 0 && cont == 1) {
+		memcpy_toio((void __iomem *)cont_pkt, &lcont_pkt,
+		    REQUEST_ENTRY_SIZE);
+	}
+}
+
+/**
+ * qlafx00_start_scsi() - Send a SCSI command to the ISP
+ * @sp: command to send to the ISP
+ *
+ * Returns non-zero if a failure occurred, else zero.
+ */
+int
+qlafx00_start_scsi(srb_t *sp)
+{
+	int		ret, nseg;
+	unsigned long   flags;
+	uint32_t        index;
+	uint32_t	handle;
+	uint16_t	cnt;
+	uint16_t	req_cnt;
+	uint16_t	tot_dsds;
+	struct req_que *req = NULL;
+	struct rsp_que *rsp = NULL;
+	struct scsi_cmnd *cmd = GET_CMD_SP(sp);
+	struct scsi_qla_host *vha = sp->fcport->vha;
+	struct qla_hw_data *ha = vha->hw;
+	struct cmd_type_7_fx00 *cmd_pkt;
+	struct cmd_type_7_fx00 lcmd_pkt;
+	struct scsi_lun llun;
+	char		tag[2];
+
+	/* Setup device pointers. */
+	ret = 0;
+
+	rsp = ha->rsp_q_map[0];
+	req = vha->req;
+
+	/* So we know we haven't pci_map'ed anything yet */
+	tot_dsds = 0;
+
+	/* Forcing marker needed for now */
+	vha->marker_needed = 0;
+
+	/* Send marker if required */
+	if (vha->marker_needed != 0) {
+		if (qla2x00_marker(vha, req, rsp, 0, 0, MK_SYNC_ALL) !=
+		    QLA_SUCCESS)
+			return QLA_FUNCTION_FAILED;
+		vha->marker_needed = 0;
+	}
+
+	/* Acquire ring specific lock */
+	spin_lock_irqsave(&ha->hardware_lock, flags);
+
+	/* Check for room in outstanding command list. */
+	handle = req->current_outstanding_cmd;
+	for (index = 1; index < req->num_outstanding_cmds; index++) {
+		handle++;
+		if (handle == req->num_outstanding_cmds)
+			handle = 1;
+		if (!req->outstanding_cmds[handle])
+			break;
+	}
+	if (index == req->num_outstanding_cmds)
+		goto queuing_error;
+
+	/* Map the sg table so we have an accurate count of sg entries needed */
+	if (scsi_sg_count(cmd)) {
+		nseg = dma_map_sg(&ha->pdev->dev, scsi_sglist(cmd),
+		    scsi_sg_count(cmd), cmd->sc_data_direction);
+		if (unlikely(!nseg))
+			goto queuing_error;
+	} else
+		nseg = 0;
+
+	tot_dsds = nseg;
+	req_cnt = qla24xx_calc_iocbs(vha, tot_dsds);
+	if (req->cnt < (req_cnt + 2)) {
+		cnt = RD_REG_DWORD_RELAXED(req->req_q_out);
+
+		if (req->ring_index < cnt)
+			req->cnt = cnt - req->ring_index;
+		else
+			req->cnt = req->length -
+				(req->ring_index - cnt);
+		if (req->cnt < (req_cnt + 2))
+			goto queuing_error;
+	}
+
+	/* Build command packet. */
+	req->current_outstanding_cmd = handle;
+	req->outstanding_cmds[handle] = sp;
+	sp->handle = handle;
+	cmd->host_scribble = (unsigned char *)(unsigned long)handle;
+	req->cnt -= req_cnt;
+
+	cmd_pkt = (struct cmd_type_7_fx00 *)req->ring_ptr;
+
+	memset(&lcmd_pkt, 0, REQUEST_ENTRY_SIZE);
+
+	lcmd_pkt.handle = MAKE_HANDLE(req->id, sp->handle);
+	lcmd_pkt.handle_hi = 0;
+	lcmd_pkt.dseg_count = cpu_to_le16(tot_dsds);
+	lcmd_pkt.tgt_idx = cpu_to_le16(sp->fcport->tgt_id);
+
+	int_to_scsilun(cmd->device->lun, &llun);
+	host_to_adap((uint8_t *)&llun, (uint8_t *)&lcmd_pkt.lun,
+	    sizeof(lcmd_pkt.lun));
+
+	/* Update tagged queuing modifier -- default is TSK_SIMPLE (0). */
+	if (scsi_populate_tag_msg(cmd, tag)) {
+		switch (tag[0]) {
+		case HEAD_OF_QUEUE_TAG:
+			lcmd_pkt.task = TSK_HEAD_OF_QUEUE;
+			break;
+		case ORDERED_QUEUE_TAG:
+			lcmd_pkt.task = TSK_ORDERED;
+			break;
+		}
+	}
+
+	/* Load SCSI command packet. */
+	host_to_adap(cmd->cmnd, lcmd_pkt.fcp_cdb, sizeof(lcmd_pkt.fcp_cdb));
+	lcmd_pkt.byte_count = cpu_to_le32((uint32_t)scsi_bufflen(cmd));
+
+	/* Build IOCB segments */
+	qlafx00_build_scsi_iocbs(sp, cmd_pkt, tot_dsds, &lcmd_pkt);
+
+	/* Set total data segment count. */
+	lcmd_pkt.entry_count = (uint8_t)req_cnt;
+
+	/* Specify response queue number where completion should happen */
+	lcmd_pkt.entry_status = (uint8_t) rsp->id;
+
+	ql_dump_buffer(ql_dbg_io + ql_dbg_buffer, vha, 0x302e,
+	    (uint8_t *)cmd->cmnd, cmd->cmd_len);
+	ql_dump_buffer(ql_dbg_io + ql_dbg_buffer, vha, 0x3032,
+	    (uint8_t *)&lcmd_pkt, REQUEST_ENTRY_SIZE);
+
+	memcpy_toio((void __iomem *)cmd_pkt, &lcmd_pkt, REQUEST_ENTRY_SIZE);
+	wmb();
+
+	/* Adjust ring index. */
+	req->ring_index++;
+	if (req->ring_index == req->length) {
+		req->ring_index = 0;
+		req->ring_ptr = req->ring;
+	} else
+		req->ring_ptr++;
+
+	sp->flags |= SRB_DMA_VALID;
+
+	/* Set chip new ring index. */
+	WRT_REG_DWORD(req->req_q_in, req->ring_index);
+	QLAFX00_SET_HST_INTR(ha, ha->rqstq_intr_code);
+
+	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+	return QLA_SUCCESS;
+
+queuing_error:
+	if (tot_dsds)
+		scsi_dma_unmap(cmd);
+
+	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+
+	return QLA_FUNCTION_FAILED;
+}
+
+void
+qlafx00_tm_iocb(srb_t *sp, struct tsk_mgmt_entry_fx00 *ptm_iocb)
+{
+	struct srb_iocb *fxio = &sp->u.iocb_cmd;
+	scsi_qla_host_t *vha = sp->fcport->vha;
+	struct req_que *req = vha->req;
+	struct tsk_mgmt_entry_fx00 tm_iocb;
+	struct scsi_lun llun;
+
+	memset(&tm_iocb, 0, sizeof(struct tsk_mgmt_entry_fx00));
+	tm_iocb.entry_type = TSK_MGMT_IOCB_TYPE_FX00;
+	tm_iocb.entry_count = 1;
+	tm_iocb.handle = cpu_to_le32(MAKE_HANDLE(req->id, sp->handle));
+	tm_iocb.handle_hi = 0;
+	tm_iocb.timeout = cpu_to_le16(qla2x00_get_async_timeout(vha) + 2);
+	tm_iocb.tgt_id = cpu_to_le16(sp->fcport->tgt_id);
+	tm_iocb.control_flags = cpu_to_le32(fxio->u.tmf.flags);
+	if (tm_iocb.control_flags == TCF_LUN_RESET) {
+		int_to_scsilun(fxio->u.tmf.lun, &llun);
+		host_to_adap((uint8_t *)&llun, (uint8_t *)&tm_iocb.lun,
+		    sizeof(struct scsi_lun));
+	}
+
+	memcpy((void __iomem *)ptm_iocb, &tm_iocb,
+	    sizeof(struct tsk_mgmt_entry_fx00));
+	wmb();
+}
+
+void
+qlafx00_abort_iocb(srb_t *sp, struct abort_iocb_entry_fx00 *pabt_iocb)
+{
+	struct srb_iocb *fxio = &sp->u.iocb_cmd;
+	scsi_qla_host_t *vha = sp->fcport->vha;
+	struct req_que *req = vha->req;
+	struct abort_iocb_entry_fx00 abt_iocb;
+
+	memset(&abt_iocb, 0, sizeof(struct abort_iocb_entry_fx00));
+	abt_iocb.entry_type = ABORT_IOCB_TYPE_FX00;
+	abt_iocb.entry_count = 1;
+	abt_iocb.handle = cpu_to_le32(MAKE_HANDLE(req->id, sp->handle));
+	abt_iocb.abort_handle =
+	    cpu_to_le32(MAKE_HANDLE(req->id, fxio->u.abt.cmd_hndl));
+	abt_iocb.tgt_id_sts = cpu_to_le16(sp->fcport->tgt_id);
+	abt_iocb.req_que_no = cpu_to_le16(req->id);
+
+	memcpy((void __iomem *)pabt_iocb, &abt_iocb,
+	    sizeof(struct abort_iocb_entry_fx00));
+	wmb();
+}
+
+void
+qlafx00_fxdisc_iocb(srb_t *sp, struct fxdisc_entry_fx00 *pfxiocb)
+{
+	struct srb_iocb *fxio = &sp->u.iocb_cmd;
+	struct qla_mt_iocb_rqst_fx00 *piocb_rqst;
+	struct fc_bsg_job *bsg_job;
+	struct fxdisc_entry_fx00 fx_iocb;
+	uint8_t entry_cnt = 1;
+
+	memset(&fx_iocb, 0, sizeof(struct fxdisc_entry_fx00));
+	fx_iocb.entry_type = FX00_IOCB_TYPE;
+	fx_iocb.handle = cpu_to_le32(sp->handle);
+	fx_iocb.entry_count = entry_cnt;
+
+	if (sp->type == SRB_FXIOCB_DCMD) {
+		fx_iocb.func_num =
+		    cpu_to_le16(sp->u.iocb_cmd.u.fxiocb.req_func_type);
+		fx_iocb.adapid = cpu_to_le32(fxio->u.fxiocb.adapter_id);
+		fx_iocb.adapid_hi = cpu_to_le32(fxio->u.fxiocb.adapter_id_hi);
+		fx_iocb.reserved_0 = cpu_to_le32(fxio->u.fxiocb.reserved_0);
+		fx_iocb.reserved_1 = cpu_to_le32(fxio->u.fxiocb.reserved_1);
+		fx_iocb.dataword_extra =
+		    cpu_to_le32(fxio->u.fxiocb.req_data_extra);
+
+		if (fxio->u.fxiocb.flags & SRB_FXDISC_REQ_DMA_VALID) {
+			fx_iocb.req_dsdcnt = cpu_to_le16(1);
+			fx_iocb.req_xfrcnt =
+			    cpu_to_le16(fxio->u.fxiocb.req_len);
+			fx_iocb.dseg_rq_address[0] =
+			    cpu_to_le32(LSD(fxio->u.fxiocb.req_dma_handle));
+			fx_iocb.dseg_rq_address[1] =
+			    cpu_to_le32(MSD(fxio->u.fxiocb.req_dma_handle));
+			fx_iocb.dseg_rq_len =
+			    cpu_to_le32(fxio->u.fxiocb.req_len);
+		}
+
+		if (fxio->u.fxiocb.flags & SRB_FXDISC_RESP_DMA_VALID) {
+			fx_iocb.rsp_dsdcnt = cpu_to_le16(1);
+			fx_iocb.rsp_xfrcnt =
+			    cpu_to_le16(fxio->u.fxiocb.rsp_len);
+			fx_iocb.dseg_rsp_address[0] =
+			    cpu_to_le32(LSD(fxio->u.fxiocb.rsp_dma_handle));
+			fx_iocb.dseg_rsp_address[1] =
+			    cpu_to_le32(MSD(fxio->u.fxiocb.rsp_dma_handle));
+			fx_iocb.dseg_rsp_len =
+			    cpu_to_le32(fxio->u.fxiocb.rsp_len);
+		}
+
+		if (fxio->u.fxiocb.flags & SRB_FXDISC_REQ_DWRD_VALID) {
+			fx_iocb.dataword =
+			    cpu_to_le32(fxio->u.fxiocb.req_data);
+		}
+		fx_iocb.flags = fxio->u.fxiocb.flags;
+	} else {
+		struct scatterlist *sg;
+		bsg_job = sp->u.bsg_job;
+		piocb_rqst = (struct qla_mt_iocb_rqst_fx00 *)
+			&bsg_job->request->rqst_data.h_vendor.vendor_cmd[1];
+
+		fx_iocb.func_num = piocb_rqst->func_type;
+		fx_iocb.adapid = piocb_rqst->adapid;
+		fx_iocb.adapid_hi = piocb_rqst->adapid_hi;
+		fx_iocb.reserved_0 = piocb_rqst->reserved_0;
+		fx_iocb.reserved_1 = piocb_rqst->reserved_1;
+		fx_iocb.dataword_extra = piocb_rqst->dataword_extra;
+		fx_iocb.dataword = piocb_rqst->dataword;
+		fx_iocb.req_xfrcnt = cpu_to_le16(piocb_rqst->req_len);
+		fx_iocb.rsp_xfrcnt = cpu_to_le16(piocb_rqst->rsp_len);
+
+		if (piocb_rqst->flags & SRB_FXDISC_REQ_DMA_VALID) {
+			int avail_dsds, tot_dsds;
+			cont_a64_entry_t lcont_pkt;
+			cont_a64_entry_t *cont_pkt = NULL;
+			uint32_t *cur_dsd;
+			int index = 0, cont = 0;
+
+			fx_iocb.req_dsdcnt =
+			    cpu_to_le16(bsg_job->request_payload.sg_cnt);
+			tot_dsds =
+			    cpu_to_le32(bsg_job->request_payload.sg_cnt);
+			cur_dsd = (uint32_t *)&fx_iocb.dseg_rq_address[0];
+			avail_dsds = 1;
+			for_each_sg(bsg_job->request_payload.sg_list, sg,
+			    tot_dsds, index) {
+				dma_addr_t sle_dma;
+
+				/* Allocate additional continuation packets? */
+				if (avail_dsds == 0) {
+					/*
+					 * Five DSDs are available in the Cont.
+					 * Type 1 IOCB.
+					 */
+					memset(&lcont_pkt, 0,
+					    REQUEST_ENTRY_SIZE);
+					cont_pkt =
+					    qlafx00_prep_cont_type1_iocb(
+						sp->fcport->vha->req,
+						&lcont_pkt);
+					cur_dsd = (uint32_t *)
+					    lcont_pkt.dseg_0_address;
+					avail_dsds = 5;
+					cont = 1;
+					entry_cnt++;
+				}
+
+				sle_dma = sg_dma_address(sg);
+				*cur_dsd++   = cpu_to_le32(LSD(sle_dma));
+				*cur_dsd++   = cpu_to_le32(MSD(sle_dma));
+				*cur_dsd++   = cpu_to_le32(sg_dma_len(sg));
+				avail_dsds--;
+
+				if (avail_dsds == 0 && cont == 1) {
+					cont = 0;
+					memcpy_toio(
+					    (void __iomem *)cont_pkt,
+					    &lcont_pkt, REQUEST_ENTRY_SIZE);
+					ql_dump_buffer(
+					    ql_dbg_user + ql_dbg_verbose,
+					    sp->fcport->vha, 0x3042,
+					    (uint8_t *)&lcont_pkt,
+					     REQUEST_ENTRY_SIZE);
+				}
+			}
+			if (avail_dsds != 0 && cont == 1) {
+				memcpy_toio((void __iomem *)cont_pkt,
+				    &lcont_pkt, REQUEST_ENTRY_SIZE);
+				ql_dump_buffer(ql_dbg_user + ql_dbg_verbose,
+				    sp->fcport->vha, 0x3043,
+				    (uint8_t *)&lcont_pkt, REQUEST_ENTRY_SIZE);
+			}
+		}
+
+		if (piocb_rqst->flags & SRB_FXDISC_RESP_DMA_VALID) {
+			int avail_dsds, tot_dsds;
+			cont_a64_entry_t lcont_pkt;
+			cont_a64_entry_t *cont_pkt = NULL;
+			uint32_t *cur_dsd;
+			int index = 0, cont = 0;
+
+			fx_iocb.rsp_dsdcnt =
+			   cpu_to_le16(bsg_job->reply_payload.sg_cnt);
+			tot_dsds = cpu_to_le32(bsg_job->reply_payload.sg_cnt);
+			cur_dsd = (uint32_t *)&fx_iocb.dseg_rsp_address[0];
+			avail_dsds = 1;
+
+			for_each_sg(bsg_job->reply_payload.sg_list, sg,
+			    tot_dsds, index) {
+				dma_addr_t sle_dma;
+
+				/* Allocate additional continuation packets? */
+				if (avail_dsds == 0) {
+					/*
+					* Five DSDs are available in the Cont.
+					* Type 1 IOCB.
+					*/
+					memset(&lcont_pkt, 0,
+					    REQUEST_ENTRY_SIZE);
+					cont_pkt =
+					    qlafx00_prep_cont_type1_iocb(
+						sp->fcport->vha->req,
+						&lcont_pkt);
+					cur_dsd = (uint32_t *)
+					    lcont_pkt.dseg_0_address;
+					avail_dsds = 5;
+					cont = 1;
+					entry_cnt++;
+				}
+
+				sle_dma = sg_dma_address(sg);
+				*cur_dsd++   = cpu_to_le32(LSD(sle_dma));
+				*cur_dsd++   = cpu_to_le32(MSD(sle_dma));
+				*cur_dsd++   = cpu_to_le32(sg_dma_len(sg));
+				avail_dsds--;
+
+				if (avail_dsds == 0 && cont == 1) {
+					cont = 0;
+					memcpy_toio((void __iomem *)cont_pkt,
+					    &lcont_pkt,
+					    REQUEST_ENTRY_SIZE);
+					ql_dump_buffer(
+					    ql_dbg_user + ql_dbg_verbose,
+					    sp->fcport->vha, 0x3045,
+					    (uint8_t *)&lcont_pkt,
+					    REQUEST_ENTRY_SIZE);
+				}
+			}
+			if (avail_dsds != 0 && cont == 1) {
+				memcpy_toio((void __iomem *)cont_pkt,
+				    &lcont_pkt, REQUEST_ENTRY_SIZE);
+				ql_dump_buffer(ql_dbg_user + ql_dbg_verbose,
+				    sp->fcport->vha, 0x3046,
+				    (uint8_t *)&lcont_pkt, REQUEST_ENTRY_SIZE);
+			}
+		}
+
+		if (piocb_rqst->flags & SRB_FXDISC_REQ_DWRD_VALID)
+			fx_iocb.dataword = cpu_to_le32(piocb_rqst->dataword);
+		fx_iocb.flags = piocb_rqst->flags;
+		fx_iocb.entry_count = entry_cnt;
+	}
+
+	ql_dump_buffer(ql_dbg_user + ql_dbg_verbose,
+	    sp->fcport->vha, 0x3047,
+	    (uint8_t *)&fx_iocb, sizeof(struct fxdisc_entry_fx00));
+
+	memcpy((void __iomem *)pfxiocb, &fx_iocb,
+	    sizeof(struct fxdisc_entry_fx00));
+	wmb();
+}
diff --git a/drivers/scsi/qla2xxx/qla_mr.h b/drivers/scsi/qla2xxx/qla_mr.h
new file mode 100644
index 00000000000..cc327dc2fd1
--- /dev/null
+++ b/drivers/scsi/qla2xxx/qla_mr.h
@@ -0,0 +1,510 @@
+/*
+ * QLogic Fibre Channel HBA Driver
+ * Copyright (c)  2003-2013 QLogic Corporation
+ *
+ * See LICENSE.qla2xxx for copyright and licensing details.
+ */
+#ifndef __QLA_MR_H
+#define __QLA_MR_H
+
+/*
+ * The PCI VendorID and DeviceID for our board.
+ */
+#define PCI_DEVICE_ID_QLOGIC_ISPF001		0xF001
+
+/* FX00 specific definitions */
+
+#define FX00_COMMAND_TYPE_7	0x07	/* Command Type 7 entry for 7XXX */
+struct cmd_type_7_fx00 {
+	uint8_t entry_type;		/* Entry type. */
+	uint8_t entry_count;		/* Entry count. */
+	uint8_t sys_define;		/* System defined. */
+	uint8_t entry_status;		/* Entry Status. */
+
+	uint32_t handle;		/* System handle. */
+	uint32_t handle_hi;
+
+	uint16_t tgt_idx;		/* Target Idx. */
+	uint16_t timeout;		/* Command timeout. */
+
+	uint16_t dseg_count;		/* Data segment count. */
+	uint16_t scsi_rsp_dsd_len;
+
+	struct scsi_lun lun;		/* LUN (LE). */
+
+	uint8_t cntrl_flags;
+
+	uint8_t task_mgmt_flags;	/* Task management flags. */
+
+	uint8_t task;
+
+	uint8_t crn;
+
+	uint8_t fcp_cdb[MAX_CMDSZ];	/* SCSI command words. */
+	uint32_t byte_count;		/* Total byte count. */
+
+	uint32_t dseg_0_address[2];	/* Data segment 0 address. */
+	uint32_t dseg_0_len;		/* Data segment 0 length. */
+};
+
+/*
+ * ISP queue - marker entry structure definition.
+ */
+struct mrk_entry_fx00 {
+	uint8_t entry_type;		/* Entry type. */
+	uint8_t entry_count;		/* Entry count. */
+	uint8_t handle_count;		/* Handle count. */
+	uint8_t entry_status;		/* Entry Status. */
+
+	uint32_t handle;		/* System handle. */
+	uint32_t handle_hi;		/* System handle. */
+
+	uint16_t tgt_id;		/* Target ID. */
+
+	uint8_t modifier;		/* Modifier (7-0). */
+	uint8_t reserved_1;
+
+	uint8_t reserved_2[5];
+
+	uint8_t lun[8];			/* FCP LUN (BE). */
+	uint8_t reserved_3[36];
+};
+
+
+#define	STATUS_TYPE_FX00	0x01		/* Status entry. */
+struct sts_entry_fx00 {
+	uint8_t entry_type;		/* Entry type. */
+	uint8_t entry_count;		/* Entry count. */
+	uint8_t sys_define;		/* System defined. */
+	uint8_t entry_status;		/* Entry Status. */
+
+	uint32_t handle;		/* System handle. */
+	uint32_t handle_hi;		/* System handle. */
+
+	uint16_t comp_status;		/* Completion status. */
+	uint16_t reserved_0;		/* OX_ID used by the firmware. */
+
+	uint32_t residual_len;		/* FW calc residual transfer length. */
+
+	uint16_t reserved_1;
+	uint16_t state_flags;		/* State flags. */
+
+	uint16_t reserved_2;
+	uint16_t scsi_status;		/* SCSI status. */
+
+	uint32_t sense_len;		/* FCP SENSE length. */
+	uint8_t data[32];		/* FCP response/sense information. */
+};
+
+
+#define MAX_HANDLE_COUNT	15
+#define MULTI_STATUS_TYPE_FX00	0x0D
+
+struct multi_sts_entry_fx00 {
+	uint8_t entry_type;		/* Entry type. */
+	uint8_t sys_define;		/* System defined. */
+	uint8_t handle_count;
+	uint8_t entry_status;
+
+	uint32_t handles[MAX_HANDLE_COUNT];
+};
+
+#define TSK_MGMT_IOCB_TYPE_FX00		0x05
+struct tsk_mgmt_entry_fx00 {
+	uint8_t entry_type;		/* Entry type. */
+	uint8_t entry_count;		/* Entry count. */
+	uint8_t sys_define;
+	uint8_t entry_status;		/* Entry Status. */
+
+	uint32_t handle;		/* System handle. */
+
+	uint32_t handle_hi;		/* System handle. */
+
+	uint16_t tgt_id;		/* Target Idx. */
+
+	uint16_t reserved_1;
+
+	uint16_t delay;			/* Activity delay in seconds. */
+
+	uint16_t timeout;		/* Command timeout. */
+
+	struct scsi_lun lun;		/* LUN (LE). */
+
+	uint32_t control_flags;		/* Control Flags. */
+
+	uint8_t reserved_2[32];
+};
+
+
+#define	ABORT_IOCB_TYPE_FX00	0x08		/* Abort IOCB status. */
+struct abort_iocb_entry_fx00 {
+	uint8_t entry_type;		/* Entry type. */
+	uint8_t entry_count;		/* Entry count. */
+	uint8_t sys_define;		/* System defined. */
+	uint8_t entry_status;		/* Entry Status. */
+
+	uint32_t handle;		/* System handle. */
+	uint32_t handle_hi;		/* System handle. */
+
+	uint16_t tgt_id_sts;		/* Completion status. */
+	uint16_t options;
+
+	uint32_t abort_handle;		/* System handle. */
+	uint32_t abort_handle_hi;	/* System handle. */
+
+	uint16_t req_que_no;
+	uint8_t reserved_1[38];
+};
+
+#define IOCTL_IOSB_TYPE_FX00	0x0C
+struct ioctl_iocb_entry_fx00 {
+	uint8_t entry_type;		/* Entry type. */
+	uint8_t entry_count;		/* Entry count. */
+	uint8_t sys_define;		/* System defined. */
+	uint8_t entry_status;		/* Entry Status. */
+
+	uint32_t handle;		/* System handle. */
+	uint32_t reserved_0;		/* System handle. */
+
+	uint16_t comp_func_num;
+	uint16_t fw_iotcl_flags;
+
+	uint32_t dataword_r;		/* Data word returned */
+	uint32_t adapid;		/* Adapter ID */
+	uint32_t adapid_hi;		/* Adapter ID high */
+	uint32_t reserved_1;
+
+	uint32_t seq_no;
+	uint8_t reserved_2[20];
+	uint32_t residuallen;
+	uint32_t status;
+};
+
+#define STATUS_CONT_TYPE_FX00 0x04
+
+#define FX00_IOCB_TYPE		0x0B
+struct fxdisc_entry_fx00 {
+	uint8_t entry_type;		/* Entry type. */
+	uint8_t entry_count;		/* Entry count. */
+	uint8_t sys_define;		/* System Defined. */
+	uint8_t entry_status;		/* Entry Status. */
+
+	uint32_t handle;		/* System handle. */
+	uint32_t reserved_0;		/* System handle. */
+
+	uint16_t func_num;
+	uint16_t req_xfrcnt;
+	uint16_t req_dsdcnt;
+	uint16_t rsp_xfrcnt;
+	uint16_t rsp_dsdcnt;
+	uint8_t flags;
+	uint8_t reserved_1;
+
+	uint32_t dseg_rq_address[2];	/* Data segment 0 address. */
+	uint32_t dseg_rq_len;		/* Data segment 0 length. */
+	uint32_t dseg_rsp_address[2];	/* Data segment 1 address. */
+	uint32_t dseg_rsp_len;		/* Data segment 1 length. */
+
+	uint32_t dataword;
+	uint32_t adapid;
+	uint32_t adapid_hi;
+	uint32_t dataword_extra;
+};
+
+struct qlafx00_tgt_node_info {
+	uint8_t tgt_node_wwpn[WWN_SIZE];
+	uint8_t tgt_node_wwnn[WWN_SIZE];
+	uint32_t tgt_node_state;
+	uint8_t reserved[128];
+	uint32_t reserved_1[8];
+	uint64_t reserved_2[4];
+} __packed;
+
+#define QLAFX00_TGT_NODE_INFO sizeof(struct qlafx00_tgt_node_info)
+
+#define QLAFX00_LINK_STATUS_DOWN	0x10
+#define QLAFX00_LINK_STATUS_UP		0x11
+
+#define QLAFX00_PORT_SPEED_2G	0x2
+#define QLAFX00_PORT_SPEED_4G	0x4
+#define QLAFX00_PORT_SPEED_8G	0x8
+#define QLAFX00_PORT_SPEED_10G	0xa
+struct port_info_data {
+	uint8_t         port_state;
+	uint8_t         port_type;
+	uint16_t        port_identifier;
+	uint32_t        up_port_state;
+	uint8_t         fw_ver_num[32];
+	uint8_t         portal_attrib;
+	uint16_t        host_option;
+	uint8_t         reset_delay;
+	uint8_t         pdwn_retry_cnt;
+	uint16_t        max_luns2tgt;
+	uint8_t         risc_ver;
+	uint8_t         pconn_option;
+	uint16_t        risc_option;
+	uint16_t        max_frame_len;
+	uint16_t        max_iocb_alloc;
+	uint16_t        exec_throttle;
+	uint8_t         retry_cnt;
+	uint8_t         retry_delay;
+	uint8_t         port_name[8];
+	uint8_t         port_id[3];
+	uint8_t         link_status;
+	uint8_t         plink_rate;
+	uint32_t        link_config;
+	uint16_t        adap_haddr;
+	uint8_t         tgt_disc;
+	uint8_t         log_tout;
+	uint8_t         node_name[8];
+	uint16_t        erisc_opt1;
+	uint8_t         resp_acc_tmr;
+	uint8_t         intr_del_tmr;
+	uint8_t         erisc_opt2;
+	uint8_t         alt_port_name[8];
+	uint8_t         alt_node_name[8];
+	uint8_t         link_down_tout;
+	uint8_t         conn_type;
+	uint8_t         fc_fw_mode;
+	uint32_t        uiReserved[48];
+} __packed;
+
+/* OS Type Designations */
+#define OS_TYPE_UNKNOWN             0
+#define OS_TYPE_LINUX               2
+
+/* Linux Info */
+#define SYSNAME_LENGTH              128
+#define NODENAME_LENGTH             64
+#define RELEASE_LENGTH              64
+#define VERSION_LENGTH              64
+#define MACHINE_LENGTH              64
+#define DOMNAME_LENGTH              64
+
+struct host_system_info {
+	uint32_t os_type;
+	char    sysname[SYSNAME_LENGTH];
+	char    nodename[NODENAME_LENGTH];
+	char    release[RELEASE_LENGTH];
+	char    version[VERSION_LENGTH];
+	char    machine[MACHINE_LENGTH];
+	char    domainname[DOMNAME_LENGTH];
+	char    hostdriver[VERSION_LENGTH];
+	uint32_t reserved[64];
+} __packed;
+
+struct register_host_info {
+	struct host_system_info     hsi;	/* host system info */
+	uint64_t        utc;			/* UTC (system time) */
+	uint32_t        reserved[64];		/* future additions */
+} __packed;
+
+
+#define QLAFX00_PORT_DATA_INFO (sizeof(struct port_info_data))
+#define QLAFX00_TGT_NODE_LIST_SIZE (sizeof(uint32_t) * 32)
+
+struct config_info_data {
+	uint8_t		product_name[256];
+	uint8_t		symbolic_name[64];
+	uint8_t		serial_num[32];
+	uint8_t		hw_version[16];
+	uint8_t		fw_version[16];
+	uint8_t		uboot_version[16];
+	uint8_t		fru_serial_num[32];
+
+	uint8_t		fc_port_count;
+	uint8_t		iscsi_port_count;
+	uint8_t		reserved1[2];
+
+	uint8_t		mode;
+	uint8_t		log_level;
+	uint8_t		reserved2[2];
+
+	uint32_t	log_size;
+
+	uint8_t		tgt_pres_mode;
+	uint8_t		iqn_flags;
+	uint8_t		lun_mapping;
+
+	uint64_t	adapter_id;
+
+	uint32_t	cluster_key_len;
+	uint8_t		cluster_key[10];
+
+	uint64_t	cluster_master_id;
+	uint64_t	cluster_slave_id;
+	uint8_t		cluster_flags;
+} __packed;
+
+#define FXDISC_GET_CONFIG_INFO		0x01
+#define FXDISC_GET_PORT_INFO		0x02
+#define FXDISC_GET_TGT_NODE_INFO	0x80
+#define FXDISC_GET_TGT_NODE_LIST	0x81
+#define FXDISC_REG_HOST_INFO		0x99
+
+#define QLAFX00_HBA_ICNTRL_REG		0x21B08
+#define QLAFX00_ICR_ENB_MASK            0x80000000
+#define QLAFX00_ICR_DIS_MASK            0x7fffffff
+#define QLAFX00_HST_RST_REG		0x18264
+#define QLAFX00_HST_TO_HBA_REG		0x20A04
+#define QLAFX00_HBA_TO_HOST_REG		0x21B70
+#define QLAFX00_HST_INT_STS_BITS	0x7
+#define QLAFX00_BAR1_BASE_ADDR_REG	0x40018
+#define QLAFX00_PEX0_WIN0_BASE_ADDR_REG	0x41824
+
+#define QLAFX00_INTR_MB_CMPLT		0x1
+#define QLAFX00_INTR_RSP_CMPLT		0x2
+#define QLAFX00_INTR_MB_RSP_CMPLT	0x3
+#define QLAFX00_INTR_ASYNC_CMPLT	0x4
+#define QLAFX00_INTR_MB_ASYNC_CMPLT	0x5
+#define QLAFX00_INTR_RSP_ASYNC_CMPLT	0x6
+#define QLAFX00_INTR_ALL_CMPLT		0x7
+
+#define QLAFX00_MBA_SYSTEM_ERR		0x8002
+#define QLAFX00_MBA_LINK_UP		0x8011
+#define QLAFX00_MBA_LINK_DOWN		0x8012
+#define QLAFX00_MBA_PORT_UPDATE		0x8014
+#define QLAFX00_MBA_SHUTDOWN_RQSTD	0x8062
+
+#define SOC_SW_RST_CONTROL_REG_CORE0     0x0020800
+#define SOC_FABRIC_RST_CONTROL_REG       0x0020840
+#define SOC_FABRIC_CONTROL_REG           0x0020200
+#define SOC_FABRIC_CONFIG_REG            0x0020204
+
+#define SOC_INTERRUPT_SOURCE_I_CONTROL_REG     0x0020B00
+#define SOC_CORE_TIMER_REG                     0x0021850
+#define SOC_IRQ_ACK_REG                        0x00218b4
+
+#define CONTINUE_A64_TYPE_FX00	0x03	/* Continuation entry. */
+
+#define QLAFX00_SET_HST_INTR(ha, value) \
+	WRT_REG_DWORD((ha)->cregbase + QLAFX00_HST_TO_HBA_REG, \
+	value)
+
+#define QLAFX00_CLR_HST_INTR(ha, value) \
+	WRT_REG_DWORD((ha)->cregbase + QLAFX00_HBA_TO_HOST_REG, \
+	~value)
+
+#define QLAFX00_RD_INTR_REG(ha) \
+	RD_REG_DWORD((ha)->cregbase + QLAFX00_HBA_TO_HOST_REG)
+
+#define QLAFX00_CLR_INTR_REG(ha, value) \
+	WRT_REG_DWORD((ha)->cregbase + QLAFX00_HBA_TO_HOST_REG, \
+	~value)
+
+#define QLAFX00_SET_HBA_SOC_REG(ha, off, val)\
+	WRT_REG_DWORD((ha)->cregbase + off, val)
+
+#define QLAFX00_GET_HBA_SOC_REG(ha, off)\
+	RD_REG_DWORD((ha)->cregbase + off)
+
+#define QLAFX00_HBA_RST_REG(ha, val)\
+	WRT_REG_DWORD((ha)->cregbase + QLAFX00_HST_RST_REG, val)
+
+#define QLAFX00_RD_ICNTRL_REG(ha) \
+	RD_REG_DWORD((ha)->cregbase + QLAFX00_HBA_ICNTRL_REG)
+
+#define QLAFX00_ENABLE_ICNTRL_REG(ha) \
+	WRT_REG_DWORD((ha)->cregbase + QLAFX00_HBA_ICNTRL_REG, \
+	(QLAFX00_GET_HBA_SOC_REG(ha, QLAFX00_HBA_ICNTRL_REG) | \
+	 QLAFX00_ICR_ENB_MASK))
+
+#define QLAFX00_DISABLE_ICNTRL_REG(ha) \
+	WRT_REG_DWORD((ha)->cregbase + QLAFX00_HBA_ICNTRL_REG, \
+	(QLAFX00_GET_HBA_SOC_REG(ha, QLAFX00_HBA_ICNTRL_REG) & \
+	 QLAFX00_ICR_DIS_MASK))
+
+#define QLAFX00_RD_REG(ha, off) \
+	RD_REG_DWORD((ha)->cregbase + off)
+
+#define QLAFX00_WR_REG(ha, off, val) \
+	WRT_REG_DWORD((ha)->cregbase + off, val)
+
+struct qla_mt_iocb_rqst_fx00 {
+	uint32_t reserved_0;
+
+	uint16_t func_type;
+	uint8_t flags;
+	uint8_t reserved_1;
+
+	uint32_t dataword;
+
+	uint32_t adapid;
+	uint32_t adapid_hi;
+
+	uint32_t dataword_extra;
+
+	uint32_t req_len;
+
+	uint32_t rsp_len;
+};
+
+struct qla_mt_iocb_rsp_fx00 {
+	uint32_t reserved_1;
+
+	uint16_t func_type;
+	uint16_t ioctl_flags;
+
+	uint32_t ioctl_data;
+
+	uint32_t adapid;
+	uint32_t adapid_hi;
+
+	uint32_t reserved_2;
+	uint32_t seq_number;
+
+	uint8_t reserved_3[20];
+
+	int32_t res_count;
+
+	uint32_t status;
+};
+
+
+#define MAILBOX_REGISTER_COUNT_FX00	16
+#define AEN_MAILBOX_REGISTER_COUNT_FX00	8
+#define MAX_FIBRE_DEVICES_FX00	512
+#define MAX_LUNS_FX00		0x1024
+#define MAX_TARGETS_FX00	MAX_ISA_DEVICES
+#define REQUEST_ENTRY_CNT_FX00		512	/* Number of request entries. */
+#define RESPONSE_ENTRY_CNT_FX00		256	/* Number of response entries.*/
+
+/*
+ * Firmware state codes for QLAFX00 adapters
+ */
+#define FSTATE_FX00_CONFIG_WAIT     0x0000	/* Waiting for driver to issue
+						 * Initialize FW Mbox cmd
+						 */
+#define FSTATE_FX00_INITIALIZED     0x1000	/* FW has been initialized by
+						 * the driver
+						 */
+
+#define FX00_DEF_RATOV	10
+
+struct mr_data_fx00 {
+	uint8_t	product_name[256];
+	uint8_t	symbolic_name[64];
+	uint8_t	serial_num[32];
+	uint8_t	hw_version[16];
+	uint8_t	fw_version[16];
+	uint8_t	uboot_version[16];
+	uint8_t	fru_serial_num[32];
+	fc_port_t       fcport;		/* fcport used for requests
+					 * that are not linked
+					 * to a particular target
+					 */
+	uint8_t fw_hbt_en;
+	uint8_t fw_hbt_cnt;
+	uint8_t fw_hbt_miss_cnt;
+	uint32_t old_fw_hbt_cnt;
+	uint16_t fw_reset_timer_tick;
+	uint8_t fw_reset_timer_exp;
+	uint32_t old_aenmbx0_state;
+};
+
+#define QLAFX00_LOOP_DOWN_TIME		615     /* 600 */
+#define QLAFX00_HEARTBEAT_INTERVAL	6	/* number of seconds */
+#define QLAFX00_HEARTBEAT_MISS_CNT	3	/* number of miss */
+#define QLAFX00_RESET_INTERVAL		120	/* number of seconds */
+#define QLAFX00_MAX_RESET_INTERVAL	600	/* number of seconds */
+#endif
diff --git a/drivers/scsi/qla2xxx/qla_os.c b/drivers/scsi/qla2xxx/qla_os.c
index 2c6dd3dfe0f..a083715843b 100644
--- a/drivers/scsi/qla2xxx/qla_os.c
+++ b/drivers/scsi/qla2xxx/qla_os.c
@@ -47,6 +47,7 @@ MODULE_PARM_DESC(ql2xenableclass2,
 		"Specify if Class 2 operations are supported from the very "
 		"beginning. Default is 0 - class 2 not supported.");
 
+
 int ql2xlogintimeout = 20;
 module_param(ql2xlogintimeout, int, S_IRUGO);
 MODULE_PARM_DESC(ql2xlogintimeout,
@@ -354,7 +355,12 @@ fail_req_map:
 
 static void qla2x00_free_req_que(struct qla_hw_data *ha, struct req_que *req)
 {
-	if (req && req->ring)
+	if (IS_QLAFX00(ha)) {
+		if (req && req->ring_fx00)
+			dma_free_coherent(&ha->pdev->dev,
+			    (req->length_fx00 + 1) * sizeof(request_t),
+			    req->ring_fx00, req->dma_fx00);
+	} else if (req && req->ring)
 		dma_free_coherent(&ha->pdev->dev,
 		(req->length + 1) * sizeof(request_t),
 		req->ring, req->dma);
@@ -368,11 +374,16 @@ static void qla2x00_free_req_que(struct qla_hw_data *ha, struct req_que *req)
 
 static void qla2x00_free_rsp_que(struct qla_hw_data *ha, struct rsp_que *rsp)
 {
-	if (rsp && rsp->ring)
+	if (IS_QLAFX00(ha)) {
+		if (rsp && rsp->ring)
+			dma_free_coherent(&ha->pdev->dev,
+			    (rsp->length_fx00 + 1) * sizeof(request_t),
+			    rsp->ring_fx00, rsp->dma_fx00);
+	} else if (rsp && rsp->ring) {
 		dma_free_coherent(&ha->pdev->dev,
 		(rsp->length + 1) * sizeof(response_t),
 		rsp->ring, rsp->dma);
-
+	}
 	kfree(rsp);
 	rsp = NULL;
 }
@@ -633,7 +644,7 @@ qla2x00_sp_free_dma(void *vha, void *ptr)
 	qla2x00_rel_sp(sp->fcport->vha, sp);
 }
 
-static void
+void
 qla2x00_sp_compl(void *data, void *ptr, int res)
 {
 	struct qla_hw_data *ha = (struct qla_hw_data *)data;
@@ -657,6 +668,9 @@ qla2x00_sp_compl(void *data, void *ptr, int res)
 	cmd->scsi_done(cmd);
 }
 
+/* If we are SP1 here, we need to still take and release the host_lock as SP1
+ * does not have the changes necessary to avoid taking host->host_lock.
+ */
 static int
 qla2xxx_queuecommand(struct Scsi_Host *host, struct scsi_cmnd *cmd)
 {
@@ -1304,6 +1318,9 @@ qla2x00_loop_reset(scsi_qla_host_t *vha)
 		}
 	}
 
+	if (IS_QLAFX00(ha))
+		return QLA_SUCCESS;
+
 	if (ha->flags.enable_lip_full_login && !IS_CNA_CAPABLE(ha)) {
 		atomic_set(&vha->loop_state, LOOP_DOWN);
 		atomic_set(&vha->loop_down_timer, LOOP_DOWN_TIME);
@@ -1858,6 +1875,7 @@ static struct isp_operations qla2100_isp_ops = {
 	.start_scsi		= qla2x00_start_scsi,
 	.abort_isp		= qla2x00_abort_isp,
 	.iospace_config     	= qla2x00_iospace_config,
+	.initialize_adapter	= qla2x00_initialize_adapter,
 };
 
 static struct isp_operations qla2300_isp_ops = {
@@ -1895,6 +1913,7 @@ static struct isp_operations qla2300_isp_ops = {
 	.start_scsi		= qla2x00_start_scsi,
 	.abort_isp		= qla2x00_abort_isp,
 	.iospace_config     	= qla2x00_iospace_config,
+	.initialize_adapter	= qla2x00_initialize_adapter,
 };
 
 static struct isp_operations qla24xx_isp_ops = {
@@ -1932,6 +1951,7 @@ static struct isp_operations qla24xx_isp_ops = {
 	.start_scsi		= qla24xx_start_scsi,
 	.abort_isp		= qla2x00_abort_isp,
 	.iospace_config     	= qla2x00_iospace_config,
+	.initialize_adapter	= qla2x00_initialize_adapter,
 };
 
 static struct isp_operations qla25xx_isp_ops = {
@@ -1969,6 +1989,7 @@ static struct isp_operations qla25xx_isp_ops = {
 	.start_scsi		= qla24xx_dif_start_scsi,
 	.abort_isp		= qla2x00_abort_isp,
 	.iospace_config     	= qla2x00_iospace_config,
+	.initialize_adapter	= qla2x00_initialize_adapter,
 };
 
 static struct isp_operations qla81xx_isp_ops = {
@@ -2006,6 +2027,7 @@ static struct isp_operations qla81xx_isp_ops = {
 	.start_scsi		= qla24xx_dif_start_scsi,
 	.abort_isp		= qla2x00_abort_isp,
 	.iospace_config     	= qla2x00_iospace_config,
+	.initialize_adapter	= qla2x00_initialize_adapter,
 };
 
 static struct isp_operations qla82xx_isp_ops = {
@@ -2043,6 +2065,7 @@ static struct isp_operations qla82xx_isp_ops = {
 	.start_scsi             = qla82xx_start_scsi,
 	.abort_isp		= qla82xx_abort_isp,
 	.iospace_config     	= qla82xx_iospace_config,
+	.initialize_adapter	= qla2x00_initialize_adapter,
 };
 
 static struct isp_operations qla83xx_isp_ops = {
@@ -2080,6 +2103,45 @@ static struct isp_operations qla83xx_isp_ops = {
 	.start_scsi		= qla24xx_dif_start_scsi,
 	.abort_isp		= qla2x00_abort_isp,
 	.iospace_config		= qla83xx_iospace_config,
+	.initialize_adapter	= qla2x00_initialize_adapter,
+};
+
+static struct isp_operations qlafx00_isp_ops = {
+	.pci_config		= qlafx00_pci_config,
+	.reset_chip		= qlafx00_soft_reset,
+	.chip_diag		= qlafx00_chip_diag,
+	.config_rings		= qlafx00_config_rings,
+	.reset_adapter		= qlafx00_soft_reset,
+	.nvram_config		= NULL,
+	.update_fw_options	= NULL,
+	.load_risc		= NULL,
+	.pci_info_str		= qlafx00_pci_info_str,
+	.fw_version_str		= qlafx00_fw_version_str,
+	.intr_handler		= qlafx00_intr_handler,
+	.enable_intrs		= qlafx00_enable_intrs,
+	.disable_intrs		= qlafx00_disable_intrs,
+	.abort_command		= qlafx00_abort_command,
+	.target_reset		= qlafx00_abort_target,
+	.lun_reset		= qlafx00_lun_reset,
+	.fabric_login		= NULL,
+	.fabric_logout		= NULL,
+	.calc_req_entries	= NULL,
+	.build_iocbs		= NULL,
+	.prep_ms_iocb		= qla24xx_prep_ms_iocb,
+	.prep_ms_fdmi_iocb	= qla24xx_prep_ms_fdmi_iocb,
+	.read_nvram		= qla24xx_read_nvram_data,
+	.write_nvram		= qla24xx_write_nvram_data,
+	.fw_dump		= NULL,
+	.beacon_on		= qla24xx_beacon_on,
+	.beacon_off		= qla24xx_beacon_off,
+	.beacon_blink		= NULL,
+	.read_optrom		= qla24xx_read_optrom_data,
+	.write_optrom		= qla24xx_write_optrom_data,
+	.get_flash_version	= qla24xx_get_flash_version,
+	.start_scsi		= qlafx00_start_scsi,
+	.abort_isp		= qlafx00_abort_isp,
+	.iospace_config		= qlafx00_iospace_config,
+	.initialize_adapter	= qlafx00_initialize_adapter,
 };
 
 static inline void
@@ -2192,6 +2254,9 @@ qla2x00_set_isp_flags(struct qla_hw_data *ha)
 		ha->device_type |= DT_T10_PI;
 		ha->fw_srisc_address = RISC_START_ADDRESS_2400;
 		break;
+	case PCI_DEVICE_ID_QLOGIC_ISPF001:
+		ha->device_type |= DT_ISPFX00;
+		break;
 	}
 
 	if (IS_QLA82XX(ha))
@@ -2265,7 +2330,8 @@ qla2x00_probe_one(struct pci_dev *pdev, const struct pci_device_id *id)
 	    pdev->device == PCI_DEVICE_ID_QLOGIC_ISP8001 ||
 	    pdev->device == PCI_DEVICE_ID_QLOGIC_ISP8021 ||
 	    pdev->device == PCI_DEVICE_ID_QLOGIC_ISP2031 ||
-	    pdev->device == PCI_DEVICE_ID_QLOGIC_ISP8031) {
+	    pdev->device == PCI_DEVICE_ID_QLOGIC_ISP8031 ||
+	    pdev->device == PCI_DEVICE_ID_QLOGIC_ISPF001) {
 		bars = pci_select_bars(pdev, IORESOURCE_MEM);
 		mem_only = 1;
 		ql_dbg_pci(ql_dbg_init, pdev, 0x0007,
@@ -2436,6 +2502,18 @@ qla2x00_probe_one(struct pci_dev *pdev, const struct pci_device_id *id)
 		ha->flash_data_off = FARX_ACCESS_FLASH_DATA_81XX;
 		ha->nvram_conf_off = ~0;
 		ha->nvram_data_off = ~0;
+	}  else if (IS_QLAFX00(ha)) {
+		ha->max_fibre_devices = MAX_FIBRE_DEVICES_FX00;
+		ha->mbx_count = MAILBOX_REGISTER_COUNT_FX00;
+		ha->aen_mbx_count = AEN_MAILBOX_REGISTER_COUNT_FX00;
+		req_length = REQUEST_ENTRY_CNT_FX00;
+		rsp_length = RESPONSE_ENTRY_CNT_FX00;
+		ha->init_cb_size = sizeof(struct init_cb_fx);
+		ha->isp_ops = &qlafx00_isp_ops;
+		ha->port_down_retry_count = 30; /* default value */
+		ha->mr.fw_hbt_cnt = QLAFX00_HEARTBEAT_INTERVAL;
+		ha->mr.fw_reset_timer_tick = QLAFX00_RESET_INTERVAL;
+		ha->mr.fw_hbt_en = 1;
 	}
 
 	ql_dbg_pci(ql_dbg_init, pdev, 0x001e,
@@ -2500,13 +2578,24 @@ qla2x00_probe_one(struct pci_dev *pdev, const struct pci_device_id *id)
 
 	host = base_vha->host;
 	base_vha->req = req;
-	host->can_queue = req->length + 128;
+	if (IS_QLAFX00(ha))
+		host->can_queue = 1024;
+	else
+		host->can_queue = req->length + 128;
 	if (IS_QLA2XXX_MIDTYPE(ha))
 		base_vha->mgmt_svr_loop_id = 10 + base_vha->vp_idx;
 	else
 		base_vha->mgmt_svr_loop_id = MANAGEMENT_SERVER +
 						base_vha->vp_idx;
 
+	/* Setup fcport template structure. */
+	ha->mr.fcport.vha = base_vha;
+	ha->mr.fcport.port_type = FCT_UNKNOWN;
+	ha->mr.fcport.loop_id = FC_NO_LOOP_ID;
+	qla2x00_set_fcport_state(&ha->mr.fcport, FCS_UNCONFIGURED);
+	ha->mr.fcport.supported_classes = FC_COS_UNSPECIFIED;
+	ha->mr.fcport.scan_state = 1;
+
 	/* Set the SG table size based on ISP type */
 	if (!IS_FWI2_CAPABLE(ha)) {
 		if (IS_QLA2100(ha))
@@ -2562,6 +2651,13 @@ que_init:
 	rsp->req = req;
 	req->rsp = rsp;
 
+	if (IS_QLAFX00(ha)) {
+		ha->rsp_q_map[0] = rsp;
+		ha->req_q_map[0] = req;
+		set_bit(0, ha->req_qid_map);
+		set_bit(0, ha->rsp_qid_map);
+	}
+
 	/* FWI2-capable only. */
 	req->req_q_in = &ha->iobase->isp24.req_q_in;
 	req->req_q_out = &ha->iobase->isp24.req_q_out;
@@ -2574,6 +2670,13 @@ que_init:
 		rsp->rsp_q_out =  &ha->mqiobase->isp25mq.rsp_q_out;
 	}
 
+	if (IS_QLAFX00(ha)) {
+		req->req_q_in = &ha->iobase->ispfx00.req_q_in;
+		req->req_q_out = &ha->iobase->ispfx00.req_q_out;
+		rsp->rsp_q_in = &ha->iobase->ispfx00.rsp_q_in;
+		rsp->rsp_q_out = &ha->iobase->ispfx00.rsp_q_out;
+	}
+
 	if (IS_QLA82XX(ha)) {
 		req->req_q_out = &ha->iobase->isp82.req_q_out[0];
 		rsp->rsp_q_in = &ha->iobase->isp82.rsp_q_in[0];
@@ -2595,7 +2698,7 @@ que_init:
 	    "req->req_q_in=%p req->req_q_out=%p rsp->rsp_q_in=%p rsp->rsp_q_out=%p.\n",
 	    req->req_q_in, req->req_q_out, rsp->rsp_q_in, rsp->rsp_q_out);
 
-	if (qla2x00_initialize_adapter(base_vha)) {
+	if (ha->isp_ops->initialize_adapter(base_vha)) {
 		ql_log(ql_log_fatal, base_vha, 0x00d6,
 		    "Failed to initialize adapter - Adapter flags %x.\n",
 		    base_vha->device_flags);
@@ -2720,6 +2823,18 @@ skip_dpc:
 
 	qla2x00_alloc_sysfs_attr(base_vha);
 
+	if (IS_QLAFX00(ha)) {
+		ret = qlafx00_fx_disc(base_vha,
+			&base_vha->hw->mr.fcport, FXDISC_GET_CONFIG_INFO);
+
+		ret = qlafx00_fx_disc(base_vha,
+			&base_vha->hw->mr.fcport, FXDISC_GET_PORT_INFO);
+
+		/* Register system information */
+		ret =  qlafx00_fx_disc(base_vha,
+			&base_vha->hw->mr.fcport, FXDISC_REG_HOST_INFO);
+	}
+
 	qla2x00_init_host_attr(base_vha);
 
 	qla2x00_dfs_setup(base_vha);
@@ -2777,6 +2892,8 @@ iospace_config_failed:
 	} else {
 		if (ha->iobase)
 			iounmap(ha->iobase);
+		if (ha->cregbase)
+			iounmap(ha->cregbase);
 	}
 	pci_release_selected_regions(ha->pdev, ha->bars);
 	kfree(ha);
@@ -2960,6 +3077,9 @@ qla2x00_remove_one(struct pci_dev *pdev)
 		if (ha->iobase)
 			iounmap(ha->iobase);
 
+		if (ha->cregbase)
+			iounmap(ha->cregbase);
+
 		if (ha->mqiobase)
 			iounmap(ha->mqiobase);
 
@@ -3068,6 +3188,12 @@ qla2x00_schedule_rport_del(struct scsi_qla_host *vha, fc_port_t *fcport,
 void qla2x00_mark_device_lost(scsi_qla_host_t *vha, fc_port_t *fcport,
     int do_login, int defer)
 {
+	if (IS_QLAFX00(vha->hw)) {
+		qla2x00_set_fcport_state(fcport, FCS_DEVICE_LOST);
+		qla2x00_schedule_rport_del(vha, fcport, defer);
+		return;
+	}
+
 	if (atomic_read(&fcport->state) == FCS_ONLINE &&
 	    vha->vp_idx == fcport->vha->vp_idx) {
 		qla2x00_set_fcport_state(fcport, FCS_DEVICE_LOST);
@@ -3710,6 +3836,22 @@ qla2x00_uevent_emit(struct scsi_qla_host *vha, u32 code)
 	kobject_uevent_env(&vha->hw->pdev->dev.kobj, KOBJ_CHANGE, envp);
 }
 
+int
+qlafx00_post_aenfx_work(struct scsi_qla_host *vha,  uint32_t evtcode,
+			uint32_t *data, int cnt)
+{
+	struct qla_work_evt *e;
+
+	e = qla2x00_alloc_work(vha, QLA_EVT_AENFX);
+	if (!e)
+		return QLA_FUNCTION_FAILED;
+
+	e->u.aenfx.evtcode = evtcode;
+	e->u.aenfx.count = cnt;
+	memcpy(e->u.aenfx.mbx, data, sizeof(*data) * cnt);
+	return qla2x00_post_work(vha, e);
+}
+
 void
 qla2x00_do_work(struct scsi_qla_host *vha)
 {
@@ -3758,6 +3900,9 @@ qla2x00_do_work(struct scsi_qla_host *vha)
 		case QLA_EVT_UEVENT:
 			qla2x00_uevent_emit(vha, e->u.uevent.code);
 			break;
+		case QLA_EVT_AENFX:
+			qlafx00_process_aen(vha, e);
+			break;
 		}
 		if (e->flags & QLA_EVT_FLAG_FREE)
 			kfree(e);
@@ -4592,6 +4737,38 @@ qla2x00_do_dpc(void *data)
 				ql_dbg(ql_dbg_dpc, base_vha, 0x4006,
 				    "FCoE context reset end.\n");
 			}
+		} else if (IS_QLAFX00(ha)) {
+			if (test_and_clear_bit(ISP_UNRECOVERABLE,
+				&base_vha->dpc_flags)) {
+				ql_dbg(ql_dbg_dpc, base_vha, 0x4020,
+				    "Firmware Reset Recovery\n");
+				if (qlafx00_reset_initialize(base_vha)) {
+					/* Failed. Abort isp later. */
+					if (!test_bit(UNLOADING,
+					    &base_vha->dpc_flags))
+						set_bit(ISP_UNRECOVERABLE,
+						    &base_vha->dpc_flags);
+						ql_dbg(ql_dbg_dpc, base_vha,
+						    0x4021,
+						    "Reset Recovery Failed\n");
+				}
+			}
+
+			if (test_and_clear_bit(FX00_TARGET_SCAN,
+				&base_vha->dpc_flags)) {
+				ql_dbg(ql_dbg_dpc, base_vha, 0x4022,
+				    "ISPFx00 Target Scan scheduled\n");
+				if (qlafx00_rescan_isp(base_vha)) {
+					if (!test_bit(UNLOADING,
+					    &base_vha->dpc_flags))
+						set_bit(ISP_UNRECOVERABLE,
+						    &base_vha->dpc_flags);
+					ql_dbg(ql_dbg_dpc, base_vha, 0x401e,
+					    "ISPFx00 Target Scan Failed\n");
+				}
+				ql_dbg(ql_dbg_dpc, base_vha, 0x401f,
+				    "ISPFx00 Target Scan End\n");
+			}
 		}
 
 		if (test_and_clear_bit(ISP_ABORT_NEEDED,
@@ -4630,6 +4807,9 @@ qla2x00_do_dpc(void *data)
 			clear_bit(SCR_PENDING, &base_vha->dpc_flags);
 		}
 
+		if (IS_QLAFX00(ha))
+			goto loop_resync_check;
+
 		if (test_bit(ISP_QUIESCE_NEEDED, &base_vha->dpc_flags)) {
 			ql_dbg(ql_dbg_dpc, base_vha, 0x4009,
 			    "Quiescence mode scheduled.\n");
@@ -4654,7 +4834,7 @@ qla2x00_do_dpc(void *data)
 		}
 
 		if (test_and_clear_bit(RESET_MARKER_NEEDED,
-							&base_vha->dpc_flags) &&
+				&base_vha->dpc_flags) &&
 		    (!(test_and_set_bit(RESET_ACTIVE, &base_vha->dpc_flags)))) {
 
 			ql_dbg(ql_dbg_dpc, base_vha, 0x400b,
@@ -4677,9 +4857,9 @@ qla2x00_do_dpc(void *data)
 			ql_dbg(ql_dbg_dpc, base_vha, 0x400e,
 			    "Relogin end.\n");
 		}
-
+loop_resync_check:
 		if (test_and_clear_bit(LOOP_RESYNC_NEEDED,
-							&base_vha->dpc_flags)) {
+		    &base_vha->dpc_flags)) {
 
 			ql_dbg(ql_dbg_dpc, base_vha, 0x400f,
 			    "Loop resync scheduled.\n");
@@ -4697,6 +4877,9 @@ qla2x00_do_dpc(void *data)
 			    "Loop resync end.\n");
 		}
 
+		if (IS_QLAFX00(ha))
+			goto intr_on_check;
+
 		if (test_bit(NPIV_CONFIG_NEEDED, &base_vha->dpc_flags) &&
 		    atomic_read(&base_vha->loop_state) == LOOP_READY) {
 			clear_bit(NPIV_CONFIG_NEEDED, &base_vha->dpc_flags);
@@ -4714,7 +4897,7 @@ qla2x00_do_dpc(void *data)
 		if (test_and_clear_bit(HOST_RAMP_UP_QUEUE_DEPTH,
 		    &base_vha->dpc_flags))
 			qla2x00_host_ramp_up_queuedepth(base_vha);
-
+intr_on_check:
 		if (!ha->interrupts_on)
 			ha->isp_ops->enable_intrs(ha);
 
@@ -4722,7 +4905,8 @@ qla2x00_do_dpc(void *data)
 					&base_vha->dpc_flags))
 			ha->isp_ops->beacon_blink(base_vha);
 
-		qla2x00_do_dpc_all_vps(base_vha);
+		if (!IS_QLAFX00(ha))
+			qla2x00_do_dpc_all_vps(base_vha);
 
 		ha->dpc_active = 0;
 end_loop:
@@ -4818,6 +5002,9 @@ qla2x00_timer(scsi_qla_host_t *vha)
 		qla82xx_watchdog(vha);
 	}
 
+	if (!vha->vp_idx && IS_QLAFX00(ha))
+		qlafx00_timer_routine(vha);
+
 	/* Loop down handler. */
 	if (atomic_read(&vha->loop_down_timer) > 0 &&
 	    !(test_bit(ABORT_ISP_ACTIVE, &vha->dpc_flags)) &&
@@ -5335,6 +5522,7 @@ static struct pci_device_id qla2xxx_pci_tbl[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_QLOGIC, PCI_DEVICE_ID_QLOGIC_ISP8001) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_QLOGIC, PCI_DEVICE_ID_QLOGIC_ISP8021) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_QLOGIC, PCI_DEVICE_ID_QLOGIC_ISP8031) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_QLOGIC, PCI_DEVICE_ID_QLOGIC_ISPF001) },
 	{ 0 },
 };
 MODULE_DEVICE_TABLE(pci, qla2xxx_pci_tbl);
diff --git a/drivers/scsi/qla2xxx/qla_target.c b/drivers/scsi/qla2xxx/qla_target.c
index 61b5d8c2b5d..fcdc22306ca 100644
--- a/drivers/scsi/qla2xxx/qla_target.c
+++ b/drivers/scsi/qla2xxx/qla_target.c
@@ -2585,25 +2585,6 @@ static void qlt_do_ctio_completion(struct scsi_qla_host *vha, uint32_t handle,
 	ha->tgt.tgt_ops->free_cmd(cmd);
 }
 
-/* ha->hardware_lock supposed to be held on entry */
-/* called via callback from qla2xxx */
-void qlt_ctio_completion(struct scsi_qla_host *vha, uint32_t handle)
-{
-	struct qla_hw_data *ha = vha->hw;
-	struct qla_tgt *tgt = ha->tgt.qla_tgt;
-
-	if (likely(tgt == NULL)) {
-		ql_dbg(ql_dbg_tgt, vha, 0xe021,
-		    "CTIO, but target mode not enabled"
-		    " (ha %d %p handle %#x)", vha->vp_idx, ha, handle);
-		return;
-	}
-
-	tgt->irq_cmd_count++;
-	qlt_do_ctio_completion(vha, handle, CTIO_SUCCESS, NULL);
-	tgt->irq_cmd_count--;
-}
-
 static inline int qlt_get_fcp_task_attr(struct scsi_qla_host *vha,
 	uint8_t task_codes)
 {
diff --git a/drivers/scsi/qla2xxx/qla_target.h b/drivers/scsi/qla2xxx/qla_target.h
index ff9ccb9fd03..b33e411f28a 100644
--- a/drivers/scsi/qla2xxx/qla_target.h
+++ b/drivers/scsi/qla2xxx/qla_target.h
@@ -980,7 +980,6 @@ extern int qlt_xmit_response(struct qla_tgt_cmd *, int, uint8_t);
 extern void qlt_xmit_tm_rsp(struct qla_tgt_mgmt_cmd *);
 extern void qlt_free_mcmd(struct qla_tgt_mgmt_cmd *);
 extern void qlt_free_cmd(struct qla_tgt_cmd *cmd);
-extern void qlt_ctio_completion(struct scsi_qla_host *, uint32_t);
 extern void qlt_async_event(uint16_t, struct scsi_qla_host *, uint16_t *);
 extern void qlt_enable_vha(struct scsi_qla_host *);
 extern void qlt_vport_create(struct scsi_qla_host *, struct qla_hw_data *);
diff --git a/drivers/scsi/qla2xxx/qla_version.h b/drivers/scsi/qla2xxx/qla_version.h
index 2b6e478d9e3..6c66d22eb1b 100644
--- a/drivers/scsi/qla2xxx/qla_version.h
+++ b/drivers/scsi/qla2xxx/qla_version.h
@@ -7,9 +7,9 @@
 /*
  * Driver version
  */
-#define QLA2XXX_VERSION      "8.04.00.08-k"
+#define QLA2XXX_VERSION      "8.05.00.03-k"
 
 #define QLA_DRIVER_MAJOR_VER	8
-#define QLA_DRIVER_MINOR_VER	4
+#define QLA_DRIVER_MINOR_VER	5
 #define QLA_DRIVER_PATCH_VER	0
 #define QLA_DRIVER_BETA_VER	0
diff --git a/drivers/scsi/qla4xxx/ql4_83xx.c b/drivers/scsi/qla4xxx/ql4_83xx.c
index 5d8fe4f7565..d607eb8e24c 100644
--- a/drivers/scsi/qla4xxx/ql4_83xx.c
+++ b/drivers/scsi/qla4xxx/ql4_83xx.c
@@ -1629,9 +1629,37 @@ static void __qla4_83xx_disable_pause(struct scsi_qla_host *ha)
 	ql4_printk(KERN_INFO, ha, "Disabled pause frames successfully.\n");
 }
 
+/**
+ * qla4_83xx_eport_init - Initialize EPort.
+ * @ha: Pointer to host adapter structure.
+ *
+ * If EPort hardware is in reset state before disabling pause, there would be
+ * serious hardware wedging issues. To prevent this perform eport init everytime
+ * before disabling pause frames.
+ **/
+static void qla4_83xx_eport_init(struct scsi_qla_host *ha)
+{
+	/* Clear the 8 registers */
+	qla4_83xx_wr_reg_indirect(ha, QLA83XX_RESET_REG, 0x0);
+	qla4_83xx_wr_reg_indirect(ha, QLA83XX_RESET_PORT0, 0x0);
+	qla4_83xx_wr_reg_indirect(ha, QLA83XX_RESET_PORT1, 0x0);
+	qla4_83xx_wr_reg_indirect(ha, QLA83XX_RESET_PORT2, 0x0);
+	qla4_83xx_wr_reg_indirect(ha, QLA83XX_RESET_PORT3, 0x0);
+	qla4_83xx_wr_reg_indirect(ha, QLA83XX_RESET_SRE_SHIM, 0x0);
+	qla4_83xx_wr_reg_indirect(ha, QLA83XX_RESET_EPG_SHIM, 0x0);
+	qla4_83xx_wr_reg_indirect(ha, QLA83XX_RESET_ETHER_PCS, 0x0);
+
+	/* Write any value to Reset Control register */
+	qla4_83xx_wr_reg_indirect(ha, QLA83XX_RESET_CONTROL, 0xFF);
+
+	ql4_printk(KERN_INFO, ha, "EPORT is out of reset.\n");
+}
+
 void qla4_83xx_disable_pause(struct scsi_qla_host *ha)
 {
 	ha->isp_ops->idc_lock(ha);
+	/* Before disabling pause frames, ensure that eport is not in reset */
+	qla4_83xx_eport_init(ha);
 	qla4_83xx_dump_pause_control_regs(ha);
 	__qla4_83xx_disable_pause(ha);
 	ha->isp_ops->idc_unlock(ha);
diff --git a/drivers/scsi/qla4xxx/ql4_83xx.h b/drivers/scsi/qla4xxx/ql4_83xx.h
index 6a00f903f2a..fab237fa32c 100644
--- a/drivers/scsi/qla4xxx/ql4_83xx.h
+++ b/drivers/scsi/qla4xxx/ql4_83xx.h
@@ -55,6 +55,16 @@
 #define QLA83XX_SET_PAUSE_VAL		0x0
 #define QLA83XX_SET_TC_MAX_CELL_VAL	0x03FF03FF
 
+#define QLA83XX_RESET_CONTROL		0x28084E50
+#define QLA83XX_RESET_REG		0x28084E60
+#define QLA83XX_RESET_PORT0		0x28084E70
+#define QLA83XX_RESET_PORT1		0x28084E80
+#define QLA83XX_RESET_PORT2		0x28084E90
+#define QLA83XX_RESET_PORT3		0x28084EA0
+#define QLA83XX_RESET_SRE_SHIM		0x28084EB0
+#define QLA83XX_RESET_EPG_SHIM		0x28084EC0
+#define QLA83XX_RESET_ETHER_PCS		0x28084ED0
+
 /* qla_83xx_reg_tbl registers */
 #define QLA83XX_PEG_HALT_STATUS1	0x34A8
 #define QLA83XX_PEG_HALT_STATUS2	0x34AC
diff --git a/drivers/scsi/qla4xxx/ql4_dbg.h b/drivers/scsi/qla4xxx/ql4_dbg.h
index 5b0afc18ef1..51c365bcf91 100644
--- a/drivers/scsi/qla4xxx/ql4_dbg.h
+++ b/drivers/scsi/qla4xxx/ql4_dbg.h
@@ -12,6 +12,7 @@
 /* #define QL_DEBUG_LEVEL_3  */		/* Output function tracing */
 /* #define QL_DEBUG_LEVEL_4  */
 /* #define QL_DEBUG_LEVEL_5  */
+/* #define QL_DEBUG_LEVEL_7  */
 /* #define QL_DEBUG_LEVEL_9  */
 
 #define QL_DEBUG_LEVEL_2	/* ALways enable error messagess */
@@ -48,6 +49,12 @@
 #define DEBUG5(x)	do {} while (0);
 #endif				/*  */
 
+#if defined(QL_DEBUG_LEVEL_7)
+#define DEBUG7(x)	do {x; } while (0)
+#else				/*  */
+#define DEBUG7(x)	do {} while (0)
+#endif				/*  */
+
 #if defined(QL_DEBUG_LEVEL_9)
 #define DEBUG9(x)	do {x;} while (0);
 #else				/*  */
diff --git a/drivers/scsi/qla4xxx/ql4_def.h b/drivers/scsi/qla4xxx/ql4_def.h
index 129f5dd0282..ddf16a86bbf 100644
--- a/drivers/scsi/qla4xxx/ql4_def.h
+++ b/drivers/scsi/qla4xxx/ql4_def.h
@@ -159,6 +159,22 @@
 #define LSDW(x) ((u32)((u64)(x)))
 #define MSDW(x) ((u32)((((u64)(x)) >> 16) >> 16))
 
+#define DEV_DB_NON_PERSISTENT	0
+#define DEV_DB_PERSISTENT	1
+
+#define COPY_ISID(dst_isid, src_isid) {			\
+	int i, j;					\
+	for (i = 0, j = ISID_SIZE - 1; i < ISID_SIZE;)	\
+		dst_isid[i++] = src_isid[j--];		\
+}
+
+#define SET_BITVAL(o, n, v) {	\
+	if (o)			\
+		n |= v;		\
+	else			\
+		n &= ~v;	\
+}
+
 /*
  * Retry & Timeout Values
  */
@@ -363,6 +379,8 @@ struct ql82xx_hw_data {
 	uint32_t flt_iscsi_param;
 	uint32_t flt_region_chap;
 	uint32_t flt_chap_size;
+	uint32_t flt_region_ddb;
+	uint32_t flt_ddb_size;
 };
 
 struct qla4_8xxx_legacy_intr_set {
@@ -501,6 +519,7 @@ struct scsi_qla_host {
 #define AF_INIT_DONE			1 /* 0x00000002 */
 #define AF_MBOX_COMMAND			2 /* 0x00000004 */
 #define AF_MBOX_COMMAND_DONE		3 /* 0x00000008 */
+#define AF_ST_DISCOVERY_IN_PROGRESS	4 /* 0x00000010 */
 #define AF_INTERRUPTS_ON		6 /* 0x00000040 */
 #define AF_GET_CRASH_RECORD		7 /* 0x00000080 */
 #define AF_LINK_UP			8 /* 0x00000100 */
diff --git a/drivers/scsi/qla4xxx/ql4_fw.h b/drivers/scsi/qla4xxx/ql4_fw.h
index ad9d2e2d370..c7b8892b5a8 100644
--- a/drivers/scsi/qla4xxx/ql4_fw.h
+++ b/drivers/scsi/qla4xxx/ql4_fw.h
@@ -288,6 +288,8 @@ union external_hw_config_reg {
 #define FA_GOLD_RISC_CODE_ADDR_82	0x80000
 #define FA_FLASH_ISCSI_CHAP		0x540000
 #define FA_FLASH_CHAP_SIZE		0xC0000
+#define FA_FLASH_ISCSI_DDB		0x420000
+#define FA_FLASH_DDB_SIZE		0x080000
 
 /* Flash Description Table */
 struct qla_fdt_layout {
@@ -348,6 +350,7 @@ struct qla_flt_header {
 #define FLT_REG_BOOT_CODE_82	0x78
 #define FLT_REG_ISCSI_PARAM	0x65
 #define FLT_REG_ISCSI_CHAP	0x63
+#define FLT_REG_ISCSI_DDB	0x6A
 
 struct qla_flt_region {
 	uint32_t code;
@@ -490,12 +493,16 @@ struct qla_flt_region {
 #define MBOX_ASTS_SUBNET_STATE_CHANGE		0x8027
 #define MBOX_ASTS_RESPONSE_QUEUE_FULL		0x8028
 #define MBOX_ASTS_IP_ADDR_STATE_CHANGED		0x8029
+#define MBOX_ASTS_IPV6_DEFAULT_ROUTER_CHANGED	0x802A
 #define MBOX_ASTS_IPV6_PREFIX_EXPIRED		0x802B
 #define MBOX_ASTS_IPV6_ND_PREFIX_IGNORED	0x802C
 #define MBOX_ASTS_IPV6_LCL_PREFIX_IGNORED	0x802D
 #define MBOX_ASTS_ICMPV6_ERROR_MSG_RCVD		0x802E
+#define MBOX_ASTS_INITIALIZATION_FAILED		0x8031
+#define MBOX_ASTS_SYSTEM_WARNING_EVENT		0x8036
 #define MBOX_ASTS_IDC_COMPLETE			0x8100
 #define MBOX_ASTS_IDC_REQUEST_NOTIFICATION	0x8101
+#define MBOX_ASTS_DCBX_CONF_CHANGE		0x8110
 #define MBOX_ASTS_TXSCVR_INSERTED		0x8130
 #define MBOX_ASTS_TXSCVR_REMOVED		0x8131
 
@@ -779,12 +786,41 @@ struct dev_db_entry {
 #define DDB_OPT_IPV6_NULL_LINK_LOCAL		0x800 /* post connection */
 #define DDB_OPT_IPV6_FW_DEFINED_LINK_LOCAL	0x800 /* pre connection */
 
+#define OPT_IS_FW_ASSIGNED_IPV6		11
+#define OPT_IPV6_DEVICE			8
+#define OPT_AUTO_SENDTGTS_DISABLE	6
+#define OPT_DISC_SESSION		4
+#define OPT_ENTRY_STATE			3
 	uint16_t exec_throttle;	/* 02-03 */
 	uint16_t exec_count;	/* 04-05 */
 	uint16_t res0;	/* 06-07 */
 	uint16_t iscsi_options;	/* 08-09 */
+#define ISCSIOPT_HEADER_DIGEST_EN		13
+#define ISCSIOPT_DATA_DIGEST_EN			12
+#define ISCSIOPT_IMMEDIATE_DATA_EN		11
+#define ISCSIOPT_INITIAL_R2T_EN			10
+#define ISCSIOPT_DATA_SEQ_IN_ORDER		9
+#define ISCSIOPT_DATA_PDU_IN_ORDER		8
+#define ISCSIOPT_CHAP_AUTH_EN			7
+#define ISCSIOPT_SNACK_REQ_EN			6
+#define ISCSIOPT_DISCOVERY_LOGOUT_EN		5
+#define ISCSIOPT_BIDI_CHAP_EN			4
+#define ISCSIOPT_DISCOVERY_AUTH_OPTIONAL	3
+#define ISCSIOPT_ERL1				1
+#define ISCSIOPT_ERL0				0
+
 	uint16_t tcp_options;	/* 0A-0B */
+#define TCPOPT_TIMESTAMP_STAT	6
+#define TCPOPT_NAGLE_DISABLE	5
+#define TCPOPT_WSF_DISABLE	4
+#define TCPOPT_TIMER_SCALE3	3
+#define TCPOPT_TIMER_SCALE2	2
+#define TCPOPT_TIMER_SCALE1	1
+#define TCPOPT_TIMESTAMP_EN	0
+
 	uint16_t ip_options;	/* 0C-0D */
+#define IPOPT_FRAGMENT_DISABLE	4
+
 	uint16_t iscsi_max_rcv_data_seg_len;	/* 0E-0F */
 #define BYTE_UNITS	512
 	uint32_t res1;	/* 10-13 */
@@ -816,6 +852,8 @@ struct dev_db_entry {
 					 * much RAM */
 	uint8_t link_local_ipv6_addr[0x10]; /* 1A0-1AF */
 	uint8_t res5[0x10];	/* 1B0-1BF */
+#define DDB_NO_LINK	0xFFFF
+#define DDB_ISNS	0xFFFD
 	uint16_t ddb_link;	/* 1C0-1C1 */
 	uint16_t chap_tbl_idx;	/* 1C2-1C3 */
 	uint16_t tgt_portal_grp; /* 1C4-1C5 */
diff --git a/drivers/scsi/qla4xxx/ql4_glbl.h b/drivers/scsi/qla4xxx/ql4_glbl.h
index 982293edf02..4a428009f69 100644
--- a/drivers/scsi/qla4xxx/ql4_glbl.h
+++ b/drivers/scsi/qla4xxx/ql4_glbl.h
@@ -191,6 +191,9 @@ int qla4xxx_ping_iocb(struct scsi_qla_host *ha, uint32_t options,
 int qla4xxx_post_ping_evt_work(struct scsi_qla_host *ha,
 			       uint32_t status, uint32_t pid,
 			       uint32_t data_size, uint8_t *data);
+int qla4xxx_flashdb_by_index(struct scsi_qla_host *ha,
+			     struct dev_db_entry *fw_ddb_entry,
+			     dma_addr_t fw_ddb_entry_dma, uint16_t ddb_index);
 
 /* BSG Functions */
 int qla4xxx_bsg_request(struct bsg_job *bsg_job);
@@ -224,8 +227,6 @@ void qla4_83xx_interrupt_service_routine(struct scsi_qla_host *ha,
 int qla4_83xx_isp_reset(struct scsi_qla_host *ha);
 void qla4_83xx_queue_iocb(struct scsi_qla_host *ha);
 void qla4_83xx_complete_iocb(struct scsi_qla_host *ha);
-uint16_t qla4_83xx_rd_shdw_req_q_out(struct scsi_qla_host *ha);
-uint16_t qla4_83xx_rd_shdw_rsp_q_in(struct scsi_qla_host *ha);
 uint32_t qla4_83xx_rd_reg(struct scsi_qla_host *ha, ulong addr);
 void qla4_83xx_wr_reg(struct scsi_qla_host *ha, ulong addr, uint32_t val);
 int qla4_83xx_rd_reg_indirect(struct scsi_qla_host *ha, uint32_t addr,
@@ -261,6 +262,10 @@ int qla4_83xx_post_idc_ack(struct scsi_qla_host *ha);
 void qla4_83xx_disable_pause(struct scsi_qla_host *ha);
 void qla4_83xx_enable_mbox_intrs(struct scsi_qla_host *ha);
 int qla4_83xx_can_perform_reset(struct scsi_qla_host *ha);
+int qla4xxx_get_default_ddb(struct scsi_qla_host *ha, uint32_t options,
+			    dma_addr_t dma_addr);
+int qla4xxx_get_uni_chap_at_index(struct scsi_qla_host *ha, char *username,
+				  char *password, uint16_t chap_index);
 
 extern int ql4xextended_error_logging;
 extern int ql4xdontresethba;
diff --git a/drivers/scsi/qla4xxx/ql4_isr.c b/drivers/scsi/qla4xxx/ql4_isr.c
index 1b83dc283d2..482287f4005 100644
--- a/drivers/scsi/qla4xxx/ql4_isr.c
+++ b/drivers/scsi/qla4xxx/ql4_isr.c
@@ -396,7 +396,6 @@ static void qla4xxx_passthru_status_entry(struct scsi_qla_host *ha,
 
 	task_data = task->dd_data;
 	memcpy(&task_data->sts, sts_entry, sizeof(struct passthru_status));
-	ha->req_q_count += task_data->iocb_req_cnt;
 	ha->iocb_cnt -= task_data->iocb_req_cnt;
 	queue_work(ha->task_wq, &task_data->task_work);
 }
@@ -416,7 +415,6 @@ static struct mrb *qla4xxx_del_mrb_from_active_array(struct scsi_qla_host *ha,
 		return mrb;
 
 	/* update counters */
-	ha->req_q_count += mrb->iocb_cnt;
 	ha->iocb_cnt -= mrb->iocb_cnt;
 
 	return mrb;
@@ -877,6 +875,43 @@ static void qla4xxx_isr_decode_mailbox(struct scsi_qla_host * ha,
 			}
 			break;
 
+		case MBOX_ASTS_IPV6_DEFAULT_ROUTER_CHANGED:
+			DEBUG2(ql4_printk(KERN_INFO, ha,
+					  "scsi%ld: AEN %04x, mbox_sts[1]=%08x, mbox_sts[2]=%08x, mbox_sts[3]=%08x, mbox_sts[4]=%08x mbox_sts[5]=%08x\n",
+					  ha->host_no, mbox_sts[0], mbox_sts[1],
+					  mbox_sts[2], mbox_sts[3], mbox_sts[4],
+					  mbox_sts[5]));
+			DEBUG2(ql4_printk(KERN_INFO, ha,
+					  "scsi%ld: AEN %04x Received IPv6 default router changed notification\n",
+					  ha->host_no, mbox_sts[0]));
+			break;
+
+		case MBOX_ASTS_INITIALIZATION_FAILED:
+			DEBUG2(ql4_printk(KERN_INFO, ha,
+					  "scsi%ld: AEN %04x, mbox_sts[3]=%08x\n",
+					  ha->host_no, mbox_sts[0],
+					  mbox_sts[3]));
+			break;
+
+		case MBOX_ASTS_SYSTEM_WARNING_EVENT:
+			DEBUG2(ql4_printk(KERN_WARNING, ha,
+					  "scsi%ld: AEN %04x, mbox_sts[1]=%08x, mbox_sts[2]=%08x, mbox_sts[3]=%08x, mbox_sts[4]=%08x mbox_sts[5]=%08x\n",
+					  ha->host_no, mbox_sts[0], mbox_sts[1],
+					  mbox_sts[2], mbox_sts[3], mbox_sts[4],
+					  mbox_sts[5]));
+			break;
+
+		case MBOX_ASTS_DCBX_CONF_CHANGE:
+			DEBUG2(ql4_printk(KERN_INFO, ha,
+					  "scsi%ld: AEN %04x, mbox_sts[1]=%08x, mbox_sts[2]=%08x, mbox_sts[3]=%08x, mbox_sts[4]=%08x mbox_sts[5]=%08x\n",
+					  ha->host_no, mbox_sts[0], mbox_sts[1],
+					  mbox_sts[2], mbox_sts[3], mbox_sts[4],
+					  mbox_sts[5]));
+			DEBUG2(ql4_printk(KERN_INFO, ha,
+					  "scsi%ld: AEN %04x Received DCBX configuration changed notification\n",
+					  ha->host_no, mbox_sts[0]));
+			break;
+
 		default:
 			DEBUG2(printk(KERN_WARNING
 				      "scsi%ld: AEN %04x UNKNOWN\n",
@@ -1099,8 +1134,8 @@ irqreturn_t qla4_82xx_intr_handler(int irq, void *dev_id)
 
 	status = qla4_82xx_rd_32(ha, ISR_INT_STATE_REG);
 	if (!ISR_IS_LEGACY_INTR_TRIGGERED(status)) {
-		DEBUG2(ql4_printk(KERN_INFO, ha,
-		    "%s legacy Int not triggered\n", __func__));
+		DEBUG7(ql4_printk(KERN_INFO, ha,
+				  "%s legacy Int not triggered\n", __func__));
 		return IRQ_NONE;
 	}
 
@@ -1158,7 +1193,7 @@ irqreturn_t qla4_83xx_intr_handler(int irq, void *dev_id)
 
 	/* Legacy interrupt is valid if bit31 of leg_int_ptr is set */
 	if (!(leg_int_ptr & LEG_INT_PTR_B31)) {
-		DEBUG2(ql4_printk(KERN_ERR, ha,
+		DEBUG7(ql4_printk(KERN_ERR, ha,
 				  "%s: Legacy Interrupt Bit 31 not set, spurious interrupt!\n",
 				  __func__));
 		return IRQ_NONE;
@@ -1166,7 +1201,7 @@ irqreturn_t qla4_83xx_intr_handler(int irq, void *dev_id)
 
 	/* Validate the PCIE function ID set in leg_int_ptr bits [19..16] */
 	if ((leg_int_ptr & PF_BITS_MASK) != ha->pf_bit) {
-		DEBUG2(ql4_printk(KERN_ERR, ha,
+		DEBUG7(ql4_printk(KERN_ERR, ha,
 				  "%s: Incorrect function ID 0x%x in legacy interrupt register, ha->pf_bit = 0x%x\n",
 				  __func__, (leg_int_ptr & PF_BITS_MASK),
 				  ha->pf_bit));
diff --git a/drivers/scsi/qla4xxx/ql4_mbx.c b/drivers/scsi/qla4xxx/ql4_mbx.c
index 160d3369721..a501beab3ff 100644
--- a/drivers/scsi/qla4xxx/ql4_mbx.c
+++ b/drivers/scsi/qla4xxx/ql4_mbx.c
@@ -1129,6 +1129,7 @@ int qla4xxx_reset_lun(struct scsi_qla_host * ha, struct ddb_entry * ddb_entry,
 {
 	uint32_t mbox_cmd[MBOX_REG_COUNT];
 	uint32_t mbox_sts[MBOX_REG_COUNT];
+	uint32_t scsi_lun[2];
 	int status = QLA_SUCCESS;
 
 	DEBUG2(printk("scsi%ld:%d:%d: lun reset issued\n", ha->host_no,
@@ -1140,10 +1141,16 @@ int qla4xxx_reset_lun(struct scsi_qla_host * ha, struct ddb_entry * ddb_entry,
 	 */
 	memset(&mbox_cmd, 0, sizeof(mbox_cmd));
 	memset(&mbox_sts, 0, sizeof(mbox_sts));
+	int_to_scsilun(lun, (struct scsi_lun *) scsi_lun);
 
 	mbox_cmd[0] = MBOX_CMD_LUN_RESET;
 	mbox_cmd[1] = ddb_entry->fw_ddb_index;
-	mbox_cmd[2] = lun << 8;
+	/* FW expects LUN bytes 0-3 in Incoming Mailbox 2
+	 * (LUN byte 0 is LSByte, byte 3 is MSByte) */
+	mbox_cmd[2] = cpu_to_le32(scsi_lun[0]);
+	/* FW expects LUN bytes 4-7 in Incoming Mailbox 3
+	 * (LUN byte 4 is LSByte, byte 7 is MSByte) */
+	mbox_cmd[3] = cpu_to_le32(scsi_lun[1]);
 	mbox_cmd[5] = 0x01;	/* Immediate Command Enable */
 
 	qla4xxx_mailbox_command(ha, MBOX_REG_COUNT, 1, &mbox_cmd[0], &mbox_sts[0]);
@@ -1281,8 +1288,8 @@ exit_about_fw:
 	return status;
 }
 
-static int qla4xxx_get_default_ddb(struct scsi_qla_host *ha, uint32_t options,
-				   dma_addr_t dma_addr)
+int qla4xxx_get_default_ddb(struct scsi_qla_host *ha, uint32_t options,
+			    dma_addr_t dma_addr)
 {
 	uint32_t mbox_cmd[MBOX_REG_COUNT];
 	uint32_t mbox_sts[MBOX_REG_COUNT];
@@ -1410,6 +1417,55 @@ exit_bootdb_failed:
 	return status;
 }
 
+int qla4xxx_flashdb_by_index(struct scsi_qla_host *ha,
+			     struct dev_db_entry *fw_ddb_entry,
+			     dma_addr_t fw_ddb_entry_dma, uint16_t ddb_index)
+{
+	uint32_t dev_db_start_offset;
+	uint32_t dev_db_end_offset;
+	int status = QLA_ERROR;
+
+	memset(fw_ddb_entry, 0, sizeof(*fw_ddb_entry));
+
+	if (is_qla40XX(ha)) {
+		dev_db_start_offset = FLASH_OFFSET_DB_INFO;
+		dev_db_end_offset = FLASH_OFFSET_DB_END;
+	} else {
+		dev_db_start_offset = FLASH_RAW_ACCESS_ADDR +
+				      (ha->hw.flt_region_ddb << 2);
+		/* flt_ddb_size is DDB table size for both ports
+		 * so divide it by 2 to calculate the offset for second port
+		 */
+		if (ha->port_num == 1)
+			dev_db_start_offset += (ha->hw.flt_ddb_size / 2);
+
+		dev_db_end_offset = dev_db_start_offset +
+				    (ha->hw.flt_ddb_size / 2);
+	}
+
+	dev_db_start_offset += (ddb_index * sizeof(*fw_ddb_entry));
+
+	if (dev_db_start_offset > dev_db_end_offset) {
+		DEBUG2(ql4_printk(KERN_ERR, ha,
+				  "%s:Invalid DDB index %d", __func__,
+				  ddb_index));
+		goto exit_fdb_failed;
+	}
+
+	if (qla4xxx_get_flash(ha, fw_ddb_entry_dma, dev_db_start_offset,
+			      sizeof(*fw_ddb_entry)) != QLA_SUCCESS) {
+		ql4_printk(KERN_ERR, ha, "scsi%ld: %s: Get Flash failed\n",
+			   ha->host_no, __func__);
+		goto exit_fdb_failed;
+	}
+
+	if (fw_ddb_entry->cookie == DDB_VALID_COOKIE)
+		status = QLA_SUCCESS;
+
+exit_fdb_failed:
+	return status;
+}
+
 int qla4xxx_get_chap(struct scsi_qla_host *ha, char *username, char *password,
 		     uint16_t idx)
 {
@@ -1503,6 +1559,62 @@ exit_set_chap:
 	return ret;
 }
 
+
+int qla4xxx_get_uni_chap_at_index(struct scsi_qla_host *ha, char *username,
+				  char *password, uint16_t chap_index)
+{
+	int rval = QLA_ERROR;
+	struct ql4_chap_table *chap_table = NULL;
+	int max_chap_entries;
+
+	if (!ha->chap_list) {
+		ql4_printk(KERN_ERR, ha, "Do not have CHAP table cache\n");
+		rval = QLA_ERROR;
+		goto exit_uni_chap;
+	}
+
+	if (!username || !password) {
+		ql4_printk(KERN_ERR, ha, "No memory for username & secret\n");
+		rval = QLA_ERROR;
+		goto exit_uni_chap;
+	}
+
+	if (is_qla80XX(ha))
+		max_chap_entries = (ha->hw.flt_chap_size / 2) /
+				   sizeof(struct ql4_chap_table);
+	else
+		max_chap_entries = MAX_CHAP_ENTRIES_40XX;
+
+	if (chap_index > max_chap_entries) {
+		ql4_printk(KERN_ERR, ha, "Invalid Chap index\n");
+		rval = QLA_ERROR;
+		goto exit_uni_chap;
+	}
+
+	mutex_lock(&ha->chap_sem);
+	chap_table = (struct ql4_chap_table *)ha->chap_list + chap_index;
+	if (chap_table->cookie != __constant_cpu_to_le16(CHAP_VALID_COOKIE)) {
+		rval = QLA_ERROR;
+		goto exit_unlock_uni_chap;
+	}
+
+	if (!(chap_table->flags & BIT_6)) {
+		ql4_printk(KERN_ERR, ha, "Unidirectional entry not set\n");
+		rval = QLA_ERROR;
+		goto exit_unlock_uni_chap;
+	}
+
+	strncpy(password, chap_table->secret, MAX_CHAP_SECRET_LEN);
+	strncpy(username, chap_table->name, MAX_CHAP_NAME_LEN);
+
+	rval = QLA_SUCCESS;
+
+exit_unlock_uni_chap:
+	mutex_unlock(&ha->chap_sem);
+exit_uni_chap:
+	return rval;
+}
+
 /**
  * qla4xxx_get_chap_index - Get chap index given username and secret
  * @ha: pointer to adapter structure
@@ -1524,7 +1636,7 @@ int qla4xxx_get_chap_index(struct scsi_qla_host *ha, char *username,
 	int max_chap_entries = 0;
 	struct ql4_chap_table *chap_table;
 
-	if (is_qla8022(ha))
+	if (is_qla80XX(ha))
 		max_chap_entries = (ha->hw.flt_chap_size / 2) /
 						sizeof(struct ql4_chap_table);
 	else
diff --git a/drivers/scsi/qla4xxx/ql4_nx.c b/drivers/scsi/qla4xxx/ql4_nx.c
index 71d3d234f52..eaf00c162eb 100644
--- a/drivers/scsi/qla4xxx/ql4_nx.c
+++ b/drivers/scsi/qla4xxx/ql4_nx.c
@@ -2089,7 +2089,7 @@ static int qla4_8xxx_minidump_process_rdmem(struct scsi_qla_host *ha,
 
 	if (r_addr & 0xf) {
 		DEBUG2(ql4_printk(KERN_INFO, ha,
-				  "[%s]: Read addr 0x%x not 16 bytes alligned\n",
+				  "[%s]: Read addr 0x%x not 16 bytes aligned\n",
 				  __func__, r_addr));
 		return QLA_ERROR;
 	}
@@ -3154,6 +3154,10 @@ qla4_8xxx_get_flt_info(struct scsi_qla_host *ha, uint32_t flt_addr)
 			hw->flt_region_chap =  start;
 			hw->flt_chap_size =  le32_to_cpu(region->size);
 			break;
+		case FLT_REG_ISCSI_DDB:
+			hw->flt_region_ddb =  start;
+			hw->flt_ddb_size =  le32_to_cpu(region->size);
+			break;
 		}
 	}
 	goto done;
@@ -3166,14 +3170,19 @@ no_flash_data:
 	hw->flt_region_boot     = FA_BOOT_CODE_ADDR_82;
 	hw->flt_region_bootload = FA_BOOT_LOAD_ADDR_82;
 	hw->flt_region_fw       = FA_RISC_CODE_ADDR_82;
-	hw->flt_region_chap	= FA_FLASH_ISCSI_CHAP;
+	hw->flt_region_chap	= FA_FLASH_ISCSI_CHAP >> 2;
 	hw->flt_chap_size	= FA_FLASH_CHAP_SIZE;
+	hw->flt_region_ddb	= FA_FLASH_ISCSI_DDB >> 2;
+	hw->flt_ddb_size	= FA_FLASH_DDB_SIZE;
 
 done:
-	DEBUG2(ql4_printk(KERN_INFO, ha, "FLT[%s]: flt=0x%x fdt=0x%x "
-	    "boot=0x%x bootload=0x%x fw=0x%x\n", loc, hw->flt_region_flt,
-	    hw->flt_region_fdt,	hw->flt_region_boot, hw->flt_region_bootload,
-	    hw->flt_region_fw));
+	DEBUG2(ql4_printk(KERN_INFO, ha,
+			  "FLT[%s]: flt=0x%x fdt=0x%x boot=0x%x bootload=0x%x fw=0x%x chap=0x%x chap_size=0x%x ddb=0x%x  ddb_size=0x%x\n",
+			  loc, hw->flt_region_flt, hw->flt_region_fdt,
+			  hw->flt_region_boot, hw->flt_region_bootload,
+			  hw->flt_region_fw, hw->flt_region_chap,
+			  hw->flt_chap_size, hw->flt_region_ddb,
+			  hw->flt_ddb_size));
 }
 
 static void
diff --git a/drivers/scsi/qla4xxx/ql4_os.c b/drivers/scsi/qla4xxx/ql4_os.c
index 6142729167f..a47f99957ba 100644
--- a/drivers/scsi/qla4xxx/ql4_os.c
+++ b/drivers/scsi/qla4xxx/ql4_os.c
@@ -166,6 +166,26 @@ static int qla4xxx_host_reset(struct Scsi_Host *shost, int reset_type);
 static int qla4xxx_change_queue_depth(struct scsi_device *sdev, int qdepth,
 				      int reason);
 
+/*
+ * iSCSI Flash DDB sysfs entry points
+ */
+static int
+qla4xxx_sysfs_ddb_set_param(struct iscsi_bus_flash_session *fnode_sess,
+			    struct iscsi_bus_flash_conn *fnode_conn,
+			    void *data, int len);
+static int
+qla4xxx_sysfs_ddb_get_param(struct iscsi_bus_flash_session *fnode_sess,
+			    int param, char *buf);
+static int qla4xxx_sysfs_ddb_add(struct Scsi_Host *shost, const char *buf,
+				 int len);
+static int
+qla4xxx_sysfs_ddb_delete(struct iscsi_bus_flash_session *fnode_sess);
+static int qla4xxx_sysfs_ddb_login(struct iscsi_bus_flash_session *fnode_sess,
+				   struct iscsi_bus_flash_conn *fnode_conn);
+static int qla4xxx_sysfs_ddb_logout(struct iscsi_bus_flash_session *fnode_sess,
+				    struct iscsi_bus_flash_conn *fnode_conn);
+static int qla4xxx_sysfs_ddb_logout_sid(struct iscsi_cls_session *cls_sess);
+
 static struct qla4_8xxx_legacy_intr_set legacy_intr[] =
     QLA82XX_LEGACY_INTR_CONFIG;
 
@@ -232,6 +252,13 @@ static struct iscsi_transport qla4xxx_iscsi_transport = {
 	.send_ping		= qla4xxx_send_ping,
 	.get_chap		= qla4xxx_get_chap_list,
 	.delete_chap		= qla4xxx_delete_chap,
+	.get_flashnode_param	= qla4xxx_sysfs_ddb_get_param,
+	.set_flashnode_param	= qla4xxx_sysfs_ddb_set_param,
+	.new_flashnode		= qla4xxx_sysfs_ddb_add,
+	.del_flashnode		= qla4xxx_sysfs_ddb_delete,
+	.login_flashnode	= qla4xxx_sysfs_ddb_login,
+	.logout_flashnode	= qla4xxx_sysfs_ddb_logout,
+	.logout_flashnode_sid	= qla4xxx_sysfs_ddb_logout_sid,
 };
 
 static struct scsi_transport_template *qla4xxx_scsi_transport;
@@ -376,6 +403,68 @@ static umode_t qla4_attr_is_visible(int param_type, int param)
 		default:
 			return 0;
 		}
+	case ISCSI_FLASHNODE_PARAM:
+		switch (param) {
+		case ISCSI_FLASHNODE_IS_FW_ASSIGNED_IPV6:
+		case ISCSI_FLASHNODE_PORTAL_TYPE:
+		case ISCSI_FLASHNODE_AUTO_SND_TGT_DISABLE:
+		case ISCSI_FLASHNODE_DISCOVERY_SESS:
+		case ISCSI_FLASHNODE_ENTRY_EN:
+		case ISCSI_FLASHNODE_HDR_DGST_EN:
+		case ISCSI_FLASHNODE_DATA_DGST_EN:
+		case ISCSI_FLASHNODE_IMM_DATA_EN:
+		case ISCSI_FLASHNODE_INITIAL_R2T_EN:
+		case ISCSI_FLASHNODE_DATASEQ_INORDER:
+		case ISCSI_FLASHNODE_PDU_INORDER:
+		case ISCSI_FLASHNODE_CHAP_AUTH_EN:
+		case ISCSI_FLASHNODE_SNACK_REQ_EN:
+		case ISCSI_FLASHNODE_DISCOVERY_LOGOUT_EN:
+		case ISCSI_FLASHNODE_BIDI_CHAP_EN:
+		case ISCSI_FLASHNODE_DISCOVERY_AUTH_OPTIONAL:
+		case ISCSI_FLASHNODE_ERL:
+		case ISCSI_FLASHNODE_TCP_TIMESTAMP_STAT:
+		case ISCSI_FLASHNODE_TCP_NAGLE_DISABLE:
+		case ISCSI_FLASHNODE_TCP_WSF_DISABLE:
+		case ISCSI_FLASHNODE_TCP_TIMER_SCALE:
+		case ISCSI_FLASHNODE_TCP_TIMESTAMP_EN:
+		case ISCSI_FLASHNODE_IP_FRAG_DISABLE:
+		case ISCSI_FLASHNODE_MAX_RECV_DLENGTH:
+		case ISCSI_FLASHNODE_MAX_XMIT_DLENGTH:
+		case ISCSI_FLASHNODE_FIRST_BURST:
+		case ISCSI_FLASHNODE_DEF_TIME2WAIT:
+		case ISCSI_FLASHNODE_DEF_TIME2RETAIN:
+		case ISCSI_FLASHNODE_MAX_R2T:
+		case ISCSI_FLASHNODE_KEEPALIVE_TMO:
+		case ISCSI_FLASHNODE_ISID:
+		case ISCSI_FLASHNODE_TSID:
+		case ISCSI_FLASHNODE_PORT:
+		case ISCSI_FLASHNODE_MAX_BURST:
+		case ISCSI_FLASHNODE_DEF_TASKMGMT_TMO:
+		case ISCSI_FLASHNODE_IPADDR:
+		case ISCSI_FLASHNODE_ALIAS:
+		case ISCSI_FLASHNODE_REDIRECT_IPADDR:
+		case ISCSI_FLASHNODE_MAX_SEGMENT_SIZE:
+		case ISCSI_FLASHNODE_LOCAL_PORT:
+		case ISCSI_FLASHNODE_IPV4_TOS:
+		case ISCSI_FLASHNODE_IPV6_TC:
+		case ISCSI_FLASHNODE_IPV6_FLOW_LABEL:
+		case ISCSI_FLASHNODE_NAME:
+		case ISCSI_FLASHNODE_TPGT:
+		case ISCSI_FLASHNODE_LINK_LOCAL_IPV6:
+		case ISCSI_FLASHNODE_DISCOVERY_PARENT_IDX:
+		case ISCSI_FLASHNODE_DISCOVERY_PARENT_TYPE:
+		case ISCSI_FLASHNODE_TCP_XMIT_WSF:
+		case ISCSI_FLASHNODE_TCP_RECV_WSF:
+		case ISCSI_FLASHNODE_CHAP_OUT_IDX:
+		case ISCSI_FLASHNODE_USERNAME:
+		case ISCSI_FLASHNODE_PASSWORD:
+		case ISCSI_FLASHNODE_STATSN:
+		case ISCSI_FLASHNODE_EXP_STATSN:
+		case ISCSI_FLASHNODE_IS_BOOT_TGT:
+			return S_IRUGO;
+		default:
+			return 0;
+		}
 	}
 
 	return 0;
@@ -391,7 +480,7 @@ static int qla4xxx_get_chap_list(struct Scsi_Host *shost, uint16_t chap_tbl_idx,
 	int valid_chap_entries = 0;
 	int ret = 0, i;
 
-	if (is_qla8022(ha))
+	if (is_qla80XX(ha))
 		max_chap_entries = (ha->hw.flt_chap_size / 2) /
 					sizeof(struct ql4_chap_table);
 	else
@@ -495,7 +584,7 @@ static int qla4xxx_delete_chap(struct Scsi_Host *shost, uint16_t chap_tbl_idx)
 
 	memset(chap_table, 0, sizeof(struct ql4_chap_table));
 
-	if (is_qla8022(ha))
+	if (is_qla80XX(ha))
 		max_chap_entries = (ha->hw.flt_chap_size / 2) /
 				   sizeof(struct ql4_chap_table);
 	else
@@ -1922,6 +2011,252 @@ static int qla4xxx_task_xmit(struct iscsi_task *task)
 	return -ENOSYS;
 }
 
+static int qla4xxx_copy_from_fwddb_param(struct iscsi_bus_flash_session *sess,
+					 struct iscsi_bus_flash_conn *conn,
+					 struct dev_db_entry *fw_ddb_entry)
+{
+	unsigned long options = 0;
+	int rc = 0;
+
+	options = le16_to_cpu(fw_ddb_entry->options);
+	conn->is_fw_assigned_ipv6 = test_bit(OPT_IS_FW_ASSIGNED_IPV6, &options);
+	if (test_bit(OPT_IPV6_DEVICE, &options)) {
+		rc = iscsi_switch_str_param(&sess->portal_type,
+					    PORTAL_TYPE_IPV6);
+		if (rc)
+			goto exit_copy;
+	} else {
+		rc = iscsi_switch_str_param(&sess->portal_type,
+					    PORTAL_TYPE_IPV4);
+		if (rc)
+			goto exit_copy;
+	}
+
+	sess->auto_snd_tgt_disable = test_bit(OPT_AUTO_SENDTGTS_DISABLE,
+					      &options);
+	sess->discovery_sess = test_bit(OPT_DISC_SESSION, &options);
+	sess->entry_state = test_bit(OPT_ENTRY_STATE, &options);
+
+	options = le16_to_cpu(fw_ddb_entry->iscsi_options);
+	conn->hdrdgst_en = test_bit(ISCSIOPT_HEADER_DIGEST_EN, &options);
+	conn->datadgst_en = test_bit(ISCSIOPT_DATA_DIGEST_EN, &options);
+	sess->imm_data_en = test_bit(ISCSIOPT_IMMEDIATE_DATA_EN, &options);
+	sess->initial_r2t_en = test_bit(ISCSIOPT_INITIAL_R2T_EN, &options);
+	sess->dataseq_inorder_en = test_bit(ISCSIOPT_DATA_SEQ_IN_ORDER,
+					    &options);
+	sess->pdu_inorder_en = test_bit(ISCSIOPT_DATA_PDU_IN_ORDER, &options);
+	sess->chap_auth_en = test_bit(ISCSIOPT_CHAP_AUTH_EN, &options);
+	conn->snack_req_en = test_bit(ISCSIOPT_SNACK_REQ_EN, &options);
+	sess->discovery_logout_en = test_bit(ISCSIOPT_DISCOVERY_LOGOUT_EN,
+					     &options);
+	sess->bidi_chap_en = test_bit(ISCSIOPT_BIDI_CHAP_EN, &options);
+	sess->discovery_auth_optional =
+			test_bit(ISCSIOPT_DISCOVERY_AUTH_OPTIONAL, &options);
+	if (test_bit(ISCSIOPT_ERL1, &options))
+		sess->erl |= BIT_1;
+	if (test_bit(ISCSIOPT_ERL0, &options))
+		sess->erl |= BIT_0;
+
+	options = le16_to_cpu(fw_ddb_entry->tcp_options);
+	conn->tcp_timestamp_stat = test_bit(TCPOPT_TIMESTAMP_STAT, &options);
+	conn->tcp_nagle_disable = test_bit(TCPOPT_NAGLE_DISABLE, &options);
+	conn->tcp_wsf_disable = test_bit(TCPOPT_WSF_DISABLE, &options);
+	if (test_bit(TCPOPT_TIMER_SCALE3, &options))
+		conn->tcp_timer_scale |= BIT_3;
+	if (test_bit(TCPOPT_TIMER_SCALE2, &options))
+		conn->tcp_timer_scale |= BIT_2;
+	if (test_bit(TCPOPT_TIMER_SCALE1, &options))
+		conn->tcp_timer_scale |= BIT_1;
+
+	conn->tcp_timer_scale >>= 1;
+	conn->tcp_timestamp_en = test_bit(TCPOPT_TIMESTAMP_EN, &options);
+
+	options = le16_to_cpu(fw_ddb_entry->ip_options);
+	conn->fragment_disable = test_bit(IPOPT_FRAGMENT_DISABLE, &options);
+
+	conn->max_recv_dlength = BYTE_UNITS *
+			  le16_to_cpu(fw_ddb_entry->iscsi_max_rcv_data_seg_len);
+	conn->max_xmit_dlength = BYTE_UNITS *
+			  le16_to_cpu(fw_ddb_entry->iscsi_max_snd_data_seg_len);
+	sess->first_burst = BYTE_UNITS *
+			       le16_to_cpu(fw_ddb_entry->iscsi_first_burst_len);
+	sess->max_burst = BYTE_UNITS *
+				 le16_to_cpu(fw_ddb_entry->iscsi_max_burst_len);
+	sess->max_r2t = le16_to_cpu(fw_ddb_entry->iscsi_max_outsnd_r2t);
+	sess->time2wait = le16_to_cpu(fw_ddb_entry->iscsi_def_time2wait);
+	sess->time2retain = le16_to_cpu(fw_ddb_entry->iscsi_def_time2retain);
+	sess->tpgt = le32_to_cpu(fw_ddb_entry->tgt_portal_grp);
+	conn->max_segment_size = le16_to_cpu(fw_ddb_entry->mss);
+	conn->tcp_xmit_wsf = fw_ddb_entry->tcp_xmt_wsf;
+	conn->tcp_recv_wsf = fw_ddb_entry->tcp_rcv_wsf;
+	conn->ipv6_flow_label = le16_to_cpu(fw_ddb_entry->ipv6_flow_lbl);
+	conn->keepalive_timeout = le16_to_cpu(fw_ddb_entry->ka_timeout);
+	conn->local_port = le16_to_cpu(fw_ddb_entry->lcl_port);
+	conn->statsn = le32_to_cpu(fw_ddb_entry->stat_sn);
+	conn->exp_statsn = le32_to_cpu(fw_ddb_entry->exp_stat_sn);
+	sess->discovery_parent_idx = le16_to_cpu(fw_ddb_entry->ddb_link);
+	sess->discovery_parent_type = le16_to_cpu(fw_ddb_entry->ddb_link);
+	sess->chap_out_idx = le16_to_cpu(fw_ddb_entry->chap_tbl_idx);
+	sess->tsid = le16_to_cpu(fw_ddb_entry->tsid);
+
+	sess->default_taskmgmt_timeout =
+				le16_to_cpu(fw_ddb_entry->def_timeout);
+	conn->port = le16_to_cpu(fw_ddb_entry->port);
+
+	options = le16_to_cpu(fw_ddb_entry->options);
+	conn->ipaddress = kzalloc(IPv6_ADDR_LEN, GFP_KERNEL);
+	if (!conn->ipaddress) {
+		rc = -ENOMEM;
+		goto exit_copy;
+	}
+
+	conn->redirect_ipaddr = kzalloc(IPv6_ADDR_LEN, GFP_KERNEL);
+	if (!conn->redirect_ipaddr) {
+		rc = -ENOMEM;
+		goto exit_copy;
+	}
+
+	memcpy(conn->ipaddress, fw_ddb_entry->ip_addr, IPv6_ADDR_LEN);
+	memcpy(conn->redirect_ipaddr, fw_ddb_entry->tgt_addr, IPv6_ADDR_LEN);
+
+	if (test_bit(OPT_IPV6_DEVICE, &options)) {
+		conn->ipv6_traffic_class = fw_ddb_entry->ipv4_tos;
+
+		conn->link_local_ipv6_addr = kzalloc(IPv6_ADDR_LEN, GFP_KERNEL);
+		if (!conn->link_local_ipv6_addr) {
+			rc = -ENOMEM;
+			goto exit_copy;
+		}
+
+		memcpy(conn->link_local_ipv6_addr,
+		       fw_ddb_entry->link_local_ipv6_addr, IPv6_ADDR_LEN);
+	} else {
+		conn->ipv4_tos = fw_ddb_entry->ipv4_tos;
+	}
+
+	if (fw_ddb_entry->iscsi_name[0]) {
+		rc = iscsi_switch_str_param(&sess->targetname,
+					    (char *)fw_ddb_entry->iscsi_name);
+		if (rc)
+			goto exit_copy;
+	}
+
+	if (fw_ddb_entry->iscsi_alias[0]) {
+		rc = iscsi_switch_str_param(&sess->targetalias,
+					    (char *)fw_ddb_entry->iscsi_alias);
+		if (rc)
+			goto exit_copy;
+	}
+
+	COPY_ISID(sess->isid, fw_ddb_entry->isid);
+
+exit_copy:
+	return rc;
+}
+
+static int qla4xxx_copy_to_fwddb_param(struct iscsi_bus_flash_session *sess,
+				       struct iscsi_bus_flash_conn *conn,
+				       struct dev_db_entry *fw_ddb_entry)
+{
+	uint16_t options;
+	int rc = 0;
+
+	options = le16_to_cpu(fw_ddb_entry->options);
+	SET_BITVAL(conn->is_fw_assigned_ipv6,  options, BIT_11);
+	if (!strncmp(sess->portal_type, PORTAL_TYPE_IPV6, 4))
+		options |= BIT_8;
+	else
+		options &= ~BIT_8;
+
+	SET_BITVAL(sess->auto_snd_tgt_disable, options, BIT_6);
+	SET_BITVAL(sess->discovery_sess, options, BIT_4);
+	SET_BITVAL(sess->entry_state, options, BIT_3);
+	fw_ddb_entry->options = cpu_to_le16(options);
+
+	options = le16_to_cpu(fw_ddb_entry->iscsi_options);
+	SET_BITVAL(conn->hdrdgst_en, options, BIT_13);
+	SET_BITVAL(conn->datadgst_en, options, BIT_12);
+	SET_BITVAL(sess->imm_data_en, options, BIT_11);
+	SET_BITVAL(sess->initial_r2t_en, options, BIT_10);
+	SET_BITVAL(sess->dataseq_inorder_en, options, BIT_9);
+	SET_BITVAL(sess->pdu_inorder_en, options, BIT_8);
+	SET_BITVAL(sess->chap_auth_en, options, BIT_7);
+	SET_BITVAL(conn->snack_req_en, options, BIT_6);
+	SET_BITVAL(sess->discovery_logout_en, options, BIT_5);
+	SET_BITVAL(sess->bidi_chap_en, options, BIT_4);
+	SET_BITVAL(sess->discovery_auth_optional, options, BIT_3);
+	SET_BITVAL(sess->erl & BIT_1, options, BIT_1);
+	SET_BITVAL(sess->erl & BIT_0, options, BIT_0);
+	fw_ddb_entry->iscsi_options = cpu_to_le16(options);
+
+	options = le16_to_cpu(fw_ddb_entry->tcp_options);
+	SET_BITVAL(conn->tcp_timestamp_stat, options, BIT_6);
+	SET_BITVAL(conn->tcp_nagle_disable, options, BIT_5);
+	SET_BITVAL(conn->tcp_wsf_disable, options, BIT_4);
+	SET_BITVAL(conn->tcp_timer_scale & BIT_2, options, BIT_3);
+	SET_BITVAL(conn->tcp_timer_scale & BIT_1, options, BIT_2);
+	SET_BITVAL(conn->tcp_timer_scale & BIT_0, options, BIT_1);
+	SET_BITVAL(conn->tcp_timestamp_en, options, BIT_0);
+	fw_ddb_entry->tcp_options = cpu_to_le16(options);
+
+	options = le16_to_cpu(fw_ddb_entry->ip_options);
+	SET_BITVAL(conn->fragment_disable, options, BIT_4);
+	fw_ddb_entry->ip_options = cpu_to_le16(options);
+
+	fw_ddb_entry->iscsi_max_outsnd_r2t = cpu_to_le16(sess->max_r2t);
+	fw_ddb_entry->iscsi_max_rcv_data_seg_len =
+			       cpu_to_le16(conn->max_recv_dlength / BYTE_UNITS);
+	fw_ddb_entry->iscsi_max_snd_data_seg_len =
+			       cpu_to_le16(conn->max_xmit_dlength / BYTE_UNITS);
+	fw_ddb_entry->iscsi_first_burst_len =
+				cpu_to_le16(sess->first_burst / BYTE_UNITS);
+	fw_ddb_entry->iscsi_max_burst_len = cpu_to_le16(sess->max_burst /
+					    BYTE_UNITS);
+	fw_ddb_entry->iscsi_def_time2wait = cpu_to_le16(sess->time2wait);
+	fw_ddb_entry->iscsi_def_time2retain = cpu_to_le16(sess->time2retain);
+	fw_ddb_entry->tgt_portal_grp = cpu_to_le16(sess->tpgt);
+	fw_ddb_entry->mss = cpu_to_le16(conn->max_segment_size);
+	fw_ddb_entry->tcp_xmt_wsf = cpu_to_le16(conn->tcp_xmit_wsf);
+	fw_ddb_entry->tcp_rcv_wsf = cpu_to_le16(conn->tcp_recv_wsf);
+	fw_ddb_entry->ipv4_tos = conn->ipv4_tos;
+	fw_ddb_entry->ipv6_flow_lbl = cpu_to_le16(conn->ipv6_flow_label);
+	fw_ddb_entry->ka_timeout = cpu_to_le16(conn->keepalive_timeout);
+	fw_ddb_entry->lcl_port = cpu_to_le16(conn->local_port);
+	fw_ddb_entry->stat_sn = cpu_to_le16(conn->statsn);
+	fw_ddb_entry->exp_stat_sn = cpu_to_le16(conn->exp_statsn);
+	fw_ddb_entry->ddb_link = cpu_to_le16(sess->discovery_parent_type);
+	fw_ddb_entry->chap_tbl_idx = cpu_to_le16(sess->chap_out_idx);
+	fw_ddb_entry->tsid = cpu_to_le16(sess->tsid);
+	fw_ddb_entry->port = cpu_to_le16(conn->port);
+	fw_ddb_entry->def_timeout =
+				cpu_to_le16(sess->default_taskmgmt_timeout);
+
+	if (conn->ipaddress)
+		memcpy(fw_ddb_entry->ip_addr, conn->ipaddress,
+		       sizeof(fw_ddb_entry->ip_addr));
+
+	if (conn->redirect_ipaddr)
+		memcpy(fw_ddb_entry->tgt_addr, conn->redirect_ipaddr,
+		       sizeof(fw_ddb_entry->tgt_addr));
+
+	if (conn->link_local_ipv6_addr)
+		memcpy(fw_ddb_entry->link_local_ipv6_addr,
+		       conn->link_local_ipv6_addr,
+		       sizeof(fw_ddb_entry->link_local_ipv6_addr));
+
+	if (sess->targetname)
+		memcpy(fw_ddb_entry->iscsi_name, sess->targetname,
+		       sizeof(fw_ddb_entry->iscsi_name));
+
+	if (sess->targetalias)
+		memcpy(fw_ddb_entry->iscsi_alias, sess->targetalias,
+		       sizeof(fw_ddb_entry->iscsi_alias));
+
+	COPY_ISID(fw_ddb_entry->isid, sess->isid);
+
+	return rc;
+}
+
 static void qla4xxx_copy_fwddb_param(struct scsi_qla_host *ha,
 				     struct dev_db_entry *fw_ddb_entry,
 				     struct iscsi_cls_session *cls_sess,
@@ -2543,6 +2878,7 @@ static void qla4_8xxx_process_fw_error(struct scsi_qla_host *ha)
 void qla4_8xxx_watchdog(struct scsi_qla_host *ha)
 {
 	uint32_t dev_state;
+	uint32_t idc_ctrl;
 
 	/* don't poll if reset is going on */
 	if (!(test_bit(DPC_RESET_ACTIVE, &ha->dpc_flags) ||
@@ -2561,10 +2897,23 @@ void qla4_8xxx_watchdog(struct scsi_qla_host *ha)
 			qla4xxx_wake_dpc(ha);
 		} else if (dev_state == QLA8XXX_DEV_NEED_RESET &&
 			   !test_bit(DPC_RESET_HA, &ha->dpc_flags)) {
+
+			ql4_printk(KERN_INFO, ha, "%s: HW State: NEED RESET!\n",
+				   __func__);
+
+			if (is_qla8032(ha)) {
+				idc_ctrl = qla4_83xx_rd_reg(ha,
+							QLA83XX_IDC_DRV_CTRL);
+				if (!(idc_ctrl & GRACEFUL_RESET_BIT1)) {
+					ql4_printk(KERN_INFO, ha, "%s: Graceful reset bit is not set\n",
+						   __func__);
+					qla4xxx_mailbox_premature_completion(
+									    ha);
+				}
+			}
+
 			if (is_qla8032(ha) ||
 			    (is_qla8022(ha) && !ql4xdontresethba)) {
-				ql4_printk(KERN_INFO, ha, "%s: HW State: "
-				    "NEED RESET!\n", __func__);
 				set_bit(DPC_RESET_HA, &ha->dpc_flags);
 				qla4xxx_wake_dpc(ha);
 			}
@@ -3737,8 +4086,8 @@ static struct isp_operations qla4_83xx_isp_ops = {
 	.reset_firmware		= qla4_8xxx_stop_firmware,
 	.queue_iocb		= qla4_83xx_queue_iocb,
 	.complete_iocb		= qla4_83xx_complete_iocb,
-	.rd_shdw_req_q_out	= qla4_83xx_rd_shdw_req_q_out,
-	.rd_shdw_rsp_q_in	= qla4_83xx_rd_shdw_rsp_q_in,
+	.rd_shdw_req_q_out	= qla4xxx_rd_shdw_req_q_out,
+	.rd_shdw_rsp_q_in	= qla4xxx_rd_shdw_rsp_q_in,
 	.get_sys_info		= qla4_8xxx_get_sys_info,
 	.rd_reg_direct		= qla4_83xx_rd_reg,
 	.wr_reg_direct		= qla4_83xx_wr_reg,
@@ -3761,11 +4110,6 @@ uint16_t qla4_82xx_rd_shdw_req_q_out(struct scsi_qla_host *ha)
 	return (uint16_t)le32_to_cpu(readl(&ha->qla4_82xx_reg->req_q_out));
 }
 
-uint16_t qla4_83xx_rd_shdw_req_q_out(struct scsi_qla_host *ha)
-{
-	return (uint16_t)le32_to_cpu(readl(&ha->qla4_83xx_reg->req_q_out));
-}
-
 uint16_t qla4xxx_rd_shdw_rsp_q_in(struct scsi_qla_host *ha)
 {
 	return (uint16_t)le32_to_cpu(ha->shadow_regs->rsp_q_in);
@@ -3776,11 +4120,6 @@ uint16_t qla4_82xx_rd_shdw_rsp_q_in(struct scsi_qla_host *ha)
 	return (uint16_t)le32_to_cpu(readl(&ha->qla4_82xx_reg->rsp_q_in));
 }
 
-uint16_t qla4_83xx_rd_shdw_rsp_q_in(struct scsi_qla_host *ha)
-{
-	return (uint16_t)le32_to_cpu(readl(&ha->qla4_83xx_reg->rsp_q_in));
-}
-
 static ssize_t qla4xxx_show_boot_eth_info(void *data, int type, char *buf)
 {
 	struct scsi_qla_host *ha = data;
@@ -4005,7 +4344,7 @@ static int get_fw_boot_info(struct scsi_qla_host *ha, uint16_t ddb_index[])
 		if (val & BIT_7)
 			ddb_index[1] = (val & 0x7f);
 
-	} else if (is_qla8022(ha)) {
+	} else if (is_qla80XX(ha)) {
 		buf = dma_alloc_coherent(&ha->pdev->dev, size,
 					 &buf_dma, GFP_KERNEL);
 		if (!buf) {
@@ -4083,7 +4422,7 @@ static int qla4xxx_get_bidi_chap(struct scsi_qla_host *ha, char *username,
 	int max_chap_entries = 0;
 	struct ql4_chap_table *chap_table;
 
-	if (is_qla8022(ha))
+	if (is_qla80XX(ha))
 		max_chap_entries = (ha->hw.flt_chap_size / 2) /
 						sizeof(struct ql4_chap_table);
 	else
@@ -5058,6 +5397,1342 @@ exit_nt_list:
 		dma_pool_free(ha->fw_ddb_dma_pool, fw_ddb_entry, fw_ddb_dma);
 }
 
+static void qla4xxx_build_new_nt_list(struct scsi_qla_host *ha,
+				      struct list_head *list_nt)
+{
+	struct dev_db_entry *fw_ddb_entry;
+	dma_addr_t fw_ddb_dma;
+	int max_ddbs;
+	int fw_idx_size;
+	int ret;
+	uint32_t idx = 0, next_idx = 0;
+	uint32_t state = 0, conn_err = 0;
+	uint16_t conn_id = 0;
+	struct qla_ddb_index  *nt_ddb_idx;
+
+	fw_ddb_entry = dma_pool_alloc(ha->fw_ddb_dma_pool, GFP_KERNEL,
+				      &fw_ddb_dma);
+	if (fw_ddb_entry == NULL) {
+		DEBUG2(ql4_printk(KERN_ERR, ha, "Out of memory\n"));
+		goto exit_new_nt_list;
+	}
+	max_ddbs =  is_qla40XX(ha) ? MAX_DEV_DB_ENTRIES_40XX :
+				     MAX_DEV_DB_ENTRIES;
+	fw_idx_size = sizeof(struct qla_ddb_index);
+
+	for (idx = 0; idx < max_ddbs; idx = next_idx) {
+		ret = qla4xxx_get_fwddb_entry(ha, idx, fw_ddb_entry, fw_ddb_dma,
+					      NULL, &next_idx, &state,
+					      &conn_err, NULL, &conn_id);
+		if (ret == QLA_ERROR)
+			break;
+
+		/* Check if NT, then add it to list */
+		if (strlen((char *)fw_ddb_entry->iscsi_name) == 0)
+			goto continue_next_new_nt;
+
+		if (!(state == DDB_DS_NO_CONNECTION_ACTIVE))
+			goto continue_next_new_nt;
+
+		DEBUG2(ql4_printk(KERN_INFO, ha,
+				  "Adding  DDB to session = 0x%x\n", idx));
+
+		nt_ddb_idx = vmalloc(fw_idx_size);
+		if (!nt_ddb_idx)
+			break;
+
+		nt_ddb_idx->fw_ddb_idx = idx;
+
+		ret = qla4xxx_is_session_exists(ha, fw_ddb_entry);
+		if (ret == QLA_SUCCESS) {
+			/* free nt_ddb_idx and do not add to list_nt */
+			vfree(nt_ddb_idx);
+			goto continue_next_new_nt;
+		}
+
+		list_add_tail(&nt_ddb_idx->list, list_nt);
+
+		ret = qla4xxx_sess_conn_setup(ha, fw_ddb_entry, RESET_ADAPTER,
+					      idx);
+		if (ret == QLA_ERROR)
+			goto exit_new_nt_list;
+
+continue_next_new_nt:
+		if (next_idx == 0)
+			break;
+	}
+
+exit_new_nt_list:
+	if (fw_ddb_entry)
+		dma_pool_free(ha->fw_ddb_dma_pool, fw_ddb_entry, fw_ddb_dma);
+}
+
+/**
+ * qla4xxx_sysfs_ddb_is_non_persistent - check for non-persistence of ddb entry
+ * @dev: dev associated with the sysfs entry
+ * @data: pointer to flashnode session object
+ *
+ * Returns:
+ *	1: if flashnode entry is non-persistent
+ *	0: if flashnode entry is persistent
+ **/
+static int qla4xxx_sysfs_ddb_is_non_persistent(struct device *dev, void *data)
+{
+	struct iscsi_bus_flash_session *fnode_sess;
+
+	if (!iscsi_flashnode_bus_match(dev, NULL))
+		return 0;
+
+	fnode_sess = iscsi_dev_to_flash_session(dev);
+
+	return (fnode_sess->flash_state == DEV_DB_NON_PERSISTENT);
+}
+
+/**
+ * qla4xxx_sysfs_ddb_tgt_create - Create sysfs entry for target
+ * @ha: pointer to host
+ * @fw_ddb_entry: flash ddb data
+ * @idx: target index
+ * @user: if set then this call is made from userland else from kernel
+ *
+ * Returns:
+ * On sucess: QLA_SUCCESS
+ * On failure: QLA_ERROR
+ *
+ * This create separate sysfs entries for session and connection attributes of
+ * the given fw ddb entry.
+ * If this is invoked as a result of a userspace call then the entry is marked
+ * as nonpersistent using flash_state field.
+ **/
+int qla4xxx_sysfs_ddb_tgt_create(struct scsi_qla_host *ha,
+				 struct dev_db_entry *fw_ddb_entry,
+				 uint16_t *idx, int user)
+{
+	struct iscsi_bus_flash_session *fnode_sess = NULL;
+	struct iscsi_bus_flash_conn *fnode_conn = NULL;
+	int rc = QLA_ERROR;
+
+	fnode_sess = iscsi_create_flashnode_sess(ha->host, *idx,
+						 &qla4xxx_iscsi_transport, 0);
+	if (!fnode_sess) {
+		ql4_printk(KERN_ERR, ha,
+			   "%s: Unable to create session sysfs entry for flashnode %d of host%lu\n",
+			   __func__, *idx, ha->host_no);
+		goto exit_tgt_create;
+	}
+
+	fnode_conn = iscsi_create_flashnode_conn(ha->host, fnode_sess,
+						 &qla4xxx_iscsi_transport, 0);
+	if (!fnode_conn) {
+		ql4_printk(KERN_ERR, ha,
+			   "%s: Unable to create conn sysfs entry for flashnode %d of host%lu\n",
+			   __func__, *idx, ha->host_no);
+		goto free_sess;
+	}
+
+	if (user) {
+		fnode_sess->flash_state = DEV_DB_NON_PERSISTENT;
+	} else {
+		fnode_sess->flash_state = DEV_DB_PERSISTENT;
+
+		if (*idx == ha->pri_ddb_idx || *idx == ha->sec_ddb_idx)
+			fnode_sess->is_boot_target = 1;
+		else
+			fnode_sess->is_boot_target = 0;
+	}
+
+	rc = qla4xxx_copy_from_fwddb_param(fnode_sess, fnode_conn,
+					   fw_ddb_entry);
+
+	ql4_printk(KERN_INFO, ha, "%s: sysfs entry %s created\n",
+		   __func__, fnode_sess->dev.kobj.name);
+
+	ql4_printk(KERN_INFO, ha, "%s: sysfs entry %s created\n",
+		   __func__, fnode_conn->dev.kobj.name);
+
+	return QLA_SUCCESS;
+
+free_sess:
+	iscsi_destroy_flashnode_sess(fnode_sess);
+
+exit_tgt_create:
+	return QLA_ERROR;
+}
+
+/**
+ * qla4xxx_sysfs_ddb_add - Add new ddb entry in flash
+ * @shost: pointer to host
+ * @buf: type of ddb entry (ipv4/ipv6)
+ * @len: length of buf
+ *
+ * This creates new ddb entry in the flash by finding first free index and
+ * storing default ddb there. And then create sysfs entry for the new ddb entry.
+ **/
+static int qla4xxx_sysfs_ddb_add(struct Scsi_Host *shost, const char *buf,
+				 int len)
+{
+	struct scsi_qla_host *ha = to_qla_host(shost);
+	struct dev_db_entry *fw_ddb_entry = NULL;
+	dma_addr_t fw_ddb_entry_dma;
+	struct device *dev;
+	uint16_t idx = 0;
+	uint16_t max_ddbs = 0;
+	uint32_t options = 0;
+	uint32_t rval = QLA_ERROR;
+
+	if (strncasecmp(PORTAL_TYPE_IPV4, buf, 4) &&
+	    strncasecmp(PORTAL_TYPE_IPV6, buf, 4)) {
+		DEBUG2(ql4_printk(KERN_ERR, ha, "%s: Invalid portal type\n",
+				  __func__));
+		goto exit_ddb_add;
+	}
+
+	max_ddbs =  is_qla40XX(ha) ? MAX_PRST_DEV_DB_ENTRIES :
+				     MAX_DEV_DB_ENTRIES;
+
+	fw_ddb_entry = dma_alloc_coherent(&ha->pdev->dev, sizeof(*fw_ddb_entry),
+					  &fw_ddb_entry_dma, GFP_KERNEL);
+	if (!fw_ddb_entry) {
+		DEBUG2(ql4_printk(KERN_ERR, ha,
+				  "%s: Unable to allocate dma buffer\n",
+				  __func__));
+		goto exit_ddb_add;
+	}
+
+	dev = iscsi_find_flashnode_sess(ha->host, NULL,
+					qla4xxx_sysfs_ddb_is_non_persistent);
+	if (dev) {
+		ql4_printk(KERN_ERR, ha,
+			   "%s: A non-persistent entry %s found\n",
+			   __func__, dev->kobj.name);
+		goto exit_ddb_add;
+	}
+
+	for (idx = 0; idx < max_ddbs; idx++) {
+		if (qla4xxx_flashdb_by_index(ha, fw_ddb_entry,
+					     fw_ddb_entry_dma, idx))
+			break;
+	}
+
+	if (idx == max_ddbs)
+		goto exit_ddb_add;
+
+	if (!strncasecmp("ipv6", buf, 4))
+		options |= IPV6_DEFAULT_DDB_ENTRY;
+
+	rval = qla4xxx_get_default_ddb(ha, options, fw_ddb_entry_dma);
+	if (rval == QLA_ERROR)
+		goto exit_ddb_add;
+
+	rval = qla4xxx_sysfs_ddb_tgt_create(ha, fw_ddb_entry, &idx, 1);
+
+exit_ddb_add:
+	if (fw_ddb_entry)
+		dma_free_coherent(&ha->pdev->dev, sizeof(*fw_ddb_entry),
+				  fw_ddb_entry, fw_ddb_entry_dma);
+	if (rval == QLA_SUCCESS)
+		return idx;
+	else
+		return -EIO;
+}
+
+/**
+ * qla4xxx_sysfs_ddb_apply - write the target ddb contents to Flash
+ * @fnode_sess: pointer to session attrs of flash ddb entry
+ * @fnode_conn: pointer to connection attrs of flash ddb entry
+ *
+ * This writes the contents of target ddb buffer to Flash with a valid cookie
+ * value in order to make the ddb entry persistent.
+ **/
+static int  qla4xxx_sysfs_ddb_apply(struct iscsi_bus_flash_session *fnode_sess,
+				    struct iscsi_bus_flash_conn *fnode_conn)
+{
+	struct Scsi_Host *shost = iscsi_flash_session_to_shost(fnode_sess);
+	struct scsi_qla_host *ha = to_qla_host(shost);
+	uint32_t dev_db_start_offset = FLASH_OFFSET_DB_INFO;
+	struct dev_db_entry *fw_ddb_entry = NULL;
+	dma_addr_t fw_ddb_entry_dma;
+	uint32_t options = 0;
+	int rval = 0;
+
+	fw_ddb_entry = dma_alloc_coherent(&ha->pdev->dev, sizeof(*fw_ddb_entry),
+					  &fw_ddb_entry_dma, GFP_KERNEL);
+	if (!fw_ddb_entry) {
+		DEBUG2(ql4_printk(KERN_ERR, ha,
+				  "%s: Unable to allocate dma buffer\n",
+				  __func__));
+		rval = -ENOMEM;
+		goto exit_ddb_apply;
+	}
+
+	if (!strncasecmp(fnode_sess->portal_type, PORTAL_TYPE_IPV6, 4))
+		options |= IPV6_DEFAULT_DDB_ENTRY;
+
+	rval = qla4xxx_get_default_ddb(ha, options, fw_ddb_entry_dma);
+	if (rval == QLA_ERROR)
+		goto exit_ddb_apply;
+
+	dev_db_start_offset += (fnode_sess->target_id *
+				sizeof(*fw_ddb_entry));
+
+	qla4xxx_copy_to_fwddb_param(fnode_sess, fnode_conn, fw_ddb_entry);
+	fw_ddb_entry->cookie = DDB_VALID_COOKIE;
+
+	rval = qla4xxx_set_flash(ha, fw_ddb_entry_dma, dev_db_start_offset,
+				 sizeof(*fw_ddb_entry), FLASH_OPT_RMW_COMMIT);
+
+	if (rval == QLA_SUCCESS) {
+		fnode_sess->flash_state = DEV_DB_PERSISTENT;
+		ql4_printk(KERN_INFO, ha,
+			   "%s: flash node %u of host %lu written to flash\n",
+			   __func__, fnode_sess->target_id, ha->host_no);
+	} else {
+		rval = -EIO;
+		ql4_printk(KERN_ERR, ha,
+			   "%s: Error while writing flash node %u of host %lu to flash\n",
+			   __func__, fnode_sess->target_id, ha->host_no);
+	}
+
+exit_ddb_apply:
+	if (fw_ddb_entry)
+		dma_free_coherent(&ha->pdev->dev, sizeof(*fw_ddb_entry),
+				  fw_ddb_entry, fw_ddb_entry_dma);
+	return rval;
+}
+
+static ssize_t qla4xxx_sysfs_ddb_conn_open(struct scsi_qla_host *ha,
+					   struct dev_db_entry *fw_ddb_entry,
+					   uint16_t idx)
+{
+	struct dev_db_entry *ddb_entry = NULL;
+	dma_addr_t ddb_entry_dma;
+	unsigned long wtime;
+	uint32_t mbx_sts = 0;
+	uint32_t state = 0, conn_err = 0;
+	uint16_t tmo = 0;
+	int ret = 0;
+
+	ddb_entry = dma_alloc_coherent(&ha->pdev->dev, sizeof(*ddb_entry),
+				       &ddb_entry_dma, GFP_KERNEL);
+	if (!ddb_entry) {
+		DEBUG2(ql4_printk(KERN_ERR, ha,
+				  "%s: Unable to allocate dma buffer\n",
+				  __func__));
+		return QLA_ERROR;
+	}
+
+	memcpy(ddb_entry, fw_ddb_entry, sizeof(*ddb_entry));
+
+	ret = qla4xxx_set_ddb_entry(ha, idx, ddb_entry_dma, &mbx_sts);
+	if (ret != QLA_SUCCESS) {
+		DEBUG2(ql4_printk(KERN_ERR, ha,
+				  "%s: Unable to set ddb entry for index %d\n",
+				  __func__, idx));
+		goto exit_ddb_conn_open;
+	}
+
+	qla4xxx_conn_open(ha, idx);
+
+	/* To ensure that sendtargets is done, wait for at least 12 secs */
+	tmo = ((ha->def_timeout > LOGIN_TOV) &&
+	       (ha->def_timeout < LOGIN_TOV * 10) ?
+	       ha->def_timeout : LOGIN_TOV);
+
+	DEBUG2(ql4_printk(KERN_INFO, ha,
+			  "Default time to wait for login to ddb %d\n", tmo));
+
+	wtime = jiffies + (HZ * tmo);
+	do {
+		ret = qla4xxx_get_fwddb_entry(ha, idx, NULL, 0, NULL,
+					      NULL, &state, &conn_err, NULL,
+					      NULL);
+		if (ret == QLA_ERROR)
+			continue;
+
+		if (state == DDB_DS_NO_CONNECTION_ACTIVE ||
+		    state == DDB_DS_SESSION_FAILED)
+			break;
+
+		schedule_timeout_uninterruptible(HZ / 10);
+	} while (time_after(wtime, jiffies));
+
+exit_ddb_conn_open:
+	if (ddb_entry)
+		dma_free_coherent(&ha->pdev->dev, sizeof(*ddb_entry),
+				  ddb_entry, ddb_entry_dma);
+	return ret;
+}
+
+static int qla4xxx_ddb_login_st(struct scsi_qla_host *ha,
+				struct dev_db_entry *fw_ddb_entry)
+{
+	struct qla_ddb_index *ddb_idx, *ddb_idx_tmp;
+	struct list_head list_nt;
+	uint16_t ddb_index;
+	int ret = 0;
+
+	if (test_bit(AF_ST_DISCOVERY_IN_PROGRESS, &ha->flags)) {
+		ql4_printk(KERN_WARNING, ha,
+			   "%s: A discovery already in progress!\n", __func__);
+		return QLA_ERROR;
+	}
+
+	INIT_LIST_HEAD(&list_nt);
+
+	set_bit(AF_ST_DISCOVERY_IN_PROGRESS, &ha->flags);
+
+	ret = qla4xxx_get_ddb_index(ha, &ddb_index);
+	if (ret == QLA_ERROR)
+		goto exit_login_st_clr_bit;
+
+	ret = qla4xxx_sysfs_ddb_conn_open(ha, fw_ddb_entry, ddb_index);
+	if (ret == QLA_ERROR)
+		goto exit_login_st;
+
+	qla4xxx_build_new_nt_list(ha, &list_nt);
+
+	list_for_each_entry_safe(ddb_idx, ddb_idx_tmp, &list_nt, list) {
+		list_del_init(&ddb_idx->list);
+		qla4xxx_clear_ddb_entry(ha, ddb_idx->fw_ddb_idx);
+		vfree(ddb_idx);
+	}
+
+exit_login_st:
+	if (qla4xxx_clear_ddb_entry(ha, ddb_index) == QLA_ERROR) {
+		ql4_printk(KERN_ERR, ha,
+			   "Unable to clear DDB index = 0x%x\n", ddb_index);
+	}
+
+	clear_bit(ddb_index, ha->ddb_idx_map);
+
+exit_login_st_clr_bit:
+	clear_bit(AF_ST_DISCOVERY_IN_PROGRESS, &ha->flags);
+	return ret;
+}
+
+static int qla4xxx_ddb_login_nt(struct scsi_qla_host *ha,
+				struct dev_db_entry *fw_ddb_entry,
+				uint16_t idx)
+{
+	int ret = QLA_ERROR;
+
+	ret = qla4xxx_is_session_exists(ha, fw_ddb_entry);
+	if (ret != QLA_SUCCESS)
+		ret = qla4xxx_sess_conn_setup(ha, fw_ddb_entry, RESET_ADAPTER,
+					      idx);
+	else
+		ret = -EPERM;
+
+	return ret;
+}
+
+/**
+ * qla4xxx_sysfs_ddb_login - Login to the specified target
+ * @fnode_sess: pointer to session attrs of flash ddb entry
+ * @fnode_conn: pointer to connection attrs of flash ddb entry
+ *
+ * This logs in to the specified target
+ **/
+static int qla4xxx_sysfs_ddb_login(struct iscsi_bus_flash_session *fnode_sess,
+				   struct iscsi_bus_flash_conn *fnode_conn)
+{
+	struct Scsi_Host *shost = iscsi_flash_session_to_shost(fnode_sess);
+	struct scsi_qla_host *ha = to_qla_host(shost);
+	struct dev_db_entry *fw_ddb_entry = NULL;
+	dma_addr_t fw_ddb_entry_dma;
+	uint32_t options = 0;
+	int ret = 0;
+
+	if (fnode_sess->flash_state == DEV_DB_NON_PERSISTENT) {
+		ql4_printk(KERN_ERR, ha,
+			   "%s: Target info is not persistent\n", __func__);
+		ret = -EIO;
+		goto exit_ddb_login;
+	}
+
+	fw_ddb_entry = dma_alloc_coherent(&ha->pdev->dev, sizeof(*fw_ddb_entry),
+					  &fw_ddb_entry_dma, GFP_KERNEL);
+	if (!fw_ddb_entry) {
+		DEBUG2(ql4_printk(KERN_ERR, ha,
+				  "%s: Unable to allocate dma buffer\n",
+				  __func__));
+		ret = -ENOMEM;
+		goto exit_ddb_login;
+	}
+
+	if (!strncasecmp(fnode_sess->portal_type, PORTAL_TYPE_IPV6, 4))
+		options |= IPV6_DEFAULT_DDB_ENTRY;
+
+	ret = qla4xxx_get_default_ddb(ha, options, fw_ddb_entry_dma);
+	if (ret == QLA_ERROR)
+		goto exit_ddb_login;
+
+	qla4xxx_copy_to_fwddb_param(fnode_sess, fnode_conn, fw_ddb_entry);
+	fw_ddb_entry->cookie = DDB_VALID_COOKIE;
+
+	if (strlen((char *)fw_ddb_entry->iscsi_name) == 0)
+		ret = qla4xxx_ddb_login_st(ha, fw_ddb_entry);
+	else
+		ret = qla4xxx_ddb_login_nt(ha, fw_ddb_entry,
+					   fnode_sess->target_id);
+
+	if (ret > 0)
+		ret = -EIO;
+
+exit_ddb_login:
+	if (fw_ddb_entry)
+		dma_free_coherent(&ha->pdev->dev, sizeof(*fw_ddb_entry),
+				  fw_ddb_entry, fw_ddb_entry_dma);
+	return ret;
+}
+
+/**
+ * qla4xxx_sysfs_ddb_logout_sid - Logout session for the specified target
+ * @cls_sess: pointer to session to be logged out
+ *
+ * This performs session log out from the specified target
+ **/
+static int qla4xxx_sysfs_ddb_logout_sid(struct iscsi_cls_session *cls_sess)
+{
+	struct iscsi_session *sess;
+	struct ddb_entry *ddb_entry = NULL;
+	struct scsi_qla_host *ha;
+	struct dev_db_entry *fw_ddb_entry = NULL;
+	dma_addr_t fw_ddb_entry_dma;
+	unsigned long flags;
+	unsigned long wtime;
+	uint32_t ddb_state;
+	int options;
+	int ret = 0;
+
+	sess = cls_sess->dd_data;
+	ddb_entry = sess->dd_data;
+	ha = ddb_entry->ha;
+
+	if (ddb_entry->ddb_type != FLASH_DDB) {
+		ql4_printk(KERN_ERR, ha, "%s: Not a flash node session\n",
+			   __func__);
+		ret = -ENXIO;
+		goto exit_ddb_logout;
+	}
+
+	if (test_bit(DF_BOOT_TGT, &ddb_entry->flags)) {
+		ql4_printk(KERN_ERR, ha,
+			   "%s: Logout from boot target entry is not permitted.\n",
+			   __func__);
+		ret = -EPERM;
+		goto exit_ddb_logout;
+	}
+
+	options = LOGOUT_OPTION_CLOSE_SESSION;
+	if (qla4xxx_session_logout_ddb(ha, ddb_entry, options) == QLA_ERROR) {
+		ql4_printk(KERN_ERR, ha, "%s: Logout failed\n", __func__);
+		ret = -EIO;
+		goto exit_ddb_logout;
+	}
+
+	fw_ddb_entry = dma_alloc_coherent(&ha->pdev->dev, sizeof(*fw_ddb_entry),
+					  &fw_ddb_entry_dma, GFP_KERNEL);
+	if (!fw_ddb_entry) {
+		ql4_printk(KERN_ERR, ha,
+			   "%s: Unable to allocate dma buffer\n", __func__);
+		ret = -ENOMEM;
+		goto exit_ddb_logout;
+	}
+
+	wtime = jiffies + (HZ * LOGOUT_TOV);
+	do {
+		ret = qla4xxx_get_fwddb_entry(ha, ddb_entry->fw_ddb_index,
+					      fw_ddb_entry, fw_ddb_entry_dma,
+					      NULL, NULL, &ddb_state, NULL,
+					      NULL, NULL);
+		if (ret == QLA_ERROR)
+			goto ddb_logout_clr_sess;
+
+		if ((ddb_state == DDB_DS_NO_CONNECTION_ACTIVE) ||
+		    (ddb_state == DDB_DS_SESSION_FAILED))
+			goto ddb_logout_clr_sess;
+
+		schedule_timeout_uninterruptible(HZ);
+	} while ((time_after(wtime, jiffies)));
+
+ddb_logout_clr_sess:
+	qla4xxx_clear_ddb_entry(ha, ddb_entry->fw_ddb_index);
+	/*
+	 * we have decremented the reference count of the driver
+	 * when we setup the session to have the driver unload
+	 * to be seamless without actually destroying the
+	 * session
+	 **/
+	try_module_get(qla4xxx_iscsi_transport.owner);
+	iscsi_destroy_endpoint(ddb_entry->conn->ep);
+
+	spin_lock_irqsave(&ha->hardware_lock, flags);
+	qla4xxx_free_ddb(ha, ddb_entry);
+	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+
+	iscsi_session_teardown(ddb_entry->sess);
+
+	ret = QLA_SUCCESS;
+
+exit_ddb_logout:
+	if (fw_ddb_entry)
+		dma_free_coherent(&ha->pdev->dev, sizeof(*fw_ddb_entry),
+				  fw_ddb_entry, fw_ddb_entry_dma);
+	return ret;
+}
+
+/**
+ * qla4xxx_sysfs_ddb_logout - Logout from the specified target
+ * @fnode_sess: pointer to session attrs of flash ddb entry
+ * @fnode_conn: pointer to connection attrs of flash ddb entry
+ *
+ * This performs log out from the specified target
+ **/
+static int qla4xxx_sysfs_ddb_logout(struct iscsi_bus_flash_session *fnode_sess,
+				    struct iscsi_bus_flash_conn *fnode_conn)
+{
+	struct Scsi_Host *shost = iscsi_flash_session_to_shost(fnode_sess);
+	struct scsi_qla_host *ha = to_qla_host(shost);
+	struct ql4_tuple_ddb *flash_tddb = NULL;
+	struct ql4_tuple_ddb *tmp_tddb = NULL;
+	struct dev_db_entry *fw_ddb_entry = NULL;
+	struct ddb_entry *ddb_entry = NULL;
+	dma_addr_t fw_ddb_dma;
+	uint32_t next_idx = 0;
+	uint32_t state = 0, conn_err = 0;
+	uint16_t conn_id = 0;
+	int idx, index;
+	int status, ret = 0;
+
+	fw_ddb_entry = dma_pool_alloc(ha->fw_ddb_dma_pool, GFP_KERNEL,
+				      &fw_ddb_dma);
+	if (fw_ddb_entry == NULL) {
+		ql4_printk(KERN_ERR, ha, "%s:Out of memory\n", __func__);
+		ret = -ENOMEM;
+		goto exit_ddb_logout;
+	}
+
+	flash_tddb = vzalloc(sizeof(*flash_tddb));
+	if (!flash_tddb) {
+		ql4_printk(KERN_WARNING, ha,
+			   "%s:Memory Allocation failed.\n", __func__);
+		ret = -ENOMEM;
+		goto exit_ddb_logout;
+	}
+
+	tmp_tddb = vzalloc(sizeof(*tmp_tddb));
+	if (!tmp_tddb) {
+		ql4_printk(KERN_WARNING, ha,
+			   "%s:Memory Allocation failed.\n", __func__);
+		ret = -ENOMEM;
+		goto exit_ddb_logout;
+	}
+
+	if (!fnode_sess->targetname) {
+		ql4_printk(KERN_ERR, ha,
+			   "%s:Cannot logout from SendTarget entry\n",
+			   __func__);
+		ret = -EPERM;
+		goto exit_ddb_logout;
+	}
+
+	if (fnode_sess->is_boot_target) {
+		ql4_printk(KERN_ERR, ha,
+			   "%s: Logout from boot target entry is not permitted.\n",
+			   __func__);
+		ret = -EPERM;
+		goto exit_ddb_logout;
+	}
+
+	strncpy(flash_tddb->iscsi_name, fnode_sess->targetname,
+		ISCSI_NAME_SIZE);
+
+	if (!strncmp(fnode_sess->portal_type, PORTAL_TYPE_IPV6, 4))
+		sprintf(flash_tddb->ip_addr, "%pI6", fnode_conn->ipaddress);
+	else
+		sprintf(flash_tddb->ip_addr, "%pI4", fnode_conn->ipaddress);
+
+	flash_tddb->tpgt = fnode_sess->tpgt;
+	flash_tddb->port = fnode_conn->port;
+
+	COPY_ISID(flash_tddb->isid, fnode_sess->isid);
+
+	for (idx = 0; idx < MAX_DDB_ENTRIES; idx++) {
+		ddb_entry = qla4xxx_lookup_ddb_by_fw_index(ha, idx);
+		if (ddb_entry == NULL)
+			continue;
+
+		if (ddb_entry->ddb_type != FLASH_DDB)
+			continue;
+
+		index = ddb_entry->sess->target_id;
+		status = qla4xxx_get_fwddb_entry(ha, index, fw_ddb_entry,
+						 fw_ddb_dma, NULL, &next_idx,
+						 &state, &conn_err, NULL,
+						 &conn_id);
+		if (status == QLA_ERROR) {
+			ret = -ENOMEM;
+			break;
+		}
+
+		qla4xxx_convert_param_ddb(fw_ddb_entry, tmp_tddb, NULL);
+
+		status = qla4xxx_compare_tuple_ddb(ha, flash_tddb, tmp_tddb,
+						   true);
+		if (status == QLA_SUCCESS) {
+			ret = qla4xxx_sysfs_ddb_logout_sid(ddb_entry->sess);
+			break;
+		}
+	}
+
+	if (idx == MAX_DDB_ENTRIES)
+		ret = -ESRCH;
+
+exit_ddb_logout:
+	if (flash_tddb)
+		vfree(flash_tddb);
+	if (tmp_tddb)
+		vfree(tmp_tddb);
+	if (fw_ddb_entry)
+		dma_pool_free(ha->fw_ddb_dma_pool, fw_ddb_entry, fw_ddb_dma);
+
+	return ret;
+}
+
+static int
+qla4xxx_sysfs_ddb_get_param(struct iscsi_bus_flash_session *fnode_sess,
+			    int param, char *buf)
+{
+	struct Scsi_Host *shost = iscsi_flash_session_to_shost(fnode_sess);
+	struct scsi_qla_host *ha = to_qla_host(shost);
+	struct iscsi_bus_flash_conn *fnode_conn;
+	struct ql4_chap_table chap_tbl;
+	struct device *dev;
+	int parent_type, parent_index = 0xffff;
+	int rc = 0;
+
+	dev = iscsi_find_flashnode_conn(fnode_sess, NULL,
+					iscsi_is_flashnode_conn_dev);
+	if (!dev)
+		return -EIO;
+
+	fnode_conn = iscsi_dev_to_flash_conn(dev);
+
+	switch (param) {
+	case ISCSI_FLASHNODE_IS_FW_ASSIGNED_IPV6:
+		rc = sprintf(buf, "%u\n", fnode_conn->is_fw_assigned_ipv6);
+		break;
+	case ISCSI_FLASHNODE_PORTAL_TYPE:
+		rc = sprintf(buf, "%s\n", fnode_sess->portal_type);
+		break;
+	case ISCSI_FLASHNODE_AUTO_SND_TGT_DISABLE:
+		rc = sprintf(buf, "%u\n", fnode_sess->auto_snd_tgt_disable);
+		break;
+	case ISCSI_FLASHNODE_DISCOVERY_SESS:
+		rc = sprintf(buf, "%u\n", fnode_sess->discovery_sess);
+		break;
+	case ISCSI_FLASHNODE_ENTRY_EN:
+		rc = sprintf(buf, "%u\n", fnode_sess->entry_state);
+		break;
+	case ISCSI_FLASHNODE_HDR_DGST_EN:
+		rc = sprintf(buf, "%u\n", fnode_conn->hdrdgst_en);
+		break;
+	case ISCSI_FLASHNODE_DATA_DGST_EN:
+		rc = sprintf(buf, "%u\n", fnode_conn->datadgst_en);
+		break;
+	case ISCSI_FLASHNODE_IMM_DATA_EN:
+		rc = sprintf(buf, "%u\n", fnode_sess->imm_data_en);
+		break;
+	case ISCSI_FLASHNODE_INITIAL_R2T_EN:
+		rc = sprintf(buf, "%u\n", fnode_sess->initial_r2t_en);
+		break;
+	case ISCSI_FLASHNODE_DATASEQ_INORDER:
+		rc = sprintf(buf, "%u\n", fnode_sess->dataseq_inorder_en);
+		break;
+	case ISCSI_FLASHNODE_PDU_INORDER:
+		rc = sprintf(buf, "%u\n", fnode_sess->pdu_inorder_en);
+		break;
+	case ISCSI_FLASHNODE_CHAP_AUTH_EN:
+		rc = sprintf(buf, "%u\n", fnode_sess->chap_auth_en);
+		break;
+	case ISCSI_FLASHNODE_SNACK_REQ_EN:
+		rc = sprintf(buf, "%u\n", fnode_conn->snack_req_en);
+		break;
+	case ISCSI_FLASHNODE_DISCOVERY_LOGOUT_EN:
+		rc = sprintf(buf, "%u\n", fnode_sess->discovery_logout_en);
+		break;
+	case ISCSI_FLASHNODE_BIDI_CHAP_EN:
+		rc = sprintf(buf, "%u\n", fnode_sess->bidi_chap_en);
+		break;
+	case ISCSI_FLASHNODE_DISCOVERY_AUTH_OPTIONAL:
+		rc = sprintf(buf, "%u\n", fnode_sess->discovery_auth_optional);
+		break;
+	case ISCSI_FLASHNODE_ERL:
+		rc = sprintf(buf, "%u\n", fnode_sess->erl);
+		break;
+	case ISCSI_FLASHNODE_TCP_TIMESTAMP_STAT:
+		rc = sprintf(buf, "%u\n", fnode_conn->tcp_timestamp_stat);
+		break;
+	case ISCSI_FLASHNODE_TCP_NAGLE_DISABLE:
+		rc = sprintf(buf, "%u\n", fnode_conn->tcp_nagle_disable);
+		break;
+	case ISCSI_FLASHNODE_TCP_WSF_DISABLE:
+		rc = sprintf(buf, "%u\n", fnode_conn->tcp_wsf_disable);
+		break;
+	case ISCSI_FLASHNODE_TCP_TIMER_SCALE:
+		rc = sprintf(buf, "%u\n", fnode_conn->tcp_timer_scale);
+		break;
+	case ISCSI_FLASHNODE_TCP_TIMESTAMP_EN:
+		rc = sprintf(buf, "%u\n", fnode_conn->tcp_timestamp_en);
+		break;
+	case ISCSI_FLASHNODE_IP_FRAG_DISABLE:
+		rc = sprintf(buf, "%u\n", fnode_conn->fragment_disable);
+		break;
+	case ISCSI_FLASHNODE_MAX_RECV_DLENGTH:
+		rc = sprintf(buf, "%u\n", fnode_conn->max_recv_dlength);
+		break;
+	case ISCSI_FLASHNODE_MAX_XMIT_DLENGTH:
+		rc = sprintf(buf, "%u\n", fnode_conn->max_xmit_dlength);
+		break;
+	case ISCSI_FLASHNODE_FIRST_BURST:
+		rc = sprintf(buf, "%u\n", fnode_sess->first_burst);
+		break;
+	case ISCSI_FLASHNODE_DEF_TIME2WAIT:
+		rc = sprintf(buf, "%u\n", fnode_sess->time2wait);
+		break;
+	case ISCSI_FLASHNODE_DEF_TIME2RETAIN:
+		rc = sprintf(buf, "%u\n", fnode_sess->time2retain);
+		break;
+	case ISCSI_FLASHNODE_MAX_R2T:
+		rc = sprintf(buf, "%u\n", fnode_sess->max_r2t);
+		break;
+	case ISCSI_FLASHNODE_KEEPALIVE_TMO:
+		rc = sprintf(buf, "%u\n", fnode_conn->keepalive_timeout);
+		break;
+	case ISCSI_FLASHNODE_ISID:
+		rc = sprintf(buf, "%02x%02x%02x%02x%02x%02x\n",
+			     fnode_sess->isid[0], fnode_sess->isid[1],
+			     fnode_sess->isid[2], fnode_sess->isid[3],
+			     fnode_sess->isid[4], fnode_sess->isid[5]);
+		break;
+	case ISCSI_FLASHNODE_TSID:
+		rc = sprintf(buf, "%u\n", fnode_sess->tsid);
+		break;
+	case ISCSI_FLASHNODE_PORT:
+		rc = sprintf(buf, "%d\n", fnode_conn->port);
+		break;
+	case ISCSI_FLASHNODE_MAX_BURST:
+		rc = sprintf(buf, "%u\n", fnode_sess->max_burst);
+		break;
+	case ISCSI_FLASHNODE_DEF_TASKMGMT_TMO:
+		rc = sprintf(buf, "%u\n",
+			     fnode_sess->default_taskmgmt_timeout);
+		break;
+	case ISCSI_FLASHNODE_IPADDR:
+		if (!strncmp(fnode_sess->portal_type, PORTAL_TYPE_IPV6, 4))
+			rc = sprintf(buf, "%pI6\n", fnode_conn->ipaddress);
+		else
+			rc = sprintf(buf, "%pI4\n", fnode_conn->ipaddress);
+		break;
+	case ISCSI_FLASHNODE_ALIAS:
+		if (fnode_sess->targetalias)
+			rc = sprintf(buf, "%s\n", fnode_sess->targetalias);
+		else
+			rc = sprintf(buf, "\n");
+		break;
+	case ISCSI_FLASHNODE_REDIRECT_IPADDR:
+		if (!strncmp(fnode_sess->portal_type, PORTAL_TYPE_IPV6, 4))
+			rc = sprintf(buf, "%pI6\n",
+				     fnode_conn->redirect_ipaddr);
+		else
+			rc = sprintf(buf, "%pI4\n",
+				     fnode_conn->redirect_ipaddr);
+		break;
+	case ISCSI_FLASHNODE_MAX_SEGMENT_SIZE:
+		rc = sprintf(buf, "%u\n", fnode_conn->max_segment_size);
+		break;
+	case ISCSI_FLASHNODE_LOCAL_PORT:
+		rc = sprintf(buf, "%u\n", fnode_conn->local_port);
+		break;
+	case ISCSI_FLASHNODE_IPV4_TOS:
+		rc = sprintf(buf, "%u\n", fnode_conn->ipv4_tos);
+		break;
+	case ISCSI_FLASHNODE_IPV6_TC:
+		if (!strncmp(fnode_sess->portal_type, PORTAL_TYPE_IPV6, 4))
+			rc = sprintf(buf, "%u\n",
+				     fnode_conn->ipv6_traffic_class);
+		else
+			rc = sprintf(buf, "\n");
+		break;
+	case ISCSI_FLASHNODE_IPV6_FLOW_LABEL:
+		rc = sprintf(buf, "%u\n", fnode_conn->ipv6_flow_label);
+		break;
+	case ISCSI_FLASHNODE_LINK_LOCAL_IPV6:
+		if (!strncmp(fnode_sess->portal_type, PORTAL_TYPE_IPV6, 4))
+			rc = sprintf(buf, "%pI6\n",
+				     fnode_conn->link_local_ipv6_addr);
+		else
+			rc = sprintf(buf, "\n");
+		break;
+	case ISCSI_FLASHNODE_DISCOVERY_PARENT_IDX:
+		if ((fnode_sess->discovery_parent_idx) >= 0  &&
+		    (fnode_sess->discovery_parent_idx < MAX_DDB_ENTRIES))
+			parent_index = fnode_sess->discovery_parent_idx;
+
+		rc = sprintf(buf, "%u\n", parent_index);
+		break;
+	case ISCSI_FLASHNODE_DISCOVERY_PARENT_TYPE:
+		if (fnode_sess->discovery_parent_type == DDB_ISNS)
+			parent_type = ISCSI_DISC_PARENT_ISNS;
+		else if (fnode_sess->discovery_parent_type == DDB_NO_LINK)
+			parent_type = ISCSI_DISC_PARENT_UNKNOWN;
+		else if (fnode_sess->discovery_parent_type >= 0  &&
+			 fnode_sess->discovery_parent_type < MAX_DDB_ENTRIES)
+			parent_type = ISCSI_DISC_PARENT_SENDTGT;
+		else
+			parent_type = ISCSI_DISC_PARENT_UNKNOWN;
+
+		rc = sprintf(buf, "%s\n",
+			     iscsi_get_discovery_parent_name(parent_type));
+		break;
+	case ISCSI_FLASHNODE_NAME:
+		if (fnode_sess->targetname)
+			rc = sprintf(buf, "%s\n", fnode_sess->targetname);
+		else
+			rc = sprintf(buf, "\n");
+		break;
+	case ISCSI_FLASHNODE_TPGT:
+		rc = sprintf(buf, "%u\n", fnode_sess->tpgt);
+		break;
+	case ISCSI_FLASHNODE_TCP_XMIT_WSF:
+		rc = sprintf(buf, "%u\n", fnode_conn->tcp_xmit_wsf);
+		break;
+	case ISCSI_FLASHNODE_TCP_RECV_WSF:
+		rc = sprintf(buf, "%u\n", fnode_conn->tcp_recv_wsf);
+		break;
+	case ISCSI_FLASHNODE_CHAP_OUT_IDX:
+		rc = sprintf(buf, "%u\n", fnode_sess->chap_out_idx);
+		break;
+	case ISCSI_FLASHNODE_USERNAME:
+		if (fnode_sess->chap_auth_en) {
+			qla4xxx_get_uni_chap_at_index(ha,
+						      chap_tbl.name,
+						      chap_tbl.secret,
+						      fnode_sess->chap_out_idx);
+			rc = sprintf(buf, "%s\n", chap_tbl.name);
+		} else {
+			rc = sprintf(buf, "\n");
+		}
+		break;
+	case ISCSI_FLASHNODE_PASSWORD:
+		if (fnode_sess->chap_auth_en) {
+			qla4xxx_get_uni_chap_at_index(ha,
+						      chap_tbl.name,
+						      chap_tbl.secret,
+						      fnode_sess->chap_out_idx);
+			rc = sprintf(buf, "%s\n", chap_tbl.secret);
+		} else {
+			rc = sprintf(buf, "\n");
+		}
+		break;
+	case ISCSI_FLASHNODE_STATSN:
+		rc = sprintf(buf, "%u\n", fnode_conn->statsn);
+		break;
+	case ISCSI_FLASHNODE_EXP_STATSN:
+		rc = sprintf(buf, "%u\n", fnode_conn->exp_statsn);
+		break;
+	case ISCSI_FLASHNODE_IS_BOOT_TGT:
+		rc = sprintf(buf, "%u\n", fnode_sess->is_boot_target);
+		break;
+	default:
+		rc = -ENOSYS;
+		break;
+	}
+	return rc;
+}
+
+/**
+ * qla4xxx_sysfs_ddb_set_param - Set parameter for firmware DDB entry
+ * @fnode_sess: pointer to session attrs of flash ddb entry
+ * @fnode_conn: pointer to connection attrs of flash ddb entry
+ * @data: Parameters and their values to update
+ * @len: len of data
+ *
+ * This sets the parameter of flash ddb entry and writes them to flash
+ **/
+static int
+qla4xxx_sysfs_ddb_set_param(struct iscsi_bus_flash_session *fnode_sess,
+			    struct iscsi_bus_flash_conn *fnode_conn,
+			    void *data, int len)
+{
+	struct Scsi_Host *shost = iscsi_flash_session_to_shost(fnode_sess);
+	struct scsi_qla_host *ha = to_qla_host(shost);
+	struct dev_db_entry *fw_ddb_entry = NULL;
+	struct iscsi_flashnode_param_info *fnode_param;
+	struct nlattr *attr;
+	int rc = QLA_ERROR;
+	uint32_t rem = len;
+
+	fw_ddb_entry = kzalloc(sizeof(*fw_ddb_entry), GFP_KERNEL);
+	if (!fw_ddb_entry) {
+		DEBUG2(ql4_printk(KERN_ERR, ha,
+				  "%s: Unable to allocate ddb buffer\n",
+				  __func__));
+		return -ENOMEM;
+	}
+
+	nla_for_each_attr(attr, data, len, rem) {
+		fnode_param = nla_data(attr);
+
+		switch (fnode_param->param) {
+		case ISCSI_FLASHNODE_IS_FW_ASSIGNED_IPV6:
+			fnode_conn->is_fw_assigned_ipv6 = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_PORTAL_TYPE:
+			memcpy(fnode_sess->portal_type, fnode_param->value,
+			       strlen(fnode_sess->portal_type));
+			break;
+		case ISCSI_FLASHNODE_AUTO_SND_TGT_DISABLE:
+			fnode_sess->auto_snd_tgt_disable =
+							fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_DISCOVERY_SESS:
+			fnode_sess->discovery_sess = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_ENTRY_EN:
+			fnode_sess->entry_state = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_HDR_DGST_EN:
+			fnode_conn->hdrdgst_en = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_DATA_DGST_EN:
+			fnode_conn->datadgst_en = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_IMM_DATA_EN:
+			fnode_sess->imm_data_en = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_INITIAL_R2T_EN:
+			fnode_sess->initial_r2t_en = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_DATASEQ_INORDER:
+			fnode_sess->dataseq_inorder_en = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_PDU_INORDER:
+			fnode_sess->pdu_inorder_en = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_CHAP_AUTH_EN:
+			fnode_sess->chap_auth_en = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_SNACK_REQ_EN:
+			fnode_conn->snack_req_en = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_DISCOVERY_LOGOUT_EN:
+			fnode_sess->discovery_logout_en = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_BIDI_CHAP_EN:
+			fnode_sess->bidi_chap_en = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_DISCOVERY_AUTH_OPTIONAL:
+			fnode_sess->discovery_auth_optional =
+							fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_ERL:
+			fnode_sess->erl = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_TCP_TIMESTAMP_STAT:
+			fnode_conn->tcp_timestamp_stat = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_TCP_NAGLE_DISABLE:
+			fnode_conn->tcp_nagle_disable = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_TCP_WSF_DISABLE:
+			fnode_conn->tcp_wsf_disable = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_TCP_TIMER_SCALE:
+			fnode_conn->tcp_timer_scale = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_TCP_TIMESTAMP_EN:
+			fnode_conn->tcp_timestamp_en = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_IP_FRAG_DISABLE:
+			fnode_conn->fragment_disable = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_MAX_RECV_DLENGTH:
+			fnode_conn->max_recv_dlength =
+					*(unsigned *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_MAX_XMIT_DLENGTH:
+			fnode_conn->max_xmit_dlength =
+					*(unsigned *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_FIRST_BURST:
+			fnode_sess->first_burst =
+					*(unsigned *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_DEF_TIME2WAIT:
+			fnode_sess->time2wait = *(uint16_t *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_DEF_TIME2RETAIN:
+			fnode_sess->time2retain =
+						*(uint16_t *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_MAX_R2T:
+			fnode_sess->max_r2t =
+					*(uint16_t *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_KEEPALIVE_TMO:
+			fnode_conn->keepalive_timeout =
+				*(uint16_t *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_ISID:
+			memcpy(fnode_sess->isid, fnode_param->value,
+			       sizeof(fnode_sess->isid));
+			break;
+		case ISCSI_FLASHNODE_TSID:
+			fnode_sess->tsid = *(uint16_t *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_PORT:
+			fnode_conn->port = *(uint16_t *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_MAX_BURST:
+			fnode_sess->max_burst = *(unsigned *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_DEF_TASKMGMT_TMO:
+			fnode_sess->default_taskmgmt_timeout =
+						*(uint16_t *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_IPADDR:
+			memcpy(fnode_conn->ipaddress, fnode_param->value,
+			       IPv6_ADDR_LEN);
+			break;
+		case ISCSI_FLASHNODE_ALIAS:
+			rc = iscsi_switch_str_param(&fnode_sess->targetalias,
+						    (char *)fnode_param->value);
+			break;
+		case ISCSI_FLASHNODE_REDIRECT_IPADDR:
+			memcpy(fnode_conn->redirect_ipaddr, fnode_param->value,
+			       IPv6_ADDR_LEN);
+			break;
+		case ISCSI_FLASHNODE_MAX_SEGMENT_SIZE:
+			fnode_conn->max_segment_size =
+					*(unsigned *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_LOCAL_PORT:
+			fnode_conn->local_port =
+						*(uint16_t *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_IPV4_TOS:
+			fnode_conn->ipv4_tos = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_IPV6_TC:
+			fnode_conn->ipv6_traffic_class = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_IPV6_FLOW_LABEL:
+			fnode_conn->ipv6_flow_label = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_NAME:
+			rc = iscsi_switch_str_param(&fnode_sess->targetname,
+						    (char *)fnode_param->value);
+			break;
+		case ISCSI_FLASHNODE_TPGT:
+			fnode_sess->tpgt = *(uint16_t *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_LINK_LOCAL_IPV6:
+			memcpy(fnode_conn->link_local_ipv6_addr,
+			       fnode_param->value, IPv6_ADDR_LEN);
+			break;
+		case ISCSI_FLASHNODE_DISCOVERY_PARENT_TYPE:
+			fnode_sess->discovery_parent_type =
+						*(uint16_t *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_TCP_XMIT_WSF:
+			fnode_conn->tcp_xmit_wsf =
+						*(uint8_t *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_TCP_RECV_WSF:
+			fnode_conn->tcp_recv_wsf =
+						*(uint8_t *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_STATSN:
+			fnode_conn->statsn = *(uint32_t *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_EXP_STATSN:
+			fnode_conn->exp_statsn =
+						*(uint32_t *)fnode_param->value;
+			break;
+		default:
+			ql4_printk(KERN_ERR, ha,
+				   "%s: No such sysfs attribute\n", __func__);
+			rc = -ENOSYS;
+			goto exit_set_param;
+		}
+	}
+
+	rc = qla4xxx_sysfs_ddb_apply(fnode_sess, fnode_conn);
+
+exit_set_param:
+	return rc;
+}
+
+/**
+ * qla4xxx_sysfs_ddb_delete - Delete firmware DDB entry
+ * @fnode_sess: pointer to session attrs of flash ddb entry
+ *
+ * This invalidates the flash ddb entry at the given index
+ **/
+static int qla4xxx_sysfs_ddb_delete(struct iscsi_bus_flash_session *fnode_sess)
+{
+	struct Scsi_Host *shost = iscsi_flash_session_to_shost(fnode_sess);
+	struct scsi_qla_host *ha = to_qla_host(shost);
+	uint32_t dev_db_start_offset;
+	uint32_t dev_db_end_offset;
+	struct dev_db_entry *fw_ddb_entry = NULL;
+	dma_addr_t fw_ddb_entry_dma;
+	uint16_t *ddb_cookie = NULL;
+	size_t ddb_size;
+	void *pddb = NULL;
+	int target_id;
+	int rc = 0;
+
+	if (!fnode_sess) {
+		rc = -EINVAL;
+		goto exit_ddb_del;
+	}
+
+	if (fnode_sess->is_boot_target) {
+		rc = -EPERM;
+		DEBUG2(ql4_printk(KERN_ERR, ha,
+				  "%s: Deletion of boot target entry is not permitted.\n",
+				  __func__));
+		goto exit_ddb_del;
+	}
+
+	if (fnode_sess->flash_state == DEV_DB_NON_PERSISTENT)
+		goto sysfs_ddb_del;
+
+	if (is_qla40XX(ha)) {
+		dev_db_start_offset = FLASH_OFFSET_DB_INFO;
+		dev_db_end_offset = FLASH_OFFSET_DB_END;
+		dev_db_start_offset += (fnode_sess->target_id *
+				       sizeof(*fw_ddb_entry));
+		ddb_size = sizeof(*fw_ddb_entry);
+	} else {
+		dev_db_start_offset = FLASH_RAW_ACCESS_ADDR +
+				      (ha->hw.flt_region_ddb << 2);
+		/* flt_ddb_size is DDB table size for both ports
+		 * so divide it by 2 to calculate the offset for second port
+		 */
+		if (ha->port_num == 1)
+			dev_db_start_offset += (ha->hw.flt_ddb_size / 2);
+
+		dev_db_end_offset = dev_db_start_offset +
+				    (ha->hw.flt_ddb_size / 2);
+
+		dev_db_start_offset += (fnode_sess->target_id *
+				       sizeof(*fw_ddb_entry));
+		dev_db_start_offset += (void *)&(fw_ddb_entry->cookie) -
+				       (void *)fw_ddb_entry;
+
+		ddb_size = sizeof(*ddb_cookie);
+	}
+
+	DEBUG2(ql4_printk(KERN_ERR, ha, "%s: start offset=%u, end offset=%u\n",
+			  __func__, dev_db_start_offset, dev_db_end_offset));
+
+	if (dev_db_start_offset > dev_db_end_offset) {
+		rc = -EIO;
+		DEBUG2(ql4_printk(KERN_ERR, ha, "%s:Invalid DDB index %u\n",
+				  __func__, fnode_sess->target_id));
+		goto exit_ddb_del;
+	}
+
+	pddb = dma_alloc_coherent(&ha->pdev->dev, ddb_size,
+				  &fw_ddb_entry_dma, GFP_KERNEL);
+	if (!pddb) {
+		rc = -ENOMEM;
+		DEBUG2(ql4_printk(KERN_ERR, ha,
+				  "%s: Unable to allocate dma buffer\n",
+				  __func__));
+		goto exit_ddb_del;
+	}
+
+	if (is_qla40XX(ha)) {
+		fw_ddb_entry = pddb;
+		memset(fw_ddb_entry, 0, ddb_size);
+		ddb_cookie = &fw_ddb_entry->cookie;
+	} else {
+		ddb_cookie = pddb;
+	}
+
+	/* invalidate the cookie */
+	*ddb_cookie = 0xFFEE;
+	qla4xxx_set_flash(ha, fw_ddb_entry_dma, dev_db_start_offset,
+			  ddb_size, FLASH_OPT_RMW_COMMIT);
+
+sysfs_ddb_del:
+	target_id = fnode_sess->target_id;
+	iscsi_destroy_flashnode_sess(fnode_sess);
+	ql4_printk(KERN_INFO, ha,
+		   "%s: session and conn entries for flashnode %u of host %lu deleted\n",
+		   __func__, target_id, ha->host_no);
+exit_ddb_del:
+	if (pddb)
+		dma_free_coherent(&ha->pdev->dev, ddb_size, pddb,
+				  fw_ddb_entry_dma);
+	return rc;
+}
+
+/**
+ * qla4xxx_sysfs_ddb_export - Create sysfs entries for firmware DDBs
+ * @ha: pointer to adapter structure
+ *
+ * Export the firmware DDB for all send targets and normal targets to sysfs.
+ **/
+static int qla4xxx_sysfs_ddb_export(struct scsi_qla_host *ha)
+{
+	struct dev_db_entry *fw_ddb_entry = NULL;
+	dma_addr_t fw_ddb_entry_dma;
+	uint16_t max_ddbs;
+	uint16_t idx = 0;
+	int ret = QLA_SUCCESS;
+
+	fw_ddb_entry = dma_alloc_coherent(&ha->pdev->dev,
+					  sizeof(*fw_ddb_entry),
+					  &fw_ddb_entry_dma, GFP_KERNEL);
+	if (!fw_ddb_entry) {
+		DEBUG2(ql4_printk(KERN_ERR, ha,
+				  "%s: Unable to allocate dma buffer\n",
+				  __func__));
+		return -ENOMEM;
+	}
+
+	max_ddbs =  is_qla40XX(ha) ? MAX_PRST_DEV_DB_ENTRIES :
+				     MAX_DEV_DB_ENTRIES;
+
+	for (idx = 0; idx < max_ddbs; idx++) {
+		if (qla4xxx_flashdb_by_index(ha, fw_ddb_entry, fw_ddb_entry_dma,
+					     idx))
+			continue;
+
+		ret = qla4xxx_sysfs_ddb_tgt_create(ha, fw_ddb_entry, &idx, 0);
+		if (ret) {
+			ret = -EIO;
+			break;
+		}
+	}
+
+	dma_free_coherent(&ha->pdev->dev, sizeof(*fw_ddb_entry), fw_ddb_entry,
+			  fw_ddb_entry_dma);
+
+	return ret;
+}
+
+static void qla4xxx_sysfs_ddb_remove(struct scsi_qla_host *ha)
+{
+	iscsi_destroy_all_flashnode(ha->host);
+}
+
 /**
  * qla4xxx_build_ddb_list - Build ddb list and setup sessions
  * @ha: pointer to adapter structure
@@ -5341,8 +7016,11 @@ static int qla4xxx_probe_adapter(struct pci_dev *pdev,
 	status = qla4xxx_initialize_adapter(ha, INIT_ADAPTER);
 
 	/* Dont retry adapter initialization if IRQ allocation failed */
-	if (!test_bit(AF_IRQ_ATTACHED, &ha->flags))
+	if (is_qla80XX(ha) && !test_bit(AF_IRQ_ATTACHED, &ha->flags)) {
+		ql4_printk(KERN_WARNING, ha, "%s: Skipping retry of adapter initialization\n",
+			   __func__);
 		goto skip_retry_init;
+	}
 
 	while ((!test_bit(AF_ONLINE, &ha->flags)) &&
 	    init_retry_count++ < MAX_INIT_RETRIES) {
@@ -5445,6 +7123,10 @@ skip_retry_init:
 		ql4_printk(KERN_ERR, ha,
 			   "%s: No iSCSI boot target configured\n", __func__);
 
+	if (qla4xxx_sysfs_ddb_export(ha))
+		ql4_printk(KERN_ERR, ha,
+			   "%s: Error exporting ddb to sysfs\n", __func__);
+
 		/* Perform the build ddb list and login to each */
 	qla4xxx_build_ddb_list(ha, INIT_ADAPTER);
 	iscsi_host_for_each_session(ha->host, qla4xxx_login_flash_ddb);
@@ -5570,6 +7252,7 @@ static void qla4xxx_remove_adapter(struct pci_dev *pdev)
 	qla4xxx_destroy_fw_ddb_session(ha);
 	qla4_8xxx_free_sysfs_attr(ha);
 
+	qla4xxx_sysfs_ddb_remove(ha);
 	scsi_remove_host(ha->host);
 
 	qla4xxx_free_adapter(ha);
@@ -5669,7 +7352,6 @@ struct srb *qla4xxx_del_from_active_array(struct scsi_qla_host *ha,
 
 	/* update counters */
 	if (srb->flags & SRB_DMA_VALID) {
-		ha->req_q_count += srb->iocb_cnt;
 		ha->iocb_cnt -= srb->iocb_cnt;
 		if (srb->cmd)
 			srb->cmd->host_scribble =
@@ -6081,6 +7763,7 @@ static int qla4xxx_host_reset(struct Scsi_Host *shost, int reset_type)
 {
 	struct scsi_qla_host *ha = to_qla_host(shost);
 	int rval = QLA_SUCCESS;
+	uint32_t idc_ctrl;
 
 	if (ql4xdontresethba) {
 		DEBUG2(ql4_printk(KERN_INFO, ha, "%s: Don't Reset HBA\n",
@@ -6111,6 +7794,14 @@ static int qla4xxx_host_reset(struct Scsi_Host *shost, int reset_type)
 	}
 
 recover_adapter:
+	/* For ISP83XX set graceful reset bit in IDC_DRV_CTRL if
+	 * reset is issued by application */
+	if (is_qla8032(ha) && test_bit(DPC_RESET_HA, &ha->dpc_flags)) {
+		idc_ctrl = qla4_83xx_rd_reg(ha, QLA83XX_IDC_DRV_CTRL);
+		qla4_83xx_wr_reg(ha, QLA83XX_IDC_DRV_CTRL,
+				 (idc_ctrl | GRACEFUL_RESET_BIT1));
+	}
+
 	rval = qla4xxx_recover_adapter(ha);
 	if (rval != QLA_SUCCESS) {
 		DEBUG2(ql4_printk(KERN_INFO, ha, "%s: recover adapter fail\n",
diff --git a/drivers/scsi/qla4xxx/ql4_version.h b/drivers/scsi/qla4xxx/ql4_version.h
index 6775a45af31..83e0fec35d5 100644
--- a/drivers/scsi/qla4xxx/ql4_version.h
+++ b/drivers/scsi/qla4xxx/ql4_version.h
@@ -5,4 +5,4 @@
  * See LICENSE.qla4xxx for copyright and licensing details.
  */
 
-#define QLA4XXX_DRIVER_VERSION	"5.03.00-k4"
+#define QLA4XXX_DRIVER_VERSION	"5.03.00-k8"
diff --git a/drivers/scsi/scsi_transport_iscsi.c b/drivers/scsi/scsi_transport_iscsi.c
index 0a74b975efd..ce06e8772f3 100644
--- a/drivers/scsi/scsi_transport_iscsi.c
+++ b/drivers/scsi/scsi_transport_iscsi.c
@@ -25,6 +25,7 @@
 #include <linux/slab.h>
 #include <linux/bsg-lib.h>
 #include <linux/idr.h>
+#include <linux/list.h>
 #include <net/tcp.h>
 #include <scsi/scsi.h>
 #include <scsi/scsi_host.h>
@@ -460,6 +461,689 @@ void iscsi_destroy_iface(struct iscsi_iface *iface)
 EXPORT_SYMBOL_GPL(iscsi_destroy_iface);
 
 /*
+ * Interface to display flash node params to sysfs
+ */
+
+#define ISCSI_FLASHNODE_ATTR(_prefix, _name, _mode, _show, _store)	\
+struct device_attribute dev_attr_##_prefix##_##_name =			\
+	__ATTR(_name, _mode, _show, _store)
+
+/* flash node session attrs show */
+#define iscsi_flashnode_sess_attr_show(type, name, param)		\
+static ssize_t								\
+show_##type##_##name(struct device *dev, struct device_attribute *attr,	\
+		     char *buf)						\
+{									\
+	struct iscsi_bus_flash_session *fnode_sess =			\
+					iscsi_dev_to_flash_session(dev);\
+	struct iscsi_transport *t = fnode_sess->transport;		\
+	return t->get_flashnode_param(fnode_sess, param, buf);		\
+}									\
+
+
+#define iscsi_flashnode_sess_attr(type, name, param)			\
+	iscsi_flashnode_sess_attr_show(type, name, param)		\
+static ISCSI_FLASHNODE_ATTR(type, name, S_IRUGO,			\
+			    show_##type##_##name, NULL);
+
+/* Flash node session attributes */
+
+iscsi_flashnode_sess_attr(fnode, auto_snd_tgt_disable,
+			  ISCSI_FLASHNODE_AUTO_SND_TGT_DISABLE);
+iscsi_flashnode_sess_attr(fnode, discovery_session,
+			  ISCSI_FLASHNODE_DISCOVERY_SESS);
+iscsi_flashnode_sess_attr(fnode, portal_type, ISCSI_FLASHNODE_PORTAL_TYPE);
+iscsi_flashnode_sess_attr(fnode, entry_enable, ISCSI_FLASHNODE_ENTRY_EN);
+iscsi_flashnode_sess_attr(fnode, immediate_data, ISCSI_FLASHNODE_IMM_DATA_EN);
+iscsi_flashnode_sess_attr(fnode, initial_r2t, ISCSI_FLASHNODE_INITIAL_R2T_EN);
+iscsi_flashnode_sess_attr(fnode, data_seq_in_order,
+			  ISCSI_FLASHNODE_DATASEQ_INORDER);
+iscsi_flashnode_sess_attr(fnode, data_pdu_in_order,
+			  ISCSI_FLASHNODE_PDU_INORDER);
+iscsi_flashnode_sess_attr(fnode, chap_auth, ISCSI_FLASHNODE_CHAP_AUTH_EN);
+iscsi_flashnode_sess_attr(fnode, discovery_logout,
+			  ISCSI_FLASHNODE_DISCOVERY_LOGOUT_EN);
+iscsi_flashnode_sess_attr(fnode, bidi_chap, ISCSI_FLASHNODE_BIDI_CHAP_EN);
+iscsi_flashnode_sess_attr(fnode, discovery_auth_optional,
+			  ISCSI_FLASHNODE_DISCOVERY_AUTH_OPTIONAL);
+iscsi_flashnode_sess_attr(fnode, erl, ISCSI_FLASHNODE_ERL);
+iscsi_flashnode_sess_attr(fnode, first_burst_len, ISCSI_FLASHNODE_FIRST_BURST);
+iscsi_flashnode_sess_attr(fnode, def_time2wait, ISCSI_FLASHNODE_DEF_TIME2WAIT);
+iscsi_flashnode_sess_attr(fnode, def_time2retain,
+			  ISCSI_FLASHNODE_DEF_TIME2RETAIN);
+iscsi_flashnode_sess_attr(fnode, max_outstanding_r2t, ISCSI_FLASHNODE_MAX_R2T);
+iscsi_flashnode_sess_attr(fnode, isid, ISCSI_FLASHNODE_ISID);
+iscsi_flashnode_sess_attr(fnode, tsid, ISCSI_FLASHNODE_TSID);
+iscsi_flashnode_sess_attr(fnode, max_burst_len, ISCSI_FLASHNODE_MAX_BURST);
+iscsi_flashnode_sess_attr(fnode, def_taskmgmt_tmo,
+			  ISCSI_FLASHNODE_DEF_TASKMGMT_TMO);
+iscsi_flashnode_sess_attr(fnode, targetalias, ISCSI_FLASHNODE_ALIAS);
+iscsi_flashnode_sess_attr(fnode, targetname, ISCSI_FLASHNODE_NAME);
+iscsi_flashnode_sess_attr(fnode, tpgt, ISCSI_FLASHNODE_TPGT);
+iscsi_flashnode_sess_attr(fnode, discovery_parent_idx,
+			  ISCSI_FLASHNODE_DISCOVERY_PARENT_IDX);
+iscsi_flashnode_sess_attr(fnode, discovery_parent_type,
+			  ISCSI_FLASHNODE_DISCOVERY_PARENT_TYPE);
+iscsi_flashnode_sess_attr(fnode, chap_in_idx, ISCSI_FLASHNODE_CHAP_IN_IDX);
+iscsi_flashnode_sess_attr(fnode, chap_out_idx, ISCSI_FLASHNODE_CHAP_OUT_IDX);
+iscsi_flashnode_sess_attr(fnode, username, ISCSI_FLASHNODE_USERNAME);
+iscsi_flashnode_sess_attr(fnode, username_in, ISCSI_FLASHNODE_USERNAME_IN);
+iscsi_flashnode_sess_attr(fnode, password, ISCSI_FLASHNODE_PASSWORD);
+iscsi_flashnode_sess_attr(fnode, password_in, ISCSI_FLASHNODE_PASSWORD_IN);
+iscsi_flashnode_sess_attr(fnode, is_boot_target, ISCSI_FLASHNODE_IS_BOOT_TGT);
+
+static struct attribute *iscsi_flashnode_sess_attrs[] = {
+	&dev_attr_fnode_auto_snd_tgt_disable.attr,
+	&dev_attr_fnode_discovery_session.attr,
+	&dev_attr_fnode_portal_type.attr,
+	&dev_attr_fnode_entry_enable.attr,
+	&dev_attr_fnode_immediate_data.attr,
+	&dev_attr_fnode_initial_r2t.attr,
+	&dev_attr_fnode_data_seq_in_order.attr,
+	&dev_attr_fnode_data_pdu_in_order.attr,
+	&dev_attr_fnode_chap_auth.attr,
+	&dev_attr_fnode_discovery_logout.attr,
+	&dev_attr_fnode_bidi_chap.attr,
+	&dev_attr_fnode_discovery_auth_optional.attr,
+	&dev_attr_fnode_erl.attr,
+	&dev_attr_fnode_first_burst_len.attr,
+	&dev_attr_fnode_def_time2wait.attr,
+	&dev_attr_fnode_def_time2retain.attr,
+	&dev_attr_fnode_max_outstanding_r2t.attr,
+	&dev_attr_fnode_isid.attr,
+	&dev_attr_fnode_tsid.attr,
+	&dev_attr_fnode_max_burst_len.attr,
+	&dev_attr_fnode_def_taskmgmt_tmo.attr,
+	&dev_attr_fnode_targetalias.attr,
+	&dev_attr_fnode_targetname.attr,
+	&dev_attr_fnode_tpgt.attr,
+	&dev_attr_fnode_discovery_parent_idx.attr,
+	&dev_attr_fnode_discovery_parent_type.attr,
+	&dev_attr_fnode_chap_in_idx.attr,
+	&dev_attr_fnode_chap_out_idx.attr,
+	&dev_attr_fnode_username.attr,
+	&dev_attr_fnode_username_in.attr,
+	&dev_attr_fnode_password.attr,
+	&dev_attr_fnode_password_in.attr,
+	&dev_attr_fnode_is_boot_target.attr,
+	NULL,
+};
+
+static umode_t iscsi_flashnode_sess_attr_is_visible(struct kobject *kobj,
+						    struct attribute *attr,
+						    int i)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct iscsi_bus_flash_session *fnode_sess =
+						iscsi_dev_to_flash_session(dev);
+	struct iscsi_transport *t = fnode_sess->transport;
+	int param;
+
+	if (attr == &dev_attr_fnode_auto_snd_tgt_disable.attr) {
+		param = ISCSI_FLASHNODE_AUTO_SND_TGT_DISABLE;
+	} else if (attr == &dev_attr_fnode_discovery_session.attr) {
+		param = ISCSI_FLASHNODE_DISCOVERY_SESS;
+	} else if (attr == &dev_attr_fnode_portal_type.attr) {
+		param = ISCSI_FLASHNODE_PORTAL_TYPE;
+	} else if (attr == &dev_attr_fnode_entry_enable.attr) {
+		param = ISCSI_FLASHNODE_ENTRY_EN;
+	} else if (attr == &dev_attr_fnode_immediate_data.attr) {
+		param = ISCSI_FLASHNODE_IMM_DATA_EN;
+	} else if (attr == &dev_attr_fnode_initial_r2t.attr) {
+		param = ISCSI_FLASHNODE_INITIAL_R2T_EN;
+	} else if (attr == &dev_attr_fnode_data_seq_in_order.attr) {
+		param = ISCSI_FLASHNODE_DATASEQ_INORDER;
+	} else if (attr == &dev_attr_fnode_data_pdu_in_order.attr) {
+		param = ISCSI_FLASHNODE_PDU_INORDER;
+	} else if (attr == &dev_attr_fnode_chap_auth.attr) {
+		param = ISCSI_FLASHNODE_CHAP_AUTH_EN;
+	} else if (attr == &dev_attr_fnode_discovery_logout.attr) {
+		param = ISCSI_FLASHNODE_DISCOVERY_LOGOUT_EN;
+	} else if (attr == &dev_attr_fnode_bidi_chap.attr) {
+		param = ISCSI_FLASHNODE_BIDI_CHAP_EN;
+	} else if (attr == &dev_attr_fnode_discovery_auth_optional.attr) {
+		param = ISCSI_FLASHNODE_DISCOVERY_AUTH_OPTIONAL;
+	} else if (attr == &dev_attr_fnode_erl.attr) {
+		param = ISCSI_FLASHNODE_ERL;
+	} else if (attr == &dev_attr_fnode_first_burst_len.attr) {
+		param = ISCSI_FLASHNODE_FIRST_BURST;
+	} else if (attr == &dev_attr_fnode_def_time2wait.attr) {
+		param = ISCSI_FLASHNODE_DEF_TIME2WAIT;
+	} else if (attr == &dev_attr_fnode_def_time2retain.attr) {
+		param = ISCSI_FLASHNODE_DEF_TIME2RETAIN;
+	} else if (attr == &dev_attr_fnode_max_outstanding_r2t.attr) {
+		param = ISCSI_FLASHNODE_MAX_R2T;
+	} else if (attr == &dev_attr_fnode_isid.attr) {
+		param = ISCSI_FLASHNODE_ISID;
+	} else if (attr == &dev_attr_fnode_tsid.attr) {
+		param = ISCSI_FLASHNODE_TSID;
+	} else if (attr == &dev_attr_fnode_max_burst_len.attr) {
+		param = ISCSI_FLASHNODE_MAX_BURST;
+	} else if (attr == &dev_attr_fnode_def_taskmgmt_tmo.attr) {
+		param = ISCSI_FLASHNODE_DEF_TASKMGMT_TMO;
+	} else if (attr == &dev_attr_fnode_targetalias.attr) {
+		param = ISCSI_FLASHNODE_ALIAS;
+	} else if (attr == &dev_attr_fnode_targetname.attr) {
+		param = ISCSI_FLASHNODE_NAME;
+	} else if (attr == &dev_attr_fnode_tpgt.attr) {
+		param = ISCSI_FLASHNODE_TPGT;
+	} else if (attr == &dev_attr_fnode_discovery_parent_idx.attr) {
+		param = ISCSI_FLASHNODE_DISCOVERY_PARENT_IDX;
+	} else if (attr == &dev_attr_fnode_discovery_parent_type.attr) {
+		param = ISCSI_FLASHNODE_DISCOVERY_PARENT_TYPE;
+	} else if (attr == &dev_attr_fnode_chap_in_idx.attr) {
+		param = ISCSI_FLASHNODE_CHAP_IN_IDX;
+	} else if (attr == &dev_attr_fnode_chap_out_idx.attr) {
+		param = ISCSI_FLASHNODE_CHAP_OUT_IDX;
+	} else if (attr == &dev_attr_fnode_username.attr) {
+		param = ISCSI_FLASHNODE_USERNAME;
+	} else if (attr == &dev_attr_fnode_username_in.attr) {
+		param = ISCSI_FLASHNODE_USERNAME_IN;
+	} else if (attr == &dev_attr_fnode_password.attr) {
+		param = ISCSI_FLASHNODE_PASSWORD;
+	} else if (attr == &dev_attr_fnode_password_in.attr) {
+		param = ISCSI_FLASHNODE_PASSWORD_IN;
+	} else if (attr == &dev_attr_fnode_is_boot_target.attr) {
+		param = ISCSI_FLASHNODE_IS_BOOT_TGT;
+	} else {
+		WARN_ONCE(1, "Invalid flashnode session attr");
+		return 0;
+	}
+
+	return t->attr_is_visible(ISCSI_FLASHNODE_PARAM, param);
+}
+
+static struct attribute_group iscsi_flashnode_sess_attr_group = {
+	.attrs = iscsi_flashnode_sess_attrs,
+	.is_visible = iscsi_flashnode_sess_attr_is_visible,
+};
+
+static const struct attribute_group *iscsi_flashnode_sess_attr_groups[] = {
+	&iscsi_flashnode_sess_attr_group,
+	NULL,
+};
+
+static void iscsi_flashnode_sess_release(struct device *dev)
+{
+	struct iscsi_bus_flash_session *fnode_sess =
+						iscsi_dev_to_flash_session(dev);
+
+	kfree(fnode_sess->targetname);
+	kfree(fnode_sess->targetalias);
+	kfree(fnode_sess->portal_type);
+	kfree(fnode_sess);
+}
+
+struct device_type iscsi_flashnode_sess_dev_type = {
+	.name = "iscsi_flashnode_sess_dev_type",
+	.groups = iscsi_flashnode_sess_attr_groups,
+	.release = iscsi_flashnode_sess_release,
+};
+
+/* flash node connection attrs show */
+#define iscsi_flashnode_conn_attr_show(type, name, param)		\
+static ssize_t								\
+show_##type##_##name(struct device *dev, struct device_attribute *attr,	\
+		     char *buf)						\
+{									\
+	struct iscsi_bus_flash_conn *fnode_conn = iscsi_dev_to_flash_conn(dev);\
+	struct iscsi_bus_flash_session *fnode_sess =			\
+				iscsi_flash_conn_to_flash_session(fnode_conn);\
+	struct iscsi_transport *t = fnode_conn->transport;		\
+	return t->get_flashnode_param(fnode_sess, param, buf);		\
+}									\
+
+
+#define iscsi_flashnode_conn_attr(type, name, param)			\
+	iscsi_flashnode_conn_attr_show(type, name, param)		\
+static ISCSI_FLASHNODE_ATTR(type, name, S_IRUGO,			\
+			    show_##type##_##name, NULL);
+
+/* Flash node connection attributes */
+
+iscsi_flashnode_conn_attr(fnode, is_fw_assigned_ipv6,
+			  ISCSI_FLASHNODE_IS_FW_ASSIGNED_IPV6);
+iscsi_flashnode_conn_attr(fnode, header_digest, ISCSI_FLASHNODE_HDR_DGST_EN);
+iscsi_flashnode_conn_attr(fnode, data_digest, ISCSI_FLASHNODE_DATA_DGST_EN);
+iscsi_flashnode_conn_attr(fnode, snack_req, ISCSI_FLASHNODE_SNACK_REQ_EN);
+iscsi_flashnode_conn_attr(fnode, tcp_timestamp_stat,
+			  ISCSI_FLASHNODE_TCP_TIMESTAMP_STAT);
+iscsi_flashnode_conn_attr(fnode, tcp_nagle_disable,
+			  ISCSI_FLASHNODE_TCP_NAGLE_DISABLE);
+iscsi_flashnode_conn_attr(fnode, tcp_wsf_disable,
+			  ISCSI_FLASHNODE_TCP_WSF_DISABLE);
+iscsi_flashnode_conn_attr(fnode, tcp_timer_scale,
+			  ISCSI_FLASHNODE_TCP_TIMER_SCALE);
+iscsi_flashnode_conn_attr(fnode, tcp_timestamp_enable,
+			  ISCSI_FLASHNODE_TCP_TIMESTAMP_EN);
+iscsi_flashnode_conn_attr(fnode, fragment_disable,
+			  ISCSI_FLASHNODE_IP_FRAG_DISABLE);
+iscsi_flashnode_conn_attr(fnode, keepalive_tmo, ISCSI_FLASHNODE_KEEPALIVE_TMO);
+iscsi_flashnode_conn_attr(fnode, port, ISCSI_FLASHNODE_PORT);
+iscsi_flashnode_conn_attr(fnode, ipaddress, ISCSI_FLASHNODE_IPADDR);
+iscsi_flashnode_conn_attr(fnode, max_recv_dlength,
+			  ISCSI_FLASHNODE_MAX_RECV_DLENGTH);
+iscsi_flashnode_conn_attr(fnode, max_xmit_dlength,
+			  ISCSI_FLASHNODE_MAX_XMIT_DLENGTH);
+iscsi_flashnode_conn_attr(fnode, local_port, ISCSI_FLASHNODE_LOCAL_PORT);
+iscsi_flashnode_conn_attr(fnode, ipv4_tos, ISCSI_FLASHNODE_IPV4_TOS);
+iscsi_flashnode_conn_attr(fnode, ipv6_traffic_class, ISCSI_FLASHNODE_IPV6_TC);
+iscsi_flashnode_conn_attr(fnode, ipv6_flow_label,
+			  ISCSI_FLASHNODE_IPV6_FLOW_LABEL);
+iscsi_flashnode_conn_attr(fnode, redirect_ipaddr,
+			  ISCSI_FLASHNODE_REDIRECT_IPADDR);
+iscsi_flashnode_conn_attr(fnode, max_segment_size,
+			  ISCSI_FLASHNODE_MAX_SEGMENT_SIZE);
+iscsi_flashnode_conn_attr(fnode, link_local_ipv6,
+			  ISCSI_FLASHNODE_LINK_LOCAL_IPV6);
+iscsi_flashnode_conn_attr(fnode, tcp_xmit_wsf, ISCSI_FLASHNODE_TCP_XMIT_WSF);
+iscsi_flashnode_conn_attr(fnode, tcp_recv_wsf, ISCSI_FLASHNODE_TCP_RECV_WSF);
+iscsi_flashnode_conn_attr(fnode, statsn, ISCSI_FLASHNODE_STATSN);
+iscsi_flashnode_conn_attr(fnode, exp_statsn, ISCSI_FLASHNODE_EXP_STATSN);
+
+static struct attribute *iscsi_flashnode_conn_attrs[] = {
+	&dev_attr_fnode_is_fw_assigned_ipv6.attr,
+	&dev_attr_fnode_header_digest.attr,
+	&dev_attr_fnode_data_digest.attr,
+	&dev_attr_fnode_snack_req.attr,
+	&dev_attr_fnode_tcp_timestamp_stat.attr,
+	&dev_attr_fnode_tcp_nagle_disable.attr,
+	&dev_attr_fnode_tcp_wsf_disable.attr,
+	&dev_attr_fnode_tcp_timer_scale.attr,
+	&dev_attr_fnode_tcp_timestamp_enable.attr,
+	&dev_attr_fnode_fragment_disable.attr,
+	&dev_attr_fnode_max_recv_dlength.attr,
+	&dev_attr_fnode_max_xmit_dlength.attr,
+	&dev_attr_fnode_keepalive_tmo.attr,
+	&dev_attr_fnode_port.attr,
+	&dev_attr_fnode_ipaddress.attr,
+	&dev_attr_fnode_redirect_ipaddr.attr,
+	&dev_attr_fnode_max_segment_size.attr,
+	&dev_attr_fnode_local_port.attr,
+	&dev_attr_fnode_ipv4_tos.attr,
+	&dev_attr_fnode_ipv6_traffic_class.attr,
+	&dev_attr_fnode_ipv6_flow_label.attr,
+	&dev_attr_fnode_link_local_ipv6.attr,
+	&dev_attr_fnode_tcp_xmit_wsf.attr,
+	&dev_attr_fnode_tcp_recv_wsf.attr,
+	&dev_attr_fnode_statsn.attr,
+	&dev_attr_fnode_exp_statsn.attr,
+	NULL,
+};
+
+static umode_t iscsi_flashnode_conn_attr_is_visible(struct kobject *kobj,
+						    struct attribute *attr,
+						    int i)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct iscsi_bus_flash_conn *fnode_conn = iscsi_dev_to_flash_conn(dev);
+	struct iscsi_transport *t = fnode_conn->transport;
+	int param;
+
+	if (attr == &dev_attr_fnode_is_fw_assigned_ipv6.attr) {
+		param = ISCSI_FLASHNODE_IS_FW_ASSIGNED_IPV6;
+	} else if (attr == &dev_attr_fnode_header_digest.attr) {
+		param = ISCSI_FLASHNODE_HDR_DGST_EN;
+	} else if (attr == &dev_attr_fnode_data_digest.attr) {
+		param = ISCSI_FLASHNODE_DATA_DGST_EN;
+	} else if (attr == &dev_attr_fnode_snack_req.attr) {
+		param = ISCSI_FLASHNODE_SNACK_REQ_EN;
+	} else if (attr == &dev_attr_fnode_tcp_timestamp_stat.attr) {
+		param = ISCSI_FLASHNODE_TCP_TIMESTAMP_STAT;
+	} else if (attr == &dev_attr_fnode_tcp_nagle_disable.attr) {
+		param = ISCSI_FLASHNODE_TCP_NAGLE_DISABLE;
+	} else if (attr == &dev_attr_fnode_tcp_wsf_disable.attr) {
+		param = ISCSI_FLASHNODE_TCP_WSF_DISABLE;
+	} else if (attr == &dev_attr_fnode_tcp_timer_scale.attr) {
+		param = ISCSI_FLASHNODE_TCP_TIMER_SCALE;
+	} else if (attr == &dev_attr_fnode_tcp_timestamp_enable.attr) {
+		param = ISCSI_FLASHNODE_TCP_TIMESTAMP_EN;
+	} else if (attr == &dev_attr_fnode_fragment_disable.attr) {
+		param = ISCSI_FLASHNODE_IP_FRAG_DISABLE;
+	} else if (attr == &dev_attr_fnode_max_recv_dlength.attr) {
+		param = ISCSI_FLASHNODE_MAX_RECV_DLENGTH;
+	} else if (attr == &dev_attr_fnode_max_xmit_dlength.attr) {
+		param = ISCSI_FLASHNODE_MAX_XMIT_DLENGTH;
+	} else if (attr == &dev_attr_fnode_keepalive_tmo.attr) {
+		param = ISCSI_FLASHNODE_KEEPALIVE_TMO;
+	} else if (attr == &dev_attr_fnode_port.attr) {
+		param = ISCSI_FLASHNODE_PORT;
+	} else if (attr == &dev_attr_fnode_ipaddress.attr) {
+		param = ISCSI_FLASHNODE_IPADDR;
+	} else if (attr == &dev_attr_fnode_redirect_ipaddr.attr) {
+		param = ISCSI_FLASHNODE_REDIRECT_IPADDR;
+	} else if (attr == &dev_attr_fnode_max_segment_size.attr) {
+		param = ISCSI_FLASHNODE_MAX_SEGMENT_SIZE;
+	} else if (attr == &dev_attr_fnode_local_port.attr) {
+		param = ISCSI_FLASHNODE_LOCAL_PORT;
+	} else if (attr == &dev_attr_fnode_ipv4_tos.attr) {
+		param = ISCSI_FLASHNODE_IPV4_TOS;
+	} else if (attr == &dev_attr_fnode_ipv6_traffic_class.attr) {
+		param = ISCSI_FLASHNODE_IPV6_TC;
+	} else if (attr == &dev_attr_fnode_ipv6_flow_label.attr) {
+		param = ISCSI_FLASHNODE_IPV6_FLOW_LABEL;
+	} else if (attr == &dev_attr_fnode_link_local_ipv6.attr) {
+		param = ISCSI_FLASHNODE_LINK_LOCAL_IPV6;
+	} else if (attr == &dev_attr_fnode_tcp_xmit_wsf.attr) {
+		param = ISCSI_FLASHNODE_TCP_XMIT_WSF;
+	} else if (attr == &dev_attr_fnode_tcp_recv_wsf.attr) {
+		param = ISCSI_FLASHNODE_TCP_RECV_WSF;
+	} else if (attr == &dev_attr_fnode_statsn.attr) {
+		param = ISCSI_FLASHNODE_STATSN;
+	} else if (attr == &dev_attr_fnode_exp_statsn.attr) {
+		param = ISCSI_FLASHNODE_EXP_STATSN;
+	} else {
+		WARN_ONCE(1, "Invalid flashnode connection attr");
+		return 0;
+	}
+
+	return t->attr_is_visible(ISCSI_FLASHNODE_PARAM, param);
+}
+
+static struct attribute_group iscsi_flashnode_conn_attr_group = {
+	.attrs = iscsi_flashnode_conn_attrs,
+	.is_visible = iscsi_flashnode_conn_attr_is_visible,
+};
+
+static const struct attribute_group *iscsi_flashnode_conn_attr_groups[] = {
+	&iscsi_flashnode_conn_attr_group,
+	NULL,
+};
+
+static void iscsi_flashnode_conn_release(struct device *dev)
+{
+	struct iscsi_bus_flash_conn *fnode_conn = iscsi_dev_to_flash_conn(dev);
+
+	kfree(fnode_conn->ipaddress);
+	kfree(fnode_conn->redirect_ipaddr);
+	kfree(fnode_conn->link_local_ipv6_addr);
+	kfree(fnode_conn);
+}
+
+struct device_type iscsi_flashnode_conn_dev_type = {
+	.name = "iscsi_flashnode_conn_dev_type",
+	.groups = iscsi_flashnode_conn_attr_groups,
+	.release = iscsi_flashnode_conn_release,
+};
+
+struct bus_type iscsi_flashnode_bus;
+
+int iscsi_flashnode_bus_match(struct device *dev,
+				     struct device_driver *drv)
+{
+	if (dev->bus == &iscsi_flashnode_bus)
+		return 1;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(iscsi_flashnode_bus_match);
+
+struct bus_type iscsi_flashnode_bus = {
+	.name = "iscsi_flashnode",
+	.match = &iscsi_flashnode_bus_match,
+};
+
+/**
+ * iscsi_create_flashnode_sess - Add flashnode session entry in sysfs
+ * @shost: pointer to host data
+ * @index: index of flashnode to add in sysfs
+ * @transport: pointer to transport data
+ * @dd_size: total size to allocate
+ *
+ * Adds a sysfs entry for the flashnode session attributes
+ *
+ * Returns:
+ *  pointer to allocated flashnode sess on sucess
+ *  %NULL on failure
+ */
+struct iscsi_bus_flash_session *
+iscsi_create_flashnode_sess(struct Scsi_Host *shost, int index,
+			    struct iscsi_transport *transport,
+			    int dd_size)
+{
+	struct iscsi_bus_flash_session *fnode_sess;
+	int err;
+
+	fnode_sess = kzalloc(sizeof(*fnode_sess) + dd_size, GFP_KERNEL);
+	if (!fnode_sess)
+		return NULL;
+
+	fnode_sess->transport = transport;
+	fnode_sess->target_id = index;
+	fnode_sess->dev.type = &iscsi_flashnode_sess_dev_type;
+	fnode_sess->dev.bus = &iscsi_flashnode_bus;
+	fnode_sess->dev.parent = &shost->shost_gendev;
+	dev_set_name(&fnode_sess->dev, "flashnode_sess-%u:%u",
+		     shost->host_no, index);
+
+	err = device_register(&fnode_sess->dev);
+	if (err)
+		goto free_fnode_sess;
+
+	if (dd_size)
+		fnode_sess->dd_data = &fnode_sess[1];
+
+	return fnode_sess;
+
+free_fnode_sess:
+	kfree(fnode_sess);
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(iscsi_create_flashnode_sess);
+
+/**
+ * iscsi_create_flashnode_conn - Add flashnode conn entry in sysfs
+ * @shost: pointer to host data
+ * @fnode_sess: pointer to the parent flashnode session entry
+ * @transport: pointer to transport data
+ * @dd_size: total size to allocate
+ *
+ * Adds a sysfs entry for the flashnode connection attributes
+ *
+ * Returns:
+ *  pointer to allocated flashnode conn on success
+ *  %NULL on failure
+ */
+struct iscsi_bus_flash_conn *
+iscsi_create_flashnode_conn(struct Scsi_Host *shost,
+			    struct iscsi_bus_flash_session *fnode_sess,
+			    struct iscsi_transport *transport,
+			    int dd_size)
+{
+	struct iscsi_bus_flash_conn *fnode_conn;
+	int err;
+
+	fnode_conn = kzalloc(sizeof(*fnode_conn) + dd_size, GFP_KERNEL);
+	if (!fnode_conn)
+		return NULL;
+
+	fnode_conn->transport = transport;
+	fnode_conn->dev.type = &iscsi_flashnode_conn_dev_type;
+	fnode_conn->dev.bus = &iscsi_flashnode_bus;
+	fnode_conn->dev.parent = &fnode_sess->dev;
+	dev_set_name(&fnode_conn->dev, "flashnode_conn-%u:%u:0",
+		     shost->host_no, fnode_sess->target_id);
+
+	err = device_register(&fnode_conn->dev);
+	if (err)
+		goto free_fnode_conn;
+
+	if (dd_size)
+		fnode_conn->dd_data = &fnode_conn[1];
+
+	return fnode_conn;
+
+free_fnode_conn:
+	kfree(fnode_conn);
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(iscsi_create_flashnode_conn);
+
+/**
+ * iscsi_is_flashnode_conn_dev - verify passed device is to be flashnode conn
+ * @dev: device to verify
+ * @data: pointer to data containing value to use for verification
+ *
+ * Verifies if the passed device is flashnode conn device
+ *
+ * Returns:
+ *  1 on success
+ *  0 on failure
+ */
+int iscsi_is_flashnode_conn_dev(struct device *dev, void *data)
+{
+	return dev->bus == &iscsi_flashnode_bus;
+}
+EXPORT_SYMBOL_GPL(iscsi_is_flashnode_conn_dev);
+
+static int iscsi_destroy_flashnode_conn(struct iscsi_bus_flash_conn *fnode_conn)
+{
+	device_unregister(&fnode_conn->dev);
+	return 0;
+}
+
+static int flashnode_match_index(struct device *dev, void *data)
+{
+	struct iscsi_bus_flash_session *fnode_sess = NULL;
+	int ret = 0;
+
+	if (!iscsi_flashnode_bus_match(dev, NULL))
+		goto exit_match_index;
+
+	fnode_sess = iscsi_dev_to_flash_session(dev);
+	ret = (fnode_sess->target_id == *((int *)data)) ? 1 : 0;
+
+exit_match_index:
+	return ret;
+}
+
+/**
+ * iscsi_get_flashnode_by_index -finds flashnode session entry by index
+ * @shost: pointer to host data
+ * @data: pointer to data containing value to use for comparison
+ * @fn: function pointer that does actual comparison
+ *
+ * Finds the flashnode session object for the passed index
+ *
+ * Returns:
+ *  pointer to found flashnode session object on success
+ *  %NULL on failure
+ */
+static struct iscsi_bus_flash_session *
+iscsi_get_flashnode_by_index(struct Scsi_Host *shost, void *data,
+			     int (*fn)(struct device *dev, void *data))
+{
+	struct iscsi_bus_flash_session *fnode_sess = NULL;
+	struct device *dev;
+
+	dev = device_find_child(&shost->shost_gendev, data, fn);
+	if (dev)
+		fnode_sess = iscsi_dev_to_flash_session(dev);
+
+	return fnode_sess;
+}
+
+/**
+ * iscsi_find_flashnode_sess - finds flashnode session entry
+ * @shost: pointer to host data
+ * @data: pointer to data containing value to use for comparison
+ * @fn: function pointer that does actual comparison
+ *
+ * Finds the flashnode session object comparing the data passed using logic
+ * defined in passed function pointer
+ *
+ * Returns:
+ *  pointer to found flashnode session device object on success
+ *  %NULL on failure
+ */
+struct device *
+iscsi_find_flashnode_sess(struct Scsi_Host *shost, void *data,
+			  int (*fn)(struct device *dev, void *data))
+{
+	struct device *dev;
+
+	dev = device_find_child(&shost->shost_gendev, data, fn);
+	return dev;
+}
+EXPORT_SYMBOL_GPL(iscsi_find_flashnode_sess);
+
+/**
+ * iscsi_find_flashnode_conn - finds flashnode connection entry
+ * @fnode_sess: pointer to parent flashnode session entry
+ * @data: pointer to data containing value to use for comparison
+ * @fn: function pointer that does actual comparison
+ *
+ * Finds the flashnode connection object comparing the data passed using logic
+ * defined in passed function pointer
+ *
+ * Returns:
+ *  pointer to found flashnode connection device object on success
+ *  %NULL on failure
+ */
+struct device *
+iscsi_find_flashnode_conn(struct iscsi_bus_flash_session *fnode_sess,
+			  void *data,
+			  int (*fn)(struct device *dev, void *data))
+{
+	struct device *dev;
+
+	dev = device_find_child(&fnode_sess->dev, data, fn);
+	return dev;
+}
+EXPORT_SYMBOL_GPL(iscsi_find_flashnode_conn);
+
+static int iscsi_iter_destroy_flashnode_conn_fn(struct device *dev, void *data)
+{
+	if (!iscsi_is_flashnode_conn_dev(dev, NULL))
+		return 0;
+
+	return iscsi_destroy_flashnode_conn(iscsi_dev_to_flash_conn(dev));
+}
+
+/**
+ * iscsi_destroy_flashnode_sess - destory flashnode session entry
+ * @fnode_sess: pointer to flashnode session entry to be destroyed
+ *
+ * Deletes the flashnode session entry and all children flashnode connection
+ * entries from sysfs
+ */
+void iscsi_destroy_flashnode_sess(struct iscsi_bus_flash_session *fnode_sess)
+{
+	int err;
+
+	err = device_for_each_child(&fnode_sess->dev, NULL,
+				    iscsi_iter_destroy_flashnode_conn_fn);
+	if (err)
+		pr_err("Could not delete all connections for %s. Error %d.\n",
+		       fnode_sess->dev.kobj.name, err);
+
+	device_unregister(&fnode_sess->dev);
+}
+EXPORT_SYMBOL_GPL(iscsi_destroy_flashnode_sess);
+
+static int iscsi_iter_destroy_flashnode_fn(struct device *dev, void *data)
+{
+	if (!iscsi_flashnode_bus_match(dev, NULL))
+		return 0;
+
+	iscsi_destroy_flashnode_sess(iscsi_dev_to_flash_session(dev));
+	return 0;
+}
+
+/**
+ * iscsi_destroy_all_flashnode - destory all flashnode session entries
+ * @shost: pointer to host data
+ *
+ * Destroys all the flashnode session entries and all corresponding children
+ * flashnode connection entries from sysfs
+ */
+void iscsi_destroy_all_flashnode(struct Scsi_Host *shost)
+{
+	device_for_each_child(&shost->shost_gendev, NULL,
+			      iscsi_iter_destroy_flashnode_fn);
+}
+EXPORT_SYMBOL_GPL(iscsi_destroy_all_flashnode);
+
+/*
  * BSG support
  */
 /**
@@ -2092,6 +2776,294 @@ static int iscsi_delete_chap(struct iscsi_transport *transport,
 	return err;
 }
 
+static const struct {
+	enum iscsi_discovery_parent_type value;
+	char				*name;
+} iscsi_discovery_parent_names[] = {
+	{ISCSI_DISC_PARENT_UNKNOWN,	"Unknown" },
+	{ISCSI_DISC_PARENT_SENDTGT,	"Sendtarget" },
+	{ISCSI_DISC_PARENT_ISNS,	"isns" },
+};
+
+char *iscsi_get_discovery_parent_name(int parent_type)
+{
+	int i;
+	char *state = "Unknown!";
+
+	for (i = 0; i < ARRAY_SIZE(iscsi_discovery_parent_names); i++) {
+		if (iscsi_discovery_parent_names[i].value & parent_type) {
+			state = iscsi_discovery_parent_names[i].name;
+			break;
+		}
+	}
+	return state;
+}
+EXPORT_SYMBOL_GPL(iscsi_get_discovery_parent_name);
+
+static int iscsi_set_flashnode_param(struct iscsi_transport *transport,
+				     struct iscsi_uevent *ev, uint32_t len)
+{
+	char *data = (char *)ev + sizeof(*ev);
+	struct Scsi_Host *shost;
+	struct iscsi_bus_flash_session *fnode_sess;
+	struct iscsi_bus_flash_conn *fnode_conn;
+	struct device *dev;
+	uint32_t *idx;
+	int err = 0;
+
+	if (!transport->set_flashnode_param) {
+		err = -ENOSYS;
+		goto exit_set_fnode;
+	}
+
+	shost = scsi_host_lookup(ev->u.set_flashnode.host_no);
+	if (!shost) {
+		pr_err("%s could not find host no %u\n",
+		       __func__, ev->u.set_flashnode.host_no);
+		err = -ENODEV;
+		goto put_host;
+	}
+
+	idx = &ev->u.set_flashnode.flashnode_idx;
+	fnode_sess = iscsi_get_flashnode_by_index(shost, idx,
+						  flashnode_match_index);
+	if (!fnode_sess) {
+		pr_err("%s could not find flashnode %u for host no %u\n",
+		       __func__, *idx, ev->u.set_flashnode.host_no);
+		err = -ENODEV;
+		goto put_host;
+	}
+
+	dev = iscsi_find_flashnode_conn(fnode_sess, NULL,
+					iscsi_is_flashnode_conn_dev);
+	if (!dev) {
+		err = -ENODEV;
+		goto put_host;
+	}
+
+	fnode_conn = iscsi_dev_to_flash_conn(dev);
+	err = transport->set_flashnode_param(fnode_sess, fnode_conn, data, len);
+
+put_host:
+	scsi_host_put(shost);
+
+exit_set_fnode:
+	return err;
+}
+
+static int iscsi_new_flashnode(struct iscsi_transport *transport,
+			       struct iscsi_uevent *ev, uint32_t len)
+{
+	char *data = (char *)ev + sizeof(*ev);
+	struct Scsi_Host *shost;
+	int index;
+	int err = 0;
+
+	if (!transport->new_flashnode) {
+		err = -ENOSYS;
+		goto exit_new_fnode;
+	}
+
+	shost = scsi_host_lookup(ev->u.new_flashnode.host_no);
+	if (!shost) {
+		pr_err("%s could not find host no %u\n",
+		       __func__, ev->u.new_flashnode.host_no);
+		err = -ENODEV;
+		goto put_host;
+	}
+
+	index = transport->new_flashnode(shost, data, len);
+
+	if (index >= 0)
+		ev->r.new_flashnode_ret.flashnode_idx = index;
+	else
+		err = -EIO;
+
+put_host:
+	scsi_host_put(shost);
+
+exit_new_fnode:
+	return err;
+}
+
+static int iscsi_del_flashnode(struct iscsi_transport *transport,
+			       struct iscsi_uevent *ev)
+{
+	struct Scsi_Host *shost;
+	struct iscsi_bus_flash_session *fnode_sess;
+	uint32_t *idx;
+	int err = 0;
+
+	if (!transport->del_flashnode) {
+		err = -ENOSYS;
+		goto exit_del_fnode;
+	}
+
+	shost = scsi_host_lookup(ev->u.del_flashnode.host_no);
+	if (!shost) {
+		pr_err("%s could not find host no %u\n",
+		       __func__, ev->u.del_flashnode.host_no);
+		err = -ENODEV;
+		goto put_host;
+	}
+
+	idx = &ev->u.del_flashnode.flashnode_idx;
+	fnode_sess = iscsi_get_flashnode_by_index(shost, idx,
+						  flashnode_match_index);
+	if (!fnode_sess) {
+		pr_err("%s could not find flashnode %u for host no %u\n",
+		       __func__, *idx, ev->u.del_flashnode.host_no);
+		err = -ENODEV;
+		goto put_host;
+	}
+
+	err = transport->del_flashnode(fnode_sess);
+
+put_host:
+	scsi_host_put(shost);
+
+exit_del_fnode:
+	return err;
+}
+
+static int iscsi_login_flashnode(struct iscsi_transport *transport,
+				 struct iscsi_uevent *ev)
+{
+	struct Scsi_Host *shost;
+	struct iscsi_bus_flash_session *fnode_sess;
+	struct iscsi_bus_flash_conn *fnode_conn;
+	struct device *dev;
+	uint32_t *idx;
+	int err = 0;
+
+	if (!transport->login_flashnode) {
+		err = -ENOSYS;
+		goto exit_login_fnode;
+	}
+
+	shost = scsi_host_lookup(ev->u.login_flashnode.host_no);
+	if (!shost) {
+		pr_err("%s could not find host no %u\n",
+		       __func__, ev->u.login_flashnode.host_no);
+		err = -ENODEV;
+		goto put_host;
+	}
+
+	idx = &ev->u.login_flashnode.flashnode_idx;
+	fnode_sess = iscsi_get_flashnode_by_index(shost, idx,
+						  flashnode_match_index);
+	if (!fnode_sess) {
+		pr_err("%s could not find flashnode %u for host no %u\n",
+		       __func__, *idx, ev->u.login_flashnode.host_no);
+		err = -ENODEV;
+		goto put_host;
+	}
+
+	dev = iscsi_find_flashnode_conn(fnode_sess, NULL,
+					iscsi_is_flashnode_conn_dev);
+	if (!dev) {
+		err = -ENODEV;
+		goto put_host;
+	}
+
+	fnode_conn = iscsi_dev_to_flash_conn(dev);
+	err = transport->login_flashnode(fnode_sess, fnode_conn);
+
+put_host:
+	scsi_host_put(shost);
+
+exit_login_fnode:
+	return err;
+}
+
+static int iscsi_logout_flashnode(struct iscsi_transport *transport,
+				  struct iscsi_uevent *ev)
+{
+	struct Scsi_Host *shost;
+	struct iscsi_bus_flash_session *fnode_sess;
+	struct iscsi_bus_flash_conn *fnode_conn;
+	struct device *dev;
+	uint32_t *idx;
+	int err = 0;
+
+	if (!transport->logout_flashnode) {
+		err = -ENOSYS;
+		goto exit_logout_fnode;
+	}
+
+	shost = scsi_host_lookup(ev->u.logout_flashnode.host_no);
+	if (!shost) {
+		pr_err("%s could not find host no %u\n",
+		       __func__, ev->u.logout_flashnode.host_no);
+		err = -ENODEV;
+		goto put_host;
+	}
+
+	idx = &ev->u.logout_flashnode.flashnode_idx;
+	fnode_sess = iscsi_get_flashnode_by_index(shost, idx,
+						  flashnode_match_index);
+	if (!fnode_sess) {
+		pr_err("%s could not find flashnode %u for host no %u\n",
+		       __func__, *idx, ev->u.logout_flashnode.host_no);
+		err = -ENODEV;
+		goto put_host;
+	}
+
+	dev = iscsi_find_flashnode_conn(fnode_sess, NULL,
+					iscsi_is_flashnode_conn_dev);
+	if (!dev) {
+		err = -ENODEV;
+		goto put_host;
+	}
+
+	fnode_conn = iscsi_dev_to_flash_conn(dev);
+
+	err = transport->logout_flashnode(fnode_sess, fnode_conn);
+
+put_host:
+	scsi_host_put(shost);
+
+exit_logout_fnode:
+	return err;
+}
+
+static int iscsi_logout_flashnode_sid(struct iscsi_transport *transport,
+				      struct iscsi_uevent *ev)
+{
+	struct Scsi_Host *shost;
+	struct iscsi_cls_session *session;
+	int err = 0;
+
+	if (!transport->logout_flashnode_sid) {
+		err = -ENOSYS;
+		goto exit_logout_sid;
+	}
+
+	shost = scsi_host_lookup(ev->u.logout_flashnode_sid.host_no);
+	if (!shost) {
+		pr_err("%s could not find host no %u\n",
+		       __func__, ev->u.logout_flashnode.host_no);
+		err = -ENODEV;
+		goto put_host;
+	}
+
+	session = iscsi_session_lookup(ev->u.logout_flashnode_sid.sid);
+	if (!session) {
+		pr_err("%s could not find session id %u\n",
+		       __func__, ev->u.logout_flashnode_sid.sid);
+		err = -EINVAL;
+		goto put_host;
+	}
+
+	err = transport->logout_flashnode_sid(session);
+
+put_host:
+	scsi_host_put(shost);
+
+exit_logout_sid:
+	return err;
+}
+
 static int
 iscsi_if_recv_msg(struct sk_buff *skb, struct nlmsghdr *nlh, uint32_t *group)
 {
@@ -2246,6 +3218,27 @@ iscsi_if_recv_msg(struct sk_buff *skb, struct nlmsghdr *nlh, uint32_t *group)
 	case ISCSI_UEVENT_DELETE_CHAP:
 		err = iscsi_delete_chap(transport, ev);
 		break;
+	case ISCSI_UEVENT_SET_FLASHNODE_PARAMS:
+		err = iscsi_set_flashnode_param(transport, ev,
+						nlmsg_attrlen(nlh,
+							      sizeof(*ev)));
+		break;
+	case ISCSI_UEVENT_NEW_FLASHNODE:
+		err = iscsi_new_flashnode(transport, ev,
+					  nlmsg_attrlen(nlh, sizeof(*ev)));
+		break;
+	case ISCSI_UEVENT_DEL_FLASHNODE:
+		err = iscsi_del_flashnode(transport, ev);
+		break;
+	case ISCSI_UEVENT_LOGIN_FLASHNODE:
+		err = iscsi_login_flashnode(transport, ev);
+		break;
+	case ISCSI_UEVENT_LOGOUT_FLASHNODE:
+		err = iscsi_logout_flashnode(transport, ev);
+		break;
+	case ISCSI_UEVENT_LOGOUT_FLASHNODE_SID:
+		err = iscsi_logout_flashnode_sid(transport, ev);
+		break;
 	default:
 		err = -ENOSYS;
 		break;
@@ -2981,10 +3974,14 @@ static __init int iscsi_transport_init(void)
 	if (err)
 		goto unregister_conn_class;
 
+	err = bus_register(&iscsi_flashnode_bus);
+	if (err)
+		goto unregister_session_class;
+
 	nls = netlink_kernel_create(&init_net, NETLINK_ISCSI, &cfg);
 	if (!nls) {
 		err = -ENOBUFS;
-		goto unregister_session_class;
+		goto unregister_flashnode_bus;
 	}
 
 	iscsi_eh_timer_workq = create_singlethread_workqueue("iscsi_eh");
@@ -2995,6 +3992,8 @@ static __init int iscsi_transport_init(void)
 
 release_nls:
 	netlink_kernel_release(nls);
+unregister_flashnode_bus:
+	bus_unregister(&iscsi_flashnode_bus);
 unregister_session_class:
 	transport_class_unregister(&iscsi_session_class);
 unregister_conn_class:
@@ -3014,6 +4013,7 @@ static void __exit iscsi_transport_exit(void)
 {
 	destroy_workqueue(iscsi_eh_timer_workq);
 	netlink_kernel_release(nls);
+	bus_unregister(&iscsi_flashnode_bus);
 	transport_class_unregister(&iscsi_connection_class);
 	transport_class_unregister(&iscsi_session_class);
 	transport_class_unregister(&iscsi_host_class);
diff --git a/drivers/scsi/st.c b/drivers/scsi/st.c
index 86974471af6..2a32036a940 100644
--- a/drivers/scsi/st.c
+++ b/drivers/scsi/st.c
@@ -4112,6 +4112,10 @@ static int st_probe(struct device *dev)
 	tpnt->disk = disk;
 	disk->private_data = &tpnt->driver;
 	disk->queue = SDp->request_queue;
+	/* SCSI tape doesn't register this gendisk via add_disk().  Manually
+	 * take queue reference that release_disk() expects. */
+	if (!blk_get_queue(disk->queue))
+		goto out_put_disk;
 	tpnt->driver = &st_template;
 
 	tpnt->device = SDp;
@@ -4185,7 +4189,7 @@ static int st_probe(struct device *dev)
 	idr_preload_end();
 	if (error < 0) {
 		pr_warn("st: idr allocation failed: %d\n", error);
-		goto out_put_disk;
+		goto out_put_queue;
 	}
 	tpnt->index = error;
 	sprintf(disk->disk_name, "st%d", tpnt->index);
@@ -4211,6 +4215,8 @@ out_remove_devs:
 	spin_lock(&st_index_lock);
 	idr_remove(&st_index_idr, tpnt->index);
 	spin_unlock(&st_index_lock);
+out_put_queue:
+	blk_put_queue(disk->queue);
 out_put_disk:
 	put_disk(disk);
 	kfree(tpnt);
diff --git a/drivers/spi/Kconfig b/drivers/spi/Kconfig
index f80eee74a31..141d8c10b76 100644
--- a/drivers/spi/Kconfig
+++ b/drivers/spi/Kconfig
@@ -55,6 +55,7 @@ comment "SPI Master Controller Drivers"
 
 config SPI_ALTERA
 	tristate "Altera SPI Controller"
+	depends on GENERIC_HARDIRQS
 	select SPI_BITBANG
 	help
 	  This is the driver for the Altera SPI Controller.
@@ -74,6 +75,17 @@ config SPI_ATMEL
 	  This selects a driver for the Atmel SPI Controller, present on
 	  many AT32 (AVR32) and AT91 (ARM) chips.
 
+config SPI_BCM2835
+	tristate "BCM2835 SPI controller"
+	depends on ARCH_BCM2835
+	help
+	  This selects a driver for the Broadcom BCM2835 SPI master.
+
+	  The BCM2835 contains two types of SPI master controller; the
+	  "universal SPI master", and the regular SPI controller. This driver
+	  is for the regular SPI controller. Slave mode operation is not also
+	  not supported.
+
 config SPI_BFIN5XX
 	tristate "SPI controller driver for ADI Blackfin5xx"
 	depends on BLACKFIN
@@ -218,16 +230,23 @@ config SPI_MPC512x_PSC
 
 config SPI_FSL_LIB
 	tristate
+	depends on OF
+
+config SPI_FSL_CPM
+	tristate
 	depends on FSL_SOC
 
 config SPI_FSL_SPI
-	bool "Freescale SPI controller"
-	depends on FSL_SOC
+	bool "Freescale SPI controller and Aeroflex Gaisler GRLIB SPI controller"
+	depends on OF
 	select SPI_FSL_LIB
+	select SPI_FSL_CPM if FSL_SOC
 	help
 	  This enables using the Freescale SPI controllers in master mode.
 	  MPC83xx platform uses the controller in cpu mode or CPM/QE mode.
 	  MPC8569 uses the controller in QE mode, MPC8610 in cpu mode.
+	  This also enables using the Aeroflex Gaisler GRLIB SPI controller in
+	  master mode.
 
 config SPI_FSL_ESPI
 	bool "Freescale eSPI controller"
@@ -310,7 +329,7 @@ config SPI_PXA2XX_DMA
 
 config SPI_PXA2XX
 	tristate "PXA2xx SSP SPI master"
-	depends on ARCH_PXA || PCI || ACPI
+	depends on (ARCH_PXA || PCI || ACPI) && GENERIC_HARDIRQS
 	select PXA_SSP if ARCH_PXA
 	help
 	  This enables using a PXA2xx or Sodaville SSP port as a SPI master
@@ -397,6 +416,14 @@ config SPI_MXS
 	help
 	  SPI driver for Freescale MXS devices.
 
+config SPI_TEGRA114
+	tristate "NVIDIA Tegra114 SPI Controller"
+	depends on ARCH_TEGRA && TEGRA20_APB_DMA
+	help
+	  SPI driver for NVIDIA Tegra114 SPI Controller interface. This controller
+	  is different than the older SoCs SPI controller and also register interface
+	  get changed with this controller.
+
 config SPI_TEGRA20_SFLASH
 	tristate "Nvidia Tegra20 Serial flash Controller"
 	depends on ARCH_TEGRA
diff --git a/drivers/spi/Makefile b/drivers/spi/Makefile
index e53c3094134..33f9c09561e 100644
--- a/drivers/spi/Makefile
+++ b/drivers/spi/Makefile
@@ -14,6 +14,7 @@ obj-$(CONFIG_SPI_ALTERA)		+= spi-altera.o
 obj-$(CONFIG_SPI_ATMEL)			+= spi-atmel.o
 obj-$(CONFIG_SPI_ATH79)			+= spi-ath79.o
 obj-$(CONFIG_SPI_AU1550)		+= spi-au1550.o
+obj-$(CONFIG_SPI_BCM2835)		+= spi-bcm2835.o
 obj-$(CONFIG_SPI_BCM63XX)		+= spi-bcm63xx.o
 obj-$(CONFIG_SPI_BFIN5XX)		+= spi-bfin5xx.o
 obj-$(CONFIG_SPI_BFIN_SPORT)		+= spi-bfin-sport.o
@@ -28,6 +29,7 @@ obj-$(CONFIG_SPI_DW_PCI)		+= spi-dw-midpci.o
 spi-dw-midpci-objs			:= spi-dw-pci.o spi-dw-mid.o
 obj-$(CONFIG_SPI_EP93XX)		+= spi-ep93xx.o
 obj-$(CONFIG_SPI_FALCON)		+= spi-falcon.o
+obj-$(CONFIG_SPI_FSL_CPM)		+= spi-fsl-cpm.o
 obj-$(CONFIG_SPI_FSL_LIB)		+= spi-fsl-lib.o
 obj-$(CONFIG_SPI_FSL_ESPI)		+= spi-fsl-espi.o
 obj-$(CONFIG_SPI_FSL_SPI)		+= spi-fsl-spi.o
@@ -63,6 +65,7 @@ obj-$(CONFIG_SPI_SH_HSPI)		+= spi-sh-hspi.o
 obj-$(CONFIG_SPI_SH_MSIOF)		+= spi-sh-msiof.o
 obj-$(CONFIG_SPI_SH_SCI)		+= spi-sh-sci.o
 obj-$(CONFIG_SPI_SIRF)		+= spi-sirf.o
+obj-$(CONFIG_SPI_TEGRA114)		+= spi-tegra114.o
 obj-$(CONFIG_SPI_TEGRA20_SFLASH)	+= spi-tegra20-sflash.o
 obj-$(CONFIG_SPI_TEGRA20_SLINK)		+= spi-tegra20-slink.o
 obj-$(CONFIG_SPI_TI_SSP)		+= spi-ti-ssp.o
diff --git a/drivers/spi/spi-atmel.c b/drivers/spi/spi-atmel.c
index 656d137db25..787bd2c22bc 100644
--- a/drivers/spi/spi-atmel.c
+++ b/drivers/spi/spi-atmel.c
@@ -15,16 +15,17 @@
 #include <linux/platform_device.h>
 #include <linux/delay.h>
 #include <linux/dma-mapping.h>
+#include <linux/dmaengine.h>
 #include <linux/err.h>
 #include <linux/interrupt.h>
 #include <linux/spi/spi.h>
 #include <linux/slab.h>
 #include <linux/platform_data/atmel.h>
+#include <linux/platform_data/dma-atmel.h>
 #include <linux/of.h>
 
-#include <asm/io.h>
-#include <asm/gpio.h>
-#include <mach/cpu.h>
+#include <linux/io.h>
+#include <linux/gpio.h>
 
 /* SPI register offsets */
 #define SPI_CR					0x0000
@@ -39,6 +40,7 @@
 #define SPI_CSR1				0x0034
 #define SPI_CSR2				0x0038
 #define SPI_CSR3				0x003c
+#define SPI_VERSION				0x00fc
 #define SPI_RPR					0x0100
 #define SPI_RCR					0x0104
 #define SPI_TPR					0x0108
@@ -71,6 +73,8 @@
 #define SPI_FDIV_SIZE				1
 #define SPI_MODFDIS_OFFSET			4
 #define SPI_MODFDIS_SIZE			1
+#define SPI_WDRBT_OFFSET			5
+#define SPI_WDRBT_SIZE				1
 #define SPI_LLB_OFFSET				7
 #define SPI_LLB_SIZE				1
 #define SPI_PCS_OFFSET				16
@@ -180,6 +184,27 @@
 #define spi_writel(port,reg,value) \
 	__raw_writel((value), (port)->regs + SPI_##reg)
 
+/* use PIO for small transfers, avoiding DMA setup/teardown overhead and
+ * cache operations; better heuristics consider wordsize and bitrate.
+ */
+#define DMA_MIN_BYTES	16
+
+struct atmel_spi_dma {
+	struct dma_chan			*chan_rx;
+	struct dma_chan			*chan_tx;
+	struct scatterlist		sgrx;
+	struct scatterlist		sgtx;
+	struct dma_async_tx_descriptor	*data_desc_rx;
+	struct dma_async_tx_descriptor	*data_desc_tx;
+
+	struct at_dma_slave	dma_slave;
+};
+
+struct atmel_spi_caps {
+	bool	is_spi2;
+	bool	has_wdrbt;
+	bool	has_dma_support;
+};
 
 /*
  * The core SPI transfer engine just talks to a register bank to set up
@@ -188,7 +213,9 @@
  */
 struct atmel_spi {
 	spinlock_t		lock;
+	unsigned long		flags;
 
+	phys_addr_t		phybase;
 	void __iomem		*regs;
 	int			irq;
 	struct clk		*clk;
@@ -197,13 +224,23 @@ struct atmel_spi {
 
 	u8			stopping;
 	struct list_head	queue;
+	struct tasklet_struct	tasklet;
 	struct spi_transfer	*current_transfer;
 	unsigned long		current_remaining_bytes;
 	struct spi_transfer	*next_transfer;
 	unsigned long		next_remaining_bytes;
+	int			done_status;
 
+	/* scratch buffer */
 	void			*buffer;
 	dma_addr_t		buffer_dma;
+
+	struct atmel_spi_caps	caps;
+
+	bool			use_dma;
+	bool			use_pdc;
+	/* dmaengine data */
+	struct atmel_spi_dma	dma;
 };
 
 /* Controller-specific per-slave state */
@@ -222,14 +259,10 @@ struct atmel_spi_device {
  *  - SPI_SR.TXEMPTY, SPI_SR.NSSR (and corresponding irqs)
  *  - SPI_CSRx.CSAAT
  *  - SPI_CSRx.SBCR allows faster clocking
- *
- * We can determine the controller version by reading the VERSION
- * register, but I haven't checked that it exists on all chips, and
- * this is cheaper anyway.
  */
-static bool atmel_spi_is_v2(void)
+static bool atmel_spi_is_v2(struct atmel_spi *as)
 {
-	return !cpu_is_at91rm9200();
+	return as->caps.is_spi2;
 }
 
 /*
@@ -250,11 +283,6 @@ static bool atmel_spi_is_v2(void)
  * Master on Chip Select 0.")  No workaround exists for that ... so for
  * nCS0 on that chip, we (a) don't use the GPIO, (b) can't support CS_HIGH,
  * and (c) will trigger that first erratum in some cases.
- *
- * TODO: Test if the atmel_spi_is_v2() branch below works on
- * AT91RM9200 if we use some other register than CSR0. However, don't
- * do this unconditionally since AP7000 has an errata where the BITS
- * field in CSR0 overrides all other CSRs.
  */
 
 static void cs_activate(struct atmel_spi *as, struct spi_device *spi)
@@ -263,15 +291,25 @@ static void cs_activate(struct atmel_spi *as, struct spi_device *spi)
 	unsigned active = spi->mode & SPI_CS_HIGH;
 	u32 mr;
 
-	if (atmel_spi_is_v2()) {
-		/*
-		 * Always use CSR0. This ensures that the clock
-		 * switches to the correct idle polarity before we
-		 * toggle the CS.
+	if (atmel_spi_is_v2(as)) {
+		spi_writel(as, CSR0 + 4 * spi->chip_select, asd->csr);
+		/* For the low SPI version, there is a issue that PDC transfer
+		 * on CS1,2,3 needs SPI_CSR0.BITS config as SPI_CSR1,2,3.BITS
 		 */
 		spi_writel(as, CSR0, asd->csr);
-		spi_writel(as, MR, SPI_BF(PCS, 0x0e) | SPI_BIT(MODFDIS)
-				| SPI_BIT(MSTR));
+		if (as->caps.has_wdrbt) {
+			spi_writel(as, MR,
+					SPI_BF(PCS, ~(0x01 << spi->chip_select))
+					| SPI_BIT(WDRBT)
+					| SPI_BIT(MODFDIS)
+					| SPI_BIT(MSTR));
+		} else {
+			spi_writel(as, MR,
+					SPI_BF(PCS, ~(0x01 << spi->chip_select))
+					| SPI_BIT(MODFDIS)
+					| SPI_BIT(MSTR));
+		}
+
 		mr = spi_readl(as, MR);
 		gpio_set_value(asd->npcs_pin, active);
 	} else {
@@ -318,10 +356,26 @@ static void cs_deactivate(struct atmel_spi *as, struct spi_device *spi)
 			asd->npcs_pin, active ? " (low)" : "",
 			mr);
 
-	if (atmel_spi_is_v2() || spi->chip_select != 0)
+	if (atmel_spi_is_v2(as) || spi->chip_select != 0)
 		gpio_set_value(asd->npcs_pin, !active);
 }
 
+static void atmel_spi_lock(struct atmel_spi *as)
+{
+	spin_lock_irqsave(&as->lock, as->flags);
+}
+
+static void atmel_spi_unlock(struct atmel_spi *as)
+{
+	spin_unlock_irqrestore(&as->lock, as->flags);
+}
+
+static inline bool atmel_spi_use_dma(struct atmel_spi *as,
+				struct spi_transfer *xfer)
+{
+	return as->use_dma && xfer->len >= DMA_MIN_BYTES;
+}
+
 static inline int atmel_spi_xfer_is_last(struct spi_message *msg,
 					struct spi_transfer *xfer)
 {
@@ -333,6 +387,265 @@ static inline int atmel_spi_xfer_can_be_chained(struct spi_transfer *xfer)
 	return xfer->delay_usecs == 0 && !xfer->cs_change;
 }
 
+static int atmel_spi_dma_slave_config(struct atmel_spi *as,
+				struct dma_slave_config *slave_config,
+				u8 bits_per_word)
+{
+	int err = 0;
+
+	if (bits_per_word > 8) {
+		slave_config->dst_addr_width = DMA_SLAVE_BUSWIDTH_2_BYTES;
+		slave_config->src_addr_width = DMA_SLAVE_BUSWIDTH_2_BYTES;
+	} else {
+		slave_config->dst_addr_width = DMA_SLAVE_BUSWIDTH_1_BYTE;
+		slave_config->src_addr_width = DMA_SLAVE_BUSWIDTH_1_BYTE;
+	}
+
+	slave_config->dst_addr = (dma_addr_t)as->phybase + SPI_TDR;
+	slave_config->src_addr = (dma_addr_t)as->phybase + SPI_RDR;
+	slave_config->src_maxburst = 1;
+	slave_config->dst_maxburst = 1;
+	slave_config->device_fc = false;
+
+	slave_config->direction = DMA_MEM_TO_DEV;
+	if (dmaengine_slave_config(as->dma.chan_tx, slave_config)) {
+		dev_err(&as->pdev->dev,
+			"failed to configure tx dma channel\n");
+		err = -EINVAL;
+	}
+
+	slave_config->direction = DMA_DEV_TO_MEM;
+	if (dmaengine_slave_config(as->dma.chan_rx, slave_config)) {
+		dev_err(&as->pdev->dev,
+			"failed to configure rx dma channel\n");
+		err = -EINVAL;
+	}
+
+	return err;
+}
+
+static bool filter(struct dma_chan *chan, void *slave)
+{
+	struct	at_dma_slave *sl = slave;
+
+	if (sl->dma_dev == chan->device->dev) {
+		chan->private = sl;
+		return true;
+	} else {
+		return false;
+	}
+}
+
+static int atmel_spi_configure_dma(struct atmel_spi *as)
+{
+	struct at_dma_slave *sdata = &as->dma.dma_slave;
+	struct dma_slave_config	slave_config;
+	int err;
+
+	if (sdata && sdata->dma_dev) {
+		dma_cap_mask_t mask;
+
+		/* Try to grab two DMA channels */
+		dma_cap_zero(mask);
+		dma_cap_set(DMA_SLAVE, mask);
+		as->dma.chan_tx = dma_request_channel(mask, filter, sdata);
+		if (as->dma.chan_tx)
+			as->dma.chan_rx =
+				dma_request_channel(mask, filter, sdata);
+	}
+	if (!as->dma.chan_rx || !as->dma.chan_tx) {
+		dev_err(&as->pdev->dev,
+			"DMA channel not available, SPI unable to use DMA\n");
+		err = -EBUSY;
+		goto error;
+	}
+
+	err = atmel_spi_dma_slave_config(as, &slave_config, 8);
+	if (err)
+		goto error;
+
+	dev_info(&as->pdev->dev,
+			"Using %s (tx) and %s (rx) for DMA transfers\n",
+			dma_chan_name(as->dma.chan_tx),
+			dma_chan_name(as->dma.chan_rx));
+	return 0;
+error:
+	if (as->dma.chan_rx)
+		dma_release_channel(as->dma.chan_rx);
+	if (as->dma.chan_tx)
+		dma_release_channel(as->dma.chan_tx);
+	return err;
+}
+
+static void atmel_spi_stop_dma(struct atmel_spi *as)
+{
+	if (as->dma.chan_rx)
+		as->dma.chan_rx->device->device_control(as->dma.chan_rx,
+							DMA_TERMINATE_ALL, 0);
+	if (as->dma.chan_tx)
+		as->dma.chan_tx->device->device_control(as->dma.chan_tx,
+							DMA_TERMINATE_ALL, 0);
+}
+
+static void atmel_spi_release_dma(struct atmel_spi *as)
+{
+	if (as->dma.chan_rx)
+		dma_release_channel(as->dma.chan_rx);
+	if (as->dma.chan_tx)
+		dma_release_channel(as->dma.chan_tx);
+}
+
+/* This function is called by the DMA driver from tasklet context */
+static void dma_callback(void *data)
+{
+	struct spi_master	*master = data;
+	struct atmel_spi	*as = spi_master_get_devdata(master);
+
+	/* trigger SPI tasklet */
+	tasklet_schedule(&as->tasklet);
+}
+
+/*
+ * Next transfer using PIO.
+ * lock is held, spi tasklet is blocked
+ */
+static void atmel_spi_next_xfer_pio(struct spi_master *master,
+				struct spi_transfer *xfer)
+{
+	struct atmel_spi	*as = spi_master_get_devdata(master);
+
+	dev_vdbg(master->dev.parent, "atmel_spi_next_xfer_pio\n");
+
+	as->current_remaining_bytes = xfer->len;
+
+	/* Make sure data is not remaining in RDR */
+	spi_readl(as, RDR);
+	while (spi_readl(as, SR) & SPI_BIT(RDRF)) {
+		spi_readl(as, RDR);
+		cpu_relax();
+	}
+
+	if (xfer->tx_buf)
+		spi_writel(as, TDR, *(u8 *)(xfer->tx_buf));
+	else
+		spi_writel(as, TDR, 0);
+
+	dev_dbg(master->dev.parent,
+		"  start pio xfer %p: len %u tx %p rx %p\n",
+		xfer, xfer->len, xfer->tx_buf, xfer->rx_buf);
+
+	/* Enable relevant interrupts */
+	spi_writel(as, IER, SPI_BIT(RDRF) | SPI_BIT(OVRES));
+}
+
+/*
+ * Submit next transfer for DMA.
+ * lock is held, spi tasklet is blocked
+ */
+static int atmel_spi_next_xfer_dma_submit(struct spi_master *master,
+				struct spi_transfer *xfer,
+				u32 *plen)
+{
+	struct atmel_spi	*as = spi_master_get_devdata(master);
+	struct dma_chan		*rxchan = as->dma.chan_rx;
+	struct dma_chan		*txchan = as->dma.chan_tx;
+	struct dma_async_tx_descriptor *rxdesc;
+	struct dma_async_tx_descriptor *txdesc;
+	struct dma_slave_config	slave_config;
+	dma_cookie_t		cookie;
+	u32	len = *plen;
+
+	dev_vdbg(master->dev.parent, "atmel_spi_next_xfer_dma_submit\n");
+
+	/* Check that the channels are available */
+	if (!rxchan || !txchan)
+		return -ENODEV;
+
+	/* release lock for DMA operations */
+	atmel_spi_unlock(as);
+
+	/* prepare the RX dma transfer */
+	sg_init_table(&as->dma.sgrx, 1);
+	if (xfer->rx_buf) {
+		as->dma.sgrx.dma_address = xfer->rx_dma + xfer->len - *plen;
+	} else {
+		as->dma.sgrx.dma_address = as->buffer_dma;
+		if (len > BUFFER_SIZE)
+			len = BUFFER_SIZE;
+	}
+
+	/* prepare the TX dma transfer */
+	sg_init_table(&as->dma.sgtx, 1);
+	if (xfer->tx_buf) {
+		as->dma.sgtx.dma_address = xfer->tx_dma + xfer->len - *plen;
+	} else {
+		as->dma.sgtx.dma_address = as->buffer_dma;
+		if (len > BUFFER_SIZE)
+			len = BUFFER_SIZE;
+		memset(as->buffer, 0, len);
+	}
+
+	sg_dma_len(&as->dma.sgtx) = len;
+	sg_dma_len(&as->dma.sgrx) = len;
+
+	*plen = len;
+
+	if (atmel_spi_dma_slave_config(as, &slave_config, 8))
+		goto err_exit;
+
+	/* Send both scatterlists */
+	rxdesc = rxchan->device->device_prep_slave_sg(rxchan,
+					&as->dma.sgrx,
+					1,
+					DMA_FROM_DEVICE,
+					DMA_PREP_INTERRUPT | DMA_CTRL_ACK,
+					NULL);
+	if (!rxdesc)
+		goto err_dma;
+
+	txdesc = txchan->device->device_prep_slave_sg(txchan,
+					&as->dma.sgtx,
+					1,
+					DMA_TO_DEVICE,
+					DMA_PREP_INTERRUPT | DMA_CTRL_ACK,
+					NULL);
+	if (!txdesc)
+		goto err_dma;
+
+	dev_dbg(master->dev.parent,
+		"  start dma xfer %p: len %u tx %p/%08x rx %p/%08x\n",
+		xfer, xfer->len, xfer->tx_buf, xfer->tx_dma,
+		xfer->rx_buf, xfer->rx_dma);
+
+	/* Enable relevant interrupts */
+	spi_writel(as, IER, SPI_BIT(OVRES));
+
+	/* Put the callback on the RX transfer only, that should finish last */
+	rxdesc->callback = dma_callback;
+	rxdesc->callback_param = master;
+
+	/* Submit and fire RX and TX with TX last so we're ready to read! */
+	cookie = rxdesc->tx_submit(rxdesc);
+	if (dma_submit_error(cookie))
+		goto err_dma;
+	cookie = txdesc->tx_submit(txdesc);
+	if (dma_submit_error(cookie))
+		goto err_dma;
+	rxchan->device->device_issue_pending(rxchan);
+	txchan->device->device_issue_pending(txchan);
+
+	/* take back lock */
+	atmel_spi_lock(as);
+	return 0;
+
+err_dma:
+	spi_writel(as, IDR, SPI_BIT(OVRES));
+	atmel_spi_stop_dma(as);
+err_exit:
+	atmel_spi_lock(as);
+	return -ENOMEM;
+}
+
 static void atmel_spi_next_xfer_data(struct spi_master *master,
 				struct spi_transfer *xfer,
 				dma_addr_t *tx_dma,
@@ -350,6 +663,7 @@ static void atmel_spi_next_xfer_data(struct spi_master *master,
 		if (len > BUFFER_SIZE)
 			len = BUFFER_SIZE;
 	}
+
 	if (xfer->tx_buf)
 		*tx_dma = xfer->tx_dma + xfer->len - *plen;
 	else {
@@ -365,10 +679,10 @@ static void atmel_spi_next_xfer_data(struct spi_master *master,
 }
 
 /*
- * Submit next transfer for DMA.
+ * Submit next transfer for PDC.
  * lock is held, spi irq is blocked
  */
-static void atmel_spi_next_xfer(struct spi_master *master,
+static void atmel_spi_pdc_next_xfer(struct spi_master *master,
 				struct spi_message *msg)
 {
 	struct atmel_spi	*as = spi_master_get_devdata(master);
@@ -465,6 +779,48 @@ static void atmel_spi_next_xfer(struct spi_master *master,
 	spi_writel(as, PTCR, SPI_BIT(TXTEN) | SPI_BIT(RXTEN));
 }
 
+/*
+ * Choose way to submit next transfer and start it.
+ * lock is held, spi tasklet is blocked
+ */
+static void atmel_spi_dma_next_xfer(struct spi_master *master,
+				struct spi_message *msg)
+{
+	struct atmel_spi	*as = spi_master_get_devdata(master);
+	struct spi_transfer	*xfer;
+	u32	remaining, len;
+
+	remaining = as->current_remaining_bytes;
+	if (remaining) {
+		xfer = as->current_transfer;
+		len = remaining;
+	} else {
+		if (!as->current_transfer)
+			xfer = list_entry(msg->transfers.next,
+				struct spi_transfer, transfer_list);
+		else
+			xfer = list_entry(
+				as->current_transfer->transfer_list.next,
+					struct spi_transfer, transfer_list);
+
+		as->current_transfer = xfer;
+		len = xfer->len;
+	}
+
+	if (atmel_spi_use_dma(as, xfer)) {
+		u32 total = len;
+		if (!atmel_spi_next_xfer_dma_submit(master, xfer, &len)) {
+			as->current_remaining_bytes = total - len;
+			return;
+		} else {
+			dev_err(&msg->spi->dev, "unable to use DMA, fallback to PIO\n");
+		}
+	}
+
+	/* use PIO if error appened using DMA */
+	atmel_spi_next_xfer_pio(master, xfer);
+}
+
 static void atmel_spi_next_message(struct spi_master *master)
 {
 	struct atmel_spi	*as = spi_master_get_devdata(master);
@@ -489,7 +845,10 @@ static void atmel_spi_next_message(struct spi_master *master)
 	} else
 		cs_activate(as, spi);
 
-	atmel_spi_next_xfer(master, msg);
+	if (as->use_pdc)
+		atmel_spi_pdc_next_xfer(master, msg);
+	else
+		atmel_spi_dma_next_xfer(master, msg);
 }
 
 /*
@@ -542,38 +901,213 @@ static void atmel_spi_dma_unmap_xfer(struct spi_master *master,
 				 xfer->len, DMA_FROM_DEVICE);
 }
 
+static void atmel_spi_disable_pdc_transfer(struct atmel_spi *as)
+{
+	spi_writel(as, PTCR, SPI_BIT(RXTDIS) | SPI_BIT(TXTDIS));
+}
+
 static void
 atmel_spi_msg_done(struct spi_master *master, struct atmel_spi *as,
-		struct spi_message *msg, int status, int stay)
+		struct spi_message *msg, int stay)
 {
-	if (!stay || status < 0)
+	if (!stay || as->done_status < 0)
 		cs_deactivate(as, msg->spi);
 	else
 		as->stay = msg->spi;
 
 	list_del(&msg->queue);
-	msg->status = status;
+	msg->status = as->done_status;
 
 	dev_dbg(master->dev.parent,
 		"xfer complete: %u bytes transferred\n",
 		msg->actual_length);
 
-	spin_unlock(&as->lock);
+	atmel_spi_unlock(as);
 	msg->complete(msg->context);
-	spin_lock(&as->lock);
+	atmel_spi_lock(as);
 
 	as->current_transfer = NULL;
 	as->next_transfer = NULL;
+	as->done_status = 0;
 
 	/* continue if needed */
-	if (list_empty(&as->queue) || as->stopping)
-		spi_writel(as, PTCR, SPI_BIT(RXTDIS) | SPI_BIT(TXTDIS));
-	else
+	if (list_empty(&as->queue) || as->stopping) {
+		if (as->use_pdc)
+			atmel_spi_disable_pdc_transfer(as);
+	} else {
 		atmel_spi_next_message(master);
+	}
+}
+
+/* Called from IRQ
+ * lock is held
+ *
+ * Must update "current_remaining_bytes" to keep track of data
+ * to transfer.
+ */
+static void
+atmel_spi_pump_pio_data(struct atmel_spi *as, struct spi_transfer *xfer)
+{
+	u8		*txp;
+	u8		*rxp;
+	unsigned long	xfer_pos = xfer->len - as->current_remaining_bytes;
+
+	if (xfer->rx_buf) {
+		rxp = ((u8 *)xfer->rx_buf) + xfer_pos;
+		*rxp = spi_readl(as, RDR);
+	} else {
+		spi_readl(as, RDR);
+	}
+
+	as->current_remaining_bytes--;
+
+	if (as->current_remaining_bytes) {
+		if (xfer->tx_buf) {
+			txp = ((u8 *)xfer->tx_buf) + xfer_pos + 1;
+			spi_writel(as, TDR, *txp);
+		} else {
+			spi_writel(as, TDR, 0);
+		}
+	}
+}
+
+/* Tasklet
+ * Called from DMA callback + pio transfer and overrun IRQ.
+ */
+static void atmel_spi_tasklet_func(unsigned long data)
+{
+	struct spi_master	*master = (struct spi_master *)data;
+	struct atmel_spi	*as = spi_master_get_devdata(master);
+	struct spi_message	*msg;
+	struct spi_transfer	*xfer;
+
+	dev_vdbg(master->dev.parent, "atmel_spi_tasklet_func\n");
+
+	atmel_spi_lock(as);
+
+	xfer = as->current_transfer;
+
+	if (xfer == NULL)
+		/* already been there */
+		goto tasklet_out;
+
+	msg = list_entry(as->queue.next, struct spi_message, queue);
+
+	if (as->current_remaining_bytes == 0) {
+		if (as->done_status < 0) {
+			/* error happened (overrun) */
+			if (atmel_spi_use_dma(as, xfer))
+				atmel_spi_stop_dma(as);
+		} else {
+			/* only update length if no error */
+			msg->actual_length += xfer->len;
+		}
+
+		if (atmel_spi_use_dma(as, xfer))
+			if (!msg->is_dma_mapped)
+				atmel_spi_dma_unmap_xfer(master, xfer);
+
+		if (xfer->delay_usecs)
+			udelay(xfer->delay_usecs);
+
+		if (atmel_spi_xfer_is_last(msg, xfer) || as->done_status < 0) {
+			/* report completed (or erroneous) message */
+			atmel_spi_msg_done(master, as, msg, xfer->cs_change);
+		} else {
+			if (xfer->cs_change) {
+				cs_deactivate(as, msg->spi);
+				udelay(1);
+				cs_activate(as, msg->spi);
+			}
+
+			/*
+			 * Not done yet. Submit the next transfer.
+			 *
+			 * FIXME handle protocol options for xfer
+			 */
+			atmel_spi_dma_next_xfer(master, msg);
+		}
+	} else {
+		/*
+		 * Keep going, we still have data to send in
+		 * the current transfer.
+		 */
+		atmel_spi_dma_next_xfer(master, msg);
+	}
+
+tasklet_out:
+	atmel_spi_unlock(as);
 }
 
+/* Interrupt
+ *
+ * No need for locking in this Interrupt handler: done_status is the
+ * only information modified. What we need is the update of this field
+ * before tasklet runs. This is ensured by using barrier.
+ */
 static irqreturn_t
-atmel_spi_interrupt(int irq, void *dev_id)
+atmel_spi_pio_interrupt(int irq, void *dev_id)
+{
+	struct spi_master	*master = dev_id;
+	struct atmel_spi	*as = spi_master_get_devdata(master);
+	u32			status, pending, imr;
+	struct spi_transfer	*xfer;
+	int			ret = IRQ_NONE;
+
+	imr = spi_readl(as, IMR);
+	status = spi_readl(as, SR);
+	pending = status & imr;
+
+	if (pending & SPI_BIT(OVRES)) {
+		ret = IRQ_HANDLED;
+		spi_writel(as, IDR, SPI_BIT(OVRES));
+		dev_warn(master->dev.parent, "overrun\n");
+
+		/*
+		 * When we get an overrun, we disregard the current
+		 * transfer. Data will not be copied back from any
+		 * bounce buffer and msg->actual_len will not be
+		 * updated with the last xfer.
+		 *
+		 * We will also not process any remaning transfers in
+		 * the message.
+		 *
+		 * All actions are done in tasklet with done_status indication
+		 */
+		as->done_status = -EIO;
+		smp_wmb();
+
+		/* Clear any overrun happening while cleaning up */
+		spi_readl(as, SR);
+
+		tasklet_schedule(&as->tasklet);
+
+	} else if (pending & SPI_BIT(RDRF)) {
+		atmel_spi_lock(as);
+
+		if (as->current_remaining_bytes) {
+			ret = IRQ_HANDLED;
+			xfer = as->current_transfer;
+			atmel_spi_pump_pio_data(as, xfer);
+			if (!as->current_remaining_bytes) {
+				/* no more data to xfer, kick tasklet */
+				spi_writel(as, IDR, pending);
+				tasklet_schedule(&as->tasklet);
+			}
+		}
+
+		atmel_spi_unlock(as);
+	} else {
+		WARN_ONCE(pending, "IRQ not handled, pending = %x\n", pending);
+		ret = IRQ_HANDLED;
+		spi_writel(as, IDR, pending);
+	}
+
+	return ret;
+}
+
+static irqreturn_t
+atmel_spi_pdc_interrupt(int irq, void *dev_id)
 {
 	struct spi_master	*master = dev_id;
 	struct atmel_spi	*as = spi_master_get_devdata(master);
@@ -582,7 +1116,7 @@ atmel_spi_interrupt(int irq, void *dev_id)
 	u32			status, pending, imr;
 	int			ret = IRQ_NONE;
 
-	spin_lock(&as->lock);
+	atmel_spi_lock(as);
 
 	xfer = as->current_transfer;
 	msg = list_entry(as->queue.next, struct spi_message, queue);
@@ -641,7 +1175,8 @@ atmel_spi_interrupt(int irq, void *dev_id)
 		/* Clear any overrun happening while cleaning up */
 		spi_readl(as, SR);
 
-		atmel_spi_msg_done(master, as, msg, -EIO, 0);
+		as->done_status = -EIO;
+		atmel_spi_msg_done(master, as, msg, 0);
 	} else if (pending & (SPI_BIT(RXBUFF) | SPI_BIT(ENDRX))) {
 		ret = IRQ_HANDLED;
 
@@ -659,7 +1194,7 @@ atmel_spi_interrupt(int irq, void *dev_id)
 
 			if (atmel_spi_xfer_is_last(msg, xfer)) {
 				/* report completed message */
-				atmel_spi_msg_done(master, as, msg, 0,
+				atmel_spi_msg_done(master, as, msg,
 						xfer->cs_change);
 			} else {
 				if (xfer->cs_change) {
@@ -673,18 +1208,18 @@ atmel_spi_interrupt(int irq, void *dev_id)
 				 *
 				 * FIXME handle protocol options for xfer
 				 */
-				atmel_spi_next_xfer(master, msg);
+				atmel_spi_pdc_next_xfer(master, msg);
 			}
 		} else {
 			/*
 			 * Keep going, we still have data to send in
 			 * the current transfer.
 			 */
-			atmel_spi_next_xfer(master, msg);
+			atmel_spi_pdc_next_xfer(master, msg);
 		}
 	}
 
-	spin_unlock(&as->lock);
+	atmel_spi_unlock(as);
 
 	return ret;
 }
@@ -719,7 +1254,7 @@ static int atmel_spi_setup(struct spi_device *spi)
 	}
 
 	/* see notes above re chipselect */
-	if (!atmel_spi_is_v2()
+	if (!atmel_spi_is_v2(as)
 			&& spi->chip_select == 0
 			&& (spi->mode & SPI_CS_HIGH)) {
 		dev_dbg(&spi->dev, "setup: can't be active-high\n");
@@ -728,7 +1263,7 @@ static int atmel_spi_setup(struct spi_device *spi)
 
 	/* v1 chips start out at half the peripheral bus speed. */
 	bus_hz = clk_get_rate(as->clk);
-	if (!atmel_spi_is_v2())
+	if (!atmel_spi_is_v2(as))
 		bus_hz /= 2;
 
 	if (spi->max_speed_hz) {
@@ -789,13 +1324,11 @@ static int atmel_spi_setup(struct spi_device *spi)
 		spi->controller_state = asd;
 		gpio_direction_output(npcs_pin, !(spi->mode & SPI_CS_HIGH));
 	} else {
-		unsigned long		flags;
-
-		spin_lock_irqsave(&as->lock, flags);
+		atmel_spi_lock(as);
 		if (as->stay == spi)
 			as->stay = NULL;
 		cs_deactivate(as, spi);
-		spin_unlock_irqrestore(&as->lock, flags);
+		atmel_spi_unlock(as);
 	}
 
 	asd->csr = csr;
@@ -804,7 +1337,7 @@ static int atmel_spi_setup(struct spi_device *spi)
 		"setup: %lu Hz bpw %u mode 0x%x -> csr%d %08x\n",
 		bus_hz / scbr, bits, spi->mode, spi->chip_select, csr);
 
-	if (!atmel_spi_is_v2())
+	if (!atmel_spi_is_v2(as))
 		spi_writel(as, CSR0 + 4 * spi->chip_select, csr);
 
 	return 0;
@@ -814,7 +1347,6 @@ static int atmel_spi_transfer(struct spi_device *spi, struct spi_message *msg)
 {
 	struct atmel_spi	*as;
 	struct spi_transfer	*xfer;
-	unsigned long		flags;
 	struct device		*controller = spi->master->dev.parent;
 	u8			bits;
 	struct atmel_spi_device	*asd;
@@ -854,13 +1386,10 @@ static int atmel_spi_transfer(struct spi_device *spi, struct spi_message *msg)
 
 		/*
 		 * DMA map early, for performance (empties dcache ASAP) and
-		 * better fault reporting.  This is a DMA-only driver.
-		 *
-		 * NOTE that if dma_unmap_single() ever starts to do work on
-		 * platforms supported by this driver, we would need to clean
-		 * up mappings for previously-mapped transfers.
+		 * better fault reporting.
 		 */
-		if (!msg->is_dma_mapped) {
+		if ((!msg->is_dma_mapped) && (atmel_spi_use_dma(as, xfer)
+			|| as->use_pdc)) {
 			if (atmel_spi_dma_map_xfer(as, xfer) < 0)
 				return -ENOMEM;
 		}
@@ -879,11 +1408,11 @@ static int atmel_spi_transfer(struct spi_device *spi, struct spi_message *msg)
 	msg->status = -EINPROGRESS;
 	msg->actual_length = 0;
 
-	spin_lock_irqsave(&as->lock, flags);
+	atmel_spi_lock(as);
 	list_add_tail(&msg->queue, &as->queue);
 	if (!as->current_transfer)
 		atmel_spi_next_message(spi->master);
-	spin_unlock_irqrestore(&as->lock, flags);
+	atmel_spi_unlock(as);
 
 	return 0;
 }
@@ -893,23 +1422,39 @@ static void atmel_spi_cleanup(struct spi_device *spi)
 	struct atmel_spi	*as = spi_master_get_devdata(spi->master);
 	struct atmel_spi_device	*asd = spi->controller_state;
 	unsigned		gpio = (unsigned) spi->controller_data;
-	unsigned long		flags;
 
 	if (!asd)
 		return;
 
-	spin_lock_irqsave(&as->lock, flags);
+	atmel_spi_lock(as);
 	if (as->stay == spi) {
 		as->stay = NULL;
 		cs_deactivate(as, spi);
 	}
-	spin_unlock_irqrestore(&as->lock, flags);
+	atmel_spi_unlock(as);
 
 	spi->controller_state = NULL;
 	gpio_free(gpio);
 	kfree(asd);
 }
 
+static inline unsigned int atmel_get_version(struct atmel_spi *as)
+{
+	return spi_readl(as, VERSION) & 0x00000fff;
+}
+
+static void atmel_get_caps(struct atmel_spi *as)
+{
+	unsigned int version;
+
+	version = atmel_get_version(as);
+	dev_info(&as->pdev->dev, "version: 0x%x\n", version);
+
+	as->caps.is_spi2 = version > 0x121;
+	as->caps.has_wdrbt = version >= 0x210;
+	as->caps.has_dma_support = version >= 0x212;
+}
+
 /*-------------------------------------------------------------------------*/
 
 static int atmel_spi_probe(struct platform_device *pdev)
@@ -963,15 +1508,39 @@ static int atmel_spi_probe(struct platform_device *pdev)
 
 	spin_lock_init(&as->lock);
 	INIT_LIST_HEAD(&as->queue);
+
 	as->pdev = pdev;
 	as->regs = ioremap(regs->start, resource_size(regs));
 	if (!as->regs)
 		goto out_free_buffer;
+	as->phybase = regs->start;
 	as->irq = irq;
 	as->clk = clk;
 
-	ret = request_irq(irq, atmel_spi_interrupt, 0,
-			dev_name(&pdev->dev), master);
+	atmel_get_caps(as);
+
+	as->use_dma = false;
+	as->use_pdc = false;
+	if (as->caps.has_dma_support) {
+		if (atmel_spi_configure_dma(as) == 0)
+			as->use_dma = true;
+	} else {
+		as->use_pdc = true;
+	}
+
+	if (as->caps.has_dma_support && !as->use_dma)
+		dev_info(&pdev->dev, "Atmel SPI Controller using PIO only\n");
+
+	if (as->use_pdc) {
+		ret = request_irq(irq, atmel_spi_pdc_interrupt, 0,
+					dev_name(&pdev->dev), master);
+	} else {
+		tasklet_init(&as->tasklet, atmel_spi_tasklet_func,
+					(unsigned long)master);
+
+		ret = request_irq(irq, atmel_spi_pio_interrupt, 0,
+					dev_name(&pdev->dev), master);
+	}
 	if (ret)
 		goto out_unmap_regs;
 
@@ -979,8 +1548,15 @@ static int atmel_spi_probe(struct platform_device *pdev)
 	clk_enable(clk);
 	spi_writel(as, CR, SPI_BIT(SWRST));
 	spi_writel(as, CR, SPI_BIT(SWRST)); /* AT91SAM9263 Rev B workaround */
-	spi_writel(as, MR, SPI_BIT(MSTR) | SPI_BIT(MODFDIS));
-	spi_writel(as, PTCR, SPI_BIT(RXTDIS) | SPI_BIT(TXTDIS));
+	if (as->caps.has_wdrbt) {
+		spi_writel(as, MR, SPI_BIT(WDRBT) | SPI_BIT(MODFDIS)
+				| SPI_BIT(MSTR));
+	} else {
+		spi_writel(as, MR, SPI_BIT(MSTR) | SPI_BIT(MODFDIS));
+	}
+
+	if (as->use_pdc)
+		spi_writel(as, PTCR, SPI_BIT(RXTDIS) | SPI_BIT(TXTDIS));
 	spi_writel(as, CR, SPI_BIT(SPIEN));
 
 	/* go! */
@@ -989,11 +1565,14 @@ static int atmel_spi_probe(struct platform_device *pdev)
 
 	ret = spi_register_master(master);
 	if (ret)
-		goto out_reset_hw;
+		goto out_free_dma;
 
 	return 0;
 
-out_reset_hw:
+out_free_dma:
+	if (as->use_dma)
+		atmel_spi_release_dma(as);
+
 	spi_writel(as, CR, SPI_BIT(SWRST));
 	spi_writel(as, CR, SPI_BIT(SWRST)); /* AT91SAM9263 Rev B workaround */
 	clk_disable(clk);
@@ -1001,6 +1580,8 @@ out_reset_hw:
 out_unmap_regs:
 	iounmap(as->regs);
 out_free_buffer:
+	if (!as->use_pdc)
+		tasklet_kill(&as->tasklet);
 	dma_free_coherent(&pdev->dev, BUFFER_SIZE, as->buffer,
 			as->buffer_dma);
 out_free:
@@ -1014,10 +1595,16 @@ static int atmel_spi_remove(struct platform_device *pdev)
 	struct spi_master	*master = platform_get_drvdata(pdev);
 	struct atmel_spi	*as = spi_master_get_devdata(master);
 	struct spi_message	*msg;
+	struct spi_transfer	*xfer;
 
 	/* reset the hardware and block queue progress */
 	spin_lock_irq(&as->lock);
 	as->stopping = 1;
+	if (as->use_dma) {
+		atmel_spi_stop_dma(as);
+		atmel_spi_release_dma(as);
+	}
+
 	spi_writel(as, CR, SPI_BIT(SWRST));
 	spi_writel(as, CR, SPI_BIT(SWRST)); /* AT91SAM9263 Rev B workaround */
 	spi_readl(as, SR);
@@ -1025,13 +1612,18 @@ static int atmel_spi_remove(struct platform_device *pdev)
 
 	/* Terminate remaining queued transfers */
 	list_for_each_entry(msg, &as->queue, queue) {
-		/* REVISIT unmapping the dma is a NOP on ARM and AVR32
-		 * but we shouldn't depend on that...
-		 */
+		list_for_each_entry(xfer, &msg->transfers, transfer_list) {
+			if (!msg->is_dma_mapped
+				&& (atmel_spi_use_dma(as, xfer)
+					|| as->use_pdc))
+				atmel_spi_dma_unmap_xfer(master, xfer);
+		}
 		msg->status = -ESHUTDOWN;
 		msg->complete(msg->context);
 	}
 
+	if (!as->use_pdc)
+		tasklet_kill(&as->tasklet);
 	dma_free_coherent(&pdev->dev, BUFFER_SIZE, as->buffer,
 			as->buffer_dma);
 
diff --git a/drivers/spi/spi-bcm2835.c b/drivers/spi/spi-bcm2835.c
new file mode 100644
index 00000000000..89c0b503311
--- /dev/null
+++ b/drivers/spi/spi-bcm2835.c
@@ -0,0 +1,422 @@
+/*
+ * Driver for Broadcom BCM2835 SPI Controllers
+ *
+ * Copyright (C) 2012 Chris Boot
+ * Copyright (C) 2013 Stephen Warren
+ *
+ * This driver is inspired by:
+ * spi-ath79.c, Copyright (C) 2009-2011 Gabor Juhos <juhosg@openwrt.org>
+ * spi-atmel.c, Copyright (C) 2006 Atmel Corporation
+ *
+ * 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/clk.h>
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/of_device.h>
+#include <linux/spi/spi.h>
+
+/* SPI register offsets */
+#define BCM2835_SPI_CS			0x00
+#define BCM2835_SPI_FIFO		0x04
+#define BCM2835_SPI_CLK			0x08
+#define BCM2835_SPI_DLEN		0x0c
+#define BCM2835_SPI_LTOH		0x10
+#define BCM2835_SPI_DC			0x14
+
+/* Bitfields in CS */
+#define BCM2835_SPI_CS_LEN_LONG		0x02000000
+#define BCM2835_SPI_CS_DMA_LEN		0x01000000
+#define BCM2835_SPI_CS_CSPOL2		0x00800000
+#define BCM2835_SPI_CS_CSPOL1		0x00400000
+#define BCM2835_SPI_CS_CSPOL0		0x00200000
+#define BCM2835_SPI_CS_RXF		0x00100000
+#define BCM2835_SPI_CS_RXR		0x00080000
+#define BCM2835_SPI_CS_TXD		0x00040000
+#define BCM2835_SPI_CS_RXD		0x00020000
+#define BCM2835_SPI_CS_DONE		0x00010000
+#define BCM2835_SPI_CS_LEN		0x00002000
+#define BCM2835_SPI_CS_REN		0x00001000
+#define BCM2835_SPI_CS_ADCS		0x00000800
+#define BCM2835_SPI_CS_INTR		0x00000400
+#define BCM2835_SPI_CS_INTD		0x00000200
+#define BCM2835_SPI_CS_DMAEN		0x00000100
+#define BCM2835_SPI_CS_TA		0x00000080
+#define BCM2835_SPI_CS_CSPOL		0x00000040
+#define BCM2835_SPI_CS_CLEAR_RX		0x00000020
+#define BCM2835_SPI_CS_CLEAR_TX		0x00000010
+#define BCM2835_SPI_CS_CPOL		0x00000008
+#define BCM2835_SPI_CS_CPHA		0x00000004
+#define BCM2835_SPI_CS_CS_10		0x00000002
+#define BCM2835_SPI_CS_CS_01		0x00000001
+
+#define BCM2835_SPI_TIMEOUT_MS	30000
+#define BCM2835_SPI_MODE_BITS	(SPI_CPOL | SPI_CPHA | SPI_CS_HIGH | SPI_NO_CS)
+
+#define DRV_NAME	"spi-bcm2835"
+
+struct bcm2835_spi {
+	void __iomem *regs;
+	struct clk *clk;
+	int irq;
+	struct completion done;
+	const u8 *tx_buf;
+	u8 *rx_buf;
+	int len;
+};
+
+static inline u32 bcm2835_rd(struct bcm2835_spi *bs, unsigned reg)
+{
+	return readl(bs->regs + reg);
+}
+
+static inline void bcm2835_wr(struct bcm2835_spi *bs, unsigned reg, u32 val)
+{
+	writel(val, bs->regs + reg);
+}
+
+static inline void bcm2835_rd_fifo(struct bcm2835_spi *bs, int len)
+{
+	u8 byte;
+
+	while (len--) {
+		byte = bcm2835_rd(bs, BCM2835_SPI_FIFO);
+		if (bs->rx_buf)
+			*bs->rx_buf++ = byte;
+	}
+}
+
+static inline void bcm2835_wr_fifo(struct bcm2835_spi *bs, int len)
+{
+	u8 byte;
+
+	if (len > bs->len)
+		len = bs->len;
+
+	while (len--) {
+		byte = bs->tx_buf ? *bs->tx_buf++ : 0;
+		bcm2835_wr(bs, BCM2835_SPI_FIFO, byte);
+		bs->len--;
+	}
+}
+
+static irqreturn_t bcm2835_spi_interrupt(int irq, void *dev_id)
+{
+	struct spi_master *master = dev_id;
+	struct bcm2835_spi *bs = spi_master_get_devdata(master);
+	u32 cs = bcm2835_rd(bs, BCM2835_SPI_CS);
+
+	/*
+	 * RXR - RX needs Reading. This means 12 (or more) bytes have been
+	 * transmitted and hence 12 (or more) bytes have been received.
+	 *
+	 * The FIFO is 16-bytes deep. We check for this interrupt to keep the
+	 * FIFO full; we have a 4-byte-time buffer for IRQ latency. We check
+	 * this before DONE (TX empty) just in case we delayed processing this
+	 * interrupt for some reason.
+	 *
+	 * We only check for this case if we have more bytes to TX; at the end
+	 * of the transfer, we ignore this pipelining optimization, and let
+	 * bcm2835_spi_finish_transfer() drain the RX FIFO.
+	 */
+	if (bs->len && (cs & BCM2835_SPI_CS_RXR)) {
+		/* Read 12 bytes of data */
+		bcm2835_rd_fifo(bs, 12);
+
+		/* Write up to 12 bytes */
+		bcm2835_wr_fifo(bs, 12);
+
+		/*
+		 * We must have written something to the TX FIFO due to the
+		 * bs->len check above, so cannot be DONE. Hence, return
+		 * early. Note that DONE could also be set if we serviced an
+		 * RXR interrupt really late.
+		 */
+		return IRQ_HANDLED;
+	}
+
+	/*
+	 * DONE - TX empty. This occurs when we first enable the transfer
+	 * since we do not pre-fill the TX FIFO. At any other time, given that
+	 * we refill the TX FIFO above based on RXR, and hence ignore DONE if
+	 * RXR is set, DONE really does mean end-of-transfer.
+	 */
+	if (cs & BCM2835_SPI_CS_DONE) {
+		if (bs->len) { /* First interrupt in a transfer */
+			bcm2835_wr_fifo(bs, 16);
+		} else { /* Transfer complete */
+			/* Disable SPI interrupts */
+			cs &= ~(BCM2835_SPI_CS_INTR | BCM2835_SPI_CS_INTD);
+			bcm2835_wr(bs, BCM2835_SPI_CS, cs);
+
+			/*
+			 * Wake up bcm2835_spi_transfer_one(), which will call
+			 * bcm2835_spi_finish_transfer(), to drain the RX FIFO.
+			 */
+			complete(&bs->done);
+		}
+
+		return IRQ_HANDLED;
+	}
+
+	return IRQ_NONE;
+}
+
+static int bcm2835_spi_start_transfer(struct spi_device *spi,
+		struct spi_transfer *tfr)
+{
+	struct bcm2835_spi *bs = spi_master_get_devdata(spi->master);
+	unsigned long spi_hz, clk_hz, cdiv;
+	u32 cs = BCM2835_SPI_CS_INTR | BCM2835_SPI_CS_INTD | BCM2835_SPI_CS_TA;
+
+	spi_hz = tfr->speed_hz;
+	clk_hz = clk_get_rate(bs->clk);
+
+	if (spi_hz >= clk_hz / 2) {
+		cdiv = 2; /* clk_hz/2 is the fastest we can go */
+	} else if (spi_hz) {
+		/* CDIV must be a power of two */
+		cdiv = roundup_pow_of_two(DIV_ROUND_UP(clk_hz, spi_hz));
+
+		if (cdiv >= 65536)
+			cdiv = 0; /* 0 is the slowest we can go */
+	} else
+		cdiv = 0; /* 0 is the slowest we can go */
+
+	if (spi->mode & SPI_CPOL)
+		cs |= BCM2835_SPI_CS_CPOL;
+	if (spi->mode & SPI_CPHA)
+		cs |= BCM2835_SPI_CS_CPHA;
+
+	if (!(spi->mode & SPI_NO_CS)) {
+		if (spi->mode & SPI_CS_HIGH) {
+			cs |= BCM2835_SPI_CS_CSPOL;
+			cs |= BCM2835_SPI_CS_CSPOL0 << spi->chip_select;
+		}
+
+		cs |= spi->chip_select;
+	}
+
+	INIT_COMPLETION(bs->done);
+	bs->tx_buf = tfr->tx_buf;
+	bs->rx_buf = tfr->rx_buf;
+	bs->len = tfr->len;
+
+	bcm2835_wr(bs, BCM2835_SPI_CLK, cdiv);
+	/*
+	 * Enable the HW block. This will immediately trigger a DONE (TX
+	 * empty) interrupt, upon which we will fill the TX FIFO with the
+	 * first TX bytes. Pre-filling the TX FIFO here to avoid the
+	 * interrupt doesn't work:-(
+	 */
+	bcm2835_wr(bs, BCM2835_SPI_CS, cs);
+
+	return 0;
+}
+
+static int bcm2835_spi_finish_transfer(struct spi_device *spi,
+		struct spi_transfer *tfr, bool cs_change)
+{
+	struct bcm2835_spi *bs = spi_master_get_devdata(spi->master);
+	u32 cs = bcm2835_rd(bs, BCM2835_SPI_CS);
+
+	/* Drain RX FIFO */
+	while (cs & BCM2835_SPI_CS_RXD) {
+		bcm2835_rd_fifo(bs, 1);
+		cs = bcm2835_rd(bs, BCM2835_SPI_CS);
+	}
+
+	if (tfr->delay_usecs)
+		udelay(tfr->delay_usecs);
+
+	if (cs_change)
+		/* Clear TA flag */
+		bcm2835_wr(bs, BCM2835_SPI_CS, cs & ~BCM2835_SPI_CS_TA);
+
+	return 0;
+}
+
+static int bcm2835_spi_transfer_one(struct spi_master *master,
+		struct spi_message *mesg)
+{
+	struct bcm2835_spi *bs = spi_master_get_devdata(master);
+	struct spi_transfer *tfr;
+	struct spi_device *spi = mesg->spi;
+	int err = 0;
+	unsigned int timeout;
+	bool cs_change;
+
+	list_for_each_entry(tfr, &mesg->transfers, transfer_list) {
+		err = bcm2835_spi_start_transfer(spi, tfr);
+		if (err)
+			goto out;
+
+		timeout = wait_for_completion_timeout(&bs->done,
+				msecs_to_jiffies(BCM2835_SPI_TIMEOUT_MS));
+		if (!timeout) {
+			err = -ETIMEDOUT;
+			goto out;
+		}
+
+		cs_change = tfr->cs_change ||
+			list_is_last(&tfr->transfer_list, &mesg->transfers);
+
+		err = bcm2835_spi_finish_transfer(spi, tfr, cs_change);
+		if (err)
+			goto out;
+
+		mesg->actual_length += (tfr->len - bs->len);
+	}
+
+out:
+	/* Clear FIFOs, and disable the HW block */
+	bcm2835_wr(bs, BCM2835_SPI_CS,
+		   BCM2835_SPI_CS_CLEAR_RX | BCM2835_SPI_CS_CLEAR_TX);
+	mesg->status = err;
+	spi_finalize_current_message(master);
+
+	return 0;
+}
+
+static int bcm2835_spi_probe(struct platform_device *pdev)
+{
+	struct spi_master *master;
+	struct bcm2835_spi *bs;
+	struct resource *res;
+	int err;
+
+	master = spi_alloc_master(&pdev->dev, sizeof(*bs));
+	if (!master) {
+		dev_err(&pdev->dev, "spi_alloc_master() failed\n");
+		return -ENOMEM;
+	}
+
+	platform_set_drvdata(pdev, master);
+
+	master->mode_bits = BCM2835_SPI_MODE_BITS;
+	master->bits_per_word_mask = BIT(8 - 1);
+	master->bus_num = -1;
+	master->num_chipselect = 3;
+	master->transfer_one_message = bcm2835_spi_transfer_one;
+	master->dev.of_node = pdev->dev.of_node;
+
+	bs = spi_master_get_devdata(master);
+
+	init_completion(&bs->done);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "could not get memory resource\n");
+		err = -ENODEV;
+		goto out_master_put;
+	}
+
+	bs->regs = devm_request_and_ioremap(&pdev->dev, res);
+	if (!bs->regs) {
+		dev_err(&pdev->dev, "could not request/map memory region\n");
+		err = -ENODEV;
+		goto out_master_put;
+	}
+
+	bs->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(bs->clk)) {
+		err = PTR_ERR(bs->clk);
+		dev_err(&pdev->dev, "could not get clk: %d\n", err);
+		goto out_master_put;
+	}
+
+	bs->irq = irq_of_parse_and_map(pdev->dev.of_node, 0);
+	if (bs->irq <= 0) {
+		dev_err(&pdev->dev, "could not get IRQ: %d\n", bs->irq);
+		err = bs->irq ? bs->irq : -ENODEV;
+		goto out_master_put;
+	}
+
+	clk_prepare_enable(bs->clk);
+
+	err = request_irq(bs->irq, bcm2835_spi_interrupt, 0,
+			dev_name(&pdev->dev), master);
+	if (err) {
+		dev_err(&pdev->dev, "could not request IRQ: %d\n", err);
+		goto out_clk_disable;
+	}
+
+	/* initialise the hardware */
+	bcm2835_wr(bs, BCM2835_SPI_CS,
+		   BCM2835_SPI_CS_CLEAR_RX | BCM2835_SPI_CS_CLEAR_TX);
+
+	err = spi_register_master(master);
+	if (err) {
+		dev_err(&pdev->dev, "could not register SPI master: %d\n", err);
+		goto out_free_irq;
+	}
+
+	return 0;
+
+out_free_irq:
+	free_irq(bs->irq, master);
+out_clk_disable:
+	clk_disable_unprepare(bs->clk);
+out_master_put:
+	spi_master_put(master);
+	return err;
+}
+
+static int bcm2835_spi_remove(struct platform_device *pdev)
+{
+	struct spi_master *master = platform_get_drvdata(pdev);
+	struct bcm2835_spi *bs = spi_master_get_devdata(master);
+
+	free_irq(bs->irq, master);
+	spi_unregister_master(master);
+
+	/* Clear FIFOs, and disable the HW block */
+	bcm2835_wr(bs, BCM2835_SPI_CS,
+		   BCM2835_SPI_CS_CLEAR_RX | BCM2835_SPI_CS_CLEAR_TX);
+
+	clk_disable_unprepare(bs->clk);
+	spi_master_put(master);
+
+	return 0;
+}
+
+static const struct of_device_id bcm2835_spi_match[] = {
+	{ .compatible = "brcm,bcm2835-spi", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, bcm2835_spi_match);
+
+static struct platform_driver bcm2835_spi_driver = {
+	.driver		= {
+		.name		= DRV_NAME,
+		.owner		= THIS_MODULE,
+		.of_match_table	= bcm2835_spi_match,
+	},
+	.probe		= bcm2835_spi_probe,
+	.remove		= bcm2835_spi_remove,
+};
+module_platform_driver(bcm2835_spi_driver);
+
+MODULE_DESCRIPTION("SPI controller driver for Broadcom BCM2835");
+MODULE_AUTHOR("Chris Boot <bootc@bootc.net>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/spi/spi-bcm63xx.c b/drivers/spi/spi-bcm63xx.c
index 9578af782a7..a4ec5f4ec81 100644
--- a/drivers/spi/spi-bcm63xx.c
+++ b/drivers/spi/spi-bcm63xx.c
@@ -46,7 +46,6 @@ struct bcm63xx_spi {
 	int			irq;
 
 	/* Platform data */
-	u32			speed_hz;
 	unsigned		fifo_size;
 	unsigned int		msg_type_shift;
 	unsigned int		msg_ctl_width;
@@ -93,40 +92,16 @@ static const unsigned bcm63xx_spi_freq_table[SPI_CLK_MASK][2] = {
 	{   391000, SPI_CLK_0_391MHZ }
 };
 
-static int bcm63xx_spi_check_transfer(struct spi_device *spi,
-					struct spi_transfer *t)
-{
-	u8 bits_per_word;
-
-	bits_per_word = (t) ? t->bits_per_word : spi->bits_per_word;
-	if (bits_per_word != 8) {
-		dev_err(&spi->dev, "%s, unsupported bits_per_word=%d\n",
-			__func__, bits_per_word);
-		return -EINVAL;
-	}
-
-	if (spi->chip_select > spi->master->num_chipselect) {
-		dev_err(&spi->dev, "%s, unsupported slave %d\n",
-			__func__, spi->chip_select);
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
 static void bcm63xx_spi_setup_transfer(struct spi_device *spi,
 				      struct spi_transfer *t)
 {
 	struct bcm63xx_spi *bs = spi_master_get_devdata(spi->master);
-	u32 hz;
 	u8 clk_cfg, reg;
 	int i;
 
-	hz = (t) ? t->speed_hz : spi->max_speed_hz;
-
 	/* Find the closest clock configuration */
 	for (i = 0; i < SPI_CLK_MASK; i++) {
-		if (hz >= bcm63xx_spi_freq_table[i][0]) {
+		if (t->speed_hz >= bcm63xx_spi_freq_table[i][0]) {
 			clk_cfg = bcm63xx_spi_freq_table[i][1];
 			break;
 		}
@@ -143,7 +118,7 @@ static void bcm63xx_spi_setup_transfer(struct spi_device *spi,
 
 	bcm_spi_writeb(bs, reg, SPI_CLK_CFG);
 	dev_dbg(&spi->dev, "Setting clock register to %02x (hz %d)\n",
-		clk_cfg, hz);
+		clk_cfg, t->speed_hz);
 }
 
 /* the spi->mode bits understood by this driver: */
@@ -151,23 +126,12 @@ static void bcm63xx_spi_setup_transfer(struct spi_device *spi,
 
 static int bcm63xx_spi_setup(struct spi_device *spi)
 {
-	struct bcm63xx_spi *bs;
-	int ret;
-
-	bs = spi_master_get_devdata(spi->master);
-
-	if (!spi->bits_per_word)
-		spi->bits_per_word = 8;
-
-	if (spi->mode & ~MODEBITS) {
-		dev_err(&spi->dev, "%s, unsupported mode bits %x\n",
-			__func__, spi->mode & ~MODEBITS);
+	if (spi->bits_per_word != 8) {
+		dev_err(&spi->dev, "%s, unsupported bits_per_word=%d\n",
+			__func__, spi->bits_per_word);
 		return -EINVAL;
 	}
 
-	dev_dbg(&spi->dev, "%s, mode %d, %u bits/w, %u nsec/bit\n",
-		__func__, spi->mode & MODEBITS, spi->bits_per_word, 0);
-
 	return 0;
 }
 
@@ -313,9 +277,12 @@ static int bcm63xx_spi_transfer_one(struct spi_master *master,
 	 * full-duplex transfers.
 	 */
 	list_for_each_entry(t, &m->transfers, transfer_list) {
-		status = bcm63xx_spi_check_transfer(spi, t);
-		if (status < 0)
+		if (t->bits_per_word != 8) {
+			dev_err(&spi->dev, "%s, unsupported bits_per_word=%d\n",
+				__func__, t->bits_per_word);
+			status = -EINVAL;
 			goto exit;
+		}
 
 		if (!first)
 			first = t;
@@ -444,18 +411,9 @@ static int bcm63xx_spi_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, master);
 	bs->pdev = pdev;
 
-	if (!devm_request_mem_region(&pdev->dev, r->start,
-					resource_size(r), PFX)) {
-		dev_err(dev, "iomem request failed\n");
-		ret = -ENXIO;
-		goto out_err;
-	}
-
-	bs->regs = devm_ioremap_nocache(&pdev->dev, r->start,
-							resource_size(r));
-	if (!bs->regs) {
-		dev_err(dev, "unable to ioremap regs\n");
-		ret = -ENOMEM;
+	bs->regs = devm_ioremap_resource(&pdev->dev, r);
+	if (IS_ERR(bs->regs)) {
+		ret = PTR_ERR(bs->regs);
 		goto out_err;
 	}
 
@@ -477,7 +435,6 @@ static int bcm63xx_spi_probe(struct platform_device *pdev)
 	master->unprepare_transfer_hardware = bcm63xx_spi_unprepare_transfer;
 	master->transfer_one_message = bcm63xx_spi_transfer_one;
 	master->mode_bits = MODEBITS;
-	bs->speed_hz = pdata->speed_hz;
 	bs->msg_type_shift = pdata->msg_type_shift;
 	bs->msg_ctl_width = pdata->msg_ctl_width;
 	bs->tx_io = (u8 *)(bs->regs + bcm63xx_spireg(SPI_MSG_DATA));
@@ -490,11 +447,11 @@ static int bcm63xx_spi_probe(struct platform_device *pdev)
 	default:
 		dev_err(dev, "unsupported MSG_CTL width: %d\n",
 			 bs->msg_ctl_width);
-		goto out_clk_disable;
+		goto out_err;
 	}
 
 	/* Initialize hardware */
-	clk_enable(bs->clk);
+	clk_prepare_enable(bs->clk);
 	bcm_spi_writeb(bs, SPI_INTR_CLEAR_ALL, SPI_INT_STATUS);
 
 	/* register and we are done */
@@ -510,7 +467,7 @@ static int bcm63xx_spi_probe(struct platform_device *pdev)
 	return 0;
 
 out_clk_disable:
-	clk_disable(clk);
+	clk_disable_unprepare(clk);
 out_err:
 	platform_set_drvdata(pdev, NULL);
 	spi_master_put(master);
@@ -531,7 +488,7 @@ static int bcm63xx_spi_remove(struct platform_device *pdev)
 	bcm_spi_writeb(bs, 0, SPI_INT_MASK);
 
 	/* HW shutdown */
-	clk_disable(bs->clk);
+	clk_disable_unprepare(bs->clk);
 	clk_put(bs->clk);
 
 	platform_set_drvdata(pdev, 0);
@@ -550,7 +507,7 @@ static int bcm63xx_spi_suspend(struct device *dev)
 
 	spi_master_suspend(master);
 
-	clk_disable(bs->clk);
+	clk_disable_unprepare(bs->clk);
 
 	return 0;
 }
@@ -561,7 +518,7 @@ static int bcm63xx_spi_resume(struct device *dev)
 			platform_get_drvdata(to_platform_device(dev));
 	struct bcm63xx_spi *bs = spi_master_get_devdata(master);
 
-	clk_enable(bs->clk);
+	clk_prepare_enable(bs->clk);
 
 	spi_master_resume(master);
 
diff --git a/drivers/spi/spi-fsl-cpm.c b/drivers/spi/spi-fsl-cpm.c
new file mode 100644
index 00000000000..07971e3fe58
--- /dev/null
+++ b/drivers/spi/spi-fsl-cpm.c
@@ -0,0 +1,387 @@
+/*
+ * Freescale SPI controller driver cpm functions.
+ *
+ * Maintainer: Kumar Gala
+ *
+ * Copyright (C) 2006 Polycom, Inc.
+ * Copyright 2010 Freescale Semiconductor, Inc.
+ *
+ * CPM SPI and QE buffer descriptors mode support:
+ * Copyright (c) 2009  MontaVista Software, Inc.
+ * Author: Anton Vorontsov <avorontsov@ru.mvista.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/types.h>
+#include <linux/kernel.h>
+#include <linux/spi/spi.h>
+#include <linux/fsl_devices.h>
+#include <linux/dma-mapping.h>
+#include <asm/cpm.h>
+#include <asm/qe.h>
+
+#include "spi-fsl-lib.h"
+#include "spi-fsl-cpm.h"
+#include "spi-fsl-spi.h"
+
+/* CPM1 and CPM2 are mutually exclusive. */
+#ifdef CONFIG_CPM1
+#include <asm/cpm1.h>
+#define CPM_SPI_CMD mk_cr_cmd(CPM_CR_CH_SPI, 0)
+#else
+#include <asm/cpm2.h>
+#define CPM_SPI_CMD mk_cr_cmd(CPM_CR_SPI_PAGE, CPM_CR_SPI_SBLOCK, 0, 0)
+#endif
+
+#define	SPIE_TXB	0x00000200	/* Last char is written to tx fifo */
+#define	SPIE_RXB	0x00000100	/* Last char is written to rx buf */
+
+/* SPCOM register values */
+#define	SPCOM_STR	(1 << 23)	/* Start transmit */
+
+#define	SPI_PRAM_SIZE	0x100
+#define	SPI_MRBLR	((unsigned int)PAGE_SIZE)
+
+static void *fsl_dummy_rx;
+static DEFINE_MUTEX(fsl_dummy_rx_lock);
+static int fsl_dummy_rx_refcnt;
+
+void fsl_spi_cpm_reinit_txrx(struct mpc8xxx_spi *mspi)
+{
+	if (mspi->flags & SPI_QE) {
+		qe_issue_cmd(QE_INIT_TX_RX, mspi->subblock,
+			     QE_CR_PROTOCOL_UNSPECIFIED, 0);
+	} else {
+		cpm_command(CPM_SPI_CMD, CPM_CR_INIT_TRX);
+		if (mspi->flags & SPI_CPM1) {
+			out_be16(&mspi->pram->rbptr,
+				 in_be16(&mspi->pram->rbase));
+			out_be16(&mspi->pram->tbptr,
+				 in_be16(&mspi->pram->tbase));
+		}
+	}
+}
+
+static void fsl_spi_cpm_bufs_start(struct mpc8xxx_spi *mspi)
+{
+	struct cpm_buf_desc __iomem *tx_bd = mspi->tx_bd;
+	struct cpm_buf_desc __iomem *rx_bd = mspi->rx_bd;
+	unsigned int xfer_len = min(mspi->count, SPI_MRBLR);
+	unsigned int xfer_ofs;
+	struct fsl_spi_reg *reg_base = mspi->reg_base;
+
+	xfer_ofs = mspi->xfer_in_progress->len - mspi->count;
+
+	if (mspi->rx_dma == mspi->dma_dummy_rx)
+		out_be32(&rx_bd->cbd_bufaddr, mspi->rx_dma);
+	else
+		out_be32(&rx_bd->cbd_bufaddr, mspi->rx_dma + xfer_ofs);
+	out_be16(&rx_bd->cbd_datlen, 0);
+	out_be16(&rx_bd->cbd_sc, BD_SC_EMPTY | BD_SC_INTRPT | BD_SC_WRAP);
+
+	if (mspi->tx_dma == mspi->dma_dummy_tx)
+		out_be32(&tx_bd->cbd_bufaddr, mspi->tx_dma);
+	else
+		out_be32(&tx_bd->cbd_bufaddr, mspi->tx_dma + xfer_ofs);
+	out_be16(&tx_bd->cbd_datlen, xfer_len);
+	out_be16(&tx_bd->cbd_sc, BD_SC_READY | BD_SC_INTRPT | BD_SC_WRAP |
+				 BD_SC_LAST);
+
+	/* start transfer */
+	mpc8xxx_spi_write_reg(&reg_base->command, SPCOM_STR);
+}
+
+int fsl_spi_cpm_bufs(struct mpc8xxx_spi *mspi,
+		     struct spi_transfer *t, bool is_dma_mapped)
+{
+	struct device *dev = mspi->dev;
+	struct fsl_spi_reg *reg_base = mspi->reg_base;
+
+	if (is_dma_mapped) {
+		mspi->map_tx_dma = 0;
+		mspi->map_rx_dma = 0;
+	} else {
+		mspi->map_tx_dma = 1;
+		mspi->map_rx_dma = 1;
+	}
+
+	if (!t->tx_buf) {
+		mspi->tx_dma = mspi->dma_dummy_tx;
+		mspi->map_tx_dma = 0;
+	}
+
+	if (!t->rx_buf) {
+		mspi->rx_dma = mspi->dma_dummy_rx;
+		mspi->map_rx_dma = 0;
+	}
+
+	if (mspi->map_tx_dma) {
+		void *nonconst_tx = (void *)mspi->tx; /* shut up gcc */
+
+		mspi->tx_dma = dma_map_single(dev, nonconst_tx, t->len,
+					      DMA_TO_DEVICE);
+		if (dma_mapping_error(dev, mspi->tx_dma)) {
+			dev_err(dev, "unable to map tx dma\n");
+			return -ENOMEM;
+		}
+	} else if (t->tx_buf) {
+		mspi->tx_dma = t->tx_dma;
+	}
+
+	if (mspi->map_rx_dma) {
+		mspi->rx_dma = dma_map_single(dev, mspi->rx, t->len,
+					      DMA_FROM_DEVICE);
+		if (dma_mapping_error(dev, mspi->rx_dma)) {
+			dev_err(dev, "unable to map rx dma\n");
+			goto err_rx_dma;
+		}
+	} else if (t->rx_buf) {
+		mspi->rx_dma = t->rx_dma;
+	}
+
+	/* enable rx ints */
+	mpc8xxx_spi_write_reg(&reg_base->mask, SPIE_RXB);
+
+	mspi->xfer_in_progress = t;
+	mspi->count = t->len;
+
+	/* start CPM transfers */
+	fsl_spi_cpm_bufs_start(mspi);
+
+	return 0;
+
+err_rx_dma:
+	if (mspi->map_tx_dma)
+		dma_unmap_single(dev, mspi->tx_dma, t->len, DMA_TO_DEVICE);
+	return -ENOMEM;
+}
+
+void fsl_spi_cpm_bufs_complete(struct mpc8xxx_spi *mspi)
+{
+	struct device *dev = mspi->dev;
+	struct spi_transfer *t = mspi->xfer_in_progress;
+
+	if (mspi->map_tx_dma)
+		dma_unmap_single(dev, mspi->tx_dma, t->len, DMA_TO_DEVICE);
+	if (mspi->map_rx_dma)
+		dma_unmap_single(dev, mspi->rx_dma, t->len, DMA_FROM_DEVICE);
+	mspi->xfer_in_progress = NULL;
+}
+
+void fsl_spi_cpm_irq(struct mpc8xxx_spi *mspi, u32 events)
+{
+	u16 len;
+	struct fsl_spi_reg *reg_base = mspi->reg_base;
+
+	dev_dbg(mspi->dev, "%s: bd datlen %d, count %d\n", __func__,
+		in_be16(&mspi->rx_bd->cbd_datlen), mspi->count);
+
+	len = in_be16(&mspi->rx_bd->cbd_datlen);
+	if (len > mspi->count) {
+		WARN_ON(1);
+		len = mspi->count;
+	}
+
+	/* Clear the events */
+	mpc8xxx_spi_write_reg(&reg_base->event, events);
+
+	mspi->count -= len;
+	if (mspi->count)
+		fsl_spi_cpm_bufs_start(mspi);
+	else
+		complete(&mspi->done);
+}
+
+static void *fsl_spi_alloc_dummy_rx(void)
+{
+	mutex_lock(&fsl_dummy_rx_lock);
+
+	if (!fsl_dummy_rx)
+		fsl_dummy_rx = kmalloc(SPI_MRBLR, GFP_KERNEL);
+	if (fsl_dummy_rx)
+		fsl_dummy_rx_refcnt++;
+
+	mutex_unlock(&fsl_dummy_rx_lock);
+
+	return fsl_dummy_rx;
+}
+
+static void fsl_spi_free_dummy_rx(void)
+{
+	mutex_lock(&fsl_dummy_rx_lock);
+
+	switch (fsl_dummy_rx_refcnt) {
+	case 0:
+		WARN_ON(1);
+		break;
+	case 1:
+		kfree(fsl_dummy_rx);
+		fsl_dummy_rx = NULL;
+		/* fall through */
+	default:
+		fsl_dummy_rx_refcnt--;
+		break;
+	}
+
+	mutex_unlock(&fsl_dummy_rx_lock);
+}
+
+static unsigned long fsl_spi_cpm_get_pram(struct mpc8xxx_spi *mspi)
+{
+	struct device *dev = mspi->dev;
+	struct device_node *np = dev->of_node;
+	const u32 *iprop;
+	int size;
+	void __iomem *spi_base;
+	unsigned long pram_ofs = -ENOMEM;
+
+	/* Can't use of_address_to_resource(), QE muram isn't at 0. */
+	iprop = of_get_property(np, "reg", &size);
+
+	/* QE with a fixed pram location? */
+	if (mspi->flags & SPI_QE && iprop && size == sizeof(*iprop) * 4)
+		return cpm_muram_alloc_fixed(iprop[2], SPI_PRAM_SIZE);
+
+	/* QE but with a dynamic pram location? */
+	if (mspi->flags & SPI_QE) {
+		pram_ofs = cpm_muram_alloc(SPI_PRAM_SIZE, 64);
+		qe_issue_cmd(QE_ASSIGN_PAGE_TO_DEVICE, mspi->subblock,
+			     QE_CR_PROTOCOL_UNSPECIFIED, pram_ofs);
+		return pram_ofs;
+	}
+
+	spi_base = of_iomap(np, 1);
+	if (spi_base == NULL)
+		return -EINVAL;
+
+	if (mspi->flags & SPI_CPM2) {
+		pram_ofs = cpm_muram_alloc(SPI_PRAM_SIZE, 64);
+		out_be16(spi_base, pram_ofs);
+	} else {
+		struct spi_pram __iomem *pram = spi_base;
+		u16 rpbase = in_be16(&pram->rpbase);
+
+		/* Microcode relocation patch applied? */
+		if (rpbase) {
+			pram_ofs = rpbase;
+		} else {
+			pram_ofs = cpm_muram_alloc(SPI_PRAM_SIZE, 64);
+			out_be16(spi_base, pram_ofs);
+		}
+	}
+
+	iounmap(spi_base);
+	return pram_ofs;
+}
+
+int fsl_spi_cpm_init(struct mpc8xxx_spi *mspi)
+{
+	struct device *dev = mspi->dev;
+	struct device_node *np = dev->of_node;
+	const u32 *iprop;
+	int size;
+	unsigned long pram_ofs;
+	unsigned long bds_ofs;
+
+	if (!(mspi->flags & SPI_CPM_MODE))
+		return 0;
+
+	if (!fsl_spi_alloc_dummy_rx())
+		return -ENOMEM;
+
+	if (mspi->flags & SPI_QE) {
+		iprop = of_get_property(np, "cell-index", &size);
+		if (iprop && size == sizeof(*iprop))
+			mspi->subblock = *iprop;
+
+		switch (mspi->subblock) {
+		default:
+			dev_warn(dev, "cell-index unspecified, assuming SPI1");
+			/* fall through */
+		case 0:
+			mspi->subblock = QE_CR_SUBBLOCK_SPI1;
+			break;
+		case 1:
+			mspi->subblock = QE_CR_SUBBLOCK_SPI2;
+			break;
+		}
+	}
+
+	pram_ofs = fsl_spi_cpm_get_pram(mspi);
+	if (IS_ERR_VALUE(pram_ofs)) {
+		dev_err(dev, "can't allocate spi parameter ram\n");
+		goto err_pram;
+	}
+
+	bds_ofs = cpm_muram_alloc(sizeof(*mspi->tx_bd) +
+				  sizeof(*mspi->rx_bd), 8);
+	if (IS_ERR_VALUE(bds_ofs)) {
+		dev_err(dev, "can't allocate bds\n");
+		goto err_bds;
+	}
+
+	mspi->dma_dummy_tx = dma_map_single(dev, empty_zero_page, PAGE_SIZE,
+					    DMA_TO_DEVICE);
+	if (dma_mapping_error(dev, mspi->dma_dummy_tx)) {
+		dev_err(dev, "unable to map dummy tx buffer\n");
+		goto err_dummy_tx;
+	}
+
+	mspi->dma_dummy_rx = dma_map_single(dev, fsl_dummy_rx, SPI_MRBLR,
+					    DMA_FROM_DEVICE);
+	if (dma_mapping_error(dev, mspi->dma_dummy_rx)) {
+		dev_err(dev, "unable to map dummy rx buffer\n");
+		goto err_dummy_rx;
+	}
+
+	mspi->pram = cpm_muram_addr(pram_ofs);
+
+	mspi->tx_bd = cpm_muram_addr(bds_ofs);
+	mspi->rx_bd = cpm_muram_addr(bds_ofs + sizeof(*mspi->tx_bd));
+
+	/* Initialize parameter ram. */
+	out_be16(&mspi->pram->tbase, cpm_muram_offset(mspi->tx_bd));
+	out_be16(&mspi->pram->rbase, cpm_muram_offset(mspi->rx_bd));
+	out_8(&mspi->pram->tfcr, CPMFCR_EB | CPMFCR_GBL);
+	out_8(&mspi->pram->rfcr, CPMFCR_EB | CPMFCR_GBL);
+	out_be16(&mspi->pram->mrblr, SPI_MRBLR);
+	out_be32(&mspi->pram->rstate, 0);
+	out_be32(&mspi->pram->rdp, 0);
+	out_be16(&mspi->pram->rbptr, 0);
+	out_be16(&mspi->pram->rbc, 0);
+	out_be32(&mspi->pram->rxtmp, 0);
+	out_be32(&mspi->pram->tstate, 0);
+	out_be32(&mspi->pram->tdp, 0);
+	out_be16(&mspi->pram->tbptr, 0);
+	out_be16(&mspi->pram->tbc, 0);
+	out_be32(&mspi->pram->txtmp, 0);
+
+	return 0;
+
+err_dummy_rx:
+	dma_unmap_single(dev, mspi->dma_dummy_tx, PAGE_SIZE, DMA_TO_DEVICE);
+err_dummy_tx:
+	cpm_muram_free(bds_ofs);
+err_bds:
+	cpm_muram_free(pram_ofs);
+err_pram:
+	fsl_spi_free_dummy_rx();
+	return -ENOMEM;
+}
+
+void fsl_spi_cpm_free(struct mpc8xxx_spi *mspi)
+{
+	struct device *dev = mspi->dev;
+
+	if (!(mspi->flags & SPI_CPM_MODE))
+		return;
+
+	dma_unmap_single(dev, mspi->dma_dummy_rx, SPI_MRBLR, DMA_FROM_DEVICE);
+	dma_unmap_single(dev, mspi->dma_dummy_tx, PAGE_SIZE, DMA_TO_DEVICE);
+	cpm_muram_free(cpm_muram_offset(mspi->tx_bd));
+	cpm_muram_free(cpm_muram_offset(mspi->pram));
+	fsl_spi_free_dummy_rx();
+}
diff --git a/drivers/spi/spi-fsl-cpm.h b/drivers/spi/spi-fsl-cpm.h
new file mode 100644
index 00000000000..c7111580548
--- /dev/null
+++ b/drivers/spi/spi-fsl-cpm.h
@@ -0,0 +1,43 @@
+/*
+ * Freescale SPI controller driver cpm functions.
+ *
+ * Maintainer: Kumar Gala
+ *
+ * Copyright (C) 2006 Polycom, Inc.
+ * Copyright 2010 Freescale Semiconductor, Inc.
+ *
+ * CPM SPI and QE buffer descriptors mode support:
+ * Copyright (c) 2009  MontaVista Software, Inc.
+ * Author: Anton Vorontsov <avorontsov@ru.mvista.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#ifndef __SPI_FSL_CPM_H__
+#define __SPI_FSL_CPM_H__
+
+#include "spi-fsl-lib.h"
+
+#ifdef CONFIG_FSL_SOC
+extern void fsl_spi_cpm_reinit_txrx(struct mpc8xxx_spi *mspi);
+extern int fsl_spi_cpm_bufs(struct mpc8xxx_spi *mspi,
+			    struct spi_transfer *t, bool is_dma_mapped);
+extern void fsl_spi_cpm_bufs_complete(struct mpc8xxx_spi *mspi);
+extern void fsl_spi_cpm_irq(struct mpc8xxx_spi *mspi, u32 events);
+extern int fsl_spi_cpm_init(struct mpc8xxx_spi *mspi);
+extern void fsl_spi_cpm_free(struct mpc8xxx_spi *mspi);
+#else
+static inline void fsl_spi_cpm_reinit_txrx(struct mpc8xxx_spi *mspi) { }
+static inline int fsl_spi_cpm_bufs(struct mpc8xxx_spi *mspi,
+				   struct spi_transfer *t,
+				   bool is_dma_mapped) { return 0; }
+static inline void fsl_spi_cpm_bufs_complete(struct mpc8xxx_spi *mspi) { }
+static inline void fsl_spi_cpm_irq(struct mpc8xxx_spi *mspi, u32 events) { }
+static inline int fsl_spi_cpm_init(struct mpc8xxx_spi *mspi) { return 0; }
+static inline void fsl_spi_cpm_free(struct mpc8xxx_spi *mspi) { }
+#endif
+
+#endif /* __SPI_FSL_CPM_H__ */
diff --git a/drivers/spi/spi-fsl-lib.c b/drivers/spi/spi-fsl-lib.c
index 8ade675a04f..a91db0e57b2 100644
--- a/drivers/spi/spi-fsl-lib.c
+++ b/drivers/spi/spi-fsl-lib.c
@@ -23,7 +23,9 @@
 #include <linux/mm.h>
 #include <linux/of_platform.h>
 #include <linux/spi/spi.h>
+#ifdef CONFIG_FSL_SOC
 #include <sysdev/fsl_soc.h>
+#endif
 
 #include "spi-fsl-lib.h"
 
@@ -208,6 +210,7 @@ int of_mpc8xxx_spi_probe(struct platform_device *ofdev)
 	/* Allocate bus num dynamically. */
 	pdata->bus_num = -1;
 
+#ifdef CONFIG_FSL_SOC
 	/* SPI controller is either clocked from QE or SoC clock. */
 	pdata->sysclk = get_brgfreq();
 	if (pdata->sysclk == -1) {
@@ -217,6 +220,11 @@ int of_mpc8xxx_spi_probe(struct platform_device *ofdev)
 			goto err;
 		}
 	}
+#else
+	ret = of_property_read_u32(np, "clock-frequency", &pdata->sysclk);
+	if (ret)
+		goto err;
+#endif
 
 	prop = of_get_property(np, "mode", NULL);
 	if (prop && !strcmp(prop, "cpu-qe"))
diff --git a/drivers/spi/spi-fsl-lib.h b/drivers/spi/spi-fsl-lib.h
index cbe881b9ea7..52db6936778 100644
--- a/drivers/spi/spi-fsl-lib.h
+++ b/drivers/spi/spi-fsl-lib.h
@@ -34,8 +34,10 @@ struct mpc8xxx_spi {
 
 	int subblock;
 	struct spi_pram __iomem *pram;
+#ifdef CONFIG_FSL_SOC
 	struct cpm_buf_desc __iomem *tx_bd;
 	struct cpm_buf_desc __iomem *rx_bd;
+#endif
 
 	struct spi_transfer *xfer_in_progress;
 
@@ -67,6 +69,15 @@ struct mpc8xxx_spi {
 
 	unsigned int flags;
 
+#ifdef CONFIG_SPI_FSL_SPI
+	int type;
+	int native_chipselects;
+	u8 max_bits_per_word;
+
+	void (*set_shifts)(u32 *rx_shift, u32 *tx_shift,
+			   int bits_per_word, int msb_first);
+#endif
+
 	struct workqueue_struct *workqueue;
 	struct work_struct work;
 
@@ -87,12 +98,12 @@ struct spi_mpc8xxx_cs {
 
 static inline void mpc8xxx_spi_write_reg(__be32 __iomem *reg, u32 val)
 {
-	out_be32(reg, val);
+	iowrite32be(val, reg);
 }
 
 static inline u32 mpc8xxx_spi_read_reg(__be32 __iomem *reg)
 {
-	return in_be32(reg);
+	return ioread32be(reg);
 }
 
 struct mpc8xxx_spi_probe_info {
diff --git a/drivers/spi/spi-fsl-spi.c b/drivers/spi/spi-fsl-spi.c
index 086a9eef2e0..14e202ee703 100644
--- a/drivers/spi/spi-fsl-spi.c
+++ b/drivers/spi/spi-fsl-spi.c
@@ -10,6 +10,10 @@
  * Copyright (c) 2009  MontaVista Software, Inc.
  * Author: Anton Vorontsov <avorontsov@ru.mvista.com>
  *
+ * GRLIB support:
+ * Copyright (c) 2012 Aeroflex Gaisler AB.
+ * Author: Andreas Larsson <andreas@gaisler.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
@@ -30,75 +34,54 @@
 #include <linux/mutex.h>
 #include <linux/of.h>
 #include <linux/of_platform.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
 #include <linux/gpio.h>
 #include <linux/of_gpio.h>
 
-#include <sysdev/fsl_soc.h>
-#include <asm/cpm.h>
-#include <asm/qe.h>
-
 #include "spi-fsl-lib.h"
+#include "spi-fsl-cpm.h"
+#include "spi-fsl-spi.h"
 
-/* CPM1 and CPM2 are mutually exclusive. */
-#ifdef CONFIG_CPM1
-#include <asm/cpm1.h>
-#define CPM_SPI_CMD mk_cr_cmd(CPM_CR_CH_SPI, 0)
-#else
-#include <asm/cpm2.h>
-#define CPM_SPI_CMD mk_cr_cmd(CPM_CR_SPI_PAGE, CPM_CR_SPI_SBLOCK, 0, 0)
-#endif
-
-/* SPI Controller registers */
-struct fsl_spi_reg {
-	u8 res1[0x20];
-	__be32 mode;
-	__be32 event;
-	__be32 mask;
-	__be32 command;
-	__be32 transmit;
-	__be32 receive;
-};
-
-/* SPI Controller mode register definitions */
-#define	SPMODE_LOOP		(1 << 30)
-#define	SPMODE_CI_INACTIVEHIGH	(1 << 29)
-#define	SPMODE_CP_BEGIN_EDGECLK	(1 << 28)
-#define	SPMODE_DIV16		(1 << 27)
-#define	SPMODE_REV		(1 << 26)
-#define	SPMODE_MS		(1 << 25)
-#define	SPMODE_ENABLE		(1 << 24)
-#define	SPMODE_LEN(x)		((x) << 20)
-#define	SPMODE_PM(x)		((x) << 16)
-#define	SPMODE_OP		(1 << 14)
-#define	SPMODE_CG(x)		((x) << 7)
+#define TYPE_FSL	0
+#define TYPE_GRLIB	1
 
-/*
- * Default for SPI Mode:
- *	SPI MODE 0 (inactive low, phase middle, MSB, 8-bit length, slow clk
- */
-#define	SPMODE_INIT_VAL (SPMODE_CI_INACTIVEHIGH | SPMODE_DIV16 | SPMODE_REV | \
-			 SPMODE_MS | SPMODE_LEN(7) | SPMODE_PM(0xf))
-
-/* SPIE register values */
-#define	SPIE_NE		0x00000200	/* Not empty */
-#define	SPIE_NF		0x00000100	/* Not full */
+struct fsl_spi_match_data {
+	int type;
+};
 
-/* SPIM register values */
-#define	SPIM_NE		0x00000200	/* Not empty */
-#define	SPIM_NF		0x00000100	/* Not full */
+static struct fsl_spi_match_data of_fsl_spi_fsl_config = {
+	.type = TYPE_FSL,
+};
 
-#define	SPIE_TXB	0x00000200	/* Last char is written to tx fifo */
-#define	SPIE_RXB	0x00000100	/* Last char is written to rx buf */
+static struct fsl_spi_match_data of_fsl_spi_grlib_config = {
+	.type = TYPE_GRLIB,
+};
 
-/* SPCOM register values */
-#define	SPCOM_STR	(1 << 23)	/* Start transmit */
+static struct of_device_id of_fsl_spi_match[] = {
+	{
+		.compatible = "fsl,spi",
+		.data = &of_fsl_spi_fsl_config,
+	},
+	{
+		.compatible = "aeroflexgaisler,spictrl",
+		.data = &of_fsl_spi_grlib_config,
+	},
+	{}
+};
+MODULE_DEVICE_TABLE(of, of_fsl_spi_match);
 
-#define	SPI_PRAM_SIZE	0x100
-#define	SPI_MRBLR	((unsigned int)PAGE_SIZE)
+static int fsl_spi_get_type(struct device *dev)
+{
+	const struct of_device_id *match;
 
-static void *fsl_dummy_rx;
-static DEFINE_MUTEX(fsl_dummy_rx_lock);
-static int fsl_dummy_rx_refcnt;
+	if (dev->of_node) {
+		match = of_match_node(of_fsl_spi_match, dev->of_node);
+		if (match && match->data)
+			return ((struct fsl_spi_match_data *)match->data)->type;
+	}
+	return TYPE_FSL;
+}
 
 static void fsl_spi_change_mode(struct spi_device *spi)
 {
@@ -119,18 +102,7 @@ static void fsl_spi_change_mode(struct spi_device *spi)
 
 	/* When in CPM mode, we need to reinit tx and rx. */
 	if (mspi->flags & SPI_CPM_MODE) {
-		if (mspi->flags & SPI_QE) {
-			qe_issue_cmd(QE_INIT_TX_RX, mspi->subblock,
-				     QE_CR_PROTOCOL_UNSPECIFIED, 0);
-		} else {
-			cpm_command(CPM_SPI_CMD, CPM_CR_INIT_TRX);
-			if (mspi->flags & SPI_CPM1) {
-				out_be16(&mspi->pram->rbptr,
-					 in_be16(&mspi->pram->rbase));
-				out_be16(&mspi->pram->tbptr,
-					 in_be16(&mspi->pram->tbase));
-			}
-		}
+		fsl_spi_cpm_reinit_txrx(mspi);
 	}
 	mpc8xxx_spi_write_reg(mode, cs->hw_mode);
 	local_irq_restore(flags);
@@ -163,6 +135,40 @@ static void fsl_spi_chipselect(struct spi_device *spi, int value)
 	}
 }
 
+static void fsl_spi_qe_cpu_set_shifts(u32 *rx_shift, u32 *tx_shift,
+				      int bits_per_word, int msb_first)
+{
+	*rx_shift = 0;
+	*tx_shift = 0;
+	if (msb_first) {
+		if (bits_per_word <= 8) {
+			*rx_shift = 16;
+			*tx_shift = 24;
+		} else if (bits_per_word <= 16) {
+			*rx_shift = 16;
+			*tx_shift = 16;
+		}
+	} else {
+		if (bits_per_word <= 8)
+			*rx_shift = 8;
+	}
+}
+
+static void fsl_spi_grlib_set_shifts(u32 *rx_shift, u32 *tx_shift,
+				     int bits_per_word, int msb_first)
+{
+	*rx_shift = 0;
+	*tx_shift = 0;
+	if (bits_per_word <= 16) {
+		if (msb_first) {
+			*rx_shift = 16; /* LSB in bit 16 */
+			*tx_shift = 32 - bits_per_word; /* MSB in bit 31 */
+		} else {
+			*rx_shift = 16 - bits_per_word; /* MSB in bit 15 */
+		}
+	}
+}
+
 static int mspi_apply_cpu_mode_quirks(struct spi_mpc8xxx_cs *cs,
 				struct spi_device *spi,
 				struct mpc8xxx_spi *mpc8xxx_spi,
@@ -173,31 +179,20 @@ static int mspi_apply_cpu_mode_quirks(struct spi_mpc8xxx_cs *cs,
 	if (bits_per_word <= 8) {
 		cs->get_rx = mpc8xxx_spi_rx_buf_u8;
 		cs->get_tx = mpc8xxx_spi_tx_buf_u8;
-		if (mpc8xxx_spi->flags & SPI_QE_CPU_MODE) {
-			cs->rx_shift = 16;
-			cs->tx_shift = 24;
-		}
 	} else if (bits_per_word <= 16) {
 		cs->get_rx = mpc8xxx_spi_rx_buf_u16;
 		cs->get_tx = mpc8xxx_spi_tx_buf_u16;
-		if (mpc8xxx_spi->flags & SPI_QE_CPU_MODE) {
-			cs->rx_shift = 16;
-			cs->tx_shift = 16;
-		}
 	} else if (bits_per_word <= 32) {
 		cs->get_rx = mpc8xxx_spi_rx_buf_u32;
 		cs->get_tx = mpc8xxx_spi_tx_buf_u32;
 	} else
 		return -EINVAL;
 
-	if (mpc8xxx_spi->flags & SPI_QE_CPU_MODE &&
-	    spi->mode & SPI_LSB_FIRST) {
-		cs->tx_shift = 0;
-		if (bits_per_word <= 8)
-			cs->rx_shift = 8;
-		else
-			cs->rx_shift = 0;
-	}
+	if (mpc8xxx_spi->set_shifts)
+		mpc8xxx_spi->set_shifts(&cs->rx_shift, &cs->tx_shift,
+					bits_per_word,
+					!(spi->mode & SPI_LSB_FIRST));
+
 	mpc8xxx_spi->rx_shift = cs->rx_shift;
 	mpc8xxx_spi->tx_shift = cs->tx_shift;
 	mpc8xxx_spi->get_rx = cs->get_rx;
@@ -246,7 +241,8 @@ static int fsl_spi_setup_transfer(struct spi_device *spi,
 
 	/* Make sure its a bit width we support [4..16, 32] */
 	if ((bits_per_word < 4)
-	    || ((bits_per_word > 16) && (bits_per_word != 32)))
+	    || ((bits_per_word > 16) && (bits_per_word != 32))
+	    || (bits_per_word > mpc8xxx_spi->max_bits_per_word))
 		return -EINVAL;
 
 	if (!hz)
@@ -295,112 +291,6 @@ static int fsl_spi_setup_transfer(struct spi_device *spi,
 	return 0;
 }
 
-static void fsl_spi_cpm_bufs_start(struct mpc8xxx_spi *mspi)
-{
-	struct cpm_buf_desc __iomem *tx_bd = mspi->tx_bd;
-	struct cpm_buf_desc __iomem *rx_bd = mspi->rx_bd;
-	unsigned int xfer_len = min(mspi->count, SPI_MRBLR);
-	unsigned int xfer_ofs;
-	struct fsl_spi_reg *reg_base = mspi->reg_base;
-
-	xfer_ofs = mspi->xfer_in_progress->len - mspi->count;
-
-	if (mspi->rx_dma == mspi->dma_dummy_rx)
-		out_be32(&rx_bd->cbd_bufaddr, mspi->rx_dma);
-	else
-		out_be32(&rx_bd->cbd_bufaddr, mspi->rx_dma + xfer_ofs);
-	out_be16(&rx_bd->cbd_datlen, 0);
-	out_be16(&rx_bd->cbd_sc, BD_SC_EMPTY | BD_SC_INTRPT | BD_SC_WRAP);
-
-	if (mspi->tx_dma == mspi->dma_dummy_tx)
-		out_be32(&tx_bd->cbd_bufaddr, mspi->tx_dma);
-	else
-		out_be32(&tx_bd->cbd_bufaddr, mspi->tx_dma + xfer_ofs);
-	out_be16(&tx_bd->cbd_datlen, xfer_len);
-	out_be16(&tx_bd->cbd_sc, BD_SC_READY | BD_SC_INTRPT | BD_SC_WRAP |
-				 BD_SC_LAST);
-
-	/* start transfer */
-	mpc8xxx_spi_write_reg(&reg_base->command, SPCOM_STR);
-}
-
-static int fsl_spi_cpm_bufs(struct mpc8xxx_spi *mspi,
-				struct spi_transfer *t, bool is_dma_mapped)
-{
-	struct device *dev = mspi->dev;
-	struct fsl_spi_reg *reg_base = mspi->reg_base;
-
-	if (is_dma_mapped) {
-		mspi->map_tx_dma = 0;
-		mspi->map_rx_dma = 0;
-	} else {
-		mspi->map_tx_dma = 1;
-		mspi->map_rx_dma = 1;
-	}
-
-	if (!t->tx_buf) {
-		mspi->tx_dma = mspi->dma_dummy_tx;
-		mspi->map_tx_dma = 0;
-	}
-
-	if (!t->rx_buf) {
-		mspi->rx_dma = mspi->dma_dummy_rx;
-		mspi->map_rx_dma = 0;
-	}
-
-	if (mspi->map_tx_dma) {
-		void *nonconst_tx = (void *)mspi->tx; /* shut up gcc */
-
-		mspi->tx_dma = dma_map_single(dev, nonconst_tx, t->len,
-					      DMA_TO_DEVICE);
-		if (dma_mapping_error(dev, mspi->tx_dma)) {
-			dev_err(dev, "unable to map tx dma\n");
-			return -ENOMEM;
-		}
-	} else if (t->tx_buf) {
-		mspi->tx_dma = t->tx_dma;
-	}
-
-	if (mspi->map_rx_dma) {
-		mspi->rx_dma = dma_map_single(dev, mspi->rx, t->len,
-					      DMA_FROM_DEVICE);
-		if (dma_mapping_error(dev, mspi->rx_dma)) {
-			dev_err(dev, "unable to map rx dma\n");
-			goto err_rx_dma;
-		}
-	} else if (t->rx_buf) {
-		mspi->rx_dma = t->rx_dma;
-	}
-
-	/* enable rx ints */
-	mpc8xxx_spi_write_reg(&reg_base->mask, SPIE_RXB);
-
-	mspi->xfer_in_progress = t;
-	mspi->count = t->len;
-
-	/* start CPM transfers */
-	fsl_spi_cpm_bufs_start(mspi);
-
-	return 0;
-
-err_rx_dma:
-	if (mspi->map_tx_dma)
-		dma_unmap_single(dev, mspi->tx_dma, t->len, DMA_TO_DEVICE);
-	return -ENOMEM;
-}
-
-static void fsl_spi_cpm_bufs_complete(struct mpc8xxx_spi *mspi)
-{
-	struct device *dev = mspi->dev;
-	struct spi_transfer *t = mspi->xfer_in_progress;
-
-	if (mspi->map_tx_dma)
-		dma_unmap_single(dev, mspi->tx_dma, t->len, DMA_TO_DEVICE);
-	if (mspi->map_rx_dma)
-		dma_unmap_single(dev, mspi->rx_dma, t->len, DMA_FROM_DEVICE);
-	mspi->xfer_in_progress = NULL;
-}
-
 static int fsl_spi_cpu_bufs(struct mpc8xxx_spi *mspi,
 				struct spi_transfer *t, unsigned int len)
 {
@@ -565,31 +455,45 @@ static int fsl_spi_setup(struct spi_device *spi)
 		cs->hw_mode = hw_mode; /* Restore settings */
 		return retval;
 	}
-	return 0;
-}
 
-static void fsl_spi_cpm_irq(struct mpc8xxx_spi *mspi, u32 events)
-{
-	u16 len;
-	struct fsl_spi_reg *reg_base = mspi->reg_base;
+	if (mpc8xxx_spi->type == TYPE_GRLIB) {
+		if (gpio_is_valid(spi->cs_gpio)) {
+			int desel;
 
-	dev_dbg(mspi->dev, "%s: bd datlen %d, count %d\n", __func__,
-		in_be16(&mspi->rx_bd->cbd_datlen), mspi->count);
+			retval = gpio_request(spi->cs_gpio,
+					      dev_name(&spi->dev));
+			if (retval)
+				return retval;
 
-	len = in_be16(&mspi->rx_bd->cbd_datlen);
-	if (len > mspi->count) {
-		WARN_ON(1);
-		len = mspi->count;
+			desel = !(spi->mode & SPI_CS_HIGH);
+			retval = gpio_direction_output(spi->cs_gpio, desel);
+			if (retval) {
+				gpio_free(spi->cs_gpio);
+				return retval;
+			}
+		} else if (spi->cs_gpio != -ENOENT) {
+			if (spi->cs_gpio < 0)
+				return spi->cs_gpio;
+			return -EINVAL;
+		}
+		/* When spi->cs_gpio == -ENOENT, a hole in the phandle list
+		 * indicates to use native chipselect if present, or allow for
+		 * an always selected chip
+		 */
 	}
 
-	/* Clear the events */
-	mpc8xxx_spi_write_reg(&reg_base->event, events);
+	/* Initialize chipselect - might be active for SPI_CS_HIGH mode */
+	fsl_spi_chipselect(spi, BITBANG_CS_INACTIVE);
 
-	mspi->count -= len;
-	if (mspi->count)
-		fsl_spi_cpm_bufs_start(mspi);
-	else
-		complete(&mspi->done);
+	return 0;
+}
+
+static void fsl_spi_cleanup(struct spi_device *spi)
+{
+	struct mpc8xxx_spi *mpc8xxx_spi = spi_master_get_devdata(spi->master);
+
+	if (mpc8xxx_spi->type == TYPE_GRLIB && gpio_is_valid(spi->cs_gpio))
+		gpio_free(spi->cs_gpio);
 }
 
 static void fsl_spi_cpu_irq(struct mpc8xxx_spi *mspi, u32 events)
@@ -646,201 +550,51 @@ static irqreturn_t fsl_spi_irq(s32 irq, void *context_data)
 	return ret;
 }
 
-static void *fsl_spi_alloc_dummy_rx(void)
-{
-	mutex_lock(&fsl_dummy_rx_lock);
-
-	if (!fsl_dummy_rx)
-		fsl_dummy_rx = kmalloc(SPI_MRBLR, GFP_KERNEL);
-	if (fsl_dummy_rx)
-		fsl_dummy_rx_refcnt++;
-
-	mutex_unlock(&fsl_dummy_rx_lock);
-
-	return fsl_dummy_rx;
-}
-
-static void fsl_spi_free_dummy_rx(void)
+static void fsl_spi_remove(struct mpc8xxx_spi *mspi)
 {
-	mutex_lock(&fsl_dummy_rx_lock);
-
-	switch (fsl_dummy_rx_refcnt) {
-	case 0:
-		WARN_ON(1);
-		break;
-	case 1:
-		kfree(fsl_dummy_rx);
-		fsl_dummy_rx = NULL;
-		/* fall through */
-	default:
-		fsl_dummy_rx_refcnt--;
-		break;
-	}
-
-	mutex_unlock(&fsl_dummy_rx_lock);
+	iounmap(mspi->reg_base);
+	fsl_spi_cpm_free(mspi);
 }
 
-static unsigned long fsl_spi_cpm_get_pram(struct mpc8xxx_spi *mspi)
+static void fsl_spi_grlib_cs_control(struct spi_device *spi, bool on)
 {
-	struct device *dev = mspi->dev;
-	struct device_node *np = dev->of_node;
-	const u32 *iprop;
-	int size;
-	void __iomem *spi_base;
-	unsigned long pram_ofs = -ENOMEM;
-
-	/* Can't use of_address_to_resource(), QE muram isn't at 0. */
-	iprop = of_get_property(np, "reg", &size);
-
-	/* QE with a fixed pram location? */
-	if (mspi->flags & SPI_QE && iprop && size == sizeof(*iprop) * 4)
-		return cpm_muram_alloc_fixed(iprop[2], SPI_PRAM_SIZE);
-
-	/* QE but with a dynamic pram location? */
-	if (mspi->flags & SPI_QE) {
-		pram_ofs = cpm_muram_alloc(SPI_PRAM_SIZE, 64);
-		qe_issue_cmd(QE_ASSIGN_PAGE_TO_DEVICE, mspi->subblock,
-				QE_CR_PROTOCOL_UNSPECIFIED, pram_ofs);
-		return pram_ofs;
-	}
-
-	spi_base = of_iomap(np, 1);
-	if (spi_base == NULL)
-		return -EINVAL;
+	struct mpc8xxx_spi *mpc8xxx_spi = spi_master_get_devdata(spi->master);
+	struct fsl_spi_reg *reg_base = mpc8xxx_spi->reg_base;
+	u32 slvsel;
+	u16 cs = spi->chip_select;
 
-	if (mspi->flags & SPI_CPM2) {
-		pram_ofs = cpm_muram_alloc(SPI_PRAM_SIZE, 64);
-		out_be16(spi_base, pram_ofs);
-	} else {
-		struct spi_pram __iomem *pram = spi_base;
-		u16 rpbase = in_be16(&pram->rpbase);
-
-		/* Microcode relocation patch applied? */
-		if (rpbase)
-			pram_ofs = rpbase;
-		else {
-			pram_ofs = cpm_muram_alloc(SPI_PRAM_SIZE, 64);
-			out_be16(spi_base, pram_ofs);
-		}
+	if (gpio_is_valid(spi->cs_gpio)) {
+		gpio_set_value(spi->cs_gpio, on);
+	} else if (cs < mpc8xxx_spi->native_chipselects) {
+		slvsel = mpc8xxx_spi_read_reg(&reg_base->slvsel);
+		slvsel = on ? (slvsel | (1 << cs)) : (slvsel & ~(1 << cs));
+		mpc8xxx_spi_write_reg(&reg_base->slvsel, slvsel);
 	}
-
-	iounmap(spi_base);
-	return pram_ofs;
 }
 
-static int fsl_spi_cpm_init(struct mpc8xxx_spi *mspi)
+static void fsl_spi_grlib_probe(struct device *dev)
 {
-	struct device *dev = mspi->dev;
-	struct device_node *np = dev->of_node;
-	const u32 *iprop;
-	int size;
-	unsigned long pram_ofs;
-	unsigned long bds_ofs;
-
-	if (!(mspi->flags & SPI_CPM_MODE))
-		return 0;
-
-	if (!fsl_spi_alloc_dummy_rx())
-		return -ENOMEM;
-
-	if (mspi->flags & SPI_QE) {
-		iprop = of_get_property(np, "cell-index", &size);
-		if (iprop && size == sizeof(*iprop))
-			mspi->subblock = *iprop;
-
-		switch (mspi->subblock) {
-		default:
-			dev_warn(dev, "cell-index unspecified, assuming SPI1");
-			/* fall through */
-		case 0:
-			mspi->subblock = QE_CR_SUBBLOCK_SPI1;
-			break;
-		case 1:
-			mspi->subblock = QE_CR_SUBBLOCK_SPI2;
-			break;
-		}
-	}
-
-	pram_ofs = fsl_spi_cpm_get_pram(mspi);
-	if (IS_ERR_VALUE(pram_ofs)) {
-		dev_err(dev, "can't allocate spi parameter ram\n");
-		goto err_pram;
-	}
+	struct fsl_spi_platform_data *pdata = dev->platform_data;
+	struct spi_master *master = dev_get_drvdata(dev);
+	struct mpc8xxx_spi *mpc8xxx_spi = spi_master_get_devdata(master);
+	struct fsl_spi_reg *reg_base = mpc8xxx_spi->reg_base;
+	int mbits;
+	u32 capabilities;
 
-	bds_ofs = cpm_muram_alloc(sizeof(*mspi->tx_bd) +
-				  sizeof(*mspi->rx_bd), 8);
-	if (IS_ERR_VALUE(bds_ofs)) {
-		dev_err(dev, "can't allocate bds\n");
-		goto err_bds;
-	}
+	capabilities = mpc8xxx_spi_read_reg(&reg_base->cap);
 
-	mspi->dma_dummy_tx = dma_map_single(dev, empty_zero_page, PAGE_SIZE,
-					    DMA_TO_DEVICE);
-	if (dma_mapping_error(dev, mspi->dma_dummy_tx)) {
-		dev_err(dev, "unable to map dummy tx buffer\n");
-		goto err_dummy_tx;
-	}
+	mpc8xxx_spi->set_shifts = fsl_spi_grlib_set_shifts;
+	mbits = SPCAP_MAXWLEN(capabilities);
+	if (mbits)
+		mpc8xxx_spi->max_bits_per_word = mbits + 1;
 
-	mspi->dma_dummy_rx = dma_map_single(dev, fsl_dummy_rx, SPI_MRBLR,
-					    DMA_FROM_DEVICE);
-	if (dma_mapping_error(dev, mspi->dma_dummy_rx)) {
-		dev_err(dev, "unable to map dummy rx buffer\n");
-		goto err_dummy_rx;
+	mpc8xxx_spi->native_chipselects = 0;
+	if (SPCAP_SSEN(capabilities)) {
+		mpc8xxx_spi->native_chipselects = SPCAP_SSSZ(capabilities);
+		mpc8xxx_spi_write_reg(&reg_base->slvsel, 0xffffffff);
 	}
-
-	mspi->pram = cpm_muram_addr(pram_ofs);
-
-	mspi->tx_bd = cpm_muram_addr(bds_ofs);
-	mspi->rx_bd = cpm_muram_addr(bds_ofs + sizeof(*mspi->tx_bd));
-
-	/* Initialize parameter ram. */
-	out_be16(&mspi->pram->tbase, cpm_muram_offset(mspi->tx_bd));
-	out_be16(&mspi->pram->rbase, cpm_muram_offset(mspi->rx_bd));
-	out_8(&mspi->pram->tfcr, CPMFCR_EB | CPMFCR_GBL);
-	out_8(&mspi->pram->rfcr, CPMFCR_EB | CPMFCR_GBL);
-	out_be16(&mspi->pram->mrblr, SPI_MRBLR);
-	out_be32(&mspi->pram->rstate, 0);
-	out_be32(&mspi->pram->rdp, 0);
-	out_be16(&mspi->pram->rbptr, 0);
-	out_be16(&mspi->pram->rbc, 0);
-	out_be32(&mspi->pram->rxtmp, 0);
-	out_be32(&mspi->pram->tstate, 0);
-	out_be32(&mspi->pram->tdp, 0);
-	out_be16(&mspi->pram->tbptr, 0);
-	out_be16(&mspi->pram->tbc, 0);
-	out_be32(&mspi->pram->txtmp, 0);
-
-	return 0;
-
-err_dummy_rx:
-	dma_unmap_single(dev, mspi->dma_dummy_tx, PAGE_SIZE, DMA_TO_DEVICE);
-err_dummy_tx:
-	cpm_muram_free(bds_ofs);
-err_bds:
-	cpm_muram_free(pram_ofs);
-err_pram:
-	fsl_spi_free_dummy_rx();
-	return -ENOMEM;
-}
-
-static void fsl_spi_cpm_free(struct mpc8xxx_spi *mspi)
-{
-	struct device *dev = mspi->dev;
-
-	if (!(mspi->flags & SPI_CPM_MODE))
-		return;
-
-	dma_unmap_single(dev, mspi->dma_dummy_rx, SPI_MRBLR, DMA_FROM_DEVICE);
-	dma_unmap_single(dev, mspi->dma_dummy_tx, PAGE_SIZE, DMA_TO_DEVICE);
-	cpm_muram_free(cpm_muram_offset(mspi->tx_bd));
-	cpm_muram_free(cpm_muram_offset(mspi->pram));
-	fsl_spi_free_dummy_rx();
-}
-
-static void fsl_spi_remove(struct mpc8xxx_spi *mspi)
-{
-	iounmap(mspi->reg_base);
-	fsl_spi_cpm_free(mspi);
+	master->num_chipselect = mpc8xxx_spi->native_chipselects;
+	pdata->cs_control = fsl_spi_grlib_cs_control;
 }
 
 static struct spi_master * fsl_spi_probe(struct device *dev,
@@ -866,27 +620,35 @@ static struct spi_master * fsl_spi_probe(struct device *dev,
 		goto err_probe;
 
 	master->setup = fsl_spi_setup;
+	master->cleanup = fsl_spi_cleanup;
 
 	mpc8xxx_spi = spi_master_get_devdata(master);
 	mpc8xxx_spi->spi_do_one_msg = fsl_spi_do_one_msg;
 	mpc8xxx_spi->spi_remove = fsl_spi_remove;
-
+	mpc8xxx_spi->max_bits_per_word = 32;
+	mpc8xxx_spi->type = fsl_spi_get_type(dev);
 
 	ret = fsl_spi_cpm_init(mpc8xxx_spi);
 	if (ret)
 		goto err_cpm_init;
 
-	if (mpc8xxx_spi->flags & SPI_QE_CPU_MODE) {
-		mpc8xxx_spi->rx_shift = 16;
-		mpc8xxx_spi->tx_shift = 24;
-	}
-
 	mpc8xxx_spi->reg_base = ioremap(mem->start, resource_size(mem));
 	if (mpc8xxx_spi->reg_base == NULL) {
 		ret = -ENOMEM;
 		goto err_ioremap;
 	}
 
+	if (mpc8xxx_spi->type == TYPE_GRLIB)
+		fsl_spi_grlib_probe(dev);
+
+	if (mpc8xxx_spi->flags & SPI_QE_CPU_MODE)
+		mpc8xxx_spi->set_shifts = fsl_spi_qe_cpu_set_shifts;
+
+	if (mpc8xxx_spi->set_shifts)
+		/* 8 bits per word and MSB first */
+		mpc8xxx_spi->set_shifts(&mpc8xxx_spi->rx_shift,
+					&mpc8xxx_spi->tx_shift, 8, 1);
+
 	/* Register for SPI Interrupt */
 	ret = request_irq(mpc8xxx_spi->irq, fsl_spi_irq,
 			  0, "fsl_spi", mpc8xxx_spi);
@@ -904,6 +666,10 @@ static struct spi_master * fsl_spi_probe(struct device *dev,
 
 	/* Enable SPI interface */
 	regval = pdata->initial_spmode | SPMODE_INIT_VAL | SPMODE_ENABLE;
+	if (mpc8xxx_spi->max_bits_per_word < 8) {
+		regval &= ~SPMODE_LEN(0xF);
+		regval |= SPMODE_LEN(mpc8xxx_spi->max_bits_per_word - 1);
+	}
 	if (mpc8xxx_spi->flags & SPI_QE_CPU_MODE)
 		regval |= SPMODE_OP;
 
@@ -1047,28 +813,31 @@ static int of_fsl_spi_probe(struct platform_device *ofdev)
 	struct device_node *np = ofdev->dev.of_node;
 	struct spi_master *master;
 	struct resource mem;
-	struct resource irq;
+	int irq, type;
 	int ret = -ENOMEM;
 
 	ret = of_mpc8xxx_spi_probe(ofdev);
 	if (ret)
 		return ret;
 
-	ret = of_fsl_spi_get_chipselects(dev);
-	if (ret)
-		goto err;
+	type = fsl_spi_get_type(&ofdev->dev);
+	if (type == TYPE_FSL) {
+		ret = of_fsl_spi_get_chipselects(dev);
+		if (ret)
+			goto err;
+	}
 
 	ret = of_address_to_resource(np, 0, &mem);
 	if (ret)
 		goto err;
 
-	ret = of_irq_to_resource(np, 0, &irq);
-	if (!ret) {
+	irq = irq_of_parse_and_map(np, 0);
+	if (!irq) {
 		ret = -EINVAL;
 		goto err;
 	}
 
-	master = fsl_spi_probe(dev, &mem, irq.start);
+	master = fsl_spi_probe(dev, &mem, irq);
 	if (IS_ERR(master)) {
 		ret = PTR_ERR(master);
 		goto err;
@@ -1077,27 +846,25 @@ static int of_fsl_spi_probe(struct platform_device *ofdev)
 	return 0;
 
 err:
-	of_fsl_spi_free_chipselects(dev);
+	if (type == TYPE_FSL)
+		of_fsl_spi_free_chipselects(dev);
 	return ret;
 }
 
 static int of_fsl_spi_remove(struct platform_device *ofdev)
 {
+	struct spi_master *master = dev_get_drvdata(&ofdev->dev);
+	struct mpc8xxx_spi *mpc8xxx_spi = spi_master_get_devdata(master);
 	int ret;
 
 	ret = mpc8xxx_spi_remove(&ofdev->dev);
 	if (ret)
 		return ret;
-	of_fsl_spi_free_chipselects(&ofdev->dev);
+	if (mpc8xxx_spi->type == TYPE_FSL)
+		of_fsl_spi_free_chipselects(&ofdev->dev);
 	return 0;
 }
 
-static const struct of_device_id of_fsl_spi_match[] = {
-	{ .compatible = "fsl,spi" },
-	{}
-};
-MODULE_DEVICE_TABLE(of, of_fsl_spi_match);
-
 static struct platform_driver of_fsl_spi_driver = {
 	.driver = {
 		.name = "fsl_spi",
@@ -1134,9 +901,7 @@ static int plat_mpc8xxx_spi_probe(struct platform_device *pdev)
 		return -EINVAL;
 
 	master = fsl_spi_probe(&pdev->dev, mem, irq);
-	if (IS_ERR(master))
-		return PTR_ERR(master);
-	return 0;
+	return PTR_RET(master);
 }
 
 static int plat_mpc8xxx_spi_remove(struct platform_device *pdev)
diff --git a/drivers/spi/spi-fsl-spi.h b/drivers/spi/spi-fsl-spi.h
new file mode 100644
index 00000000000..9a6dae00e3f
--- /dev/null
+++ b/drivers/spi/spi-fsl-spi.h
@@ -0,0 +1,72 @@
+/*
+ * Freescale SPI controller driver.
+ *
+ * Maintainer: Kumar Gala
+ *
+ * Copyright (C) 2006 Polycom, Inc.
+ * Copyright 2010 Freescale Semiconductor, Inc.
+ *
+ * CPM SPI and QE buffer descriptors mode support:
+ * Copyright (c) 2009  MontaVista Software, Inc.
+ * Author: Anton Vorontsov <avorontsov@ru.mvista.com>
+ *
+ * GRLIB support:
+ * Copyright (c) 2012 Aeroflex Gaisler AB.
+ * Author: Andreas Larsson <andreas@gaisler.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#ifndef __SPI_FSL_SPI_H__
+#define __SPI_FSL_SPI_H__
+
+/* SPI Controller registers */
+struct fsl_spi_reg {
+	__be32 cap; /* TYPE_GRLIB specific */
+	u8 res1[0x1C];
+	__be32 mode;
+	__be32 event;
+	__be32 mask;
+	__be32 command;
+	__be32 transmit;
+	__be32 receive;
+	__be32 slvsel; /* TYPE_GRLIB specific */
+};
+
+/* SPI Controller mode register definitions */
+#define	SPMODE_LOOP		(1 << 30)
+#define	SPMODE_CI_INACTIVEHIGH	(1 << 29)
+#define	SPMODE_CP_BEGIN_EDGECLK	(1 << 28)
+#define	SPMODE_DIV16		(1 << 27)
+#define	SPMODE_REV		(1 << 26)
+#define	SPMODE_MS		(1 << 25)
+#define	SPMODE_ENABLE		(1 << 24)
+#define	SPMODE_LEN(x)		((x) << 20)
+#define	SPMODE_PM(x)		((x) << 16)
+#define	SPMODE_OP		(1 << 14)
+#define	SPMODE_CG(x)		((x) << 7)
+
+/* TYPE_GRLIB SPI Controller capability register definitions */
+#define SPCAP_SSEN(x)		(((x) >> 16) & 0x1)
+#define SPCAP_SSSZ(x)		(((x) >> 24) & 0xff)
+#define SPCAP_MAXWLEN(x)	(((x) >> 20) & 0xf)
+
+/*
+ * Default for SPI Mode:
+ *	SPI MODE 0 (inactive low, phase middle, MSB, 8-bit length, slow clk
+ */
+#define	SPMODE_INIT_VAL (SPMODE_CI_INACTIVEHIGH | SPMODE_DIV16 | SPMODE_REV | \
+			 SPMODE_MS | SPMODE_LEN(7) | SPMODE_PM(0xf))
+
+/* SPIE register values */
+#define	SPIE_NE		0x00000200	/* Not empty */
+#define	SPIE_NF		0x00000100	/* Not full */
+
+/* SPIM register values */
+#define	SPIM_NE		0x00000200	/* Not empty */
+#define	SPIM_NF		0x00000100	/* Not full */
+
+#endif /* __SPI_FSL_SPI_H__ */
diff --git a/drivers/spi/spi-gpio.c b/drivers/spi/spi-gpio.c
index 9ddef55a716..0021fc4c45b 100644
--- a/drivers/spi/spi-gpio.c
+++ b/drivers/spi/spi-gpio.c
@@ -265,9 +265,9 @@ static int spi_gpio_setup(struct spi_device *spi)
 		}
 	}
 	if (!status) {
-		status = spi_bitbang_setup(spi);
 		/* in case it was initialized from static board data */
 		spi_gpio->cs_gpios[spi->chip_select] = cs;
+		status = spi_bitbang_setup(spi);
 	}
 
 	if (status) {
diff --git a/drivers/spi/spi-mpc512x-psc.c b/drivers/spi/spi-mpc512x-psc.c
index 89480b281d7..dfddf336912 100644
--- a/drivers/spi/spi-mpc512x-psc.c
+++ b/drivers/spi/spi-mpc512x-psc.c
@@ -28,6 +28,7 @@
 #include <linux/clk.h>
 #include <linux/spi/spi.h>
 #include <linux/fsl_devices.h>
+#include <linux/gpio.h>
 #include <asm/mpc52xx_psc.h>
 
 struct mpc512x_psc_spi {
@@ -113,7 +114,7 @@ static void mpc512x_psc_spi_activate_cs(struct spi_device *spi)
 	out_be32(&psc->ccr, ccr);
 	mps->bits_per_word = cs->bits_per_word;
 
-	if (mps->cs_control)
+	if (mps->cs_control && gpio_is_valid(spi->cs_gpio))
 		mps->cs_control(spi, (spi->mode & SPI_CS_HIGH) ? 1 : 0);
 }
 
@@ -121,7 +122,7 @@ static void mpc512x_psc_spi_deactivate_cs(struct spi_device *spi)
 {
 	struct mpc512x_psc_spi *mps = spi_master_get_devdata(spi->master);
 
-	if (mps->cs_control)
+	if (mps->cs_control && gpio_is_valid(spi->cs_gpio))
 		mps->cs_control(spi, (spi->mode & SPI_CS_HIGH) ? 0 : 1);
 
 }
@@ -148,6 +149,9 @@ static int mpc512x_psc_spi_transfer_rxtx(struct spi_device *spi,
 	in_8(&psc->mode);
 	out_8(&psc->mode, 0x0);
 
+	/* enable transmiter/receiver */
+	out_8(&psc->command, MPC52xx_PSC_TX_ENABLE | MPC52xx_PSC_RX_ENABLE);
+
 	while (len) {
 		int count;
 		int i;
@@ -164,7 +168,7 @@ static int mpc512x_psc_spi_transfer_rxtx(struct spi_device *spi,
 
 		for (i = count; i > 0; i--) {
 			data = tx_buf ? *tx_buf++ : 0;
-			if (len == EOFBYTE)
+			if (len == EOFBYTE && t->cs_change)
 				setbits32(&fifo->txcmd, MPC512x_PSC_FIFO_EOF);
 			out_8(&fifo->txdata_8, data);
 			len--;
@@ -176,10 +180,6 @@ static int mpc512x_psc_spi_transfer_rxtx(struct spi_device *spi,
 		out_be32(&fifo->txisr, MPC512x_PSC_FIFO_EMPTY);
 		out_be32(&fifo->tximr, MPC512x_PSC_FIFO_EMPTY);
 
-		/* enable transmiter/receiver */
-		out_8(&psc->command,
-		      MPC52xx_PSC_TX_ENABLE | MPC52xx_PSC_RX_ENABLE);
-
 		wait_for_completion(&mps->done);
 
 		mdelay(1);
@@ -204,9 +204,6 @@ static int mpc512x_psc_spi_transfer_rxtx(struct spi_device *spi,
 		while (in_be32(&fifo->rxcnt)) {
 			in_8(&fifo->rxdata_8);
 		}
-
-		out_8(&psc->command,
-		      MPC52xx_PSC_TX_DISABLE | MPC52xx_PSC_RX_DISABLE);
 	}
 	/* disable transmiter/receiver and fifo interrupt */
 	out_8(&psc->command, MPC52xx_PSC_TX_DISABLE | MPC52xx_PSC_RX_DISABLE);
@@ -278,6 +275,7 @@ static int mpc512x_psc_spi_setup(struct spi_device *spi)
 	struct mpc512x_psc_spi *mps = spi_master_get_devdata(spi->master);
 	struct mpc512x_psc_spi_cs *cs = spi->controller_state;
 	unsigned long flags;
+	int ret;
 
 	if (spi->bits_per_word % 8)
 		return -EINVAL;
@@ -286,6 +284,19 @@ static int mpc512x_psc_spi_setup(struct spi_device *spi)
 		cs = kzalloc(sizeof *cs, GFP_KERNEL);
 		if (!cs)
 			return -ENOMEM;
+
+		if (gpio_is_valid(spi->cs_gpio)) {
+			ret = gpio_request(spi->cs_gpio, dev_name(&spi->dev));
+			if (ret) {
+				dev_err(&spi->dev, "can't get CS gpio: %d\n",
+					ret);
+				kfree(cs);
+				return ret;
+			}
+			gpio_direction_output(spi->cs_gpio,
+					spi->mode & SPI_CS_HIGH ? 0 : 1);
+		}
+
 		spi->controller_state = cs;
 	}
 
@@ -319,6 +330,8 @@ static int mpc512x_psc_spi_transfer(struct spi_device *spi,
 
 static void mpc512x_psc_spi_cleanup(struct spi_device *spi)
 {
+	if (gpio_is_valid(spi->cs_gpio))
+		gpio_free(spi->cs_gpio);
 	kfree(spi->controller_state);
 }
 
@@ -405,6 +418,11 @@ static irqreturn_t mpc512x_psc_spi_isr(int irq, void *dev_id)
 	return IRQ_NONE;
 }
 
+static void mpc512x_spi_cs_control(struct spi_device *spi, bool onoff)
+{
+	gpio_set_value(spi->cs_gpio, onoff);
+}
+
 /* bus_num is used only for the case dev->platform_data == NULL */
 static int mpc512x_psc_spi_do_probe(struct device *dev, u32 regaddr,
 					      u32 size, unsigned int irq,
@@ -425,12 +443,9 @@ static int mpc512x_psc_spi_do_probe(struct device *dev, u32 regaddr,
 	mps->irq = irq;
 
 	if (pdata == NULL) {
-		dev_err(dev, "probe called without platform data, no "
-			"cs_control function will be called\n");
-		mps->cs_control = NULL;
+		mps->cs_control = mpc512x_spi_cs_control;
 		mps->sysclk = 0;
 		master->bus_num = bus_num;
-		master->num_chipselect = 255;
 	} else {
 		mps->cs_control = pdata->cs_control;
 		mps->sysclk = pdata->sysclk;
diff --git a/drivers/spi/spi-mxs.c b/drivers/spi/spi-mxs.c
index 22a0af0147f..a1d5778e2bb 100644
--- a/drivers/spi/spi-mxs.c
+++ b/drivers/spi/spi-mxs.c
@@ -612,6 +612,7 @@ static int mxs_spi_probe(struct platform_device *pdev)
 	ssp->dmach = dma_request_channel(mask, mxs_ssp_dma_filter, ssp);
 	if (!ssp->dmach) {
 		dev_err(ssp->dev, "Failed to request DMA\n");
+		ret = -ENODEV;
 		goto out_master_free;
 	}
 
diff --git a/drivers/spi/spi-oc-tiny.c b/drivers/spi/spi-oc-tiny.c
index cb2e284bd81..e60a776ed2d 100644
--- a/drivers/spi/spi-oc-tiny.c
+++ b/drivers/spi/spi-oc-tiny.c
@@ -393,8 +393,6 @@ static const struct of_device_id tiny_spi_match[] = {
 	{},
 };
 MODULE_DEVICE_TABLE(of, tiny_spi_match);
-#else /* CONFIG_OF */
-#define tiny_spi_match NULL
 #endif /* CONFIG_OF */
 
 static struct platform_driver tiny_spi_driver = {
@@ -404,7 +402,7 @@ static struct platform_driver tiny_spi_driver = {
 		.name = DRV_NAME,
 		.owner = THIS_MODULE,
 		.pm = NULL,
-		.of_match_table = tiny_spi_match,
+		.of_match_table = of_match_ptr(tiny_spi_match),
 	},
 };
 module_platform_driver(tiny_spi_driver);
diff --git a/drivers/spi/spi-omap2-mcspi.c b/drivers/spi/spi-omap2-mcspi.c
index 893c3d78e42..86d2158946b 100644
--- a/drivers/spi/spi-omap2-mcspi.c
+++ b/drivers/spi/spi-omap2-mcspi.c
@@ -285,8 +285,12 @@ static int mcspi_wait_for_reg_bit(void __iomem *reg, unsigned long bit)
 
 	timeout = jiffies + msecs_to_jiffies(1000);
 	while (!(__raw_readl(reg) & bit)) {
-		if (time_after(jiffies, timeout))
-			return -1;
+		if (time_after(jiffies, timeout)) {
+			if (!(__raw_readl(reg) & bit))
+				return -ETIMEDOUT;
+			else
+				return 0;
+		}
 		cpu_relax();
 	}
 	return 0;
@@ -805,6 +809,10 @@ static int omap2_mcspi_setup_transfer(struct spi_device *spi,
 	return 0;
 }
 
+/*
+ * Note that we currently allow DMA only if we get a channel
+ * for both rx and tx. Otherwise we'll do PIO for both rx and tx.
+ */
 static int omap2_mcspi_request_dma(struct spi_device *spi)
 {
 	struct spi_master	*master = spi->master;
@@ -823,21 +831,22 @@ static int omap2_mcspi_request_dma(struct spi_device *spi)
 	dma_cap_set(DMA_SLAVE, mask);
 	sig = mcspi_dma->dma_rx_sync_dev;
 	mcspi_dma->dma_rx = dma_request_channel(mask, omap_dma_filter_fn, &sig);
-	if (!mcspi_dma->dma_rx) {
-		dev_err(&spi->dev, "no RX DMA engine channel for McSPI\n");
-		return -EAGAIN;
-	}
+	if (!mcspi_dma->dma_rx)
+		goto no_dma;
 
 	sig = mcspi_dma->dma_tx_sync_dev;
 	mcspi_dma->dma_tx = dma_request_channel(mask, omap_dma_filter_fn, &sig);
 	if (!mcspi_dma->dma_tx) {
-		dev_err(&spi->dev, "no TX DMA engine channel for McSPI\n");
 		dma_release_channel(mcspi_dma->dma_rx);
 		mcspi_dma->dma_rx = NULL;
-		return -EAGAIN;
+		goto no_dma;
 	}
 
 	return 0;
+
+no_dma:
+	dev_warn(&spi->dev, "not using DMA for McSPI\n");
+	return -EAGAIN;
 }
 
 static int omap2_mcspi_setup(struct spi_device *spi)
@@ -870,7 +879,7 @@ static int omap2_mcspi_setup(struct spi_device *spi)
 
 	if (!mcspi_dma->dma_rx || !mcspi_dma->dma_tx) {
 		ret = omap2_mcspi_request_dma(spi);
-		if (ret < 0)
+		if (ret < 0 && ret != -EAGAIN)
 			return ret;
 	}
 
@@ -928,6 +937,7 @@ static void omap2_mcspi_work(struct omap2_mcspi *mcspi, struct spi_message *m)
 	struct spi_device		*spi;
 	struct spi_transfer		*t = NULL;
 	struct spi_master		*master;
+	struct omap2_mcspi_dma		*mcspi_dma;
 	int				cs_active = 0;
 	struct omap2_mcspi_cs		*cs;
 	struct omap2_mcspi_device_config *cd;
@@ -937,6 +947,7 @@ static void omap2_mcspi_work(struct omap2_mcspi *mcspi, struct spi_message *m)
 
 	spi = m->spi;
 	master = spi->master;
+	mcspi_dma = mcspi->dma_channels + spi->chip_select;
 	cs = spi->controller_state;
 	cd = spi->controller_data;
 
@@ -993,7 +1004,8 @@ static void omap2_mcspi_work(struct omap2_mcspi *mcspi, struct spi_message *m)
 				__raw_writel(0, cs->base
 						+ OMAP2_MCSPI_TX0);
 
-			if (m->is_dma_mapped || t->len >= DMA_MIN_BYTES)
+			if ((mcspi_dma->dma_rx && mcspi_dma->dma_tx) &&
+			    (m->is_dma_mapped || t->len >= DMA_MIN_BYTES))
 				count = omap2_mcspi_txrx_dma(spi, t);
 			else
 				count = omap2_mcspi_txrx_pio(spi, t);
@@ -1040,10 +1052,14 @@ static void omap2_mcspi_work(struct omap2_mcspi *mcspi, struct spi_message *m)
 static int omap2_mcspi_transfer_one_message(struct spi_master *master,
 		struct spi_message *m)
 {
+	struct spi_device	*spi;
 	struct omap2_mcspi	*mcspi;
+	struct omap2_mcspi_dma	*mcspi_dma;
 	struct spi_transfer	*t;
 
+	spi = m->spi;
 	mcspi = spi_master_get_devdata(master);
+	mcspi_dma = mcspi->dma_channels + spi->chip_select;
 	m->actual_length = 0;
 	m->status = 0;
 
@@ -1078,7 +1094,7 @@ static int omap2_mcspi_transfer_one_message(struct spi_master *master,
 		if (m->is_dma_mapped || len < DMA_MIN_BYTES)
 			continue;
 
-		if (tx_buf != NULL) {
+		if (mcspi_dma->dma_tx && tx_buf != NULL) {
 			t->tx_dma = dma_map_single(mcspi->dev, (void *) tx_buf,
 					len, DMA_TO_DEVICE);
 			if (dma_mapping_error(mcspi->dev, t->tx_dma)) {
@@ -1087,7 +1103,7 @@ static int omap2_mcspi_transfer_one_message(struct spi_master *master,
 				return -EINVAL;
 			}
 		}
-		if (rx_buf != NULL) {
+		if (mcspi_dma->dma_rx && rx_buf != NULL) {
 			t->rx_dma = dma_map_single(mcspi->dev, rx_buf, t->len,
 					DMA_FROM_DEVICE);
 			if (dma_mapping_error(mcspi->dev, t->rx_dma)) {
@@ -1277,7 +1293,8 @@ static int omap2_mcspi_probe(struct platform_device *pdev)
 	pm_runtime_set_autosuspend_delay(&pdev->dev, SPI_AUTOSUSPEND_TIMEOUT);
 	pm_runtime_enable(&pdev->dev);
 
-	if (status || omap2_mcspi_master_setup(mcspi) < 0)
+	status = omap2_mcspi_master_setup(mcspi);
+	if (status < 0)
 		goto disable_pm;
 
 	status = spi_register_master(master);
diff --git a/drivers/spi/spi-pxa2xx-pci.c b/drivers/spi/spi-pxa2xx-pci.c
index 364964d2ed0..74bc1877565 100644
--- a/drivers/spi/spi-pxa2xx-pci.c
+++ b/drivers/spi/spi-pxa2xx-pci.c
@@ -22,7 +22,7 @@ static int ce4100_spi_probe(struct pci_dev *dev,
 		return ret;
 
 	ret = pcim_iomap_regions(dev, 1 << 0, "PXA2xx SPI");
-	if (!ret)
+	if (ret)
 		return ret;
 
 	memset(&spi_pdata, 0, sizeof(spi_pdata));
@@ -47,8 +47,8 @@ static int ce4100_spi_probe(struct pci_dev *dev,
 	pi.size_data = sizeof(spi_pdata);
 
 	pdev = platform_device_register_full(&pi);
-	if (!pdev)
-		return -ENOMEM;
+	if (IS_ERR(pdev))
+		return PTR_ERR(pdev);
 
 	pci_set_drvdata(dev, pdev);
 
diff --git a/drivers/spi/spi-pxa2xx.c b/drivers/spi/spi-pxa2xx.c
index 90b27a3508a..f5d84d6f822 100644
--- a/drivers/spi/spi-pxa2xx.c
+++ b/drivers/spi/spi-pxa2xx.c
@@ -22,6 +22,7 @@
 #include <linux/device.h>
 #include <linux/ioport.h>
 #include <linux/errno.h>
+#include <linux/err.h>
 #include <linux/interrupt.h>
 #include <linux/platform_device.h>
 #include <linux/spi/pxa2xx_spi.h>
@@ -68,6 +69,7 @@ MODULE_ALIAS("platform:pxa2xx-spi");
 #define LPSS_TX_HITHRESH_DFLT	224
 
 /* Offset from drv_data->lpss_base */
+#define SSP_REG			0x0c
 #define SPI_CS_CONTROL		0x18
 #define SPI_CS_CONTROL_SW_MODE	BIT(0)
 #define SPI_CS_CONTROL_CS_HIGH	BIT(1)
@@ -138,6 +140,10 @@ detection_done:
 	/* Enable software chip select control */
 	value = SPI_CS_CONTROL_SW_MODE | SPI_CS_CONTROL_CS_HIGH;
 	__lpss_ssp_write_priv(drv_data, SPI_CS_CONTROL, value);
+
+	/* Enable multiblock DMA transfers */
+	if (drv_data->master_info->enable_dma)
+		__lpss_ssp_write_priv(drv_data, SSP_REG, 1);
 }
 
 static void lpss_ssp_cs_control(struct driver_data *drv_data, bool enable)
@@ -1083,11 +1089,9 @@ pxa2xx_spi_acpi_get_pdata(struct platform_device *pdev)
 	ssp = &pdata->ssp;
 
 	ssp->phys_base = res->start;
-	ssp->mmio_base = devm_request_and_ioremap(&pdev->dev, res);
-	if (!ssp->mmio_base) {
-		dev_err(&pdev->dev, "failed to ioremap mmio_base\n");
-		return NULL;
-	}
+	ssp->mmio_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(ssp->mmio_base))
+		return PTR_ERR(ssp->mmio_base);
 
 	ssp->clk = devm_clk_get(&pdev->dev, NULL);
 	ssp->irq = platform_get_irq(pdev, 0);
@@ -1168,7 +1172,6 @@ static int pxa2xx_spi_probe(struct platform_device *pdev)
 
 	master->dev.parent = &pdev->dev;
 	master->dev.of_node = pdev->dev.of_node;
-	ACPI_HANDLE_SET(&master->dev, ACPI_HANDLE(&pdev->dev));
 	/* the spi->mode bits understood by this driver: */
 	master->mode_bits = SPI_CPOL | SPI_CPHA | SPI_CS_HIGH | SPI_LOOP;
 
diff --git a/drivers/spi/spi-s3c64xx.c b/drivers/spi/spi-s3c64xx.c
index e862ab8853a..5000586cb98 100644
--- a/drivers/spi/spi-s3c64xx.c
+++ b/drivers/spi/spi-s3c64xx.c
@@ -24,6 +24,7 @@
 #include <linux/delay.h>
 #include <linux/clk.h>
 #include <linux/dma-mapping.h>
+#include <linux/dmaengine.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/spi/spi.h>
@@ -31,9 +32,12 @@
 #include <linux/of.h>
 #include <linux/of_gpio.h>
 
-#include <mach/dma.h>
 #include <linux/platform_data/spi-s3c64xx.h>
 
+#ifdef CONFIG_S3C_DMA
+#include <mach/dma.h>
+#endif
+
 #define MAX_SPI_PORTS		3
 
 /* Registers and bit-fields */
@@ -131,9 +135,9 @@
 #define TXBUSY    (1<<3)
 
 struct s3c64xx_spi_dma_data {
-	unsigned		ch;
+	struct dma_chan *ch;
 	enum dma_transfer_direction direction;
-	enum dma_ch	dmach;
+	unsigned int dmach;
 };
 
 /**
@@ -195,16 +199,14 @@ struct s3c64xx_spi_driver_data {
 	unsigned                        cur_speed;
 	struct s3c64xx_spi_dma_data	rx_dma;
 	struct s3c64xx_spi_dma_data	tx_dma;
+#ifdef CONFIG_S3C_DMA
 	struct samsung_dma_ops		*ops;
+#endif
 	struct s3c64xx_spi_port_config	*port_conf;
 	unsigned int			port_id;
 	unsigned long			gpios[4];
 };
 
-static struct s3c2410_dma_client s3c64xx_spi_dma_client = {
-	.name = "samsung-spi-dma",
-};
-
 static void flush_fifo(struct s3c64xx_spi_driver_data *sdd)
 {
 	void __iomem *regs = sdd->regs;
@@ -281,6 +283,13 @@ static void s3c64xx_spi_dmacb(void *data)
 	spin_unlock_irqrestore(&sdd->lock, flags);
 }
 
+#ifdef CONFIG_S3C_DMA
+/* FIXME: remove this section once arch/arm/mach-s3c64xx uses dmaengine */
+
+static struct s3c2410_dma_client s3c64xx_spi_dma_client = {
+	.name = "samsung-spi-dma",
+};
+
 static void prepare_dma(struct s3c64xx_spi_dma_data *dma,
 					unsigned len, dma_addr_t buf)
 {
@@ -294,14 +303,14 @@ static void prepare_dma(struct s3c64xx_spi_dma_data *dma,
 		config.direction = sdd->rx_dma.direction;
 		config.fifo = sdd->sfr_start + S3C64XX_SPI_RX_DATA;
 		config.width = sdd->cur_bpw / 8;
-		sdd->ops->config(sdd->rx_dma.ch, &config);
+		sdd->ops->config((enum dma_ch)sdd->rx_dma.ch, &config);
 	} else {
 		sdd = container_of((void *)dma,
 			struct s3c64xx_spi_driver_data, tx_dma);
 		config.direction =  sdd->tx_dma.direction;
 		config.fifo = sdd->sfr_start + S3C64XX_SPI_TX_DATA;
 		config.width = sdd->cur_bpw / 8;
-		sdd->ops->config(sdd->tx_dma.ch, &config);
+		sdd->ops->config((enum dma_ch)sdd->tx_dma.ch, &config);
 	}
 
 	info.cap = DMA_SLAVE;
@@ -311,8 +320,8 @@ static void prepare_dma(struct s3c64xx_spi_dma_data *dma,
 	info.direction = dma->direction;
 	info.buf = buf;
 
-	sdd->ops->prepare(dma->ch, &info);
-	sdd->ops->trigger(dma->ch);
+	sdd->ops->prepare((enum dma_ch)dma->ch, &info);
+	sdd->ops->trigger((enum dma_ch)dma->ch);
 }
 
 static int acquire_dma(struct s3c64xx_spi_driver_data *sdd)
@@ -325,12 +334,150 @@ static int acquire_dma(struct s3c64xx_spi_driver_data *sdd)
 	req.cap = DMA_SLAVE;
 	req.client = &s3c64xx_spi_dma_client;
 
-	sdd->rx_dma.ch = sdd->ops->request(sdd->rx_dma.dmach, &req, dev, "rx");
-	sdd->tx_dma.ch = sdd->ops->request(sdd->tx_dma.dmach, &req, dev, "tx");
+	sdd->rx_dma.ch = (void *)sdd->ops->request(sdd->rx_dma.dmach, &req, dev, "rx");
+	sdd->tx_dma.ch = (void *)sdd->ops->request(sdd->tx_dma.dmach, &req, dev, "tx");
 
 	return 1;
 }
 
+static int s3c64xx_spi_prepare_transfer(struct spi_master *spi)
+{
+	struct s3c64xx_spi_driver_data *sdd = spi_master_get_devdata(spi);
+
+	/* Acquire DMA channels */
+	while (!acquire_dma(sdd))
+		usleep_range(10000, 11000);
+
+	pm_runtime_get_sync(&sdd->pdev->dev);
+
+	return 0;
+}
+
+static int s3c64xx_spi_unprepare_transfer(struct spi_master *spi)
+{
+	struct s3c64xx_spi_driver_data *sdd = spi_master_get_devdata(spi);
+
+	/* Free DMA channels */
+	sdd->ops->release((enum dma_ch)sdd->rx_dma.ch, &s3c64xx_spi_dma_client);
+	sdd->ops->release((enum dma_ch)sdd->tx_dma.ch, &s3c64xx_spi_dma_client);
+
+	pm_runtime_put(&sdd->pdev->dev);
+
+	return 0;
+}
+
+static void s3c64xx_spi_dma_stop(struct s3c64xx_spi_driver_data *sdd,
+				 struct s3c64xx_spi_dma_data *dma)
+{
+	sdd->ops->stop((enum dma_ch)dma->ch);
+}
+#else
+
+static void prepare_dma(struct s3c64xx_spi_dma_data *dma,
+					unsigned len, dma_addr_t buf)
+{
+	struct s3c64xx_spi_driver_data *sdd;
+	struct dma_slave_config config;
+	struct scatterlist sg;
+	struct dma_async_tx_descriptor *desc;
+
+	if (dma->direction == DMA_DEV_TO_MEM) {
+		sdd = container_of((void *)dma,
+			struct s3c64xx_spi_driver_data, rx_dma);
+		config.direction = dma->direction;
+		config.src_addr = sdd->sfr_start + S3C64XX_SPI_RX_DATA;
+		config.src_addr_width = sdd->cur_bpw / 8;
+		config.src_maxburst = 1;
+		dmaengine_slave_config(dma->ch, &config);
+	} else {
+		sdd = container_of((void *)dma,
+			struct s3c64xx_spi_driver_data, tx_dma);
+		config.direction = dma->direction;
+		config.dst_addr = sdd->sfr_start + S3C64XX_SPI_TX_DATA;
+		config.dst_addr_width = sdd->cur_bpw / 8;
+		config.dst_maxburst = 1;
+		dmaengine_slave_config(dma->ch, &config);
+	}
+
+	sg_init_table(&sg, 1);
+	sg_dma_len(&sg) = len;
+	sg_set_page(&sg, pfn_to_page(PFN_DOWN(buf)),
+		    len, offset_in_page(buf));
+	sg_dma_address(&sg) = buf;
+
+	desc = dmaengine_prep_slave_sg(dma->ch,
+		&sg, 1, dma->direction, DMA_PREP_INTERRUPT);
+
+	desc->callback = s3c64xx_spi_dmacb;
+	desc->callback_param = dma;
+
+	dmaengine_submit(desc);
+	dma_async_issue_pending(dma->ch);
+}
+
+static int s3c64xx_spi_prepare_transfer(struct spi_master *spi)
+{
+	struct s3c64xx_spi_driver_data *sdd = spi_master_get_devdata(spi);
+	dma_filter_fn filter = sdd->cntrlr_info->filter;
+	struct device *dev = &sdd->pdev->dev;
+	dma_cap_mask_t mask;
+	int ret;
+
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
+
+	/* Acquire DMA channels */
+	sdd->rx_dma.ch = dma_request_slave_channel_compat(mask, filter,
+				(void*)sdd->rx_dma.dmach, dev, "rx");
+	if (!sdd->rx_dma.ch) {
+		dev_err(dev, "Failed to get RX DMA channel\n");
+		ret = -EBUSY;
+		goto out;
+	}
+
+	sdd->tx_dma.ch = dma_request_slave_channel_compat(mask, filter,
+				(void*)sdd->tx_dma.dmach, dev, "tx");
+	if (!sdd->tx_dma.ch) {
+		dev_err(dev, "Failed to get TX DMA channel\n");
+		ret = -EBUSY;
+		goto out_rx;
+	}
+
+	ret = pm_runtime_get_sync(&sdd->pdev->dev);
+	if (ret != 0) {
+		dev_err(dev, "Failed to enable device: %d\n", ret);
+		goto out_tx;
+	}
+
+	return 0;
+
+out_tx:
+	dma_release_channel(sdd->tx_dma.ch);
+out_rx:
+	dma_release_channel(sdd->rx_dma.ch);
+out:
+	return ret;
+}
+
+static int s3c64xx_spi_unprepare_transfer(struct spi_master *spi)
+{
+	struct s3c64xx_spi_driver_data *sdd = spi_master_get_devdata(spi);
+
+	/* Free DMA channels */
+	dma_release_channel(sdd->rx_dma.ch);
+	dma_release_channel(sdd->tx_dma.ch);
+
+	pm_runtime_put(&sdd->pdev->dev);
+	return 0;
+}
+
+static void s3c64xx_spi_dma_stop(struct s3c64xx_spi_driver_data *sdd,
+				 struct s3c64xx_spi_dma_data *dma)
+{
+	dmaengine_terminate_all(dma->ch);
+}
+#endif
+
 static void enable_datapath(struct s3c64xx_spi_driver_data *sdd,
 				struct spi_device *spi,
 				struct spi_transfer *xfer, int dma_mode)
@@ -713,9 +860,9 @@ static int s3c64xx_spi_transfer_one_message(struct spi_master *master,
 		}
 
 		/* Polling method for xfers not bigger than FIFO capacity */
-		if (xfer->len <= ((FIFO_LVL_MASK(sdd) >> 1) + 1))
-			use_dma = 0;
-		else
+		use_dma = 0;
+		if (sdd->rx_dma.ch && sdd->tx_dma.ch &&
+		    (xfer->len > ((FIFO_LVL_MASK(sdd) >> 1) + 1)))
 			use_dma = 1;
 
 		spin_lock_irqsave(&sdd->lock, flags);
@@ -750,10 +897,10 @@ static int s3c64xx_spi_transfer_one_message(struct spi_master *master,
 			if (use_dma) {
 				if (xfer->tx_buf != NULL
 						&& (sdd->state & TXBUSY))
-					sdd->ops->stop(sdd->tx_dma.ch);
+					s3c64xx_spi_dma_stop(sdd, &sdd->tx_dma);
 				if (xfer->rx_buf != NULL
 						&& (sdd->state & RXBUSY))
-					sdd->ops->stop(sdd->rx_dma.ch);
+					s3c64xx_spi_dma_stop(sdd, &sdd->rx_dma);
 			}
 
 			goto out;
@@ -790,34 +937,7 @@ out:
 	return 0;
 }
 
-static int s3c64xx_spi_prepare_transfer(struct spi_master *spi)
-{
-	struct s3c64xx_spi_driver_data *sdd = spi_master_get_devdata(spi);
-
-	/* Acquire DMA channels */
-	while (!acquire_dma(sdd))
-		usleep_range(10000, 11000);
-
-	pm_runtime_get_sync(&sdd->pdev->dev);
-
-	return 0;
-}
-
-static int s3c64xx_spi_unprepare_transfer(struct spi_master *spi)
-{
-	struct s3c64xx_spi_driver_data *sdd = spi_master_get_devdata(spi);
-
-	/* Free DMA channels */
-	sdd->ops->release(sdd->rx_dma.ch, &s3c64xx_spi_dma_client);
-	sdd->ops->release(sdd->tx_dma.ch, &s3c64xx_spi_dma_client);
-
-	pm_runtime_put(&sdd->pdev->dev);
-
-	return 0;
-}
-
 static struct s3c64xx_spi_csinfo *s3c64xx_get_slave_ctrldata(
-				struct s3c64xx_spi_driver_data *sdd,
 				struct spi_device *spi)
 {
 	struct s3c64xx_spi_csinfo *cs;
@@ -874,7 +994,7 @@ static int s3c64xx_spi_setup(struct spi_device *spi)
 
 	sdd = spi_master_get_devdata(spi->master);
 	if (!cs && spi->dev.of_node) {
-		cs = s3c64xx_get_slave_ctrldata(sdd, spi);
+		cs = s3c64xx_get_slave_ctrldata(spi);
 		spi->controller_data = cs;
 	}
 
@@ -912,15 +1032,6 @@ static int s3c64xx_spi_setup(struct spi_device *spi)
 
 	spin_unlock_irqrestore(&sdd->lock, flags);
 
-	if (spi->bits_per_word != 8
-			&& spi->bits_per_word != 16
-			&& spi->bits_per_word != 32) {
-		dev_err(&spi->dev, "setup: %dbits/wrd not supported!\n",
-							spi->bits_per_word);
-		err = -EINVAL;
-		goto setup_exit;
-	}
-
 	pm_runtime_get_sync(&sdd->pdev->dev);
 
 	/* Check if we can provide the requested rate */
@@ -994,25 +1105,30 @@ static irqreturn_t s3c64xx_spi_irq(int irq, void *data)
 {
 	struct s3c64xx_spi_driver_data *sdd = data;
 	struct spi_master *spi = sdd->master;
-	unsigned int val;
-
-	val = readl(sdd->regs + S3C64XX_SPI_PENDING_CLR);
+	unsigned int val, clr = 0;
 
-	val &= S3C64XX_SPI_PND_RX_OVERRUN_CLR |
-		S3C64XX_SPI_PND_RX_UNDERRUN_CLR |
-		S3C64XX_SPI_PND_TX_OVERRUN_CLR |
-		S3C64XX_SPI_PND_TX_UNDERRUN_CLR;
+	val = readl(sdd->regs + S3C64XX_SPI_STATUS);
 
-	writel(val, sdd->regs + S3C64XX_SPI_PENDING_CLR);
-
-	if (val & S3C64XX_SPI_PND_RX_OVERRUN_CLR)
+	if (val & S3C64XX_SPI_ST_RX_OVERRUN_ERR) {
+		clr = S3C64XX_SPI_PND_RX_OVERRUN_CLR;
 		dev_err(&spi->dev, "RX overrun\n");
-	if (val & S3C64XX_SPI_PND_RX_UNDERRUN_CLR)
+	}
+	if (val & S3C64XX_SPI_ST_RX_UNDERRUN_ERR) {
+		clr |= S3C64XX_SPI_PND_RX_UNDERRUN_CLR;
 		dev_err(&spi->dev, "RX underrun\n");
-	if (val & S3C64XX_SPI_PND_TX_OVERRUN_CLR)
+	}
+	if (val & S3C64XX_SPI_ST_TX_OVERRUN_ERR) {
+		clr |= S3C64XX_SPI_PND_TX_OVERRUN_CLR;
 		dev_err(&spi->dev, "TX overrun\n");
-	if (val & S3C64XX_SPI_PND_TX_UNDERRUN_CLR)
+	}
+	if (val & S3C64XX_SPI_ST_TX_UNDERRUN_ERR) {
+		clr |= S3C64XX_SPI_PND_TX_UNDERRUN_CLR;
 		dev_err(&spi->dev, "TX underrun\n");
+	}
+
+	/* Clear the pending irq by setting and then clearing it */
+	writel(clr, sdd->regs + S3C64XX_SPI_PENDING_CLR);
+	writel(0, sdd->regs + S3C64XX_SPI_PENDING_CLR);
 
 	return IRQ_HANDLED;
 }
@@ -1036,9 +1152,13 @@ static void s3c64xx_spi_hwinit(struct s3c64xx_spi_driver_data *sdd, int channel)
 	writel(0, regs + S3C64XX_SPI_MODE_CFG);
 	writel(0, regs + S3C64XX_SPI_PACKET_CNT);
 
-	/* Clear any irq pending bits */
-	writel(readl(regs + S3C64XX_SPI_PENDING_CLR),
-				regs + S3C64XX_SPI_PENDING_CLR);
+	/* Clear any irq pending bits, should set and clear the bits */
+	val = S3C64XX_SPI_PND_RX_OVERRUN_CLR |
+		S3C64XX_SPI_PND_RX_UNDERRUN_CLR |
+		S3C64XX_SPI_PND_TX_OVERRUN_CLR |
+		S3C64XX_SPI_PND_TX_UNDERRUN_CLR;
+	writel(val, regs + S3C64XX_SPI_PENDING_CLR);
+	writel(0, regs + S3C64XX_SPI_PENDING_CLR);
 
 	writel(0, regs + S3C64XX_SPI_SWAP_CFG);
 
@@ -1052,41 +1172,6 @@ static void s3c64xx_spi_hwinit(struct s3c64xx_spi_driver_data *sdd, int channel)
 }
 
 #ifdef CONFIG_OF
-static int s3c64xx_spi_parse_dt_gpio(struct s3c64xx_spi_driver_data *sdd)
-{
-	struct device *dev = &sdd->pdev->dev;
-	int idx, gpio, ret;
-
-	/* find gpios for mosi, miso and clock lines */
-	for (idx = 0; idx < 3; idx++) {
-		gpio = of_get_gpio(dev->of_node, idx);
-		if (!gpio_is_valid(gpio)) {
-			dev_err(dev, "invalid gpio[%d]: %d\n", idx, gpio);
-			goto free_gpio;
-		}
-		sdd->gpios[idx] = gpio;
-		ret = gpio_request(gpio, "spi-bus");
-		if (ret) {
-			dev_err(dev, "gpio [%d] request failed: %d\n",
-				gpio, ret);
-			goto free_gpio;
-		}
-	}
-	return 0;
-
-free_gpio:
-	while (--idx >= 0)
-		gpio_free(sdd->gpios[idx]);
-	return -EINVAL;
-}
-
-static void s3c64xx_spi_dt_gpio_free(struct s3c64xx_spi_driver_data *sdd)
-{
-	unsigned int idx;
-	for (idx = 0; idx < 3; idx++)
-		gpio_free(sdd->gpios[idx]);
-}
-
 static struct s3c64xx_spi_info *s3c64xx_spi_parse_dt(struct device *dev)
 {
 	struct s3c64xx_spi_info *sci;
@@ -1119,15 +1204,6 @@ static struct s3c64xx_spi_info *s3c64xx_spi_parse_dt(struct device *dev)
 {
 	return dev->platform_data;
 }
-
-static int s3c64xx_spi_parse_dt_gpio(struct s3c64xx_spi_driver_data *sdd)
-{
-	return -EINVAL;
-}
-
-static void s3c64xx_spi_dt_gpio_free(struct s3c64xx_spi_driver_data *sdd)
-{
-}
 #endif
 
 static const struct of_device_id s3c64xx_spi_dt_match[];
@@ -1238,6 +1314,7 @@ static int s3c64xx_spi_probe(struct platform_device *pdev)
 	master->unprepare_transfer_hardware = s3c64xx_spi_unprepare_transfer;
 	master->num_chipselect = sci->num_cs;
 	master->dma_alignment = 8;
+	master->bits_per_word_mask = BIT(32 - 1) | BIT(16 - 1) | BIT(8 - 1);
 	/* the spi->mode bits understood by this driver: */
 	master->mode_bits = SPI_CPOL | SPI_CPHA | SPI_CS_HIGH;
 
@@ -1247,10 +1324,7 @@ static int s3c64xx_spi_probe(struct platform_device *pdev)
 		goto err0;
 	}
 
-	if (!sci->cfg_gpio && pdev->dev.of_node) {
-		if (s3c64xx_spi_parse_dt_gpio(sdd))
-			return -EBUSY;
-	} else if (sci->cfg_gpio == NULL || sci->cfg_gpio()) {
+	if (sci->cfg_gpio && sci->cfg_gpio()) {
 		dev_err(&pdev->dev, "Unable to config gpio\n");
 		ret = -EBUSY;
 		goto err0;
@@ -1261,13 +1335,13 @@ static int s3c64xx_spi_probe(struct platform_device *pdev)
 	if (IS_ERR(sdd->clk)) {
 		dev_err(&pdev->dev, "Unable to acquire clock 'spi'\n");
 		ret = PTR_ERR(sdd->clk);
-		goto err1;
+		goto err0;
 	}
 
 	if (clk_prepare_enable(sdd->clk)) {
 		dev_err(&pdev->dev, "Couldn't enable clock 'spi'\n");
 		ret = -EBUSY;
-		goto err1;
+		goto err0;
 	}
 
 	sprintf(clk_name, "spi_busclk%d", sci->src_clk_nr);
@@ -1324,9 +1398,6 @@ err3:
 	clk_disable_unprepare(sdd->src_clk);
 err2:
 	clk_disable_unprepare(sdd->clk);
-err1:
-	if (!sdd->cntrlr_info->cfg_gpio && pdev->dev.of_node)
-		s3c64xx_spi_dt_gpio_free(sdd);
 err0:
 	platform_set_drvdata(pdev, NULL);
 	spi_master_put(master);
@@ -1349,16 +1420,13 @@ static int s3c64xx_spi_remove(struct platform_device *pdev)
 
 	clk_disable_unprepare(sdd->clk);
 
-	if (!sdd->cntrlr_info->cfg_gpio && pdev->dev.of_node)
-		s3c64xx_spi_dt_gpio_free(sdd);
-
 	platform_set_drvdata(pdev, NULL);
 	spi_master_put(master);
 
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int s3c64xx_spi_suspend(struct device *dev)
 {
 	struct spi_master *master = dev_get_drvdata(dev);
@@ -1370,9 +1438,6 @@ static int s3c64xx_spi_suspend(struct device *dev)
 	clk_disable_unprepare(sdd->src_clk);
 	clk_disable_unprepare(sdd->clk);
 
-	if (!sdd->cntrlr_info->cfg_gpio && dev->of_node)
-		s3c64xx_spi_dt_gpio_free(sdd);
-
 	sdd->cur_speed = 0; /* Output Clock is stopped */
 
 	return 0;
@@ -1384,9 +1449,7 @@ static int s3c64xx_spi_resume(struct device *dev)
 	struct s3c64xx_spi_driver_data *sdd = spi_master_get_devdata(master);
 	struct s3c64xx_spi_info *sci = sdd->cntrlr_info;
 
-	if (!sci->cfg_gpio && dev->of_node)
-		s3c64xx_spi_parse_dt_gpio(sdd);
-	else
+	if (sci->cfg_gpio)
 		sci->cfg_gpio();
 
 	/* Enable the clock */
@@ -1399,7 +1462,7 @@ static int s3c64xx_spi_resume(struct device *dev)
 
 	return 0;
 }
-#endif /* CONFIG_PM */
+#endif /* CONFIG_PM_SLEEP */
 
 #ifdef CONFIG_PM_RUNTIME
 static int s3c64xx_spi_runtime_suspend(struct device *dev)
diff --git a/drivers/spi/spi-sh-msiof.c b/drivers/spi/spi-sh-msiof.c
index 8b40d0884f8..2bc5a6b8630 100644
--- a/drivers/spi/spi-sh-msiof.c
+++ b/drivers/spi/spi-sh-msiof.c
@@ -764,8 +764,6 @@ static const struct of_device_id sh_msiof_match[] = {
 	{},
 };
 MODULE_DEVICE_TABLE(of, sh_msiof_match);
-#else
-#define sh_msiof_match NULL
 #endif
 
 static struct dev_pm_ops sh_msiof_spi_dev_pm_ops = {
@@ -780,7 +778,7 @@ static struct platform_driver sh_msiof_spi_drv = {
 		.name		= "spi_sh_msiof",
 		.owner		= THIS_MODULE,
 		.pm		= &sh_msiof_spi_dev_pm_ops,
-		.of_match_table = sh_msiof_match,
+		.of_match_table = of_match_ptr(sh_msiof_match),
 	},
 };
 module_platform_driver(sh_msiof_spi_drv);
diff --git a/drivers/spi/spi-sirf.c b/drivers/spi/spi-sirf.c
index f59d4177b41..0808cd56bf8 100644
--- a/drivers/spi/spi-sirf.c
+++ b/drivers/spi/spi-sirf.c
@@ -660,7 +660,7 @@ static const struct of_device_id spi_sirfsoc_of_match[] = {
 	{ .compatible = "sirf,marco-spi", },
 	{}
 };
-MODULE_DEVICE_TABLE(of, sirfsoc_spi_of_match);
+MODULE_DEVICE_TABLE(of, spi_sirfsoc_of_match);
 
 static struct platform_driver spi_sirfsoc_driver = {
 	.driver = {
diff --git a/drivers/spi/spi-tegra114.c b/drivers/spi/spi-tegra114.c
new file mode 100644
index 00000000000..598eb45e800
--- /dev/null
+++ b/drivers/spi/spi-tegra114.c
@@ -0,0 +1,1246 @@
+/*
+ * SPI driver for NVIDIA's Tegra114 SPI Controller.
+ *
+ * Copyright (c) 2013, NVIDIA 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 it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/clk.h>
+#include <linux/clk/tegra.h>
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/dmaengine.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmapool.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/kthread.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/spi/spi.h>
+
+#define SPI_COMMAND1				0x000
+#define SPI_BIT_LENGTH(x)			(((x) & 0x1f) << 0)
+#define SPI_PACKED				(1 << 5)
+#define SPI_TX_EN				(1 << 11)
+#define SPI_RX_EN				(1 << 12)
+#define SPI_BOTH_EN_BYTE			(1 << 13)
+#define SPI_BOTH_EN_BIT				(1 << 14)
+#define SPI_LSBYTE_FE				(1 << 15)
+#define SPI_LSBIT_FE				(1 << 16)
+#define SPI_BIDIROE				(1 << 17)
+#define SPI_IDLE_SDA_DRIVE_LOW			(0 << 18)
+#define SPI_IDLE_SDA_DRIVE_HIGH			(1 << 18)
+#define SPI_IDLE_SDA_PULL_LOW			(2 << 18)
+#define SPI_IDLE_SDA_PULL_HIGH			(3 << 18)
+#define SPI_IDLE_SDA_MASK			(3 << 18)
+#define SPI_CS_SS_VAL				(1 << 20)
+#define SPI_CS_SW_HW				(1 << 21)
+/* SPI_CS_POL_INACTIVE bits are default high */
+#define SPI_CS_POL_INACTIVE			22
+#define SPI_CS_POL_INACTIVE_0			(1 << 22)
+#define SPI_CS_POL_INACTIVE_1			(1 << 23)
+#define SPI_CS_POL_INACTIVE_2			(1 << 24)
+#define SPI_CS_POL_INACTIVE_3			(1 << 25)
+#define SPI_CS_POL_INACTIVE_MASK		(0xF << 22)
+
+#define SPI_CS_SEL_0				(0 << 26)
+#define SPI_CS_SEL_1				(1 << 26)
+#define SPI_CS_SEL_2				(2 << 26)
+#define SPI_CS_SEL_3				(3 << 26)
+#define SPI_CS_SEL_MASK				(3 << 26)
+#define SPI_CS_SEL(x)				(((x) & 0x3) << 26)
+#define SPI_CONTROL_MODE_0			(0 << 28)
+#define SPI_CONTROL_MODE_1			(1 << 28)
+#define SPI_CONTROL_MODE_2			(2 << 28)
+#define SPI_CONTROL_MODE_3			(3 << 28)
+#define SPI_CONTROL_MODE_MASK			(3 << 28)
+#define SPI_MODE_SEL(x)				(((x) & 0x3) << 28)
+#define SPI_M_S					(1 << 30)
+#define SPI_PIO					(1 << 31)
+
+#define SPI_COMMAND2				0x004
+#define SPI_TX_TAP_DELAY(x)			(((x) & 0x3F) << 6)
+#define SPI_RX_TAP_DELAY(x)			(((x) & 0x3F) << 0)
+
+#define SPI_CS_TIMING1				0x008
+#define SPI_SETUP_HOLD(setup, hold)		(((setup) << 4) | (hold))
+#define SPI_CS_SETUP_HOLD(reg, cs, val)			\
+		((((val) & 0xFFu) << ((cs) * 8)) |	\
+		((reg) & ~(0xFFu << ((cs) * 8))))
+
+#define SPI_CS_TIMING2				0x00C
+#define CYCLES_BETWEEN_PACKETS_0(x)		(((x) & 0x1F) << 0)
+#define CS_ACTIVE_BETWEEN_PACKETS_0		(1 << 5)
+#define CYCLES_BETWEEN_PACKETS_1(x)		(((x) & 0x1F) << 8)
+#define CS_ACTIVE_BETWEEN_PACKETS_1		(1 << 13)
+#define CYCLES_BETWEEN_PACKETS_2(x)		(((x) & 0x1F) << 16)
+#define CS_ACTIVE_BETWEEN_PACKETS_2		(1 << 21)
+#define CYCLES_BETWEEN_PACKETS_3(x)		(((x) & 0x1F) << 24)
+#define CS_ACTIVE_BETWEEN_PACKETS_3		(1 << 29)
+#define SPI_SET_CS_ACTIVE_BETWEEN_PACKETS(reg, cs, val)		\
+		(reg = (((val) & 0x1) << ((cs) * 8 + 5)) |	\
+			((reg) & ~(1 << ((cs) * 8 + 5))))
+#define SPI_SET_CYCLES_BETWEEN_PACKETS(reg, cs, val)		\
+		(reg = (((val) & 0xF) << ((cs) * 8)) |		\
+			((reg) & ~(0xF << ((cs) * 8))))
+
+#define SPI_TRANS_STATUS			0x010
+#define SPI_BLK_CNT(val)			(((val) >> 0) & 0xFFFF)
+#define SPI_SLV_IDLE_COUNT(val)			(((val) >> 16) & 0xFF)
+#define SPI_RDY					(1 << 30)
+
+#define SPI_FIFO_STATUS				0x014
+#define SPI_RX_FIFO_EMPTY			(1 << 0)
+#define SPI_RX_FIFO_FULL			(1 << 1)
+#define SPI_TX_FIFO_EMPTY			(1 << 2)
+#define SPI_TX_FIFO_FULL			(1 << 3)
+#define SPI_RX_FIFO_UNF				(1 << 4)
+#define SPI_RX_FIFO_OVF				(1 << 5)
+#define SPI_TX_FIFO_UNF				(1 << 6)
+#define SPI_TX_FIFO_OVF				(1 << 7)
+#define SPI_ERR					(1 << 8)
+#define SPI_TX_FIFO_FLUSH			(1 << 14)
+#define SPI_RX_FIFO_FLUSH			(1 << 15)
+#define SPI_TX_FIFO_EMPTY_COUNT(val)		(((val) >> 16) & 0x7F)
+#define SPI_RX_FIFO_FULL_COUNT(val)		(((val) >> 23) & 0x7F)
+#define SPI_FRAME_END				(1 << 30)
+#define SPI_CS_INACTIVE				(1 << 31)
+
+#define SPI_FIFO_ERROR				(SPI_RX_FIFO_UNF | \
+			SPI_RX_FIFO_OVF | SPI_TX_FIFO_UNF | SPI_TX_FIFO_OVF)
+#define SPI_FIFO_EMPTY			(SPI_RX_FIFO_EMPTY | SPI_TX_FIFO_EMPTY)
+
+#define SPI_TX_DATA				0x018
+#define SPI_RX_DATA				0x01C
+
+#define SPI_DMA_CTL				0x020
+#define SPI_TX_TRIG_1				(0 << 15)
+#define SPI_TX_TRIG_4				(1 << 15)
+#define SPI_TX_TRIG_8				(2 << 15)
+#define SPI_TX_TRIG_16				(3 << 15)
+#define SPI_TX_TRIG_MASK			(3 << 15)
+#define SPI_RX_TRIG_1				(0 << 19)
+#define SPI_RX_TRIG_4				(1 << 19)
+#define SPI_RX_TRIG_8				(2 << 19)
+#define SPI_RX_TRIG_16				(3 << 19)
+#define SPI_RX_TRIG_MASK			(3 << 19)
+#define SPI_IE_TX				(1 << 28)
+#define SPI_IE_RX				(1 << 29)
+#define SPI_CONT				(1 << 30)
+#define SPI_DMA					(1 << 31)
+#define SPI_DMA_EN				SPI_DMA
+
+#define SPI_DMA_BLK				0x024
+#define SPI_DMA_BLK_SET(x)			(((x) & 0xFFFF) << 0)
+
+#define SPI_TX_FIFO				0x108
+#define SPI_RX_FIFO				0x188
+#define MAX_CHIP_SELECT				4
+#define SPI_FIFO_DEPTH				64
+#define DATA_DIR_TX				(1 << 0)
+#define DATA_DIR_RX				(1 << 1)
+
+#define SPI_DMA_TIMEOUT				(msecs_to_jiffies(1000))
+#define DEFAULT_SPI_DMA_BUF_LEN			(16*1024)
+#define TX_FIFO_EMPTY_COUNT_MAX			SPI_TX_FIFO_EMPTY_COUNT(0x40)
+#define RX_FIFO_FULL_COUNT_ZERO			SPI_RX_FIFO_FULL_COUNT(0)
+#define MAX_HOLD_CYCLES				16
+#define SPI_DEFAULT_SPEED			25000000
+
+#define MAX_CHIP_SELECT				4
+#define SPI_FIFO_DEPTH				64
+
+struct tegra_spi_data {
+	struct device				*dev;
+	struct spi_master			*master;
+	spinlock_t				lock;
+
+	struct clk				*clk;
+	void __iomem				*base;
+	phys_addr_t				phys;
+	unsigned				irq;
+	int					dma_req_sel;
+	u32					spi_max_frequency;
+	u32					cur_speed;
+
+	struct spi_device			*cur_spi;
+	unsigned				cur_pos;
+	unsigned				cur_len;
+	unsigned				words_per_32bit;
+	unsigned				bytes_per_word;
+	unsigned				curr_dma_words;
+	unsigned				cur_direction;
+
+	unsigned				cur_rx_pos;
+	unsigned				cur_tx_pos;
+
+	unsigned				dma_buf_size;
+	unsigned				max_buf_size;
+	bool					is_curr_dma_xfer;
+
+	struct completion			rx_dma_complete;
+	struct completion			tx_dma_complete;
+
+	u32					tx_status;
+	u32					rx_status;
+	u32					status_reg;
+	bool					is_packed;
+	unsigned long				packed_size;
+
+	u32					command1_reg;
+	u32					dma_control_reg;
+	u32					def_command1_reg;
+	u32					spi_cs_timing;
+
+	struct completion			xfer_completion;
+	struct spi_transfer			*curr_xfer;
+	struct dma_chan				*rx_dma_chan;
+	u32					*rx_dma_buf;
+	dma_addr_t				rx_dma_phys;
+	struct dma_async_tx_descriptor		*rx_dma_desc;
+
+	struct dma_chan				*tx_dma_chan;
+	u32					*tx_dma_buf;
+	dma_addr_t				tx_dma_phys;
+	struct dma_async_tx_descriptor		*tx_dma_desc;
+};
+
+static int tegra_spi_runtime_suspend(struct device *dev);
+static int tegra_spi_runtime_resume(struct device *dev);
+
+static inline unsigned long tegra_spi_readl(struct tegra_spi_data *tspi,
+		unsigned long reg)
+{
+	return readl(tspi->base + reg);
+}
+
+static inline void tegra_spi_writel(struct tegra_spi_data *tspi,
+		unsigned long val, unsigned long reg)
+{
+	writel(val, tspi->base + reg);
+
+	/* Read back register to make sure that register writes completed */
+	if (reg != SPI_TX_FIFO)
+		readl(tspi->base + SPI_COMMAND1);
+}
+
+static void tegra_spi_clear_status(struct tegra_spi_data *tspi)
+{
+	unsigned long val;
+
+	/* Write 1 to clear status register */
+	val = tegra_spi_readl(tspi, SPI_TRANS_STATUS);
+	tegra_spi_writel(tspi, val, SPI_TRANS_STATUS);
+
+	/* Clear fifo status error if any */
+	val = tegra_spi_readl(tspi, SPI_FIFO_STATUS);
+	if (val & SPI_ERR)
+		tegra_spi_writel(tspi, SPI_ERR | SPI_FIFO_ERROR,
+				SPI_FIFO_STATUS);
+}
+
+static unsigned tegra_spi_calculate_curr_xfer_param(
+	struct spi_device *spi, struct tegra_spi_data *tspi,
+	struct spi_transfer *t)
+{
+	unsigned remain_len = t->len - tspi->cur_pos;
+	unsigned max_word;
+	unsigned bits_per_word = t->bits_per_word;
+	unsigned max_len;
+	unsigned total_fifo_words;
+
+	tspi->bytes_per_word = (bits_per_word - 1) / 8 + 1;
+
+	if (bits_per_word == 8 || bits_per_word == 16) {
+		tspi->is_packed = 1;
+		tspi->words_per_32bit = 32/bits_per_word;
+	} else {
+		tspi->is_packed = 0;
+		tspi->words_per_32bit = 1;
+	}
+
+	if (tspi->is_packed) {
+		max_len = min(remain_len, tspi->max_buf_size);
+		tspi->curr_dma_words = max_len/tspi->bytes_per_word;
+		total_fifo_words = (max_len + 3) / 4;
+	} else {
+		max_word = (remain_len - 1) / tspi->bytes_per_word + 1;
+		max_word = min(max_word, tspi->max_buf_size/4);
+		tspi->curr_dma_words = max_word;
+		total_fifo_words = max_word;
+	}
+	return total_fifo_words;
+}
+
+static unsigned tegra_spi_fill_tx_fifo_from_client_txbuf(
+	struct tegra_spi_data *tspi, struct spi_transfer *t)
+{
+	unsigned nbytes;
+	unsigned tx_empty_count;
+	unsigned long fifo_status;
+	unsigned max_n_32bit;
+	unsigned i, count;
+	unsigned long x;
+	unsigned int written_words;
+	unsigned fifo_words_left;
+	u8 *tx_buf = (u8 *)t->tx_buf + tspi->cur_tx_pos;
+
+	fifo_status = tegra_spi_readl(tspi, SPI_FIFO_STATUS);
+	tx_empty_count = SPI_TX_FIFO_EMPTY_COUNT(fifo_status);
+
+	if (tspi->is_packed) {
+		fifo_words_left = tx_empty_count * tspi->words_per_32bit;
+		written_words = min(fifo_words_left, tspi->curr_dma_words);
+		nbytes = written_words * tspi->bytes_per_word;
+		max_n_32bit = DIV_ROUND_UP(nbytes, 4);
+		for (count = 0; count < max_n_32bit; count++) {
+			x = 0;
+			for (i = 0; (i < 4) && nbytes; i++, nbytes--)
+				x |= (*tx_buf++) << (i*8);
+			tegra_spi_writel(tspi, x, SPI_TX_FIFO);
+		}
+	} else {
+		max_n_32bit = min(tspi->curr_dma_words,  tx_empty_count);
+		written_words = max_n_32bit;
+		nbytes = written_words * tspi->bytes_per_word;
+		for (count = 0; count < max_n_32bit; count++) {
+			x = 0;
+			for (i = 0; nbytes && (i < tspi->bytes_per_word);
+							i++, nbytes--)
+				x |= ((*tx_buf++) << i*8);
+			tegra_spi_writel(tspi, x, SPI_TX_FIFO);
+		}
+	}
+	tspi->cur_tx_pos += written_words * tspi->bytes_per_word;
+	return written_words;
+}
+
+static unsigned int tegra_spi_read_rx_fifo_to_client_rxbuf(
+		struct tegra_spi_data *tspi, struct spi_transfer *t)
+{
+	unsigned rx_full_count;
+	unsigned long fifo_status;
+	unsigned i, count;
+	unsigned long x;
+	unsigned int read_words = 0;
+	unsigned len;
+	u8 *rx_buf = (u8 *)t->rx_buf + tspi->cur_rx_pos;
+
+	fifo_status = tegra_spi_readl(tspi, SPI_FIFO_STATUS);
+	rx_full_count = SPI_RX_FIFO_FULL_COUNT(fifo_status);
+	if (tspi->is_packed) {
+		len = tspi->curr_dma_words * tspi->bytes_per_word;
+		for (count = 0; count < rx_full_count; count++) {
+			x = tegra_spi_readl(tspi, SPI_RX_FIFO);
+			for (i = 0; len && (i < 4); i++, len--)
+				*rx_buf++ = (x >> i*8) & 0xFF;
+		}
+		tspi->cur_rx_pos += tspi->curr_dma_words * tspi->bytes_per_word;
+		read_words += tspi->curr_dma_words;
+	} else {
+		unsigned int rx_mask;
+		unsigned int bits_per_word = t->bits_per_word;
+
+		rx_mask = (1 << bits_per_word) - 1;
+		for (count = 0; count < rx_full_count; count++) {
+			x = tegra_spi_readl(tspi, SPI_RX_FIFO);
+			x &= rx_mask;
+			for (i = 0; (i < tspi->bytes_per_word); i++)
+				*rx_buf++ = (x >> (i*8)) & 0xFF;
+		}
+		tspi->cur_rx_pos += rx_full_count * tspi->bytes_per_word;
+		read_words += rx_full_count;
+	}
+	return read_words;
+}
+
+static void tegra_spi_copy_client_txbuf_to_spi_txbuf(
+		struct tegra_spi_data *tspi, struct spi_transfer *t)
+{
+	unsigned len;
+
+	/* Make the dma buffer to read by cpu */
+	dma_sync_single_for_cpu(tspi->dev, tspi->tx_dma_phys,
+				tspi->dma_buf_size, DMA_TO_DEVICE);
+
+	if (tspi->is_packed) {
+		len = tspi->curr_dma_words * tspi->bytes_per_word;
+		memcpy(tspi->tx_dma_buf, t->tx_buf + tspi->cur_pos, len);
+	} else {
+		unsigned int i;
+		unsigned int count;
+		u8 *tx_buf = (u8 *)t->tx_buf + tspi->cur_tx_pos;
+		unsigned consume = tspi->curr_dma_words * tspi->bytes_per_word;
+		unsigned int x;
+
+		for (count = 0; count < tspi->curr_dma_words; count++) {
+			x = 0;
+			for (i = 0; consume && (i < tspi->bytes_per_word);
+							i++, consume--)
+				x |= ((*tx_buf++) << i * 8);
+			tspi->tx_dma_buf[count] = x;
+		}
+	}
+	tspi->cur_tx_pos += tspi->curr_dma_words * tspi->bytes_per_word;
+
+	/* Make the dma buffer to read by dma */
+	dma_sync_single_for_device(tspi->dev, tspi->tx_dma_phys,
+				tspi->dma_buf_size, DMA_TO_DEVICE);
+}
+
+static void tegra_spi_copy_spi_rxbuf_to_client_rxbuf(
+		struct tegra_spi_data *tspi, struct spi_transfer *t)
+{
+	unsigned len;
+
+	/* Make the dma buffer to read by cpu */
+	dma_sync_single_for_cpu(tspi->dev, tspi->rx_dma_phys,
+		tspi->dma_buf_size, DMA_FROM_DEVICE);
+
+	if (tspi->is_packed) {
+		len = tspi->curr_dma_words * tspi->bytes_per_word;
+		memcpy(t->rx_buf + tspi->cur_rx_pos, tspi->rx_dma_buf, len);
+	} else {
+		unsigned int i;
+		unsigned int count;
+		unsigned char *rx_buf = t->rx_buf + tspi->cur_rx_pos;
+		unsigned int x;
+		unsigned int rx_mask;
+		unsigned int bits_per_word = t->bits_per_word;
+
+		rx_mask = (1 << bits_per_word) - 1;
+		for (count = 0; count < tspi->curr_dma_words; count++) {
+			x = tspi->rx_dma_buf[count];
+			x &= rx_mask;
+			for (i = 0; (i < tspi->bytes_per_word); i++)
+				*rx_buf++ = (x >> (i*8)) & 0xFF;
+		}
+	}
+	tspi->cur_rx_pos += tspi->curr_dma_words * tspi->bytes_per_word;
+
+	/* Make the dma buffer to read by dma */
+	dma_sync_single_for_device(tspi->dev, tspi->rx_dma_phys,
+		tspi->dma_buf_size, DMA_FROM_DEVICE);
+}
+
+static void tegra_spi_dma_complete(void *args)
+{
+	struct completion *dma_complete = args;
+
+	complete(dma_complete);
+}
+
+static int tegra_spi_start_tx_dma(struct tegra_spi_data *tspi, int len)
+{
+	INIT_COMPLETION(tspi->tx_dma_complete);
+	tspi->tx_dma_desc = dmaengine_prep_slave_single(tspi->tx_dma_chan,
+				tspi->tx_dma_phys, len, DMA_MEM_TO_DEV,
+				DMA_PREP_INTERRUPT |  DMA_CTRL_ACK);
+	if (!tspi->tx_dma_desc) {
+		dev_err(tspi->dev, "Not able to get desc for Tx\n");
+		return -EIO;
+	}
+
+	tspi->tx_dma_desc->callback = tegra_spi_dma_complete;
+	tspi->tx_dma_desc->callback_param = &tspi->tx_dma_complete;
+
+	dmaengine_submit(tspi->tx_dma_desc);
+	dma_async_issue_pending(tspi->tx_dma_chan);
+	return 0;
+}
+
+static int tegra_spi_start_rx_dma(struct tegra_spi_data *tspi, int len)
+{
+	INIT_COMPLETION(tspi->rx_dma_complete);
+	tspi->rx_dma_desc = dmaengine_prep_slave_single(tspi->rx_dma_chan,
+				tspi->rx_dma_phys, len, DMA_DEV_TO_MEM,
+				DMA_PREP_INTERRUPT |  DMA_CTRL_ACK);
+	if (!tspi->rx_dma_desc) {
+		dev_err(tspi->dev, "Not able to get desc for Rx\n");
+		return -EIO;
+	}
+
+	tspi->rx_dma_desc->callback = tegra_spi_dma_complete;
+	tspi->rx_dma_desc->callback_param = &tspi->rx_dma_complete;
+
+	dmaengine_submit(tspi->rx_dma_desc);
+	dma_async_issue_pending(tspi->rx_dma_chan);
+	return 0;
+}
+
+static int tegra_spi_start_dma_based_transfer(
+		struct tegra_spi_data *tspi, struct spi_transfer *t)
+{
+	unsigned long val;
+	unsigned int len;
+	int ret = 0;
+	unsigned long status;
+
+	/* Make sure that Rx and Tx fifo are empty */
+	status = tegra_spi_readl(tspi, SPI_FIFO_STATUS);
+	if ((status & SPI_FIFO_EMPTY) != SPI_FIFO_EMPTY) {
+		dev_err(tspi->dev,
+			"Rx/Tx fifo are not empty status 0x%08lx\n", status);
+		return -EIO;
+	}
+
+	val = SPI_DMA_BLK_SET(tspi->curr_dma_words - 1);
+	tegra_spi_writel(tspi, val, SPI_DMA_BLK);
+
+	if (tspi->is_packed)
+		len = DIV_ROUND_UP(tspi->curr_dma_words * tspi->bytes_per_word,
+					4) * 4;
+	else
+		len = tspi->curr_dma_words * 4;
+
+	/* Set attention level based on length of transfer */
+	if (len & 0xF)
+		val |= SPI_TX_TRIG_1 | SPI_RX_TRIG_1;
+	else if (((len) >> 4) & 0x1)
+		val |= SPI_TX_TRIG_4 | SPI_RX_TRIG_4;
+	else
+		val |= SPI_TX_TRIG_8 | SPI_RX_TRIG_8;
+
+	if (tspi->cur_direction & DATA_DIR_TX)
+		val |= SPI_IE_TX;
+
+	if (tspi->cur_direction & DATA_DIR_RX)
+		val |= SPI_IE_RX;
+
+	tegra_spi_writel(tspi, val, SPI_DMA_CTL);
+	tspi->dma_control_reg = val;
+
+	if (tspi->cur_direction & DATA_DIR_TX) {
+		tegra_spi_copy_client_txbuf_to_spi_txbuf(tspi, t);
+		ret = tegra_spi_start_tx_dma(tspi, len);
+		if (ret < 0) {
+			dev_err(tspi->dev,
+				"Starting tx dma failed, err %d\n", ret);
+			return ret;
+		}
+	}
+
+	if (tspi->cur_direction & DATA_DIR_RX) {
+		/* Make the dma buffer to read by dma */
+		dma_sync_single_for_device(tspi->dev, tspi->rx_dma_phys,
+				tspi->dma_buf_size, DMA_FROM_DEVICE);
+
+		ret = tegra_spi_start_rx_dma(tspi, len);
+		if (ret < 0) {
+			dev_err(tspi->dev,
+				"Starting rx dma failed, err %d\n", ret);
+			if (tspi->cur_direction & DATA_DIR_TX)
+				dmaengine_terminate_all(tspi->tx_dma_chan);
+			return ret;
+		}
+	}
+	tspi->is_curr_dma_xfer = true;
+	tspi->dma_control_reg = val;
+
+	val |= SPI_DMA_EN;
+	tegra_spi_writel(tspi, val, SPI_DMA_CTL);
+	return ret;
+}
+
+static int tegra_spi_start_cpu_based_transfer(
+		struct tegra_spi_data *tspi, struct spi_transfer *t)
+{
+	unsigned long val;
+	unsigned cur_words;
+
+	if (tspi->cur_direction & DATA_DIR_TX)
+		cur_words = tegra_spi_fill_tx_fifo_from_client_txbuf(tspi, t);
+	else
+		cur_words = tspi->curr_dma_words;
+
+	val = SPI_DMA_BLK_SET(cur_words - 1);
+	tegra_spi_writel(tspi, val, SPI_DMA_BLK);
+
+	val = 0;
+	if (tspi->cur_direction & DATA_DIR_TX)
+		val |= SPI_IE_TX;
+
+	if (tspi->cur_direction & DATA_DIR_RX)
+		val |= SPI_IE_RX;
+
+	tegra_spi_writel(tspi, val, SPI_DMA_CTL);
+	tspi->dma_control_reg = val;
+
+	tspi->is_curr_dma_xfer = false;
+
+	val |= SPI_DMA_EN;
+	tegra_spi_writel(tspi, val, SPI_DMA_CTL);
+	return 0;
+}
+
+static int tegra_spi_init_dma_param(struct tegra_spi_data *tspi,
+			bool dma_to_memory)
+{
+	struct dma_chan *dma_chan;
+	u32 *dma_buf;
+	dma_addr_t dma_phys;
+	int ret;
+	struct dma_slave_config dma_sconfig;
+	dma_cap_mask_t mask;
+
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
+	dma_chan = dma_request_channel(mask, NULL, NULL);
+	if (!dma_chan) {
+		dev_err(tspi->dev,
+			"Dma channel is not available, will try later\n");
+		return -EPROBE_DEFER;
+	}
+
+	dma_buf = dma_alloc_coherent(tspi->dev, tspi->dma_buf_size,
+				&dma_phys, GFP_KERNEL);
+	if (!dma_buf) {
+		dev_err(tspi->dev, " Not able to allocate the dma buffer\n");
+		dma_release_channel(dma_chan);
+		return -ENOMEM;
+	}
+
+	dma_sconfig.slave_id = tspi->dma_req_sel;
+	if (dma_to_memory) {
+		dma_sconfig.src_addr = tspi->phys + SPI_RX_FIFO;
+		dma_sconfig.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+		dma_sconfig.src_maxburst = 0;
+	} else {
+		dma_sconfig.dst_addr = tspi->phys + SPI_TX_FIFO;
+		dma_sconfig.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+		dma_sconfig.dst_maxburst = 0;
+	}
+
+	ret = dmaengine_slave_config(dma_chan, &dma_sconfig);
+	if (ret)
+		goto scrub;
+	if (dma_to_memory) {
+		tspi->rx_dma_chan = dma_chan;
+		tspi->rx_dma_buf = dma_buf;
+		tspi->rx_dma_phys = dma_phys;
+	} else {
+		tspi->tx_dma_chan = dma_chan;
+		tspi->tx_dma_buf = dma_buf;
+		tspi->tx_dma_phys = dma_phys;
+	}
+	return 0;
+
+scrub:
+	dma_free_coherent(tspi->dev, tspi->dma_buf_size, dma_buf, dma_phys);
+	dma_release_channel(dma_chan);
+	return ret;
+}
+
+static void tegra_spi_deinit_dma_param(struct tegra_spi_data *tspi,
+	bool dma_to_memory)
+{
+	u32 *dma_buf;
+	dma_addr_t dma_phys;
+	struct dma_chan *dma_chan;
+
+	if (dma_to_memory) {
+		dma_buf = tspi->rx_dma_buf;
+		dma_chan = tspi->rx_dma_chan;
+		dma_phys = tspi->rx_dma_phys;
+		tspi->rx_dma_chan = NULL;
+		tspi->rx_dma_buf = NULL;
+	} else {
+		dma_buf = tspi->tx_dma_buf;
+		dma_chan = tspi->tx_dma_chan;
+		dma_phys = tspi->tx_dma_phys;
+		tspi->tx_dma_buf = NULL;
+		tspi->tx_dma_chan = NULL;
+	}
+	if (!dma_chan)
+		return;
+
+	dma_free_coherent(tspi->dev, tspi->dma_buf_size, dma_buf, dma_phys);
+	dma_release_channel(dma_chan);
+}
+
+static int tegra_spi_start_transfer_one(struct spi_device *spi,
+		struct spi_transfer *t, bool is_first_of_msg,
+		bool is_single_xfer)
+{
+	struct tegra_spi_data *tspi = spi_master_get_devdata(spi->master);
+	u32 speed = t->speed_hz;
+	u8 bits_per_word = t->bits_per_word;
+	unsigned total_fifo_words;
+	int ret;
+	unsigned long command1;
+	int req_mode;
+
+	if (speed != tspi->cur_speed) {
+		clk_set_rate(tspi->clk, speed);
+		tspi->cur_speed = speed;
+	}
+
+	tspi->cur_spi = spi;
+	tspi->cur_pos = 0;
+	tspi->cur_rx_pos = 0;
+	tspi->cur_tx_pos = 0;
+	tspi->curr_xfer = t;
+	total_fifo_words = tegra_spi_calculate_curr_xfer_param(spi, tspi, t);
+
+	if (is_first_of_msg) {
+		tegra_spi_clear_status(tspi);
+
+		command1 = tspi->def_command1_reg;
+		command1 |= SPI_BIT_LENGTH(bits_per_word - 1);
+
+		command1 &= ~SPI_CONTROL_MODE_MASK;
+		req_mode = spi->mode & 0x3;
+		if (req_mode == SPI_MODE_0)
+			command1 |= SPI_CONTROL_MODE_0;
+		else if (req_mode == SPI_MODE_1)
+			command1 |= SPI_CONTROL_MODE_1;
+		else if (req_mode == SPI_MODE_2)
+			command1 |= SPI_CONTROL_MODE_2;
+		else if (req_mode == SPI_MODE_3)
+			command1 |= SPI_CONTROL_MODE_3;
+
+		tegra_spi_writel(tspi, command1, SPI_COMMAND1);
+
+		command1 |= SPI_CS_SW_HW;
+		if (spi->mode & SPI_CS_HIGH)
+			command1 |= SPI_CS_SS_VAL;
+		else
+			command1 &= ~SPI_CS_SS_VAL;
+
+		tegra_spi_writel(tspi, 0, SPI_COMMAND2);
+	} else {
+		command1 = tspi->command1_reg;
+		command1 &= ~SPI_BIT_LENGTH(~0);
+		command1 |= SPI_BIT_LENGTH(bits_per_word - 1);
+	}
+
+	if (tspi->is_packed)
+		command1 |= SPI_PACKED;
+
+	command1 &= ~(SPI_CS_SEL_MASK | SPI_TX_EN | SPI_RX_EN);
+	tspi->cur_direction = 0;
+	if (t->rx_buf) {
+		command1 |= SPI_RX_EN;
+		tspi->cur_direction |= DATA_DIR_RX;
+	}
+	if (t->tx_buf) {
+		command1 |= SPI_TX_EN;
+		tspi->cur_direction |= DATA_DIR_TX;
+	}
+	command1 |= SPI_CS_SEL(spi->chip_select);
+	tegra_spi_writel(tspi, command1, SPI_COMMAND1);
+	tspi->command1_reg = command1;
+
+	dev_dbg(tspi->dev, "The def 0x%x and written 0x%lx\n",
+				tspi->def_command1_reg, command1);
+
+	if (total_fifo_words > SPI_FIFO_DEPTH)
+		ret = tegra_spi_start_dma_based_transfer(tspi, t);
+	else
+		ret = tegra_spi_start_cpu_based_transfer(tspi, t);
+	return ret;
+}
+
+static int tegra_spi_setup(struct spi_device *spi)
+{
+	struct tegra_spi_data *tspi = spi_master_get_devdata(spi->master);
+	unsigned long val;
+	unsigned long flags;
+	int ret;
+	unsigned int cs_pol_bit[MAX_CHIP_SELECT] = {
+			SPI_CS_POL_INACTIVE_0,
+			SPI_CS_POL_INACTIVE_1,
+			SPI_CS_POL_INACTIVE_2,
+			SPI_CS_POL_INACTIVE_3,
+	};
+
+	dev_dbg(&spi->dev, "setup %d bpw, %scpol, %scpha, %dHz\n",
+		spi->bits_per_word,
+		spi->mode & SPI_CPOL ? "" : "~",
+		spi->mode & SPI_CPHA ? "" : "~",
+		spi->max_speed_hz);
+
+	BUG_ON(spi->chip_select >= MAX_CHIP_SELECT);
+
+	/* Set speed to the spi max fequency if spi device has not set */
+	spi->max_speed_hz = spi->max_speed_hz ? : tspi->spi_max_frequency;
+
+	ret = pm_runtime_get_sync(tspi->dev);
+	if (ret < 0) {
+		dev_err(tspi->dev, "pm runtime failed, e = %d\n", ret);
+		return ret;
+	}
+
+	spin_lock_irqsave(&tspi->lock, flags);
+	val = tspi->def_command1_reg;
+	if (spi->mode & SPI_CS_HIGH)
+		val &= ~cs_pol_bit[spi->chip_select];
+	else
+		val |= cs_pol_bit[spi->chip_select];
+	tspi->def_command1_reg = val;
+	tegra_spi_writel(tspi, tspi->def_command1_reg, SPI_COMMAND1);
+	spin_unlock_irqrestore(&tspi->lock, flags);
+
+	pm_runtime_put(tspi->dev);
+	return 0;
+}
+
+static int tegra_spi_transfer_one_message(struct spi_master *master,
+			struct spi_message *msg)
+{
+	bool is_first_msg = true;
+	int single_xfer;
+	struct tegra_spi_data *tspi = spi_master_get_devdata(master);
+	struct spi_transfer *xfer;
+	struct spi_device *spi = msg->spi;
+	int ret;
+
+	msg->status = 0;
+	msg->actual_length = 0;
+
+	ret = pm_runtime_get_sync(tspi->dev);
+	if (ret < 0) {
+		dev_err(tspi->dev, "runtime PM get failed: %d\n", ret);
+		msg->status = ret;
+		spi_finalize_current_message(master);
+		return ret;
+	}
+
+	single_xfer = list_is_singular(&msg->transfers);
+	list_for_each_entry(xfer, &msg->transfers, transfer_list) {
+		INIT_COMPLETION(tspi->xfer_completion);
+		ret = tegra_spi_start_transfer_one(spi, xfer,
+					is_first_msg, single_xfer);
+		if (ret < 0) {
+			dev_err(tspi->dev,
+				"spi can not start transfer, err %d\n", ret);
+			goto exit;
+		}
+		is_first_msg = false;
+		ret = wait_for_completion_timeout(&tspi->xfer_completion,
+						SPI_DMA_TIMEOUT);
+		if (WARN_ON(ret == 0)) {
+			dev_err(tspi->dev,
+				"spi trasfer timeout, err %d\n", ret);
+			ret = -EIO;
+			goto exit;
+		}
+
+		if (tspi->tx_status ||  tspi->rx_status) {
+			dev_err(tspi->dev, "Error in Transfer\n");
+			ret = -EIO;
+			goto exit;
+		}
+		msg->actual_length += xfer->len;
+		if (xfer->cs_change && xfer->delay_usecs) {
+			tegra_spi_writel(tspi, tspi->def_command1_reg,
+					SPI_COMMAND1);
+			udelay(xfer->delay_usecs);
+		}
+	}
+	ret = 0;
+exit:
+	tegra_spi_writel(tspi, tspi->def_command1_reg, SPI_COMMAND1);
+	pm_runtime_put(tspi->dev);
+	msg->status = ret;
+	spi_finalize_current_message(master);
+	return ret;
+}
+
+static irqreturn_t handle_cpu_based_xfer(struct tegra_spi_data *tspi)
+{
+	struct spi_transfer *t = tspi->curr_xfer;
+	unsigned long flags;
+
+	spin_lock_irqsave(&tspi->lock, flags);
+	if (tspi->tx_status ||  tspi->rx_status) {
+		dev_err(tspi->dev, "CpuXfer ERROR bit set 0x%x\n",
+			tspi->status_reg);
+		dev_err(tspi->dev, "CpuXfer 0x%08x:0x%08x\n",
+			tspi->command1_reg, tspi->dma_control_reg);
+		tegra_periph_reset_assert(tspi->clk);
+		udelay(2);
+		tegra_periph_reset_deassert(tspi->clk);
+		complete(&tspi->xfer_completion);
+		goto exit;
+	}
+
+	if (tspi->cur_direction & DATA_DIR_RX)
+		tegra_spi_read_rx_fifo_to_client_rxbuf(tspi, t);
+
+	if (tspi->cur_direction & DATA_DIR_TX)
+		tspi->cur_pos = tspi->cur_tx_pos;
+	else
+		tspi->cur_pos = tspi->cur_rx_pos;
+
+	if (tspi->cur_pos == t->len) {
+		complete(&tspi->xfer_completion);
+		goto exit;
+	}
+
+	tegra_spi_calculate_curr_xfer_param(tspi->cur_spi, tspi, t);
+	tegra_spi_start_cpu_based_transfer(tspi, t);
+exit:
+	spin_unlock_irqrestore(&tspi->lock, flags);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t handle_dma_based_xfer(struct tegra_spi_data *tspi)
+{
+	struct spi_transfer *t = tspi->curr_xfer;
+	long wait_status;
+	int err = 0;
+	unsigned total_fifo_words;
+	unsigned long flags;
+
+	/* Abort dmas if any error */
+	if (tspi->cur_direction & DATA_DIR_TX) {
+		if (tspi->tx_status) {
+			dmaengine_terminate_all(tspi->tx_dma_chan);
+			err += 1;
+		} else {
+			wait_status = wait_for_completion_interruptible_timeout(
+				&tspi->tx_dma_complete, SPI_DMA_TIMEOUT);
+			if (wait_status <= 0) {
+				dmaengine_terminate_all(tspi->tx_dma_chan);
+				dev_err(tspi->dev, "TxDma Xfer failed\n");
+				err += 1;
+			}
+		}
+	}
+
+	if (tspi->cur_direction & DATA_DIR_RX) {
+		if (tspi->rx_status) {
+			dmaengine_terminate_all(tspi->rx_dma_chan);
+			err += 2;
+		} else {
+			wait_status = wait_for_completion_interruptible_timeout(
+				&tspi->rx_dma_complete, SPI_DMA_TIMEOUT);
+			if (wait_status <= 0) {
+				dmaengine_terminate_all(tspi->rx_dma_chan);
+				dev_err(tspi->dev, "RxDma Xfer failed\n");
+				err += 2;
+			}
+		}
+	}
+
+	spin_lock_irqsave(&tspi->lock, flags);
+	if (err) {
+		dev_err(tspi->dev, "DmaXfer: ERROR bit set 0x%x\n",
+			tspi->status_reg);
+		dev_err(tspi->dev, "DmaXfer 0x%08x:0x%08x\n",
+			tspi->command1_reg, tspi->dma_control_reg);
+		tegra_periph_reset_assert(tspi->clk);
+		udelay(2);
+		tegra_periph_reset_deassert(tspi->clk);
+		complete(&tspi->xfer_completion);
+		spin_unlock_irqrestore(&tspi->lock, flags);
+		return IRQ_HANDLED;
+	}
+
+	if (tspi->cur_direction & DATA_DIR_RX)
+		tegra_spi_copy_spi_rxbuf_to_client_rxbuf(tspi, t);
+
+	if (tspi->cur_direction & DATA_DIR_TX)
+		tspi->cur_pos = tspi->cur_tx_pos;
+	else
+		tspi->cur_pos = tspi->cur_rx_pos;
+
+	if (tspi->cur_pos == t->len) {
+		complete(&tspi->xfer_completion);
+		goto exit;
+	}
+
+	/* Continue transfer in current message */
+	total_fifo_words = tegra_spi_calculate_curr_xfer_param(tspi->cur_spi,
+							tspi, t);
+	if (total_fifo_words > SPI_FIFO_DEPTH)
+		err = tegra_spi_start_dma_based_transfer(tspi, t);
+	else
+		err = tegra_spi_start_cpu_based_transfer(tspi, t);
+
+exit:
+	spin_unlock_irqrestore(&tspi->lock, flags);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t tegra_spi_isr_thread(int irq, void *context_data)
+{
+	struct tegra_spi_data *tspi = context_data;
+
+	if (!tspi->is_curr_dma_xfer)
+		return handle_cpu_based_xfer(tspi);
+	return handle_dma_based_xfer(tspi);
+}
+
+static irqreturn_t tegra_spi_isr(int irq, void *context_data)
+{
+	struct tegra_spi_data *tspi = context_data;
+
+	tspi->status_reg = tegra_spi_readl(tspi, SPI_FIFO_STATUS);
+	if (tspi->cur_direction & DATA_DIR_TX)
+		tspi->tx_status = tspi->status_reg &
+					(SPI_TX_FIFO_UNF | SPI_TX_FIFO_OVF);
+
+	if (tspi->cur_direction & DATA_DIR_RX)
+		tspi->rx_status = tspi->status_reg &
+					(SPI_RX_FIFO_OVF | SPI_RX_FIFO_UNF);
+	tegra_spi_clear_status(tspi);
+
+	return IRQ_WAKE_THREAD;
+}
+
+static void tegra_spi_parse_dt(struct platform_device *pdev,
+	struct tegra_spi_data *tspi)
+{
+	struct device_node *np = pdev->dev.of_node;
+	u32 of_dma[2];
+
+	if (of_property_read_u32_array(np, "nvidia,dma-request-selector",
+				of_dma, 2) >= 0)
+		tspi->dma_req_sel = of_dma[1];
+
+	if (of_property_read_u32(np, "spi-max-frequency",
+				&tspi->spi_max_frequency))
+		tspi->spi_max_frequency = 25000000; /* 25MHz */
+}
+
+static struct of_device_id tegra_spi_of_match[] = {
+	{ .compatible = "nvidia,tegra114-spi", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, tegra_spi_of_match);
+
+static int tegra_spi_probe(struct platform_device *pdev)
+{
+	struct spi_master	*master;
+	struct tegra_spi_data	*tspi;
+	struct resource		*r;
+	int ret, spi_irq;
+
+	master = spi_alloc_master(&pdev->dev, sizeof(*tspi));
+	if (!master) {
+		dev_err(&pdev->dev, "master allocation failed\n");
+		return -ENOMEM;
+	}
+	dev_set_drvdata(&pdev->dev, master);
+	tspi = spi_master_get_devdata(master);
+
+	/* Parse DT */
+	tegra_spi_parse_dt(pdev, tspi);
+
+	/* the spi->mode bits understood by this driver: */
+	master->mode_bits = SPI_CPOL | SPI_CPHA | SPI_CS_HIGH;
+	master->setup = tegra_spi_setup;
+	master->transfer_one_message = tegra_spi_transfer_one_message;
+	master->num_chipselect = MAX_CHIP_SELECT;
+	master->bus_num = -1;
+
+	tspi->master = master;
+	tspi->dev = &pdev->dev;
+	spin_lock_init(&tspi->lock);
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!r) {
+		dev_err(&pdev->dev, "No IO memory resource\n");
+		ret = -ENODEV;
+		goto exit_free_master;
+	}
+	tspi->phys = r->start;
+	tspi->base = devm_ioremap_resource(&pdev->dev, r);
+	if (IS_ERR(tspi->base)) {
+		ret = PTR_ERR(tspi->base);
+		dev_err(&pdev->dev, "ioremap failed: err = %d\n", ret);
+		goto exit_free_master;
+	}
+
+	spi_irq = platform_get_irq(pdev, 0);
+	tspi->irq = spi_irq;
+	ret = request_threaded_irq(tspi->irq, tegra_spi_isr,
+			tegra_spi_isr_thread, IRQF_ONESHOT,
+			dev_name(&pdev->dev), tspi);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Failed to register ISR for IRQ %d\n",
+					tspi->irq);
+		goto exit_free_master;
+	}
+
+	tspi->clk = devm_clk_get(&pdev->dev, "spi");
+	if (IS_ERR(tspi->clk)) {
+		dev_err(&pdev->dev, "can not get clock\n");
+		ret = PTR_ERR(tspi->clk);
+		goto exit_free_irq;
+	}
+
+	tspi->max_buf_size = SPI_FIFO_DEPTH << 2;
+	tspi->dma_buf_size = DEFAULT_SPI_DMA_BUF_LEN;
+
+	if (tspi->dma_req_sel) {
+		ret = tegra_spi_init_dma_param(tspi, true);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "RxDma Init failed, err %d\n", ret);
+			goto exit_free_irq;
+		}
+
+		ret = tegra_spi_init_dma_param(tspi, false);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "TxDma Init failed, err %d\n", ret);
+			goto exit_rx_dma_free;
+		}
+		tspi->max_buf_size = tspi->dma_buf_size;
+		init_completion(&tspi->tx_dma_complete);
+		init_completion(&tspi->rx_dma_complete);
+	}
+
+	init_completion(&tspi->xfer_completion);
+
+	pm_runtime_enable(&pdev->dev);
+	if (!pm_runtime_enabled(&pdev->dev)) {
+		ret = tegra_spi_runtime_resume(&pdev->dev);
+		if (ret)
+			goto exit_pm_disable;
+	}
+
+	ret = pm_runtime_get_sync(&pdev->dev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "pm runtime get failed, e = %d\n", ret);
+		goto exit_pm_disable;
+	}
+	tspi->def_command1_reg  = SPI_M_S;
+	tegra_spi_writel(tspi, tspi->def_command1_reg, SPI_COMMAND1);
+	pm_runtime_put(&pdev->dev);
+
+	master->dev.of_node = pdev->dev.of_node;
+	ret = spi_register_master(master);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "can not register to master err %d\n", ret);
+		goto exit_pm_disable;
+	}
+	return ret;
+
+exit_pm_disable:
+	pm_runtime_disable(&pdev->dev);
+	if (!pm_runtime_status_suspended(&pdev->dev))
+		tegra_spi_runtime_suspend(&pdev->dev);
+	tegra_spi_deinit_dma_param(tspi, false);
+exit_rx_dma_free:
+	tegra_spi_deinit_dma_param(tspi, true);
+exit_free_irq:
+	free_irq(spi_irq, tspi);
+exit_free_master:
+	spi_master_put(master);
+	return ret;
+}
+
+static int tegra_spi_remove(struct platform_device *pdev)
+{
+	struct spi_master *master = dev_get_drvdata(&pdev->dev);
+	struct tegra_spi_data	*tspi = spi_master_get_devdata(master);
+
+	free_irq(tspi->irq, tspi);
+	spi_unregister_master(master);
+
+	if (tspi->tx_dma_chan)
+		tegra_spi_deinit_dma_param(tspi, false);
+
+	if (tspi->rx_dma_chan)
+		tegra_spi_deinit_dma_param(tspi, true);
+
+	pm_runtime_disable(&pdev->dev);
+	if (!pm_runtime_status_suspended(&pdev->dev))
+		tegra_spi_runtime_suspend(&pdev->dev);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int tegra_spi_suspend(struct device *dev)
+{
+	struct spi_master *master = dev_get_drvdata(dev);
+
+	return spi_master_suspend(master);
+}
+
+static int tegra_spi_resume(struct device *dev)
+{
+	struct spi_master *master = dev_get_drvdata(dev);
+	struct tegra_spi_data *tspi = spi_master_get_devdata(master);
+	int ret;
+
+	ret = pm_runtime_get_sync(dev);
+	if (ret < 0) {
+		dev_err(dev, "pm runtime failed, e = %d\n", ret);
+		return ret;
+	}
+	tegra_spi_writel(tspi, tspi->command1_reg, SPI_COMMAND1);
+	pm_runtime_put(dev);
+
+	return spi_master_resume(master);
+}
+#endif
+
+static int tegra_spi_runtime_suspend(struct device *dev)
+{
+	struct spi_master *master = dev_get_drvdata(dev);
+	struct tegra_spi_data *tspi = spi_master_get_devdata(master);
+
+	/* Flush all write which are in PPSB queue by reading back */
+	tegra_spi_readl(tspi, SPI_COMMAND1);
+
+	clk_disable_unprepare(tspi->clk);
+	return 0;
+}
+
+static int tegra_spi_runtime_resume(struct device *dev)
+{
+	struct spi_master *master = dev_get_drvdata(dev);
+	struct tegra_spi_data *tspi = spi_master_get_devdata(master);
+	int ret;
+
+	ret = clk_prepare_enable(tspi->clk);
+	if (ret < 0) {
+		dev_err(tspi->dev, "clk_prepare failed: %d\n", ret);
+		return ret;
+	}
+	return 0;
+}
+
+static const struct dev_pm_ops tegra_spi_pm_ops = {
+	SET_RUNTIME_PM_OPS(tegra_spi_runtime_suspend,
+		tegra_spi_runtime_resume, NULL)
+	SET_SYSTEM_SLEEP_PM_OPS(tegra_spi_suspend, tegra_spi_resume)
+};
+static struct platform_driver tegra_spi_driver = {
+	.driver = {
+		.name		= "spi-tegra114",
+		.owner		= THIS_MODULE,
+		.pm		= &tegra_spi_pm_ops,
+		.of_match_table	= tegra_spi_of_match,
+	},
+	.probe =	tegra_spi_probe,
+	.remove =	tegra_spi_remove,
+};
+module_platform_driver(tegra_spi_driver);
+
+MODULE_ALIAS("platform:spi-tegra114");
+MODULE_DESCRIPTION("NVIDIA Tegra114 SPI Controller Driver");
+MODULE_AUTHOR("Laxman Dewangan <ldewangan@nvidia.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/spi/spi-tegra20-sflash.c b/drivers/spi/spi-tegra20-sflash.c
index 3d6a12b2af0..d65c000efe3 100644
--- a/drivers/spi/spi-tegra20-sflash.c
+++ b/drivers/spi/spi-tegra20-sflash.c
@@ -33,7 +33,6 @@
 #include <linux/of.h>
 #include <linux/of_device.h>
 #include <linux/spi/spi.h>
-#include <linux/spi/spi-tegra.h>
 #include <linux/clk/tegra.h>
 
 #define SPI_COMMAND				0x000
@@ -439,23 +438,13 @@ static irqreturn_t tegra_sflash_isr(int irq, void *context_data)
 	return handle_cpu_based_xfer(tsd);
 }
 
-static struct tegra_spi_platform_data *tegra_sflash_parse_dt(
-		struct platform_device *pdev)
+static void tegra_sflash_parse_dt(struct tegra_sflash_data *tsd)
 {
-	struct tegra_spi_platform_data *pdata;
-	struct device_node *np = pdev->dev.of_node;
-	u32 max_freq;
-
-	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
-	if (!pdata) {
-		dev_err(&pdev->dev, "Memory alloc for pdata failed\n");
-		return NULL;
-	}
-
-	if (!of_property_read_u32(np, "spi-max-frequency", &max_freq))
-		pdata->spi_max_frequency = max_freq;
+	struct device_node *np = tsd->dev->of_node;
 
-	return pdata;
+	if (of_property_read_u32(np, "spi-max-frequency",
+					&tsd->spi_max_frequency))
+		tsd->spi_max_frequency = 25000000; /* 25MHz */
 }
 
 static struct of_device_id tegra_sflash_of_match[] = {
@@ -469,28 +458,15 @@ static int tegra_sflash_probe(struct platform_device *pdev)
 	struct spi_master	*master;
 	struct tegra_sflash_data	*tsd;
 	struct resource		*r;
-	struct tegra_spi_platform_data *pdata = pdev->dev.platform_data;
 	int ret;
 	const struct of_device_id *match;
 
-	match = of_match_device(of_match_ptr(tegra_sflash_of_match),
-					&pdev->dev);
+	match = of_match_device(tegra_sflash_of_match, &pdev->dev);
 	if (!match) {
 		dev_err(&pdev->dev, "Error: No device match found\n");
 		return -ENODEV;
 	}
 
-	if (!pdata && pdev->dev.of_node)
-		pdata = tegra_sflash_parse_dt(pdev);
-
-	if (!pdata) {
-		dev_err(&pdev->dev, "No platform data, exiting\n");
-		return -ENODEV;
-	}
-
-	if (!pdata->spi_max_frequency)
-		pdata->spi_max_frequency = 25000000; /* 25MHz */
-
 	master = spi_alloc_master(&pdev->dev, sizeof(*tsd));
 	if (!master) {
 		dev_err(&pdev->dev, "master allocation failed\n");
@@ -510,6 +486,8 @@ static int tegra_sflash_probe(struct platform_device *pdev)
 	tsd->dev = &pdev->dev;
 	spin_lock_init(&tsd->lock);
 
+	tegra_sflash_parse_dt(tsd);
+
 	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!r) {
 		dev_err(&pdev->dev, "No IO memory resource\n");
@@ -538,7 +516,6 @@ static int tegra_sflash_probe(struct platform_device *pdev)
 		goto exit_free_irq;
 	}
 
-	tsd->spi_max_frequency = pdata->spi_max_frequency;
 	init_completion(&tsd->xfer_completion);
 	pm_runtime_enable(&pdev->dev);
 	if (!pm_runtime_enabled(&pdev->dev)) {
@@ -658,7 +635,7 @@ static struct platform_driver tegra_sflash_driver = {
 		.name		= "spi-tegra-sflash",
 		.owner		= THIS_MODULE,
 		.pm		= &slink_pm_ops,
-		.of_match_table	= of_match_ptr(tegra_sflash_of_match),
+		.of_match_table	= tegra_sflash_of_match,
 	},
 	.probe =	tegra_sflash_probe,
 	.remove =	tegra_sflash_remove,
diff --git a/drivers/spi/spi-tegra20-slink.c b/drivers/spi/spi-tegra20-slink.c
index b8698b389ef..3faf88d003d 100644
--- a/drivers/spi/spi-tegra20-slink.c
+++ b/drivers/spi/spi-tegra20-slink.c
@@ -34,7 +34,6 @@
 #include <linux/of.h>
 #include <linux/of_device.h>
 #include <linux/spi/spi.h>
-#include <linux/spi/spi-tegra.h>
 #include <linux/clk/tegra.h>
 
 #define SLINK_COMMAND			0x000
@@ -189,7 +188,6 @@ struct tegra_slink_data {
 	unsigned				dma_buf_size;
 	unsigned				max_buf_size;
 	bool					is_curr_dma_xfer;
-	bool					is_hw_based_cs;
 
 	struct completion			rx_dma_complete;
 	struct completion			tx_dma_complete;
@@ -375,9 +373,6 @@ static unsigned int tegra_slink_read_rx_fifo_to_client_rxbuf(
 		tspi->cur_rx_pos += tspi->curr_dma_words * tspi->bytes_per_word;
 		read_words += tspi->curr_dma_words;
 	} else {
-		unsigned int bits_per_word;
-
-		bits_per_word = t->bits_per_word;
 		for (count = 0; count < rx_full_count; count++) {
 			x = tegra_slink_readl(tspi, SLINK_RX_FIFO);
 			for (i = 0; (i < tspi->bytes_per_word); i++)
@@ -720,7 +715,6 @@ static int tegra_slink_start_transfer_one(struct spi_device *spi,
 	u8 bits_per_word;
 	unsigned total_fifo_words;
 	int ret;
-	struct tegra_spi_device_controller_data *cdata = spi->controller_data;
 	unsigned long command;
 	unsigned long command2;
 
@@ -743,39 +737,11 @@ static int tegra_slink_start_transfer_one(struct spi_device *spi,
 
 		command = tspi->def_command_reg;
 		command |= SLINK_BIT_LENGTH(bits_per_word - 1);
+		command |= SLINK_CS_SW | SLINK_CS_VALUE;
 
 		command2 = tspi->def_command2_reg;
 		command2 |= SLINK_SS_EN_CS(spi->chip_select);
 
-		/* possibly use the hw based chip select */
-		tspi->is_hw_based_cs = false;
-		if (cdata && cdata->is_hw_based_cs && is_single_xfer &&
-			((tspi->curr_dma_words * tspi->bytes_per_word) ==
-						(t->len - tspi->cur_pos))) {
-			int setup_count;
-			int sts2;
-
-			setup_count = cdata->cs_setup_clk_count >> 1;
-			setup_count = max(setup_count, 3);
-			command2 |= SLINK_SS_SETUP(setup_count);
-			if (tspi->chip_data->cs_hold_time) {
-				int hold_count;
-
-				hold_count = cdata->cs_hold_clk_count;
-				hold_count = max(hold_count, 0xF);
-				sts2 = tegra_slink_readl(tspi, SLINK_STATUS2);
-				sts2 &= ~SLINK_SS_HOLD_TIME(0xF);
-				sts2 |= SLINK_SS_HOLD_TIME(hold_count);
-				tegra_slink_writel(tspi, sts2, SLINK_STATUS2);
-			}
-			tspi->is_hw_based_cs = true;
-		}
-
-		if (tspi->is_hw_based_cs)
-			command &= ~SLINK_CS_SW;
-		else
-			command |= SLINK_CS_SW | SLINK_CS_VALUE;
-
 		command &= ~SLINK_MODES;
 		if (spi->mode & SPI_CPHA)
 			command |= SLINK_CK_SDA;
@@ -858,21 +824,6 @@ static int tegra_slink_setup(struct spi_device *spi)
 	return 0;
 }
 
-static int tegra_slink_prepare_transfer(struct spi_master *master)
-{
-	struct tegra_slink_data *tspi = spi_master_get_devdata(master);
-
-	return pm_runtime_get_sync(tspi->dev);
-}
-
-static int tegra_slink_unprepare_transfer(struct spi_master *master)
-{
-	struct tegra_slink_data *tspi = spi_master_get_devdata(master);
-
-	pm_runtime_put(tspi->dev);
-	return 0;
-}
-
 static int tegra_slink_transfer_one_message(struct spi_master *master,
 			struct spi_message *msg)
 {
@@ -885,6 +836,12 @@ static int tegra_slink_transfer_one_message(struct spi_master *master,
 
 	msg->status = 0;
 	msg->actual_length = 0;
+	ret = pm_runtime_get_sync(tspi->dev);
+	if (ret < 0) {
+		dev_err(tspi->dev, "runtime get failed: %d\n", ret);
+		goto done;
+	}
+
 	single_xfer = list_is_singular(&msg->transfers);
 	list_for_each_entry(xfer, &msg->transfers, transfer_list) {
 		INIT_COMPLETION(tspi->xfer_completion);
@@ -921,6 +878,8 @@ static int tegra_slink_transfer_one_message(struct spi_master *master,
 exit:
 	tegra_slink_writel(tspi, tspi->def_command_reg, SLINK_COMMAND);
 	tegra_slink_writel(tspi, tspi->def_command2_reg, SLINK_COMMAND2);
+	pm_runtime_put(tspi->dev);
+done:
 	msg->status = ret;
 	spi_finalize_current_message(master);
 	return ret;
@@ -1072,36 +1031,25 @@ static irqreturn_t tegra_slink_isr(int irq, void *context_data)
 	return IRQ_WAKE_THREAD;
 }
 
-static struct tegra_spi_platform_data *tegra_slink_parse_dt(
-		struct platform_device *pdev)
+static void tegra_slink_parse_dt(struct tegra_slink_data *tspi)
 {
-	struct tegra_spi_platform_data *pdata;
-	const unsigned int *prop;
-	struct device_node *np = pdev->dev.of_node;
+	struct device_node *np = tspi->dev->of_node;
 	u32 of_dma[2];
 
-	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
-	if (!pdata) {
-		dev_err(&pdev->dev, "Memory alloc for pdata failed\n");
-		return NULL;
-	}
-
 	if (of_property_read_u32_array(np, "nvidia,dma-request-selector",
 				of_dma, 2) >= 0)
-		pdata->dma_req_sel = of_dma[1];
+		tspi->dma_req_sel = of_dma[1];
 
-	prop = of_get_property(np, "spi-max-frequency", NULL);
-	if (prop)
-		pdata->spi_max_frequency = be32_to_cpup(prop);
-
-	return pdata;
+	if (of_property_read_u32(np, "spi-max-frequency",
+					&tspi->spi_max_frequency))
+		tspi->spi_max_frequency = 25000000; /* 25MHz */
 }
 
-const struct tegra_slink_chip_data tegra30_spi_cdata = {
+static const struct tegra_slink_chip_data tegra30_spi_cdata = {
 	.cs_hold_time = true,
 };
 
-const struct tegra_slink_chip_data tegra20_spi_cdata = {
+static const struct tegra_slink_chip_data tegra20_spi_cdata = {
 	.cs_hold_time = false,
 };
 
@@ -1117,27 +1065,16 @@ static int tegra_slink_probe(struct platform_device *pdev)
 	struct spi_master	*master;
 	struct tegra_slink_data	*tspi;
 	struct resource		*r;
-	struct tegra_spi_platform_data *pdata = pdev->dev.platform_data;
 	int ret, spi_irq;
 	const struct tegra_slink_chip_data *cdata = NULL;
 	const struct of_device_id *match;
 
-	match = of_match_device(of_match_ptr(tegra_slink_of_match), &pdev->dev);
+	match = of_match_device(tegra_slink_of_match, &pdev->dev);
 	if (!match) {
 		dev_err(&pdev->dev, "Error: No device match found\n");
 		return -ENODEV;
 	}
 	cdata = match->data;
-	if (!pdata && pdev->dev.of_node)
-		pdata = tegra_slink_parse_dt(pdev);
-
-	if (!pdata) {
-		dev_err(&pdev->dev, "No platform data, exiting\n");
-		return -ENODEV;
-	}
-
-	if (!pdata->spi_max_frequency)
-		pdata->spi_max_frequency = 25000000; /* 25MHz */
 
 	master = spi_alloc_master(&pdev->dev, sizeof(*tspi));
 	if (!master) {
@@ -1148,20 +1085,19 @@ static int tegra_slink_probe(struct platform_device *pdev)
 	/* the spi->mode bits understood by this driver: */
 	master->mode_bits = SPI_CPOL | SPI_CPHA | SPI_CS_HIGH;
 	master->setup = tegra_slink_setup;
-	master->prepare_transfer_hardware = tegra_slink_prepare_transfer;
 	master->transfer_one_message = tegra_slink_transfer_one_message;
-	master->unprepare_transfer_hardware = tegra_slink_unprepare_transfer;
 	master->num_chipselect = MAX_CHIP_SELECT;
 	master->bus_num = -1;
 
 	dev_set_drvdata(&pdev->dev, master);
 	tspi = spi_master_get_devdata(master);
 	tspi->master = master;
-	tspi->dma_req_sel = pdata->dma_req_sel;
 	tspi->dev = &pdev->dev;
 	tspi->chip_data = cdata;
 	spin_lock_init(&tspi->lock);
 
+	tegra_slink_parse_dt(tspi);
+
 	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!r) {
 		dev_err(&pdev->dev, "No IO memory resource\n");
@@ -1195,9 +1131,8 @@ static int tegra_slink_probe(struct platform_device *pdev)
 
 	tspi->max_buf_size = SLINK_FIFO_DEPTH << 2;
 	tspi->dma_buf_size = DEFAULT_SPI_DMA_BUF_LEN;
-	tspi->spi_max_frequency = pdata->spi_max_frequency;
 
-	if (pdata->dma_req_sel) {
+	if (tspi->dma_req_sel) {
 		ret = tegra_slink_init_dma_param(tspi, true);
 		if (ret < 0) {
 			dev_err(&pdev->dev, "RxDma Init failed, err %d\n", ret);
@@ -1340,7 +1275,7 @@ static struct platform_driver tegra_slink_driver = {
 		.name		= "spi-tegra-slink",
 		.owner		= THIS_MODULE,
 		.pm		= &slink_pm_ops,
-		.of_match_table	= of_match_ptr(tegra_slink_of_match),
+		.of_match_table	= tegra_slink_of_match,
 	},
 	.probe =	tegra_slink_probe,
 	.remove =	tegra_slink_remove,
diff --git a/drivers/spi/spi-topcliff-pch.c b/drivers/spi/spi-topcliff-pch.c
index f756481b0fe..35f60bd252d 100644
--- a/drivers/spi/spi-topcliff-pch.c
+++ b/drivers/spi/spi-topcliff-pch.c
@@ -615,7 +615,7 @@ static void pch_spi_set_tx(struct pch_spi_data *data, int *bpw)
 	int size;
 	u32 n_writes;
 	int j;
-	struct spi_message *pmsg;
+	struct spi_message *pmsg, *tmp;
 	const u8 *tx_buf;
 	const u16 *tx_sbuf;
 
@@ -656,7 +656,7 @@ static void pch_spi_set_tx(struct pch_spi_data *data, int *bpw)
 	if (!data->pkt_rx_buff) {
 		/* flush queue and set status of all transfers to -ENOMEM */
 		dev_err(&data->master->dev, "%s :kzalloc failed\n", __func__);
-		list_for_each_entry(pmsg, data->queue.next, queue) {
+		list_for_each_entry_safe(pmsg, tmp, data->queue.next, queue) {
 			pmsg->status = -ENOMEM;
 
 			if (pmsg->complete != 0)
@@ -703,7 +703,7 @@ static void pch_spi_set_tx(struct pch_spi_data *data, int *bpw)
 
 static void pch_spi_nomore_transfer(struct pch_spi_data *data)
 {
-	struct spi_message *pmsg;
+	struct spi_message *pmsg, *tmp;
 	dev_dbg(&data->master->dev, "%s called\n", __func__);
 	/* Invoke complete callback
 	 * [To the spi core..indicating end of transfer] */
@@ -740,7 +740,7 @@ static void pch_spi_nomore_transfer(struct pch_spi_data *data)
 		dev_dbg(&data->master->dev,
 			"%s suspend/remove initiated, flushing queue\n",
 			__func__);
-		list_for_each_entry(pmsg, data->queue.next, queue) {
+		list_for_each_entry_safe(pmsg, tmp, data->queue.next, queue) {
 			pmsg->status = -EIO;
 
 			if (pmsg->complete)
@@ -1187,7 +1187,7 @@ static void pch_spi_handle_dma(struct pch_spi_data *data, int *bpw)
 
 static void pch_spi_process_messages(struct work_struct *pwork)
 {
-	struct spi_message *pmsg;
+	struct spi_message *pmsg, *tmp;
 	struct pch_spi_data *data;
 	int bpw;
 
@@ -1199,7 +1199,7 @@ static void pch_spi_process_messages(struct work_struct *pwork)
 	if (data->board_dat->suspend_sts || (data->status == STATUS_EXITING)) {
 		dev_dbg(&data->master->dev, "%s suspend/remove initiated,"
 			"flushing queue\n", __func__);
-		list_for_each_entry(pmsg, data->queue.next, queue) {
+		list_for_each_entry_safe(pmsg, tmp, data->queue.next, queue) {
 			pmsg->status = -EIO;
 
 			if (pmsg->complete != 0) {
@@ -1789,8 +1789,10 @@ static int __init pch_spi_init(void)
 		return ret;
 
 	ret = pci_register_driver(&pch_spi_pcidev_driver);
-	if (ret)
+	if (ret) {
+		platform_driver_unregister(&pch_spi_pd_driver);
 		return ret;
+	}
 
 	return 0;
 }
diff --git a/drivers/spi/spi.c b/drivers/spi/spi.c
index f996c600eb8..163fd802b7a 100644
--- a/drivers/spi/spi.c
+++ b/drivers/spi/spi.c
@@ -543,17 +543,16 @@ static void spi_pump_messages(struct kthread_work *work)
 	/* Lock queue and check for queue work */
 	spin_lock_irqsave(&master->queue_lock, flags);
 	if (list_empty(&master->queue) || !master->running) {
-		if (master->busy && master->unprepare_transfer_hardware) {
-			ret = master->unprepare_transfer_hardware(master);
-			if (ret) {
-				spin_unlock_irqrestore(&master->queue_lock, flags);
-				dev_err(&master->dev,
-					"failed to unprepare transfer hardware\n");
-				return;
-			}
+		if (!master->busy) {
+			spin_unlock_irqrestore(&master->queue_lock, flags);
+			return;
 		}
 		master->busy = false;
 		spin_unlock_irqrestore(&master->queue_lock, flags);
+		if (master->unprepare_transfer_hardware &&
+		    master->unprepare_transfer_hardware(master))
+			dev_err(&master->dev,
+				"failed to unprepare transfer hardware\n");
 		return;
 	}
 
@@ -984,7 +983,7 @@ static void acpi_register_spi_devices(struct spi_master *master)
 	acpi_status status;
 	acpi_handle handle;
 
-	handle = ACPI_HANDLE(&master->dev);
+	handle = ACPI_HANDLE(master->dev.parent);
 	if (!handle)
 		return;
 
@@ -1377,6 +1376,14 @@ static int __spi_async(struct spi_device *spi, struct spi_message *message)
 			xfer->bits_per_word = spi->bits_per_word;
 		if (!xfer->speed_hz)
 			xfer->speed_hz = spi->max_speed_hz;
+		if (master->bits_per_word_mask) {
+			/* Only 32 bits fit in the mask */
+			if (xfer->bits_per_word > 32)
+				return -EINVAL;
+			if (!(master->bits_per_word_mask &
+					BIT(xfer->bits_per_word - 1)))
+				return -EINVAL;
+		}
 	}
 
 	message->spi = spi;
diff --git a/drivers/spi/spidev.c b/drivers/spi/spidev.c
index 2e0655dbe07..911e9e0711d 100644
--- a/drivers/spi/spidev.c
+++ b/drivers/spi/spidev.c
@@ -603,7 +603,7 @@ static int spidev_probe(struct spi_device *spi)
 		dev = device_create(spidev_class, &spi->dev, spidev->devt,
 				    spidev, "spidev%d.%d",
 				    spi->master->bus_num, spi->chip_select);
-		status = IS_ERR(dev) ? PTR_ERR(dev) : 0;
+		status = PTR_RET(dev);
 	} else {
 		dev_dbg(&spi->dev, "no minor number available!\n");
 		status = -ENODEV;
diff --git a/drivers/ssb/driver_chipcommon_pmu.c b/drivers/ssb/driver_chipcommon_pmu.c
index 4c0f6d883dd..7b0bce93676 100644
--- a/drivers/ssb/driver_chipcommon_pmu.c
+++ b/drivers/ssb/driver_chipcommon_pmu.c
@@ -675,3 +675,32 @@ u32 ssb_pmu_get_controlclock(struct ssb_chipcommon *cc)
 		return 0;
 	}
 }
+
+void ssb_pmu_spuravoid_pllupdate(struct ssb_chipcommon *cc, int spuravoid)
+{
+	u32 pmu_ctl = 0;
+
+	switch (cc->dev->bus->chip_id) {
+	case 0x4322:
+		ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL0, 0x11100070);
+		ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL1, 0x1014140a);
+		ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL5, 0x88888854);
+		if (spuravoid == 1)
+			ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL2, 0x05201828);
+		else
+			ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL2, 0x05001828);
+		pmu_ctl = SSB_CHIPCO_PMU_CTL_PLL_UPD;
+		break;
+	case 43222:
+		/* TODO: BCM43222 requires updating PLLs too */
+		return;
+	default:
+		ssb_printk(KERN_ERR PFX
+			   "Unknown spuravoidance settings for chip 0x%04X, not changing PLL\n",
+			   cc->dev->bus->chip_id);
+		return;
+	}
+
+	chipco_set32(cc, SSB_CHIPCO_PMU_CTL, pmu_ctl);
+}
+EXPORT_SYMBOL_GPL(ssb_pmu_spuravoid_pllupdate);
diff --git a/drivers/ssbi/Kconfig b/drivers/ssbi/Kconfig
new file mode 100644
index 00000000000..1ae4040afed
--- /dev/null
+++ b/drivers/ssbi/Kconfig
@@ -0,0 +1,16 @@
+#
+# SSBI bus support
+#
+
+menu "Qualcomm MSM SSBI bus support"
+
+config SSBI
+	tristate "Qualcomm Single-wire Serial Bus Interface (SSBI)"
+	help
+	  If you say yes to this option, support will be included for the
+	  built-in SSBI interface on Qualcomm MSM family processors.
+
+	  This is required for communicating with Qualcomm PMICs and
+	  other devices that have the SSBI interface.
+
+endmenu
diff --git a/drivers/ssbi/Makefile b/drivers/ssbi/Makefile
new file mode 100644
index 00000000000..38fb70c31ca
--- /dev/null
+++ b/drivers/ssbi/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_SSBI) += ssbi.o
diff --git a/drivers/ssbi/ssbi.c b/drivers/ssbi/ssbi.c
new file mode 100644
index 00000000000..f32da0258a8
--- /dev/null
+++ b/drivers/ssbi/ssbi.c
@@ -0,0 +1,379 @@
+/* Copyright (c) 2009-2013, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2010, Google Inc.
+ *
+ * Original authors: Code Aurora Forum
+ *
+ * Author: Dima Zavin <dima@android.com>
+ *  - Largely rewritten from original to not be an i2c driver.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only 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.
+ */
+
+#define pr_fmt(fmt) "%s: " fmt, __func__
+
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/ssbi.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+
+/* SSBI 2.0 controller registers */
+#define SSBI2_CMD			0x0008
+#define SSBI2_RD			0x0010
+#define SSBI2_STATUS			0x0014
+#define SSBI2_MODE2			0x001C
+
+/* SSBI_CMD fields */
+#define SSBI_CMD_RDWRN			(1 << 24)
+
+/* SSBI_STATUS fields */
+#define SSBI_STATUS_RD_READY		(1 << 2)
+#define SSBI_STATUS_READY		(1 << 1)
+#define SSBI_STATUS_MCHN_BUSY		(1 << 0)
+
+/* SSBI_MODE2 fields */
+#define SSBI_MODE2_REG_ADDR_15_8_SHFT	0x04
+#define SSBI_MODE2_REG_ADDR_15_8_MASK	(0x7f << SSBI_MODE2_REG_ADDR_15_8_SHFT)
+
+#define SET_SSBI_MODE2_REG_ADDR_15_8(MD, AD) \
+	(((MD) & 0x0F) | ((((AD) >> 8) << SSBI_MODE2_REG_ADDR_15_8_SHFT) & \
+	SSBI_MODE2_REG_ADDR_15_8_MASK))
+
+/* SSBI PMIC Arbiter command registers */
+#define SSBI_PA_CMD			0x0000
+#define SSBI_PA_RD_STATUS		0x0004
+
+/* SSBI_PA_CMD fields */
+#define SSBI_PA_CMD_RDWRN		(1 << 24)
+#define SSBI_PA_CMD_ADDR_MASK		0x7fff /* REG_ADDR_7_0, REG_ADDR_8_14*/
+
+/* SSBI_PA_RD_STATUS fields */
+#define SSBI_PA_RD_STATUS_TRANS_DONE	(1 << 27)
+#define SSBI_PA_RD_STATUS_TRANS_DENIED	(1 << 26)
+
+#define SSBI_TIMEOUT_US			100
+
+struct ssbi {
+	struct device		*slave;
+	void __iomem		*base;
+	spinlock_t		lock;
+	enum ssbi_controller_type controller_type;
+	int (*read)(struct ssbi *, u16 addr, u8 *buf, int len);
+	int (*write)(struct ssbi *, u16 addr, u8 *buf, int len);
+};
+
+#define to_ssbi(dev)	platform_get_drvdata(to_platform_device(dev))
+
+static inline u32 ssbi_readl(struct ssbi *ssbi, u32 reg)
+{
+	return readl(ssbi->base + reg);
+}
+
+static inline void ssbi_writel(struct ssbi *ssbi, u32 val, u32 reg)
+{
+	writel(val, ssbi->base + reg);
+}
+
+/*
+ * Via private exchange with one of the original authors, the hardware
+ * should generally finish a transaction in about 5us.  The worst
+ * case, is when using the arbiter and both other CPUs have just
+ * started trying to use the SSBI bus will result in a time of about
+ * 20us.  It should never take longer than this.
+ *
+ * As such, this wait merely spins, with a udelay.
+ */
+static int ssbi_wait_mask(struct ssbi *ssbi, u32 set_mask, u32 clr_mask)
+{
+	u32 timeout = SSBI_TIMEOUT_US;
+	u32 val;
+
+	while (timeout--) {
+		val = ssbi_readl(ssbi, SSBI2_STATUS);
+		if (((val & set_mask) == set_mask) && ((val & clr_mask) == 0))
+			return 0;
+		udelay(1);
+	}
+
+	return -ETIMEDOUT;
+}
+
+static int
+ssbi_read_bytes(struct ssbi *ssbi, u16 addr, u8 *buf, int len)
+{
+	u32 cmd = SSBI_CMD_RDWRN | ((addr & 0xff) << 16);
+	int ret = 0;
+
+	if (ssbi->controller_type == MSM_SBI_CTRL_SSBI2) {
+		u32 mode2 = ssbi_readl(ssbi, SSBI2_MODE2);
+		mode2 = SET_SSBI_MODE2_REG_ADDR_15_8(mode2, addr);
+		ssbi_writel(ssbi, mode2, SSBI2_MODE2);
+	}
+
+	while (len) {
+		ret = ssbi_wait_mask(ssbi, SSBI_STATUS_READY, 0);
+		if (ret)
+			goto err;
+
+		ssbi_writel(ssbi, cmd, SSBI2_CMD);
+		ret = ssbi_wait_mask(ssbi, SSBI_STATUS_RD_READY, 0);
+		if (ret)
+			goto err;
+		*buf++ = ssbi_readl(ssbi, SSBI2_RD) & 0xff;
+		len--;
+	}
+
+err:
+	return ret;
+}
+
+static int
+ssbi_write_bytes(struct ssbi *ssbi, u16 addr, u8 *buf, int len)
+{
+	int ret = 0;
+
+	if (ssbi->controller_type == MSM_SBI_CTRL_SSBI2) {
+		u32 mode2 = ssbi_readl(ssbi, SSBI2_MODE2);
+		mode2 = SET_SSBI_MODE2_REG_ADDR_15_8(mode2, addr);
+		ssbi_writel(ssbi, mode2, SSBI2_MODE2);
+	}
+
+	while (len) {
+		ret = ssbi_wait_mask(ssbi, SSBI_STATUS_READY, 0);
+		if (ret)
+			goto err;
+
+		ssbi_writel(ssbi, ((addr & 0xff) << 16) | *buf, SSBI2_CMD);
+		ret = ssbi_wait_mask(ssbi, 0, SSBI_STATUS_MCHN_BUSY);
+		if (ret)
+			goto err;
+		buf++;
+		len--;
+	}
+
+err:
+	return ret;
+}
+
+/*
+ * See ssbi_wait_mask for an explanation of the time and the
+ * busywait.
+ */
+static inline int
+ssbi_pa_transfer(struct ssbi *ssbi, u32 cmd, u8 *data)
+{
+	u32 timeout = SSBI_TIMEOUT_US;
+	u32 rd_status = 0;
+
+	ssbi_writel(ssbi, cmd, SSBI_PA_CMD);
+
+	while (timeout--) {
+		rd_status = ssbi_readl(ssbi, SSBI_PA_RD_STATUS);
+
+		if (rd_status & SSBI_PA_RD_STATUS_TRANS_DENIED)
+			return -EPERM;
+
+		if (rd_status & SSBI_PA_RD_STATUS_TRANS_DONE) {
+			if (data)
+				*data = rd_status & 0xff;
+			return 0;
+		}
+		udelay(1);
+	}
+
+	return -ETIMEDOUT;
+}
+
+static int
+ssbi_pa_read_bytes(struct ssbi *ssbi, u16 addr, u8 *buf, int len)
+{
+	u32 cmd;
+	int ret = 0;
+
+	cmd = SSBI_PA_CMD_RDWRN | (addr & SSBI_PA_CMD_ADDR_MASK) << 8;
+
+	while (len) {
+		ret = ssbi_pa_transfer(ssbi, cmd, buf);
+		if (ret)
+			goto err;
+		buf++;
+		len--;
+	}
+
+err:
+	return ret;
+}
+
+static int
+ssbi_pa_write_bytes(struct ssbi *ssbi, u16 addr, u8 *buf, int len)
+{
+	u32 cmd;
+	int ret = 0;
+
+	while (len) {
+		cmd = (addr & SSBI_PA_CMD_ADDR_MASK) << 8 | *buf;
+		ret = ssbi_pa_transfer(ssbi, cmd, NULL);
+		if (ret)
+			goto err;
+		buf++;
+		len--;
+	}
+
+err:
+	return ret;
+}
+
+int ssbi_read(struct device *dev, u16 addr, u8 *buf, int len)
+{
+	struct ssbi *ssbi = to_ssbi(dev);
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&ssbi->lock, flags);
+	ret = ssbi->read(ssbi, addr, buf, len);
+	spin_unlock_irqrestore(&ssbi->lock, flags);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(ssbi_read);
+
+int ssbi_write(struct device *dev, u16 addr, u8 *buf, int len)
+{
+	struct ssbi *ssbi = to_ssbi(dev);
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&ssbi->lock, flags);
+	ret = ssbi->write(ssbi, addr, buf, len);
+	spin_unlock_irqrestore(&ssbi->lock, flags);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(ssbi_write);
+
+static int ssbi_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct resource *mem_res;
+	struct ssbi *ssbi;
+	int ret = 0;
+	const char *type;
+
+	ssbi = kzalloc(sizeof(struct ssbi), GFP_KERNEL);
+	if (!ssbi) {
+		pr_err("can not allocate ssbi_data\n");
+		return -ENOMEM;
+	}
+
+	mem_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!mem_res) {
+		pr_err("missing mem resource\n");
+		ret = -EINVAL;
+		goto err_get_mem_res;
+	}
+
+	ssbi->base = ioremap(mem_res->start, resource_size(mem_res));
+	if (!ssbi->base) {
+		pr_err("ioremap of 0x%p failed\n", (void *)mem_res->start);
+		ret = -EINVAL;
+		goto err_ioremap;
+	}
+	platform_set_drvdata(pdev, ssbi);
+
+	type = of_get_property(np, "qcom,controller-type", NULL);
+	if (type == NULL) {
+		pr_err("Missing qcom,controller-type property\n");
+		ret = -EINVAL;
+		goto err_ssbi_controller;
+	}
+	dev_info(&pdev->dev, "SSBI controller type: '%s'\n", type);
+	if (strcmp(type, "ssbi") == 0)
+		ssbi->controller_type = MSM_SBI_CTRL_SSBI;
+	else if (strcmp(type, "ssbi2") == 0)
+		ssbi->controller_type = MSM_SBI_CTRL_SSBI2;
+	else if (strcmp(type, "pmic-arbiter") == 0)
+		ssbi->controller_type = MSM_SBI_CTRL_PMIC_ARBITER;
+	else {
+		pr_err("Unknown qcom,controller-type\n");
+		ret = -EINVAL;
+		goto err_ssbi_controller;
+	}
+
+	if (ssbi->controller_type == MSM_SBI_CTRL_PMIC_ARBITER) {
+		ssbi->read = ssbi_pa_read_bytes;
+		ssbi->write = ssbi_pa_write_bytes;
+	} else {
+		ssbi->read = ssbi_read_bytes;
+		ssbi->write = ssbi_write_bytes;
+	}
+
+	spin_lock_init(&ssbi->lock);
+
+	ret = of_platform_populate(np, NULL, NULL, &pdev->dev);
+	if (ret)
+		goto err_ssbi_controller;
+
+	return 0;
+
+err_ssbi_controller:
+	platform_set_drvdata(pdev, NULL);
+	iounmap(ssbi->base);
+err_ioremap:
+err_get_mem_res:
+	kfree(ssbi);
+	return ret;
+}
+
+static int ssbi_remove(struct platform_device *pdev)
+{
+	struct ssbi *ssbi = platform_get_drvdata(pdev);
+
+	platform_set_drvdata(pdev, NULL);
+	iounmap(ssbi->base);
+	kfree(ssbi);
+	return 0;
+}
+
+static struct of_device_id ssbi_match_table[] = {
+	{ .compatible = "qcom,ssbi" },
+	{}
+};
+
+static struct platform_driver ssbi_driver = {
+	.probe		= ssbi_probe,
+	.remove		= ssbi_remove,
+	.driver		= {
+		.name	= "ssbi",
+		.owner	= THIS_MODULE,
+		.of_match_table = ssbi_match_table,
+	},
+};
+
+static int __init ssbi_init(void)
+{
+	return platform_driver_register(&ssbi_driver);
+}
+module_init(ssbi_init);
+
+static void __exit ssbi_exit(void)
+{
+	platform_driver_unregister(&ssbi_driver);
+}
+module_exit(ssbi_exit)
+
+MODULE_LICENSE("GPL v2");
+MODULE_VERSION("1.0");
+MODULE_ALIAS("platform:ssbi");
+MODULE_AUTHOR("Dima Zavin <dima@android.com>");
diff --git a/drivers/staging/Kconfig b/drivers/staging/Kconfig
index 093f10c88cc..4e8a1794f50 100644
--- a/drivers/staging/Kconfig
+++ b/drivers/staging/Kconfig
@@ -110,19 +110,15 @@ source "drivers/staging/nvec/Kconfig"
 
 source "drivers/staging/media/Kconfig"
 
-source "drivers/staging/net/Kconfig"
-
 source "drivers/staging/android/Kconfig"
 
 source "drivers/staging/ozwpan/Kconfig"
 
-source "drivers/staging/ccg/Kconfig"
-
 source "drivers/staging/gdm72xx/Kconfig"
 
 source "drivers/staging/csr/Kconfig"
 
-source "drivers/staging/omap-thermal/Kconfig"
+source "drivers/staging/ti-soc-thermal/Kconfig"
 
 source "drivers/staging/silicom/Kconfig"
 
@@ -140,4 +136,8 @@ source "drivers/staging/zcache/Kconfig"
 
 source "drivers/staging/goldfish/Kconfig"
 
+source "drivers/staging/netlogic/Kconfig"
+
+source "drivers/staging/dwc2/Kconfig"
+
 endif # STAGING
diff --git a/drivers/staging/Makefile b/drivers/staging/Makefile
index fa41b04cf4c..415772ea306 100644
--- a/drivers/staging/Makefile
+++ b/drivers/staging/Makefile
@@ -4,7 +4,6 @@
 obj-$(CONFIG_STAGING)		+= staging.o
 
 obj-y				+= media/
-obj-y				+= net/
 obj-$(CONFIG_ET131X)		+= et131x/
 obj-$(CONFIG_SLICOSS)		+= slicoss/
 obj-$(CONFIG_USBIP_CORE)	+= usbip/
@@ -23,6 +22,7 @@ obj-$(CONFIG_RTS5139)		+= rts5139/
 obj-$(CONFIG_TRANZPORT)		+= frontier/
 obj-$(CONFIG_IDE_PHISON)	+= phison/
 obj-$(CONFIG_LINE6_USB)		+= line6/
+obj-$(CONFIG_NETLOGIC_XLR_NET)	+= netlogic/
 obj-$(CONFIG_USB_SERIAL_QUATECH2)	+= serqt_usb2/
 obj-$(CONFIG_OCTEON_ETHERNET)	+= octeon/
 obj-$(CONFIG_VT6655)		+= vt6655/
@@ -50,10 +50,9 @@ obj-$(CONFIG_TOUCHSCREEN_SYNAPTICS_I2C_RMI4)	+= ste_rmi4/
 obj-$(CONFIG_MFD_NVEC)		+= nvec/
 obj-$(CONFIG_ANDROID)		+= android/
 obj-$(CONFIG_USB_WPAN_HCD)	+= ozwpan/
-obj-$(CONFIG_USB_G_CCG)		+= ccg/
 obj-$(CONFIG_WIMAX_GDM72XX)	+= gdm72xx/
 obj-$(CONFIG_CSR_WIFI)		+= csr/
-obj-$(CONFIG_OMAP_BANDGAP)	+= omap-thermal/
+obj-$(CONFIG_TI_SOC_THERMAL)	+= ti-soc-thermal/
 obj-$(CONFIG_NET_VENDOR_SILICOM)	+= silicom/
 obj-$(CONFIG_CED1401)		+= ced1401/
 obj-$(CONFIG_DRM_IMX)		+= imx-drm/
@@ -62,3 +61,4 @@ obj-$(CONFIG_SB105X)		+= sb105x/
 obj-$(CONFIG_FIREWIRE_SERIAL)	+= fwserial/
 obj-$(CONFIG_ZCACHE)		+= zcache/
 obj-$(CONFIG_GOLDFISH)		+= goldfish/
+obj-$(CONFIG_USB_DWC2)		+= dwc2/
diff --git a/drivers/staging/android/Kconfig b/drivers/staging/android/Kconfig
index 465a28c08f2..9f61d46da15 100644
--- a/drivers/staging/android/Kconfig
+++ b/drivers/staging/android/Kconfig
@@ -22,7 +22,7 @@ config ANDROID_BINDER_IPC
 config ASHMEM
 	bool "Enable the Anonymous Shared Memory Subsystem"
 	default n
-	depends on SHMEM || TINY_SHMEM
+	depends on SHMEM
 	---help---
 	  The ashmem subsystem is a new shared memory allocator, similar to
 	  POSIX SHM but with different behavior and sporting a simpler
@@ -72,6 +72,33 @@ config ANDROID_INTF_ALARM_DEV
 	  elapsed realtime, and a non-wakeup alarm on the monotonic clock.
 	  Also exports the alarm interface to user-space.
 
+config SYNC
+	bool "Synchronization framework"
+	default n
+	select ANON_INODES
+	help
+	  This option enables the framework for synchronization between multiple
+	  drivers.  Sync implementations can take advantage of hardware
+	  synchronization built into devices like GPUs.
+
+config SW_SYNC
+	bool "Software synchronization objects"
+	default n
+	depends on SYNC
+	help
+	  A sync object driver that uses a 32bit counter to coordinate
+	  syncrhronization.  Useful when there is no hardware primitive backing
+	  the synchronization.
+
+config SW_SYNC_USER
+	bool "Userspace API for SW_SYNC"
+	default n
+	depends on SW_SYNC
+	help
+	  Provides a user space API to the sw sync object.
+	  *WARNING* improper use of this can result in deadlocking kernel
+	  drivers from userspace.
+
 endif # if ANDROID
 
 endmenu
diff --git a/drivers/staging/android/Makefile b/drivers/staging/android/Makefile
index b35a631734d..c136299e05a 100644
--- a/drivers/staging/android/Makefile
+++ b/drivers/staging/android/Makefile
@@ -7,3 +7,5 @@ obj-$(CONFIG_ANDROID_TIMED_OUTPUT)	+= timed_output.o
 obj-$(CONFIG_ANDROID_TIMED_GPIO)	+= timed_gpio.o
 obj-$(CONFIG_ANDROID_LOW_MEMORY_KILLER)	+= lowmemorykiller.o
 obj-$(CONFIG_ANDROID_INTF_ALARM_DEV)	+= alarm-dev.o
+obj-$(CONFIG_SYNC)			+= sync.o
+obj-$(CONFIG_SW_SYNC)			+= sw_sync.o
diff --git a/drivers/staging/android/ashmem.c b/drivers/staging/android/ashmem.c
index 634b9ae713e..e681bdd9aa5 100644
--- a/drivers/staging/android/ashmem.c
+++ b/drivers/staging/android/ashmem.c
@@ -414,20 +414,29 @@ out:
 static int set_name(struct ashmem_area *asma, void __user *name)
 {
 	int ret = 0;
+	char local_name[ASHMEM_NAME_LEN];
 
-	mutex_lock(&ashmem_mutex);
+	/*
+	 * Holding the ashmem_mutex while doing a copy_from_user might cause
+	 * an data abort which would try to access mmap_sem. If another
+	 * thread has invoked ashmem_mmap then it will be holding the
+	 * semaphore and will be waiting for ashmem_mutex, there by leading to
+	 * deadlock. We'll release the mutex  and take the name to a local
+	 * variable that does not need protection and later copy the local
+	 * variable to the structure member with lock held.
+	 */
+	if (copy_from_user(local_name, name, ASHMEM_NAME_LEN))
+		return -EFAULT;
 
+	mutex_lock(&ashmem_mutex);
 	/* cannot change an existing mapping's name */
 	if (unlikely(asma->file)) {
 		ret = -EINVAL;
 		goto out;
 	}
-
-	if (unlikely(copy_from_user(asma->name + ASHMEM_NAME_PREFIX_LEN,
-				    name, ASHMEM_NAME_LEN)))
-		ret = -EFAULT;
+	memcpy(asma->name + ASHMEM_NAME_PREFIX_LEN,
+		local_name, ASHMEM_NAME_LEN);
 	asma->name[ASHMEM_FULL_NAME_LEN-1] = '\0';
-
 out:
 	mutex_unlock(&ashmem_mutex);
 
@@ -437,26 +446,36 @@ out:
 static int get_name(struct ashmem_area *asma, void __user *name)
 {
 	int ret = 0;
+	size_t len;
+	/*
+	 * Have a local variable to which we'll copy the content
+	 * from asma with the lock held. Later we can copy this to the user
+	 * space safely without holding any locks. So even if we proceed to
+	 * wait for mmap_sem, it won't lead to deadlock.
+	 */
+	char local_name[ASHMEM_NAME_LEN];
 
 	mutex_lock(&ashmem_mutex);
 	if (asma->name[ASHMEM_NAME_PREFIX_LEN] != '\0') {
-		size_t len;
 
 		/*
 		 * Copying only `len', instead of ASHMEM_NAME_LEN, bytes
 		 * prevents us from revealing one user's stack to another.
 		 */
 		len = strlen(asma->name + ASHMEM_NAME_PREFIX_LEN) + 1;
-		if (unlikely(copy_to_user(name,
-				asma->name + ASHMEM_NAME_PREFIX_LEN, len)))
-			ret = -EFAULT;
+		memcpy(local_name, asma->name + ASHMEM_NAME_PREFIX_LEN, len);
 	} else {
-		if (unlikely(copy_to_user(name, ASHMEM_NAME_DEF,
-					  sizeof(ASHMEM_NAME_DEF))))
-			ret = -EFAULT;
+		len = sizeof(ASHMEM_NAME_DEF);
+		memcpy(local_name, ASHMEM_NAME_DEF, len);
 	}
 	mutex_unlock(&ashmem_mutex);
 
+	/*
+	 * Now we are just copying from the stack variable to userland
+	 * No lock held
+	 */
+	if (unlikely(copy_to_user(name, local_name, len)))
+		ret = -EFAULT;
 	return ret;
 }
 
@@ -683,6 +702,23 @@ static long ashmem_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 	return ret;
 }
 
+/* support of 32bit userspace on 64bit platforms */
+#ifdef CONFIG_COMPAT
+static long compat_ashmem_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+
+	switch (cmd) {
+	case COMPAT_ASHMEM_SET_SIZE:
+		cmd = ASHMEM_SET_SIZE;
+		break;
+	case COMPAT_ASHMEM_SET_PROT_MASK:
+		cmd = ASHMEM_SET_PROT_MASK;
+		break;
+	}
+	return ashmem_ioctl(file, cmd, arg);
+}
+#endif
+
 static const struct file_operations ashmem_fops = {
 	.owner = THIS_MODULE,
 	.open = ashmem_open,
@@ -691,7 +727,9 @@ static const struct file_operations ashmem_fops = {
 	.llseek = ashmem_llseek,
 	.mmap = ashmem_mmap,
 	.unlocked_ioctl = ashmem_ioctl,
-	.compat_ioctl = ashmem_ioctl,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl = compat_ashmem_ioctl,
+#endif
 };
 
 static struct miscdevice ashmem_misc = {
diff --git a/drivers/staging/android/ashmem.h b/drivers/staging/android/ashmem.h
index 1976b10ef93..8dc0f0d3adf 100644
--- a/drivers/staging/android/ashmem.h
+++ b/drivers/staging/android/ashmem.h
@@ -14,6 +14,7 @@
 
 #include <linux/limits.h>
 #include <linux/ioctl.h>
+#include <linux/compat.h>
 
 #define ASHMEM_NAME_LEN		256
 
@@ -45,4 +46,10 @@ struct ashmem_pin {
 #define ASHMEM_GET_PIN_STATUS	_IO(__ASHMEMIOC, 9)
 #define ASHMEM_PURGE_ALL_CACHES	_IO(__ASHMEMIOC, 10)
 
+/* support of 32bit userspace on 64bit platforms */
+#ifdef CONFIG_COMPAT
+#define COMPAT_ASHMEM_SET_SIZE		_IOW(__ASHMEMIOC, 3, compat_size_t)
+#define COMPAT_ASHMEM_SET_PROT_MASK	_IOW(__ASHMEMIOC, 5, unsigned int)
+#endif
+
 #endif	/* _LINUX_ASHMEM_H */
diff --git a/drivers/staging/android/binder.c b/drivers/staging/android/binder.c
index 24456a0de6b..1567ac296b3 100644
--- a/drivers/staging/android/binder.c
+++ b/drivers/staging/android/binder.c
@@ -2878,82 +2878,109 @@ static int binder_release(struct inode *nodp, struct file *filp)
 	return 0;
 }
 
+static int binder_node_release(struct binder_node *node, int refs)
+{
+	struct binder_ref *ref;
+	int death = 0;
+
+	list_del_init(&node->work.entry);
+	binder_release_work(&node->async_todo);
+
+	if (hlist_empty(&node->refs)) {
+		kfree(node);
+		binder_stats_deleted(BINDER_STAT_NODE);
+
+		return refs;
+	}
+
+	node->proc = NULL;
+	node->local_strong_refs = 0;
+	node->local_weak_refs = 0;
+	hlist_add_head(&node->dead_node, &binder_dead_nodes);
+
+	hlist_for_each_entry(ref, &node->refs, node_entry) {
+		refs++;
+
+		if (!ref->death)
+			goto out;
+
+		death++;
+
+		if (list_empty(&ref->death->work.entry)) {
+			ref->death->work.type = BINDER_WORK_DEAD_BINDER;
+			list_add_tail(&ref->death->work.entry,
+				      &ref->proc->todo);
+			wake_up_interruptible(&ref->proc->wait);
+		} else
+			BUG();
+	}
+
+out:
+	binder_debug(BINDER_DEBUG_DEAD_BINDER,
+		     "node %d now dead, refs %d, death %d\n",
+		     node->debug_id, refs, death);
+
+	return refs;
+}
+
 static void binder_deferred_release(struct binder_proc *proc)
 {
 	struct binder_transaction *t;
 	struct rb_node *n;
-	int threads, nodes, incoming_refs, outgoing_refs, buffers, active_transactions, page_count;
+	int threads, nodes, incoming_refs, outgoing_refs, buffers,
+		active_transactions, page_count;
 
 	BUG_ON(proc->vma);
 	BUG_ON(proc->files);
 
 	hlist_del(&proc->proc_node);
+
 	if (binder_context_mgr_node && binder_context_mgr_node->proc == proc) {
 		binder_debug(BINDER_DEBUG_DEAD_BINDER,
-			     "binder_release: %d context_mgr_node gone\n",
-			     proc->pid);
+			     "%s: %d context_mgr_node gone\n",
+			     __func__, proc->pid);
 		binder_context_mgr_node = NULL;
 	}
 
 	threads = 0;
 	active_transactions = 0;
 	while ((n = rb_first(&proc->threads))) {
-		struct binder_thread *thread = rb_entry(n, struct binder_thread, rb_node);
+		struct binder_thread *thread;
+
+		thread = rb_entry(n, struct binder_thread, rb_node);
 		threads++;
 		active_transactions += binder_free_thread(proc, thread);
 	}
+
 	nodes = 0;
 	incoming_refs = 0;
 	while ((n = rb_first(&proc->nodes))) {
-		struct binder_node *node = rb_entry(n, struct binder_node, rb_node);
+		struct binder_node *node;
 
+		node = rb_entry(n, struct binder_node, rb_node);
 		nodes++;
 		rb_erase(&node->rb_node, &proc->nodes);
-		list_del_init(&node->work.entry);
-		binder_release_work(&node->async_todo);
-		if (hlist_empty(&node->refs)) {
-			kfree(node);
-			binder_stats_deleted(BINDER_STAT_NODE);
-		} else {
-			struct binder_ref *ref;
-			int death = 0;
-
-			node->proc = NULL;
-			node->local_strong_refs = 0;
-			node->local_weak_refs = 0;
-			hlist_add_head(&node->dead_node, &binder_dead_nodes);
-
-			hlist_for_each_entry(ref, &node->refs, node_entry) {
-				incoming_refs++;
-				if (ref->death) {
-					death++;
-					if (list_empty(&ref->death->work.entry)) {
-						ref->death->work.type = BINDER_WORK_DEAD_BINDER;
-						list_add_tail(&ref->death->work.entry, &ref->proc->todo);
-						wake_up_interruptible(&ref->proc->wait);
-					} else
-						BUG();
-				}
-			}
-			binder_debug(BINDER_DEBUG_DEAD_BINDER,
-				     "node %d now dead, refs %d, death %d\n",
-				      node->debug_id, incoming_refs, death);
-		}
+		incoming_refs = binder_node_release(node, incoming_refs);
 	}
+
 	outgoing_refs = 0;
 	while ((n = rb_first(&proc->refs_by_desc))) {
-		struct binder_ref *ref = rb_entry(n, struct binder_ref,
-						  rb_node_desc);
+		struct binder_ref *ref;
+
+		ref = rb_entry(n, struct binder_ref, rb_node_desc);
 		outgoing_refs++;
 		binder_delete_ref(ref);
 	}
+
 	binder_release_work(&proc->todo);
 	binder_release_work(&proc->delivered_death);
-	buffers = 0;
 
+	buffers = 0;
 	while ((n = rb_first(&proc->allocated_buffers))) {
-		struct binder_buffer *buffer = rb_entry(n, struct binder_buffer,
-							rb_node);
+		struct binder_buffer *buffer;
+
+		buffer = rb_entry(n, struct binder_buffer, rb_node);
+
 		t = buffer->transaction;
 		if (t) {
 			t->buffer = NULL;
@@ -2962,6 +2989,7 @@ static void binder_deferred_release(struct binder_proc *proc)
 			       proc->pid, t->debug_id);
 			/*BUG();*/
 		}
+
 		binder_free_buf(proc, buffer);
 		buffers++;
 	}
@@ -2971,18 +2999,20 @@ static void binder_deferred_release(struct binder_proc *proc)
 	page_count = 0;
 	if (proc->pages) {
 		int i;
+
 		for (i = 0; i < proc->buffer_size / PAGE_SIZE; i++) {
-			if (proc->pages[i]) {
-				void *page_addr = proc->buffer + i * PAGE_SIZE;
-				binder_debug(BINDER_DEBUG_BUFFER_ALLOC,
-					     "binder_release: %d: page %d at %p not freed\n",
-					     proc->pid, i,
-					     page_addr);
-				unmap_kernel_range((unsigned long)page_addr,
-					PAGE_SIZE);
-				__free_page(proc->pages[i]);
-				page_count++;
-			}
+			void *page_addr;
+
+			if (!proc->pages[i])
+				continue;
+
+			page_addr = proc->buffer + i * PAGE_SIZE;
+			binder_debug(BINDER_DEBUG_BUFFER_ALLOC,
+				     "%s: %d: page %d at %p not freed\n",
+				     __func__, proc->pid, i, page_addr);
+			unmap_kernel_range((unsigned long)page_addr, PAGE_SIZE);
+			__free_page(proc->pages[i]);
+			page_count++;
 		}
 		kfree(proc->pages);
 		vfree(proc->buffer);
@@ -2991,9 +3021,9 @@ static void binder_deferred_release(struct binder_proc *proc)
 	put_task_struct(proc->tsk);
 
 	binder_debug(BINDER_DEBUG_OPEN_CLOSE,
-		     "binder_release: %d threads %d, nodes %d (ref %d), refs %d, active transactions %d, buffers %d, pages %d\n",
-		     proc->pid, threads, nodes, incoming_refs, outgoing_refs,
-		     active_transactions, buffers, page_count);
+		     "%s: %d threads %d, nodes %d (ref %d), refs %d, active transactions %d, buffers %d, pages %d\n",
+		     __func__, proc->pid, threads, nodes, incoming_refs,
+		     outgoing_refs, active_transactions, buffers, page_count);
 
 	kfree(proc);
 }
diff --git a/drivers/staging/android/binder.h b/drivers/staging/android/binder.h
index f240464effd..dbe81ceca1b 100644
--- a/drivers/staging/android/binder.h
+++ b/drivers/staging/android/binder.h
@@ -85,11 +85,11 @@ struct binder_version {
 #define BINDER_CURRENT_PROTOCOL_VERSION 7
 
 #define BINDER_WRITE_READ		_IOWR('b', 1, struct binder_write_read)
-#define	BINDER_SET_IDLE_TIMEOUT		_IOW('b', 3, int64_t)
+#define	BINDER_SET_IDLE_TIMEOUT		_IOW('b', 3, __s64)
 #define	BINDER_SET_MAX_THREADS		_IOW('b', 5, size_t)
-#define	BINDER_SET_IDLE_PRIORITY	_IOW('b', 6, int)
-#define	BINDER_SET_CONTEXT_MGR		_IOW('b', 7, int)
-#define	BINDER_THREAD_EXIT		_IOW('b', 8, int)
+#define	BINDER_SET_IDLE_PRIORITY	_IOW('b', 6, __s32)
+#define	BINDER_SET_CONTEXT_MGR		_IOW('b', 7, __s32)
+#define	BINDER_THREAD_EXIT		_IOW('b', 8, __s32)
 #define BINDER_VERSION			_IOWR('b', 9, struct binder_version)
 
 /*
diff --git a/drivers/staging/android/logger.c b/drivers/staging/android/logger.c
index dbc63cbb4d3..b14a5574255 100644
--- a/drivers/staging/android/logger.c
+++ b/drivers/staging/android/logger.c
@@ -68,6 +68,8 @@ static LIST_HEAD(log_list);
  * @log:	The associated log
  * @list:	The associated entry in @logger_log's list
  * @r_off:	The current read head offset.
+ * @r_all:	Reader can read all entries
+ * @r_ver:	Reader ABI version
  *
  * This object lives from open to release, so we don't need additional
  * reference counting. The structure is protected by log->mutex.
@@ -76,6 +78,8 @@ struct logger_reader {
 	struct logger_log	*log;
 	struct list_head	list;
 	size_t			r_off;
+	bool			r_all;
+	int			r_ver;
 };
 
 /* logger_offset - returns index 'n' into the log via (optimized) modulus */
@@ -109,8 +113,29 @@ static inline struct logger_log *file_get_log(struct file *file)
 }
 
 /*
- * get_entry_len - Grabs the length of the payload of the next entry starting
- * from 'off'.
+ * get_entry_header - returns a pointer to the logger_entry header within
+ * 'log' starting at offset 'off'. A temporary logger_entry 'scratch' must
+ * be provided. Typically the return value will be a pointer within
+ * 'logger->buf'.  However, a pointer to 'scratch' may be returned if
+ * the log entry spans the end and beginning of the circular buffer.
+ */
+static struct logger_entry *get_entry_header(struct logger_log *log,
+		size_t off, struct logger_entry *scratch)
+{
+	size_t len = min(sizeof(struct logger_entry), log->size - off);
+	if (len != sizeof(struct logger_entry)) {
+		memcpy(((void *) scratch), log->buffer + off, len);
+		memcpy(((void *) scratch) + len, log->buffer,
+			sizeof(struct logger_entry) - len);
+		return scratch;
+	}
+
+	return (struct logger_entry *) (log->buffer + off);
+}
+
+/*
+ * get_entry_msg_len - Grabs the length of the message of the entry
+ * starting from from 'off'.
  *
  * An entry length is 2 bytes (16 bits) in host endian order.
  * In the log, the length does not include the size of the log entry structure.
@@ -118,20 +143,45 @@ static inline struct logger_log *file_get_log(struct file *file)
  *
  * Caller needs to hold log->mutex.
  */
-static __u32 get_entry_len(struct logger_log *log, size_t off)
+static __u32 get_entry_msg_len(struct logger_log *log, size_t off)
 {
-	__u16 val;
+	struct logger_entry scratch;
+	struct logger_entry *entry;
 
-	/* copy 2 bytes from buffer, in memcpy order, */
-	/* handling possible wrap at end of buffer */
+	entry = get_entry_header(log, off, &scratch);
+	return entry->len;
+}
 
-	((__u8 *)&val)[0] = log->buffer[off];
-	if (likely(off+1 < log->size))
-		((__u8 *)&val)[1] = log->buffer[off+1];
+static size_t get_user_hdr_len(int ver)
+{
+	if (ver < 2)
+		return sizeof(struct user_logger_entry_compat);
 	else
-		((__u8 *)&val)[1] = log->buffer[0];
+		return sizeof(struct logger_entry);
+}
 
-	return sizeof(struct logger_entry) + val;
+static ssize_t copy_header_to_user(int ver, struct logger_entry *entry,
+					 char __user *buf)
+{
+	void *hdr;
+	size_t hdr_len;
+	struct user_logger_entry_compat v1;
+
+	if (ver < 2) {
+		v1.len      = entry->len;
+		v1.__pad    = 0;
+		v1.pid      = entry->pid;
+		v1.tid      = entry->tid;
+		v1.sec      = entry->sec;
+		v1.nsec     = entry->nsec;
+		hdr         = &v1;
+		hdr_len     = sizeof(struct user_logger_entry_compat);
+	} else {
+		hdr         = entry;
+		hdr_len     = sizeof(struct logger_entry);
+	}
+
+	return copy_to_user(buf, hdr, hdr_len);
 }
 
 /*
@@ -145,15 +195,31 @@ static ssize_t do_read_log_to_user(struct logger_log *log,
 				   char __user *buf,
 				   size_t count)
 {
+	struct logger_entry scratch;
+	struct logger_entry *entry;
 	size_t len;
+	size_t msg_start;
 
 	/*
-	 * We read from the log in two disjoint operations. First, we read from
-	 * the current read head offset up to 'count' bytes or to the end of
+	 * First, copy the header to userspace, using the version of
+	 * the header requested
+	 */
+	entry = get_entry_header(log, reader->r_off, &scratch);
+	if (copy_header_to_user(reader->r_ver, entry, buf))
+		return -EFAULT;
+
+	count -= get_user_hdr_len(reader->r_ver);
+	buf += get_user_hdr_len(reader->r_ver);
+	msg_start = logger_offset(log,
+		reader->r_off + sizeof(struct logger_entry));
+
+	/*
+	 * We read from the msg in two disjoint operations. First, we read from
+	 * the current msg head offset up to 'count' bytes or to the end of
 	 * the log, whichever comes first.
 	 */
-	len = min(count, log->size - reader->r_off);
-	if (copy_to_user(buf, log->buffer + reader->r_off, len))
+	len = min(count, log->size - msg_start);
+	if (copy_to_user(buf, log->buffer + msg_start, len))
 		return -EFAULT;
 
 	/*
@@ -164,9 +230,34 @@ static ssize_t do_read_log_to_user(struct logger_log *log,
 		if (copy_to_user(buf + len, log->buffer, count - len))
 			return -EFAULT;
 
-	reader->r_off = logger_offset(log, reader->r_off + count);
+	reader->r_off = logger_offset(log, reader->r_off +
+		sizeof(struct logger_entry) + count);
 
-	return count;
+	return count + get_user_hdr_len(reader->r_ver);
+}
+
+/*
+ * get_next_entry_by_uid - Starting at 'off', returns an offset into
+ * 'log->buffer' which contains the first entry readable by 'euid'
+ */
+static size_t get_next_entry_by_uid(struct logger_log *log,
+		size_t off, uid_t euid)
+{
+	while (off != log->w_off) {
+		struct logger_entry *entry;
+		struct logger_entry scratch;
+		size_t next_len;
+
+		entry = get_entry_header(log, off, &scratch);
+
+		if (entry->euid == euid)
+			return off;
+
+		next_len = sizeof(struct logger_entry) + entry->len;
+		off = logger_offset(log, off + next_len);
+	}
+
+	return off;
 }
 
 /*
@@ -178,7 +269,7 @@ static ssize_t do_read_log_to_user(struct logger_log *log,
  *	- If there are no log entries to read, blocks until log is written to
  *	- Atomically reads exactly one log entry
  *
- * Optimal read size is LOGGER_ENTRY_MAX_LEN. Will set errno to EINVAL if read
+ * Will set errno to EINVAL if read
  * buffer is insufficient to hold next entry.
  */
 static ssize_t logger_read(struct file *file, char __user *buf,
@@ -219,6 +310,10 @@ start:
 
 	mutex_lock(&log->mutex);
 
+	if (!reader->r_all)
+		reader->r_off = get_next_entry_by_uid(log,
+			reader->r_off, current_euid());
+
 	/* is there still something to read or did we race? */
 	if (unlikely(log->w_off == reader->r_off)) {
 		mutex_unlock(&log->mutex);
@@ -226,7 +321,8 @@ start:
 	}
 
 	/* get the size of the next entry */
-	ret = get_entry_len(log, reader->r_off);
+	ret = get_user_hdr_len(reader->r_ver) +
+		get_entry_msg_len(log, reader->r_off);
 	if (count < ret) {
 		ret = -EINVAL;
 		goto out;
@@ -252,7 +348,8 @@ static size_t get_next_entry(struct logger_log *log, size_t off, size_t len)
 	size_t count = 0;
 
 	do {
-		size_t nr = get_entry_len(log, off);
+		size_t nr = sizeof(struct logger_entry) +
+			get_entry_msg_len(log, off);
 		off = logger_offset(log, off + nr);
 		count += nr;
 	} while (count < len);
@@ -382,7 +479,9 @@ static ssize_t logger_aio_write(struct kiocb *iocb, const struct iovec *iov,
 	header.tid = current->pid;
 	header.sec = now.tv_sec;
 	header.nsec = now.tv_nsec;
+	header.euid = current_euid();
 	header.len = min_t(size_t, iocb->ki_left, LOGGER_ENTRY_MAX_PAYLOAD);
+	header.hdr_size = sizeof(struct logger_entry);
 
 	/* null writes succeed, return zero */
 	if (unlikely(!header.len))
@@ -463,6 +562,10 @@ static int logger_open(struct inode *inode, struct file *file)
 			return -ENOMEM;
 
 		reader->log = log;
+		reader->r_ver = 1;
+		reader->r_all = in_egroup_p(inode->i_gid) ||
+			capable(CAP_SYSLOG);
+
 		INIT_LIST_HEAD(&reader->list);
 
 		mutex_lock(&log->mutex);
@@ -522,6 +625,10 @@ static unsigned int logger_poll(struct file *file, poll_table *wait)
 	poll_wait(file, &log->wq, wait);
 
 	mutex_lock(&log->mutex);
+	if (!reader->r_all)
+		reader->r_off = get_next_entry_by_uid(log,
+			reader->r_off, current_euid());
+
 	if (log->w_off != reader->r_off)
 		ret |= POLLIN | POLLRDNORM;
 	mutex_unlock(&log->mutex);
@@ -529,11 +636,25 @@ static unsigned int logger_poll(struct file *file, poll_table *wait)
 	return ret;
 }
 
+static long logger_set_version(struct logger_reader *reader, void __user *arg)
+{
+	int version;
+	if (copy_from_user(&version, arg, sizeof(int)))
+		return -EFAULT;
+
+	if ((version < 1) || (version > 2))
+		return -EINVAL;
+
+	reader->r_ver = version;
+	return 0;
+}
+
 static long logger_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 {
 	struct logger_log *log = file_get_log(file);
 	struct logger_reader *reader;
-	long ret = -ENOTTY;
+	long ret = -EINVAL;
+	void __user *argp = (void __user *) arg;
 
 	mutex_lock(&log->mutex);
 
@@ -558,8 +679,14 @@ static long logger_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 			break;
 		}
 		reader = file->private_data;
+
+		if (!reader->r_all)
+			reader->r_off = get_next_entry_by_uid(log,
+				reader->r_off, current_euid());
+
 		if (log->w_off != reader->r_off)
-			ret = get_entry_len(log, reader->r_off);
+			ret = get_user_hdr_len(reader->r_ver) +
+				get_entry_msg_len(log, reader->r_off);
 		else
 			ret = 0;
 		break;
@@ -568,11 +695,32 @@ static long logger_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 			ret = -EBADF;
 			break;
 		}
+		if (!(in_egroup_p(file->f_dentry->d_inode->i_gid) ||
+				capable(CAP_SYSLOG))) {
+			ret = -EPERM;
+			break;
+		}
 		list_for_each_entry(reader, &log->readers, list)
 			reader->r_off = log->w_off;
 		log->head = log->w_off;
 		ret = 0;
 		break;
+	case LOGGER_GET_VERSION:
+		if (!(file->f_mode & FMODE_READ)) {
+			ret = -EBADF;
+			break;
+		}
+		reader = file->private_data;
+		ret = reader->r_ver;
+		break;
+	case LOGGER_SET_VERSION:
+		if (!(file->f_mode & FMODE_READ)) {
+			ret = -EBADF;
+			break;
+		}
+		reader = file->private_data;
+		ret = logger_set_version(reader, argp);
+		break;
 	}
 
 	mutex_unlock(&log->mutex);
@@ -592,8 +740,8 @@ static const struct file_operations logger_fops = {
 };
 
 /*
- * Log size must be a power of two, greater than LOGGER_ENTRY_MAX_LEN,
- * and less than LONG_MAX minus LOGGER_ENTRY_MAX_LEN.
+ * Log size must must be a power of two, and greater than
+ * (LOGGER_ENTRY_MAX_PAYLOAD + sizeof(struct logger_entry)).
  */
 static int __init create_log(char *log_name, int size)
 {
diff --git a/drivers/staging/android/logger.h b/drivers/staging/android/logger.h
index 9b929a8c746..cc6bbd99c8e 100644
--- a/drivers/staging/android/logger.h
+++ b/drivers/staging/android/logger.h
@@ -21,7 +21,7 @@
 #include <linux/ioctl.h>
 
 /**
- * struct logger_entry - defines a single entry that is given to a logger
+ * struct user_logger_entry_compat - defines a single entry that is given to a logger
  * @len:	The length of the payload
  * @__pad:	Two bytes of padding that appear to be required
  * @pid:	The generating process' process ID
@@ -29,8 +29,12 @@
  * @sec:	The number of seconds that have elapsed since the Epoch
  * @nsec:	The number of nanoseconds that have elapsed since @sec
  * @msg:	The message that is to be logged
+ *
+ * The userspace structure for version 1 of the logger_entry ABI.
+ * This structure is returned to userspace unless the caller requests
+ * an upgrade to a newer ABI version.
  */
-struct logger_entry {
+struct user_logger_entry_compat {
 	__u16		len;
 	__u16		__pad;
 	__s32		pid;
@@ -40,14 +44,38 @@ struct logger_entry {
 	char		msg[0];
 };
 
+/**
+ * struct logger_entry - defines a single entry that is given to a logger
+ * @len:	The length of the payload
+ * @hdr_size:	sizeof(struct logger_entry_v2)
+ * @pid:	The generating process' process ID
+ * @tid:	The generating process' thread ID
+ * @sec:	The number of seconds that have elapsed since the Epoch
+ * @nsec:	The number of nanoseconds that have elapsed since @sec
+ * @euid:	Effective UID of logger
+ * @msg:	The message that is to be logged
+ *
+ * The structure for version 2 of the logger_entry ABI.
+ * This structure is returned to userspace if ioctl(LOGGER_SET_VERSION)
+ * is called with version >= 2
+ */
+struct logger_entry {
+	__u16		len;
+	__u16		hdr_size;
+	__s32		pid;
+	__s32		tid;
+	__s32		sec;
+	__s32		nsec;
+	uid_t		euid;
+	char		msg[0];
+};
+
 #define LOGGER_LOG_RADIO	"log_radio"	/* radio-related messages */
 #define LOGGER_LOG_EVENTS	"log_events"	/* system/hardware events */
 #define LOGGER_LOG_SYSTEM	"log_system"	/* system/framework messages */
 #define LOGGER_LOG_MAIN		"log_main"	/* everything else */
 
-#define LOGGER_ENTRY_MAX_LEN		(4*1024)
-#define LOGGER_ENTRY_MAX_PAYLOAD	\
-	(LOGGER_ENTRY_MAX_LEN - sizeof(struct logger_entry))
+#define LOGGER_ENTRY_MAX_PAYLOAD	4076
 
 #define __LOGGERIO	0xAE
 
@@ -55,5 +83,7 @@ struct logger_entry {
 #define LOGGER_GET_LOG_LEN		_IO(__LOGGERIO, 2) /* used log len */
 #define LOGGER_GET_NEXT_ENTRY_LEN	_IO(__LOGGERIO, 3) /* next entry len */
 #define LOGGER_FLUSH_LOG		_IO(__LOGGERIO, 4) /* flush log */
+#define LOGGER_GET_VERSION		_IO(__LOGGERIO, 5) /* abi version */
+#define LOGGER_SET_VERSION		_IO(__LOGGERIO, 6) /* abi version */
 
 #endif /* _LINUX_LOGGER_H */
diff --git a/drivers/staging/android/lowmemorykiller.c b/drivers/staging/android/lowmemorykiller.c
index 3b91b0fd4de..fe74494868e 100644
--- a/drivers/staging/android/lowmemorykiller.c
+++ b/drivers/staging/android/lowmemorykiller.c
@@ -30,16 +30,19 @@
  *
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/mm.h>
 #include <linux/oom.h>
 #include <linux/sched.h>
+#include <linux/swap.h>
 #include <linux/rcupdate.h>
 #include <linux/profile.h>
 #include <linux/notifier.h>
 
-static uint32_t lowmem_debug_level = 2;
+static uint32_t lowmem_debug_level = 1;
 static short lowmem_adj[6] = {
 	0,
 	1,
@@ -60,7 +63,7 @@ static unsigned long lowmem_deathpending_timeout;
 #define lowmem_print(level, x...)			\
 	do {						\
 		if (lowmem_debug_level >= (level))	\
-			printk(x);			\
+			pr_info(x);			\
 	} while (0)
 
 static int lowmem_shrink(struct shrinker *s, struct shrink_control *sc)
@@ -74,7 +77,7 @@ static int lowmem_shrink(struct shrinker *s, struct shrink_control *sc)
 	int selected_tasksize = 0;
 	short selected_oom_score_adj;
 	int array_size = ARRAY_SIZE(lowmem_adj);
-	int other_free = global_page_state(NR_FREE_PAGES);
+	int other_free = global_page_state(NR_FREE_PAGES) - totalreserve_pages;
 	int other_file = global_page_state(NR_FILE_PAGES) -
 						global_page_state(NR_SHMEM);
 
diff --git a/drivers/staging/android/sw_sync.c b/drivers/staging/android/sw_sync.c
new file mode 100644
index 00000000000..4928f93bdf3
--- /dev/null
+++ b/drivers/staging/android/sw_sync.c
@@ -0,0 +1,264 @@
+/*
+ * drivers/base/sw_sync.c
+ *
+ * Copyright (C) 2012 Google, Inc.
+ *
+ * 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/export.h>
+#include <linux/file.h>
+#include <linux/fs.h>
+#include <linux/miscdevice.h>
+#include <linux/module.h>
+#include <linux/syscalls.h>
+#include <linux/uaccess.h>
+
+#include "sw_sync.h"
+
+static int sw_sync_cmp(u32 a, u32 b)
+{
+	if (a == b)
+		return 0;
+
+	return ((s32)a - (s32)b) < 0 ? -1 : 1;
+}
+
+struct sync_pt *sw_sync_pt_create(struct sw_sync_timeline *obj, u32 value)
+{
+	struct sw_sync_pt *pt;
+
+	pt = (struct sw_sync_pt *)
+		sync_pt_create(&obj->obj, sizeof(struct sw_sync_pt));
+
+	pt->value = value;
+
+	return (struct sync_pt *)pt;
+}
+EXPORT_SYMBOL(sw_sync_pt_create);
+
+static struct sync_pt *sw_sync_pt_dup(struct sync_pt *sync_pt)
+{
+	struct sw_sync_pt *pt = (struct sw_sync_pt *) sync_pt;
+	struct sw_sync_timeline *obj =
+		(struct sw_sync_timeline *)sync_pt->parent;
+
+	return (struct sync_pt *) sw_sync_pt_create(obj, pt->value);
+}
+
+static int sw_sync_pt_has_signaled(struct sync_pt *sync_pt)
+{
+	struct sw_sync_pt *pt = (struct sw_sync_pt *)sync_pt;
+	struct sw_sync_timeline *obj =
+		(struct sw_sync_timeline *)sync_pt->parent;
+
+	return sw_sync_cmp(obj->value, pt->value) >= 0;
+}
+
+static int sw_sync_pt_compare(struct sync_pt *a, struct sync_pt *b)
+{
+	struct sw_sync_pt *pt_a = (struct sw_sync_pt *)a;
+	struct sw_sync_pt *pt_b = (struct sw_sync_pt *)b;
+
+	return sw_sync_cmp(pt_a->value, pt_b->value);
+}
+
+static int sw_sync_fill_driver_data(struct sync_pt *sync_pt,
+				    void *data, int size)
+{
+	struct sw_sync_pt *pt = (struct sw_sync_pt *)sync_pt;
+
+	if (size < sizeof(pt->value))
+		return -ENOMEM;
+
+	memcpy(data, &pt->value, sizeof(pt->value));
+
+	return sizeof(pt->value);
+}
+
+static void sw_sync_timeline_value_str(struct sync_timeline *sync_timeline,
+				       char *str, int size)
+{
+	struct sw_sync_timeline *timeline =
+		(struct sw_sync_timeline *)sync_timeline;
+	snprintf(str, size, "%d", timeline->value);
+}
+
+static void sw_sync_pt_value_str(struct sync_pt *sync_pt,
+				       char *str, int size)
+{
+	struct sw_sync_pt *pt = (struct sw_sync_pt *)sync_pt;
+	snprintf(str, size, "%d", pt->value);
+}
+
+static struct sync_timeline_ops sw_sync_timeline_ops = {
+	.driver_name = "sw_sync",
+	.dup = sw_sync_pt_dup,
+	.has_signaled = sw_sync_pt_has_signaled,
+	.compare = sw_sync_pt_compare,
+	.fill_driver_data = sw_sync_fill_driver_data,
+	.timeline_value_str = sw_sync_timeline_value_str,
+	.pt_value_str = sw_sync_pt_value_str,
+};
+
+
+struct sw_sync_timeline *sw_sync_timeline_create(const char *name)
+{
+	struct sw_sync_timeline *obj = (struct sw_sync_timeline *)
+		sync_timeline_create(&sw_sync_timeline_ops,
+				     sizeof(struct sw_sync_timeline),
+				     name);
+
+	return obj;
+}
+EXPORT_SYMBOL(sw_sync_timeline_create);
+
+void sw_sync_timeline_inc(struct sw_sync_timeline *obj, u32 inc)
+{
+	obj->value += inc;
+
+	sync_timeline_signal(&obj->obj);
+}
+EXPORT_SYMBOL(sw_sync_timeline_inc);
+
+#ifdef CONFIG_SW_SYNC_USER
+/* *WARNING*
+ *
+ * improper use of this can result in deadlocking kernel drivers from userspace.
+ */
+
+/* opening sw_sync create a new sync obj */
+static int sw_sync_open(struct inode *inode, struct file *file)
+{
+	struct sw_sync_timeline *obj;
+	char task_comm[TASK_COMM_LEN];
+
+	get_task_comm(task_comm, current);
+
+	obj = sw_sync_timeline_create(task_comm);
+	if (obj == NULL)
+		return -ENOMEM;
+
+	file->private_data = obj;
+
+	return 0;
+}
+
+static int sw_sync_release(struct inode *inode, struct file *file)
+{
+	struct sw_sync_timeline *obj = file->private_data;
+	sync_timeline_destroy(&obj->obj);
+	return 0;
+}
+
+static long sw_sync_ioctl_create_fence(struct sw_sync_timeline *obj, unsigned long arg)
+{
+	int fd = get_unused_fd();
+	int err;
+	struct sync_pt *pt;
+	struct sync_fence *fence;
+	struct sw_sync_create_fence_data data;
+
+	if (fd < 0)
+		return fd;
+
+	if (copy_from_user(&data, (void __user *)arg, sizeof(data))) {
+		err = -EFAULT;
+		goto err;
+	}
+
+	pt = sw_sync_pt_create(obj, data.value);
+	if (pt == NULL) {
+		err = -ENOMEM;
+		goto err;
+	}
+
+	data.name[sizeof(data.name) - 1] = '\0';
+	fence = sync_fence_create(data.name, pt);
+	if (fence == NULL) {
+		sync_pt_free(pt);
+		err = -ENOMEM;
+		goto err;
+	}
+
+	data.fence = fd;
+	if (copy_to_user((void __user *)arg, &data, sizeof(data))) {
+		sync_fence_put(fence);
+		err = -EFAULT;
+		goto err;
+	}
+
+	sync_fence_install(fence, fd);
+
+	return 0;
+
+err:
+	put_unused_fd(fd);
+	return err;
+}
+
+static long sw_sync_ioctl_inc(struct sw_sync_timeline *obj, unsigned long arg)
+{
+	u32 value;
+
+	if (copy_from_user(&value, (void __user *)arg, sizeof(value)))
+		return -EFAULT;
+
+	sw_sync_timeline_inc(obj, value);
+
+	return 0;
+}
+
+static long sw_sync_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	struct sw_sync_timeline *obj = file->private_data;
+
+	switch (cmd) {
+	case SW_SYNC_IOC_CREATE_FENCE:
+		return sw_sync_ioctl_create_fence(obj, arg);
+
+	case SW_SYNC_IOC_INC:
+		return sw_sync_ioctl_inc(obj, arg);
+
+	default:
+		return -ENOTTY;
+	}
+}
+
+static const struct file_operations sw_sync_fops = {
+	.owner = THIS_MODULE,
+	.open = sw_sync_open,
+	.release = sw_sync_release,
+	.unlocked_ioctl = sw_sync_ioctl,
+	.compat_ioctl = sw_sync_ioctl,
+};
+
+static struct miscdevice sw_sync_dev = {
+	.minor	= MISC_DYNAMIC_MINOR,
+	.name	= "sw_sync",
+	.fops	= &sw_sync_fops,
+};
+
+static int __init sw_sync_device_init(void)
+{
+	return misc_register(&sw_sync_dev);
+}
+
+static void __exit sw_sync_device_remove(void)
+{
+	misc_deregister(&sw_sync_dev);
+}
+
+module_init(sw_sync_device_init);
+module_exit(sw_sync_device_remove);
+
+#endif /* CONFIG_SW_SYNC_USER */
diff --git a/drivers/staging/android/sw_sync.h b/drivers/staging/android/sw_sync.h
new file mode 100644
index 00000000000..585040be5f1
--- /dev/null
+++ b/drivers/staging/android/sw_sync.h
@@ -0,0 +1,58 @@
+/*
+ * include/linux/sw_sync.h
+ *
+ * Copyright (C) 2012 Google, Inc.
+ *
+ * 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.
+ *
+ */
+
+#ifndef _LINUX_SW_SYNC_H
+#define _LINUX_SW_SYNC_H
+
+#include <linux/types.h>
+
+#ifdef __KERNEL__
+
+#include "sync.h"
+
+struct sw_sync_timeline {
+	struct	sync_timeline	obj;
+
+	u32			value;
+};
+
+struct sw_sync_pt {
+	struct sync_pt		pt;
+
+	u32			value;
+};
+
+struct sw_sync_timeline *sw_sync_timeline_create(const char *name);
+void sw_sync_timeline_inc(struct sw_sync_timeline *obj, u32 inc);
+
+struct sync_pt *sw_sync_pt_create(struct sw_sync_timeline *obj, u32 value);
+
+#endif /* __KERNEL __ */
+
+struct sw_sync_create_fence_data {
+	__u32	value;
+	char	name[32];
+	__s32	fence; /* fd of new fence */
+};
+
+#define SW_SYNC_IOC_MAGIC	'W'
+
+#define SW_SYNC_IOC_CREATE_FENCE	_IOWR(SW_SYNC_IOC_MAGIC, 0,\
+		struct sw_sync_create_fence_data)
+#define SW_SYNC_IOC_INC			_IOW(SW_SYNC_IOC_MAGIC, 1, __u32)
+
+
+#endif /* _LINUX_SW_SYNC_H */
diff --git a/drivers/staging/android/sync.c b/drivers/staging/android/sync.c
new file mode 100644
index 00000000000..3893a357476
--- /dev/null
+++ b/drivers/staging/android/sync.c
@@ -0,0 +1,1017 @@
+/*
+ * drivers/base/sync.c
+ *
+ * Copyright (C) 2012 Google, Inc.
+ *
+ * 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/debugfs.h>
+#include <linux/export.h>
+#include <linux/file.h>
+#include <linux/fs.h>
+#include <linux/kernel.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+#include <linux/seq_file.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/anon_inodes.h>
+
+#include "sync.h"
+
+#define CREATE_TRACE_POINTS
+#include "trace/sync.h"
+
+static void sync_fence_signal_pt(struct sync_pt *pt);
+static int _sync_pt_has_signaled(struct sync_pt *pt);
+static void sync_fence_free(struct kref *kref);
+static void sync_dump(void);
+
+static LIST_HEAD(sync_timeline_list_head);
+static DEFINE_SPINLOCK(sync_timeline_list_lock);
+
+static LIST_HEAD(sync_fence_list_head);
+static DEFINE_SPINLOCK(sync_fence_list_lock);
+
+struct sync_timeline *sync_timeline_create(const struct sync_timeline_ops *ops,
+					   int size, const char *name)
+{
+	struct sync_timeline *obj;
+	unsigned long flags;
+
+	if (size < sizeof(struct sync_timeline))
+		return NULL;
+
+	obj = kzalloc(size, GFP_KERNEL);
+	if (obj == NULL)
+		return NULL;
+
+	kref_init(&obj->kref);
+	obj->ops = ops;
+	strlcpy(obj->name, name, sizeof(obj->name));
+
+	INIT_LIST_HEAD(&obj->child_list_head);
+	spin_lock_init(&obj->child_list_lock);
+
+	INIT_LIST_HEAD(&obj->active_list_head);
+	spin_lock_init(&obj->active_list_lock);
+
+	spin_lock_irqsave(&sync_timeline_list_lock, flags);
+	list_add_tail(&obj->sync_timeline_list, &sync_timeline_list_head);
+	spin_unlock_irqrestore(&sync_timeline_list_lock, flags);
+
+	return obj;
+}
+EXPORT_SYMBOL(sync_timeline_create);
+
+static void sync_timeline_free(struct kref *kref)
+{
+	struct sync_timeline *obj =
+		container_of(kref, struct sync_timeline, kref);
+	unsigned long flags;
+
+	if (obj->ops->release_obj)
+		obj->ops->release_obj(obj);
+
+	spin_lock_irqsave(&sync_timeline_list_lock, flags);
+	list_del(&obj->sync_timeline_list);
+	spin_unlock_irqrestore(&sync_timeline_list_lock, flags);
+
+	kfree(obj);
+}
+
+void sync_timeline_destroy(struct sync_timeline *obj)
+{
+	obj->destroyed = true;
+
+	/*
+	 * If this is not the last reference, signal any children
+	 * that their parent is going away.
+	 */
+
+	if (!kref_put(&obj->kref, sync_timeline_free))
+		sync_timeline_signal(obj);
+}
+EXPORT_SYMBOL(sync_timeline_destroy);
+
+static void sync_timeline_add_pt(struct sync_timeline *obj, struct sync_pt *pt)
+{
+	unsigned long flags;
+
+	pt->parent = obj;
+
+	spin_lock_irqsave(&obj->child_list_lock, flags);
+	list_add_tail(&pt->child_list, &obj->child_list_head);
+	spin_unlock_irqrestore(&obj->child_list_lock, flags);
+}
+
+static void sync_timeline_remove_pt(struct sync_pt *pt)
+{
+	struct sync_timeline *obj = pt->parent;
+	unsigned long flags;
+
+	spin_lock_irqsave(&obj->active_list_lock, flags);
+	if (!list_empty(&pt->active_list))
+		list_del_init(&pt->active_list);
+	spin_unlock_irqrestore(&obj->active_list_lock, flags);
+
+	spin_lock_irqsave(&obj->child_list_lock, flags);
+	if (!list_empty(&pt->child_list)) {
+		list_del_init(&pt->child_list);
+	}
+	spin_unlock_irqrestore(&obj->child_list_lock, flags);
+}
+
+void sync_timeline_signal(struct sync_timeline *obj)
+{
+	unsigned long flags;
+	LIST_HEAD(signaled_pts);
+	struct list_head *pos, *n;
+
+	trace_sync_timeline(obj);
+
+	spin_lock_irqsave(&obj->active_list_lock, flags);
+
+	list_for_each_safe(pos, n, &obj->active_list_head) {
+		struct sync_pt *pt =
+			container_of(pos, struct sync_pt, active_list);
+
+		if (_sync_pt_has_signaled(pt)) {
+			list_del_init(pos);
+			list_add(&pt->signaled_list, &signaled_pts);
+			kref_get(&pt->fence->kref);
+		}
+	}
+
+	spin_unlock_irqrestore(&obj->active_list_lock, flags);
+
+	list_for_each_safe(pos, n, &signaled_pts) {
+		struct sync_pt *pt =
+			container_of(pos, struct sync_pt, signaled_list);
+
+		list_del_init(pos);
+		sync_fence_signal_pt(pt);
+		kref_put(&pt->fence->kref, sync_fence_free);
+	}
+}
+EXPORT_SYMBOL(sync_timeline_signal);
+
+struct sync_pt *sync_pt_create(struct sync_timeline *parent, int size)
+{
+	struct sync_pt *pt;
+
+	if (size < sizeof(struct sync_pt))
+		return NULL;
+
+	pt = kzalloc(size, GFP_KERNEL);
+	if (pt == NULL)
+		return NULL;
+
+	INIT_LIST_HEAD(&pt->active_list);
+	kref_get(&parent->kref);
+	sync_timeline_add_pt(parent, pt);
+
+	return pt;
+}
+EXPORT_SYMBOL(sync_pt_create);
+
+void sync_pt_free(struct sync_pt *pt)
+{
+	if (pt->parent->ops->free_pt)
+		pt->parent->ops->free_pt(pt);
+
+	sync_timeline_remove_pt(pt);
+
+	kref_put(&pt->parent->kref, sync_timeline_free);
+
+	kfree(pt);
+}
+EXPORT_SYMBOL(sync_pt_free);
+
+/* call with pt->parent->active_list_lock held */
+static int _sync_pt_has_signaled(struct sync_pt *pt)
+{
+	int old_status = pt->status;
+
+	if (!pt->status)
+		pt->status = pt->parent->ops->has_signaled(pt);
+
+	if (!pt->status && pt->parent->destroyed)
+		pt->status = -ENOENT;
+
+	if (pt->status != old_status)
+		pt->timestamp = ktime_get();
+
+	return pt->status;
+}
+
+static struct sync_pt *sync_pt_dup(struct sync_pt *pt)
+{
+	return pt->parent->ops->dup(pt);
+}
+
+/* Adds a sync pt to the active queue.  Called when added to a fence */
+static void sync_pt_activate(struct sync_pt *pt)
+{
+	struct sync_timeline *obj = pt->parent;
+	unsigned long flags;
+	int err;
+
+	spin_lock_irqsave(&obj->active_list_lock, flags);
+
+	err = _sync_pt_has_signaled(pt);
+	if (err != 0)
+		goto out;
+
+	list_add_tail(&pt->active_list, &obj->active_list_head);
+
+out:
+	spin_unlock_irqrestore(&obj->active_list_lock, flags);
+}
+
+static int sync_fence_release(struct inode *inode, struct file *file);
+static unsigned int sync_fence_poll(struct file *file, poll_table *wait);
+static long sync_fence_ioctl(struct file *file, unsigned int cmd,
+			     unsigned long arg);
+
+
+static const struct file_operations sync_fence_fops = {
+	.release = sync_fence_release,
+	.poll = sync_fence_poll,
+	.unlocked_ioctl = sync_fence_ioctl,
+	.compat_ioctl = sync_fence_ioctl,
+};
+
+static struct sync_fence *sync_fence_alloc(const char *name)
+{
+	struct sync_fence *fence;
+	unsigned long flags;
+
+	fence = kzalloc(sizeof(struct sync_fence), GFP_KERNEL);
+	if (fence == NULL)
+		return NULL;
+
+	fence->file = anon_inode_getfile("sync_fence", &sync_fence_fops,
+					 fence, 0);
+	if (IS_ERR(fence->file))
+		goto err;
+
+	kref_init(&fence->kref);
+	strlcpy(fence->name, name, sizeof(fence->name));
+
+	INIT_LIST_HEAD(&fence->pt_list_head);
+	INIT_LIST_HEAD(&fence->waiter_list_head);
+	spin_lock_init(&fence->waiter_list_lock);
+
+	init_waitqueue_head(&fence->wq);
+
+	spin_lock_irqsave(&sync_fence_list_lock, flags);
+	list_add_tail(&fence->sync_fence_list, &sync_fence_list_head);
+	spin_unlock_irqrestore(&sync_fence_list_lock, flags);
+
+	return fence;
+
+err:
+	kfree(fence);
+	return NULL;
+}
+
+/* TODO: implement a create which takes more that one sync_pt */
+struct sync_fence *sync_fence_create(const char *name, struct sync_pt *pt)
+{
+	struct sync_fence *fence;
+
+	if (pt->fence)
+		return NULL;
+
+	fence = sync_fence_alloc(name);
+	if (fence == NULL)
+		return NULL;
+
+	pt->fence = fence;
+	list_add(&pt->pt_list, &fence->pt_list_head);
+	sync_pt_activate(pt);
+
+	/*
+	 * signal the fence in case pt was activated before
+	 * sync_pt_activate(pt) was called
+	 */
+	sync_fence_signal_pt(pt);
+
+	return fence;
+}
+EXPORT_SYMBOL(sync_fence_create);
+
+static int sync_fence_copy_pts(struct sync_fence *dst, struct sync_fence *src)
+{
+	struct list_head *pos;
+
+	list_for_each(pos, &src->pt_list_head) {
+		struct sync_pt *orig_pt =
+			container_of(pos, struct sync_pt, pt_list);
+		struct sync_pt *new_pt = sync_pt_dup(orig_pt);
+
+		if (new_pt == NULL)
+			return -ENOMEM;
+
+		new_pt->fence = dst;
+		list_add(&new_pt->pt_list, &dst->pt_list_head);
+	}
+
+	return 0;
+}
+
+static int sync_fence_merge_pts(struct sync_fence *dst, struct sync_fence *src)
+{
+	struct list_head *src_pos, *dst_pos, *n;
+
+	list_for_each(src_pos, &src->pt_list_head) {
+		struct sync_pt *src_pt =
+			container_of(src_pos, struct sync_pt, pt_list);
+		bool collapsed = false;
+
+		list_for_each_safe(dst_pos, n, &dst->pt_list_head) {
+			struct sync_pt *dst_pt =
+				container_of(dst_pos, struct sync_pt, pt_list);
+			/* collapse two sync_pts on the same timeline
+			 * to a single sync_pt that will signal at
+			 * the later of the two
+			 */
+			if (dst_pt->parent == src_pt->parent) {
+				if (dst_pt->parent->ops->compare(dst_pt, src_pt)
+						 == -1) {
+					struct sync_pt *new_pt =
+						sync_pt_dup(src_pt);
+					if (new_pt == NULL)
+						return -ENOMEM;
+
+					new_pt->fence = dst;
+					list_replace(&dst_pt->pt_list,
+						     &new_pt->pt_list);
+					sync_pt_free(dst_pt);
+				}
+				collapsed = true;
+				break;
+			}
+		}
+
+		if (!collapsed) {
+			struct sync_pt *new_pt = sync_pt_dup(src_pt);
+
+			if (new_pt == NULL)
+				return -ENOMEM;
+
+			new_pt->fence = dst;
+			list_add(&new_pt->pt_list, &dst->pt_list_head);
+		}
+	}
+
+	return 0;
+}
+
+static void sync_fence_detach_pts(struct sync_fence *fence)
+{
+	struct list_head *pos, *n;
+
+	list_for_each_safe(pos, n, &fence->pt_list_head) {
+		struct sync_pt *pt = container_of(pos, struct sync_pt, pt_list);
+		sync_timeline_remove_pt(pt);
+	}
+}
+
+static void sync_fence_free_pts(struct sync_fence *fence)
+{
+	struct list_head *pos, *n;
+
+	list_for_each_safe(pos, n, &fence->pt_list_head) {
+		struct sync_pt *pt = container_of(pos, struct sync_pt, pt_list);
+		sync_pt_free(pt);
+	}
+}
+
+struct sync_fence *sync_fence_fdget(int fd)
+{
+	struct file *file = fget(fd);
+
+	if (file == NULL)
+		return NULL;
+
+	if (file->f_op != &sync_fence_fops)
+		goto err;
+
+	return file->private_data;
+
+err:
+	fput(file);
+	return NULL;
+}
+EXPORT_SYMBOL(sync_fence_fdget);
+
+void sync_fence_put(struct sync_fence *fence)
+{
+	fput(fence->file);
+}
+EXPORT_SYMBOL(sync_fence_put);
+
+void sync_fence_install(struct sync_fence *fence, int fd)
+{
+	fd_install(fd, fence->file);
+}
+EXPORT_SYMBOL(sync_fence_install);
+
+static int sync_fence_get_status(struct sync_fence *fence)
+{
+	struct list_head *pos;
+	int status = 1;
+
+	list_for_each(pos, &fence->pt_list_head) {
+		struct sync_pt *pt = container_of(pos, struct sync_pt, pt_list);
+		int pt_status = pt->status;
+
+		if (pt_status < 0) {
+			status = pt_status;
+			break;
+		} else if (status == 1) {
+			status = pt_status;
+		}
+	}
+
+	return status;
+}
+
+struct sync_fence *sync_fence_merge(const char *name,
+				    struct sync_fence *a, struct sync_fence *b)
+{
+	struct sync_fence *fence;
+	struct list_head *pos;
+	int err;
+
+	fence = sync_fence_alloc(name);
+	if (fence == NULL)
+		return NULL;
+
+	err = sync_fence_copy_pts(fence, a);
+	if (err < 0)
+		goto err;
+
+	err = sync_fence_merge_pts(fence, b);
+	if (err < 0)
+		goto err;
+
+	list_for_each(pos, &fence->pt_list_head) {
+		struct sync_pt *pt =
+			container_of(pos, struct sync_pt, pt_list);
+		sync_pt_activate(pt);
+	}
+
+	/*
+	 * signal the fence in case one of it's pts were activated before
+	 * they were activated
+	 */
+	sync_fence_signal_pt(list_first_entry(&fence->pt_list_head,
+					      struct sync_pt,
+					      pt_list));
+
+	return fence;
+err:
+	sync_fence_free_pts(fence);
+	kfree(fence);
+	return NULL;
+}
+EXPORT_SYMBOL(sync_fence_merge);
+
+static void sync_fence_signal_pt(struct sync_pt *pt)
+{
+	LIST_HEAD(signaled_waiters);
+	struct sync_fence *fence = pt->fence;
+	struct list_head *pos;
+	struct list_head *n;
+	unsigned long flags;
+	int status;
+
+	status = sync_fence_get_status(fence);
+
+	spin_lock_irqsave(&fence->waiter_list_lock, flags);
+	/*
+	 * this should protect against two threads racing on the signaled
+	 * false -> true transition
+	 */
+	if (status && !fence->status) {
+		list_for_each_safe(pos, n, &fence->waiter_list_head)
+			list_move(pos, &signaled_waiters);
+
+		fence->status = status;
+	} else {
+		status = 0;
+	}
+	spin_unlock_irqrestore(&fence->waiter_list_lock, flags);
+
+	if (status) {
+		list_for_each_safe(pos, n, &signaled_waiters) {
+			struct sync_fence_waiter *waiter =
+				container_of(pos, struct sync_fence_waiter,
+					     waiter_list);
+
+			list_del(pos);
+			waiter->callback(fence, waiter);
+		}
+		wake_up(&fence->wq);
+	}
+}
+
+int sync_fence_wait_async(struct sync_fence *fence,
+			  struct sync_fence_waiter *waiter)
+{
+	unsigned long flags;
+	int err = 0;
+
+	spin_lock_irqsave(&fence->waiter_list_lock, flags);
+
+	if (fence->status) {
+		err = fence->status;
+		goto out;
+	}
+
+	list_add_tail(&waiter->waiter_list, &fence->waiter_list_head);
+out:
+	spin_unlock_irqrestore(&fence->waiter_list_lock, flags);
+
+	return err;
+}
+EXPORT_SYMBOL(sync_fence_wait_async);
+
+int sync_fence_cancel_async(struct sync_fence *fence,
+			     struct sync_fence_waiter *waiter)
+{
+	struct list_head *pos;
+	struct list_head *n;
+	unsigned long flags;
+	int ret = -ENOENT;
+
+	spin_lock_irqsave(&fence->waiter_list_lock, flags);
+	/*
+	 * Make sure waiter is still in waiter_list because it is possible for
+	 * the waiter to be removed from the list while the callback is still
+	 * pending.
+	 */
+	list_for_each_safe(pos, n, &fence->waiter_list_head) {
+		struct sync_fence_waiter *list_waiter =
+			container_of(pos, struct sync_fence_waiter,
+				     waiter_list);
+		if (list_waiter == waiter) {
+			list_del(pos);
+			ret = 0;
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&fence->waiter_list_lock, flags);
+	return ret;
+}
+EXPORT_SYMBOL(sync_fence_cancel_async);
+
+static bool sync_fence_check(struct sync_fence *fence)
+{
+	/*
+	 * Make sure that reads to fence->status are ordered with the
+	 * wait queue event triggering
+	 */
+	smp_rmb();
+	return fence->status != 0;
+}
+
+int sync_fence_wait(struct sync_fence *fence, long timeout)
+{
+	int err = 0;
+	struct sync_pt *pt;
+
+	trace_sync_wait(fence, 1);
+	list_for_each_entry(pt, &fence->pt_list_head, pt_list)
+		trace_sync_pt(pt);
+
+	if (timeout > 0) {
+		timeout = msecs_to_jiffies(timeout);
+		err = wait_event_interruptible_timeout(fence->wq,
+						       sync_fence_check(fence),
+						       timeout);
+	} else if (timeout < 0) {
+		err = wait_event_interruptible(fence->wq,
+					       sync_fence_check(fence));
+	}
+	trace_sync_wait(fence, 0);
+
+	if (err < 0)
+		return err;
+
+	if (fence->status < 0) {
+		pr_info("fence error %d on [%p]\n", fence->status, fence);
+		sync_dump();
+		return fence->status;
+	}
+
+	if (fence->status == 0) {
+		if (timeout > 0) {
+			pr_info("fence timeout on [%p] after %dms\n", fence,
+				jiffies_to_msecs(timeout));
+			sync_dump();
+		}
+		return -ETIME;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(sync_fence_wait);
+
+static void sync_fence_free(struct kref *kref)
+{
+	struct sync_fence *fence = container_of(kref, struct sync_fence, kref);
+
+	sync_fence_free_pts(fence);
+
+	kfree(fence);
+}
+
+static int sync_fence_release(struct inode *inode, struct file *file)
+{
+	struct sync_fence *fence = file->private_data;
+	unsigned long flags;
+
+	/*
+	 * We need to remove all ways to access this fence before droping
+	 * our ref.
+	 *
+	 * start with its membership in the global fence list
+	 */
+	spin_lock_irqsave(&sync_fence_list_lock, flags);
+	list_del(&fence->sync_fence_list);
+	spin_unlock_irqrestore(&sync_fence_list_lock, flags);
+
+	/*
+	 * remove its pts from their parents so that sync_timeline_signal()
+	 * can't reference the fence.
+	 */
+	sync_fence_detach_pts(fence);
+
+	kref_put(&fence->kref, sync_fence_free);
+
+	return 0;
+}
+
+static unsigned int sync_fence_poll(struct file *file, poll_table *wait)
+{
+	struct sync_fence *fence = file->private_data;
+
+	poll_wait(file, &fence->wq, wait);
+
+	/*
+	 * Make sure that reads to fence->status are ordered with the
+	 * wait queue event triggering
+	 */
+	smp_rmb();
+
+	if (fence->status == 1)
+		return POLLIN;
+	else if (fence->status < 0)
+		return POLLERR;
+	else
+		return 0;
+}
+
+static long sync_fence_ioctl_wait(struct sync_fence *fence, unsigned long arg)
+{
+	__s32 value;
+
+	if (copy_from_user(&value, (void __user *)arg, sizeof(value)))
+		return -EFAULT;
+
+	return sync_fence_wait(fence, value);
+}
+
+static long sync_fence_ioctl_merge(struct sync_fence *fence, unsigned long arg)
+{
+	int fd = get_unused_fd();
+	int err;
+	struct sync_fence *fence2, *fence3;
+	struct sync_merge_data data;
+
+	if (fd < 0)
+		return fd;
+
+	if (copy_from_user(&data, (void __user *)arg, sizeof(data))) {
+		err = -EFAULT;
+		goto err_put_fd;
+	}
+
+	fence2 = sync_fence_fdget(data.fd2);
+	if (fence2 == NULL) {
+		err = -ENOENT;
+		goto err_put_fd;
+	}
+
+	data.name[sizeof(data.name) - 1] = '\0';
+	fence3 = sync_fence_merge(data.name, fence, fence2);
+	if (fence3 == NULL) {
+		err = -ENOMEM;
+		goto err_put_fence2;
+	}
+
+	data.fence = fd;
+	if (copy_to_user((void __user *)arg, &data, sizeof(data))) {
+		err = -EFAULT;
+		goto err_put_fence3;
+	}
+
+	sync_fence_install(fence3, fd);
+	sync_fence_put(fence2);
+	return 0;
+
+err_put_fence3:
+	sync_fence_put(fence3);
+
+err_put_fence2:
+	sync_fence_put(fence2);
+
+err_put_fd:
+	put_unused_fd(fd);
+	return err;
+}
+
+static int sync_fill_pt_info(struct sync_pt *pt, void *data, int size)
+{
+	struct sync_pt_info *info = data;
+	int ret;
+
+	if (size < sizeof(struct sync_pt_info))
+		return -ENOMEM;
+
+	info->len = sizeof(struct sync_pt_info);
+
+	if (pt->parent->ops->fill_driver_data) {
+		ret = pt->parent->ops->fill_driver_data(pt, info->driver_data,
+							size - sizeof(*info));
+		if (ret < 0)
+			return ret;
+
+		info->len += ret;
+	}
+
+	strlcpy(info->obj_name, pt->parent->name, sizeof(info->obj_name));
+	strlcpy(info->driver_name, pt->parent->ops->driver_name,
+		sizeof(info->driver_name));
+	info->status = pt->status;
+	info->timestamp_ns = ktime_to_ns(pt->timestamp);
+
+	return info->len;
+}
+
+static long sync_fence_ioctl_fence_info(struct sync_fence *fence,
+					unsigned long arg)
+{
+	struct sync_fence_info_data *data;
+	struct list_head *pos;
+	__u32 size;
+	__u32 len = 0;
+	int ret;
+
+	if (copy_from_user(&size, (void __user *)arg, sizeof(size)))
+		return -EFAULT;
+
+	if (size < sizeof(struct sync_fence_info_data))
+		return -EINVAL;
+
+	if (size > 4096)
+		size = 4096;
+
+	data = kzalloc(size, GFP_KERNEL);
+	if (data == NULL)
+		return -ENOMEM;
+
+	strlcpy(data->name, fence->name, sizeof(data->name));
+	data->status = fence->status;
+	len = sizeof(struct sync_fence_info_data);
+
+	list_for_each(pos, &fence->pt_list_head) {
+		struct sync_pt *pt =
+			container_of(pos, struct sync_pt, pt_list);
+
+		ret = sync_fill_pt_info(pt, (u8 *)data + len, size - len);
+
+		if (ret < 0)
+			goto out;
+
+		len += ret;
+	}
+
+	data->len = len;
+
+	if (copy_to_user((void __user *)arg, data, len))
+		ret = -EFAULT;
+	else
+		ret = 0;
+
+out:
+	kfree(data);
+
+	return ret;
+}
+
+static long sync_fence_ioctl(struct file *file, unsigned int cmd,
+			     unsigned long arg)
+{
+	struct sync_fence *fence = file->private_data;
+	switch (cmd) {
+	case SYNC_IOC_WAIT:
+		return sync_fence_ioctl_wait(fence, arg);
+
+	case SYNC_IOC_MERGE:
+		return sync_fence_ioctl_merge(fence, arg);
+
+	case SYNC_IOC_FENCE_INFO:
+		return sync_fence_ioctl_fence_info(fence, arg);
+
+	default:
+		return -ENOTTY;
+	}
+}
+
+#ifdef CONFIG_DEBUG_FS
+static const char *sync_status_str(int status)
+{
+	if (status > 0)
+		return "signaled";
+	else if (status == 0)
+		return "active";
+	else
+		return "error";
+}
+
+static void sync_print_pt(struct seq_file *s, struct sync_pt *pt, bool fence)
+{
+	int status = pt->status;
+	seq_printf(s, "  %s%spt %s",
+		   fence ? pt->parent->name : "",
+		   fence ? "_" : "",
+		   sync_status_str(status));
+	if (pt->status) {
+		struct timeval tv = ktime_to_timeval(pt->timestamp);
+		seq_printf(s, "@%ld.%06ld", tv.tv_sec, tv.tv_usec);
+	}
+
+	if (pt->parent->ops->timeline_value_str &&
+	    pt->parent->ops->pt_value_str) {
+		char value[64];
+		pt->parent->ops->pt_value_str(pt, value, sizeof(value));
+		seq_printf(s, ": %s", value);
+		if (fence) {
+			pt->parent->ops->timeline_value_str(pt->parent, value,
+						    sizeof(value));
+			seq_printf(s, " / %s", value);
+		}
+	} else if (pt->parent->ops->print_pt) {
+		seq_printf(s, ": ");
+		pt->parent->ops->print_pt(s, pt);
+	}
+
+	seq_printf(s, "\n");
+}
+
+static void sync_print_obj(struct seq_file *s, struct sync_timeline *obj)
+{
+	struct list_head *pos;
+	unsigned long flags;
+
+	seq_printf(s, "%s %s", obj->name, obj->ops->driver_name);
+
+	if (obj->ops->timeline_value_str) {
+		char value[64];
+		obj->ops->timeline_value_str(obj, value, sizeof(value));
+		seq_printf(s, ": %s", value);
+	} else if (obj->ops->print_obj) {
+		seq_printf(s, ": ");
+		obj->ops->print_obj(s, obj);
+	}
+
+	seq_printf(s, "\n");
+
+	spin_lock_irqsave(&obj->child_list_lock, flags);
+	list_for_each(pos, &obj->child_list_head) {
+		struct sync_pt *pt =
+			container_of(pos, struct sync_pt, child_list);
+		sync_print_pt(s, pt, false);
+	}
+	spin_unlock_irqrestore(&obj->child_list_lock, flags);
+}
+
+static void sync_print_fence(struct seq_file *s, struct sync_fence *fence)
+{
+	struct list_head *pos;
+	unsigned long flags;
+
+	seq_printf(s, "[%p] %s: %s\n", fence, fence->name,
+		   sync_status_str(fence->status));
+
+	list_for_each(pos, &fence->pt_list_head) {
+		struct sync_pt *pt =
+			container_of(pos, struct sync_pt, pt_list);
+		sync_print_pt(s, pt, true);
+	}
+
+	spin_lock_irqsave(&fence->waiter_list_lock, flags);
+	list_for_each(pos, &fence->waiter_list_head) {
+		struct sync_fence_waiter *waiter =
+			container_of(pos, struct sync_fence_waiter,
+				     waiter_list);
+
+		seq_printf(s, "waiter %pF\n", waiter->callback);
+	}
+	spin_unlock_irqrestore(&fence->waiter_list_lock, flags);
+}
+
+static int sync_debugfs_show(struct seq_file *s, void *unused)
+{
+	unsigned long flags;
+	struct list_head *pos;
+
+	seq_printf(s, "objs:\n--------------\n");
+
+	spin_lock_irqsave(&sync_timeline_list_lock, flags);
+	list_for_each(pos, &sync_timeline_list_head) {
+		struct sync_timeline *obj =
+			container_of(pos, struct sync_timeline,
+				     sync_timeline_list);
+
+		sync_print_obj(s, obj);
+		seq_printf(s, "\n");
+	}
+	spin_unlock_irqrestore(&sync_timeline_list_lock, flags);
+
+	seq_printf(s, "fences:\n--------------\n");
+
+	spin_lock_irqsave(&sync_fence_list_lock, flags);
+	list_for_each(pos, &sync_fence_list_head) {
+		struct sync_fence *fence =
+			container_of(pos, struct sync_fence, sync_fence_list);
+
+		sync_print_fence(s, fence);
+		seq_printf(s, "\n");
+	}
+	spin_unlock_irqrestore(&sync_fence_list_lock, flags);
+	return 0;
+}
+
+static int sync_debugfs_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, sync_debugfs_show, inode->i_private);
+}
+
+static const struct file_operations sync_debugfs_fops = {
+	.open           = sync_debugfs_open,
+	.read           = seq_read,
+	.llseek         = seq_lseek,
+	.release        = single_release,
+};
+
+static __init int sync_debugfs_init(void)
+{
+	debugfs_create_file("sync", S_IRUGO, NULL, NULL, &sync_debugfs_fops);
+	return 0;
+}
+late_initcall(sync_debugfs_init);
+
+#define DUMP_CHUNK 256
+static char sync_dump_buf[64 * 1024];
+void sync_dump(void)
+{
+	struct seq_file s = {
+		.buf = sync_dump_buf,
+		.size = sizeof(sync_dump_buf) - 1,
+	};
+	int i;
+
+	sync_debugfs_show(&s, NULL);
+
+	for (i = 0; i < s.count; i += DUMP_CHUNK) {
+		if ((s.count - i) > DUMP_CHUNK) {
+			char c = s.buf[i + DUMP_CHUNK];
+			s.buf[i + DUMP_CHUNK] = 0;
+			pr_cont("%s", s.buf + i);
+			s.buf[i + DUMP_CHUNK] = c;
+		} else {
+			s.buf[s.count] = 0;
+			pr_cont("%s", s.buf + i);
+		}
+	}
+}
+#else
+static void sync_dump(void)
+{
+}
+#endif
diff --git a/drivers/staging/android/sync.h b/drivers/staging/android/sync.h
new file mode 100644
index 00000000000..38ea986dc70
--- /dev/null
+++ b/drivers/staging/android/sync.h
@@ -0,0 +1,426 @@
+/*
+ * include/linux/sync.h
+ *
+ * Copyright (C) 2012 Google, Inc.
+ *
+ * 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.
+ *
+ */
+
+#ifndef _LINUX_SYNC_H
+#define _LINUX_SYNC_H
+
+#include <linux/types.h>
+#ifdef __KERNEL__
+
+#include <linux/kref.h>
+#include <linux/ktime.h>
+#include <linux/list.h>
+#include <linux/spinlock.h>
+#include <linux/wait.h>
+
+struct sync_timeline;
+struct sync_pt;
+struct sync_fence;
+
+/**
+ * struct sync_timeline_ops - sync object implementation ops
+ * @driver_name:	name of the implentation
+ * @dup:		duplicate a sync_pt
+ * @has_signaled:	returns:
+ *			  1 if pt has signaled
+ *			  0 if pt has not signaled
+ *			 <0 on error
+ * @compare:		returns:
+ *			  1 if b will signal before a
+ *			  0 if a and b will signal at the same time
+ *			 -1 if a will signabl before b
+ * @free_pt:		called before sync_pt is freed
+ * @release_obj:	called before sync_timeline is freed
+ * @print_obj:		deprecated
+ * @print_pt:		deprecated
+ * @fill_driver_data:	write implmentation specific driver data to data.
+ *			  should return an error if there is not enough room
+ *			  as specified by size.  This information is returned
+ *			  to userspace by SYNC_IOC_FENCE_INFO.
+ * @timeline_value_str: fill str with the value of the sync_timeline's counter
+ * @pt_value_str:	fill str with the value of the sync_pt
+ */
+struct sync_timeline_ops {
+	const char *driver_name;
+
+	/* required */
+	struct sync_pt *(*dup)(struct sync_pt *pt);
+
+	/* required */
+	int (*has_signaled)(struct sync_pt *pt);
+
+	/* required */
+	int (*compare)(struct sync_pt *a, struct sync_pt *b);
+
+	/* optional */
+	void (*free_pt)(struct sync_pt *sync_pt);
+
+	/* optional */
+	void (*release_obj)(struct sync_timeline *sync_timeline);
+
+	/* deprecated */
+	void (*print_obj)(struct seq_file *s,
+			  struct sync_timeline *sync_timeline);
+
+	/* deprecated */
+	void (*print_pt)(struct seq_file *s, struct sync_pt *sync_pt);
+
+	/* optional */
+	int (*fill_driver_data)(struct sync_pt *syncpt, void *data, int size);
+
+	/* optional */
+	void (*timeline_value_str)(struct sync_timeline *timeline, char *str,
+				   int size);
+
+	/* optional */
+	void (*pt_value_str)(struct sync_pt *pt, char *str, int size);
+};
+
+/**
+ * struct sync_timeline - sync object
+ * @kref:		reference count on fence.
+ * @ops:		ops that define the implementaiton of the sync_timeline
+ * @name:		name of the sync_timeline. Useful for debugging
+ * @destoryed:		set when sync_timeline is destroyed
+ * @child_list_head:	list of children sync_pts for this sync_timeline
+ * @child_list_lock:	lock protecting @child_list_head, destroyed, and
+ *			  sync_pt.status
+ * @active_list_head:	list of active (unsignaled/errored) sync_pts
+ * @sync_timeline_list:	membership in global sync_timeline_list
+ */
+struct sync_timeline {
+	struct kref		kref;
+	const struct sync_timeline_ops	*ops;
+	char			name[32];
+
+	/* protected by child_list_lock */
+	bool			destroyed;
+
+	struct list_head	child_list_head;
+	spinlock_t		child_list_lock;
+
+	struct list_head	active_list_head;
+	spinlock_t		active_list_lock;
+
+	struct list_head	sync_timeline_list;
+};
+
+/**
+ * struct sync_pt - sync point
+ * @parent:		sync_timeline to which this sync_pt belongs
+ * @child_list:		membership in sync_timeline.child_list_head
+ * @active_list:	membership in sync_timeline.active_list_head
+ * @signaled_list:	membership in temorary signaled_list on stack
+ * @fence:		sync_fence to which the sync_pt belongs
+ * @pt_list:		membership in sync_fence.pt_list_head
+ * @status:		1: signaled, 0:active, <0: error
+ * @timestamp:		time which sync_pt status transitioned from active to
+ *			  singaled or error.
+ */
+struct sync_pt {
+	struct sync_timeline		*parent;
+	struct list_head	child_list;
+
+	struct list_head	active_list;
+	struct list_head	signaled_list;
+
+	struct sync_fence	*fence;
+	struct list_head	pt_list;
+
+	/* protected by parent->active_list_lock */
+	int			status;
+
+	ktime_t			timestamp;
+};
+
+/**
+ * struct sync_fence - sync fence
+ * @file:		file representing this fence
+ * @kref:		referenace count on fence.
+ * @name:		name of sync_fence.  Useful for debugging
+ * @pt_list_head:	list of sync_pts in ths fence.  immutable once fence
+ *			  is created
+ * @waiter_list_head:	list of asynchronous waiters on this fence
+ * @waiter_list_lock:	lock protecting @waiter_list_head and @status
+ * @status:		1: signaled, 0:active, <0: error
+ *
+ * @wq:			wait queue for fence signaling
+ * @sync_fence_list:	membership in global fence list
+ */
+struct sync_fence {
+	struct file		*file;
+	struct kref		kref;
+	char			name[32];
+
+	/* this list is immutable once the fence is created */
+	struct list_head	pt_list_head;
+
+	struct list_head	waiter_list_head;
+	spinlock_t		waiter_list_lock; /* also protects status */
+	int			status;
+
+	wait_queue_head_t	wq;
+
+	struct list_head	sync_fence_list;
+};
+
+struct sync_fence_waiter;
+typedef void (*sync_callback_t)(struct sync_fence *fence,
+				struct sync_fence_waiter *waiter);
+
+/**
+ * struct sync_fence_waiter - metadata for asynchronous waiter on a fence
+ * @waiter_list:	membership in sync_fence.waiter_list_head
+ * @callback:		function pointer to call when fence signals
+ * @callback_data:	pointer to pass to @callback
+ */
+struct sync_fence_waiter {
+	struct list_head	waiter_list;
+
+	sync_callback_t		callback;
+};
+
+static inline void sync_fence_waiter_init(struct sync_fence_waiter *waiter,
+					  sync_callback_t callback)
+{
+	waiter->callback = callback;
+}
+
+/*
+ * API for sync_timeline implementers
+ */
+
+/**
+ * sync_timeline_create() - creates a sync object
+ * @ops:	specifies the implemention ops for the object
+ * @size:	size to allocate for this obj
+ * @name:	sync_timeline name
+ *
+ * Creates a new sync_timeline which will use the implemetation specified by
+ * @ops.  @size bytes will be allocated allowing for implemntation specific
+ * data to be kept after the generic sync_timeline stuct.
+ */
+struct sync_timeline *sync_timeline_create(const struct sync_timeline_ops *ops,
+					   int size, const char *name);
+
+/**
+ * sync_timeline_destory() - destorys a sync object
+ * @obj:	sync_timeline to destroy
+ *
+ * A sync implemntation should call this when the @obj is going away
+ * (i.e. module unload.)  @obj won't actually be freed until all its childern
+ * sync_pts are freed.
+ */
+void sync_timeline_destroy(struct sync_timeline *obj);
+
+/**
+ * sync_timeline_signal() - signal a status change on a sync_timeline
+ * @obj:	sync_timeline to signal
+ *
+ * A sync implemntation should call this any time one of it's sync_pts
+ * has signaled or has an error condition.
+ */
+void sync_timeline_signal(struct sync_timeline *obj);
+
+/**
+ * sync_pt_create() - creates a sync pt
+ * @parent:	sync_pt's parent sync_timeline
+ * @size:	size to allocate for this pt
+ *
+ * Creates a new sync_pt as a chiled of @parent.  @size bytes will be
+ * allocated allowing for implemntation specific data to be kept after
+ * the generic sync_timeline struct.
+ */
+struct sync_pt *sync_pt_create(struct sync_timeline *parent, int size);
+
+/**
+ * sync_pt_free() - frees a sync pt
+ * @pt:		sync_pt to free
+ *
+ * This should only be called on sync_pts which have been created but
+ * not added to a fence.
+ */
+void sync_pt_free(struct sync_pt *pt);
+
+/**
+ * sync_fence_create() - creates a sync fence
+ * @name:	name of fence to create
+ * @pt:		sync_pt to add to the fence
+ *
+ * Creates a fence containg @pt.  Once this is called, the fence takes
+ * ownership of @pt.
+ */
+struct sync_fence *sync_fence_create(const char *name, struct sync_pt *pt);
+
+/*
+ * API for sync_fence consumers
+ */
+
+/**
+ * sync_fence_merge() - merge two fences
+ * @name:	name of new fence
+ * @a:		fence a
+ * @b:		fence b
+ *
+ * Creates a new fence which contains copies of all the sync_pts in both
+ * @a and @b.  @a and @b remain valid, independent fences.
+ */
+struct sync_fence *sync_fence_merge(const char *name,
+				    struct sync_fence *a, struct sync_fence *b);
+
+/**
+ * sync_fence_fdget() - get a fence from an fd
+ * @fd:		fd referencing a fence
+ *
+ * Ensures @fd references a valid fence, increments the refcount of the backing
+ * file, and returns the fence.
+ */
+struct sync_fence *sync_fence_fdget(int fd);
+
+/**
+ * sync_fence_put() - puts a refernnce of a sync fence
+ * @fence:	fence to put
+ *
+ * Puts a reference on @fence.  If this is the last reference, the fence and
+ * all it's sync_pts will be freed
+ */
+void sync_fence_put(struct sync_fence *fence);
+
+/**
+ * sync_fence_install() - installs a fence into a file descriptor
+ * @fence:	fence to instal
+ * @fd:		file descriptor in which to install the fence
+ *
+ * Installs @fence into @fd.  @fd's should be acquired through get_unused_fd().
+ */
+void sync_fence_install(struct sync_fence *fence, int fd);
+
+/**
+ * sync_fence_wait_async() - registers and async wait on the fence
+ * @fence:		fence to wait on
+ * @waiter:		waiter callback struck
+ *
+ * Returns 1 if @fence has already signaled.
+ *
+ * Registers a callback to be called when @fence signals or has an error.
+ * @waiter should be initialized with sync_fence_waiter_init().
+ */
+int sync_fence_wait_async(struct sync_fence *fence,
+			  struct sync_fence_waiter *waiter);
+
+/**
+ * sync_fence_cancel_async() - cancels an async wait
+ * @fence:		fence to wait on
+ * @waiter:		waiter callback struck
+ *
+ * returns 0 if waiter was removed from fence's async waiter list.
+ * returns -ENOENT if waiter was not found on fence's async waiter list.
+ *
+ * Cancels a previously registered async wait.  Will fail gracefully if
+ * @waiter was never registered or if @fence has already signaled @waiter.
+ */
+int sync_fence_cancel_async(struct sync_fence *fence,
+			    struct sync_fence_waiter *waiter);
+
+/**
+ * sync_fence_wait() - wait on fence
+ * @fence:	fence to wait on
+ * @tiemout:	timeout in ms
+ *
+ * Wait for @fence to be signaled or have an error.  Waits indefinitely
+ * if @timeout < 0
+ */
+int sync_fence_wait(struct sync_fence *fence, long timeout);
+
+#endif /* __KERNEL__ */
+
+/**
+ * struct sync_merge_data - data passed to merge ioctl
+ * @fd2:	file descriptor of second fence
+ * @name:	name of new fence
+ * @fence:	returns the fd of the new fence to userspace
+ */
+struct sync_merge_data {
+	__s32	fd2; /* fd of second fence */
+	char	name[32]; /* name of new fence */
+	__s32	fence; /* fd on newly created fence */
+};
+
+/**
+ * struct sync_pt_info - detailed sync_pt information
+ * @len:		length of sync_pt_info including any driver_data
+ * @obj_name:		name of parent sync_timeline
+ * @driver_name:	name of driver implmenting the parent
+ * @status:		status of the sync_pt 0:active 1:signaled <0:error
+ * @timestamp_ns:	timestamp of status change in nanoseconds
+ * @driver_data:	any driver dependant data
+ */
+struct sync_pt_info {
+	__u32	len;
+	char	obj_name[32];
+	char	driver_name[32];
+	__s32	status;
+	__u64	timestamp_ns;
+
+	__u8	driver_data[0];
+};
+
+/**
+ * struct sync_fence_info_data - data returned from fence info ioctl
+ * @len:	ioctl caller writes the size of the buffer its passing in.
+ *		ioctl returns length of sync_fence_data reutnred to userspace
+ *		including pt_info.
+ * @name:	name of fence
+ * @status:	status of fence. 1: signaled 0:active <0:error
+ * @pt_info:	a sync_pt_info struct for every sync_pt in the fence
+ */
+struct sync_fence_info_data {
+	__u32	len;
+	char	name[32];
+	__s32	status;
+
+	__u8	pt_info[0];
+};
+
+#define SYNC_IOC_MAGIC		'>'
+
+/**
+ * DOC: SYNC_IOC_WAIT - wait for a fence to signal
+ *
+ * pass timeout in milliseconds.  Waits indefinitely timeout < 0.
+ */
+#define SYNC_IOC_WAIT		_IOW(SYNC_IOC_MAGIC, 0, __s32)
+
+/**
+ * DOC: SYNC_IOC_MERGE - merge two fences
+ *
+ * Takes a struct sync_merge_data.  Creates a new fence containing copies of
+ * the sync_pts in both the calling fd and sync_merge_data.fd2.  Returns the
+ * new fence's fd in sync_merge_data.fence
+ */
+#define SYNC_IOC_MERGE		_IOWR(SYNC_IOC_MAGIC, 1, struct sync_merge_data)
+
+/**
+ * DOC: SYNC_IOC_FENCE_INFO - get detailed information on a fence
+ *
+ * Takes a struct sync_fence_info_data with extra space allocated for pt_info.
+ * Caller should write the size of the buffer into len.  On return, len is
+ * updated to reflect the total size of the sync_fence_info_data including
+ * pt_info.
+ *
+ * pt_info is a buffer containing sync_pt_infos for every sync_pt in the fence.
+ * To itterate over the sync_pt_infos, use the sync_pt_info.len field.
+ */
+#define SYNC_IOC_FENCE_INFO	_IOWR(SYNC_IOC_MAGIC, 2,\
+	struct sync_fence_info_data)
+
+#endif /* _LINUX_SYNC_H */
diff --git a/drivers/staging/android/trace/sync.h b/drivers/staging/android/trace/sync.h
new file mode 100644
index 00000000000..95462359ba5
--- /dev/null
+++ b/drivers/staging/android/trace/sync.h
@@ -0,0 +1,82 @@
+#undef TRACE_SYSTEM
+#define TRACE_INCLUDE_PATH ../../drivers/staging/android/trace
+#define TRACE_SYSTEM sync
+
+#if !defined(_TRACE_SYNC_H) || defined(TRACE_HEADER_MULTI_READ)
+#define _TRACE_SYNC_H
+
+#include "../sync.h"
+#include <linux/tracepoint.h>
+
+TRACE_EVENT(sync_timeline,
+	TP_PROTO(struct sync_timeline *timeline),
+
+	TP_ARGS(timeline),
+
+	TP_STRUCT__entry(
+			__string(name, timeline->name)
+			__array(char, value, 32)
+	),
+
+	TP_fast_assign(
+			__assign_str(name, timeline->name);
+			if (timeline->ops->timeline_value_str) {
+				timeline->ops->timeline_value_str(timeline,
+							__entry->value,
+							sizeof(__entry->value));
+			} else {
+				__entry->value[0] = '\0';
+			}
+	),
+
+	TP_printk("name=%s value=%s", __get_str(name), __entry->value)
+);
+
+TRACE_EVENT(sync_wait,
+	TP_PROTO(struct sync_fence *fence, int begin),
+
+	TP_ARGS(fence, begin),
+
+	TP_STRUCT__entry(
+			__string(name, fence->name)
+			__field(s32, status)
+			__field(u32, begin)
+	),
+
+	TP_fast_assign(
+			__assign_str(name, fence->name);
+			__entry->status = fence->status;
+			__entry->begin = begin;
+	),
+
+	TP_printk("%s name=%s state=%d", __entry->begin ? "begin" : "end",
+			__get_str(name), __entry->status)
+);
+
+TRACE_EVENT(sync_pt,
+	TP_PROTO(struct sync_pt *pt),
+
+	TP_ARGS(pt),
+
+	TP_STRUCT__entry(
+		__string(timeline, pt->parent->name)
+		__array(char, value, 32)
+	),
+
+	TP_fast_assign(
+		__assign_str(timeline, pt->parent->name);
+		if (pt->parent->ops->pt_value_str) {
+			pt->parent->ops->pt_value_str(pt, __entry->value,
+							sizeof(__entry->value));
+		} else {
+			__entry->value[0] = '\0';
+		}
+	),
+
+	TP_printk("name=%s value=%s", __get_str(timeline), __entry->value)
+);
+
+#endif /* if !defined(_TRACE_SYNC_H) || defined(TRACE_HEADER_MULTI_READ) */
+
+/* This part must be outside protection */
+#include <trace/define_trace.h>
diff --git a/drivers/staging/bcm/Bcmchar.c b/drivers/staging/bcm/Bcmchar.c
index 491e2bfbc46..35641e52939 100644
--- a/drivers/staging/bcm/Bcmchar.c
+++ b/drivers/staging/bcm/Bcmchar.c
@@ -1148,8 +1148,8 @@ cntrlEnd:
 
 		if (((ULONG)pBulkBuffer->Register & 0x0F000000) != 0x0F000000 ||
 			((ULONG)pBulkBuffer->Register & 0x3)) {
-			kfree(pvBuffer);
 			BCM_DEBUG_PRINT (Adapter, DBG_TYPE_PRINTK, 0, 0, "WRM Done On invalid Address : %x Access Denied.\n", (int)pBulkBuffer->Register);
+			kfree(pvBuffer);
 			Status = -EINVAL;
 			break;
 		}
diff --git a/drivers/staging/bcm/InterfaceDld.c b/drivers/staging/bcm/InterfaceDld.c
index 64ea6edb9dc..348ad75b340 100644
--- a/drivers/staging/bcm/InterfaceDld.c
+++ b/drivers/staging/bcm/InterfaceDld.c
@@ -205,30 +205,6 @@ static int bcm_download_config_file(struct bcm_mini_adapter *Adapter, struct bcm
 	return retval;
 }
 
-static int bcm_compare_buff_contents(unsigned char *readbackbuff, unsigned char *buff, unsigned int len)
-{
-	int retval = STATUS_SUCCESS;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-	if ((len-sizeof(unsigned int)) < 4) {
-		if (memcmp(readbackbuff , buff, len))
-			retval = -EINVAL;
-	} else {
-		len -= 4;
-
-		while (len) {
-			if (*(unsigned int *)&readbackbuff[len] != *(unsigned int *)&buff[len]) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "Firmware Download is not proper");
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "Val from Binary %x, Val From Read Back %x ", *(unsigned int *)&buff[len], *(unsigned int*)&readbackbuff[len]);
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "len =%x!!!", len);
-				retval = -EINVAL;
-				break;
-			}
-			len -= 4;
-		}
-	}
-	return retval;
-}
-
 int bcm_ioctl_fw_download(struct bcm_mini_adapter *Adapter, struct bcm_firmware_info *psFwInfo)
 {
 	int retval = STATUS_SUCCESS;
@@ -321,9 +297,11 @@ static INT buffRdbkVerify(struct bcm_mini_adapter *Adapter, PUCHAR mappedbuffer,
 			break;
 		}
 
-		retval = bcm_compare_buff_contents(readbackbuff, mappedbuffer, len);
-		if (STATUS_SUCCESS != retval)
-			break;
+		if (memcmp(readbackbuff, mappedbuffer, len) != 0) {
+			pr_err("%s() failed.  The firmware doesn't match what was written",
+			       __func__);
+			retval = -EIO;
+		}
 
 		u32StartingAddress += len;
 		u32FirmwareLength -= len;
diff --git a/drivers/staging/bcm/PHSModule.c b/drivers/staging/bcm/PHSModule.c
index 7028bc95b4f..af5d22faa7f 100644
--- a/drivers/staging/bcm/PHSModule.c
+++ b/drivers/staging/bcm/PHSModule.c
@@ -1,263 +1,238 @@
 #include "headers.h"
 
-static UINT CreateSFToClassifierRuleMapping(B_UINT16 uiVcid,B_UINT16  uiClsId, struct bcm_phs_table *psServiceFlowTable, struct bcm_phs_rule *psPhsRule, B_UINT8 u8AssociatedPHSI);
+static UINT CreateSFToClassifierRuleMapping(B_UINT16 uiVcid, B_UINT16 uiClsId, struct bcm_phs_table *psServiceFlowTable, struct bcm_phs_rule *psPhsRule, B_UINT8 u8AssociatedPHSI);
 
-static UINT CreateClassiferToPHSRuleMapping(B_UINT16 uiVcid,B_UINT16  uiClsId, struct bcm_phs_entry *pstServiceFlowEntry, struct bcm_phs_rule *psPhsRule, B_UINT8 u8AssociatedPHSI);
+static UINT CreateClassiferToPHSRuleMapping(B_UINT16 uiVcid, B_UINT16  uiClsId, struct bcm_phs_entry *pstServiceFlowEntry, struct bcm_phs_rule *psPhsRule, B_UINT8 u8AssociatedPHSI);
 
-static UINT CreateClassifierPHSRule(B_UINT16  uiClsId, struct bcm_phs_classifier_table *psaClassifiertable, struct bcm_phs_rule *psPhsRule, enum bcm_phs_classifier_context eClsContext,B_UINT8 u8AssociatedPHSI);
+static UINT CreateClassifierPHSRule(B_UINT16  uiClsId, struct bcm_phs_classifier_table *psaClassifiertable, struct bcm_phs_rule *psPhsRule, enum bcm_phs_classifier_context eClsContext, B_UINT8 u8AssociatedPHSI);
 
-static UINT UpdateClassifierPHSRule(B_UINT16  uiClsId, struct bcm_phs_classifier_entry *pstClassifierEntry, struct bcm_phs_classifier_table *psaClassifiertable, struct bcm_phs_rule *psPhsRule, B_UINT8 u8AssociatedPHSI);
+static UINT UpdateClassifierPHSRule(B_UINT16 uiClsId, struct bcm_phs_classifier_entry *pstClassifierEntry, struct bcm_phs_classifier_table *psaClassifiertable, struct bcm_phs_rule *psPhsRule, B_UINT8 u8AssociatedPHSI);
 
 static BOOLEAN ValidatePHSRuleComplete(struct bcm_phs_rule *psPhsRule);
 
-static BOOLEAN DerefPhsRule(B_UINT16  uiClsId, struct bcm_phs_classifier_table *psaClassifiertable, struct bcm_phs_rule *pstPhsRule);
+static BOOLEAN DerefPhsRule(B_UINT16 uiClsId, struct bcm_phs_classifier_table *psaClassifiertable, struct bcm_phs_rule *pstPhsRule);
 
-static UINT GetClassifierEntry(struct bcm_phs_classifier_table *pstClassifierTable,B_UINT32 uiClsid, enum bcm_phs_classifier_context eClsContext, struct bcm_phs_classifier_entry **ppstClassifierEntry);
+static UINT GetClassifierEntry(struct bcm_phs_classifier_table *pstClassifierTable, B_UINT32 uiClsid, enum bcm_phs_classifier_context eClsContext, struct bcm_phs_classifier_entry **ppstClassifierEntry);
 
-static UINT GetPhsRuleEntry(struct bcm_phs_classifier_table *pstClassifierTable,B_UINT32 uiPHSI, enum bcm_phs_classifier_context eClsContext, struct bcm_phs_rule **ppstPhsRule);
+static UINT GetPhsRuleEntry(struct bcm_phs_classifier_table *pstClassifierTable, B_UINT32 uiPHSI, enum bcm_phs_classifier_context eClsContext, struct bcm_phs_rule **ppstPhsRule);
 
 static void free_phs_serviceflow_rules(struct bcm_phs_table *psServiceFlowRulesTable);
 
 static int phs_compress(struct bcm_phs_rule *phs_members, unsigned char *in_buf,
-						unsigned char *out_buf,unsigned int *header_size,UINT *new_header_size );
+			unsigned char *out_buf, unsigned int *header_size, UINT *new_header_size);
 
+static int verify_suppress_phsf(unsigned char *in_buffer, unsigned char *out_buffer,
+				unsigned char *phsf, unsigned char *phsm, unsigned int phss, unsigned int phsv, UINT *new_header_size);
 
-static int verify_suppress_phsf(unsigned char *in_buffer,unsigned char *out_buffer,
-								unsigned char *phsf,unsigned char *phsm,unsigned int phss,unsigned int phsv,UINT *new_header_size );
-
-static int phs_decompress(unsigned char *in_buf,unsigned char *out_buf,\
-						  struct bcm_phs_rule *phs_rules, UINT *header_size);
-
-
-static ULONG PhsCompress(void* pvContext,
-				  B_UINT16 uiVcid,
-				  B_UINT16 uiClsId,
-				  void *pvInputBuffer,
-				  void *pvOutputBuffer,
-				  UINT *pOldHeaderSize,
-				  UINT *pNewHeaderSize );
-
-static ULONG PhsDeCompress(void* pvContext,
-				  B_UINT16 uiVcid,
-				  void *pvInputBuffer,
-				  void *pvOutputBuffer,
-				  UINT *pInHeaderSize,
-				  UINT *pOutHeaderSize);
+static int phs_decompress(unsigned char *in_buf, unsigned char *out_buf,
+			struct bcm_phs_rule *phs_rules, UINT *header_size);
 
+static ULONG PhsCompress(void *pvContext,
+			B_UINT16 uiVcid,
+			B_UINT16 uiClsId,
+			void *pvInputBuffer,
+			void *pvOutputBuffer,
+			UINT *pOldHeaderSize,
+			UINT *pNewHeaderSize);
 
+static ULONG PhsDeCompress(void *pvContext,
+			B_UINT16 uiVcid,
+			void *pvInputBuffer,
+			void *pvOutputBuffer,
+			UINT *pInHeaderSize,
+			UINT *pOutHeaderSize);
 
 #define IN
 #define OUT
 
 /*
-Function:				PHSTransmit
-
-Description:			This routine handle PHS(Payload Header Suppression for Tx path.
-					It extracts a fragment of the NDIS_PACKET containing the header
-					to be suppressed. It then suppresses the header by invoking PHS exported compress routine.
-					The header data after suppression is copied back to the NDIS_PACKET.
-
-
-Input parameters:		IN struct bcm_mini_adapter *Adapter         - Miniport Adapter Context
-						IN Packet 				- NDIS packet containing data to be transmitted
-						IN USHORT Vcid        - vcid pertaining to connection on which the packet is being sent.Used to
-										        identify PHS rule to be applied.
-						B_UINT16 uiClassifierRuleID - Classifier Rule ID
-						BOOLEAN bHeaderSuppressionEnabled - indicates if header suprression is enabled for SF.
-
-Return:					STATUS_SUCCESS - If the send was successful.
-						Other  - If an error occurred.
-*/
+ * Function: PHSTransmit
+ * Description:	This routine handle PHS(Payload Header Suppression for Tx path.
+ *	It extracts a fragment of the NDIS_PACKET containing the header
+ *	to be suppressed. It then suppresses the header by invoking PHS exported compress routine.
+ *	The header data after suppression is copied back to the NDIS_PACKET.
+ *
+ * Input parameters: IN struct bcm_mini_adapter *Adapter         - Miniport Adapter Context
+ *	IN Packet - NDIS packet containing data to be transmitted
+ *	IN USHORT Vcid - vcid pertaining to connection on which the packet is being sent.Used to
+ *		identify PHS rule to be applied.
+ *	B_UINT16 uiClassifierRuleID - Classifier Rule ID
+ *	BOOLEAN bHeaderSuppressionEnabled - indicates if header suprression is enabled for SF.
+ *
+ * Return:	STATUS_SUCCESS - If the send was successful.
+ *	Other  - If an error occurred.
+ */
 
 int PHSTransmit(struct bcm_mini_adapter *Adapter,
-					 struct sk_buff	**pPacket,
-					 USHORT Vcid,
-					 B_UINT16 uiClassifierRuleID,
-					 BOOLEAN bHeaderSuppressionEnabled,
-					 UINT *PacketLen,
-					 UCHAR bEthCSSupport)
+		struct sk_buff **pPacket,
+		USHORT Vcid,
+		B_UINT16 uiClassifierRuleID,
+		BOOLEAN bHeaderSuppressionEnabled,
+		UINT *PacketLen,
+		UCHAR bEthCSSupport)
 {
-
-	//PHS Sepcific
-	UINT    unPHSPktHdrBytesCopied = 0;
-	UINT	unPhsOldHdrSize = 0;
-	UINT	unPHSNewPktHeaderLen = 0;
+	/* PHS Sepcific */
+	UINT unPHSPktHdrBytesCopied = 0;
+	UINT unPhsOldHdrSize = 0;
+	UINT unPHSNewPktHeaderLen = 0;
 	/* Pointer to PHS IN Hdr Buffer */
-	PUCHAR pucPHSPktHdrInBuf =
-				Adapter->stPhsTxContextInfo.ucaHdrSuppressionInBuf;
+	PUCHAR pucPHSPktHdrInBuf = Adapter->stPhsTxContextInfo.ucaHdrSuppressionInBuf;
 	/* Pointer to PHS OUT Hdr Buffer */
-	PUCHAR  pucPHSPktHdrOutBuf =
-					Adapter->stPhsTxContextInfo.ucaHdrSuppressionOutBuf;
-	UINT       usPacketType;
-	UINT       BytesToRemove=0;
-	BOOLEAN  bPHSI = 0;
+	PUCHAR pucPHSPktHdrOutBuf = Adapter->stPhsTxContextInfo.ucaHdrSuppressionOutBuf;
+	UINT usPacketType;
+	UINT BytesToRemove = 0;
+	BOOLEAN bPHSI = 0;
 	LONG ulPhsStatus = 0;
-	UINT 	numBytesCompressed = 0;
+	UINT numBytesCompressed = 0;
 	struct sk_buff *newPacket = NULL;
 	struct sk_buff *Packet = *pPacket;
 
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL, "In PHSTransmit");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL, "In PHSTransmit");
 
-	if(!bEthCSSupport)
-		BytesToRemove=ETH_HLEN;
+	if (!bEthCSSupport)
+		BytesToRemove = ETH_HLEN;
 	/*
-		Accumulate the header upto the size we support suppression
-		from NDIS packet
-	*/
-
-	usPacketType=((struct ethhdr *)(Packet->data))->h_proto;
+	 * Accumulate the header upto the size we support suppression
+	 * from NDIS packet
+	 */
 
+	usPacketType = ((struct ethhdr *)(Packet->data))->h_proto;
 
 	pucPHSPktHdrInBuf = Packet->data + BytesToRemove;
-	//considering data after ethernet header
-	if((*PacketLen - BytesToRemove) < MAX_PHS_LENGTHS)
-	{
-
+	/* considering data after ethernet header */
+	if ((*PacketLen - BytesToRemove) < MAX_PHS_LENGTHS)
 		unPHSPktHdrBytesCopied = (*PacketLen - BytesToRemove);
-	}
 	else
-	{
 		unPHSPktHdrBytesCopied = MAX_PHS_LENGTHS;
-	}
 
-	if( (unPHSPktHdrBytesCopied > 0 ) &&
-		(unPHSPktHdrBytesCopied <= MAX_PHS_LENGTHS))
-	{
-
-
-		// Step 2 Suppress Header using PHS and fill into intermediate ucaPHSPktHdrOutBuf.
-	// Suppress only if IP Header and PHS Enabled For the Service Flow
-		if(((usPacketType == ETHERNET_FRAMETYPE_IPV4) ||
-			(usPacketType == ETHERNET_FRAMETYPE_IPV6)) &&
-			(bHeaderSuppressionEnabled))
-		{
-				BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,"\nTrying to PHS Compress Using Classifier rule 0x%X",uiClassifierRuleID);
-
-
-				unPHSNewPktHeaderLen = unPHSPktHdrBytesCopied;
-				ulPhsStatus = PhsCompress(&Adapter->stBCMPhsContext,
-					Vcid,
-					uiClassifierRuleID,
-					pucPHSPktHdrInBuf,
-					pucPHSPktHdrOutBuf,
-					&unPhsOldHdrSize,
-					&unPHSNewPktHeaderLen);
-				BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,"\nPHS Old header Size : %d New Header Size  %d\n",unPhsOldHdrSize,unPHSNewPktHeaderLen);
-
-				if(unPHSNewPktHeaderLen == unPhsOldHdrSize)
-				{
-					if(  ulPhsStatus == STATUS_PHS_COMPRESSED)
-							bPHSI = *pucPHSPktHdrOutBuf;
-					ulPhsStatus = STATUS_PHS_NOCOMPRESSION;
-				}
-
-				if(  ulPhsStatus == STATUS_PHS_COMPRESSED)
-				{
-					BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,"PHS Sending packet Compressed");
+	if ((unPHSPktHdrBytesCopied > 0) &&
+		(unPHSPktHdrBytesCopied <= MAX_PHS_LENGTHS)) {
 
-					if(skb_cloned(Packet))
-					{
-						newPacket = skb_copy(Packet, GFP_ATOMIC);
+		/*
+		 * Step 2 Suppress Header using PHS and fill into intermediate ucaPHSPktHdrOutBuf.
+		 * Suppress only if IP Header and PHS Enabled For the Service Flow
+		 */
+		if (((usPacketType == ETHERNET_FRAMETYPE_IPV4) ||
+				(usPacketType == ETHERNET_FRAMETYPE_IPV6)) &&
+			(bHeaderSuppressionEnabled)) {
+
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL, "\nTrying to PHS Compress Using Classifier rule 0x%X", uiClassifierRuleID);
+			unPHSNewPktHeaderLen = unPHSPktHdrBytesCopied;
+			ulPhsStatus = PhsCompress(&Adapter->stBCMPhsContext,
+						Vcid,
+						uiClassifierRuleID,
+						pucPHSPktHdrInBuf,
+						pucPHSPktHdrOutBuf,
+						&unPhsOldHdrSize,
+						&unPHSNewPktHeaderLen);
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL, "\nPHS Old header Size : %d New Header Size  %d\n", unPhsOldHdrSize, unPHSNewPktHeaderLen);
+
+			if (unPHSNewPktHeaderLen == unPhsOldHdrSize) {
+
+				if (ulPhsStatus == STATUS_PHS_COMPRESSED)
+					bPHSI = *pucPHSPktHdrOutBuf;
+
+				ulPhsStatus = STATUS_PHS_NOCOMPRESSION;
+			}
 
-						if(newPacket == NULL)
-							return STATUS_FAILURE;
+			if (ulPhsStatus == STATUS_PHS_COMPRESSED) {
 
-						dev_kfree_skb(Packet);
-						*pPacket = Packet = newPacket;
-						pucPHSPktHdrInBuf = Packet->data  + BytesToRemove;
-					}
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL, "PHS Sending packet Compressed");
 
-					numBytesCompressed = unPhsOldHdrSize - (unPHSNewPktHeaderLen+PHSI_LEN);
+				if (skb_cloned(Packet)) {
+					newPacket = skb_copy(Packet, GFP_ATOMIC);
 
-					memcpy(pucPHSPktHdrInBuf + numBytesCompressed, pucPHSPktHdrOutBuf, unPHSNewPktHeaderLen + PHSI_LEN);
-					memcpy(Packet->data + numBytesCompressed, Packet->data, BytesToRemove);
-					skb_pull(Packet, numBytesCompressed);
+					if (newPacket == NULL)
+						return STATUS_FAILURE;
 
-					return STATUS_SUCCESS;
+					dev_kfree_skb(Packet);
+					*pPacket = Packet = newPacket;
+					pucPHSPktHdrInBuf = Packet->data  + BytesToRemove;
 				}
 
-				else
-				{
-					//if one byte headroom is not available, increase it through skb_cow
-					if(!(skb_headroom(Packet) > 0))
-					{
-						if(skb_cow(Packet, 1))
-						{
-							BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "SKB Cow Failed\n");
-							return STATUS_FAILURE;
-						}
-					}
-					skb_push(Packet, 1);
+				numBytesCompressed = unPhsOldHdrSize - (unPHSNewPktHeaderLen + PHSI_LEN);
 
-					// CAUTION: The MAC Header is getting corrupted here for IP CS - can be saved by copying 14 Bytes.  not needed .... hence corrupting it.
-					*(Packet->data + BytesToRemove) = bPHSI;
-					return STATUS_SUCCESS;
-			}
-		}
-		else
-		{
-			if(!bHeaderSuppressionEnabled)
-			{
-				BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,"\nHeader Suppression Disabled For SF: No PHS\n");
+				memcpy(pucPHSPktHdrInBuf + numBytesCompressed, pucPHSPktHdrOutBuf, unPHSNewPktHeaderLen + PHSI_LEN);
+				memcpy(Packet->data + numBytesCompressed, Packet->data, BytesToRemove);
+				skb_pull(Packet, numBytesCompressed);
+
+				return STATUS_SUCCESS;
+			} else {
+				/* if one byte headroom is not available, increase it through skb_cow */
+				if (!(skb_headroom(Packet) > 0)) {
+
+					if (skb_cow(Packet, 1)) {
+						BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "SKB Cow Failed\n");
+						return STATUS_FAILURE;
+					}
+				}
+				skb_push(Packet, 1);
+
+				/*
+				 * CAUTION: The MAC Header is getting corrupted
+				 * here for IP CS - can be saved by copying 14
+				 * Bytes.  not needed .... hence corrupting it.
+				 */
+				*(Packet->data + BytesToRemove) = bPHSI;
+				return STATUS_SUCCESS;
 			}
+		} else {
+
+			if (!bHeaderSuppressionEnabled)
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL, "\nHeader Suppression Disabled For SF: No PHS\n");
 
 			return STATUS_SUCCESS;
 		}
 	}
 
-	//BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,"PHSTransmit : Dumping data packet After PHS");
+	/* BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,"PHSTransmit : Dumping data packet After PHS"); */
 	return STATUS_SUCCESS;
 }
 
 int PHSReceive(struct bcm_mini_adapter *Adapter,
-					USHORT usVcid,
-					struct sk_buff *packet,
-					UINT *punPacketLen,
-					UCHAR *pucEthernetHdr,
-					UINT	bHeaderSuppressionEnabled)
+	USHORT usVcid,
+	struct sk_buff *packet,
+	UINT *punPacketLen,
+	UCHAR *pucEthernetHdr,
+	UINT bHeaderSuppressionEnabled)
 {
-	u32   nStandardPktHdrLen            		= 0;
-	u32   nTotalsuppressedPktHdrBytes  = 0;
-	int     ulPhsStatus 		= 0;
-	PUCHAR pucInBuff = NULL ;
+	u32 nStandardPktHdrLen = 0;
+	u32 nTotalsuppressedPktHdrBytes = 0;
+	int ulPhsStatus	= 0;
+	PUCHAR pucInBuff = NULL;
 	UINT TotalBytesAdded = 0;
-	if(!bHeaderSuppressionEnabled)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_RECEIVE,DBG_LVL_ALL,"\nPhs Disabled for incoming packet");
+
+	if (!bHeaderSuppressionEnabled) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_RECEIVE, DBG_LVL_ALL, "\nPhs Disabled for incoming packet");
 		return ulPhsStatus;
 	}
 
 	pucInBuff = packet->data;
 
-	//Restore  PHS suppressed header
+	/* Restore PHS suppressed header */
 	nStandardPktHdrLen = packet->len;
 	ulPhsStatus = PhsDeCompress(&Adapter->stBCMPhsContext,
-		usVcid,
-		pucInBuff,
-		Adapter->ucaPHSPktRestoreBuf,
-		&nTotalsuppressedPktHdrBytes,
-		&nStandardPktHdrLen);
+				usVcid,
+				pucInBuff,
+				Adapter->ucaPHSPktRestoreBuf,
+				&nTotalsuppressedPktHdrBytes,
+				&nStandardPktHdrLen);
 
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_RECEIVE,DBG_LVL_ALL,"\nSuppressed PktHdrLen : 0x%x Restored PktHdrLen : 0x%x",
-					nTotalsuppressedPktHdrBytes,nStandardPktHdrLen);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_RECEIVE, DBG_LVL_ALL, "\nSuppressed PktHdrLen : 0x%x Restored PktHdrLen : 0x%x",
+			nTotalsuppressedPktHdrBytes, nStandardPktHdrLen);
 
-	if(ulPhsStatus != STATUS_PHS_COMPRESSED)
-	{
+	if (ulPhsStatus != STATUS_PHS_COMPRESSED) {
 		skb_pull(packet, 1);
 		return STATUS_SUCCESS;
-	}
-	else
-	{
+	} else {
 		TotalBytesAdded = nStandardPktHdrLen - nTotalsuppressedPktHdrBytes - PHSI_LEN;
-		if(TotalBytesAdded)
-		{
-			if(skb_headroom(packet) >= (SKB_RESERVE_ETHERNET_HEADER + TotalBytesAdded))
+
+		if (TotalBytesAdded) {
+			if (skb_headroom(packet) >= (SKB_RESERVE_ETHERNET_HEADER + TotalBytesAdded))
 				skb_push(packet, TotalBytesAdded);
-			else
-			{
-				if(skb_cow(packet, skb_headroom(packet) + TotalBytesAdded))
-				{
-					BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "cow failed in receive\n");
+			else {
+				if (skb_cow(packet, skb_headroom(packet) + TotalBytesAdded)) {
+					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "cow failed in receive\n");
 					return STATUS_FAILURE;
 				}
 
@@ -271,90 +246,80 @@ int PHSReceive(struct bcm_mini_adapter *Adapter,
 	return STATUS_SUCCESS;
 }
 
-void DumpFullPacket(UCHAR *pBuf,UINT nPktLen)
+void DumpFullPacket(UCHAR *pBuf, UINT nPktLen)
 {
 	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-    BCM_DEBUG_PRINT(Adapter,DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,"Dumping Data Packet");
-    BCM_DEBUG_PRINT_BUFFER(Adapter,DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,pBuf,nPktLen);
+
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Dumping Data Packet");
+	BCM_DEBUG_PRINT_BUFFER(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, pBuf, nPktLen);
 }
 
-//-----------------------------------------------------------------------------
-// Procedure:   phs_init
-//
-// Description: This routine is responsible for allocating memory for classifier and
-// PHS rules.
-//
-// Arguments:
-// pPhsdeviceExtension - ptr to Device extension containing PHS Classifier rules and PHS Rules , RX, TX buffer etc
-//
-// Returns:
-// TRUE(1)	-If allocation of memory was success full.
-// FALSE	-If allocation of memory fails.
-//-----------------------------------------------------------------------------
+/*
+ * Procedure:   phs_init
+ *
+ * Description: This routine is responsible for allocating memory for classifier and
+ * PHS rules.
+ *
+ * Arguments:
+ * pPhsdeviceExtension - ptr to Device extension containing PHS Classifier rules and PHS Rules , RX, TX buffer etc
+ *
+ * Returns:
+ * TRUE(1)	-If allocation of memory was successful.
+ * FALSE	-If allocation of memory fails.
+ */
 int phs_init(struct bcm_phs_extension *pPhsdeviceExtension, struct bcm_mini_adapter *Adapter)
 {
 	int i;
 	struct bcm_phs_table *pstServiceFlowTable;
-    BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "\nPHS:phs_init function ");
 
-	if(pPhsdeviceExtension->pstServiceFlowPhsRulesTable)
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "\nPHS:phs_init function");
+
+	if (pPhsdeviceExtension->pstServiceFlowPhsRulesTable)
 		return -EINVAL;
 
-	pPhsdeviceExtension->pstServiceFlowPhsRulesTable =
-		kzalloc(sizeof(struct bcm_phs_table), GFP_KERNEL);
+	pPhsdeviceExtension->pstServiceFlowPhsRulesTable = kzalloc(sizeof(struct bcm_phs_table), GFP_KERNEL);
 
-    if(!pPhsdeviceExtension->pstServiceFlowPhsRulesTable)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "\nAllocation ServiceFlowPhsRulesTable failed");
+	if (!pPhsdeviceExtension->pstServiceFlowPhsRulesTable) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "\nAllocation ServiceFlowPhsRulesTable failed");
 		return -ENOMEM;
 	}
 
 	pstServiceFlowTable = pPhsdeviceExtension->pstServiceFlowPhsRulesTable;
-	for(i=0;i<MAX_SERVICEFLOWS;i++)
-	{
+	for (i = 0; i < MAX_SERVICEFLOWS; i++) {
 		struct bcm_phs_entry sServiceFlow = pstServiceFlowTable->stSFList[i];
 		sServiceFlow.pstClassifierTable = kzalloc(sizeof(struct bcm_phs_classifier_table), GFP_KERNEL);
-		if(!sServiceFlow.pstClassifierTable)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "\nAllocation failed");
-			free_phs_serviceflow_rules(pPhsdeviceExtension->
-                pstServiceFlowPhsRulesTable);
+		if (!sServiceFlow.pstClassifierTable) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "\nAllocation failed");
+			free_phs_serviceflow_rules(pPhsdeviceExtension->pstServiceFlowPhsRulesTable);
 			pPhsdeviceExtension->pstServiceFlowPhsRulesTable = NULL;
 			return -ENOMEM;
 		}
 	}
 
 	pPhsdeviceExtension->CompressedTxBuffer = kmalloc(PHS_BUFFER_SIZE, GFP_KERNEL);
-
-    if(pPhsdeviceExtension->CompressedTxBuffer == NULL)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "\nAllocation failed");
+	if (pPhsdeviceExtension->CompressedTxBuffer == NULL) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "\nAllocation failed");
 		free_phs_serviceflow_rules(pPhsdeviceExtension->pstServiceFlowPhsRulesTable);
 		pPhsdeviceExtension->pstServiceFlowPhsRulesTable = NULL;
 		return -ENOMEM;
 	}
 
-    pPhsdeviceExtension->UnCompressedRxBuffer = kmalloc(PHS_BUFFER_SIZE, GFP_KERNEL);
-	if(pPhsdeviceExtension->UnCompressedRxBuffer == NULL)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "\nAllocation failed");
+	pPhsdeviceExtension->UnCompressedRxBuffer = kmalloc(PHS_BUFFER_SIZE, GFP_KERNEL);
+	if (pPhsdeviceExtension->UnCompressedRxBuffer == NULL) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "\nAllocation failed");
 		kfree(pPhsdeviceExtension->CompressedTxBuffer);
 		free_phs_serviceflow_rules(pPhsdeviceExtension->pstServiceFlowPhsRulesTable);
 		pPhsdeviceExtension->pstServiceFlowPhsRulesTable = NULL;
 		return -ENOMEM;
 	}
 
-
-
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "\n phs_init Successful");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "\n phs_init Successful");
 	return STATUS_SUCCESS;
 }
 
-
 int PhsCleanup(IN struct bcm_phs_extension *pPHSDeviceExt)
 {
-	if(pPHSDeviceExt->pstServiceFlowPhsRulesTable)
-	{
+	if (pPHSDeviceExt->pstServiceFlowPhsRulesTable) {
 		free_phs_serviceflow_rules(pPHSDeviceExt->pstServiceFlowPhsRulesTable);
 		pPHSDeviceExt->pstServiceFlowPhsRulesTable = NULL;
 	}
@@ -368,200 +333,173 @@ int PhsCleanup(IN struct bcm_phs_extension *pPHSDeviceExt)
 	return 0;
 }
 
-
-
-//PHS functions
-/*++
-PhsUpdateClassifierRule
-
-Routine Description:
-    Exported function to add or modify a PHS Rule.
-
-Arguments:
-	IN void* pvContext - PHS Driver Specific Context
-	IN B_UINT16 uiVcid    - The Service Flow ID for which the PHS rule applies
-	IN B_UINT16  uiClsId   - The Classifier ID within the Service Flow for which the PHS rule applies.
-	IN struct bcm_phs_rule *psPhsRule - The PHS Rule strcuture to be added to the PHS Rule table.
-
-Return Value:
-
-    0 if successful,
-    >0 Error.
-
---*/
-ULONG PhsUpdateClassifierRule(IN void* pvContext,
-								IN B_UINT16  uiVcid ,
-								IN B_UINT16  uiClsId   ,
-								IN struct bcm_phs_rule *psPhsRule,
-								IN B_UINT8  u8AssociatedPHSI)
+/*
+ * PHS functions
+ * PhsUpdateClassifierRule
+ *
+ * Routine Description:
+ *   Exported function to add or modify a PHS Rule.
+ *
+ * Arguments:
+ *	IN void* pvContext - PHS Driver Specific Context
+ *	IN B_UINT16 uiVcid    - The Service Flow ID for which the PHS rule applies
+ *	IN B_UINT16  uiClsId   - The Classifier ID within the Service Flow for which the PHS rule applies.
+ *	IN struct bcm_phs_rule *psPhsRule - The PHS Rule strcuture to be added to the PHS Rule table.
+ *
+ * Return Value:
+ *
+ * 0 if successful,
+ * >0 Error.
+ */
+ULONG PhsUpdateClassifierRule(IN void *pvContext,
+			IN B_UINT16 uiVcid ,
+			IN B_UINT16 uiClsId   ,
+			IN struct bcm_phs_rule *psPhsRule,
+			IN B_UINT8 u8AssociatedPHSI)
 {
-	ULONG lStatus =0;
-	UINT nSFIndex =0 ;
+	ULONG lStatus = 0;
+	UINT nSFIndex = 0;
 	struct bcm_phs_entry *pstServiceFlowEntry = NULL;
 	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_phs_extension *pDeviceExtension = (struct bcm_phs_extension *)pvContext;
 
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "PHS With Corr2 Changes\n");
 
-
-	struct bcm_phs_extension *pDeviceExtension= (struct bcm_phs_extension *)pvContext;
-
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL,"PHS With Corr2 Changes \n");
-
-	if(pDeviceExtension == NULL)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL,"Invalid Device Extension\n");
+	if (pDeviceExtension == NULL) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "Invalid Device Extension\n");
 		return ERR_PHS_INVALID_DEVICE_EXETENSION;
 	}
 
-
-	if(u8AssociatedPHSI == 0)
-	{
+	if (u8AssociatedPHSI == 0)
 		return ERR_PHS_INVALID_PHS_RULE;
-	}
 
 	/* Retrieve the SFID Entry Index for requested Service Flow */
-
 	nSFIndex = GetServiceFlowEntry(pDeviceExtension->pstServiceFlowPhsRulesTable,
-	                uiVcid,&pstServiceFlowEntry);
+				uiVcid, &pstServiceFlowEntry);
 
-    if(nSFIndex == PHS_INVALID_TABLE_INDEX)
-	{
+	if (nSFIndex == PHS_INVALID_TABLE_INDEX) {
 		/* This is a new SF. Create a mapping entry for this */
 		lStatus = CreateSFToClassifierRuleMapping(uiVcid, uiClsId,
-		      pDeviceExtension->pstServiceFlowPhsRulesTable, psPhsRule, u8AssociatedPHSI);
+							pDeviceExtension->pstServiceFlowPhsRulesTable, psPhsRule, u8AssociatedPHSI);
 		return lStatus;
 	}
 
 	/* SF already Exists Add PHS Rule to existing SF */
-  	lStatus = CreateClassiferToPHSRuleMapping(uiVcid, uiClsId,
-  	          pstServiceFlowEntry, psPhsRule, u8AssociatedPHSI);
+	lStatus = CreateClassiferToPHSRuleMapping(uiVcid, uiClsId,
+						pstServiceFlowEntry, psPhsRule, u8AssociatedPHSI);
 
-    return lStatus;
+	return lStatus;
 }
 
-/*++
-PhsDeletePHSRule
-
-Routine Description:
-   Deletes the specified phs Rule within Vcid
-
-Arguments:
-	IN void* pvContext - PHS Driver Specific Context
-	IN B_UINT16  uiVcid    - The Service Flow ID for which the PHS rule applies
-	IN B_UINT8  u8PHSI   - the PHS Index identifying PHS rule to be deleted.
-
-Return Value:
-
-    0 if successful,
-    >0 Error.
-
---*/
-
-ULONG PhsDeletePHSRule(IN void* pvContext,IN B_UINT16 uiVcid,IN B_UINT8 u8PHSI)
+/*
+ * PhsDeletePHSRule
+ *
+ * Routine Description:
+ *   Deletes the specified phs Rule within Vcid
+ *
+ * Arguments:
+ *	IN void* pvContext - PHS Driver Specific Context
+ *	IN B_UINT16  uiVcid    - The Service Flow ID for which the PHS rule applies
+ *	IN B_UINT8  u8PHSI   - the PHS Index identifying PHS rule to be deleted.
+ *
+ * Return Value:
+ *
+ * 0 if successful,
+ * >0 Error.
+ */
+ULONG PhsDeletePHSRule(IN void *pvContext, IN B_UINT16 uiVcid, IN B_UINT8 u8PHSI)
 {
-	ULONG lStatus =0;
-	UINT nSFIndex =0, nClsidIndex =0 ;
+	ULONG lStatus = 0;
+	UINT nSFIndex = 0, nClsidIndex = 0;
 	struct bcm_phs_entry *pstServiceFlowEntry = NULL;
 	struct bcm_phs_classifier_table *pstClassifierRulesTable = NULL;
 	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_phs_extension *pDeviceExtension = (struct bcm_phs_extension *)pvContext;
 
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "======>\n");
 
-	struct bcm_phs_extension *pDeviceExtension= (struct bcm_phs_extension *)pvContext;
+	if (pDeviceExtension) {
+		/* Retrieve the SFID Entry Index for requested Service Flow */
+		nSFIndex = GetServiceFlowEntry(pDeviceExtension->pstServiceFlowPhsRulesTable, uiVcid, &pstServiceFlowEntry);
 
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "======>\n");
-
-	if(pDeviceExtension)
-	{
-
-		//Retrieve the SFID Entry Index for requested Service Flow
-		nSFIndex = GetServiceFlowEntry(pDeviceExtension
-		      ->pstServiceFlowPhsRulesTable,uiVcid,&pstServiceFlowEntry);
-
-       if(nSFIndex == PHS_INVALID_TABLE_INDEX)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "SFID Match Failed\n");
+		if (nSFIndex == PHS_INVALID_TABLE_INDEX) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "SFID Match Failed\n");
 			return ERR_SF_MATCH_FAIL;
 		}
 
-		pstClassifierRulesTable=pstServiceFlowEntry->pstClassifierTable;
-		if(pstClassifierRulesTable)
-		{
-			for(nClsidIndex=0;nClsidIndex<MAX_PHSRULE_PER_SF;nClsidIndex++)
-			{
-				if(pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex].bUsed && pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex].pstPhsRule)
-				{
-					if(pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex].pstPhsRule->u8PHSI == u8PHSI)					{
-						if(pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex].pstPhsRule->u8RefCnt)
+		pstClassifierRulesTable = pstServiceFlowEntry->pstClassifierTable;
+		if (pstClassifierRulesTable) {
+			for (nClsidIndex = 0; nClsidIndex < MAX_PHSRULE_PER_SF; nClsidIndex++) {
+				if (pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex].bUsed && pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex].pstPhsRule) {
+					if (pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex].pstPhsRule->u8PHSI == u8PHSI) {
+
+						if (pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex].pstPhsRule->u8RefCnt)
 							pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex].pstPhsRule->u8RefCnt--;
-						if(0 == pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex].pstPhsRule->u8RefCnt)
+
+						if (0 == pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex].pstPhsRule->u8RefCnt)
 							kfree(pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex].pstPhsRule);
+
 						memset(&pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex], 0,
 							sizeof(struct bcm_phs_classifier_entry));
 					}
 				}
 			}
 		}
-
 	}
 	return lStatus;
 }
 
-/*++
-PhsDeleteClassifierRule
-
-Routine Description:
-    Exported function to Delete a PHS Rule for the SFID,CLSID Pair.
-
-Arguments:
-	IN void* pvContext - PHS Driver Specific Context
-	IN B_UINT16  uiVcid    - The Service Flow ID for which the PHS rule applies
-	IN B_UINT16  uiClsId   - The Classifier ID within the Service Flow for which the PHS rule applies.
-
-Return Value:
-
-    0 if successful,
-    >0 Error.
-
---*/
-ULONG PhsDeleteClassifierRule(IN void* pvContext,IN B_UINT16 uiVcid ,IN B_UINT16  uiClsId)
+/*
+ * PhsDeleteClassifierRule
+ *
+ * Routine Description:
+ *    Exported function to Delete a PHS Rule for the SFID,CLSID Pair.
+ *
+ * Arguments:
+ *	IN void* pvContext - PHS Driver Specific Context
+ *	IN B_UINT16  uiVcid    - The Service Flow ID for which the PHS rule applies
+ *	IN B_UINT16  uiClsId   - The Classifier ID within the Service Flow for which the PHS rule applies.
+ *
+ * Return Value:
+ *
+ * 0 if successful,
+ * >0 Error.
+ */
+ULONG PhsDeleteClassifierRule(IN void *pvContext, IN B_UINT16 uiVcid, IN B_UINT16 uiClsId)
 {
-	ULONG lStatus =0;
-	UINT nSFIndex =0, nClsidIndex =0 ;
+	ULONG lStatus = 0;
+	UINT nSFIndex = 0, nClsidIndex = 0;
 	struct bcm_phs_entry *pstServiceFlowEntry = NULL;
 	struct bcm_phs_classifier_entry *pstClassifierEntry = NULL;
 	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-	struct bcm_phs_extension *pDeviceExtension= (struct bcm_phs_extension *)pvContext;
-
-	if(pDeviceExtension)
-	{
-		//Retrieve the SFID Entry Index for requested Service Flow
-		nSFIndex = GetServiceFlowEntry(pDeviceExtension
-		      ->pstServiceFlowPhsRulesTable, uiVcid, &pstServiceFlowEntry);
-		if(nSFIndex == PHS_INVALID_TABLE_INDEX)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL,"SFID Match Failed\n");
+	struct bcm_phs_extension *pDeviceExtension = (struct bcm_phs_extension *)pvContext;
+
+	if (pDeviceExtension) {
+		/* Retrieve the SFID Entry Index for requested Service Flow */
+		nSFIndex = GetServiceFlowEntry(pDeviceExtension->pstServiceFlowPhsRulesTable, uiVcid, &pstServiceFlowEntry);
+		if (nSFIndex == PHS_INVALID_TABLE_INDEX) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "SFID Match Failed\n");
 			return ERR_SF_MATCH_FAIL;
 		}
 
 		nClsidIndex = GetClassifierEntry(pstServiceFlowEntry->pstClassifierTable,
-                  uiClsId, eActiveClassifierRuleContext, &pstClassifierEntry);
-		if((nClsidIndex != PHS_INVALID_TABLE_INDEX) && (!pstClassifierEntry->bUnclassifiedPHSRule))
-		{
-			if(pstClassifierEntry->pstPhsRule)
-			{
-				if(pstClassifierEntry->pstPhsRule->u8RefCnt)
-				pstClassifierEntry->pstPhsRule->u8RefCnt--;
-				if(0==pstClassifierEntry->pstPhsRule->u8RefCnt)
-					kfree(pstClassifierEntry->pstPhsRule);
+						uiClsId, eActiveClassifierRuleContext, &pstClassifierEntry);
 
+		if ((nClsidIndex != PHS_INVALID_TABLE_INDEX) && (!pstClassifierEntry->bUnclassifiedPHSRule)) {
+			if (pstClassifierEntry->pstPhsRule) {
+				if (pstClassifierEntry->pstPhsRule->u8RefCnt)
+					pstClassifierEntry->pstPhsRule->u8RefCnt--;
+
+				if (0 == pstClassifierEntry->pstPhsRule->u8RefCnt)
+					kfree(pstClassifierEntry->pstPhsRule);
 			}
 			memset(pstClassifierEntry, 0, sizeof(struct bcm_phs_classifier_entry));
 		}
 
 		nClsidIndex = GetClassifierEntry(pstServiceFlowEntry->pstClassifierTable,
-                    uiClsId,eOldClassifierRuleContext,&pstClassifierEntry);
+						uiClsId, eOldClassifierRuleContext, &pstClassifierEntry);
 
-	   if((nClsidIndex != PHS_INVALID_TABLE_INDEX) && (!pstClassifierEntry->bUnclassifiedPHSRule))
-		{
+		if ((nClsidIndex != PHS_INVALID_TABLE_INDEX) && (!pstClassifierEntry->bUnclassifiedPHSRule)) {
 			kfree(pstClassifierEntry->pstPhsRule);
 			memset(pstClassifierEntry, 0, sizeof(struct bcm_phs_classifier_entry));
 		}
@@ -569,261 +507,223 @@ ULONG PhsDeleteClassifierRule(IN void* pvContext,IN B_UINT16 uiVcid ,IN B_UINT16
 	return lStatus;
 }
 
-/*++
-PhsDeleteSFRules
-
-Routine Description:
-    Exported function to Delete a all PHS Rules for the SFID.
-
-Arguments:
-	IN void* pvContext - PHS Driver Specific Context
-	IN B_UINT16 uiVcid   - The Service Flow ID for which the PHS rules need to be deleted
-
-Return Value:
-
-    0 if successful,
-    >0 Error.
-
---*/
-ULONG PhsDeleteSFRules(IN void* pvContext,IN B_UINT16 uiVcid)
+/*
+ * PhsDeleteSFRules
+ *
+ * Routine Description:
+ *    Exported function to Delete a all PHS Rules for the SFID.
+ *
+ * Arguments:
+ *	IN void* pvContext - PHS Driver Specific Context
+ *	IN B_UINT16 uiVcid   - The Service Flow ID for which the PHS rules need to be deleted
+ *
+ * Return Value:
+ *
+ * 0 if successful,
+ * >0 Error.
+ */
+ULONG PhsDeleteSFRules(IN void *pvContext, IN B_UINT16 uiVcid)
 {
-
-	ULONG lStatus =0;
-	UINT nSFIndex =0, nClsidIndex =0  ;
+	ULONG lStatus = 0;
+	UINT nSFIndex = 0, nClsidIndex = 0;
 	struct bcm_phs_entry *pstServiceFlowEntry = NULL;
 	struct bcm_phs_classifier_table *pstClassifierRulesTable = NULL;
 	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-	struct bcm_phs_extension *pDeviceExtension= (struct bcm_phs_extension *)pvContext;
-    BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL,"====> \n");
+	struct bcm_phs_extension *pDeviceExtension = (struct bcm_phs_extension *)pvContext;
+
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "====>\n");
 
-	if(pDeviceExtension)
-	{
-		//Retrieve the SFID Entry Index for requested Service Flow
+	if (pDeviceExtension) {
+		/* Retrieve the SFID Entry Index for requested Service Flow */
 		nSFIndex = GetServiceFlowEntry(pDeviceExtension->pstServiceFlowPhsRulesTable,
-		                  uiVcid,&pstServiceFlowEntry);
-		if(nSFIndex == PHS_INVALID_TABLE_INDEX)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "SFID Match Failed\n");
+					uiVcid, &pstServiceFlowEntry);
+		if (nSFIndex == PHS_INVALID_TABLE_INDEX) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "SFID Match Failed\n");
 			return ERR_SF_MATCH_FAIL;
 		}
 
-		pstClassifierRulesTable=pstServiceFlowEntry->pstClassifierTable;
-		if(pstClassifierRulesTable)
-		{
-			for(nClsidIndex=0;nClsidIndex<MAX_PHSRULE_PER_SF;nClsidIndex++)
-			{
-				if(pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex].pstPhsRule)
-				{
-					if(pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex]
-                                                        .pstPhsRule->u8RefCnt)
-						pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex]
-						                                    .pstPhsRule->u8RefCnt--;
-					if(0==pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex]
-                                                          .pstPhsRule->u8RefCnt)
+		pstClassifierRulesTable = pstServiceFlowEntry->pstClassifierTable;
+		if (pstClassifierRulesTable) {
+			for (nClsidIndex = 0; nClsidIndex < MAX_PHSRULE_PER_SF; nClsidIndex++) {
+				if (pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex].pstPhsRule) {
+
+					if (pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex].pstPhsRule->u8RefCnt)
+						pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex].pstPhsRule->u8RefCnt--;
+
+					if (0 == pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex].pstPhsRule->u8RefCnt)
 						kfree(pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex].pstPhsRule);
-					    pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex]
-                                        .pstPhsRule = NULL;
+
+					pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex].pstPhsRule = NULL;
 				}
 				memset(&pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex], 0, sizeof(struct bcm_phs_classifier_entry));
-				if(pstClassifierRulesTable->stOldPhsRulesList[nClsidIndex].pstPhsRule)
-				{
-					if(pstClassifierRulesTable->stOldPhsRulesList[nClsidIndex]
-                                        .pstPhsRule->u8RefCnt)
-						pstClassifierRulesTable->stOldPhsRulesList[nClsidIndex]
-						                  .pstPhsRule->u8RefCnt--;
-					if(0 == pstClassifierRulesTable->stOldPhsRulesList[nClsidIndex]
-                                        .pstPhsRule->u8RefCnt)
-						kfree(pstClassifierRulesTable
-						      ->stOldPhsRulesList[nClsidIndex].pstPhsRule);
-					pstClassifierRulesTable->stOldPhsRulesList[nClsidIndex]
-                              .pstPhsRule = NULL;
+				if (pstClassifierRulesTable->stOldPhsRulesList[nClsidIndex].pstPhsRule) {
+
+					if (pstClassifierRulesTable->stOldPhsRulesList[nClsidIndex].pstPhsRule->u8RefCnt)
+						pstClassifierRulesTable->stOldPhsRulesList[nClsidIndex].pstPhsRule->u8RefCnt--;
+
+					if (0 == pstClassifierRulesTable->stOldPhsRulesList[nClsidIndex].pstPhsRule->u8RefCnt)
+						kfree(pstClassifierRulesTable->stOldPhsRulesList[nClsidIndex].pstPhsRule);
+
+					pstClassifierRulesTable->stOldPhsRulesList[nClsidIndex].pstPhsRule = NULL;
 				}
 				memset(&pstClassifierRulesTable->stOldPhsRulesList[nClsidIndex], 0, sizeof(struct bcm_phs_classifier_entry));
 			}
 		}
 		pstServiceFlowEntry->bUsed = FALSE;
 		pstServiceFlowEntry->uiVcid = 0;
-
 	}
 
 	return lStatus;
 }
 
-
-/*++
-PhsCompress
-
-Routine Description:
-    Exported function to compress the data using PHS.
-
-Arguments:
-	IN void* pvContext - PHS Driver Specific Context.
-	IN B_UINT16 uiVcid    - The Service Flow ID to which current packet header compression applies.
-	IN UINT  uiClsId   - The Classifier ID to which current packet header compression applies.
-	IN void *pvInputBuffer - The Input buffer containg packet header data
-	IN void *pvOutputBuffer - The output buffer returned by this function after PHS
-	IN UINT *pOldHeaderSize  - The actual size of the header before PHS
-	IN UINT *pNewHeaderSize - The new size of the header after applying PHS
-
-Return Value:
-
-    0 if successful,
-    >0 Error.
-
---*/
-ULONG PhsCompress(IN void* pvContext,
-				  IN B_UINT16 uiVcid,
-				  IN B_UINT16 uiClsId,
-				  IN void *pvInputBuffer,
-				  OUT void *pvOutputBuffer,
-				  OUT UINT *pOldHeaderSize,
-				  OUT UINT *pNewHeaderSize )
+/*
+ * PhsCompress
+ *
+ * Routine Description:
+ *    Exported function to compress the data using PHS.
+ *
+ * Arguments:
+ *	IN void* pvContext - PHS Driver Specific Context.
+ *	IN B_UINT16 uiVcid    - The Service Flow ID to which current packet header compression applies.
+ *	IN UINT  uiClsId   - The Classifier ID to which current packet header compression applies.
+ *	IN void *pvInputBuffer - The Input buffer containg packet header data
+ *	IN void *pvOutputBuffer - The output buffer returned by this function after PHS
+ *	IN UINT *pOldHeaderSize  - The actual size of the header before PHS
+ *	IN UINT *pNewHeaderSize - The new size of the header after applying PHS
+ *
+ * Return Value:
+ *
+ * 0 if successful,
+ * >0 Error.
+ */
+ULONG PhsCompress(IN void *pvContext,
+		IN B_UINT16 uiVcid,
+		IN B_UINT16 uiClsId,
+		IN void *pvInputBuffer,
+		OUT void *pvOutputBuffer,
+		OUT UINT *pOldHeaderSize,
+		OUT UINT *pNewHeaderSize)
 {
-	UINT nSFIndex =0, nClsidIndex =0  ;
+	UINT nSFIndex = 0, nClsidIndex = 0;
 	struct bcm_phs_entry *pstServiceFlowEntry = NULL;
 	struct bcm_phs_classifier_entry *pstClassifierEntry = NULL;
 	struct bcm_phs_rule *pstPhsRule = NULL;
-	ULONG lStatus =0;
+	ULONG lStatus = 0;
 	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_phs_extension *pDeviceExtension = (struct bcm_phs_extension *)pvContext;
 
-
-
-	struct bcm_phs_extension *pDeviceExtension= (struct bcm_phs_extension *)pvContext;
-
-
-	if(pDeviceExtension == NULL)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,"Invalid Device Extension\n");
-		lStatus =  STATUS_PHS_NOCOMPRESSION ;
+	if (pDeviceExtension == NULL) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL, "Invalid Device Extension\n");
+		lStatus = STATUS_PHS_NOCOMPRESSION;
 		return lStatus;
-
 	}
 
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,"Suppressing header \n");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL, "Suppressing header\n");
 
-
-	//Retrieve the SFID Entry Index for requested Service Flow
+	/* Retrieve the SFID Entry Index for requested Service Flow */
 	nSFIndex = GetServiceFlowEntry(pDeviceExtension->pstServiceFlowPhsRulesTable,
-	                  uiVcid,&pstServiceFlowEntry);
-	if(nSFIndex == PHS_INVALID_TABLE_INDEX)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,"SFID Match Failed\n");
-		lStatus =  STATUS_PHS_NOCOMPRESSION ;
+				uiVcid, &pstServiceFlowEntry);
+	if (nSFIndex == PHS_INVALID_TABLE_INDEX) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL, "SFID Match Failed\n");
+		lStatus = STATUS_PHS_NOCOMPRESSION;
 		return lStatus;
 	}
 
 	nClsidIndex = GetClassifierEntry(pstServiceFlowEntry->pstClassifierTable,
-                uiClsId,eActiveClassifierRuleContext,&pstClassifierEntry);
+					uiClsId, eActiveClassifierRuleContext, &pstClassifierEntry);
 
-    if(nClsidIndex == PHS_INVALID_TABLE_INDEX)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,"No PHS Rule Defined For Classifier\n");
-		lStatus =  STATUS_PHS_NOCOMPRESSION ;
+	if (nClsidIndex == PHS_INVALID_TABLE_INDEX) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL, "No PHS Rule Defined For Classifier\n");
+		lStatus =  STATUS_PHS_NOCOMPRESSION;
 		return lStatus;
 	}
 
-
-	//get rule from SF id,Cls ID pair and proceed
-	pstPhsRule =  pstClassifierEntry->pstPhsRule;
-
-	if(!ValidatePHSRuleComplete(pstPhsRule))
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL,"PHS Rule Defined For Classifier But Not Complete\n");
-		lStatus =  STATUS_PHS_NOCOMPRESSION ;
+	/* get rule from SF id,Cls ID pair and proceed */
+	pstPhsRule = pstClassifierEntry->pstPhsRule;
+	if (!ValidatePHSRuleComplete(pstPhsRule)) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "PHS Rule Defined For Classifier But Not Complete\n");
+		lStatus = STATUS_PHS_NOCOMPRESSION;
 		return lStatus;
 	}
 
-	//Compress Packet
-	lStatus = phs_compress(pstPhsRule,(PUCHAR)pvInputBuffer,
-	      (PUCHAR)pvOutputBuffer, pOldHeaderSize,pNewHeaderSize);
+	/* Compress Packet */
+	lStatus = phs_compress(pstPhsRule, (PUCHAR)pvInputBuffer,
+			(PUCHAR)pvOutputBuffer, pOldHeaderSize, pNewHeaderSize);
 
-	if(lStatus == STATUS_PHS_COMPRESSED)
-	{
+	if (lStatus == STATUS_PHS_COMPRESSED) {
 		pstPhsRule->PHSModifiedBytes += *pOldHeaderSize - *pNewHeaderSize - 1;
 		pstPhsRule->PHSModifiedNumPackets++;
-	}
-	else
+	} else
 		pstPhsRule->PHSErrorNumPackets++;
 
 	return lStatus;
 }
 
-/*++
-PhsDeCompress
-
-Routine Description:
-    Exported function to restore the packet header in Rx path.
-
-Arguments:
-	IN void* pvContext - PHS Driver Specific Context.
-	IN B_UINT16 uiVcid    - The Service Flow ID to which current packet header restoration applies.
-	IN  void *pvInputBuffer - The Input buffer containg suppressed packet header data
-	OUT void *pvOutputBuffer - The output buffer returned by this function after restoration
-	OUT UINT *pHeaderSize   - The packet header size after restoration is returned in this parameter.
-
-Return Value:
-
-    0 if successful,
-    >0 Error.
-
---*/
-ULONG PhsDeCompress(IN void* pvContext,
-				  IN B_UINT16 uiVcid,
-				  IN void *pvInputBuffer,
-				  OUT void *pvOutputBuffer,
-				  OUT UINT *pInHeaderSize,
-				  OUT UINT *pOutHeaderSize )
+/*
+ * PhsDeCompress
+ *
+ * Routine Description:
+ *    Exported function to restore the packet header in Rx path.
+ *
+ * Arguments:
+ *	IN void* pvContext - PHS Driver Specific Context.
+ *	IN B_UINT16 uiVcid    - The Service Flow ID to which current packet header restoration applies.
+ *	IN  void *pvInputBuffer - The Input buffer containg suppressed packet header data
+ *	OUT void *pvOutputBuffer - The output buffer returned by this function after restoration
+ *	OUT UINT *pHeaderSize   - The packet header size after restoration is returned in this parameter.
+ *
+ * Return Value:
+ *
+ * 0 if successful,
+ * >0 Error.
+ */
+ULONG PhsDeCompress(IN void *pvContext,
+		IN B_UINT16 uiVcid,
+		IN void *pvInputBuffer,
+		OUT void *pvOutputBuffer,
+		OUT UINT *pInHeaderSize,
+		OUT UINT *pOutHeaderSize)
 {
-	UINT nSFIndex =0, nPhsRuleIndex =0 ;
+	UINT nSFIndex = 0, nPhsRuleIndex = 0;
 	struct bcm_phs_entry *pstServiceFlowEntry = NULL;
 	struct bcm_phs_rule *pstPhsRule = NULL;
 	UINT phsi;
 	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-	struct bcm_phs_extension *pDeviceExtension=
-        (struct bcm_phs_extension *)pvContext;
+	struct bcm_phs_extension *pDeviceExtension = (struct bcm_phs_extension *)pvContext;
 
 	*pInHeaderSize = 0;
-
-	if(pDeviceExtension == NULL)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_RECEIVE,DBG_LVL_ALL,"Invalid Device Extension\n");
+	if (pDeviceExtension == NULL) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_RECEIVE, DBG_LVL_ALL, "Invalid Device Extension\n");
 		return ERR_PHS_INVALID_DEVICE_EXETENSION;
 	}
 
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_RECEIVE,DBG_LVL_ALL,"Restoring header\n");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_RECEIVE, DBG_LVL_ALL, "Restoring header\n");
 
 	phsi = *((unsigned char *)(pvInputBuffer));
-    BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_RECEIVE,DBG_LVL_ALL,"PHSI To Be Used For restore : %x\n",phsi);
-    if(phsi == UNCOMPRESSED_PACKET )
-	{
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_RECEIVE, DBG_LVL_ALL, "PHSI To Be Used For restore : %x\n", phsi);
+	if (phsi == UNCOMPRESSED_PACKET)
 		return STATUS_PHS_NOCOMPRESSION;
-	}
 
-	//Retrieve the SFID Entry Index for requested Service Flow
+	/* Retrieve the SFID Entry Index for requested Service Flow */
 	nSFIndex = GetServiceFlowEntry(pDeviceExtension->pstServiceFlowPhsRulesTable,
-	      uiVcid,&pstServiceFlowEntry);
-	if(nSFIndex == PHS_INVALID_TABLE_INDEX)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_RECEIVE,DBG_LVL_ALL,"SFID Match Failed During Lookup\n");
+				uiVcid, &pstServiceFlowEntry);
+	if (nSFIndex == PHS_INVALID_TABLE_INDEX) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_RECEIVE, DBG_LVL_ALL, "SFID Match Failed During Lookup\n");
 		return ERR_SF_MATCH_FAIL;
 	}
 
-	nPhsRuleIndex = GetPhsRuleEntry(pstServiceFlowEntry->pstClassifierTable,phsi,
-          eActiveClassifierRuleContext,&pstPhsRule);
-	if(nPhsRuleIndex == PHS_INVALID_TABLE_INDEX)
-	{
-		//Phs Rule does not exist in  active rules table. Lets try in the old rules table.
+	nPhsRuleIndex = GetPhsRuleEntry(pstServiceFlowEntry->pstClassifierTable, phsi,
+					eActiveClassifierRuleContext, &pstPhsRule);
+	if (nPhsRuleIndex == PHS_INVALID_TABLE_INDEX) {
+		/* Phs Rule does not exist in  active rules table. Lets try in the old rules table. */
 		nPhsRuleIndex = GetPhsRuleEntry(pstServiceFlowEntry->pstClassifierTable,
-		      phsi,eOldClassifierRuleContext,&pstPhsRule);
-		if(nPhsRuleIndex == PHS_INVALID_TABLE_INDEX)
-		{
+						phsi, eOldClassifierRuleContext, &pstPhsRule);
+		if (nPhsRuleIndex == PHS_INVALID_TABLE_INDEX)
 			return ERR_PHSRULE_MATCH_FAIL;
-		}
-
 	}
 
 	*pInHeaderSize = phs_decompress((PUCHAR)pvInputBuffer,
-            (PUCHAR)pvOutputBuffer,pstPhsRule,pOutHeaderSize);
+					(PUCHAR)pvOutputBuffer, pstPhsRule, pOutHeaderSize);
 
 	pstPhsRule->PHSModifiedBytes += *pOutHeaderSize - *pInHeaderSize - 1;
 
@@ -831,112 +731,94 @@ ULONG PhsDeCompress(IN void* pvContext,
 	return STATUS_PHS_COMPRESSED;
 }
 
-
-//-----------------------------------------------------------------------------
-// Procedure:   free_phs_serviceflow_rules
-//
-// Description: This routine is responsible for freeing memory allocated for PHS rules.
-//
-// Arguments:
-// rules	- ptr to S_SERVICEFLOW_TABLE structure.
-//
-// Returns:
-// Does not return any value.
-//-----------------------------------------------------------------------------
-
+/*
+ * Procedure:   free_phs_serviceflow_rules
+ *
+ * Description: This routine is responsible for freeing memory allocated for PHS rules.
+ *
+ * Arguments:
+ * rules	- ptr to S_SERVICEFLOW_TABLE structure.
+ *
+ * Returns:
+ * Does not return any value.
+ */
 static void free_phs_serviceflow_rules(struct bcm_phs_table *psServiceFlowRulesTable)
 {
-	int i,j;
+	int i, j;
 	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "=======>\n");
-    if(psServiceFlowRulesTable)
-	{
-		for(i=0;i<MAX_SERVICEFLOWS;i++)
-		{
-			struct bcm_phs_entry stServiceFlowEntry =
-                psServiceFlowRulesTable->stSFList[i];
-			struct bcm_phs_classifier_table *pstClassifierRulesTable =
-                stServiceFlowEntry.pstClassifierTable;
-
-			if(pstClassifierRulesTable)
-			{
-				for(j=0;j<MAX_PHSRULE_PER_SF;j++)
-				{
-					if(pstClassifierRulesTable->stActivePhsRulesList[j].pstPhsRule)
-					{
-						if(pstClassifierRulesTable->stActivePhsRulesList[j].pstPhsRule
-                                                                                        ->u8RefCnt)
-							pstClassifierRulesTable->stActivePhsRulesList[j].pstPhsRule
-  							                                                ->u8RefCnt--;
-						if(0==pstClassifierRulesTable->stActivePhsRulesList[j].pstPhsRule
-                                                                ->u8RefCnt)
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "=======>\n");
+
+	if (psServiceFlowRulesTable) {
+		for (i = 0; i < MAX_SERVICEFLOWS; i++) {
+			struct bcm_phs_entry stServiceFlowEntry = psServiceFlowRulesTable->stSFList[i];
+			struct bcm_phs_classifier_table *pstClassifierRulesTable = stServiceFlowEntry.pstClassifierTable;
+
+			if (pstClassifierRulesTable) {
+				for (j = 0; j < MAX_PHSRULE_PER_SF; j++) {
+					if (pstClassifierRulesTable->stActivePhsRulesList[j].pstPhsRule) {
+
+						if (pstClassifierRulesTable->stActivePhsRulesList[j].pstPhsRule->u8RefCnt)
+							pstClassifierRulesTable->stActivePhsRulesList[j].pstPhsRule->u8RefCnt--;
+
+						if (0 == pstClassifierRulesTable->stActivePhsRulesList[j].pstPhsRule->u8RefCnt)
 							kfree(pstClassifierRulesTable->stActivePhsRulesList[j].pstPhsRule);
+
 						pstClassifierRulesTable->stActivePhsRulesList[j].pstPhsRule = NULL;
 					}
-					if(pstClassifierRulesTable->stOldPhsRulesList[j].pstPhsRule)
-					{
-						if(pstClassifierRulesTable->stOldPhsRulesList[j].pstPhsRule
-                                                                ->u8RefCnt)
-							pstClassifierRulesTable->stOldPhsRulesList[j].pstPhsRule
-							                                          ->u8RefCnt--;
-						if(0==pstClassifierRulesTable->stOldPhsRulesList[j].pstPhsRule
-                                                                      ->u8RefCnt)
+
+					if (pstClassifierRulesTable->stOldPhsRulesList[j].pstPhsRule) {
+
+						if (pstClassifierRulesTable->stOldPhsRulesList[j].pstPhsRule->u8RefCnt)
+							pstClassifierRulesTable->stOldPhsRulesList[j].pstPhsRule->u8RefCnt--;
+
+						if (0 == pstClassifierRulesTable->stOldPhsRulesList[j].pstPhsRule->u8RefCnt)
 							kfree(pstClassifierRulesTable->stOldPhsRulesList[j].pstPhsRule);
+
 						pstClassifierRulesTable->stOldPhsRulesList[j].pstPhsRule = NULL;
 					}
 				}
 				kfree(pstClassifierRulesTable);
-			    stServiceFlowEntry.pstClassifierTable = pstClassifierRulesTable = NULL;
+				stServiceFlowEntry.pstClassifierTable = pstClassifierRulesTable = NULL;
 			}
 		}
 	}
 
-    kfree(psServiceFlowRulesTable);
-    psServiceFlowRulesTable = NULL;
+	kfree(psServiceFlowRulesTable);
+	psServiceFlowRulesTable = NULL;
 }
 
-
-
 static BOOLEAN ValidatePHSRuleComplete(IN struct bcm_phs_rule *psPhsRule)
 {
-	if(psPhsRule)
-	{
-		if(!psPhsRule->u8PHSI)
-		{
-			// PHSI is not valid
+	if (psPhsRule) {
+		if (!psPhsRule->u8PHSI) {
+			/* PHSI is not valid */
 			return FALSE;
 		}
 
-		if(!psPhsRule->u8PHSS)
-		{
-			//PHSS Is Undefined
+		if (!psPhsRule->u8PHSS) {
+			/* PHSS Is Undefined */
 			return FALSE;
 		}
 
-		//Check if PHSF is defines for the PHS Rule
-		if(!psPhsRule->u8PHSFLength) // If any part of PHSF is valid then Rule contains valid PHSF
-		{
+		/* Check if PHSF is defines for the PHS Rule */
+		if (!psPhsRule->u8PHSFLength) /* If any part of PHSF is valid then Rule contains valid PHSF */
 			return FALSE;
-		}
+
 		return TRUE;
-	}
-	else
-	{
+	} else
 		return FALSE;
-	}
 }
 
 UINT GetServiceFlowEntry(IN struct bcm_phs_table *psServiceFlowTable,
-    IN B_UINT16 uiVcid, struct bcm_phs_entry **ppstServiceFlowEntry)
+			IN B_UINT16 uiVcid,
+			struct bcm_phs_entry **ppstServiceFlowEntry)
 {
-	int  i;
-	for(i=0;i<MAX_SERVICEFLOWS;i++)
-	{
-		if(psServiceFlowTable->stSFList[i].bUsed)
-		{
-			if(psServiceFlowTable->stSFList[i].uiVcid == uiVcid)
-			{
+	int i;
+
+	for (i = 0; i < MAX_SERVICEFLOWS; i++) {
+		if (psServiceFlowTable->stSFList[i].bUsed) {
+			if (psServiceFlowTable->stSFList[i].uiVcid == uiVcid) {
 				*ppstServiceFlowEntry = &psServiceFlowTable->stSFList[i];
 				return i;
 			}
@@ -947,34 +829,26 @@ UINT GetServiceFlowEntry(IN struct bcm_phs_table *psServiceFlowTable,
 	return PHS_INVALID_TABLE_INDEX;
 }
 
-
 UINT GetClassifierEntry(IN struct bcm_phs_classifier_table *pstClassifierTable,
-        IN B_UINT32 uiClsid, enum bcm_phs_classifier_context eClsContext,
-        OUT struct bcm_phs_classifier_entry **ppstClassifierEntry)
+			IN B_UINT32 uiClsid, enum bcm_phs_classifier_context eClsContext,
+			OUT struct bcm_phs_classifier_entry **ppstClassifierEntry)
 {
 	int  i;
 	struct bcm_phs_classifier_entry *psClassifierRules = NULL;
-	for(i=0;i<MAX_PHSRULE_PER_SF;i++)
-	{
 
-		if(eClsContext == eActiveClassifierRuleContext)
-		{
+	for (i = 0; i < MAX_PHSRULE_PER_SF; i++) {
+
+		if (eClsContext == eActiveClassifierRuleContext)
 			psClassifierRules = &pstClassifierTable->stActivePhsRulesList[i];
-		}
 		else
-		{
 			psClassifierRules = &pstClassifierTable->stOldPhsRulesList[i];
-		}
 
-		if(psClassifierRules->bUsed)
-		{
-			if(psClassifierRules->uiClassifierRuleId == uiClsid)
-			{
+		if (psClassifierRules->bUsed) {
+			if (psClassifierRules->uiClassifierRuleId == uiClsid) {
 				*ppstClassifierEntry = psClassifierRules;
 				return i;
 			}
 		}
-
 	}
 
 	*ppstClassifierEntry = NULL;
@@ -982,254 +856,227 @@ UINT GetClassifierEntry(IN struct bcm_phs_classifier_table *pstClassifierTable,
 }
 
 static UINT GetPhsRuleEntry(IN struct bcm_phs_classifier_table *pstClassifierTable,
-			    IN B_UINT32 uiPHSI, enum bcm_phs_classifier_context eClsContext,
-			    OUT struct bcm_phs_rule **ppstPhsRule)
+			IN B_UINT32 uiPHSI, enum bcm_phs_classifier_context eClsContext,
+			OUT struct bcm_phs_rule **ppstPhsRule)
 {
 	int  i;
 	struct bcm_phs_classifier_entry *pstClassifierRule = NULL;
-	for(i=0;i<MAX_PHSRULE_PER_SF;i++)
-	{
-		if(eClsContext == eActiveClassifierRuleContext)
-		{
+
+	for (i = 0; i < MAX_PHSRULE_PER_SF; i++) {
+		if (eClsContext == eActiveClassifierRuleContext)
 			pstClassifierRule = &pstClassifierTable->stActivePhsRulesList[i];
-		}
 		else
-		{
 			pstClassifierRule = &pstClassifierTable->stOldPhsRulesList[i];
-		}
-		if(pstClassifierRule->bUsed)
-		{
-			if(pstClassifierRule->u8PHSI == uiPHSI)
-			{
+
+		if (pstClassifierRule->bUsed) {
+			if (pstClassifierRule->u8PHSI == uiPHSI) {
 				*ppstPhsRule = pstClassifierRule->pstPhsRule;
 				return i;
 			}
 		}
-
 	}
 
 	*ppstPhsRule = NULL;
 	return PHS_INVALID_TABLE_INDEX;
 }
 
-UINT CreateSFToClassifierRuleMapping(IN B_UINT16 uiVcid,IN B_UINT16  uiClsId,
-                      IN struct bcm_phs_table *psServiceFlowTable, struct bcm_phs_rule *psPhsRule,
-                      B_UINT8 u8AssociatedPHSI)
+UINT CreateSFToClassifierRuleMapping(IN B_UINT16 uiVcid, IN B_UINT16  uiClsId,
+				IN struct bcm_phs_table *psServiceFlowTable,
+				struct bcm_phs_rule *psPhsRule,
+				B_UINT8 u8AssociatedPHSI)
 {
-
 	struct bcm_phs_classifier_table *psaClassifiertable = NULL;
 	UINT uiStatus = 0;
 	int iSfIndex;
-	BOOLEAN bFreeEntryFound =FALSE;
-	//Check for a free entry in SFID table
-	for(iSfIndex=0;iSfIndex < MAX_SERVICEFLOWS;iSfIndex++)
-	{
-		if(!psServiceFlowTable->stSFList[iSfIndex].bUsed)
-		{
+	BOOLEAN bFreeEntryFound = FALSE;
+
+	/* Check for a free entry in SFID table */
+	for (iSfIndex = 0; iSfIndex < MAX_SERVICEFLOWS; iSfIndex++) {
+		if (!psServiceFlowTable->stSFList[iSfIndex].bUsed) {
 			bFreeEntryFound = TRUE;
 			break;
 		}
 	}
 
-	if(!bFreeEntryFound)
+	if (!bFreeEntryFound)
 		return ERR_SFTABLE_FULL;
 
-
 	psaClassifiertable = psServiceFlowTable->stSFList[iSfIndex].pstClassifierTable;
-	uiStatus = CreateClassifierPHSRule(uiClsId,psaClassifiertable,psPhsRule,
-	                      eActiveClassifierRuleContext,u8AssociatedPHSI);
-	if(uiStatus == PHS_SUCCESS)
-	{
-		//Add entry at free index to the SF
+	uiStatus = CreateClassifierPHSRule(uiClsId, psaClassifiertable, psPhsRule,
+					eActiveClassifierRuleContext, u8AssociatedPHSI);
+	if (uiStatus == PHS_SUCCESS) {
+		/* Add entry at free index to the SF */
 		psServiceFlowTable->stSFList[iSfIndex].bUsed = TRUE;
 		psServiceFlowTable->stSFList[iSfIndex].uiVcid = uiVcid;
 	}
 
 	return uiStatus;
-
 }
 
 UINT CreateClassiferToPHSRuleMapping(IN B_UINT16 uiVcid,
-            IN B_UINT16  uiClsId,IN struct bcm_phs_entry *pstServiceFlowEntry,
-              struct bcm_phs_rule *psPhsRule, B_UINT8 u8AssociatedPHSI)
+				IN B_UINT16 uiClsId,
+				IN struct bcm_phs_entry *pstServiceFlowEntry,
+				struct bcm_phs_rule *psPhsRule,
+				B_UINT8 u8AssociatedPHSI)
 {
 	struct bcm_phs_classifier_entry *pstClassifierEntry = NULL;
-	UINT uiStatus =PHS_SUCCESS;
+	UINT uiStatus = PHS_SUCCESS;
 	UINT nClassifierIndex = 0;
 	struct bcm_phs_classifier_table *psaClassifiertable = NULL;
 	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-    psaClassifiertable = pstServiceFlowEntry->pstClassifierTable;
 
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "==>");
+	psaClassifiertable = pstServiceFlowEntry->pstClassifierTable;
+
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "==>");
 
 	/* Check if the supplied Classifier already exists */
-	nClassifierIndex =GetClassifierEntry(
-	            pstServiceFlowEntry->pstClassifierTable,uiClsId,
-	            eActiveClassifierRuleContext,&pstClassifierEntry);
-	if(nClassifierIndex == PHS_INVALID_TABLE_INDEX)
-	{
+	nClassifierIndex = GetClassifierEntry(
+		pstServiceFlowEntry->pstClassifierTable,
+		uiClsId,
+		eActiveClassifierRuleContext,
+		&pstClassifierEntry);
+
+	if (nClassifierIndex == PHS_INVALID_TABLE_INDEX) {
 		/*
-		    The Classifier doesn't exist. So its a new classifier being added.
-		     Add new entry to associate PHS Rule to the Classifier
-		*/
+		 * The Classifier doesn't exist. So its a new classifier being added.
+		 * Add new entry to associate PHS Rule to the Classifier
+		 */
 
-		uiStatus = CreateClassifierPHSRule(uiClsId,psaClassifiertable,
-		    psPhsRule,eActiveClassifierRuleContext,u8AssociatedPHSI);
+		uiStatus = CreateClassifierPHSRule(uiClsId, psaClassifiertable,
+						psPhsRule,
+						eActiveClassifierRuleContext,
+						u8AssociatedPHSI);
 		return uiStatus;
 	}
 
 	/*
-	  The Classifier exists.The PHS Rule for this classifier
-	  is being modified
-	  */
-	if(pstClassifierEntry->u8PHSI == psPhsRule->u8PHSI)
-	{
-		if(pstClassifierEntry->pstPhsRule == NULL)
+	 * The Classifier exists.The PHS Rule for this classifier
+	 * is being modified
+	 */
+
+	if (pstClassifierEntry->u8PHSI == psPhsRule->u8PHSI) {
+		if (pstClassifierEntry->pstPhsRule == NULL)
 			return ERR_PHS_INVALID_PHS_RULE;
 
 		/*
-		    This rule already exists if any fields are changed for this PHS
-		    rule update them.
+		 * This rule already exists if any fields are changed for this PHS
+		 * rule update them.
 		 */
-		 /* If any part of PHSF is valid then we update PHSF */
-		if(psPhsRule->u8PHSFLength)
-		{
-			//update PHSF
+		/* If any part of PHSF is valid then we update PHSF */
+		if (psPhsRule->u8PHSFLength) {
+			/* update PHSF */
 			memcpy(pstClassifierEntry->pstPhsRule->u8PHSF,
-			    psPhsRule->u8PHSF , MAX_PHS_LENGTHS);
+				psPhsRule->u8PHSF, MAX_PHS_LENGTHS);
 		}
-		if(psPhsRule->u8PHSFLength)
-		{
-			//update PHSFLen
-			pstClassifierEntry->pstPhsRule->u8PHSFLength =
-			    psPhsRule->u8PHSFLength;
+
+		if (psPhsRule->u8PHSFLength) {
+			/* update PHSFLen */
+			pstClassifierEntry->pstPhsRule->u8PHSFLength = psPhsRule->u8PHSFLength;
 		}
-		if(psPhsRule->u8PHSMLength)
-		{
-			//update PHSM
+
+		if (psPhsRule->u8PHSMLength) {
+			/* update PHSM */
 			memcpy(pstClassifierEntry->pstPhsRule->u8PHSM,
-			    psPhsRule->u8PHSM, MAX_PHS_LENGTHS);
+				psPhsRule->u8PHSM, MAX_PHS_LENGTHS);
 		}
-		if(psPhsRule->u8PHSMLength)
-		{
-			//update PHSM Len
+
+		if (psPhsRule->u8PHSMLength) {
+			/* update PHSM Len */
 			pstClassifierEntry->pstPhsRule->u8PHSMLength =
-			    psPhsRule->u8PHSMLength;
+				psPhsRule->u8PHSMLength;
 		}
-		if(psPhsRule->u8PHSS)
-		{
-			//update PHSS
+
+		if (psPhsRule->u8PHSS) {
+			/* update PHSS */
 			pstClassifierEntry->pstPhsRule->u8PHSS = psPhsRule->u8PHSS;
 		}
 
-		//update PHSV
+		/* update PHSV */
 		pstClassifierEntry->pstPhsRule->u8PHSV = psPhsRule->u8PHSV;
-
+	} else {
+		/* A new rule is being set for this classifier. */
+		uiStatus = UpdateClassifierPHSRule(uiClsId, pstClassifierEntry,
+						psaClassifiertable, psPhsRule, u8AssociatedPHSI);
 	}
-	else
-	{
-		/*
-		  A new rule is being set for this classifier.
-		*/
-		uiStatus=UpdateClassifierPHSRule( uiClsId,  pstClassifierEntry,
-		      psaClassifiertable,  psPhsRule, u8AssociatedPHSI);
-	}
-
-
 
 	return uiStatus;
 }
 
 static UINT CreateClassifierPHSRule(IN B_UINT16  uiClsId,
-    struct bcm_phs_classifier_table *psaClassifiertable, struct bcm_phs_rule *psPhsRule,
-    enum bcm_phs_classifier_context eClsContext,B_UINT8 u8AssociatedPHSI)
+				struct bcm_phs_classifier_table *psaClassifiertable,
+				struct bcm_phs_rule *psPhsRule,
+				enum bcm_phs_classifier_context eClsContext,
+				B_UINT8 u8AssociatedPHSI)
 {
 	UINT iClassifierIndex = 0;
 	BOOLEAN bFreeEntryFound = FALSE;
 	struct bcm_phs_classifier_entry *psClassifierRules = NULL;
 	UINT nStatus = PHS_SUCCESS;
 	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL,"Inside CreateClassifierPHSRule");
-    if(psaClassifiertable == NULL)
-	{
+
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "Inside CreateClassifierPHSRule");
+
+	if (psaClassifiertable == NULL)
 		return ERR_INVALID_CLASSIFIERTABLE_FOR_SF;
-	}
 
-	if(eClsContext == eOldClassifierRuleContext)
-	{
-		/* If An Old Entry for this classifier ID already exists in the
-		    old rules table replace it. */
+	if (eClsContext == eOldClassifierRuleContext) {
+		/*
+		 * If An Old Entry for this classifier ID already exists in the
+		 * old rules table replace it.
+		 */
 
 		iClassifierIndex =
-		GetClassifierEntry(psaClassifiertable, uiClsId,
-		            eClsContext,&psClassifierRules);
-		if(iClassifierIndex != PHS_INVALID_TABLE_INDEX)
-		{
+			GetClassifierEntry(psaClassifiertable, uiClsId,
+					eClsContext, &psClassifierRules);
+
+		if (iClassifierIndex != PHS_INVALID_TABLE_INDEX) {
 			/*
-			    The Classifier already exists in the old rules table
-		        Lets replace the old classifier with the new one.
-			*/
+			 * The Classifier already exists in the old rules table
+			 * Lets replace the old classifier with the new one.
+			 */
 			bFreeEntryFound = TRUE;
 		}
 	}
 
-	if(!bFreeEntryFound)
-	{
-		/*
-		  Continue to search for a free location to add the rule
-		*/
-		for(iClassifierIndex = 0; iClassifierIndex <
-            MAX_PHSRULE_PER_SF; iClassifierIndex++)
-		{
-			if(eClsContext == eActiveClassifierRuleContext)
-			{
-				psClassifierRules =
-              &psaClassifiertable->stActivePhsRulesList[iClassifierIndex];
-			}
+	if (!bFreeEntryFound) {
+		/* Continue to search for a free location to add the rule */
+		for (iClassifierIndex = 0; iClassifierIndex <
+			     MAX_PHSRULE_PER_SF; iClassifierIndex++) {
+			if (eClsContext == eActiveClassifierRuleContext)
+				psClassifierRules = &psaClassifiertable->stActivePhsRulesList[iClassifierIndex];
 			else
-			{
-				psClassifierRules =
-                &psaClassifiertable->stOldPhsRulesList[iClassifierIndex];
-			}
+				psClassifierRules = &psaClassifiertable->stOldPhsRulesList[iClassifierIndex];
 
-			if(!psClassifierRules->bUsed)
-			{
+			if (!psClassifierRules->bUsed) {
 				bFreeEntryFound = TRUE;
 				break;
 			}
 		}
 	}
 
-	if(!bFreeEntryFound)
-	{
-		if(eClsContext == eActiveClassifierRuleContext)
-		{
+	if (!bFreeEntryFound) {
+
+		if (eClsContext == eActiveClassifierRuleContext)
 			return ERR_CLSASSIFIER_TABLE_FULL;
-		}
-		else
-		{
-			//Lets replace the oldest rule if we are looking in old Rule table
-			if(psaClassifiertable->uiOldestPhsRuleIndex >=
-                MAX_PHSRULE_PER_SF)
-			{
-				psaClassifiertable->uiOldestPhsRuleIndex =0;
-			}
+		else {
+			/* Lets replace the oldest rule if we are looking in old Rule table */
+			if (psaClassifiertable->uiOldestPhsRuleIndex >= MAX_PHSRULE_PER_SF)
+				psaClassifiertable->uiOldestPhsRuleIndex = 0;
 
 			iClassifierIndex = psaClassifiertable->uiOldestPhsRuleIndex;
-			psClassifierRules =
-              &psaClassifiertable->stOldPhsRulesList[iClassifierIndex];
+			psClassifierRules = &psaClassifiertable->stOldPhsRulesList[iClassifierIndex];
 
-          (psaClassifiertable->uiOldestPhsRuleIndex)++;
+			(psaClassifiertable->uiOldestPhsRuleIndex)++;
 		}
 	}
 
-	if(eClsContext == eOldClassifierRuleContext)
-	{
-		if(psClassifierRules->pstPhsRule == NULL)
-		{
-			psClassifierRules->pstPhsRule = kmalloc(sizeof(struct bcm_phs_rule),GFP_KERNEL);
+	if (eClsContext == eOldClassifierRuleContext) {
+
+		if (psClassifierRules->pstPhsRule == NULL) {
 
-          if(NULL == psClassifierRules->pstPhsRule)
+			psClassifierRules->pstPhsRule = kmalloc(sizeof(struct bcm_phs_rule), GFP_KERNEL);
+
+			if (NULL == psClassifierRules->pstPhsRule)
 				return ERR_PHSRULE_MEMALLOC_FAIL;
 		}
 
@@ -1238,70 +1085,62 @@ static UINT CreateClassifierPHSRule(IN B_UINT16  uiClsId,
 		psClassifierRules->u8PHSI = psPhsRule->u8PHSI;
 		psClassifierRules->bUnclassifiedPHSRule = psPhsRule->bUnclassifiedPHSRule;
 
-        /* Update The PHS rule */
-		memcpy(psClassifierRules->pstPhsRule,
-		    psPhsRule, sizeof(struct bcm_phs_rule));
-	}
-	else
-	{
-		nStatus = UpdateClassifierPHSRule(uiClsId,psClassifierRules,
-            psaClassifiertable,psPhsRule,u8AssociatedPHSI);
-	}
+		/* Update The PHS rule */
+		memcpy(psClassifierRules->pstPhsRule, psPhsRule, sizeof(struct bcm_phs_rule));
+	} else
+		nStatus = UpdateClassifierPHSRule(uiClsId, psClassifierRules,
+						psaClassifiertable, psPhsRule, u8AssociatedPHSI);
+
 	return nStatus;
 }
 
-
 static UINT UpdateClassifierPHSRule(IN B_UINT16  uiClsId,
-      IN struct bcm_phs_classifier_entry *pstClassifierEntry,
-      struct bcm_phs_classifier_table *psaClassifiertable, struct bcm_phs_rule *psPhsRule,
-      B_UINT8 u8AssociatedPHSI)
+				IN struct bcm_phs_classifier_entry *pstClassifierEntry,
+				struct bcm_phs_classifier_table *psaClassifiertable,
+				struct bcm_phs_rule *psPhsRule,
+				B_UINT8 u8AssociatedPHSI)
 {
 	struct bcm_phs_rule *pstAddPhsRule = NULL;
-	UINT              nPhsRuleIndex = 0;
-	BOOLEAN       bPHSRuleOrphaned = FALSE;
+	UINT nPhsRuleIndex = 0;
+	BOOLEAN bPHSRuleOrphaned = FALSE;
 	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-	psPhsRule->u8RefCnt =0;
-
-	/* Step 1 Deref Any Exisiting PHS Rule in this classifier Entry*/
-	bPHSRuleOrphaned = DerefPhsRule( uiClsId, psaClassifiertable,
-	    pstClassifierEntry->pstPhsRule);
-
-	//Step 2 Search if there is a PHS Rule with u8AssociatedPHSI in Classifier table for this SF
-	nPhsRuleIndex =GetPhsRuleEntry(psaClassifiertable,u8AssociatedPHSI,
-	    eActiveClassifierRuleContext, &pstAddPhsRule);
-	if(PHS_INVALID_TABLE_INDEX == nPhsRuleIndex)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "\nAdding New PHSRuleEntry For Classifier");
-
-		if(psPhsRule->u8PHSI == 0)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "\nError PHSI is Zero\n");
+
+	psPhsRule->u8RefCnt = 0;
+
+	/* Step 1 Deref Any Exisiting PHS Rule in this classifier Entry */
+	bPHSRuleOrphaned = DerefPhsRule(uiClsId, psaClassifiertable,
+					pstClassifierEntry->pstPhsRule);
+
+	/* Step 2 Search if there is a PHS Rule with u8AssociatedPHSI in Classifier table for this SF */
+	nPhsRuleIndex = GetPhsRuleEntry(psaClassifiertable, u8AssociatedPHSI,
+					eActiveClassifierRuleContext, &pstAddPhsRule);
+	if (PHS_INVALID_TABLE_INDEX == nPhsRuleIndex) {
+
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "\nAdding New PHSRuleEntry For Classifier");
+
+		if (psPhsRule->u8PHSI == 0) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "\nError PHSI is Zero\n");
 			return ERR_PHS_INVALID_PHS_RULE;
 		}
-		//Step 2.a PHS Rule Does Not Exist .Create New PHS Rule for uiClsId
-		if(FALSE == bPHSRuleOrphaned)
-		{
+
+		/* Step 2.a PHS Rule Does Not Exist .Create New PHS Rule for uiClsId */
+		if (FALSE == bPHSRuleOrphaned) {
+
 			pstClassifierEntry->pstPhsRule = kmalloc(sizeof(struct bcm_phs_rule), GFP_KERNEL);
-			if(NULL == pstClassifierEntry->pstPhsRule)
-			{
+			if (NULL == pstClassifierEntry->pstPhsRule)
 				return ERR_PHSRULE_MEMALLOC_FAIL;
-			}
 		}
 		memcpy(pstClassifierEntry->pstPhsRule, psPhsRule, sizeof(struct bcm_phs_rule));
-
-	}
-	else
-	{
-		//Step 2.b PHS Rule  Exists Tie uiClsId with the existing PHS Rule
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "\nTying Classifier to Existing PHS Rule");
-		if(bPHSRuleOrphaned)
-		{
+	} else {
+		/* Step 2.b PHS Rule  Exists Tie uiClsId with the existing PHS Rule */
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "\nTying Classifier to Existing PHS Rule");
+		if (bPHSRuleOrphaned) {
 			kfree(pstClassifierEntry->pstPhsRule);
 			pstClassifierEntry->pstPhsRule = NULL;
 		}
 		pstClassifierEntry->pstPhsRule = pstAddPhsRule;
-
 	}
+
 	pstClassifierEntry->bUsed = TRUE;
 	pstClassifierEntry->u8PHSI = pstClassifierEntry->pstPhsRule->u8PHSI;
 	pstClassifierEntry->uiClassifierRuleId = uiClsId;
@@ -1309,79 +1148,74 @@ static UINT UpdateClassifierPHSRule(IN B_UINT16  uiClsId,
 	pstClassifierEntry->bUnclassifiedPHSRule = pstClassifierEntry->pstPhsRule->bUnclassifiedPHSRule;
 
 	return PHS_SUCCESS;
-
 }
 
 static BOOLEAN DerefPhsRule(IN B_UINT16  uiClsId, struct bcm_phs_classifier_table *psaClassifiertable, struct bcm_phs_rule *pstPhsRule)
 {
-	if(pstPhsRule==NULL)
+	if (pstPhsRule == NULL)
 		return FALSE;
-	if(pstPhsRule->u8RefCnt)
+
+	if (pstPhsRule->u8RefCnt)
 		pstPhsRule->u8RefCnt--;
-	if(0==pstPhsRule->u8RefCnt)
-	{
-		/*if(pstPhsRule->u8PHSI)
-		//Store the currently active rule into the old rules list
-		CreateClassifierPHSRule(uiClsId,psaClassifiertable,pstPhsRule,eOldClassifierRuleContext,pstPhsRule->u8PHSI);*/
+
+	if (0 == pstPhsRule->u8RefCnt) {
+		/*
+		 * if(pstPhsRule->u8PHSI)
+		 * Store the currently active rule into the old rules list
+		 * CreateClassifierPHSRule(uiClsId,psaClassifiertable,pstPhsRule,eOldClassifierRuleContext,pstPhsRule->u8PHSI);
+		 */
 		return TRUE;
-	}
-	else
-	{
+	} else
 		return FALSE;
-	}
 }
 
 void DumpPhsRules(struct bcm_phs_extension *pDeviceExtension)
 {
-	int i,j,k,l;
+	int i, j, k, l;
 	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-    BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\n Dumping PHS Rules : \n");
-	for(i=0;i<MAX_SERVICEFLOWS;i++)
-	{
+
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\n Dumping PHS Rules :\n");
+
+	for (i = 0; i < MAX_SERVICEFLOWS; i++) {
+
 		struct bcm_phs_entry stServFlowEntry =
-				pDeviceExtension->pstServiceFlowPhsRulesTable->stSFList[i];
-		if(stServFlowEntry.bUsed)
-		{
-			for(j=0;j<MAX_PHSRULE_PER_SF;j++)
-			{
-				for(l=0;l<2;l++)
-				{
+			pDeviceExtension->pstServiceFlowPhsRulesTable->stSFList[i];
+		if (stServFlowEntry.bUsed) {
+
+			for (j = 0; j < MAX_PHSRULE_PER_SF; j++) {
+
+				for (l = 0; l < 2; l++) {
 					struct bcm_phs_classifier_entry stClsEntry;
-					if(l==0)
-					{
+
+					if (l == 0) {
 						stClsEntry = stServFlowEntry.pstClassifierTable->stActivePhsRulesList[j];
-						if(stClsEntry.bUsed)
-							BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n Active PHS Rule : \n");
-					}
-					else
-					{
+						if (stClsEntry.bUsed)
+							BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n Active PHS Rule :\n");
+					} else {
 						stClsEntry = stServFlowEntry.pstClassifierTable->stOldPhsRulesList[j];
-						if(stClsEntry.bUsed)
-							BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n Old PHS Rule : \n");
+						if (stClsEntry.bUsed)
+							BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n Old PHS Rule :\n");
 					}
-					if(stClsEntry.bUsed)
-					{
-
-						BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\n VCID  : %#X",stServFlowEntry.uiVcid);
-						BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n ClassifierID  : %#X",stClsEntry.uiClassifierRuleId);
-						BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n PHSRuleID  : %#X",stClsEntry.u8PHSI);
-						BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n****************PHS Rule********************\n");
-						BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n PHSI  : %#X",stClsEntry.pstPhsRule->u8PHSI);
-						BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n PHSFLength : %#X ",stClsEntry.pstPhsRule->u8PHSFLength);
-						BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n PHSF : ");
-						for(k=0;k<stClsEntry.pstPhsRule->u8PHSFLength;k++)
-						{
-							BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "%#X  ",stClsEntry.pstPhsRule->u8PHSF[k]);
-						}
-						BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n PHSMLength  : %#X",stClsEntry.pstPhsRule->u8PHSMLength);
-						BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n PHSM :");
-						for(k=0;k<stClsEntry.pstPhsRule->u8PHSMLength;k++)
-						{
-							BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "%#X  ",stClsEntry.pstPhsRule->u8PHSM[k]);
-						}
-						BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n PHSS : %#X ",stClsEntry.pstPhsRule->u8PHSS);
-						BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n PHSV  : %#X",stClsEntry.pstPhsRule->u8PHSV);
-						BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\n********************************************\n");
+
+					if (stClsEntry.bUsed) {
+						BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\n VCID  : %#X", stServFlowEntry.uiVcid);
+						BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n ClassifierID  : %#X", stClsEntry.uiClassifierRuleId);
+						BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n PHSRuleID  : %#X", stClsEntry.u8PHSI);
+						BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n****************PHS Rule********************\n");
+						BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n PHSI  : %#X", stClsEntry.pstPhsRule->u8PHSI);
+						BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n PHSFLength : %#X ", stClsEntry.pstPhsRule->u8PHSFLength);
+						BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n PHSF : ");
+
+						for (k = 0 ; k < stClsEntry.pstPhsRule->u8PHSFLength; k++)
+							BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "%#X  ", stClsEntry.pstPhsRule->u8PHSF[k]);
+						BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n PHSMLength  : %#X", stClsEntry.pstPhsRule->u8PHSMLength);
+						BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n PHSM :");
+
+						for (k = 0; k < stClsEntry.pstPhsRule->u8PHSMLength; k++)
+							BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "%#X  ", stClsEntry.pstPhsRule->u8PHSM[k]);
+						BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n PHSS : %#X ", stClsEntry.pstPhsRule->u8PHSS);
+						BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n PHSV  : %#X", stClsEntry.pstPhsRule->u8PHSV);
+						BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\n********************************************\n");
 					}
 				}
 			}
@@ -1389,66 +1223,69 @@ void DumpPhsRules(struct bcm_phs_extension *pDeviceExtension)
 	}
 }
 
-
-//-----------------------------------------------------------------------------
-// Procedure:   phs_decompress
-//
-// Description: This routine restores the static fields within the packet.
-//
-// Arguments:
-//	in_buf			- ptr to incoming packet buffer.
-//	out_buf			- ptr to output buffer where the suppressed header is copied.
-//	decomp_phs_rules - ptr to PHS rule.
-//	header_size		- ptr to field which holds the phss or phsf_length.
-//
-// Returns:
-//	size -The number of bytes of dynamic fields present with in the incoming packet
-//			header.
-//	0	-If PHS rule is NULL.If PHSI is 0 indicateing packet as uncompressed.
-//-----------------------------------------------------------------------------
-
-int phs_decompress(unsigned char *in_buf,unsigned char *out_buf,
-		struct bcm_phs_rule *decomp_phs_rules, UINT *header_size)
+/*
+ * Procedure:   phs_decompress
+ *
+ * Description: This routine restores the static fields within the packet.
+ *
+ * Arguments:
+ *	in_buf			- ptr to incoming packet buffer.
+ *	out_buf			- ptr to output buffer where the suppressed header is copied.
+ *	decomp_phs_rules - ptr to PHS rule.
+ *	header_size		- ptr to field which holds the phss or phsf_length.
+ *
+ * Returns:
+ *	size -The number of bytes of dynamic fields present with in the incoming packet
+ *			header.
+ *	0	-If PHS rule is NULL.If PHSI is 0 indicateing packet as uncompressed.
+ */
+int phs_decompress(unsigned char *in_buf,
+		unsigned char *out_buf,
+		struct bcm_phs_rule *decomp_phs_rules,
+		UINT *header_size)
 {
-	int phss,size=0;
+	int phss, size = 0;
 	struct bcm_phs_rule *tmp_memb;
-	int bit,i=0;
-	unsigned char *phsf,*phsm;
-	int in_buf_len = *header_size-1;
+	int bit, i = 0;
+	unsigned char *phsf, *phsm;
+	int in_buf_len = *header_size - 1;
 	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
+
 	in_buf++;
-    BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_RECEIVE,DBG_LVL_ALL,"====>\n");
+
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_RECEIVE, DBG_LVL_ALL, "====>\n");
 	*header_size = 0;
 
-	if((decomp_phs_rules == NULL ))
+	if ((decomp_phs_rules == NULL))
 		return 0;
 
-
 	tmp_memb = decomp_phs_rules;
-	//BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_RECEIVE,DBG_LVL_ALL,"\nDECOMP:In phs_decompress PHSI 1  %d",phsi));
-	//*header_size = tmp_memb->u8PHSFLength;
-	phss         = tmp_memb->u8PHSS;
-	phsf         = tmp_memb->u8PHSF;
-	phsm         = tmp_memb->u8PHSM;
-
-	if(phss > MAX_PHS_LENGTHS)
+	/*
+	 * BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_RECEIVE,DBG_LVL_ALL,"\nDECOMP:In phs_decompress PHSI 1  %d",phsi));
+	 * header_size = tmp_memb->u8PHSFLength;
+	 */
+	phss = tmp_memb->u8PHSS;
+	phsf = tmp_memb->u8PHSF;
+	phsm = tmp_memb->u8PHSM;
+
+	if (phss > MAX_PHS_LENGTHS)
 		phss = MAX_PHS_LENGTHS;
-	//BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_RECEIVE,DBG_LVL_ALL,"\nDECOMP:In phs_decompress PHSI  %d phss %d index %d",phsi,phss,index));
-	while((phss > 0) && (size < in_buf_len))
-	{
-		bit =  ((*phsm << i)& SUPPRESS);
 
-		if(bit == SUPPRESS)
-		{
+	/*
+	 * BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_RECEIVE,DBG_LVL_ALL,"\nDECOMP:
+	 * In phs_decompress PHSI  %d phss %d index %d",phsi,phss,index));
+	 */
+	while ((phss > 0) && (size < in_buf_len)) {
+		bit = ((*phsm << i) & SUPPRESS);
+
+		if (bit == SUPPRESS) {
 			*out_buf = *phsf;
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_RECEIVE,DBG_LVL_ALL,"\nDECOMP:In phss  %d phsf %d ouput %d",
-              phss,*phsf,*out_buf);
-		}
-		else
-		{
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_RECEIVE, DBG_LVL_ALL, "\nDECOMP:In phss  %d phsf %d ouput %d",
+					phss, *phsf, *out_buf);
+		} else {
 			*out_buf = *in_buf;
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_RECEIVE,DBG_LVL_ALL,"\nDECOMP:In phss  %d input %d ouput %d",
-            phss,*in_buf,*out_buf);
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_RECEIVE, DBG_LVL_ALL, "\nDECOMP:In phss  %d input %d ouput %d",
+					phss, *in_buf, *out_buf);
 			in_buf++;
 			size++;
 		}
@@ -1456,156 +1293,159 @@ int phs_decompress(unsigned char *in_buf,unsigned char *out_buf,
 		phsf++;
 		phss--;
 		i++;
-		*header_size=*header_size + 1;
+		*header_size = *header_size + 1;
 
-		if(i > MAX_NO_BIT)
-		{
-			i=0;
+		if (i > MAX_NO_BIT) {
+			i = 0;
 			phsm++;
 		}
 	}
+
 	return size;
 }
 
-
-
-
-//-----------------------------------------------------------------------------
-// Procedure:   phs_compress
-//
-// Description: This routine suppresses the static fields within the packet.Before
-// that it will verify the fields to be suppressed with the corresponding fields in the
-// phsf. For verification it checks the phsv field of PHS rule. If set and verification
-// succeeds it suppresses the field.If any one static field is found different none of
-// the static fields are suppressed then the packet is sent as uncompressed packet with
-// phsi=0.
-//
-// Arguments:
-//	phs_rule - ptr to PHS rule.
-//	in_buf		- ptr to incoming packet buffer.
-//	out_buf		- ptr to output buffer where the suppressed header is copied.
-//	header_size	- ptr to field which holds the phss.
-//
-// Returns:
-//	size-The number of bytes copied into the output buffer i.e dynamic fields
-//	0	-If PHS rule is NULL.If PHSV field is not set.If the verification fails.
-//-----------------------------------------------------------------------------
-static int phs_compress(struct bcm_phs_rule *phs_rule, unsigned char *in_buf
-			,unsigned char *out_buf,UINT *header_size,UINT *new_header_size)
+/*
+ * Procedure:   phs_compress
+ *
+ * Description: This routine suppresses the static fields within the packet.Before
+ * that it will verify the fields to be suppressed with the corresponding fields in the
+ * phsf. For verification it checks the phsv field of PHS rule. If set and verification
+ * succeeds it suppresses the field.If any one static field is found different none of
+ * the static fields are suppressed then the packet is sent as uncompressed packet with
+ * phsi=0.
+ *
+ * Arguments:
+ *	phs_rule - ptr to PHS rule.
+ *	in_buf		- ptr to incoming packet buffer.
+ *	out_buf		- ptr to output buffer where the suppressed header is copied.
+ *	header_size	- ptr to field which holds the phss.
+ *
+ * Returns:
+ *	size-The number of bytes copied into the output buffer i.e dynamic fields
+ *	0	-If PHS rule is NULL.If PHSV field is not set.If the verification fails.
+ */
+static int phs_compress(struct bcm_phs_rule *phs_rule,
+			unsigned char *in_buf,
+			unsigned char *out_buf,
+			UINT *header_size,
+			UINT *new_header_size)
 {
 	unsigned char *old_addr = out_buf;
 	int suppress = 0;
 	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-    if(phs_rule == NULL)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,"\nphs_compress(): phs_rule null!");
+
+	if (phs_rule == NULL) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL, "\nphs_compress(): phs_rule null!");
 		*out_buf = ZERO_PHSI;
 		return STATUS_PHS_NOCOMPRESSION;
 	}
 
-
-	if(phs_rule->u8PHSS <= *new_header_size)
-	{
+	if (phs_rule->u8PHSS <= *new_header_size)
 		*header_size = phs_rule->u8PHSS;
-	}
 	else
-	{
 		*header_size = *new_header_size;
-	}
-	//To copy PHSI
+
+	/* To copy PHSI */
 	out_buf++;
-	suppress = verify_suppress_phsf(in_buf,out_buf,phs_rule->u8PHSF,
-        phs_rule->u8PHSM, phs_rule->u8PHSS, phs_rule->u8PHSV,new_header_size);
+	suppress = verify_suppress_phsf(in_buf, out_buf, phs_rule->u8PHSF,
+					phs_rule->u8PHSM, phs_rule->u8PHSS,
+					phs_rule->u8PHSV, new_header_size);
 
-	if(suppress == STATUS_PHS_COMPRESSED)
-	{
+	if (suppress == STATUS_PHS_COMPRESSED) {
 		*old_addr = (unsigned char)phs_rule->u8PHSI;
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,"\nCOMP:In phs_compress phsi %d",phs_rule->u8PHSI);
-	}
-	else
-	{
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL, "\nCOMP:In phs_compress phsi %d", phs_rule->u8PHSI);
+	} else {
 		*old_addr = ZERO_PHSI;
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,"\nCOMP:In phs_compress PHSV Verification failed");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL, "\nCOMP:In phs_compress PHSV Verification failed");
 	}
+
 	return suppress;
 }
 
-
-//-----------------------------------------------------------------------------
-// Procedure:	verify_suppress_phsf
-//
-// Description: This routine verifies the fields of the packet and if all the
-// static fields are equal it adds the phsi of that PHS rule.If any static
-// field differs it woun't suppress any field.
-//
-// Arguments:
-// rules_set	- ptr to classifier_rules.
-// in_buffer	- ptr to incoming packet buffer.
-// out_buffer	- ptr to output buffer where the suppressed header is copied.
-// phsf			- ptr to phsf.
-// phsm			- ptr to phsm.
-// phss			- variable holding phss.
-//
-// Returns:
-//	size-The number of bytes copied into the output buffer i.e dynamic fields.
-//	0	-Packet has failed the verification.
-//-----------------------------------------------------------------------------
-
-static int verify_suppress_phsf(unsigned char *in_buffer,unsigned char *out_buffer,
-				unsigned char *phsf,unsigned char *phsm,unsigned int phss,
-				unsigned int phsv,UINT* new_header_size)
+/*
+ * Procedure:	verify_suppress_phsf
+ *
+ * Description: This routine verifies the fields of the packet and if all the
+ * static fields are equal it adds the phsi of that PHS rule.If any static
+ * field differs it woun't suppress any field.
+ *
+ * Arguments:
+ * rules_set	- ptr to classifier_rules.
+ * in_buffer	- ptr to incoming packet buffer.
+ * out_buffer	- ptr to output buffer where the suppressed header is copied.
+ * phsf			- ptr to phsf.
+ * phsm			- ptr to phsm.
+ * phss			- variable holding phss.
+ *
+ * Returns:
+ *	size-The number of bytes copied into the output buffer i.e dynamic fields.
+ *	0	-Packet has failed the verification.
+ */
+static int verify_suppress_phsf(unsigned char *in_buffer,
+				unsigned char *out_buffer,
+				unsigned char *phsf,
+				unsigned char *phsm,
+				unsigned int phss,
+				unsigned int phsv,
+				UINT *new_header_size)
 {
-	unsigned int size=0;
-	int bit,i=0;
+	unsigned int size = 0;
+	int bit, i = 0;
 	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-    BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,"\nCOMP:In verify_phsf PHSM - 0x%X",*phsm);
 
-
-	if(phss>(*new_header_size))
-	{
-		phss=*new_header_size;
-	}
-	while(phss > 0)
-	{
-		bit = ((*phsm << i)& SUPPRESS);
-		if(bit == SUPPRESS)
-		{
-
-			if(*in_buffer != *phsf)
-			{
-				if(phsv == VERIFY)
-				{
-					BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,"\nCOMP:In verify_phsf failed for field  %d buf  %d phsf %d",phss,*in_buffer,*phsf);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL, "\nCOMP:In verify_phsf PHSM - 0x%X", *phsm);
+
+	if (phss > (*new_header_size))
+		phss = *new_header_size;
+
+	while (phss > 0) {
+		bit = ((*phsm << i) & SUPPRESS);
+		if (bit == SUPPRESS) {
+			if (*in_buffer != *phsf) {
+				if (phsv == VERIFY) {
+					BCM_DEBUG_PRINT(Adapter,
+							DBG_TYPE_OTHERS,
+							PHS_SEND,
+							DBG_LVL_ALL,
+							"\nCOMP:In verify_phsf failed for field  %d buf  %d phsf %d",
+							phss,
+							*in_buffer,
+							*phsf);
 					return STATUS_PHS_NOCOMPRESSION;
 				}
-			}
-			else
-				BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,"\nCOMP:In verify_phsf success for field  %d buf  %d phsf %d",phss,*in_buffer,*phsf);
-		}
-		else
-		{
+			} else
+				BCM_DEBUG_PRINT(Adapter,
+						DBG_TYPE_OTHERS,
+						PHS_SEND,
+						DBG_LVL_ALL,
+						"\nCOMP:In verify_phsf success for field  %d buf  %d phsf %d",
+						phss,
+						*in_buffer,
+						*phsf);
+		} else {
 			*out_buffer = *in_buffer;
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,"\nCOMP:In copying_header input %d  out %d",*in_buffer,*out_buffer);
+			BCM_DEBUG_PRINT(Adapter,
+					DBG_TYPE_OTHERS,
+					PHS_SEND,
+					DBG_LVL_ALL,
+					"\nCOMP:In copying_header input %d  out %d",
+					*in_buffer,
+					*out_buffer);
 			out_buffer++;
 			size++;
 		}
+
 		in_buffer++;
 		phsf++;
 		phss--;
 		i++;
-		if(i > MAX_NO_BIT)
-		{
-			i=0;
+
+		if (i > MAX_NO_BIT) {
+			i = 0;
 			phsm++;
 		}
 	}
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,"\nCOMP:In verify_phsf success");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL, "\nCOMP:In verify_phsf success");
 	*new_header_size = size;
 	return STATUS_PHS_COMPRESSED;
 }
-
-
-
-
-
-
diff --git a/drivers/staging/bcm/nvm.c b/drivers/staging/bcm/nvm.c
index e6152f4df14..bea1330f7ea 100644
--- a/drivers/staging/bcm/nvm.c
+++ b/drivers/staging/bcm/nvm.c
@@ -2228,20 +2228,20 @@ int BcmAllocFlashCSStructure(struct bcm_mini_adapter *psAdapter)
 		BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_PRINTK, 0, 0, "Adapter structure point is NULL");
 		return -EINVAL;
 	}
-	psAdapter->psFlashCSInfo = (struct bcm_flash_cs_info *)kzalloc(sizeof(struct bcm_flash_cs_info), GFP_KERNEL);
+	psAdapter->psFlashCSInfo = kzalloc(sizeof(struct bcm_flash_cs_info), GFP_KERNEL);
 	if (psAdapter->psFlashCSInfo == NULL) {
 		BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_PRINTK, 0, 0, "Can't Allocate memory for Flash 1.x");
 		return -ENOMEM;
 	}
 
-	psAdapter->psFlash2xCSInfo = (struct bcm_flash2x_cs_info *)kzalloc(sizeof(struct bcm_flash2x_cs_info), GFP_KERNEL);
+	psAdapter->psFlash2xCSInfo = kzalloc(sizeof(struct bcm_flash2x_cs_info), GFP_KERNEL);
 	if (!psAdapter->psFlash2xCSInfo) {
 		BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_PRINTK, 0, 0, "Can't Allocate memory for Flash 2.x");
 		kfree(psAdapter->psFlashCSInfo);
 		return -ENOMEM;
 	}
 
-	psAdapter->psFlash2xVendorInfo = (struct bcm_flash2x_vendor_info *)kzalloc(sizeof(struct bcm_flash2x_vendor_info), GFP_KERNEL);
+	psAdapter->psFlash2xVendorInfo = kzalloc(sizeof(struct bcm_flash2x_vendor_info), GFP_KERNEL);
 	if (!psAdapter->psFlash2xVendorInfo) {
 		BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_PRINTK, 0, 0, "Can't Allocate Vendor Info Memory for Flash 2.x");
 		kfree(psAdapter->psFlashCSInfo);
@@ -4074,7 +4074,7 @@ int BcmCopySection(struct bcm_mini_adapter *Adapter,
 	else
 		BuffSize = numOfBytes;
 
-	pBuff = (PCHAR)kzalloc(BuffSize, GFP_KERNEL);
+	pBuff = kzalloc(BuffSize, GFP_KERNEL);
 	if (!pBuff) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Memory allocation failed.. ");
 		return -ENOMEM;
@@ -4154,7 +4154,7 @@ int SaveHeaderIfPresent(struct bcm_mini_adapter *Adapter, PUCHAR pBuff, unsigned
 	}
 	/* If Header is present overwrite passed buffer with this */
 	if (bHasHeader && (Adapter->bHeaderChangeAllowed == FALSE)) {
-		pTempBuff = (PUCHAR)kzalloc(HeaderSizeToProtect, GFP_KERNEL);
+		pTempBuff = kzalloc(HeaderSizeToProtect, GFP_KERNEL);
 		if (!pTempBuff) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Memory allocation failed");
 			return -ENOMEM;
@@ -4563,7 +4563,7 @@ static int CorruptDSDSig(struct bcm_mini_adapter *Adapter, enum bcm_flash2x_sect
 		}
 	}
 
-	pBuff = (PUCHAR)kzalloc(MAX_RW_SIZE, GFP_KERNEL);
+	pBuff = kzalloc(MAX_RW_SIZE, GFP_KERNEL);
 	if (!pBuff) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Can't allocate memorey");
 		return -ENOMEM;
@@ -4622,7 +4622,7 @@ static int CorruptISOSig(struct bcm_mini_adapter *Adapter, enum bcm_flash2x_sect
 		return SECTOR_IS_NOT_WRITABLE;
 	}
 
-	pBuff = (PUCHAR)kzalloc(MAX_RW_SIZE, GFP_KERNEL);
+	pBuff = kzalloc(MAX_RW_SIZE, GFP_KERNEL);
 	if (!pBuff) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Can't allocate memorey");
 		return -ENOMEM;
diff --git a/drivers/staging/ccg/Kconfig b/drivers/staging/ccg/Kconfig
deleted file mode 100644
index 7ed5bc6caad..00000000000
--- a/drivers/staging/ccg/Kconfig
+++ /dev/null
@@ -1,25 +0,0 @@
-if USB_GADGET
-
-config USB_G_CCG
-	tristate "Configurable Composite Gadget (STAGING)"
-	depends on STAGING && BLOCK && NET && !USB_ZERO && !USB_ZERO_HNPTEST && !USB_AUDIO && !GADGET_UAC1 && !USB_ETH && !USB_ETH_RNDIS && !USB_ETH_EEM && !USB_G_NCM && !USB_GADGETFS && !USB_FUNCTIONFS && !USB_FUNCTIONFS_ETH && !USB_FUNCTIONFS_RNDIS && !USB_FUNCTIONFS_GENERIC && !USB_FILE_STORAGE && !USB_FILE_STORAGE_TEST && !USB_MASS_STORAGE && !USB_G_SERIAL && !USB_MIDI_GADGET && !USB_G_PRINTER && !USB_CDC_COMPOSITE && !USB_G_NOKIA && !USB_G_ACM_MS && !USB_G_MULTI && !USB_G_MULTI_RNDIS && !USB_G_MULTI_CDC && !USB_G_HID && !USB_G_DBGP && !USB_G_WEBCAM && TTY
-	help
-	  The Configurable Composite Gadget supports multiple USB
-	  functions: acm, mass storage, rndis and FunctionFS.
-	  Each function can be configured and enabled/disabled
-	  dynamically from userspace through a sysfs interface.
-
-	  In order to compile this (either as a module or built-in),
-	  "USB Gadget Drivers" and anything under it must not be
-	  selected compiled-in in
-	  Device Drivers->USB Support->USB Gadget Support.
-	  However, you can say "M" there, if you do, the
-	  Configurable Composite Gadget can be compiled "M" only
-	  or not at all.
-
-	  BIG FAT NOTE: DON'T RELY ON THIS USERINTERFACE HERE! AS PART
-	  OF THE REWORK DONE HERE WILL BE A NEW USER INTERFACE WITHOUT ANY
-	  COMPATIBILITY TO THIS SYSFS INTERFACE HERE. BE AWARE OF THIS
-	  BEFORE SELECTING THIS.
-
-endif # USB_GADGET
diff --git a/drivers/staging/ccg/Makefile b/drivers/staging/ccg/Makefile
deleted file mode 100644
index 814fa9de5f5..00000000000
--- a/drivers/staging/ccg/Makefile
+++ /dev/null
@@ -1,2 +0,0 @@
-g_ccg-y				:= ccg.o
-obj-$(CONFIG_USB_G_CCG)		+= g_ccg.o
diff --git a/drivers/staging/ccg/TODO b/drivers/staging/ccg/TODO
deleted file mode 100644
index 18612fe7017..00000000000
--- a/drivers/staging/ccg/TODO
+++ /dev/null
@@ -1,6 +0,0 @@
-TODO:
-	- change configuration interface from sysfs to configfs
-
-Please send patches to Greg Kroah-Hartmann <gregkh@linuxfoundation.org>,
-Andrzej Pietrasiewicz <andrzej.p@samsung.com>, and
-Cc: Mike Lockwood <lockwood@android.com>
diff --git a/drivers/staging/ccg/ccg.c b/drivers/staging/ccg/ccg.c
deleted file mode 100644
index ffc5f73a5b5..00000000000
--- a/drivers/staging/ccg/ccg.c
+++ /dev/null
@@ -1,1292 +0,0 @@
-/*
- * Configurable Composite Gadget
- *
- * Initially contributed as "Android Composite Gdaget" by:
- *
- * Copyright (C) 2008 Google, Inc.
- * Author: Mike Lockwood <lockwood@android.com>
- *         Benoit Goby <benoit@android.com>
- *
- * Tailoring it to become a generic Configurable Composite Gadget is
- *
- * Copyright (C) 2012 Samsung Electronics
- * Author: Andrzej Pietrasiewicz <andrzej.p@samsung.com>
- *
- * 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/init.h>
-#include <linux/module.h>
-#include <linux/fs.h>
-#include <linux/delay.h>
-#include <linux/kernel.h>
-#include <linux/utsname.h>
-#include <linux/platform_device.h>
-
-#include <linux/usb/ch9.h>
-#include "composite.h"
-#include <linux/usb/gadget.h>
-
-#include "gadget_chips.h"
-
-/*
- * Kbuild is not very cooperative with respect to linking separately
- * compiled library objects into one module.  So for now we won't use
- * separate compilation ... ensuring init/exit sections work to shrink
- * the runtime footprint, and giving us at least some parts of what
- * a "gcc --combine ... part1.c part2.c part3.c ... " build would.
- */
-#include "usbstring.c"
-#include "config.c"
-#include "epautoconf.c"
-#include "composite.c"
-
-#include "f_mass_storage.c"
-#include "u_serial.c"
-#include "f_acm.c"
-#define USB_ETH_RNDIS y
-#include "f_rndis.c"
-#include "rndis.c"
-#include "u_ether.c"
-#include "f_fs.c"
-
-MODULE_AUTHOR("Mike Lockwood, Andrzej Pietrasiewicz");
-MODULE_DESCRIPTION("Configurable Composite USB Gadget");
-MODULE_LICENSE("GPL");
-MODULE_VERSION("1.0");
-
-static const char longname[] = "Configurable Composite Gadget";
-
-/* Default vendor and product IDs, overridden by userspace */
-#define VENDOR_ID		0x1d6b /* Linux Foundation */
-#define PRODUCT_ID		0x0107
-#define GFS_MAX_DEVS		10
-
-struct ccg_usb_function {
-	char *name;
-	void *config;
-
-	struct device *dev;
-	char *dev_name;
-	struct device_attribute **attributes;
-
-	/* for ccg_dev.enabled_functions */
-	struct list_head enabled_list;
-
-	/* Optional: initialization during gadget bind */
-	int (*init)(struct ccg_usb_function *, struct usb_composite_dev *);
-	/* Optional: cleanup during gadget unbind */
-	void (*cleanup)(struct ccg_usb_function *);
-
-	int (*bind_config)(struct ccg_usb_function *,
-			   struct usb_configuration *);
-
-	/* Optional: called when the configuration is removed */
-	void (*unbind_config)(struct ccg_usb_function *,
-			      struct usb_configuration *);
-	/* Optional: handle ctrl requests before the device is configured */
-	int (*ctrlrequest)(struct ccg_usb_function *,
-			   struct usb_composite_dev *,
-			   const struct usb_ctrlrequest *);
-};
-
-struct ffs_obj {
-	const char *name;
-	bool mounted;
-	bool desc_ready;
-	bool used;
-	struct ffs_data *ffs_data;
-};
-
-struct ccg_dev {
-	struct ccg_usb_function **functions;
-	struct list_head enabled_functions;
-	struct usb_composite_dev *cdev;
-	struct device *dev;
-
-	bool enabled;
-	struct mutex mutex;
-	bool connected;
-	bool sw_connected;
-	struct work_struct work;
-
-	unsigned int max_func_num;
-	unsigned int func_num;
-	struct ffs_obj ffs_tab[GFS_MAX_DEVS];
-};
-
-static struct class *ccg_class;
-static struct ccg_dev *_ccg_dev;
-static int ccg_bind_config(struct usb_configuration *c);
-static void ccg_unbind_config(struct usb_configuration *c);
-
-static char func_names_buf[256];
-
-static struct usb_device_descriptor device_desc = {
-	.bLength              = sizeof(device_desc),
-	.bDescriptorType      = USB_DT_DEVICE,
-	.bcdUSB               = __constant_cpu_to_le16(0x0200),
-	.bDeviceClass         = USB_CLASS_PER_INTERFACE,
-	.idVendor             = __constant_cpu_to_le16(VENDOR_ID),
-	.idProduct            = __constant_cpu_to_le16(PRODUCT_ID),
-	.bcdDevice            = __constant_cpu_to_le16(0xffff),
-	.bNumConfigurations   = 1,
-};
-
-static struct usb_configuration ccg_config_driver = {
-	.label		= "ccg",
-	.unbind		= ccg_unbind_config,
-	.bConfigurationValue = 1,
-	.bmAttributes	= USB_CONFIG_ATT_ONE | USB_CONFIG_ATT_SELFPOWER,
-	.bMaxPower	= 0xFA, /* 500ma */
-};
-
-static void ccg_work(struct work_struct *data)
-{
-	struct ccg_dev *dev = container_of(data, struct ccg_dev, work);
-	struct usb_composite_dev *cdev = dev->cdev;
-	static char *disconnected[2] = { "USB_STATE=DISCONNECTED", NULL };
-	static char *connected[2]    = { "USB_STATE=CONNECTED", NULL };
-	static char *configured[2]   = { "USB_STATE=CONFIGURED", NULL };
-	char **uevent_envp = NULL;
-	unsigned long flags;
-
-	spin_lock_irqsave(&cdev->lock, flags);
-	if (cdev->config)
-		uevent_envp = configured;
-	else if (dev->connected != dev->sw_connected)
-		uevent_envp = dev->connected ? connected : disconnected;
-	dev->sw_connected = dev->connected;
-	spin_unlock_irqrestore(&cdev->lock, flags);
-
-	if (uevent_envp) {
-		kobject_uevent_env(&dev->dev->kobj, KOBJ_CHANGE, uevent_envp);
-		pr_info("%s: sent uevent %s\n", __func__, uevent_envp[0]);
-	} else {
-		pr_info("%s: did not send uevent (%d %d %p)\n", __func__,
-			 dev->connected, dev->sw_connected, cdev->config);
-	}
-}
-
-
-/*-------------------------------------------------------------------------*/
-/* Supported functions initialization */
-
-static struct ffs_obj *functionfs_find_dev(struct ccg_dev *dev,
-					   const char *dev_name)
-{
-	int i;
-
-	for (i = 0; i < dev->max_func_num; i++)
-		if (strcmp(dev->ffs_tab[i].name, dev_name) == 0)
-			return &dev->ffs_tab[i];
-
-	return NULL;
-}
-
-static bool functionfs_all_ready(struct ccg_dev *dev)
-{
-	int i;
-
-	for (i = 0; i < dev->max_func_num; i++)
-		if (dev->ffs_tab[i].used && !dev->ffs_tab[i].desc_ready)
-			return false;
-
-	return true;
-}
-
-static int functionfs_ready_callback(struct ffs_data *ffs)
-{
-	struct ffs_obj *ffs_obj;
-	int ret;
-
-	mutex_lock(&_ccg_dev->mutex);
-
-	ffs_obj = ffs->private_data;
-	if (!ffs_obj) {
-		ret = -EINVAL;
-		goto done;
-	}
-	if (WARN_ON(ffs_obj->desc_ready)) {
-		ret = -EBUSY;
-		goto done;
-	}
-	ffs_obj->ffs_data = ffs;
-
-	if (functionfs_all_ready(_ccg_dev)) {
-		ret = -EBUSY;
-		goto done;
-	}
-	ffs_obj->desc_ready = true;
-
-done:
-	mutex_unlock(&_ccg_dev->mutex);
-	return ret;
-}
-
-static void reset_usb(struct ccg_dev *dev)
-{
-	/* Cancel pending control requests */
-	usb_ep_dequeue(dev->cdev->gadget->ep0, dev->cdev->req);
-	usb_remove_config(dev->cdev, &ccg_config_driver);
-	dev->enabled = false;
-	usb_gadget_disconnect(dev->cdev->gadget);
-}
-
-static void functionfs_closed_callback(struct ffs_data *ffs)
-{
-	struct ffs_obj *ffs_obj;
-
-	mutex_lock(&_ccg_dev->mutex);
-
-	ffs_obj = ffs->private_data;
-	if (!ffs_obj)
-		goto done;
-
-	ffs_obj->desc_ready = false;
-
-	if (_ccg_dev->enabled)
-		reset_usb(_ccg_dev);
-
-done:
-	mutex_unlock(&_ccg_dev->mutex);
-}
-
-static void *functionfs_acquire_dev_callback(const char *dev_name)
-{
-	struct ffs_obj *ffs_dev;
-
-	mutex_lock(&_ccg_dev->mutex);
-
-	ffs_dev = functionfs_find_dev(_ccg_dev, dev_name);
-	if (!ffs_dev) {
-		ffs_dev = ERR_PTR(-ENODEV);
-		goto done;
-	}
-
-	if (ffs_dev->mounted) {
-		ffs_dev = ERR_PTR(-EBUSY);
-		goto done;
-	}
-	ffs_dev->mounted = true;
-
-done:
-	mutex_unlock(&_ccg_dev->mutex);
-	return ffs_dev;
-}
-
-static void functionfs_release_dev_callback(struct ffs_data *ffs_data)
-{
-	struct ffs_obj *ffs_dev;
-
-	mutex_lock(&_ccg_dev->mutex);
-
-	ffs_dev = ffs_data->private_data;
-	if (ffs_dev)
-		ffs_dev->mounted = false;
-
-	mutex_unlock(&_ccg_dev->mutex);
-}
-
-static int functionfs_function_init(struct ccg_usb_function *f,
-				struct usb_composite_dev *cdev)
-{
-	return functionfs_init();
-}
-
-static void functionfs_function_cleanup(struct ccg_usb_function *f)
-{
-	functionfs_cleanup();
-}
-
-static int functionfs_function_bind_config(struct ccg_usb_function *f,
-					   struct usb_configuration *c)
-{
-	struct ccg_dev *dev = _ccg_dev;
-	int i, ret;
-
-	for (i = dev->max_func_num; i--; ) {
-		if (!dev->ffs_tab[i].used)
-			continue;
-		ret = functionfs_bind(dev->ffs_tab[i].ffs_data, c->cdev);
-		if (unlikely(ret < 0)) {
-			while (++i < dev->max_func_num)
-				functionfs_unbind(dev->ffs_tab[i].ffs_data);
-			return ret;
-		}
-	}
-
-	for (i = dev->max_func_num; i--; ) {
-		if (!dev->ffs_tab[i].used)
-			continue;
-		ret = functionfs_bind_config(c->cdev, c,
-					     dev->ffs_tab[i].ffs_data);
-		if (unlikely(ret < 0))
-			return ret;
-	}
-
-	return 0;
-}
-
-static void functionfs_function_unbind_config(struct ccg_usb_function *f,
-					      struct usb_configuration *c)
-{
-	struct ccg_dev *dev = _ccg_dev;
-	int i;
-
-	for (i = dev->max_func_num; i--; )
-		if (dev->ffs_tab[i].ffs_data)
-			functionfs_unbind(dev->ffs_tab[i].ffs_data);
-}
-
-static ssize_t functionfs_user_functions_show(struct device *_dev,
-					      struct device_attribute *attr,
-					      char *buf)
-{
-	struct ccg_dev *dev = _ccg_dev;
-	char *buff = buf;
-	int i;
-
-	mutex_lock(&dev->mutex);
-
-	for (i = 0; i < dev->max_func_num; i++)
-		buff += snprintf(buff, PAGE_SIZE + buf - buff, "%s,",
-				 dev->ffs_tab[i].name);
-
-	mutex_unlock(&dev->mutex);
-
-	if (buff != buf)
-		*(buff - 1) = '\n';
-	return buff - buf;
-}
-
-static ssize_t functionfs_user_functions_store(struct device *_dev,
-					       struct device_attribute *attr,
-					       const char *buff, size_t size)
-{
-	struct ccg_dev *dev = _ccg_dev;
-	char *name, *b;
-	ssize_t ret = size;
-	int i;
-
-	buff = skip_spaces(buff);
-	if (!*buff)
-		return -EINVAL;
-
-	mutex_lock(&dev->mutex);
-
-	if (dev->enabled) {
-		ret = -EBUSY;
-		goto end;
-	}
-
-	for (i = 0; i < dev->max_func_num; i++)
-		if (dev->ffs_tab[i].mounted) {
-			ret = -EBUSY;
-			goto end;
-		}
-
-	strlcpy(func_names_buf, buff, sizeof(func_names_buf));
-	b = strim(func_names_buf);
-
-	/* replace the list of functions */
-	dev->max_func_num = 0;
-	while (b) {
-		name = strsep(&b, ",");
-		if (dev->max_func_num == GFS_MAX_DEVS) {
-			ret = -ENOSPC;
-			goto end;
-		}
-		if (functionfs_find_dev(dev, name)) {
-			ret = -EEXIST;
-			continue;
-		}
-		dev->ffs_tab[dev->max_func_num++].name = name;
-	}
-
-end:
-	mutex_unlock(&dev->mutex);
-	return ret;
-}
-
-static DEVICE_ATTR(user_functions, S_IRUGO | S_IWUSR,
-		   functionfs_user_functions_show,
-		   functionfs_user_functions_store);
-
-static ssize_t functionfs_max_user_functions_show(struct device *_dev,
-						  struct device_attribute *attr,
-						  char *buf)
-{
-	return sprintf(buf, "%d", GFS_MAX_DEVS);
-}
-
-static DEVICE_ATTR(max_user_functions, S_IRUGO,
-		   functionfs_max_user_functions_show, NULL);
-
-static struct device_attribute *functionfs_function_attributes[] = {
-	&dev_attr_user_functions,
-	&dev_attr_max_user_functions,
-	NULL
-};
-
-static struct ccg_usb_function functionfs_function = {
-	.name		= "fs",
-	.init		= functionfs_function_init,
-	.cleanup	= functionfs_function_cleanup,
-	.bind_config	= functionfs_function_bind_config,
-	.unbind_config  = functionfs_function_unbind_config,
-	.attributes	= functionfs_function_attributes,
-};
-
-#define MAX_ACM_INSTANCES 4
-struct acm_function_config {
-	int instances;
-};
-
-static int
-acm_function_init(struct ccg_usb_function *f, struct usb_composite_dev *cdev)
-{
-	f->config = kzalloc(sizeof(struct acm_function_config), GFP_KERNEL);
-	if (!f->config)
-		return -ENOMEM;
-
-	return gserial_setup(cdev->gadget, MAX_ACM_INSTANCES);
-}
-
-static void acm_function_cleanup(struct ccg_usb_function *f)
-{
-	gserial_cleanup();
-	kfree(f->config);
-	f->config = NULL;
-}
-
-static int
-acm_function_bind_config(struct ccg_usb_function *f,
-		struct usb_configuration *c)
-{
-	int i;
-	int ret = 0;
-	struct acm_function_config *config = f->config;
-
-	for (i = 0; i < config->instances; i++) {
-		ret = acm_bind_config(c, i);
-		if (ret) {
-			pr_err("Could not bind acm%u config\n", i);
-			break;
-		}
-	}
-
-	return ret;
-}
-
-static ssize_t acm_instances_show(struct device *dev,
-		struct device_attribute *attr, char *buf)
-{
-	struct ccg_usb_function *f = dev_get_drvdata(dev);
-	struct acm_function_config *config = f->config;
-	return sprintf(buf, "%d\n", config->instances);
-}
-
-static ssize_t acm_instances_store(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t size)
-{
-	struct ccg_usb_function *f = dev_get_drvdata(dev);
-	struct acm_function_config *config = f->config;
-	int value;
-	int ret = 0;
-
-	ret = kstrtoint(buf, 10, &value);
-	if (ret)
-		return ret;
-
-	if (value > MAX_ACM_INSTANCES)
-		return -EINVAL;
-
-	config->instances = value;
-
-	return size;
-}
-
-static DEVICE_ATTR(instances, S_IRUGO | S_IWUSR, acm_instances_show,
-						 acm_instances_store);
-static struct device_attribute *acm_function_attributes[] = {
-	&dev_attr_instances,
-	NULL
-};
-
-static struct ccg_usb_function acm_function = {
-	.name		= "acm",
-	.init		= acm_function_init,
-	.cleanup	= acm_function_cleanup,
-	.bind_config	= acm_function_bind_config,
-	.attributes	= acm_function_attributes,
-};
-
-struct rndis_function_config {
-	u8      ethaddr[ETH_ALEN];
-	u32     vendorID;
-	char	manufacturer[256];
-	/* "Wireless" RNDIS; auto-detected by Windows */
-	bool	wceis;
-};
-
-static int rndis_function_init(struct ccg_usb_function *f,
-			       struct usb_composite_dev *cdev)
-{
-	f->config = kzalloc(sizeof(struct rndis_function_config), GFP_KERNEL);
-	if (!f->config)
-		return -ENOMEM;
-	return 0;
-}
-
-static void rndis_function_cleanup(struct ccg_usb_function *f)
-{
-	kfree(f->config);
-	f->config = NULL;
-}
-
-static int rndis_function_bind_config(struct ccg_usb_function *f,
-				      struct usb_configuration *c)
-{
-	int ret;
-	struct rndis_function_config *rndis = f->config;
-
-	if (!rndis) {
-		pr_err("%s: rndis_pdata\n", __func__);
-		return -1;
-	}
-
-	pr_info("%s MAC: %pM\n", __func__, rndis->ethaddr);
-
-	ret = gether_setup_name(c->cdev->gadget, rndis->ethaddr, "rndis");
-	if (ret) {
-		pr_err("%s: gether_setup failed\n", __func__);
-		return ret;
-	}
-
-	if (rndis->wceis) {
-		/* "Wireless" RNDIS; auto-detected by Windows */
-		rndis_iad_descriptor.bFunctionClass =
-						USB_CLASS_WIRELESS_CONTROLLER;
-		rndis_iad_descriptor.bFunctionSubClass = 0x01;
-		rndis_iad_descriptor.bFunctionProtocol = 0x03;
-		rndis_control_intf.bInterfaceClass =
-						USB_CLASS_WIRELESS_CONTROLLER;
-		rndis_control_intf.bInterfaceSubClass =	 0x01;
-		rndis_control_intf.bInterfaceProtocol =	 0x03;
-	}
-
-	return rndis_bind_config_vendor(c, rndis->ethaddr, rndis->vendorID,
-					   rndis->manufacturer);
-}
-
-static void rndis_function_unbind_config(struct ccg_usb_function *f,
-						struct usb_configuration *c)
-{
-	gether_cleanup();
-}
-
-static ssize_t rndis_manufacturer_show(struct device *dev,
-		struct device_attribute *attr, char *buf)
-{
-	struct ccg_usb_function *f = dev_get_drvdata(dev);
-	struct rndis_function_config *config = f->config;
-	return sprintf(buf, "%s\n", config->manufacturer);
-}
-
-static ssize_t rndis_manufacturer_store(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t size)
-{
-	struct ccg_usb_function *f = dev_get_drvdata(dev);
-	struct rndis_function_config *config = f->config;
-
-	if (size >= sizeof(config->manufacturer))
-		return -EINVAL;
-	memcpy(config->manufacturer, buf, size);
-	config->manufacturer[size] = 0;
-
-	return size;
-}
-
-static DEVICE_ATTR(manufacturer, S_IRUGO | S_IWUSR, rndis_manufacturer_show,
-						    rndis_manufacturer_store);
-
-static ssize_t rndis_wceis_show(struct device *dev,
-		struct device_attribute *attr, char *buf)
-{
-	struct ccg_usb_function *f = dev_get_drvdata(dev);
-	struct rndis_function_config *config = f->config;
-	return sprintf(buf, "%d\n", config->wceis);
-}
-
-static ssize_t rndis_wceis_store(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t size)
-{
-	struct ccg_usb_function *f = dev_get_drvdata(dev);
-	struct rndis_function_config *config = f->config;
-	int value;
-	int ret;
-
-	ret = kstrtoint(buf, 10, &value);
-	if (ret)
-		return ret;
-
-	config->wceis = value;
-
-	return size;
-}
-
-static DEVICE_ATTR(wceis, S_IRUGO | S_IWUSR, rndis_wceis_show,
-					     rndis_wceis_store);
-
-static ssize_t rndis_ethaddr_show(struct device *dev,
-		struct device_attribute *attr, char *buf)
-{
-	struct ccg_usb_function *f = dev_get_drvdata(dev);
-	struct rndis_function_config *rndis = f->config;
-	return sprintf(buf, "%pM\n", rndis->ethaddr);
-}
-
-static ssize_t rndis_ethaddr_store(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t size)
-{
-	struct ccg_usb_function *f = dev_get_drvdata(dev);
-	struct rndis_function_config *rndis = f->config;
-	unsigned char tmp[6];
-
-	if (sscanf(buf, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
-		   tmp + 0, tmp + 1, tmp + 2, tmp + 3, tmp + 4, tmp + 5) !=
-	    ETH_ALEN)
-		return -EINVAL;
-
-	memcpy(rndis->ethaddr, tmp, ETH_ALEN);
-
-	return ETH_ALEN;
-
-}
-
-static DEVICE_ATTR(ethaddr, S_IRUGO | S_IWUSR, rndis_ethaddr_show,
-					       rndis_ethaddr_store);
-
-static ssize_t rndis_vendorID_show(struct device *dev,
-		struct device_attribute *attr, char *buf)
-{
-	struct ccg_usb_function *f = dev_get_drvdata(dev);
-	struct rndis_function_config *config = f->config;
-	return sprintf(buf, "%04x\n", config->vendorID);
-}
-
-static ssize_t rndis_vendorID_store(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t size)
-{
-	struct ccg_usb_function *f = dev_get_drvdata(dev);
-	struct rndis_function_config *config = f->config;
-	int value;
-	int ret;
-
-	ret = kstrtou32(buf, 16, &value);
-	if (ret)
-		return ret;
-
-	config->vendorID = value;
-
-	return size;
-}
-
-static DEVICE_ATTR(vendorID, S_IRUGO | S_IWUSR, rndis_vendorID_show,
-						rndis_vendorID_store);
-
-static struct device_attribute *rndis_function_attributes[] = {
-	&dev_attr_manufacturer,
-	&dev_attr_wceis,
-	&dev_attr_ethaddr,
-	&dev_attr_vendorID,
-	NULL
-};
-
-static struct ccg_usb_function rndis_function = {
-	.name		= "rndis",
-	.init		= rndis_function_init,
-	.cleanup	= rndis_function_cleanup,
-	.bind_config	= rndis_function_bind_config,
-	.unbind_config	= rndis_function_unbind_config,
-	.attributes	= rndis_function_attributes,
-};
-
-static int mass_storage_function_init(struct ccg_usb_function *f,
-					struct usb_composite_dev *cdev)
-{
-	struct fsg_config fsg;
-	struct fsg_common *common;
-	int err;
-
-	memset(&fsg, 0, sizeof(fsg));
-	fsg.nluns = 1;
-	fsg.luns[0].removable = 1;
-	fsg.vendor_name = iManufacturer;
-	fsg.product_name = iProduct;
-
-	common = fsg_common_init(NULL, cdev, &fsg);
-	if (IS_ERR(common))
-		return PTR_ERR(common);
-
-	err = sysfs_create_link(&f->dev->kobj,
-				&common->luns[0].dev.kobj,
-				"lun");
-	if (err) {
-		fsg_common_put(common);
-		return err;
-	}
-
-	f->config = common;
-	return 0;
-}
-
-static void mass_storage_function_cleanup(struct ccg_usb_function *f)
-{
-	fsg_common_put(f->config);
-	f->config = NULL;
-}
-
-static int mass_storage_function_bind_config(struct ccg_usb_function *f,
-					     struct usb_configuration *c)
-{
-	struct fsg_common *common = f->config;
-	return fsg_bind_config(c->cdev, c, common);
-}
-
-static struct ccg_usb_function mass_storage_function = {
-	.name		= "mass_storage",
-	.init		= mass_storage_function_init,
-	.cleanup	= mass_storage_function_cleanup,
-	.bind_config	= mass_storage_function_bind_config,
-};
-
-static struct ccg_usb_function *supported_functions[] = {
-	&functionfs_function,
-	&acm_function,
-	&rndis_function,
-	&mass_storage_function,
-	NULL
-};
-
-
-static int ccg_init_functions(struct ccg_usb_function **functions,
-				  struct usb_composite_dev *cdev)
-{
-	struct ccg_dev *dev = _ccg_dev;
-	struct ccg_usb_function *f;
-	struct device_attribute **attrs;
-	struct device_attribute *attr;
-	int err;
-	int index = 0;
-
-	for (; (f = *functions++); index++) {
-		f->dev_name = kasprintf(GFP_KERNEL, "f_%s", f->name);
-		if (!f->dev_name) {
-			pr_err("%s: Failed to alloc name %s", __func__,
-			       f->name);
-			err = -ENOMEM;
-			goto err_alloc;
-		}
-		f->dev = device_create(ccg_class, dev->dev,
-				MKDEV(0, index), f, f->dev_name);
-		if (IS_ERR(f->dev)) {
-			pr_err("%s: Failed to create dev %s", __func__,
-							f->dev_name);
-			err = PTR_ERR(f->dev);
-			f->dev = NULL;
-			goto err_create;
-		}
-
-		if (f->init) {
-			err = f->init(f, cdev);
-			if (err) {
-				pr_err("%s: Failed to init %s", __func__,
-								f->name);
-				goto err_out;
-			}
-		}
-
-		attrs = f->attributes;
-		if (attrs) {
-			while ((attr = *attrs++) && !err)
-				err = device_create_file(f->dev, attr);
-		}
-		if (err) {
-			pr_err("%s: Failed to create function %s attributes",
-					__func__, f->name);
-			goto err_uninit;
-		}
-	}
-	return 0;
-
-err_uninit:
-	if (f->cleanup)
-		f->cleanup(f);
-err_out:
-	device_destroy(ccg_class, f->dev->devt);
-	f->dev = NULL;
-err_create:
-	kfree(f->dev_name);
-err_alloc:
-	return err;
-}
-
-static void ccg_cleanup_functions(struct ccg_usb_function **functions)
-{
-	struct ccg_usb_function *f;
-
-	while (*functions) {
-		f = *functions++;
-
-		if (f->dev) {
-			if (f->cleanup)
-				f->cleanup(f);
-			device_destroy(ccg_class, f->dev->devt);
-			kfree(f->dev_name);
-		}
-	}
-}
-
-static int ccg_bind_enabled_functions(struct ccg_dev *dev,
-				      struct usb_configuration *c)
-{
-	struct ccg_usb_function *f;
-	int ret;
-
-	list_for_each_entry(f, &dev->enabled_functions, enabled_list) {
-		ret = f->bind_config(f, c);
-		if (ret) {
-			pr_err("%s: %s failed", __func__, f->name);
-			return ret;
-		}
-	}
-	return 0;
-}
-
-static void ccg_unbind_enabled_functions(struct ccg_dev *dev,
-					 struct usb_configuration *c)
-{
-	struct ccg_usb_function *f;
-
-	list_for_each_entry(f, &dev->enabled_functions, enabled_list)
-		if (f->unbind_config)
-			f->unbind_config(f, c);
-}
-
-static int ccg_enable_function(struct ccg_dev *dev, char *name)
-{
-	struct ccg_usb_function **functions = dev->functions;
-	struct ccg_usb_function *f;
-	while ((f = *functions++)) {
-		if (!strcmp(name, f->name)) {
-			list_add_tail(&f->enabled_list,
-						&dev->enabled_functions);
-			return 0;
-		}
-	}
-	return -EINVAL;
-}
-
-/*-------------------------------------------------------------------------*/
-/* /sys/class/ccg_usb/ccg%d/ interface */
-
-static ssize_t
-functions_show(struct device *pdev, struct device_attribute *attr, char *buf)
-{
-	struct ccg_dev *dev = dev_get_drvdata(pdev);
-	struct ccg_usb_function *f;
-	char *buff = buf;
-	int i;
-
-	mutex_lock(&dev->mutex);
-
-	list_for_each_entry(f, &dev->enabled_functions, enabled_list)
-		buff += sprintf(buff, "%s,", f->name);
-	for (i = 0; i < dev->max_func_num; i++)
-		if (dev->ffs_tab[i].used)
-			buff += sprintf(buff, "%s", dev->ffs_tab[i].name);
-
-	mutex_unlock(&dev->mutex);
-
-	if (buff != buf)
-		*(buff-1) = '\n';
-	return buff - buf;
-}
-
-static ssize_t
-functions_store(struct device *pdev, struct device_attribute *attr,
-			       const char *buff, size_t size)
-{
-	struct ccg_dev *dev = dev_get_drvdata(pdev);
-	char *name;
-	char buf[256], *b;
-	int err, i;
-	bool functionfs_enabled;
-
-	buff = skip_spaces(buff);
-	if (!*buff)
-		return -EINVAL;
-
-	mutex_lock(&dev->mutex);
-
-	if (dev->enabled) {
-		mutex_unlock(&dev->mutex);
-		return -EBUSY;
-	}
-
-	INIT_LIST_HEAD(&dev->enabled_functions);
-	functionfs_enabled = false;
-	for (i = 0; i < dev->max_func_num; i++)
-		dev->ffs_tab[i].used = false;
-
-	strlcpy(buf, buff, sizeof(buf));
-	b = strim(buf);
-
-	while (b) {
-		struct ffs_obj *user_func;
-
-		name = strsep(&b, ",");
-		/* handle FunctionFS implicitly */
-		if (!strcmp(name, functionfs_function.name)) {
-			pr_err("ccg_usb: Cannot explicitly enable '%s'", name);
-			continue;
-		}
-		user_func = functionfs_find_dev(dev, name);
-		if (user_func)
-			name = functionfs_function.name;
-		err = 0;
-		if (!user_func || !functionfs_enabled)
-			err = ccg_enable_function(dev, name);
-		if (err)
-			pr_err("ccg_usb: Cannot enable '%s'", name);
-		else if (user_func) {
-			user_func->used = true;
-			dev->func_num++;
-			functionfs_enabled = true;
-		}
-	}
-
-	mutex_unlock(&dev->mutex);
-
-	return size;
-}
-
-static ssize_t enable_show(struct device *pdev, struct device_attribute *attr,
-			   char *buf)
-{
-	struct ccg_dev *dev = dev_get_drvdata(pdev);
-	return sprintf(buf, "%d\n", dev->enabled);
-}
-
-static ssize_t enable_store(struct device *pdev, struct device_attribute *attr,
-			    const char *buff, size_t size)
-{
-	struct ccg_dev *dev = dev_get_drvdata(pdev);
-	struct usb_composite_dev *cdev = dev->cdev;
-	int enabled = 0;
-
-	mutex_lock(&dev->mutex);
-	sscanf(buff, "%d", &enabled);
-	if (enabled && dev->func_num && !functionfs_all_ready(dev)) {
-		mutex_unlock(&dev->mutex);
-		return -ENODEV;
-	}
-
-	if (enabled && !dev->enabled) {
-		int ret;
-
-		cdev->next_string_id = 0;
-		/*
-		 * Update values in composite driver's copy of
-		 * device descriptor.
-		 */
-		cdev->desc.bDeviceClass = device_desc.bDeviceClass;
-		cdev->desc.bDeviceSubClass = device_desc.bDeviceSubClass;
-		cdev->desc.bDeviceProtocol = device_desc.bDeviceProtocol;
-		cdev->desc.idVendor = idVendor;
-		cdev->desc.idProduct = idProduct;
-		cdev->desc.bcdDevice = bcdDevice;
-
-		usb_add_config(cdev, &ccg_config_driver, ccg_bind_config);
-		dev->enabled = true;
-		ret = usb_gadget_connect(cdev->gadget);
-		if (ret) {
-			dev->enabled = false;
-			usb_remove_config(cdev, &ccg_config_driver);
-		}
-	} else if (!enabled && dev->enabled) {
-		reset_usb(dev);
-	} else {
-		pr_err("ccg_usb: already %s\n",
-			dev->enabled ? "enabled" : "disabled");
-	}
-
-	mutex_unlock(&dev->mutex);
-	return size;
-}
-
-static ssize_t state_show(struct device *pdev, struct device_attribute *attr,
-			   char *buf)
-{
-	struct ccg_dev *dev = dev_get_drvdata(pdev);
-	struct usb_composite_dev *cdev = dev->cdev;
-	char *state = "DISCONNECTED";
-	unsigned long flags;
-
-	if (!cdev)
-		goto out;
-
-	spin_lock_irqsave(&cdev->lock, flags);
-	if (cdev->config)
-		state = "CONFIGURED";
-	else if (dev->connected)
-		state = "CONNECTED";
-	spin_unlock_irqrestore(&cdev->lock, flags);
-out:
-	return sprintf(buf, "%s\n", state);
-}
-
-#define DESCRIPTOR_ATTR(field, format_string)				\
-static ssize_t								\
-field ## _show(struct device *dev, struct device_attribute *attr,	\
-		char *buf)						\
-{									\
-	return sprintf(buf, format_string, device_desc.field);		\
-}									\
-static ssize_t								\
-field ## _store(struct device *dev, struct device_attribute *attr,	\
-		const char *buf, size_t size)				\
-{									\
-	int value;							\
-	if (sscanf(buf, format_string, &value) == 1) {			\
-		device_desc.field = value;				\
-		return size;						\
-	}								\
-	return -1;							\
-}									\
-static DEVICE_ATTR(field, S_IRUGO | S_IWUSR, field ## _show, field ## _store);
-
-DESCRIPTOR_ATTR(bDeviceClass, "%d\n")
-DESCRIPTOR_ATTR(bDeviceSubClass, "%d\n")
-DESCRIPTOR_ATTR(bDeviceProtocol, "%d\n")
-
-static DEVICE_ATTR(functions, S_IRUGO | S_IWUSR, functions_show,
-						 functions_store);
-static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR, enable_show, enable_store);
-static DEVICE_ATTR(state, S_IRUGO, state_show, NULL);
-
-static struct device_attribute *ccg_usb_attributes[] = {
-	&dev_attr_bDeviceClass,
-	&dev_attr_bDeviceSubClass,
-	&dev_attr_bDeviceProtocol,
-	&dev_attr_functions,
-	&dev_attr_enable,
-	&dev_attr_state,
-	NULL
-};
-
-/*-------------------------------------------------------------------------*/
-/* Composite driver */
-
-static int ccg_bind_config(struct usb_configuration *c)
-{
-	struct ccg_dev *dev = _ccg_dev;
-	return ccg_bind_enabled_functions(dev, c);
-}
-
-static void ccg_unbind_config(struct usb_configuration *c)
-{
-	struct ccg_dev *dev = _ccg_dev;
-
-	ccg_unbind_enabled_functions(dev, c);
-
-	usb_ep_autoconfig_reset(dev->cdev->gadget);
-}
-
-static int ccg_bind(struct usb_composite_dev *cdev)
-{
-	struct ccg_dev *dev = _ccg_dev;
-	struct usb_gadget	*gadget = cdev->gadget;
-	int			gcnum, ret;
-
-	/*
-	 * Start disconnected. Userspace will connect the gadget once
-	 * it is done configuring the functions.
-	 */
-	usb_gadget_disconnect(gadget);
-
-	ret = ccg_init_functions(dev->functions, cdev);
-	if (ret)
-		return ret;
-
-	gcnum = usb_gadget_controller_number(gadget);
-	if (gcnum >= 0)
-		device_desc.bcdDevice = cpu_to_le16(0x0200 + gcnum);
-	else {
-		pr_warn("%s: controller '%s' not recognized\n",
-			longname, gadget->name);
-		device_desc.bcdDevice = __constant_cpu_to_le16(0x9999);
-	}
-
-	usb_gadget_set_selfpowered(gadget);
-	dev->cdev = cdev;
-
-	return 0;
-}
-
-static int ccg_usb_unbind(struct usb_composite_dev *cdev)
-{
-	struct ccg_dev *dev = _ccg_dev;
-
-	cancel_work_sync(&dev->work);
-	ccg_cleanup_functions(dev->functions);
-	return 0;
-}
-
-static struct usb_composite_driver ccg_usb_driver = {
-	.name		= "configurable_usb",
-	.dev		= &device_desc,
-	.bind		= ccg_bind,
-	.unbind		= ccg_usb_unbind,
-	.needs_serial	= true,
-	.iManufacturer	= "Linux Foundation",
-	.iProduct	= longname,
-	.iSerialNumber	= "1234567890123456",
-};
-
-static int ccg_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *c)
-{
-	struct ccg_dev		*dev = _ccg_dev;
-	struct usb_composite_dev	*cdev = get_gadget_data(gadget);
-	struct usb_request		*req = cdev->req;
-	struct ccg_usb_function	*f;
-	int value = -EOPNOTSUPP;
-	unsigned long flags;
-
-	req->zero = 0;
-	req->complete = composite_setup_complete;
-	req->length = 0;
-	gadget->ep0->driver_data = cdev;
-
-	list_for_each_entry(f, &dev->enabled_functions, enabled_list) {
-		if (f->ctrlrequest) {
-			value = f->ctrlrequest(f, cdev, c);
-			if (value >= 0)
-				break;
-		}
-	}
-
-	if (value < 0)
-		value = composite_setup(gadget, c);
-
-	spin_lock_irqsave(&cdev->lock, flags);
-	if (!dev->connected) {
-		dev->connected = 1;
-		schedule_work(&dev->work);
-	} else if (c->bRequest == USB_REQ_SET_CONFIGURATION &&
-						cdev->config) {
-		schedule_work(&dev->work);
-	}
-	spin_unlock_irqrestore(&cdev->lock, flags);
-
-	return value;
-}
-
-static void ccg_disconnect(struct usb_gadget *gadget)
-{
-	struct ccg_dev *dev = _ccg_dev;
-	struct usb_composite_dev *cdev = get_gadget_data(gadget);
-	unsigned long flags;
-
-	composite_disconnect(gadget);
-
-	spin_lock_irqsave(&cdev->lock, flags);
-	dev->connected = 0;
-	schedule_work(&dev->work);
-	spin_unlock_irqrestore(&cdev->lock, flags);
-}
-
-static int ccg_create_device(struct ccg_dev *dev)
-{
-	struct device_attribute **attrs = ccg_usb_attributes;
-	struct device_attribute *attr;
-	int err;
-
-	dev->dev = device_create(ccg_class, NULL, MKDEV(0, 0), NULL, "ccg0");
-	if (IS_ERR(dev->dev))
-		return PTR_ERR(dev->dev);
-
-	dev_set_drvdata(dev->dev, dev);
-
-	while ((attr = *attrs++)) {
-		err = device_create_file(dev->dev, attr);
-		if (err) {
-			device_destroy(ccg_class, dev->dev->devt);
-			return err;
-		}
-	}
-	return 0;
-}
-
-
-static int __init ccg_init(void)
-{
-	struct ccg_dev *dev;
-	int err;
-
-	ccg_class = class_create(THIS_MODULE, "ccg_usb");
-	if (IS_ERR(ccg_class))
-		return PTR_ERR(ccg_class);
-
-	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
-	if (!dev) {
-		class_destroy(ccg_class);
-		return -ENOMEM;
-	}
-
-	dev->functions = supported_functions;
-	INIT_LIST_HEAD(&dev->enabled_functions);
-	INIT_WORK(&dev->work, ccg_work);
-	mutex_init(&dev->mutex);
-
-	err = ccg_create_device(dev);
-	if (err) {
-		class_destroy(ccg_class);
-		kfree(dev);
-		return err;
-	}
-
-	_ccg_dev = dev;
-
-	/* Override composite driver functions */
-	composite_driver.setup = ccg_setup;
-	composite_driver.disconnect = ccg_disconnect;
-
-	err = usb_composite_probe(&ccg_usb_driver);
-	if (err) {
-		class_destroy(ccg_class);
-		kfree(dev);
-	}
-
-	return err;
-}
-module_init(ccg_init);
-
-static void __exit ccg_exit(void)
-{
-	usb_composite_unregister(&ccg_usb_driver);
-	class_destroy(ccg_class);
-	kfree(_ccg_dev);
-	_ccg_dev = NULL;
-}
-module_exit(ccg_exit);
diff --git a/drivers/staging/ccg/composite.c b/drivers/staging/ccg/composite.c
deleted file mode 100644
index 228b4574f22..00000000000
--- a/drivers/staging/ccg/composite.c
+++ /dev/null
@@ -1,1688 +0,0 @@
-/*
- * composite.c - infrastructure for Composite USB Gadgets
- *
- * Copyright (C) 2006-2008 David Brownell
- *
- * 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.
- */
-
-/* #define VERBOSE_DEBUG */
-
-#include <linux/kallsyms.h>
-#include <linux/kernel.h>
-#include <linux/slab.h>
-#include <linux/module.h>
-#include <linux/device.h>
-#include <linux/utsname.h>
-
-#include <linux/usb/composite.h>
-#include <asm/unaligned.h>
-
-/*
- * The code in this file is utility code, used to build a gadget driver
- * from one or more "function" drivers, one or more "configuration"
- * objects, and a "usb_composite_driver" by gluing them together along
- * with the relevant device-wide data.
- */
-
-/* big enough to hold our biggest descriptor */
-#define USB_BUFSIZ	1024
-
-static struct usb_composite_driver *composite;
-
-/* Some systems will need runtime overrides for the  product identifiers
- * published in the device descriptor, either numbers or strings or both.
- * String parameters are in UTF-8 (superset of ASCII's 7 bit characters).
- */
-
-static ushort idVendor;
-module_param(idVendor, ushort, 0644);
-MODULE_PARM_DESC(idVendor, "USB Vendor ID");
-
-static ushort idProduct;
-module_param(idProduct, ushort, 0644);
-MODULE_PARM_DESC(idProduct, "USB Product ID");
-
-static ushort bcdDevice;
-module_param(bcdDevice, ushort, 0644);
-MODULE_PARM_DESC(bcdDevice, "USB Device version (BCD)");
-
-static char *iManufacturer;
-module_param(iManufacturer, charp, 0644);
-MODULE_PARM_DESC(iManufacturer, "USB Manufacturer string");
-
-static char *iProduct;
-module_param(iProduct, charp, 0644);
-MODULE_PARM_DESC(iProduct, "USB Product string");
-
-static char *iSerialNumber;
-module_param(iSerialNumber, charp, 0644);
-MODULE_PARM_DESC(iSerialNumber, "SerialNumber string");
-
-static char composite_manufacturer[50];
-
-/*-------------------------------------------------------------------------*/
-/**
- * next_ep_desc() - advance to the next EP descriptor
- * @t: currect pointer within descriptor array
- *
- * Return: next EP descriptor or NULL
- *
- * Iterate over @t until either EP descriptor found or
- * NULL (that indicates end of list) encountered
- */
-static struct usb_descriptor_header**
-next_ep_desc(struct usb_descriptor_header **t)
-{
-	for (; *t; t++) {
-		if ((*t)->bDescriptorType == USB_DT_ENDPOINT)
-			return t;
-	}
-	return NULL;
-}
-
-/*
- * for_each_ep_desc()- iterate over endpoint descriptors in the
- *		descriptors list
- * @start:	pointer within descriptor array.
- * @ep_desc:	endpoint descriptor to use as the loop cursor
- */
-#define for_each_ep_desc(start, ep_desc) \
-	for (ep_desc = next_ep_desc(start); \
-	      ep_desc; ep_desc = next_ep_desc(ep_desc+1))
-
-/**
- * config_ep_by_speed() - configures the given endpoint
- * according to gadget speed.
- * @g: pointer to the gadget
- * @f: usb function
- * @_ep: the endpoint to configure
- *
- * Return: error code, 0 on success
- *
- * This function chooses the right descriptors for a given
- * endpoint according to gadget speed and saves it in the
- * endpoint desc field. If the endpoint already has a descriptor
- * assigned to it - overwrites it with currently corresponding
- * descriptor. The endpoint maxpacket field is updated according
- * to the chosen descriptor.
- * Note: the supplied function should hold all the descriptors
- * for supported speeds
- */
-int config_ep_by_speed(struct usb_gadget *g,
-			struct usb_function *f,
-			struct usb_ep *_ep)
-{
-	struct usb_composite_dev	*cdev = get_gadget_data(g);
-	struct usb_endpoint_descriptor *chosen_desc = NULL;
-	struct usb_descriptor_header **speed_desc = NULL;
-
-	struct usb_ss_ep_comp_descriptor *comp_desc = NULL;
-	int want_comp_desc = 0;
-
-	struct usb_descriptor_header **d_spd; /* cursor for speed desc */
-
-	if (!g || !f || !_ep)
-		return -EIO;
-
-	/* select desired speed */
-	switch (g->speed) {
-	case USB_SPEED_SUPER:
-		if (gadget_is_superspeed(g)) {
-			speed_desc = f->ss_descriptors;
-			want_comp_desc = 1;
-			break;
-		}
-		/* else: Fall trough */
-	case USB_SPEED_HIGH:
-		if (gadget_is_dualspeed(g)) {
-			speed_desc = f->hs_descriptors;
-			break;
-		}
-		/* else: fall through */
-	default:
-		speed_desc = f->descriptors;
-	}
-	/* find descriptors */
-	for_each_ep_desc(speed_desc, d_spd) {
-		chosen_desc = (struct usb_endpoint_descriptor *)*d_spd;
-		if (chosen_desc->bEndpointAddress == _ep->address)
-			goto ep_found;
-	}
-	return -EIO;
-
-ep_found:
-	/* commit results */
-	_ep->maxpacket = usb_endpoint_maxp(chosen_desc);
-	_ep->desc = chosen_desc;
-	_ep->comp_desc = NULL;
-	_ep->maxburst = 0;
-	_ep->mult = 0;
-	if (!want_comp_desc)
-		return 0;
-
-	/*
-	 * Companion descriptor should follow EP descriptor
-	 * USB 3.0 spec, #9.6.7
-	 */
-	comp_desc = (struct usb_ss_ep_comp_descriptor *)*(++d_spd);
-	if (!comp_desc ||
-	    (comp_desc->bDescriptorType != USB_DT_SS_ENDPOINT_COMP))
-		return -EIO;
-	_ep->comp_desc = comp_desc;
-	if (g->speed == USB_SPEED_SUPER) {
-		switch (usb_endpoint_type(_ep->desc)) {
-		case USB_ENDPOINT_XFER_ISOC:
-			/* mult: bits 1:0 of bmAttributes */
-			_ep->mult = comp_desc->bmAttributes & 0x3;
-		case USB_ENDPOINT_XFER_BULK:
-		case USB_ENDPOINT_XFER_INT:
-			_ep->maxburst = comp_desc->bMaxBurst + 1;
-			break;
-		default:
-			if (comp_desc->bMaxBurst != 0)
-				ERROR(cdev, "ep0 bMaxBurst must be 0\n");
-			_ep->maxburst = 1;
-			break;
-		}
-	}
-	return 0;
-}
-
-/**
- * usb_add_function() - add a function to a configuration
- * @config: the configuration
- * @function: the function being added
- * Context: single threaded during gadget setup
- *
- * After initialization, each configuration must have one or more
- * functions added to it.  Adding a function involves calling its @bind()
- * method to allocate resources such as interface and string identifiers
- * and endpoints.
- *
- * This function returns the value of the function's bind(), which is
- * zero for success else a negative errno value.
- */
-int usb_add_function(struct usb_configuration *config,
-		struct usb_function *function)
-{
-	int	value = -EINVAL;
-
-	DBG(config->cdev, "adding '%s'/%p to config '%s'/%p\n",
-			function->name, function,
-			config->label, config);
-
-	if (!function->set_alt || !function->disable)
-		goto done;
-
-	function->config = config;
-	list_add_tail(&function->list, &config->functions);
-
-	/* REVISIT *require* function->bind? */
-	if (function->bind) {
-		value = function->bind(config, function);
-		if (value < 0) {
-			list_del(&function->list);
-			function->config = NULL;
-		}
-	} else
-		value = 0;
-
-	/* We allow configurations that don't work at both speeds.
-	 * If we run into a lowspeed Linux system, treat it the same
-	 * as full speed ... it's the function drivers that will need
-	 * to avoid bulk and ISO transfers.
-	 */
-	if (!config->fullspeed && function->descriptors)
-		config->fullspeed = true;
-	if (!config->highspeed && function->hs_descriptors)
-		config->highspeed = true;
-	if (!config->superspeed && function->ss_descriptors)
-		config->superspeed = true;
-
-done:
-	if (value)
-		DBG(config->cdev, "adding '%s'/%p --> %d\n",
-				function->name, function, value);
-	return value;
-}
-
-/**
- * usb_function_deactivate - prevent function and gadget enumeration
- * @function: the function that isn't yet ready to respond
- *
- * Blocks response of the gadget driver to host enumeration by
- * preventing the data line pullup from being activated.  This is
- * normally called during @bind() processing to change from the
- * initial "ready to respond" state, or when a required resource
- * becomes available.
- *
- * For example, drivers that serve as a passthrough to a userspace
- * daemon can block enumeration unless that daemon (such as an OBEX,
- * MTP, or print server) is ready to handle host requests.
- *
- * Not all systems support software control of their USB peripheral
- * data pullups.
- *
- * Returns zero on success, else negative errno.
- */
-int usb_function_deactivate(struct usb_function *function)
-{
-	struct usb_composite_dev	*cdev = function->config->cdev;
-	unsigned long			flags;
-	int				status = 0;
-
-	spin_lock_irqsave(&cdev->lock, flags);
-
-	if (cdev->deactivations == 0)
-		status = usb_gadget_disconnect(cdev->gadget);
-	if (status == 0)
-		cdev->deactivations++;
-
-	spin_unlock_irqrestore(&cdev->lock, flags);
-	return status;
-}
-
-/**
- * usb_function_activate - allow function and gadget enumeration
- * @function: function on which usb_function_activate() was called
- *
- * Reverses effect of usb_function_deactivate().  If no more functions
- * are delaying their activation, the gadget driver will respond to
- * host enumeration procedures.
- *
- * Returns zero on success, else negative errno.
- */
-int usb_function_activate(struct usb_function *function)
-{
-	struct usb_composite_dev	*cdev = function->config->cdev;
-	unsigned long			flags;
-	int				status = 0;
-
-	spin_lock_irqsave(&cdev->lock, flags);
-
-	if (WARN_ON(cdev->deactivations == 0))
-		status = -EINVAL;
-	else {
-		cdev->deactivations--;
-		if (cdev->deactivations == 0)
-			status = usb_gadget_connect(cdev->gadget);
-	}
-
-	spin_unlock_irqrestore(&cdev->lock, flags);
-	return status;
-}
-
-/**
- * usb_interface_id() - allocate an unused interface ID
- * @config: configuration associated with the interface
- * @function: function handling the interface
- * Context: single threaded during gadget setup
- *
- * usb_interface_id() is called from usb_function.bind() callbacks to
- * allocate new interface IDs.  The function driver will then store that
- * ID in interface, association, CDC union, and other descriptors.  It
- * will also handle any control requests targeted at that interface,
- * particularly changing its altsetting via set_alt().  There may
- * also be class-specific or vendor-specific requests to handle.
- *
- * All interface identifier should be allocated using this routine, to
- * ensure that for example different functions don't wrongly assign
- * different meanings to the same identifier.  Note that since interface
- * identifiers are configuration-specific, functions used in more than
- * one configuration (or more than once in a given configuration) need
- * multiple versions of the relevant descriptors.
- *
- * Returns the interface ID which was allocated; or -ENODEV if no
- * more interface IDs can be allocated.
- */
-int usb_interface_id(struct usb_configuration *config,
-		struct usb_function *function)
-{
-	unsigned id = config->next_interface_id;
-
-	if (id < MAX_CONFIG_INTERFACES) {
-		config->interface[id] = function;
-		config->next_interface_id = id + 1;
-		return id;
-	}
-	return -ENODEV;
-}
-
-static int config_buf(struct usb_configuration *config,
-		enum usb_device_speed speed, void *buf, u8 type)
-{
-	struct usb_config_descriptor	*c = buf;
-	void				*next = buf + USB_DT_CONFIG_SIZE;
-	int				len = USB_BUFSIZ - USB_DT_CONFIG_SIZE;
-	struct usb_function		*f;
-	int				status;
-
-	/* write the config descriptor */
-	c = buf;
-	c->bLength = USB_DT_CONFIG_SIZE;
-	c->bDescriptorType = type;
-	/* wTotalLength is written later */
-	c->bNumInterfaces = config->next_interface_id;
-	c->bConfigurationValue = config->bConfigurationValue;
-	c->iConfiguration = config->iConfiguration;
-	c->bmAttributes = USB_CONFIG_ATT_ONE | config->bmAttributes;
-	c->bMaxPower = config->bMaxPower ? : (CONFIG_USB_GADGET_VBUS_DRAW / 2);
-
-	/* There may be e.g. OTG descriptors */
-	if (config->descriptors) {
-		status = usb_descriptor_fillbuf(next, len,
-				config->descriptors);
-		if (status < 0)
-			return status;
-		len -= status;
-		next += status;
-	}
-
-	/* add each function's descriptors */
-	list_for_each_entry(f, &config->functions, list) {
-		struct usb_descriptor_header **descriptors;
-
-		switch (speed) {
-		case USB_SPEED_SUPER:
-			descriptors = f->ss_descriptors;
-			break;
-		case USB_SPEED_HIGH:
-			descriptors = f->hs_descriptors;
-			break;
-		default:
-			descriptors = f->descriptors;
-		}
-
-		if (!descriptors)
-			continue;
-		status = usb_descriptor_fillbuf(next, len,
-			(const struct usb_descriptor_header **) descriptors);
-		if (status < 0)
-			return status;
-		len -= status;
-		next += status;
-	}
-
-	len = next - buf;
-	c->wTotalLength = cpu_to_le16(len);
-	return len;
-}
-
-static int config_desc(struct usb_composite_dev *cdev, unsigned w_value)
-{
-	struct usb_gadget		*gadget = cdev->gadget;
-	struct usb_configuration	*c;
-	u8				type = w_value >> 8;
-	enum usb_device_speed		speed = USB_SPEED_UNKNOWN;
-
-	if (gadget->speed == USB_SPEED_SUPER)
-		speed = gadget->speed;
-	else if (gadget_is_dualspeed(gadget)) {
-		int	hs = 0;
-		if (gadget->speed == USB_SPEED_HIGH)
-			hs = 1;
-		if (type == USB_DT_OTHER_SPEED_CONFIG)
-			hs = !hs;
-		if (hs)
-			speed = USB_SPEED_HIGH;
-
-	}
-
-	/* This is a lookup by config *INDEX* */
-	w_value &= 0xff;
-	list_for_each_entry(c, &cdev->configs, list) {
-		/* ignore configs that won't work at this speed */
-		switch (speed) {
-		case USB_SPEED_SUPER:
-			if (!c->superspeed)
-				continue;
-			break;
-		case USB_SPEED_HIGH:
-			if (!c->highspeed)
-				continue;
-			break;
-		default:
-			if (!c->fullspeed)
-				continue;
-		}
-
-		if (w_value == 0)
-			return config_buf(c, speed, cdev->req->buf, type);
-		w_value--;
-	}
-	return -EINVAL;
-}
-
-static int count_configs(struct usb_composite_dev *cdev, unsigned type)
-{
-	struct usb_gadget		*gadget = cdev->gadget;
-	struct usb_configuration	*c;
-	unsigned			count = 0;
-	int				hs = 0;
-	int				ss = 0;
-
-	if (gadget_is_dualspeed(gadget)) {
-		if (gadget->speed == USB_SPEED_HIGH)
-			hs = 1;
-		if (gadget->speed == USB_SPEED_SUPER)
-			ss = 1;
-		if (type == USB_DT_DEVICE_QUALIFIER)
-			hs = !hs;
-	}
-	list_for_each_entry(c, &cdev->configs, list) {
-		/* ignore configs that won't work at this speed */
-		if (ss) {
-			if (!c->superspeed)
-				continue;
-		} else if (hs) {
-			if (!c->highspeed)
-				continue;
-		} else {
-			if (!c->fullspeed)
-				continue;
-		}
-		count++;
-	}
-	return count;
-}
-
-/**
- * bos_desc() - prepares the BOS descriptor.
- * @cdev: pointer to usb_composite device to generate the bos
- *	descriptor for
- *
- * This function generates the BOS (Binary Device Object)
- * descriptor and its device capabilities descriptors. The BOS
- * descriptor should be supported by a SuperSpeed device.
- */
-static int bos_desc(struct usb_composite_dev *cdev)
-{
-	struct usb_ext_cap_descriptor	*usb_ext;
-	struct usb_ss_cap_descriptor	*ss_cap;
-	struct usb_dcd_config_params	dcd_config_params;
-	struct usb_bos_descriptor	*bos = cdev->req->buf;
-
-	bos->bLength = USB_DT_BOS_SIZE;
-	bos->bDescriptorType = USB_DT_BOS;
-
-	bos->wTotalLength = cpu_to_le16(USB_DT_BOS_SIZE);
-	bos->bNumDeviceCaps = 0;
-
-	/*
-	 * A SuperSpeed device shall include the USB2.0 extension descriptor
-	 * and shall support LPM when operating in USB2.0 HS mode.
-	 */
-	usb_ext = cdev->req->buf + le16_to_cpu(bos->wTotalLength);
-	bos->bNumDeviceCaps++;
-	le16_add_cpu(&bos->wTotalLength, USB_DT_USB_EXT_CAP_SIZE);
-	usb_ext->bLength = USB_DT_USB_EXT_CAP_SIZE;
-	usb_ext->bDescriptorType = USB_DT_DEVICE_CAPABILITY;
-	usb_ext->bDevCapabilityType = USB_CAP_TYPE_EXT;
-	usb_ext->bmAttributes = cpu_to_le32(USB_LPM_SUPPORT);
-
-	/*
-	 * The Superspeed USB Capability descriptor shall be implemented by all
-	 * SuperSpeed devices.
-	 */
-	ss_cap = cdev->req->buf + le16_to_cpu(bos->wTotalLength);
-	bos->bNumDeviceCaps++;
-	le16_add_cpu(&bos->wTotalLength, USB_DT_USB_SS_CAP_SIZE);
-	ss_cap->bLength = USB_DT_USB_SS_CAP_SIZE;
-	ss_cap->bDescriptorType = USB_DT_DEVICE_CAPABILITY;
-	ss_cap->bDevCapabilityType = USB_SS_CAP_TYPE;
-	ss_cap->bmAttributes = 0; /* LTM is not supported yet */
-	ss_cap->wSpeedSupported = cpu_to_le16(USB_LOW_SPEED_OPERATION |
-				USB_FULL_SPEED_OPERATION |
-				USB_HIGH_SPEED_OPERATION |
-				USB_5GBPS_OPERATION);
-	ss_cap->bFunctionalitySupport = USB_LOW_SPEED_OPERATION;
-
-	/* Get Controller configuration */
-	if (cdev->gadget->ops->get_config_params)
-		cdev->gadget->ops->get_config_params(&dcd_config_params);
-	else {
-		dcd_config_params.bU1devExitLat = USB_DEFAULT_U1_DEV_EXIT_LAT;
-		dcd_config_params.bU2DevExitLat =
-			cpu_to_le16(USB_DEFAULT_U2_DEV_EXIT_LAT);
-	}
-	ss_cap->bU1devExitLat = dcd_config_params.bU1devExitLat;
-	ss_cap->bU2DevExitLat = dcd_config_params.bU2DevExitLat;
-
-	return le16_to_cpu(bos->wTotalLength);
-}
-
-static void device_qual(struct usb_composite_dev *cdev)
-{
-	struct usb_qualifier_descriptor	*qual = cdev->req->buf;
-
-	qual->bLength = sizeof(*qual);
-	qual->bDescriptorType = USB_DT_DEVICE_QUALIFIER;
-	/* POLICY: same bcdUSB and device type info at both speeds */
-	qual->bcdUSB = cdev->desc.bcdUSB;
-	qual->bDeviceClass = cdev->desc.bDeviceClass;
-	qual->bDeviceSubClass = cdev->desc.bDeviceSubClass;
-	qual->bDeviceProtocol = cdev->desc.bDeviceProtocol;
-	/* ASSUME same EP0 fifo size at both speeds */
-	qual->bMaxPacketSize0 = cdev->gadget->ep0->maxpacket;
-	qual->bNumConfigurations = count_configs(cdev, USB_DT_DEVICE_QUALIFIER);
-	qual->bRESERVED = 0;
-}
-
-/*-------------------------------------------------------------------------*/
-
-static void reset_config(struct usb_composite_dev *cdev)
-{
-	struct usb_function		*f;
-
-	DBG(cdev, "reset config\n");
-
-	list_for_each_entry(f, &cdev->config->functions, list) {
-		if (f->disable)
-			f->disable(f);
-
-		bitmap_zero(f->endpoints, 32);
-	}
-	cdev->config = NULL;
-}
-
-static int set_config(struct usb_composite_dev *cdev,
-		const struct usb_ctrlrequest *ctrl, unsigned number)
-{
-	struct usb_gadget	*gadget = cdev->gadget;
-	struct usb_configuration *c = NULL;
-	int			result = -EINVAL;
-	unsigned		power = gadget_is_otg(gadget) ? 8 : 100;
-	int			tmp;
-
-	if (number) {
-		list_for_each_entry(c, &cdev->configs, list) {
-			if (c->bConfigurationValue == number) {
-				/*
-				 * We disable the FDs of the previous
-				 * configuration only if the new configuration
-				 * is a valid one
-				 */
-				if (cdev->config)
-					reset_config(cdev);
-				result = 0;
-				break;
-			}
-		}
-		if (result < 0)
-			goto done;
-	} else { /* Zero configuration value - need to reset the config */
-		if (cdev->config)
-			reset_config(cdev);
-		result = 0;
-	}
-
-	INFO(cdev, "%s config #%d: %s\n",
-	     usb_speed_string(gadget->speed),
-	     number, c ? c->label : "unconfigured");
-
-	if (!c)
-		goto done;
-
-	cdev->config = c;
-
-	/* Initialize all interfaces by setting them to altsetting zero. */
-	for (tmp = 0; tmp < MAX_CONFIG_INTERFACES; tmp++) {
-		struct usb_function	*f = c->interface[tmp];
-		struct usb_descriptor_header **descriptors;
-
-		if (!f)
-			break;
-
-		/*
-		 * Record which endpoints are used by the function. This is used
-		 * to dispatch control requests targeted at that endpoint to the
-		 * function's setup callback instead of the current
-		 * configuration's setup callback.
-		 */
-		switch (gadget->speed) {
-		case USB_SPEED_SUPER:
-			descriptors = f->ss_descriptors;
-			break;
-		case USB_SPEED_HIGH:
-			descriptors = f->hs_descriptors;
-			break;
-		default:
-			descriptors = f->descriptors;
-		}
-
-		for (; *descriptors; ++descriptors) {
-			struct usb_endpoint_descriptor *ep;
-			int addr;
-
-			if ((*descriptors)->bDescriptorType != USB_DT_ENDPOINT)
-				continue;
-
-			ep = (struct usb_endpoint_descriptor *)*descriptors;
-			addr = ((ep->bEndpointAddress & 0x80) >> 3)
-			     |  (ep->bEndpointAddress & 0x0f);
-			set_bit(addr, f->endpoints);
-		}
-
-		result = f->set_alt(f, tmp, 0);
-		if (result < 0) {
-			DBG(cdev, "interface %d (%s/%p) alt 0 --> %d\n",
-					tmp, f->name, f, result);
-
-			reset_config(cdev);
-			goto done;
-		}
-
-		if (result == USB_GADGET_DELAYED_STATUS) {
-			DBG(cdev,
-			 "%s: interface %d (%s) requested delayed status\n",
-					__func__, tmp, f->name);
-			cdev->delayed_status++;
-			DBG(cdev, "delayed_status count %d\n",
-					cdev->delayed_status);
-		}
-	}
-
-	/* when we return, be sure our power usage is valid */
-	power = c->bMaxPower ? (2 * c->bMaxPower) : CONFIG_USB_GADGET_VBUS_DRAW;
-done:
-	usb_gadget_vbus_draw(gadget, power);
-	if (result >= 0 && cdev->delayed_status)
-		result = USB_GADGET_DELAYED_STATUS;
-	return result;
-}
-
-/**
- * usb_add_config() - add a configuration to a device.
- * @cdev: wraps the USB gadget
- * @config: the configuration, with bConfigurationValue assigned
- * @bind: the configuration's bind function
- * Context: single threaded during gadget setup
- *
- * One of the main tasks of a composite @bind() routine is to
- * add each of the configurations it supports, using this routine.
- *
- * This function returns the value of the configuration's @bind(), which
- * is zero for success else a negative errno value.  Binding configurations
- * assigns global resources including string IDs, and per-configuration
- * resources such as interface IDs and endpoints.
- */
-int usb_add_config(struct usb_composite_dev *cdev,
-		struct usb_configuration *config,
-		int (*bind)(struct usb_configuration *))
-{
-	int				status = -EINVAL;
-	struct usb_configuration	*c;
-
-	DBG(cdev, "adding config #%u '%s'/%p\n",
-			config->bConfigurationValue,
-			config->label, config);
-
-	if (!config->bConfigurationValue || !bind)
-		goto done;
-
-	/* Prevent duplicate configuration identifiers */
-	list_for_each_entry(c, &cdev->configs, list) {
-		if (c->bConfigurationValue == config->bConfigurationValue) {
-			status = -EBUSY;
-			goto done;
-		}
-	}
-
-	config->cdev = cdev;
-	list_add_tail(&config->list, &cdev->configs);
-
-	INIT_LIST_HEAD(&config->functions);
-	config->next_interface_id = 0;
-	memset(config->interface, 0, sizeof(config->interface));
-
-	status = bind(config);
-	if (status < 0) {
-		while (!list_empty(&config->functions)) {
-			struct usb_function		*f;
-
-			f = list_first_entry(&config->functions,
-					struct usb_function, list);
-			list_del(&f->list);
-			if (f->unbind) {
-				DBG(cdev, "unbind function '%s'/%p\n",
-					f->name, f);
-				f->unbind(config, f);
-				/* may free memory for "f" */
-			}
-		}
-		list_del(&config->list);
-		config->cdev = NULL;
-	} else {
-		unsigned	i;
-
-		DBG(cdev, "cfg %d/%p speeds:%s%s%s\n",
-			config->bConfigurationValue, config,
-			config->superspeed ? " super" : "",
-			config->highspeed ? " high" : "",
-			config->fullspeed
-				? (gadget_is_dualspeed(cdev->gadget)
-					? " full"
-					: " full/low")
-				: "");
-
-		for (i = 0; i < MAX_CONFIG_INTERFACES; i++) {
-			struct usb_function	*f = config->interface[i];
-
-			if (!f)
-				continue;
-			DBG(cdev, "  interface %d = %s/%p\n",
-				i, f->name, f);
-		}
-	}
-
-	/* set_alt(), or next bind(), sets up
-	 * ep->driver_data as needed.
-	 */
-	usb_ep_autoconfig_reset(cdev->gadget);
-
-done:
-	if (status)
-		DBG(cdev, "added config '%s'/%u --> %d\n", config->label,
-				config->bConfigurationValue, status);
-	return status;
-}
-
-static void remove_config(struct usb_composite_dev *cdev,
-			      struct usb_configuration *config)
-{
-	while (!list_empty(&config->functions)) {
-		struct usb_function		*f;
-
-		f = list_first_entry(&config->functions,
-				struct usb_function, list);
-		list_del(&f->list);
-		if (f->unbind) {
-			DBG(cdev, "unbind function '%s'/%p\n", f->name, f);
-			f->unbind(config, f);
-			/* may free memory for "f" */
-		}
-	}
-	list_del(&config->list);
-	if (config->unbind) {
-		DBG(cdev, "unbind config '%s'/%p\n", config->label, config);
-		config->unbind(config);
-			/* may free memory for "c" */
-	}
-}
-
-/**
- * usb_remove_config() - remove a configuration from a device.
- * @cdev: wraps the USB gadget
- * @config: the configuration
- *
- * Drivers must call usb_gadget_disconnect before calling this function
- * to disconnect the device from the host and make sure the host will not
- * try to enumerate the device while we are changing the config list.
- */
-void usb_remove_config(struct usb_composite_dev *cdev,
-		      struct usb_configuration *config)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&cdev->lock, flags);
-
-	if (cdev->config == config)
-		reset_config(cdev);
-
-	spin_unlock_irqrestore(&cdev->lock, flags);
-
-	remove_config(cdev, config);
-}
-
-/*-------------------------------------------------------------------------*/
-
-/* We support strings in multiple languages ... string descriptor zero
- * says which languages are supported.  The typical case will be that
- * only one language (probably English) is used, with I18N handled on
- * the host side.
- */
-
-static void collect_langs(struct usb_gadget_strings **sp, __le16 *buf)
-{
-	const struct usb_gadget_strings	*s;
-	__le16				language;
-	__le16				*tmp;
-
-	while (*sp) {
-		s = *sp;
-		language = cpu_to_le16(s->language);
-		for (tmp = buf; *tmp && tmp < &buf[126]; tmp++) {
-			if (*tmp == language)
-				goto repeat;
-		}
-		*tmp++ = language;
-repeat:
-		sp++;
-	}
-}
-
-static int lookup_string(
-	struct usb_gadget_strings	**sp,
-	void				*buf,
-	u16				language,
-	int				id
-)
-{
-	struct usb_gadget_strings	*s;
-	int				value;
-
-	while (*sp) {
-		s = *sp++;
-		if (s->language != language)
-			continue;
-		value = usb_gadget_get_string(s, id, buf);
-		if (value > 0)
-			return value;
-	}
-	return -EINVAL;
-}
-
-static int get_string(struct usb_composite_dev *cdev,
-		void *buf, u16 language, int id)
-{
-	struct usb_configuration	*c;
-	struct usb_function		*f;
-	int				len;
-	const char			*str;
-
-	/* Yes, not only is USB's I18N support probably more than most
-	 * folk will ever care about ... also, it's all supported here.
-	 * (Except for UTF8 support for Unicode's "Astral Planes".)
-	 */
-
-	/* 0 == report all available language codes */
-	if (id == 0) {
-		struct usb_string_descriptor	*s = buf;
-		struct usb_gadget_strings	**sp;
-
-		memset(s, 0, 256);
-		s->bDescriptorType = USB_DT_STRING;
-
-		sp = composite->strings;
-		if (sp)
-			collect_langs(sp, s->wData);
-
-		list_for_each_entry(c, &cdev->configs, list) {
-			sp = c->strings;
-			if (sp)
-				collect_langs(sp, s->wData);
-
-			list_for_each_entry(f, &c->functions, list) {
-				sp = f->strings;
-				if (sp)
-					collect_langs(sp, s->wData);
-			}
-		}
-
-		for (len = 0; len <= 126 && s->wData[len]; len++)
-			continue;
-		if (!len)
-			return -EINVAL;
-
-		s->bLength = 2 * (len + 1);
-		return s->bLength;
-	}
-
-	/* Otherwise, look up and return a specified string.  First
-	 * check if the string has not been overridden.
-	 */
-	if (cdev->manufacturer_override == id)
-		str = iManufacturer ?: composite->iManufacturer ?:
-			composite_manufacturer;
-	else if (cdev->product_override == id)
-		str = iProduct ?: composite->iProduct;
-	else if (cdev->serial_override == id)
-		str = iSerialNumber ?: composite->iSerialNumber;
-	else
-		str = NULL;
-	if (str) {
-		struct usb_gadget_strings strings = {
-			.language = language,
-			.strings  = &(struct usb_string) { 0xff, str }
-		};
-		return usb_gadget_get_string(&strings, 0xff, buf);
-	}
-
-	/* String IDs are device-scoped, so we look up each string
-	 * table we're told about.  These lookups are infrequent;
-	 * simpler-is-better here.
-	 */
-	if (composite->strings) {
-		len = lookup_string(composite->strings, buf, language, id);
-		if (len > 0)
-			return len;
-	}
-	list_for_each_entry(c, &cdev->configs, list) {
-		if (c->strings) {
-			len = lookup_string(c->strings, buf, language, id);
-			if (len > 0)
-				return len;
-		}
-		list_for_each_entry(f, &c->functions, list) {
-			if (!f->strings)
-				continue;
-			len = lookup_string(f->strings, buf, language, id);
-			if (len > 0)
-				return len;
-		}
-	}
-	return -EINVAL;
-}
-
-/**
- * usb_string_id() - allocate an unused string ID
- * @cdev: the device whose string descriptor IDs are being allocated
- * Context: single threaded during gadget setup
- *
- * @usb_string_id() is called from bind() callbacks to allocate
- * string IDs.  Drivers for functions, configurations, or gadgets will
- * then store that ID in the appropriate descriptors and string table.
- *
- * All string identifier should be allocated using this,
- * @usb_string_ids_tab() or @usb_string_ids_n() routine, to ensure
- * that for example different functions don't wrongly assign different
- * meanings to the same identifier.
- */
-int usb_string_id(struct usb_composite_dev *cdev)
-{
-	if (cdev->next_string_id < 254) {
-		/* string id 0 is reserved by USB spec for list of
-		 * supported languages */
-		/* 255 reserved as well? -- mina86 */
-		cdev->next_string_id++;
-		return cdev->next_string_id;
-	}
-	return -ENODEV;
-}
-
-/**
- * usb_string_ids() - allocate unused string IDs in batch
- * @cdev: the device whose string descriptor IDs are being allocated
- * @str: an array of usb_string objects to assign numbers to
- * Context: single threaded during gadget setup
- *
- * @usb_string_ids() is called from bind() callbacks to allocate
- * string IDs.  Drivers for functions, configurations, or gadgets will
- * then copy IDs from the string table to the appropriate descriptors
- * and string table for other languages.
- *
- * All string identifier should be allocated using this,
- * @usb_string_id() or @usb_string_ids_n() routine, to ensure that for
- * example different functions don't wrongly assign different meanings
- * to the same identifier.
- */
-int usb_string_ids_tab(struct usb_composite_dev *cdev, struct usb_string *str)
-{
-	int next = cdev->next_string_id;
-
-	for (; str->s; ++str) {
-		if (unlikely(next >= 254))
-			return -ENODEV;
-		str->id = ++next;
-	}
-
-	cdev->next_string_id = next;
-
-	return 0;
-}
-
-/**
- * usb_string_ids_n() - allocate unused string IDs in batch
- * @c: the device whose string descriptor IDs are being allocated
- * @n: number of string IDs to allocate
- * Context: single threaded during gadget setup
- *
- * Returns the first requested ID.  This ID and next @n-1 IDs are now
- * valid IDs.  At least provided that @n is non-zero because if it
- * is, returns last requested ID which is now very useful information.
- *
- * @usb_string_ids_n() is called from bind() callbacks to allocate
- * string IDs.  Drivers for functions, configurations, or gadgets will
- * then store that ID in the appropriate descriptors and string table.
- *
- * All string identifier should be allocated using this,
- * @usb_string_id() or @usb_string_ids_n() routine, to ensure that for
- * example different functions don't wrongly assign different meanings
- * to the same identifier.
- */
-int usb_string_ids_n(struct usb_composite_dev *c, unsigned n)
-{
-	unsigned next = c->next_string_id;
-	if (unlikely(n > 254 || (unsigned)next + n > 254))
-		return -ENODEV;
-	c->next_string_id += n;
-	return next + 1;
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-static void composite_setup_complete(struct usb_ep *ep, struct usb_request *req)
-{
-	if (req->status || req->actual != req->length)
-		DBG((struct usb_composite_dev *) ep->driver_data,
-				"setup complete --> %d, %d/%d\n",
-				req->status, req->actual, req->length);
-}
-
-/*
- * The setup() callback implements all the ep0 functionality that's
- * not handled lower down, in hardware or the hardware driver(like
- * device and endpoint feature flags, and their status).  It's all
- * housekeeping for the gadget function we're implementing.  Most of
- * the work is in config and function specific setup.
- */
-static int
-composite_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
-{
-	struct usb_composite_dev	*cdev = get_gadget_data(gadget);
-	struct usb_request		*req = cdev->req;
-	int				value = -EOPNOTSUPP;
-	int				status = 0;
-	u16				w_index = le16_to_cpu(ctrl->wIndex);
-	u8				intf = w_index & 0xFF;
-	u16				w_value = le16_to_cpu(ctrl->wValue);
-	u16				w_length = le16_to_cpu(ctrl->wLength);
-	struct usb_function		*f = NULL;
-	u8				endp;
-
-	/* partial re-init of the response message; the function or the
-	 * gadget might need to intercept e.g. a control-OUT completion
-	 * when we delegate to it.
-	 */
-	req->zero = 0;
-	req->complete = composite_setup_complete;
-	req->length = 0;
-	gadget->ep0->driver_data = cdev;
-
-	switch (ctrl->bRequest) {
-
-	/* we handle all standard USB descriptors */
-	case USB_REQ_GET_DESCRIPTOR:
-		if (ctrl->bRequestType != USB_DIR_IN)
-			goto unknown;
-		switch (w_value >> 8) {
-
-		case USB_DT_DEVICE:
-			cdev->desc.bNumConfigurations =
-				count_configs(cdev, USB_DT_DEVICE);
-			cdev->desc.bMaxPacketSize0 =
-				cdev->gadget->ep0->maxpacket;
-			if (gadget_is_superspeed(gadget)) {
-				if (gadget->speed >= USB_SPEED_SUPER) {
-					cdev->desc.bcdUSB = cpu_to_le16(0x0300);
-					cdev->desc.bMaxPacketSize0 = 9;
-				} else {
-					cdev->desc.bcdUSB = cpu_to_le16(0x0210);
-				}
-			}
-
-			value = min(w_length, (u16) sizeof cdev->desc);
-			memcpy(req->buf, &cdev->desc, value);
-			break;
-		case USB_DT_DEVICE_QUALIFIER:
-			if (!gadget_is_dualspeed(gadget) ||
-			    gadget->speed >= USB_SPEED_SUPER)
-				break;
-			device_qual(cdev);
-			value = min_t(int, w_length,
-				sizeof(struct usb_qualifier_descriptor));
-			break;
-		case USB_DT_OTHER_SPEED_CONFIG:
-			if (!gadget_is_dualspeed(gadget) ||
-			    gadget->speed >= USB_SPEED_SUPER)
-				break;
-			/* FALLTHROUGH */
-		case USB_DT_CONFIG:
-			value = config_desc(cdev, w_value);
-			if (value >= 0)
-				value = min(w_length, (u16) value);
-			break;
-		case USB_DT_STRING:
-			value = get_string(cdev, req->buf,
-					w_index, w_value & 0xff);
-			if (value >= 0)
-				value = min(w_length, (u16) value);
-			break;
-		case USB_DT_BOS:
-			if (gadget_is_superspeed(gadget)) {
-				value = bos_desc(cdev);
-				value = min(w_length, (u16) value);
-			}
-			break;
-		}
-		break;
-
-	/* any number of configs can work */
-	case USB_REQ_SET_CONFIGURATION:
-		if (ctrl->bRequestType != 0)
-			goto unknown;
-		if (gadget_is_otg(gadget)) {
-			if (gadget->a_hnp_support)
-				DBG(cdev, "HNP available\n");
-			else if (gadget->a_alt_hnp_support)
-				DBG(cdev, "HNP on another port\n");
-			else
-				VDBG(cdev, "HNP inactive\n");
-		}
-		spin_lock(&cdev->lock);
-		value = set_config(cdev, ctrl, w_value);
-		spin_unlock(&cdev->lock);
-		break;
-	case USB_REQ_GET_CONFIGURATION:
-		if (ctrl->bRequestType != USB_DIR_IN)
-			goto unknown;
-		if (cdev->config)
-			*(u8 *)req->buf = cdev->config->bConfigurationValue;
-		else
-			*(u8 *)req->buf = 0;
-		value = min(w_length, (u16) 1);
-		break;
-
-	/* function drivers must handle get/set altsetting; if there's
-	 * no get() method, we know only altsetting zero works.
-	 */
-	case USB_REQ_SET_INTERFACE:
-		if (ctrl->bRequestType != USB_RECIP_INTERFACE)
-			goto unknown;
-		if (!cdev->config || intf >= MAX_CONFIG_INTERFACES)
-			break;
-		f = cdev->config->interface[intf];
-		if (!f)
-			break;
-		if (w_value && !f->set_alt)
-			break;
-		value = f->set_alt(f, w_index, w_value);
-		if (value == USB_GADGET_DELAYED_STATUS) {
-			DBG(cdev,
-			 "%s: interface %d (%s) requested delayed status\n",
-					__func__, intf, f->name);
-			cdev->delayed_status++;
-			DBG(cdev, "delayed_status count %d\n",
-					cdev->delayed_status);
-		}
-		break;
-	case USB_REQ_GET_INTERFACE:
-		if (ctrl->bRequestType != (USB_DIR_IN|USB_RECIP_INTERFACE))
-			goto unknown;
-		if (!cdev->config || intf >= MAX_CONFIG_INTERFACES)
-			break;
-		f = cdev->config->interface[intf];
-		if (!f)
-			break;
-		/* lots of interfaces only need altsetting zero... */
-		value = f->get_alt ? f->get_alt(f, w_index) : 0;
-		if (value < 0)
-			break;
-		*((u8 *)req->buf) = value;
-		value = min(w_length, (u16) 1);
-		break;
-
-	/*
-	 * USB 3.0 additions:
-	 * Function driver should handle get_status request. If such cb
-	 * wasn't supplied we respond with default value = 0
-	 * Note: function driver should supply such cb only for the first
-	 * interface of the function
-	 */
-	case USB_REQ_GET_STATUS:
-		if (!gadget_is_superspeed(gadget))
-			goto unknown;
-		if (ctrl->bRequestType != (USB_DIR_IN | USB_RECIP_INTERFACE))
-			goto unknown;
-		value = 2;	/* This is the length of the get_status reply */
-		put_unaligned_le16(0, req->buf);
-		if (!cdev->config || intf >= MAX_CONFIG_INTERFACES)
-			break;
-		f = cdev->config->interface[intf];
-		if (!f)
-			break;
-		status = f->get_status ? f->get_status(f) : 0;
-		if (status < 0)
-			break;
-		put_unaligned_le16(status & 0x0000ffff, req->buf);
-		break;
-	/*
-	 * Function drivers should handle SetFeature/ClearFeature
-	 * (FUNCTION_SUSPEND) request. function_suspend cb should be supplied
-	 * only for the first interface of the function
-	 */
-	case USB_REQ_CLEAR_FEATURE:
-	case USB_REQ_SET_FEATURE:
-		if (!gadget_is_superspeed(gadget))
-			goto unknown;
-		if (ctrl->bRequestType != (USB_DIR_OUT | USB_RECIP_INTERFACE))
-			goto unknown;
-		switch (w_value) {
-		case USB_INTRF_FUNC_SUSPEND:
-			if (!cdev->config || intf >= MAX_CONFIG_INTERFACES)
-				break;
-			f = cdev->config->interface[intf];
-			if (!f)
-				break;
-			value = 0;
-			if (f->func_suspend)
-				value = f->func_suspend(f, w_index >> 8);
-			if (value < 0) {
-				ERROR(cdev,
-				      "func_suspend() returned error %d\n",
-				      value);
-				value = 0;
-			}
-			break;
-		}
-		break;
-	default:
-unknown:
-		VDBG(cdev,
-			"non-core control req%02x.%02x v%04x i%04x l%d\n",
-			ctrl->bRequestType, ctrl->bRequest,
-			w_value, w_index, w_length);
-
-		/* functions always handle their interfaces and endpoints...
-		 * punt other recipients (other, WUSB, ...) to the current
-		 * configuration code.
-		 *
-		 * REVISIT it could make sense to let the composite device
-		 * take such requests too, if that's ever needed:  to work
-		 * in config 0, etc.
-		 */
-		switch (ctrl->bRequestType & USB_RECIP_MASK) {
-		case USB_RECIP_INTERFACE:
-			if (!cdev->config || intf >= MAX_CONFIG_INTERFACES)
-				break;
-			f = cdev->config->interface[intf];
-			break;
-
-		case USB_RECIP_ENDPOINT:
-			endp = ((w_index & 0x80) >> 3) | (w_index & 0x0f);
-			list_for_each_entry(f, &cdev->config->functions, list) {
-				if (test_bit(endp, f->endpoints))
-					break;
-			}
-			if (&f->list == &cdev->config->functions)
-				f = NULL;
-			break;
-		}
-
-		if (f && f->setup)
-			value = f->setup(f, ctrl);
-		else {
-			struct usb_configuration	*c;
-
-			c = cdev->config;
-			if (c && c->setup)
-				value = c->setup(c, ctrl);
-		}
-
-		goto done;
-	}
-
-	/* respond with data transfer before status phase? */
-	if (value >= 0 && value != USB_GADGET_DELAYED_STATUS) {
-		req->length = value;
-		req->zero = value < w_length;
-		value = usb_ep_queue(gadget->ep0, req, GFP_ATOMIC);
-		if (value < 0) {
-			DBG(cdev, "ep_queue --> %d\n", value);
-			req->status = 0;
-			composite_setup_complete(gadget->ep0, req);
-		}
-	} else if (value == USB_GADGET_DELAYED_STATUS && w_length != 0) {
-		WARN(cdev,
-			"%s: Delayed status not supported for w_length != 0",
-			__func__);
-	}
-
-done:
-	/* device either stalls (value < 0) or reports success */
-	return value;
-}
-
-static void composite_disconnect(struct usb_gadget *gadget)
-{
-	struct usb_composite_dev	*cdev = get_gadget_data(gadget);
-	unsigned long			flags;
-
-	/* REVISIT:  should we have config and device level
-	 * disconnect callbacks?
-	 */
-	spin_lock_irqsave(&cdev->lock, flags);
-	if (cdev->config)
-		reset_config(cdev);
-	if (composite->disconnect)
-		composite->disconnect(cdev);
-	spin_unlock_irqrestore(&cdev->lock, flags);
-}
-
-/*-------------------------------------------------------------------------*/
-
-static ssize_t composite_show_suspended(struct device *dev,
-					struct device_attribute *attr,
-					char *buf)
-{
-	struct usb_gadget *gadget = dev_to_usb_gadget(dev);
-	struct usb_composite_dev *cdev = get_gadget_data(gadget);
-
-	return sprintf(buf, "%d\n", cdev->suspended);
-}
-
-static DEVICE_ATTR(suspended, 0444, composite_show_suspended, NULL);
-
-static void
-composite_unbind(struct usb_gadget *gadget)
-{
-	struct usb_composite_dev	*cdev = get_gadget_data(gadget);
-
-	/* composite_disconnect() must already have been called
-	 * by the underlying peripheral controller driver!
-	 * so there's no i/o concurrency that could affect the
-	 * state protected by cdev->lock.
-	 */
-	WARN_ON(cdev->config);
-
-	while (!list_empty(&cdev->configs)) {
-		struct usb_configuration	*c;
-		c = list_first_entry(&cdev->configs,
-				struct usb_configuration, list);
-		remove_config(cdev, c);
-	}
-	if (composite->unbind)
-		composite->unbind(cdev);
-
-	if (cdev->req) {
-		kfree(cdev->req->buf);
-		usb_ep_free_request(gadget->ep0, cdev->req);
-	}
-	device_remove_file(&gadget->dev, &dev_attr_suspended);
-	kfree(cdev);
-	set_gadget_data(gadget, NULL);
-	composite = NULL;
-}
-
-static u8 override_id(struct usb_composite_dev *cdev, u8 *desc)
-{
-	if (!*desc) {
-		int ret = usb_string_id(cdev);
-		if (unlikely(ret < 0))
-			WARNING(cdev, "failed to override string ID\n");
-		else
-			*desc = ret;
-	}
-
-	return *desc;
-}
-
-static int composite_bind(struct usb_gadget *gadget,
-		struct usb_gadget_driver *driver)
-{
-	struct usb_composite_dev	*cdev;
-	int				status = -ENOMEM;
-
-	cdev = kzalloc(sizeof *cdev, GFP_KERNEL);
-	if (!cdev)
-		return status;
-
-	spin_lock_init(&cdev->lock);
-	cdev->gadget = gadget;
-	set_gadget_data(gadget, cdev);
-	INIT_LIST_HEAD(&cdev->configs);
-
-	/* preallocate control response and buffer */
-	cdev->req = usb_ep_alloc_request(gadget->ep0, GFP_KERNEL);
-	if (!cdev->req)
-		goto fail;
-	cdev->req->buf = kmalloc(USB_BUFSIZ, GFP_KERNEL);
-	if (!cdev->req->buf)
-		goto fail;
-	cdev->req->complete = composite_setup_complete;
-	gadget->ep0->driver_data = cdev;
-
-	cdev->bufsiz = USB_BUFSIZ;
-	cdev->driver = composite;
-
-	/*
-	 * As per USB compliance update, a device that is actively drawing
-	 * more than 100mA from USB must report itself as bus-powered in
-	 * the GetStatus(DEVICE) call.
-	 */
-	if (CONFIG_USB_GADGET_VBUS_DRAW <= USB_SELF_POWER_VBUS_MAX_DRAW)
-		usb_gadget_set_selfpowered(gadget);
-
-	/* interface and string IDs start at zero via kzalloc.
-	 * we force endpoints to start unassigned; few controller
-	 * drivers will zero ep->driver_data.
-	 */
-	usb_ep_autoconfig_reset(cdev->gadget);
-
-	/* composite gadget needs to assign strings for whole device (like
-	 * serial number), register function drivers, potentially update
-	 * power state and consumption, etc
-	 */
-	status = composite->bind(cdev);
-	if (status < 0)
-		goto fail;
-
-	cdev->desc = *composite->dev;
-
-	/* standardized runtime overrides for device ID data */
-	if (idVendor)
-		cdev->desc.idVendor = cpu_to_le16(idVendor);
-	else
-		idVendor = le16_to_cpu(cdev->desc.idVendor);
-	if (idProduct)
-		cdev->desc.idProduct = cpu_to_le16(idProduct);
-	else
-		idProduct = le16_to_cpu(cdev->desc.idProduct);
-	if (bcdDevice)
-		cdev->desc.bcdDevice = cpu_to_le16(bcdDevice);
-	else
-		bcdDevice = le16_to_cpu(cdev->desc.bcdDevice);
-
-	/* string overrides */
-	if (iManufacturer || !cdev->desc.iManufacturer) {
-		if (!iManufacturer && !composite->iManufacturer &&
-		    !*composite_manufacturer)
-			snprintf(composite_manufacturer,
-				 sizeof composite_manufacturer,
-				 "%s %s with %s",
-				 init_utsname()->sysname,
-				 init_utsname()->release,
-				 gadget->name);
-
-		cdev->manufacturer_override =
-			override_id(cdev, &cdev->desc.iManufacturer);
-	}
-
-	if (iProduct || (!cdev->desc.iProduct && composite->iProduct))
-		cdev->product_override =
-			override_id(cdev, &cdev->desc.iProduct);
-
-	if (iSerialNumber ||
-	    (!cdev->desc.iSerialNumber && composite->iSerialNumber))
-		cdev->serial_override =
-			override_id(cdev, &cdev->desc.iSerialNumber);
-
-	/* has userspace failed to provide a serial number? */
-	if (composite->needs_serial && !cdev->desc.iSerialNumber)
-		WARNING(cdev, "userspace failed to provide iSerialNumber\n");
-
-	/* finish up */
-	status = device_create_file(&gadget->dev, &dev_attr_suspended);
-	if (status)
-		goto fail;
-
-	INFO(cdev, "%s ready\n", composite->name);
-	return 0;
-
-fail:
-	composite_unbind(gadget);
-	return status;
-}
-
-/*-------------------------------------------------------------------------*/
-
-static void
-composite_suspend(struct usb_gadget *gadget)
-{
-	struct usb_composite_dev	*cdev = get_gadget_data(gadget);
-	struct usb_function		*f;
-
-	/* REVISIT:  should we have config level
-	 * suspend/resume callbacks?
-	 */
-	DBG(cdev, "suspend\n");
-	if (cdev->config) {
-		list_for_each_entry(f, &cdev->config->functions, list) {
-			if (f->suspend)
-				f->suspend(f);
-		}
-	}
-	if (composite->suspend)
-		composite->suspend(cdev);
-
-	cdev->suspended = 1;
-
-	usb_gadget_vbus_draw(gadget, 2);
-}
-
-static void
-composite_resume(struct usb_gadget *gadget)
-{
-	struct usb_composite_dev	*cdev = get_gadget_data(gadget);
-	struct usb_function		*f;
-	u8				maxpower;
-
-	/* REVISIT:  should we have config level
-	 * suspend/resume callbacks?
-	 */
-	DBG(cdev, "resume\n");
-	if (composite->resume)
-		composite->resume(cdev);
-	if (cdev->config) {
-		list_for_each_entry(f, &cdev->config->functions, list) {
-			if (f->resume)
-				f->resume(f);
-		}
-
-		maxpower = cdev->config->bMaxPower;
-
-		usb_gadget_vbus_draw(gadget, maxpower ?
-			(2 * maxpower) : CONFIG_USB_GADGET_VBUS_DRAW);
-	}
-
-	cdev->suspended = 0;
-}
-
-/*-------------------------------------------------------------------------*/
-
-static struct usb_gadget_driver composite_driver = {
-	.bind		= composite_bind,
-	.unbind		= composite_unbind,
-
-	.setup		= composite_setup,
-	.disconnect	= composite_disconnect,
-
-	.suspend	= composite_suspend,
-	.resume		= composite_resume,
-
-	.driver	= {
-		.owner		= THIS_MODULE,
-	},
-};
-
-/**
- * usb_composite_probe() - register a composite driver
- * @driver: the driver to register
- * @bind: the callback used to allocate resources that are shared across the
- *	whole device, such as string IDs, and add its configurations using
- *	@usb_add_config().  This may fail by returning a negative errno
- *	value; it should return zero on successful initialization.
- * Context: single threaded during gadget setup
- *
- * This function is used to register drivers using the composite driver
- * framework.  The return value is zero, or a negative errno value.
- * Those values normally come from the driver's @bind method, which does
- * all the work of setting up the driver to match the hardware.
- *
- * On successful return, the gadget is ready to respond to requests from
- * the host, unless one of its components invokes usb_gadget_disconnect()
- * while it was binding.  That would usually be done in order to wait for
- * some userspace participation.
- */
-int usb_composite_probe(struct usb_composite_driver *driver)
-{
-	if (!driver || !driver->dev || composite || !driver->bind)
-		return -EINVAL;
-
-	if (!driver->name)
-		driver->name = "composite";
-	if (!driver->iProduct)
-		driver->iProduct = driver->name;
-	composite_driver.function =  (char *) driver->name;
-	composite_driver.driver.name = driver->name;
-	composite_driver.max_speed = driver->max_speed;
-	composite = driver;
-
-	return usb_gadget_probe_driver(&composite_driver);
-}
-
-/**
- * usb_composite_unregister() - unregister a composite driver
- * @driver: the driver to unregister
- *
- * This function is used to unregister drivers using the composite
- * driver framework.
- */
-void usb_composite_unregister(struct usb_composite_driver *driver)
-{
-	if (composite != driver)
-		return;
-	usb_gadget_unregister_driver(&composite_driver);
-}
-
-/**
- * usb_composite_setup_continue() - Continue with the control transfer
- * @cdev: the composite device who's control transfer was kept waiting
- *
- * This function must be called by the USB function driver to continue
- * with the control transfer's data/status stage in case it had requested to
- * delay the data/status stages. A USB function's setup handler (e.g. set_alt())
- * can request the composite framework to delay the setup request's data/status
- * stages by returning USB_GADGET_DELAYED_STATUS.
- */
-void usb_composite_setup_continue(struct usb_composite_dev *cdev)
-{
-	int			value;
-	struct usb_request	*req = cdev->req;
-	unsigned long		flags;
-
-	DBG(cdev, "%s\n", __func__);
-	spin_lock_irqsave(&cdev->lock, flags);
-
-	if (cdev->delayed_status == 0) {
-		WARN(cdev, "%s: Unexpected call\n", __func__);
-
-	} else if (--cdev->delayed_status == 0) {
-		DBG(cdev, "%s: Completing delayed status\n", __func__);
-		req->length = 0;
-		value = usb_ep_queue(cdev->gadget->ep0, req, GFP_ATOMIC);
-		if (value < 0) {
-			DBG(cdev, "ep_queue --> %d\n", value);
-			req->status = 0;
-			composite_setup_complete(cdev->gadget->ep0, req);
-		}
-	}
-
-	spin_unlock_irqrestore(&cdev->lock, flags);
-}
-
diff --git a/drivers/staging/ccg/composite.h b/drivers/staging/ccg/composite.h
deleted file mode 100644
index 19a5adf18bf..00000000000
--- a/drivers/staging/ccg/composite.h
+++ /dev/null
@@ -1,395 +0,0 @@
-/*
- * composite.h -- framework for usb gadgets which are composite devices
- *
- * Copyright (C) 2006-2008 David Brownell
- *
- * 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
- */
-
-#ifndef	__LINUX_USB_COMPOSITE_H
-#define	__LINUX_USB_COMPOSITE_H
-
-/*
- * This framework is an optional layer on top of the USB Gadget interface,
- * making it easier to build (a) Composite devices, supporting multiple
- * functions within any single configuration, and (b) Multi-configuration
- * devices, also supporting multiple functions but without necessarily
- * having more than one function per configuration.
- *
- * Example:  a device with a single configuration supporting both network
- * link and mass storage functions is a composite device.  Those functions
- * might alternatively be packaged in individual configurations, but in
- * the composite model the host can use both functions at the same time.
- */
-
-#include <linux/usb/ch9.h>
-#include <linux/usb/gadget.h>
-
-/*
- * USB function drivers should return USB_GADGET_DELAYED_STATUS if they
- * wish to delay the data/status stages of the control transfer till they
- * are ready. The control transfer will then be kept from completing till
- * all the function drivers that requested for USB_GADGET_DELAYED_STAUS
- * invoke usb_composite_setup_continue().
- */
-#define USB_GADGET_DELAYED_STATUS       0x7fff	/* Impossibly large value */
-
-struct usb_configuration;
-
-/**
- * struct usb_function - describes one function of a configuration
- * @name: For diagnostics, identifies the function.
- * @strings: tables of strings, keyed by identifiers assigned during bind()
- *	and by language IDs provided in control requests
- * @descriptors: Table of full (or low) speed descriptors, using interface and
- *	string identifiers assigned during @bind().  If this pointer is null,
- *	the function will not be available at full speed (or at low speed).
- * @hs_descriptors: Table of high speed descriptors, using interface and
- *	string identifiers assigned during @bind().  If this pointer is null,
- *	the function will not be available at high speed.
- * @ss_descriptors: Table of super speed descriptors, using interface and
- *	string identifiers assigned during @bind(). If this
- *	pointer is null after initiation, the function will not
- *	be available at super speed.
- * @config: assigned when @usb_add_function() is called; this is the
- *	configuration with which this function is associated.
- * @bind: Before the gadget can register, all of its functions bind() to the
- *	available resources including string and interface identifiers used
- *	in interface or class descriptors; endpoints; I/O buffers; and so on.
- * @unbind: Reverses @bind; called as a side effect of unregistering the
- *	driver which added this function.
- * @set_alt: (REQUIRED) Reconfigures altsettings; function drivers may
- *	initialize usb_ep.driver data at this time (when it is used).
- *	Note that setting an interface to its current altsetting resets
- *	interface state, and that all interfaces have a disabled state.
- * @get_alt: Returns the active altsetting.  If this is not provided,
- *	then only altsetting zero is supported.
- * @disable: (REQUIRED) Indicates the function should be disabled.  Reasons
- *	include host resetting or reconfiguring the gadget, and disconnection.
- * @setup: Used for interface-specific control requests.
- * @suspend: Notifies functions when the host stops sending USB traffic.
- * @resume: Notifies functions when the host restarts USB traffic.
- * @get_status: Returns function status as a reply to
- *	GetStatus() request when the recepient is Interface.
- * @func_suspend: callback to be called when
- *	SetFeature(FUNCTION_SUSPEND) is reseived
- *
- * A single USB function uses one or more interfaces, and should in most
- * cases support operation at both full and high speeds.  Each function is
- * associated by @usb_add_function() with a one configuration; that function
- * causes @bind() to be called so resources can be allocated as part of
- * setting up a gadget driver.  Those resources include endpoints, which
- * should be allocated using @usb_ep_autoconfig().
- *
- * To support dual speed operation, a function driver provides descriptors
- * for both high and full speed operation.  Except in rare cases that don't
- * involve bulk endpoints, each speed needs different endpoint descriptors.
- *
- * Function drivers choose their own strategies for managing instance data.
- * The simplest strategy just declares it "static', which means the function
- * can only be activated once.  If the function needs to be exposed in more
- * than one configuration at a given speed, it needs to support multiple
- * usb_function structures (one for each configuration).
- *
- * A more complex strategy might encapsulate a @usb_function structure inside
- * a driver-specific instance structure to allows multiple activations.  An
- * example of multiple activations might be a CDC ACM function that supports
- * two or more distinct instances within the same configuration, providing
- * several independent logical data links to a USB host.
- */
-struct usb_function {
-	const char			*name;
-	struct usb_gadget_strings	**strings;
-	struct usb_descriptor_header	**descriptors;
-	struct usb_descriptor_header	**hs_descriptors;
-	struct usb_descriptor_header	**ss_descriptors;
-
-	struct usb_configuration	*config;
-
-	/* REVISIT:  bind() functions can be marked __init, which
-	 * makes trouble for section mismatch analysis.  See if
-	 * we can't restructure things to avoid mismatching.
-	 * Related:  unbind() may kfree() but bind() won't...
-	 */
-
-	/* configuration management:  bind/unbind */
-	int			(*bind)(struct usb_configuration *,
-					struct usb_function *);
-	void			(*unbind)(struct usb_configuration *,
-					struct usb_function *);
-
-	/* runtime state management */
-	int			(*set_alt)(struct usb_function *,
-					unsigned interface, unsigned alt);
-	int			(*get_alt)(struct usb_function *,
-					unsigned interface);
-	void			(*disable)(struct usb_function *);
-	int			(*setup)(struct usb_function *,
-					const struct usb_ctrlrequest *);
-	void			(*suspend)(struct usb_function *);
-	void			(*resume)(struct usb_function *);
-
-	/* USB 3.0 additions */
-	int			(*get_status)(struct usb_function *);
-	int			(*func_suspend)(struct usb_function *,
-						u8 suspend_opt);
-	/* private: */
-	/* internals */
-	struct list_head		list;
-	DECLARE_BITMAP(endpoints, 32);
-};
-
-int usb_add_function(struct usb_configuration *, struct usb_function *);
-
-int usb_function_deactivate(struct usb_function *);
-int usb_function_activate(struct usb_function *);
-
-int usb_interface_id(struct usb_configuration *, struct usb_function *);
-
-int config_ep_by_speed(struct usb_gadget *g, struct usb_function *f,
-			struct usb_ep *_ep);
-
-#define	MAX_CONFIG_INTERFACES		16	/* arbitrary; max 255 */
-
-/**
- * struct usb_configuration - represents one gadget configuration
- * @label: For diagnostics, describes the configuration.
- * @strings: Tables of strings, keyed by identifiers assigned during @bind()
- *	and by language IDs provided in control requests.
- * @descriptors: Table of descriptors preceding all function descriptors.
- *	Examples include OTG and vendor-specific descriptors.
- * @unbind: Reverses @bind; called as a side effect of unregistering the
- *	driver which added this configuration.
- * @setup: Used to delegate control requests that aren't handled by standard
- *	device infrastructure or directed at a specific interface.
- * @bConfigurationValue: Copied into configuration descriptor.
- * @iConfiguration: Copied into configuration descriptor.
- * @bmAttributes: Copied into configuration descriptor.
- * @bMaxPower: Copied into configuration descriptor.
- * @cdev: assigned by @usb_add_config() before calling @bind(); this is
- *	the device associated with this configuration.
- *
- * Configurations are building blocks for gadget drivers structured around
- * function drivers.  Simple USB gadgets require only one function and one
- * configuration, and handle dual-speed hardware by always providing the same
- * functionality.  Slightly more complex gadgets may have more than one
- * single-function configuration at a given speed; or have configurations
- * that only work at one speed.
- *
- * Composite devices are, by definition, ones with configurations which
- * include more than one function.
- *
- * The lifecycle of a usb_configuration includes allocation, initialization
- * of the fields described above, and calling @usb_add_config() to set up
- * internal data and bind it to a specific device.  The configuration's
- * @bind() method is then used to initialize all the functions and then
- * call @usb_add_function() for them.
- *
- * Those functions would normally be independent of each other, but that's
- * not mandatory.  CDC WMC devices are an example where functions often
- * depend on other functions, with some functions subsidiary to others.
- * Such interdependency may be managed in any way, so long as all of the
- * descriptors complete by the time the composite driver returns from
- * its bind() routine.
- */
-struct usb_configuration {
-	const char			*label;
-	struct usb_gadget_strings	**strings;
-	const struct usb_descriptor_header **descriptors;
-
-	/* REVISIT:  bind() functions can be marked __init, which
-	 * makes trouble for section mismatch analysis.  See if
-	 * we can't restructure things to avoid mismatching...
-	 */
-
-	/* configuration management: unbind/setup */
-	void			(*unbind)(struct usb_configuration *);
-	int			(*setup)(struct usb_configuration *,
-					const struct usb_ctrlrequest *);
-
-	/* fields in the config descriptor */
-	u8			bConfigurationValue;
-	u8			iConfiguration;
-	u8			bmAttributes;
-	u8			bMaxPower;
-
-	struct usb_composite_dev	*cdev;
-
-	/* private: */
-	/* internals */
-	struct list_head	list;
-	struct list_head	functions;
-	u8			next_interface_id;
-	unsigned		superspeed:1;
-	unsigned		highspeed:1;
-	unsigned		fullspeed:1;
-	struct usb_function	*interface[MAX_CONFIG_INTERFACES];
-};
-
-int usb_add_config(struct usb_composite_dev *,
-		struct usb_configuration *,
-		int (*)(struct usb_configuration *));
-
-void usb_remove_config(struct usb_composite_dev *,
-		struct usb_configuration *);
-
-/**
- * struct usb_composite_driver - groups configurations into a gadget
- * @name: For diagnostics, identifies the driver.
- * @iProduct: Used as iProduct override if @dev->iProduct is not set.
- *	If NULL value of @name is taken.
- * @iManufacturer: Used as iManufacturer override if @dev->iManufacturer is
- *	not set. If NULL a default "<system> <release> with <udc>" value
- *	will be used.
- * @iSerialNumber: Used as iSerialNumber override if @dev->iSerialNumber is
- *	not set.
- * @dev: Template descriptor for the device, including default device
- *	identifiers.
- * @strings: tables of strings, keyed by identifiers assigned during @bind
- *	and language IDs provided in control requests
- * @max_speed: Highest speed the driver supports.
- * @needs_serial: set to 1 if the gadget needs userspace to provide
- * 	a serial number.  If one is not provided, warning will be printed.
- * @bind: (REQUIRED) Used to allocate resources that are shared across the
- *	whole device, such as string IDs, and add its configurations using
- *	@usb_add_config(). This may fail by returning a negative errno
- *	value; it should return zero on successful initialization.
- * @unbind: Reverses @bind; called as a side effect of unregistering
- *	this driver.
- * @disconnect: optional driver disconnect method
- * @suspend: Notifies when the host stops sending USB traffic,
- *	after function notifications
- * @resume: Notifies configuration when the host restarts USB traffic,
- *	before function notifications
- *
- * Devices default to reporting self powered operation.  Devices which rely
- * on bus powered operation should report this in their @bind method.
- *
- * Before returning from @bind, various fields in the template descriptor
- * may be overridden.  These include the idVendor/idProduct/bcdDevice values
- * normally to bind the appropriate host side driver, and the three strings
- * (iManufacturer, iProduct, iSerialNumber) normally used to provide user
- * meaningful device identifiers.  (The strings will not be defined unless
- * they are defined in @dev and @strings.)  The correct ep0 maxpacket size
- * is also reported, as defined by the underlying controller driver.
- */
-struct usb_composite_driver {
-	const char				*name;
-	const char				*iProduct;
-	const char				*iManufacturer;
-	const char				*iSerialNumber;
-	const struct usb_device_descriptor	*dev;
-	struct usb_gadget_strings		**strings;
-	enum usb_device_speed			max_speed;
-	unsigned		needs_serial:1;
-
-	int			(*bind)(struct usb_composite_dev *cdev);
-	int			(*unbind)(struct usb_composite_dev *);
-
-	void			(*disconnect)(struct usb_composite_dev *);
-
-	/* global suspend hooks */
-	void			(*suspend)(struct usb_composite_dev *);
-	void			(*resume)(struct usb_composite_dev *);
-};
-
-extern int usb_composite_probe(struct usb_composite_driver *driver);
-extern void usb_composite_unregister(struct usb_composite_driver *driver);
-extern void usb_composite_setup_continue(struct usb_composite_dev *cdev);
-
-
-/**
- * struct usb_composite_device - represents one composite usb gadget
- * @gadget: read-only, abstracts the gadget's usb peripheral controller
- * @req: used for control responses; buffer is pre-allocated
- * @bufsiz: size of buffer pre-allocated in @req
- * @config: the currently active configuration
- *
- * One of these devices is allocated and initialized before the
- * associated device driver's bind() is called.
- *
- * OPEN ISSUE:  it appears that some WUSB devices will need to be
- * built by combining a normal (wired) gadget with a wireless one.
- * This revision of the gadget framework should probably try to make
- * sure doing that won't hurt too much.
- *
- * One notion for how to handle Wireless USB devices involves:
- * (a) a second gadget here, discovery mechanism TBD, but likely
- *     needing separate "register/unregister WUSB gadget" calls;
- * (b) updates to usb_gadget to include flags "is it wireless",
- *     "is it wired", plus (presumably in a wrapper structure)
- *     bandgroup and PHY info;
- * (c) presumably a wireless_ep wrapping a usb_ep, and reporting
- *     wireless-specific parameters like maxburst and maxsequence;
- * (d) configurations that are specific to wireless links;
- * (e) function drivers that understand wireless configs and will
- *     support wireless for (additional) function instances;
- * (f) a function to support association setup (like CBAF), not
- *     necessarily requiring a wireless adapter;
- * (g) composite device setup that can create one or more wireless
- *     configs, including appropriate association setup support;
- * (h) more, TBD.
- */
-struct usb_composite_dev {
-	struct usb_gadget		*gadget;
-	struct usb_request		*req;
-	unsigned			bufsiz;
-
-	struct usb_configuration	*config;
-
-	/* private: */
-	/* internals */
-	unsigned int			suspended:1;
-	struct usb_device_descriptor	desc;
-	struct list_head		configs;
-	struct usb_composite_driver	*driver;
-	u8				next_string_id;
-	u8				manufacturer_override;
-	u8				product_override;
-	u8				serial_override;
-
-	/* the gadget driver won't enable the data pullup
-	 * while the deactivation count is nonzero.
-	 */
-	unsigned			deactivations;
-
-	/* the composite driver won't complete the control transfer's
-	 * data/status stages till delayed_status is zero.
-	 */
-	int				delayed_status;
-
-	/* protects deactivations and delayed_status counts*/
-	spinlock_t			lock;
-};
-
-extern int usb_string_id(struct usb_composite_dev *c);
-extern int usb_string_ids_tab(struct usb_composite_dev *c,
-			      struct usb_string *str);
-extern int usb_string_ids_n(struct usb_composite_dev *c, unsigned n);
-
-
-/* messaging utils */
-#define DBG(d, fmt, args...) \
-	dev_dbg(&(d)->gadget->dev , fmt , ## args)
-#define VDBG(d, fmt, args...) \
-	dev_vdbg(&(d)->gadget->dev , fmt , ## args)
-#define ERROR(d, fmt, args...) \
-	dev_err(&(d)->gadget->dev , fmt , ## args)
-#define WARNING(d, fmt, args...) \
-	dev_warn(&(d)->gadget->dev , fmt , ## args)
-#define INFO(d, fmt, args...) \
-	dev_info(&(d)->gadget->dev , fmt , ## args)
-
-#endif	/* __LINUX_USB_COMPOSITE_H */
diff --git a/drivers/staging/ccg/config.c b/drivers/staging/ccg/config.c
deleted file mode 100644
index 7542a72ce51..00000000000
--- a/drivers/staging/ccg/config.c
+++ /dev/null
@@ -1,158 +0,0 @@
-/*
- * usb/gadget/config.c -- simplify building config descriptors
- *
- * Copyright (C) 2003 David Brownell
- *
- * 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/errno.h>
-#include <linux/slab.h>
-#include <linux/kernel.h>
-#include <linux/list.h>
-#include <linux/string.h>
-#include <linux/device.h>
-
-#include <linux/usb/ch9.h>
-#include <linux/usb/gadget.h>
-
-
-/**
- * usb_descriptor_fillbuf - fill buffer with descriptors
- * @buf: Buffer to be filled
- * @buflen: Size of buf
- * @src: Array of descriptor pointers, terminated by null pointer.
- *
- * Copies descriptors into the buffer, returning the length or a
- * negative error code if they can't all be copied.  Useful when
- * assembling descriptors for an associated set of interfaces used
- * as part of configuring a composite device; or in other cases where
- * sets of descriptors need to be marshaled.
- */
-int
-usb_descriptor_fillbuf(void *buf, unsigned buflen,
-		const struct usb_descriptor_header **src)
-{
-	u8	*dest = buf;
-
-	if (!src)
-		return -EINVAL;
-
-	/* fill buffer from src[] until null descriptor ptr */
-	for (; NULL != *src; src++) {
-		unsigned		len = (*src)->bLength;
-
-		if (len > buflen)
-			return -EINVAL;
-		memcpy(dest, *src, len);
-		buflen -= len;
-		dest += len;
-	}
-	return dest - (u8 *)buf;
-}
-
-
-/**
- * usb_gadget_config_buf - builts a complete configuration descriptor
- * @config: Header for the descriptor, including characteristics such
- *	as power requirements and number of interfaces.
- * @desc: Null-terminated vector of pointers to the descriptors (interface,
- *	endpoint, etc) defining all functions in this device configuration.
- * @buf: Buffer for the resulting configuration descriptor.
- * @length: Length of buffer.  If this is not big enough to hold the
- *	entire configuration descriptor, an error code will be returned.
- *
- * This copies descriptors into the response buffer, building a descriptor
- * for that configuration.  It returns the buffer length or a negative
- * status code.  The config.wTotalLength field is set to match the length
- * of the result, but other descriptor fields (including power usage and
- * interface count) must be set by the caller.
- *
- * Gadget drivers could use this when constructing a config descriptor
- * in response to USB_REQ_GET_DESCRIPTOR.  They will need to patch the
- * resulting bDescriptorType value if USB_DT_OTHER_SPEED_CONFIG is needed.
- */
-int usb_gadget_config_buf(
-	const struct usb_config_descriptor	*config,
-	void					*buf,
-	unsigned				length,
-	const struct usb_descriptor_header	**desc
-)
-{
-	struct usb_config_descriptor		*cp = buf;
-	int					len;
-
-	/* config descriptor first */
-	if (length < USB_DT_CONFIG_SIZE || !desc)
-		return -EINVAL;
-	*cp = *config;
-
-	/* then interface/endpoint/class/vendor/... */
-	len = usb_descriptor_fillbuf(USB_DT_CONFIG_SIZE + (u8*)buf,
-			length - USB_DT_CONFIG_SIZE, desc);
-	if (len < 0)
-		return len;
-	len += USB_DT_CONFIG_SIZE;
-	if (len > 0xffff)
-		return -EINVAL;
-
-	/* patch up the config descriptor */
-	cp->bLength = USB_DT_CONFIG_SIZE;
-	cp->bDescriptorType = USB_DT_CONFIG;
-	cp->wTotalLength = cpu_to_le16(len);
-	cp->bmAttributes |= USB_CONFIG_ATT_ONE;
-	return len;
-}
-
-/**
- * usb_copy_descriptors - copy a vector of USB descriptors
- * @src: null-terminated vector to copy
- * Context: initialization code, which may sleep
- *
- * This makes a copy of a vector of USB descriptors.  Its primary use
- * is to support usb_function objects which can have multiple copies,
- * each needing different descriptors.  Functions may have static
- * tables of descriptors, which are used as templates and customized
- * with identifiers (for interfaces, strings, endpoints, and more)
- * as needed by a given function instance.
- */
-struct usb_descriptor_header **
-usb_copy_descriptors(struct usb_descriptor_header **src)
-{
-	struct usb_descriptor_header **tmp;
-	unsigned bytes;
-	unsigned n_desc;
-	void *mem;
-	struct usb_descriptor_header **ret;
-
-	/* count descriptors and their sizes; then add vector size */
-	for (bytes = 0, n_desc = 0, tmp = src; *tmp; tmp++, n_desc++)
-		bytes += (*tmp)->bLength;
-	bytes += (n_desc + 1) * sizeof(*tmp);
-
-	mem = kmalloc(bytes, GFP_KERNEL);
-	if (!mem)
-		return NULL;
-
-	/* fill in pointers starting at "tmp",
-	 * to descriptors copied starting at "mem";
-	 * and return "ret"
-	 */
-	tmp = mem;
-	ret = mem;
-	mem += (n_desc + 1) * sizeof(*tmp);
-	while (*src) {
-		memcpy(mem, *src, (*src)->bLength);
-		*tmp = mem;
-		tmp++;
-		mem += (*src)->bLength;
-		src++;
-	}
-	*tmp = NULL;
-
-	return ret;
-}
-
diff --git a/drivers/staging/ccg/epautoconf.c b/drivers/staging/ccg/epautoconf.c
deleted file mode 100644
index 51f3d42f5a6..00000000000
--- a/drivers/staging/ccg/epautoconf.c
+++ /dev/null
@@ -1,393 +0,0 @@
-/*
- * epautoconf.c -- endpoint autoconfiguration for usb gadget drivers
- *
- * Copyright (C) 2004 David Brownell
- *
- * 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/kernel.h>
-#include <linux/init.h>
-#include <linux/types.h>
-#include <linux/device.h>
-
-#include <linux/ctype.h>
-#include <linux/string.h>
-
-#include <linux/usb/ch9.h>
-#include <linux/usb/gadget.h>
-
-#include "gadget_chips.h"
-
-
-/* we must assign addresses for configurable endpoints (like net2280) */
-static unsigned epnum;
-
-// #define MANY_ENDPOINTS
-#ifdef MANY_ENDPOINTS
-/* more than 15 configurable endpoints */
-static unsigned in_epnum;
-#endif
-
-
-/*
- * This should work with endpoints from controller drivers sharing the
- * same endpoint naming convention.  By example:
- *
- *	- ep1, ep2, ... address is fixed, not direction or type
- *	- ep1in, ep2out, ... address and direction are fixed, not type
- *	- ep1-bulk, ep2-bulk, ... address and type are fixed, not direction
- *	- ep1in-bulk, ep2out-iso, ... all three are fixed
- *	- ep-* ... no functionality restrictions
- *
- * Type suffixes are "-bulk", "-iso", or "-int".  Numbers are decimal.
- * Less common restrictions are implied by gadget_is_*().
- *
- * NOTE:  each endpoint is unidirectional, as specified by its USB
- * descriptor; and isn't specific to a configuration or altsetting.
- */
-static int
-ep_matches (
-	struct usb_gadget		*gadget,
-	struct usb_ep			*ep,
-	struct usb_endpoint_descriptor	*desc,
-	struct usb_ss_ep_comp_descriptor *ep_comp
-)
-{
-	u8		type;
-	const char	*tmp;
-	u16		max;
-
-	int		num_req_streams = 0;
-
-	/* endpoint already claimed? */
-	if (NULL != ep->driver_data)
-		return 0;
-
-	/* only support ep0 for portable CONTROL traffic */
-	type = desc->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK;
-	if (USB_ENDPOINT_XFER_CONTROL == type)
-		return 0;
-
-	/* some other naming convention */
-	if ('e' != ep->name[0])
-		return 0;
-
-	/* type-restriction:  "-iso", "-bulk", or "-int".
-	 * direction-restriction:  "in", "out".
-	 */
-	if ('-' != ep->name[2]) {
-		tmp = strrchr (ep->name, '-');
-		if (tmp) {
-			switch (type) {
-			case USB_ENDPOINT_XFER_INT:
-				/* bulk endpoints handle interrupt transfers,
-				 * except the toggle-quirky iso-synch kind
-				 */
-				if ('s' == tmp[2])	// == "-iso"
-					return 0;
-				/* for now, avoid PXA "interrupt-in";
-				 * it's documented as never using DATA1.
-				 */
-				if (gadget_is_pxa (gadget)
-						&& 'i' == tmp [1])
-					return 0;
-				break;
-			case USB_ENDPOINT_XFER_BULK:
-				if ('b' != tmp[1])	// != "-bulk"
-					return 0;
-				break;
-			case USB_ENDPOINT_XFER_ISOC:
-				if ('s' != tmp[2])	// != "-iso"
-					return 0;
-			}
-		} else {
-			tmp = ep->name + strlen (ep->name);
-		}
-
-		/* direction-restriction:  "..in-..", "out-.." */
-		tmp--;
-		if (!isdigit (*tmp)) {
-			if (desc->bEndpointAddress & USB_DIR_IN) {
-				if ('n' != *tmp)
-					return 0;
-			} else {
-				if ('t' != *tmp)
-					return 0;
-			}
-		}
-	}
-
-	/*
-	 * Get the number of required streams from the EP companion
-	 * descriptor and see if the EP matches it
-	 */
-	if (usb_endpoint_xfer_bulk(desc)) {
-		if (ep_comp && gadget->max_speed >= USB_SPEED_SUPER) {
-			num_req_streams = ep_comp->bmAttributes & 0x1f;
-			if (num_req_streams > ep->max_streams)
-				return 0;
-		}
-
-	}
-
-	/*
-	 * If the protocol driver hasn't yet decided on wMaxPacketSize
-	 * and wants to know the maximum possible, provide the info.
-	 */
-	if (desc->wMaxPacketSize == 0)
-		desc->wMaxPacketSize = cpu_to_le16(ep->maxpacket);
-
-	/* endpoint maxpacket size is an input parameter, except for bulk
-	 * where it's an output parameter representing the full speed limit.
-	 * the usb spec fixes high speed bulk maxpacket at 512 bytes.
-	 */
-	max = 0x7ff & usb_endpoint_maxp(desc);
-	switch (type) {
-	case USB_ENDPOINT_XFER_INT:
-		/* INT:  limit 64 bytes full speed, 1024 high/super speed */
-		if (!gadget_is_dualspeed(gadget) && max > 64)
-			return 0;
-		/* FALLTHROUGH */
-
-	case USB_ENDPOINT_XFER_ISOC:
-		/* ISO:  limit 1023 bytes full speed, 1024 high/super speed */
-		if (ep->maxpacket < max)
-			return 0;
-		if (!gadget_is_dualspeed(gadget) && max > 1023)
-			return 0;
-
-		/* BOTH:  "high bandwidth" works only at high speed */
-		if ((desc->wMaxPacketSize & cpu_to_le16(3<<11))) {
-			if (!gadget_is_dualspeed(gadget))
-				return 0;
-			/* configure your hardware with enough buffering!! */
-		}
-		break;
-	}
-
-	/* MATCH!! */
-
-	/* report address */
-	desc->bEndpointAddress &= USB_DIR_IN;
-	if (isdigit (ep->name [2])) {
-		u8	num = simple_strtoul (&ep->name [2], NULL, 10);
-		desc->bEndpointAddress |= num;
-#ifdef	MANY_ENDPOINTS
-	} else if (desc->bEndpointAddress & USB_DIR_IN) {
-		if (++in_epnum > 15)
-			return 0;
-		desc->bEndpointAddress = USB_DIR_IN | in_epnum;
-#endif
-	} else {
-		if (++epnum > 15)
-			return 0;
-		desc->bEndpointAddress |= epnum;
-	}
-
-	/* report (variable) full speed bulk maxpacket */
-	if ((USB_ENDPOINT_XFER_BULK == type) && !ep_comp) {
-		int size = ep->maxpacket;
-
-		/* min() doesn't work on bitfields with gcc-3.5 */
-		if (size > 64)
-			size = 64;
-		desc->wMaxPacketSize = cpu_to_le16(size);
-	}
-	ep->address = desc->bEndpointAddress;
-	return 1;
-}
-
-static struct usb_ep *
-find_ep (struct usb_gadget *gadget, const char *name)
-{
-	struct usb_ep	*ep;
-
-	list_for_each_entry (ep, &gadget->ep_list, ep_list) {
-		if (0 == strcmp (ep->name, name))
-			return ep;
-	}
-	return NULL;
-}
-
-/**
- * usb_ep_autoconfig_ss() - choose an endpoint matching the ep
- * descriptor and ep companion descriptor
- * @gadget: The device to which the endpoint must belong.
- * @desc: Endpoint descriptor, with endpoint direction and transfer mode
- *    initialized.  For periodic transfers, the maximum packet
- *    size must also be initialized.  This is modified on
- *    success.
- * @ep_comp: Endpoint companion descriptor, with the required
- *    number of streams. Will be modified when the chosen EP
- *    supports a different number of streams.
- *
- * This routine replaces the usb_ep_autoconfig when needed
- * superspeed enhancments. If such enhancemnets are required,
- * the FD should call usb_ep_autoconfig_ss directly and provide
- * the additional ep_comp parameter.
- *
- * By choosing an endpoint to use with the specified descriptor,
- * this routine simplifies writing gadget drivers that work with
- * multiple USB device controllers.  The endpoint would be
- * passed later to usb_ep_enable(), along with some descriptor.
- *
- * That second descriptor won't always be the same as the first one.
- * For example, isochronous endpoints can be autoconfigured for high
- * bandwidth, and then used in several lower bandwidth altsettings.
- * Also, high and full speed descriptors will be different.
- *
- * Be sure to examine and test the results of autoconfiguration
- * on your hardware.  This code may not make the best choices
- * about how to use the USB controller, and it can't know all
- * the restrictions that may apply. Some combinations of driver
- * and hardware won't be able to autoconfigure.
- *
- * On success, this returns an un-claimed usb_ep, and modifies the endpoint
- * descriptor bEndpointAddress.  For bulk endpoints, the wMaxPacket value
- * is initialized as if the endpoint were used at full speed and
- * the bmAttribute field in the ep companion descriptor is
- * updated with the assigned number of streams if it is
- * different from the original value. To prevent the endpoint
- * from being returned by a later autoconfig call, claim it by
- * assigning ep->driver_data to some non-null value.
- *
- * On failure, this returns a null endpoint descriptor.
- */
-struct usb_ep *usb_ep_autoconfig_ss(
-	struct usb_gadget		*gadget,
-	struct usb_endpoint_descriptor	*desc,
-	struct usb_ss_ep_comp_descriptor *ep_comp
-)
-{
-	struct usb_ep	*ep;
-	u8		type;
-
-	type = desc->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK;
-
-	/* First, apply chip-specific "best usage" knowledge.
-	 * This might make a good usb_gadget_ops hook ...
-	 */
-	if (gadget_is_net2280 (gadget) && type == USB_ENDPOINT_XFER_INT) {
-		/* ep-e, ep-f are PIO with only 64 byte fifos */
-		ep = find_ep (gadget, "ep-e");
-		if (ep && ep_matches(gadget, ep, desc, ep_comp))
-			goto found_ep;
-		ep = find_ep (gadget, "ep-f");
-		if (ep && ep_matches(gadget, ep, desc, ep_comp))
-			goto found_ep;
-
-	} else if (gadget_is_goku (gadget)) {
-		if (USB_ENDPOINT_XFER_INT == type) {
-			/* single buffering is enough */
-			ep = find_ep(gadget, "ep3-bulk");
-			if (ep && ep_matches(gadget, ep, desc, ep_comp))
-				goto found_ep;
-		} else if (USB_ENDPOINT_XFER_BULK == type
-				&& (USB_DIR_IN & desc->bEndpointAddress)) {
-			/* DMA may be available */
-			ep = find_ep(gadget, "ep2-bulk");
-			if (ep && ep_matches(gadget, ep, desc,
-					      ep_comp))
-				goto found_ep;
-		}
-
-#ifdef CONFIG_BLACKFIN
-	} else if (gadget_is_musbhdrc(gadget)) {
-		if ((USB_ENDPOINT_XFER_BULK == type) ||
-		    (USB_ENDPOINT_XFER_ISOC == type)) {
-			if (USB_DIR_IN & desc->bEndpointAddress)
-				ep = find_ep (gadget, "ep5in");
-			else
-				ep = find_ep (gadget, "ep6out");
-		} else if (USB_ENDPOINT_XFER_INT == type) {
-			if (USB_DIR_IN & desc->bEndpointAddress)
-				ep = find_ep(gadget, "ep1in");
-			else
-				ep = find_ep(gadget, "ep2out");
-		} else
-			ep = NULL;
-		if (ep && ep_matches(gadget, ep, desc, ep_comp))
-			goto found_ep;
-#endif
-	}
-
-	/* Second, look at endpoints until an unclaimed one looks usable */
-	list_for_each_entry (ep, &gadget->ep_list, ep_list) {
-		if (ep_matches(gadget, ep, desc, ep_comp))
-			goto found_ep;
-	}
-
-	/* Fail */
-	return NULL;
-found_ep:
-	ep->desc = NULL;
-	ep->comp_desc = NULL;
-	return ep;
-}
-
-/**
- * usb_ep_autoconfig() - choose an endpoint matching the
- * descriptor
- * @gadget: The device to which the endpoint must belong.
- * @desc: Endpoint descriptor, with endpoint direction and transfer mode
- *	initialized.  For periodic transfers, the maximum packet
- *	size must also be initialized.  This is modified on success.
- *
- * By choosing an endpoint to use with the specified descriptor, this
- * routine simplifies writing gadget drivers that work with multiple
- * USB device controllers.  The endpoint would be passed later to
- * usb_ep_enable(), along with some descriptor.
- *
- * That second descriptor won't always be the same as the first one.
- * For example, isochronous endpoints can be autoconfigured for high
- * bandwidth, and then used in several lower bandwidth altsettings.
- * Also, high and full speed descriptors will be different.
- *
- * Be sure to examine and test the results of autoconfiguration on your
- * hardware.  This code may not make the best choices about how to use the
- * USB controller, and it can't know all the restrictions that may apply.
- * Some combinations of driver and hardware won't be able to autoconfigure.
- *
- * On success, this returns an un-claimed usb_ep, and modifies the endpoint
- * descriptor bEndpointAddress.  For bulk endpoints, the wMaxPacket value
- * is initialized as if the endpoint were used at full speed.  To prevent
- * the endpoint from being returned by a later autoconfig call, claim it
- * by assigning ep->driver_data to some non-null value.
- *
- * On failure, this returns a null endpoint descriptor.
- */
-struct usb_ep *usb_ep_autoconfig(
-	struct usb_gadget		*gadget,
-	struct usb_endpoint_descriptor	*desc
-)
-{
-	return usb_ep_autoconfig_ss(gadget, desc, NULL);
-}
-
-
-/**
- * usb_ep_autoconfig_reset - reset endpoint autoconfig state
- * @gadget: device for which autoconfig state will be reset
- *
- * Use this for devices where one configuration may need to assign
- * endpoint resources very differently from the next one.  It clears
- * state such as ep->driver_data and the record of assigned endpoints
- * used by usb_ep_autoconfig().
- */
-void usb_ep_autoconfig_reset (struct usb_gadget *gadget)
-{
-	struct usb_ep	*ep;
-
-	list_for_each_entry (ep, &gadget->ep_list, ep_list) {
-		ep->driver_data = NULL;
-	}
-#ifdef	MANY_ENDPOINTS
-	in_epnum = 0;
-#endif
-	epnum = 0;
-}
-
diff --git a/drivers/staging/ccg/f_acm.c b/drivers/staging/ccg/f_acm.c
deleted file mode 100644
index d672250a61f..00000000000
--- a/drivers/staging/ccg/f_acm.c
+++ /dev/null
@@ -1,814 +0,0 @@
-/*
- * f_acm.c -- USB CDC serial (ACM) function driver
- *
- * Copyright (C) 2003 Al Borchers (alborchers@steinerpoint.com)
- * Copyright (C) 2008 by David Brownell
- * Copyright (C) 2008 by Nokia Corporation
- * Copyright (C) 2009 by Samsung Electronics
- * Author: Michal Nazarewicz (mina86@mina86.com)
- *
- * This software is distributed under the terms of the GNU General
- * Public License ("GPL") as published by the Free Software Foundation,
- * either version 2 of that License or (at your option) any later version.
- */
-
-/* #define VERBOSE_DEBUG */
-
-#include <linux/slab.h>
-#include <linux/kernel.h>
-#include <linux/device.h>
-
-#include "u_serial.h"
-#include "gadget_chips.h"
-
-
-/*
- * This CDC ACM function support just wraps control functions and
- * notifications around the generic serial-over-usb code.
- *
- * Because CDC ACM is standardized by the USB-IF, many host operating
- * systems have drivers for it.  Accordingly, ACM is the preferred
- * interop solution for serial-port type connections.  The control
- * models are often not necessary, and in any case don't do much in
- * this bare-bones implementation.
- *
- * Note that even MS-Windows has some support for ACM.  However, that
- * support is somewhat broken because when you use ACM in a composite
- * device, having multiple interfaces confuses the poor OS.  It doesn't
- * seem to understand CDC Union descriptors.  The new "association"
- * descriptors (roughly equivalent to CDC Unions) may sometimes help.
- */
-
-struct f_acm {
-	struct gserial			port;
-	u8				ctrl_id, data_id;
-	u8				port_num;
-
-	u8				pending;
-
-	/* lock is mostly for pending and notify_req ... they get accessed
-	 * by callbacks both from tty (open/close/break) under its spinlock,
-	 * and notify_req.complete() which can't use that lock.
-	 */
-	spinlock_t			lock;
-
-	struct usb_ep			*notify;
-	struct usb_request		*notify_req;
-
-	struct usb_cdc_line_coding	port_line_coding;	/* 8-N-1 etc */
-
-	/* SetControlLineState request -- CDC 1.1 section 6.2.14 (INPUT) */
-	u16				port_handshake_bits;
-#define ACM_CTRL_RTS	(1 << 1)	/* unused with full duplex */
-#define ACM_CTRL_DTR	(1 << 0)	/* host is ready for data r/w */
-
-	/* SerialState notification -- CDC 1.1 section 6.3.5 (OUTPUT) */
-	u16				serial_state;
-#define ACM_CTRL_OVERRUN	(1 << 6)
-#define ACM_CTRL_PARITY		(1 << 5)
-#define ACM_CTRL_FRAMING	(1 << 4)
-#define ACM_CTRL_RI		(1 << 3)
-#define ACM_CTRL_BRK		(1 << 2)
-#define ACM_CTRL_DSR		(1 << 1)
-#define ACM_CTRL_DCD		(1 << 0)
-};
-
-static inline struct f_acm *func_to_acm(struct usb_function *f)
-{
-	return container_of(f, struct f_acm, port.func);
-}
-
-static inline struct f_acm *port_to_acm(struct gserial *p)
-{
-	return container_of(p, struct f_acm, port);
-}
-
-/*-------------------------------------------------------------------------*/
-
-/* notification endpoint uses smallish and infrequent fixed-size messages */
-
-#define GS_LOG2_NOTIFY_INTERVAL		5	/* 1 << 5 == 32 msec */
-#define GS_NOTIFY_MAXPACKET		10	/* notification + 2 bytes */
-
-/* interface and class descriptors: */
-
-static struct usb_interface_assoc_descriptor
-acm_iad_descriptor = {
-	.bLength =		sizeof acm_iad_descriptor,
-	.bDescriptorType =	USB_DT_INTERFACE_ASSOCIATION,
-
-	/* .bFirstInterface =	DYNAMIC, */
-	.bInterfaceCount = 	2,	// control + data
-	.bFunctionClass =	USB_CLASS_COMM,
-	.bFunctionSubClass =	USB_CDC_SUBCLASS_ACM,
-	.bFunctionProtocol =	USB_CDC_ACM_PROTO_AT_V25TER,
-	/* .iFunction =		DYNAMIC */
-};
-
-
-static struct usb_interface_descriptor acm_control_interface_desc = {
-	.bLength =		USB_DT_INTERFACE_SIZE,
-	.bDescriptorType =	USB_DT_INTERFACE,
-	/* .bInterfaceNumber = DYNAMIC */
-	.bNumEndpoints =	1,
-	.bInterfaceClass =	USB_CLASS_COMM,
-	.bInterfaceSubClass =	USB_CDC_SUBCLASS_ACM,
-	.bInterfaceProtocol =	USB_CDC_ACM_PROTO_AT_V25TER,
-	/* .iInterface = DYNAMIC */
-};
-
-static struct usb_interface_descriptor acm_data_interface_desc = {
-	.bLength =		USB_DT_INTERFACE_SIZE,
-	.bDescriptorType =	USB_DT_INTERFACE,
-	/* .bInterfaceNumber = DYNAMIC */
-	.bNumEndpoints =	2,
-	.bInterfaceClass =	USB_CLASS_CDC_DATA,
-	.bInterfaceSubClass =	0,
-	.bInterfaceProtocol =	0,
-	/* .iInterface = DYNAMIC */
-};
-
-static struct usb_cdc_header_desc acm_header_desc = {
-	.bLength =		sizeof(acm_header_desc),
-	.bDescriptorType =	USB_DT_CS_INTERFACE,
-	.bDescriptorSubType =	USB_CDC_HEADER_TYPE,
-	.bcdCDC =		cpu_to_le16(0x0110),
-};
-
-static struct usb_cdc_call_mgmt_descriptor
-acm_call_mgmt_descriptor = {
-	.bLength =		sizeof(acm_call_mgmt_descriptor),
-	.bDescriptorType =	USB_DT_CS_INTERFACE,
-	.bDescriptorSubType =	USB_CDC_CALL_MANAGEMENT_TYPE,
-	.bmCapabilities =	0,
-	/* .bDataInterface = DYNAMIC */
-};
-
-static struct usb_cdc_acm_descriptor acm_descriptor = {
-	.bLength =		sizeof(acm_descriptor),
-	.bDescriptorType =	USB_DT_CS_INTERFACE,
-	.bDescriptorSubType =	USB_CDC_ACM_TYPE,
-	.bmCapabilities =	USB_CDC_CAP_LINE,
-};
-
-static struct usb_cdc_union_desc acm_union_desc = {
-	.bLength =		sizeof(acm_union_desc),
-	.bDescriptorType =	USB_DT_CS_INTERFACE,
-	.bDescriptorSubType =	USB_CDC_UNION_TYPE,
-	/* .bMasterInterface0 =	DYNAMIC */
-	/* .bSlaveInterface0 =	DYNAMIC */
-};
-
-/* full speed support: */
-
-static struct usb_endpoint_descriptor acm_fs_notify_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-	.bEndpointAddress =	USB_DIR_IN,
-	.bmAttributes =		USB_ENDPOINT_XFER_INT,
-	.wMaxPacketSize =	cpu_to_le16(GS_NOTIFY_MAXPACKET),
-	.bInterval =		1 << GS_LOG2_NOTIFY_INTERVAL,
-};
-
-static struct usb_endpoint_descriptor acm_fs_in_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-	.bEndpointAddress =	USB_DIR_IN,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-};
-
-static struct usb_endpoint_descriptor acm_fs_out_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-	.bEndpointAddress =	USB_DIR_OUT,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-};
-
-static struct usb_descriptor_header *acm_fs_function[] = {
-	(struct usb_descriptor_header *) &acm_iad_descriptor,
-	(struct usb_descriptor_header *) &acm_control_interface_desc,
-	(struct usb_descriptor_header *) &acm_header_desc,
-	(struct usb_descriptor_header *) &acm_call_mgmt_descriptor,
-	(struct usb_descriptor_header *) &acm_descriptor,
-	(struct usb_descriptor_header *) &acm_union_desc,
-	(struct usb_descriptor_header *) &acm_fs_notify_desc,
-	(struct usb_descriptor_header *) &acm_data_interface_desc,
-	(struct usb_descriptor_header *) &acm_fs_in_desc,
-	(struct usb_descriptor_header *) &acm_fs_out_desc,
-	NULL,
-};
-
-/* high speed support: */
-
-static struct usb_endpoint_descriptor acm_hs_notify_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-	.bEndpointAddress =	USB_DIR_IN,
-	.bmAttributes =		USB_ENDPOINT_XFER_INT,
-	.wMaxPacketSize =	cpu_to_le16(GS_NOTIFY_MAXPACKET),
-	.bInterval =		GS_LOG2_NOTIFY_INTERVAL+4,
-};
-
-static struct usb_endpoint_descriptor acm_hs_in_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-	.wMaxPacketSize =	cpu_to_le16(512),
-};
-
-static struct usb_endpoint_descriptor acm_hs_out_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-	.wMaxPacketSize =	cpu_to_le16(512),
-};
-
-static struct usb_descriptor_header *acm_hs_function[] = {
-	(struct usb_descriptor_header *) &acm_iad_descriptor,
-	(struct usb_descriptor_header *) &acm_control_interface_desc,
-	(struct usb_descriptor_header *) &acm_header_desc,
-	(struct usb_descriptor_header *) &acm_call_mgmt_descriptor,
-	(struct usb_descriptor_header *) &acm_descriptor,
-	(struct usb_descriptor_header *) &acm_union_desc,
-	(struct usb_descriptor_header *) &acm_hs_notify_desc,
-	(struct usb_descriptor_header *) &acm_data_interface_desc,
-	(struct usb_descriptor_header *) &acm_hs_in_desc,
-	(struct usb_descriptor_header *) &acm_hs_out_desc,
-	NULL,
-};
-
-static struct usb_endpoint_descriptor acm_ss_in_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-	.wMaxPacketSize =	cpu_to_le16(1024),
-};
-
-static struct usb_endpoint_descriptor acm_ss_out_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-	.wMaxPacketSize =	cpu_to_le16(1024),
-};
-
-static struct usb_ss_ep_comp_descriptor acm_ss_bulk_comp_desc = {
-	.bLength =              sizeof acm_ss_bulk_comp_desc,
-	.bDescriptorType =      USB_DT_SS_ENDPOINT_COMP,
-};
-
-static struct usb_descriptor_header *acm_ss_function[] = {
-	(struct usb_descriptor_header *) &acm_iad_descriptor,
-	(struct usb_descriptor_header *) &acm_control_interface_desc,
-	(struct usb_descriptor_header *) &acm_header_desc,
-	(struct usb_descriptor_header *) &acm_call_mgmt_descriptor,
-	(struct usb_descriptor_header *) &acm_descriptor,
-	(struct usb_descriptor_header *) &acm_union_desc,
-	(struct usb_descriptor_header *) &acm_hs_notify_desc,
-	(struct usb_descriptor_header *) &acm_ss_bulk_comp_desc,
-	(struct usb_descriptor_header *) &acm_data_interface_desc,
-	(struct usb_descriptor_header *) &acm_ss_in_desc,
-	(struct usb_descriptor_header *) &acm_ss_bulk_comp_desc,
-	(struct usb_descriptor_header *) &acm_ss_out_desc,
-	(struct usb_descriptor_header *) &acm_ss_bulk_comp_desc,
-	NULL,
-};
-
-/* string descriptors: */
-
-#define ACM_CTRL_IDX	0
-#define ACM_DATA_IDX	1
-#define ACM_IAD_IDX	2
-
-/* static strings, in UTF-8 */
-static struct usb_string acm_string_defs[] = {
-	[ACM_CTRL_IDX].s = "CDC Abstract Control Model (ACM)",
-	[ACM_DATA_IDX].s = "CDC ACM Data",
-	[ACM_IAD_IDX ].s = "CDC Serial",
-	{  /* ZEROES END LIST */ },
-};
-
-static struct usb_gadget_strings acm_string_table = {
-	.language =		0x0409,	/* en-us */
-	.strings =		acm_string_defs,
-};
-
-static struct usb_gadget_strings *acm_strings[] = {
-	&acm_string_table,
-	NULL,
-};
-
-/*-------------------------------------------------------------------------*/
-
-/* ACM control ... data handling is delegated to tty library code.
- * The main task of this function is to activate and deactivate
- * that code based on device state; track parameters like line
- * speed, handshake state, and so on; and issue notifications.
- */
-
-static void acm_complete_set_line_coding(struct usb_ep *ep,
-		struct usb_request *req)
-{
-	struct f_acm	*acm = ep->driver_data;
-	struct usb_composite_dev *cdev = acm->port.func.config->cdev;
-
-	if (req->status != 0) {
-		DBG(cdev, "acm ttyGS%d completion, err %d\n",
-				acm->port_num, req->status);
-		return;
-	}
-
-	/* normal completion */
-	if (req->actual != sizeof(acm->port_line_coding)) {
-		DBG(cdev, "acm ttyGS%d short resp, len %d\n",
-				acm->port_num, req->actual);
-		usb_ep_set_halt(ep);
-	} else {
-		struct usb_cdc_line_coding	*value = req->buf;
-
-		/* REVISIT:  we currently just remember this data.
-		 * If we change that, (a) validate it first, then
-		 * (b) update whatever hardware needs updating,
-		 * (c) worry about locking.  This is information on
-		 * the order of 9600-8-N-1 ... most of which means
-		 * nothing unless we control a real RS232 line.
-		 */
-		acm->port_line_coding = *value;
-	}
-}
-
-static int acm_setup(struct usb_function *f, const struct usb_ctrlrequest *ctrl)
-{
-	struct f_acm		*acm = func_to_acm(f);
-	struct usb_composite_dev *cdev = f->config->cdev;
-	struct usb_request	*req = cdev->req;
-	int			value = -EOPNOTSUPP;
-	u16			w_index = le16_to_cpu(ctrl->wIndex);
-	u16			w_value = le16_to_cpu(ctrl->wValue);
-	u16			w_length = le16_to_cpu(ctrl->wLength);
-
-	/* composite driver infrastructure handles everything except
-	 * CDC class messages; interface activation uses set_alt().
-	 *
-	 * Note CDC spec table 4 lists the ACM request profile.  It requires
-	 * encapsulated command support ... we don't handle any, and respond
-	 * to them by stalling.  Options include get/set/clear comm features
-	 * (not that useful) and SEND_BREAK.
-	 */
-	switch ((ctrl->bRequestType << 8) | ctrl->bRequest) {
-
-	/* SET_LINE_CODING ... just read and save what the host sends */
-	case ((USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
-			| USB_CDC_REQ_SET_LINE_CODING:
-		if (w_length != sizeof(struct usb_cdc_line_coding)
-				|| w_index != acm->ctrl_id)
-			goto invalid;
-
-		value = w_length;
-		cdev->gadget->ep0->driver_data = acm;
-		req->complete = acm_complete_set_line_coding;
-		break;
-
-	/* GET_LINE_CODING ... return what host sent, or initial value */
-	case ((USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
-			| USB_CDC_REQ_GET_LINE_CODING:
-		if (w_index != acm->ctrl_id)
-			goto invalid;
-
-		value = min_t(unsigned, w_length,
-				sizeof(struct usb_cdc_line_coding));
-		memcpy(req->buf, &acm->port_line_coding, value);
-		break;
-
-	/* SET_CONTROL_LINE_STATE ... save what the host sent */
-	case ((USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
-			| USB_CDC_REQ_SET_CONTROL_LINE_STATE:
-		if (w_index != acm->ctrl_id)
-			goto invalid;
-
-		value = 0;
-
-		/* FIXME we should not allow data to flow until the
-		 * host sets the ACM_CTRL_DTR bit; and when it clears
-		 * that bit, we should return to that no-flow state.
-		 */
-		acm->port_handshake_bits = w_value;
-		break;
-
-	default:
-invalid:
-		VDBG(cdev, "invalid control req%02x.%02x v%04x i%04x l%d\n",
-			ctrl->bRequestType, ctrl->bRequest,
-			w_value, w_index, w_length);
-	}
-
-	/* respond with data transfer or status phase? */
-	if (value >= 0) {
-		DBG(cdev, "acm ttyGS%d req%02x.%02x v%04x i%04x l%d\n",
-			acm->port_num, ctrl->bRequestType, ctrl->bRequest,
-			w_value, w_index, w_length);
-		req->zero = 0;
-		req->length = value;
-		value = usb_ep_queue(cdev->gadget->ep0, req, GFP_ATOMIC);
-		if (value < 0)
-			ERROR(cdev, "acm response on ttyGS%d, err %d\n",
-					acm->port_num, value);
-	}
-
-	/* device either stalls (value < 0) or reports success */
-	return value;
-}
-
-static int acm_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
-{
-	struct f_acm		*acm = func_to_acm(f);
-	struct usb_composite_dev *cdev = f->config->cdev;
-
-	/* we know alt == 0, so this is an activation or a reset */
-
-	if (intf == acm->ctrl_id) {
-		if (acm->notify->driver_data) {
-			VDBG(cdev, "reset acm control interface %d\n", intf);
-			usb_ep_disable(acm->notify);
-		} else {
-			VDBG(cdev, "init acm ctrl interface %d\n", intf);
-			if (config_ep_by_speed(cdev->gadget, f, acm->notify))
-				return -EINVAL;
-		}
-		usb_ep_enable(acm->notify);
-		acm->notify->driver_data = acm;
-
-	} else if (intf == acm->data_id) {
-		if (acm->port.in->driver_data) {
-			DBG(cdev, "reset acm ttyGS%d\n", acm->port_num);
-			gserial_disconnect(&acm->port);
-		}
-		if (!acm->port.in->desc || !acm->port.out->desc) {
-			DBG(cdev, "activate acm ttyGS%d\n", acm->port_num);
-			if (config_ep_by_speed(cdev->gadget, f,
-					       acm->port.in) ||
-			    config_ep_by_speed(cdev->gadget, f,
-					       acm->port.out)) {
-				acm->port.in->desc = NULL;
-				acm->port.out->desc = NULL;
-				return -EINVAL;
-			}
-		}
-		gserial_connect(&acm->port, acm->port_num);
-
-	} else
-		return -EINVAL;
-
-	return 0;
-}
-
-static void acm_disable(struct usb_function *f)
-{
-	struct f_acm	*acm = func_to_acm(f);
-	struct usb_composite_dev *cdev = f->config->cdev;
-
-	DBG(cdev, "acm ttyGS%d deactivated\n", acm->port_num);
-	gserial_disconnect(&acm->port);
-	usb_ep_disable(acm->notify);
-	acm->notify->driver_data = NULL;
-}
-
-/*-------------------------------------------------------------------------*/
-
-/**
- * acm_cdc_notify - issue CDC notification to host
- * @acm: wraps host to be notified
- * @type: notification type
- * @value: Refer to cdc specs, wValue field.
- * @data: data to be sent
- * @length: size of data
- * Context: irqs blocked, acm->lock held, acm_notify_req non-null
- *
- * Returns zero on success or a negative errno.
- *
- * See section 6.3.5 of the CDC 1.1 specification for information
- * about the only notification we issue:  SerialState change.
- */
-static int acm_cdc_notify(struct f_acm *acm, u8 type, u16 value,
-		void *data, unsigned length)
-{
-	struct usb_ep			*ep = acm->notify;
-	struct usb_request		*req;
-	struct usb_cdc_notification	*notify;
-	const unsigned			len = sizeof(*notify) + length;
-	void				*buf;
-	int				status;
-
-	req = acm->notify_req;
-	acm->notify_req = NULL;
-	acm->pending = false;
-
-	req->length = len;
-	notify = req->buf;
-	buf = notify + 1;
-
-	notify->bmRequestType = USB_DIR_IN | USB_TYPE_CLASS
-			| USB_RECIP_INTERFACE;
-	notify->bNotificationType = type;
-	notify->wValue = cpu_to_le16(value);
-	notify->wIndex = cpu_to_le16(acm->ctrl_id);
-	notify->wLength = cpu_to_le16(length);
-	memcpy(buf, data, length);
-
-	/* ep_queue() can complete immediately if it fills the fifo... */
-	spin_unlock(&acm->lock);
-	status = usb_ep_queue(ep, req, GFP_ATOMIC);
-	spin_lock(&acm->lock);
-
-	if (status < 0) {
-		ERROR(acm->port.func.config->cdev,
-				"acm ttyGS%d can't notify serial state, %d\n",
-				acm->port_num, status);
-		acm->notify_req = req;
-	}
-
-	return status;
-}
-
-static int acm_notify_serial_state(struct f_acm *acm)
-{
-	struct usb_composite_dev *cdev = acm->port.func.config->cdev;
-	int			status;
-
-	spin_lock(&acm->lock);
-	if (acm->notify_req) {
-		DBG(cdev, "acm ttyGS%d serial state %04x\n",
-				acm->port_num, acm->serial_state);
-		status = acm_cdc_notify(acm, USB_CDC_NOTIFY_SERIAL_STATE,
-				0, &acm->serial_state, sizeof(acm->serial_state));
-	} else {
-		acm->pending = true;
-		status = 0;
-	}
-	spin_unlock(&acm->lock);
-	return status;
-}
-
-static void acm_cdc_notify_complete(struct usb_ep *ep, struct usb_request *req)
-{
-	struct f_acm		*acm = req->context;
-	u8			doit = false;
-
-	/* on this call path we do NOT hold the port spinlock,
-	 * which is why ACM needs its own spinlock
-	 */
-	spin_lock(&acm->lock);
-	if (req->status != -ESHUTDOWN)
-		doit = acm->pending;
-	acm->notify_req = req;
-	spin_unlock(&acm->lock);
-
-	if (doit)
-		acm_notify_serial_state(acm);
-}
-
-/* connect == the TTY link is open */
-
-static void acm_connect(struct gserial *port)
-{
-	struct f_acm		*acm = port_to_acm(port);
-
-	acm->serial_state |= ACM_CTRL_DSR | ACM_CTRL_DCD;
-	acm_notify_serial_state(acm);
-}
-
-static void acm_disconnect(struct gserial *port)
-{
-	struct f_acm		*acm = port_to_acm(port);
-
-	acm->serial_state &= ~(ACM_CTRL_DSR | ACM_CTRL_DCD);
-	acm_notify_serial_state(acm);
-}
-
-static int acm_send_break(struct gserial *port, int duration)
-{
-	struct f_acm		*acm = port_to_acm(port);
-	u16			state;
-
-	state = acm->serial_state;
-	state &= ~ACM_CTRL_BRK;
-	if (duration)
-		state |= ACM_CTRL_BRK;
-
-	acm->serial_state = state;
-	return acm_notify_serial_state(acm);
-}
-
-/*-------------------------------------------------------------------------*/
-
-/* ACM function driver setup/binding */
-static int
-acm_bind(struct usb_configuration *c, struct usb_function *f)
-{
-	struct usb_composite_dev *cdev = c->cdev;
-	struct f_acm		*acm = func_to_acm(f);
-	int			status;
-	struct usb_ep		*ep;
-
-	/* allocate instance-specific interface IDs, and patch descriptors */
-	status = usb_interface_id(c, f);
-	if (status < 0)
-		goto fail;
-	acm->ctrl_id = status;
-	acm_iad_descriptor.bFirstInterface = status;
-
-	acm_control_interface_desc.bInterfaceNumber = status;
-	acm_union_desc .bMasterInterface0 = status;
-
-	status = usb_interface_id(c, f);
-	if (status < 0)
-		goto fail;
-	acm->data_id = status;
-
-	acm_data_interface_desc.bInterfaceNumber = status;
-	acm_union_desc.bSlaveInterface0 = status;
-	acm_call_mgmt_descriptor.bDataInterface = status;
-
-	status = -ENODEV;
-
-	/* allocate instance-specific endpoints */
-	ep = usb_ep_autoconfig(cdev->gadget, &acm_fs_in_desc);
-	if (!ep)
-		goto fail;
-	acm->port.in = ep;
-	ep->driver_data = cdev;	/* claim */
-
-	ep = usb_ep_autoconfig(cdev->gadget, &acm_fs_out_desc);
-	if (!ep)
-		goto fail;
-	acm->port.out = ep;
-	ep->driver_data = cdev;	/* claim */
-
-	ep = usb_ep_autoconfig(cdev->gadget, &acm_fs_notify_desc);
-	if (!ep)
-		goto fail;
-	acm->notify = ep;
-	ep->driver_data = cdev;	/* claim */
-
-	/* allocate notification */
-	acm->notify_req = gs_alloc_req(ep,
-			sizeof(struct usb_cdc_notification) + 2,
-			GFP_KERNEL);
-	if (!acm->notify_req)
-		goto fail;
-
-	acm->notify_req->complete = acm_cdc_notify_complete;
-	acm->notify_req->context = acm;
-
-	/* copy descriptors */
-	f->descriptors = usb_copy_descriptors(acm_fs_function);
-	if (!f->descriptors)
-		goto fail;
-
-	/* support all relevant hardware speeds... we expect that when
-	 * hardware is dual speed, all bulk-capable endpoints work at
-	 * both speeds
-	 */
-	if (gadget_is_dualspeed(c->cdev->gadget)) {
-		acm_hs_in_desc.bEndpointAddress =
-				acm_fs_in_desc.bEndpointAddress;
-		acm_hs_out_desc.bEndpointAddress =
-				acm_fs_out_desc.bEndpointAddress;
-		acm_hs_notify_desc.bEndpointAddress =
-				acm_fs_notify_desc.bEndpointAddress;
-
-		/* copy descriptors */
-		f->hs_descriptors = usb_copy_descriptors(acm_hs_function);
-	}
-	if (gadget_is_superspeed(c->cdev->gadget)) {
-		acm_ss_in_desc.bEndpointAddress =
-			acm_fs_in_desc.bEndpointAddress;
-		acm_ss_out_desc.bEndpointAddress =
-			acm_fs_out_desc.bEndpointAddress;
-
-		/* copy descriptors, and track endpoint copies */
-		f->ss_descriptors = usb_copy_descriptors(acm_ss_function);
-		if (!f->ss_descriptors)
-			goto fail;
-	}
-
-	DBG(cdev, "acm ttyGS%d: %s speed IN/%s OUT/%s NOTIFY/%s\n",
-			acm->port_num,
-			gadget_is_superspeed(c->cdev->gadget) ? "super" :
-			gadget_is_dualspeed(c->cdev->gadget) ? "dual" : "full",
-			acm->port.in->name, acm->port.out->name,
-			acm->notify->name);
-	return 0;
-
-fail:
-	if (acm->notify_req)
-		gs_free_req(acm->notify, acm->notify_req);
-
-	/* we might as well release our claims on endpoints */
-	if (acm->notify)
-		acm->notify->driver_data = NULL;
-	if (acm->port.out)
-		acm->port.out->driver_data = NULL;
-	if (acm->port.in)
-		acm->port.in->driver_data = NULL;
-
-	ERROR(cdev, "%s/%p: can't bind, err %d\n", f->name, f, status);
-
-	return status;
-}
-
-static void
-acm_unbind(struct usb_configuration *c, struct usb_function *f)
-{
-	struct f_acm		*acm = func_to_acm(f);
-
-	if (gadget_is_dualspeed(c->cdev->gadget))
-		usb_free_descriptors(f->hs_descriptors);
-	if (gadget_is_superspeed(c->cdev->gadget))
-		usb_free_descriptors(f->ss_descriptors);
-	usb_free_descriptors(f->descriptors);
-	gs_free_req(acm->notify, acm->notify_req);
-	kfree(acm);
-}
-
-/* Some controllers can't support CDC ACM ... */
-static inline bool can_support_cdc(struct usb_configuration *c)
-{
-	/* everything else is *probably* fine ... */
-	return true;
-}
-
-/**
- * acm_bind_config - add a CDC ACM function to a configuration
- * @c: the configuration to support the CDC ACM instance
- * @port_num: /dev/ttyGS* port this interface will use
- * Context: single threaded during gadget setup
- *
- * Returns zero on success, else negative errno.
- *
- * Caller must have called @gserial_setup() with enough ports to
- * handle all the ones it binds.  Caller is also responsible
- * for calling @gserial_cleanup() before module unload.
- */
-int acm_bind_config(struct usb_configuration *c, u8 port_num)
-{
-	struct f_acm	*acm;
-	int		status;
-
-	if (!can_support_cdc(c))
-		return -EINVAL;
-
-	/* REVISIT might want instance-specific strings to help
-	 * distinguish instances ...
-	 */
-
-	/* maybe allocate device-global string IDs, and patch descriptors */
-	if (acm_string_defs[ACM_CTRL_IDX].id == 0) {
-		status = usb_string_id(c->cdev);
-		if (status < 0)
-			return status;
-		acm_string_defs[ACM_CTRL_IDX].id = status;
-
-		acm_control_interface_desc.iInterface = status;
-
-		status = usb_string_id(c->cdev);
-		if (status < 0)
-			return status;
-		acm_string_defs[ACM_DATA_IDX].id = status;
-
-		acm_data_interface_desc.iInterface = status;
-
-		status = usb_string_id(c->cdev);
-		if (status < 0)
-			return status;
-		acm_string_defs[ACM_IAD_IDX].id = status;
-
-		acm_iad_descriptor.iFunction = status;
-	}
-
-	/* allocate and initialize one new instance */
-	acm = kzalloc(sizeof *acm, GFP_KERNEL);
-	if (!acm)
-		return -ENOMEM;
-
-	spin_lock_init(&acm->lock);
-
-	acm->port_num = port_num;
-
-	acm->port.connect = acm_connect;
-	acm->port.disconnect = acm_disconnect;
-	acm->port.send_break = acm_send_break;
-
-	acm->port.func.name = "acm";
-	acm->port.func.strings = acm_strings;
-	/* descriptors are per-instance copies */
-	acm->port.func.bind = acm_bind;
-	acm->port.func.unbind = acm_unbind;
-	acm->port.func.set_alt = acm_set_alt;
-	acm->port.func.setup = acm_setup;
-	acm->port.func.disable = acm_disable;
-
-	status = usb_add_function(c, &acm->port.func);
-	if (status)
-		kfree(acm);
-	return status;
-}
diff --git a/drivers/staging/ccg/f_fs.c b/drivers/staging/ccg/f_fs.c
deleted file mode 100644
index f6373dade7f..00000000000
--- a/drivers/staging/ccg/f_fs.c
+++ /dev/null
@@ -1,2456 +0,0 @@
-/*
- * f_fs.c -- user mode file system API for USB composite function controllers
- *
- * Copyright (C) 2010 Samsung Electronics
- * Author: Michal Nazarewicz <mina86@mina86.com>
- *
- * Based on inode.c (GadgetFS) which was:
- * Copyright (C) 2003-2004 David Brownell
- * Copyright (C) 2003 Agilent Technologies
- *
- * 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.
- */
-
-
-/* #define DEBUG */
-/* #define VERBOSE_DEBUG */
-
-#include <linux/blkdev.h>
-#include <linux/pagemap.h>
-#include <linux/export.h>
-#include <linux/hid.h>
-#include <asm/unaligned.h>
-
-#include <linux/usb/composite.h>
-#include <linux/usb/functionfs.h>
-
-
-#define FUNCTIONFS_MAGIC	0xa647361 /* Chosen by a honest dice roll ;) */
-
-
-/* Debugging ****************************************************************/
-
-#ifdef VERBOSE_DEBUG
-#  define pr_vdebug pr_debug
-#  define ffs_dump_mem(prefix, ptr, len) \
-	print_hex_dump_bytes(pr_fmt(prefix ": "), DUMP_PREFIX_NONE, ptr, len)
-#else
-#  define pr_vdebug(...)                 do { } while (0)
-#  define ffs_dump_mem(prefix, ptr, len) do { } while (0)
-#endif /* VERBOSE_DEBUG */
-
-#define ENTER()    pr_vdebug("%s()\n", __func__)
-
-
-/* The data structure and setup file ****************************************/
-
-enum ffs_state {
-	/*
-	 * Waiting for descriptors and strings.
-	 *
-	 * In this state no open(2), read(2) or write(2) on epfiles
-	 * may succeed (which should not be the problem as there
-	 * should be no such files opened in the first place).
-	 */
-	FFS_READ_DESCRIPTORS,
-	FFS_READ_STRINGS,
-
-	/*
-	 * We've got descriptors and strings.  We are or have called
-	 * functionfs_ready_callback().  functionfs_bind() may have
-	 * been called but we don't know.
-	 *
-	 * This is the only state in which operations on epfiles may
-	 * succeed.
-	 */
-	FFS_ACTIVE,
-
-	/*
-	 * All endpoints have been closed.  This state is also set if
-	 * we encounter an unrecoverable error.  The only
-	 * unrecoverable error is situation when after reading strings
-	 * from user space we fail to initialise epfiles or
-	 * functionfs_ready_callback() returns with error (<0).
-	 *
-	 * In this state no open(2), read(2) or write(2) (both on ep0
-	 * as well as epfile) may succeed (at this point epfiles are
-	 * unlinked and all closed so this is not a problem; ep0 is
-	 * also closed but ep0 file exists and so open(2) on ep0 must
-	 * fail).
-	 */
-	FFS_CLOSING
-};
-
-
-enum ffs_setup_state {
-	/* There is no setup request pending. */
-	FFS_NO_SETUP,
-	/*
-	 * User has read events and there was a setup request event
-	 * there.  The next read/write on ep0 will handle the
-	 * request.
-	 */
-	FFS_SETUP_PENDING,
-	/*
-	 * There was event pending but before user space handled it
-	 * some other event was introduced which canceled existing
-	 * setup.  If this state is set read/write on ep0 return
-	 * -EIDRM.  This state is only set when adding event.
-	 */
-	FFS_SETUP_CANCELED
-};
-
-
-
-struct ffs_epfile;
-struct ffs_function;
-
-struct ffs_data {
-	struct usb_gadget		*gadget;
-
-	/*
-	 * Protect access read/write operations, only one read/write
-	 * at a time.  As a consequence protects ep0req and company.
-	 * While setup request is being processed (queued) this is
-	 * held.
-	 */
-	struct mutex			mutex;
-
-	/*
-	 * Protect access to endpoint related structures (basically
-	 * usb_ep_queue(), usb_ep_dequeue(), etc. calls) except for
-	 * endpoint zero.
-	 */
-	spinlock_t			eps_lock;
-
-	/*
-	 * XXX REVISIT do we need our own request? Since we are not
-	 * handling setup requests immediately user space may be so
-	 * slow that another setup will be sent to the gadget but this
-	 * time not to us but another function and then there could be
-	 * a race.  Is that the case? Or maybe we can use cdev->req
-	 * after all, maybe we just need some spinlock for that?
-	 */
-	struct usb_request		*ep0req;		/* P: mutex */
-	struct completion		ep0req_completion;	/* P: mutex */
-	int				ep0req_status;		/* P: mutex */
-
-	/* reference counter */
-	atomic_t			ref;
-	/* how many files are opened (EP0 and others) */
-	atomic_t			opened;
-
-	/* EP0 state */
-	enum ffs_state			state;
-
-	/*
-	 * Possible transitions:
-	 * + FFS_NO_SETUP       -> FFS_SETUP_PENDING  -- P: ev.waitq.lock
-	 *               happens only in ep0 read which is P: mutex
-	 * + FFS_SETUP_PENDING  -> FFS_NO_SETUP       -- P: ev.waitq.lock
-	 *               happens only in ep0 i/o  which is P: mutex
-	 * + FFS_SETUP_PENDING  -> FFS_SETUP_CANCELED -- P: ev.waitq.lock
-	 * + FFS_SETUP_CANCELED -> FFS_NO_SETUP       -- cmpxchg
-	 */
-	enum ffs_setup_state		setup_state;
-
-#define FFS_SETUP_STATE(ffs)					\
-	((enum ffs_setup_state)cmpxchg(&(ffs)->setup_state,	\
-				       FFS_SETUP_CANCELED, FFS_NO_SETUP))
-
-	/* Events & such. */
-	struct {
-		u8				types[4];
-		unsigned short			count;
-		/* XXX REVISIT need to update it in some places, or do we? */
-		unsigned short			can_stall;
-		struct usb_ctrlrequest		setup;
-
-		wait_queue_head_t		waitq;
-	} ev; /* the whole structure, P: ev.waitq.lock */
-
-	/* Flags */
-	unsigned long			flags;
-#define FFS_FL_CALL_CLOSED_CALLBACK 0
-#define FFS_FL_BOUND                1
-
-	/* Active function */
-	struct ffs_function		*func;
-
-	/*
-	 * Device name, write once when file system is mounted.
-	 * Intended for user to read if she wants.
-	 */
-	const char			*dev_name;
-	/* Private data for our user (ie. gadget).  Managed by user. */
-	void				*private_data;
-
-	/* filled by __ffs_data_got_descs() */
-	/*
-	 * Real descriptors are 16 bytes after raw_descs (so you need
-	 * to skip 16 bytes (ie. ffs->raw_descs + 16) to get to the
-	 * first full speed descriptor).  raw_descs_length and
-	 * raw_fs_descs_length do not have those 16 bytes added.
-	 */
-	const void			*raw_descs;
-	unsigned			raw_descs_length;
-	unsigned			raw_fs_descs_length;
-	unsigned			fs_descs_count;
-	unsigned			hs_descs_count;
-
-	unsigned short			strings_count;
-	unsigned short			interfaces_count;
-	unsigned short			eps_count;
-	unsigned short			_pad1;
-
-	/* filled by __ffs_data_got_strings() */
-	/* ids in stringtabs are set in functionfs_bind() */
-	const void			*raw_strings;
-	struct usb_gadget_strings	**stringtabs;
-
-	/*
-	 * File system's super block, write once when file system is
-	 * mounted.
-	 */
-	struct super_block		*sb;
-
-	/* File permissions, written once when fs is mounted */
-	struct ffs_file_perms {
-		umode_t				mode;
-		uid_t				uid;
-		gid_t				gid;
-	}				file_perms;
-
-	/*
-	 * The endpoint files, filled by ffs_epfiles_create(),
-	 * destroyed by ffs_epfiles_destroy().
-	 */
-	struct ffs_epfile		*epfiles;
-};
-
-/* Reference counter handling */
-static void ffs_data_get(struct ffs_data *ffs);
-static void ffs_data_put(struct ffs_data *ffs);
-/* Creates new ffs_data object. */
-static struct ffs_data *__must_check ffs_data_new(void) __attribute__((malloc));
-
-/* Opened counter handling. */
-static void ffs_data_opened(struct ffs_data *ffs);
-static void ffs_data_closed(struct ffs_data *ffs);
-
-/* Called with ffs->mutex held; take over ownership of data. */
-static int __must_check
-__ffs_data_got_descs(struct ffs_data *ffs, char *data, size_t len);
-static int __must_check
-__ffs_data_got_strings(struct ffs_data *ffs, char *data, size_t len);
-
-
-/* The function structure ***************************************************/
-
-struct ffs_ep;
-
-struct ffs_function {
-	struct usb_configuration	*conf;
-	struct usb_gadget		*gadget;
-	struct ffs_data			*ffs;
-
-	struct ffs_ep			*eps;
-	u8				eps_revmap[16];
-	short				*interfaces_nums;
-
-	struct usb_function		function;
-};
-
-
-static struct ffs_function *ffs_func_from_usb(struct usb_function *f)
-{
-	return container_of(f, struct ffs_function, function);
-}
-
-static void ffs_func_free(struct ffs_function *func);
-
-static void ffs_func_eps_disable(struct ffs_function *func);
-static int __must_check ffs_func_eps_enable(struct ffs_function *func);
-
-static int ffs_func_bind(struct usb_configuration *,
-			 struct usb_function *);
-static void ffs_func_unbind(struct usb_configuration *,
-			    struct usb_function *);
-static int ffs_func_set_alt(struct usb_function *, unsigned, unsigned);
-static void ffs_func_disable(struct usb_function *);
-static int ffs_func_setup(struct usb_function *,
-			  const struct usb_ctrlrequest *);
-static void ffs_func_suspend(struct usb_function *);
-static void ffs_func_resume(struct usb_function *);
-
-
-static int ffs_func_revmap_ep(struct ffs_function *func, u8 num);
-static int ffs_func_revmap_intf(struct ffs_function *func, u8 intf);
-
-
-/* The endpoints structures *************************************************/
-
-struct ffs_ep {
-	struct usb_ep			*ep;	/* P: ffs->eps_lock */
-	struct usb_request		*req;	/* P: epfile->mutex */
-
-	/* [0]: full speed, [1]: high speed */
-	struct usb_endpoint_descriptor	*descs[2];
-
-	u8				num;
-
-	int				status;	/* P: epfile->mutex */
-};
-
-struct ffs_epfile {
-	/* Protects ep->ep and ep->req. */
-	struct mutex			mutex;
-	wait_queue_head_t		wait;
-
-	struct ffs_data			*ffs;
-	struct ffs_ep			*ep;	/* P: ffs->eps_lock */
-
-	struct dentry			*dentry;
-
-	char				name[5];
-
-	unsigned char			in;	/* P: ffs->eps_lock */
-	unsigned char			isoc;	/* P: ffs->eps_lock */
-
-	unsigned char			_pad;
-};
-
-static int  __must_check ffs_epfiles_create(struct ffs_data *ffs);
-static void ffs_epfiles_destroy(struct ffs_epfile *epfiles, unsigned count);
-
-static struct inode *__must_check
-ffs_sb_create_file(struct super_block *sb, const char *name, void *data,
-		   const struct file_operations *fops,
-		   struct dentry **dentry_p);
-
-
-/* Misc helper functions ****************************************************/
-
-static int ffs_mutex_lock(struct mutex *mutex, unsigned nonblock)
-	__attribute__((warn_unused_result, nonnull));
-static char *ffs_prepare_buffer(const char * __user buf, size_t len)
-	__attribute__((warn_unused_result, nonnull));
-
-
-/* Control file aka ep0 *****************************************************/
-
-static void ffs_ep0_complete(struct usb_ep *ep, struct usb_request *req)
-{
-	struct ffs_data *ffs = req->context;
-
-	complete_all(&ffs->ep0req_completion);
-}
-
-static int __ffs_ep0_queue_wait(struct ffs_data *ffs, char *data, size_t len)
-{
-	struct usb_request *req = ffs->ep0req;
-	int ret;
-
-	req->zero     = len < le16_to_cpu(ffs->ev.setup.wLength);
-
-	spin_unlock_irq(&ffs->ev.waitq.lock);
-
-	req->buf      = data;
-	req->length   = len;
-
-	/*
-	 * UDC layer requires to provide a buffer even for ZLP, but should
-	 * not use it at all. Let's provide some poisoned pointer to catch
-	 * possible bug in the driver.
-	 */
-	if (req->buf == NULL)
-		req->buf = (void *)0xDEADBABE;
-
-	INIT_COMPLETION(ffs->ep0req_completion);
-
-	ret = usb_ep_queue(ffs->gadget->ep0, req, GFP_ATOMIC);
-	if (unlikely(ret < 0))
-		return ret;
-
-	ret = wait_for_completion_interruptible(&ffs->ep0req_completion);
-	if (unlikely(ret)) {
-		usb_ep_dequeue(ffs->gadget->ep0, req);
-		return -EINTR;
-	}
-
-	ffs->setup_state = FFS_NO_SETUP;
-	return ffs->ep0req_status;
-}
-
-static int __ffs_ep0_stall(struct ffs_data *ffs)
-{
-	if (ffs->ev.can_stall) {
-		pr_vdebug("ep0 stall\n");
-		usb_ep_set_halt(ffs->gadget->ep0);
-		ffs->setup_state = FFS_NO_SETUP;
-		return -EL2HLT;
-	} else {
-		pr_debug("bogus ep0 stall!\n");
-		return -ESRCH;
-	}
-}
-
-static ssize_t ffs_ep0_write(struct file *file, const char __user *buf,
-			     size_t len, loff_t *ptr)
-{
-	struct ffs_data *ffs = file->private_data;
-	ssize_t ret;
-	char *data;
-
-	ENTER();
-
-	/* Fast check if setup was canceled */
-	if (FFS_SETUP_STATE(ffs) == FFS_SETUP_CANCELED)
-		return -EIDRM;
-
-	/* Acquire mutex */
-	ret = ffs_mutex_lock(&ffs->mutex, file->f_flags & O_NONBLOCK);
-	if (unlikely(ret < 0))
-		return ret;
-
-	/* Check state */
-	switch (ffs->state) {
-	case FFS_READ_DESCRIPTORS:
-	case FFS_READ_STRINGS:
-		/* Copy data */
-		if (unlikely(len < 16)) {
-			ret = -EINVAL;
-			break;
-		}
-
-		data = ffs_prepare_buffer(buf, len);
-		if (IS_ERR(data)) {
-			ret = PTR_ERR(data);
-			break;
-		}
-
-		/* Handle data */
-		if (ffs->state == FFS_READ_DESCRIPTORS) {
-			pr_info("read descriptors\n");
-			ret = __ffs_data_got_descs(ffs, data, len);
-			if (unlikely(ret < 0))
-				break;
-
-			ffs->state = FFS_READ_STRINGS;
-			ret = len;
-		} else {
-			pr_info("read strings\n");
-			ret = __ffs_data_got_strings(ffs, data, len);
-			if (unlikely(ret < 0))
-				break;
-
-			ret = ffs_epfiles_create(ffs);
-			if (unlikely(ret)) {
-				ffs->state = FFS_CLOSING;
-				break;
-			}
-
-			ffs->state = FFS_ACTIVE;
-			mutex_unlock(&ffs->mutex);
-
-			ret = functionfs_ready_callback(ffs);
-			if (unlikely(ret < 0)) {
-				ffs->state = FFS_CLOSING;
-				return ret;
-			}
-
-			set_bit(FFS_FL_CALL_CLOSED_CALLBACK, &ffs->flags);
-			return len;
-		}
-		break;
-
-	case FFS_ACTIVE:
-		data = NULL;
-		/*
-		 * We're called from user space, we can use _irq
-		 * rather then _irqsave
-		 */
-		spin_lock_irq(&ffs->ev.waitq.lock);
-		switch (FFS_SETUP_STATE(ffs)) {
-		case FFS_SETUP_CANCELED:
-			ret = -EIDRM;
-			goto done_spin;
-
-		case FFS_NO_SETUP:
-			ret = -ESRCH;
-			goto done_spin;
-
-		case FFS_SETUP_PENDING:
-			break;
-		}
-
-		/* FFS_SETUP_PENDING */
-		if (!(ffs->ev.setup.bRequestType & USB_DIR_IN)) {
-			spin_unlock_irq(&ffs->ev.waitq.lock);
-			ret = __ffs_ep0_stall(ffs);
-			break;
-		}
-
-		/* FFS_SETUP_PENDING and not stall */
-		len = min(len, (size_t)le16_to_cpu(ffs->ev.setup.wLength));
-
-		spin_unlock_irq(&ffs->ev.waitq.lock);
-
-		data = ffs_prepare_buffer(buf, len);
-		if (IS_ERR(data)) {
-			ret = PTR_ERR(data);
-			break;
-		}
-
-		spin_lock_irq(&ffs->ev.waitq.lock);
-
-		/*
-		 * We are guaranteed to be still in FFS_ACTIVE state
-		 * but the state of setup could have changed from
-		 * FFS_SETUP_PENDING to FFS_SETUP_CANCELED so we need
-		 * to check for that.  If that happened we copied data
-		 * from user space in vain but it's unlikely.
-		 *
-		 * For sure we are not in FFS_NO_SETUP since this is
-		 * the only place FFS_SETUP_PENDING -> FFS_NO_SETUP
-		 * transition can be performed and it's protected by
-		 * mutex.
-		 */
-		if (FFS_SETUP_STATE(ffs) == FFS_SETUP_CANCELED) {
-			ret = -EIDRM;
-done_spin:
-			spin_unlock_irq(&ffs->ev.waitq.lock);
-		} else {
-			/* unlocks spinlock */
-			ret = __ffs_ep0_queue_wait(ffs, data, len);
-		}
-		kfree(data);
-		break;
-
-	default:
-		ret = -EBADFD;
-		break;
-	}
-
-	mutex_unlock(&ffs->mutex);
-	return ret;
-}
-
-static ssize_t __ffs_ep0_read_events(struct ffs_data *ffs, char __user *buf,
-				     size_t n)
-{
-	/*
-	 * We are holding ffs->ev.waitq.lock and ffs->mutex and we need
-	 * to release them.
-	 */
-	struct usb_functionfs_event events[n];
-	unsigned i = 0;
-
-	memset(events, 0, sizeof events);
-
-	do {
-		events[i].type = ffs->ev.types[i];
-		if (events[i].type == FUNCTIONFS_SETUP) {
-			events[i].u.setup = ffs->ev.setup;
-			ffs->setup_state = FFS_SETUP_PENDING;
-		}
-	} while (++i < n);
-
-	if (n < ffs->ev.count) {
-		ffs->ev.count -= n;
-		memmove(ffs->ev.types, ffs->ev.types + n,
-			ffs->ev.count * sizeof *ffs->ev.types);
-	} else {
-		ffs->ev.count = 0;
-	}
-
-	spin_unlock_irq(&ffs->ev.waitq.lock);
-	mutex_unlock(&ffs->mutex);
-
-	return unlikely(__copy_to_user(buf, events, sizeof events))
-		? -EFAULT : sizeof events;
-}
-
-static ssize_t ffs_ep0_read(struct file *file, char __user *buf,
-			    size_t len, loff_t *ptr)
-{
-	struct ffs_data *ffs = file->private_data;
-	char *data = NULL;
-	size_t n;
-	int ret;
-
-	ENTER();
-
-	/* Fast check if setup was canceled */
-	if (FFS_SETUP_STATE(ffs) == FFS_SETUP_CANCELED)
-		return -EIDRM;
-
-	/* Acquire mutex */
-	ret = ffs_mutex_lock(&ffs->mutex, file->f_flags & O_NONBLOCK);
-	if (unlikely(ret < 0))
-		return ret;
-
-	/* Check state */
-	if (ffs->state != FFS_ACTIVE) {
-		ret = -EBADFD;
-		goto done_mutex;
-	}
-
-	/*
-	 * We're called from user space, we can use _irq rather then
-	 * _irqsave
-	 */
-	spin_lock_irq(&ffs->ev.waitq.lock);
-
-	switch (FFS_SETUP_STATE(ffs)) {
-	case FFS_SETUP_CANCELED:
-		ret = -EIDRM;
-		break;
-
-	case FFS_NO_SETUP:
-		n = len / sizeof(struct usb_functionfs_event);
-		if (unlikely(!n)) {
-			ret = -EINVAL;
-			break;
-		}
-
-		if ((file->f_flags & O_NONBLOCK) && !ffs->ev.count) {
-			ret = -EAGAIN;
-			break;
-		}
-
-		if (wait_event_interruptible_exclusive_locked_irq(ffs->ev.waitq,
-							ffs->ev.count)) {
-			ret = -EINTR;
-			break;
-		}
-
-		return __ffs_ep0_read_events(ffs, buf,
-					     min(n, (size_t)ffs->ev.count));
-
-	case FFS_SETUP_PENDING:
-		if (ffs->ev.setup.bRequestType & USB_DIR_IN) {
-			spin_unlock_irq(&ffs->ev.waitq.lock);
-			ret = __ffs_ep0_stall(ffs);
-			goto done_mutex;
-		}
-
-		len = min(len, (size_t)le16_to_cpu(ffs->ev.setup.wLength));
-
-		spin_unlock_irq(&ffs->ev.waitq.lock);
-
-		if (likely(len)) {
-			data = kmalloc(len, GFP_KERNEL);
-			if (unlikely(!data)) {
-				ret = -ENOMEM;
-				goto done_mutex;
-			}
-		}
-
-		spin_lock_irq(&ffs->ev.waitq.lock);
-
-		/* See ffs_ep0_write() */
-		if (FFS_SETUP_STATE(ffs) == FFS_SETUP_CANCELED) {
-			ret = -EIDRM;
-			break;
-		}
-
-		/* unlocks spinlock */
-		ret = __ffs_ep0_queue_wait(ffs, data, len);
-		if (likely(ret > 0) && unlikely(__copy_to_user(buf, data, len)))
-			ret = -EFAULT;
-		goto done_mutex;
-
-	default:
-		ret = -EBADFD;
-		break;
-	}
-
-	spin_unlock_irq(&ffs->ev.waitq.lock);
-done_mutex:
-	mutex_unlock(&ffs->mutex);
-	kfree(data);
-	return ret;
-}
-
-static int ffs_ep0_open(struct inode *inode, struct file *file)
-{
-	struct ffs_data *ffs = inode->i_private;
-
-	ENTER();
-
-	if (unlikely(ffs->state == FFS_CLOSING))
-		return -EBUSY;
-
-	file->private_data = ffs;
-	ffs_data_opened(ffs);
-
-	return 0;
-}
-
-static int ffs_ep0_release(struct inode *inode, struct file *file)
-{
-	struct ffs_data *ffs = file->private_data;
-
-	ENTER();
-
-	ffs_data_closed(ffs);
-
-	return 0;
-}
-
-static long ffs_ep0_ioctl(struct file *file, unsigned code, unsigned long value)
-{
-	struct ffs_data *ffs = file->private_data;
-	struct usb_gadget *gadget = ffs->gadget;
-	long ret;
-
-	ENTER();
-
-	if (code == FUNCTIONFS_INTERFACE_REVMAP) {
-		struct ffs_function *func = ffs->func;
-		ret = func ? ffs_func_revmap_intf(func, value) : -ENODEV;
-	} else if (gadget && gadget->ops->ioctl) {
-		ret = gadget->ops->ioctl(gadget, code, value);
-	} else {
-		ret = -ENOTTY;
-	}
-
-	return ret;
-}
-
-static const struct file_operations ffs_ep0_operations = {
-	.owner =	THIS_MODULE,
-	.llseek =	no_llseek,
-
-	.open =		ffs_ep0_open,
-	.write =	ffs_ep0_write,
-	.read =		ffs_ep0_read,
-	.release =	ffs_ep0_release,
-	.unlocked_ioctl =	ffs_ep0_ioctl,
-};
-
-
-/* "Normal" endpoints operations ********************************************/
-
-static void ffs_epfile_io_complete(struct usb_ep *_ep, struct usb_request *req)
-{
-	ENTER();
-	if (likely(req->context)) {
-		struct ffs_ep *ep = _ep->driver_data;
-		ep->status = req->status ? req->status : req->actual;
-		complete(req->context);
-	}
-}
-
-static ssize_t ffs_epfile_io(struct file *file,
-			     char __user *buf, size_t len, int read)
-{
-	struct ffs_epfile *epfile = file->private_data;
-	struct ffs_ep *ep;
-	char *data = NULL;
-	ssize_t ret;
-	int halt;
-
-	goto first_try;
-	do {
-		spin_unlock_irq(&epfile->ffs->eps_lock);
-		mutex_unlock(&epfile->mutex);
-
-first_try:
-		/* Are we still active? */
-		if (WARN_ON(epfile->ffs->state != FFS_ACTIVE)) {
-			ret = -ENODEV;
-			goto error;
-		}
-
-		/* Wait for endpoint to be enabled */
-		ep = epfile->ep;
-		if (!ep) {
-			if (file->f_flags & O_NONBLOCK) {
-				ret = -EAGAIN;
-				goto error;
-			}
-
-			if (wait_event_interruptible(epfile->wait,
-						     (ep = epfile->ep))) {
-				ret = -EINTR;
-				goto error;
-			}
-		}
-
-		/* Do we halt? */
-		halt = !read == !epfile->in;
-		if (halt && epfile->isoc) {
-			ret = -EINVAL;
-			goto error;
-		}
-
-		/* Allocate & copy */
-		if (!halt && !data) {
-			data = kzalloc(len, GFP_KERNEL);
-			if (unlikely(!data))
-				return -ENOMEM;
-
-			if (!read &&
-			    unlikely(__copy_from_user(data, buf, len))) {
-				ret = -EFAULT;
-				goto error;
-			}
-		}
-
-		/* We will be using request */
-		ret = ffs_mutex_lock(&epfile->mutex,
-				     file->f_flags & O_NONBLOCK);
-		if (unlikely(ret))
-			goto error;
-
-		/*
-		 * We're called from user space, we can use _irq rather then
-		 * _irqsave
-		 */
-		spin_lock_irq(&epfile->ffs->eps_lock);
-
-		/*
-		 * While we were acquiring mutex endpoint got disabled
-		 * or changed?
-		 */
-	} while (unlikely(epfile->ep != ep));
-
-	/* Halt */
-	if (unlikely(halt)) {
-		if (likely(epfile->ep == ep) && !WARN_ON(!ep->ep))
-			usb_ep_set_halt(ep->ep);
-		spin_unlock_irq(&epfile->ffs->eps_lock);
-		ret = -EBADMSG;
-	} else {
-		/* Fire the request */
-		DECLARE_COMPLETION_ONSTACK(done);
-
-		struct usb_request *req = ep->req;
-		req->context  = &done;
-		req->complete = ffs_epfile_io_complete;
-		req->buf      = data;
-		req->length   = len;
-
-		ret = usb_ep_queue(ep->ep, req, GFP_ATOMIC);
-
-		spin_unlock_irq(&epfile->ffs->eps_lock);
-
-		if (unlikely(ret < 0)) {
-			/* nop */
-		} else if (unlikely(wait_for_completion_interruptible(&done))) {
-			ret = -EINTR;
-			usb_ep_dequeue(ep->ep, req);
-		} else {
-			ret = ep->status;
-			if (read && ret > 0 &&
-			    unlikely(copy_to_user(buf, data, ret)))
-				ret = -EFAULT;
-		}
-	}
-
-	mutex_unlock(&epfile->mutex);
-error:
-	kfree(data);
-	return ret;
-}
-
-static ssize_t
-ffs_epfile_write(struct file *file, const char __user *buf, size_t len,
-		 loff_t *ptr)
-{
-	ENTER();
-
-	return ffs_epfile_io(file, (char __user *)buf, len, 0);
-}
-
-static ssize_t
-ffs_epfile_read(struct file *file, char __user *buf, size_t len, loff_t *ptr)
-{
-	ENTER();
-
-	return ffs_epfile_io(file, buf, len, 1);
-}
-
-static int
-ffs_epfile_open(struct inode *inode, struct file *file)
-{
-	struct ffs_epfile *epfile = inode->i_private;
-
-	ENTER();
-
-	if (WARN_ON(epfile->ffs->state != FFS_ACTIVE))
-		return -ENODEV;
-
-	file->private_data = epfile;
-	ffs_data_opened(epfile->ffs);
-
-	return 0;
-}
-
-static int
-ffs_epfile_release(struct inode *inode, struct file *file)
-{
-	struct ffs_epfile *epfile = inode->i_private;
-
-	ENTER();
-
-	ffs_data_closed(epfile->ffs);
-
-	return 0;
-}
-
-static long ffs_epfile_ioctl(struct file *file, unsigned code,
-			     unsigned long value)
-{
-	struct ffs_epfile *epfile = file->private_data;
-	int ret;
-
-	ENTER();
-
-	if (WARN_ON(epfile->ffs->state != FFS_ACTIVE))
-		return -ENODEV;
-
-	spin_lock_irq(&epfile->ffs->eps_lock);
-	if (likely(epfile->ep)) {
-		switch (code) {
-		case FUNCTIONFS_FIFO_STATUS:
-			ret = usb_ep_fifo_status(epfile->ep->ep);
-			break;
-		case FUNCTIONFS_FIFO_FLUSH:
-			usb_ep_fifo_flush(epfile->ep->ep);
-			ret = 0;
-			break;
-		case FUNCTIONFS_CLEAR_HALT:
-			ret = usb_ep_clear_halt(epfile->ep->ep);
-			break;
-		case FUNCTIONFS_ENDPOINT_REVMAP:
-			ret = epfile->ep->num;
-			break;
-		default:
-			ret = -ENOTTY;
-		}
-	} else {
-		ret = -ENODEV;
-	}
-	spin_unlock_irq(&epfile->ffs->eps_lock);
-
-	return ret;
-}
-
-static const struct file_operations ffs_epfile_operations = {
-	.owner =	THIS_MODULE,
-	.llseek =	no_llseek,
-
-	.open =		ffs_epfile_open,
-	.write =	ffs_epfile_write,
-	.read =		ffs_epfile_read,
-	.release =	ffs_epfile_release,
-	.unlocked_ioctl =	ffs_epfile_ioctl,
-};
-
-
-/* File system and super block operations ***********************************/
-
-/*
- * Mounting the file system creates a controller file, used first for
- * function configuration then later for event monitoring.
- */
-
-static struct inode *__must_check
-ffs_sb_make_inode(struct super_block *sb, void *data,
-		  const struct file_operations *fops,
-		  const struct inode_operations *iops,
-		  struct ffs_file_perms *perms)
-{
-	struct inode *inode;
-
-	ENTER();
-
-	inode = new_inode(sb);
-
-	if (likely(inode)) {
-		struct timespec current_time = CURRENT_TIME;
-
-		inode->i_ino	 = get_next_ino();
-		inode->i_mode    = perms->mode;
-		inode->i_uid     = perms->uid;
-		inode->i_gid     = perms->gid;
-		inode->i_atime   = current_time;
-		inode->i_mtime   = current_time;
-		inode->i_ctime   = current_time;
-		inode->i_private = data;
-		if (fops)
-			inode->i_fop = fops;
-		if (iops)
-			inode->i_op  = iops;
-	}
-
-	return inode;
-}
-
-/* Create "regular" file */
-static struct inode *ffs_sb_create_file(struct super_block *sb,
-					const char *name, void *data,
-					const struct file_operations *fops,
-					struct dentry **dentry_p)
-{
-	struct ffs_data	*ffs = sb->s_fs_info;
-	struct dentry	*dentry;
-	struct inode	*inode;
-
-	ENTER();
-
-	dentry = d_alloc_name(sb->s_root, name);
-	if (unlikely(!dentry))
-		return NULL;
-
-	inode = ffs_sb_make_inode(sb, data, fops, NULL, &ffs->file_perms);
-	if (unlikely(!inode)) {
-		dput(dentry);
-		return NULL;
-	}
-
-	d_add(dentry, inode);
-	if (dentry_p)
-		*dentry_p = dentry;
-
-	return inode;
-}
-
-/* Super block */
-static const struct super_operations ffs_sb_operations = {
-	.statfs =	simple_statfs,
-	.drop_inode =	generic_delete_inode,
-};
-
-struct ffs_sb_fill_data {
-	struct ffs_file_perms perms;
-	umode_t root_mode;
-	const char *dev_name;
-	union {
-		/* set by ffs_fs_mount(), read by ffs_sb_fill() */
-		void *private_data;
-		/* set by ffs_sb_fill(), read by ffs_fs_mount */
-		struct ffs_data *ffs_data;
-	};
-};
-
-static int ffs_sb_fill(struct super_block *sb, void *_data, int silent)
-{
-	struct ffs_sb_fill_data *data = _data;
-	struct inode	*inode;
-	struct ffs_data	*ffs;
-
-	ENTER();
-
-	/* Initialise data */
-	ffs = ffs_data_new();
-	if (unlikely(!ffs))
-		goto Enomem;
-
-	ffs->sb              = sb;
-	ffs->dev_name        = kstrdup(data->dev_name, GFP_KERNEL);
-	if (unlikely(!ffs->dev_name))
-		goto Enomem;
-	ffs->file_perms      = data->perms;
-	ffs->private_data    = data->private_data;
-
-	/* used by the caller of this function */
-	data->ffs_data       = ffs;
-
-	sb->s_fs_info        = ffs;
-	sb->s_blocksize      = PAGE_CACHE_SIZE;
-	sb->s_blocksize_bits = PAGE_CACHE_SHIFT;
-	sb->s_magic          = FUNCTIONFS_MAGIC;
-	sb->s_op             = &ffs_sb_operations;
-	sb->s_time_gran      = 1;
-
-	/* Root inode */
-	data->perms.mode = data->root_mode;
-	inode = ffs_sb_make_inode(sb, NULL,
-				  &simple_dir_operations,
-				  &simple_dir_inode_operations,
-				  &data->perms);
-	sb->s_root = d_make_root(inode);
-	if (unlikely(!sb->s_root))
-		goto Enomem;
-
-	/* EP0 file */
-	if (unlikely(!ffs_sb_create_file(sb, "ep0", ffs,
-					 &ffs_ep0_operations, NULL)))
-		goto Enomem;
-
-	return 0;
-
-Enomem:
-	return -ENOMEM;
-}
-
-static int ffs_fs_parse_opts(struct ffs_sb_fill_data *data, char *opts)
-{
-	ENTER();
-
-	if (!opts || !*opts)
-		return 0;
-
-	for (;;) {
-		char *end, *eq, *comma;
-		unsigned long value;
-
-		/* Option limit */
-		comma = strchr(opts, ',');
-		if (comma)
-			*comma = 0;
-
-		/* Value limit */
-		eq = strchr(opts, '=');
-		if (unlikely(!eq)) {
-			pr_err("'=' missing in %s\n", opts);
-			return -EINVAL;
-		}
-		*eq = 0;
-
-		/* Parse value */
-		value = simple_strtoul(eq + 1, &end, 0);
-		if (unlikely(*end != ',' && *end != 0)) {
-			pr_err("%s: invalid value: %s\n", opts, eq + 1);
-			return -EINVAL;
-		}
-
-		/* Interpret option */
-		switch (eq - opts) {
-		case 5:
-			if (!memcmp(opts, "rmode", 5))
-				data->root_mode  = (value & 0555) | S_IFDIR;
-			else if (!memcmp(opts, "fmode", 5))
-				data->perms.mode = (value & 0666) | S_IFREG;
-			else
-				goto invalid;
-			break;
-
-		case 4:
-			if (!memcmp(opts, "mode", 4)) {
-				data->root_mode  = (value & 0555) | S_IFDIR;
-				data->perms.mode = (value & 0666) | S_IFREG;
-			} else {
-				goto invalid;
-			}
-			break;
-
-		case 3:
-			if (!memcmp(opts, "uid", 3))
-				data->perms.uid = value;
-			else if (!memcmp(opts, "gid", 3))
-				data->perms.gid = value;
-			else
-				goto invalid;
-			break;
-
-		default:
-invalid:
-			pr_err("%s: invalid option\n", opts);
-			return -EINVAL;
-		}
-
-		/* Next iteration */
-		if (!comma)
-			break;
-		opts = comma + 1;
-	}
-
-	return 0;
-}
-
-/* "mount -t functionfs dev_name /dev/function" ends up here */
-
-static struct dentry *
-ffs_fs_mount(struct file_system_type *t, int flags,
-	      const char *dev_name, void *opts)
-{
-	struct ffs_sb_fill_data data = {
-		.perms = {
-			.mode = S_IFREG | 0600,
-			.uid = 0,
-			.gid = 0
-		},
-		.root_mode = S_IFDIR | 0500,
-	};
-	struct dentry *rv;
-	int ret;
-	void *ffs_dev;
-
-	ENTER();
-
-	ret = ffs_fs_parse_opts(&data, opts);
-	if (unlikely(ret < 0))
-		return ERR_PTR(ret);
-
-	ffs_dev = functionfs_acquire_dev_callback(dev_name);
-	if (IS_ERR(ffs_dev))
-		return ffs_dev;
-
-	data.dev_name = dev_name;
-	data.private_data = ffs_dev;
-	rv = mount_nodev(t, flags, &data, ffs_sb_fill);
-
-	/* data.ffs_data is set by ffs_sb_fill */
-	if (IS_ERR(rv))
-		functionfs_release_dev_callback(data.ffs_data);
-
-	return rv;
-}
-
-static void
-ffs_fs_kill_sb(struct super_block *sb)
-{
-	ENTER();
-
-	kill_litter_super(sb);
-	if (sb->s_fs_info) {
-		functionfs_release_dev_callback(sb->s_fs_info);
-		ffs_data_put(sb->s_fs_info);
-	}
-}
-
-static struct file_system_type ffs_fs_type = {
-	.owner		= THIS_MODULE,
-	.name		= "functionfs",
-	.mount		= ffs_fs_mount,
-	.kill_sb	= ffs_fs_kill_sb,
-};
-MODULE_ALIAS_FS("functionfs");
-
-
-/* Driver's main init/cleanup functions *************************************/
-
-static int functionfs_init(void)
-{
-	int ret;
-
-	ENTER();
-
-	ret = register_filesystem(&ffs_fs_type);
-	if (likely(!ret))
-		pr_info("file system registered\n");
-	else
-		pr_err("failed registering file system (%d)\n", ret);
-
-	return ret;
-}
-
-static void functionfs_cleanup(void)
-{
-	ENTER();
-
-	pr_info("unloading\n");
-	unregister_filesystem(&ffs_fs_type);
-}
-
-
-/* ffs_data and ffs_function construction and destruction code **************/
-
-static void ffs_data_clear(struct ffs_data *ffs);
-static void ffs_data_reset(struct ffs_data *ffs);
-
-static void ffs_data_get(struct ffs_data *ffs)
-{
-	ENTER();
-
-	atomic_inc(&ffs->ref);
-}
-
-static void ffs_data_opened(struct ffs_data *ffs)
-{
-	ENTER();
-
-	atomic_inc(&ffs->ref);
-	atomic_inc(&ffs->opened);
-}
-
-static void ffs_data_put(struct ffs_data *ffs)
-{
-	ENTER();
-
-	if (unlikely(atomic_dec_and_test(&ffs->ref))) {
-		pr_info("%s(): freeing\n", __func__);
-		ffs_data_clear(ffs);
-		BUG_ON(waitqueue_active(&ffs->ev.waitq) ||
-		       waitqueue_active(&ffs->ep0req_completion.wait));
-		kfree(ffs->dev_name);
-		kfree(ffs);
-	}
-}
-
-static void ffs_data_closed(struct ffs_data *ffs)
-{
-	ENTER();
-
-	if (atomic_dec_and_test(&ffs->opened)) {
-		ffs->state = FFS_CLOSING;
-		ffs_data_reset(ffs);
-	}
-
-	ffs_data_put(ffs);
-}
-
-static struct ffs_data *ffs_data_new(void)
-{
-	struct ffs_data *ffs = kzalloc(sizeof *ffs, GFP_KERNEL);
-	if (unlikely(!ffs))
-		return 0;
-
-	ENTER();
-
-	atomic_set(&ffs->ref, 1);
-	atomic_set(&ffs->opened, 0);
-	ffs->state = FFS_READ_DESCRIPTORS;
-	mutex_init(&ffs->mutex);
-	spin_lock_init(&ffs->eps_lock);
-	init_waitqueue_head(&ffs->ev.waitq);
-	init_completion(&ffs->ep0req_completion);
-
-	/* XXX REVISIT need to update it in some places, or do we? */
-	ffs->ev.can_stall = 1;
-
-	return ffs;
-}
-
-static void ffs_data_clear(struct ffs_data *ffs)
-{
-	ENTER();
-
-	if (test_and_clear_bit(FFS_FL_CALL_CLOSED_CALLBACK, &ffs->flags))
-		functionfs_closed_callback(ffs);
-
-	BUG_ON(ffs->gadget);
-
-	if (ffs->epfiles)
-		ffs_epfiles_destroy(ffs->epfiles, ffs->eps_count);
-
-	kfree(ffs->raw_descs);
-	kfree(ffs->raw_strings);
-	kfree(ffs->stringtabs);
-}
-
-static void ffs_data_reset(struct ffs_data *ffs)
-{
-	ENTER();
-
-	ffs_data_clear(ffs);
-
-	ffs->epfiles = NULL;
-	ffs->raw_descs = NULL;
-	ffs->raw_strings = NULL;
-	ffs->stringtabs = NULL;
-
-	ffs->raw_descs_length = 0;
-	ffs->raw_fs_descs_length = 0;
-	ffs->fs_descs_count = 0;
-	ffs->hs_descs_count = 0;
-
-	ffs->strings_count = 0;
-	ffs->interfaces_count = 0;
-	ffs->eps_count = 0;
-
-	ffs->ev.count = 0;
-
-	ffs->state = FFS_READ_DESCRIPTORS;
-	ffs->setup_state = FFS_NO_SETUP;
-	ffs->flags = 0;
-}
-
-
-static int functionfs_bind(struct ffs_data *ffs, struct usb_composite_dev *cdev)
-{
-	struct usb_gadget_strings **lang;
-	int first_id;
-
-	ENTER();
-
-	if (WARN_ON(ffs->state != FFS_ACTIVE
-		 || test_and_set_bit(FFS_FL_BOUND, &ffs->flags)))
-		return -EBADFD;
-
-	first_id = usb_string_ids_n(cdev, ffs->strings_count);
-	if (unlikely(first_id < 0))
-		return first_id;
-
-	ffs->ep0req = usb_ep_alloc_request(cdev->gadget->ep0, GFP_KERNEL);
-	if (unlikely(!ffs->ep0req))
-		return -ENOMEM;
-	ffs->ep0req->complete = ffs_ep0_complete;
-	ffs->ep0req->context = ffs;
-
-	lang = ffs->stringtabs;
-	for (lang = ffs->stringtabs; *lang; ++lang) {
-		struct usb_string *str = (*lang)->strings;
-		int id = first_id;
-		for (; str->s; ++id, ++str)
-			str->id = id;
-	}
-
-	ffs->gadget = cdev->gadget;
-	ffs_data_get(ffs);
-	return 0;
-}
-
-static void functionfs_unbind(struct ffs_data *ffs)
-{
-	ENTER();
-
-	if (!WARN_ON(!ffs->gadget)) {
-		usb_ep_free_request(ffs->gadget->ep0, ffs->ep0req);
-		ffs->ep0req = NULL;
-		ffs->gadget = NULL;
-		ffs_data_put(ffs);
-		clear_bit(FFS_FL_BOUND, &ffs->flags);
-	}
-}
-
-static int ffs_epfiles_create(struct ffs_data *ffs)
-{
-	struct ffs_epfile *epfile, *epfiles;
-	unsigned i, count;
-
-	ENTER();
-
-	count = ffs->eps_count;
-	epfiles = kcalloc(count, sizeof(*epfiles), GFP_KERNEL);
-	if (!epfiles)
-		return -ENOMEM;
-
-	epfile = epfiles;
-	for (i = 1; i <= count; ++i, ++epfile) {
-		epfile->ffs = ffs;
-		mutex_init(&epfile->mutex);
-		init_waitqueue_head(&epfile->wait);
-		sprintf(epfiles->name, "ep%u",  i);
-		if (!unlikely(ffs_sb_create_file(ffs->sb, epfiles->name, epfile,
-						 &ffs_epfile_operations,
-						 &epfile->dentry))) {
-			ffs_epfiles_destroy(epfiles, i - 1);
-			return -ENOMEM;
-		}
-	}
-
-	ffs->epfiles = epfiles;
-	return 0;
-}
-
-static void ffs_epfiles_destroy(struct ffs_epfile *epfiles, unsigned count)
-{
-	struct ffs_epfile *epfile = epfiles;
-
-	ENTER();
-
-	for (; count; --count, ++epfile) {
-		BUG_ON(mutex_is_locked(&epfile->mutex) ||
-		       waitqueue_active(&epfile->wait));
-		if (epfile->dentry) {
-			d_delete(epfile->dentry);
-			dput(epfile->dentry);
-			epfile->dentry = NULL;
-		}
-	}
-
-	kfree(epfiles);
-}
-
-static int functionfs_bind_config(struct usb_composite_dev *cdev,
-				  struct usb_configuration *c,
-				  struct ffs_data *ffs)
-{
-	struct ffs_function *func;
-	int ret;
-
-	ENTER();
-
-	func = kzalloc(sizeof *func, GFP_KERNEL);
-	if (unlikely(!func))
-		return -ENOMEM;
-
-	func->function.name    = "Function FS Gadget";
-	func->function.strings = ffs->stringtabs;
-
-	func->function.bind    = ffs_func_bind;
-	func->function.unbind  = ffs_func_unbind;
-	func->function.set_alt = ffs_func_set_alt;
-	func->function.disable = ffs_func_disable;
-	func->function.setup   = ffs_func_setup;
-	func->function.suspend = ffs_func_suspend;
-	func->function.resume  = ffs_func_resume;
-
-	func->conf   = c;
-	func->gadget = cdev->gadget;
-	func->ffs = ffs;
-	ffs_data_get(ffs);
-
-	ret = usb_add_function(c, &func->function);
-	if (unlikely(ret))
-		ffs_func_free(func);
-
-	return ret;
-}
-
-static void ffs_func_free(struct ffs_function *func)
-{
-	struct ffs_ep *ep         = func->eps;
-	unsigned count            = func->ffs->eps_count;
-	unsigned long flags;
-
-	ENTER();
-
-	/* cleanup after autoconfig */
-	spin_lock_irqsave(&func->ffs->eps_lock, flags);
-	do {
-		if (ep->ep && ep->req)
-			usb_ep_free_request(ep->ep, ep->req);
-		ep->req = NULL;
-		++ep;
-	} while (--count);
-	spin_unlock_irqrestore(&func->ffs->eps_lock, flags);
-
-	ffs_data_put(func->ffs);
-
-	kfree(func->eps);
-	/*
-	 * eps and interfaces_nums are allocated in the same chunk so
-	 * only one free is required.  Descriptors are also allocated
-	 * in the same chunk.
-	 */
-
-	kfree(func);
-}
-
-static void ffs_func_eps_disable(struct ffs_function *func)
-{
-	struct ffs_ep *ep         = func->eps;
-	struct ffs_epfile *epfile = func->ffs->epfiles;
-	unsigned count            = func->ffs->eps_count;
-	unsigned long flags;
-
-	spin_lock_irqsave(&func->ffs->eps_lock, flags);
-	do {
-		/* pending requests get nuked */
-		if (likely(ep->ep))
-			usb_ep_disable(ep->ep);
-		epfile->ep = NULL;
-
-		++ep;
-		++epfile;
-	} while (--count);
-	spin_unlock_irqrestore(&func->ffs->eps_lock, flags);
-}
-
-static int ffs_func_eps_enable(struct ffs_function *func)
-{
-	struct ffs_data *ffs      = func->ffs;
-	struct ffs_ep *ep         = func->eps;
-	struct ffs_epfile *epfile = ffs->epfiles;
-	unsigned count            = ffs->eps_count;
-	unsigned long flags;
-	int ret = 0;
-
-	spin_lock_irqsave(&func->ffs->eps_lock, flags);
-	do {
-		struct usb_endpoint_descriptor *ds;
-		ds = ep->descs[ep->descs[1] ? 1 : 0];
-
-		ep->ep->driver_data = ep;
-		ep->ep->desc = ds;
-		ret = usb_ep_enable(ep->ep);
-		if (likely(!ret)) {
-			epfile->ep = ep;
-			epfile->in = usb_endpoint_dir_in(ds);
-			epfile->isoc = usb_endpoint_xfer_isoc(ds);
-		} else {
-			break;
-		}
-
-		wake_up(&epfile->wait);
-
-		++ep;
-		++epfile;
-	} while (--count);
-	spin_unlock_irqrestore(&func->ffs->eps_lock, flags);
-
-	return ret;
-}
-
-
-/* Parsing and building descriptors and strings *****************************/
-
-/*
- * This validates if data pointed by data is a valid USB descriptor as
- * well as record how many interfaces, endpoints and strings are
- * required by given configuration.  Returns address after the
- * descriptor or NULL if data is invalid.
- */
-
-enum ffs_entity_type {
-	FFS_DESCRIPTOR, FFS_INTERFACE, FFS_STRING, FFS_ENDPOINT
-};
-
-typedef int (*ffs_entity_callback)(enum ffs_entity_type entity,
-				   u8 *valuep,
-				   struct usb_descriptor_header *desc,
-				   void *priv);
-
-static int __must_check ffs_do_desc(char *data, unsigned len,
-				    ffs_entity_callback entity, void *priv)
-{
-	struct usb_descriptor_header *_ds = (void *)data;
-	u8 length;
-	int ret;
-
-	ENTER();
-
-	/* At least two bytes are required: length and type */
-	if (len < 2) {
-		pr_vdebug("descriptor too short\n");
-		return -EINVAL;
-	}
-
-	/* If we have at least as many bytes as the descriptor takes? */
-	length = _ds->bLength;
-	if (len < length) {
-		pr_vdebug("descriptor longer then available data\n");
-		return -EINVAL;
-	}
-
-#define __entity_check_INTERFACE(val)  1
-#define __entity_check_STRING(val)     (val)
-#define __entity_check_ENDPOINT(val)   ((val) & USB_ENDPOINT_NUMBER_MASK)
-#define __entity(type, val) do {					\
-		pr_vdebug("entity " #type "(%02x)\n", (val));		\
-		if (unlikely(!__entity_check_ ##type(val))) {		\
-			pr_vdebug("invalid entity's value\n");		\
-			return -EINVAL;					\
-		}							\
-		ret = entity(FFS_ ##type, &val, _ds, priv);		\
-		if (unlikely(ret < 0)) {				\
-			pr_debug("entity " #type "(%02x); ret = %d\n",	\
-				 (val), ret);				\
-			return ret;					\
-		}							\
-	} while (0)
-
-	/* Parse descriptor depending on type. */
-	switch (_ds->bDescriptorType) {
-	case USB_DT_DEVICE:
-	case USB_DT_CONFIG:
-	case USB_DT_STRING:
-	case USB_DT_DEVICE_QUALIFIER:
-		/* function can't have any of those */
-		pr_vdebug("descriptor reserved for gadget: %d\n",
-		      _ds->bDescriptorType);
-		return -EINVAL;
-
-	case USB_DT_INTERFACE: {
-		struct usb_interface_descriptor *ds = (void *)_ds;
-		pr_vdebug("interface descriptor\n");
-		if (length != sizeof *ds)
-			goto inv_length;
-
-		__entity(INTERFACE, ds->bInterfaceNumber);
-		if (ds->iInterface)
-			__entity(STRING, ds->iInterface);
-	}
-		break;
-
-	case USB_DT_ENDPOINT: {
-		struct usb_endpoint_descriptor *ds = (void *)_ds;
-		pr_vdebug("endpoint descriptor\n");
-		if (length != USB_DT_ENDPOINT_SIZE &&
-		    length != USB_DT_ENDPOINT_AUDIO_SIZE)
-			goto inv_length;
-		__entity(ENDPOINT, ds->bEndpointAddress);
-	}
-		break;
-
-	case HID_DT_HID:
-		pr_vdebug("hid descriptor\n");
-		if (length != sizeof(struct hid_descriptor))
-			goto inv_length;
-		break;
-
-	case USB_DT_OTG:
-		if (length != sizeof(struct usb_otg_descriptor))
-			goto inv_length;
-		break;
-
-	case USB_DT_INTERFACE_ASSOCIATION: {
-		struct usb_interface_assoc_descriptor *ds = (void *)_ds;
-		pr_vdebug("interface association descriptor\n");
-		if (length != sizeof *ds)
-			goto inv_length;
-		if (ds->iFunction)
-			__entity(STRING, ds->iFunction);
-	}
-		break;
-
-	case USB_DT_OTHER_SPEED_CONFIG:
-	case USB_DT_INTERFACE_POWER:
-	case USB_DT_DEBUG:
-	case USB_DT_SECURITY:
-	case USB_DT_CS_RADIO_CONTROL:
-		/* TODO */
-		pr_vdebug("unimplemented descriptor: %d\n", _ds->bDescriptorType);
-		return -EINVAL;
-
-	default:
-		/* We should never be here */
-		pr_vdebug("unknown descriptor: %d\n", _ds->bDescriptorType);
-		return -EINVAL;
-
-inv_length:
-		pr_vdebug("invalid length: %d (descriptor %d)\n",
-			  _ds->bLength, _ds->bDescriptorType);
-		return -EINVAL;
-	}
-
-#undef __entity
-#undef __entity_check_DESCRIPTOR
-#undef __entity_check_INTERFACE
-#undef __entity_check_STRING
-#undef __entity_check_ENDPOINT
-
-	return length;
-}
-
-static int __must_check ffs_do_descs(unsigned count, char *data, unsigned len,
-				     ffs_entity_callback entity, void *priv)
-{
-	const unsigned _len = len;
-	unsigned long num = 0;
-
-	ENTER();
-
-	for (;;) {
-		int ret;
-
-		if (num == count)
-			data = NULL;
-
-		/* Record "descriptor" entity */
-		ret = entity(FFS_DESCRIPTOR, (u8 *)num, (void *)data, priv);
-		if (unlikely(ret < 0)) {
-			pr_debug("entity DESCRIPTOR(%02lx); ret = %d\n",
-				 num, ret);
-			return ret;
-		}
-
-		if (!data)
-			return _len - len;
-
-		ret = ffs_do_desc(data, len, entity, priv);
-		if (unlikely(ret < 0)) {
-			pr_debug("%s returns %d\n", __func__, ret);
-			return ret;
-		}
-
-		len -= ret;
-		data += ret;
-		++num;
-	}
-}
-
-static int __ffs_data_do_entity(enum ffs_entity_type type,
-				u8 *valuep, struct usb_descriptor_header *desc,
-				void *priv)
-{
-	struct ffs_data *ffs = priv;
-
-	ENTER();
-
-	switch (type) {
-	case FFS_DESCRIPTOR:
-		break;
-
-	case FFS_INTERFACE:
-		/*
-		 * Interfaces are indexed from zero so if we
-		 * encountered interface "n" then there are at least
-		 * "n+1" interfaces.
-		 */
-		if (*valuep >= ffs->interfaces_count)
-			ffs->interfaces_count = *valuep + 1;
-		break;
-
-	case FFS_STRING:
-		/*
-		 * Strings are indexed from 1 (0 is magic ;) reserved
-		 * for languages list or some such)
-		 */
-		if (*valuep > ffs->strings_count)
-			ffs->strings_count = *valuep;
-		break;
-
-	case FFS_ENDPOINT:
-		/* Endpoints are indexed from 1 as well. */
-		if ((*valuep & USB_ENDPOINT_NUMBER_MASK) > ffs->eps_count)
-			ffs->eps_count = (*valuep & USB_ENDPOINT_NUMBER_MASK);
-		break;
-	}
-
-	return 0;
-}
-
-static int __ffs_data_got_descs(struct ffs_data *ffs,
-				char *const _data, size_t len)
-{
-	unsigned fs_count, hs_count;
-	int fs_len, ret = -EINVAL;
-	char *data = _data;
-
-	ENTER();
-
-	if (unlikely(get_unaligned_le32(data) != FUNCTIONFS_DESCRIPTORS_MAGIC ||
-		     get_unaligned_le32(data + 4) != len))
-		goto error;
-	fs_count = get_unaligned_le32(data +  8);
-	hs_count = get_unaligned_le32(data + 12);
-
-	if (!fs_count && !hs_count)
-		goto einval;
-
-	data += 16;
-	len  -= 16;
-
-	if (likely(fs_count)) {
-		fs_len = ffs_do_descs(fs_count, data, len,
-				      __ffs_data_do_entity, ffs);
-		if (unlikely(fs_len < 0)) {
-			ret = fs_len;
-			goto error;
-		}
-
-		data += fs_len;
-		len  -= fs_len;
-	} else {
-		fs_len = 0;
-	}
-
-	if (likely(hs_count)) {
-		ret = ffs_do_descs(hs_count, data, len,
-				   __ffs_data_do_entity, ffs);
-		if (unlikely(ret < 0))
-			goto error;
-	} else {
-		ret = 0;
-	}
-
-	if (unlikely(len != ret))
-		goto einval;
-
-	ffs->raw_fs_descs_length = fs_len;
-	ffs->raw_descs_length    = fs_len + ret;
-	ffs->raw_descs           = _data;
-	ffs->fs_descs_count      = fs_count;
-	ffs->hs_descs_count      = hs_count;
-
-	return 0;
-
-einval:
-	ret = -EINVAL;
-error:
-	kfree(_data);
-	return ret;
-}
-
-static int __ffs_data_got_strings(struct ffs_data *ffs,
-				  char *const _data, size_t len)
-{
-	u32 str_count, needed_count, lang_count;
-	struct usb_gadget_strings **stringtabs, *t;
-	struct usb_string *strings, *s;
-	const char *data = _data;
-
-	ENTER();
-
-	if (unlikely(get_unaligned_le32(data) != FUNCTIONFS_STRINGS_MAGIC ||
-		     get_unaligned_le32(data + 4) != len))
-		goto error;
-	str_count  = get_unaligned_le32(data + 8);
-	lang_count = get_unaligned_le32(data + 12);
-
-	/* if one is zero the other must be zero */
-	if (unlikely(!str_count != !lang_count))
-		goto error;
-
-	/* Do we have at least as many strings as descriptors need? */
-	needed_count = ffs->strings_count;
-	if (unlikely(str_count < needed_count))
-		goto error;
-
-	/*
-	 * If we don't need any strings just return and free all
-	 * memory.
-	 */
-	if (!needed_count) {
-		kfree(_data);
-		return 0;
-	}
-
-	/* Allocate everything in one chunk so there's less maintenance. */
-	{
-		struct {
-			struct usb_gadget_strings *stringtabs[lang_count + 1];
-			struct usb_gadget_strings stringtab[lang_count];
-			struct usb_string strings[lang_count*(needed_count+1)];
-		} *d;
-		unsigned i = 0;
-
-		d = kmalloc(sizeof *d, GFP_KERNEL);
-		if (unlikely(!d)) {
-			kfree(_data);
-			return -ENOMEM;
-		}
-
-		stringtabs = d->stringtabs;
-		t = d->stringtab;
-		i = lang_count;
-		do {
-			*stringtabs++ = t++;
-		} while (--i);
-		*stringtabs = NULL;
-
-		stringtabs = d->stringtabs;
-		t = d->stringtab;
-		s = d->strings;
-		strings = s;
-	}
-
-	/* For each language */
-	data += 16;
-	len -= 16;
-
-	do { /* lang_count > 0 so we can use do-while */
-		unsigned needed = needed_count;
-
-		if (unlikely(len < 3))
-			goto error_free;
-		t->language = get_unaligned_le16(data);
-		t->strings  = s;
-		++t;
-
-		data += 2;
-		len -= 2;
-
-		/* For each string */
-		do { /* str_count > 0 so we can use do-while */
-			size_t length = strnlen(data, len);
-
-			if (unlikely(length == len))
-				goto error_free;
-
-			/*
-			 * User may provide more strings then we need,
-			 * if that's the case we simply ignore the
-			 * rest
-			 */
-			if (likely(needed)) {
-				/*
-				 * s->id will be set while adding
-				 * function to configuration so for
-				 * now just leave garbage here.
-				 */
-				s->s = data;
-				--needed;
-				++s;
-			}
-
-			data += length + 1;
-			len -= length + 1;
-		} while (--str_count);
-
-		s->id = 0;   /* terminator */
-		s->s = NULL;
-		++s;
-
-	} while (--lang_count);
-
-	/* Some garbage left? */
-	if (unlikely(len))
-		goto error_free;
-
-	/* Done! */
-	ffs->stringtabs = stringtabs;
-	ffs->raw_strings = _data;
-
-	return 0;
-
-error_free:
-	kfree(stringtabs);
-error:
-	kfree(_data);
-	return -EINVAL;
-}
-
-
-/* Events handling and management *******************************************/
-
-static void __ffs_event_add(struct ffs_data *ffs,
-			    enum usb_functionfs_event_type type)
-{
-	enum usb_functionfs_event_type rem_type1, rem_type2 = type;
-	int neg = 0;
-
-	/*
-	 * Abort any unhandled setup
-	 *
-	 * We do not need to worry about some cmpxchg() changing value
-	 * of ffs->setup_state without holding the lock because when
-	 * state is FFS_SETUP_PENDING cmpxchg() in several places in
-	 * the source does nothing.
-	 */
-	if (ffs->setup_state == FFS_SETUP_PENDING)
-		ffs->setup_state = FFS_SETUP_CANCELED;
-
-	switch (type) {
-	case FUNCTIONFS_RESUME:
-		rem_type2 = FUNCTIONFS_SUSPEND;
-		/* FALL THROUGH */
-	case FUNCTIONFS_SUSPEND:
-	case FUNCTIONFS_SETUP:
-		rem_type1 = type;
-		/* Discard all similar events */
-		break;
-
-	case FUNCTIONFS_BIND:
-	case FUNCTIONFS_UNBIND:
-	case FUNCTIONFS_DISABLE:
-	case FUNCTIONFS_ENABLE:
-		/* Discard everything other then power management. */
-		rem_type1 = FUNCTIONFS_SUSPEND;
-		rem_type2 = FUNCTIONFS_RESUME;
-		neg = 1;
-		break;
-
-	default:
-		BUG();
-	}
-
-	{
-		u8 *ev  = ffs->ev.types, *out = ev;
-		unsigned n = ffs->ev.count;
-		for (; n; --n, ++ev)
-			if ((*ev == rem_type1 || *ev == rem_type2) == neg)
-				*out++ = *ev;
-			else
-				pr_vdebug("purging event %d\n", *ev);
-		ffs->ev.count = out - ffs->ev.types;
-	}
-
-	pr_vdebug("adding event %d\n", type);
-	ffs->ev.types[ffs->ev.count++] = type;
-	wake_up_locked(&ffs->ev.waitq);
-}
-
-static void ffs_event_add(struct ffs_data *ffs,
-			  enum usb_functionfs_event_type type)
-{
-	unsigned long flags;
-	spin_lock_irqsave(&ffs->ev.waitq.lock, flags);
-	__ffs_event_add(ffs, type);
-	spin_unlock_irqrestore(&ffs->ev.waitq.lock, flags);
-}
-
-
-/* Bind/unbind USB function hooks *******************************************/
-
-static int __ffs_func_bind_do_descs(enum ffs_entity_type type, u8 *valuep,
-				    struct usb_descriptor_header *desc,
-				    void *priv)
-{
-	struct usb_endpoint_descriptor *ds = (void *)desc;
-	struct ffs_function *func = priv;
-	struct ffs_ep *ffs_ep;
-
-	/*
-	 * If hs_descriptors is not NULL then we are reading hs
-	 * descriptors now
-	 */
-	const int isHS = func->function.hs_descriptors != NULL;
-	unsigned idx;
-
-	if (type != FFS_DESCRIPTOR)
-		return 0;
-
-	if (isHS)
-		func->function.hs_descriptors[(long)valuep] = desc;
-	else
-		func->function.descriptors[(long)valuep]    = desc;
-
-	if (!desc || desc->bDescriptorType != USB_DT_ENDPOINT)
-		return 0;
-
-	idx = (ds->bEndpointAddress & USB_ENDPOINT_NUMBER_MASK) - 1;
-	ffs_ep = func->eps + idx;
-
-	if (unlikely(ffs_ep->descs[isHS])) {
-		pr_vdebug("two %sspeed descriptors for EP %d\n",
-			  isHS ? "high" : "full",
-			  ds->bEndpointAddress & USB_ENDPOINT_NUMBER_MASK);
-		return -EINVAL;
-	}
-	ffs_ep->descs[isHS] = ds;
-
-	ffs_dump_mem(": Original  ep desc", ds, ds->bLength);
-	if (ffs_ep->ep) {
-		ds->bEndpointAddress = ffs_ep->descs[0]->bEndpointAddress;
-		if (!ds->wMaxPacketSize)
-			ds->wMaxPacketSize = ffs_ep->descs[0]->wMaxPacketSize;
-	} else {
-		struct usb_request *req;
-		struct usb_ep *ep;
-
-		pr_vdebug("autoconfig\n");
-		ep = usb_ep_autoconfig(func->gadget, ds);
-		if (unlikely(!ep))
-			return -ENOTSUPP;
-		ep->driver_data = func->eps + idx;
-
-		req = usb_ep_alloc_request(ep, GFP_KERNEL);
-		if (unlikely(!req))
-			return -ENOMEM;
-
-		ffs_ep->ep  = ep;
-		ffs_ep->req = req;
-		func->eps_revmap[ds->bEndpointAddress &
-				 USB_ENDPOINT_NUMBER_MASK] = idx + 1;
-	}
-	ffs_dump_mem(": Rewritten ep desc", ds, ds->bLength);
-
-	return 0;
-}
-
-static int __ffs_func_bind_do_nums(enum ffs_entity_type type, u8 *valuep,
-				   struct usb_descriptor_header *desc,
-				   void *priv)
-{
-	struct ffs_function *func = priv;
-	unsigned idx;
-	u8 newValue;
-
-	switch (type) {
-	default:
-	case FFS_DESCRIPTOR:
-		/* Handled in previous pass by __ffs_func_bind_do_descs() */
-		return 0;
-
-	case FFS_INTERFACE:
-		idx = *valuep;
-		if (func->interfaces_nums[idx] < 0) {
-			int id = usb_interface_id(func->conf, &func->function);
-			if (unlikely(id < 0))
-				return id;
-			func->interfaces_nums[idx] = id;
-		}
-		newValue = func->interfaces_nums[idx];
-		break;
-
-	case FFS_STRING:
-		/* String' IDs are allocated when fsf_data is bound to cdev */
-		newValue = func->ffs->stringtabs[0]->strings[*valuep - 1].id;
-		break;
-
-	case FFS_ENDPOINT:
-		/*
-		 * USB_DT_ENDPOINT are handled in
-		 * __ffs_func_bind_do_descs().
-		 */
-		if (desc->bDescriptorType == USB_DT_ENDPOINT)
-			return 0;
-
-		idx = (*valuep & USB_ENDPOINT_NUMBER_MASK) - 1;
-		if (unlikely(!func->eps[idx].ep))
-			return -EINVAL;
-
-		{
-			struct usb_endpoint_descriptor **descs;
-			descs = func->eps[idx].descs;
-			newValue = descs[descs[0] ? 0 : 1]->bEndpointAddress;
-		}
-		break;
-	}
-
-	pr_vdebug("%02x -> %02x\n", *valuep, newValue);
-	*valuep = newValue;
-	return 0;
-}
-
-static int ffs_func_bind(struct usb_configuration *c,
-			 struct usb_function *f)
-{
-	struct ffs_function *func = ffs_func_from_usb(f);
-	struct ffs_data *ffs = func->ffs;
-
-	const int full = !!func->ffs->fs_descs_count;
-	const int high = gadget_is_dualspeed(func->gadget) &&
-		func->ffs->hs_descs_count;
-
-	int ret;
-
-	/* Make it a single chunk, less management later on */
-	struct {
-		struct ffs_ep eps[ffs->eps_count];
-		struct usb_descriptor_header
-			*fs_descs[full ? ffs->fs_descs_count + 1 : 0];
-		struct usb_descriptor_header
-			*hs_descs[high ? ffs->hs_descs_count + 1 : 0];
-		short inums[ffs->interfaces_count];
-		char raw_descs[high ? ffs->raw_descs_length
-				    : ffs->raw_fs_descs_length];
-	} *data;
-
-	ENTER();
-
-	/* Only high speed but not supported by gadget? */
-	if (unlikely(!(full | high)))
-		return -ENOTSUPP;
-
-	/* Allocate */
-	data = kmalloc(sizeof *data, GFP_KERNEL);
-	if (unlikely(!data))
-		return -ENOMEM;
-
-	/* Zero */
-	memset(data->eps, 0, sizeof data->eps);
-	memcpy(data->raw_descs, ffs->raw_descs + 16, sizeof data->raw_descs);
-	memset(data->inums, 0xff, sizeof data->inums);
-	for (ret = ffs->eps_count; ret; --ret)
-		data->eps[ret].num = -1;
-
-	/* Save pointers */
-	func->eps             = data->eps;
-	func->interfaces_nums = data->inums;
-
-	/*
-	 * Go through all the endpoint descriptors and allocate
-	 * endpoints first, so that later we can rewrite the endpoint
-	 * numbers without worrying that it may be described later on.
-	 */
-	if (likely(full)) {
-		func->function.descriptors = data->fs_descs;
-		ret = ffs_do_descs(ffs->fs_descs_count,
-				   data->raw_descs,
-				   sizeof data->raw_descs,
-				   __ffs_func_bind_do_descs, func);
-		if (unlikely(ret < 0))
-			goto error;
-	} else {
-		ret = 0;
-	}
-
-	if (likely(high)) {
-		func->function.hs_descriptors = data->hs_descs;
-		ret = ffs_do_descs(ffs->hs_descs_count,
-				   data->raw_descs + ret,
-				   (sizeof data->raw_descs) - ret,
-				   __ffs_func_bind_do_descs, func);
-	}
-
-	/*
-	 * Now handle interface numbers allocation and interface and
-	 * endpoint numbers rewriting.  We can do that in one go
-	 * now.
-	 */
-	ret = ffs_do_descs(ffs->fs_descs_count +
-			   (high ? ffs->hs_descs_count : 0),
-			   data->raw_descs, sizeof data->raw_descs,
-			   __ffs_func_bind_do_nums, func);
-	if (unlikely(ret < 0))
-		goto error;
-
-	/* And we're done */
-	ffs_event_add(ffs, FUNCTIONFS_BIND);
-	return 0;
-
-error:
-	/* XXX Do we need to release all claimed endpoints here? */
-	return ret;
-}
-
-
-/* Other USB function hooks *************************************************/
-
-static void ffs_func_unbind(struct usb_configuration *c,
-			    struct usb_function *f)
-{
-	struct ffs_function *func = ffs_func_from_usb(f);
-	struct ffs_data *ffs = func->ffs;
-
-	ENTER();
-
-	if (ffs->func == func) {
-		ffs_func_eps_disable(func);
-		ffs->func = NULL;
-	}
-
-	ffs_event_add(ffs, FUNCTIONFS_UNBIND);
-
-	ffs_func_free(func);
-}
-
-static int ffs_func_set_alt(struct usb_function *f,
-			    unsigned interface, unsigned alt)
-{
-	struct ffs_function *func = ffs_func_from_usb(f);
-	struct ffs_data *ffs = func->ffs;
-	int ret = 0, intf;
-
-	if (alt != (unsigned)-1) {
-		intf = ffs_func_revmap_intf(func, interface);
-		if (unlikely(intf < 0))
-			return intf;
-	}
-
-	if (ffs->func)
-		ffs_func_eps_disable(ffs->func);
-
-	if (ffs->state != FFS_ACTIVE)
-		return -ENODEV;
-
-	if (alt == (unsigned)-1) {
-		ffs->func = NULL;
-		ffs_event_add(ffs, FUNCTIONFS_DISABLE);
-		return 0;
-	}
-
-	ffs->func = func;
-	ret = ffs_func_eps_enable(func);
-	if (likely(ret >= 0))
-		ffs_event_add(ffs, FUNCTIONFS_ENABLE);
-	return ret;
-}
-
-static void ffs_func_disable(struct usb_function *f)
-{
-	ffs_func_set_alt(f, 0, (unsigned)-1);
-}
-
-static int ffs_func_setup(struct usb_function *f,
-			  const struct usb_ctrlrequest *creq)
-{
-	struct ffs_function *func = ffs_func_from_usb(f);
-	struct ffs_data *ffs = func->ffs;
-	unsigned long flags;
-	int ret;
-
-	ENTER();
-
-	pr_vdebug("creq->bRequestType = %02x\n", creq->bRequestType);
-	pr_vdebug("creq->bRequest     = %02x\n", creq->bRequest);
-	pr_vdebug("creq->wValue       = %04x\n", le16_to_cpu(creq->wValue));
-	pr_vdebug("creq->wIndex       = %04x\n", le16_to_cpu(creq->wIndex));
-	pr_vdebug("creq->wLength      = %04x\n", le16_to_cpu(creq->wLength));
-
-	/*
-	 * Most requests directed to interface go through here
-	 * (notable exceptions are set/get interface) so we need to
-	 * handle them.  All other either handled by composite or
-	 * passed to usb_configuration->setup() (if one is set).  No
-	 * matter, we will handle requests directed to endpoint here
-	 * as well (as it's straightforward) but what to do with any
-	 * other request?
-	 */
-	if (ffs->state != FFS_ACTIVE)
-		return -ENODEV;
-
-	switch (creq->bRequestType & USB_RECIP_MASK) {
-	case USB_RECIP_INTERFACE:
-		ret = ffs_func_revmap_intf(func, le16_to_cpu(creq->wIndex));
-		if (unlikely(ret < 0))
-			return ret;
-		break;
-
-	case USB_RECIP_ENDPOINT:
-		ret = ffs_func_revmap_ep(func, le16_to_cpu(creq->wIndex));
-		if (unlikely(ret < 0))
-			return ret;
-		break;
-
-	default:
-		return -EOPNOTSUPP;
-	}
-
-	spin_lock_irqsave(&ffs->ev.waitq.lock, flags);
-	ffs->ev.setup = *creq;
-	ffs->ev.setup.wIndex = cpu_to_le16(ret);
-	__ffs_event_add(ffs, FUNCTIONFS_SETUP);
-	spin_unlock_irqrestore(&ffs->ev.waitq.lock, flags);
-
-	return 0;
-}
-
-static void ffs_func_suspend(struct usb_function *f)
-{
-	ENTER();
-	ffs_event_add(ffs_func_from_usb(f)->ffs, FUNCTIONFS_SUSPEND);
-}
-
-static void ffs_func_resume(struct usb_function *f)
-{
-	ENTER();
-	ffs_event_add(ffs_func_from_usb(f)->ffs, FUNCTIONFS_RESUME);
-}
-
-
-/* Endpoint and interface numbers reverse mapping ***************************/
-
-static int ffs_func_revmap_ep(struct ffs_function *func, u8 num)
-{
-	num = func->eps_revmap[num & USB_ENDPOINT_NUMBER_MASK];
-	return num ? num : -EDOM;
-}
-
-static int ffs_func_revmap_intf(struct ffs_function *func, u8 intf)
-{
-	short *nums = func->interfaces_nums;
-	unsigned count = func->ffs->interfaces_count;
-
-	for (; count; --count, ++nums) {
-		if (*nums >= 0 && *nums == intf)
-			return nums - func->interfaces_nums;
-	}
-
-	return -EDOM;
-}
-
-
-/* Misc helper functions ****************************************************/
-
-static int ffs_mutex_lock(struct mutex *mutex, unsigned nonblock)
-{
-	return nonblock
-		? likely(mutex_trylock(mutex)) ? 0 : -EAGAIN
-		: mutex_lock_interruptible(mutex);
-}
-
-static char *ffs_prepare_buffer(const char * __user buf, size_t len)
-{
-	char *data;
-
-	if (unlikely(!len))
-		return NULL;
-
-	data = kmalloc(len, GFP_KERNEL);
-	if (unlikely(!data))
-		return ERR_PTR(-ENOMEM);
-
-	if (unlikely(__copy_from_user(data, buf, len))) {
-		kfree(data);
-		return ERR_PTR(-EFAULT);
-	}
-
-	pr_vdebug("Buffer from user space:\n");
-	ffs_dump_mem("", data, len);
-
-	return data;
-}
diff --git a/drivers/staging/ccg/f_mass_storage.c b/drivers/staging/ccg/f_mass_storage.c
deleted file mode 100644
index 20bc2b454ac..00000000000
--- a/drivers/staging/ccg/f_mass_storage.c
+++ /dev/null
@@ -1,3135 +0,0 @@
-/*
- * f_mass_storage.c -- Mass Storage USB Composite Function
- *
- * Copyright (C) 2003-2008 Alan Stern
- * Copyright (C) 2009 Samsung Electronics
- *                    Author: Michal Nazarewicz <mina86@mina86.com>
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions, and the following disclaimer,
- *    without modification.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in the
- *    documentation and/or other materials provided with the distribution.
- * 3. The names of the above-listed copyright holders may not be used
- *    to endorse or promote products derived from this software without
- *    specific prior written permission.
- *
- * ALTERNATIVELY, this software may be distributed under the terms of the
- * GNU General Public License ("GPL") as published by the Free Software
- * Foundation, either version 2 of that License or (at your option) any
- * later version.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
- * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
- * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
- * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-/*
- * The Mass Storage Function acts as a USB Mass Storage device,
- * appearing to the host as a disk drive or as a CD-ROM drive.  In
- * addition to providing an example of a genuinely useful composite
- * function for a USB device, it also illustrates a technique of
- * double-buffering for increased throughput.
- *
- * For more information about MSF and in particular its module
- * parameters and sysfs interface read the
- * <Documentation/usb/mass-storage.txt> file.
- */
-
-/*
- * MSF is configured by specifying a fsg_config structure.  It has the
- * following fields:
- *
- *	nluns		Number of LUNs function have (anywhere from 1
- *				to FSG_MAX_LUNS which is 8).
- *	luns		An array of LUN configuration values.  This
- *				should be filled for each LUN that
- *				function will include (ie. for "nluns"
- *				LUNs).  Each element of the array has
- *				the following fields:
- *	->filename	The path to the backing file for the LUN.
- *				Required if LUN is not marked as
- *				removable.
- *	->ro		Flag specifying access to the LUN shall be
- *				read-only.  This is implied if CD-ROM
- *				emulation is enabled as well as when
- *				it was impossible to open "filename"
- *				in R/W mode.
- *	->removable	Flag specifying that LUN shall be indicated as
- *				being removable.
- *	->cdrom		Flag specifying that LUN shall be reported as
- *				being a CD-ROM.
- *	->nofua		Flag specifying that FUA flag in SCSI WRITE(10,12)
- *				commands for this LUN shall be ignored.
- *
- *	vendor_name
- *	product_name
- *	release		Information used as a reply to INQUIRY
- *				request.  To use default set to NULL,
- *				NULL, 0xffff respectively.  The first
- *				field should be 8 and the second 16
- *				characters or less.
- *
- *	can_stall	Set to permit function to halt bulk endpoints.
- *				Disabled on some USB devices known not
- *				to work correctly.  You should set it
- *				to true.
- *
- * If "removable" is not set for a LUN then a backing file must be
- * specified.  If it is set, then NULL filename means the LUN's medium
- * is not loaded (an empty string as "filename" in the fsg_config
- * structure causes error).  The CD-ROM emulation includes a single
- * data track and no audio tracks; hence there need be only one
- * backing file per LUN.
- *
- * This function is heavily based on "File-backed Storage Gadget" by
- * Alan Stern which in turn is heavily based on "Gadget Zero" by David
- * Brownell.  The driver's SCSI command interface was based on the
- * "Information technology - Small Computer System Interface - 2"
- * document from X3T9.2 Project 375D, Revision 10L, 7-SEP-93,
- * available at <http://www.t10.org/ftp/t10/drafts/s2/s2-r10l.pdf>.
- * The single exception is opcode 0x23 (READ FORMAT CAPACITIES), which
- * was based on the "Universal Serial Bus Mass Storage Class UFI
- * Command Specification" document, Revision 1.0, December 14, 1998,
- * available at
- * <http://www.usb.org/developers/devclass_docs/usbmass-ufi10.pdf>.
- */
-
-/*
- *				Driver Design
- *
- * The MSF is fairly straightforward.  There is a main kernel
- * thread that handles most of the work.  Interrupt routines field
- * callbacks from the controller driver: bulk- and interrupt-request
- * completion notifications, endpoint-0 events, and disconnect events.
- * Completion events are passed to the main thread by wakeup calls.  Many
- * ep0 requests are handled at interrupt time, but SetInterface,
- * SetConfiguration, and device reset requests are forwarded to the
- * thread in the form of "exceptions" using SIGUSR1 signals (since they
- * should interrupt any ongoing file I/O operations).
- *
- * The thread's main routine implements the standard command/data/status
- * parts of a SCSI interaction.  It and its subroutines are full of tests
- * for pending signals/exceptions -- all this polling is necessary since
- * the kernel has no setjmp/longjmp equivalents.  (Maybe this is an
- * indication that the driver really wants to be running in userspace.)
- * An important point is that so long as the thread is alive it keeps an
- * open reference to the backing file.  This will prevent unmounting
- * the backing file's underlying filesystem and could cause problems
- * during system shutdown, for example.  To prevent such problems, the
- * thread catches INT, TERM, and KILL signals and converts them into
- * an EXIT exception.
- *
- * In normal operation the main thread is started during the gadget's
- * fsg_bind() callback and stopped during fsg_unbind().  But it can
- * also exit when it receives a signal, and there's no point leaving
- * the gadget running when the thread is dead.  As of this moment, MSF
- * provides no way to deregister the gadget when thread dies -- maybe
- * a callback functions is needed.
- *
- * To provide maximum throughput, the driver uses a circular pipeline of
- * buffer heads (struct fsg_buffhd).  In principle the pipeline can be
- * arbitrarily long; in practice the benefits don't justify having more
- * than 2 stages (i.e., double buffering).  But it helps to think of the
- * pipeline as being a long one.  Each buffer head contains a bulk-in and
- * a bulk-out request pointer (since the buffer can be used for both
- * output and input -- directions always are given from the host's
- * point of view) as well as a pointer to the buffer and various state
- * variables.
- *
- * Use of the pipeline follows a simple protocol.  There is a variable
- * (fsg->next_buffhd_to_fill) that points to the next buffer head to use.
- * At any time that buffer head may still be in use from an earlier
- * request, so each buffer head has a state variable indicating whether
- * it is EMPTY, FULL, or BUSY.  Typical use involves waiting for the
- * buffer head to be EMPTY, filling the buffer either by file I/O or by
- * USB I/O (during which the buffer head is BUSY), and marking the buffer
- * head FULL when the I/O is complete.  Then the buffer will be emptied
- * (again possibly by USB I/O, during which it is marked BUSY) and
- * finally marked EMPTY again (possibly by a completion routine).
- *
- * A module parameter tells the driver to avoid stalling the bulk
- * endpoints wherever the transport specification allows.  This is
- * necessary for some UDCs like the SuperH, which cannot reliably clear a
- * halt on a bulk endpoint.  However, under certain circumstances the
- * Bulk-only specification requires a stall.  In such cases the driver
- * will halt the endpoint and set a flag indicating that it should clear
- * the halt in software during the next device reset.  Hopefully this
- * will permit everything to work correctly.  Furthermore, although the
- * specification allows the bulk-out endpoint to halt when the host sends
- * too much data, implementing this would cause an unavoidable race.
- * The driver will always use the "no-stall" approach for OUT transfers.
- *
- * One subtle point concerns sending status-stage responses for ep0
- * requests.  Some of these requests, such as device reset, can involve
- * interrupting an ongoing file I/O operation, which might take an
- * arbitrarily long time.  During that delay the host might give up on
- * the original ep0 request and issue a new one.  When that happens the
- * driver should not notify the host about completion of the original
- * request, as the host will no longer be waiting for it.  So the driver
- * assigns to each ep0 request a unique tag, and it keeps track of the
- * tag value of the request associated with a long-running exception
- * (device-reset, interface-change, or configuration-change).  When the
- * exception handler is finished, the status-stage response is submitted
- * only if the current ep0 request tag is equal to the exception request
- * tag.  Thus only the most recently received ep0 request will get a
- * status-stage response.
- *
- * Warning: This driver source file is too long.  It ought to be split up
- * into a header file plus about 3 separate .c files, to handle the details
- * of the Gadget, USB Mass Storage, and SCSI protocols.
- */
-
-
-/* #define VERBOSE_DEBUG */
-/* #define DUMP_MSGS */
-
-#include <linux/blkdev.h>
-#include <linux/completion.h>
-#include <linux/dcache.h>
-#include <linux/delay.h>
-#include <linux/device.h>
-#include <linux/fcntl.h>
-#include <linux/file.h>
-#include <linux/fs.h>
-#include <linux/kref.h>
-#include <linux/kthread.h>
-#include <linux/limits.h>
-#include <linux/rwsem.h>
-#include <linux/slab.h>
-#include <linux/spinlock.h>
-#include <linux/string.h>
-#include <linux/freezer.h>
-#include <linux/utsname.h>
-
-#include <linux/usb/ch9.h>
-#include <linux/usb/gadget.h>
-#include <linux/usb/composite.h>
-
-#include "gadget_chips.h"
-
-
-/*------------------------------------------------------------------------*/
-
-#define FSG_DRIVER_DESC		"Mass Storage Function"
-#define FSG_DRIVER_VERSION	"2009/09/11"
-
-static const char fsg_string_interface[] = "Mass Storage";
-
-#define FSG_NO_DEVICE_STRINGS    1
-#define FSG_NO_OTG               1
-#define FSG_NO_INTR_EP           1
-
-#include "storage_common.c"
-
-
-/*-------------------------------------------------------------------------*/
-
-struct fsg_dev;
-struct fsg_common;
-
-/* FSF callback functions */
-struct fsg_operations {
-	/*
-	 * Callback function to call when thread exits.  If no
-	 * callback is set or it returns value lower then zero MSF
-	 * will force eject all LUNs it operates on (including those
-	 * marked as non-removable or with prevent_medium_removal flag
-	 * set).
-	 */
-	int (*thread_exits)(struct fsg_common *common);
-
-	/*
-	 * Called prior to ejection.  Negative return means error,
-	 * zero means to continue with ejection, positive means not to
-	 * eject.
-	 */
-	int (*pre_eject)(struct fsg_common *common,
-			 struct fsg_lun *lun, int num);
-	/*
-	 * Called after ejection.  Negative return means error, zero
-	 * or positive is just a success.
-	 */
-	int (*post_eject)(struct fsg_common *common,
-			  struct fsg_lun *lun, int num);
-};
-
-/* Data shared by all the FSG instances. */
-struct fsg_common {
-	struct usb_gadget	*gadget;
-	struct usb_composite_dev *cdev;
-	struct fsg_dev		*fsg, *new_fsg;
-	wait_queue_head_t	fsg_wait;
-
-	/* filesem protects: backing files in use */
-	struct rw_semaphore	filesem;
-
-	/* lock protects: state, all the req_busy's */
-	spinlock_t		lock;
-
-	struct usb_ep		*ep0;		/* Copy of gadget->ep0 */
-	struct usb_request	*ep0req;	/* Copy of cdev->req */
-	unsigned int		ep0_req_tag;
-
-	struct fsg_buffhd	*next_buffhd_to_fill;
-	struct fsg_buffhd	*next_buffhd_to_drain;
-	struct fsg_buffhd	*buffhds;
-
-	int			cmnd_size;
-	u8			cmnd[MAX_COMMAND_SIZE];
-
-	unsigned int		nluns;
-	unsigned int		lun;
-	struct fsg_lun		*luns;
-	struct fsg_lun		*curlun;
-
-	unsigned int		bulk_out_maxpacket;
-	enum fsg_state		state;		/* For exception handling */
-	unsigned int		exception_req_tag;
-
-	enum data_direction	data_dir;
-	u32			data_size;
-	u32			data_size_from_cmnd;
-	u32			tag;
-	u32			residue;
-	u32			usb_amount_left;
-
-	unsigned int		can_stall:1;
-	unsigned int		free_storage_on_release:1;
-	unsigned int		phase_error:1;
-	unsigned int		short_packet_received:1;
-	unsigned int		bad_lun_okay:1;
-	unsigned int		running:1;
-
-	int			thread_wakeup_needed;
-	struct completion	thread_notifier;
-	struct task_struct	*thread_task;
-
-	/* Callback functions. */
-	const struct fsg_operations	*ops;
-	/* Gadget's private data. */
-	void			*private_data;
-
-	/*
-	 * Vendor (8 chars), product (16 chars), release (4
-	 * hexadecimal digits) and NUL byte
-	 */
-	char inquiry_string[8 + 16 + 4 + 1];
-
-	struct kref		ref;
-};
-
-struct fsg_config {
-	unsigned nluns;
-	struct fsg_lun_config {
-		const char *filename;
-		char ro;
-		char removable;
-		char cdrom;
-		char nofua;
-	} luns[FSG_MAX_LUNS];
-
-	/* Callback functions. */
-	const struct fsg_operations	*ops;
-	/* Gadget's private data. */
-	void			*private_data;
-
-	const char *vendor_name;		/*  8 characters or less */
-	const char *product_name;		/* 16 characters or less */
-	u16 release;
-
-	char			can_stall;
-};
-
-struct fsg_dev {
-	struct usb_function	function;
-	struct usb_gadget	*gadget;	/* Copy of cdev->gadget */
-	struct fsg_common	*common;
-
-	u16			interface_number;
-
-	unsigned int		bulk_in_enabled:1;
-	unsigned int		bulk_out_enabled:1;
-
-	unsigned long		atomic_bitflags;
-#define IGNORE_BULK_OUT		0
-
-	struct usb_ep		*bulk_in;
-	struct usb_ep		*bulk_out;
-};
-
-static inline int __fsg_is_set(struct fsg_common *common,
-			       const char *func, unsigned line)
-{
-	if (common->fsg)
-		return 1;
-	ERROR(common, "common->fsg is NULL in %s at %u\n", func, line);
-	WARN_ON(1);
-	return 0;
-}
-
-#define fsg_is_set(common) likely(__fsg_is_set(common, __func__, __LINE__))
-
-static inline struct fsg_dev *fsg_from_func(struct usb_function *f)
-{
-	return container_of(f, struct fsg_dev, function);
-}
-
-typedef void (*fsg_routine_t)(struct fsg_dev *);
-
-static int exception_in_progress(struct fsg_common *common)
-{
-	return common->state > FSG_STATE_IDLE;
-}
-
-/* Make bulk-out requests be divisible by the maxpacket size */
-static void set_bulk_out_req_length(struct fsg_common *common,
-				    struct fsg_buffhd *bh, unsigned int length)
-{
-	unsigned int	rem;
-
-	bh->bulk_out_intended_length = length;
-	rem = length % common->bulk_out_maxpacket;
-	if (rem > 0)
-		length += common->bulk_out_maxpacket - rem;
-	bh->outreq->length = length;
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-static int fsg_set_halt(struct fsg_dev *fsg, struct usb_ep *ep)
-{
-	const char	*name;
-
-	if (ep == fsg->bulk_in)
-		name = "bulk-in";
-	else if (ep == fsg->bulk_out)
-		name = "bulk-out";
-	else
-		name = ep->name;
-	DBG(fsg, "%s set halt\n", name);
-	return usb_ep_set_halt(ep);
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-/* These routines may be called in process context or in_irq */
-
-/* Caller must hold fsg->lock */
-static void wakeup_thread(struct fsg_common *common)
-{
-	/* Tell the main thread that something has happened */
-	common->thread_wakeup_needed = 1;
-	if (common->thread_task)
-		wake_up_process(common->thread_task);
-}
-
-static void raise_exception(struct fsg_common *common, enum fsg_state new_state)
-{
-	unsigned long		flags;
-
-	/*
-	 * Do nothing if a higher-priority exception is already in progress.
-	 * If a lower-or-equal priority exception is in progress, preempt it
-	 * and notify the main thread by sending it a signal.
-	 */
-	spin_lock_irqsave(&common->lock, flags);
-	if (common->state <= new_state) {
-		common->exception_req_tag = common->ep0_req_tag;
-		common->state = new_state;
-		if (common->thread_task)
-			send_sig_info(SIGUSR1, SEND_SIG_FORCED,
-				      common->thread_task);
-	}
-	spin_unlock_irqrestore(&common->lock, flags);
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-static int ep0_queue(struct fsg_common *common)
-{
-	int	rc;
-
-	rc = usb_ep_queue(common->ep0, common->ep0req, GFP_ATOMIC);
-	common->ep0->driver_data = common;
-	if (rc != 0 && rc != -ESHUTDOWN) {
-		/* We can't do much more than wait for a reset */
-		WARNING(common, "error in submission: %s --> %d\n",
-			common->ep0->name, rc);
-	}
-	return rc;
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-/* Completion handlers. These always run in_irq. */
-
-static void bulk_in_complete(struct usb_ep *ep, struct usb_request *req)
-{
-	struct fsg_common	*common = ep->driver_data;
-	struct fsg_buffhd	*bh = req->context;
-
-	if (req->status || req->actual != req->length)
-		DBG(common, "%s --> %d, %u/%u\n", __func__,
-		    req->status, req->actual, req->length);
-	if (req->status == -ECONNRESET)		/* Request was cancelled */
-		usb_ep_fifo_flush(ep);
-
-	/* Hold the lock while we update the request and buffer states */
-	smp_wmb();
-	spin_lock(&common->lock);
-	bh->inreq_busy = 0;
-	bh->state = BUF_STATE_EMPTY;
-	wakeup_thread(common);
-	spin_unlock(&common->lock);
-}
-
-static void bulk_out_complete(struct usb_ep *ep, struct usb_request *req)
-{
-	struct fsg_common	*common = ep->driver_data;
-	struct fsg_buffhd	*bh = req->context;
-
-	dump_msg(common, "bulk-out", req->buf, req->actual);
-	if (req->status || req->actual != bh->bulk_out_intended_length)
-		DBG(common, "%s --> %d, %u/%u\n", __func__,
-		    req->status, req->actual, bh->bulk_out_intended_length);
-	if (req->status == -ECONNRESET)		/* Request was cancelled */
-		usb_ep_fifo_flush(ep);
-
-	/* Hold the lock while we update the request and buffer states */
-	smp_wmb();
-	spin_lock(&common->lock);
-	bh->outreq_busy = 0;
-	bh->state = BUF_STATE_FULL;
-	wakeup_thread(common);
-	spin_unlock(&common->lock);
-}
-
-static int fsg_setup(struct usb_function *f,
-		     const struct usb_ctrlrequest *ctrl)
-{
-	struct fsg_dev		*fsg = fsg_from_func(f);
-	struct usb_request	*req = fsg->common->ep0req;
-	u16			w_index = le16_to_cpu(ctrl->wIndex);
-	u16			w_value = le16_to_cpu(ctrl->wValue);
-	u16			w_length = le16_to_cpu(ctrl->wLength);
-
-	if (!fsg_is_set(fsg->common))
-		return -EOPNOTSUPP;
-
-	++fsg->common->ep0_req_tag;	/* Record arrival of a new request */
-	req->context = NULL;
-	req->length = 0;
-	dump_msg(fsg, "ep0-setup", (u8 *) ctrl, sizeof(*ctrl));
-
-	switch (ctrl->bRequest) {
-
-	case US_BULK_RESET_REQUEST:
-		if (ctrl->bRequestType !=
-		    (USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE))
-			break;
-		if (w_index != fsg->interface_number || w_value != 0 ||
-				w_length != 0)
-			return -EDOM;
-
-		/*
-		 * Raise an exception to stop the current operation
-		 * and reinitialize our state.
-		 */
-		DBG(fsg, "bulk reset request\n");
-		raise_exception(fsg->common, FSG_STATE_RESET);
-		return DELAYED_STATUS;
-
-	case US_BULK_GET_MAX_LUN:
-		if (ctrl->bRequestType !=
-		    (USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE))
-			break;
-		if (w_index != fsg->interface_number || w_value != 0 ||
-				w_length != 1)
-			return -EDOM;
-		VDBG(fsg, "get max LUN\n");
-		*(u8 *)req->buf = fsg->common->nluns - 1;
-
-		/* Respond with data/status */
-		req->length = min((u16)1, w_length);
-		return ep0_queue(fsg->common);
-	}
-
-	VDBG(fsg,
-	     "unknown class-specific control req %02x.%02x v%04x i%04x l%u\n",
-	     ctrl->bRequestType, ctrl->bRequest,
-	     le16_to_cpu(ctrl->wValue), w_index, w_length);
-	return -EOPNOTSUPP;
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-/* All the following routines run in process context */
-
-/* Use this for bulk or interrupt transfers, not ep0 */
-static void start_transfer(struct fsg_dev *fsg, struct usb_ep *ep,
-			   struct usb_request *req, int *pbusy,
-			   enum fsg_buffer_state *state)
-{
-	int	rc;
-
-	if (ep == fsg->bulk_in)
-		dump_msg(fsg, "bulk-in", req->buf, req->length);
-
-	spin_lock_irq(&fsg->common->lock);
-	*pbusy = 1;
-	*state = BUF_STATE_BUSY;
-	spin_unlock_irq(&fsg->common->lock);
-	rc = usb_ep_queue(ep, req, GFP_KERNEL);
-	if (rc != 0) {
-		*pbusy = 0;
-		*state = BUF_STATE_EMPTY;
-
-		/* We can't do much more than wait for a reset */
-
-		/*
-		 * Note: currently the net2280 driver fails zero-length
-		 * submissions if DMA is enabled.
-		 */
-		if (rc != -ESHUTDOWN &&
-		    !(rc == -EOPNOTSUPP && req->length == 0))
-			WARNING(fsg, "error in submission: %s --> %d\n",
-				ep->name, rc);
-	}
-}
-
-static bool start_in_transfer(struct fsg_common *common, struct fsg_buffhd *bh)
-{
-	if (!fsg_is_set(common))
-		return false;
-	start_transfer(common->fsg, common->fsg->bulk_in,
-		       bh->inreq, &bh->inreq_busy, &bh->state);
-	return true;
-}
-
-static bool start_out_transfer(struct fsg_common *common, struct fsg_buffhd *bh)
-{
-	if (!fsg_is_set(common))
-		return false;
-	start_transfer(common->fsg, common->fsg->bulk_out,
-		       bh->outreq, &bh->outreq_busy, &bh->state);
-	return true;
-}
-
-static int sleep_thread(struct fsg_common *common)
-{
-	int	rc = 0;
-
-	/* Wait until a signal arrives or we are woken up */
-	for (;;) {
-		try_to_freeze();
-		set_current_state(TASK_INTERRUPTIBLE);
-		if (signal_pending(current)) {
-			rc = -EINTR;
-			break;
-		}
-		if (common->thread_wakeup_needed)
-			break;
-		schedule();
-	}
-	__set_current_state(TASK_RUNNING);
-	common->thread_wakeup_needed = 0;
-	return rc;
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-static int do_read(struct fsg_common *common)
-{
-	struct fsg_lun		*curlun = common->curlun;
-	u32			lba;
-	struct fsg_buffhd	*bh;
-	int			rc;
-	u32			amount_left;
-	loff_t			file_offset, file_offset_tmp;
-	unsigned int		amount;
-	ssize_t			nread;
-
-	/*
-	 * Get the starting Logical Block Address and check that it's
-	 * not too big.
-	 */
-	if (common->cmnd[0] == READ_6)
-		lba = get_unaligned_be24(&common->cmnd[1]);
-	else {
-		lba = get_unaligned_be32(&common->cmnd[2]);
-
-		/*
-		 * We allow DPO (Disable Page Out = don't save data in the
-		 * cache) and FUA (Force Unit Access = don't read from the
-		 * cache), but we don't implement them.
-		 */
-		if ((common->cmnd[1] & ~0x18) != 0) {
-			curlun->sense_data = SS_INVALID_FIELD_IN_CDB;
-			return -EINVAL;
-		}
-	}
-	if (lba >= curlun->num_sectors) {
-		curlun->sense_data = SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
-		return -EINVAL;
-	}
-	file_offset = ((loff_t) lba) << curlun->blkbits;
-
-	/* Carry out the file reads */
-	amount_left = common->data_size_from_cmnd;
-	if (unlikely(amount_left == 0))
-		return -EIO;		/* No default reply */
-
-	for (;;) {
-		/*
-		 * Figure out how much we need to read:
-		 * Try to read the remaining amount.
-		 * But don't read more than the buffer size.
-		 * And don't try to read past the end of the file.
-		 */
-		amount = min(amount_left, FSG_BUFLEN);
-		amount = min((loff_t)amount,
-			     curlun->file_length - file_offset);
-
-		/* Wait for the next buffer to become available */
-		bh = common->next_buffhd_to_fill;
-		while (bh->state != BUF_STATE_EMPTY) {
-			rc = sleep_thread(common);
-			if (rc)
-				return rc;
-		}
-
-		/*
-		 * If we were asked to read past the end of file,
-		 * end with an empty buffer.
-		 */
-		if (amount == 0) {
-			curlun->sense_data =
-					SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
-			curlun->sense_data_info =
-					file_offset >> curlun->blkbits;
-			curlun->info_valid = 1;
-			bh->inreq->length = 0;
-			bh->state = BUF_STATE_FULL;
-			break;
-		}
-
-		/* Perform the read */
-		file_offset_tmp = file_offset;
-		nread = vfs_read(curlun->filp,
-				 (char __user *)bh->buf,
-				 amount, &file_offset_tmp);
-		VLDBG(curlun, "file read %u @ %llu -> %d\n", amount,
-		      (unsigned long long)file_offset, (int)nread);
-		if (signal_pending(current))
-			return -EINTR;
-
-		if (nread < 0) {
-			LDBG(curlun, "error in file read: %d\n", (int)nread);
-			nread = 0;
-		} else if (nread < amount) {
-			LDBG(curlun, "partial file read: %d/%u\n",
-			     (int)nread, amount);
-			nread = round_down(nread, curlun->blksize);
-		}
-		file_offset  += nread;
-		amount_left  -= nread;
-		common->residue -= nread;
-
-		/*
-		 * Except at the end of the transfer, nread will be
-		 * equal to the buffer size, which is divisible by the
-		 * bulk-in maxpacket size.
-		 */
-		bh->inreq->length = nread;
-		bh->state = BUF_STATE_FULL;
-
-		/* If an error occurred, report it and its position */
-		if (nread < amount) {
-			curlun->sense_data = SS_UNRECOVERED_READ_ERROR;
-			curlun->sense_data_info =
-					file_offset >> curlun->blkbits;
-			curlun->info_valid = 1;
-			break;
-		}
-
-		if (amount_left == 0)
-			break;		/* No more left to read */
-
-		/* Send this buffer and go read some more */
-		bh->inreq->zero = 0;
-		if (!start_in_transfer(common, bh))
-			/* Don't know what to do if common->fsg is NULL */
-			return -EIO;
-		common->next_buffhd_to_fill = bh->next;
-	}
-
-	return -EIO;		/* No default reply */
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-static int do_write(struct fsg_common *common)
-{
-	struct fsg_lun		*curlun = common->curlun;
-	u32			lba;
-	struct fsg_buffhd	*bh;
-	int			get_some_more;
-	u32			amount_left_to_req, amount_left_to_write;
-	loff_t			usb_offset, file_offset, file_offset_tmp;
-	unsigned int		amount;
-	ssize_t			nwritten;
-	int			rc;
-
-	if (curlun->ro) {
-		curlun->sense_data = SS_WRITE_PROTECTED;
-		return -EINVAL;
-	}
-	spin_lock(&curlun->filp->f_lock);
-	curlun->filp->f_flags &= ~O_SYNC;	/* Default is not to wait */
-	spin_unlock(&curlun->filp->f_lock);
-
-	/*
-	 * Get the starting Logical Block Address and check that it's
-	 * not too big
-	 */
-	if (common->cmnd[0] == WRITE_6)
-		lba = get_unaligned_be24(&common->cmnd[1]);
-	else {
-		lba = get_unaligned_be32(&common->cmnd[2]);
-
-		/*
-		 * We allow DPO (Disable Page Out = don't save data in the
-		 * cache) and FUA (Force Unit Access = write directly to the
-		 * medium).  We don't implement DPO; we implement FUA by
-		 * performing synchronous output.
-		 */
-		if (common->cmnd[1] & ~0x18) {
-			curlun->sense_data = SS_INVALID_FIELD_IN_CDB;
-			return -EINVAL;
-		}
-		if (!curlun->nofua && (common->cmnd[1] & 0x08)) { /* FUA */
-			spin_lock(&curlun->filp->f_lock);
-			curlun->filp->f_flags |= O_SYNC;
-			spin_unlock(&curlun->filp->f_lock);
-		}
-	}
-	if (lba >= curlun->num_sectors) {
-		curlun->sense_data = SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
-		return -EINVAL;
-	}
-
-	/* Carry out the file writes */
-	get_some_more = 1;
-	file_offset = usb_offset = ((loff_t) lba) << curlun->blkbits;
-	amount_left_to_req = common->data_size_from_cmnd;
-	amount_left_to_write = common->data_size_from_cmnd;
-
-	while (amount_left_to_write > 0) {
-
-		/* Queue a request for more data from the host */
-		bh = common->next_buffhd_to_fill;
-		if (bh->state == BUF_STATE_EMPTY && get_some_more) {
-
-			/*
-			 * Figure out how much we want to get:
-			 * Try to get the remaining amount,
-			 * but not more than the buffer size.
-			 */
-			amount = min(amount_left_to_req, FSG_BUFLEN);
-
-			/* Beyond the end of the backing file? */
-			if (usb_offset >= curlun->file_length) {
-				get_some_more = 0;
-				curlun->sense_data =
-					SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
-				curlun->sense_data_info =
-					usb_offset >> curlun->blkbits;
-				curlun->info_valid = 1;
-				continue;
-			}
-
-			/* Get the next buffer */
-			usb_offset += amount;
-			common->usb_amount_left -= amount;
-			amount_left_to_req -= amount;
-			if (amount_left_to_req == 0)
-				get_some_more = 0;
-
-			/*
-			 * Except at the end of the transfer, amount will be
-			 * equal to the buffer size, which is divisible by
-			 * the bulk-out maxpacket size.
-			 */
-			set_bulk_out_req_length(common, bh, amount);
-			if (!start_out_transfer(common, bh))
-				/* Dunno what to do if common->fsg is NULL */
-				return -EIO;
-			common->next_buffhd_to_fill = bh->next;
-			continue;
-		}
-
-		/* Write the received data to the backing file */
-		bh = common->next_buffhd_to_drain;
-		if (bh->state == BUF_STATE_EMPTY && !get_some_more)
-			break;			/* We stopped early */
-		if (bh->state == BUF_STATE_FULL) {
-			smp_rmb();
-			common->next_buffhd_to_drain = bh->next;
-			bh->state = BUF_STATE_EMPTY;
-
-			/* Did something go wrong with the transfer? */
-			if (bh->outreq->status != 0) {
-				curlun->sense_data = SS_COMMUNICATION_FAILURE;
-				curlun->sense_data_info =
-					file_offset >> curlun->blkbits;
-				curlun->info_valid = 1;
-				break;
-			}
-
-			amount = bh->outreq->actual;
-			if (curlun->file_length - file_offset < amount) {
-				LERROR(curlun,
-				       "write %u @ %llu beyond end %llu\n",
-				       amount, (unsigned long long)file_offset,
-				       (unsigned long long)curlun->file_length);
-				amount = curlun->file_length - file_offset;
-			}
-
-			/* Don't accept excess data.  The spec doesn't say
-			 * what to do in this case.  We'll ignore the error.
-			 */
-			amount = min(amount, bh->bulk_out_intended_length);
-
-			/* Don't write a partial block */
-			amount = round_down(amount, curlun->blksize);
-			if (amount == 0)
-				goto empty_write;
-
-			/* Perform the write */
-			file_offset_tmp = file_offset;
-			nwritten = vfs_write(curlun->filp,
-					     (char __user *)bh->buf,
-					     amount, &file_offset_tmp);
-			VLDBG(curlun, "file write %u @ %llu -> %d\n", amount,
-			      (unsigned long long)file_offset, (int)nwritten);
-			if (signal_pending(current))
-				return -EINTR;		/* Interrupted! */
-
-			if (nwritten < 0) {
-				LDBG(curlun, "error in file write: %d\n",
-				     (int)nwritten);
-				nwritten = 0;
-			} else if (nwritten < amount) {
-				LDBG(curlun, "partial file write: %d/%u\n",
-				     (int)nwritten, amount);
-				nwritten = round_down(nwritten, curlun->blksize);
-			}
-			file_offset += nwritten;
-			amount_left_to_write -= nwritten;
-			common->residue -= nwritten;
-
-			/* If an error occurred, report it and its position */
-			if (nwritten < amount) {
-				curlun->sense_data = SS_WRITE_ERROR;
-				curlun->sense_data_info =
-					file_offset >> curlun->blkbits;
-				curlun->info_valid = 1;
-				break;
-			}
-
- empty_write:
-			/* Did the host decide to stop early? */
-			if (bh->outreq->actual < bh->bulk_out_intended_length) {
-				common->short_packet_received = 1;
-				break;
-			}
-			continue;
-		}
-
-		/* Wait for something to happen */
-		rc = sleep_thread(common);
-		if (rc)
-			return rc;
-	}
-
-	return -EIO;		/* No default reply */
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-static int do_synchronize_cache(struct fsg_common *common)
-{
-	struct fsg_lun	*curlun = common->curlun;
-	int		rc;
-
-	/* We ignore the requested LBA and write out all file's
-	 * dirty data buffers. */
-	rc = fsg_lun_fsync_sub(curlun);
-	if (rc)
-		curlun->sense_data = SS_WRITE_ERROR;
-	return 0;
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-static void invalidate_sub(struct fsg_lun *curlun)
-{
-	struct file	*filp = curlun->filp;
-	struct inode	*inode = file_inode(filp);
-	unsigned long	rc;
-
-	rc = invalidate_mapping_pages(inode->i_mapping, 0, -1);
-	VLDBG(curlun, "invalidate_mapping_pages -> %ld\n", rc);
-}
-
-static int do_verify(struct fsg_common *common)
-{
-	struct fsg_lun		*curlun = common->curlun;
-	u32			lba;
-	u32			verification_length;
-	struct fsg_buffhd	*bh = common->next_buffhd_to_fill;
-	loff_t			file_offset, file_offset_tmp;
-	u32			amount_left;
-	unsigned int		amount;
-	ssize_t			nread;
-
-	/*
-	 * Get the starting Logical Block Address and check that it's
-	 * not too big.
-	 */
-	lba = get_unaligned_be32(&common->cmnd[2]);
-	if (lba >= curlun->num_sectors) {
-		curlun->sense_data = SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
-		return -EINVAL;
-	}
-
-	/*
-	 * We allow DPO (Disable Page Out = don't save data in the
-	 * cache) but we don't implement it.
-	 */
-	if (common->cmnd[1] & ~0x10) {
-		curlun->sense_data = SS_INVALID_FIELD_IN_CDB;
-		return -EINVAL;
-	}
-
-	verification_length = get_unaligned_be16(&common->cmnd[7]);
-	if (unlikely(verification_length == 0))
-		return -EIO;		/* No default reply */
-
-	/* Prepare to carry out the file verify */
-	amount_left = verification_length << curlun->blkbits;
-	file_offset = ((loff_t) lba) << curlun->blkbits;
-
-	/* Write out all the dirty buffers before invalidating them */
-	fsg_lun_fsync_sub(curlun);
-	if (signal_pending(current))
-		return -EINTR;
-
-	invalidate_sub(curlun);
-	if (signal_pending(current))
-		return -EINTR;
-
-	/* Just try to read the requested blocks */
-	while (amount_left > 0) {
-		/*
-		 * Figure out how much we need to read:
-		 * Try to read the remaining amount, but not more than
-		 * the buffer size.
-		 * And don't try to read past the end of the file.
-		 */
-		amount = min(amount_left, FSG_BUFLEN);
-		amount = min((loff_t)amount,
-			     curlun->file_length - file_offset);
-		if (amount == 0) {
-			curlun->sense_data =
-					SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
-			curlun->sense_data_info =
-				file_offset >> curlun->blkbits;
-			curlun->info_valid = 1;
-			break;
-		}
-
-		/* Perform the read */
-		file_offset_tmp = file_offset;
-		nread = vfs_read(curlun->filp,
-				(char __user *) bh->buf,
-				amount, &file_offset_tmp);
-		VLDBG(curlun, "file read %u @ %llu -> %d\n", amount,
-				(unsigned long long) file_offset,
-				(int) nread);
-		if (signal_pending(current))
-			return -EINTR;
-
-		if (nread < 0) {
-			LDBG(curlun, "error in file verify: %d\n", (int)nread);
-			nread = 0;
-		} else if (nread < amount) {
-			LDBG(curlun, "partial file verify: %d/%u\n",
-			     (int)nread, amount);
-			nread = round_down(nread, curlun->blksize);
-		}
-		if (nread == 0) {
-			curlun->sense_data = SS_UNRECOVERED_READ_ERROR;
-			curlun->sense_data_info =
-				file_offset >> curlun->blkbits;
-			curlun->info_valid = 1;
-			break;
-		}
-		file_offset += nread;
-		amount_left -= nread;
-	}
-	return 0;
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-static int do_inquiry(struct fsg_common *common, struct fsg_buffhd *bh)
-{
-	struct fsg_lun *curlun = common->curlun;
-	u8	*buf = (u8 *) bh->buf;
-
-	if (!curlun) {		/* Unsupported LUNs are okay */
-		common->bad_lun_okay = 1;
-		memset(buf, 0, 36);
-		buf[0] = 0x7f;		/* Unsupported, no device-type */
-		buf[4] = 31;		/* Additional length */
-		return 36;
-	}
-
-	buf[0] = curlun->cdrom ? TYPE_ROM : TYPE_DISK;
-	buf[1] = curlun->removable ? 0x80 : 0;
-	buf[2] = 2;		/* ANSI SCSI level 2 */
-	buf[3] = 2;		/* SCSI-2 INQUIRY data format */
-	buf[4] = 31;		/* Additional length */
-	buf[5] = 0;		/* No special options */
-	buf[6] = 0;
-	buf[7] = 0;
-	memcpy(buf + 8, common->inquiry_string, sizeof common->inquiry_string);
-	return 36;
-}
-
-static int do_request_sense(struct fsg_common *common, struct fsg_buffhd *bh)
-{
-	struct fsg_lun	*curlun = common->curlun;
-	u8		*buf = (u8 *) bh->buf;
-	u32		sd, sdinfo;
-	int		valid;
-
-	/*
-	 * From the SCSI-2 spec., section 7.9 (Unit attention condition):
-	 *
-	 * If a REQUEST SENSE command is received from an initiator
-	 * with a pending unit attention condition (before the target
-	 * generates the contingent allegiance condition), then the
-	 * target shall either:
-	 *   a) report any pending sense data and preserve the unit
-	 *	attention condition on the logical unit, or,
-	 *   b) report the unit attention condition, may discard any
-	 *	pending sense data, and clear the unit attention
-	 *	condition on the logical unit for that initiator.
-	 *
-	 * FSG normally uses option a); enable this code to use option b).
-	 */
-#if 0
-	if (curlun && curlun->unit_attention_data != SS_NO_SENSE) {
-		curlun->sense_data = curlun->unit_attention_data;
-		curlun->unit_attention_data = SS_NO_SENSE;
-	}
-#endif
-
-	if (!curlun) {		/* Unsupported LUNs are okay */
-		common->bad_lun_okay = 1;
-		sd = SS_LOGICAL_UNIT_NOT_SUPPORTED;
-		sdinfo = 0;
-		valid = 0;
-	} else {
-		sd = curlun->sense_data;
-		sdinfo = curlun->sense_data_info;
-		valid = curlun->info_valid << 7;
-		curlun->sense_data = SS_NO_SENSE;
-		curlun->sense_data_info = 0;
-		curlun->info_valid = 0;
-	}
-
-	memset(buf, 0, 18);
-	buf[0] = valid | 0x70;			/* Valid, current error */
-	buf[2] = SK(sd);
-	put_unaligned_be32(sdinfo, &buf[3]);	/* Sense information */
-	buf[7] = 18 - 8;			/* Additional sense length */
-	buf[12] = ASC(sd);
-	buf[13] = ASCQ(sd);
-	return 18;
-}
-
-static int do_read_capacity(struct fsg_common *common, struct fsg_buffhd *bh)
-{
-	struct fsg_lun	*curlun = common->curlun;
-	u32		lba = get_unaligned_be32(&common->cmnd[2]);
-	int		pmi = common->cmnd[8];
-	u8		*buf = (u8 *)bh->buf;
-
-	/* Check the PMI and LBA fields */
-	if (pmi > 1 || (pmi == 0 && lba != 0)) {
-		curlun->sense_data = SS_INVALID_FIELD_IN_CDB;
-		return -EINVAL;
-	}
-
-	put_unaligned_be32(curlun->num_sectors - 1, &buf[0]);
-						/* Max logical block */
-	put_unaligned_be32(curlun->blksize, &buf[4]);/* Block length */
-	return 8;
-}
-
-static int do_read_header(struct fsg_common *common, struct fsg_buffhd *bh)
-{
-	struct fsg_lun	*curlun = common->curlun;
-	int		msf = common->cmnd[1] & 0x02;
-	u32		lba = get_unaligned_be32(&common->cmnd[2]);
-	u8		*buf = (u8 *)bh->buf;
-
-	if (common->cmnd[1] & ~0x02) {		/* Mask away MSF */
-		curlun->sense_data = SS_INVALID_FIELD_IN_CDB;
-		return -EINVAL;
-	}
-	if (lba >= curlun->num_sectors) {
-		curlun->sense_data = SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
-		return -EINVAL;
-	}
-
-	memset(buf, 0, 8);
-	buf[0] = 0x01;		/* 2048 bytes of user data, rest is EC */
-	store_cdrom_address(&buf[4], msf, lba);
-	return 8;
-}
-
-static int do_read_toc(struct fsg_common *common, struct fsg_buffhd *bh)
-{
-	struct fsg_lun	*curlun = common->curlun;
-	int		msf = common->cmnd[1] & 0x02;
-	int		start_track = common->cmnd[6];
-	u8		*buf = (u8 *)bh->buf;
-
-	if ((common->cmnd[1] & ~0x02) != 0 ||	/* Mask away MSF */
-			start_track > 1) {
-		curlun->sense_data = SS_INVALID_FIELD_IN_CDB;
-		return -EINVAL;
-	}
-
-	memset(buf, 0, 20);
-	buf[1] = (20-2);		/* TOC data length */
-	buf[2] = 1;			/* First track number */
-	buf[3] = 1;			/* Last track number */
-	buf[5] = 0x16;			/* Data track, copying allowed */
-	buf[6] = 0x01;			/* Only track is number 1 */
-	store_cdrom_address(&buf[8], msf, 0);
-
-	buf[13] = 0x16;			/* Lead-out track is data */
-	buf[14] = 0xAA;			/* Lead-out track number */
-	store_cdrom_address(&buf[16], msf, curlun->num_sectors);
-	return 20;
-}
-
-static int do_mode_sense(struct fsg_common *common, struct fsg_buffhd *bh)
-{
-	struct fsg_lun	*curlun = common->curlun;
-	int		mscmnd = common->cmnd[0];
-	u8		*buf = (u8 *) bh->buf;
-	u8		*buf0 = buf;
-	int		pc, page_code;
-	int		changeable_values, all_pages;
-	int		valid_page = 0;
-	int		len, limit;
-
-	if ((common->cmnd[1] & ~0x08) != 0) {	/* Mask away DBD */
-		curlun->sense_data = SS_INVALID_FIELD_IN_CDB;
-		return -EINVAL;
-	}
-	pc = common->cmnd[2] >> 6;
-	page_code = common->cmnd[2] & 0x3f;
-	if (pc == 3) {
-		curlun->sense_data = SS_SAVING_PARAMETERS_NOT_SUPPORTED;
-		return -EINVAL;
-	}
-	changeable_values = (pc == 1);
-	all_pages = (page_code == 0x3f);
-
-	/*
-	 * Write the mode parameter header.  Fixed values are: default
-	 * medium type, no cache control (DPOFUA), and no block descriptors.
-	 * The only variable value is the WriteProtect bit.  We will fill in
-	 * the mode data length later.
-	 */
-	memset(buf, 0, 8);
-	if (mscmnd == MODE_SENSE) {
-		buf[2] = (curlun->ro ? 0x80 : 0x00);		/* WP, DPOFUA */
-		buf += 4;
-		limit = 255;
-	} else {			/* MODE_SENSE_10 */
-		buf[3] = (curlun->ro ? 0x80 : 0x00);		/* WP, DPOFUA */
-		buf += 8;
-		limit = 65535;		/* Should really be FSG_BUFLEN */
-	}
-
-	/* No block descriptors */
-
-	/*
-	 * The mode pages, in numerical order.  The only page we support
-	 * is the Caching page.
-	 */
-	if (page_code == 0x08 || all_pages) {
-		valid_page = 1;
-		buf[0] = 0x08;		/* Page code */
-		buf[1] = 10;		/* Page length */
-		memset(buf+2, 0, 10);	/* None of the fields are changeable */
-
-		if (!changeable_values) {
-			buf[2] = 0x04;	/* Write cache enable, */
-					/* Read cache not disabled */
-					/* No cache retention priorities */
-			put_unaligned_be16(0xffff, &buf[4]);
-					/* Don't disable prefetch */
-					/* Minimum prefetch = 0 */
-			put_unaligned_be16(0xffff, &buf[8]);
-					/* Maximum prefetch */
-			put_unaligned_be16(0xffff, &buf[10]);
-					/* Maximum prefetch ceiling */
-		}
-		buf += 12;
-	}
-
-	/*
-	 * Check that a valid page was requested and the mode data length
-	 * isn't too long.
-	 */
-	len = buf - buf0;
-	if (!valid_page || len > limit) {
-		curlun->sense_data = SS_INVALID_FIELD_IN_CDB;
-		return -EINVAL;
-	}
-
-	/*  Store the mode data length */
-	if (mscmnd == MODE_SENSE)
-		buf0[0] = len - 1;
-	else
-		put_unaligned_be16(len - 2, buf0);
-	return len;
-}
-
-static int do_start_stop(struct fsg_common *common)
-{
-	struct fsg_lun	*curlun = common->curlun;
-	int		loej, start;
-
-	if (!curlun) {
-		return -EINVAL;
-	} else if (!curlun->removable) {
-		curlun->sense_data = SS_INVALID_COMMAND;
-		return -EINVAL;
-	} else if ((common->cmnd[1] & ~0x01) != 0 || /* Mask away Immed */
-		   (common->cmnd[4] & ~0x03) != 0) { /* Mask LoEj, Start */
-		curlun->sense_data = SS_INVALID_FIELD_IN_CDB;
-		return -EINVAL;
-	}
-
-	loej  = common->cmnd[4] & 0x02;
-	start = common->cmnd[4] & 0x01;
-
-	/*
-	 * Our emulation doesn't support mounting; the medium is
-	 * available for use as soon as it is loaded.
-	 */
-	if (start) {
-		if (!fsg_lun_is_open(curlun)) {
-			curlun->sense_data = SS_MEDIUM_NOT_PRESENT;
-			return -EINVAL;
-		}
-		return 0;
-	}
-
-	/* Are we allowed to unload the media? */
-	if (curlun->prevent_medium_removal) {
-		LDBG(curlun, "unload attempt prevented\n");
-		curlun->sense_data = SS_MEDIUM_REMOVAL_PREVENTED;
-		return -EINVAL;
-	}
-
-	if (!loej)
-		return 0;
-
-	/* Simulate an unload/eject */
-	if (common->ops && common->ops->pre_eject) {
-		int r = common->ops->pre_eject(common, curlun,
-					       curlun - common->luns);
-		if (unlikely(r < 0))
-			return r;
-		else if (r)
-			return 0;
-	}
-
-	up_read(&common->filesem);
-	down_write(&common->filesem);
-	fsg_lun_close(curlun);
-	up_write(&common->filesem);
-	down_read(&common->filesem);
-
-	return common->ops && common->ops->post_eject
-		? min(0, common->ops->post_eject(common, curlun,
-						 curlun - common->luns))
-		: 0;
-}
-
-static int do_prevent_allow(struct fsg_common *common)
-{
-	struct fsg_lun	*curlun = common->curlun;
-	int		prevent;
-
-	if (!common->curlun) {
-		return -EINVAL;
-	} else if (!common->curlun->removable) {
-		common->curlun->sense_data = SS_INVALID_COMMAND;
-		return -EINVAL;
-	}
-
-	prevent = common->cmnd[4] & 0x01;
-	if ((common->cmnd[4] & ~0x01) != 0) {	/* Mask away Prevent */
-		curlun->sense_data = SS_INVALID_FIELD_IN_CDB;
-		return -EINVAL;
-	}
-
-	if (curlun->prevent_medium_removal && !prevent)
-		fsg_lun_fsync_sub(curlun);
-	curlun->prevent_medium_removal = prevent;
-	return 0;
-}
-
-static int do_read_format_capacities(struct fsg_common *common,
-			struct fsg_buffhd *bh)
-{
-	struct fsg_lun	*curlun = common->curlun;
-	u8		*buf = (u8 *) bh->buf;
-
-	buf[0] = buf[1] = buf[2] = 0;
-	buf[3] = 8;	/* Only the Current/Maximum Capacity Descriptor */
-	buf += 4;
-
-	put_unaligned_be32(curlun->num_sectors, &buf[0]);
-						/* Number of blocks */
-	put_unaligned_be32(curlun->blksize, &buf[4]);/* Block length */
-	buf[4] = 0x02;				/* Current capacity */
-	return 12;
-}
-
-static int do_mode_select(struct fsg_common *common, struct fsg_buffhd *bh)
-{
-	struct fsg_lun	*curlun = common->curlun;
-
-	/* We don't support MODE SELECT */
-	if (curlun)
-		curlun->sense_data = SS_INVALID_COMMAND;
-	return -EINVAL;
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-static int halt_bulk_in_endpoint(struct fsg_dev *fsg)
-{
-	int	rc;
-
-	rc = fsg_set_halt(fsg, fsg->bulk_in);
-	if (rc == -EAGAIN)
-		VDBG(fsg, "delayed bulk-in endpoint halt\n");
-	while (rc != 0) {
-		if (rc != -EAGAIN) {
-			WARNING(fsg, "usb_ep_set_halt -> %d\n", rc);
-			rc = 0;
-			break;
-		}
-
-		/* Wait for a short time and then try again */
-		if (msleep_interruptible(100) != 0)
-			return -EINTR;
-		rc = usb_ep_set_halt(fsg->bulk_in);
-	}
-	return rc;
-}
-
-static int wedge_bulk_in_endpoint(struct fsg_dev *fsg)
-{
-	int	rc;
-
-	DBG(fsg, "bulk-in set wedge\n");
-	rc = usb_ep_set_wedge(fsg->bulk_in);
-	if (rc == -EAGAIN)
-		VDBG(fsg, "delayed bulk-in endpoint wedge\n");
-	while (rc != 0) {
-		if (rc != -EAGAIN) {
-			WARNING(fsg, "usb_ep_set_wedge -> %d\n", rc);
-			rc = 0;
-			break;
-		}
-
-		/* Wait for a short time and then try again */
-		if (msleep_interruptible(100) != 0)
-			return -EINTR;
-		rc = usb_ep_set_wedge(fsg->bulk_in);
-	}
-	return rc;
-}
-
-static int throw_away_data(struct fsg_common *common)
-{
-	struct fsg_buffhd	*bh;
-	u32			amount;
-	int			rc;
-
-	for (bh = common->next_buffhd_to_drain;
-	     bh->state != BUF_STATE_EMPTY || common->usb_amount_left > 0;
-	     bh = common->next_buffhd_to_drain) {
-
-		/* Throw away the data in a filled buffer */
-		if (bh->state == BUF_STATE_FULL) {
-			smp_rmb();
-			bh->state = BUF_STATE_EMPTY;
-			common->next_buffhd_to_drain = bh->next;
-
-			/* A short packet or an error ends everything */
-			if (bh->outreq->actual < bh->bulk_out_intended_length ||
-			    bh->outreq->status != 0) {
-				raise_exception(common,
-						FSG_STATE_ABORT_BULK_OUT);
-				return -EINTR;
-			}
-			continue;
-		}
-
-		/* Try to submit another request if we need one */
-		bh = common->next_buffhd_to_fill;
-		if (bh->state == BUF_STATE_EMPTY
-		 && common->usb_amount_left > 0) {
-			amount = min(common->usb_amount_left, FSG_BUFLEN);
-
-			/*
-			 * Except at the end of the transfer, amount will be
-			 * equal to the buffer size, which is divisible by
-			 * the bulk-out maxpacket size.
-			 */
-			set_bulk_out_req_length(common, bh, amount);
-			if (!start_out_transfer(common, bh))
-				/* Dunno what to do if common->fsg is NULL */
-				return -EIO;
-			common->next_buffhd_to_fill = bh->next;
-			common->usb_amount_left -= amount;
-			continue;
-		}
-
-		/* Otherwise wait for something to happen */
-		rc = sleep_thread(common);
-		if (rc)
-			return rc;
-	}
-	return 0;
-}
-
-static int finish_reply(struct fsg_common *common)
-{
-	struct fsg_buffhd	*bh = common->next_buffhd_to_fill;
-	int			rc = 0;
-
-	switch (common->data_dir) {
-	case DATA_DIR_NONE:
-		break;			/* Nothing to send */
-
-	/*
-	 * If we don't know whether the host wants to read or write,
-	 * this must be CB or CBI with an unknown command.  We mustn't
-	 * try to send or receive any data.  So stall both bulk pipes
-	 * if we can and wait for a reset.
-	 */
-	case DATA_DIR_UNKNOWN:
-		if (!common->can_stall) {
-			/* Nothing */
-		} else if (fsg_is_set(common)) {
-			fsg_set_halt(common->fsg, common->fsg->bulk_out);
-			rc = halt_bulk_in_endpoint(common->fsg);
-		} else {
-			/* Don't know what to do if common->fsg is NULL */
-			rc = -EIO;
-		}
-		break;
-
-	/* All but the last buffer of data must have already been sent */
-	case DATA_DIR_TO_HOST:
-		if (common->data_size == 0) {
-			/* Nothing to send */
-
-		/* Don't know what to do if common->fsg is NULL */
-		} else if (!fsg_is_set(common)) {
-			rc = -EIO;
-
-		/* If there's no residue, simply send the last buffer */
-		} else if (common->residue == 0) {
-			bh->inreq->zero = 0;
-			if (!start_in_transfer(common, bh))
-				return -EIO;
-			common->next_buffhd_to_fill = bh->next;
-
-		/*
-		 * For Bulk-only, mark the end of the data with a short
-		 * packet.  If we are allowed to stall, halt the bulk-in
-		 * endpoint.  (Note: This violates the Bulk-Only Transport
-		 * specification, which requires us to pad the data if we
-		 * don't halt the endpoint.  Presumably nobody will mind.)
-		 */
-		} else {
-			bh->inreq->zero = 1;
-			if (!start_in_transfer(common, bh))
-				rc = -EIO;
-			common->next_buffhd_to_fill = bh->next;
-			if (common->can_stall)
-				rc = halt_bulk_in_endpoint(common->fsg);
-		}
-		break;
-
-	/*
-	 * We have processed all we want from the data the host has sent.
-	 * There may still be outstanding bulk-out requests.
-	 */
-	case DATA_DIR_FROM_HOST:
-		if (common->residue == 0) {
-			/* Nothing to receive */
-
-		/* Did the host stop sending unexpectedly early? */
-		} else if (common->short_packet_received) {
-			raise_exception(common, FSG_STATE_ABORT_BULK_OUT);
-			rc = -EINTR;
-
-		/*
-		 * We haven't processed all the incoming data.  Even though
-		 * we may be allowed to stall, doing so would cause a race.
-		 * The controller may already have ACK'ed all the remaining
-		 * bulk-out packets, in which case the host wouldn't see a
-		 * STALL.  Not realizing the endpoint was halted, it wouldn't
-		 * clear the halt -- leading to problems later on.
-		 */
-#if 0
-		} else if (common->can_stall) {
-			if (fsg_is_set(common))
-				fsg_set_halt(common->fsg,
-					     common->fsg->bulk_out);
-			raise_exception(common, FSG_STATE_ABORT_BULK_OUT);
-			rc = -EINTR;
-#endif
-
-		/*
-		 * We can't stall.  Read in the excess data and throw it
-		 * all away.
-		 */
-		} else {
-			rc = throw_away_data(common);
-		}
-		break;
-	}
-	return rc;
-}
-
-static int send_status(struct fsg_common *common)
-{
-	struct fsg_lun		*curlun = common->curlun;
-	struct fsg_buffhd	*bh;
-	struct bulk_cs_wrap	*csw;
-	int			rc;
-	u8			status = US_BULK_STAT_OK;
-	u32			sd, sdinfo = 0;
-
-	/* Wait for the next buffer to become available */
-	bh = common->next_buffhd_to_fill;
-	while (bh->state != BUF_STATE_EMPTY) {
-		rc = sleep_thread(common);
-		if (rc)
-			return rc;
-	}
-
-	if (curlun) {
-		sd = curlun->sense_data;
-		sdinfo = curlun->sense_data_info;
-	} else if (common->bad_lun_okay)
-		sd = SS_NO_SENSE;
-	else
-		sd = SS_LOGICAL_UNIT_NOT_SUPPORTED;
-
-	if (common->phase_error) {
-		DBG(common, "sending phase-error status\n");
-		status = US_BULK_STAT_PHASE;
-		sd = SS_INVALID_COMMAND;
-	} else if (sd != SS_NO_SENSE) {
-		DBG(common, "sending command-failure status\n");
-		status = US_BULK_STAT_FAIL;
-		VDBG(common, "  sense data: SK x%02x, ASC x%02x, ASCQ x%02x;"
-				"  info x%x\n",
-				SK(sd), ASC(sd), ASCQ(sd), sdinfo);
-	}
-
-	/* Store and send the Bulk-only CSW */
-	csw = (void *)bh->buf;
-
-	csw->Signature = cpu_to_le32(US_BULK_CS_SIGN);
-	csw->Tag = common->tag;
-	csw->Residue = cpu_to_le32(common->residue);
-	csw->Status = status;
-
-	bh->inreq->length = US_BULK_CS_WRAP_LEN;
-	bh->inreq->zero = 0;
-	if (!start_in_transfer(common, bh))
-		/* Don't know what to do if common->fsg is NULL */
-		return -EIO;
-
-	common->next_buffhd_to_fill = bh->next;
-	return 0;
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-/*
- * Check whether the command is properly formed and whether its data size
- * and direction agree with the values we already have.
- */
-static int check_command(struct fsg_common *common, int cmnd_size,
-			 enum data_direction data_dir, unsigned int mask,
-			 int needs_medium, const char *name)
-{
-	int			i;
-	int			lun = common->cmnd[1] >> 5;
-	static const char	dirletter[4] = {'u', 'o', 'i', 'n'};
-	char			hdlen[20];
-	struct fsg_lun		*curlun;
-
-	hdlen[0] = 0;
-	if (common->data_dir != DATA_DIR_UNKNOWN)
-		sprintf(hdlen, ", H%c=%u", dirletter[(int) common->data_dir],
-			common->data_size);
-	VDBG(common, "SCSI command: %s;  Dc=%d, D%c=%u;  Hc=%d%s\n",
-	     name, cmnd_size, dirletter[(int) data_dir],
-	     common->data_size_from_cmnd, common->cmnd_size, hdlen);
-
-	/*
-	 * We can't reply at all until we know the correct data direction
-	 * and size.
-	 */
-	if (common->data_size_from_cmnd == 0)
-		data_dir = DATA_DIR_NONE;
-	if (common->data_size < common->data_size_from_cmnd) {
-		/*
-		 * Host data size < Device data size is a phase error.
-		 * Carry out the command, but only transfer as much as
-		 * we are allowed.
-		 */
-		common->data_size_from_cmnd = common->data_size;
-		common->phase_error = 1;
-	}
-	common->residue = common->data_size;
-	common->usb_amount_left = common->data_size;
-
-	/* Conflicting data directions is a phase error */
-	if (common->data_dir != data_dir && common->data_size_from_cmnd > 0) {
-		common->phase_error = 1;
-		return -EINVAL;
-	}
-
-	/* Verify the length of the command itself */
-	if (cmnd_size != common->cmnd_size) {
-
-		/*
-		 * Special case workaround: There are plenty of buggy SCSI
-		 * implementations. Many have issues with cbw->Length
-		 * field passing a wrong command size. For those cases we
-		 * always try to work around the problem by using the length
-		 * sent by the host side provided it is at least as large
-		 * as the correct command length.
-		 * Examples of such cases would be MS-Windows, which issues
-		 * REQUEST SENSE with cbw->Length == 12 where it should
-		 * be 6, and xbox360 issuing INQUIRY, TEST UNIT READY and
-		 * REQUEST SENSE with cbw->Length == 10 where it should
-		 * be 6 as well.
-		 */
-		if (cmnd_size <= common->cmnd_size) {
-			DBG(common, "%s is buggy! Expected length %d "
-			    "but we got %d\n", name,
-			    cmnd_size, common->cmnd_size);
-			cmnd_size = common->cmnd_size;
-		} else {
-			common->phase_error = 1;
-			return -EINVAL;
-		}
-	}
-
-	/* Check that the LUN values are consistent */
-	if (common->lun != lun)
-		DBG(common, "using LUN %d from CBW, not LUN %d from CDB\n",
-		    common->lun, lun);
-
-	/* Check the LUN */
-	curlun = common->curlun;
-	if (curlun) {
-		if (common->cmnd[0] != REQUEST_SENSE) {
-			curlun->sense_data = SS_NO_SENSE;
-			curlun->sense_data_info = 0;
-			curlun->info_valid = 0;
-		}
-	} else {
-		common->bad_lun_okay = 0;
-
-		/*
-		 * INQUIRY and REQUEST SENSE commands are explicitly allowed
-		 * to use unsupported LUNs; all others may not.
-		 */
-		if (common->cmnd[0] != INQUIRY &&
-		    common->cmnd[0] != REQUEST_SENSE) {
-			DBG(common, "unsupported LUN %d\n", common->lun);
-			return -EINVAL;
-		}
-	}
-
-	/*
-	 * If a unit attention condition exists, only INQUIRY and
-	 * REQUEST SENSE commands are allowed; anything else must fail.
-	 */
-	if (curlun && curlun->unit_attention_data != SS_NO_SENSE &&
-	    common->cmnd[0] != INQUIRY &&
-	    common->cmnd[0] != REQUEST_SENSE) {
-		curlun->sense_data = curlun->unit_attention_data;
-		curlun->unit_attention_data = SS_NO_SENSE;
-		return -EINVAL;
-	}
-
-	/* Check that only command bytes listed in the mask are non-zero */
-	common->cmnd[1] &= 0x1f;			/* Mask away the LUN */
-	for (i = 1; i < cmnd_size; ++i) {
-		if (common->cmnd[i] && !(mask & (1 << i))) {
-			if (curlun)
-				curlun->sense_data = SS_INVALID_FIELD_IN_CDB;
-			return -EINVAL;
-		}
-	}
-
-	/* If the medium isn't mounted and the command needs to access
-	 * it, return an error. */
-	if (curlun && !fsg_lun_is_open(curlun) && needs_medium) {
-		curlun->sense_data = SS_MEDIUM_NOT_PRESENT;
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-/* wrapper of check_command for data size in blocks handling */
-static int check_command_size_in_blocks(struct fsg_common *common,
-		int cmnd_size, enum data_direction data_dir,
-		unsigned int mask, int needs_medium, const char *name)
-{
-	if (common->curlun)
-		common->data_size_from_cmnd <<= common->curlun->blkbits;
-	return check_command(common, cmnd_size, data_dir,
-			mask, needs_medium, name);
-}
-
-static int do_scsi_command(struct fsg_common *common)
-{
-	struct fsg_buffhd	*bh;
-	int			rc;
-	int			reply = -EINVAL;
-	int			i;
-	static char		unknown[16];
-
-	dump_cdb(common);
-
-	/* Wait for the next buffer to become available for data or status */
-	bh = common->next_buffhd_to_fill;
-	common->next_buffhd_to_drain = bh;
-	while (bh->state != BUF_STATE_EMPTY) {
-		rc = sleep_thread(common);
-		if (rc)
-			return rc;
-	}
-	common->phase_error = 0;
-	common->short_packet_received = 0;
-
-	down_read(&common->filesem);	/* We're using the backing file */
-	switch (common->cmnd[0]) {
-
-	case INQUIRY:
-		common->data_size_from_cmnd = common->cmnd[4];
-		reply = check_command(common, 6, DATA_DIR_TO_HOST,
-				      (1<<4), 0,
-				      "INQUIRY");
-		if (reply == 0)
-			reply = do_inquiry(common, bh);
-		break;
-
-	case MODE_SELECT:
-		common->data_size_from_cmnd = common->cmnd[4];
-		reply = check_command(common, 6, DATA_DIR_FROM_HOST,
-				      (1<<1) | (1<<4), 0,
-				      "MODE SELECT(6)");
-		if (reply == 0)
-			reply = do_mode_select(common, bh);
-		break;
-
-	case MODE_SELECT_10:
-		common->data_size_from_cmnd =
-			get_unaligned_be16(&common->cmnd[7]);
-		reply = check_command(common, 10, DATA_DIR_FROM_HOST,
-				      (1<<1) | (3<<7), 0,
-				      "MODE SELECT(10)");
-		if (reply == 0)
-			reply = do_mode_select(common, bh);
-		break;
-
-	case MODE_SENSE:
-		common->data_size_from_cmnd = common->cmnd[4];
-		reply = check_command(common, 6, DATA_DIR_TO_HOST,
-				      (1<<1) | (1<<2) | (1<<4), 0,
-				      "MODE SENSE(6)");
-		if (reply == 0)
-			reply = do_mode_sense(common, bh);
-		break;
-
-	case MODE_SENSE_10:
-		common->data_size_from_cmnd =
-			get_unaligned_be16(&common->cmnd[7]);
-		reply = check_command(common, 10, DATA_DIR_TO_HOST,
-				      (1<<1) | (1<<2) | (3<<7), 0,
-				      "MODE SENSE(10)");
-		if (reply == 0)
-			reply = do_mode_sense(common, bh);
-		break;
-
-	case ALLOW_MEDIUM_REMOVAL:
-		common->data_size_from_cmnd = 0;
-		reply = check_command(common, 6, DATA_DIR_NONE,
-				      (1<<4), 0,
-				      "PREVENT-ALLOW MEDIUM REMOVAL");
-		if (reply == 0)
-			reply = do_prevent_allow(common);
-		break;
-
-	case READ_6:
-		i = common->cmnd[4];
-		common->data_size_from_cmnd = (i == 0) ? 256 : i;
-		reply = check_command_size_in_blocks(common, 6,
-				      DATA_DIR_TO_HOST,
-				      (7<<1) | (1<<4), 1,
-				      "READ(6)");
-		if (reply == 0)
-			reply = do_read(common);
-		break;
-
-	case READ_10:
-		common->data_size_from_cmnd =
-				get_unaligned_be16(&common->cmnd[7]);
-		reply = check_command_size_in_blocks(common, 10,
-				      DATA_DIR_TO_HOST,
-				      (1<<1) | (0xf<<2) | (3<<7), 1,
-				      "READ(10)");
-		if (reply == 0)
-			reply = do_read(common);
-		break;
-
-	case READ_12:
-		common->data_size_from_cmnd =
-				get_unaligned_be32(&common->cmnd[6]);
-		reply = check_command_size_in_blocks(common, 12,
-				      DATA_DIR_TO_HOST,
-				      (1<<1) | (0xf<<2) | (0xf<<6), 1,
-				      "READ(12)");
-		if (reply == 0)
-			reply = do_read(common);
-		break;
-
-	case READ_CAPACITY:
-		common->data_size_from_cmnd = 8;
-		reply = check_command(common, 10, DATA_DIR_TO_HOST,
-				      (0xf<<2) | (1<<8), 1,
-				      "READ CAPACITY");
-		if (reply == 0)
-			reply = do_read_capacity(common, bh);
-		break;
-
-	case READ_HEADER:
-		if (!common->curlun || !common->curlun->cdrom)
-			goto unknown_cmnd;
-		common->data_size_from_cmnd =
-			get_unaligned_be16(&common->cmnd[7]);
-		reply = check_command(common, 10, DATA_DIR_TO_HOST,
-				      (3<<7) | (0x1f<<1), 1,
-				      "READ HEADER");
-		if (reply == 0)
-			reply = do_read_header(common, bh);
-		break;
-
-	case READ_TOC:
-		if (!common->curlun || !common->curlun->cdrom)
-			goto unknown_cmnd;
-		common->data_size_from_cmnd =
-			get_unaligned_be16(&common->cmnd[7]);
-		reply = check_command(common, 10, DATA_DIR_TO_HOST,
-				      (7<<6) | (1<<1), 1,
-				      "READ TOC");
-		if (reply == 0)
-			reply = do_read_toc(common, bh);
-		break;
-
-	case READ_FORMAT_CAPACITIES:
-		common->data_size_from_cmnd =
-			get_unaligned_be16(&common->cmnd[7]);
-		reply = check_command(common, 10, DATA_DIR_TO_HOST,
-				      (3<<7), 1,
-				      "READ FORMAT CAPACITIES");
-		if (reply == 0)
-			reply = do_read_format_capacities(common, bh);
-		break;
-
-	case REQUEST_SENSE:
-		common->data_size_from_cmnd = common->cmnd[4];
-		reply = check_command(common, 6, DATA_DIR_TO_HOST,
-				      (1<<4), 0,
-				      "REQUEST SENSE");
-		if (reply == 0)
-			reply = do_request_sense(common, bh);
-		break;
-
-	case START_STOP:
-		common->data_size_from_cmnd = 0;
-		reply = check_command(common, 6, DATA_DIR_NONE,
-				      (1<<1) | (1<<4), 0,
-				      "START-STOP UNIT");
-		if (reply == 0)
-			reply = do_start_stop(common);
-		break;
-
-	case SYNCHRONIZE_CACHE:
-		common->data_size_from_cmnd = 0;
-		reply = check_command(common, 10, DATA_DIR_NONE,
-				      (0xf<<2) | (3<<7), 1,
-				      "SYNCHRONIZE CACHE");
-		if (reply == 0)
-			reply = do_synchronize_cache(common);
-		break;
-
-	case TEST_UNIT_READY:
-		common->data_size_from_cmnd = 0;
-		reply = check_command(common, 6, DATA_DIR_NONE,
-				0, 1,
-				"TEST UNIT READY");
-		break;
-
-	/*
-	 * Although optional, this command is used by MS-Windows.  We
-	 * support a minimal version: BytChk must be 0.
-	 */
-	case VERIFY:
-		common->data_size_from_cmnd = 0;
-		reply = check_command(common, 10, DATA_DIR_NONE,
-				      (1<<1) | (0xf<<2) | (3<<7), 1,
-				      "VERIFY");
-		if (reply == 0)
-			reply = do_verify(common);
-		break;
-
-	case WRITE_6:
-		i = common->cmnd[4];
-		common->data_size_from_cmnd = (i == 0) ? 256 : i;
-		reply = check_command_size_in_blocks(common, 6,
-				      DATA_DIR_FROM_HOST,
-				      (7<<1) | (1<<4), 1,
-				      "WRITE(6)");
-		if (reply == 0)
-			reply = do_write(common);
-		break;
-
-	case WRITE_10:
-		common->data_size_from_cmnd =
-				get_unaligned_be16(&common->cmnd[7]);
-		reply = check_command_size_in_blocks(common, 10,
-				      DATA_DIR_FROM_HOST,
-				      (1<<1) | (0xf<<2) | (3<<7), 1,
-				      "WRITE(10)");
-		if (reply == 0)
-			reply = do_write(common);
-		break;
-
-	case WRITE_12:
-		common->data_size_from_cmnd =
-				get_unaligned_be32(&common->cmnd[6]);
-		reply = check_command_size_in_blocks(common, 12,
-				      DATA_DIR_FROM_HOST,
-				      (1<<1) | (0xf<<2) | (0xf<<6), 1,
-				      "WRITE(12)");
-		if (reply == 0)
-			reply = do_write(common);
-		break;
-
-	/*
-	 * Some mandatory commands that we recognize but don't implement.
-	 * They don't mean much in this setting.  It's left as an exercise
-	 * for anyone interested to implement RESERVE and RELEASE in terms
-	 * of Posix locks.
-	 */
-	case FORMAT_UNIT:
-	case RELEASE:
-	case RESERVE:
-	case SEND_DIAGNOSTIC:
-		/* Fall through */
-
-	default:
-unknown_cmnd:
-		common->data_size_from_cmnd = 0;
-		sprintf(unknown, "Unknown x%02x", common->cmnd[0]);
-		reply = check_command(common, common->cmnd_size,
-				      DATA_DIR_UNKNOWN, ~0, 0, unknown);
-		if (reply == 0) {
-			common->curlun->sense_data = SS_INVALID_COMMAND;
-			reply = -EINVAL;
-		}
-		break;
-	}
-	up_read(&common->filesem);
-
-	if (reply == -EINTR || signal_pending(current))
-		return -EINTR;
-
-	/* Set up the single reply buffer for finish_reply() */
-	if (reply == -EINVAL)
-		reply = 0;		/* Error reply length */
-	if (reply >= 0 && common->data_dir == DATA_DIR_TO_HOST) {
-		reply = min((u32)reply, common->data_size_from_cmnd);
-		bh->inreq->length = reply;
-		bh->state = BUF_STATE_FULL;
-		common->residue -= reply;
-	}				/* Otherwise it's already set */
-
-	return 0;
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-static int received_cbw(struct fsg_dev *fsg, struct fsg_buffhd *bh)
-{
-	struct usb_request	*req = bh->outreq;
-	struct bulk_cb_wrap	*cbw = req->buf;
-	struct fsg_common	*common = fsg->common;
-
-	/* Was this a real packet?  Should it be ignored? */
-	if (req->status || test_bit(IGNORE_BULK_OUT, &fsg->atomic_bitflags))
-		return -EINVAL;
-
-	/* Is the CBW valid? */
-	if (req->actual != US_BULK_CB_WRAP_LEN ||
-			cbw->Signature != cpu_to_le32(
-				US_BULK_CB_SIGN)) {
-		DBG(fsg, "invalid CBW: len %u sig 0x%x\n",
-				req->actual,
-				le32_to_cpu(cbw->Signature));
-
-		/*
-		 * The Bulk-only spec says we MUST stall the IN endpoint
-		 * (6.6.1), so it's unavoidable.  It also says we must
-		 * retain this state until the next reset, but there's
-		 * no way to tell the controller driver it should ignore
-		 * Clear-Feature(HALT) requests.
-		 *
-		 * We aren't required to halt the OUT endpoint; instead
-		 * we can simply accept and discard any data received
-		 * until the next reset.
-		 */
-		wedge_bulk_in_endpoint(fsg);
-		set_bit(IGNORE_BULK_OUT, &fsg->atomic_bitflags);
-		return -EINVAL;
-	}
-
-	/* Is the CBW meaningful? */
-	if (cbw->Lun >= FSG_MAX_LUNS || cbw->Flags & ~US_BULK_FLAG_IN ||
-			cbw->Length <= 0 || cbw->Length > MAX_COMMAND_SIZE) {
-		DBG(fsg, "non-meaningful CBW: lun = %u, flags = 0x%x, "
-				"cmdlen %u\n",
-				cbw->Lun, cbw->Flags, cbw->Length);
-
-		/*
-		 * We can do anything we want here, so let's stall the
-		 * bulk pipes if we are allowed to.
-		 */
-		if (common->can_stall) {
-			fsg_set_halt(fsg, fsg->bulk_out);
-			halt_bulk_in_endpoint(fsg);
-		}
-		return -EINVAL;
-	}
-
-	/* Save the command for later */
-	common->cmnd_size = cbw->Length;
-	memcpy(common->cmnd, cbw->CDB, common->cmnd_size);
-	if (cbw->Flags & US_BULK_FLAG_IN)
-		common->data_dir = DATA_DIR_TO_HOST;
-	else
-		common->data_dir = DATA_DIR_FROM_HOST;
-	common->data_size = le32_to_cpu(cbw->DataTransferLength);
-	if (common->data_size == 0)
-		common->data_dir = DATA_DIR_NONE;
-	common->lun = cbw->Lun;
-	if (common->lun >= 0 && common->lun < common->nluns)
-		common->curlun = &common->luns[common->lun];
-	else
-		common->curlun = NULL;
-	common->tag = cbw->Tag;
-	return 0;
-}
-
-static int get_next_command(struct fsg_common *common)
-{
-	struct fsg_buffhd	*bh;
-	int			rc = 0;
-
-	/* Wait for the next buffer to become available */
-	bh = common->next_buffhd_to_fill;
-	while (bh->state != BUF_STATE_EMPTY) {
-		rc = sleep_thread(common);
-		if (rc)
-			return rc;
-	}
-
-	/* Queue a request to read a Bulk-only CBW */
-	set_bulk_out_req_length(common, bh, US_BULK_CB_WRAP_LEN);
-	if (!start_out_transfer(common, bh))
-		/* Don't know what to do if common->fsg is NULL */
-		return -EIO;
-
-	/*
-	 * We will drain the buffer in software, which means we
-	 * can reuse it for the next filling.  No need to advance
-	 * next_buffhd_to_fill.
-	 */
-
-	/* Wait for the CBW to arrive */
-	while (bh->state != BUF_STATE_FULL) {
-		rc = sleep_thread(common);
-		if (rc)
-			return rc;
-	}
-	smp_rmb();
-	rc = fsg_is_set(common) ? received_cbw(common->fsg, bh) : -EIO;
-	bh->state = BUF_STATE_EMPTY;
-
-	return rc;
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-static int alloc_request(struct fsg_common *common, struct usb_ep *ep,
-		struct usb_request **preq)
-{
-	*preq = usb_ep_alloc_request(ep, GFP_ATOMIC);
-	if (*preq)
-		return 0;
-	ERROR(common, "can't allocate request for %s\n", ep->name);
-	return -ENOMEM;
-}
-
-/* Reset interface setting and re-init endpoint state (toggle etc). */
-static int do_set_interface(struct fsg_common *common, struct fsg_dev *new_fsg)
-{
-	struct fsg_dev *fsg;
-	int i, rc = 0;
-
-	if (common->running)
-		DBG(common, "reset interface\n");
-
-reset:
-	/* Deallocate the requests */
-	if (common->fsg) {
-		fsg = common->fsg;
-
-		for (i = 0; i < fsg_num_buffers; ++i) {
-			struct fsg_buffhd *bh = &common->buffhds[i];
-
-			if (bh->inreq) {
-				usb_ep_free_request(fsg->bulk_in, bh->inreq);
-				bh->inreq = NULL;
-			}
-			if (bh->outreq) {
-				usb_ep_free_request(fsg->bulk_out, bh->outreq);
-				bh->outreq = NULL;
-			}
-		}
-
-		/* Disable the endpoints */
-		if (fsg->bulk_in_enabled) {
-			usb_ep_disable(fsg->bulk_in);
-			fsg->bulk_in_enabled = 0;
-		}
-		if (fsg->bulk_out_enabled) {
-			usb_ep_disable(fsg->bulk_out);
-			fsg->bulk_out_enabled = 0;
-		}
-
-		common->fsg = NULL;
-		wake_up(&common->fsg_wait);
-	}
-
-	common->running = 0;
-	if (!new_fsg || rc)
-		return rc;
-
-	common->fsg = new_fsg;
-	fsg = common->fsg;
-
-	/* Enable the endpoints */
-	rc = config_ep_by_speed(common->gadget, &(fsg->function), fsg->bulk_in);
-	if (rc)
-		goto reset;
-	rc = usb_ep_enable(fsg->bulk_in);
-	if (rc)
-		goto reset;
-	fsg->bulk_in->driver_data = common;
-	fsg->bulk_in_enabled = 1;
-
-	rc = config_ep_by_speed(common->gadget, &(fsg->function),
-				fsg->bulk_out);
-	if (rc)
-		goto reset;
-	rc = usb_ep_enable(fsg->bulk_out);
-	if (rc)
-		goto reset;
-	fsg->bulk_out->driver_data = common;
-	fsg->bulk_out_enabled = 1;
-	common->bulk_out_maxpacket = usb_endpoint_maxp(fsg->bulk_out->desc);
-	clear_bit(IGNORE_BULK_OUT, &fsg->atomic_bitflags);
-
-	/* Allocate the requests */
-	for (i = 0; i < fsg_num_buffers; ++i) {
-		struct fsg_buffhd	*bh = &common->buffhds[i];
-
-		rc = alloc_request(common, fsg->bulk_in, &bh->inreq);
-		if (rc)
-			goto reset;
-		rc = alloc_request(common, fsg->bulk_out, &bh->outreq);
-		if (rc)
-			goto reset;
-		bh->inreq->buf = bh->outreq->buf = bh->buf;
-		bh->inreq->context = bh->outreq->context = bh;
-		bh->inreq->complete = bulk_in_complete;
-		bh->outreq->complete = bulk_out_complete;
-	}
-
-	common->running = 1;
-	for (i = 0; i < common->nluns; ++i)
-		common->luns[i].unit_attention_data = SS_RESET_OCCURRED;
-	return rc;
-}
-
-
-/****************************** ALT CONFIGS ******************************/
-
-static int fsg_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
-{
-	struct fsg_dev *fsg = fsg_from_func(f);
-	fsg->common->new_fsg = fsg;
-	raise_exception(fsg->common, FSG_STATE_CONFIG_CHANGE);
-	return USB_GADGET_DELAYED_STATUS;
-}
-
-static void fsg_disable(struct usb_function *f)
-{
-	struct fsg_dev *fsg = fsg_from_func(f);
-	fsg->common->new_fsg = NULL;
-	raise_exception(fsg->common, FSG_STATE_CONFIG_CHANGE);
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-static void handle_exception(struct fsg_common *common)
-{
-	siginfo_t		info;
-	int			i;
-	struct fsg_buffhd	*bh;
-	enum fsg_state		old_state;
-	struct fsg_lun		*curlun;
-	unsigned int		exception_req_tag;
-
-	/*
-	 * Clear the existing signals.  Anything but SIGUSR1 is converted
-	 * into a high-priority EXIT exception.
-	 */
-	for (;;) {
-		int sig =
-			dequeue_signal_lock(current, &current->blocked, &info);
-		if (!sig)
-			break;
-		if (sig != SIGUSR1) {
-			if (common->state < FSG_STATE_EXIT)
-				DBG(common, "Main thread exiting on signal\n");
-			raise_exception(common, FSG_STATE_EXIT);
-		}
-	}
-
-	/* Cancel all the pending transfers */
-	if (likely(common->fsg)) {
-		for (i = 0; i < fsg_num_buffers; ++i) {
-			bh = &common->buffhds[i];
-			if (bh->inreq_busy)
-				usb_ep_dequeue(common->fsg->bulk_in, bh->inreq);
-			if (bh->outreq_busy)
-				usb_ep_dequeue(common->fsg->bulk_out,
-					       bh->outreq);
-		}
-
-		/* Wait until everything is idle */
-		for (;;) {
-			int num_active = 0;
-			for (i = 0; i < fsg_num_buffers; ++i) {
-				bh = &common->buffhds[i];
-				num_active += bh->inreq_busy + bh->outreq_busy;
-			}
-			if (num_active == 0)
-				break;
-			if (sleep_thread(common))
-				return;
-		}
-
-		/* Clear out the controller's fifos */
-		if (common->fsg->bulk_in_enabled)
-			usb_ep_fifo_flush(common->fsg->bulk_in);
-		if (common->fsg->bulk_out_enabled)
-			usb_ep_fifo_flush(common->fsg->bulk_out);
-	}
-
-	/*
-	 * Reset the I/O buffer states and pointers, the SCSI
-	 * state, and the exception.  Then invoke the handler.
-	 */
-	spin_lock_irq(&common->lock);
-
-	for (i = 0; i < fsg_num_buffers; ++i) {
-		bh = &common->buffhds[i];
-		bh->state = BUF_STATE_EMPTY;
-	}
-	common->next_buffhd_to_fill = &common->buffhds[0];
-	common->next_buffhd_to_drain = &common->buffhds[0];
-	exception_req_tag = common->exception_req_tag;
-	old_state = common->state;
-
-	if (old_state == FSG_STATE_ABORT_BULK_OUT)
-		common->state = FSG_STATE_STATUS_PHASE;
-	else {
-		for (i = 0; i < common->nluns; ++i) {
-			curlun = &common->luns[i];
-			curlun->prevent_medium_removal = 0;
-			curlun->sense_data = SS_NO_SENSE;
-			curlun->unit_attention_data = SS_NO_SENSE;
-			curlun->sense_data_info = 0;
-			curlun->info_valid = 0;
-		}
-		common->state = FSG_STATE_IDLE;
-	}
-	spin_unlock_irq(&common->lock);
-
-	/* Carry out any extra actions required for the exception */
-	switch (old_state) {
-	case FSG_STATE_ABORT_BULK_OUT:
-		send_status(common);
-		spin_lock_irq(&common->lock);
-		if (common->state == FSG_STATE_STATUS_PHASE)
-			common->state = FSG_STATE_IDLE;
-		spin_unlock_irq(&common->lock);
-		break;
-
-	case FSG_STATE_RESET:
-		/*
-		 * In case we were forced against our will to halt a
-		 * bulk endpoint, clear the halt now.  (The SuperH UDC
-		 * requires this.)
-		 */
-		if (!fsg_is_set(common))
-			break;
-		if (test_and_clear_bit(IGNORE_BULK_OUT,
-				       &common->fsg->atomic_bitflags))
-			usb_ep_clear_halt(common->fsg->bulk_in);
-
-		if (common->ep0_req_tag == exception_req_tag)
-			ep0_queue(common);	/* Complete the status stage */
-
-		/*
-		 * Technically this should go here, but it would only be
-		 * a waste of time.  Ditto for the INTERFACE_CHANGE and
-		 * CONFIG_CHANGE cases.
-		 */
-		/* for (i = 0; i < common->nluns; ++i) */
-		/*	common->luns[i].unit_attention_data = */
-		/*		SS_RESET_OCCURRED;  */
-		break;
-
-	case FSG_STATE_CONFIG_CHANGE:
-		do_set_interface(common, common->new_fsg);
-		if (common->new_fsg)
-			usb_composite_setup_continue(common->cdev);
-		break;
-
-	case FSG_STATE_EXIT:
-	case FSG_STATE_TERMINATED:
-		do_set_interface(common, NULL);		/* Free resources */
-		spin_lock_irq(&common->lock);
-		common->state = FSG_STATE_TERMINATED;	/* Stop the thread */
-		spin_unlock_irq(&common->lock);
-		break;
-
-	case FSG_STATE_INTERFACE_CHANGE:
-	case FSG_STATE_DISCONNECT:
-	case FSG_STATE_COMMAND_PHASE:
-	case FSG_STATE_DATA_PHASE:
-	case FSG_STATE_STATUS_PHASE:
-	case FSG_STATE_IDLE:
-		break;
-	}
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-static int fsg_main_thread(void *common_)
-{
-	struct fsg_common	*common = common_;
-
-	/*
-	 * Allow the thread to be killed by a signal, but set the signal mask
-	 * to block everything but INT, TERM, KILL, and USR1.
-	 */
-	allow_signal(SIGINT);
-	allow_signal(SIGTERM);
-	allow_signal(SIGKILL);
-	allow_signal(SIGUSR1);
-
-	/* Allow the thread to be frozen */
-	set_freezable();
-
-	/*
-	 * Arrange for userspace references to be interpreted as kernel
-	 * pointers.  That way we can pass a kernel pointer to a routine
-	 * that expects a __user pointer and it will work okay.
-	 */
-	set_fs(get_ds());
-
-	/* The main loop */
-	while (common->state != FSG_STATE_TERMINATED) {
-		if (exception_in_progress(common) || signal_pending(current)) {
-			handle_exception(common);
-			continue;
-		}
-
-		if (!common->running) {
-			sleep_thread(common);
-			continue;
-		}
-
-		if (get_next_command(common))
-			continue;
-
-		spin_lock_irq(&common->lock);
-		if (!exception_in_progress(common))
-			common->state = FSG_STATE_DATA_PHASE;
-		spin_unlock_irq(&common->lock);
-
-		if (do_scsi_command(common) || finish_reply(common))
-			continue;
-
-		spin_lock_irq(&common->lock);
-		if (!exception_in_progress(common))
-			common->state = FSG_STATE_STATUS_PHASE;
-		spin_unlock_irq(&common->lock);
-
-		if (send_status(common))
-			continue;
-
-		spin_lock_irq(&common->lock);
-		if (!exception_in_progress(common))
-			common->state = FSG_STATE_IDLE;
-		spin_unlock_irq(&common->lock);
-	}
-
-	spin_lock_irq(&common->lock);
-	common->thread_task = NULL;
-	spin_unlock_irq(&common->lock);
-
-	if (!common->ops || !common->ops->thread_exits
-	 || common->ops->thread_exits(common) < 0) {
-		struct fsg_lun *curlun = common->luns;
-		unsigned i = common->nluns;
-
-		down_write(&common->filesem);
-		for (; i--; ++curlun) {
-			if (!fsg_lun_is_open(curlun))
-				continue;
-
-			fsg_lun_close(curlun);
-			curlun->unit_attention_data = SS_MEDIUM_NOT_PRESENT;
-		}
-		up_write(&common->filesem);
-	}
-
-	/* Let fsg_unbind() know the thread has exited */
-	complete_and_exit(&common->thread_notifier, 0);
-}
-
-
-/*************************** DEVICE ATTRIBUTES ***************************/
-
-static DEVICE_ATTR(ro, 0644, fsg_show_ro, fsg_store_ro);
-static DEVICE_ATTR(nofua, 0644, fsg_show_nofua, fsg_store_nofua);
-static DEVICE_ATTR(file, 0644, fsg_show_file, fsg_store_file);
-
-static struct device_attribute dev_attr_ro_cdrom =
-	__ATTR(ro, 0444, fsg_show_ro, NULL);
-static struct device_attribute dev_attr_file_nonremovable =
-	__ATTR(file, 0444, fsg_show_file, NULL);
-
-
-/****************************** FSG COMMON ******************************/
-
-static void fsg_common_release(struct kref *ref);
-
-static void fsg_lun_release(struct device *dev)
-{
-	/* Nothing needs to be done */
-}
-
-static inline void fsg_common_get(struct fsg_common *common)
-{
-	kref_get(&common->ref);
-}
-
-static inline void fsg_common_put(struct fsg_common *common)
-{
-	kref_put(&common->ref, fsg_common_release);
-}
-
-static struct fsg_common *fsg_common_init(struct fsg_common *common,
-					  struct usb_composite_dev *cdev,
-					  struct fsg_config *cfg)
-{
-	struct usb_gadget *gadget = cdev->gadget;
-	struct fsg_buffhd *bh;
-	struct fsg_lun *curlun;
-	struct fsg_lun_config *lcfg;
-	int nluns, i, rc;
-	char *pathbuf;
-
-	rc = fsg_num_buffers_validate();
-	if (rc != 0)
-		return ERR_PTR(rc);
-
-	/* Find out how many LUNs there should be */
-	nluns = cfg->nluns;
-	if (nluns < 1 || nluns > FSG_MAX_LUNS) {
-		dev_err(&gadget->dev, "invalid number of LUNs: %u\n", nluns);
-		return ERR_PTR(-EINVAL);
-	}
-
-	/* Allocate? */
-	if (!common) {
-		common = kzalloc(sizeof *common, GFP_KERNEL);
-		if (!common)
-			return ERR_PTR(-ENOMEM);
-		common->free_storage_on_release = 1;
-	} else {
-		memset(common, 0, sizeof *common);
-		common->free_storage_on_release = 0;
-	}
-
-	common->buffhds = kcalloc(fsg_num_buffers,
-				  sizeof *(common->buffhds), GFP_KERNEL);
-	if (!common->buffhds) {
-		if (common->free_storage_on_release)
-			kfree(common);
-		return ERR_PTR(-ENOMEM);
-	}
-
-	common->ops = cfg->ops;
-	common->private_data = cfg->private_data;
-
-	common->gadget = gadget;
-	common->ep0 = gadget->ep0;
-	common->ep0req = cdev->req;
-	common->cdev = cdev;
-
-	/* Maybe allocate device-global string IDs, and patch descriptors */
-	if (fsg_strings[FSG_STRING_INTERFACE].id == 0) {
-		rc = usb_string_id(cdev);
-		if (unlikely(rc < 0))
-			goto error_release;
-		fsg_strings[FSG_STRING_INTERFACE].id = rc;
-		fsg_intf_desc.iInterface = rc;
-	}
-
-	/*
-	 * Create the LUNs, open their backing files, and register the
-	 * LUN devices in sysfs.
-	 */
-	curlun = kcalloc(nluns, sizeof(*curlun), GFP_KERNEL);
-	if (unlikely(!curlun)) {
-		rc = -ENOMEM;
-		goto error_release;
-	}
-	common->luns = curlun;
-
-	init_rwsem(&common->filesem);
-
-	for (i = 0, lcfg = cfg->luns; i < nluns; ++i, ++curlun, ++lcfg) {
-		curlun->cdrom = !!lcfg->cdrom;
-		curlun->ro = lcfg->cdrom || lcfg->ro;
-		curlun->initially_ro = curlun->ro;
-		curlun->removable = lcfg->removable;
-		curlun->dev.release = fsg_lun_release;
-		curlun->dev.parent = &gadget->dev;
-		/* curlun->dev.driver = &fsg_driver.driver; XXX */
-		dev_set_drvdata(&curlun->dev, &common->filesem);
-		dev_set_name(&curlun->dev, "lun%d", i);
-
-		rc = device_register(&curlun->dev);
-		if (rc) {
-			INFO(common, "failed to register LUN%d: %d\n", i, rc);
-			common->nluns = i;
-			put_device(&curlun->dev);
-			goto error_release;
-		}
-
-		rc = device_create_file(&curlun->dev,
-					curlun->cdrom
-				      ? &dev_attr_ro_cdrom
-				      : &dev_attr_ro);
-		if (rc)
-			goto error_luns;
-		rc = device_create_file(&curlun->dev,
-					curlun->removable
-				      ? &dev_attr_file
-				      : &dev_attr_file_nonremovable);
-		if (rc)
-			goto error_luns;
-		rc = device_create_file(&curlun->dev, &dev_attr_nofua);
-		if (rc)
-			goto error_luns;
-
-		if (lcfg->filename) {
-			rc = fsg_lun_open(curlun, lcfg->filename);
-			if (rc)
-				goto error_luns;
-		} else if (!curlun->removable) {
-			ERROR(common, "no file given for LUN%d\n", i);
-			rc = -EINVAL;
-			goto error_luns;
-		}
-	}
-	common->nluns = nluns;
-
-	/* Data buffers cyclic list */
-	bh = common->buffhds;
-	i = fsg_num_buffers;
-	goto buffhds_first_it;
-	do {
-		bh->next = bh + 1;
-		++bh;
-buffhds_first_it:
-		bh->buf = kmalloc(FSG_BUFLEN, GFP_KERNEL);
-		if (unlikely(!bh->buf)) {
-			rc = -ENOMEM;
-			goto error_release;
-		}
-	} while (--i);
-	bh->next = common->buffhds;
-
-	/* Prepare inquiryString */
-	if (cfg->release != 0xffff) {
-		i = cfg->release;
-	} else {
-		i = usb_gadget_controller_number(gadget);
-		if (i >= 0) {
-			i = 0x0300 + i;
-		} else {
-			WARNING(common, "controller '%s' not recognized\n",
-				gadget->name);
-			i = 0x0399;
-		}
-	}
-	snprintf(common->inquiry_string, sizeof common->inquiry_string,
-		 "%-8s%-16s%04x", cfg->vendor_name ?: "Linux",
-		 /* Assume product name dependent on the first LUN */
-		 cfg->product_name ?: (common->luns->cdrom
-				     ? "File-Stor Gadget"
-				     : "File-CD Gadget"),
-		 i);
-
-	/*
-	 * Some peripheral controllers are known not to be able to
-	 * halt bulk endpoints correctly.  If one of them is present,
-	 * disable stalls.
-	 */
-	common->can_stall = cfg->can_stall &&
-		!(gadget_is_at91(common->gadget));
-
-	spin_lock_init(&common->lock);
-	kref_init(&common->ref);
-
-	/* Tell the thread to start working */
-	common->thread_task =
-		kthread_create(fsg_main_thread, common, "file-storage");
-	if (IS_ERR(common->thread_task)) {
-		rc = PTR_ERR(common->thread_task);
-		goto error_release;
-	}
-	init_completion(&common->thread_notifier);
-	init_waitqueue_head(&common->fsg_wait);
-
-	/* Information */
-	INFO(common, FSG_DRIVER_DESC ", version: " FSG_DRIVER_VERSION "\n");
-	INFO(common, "Number of LUNs=%d\n", common->nluns);
-
-	pathbuf = kmalloc(PATH_MAX, GFP_KERNEL);
-	for (i = 0, nluns = common->nluns, curlun = common->luns;
-	     i < nluns;
-	     ++curlun, ++i) {
-		char *p = "(no medium)";
-		if (fsg_lun_is_open(curlun)) {
-			p = "(error)";
-			if (pathbuf) {
-				p = d_path(&curlun->filp->f_path,
-					   pathbuf, PATH_MAX);
-				if (IS_ERR(p))
-					p = "(error)";
-			}
-		}
-		LINFO(curlun, "LUN: %s%s%sfile: %s\n",
-		      curlun->removable ? "removable " : "",
-		      curlun->ro ? "read only " : "",
-		      curlun->cdrom ? "CD-ROM " : "",
-		      p);
-	}
-	kfree(pathbuf);
-
-	DBG(common, "I/O thread pid: %d\n", task_pid_nr(common->thread_task));
-
-	wake_up_process(common->thread_task);
-
-	return common;
-
-error_luns:
-	common->nluns = i + 1;
-error_release:
-	common->state = FSG_STATE_TERMINATED;	/* The thread is dead */
-	/* Call fsg_common_release() directly, ref might be not initialised. */
-	fsg_common_release(&common->ref);
-	return ERR_PTR(rc);
-}
-
-static void fsg_common_release(struct kref *ref)
-{
-	struct fsg_common *common = container_of(ref, struct fsg_common, ref);
-
-	/* If the thread isn't already dead, tell it to exit now */
-	if (common->state != FSG_STATE_TERMINATED) {
-		raise_exception(common, FSG_STATE_EXIT);
-		wait_for_completion(&common->thread_notifier);
-	}
-
-	if (likely(common->luns)) {
-		struct fsg_lun *lun = common->luns;
-		unsigned i = common->nluns;
-
-		/* In error recovery common->nluns may be zero. */
-		for (; i; --i, ++lun) {
-			device_remove_file(&lun->dev, &dev_attr_nofua);
-			device_remove_file(&lun->dev,
-					   lun->cdrom
-					 ? &dev_attr_ro_cdrom
-					 : &dev_attr_ro);
-			device_remove_file(&lun->dev,
-					   lun->removable
-					 ? &dev_attr_file
-					 : &dev_attr_file_nonremovable);
-			fsg_lun_close(lun);
-			device_unregister(&lun->dev);
-		}
-
-		kfree(common->luns);
-	}
-
-	{
-		struct fsg_buffhd *bh = common->buffhds;
-		unsigned i = fsg_num_buffers;
-		do {
-			kfree(bh->buf);
-		} while (++bh, --i);
-	}
-
-	kfree(common->buffhds);
-	if (common->free_storage_on_release)
-		kfree(common);
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-static void fsg_unbind(struct usb_configuration *c, struct usb_function *f)
-{
-	struct fsg_dev		*fsg = fsg_from_func(f);
-	struct fsg_common	*common = fsg->common;
-
-	DBG(fsg, "unbind\n");
-	if (fsg->common->fsg == fsg) {
-		fsg->common->new_fsg = NULL;
-		raise_exception(fsg->common, FSG_STATE_CONFIG_CHANGE);
-		/* FIXME: make interruptible or killable somehow? */
-		wait_event(common->fsg_wait, common->fsg != fsg);
-	}
-
-	fsg_common_put(common);
-	usb_free_descriptors(fsg->function.descriptors);
-	usb_free_descriptors(fsg->function.hs_descriptors);
-	usb_free_descriptors(fsg->function.ss_descriptors);
-	kfree(fsg);
-}
-
-static int fsg_bind(struct usb_configuration *c, struct usb_function *f)
-{
-	struct fsg_dev		*fsg = fsg_from_func(f);
-	struct usb_gadget	*gadget = c->cdev->gadget;
-	int			i;
-	struct usb_ep		*ep;
-
-	fsg->gadget = gadget;
-
-	/* New interface */
-	i = usb_interface_id(c, f);
-	if (i < 0)
-		return i;
-	fsg_intf_desc.bInterfaceNumber = i;
-	fsg->interface_number = i;
-
-	/* Find all the endpoints we will use */
-	ep = usb_ep_autoconfig(gadget, &fsg_fs_bulk_in_desc);
-	if (!ep)
-		goto autoconf_fail;
-	ep->driver_data = fsg->common;	/* claim the endpoint */
-	fsg->bulk_in = ep;
-
-	ep = usb_ep_autoconfig(gadget, &fsg_fs_bulk_out_desc);
-	if (!ep)
-		goto autoconf_fail;
-	ep->driver_data = fsg->common;	/* claim the endpoint */
-	fsg->bulk_out = ep;
-
-	/* Copy descriptors */
-	f->descriptors = usb_copy_descriptors(fsg_fs_function);
-	if (unlikely(!f->descriptors))
-		return -ENOMEM;
-
-	if (gadget_is_dualspeed(gadget)) {
-		/* Assume endpoint addresses are the same for both speeds */
-		fsg_hs_bulk_in_desc.bEndpointAddress =
-			fsg_fs_bulk_in_desc.bEndpointAddress;
-		fsg_hs_bulk_out_desc.bEndpointAddress =
-			fsg_fs_bulk_out_desc.bEndpointAddress;
-		f->hs_descriptors = usb_copy_descriptors(fsg_hs_function);
-		if (unlikely(!f->hs_descriptors)) {
-			usb_free_descriptors(f->descriptors);
-			return -ENOMEM;
-		}
-	}
-
-	if (gadget_is_superspeed(gadget)) {
-		unsigned	max_burst;
-
-		/* Calculate bMaxBurst, we know packet size is 1024 */
-		max_burst = min_t(unsigned, FSG_BUFLEN / 1024, 15);
-
-		fsg_ss_bulk_in_desc.bEndpointAddress =
-			fsg_fs_bulk_in_desc.bEndpointAddress;
-		fsg_ss_bulk_in_comp_desc.bMaxBurst = max_burst;
-
-		fsg_ss_bulk_out_desc.bEndpointAddress =
-			fsg_fs_bulk_out_desc.bEndpointAddress;
-		fsg_ss_bulk_out_comp_desc.bMaxBurst = max_burst;
-
-		f->ss_descriptors = usb_copy_descriptors(fsg_ss_function);
-		if (unlikely(!f->ss_descriptors)) {
-			usb_free_descriptors(f->hs_descriptors);
-			usb_free_descriptors(f->descriptors);
-			return -ENOMEM;
-		}
-	}
-
-	return 0;
-
-autoconf_fail:
-	ERROR(fsg, "unable to autoconfigure all endpoints\n");
-	return -ENOTSUPP;
-}
-
-
-/****************************** ADD FUNCTION ******************************/
-
-static struct usb_gadget_strings *fsg_strings_array[] = {
-	&fsg_stringtab,
-	NULL,
-};
-
-static int fsg_bind_config(struct usb_composite_dev *cdev,
-			   struct usb_configuration *c,
-			   struct fsg_common *common)
-{
-	struct fsg_dev *fsg;
-	int rc;
-
-	fsg = kzalloc(sizeof *fsg, GFP_KERNEL);
-	if (unlikely(!fsg))
-		return -ENOMEM;
-
-	fsg->function.name        = FSG_DRIVER_DESC;
-	fsg->function.strings     = fsg_strings_array;
-	fsg->function.bind        = fsg_bind;
-	fsg->function.unbind      = fsg_unbind;
-	fsg->function.setup       = fsg_setup;
-	fsg->function.set_alt     = fsg_set_alt;
-	fsg->function.disable     = fsg_disable;
-
-	fsg->common               = common;
-	/*
-	 * Our caller holds a reference to common structure so we
-	 * don't have to be worry about it being freed until we return
-	 * from this function.  So instead of incrementing counter now
-	 * and decrement in error recovery we increment it only when
-	 * call to usb_add_function() was successful.
-	 */
-
-	rc = usb_add_function(c, &fsg->function);
-	if (unlikely(rc))
-		kfree(fsg);
-	else
-		fsg_common_get(fsg->common);
-	return rc;
-}
-
-
-/************************* Module parameters *************************/
-
-struct fsg_module_parameters {
-	char		*file[FSG_MAX_LUNS];
-	bool		ro[FSG_MAX_LUNS];
-	bool		removable[FSG_MAX_LUNS];
-	bool		cdrom[FSG_MAX_LUNS];
-	bool		nofua[FSG_MAX_LUNS];
-
-	unsigned int	file_count, ro_count, removable_count, cdrom_count;
-	unsigned int	nofua_count;
-	unsigned int	luns;	/* nluns */
-	bool		stall;	/* can_stall */
-};
-
-#define _FSG_MODULE_PARAM_ARRAY(prefix, params, name, type, desc)	\
-	module_param_array_named(prefix ## name, params.name, type,	\
-				 &prefix ## params.name ## _count,	\
-				 S_IRUGO);				\
-	MODULE_PARM_DESC(prefix ## name, desc)
-
-#define _FSG_MODULE_PARAM(prefix, params, name, type, desc)		\
-	module_param_named(prefix ## name, params.name, type,		\
-			   S_IRUGO);					\
-	MODULE_PARM_DESC(prefix ## name, desc)
-
-#define FSG_MODULE_PARAMETERS(prefix, params)				\
-	_FSG_MODULE_PARAM_ARRAY(prefix, params, file, charp,		\
-				"names of backing files or devices");	\
-	_FSG_MODULE_PARAM_ARRAY(prefix, params, ro, bool,		\
-				"true to force read-only");		\
-	_FSG_MODULE_PARAM_ARRAY(prefix, params, removable, bool,	\
-				"true to simulate removable media");	\
-	_FSG_MODULE_PARAM_ARRAY(prefix, params, cdrom, bool,		\
-				"true to simulate CD-ROM instead of disk"); \
-	_FSG_MODULE_PARAM_ARRAY(prefix, params, nofua, bool,		\
-				"true to ignore SCSI WRITE(10,12) FUA bit"); \
-	_FSG_MODULE_PARAM(prefix, params, luns, uint,			\
-			  "number of LUNs");				\
-	_FSG_MODULE_PARAM(prefix, params, stall, bool,			\
-			  "false to prevent bulk stalls")
-
-static void
-fsg_config_from_params(struct fsg_config *cfg,
-		       const struct fsg_module_parameters *params)
-{
-	struct fsg_lun_config *lun;
-	unsigned i;
-
-	/* Configure LUNs */
-	cfg->nluns =
-		min(params->luns ?: (params->file_count ?: 1u),
-		    (unsigned)FSG_MAX_LUNS);
-	for (i = 0, lun = cfg->luns; i < cfg->nluns; ++i, ++lun) {
-		lun->ro = !!params->ro[i];
-		lun->cdrom = !!params->cdrom[i];
-		lun->removable = !!params->removable[i];
-		lun->filename =
-			params->file_count > i && params->file[i][0]
-			? params->file[i]
-			: 0;
-	}
-
-	/* Let MSF use defaults */
-	cfg->vendor_name = 0;
-	cfg->product_name = 0;
-	cfg->release = 0xffff;
-
-	cfg->ops = NULL;
-	cfg->private_data = NULL;
-
-	/* Finalise */
-	cfg->can_stall = params->stall;
-}
-
-static inline struct fsg_common *
-fsg_common_from_params(struct fsg_common *common,
-		       struct usb_composite_dev *cdev,
-		       const struct fsg_module_parameters *params)
-	__attribute__((unused));
-static inline struct fsg_common *
-fsg_common_from_params(struct fsg_common *common,
-		       struct usb_composite_dev *cdev,
-		       const struct fsg_module_parameters *params)
-{
-	struct fsg_config cfg;
-	fsg_config_from_params(&cfg, params);
-	return fsg_common_init(common, cdev, &cfg);
-}
diff --git a/drivers/staging/ccg/f_rndis.c b/drivers/staging/ccg/f_rndis.c
deleted file mode 100644
index b1681e45aca..00000000000
--- a/drivers/staging/ccg/f_rndis.c
+++ /dev/null
@@ -1,918 +0,0 @@
-/*
- * f_rndis.c -- RNDIS link function driver
- *
- * Copyright (C) 2003-2005,2008 David Brownell
- * Copyright (C) 2003-2004 Robert Schwebel, Benedikt Spranger
- * Copyright (C) 2008 Nokia Corporation
- * Copyright (C) 2009 Samsung Electronics
- *                    Author: Michal Nazarewicz (mina86@mina86.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.
- */
-
-/* #define VERBOSE_DEBUG */
-
-#include <linux/slab.h>
-#include <linux/kernel.h>
-#include <linux/device.h>
-#include <linux/etherdevice.h>
-
-#include <linux/atomic.h>
-
-#include "u_ether.h"
-#include "rndis.h"
-
-
-/*
- * This function is an RNDIS Ethernet port -- a Microsoft protocol that's
- * been promoted instead of the standard CDC Ethernet.  The published RNDIS
- * spec is ambiguous, incomplete, and needlessly complex.  Variants such as
- * ActiveSync have even worse status in terms of specification.
- *
- * In short:  it's a protocol controlled by (and for) Microsoft, not for an
- * Open ecosystem or markets.  Linux supports it *only* because Microsoft
- * doesn't support the CDC Ethernet standard.
- *
- * The RNDIS data transfer model is complex, with multiple Ethernet packets
- * per USB message, and out of band data.  The control model is built around
- * what's essentially an "RNDIS RPC" protocol.  It's all wrapped in a CDC ACM
- * (modem, not Ethernet) veneer, with those ACM descriptors being entirely
- * useless (they're ignored).  RNDIS expects to be the only function in its
- * configuration, so it's no real help if you need composite devices; and
- * it expects to be the first configuration too.
- *
- * There is a single technical advantage of RNDIS over CDC Ethernet, if you
- * discount the fluff that its RPC can be made to deliver: it doesn't need
- * a NOP altsetting for the data interface.  That lets it work on some of the
- * "so smart it's stupid" hardware which takes over configuration changes
- * from the software, and adds restrictions like "no altsettings".
- *
- * Unfortunately MSFT's RNDIS drivers are buggy.  They hang or oops, and
- * have all sorts of contrary-to-specification oddities that can prevent
- * them from working sanely.  Since bugfixes (or accurate specs, letting
- * Linux work around those bugs) are unlikely to ever come from MSFT, you
- * may want to avoid using RNDIS on purely operational grounds.
- *
- * Omissions from the RNDIS 1.0 specification include:
- *
- *   - Power management ... references data that's scattered around lots
- *     of other documentation, which is incorrect/incomplete there too.
- *
- *   - There are various undocumented protocol requirements, like the need
- *     to send garbage in some control-OUT messages.
- *
- *   - MS-Windows drivers sometimes emit undocumented requests.
- */
-
-struct f_rndis {
-	struct gether			port;
-	u8				ctrl_id, data_id;
-	u8				ethaddr[ETH_ALEN];
-	u32				vendorID;
-	const char			*manufacturer;
-	int				config;
-
-	struct usb_ep			*notify;
-	struct usb_request		*notify_req;
-	atomic_t			notify_count;
-};
-
-static inline struct f_rndis *func_to_rndis(struct usb_function *f)
-{
-	return container_of(f, struct f_rndis, port.func);
-}
-
-/* peak (theoretical) bulk transfer rate in bits-per-second */
-static unsigned int bitrate(struct usb_gadget *g)
-{
-	if (gadget_is_superspeed(g) && g->speed == USB_SPEED_SUPER)
-		return 13 * 1024 * 8 * 1000 * 8;
-	else if (gadget_is_dualspeed(g) && g->speed == USB_SPEED_HIGH)
-		return 13 * 512 * 8 * 1000 * 8;
-	else
-		return 19 * 64 * 1 * 1000 * 8;
-}
-
-/*-------------------------------------------------------------------------*/
-
-/*
- */
-
-#define LOG2_STATUS_INTERVAL_MSEC	5	/* 1 << 5 == 32 msec */
-#define STATUS_BYTECOUNT		8	/* 8 bytes data */
-
-
-/* interface descriptor: */
-
-static struct usb_interface_descriptor rndis_control_intf = {
-	.bLength =		sizeof rndis_control_intf,
-	.bDescriptorType =	USB_DT_INTERFACE,
-
-	/* .bInterfaceNumber = DYNAMIC */
-	/* status endpoint is optional; this could be patched later */
-	.bNumEndpoints =	1,
-	.bInterfaceClass =	USB_CLASS_COMM,
-	.bInterfaceSubClass =   USB_CDC_SUBCLASS_ACM,
-	.bInterfaceProtocol =   USB_CDC_ACM_PROTO_VENDOR,
-	/* .iInterface = DYNAMIC */
-};
-
-static struct usb_cdc_header_desc header_desc = {
-	.bLength =		sizeof header_desc,
-	.bDescriptorType =	USB_DT_CS_INTERFACE,
-	.bDescriptorSubType =	USB_CDC_HEADER_TYPE,
-
-	.bcdCDC =		cpu_to_le16(0x0110),
-};
-
-static struct usb_cdc_call_mgmt_descriptor call_mgmt_descriptor = {
-	.bLength =		sizeof call_mgmt_descriptor,
-	.bDescriptorType =	USB_DT_CS_INTERFACE,
-	.bDescriptorSubType =	USB_CDC_CALL_MANAGEMENT_TYPE,
-
-	.bmCapabilities =	0x00,
-	.bDataInterface =	0x01,
-};
-
-static struct usb_cdc_acm_descriptor rndis_acm_descriptor = {
-	.bLength =		sizeof rndis_acm_descriptor,
-	.bDescriptorType =	USB_DT_CS_INTERFACE,
-	.bDescriptorSubType =	USB_CDC_ACM_TYPE,
-
-	.bmCapabilities =	0x00,
-};
-
-static struct usb_cdc_union_desc rndis_union_desc = {
-	.bLength =		sizeof(rndis_union_desc),
-	.bDescriptorType =	USB_DT_CS_INTERFACE,
-	.bDescriptorSubType =	USB_CDC_UNION_TYPE,
-	/* .bMasterInterface0 =	DYNAMIC */
-	/* .bSlaveInterface0 =	DYNAMIC */
-};
-
-/* the data interface has two bulk endpoints */
-
-static struct usb_interface_descriptor rndis_data_intf = {
-	.bLength =		sizeof rndis_data_intf,
-	.bDescriptorType =	USB_DT_INTERFACE,
-
-	/* .bInterfaceNumber = DYNAMIC */
-	.bNumEndpoints =	2,
-	.bInterfaceClass =	USB_CLASS_CDC_DATA,
-	.bInterfaceSubClass =	0,
-	.bInterfaceProtocol =	0,
-	/* .iInterface = DYNAMIC */
-};
-
-
-static struct usb_interface_assoc_descriptor
-rndis_iad_descriptor = {
-	.bLength =		sizeof rndis_iad_descriptor,
-	.bDescriptorType =	USB_DT_INTERFACE_ASSOCIATION,
-
-	.bFirstInterface =	0, /* XXX, hardcoded */
-	.bInterfaceCount = 	2,	// control + data
-	.bFunctionClass =	USB_CLASS_COMM,
-	.bFunctionSubClass =	USB_CDC_SUBCLASS_ETHERNET,
-	.bFunctionProtocol =	USB_CDC_PROTO_NONE,
-	/* .iFunction = DYNAMIC */
-};
-
-/* full speed support: */
-
-static struct usb_endpoint_descriptor fs_notify_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-
-	.bEndpointAddress =	USB_DIR_IN,
-	.bmAttributes =		USB_ENDPOINT_XFER_INT,
-	.wMaxPacketSize =	cpu_to_le16(STATUS_BYTECOUNT),
-	.bInterval =		1 << LOG2_STATUS_INTERVAL_MSEC,
-};
-
-static struct usb_endpoint_descriptor fs_in_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-
-	.bEndpointAddress =	USB_DIR_IN,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-};
-
-static struct usb_endpoint_descriptor fs_out_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-
-	.bEndpointAddress =	USB_DIR_OUT,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-};
-
-static struct usb_descriptor_header *eth_fs_function[] = {
-	(struct usb_descriptor_header *) &rndis_iad_descriptor,
-
-	/* control interface matches ACM, not Ethernet */
-	(struct usb_descriptor_header *) &rndis_control_intf,
-	(struct usb_descriptor_header *) &header_desc,
-	(struct usb_descriptor_header *) &call_mgmt_descriptor,
-	(struct usb_descriptor_header *) &rndis_acm_descriptor,
-	(struct usb_descriptor_header *) &rndis_union_desc,
-	(struct usb_descriptor_header *) &fs_notify_desc,
-
-	/* data interface has no altsetting */
-	(struct usb_descriptor_header *) &rndis_data_intf,
-	(struct usb_descriptor_header *) &fs_in_desc,
-	(struct usb_descriptor_header *) &fs_out_desc,
-	NULL,
-};
-
-/* high speed support: */
-
-static struct usb_endpoint_descriptor hs_notify_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-
-	.bEndpointAddress =	USB_DIR_IN,
-	.bmAttributes =		USB_ENDPOINT_XFER_INT,
-	.wMaxPacketSize =	cpu_to_le16(STATUS_BYTECOUNT),
-	.bInterval =		LOG2_STATUS_INTERVAL_MSEC + 4,
-};
-
-static struct usb_endpoint_descriptor hs_in_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-
-	.bEndpointAddress =	USB_DIR_IN,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-	.wMaxPacketSize =	cpu_to_le16(512),
-};
-
-static struct usb_endpoint_descriptor hs_out_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-
-	.bEndpointAddress =	USB_DIR_OUT,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-	.wMaxPacketSize =	cpu_to_le16(512),
-};
-
-static struct usb_descriptor_header *eth_hs_function[] = {
-	(struct usb_descriptor_header *) &rndis_iad_descriptor,
-
-	/* control interface matches ACM, not Ethernet */
-	(struct usb_descriptor_header *) &rndis_control_intf,
-	(struct usb_descriptor_header *) &header_desc,
-	(struct usb_descriptor_header *) &call_mgmt_descriptor,
-	(struct usb_descriptor_header *) &rndis_acm_descriptor,
-	(struct usb_descriptor_header *) &rndis_union_desc,
-	(struct usb_descriptor_header *) &hs_notify_desc,
-
-	/* data interface has no altsetting */
-	(struct usb_descriptor_header *) &rndis_data_intf,
-	(struct usb_descriptor_header *) &hs_in_desc,
-	(struct usb_descriptor_header *) &hs_out_desc,
-	NULL,
-};
-
-/* super speed support: */
-
-static struct usb_endpoint_descriptor ss_notify_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-
-	.bEndpointAddress =	USB_DIR_IN,
-	.bmAttributes =		USB_ENDPOINT_XFER_INT,
-	.wMaxPacketSize =	cpu_to_le16(STATUS_BYTECOUNT),
-	.bInterval =		LOG2_STATUS_INTERVAL_MSEC + 4,
-};
-
-static struct usb_ss_ep_comp_descriptor ss_intr_comp_desc = {
-	.bLength =		sizeof ss_intr_comp_desc,
-	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
-
-	/* the following 3 values can be tweaked if necessary */
-	/* .bMaxBurst =		0, */
-	/* .bmAttributes =	0, */
-	.wBytesPerInterval =	cpu_to_le16(STATUS_BYTECOUNT),
-};
-
-static struct usb_endpoint_descriptor ss_in_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-
-	.bEndpointAddress =	USB_DIR_IN,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-	.wMaxPacketSize =	cpu_to_le16(1024),
-};
-
-static struct usb_endpoint_descriptor ss_out_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-
-	.bEndpointAddress =	USB_DIR_OUT,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-	.wMaxPacketSize =	cpu_to_le16(1024),
-};
-
-static struct usb_ss_ep_comp_descriptor ss_bulk_comp_desc = {
-	.bLength =		sizeof ss_bulk_comp_desc,
-	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
-
-	/* the following 2 values can be tweaked if necessary */
-	/* .bMaxBurst =		0, */
-	/* .bmAttributes =	0, */
-};
-
-static struct usb_descriptor_header *eth_ss_function[] = {
-	(struct usb_descriptor_header *) &rndis_iad_descriptor,
-
-	/* control interface matches ACM, not Ethernet */
-	(struct usb_descriptor_header *) &rndis_control_intf,
-	(struct usb_descriptor_header *) &header_desc,
-	(struct usb_descriptor_header *) &call_mgmt_descriptor,
-	(struct usb_descriptor_header *) &rndis_acm_descriptor,
-	(struct usb_descriptor_header *) &rndis_union_desc,
-	(struct usb_descriptor_header *) &ss_notify_desc,
-	(struct usb_descriptor_header *) &ss_intr_comp_desc,
-
-	/* data interface has no altsetting */
-	(struct usb_descriptor_header *) &rndis_data_intf,
-	(struct usb_descriptor_header *) &ss_in_desc,
-	(struct usb_descriptor_header *) &ss_bulk_comp_desc,
-	(struct usb_descriptor_header *) &ss_out_desc,
-	(struct usb_descriptor_header *) &ss_bulk_comp_desc,
-	NULL,
-};
-
-/* string descriptors: */
-
-static struct usb_string rndis_string_defs[] = {
-	[0].s = "RNDIS Communications Control",
-	[1].s = "RNDIS Ethernet Data",
-	[2].s = "RNDIS",
-	{  } /* end of list */
-};
-
-static struct usb_gadget_strings rndis_string_table = {
-	.language =		0x0409,	/* en-us */
-	.strings =		rndis_string_defs,
-};
-
-static struct usb_gadget_strings *rndis_strings[] = {
-	&rndis_string_table,
-	NULL,
-};
-
-/*-------------------------------------------------------------------------*/
-
-static struct sk_buff *rndis_add_header(struct gether *port,
-					struct sk_buff *skb)
-{
-	struct sk_buff *skb2;
-
-	skb2 = skb_realloc_headroom(skb, sizeof(struct rndis_packet_msg_type));
-	if (skb2)
-		rndis_add_hdr(skb2);
-
-	dev_kfree_skb_any(skb);
-	return skb2;
-}
-
-static void rndis_response_available(void *_rndis)
-{
-	struct f_rndis			*rndis = _rndis;
-	struct usb_request		*req = rndis->notify_req;
-	struct usb_composite_dev	*cdev = rndis->port.func.config->cdev;
-	__le32				*data = req->buf;
-	int				status;
-
-	if (atomic_inc_return(&rndis->notify_count) != 1)
-		return;
-
-	/* Send RNDIS RESPONSE_AVAILABLE notification; a
-	 * USB_CDC_NOTIFY_RESPONSE_AVAILABLE "should" work too
-	 *
-	 * This is the only notification defined by RNDIS.
-	 */
-	data[0] = cpu_to_le32(1);
-	data[1] = cpu_to_le32(0);
-
-	status = usb_ep_queue(rndis->notify, req, GFP_ATOMIC);
-	if (status) {
-		atomic_dec(&rndis->notify_count);
-		DBG(cdev, "notify/0 --> %d\n", status);
-	}
-}
-
-static void rndis_response_complete(struct usb_ep *ep, struct usb_request *req)
-{
-	struct f_rndis			*rndis = req->context;
-	struct usb_composite_dev	*cdev = rndis->port.func.config->cdev;
-	int				status = req->status;
-
-	/* after TX:
-	 *  - USB_CDC_GET_ENCAPSULATED_RESPONSE (ep0/control)
-	 *  - RNDIS_RESPONSE_AVAILABLE (status/irq)
-	 */
-	switch (status) {
-	case -ECONNRESET:
-	case -ESHUTDOWN:
-		/* connection gone */
-		atomic_set(&rndis->notify_count, 0);
-		break;
-	default:
-		DBG(cdev, "RNDIS %s response error %d, %d/%d\n",
-			ep->name, status,
-			req->actual, req->length);
-		/* FALLTHROUGH */
-	case 0:
-		if (ep != rndis->notify)
-			break;
-
-		/* handle multiple pending RNDIS_RESPONSE_AVAILABLE
-		 * notifications by resending until we're done
-		 */
-		if (atomic_dec_and_test(&rndis->notify_count))
-			break;
-		status = usb_ep_queue(rndis->notify, req, GFP_ATOMIC);
-		if (status) {
-			atomic_dec(&rndis->notify_count);
-			DBG(cdev, "notify/1 --> %d\n", status);
-		}
-		break;
-	}
-}
-
-static void rndis_command_complete(struct usb_ep *ep, struct usb_request *req)
-{
-	struct f_rndis			*rndis = req->context;
-	struct usb_composite_dev	*cdev = rndis->port.func.config->cdev;
-	int				status;
-
-	/* received RNDIS command from USB_CDC_SEND_ENCAPSULATED_COMMAND */
-//	spin_lock(&dev->lock);
-	status = rndis_msg_parser(rndis->config, (u8 *) req->buf);
-	if (status < 0)
-		ERROR(cdev, "RNDIS command error %d, %d/%d\n",
-			status, req->actual, req->length);
-//	spin_unlock(&dev->lock);
-}
-
-static int
-rndis_setup(struct usb_function *f, const struct usb_ctrlrequest *ctrl)
-{
-	struct f_rndis		*rndis = func_to_rndis(f);
-	struct usb_composite_dev *cdev = f->config->cdev;
-	struct usb_request	*req = cdev->req;
-	int			value = -EOPNOTSUPP;
-	u16			w_index = le16_to_cpu(ctrl->wIndex);
-	u16			w_value = le16_to_cpu(ctrl->wValue);
-	u16			w_length = le16_to_cpu(ctrl->wLength);
-
-	/* composite driver infrastructure handles everything except
-	 * CDC class messages; interface activation uses set_alt().
-	 */
-	switch ((ctrl->bRequestType << 8) | ctrl->bRequest) {
-
-	/* RNDIS uses the CDC command encapsulation mechanism to implement
-	 * an RPC scheme, with much getting/setting of attributes by OID.
-	 */
-	case ((USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
-			| USB_CDC_SEND_ENCAPSULATED_COMMAND:
-		if (w_value || w_index != rndis->ctrl_id)
-			goto invalid;
-		/* read the request; process it later */
-		value = w_length;
-		req->complete = rndis_command_complete;
-		req->context = rndis;
-		/* later, rndis_response_available() sends a notification */
-		break;
-
-	case ((USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
-			| USB_CDC_GET_ENCAPSULATED_RESPONSE:
-		if (w_value || w_index != rndis->ctrl_id)
-			goto invalid;
-		else {
-			u8 *buf;
-			u32 n;
-
-			/* return the result */
-			buf = rndis_get_next_response(rndis->config, &n);
-			if (buf) {
-				memcpy(req->buf, buf, n);
-				req->complete = rndis_response_complete;
-				req->context = rndis;
-				rndis_free_response(rndis->config, buf);
-				value = n;
-			}
-			/* else stalls ... spec says to avoid that */
-		}
-		break;
-
-	default:
-invalid:
-		VDBG(cdev, "invalid control req%02x.%02x v%04x i%04x l%d\n",
-			ctrl->bRequestType, ctrl->bRequest,
-			w_value, w_index, w_length);
-	}
-
-	/* respond with data transfer or status phase? */
-	if (value >= 0) {
-		DBG(cdev, "rndis req%02x.%02x v%04x i%04x l%d\n",
-			ctrl->bRequestType, ctrl->bRequest,
-			w_value, w_index, w_length);
-		req->zero = (value < w_length);
-		req->length = value;
-		value = usb_ep_queue(cdev->gadget->ep0, req, GFP_ATOMIC);
-		if (value < 0)
-			ERROR(cdev, "rndis response on err %d\n", value);
-	}
-
-	/* device either stalls (value < 0) or reports success */
-	return value;
-}
-
-
-static int rndis_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
-{
-	struct f_rndis		*rndis = func_to_rndis(f);
-	struct usb_composite_dev *cdev = f->config->cdev;
-
-	/* we know alt == 0 */
-
-	if (intf == rndis->ctrl_id) {
-		if (rndis->notify->driver_data) {
-			VDBG(cdev, "reset rndis control %d\n", intf);
-			usb_ep_disable(rndis->notify);
-		}
-		if (!rndis->notify->desc) {
-			VDBG(cdev, "init rndis ctrl %d\n", intf);
-			if (config_ep_by_speed(cdev->gadget, f, rndis->notify))
-				goto fail;
-		}
-		usb_ep_enable(rndis->notify);
-		rndis->notify->driver_data = rndis;
-
-	} else if (intf == rndis->data_id) {
-		struct net_device	*net;
-
-		if (rndis->port.in_ep->driver_data) {
-			DBG(cdev, "reset rndis\n");
-			gether_disconnect(&rndis->port);
-		}
-
-		if (!rndis->port.in_ep->desc || !rndis->port.out_ep->desc) {
-			DBG(cdev, "init rndis\n");
-			if (config_ep_by_speed(cdev->gadget, f,
-					       rndis->port.in_ep) ||
-			    config_ep_by_speed(cdev->gadget, f,
-					       rndis->port.out_ep)) {
-				rndis->port.in_ep->desc = NULL;
-				rndis->port.out_ep->desc = NULL;
-				goto fail;
-			}
-		}
-
-		/* Avoid ZLPs; they can be troublesome. */
-		rndis->port.is_zlp_ok = false;
-
-		/* RNDIS should be in the "RNDIS uninitialized" state,
-		 * either never activated or after rndis_uninit().
-		 *
-		 * We don't want data to flow here until a nonzero packet
-		 * filter is set, at which point it enters "RNDIS data
-		 * initialized" state ... but we do want the endpoints
-		 * to be activated.  It's a strange little state.
-		 *
-		 * REVISIT the RNDIS gadget code has done this wrong for a
-		 * very long time.  We need another call to the link layer
-		 * code -- gether_updown(...bool) maybe -- to do it right.
-		 */
-		rndis->port.cdc_filter = 0;
-
-		DBG(cdev, "RNDIS RX/TX early activation ... \n");
-		net = gether_connect(&rndis->port);
-		if (IS_ERR(net))
-			return PTR_ERR(net);
-
-		rndis_set_param_dev(rndis->config, net,
-				&rndis->port.cdc_filter);
-	} else
-		goto fail;
-
-	return 0;
-fail:
-	return -EINVAL;
-}
-
-static void rndis_disable(struct usb_function *f)
-{
-	struct f_rndis		*rndis = func_to_rndis(f);
-	struct usb_composite_dev *cdev = f->config->cdev;
-
-	if (!rndis->notify->driver_data)
-		return;
-
-	DBG(cdev, "rndis deactivated\n");
-
-	rndis_uninit(rndis->config);
-	gether_disconnect(&rndis->port);
-
-	usb_ep_disable(rndis->notify);
-	rndis->notify->driver_data = NULL;
-}
-
-/*-------------------------------------------------------------------------*/
-
-/*
- * This isn't quite the same mechanism as CDC Ethernet, since the
- * notification scheme passes less data, but the same set of link
- * states must be tested.  A key difference is that altsettings are
- * not used to tell whether the link should send packets or not.
- */
-
-static void rndis_open(struct gether *geth)
-{
-	struct f_rndis		*rndis = func_to_rndis(&geth->func);
-	struct usb_composite_dev *cdev = geth->func.config->cdev;
-
-	DBG(cdev, "%s\n", __func__);
-
-	rndis_set_param_medium(rndis->config, RNDIS_MEDIUM_802_3,
-				bitrate(cdev->gadget) / 100);
-	rndis_signal_connect(rndis->config);
-}
-
-static void rndis_close(struct gether *geth)
-{
-	struct f_rndis		*rndis = func_to_rndis(&geth->func);
-
-	DBG(geth->func.config->cdev, "%s\n", __func__);
-
-	rndis_set_param_medium(rndis->config, RNDIS_MEDIUM_802_3, 0);
-	rndis_signal_disconnect(rndis->config);
-}
-
-/*-------------------------------------------------------------------------*/
-
-/* ethernet function driver setup/binding */
-
-static int
-rndis_bind(struct usb_configuration *c, struct usb_function *f)
-{
-	struct usb_composite_dev *cdev = c->cdev;
-	struct f_rndis		*rndis = func_to_rndis(f);
-	int			status;
-	struct usb_ep		*ep;
-
-	/* allocate instance-specific interface IDs */
-	status = usb_interface_id(c, f);
-	if (status < 0)
-		goto fail;
-	rndis->ctrl_id = status;
-	rndis_iad_descriptor.bFirstInterface = status;
-
-	rndis_control_intf.bInterfaceNumber = status;
-	rndis_union_desc.bMasterInterface0 = status;
-
-	status = usb_interface_id(c, f);
-	if (status < 0)
-		goto fail;
-	rndis->data_id = status;
-
-	rndis_data_intf.bInterfaceNumber = status;
-	rndis_union_desc.bSlaveInterface0 = status;
-
-	status = -ENODEV;
-
-	/* allocate instance-specific endpoints */
-	ep = usb_ep_autoconfig(cdev->gadget, &fs_in_desc);
-	if (!ep)
-		goto fail;
-	rndis->port.in_ep = ep;
-	ep->driver_data = cdev;	/* claim */
-
-	ep = usb_ep_autoconfig(cdev->gadget, &fs_out_desc);
-	if (!ep)
-		goto fail;
-	rndis->port.out_ep = ep;
-	ep->driver_data = cdev;	/* claim */
-
-	/* NOTE:  a status/notification endpoint is, strictly speaking,
-	 * optional.  We don't treat it that way though!  It's simpler,
-	 * and some newer profiles don't treat it as optional.
-	 */
-	ep = usb_ep_autoconfig(cdev->gadget, &fs_notify_desc);
-	if (!ep)
-		goto fail;
-	rndis->notify = ep;
-	ep->driver_data = cdev;	/* claim */
-
-	status = -ENOMEM;
-
-	/* allocate notification request and buffer */
-	rndis->notify_req = usb_ep_alloc_request(ep, GFP_KERNEL);
-	if (!rndis->notify_req)
-		goto fail;
-	rndis->notify_req->buf = kmalloc(STATUS_BYTECOUNT, GFP_KERNEL);
-	if (!rndis->notify_req->buf)
-		goto fail;
-	rndis->notify_req->length = STATUS_BYTECOUNT;
-	rndis->notify_req->context = rndis;
-	rndis->notify_req->complete = rndis_response_complete;
-
-	/* copy descriptors, and track endpoint copies */
-	f->descriptors = usb_copy_descriptors(eth_fs_function);
-	if (!f->descriptors)
-		goto fail;
-
-	/* support all relevant hardware speeds... we expect that when
-	 * hardware is dual speed, all bulk-capable endpoints work at
-	 * both speeds
-	 */
-	if (gadget_is_dualspeed(c->cdev->gadget)) {
-		hs_in_desc.bEndpointAddress =
-				fs_in_desc.bEndpointAddress;
-		hs_out_desc.bEndpointAddress =
-				fs_out_desc.bEndpointAddress;
-		hs_notify_desc.bEndpointAddress =
-				fs_notify_desc.bEndpointAddress;
-
-		/* copy descriptors, and track endpoint copies */
-		f->hs_descriptors = usb_copy_descriptors(eth_hs_function);
-		if (!f->hs_descriptors)
-			goto fail;
-	}
-
-	if (gadget_is_superspeed(c->cdev->gadget)) {
-		ss_in_desc.bEndpointAddress =
-				fs_in_desc.bEndpointAddress;
-		ss_out_desc.bEndpointAddress =
-				fs_out_desc.bEndpointAddress;
-		ss_notify_desc.bEndpointAddress =
-				fs_notify_desc.bEndpointAddress;
-
-		/* copy descriptors, and track endpoint copies */
-		f->ss_descriptors = usb_copy_descriptors(eth_ss_function);
-		if (!f->ss_descriptors)
-			goto fail;
-	}
-
-	rndis->port.open = rndis_open;
-	rndis->port.close = rndis_close;
-
-	status = rndis_register(rndis_response_available, rndis);
-	if (status < 0)
-		goto fail;
-	rndis->config = status;
-
-	rndis_set_param_medium(rndis->config, RNDIS_MEDIUM_802_3, 0);
-	rndis_set_host_mac(rndis->config, rndis->ethaddr);
-
-	if (rndis->manufacturer && rndis->vendorID &&
-			rndis_set_param_vendor(rndis->config, rndis->vendorID,
-					       rndis->manufacturer))
-		goto fail;
-
-	/* NOTE:  all that is done without knowing or caring about
-	 * the network link ... which is unavailable to this code
-	 * until we're activated via set_alt().
-	 */
-
-	DBG(cdev, "RNDIS: %s speed IN/%s OUT/%s NOTIFY/%s\n",
-			gadget_is_superspeed(c->cdev->gadget) ? "super" :
-			gadget_is_dualspeed(c->cdev->gadget) ? "dual" : "full",
-			rndis->port.in_ep->name, rndis->port.out_ep->name,
-			rndis->notify->name);
-	return 0;
-
-fail:
-	if (gadget_is_superspeed(c->cdev->gadget) && f->ss_descriptors)
-		usb_free_descriptors(f->ss_descriptors);
-	if (gadget_is_dualspeed(c->cdev->gadget) && f->hs_descriptors)
-		usb_free_descriptors(f->hs_descriptors);
-	if (f->descriptors)
-		usb_free_descriptors(f->descriptors);
-
-	if (rndis->notify_req) {
-		kfree(rndis->notify_req->buf);
-		usb_ep_free_request(rndis->notify, rndis->notify_req);
-	}
-
-	/* we might as well release our claims on endpoints */
-	if (rndis->notify)
-		rndis->notify->driver_data = NULL;
-	if (rndis->port.out_ep->desc)
-		rndis->port.out_ep->driver_data = NULL;
-	if (rndis->port.in_ep->desc)
-		rndis->port.in_ep->driver_data = NULL;
-
-	ERROR(cdev, "%s: can't bind, err %d\n", f->name, status);
-
-	return status;
-}
-
-static void
-rndis_unbind(struct usb_configuration *c, struct usb_function *f)
-{
-	struct f_rndis		*rndis = func_to_rndis(f);
-
-	rndis_deregister(rndis->config);
-	rndis_exit();
-	rndis_string_defs[0].id = 0;
-
-	if (gadget_is_superspeed(c->cdev->gadget))
-		usb_free_descriptors(f->ss_descriptors);
-	if (gadget_is_dualspeed(c->cdev->gadget))
-		usb_free_descriptors(f->hs_descriptors);
-	usb_free_descriptors(f->descriptors);
-
-	kfree(rndis->notify_req->buf);
-	usb_ep_free_request(rndis->notify, rndis->notify_req);
-
-	kfree(rndis);
-}
-
-/* Some controllers can't support RNDIS ... */
-static inline bool can_support_rndis(struct usb_configuration *c)
-{
-	/* everything else is *presumably* fine */
-	return true;
-}
-
-int
-rndis_bind_config_vendor(struct usb_configuration *c, u8 ethaddr[ETH_ALEN],
-				u32 vendorID, const char *manufacturer)
-{
-	struct f_rndis	*rndis;
-	int		status;
-
-	if (!can_support_rndis(c) || !ethaddr)
-		return -EINVAL;
-
-	/* maybe allocate device-global string IDs */
-	if (rndis_string_defs[0].id == 0) {
-
-		/* ... and setup RNDIS itself */
-		status = rndis_init();
-		if (status < 0)
-			return status;
-
-		/* control interface label */
-		status = usb_string_id(c->cdev);
-		if (status < 0)
-			return status;
-		rndis_string_defs[0].id = status;
-		rndis_control_intf.iInterface = status;
-
-		/* data interface label */
-		status = usb_string_id(c->cdev);
-		if (status < 0)
-			return status;
-		rndis_string_defs[1].id = status;
-		rndis_data_intf.iInterface = status;
-
-		/* IAD iFunction label */
-		status = usb_string_id(c->cdev);
-		if (status < 0)
-			return status;
-		rndis_string_defs[2].id = status;
-		rndis_iad_descriptor.iFunction = status;
-	}
-
-	/* allocate and initialize one new instance */
-	status = -ENOMEM;
-	rndis = kzalloc(sizeof *rndis, GFP_KERNEL);
-	if (!rndis)
-		goto fail;
-
-	memcpy(rndis->ethaddr, ethaddr, ETH_ALEN);
-	rndis->vendorID = vendorID;
-	rndis->manufacturer = manufacturer;
-
-	/* RNDIS activates when the host changes this filter */
-	rndis->port.cdc_filter = 0;
-
-	/* RNDIS has special (and complex) framing */
-	rndis->port.header_len = sizeof(struct rndis_packet_msg_type);
-	rndis->port.wrap = rndis_add_header;
-	rndis->port.unwrap = rndis_rm_hdr;
-
-	rndis->port.func.name = "rndis";
-	rndis->port.func.strings = rndis_strings;
-	/* descriptors are per-instance copies */
-	rndis->port.func.bind = rndis_bind;
-	rndis->port.func.unbind = rndis_unbind;
-	rndis->port.func.set_alt = rndis_set_alt;
-	rndis->port.func.setup = rndis_setup;
-	rndis->port.func.disable = rndis_disable;
-
-	status = usb_add_function(c, &rndis->port.func);
-	if (status) {
-		kfree(rndis);
-fail:
-		rndis_exit();
-	}
-	return status;
-}
diff --git a/drivers/staging/ccg/gadget_chips.h b/drivers/staging/ccg/gadget_chips.h
deleted file mode 100644
index 0ccca58e7a8..00000000000
--- a/drivers/staging/ccg/gadget_chips.h
+++ /dev/null
@@ -1,150 +0,0 @@
-/*
- * USB device controllers have lots of quirks.  Use these macros in
- * gadget drivers or other code that needs to deal with them, and which
- * autoconfigures instead of using early binding to the hardware.
- *
- * This SHOULD eventually work like the ARM mach_is_*() stuff, driven by
- * some config file that gets updated as new hardware is supported.
- * (And avoiding all runtime comparisons in typical one-choice configs!)
- *
- * NOTE:  some of these controller drivers may not be available yet.
- * Some are available on 2.4 kernels; several are available, but not
- * yet pushed in the 2.6 mainline tree.
- */
-
-#ifndef __GADGET_CHIPS_H
-#define __GADGET_CHIPS_H
-
-/*
- * NOTICE: the entries below are alphabetical and should be kept
- * that way.
- *
- * Always be sure to add new entries to the correct position or
- * accept the bashing later.
- *
- * If you have forgotten the alphabetical order let VIM/EMACS
- * do that for you.
- */
-#define gadget_is_amd5536udc(g)		(!strcmp("amd5536udc", (g)->name))
-#define gadget_is_at91(g)		(!strcmp("at91_udc", (g)->name))
-#define gadget_is_atmel_usba(g)		(!strcmp("atmel_usba_udc", (g)->name))
-#define gadget_is_bcm63xx(g)		(!strcmp("bcm63xx_udc", (g)->name))
-#define gadget_is_ci13xxx_msm(g)	(!strcmp("ci13xxx_msm", (g)->name))
-#define gadget_is_ci13xxx_pci(g)	(!strcmp("ci13xxx_pci", (g)->name))
-#define gadget_is_dummy(g)		(!strcmp("dummy_udc", (g)->name))
-#define gadget_is_dwc3(g)		(!strcmp("dwc3-gadget", (g)->name))
-#define gadget_is_fsl_qe(g)		(!strcmp("fsl_qe_udc", (g)->name))
-#define gadget_is_fsl_usb2(g)		(!strcmp("fsl-usb2-udc", (g)->name))
-#define gadget_is_goku(g)		(!strcmp("goku_udc", (g)->name))
-#define gadget_is_imx(g)		(!strcmp("imx_udc", (g)->name))
-#define gadget_is_langwell(g)		(!strcmp("langwell_udc", (g)->name))
-#define gadget_is_lpc32xx(g)		(!strcmp("lpc32xx_udc", (g)->name))
-#define gadget_is_m66592(g)		(!strcmp("m66592_udc", (g)->name))
-#define gadget_is_musbhdrc(g)		(!strcmp("musb-hdrc", (g)->name))
-#define gadget_is_net2272(g)		(!strcmp("net2272", (g)->name))
-#define gadget_is_net2280(g)		(!strcmp("net2280", (g)->name))
-#define gadget_is_omap(g)		(!strcmp("omap_udc", (g)->name))
-#define gadget_is_pch(g)		(!strcmp("pch_udc", (g)->name))
-#define gadget_is_pxa(g)		(!strcmp("pxa25x_udc", (g)->name))
-#define gadget_is_pxa27x(g)		(!strcmp("pxa27x_udc", (g)->name))
-#define gadget_is_r8a66597(g)		(!strcmp("r8a66597_udc", (g)->name))
-#define gadget_is_renesas_usbhs(g)	(!strcmp("renesas_usbhs_udc", (g)->name))
-#define gadget_is_s3c2410(g)		(!strcmp("s3c2410_udc", (g)->name))
-#define gadget_is_s3c_hsotg(g)		(!strcmp("s3c-hsotg", (g)->name))
-#define gadget_is_s3c_hsudc(g)		(!strcmp("s3c-hsudc", (g)->name))
-
-/**
- * usb_gadget_controller_number - support bcdDevice id convention
- * @gadget: the controller being driven
- *
- * Return a 2-digit BCD value associated with the peripheral controller,
- * suitable for use as part of a bcdDevice value, or a negative error code.
- *
- * NOTE:  this convention is purely optional, and has no meaning in terms of
- * any USB specification.  If you want to use a different convention in your
- * gadget driver firmware -- maybe a more formal revision ID -- feel free.
- *
- * Hosts see these bcdDevice numbers, and are allowed (but not encouraged!)
- * to change their behavior accordingly.  For example it might help avoiding
- * some chip bug.
- */
-static inline int usb_gadget_controller_number(struct usb_gadget *gadget)
-{
-	if (gadget_is_net2280(gadget))
-		return 0x01;
-	else if (gadget_is_dummy(gadget))
-		return 0x02;
-	else if (gadget_is_pxa(gadget))
-		return 0x03;
-	else if (gadget_is_goku(gadget))
-		return 0x06;
-	else if (gadget_is_omap(gadget))
-		return 0x08;
-	else if (gadget_is_pxa27x(gadget))
-		return 0x11;
-	else if (gadget_is_s3c2410(gadget))
-		return 0x12;
-	else if (gadget_is_at91(gadget))
-		return 0x13;
-	else if (gadget_is_imx(gadget))
-		return 0x14;
-	else if (gadget_is_musbhdrc(gadget))
-		return 0x16;
-	else if (gadget_is_atmel_usba(gadget))
-		return 0x18;
-	else if (gadget_is_fsl_usb2(gadget))
-		return 0x19;
-	else if (gadget_is_amd5536udc(gadget))
-		return 0x20;
-	else if (gadget_is_m66592(gadget))
-		return 0x21;
-	else if (gadget_is_fsl_qe(gadget))
-		return 0x22;
-	else if (gadget_is_ci13xxx_pci(gadget))
-		return 0x23;
-	else if (gadget_is_langwell(gadget))
-		return 0x24;
-	else if (gadget_is_r8a66597(gadget))
-		return 0x25;
-	else if (gadget_is_s3c_hsotg(gadget))
-		return 0x26;
-	else if (gadget_is_pch(gadget))
-		return 0x27;
-	else if (gadget_is_ci13xxx_msm(gadget))
-		return 0x28;
-	else if (gadget_is_renesas_usbhs(gadget))
-		return 0x29;
-	else if (gadget_is_s3c_hsudc(gadget))
-		return 0x30;
-	else if (gadget_is_net2272(gadget))
-		return 0x31;
-	else if (gadget_is_dwc3(gadget))
-		return 0x32;
-	else if (gadget_is_lpc32xx(gadget))
-		return 0x33;
-	else if (gadget_is_bcm63xx(gadget))
-		return 0x34;
-
-	return -ENOENT;
-}
-
-
-/**
- * gadget_supports_altsettings - return true if altsettings work
- * @gadget: the gadget in question
- */
-static inline bool gadget_supports_altsettings(struct usb_gadget *gadget)
-{
-	/* PXA 21x/25x/26x has no altsettings at all */
-	if (gadget_is_pxa(gadget))
-		return false;
-
-	/* PXA 27x and 3xx have *broken* altsetting support */
-	if (gadget_is_pxa27x(gadget))
-		return false;
-
-	/* Everything else is *presumably* fine ... */
-	return true;
-}
-
-#endif /* __GADGET_CHIPS_H */
diff --git a/drivers/staging/ccg/ndis.h b/drivers/staging/ccg/ndis.h
deleted file mode 100644
index a19f72dec0c..00000000000
--- a/drivers/staging/ccg/ndis.h
+++ /dev/null
@@ -1,47 +0,0 @@
-/*
- * ndis.h
- *
- * ntddndis.h modified by Benedikt Spranger <b.spranger@pengutronix.de>
- *
- * Thanks to the cygwin development team,
- * espacially to Casper S. Hornstrup <chorns@users.sourceforge.net>
- *
- * THIS SOFTWARE IS NOT COPYRIGHTED
- *
- * This source code is offered for use in the public domain. You may
- * use, modify or distribute it freely.
- */
-
-#ifndef _LINUX_NDIS_H
-#define _LINUX_NDIS_H
-
-enum NDIS_DEVICE_POWER_STATE {
-	NdisDeviceStateUnspecified = 0,
-	NdisDeviceStateD0,
-	NdisDeviceStateD1,
-	NdisDeviceStateD2,
-	NdisDeviceStateD3,
-	NdisDeviceStateMaximum
-};
-
-struct NDIS_PM_WAKE_UP_CAPABILITIES {
-	enum NDIS_DEVICE_POWER_STATE  MinMagicPacketWakeUp;
-	enum NDIS_DEVICE_POWER_STATE  MinPatternWakeUp;
-	enum NDIS_DEVICE_POWER_STATE  MinLinkChangeWakeUp;
-};
-
-struct NDIS_PNP_CAPABILITIES {
-	__le32					Flags;
-	struct NDIS_PM_WAKE_UP_CAPABILITIES	WakeUpCapabilities;
-};
-
-struct NDIS_PM_PACKET_PATTERN {
-	__le32	Priority;
-	__le32	Reserved;
-	__le32	MaskSize;
-	__le32	PatternOffset;
-	__le32	PatternSize;
-	__le32	PatternFlags;
-};
-
-#endif /* _LINUX_NDIS_H */
diff --git a/drivers/staging/ccg/rndis.c b/drivers/staging/ccg/rndis.c
deleted file mode 100644
index d9297eebbf7..00000000000
--- a/drivers/staging/ccg/rndis.c
+++ /dev/null
@@ -1,1175 +0,0 @@
-/*
- * RNDIS MSG parser
- *
- * Authors:	Benedikt Spranger, Pengutronix
- *		Robert Schwebel, Pengutronix
- *
- *              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 software was originally developed in conformance with
- *		Microsoft's Remote NDIS Specification License Agreement.
- *
- * 03/12/2004 Kai-Uwe Bloem <linux-development@auerswald.de>
- *		Fixed message length bug in init_response
- *
- * 03/25/2004 Kai-Uwe Bloem <linux-development@auerswald.de>
- *		Fixed rndis_rm_hdr length bug.
- *
- * Copyright (C) 2004 by David Brownell
- *		updates to merge with Linux 2.6, better match RNDIS spec
- */
-
-#include <linux/module.h>
-#include <linux/moduleparam.h>
-#include <linux/kernel.h>
-#include <linux/errno.h>
-#include <linux/init.h>
-#include <linux/list.h>
-#include <linux/proc_fs.h>
-#include <linux/slab.h>
-#include <linux/seq_file.h>
-#include <linux/netdevice.h>
-
-#include <asm/io.h>
-#include <asm/byteorder.h>
-#include <asm/unaligned.h>
-
-
-#undef	VERBOSE_DEBUG
-
-#include "rndis.h"
-
-
-/* The driver for your USB chip needs to support ep0 OUT to work with
- * RNDIS, plus all three CDC Ethernet endpoints (interrupt not optional).
- *
- * Windows hosts need an INF file like Documentation/usb/linux.inf
- * and will be happier if you provide the host_addr module parameter.
- */
-
-#if 0
-static int rndis_debug = 0;
-module_param (rndis_debug, int, 0);
-MODULE_PARM_DESC (rndis_debug, "enable debugging");
-#else
-#define rndis_debug		0
-#endif
-
-#define RNDIS_MAX_CONFIGS	1
-
-
-static rndis_params rndis_per_dev_params[RNDIS_MAX_CONFIGS];
-
-/* Driver Version */
-static const __le32 rndis_driver_version = cpu_to_le32(1);
-
-/* Function Prototypes */
-static rndis_resp_t *rndis_add_response(int configNr, u32 length);
-
-
-/* supported OIDs */
-static const u32 oid_supported_list[] =
-{
-	/* the general stuff */
-	RNDIS_OID_GEN_SUPPORTED_LIST,
-	RNDIS_OID_GEN_HARDWARE_STATUS,
-	RNDIS_OID_GEN_MEDIA_SUPPORTED,
-	RNDIS_OID_GEN_MEDIA_IN_USE,
-	RNDIS_OID_GEN_MAXIMUM_FRAME_SIZE,
-	RNDIS_OID_GEN_LINK_SPEED,
-	RNDIS_OID_GEN_TRANSMIT_BLOCK_SIZE,
-	RNDIS_OID_GEN_RECEIVE_BLOCK_SIZE,
-	RNDIS_OID_GEN_VENDOR_ID,
-	RNDIS_OID_GEN_VENDOR_DESCRIPTION,
-	RNDIS_OID_GEN_VENDOR_DRIVER_VERSION,
-	RNDIS_OID_GEN_CURRENT_PACKET_FILTER,
-	RNDIS_OID_GEN_MAXIMUM_TOTAL_SIZE,
-	RNDIS_OID_GEN_MEDIA_CONNECT_STATUS,
-	RNDIS_OID_GEN_PHYSICAL_MEDIUM,
-
-	/* the statistical stuff */
-	RNDIS_OID_GEN_XMIT_OK,
-	RNDIS_OID_GEN_RCV_OK,
-	RNDIS_OID_GEN_XMIT_ERROR,
-	RNDIS_OID_GEN_RCV_ERROR,
-	RNDIS_OID_GEN_RCV_NO_BUFFER,
-#ifdef	RNDIS_OPTIONAL_STATS
-	RNDIS_OID_GEN_DIRECTED_BYTES_XMIT,
-	RNDIS_OID_GEN_DIRECTED_FRAMES_XMIT,
-	RNDIS_OID_GEN_MULTICAST_BYTES_XMIT,
-	RNDIS_OID_GEN_MULTICAST_FRAMES_XMIT,
-	RNDIS_OID_GEN_BROADCAST_BYTES_XMIT,
-	RNDIS_OID_GEN_BROADCAST_FRAMES_XMIT,
-	RNDIS_OID_GEN_DIRECTED_BYTES_RCV,
-	RNDIS_OID_GEN_DIRECTED_FRAMES_RCV,
-	RNDIS_OID_GEN_MULTICAST_BYTES_RCV,
-	RNDIS_OID_GEN_MULTICAST_FRAMES_RCV,
-	RNDIS_OID_GEN_BROADCAST_BYTES_RCV,
-	RNDIS_OID_GEN_BROADCAST_FRAMES_RCV,
-	RNDIS_OID_GEN_RCV_CRC_ERROR,
-	RNDIS_OID_GEN_TRANSMIT_QUEUE_LENGTH,
-#endif	/* RNDIS_OPTIONAL_STATS */
-
-	/* mandatory 802.3 */
-	/* the general stuff */
-	RNDIS_OID_802_3_PERMANENT_ADDRESS,
-	RNDIS_OID_802_3_CURRENT_ADDRESS,
-	RNDIS_OID_802_3_MULTICAST_LIST,
-	RNDIS_OID_802_3_MAC_OPTIONS,
-	RNDIS_OID_802_3_MAXIMUM_LIST_SIZE,
-
-	/* the statistical stuff */
-	RNDIS_OID_802_3_RCV_ERROR_ALIGNMENT,
-	RNDIS_OID_802_3_XMIT_ONE_COLLISION,
-	RNDIS_OID_802_3_XMIT_MORE_COLLISIONS,
-#ifdef	RNDIS_OPTIONAL_STATS
-	RNDIS_OID_802_3_XMIT_DEFERRED,
-	RNDIS_OID_802_3_XMIT_MAX_COLLISIONS,
-	RNDIS_OID_802_3_RCV_OVERRUN,
-	RNDIS_OID_802_3_XMIT_UNDERRUN,
-	RNDIS_OID_802_3_XMIT_HEARTBEAT_FAILURE,
-	RNDIS_OID_802_3_XMIT_TIMES_CRS_LOST,
-	RNDIS_OID_802_3_XMIT_LATE_COLLISIONS,
-#endif	/* RNDIS_OPTIONAL_STATS */
-
-#ifdef	RNDIS_PM
-	/* PM and wakeup are "mandatory" for USB, but the RNDIS specs
-	 * don't say what they mean ... and the NDIS specs are often
-	 * confusing and/or ambiguous in this context.  (That is, more
-	 * so than their specs for the other OIDs.)
-	 *
-	 * FIXME someone who knows what these should do, please
-	 * implement them!
-	 */
-
-	/* power management */
-	OID_PNP_CAPABILITIES,
-	OID_PNP_QUERY_POWER,
-	OID_PNP_SET_POWER,
-
-#ifdef	RNDIS_WAKEUP
-	/* wake up host */
-	OID_PNP_ENABLE_WAKE_UP,
-	OID_PNP_ADD_WAKE_UP_PATTERN,
-	OID_PNP_REMOVE_WAKE_UP_PATTERN,
-#endif	/* RNDIS_WAKEUP */
-#endif	/* RNDIS_PM */
-};
-
-
-/* NDIS Functions */
-static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
-			       unsigned buf_len, rndis_resp_t *r)
-{
-	int retval = -ENOTSUPP;
-	u32 length = 4;	/* usually */
-	__le32 *outbuf;
-	int i, count;
-	rndis_query_cmplt_type *resp;
-	struct net_device *net;
-	struct rtnl_link_stats64 temp;
-	const struct rtnl_link_stats64 *stats;
-
-	if (!r) return -ENOMEM;
-	resp = (rndis_query_cmplt_type *)r->buf;
-
-	if (!resp) return -ENOMEM;
-
-	if (buf_len && rndis_debug > 1) {
-		pr_debug("query OID %08x value, len %d:\n", OID, buf_len);
-		for (i = 0; i < buf_len; i += 16) {
-			pr_debug("%03d: %08x %08x %08x %08x\n", i,
-				get_unaligned_le32(&buf[i]),
-				get_unaligned_le32(&buf[i + 4]),
-				get_unaligned_le32(&buf[i + 8]),
-				get_unaligned_le32(&buf[i + 12]));
-		}
-	}
-
-	/* response goes here, right after the header */
-	outbuf = (__le32 *)&resp[1];
-	resp->InformationBufferOffset = cpu_to_le32(16);
-
-	net = rndis_per_dev_params[configNr].dev;
-	stats = dev_get_stats(net, &temp);
-
-	switch (OID) {
-
-	/* general oids (table 4-1) */
-
-	/* mandatory */
-	case RNDIS_OID_GEN_SUPPORTED_LIST:
-		pr_debug("%s: RNDIS_OID_GEN_SUPPORTED_LIST\n", __func__);
-		length = sizeof(oid_supported_list);
-		count  = length / sizeof(u32);
-		for (i = 0; i < count; i++)
-			outbuf[i] = cpu_to_le32(oid_supported_list[i]);
-		retval = 0;
-		break;
-
-	/* mandatory */
-	case RNDIS_OID_GEN_HARDWARE_STATUS:
-		pr_debug("%s: RNDIS_OID_GEN_HARDWARE_STATUS\n", __func__);
-		/* Bogus question!
-		 * Hardware must be ready to receive high level protocols.
-		 * BTW:
-		 * reddite ergo quae sunt Caesaris Caesari
-		 * et quae sunt Dei Deo!
-		 */
-		*outbuf = cpu_to_le32(0);
-		retval = 0;
-		break;
-
-	/* mandatory */
-	case RNDIS_OID_GEN_MEDIA_SUPPORTED:
-		pr_debug("%s: RNDIS_OID_GEN_MEDIA_SUPPORTED\n", __func__);
-		*outbuf = cpu_to_le32(rndis_per_dev_params[configNr].medium);
-		retval = 0;
-		break;
-
-	/* mandatory */
-	case RNDIS_OID_GEN_MEDIA_IN_USE:
-		pr_debug("%s: RNDIS_OID_GEN_MEDIA_IN_USE\n", __func__);
-		/* one medium, one transport... (maybe you do it better) */
-		*outbuf = cpu_to_le32(rndis_per_dev_params[configNr].medium);
-		retval = 0;
-		break;
-
-	/* mandatory */
-	case RNDIS_OID_GEN_MAXIMUM_FRAME_SIZE:
-		pr_debug("%s: RNDIS_OID_GEN_MAXIMUM_FRAME_SIZE\n", __func__);
-		if (rndis_per_dev_params[configNr].dev) {
-			*outbuf = cpu_to_le32(
-				rndis_per_dev_params[configNr].dev->mtu);
-			retval = 0;
-		}
-		break;
-
-	/* mandatory */
-	case RNDIS_OID_GEN_LINK_SPEED:
-		if (rndis_debug > 1)
-			pr_debug("%s: RNDIS_OID_GEN_LINK_SPEED\n", __func__);
-		if (rndis_per_dev_params[configNr].media_state
-				== RNDIS_MEDIA_STATE_DISCONNECTED)
-			*outbuf = cpu_to_le32(0);
-		else
-			*outbuf = cpu_to_le32(
-				rndis_per_dev_params[configNr].speed);
-		retval = 0;
-		break;
-
-	/* mandatory */
-	case RNDIS_OID_GEN_TRANSMIT_BLOCK_SIZE:
-		pr_debug("%s: RNDIS_OID_GEN_TRANSMIT_BLOCK_SIZE\n", __func__);
-		if (rndis_per_dev_params[configNr].dev) {
-			*outbuf = cpu_to_le32(
-				rndis_per_dev_params[configNr].dev->mtu);
-			retval = 0;
-		}
-		break;
-
-	/* mandatory */
-	case RNDIS_OID_GEN_RECEIVE_BLOCK_SIZE:
-		pr_debug("%s: RNDIS_OID_GEN_RECEIVE_BLOCK_SIZE\n", __func__);
-		if (rndis_per_dev_params[configNr].dev) {
-			*outbuf = cpu_to_le32(
-				rndis_per_dev_params[configNr].dev->mtu);
-			retval = 0;
-		}
-		break;
-
-	/* mandatory */
-	case RNDIS_OID_GEN_VENDOR_ID:
-		pr_debug("%s: RNDIS_OID_GEN_VENDOR_ID\n", __func__);
-		*outbuf = cpu_to_le32(
-			rndis_per_dev_params[configNr].vendorID);
-		retval = 0;
-		break;
-
-	/* mandatory */
-	case RNDIS_OID_GEN_VENDOR_DESCRIPTION:
-		pr_debug("%s: RNDIS_OID_GEN_VENDOR_DESCRIPTION\n", __func__);
-		if (rndis_per_dev_params[configNr].vendorDescr) {
-			length = strlen(rndis_per_dev_params[configNr].
-					vendorDescr);
-			memcpy(outbuf,
-				rndis_per_dev_params[configNr].vendorDescr,
-				length);
-		} else {
-			outbuf[0] = 0;
-		}
-		retval = 0;
-		break;
-
-	case RNDIS_OID_GEN_VENDOR_DRIVER_VERSION:
-		pr_debug("%s: RNDIS_OID_GEN_VENDOR_DRIVER_VERSION\n", __func__);
-		/* Created as LE */
-		*outbuf = rndis_driver_version;
-		retval = 0;
-		break;
-
-	/* mandatory */
-	case RNDIS_OID_GEN_CURRENT_PACKET_FILTER:
-		pr_debug("%s: RNDIS_OID_GEN_CURRENT_PACKET_FILTER\n", __func__);
-		*outbuf = cpu_to_le32(*rndis_per_dev_params[configNr].filter);
-		retval = 0;
-		break;
-
-	/* mandatory */
-	case RNDIS_OID_GEN_MAXIMUM_TOTAL_SIZE:
-		pr_debug("%s: RNDIS_OID_GEN_MAXIMUM_TOTAL_SIZE\n", __func__);
-		*outbuf = cpu_to_le32(RNDIS_MAX_TOTAL_SIZE);
-		retval = 0;
-		break;
-
-	/* mandatory */
-	case RNDIS_OID_GEN_MEDIA_CONNECT_STATUS:
-		if (rndis_debug > 1)
-			pr_debug("%s: RNDIS_OID_GEN_MEDIA_CONNECT_STATUS\n", __func__);
-		*outbuf = cpu_to_le32(rndis_per_dev_params[configNr]
-						.media_state);
-		retval = 0;
-		break;
-
-	case RNDIS_OID_GEN_PHYSICAL_MEDIUM:
-		pr_debug("%s: RNDIS_OID_GEN_PHYSICAL_MEDIUM\n", __func__);
-		*outbuf = cpu_to_le32(0);
-		retval = 0;
-		break;
-
-	/* The RNDIS specification is incomplete/wrong.   Some versions
-	 * of MS-Windows expect OIDs that aren't specified there.  Other
-	 * versions emit undefined RNDIS messages. DOCUMENT ALL THESE!
-	 */
-	case RNDIS_OID_GEN_MAC_OPTIONS:		/* from WinME */
-		pr_debug("%s: RNDIS_OID_GEN_MAC_OPTIONS\n", __func__);
-		*outbuf = cpu_to_le32(
-			  RNDIS_MAC_OPTION_RECEIVE_SERIALIZED
-			| RNDIS_MAC_OPTION_FULL_DUPLEX);
-		retval = 0;
-		break;
-
-	/* statistics OIDs (table 4-2) */
-
-	/* mandatory */
-	case RNDIS_OID_GEN_XMIT_OK:
-		if (rndis_debug > 1)
-			pr_debug("%s: RNDIS_OID_GEN_XMIT_OK\n", __func__);
-		if (stats) {
-			*outbuf = cpu_to_le32(stats->tx_packets
-				- stats->tx_errors - stats->tx_dropped);
-			retval = 0;
-		}
-		break;
-
-	/* mandatory */
-	case RNDIS_OID_GEN_RCV_OK:
-		if (rndis_debug > 1)
-			pr_debug("%s: RNDIS_OID_GEN_RCV_OK\n", __func__);
-		if (stats) {
-			*outbuf = cpu_to_le32(stats->rx_packets
-				- stats->rx_errors - stats->rx_dropped);
-			retval = 0;
-		}
-		break;
-
-	/* mandatory */
-	case RNDIS_OID_GEN_XMIT_ERROR:
-		if (rndis_debug > 1)
-			pr_debug("%s: RNDIS_OID_GEN_XMIT_ERROR\n", __func__);
-		if (stats) {
-			*outbuf = cpu_to_le32(stats->tx_errors);
-			retval = 0;
-		}
-		break;
-
-	/* mandatory */
-	case RNDIS_OID_GEN_RCV_ERROR:
-		if (rndis_debug > 1)
-			pr_debug("%s: RNDIS_OID_GEN_RCV_ERROR\n", __func__);
-		if (stats) {
-			*outbuf = cpu_to_le32(stats->rx_errors);
-			retval = 0;
-		}
-		break;
-
-	/* mandatory */
-	case RNDIS_OID_GEN_RCV_NO_BUFFER:
-		pr_debug("%s: RNDIS_OID_GEN_RCV_NO_BUFFER\n", __func__);
-		if (stats) {
-			*outbuf = cpu_to_le32(stats->rx_dropped);
-			retval = 0;
-		}
-		break;
-
-	/* ieee802.3 OIDs (table 4-3) */
-
-	/* mandatory */
-	case RNDIS_OID_802_3_PERMANENT_ADDRESS:
-		pr_debug("%s: RNDIS_OID_802_3_PERMANENT_ADDRESS\n", __func__);
-		if (rndis_per_dev_params[configNr].dev) {
-			length = ETH_ALEN;
-			memcpy(outbuf,
-				rndis_per_dev_params[configNr].host_mac,
-				length);
-			retval = 0;
-		}
-		break;
-
-	/* mandatory */
-	case RNDIS_OID_802_3_CURRENT_ADDRESS:
-		pr_debug("%s: RNDIS_OID_802_3_CURRENT_ADDRESS\n", __func__);
-		if (rndis_per_dev_params[configNr].dev) {
-			length = ETH_ALEN;
-			memcpy(outbuf,
-				rndis_per_dev_params [configNr].host_mac,
-				length);
-			retval = 0;
-		}
-		break;
-
-	/* mandatory */
-	case RNDIS_OID_802_3_MULTICAST_LIST:
-		pr_debug("%s: RNDIS_OID_802_3_MULTICAST_LIST\n", __func__);
-		/* Multicast base address only */
-		*outbuf = cpu_to_le32(0xE0000000);
-		retval = 0;
-		break;
-
-	/* mandatory */
-	case RNDIS_OID_802_3_MAXIMUM_LIST_SIZE:
-		pr_debug("%s: RNDIS_OID_802_3_MAXIMUM_LIST_SIZE\n", __func__);
-		/* Multicast base address only */
-		*outbuf = cpu_to_le32(1);
-		retval = 0;
-		break;
-
-	case RNDIS_OID_802_3_MAC_OPTIONS:
-		pr_debug("%s: RNDIS_OID_802_3_MAC_OPTIONS\n", __func__);
-		*outbuf = cpu_to_le32(0);
-		retval = 0;
-		break;
-
-	/* ieee802.3 statistics OIDs (table 4-4) */
-
-	/* mandatory */
-	case RNDIS_OID_802_3_RCV_ERROR_ALIGNMENT:
-		pr_debug("%s: RNDIS_OID_802_3_RCV_ERROR_ALIGNMENT\n", __func__);
-		if (stats) {
-			*outbuf = cpu_to_le32(stats->rx_frame_errors);
-			retval = 0;
-		}
-		break;
-
-	/* mandatory */
-	case RNDIS_OID_802_3_XMIT_ONE_COLLISION:
-		pr_debug("%s: RNDIS_OID_802_3_XMIT_ONE_COLLISION\n", __func__);
-		*outbuf = cpu_to_le32(0);
-		retval = 0;
-		break;
-
-	/* mandatory */
-	case RNDIS_OID_802_3_XMIT_MORE_COLLISIONS:
-		pr_debug("%s: RNDIS_OID_802_3_XMIT_MORE_COLLISIONS\n", __func__);
-		*outbuf = cpu_to_le32(0);
-		retval = 0;
-		break;
-
-	default:
-		pr_warning("%s: query unknown OID 0x%08X\n",
-			 __func__, OID);
-	}
-	if (retval < 0)
-		length = 0;
-
-	resp->InformationBufferLength = cpu_to_le32(length);
-	r->length = length + sizeof(*resp);
-	resp->MessageLength = cpu_to_le32(r->length);
-	return retval;
-}
-
-static int gen_ndis_set_resp(u8 configNr, u32 OID, u8 *buf, u32 buf_len,
-			     rndis_resp_t *r)
-{
-	rndis_set_cmplt_type *resp;
-	int i, retval = -ENOTSUPP;
-	struct rndis_params *params;
-
-	if (!r)
-		return -ENOMEM;
-	resp = (rndis_set_cmplt_type *)r->buf;
-	if (!resp)
-		return -ENOMEM;
-
-	if (buf_len && rndis_debug > 1) {
-		pr_debug("set OID %08x value, len %d:\n", OID, buf_len);
-		for (i = 0; i < buf_len; i += 16) {
-			pr_debug("%03d: %08x %08x %08x %08x\n", i,
-				get_unaligned_le32(&buf[i]),
-				get_unaligned_le32(&buf[i + 4]),
-				get_unaligned_le32(&buf[i + 8]),
-				get_unaligned_le32(&buf[i + 12]));
-		}
-	}
-
-	params = &rndis_per_dev_params[configNr];
-	switch (OID) {
-	case RNDIS_OID_GEN_CURRENT_PACKET_FILTER:
-
-		/* these NDIS_PACKET_TYPE_* bitflags are shared with
-		 * cdc_filter; it's not RNDIS-specific
-		 * NDIS_PACKET_TYPE_x == USB_CDC_PACKET_TYPE_x for x in:
-		 *	PROMISCUOUS, DIRECTED,
-		 *	MULTICAST, ALL_MULTICAST, BROADCAST
-		 */
-		*params->filter = (u16)get_unaligned_le32(buf);
-		pr_debug("%s: RNDIS_OID_GEN_CURRENT_PACKET_FILTER %08x\n",
-			__func__, *params->filter);
-
-		/* this call has a significant side effect:  it's
-		 * what makes the packet flow start and stop, like
-		 * activating the CDC Ethernet altsetting.
-		 */
-		retval = 0;
-		if (*params->filter) {
-			params->state = RNDIS_DATA_INITIALIZED;
-			netif_carrier_on(params->dev);
-			if (netif_running(params->dev))
-				netif_wake_queue(params->dev);
-		} else {
-			params->state = RNDIS_INITIALIZED;
-			netif_carrier_off(params->dev);
-			netif_stop_queue(params->dev);
-		}
-		break;
-
-	case RNDIS_OID_802_3_MULTICAST_LIST:
-		/* I think we can ignore this */
-		pr_debug("%s: RNDIS_OID_802_3_MULTICAST_LIST\n", __func__);
-		retval = 0;
-		break;
-
-	default:
-		pr_warning("%s: set unknown OID 0x%08X, size %d\n",
-			 __func__, OID, buf_len);
-	}
-
-	return retval;
-}
-
-/*
- * Response Functions
- */
-
-static int rndis_init_response(int configNr, rndis_init_msg_type *buf)
-{
-	rndis_init_cmplt_type *resp;
-	rndis_resp_t *r;
-	struct rndis_params *params = rndis_per_dev_params + configNr;
-
-	if (!params->dev)
-		return -ENOTSUPP;
-
-	r = rndis_add_response(configNr, sizeof(rndis_init_cmplt_type));
-	if (!r)
-		return -ENOMEM;
-	resp = (rndis_init_cmplt_type *)r->buf;
-
-	resp->MessageType = cpu_to_le32(RNDIS_MSG_INIT_C);
-	resp->MessageLength = cpu_to_le32(52);
-	resp->RequestID = buf->RequestID; /* Still LE in msg buffer */
-	resp->Status = cpu_to_le32(RNDIS_STATUS_SUCCESS);
-	resp->MajorVersion = cpu_to_le32(RNDIS_MAJOR_VERSION);
-	resp->MinorVersion = cpu_to_le32(RNDIS_MINOR_VERSION);
-	resp->DeviceFlags = cpu_to_le32(RNDIS_DF_CONNECTIONLESS);
-	resp->Medium = cpu_to_le32(RNDIS_MEDIUM_802_3);
-	resp->MaxPacketsPerTransfer = cpu_to_le32(1);
-	resp->MaxTransferSize = cpu_to_le32(
-		  params->dev->mtu
-		+ sizeof(struct ethhdr)
-		+ sizeof(struct rndis_packet_msg_type)
-		+ 22);
-	resp->PacketAlignmentFactor = cpu_to_le32(0);
-	resp->AFListOffset = cpu_to_le32(0);
-	resp->AFListSize = cpu_to_le32(0);
-
-	params->resp_avail(params->v);
-	return 0;
-}
-
-static int rndis_query_response(int configNr, rndis_query_msg_type *buf)
-{
-	rndis_query_cmplt_type *resp;
-	rndis_resp_t *r;
-	struct rndis_params *params = rndis_per_dev_params + configNr;
-
-	/* pr_debug("%s: OID = %08X\n", __func__, cpu_to_le32(buf->OID)); */
-	if (!params->dev)
-		return -ENOTSUPP;
-
-	/*
-	 * we need more memory:
-	 * gen_ndis_query_resp expects enough space for
-	 * rndis_query_cmplt_type followed by data.
-	 * oid_supported_list is the largest data reply
-	 */
-	r = rndis_add_response(configNr,
-		sizeof(oid_supported_list) + sizeof(rndis_query_cmplt_type));
-	if (!r)
-		return -ENOMEM;
-	resp = (rndis_query_cmplt_type *)r->buf;
-
-	resp->MessageType = cpu_to_le32(RNDIS_MSG_QUERY_C);
-	resp->RequestID = buf->RequestID; /* Still LE in msg buffer */
-
-	if (gen_ndis_query_resp(configNr, le32_to_cpu(buf->OID),
-			le32_to_cpu(buf->InformationBufferOffset)
-					+ 8 + (u8 *)buf,
-			le32_to_cpu(buf->InformationBufferLength),
-			r)) {
-		/* OID not supported */
-		resp->Status = cpu_to_le32(RNDIS_STATUS_NOT_SUPPORTED);
-		resp->MessageLength = cpu_to_le32(sizeof *resp);
-		resp->InformationBufferLength = cpu_to_le32(0);
-		resp->InformationBufferOffset = cpu_to_le32(0);
-	} else
-		resp->Status = cpu_to_le32(RNDIS_STATUS_SUCCESS);
-
-	params->resp_avail(params->v);
-	return 0;
-}
-
-static int rndis_set_response(int configNr, rndis_set_msg_type *buf)
-{
-	u32 BufLength, BufOffset;
-	rndis_set_cmplt_type *resp;
-	rndis_resp_t *r;
-	struct rndis_params *params = rndis_per_dev_params + configNr;
-
-	r = rndis_add_response(configNr, sizeof(rndis_set_cmplt_type));
-	if (!r)
-		return -ENOMEM;
-	resp = (rndis_set_cmplt_type *)r->buf;
-
-	BufLength = le32_to_cpu(buf->InformationBufferLength);
-	BufOffset = le32_to_cpu(buf->InformationBufferOffset);
-
-#ifdef	VERBOSE_DEBUG
-	pr_debug("%s: Length: %d\n", __func__, BufLength);
-	pr_debug("%s: Offset: %d\n", __func__, BufOffset);
-	pr_debug("%s: InfoBuffer: ", __func__);
-
-	for (i = 0; i < BufLength; i++) {
-		pr_debug("%02x ", *(((u8 *) buf) + i + 8 + BufOffset));
-	}
-
-	pr_debug("\n");
-#endif
-
-	resp->MessageType = cpu_to_le32(RNDIS_MSG_SET_C);
-	resp->MessageLength = cpu_to_le32(16);
-	resp->RequestID = buf->RequestID; /* Still LE in msg buffer */
-	if (gen_ndis_set_resp(configNr, le32_to_cpu(buf->OID),
-			((u8 *)buf) + 8 + BufOffset, BufLength, r))
-		resp->Status = cpu_to_le32(RNDIS_STATUS_NOT_SUPPORTED);
-	else
-		resp->Status = cpu_to_le32(RNDIS_STATUS_SUCCESS);
-
-	params->resp_avail(params->v);
-	return 0;
-}
-
-static int rndis_reset_response(int configNr, rndis_reset_msg_type *buf)
-{
-	rndis_reset_cmplt_type *resp;
-	rndis_resp_t *r;
-	struct rndis_params *params = rndis_per_dev_params + configNr;
-
-	r = rndis_add_response(configNr, sizeof(rndis_reset_cmplt_type));
-	if (!r)
-		return -ENOMEM;
-	resp = (rndis_reset_cmplt_type *)r->buf;
-
-	resp->MessageType = cpu_to_le32(RNDIS_MSG_RESET_C);
-	resp->MessageLength = cpu_to_le32(16);
-	resp->Status = cpu_to_le32(RNDIS_STATUS_SUCCESS);
-	/* resent information */
-	resp->AddressingReset = cpu_to_le32(1);
-
-	params->resp_avail(params->v);
-	return 0;
-}
-
-static int rndis_keepalive_response(int configNr,
-				    rndis_keepalive_msg_type *buf)
-{
-	rndis_keepalive_cmplt_type *resp;
-	rndis_resp_t *r;
-	struct rndis_params *params = rndis_per_dev_params + configNr;
-
-	/* host "should" check only in RNDIS_DATA_INITIALIZED state */
-
-	r = rndis_add_response(configNr, sizeof(rndis_keepalive_cmplt_type));
-	if (!r)
-		return -ENOMEM;
-	resp = (rndis_keepalive_cmplt_type *)r->buf;
-
-	resp->MessageType = cpu_to_le32(RNDIS_MSG_KEEPALIVE_C);
-	resp->MessageLength = cpu_to_le32(16);
-	resp->RequestID = buf->RequestID; /* Still LE in msg buffer */
-	resp->Status = cpu_to_le32(RNDIS_STATUS_SUCCESS);
-
-	params->resp_avail(params->v);
-	return 0;
-}
-
-
-/*
- * Device to Host Comunication
- */
-static int rndis_indicate_status_msg(int configNr, u32 status)
-{
-	rndis_indicate_status_msg_type *resp;
-	rndis_resp_t *r;
-	struct rndis_params *params = rndis_per_dev_params + configNr;
-
-	if (params->state == RNDIS_UNINITIALIZED)
-		return -ENOTSUPP;
-
-	r = rndis_add_response(configNr,
-				sizeof(rndis_indicate_status_msg_type));
-	if (!r)
-		return -ENOMEM;
-	resp = (rndis_indicate_status_msg_type *)r->buf;
-
-	resp->MessageType = cpu_to_le32(RNDIS_MSG_INDICATE);
-	resp->MessageLength = cpu_to_le32(20);
-	resp->Status = cpu_to_le32(status);
-	resp->StatusBufferLength = cpu_to_le32(0);
-	resp->StatusBufferOffset = cpu_to_le32(0);
-
-	params->resp_avail(params->v);
-	return 0;
-}
-
-int rndis_signal_connect(int configNr)
-{
-	rndis_per_dev_params[configNr].media_state
-			= RNDIS_MEDIA_STATE_CONNECTED;
-	return rndis_indicate_status_msg(configNr,
-					  RNDIS_STATUS_MEDIA_CONNECT);
-}
-
-int rndis_signal_disconnect(int configNr)
-{
-	rndis_per_dev_params[configNr].media_state
-			= RNDIS_MEDIA_STATE_DISCONNECTED;
-	return rndis_indicate_status_msg(configNr,
-					  RNDIS_STATUS_MEDIA_DISCONNECT);
-}
-
-void rndis_uninit(int configNr)
-{
-	u8 *buf;
-	u32 length;
-
-	if (configNr >= RNDIS_MAX_CONFIGS)
-		return;
-	rndis_per_dev_params[configNr].state = RNDIS_UNINITIALIZED;
-
-	/* drain the response queue */
-	while ((buf = rndis_get_next_response(configNr, &length)))
-		rndis_free_response(configNr, buf);
-}
-
-void rndis_set_host_mac(int configNr, const u8 *addr)
-{
-	rndis_per_dev_params[configNr].host_mac = addr;
-}
-
-/*
- * Message Parser
- */
-int rndis_msg_parser(u8 configNr, u8 *buf)
-{
-	u32 MsgType, MsgLength;
-	__le32 *tmp;
-	struct rndis_params *params;
-
-	if (!buf)
-		return -ENOMEM;
-
-	tmp = (__le32 *)buf;
-	MsgType   = get_unaligned_le32(tmp++);
-	MsgLength = get_unaligned_le32(tmp++);
-
-	if (configNr >= RNDIS_MAX_CONFIGS)
-		return -ENOTSUPP;
-	params = &rndis_per_dev_params[configNr];
-
-	/* NOTE: RNDIS is *EXTREMELY* chatty ... Windows constantly polls for
-	 * rx/tx statistics and link status, in addition to KEEPALIVE traffic
-	 * and normal HC level polling to see if there's any IN traffic.
-	 */
-
-	/* For USB: responses may take up to 10 seconds */
-	switch (MsgType) {
-	case RNDIS_MSG_INIT:
-		pr_debug("%s: RNDIS_MSG_INIT\n",
-			__func__);
-		params->state = RNDIS_INITIALIZED;
-		return rndis_init_response(configNr,
-					(rndis_init_msg_type *)buf);
-
-	case RNDIS_MSG_HALT:
-		pr_debug("%s: RNDIS_MSG_HALT\n",
-			__func__);
-		params->state = RNDIS_UNINITIALIZED;
-		if (params->dev) {
-			netif_carrier_off(params->dev);
-			netif_stop_queue(params->dev);
-		}
-		return 0;
-
-	case RNDIS_MSG_QUERY:
-		return rndis_query_response(configNr,
-					(rndis_query_msg_type *)buf);
-
-	case RNDIS_MSG_SET:
-		return rndis_set_response(configNr,
-					(rndis_set_msg_type *)buf);
-
-	case RNDIS_MSG_RESET:
-		pr_debug("%s: RNDIS_MSG_RESET\n",
-			__func__);
-		return rndis_reset_response(configNr,
-					(rndis_reset_msg_type *)buf);
-
-	case RNDIS_MSG_KEEPALIVE:
-		/* For USB: host does this every 5 seconds */
-		if (rndis_debug > 1)
-			pr_debug("%s: RNDIS_MSG_KEEPALIVE\n",
-				__func__);
-		return rndis_keepalive_response(configNr,
-						 (rndis_keepalive_msg_type *)
-						 buf);
-
-	default:
-		/* At least Windows XP emits some undefined RNDIS messages.
-		 * In one case those messages seemed to relate to the host
-		 * suspending itself.
-		 */
-		pr_warning("%s: unknown RNDIS message 0x%08X len %d\n",
-			__func__, MsgType, MsgLength);
-		print_hex_dump_bytes(__func__, DUMP_PREFIX_OFFSET,
-				     buf, MsgLength);
-		break;
-	}
-
-	return -ENOTSUPP;
-}
-
-int rndis_register(void (*resp_avail)(void *v), void *v)
-{
-	u8 i;
-
-	if (!resp_avail)
-		return -EINVAL;
-
-	for (i = 0; i < RNDIS_MAX_CONFIGS; i++) {
-		if (!rndis_per_dev_params[i].used) {
-			rndis_per_dev_params[i].used = 1;
-			rndis_per_dev_params[i].resp_avail = resp_avail;
-			rndis_per_dev_params[i].v = v;
-			pr_debug("%s: configNr = %d\n", __func__, i);
-			return i;
-		}
-	}
-	pr_debug("failed\n");
-
-	return -ENODEV;
-}
-
-void rndis_deregister(int configNr)
-{
-	pr_debug("%s:\n", __func__);
-
-	if (configNr >= RNDIS_MAX_CONFIGS) return;
-	rndis_per_dev_params[configNr].used = 0;
-}
-
-int rndis_set_param_dev(u8 configNr, struct net_device *dev, u16 *cdc_filter)
-{
-	pr_debug("%s:\n", __func__);
-	if (!dev)
-		return -EINVAL;
-	if (configNr >= RNDIS_MAX_CONFIGS) return -1;
-
-	rndis_per_dev_params[configNr].dev = dev;
-	rndis_per_dev_params[configNr].filter = cdc_filter;
-
-	return 0;
-}
-
-int rndis_set_param_vendor(u8 configNr, u32 vendorID, const char *vendorDescr)
-{
-	pr_debug("%s:\n", __func__);
-	if (!vendorDescr) return -1;
-	if (configNr >= RNDIS_MAX_CONFIGS) return -1;
-
-	rndis_per_dev_params[configNr].vendorID = vendorID;
-	rndis_per_dev_params[configNr].vendorDescr = vendorDescr;
-
-	return 0;
-}
-
-int rndis_set_param_medium(u8 configNr, u32 medium, u32 speed)
-{
-	pr_debug("%s: %u %u\n", __func__, medium, speed);
-	if (configNr >= RNDIS_MAX_CONFIGS) return -1;
-
-	rndis_per_dev_params[configNr].medium = medium;
-	rndis_per_dev_params[configNr].speed = speed;
-
-	return 0;
-}
-
-void rndis_add_hdr(struct sk_buff *skb)
-{
-	struct rndis_packet_msg_type *header;
-
-	if (!skb)
-		return;
-	header = (void *)skb_push(skb, sizeof(*header));
-	memset(header, 0, sizeof *header);
-	header->MessageType = cpu_to_le32(RNDIS_MSG_PACKET);
-	header->MessageLength = cpu_to_le32(skb->len);
-	header->DataOffset = cpu_to_le32(36);
-	header->DataLength = cpu_to_le32(skb->len - sizeof(*header));
-}
-
-void rndis_free_response(int configNr, u8 *buf)
-{
-	rndis_resp_t *r;
-	struct list_head *act, *tmp;
-
-	list_for_each_safe(act, tmp,
-			&(rndis_per_dev_params[configNr].resp_queue))
-	{
-		r = list_entry(act, rndis_resp_t, list);
-		if (r && r->buf == buf) {
-			list_del(&r->list);
-			kfree(r);
-		}
-	}
-}
-
-u8 *rndis_get_next_response(int configNr, u32 *length)
-{
-	rndis_resp_t *r;
-	struct list_head *act, *tmp;
-
-	if (!length) return NULL;
-
-	list_for_each_safe(act, tmp,
-			&(rndis_per_dev_params[configNr].resp_queue))
-	{
-		r = list_entry(act, rndis_resp_t, list);
-		if (!r->send) {
-			r->send = 1;
-			*length = r->length;
-			return r->buf;
-		}
-	}
-
-	return NULL;
-}
-
-static rndis_resp_t *rndis_add_response(int configNr, u32 length)
-{
-	rndis_resp_t *r;
-
-	/* NOTE: this gets copied into ether.c USB_BUFSIZ bytes ... */
-	r = kmalloc(sizeof(rndis_resp_t) + length, GFP_ATOMIC);
-	if (!r) return NULL;
-
-	r->buf = (u8 *)(r + 1);
-	r->length = length;
-	r->send = 0;
-
-	list_add_tail(&r->list,
-		&(rndis_per_dev_params[configNr].resp_queue));
-	return r;
-}
-
-int rndis_rm_hdr(struct gether *port,
-			struct sk_buff *skb,
-			struct sk_buff_head *list)
-{
-	/* tmp points to a struct rndis_packet_msg_type */
-	__le32 *tmp = (void *)skb->data;
-
-	/* MessageType, MessageLength */
-	if (cpu_to_le32(RNDIS_MSG_PACKET)
-			!= get_unaligned(tmp++)) {
-		dev_kfree_skb_any(skb);
-		return -EINVAL;
-	}
-	tmp++;
-
-	/* DataOffset, DataLength */
-	if (!skb_pull(skb, get_unaligned_le32(tmp++) + 8)) {
-		dev_kfree_skb_any(skb);
-		return -EOVERFLOW;
-	}
-	skb_trim(skb, get_unaligned_le32(tmp++));
-
-	skb_queue_tail(list, skb);
-	return 0;
-}
-
-#ifdef CONFIG_USB_GADGET_DEBUG_FILES
-
-static int rndis_proc_show(struct seq_file *m, void *v)
-{
-	rndis_params *param = m->private;
-
-	seq_printf(m,
-			 "Config Nr. %d\n"
-			 "used      : %s\n"
-			 "state     : %s\n"
-			 "medium    : 0x%08X\n"
-			 "speed     : %d\n"
-			 "cable     : %s\n"
-			 "vendor ID : 0x%08X\n"
-			 "vendor    : %s\n",
-			 param->confignr, (param->used) ? "y" : "n",
-			 ({ char *s = "?";
-			 switch (param->state) {
-			 case RNDIS_UNINITIALIZED:
-				s = "RNDIS_UNINITIALIZED"; break;
-			 case RNDIS_INITIALIZED:
-				s = "RNDIS_INITIALIZED"; break;
-			 case RNDIS_DATA_INITIALIZED:
-				s = "RNDIS_DATA_INITIALIZED"; break;
-			}; s; }),
-			 param->medium,
-			 (param->media_state) ? 0 : param->speed*100,
-			 (param->media_state) ? "disconnected" : "connected",
-			 param->vendorID, param->vendorDescr);
-	return 0;
-}
-
-static ssize_t rndis_proc_write(struct file *file, const char __user *buffer,
-				size_t count, loff_t *ppos)
-{
-	rndis_params *p = PDE(file_inode(file))->data;
-	u32 speed = 0;
-	int i, fl_speed = 0;
-
-	for (i = 0; i < count; i++) {
-		char c;
-		if (get_user(c, buffer))
-			return -EFAULT;
-		switch (c) {
-		case '0':
-		case '1':
-		case '2':
-		case '3':
-		case '4':
-		case '5':
-		case '6':
-		case '7':
-		case '8':
-		case '9':
-			fl_speed = 1;
-			speed = speed * 10 + c - '0';
-			break;
-		case 'C':
-		case 'c':
-			rndis_signal_connect(p->confignr);
-			break;
-		case 'D':
-		case 'd':
-			rndis_signal_disconnect(p->confignr);
-			break;
-		default:
-			if (fl_speed) p->speed = speed;
-			else pr_debug("%c is not valid\n", c);
-			break;
-		}
-
-		buffer++;
-	}
-
-	return count;
-}
-
-static int rndis_proc_open(struct inode *inode, struct file *file)
-{
-	return single_open(file, rndis_proc_show, PDE(inode)->data);
-}
-
-static const struct file_operations rndis_proc_fops = {
-	.owner		= THIS_MODULE,
-	.open		= rndis_proc_open,
-	.read		= seq_read,
-	.llseek		= seq_lseek,
-	.release	= single_release,
-	.write		= rndis_proc_write,
-};
-
-#define	NAME_TEMPLATE "driver/rndis-%03d"
-
-static struct proc_dir_entry *rndis_connect_state [RNDIS_MAX_CONFIGS];
-
-#endif /* CONFIG_USB_GADGET_DEBUG_FILES */
-
-
-int rndis_init(void)
-{
-	u8 i;
-
-	for (i = 0; i < RNDIS_MAX_CONFIGS; i++) {
-#ifdef	CONFIG_USB_GADGET_DEBUG_FILES
-		char name [20];
-
-		sprintf(name, NAME_TEMPLATE, i);
-		rndis_connect_state[i] = proc_create_data(name, 0660, NULL,
-					&rndis_proc_fops,
-					(void *)(rndis_per_dev_params + i));
-		if (!rndis_connect_state[i]) {
-			pr_debug("%s: remove entries", __func__);
-			while (i) {
-				sprintf(name, NAME_TEMPLATE, --i);
-				remove_proc_entry(name, NULL);
-			}
-			pr_debug("\n");
-			return -EIO;
-		}
-#endif
-		rndis_per_dev_params[i].confignr = i;
-		rndis_per_dev_params[i].used = 0;
-		rndis_per_dev_params[i].state = RNDIS_UNINITIALIZED;
-		rndis_per_dev_params[i].media_state
-				= RNDIS_MEDIA_STATE_DISCONNECTED;
-		INIT_LIST_HEAD(&(rndis_per_dev_params[i].resp_queue));
-	}
-
-	return 0;
-}
-
-void rndis_exit(void)
-{
-#ifdef CONFIG_USB_GADGET_DEBUG_FILES
-	u8 i;
-	char name[20];
-
-	for (i = 0; i < RNDIS_MAX_CONFIGS; i++) {
-		sprintf(name, NAME_TEMPLATE, i);
-		remove_proc_entry(name, NULL);
-	}
-#endif
-}
diff --git a/drivers/staging/ccg/rndis.h b/drivers/staging/ccg/rndis.h
deleted file mode 100644
index 0647f2f34e8..00000000000
--- a/drivers/staging/ccg/rndis.h
+++ /dev/null
@@ -1,222 +0,0 @@
-/*
- * RNDIS	Definitions for Remote NDIS
- *
- * Authors:	Benedikt Spranger, Pengutronix
- *		Robert Schwebel, Pengutronix
- *
- *		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 software was originally developed in conformance with
- *		Microsoft's Remote NDIS Specification License Agreement.
- */
-
-#ifndef _LINUX_RNDIS_H
-#define _LINUX_RNDIS_H
-
-#include <linux/rndis.h>
-#include "ndis.h"
-
-#define RNDIS_MAXIMUM_FRAME_SIZE	1518
-#define RNDIS_MAX_TOTAL_SIZE		1558
-
-typedef struct rndis_init_msg_type
-{
-	__le32	MessageType;
-	__le32	MessageLength;
-	__le32	RequestID;
-	__le32	MajorVersion;
-	__le32	MinorVersion;
-	__le32	MaxTransferSize;
-} rndis_init_msg_type;
-
-typedef struct rndis_init_cmplt_type
-{
-	__le32	MessageType;
-	__le32	MessageLength;
-	__le32	RequestID;
-	__le32	Status;
-	__le32	MajorVersion;
-	__le32	MinorVersion;
-	__le32	DeviceFlags;
-	__le32	Medium;
-	__le32	MaxPacketsPerTransfer;
-	__le32	MaxTransferSize;
-	__le32	PacketAlignmentFactor;
-	__le32	AFListOffset;
-	__le32	AFListSize;
-} rndis_init_cmplt_type;
-
-typedef struct rndis_halt_msg_type
-{
-	__le32	MessageType;
-	__le32	MessageLength;
-	__le32	RequestID;
-} rndis_halt_msg_type;
-
-typedef struct rndis_query_msg_type
-{
-	__le32	MessageType;
-	__le32	MessageLength;
-	__le32	RequestID;
-	__le32	OID;
-	__le32	InformationBufferLength;
-	__le32	InformationBufferOffset;
-	__le32	DeviceVcHandle;
-} rndis_query_msg_type;
-
-typedef struct rndis_query_cmplt_type
-{
-	__le32	MessageType;
-	__le32	MessageLength;
-	__le32	RequestID;
-	__le32	Status;
-	__le32	InformationBufferLength;
-	__le32	InformationBufferOffset;
-} rndis_query_cmplt_type;
-
-typedef struct rndis_set_msg_type
-{
-	__le32	MessageType;
-	__le32	MessageLength;
-	__le32	RequestID;
-	__le32	OID;
-	__le32	InformationBufferLength;
-	__le32	InformationBufferOffset;
-	__le32	DeviceVcHandle;
-} rndis_set_msg_type;
-
-typedef struct rndis_set_cmplt_type
-{
-	__le32	MessageType;
-	__le32	MessageLength;
-	__le32	RequestID;
-	__le32	Status;
-} rndis_set_cmplt_type;
-
-typedef struct rndis_reset_msg_type
-{
-	__le32	MessageType;
-	__le32	MessageLength;
-	__le32	Reserved;
-} rndis_reset_msg_type;
-
-typedef struct rndis_reset_cmplt_type
-{
-	__le32	MessageType;
-	__le32	MessageLength;
-	__le32	Status;
-	__le32	AddressingReset;
-} rndis_reset_cmplt_type;
-
-typedef struct rndis_indicate_status_msg_type
-{
-	__le32	MessageType;
-	__le32	MessageLength;
-	__le32	Status;
-	__le32	StatusBufferLength;
-	__le32	StatusBufferOffset;
-} rndis_indicate_status_msg_type;
-
-typedef struct rndis_keepalive_msg_type
-{
-	__le32	MessageType;
-	__le32	MessageLength;
-	__le32	RequestID;
-} rndis_keepalive_msg_type;
-
-typedef struct rndis_keepalive_cmplt_type
-{
-	__le32	MessageType;
-	__le32	MessageLength;
-	__le32	RequestID;
-	__le32	Status;
-} rndis_keepalive_cmplt_type;
-
-struct rndis_packet_msg_type
-{
-	__le32	MessageType;
-	__le32	MessageLength;
-	__le32	DataOffset;
-	__le32	DataLength;
-	__le32	OOBDataOffset;
-	__le32	OOBDataLength;
-	__le32	NumOOBDataElements;
-	__le32	PerPacketInfoOffset;
-	__le32	PerPacketInfoLength;
-	__le32	VcHandle;
-	__le32	Reserved;
-} __attribute__ ((packed));
-
-struct rndis_config_parameter
-{
-	__le32	ParameterNameOffset;
-	__le32	ParameterNameLength;
-	__le32	ParameterType;
-	__le32	ParameterValueOffset;
-	__le32	ParameterValueLength;
-};
-
-/* implementation specific */
-enum rndis_state
-{
-	RNDIS_UNINITIALIZED,
-	RNDIS_INITIALIZED,
-	RNDIS_DATA_INITIALIZED,
-};
-
-typedef struct rndis_resp_t
-{
-	struct list_head	list;
-	u8			*buf;
-	u32			length;
-	int			send;
-} rndis_resp_t;
-
-typedef struct rndis_params
-{
-	u8			confignr;
-	u8			used;
-	u16			saved_filter;
-	enum rndis_state	state;
-	u32			medium;
-	u32			speed;
-	u32			media_state;
-
-	const u8		*host_mac;
-	u16			*filter;
-	struct net_device	*dev;
-
-	u32			vendorID;
-	const char		*vendorDescr;
-	void			(*resp_avail)(void *v);
-	void			*v;
-	struct list_head	resp_queue;
-} rndis_params;
-
-/* RNDIS Message parser and other useless functions */
-int  rndis_msg_parser (u8 configNr, u8 *buf);
-int  rndis_register(void (*resp_avail)(void *v), void *v);
-void rndis_deregister (int configNr);
-int  rndis_set_param_dev (u8 configNr, struct net_device *dev,
-			 u16 *cdc_filter);
-int  rndis_set_param_vendor (u8 configNr, u32 vendorID,
-			    const char *vendorDescr);
-int  rndis_set_param_medium (u8 configNr, u32 medium, u32 speed);
-void rndis_add_hdr (struct sk_buff *skb);
-int rndis_rm_hdr(struct gether *port, struct sk_buff *skb,
-			struct sk_buff_head *list);
-u8   *rndis_get_next_response (int configNr, u32 *length);
-void rndis_free_response (int configNr, u8 *buf);
-
-void rndis_uninit (int configNr);
-int  rndis_signal_connect (int configNr);
-int  rndis_signal_disconnect (int configNr);
-int  rndis_state (int configNr);
-extern void rndis_set_host_mac (int configNr, const u8 *addr);
-
-int rndis_init(void);
-void rndis_exit (void);
-
-#endif  /* _LINUX_RNDIS_H */
diff --git a/drivers/staging/ccg/storage_common.c b/drivers/staging/ccg/storage_common.c
deleted file mode 100644
index abb01ac74ce..00000000000
--- a/drivers/staging/ccg/storage_common.c
+++ /dev/null
@@ -1,893 +0,0 @@
-/*
- * storage_common.c -- Common definitions for mass storage functionality
- *
- * Copyright (C) 2003-2008 Alan Stern
- * Copyeight (C) 2009 Samsung Electronics
- * Author: Michal Nazarewicz (mina86@mina86.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 file requires the following identifiers used in USB strings to
- * be defined (each of type pointer to char):
- *  - fsg_string_manufacturer -- name of the manufacturer
- *  - fsg_string_product      -- name of the product
- *  - fsg_string_config       -- name of the configuration
- *  - fsg_string_interface    -- name of the interface
- * The first four are only needed when FSG_DESCRIPTORS_DEVICE_STRINGS
- * macro is defined prior to including this file.
- */
-
-/*
- * When FSG_NO_INTR_EP is defined fsg_fs_intr_in_desc and
- * fsg_hs_intr_in_desc objects as well as
- * FSG_FS_FUNCTION_PRE_EP_ENTRIES and FSG_HS_FUNCTION_PRE_EP_ENTRIES
- * macros are not defined.
- *
- * When FSG_NO_DEVICE_STRINGS is defined FSG_STRING_MANUFACTURER,
- * FSG_STRING_PRODUCT, FSG_STRING_SERIAL and FSG_STRING_CONFIG are not
- * defined (as well as corresponding entries in string tables are
- * missing) and FSG_STRING_INTERFACE has value of zero.
- *
- * When FSG_NO_OTG is defined fsg_otg_desc won't be defined.
- */
-
-/*
- * When USB_GADGET_DEBUG_FILES is defined the module param num_buffers
- * sets the number of pipeline buffers (length of the fsg_buffhd array).
- * The valid range of num_buffers is: num >= 2 && num <= 4.
- */
-
-
-#include <linux/usb/storage.h>
-#include <scsi/scsi.h>
-#include <asm/unaligned.h>
-
-
-/*
- * Thanks to NetChip Technologies for donating this product ID.
- *
- * DO NOT REUSE THESE IDs with any other driver!!  Ever!!
- * Instead:  allocate your own, using normal USB-IF procedures.
- */
-#define FSG_VENDOR_ID	0x0525	/* NetChip */
-#define FSG_PRODUCT_ID	0xa4a5	/* Linux-USB File-backed Storage Gadget */
-
-
-/*-------------------------------------------------------------------------*/
-
-
-#ifndef DEBUG
-#undef VERBOSE_DEBUG
-#undef DUMP_MSGS
-#endif /* !DEBUG */
-
-#ifdef VERBOSE_DEBUG
-#define VLDBG	LDBG
-#else
-#define VLDBG(lun, fmt, args...) do { } while (0)
-#endif /* VERBOSE_DEBUG */
-
-#define LDBG(lun, fmt, args...)   dev_dbg (&(lun)->dev, fmt, ## args)
-#define LERROR(lun, fmt, args...) dev_err (&(lun)->dev, fmt, ## args)
-#define LWARN(lun, fmt, args...)  dev_warn(&(lun)->dev, fmt, ## args)
-#define LINFO(lun, fmt, args...)  dev_info(&(lun)->dev, fmt, ## args)
-
-/*
- * Keep those macros in sync with those in
- * include/linux/usb/composite.h or else GCC will complain.  If they
- * are identical (the same names of arguments, white spaces in the
- * same places) GCC will allow redefinition otherwise (even if some
- * white space is removed or added) warning will be issued.
- *
- * Those macros are needed here because File Storage Gadget does not
- * include the composite.h header.  For composite gadgets those macros
- * are redundant since composite.h is included any way.
- *
- * One could check whether those macros are already defined (which
- * would indicate composite.h had been included) or not (which would
- * indicate we were in FSG) but this is not done because a warning is
- * desired if definitions here differ from the ones in composite.h.
- *
- * We want the definitions to match and be the same in File Storage
- * Gadget as well as Mass Storage Function (and so composite gadgets
- * using MSF).  If someone changes them in composite.h it will produce
- * a warning in this file when building MSF.
- */
-#define DBG(d, fmt, args...)     dev_dbg(&(d)->gadget->dev , fmt , ## args)
-#define VDBG(d, fmt, args...)    dev_vdbg(&(d)->gadget->dev , fmt , ## args)
-#define ERROR(d, fmt, args...)   dev_err(&(d)->gadget->dev , fmt , ## args)
-#define WARNING(d, fmt, args...) dev_warn(&(d)->gadget->dev , fmt , ## args)
-#define INFO(d, fmt, args...)    dev_info(&(d)->gadget->dev , fmt , ## args)
-
-
-
-#ifdef DUMP_MSGS
-
-#  define dump_msg(fsg, /* const char * */ label,			\
-		   /* const u8 * */ buf, /* unsigned */ length) do {	\
-	if (length < 512) {						\
-		DBG(fsg, "%s, length %u:\n", label, length);		\
-		print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_OFFSET,	\
-			       16, 1, buf, length, 0);			\
-	}								\
-} while (0)
-
-#  define dump_cdb(fsg) do { } while (0)
-
-#else
-
-#  define dump_msg(fsg, /* const char * */ label, \
-		   /* const u8 * */ buf, /* unsigned */ length) do { } while (0)
-
-#  ifdef VERBOSE_DEBUG
-
-#    define dump_cdb(fsg)						\
-	print_hex_dump(KERN_DEBUG, "SCSI CDB: ", DUMP_PREFIX_NONE,	\
-		       16, 1, (fsg)->cmnd, (fsg)->cmnd_size, 0)		\
-
-#  else
-
-#    define dump_cdb(fsg) do { } while (0)
-
-#  endif /* VERBOSE_DEBUG */
-
-#endif /* DUMP_MSGS */
-
-/*-------------------------------------------------------------------------*/
-
-/* CBI Interrupt data structure */
-struct interrupt_data {
-	u8	bType;
-	u8	bValue;
-};
-
-#define CBI_INTERRUPT_DATA_LEN		2
-
-/* CBI Accept Device-Specific Command request */
-#define USB_CBI_ADSC_REQUEST		0x00
-
-
-/* Length of a SCSI Command Data Block */
-#define MAX_COMMAND_SIZE	16
-
-/* SCSI Sense Key/Additional Sense Code/ASC Qualifier values */
-#define SS_NO_SENSE				0
-#define SS_COMMUNICATION_FAILURE		0x040800
-#define SS_INVALID_COMMAND			0x052000
-#define SS_INVALID_FIELD_IN_CDB			0x052400
-#define SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE	0x052100
-#define SS_LOGICAL_UNIT_NOT_SUPPORTED		0x052500
-#define SS_MEDIUM_NOT_PRESENT			0x023a00
-#define SS_MEDIUM_REMOVAL_PREVENTED		0x055302
-#define SS_NOT_READY_TO_READY_TRANSITION	0x062800
-#define SS_RESET_OCCURRED			0x062900
-#define SS_SAVING_PARAMETERS_NOT_SUPPORTED	0x053900
-#define SS_UNRECOVERED_READ_ERROR		0x031100
-#define SS_WRITE_ERROR				0x030c02
-#define SS_WRITE_PROTECTED			0x072700
-
-#define SK(x)		((u8) ((x) >> 16))	/* Sense Key byte, etc. */
-#define ASC(x)		((u8) ((x) >> 8))
-#define ASCQ(x)		((u8) (x))
-
-
-/*-------------------------------------------------------------------------*/
-
-
-struct fsg_lun {
-	struct file	*filp;
-	loff_t		file_length;
-	loff_t		num_sectors;
-
-	unsigned int	initially_ro:1;
-	unsigned int	ro:1;
-	unsigned int	removable:1;
-	unsigned int	cdrom:1;
-	unsigned int	prevent_medium_removal:1;
-	unsigned int	registered:1;
-	unsigned int	info_valid:1;
-	unsigned int	nofua:1;
-
-	u32		sense_data;
-	u32		sense_data_info;
-	u32		unit_attention_data;
-
-	unsigned int	blkbits;	/* Bits of logical block size of bound block device */
-	unsigned int	blksize;	/* logical block size of bound block device */
-	struct device	dev;
-};
-
-#define fsg_lun_is_open(curlun)	((curlun)->filp != NULL)
-
-static struct fsg_lun *fsg_lun_from_dev(struct device *dev)
-{
-	return container_of(dev, struct fsg_lun, dev);
-}
-
-
-/* Big enough to hold our biggest descriptor */
-#define EP0_BUFSIZE	256
-#define DELAYED_STATUS	(EP0_BUFSIZE + 999)	/* An impossibly large value */
-
-#ifdef CONFIG_USB_GADGET_DEBUG_FILES
-
-static unsigned int fsg_num_buffers = CONFIG_USB_GADGET_STORAGE_NUM_BUFFERS;
-module_param_named(num_buffers, fsg_num_buffers, uint, S_IRUGO);
-MODULE_PARM_DESC(num_buffers, "Number of pipeline buffers");
-
-#else
-
-/*
- * Number of buffers we will use.
- * 2 is usually enough for good buffering pipeline
- */
-#define fsg_num_buffers	CONFIG_USB_GADGET_STORAGE_NUM_BUFFERS
-
-#endif /* CONFIG_USB_DEBUG */
-
-/* check if fsg_num_buffers is within a valid range */
-static inline int fsg_num_buffers_validate(void)
-{
-	if (fsg_num_buffers >= 2 && fsg_num_buffers <= 4)
-		return 0;
-	pr_err("fsg_num_buffers %u is out of range (%d to %d)\n",
-	       fsg_num_buffers, 2 ,4);
-	return -EINVAL;
-}
-
-/* Default size of buffer length. */
-#define FSG_BUFLEN	((u32)16384)
-
-/* Maximal number of LUNs supported in mass storage function */
-#define FSG_MAX_LUNS	8
-
-enum fsg_buffer_state {
-	BUF_STATE_EMPTY = 0,
-	BUF_STATE_FULL,
-	BUF_STATE_BUSY
-};
-
-struct fsg_buffhd {
-	void				*buf;
-	enum fsg_buffer_state		state;
-	struct fsg_buffhd		*next;
-
-	/*
-	 * The NetChip 2280 is faster, and handles some protocol faults
-	 * better, if we don't submit any short bulk-out read requests.
-	 * So we will record the intended request length here.
-	 */
-	unsigned int			bulk_out_intended_length;
-
-	struct usb_request		*inreq;
-	int				inreq_busy;
-	struct usb_request		*outreq;
-	int				outreq_busy;
-};
-
-enum fsg_state {
-	/* This one isn't used anywhere */
-	FSG_STATE_COMMAND_PHASE = -10,
-	FSG_STATE_DATA_PHASE,
-	FSG_STATE_STATUS_PHASE,
-
-	FSG_STATE_IDLE = 0,
-	FSG_STATE_ABORT_BULK_OUT,
-	FSG_STATE_RESET,
-	FSG_STATE_INTERFACE_CHANGE,
-	FSG_STATE_CONFIG_CHANGE,
-	FSG_STATE_DISCONNECT,
-	FSG_STATE_EXIT,
-	FSG_STATE_TERMINATED
-};
-
-enum data_direction {
-	DATA_DIR_UNKNOWN = 0,
-	DATA_DIR_FROM_HOST,
-	DATA_DIR_TO_HOST,
-	DATA_DIR_NONE
-};
-
-
-/*-------------------------------------------------------------------------*/
-
-
-static inline u32 get_unaligned_be24(u8 *buf)
-{
-	return 0xffffff & (u32) get_unaligned_be32(buf - 1);
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-
-enum {
-#ifndef FSG_NO_DEVICE_STRINGS
-	FSG_STRING_MANUFACTURER	= 1,
-	FSG_STRING_PRODUCT,
-	FSG_STRING_SERIAL,
-	FSG_STRING_CONFIG,
-#endif
-	FSG_STRING_INTERFACE
-};
-
-
-#ifndef FSG_NO_OTG
-static struct usb_otg_descriptor
-fsg_otg_desc = {
-	.bLength =		sizeof fsg_otg_desc,
-	.bDescriptorType =	USB_DT_OTG,
-
-	.bmAttributes =		USB_OTG_SRP,
-};
-#endif
-
-/* There is only one interface. */
-
-static struct usb_interface_descriptor
-fsg_intf_desc = {
-	.bLength =		sizeof fsg_intf_desc,
-	.bDescriptorType =	USB_DT_INTERFACE,
-
-	.bNumEndpoints =	2,		/* Adjusted during fsg_bind() */
-	.bInterfaceClass =	USB_CLASS_MASS_STORAGE,
-	.bInterfaceSubClass =	USB_SC_SCSI,	/* Adjusted during fsg_bind() */
-	.bInterfaceProtocol =	USB_PR_BULK,	/* Adjusted during fsg_bind() */
-	.iInterface =		FSG_STRING_INTERFACE,
-};
-
-/*
- * Three full-speed endpoint descriptors: bulk-in, bulk-out, and
- * interrupt-in.
- */
-
-static struct usb_endpoint_descriptor
-fsg_fs_bulk_in_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-
-	.bEndpointAddress =	USB_DIR_IN,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-	/* wMaxPacketSize set by autoconfiguration */
-};
-
-static struct usb_endpoint_descriptor
-fsg_fs_bulk_out_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-
-	.bEndpointAddress =	USB_DIR_OUT,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-	/* wMaxPacketSize set by autoconfiguration */
-};
-
-#ifndef FSG_NO_INTR_EP
-
-static struct usb_endpoint_descriptor
-fsg_fs_intr_in_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-
-	.bEndpointAddress =	USB_DIR_IN,
-	.bmAttributes =		USB_ENDPOINT_XFER_INT,
-	.wMaxPacketSize =	cpu_to_le16(2),
-	.bInterval =		32,	/* frames -> 32 ms */
-};
-
-#ifndef FSG_NO_OTG
-#  define FSG_FS_FUNCTION_PRE_EP_ENTRIES	2
-#else
-#  define FSG_FS_FUNCTION_PRE_EP_ENTRIES	1
-#endif
-
-#endif
-
-static struct usb_descriptor_header *fsg_fs_function[] = {
-#ifndef FSG_NO_OTG
-	(struct usb_descriptor_header *) &fsg_otg_desc,
-#endif
-	(struct usb_descriptor_header *) &fsg_intf_desc,
-	(struct usb_descriptor_header *) &fsg_fs_bulk_in_desc,
-	(struct usb_descriptor_header *) &fsg_fs_bulk_out_desc,
-#ifndef FSG_NO_INTR_EP
-	(struct usb_descriptor_header *) &fsg_fs_intr_in_desc,
-#endif
-	NULL,
-};
-
-
-/*
- * USB 2.0 devices need to expose both high speed and full speed
- * descriptors, unless they only run at full speed.
- *
- * That means alternate endpoint descriptors (bigger packets)
- * and a "device qualifier" ... plus more construction options
- * for the configuration descriptor.
- */
-static struct usb_endpoint_descriptor
-fsg_hs_bulk_in_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-
-	/* bEndpointAddress copied from fs_bulk_in_desc during fsg_bind() */
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-	.wMaxPacketSize =	cpu_to_le16(512),
-};
-
-static struct usb_endpoint_descriptor
-fsg_hs_bulk_out_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-
-	/* bEndpointAddress copied from fs_bulk_out_desc during fsg_bind() */
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-	.wMaxPacketSize =	cpu_to_le16(512),
-	.bInterval =		1,	/* NAK every 1 uframe */
-};
-
-#ifndef FSG_NO_INTR_EP
-
-static struct usb_endpoint_descriptor
-fsg_hs_intr_in_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-
-	/* bEndpointAddress copied from fs_intr_in_desc during fsg_bind() */
-	.bmAttributes =		USB_ENDPOINT_XFER_INT,
-	.wMaxPacketSize =	cpu_to_le16(2),
-	.bInterval =		9,	/* 2**(9-1) = 256 uframes -> 32 ms */
-};
-
-#ifndef FSG_NO_OTG
-#  define FSG_HS_FUNCTION_PRE_EP_ENTRIES	2
-#else
-#  define FSG_HS_FUNCTION_PRE_EP_ENTRIES	1
-#endif
-
-#endif
-
-static struct usb_descriptor_header *fsg_hs_function[] = {
-#ifndef FSG_NO_OTG
-	(struct usb_descriptor_header *) &fsg_otg_desc,
-#endif
-	(struct usb_descriptor_header *) &fsg_intf_desc,
-	(struct usb_descriptor_header *) &fsg_hs_bulk_in_desc,
-	(struct usb_descriptor_header *) &fsg_hs_bulk_out_desc,
-#ifndef FSG_NO_INTR_EP
-	(struct usb_descriptor_header *) &fsg_hs_intr_in_desc,
-#endif
-	NULL,
-};
-
-static struct usb_endpoint_descriptor
-fsg_ss_bulk_in_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-
-	/* bEndpointAddress copied from fs_bulk_in_desc during fsg_bind() */
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-	.wMaxPacketSize =	cpu_to_le16(1024),
-};
-
-static struct usb_ss_ep_comp_descriptor fsg_ss_bulk_in_comp_desc = {
-	.bLength =		sizeof(fsg_ss_bulk_in_comp_desc),
-	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
-
-	/*.bMaxBurst =		DYNAMIC, */
-};
-
-static struct usb_endpoint_descriptor
-fsg_ss_bulk_out_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-
-	/* bEndpointAddress copied from fs_bulk_out_desc during fsg_bind() */
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-	.wMaxPacketSize =	cpu_to_le16(1024),
-};
-
-static struct usb_ss_ep_comp_descriptor fsg_ss_bulk_out_comp_desc = {
-	.bLength =		sizeof(fsg_ss_bulk_in_comp_desc),
-	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
-
-	/*.bMaxBurst =		DYNAMIC, */
-};
-
-#ifndef FSG_NO_INTR_EP
-
-static struct usb_endpoint_descriptor
-fsg_ss_intr_in_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-
-	/* bEndpointAddress copied from fs_intr_in_desc during fsg_bind() */
-	.bmAttributes =		USB_ENDPOINT_XFER_INT,
-	.wMaxPacketSize =	cpu_to_le16(2),
-	.bInterval =		9,	/* 2**(9-1) = 256 uframes -> 32 ms */
-};
-
-static struct usb_ss_ep_comp_descriptor fsg_ss_intr_in_comp_desc = {
-	.bLength =		sizeof(fsg_ss_bulk_in_comp_desc),
-	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
-
-	.wBytesPerInterval =	cpu_to_le16(2),
-};
-
-#ifndef FSG_NO_OTG
-#  define FSG_SS_FUNCTION_PRE_EP_ENTRIES	2
-#else
-#  define FSG_SS_FUNCTION_PRE_EP_ENTRIES	1
-#endif
-
-#endif
-
-static __maybe_unused struct usb_ext_cap_descriptor fsg_ext_cap_desc = {
-	.bLength =		USB_DT_USB_EXT_CAP_SIZE,
-	.bDescriptorType =	USB_DT_DEVICE_CAPABILITY,
-	.bDevCapabilityType =	USB_CAP_TYPE_EXT,
-
-	.bmAttributes =		cpu_to_le32(USB_LPM_SUPPORT),
-};
-
-static __maybe_unused struct usb_ss_cap_descriptor fsg_ss_cap_desc = {
-	.bLength =		USB_DT_USB_SS_CAP_SIZE,
-	.bDescriptorType =	USB_DT_DEVICE_CAPABILITY,
-	.bDevCapabilityType =	USB_SS_CAP_TYPE,
-
-	/* .bmAttributes = LTM is not supported yet */
-
-	.wSpeedSupported =	cpu_to_le16(USB_LOW_SPEED_OPERATION
-		| USB_FULL_SPEED_OPERATION
-		| USB_HIGH_SPEED_OPERATION
-		| USB_5GBPS_OPERATION),
-	.bFunctionalitySupport = USB_LOW_SPEED_OPERATION,
-	.bU1devExitLat =	USB_DEFAULT_U1_DEV_EXIT_LAT,
-	.bU2DevExitLat =	cpu_to_le16(USB_DEFAULT_U2_DEV_EXIT_LAT),
-};
-
-static __maybe_unused struct usb_bos_descriptor fsg_bos_desc = {
-	.bLength =		USB_DT_BOS_SIZE,
-	.bDescriptorType =	USB_DT_BOS,
-
-	.wTotalLength =		cpu_to_le16(USB_DT_BOS_SIZE
-				+ USB_DT_USB_EXT_CAP_SIZE
-				+ USB_DT_USB_SS_CAP_SIZE),
-
-	.bNumDeviceCaps =	2,
-};
-
-static struct usb_descriptor_header *fsg_ss_function[] = {
-#ifndef FSG_NO_OTG
-	(struct usb_descriptor_header *) &fsg_otg_desc,
-#endif
-	(struct usb_descriptor_header *) &fsg_intf_desc,
-	(struct usb_descriptor_header *) &fsg_ss_bulk_in_desc,
-	(struct usb_descriptor_header *) &fsg_ss_bulk_in_comp_desc,
-	(struct usb_descriptor_header *) &fsg_ss_bulk_out_desc,
-	(struct usb_descriptor_header *) &fsg_ss_bulk_out_comp_desc,
-#ifndef FSG_NO_INTR_EP
-	(struct usb_descriptor_header *) &fsg_ss_intr_in_desc,
-	(struct usb_descriptor_header *) &fsg_ss_intr_in_comp_desc,
-#endif
-	NULL,
-};
-
-/* Maxpacket and other transfer characteristics vary by speed. */
-static __maybe_unused struct usb_endpoint_descriptor *
-fsg_ep_desc(struct usb_gadget *g, struct usb_endpoint_descriptor *fs,
-		struct usb_endpoint_descriptor *hs,
-		struct usb_endpoint_descriptor *ss)
-{
-	if (gadget_is_superspeed(g) && g->speed == USB_SPEED_SUPER)
-		return ss;
-	else if (gadget_is_dualspeed(g) && g->speed == USB_SPEED_HIGH)
-		return hs;
-	return fs;
-}
-
-
-/* Static strings, in UTF-8 (for simplicity we use only ASCII characters) */
-static struct usb_string		fsg_strings[] = {
-#ifndef FSG_NO_DEVICE_STRINGS
-	{FSG_STRING_MANUFACTURER,	fsg_string_manufacturer},
-	{FSG_STRING_PRODUCT,		fsg_string_product},
-	{FSG_STRING_SERIAL,		""},
-	{FSG_STRING_CONFIG,		fsg_string_config},
-#endif
-	{FSG_STRING_INTERFACE,		fsg_string_interface},
-	{}
-};
-
-static struct usb_gadget_strings	fsg_stringtab = {
-	.language	= 0x0409,		/* en-us */
-	.strings	= fsg_strings,
-};
-
-
- /*-------------------------------------------------------------------------*/
-
-/*
- * If the next two routines are called while the gadget is registered,
- * the caller must own fsg->filesem for writing.
- */
-
-static void fsg_lun_close(struct fsg_lun *curlun)
-{
-	if (curlun->filp) {
-		LDBG(curlun, "close backing file\n");
-		fput(curlun->filp);
-		curlun->filp = NULL;
-	}
-}
-
-
-static int fsg_lun_open(struct fsg_lun *curlun, const char *filename)
-{
-	int				ro;
-	struct file			*filp = NULL;
-	int				rc = -EINVAL;
-	struct inode			*inode = NULL;
-	loff_t				size;
-	loff_t				num_sectors;
-	loff_t				min_sectors;
-	unsigned int			blkbits;
-	unsigned int			blksize;
-
-	/* R/W if we can, R/O if we must */
-	ro = curlun->initially_ro;
-	if (!ro) {
-		filp = filp_open(filename, O_RDWR | O_LARGEFILE, 0);
-		if (PTR_ERR(filp) == -EROFS || PTR_ERR(filp) == -EACCES)
-			ro = 1;
-	}
-	if (ro)
-		filp = filp_open(filename, O_RDONLY | O_LARGEFILE, 0);
-	if (IS_ERR(filp)) {
-		LINFO(curlun, "unable to open backing file: %s\n", filename);
-		return PTR_ERR(filp);
-	}
-
-	if (!(filp->f_mode & FMODE_WRITE))
-		ro = 1;
-
-	inode = file_inode(filp);
-	if ((!S_ISREG(inode->i_mode) && !S_ISBLK(inode->i_mode))) {
-		LINFO(curlun, "invalid file type: %s\n", filename);
-		goto out;
-	}
-
-	/*
-	 * If we can't read the file, it's no good.
-	 * If we can't write the file, use it read-only.
-	 */
-	if (!(filp->f_op->read || filp->f_op->aio_read)) {
-		LINFO(curlun, "file not readable: %s\n", filename);
-		goto out;
-	}
-	if (!(filp->f_op->write || filp->f_op->aio_write))
-		ro = 1;
-
-	size = i_size_read(inode->i_mapping->host);
-	if (size < 0) {
-		LINFO(curlun, "unable to find file size: %s\n", filename);
-		rc = (int) size;
-		goto out;
-	}
-
-	if (curlun->cdrom) {
-		blksize = 2048;
-		blkbits = 11;
-	} else if (inode->i_bdev) {
-		blksize = bdev_logical_block_size(inode->i_bdev);
-		blkbits = blksize_bits(blksize);
-	} else {
-		blksize = 512;
-		blkbits = 9;
-	}
-
-	num_sectors = size >> blkbits; /* File size in logic-block-size blocks */
-	min_sectors = 1;
-	if (curlun->cdrom) {
-		min_sectors = 300;	/* Smallest track is 300 frames */
-		if (num_sectors >= 256*60*75) {
-			num_sectors = 256*60*75 - 1;
-			LINFO(curlun, "file too big: %s\n", filename);
-			LINFO(curlun, "using only first %d blocks\n",
-					(int) num_sectors);
-		}
-	}
-	if (num_sectors < min_sectors) {
-		LINFO(curlun, "file too small: %s\n", filename);
-		rc = -ETOOSMALL;
-		goto out;
-	}
-
-	if (fsg_lun_is_open(curlun))
-		fsg_lun_close(curlun);
-
-	curlun->blksize = blksize;
-	curlun->blkbits = blkbits;
-	curlun->ro = ro;
-	curlun->filp = filp;
-	curlun->file_length = size;
-	curlun->num_sectors = num_sectors;
-	LDBG(curlun, "open backing file: %s\n", filename);
-	return 0;
-
-out:
-	fput(filp);
-	return rc;
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-/*
- * Sync the file data, don't bother with the metadata.
- * This code was copied from fs/buffer.c:sys_fdatasync().
- */
-static int fsg_lun_fsync_sub(struct fsg_lun *curlun)
-{
-	struct file	*filp = curlun->filp;
-
-	if (curlun->ro || !filp)
-		return 0;
-	return vfs_fsync(filp, 1);
-}
-
-static void store_cdrom_address(u8 *dest, int msf, u32 addr)
-{
-	if (msf) {
-		/* Convert to Minutes-Seconds-Frames */
-		addr >>= 2;		/* Convert to 2048-byte frames */
-		addr += 2*75;		/* Lead-in occupies 2 seconds */
-		dest[3] = addr % 75;	/* Frames */
-		addr /= 75;
-		dest[2] = addr % 60;	/* Seconds */
-		addr /= 60;
-		dest[1] = addr;		/* Minutes */
-		dest[0] = 0;		/* Reserved */
-	} else {
-		/* Absolute sector */
-		put_unaligned_be32(addr, dest);
-	}
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-
-static ssize_t fsg_show_ro(struct device *dev, struct device_attribute *attr,
-			   char *buf)
-{
-	struct fsg_lun	*curlun = fsg_lun_from_dev(dev);
-
-	return sprintf(buf, "%d\n", fsg_lun_is_open(curlun)
-				  ? curlun->ro
-				  : curlun->initially_ro);
-}
-
-static ssize_t fsg_show_nofua(struct device *dev, struct device_attribute *attr,
-			      char *buf)
-{
-	struct fsg_lun	*curlun = fsg_lun_from_dev(dev);
-
-	return sprintf(buf, "%u\n", curlun->nofua);
-}
-
-static ssize_t fsg_show_file(struct device *dev, struct device_attribute *attr,
-			     char *buf)
-{
-	struct fsg_lun	*curlun = fsg_lun_from_dev(dev);
-	struct rw_semaphore	*filesem = dev_get_drvdata(dev);
-	char		*p;
-	ssize_t		rc;
-
-	down_read(filesem);
-	if (fsg_lun_is_open(curlun)) {	/* Get the complete pathname */
-		p = d_path(&curlun->filp->f_path, buf, PAGE_SIZE - 1);
-		if (IS_ERR(p))
-			rc = PTR_ERR(p);
-		else {
-			rc = strlen(p);
-			memmove(buf, p, rc);
-			buf[rc] = '\n';		/* Add a newline */
-			buf[++rc] = 0;
-		}
-	} else {				/* No file, return 0 bytes */
-		*buf = 0;
-		rc = 0;
-	}
-	up_read(filesem);
-	return rc;
-}
-
-
-static ssize_t fsg_store_ro(struct device *dev, struct device_attribute *attr,
-			    const char *buf, size_t count)
-{
-	ssize_t		rc;
-	struct fsg_lun	*curlun = fsg_lun_from_dev(dev);
-	struct rw_semaphore	*filesem = dev_get_drvdata(dev);
-	unsigned	ro;
-
-	rc = kstrtouint(buf, 2, &ro);
-	if (rc)
-		return rc;
-
-	/*
-	 * Allow the write-enable status to change only while the
-	 * backing file is closed.
-	 */
-	down_read(filesem);
-	if (fsg_lun_is_open(curlun)) {
-		LDBG(curlun, "read-only status change prevented\n");
-		rc = -EBUSY;
-	} else {
-		curlun->ro = ro;
-		curlun->initially_ro = ro;
-		LDBG(curlun, "read-only status set to %d\n", curlun->ro);
-		rc = count;
-	}
-	up_read(filesem);
-	return rc;
-}
-
-static ssize_t fsg_store_nofua(struct device *dev,
-			       struct device_attribute *attr,
-			       const char *buf, size_t count)
-{
-	struct fsg_lun	*curlun = fsg_lun_from_dev(dev);
-	unsigned	nofua;
-	int		ret;
-
-	ret = kstrtouint(buf, 2, &nofua);
-	if (ret)
-		return ret;
-
-	/* Sync data when switching from async mode to sync */
-	if (!nofua && curlun->nofua)
-		fsg_lun_fsync_sub(curlun);
-
-	curlun->nofua = nofua;
-
-	return count;
-}
-
-static ssize_t fsg_store_file(struct device *dev, struct device_attribute *attr,
-			      const char *buf, size_t count)
-{
-	struct fsg_lun	*curlun = fsg_lun_from_dev(dev);
-	struct rw_semaphore	*filesem = dev_get_drvdata(dev);
-	int		rc = 0;
-
-	if (curlun->prevent_medium_removal && fsg_lun_is_open(curlun)) {
-		LDBG(curlun, "eject attempt prevented\n");
-		return -EBUSY;				/* "Door is locked" */
-	}
-
-	/* Remove a trailing newline */
-	if (count > 0 && buf[count-1] == '\n')
-		((char *) buf)[count-1] = 0;		/* Ugh! */
-
-	/* Load new medium */
-	down_write(filesem);
-	if (count > 0 && buf[0]) {
-		/* fsg_lun_open() will close existing file if any. */
-		rc = fsg_lun_open(curlun, buf);
-		if (rc == 0)
-			curlun->unit_attention_data =
-					SS_NOT_READY_TO_READY_TRANSITION;
-	} else if (fsg_lun_is_open(curlun)) {
-		fsg_lun_close(curlun);
-		curlun->unit_attention_data = SS_MEDIUM_NOT_PRESENT;
-	}
-	up_write(filesem);
-	return (rc < 0 ? rc : count);
-}
diff --git a/drivers/staging/ccg/sysfs-class-ccg_usb b/drivers/staging/ccg/sysfs-class-ccg_usb
deleted file mode 100644
index dd12a332fb0..00000000000
--- a/drivers/staging/ccg/sysfs-class-ccg_usb
+++ /dev/null
@@ -1,158 +0,0 @@
-What:		/sys/class/ccg_usb
-Date:		May 2012
-KernelVersion:	3.4
-Contact:	linux-usb@vger.kernel.org
-Description:
-		The ccg_usb/ class subdirectory belongs to ccg
-		USB gadget.
-
-What:		/sys/class/ccg_usb/ccgX
-Date:		May 2012
-KernelVersion:	3.4
-Contact:	linux-usb@vger.kernel.org
-Description:
-		The /sys/class/ccg_usb/ccg{0,1,2,3...} class
-		subdirectories correspond to each ccg gadget device;
-		at the time of this writing there is only ccg0 and it
-		represents the ccg gadget.
-
-What:		/sys/class/ccg_usb/ccgX/functions
-Date:		May 2012
-KernelVersion:	3.4
-Contact:	linux-usb@vger.kernel.org
-Description:
-		A comma-separated list of USB function names to be activated
-		in this ccg gadget. It includes both the functions provided
-		in-kernel by the ccg gadget and the functions provided from
-		userspace through FunctionFS.
-
-What:		/sys/class/ccg_usb/ccgX/enable
-Date:		May 2012
-KernelVersion:	3.4
-Contact:	linux-usb@vger.kernel.org
-Description:
-		A flag activating/deactivating the ccg usb gadget.
-
-What:		/sys/class/ccg_usb/ccgX/state
-Date:		May 2012
-KernelVersion:	3.4
-Contact:	linux-usb@vger.kernel.org
-Description:
-		Configurable usb gadget state:
-
-		DISCONNECTED
-		CONNECTED
-		CONFIGURED
-
-What:		/sys/class/ccg_usb/ccgX/f_acm/
-Date:		May 2012
-KernelVersion:	3.4
-Contact:	linux-usb@vger.kernel.org
-Description:
-		The /sys/class/ccg_usb/ccgX/f_acm subdirectory
-		corresponds to the gadget's USB CDC serial (ACM) function
-		driver.
-
-What:		/sys/class/ccg_usb/ccgX/f_acm/instances
-Date:		May 2012
-KernelVersion:	3.4
-Contact:	linux-usb@vger.kernel.org
-Description:
-		Maximum number of the /dev/ttyGS<X> interface the driver uses.
-
-What:		/sys/class/ccg_usb/ccgX/f_fs
-Date:		May 2012
-KernelVersion:	3.4
-Contact:	linux-usb@vger.kernel.org
-Description:
-		The /sys/class/ccg_usb/ccgX/f_fs subdirectory
-		corresponds to the gadget's FunctionFS driver.
-
-What:		/sys/class/ccg_usb/ccgX/f_fs/user_functions
-Date:		May 2012
-KernelVersion:	3.4
-Contact:	linux-usb@vger.kernel.org
-Description:
-		A comma-separeted list of USB function names to be supported
-		from userspace. No other userspace FunctionFS functions can
-		be supported than listed here. However, the actual activation
-		of these functions is still done through
-		/sys/class/ccg_usb/ccgX/functions, where it is possible
-		to specify any subset (including maximum and empty) of
-		/sys/class/ccg_usb/ccgX/f_fs/user_functions.
-
-What:		/sys/class/ccg_usb/ccgX/f_fs/max_user_functions
-Date:		May 2012
-KernelVersion:	3.4
-Contact:	linux-usb@vger.kernel.org
-Description:
-		Maximum number of USB functions to be supported from userspace.
-
-What:		/sys/class/ccg_usb/ccgX/f_rndis
-Date:		May 2012
-KernelVersion:	3.4
-Contact:	linux-usb@vger.kernel.org
-Description:
-		The /sys/class/ccg_usb/ccgX/f_rndis subdirectory
-		corresponds to the gadget's RNDIS driver.
-
-What:		/sys/class/ccg_usb/ccgX/f_rndis/manufacturer
-Date:		May 2012
-KernelVersion:	3.4
-Contact:	linux-usb@vger.kernel.org
-Description:
-		RNDIS Ethernet port manufacturer string.
-
-What:		/sys/class/ccg_usb/ccgX/f_rndis/wceis
-Date:		May 2012
-KernelVersion:	3.4
-Contact:	linux-usb@vger.kernel.org
-Description:
-		RNDIS Ethernet port wireless flag.
-
-What:		/sys/class/ccg_usb/ccgX/f_rndis/ethaddr
-Date:		May 2012
-KernelVersion:	3.4
-Contact:	linux-usb@vger.kernel.org
-Description:
-		RNDIS Ethernet port Ethernet address.
-
-What:		/sys/class/ccg_usb/ccgX/f_rndis/vendorID
-Date:		May 2012
-KernelVersion:	3.4
-Contact:	linux-usb@vger.kernel.org
-Description:
-		RNDIS Ethernet port vendor ID.
-
-What:		/sys/class/ccg_usb/ccgX/f_mass_storage
-Date:		May 2012
-KernelVersion:	3.4
-Contact:	linux-usb@vger.kernel.org
-Description:
-		The /sys/class/ccg_usb/ccgX/f_mass_storage subdirectory
-		corresponds to the gadget's USB mass storage driver.
-
-What:		/sys/class/ccg_usb/ccgX/f_mass_storage/lun
-Date:		May 2012
-KernelVersion:	3.4
-Contact:	linux-usb@vger.kernel.org
-Description:
-		The /sys/class/ccg_usb/ccgX/f_mass_storage/lun
-		subdirectory corresponds to the gadget's USB mass storage
-		driver and its underlying storage.
-
-What:		/sys/class/ccg_usb/ccgX/f_mass_storage/lun
-Date:		May 2012
-KernelVersion:	3.4
-Contact:	linux-usb@vger.kernel.org
-Description:
-		The /sys/class/ccg_usb/ccgX/f_mass_storage/lun
-		subdirectory corresponds to the gadget's USB mass storage
-		driver and its underlying storage.
-
-What:		/sys/class/ccg_usb/ccgX/f_mass_storage/lun/file
-Date:		May 2012
-KernelVersion:	3.4
-Contact:	linux-usb@vger.kernel.org
-Description:
-		Gadget's USB mass storage underlying file.
diff --git a/drivers/staging/ccg/u_ether.c b/drivers/staging/ccg/u_ether.c
deleted file mode 100644
index fed78865adc..00000000000
--- a/drivers/staging/ccg/u_ether.c
+++ /dev/null
@@ -1,986 +0,0 @@
-/*
- * u_ether.c -- Ethernet-over-USB link layer utilities for Gadget stack
- *
- * Copyright (C) 2003-2005,2008 David Brownell
- * Copyright (C) 2003-2004 Robert Schwebel, Benedikt Spranger
- * Copyright (C) 2008 Nokia Corporation
- *
- * 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.
- */
-
-/* #define VERBOSE_DEBUG */
-
-#include <linux/kernel.h>
-#include <linux/gfp.h>
-#include <linux/device.h>
-#include <linux/ctype.h>
-#include <linux/etherdevice.h>
-#include <linux/ethtool.h>
-
-#include "u_ether.h"
-
-
-/*
- * This component encapsulates the Ethernet link glue needed to provide
- * one (!) network link through the USB gadget stack, normally "usb0".
- *
- * The control and data models are handled by the function driver which
- * connects to this code; such as CDC Ethernet (ECM or EEM),
- * "CDC Subset", or RNDIS.  That includes all descriptor and endpoint
- * management.
- *
- * Link level addressing is handled by this component using module
- * parameters; if no such parameters are provided, random link level
- * addresses are used.  Each end of the link uses one address.  The
- * host end address is exported in various ways, and is often recorded
- * in configuration databases.
- *
- * The driver which assembles each configuration using such a link is
- * responsible for ensuring that each configuration includes at most one
- * instance of is network link.  (The network layer provides ways for
- * this single "physical" link to be used by multiple virtual links.)
- */
-
-#define UETH__VERSION	"29-May-2008"
-
-struct eth_dev {
-	/* lock is held while accessing port_usb
-	 * or updating its backlink port_usb->ioport
-	 */
-	spinlock_t		lock;
-	struct gether		*port_usb;
-
-	struct net_device	*net;
-	struct usb_gadget	*gadget;
-
-	spinlock_t		req_lock;	/* guard {rx,tx}_reqs */
-	struct list_head	tx_reqs, rx_reqs;
-	atomic_t		tx_qlen;
-
-	struct sk_buff_head	rx_frames;
-
-	unsigned		header_len;
-	struct sk_buff		*(*wrap)(struct gether *, struct sk_buff *skb);
-	int			(*unwrap)(struct gether *,
-						struct sk_buff *skb,
-						struct sk_buff_head *list);
-
-	struct work_struct	work;
-
-	unsigned long		todo;
-#define	WORK_RX_MEMORY		0
-
-	bool			zlp;
-	u8			host_mac[ETH_ALEN];
-};
-
-/*-------------------------------------------------------------------------*/
-
-#define RX_EXTRA	20	/* bytes guarding against rx overflows */
-
-#define DEFAULT_QLEN	2	/* double buffering by default */
-
-static unsigned qmult = 5;
-module_param(qmult, uint, S_IRUGO|S_IWUSR);
-MODULE_PARM_DESC(qmult, "queue length multiplier at high/super speed");
-
-/* for dual-speed hardware, use deeper queues at high/super speed */
-static inline int qlen(struct usb_gadget *gadget)
-{
-	if (gadget_is_dualspeed(gadget) && (gadget->speed == USB_SPEED_HIGH ||
-					    gadget->speed == USB_SPEED_SUPER))
-		return qmult * DEFAULT_QLEN;
-	else
-		return DEFAULT_QLEN;
-}
-
-/*-------------------------------------------------------------------------*/
-
-/* REVISIT there must be a better way than having two sets
- * of debug calls ...
- */
-
-#undef DBG
-#undef VDBG
-#undef ERROR
-#undef INFO
-
-#define xprintk(d, level, fmt, args...) \
-	printk(level "%s: " fmt , (d)->net->name , ## args)
-
-#ifdef DEBUG
-#undef DEBUG
-#define DBG(dev, fmt, args...) \
-	xprintk(dev , KERN_DEBUG , fmt , ## args)
-#else
-#define DBG(dev, fmt, args...) \
-	do { } while (0)
-#endif /* DEBUG */
-
-#ifdef VERBOSE_DEBUG
-#define VDBG	DBG
-#else
-#define VDBG(dev, fmt, args...) \
-	do { } while (0)
-#endif /* DEBUG */
-
-#define ERROR(dev, fmt, args...) \
-	xprintk(dev , KERN_ERR , fmt , ## args)
-#define INFO(dev, fmt, args...) \
-	xprintk(dev , KERN_INFO , fmt , ## args)
-
-/*-------------------------------------------------------------------------*/
-
-/* NETWORK DRIVER HOOKUP (to the layer above this driver) */
-
-static int ueth_change_mtu(struct net_device *net, int new_mtu)
-{
-	struct eth_dev	*dev = netdev_priv(net);
-	unsigned long	flags;
-	int		status = 0;
-
-	/* don't change MTU on "live" link (peer won't know) */
-	spin_lock_irqsave(&dev->lock, flags);
-	if (dev->port_usb)
-		status = -EBUSY;
-	else if (new_mtu <= ETH_HLEN || new_mtu > ETH_FRAME_LEN)
-		status = -ERANGE;
-	else
-		net->mtu = new_mtu;
-	spin_unlock_irqrestore(&dev->lock, flags);
-
-	return status;
-}
-
-static void eth_get_drvinfo(struct net_device *net, struct ethtool_drvinfo *p)
-{
-	struct eth_dev *dev = netdev_priv(net);
-
-	strlcpy(p->driver, "g_ether", sizeof(p->driver));
-	strlcpy(p->version, UETH__VERSION, sizeof(p->version));
-	strlcpy(p->fw_version, dev->gadget->name, sizeof(p->fw_version));
-	strlcpy(p->bus_info, dev_name(&dev->gadget->dev), sizeof(p->bus_info));
-}
-
-/* REVISIT can also support:
- *   - WOL (by tracking suspends and issuing remote wakeup)
- *   - msglevel (implies updated messaging)
- *   - ... probably more ethtool ops
- */
-
-static const struct ethtool_ops ops = {
-	.get_drvinfo = eth_get_drvinfo,
-	.get_link = ethtool_op_get_link,
-};
-
-static void defer_kevent(struct eth_dev *dev, int flag)
-{
-	if (test_and_set_bit(flag, &dev->todo))
-		return;
-	if (!schedule_work(&dev->work))
-		ERROR(dev, "kevent %d may have been dropped\n", flag);
-	else
-		DBG(dev, "kevent %d scheduled\n", flag);
-}
-
-static void rx_complete(struct usb_ep *ep, struct usb_request *req);
-
-static int
-rx_submit(struct eth_dev *dev, struct usb_request *req, gfp_t gfp_flags)
-{
-	struct sk_buff	*skb;
-	int		retval = -ENOMEM;
-	size_t		size = 0;
-	struct usb_ep	*out;
-	unsigned long	flags;
-
-	spin_lock_irqsave(&dev->lock, flags);
-	if (dev->port_usb)
-		out = dev->port_usb->out_ep;
-	else
-		out = NULL;
-	spin_unlock_irqrestore(&dev->lock, flags);
-
-	if (!out)
-		return -ENOTCONN;
-
-
-	/* Padding up to RX_EXTRA handles minor disagreements with host.
-	 * Normally we use the USB "terminate on short read" convention;
-	 * so allow up to (N*maxpacket), since that memory is normally
-	 * already allocated.  Some hardware doesn't deal well with short
-	 * reads (e.g. DMA must be N*maxpacket), so for now don't trim a
-	 * byte off the end (to force hardware errors on overflow).
-	 *
-	 * RNDIS uses internal framing, and explicitly allows senders to
-	 * pad to end-of-packet.  That's potentially nice for speed, but
-	 * means receivers can't recover lost synch on their own (because
-	 * new packets don't only start after a short RX).
-	 */
-	size += sizeof(struct ethhdr) + dev->net->mtu + RX_EXTRA;
-	size += dev->port_usb->header_len;
-	size += out->maxpacket - 1;
-	size -= size % out->maxpacket;
-
-	if (dev->port_usb->is_fixed)
-		size = max_t(size_t, size, dev->port_usb->fixed_out_len);
-
-	skb = alloc_skb(size + NET_IP_ALIGN, gfp_flags);
-	if (skb == NULL) {
-		DBG(dev, "no rx skb\n");
-		goto enomem;
-	}
-
-	/* Some platforms perform better when IP packets are aligned,
-	 * but on at least one, checksumming fails otherwise.  Note:
-	 * RNDIS headers involve variable numbers of LE32 values.
-	 */
-	skb_reserve(skb, NET_IP_ALIGN);
-
-	req->buf = skb->data;
-	req->length = size;
-	req->complete = rx_complete;
-	req->context = skb;
-
-	retval = usb_ep_queue(out, req, gfp_flags);
-	if (retval == -ENOMEM)
-enomem:
-		defer_kevent(dev, WORK_RX_MEMORY);
-	if (retval) {
-		DBG(dev, "rx submit --> %d\n", retval);
-		if (skb)
-			dev_kfree_skb_any(skb);
-		spin_lock_irqsave(&dev->req_lock, flags);
-		list_add(&req->list, &dev->rx_reqs);
-		spin_unlock_irqrestore(&dev->req_lock, flags);
-	}
-	return retval;
-}
-
-static void rx_complete(struct usb_ep *ep, struct usb_request *req)
-{
-	struct sk_buff	*skb = req->context, *skb2;
-	struct eth_dev	*dev = ep->driver_data;
-	int		status = req->status;
-
-	switch (status) {
-
-	/* normal completion */
-	case 0:
-		skb_put(skb, req->actual);
-
-		if (dev->unwrap) {
-			unsigned long	flags;
-
-			spin_lock_irqsave(&dev->lock, flags);
-			if (dev->port_usb) {
-				status = dev->unwrap(dev->port_usb,
-							skb,
-							&dev->rx_frames);
-			} else {
-				dev_kfree_skb_any(skb);
-				status = -ENOTCONN;
-			}
-			spin_unlock_irqrestore(&dev->lock, flags);
-		} else {
-			skb_queue_tail(&dev->rx_frames, skb);
-		}
-		skb = NULL;
-
-		skb2 = skb_dequeue(&dev->rx_frames);
-		while (skb2) {
-			if (status < 0
-					|| ETH_HLEN > skb2->len
-					|| skb2->len > ETH_FRAME_LEN) {
-				dev->net->stats.rx_errors++;
-				dev->net->stats.rx_length_errors++;
-				DBG(dev, "rx length %d\n", skb2->len);
-				dev_kfree_skb_any(skb2);
-				goto next_frame;
-			}
-			skb2->protocol = eth_type_trans(skb2, dev->net);
-			dev->net->stats.rx_packets++;
-			dev->net->stats.rx_bytes += skb2->len;
-
-			/* no buffer copies needed, unless hardware can't
-			 * use skb buffers.
-			 */
-			status = netif_rx(skb2);
-next_frame:
-			skb2 = skb_dequeue(&dev->rx_frames);
-		}
-		break;
-
-	/* software-driven interface shutdown */
-	case -ECONNRESET:		/* unlink */
-	case -ESHUTDOWN:		/* disconnect etc */
-		VDBG(dev, "rx shutdown, code %d\n", status);
-		goto quiesce;
-
-	/* for hardware automagic (such as pxa) */
-	case -ECONNABORTED:		/* endpoint reset */
-		DBG(dev, "rx %s reset\n", ep->name);
-		defer_kevent(dev, WORK_RX_MEMORY);
-quiesce:
-		dev_kfree_skb_any(skb);
-		goto clean;
-
-	/* data overrun */
-	case -EOVERFLOW:
-		dev->net->stats.rx_over_errors++;
-		/* FALLTHROUGH */
-
-	default:
-		dev->net->stats.rx_errors++;
-		DBG(dev, "rx status %d\n", status);
-		break;
-	}
-
-	if (skb)
-		dev_kfree_skb_any(skb);
-	if (!netif_running(dev->net)) {
-clean:
-		spin_lock(&dev->req_lock);
-		list_add(&req->list, &dev->rx_reqs);
-		spin_unlock(&dev->req_lock);
-		req = NULL;
-	}
-	if (req)
-		rx_submit(dev, req, GFP_ATOMIC);
-}
-
-static int prealloc(struct list_head *list, struct usb_ep *ep, unsigned n)
-{
-	unsigned		i;
-	struct usb_request	*req;
-
-	if (!n)
-		return -ENOMEM;
-
-	/* queue/recycle up to N requests */
-	i = n;
-	list_for_each_entry(req, list, list) {
-		if (i-- == 0)
-			goto extra;
-	}
-	while (i--) {
-		req = usb_ep_alloc_request(ep, GFP_ATOMIC);
-		if (!req)
-			return list_empty(list) ? -ENOMEM : 0;
-		list_add(&req->list, list);
-	}
-	return 0;
-
-extra:
-	/* free extras */
-	for (;;) {
-		struct list_head	*next;
-
-		next = req->list.next;
-		list_del(&req->list);
-		usb_ep_free_request(ep, req);
-
-		if (next == list)
-			break;
-
-		req = container_of(next, struct usb_request, list);
-	}
-	return 0;
-}
-
-static int alloc_requests(struct eth_dev *dev, struct gether *link, unsigned n)
-{
-	int	status;
-
-	spin_lock(&dev->req_lock);
-	status = prealloc(&dev->tx_reqs, link->in_ep, n);
-	if (status < 0)
-		goto fail;
-	status = prealloc(&dev->rx_reqs, link->out_ep, n);
-	if (status < 0)
-		goto fail;
-	goto done;
-fail:
-	DBG(dev, "can't alloc requests\n");
-done:
-	spin_unlock(&dev->req_lock);
-	return status;
-}
-
-static void rx_fill(struct eth_dev *dev, gfp_t gfp_flags)
-{
-	struct usb_request	*req;
-	unsigned long		flags;
-
-	/* fill unused rxq slots with some skb */
-	spin_lock_irqsave(&dev->req_lock, flags);
-	while (!list_empty(&dev->rx_reqs)) {
-		req = container_of(dev->rx_reqs.next,
-				struct usb_request, list);
-		list_del_init(&req->list);
-		spin_unlock_irqrestore(&dev->req_lock, flags);
-
-		if (rx_submit(dev, req, gfp_flags) < 0) {
-			defer_kevent(dev, WORK_RX_MEMORY);
-			return;
-		}
-
-		spin_lock_irqsave(&dev->req_lock, flags);
-	}
-	spin_unlock_irqrestore(&dev->req_lock, flags);
-}
-
-static void eth_work(struct work_struct *work)
-{
-	struct eth_dev	*dev = container_of(work, struct eth_dev, work);
-
-	if (test_and_clear_bit(WORK_RX_MEMORY, &dev->todo)) {
-		if (netif_running(dev->net))
-			rx_fill(dev, GFP_KERNEL);
-	}
-
-	if (dev->todo)
-		DBG(dev, "work done, flags = 0x%lx\n", dev->todo);
-}
-
-static void tx_complete(struct usb_ep *ep, struct usb_request *req)
-{
-	struct sk_buff	*skb = req->context;
-	struct eth_dev	*dev = ep->driver_data;
-
-	switch (req->status) {
-	default:
-		dev->net->stats.tx_errors++;
-		VDBG(dev, "tx err %d\n", req->status);
-		/* FALLTHROUGH */
-	case -ECONNRESET:		/* unlink */
-	case -ESHUTDOWN:		/* disconnect etc */
-		break;
-	case 0:
-		dev->net->stats.tx_bytes += skb->len;
-	}
-	dev->net->stats.tx_packets++;
-
-	spin_lock(&dev->req_lock);
-	list_add(&req->list, &dev->tx_reqs);
-	spin_unlock(&dev->req_lock);
-	dev_kfree_skb_any(skb);
-
-	atomic_dec(&dev->tx_qlen);
-	if (netif_carrier_ok(dev->net))
-		netif_wake_queue(dev->net);
-}
-
-static inline int is_promisc(u16 cdc_filter)
-{
-	return cdc_filter & USB_CDC_PACKET_TYPE_PROMISCUOUS;
-}
-
-static netdev_tx_t eth_start_xmit(struct sk_buff *skb,
-					struct net_device *net)
-{
-	struct eth_dev		*dev = netdev_priv(net);
-	int			length = skb->len;
-	int			retval;
-	struct usb_request	*req = NULL;
-	unsigned long		flags;
-	struct usb_ep		*in;
-	u16			cdc_filter;
-
-	spin_lock_irqsave(&dev->lock, flags);
-	if (dev->port_usb) {
-		in = dev->port_usb->in_ep;
-		cdc_filter = dev->port_usb->cdc_filter;
-	} else {
-		in = NULL;
-		cdc_filter = 0;
-	}
-	spin_unlock_irqrestore(&dev->lock, flags);
-
-	if (!in) {
-		dev_kfree_skb_any(skb);
-		return NETDEV_TX_OK;
-	}
-
-	/* apply outgoing CDC or RNDIS filters */
-	if (!is_promisc(cdc_filter)) {
-		u8		*dest = skb->data;
-
-		if (is_multicast_ether_addr(dest)) {
-			u16	type;
-
-			/* ignores USB_CDC_PACKET_TYPE_MULTICAST and host
-			 * SET_ETHERNET_MULTICAST_FILTERS requests
-			 */
-			if (is_broadcast_ether_addr(dest))
-				type = USB_CDC_PACKET_TYPE_BROADCAST;
-			else
-				type = USB_CDC_PACKET_TYPE_ALL_MULTICAST;
-			if (!(cdc_filter & type)) {
-				dev_kfree_skb_any(skb);
-				return NETDEV_TX_OK;
-			}
-		}
-		/* ignores USB_CDC_PACKET_TYPE_DIRECTED */
-	}
-
-	spin_lock_irqsave(&dev->req_lock, flags);
-	/*
-	 * this freelist can be empty if an interrupt triggered disconnect()
-	 * and reconfigured the gadget (shutting down this queue) after the
-	 * network stack decided to xmit but before we got the spinlock.
-	 */
-	if (list_empty(&dev->tx_reqs)) {
-		spin_unlock_irqrestore(&dev->req_lock, flags);
-		return NETDEV_TX_BUSY;
-	}
-
-	req = container_of(dev->tx_reqs.next, struct usb_request, list);
-	list_del(&req->list);
-
-	/* temporarily stop TX queue when the freelist empties */
-	if (list_empty(&dev->tx_reqs))
-		netif_stop_queue(net);
-	spin_unlock_irqrestore(&dev->req_lock, flags);
-
-	/* no buffer copies needed, unless the network stack did it
-	 * or the hardware can't use skb buffers.
-	 * or there's not enough space for extra headers we need
-	 */
-	if (dev->wrap) {
-		unsigned long	flags;
-
-		spin_lock_irqsave(&dev->lock, flags);
-		if (dev->port_usb)
-			skb = dev->wrap(dev->port_usb, skb);
-		spin_unlock_irqrestore(&dev->lock, flags);
-		if (!skb)
-			goto drop;
-
-		length = skb->len;
-	}
-	req->buf = skb->data;
-	req->context = skb;
-	req->complete = tx_complete;
-
-	/* NCM requires no zlp if transfer is dwNtbInMaxSize */
-	if (dev->port_usb->is_fixed &&
-	    length == dev->port_usb->fixed_in_len &&
-	    (length % in->maxpacket) == 0)
-		req->zero = 0;
-	else
-		req->zero = 1;
-
-	/* use zlp framing on tx for strict CDC-Ether conformance,
-	 * though any robust network rx path ignores extra padding.
-	 * and some hardware doesn't like to write zlps.
-	 */
-	if (req->zero && !dev->zlp && (length % in->maxpacket) == 0)
-		length++;
-
-	req->length = length;
-
-	/* throttle high/super speed IRQ rate back slightly */
-	if (gadget_is_dualspeed(dev->gadget))
-		req->no_interrupt = (dev->gadget->speed == USB_SPEED_HIGH ||
-				     dev->gadget->speed == USB_SPEED_SUPER)
-			? ((atomic_read(&dev->tx_qlen) % qmult) != 0)
-			: 0;
-
-	retval = usb_ep_queue(in, req, GFP_ATOMIC);
-	switch (retval) {
-	default:
-		DBG(dev, "tx queue err %d\n", retval);
-		break;
-	case 0:
-		net->trans_start = jiffies;
-		atomic_inc(&dev->tx_qlen);
-	}
-
-	if (retval) {
-		dev_kfree_skb_any(skb);
-drop:
-		dev->net->stats.tx_dropped++;
-		spin_lock_irqsave(&dev->req_lock, flags);
-		if (list_empty(&dev->tx_reqs))
-			netif_start_queue(net);
-		list_add(&req->list, &dev->tx_reqs);
-		spin_unlock_irqrestore(&dev->req_lock, flags);
-	}
-	return NETDEV_TX_OK;
-}
-
-/*-------------------------------------------------------------------------*/
-
-static void eth_start(struct eth_dev *dev, gfp_t gfp_flags)
-{
-	DBG(dev, "%s\n", __func__);
-
-	/* fill the rx queue */
-	rx_fill(dev, gfp_flags);
-
-	/* and open the tx floodgates */
-	atomic_set(&dev->tx_qlen, 0);
-	netif_wake_queue(dev->net);
-}
-
-static int eth_open(struct net_device *net)
-{
-	struct eth_dev	*dev = netdev_priv(net);
-	struct gether	*link;
-
-	DBG(dev, "%s\n", __func__);
-	if (netif_carrier_ok(dev->net))
-		eth_start(dev, GFP_KERNEL);
-
-	spin_lock_irq(&dev->lock);
-	link = dev->port_usb;
-	if (link && link->open)
-		link->open(link);
-	spin_unlock_irq(&dev->lock);
-
-	return 0;
-}
-
-static int eth_stop(struct net_device *net)
-{
-	struct eth_dev	*dev = netdev_priv(net);
-	unsigned long	flags;
-
-	VDBG(dev, "%s\n", __func__);
-	netif_stop_queue(net);
-
-	DBG(dev, "stop stats: rx/tx %ld/%ld, errs %ld/%ld\n",
-		dev->net->stats.rx_packets, dev->net->stats.tx_packets,
-		dev->net->stats.rx_errors, dev->net->stats.tx_errors
-		);
-
-	/* ensure there are no more active requests */
-	spin_lock_irqsave(&dev->lock, flags);
-	if (dev->port_usb) {
-		struct gether	*link = dev->port_usb;
-
-		if (link->close)
-			link->close(link);
-
-		/* NOTE:  we have no abort-queue primitive we could use
-		 * to cancel all pending I/O.  Instead, we disable then
-		 * reenable the endpoints ... this idiom may leave toggle
-		 * wrong, but that's a self-correcting error.
-		 *
-		 * REVISIT:  we *COULD* just let the transfers complete at
-		 * their own pace; the network stack can handle old packets.
-		 * For the moment we leave this here, since it works.
-		 */
-		usb_ep_disable(link->in_ep);
-		usb_ep_disable(link->out_ep);
-		if (netif_carrier_ok(net)) {
-			DBG(dev, "host still using in/out endpoints\n");
-			usb_ep_enable(link->in_ep);
-			usb_ep_enable(link->out_ep);
-		}
-	}
-	spin_unlock_irqrestore(&dev->lock, flags);
-
-	return 0;
-}
-
-/*-------------------------------------------------------------------------*/
-
-/* initial value, changed by "ifconfig usb0 hw ether xx:xx:xx:xx:xx:xx" */
-static char *dev_addr;
-module_param(dev_addr, charp, S_IRUGO);
-MODULE_PARM_DESC(dev_addr, "Device Ethernet Address");
-
-/* this address is invisible to ifconfig */
-static char *host_addr;
-module_param(host_addr, charp, S_IRUGO);
-MODULE_PARM_DESC(host_addr, "Host Ethernet Address");
-
-static int get_ether_addr(const char *str, u8 *dev_addr)
-{
-	if (str) {
-		unsigned	i;
-
-		for (i = 0; i < 6; i++) {
-			unsigned char num;
-
-			if ((*str == '.') || (*str == ':'))
-				str++;
-			num = hex_to_bin(*str++) << 4;
-			num |= hex_to_bin(*str++);
-			dev_addr [i] = num;
-		}
-		if (is_valid_ether_addr(dev_addr))
-			return 0;
-	}
-	eth_random_addr(dev_addr);
-	return 1;
-}
-
-static struct eth_dev *the_dev;
-
-static const struct net_device_ops eth_netdev_ops = {
-	.ndo_open		= eth_open,
-	.ndo_stop		= eth_stop,
-	.ndo_start_xmit		= eth_start_xmit,
-	.ndo_change_mtu		= ueth_change_mtu,
-	.ndo_set_mac_address 	= eth_mac_addr,
-	.ndo_validate_addr	= eth_validate_addr,
-};
-
-static struct device_type gadget_type = {
-	.name	= "gadget",
-};
-
-/**
- * gether_setup_name - initialize one ethernet-over-usb link
- * @g: gadget to associated with these links
- * @ethaddr: NULL, or a buffer in which the ethernet address of the
- *	host side of the link is recorded
- * @netname: name for network device (for example, "usb")
- * Context: may sleep
- *
- * This sets up the single network link that may be exported by a
- * gadget driver using this framework.  The link layer addresses are
- * set up using module parameters.
- *
- * Returns negative errno, or zero on success
- */
-int gether_setup_name(struct usb_gadget *g, u8 ethaddr[ETH_ALEN],
-		const char *netname)
-{
-	struct eth_dev		*dev;
-	struct net_device	*net;
-	int			status;
-
-	if (the_dev)
-		return -EBUSY;
-
-	net = alloc_etherdev(sizeof *dev);
-	if (!net)
-		return -ENOMEM;
-
-	dev = netdev_priv(net);
-	spin_lock_init(&dev->lock);
-	spin_lock_init(&dev->req_lock);
-	INIT_WORK(&dev->work, eth_work);
-	INIT_LIST_HEAD(&dev->tx_reqs);
-	INIT_LIST_HEAD(&dev->rx_reqs);
-
-	skb_queue_head_init(&dev->rx_frames);
-
-	/* network device setup */
-	dev->net = net;
-	snprintf(net->name, sizeof(net->name), "%s%%d", netname);
-
-	if (get_ether_addr(dev_addr, net->dev_addr))
-		dev_warn(&g->dev,
-			"using random %s ethernet address\n", "self");
-	if (get_ether_addr(host_addr, dev->host_mac))
-		dev_warn(&g->dev,
-			"using random %s ethernet address\n", "host");
-
-	if (ethaddr)
-		memcpy(ethaddr, dev->host_mac, ETH_ALEN);
-
-	net->netdev_ops = &eth_netdev_ops;
-
-	SET_ETHTOOL_OPS(net, &ops);
-
-	dev->gadget = g;
-	SET_NETDEV_DEV(net, &g->dev);
-	SET_NETDEV_DEVTYPE(net, &gadget_type);
-
-	status = register_netdev(net);
-	if (status < 0) {
-		dev_dbg(&g->dev, "register_netdev failed, %d\n", status);
-		free_netdev(net);
-	} else {
-		INFO(dev, "MAC %pM\n", net->dev_addr);
-		INFO(dev, "HOST MAC %pM\n", dev->host_mac);
-
-		the_dev = dev;
-
-		/* two kinds of host-initiated state changes:
-		 *  - iff DATA transfer is active, carrier is "on"
-		 *  - tx queueing enabled if open *and* carrier is "on"
-		 */
-		netif_carrier_off(net);
-	}
-
-	return status;
-}
-
-/**
- * gether_cleanup - remove Ethernet-over-USB device
- * Context: may sleep
- *
- * This is called to free all resources allocated by @gether_setup().
- */
-void gether_cleanup(void)
-{
-	if (!the_dev)
-		return;
-
-	unregister_netdev(the_dev->net);
-	flush_work(&the_dev->work);
-	free_netdev(the_dev->net);
-
-	the_dev = NULL;
-}
-
-
-/**
- * gether_connect - notify network layer that USB link is active
- * @link: the USB link, set up with endpoints, descriptors matching
- *	current device speed, and any framing wrapper(s) set up.
- * Context: irqs blocked
- *
- * This is called to activate endpoints and let the network layer know
- * the connection is active ("carrier detect").  It may cause the I/O
- * queues to open and start letting network packets flow, but will in
- * any case activate the endpoints so that they respond properly to the
- * USB host.
- *
- * Verify net_device pointer returned using IS_ERR().  If it doesn't
- * indicate some error code (negative errno), ep->driver_data values
- * have been overwritten.
- */
-struct net_device *gether_connect(struct gether *link)
-{
-	struct eth_dev		*dev = the_dev;
-	int			result = 0;
-
-	if (!dev)
-		return ERR_PTR(-EINVAL);
-
-	link->in_ep->driver_data = dev;
-	result = usb_ep_enable(link->in_ep);
-	if (result != 0) {
-		DBG(dev, "enable %s --> %d\n",
-			link->in_ep->name, result);
-		goto fail0;
-	}
-
-	link->out_ep->driver_data = dev;
-	result = usb_ep_enable(link->out_ep);
-	if (result != 0) {
-		DBG(dev, "enable %s --> %d\n",
-			link->out_ep->name, result);
-		goto fail1;
-	}
-
-	if (result == 0)
-		result = alloc_requests(dev, link, qlen(dev->gadget));
-
-	if (result == 0) {
-		dev->zlp = link->is_zlp_ok;
-		DBG(dev, "qlen %d\n", qlen(dev->gadget));
-
-		dev->header_len = link->header_len;
-		dev->unwrap = link->unwrap;
-		dev->wrap = link->wrap;
-
-		spin_lock(&dev->lock);
-		dev->port_usb = link;
-		link->ioport = dev;
-		if (netif_running(dev->net)) {
-			if (link->open)
-				link->open(link);
-		} else {
-			if (link->close)
-				link->close(link);
-		}
-		spin_unlock(&dev->lock);
-
-		netif_carrier_on(dev->net);
-		if (netif_running(dev->net))
-			eth_start(dev, GFP_ATOMIC);
-
-	/* on error, disable any endpoints  */
-	} else {
-		(void) usb_ep_disable(link->out_ep);
-fail1:
-		(void) usb_ep_disable(link->in_ep);
-	}
-fail0:
-	/* caller is responsible for cleanup on error */
-	if (result < 0)
-		return ERR_PTR(result);
-	return dev->net;
-}
-
-/**
- * gether_disconnect - notify network layer that USB link is inactive
- * @link: the USB link, on which gether_connect() was called
- * Context: irqs blocked
- *
- * This is called to deactivate endpoints and let the network layer know
- * the connection went inactive ("no carrier").
- *
- * On return, the state is as if gether_connect() had never been called.
- * The endpoints are inactive, and accordingly without active USB I/O.
- * Pointers to endpoint descriptors and endpoint private data are nulled.
- */
-void gether_disconnect(struct gether *link)
-{
-	struct eth_dev		*dev = link->ioport;
-	struct usb_request	*req;
-
-	WARN_ON(!dev);
-	if (!dev)
-		return;
-
-	DBG(dev, "%s\n", __func__);
-
-	netif_stop_queue(dev->net);
-	netif_carrier_off(dev->net);
-
-	/* disable endpoints, forcing (synchronous) completion
-	 * of all pending i/o.  then free the request objects
-	 * and forget about the endpoints.
-	 */
-	usb_ep_disable(link->in_ep);
-	spin_lock(&dev->req_lock);
-	while (!list_empty(&dev->tx_reqs)) {
-		req = container_of(dev->tx_reqs.next,
-					struct usb_request, list);
-		list_del(&req->list);
-
-		spin_unlock(&dev->req_lock);
-		usb_ep_free_request(link->in_ep, req);
-		spin_lock(&dev->req_lock);
-	}
-	spin_unlock(&dev->req_lock);
-	link->in_ep->driver_data = NULL;
-	link->in_ep->desc = NULL;
-
-	usb_ep_disable(link->out_ep);
-	spin_lock(&dev->req_lock);
-	while (!list_empty(&dev->rx_reqs)) {
-		req = container_of(dev->rx_reqs.next,
-					struct usb_request, list);
-		list_del(&req->list);
-
-		spin_unlock(&dev->req_lock);
-		usb_ep_free_request(link->out_ep, req);
-		spin_lock(&dev->req_lock);
-	}
-	spin_unlock(&dev->req_lock);
-	link->out_ep->driver_data = NULL;
-	link->out_ep->desc = NULL;
-
-	/* finish forgetting about this USB link episode */
-	dev->header_len = 0;
-	dev->unwrap = NULL;
-	dev->wrap = NULL;
-
-	spin_lock(&dev->lock);
-	dev->port_usb = NULL;
-	link->ioport = NULL;
-	spin_unlock(&dev->lock);
-}
diff --git a/drivers/staging/ccg/u_ether.h b/drivers/staging/ccg/u_ether.h
deleted file mode 100644
index 6f4a1623d85..00000000000
--- a/drivers/staging/ccg/u_ether.h
+++ /dev/null
@@ -1,154 +0,0 @@
-/*
- * u_ether.h -- interface to USB gadget "ethernet link" utilities
- *
- * Copyright (C) 2003-2005,2008 David Brownell
- * Copyright (C) 2003-2004 Robert Schwebel, Benedikt Spranger
- * Copyright (C) 2008 Nokia Corporation
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- */
-
-#ifndef __U_ETHER_H
-#define __U_ETHER_H
-
-#include <linux/err.h>
-#include <linux/if_ether.h>
-#include <linux/usb/composite.h>
-#include <linux/usb/cdc.h>
-
-#include "gadget_chips.h"
-
-
-/*
- * This represents the USB side of an "ethernet" link, managed by a USB
- * function which provides control and (maybe) framing.  Two functions
- * in different configurations could share the same ethernet link/netdev,
- * using different host interaction models.
- *
- * There is a current limitation that only one instance of this link may
- * be present in any given configuration.  When that's a problem, network
- * layer facilities can be used to package multiple logical links on this
- * single "physical" one.
- */
-struct gether {
-	struct usb_function		func;
-
-	/* updated by gether_{connect,disconnect} */
-	struct eth_dev			*ioport;
-
-	/* endpoints handle full and/or high speeds */
-	struct usb_ep			*in_ep;
-	struct usb_ep			*out_ep;
-
-	bool				is_zlp_ok;
-
-	u16				cdc_filter;
-
-	/* hooks for added framing, as needed for RNDIS and EEM. */
-	u32				header_len;
-	/* NCM requires fixed size bundles */
-	bool				is_fixed;
-	u32				fixed_out_len;
-	u32				fixed_in_len;
-	struct sk_buff			*(*wrap)(struct gether *port,
-						struct sk_buff *skb);
-	int				(*unwrap)(struct gether *port,
-						struct sk_buff *skb,
-						struct sk_buff_head *list);
-
-	/* called on network open/close */
-	void				(*open)(struct gether *);
-	void				(*close)(struct gether *);
-};
-
-#define	DEFAULT_FILTER	(USB_CDC_PACKET_TYPE_BROADCAST \
-			|USB_CDC_PACKET_TYPE_ALL_MULTICAST \
-			|USB_CDC_PACKET_TYPE_PROMISCUOUS \
-			|USB_CDC_PACKET_TYPE_DIRECTED)
-
-/* variant of gether_setup that allows customizing network device name */
-int gether_setup_name(struct usb_gadget *g, u8 ethaddr[ETH_ALEN],
-		const char *netname);
-
-/* netdev setup/teardown as directed by the gadget driver */
-/* gether_setup - initialize one ethernet-over-usb link
- * @g: gadget to associated with these links
- * @ethaddr: NULL, or a buffer in which the ethernet address of the
- *	host side of the link is recorded
- * Context: may sleep
- *
- * This sets up the single network link that may be exported by a
- * gadget driver using this framework.  The link layer addresses are
- * set up using module parameters.
- *
- * Returns negative errno, or zero on success
- */
-static inline int gether_setup(struct usb_gadget *g, u8 ethaddr[ETH_ALEN])
-{
-	return gether_setup_name(g, ethaddr, "usb");
-}
-
-void gether_cleanup(void);
-
-/* connect/disconnect is handled by individual functions */
-struct net_device *gether_connect(struct gether *);
-void gether_disconnect(struct gether *);
-
-/* Some controllers can't support CDC Ethernet (ECM) ... */
-static inline bool can_support_ecm(struct usb_gadget *gadget)
-{
-	if (!gadget_supports_altsettings(gadget))
-		return false;
-
-	/* Everything else is *presumably* fine ... but this is a bit
-	 * chancy, so be **CERTAIN** there are no hardware issues with
-	 * your controller.  Add it above if it can't handle CDC.
-	 */
-	return true;
-}
-
-/* each configuration may bind one instance of an ethernet link */
-int geth_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN]);
-int ecm_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN]);
-int ncm_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN]);
-int eem_bind_config(struct usb_configuration *c);
-
-#ifdef USB_ETH_RNDIS
-
-int rndis_bind_config_vendor(struct usb_configuration *c, u8 ethaddr[ETH_ALEN],
-				u32 vendorID, const char *manufacturer);
-
-#else
-
-static inline int
-rndis_bind_config_vendor(struct usb_configuration *c, u8 ethaddr[ETH_ALEN],
-				u32 vendorID, const char *manufacturer)
-{
-	return 0;
-}
-
-#endif
-
-/**
- * rndis_bind_config - add RNDIS network link to a configuration
- * @c: the configuration to support the network link
- * @ethaddr: a buffer in which the ethernet address of the host side
- *	side of the link was recorded
- * Context: single threaded during gadget setup
- *
- * Returns zero on success, else negative errno.
- *
- * Caller must have called @gether_setup().  Caller is also responsible
- * for calling @gether_cleanup() before module unload.
- */
-static inline int rndis_bind_config(struct usb_configuration *c,
-				    u8 ethaddr[ETH_ALEN])
-{
-	return rndis_bind_config_vendor(c, ethaddr, 0, NULL);
-}
-
-
-#endif /* __U_ETHER_H */
diff --git a/drivers/staging/ccg/u_serial.c b/drivers/staging/ccg/u_serial.c
deleted file mode 100644
index b10947ae0ac..00000000000
--- a/drivers/staging/ccg/u_serial.c
+++ /dev/null
@@ -1,1339 +0,0 @@
-/*
- * u_serial.c - utilities for USB gadget "serial port"/TTY support
- *
- * Copyright (C) 2003 Al Borchers (alborchers@steinerpoint.com)
- * Copyright (C) 2008 David Brownell
- * Copyright (C) 2008 by Nokia Corporation
- *
- * This code also borrows from usbserial.c, which is
- * Copyright (C) 1999 - 2002 Greg Kroah-Hartman (greg@kroah.com)
- * Copyright (C) 2000 Peter Berger (pberger@brimson.com)
- * Copyright (C) 2000 Al Borchers (alborchers@steinerpoint.com)
- *
- * This software is distributed under the terms of the GNU General
- * Public License ("GPL") as published by the Free Software Foundation,
- * either version 2 of that License or (at your option) any later version.
- */
-
-/* #define VERBOSE_DEBUG */
-
-#include <linux/kernel.h>
-#include <linux/sched.h>
-#include <linux/interrupt.h>
-#include <linux/device.h>
-#include <linux/delay.h>
-#include <linux/tty.h>
-#include <linux/tty_flip.h>
-#include <linux/slab.h>
-#include <linux/export.h>
-
-#include "u_serial.h"
-
-
-/*
- * This component encapsulates the TTY layer glue needed to provide basic
- * "serial port" functionality through the USB gadget stack.  Each such
- * port is exposed through a /dev/ttyGS* node.
- *
- * After initialization (gserial_setup), these TTY port devices stay
- * available until they are removed (gserial_cleanup).  Each one may be
- * connected to a USB function (gserial_connect), or disconnected (with
- * gserial_disconnect) when the USB host issues a config change event.
- * Data can only flow when the port is connected to the host.
- *
- * A given TTY port can be made available in multiple configurations.
- * For example, each one might expose a ttyGS0 node which provides a
- * login application.  In one case that might use CDC ACM interface 0,
- * while another configuration might use interface 3 for that.  The
- * work to handle that (including descriptor management) is not part
- * of this component.
- *
- * Configurations may expose more than one TTY port.  For example, if
- * ttyGS0 provides login service, then ttyGS1 might provide dialer access
- * for a telephone or fax link.  And ttyGS2 might be something that just
- * needs a simple byte stream interface for some messaging protocol that
- * is managed in userspace ... OBEX, PTP, and MTP have been mentioned.
- */
-
-#define PREFIX	"ttyGS"
-
-/*
- * gserial is the lifecycle interface, used by USB functions
- * gs_port is the I/O nexus, used by the tty driver
- * tty_struct links to the tty/filesystem framework
- *
- * gserial <---> gs_port ... links will be null when the USB link is
- * inactive; managed by gserial_{connect,disconnect}().  each gserial
- * instance can wrap its own USB control protocol.
- *	gserial->ioport == usb_ep->driver_data ... gs_port
- *	gs_port->port_usb ... gserial
- *
- * gs_port <---> tty_struct ... links will be null when the TTY file
- * isn't opened; managed by gs_open()/gs_close()
- *	gserial->port_tty ... tty_struct
- *	tty_struct->driver_data ... gserial
- */
-
-/* RX and TX queues can buffer QUEUE_SIZE packets before they hit the
- * next layer of buffering.  For TX that's a circular buffer; for RX
- * consider it a NOP.  A third layer is provided by the TTY code.
- */
-#define QUEUE_SIZE		16
-#define WRITE_BUF_SIZE		8192		/* TX only */
-
-/* circular buffer */
-struct gs_buf {
-	unsigned		buf_size;
-	char			*buf_buf;
-	char			*buf_get;
-	char			*buf_put;
-};
-
-/*
- * The port structure holds info for each port, one for each minor number
- * (and thus for each /dev/ node).
- */
-struct gs_port {
-	struct tty_port		port;
-	spinlock_t		port_lock;	/* guard port_* access */
-
-	struct gserial		*port_usb;
-
-	bool			openclose;	/* open/close in progress */
-	u8			port_num;
-
-	struct list_head	read_pool;
-	int read_started;
-	int read_allocated;
-	struct list_head	read_queue;
-	unsigned		n_read;
-	struct tasklet_struct	push;
-
-	struct list_head	write_pool;
-	int write_started;
-	int write_allocated;
-	struct gs_buf		port_write_buf;
-	wait_queue_head_t	drain_wait;	/* wait while writes drain */
-
-	/* REVISIT this state ... */
-	struct usb_cdc_line_coding port_line_coding;	/* 8-N-1 etc */
-};
-
-/* increase N_PORTS if you need more */
-#define N_PORTS		4
-static struct portmaster {
-	struct mutex	lock;			/* protect open/close */
-	struct gs_port	*port;
-} ports[N_PORTS];
-static unsigned	n_ports;
-
-#define GS_CLOSE_TIMEOUT		15		/* seconds */
-
-
-
-#ifdef VERBOSE_DEBUG
-#define pr_vdebug(fmt, arg...) \
-	pr_debug(fmt, ##arg)
-#else
-#define pr_vdebug(fmt, arg...) \
-	({ if (0) pr_debug(fmt, ##arg); })
-#endif
-
-/*-------------------------------------------------------------------------*/
-
-/* Circular Buffer */
-
-/*
- * gs_buf_alloc
- *
- * Allocate a circular buffer and all associated memory.
- */
-static int gs_buf_alloc(struct gs_buf *gb, unsigned size)
-{
-	gb->buf_buf = kmalloc(size, GFP_KERNEL);
-	if (gb->buf_buf == NULL)
-		return -ENOMEM;
-
-	gb->buf_size = size;
-	gb->buf_put = gb->buf_buf;
-	gb->buf_get = gb->buf_buf;
-
-	return 0;
-}
-
-/*
- * gs_buf_free
- *
- * Free the buffer and all associated memory.
- */
-static void gs_buf_free(struct gs_buf *gb)
-{
-	kfree(gb->buf_buf);
-	gb->buf_buf = NULL;
-}
-
-/*
- * gs_buf_clear
- *
- * Clear out all data in the circular buffer.
- */
-static void gs_buf_clear(struct gs_buf *gb)
-{
-	gb->buf_get = gb->buf_put;
-	/* equivalent to a get of all data available */
-}
-
-/*
- * gs_buf_data_avail
- *
- * Return the number of bytes of data written into the circular
- * buffer.
- */
-static unsigned gs_buf_data_avail(struct gs_buf *gb)
-{
-	return (gb->buf_size + gb->buf_put - gb->buf_get) % gb->buf_size;
-}
-
-/*
- * gs_buf_space_avail
- *
- * Return the number of bytes of space available in the circular
- * buffer.
- */
-static unsigned gs_buf_space_avail(struct gs_buf *gb)
-{
-	return (gb->buf_size + gb->buf_get - gb->buf_put - 1) % gb->buf_size;
-}
-
-/*
- * gs_buf_put
- *
- * Copy data data from a user buffer and put it into the circular buffer.
- * Restrict to the amount of space available.
- *
- * Return the number of bytes copied.
- */
-static unsigned
-gs_buf_put(struct gs_buf *gb, const char *buf, unsigned count)
-{
-	unsigned len;
-
-	len  = gs_buf_space_avail(gb);
-	if (count > len)
-		count = len;
-
-	if (count == 0)
-		return 0;
-
-	len = gb->buf_buf + gb->buf_size - gb->buf_put;
-	if (count > len) {
-		memcpy(gb->buf_put, buf, len);
-		memcpy(gb->buf_buf, buf+len, count - len);
-		gb->buf_put = gb->buf_buf + count - len;
-	} else {
-		memcpy(gb->buf_put, buf, count);
-		if (count < len)
-			gb->buf_put += count;
-		else /* count == len */
-			gb->buf_put = gb->buf_buf;
-	}
-
-	return count;
-}
-
-/*
- * gs_buf_get
- *
- * Get data from the circular buffer and copy to the given buffer.
- * Restrict to the amount of data available.
- *
- * Return the number of bytes copied.
- */
-static unsigned
-gs_buf_get(struct gs_buf *gb, char *buf, unsigned count)
-{
-	unsigned len;
-
-	len = gs_buf_data_avail(gb);
-	if (count > len)
-		count = len;
-
-	if (count == 0)
-		return 0;
-
-	len = gb->buf_buf + gb->buf_size - gb->buf_get;
-	if (count > len) {
-		memcpy(buf, gb->buf_get, len);
-		memcpy(buf+len, gb->buf_buf, count - len);
-		gb->buf_get = gb->buf_buf + count - len;
-	} else {
-		memcpy(buf, gb->buf_get, count);
-		if (count < len)
-			gb->buf_get += count;
-		else /* count == len */
-			gb->buf_get = gb->buf_buf;
-	}
-
-	return count;
-}
-
-/*-------------------------------------------------------------------------*/
-
-/* I/O glue between TTY (upper) and USB function (lower) driver layers */
-
-/*
- * gs_alloc_req
- *
- * Allocate a usb_request and its buffer.  Returns a pointer to the
- * usb_request or NULL if there is an error.
- */
-struct usb_request *
-gs_alloc_req(struct usb_ep *ep, unsigned len, gfp_t kmalloc_flags)
-{
-	struct usb_request *req;
-
-	req = usb_ep_alloc_request(ep, kmalloc_flags);
-
-	if (req != NULL) {
-		req->length = len;
-		req->buf = kmalloc(len, kmalloc_flags);
-		if (req->buf == NULL) {
-			usb_ep_free_request(ep, req);
-			return NULL;
-		}
-	}
-
-	return req;
-}
-
-/*
- * gs_free_req
- *
- * Free a usb_request and its buffer.
- */
-void gs_free_req(struct usb_ep *ep, struct usb_request *req)
-{
-	kfree(req->buf);
-	usb_ep_free_request(ep, req);
-}
-
-/*
- * gs_send_packet
- *
- * If there is data to send, a packet is built in the given
- * buffer and the size is returned.  If there is no data to
- * send, 0 is returned.
- *
- * Called with port_lock held.
- */
-static unsigned
-gs_send_packet(struct gs_port *port, char *packet, unsigned size)
-{
-	unsigned len;
-
-	len = gs_buf_data_avail(&port->port_write_buf);
-	if (len < size)
-		size = len;
-	if (size != 0)
-		size = gs_buf_get(&port->port_write_buf, packet, size);
-	return size;
-}
-
-/*
- * gs_start_tx
- *
- * This function finds available write requests, calls
- * gs_send_packet to fill these packets with data, and
- * continues until either there are no more write requests
- * available or no more data to send.  This function is
- * run whenever data arrives or write requests are available.
- *
- * Context: caller owns port_lock; port_usb is non-null.
- */
-static int gs_start_tx(struct gs_port *port)
-/*
-__releases(&port->port_lock)
-__acquires(&port->port_lock)
-*/
-{
-	struct list_head	*pool = &port->write_pool;
-	struct usb_ep		*in = port->port_usb->in;
-	int			status = 0;
-	bool			do_tty_wake = false;
-
-	while (!list_empty(pool)) {
-		struct usb_request	*req;
-		int			len;
-
-		if (port->write_started >= QUEUE_SIZE)
-			break;
-
-		req = list_entry(pool->next, struct usb_request, list);
-		len = gs_send_packet(port, req->buf, in->maxpacket);
-		if (len == 0) {
-			wake_up_interruptible(&port->drain_wait);
-			break;
-		}
-		do_tty_wake = true;
-
-		req->length = len;
-		list_del(&req->list);
-		req->zero = (gs_buf_data_avail(&port->port_write_buf) == 0);
-
-		pr_vdebug(PREFIX "%d: tx len=%d, 0x%02x 0x%02x 0x%02x ...\n",
-				port->port_num, len, *((u8 *)req->buf),
-				*((u8 *)req->buf+1), *((u8 *)req->buf+2));
-
-		/* Drop lock while we call out of driver; completions
-		 * could be issued while we do so.  Disconnection may
-		 * happen too; maybe immediately before we queue this!
-		 *
-		 * NOTE that we may keep sending data for a while after
-		 * the TTY closed (dev->ioport->port_tty is NULL).
-		 */
-		spin_unlock(&port->port_lock);
-		status = usb_ep_queue(in, req, GFP_ATOMIC);
-		spin_lock(&port->port_lock);
-
-		if (status) {
-			pr_debug("%s: %s %s err %d\n",
-					__func__, "queue", in->name, status);
-			list_add(&req->list, pool);
-			break;
-		}
-
-		port->write_started++;
-
-		/* abort immediately after disconnect */
-		if (!port->port_usb)
-			break;
-	}
-
-	if (do_tty_wake && port->port.tty)
-		tty_wakeup(port->port.tty);
-	return status;
-}
-
-/*
- * Context: caller owns port_lock, and port_usb is set
- */
-static unsigned gs_start_rx(struct gs_port *port)
-/*
-__releases(&port->port_lock)
-__acquires(&port->port_lock)
-*/
-{
-	struct list_head	*pool = &port->read_pool;
-	struct usb_ep		*out = port->port_usb->out;
-
-	while (!list_empty(pool)) {
-		struct usb_request	*req;
-		int			status;
-		struct tty_struct	*tty;
-
-		/* no more rx if closed */
-		tty = port->port.tty;
-		if (!tty)
-			break;
-
-		if (port->read_started >= QUEUE_SIZE)
-			break;
-
-		req = list_entry(pool->next, struct usb_request, list);
-		list_del(&req->list);
-		req->length = out->maxpacket;
-
-		/* drop lock while we call out; the controller driver
-		 * may need to call us back (e.g. for disconnect)
-		 */
-		spin_unlock(&port->port_lock);
-		status = usb_ep_queue(out, req, GFP_ATOMIC);
-		spin_lock(&port->port_lock);
-
-		if (status) {
-			pr_debug("%s: %s %s err %d\n",
-					__func__, "queue", out->name, status);
-			list_add(&req->list, pool);
-			break;
-		}
-		port->read_started++;
-
-		/* abort immediately after disconnect */
-		if (!port->port_usb)
-			break;
-	}
-	return port->read_started;
-}
-
-/*
- * RX tasklet takes data out of the RX queue and hands it up to the TTY
- * layer until it refuses to take any more data (or is throttled back).
- * Then it issues reads for any further data.
- *
- * If the RX queue becomes full enough that no usb_request is queued,
- * the OUT endpoint may begin NAKing as soon as its FIFO fills up.
- * So QUEUE_SIZE packets plus however many the FIFO holds (usually two)
- * can be buffered before the TTY layer's buffers (currently 64 KB).
- */
-static void gs_rx_push(unsigned long _port)
-{
-	struct gs_port		*port = (void *)_port;
-	struct tty_struct	*tty;
-	struct list_head	*queue = &port->read_queue;
-	bool			disconnect = false;
-	bool			do_push = false;
-
-	/* hand any queued data to the tty */
-	spin_lock_irq(&port->port_lock);
-	tty = port->port.tty;
-	while (!list_empty(queue)) {
-		struct usb_request	*req;
-
-		req = list_first_entry(queue, struct usb_request, list);
-
-		/* leave data queued if tty was rx throttled */
-		if (tty && test_bit(TTY_THROTTLED, &tty->flags))
-			break;
-
-		switch (req->status) {
-		case -ESHUTDOWN:
-			disconnect = true;
-			pr_vdebug(PREFIX "%d: shutdown\n", port->port_num);
-			break;
-
-		default:
-			/* presumably a transient fault */
-			pr_warning(PREFIX "%d: unexpected RX status %d\n",
-					port->port_num, req->status);
-			/* FALLTHROUGH */
-		case 0:
-			/* normal completion */
-			break;
-		}
-
-		/* push data to (open) tty */
-		if (req->actual) {
-			char		*packet = req->buf;
-			unsigned	size = req->actual;
-			unsigned	n;
-			int		count;
-
-			/* we may have pushed part of this packet already... */
-			n = port->n_read;
-			if (n) {
-				packet += n;
-				size -= n;
-			}
-
-			count = tty_insert_flip_string(&port->port, packet, size);
-			if (count)
-				do_push = true;
-			if (count != size) {
-				/* stop pushing; TTY layer can't handle more */
-				port->n_read += count;
-				pr_vdebug(PREFIX "%d: rx block %d/%d\n",
-						port->port_num,
-						count, req->actual);
-				break;
-			}
-			port->n_read = 0;
-		}
-		list_move(&req->list, &port->read_pool);
-		port->read_started--;
-	}
-
-	/* Push from tty to ldisc; without low_latency set this is handled by
-	 * a workqueue, so we won't get callbacks and can hold port_lock
-	 */
-	if (do_push)
-		tty_flip_buffer_push(&port->port);
-
-
-	/* We want our data queue to become empty ASAP, keeping data
-	 * in the tty and ldisc (not here).  If we couldn't push any
-	 * this time around, there may be trouble unless there's an
-	 * implicit tty_unthrottle() call on its way...
-	 *
-	 * REVISIT we should probably add a timer to keep the tasklet
-	 * from starving ... but it's not clear that case ever happens.
-	 */
-	if (!list_empty(queue) && tty) {
-		if (!test_bit(TTY_THROTTLED, &tty->flags)) {
-			if (do_push)
-				tasklet_schedule(&port->push);
-			else
-				pr_warning(PREFIX "%d: RX not scheduled?\n",
-					port->port_num);
-		}
-	}
-
-	/* If we're still connected, refill the USB RX queue. */
-	if (!disconnect && port->port_usb)
-		gs_start_rx(port);
-
-	spin_unlock_irq(&port->port_lock);
-}
-
-static void gs_read_complete(struct usb_ep *ep, struct usb_request *req)
-{
-	struct gs_port	*port = ep->driver_data;
-
-	/* Queue all received data until the tty layer is ready for it. */
-	spin_lock(&port->port_lock);
-	list_add_tail(&req->list, &port->read_queue);
-	tasklet_schedule(&port->push);
-	spin_unlock(&port->port_lock);
-}
-
-static void gs_write_complete(struct usb_ep *ep, struct usb_request *req)
-{
-	struct gs_port	*port = ep->driver_data;
-
-	spin_lock(&port->port_lock);
-	list_add(&req->list, &port->write_pool);
-	port->write_started--;
-
-	switch (req->status) {
-	default:
-		/* presumably a transient fault */
-		pr_warning("%s: unexpected %s status %d\n",
-				__func__, ep->name, req->status);
-		/* FALL THROUGH */
-	case 0:
-		/* normal completion */
-		gs_start_tx(port);
-		break;
-
-	case -ESHUTDOWN:
-		/* disconnect */
-		pr_vdebug("%s: %s shutdown\n", __func__, ep->name);
-		break;
-	}
-
-	spin_unlock(&port->port_lock);
-}
-
-static void gs_free_requests(struct usb_ep *ep, struct list_head *head,
-							 int *allocated)
-{
-	struct usb_request	*req;
-
-	while (!list_empty(head)) {
-		req = list_entry(head->next, struct usb_request, list);
-		list_del(&req->list);
-		gs_free_req(ep, req);
-		if (allocated)
-			(*allocated)--;
-	}
-}
-
-static int gs_alloc_requests(struct usb_ep *ep, struct list_head *head,
-		void (*fn)(struct usb_ep *, struct usb_request *),
-		int *allocated)
-{
-	int			i;
-	struct usb_request	*req;
-	int n = allocated ? QUEUE_SIZE - *allocated : QUEUE_SIZE;
-
-	/* Pre-allocate up to QUEUE_SIZE transfers, but if we can't
-	 * do quite that many this time, don't fail ... we just won't
-	 * be as speedy as we might otherwise be.
-	 */
-	for (i = 0; i < n; i++) {
-		req = gs_alloc_req(ep, ep->maxpacket, GFP_ATOMIC);
-		if (!req)
-			return list_empty(head) ? -ENOMEM : 0;
-		req->complete = fn;
-		list_add_tail(&req->list, head);
-		if (allocated)
-			(*allocated)++;
-	}
-	return 0;
-}
-
-/**
- * gs_start_io - start USB I/O streams
- * @dev: encapsulates endpoints to use
- * Context: holding port_lock; port_tty and port_usb are non-null
- *
- * We only start I/O when something is connected to both sides of
- * this port.  If nothing is listening on the host side, we may
- * be pointlessly filling up our TX buffers and FIFO.
- */
-static int gs_start_io(struct gs_port *port)
-{
-	struct list_head	*head = &port->read_pool;
-	struct usb_ep		*ep = port->port_usb->out;
-	int			status;
-	unsigned		started;
-
-	/* Allocate RX and TX I/O buffers.  We can't easily do this much
-	 * earlier (with GFP_KERNEL) because the requests are coupled to
-	 * endpoints, as are the packet sizes we'll be using.  Different
-	 * configurations may use different endpoints with a given port;
-	 * and high speed vs full speed changes packet sizes too.
-	 */
-	status = gs_alloc_requests(ep, head, gs_read_complete,
-		&port->read_allocated);
-	if (status)
-		return status;
-
-	status = gs_alloc_requests(port->port_usb->in, &port->write_pool,
-			gs_write_complete, &port->write_allocated);
-	if (status) {
-		gs_free_requests(ep, head, &port->read_allocated);
-		return status;
-	}
-
-	/* queue read requests */
-	port->n_read = 0;
-	started = gs_start_rx(port);
-
-	/* unblock any pending writes into our circular buffer */
-	if (started) {
-		tty_wakeup(port->port.tty);
-	} else {
-		gs_free_requests(ep, head, &port->read_allocated);
-		gs_free_requests(port->port_usb->in, &port->write_pool,
-			&port->write_allocated);
-		status = -EIO;
-	}
-
-	return status;
-}
-
-/*-------------------------------------------------------------------------*/
-
-/* TTY Driver */
-
-/*
- * gs_open sets up the link between a gs_port and its associated TTY.
- * That link is broken *only* by TTY close(), and all driver methods
- * know that.
- */
-static int gs_open(struct tty_struct *tty, struct file *file)
-{
-	int		port_num = tty->index;
-	struct gs_port	*port;
-	int		status;
-
-	do {
-		mutex_lock(&ports[port_num].lock);
-		port = ports[port_num].port;
-		if (!port)
-			status = -ENODEV;
-		else {
-			spin_lock_irq(&port->port_lock);
-
-			/* already open?  Great. */
-			if (port->port.count) {
-				status = 0;
-				port->port.count++;
-
-			/* currently opening/closing? wait ... */
-			} else if (port->openclose) {
-				status = -EBUSY;
-
-			/* ... else we do the work */
-			} else {
-				status = -EAGAIN;
-				port->openclose = true;
-			}
-			spin_unlock_irq(&port->port_lock);
-		}
-		mutex_unlock(&ports[port_num].lock);
-
-		switch (status) {
-		default:
-			/* fully handled */
-			return status;
-		case -EAGAIN:
-			/* must do the work */
-			break;
-		case -EBUSY:
-			/* wait for EAGAIN task to finish */
-			msleep(1);
-			/* REVISIT could have a waitchannel here, if
-			 * concurrent open performance is important
-			 */
-			break;
-		}
-	} while (status != -EAGAIN);
-
-	/* Do the "real open" */
-	spin_lock_irq(&port->port_lock);
-
-	/* allocate circular buffer on first open */
-	if (port->port_write_buf.buf_buf == NULL) {
-
-		spin_unlock_irq(&port->port_lock);
-		status = gs_buf_alloc(&port->port_write_buf, WRITE_BUF_SIZE);
-		spin_lock_irq(&port->port_lock);
-
-		if (status) {
-			pr_debug("gs_open: ttyGS%d (%p,%p) no buffer\n",
-				port->port_num, tty, file);
-			port->openclose = false;
-			goto exit_unlock_port;
-		}
-	}
-
-	/* REVISIT if REMOVED (ports[].port NULL), abort the open
-	 * to let rmmod work faster (but this way isn't wrong).
-	 */
-
-	/* REVISIT maybe wait for "carrier detect" */
-
-	tty->driver_data = port;
-	port->port.tty = tty;
-
-	port->port.count = 1;
-	port->openclose = false;
-
-	/* if connected, start the I/O stream */
-	if (port->port_usb) {
-		struct gserial	*gser = port->port_usb;
-
-		pr_debug("gs_open: start ttyGS%d\n", port->port_num);
-		gs_start_io(port);
-
-		if (gser->connect)
-			gser->connect(gser);
-	}
-
-	pr_debug("gs_open: ttyGS%d (%p,%p)\n", port->port_num, tty, file);
-
-	status = 0;
-
-exit_unlock_port:
-	spin_unlock_irq(&port->port_lock);
-	return status;
-}
-
-static int gs_writes_finished(struct gs_port *p)
-{
-	int cond;
-
-	/* return true on disconnect or empty buffer */
-	spin_lock_irq(&p->port_lock);
-	cond = (p->port_usb == NULL) || !gs_buf_data_avail(&p->port_write_buf);
-	spin_unlock_irq(&p->port_lock);
-
-	return cond;
-}
-
-static void gs_close(struct tty_struct *tty, struct file *file)
-{
-	struct gs_port *port = tty->driver_data;
-	struct gserial	*gser;
-
-	spin_lock_irq(&port->port_lock);
-
-	if (port->port.count != 1) {
-		if (port->port.count == 0)
-			WARN_ON(1);
-		else
-			--port->port.count;
-		goto exit;
-	}
-
-	pr_debug("gs_close: ttyGS%d (%p,%p) ...\n", port->port_num, tty, file);
-
-	/* mark port as closing but in use; we can drop port lock
-	 * and sleep if necessary
-	 */
-	port->openclose = true;
-	port->port.count = 0;
-
-	gser = port->port_usb;
-	if (gser && gser->disconnect)
-		gser->disconnect(gser);
-
-	/* wait for circular write buffer to drain, disconnect, or at
-	 * most GS_CLOSE_TIMEOUT seconds; then discard the rest
-	 */
-	if (gs_buf_data_avail(&port->port_write_buf) > 0 && gser) {
-		spin_unlock_irq(&port->port_lock);
-		wait_event_interruptible_timeout(port->drain_wait,
-					gs_writes_finished(port),
-					GS_CLOSE_TIMEOUT * HZ);
-		spin_lock_irq(&port->port_lock);
-		gser = port->port_usb;
-	}
-
-	/* Iff we're disconnected, there can be no I/O in flight so it's
-	 * ok to free the circular buffer; else just scrub it.  And don't
-	 * let the push tasklet fire again until we're re-opened.
-	 */
-	if (gser == NULL)
-		gs_buf_free(&port->port_write_buf);
-	else
-		gs_buf_clear(&port->port_write_buf);
-
-	tty->driver_data = NULL;
-	port->port.tty = NULL;
-
-	port->openclose = false;
-
-	pr_debug("gs_close: ttyGS%d (%p,%p) done!\n",
-			port->port_num, tty, file);
-
-	wake_up_interruptible(&port->port.close_wait);
-exit:
-	spin_unlock_irq(&port->port_lock);
-}
-
-static int gs_write(struct tty_struct *tty, const unsigned char *buf, int count)
-{
-	struct gs_port	*port = tty->driver_data;
-	unsigned long	flags;
-	int		status;
-
-	pr_vdebug("gs_write: ttyGS%d (%p) writing %d bytes\n",
-			port->port_num, tty, count);
-
-	spin_lock_irqsave(&port->port_lock, flags);
-	if (count)
-		count = gs_buf_put(&port->port_write_buf, buf, count);
-	/* treat count == 0 as flush_chars() */
-	if (port->port_usb)
-		status = gs_start_tx(port);
-	spin_unlock_irqrestore(&port->port_lock, flags);
-
-	return count;
-}
-
-static int gs_put_char(struct tty_struct *tty, unsigned char ch)
-{
-	struct gs_port	*port = tty->driver_data;
-	unsigned long	flags;
-	int		status;
-
-	pr_vdebug("gs_put_char: (%d,%p) char=0x%x, called from %pf\n",
-		port->port_num, tty, ch, __builtin_return_address(0));
-
-	spin_lock_irqsave(&port->port_lock, flags);
-	status = gs_buf_put(&port->port_write_buf, &ch, 1);
-	spin_unlock_irqrestore(&port->port_lock, flags);
-
-	return status;
-}
-
-static void gs_flush_chars(struct tty_struct *tty)
-{
-	struct gs_port	*port = tty->driver_data;
-	unsigned long	flags;
-
-	pr_vdebug("gs_flush_chars: (%d,%p)\n", port->port_num, tty);
-
-	spin_lock_irqsave(&port->port_lock, flags);
-	if (port->port_usb)
-		gs_start_tx(port);
-	spin_unlock_irqrestore(&port->port_lock, flags);
-}
-
-static int gs_write_room(struct tty_struct *tty)
-{
-	struct gs_port	*port = tty->driver_data;
-	unsigned long	flags;
-	int		room = 0;
-
-	spin_lock_irqsave(&port->port_lock, flags);
-	if (port->port_usb)
-		room = gs_buf_space_avail(&port->port_write_buf);
-	spin_unlock_irqrestore(&port->port_lock, flags);
-
-	pr_vdebug("gs_write_room: (%d,%p) room=%d\n",
-		port->port_num, tty, room);
-
-	return room;
-}
-
-static int gs_chars_in_buffer(struct tty_struct *tty)
-{
-	struct gs_port	*port = tty->driver_data;
-	unsigned long	flags;
-	int		chars = 0;
-
-	spin_lock_irqsave(&port->port_lock, flags);
-	chars = gs_buf_data_avail(&port->port_write_buf);
-	spin_unlock_irqrestore(&port->port_lock, flags);
-
-	pr_vdebug("gs_chars_in_buffer: (%d,%p) chars=%d\n",
-		port->port_num, tty, chars);
-
-	return chars;
-}
-
-/* undo side effects of setting TTY_THROTTLED */
-static void gs_unthrottle(struct tty_struct *tty)
-{
-	struct gs_port		*port = tty->driver_data;
-	unsigned long		flags;
-
-	spin_lock_irqsave(&port->port_lock, flags);
-	if (port->port_usb) {
-		/* Kickstart read queue processing.  We don't do xon/xoff,
-		 * rts/cts, or other handshaking with the host, but if the
-		 * read queue backs up enough we'll be NAKing OUT packets.
-		 */
-		tasklet_schedule(&port->push);
-		pr_vdebug(PREFIX "%d: unthrottle\n", port->port_num);
-	}
-	spin_unlock_irqrestore(&port->port_lock, flags);
-}
-
-static int gs_break_ctl(struct tty_struct *tty, int duration)
-{
-	struct gs_port	*port = tty->driver_data;
-	int		status = 0;
-	struct gserial	*gser;
-
-	pr_vdebug("gs_break_ctl: ttyGS%d, send break (%d) \n",
-			port->port_num, duration);
-
-	spin_lock_irq(&port->port_lock);
-	gser = port->port_usb;
-	if (gser && gser->send_break)
-		status = gser->send_break(gser, duration);
-	spin_unlock_irq(&port->port_lock);
-
-	return status;
-}
-
-static const struct tty_operations gs_tty_ops = {
-	.open =			gs_open,
-	.close =		gs_close,
-	.write =		gs_write,
-	.put_char =		gs_put_char,
-	.flush_chars =		gs_flush_chars,
-	.write_room =		gs_write_room,
-	.chars_in_buffer =	gs_chars_in_buffer,
-	.unthrottle =		gs_unthrottle,
-	.break_ctl =		gs_break_ctl,
-};
-
-/*-------------------------------------------------------------------------*/
-
-static struct tty_driver *gs_tty_driver;
-
-static int
-gs_port_alloc(unsigned port_num, struct usb_cdc_line_coding *coding)
-{
-	struct gs_port	*port;
-
-	port = kzalloc(sizeof(struct gs_port), GFP_KERNEL);
-	if (port == NULL)
-		return -ENOMEM;
-
-	tty_port_init(&port->port);
-	spin_lock_init(&port->port_lock);
-	init_waitqueue_head(&port->drain_wait);
-
-	tasklet_init(&port->push, gs_rx_push, (unsigned long) port);
-
-	INIT_LIST_HEAD(&port->read_pool);
-	INIT_LIST_HEAD(&port->read_queue);
-	INIT_LIST_HEAD(&port->write_pool);
-
-	port->port_num = port_num;
-	port->port_line_coding = *coding;
-
-	ports[port_num].port = port;
-
-	return 0;
-}
-
-/**
- * gserial_setup - initialize TTY driver for one or more ports
- * @g: gadget to associate with these ports
- * @count: how many ports to support
- * Context: may sleep
- *
- * The TTY stack needs to know in advance how many devices it should
- * plan to manage.  Use this call to set up the ports you will be
- * exporting through USB.  Later, connect them to functions based
- * on what configuration is activated by the USB host; and disconnect
- * them as appropriate.
- *
- * An example would be a two-configuration device in which both
- * configurations expose port 0, but through different functions.
- * One configuration could even expose port 1 while the other
- * one doesn't.
- *
- * Returns negative errno or zero.
- */
-int gserial_setup(struct usb_gadget *g, unsigned count)
-{
-	unsigned			i;
-	struct usb_cdc_line_coding	coding;
-	int				status;
-
-	if (count == 0 || count > N_PORTS)
-		return -EINVAL;
-
-	gs_tty_driver = alloc_tty_driver(count);
-	if (!gs_tty_driver)
-		return -ENOMEM;
-
-	gs_tty_driver->driver_name = "g_serial";
-	gs_tty_driver->name = PREFIX;
-	/* uses dynamically assigned dev_t values */
-
-	gs_tty_driver->type = TTY_DRIVER_TYPE_SERIAL;
-	gs_tty_driver->subtype = SERIAL_TYPE_NORMAL;
-	gs_tty_driver->flags = TTY_DRIVER_REAL_RAW | TTY_DRIVER_DYNAMIC_DEV;
-	gs_tty_driver->init_termios = tty_std_termios;
-
-	/* 9600-8-N-1 ... matches defaults expected by "usbser.sys" on
-	 * MS-Windows.  Otherwise, most of these flags shouldn't affect
-	 * anything unless we were to actually hook up to a serial line.
-	 */
-	gs_tty_driver->init_termios.c_cflag =
-			B9600 | CS8 | CREAD | HUPCL | CLOCAL;
-	gs_tty_driver->init_termios.c_ispeed = 9600;
-	gs_tty_driver->init_termios.c_ospeed = 9600;
-
-	coding.dwDTERate = cpu_to_le32(9600);
-	coding.bCharFormat = 8;
-	coding.bParityType = USB_CDC_NO_PARITY;
-	coding.bDataBits = USB_CDC_1_STOP_BITS;
-
-	tty_set_operations(gs_tty_driver, &gs_tty_ops);
-
-	/* make devices be openable */
-	for (i = 0; i < count; i++) {
-		mutex_init(&ports[i].lock);
-		status = gs_port_alloc(i, &coding);
-		if (status) {
-			count = i;
-			goto fail;
-		}
-	}
-	n_ports = count;
-
-	/* export the driver ... */
-	status = tty_register_driver(gs_tty_driver);
-	if (status) {
-		pr_err("%s: cannot register, err %d\n",
-				__func__, status);
-		goto fail;
-	}
-
-	/* ... and sysfs class devices, so mdev/udev make /dev/ttyGS* */
-	for (i = 0; i < count; i++) {
-		struct device	*tty_dev;
-
-		tty_dev = tty_register_device(gs_tty_driver, i, &g->dev);
-		if (IS_ERR(tty_dev))
-			pr_warning("%s: no classdev for port %d, err %ld\n",
-				__func__, i, PTR_ERR(tty_dev));
-	}
-
-	pr_debug("%s: registered %d ttyGS* device%s\n", __func__,
-			count, (count == 1) ? "" : "s");
-
-	return status;
-fail:
-	while (count--) {
-		tty_port_destroy(&ports[count].port->port);
-		kfree(ports[count].port);
-	}
-	put_tty_driver(gs_tty_driver);
-	gs_tty_driver = NULL;
-	return status;
-}
-
-static int gs_closed(struct gs_port *port)
-{
-	int cond;
-
-	spin_lock_irq(&port->port_lock);
-	cond = (port->port.count == 0) && !port->openclose;
-	spin_unlock_irq(&port->port_lock);
-	return cond;
-}
-
-/**
- * gserial_cleanup - remove TTY-over-USB driver and devices
- * Context: may sleep
- *
- * This is called to free all resources allocated by @gserial_setup().
- * Accordingly, it may need to wait until some open /dev/ files have
- * closed.
- *
- * The caller must have issued @gserial_disconnect() for any ports
- * that had previously been connected, so that there is never any
- * I/O pending when it's called.
- */
-void gserial_cleanup(void)
-{
-	unsigned	i;
-	struct gs_port	*port;
-
-	if (!gs_tty_driver)
-		return;
-
-	/* start sysfs and /dev/ttyGS* node removal */
-	for (i = 0; i < n_ports; i++)
-		tty_unregister_device(gs_tty_driver, i);
-
-	for (i = 0; i < n_ports; i++) {
-		/* prevent new opens */
-		mutex_lock(&ports[i].lock);
-		port = ports[i].port;
-		ports[i].port = NULL;
-		mutex_unlock(&ports[i].lock);
-
-		tasklet_kill(&port->push);
-
-		/* wait for old opens to finish */
-		wait_event(port->port.close_wait, gs_closed(port));
-
-		WARN_ON(port->port_usb != NULL);
-
-		tty_port_destroy(&port->port);
-		kfree(port);
-	}
-	n_ports = 0;
-
-	tty_unregister_driver(gs_tty_driver);
-	put_tty_driver(gs_tty_driver);
-	gs_tty_driver = NULL;
-
-	pr_debug("%s: cleaned up ttyGS* support\n", __func__);
-}
-
-/**
- * gserial_connect - notify TTY I/O glue that USB link is active
- * @gser: the function, set up with endpoints and descriptors
- * @port_num: which port is active
- * Context: any (usually from irq)
- *
- * This is called activate endpoints and let the TTY layer know that
- * the connection is active ... not unlike "carrier detect".  It won't
- * necessarily start I/O queues; unless the TTY is held open by any
- * task, there would be no point.  However, the endpoints will be
- * activated so the USB host can perform I/O, subject to basic USB
- * hardware flow control.
- *
- * Caller needs to have set up the endpoints and USB function in @dev
- * before calling this, as well as the appropriate (speed-specific)
- * endpoint descriptors, and also have set up the TTY driver by calling
- * @gserial_setup().
- *
- * Returns negative errno or zero.
- * On success, ep->driver_data will be overwritten.
- */
-int gserial_connect(struct gserial *gser, u8 port_num)
-{
-	struct gs_port	*port;
-	unsigned long	flags;
-	int		status;
-
-	if (!gs_tty_driver || port_num >= n_ports)
-		return -ENXIO;
-
-	/* we "know" gserial_cleanup() hasn't been called */
-	port = ports[port_num].port;
-
-	/* activate the endpoints */
-	status = usb_ep_enable(gser->in);
-	if (status < 0)
-		return status;
-	gser->in->driver_data = port;
-
-	status = usb_ep_enable(gser->out);
-	if (status < 0)
-		goto fail_out;
-	gser->out->driver_data = port;
-
-	/* then tell the tty glue that I/O can work */
-	spin_lock_irqsave(&port->port_lock, flags);
-	gser->ioport = port;
-	port->port_usb = gser;
-
-	/* REVISIT unclear how best to handle this state...
-	 * we don't really couple it with the Linux TTY.
-	 */
-	gser->port_line_coding = port->port_line_coding;
-
-	/* REVISIT if waiting on "carrier detect", signal. */
-
-	/* if it's already open, start I/O ... and notify the serial
-	 * protocol about open/close status (connect/disconnect).
-	 */
-	if (port->port.count) {
-		pr_debug("gserial_connect: start ttyGS%d\n", port->port_num);
-		gs_start_io(port);
-		if (gser->connect)
-			gser->connect(gser);
-	} else {
-		if (gser->disconnect)
-			gser->disconnect(gser);
-	}
-
-	spin_unlock_irqrestore(&port->port_lock, flags);
-
-	return status;
-
-fail_out:
-	usb_ep_disable(gser->in);
-	gser->in->driver_data = NULL;
-	return status;
-}
-
-/**
- * gserial_disconnect - notify TTY I/O glue that USB link is inactive
- * @gser: the function, on which gserial_connect() was called
- * Context: any (usually from irq)
- *
- * This is called to deactivate endpoints and let the TTY layer know
- * that the connection went inactive ... not unlike "hangup".
- *
- * On return, the state is as if gserial_connect() had never been called;
- * there is no active USB I/O on these endpoints.
- */
-void gserial_disconnect(struct gserial *gser)
-{
-	struct gs_port	*port = gser->ioport;
-	unsigned long	flags;
-
-	if (!port)
-		return;
-
-	/* tell the TTY glue not to do I/O here any more */
-	spin_lock_irqsave(&port->port_lock, flags);
-
-	/* REVISIT as above: how best to track this? */
-	port->port_line_coding = gser->port_line_coding;
-
-	port->port_usb = NULL;
-	gser->ioport = NULL;
-	if (port->port.count > 0 || port->openclose) {
-		wake_up_interruptible(&port->drain_wait);
-		if (port->port.tty)
-			tty_hangup(port->port.tty);
-	}
-	spin_unlock_irqrestore(&port->port_lock, flags);
-
-	/* disable endpoints, aborting down any active I/O */
-	usb_ep_disable(gser->out);
-	gser->out->driver_data = NULL;
-
-	usb_ep_disable(gser->in);
-	gser->in->driver_data = NULL;
-
-	/* finally, free any unused/unusable I/O buffers */
-	spin_lock_irqsave(&port->port_lock, flags);
-	if (port->port.count == 0 && !port->openclose)
-		gs_buf_free(&port->port_write_buf);
-	gs_free_requests(gser->out, &port->read_pool, NULL);
-	gs_free_requests(gser->out, &port->read_queue, NULL);
-	gs_free_requests(gser->in, &port->write_pool, NULL);
-
-	port->read_allocated = port->read_started =
-		port->write_allocated = port->write_started = 0;
-
-	spin_unlock_irqrestore(&port->port_lock, flags);
-}
diff --git a/drivers/staging/ccg/u_serial.h b/drivers/staging/ccg/u_serial.h
deleted file mode 100644
index 9b0fe6450fb..00000000000
--- a/drivers/staging/ccg/u_serial.h
+++ /dev/null
@@ -1,65 +0,0 @@
-/*
- * u_serial.h - interface to USB gadget "serial port"/TTY utilities
- *
- * Copyright (C) 2008 David Brownell
- * Copyright (C) 2008 by Nokia Corporation
- *
- * This software is distributed under the terms of the GNU General
- * Public License ("GPL") as published by the Free Software Foundation,
- * either version 2 of that License or (at your option) any later version.
- */
-
-#ifndef __U_SERIAL_H
-#define __U_SERIAL_H
-
-#include <linux/usb/composite.h>
-#include <linux/usb/cdc.h>
-
-/*
- * One non-multiplexed "serial" I/O port ... there can be several of these
- * on any given USB peripheral device, if it provides enough endpoints.
- *
- * The "u_serial" utility component exists to do one thing:  manage TTY
- * style I/O using the USB peripheral endpoints listed here, including
- * hookups to sysfs and /dev for each logical "tty" device.
- *
- * REVISIT at least ACM could support tiocmget() if needed.
- *
- * REVISIT someday, allow multiplexing several TTYs over these endpoints.
- */
-struct gserial {
-	struct usb_function		func;
-
-	/* port is managed by gserial_{connect,disconnect} */
-	struct gs_port			*ioport;
-
-	struct usb_ep			*in;
-	struct usb_ep			*out;
-
-	/* REVISIT avoid this CDC-ACM support harder ... */
-	struct usb_cdc_line_coding port_line_coding;	/* 9600-8-N-1 etc */
-
-	/* notification callbacks */
-	void (*connect)(struct gserial *p);
-	void (*disconnect)(struct gserial *p);
-	int (*send_break)(struct gserial *p, int duration);
-};
-
-/* utilities to allocate/free request and buffer */
-struct usb_request *gs_alloc_req(struct usb_ep *ep, unsigned len, gfp_t flags);
-void gs_free_req(struct usb_ep *, struct usb_request *req);
-
-/* port setup/teardown is handled by gadget driver */
-int gserial_setup(struct usb_gadget *g, unsigned n_ports);
-void gserial_cleanup(void);
-
-/* connect/disconnect is handled by individual functions */
-int gserial_connect(struct gserial *, u8 port_num);
-void gserial_disconnect(struct gserial *);
-
-/* functions are bound to configurations by a config or gadget driver */
-int acm_bind_config(struct usb_configuration *c, u8 port_num);
-int gser_bind_config(struct usb_configuration *c, u8 port_num);
-int obex_bind_config(struct usb_configuration *c, u8 port_num);
-
-#endif /* __U_SERIAL_H */
diff --git a/drivers/staging/ccg/usbstring.c b/drivers/staging/ccg/usbstring.c
deleted file mode 100644
index 4d25b9009ed..00000000000
--- a/drivers/staging/ccg/usbstring.c
+++ /dev/null
@@ -1,71 +0,0 @@
-/*
- * Copyright (C) 2003 David Brownell
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU Lesser General Public License as published
- * by the Free Software Foundation; either version 2.1 of the License, or
- * (at your option) any later version.
- */
-
-#include <linux/errno.h>
-#include <linux/kernel.h>
-#include <linux/list.h>
-#include <linux/string.h>
-#include <linux/device.h>
-#include <linux/init.h>
-#include <linux/nls.h>
-
-#include <linux/usb/ch9.h>
-#include <linux/usb/gadget.h>
-
-
-/**
- * usb_gadget_get_string - fill out a string descriptor 
- * @table: of c strings encoded using UTF-8
- * @id: string id, from low byte of wValue in get string descriptor
- * @buf: at least 256 bytes, must be 16-bit aligned
- *
- * Finds the UTF-8 string matching the ID, and converts it into a
- * string descriptor in utf16-le.
- * Returns length of descriptor (always even) or negative errno
- *
- * If your driver needs stings in multiple languages, you'll probably
- * "switch (wIndex) { ... }"  in your ep0 string descriptor logic,
- * using this routine after choosing which set of UTF-8 strings to use.
- * Note that US-ASCII is a strict subset of UTF-8; any string bytes with
- * the eighth bit set will be multibyte UTF-8 characters, not ISO-8859/1
- * characters (which are also widely used in C strings).
- */
-int
-usb_gadget_get_string (struct usb_gadget_strings *table, int id, u8 *buf)
-{
-	struct usb_string	*s;
-	int			len;
-
-	/* descriptor 0 has the language id */
-	if (id == 0) {
-		buf [0] = 4;
-		buf [1] = USB_DT_STRING;
-		buf [2] = (u8) table->language;
-		buf [3] = (u8) (table->language >> 8);
-		return 4;
-	}
-	for (s = table->strings; s && s->s; s++)
-		if (s->id == id)
-			break;
-
-	/* unrecognized: stall. */
-	if (!s || !s->s)
-		return -EINVAL;
-
-	/* string descriptors have length, tag, then UTF16-LE text */
-	len = min ((size_t) 126, strlen (s->s));
-	len = utf8s_to_utf16s(s->s, len, UTF16_LITTLE_ENDIAN,
-			(wchar_t *) &buf[2], 126);
-	if (len < 0)
-		return -EINVAL;
-	buf [0] = (len + 1) * 2;
-	buf [1] = USB_DT_STRING;
-	return buf [0];
-}
-
diff --git a/drivers/staging/comedi/Kconfig b/drivers/staging/comedi/Kconfig
index 1967852eeb1..7871579bb83 100644
--- a/drivers/staging/comedi/Kconfig
+++ b/drivers/staging/comedi/Kconfig
@@ -230,9 +230,9 @@ config COMEDI_AMPLC_PC236_ISA
 
 config COMEDI_AMPLC_PC263_ISA
 	tristate "Amplicon PC263 relay board support"
-	select COMEDI_AMPLC_PC263
 	---help---
-	  Enable support for Amplicon PC263 ISA relay board.
+	  Enable support for Amplicon PC263 ISA relay board.  This board has
+	  16 reed relay output channels.
 
 	  To compile this driver as a module, choose M here: the module will be
 	  called amplc_pc263.
@@ -484,6 +484,19 @@ config COMEDI_NI_ATMIO16D
 	  To compile this driver as a module, choose M here: the module will be
 	  called ni_atmio16d.
 
+config COMEDI_NI_LABPC_ISA
+	tristate "NI Lab-PC and compatibles ISA support"
+	select COMEDI_NI_LABPC
+	depends on VIRT_TO_BUS
+	---help---
+	  Enable support for National Instruments Lab-PC and compatibles
+	  Lab-PC-1200, Lab-PC-1200AI, Lab-PC+.
+	  Kernel-level ISA plug-and-play support for the lab-pc-1200 boards has
+	  not yet been added to the driver.
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called ni_labpc.
+
 config COMEDI_PCMAD
 	tristate "Winsystems PCM-A/D12 and PCM-A/D16 PC/104 board support"
 	---help---
@@ -728,6 +741,16 @@ config COMEDI_ADV_PCI1723
 	  To compile this driver as a module, choose M here: the module will be
 	  called adv_pci1723.
 
+config COMEDI_ADV_PCI1724
+	tristate "Advantech PCI-1724U support"
+	---help---
+	  Enable support for Advantech PCI-1724U cards.  These are 32-channel
+	  analog output cards with voltage and current loop output ranges and
+	  14-bit resolution.
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called adv_pci1724.
+
 config COMEDI_ADV_PCI_DIO
 	tristate "Advantech PCI DIO card support"
 	select COMEDI_8255
@@ -748,7 +771,7 @@ config COMEDI_AMPLC_DIO200_PCI
 	  and PCIe296 DIO boards.
 
 	  To compile this driver as a module, choose M here: the module will be
-	  called amplc_dio200.
+	  called amplc_dio200_pci.
 
 config COMEDI_AMPLC_PC236_PCI
 	tristate "Amplicon PCI236 DIO board support"
@@ -761,12 +784,12 @@ config COMEDI_AMPLC_PC236_PCI
 
 config COMEDI_AMPLC_PC263_PCI
 	tristate "Amplicon PCI263 relay board support"
-	select COMEDI_AMPLC_PC263
 	---help---
-	  Enable support for Amplicon PCI263 relay board.
+	  Enable support for Amplicon PCI263 relay board.  This is a PCI board
+	  with 16 reed relay output channels.
 
 	  To compile this driver as a module, choose M here: the module will be
-	  called amplc_pc263.
+	  called amplc_pci263.
 
 config COMEDI_AMPLC_PCI224
 	tristate "Amplicon PCI224 and PCI234 support"
@@ -983,7 +1006,7 @@ config COMEDI_NI_660X
 	select COMEDI_NI_TIOCMD
 	---help---
 	  Enable support for National Instruments PCI-6601 (ni_660x), PCI-6602,
-	  PXI-6602 and PXI-6608.
+	  PXI-6602, PXI-6608 and PXI-6624.
 
 	  To compile this driver as a module, choose M here: the module will be
 	  called ni_660x.
@@ -997,20 +1020,15 @@ config COMEDI_NI_670X
 	  To compile this driver as a module, choose M here: the module will be
 	  called ni_670x.
 
-config COMEDI_NI_LABPC
-	tristate "NI Lab-PC and compatibles ISA and PCI support"
+config COMEDI_NI_LABPC_PCI
+	tristate "NI Lab-PC PCI-1200 support"
+	select COMEDI_NI_LABPC
 	select COMEDI_MITE
-	select COMEDI_8255
-	select COMEDI_FC
-	depends on VIRT_TO_BUS
 	---help---
-	  Enable support for National Instruments Lab-PC and compatibles
-	  Lab-PC-1200, Lab-PC-1200AI, Lab-PC+ and PCI-1200.
-	  Kernel-level ISA plug-and-play support for the lab-pc-1200 boards has
-	  not yet been added to the driver.
+	  Enable support for National Instruments Lab-PC PCI-1200.
 
 	  To compile this driver as a module, choose M here: the module will be
-	  called ni_labpc.
+	  called ni_labpc_pci.
 
 config COMEDI_NI_PCIDIO
 	tristate "NI PCI-DIO32HS, PCI-6533, PCI-6534 support"
@@ -1132,7 +1150,7 @@ config COMEDI_NI_DAQ_DIO24_CS
 
 config COMEDI_NI_LABPC_CS
 	tristate "NI DAQCard-1200 PCMCIA support"
-	depends on COMEDI_NI_LABPC
+	select COMEDI_NI_LABPC
 	---help---
 	  Enable support for the National Instruments PCMCIA DAQCard-1200
 
@@ -1241,12 +1259,14 @@ config COMEDI_AMPLC_PC236
 	tristate
 	select COMEDI_8255
 
-config COMEDI_AMPLC_PC263
+config COMEDI_DAS08
 	tristate
+	select COMEDI_8255
 
-config COMEDI_DAS08
+config COMEDI_NI_LABPC
 	tristate
 	select COMEDI_8255
+	select COMEDI_FC
 
 config COMEDI_NI_TIO
 	tristate
diff --git a/drivers/staging/comedi/comedi_buf.c b/drivers/staging/comedi/comedi_buf.c
index 9b997ae6779..ca709901fb3 100644
--- a/drivers/staging/comedi/comedi_buf.c
+++ b/drivers/staging/comedi/comedi_buf.c
@@ -192,7 +192,7 @@ unsigned int comedi_buf_write_alloc(struct comedi_async *async,
 {
 	return __comedi_buf_write_alloc(async, nbytes, 0);
 }
-EXPORT_SYMBOL(comedi_buf_write_alloc);
+EXPORT_SYMBOL_GPL(comedi_buf_write_alloc);
 
 /*
  * munging is applied to data by core as it passes between user
@@ -263,7 +263,7 @@ unsigned int comedi_buf_write_free(struct comedi_async *async,
 
 	return nbytes;
 }
-EXPORT_SYMBOL(comedi_buf_write_free);
+EXPORT_SYMBOL_GPL(comedi_buf_write_free);
 
 unsigned int comedi_buf_read_n_available(struct comedi_async *async)
 {
@@ -282,7 +282,7 @@ unsigned int comedi_buf_read_n_available(struct comedi_async *async)
 
 	return num_bytes;
 }
-EXPORT_SYMBOL(comedi_buf_read_n_available);
+EXPORT_SYMBOL_GPL(comedi_buf_read_n_available);
 
 /* allocates a chunk for the reader from filled (and munged) buffer space */
 unsigned int comedi_buf_read_alloc(struct comedi_async *async,
@@ -304,7 +304,7 @@ unsigned int comedi_buf_read_alloc(struct comedi_async *async,
 
 	return nbytes;
 }
-EXPORT_SYMBOL(comedi_buf_read_alloc);
+EXPORT_SYMBOL_GPL(comedi_buf_read_alloc);
 
 static unsigned int comedi_buf_read_n_allocated(struct comedi_async *async)
 {
@@ -332,7 +332,7 @@ unsigned int comedi_buf_read_free(struct comedi_async *async,
 	async->buf_read_ptr %= async->prealloc_bufsz;
 	return nbytes;
 }
-EXPORT_SYMBOL(comedi_buf_read_free);
+EXPORT_SYMBOL_GPL(comedi_buf_read_free);
 
 int comedi_buf_put(struct comedi_async *async, short x)
 {
@@ -346,7 +346,7 @@ int comedi_buf_put(struct comedi_async *async, short x)
 	comedi_buf_write_free(async, sizeof(short));
 	return 1;
 }
-EXPORT_SYMBOL(comedi_buf_put);
+EXPORT_SYMBOL_GPL(comedi_buf_put);
 
 int comedi_buf_get(struct comedi_async *async, short *x)
 {
@@ -359,7 +359,7 @@ int comedi_buf_get(struct comedi_async *async, short *x)
 	comedi_buf_read_free(async, sizeof(short));
 	return 1;
 }
-EXPORT_SYMBOL(comedi_buf_get);
+EXPORT_SYMBOL_GPL(comedi_buf_get);
 
 void comedi_buf_memcpy_to(struct comedi_async *async, unsigned int offset,
 			  const void *data, unsigned int num_bytes)
@@ -385,7 +385,7 @@ void comedi_buf_memcpy_to(struct comedi_async *async, unsigned int offset,
 		write_ptr = 0;
 	}
 }
-EXPORT_SYMBOL(comedi_buf_memcpy_to);
+EXPORT_SYMBOL_GPL(comedi_buf_memcpy_to);
 
 void comedi_buf_memcpy_from(struct comedi_async *async, unsigned int offset,
 			    void *dest, unsigned int nbytes)
@@ -412,4 +412,4 @@ void comedi_buf_memcpy_from(struct comedi_async *async, unsigned int offset,
 		read_ptr = 0;
 	}
 }
-EXPORT_SYMBOL(comedi_buf_memcpy_from);
+EXPORT_SYMBOL_GPL(comedi_buf_memcpy_from);
diff --git a/drivers/staging/comedi/comedi_fops.c b/drivers/staging/comedi/comedi_fops.c
index e336b281b84..00f2547024e 100644
--- a/drivers/staging/comedi/comedi_fops.c
+++ b/drivers/staging/comedi/comedi_fops.c
@@ -49,9 +49,13 @@
 
 #include "comedi_internal.h"
 
+#define COMEDI_NUM_MINORS 0x100
+#define COMEDI_NUM_SUBDEVICE_MINORS	\
+	(COMEDI_NUM_MINORS - COMEDI_NUM_BOARD_MINORS)
+
 #ifdef CONFIG_COMEDI_DEBUG
 int comedi_debug;
-EXPORT_SYMBOL(comedi_debug);
+EXPORT_SYMBOL_GPL(comedi_debug);
 module_param(comedi_debug, int, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(comedi_debug,
 		 "enable comedi core and driver debugging if non-zero (default 0)"
@@ -77,57 +81,151 @@ MODULE_PARM_DESC(comedi_default_buf_maxsize_kb,
 		 "default maximum size of asynchronous buffer in KiB (default "
 		 __MODULE_STRING(CONFIG_COMEDI_DEFAULT_BUF_MAXSIZE_KB) ")");
 
-struct comedi_file_info {
-	struct comedi_device *device;
-	struct comedi_subdevice *read_subdevice;
-	struct comedi_subdevice *write_subdevice;
-	struct device *hardware_device;
-};
+static DEFINE_MUTEX(comedi_board_minor_table_lock);
+static struct comedi_device
+*comedi_board_minor_table[COMEDI_NUM_BOARD_MINORS];
+
+static DEFINE_MUTEX(comedi_subdevice_minor_table_lock);
+/* Note: indexed by minor - COMEDI_NUM_BOARD_MINORS. */
+static struct comedi_subdevice
+*comedi_subdevice_minor_table[COMEDI_NUM_SUBDEVICE_MINORS];
+
+static struct class *comedi_class;
+static struct cdev comedi_cdev;
+
+static void comedi_device_init(struct comedi_device *dev)
+{
+	spin_lock_init(&dev->spinlock);
+	mutex_init(&dev->mutex);
+	dev->minor = -1;
+}
+
+static void comedi_device_cleanup(struct comedi_device *dev)
+{
+	struct module *driver_module = NULL;
+
+	if (dev == NULL)
+		return;
+	mutex_lock(&dev->mutex);
+	if (dev->attached)
+		driver_module = dev->driver->module;
+	comedi_device_detach(dev);
+	while (dev->use_count > 0) {
+		if (driver_module)
+			module_put(driver_module);
+		module_put(THIS_MODULE);
+		dev->use_count--;
+	}
+	mutex_unlock(&dev->mutex);
+	mutex_destroy(&dev->mutex);
+}
+
+static bool comedi_clear_board_dev(struct comedi_device *dev)
+{
+	unsigned int i = dev->minor;
+	bool cleared = false;
+
+	mutex_lock(&comedi_board_minor_table_lock);
+	if (dev == comedi_board_minor_table[i]) {
+		comedi_board_minor_table[i] = NULL;
+		cleared = true;
+	}
+	mutex_unlock(&comedi_board_minor_table_lock);
+	return cleared;
+}
+
+static struct comedi_device *comedi_clear_board_minor(unsigned minor)
+{
+	struct comedi_device *dev;
+
+	mutex_lock(&comedi_board_minor_table_lock);
+	dev = comedi_board_minor_table[minor];
+	comedi_board_minor_table[minor] = NULL;
+	mutex_unlock(&comedi_board_minor_table_lock);
+	return dev;
+}
+
+static void comedi_free_board_dev(struct comedi_device *dev)
+{
+	if (dev) {
+		if (dev->class_dev) {
+			device_destroy(comedi_class,
+				       MKDEV(COMEDI_MAJOR, dev->minor));
+		}
+		comedi_device_cleanup(dev);
+		kfree(dev);
+	}
+}
 
-static DEFINE_SPINLOCK(comedi_file_info_table_lock);
-static struct comedi_file_info *comedi_file_info_table[COMEDI_NUM_MINORS];
+static struct comedi_subdevice
+*comedi_subdevice_from_minor(unsigned minor)
+{
+	struct comedi_subdevice *s;
+	unsigned int i = minor - COMEDI_NUM_BOARD_MINORS;
+
+	BUG_ON(i >= COMEDI_NUM_SUBDEVICE_MINORS);
+	mutex_lock(&comedi_subdevice_minor_table_lock);
+	s = comedi_subdevice_minor_table[i];
+	mutex_unlock(&comedi_subdevice_minor_table_lock);
+	return s;
+}
 
-static struct comedi_file_info *comedi_file_info_from_minor(unsigned minor)
+static struct comedi_device *comedi_dev_from_board_minor(unsigned minor)
 {
-	struct comedi_file_info *info;
+	struct comedi_device *dev;
 
-	BUG_ON(minor >= COMEDI_NUM_MINORS);
-	spin_lock(&comedi_file_info_table_lock);
-	info = comedi_file_info_table[minor];
-	spin_unlock(&comedi_file_info_table_lock);
-	return info;
+	BUG_ON(minor >= COMEDI_NUM_BOARD_MINORS);
+	mutex_lock(&comedi_board_minor_table_lock);
+	dev = comedi_board_minor_table[minor];
+	mutex_unlock(&comedi_board_minor_table_lock);
+	return dev;
 }
 
-static struct comedi_device *
-comedi_dev_from_file_info(struct comedi_file_info *info)
+static struct comedi_device *comedi_dev_from_subdevice_minor(unsigned minor)
 {
-	return info ? info->device : NULL;
+	struct comedi_subdevice *s;
+
+	s = comedi_subdevice_from_minor(minor);
+	return s ? s->device : NULL;
 }
 
 struct comedi_device *comedi_dev_from_minor(unsigned minor)
 {
-	return comedi_dev_from_file_info(comedi_file_info_from_minor(minor));
+	if (minor < COMEDI_NUM_BOARD_MINORS)
+		return comedi_dev_from_board_minor(minor);
+	else
+		return comedi_dev_from_subdevice_minor(minor);
 }
 EXPORT_SYMBOL_GPL(comedi_dev_from_minor);
 
 static struct comedi_subdevice *
-comedi_read_subdevice(const struct comedi_file_info *info)
+comedi_read_subdevice(const struct comedi_device *dev, unsigned int minor)
 {
-	if (info->read_subdevice)
-		return info->read_subdevice;
-	if (info->device)
-		return info->device->read_subdev;
-	return NULL;
+	struct comedi_subdevice *s;
+
+	if (minor >= COMEDI_NUM_BOARD_MINORS) {
+		s = comedi_subdevice_from_minor(minor);
+		if (!s || s->device != dev)
+			return NULL;
+		if (s->subdev_flags & SDF_CMD_READ)
+			return s;
+	}
+	return dev->read_subdev;
 }
 
 static struct comedi_subdevice *
-comedi_write_subdevice(const struct comedi_file_info *info)
+comedi_write_subdevice(const struct comedi_device *dev, unsigned int minor)
 {
-	if (info->write_subdevice)
-		return info->write_subdevice;
-	if (info->device)
-		return info->device->write_subdev;
-	return NULL;
+	struct comedi_subdevice *s;
+
+	if (minor >= COMEDI_NUM_BOARD_MINORS) {
+		s = comedi_subdevice_from_minor(minor);
+		if (!s || s->device != dev)
+			return NULL;
+		if (s->subdev_flags & SDF_CMD_WRITE)
+			return s;
+	}
+	return dev->write_subdev;
 }
 
 static int resize_async_buffer(struct comedi_device *dev,
@@ -172,27 +270,34 @@ static int resize_async_buffer(struct comedi_device *dev,
 
 /* sysfs attribute files */
 
-static ssize_t show_max_read_buffer_kb(struct device *dev,
+static ssize_t show_max_read_buffer_kb(struct device *csdev,
 				       struct device_attribute *attr, char *buf)
 {
-	struct comedi_file_info *info = dev_get_drvdata(dev);
-	struct comedi_subdevice *s = comedi_read_subdevice(info);
+	unsigned int minor = MINOR(csdev->devt);
+	struct comedi_device *dev;
+	struct comedi_subdevice *s;
 	unsigned int size = 0;
 
-	mutex_lock(&info->device->mutex);
+	dev = comedi_dev_from_minor(minor);
+	if (!dev)
+		return -ENODEV;
+
+	mutex_lock(&dev->mutex);
+	s = comedi_read_subdevice(dev, minor);
 	if (s && (s->subdev_flags & SDF_CMD_READ) && s->async)
 		size = s->async->max_bufsize / 1024;
-	mutex_unlock(&info->device->mutex);
+	mutex_unlock(&dev->mutex);
 
 	return snprintf(buf, PAGE_SIZE, "%i\n", size);
 }
 
-static ssize_t store_max_read_buffer_kb(struct device *dev,
+static ssize_t store_max_read_buffer_kb(struct device *csdev,
 					struct device_attribute *attr,
 					const char *buf, size_t count)
 {
-	struct comedi_file_info *info = dev_get_drvdata(dev);
-	struct comedi_subdevice *s = comedi_read_subdevice(info);
+	unsigned int minor = MINOR(csdev->devt);
+	struct comedi_device *dev;
+	struct comedi_subdevice *s;
 	unsigned int size;
 	int err;
 
@@ -203,37 +308,49 @@ static ssize_t store_max_read_buffer_kb(struct device *dev,
 		return -EINVAL;
 	size *= 1024;
 
-	mutex_lock(&info->device->mutex);
+	dev = comedi_dev_from_minor(minor);
+	if (!dev)
+		return -ENODEV;
+
+	mutex_lock(&dev->mutex);
+	s = comedi_read_subdevice(dev, minor);
 	if (s && (s->subdev_flags & SDF_CMD_READ) && s->async)
 		s->async->max_bufsize = size;
 	else
 		err = -EINVAL;
-	mutex_unlock(&info->device->mutex);
+	mutex_unlock(&dev->mutex);
 
 	return err ? err : count;
 }
 
-static ssize_t show_read_buffer_kb(struct device *dev,
+static ssize_t show_read_buffer_kb(struct device *csdev,
 				   struct device_attribute *attr, char *buf)
 {
-	struct comedi_file_info *info = dev_get_drvdata(dev);
-	struct comedi_subdevice *s = comedi_read_subdevice(info);
+	unsigned int minor = MINOR(csdev->devt);
+	struct comedi_device *dev;
+	struct comedi_subdevice *s;
 	unsigned int size = 0;
 
-	mutex_lock(&info->device->mutex);
+	dev = comedi_dev_from_minor(minor);
+	if (!dev)
+		return -ENODEV;
+
+	mutex_lock(&dev->mutex);
+	s = comedi_read_subdevice(dev, minor);
 	if (s && (s->subdev_flags & SDF_CMD_READ) && s->async)
 		size = s->async->prealloc_bufsz / 1024;
-	mutex_unlock(&info->device->mutex);
+	mutex_unlock(&dev->mutex);
 
 	return snprintf(buf, PAGE_SIZE, "%i\n", size);
 }
 
-static ssize_t store_read_buffer_kb(struct device *dev,
+static ssize_t store_read_buffer_kb(struct device *csdev,
 				    struct device_attribute *attr,
 				    const char *buf, size_t count)
 {
-	struct comedi_file_info *info = dev_get_drvdata(dev);
-	struct comedi_subdevice *s = comedi_read_subdevice(info);
+	unsigned int minor = MINOR(csdev->devt);
+	struct comedi_device *dev;
+	struct comedi_subdevice *s;
 	unsigned int size;
 	int err;
 
@@ -244,38 +361,50 @@ static ssize_t store_read_buffer_kb(struct device *dev,
 		return -EINVAL;
 	size *= 1024;
 
-	mutex_lock(&info->device->mutex);
+	dev = comedi_dev_from_minor(minor);
+	if (!dev)
+		return -ENODEV;
+
+	mutex_lock(&dev->mutex);
+	s = comedi_read_subdevice(dev, minor);
 	if (s && (s->subdev_flags & SDF_CMD_READ) && s->async)
-		err = resize_async_buffer(info->device, s, s->async, size);
+		err = resize_async_buffer(dev, s, s->async, size);
 	else
 		err = -EINVAL;
-	mutex_unlock(&info->device->mutex);
+	mutex_unlock(&dev->mutex);
 
 	return err ? err : count;
 }
 
-static ssize_t show_max_write_buffer_kb(struct device *dev,
+static ssize_t show_max_write_buffer_kb(struct device *csdev,
 					struct device_attribute *attr,
 					char *buf)
 {
-	struct comedi_file_info *info = dev_get_drvdata(dev);
-	struct comedi_subdevice *s = comedi_write_subdevice(info);
+	unsigned int minor = MINOR(csdev->devt);
+	struct comedi_device *dev;
+	struct comedi_subdevice *s;
 	unsigned int size = 0;
 
-	mutex_lock(&info->device->mutex);
+	dev = comedi_dev_from_minor(minor);
+	if (!dev)
+		return -ENODEV;
+
+	mutex_lock(&dev->mutex);
+	s = comedi_write_subdevice(dev, minor);
 	if (s && (s->subdev_flags & SDF_CMD_WRITE) && s->async)
 		size = s->async->max_bufsize / 1024;
-	mutex_unlock(&info->device->mutex);
+	mutex_unlock(&dev->mutex);
 
 	return snprintf(buf, PAGE_SIZE, "%i\n", size);
 }
 
-static ssize_t store_max_write_buffer_kb(struct device *dev,
+static ssize_t store_max_write_buffer_kb(struct device *csdev,
 					 struct device_attribute *attr,
 					 const char *buf, size_t count)
 {
-	struct comedi_file_info *info = dev_get_drvdata(dev);
-	struct comedi_subdevice *s = comedi_write_subdevice(info);
+	unsigned int minor = MINOR(csdev->devt);
+	struct comedi_device *dev;
+	struct comedi_subdevice *s;
 	unsigned int size;
 	int err;
 
@@ -286,37 +415,49 @@ static ssize_t store_max_write_buffer_kb(struct device *dev,
 		return -EINVAL;
 	size *= 1024;
 
-	mutex_lock(&info->device->mutex);
+	dev = comedi_dev_from_minor(minor);
+	if (!dev)
+		return -ENODEV;
+
+	mutex_lock(&dev->mutex);
+	s = comedi_write_subdevice(dev, minor);
 	if (s && (s->subdev_flags & SDF_CMD_WRITE) && s->async)
 		s->async->max_bufsize = size;
 	else
 		err = -EINVAL;
-	mutex_unlock(&info->device->mutex);
+	mutex_unlock(&dev->mutex);
 
 	return err ? err : count;
 }
 
-static ssize_t show_write_buffer_kb(struct device *dev,
+static ssize_t show_write_buffer_kb(struct device *csdev,
 				    struct device_attribute *attr, char *buf)
 {
-	struct comedi_file_info *info = dev_get_drvdata(dev);
-	struct comedi_subdevice *s = comedi_write_subdevice(info);
+	unsigned int minor = MINOR(csdev->devt);
+	struct comedi_device *dev;
+	struct comedi_subdevice *s;
 	unsigned int size = 0;
 
-	mutex_lock(&info->device->mutex);
+	dev = comedi_dev_from_minor(minor);
+	if (!dev)
+		return -ENODEV;
+
+	mutex_lock(&dev->mutex);
+	s = comedi_write_subdevice(dev, minor);
 	if (s && (s->subdev_flags & SDF_CMD_WRITE) && s->async)
 		size = s->async->prealloc_bufsz / 1024;
-	mutex_unlock(&info->device->mutex);
+	mutex_unlock(&dev->mutex);
 
 	return snprintf(buf, PAGE_SIZE, "%i\n", size);
 }
 
-static ssize_t store_write_buffer_kb(struct device *dev,
+static ssize_t store_write_buffer_kb(struct device *csdev,
 				     struct device_attribute *attr,
 				     const char *buf, size_t count)
 {
-	struct comedi_file_info *info = dev_get_drvdata(dev);
-	struct comedi_subdevice *s = comedi_write_subdevice(info);
+	unsigned int minor = MINOR(csdev->devt);
+	struct comedi_device *dev;
+	struct comedi_subdevice *s;
 	unsigned int size;
 	int err;
 
@@ -327,12 +468,17 @@ static ssize_t store_write_buffer_kb(struct device *dev,
 		return -EINVAL;
 	size *= 1024;
 
-	mutex_lock(&info->device->mutex);
+	dev = comedi_dev_from_minor(minor);
+	if (!dev)
+		return -ENODEV;
+
+	mutex_lock(&dev->mutex);
+	s = comedi_write_subdevice(dev, minor);
 	if (s && (s->subdev_flags & SDF_CMD_WRITE) && s->async)
-		err = resize_async_buffer(info->device, s, s->async, size);
+		err = resize_async_buffer(dev, s, s->async, size);
 	else
 		err = -EINVAL;
-	mutex_unlock(&info->device->mutex);
+	mutex_unlock(&dev->mutex);
 
 	return err ? err : count;
 }
@@ -463,7 +609,6 @@ static int do_devconfig_ioctl(struct comedi_device *dev,
 			      struct comedi_devconfig __user *arg)
 {
 	struct comedi_devconfig it;
-	int ret;
 
 	if (!capable(CAP_SYS_ADMIN))
 		return -EPERM;
@@ -490,15 +635,12 @@ static int do_devconfig_ioctl(struct comedi_device *dev,
 		return -EINVAL;
 	}
 
-	ret = comedi_device_attach(dev, &it);
-	if (ret == 0) {
-		if (!try_module_get(dev->driver->module)) {
-			comedi_device_detach(dev);
-			ret = -ENOSYS;
-		}
-	}
+	if (dev->minor >= comedi_num_legacy_minors)
+		/* don't re-use dynamically allocated comedi devices */
+		return -EBUSY;
 
-	return ret;
+	/* This increments the driver module count on success. */
+	return comedi_device_attach(dev, &it);
 }
 
 /*
@@ -581,7 +723,6 @@ static int do_devinfo_ioctl(struct comedi_device *dev,
 			    struct file *file)
 {
 	const unsigned minor = iminor(file_inode(file));
-	struct comedi_file_info *info = comedi_file_info_from_minor(minor);
 	struct comedi_subdevice *s;
 	struct comedi_devinfo devinfo;
 
@@ -593,13 +734,13 @@ static int do_devinfo_ioctl(struct comedi_device *dev,
 	strlcpy(devinfo.driver_name, dev->driver->driver_name, COMEDI_NAMELEN);
 	strlcpy(devinfo.board_name, dev->board_name, COMEDI_NAMELEN);
 
-	s = comedi_read_subdevice(info);
+	s = comedi_read_subdevice(dev, minor);
 	if (s)
 		devinfo.read_subdevice = s->index;
 	else
 		devinfo.read_subdevice = -1;
 
-	s = comedi_write_subdevice(info);
+	s = comedi_write_subdevice(dev, minor);
 	if (s)
 		devinfo.write_subdevice = s->index;
 	else
@@ -1616,8 +1757,7 @@ static long comedi_unlocked_ioctl(struct file *file, unsigned int cmd,
 				  unsigned long arg)
 {
 	const unsigned minor = iminor(file_inode(file));
-	struct comedi_file_info *info = comedi_file_info_from_minor(minor);
-	struct comedi_device *dev = comedi_dev_from_file_info(info);
+	struct comedi_device *dev = comedi_dev_from_minor(minor);
 	int rc;
 
 	if (!dev)
@@ -1635,9 +1775,18 @@ static long comedi_unlocked_ioctl(struct file *file, unsigned int cmd,
 		}
 		rc = do_devconfig_ioctl(dev,
 					(struct comedi_devconfig __user *)arg);
-		if (rc == 0)
-			/* Evade comedi_auto_unconfig(). */
-			info->hardware_device = NULL;
+		if (rc == 0) {
+			if (arg == 0 &&
+			    dev->minor >= comedi_num_legacy_minors) {
+				/* Successfully unconfigured a dynamically
+				 * allocated device.  Try and remove it. */
+				if (comedi_clear_board_dev(dev)) {
+					mutex_unlock(&dev->mutex);
+					comedi_free_board_dev(dev);
+					return rc;
+				}
+			}
+		}
 		goto done;
 	}
 
@@ -1744,8 +1893,7 @@ static struct vm_operations_struct comedi_vm_ops = {
 static int comedi_mmap(struct file *file, struct vm_area_struct *vma)
 {
 	const unsigned minor = iminor(file_inode(file));
-	struct comedi_file_info *info = comedi_file_info_from_minor(minor);
-	struct comedi_device *dev = comedi_dev_from_file_info(info);
+	struct comedi_device *dev = comedi_dev_from_minor(minor);
 	struct comedi_subdevice *s;
 	struct comedi_async *async;
 	unsigned long start = vma->vm_start;
@@ -1766,9 +1914,9 @@ static int comedi_mmap(struct file *file, struct vm_area_struct *vma)
 	}
 
 	if (vma->vm_flags & VM_WRITE)
-		s = comedi_write_subdevice(info);
+		s = comedi_write_subdevice(dev, minor);
 	else
-		s = comedi_read_subdevice(info);
+		s = comedi_read_subdevice(dev, minor);
 	if (!s) {
 		retval = -EINVAL;
 		goto done;
@@ -1824,8 +1972,7 @@ static unsigned int comedi_poll(struct file *file, poll_table *wait)
 {
 	unsigned int mask = 0;
 	const unsigned minor = iminor(file_inode(file));
-	struct comedi_file_info *info = comedi_file_info_from_minor(minor);
-	struct comedi_device *dev = comedi_dev_from_file_info(info);
+	struct comedi_device *dev = comedi_dev_from_minor(minor);
 	struct comedi_subdevice *s;
 
 	if (!dev)
@@ -1838,7 +1985,7 @@ static unsigned int comedi_poll(struct file *file, poll_table *wait)
 		goto done;
 	}
 
-	s = comedi_read_subdevice(info);
+	s = comedi_read_subdevice(dev, minor);
 	if (s && s->async) {
 		poll_wait(file, &s->async->wait_head, wait);
 		if (!s->busy || !comedi_is_subdevice_running(s) ||
@@ -1846,7 +1993,7 @@ static unsigned int comedi_poll(struct file *file, poll_table *wait)
 			mask |= POLLIN | POLLRDNORM;
 	}
 
-	s = comedi_write_subdevice(info);
+	s = comedi_write_subdevice(dev, minor);
 	if (s && s->async) {
 		unsigned int bps = bytes_per_sample(s->async->subdevice);
 
@@ -1870,8 +2017,7 @@ static ssize_t comedi_write(struct file *file, const char __user *buf,
 	int n, m, count = 0, retval = 0;
 	DECLARE_WAITQUEUE(wait, current);
 	const unsigned minor = iminor(file_inode(file));
-	struct comedi_file_info *info = comedi_file_info_from_minor(minor);
-	struct comedi_device *dev = comedi_dev_from_file_info(info);
+	struct comedi_device *dev = comedi_dev_from_minor(minor);
 
 	if (!dev)
 		return -ENODEV;
@@ -1881,7 +2027,7 @@ static ssize_t comedi_write(struct file *file, const char __user *buf,
 		return -ENODEV;
 	}
 
-	s = comedi_write_subdevice(info);
+	s = comedi_write_subdevice(dev, minor);
 	if (!s || !s->async)
 		return -EIO;
 
@@ -1965,8 +2111,7 @@ static ssize_t comedi_read(struct file *file, char __user *buf, size_t nbytes,
 	int n, m, count = 0, retval = 0;
 	DECLARE_WAITQUEUE(wait, current);
 	const unsigned minor = iminor(file_inode(file));
-	struct comedi_file_info *info = comedi_file_info_from_minor(minor);
-	struct comedi_device *dev = comedi_dev_from_file_info(info);
+	struct comedi_device *dev = comedi_dev_from_minor(minor);
 
 	if (!dev)
 		return -ENODEV;
@@ -1976,7 +2121,7 @@ static ssize_t comedi_read(struct file *file, char __user *buf, size_t nbytes,
 		return -ENODEV;
 	}
 
-	s = comedi_read_subdevice(info);
+	s = comedi_read_subdevice(dev, minor);
 	if (!s || !s->async)
 		return -EIO;
 
@@ -2067,12 +2212,12 @@ static int comedi_open(struct inode *inode, struct file *file)
 	/* This is slightly hacky, but we want module autoloading
 	 * to work for root.
 	 * case: user opens device, attached -> ok
-	 * case: user opens device, unattached, in_request_module=0 -> autoload
-	 * case: user opens device, unattached, in_request_module=1 -> fail
+	 * case: user opens device, unattached, !in_request_module -> autoload
+	 * case: user opens device, unattached, in_request_module -> fail
 	 * case: root opens device, attached -> ok
-	 * case: root opens device, unattached, in_request_module=1 -> ok
+	 * case: root opens device, unattached, in_request_module -> ok
 	 *   (typically called from modprobe)
-	 * case: root opens device, unattached, in_request_module=0 -> autoload
+	 * case: root opens device, unattached, !in_request_module -> autoload
 	 *
 	 * The last could be changed to "-> ok", which would deny root
 	 * autoloading.
@@ -2088,7 +2233,7 @@ static int comedi_open(struct inode *inode, struct file *file)
 	if (capable(CAP_NET_ADMIN) && dev->in_request_module)
 		goto ok;
 
-	dev->in_request_module = 1;
+	dev->in_request_module = true;
 
 #ifdef CONFIG_KMOD
 	mutex_unlock(&dev->mutex);
@@ -2096,7 +2241,7 @@ static int comedi_open(struct inode *inode, struct file *file)
 	mutex_lock(&dev->mutex);
 #endif
 
-	dev->in_request_module = 0;
+	dev->in_request_module = false;
 
 	if (!dev->attached && !capable(CAP_NET_ADMIN)) {
 		DPRINTK("not attached and not CAP_NET_ADMIN\n");
@@ -2195,14 +2340,11 @@ static const struct file_operations comedi_fops = {
 	.llseek = noop_llseek,
 };
 
-static struct class *comedi_class;
-static struct cdev comedi_cdev;
-
 void comedi_error(const struct comedi_device *dev, const char *s)
 {
 	dev_err(dev->class_dev, "%s: %s\n", dev->driver->driver_name, s);
 }
-EXPORT_SYMBOL(comedi_error);
+EXPORT_SYMBOL_GPL(comedi_error);
 
 void comedi_event(struct comedi_device *dev, struct comedi_subdevice *s)
 {
@@ -2245,150 +2387,104 @@ void comedi_event(struct comedi_device *dev, struct comedi_subdevice *s)
 	}
 	s->async->events = 0;
 }
-EXPORT_SYMBOL(comedi_event);
-
-static void comedi_device_init(struct comedi_device *dev)
-{
-	memset(dev, 0, sizeof(*dev));
-	spin_lock_init(&dev->spinlock);
-	mutex_init(&dev->mutex);
-	dev->minor = -1;
-}
-
-static void comedi_device_cleanup(struct comedi_device *dev)
-{
-	if (dev == NULL)
-		return;
-	mutex_lock(&dev->mutex);
-	comedi_device_detach(dev);
-	mutex_unlock(&dev->mutex);
-	mutex_destroy(&dev->mutex);
-}
+EXPORT_SYMBOL_GPL(comedi_event);
 
-int comedi_alloc_board_minor(struct device *hardware_device)
+/* Note: the ->mutex is pre-locked on successful return */
+struct comedi_device *comedi_alloc_board_minor(struct device *hardware_device)
 {
-	struct comedi_file_info *info;
+	struct comedi_device *dev;
 	struct device *csdev;
 	unsigned i;
 
-	info = kzalloc(sizeof(*info), GFP_KERNEL);
-	if (info == NULL)
-		return -ENOMEM;
-	info->device = kzalloc(sizeof(struct comedi_device), GFP_KERNEL);
-	if (info->device == NULL) {
-		kfree(info);
-		return -ENOMEM;
-	}
-	info->hardware_device = hardware_device;
-	comedi_device_init(info->device);
-	spin_lock(&comedi_file_info_table_lock);
-	for (i = 0; i < COMEDI_NUM_BOARD_MINORS; ++i) {
-		if (comedi_file_info_table[i] == NULL) {
-			comedi_file_info_table[i] = info;
+	dev = kzalloc(sizeof(struct comedi_device), GFP_KERNEL);
+	if (dev == NULL)
+		return ERR_PTR(-ENOMEM);
+	comedi_device_init(dev);
+	comedi_set_hw_dev(dev, hardware_device);
+	mutex_lock(&dev->mutex);
+	mutex_lock(&comedi_board_minor_table_lock);
+	for (i = hardware_device ? comedi_num_legacy_minors : 0;
+	     i < COMEDI_NUM_BOARD_MINORS; ++i) {
+		if (comedi_board_minor_table[i] == NULL) {
+			comedi_board_minor_table[i] = dev;
 			break;
 		}
 	}
-	spin_unlock(&comedi_file_info_table_lock);
+	mutex_unlock(&comedi_board_minor_table_lock);
 	if (i == COMEDI_NUM_BOARD_MINORS) {
-		comedi_device_cleanup(info->device);
-		kfree(info->device);
-		kfree(info);
+		mutex_unlock(&dev->mutex);
+		comedi_device_cleanup(dev);
+		kfree(dev);
 		pr_err("comedi: error: ran out of minor numbers for board device files.\n");
-		return -EBUSY;
+		return ERR_PTR(-EBUSY);
 	}
-	info->device->minor = i;
+	dev->minor = i;
 	csdev = device_create(comedi_class, hardware_device,
 			      MKDEV(COMEDI_MAJOR, i), NULL, "comedi%i", i);
 	if (!IS_ERR(csdev))
-		info->device->class_dev = csdev;
-	dev_set_drvdata(csdev, info);
+		dev->class_dev = csdev;
 
-	return i;
+	/* Note: dev->mutex needs to be unlocked by the caller. */
+	return dev;
 }
 
-void comedi_free_board_minor(unsigned minor)
+static void comedi_free_board_minor(unsigned minor)
 {
-	struct comedi_file_info *info;
-
 	BUG_ON(minor >= COMEDI_NUM_BOARD_MINORS);
-	spin_lock(&comedi_file_info_table_lock);
-	info = comedi_file_info_table[minor];
-	comedi_file_info_table[minor] = NULL;
-	spin_unlock(&comedi_file_info_table_lock);
-
-	if (info) {
-		struct comedi_device *dev = info->device;
-		if (dev) {
-			if (dev->class_dev) {
-				device_destroy(comedi_class,
-					       MKDEV(COMEDI_MAJOR, dev->minor));
-			}
-			comedi_device_cleanup(dev);
-			kfree(dev);
-		}
-		kfree(info);
-	}
+	comedi_free_board_dev(comedi_clear_board_minor(minor));
 }
 
-int comedi_find_board_minor(struct device *hardware_device)
+void comedi_release_hardware_device(struct device *hardware_device)
 {
 	int minor;
-	struct comedi_file_info *info;
-
-	for (minor = 0; minor < COMEDI_NUM_BOARD_MINORS; minor++) {
-		spin_lock(&comedi_file_info_table_lock);
-		info = comedi_file_info_table[minor];
-		if (info && info->hardware_device == hardware_device) {
-			spin_unlock(&comedi_file_info_table_lock);
-			return minor;
+	struct comedi_device *dev;
+
+	for (minor = comedi_num_legacy_minors; minor < COMEDI_NUM_BOARD_MINORS;
+	     minor++) {
+		mutex_lock(&comedi_board_minor_table_lock);
+		dev = comedi_board_minor_table[minor];
+		if (dev && dev->hw_dev == hardware_device) {
+			comedi_board_minor_table[minor] = NULL;
+			mutex_unlock(&comedi_board_minor_table_lock);
+			comedi_free_board_dev(dev);
+			break;
 		}
-		spin_unlock(&comedi_file_info_table_lock);
+		mutex_unlock(&comedi_board_minor_table_lock);
 	}
-	return -ENODEV;
 }
 
 int comedi_alloc_subdevice_minor(struct comedi_subdevice *s)
 {
 	struct comedi_device *dev = s->device;
-	struct comedi_file_info *info;
 	struct device *csdev;
 	unsigned i;
 
-	info = kzalloc(sizeof(*info), GFP_KERNEL);
-	if (!info)
-		return -ENOMEM;
-	info->device = dev;
-	if (s->subdev_flags & SDF_CMD_READ)
-		info->read_subdevice = s;
-	if (s->subdev_flags & SDF_CMD_WRITE)
-		info->write_subdevice = s;
-	spin_lock(&comedi_file_info_table_lock);
-	for (i = COMEDI_FIRST_SUBDEVICE_MINOR; i < COMEDI_NUM_MINORS; ++i) {
-		if (comedi_file_info_table[i] == NULL) {
-			comedi_file_info_table[i] = info;
+	mutex_lock(&comedi_subdevice_minor_table_lock);
+	for (i = 0; i < COMEDI_NUM_SUBDEVICE_MINORS; ++i) {
+		if (comedi_subdevice_minor_table[i] == NULL) {
+			comedi_subdevice_minor_table[i] = s;
 			break;
 		}
 	}
-	spin_unlock(&comedi_file_info_table_lock);
-	if (i == COMEDI_NUM_MINORS) {
-		kfree(info);
+	mutex_unlock(&comedi_subdevice_minor_table_lock);
+	if (i == COMEDI_NUM_SUBDEVICE_MINORS) {
 		pr_err("comedi: error: ran out of minor numbers for subdevice files.\n");
 		return -EBUSY;
 	}
+	i += COMEDI_NUM_BOARD_MINORS;
 	s->minor = i;
 	csdev = device_create(comedi_class, dev->class_dev,
 			      MKDEV(COMEDI_MAJOR, i), NULL, "comedi%i_subd%i",
 			      dev->minor, s->index);
 	if (!IS_ERR(csdev))
 		s->class_dev = csdev;
-	dev_set_drvdata(csdev, info);
 
 	return 0;
 }
 
 void comedi_free_subdevice_minor(struct comedi_subdevice *s)
 {
-	struct comedi_file_info *info;
+	unsigned int i;
 
 	if (s == NULL)
 		return;
@@ -2396,18 +2492,17 @@ void comedi_free_subdevice_minor(struct comedi_subdevice *s)
 		return;
 
 	BUG_ON(s->minor >= COMEDI_NUM_MINORS);
-	BUG_ON(s->minor < COMEDI_FIRST_SUBDEVICE_MINOR);
-
-	spin_lock(&comedi_file_info_table_lock);
-	info = comedi_file_info_table[s->minor];
-	comedi_file_info_table[s->minor] = NULL;
-	spin_unlock(&comedi_file_info_table_lock);
+	BUG_ON(s->minor < COMEDI_NUM_BOARD_MINORS);
 
+	i = s->minor - COMEDI_NUM_BOARD_MINORS;
+	mutex_lock(&comedi_subdevice_minor_table_lock);
+	if (s == comedi_subdevice_minor_table[i])
+		comedi_subdevice_minor_table[i] = NULL;
+	mutex_unlock(&comedi_subdevice_minor_table_lock);
 	if (s->class_dev) {
 		device_destroy(comedi_class, MKDEV(COMEDI_MAJOR, s->minor));
 		s->class_dev = NULL;
 	}
-	kfree(info);
 }
 
 static void comedi_cleanup_board_minors(void)
@@ -2432,9 +2527,6 @@ static int __init comedi_init(void)
 		return -EINVAL;
 	}
 
-	memset(comedi_file_info_table, 0,
-	       sizeof(struct comedi_file_info *) * COMEDI_NUM_MINORS);
-
 	retval = register_chrdev_region(MKDEV(COMEDI_MAJOR, 0),
 					COMEDI_NUM_MINORS, "comedi");
 	if (retval)
@@ -2463,14 +2555,17 @@ static int __init comedi_init(void)
 
 	/* create devices files for legacy/manual use */
 	for (i = 0; i < comedi_num_legacy_minors; i++) {
-		int minor;
-		minor = comedi_alloc_board_minor(NULL);
-		if (minor < 0) {
+		struct comedi_device *dev;
+		dev = comedi_alloc_board_minor(NULL);
+		if (IS_ERR(dev)) {
 			comedi_cleanup_board_minors();
 			cdev_del(&comedi_cdev);
 			unregister_chrdev_region(MKDEV(COMEDI_MAJOR, 0),
 						 COMEDI_NUM_MINORS);
-			return minor;
+			return PTR_ERR(dev);
+		} else {
+			/* comedi_alloc_board_minor() locked the mutex */
+			mutex_unlock(&dev->mutex);
 		}
 	}
 
@@ -2483,8 +2578,10 @@ static void __exit comedi_cleanup(void)
 	int i;
 
 	comedi_cleanup_board_minors();
-	for (i = 0; i < COMEDI_NUM_MINORS; ++i)
-		BUG_ON(comedi_file_info_table[i]);
+	for (i = 0; i < COMEDI_NUM_BOARD_MINORS; ++i)
+		BUG_ON(comedi_board_minor_table[i]);
+	for (i = 0; i < COMEDI_NUM_SUBDEVICE_MINORS; ++i)
+		BUG_ON(comedi_subdevice_minor_table[i]);
 
 	class_destroy(comedi_class);
 	cdev_del(&comedi_cdev);
diff --git a/drivers/staging/comedi/comedi_internal.h b/drivers/staging/comedi/comedi_internal.h
index b3743135f4a..d5e03e558b3 100644
--- a/drivers/staging/comedi/comedi_internal.h
+++ b/drivers/staging/comedi/comedi_internal.h
@@ -8,9 +8,8 @@
  */
 int do_rangeinfo_ioctl(struct comedi_device *dev,
 		       struct comedi_rangeinfo __user *arg);
-int comedi_alloc_board_minor(struct device *hardware_device);
-void comedi_free_board_minor(unsigned minor);
-int comedi_find_board_minor(struct device *hardware_device);
+struct comedi_device *comedi_alloc_board_minor(struct device *hardware_device);
+void comedi_release_hardware_device(struct device *hardware_device);
 int comedi_alloc_subdevice_minor(struct comedi_subdevice *s);
 void comedi_free_subdevice_minor(struct comedi_subdevice *s);
 
diff --git a/drivers/staging/comedi/comedi_pci.c b/drivers/staging/comedi/comedi_pci.c
index 37d2e467736..5fad084cfbd 100644
--- a/drivers/staging/comedi/comedi_pci.c
+++ b/drivers/staging/comedi/comedi_pci.c
@@ -36,20 +36,25 @@ EXPORT_SYMBOL_GPL(comedi_to_pci_dev);
 
 /**
  * comedi_pci_enable() - Enable the PCI device and request the regions.
- * @pcidev: pci_dev struct
- * @res_name: name for the requested reqource
+ * @dev: comedi_device struct
  */
-int comedi_pci_enable(struct pci_dev *pcidev, const char *res_name)
+int comedi_pci_enable(struct comedi_device *dev)
 {
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	int rc;
 
+	if (!pcidev)
+		return -ENODEV;
+
 	rc = pci_enable_device(pcidev);
 	if (rc < 0)
 		return rc;
 
-	rc = pci_request_regions(pcidev, res_name);
+	rc = pci_request_regions(pcidev, dev->board_name);
 	if (rc < 0)
 		pci_disable_device(pcidev);
+	else
+		dev->ioenabled = true;
 
 	return rc;
 }
@@ -57,14 +62,17 @@ EXPORT_SYMBOL_GPL(comedi_pci_enable);
 
 /**
  * comedi_pci_disable() - Release the regions and disable the PCI device.
- * @pcidev: pci_dev struct
- *
- * This must be matched with a previous successful call to comedi_pci_enable().
+ * @dev: comedi_device struct
  */
-void comedi_pci_disable(struct pci_dev *pcidev)
+void comedi_pci_disable(struct comedi_device *dev)
 {
-	pci_release_regions(pcidev);
-	pci_disable_device(pcidev);
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
+	if (pcidev && dev->ioenabled) {
+		pci_release_regions(pcidev);
+		pci_disable_device(pcidev);
+	}
+	dev->ioenabled = false;
 }
 EXPORT_SYMBOL_GPL(comedi_pci_disable);
 
@@ -72,13 +80,15 @@ EXPORT_SYMBOL_GPL(comedi_pci_disable);
  * comedi_pci_auto_config() - Configure/probe a comedi PCI driver.
  * @pcidev: pci_dev struct
  * @driver: comedi_driver struct
+ * @context: driver specific data, passed to comedi_auto_config()
  *
  * Typically called from the pci_driver (*probe) function.
  */
 int comedi_pci_auto_config(struct pci_dev *pcidev,
-			   struct comedi_driver *driver)
+			   struct comedi_driver *driver,
+			   unsigned long context)
 {
-	return comedi_auto_config(&pcidev->dev, driver, 0);
+	return comedi_auto_config(&pcidev->dev, driver, context);
 }
 EXPORT_SYMBOL_GPL(comedi_pci_auto_config);
 
diff --git a/drivers/staging/comedi/comedidev.h b/drivers/staging/comedi/comedidev.h
index f3a990b45df..cdd472094ce 100644
--- a/drivers/staging/comedi/comedidev.h
+++ b/drivers/staging/comedi/comedidev.h
@@ -53,9 +53,7 @@
 	COMEDI_MINORVERSION, COMEDI_MICROVERSION)
 #define COMEDI_RELEASE VERSION
 
-#define COMEDI_NUM_MINORS 0x100
 #define COMEDI_NUM_BOARD_MINORS 0x30
-#define COMEDI_FIRST_SUBDEVICE_MINOR COMEDI_NUM_BOARD_MINORS
 
 struct comedi_subdevice {
 	struct comedi_device *device;
@@ -207,16 +205,18 @@ struct comedi_device {
 
 	const char *board_name;
 	const void *board_ptr;
-	int attached;
+	bool attached:1;
+	bool in_request_module:1;
+	bool ioenabled:1;
 	spinlock_t spinlock;
 	struct mutex mutex;
-	int in_request_module;
 
 	int n_subdevices;
 	struct comedi_subdevice *subdevices;
 
 	/* dumb */
 	unsigned long iobase;
+	unsigned long iolen;
 	unsigned int irq;
 
 	struct comedi_subdevice *read_subdev;
@@ -293,6 +293,10 @@ extern const struct comedi_lrange range_bipolar5;
 extern const struct comedi_lrange range_bipolar2_5;
 extern const struct comedi_lrange range_unipolar10;
 extern const struct comedi_lrange range_unipolar5;
+extern const struct comedi_lrange range_unipolar2_5;
+extern const struct comedi_lrange range_0_20mA;
+extern const struct comedi_lrange range_4_20mA;
+extern const struct comedi_lrange range_0_32mA;
 extern const struct comedi_lrange range_unknown;
 
 #define range_digital		range_unipolar5
@@ -345,6 +349,14 @@ void comedi_buf_memcpy_from(struct comedi_async *async, unsigned int offset,
 
 int comedi_alloc_subdevices(struct comedi_device *, int);
 
+void comedi_spriv_free(struct comedi_device *, int subdev_num);
+
+int __comedi_request_region(struct comedi_device *,
+			    unsigned long start, unsigned long len);
+int comedi_request_region(struct comedi_device *,
+			  unsigned long start, unsigned long len);
+void comedi_legacy_detach(struct comedi_device *);
+
 int comedi_auto_config(struct device *, struct comedi_driver *,
 		       unsigned long context);
 void comedi_auto_unconfig(struct device *);
@@ -384,10 +396,11 @@ struct pci_driver;
 
 struct pci_dev *comedi_to_pci_dev(struct comedi_device *);
 
-int comedi_pci_enable(struct pci_dev *, const char *);
-void comedi_pci_disable(struct pci_dev *);
+int comedi_pci_enable(struct comedi_device *);
+void comedi_pci_disable(struct comedi_device *);
 
-int comedi_pci_auto_config(struct pci_dev *, struct comedi_driver *);
+int comedi_pci_auto_config(struct pci_dev *, struct comedi_driver *,
+			   unsigned long context);
 void comedi_pci_auto_unconfig(struct pci_dev *);
 
 int comedi_pci_driver_register(struct comedi_driver *, struct pci_driver *);
@@ -420,12 +433,12 @@ static inline struct pci_dev *comedi_to_pci_dev(struct comedi_device *dev)
 	return NULL;
 }
 
-static inline int comedi_pci_enable(struct pci_dev *dev, const char *name)
+static inline int comedi_pci_enable(struct comedi_device *dev)
 {
 	return -ENOSYS;
 }
 
-static inline void comedi_pci_disable(struct pci_dev *dev)
+static inline void comedi_pci_disable(struct comedi_device *dev)
 {
 }
 
diff --git a/drivers/staging/comedi/drivers.c b/drivers/staging/comedi/drivers.c
index 64be7c5e891..06d190f8fd3 100644
--- a/drivers/staging/comedi/drivers.c
+++ b/drivers/staging/comedi/drivers.c
@@ -37,6 +37,7 @@
 #include <linux/cdev.h>
 #include <linux/dma-mapping.h>
 #include <linux/io.h>
+#include <linux/interrupt.h>
 
 #include "comedidev.h"
 #include "comedi_internal.h"
@@ -86,6 +87,18 @@ int comedi_alloc_subdevices(struct comedi_device *dev, int num_subdevices)
 }
 EXPORT_SYMBOL_GPL(comedi_alloc_subdevices);
 
+void comedi_spriv_free(struct comedi_device *dev, int subdev_num)
+{
+	struct comedi_subdevice *s;
+
+	if (dev->subdevices && subdev_num < dev->n_subdevices) {
+		s = &dev->subdevices[subdev_num];
+		kfree(s->private);
+		s->private = NULL;
+	}
+}
+EXPORT_SYMBOL_GPL(comedi_spriv_free);
+
 static void cleanup_device(struct comedi_device *dev)
 {
 	int i;
@@ -110,6 +123,8 @@ static void cleanup_device(struct comedi_device *dev)
 	dev->board_name = NULL;
 	dev->board_ptr = NULL;
 	dev->iobase = 0;
+	dev->iolen = 0;
+	dev->ioenabled = false;
 	dev->irq = 0;
 	dev->read_subdev = NULL;
 	dev->write_subdev = NULL;
@@ -118,24 +133,14 @@ static void cleanup_device(struct comedi_device *dev)
 	comedi_clear_hw_dev(dev);
 }
 
-static void __comedi_device_detach(struct comedi_device *dev)
+void comedi_device_detach(struct comedi_device *dev)
 {
-	dev->attached = 0;
+	dev->attached = false;
 	if (dev->driver)
 		dev->driver->detach(dev);
-	else
-		dev_warn(dev->class_dev,
-			 "BUG: dev->driver=NULL in comedi_device_detach()\n");
 	cleanup_device(dev);
 }
 
-void comedi_device_detach(struct comedi_device *dev)
-{
-	if (!dev->attached)
-		return;
-	__comedi_device_detach(dev);
-}
-
 static int poll_invalid(struct comedi_device *dev, struct comedi_subdevice *s)
 {
 	return -EINVAL;
@@ -276,21 +281,15 @@ static int __comedi_device_postconfig(struct comedi_device *dev)
 }
 
 /* do a little post-config cleanup */
-/* called with module refcount incremented, decrements it */
 static int comedi_device_postconfig(struct comedi_device *dev)
 {
-	int ret = __comedi_device_postconfig(dev);
-	module_put(dev->driver->module);
-	if (ret < 0) {
-		__comedi_device_detach(dev);
+	int ret;
+
+	ret = __comedi_device_postconfig(dev);
+	if (ret < 0)
 		return ret;
-	}
-	if (!dev->board_name) {
-		dev_warn(dev->class_dev, "BUG: dev->board_name=NULL\n");
-		dev->board_name = "BUG";
-	}
 	smp_wmb();
-	dev->attached = 1;
+	dev->attached = true;
 	return 0;
 }
 
@@ -352,6 +351,71 @@ static void comedi_report_boards(struct comedi_driver *driv)
 		pr_info(" %s\n", driv->driver_name);
 }
 
+/**
+ * __comedi_request_region() - Request an I/O reqion for a legacy driver.
+ * @dev: comedi_device struct
+ * @start: base address of the I/O reqion
+ * @len: length of the I/O region
+ */
+int __comedi_request_region(struct comedi_device *dev,
+			    unsigned long start, unsigned long len)
+{
+	if (!start) {
+		dev_warn(dev->class_dev,
+			 "%s: a I/O base address must be specified\n",
+			 dev->board_name);
+		return -EINVAL;
+	}
+
+	if (!request_region(start, len, dev->board_name)) {
+		dev_warn(dev->class_dev, "%s: I/O port conflict (%#lx,%lu)\n",
+			 dev->board_name, start, len);
+		return -EIO;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(__comedi_request_region);
+
+/**
+ * comedi_request_region() - Request an I/O reqion for a legacy driver.
+ * @dev: comedi_device struct
+ * @start: base address of the I/O reqion
+ * @len: length of the I/O region
+ */
+int comedi_request_region(struct comedi_device *dev,
+			  unsigned long start, unsigned long len)
+{
+	int ret;
+
+	ret = __comedi_request_region(dev, start, len);
+	if (ret == 0) {
+		dev->iobase = start;
+		dev->iolen = len;
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(comedi_request_region);
+
+/**
+ * comedi_legacy_detach() - A generic (*detach) function for legacy drivers.
+ * @dev: comedi_device struct
+ */
+void comedi_legacy_detach(struct comedi_device *dev)
+{
+	if (dev->irq) {
+		free_irq(dev->irq, dev);
+		dev->irq = 0;
+	}
+	if (dev->iobase && dev->iolen) {
+		release_region(dev->iobase, dev->iolen);
+		dev->iobase = 0;
+		dev->iolen = 0;
+	}
+}
+EXPORT_SYMBOL_GPL(comedi_legacy_detach);
+
 int comedi_device_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	struct comedi_driver *driv;
@@ -393,22 +457,35 @@ int comedi_device_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	/* initialize dev->driver here so
 	 * comedi_error() can be called from attach */
 	dev->driver = driv;
+	dev->board_name = dev->board_ptr ? *(const char **)dev->board_ptr
+					 : dev->driver->driver_name;
 	ret = driv->attach(dev, it);
+	if (ret >= 0)
+		ret = comedi_device_postconfig(dev);
 	if (ret < 0) {
+		comedi_device_detach(dev);
 		module_put(dev->driver->module);
-		__comedi_device_detach(dev);
-		return ret;
 	}
-	return comedi_device_postconfig(dev);
+	/* On success, the driver module count has been incremented. */
+	return ret;
 }
 
 int comedi_auto_config(struct device *hardware_device,
 		       struct comedi_driver *driver, unsigned long context)
 {
-	int minor;
-	struct comedi_device *comedi_dev;
+	struct comedi_device *dev;
 	int ret;
 
+	if (!hardware_device) {
+		pr_warn("BUG! comedi_auto_config called with NULL hardware_device\n");
+		return -EINVAL;
+	}
+	if (!driver) {
+		dev_warn(hardware_device,
+			 "BUG! comedi_auto_config called with NULL comedi driver\n");
+		return -EINVAL;
+	}
+
 	if (!driver->auto_attach) {
 		dev_warn(hardware_device,
 			 "BUG! comedi driver '%s' has no auto_attach handler\n",
@@ -416,46 +493,31 @@ int comedi_auto_config(struct device *hardware_device,
 		return -EINVAL;
 	}
 
-	minor = comedi_alloc_board_minor(hardware_device);
-	if (minor < 0)
-		return minor;
-
-	comedi_dev = comedi_dev_from_minor(minor);
-
-	mutex_lock(&comedi_dev->mutex);
-	if (comedi_dev->attached)
-		ret = -EBUSY;
-	else if (!try_module_get(driver->module))
-		ret = -EIO;
-	else {
-		comedi_set_hw_dev(comedi_dev, hardware_device);
-		comedi_dev->driver = driver;
-		ret = driver->auto_attach(comedi_dev, context);
-		if (ret < 0) {
-			module_put(driver->module);
-			__comedi_device_detach(comedi_dev);
-		} else {
-			ret = comedi_device_postconfig(comedi_dev);
-		}
-	}
-	mutex_unlock(&comedi_dev->mutex);
+	dev = comedi_alloc_board_minor(hardware_device);
+	if (IS_ERR(dev))
+		return PTR_ERR(dev);
+	/* Note: comedi_alloc_board_minor() locked dev->mutex. */
 
+	dev->driver = driver;
+	dev->board_name = dev->driver->driver_name;
+	ret = driver->auto_attach(dev, context);
+	if (ret >= 0)
+		ret = comedi_device_postconfig(dev);
 	if (ret < 0)
-		comedi_free_board_minor(minor);
+		comedi_device_detach(dev);
+	mutex_unlock(&dev->mutex);
+
+	if (ret < 0)
+		comedi_release_hardware_device(hardware_device);
 	return ret;
 }
 EXPORT_SYMBOL_GPL(comedi_auto_config);
 
 void comedi_auto_unconfig(struct device *hardware_device)
 {
-	int minor;
-
 	if (hardware_device == NULL)
 		return;
-	minor = comedi_find_board_minor(hardware_device);
-	if (minor < 0)
-		return;
-	comedi_free_board_minor(minor);
+	comedi_release_hardware_device(hardware_device);
 }
 EXPORT_SYMBOL_GPL(comedi_auto_unconfig);
 
@@ -466,7 +528,7 @@ int comedi_driver_register(struct comedi_driver *driver)
 
 	return 0;
 }
-EXPORT_SYMBOL(comedi_driver_register);
+EXPORT_SYMBOL_GPL(comedi_driver_register);
 
 int comedi_driver_unregister(struct comedi_driver *driver)
 {
@@ -504,4 +566,4 @@ int comedi_driver_unregister(struct comedi_driver *driver)
 	}
 	return -EINVAL;
 }
-EXPORT_SYMBOL(comedi_driver_unregister);
+EXPORT_SYMBOL_GPL(comedi_driver_unregister);
diff --git a/drivers/staging/comedi/drivers/8255.c b/drivers/staging/comedi/drivers/8255.c
index c7aa41ad842..1d48aa602ec 100644
--- a/drivers/staging/comedi/drivers/8255.c
+++ b/drivers/staging/comedi/drivers/8255.c
@@ -129,7 +129,7 @@ void subdev_8255_interrupt(struct comedi_device *dev,
 
 	comedi_event(dev, s);
 }
-EXPORT_SYMBOL(subdev_8255_interrupt);
+EXPORT_SYMBOL_GPL(subdev_8255_interrupt);
 
 static int subdev_8255_insn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
@@ -314,7 +314,7 @@ int subdev_8255_init(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	return 0;
 }
-EXPORT_SYMBOL(subdev_8255_init);
+EXPORT_SYMBOL_GPL(subdev_8255_init);
 
 int subdev_8255_init_irq(struct comedi_device *dev, struct comedi_subdevice *s,
 			 int (*io) (int, int, int, unsigned long),
@@ -332,13 +332,7 @@ int subdev_8255_init_irq(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	return 0;
 }
-EXPORT_SYMBOL(subdev_8255_init_irq);
-
-void subdev_8255_cleanup(struct comedi_device *dev, struct comedi_subdevice *s)
-{
-	kfree(s->private);
-}
-EXPORT_SYMBOL(subdev_8255_cleanup);
+EXPORT_SYMBOL_GPL(subdev_8255_init_irq);
 
 /*
 
@@ -354,8 +348,6 @@ static int dev_8255_attach(struct comedi_device *dev,
 	unsigned long iobase;
 	int i;
 
-	dev->board_name = "8255";
-
 	for (i = 0; i < COMEDI_NDEVCONFOPTS; i++) {
 		iobase = it->options[i];
 		if (!iobase)
@@ -374,16 +366,13 @@ static int dev_8255_attach(struct comedi_device *dev,
 		s = &dev->subdevices[i];
 		iobase = it->options[i];
 
-		if (!request_region(iobase, _8255_SIZE, "8255")) {
-			dev_warn(dev->class_dev,
-				"0x%04lx (I/O port conflict)\n", iobase);
-
+		ret = __comedi_request_region(dev, iobase, _8255_SIZE);
+		if (ret) {
 			s->type = COMEDI_SUBD_UNUSED;
 		} else {
 			ret = subdev_8255_init(dev, s, NULL, iobase);
 			if (ret)
 				return ret;
-			dev_info(dev->class_dev, "0x%04lx\n", iobase);
 		}
 	}
 
@@ -402,7 +391,7 @@ static void dev_8255_detach(struct comedi_device *dev)
 			spriv = s->private;
 			release_region(spriv->iobase, _8255_SIZE);
 		}
-		subdev_8255_cleanup(dev, s);
+		comedi_spriv_free(dev, i);
 	}
 }
 
diff --git a/drivers/staging/comedi/drivers/8255.h b/drivers/staging/comedi/drivers/8255.h
index 1e589b4b8b7..0f6e7492b7d 100644
--- a/drivers/staging/comedi/drivers/8255.h
+++ b/drivers/staging/comedi/drivers/8255.h
@@ -32,7 +32,6 @@ int subdev_8255_init(struct comedi_device *dev, struct comedi_subdevice *s,
 int subdev_8255_init_irq(struct comedi_device *dev, struct comedi_subdevice *s,
 			 int (*io) (int, int, int, unsigned long),
 			 unsigned long iobase);
-void subdev_8255_cleanup(struct comedi_device *dev, struct comedi_subdevice *s);
 void subdev_8255_interrupt(struct comedi_device *dev,
 			   struct comedi_subdevice *s);
 
diff --git a/drivers/staging/comedi/drivers/8255_pci.c b/drivers/staging/comedi/drivers/8255_pci.c
index 0ae356ae56e..76dec96aeb2 100644
--- a/drivers/staging/comedi/drivers/8255_pci.c
+++ b/drivers/staging/comedi/drivers/8255_pci.c
@@ -60,126 +60,98 @@ Configuration Options: not applicable, uses PCI auto config
 
 #include "8255.h"
 
-/*
- * PCI Device ID's supported by this driver
- */
-#define PCI_DEVICE_ID_ADLINK_PCI7224	0x7224
-#define PCI_DEVICE_ID_ADLINK_PCI7248	0x7248
-#define PCI_DEVICE_ID_ADLINK_PCI7296	0x7296
-
-#define PCI_DEVICE_ID_CB_PCIDIO48H	0x000b
-#define PCI_DEVICE_ID_CB_PCIDIO24H	0x0014
-#define PCI_DEVICE_ID_CB_PCIDIO96H	0x0017
-#define PCI_DEVICE_ID_CB_PCIDIO24	0x0028
-
-#define PCI_DEVICE_ID_NI_PCIDIO96	0x0160
-#define PCI_DEVICE_ID_NI_PCI6503	0x0400
-#define PCI_DEVICE_ID_NI_PCI6503B	0x1250
-#define PCI_DEVICE_ID_NI_PXI6508	0x13c0
-#define PCI_DEVICE_ID_NI_PCIDIO96B	0x1630
-#define PCI_DEVICE_ID_NI_PCI6503X	0x17d0
-#define PCI_DEVICE_ID_NI_PXI_6503	0x1800
+enum pci_8255_boardid {
+	BOARD_ADLINK_PCI7224,
+	BOARD_ADLINK_PCI7248,
+	BOARD_ADLINK_PCI7296,
+	BOARD_CB_PCIDIO24,
+	BOARD_CB_PCIDIO24H,
+	BOARD_CB_PCIDIO48H,
+	BOARD_CB_PCIDIO96H,
+	BOARD_NI_PCIDIO96,
+	BOARD_NI_PCIDIO96B,
+	BOARD_NI_PXI6508,
+	BOARD_NI_PCI6503,
+	BOARD_NI_PCI6503B,
+	BOARD_NI_PCI6503X,
+	BOARD_NI_PXI_6503,
+};
 
 struct pci_8255_boardinfo {
 	const char *name;
-	unsigned short vendor;
-	unsigned short device;
 	int dio_badr;
-	int is_mmio;
 	int n_8255;
 };
 
 static const struct pci_8255_boardinfo pci_8255_boards[] = {
-	{
+	[BOARD_ADLINK_PCI7224] = {
 		.name		= "adl_pci-7224",
-		.vendor		= PCI_VENDOR_ID_ADLINK,
-		.device		= PCI_DEVICE_ID_ADLINK_PCI7224,
 		.dio_badr	= 2,
 		.n_8255		= 1,
-	}, {
+	},
+	[BOARD_ADLINK_PCI7248] = {
 		.name		= "adl_pci-7248",
-		.vendor		= PCI_VENDOR_ID_ADLINK,
-		.device		= PCI_DEVICE_ID_ADLINK_PCI7248,
 		.dio_badr	= 2,
 		.n_8255		= 2,
-	}, {
+	},
+	[BOARD_ADLINK_PCI7296] = {
 		.name		= "adl_pci-7296",
-		.vendor		= PCI_VENDOR_ID_ADLINK,
-		.device		= PCI_DEVICE_ID_ADLINK_PCI7296,
 		.dio_badr	= 2,
 		.n_8255		= 4,
-	}, {
+	},
+	[BOARD_CB_PCIDIO24] = {
 		.name		= "cb_pci-dio24",
-		.vendor		= PCI_VENDOR_ID_CB,
-		.device		= PCI_DEVICE_ID_CB_PCIDIO24,
 		.dio_badr	= 2,
 		.n_8255		= 1,
-	}, {
+	},
+	[BOARD_CB_PCIDIO24H] = {
 		.name		= "cb_pci-dio24h",
-		.vendor		= PCI_VENDOR_ID_CB,
-		.device		= PCI_DEVICE_ID_CB_PCIDIO24H,
 		.dio_badr	= 2,
 		.n_8255		= 1,
-	}, {
+	},
+	[BOARD_CB_PCIDIO48H] = {
 		.name		= "cb_pci-dio48h",
-		.vendor		= PCI_VENDOR_ID_CB,
-		.device		= PCI_DEVICE_ID_CB_PCIDIO48H,
 		.dio_badr	= 1,
 		.n_8255		= 2,
-	}, {
+	},
+	[BOARD_CB_PCIDIO96H] = {
 		.name		= "cb_pci-dio96h",
-		.vendor		= PCI_VENDOR_ID_CB,
-		.device		= PCI_DEVICE_ID_CB_PCIDIO96H,
 		.dio_badr	= 2,
 		.n_8255		= 4,
-	}, {
+	},
+	[BOARD_NI_PCIDIO96] = {
 		.name		= "ni_pci-dio-96",
-		.vendor		= PCI_VENDOR_ID_NI,
-		.device		= PCI_DEVICE_ID_NI_PCIDIO96,
 		.dio_badr	= 1,
-		.is_mmio	= 1,
 		.n_8255		= 4,
-	}, {
+	},
+	[BOARD_NI_PCIDIO96B] = {
 		.name		= "ni_pci-dio-96b",
-		.vendor		= PCI_VENDOR_ID_NI,
-		.device		= PCI_DEVICE_ID_NI_PCIDIO96B,
 		.dio_badr	= 1,
-		.is_mmio	= 1,
 		.n_8255		= 4,
-	}, {
+	},
+	[BOARD_NI_PXI6508] = {
 		.name		= "ni_pxi-6508",
-		.vendor		= PCI_VENDOR_ID_NI,
-		.device		= PCI_DEVICE_ID_NI_PXI6508,
 		.dio_badr	= 1,
-		.is_mmio	= 1,
 		.n_8255		= 4,
-	}, {
+	},
+	[BOARD_NI_PCI6503] = {
 		.name		= "ni_pci-6503",
-		.vendor		= PCI_VENDOR_ID_NI,
-		.device		= PCI_DEVICE_ID_NI_PCI6503,
 		.dio_badr	= 1,
-		.is_mmio	= 1,
 		.n_8255		= 1,
-	}, {
+	},
+	[BOARD_NI_PCI6503B] = {
 		.name		= "ni_pci-6503b",
-		.vendor		= PCI_VENDOR_ID_NI,
-		.device		= PCI_DEVICE_ID_NI_PCI6503B,
 		.dio_badr	= 1,
-		.is_mmio	= 1,
 		.n_8255		= 1,
-	}, {
+	},
+	[BOARD_NI_PCI6503X] = {
 		.name		= "ni_pci-6503x",
-		.vendor		= PCI_VENDOR_ID_NI,
-		.device		= PCI_DEVICE_ID_NI_PCI6503X,
 		.dio_badr	= 1,
-		.is_mmio	= 1,
 		.n_8255		= 1,
-	}, {
+	},
+	[BOARD_NI_PXI_6503] = {
 		.name		= "ni_pxi-6503",
-		.vendor		= PCI_VENDOR_ID_NI,
-		.device		= PCI_DEVICE_ID_NI_PXI_6503,
 		.dio_badr	= 1,
-		.is_mmio	= 1,
 		.n_8255		= 1,
 	},
 };
@@ -200,34 +172,19 @@ static int pci_8255_mmio(int dir, int port, int data, unsigned long iobase)
 	}
 }
 
-static const void *pci_8255_find_boardinfo(struct comedi_device *dev,
-					      struct pci_dev *pcidev)
-{
-	const struct pci_8255_boardinfo *board;
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(pci_8255_boards); i++) {
-		board = &pci_8255_boards[i];
-		if (pcidev->vendor == board->vendor &&
-		    pcidev->device == board->device)
-			return board;
-	}
-	return NULL;
-}
-
 static int pci_8255_auto_attach(struct comedi_device *dev,
-					  unsigned long context_unused)
+				unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct pci_8255_boardinfo *board;
+	const struct pci_8255_boardinfo *board = NULL;
 	struct pci_8255_private *devpriv;
 	struct comedi_subdevice *s;
-	resource_size_t iobase;
-	unsigned long len;
+	bool is_mmio;
 	int ret;
 	int i;
 
-	board = pci_8255_find_boardinfo(dev, pcidev);
+	if (context < ARRAY_SIZE(pci_8255_boards))
+		board = &pci_8255_boards[context];
 	if (!board)
 		return -ENODEV;
 	dev->board_ptr = board;
@@ -238,18 +195,19 @@ static int pci_8255_auto_attach(struct comedi_device *dev,
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
-	iobase = pci_resource_start(pcidev, board->dio_badr);
-	len = pci_resource_len(pcidev, board->dio_badr);
 
-	if (board->is_mmio) {
-		devpriv->mmio_base = ioremap(iobase, len);
+	is_mmio = (pci_resource_flags(pcidev, board->dio_badr) &
+		   IORESOURCE_MEM) != 0;
+	if (is_mmio) {
+		devpriv->mmio_base = pci_ioremap_bar(pcidev, board->dio_badr);
 		if (!devpriv->mmio_base)
 			return -ENOMEM;
+	} else {
+		dev->iobase = pci_resource_start(pcidev, board->dio_badr);
 	}
-	dev->iobase = iobase;
 
 	/*
 	 * One, two, or four subdevices are setup by this driver depending
@@ -261,8 +219,10 @@ static int pci_8255_auto_attach(struct comedi_device *dev,
 		return ret;
 
 	for (i = 0; i < board->n_8255; i++) {
+		unsigned long iobase;
+
 		s = &dev->subdevices[i];
-		if (board->is_mmio) {
+		if (is_mmio) {
 			iobase = (unsigned long)(devpriv->mmio_base + (i * 4));
 			ret = subdev_8255_init(dev, s, pci_8255_mmio, iobase);
 		} else {
@@ -281,26 +241,14 @@ static int pci_8255_auto_attach(struct comedi_device *dev,
 
 static void pci_8255_detach(struct comedi_device *dev)
 {
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct pci_8255_boardinfo *board = comedi_board(dev);
 	struct pci_8255_private *devpriv = dev->private;
-	struct comedi_subdevice *s;
 	int i;
 
-	if (!board || !devpriv)
-		return;
-	if (dev->subdevices) {
-		for (i = 0; i < board->n_8255; i++) {
-			s = &dev->subdevices[i];
-			subdev_8255_cleanup(dev, s);
-		}
-	}
-	if (pcidev) {
-		if (devpriv->mmio_base)
-			iounmap(devpriv->mmio_base);
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-	}
+	for (i = 0; i < dev->n_subdevices; i++)
+		comedi_spriv_free(dev, i);
+	if (devpriv && devpriv->mmio_base)
+		iounmap(devpriv->mmio_base);
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver pci_8255_driver = {
@@ -311,26 +259,26 @@ static struct comedi_driver pci_8255_driver = {
 };
 
 static int pci_8255_pci_probe(struct pci_dev *dev,
-					const struct pci_device_id *ent)
+			      const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &pci_8255_driver);
+	return comedi_pci_auto_config(dev, &pci_8255_driver, id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(pci_8255_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_ADLINK_PCI7224) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_ADLINK_PCI7248) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_ADLINK_PCI7296) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, PCI_DEVICE_ID_CB_PCIDIO24) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, PCI_DEVICE_ID_CB_PCIDIO24H) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, PCI_DEVICE_ID_CB_PCIDIO48H) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, PCI_DEVICE_ID_CB_PCIDIO96H) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, PCI_DEVICE_ID_NI_PCIDIO96) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, PCI_DEVICE_ID_NI_PCIDIO96B) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, PCI_DEVICE_ID_NI_PXI6508) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, PCI_DEVICE_ID_NI_PCI6503) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, PCI_DEVICE_ID_NI_PCI6503B) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, PCI_DEVICE_ID_NI_PCI6503X) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, PCI_DEVICE_ID_NI_PXI_6503) },
+	{ PCI_VDEVICE(ADLINK, 0x7224), BOARD_ADLINK_PCI7224 },
+	{ PCI_VDEVICE(ADLINK, 0x7248), BOARD_ADLINK_PCI7248 },
+	{ PCI_VDEVICE(ADLINK, 0x7296), BOARD_ADLINK_PCI7296 },
+	{ PCI_VDEVICE(CB, 0x0028), BOARD_CB_PCIDIO24 },
+	{ PCI_VDEVICE(CB, 0x0014), BOARD_CB_PCIDIO24H },
+	{ PCI_VDEVICE(CB, 0x000b), BOARD_CB_PCIDIO48H },
+	{ PCI_VDEVICE(CB, 0x0017), BOARD_CB_PCIDIO96H },
+	{ PCI_VDEVICE(NI, 0x0160), BOARD_NI_PCIDIO96 },
+	{ PCI_VDEVICE(NI, 0x1630), BOARD_NI_PCIDIO96B },
+	{ PCI_VDEVICE(NI, 0x13c0), BOARD_NI_PXI6508 },
+	{ PCI_VDEVICE(NI, 0x0400), BOARD_NI_PCI6503 },
+	{ PCI_VDEVICE(NI, 0x1250), BOARD_NI_PCI6503B },
+	{ PCI_VDEVICE(NI, 0x17d0), BOARD_NI_PCI6503X },
+	{ PCI_VDEVICE(NI, 0x1800), BOARD_NI_PXI_6503 },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, pci_8255_pci_table);
diff --git a/drivers/staging/comedi/drivers/Makefile b/drivers/staging/comedi/drivers/Makefile
index 315e836ff99..57e984f0f46 100644
--- a/drivers/staging/comedi/drivers/Makefile
+++ b/drivers/staging/comedi/drivers/Makefile
@@ -12,6 +12,8 @@ obj-$(CONFIG_COMEDI_SKEL)		+= skel.o
 
 # Comedi ISA drivers
 obj-$(CONFIG_COMEDI_ACL7225B)		+= acl7225b.o
+obj-$(CONFIG_COMEDI_AMPLC_DIO200_ISA)	+= amplc_dio200.o
+obj-$(CONFIG_COMEDI_AMPLC_PC263_ISA)	+= amplc_pc263.o
 obj-$(CONFIG_COMEDI_PCL711)		+= pcl711.o
 obj-$(CONFIG_COMEDI_PCL724)		+= pcl724.o
 obj-$(CONFIG_COMEDI_PCL725)		+= pcl725.o
@@ -75,10 +77,11 @@ obj-$(CONFIG_COMEDI_ADL_PCI9111)	+= adl_pci9111.o
 obj-$(CONFIG_COMEDI_ADL_PCI9118)	+= adl_pci9118.o
 obj-$(CONFIG_COMEDI_ADV_PCI1710)	+= adv_pci1710.o
 obj-$(CONFIG_COMEDI_ADV_PCI1723)	+= adv_pci1723.o
+obj-$(CONFIG_COMEDI_ADV_PCI1724)	+= adv_pci1724.o
 obj-$(CONFIG_COMEDI_ADV_PCI_DIO)	+= adv_pci_dio.o
-obj-$(CONFIG_COMEDI_AMPLC_DIO200)	+= amplc_dio200.o
+obj-$(CONFIG_COMEDI_AMPLC_DIO200_PCI)	+= amplc_dio200_pci.o
 obj-$(CONFIG_COMEDI_AMPLC_PC236)	+= amplc_pc236.o
-obj-$(CONFIG_COMEDI_AMPLC_PC263)	+= amplc_pc263.o
+obj-$(CONFIG_COMEDI_AMPLC_PC263_PCI)	+= amplc_pci263.o
 obj-$(CONFIG_COMEDI_AMPLC_PCI224)	+= amplc_pci224.o
 obj-$(CONFIG_COMEDI_AMPLC_PCI230)	+= amplc_pci230.o
 obj-$(CONFIG_COMEDI_CONTEC_PCI_DIO)	+= contec_pci_dio.o
@@ -103,6 +106,7 @@ obj-$(CONFIG_COMEDI_NI_6527)		+= ni_6527.o
 obj-$(CONFIG_COMEDI_NI_65XX)		+= ni_65xx.o
 obj-$(CONFIG_COMEDI_NI_660X)		+= ni_660x.o
 obj-$(CONFIG_COMEDI_NI_670X)		+= ni_670x.o
+obj-$(CONFIG_COMEDI_NI_LABPC_PCI)	+= ni_labpc_pci.o
 obj-$(CONFIG_COMEDI_NI_PCIDIO)		+= ni_pcidio.o
 obj-$(CONFIG_COMEDI_NI_PCIMIO)		+= ni_pcimio.o
 obj-$(CONFIG_COMEDI_RTD520)		+= rtd520.o
@@ -133,5 +137,6 @@ obj-$(CONFIG_COMEDI_NI_TIOCMD)		+= ni_tiocmd.o
 obj-$(CONFIG_COMEDI_NI_LABPC)		+= ni_labpc.o
 
 obj-$(CONFIG_COMEDI_8255)		+= 8255.o
+obj-$(CONFIG_COMEDI_AMPLC_DIO200)	+= amplc_dio200_common.o
 obj-$(CONFIG_COMEDI_DAS08)		+= das08.o
 obj-$(CONFIG_COMEDI_FC)			+= comedi_fc.o
diff --git a/drivers/staging/comedi/drivers/acl7225b.c b/drivers/staging/comedi/drivers/acl7225b.c
index 28c7fd3a96b..9e2c7aeea53 100644
--- a/drivers/staging/comedi/drivers/acl7225b.c
+++ b/drivers/staging/comedi/drivers/acl7225b.c
@@ -15,43 +15,59 @@ Devices: [Adlink] ACL-7225b (acl7225b), [ICP] P16R16DIO (p16r16dio)
 
 #include <linux/ioport.h>
 
-#define ACL7225_SIZE   8	/* Requires 8 ioports, but only 4 are used */
-#define P16R16DIO_SIZE 4
 #define ACL7225_RIO_LO 0	/* Relays input/output low byte (R0-R7) */
 #define ACL7225_RIO_HI 1	/* Relays input/output high byte (R8-R15) */
 #define ACL7225_DI_LO  2	/* Digital input low byte (DI0-DI7) */
 #define ACL7225_DI_HI  3	/* Digital input high byte (DI8-DI15) */
 
-struct boardtype {
-	const char *name;	/*  driver name */
-	int io_range;		/*  len of I/O space */
+struct acl7225b_boardinfo {
+	const char *name;
+	int io_range;
 };
 
-static int acl7225b_do_insn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data)
+static const struct acl7225b_boardinfo acl7225b_boards[] = {
+	{
+		.name		= "acl7225b",
+		.io_range	= 8,		/* only 4 are used */
+	}, {
+		.name		= "p16r16dio",
+		.io_range	= 4,
+	},
+};
+
+static int acl7225b_do_insn_bits(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 struct comedi_insn *insn,
+				 unsigned int *data)
 {
-	if (data[0]) {
-		s->state &= ~data[0];
-		s->state |= (data[0] & data[1]);
+	unsigned long reg = (unsigned long)s->private;
+	unsigned int mask = data[0];
+	unsigned int bits = data[1];
+
+	if (mask) {
+		s->state &= ~mask;
+		s->state |= (bits & mask);
+
+		if (mask & 0x00ff)
+			outb(s->state & 0xff, dev->iobase + reg);
+		if (mask & 0xff00)
+			outb((s->state >> 8), dev->iobase + reg + 1);
 	}
-	if (data[0] & 0x00ff)
-		outb(s->state & 0xff, dev->iobase + (unsigned long)s->private);
-	if (data[0] & 0xff00)
-		outb((s->state >> 8),
-		     dev->iobase + (unsigned long)s->private + 1);
 
 	data[1] = s->state;
 
 	return insn->n;
 }
 
-static int acl7225b_di_insn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data)
+static int acl7225b_di_insn_bits(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 struct comedi_insn *insn,
+				 unsigned int *data)
 {
-	data[1] = inb(dev->iobase + (unsigned long)s->private) |
-	    (inb(dev->iobase + (unsigned long)s->private + 1) << 8);
+	unsigned long reg = (unsigned long)s->private;
+
+	data[1] = inb(dev->iobase + reg) |
+		  (inb(dev->iobase + reg + 1) << 8);
 
 	return insn->n;
 }
@@ -59,23 +75,13 @@ static int acl7225b_di_insn(struct comedi_device *dev,
 static int acl7225b_attach(struct comedi_device *dev,
 			   struct comedi_devconfig *it)
 {
-	const struct boardtype *board = comedi_board(dev);
+	const struct acl7225b_boardinfo *board = comedi_board(dev);
 	struct comedi_subdevice *s;
-	int iobase, iorange;
 	int ret;
 
-	iobase = it->options[0];
-	iorange = board->io_range;
-	printk(KERN_INFO "comedi%d: acl7225b: board=%s 0x%04x\n", dev->minor,
-	       board->name, iobase);
-	if (!request_region(iobase, iorange, "acl7225b")) {
-		printk(KERN_ERR "comedi%d: request_region failed - I/O port conflict\n",
-			dev->minor);
-		return -EIO;
-	}
-	dev->board_name = board->name;
-	dev->iobase = iobase;
-	dev->irq = 0;
+	ret = comedi_request_region(dev, it->options[0], board->io_range);
+	if (ret)
+		return ret;
 
 	ret = comedi_alloc_subdevices(dev, 3);
 	if (ret)
@@ -83,61 +89,48 @@ static int acl7225b_attach(struct comedi_device *dev,
 
 	s = &dev->subdevices[0];
 	/* Relays outputs */
-	s->type = COMEDI_SUBD_DO;
-	s->subdev_flags = SDF_WRITABLE;
-	s->maxdata = 1;
-	s->n_chan = 16;
-	s->insn_bits = acl7225b_do_insn;
-	s->range_table = &range_digital;
-	s->private = (void *)ACL7225_RIO_LO;
+	s->type		= COMEDI_SUBD_DO;
+	s->subdev_flags	= SDF_WRITABLE;
+	s->maxdata	= 1;
+	s->n_chan	= 16;
+	s->insn_bits	= acl7225b_do_insn_bits;
+	s->range_table	= &range_digital;
+	s->private	= (void *)ACL7225_RIO_LO;
 
 	s = &dev->subdevices[1];
 	/* Relays status */
-	s->type = COMEDI_SUBD_DI;
-	s->subdev_flags = SDF_READABLE;
-	s->maxdata = 1;
-	s->n_chan = 16;
-	s->insn_bits = acl7225b_di_insn;
-	s->range_table = &range_digital;
-	s->private = (void *)ACL7225_RIO_LO;
+	s->type		= COMEDI_SUBD_DI;
+	s->subdev_flags	= SDF_READABLE;
+	s->maxdata	= 1;
+	s->n_chan	= 16;
+	s->insn_bits	= acl7225b_di_insn_bits;
+	s->range_table	= &range_digital;
+	s->private	= (void *)ACL7225_RIO_LO;
 
 	s = &dev->subdevices[2];
 	/* Isolated digital inputs */
-	s->type = COMEDI_SUBD_DI;
-	s->subdev_flags = SDF_READABLE;
-	s->maxdata = 1;
-	s->n_chan = 16;
-	s->insn_bits = acl7225b_di_insn;
-	s->range_table = &range_digital;
-	s->private = (void *)ACL7225_DI_LO;
+	s->type		= COMEDI_SUBD_DI;
+	s->subdev_flags	= SDF_READABLE;
+	s->maxdata	= 1;
+	s->n_chan	= 16;
+	s->insn_bits	= acl7225b_di_insn_bits;
+	s->range_table	= &range_digital;
+	s->private	= (void *)ACL7225_DI_LO;
 
 	return 0;
 }
 
-static void acl7225b_detach(struct comedi_device *dev)
-{
-	const struct boardtype *board = comedi_board(dev);
-
-	if (dev->iobase)
-		release_region(dev->iobase, board->io_range);
-}
-
-static const struct boardtype boardtypes[] = {
-	{ "acl7225b", ACL7225_SIZE, },
-	{ "p16r16dio", P16R16DIO_SIZE, },
-};
-
 static struct comedi_driver acl7225b_driver = {
 	.driver_name	= "acl7225b",
 	.module		= THIS_MODULE,
 	.attach		= acl7225b_attach,
-	.detach		= acl7225b_detach,
-	.board_name	= &boardtypes[0].name,
-	.num_names	= ARRAY_SIZE(boardtypes),
-	.offset		= sizeof(struct boardtype),
+	.detach		= comedi_legacy_detach,
+	.board_name	= &acl7225b_boards[0].name,
+	.num_names	= ARRAY_SIZE(acl7225b_boards),
+	.offset		= sizeof(struct acl7225b_boardinfo),
 };
 module_comedi_driver(acl7225b_driver);
 
+MODULE_DESCRIPTION("Comedi: NuDAQ ACL-7225B, 16 Relay & 16 Isolated DI Card");
 MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/addi-data/APCI1710_Ssi.c b/drivers/staging/comedi/drivers/addi-data/APCI1710_Ssi.c
index 1e05732e9f3..97e7eec343d 100644
--- a/drivers/staging/comedi/drivers/addi-data/APCI1710_Ssi.c
+++ b/drivers/staging/comedi/drivers/addi-data/APCI1710_Ssi.c
@@ -704,9 +704,9 @@ static int i_APCI1710_InsnReadSSIValue(struct comedi_device *dev,
 |                                        unsigned char *_   pb_ChannelStatus)          |
 +----------------------------------------------------------------------------+
 | Task              :
-					(0) Set the digital output from selected SSI moule         |
+					(0) Set the digital output from selected SSI module         |
 |                     (b_ModuleNbr) ON
-                    (1) Set the digital output from selected SSI moule         |
+                    (1) Set the digital output from selected SSI module         |
 |                     (b_ModuleNbr) OFF
 					(2)Read the status from selected SSI digital input        |
 |                     (b_InputChannel)
diff --git a/drivers/staging/comedi/drivers/addi-data/addi_common.c b/drivers/staging/comedi/drivers/addi-data/addi_common.c
index 1051fa5ce8f..0c3db57a50f 100644
--- a/drivers/staging/comedi/drivers/addi-data/addi_common.c
+++ b/drivers/staging/comedi/drivers/addi-data/addi_common.c
@@ -84,37 +84,16 @@ static int i_ADDI_Reset(struct comedi_device *dev)
 	return 0;
 }
 
-static const void *addi_find_boardinfo(struct comedi_device *dev,
-				       struct pci_dev *pcidev)
-{
-	const void *p = dev->driver->board_name;
-	const struct addi_board *this_board;
-	int i;
-
-	for (i = 0; i < dev->driver->num_names; i++) {
-		this_board = p;
-		if (this_board->i_VendorId == pcidev->vendor &&
-		    this_board->i_DeviceId == pcidev->device)
-			return this_board;
-		p += dev->driver->offset;
-	}
-	return NULL;
-}
-
 static int addi_auto_attach(struct comedi_device *dev,
 				      unsigned long context_unused)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct addi_board *this_board;
+	const struct addi_board *this_board = comedi_board(dev);
 	struct addi_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret, n_subdevices;
 	unsigned int dw_Dummy;
 
-	this_board = addi_find_boardinfo(dev, pcidev);
-	if (!this_board)
-		return -ENODEV;
-	dev->board_ptr = this_board;
 	dev->board_name = this_board->pc_DriverName;
 
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
@@ -122,7 +101,7 @@ static int addi_auto_attach(struct comedi_device *dev,
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
 
@@ -141,8 +120,7 @@ static int addi_auto_attach(struct comedi_device *dev,
 		/* board has an ADDIDATA_9054 eeprom */
 		dev->iobase = pci_resource_start(pcidev, 2);
 		devpriv->iobase = pci_resource_start(pcidev, 2);
-		devpriv->dw_AiBase = ioremap(pci_resource_start(pcidev, 3),
-					     this_board->i_IorangeBase3);
+		devpriv->dw_AiBase = pci_ioremap_bar(pcidev, 3);
 	}
 	devpriv->i_IobaseReserved = pci_resource_start(pcidev, 3);
 
@@ -338,7 +316,6 @@ static int addi_auto_attach(struct comedi_device *dev,
 
 static void i_ADDI_Detach(struct comedi_device *dev)
 {
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct addi_private *devpriv = dev->private;
 
 	if (devpriv) {
@@ -349,8 +326,5 @@ static void i_ADDI_Detach(struct comedi_device *dev)
 		if (devpriv->dw_AiBase)
 			iounmap(devpriv->dw_AiBase);
 	}
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-	}
+	comedi_pci_disable(dev);
 }
diff --git a/drivers/staging/comedi/drivers/addi-data/addi_common.h b/drivers/staging/comedi/drivers/addi-data/addi_common.h
index 6d8b29f945d..c034bf1426e 100644
--- a/drivers/staging/comedi/drivers/addi-data/addi_common.h
+++ b/drivers/staging/comedi/drivers/addi-data/addi_common.h
@@ -45,12 +45,7 @@
 /* structure for the boardtype */
 struct addi_board {
 	const char *pc_DriverName;	/*  driver name */
-	int i_VendorId;		/* PCI vendor a device ID of card */
-	int i_DeviceId;
-	int i_IorangeBase0;
 	int i_IorangeBase1;
-	int i_IorangeBase2;	/*   base 2 range */
-	int i_IorangeBase3;	/*   base 3 range */
 	int i_PCIEeprom;	/*  eeprom present or not */
 	char *pc_EepromChip;	/*  type of chip */
 	int i_NbrAiChannel;	/*  num of A/D chans */
diff --git a/drivers/staging/comedi/drivers/addi_apci_035.c b/drivers/staging/comedi/drivers/addi_apci_035.c
index 5a53e58258a..43c2c10a7c3 100644
--- a/drivers/staging/comedi/drivers/addi_apci_035.c
+++ b/drivers/staging/comedi/drivers/addi_apci_035.c
@@ -15,9 +15,6 @@
 static const struct addi_board apci035_boardtypes[] = {
 	{
 		.pc_DriverName		= "apci035",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x0300,
-		.i_IorangeBase0		= 127,
 		.i_IorangeBase1		= APCI035_ADDRESS_RANGE,
 		.i_PCIEeprom		= 1,
 		.pc_EepromChip		= ADDIDATA_S5920,
@@ -39,20 +36,25 @@ static const struct addi_board apci035_boardtypes[] = {
 	},
 };
 
+static int apci035_auto_attach(struct comedi_device *dev,
+			       unsigned long context)
+{
+	dev->board_ptr = &apci035_boardtypes[0];
+
+	return addi_auto_attach(dev, context);
+}
+
 static struct comedi_driver apci035_driver = {
 	.driver_name	= "addi_apci_035",
 	.module		= THIS_MODULE,
-	.auto_attach	= addi_auto_attach,
+	.auto_attach	= apci035_auto_attach,
 	.detach		= i_ADDI_Detach,
-	.num_names	= ARRAY_SIZE(apci035_boardtypes),
-	.board_name	= &apci035_boardtypes[0].pc_DriverName,
-	.offset		= sizeof(struct addi_board),
 };
 
 static int apci035_pci_probe(struct pci_dev *dev,
-				       const struct pci_device_id *ent)
+			     const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &apci035_driver);
+	return comedi_pci_auto_config(dev, &apci035_driver, id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(apci035_pci_table) = {
diff --git a/drivers/staging/comedi/drivers/addi_apci_1032.c b/drivers/staging/comedi/drivers/addi_apci_1032.c
index c0d0429c35c..3d4878facc2 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1032.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1032.c
@@ -296,14 +296,12 @@ static int apci1032_auto_attach(struct comedi_device *dev,
 	struct comedi_subdevice *s;
 	int ret;
 
-	dev->board_name = dev->driver->driver_name;
-
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
 
@@ -353,16 +351,11 @@ static int apci1032_auto_attach(struct comedi_device *dev,
 
 static void apci1032_detach(struct comedi_device *dev)
 {
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-
 	if (dev->iobase)
 		apci1032_reset(dev);
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-	}
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver apci1032_driver = {
@@ -373,9 +366,9 @@ static struct comedi_driver apci1032_driver = {
 };
 
 static int apci1032_pci_probe(struct pci_dev *dev,
-					const struct pci_device_id *ent)
+			      const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &apci1032_driver);
+	return comedi_pci_auto_config(dev, &apci1032_driver, id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(apci1032_pci_table) = {
diff --git a/drivers/staging/comedi/drivers/addi_apci_1500.c b/drivers/staging/comedi/drivers/addi_apci_1500.c
index 9c2f8eeb797..b52cfe01e6c 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1500.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1500.c
@@ -13,11 +13,7 @@
 static const struct addi_board apci1500_boardtypes[] = {
 	{
 		.pc_DriverName		= "apci1500",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA_OLD,
-		.i_DeviceId		= 0x80fc,
-		.i_IorangeBase0		= 128,
 		.i_IorangeBase1		= APCI1500_ADDRESS_RANGE,
-		.i_IorangeBase2		= 4,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.i_NbrDiChannel		= 16,
 		.i_NbrDoChannel		= 16,
@@ -39,24 +35,29 @@ static const struct addi_board apci1500_boardtypes[] = {
 	},
 };
 
+static int apci1500_auto_attach(struct comedi_device *dev,
+				unsigned long context)
+{
+	dev->board_ptr = &apci1500_boardtypes[0];
+
+	return addi_auto_attach(dev, context);
+}
+
 static struct comedi_driver apci1500_driver = {
 	.driver_name	= "addi_apci_1500",
 	.module		= THIS_MODULE,
-	.auto_attach	= addi_auto_attach,
+	.auto_attach	= apci1500_auto_attach,
 	.detach		= i_ADDI_Detach,
-	.num_names	= ARRAY_SIZE(apci1500_boardtypes),
-	.board_name	= &apci1500_boardtypes[0].pc_DriverName,
-	.offset		= sizeof(struct addi_board),
 };
 
 static int apci1500_pci_probe(struct pci_dev *dev,
-					const struct pci_device_id *ent)
+			      const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &apci1500_driver);
+	return comedi_pci_auto_config(dev, &apci1500_driver, id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(apci1500_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA_OLD, 0x80fc) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_AMCC, 0x80fc) },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, apci1500_pci_table);
diff --git a/drivers/staging/comedi/drivers/addi_apci_1516.c b/drivers/staging/comedi/drivers/addi_apci_1516.c
index 69e39963841..ed01c56630b 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1516.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1516.c
@@ -36,13 +36,6 @@
 #include "comedi_fc.h"
 
 /*
- * PCI device ids supported by this driver
- */
-#define PCI_DEVICE_ID_APCI1016		0x1000
-#define PCI_DEVICE_ID_APCI1516		0x1001
-#define PCI_DEVICE_ID_APCI2016		0x1002
-
-/*
  * PCI bar 1 I/O Register map - Digital input/output
  */
 #define APCI1516_DI_REG			0x00
@@ -53,28 +46,32 @@
  */
 #define APCI1516_WDOG_REG		0x00
 
+enum apci1516_boardid {
+	BOARD_APCI1016,
+	BOARD_APCI1516,
+	BOARD_APCI2016,
+};
+
 struct apci1516_boardinfo {
 	const char *name;
-	unsigned short device;
 	int di_nchan;
 	int do_nchan;
 	int has_wdog;
 };
 
 static const struct apci1516_boardinfo apci1516_boardtypes[] = {
-	{
+	[BOARD_APCI1016] = {
 		.name		= "apci1016",
-		.device		= PCI_DEVICE_ID_APCI1016,
 		.di_nchan	= 16,
-	}, {
+	},
+	[BOARD_APCI1516] = {
 		.name		= "apci1516",
-		.device		= PCI_DEVICE_ID_APCI1516,
 		.di_nchan	= 8,
 		.do_nchan	= 8,
 		.has_wdog	= 1,
-	}, {
+	},
+	[BOARD_APCI2016] = {
 		.name		= "apci2016",
-		.device		= PCI_DEVICE_ID_APCI2016,
 		.do_nchan	= 16,
 		.has_wdog	= 1,
 	},
@@ -130,30 +127,17 @@ static int apci1516_reset(struct comedi_device *dev)
 	return 0;
 }
 
-static const void *apci1516_find_boardinfo(struct comedi_device *dev,
-					   struct pci_dev *pcidev)
-{
-	const struct apci1516_boardinfo *this_board;
-	int i;
-
-	for (i = 0; i < dev->driver->num_names; i++) {
-		this_board = &apci1516_boardtypes[i];
-		if (this_board->device == pcidev->device)
-			return this_board;
-	}
-	return NULL;
-}
-
 static int apci1516_auto_attach(struct comedi_device *dev,
-					  unsigned long context_unused)
+				unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct apci1516_boardinfo *this_board;
+	const struct apci1516_boardinfo *this_board = NULL;
 	struct apci1516_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret;
 
-	this_board = apci1516_find_boardinfo(dev, pcidev);
+	if (context < ARRAY_SIZE(apci1516_boardtypes))
+		this_board = &apci1516_boardtypes[context];
 	if (!this_board)
 		return -ENODEV;
 	dev->board_ptr = this_board;
@@ -164,7 +148,7 @@ static int apci1516_auto_attach(struct comedi_device *dev,
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
 
@@ -217,14 +201,10 @@ static int apci1516_auto_attach(struct comedi_device *dev,
 
 static void apci1516_detach(struct comedi_device *dev)
 {
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-
 	if (dev->iobase)
 		apci1516_reset(dev);
-	if (dev->subdevices)
-		addi_watchdog_cleanup(&dev->subdevices[2]);
-	if (dev->iobase)
-		comedi_pci_disable(pcidev);
+	comedi_spriv_free(dev, 2);
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver apci1516_driver = {
@@ -235,15 +215,15 @@ static struct comedi_driver apci1516_driver = {
 };
 
 static int apci1516_pci_probe(struct pci_dev *dev,
-					const struct pci_device_id *ent)
+			      const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &apci1516_driver);
+	return comedi_pci_auto_config(dev, &apci1516_driver, id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(apci1516_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, PCI_DEVICE_ID_APCI1016) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, PCI_DEVICE_ID_APCI1516) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, PCI_DEVICE_ID_APCI2016) },
+	{ PCI_VDEVICE(ADDIDATA, 0x1000), BOARD_APCI1016 },
+	{ PCI_VDEVICE(ADDIDATA, 0x1001), BOARD_APCI1516 },
+	{ PCI_VDEVICE(ADDIDATA, 0x1002), BOARD_APCI2016 },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, apci1516_pci_table);
diff --git a/drivers/staging/comedi/drivers/addi_apci_1564.c b/drivers/staging/comedi/drivers/addi_apci_1564.c
index ddea64df918..22bace62210 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1564.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1564.c
@@ -13,9 +13,6 @@
 static const struct addi_board apci1564_boardtypes[] = {
 	{
 		.pc_DriverName		= "apci1564",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x1006,
-		.i_IorangeBase0		= 128,
 		.i_IorangeBase1		= APCI1564_ADDRESS_RANGE,
 		.i_PCIEeprom		= ADDIDATA_EEPROM,
 		.pc_EepromChip		= ADDIDATA_93C76,
@@ -36,20 +33,25 @@ static const struct addi_board apci1564_boardtypes[] = {
 	},
 };
 
+static int apci1564_auto_attach(struct comedi_device *dev,
+				unsigned long context)
+{
+	dev->board_ptr = &apci1564_boardtypes[0];
+
+	return addi_auto_attach(dev, context);
+}
+
 static struct comedi_driver apci1564_driver = {
 	.driver_name	= "addi_apci_1564",
 	.module		= THIS_MODULE,
-	.auto_attach	= addi_auto_attach,
+	.auto_attach	= apci1564_auto_attach,
 	.detach		= i_ADDI_Detach,
-	.num_names	= ARRAY_SIZE(apci1564_boardtypes),
-	.board_name	= &apci1564_boardtypes[0].pc_DriverName,
-	.offset		= sizeof(struct addi_board),
 };
 
 static int apci1564_pci_probe(struct pci_dev *dev,
-					const struct pci_device_id *ent)
+			      const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &apci1564_driver);
+	return comedi_pci_auto_config(dev, &apci1564_driver, id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(apci1564_pci_table) = {
diff --git a/drivers/staging/comedi/drivers/addi_apci_16xx.c b/drivers/staging/comedi/drivers/addi_apci_16xx.c
index e51f8000136..4c6a9b5a06a 100644
--- a/drivers/staging/comedi/drivers/addi_apci_16xx.c
+++ b/drivers/staging/comedi/drivers/addi_apci_16xx.c
@@ -34,35 +34,29 @@
 #include "../comedidev.h"
 
 /*
- * PCI device ids supported by this driver
- */
-#define PCI_DEVICE_ID_APCI1648		0x1009
-#define PCI_DEVICE_ID_APCI1696		0x100a
-
-/*
  * Register I/O map
  */
 #define APCI16XX_IN_REG(x)		(((x) * 4) + 0x08)
 #define APCI16XX_OUT_REG(x)		(((x) * 4) + 0x14)
 #define APCI16XX_DIR_REG(x)		(((x) * 4) + 0x20)
 
+enum apci16xx_boardid {
+	BOARD_APCI1648,
+	BOARD_APCI1696,
+};
+
 struct apci16xx_boardinfo {
 	const char *name;
-	unsigned short vendor;
-	unsigned short device;
 	int n_chan;
 };
 
 static const struct apci16xx_boardinfo apci16xx_boardtypes[] = {
-	{
+	[BOARD_APCI1648] = {
 		.name		= "apci1648",
-		.vendor		= PCI_VENDOR_ID_ADDIDATA,
-		.device		= PCI_DEVICE_ID_APCI1648,
 		.n_chan		= 48,		/* 2 subdevices */
-	}, {
+	},
+	[BOARD_APCI1696] = {
 		.name		= "apci1696",
-		.vendor		= PCI_VENDOR_ID_ADDIDATA,
-		.device		= PCI_DEVICE_ID_APCI1696,
 		.n_chan		= 96,		/* 3 subdevices */
 	},
 };
@@ -130,39 +124,25 @@ static int apci16xx_dio_insn_bits(struct comedi_device *dev,
 	return insn->n;
 }
 
-static const void *apci16xx_find_boardinfo(struct comedi_device *dev,
-					   struct pci_dev *pcidev)
-{
-	const struct apci16xx_boardinfo *board;
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(apci16xx_boardtypes); i++) {
-		board = &apci16xx_boardtypes[i];
-		if (board->vendor == pcidev->vendor &&
-		    board->device == pcidev->device)
-			return board;
-	}
-	return NULL;
-}
-
 static int apci16xx_auto_attach(struct comedi_device *dev,
-				unsigned long context_unused)
+				unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct apci16xx_boardinfo *board;
+	const struct apci16xx_boardinfo *board = NULL;
 	struct comedi_subdevice *s;
 	unsigned int n_subdevs;
 	unsigned int last;
 	int i;
 	int ret;
 
-	board = apci16xx_find_boardinfo(dev, pcidev);
+	if (context < ARRAY_SIZE(apci16xx_boardtypes))
+		board = &apci16xx_boardtypes[context];
 	if (!board)
 		return -ENODEV;
 	dev->board_ptr = board;
 	dev->board_name = board->name;
 
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
 
@@ -204,35 +184,22 @@ static int apci16xx_auto_attach(struct comedi_device *dev,
 	return 0;
 }
 
-static void apci16xx_detach(struct comedi_device *dev)
-{
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-	}
-}
-
 static struct comedi_driver apci16xx_driver = {
 	.driver_name	= "addi_apci_16xx",
 	.module		= THIS_MODULE,
 	.auto_attach	= apci16xx_auto_attach,
-	.detach		= apci16xx_detach,
-	.num_names	= ARRAY_SIZE(apci16xx_boardtypes),
-	.board_name	= &apci16xx_boardtypes[0].name,
-	.offset		= sizeof(struct apci16xx_boardinfo),
+	.detach		= comedi_pci_disable,
 };
 
 static int apci16xx_pci_probe(struct pci_dev *dev,
-					const struct pci_device_id *ent)
+			      const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &apci16xx_driver);
+	return comedi_pci_auto_config(dev, &apci16xx_driver, id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(apci16xx_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, PCI_DEVICE_ID_APCI1648) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, PCI_DEVICE_ID_APCI1696) },
+	{ PCI_VDEVICE(ADDIDATA, 0x1009), BOARD_APCI1648 },
+	{ PCI_VDEVICE(ADDIDATA, 0x100a), BOARD_APCI1696 },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, apci16xx_pci_table);
diff --git a/drivers/staging/comedi/drivers/addi_apci_1710.c b/drivers/staging/comedi/drivers/addi_apci_1710.c
index e83e829831b..c9e6471eb06 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1710.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1710.c
@@ -21,72 +21,29 @@ static void fpu_end(void)
 #include "addi-data/addi_eeprom.c"
 #include "addi-data/hwdrv_APCI1710.c"
 
-static const struct addi_board apci1710_boardtypes[] = {
-	{
-		.pc_DriverName		= "apci1710",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA_OLD,
-		.i_DeviceId		= APCI1710_BOARD_DEVICE_ID,
-		.interrupt		= v_APCI1710_Interrupt,
-	},
-};
-
 static irqreturn_t v_ADDI_Interrupt(int irq, void *d)
 {
-	struct comedi_device *dev = d;
-	const struct addi_board *this_board = comedi_board(dev);
-
-	this_board->interrupt(irq, d);
+	v_APCI1710_Interrupt(irq, d);
 	return IRQ_RETVAL(1);
 }
 
-static const void *apci1710_find_boardinfo(struct comedi_device *dev,
-					   struct pci_dev *pcidev)
-{
-	const struct addi_board *this_board;
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(apci1710_boardtypes); i++) {
-		this_board = &apci1710_boardtypes[i];
-		if (this_board->i_VendorId == pcidev->vendor &&
-		    this_board->i_DeviceId == pcidev->device)
-			return this_board;
-	}
-	return NULL;
-}
-
 static int apci1710_auto_attach(struct comedi_device *dev,
 					  unsigned long context_unused)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct addi_board *this_board;
 	struct addi_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret;
 
-	this_board = apci1710_find_boardinfo(dev, pcidev);
-	if (!this_board)
-		return -ENODEV;
-	dev->board_ptr = this_board;
-	dev->board_name = this_board->pc_DriverName;
-
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
-
-	if (this_board->i_IorangeBase1)
-		dev->iobase = pci_resource_start(pcidev, 1);
-	else
-		dev->iobase = pci_resource_start(pcidev, 0);
-
-	devpriv->iobase = dev->iobase;
-	devpriv->i_IobaseAmcc = pci_resource_start(pcidev, 0);
-	devpriv->i_IobaseAddon = pci_resource_start(pcidev, 2);
-	devpriv->i_IobaseReserved = pci_resource_start(pcidev, 3);
+	devpriv->s_BoardInfos.ui_Address = pci_resource_start(pcidev, 2);
 
 	if (pcidev->irq > 0) {
 		ret = request_irq(pcidev->irq, v_ADDI_Interrupt, IRQF_SHARED,
@@ -97,24 +54,17 @@ static int apci1710_auto_attach(struct comedi_device *dev,
 
 	i_ADDI_AttachPCI1710(dev);
 
-	devpriv->s_BoardInfos.ui_Address = pci_resource_start(pcidev, 2);
-
 	i_APCI1710_Reset(dev);
 	return 0;
 }
 
 static void apci1710_detach(struct comedi_device *dev)
 {
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-
 	if (dev->iobase)
 		i_APCI1710_Reset(dev);
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-	}
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver apci1710_driver = {
@@ -125,13 +75,13 @@ static struct comedi_driver apci1710_driver = {
 };
 
 static int apci1710_pci_probe(struct pci_dev *dev,
-					const struct pci_device_id *ent)
+			      const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &apci1710_driver);
+	return comedi_pci_auto_config(dev, &apci1710_driver, id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(apci1710_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA_OLD, APCI1710_BOARD_DEVICE_ID) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_AMCC, APCI1710_BOARD_DEVICE_ID) },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, apci1710_pci_table);
diff --git a/drivers/staging/comedi/drivers/addi_apci_2032.c b/drivers/staging/comedi/drivers/addi_apci_2032.c
index 9ce1d26aff2..b666637f61b 100644
--- a/drivers/staging/comedi/drivers/addi_apci_2032.c
+++ b/drivers/staging/comedi/drivers/addi_apci_2032.c
@@ -287,9 +287,7 @@ static int apci2032_auto_attach(struct comedi_device *dev,
 	struct comedi_subdevice *s;
 	int ret;
 
-	dev->board_name = dev->driver->driver_name;
-
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
 	dev->iobase = pci_resource_start(pcidev, 1);
@@ -350,20 +348,14 @@ static int apci2032_auto_attach(struct comedi_device *dev,
 
 static void apci2032_detach(struct comedi_device *dev)
 {
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-
 	if (dev->iobase)
 		apci2032_reset(dev);
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 	if (dev->read_subdev)
 		kfree(dev->read_subdev->private);
-	if (dev->subdevices)
-		addi_watchdog_cleanup(&dev->subdevices[1]);
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-	}
+	comedi_spriv_free(dev, 1);
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver apci2032_driver = {
@@ -374,9 +366,9 @@ static struct comedi_driver apci2032_driver = {
 };
 
 static int apci2032_pci_probe(struct pci_dev *dev,
-					const struct pci_device_id *ent)
+			      const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &apci2032_driver);
+	return comedi_pci_auto_config(dev, &apci2032_driver, id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(apci2032_pci_table) = {
diff --git a/drivers/staging/comedi/drivers/addi_apci_2200.c b/drivers/staging/comedi/drivers/addi_apci_2200.c
index b1c4226902e..1cdc08d7979 100644
--- a/drivers/staging/comedi/drivers/addi_apci_2200.c
+++ b/drivers/staging/comedi/drivers/addi_apci_2200.c
@@ -88,9 +88,7 @@ static int apci2200_auto_attach(struct comedi_device *dev,
 	struct comedi_subdevice *s;
 	int ret;
 
-	dev->board_name = dev->driver->driver_name;
-
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
 
@@ -130,16 +128,10 @@ static int apci2200_auto_attach(struct comedi_device *dev,
 
 static void apci2200_detach(struct comedi_device *dev)
 {
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-
 	if (dev->iobase)
 		apci2200_reset(dev);
-	if (dev->subdevices)
-		addi_watchdog_cleanup(&dev->subdevices[2]);
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-	}
+	comedi_spriv_free(dev, 2);
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver apci2200_driver = {
@@ -150,9 +142,9 @@ static struct comedi_driver apci2200_driver = {
 };
 
 static int apci2200_pci_probe(struct pci_dev *dev,
-					const struct pci_device_id *ent)
+			      const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &apci2200_driver);
+	return comedi_pci_auto_config(dev, &apci2200_driver, id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(apci2200_pci_table) = {
diff --git a/drivers/staging/comedi/drivers/addi_apci_3120.c b/drivers/staging/comedi/drivers/addi_apci_3120.c
index 917234d24e9..317a26d97c2 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3120.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3120.c
@@ -8,11 +8,14 @@
 
 #include "addi-data/hwdrv_apci3120.c"
 
+enum apci3120_boardid {
+	BOARD_APCI3120,
+	BOARD_APCI3001,
+};
+
 static const struct addi_board apci3120_boardtypes[] = {
-	{
+	[BOARD_APCI3120] = {
 		.pc_DriverName		= "apci3120",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA_OLD,
-		.i_DeviceId		= 0x818D,
 		.i_NbrAiChannel		= 16,
 		.i_NbrAiChannelDiff	= 8,
 		.i_AiChannelList	= 16,
@@ -23,10 +26,9 @@ static const struct addi_board apci3120_boardtypes[] = {
 		.i_NbrDoChannel		= 4,
 		.i_DoMaxdata		= 0x0f,
 		.interrupt		= v_APCI3120_Interrupt,
-	}, {
+	},
+	[BOARD_APCI3001] = {
 		.pc_DriverName		= "apci3001",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA_OLD,
-		.i_DeviceId		= 0x828D,
 		.i_NbrAiChannel		= 16,
 		.i_NbrAiChannelDiff	= 8,
 		.i_AiChannelList	= 16,
@@ -47,31 +49,17 @@ static irqreturn_t v_ADDI_Interrupt(int irq, void *d)
 	return IRQ_RETVAL(1);
 }
 
-static const void *apci3120_find_boardinfo(struct comedi_device *dev,
-					   struct pci_dev *pcidev)
-{
-	const struct addi_board *this_board;
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(apci3120_boardtypes); i++) {
-		this_board = &apci3120_boardtypes[i];
-		if (this_board->i_VendorId == pcidev->vendor &&
-		    this_board->i_DeviceId == pcidev->device)
-			return this_board;
-	}
-	return NULL;
-}
-
 static int apci3120_auto_attach(struct comedi_device *dev,
-					  unsigned long context_unused)
+				unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct addi_board *this_board;
+	const struct addi_board *this_board = NULL;
 	struct addi_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret, pages, i;
 
-	this_board = apci3120_find_boardinfo(dev, pcidev);
+	if (context < ARRAY_SIZE(apci3120_boardtypes))
+		this_board = &apci3120_boardtypes[context];
 	if (!this_board)
 		return -ENODEV;
 	dev->board_ptr = this_board;
@@ -82,7 +70,7 @@ static int apci3120_auto_attach(struct comedi_device *dev,
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
 	pci_set_master(pcidev);
@@ -215,7 +203,6 @@ static int apci3120_auto_attach(struct comedi_device *dev,
 
 static void apci3120_detach(struct comedi_device *dev)
 {
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct addi_private *devpriv = dev->private;
 
 	if (devpriv) {
@@ -234,10 +221,7 @@ static void apci3120_detach(struct comedi_device *dev)
 				devpriv->ui_DmaBufferPages[1]);
 		}
 	}
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-	}
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver apci3120_driver = {
@@ -248,14 +232,14 @@ static struct comedi_driver apci3120_driver = {
 };
 
 static int apci3120_pci_probe(struct pci_dev *dev,
-					const struct pci_device_id *ent)
+			      const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &apci3120_driver);
+	return comedi_pci_auto_config(dev, &apci3120_driver, id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(apci3120_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA_OLD, 0x818d) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA_OLD, 0x828d) },
+	{ PCI_VDEVICE(AMCC, 0x818d), BOARD_APCI3120 },
+	{ PCI_VDEVICE(AMCC, 0x828d), BOARD_APCI3001 },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, apci3120_pci_table);
diff --git a/drivers/staging/comedi/drivers/addi_apci_3200.c b/drivers/staging/comedi/drivers/addi_apci_3200.c
index 90ee4f844f9..17b540d3c6a 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3200.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3200.c
@@ -22,15 +22,15 @@ static void fpu_end(void)
 #include "addi-data/hwdrv_apci3200.c"
 #include "addi-data/addi_common.c"
 
+enum apci3200_boardid {
+	BOARD_APCI3200,
+	BOARD_APCI3300,
+};
+
 static const struct addi_board apci3200_boardtypes[] = {
-	{
+	[BOARD_APCI3200] = {
 		.pc_DriverName		= "apci3200",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x3000,
-		.i_IorangeBase0		= 128,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 4,
-		.i_IorangeBase3		= 4,
 		.i_PCIEeprom		= ADDIDATA_EEPROM,
 		.pc_EepromChip		= ADDIDATA_S5920,
 		.i_NbrAiChannel		= 16,
@@ -53,14 +53,10 @@ static const struct addi_board apci3200_boardtypes[] = {
 		.ai_cancel		= i_APCI3200_StopCyclicAcquisition,
 		.di_bits		= apci3200_di_insn_bits,
 		.do_bits		= apci3200_do_insn_bits,
-	}, {
+	},
+	[BOARD_APCI3300] = {
 		.pc_DriverName		= "apci3300",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x3007,
-		.i_IorangeBase0		= 128,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 4,
-		.i_IorangeBase3		= 4,
 		.i_PCIEeprom		= ADDIDATA_EEPROM,
 		.pc_EepromChip		= ADDIDATA_S5920,
 		.i_NbrAiChannelDiff	= 8,
@@ -85,29 +81,40 @@ static const struct addi_board apci3200_boardtypes[] = {
 	},
 };
 
-static DEFINE_PCI_DEVICE_TABLE(apci3200_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3000) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3007) },
-	{ 0 }
-};
-MODULE_DEVICE_TABLE(pci, apci3200_pci_table);
+static int apci3200_auto_attach(struct comedi_device *dev,
+				unsigned long context)
+{
+	const struct addi_board *board = NULL;
+
+	if (context < ARRAY_SIZE(apci3200_boardtypes))
+		board = &apci3200_boardtypes[context];
+	if (!board)
+		return -ENODEV;
+	dev->board_ptr = board;
+
+	return addi_auto_attach(dev, context);
+}
 
 static struct comedi_driver apci3200_driver = {
 	.driver_name	= "addi_apci_3200",
 	.module		= THIS_MODULE,
-	.auto_attach	= addi_auto_attach,
+	.auto_attach	= apci3200_auto_attach,
 	.detach		= i_ADDI_Detach,
-	.num_names	= ARRAY_SIZE(apci3200_boardtypes),
-	.board_name	= &apci3200_boardtypes[0].pc_DriverName,
-	.offset		= sizeof(struct addi_board),
 };
 
 static int apci3200_pci_probe(struct pci_dev *dev,
-					const struct pci_device_id *ent)
+			      const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &apci3200_driver);
+	return comedi_pci_auto_config(dev, &apci3200_driver, id->driver_data);
 }
 
+static DEFINE_PCI_DEVICE_TABLE(apci3200_pci_table) = {
+	{ PCI_VDEVICE(ADDIDATA, 0x3000), BOARD_APCI3200 },
+	{ PCI_VDEVICE(ADDIDATA, 0x3007), BOARD_APCI3300 },
+	{ 0 }
+};
+MODULE_DEVICE_TABLE(pci, apci3200_pci_table);
+
 static struct pci_driver apci3200_pci_driver = {
 	.name		= "addi_apci_3200",
 	.id_table	= apci3200_pci_table,
diff --git a/drivers/staging/comedi/drivers/addi_apci_3501.c b/drivers/staging/comedi/drivers/addi_apci_3501.c
index 786fcaf82c3..a0cf6ecdef0 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3501.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3501.c
@@ -339,14 +339,12 @@ static int apci3501_auto_attach(struct comedi_device *dev,
 	int ao_n_chan;
 	int ret;
 
-	dev->board_name = dev->driver->driver_name;
-
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
 
@@ -423,16 +421,11 @@ static int apci3501_auto_attach(struct comedi_device *dev,
 
 static void apci3501_detach(struct comedi_device *dev)
 {
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-
 	if (dev->iobase)
 		apci3501_reset(dev);
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-	}
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver apci3501_driver = {
@@ -443,9 +436,9 @@ static struct comedi_driver apci3501_driver = {
 };
 
 static int apci3501_pci_probe(struct pci_dev *dev,
-					const struct pci_device_id *ent)
+			      const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &apci3501_driver);
+	return comedi_pci_auto_config(dev, &apci3501_driver, id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(apci3501_pci_table) = {
diff --git a/drivers/staging/comedi/drivers/addi_apci_3xxx.c b/drivers/staging/comedi/drivers/addi_apci_3xxx.c
index 09d4b21fce2..ec4d6ca6863 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3xxx.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3xxx.c
@@ -10,15 +10,38 @@
 #include "addi-data/hwdrv_apci3xxx.c"
 #include "addi-data/addi_common.c"
 
+enum apci3xxx_boardid {
+	BOARD_APCI3000_16,
+	BOARD_APCI3000_8,
+	BOARD_APCI3000_4,
+	BOARD_APCI3006_16,
+	BOARD_APCI3006_8,
+	BOARD_APCI3006_4,
+	BOARD_APCI3010_16,
+	BOARD_APCI3010_8,
+	BOARD_APCI3010_4,
+	BOARD_APCI3016_16,
+	BOARD_APCI3016_8,
+	BOARD_APCI3016_4,
+	BOARD_APCI3100_16_4,
+	BOARD_APCI3100_8_4,
+	BOARD_APCI3106_16_4,
+	BOARD_APCI3106_8_4,
+	BOARD_APCI3110_16_4,
+	BOARD_APCI3110_8_4,
+	BOARD_APCI3116_16_4,
+	BOARD_APCI3116_8_4,
+	BOARD_APCI3003,
+	BOARD_APCI3002_16,
+	BOARD_APCI3002_8,
+	BOARD_APCI3002_4,
+	BOARD_APCI3500,
+};
+
 static const struct addi_board apci3xxx_boardtypes[] = {
-	{
+	[BOARD_APCI3000_16] = {
 		.pc_DriverName		= "apci3000-16",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x3010,
-		.i_IorangeBase0		= 256,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 256,
-		.i_IorangeBase3		= 256,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.pc_EepromChip		= ADDIDATA_9054,
 		.i_NbrAiChannel		= 16,
@@ -37,14 +60,10 @@ static const struct addi_board apci3xxx_boardtypes[] = {
 		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
 		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
 		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
-	}, {
+	},
+	[BOARD_APCI3000_8] = {
 		.pc_DriverName		= "apci3000-8",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x300F,
-		.i_IorangeBase0		= 256,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 256,
-		.i_IorangeBase3		= 256,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.pc_EepromChip		= ADDIDATA_9054,
 		.i_NbrAiChannel		= 8,
@@ -63,14 +82,10 @@ static const struct addi_board apci3xxx_boardtypes[] = {
 		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
 		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
 		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
-	}, {
+	},
+	[BOARD_APCI3000_4] = {
 		.pc_DriverName		= "apci3000-4",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x300E,
-		.i_IorangeBase0		= 256,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 256,
-		.i_IorangeBase3		= 256,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.pc_EepromChip		= ADDIDATA_9054,
 		.i_NbrAiChannel		= 4,
@@ -89,14 +104,10 @@ static const struct addi_board apci3xxx_boardtypes[] = {
 		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
 		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
 		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
-	}, {
+	},
+	[BOARD_APCI3006_16] = {
 		.pc_DriverName		= "apci3006-16",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x3013,
-		.i_IorangeBase0		= 256,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 256,
-		.i_IorangeBase3		= 256,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.pc_EepromChip		= ADDIDATA_9054,
 		.i_NbrAiChannel		= 16,
@@ -115,14 +126,10 @@ static const struct addi_board apci3xxx_boardtypes[] = {
 		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
 		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
 		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
-	}, {
+	},
+	[BOARD_APCI3006_8] = {
 		.pc_DriverName		= "apci3006-8",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x3014,
-		.i_IorangeBase0		= 256,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 256,
-		.i_IorangeBase3		= 256,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.pc_EepromChip		= ADDIDATA_9054,
 		.i_NbrAiChannel		= 8,
@@ -141,14 +148,10 @@ static const struct addi_board apci3xxx_boardtypes[] = {
 		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
 		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
 		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
-	}, {
+	},
+	[BOARD_APCI3006_4] = {
 		.pc_DriverName		= "apci3006-4",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x3015,
-		.i_IorangeBase0		= 256,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 256,
-		.i_IorangeBase3		= 256,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.pc_EepromChip		= ADDIDATA_9054,
 		.i_NbrAiChannel		= 4,
@@ -167,14 +170,10 @@ static const struct addi_board apci3xxx_boardtypes[] = {
 		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
 		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
 		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
-	}, {
+	},
+	[BOARD_APCI3010_16] = {
 		.pc_DriverName		= "apci3010-16",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x3016,
-		.i_IorangeBase0		= 256,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 256,
-		.i_IorangeBase3		= 256,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.pc_EepromChip		= ADDIDATA_9054,
 		.i_NbrAiChannel		= 16,
@@ -198,14 +197,10 @@ static const struct addi_board apci3xxx_boardtypes[] = {
 		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
 		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
 		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
-	}, {
+	},
+	[BOARD_APCI3010_8] = {
 		.pc_DriverName		= "apci3010-8",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x3017,
-		.i_IorangeBase0		= 256,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 256,
-		.i_IorangeBase3		= 256,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.pc_EepromChip		= ADDIDATA_9054,
 		.i_NbrAiChannel		= 8,
@@ -229,14 +224,10 @@ static const struct addi_board apci3xxx_boardtypes[] = {
 		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
 		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
 		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
-	}, {
+	},
+	[BOARD_APCI3010_4] = {
 		.pc_DriverName		= "apci3010-4",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x3018,
-		.i_IorangeBase0		= 256,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 256,
-		.i_IorangeBase3		= 256,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.pc_EepromChip		= ADDIDATA_9054,
 		.i_NbrAiChannel		= 4,
@@ -260,14 +251,10 @@ static const struct addi_board apci3xxx_boardtypes[] = {
 		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
 		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
 		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
-	}, {
+	},
+	[BOARD_APCI3016_16] = {
 		.pc_DriverName		= "apci3016-16",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x3019,
-		.i_IorangeBase0		= 256,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 256,
-		.i_IorangeBase3		= 256,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.pc_EepromChip		= ADDIDATA_9054,
 		.i_NbrAiChannel		= 16,
@@ -291,14 +278,10 @@ static const struct addi_board apci3xxx_boardtypes[] = {
 		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
 		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
 		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
-	}, {
+	},
+	[BOARD_APCI3016_8] = {
 		.pc_DriverName		= "apci3016-8",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x301A,
-		.i_IorangeBase0		= 256,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 256,
-		.i_IorangeBase3		= 256,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.pc_EepromChip		= ADDIDATA_9054,
 		.i_NbrAiChannel		= 8,
@@ -322,14 +305,10 @@ static const struct addi_board apci3xxx_boardtypes[] = {
 		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
 		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
 		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
-	}, {
+	},
+	[BOARD_APCI3016_4] = {
 		.pc_DriverName		= "apci3016-4",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x301B,
-		.i_IorangeBase0		= 256,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 256,
-		.i_IorangeBase3		= 256,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.pc_EepromChip		= ADDIDATA_9054,
 		.i_NbrAiChannel		= 4,
@@ -353,14 +332,10 @@ static const struct addi_board apci3xxx_boardtypes[] = {
 		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
 		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
 		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
-	}, {
+	},
+	[BOARD_APCI3100_16_4] = {
 		.pc_DriverName		= "apci3100-16-4",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x301C,
-		.i_IorangeBase0		= 256,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 256,
-		.i_IorangeBase3		= 256,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.pc_EepromChip		= ADDIDATA_9054,
 		.i_NbrAiChannel		= 16,
@@ -383,14 +358,10 @@ static const struct addi_board apci3xxx_boardtypes[] = {
 		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
 		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
 		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
-	}, {
+	},
+	[BOARD_APCI3100_8_4] = {
 		.pc_DriverName		= "apci3100-8-4",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x301D,
-		.i_IorangeBase0		= 256,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 256,
-		.i_IorangeBase3		= 256,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.pc_EepromChip		= ADDIDATA_9054,
 		.i_NbrAiChannel		= 8,
@@ -413,14 +384,10 @@ static const struct addi_board apci3xxx_boardtypes[] = {
 		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
 		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
 		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
-	}, {
+	},
+	[BOARD_APCI3106_16_4] = {
 		.pc_DriverName		= "apci3106-16-4",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x301E,
-		.i_IorangeBase0		= 256,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 256,
-		.i_IorangeBase3		= 256,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.pc_EepromChip		= ADDIDATA_9054,
 		.i_NbrAiChannel		= 16,
@@ -443,14 +410,10 @@ static const struct addi_board apci3xxx_boardtypes[] = {
 		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
 		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
 		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
-	}, {
+	},
+	[BOARD_APCI3106_8_4] = {
 		.pc_DriverName		= "apci3106-8-4",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x301F,
-		.i_IorangeBase0		= 256,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 256,
-		.i_IorangeBase3		= 256,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.pc_EepromChip		= ADDIDATA_9054,
 		.i_NbrAiChannel		= 8,
@@ -473,14 +436,10 @@ static const struct addi_board apci3xxx_boardtypes[] = {
 		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
 		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
 		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
-	}, {
+	},
+	[BOARD_APCI3110_16_4] = {
 		.pc_DriverName		= "apci3110-16-4",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x3020,
-		.i_IorangeBase0		= 256,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 256,
-		.i_IorangeBase3		= 256,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.pc_EepromChip		= ADDIDATA_9054,
 		.i_NbrAiChannel		= 16,
@@ -508,14 +467,10 @@ static const struct addi_board apci3xxx_boardtypes[] = {
 		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
 		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
 		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
-	}, {
+	},
+	[BOARD_APCI3110_8_4] = {
 		.pc_DriverName		= "apci3110-8-4",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x3021,
-		.i_IorangeBase0		= 256,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 256,
-		.i_IorangeBase3		= 256,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.pc_EepromChip		= ADDIDATA_9054,
 		.i_NbrAiChannel		= 8,
@@ -543,14 +498,10 @@ static const struct addi_board apci3xxx_boardtypes[] = {
 		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
 		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
 		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
-	}, {
+	},
+	[BOARD_APCI3116_16_4] = {
 		.pc_DriverName		= "apci3116-16-4",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x3022,
-		.i_IorangeBase0		= 256,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 256,
-		.i_IorangeBase3		= 256,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.pc_EepromChip		= ADDIDATA_9054,
 		.i_NbrAiChannel		= 16,
@@ -578,14 +529,10 @@ static const struct addi_board apci3xxx_boardtypes[] = {
 		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
 		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
 		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
-	}, {
+	},
+	[BOARD_APCI3116_8_4] = {
 		.pc_DriverName		= "apci3116-8-4",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x3023,
-		.i_IorangeBase0		= 256,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 256,
-		.i_IorangeBase3		= 256,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.pc_EepromChip		= ADDIDATA_9054,
 		.i_NbrAiChannel		= 8,
@@ -613,14 +560,10 @@ static const struct addi_board apci3xxx_boardtypes[] = {
 		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
 		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
 		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
-	}, {
+	},
+	[BOARD_APCI3003] = {
 		.pc_DriverName		= "apci3003",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x300B,
-		.i_IorangeBase0		= 256,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 256,
-		.i_IorangeBase3		= 256,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.pc_EepromChip		= ADDIDATA_9054,
 		.i_NbrAiChannelDiff	= 4,
@@ -638,14 +581,10 @@ static const struct addi_board apci3xxx_boardtypes[] = {
 		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
 		.di_bits		= apci3xxx_di_insn_bits,
 		.do_bits		= apci3xxx_do_insn_bits,
-	}, {
+	},
+	[BOARD_APCI3002_16] = {
 		.pc_DriverName		= "apci3002-16",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x3002,
-		.i_IorangeBase0		= 256,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 256,
-		.i_IorangeBase3		= 256,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.pc_EepromChip		= ADDIDATA_9054,
 		.i_NbrAiChannelDiff	= 16,
@@ -663,14 +602,10 @@ static const struct addi_board apci3xxx_boardtypes[] = {
 		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
 		.di_bits		= apci3xxx_di_insn_bits,
 		.do_bits		= apci3xxx_do_insn_bits,
-	}, {
+	},
+	[BOARD_APCI3002_8] = {
 		.pc_DriverName		= "apci3002-8",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x3003,
-		.i_IorangeBase0		= 256,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 256,
-		.i_IorangeBase3		= 256,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.pc_EepromChip		= ADDIDATA_9054,
 		.i_NbrAiChannelDiff	= 8,
@@ -688,14 +623,10 @@ static const struct addi_board apci3xxx_boardtypes[] = {
 		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
 		.di_bits		= apci3xxx_di_insn_bits,
 		.do_bits		= apci3xxx_do_insn_bits,
-	}, {
+	},
+	[BOARD_APCI3002_4] = {
 		.pc_DriverName		= "apci3002-4",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x3004,
-		.i_IorangeBase0		= 256,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 256,
-		.i_IorangeBase3		= 256,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.pc_EepromChip		= ADDIDATA_9054,
 		.i_NbrAiChannelDiff	= 4,
@@ -713,14 +644,10 @@ static const struct addi_board apci3xxx_boardtypes[] = {
 		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
 		.di_bits		= apci3xxx_di_insn_bits,
 		.do_bits		= apci3xxx_do_insn_bits,
-	}, {
+	},
+	[BOARD_APCI3500] = {
 		.pc_DriverName		= "apci3500",
-		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
-		.i_DeviceId		= 0x3024,
-		.i_IorangeBase0		= 256,
 		.i_IorangeBase1		= 256,
-		.i_IorangeBase2		= 256,
-		.i_IorangeBase3		= 256,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.pc_EepromChip		= ADDIDATA_9054,
 		.i_NbrAoChannel		= 4,
@@ -737,48 +664,59 @@ static const struct addi_board apci3xxx_boardtypes[] = {
 	},
 };
 
+static int apci3xxx_auto_attach(struct comedi_device *dev,
+				unsigned long context)
+{
+	const struct addi_board *board = NULL;
+
+	if (context < ARRAY_SIZE(apci3xxx_boardtypes))
+		board = &apci3xxx_boardtypes[context];
+	if (!board)
+		return -ENODEV;
+	dev->board_ptr = board;
+
+	return addi_auto_attach(dev, context);
+}
+
 static struct comedi_driver apci3xxx_driver = {
 	.driver_name	= "addi_apci_3xxx",
 	.module		= THIS_MODULE,
-	.auto_attach	= addi_auto_attach,
+	.auto_attach	= apci3xxx_auto_attach,
 	.detach		= i_ADDI_Detach,
-	.num_names	= ARRAY_SIZE(apci3xxx_boardtypes),
-	.board_name	= &apci3xxx_boardtypes[0].pc_DriverName,
-	.offset		= sizeof(struct addi_board),
 };
 
 static int apci3xxx_pci_probe(struct pci_dev *dev,
-					const struct pci_device_id *ent)
+			      const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &apci3xxx_driver);
+	return comedi_pci_auto_config(dev, &apci3xxx_driver, id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(apci3xxx_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3010) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x300f) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x300e) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3013) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3014) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3015) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3016) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3017) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3018) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3019) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x301a) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x301b) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x301c) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x301d) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x301e) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x301f) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3020) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3021) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3022) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3023) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x300B) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3002) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3003) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3004) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3024) },
+	{ PCI_VDEVICE(ADDIDATA, 0x3010), BOARD_APCI3000_16 },
+	{ PCI_VDEVICE(ADDIDATA, 0x300f), BOARD_APCI3000_8 },
+	{ PCI_VDEVICE(ADDIDATA, 0x300e), BOARD_APCI3000_4 },
+	{ PCI_VDEVICE(ADDIDATA, 0x3013), BOARD_APCI3006_16 },
+	{ PCI_VDEVICE(ADDIDATA, 0x3014), BOARD_APCI3006_8 },
+	{ PCI_VDEVICE(ADDIDATA, 0x3015), BOARD_APCI3006_4 },
+	{ PCI_VDEVICE(ADDIDATA, 0x3016), BOARD_APCI3010_16 },
+	{ PCI_VDEVICE(ADDIDATA, 0x3017), BOARD_APCI3010_8 },
+	{ PCI_VDEVICE(ADDIDATA, 0x3018), BOARD_APCI3010_4 },
+	{ PCI_VDEVICE(ADDIDATA, 0x3019), BOARD_APCI3016_16 },
+	{ PCI_VDEVICE(ADDIDATA, 0x301a), BOARD_APCI3016_8 },
+	{ PCI_VDEVICE(ADDIDATA, 0x301b), BOARD_APCI3016_4 },
+	{ PCI_VDEVICE(ADDIDATA, 0x301c), BOARD_APCI3100_16_4 },
+	{ PCI_VDEVICE(ADDIDATA, 0x301d), BOARD_APCI3100_8_4 },
+	{ PCI_VDEVICE(ADDIDATA, 0x301e), BOARD_APCI3106_16_4 },
+	{ PCI_VDEVICE(ADDIDATA, 0x301f), BOARD_APCI3106_8_4 },
+	{ PCI_VDEVICE(ADDIDATA, 0x3020), BOARD_APCI3110_16_4 },
+	{ PCI_VDEVICE(ADDIDATA, 0x3021), BOARD_APCI3110_8_4 },
+	{ PCI_VDEVICE(ADDIDATA, 0x3022), BOARD_APCI3116_16_4 },
+	{ PCI_VDEVICE(ADDIDATA, 0x3023), BOARD_APCI3116_8_4 },
+	{ PCI_VDEVICE(ADDIDATA, 0x300B), BOARD_APCI3003 },
+	{ PCI_VDEVICE(ADDIDATA, 0x3002), BOARD_APCI3002_16 },
+	{ PCI_VDEVICE(ADDIDATA, 0x3003), BOARD_APCI3002_8 },
+	{ PCI_VDEVICE(ADDIDATA, 0x3004), BOARD_APCI3002_4 },
+	{ PCI_VDEVICE(ADDIDATA, 0x3024), BOARD_APCI3500 },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, apci3xxx_pci_table);
diff --git a/drivers/staging/comedi/drivers/addi_watchdog.c b/drivers/staging/comedi/drivers/addi_watchdog.c
index 375ab665e09..1666b5f510d 100644
--- a/drivers/staging/comedi/drivers/addi_watchdog.c
+++ b/drivers/staging/comedi/drivers/addi_watchdog.c
@@ -150,12 +150,6 @@ int addi_watchdog_init(struct comedi_subdevice *s, unsigned long iobase)
 }
 EXPORT_SYMBOL_GPL(addi_watchdog_init);
 
-void addi_watchdog_cleanup(struct comedi_subdevice *s)
-{
-	kfree(s->private);
-}
-EXPORT_SYMBOL_GPL(addi_watchdog_cleanup);
-
 static int __init addi_watchdog_module_init(void)
 {
 	return 0;
diff --git a/drivers/staging/comedi/drivers/addi_watchdog.h b/drivers/staging/comedi/drivers/addi_watchdog.h
index f374a7bff44..83b47befa4d 100644
--- a/drivers/staging/comedi/drivers/addi_watchdog.h
+++ b/drivers/staging/comedi/drivers/addi_watchdog.h
@@ -5,6 +5,5 @@
 
 void addi_watchdog_reset(unsigned long iobase);
 int addi_watchdog_init(struct comedi_subdevice *, unsigned long iobase);
-void addi_watchdog_cleanup(struct comedi_subdevice *s);
 
 #endif
diff --git a/drivers/staging/comedi/drivers/adl_pci6208.c b/drivers/staging/comedi/drivers/adl_pci6208.c
index 7b3e331616e..8a438ff1bd4 100644
--- a/drivers/staging/comedi/drivers/adl_pci6208.c
+++ b/drivers/staging/comedi/drivers/adl_pci6208.c
@@ -47,12 +47,6 @@ References:
 #include "../comedidev.h"
 
 /*
- * ADLINK PCI Device ID's supported by this driver
- */
-#define PCI_DEVICE_ID_PCI6208		0x6208
-#define PCI_DEVICE_ID_PCI6216		0x6216
-
-/*
  * PCI-6208/6216-GL register map
  */
 #define PCI6208_AO_CONTROL(x)		(0x00 + (2 * (x)))
@@ -66,20 +60,23 @@ References:
 
 #define PCI6208_MAX_AO_CHANNELS		16
 
+enum pci6208_boardid {
+	BOARD_PCI6208,
+	BOARD_PCI6216,
+};
+
 struct pci6208_board {
 	const char *name;
-	unsigned short dev_id;
 	int ao_chans;
 };
 
 static const struct pci6208_board pci6208_boards[] = {
-	{
+	[BOARD_PCI6208] = {
 		.name		= "adl_pci6208",
-		.dev_id		= PCI_DEVICE_ID_PCI6208,
 		.ao_chans	= 8,
-	}, {
+	},
+	[BOARD_PCI6216] = {
 		.name		= "adl_pci6216",
-		.dev_id		= PCI_DEVICE_ID_PCI6216,
 		.ao_chans	= 16,
 	},
 };
@@ -162,31 +159,18 @@ static int pci6208_do_insn_bits(struct comedi_device *dev,
 	return insn->n;
 }
 
-static const void *pci6208_find_boardinfo(struct comedi_device *dev,
-					  struct pci_dev *pcidev)
-{
-	const struct pci6208_board *boardinfo;
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(pci6208_boards); i++) {
-		boardinfo = &pci6208_boards[i];
-		if (boardinfo->dev_id == pcidev->device)
-			return boardinfo;
-	}
-	return NULL;
-}
-
 static int pci6208_auto_attach(struct comedi_device *dev,
-					 unsigned long context_unused)
+			       unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct pci6208_board *boardinfo;
+	const struct pci6208_board *boardinfo = NULL;
 	struct pci6208_private *devpriv;
 	struct comedi_subdevice *s;
 	unsigned int val;
 	int ret;
 
-	boardinfo = pci6208_find_boardinfo(dev, pcidev);
+	if (context < ARRAY_SIZE(pci6208_boards))
+		boardinfo = &pci6208_boards[context];
 	if (!boardinfo)
 		return -ENODEV;
 	dev->board_ptr = boardinfo;
@@ -197,7 +181,7 @@ static int pci6208_auto_attach(struct comedi_device *dev,
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
 	dev->iobase = pci_resource_start(pcidev, 2);
@@ -249,32 +233,23 @@ static int pci6208_auto_attach(struct comedi_device *dev,
 	return 0;
 }
 
-static void pci6208_detach(struct comedi_device *dev)
-{
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-	}
-}
-
 static struct comedi_driver adl_pci6208_driver = {
 	.driver_name	= "adl_pci6208",
 	.module		= THIS_MODULE,
 	.auto_attach	= pci6208_auto_attach,
-	.detach		= pci6208_detach,
+	.detach		= comedi_pci_disable,
 };
 
 static int adl_pci6208_pci_probe(struct pci_dev *dev,
-					   const struct pci_device_id *ent)
+				 const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &adl_pci6208_driver);
+	return comedi_pci_auto_config(dev, &adl_pci6208_driver,
+				      id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(adl_pci6208_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI6208) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI6216) },
+	{ PCI_VDEVICE(ADLINK, 0x6208), BOARD_PCI6208 },
+	{ PCI_VDEVICE(ADLINK, 0x6216), BOARD_PCI6216 },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, adl_pci6208_pci_table);
diff --git a/drivers/staging/comedi/drivers/adl_pci7x3x.c b/drivers/staging/comedi/drivers/adl_pci7x3x.c
index f27f48e6e70..e3960745f50 100644
--- a/drivers/staging/comedi/drivers/adl_pci7x3x.c
+++ b/drivers/staging/comedi/drivers/adl_pci7x3x.c
@@ -53,60 +53,57 @@ Configuration Options: not applicable, uses comedi PCI auto config
 #include "../comedidev.h"
 
 /*
- * PCI Device ID's supported by this driver
- */
-#define PCI_DEVICE_ID_PCI7230	0x7230
-#define PCI_DEVICE_ID_PCI7233	0x7233
-#define PCI_DEVICE_ID_PCI7234	0x7234
-#define PCI_DEVICE_ID_PCI7432	0x7432
-#define PCI_DEVICE_ID_PCI7433	0x7433
-#define PCI_DEVICE_ID_PCI7434	0x7434
-
-/*
  * Register I/O map (32-bit access only)
  */
 #define PCI7X3X_DIO_REG		0x00
 #define PCI743X_DIO_REG		0x04
 
+enum apci1516_boardid {
+	BOARD_PCI7230,
+	BOARD_PCI7233,
+	BOARD_PCI7234,
+	BOARD_PCI7432,
+	BOARD_PCI7433,
+	BOARD_PCI7434,
+};
+
 struct adl_pci7x3x_boardinfo {
 	const char *name;
-	unsigned short device;
 	int nsubdevs;
 	int di_nchan;
 	int do_nchan;
 };
 
 static const struct adl_pci7x3x_boardinfo adl_pci7x3x_boards[] = {
-	{
+	[BOARD_PCI7230] = {
 		.name		= "adl_pci7230",
-		.device		= PCI_DEVICE_ID_PCI7230,
 		.nsubdevs	= 2,
 		.di_nchan	= 16,
 		.do_nchan	= 16,
-	}, {
+	},
+	[BOARD_PCI7233] = {
 		.name		= "adl_pci7233",
-		.device		= PCI_DEVICE_ID_PCI7233,
 		.nsubdevs	= 1,
 		.di_nchan	= 32,
-	}, {
+	},
+	[BOARD_PCI7234] = {
 		.name		= "adl_pci7234",
-		.device		= PCI_DEVICE_ID_PCI7234,
 		.nsubdevs	= 1,
 		.do_nchan	= 32,
-	}, {
+	},
+	[BOARD_PCI7432] = {
 		.name		= "adl_pci7432",
-		.device		= PCI_DEVICE_ID_PCI7432,
 		.nsubdevs	= 2,
 		.di_nchan	= 32,
 		.do_nchan	= 32,
-	}, {
+	},
+	[BOARD_PCI7433] = {
 		.name		= "adl_pci7433",
-		.device		= PCI_DEVICE_ID_PCI7433,
 		.nsubdevs	= 2,
 		.di_nchan	= 64,
-	}, {
+	},
+	[BOARD_PCI7434] = {
 		.name		= "adl_pci7434",
-		.device		= PCI_DEVICE_ID_PCI7434,
 		.nsubdevs	= 2,
 		.do_nchan	= 64,
 	}
@@ -150,37 +147,24 @@ static int adl_pci7x3x_di_insn_bits(struct comedi_device *dev,
 	return insn->n;
 }
 
-static const void *adl_pci7x3x_find_boardinfo(struct comedi_device *dev,
-					      struct pci_dev *pcidev)
-{
-	const struct adl_pci7x3x_boardinfo *board;
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(adl_pci7x3x_boards); i++) {
-		board = &adl_pci7x3x_boards[i];
-		if (pcidev->device == board->device)
-			return board;
-	}
-	return NULL;
-}
-
 static int adl_pci7x3x_auto_attach(struct comedi_device *dev,
-					     unsigned long context_unused)
+				   unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct adl_pci7x3x_boardinfo *board;
+	const struct adl_pci7x3x_boardinfo *board = NULL;
 	struct comedi_subdevice *s;
 	int subdev;
 	int nchan;
 	int ret;
 
-	board = adl_pci7x3x_find_boardinfo(dev, pcidev);
+	if (context < ARRAY_SIZE(adl_pci7x3x_boards))
+		board = &adl_pci7x3x_boards[context];
 	if (!board)
 		return -ENODEV;
 	dev->board_ptr = board;
 	dev->board_name = board->name;
 
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
 	dev->iobase = pci_resource_start(pcidev, 2);
@@ -275,36 +259,27 @@ static int adl_pci7x3x_auto_attach(struct comedi_device *dev,
 	return 0;
 }
 
-static void adl_pci7x3x_detach(struct comedi_device *dev)
-{
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-	}
-}
-
 static struct comedi_driver adl_pci7x3x_driver = {
 	.driver_name	= "adl_pci7x3x",
 	.module		= THIS_MODULE,
 	.auto_attach	= adl_pci7x3x_auto_attach,
-	.detach		= adl_pci7x3x_detach,
+	.detach		= comedi_pci_disable,
 };
 
 static int adl_pci7x3x_pci_probe(struct pci_dev *dev,
-					   const struct pci_device_id *ent)
+				 const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &adl_pci7x3x_driver);
+	return comedi_pci_auto_config(dev, &adl_pci7x3x_driver,
+				      id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(adl_pci7x3x_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI7230) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI7233) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI7234) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI7432) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI7433) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI7434) },
+	{ PCI_VDEVICE(ADLINK, 0x7230), BOARD_PCI7230 },
+	{ PCI_VDEVICE(ADLINK, 0x7233), BOARD_PCI7233 },
+	{ PCI_VDEVICE(ADLINK, 0x7234), BOARD_PCI7234 },
+	{ PCI_VDEVICE(ADLINK, 0x7432), BOARD_PCI7432 },
+	{ PCI_VDEVICE(ADLINK, 0x7433), BOARD_PCI7433 },
+	{ PCI_VDEVICE(ADLINK, 0x7434), BOARD_PCI7434 },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, adl_pci7x3x_pci_table);
diff --git a/drivers/staging/comedi/drivers/adl_pci8164.c b/drivers/staging/comedi/drivers/adl_pci8164.c
index d06b83f3865..b3ec60afe3a 100644
--- a/drivers/staging/comedi/drivers/adl_pci8164.c
+++ b/drivers/staging/comedi/drivers/adl_pci8164.c
@@ -1,217 +1,74 @@
 /*
-    comedi/drivers/adl_pci8164.c
-
-    Hardware comedi driver fot PCI-8164 Adlink card
-    Copyright (C) 2004 Michel Lachine <mike@mikelachaine.ca>
-
-    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.
+ * comedi/drivers/adl_pci8164.c
+ *
+ * Hardware comedi driver for PCI-8164 Adlink card
+ * Copyright (C) 2004 Michel Lachine <mike@mikelachaine.ca>
+ *
+ * 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.
+ */
 
-*/
 /*
-Driver: adl_pci8164
-Description: Driver for the Adlink PCI-8164 4 Axes Motion Control board
-Devices: [ADLink] PCI-8164 (adl_pci8164)
-Author: Michel Lachaine <mike@mikelachaine.ca>
-Status: experimental
-Updated: Mon, 14 Apr 2008 15:10:32 +0100
-
-Configuration Options: not applicable, uses PCI auto config
-*/
+ * Driver: adl_pci8164
+ * Description: Driver for the Adlink PCI-8164 4 Axes Motion Control board
+ * Devices: (ADLink) PCI-8164 [adl_pci8164]
+ * Author: Michel Lachaine <mike@mikelachaine.ca>
+ * Status: experimental
+ * Updated: Mon, 14 Apr 2008 15:10:32 +0100
+ *
+ * Configuration Options: not applicable, uses PCI auto config
+ */
 
 #include <linux/kernel.h>
 #include <linux/pci.h>
-#include <linux/delay.h>
 
 #include "../comedidev.h"
-#include "comedi_fc.h"
-#include "8253.h"
-
-#define PCI8164_AXIS_X  0x00
-#define PCI8164_AXIS_Y  0x08
-#define PCI8164_AXIS_Z  0x10
-#define PCI8164_AXIS_U  0x18
 
-#define PCI8164_MSTS	0x00
-#define PCI8164_SSTS    0x02
-#define PCI8164_BUF0    0x04
-#define PCI8164_BUF1    0x06
-
-#define PCI8164_CMD     0x00
-#define PCI8164_OTP     0x02
-
-#define PCI_DEVICE_ID_PCI8164 0x8164
-
-/*
- all the read commands are the same except for the addition a constant
- * const to the data for inw()
- */
-static void adl_pci8164_insn_read(struct comedi_device *dev,
-				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn,
-				  unsigned int *data,
-				  char *action, unsigned short offset)
-{
-	int axis, axis_reg;
-	char axisname;
-
-	axis = CR_CHAN(insn->chanspec);
-
-	switch (axis) {
-	case 0:
-		axis_reg = PCI8164_AXIS_X;
-		axisname = 'X';
-		break;
-	case 1:
-		axis_reg = PCI8164_AXIS_Y;
-		axisname = 'Y';
-		break;
-	case 2:
-		axis_reg = PCI8164_AXIS_Z;
-		axisname = 'Z';
-		break;
-	case 3:
-		axis_reg = PCI8164_AXIS_U;
-		axisname = 'U';
-		break;
-	default:
-		axis_reg = PCI8164_AXIS_X;
-		axisname = 'X';
-	}
-
-	data[0] = inw(dev->iobase + axis_reg + offset);
-	dev_dbg(dev->class_dev,
-		"pci8164 %s read -> %04X:%04X on axis %c\n",
-		action, data[0], data[1], axisname);
-}
-
-static int adl_pci8164_insn_read_msts(struct comedi_device *dev,
-				      struct comedi_subdevice *s,
-				      struct comedi_insn *insn,
-				      unsigned int *data)
-{
-	adl_pci8164_insn_read(dev, s, insn, data, "MSTS", PCI8164_MSTS);
-	return 2;
-}
-
-static int adl_pci8164_insn_read_ssts(struct comedi_device *dev,
-				      struct comedi_subdevice *s,
-				      struct comedi_insn *insn,
-				      unsigned int *data)
-{
-	adl_pci8164_insn_read(dev, s, insn, data, "SSTS", PCI8164_SSTS);
-	return 2;
-}
-
-static int adl_pci8164_insn_read_buf0(struct comedi_device *dev,
-				      struct comedi_subdevice *s,
-				      struct comedi_insn *insn,
-				      unsigned int *data)
-{
-	adl_pci8164_insn_read(dev, s, insn, data, "BUF0", PCI8164_BUF0);
-	return 2;
-}
+#define PCI8164_AXIS(x)		((x) * 0x08)
+#define PCI8164_CMD_MSTS_REG	0x00
+#define PCI8164_OTP_SSTS_REG	0x02
+#define PCI8164_BUF0_REG	0x04
+#define PCI8164_BUF1_REG	0x06
 
-static int adl_pci8164_insn_read_buf1(struct comedi_device *dev,
-				      struct comedi_subdevice *s,
-				      struct comedi_insn *insn,
-				      unsigned int *data)
-{
-	adl_pci8164_insn_read(dev, s, insn, data, "BUF1", PCI8164_BUF1);
-	return 2;
-}
-
-/*
- all the write commands are the same except for the addition a constant
- * const to the data for outw()
- */
-static void adl_pci8164_insn_out(struct comedi_device *dev,
+static int adl_pci8164_insn_read(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
 				 struct comedi_insn *insn,
-				 unsigned int *data,
-				 char *action, unsigned short offset)
+				 unsigned int *data)
 {
-	unsigned int axis, axis_reg;
-
-	char axisname;
-
-	axis = CR_CHAN(insn->chanspec);
+	unsigned long offset = (unsigned long)s->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	int i;
 
-	switch (axis) {
-	case 0:
-		axis_reg = PCI8164_AXIS_X;
-		axisname = 'X';
-		break;
-	case 1:
-		axis_reg = PCI8164_AXIS_Y;
-		axisname = 'Y';
-		break;
-	case 2:
-		axis_reg = PCI8164_AXIS_Z;
-		axisname = 'Z';
-		break;
-	case 3:
-		axis_reg = PCI8164_AXIS_U;
-		axisname = 'U';
-		break;
-	default:
-		axis_reg = PCI8164_AXIS_X;
-		axisname = 'X';
-	}
-
-	outw(data[0], dev->iobase + axis_reg + offset);
-
-	dev_dbg(dev->class_dev,
-		"pci8164 %s write -> %04X:%04X on axis %c\n",
-		action, data[0], data[1], axisname);
+	for (i = 0; i < insn->n; i++)
+		data[i] = inw(dev->iobase + PCI8164_AXIS(chan) + offset);
 
+	return insn->n;
 }
 
-static int adl_pci8164_insn_write_cmd(struct comedi_device *dev,
-				      struct comedi_subdevice *s,
-				      struct comedi_insn *insn,
-				      unsigned int *data)
-{
-	adl_pci8164_insn_out(dev, s, insn, data, "CMD", PCI8164_CMD);
-	return 2;
-}
-
-static int adl_pci8164_insn_write_otp(struct comedi_device *dev,
-				      struct comedi_subdevice *s,
-				      struct comedi_insn *insn,
-				      unsigned int *data)
+static int adl_pci8164_insn_write(struct comedi_device *dev,
+				  struct comedi_subdevice *s,
+				  struct comedi_insn *insn,
+				  unsigned int *data)
 {
-	adl_pci8164_insn_out(dev, s, insn, data, "OTP", PCI8164_OTP);
-	return 2;
-}
+	unsigned long offset = (unsigned long)s->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	int i;
 
-static int adl_pci8164_insn_write_buf0(struct comedi_device *dev,
-				       struct comedi_subdevice *s,
-				       struct comedi_insn *insn,
-				       unsigned int *data)
-{
-	adl_pci8164_insn_out(dev, s, insn, data, "BUF0", PCI8164_BUF0);
-	return 2;
-}
+	for (i = 0; i < insn->n; i++)
+		outw(data[i], dev->iobase + PCI8164_AXIS(chan) + offset);
 
-static int adl_pci8164_insn_write_buf1(struct comedi_device *dev,
-				       struct comedi_subdevice *s,
-				       struct comedi_insn *insn,
-				       unsigned int *data)
-{
-	adl_pci8164_insn_out(dev, s, insn, data, "BUF1", PCI8164_BUF1);
-	return 2;
+	return insn->n;
 }
 
 static int adl_pci8164_auto_attach(struct comedi_device *dev,
@@ -221,9 +78,7 @@ static int adl_pci8164_auto_attach(struct comedi_device *dev,
 	struct comedi_subdevice *s;
 	int ret;
 
-	dev->board_name = dev->driver->driver_name;
-
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
 	dev->iobase = pci_resource_start(pcidev, 2);
@@ -232,77 +87,70 @@ static int adl_pci8164_auto_attach(struct comedi_device *dev,
 	if (ret)
 		return ret;
 
+	/* read MSTS register / write CMD register for each axis (channel) */
 	s = &dev->subdevices[0];
-	s->type = COMEDI_SUBD_PROC;
-	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-	s->n_chan = 4;
-	s->maxdata = 0xffff;
-	s->len_chanlist = 4;
-	/* s->range_table = &range_axis; */
-	s->insn_read = adl_pci8164_insn_read_msts;
-	s->insn_write = adl_pci8164_insn_write_cmd;
-
+	s->type		= COMEDI_SUBD_PROC;
+	s->subdev_flags	= SDF_READABLE | SDF_WRITABLE;
+	s->n_chan	= 4;
+	s->maxdata	= 0xffff;
+	s->len_chanlist	= 4;
+	s->insn_read	= adl_pci8164_insn_read;
+	s->insn_write	= adl_pci8164_insn_write;
+	s->private	= (void *)PCI8164_CMD_MSTS_REG;
+
+	/* read SSTS register / write OTP register for each axis (channel) */
 	s = &dev->subdevices[1];
-	s->type = COMEDI_SUBD_PROC;
-	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-	s->n_chan = 4;
-	s->maxdata = 0xffff;
-	s->len_chanlist = 4;
-	/* s->range_table = &range_axis; */
-	s->insn_read = adl_pci8164_insn_read_ssts;
-	s->insn_write = adl_pci8164_insn_write_otp;
-
+	s->type		= COMEDI_SUBD_PROC;
+	s->subdev_flags	= SDF_READABLE | SDF_WRITABLE;
+	s->n_chan	= 4;
+	s->maxdata	= 0xffff;
+	s->len_chanlist	= 4;
+	s->insn_read	= adl_pci8164_insn_read;
+	s->insn_write	= adl_pci8164_insn_write;
+	s->private	= (void *)PCI8164_OTP_SSTS_REG;
+
+	/* read/write BUF0 register for each axis (channel) */
 	s = &dev->subdevices[2];
-	s->type = COMEDI_SUBD_PROC;
-	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-	s->n_chan = 4;
-	s->maxdata = 0xffff;
-	s->len_chanlist = 4;
-	/* s->range_table = &range_axis; */
-	s->insn_read = adl_pci8164_insn_read_buf0;
-	s->insn_write = adl_pci8164_insn_write_buf0;
-
+	s->type		= COMEDI_SUBD_PROC;
+	s->subdev_flags	= SDF_READABLE | SDF_WRITABLE;
+	s->n_chan	= 4;
+	s->maxdata	= 0xffff;
+	s->len_chanlist	= 4;
+	s->insn_read	= adl_pci8164_insn_read;
+	s->insn_write	= adl_pci8164_insn_write;
+	s->private	= (void *)PCI8164_BUF0_REG;
+
+	/* read/write BUF1 register for each axis (channel) */
 	s = &dev->subdevices[3];
-	s->type = COMEDI_SUBD_PROC;
-	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-	s->n_chan = 4;
-	s->maxdata = 0xffff;
-	s->len_chanlist = 4;
-	/* s->range_table = &range_axis; */
-	s->insn_read = adl_pci8164_insn_read_buf1;
-	s->insn_write = adl_pci8164_insn_write_buf1;
-
-	dev_info(dev->class_dev, "%s attached\n", dev->board_name);
+	s->type		= COMEDI_SUBD_PROC;
+	s->subdev_flags	= SDF_READABLE | SDF_WRITABLE;
+	s->n_chan	= 4;
+	s->maxdata	= 0xffff;
+	s->len_chanlist	= 4;
+	s->insn_read	= adl_pci8164_insn_read;
+	s->insn_write	= adl_pci8164_insn_write;
+	s->private	= (void *)PCI8164_BUF1_REG;
 
 	return 0;
 }
 
-static void adl_pci8164_detach(struct comedi_device *dev)
-{
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-	}
-}
-
 static struct comedi_driver adl_pci8164_driver = {
 	.driver_name	= "adl_pci8164",
 	.module		= THIS_MODULE,
 	.auto_attach	= adl_pci8164_auto_attach,
-	.detach		= adl_pci8164_detach,
+	.detach		= comedi_pci_disable,
 };
 
 static int adl_pci8164_pci_probe(struct pci_dev *dev,
-					   const struct pci_device_id *ent)
+				 const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &adl_pci8164_driver);
+	return comedi_pci_auto_config(dev, &adl_pci8164_driver,
+				      id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(adl_pci8164_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI8164) },
-	{0}
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, 0x8164) },
+	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, adl_pci8164_pci_table);
 
diff --git a/drivers/staging/comedi/drivers/adl_pci9111.c b/drivers/staging/comedi/drivers/adl_pci9111.c
index eeb10ec7f17..6247fdcedcb 100644
--- a/drivers/staging/comedi/drivers/adl_pci9111.c
+++ b/drivers/staging/comedi/drivers/adl_pci9111.c
@@ -75,6 +75,7 @@ TODO:
 #include "../comedidev.h"
 
 #include "8253.h"
+#include "plx9052.h"
 #include "comedi_fc.h"
 
 #define PCI9111_DRIVER_NAME	"adl_pci9111"
@@ -120,6 +121,14 @@ TODO:
 #define PCI9111_8254_BASE_REG		0x40
 #define PCI9111_INT_CLR_REG		0x48
 
+/* PLX 9052 Local Interrupt 1 enabled and active */
+#define PCI9111_LI1_ACTIVE	(PLX9052_INTCSR_LI1ENAB |	\
+				 PLX9052_INTCSR_LI1STAT)
+
+/* PLX 9052 Local Interrupt 2 enabled and active */
+#define PCI9111_LI2_ACTIVE	(PLX9052_INTCSR_LI2ENAB |	\
+				 PLX9052_INTCSR_LI2STAT)
+
 static const struct comedi_lrange pci9111_ai_range = {
 	5,
 	{
@@ -150,17 +159,6 @@ struct pci9111_private_data {
 	short ai_bounce_buffer[2 * PCI9111_FIFO_HALF_SIZE];
 };
 
-#define PLX9050_REGISTER_INTERRUPT_CONTROL 0x4c
-
-#define PLX9050_LINTI1_ENABLE		(1 << 0)
-#define PLX9050_LINTI1_ACTIVE_HIGH	(1 << 1)
-#define PLX9050_LINTI1_STATUS		(1 << 2)
-#define PLX9050_LINTI2_ENABLE		(1 << 3)
-#define PLX9050_LINTI2_ACTIVE_HIGH	(1 << 4)
-#define PLX9050_LINTI2_STATUS		(1 << 5)
-#define PLX9050_PCI_INTERRUPT_ENABLE	(1 << 6)
-#define PLX9050_SOFTWARE_INTERRUPT	(1 << 7)
-
 static void plx9050_interrupt_control(unsigned long io_base,
 				      bool LINTi1_enable,
 				      bool LINTi1_active_high,
@@ -171,18 +169,18 @@ static void plx9050_interrupt_control(unsigned long io_base,
 	int flags = 0;
 
 	if (LINTi1_enable)
-		flags |= PLX9050_LINTI1_ENABLE;
+		flags |= PLX9052_INTCSR_LI1ENAB;
 	if (LINTi1_active_high)
-		flags |= PLX9050_LINTI1_ACTIVE_HIGH;
+		flags |= PLX9052_INTCSR_LI1POL;
 	if (LINTi2_enable)
-		flags |= PLX9050_LINTI2_ENABLE;
+		flags |= PLX9052_INTCSR_LI2ENAB;
 	if (LINTi2_active_high)
-		flags |= PLX9050_LINTI2_ACTIVE_HIGH;
+		flags |= PLX9052_INTCSR_LI2POL;
 
 	if (interrupt_enable)
-		flags |= PLX9050_PCI_INTERRUPT_ENABLE;
+		flags |= PLX9052_INTCSR_PCIENAB;
 
-	outb(flags, io_base + PLX9050_REGISTER_INTERRUPT_CONTROL);
+	outb(flags, io_base + PLX9052_INTCSR);
 }
 
 static void pci9111_timer_set(struct comedi_device *dev)
@@ -607,21 +605,17 @@ static irqreturn_t pci9111_interrupt(int irq, void *p_device)
 	spin_lock_irqsave(&dev->spinlock, irq_flags);
 
 	/*  Check if we are source of interrupt */
-	intcsr = inb(dev_private->lcr_io_base +
-		     PLX9050_REGISTER_INTERRUPT_CONTROL);
-	if (!(((intcsr & PLX9050_PCI_INTERRUPT_ENABLE) != 0)
-	      && (((intcsr & (PLX9050_LINTI1_ENABLE | PLX9050_LINTI1_STATUS))
-		   == (PLX9050_LINTI1_ENABLE | PLX9050_LINTI1_STATUS))
-		  || ((intcsr & (PLX9050_LINTI2_ENABLE | PLX9050_LINTI2_STATUS))
-		      == (PLX9050_LINTI2_ENABLE | PLX9050_LINTI2_STATUS))))) {
+	intcsr = inb(dev_private->lcr_io_base + PLX9052_INTCSR);
+	if (!(((intcsr & PLX9052_INTCSR_PCIENAB) != 0) &&
+	      (((intcsr & PCI9111_LI1_ACTIVE) == PCI9111_LI1_ACTIVE) ||
+	       ((intcsr & PCI9111_LI2_ACTIVE) == PCI9111_LI2_ACTIVE)))) {
 		/*  Not the source of the interrupt. */
-		/*  (N.B. not using PLX9050_SOFTWARE_INTERRUPT) */
+		/*  (N.B. not using PLX9052_INTCSR_SOFTINT) */
 		spin_unlock_irqrestore(&dev->spinlock, irq_flags);
 		return IRQ_NONE;
 	}
 
-	if ((intcsr & (PLX9050_LINTI1_ENABLE | PLX9050_LINTI1_STATUS)) ==
-	    (PLX9050_LINTI1_ENABLE | PLX9050_LINTI1_STATUS)) {
+	if ((intcsr & PCI9111_LI1_ACTIVE) == PCI9111_LI1_ACTIVE) {
 		/*  Interrupt comes from fifo_half-full signal */
 
 		status = inb(dev->iobase + PCI9111_AI_RANGE_STAT_REG);
@@ -865,14 +859,12 @@ static int pci9111_auto_attach(struct comedi_device *dev,
 	struct comedi_subdevice *s;
 	int ret;
 
-	dev->board_name = dev->driver->driver_name;
-
 	dev_private = kzalloc(sizeof(*dev_private), GFP_KERNEL);
 	if (!dev_private)
 		return -ENOMEM;
 	dev->private = dev_private;
 
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
 	dev_private->lcr_io_base = pci_resource_start(pcidev, 1);
@@ -939,16 +931,11 @@ static int pci9111_auto_attach(struct comedi_device *dev,
 
 static void pci9111_detach(struct comedi_device *dev)
 {
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-
 	if (dev->iobase)
 		pci9111_reset(dev);
 	if (dev->irq != 0)
 		free_irq(dev->irq, dev);
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-	}
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver adl_pci9111_driver = {
@@ -959,9 +946,10 @@ static struct comedi_driver adl_pci9111_driver = {
 };
 
 static int pci9111_pci_probe(struct pci_dev *dev,
-				       const struct pci_device_id *ent)
+			     const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &adl_pci9111_driver);
+	return comedi_pci_auto_config(dev, &adl_pci9111_driver,
+				      id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(pci9111_pci_table) = {
diff --git a/drivers/staging/comedi/drivers/adl_pci9118.c b/drivers/staging/comedi/drivers/adl_pci9118.c
index 4dbac7459a4..cb4ef2dcbf0 100644
--- a/drivers/staging/comedi/drivers/adl_pci9118.c
+++ b/drivers/staging/comedi/drivers/adl_pci9118.c
@@ -1970,12 +1970,9 @@ static int pci9118_common_attach(struct comedi_device *dev, int disable_irq,
 	u16 u16w;
 
 	dev->board_name = this_board->name;
-	ret = comedi_pci_enable(pcidev, dev->board_name);
-	if (ret) {
-		dev_err(dev->class_dev,
-			"cannot enable PCI device %s\n", pci_name(pcidev));
+	ret = comedi_pci_enable(dev);
+	if (ret)
 		return ret;
-	}
 	if (master)
 		pci_set_master(pcidev);
 
@@ -2202,12 +2199,9 @@ static void pci9118_detach(struct comedi_device *dev)
 			free_pages((unsigned long)devpriv->dmabuf_virt[1],
 				   devpriv->dmabuf_pages[1]);
 	}
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-
+	comedi_pci_disable(dev);
+	if (pcidev)
 		pci_dev_put(pcidev);
-	}
 }
 
 static struct comedi_driver adl_pci9118_driver = {
@@ -2222,9 +2216,10 @@ static struct comedi_driver adl_pci9118_driver = {
 };
 
 static int adl_pci9118_pci_probe(struct pci_dev *dev,
-					   const struct pci_device_id *ent)
+				 const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &adl_pci9118_driver);
+	return comedi_pci_auto_config(dev, &adl_pci9118_driver,
+				      id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(adl_pci9118_pci_table) = {
diff --git a/drivers/staging/comedi/drivers/adq12b.c b/drivers/staging/comedi/drivers/adq12b.c
index f7950dfe2dd..71142e36e7f 100644
--- a/drivers/staging/comedi/drivers/adq12b.c
+++ b/drivers/staging/comedi/drivers/adq12b.c
@@ -213,43 +213,19 @@ static int adq12b_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	struct adq12b_private *devpriv;
 	struct comedi_subdevice *s;
-	unsigned long iobase;
-	int unipolar, differential;
 	int ret;
 
-	dev->board_name = dev->driver->driver_name;
-
-	iobase = it->options[0];
-	unipolar = it->options[1];
-	differential = it->options[2];
-
-	printk(KERN_INFO "comedi%d: adq12b called with options base=0x%03lx, "
-	       "%s and %s\n", dev->minor, iobase,
-	       (unipolar == 1) ? "unipolar" : "bipolar",
-	       (differential == 1) ? "differential" : "single-ended");
-
-	/* if no address was specified, try the default 0x300 */
-	if (iobase == 0) {
-		printk(KERN_WARNING "comedi%d: adq12b warning: I/O base "
-		       "address not specified. Trying the default 0x300.\n",
-		       dev->minor);
-		iobase = 0x300;
-	}
-
-	printk("comedi%d: adq12b: 0x%04lx ", dev->minor, iobase);
-	if (!request_region(iobase, ADQ12B_SIZE, "adq12b")) {
-		printk("I/O port conflict\n");
-		return -EIO;
-	}
-	dev->iobase = iobase;
+	ret = comedi_request_region(dev, it->options[0], ADQ12B_SIZE);
+	if (ret)
+		return ret;
 
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	devpriv->unipolar = unipolar;
-	devpriv->differential = differential;
+	devpriv->unipolar = it->options[1];
+	devpriv->differential = it->options[2];
 	devpriv->digital_state = 0;
 	/*
 	 * initialize channel and range to -1 so we make sure we
@@ -265,7 +241,7 @@ static int adq12b_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s = &dev->subdevices[0];
 	/* analog input subdevice */
 	s->type = COMEDI_SUBD_AI;
-	if (differential) {
+	if (devpriv->differential) {
 		s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_DIFF;
 		s->n_chan = 8;
 	} else {
@@ -273,7 +249,7 @@ static int adq12b_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		s->n_chan = 16;
 	}
 
-	if (unipolar)
+	if (devpriv->unipolar)
 		s->range_table = &range_adq12b_ai_unipolar;
 	else
 		s->range_table = &range_adq12b_ai_bipolar;
@@ -302,22 +278,14 @@ static int adq12b_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->range_table = &range_digital;
 	s->insn_bits = adq12b_do_insn_bits;
 
-	printk(KERN_INFO "attached\n");
-
 	return 0;
 }
 
-static void adq12b_detach(struct comedi_device *dev)
-{
-	if (dev->iobase)
-		release_region(dev->iobase, ADQ12B_SIZE);
-}
-
 static struct comedi_driver adq12b_driver = {
 	.driver_name	= "adq12b",
 	.module		= THIS_MODULE,
 	.attach		= adq12b_attach,
-	.detach		= adq12b_detach,
+	.detach		= comedi_legacy_detach,
 };
 module_comedi_driver(adq12b_driver);
 
diff --git a/drivers/staging/comedi/drivers/adv_pci1710.c b/drivers/staging/comedi/drivers/adv_pci1710.c
index 3d788c76d64..f847bbc175e 100644
--- a/drivers/staging/comedi/drivers/adv_pci1710.c
+++ b/drivers/staging/comedi/drivers/adv_pci1710.c
@@ -59,9 +59,6 @@ Configuration options:
 #define TYPE_PCI1713	2
 #define TYPE_PCI1720	3
 
-#define IORANGE_171x	32
-#define IORANGE_1720	16
-
 #define PCI171x_AD_DATA	 0	/* R:   A/D data */
 #define PCI171x_SOFTTRG	 0	/* W:   soft trigger for A/D */
 #define PCI171x_RANGE	 2	/* W:   A/D gain/range register */
@@ -178,10 +175,17 @@ static const struct comedi_lrange range_pci171x_da = { 2, {
 							   }
 };
 
+enum pci1710_boardid {
+	BOARD_PCI1710,
+	BOARD_PCI1710HG,
+	BOARD_PCI1711,
+	BOARD_PCI1713,
+	BOARD_PCI1720,
+	BOARD_PCI1731,
+};
+
 struct boardtype {
 	const char *name;	/*  board name */
-	int device_id;
-	int iorange;		/*  I/O range len */
 	char have_irq;		/*  1=card support IRQ */
 	char cardtype;		/*  0=1710& co. 2=1713, ... */
 	int n_aichan;		/*  num of A/D chans */
@@ -200,10 +204,8 @@ struct boardtype {
 };
 
 static const struct boardtype boardtypes[] = {
-	{
+	[BOARD_PCI1710] = {
 		.name		= "pci1710",
-		.device_id	= 0x1710,
-		.iorange	= IORANGE_171x,
 		.have_irq	= 1,
 		.cardtype	= TYPE_PCI171X,
 		.n_aichan	= 16,
@@ -219,10 +221,9 @@ static const struct boardtype boardtypes[] = {
 		.rangelist_ao	= &range_pci171x_da,
 		.ai_ns_min	= 10000,
 		.fifo_half_size	= 2048,
-	}, {
+	},
+	[BOARD_PCI1710HG] = {
 		.name		= "pci1710hg",
-		.device_id	= 0x1710,
-		.iorange	= IORANGE_171x,
 		.have_irq	= 1,
 		.cardtype	= TYPE_PCI171X,
 		.n_aichan	= 16,
@@ -238,10 +239,9 @@ static const struct boardtype boardtypes[] = {
 		.rangelist_ao	= &range_pci171x_da,
 		.ai_ns_min	= 10000,
 		.fifo_half_size	= 2048,
-	}, {
+	},
+	[BOARD_PCI1711] = {
 		.name		= "pci1711",
-		.device_id	= 0x1711,
-		.iorange	= IORANGE_171x,
 		.have_irq	= 1,
 		.cardtype	= TYPE_PCI171X,
 		.n_aichan	= 16,
@@ -256,10 +256,9 @@ static const struct boardtype boardtypes[] = {
 		.rangelist_ao	= &range_pci171x_da,
 		.ai_ns_min	= 10000,
 		.fifo_half_size	= 512,
-	}, {
+	},
+	[BOARD_PCI1713] = {
 		.name		= "pci1713",
-		.device_id	= 0x1713,
-		.iorange	= IORANGE_171x,
 		.have_irq	= 1,
 		.cardtype	= TYPE_PCI1713,
 		.n_aichan	= 32,
@@ -269,18 +268,16 @@ static const struct boardtype boardtypes[] = {
 		.rangecode_ai	= range_codes_pci1710_3,
 		.ai_ns_min	= 10000,
 		.fifo_half_size	= 2048,
-	}, {
+	},
+	[BOARD_PCI1720] = {
 		.name		= "pci1720",
-		.device_id	= 0x1720,
-		.iorange	= IORANGE_1720,
 		.cardtype	= TYPE_PCI1720,
 		.n_aochan	= 4,
 		.ao_maxdata	= 0x0fff,
 		.rangelist_ao	= &range_pci1720,
-	}, {
+	},
+	[BOARD_PCI1731] = {
 		.name		= "pci1731",
-		.device_id	= 0x1731,
-		.iorange	= IORANGE_171x,
 		.have_irq	= 1,
 		.cardtype	= TYPE_PCI171X,
 		.n_aichan	= 16,
@@ -307,7 +304,7 @@ struct pci1710_private {
 	unsigned int ai_et_CntrlReg;
 	unsigned int ai_et_MuxVal;
 	unsigned int ai_et_div1, ai_et_div2;
-	unsigned int act_chanlist[32];	/*  list of scaned channel */
+	unsigned int act_chanlist[32];	/*  list of scanned channel */
 	unsigned char act_chanlist_len;	/*  len of scanlist */
 	unsigned char act_chanlist_pos;	/*  actual position in MUX list */
 	unsigned char da_ranges;	/*  copy of D/A outpit range register */
@@ -334,7 +331,7 @@ static const unsigned int muxonechan[] = {
 
 /*
 ==============================================================================
- Check if channel list from user is builded correctly
+ Check if channel list from user is built correctly
  If it's ok, then program scan/gain logic.
  This works for all cards.
 */
@@ -1220,30 +1217,17 @@ static int pci1710_reset(struct comedi_device *dev)
 	}
 }
 
-static const void *pci1710_find_boardinfo(struct comedi_device *dev,
-					  struct pci_dev *pcidev)
-{
-	const struct boardtype *this_board;
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(boardtypes); i++) {
-		this_board = &boardtypes[i];
-		if (pcidev->device == this_board->device_id)
-			return this_board;
-	}
-	return NULL;
-}
-
 static int pci1710_auto_attach(struct comedi_device *dev,
-					 unsigned long context_unused)
+			       unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct boardtype *this_board;
+	const struct boardtype *this_board = NULL;
 	struct pci1710_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret, subdev, n_subdevices;
 
-	this_board = pci1710_find_boardinfo(dev, pcidev);
+	if (context < ARRAY_SIZE(boardtypes))
+		this_board = &boardtypes[context];
 	if (!this_board)
 		return -ENODEV;
 	dev->board_ptr = this_board;
@@ -1254,7 +1238,7 @@ static int pci1710_auto_attach(struct comedi_device *dev,
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
 	dev->iobase = pci_resource_start(pcidev, 2);
@@ -1378,16 +1362,11 @@ static int pci1710_auto_attach(struct comedi_device *dev,
 
 static void pci1710_detach(struct comedi_device *dev)
 {
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-
 	if (dev->iobase)
 		pci1710_reset(dev);
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-	}
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver adv_pci1710_driver = {
@@ -1398,17 +1377,68 @@ static struct comedi_driver adv_pci1710_driver = {
 };
 
 static int adv_pci1710_pci_probe(struct pci_dev *dev,
-					   const struct pci_device_id *ent)
+				 const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &adv_pci1710_driver);
+	return comedi_pci_auto_config(dev, &adv_pci1710_driver,
+				      id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(adv_pci1710_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1710) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1711) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1713) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1720) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1731) },
+	{
+		PCI_DEVICE_SUB(PCI_VENDOR_ID_ADVANTECH, 0x1710,
+			       PCI_VENDOR_ID_PLX, PCI_DEVICE_ID_PLX_9050),
+		.driver_data = BOARD_PCI1710,
+	}, {
+		PCI_DEVICE_SUB(PCI_VENDOR_ID_ADVANTECH, 0x1710,
+			       PCI_VENDOR_ID_ADVANTECH, 0x0000),
+		.driver_data = BOARD_PCI1710,
+	}, {
+		PCI_DEVICE_SUB(PCI_VENDOR_ID_ADVANTECH, 0x1710,
+			       PCI_VENDOR_ID_ADVANTECH, 0xb100),
+		.driver_data = BOARD_PCI1710,
+	}, {
+		PCI_DEVICE_SUB(PCI_VENDOR_ID_ADVANTECH, 0x1710,
+			       PCI_VENDOR_ID_ADVANTECH, 0xb200),
+		.driver_data = BOARD_PCI1710,
+	}, {
+		PCI_DEVICE_SUB(PCI_VENDOR_ID_ADVANTECH, 0x1710,
+			       PCI_VENDOR_ID_ADVANTECH, 0xc100),
+		.driver_data = BOARD_PCI1710,
+	}, {
+		PCI_DEVICE_SUB(PCI_VENDOR_ID_ADVANTECH, 0x1710,
+			       PCI_VENDOR_ID_ADVANTECH, 0xc200),
+		.driver_data = BOARD_PCI1710,
+	}, {
+		PCI_DEVICE_SUB(PCI_VENDOR_ID_ADVANTECH, 0x1710, 0x1000, 0xd100),
+		.driver_data = BOARD_PCI1710,
+	}, {
+		PCI_DEVICE_SUB(PCI_VENDOR_ID_ADVANTECH, 0x1710,
+			       PCI_VENDOR_ID_ADVANTECH, 0x0002),
+		.driver_data = BOARD_PCI1710HG,
+	}, {
+		PCI_DEVICE_SUB(PCI_VENDOR_ID_ADVANTECH, 0x1710,
+			       PCI_VENDOR_ID_ADVANTECH, 0xb102),
+		.driver_data = BOARD_PCI1710HG,
+	}, {
+		PCI_DEVICE_SUB(PCI_VENDOR_ID_ADVANTECH, 0x1710,
+			       PCI_VENDOR_ID_ADVANTECH, 0xb202),
+		.driver_data = BOARD_PCI1710HG,
+	}, {
+		PCI_DEVICE_SUB(PCI_VENDOR_ID_ADVANTECH, 0x1710,
+			       PCI_VENDOR_ID_ADVANTECH, 0xc102),
+		.driver_data = BOARD_PCI1710HG,
+	}, {
+		PCI_DEVICE_SUB(PCI_VENDOR_ID_ADVANTECH, 0x1710,
+			       PCI_VENDOR_ID_ADVANTECH, 0xc202),
+		.driver_data = BOARD_PCI1710HG,
+	}, {
+		PCI_DEVICE_SUB(PCI_VENDOR_ID_ADVANTECH, 0x1710, 0x1000, 0xd102),
+		.driver_data = BOARD_PCI1710HG,
+	},
+	{ PCI_VDEVICE(ADVANTECH, 0x1711), BOARD_PCI1711 },
+	{ PCI_VDEVICE(ADVANTECH, 0x1713), BOARD_PCI1713 },
+	{ PCI_VDEVICE(ADVANTECH, 0x1720), BOARD_PCI1720 },
+	{ PCI_VDEVICE(ADVANTECH, 0x1731), BOARD_PCI1731 },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, adv_pci1710_pci_table);
diff --git a/drivers/staging/comedi/drivers/adv_pci1723.c b/drivers/staging/comedi/drivers/adv_pci1723.c
index 02ce55a01d2..ccc114d6c08 100644
--- a/drivers/staging/comedi/drivers/adv_pci1723.c
+++ b/drivers/staging/comedi/drivers/adv_pci1723.c
@@ -242,14 +242,12 @@ static int pci1723_auto_attach(struct comedi_device *dev,
 	struct comedi_subdevice *s;
 	int ret;
 
-	dev->board_name = dev->driver->driver_name;
-
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
 	dev->iobase = pci_resource_start(pcidev, 2);
@@ -306,14 +304,9 @@ static int pci1723_auto_attach(struct comedi_device *dev,
 
 static void pci1723_detach(struct comedi_device *dev)
 {
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-
-	if (pcidev) {
-		if (dev->iobase) {
-			pci1723_reset(dev);
-			comedi_pci_disable(pcidev);
-		}
-	}
+	if (dev->iobase)
+		pci1723_reset(dev);
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver adv_pci1723_driver = {
@@ -324,9 +317,10 @@ static struct comedi_driver adv_pci1723_driver = {
 };
 
 static int adv_pci1723_pci_probe(struct pci_dev *dev,
-					   const struct pci_device_id *ent)
+				 const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &adv_pci1723_driver);
+	return comedi_pci_auto_config(dev, &adv_pci1723_driver,
+				      id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(adv_pci1723_pci_table) = {
diff --git a/drivers/staging/comedi/drivers/adv_pci1724.c b/drivers/staging/comedi/drivers/adv_pci1724.c
new file mode 100644
index 00000000000..e60f12578d4
--- /dev/null
+++ b/drivers/staging/comedi/drivers/adv_pci1724.c
@@ -0,0 +1,409 @@
+/*
+    comedi/drivers/adv_pci1724.c
+    This is a driver for the Advantech PCI-1724U card.
+
+    Author:  Frank Mori Hess <fmh6jj@gmail.com>
+    Copyright (C) 2013 GnuBIO Inc
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-8 David A. Schleef <ds@schleef.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.
+
+************************************************************************/
+
+/*
+
+Driver: adv_1724
+Description: Advantech PCI-1724U
+Author: Frank Mori Hess <fmh6jj@gmail.com>
+Status: works
+Updated: 2013-02-09
+Devices: [Advantech] PCI-1724U (adv_pci1724)
+
+Subdevice 0 is the analog output.
+Subdevice 1 is the offset calibration for the analog output.
+Subdevice 2 is the gain calibration for the analog output.
+
+The calibration offset and gains have quite a large effect
+on the analog output, so it is possible to adjust the analog output to
+have an output range significantly different from the board's
+nominal output ranges.  For a calibrated +/- 10V range, the analog
+output's offset will be set somewhere near mid-range (0x2000) and its
+gain will be near maximum (0x3fff).
+
+There is really no difference between the board's documented 0-20mA
+versus 4-20mA output ranges.  To pick one or the other is simply a matter
+of adjusting the offset and gain calibration until the board outputs in
+the desired range.
+
+Configuration options:
+   None
+
+Manual configuration of comedi devices is not supported by this driver;
+supported PCI devices are configured as comedi devices automatically.
+
+*/
+
+#include <linux/pci.h>
+
+#include "../comedidev.h"
+
+#define PCI_VENDOR_ID_ADVANTECH	0x13fe
+
+#define NUM_AO_CHANNELS 32
+
+/* register offsets */
+enum board_registers {
+	DAC_CONTROL_REG = 0x0,
+	SYNC_OUTPUT_REG = 0x4,
+	EEPROM_CONTROL_REG = 0x8,
+	SYNC_OUTPUT_TRIGGER_REG = 0xc,
+	BOARD_ID_REG = 0x10
+};
+
+/* bit definitions for registers */
+enum dac_control_contents {
+	DAC_DATA_MASK = 0x3fff,
+	DAC_DESTINATION_MASK = 0xc000,
+	DAC_NORMAL_MODE = 0xc000,
+	DAC_OFFSET_MODE = 0x8000,
+	DAC_GAIN_MODE = 0x4000,
+	DAC_CHANNEL_SELECT_MASK = 0xf0000,
+	DAC_GROUP_SELECT_MASK = 0xf00000
+};
+
+static uint32_t dac_data_bits(uint16_t dac_data)
+{
+	return dac_data & DAC_DATA_MASK;
+}
+
+static uint32_t dac_channel_select_bits(unsigned channel)
+{
+	return (channel << 16) & DAC_CHANNEL_SELECT_MASK;
+}
+
+static uint32_t dac_group_select_bits(unsigned group)
+{
+	return (1 << (20 + group)) & DAC_GROUP_SELECT_MASK;
+}
+
+static uint32_t dac_channel_and_group_select_bits(unsigned comedi_channel)
+{
+	return dac_channel_select_bits(comedi_channel % 8) |
+		dac_group_select_bits(comedi_channel / 8);
+}
+
+enum sync_output_contents {
+	SYNC_MODE = 0x1,
+	DAC_BUSY = 0x2, /* dac state machine is not ready */
+};
+
+enum sync_output_trigger_contents {
+	SYNC_TRIGGER_BITS = 0x0 /* any value works */
+};
+
+enum board_id_contents {
+	BOARD_ID_MASK = 0xf
+};
+
+static const struct comedi_lrange ao_ranges_1724 = { 4,
+	{
+		BIP_RANGE(10),
+		RANGE_mA(0, 20),
+		RANGE_mA(4, 20),
+		RANGE_unitless(0, 1)
+	}
+};
+
+static const struct comedi_lrange *const ao_range_list_1724[NUM_AO_CHANNELS] = {
+	[0 ... NUM_AO_CHANNELS - 1] = &ao_ranges_1724,
+};
+
+/* this structure is for data unique to this hardware driver. */
+struct adv_pci1724_private {
+	int ao_value[NUM_AO_CHANNELS];
+	int offset_value[NUM_AO_CHANNELS];
+	int gain_value[NUM_AO_CHANNELS];
+};
+
+static int wait_for_dac_idle(struct comedi_device *dev)
+{
+	static const int timeout = 10000;
+	int i;
+
+	for (i = 0; i < timeout; ++i) {
+		if ((inl(dev->iobase + SYNC_OUTPUT_REG) & DAC_BUSY) == 0)
+			break;
+		udelay(1);
+	}
+	if (i == timeout) {
+		comedi_error(dev, "Timed out waiting for dac to become idle.");
+		return -EIO;
+	}
+	return 0;
+}
+
+static int set_dac(struct comedi_device *dev, unsigned mode, unsigned channel,
+		   unsigned data)
+{
+	int retval;
+	unsigned control_bits;
+
+	retval = wait_for_dac_idle(dev);
+	if (retval < 0)
+		return retval;
+
+	control_bits = mode;
+	control_bits |= dac_channel_and_group_select_bits(channel);
+	control_bits |= dac_data_bits(data);
+	outl(control_bits, dev->iobase + DAC_CONTROL_REG);
+	return 0;
+}
+
+static int ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
+		    struct comedi_insn *insn, unsigned int *data)
+{
+	struct adv_pci1724_private *devpriv = dev->private;
+	int channel = CR_CHAN(insn->chanspec);
+	int retval;
+	int i;
+
+	/* turn off synchronous mode */
+	outl(0, dev->iobase + SYNC_OUTPUT_REG);
+
+	for (i = 0; i < insn->n; ++i) {
+		retval = set_dac(dev, DAC_NORMAL_MODE, channel, data[i]);
+		if (retval < 0)
+			return retval;
+		devpriv->ao_value[channel] = data[i];
+	}
+	return insn->n;
+}
+
+static int ao_readback_insn(struct comedi_device *dev,
+			    struct comedi_subdevice *s,
+			    struct comedi_insn *insn, unsigned int *data)
+{
+	struct adv_pci1724_private *devpriv = dev->private;
+	int channel = CR_CHAN(insn->chanspec);
+	int i;
+
+	if (devpriv->ao_value[channel] < 0) {
+		comedi_error(dev,
+			     "Cannot read back channels which have not yet been written to.");
+		return -EIO;
+	}
+	for (i = 0; i < insn->n; i++)
+		data[i] = devpriv->ao_value[channel];
+
+	return insn->n;
+}
+
+static int offset_write_insn(struct comedi_device *dev,
+			     struct comedi_subdevice *s,
+			     struct comedi_insn *insn, unsigned int *data)
+{
+	struct adv_pci1724_private *devpriv = dev->private;
+	int channel = CR_CHAN(insn->chanspec);
+	int retval;
+	int i;
+
+	/* turn off synchronous mode */
+	outl(0, dev->iobase + SYNC_OUTPUT_REG);
+
+	for (i = 0; i < insn->n; ++i) {
+		retval = set_dac(dev, DAC_OFFSET_MODE, channel, data[i]);
+		if (retval < 0)
+			return retval;
+		devpriv->offset_value[channel] = data[i];
+	}
+
+	return insn->n;
+}
+
+static int offset_read_insn(struct comedi_device *dev,
+			    struct comedi_subdevice *s,
+			    struct comedi_insn *insn, unsigned int *data)
+{
+	struct adv_pci1724_private *devpriv = dev->private;
+	unsigned int channel = CR_CHAN(insn->chanspec);
+	int i;
+
+	if (devpriv->offset_value[channel] < 0) {
+		comedi_error(dev,
+			     "Cannot read back channels which have not yet been written to.");
+		return -EIO;
+	}
+	for (i = 0; i < insn->n; i++)
+		data[i] = devpriv->offset_value[channel];
+
+	return insn->n;
+}
+
+static int gain_write_insn(struct comedi_device *dev,
+			   struct comedi_subdevice *s,
+			   struct comedi_insn *insn, unsigned int *data)
+{
+	struct adv_pci1724_private *devpriv = dev->private;
+	int channel = CR_CHAN(insn->chanspec);
+	int retval;
+	int i;
+
+	/* turn off synchronous mode */
+	outl(0, dev->iobase + SYNC_OUTPUT_REG);
+
+	for (i = 0; i < insn->n; ++i) {
+		retval = set_dac(dev, DAC_GAIN_MODE, channel, data[i]);
+		if (retval < 0)
+			return retval;
+		devpriv->gain_value[channel] = data[i];
+	}
+
+	return insn->n;
+}
+
+static int gain_read_insn(struct comedi_device *dev,
+			  struct comedi_subdevice *s, struct comedi_insn *insn,
+			  unsigned int *data)
+{
+	struct adv_pci1724_private *devpriv = dev->private;
+	unsigned int channel = CR_CHAN(insn->chanspec);
+	int i;
+
+	if (devpriv->gain_value[channel] < 0) {
+		comedi_error(dev,
+			     "Cannot read back channels which have not yet been written to.");
+		return -EIO;
+	}
+	for (i = 0; i < insn->n; i++)
+		data[i] = devpriv->gain_value[channel];
+
+	return insn->n;
+}
+
+/* Allocate and initialize the subdevice structures.
+ */
+static int setup_subdevices(struct comedi_device *dev)
+{
+	struct comedi_subdevice *s;
+	int ret;
+
+	ret = comedi_alloc_subdevices(dev, 3);
+	if (ret)
+		return ret;
+
+	/* analog output subdevice */
+	s = &dev->subdevices[0];
+	s->type = COMEDI_SUBD_AO;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_GROUND;
+	s->n_chan = NUM_AO_CHANNELS;
+	s->maxdata = 0x3fff;
+	s->range_table_list = ao_range_list_1724;
+	s->insn_read = ao_readback_insn;
+	s->insn_write = ao_winsn;
+
+	/* offset calibration */
+	s = &dev->subdevices[1];
+	s->type = COMEDI_SUBD_CALIB;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
+	s->n_chan = NUM_AO_CHANNELS;
+	s->insn_read = offset_read_insn;
+	s->insn_write = offset_write_insn;
+	s->maxdata = 0x3fff;
+
+	/* gain calibration */
+	s = &dev->subdevices[2];
+	s->type = COMEDI_SUBD_CALIB;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
+	s->n_chan = NUM_AO_CHANNELS;
+	s->insn_read = gain_read_insn;
+	s->insn_write = gain_write_insn;
+	s->maxdata = 0x3fff;
+
+	return 0;
+}
+
+static int adv_pci1724_auto_attach(struct comedi_device *dev,
+				   unsigned long context_unused)
+{
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	struct adv_pci1724_private *devpriv;
+	int i;
+	int retval;
+	unsigned int board_id;
+
+	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
+	if (!devpriv)
+		return -ENOMEM;
+	dev->private = devpriv;
+
+	/* init software copies of output values to indicate we don't know
+	 * what the output value is since it has never been written. */
+	for (i = 0; i < NUM_AO_CHANNELS; ++i) {
+		devpriv->ao_value[i] = -1;
+		devpriv->offset_value[i] = -1;
+		devpriv->gain_value[i] = -1;
+	}
+
+	retval = comedi_pci_enable(dev);
+	if (retval)
+		return retval;
+
+	dev->iobase = pci_resource_start(pcidev, 2);
+	board_id = inl(dev->iobase + BOARD_ID_REG) & BOARD_ID_MASK;
+	dev_info(dev->class_dev, "board id: %d\n", board_id);
+
+	retval = setup_subdevices(dev);
+	if (retval < 0)
+		return retval;
+
+	dev_info(dev->class_dev, "%s (pci %s) attached, board id: %u\n",
+		 dev->board_name, pci_name(pcidev), board_id);
+	return 0;
+}
+
+static struct comedi_driver adv_pci1724_driver = {
+	.driver_name = "adv_pci1724",
+	.module = THIS_MODULE,
+	.auto_attach = adv_pci1724_auto_attach,
+	.detach = comedi_pci_disable,
+};
+
+static int adv_pci1724_pci_probe(struct pci_dev *dev,
+				 const struct pci_device_id *id)
+{
+	return comedi_pci_auto_config(dev, &adv_pci1724_driver,
+				      id->driver_data);
+}
+
+static DEFINE_PCI_DEVICE_TABLE(adv_pci1724_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1724) },
+	{ 0 }
+};
+MODULE_DEVICE_TABLE(pci, adv_pci1724_pci_table);
+
+static struct pci_driver adv_pci1724_pci_driver = {
+	.name = "adv_pci1724",
+	.id_table = adv_pci1724_pci_table,
+	.probe = adv_pci1724_pci_probe,
+	.remove = comedi_pci_auto_unconfig,
+};
+
+module_comedi_pci_driver(adv_pci1724_driver, adv_pci1724_pci_driver);
+
+MODULE_AUTHOR("Frank Mori Hess <fmh6jj@gmail.com>");
+MODULE_DESCRIPTION("Advantech PCI-1724U Comedi driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/adv_pci_dio.c b/drivers/staging/comedi/drivers/adv_pci_dio.c
index 338c43e716b..f70c67471c1 100644
--- a/drivers/staging/comedi/drivers/adv_pci_dio.c
+++ b/drivers/staging/comedi/drivers/adv_pci_dio.c
@@ -236,8 +236,6 @@ struct diosubd_data {
 
 struct dio_boardtype {
 	const char *name;	/*  board name */
-	int vendor_id;		/*  vendor/device PCI ID */
-	int device_id;
 	int main_pci_region;	/*  main I/O PCI region */
 	enum hw_cards_id cardtype;
 	int nsubdevs;
@@ -250,10 +248,8 @@ struct dio_boardtype {
 };
 
 static const struct dio_boardtype boardtypes[] = {
-	{
+	[TYPE_PCI1730] = {
 		.name		= "pci1730",
-		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
-		.device_id	= 0x1730,
 		.main_pci_region = PCIDIO_MAINREG,
 		.cardtype	= TYPE_PCI1730,
 		.nsubdevs	= 5,
@@ -263,30 +259,27 @@ static const struct dio_boardtype boardtypes[] = {
 		.sdo[1]		= { 16, PCI1730_IDO, 2, 0, },
 		.boardid	= { 4, PCI173x_BOARDID, 1, SDF_INTERNAL, },
 		.io_access	= IO_8b,
-	}, {
+	},
+	[TYPE_PCI1733] = {
 		.name		= "pci1733",
-		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
-		.device_id	= 0x1733,
 		.main_pci_region = PCIDIO_MAINREG,
 		.cardtype	= TYPE_PCI1733,
 		.nsubdevs	= 2,
 		.sdi[1]		= { 32, PCI1733_IDI, 4, 0, },
 		.boardid	= { 4, PCI173x_BOARDID, 1, SDF_INTERNAL, },
 		.io_access	= IO_8b,
-	}, {
+	},
+	[TYPE_PCI1734] = {
 		.name		= "pci1734",
-		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
-		.device_id	= 0x1734,
 		.main_pci_region = PCIDIO_MAINREG,
 		.cardtype	= TYPE_PCI1734,
 		.nsubdevs	= 2,
 		.sdo[1]		= { 32, PCI1734_IDO, 4, 0, },
 		.boardid	= { 4, PCI173x_BOARDID, 1, SDF_INTERNAL, },
 		.io_access	= IO_8b,
-	}, {
+	},
+	[TYPE_PCI1735] = {
 		.name		= "pci1735",
-		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
-		.device_id	= 0x1735,
 		.main_pci_region = PCIDIO_MAINREG,
 		.cardtype	= TYPE_PCI1735,
 		.nsubdevs	= 4,
@@ -295,10 +288,9 @@ static const struct dio_boardtype boardtypes[] = {
 		.boardid	= { 4, PCI1735_BOARDID, 1, SDF_INTERNAL, },
 		.s8254[0]	= { 3, PCI1735_C8254, 1, 0, },
 		.io_access	= IO_8b,
-	}, {
+	},
+	[TYPE_PCI1736] = {
 		.name		= "pci1736",
-		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
-		.device_id	= 0x1736,
 		.main_pci_region = PCI1736_MAINREG,
 		.cardtype	= TYPE_PCI1736,
 		.nsubdevs	= 3,
@@ -306,39 +298,35 @@ static const struct dio_boardtype boardtypes[] = {
 		.sdo[1]		= { 16, PCI1736_IDO, 2, 0, },
 		.boardid	= { 4, PCI1736_BOARDID, 1, SDF_INTERNAL, },
 		.io_access	= IO_8b,
-	}, {
+	},
+	[TYPE_PCI1739] = {
 		.name		= "pci1739",
-		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
-		.device_id	= 0x1739,
 		.main_pci_region = PCIDIO_MAINREG,
 		.cardtype	= TYPE_PCI1739,
 		.nsubdevs	= 2,
 		.sdio[0]	= { 48, PCI1739_DIO, 2, 0, },
 		.io_access	= IO_8b,
-	}, {
+	},
+	[TYPE_PCI1750] = {
 		.name		= "pci1750",
-		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
-		.device_id	= 0x1750,
 		.main_pci_region = PCIDIO_MAINREG,
 		.cardtype	= TYPE_PCI1750,
 		.nsubdevs	= 2,
 		.sdi[1]		= { 16, PCI1750_IDI, 2, 0, },
 		.sdo[1]		= { 16, PCI1750_IDO, 2, 0, },
 		.io_access	= IO_8b,
-	}, {
+	},
+	[TYPE_PCI1751] = {
 		.name		= "pci1751",
-		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
-		.device_id	= 0x1751,
 		.main_pci_region = PCIDIO_MAINREG,
 		.cardtype	= TYPE_PCI1751,
 		.nsubdevs	= 3,
 		.sdio[0]	= { 48, PCI1751_DIO, 2, 0, },
 		.s8254[0]	= { 3, PCI1751_CNT, 1, 0, },
 		.io_access	= IO_8b,
-	}, {
+	},
+	[TYPE_PCI1752] = {
 		.name		= "pci1752",
-		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
-		.device_id	= 0x1752,
 		.main_pci_region = PCIDIO_MAINREG,
 		.cardtype	= TYPE_PCI1752,
 		.nsubdevs	= 3,
@@ -346,29 +334,26 @@ static const struct dio_boardtype boardtypes[] = {
 		.sdo[1]		= { 32, PCI1752_IDO2, 2, 0, },
 		.boardid	= { 4, PCI175x_BOARDID, 1, SDF_INTERNAL, },
 		.io_access	= IO_16b,
-	}, {
+	},
+	[TYPE_PCI1753] = {
 		.name		= "pci1753",
-		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
-		.device_id	= 0x1753,
 		.main_pci_region = PCIDIO_MAINREG,
 		.cardtype	= TYPE_PCI1753,
 		.nsubdevs	= 4,
 		.sdio[0]	= { 96, PCI1753_DIO, 4, 0, },
 		.io_access	= IO_8b,
-	}, {
+	},
+	[TYPE_PCI1753E] = {
 		.name		= "pci1753e",
-		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
-		.device_id	= 0x1753,
 		.main_pci_region = PCIDIO_MAINREG,
 		.cardtype	= TYPE_PCI1753E,
 		.nsubdevs	= 8,
 		.sdio[0]	= { 96, PCI1753_DIO, 4, 0, },
 		.sdio[1]	= { 96, PCI1753E_DIO, 4, 0, },
 		.io_access	= IO_8b,
-	}, {
+	},
+	[TYPE_PCI1754] = {
 		.name		= "pci1754",
-		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
-		.device_id	= 0x1754,
 		.main_pci_region = PCIDIO_MAINREG,
 		.cardtype	= TYPE_PCI1754,
 		.nsubdevs	= 3,
@@ -376,10 +361,9 @@ static const struct dio_boardtype boardtypes[] = {
 		.sdi[1]		= { 32, PCI1754_IDI2, 2, 0, },
 		.boardid	= { 4, PCI175x_BOARDID, 1, SDF_INTERNAL, },
 		.io_access	= IO_16b,
-	}, {
+	},
+	[TYPE_PCI1756] = {
 		.name		= "pci1756",
-		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
-		.device_id	= 0x1756,
 		.main_pci_region = PCIDIO_MAINREG,
 		.cardtype	= TYPE_PCI1756,
 		.nsubdevs	= 3,
@@ -387,19 +371,17 @@ static const struct dio_boardtype boardtypes[] = {
 		.sdo[1]		= { 32, PCI1756_IDO, 2, 0, },
 		.boardid	= { 4, PCI175x_BOARDID, 1, SDF_INTERNAL, },
 		.io_access	= IO_16b,
-	}, {
+	},
+	[TYPE_PCI1760] = {
 		/* This card has its own 'attach' */
 		.name		= "pci1760",
-		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
-		.device_id	= 0x1760,
 		.main_pci_region = 0,
 		.cardtype	= TYPE_PCI1760,
 		.nsubdevs	= 4,
 		.io_access	= IO_8b,
-	}, {
+	},
+	[TYPE_PCI1762] = {
 		.name		= "pci1762",
-		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
-		.device_id	= 0x1762,
 		.main_pci_region = PCIDIO_MAINREG,
 		.cardtype	= TYPE_PCI1762,
 		.nsubdevs	= 3,
@@ -1076,31 +1058,50 @@ static int pci_dio_add_8254(struct comedi_device *dev,
 	return 0;
 }
 
-static const void *pci_dio_find_boardinfo(struct comedi_device *dev,
-					  struct pci_dev *pcidev)
+static unsigned long pci_dio_override_cardtype(struct pci_dev *pcidev,
+					       unsigned long cardtype)
 {
-	const struct dio_boardtype *this_board;
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(boardtypes); ++i) {
-		this_board = &boardtypes[i];
-		if (this_board->vendor_id == pcidev->vendor &&
-		    this_board->device_id == pcidev->device)
-			return this_board;
+	/*
+	 * Change cardtype from TYPE_PCI1753 to TYPE_PCI1753E if expansion
+	 * board available.  Need to enable PCI device and request the main
+	 * registers PCI BAR temporarily to perform the test.
+	 */
+	if (cardtype != TYPE_PCI1753)
+		return cardtype;
+	if (pci_enable_device(pcidev) < 0)
+		return cardtype;
+	if (pci_request_region(pcidev, PCIDIO_MAINREG, "adv_pci_dio") == 0) {
+		/*
+		 * This test is based on Advantech's "advdaq" driver source
+		 * (which declares its module licence as "GPL" although the
+		 * driver source does not include a "COPYING" file).
+		 */
+		unsigned long reg =
+			pci_resource_start(pcidev, PCIDIO_MAINREG) + 53;
+
+		outb(0x05, reg);
+		if ((inb(reg) & 0x07) == 0x02) {
+			outb(0x02, reg);
+			if ((inb(reg) & 0x07) == 0x05)
+				cardtype = TYPE_PCI1753E;
+		}
+		pci_release_region(pcidev, PCIDIO_MAINREG);
 	}
-	return NULL;
+	pci_disable_device(pcidev);
+	return cardtype;
 }
 
 static int pci_dio_auto_attach(struct comedi_device *dev,
-					 unsigned long context_unused)
+			       unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct dio_boardtype *this_board;
+	const struct dio_boardtype *this_board = NULL;
 	struct pci_dio_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret, subdev, i, j;
 
-	this_board = pci_dio_find_boardinfo(dev, pcidev);
+	if (context < ARRAY_SIZE(boardtypes))
+		this_board = &boardtypes[context];
 	if (!this_board)
 		return -ENODEV;
 	dev->board_ptr = this_board;
@@ -1111,7 +1112,7 @@ static int pci_dio_auto_attach(struct comedi_device *dev,
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
 	dev->iobase = pci_resource_start(pcidev, this_board->main_pci_region);
@@ -1172,7 +1173,6 @@ static int pci_dio_auto_attach(struct comedi_device *dev,
 static void pci_dio_detach(struct comedi_device *dev)
 {
 	struct pci_dio_private *devpriv = dev->private;
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct comedi_subdevice *s;
 	int i;
 
@@ -1180,18 +1180,13 @@ static void pci_dio_detach(struct comedi_device *dev)
 		if (devpriv->valid)
 			pci_dio_reset(dev);
 	}
-	if (dev->subdevices) {
-		for (i = 0; i < dev->n_subdevices; i++) {
-			s = &dev->subdevices[i];
-			if (s->type == COMEDI_SUBD_DIO)
-				subdev_8255_cleanup(dev, s);
-			s->private = NULL;
-		}
-	}
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
+	for (i = 0; i < dev->n_subdevices; i++) {
+		s = &dev->subdevices[i];
+		if (s->type == COMEDI_SUBD_DIO)
+			comedi_spriv_free(dev, i);
+		s->private = NULL; /* some private data is static */
 	}
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver adv_pci_dio_driver = {
@@ -1202,26 +1197,29 @@ static struct comedi_driver adv_pci_dio_driver = {
 };
 
 static int adv_pci_dio_pci_probe(struct pci_dev *dev,
-					   const struct pci_device_id *ent)
+				 const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &adv_pci_dio_driver);
+	unsigned long cardtype;
+
+	cardtype = pci_dio_override_cardtype(dev, id->driver_data);
+	return comedi_pci_auto_config(dev, &adv_pci_dio_driver, cardtype);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(adv_pci_dio_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1730) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1733) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1734) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1735) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1736) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1739) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1750) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1751) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1752) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1753) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1754) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1756) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1760) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1762) },
+	{ PCI_VDEVICE(ADVANTECH, 0x1730), TYPE_PCI1730 },
+	{ PCI_VDEVICE(ADVANTECH, 0x1733), TYPE_PCI1733 },
+	{ PCI_VDEVICE(ADVANTECH, 0x1734), TYPE_PCI1734 },
+	{ PCI_VDEVICE(ADVANTECH, 0x1735), TYPE_PCI1735 },
+	{ PCI_VDEVICE(ADVANTECH, 0x1736), TYPE_PCI1736 },
+	{ PCI_VDEVICE(ADVANTECH, 0x1739), TYPE_PCI1739 },
+	{ PCI_VDEVICE(ADVANTECH, 0x1750), TYPE_PCI1750 },
+	{ PCI_VDEVICE(ADVANTECH, 0x1751), TYPE_PCI1751 },
+	{ PCI_VDEVICE(ADVANTECH, 0x1752), TYPE_PCI1752 },
+	{ PCI_VDEVICE(ADVANTECH, 0x1753), TYPE_PCI1753 },
+	{ PCI_VDEVICE(ADVANTECH, 0x1754), TYPE_PCI1754 },
+	{ PCI_VDEVICE(ADVANTECH, 0x1756), TYPE_PCI1756 },
+	{ PCI_VDEVICE(ADVANTECH, 0x1760), TYPE_PCI1760 },
+	{ PCI_VDEVICE(ADVANTECH, 0x1762), TYPE_PCI1762 },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, adv_pci_dio_pci_table);
diff --git a/drivers/staging/comedi/drivers/aio_aio12_8.c b/drivers/staging/comedi/drivers/aio_aio12_8.c
index 601f03d5897..e2dc08a058b 100644
--- a/drivers/staging/comedi/drivers/aio_aio12_8.c
+++ b/drivers/staging/comedi/drivers/aio_aio12_8.c
@@ -200,17 +200,11 @@ static int aio_aio12_8_attach(struct comedi_device *dev,
 	const struct aio12_8_boardtype *board = comedi_board(dev);
 	struct aio12_8_private *devpriv;
 	struct comedi_subdevice *s;
-	int iobase;
 	int ret;
 
-	dev->board_name = board->name;
-
-	iobase = it->options[0];
-	if (!request_region(iobase, 32, dev->board_name)) {
-		printk(KERN_ERR "I/O port conflict");
-		return -EIO;
-	}
-	dev->iobase = iobase;
+	ret = comedi_request_region(dev, it->options[0], 32);
+	if (ret)
+		return ret;
 
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
@@ -250,8 +244,8 @@ static int aio_aio12_8_attach(struct comedi_device *dev,
 
 	s = &dev->subdevices[2];
 	/* 8255 Digital i/o subdevice */
-	iobase = dev->iobase + AIO12_8_8255_BASE_REG;
-	ret = subdev_8255_init(dev, s, NULL, iobase);
+	ret = subdev_8255_init(dev, s, NULL,
+			       dev->iobase + AIO12_8_8255_BASE_REG);
 	if (ret)
 		return ret;
 
@@ -267,10 +261,8 @@ static int aio_aio12_8_attach(struct comedi_device *dev,
 
 static void aio_aio12_8_detach(struct comedi_device *dev)
 {
-	if (dev->subdevices)
-		subdev_8255_cleanup(dev, &dev->subdevices[2]);
-	if (dev->iobase)
-		release_region(dev->iobase, 24);
+	comedi_spriv_free(dev, 2);
+	comedi_legacy_detach(dev);
 }
 
 static struct comedi_driver aio_aio12_8_driver = {
diff --git a/drivers/staging/comedi/drivers/aio_iiro_16.c b/drivers/staging/comedi/drivers/aio_iiro_16.c
index 64c1ae58ce7..126854c0509 100644
--- a/drivers/staging/comedi/drivers/aio_iiro_16.c
+++ b/drivers/staging/comedi/drivers/aio_iiro_16.c
@@ -77,22 +77,12 @@ static int aio_iiro_16_dio_insn_bits_read(struct comedi_device *dev,
 static int aio_iiro_16_attach(struct comedi_device *dev,
 			      struct comedi_devconfig *it)
 {
-	int iobase;
 	struct comedi_subdevice *s;
 	int ret;
 
-	printk(KERN_INFO "comedi%d: aio_iiro_16: ", dev->minor);
-
-	dev->board_name = dev->driver->driver_name;
-
-	iobase = it->options[0];
-
-	if (!request_region(iobase, AIO_IIRO_16_SIZE, dev->board_name)) {
-		printk("I/O port conflict");
-		return -EIO;
-	}
-
-	dev->iobase = iobase;
+	ret = comedi_request_region(dev, it->options[0], AIO_IIRO_16_SIZE);
+	if (ret)
+		return ret;
 
 	ret = comedi_alloc_subdevices(dev, 2);
 	if (ret)
@@ -114,22 +104,14 @@ static int aio_iiro_16_attach(struct comedi_device *dev,
 	s->range_table = &range_digital;
 	s->insn_bits = aio_iiro_16_dio_insn_bits_read;
 
-	printk("attached\n");
-
 	return 1;
 }
 
-static void aio_iiro_16_detach(struct comedi_device *dev)
-{
-	if (dev->iobase)
-		release_region(dev->iobase, AIO_IIRO_16_SIZE);
-}
-
 static struct comedi_driver aio_iiro_16_driver = {
 	.driver_name	= "aio_iiro_16",
 	.module		= THIS_MODULE,
 	.attach		= aio_iiro_16_attach,
-	.detach		= aio_iiro_16_detach,
+	.detach		= comedi_legacy_detach,
 };
 module_comedi_driver(aio_iiro_16_driver);
 
diff --git a/drivers/staging/comedi/drivers/am9513.h b/drivers/staging/comedi/drivers/am9513.h
deleted file mode 100644
index 0bb839e5149..00000000000
--- a/drivers/staging/comedi/drivers/am9513.h
+++ /dev/null
@@ -1,79 +0,0 @@
-/*
-    module/am9513.h
-    value added preprocessor definitions for Am9513 timer chip
-
-    COMEDI - Linux Control and Measurement Device Interface
-    Copyright (C) 1998 David A. Schleef <ds@schleef.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 _AM9513_H_
-#define _AM9513_H_
-
-#if 0
-
-/*
- *	Before including this file, the following need to be defined:
- */
-#define Am9513_8BITBUS xxx
-/* or */
-#define Am9513_16BITBUS xxx
-
-#define Am9513_output_control(a)	xxx
-#define Am9513_input_status()		xxx
-#define Am9513_output_data(a)		xxx
-#define Am9513_input_data()		xxx
-
-#endif
-
-/*
- *
- */
-
-#ifdef Am9513_8BITBUS
-
-#define Am9513_write_register(reg, val)				\
-	do {							\
-		Am9513_output_control(reg);			\
-		Am9513_output_data(val>>8);			\
-		Am9513_output_data(val&0xff);			\
-	} while (0)
-
-#define Am9513_read_register(reg, val)				\
-	do {							\
-		Am9513_output_control(reg);			\
-		val = Am9513_input_data()<<8;			\
-		val |= Am9513_input_data();			\
-	} while (0)
-
-#else /* Am9513_16BITBUS */
-
-#define Am9513_write_register(reg, val)				\
-	do {							\
-		Am9513_output_control(reg);			\
-		Am9513_output_data(val);			\
-	} while (0)
-
-#define Am9513_read_register(reg, val)				\
-	do {							\
-		Am9513_output_control(reg);			\
-		val = Am9513_input_data();			\
-	} while (0)
-
-#endif
-
-#endif
diff --git a/drivers/staging/comedi/drivers/amplc_dio200.c b/drivers/staging/comedi/drivers/amplc_dio200.c
index 7c53dea12c7..297750bef0f 100644
--- a/drivers/staging/comedi/drivers/amplc_dio200.c
+++ b/drivers/staging/comedi/drivers/amplc_dio200.c
@@ -1,10 +1,9 @@
 /*
     comedi/drivers/amplc_dio200.c
-    Driver for Amplicon PC272E and PCI272 DIO boards.
-    (Support for other boards in Amplicon 200 series may be added at
-    a later date, e.g. PCI215.)
 
-    Copyright (C) 2005 MEV Ltd. <http://www.mev.co.uk/>
+    Driver for Amplicon PC212E, PC214E, PC215E, PC218E, PC272E.
+
+    Copyright (C) 2005-2013 MEV Ltd. <http://www.mev.co.uk/>
 
     COMEDI - Linux Control and Measurement Device Interface
     Copyright (C) 1998,2000 David A. Schleef <ds@schleef.org>
@@ -26,26 +25,23 @@
 */
 /*
  * Driver: amplc_dio200
- * Description: Amplicon 200 Series Digital I/O
+ * Description: Amplicon 200 Series ISA Digital I/O
  * Author: Ian Abbott <abbotti@mev.co.uk>
  * Devices: [Amplicon] PC212E (pc212e), PC214E (pc214e), PC215E (pc215e),
- *   PCI215 (pci215), PCIe215 (pcie215), PC218E (pc218e), PCIe236 (pcie236),
- *   PC272E (pc272e), PCI272 (pci272), PCIe296 (pcie296)
- * Updated: Wed, 24 Oct 2012 16:22:34 +0100
+ *   PC218E (pc218e), PC272E (pc272e)
+ * Updated: Mon, 18 Mar 2013 14:40:41 +0000
+ *
  * Status: works
  *
- * Configuration options - PC212E, PC214E, PC215E, PC218E, PC272E:
+ * Configuration options:
  *   [0] - I/O port base address
  *   [1] - IRQ (optional, but commands won't work without it)
  *
- * Manual configuration of PCI(e) cards is not supported; they are configured
- * automatically.
- *
  * Passing a zero for an option is the same as leaving it unspecified.
  *
  * SUBDEVICES
  *
- *                     PC212E         PC214E      PC215E/PCI215
+ *                     PC212E         PC214E         PC215E
  *                  -------------  -------------  -------------
  *   Subdevices           6              4              5
  *    0                 PPI-X          PPI-X          PPI-X
@@ -55,29 +51,16 @@
  *    4                 CTR-Z2                      INTERRUPT
  *    5               INTERRUPT
  *
- *                     PCIe215        PC218E         PCIe236
- *                  -------------  -------------  -------------
- *   Subdevices           8              7              8
- *    0                 PPI-X          CTR-X1         PPI-X
- *    1                 UNUSED         CTR-X2         UNUSED
- *    2                 PPI-Y          CTR-Y1         UNUSED
- *    3                 UNUSED         CTR-Y2         UNUSED
- *    4                 CTR-Z1         CTR-Z1         CTR-Z1
- *    5                 CTR-Z2         CTR-Z2         CTR-Z2
- *    6                 TIMER        INTERRUPT        TIMER
- *    7               INTERRUPT                     INTERRUPT
- *
- *                  PC272E/PCI272     PCIe296
+ *                     PC218E         PC272E
  *                  -------------  -------------
- *   Subdevices           4              8
- *    0                 PPI-X          PPI-X1
- *    1                 PPI-Y          PPI-X2
- *    2                 PPI-Z          PPI-Y1
- *    3               INTERRUPT        PPI-Y2
- *    4                                CTR-Z1
- *    5                                CTR-Z2
- *    6                                TIMER
- *    7                              INTERRUPT
+ *   Subdevices           7              4
+ *    0                 CTR-X1         PPI-X
+ *    1                 CTR-X2         PPI-Y
+ *    2                 CTR-Y1         PPI-Z
+ *    3                 CTR-Y2       INTERRUPT
+ *    4                 CTR-Z1
+ *    5                 CTR-Z2
+ *    6               INTERRUPT
  *
  * Each PPI is a 8255 chip providing 24 DIO channels.  The DIO channels
  * are configurable as inputs or outputs in four groups:
@@ -120,14 +103,6 @@
  *         the SK1 connector.  This pin is shared by all three counter
  *         channels on the chip.
  *
- *     For the PCIe boards, clock sources in the range 0 to 31 are allowed
- *     and the following additional clock sources are defined:
- *
- *       8.  HIGH logic level.
- *       9.  LOW logic level.
- *      10.  "Pattern present" signal.
- *      11.  Internal 20 MHz clock.
- *
  *   INSN_CONFIG_GET_CLOCK_SRC.  Returns the counter channel's current
  *     clock source in data[1].  For internal clock sources, data[2] is set
  *     to the period in ns.
@@ -149,27 +124,6 @@
  *       6.  Reserved.
  *       7.  Reserved.
  *
- *     For the PCIe boards, gate sources in the range 0 to 31 are allowed;
- *     the following additional clock sources and clock sources 6 and 7 are
- *     (re)defined:
- *
- *       6.  /GAT n, negated version of the counter channel's dedicated
- *         GAT input (negated version of gate source 2).
- *       7.  OUT n-2, the non-inverted output of counter channel n-2
- *         (negated version of gate source 3).
- *       8.  "Pattern present" signal, HIGH while pattern present.
- *       9.  "Pattern occurred" latched signal, latches HIGH when pattern
- *         occurs.
- *      10.  "Pattern gone away" latched signal, latches LOW when pattern
- *         goes away after it occurred.
- *      11.  Negated "pattern present" signal, LOW while pattern present
- *         (negated version of gate source 8).
- *      12.  Negated "pattern occurred" latched signal, latches LOW when
- *         pattern occurs (negated version of gate source 9).
- *      13.  Negated "pattern gone away" latched signal, latches LOW when
- *         pattern goes away after it occurred (negated version of gate
- *         source 10).
- *
  *   INSN_CONFIG_GET_GATE_SRC.  Returns the counter channel's current gate
  *     source in data[2].
  *
@@ -186,8 +140,6 @@
  *   3.  The counter subdevices are connected in a ring, so the highest
  *   counter subdevice precedes the lowest.
  *
- * The 'TIMER' subdevice is a free-running 32-bit timer subdevice.
- *
  * The 'INTERRUPT' subdevice pretends to be a digital input subdevice.  The
  * digital inputs come from the interrupt status register.  The number of
  * channels matches the number of interrupt sources.  The PC214E does not
@@ -196,7 +148,7 @@
  *
  * INTERRUPT SOURCES
  *
- *                     PC212E         PC214E      PC215E/PCI215
+ *                     PC212E         PC214E         PC215E
  *                  -------------  -------------  -------------
  *   Sources              6              1              6
  *    0               PPI-X-C0       JUMPER-J5      PPI-X-C0
@@ -206,25 +158,15 @@
  *    4              CTR-Z1-OUT1                   CTR-Z1-OUT1
  *    5              CTR-Z2-OUT1                   CTR-Z2-OUT1
  *
- *                     PCIe215        PC218E         PCIe236
- *                  -------------  -------------  -------------
- *   Sources              6              6              6
- *    0               PPI-X-C0      CTR-X1-OUT1     PPI-X-C0
- *    1               PPI-X-C3      CTR-X2-OUT1     PPI-X-C3
- *    2               PPI-Y-C0      CTR-Y1-OUT1      unused
- *    3               PPI-Y-C3      CTR-Y2-OUT1      unused
- *    4              CTR-Z1-OUT1    CTR-Z1-OUT1    CTR-Z1-OUT1
- *    5              CTR-Z2-OUT1    CTR-Z2-OUT1    CTR-Z2-OUT1
- *
- *                  PC272E/PCI272     PCIe296
+ *                     PC218E         PC272E
  *                  -------------  -------------
  *   Sources              6              6
- *    0               PPI-X-C0       PPI-X1-C0
- *    1               PPI-X-C3       PPI-X1-C3
- *    2               PPI-Y-C0       PPI-Y1-C0
- *    3               PPI-Y-C3       PPI-Y1-C3
- *    4               PPI-Z-C0      CTR-Z1-OUT1
- *    5               PPI-Z-C3      CTR-Z2-OUT1
+ *    0              CTR-X1-OUT1     PPI-X-C0
+ *    1              CTR-X2-OUT1     PPI-X-C3
+ *    2              CTR-Y1-OUT1     PPI-Y-C0
+ *    3              CTR-Y2-OUT1     PPI-Y-C3
+ *    4              CTR-Z1-OUT1     PPI-Z-C0
+ *    5              CTR-Z2-OUT1     PPI-Z-C3
  *
  * When an interrupt source is enabled in the interrupt source enable
  * register, a rising edge on the source signal latches the corresponding
@@ -232,14 +174,11 @@
  *
  * When the interrupt status register value as a whole (actually, just the
  * 6 least significant bits) goes from zero to non-zero, the board will
- * generate an interrupt.  For level-triggered hardware interrupts (PCI
- * card), the interrupt will remain asserted until the interrupt status
- * register is cleared to zero.  For edge-triggered hardware interrupts
- * (ISA card), no further interrupts will occur until the interrupt status
- * register is cleared to zero.  To clear a bit to zero in the interrupt
- * status register, the corresponding interrupt source must be disabled
- * in the interrupt source enable register (there is no separate interrupt
- * clear register).
+ * generate an interrupt.  No further interrupts will occur until the
+ * interrupt status register is cleared to zero.  To clear a bit to zero in
+ * the interrupt status register, the corresponding interrupt source must
+ * be disabled in the interrupt source enable register (there is no
+ * separate interrupt clear register).
  *
  * The PC214E does not have an interrupt source enable register or an
  * interrupt status register; its 'INTERRUPT' subdevice has a single
@@ -258,1835 +197,116 @@
  * order they appear in the channel list.
  */
 
-#include <linux/pci.h>
-#include <linux/interrupt.h>
 #include <linux/slab.h>
 
 #include "../comedidev.h"
 
-#include "comedi_fc.h"
-#include "8253.h"
-
-#define DIO200_DRIVER_NAME	"amplc_dio200"
-
-#define DO_ISA	IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_ISA)
-#define DO_PCI	IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_PCI)
-
-/* PCI IDs */
-#define PCI_DEVICE_ID_AMPLICON_PCI272 0x000a
-#define PCI_DEVICE_ID_AMPLICON_PCI215 0x000b
-#define PCI_DEVICE_ID_AMPLICON_PCIE236 0x0011
-#define PCI_DEVICE_ID_AMPLICON_PCIE215 0x0012
-#define PCI_DEVICE_ID_AMPLICON_PCIE296 0x0014
-
-/* 8255 control register bits */
-#define CR_C_LO_IO	0x01
-#define CR_B_IO		0x02
-#define CR_B_MODE	0x04
-#define CR_C_HI_IO	0x08
-#define CR_A_IO		0x10
-#define CR_A_MODE(a)	((a)<<5)
-#define CR_CW		0x80
-
-/* 200 series registers */
-#define DIO200_IO_SIZE		0x20
-#define DIO200_PCIE_IO_SIZE	0x4000
-#define DIO200_XCLK_SCE		0x18	/* Group X clock selection register */
-#define DIO200_YCLK_SCE		0x19	/* Group Y clock selection register */
-#define DIO200_ZCLK_SCE		0x1a	/* Group Z clock selection register */
-#define DIO200_XGAT_SCE		0x1b	/* Group X gate selection register */
-#define DIO200_YGAT_SCE		0x1c	/* Group Y gate selection register */
-#define DIO200_ZGAT_SCE		0x1d	/* Group Z gate selection register */
-#define DIO200_INT_SCE		0x1e	/* Interrupt enable/status register */
-/* Extra registers for new PCIe boards */
-#define DIO200_ENHANCE		0x20	/* 1 to enable enhanced features */
-#define DIO200_VERSION		0x24	/* Hardware version register */
-#define DIO200_TS_CONFIG	0x600	/* Timestamp timer config register */
-#define DIO200_TS_COUNT		0x602	/* Timestamp timer count register */
-
-/*
- * Functions for constructing value for DIO_200_?CLK_SCE and
- * DIO_200_?GAT_SCE registers:
- *
- * 'which' is: 0 for CTR-X1, CTR-Y1, CTR-Z1; 1 for CTR-X2, CTR-Y2 or CTR-Z2.
- * 'chan' is the channel: 0, 1 or 2.
- * 'source' is the signal source: 0 to 7, or 0 to 31 for "enhanced" boards.
- */
-static unsigned char clk_gat_sce(unsigned int which, unsigned int chan,
-				 unsigned int source)
-{
-	return (which << 5) | (chan << 3) |
-	       ((source & 030) << 3) | (source & 007);
-}
-
-static unsigned char clk_sce(unsigned int which, unsigned int chan,
-			     unsigned int source)
-{
-	return clk_gat_sce(which, chan, source);
-}
-
-static unsigned char gat_sce(unsigned int which, unsigned int chan,
-			     unsigned int source)
-{
-	return clk_gat_sce(which, chan, source);
-}
-
-/*
- * Periods of the internal clock sources in nanoseconds.
- */
-static const unsigned int clock_period[32] = {
-	[1] = 100,		/* 10 MHz */
-	[2] = 1000,		/* 1 MHz */
-	[3] = 10000,		/* 100 kHz */
-	[4] = 100000,		/* 10 kHz */
-	[5] = 1000000,		/* 1 kHz */
-	[11] = 50,		/* 20 MHz (enhanced boards) */
-	/* clock sources 12 and later reserved for enhanced boards */
-};
-
-/*
- * Timestamp timer configuration register (for new PCIe boards).
- */
-#define TS_CONFIG_RESET		0x100	/* Reset counter to zero. */
-#define TS_CONFIG_CLK_SRC_MASK	0x0FF	/* Clock source. */
-#define TS_CONFIG_MAX_CLK_SRC	2	/* Maximum clock source value. */
-
-/*
- * Periods of the timestamp timer clock sources in nanoseconds.
- */
-static const unsigned int ts_clock_period[TS_CONFIG_MAX_CLK_SRC + 1] = {
-	1,			/* 1 nanosecond (but with 20 ns granularity). */
-	1000,			/* 1 microsecond. */
-	1000000,		/* 1 millisecond. */
-};
-
-/*
- * Register region.
- */
-enum dio200_regtype { no_regtype = 0, io_regtype, mmio_regtype };
-struct dio200_region {
-	union {
-		unsigned long iobase;		/* I/O base address */
-		unsigned char __iomem *membase;	/* mapped MMIO base address */
-	} u;
-	enum dio200_regtype regtype;
-};
+#include "amplc_dio200.h"
 
 /*
  * Board descriptions.
  */
-
-enum dio200_bustype { isa_bustype, pci_bustype };
-
-enum dio200_model {
-	pc212e_model,
-	pc214e_model,
-	pc215e_model, pci215_model, pcie215_model,
-	pc218e_model,
-	pcie236_model,
-	pc272e_model, pci272_model,
-	pcie296_model,
-};
-
-enum dio200_layout_idx {
-#if DO_ISA
-	pc212_layout,
-	pc214_layout,
-#endif
-	pc215_layout,
-#if DO_ISA
-	pc218_layout,
-#endif
-	pc272_layout,
-#if DO_PCI
-	pcie215_layout,
-	pcie236_layout,
-	pcie296_layout,
-#endif
-};
-
-struct dio200_board {
-	const char *name;
-	unsigned short devid;
-	enum dio200_bustype bustype;
-	enum dio200_model model;
-	enum dio200_layout_idx layout;
-	unsigned char mainbar;
-	unsigned char mainshift;
-	unsigned int mainsize;
-};
-
-static const struct dio200_board dio200_boards[] = {
-#if DO_ISA
-	{
-	 .name = "pc212e",
-	 .bustype = isa_bustype,
-	 .model = pc212e_model,
-	 .layout = pc212_layout,
-	 .mainsize = DIO200_IO_SIZE,
-	 },
-	{
-	 .name = "pc214e",
-	 .bustype = isa_bustype,
-	 .model = pc214e_model,
-	 .layout = pc214_layout,
-	 .mainsize = DIO200_IO_SIZE,
-	 },
-	{
-	 .name = "pc215e",
-	 .bustype = isa_bustype,
-	 .model = pc215e_model,
-	 .layout = pc215_layout,
-	 .mainsize = DIO200_IO_SIZE,
-	 },
-	{
-	 .name = "pc218e",
-	 .bustype = isa_bustype,
-	 .model = pc218e_model,
-	 .layout = pc218_layout,
-	 .mainsize = DIO200_IO_SIZE,
-	 },
+static const struct dio200_board dio200_isa_boards[] = {
 	{
-	 .name = "pc272e",
-	 .bustype = isa_bustype,
-	 .model = pc272e_model,
-	 .layout = pc272_layout,
-	 .mainsize = DIO200_IO_SIZE,
-	 },
-#endif
-#if DO_PCI
+		.name = "pc212e",
+		.bustype = isa_bustype,
+		.mainsize = DIO200_IO_SIZE,
+		.layout = {
+			.n_subdevs = 6,
+			.sdtype = {sd_8255, sd_8254, sd_8254, sd_8254, sd_8254,
+				   sd_intr},
+			.sdinfo = {0x00, 0x08, 0x0C, 0x10, 0x14, 0x3F},
+			.has_int_sce = true,
+			.has_clk_gat_sce = true,
+		},
+	},
 	{
-	 .name = "pci215",
-	 .devid = PCI_DEVICE_ID_AMPLICON_PCI215,
-	 .bustype = pci_bustype,
-	 .model = pci215_model,
-	 .layout = pc215_layout,
-	 .mainbar = 2,
-	 .mainsize = DIO200_IO_SIZE,
-	 },
+		.name = "pc214e",
+		.bustype = isa_bustype,
+		.mainsize = DIO200_IO_SIZE,
+		.layout = {
+			.n_subdevs = 4,
+			.sdtype = {sd_8255, sd_8255, sd_8254, sd_intr},
+			.sdinfo = {0x00, 0x08, 0x10, 0x01},
+		},
+	},
 	{
-	 .name = "pci272",
-	 .devid = PCI_DEVICE_ID_AMPLICON_PCI272,
-	 .bustype = pci_bustype,
-	 .model = pci272_model,
-	 .layout = pc272_layout,
-	 .mainbar = 2,
-	 .mainsize = DIO200_IO_SIZE,
-	 },
+		.name = "pc215e",
+		.bustype = isa_bustype,
+		.mainsize = DIO200_IO_SIZE,
+		.layout = {
+			.n_subdevs = 5,
+			.sdtype = {sd_8255, sd_8255, sd_8254, sd_8254, sd_intr},
+			.sdinfo = {0x00, 0x08, 0x10, 0x14, 0x3F},
+			.has_int_sce = true,
+			.has_clk_gat_sce = true,
+		},
+	},
 	{
-	 .name = "pcie215",
-	 .devid = PCI_DEVICE_ID_AMPLICON_PCIE215,
-	 .bustype = pci_bustype,
-	 .model = pcie215_model,
-	 .layout = pcie215_layout,
-	 .mainbar = 1,
-	 .mainshift = 3,
-	 .mainsize = DIO200_PCIE_IO_SIZE,
-	 },
+		.name = "pc218e",
+		.bustype = isa_bustype,
+		.mainsize = DIO200_IO_SIZE,
+		.layout = {
+			.n_subdevs = 7,
+			.sdtype = {sd_8254, sd_8254, sd_8255, sd_8254, sd_8254,
+				   sd_intr},
+			.sdinfo = {0x00, 0x04, 0x08, 0x0C, 0x10, 0x14, 0x3F},
+			.has_int_sce = true,
+			.has_clk_gat_sce = true,
+		},
+	},
 	{
-	 .name = "pcie236",
-	 .devid = PCI_DEVICE_ID_AMPLICON_PCIE236,
-	 .bustype = pci_bustype,
-	 .model = pcie236_model,
-	 .layout = pcie236_layout,
-	 .mainbar = 1,
-	 .mainshift = 3,
-	 .mainsize = DIO200_PCIE_IO_SIZE,
-	 },
-	{
-	 .name = "pcie296",
-	 .devid = PCI_DEVICE_ID_AMPLICON_PCIE296,
-	 .bustype = pci_bustype,
-	 .model = pcie296_model,
-	 .layout = pcie296_layout,
-	 .mainbar = 1,
-	 .mainshift = 3,
-	 .mainsize = DIO200_PCIE_IO_SIZE,
-	 },
-#endif
-};
-
-/*
- * Layout descriptions - some ISA and PCI board descriptions share the same
- * layout.
- */
-
-enum dio200_sdtype { sd_none, sd_intr, sd_8255, sd_8254, sd_timer };
-
-#define DIO200_MAX_SUBDEVS	8
-#define DIO200_MAX_ISNS		6
-
-struct dio200_layout {
-	unsigned short n_subdevs;	/* number of subdevices */
-	unsigned char sdtype[DIO200_MAX_SUBDEVS];	/* enum dio200_sdtype */
-	unsigned char sdinfo[DIO200_MAX_SUBDEVS];	/* depends on sdtype */
-	char has_int_sce;	/* has interrupt enable/status register */
-	char has_clk_gat_sce;	/* has clock/gate selection registers */
-	char has_enhancements;	/* has enhanced features */
-};
-
-static const struct dio200_layout dio200_layouts[] = {
-#if DO_ISA
-	[pc212_layout] = {
-			  .n_subdevs = 6,
-			  .sdtype = {sd_8255, sd_8254, sd_8254, sd_8254,
-				     sd_8254,
-				     sd_intr},
-			  .sdinfo = {0x00, 0x08, 0x0C, 0x10, 0x14,
-				     0x3F},
-			  .has_int_sce = 1,
-			  .has_clk_gat_sce = 1,
-			  },
-	[pc214_layout] = {
-			  .n_subdevs = 4,
-			  .sdtype = {sd_8255, sd_8255, sd_8254,
-				     sd_intr},
-			  .sdinfo = {0x00, 0x08, 0x10, 0x01},
-			  .has_int_sce = 0,
-			  .has_clk_gat_sce = 0,
-			  },
-#endif
-	[pc215_layout] = {
-			  .n_subdevs = 5,
-			  .sdtype = {sd_8255, sd_8255, sd_8254,
-				     sd_8254,
-				     sd_intr},
-			  .sdinfo = {0x00, 0x08, 0x10, 0x14, 0x3F},
-			  .has_int_sce = 1,
-			  .has_clk_gat_sce = 1,
-			  },
-#if DO_ISA
-	[pc218_layout] = {
-			  .n_subdevs = 7,
-			  .sdtype = {sd_8254, sd_8254, sd_8255, sd_8254,
-				     sd_8254,
-				     sd_intr},
-			  .sdinfo = {0x00, 0x04, 0x08, 0x0C, 0x10,
-				     0x14,
-				     0x3F},
-			  .has_int_sce = 1,
-			  .has_clk_gat_sce = 1,
-			  },
-#endif
-	[pc272_layout] = {
-			  .n_subdevs = 4,
-			  .sdtype = {sd_8255, sd_8255, sd_8255,
-				     sd_intr},
-			  .sdinfo = {0x00, 0x08, 0x10, 0x3F},
-			  .has_int_sce = 1,
-			  .has_clk_gat_sce = 0,
-			  },
-#if DO_PCI
-	[pcie215_layout] = {
-			  .n_subdevs = 8,
-			  .sdtype = {sd_8255, sd_none, sd_8255, sd_none,
-				     sd_8254, sd_8254, sd_timer, sd_intr},
-			  .sdinfo = {0x00, 0x00, 0x08, 0x00,
-				     0x10, 0x14, 0x00, 0x3F},
-			  .has_int_sce = 1,
-			  .has_clk_gat_sce = 1,
-			  .has_enhancements = 1,
-			  },
-	[pcie236_layout] = {
-			  .n_subdevs = 8,
-			  .sdtype = {sd_8255, sd_none, sd_none, sd_none,
-				     sd_8254, sd_8254, sd_timer, sd_intr},
-			  .sdinfo = {0x00, 0x00, 0x00, 0x00,
-				     0x10, 0x14, 0x00, 0x3F},
-			  .has_int_sce = 1,
-			  .has_clk_gat_sce = 1,
-			  .has_enhancements = 1,
-			  },
-	[pcie296_layout] = {
-			  .n_subdevs = 8,
-			  .sdtype = {sd_8255, sd_8255, sd_8255, sd_8255,
-				     sd_8254, sd_8254, sd_timer, sd_intr},
-			  .sdinfo = {0x00, 0x04, 0x08, 0x0C,
-				     0x10, 0x14, 0x00, 0x3F},
-			  .has_int_sce = 1,
-			  .has_clk_gat_sce = 1,
-			  .has_enhancements = 1,
-			  },
-#endif
-};
-
-/* this structure is for data unique to this hardware driver.  If
-   several hardware drivers keep similar information in this structure,
-   feel free to suggest moving the variable to the struct comedi_device struct.
- */
-struct dio200_private {
-	struct dio200_region io;	/* Register region */
-	int intr_sd;
-};
-
-struct dio200_subdev_8254 {
-	unsigned int ofs;		/* Counter base offset */
-	unsigned int clk_sce_ofs;	/* CLK_SCE base address */
-	unsigned int gat_sce_ofs;	/* GAT_SCE base address */
-	int which;			/* Bit 5 of CLK_SCE or GAT_SCE */
-	unsigned int clock_src[3];	/* Current clock sources */
-	unsigned int gate_src[3];	/* Current gate sources */
-	spinlock_t spinlock;
-};
-
-struct dio200_subdev_8255 {
-	unsigned int ofs;		/* DIO base offset */
+		.name = "pc272e",
+		.bustype = isa_bustype,
+		.mainsize = DIO200_IO_SIZE,
+		.layout = {
+			.n_subdevs = 4,
+			.sdtype = {sd_8255, sd_8255, sd_8255, sd_intr},
+			.sdinfo = {0x00, 0x08, 0x10, 0x3F},
+			.has_int_sce = true,
+		},
+	},
 };
 
-struct dio200_subdev_intr {
-	unsigned int ofs;
-	spinlock_t spinlock;
-	int active;
-	unsigned int valid_isns;
-	unsigned int enabled_isns;
-	unsigned int stopcount;
-	int continuous;
-};
-
-static inline const struct dio200_layout *
-dio200_board_layout(const struct dio200_board *board)
-{
-	return &dio200_layouts[board->layout];
-}
-
-static inline const struct dio200_layout *
-dio200_dev_layout(struct comedi_device *dev)
-{
-	return dio200_board_layout(comedi_board(dev));
-}
-
-static inline bool is_pci_board(const struct dio200_board *board)
-{
-	return DO_PCI && board->bustype == pci_bustype;
-}
-
-static inline bool is_isa_board(const struct dio200_board *board)
-{
-	return DO_ISA && board->bustype == isa_bustype;
-}
-
-/*
- * Read 8-bit register.
- */
-static unsigned char dio200_read8(struct comedi_device *dev,
-				  unsigned int offset)
-{
-	const struct dio200_board *thisboard = comedi_board(dev);
-	struct dio200_private *devpriv = dev->private;
-
-	offset <<= thisboard->mainshift;
-	if (devpriv->io.regtype == io_regtype)
-		return inb(devpriv->io.u.iobase + offset);
-	else
-		return readb(devpriv->io.u.membase + offset);
-}
-
-/*
- * Write 8-bit register.
- */
-static void dio200_write8(struct comedi_device *dev, unsigned int offset,
-			  unsigned char val)
-{
-	const struct dio200_board *thisboard = comedi_board(dev);
-	struct dio200_private *devpriv = dev->private;
-
-	offset <<= thisboard->mainshift;
-	if (devpriv->io.regtype == io_regtype)
-		outb(val, devpriv->io.u.iobase + offset);
-	else
-		writeb(val, devpriv->io.u.membase + offset);
-}
-
-/*
- * Read 32-bit register.
- */
-static unsigned int dio200_read32(struct comedi_device *dev,
-				  unsigned int offset)
-{
-	const struct dio200_board *thisboard = comedi_board(dev);
-	struct dio200_private *devpriv = dev->private;
-
-	offset <<= thisboard->mainshift;
-	if (devpriv->io.regtype == io_regtype)
-		return inl(devpriv->io.u.iobase + offset);
-	else
-		return readl(devpriv->io.u.membase + offset);
-}
-
-/*
- * Write 32-bit register.
- */
-static void dio200_write32(struct comedi_device *dev, unsigned int offset,
-			   unsigned int val)
-{
-	const struct dio200_board *thisboard = comedi_board(dev);
-	struct dio200_private *devpriv = dev->private;
-
-	offset <<= thisboard->mainshift;
-	if (devpriv->io.regtype == io_regtype)
-		outl(val, devpriv->io.u.iobase + offset);
-	else
-		writel(val, devpriv->io.u.membase + offset);
-}
-
-/*
- * This function looks for a board matching the supplied PCI device.
- */
-static const struct dio200_board *
-dio200_find_pci_board(struct pci_dev *pci_dev)
-{
-	unsigned int i;
-
-	for (i = 0; i < ARRAY_SIZE(dio200_boards); i++)
-		if (is_pci_board(&dio200_boards[i]) &&
-		    pci_dev->device == dio200_boards[i].devid)
-			return &dio200_boards[i];
-	return NULL;
-}
-
-/*
- * This function checks and requests an I/O region, reporting an error
- * if there is a conflict.
- */
-static int
-dio200_request_region(struct comedi_device *dev,
-		      unsigned long from, unsigned long extent)
-{
-	if (!from || !request_region(from, extent, DIO200_DRIVER_NAME)) {
-		dev_err(dev->class_dev, "I/O port conflict (%#lx,%lu)!\n",
-			from, extent);
-		return -EIO;
-	}
-	return 0;
-}
-
-/*
- * 'insn_bits' function for an 'INTERRUPT' subdevice.
- */
-static int
-dio200_subdev_intr_insn_bits(struct comedi_device *dev,
-			     struct comedi_subdevice *s,
-			     struct comedi_insn *insn, unsigned int *data)
-{
-	const struct dio200_layout *layout = dio200_dev_layout(dev);
-	struct dio200_subdev_intr *subpriv = s->private;
-
-	if (layout->has_int_sce) {
-		/* Just read the interrupt status register.  */
-		data[1] = dio200_read8(dev, subpriv->ofs) & subpriv->valid_isns;
-	} else {
-		/* No interrupt status register. */
-		data[0] = 0;
-	}
-
-	return insn->n;
-}
-
-/*
- * Called to stop acquisition for an 'INTERRUPT' subdevice.
- */
-static void dio200_stop_intr(struct comedi_device *dev,
-			     struct comedi_subdevice *s)
-{
-	const struct dio200_layout *layout = dio200_dev_layout(dev);
-	struct dio200_subdev_intr *subpriv = s->private;
-
-	subpriv->active = 0;
-	subpriv->enabled_isns = 0;
-	if (layout->has_int_sce)
-		dio200_write8(dev, subpriv->ofs, 0);
-}
-
-/*
- * Called to start acquisition for an 'INTERRUPT' subdevice.
- */
-static int dio200_start_intr(struct comedi_device *dev,
-			     struct comedi_subdevice *s)
-{
-	unsigned int n;
-	unsigned isn_bits;
-	const struct dio200_layout *layout = dio200_dev_layout(dev);
-	struct dio200_subdev_intr *subpriv = s->private;
-	struct comedi_cmd *cmd = &s->async->cmd;
-	int retval = 0;
-
-	if (!subpriv->continuous && subpriv->stopcount == 0) {
-		/* An empty acquisition! */
-		s->async->events |= COMEDI_CB_EOA;
-		subpriv->active = 0;
-		retval = 1;
-	} else {
-		/* Determine interrupt sources to enable. */
-		isn_bits = 0;
-		if (cmd->chanlist) {
-			for (n = 0; n < cmd->chanlist_len; n++)
-				isn_bits |= (1U << CR_CHAN(cmd->chanlist[n]));
-		}
-		isn_bits &= subpriv->valid_isns;
-		/* Enable interrupt sources. */
-		subpriv->enabled_isns = isn_bits;
-		if (layout->has_int_sce)
-			dio200_write8(dev, subpriv->ofs, isn_bits);
-	}
-
-	return retval;
-}
-
-/*
- * Internal trigger function to start acquisition for an 'INTERRUPT' subdevice.
- */
-static int
-dio200_inttrig_start_intr(struct comedi_device *dev, struct comedi_subdevice *s,
-			  unsigned int trignum)
-{
-	struct dio200_subdev_intr *subpriv;
-	unsigned long flags;
-	int event = 0;
-
-	if (trignum != 0)
-		return -EINVAL;
-
-	subpriv = s->private;
-
-	spin_lock_irqsave(&subpriv->spinlock, flags);
-	s->async->inttrig = NULL;
-	if (subpriv->active)
-		event = dio200_start_intr(dev, s);
-
-	spin_unlock_irqrestore(&subpriv->spinlock, flags);
-
-	if (event)
-		comedi_event(dev, s);
-
-	return 1;
-}
-
-/*
- * This is called from the interrupt service routine to handle a read
- * scan on an 'INTERRUPT' subdevice.
- */
-static int dio200_handle_read_intr(struct comedi_device *dev,
-				   struct comedi_subdevice *s)
-{
-	const struct dio200_layout *layout = dio200_dev_layout(dev);
-	struct dio200_subdev_intr *subpriv = s->private;
-	unsigned triggered;
-	unsigned intstat;
-	unsigned cur_enabled;
-	unsigned int oldevents;
-	unsigned long flags;
-
-	triggered = 0;
-
-	spin_lock_irqsave(&subpriv->spinlock, flags);
-	oldevents = s->async->events;
-	if (layout->has_int_sce) {
-		/*
-		 * Collect interrupt sources that have triggered and disable
-		 * them temporarily.  Loop around until no extra interrupt
-		 * sources have triggered, at which point, the valid part of
-		 * the interrupt status register will read zero, clearing the
-		 * cause of the interrupt.
-		 *
-		 * Mask off interrupt sources already seen to avoid infinite
-		 * loop in case of misconfiguration.
-		 */
-		cur_enabled = subpriv->enabled_isns;
-		while ((intstat = (dio200_read8(dev, subpriv->ofs) &
-				   subpriv->valid_isns & ~triggered)) != 0) {
-			triggered |= intstat;
-			cur_enabled &= ~triggered;
-			dio200_write8(dev, subpriv->ofs, cur_enabled);
-		}
-	} else {
-		/*
-		 * No interrupt status register.  Assume the single interrupt
-		 * source has triggered.
-		 */
-		triggered = subpriv->enabled_isns;
-	}
-
-	if (triggered) {
-		/*
-		 * Some interrupt sources have triggered and have been
-		 * temporarily disabled to clear the cause of the interrupt.
-		 *
-		 * Reenable them NOW to minimize the time they are disabled.
-		 */
-		cur_enabled = subpriv->enabled_isns;
-		if (layout->has_int_sce)
-			dio200_write8(dev, subpriv->ofs, cur_enabled);
-
-		if (subpriv->active) {
-			/*
-			 * The command is still active.
-			 *
-			 * Ignore interrupt sources that the command isn't
-			 * interested in (just in case there's a race
-			 * condition).
-			 */
-			if (triggered & subpriv->enabled_isns) {
-				/* Collect scan data. */
-				short val;
-				unsigned int n, ch, len;
-
-				val = 0;
-				len = s->async->cmd.chanlist_len;
-				for (n = 0; n < len; n++) {
-					ch = CR_CHAN(s->async->cmd.chanlist[n]);
-					if (triggered & (1U << ch))
-						val |= (1U << n);
-				}
-				/* Write the scan to the buffer. */
-				if (comedi_buf_put(s->async, val)) {
-					s->async->events |= (COMEDI_CB_BLOCK |
-							     COMEDI_CB_EOS);
-				} else {
-					/* Error!  Stop acquisition.  */
-					dio200_stop_intr(dev, s);
-					s->async->events |= COMEDI_CB_ERROR
-					    | COMEDI_CB_OVERFLOW;
-					comedi_error(dev, "buffer overflow");
-				}
-
-				/* Check for end of acquisition. */
-				if (!subpriv->continuous) {
-					/* stop_src == TRIG_COUNT */
-					if (subpriv->stopcount > 0) {
-						subpriv->stopcount--;
-						if (subpriv->stopcount == 0) {
-							s->async->events |=
-							    COMEDI_CB_EOA;
-							dio200_stop_intr(dev,
-									 s);
-						}
-					}
-				}
-			}
-		}
-	}
-	spin_unlock_irqrestore(&subpriv->spinlock, flags);
-
-	if (oldevents != s->async->events)
-		comedi_event(dev, s);
-
-	return (triggered != 0);
-}
-
-/*
- * 'cancel' function for an 'INTERRUPT' subdevice.
- */
-static int dio200_subdev_intr_cancel(struct comedi_device *dev,
-				     struct comedi_subdevice *s)
-{
-	struct dio200_subdev_intr *subpriv = s->private;
-	unsigned long flags;
-
-	spin_lock_irqsave(&subpriv->spinlock, flags);
-	if (subpriv->active)
-		dio200_stop_intr(dev, s);
-
-	spin_unlock_irqrestore(&subpriv->spinlock, flags);
-
-	return 0;
-}
-
-/*
- * 'do_cmdtest' function for an 'INTERRUPT' subdevice.
- */
-static int
-dio200_subdev_intr_cmdtest(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_cmd *cmd)
-{
-	int err = 0;
-
-	/* Step 1 : check if triggers are trivially valid */
-
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_INT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
-
-	if (err)
-		return 1;
-
-	/* Step 2a : make sure trigger sources are unique */
-
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
-
-	/* Step 2b : and mutually compatible */
-
-	if (err)
-		return 2;
-
-	/* Step 3: check if arguments are trivially valid */
-
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
-
-	switch (cmd->stop_src) {
-	case TRIG_COUNT:
-		/* any count allowed */
-		break;
-	case TRIG_NONE:
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
-		break;
-	default:
-		break;
-	}
-
-	if (err)
-		return 3;
-
-	/* step 4: fix up any arguments */
-
-	/* if (err) return 4; */
-
-	return 0;
-}
-
-/*
- * 'do_cmd' function for an 'INTERRUPT' subdevice.
- */
-static int dio200_subdev_intr_cmd(struct comedi_device *dev,
-				  struct comedi_subdevice *s)
-{
-	struct comedi_cmd *cmd = &s->async->cmd;
-	struct dio200_subdev_intr *subpriv = s->private;
-	unsigned long flags;
-	int event = 0;
-
-	spin_lock_irqsave(&subpriv->spinlock, flags);
-	subpriv->active = 1;
-
-	/* Set up end of acquisition. */
-	switch (cmd->stop_src) {
-	case TRIG_COUNT:
-		subpriv->continuous = 0;
-		subpriv->stopcount = cmd->stop_arg;
-		break;
-	default:
-		/* TRIG_NONE */
-		subpriv->continuous = 1;
-		subpriv->stopcount = 0;
-		break;
-	}
-
-	/* Set up start of acquisition. */
-	switch (cmd->start_src) {
-	case TRIG_INT:
-		s->async->inttrig = dio200_inttrig_start_intr;
-		break;
-	default:
-		/* TRIG_NOW */
-		event = dio200_start_intr(dev, s);
-		break;
-	}
-	spin_unlock_irqrestore(&subpriv->spinlock, flags);
-
-	if (event)
-		comedi_event(dev, s);
-
-	return 0;
-}
-
-/*
- * This function initializes an 'INTERRUPT' subdevice.
- */
-static int
-dio200_subdev_intr_init(struct comedi_device *dev, struct comedi_subdevice *s,
-			unsigned int offset, unsigned valid_isns)
-{
-	const struct dio200_layout *layout = dio200_dev_layout(dev);
-	struct dio200_subdev_intr *subpriv;
-
-	subpriv = kzalloc(sizeof(*subpriv), GFP_KERNEL);
-	if (!subpriv)
-		return -ENOMEM;
-
-	subpriv->ofs = offset;
-	subpriv->valid_isns = valid_isns;
-	spin_lock_init(&subpriv->spinlock);
-
-	if (layout->has_int_sce)
-		/* Disable interrupt sources. */
-		dio200_write8(dev, subpriv->ofs, 0);
-
-	s->private = subpriv;
-	s->type = COMEDI_SUBD_DI;
-	s->subdev_flags = SDF_READABLE | SDF_CMD_READ;
-	if (layout->has_int_sce) {
-		s->n_chan = DIO200_MAX_ISNS;
-		s->len_chanlist = DIO200_MAX_ISNS;
-	} else {
-		/* No interrupt source register.  Support single channel. */
-		s->n_chan = 1;
-		s->len_chanlist = 1;
-	}
-	s->range_table = &range_digital;
-	s->maxdata = 1;
-	s->insn_bits = dio200_subdev_intr_insn_bits;
-	s->do_cmdtest = dio200_subdev_intr_cmdtest;
-	s->do_cmd = dio200_subdev_intr_cmd;
-	s->cancel = dio200_subdev_intr_cancel;
-
-	return 0;
-}
-
-/*
- * This function cleans up an 'INTERRUPT' subdevice.
- */
-static void
-dio200_subdev_intr_cleanup(struct comedi_device *dev,
-			   struct comedi_subdevice *s)
-{
-	struct dio200_subdev_intr *subpriv = s->private;
-	kfree(subpriv);
-}
-
-/*
- * Interrupt service routine.
- */
-static irqreturn_t dio200_interrupt(int irq, void *d)
-{
-	struct comedi_device *dev = d;
-	struct dio200_private *devpriv = dev->private;
-	struct comedi_subdevice *s;
-	int handled;
-
-	if (!dev->attached)
-		return IRQ_NONE;
-
-	if (devpriv->intr_sd >= 0) {
-		s = &dev->subdevices[devpriv->intr_sd];
-		handled = dio200_handle_read_intr(dev, s);
-	} else {
-		handled = 0;
-	}
-
-	return IRQ_RETVAL(handled);
-}
-
-/*
- * Read an '8254' counter subdevice channel.
- */
-static unsigned int
-dio200_subdev_8254_read_chan(struct comedi_device *dev,
-			     struct comedi_subdevice *s, unsigned int chan)
-{
-	struct dio200_subdev_8254 *subpriv = s->private;
-	unsigned int val;
-
-	/* latch counter */
-	val = chan << 6;
-	dio200_write8(dev, subpriv->ofs + i8254_control_reg, val);
-	/* read lsb, msb */
-	val = dio200_read8(dev, subpriv->ofs + chan);
-	val += dio200_read8(dev, subpriv->ofs + chan) << 8;
-	return val;
-}
-
-/*
- * Write an '8254' subdevice channel.
- */
-static void
-dio200_subdev_8254_write_chan(struct comedi_device *dev,
-			      struct comedi_subdevice *s, unsigned int chan,
-			      unsigned int count)
-{
-	struct dio200_subdev_8254 *subpriv = s->private;
-
-	/* write lsb, msb */
-	dio200_write8(dev, subpriv->ofs + chan, count & 0xff);
-	dio200_write8(dev, subpriv->ofs + chan, (count >> 8) & 0xff);
-}
-
-/*
- * Set mode of an '8254' subdevice channel.
- */
-static void
-dio200_subdev_8254_set_mode(struct comedi_device *dev,
-			    struct comedi_subdevice *s, unsigned int chan,
-			    unsigned int mode)
-{
-	struct dio200_subdev_8254 *subpriv = s->private;
-	unsigned int byte;
-
-	byte = chan << 6;
-	byte |= 0x30;		/* access order: lsb, msb */
-	byte |= (mode & 0xf);	/* counter mode and BCD|binary */
-	dio200_write8(dev, subpriv->ofs + i8254_control_reg, byte);
-}
-
-/*
- * Read status byte of an '8254' counter subdevice channel.
- */
-static unsigned int
-dio200_subdev_8254_status(struct comedi_device *dev,
-			  struct comedi_subdevice *s, unsigned int chan)
-{
-	struct dio200_subdev_8254 *subpriv = s->private;
-
-	/* latch status */
-	dio200_write8(dev, subpriv->ofs + i8254_control_reg,
-		      0xe0 | (2 << chan));
-	/* read status */
-	return dio200_read8(dev, subpriv->ofs + chan);
-}
-
-/*
- * Handle 'insn_read' for an '8254' counter subdevice.
- */
-static int
-dio200_subdev_8254_read(struct comedi_device *dev, struct comedi_subdevice *s,
-			struct comedi_insn *insn, unsigned int *data)
-{
-	struct dio200_subdev_8254 *subpriv = s->private;
-	int chan = CR_CHAN(insn->chanspec);
-	unsigned int n;
-	unsigned long flags;
-
-	for (n = 0; n < insn->n; n++) {
-		spin_lock_irqsave(&subpriv->spinlock, flags);
-		data[n] = dio200_subdev_8254_read_chan(dev, s, chan);
-		spin_unlock_irqrestore(&subpriv->spinlock, flags);
-	}
-	return insn->n;
-}
-
-/*
- * Handle 'insn_write' for an '8254' counter subdevice.
- */
-static int
-dio200_subdev_8254_write(struct comedi_device *dev, struct comedi_subdevice *s,
-			 struct comedi_insn *insn, unsigned int *data)
-{
-	struct dio200_subdev_8254 *subpriv = s->private;
-	int chan = CR_CHAN(insn->chanspec);
-	unsigned int n;
-	unsigned long flags;
-
-	for (n = 0; n < insn->n; n++) {
-		spin_lock_irqsave(&subpriv->spinlock, flags);
-		dio200_subdev_8254_write_chan(dev, s, chan, data[n]);
-		spin_unlock_irqrestore(&subpriv->spinlock, flags);
-	}
-	return insn->n;
-}
-
-/*
- * Set gate source for an '8254' counter subdevice channel.
- */
-static int
-dio200_subdev_8254_set_gate_src(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				unsigned int counter_number,
-				unsigned int gate_src)
-{
-	const struct dio200_layout *layout = dio200_dev_layout(dev);
-	struct dio200_subdev_8254 *subpriv = s->private;
-	unsigned char byte;
-
-	if (!layout->has_clk_gat_sce)
-		return -1;
-	if (counter_number > 2)
-		return -1;
-	if (gate_src > (layout->has_enhancements ? 31 : 7))
-		return -1;
-
-	subpriv->gate_src[counter_number] = gate_src;
-	byte = gat_sce(subpriv->which, counter_number, gate_src);
-	dio200_write8(dev, subpriv->gat_sce_ofs, byte);
-
-	return 0;
-}
-
-/*
- * Get gate source for an '8254' counter subdevice channel.
- */
-static int
-dio200_subdev_8254_get_gate_src(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				unsigned int counter_number)
-{
-	const struct dio200_layout *layout = dio200_dev_layout(dev);
-	struct dio200_subdev_8254 *subpriv = s->private;
-
-	if (!layout->has_clk_gat_sce)
-		return -1;
-	if (counter_number > 2)
-		return -1;
-
-	return subpriv->gate_src[counter_number];
-}
-
-/*
- * Set clock source for an '8254' counter subdevice channel.
- */
-static int
-dio200_subdev_8254_set_clock_src(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 unsigned int counter_number,
-				 unsigned int clock_src)
-{
-	const struct dio200_layout *layout = dio200_dev_layout(dev);
-	struct dio200_subdev_8254 *subpriv = s->private;
-	unsigned char byte;
-
-	if (!layout->has_clk_gat_sce)
-		return -1;
-	if (counter_number > 2)
-		return -1;
-	if (clock_src > (layout->has_enhancements ? 31 : 7))
-		return -1;
-
-	subpriv->clock_src[counter_number] = clock_src;
-	byte = clk_sce(subpriv->which, counter_number, clock_src);
-	dio200_write8(dev, subpriv->clk_sce_ofs, byte);
-
-	return 0;
-}
-
-/*
- * Get clock source for an '8254' counter subdevice channel.
- */
-static int
-dio200_subdev_8254_get_clock_src(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 unsigned int counter_number,
-				 unsigned int *period_ns)
-{
-	const struct dio200_layout *layout = dio200_dev_layout(dev);
-	struct dio200_subdev_8254 *subpriv = s->private;
-	unsigned clock_src;
-
-	if (!layout->has_clk_gat_sce)
-		return -1;
-	if (counter_number > 2)
-		return -1;
-
-	clock_src = subpriv->clock_src[counter_number];
-	*period_ns = clock_period[clock_src];
-	return clock_src;
-}
-
-/*
- * Handle 'insn_config' for an '8254' counter subdevice.
- */
-static int
-dio200_subdev_8254_config(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data)
-{
-	struct dio200_subdev_8254 *subpriv = s->private;
-	int ret = 0;
-	int chan = CR_CHAN(insn->chanspec);
-	unsigned long flags;
-
-	spin_lock_irqsave(&subpriv->spinlock, flags);
-	switch (data[0]) {
-	case INSN_CONFIG_SET_COUNTER_MODE:
-		if (data[1] > (I8254_MODE5 | I8254_BINARY))
-			ret = -EINVAL;
-		else
-			dio200_subdev_8254_set_mode(dev, s, chan, data[1]);
-		break;
-	case INSN_CONFIG_8254_READ_STATUS:
-		data[1] = dio200_subdev_8254_status(dev, s, chan);
-		break;
-	case INSN_CONFIG_SET_GATE_SRC:
-		ret = dio200_subdev_8254_set_gate_src(dev, s, chan, data[2]);
-		if (ret < 0)
-			ret = -EINVAL;
-		break;
-	case INSN_CONFIG_GET_GATE_SRC:
-		ret = dio200_subdev_8254_get_gate_src(dev, s, chan);
-		if (ret < 0) {
-			ret = -EINVAL;
-			break;
-		}
-		data[2] = ret;
-		break;
-	case INSN_CONFIG_SET_CLOCK_SRC:
-		ret = dio200_subdev_8254_set_clock_src(dev, s, chan, data[1]);
-		if (ret < 0)
-			ret = -EINVAL;
-		break;
-	case INSN_CONFIG_GET_CLOCK_SRC:
-		ret = dio200_subdev_8254_get_clock_src(dev, s, chan, &data[2]);
-		if (ret < 0) {
-			ret = -EINVAL;
-			break;
-		}
-		data[1] = ret;
-		break;
-	default:
-		ret = -EINVAL;
-		break;
-	}
-	spin_unlock_irqrestore(&subpriv->spinlock, flags);
-	return ret < 0 ? ret : insn->n;
-}
-
-/*
- * This function initializes an '8254' counter subdevice.
- */
-static int
-dio200_subdev_8254_init(struct comedi_device *dev, struct comedi_subdevice *s,
-			unsigned int offset)
-{
-	const struct dio200_layout *layout = dio200_dev_layout(dev);
-	struct dio200_subdev_8254 *subpriv;
-	unsigned int chan;
-
-	subpriv = kzalloc(sizeof(*subpriv), GFP_KERNEL);
-	if (!subpriv)
-		return -ENOMEM;
-
-	s->private = subpriv;
-	s->type = COMEDI_SUBD_COUNTER;
-	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
-	s->n_chan = 3;
-	s->maxdata = 0xFFFF;
-	s->insn_read = dio200_subdev_8254_read;
-	s->insn_write = dio200_subdev_8254_write;
-	s->insn_config = dio200_subdev_8254_config;
-
-	spin_lock_init(&subpriv->spinlock);
-	subpriv->ofs = offset;
-	if (layout->has_clk_gat_sce) {
-		/* Derive CLK_SCE and GAT_SCE register offsets from
-		 * 8254 offset. */
-		subpriv->clk_sce_ofs = DIO200_XCLK_SCE + (offset >> 3);
-		subpriv->gat_sce_ofs = DIO200_XGAT_SCE + (offset >> 3);
-		subpriv->which = (offset >> 2) & 1;
-	}
-
-	/* Initialize channels. */
-	for (chan = 0; chan < 3; chan++) {
-		dio200_subdev_8254_set_mode(dev, s, chan,
-					    I8254_MODE0 | I8254_BINARY);
-		if (layout->has_clk_gat_sce) {
-			/* Gate source 0 is VCC (logic 1). */
-			dio200_subdev_8254_set_gate_src(dev, s, chan, 0);
-			/* Clock source 0 is the dedicated clock input. */
-			dio200_subdev_8254_set_clock_src(dev, s, chan, 0);
-		}
-	}
-
-	return 0;
-}
-
-/*
- * This function cleans up an '8254' counter subdevice.
- */
-static void
-dio200_subdev_8254_cleanup(struct comedi_device *dev,
-			   struct comedi_subdevice *s)
-{
-	struct dio200_subdev_intr *subpriv = s->private;
-	kfree(subpriv);
-}
-
-/*
- * This function sets I/O directions for an '8255' DIO subdevice.
- */
-static void dio200_subdev_8255_set_dir(struct comedi_device *dev,
-				       struct comedi_subdevice *s)
-{
-	struct dio200_subdev_8255 *subpriv = s->private;
-	int config;
-
-	config = CR_CW;
-	/* 1 in io_bits indicates output, 1 in config indicates input */
-	if (!(s->io_bits & 0x0000ff))
-		config |= CR_A_IO;
-	if (!(s->io_bits & 0x00ff00))
-		config |= CR_B_IO;
-	if (!(s->io_bits & 0x0f0000))
-		config |= CR_C_LO_IO;
-	if (!(s->io_bits & 0xf00000))
-		config |= CR_C_HI_IO;
-	dio200_write8(dev, subpriv->ofs + 3, config);
-}
-
-/*
- * Handle 'insn_bits' for an '8255' DIO subdevice.
- */
-static int dio200_subdev_8255_bits(struct comedi_device *dev,
-				   struct comedi_subdevice *s,
-				   struct comedi_insn *insn, unsigned int *data)
-{
-	struct dio200_subdev_8255 *subpriv = s->private;
-
-	if (data[0]) {
-		s->state &= ~data[0];
-		s->state |= (data[0] & data[1]);
-		if (data[0] & 0xff)
-			dio200_write8(dev, subpriv->ofs, s->state & 0xff);
-		if (data[0] & 0xff00)
-			dio200_write8(dev, subpriv->ofs + 1,
-				      (s->state >> 8) & 0xff);
-		if (data[0] & 0xff0000)
-			dio200_write8(dev, subpriv->ofs + 2,
-				      (s->state >> 16) & 0xff);
-	}
-	data[1] = dio200_read8(dev, subpriv->ofs);
-	data[1] |= dio200_read8(dev, subpriv->ofs + 1) << 8;
-	data[1] |= dio200_read8(dev, subpriv->ofs + 2) << 16;
-	return 2;
-}
-
-/*
- * Handle 'insn_config' for an '8255' DIO subdevice.
- */
-static int dio200_subdev_8255_config(struct comedi_device *dev,
-				     struct comedi_subdevice *s,
-				     struct comedi_insn *insn,
-				     unsigned int *data)
-{
-	unsigned int mask;
-	unsigned int bits;
-
-	mask = 1 << CR_CHAN(insn->chanspec);
-	if (mask & 0x0000ff)
-		bits = 0x0000ff;
-	else if (mask & 0x00ff00)
-		bits = 0x00ff00;
-	else if (mask & 0x0f0000)
-		bits = 0x0f0000;
-	else
-		bits = 0xf00000;
-	switch (data[0]) {
-	case INSN_CONFIG_DIO_INPUT:
-		s->io_bits &= ~bits;
-		break;
-	case INSN_CONFIG_DIO_OUTPUT:
-		s->io_bits |= bits;
-		break;
-	case INSN_CONFIG_DIO_QUERY:
-		data[1] = (s->io_bits & bits) ? COMEDI_OUTPUT : COMEDI_INPUT;
-		return insn->n;
-		break;
-	default:
-		return -EINVAL;
-	}
-	dio200_subdev_8255_set_dir(dev, s);
-	return 1;
-}
-
-/*
- * This function initializes an '8255' DIO subdevice.
- *
- * offset is the offset to the 8255 chip.
- */
-static int dio200_subdev_8255_init(struct comedi_device *dev,
-				   struct comedi_subdevice *s,
-				   unsigned int offset)
-{
-	struct dio200_subdev_8255 *subpriv;
-
-	subpriv = kzalloc(sizeof(*subpriv), GFP_KERNEL);
-	if (!subpriv)
-		return -ENOMEM;
-	subpriv->ofs = offset;
-	s->private = subpriv;
-	s->type = COMEDI_SUBD_DIO;
-	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-	s->n_chan = 24;
-	s->range_table = &range_digital;
-	s->maxdata = 1;
-	s->insn_bits = dio200_subdev_8255_bits;
-	s->insn_config = dio200_subdev_8255_config;
-	s->state = 0;
-	s->io_bits = 0;
-	dio200_subdev_8255_set_dir(dev, s);
-	return 0;
-}
-
-/*
- * This function cleans up an '8255' DIO subdevice.
- */
-static void dio200_subdev_8255_cleanup(struct comedi_device *dev,
-				       struct comedi_subdevice *s)
-{
-	struct dio200_subdev_8255 *subpriv = s->private;
-
-	kfree(subpriv);
-}
-
-/*
- * Handle 'insn_read' for a timer subdevice.
- */
-static int dio200_subdev_timer_read(struct comedi_device *dev,
-				    struct comedi_subdevice *s,
-				    struct comedi_insn *insn,
-				    unsigned int *data)
-{
-	unsigned int n;
-
-	for (n = 0; n < insn->n; n++)
-		data[n] = dio200_read32(dev, DIO200_TS_COUNT);
-	return n;
-}
-
-/*
- * Reset timer subdevice.
- */
-static void dio200_subdev_timer_reset(struct comedi_device *dev,
-				      struct comedi_subdevice *s)
-{
-	unsigned int clock;
-
-	clock = dio200_read32(dev, DIO200_TS_CONFIG) & TS_CONFIG_CLK_SRC_MASK;
-	dio200_write32(dev, DIO200_TS_CONFIG, clock | TS_CONFIG_RESET);
-	dio200_write32(dev, DIO200_TS_CONFIG, clock);
-}
-
-/*
- * Get timer subdevice clock source and period.
- */
-static void dio200_subdev_timer_get_clock_src(struct comedi_device *dev,
-					      struct comedi_subdevice *s,
-					      unsigned int *src,
-					      unsigned int *period)
-{
-	unsigned int clk;
-
-	clk = dio200_read32(dev, DIO200_TS_CONFIG) & TS_CONFIG_CLK_SRC_MASK;
-	*src = clk;
-	*period = (clk < ARRAY_SIZE(ts_clock_period)) ?
-		  ts_clock_period[clk] : 0;
-}
-
-/*
- * Set timer subdevice clock source.
- */
-static int dio200_subdev_timer_set_clock_src(struct comedi_device *dev,
-					     struct comedi_subdevice *s,
-					     unsigned int src)
-{
-	if (src > TS_CONFIG_MAX_CLK_SRC)
-		return -EINVAL;
-	dio200_write32(dev, DIO200_TS_CONFIG, src);
-	return 0;
-}
-
-/*
- * Handle 'insn_config' for a timer subdevice.
- */
-static int dio200_subdev_timer_config(struct comedi_device *dev,
-				      struct comedi_subdevice *s,
-				      struct comedi_insn *insn,
-				      unsigned int *data)
-{
-	int ret = 0;
-
-	switch (data[0]) {
-	case INSN_CONFIG_RESET:
-		dio200_subdev_timer_reset(dev, s);
-		break;
-	case INSN_CONFIG_SET_CLOCK_SRC:
-		ret = dio200_subdev_timer_set_clock_src(dev, s, data[1]);
-		if (ret < 0)
-			ret = -EINVAL;
-		break;
-	case INSN_CONFIG_GET_CLOCK_SRC:
-		dio200_subdev_timer_get_clock_src(dev, s, &data[1], &data[2]);
-		break;
-	default:
-		ret = -EINVAL;
-		break;
-	}
-	return ret < 0 ? ret : insn->n;
-}
-
-/*
- * This function initializes a timer subdevice.
- *
- * Uses the timestamp timer registers.  There is only one timestamp timer.
- */
-static int dio200_subdev_timer_init(struct comedi_device *dev,
-				    struct comedi_subdevice *s)
-{
-	s->type = COMEDI_SUBD_TIMER;
-	s->subdev_flags = SDF_READABLE | SDF_LSAMPL;
-	s->n_chan = 1;
-	s->maxdata = 0xFFFFFFFF;
-	s->insn_read = dio200_subdev_timer_read;
-	s->insn_config = dio200_subdev_timer_config;
-	return 0;
-}
-
-/*
- * This function cleans up a timer subdevice.
- */
-static void dio200_subdev_timer_cleanup(struct comedi_device *dev,
-					struct comedi_subdevice *s)
-{
-	/* Nothing to do. */
-}
-
-/*
- * This function does some special set-up for the PCIe boards
- * PCIe215, PCIe236, PCIe296.
- */
-static int dio200_pcie_board_setup(struct comedi_device *dev)
-{
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	void __iomem *brbase;
-	resource_size_t brlen;
-
-	/*
-	 * The board uses Altera Cyclone IV with PCI-Express hard IP.
-	 * The FPGA configuration has the PCI-Express Avalon-MM Bridge
-	 * Control registers in PCI BAR 0, offset 0, and the length of
-	 * these registers is 0x4000.
-	 *
-	 * We need to write 0x80 to the "Avalon-MM to PCI-Express Interrupt
-	 * Enable" register at offset 0x50 to allow generation of PCIe
-	 * interrupts when RXmlrq_i is asserted in the SOPC Builder system.
-	 */
-	brlen = pci_resource_len(pcidev, 0);
-	if (brlen < 0x4000 ||
-			!(pci_resource_flags(pcidev, 0) & IORESOURCE_MEM)) {
-		dev_err(dev->class_dev, "error! bad PCI region!\n");
-		return -EINVAL;
-	}
-	brbase = ioremap_nocache(pci_resource_start(pcidev, 0), brlen);
-	if (!brbase) {
-		dev_err(dev->class_dev, "error! failed to map registers!\n");
-		return -ENOMEM;
-	}
-	writel(0x80, brbase + 0x50);
-	iounmap(brbase);
-	/* Enable "enhanced" features of board. */
-	dio200_write8(dev, DIO200_ENHANCE, 1);
-	return 0;
-}
-
-static void dio200_report_attach(struct comedi_device *dev, unsigned int irq)
-{
-	const struct dio200_board *thisboard = comedi_board(dev);
-	struct dio200_private *devpriv = dev->private;
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	char tmpbuf[60];
-	int tmplen;
-
-	if (is_isa_board(thisboard))
-		tmplen = scnprintf(tmpbuf, sizeof(tmpbuf),
-				   "(base %#lx) ", devpriv->io.u.iobase);
-	else if (is_pci_board(thisboard))
-		tmplen = scnprintf(tmpbuf, sizeof(tmpbuf),
-				   "(pci %s) ", pci_name(pcidev));
-	else
-		tmplen = 0;
-	if (irq)
-		tmplen += scnprintf(&tmpbuf[tmplen], sizeof(tmpbuf) - tmplen,
-				    "(irq %u%s) ", irq,
-				    (dev->irq ? "" : " UNAVAILABLE"));
-	else
-		tmplen += scnprintf(&tmpbuf[tmplen], sizeof(tmpbuf) - tmplen,
-				    "(no irq) ");
-	dev_info(dev->class_dev, "%s %sattached\n", dev->board_name, tmpbuf);
-}
-
-static int dio200_common_attach(struct comedi_device *dev, unsigned int irq,
-				unsigned long req_irq_flags)
-{
-	const struct dio200_board *thisboard = comedi_board(dev);
-	struct dio200_private *devpriv = dev->private;
-	const struct dio200_layout *layout = dio200_board_layout(thisboard);
-	struct comedi_subdevice *s;
-	int sdx;
-	unsigned int n;
-	int ret;
-
-	devpriv->intr_sd = -1;
-	dev->board_name = thisboard->name;
-
-	ret = comedi_alloc_subdevices(dev, layout->n_subdevs);
-	if (ret)
-		return ret;
-
-	for (n = 0; n < dev->n_subdevices; n++) {
-		s = &dev->subdevices[n];
-		switch (layout->sdtype[n]) {
-		case sd_8254:
-			/* counter subdevice (8254) */
-			ret = dio200_subdev_8254_init(dev, s,
-						      layout->sdinfo[n]);
-			if (ret < 0)
-				return ret;
-			break;
-		case sd_8255:
-			/* digital i/o subdevice (8255) */
-			ret = dio200_subdev_8255_init(dev, s,
-						      layout->sdinfo[n]);
-			if (ret < 0)
-				return ret;
-			break;
-		case sd_intr:
-			/* 'INTERRUPT' subdevice */
-			if (irq) {
-				ret = dio200_subdev_intr_init(dev, s,
-							      DIO200_INT_SCE,
-							      layout->sdinfo[n]
-							     );
-				if (ret < 0)
-					return ret;
-				devpriv->intr_sd = n;
-			} else {
-				s->type = COMEDI_SUBD_UNUSED;
-			}
-			break;
-		case sd_timer:
-			/* Only on PCIe boards. */
-			if (DO_PCI) {
-				ret = dio200_subdev_timer_init(dev, s);
-				if (ret < 0)
-					return ret;
-			} else {
-				s->type = COMEDI_SUBD_UNUSED;
-			}
-			break;
-		default:
-			s->type = COMEDI_SUBD_UNUSED;
-			break;
-		}
-	}
-	sdx = devpriv->intr_sd;
-	if (sdx >= 0 && sdx < dev->n_subdevices)
-		dev->read_subdev = &dev->subdevices[sdx];
-	if (irq) {
-		if (request_irq(irq, dio200_interrupt, req_irq_flags,
-				DIO200_DRIVER_NAME, dev) >= 0) {
-			dev->irq = irq;
-		} else {
-			dev_warn(dev->class_dev,
-				 "warning! irq %u unavailable!\n", irq);
-		}
-	}
-	dio200_report_attach(dev, irq);
-	return 1;
-}
-
-/*
- * Attach is called by the Comedi core to configure the driver
- * for a particular board.  If you specified a board_name array
- * in the driver structure, dev->board_ptr contains that
- * address.
- */
 static int dio200_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	const struct dio200_board *thisboard = comedi_board(dev);
 	struct dio200_private *devpriv;
+	unsigned int irq;
 	int ret;
 
-	dev_info(dev->class_dev, DIO200_DRIVER_NAME ": attach\n");
+	irq = it->options[1];
 
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	/* Process options and reserve resources according to bus type. */
-	if (is_isa_board(thisboard)) {
-		unsigned long iobase;
-		unsigned int irq;
-
-		iobase = it->options[0];
-		irq = it->options[1];
-		ret = dio200_request_region(dev, iobase, thisboard->mainsize);
-		if (ret < 0)
-			return ret;
-		devpriv->io.u.iobase = iobase;
-		devpriv->io.regtype = io_regtype;
-		return dio200_common_attach(dev, irq, 0);
-	} else if (is_pci_board(thisboard)) {
-		dev_err(dev->class_dev,
-			"Manual configuration of PCI board '%s' is not supported\n",
-			thisboard->name);
-		return -EIO;
-	} else {
-		dev_err(dev->class_dev, DIO200_DRIVER_NAME
-			": BUG! cannot determine board type!\n");
-		return -EINVAL;
-	}
-}
-
-/*
- * The auto_attach hook is called at PCI probe time via
- * comedi_pci_auto_config().  dev->board_ptr is NULL on entry.
- * There should be a board entry matching the supplied PCI device.
- */
-static int dio200_auto_attach(struct comedi_device *dev,
-					unsigned long context_unused)
-{
-	struct pci_dev *pci_dev = comedi_to_pci_dev(dev);
-	const struct dio200_board *thisboard;
-	struct dio200_private *devpriv;
-	resource_size_t base, len;
-	unsigned int bar;
-	int ret;
-
-	if (!DO_PCI)
-		return -EINVAL;
-
-	dev_info(dev->class_dev, DIO200_DRIVER_NAME ": attach pci %s\n",
-		 pci_name(pci_dev));
-
-	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
-	if (!devpriv)
-		return -ENOMEM;
-	dev->private = devpriv;
-
-	dev->board_ptr = dio200_find_pci_board(pci_dev);
-	if (dev->board_ptr == NULL) {
-		dev_err(dev->class_dev, "BUG! cannot determine board type!\n");
-		return -EINVAL;
-	}
-	thisboard = comedi_board(dev);
-	ret = comedi_pci_enable(pci_dev, DIO200_DRIVER_NAME);
-	if (ret < 0) {
-		dev_err(dev->class_dev,
-			"error! cannot enable PCI device and request regions!\n");
+	ret = comedi_request_region(dev, it->options[0], thisboard->mainsize);
+	if (ret)
 		return ret;
-	}
-	bar = thisboard->mainbar;
-	base = pci_resource_start(pci_dev, bar);
-	len = pci_resource_len(pci_dev, bar);
-	if (len < thisboard->mainsize) {
-		dev_err(dev->class_dev, "error! PCI region size too small!\n");
-		return -EINVAL;
-	}
-	if ((pci_resource_flags(pci_dev, bar) & IORESOURCE_MEM) != 0) {
-		devpriv->io.u.membase = ioremap_nocache(base, len);
-		if (!devpriv->io.u.membase) {
-			dev_err(dev->class_dev,
-				"error! cannot remap registers\n");
-			return -ENOMEM;
-		}
-		devpriv->io.regtype = mmio_regtype;
-	} else {
-		devpriv->io.u.iobase = (unsigned long)base;
-		devpriv->io.regtype = io_regtype;
-	}
-	switch (thisboard->model) {
-	case pcie215_model:
-	case pcie236_model:
-	case pcie296_model:
-		ret = dio200_pcie_board_setup(dev);
-		if (ret < 0)
-			return ret;
-		break;
-	default:
-		break;
-	}
-	return dio200_common_attach(dev, pci_dev->irq, IRQF_SHARED);
+	devpriv->io.u.iobase = dev->iobase;
+	devpriv->io.regtype = io_regtype;
+	return amplc_dio200_common_attach(dev, irq, 0);
 }
 
 static void dio200_detach(struct comedi_device *dev)
 {
-	const struct dio200_board *thisboard = comedi_board(dev);
-	struct dio200_private *devpriv = dev->private;
-	const struct dio200_layout *layout;
-	unsigned n;
-
-	if (!thisboard || !devpriv)
-		return;
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (dev->subdevices) {
-		layout = dio200_board_layout(thisboard);
-		for (n = 0; n < dev->n_subdevices; n++) {
-			struct comedi_subdevice *s = &dev->subdevices[n];
-			switch (layout->sdtype[n]) {
-			case sd_8254:
-				dio200_subdev_8254_cleanup(dev, s);
-				break;
-			case sd_8255:
-				dio200_subdev_8255_cleanup(dev, s);
-				break;
-			case sd_intr:
-				dio200_subdev_intr_cleanup(dev, s);
-				break;
-			case sd_timer:
-				/* Only on PCIe boards. */
-				if (DO_PCI)
-					dio200_subdev_timer_cleanup(dev, s);
-				break;
-			default:
-				break;
-			}
-		}
-	}
-	if (is_isa_board(thisboard)) {
-		if (devpriv->io.regtype == io_regtype)
-			release_region(devpriv->io.u.iobase,
-				       thisboard->mainsize);
-	} else if (is_pci_board(thisboard)) {
-		struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-		if (pcidev) {
-			if (devpriv->io.regtype != no_regtype) {
-				if (devpriv->io.regtype == mmio_regtype)
-					iounmap(devpriv->io.u.membase);
-				comedi_pci_disable(pcidev);
-			}
-		}
-	}
+	amplc_dio200_common_detach(dev);
+	comedi_legacy_detach(dev);
 }
 
-/*
- * The struct comedi_driver structure tells the Comedi core module
- * which functions to call to configure/deconfigure (attach/detach)
- * the board, and also about the kernel module that contains
- * the device code.
- */
 static struct comedi_driver amplc_dio200_driver = {
-	.driver_name = DIO200_DRIVER_NAME,
+	.driver_name = "amplc_dio200",
 	.module = THIS_MODULE,
 	.attach = dio200_attach,
-	.auto_attach = dio200_auto_attach,
 	.detach = dio200_detach,
-	.board_name = &dio200_boards[0].name,
+	.board_name = &dio200_isa_boards[0].name,
 	.offset = sizeof(struct dio200_board),
-	.num_names = ARRAY_SIZE(dio200_boards),
-};
-
-#if DO_PCI
-static DEFINE_PCI_DEVICE_TABLE(dio200_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI215) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI272) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCIE236) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCIE215) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCIE296) },
-	{0}
-};
-
-MODULE_DEVICE_TABLE(pci, dio200_pci_table);
-
-static int amplc_dio200_pci_probe(struct pci_dev *dev,
-						   const struct pci_device_id
-						   *ent)
-{
-	return comedi_pci_auto_config(dev, &amplc_dio200_driver);
-}
-
-static struct pci_driver amplc_dio200_pci_driver = {
-	.name = DIO200_DRIVER_NAME,
-	.id_table = dio200_pci_table,
-	.probe = &amplc_dio200_pci_probe,
-	.remove		= comedi_pci_auto_unconfig,
+	.num_names = ARRAY_SIZE(dio200_isa_boards),
 };
-module_comedi_pci_driver(amplc_dio200_driver, amplc_dio200_pci_driver);
-#else
 module_comedi_driver(amplc_dio200_driver);
-#endif
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
+MODULE_DESCRIPTION("Comedi driver for Amplicon 200 Series ISA DIO boards");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/amplc_dio200.h b/drivers/staging/comedi/drivers/amplc_dio200.h
new file mode 100644
index 00000000000..cf2e7261740
--- /dev/null
+++ b/drivers/staging/comedi/drivers/amplc_dio200.h
@@ -0,0 +1,95 @@
+/*
+    comedi/drivers/amplc_dio.h
+
+    Header for amplc_dio200.c, amplc_dio200_common.c and
+    amplc_dio200_pci.c.
+
+    Copyright (C) 2005-2013 MEV Ltd. <http://www.mev.co.uk/>
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1998,2000 David A. Schleef <ds@schleef.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 AMPLC_DIO200_H_INCLUDED
+#define AMPLC_DIO200_H_INCLUDED
+
+/* 200 series register area sizes */
+#define DIO200_IO_SIZE		0x20
+#define DIO200_PCIE_IO_SIZE	0x4000
+
+/*
+ * Register region.
+ */
+enum dio200_regtype { no_regtype = 0, io_regtype, mmio_regtype };
+struct dio200_region {
+	union {
+		unsigned long iobase;		/* I/O base address */
+		unsigned char __iomem *membase;	/* mapped MMIO base address */
+	} u;
+	enum dio200_regtype regtype;
+};
+
+/*
+ * Subdevice types.
+ */
+enum dio200_sdtype { sd_none, sd_intr, sd_8255, sd_8254, sd_timer };
+
+#define DIO200_MAX_SUBDEVS	8
+#define DIO200_MAX_ISNS		6
+
+/*
+ * Board descriptions.
+ */
+
+struct dio200_layout {
+	unsigned short n_subdevs;	/* number of subdevices */
+	unsigned char sdtype[DIO200_MAX_SUBDEVS];	/* enum dio200_sdtype */
+	unsigned char sdinfo[DIO200_MAX_SUBDEVS];	/* depends on sdtype */
+	bool has_int_sce:1;		/* has interrupt enable/status reg */
+	bool has_clk_gat_sce:1;		/* has clock/gate selection registers */
+	bool has_enhancements:1;	/* has enhanced features */
+};
+
+enum dio200_bustype { isa_bustype, pci_bustype };
+
+struct dio200_board {
+	const char *name;
+	struct dio200_layout layout;
+	enum dio200_bustype bustype;
+	unsigned char mainbar;
+	unsigned char mainshift;
+	unsigned int mainsize;
+};
+
+/*
+ * Comedi device private data.
+ */
+struct dio200_private {
+	struct dio200_region io;	/* Register region */
+	int intr_sd;
+};
+
+int amplc_dio200_common_attach(struct comedi_device *dev, unsigned int irq,
+			       unsigned long req_irq_flags);
+
+void amplc_dio200_common_detach(struct comedi_device *dev);
+
+/* Used by initialization of PCIe boards. */
+void amplc_dio200_set_enhance(struct comedi_device *dev, unsigned char val);
+
+#endif
diff --git a/drivers/staging/comedi/drivers/amplc_dio200_common.c b/drivers/staging/comedi/drivers/amplc_dio200_common.c
new file mode 100644
index 00000000000..3403e5ccfa9
--- /dev/null
+++ b/drivers/staging/comedi/drivers/amplc_dio200_common.c
@@ -0,0 +1,1271 @@
+/*
+    comedi/drivers/amplc_dio200_common.c
+
+    Common support code for "amplc_dio200" and "amplc_dio200_pci".
+
+    Copyright (C) 2005-2013 MEV Ltd. <http://www.mev.co.uk/>
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1998,2000 David A. Schleef <ds@schleef.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/interrupt.h>
+#include <linux/slab.h>
+
+#include "../comedidev.h"
+
+#include "amplc_dio200.h"
+#include "comedi_fc.h"
+#include "8253.h"
+
+/* 8255 control register bits */
+#define CR_C_LO_IO	0x01
+#define CR_B_IO		0x02
+#define CR_B_MODE	0x04
+#define CR_C_HI_IO	0x08
+#define CR_A_IO		0x10
+#define CR_A_MODE(a)	((a)<<5)
+#define CR_CW		0x80
+
+/* 200 series registers */
+#define DIO200_IO_SIZE		0x20
+#define DIO200_PCIE_IO_SIZE	0x4000
+#define DIO200_XCLK_SCE		0x18	/* Group X clock selection register */
+#define DIO200_YCLK_SCE		0x19	/* Group Y clock selection register */
+#define DIO200_ZCLK_SCE		0x1a	/* Group Z clock selection register */
+#define DIO200_XGAT_SCE		0x1b	/* Group X gate selection register */
+#define DIO200_YGAT_SCE		0x1c	/* Group Y gate selection register */
+#define DIO200_ZGAT_SCE		0x1d	/* Group Z gate selection register */
+#define DIO200_INT_SCE		0x1e	/* Interrupt enable/status register */
+/* Extra registers for new PCIe boards */
+#define DIO200_ENHANCE		0x20	/* 1 to enable enhanced features */
+#define DIO200_VERSION		0x24	/* Hardware version register */
+#define DIO200_TS_CONFIG	0x600	/* Timestamp timer config register */
+#define DIO200_TS_COUNT		0x602	/* Timestamp timer count register */
+
+/*
+ * Functions for constructing value for DIO_200_?CLK_SCE and
+ * DIO_200_?GAT_SCE registers:
+ *
+ * 'which' is: 0 for CTR-X1, CTR-Y1, CTR-Z1; 1 for CTR-X2, CTR-Y2 or CTR-Z2.
+ * 'chan' is the channel: 0, 1 or 2.
+ * 'source' is the signal source: 0 to 7, or 0 to 31 for "enhanced" boards.
+ */
+static unsigned char clk_gat_sce(unsigned int which, unsigned int chan,
+				 unsigned int source)
+{
+	return (which << 5) | (chan << 3) |
+	       ((source & 030) << 3) | (source & 007);
+}
+
+static unsigned char clk_sce(unsigned int which, unsigned int chan,
+			     unsigned int source)
+{
+	return clk_gat_sce(which, chan, source);
+}
+
+static unsigned char gat_sce(unsigned int which, unsigned int chan,
+			     unsigned int source)
+{
+	return clk_gat_sce(which, chan, source);
+}
+
+/*
+ * Periods of the internal clock sources in nanoseconds.
+ */
+static const unsigned int clock_period[32] = {
+	[1] = 100,		/* 10 MHz */
+	[2] = 1000,		/* 1 MHz */
+	[3] = 10000,		/* 100 kHz */
+	[4] = 100000,		/* 10 kHz */
+	[5] = 1000000,		/* 1 kHz */
+	[11] = 50,		/* 20 MHz (enhanced boards) */
+	/* clock sources 12 and later reserved for enhanced boards */
+};
+
+/*
+ * Timestamp timer configuration register (for new PCIe boards).
+ */
+#define TS_CONFIG_RESET		0x100	/* Reset counter to zero. */
+#define TS_CONFIG_CLK_SRC_MASK	0x0FF	/* Clock source. */
+#define TS_CONFIG_MAX_CLK_SRC	2	/* Maximum clock source value. */
+
+/*
+ * Periods of the timestamp timer clock sources in nanoseconds.
+ */
+static const unsigned int ts_clock_period[TS_CONFIG_MAX_CLK_SRC + 1] = {
+	1,			/* 1 nanosecond (but with 20 ns granularity). */
+	1000,			/* 1 microsecond. */
+	1000000,		/* 1 millisecond. */
+};
+
+struct dio200_subdev_8254 {
+	unsigned int ofs;		/* Counter base offset */
+	unsigned int clk_sce_ofs;	/* CLK_SCE base address */
+	unsigned int gat_sce_ofs;	/* GAT_SCE base address */
+	int which;			/* Bit 5 of CLK_SCE or GAT_SCE */
+	unsigned int clock_src[3];	/* Current clock sources */
+	unsigned int gate_src[3];	/* Current gate sources */
+	spinlock_t spinlock;
+};
+
+struct dio200_subdev_8255 {
+	unsigned int ofs;		/* DIO base offset */
+};
+
+struct dio200_subdev_intr {
+	spinlock_t spinlock;
+	unsigned int ofs;
+	unsigned int valid_isns;
+	unsigned int enabled_isns;
+	unsigned int stopcount;
+	bool active:1;
+	bool continuous:1;
+};
+
+static inline const struct dio200_layout *
+dio200_board_layout(const struct dio200_board *board)
+{
+	return &board->layout;
+}
+
+static inline const struct dio200_layout *
+dio200_dev_layout(struct comedi_device *dev)
+{
+	return dio200_board_layout(comedi_board(dev));
+}
+
+/*
+ * Read 8-bit register.
+ */
+static unsigned char dio200_read8(struct comedi_device *dev,
+				  unsigned int offset)
+{
+	const struct dio200_board *thisboard = comedi_board(dev);
+	struct dio200_private *devpriv = dev->private;
+
+	offset <<= thisboard->mainshift;
+	if (devpriv->io.regtype == io_regtype)
+		return inb(devpriv->io.u.iobase + offset);
+	else
+		return readb(devpriv->io.u.membase + offset);
+}
+
+/*
+ * Write 8-bit register.
+ */
+static void dio200_write8(struct comedi_device *dev, unsigned int offset,
+			  unsigned char val)
+{
+	const struct dio200_board *thisboard = comedi_board(dev);
+	struct dio200_private *devpriv = dev->private;
+
+	offset <<= thisboard->mainshift;
+	if (devpriv->io.regtype == io_regtype)
+		outb(val, devpriv->io.u.iobase + offset);
+	else
+		writeb(val, devpriv->io.u.membase + offset);
+}
+
+/*
+ * Read 32-bit register.
+ */
+static unsigned int dio200_read32(struct comedi_device *dev,
+				  unsigned int offset)
+{
+	const struct dio200_board *thisboard = comedi_board(dev);
+	struct dio200_private *devpriv = dev->private;
+
+	offset <<= thisboard->mainshift;
+	if (devpriv->io.regtype == io_regtype)
+		return inl(devpriv->io.u.iobase + offset);
+	else
+		return readl(devpriv->io.u.membase + offset);
+}
+
+/*
+ * Write 32-bit register.
+ */
+static void dio200_write32(struct comedi_device *dev, unsigned int offset,
+			   unsigned int val)
+{
+	const struct dio200_board *thisboard = comedi_board(dev);
+	struct dio200_private *devpriv = dev->private;
+
+	offset <<= thisboard->mainshift;
+	if (devpriv->io.regtype == io_regtype)
+		outl(val, devpriv->io.u.iobase + offset);
+	else
+		writel(val, devpriv->io.u.membase + offset);
+}
+
+/*
+ * 'insn_bits' function for an 'INTERRUPT' subdevice.
+ */
+static int
+dio200_subdev_intr_insn_bits(struct comedi_device *dev,
+			     struct comedi_subdevice *s,
+			     struct comedi_insn *insn, unsigned int *data)
+{
+	const struct dio200_layout *layout = dio200_dev_layout(dev);
+	struct dio200_subdev_intr *subpriv = s->private;
+
+	if (layout->has_int_sce) {
+		/* Just read the interrupt status register.  */
+		data[1] = dio200_read8(dev, subpriv->ofs) & subpriv->valid_isns;
+	} else {
+		/* No interrupt status register. */
+		data[0] = 0;
+	}
+
+	return insn->n;
+}
+
+/*
+ * Called to stop acquisition for an 'INTERRUPT' subdevice.
+ */
+static void dio200_stop_intr(struct comedi_device *dev,
+			     struct comedi_subdevice *s)
+{
+	const struct dio200_layout *layout = dio200_dev_layout(dev);
+	struct dio200_subdev_intr *subpriv = s->private;
+
+	subpriv->active = false;
+	subpriv->enabled_isns = 0;
+	if (layout->has_int_sce)
+		dio200_write8(dev, subpriv->ofs, 0);
+}
+
+/*
+ * Called to start acquisition for an 'INTERRUPT' subdevice.
+ */
+static int dio200_start_intr(struct comedi_device *dev,
+			     struct comedi_subdevice *s)
+{
+	unsigned int n;
+	unsigned isn_bits;
+	const struct dio200_layout *layout = dio200_dev_layout(dev);
+	struct dio200_subdev_intr *subpriv = s->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
+	int retval = 0;
+
+	if (!subpriv->continuous && subpriv->stopcount == 0) {
+		/* An empty acquisition! */
+		s->async->events |= COMEDI_CB_EOA;
+		subpriv->active = false;
+		retval = 1;
+	} else {
+		/* Determine interrupt sources to enable. */
+		isn_bits = 0;
+		if (cmd->chanlist) {
+			for (n = 0; n < cmd->chanlist_len; n++)
+				isn_bits |= (1U << CR_CHAN(cmd->chanlist[n]));
+		}
+		isn_bits &= subpriv->valid_isns;
+		/* Enable interrupt sources. */
+		subpriv->enabled_isns = isn_bits;
+		if (layout->has_int_sce)
+			dio200_write8(dev, subpriv->ofs, isn_bits);
+	}
+
+	return retval;
+}
+
+/*
+ * Internal trigger function to start acquisition for an 'INTERRUPT' subdevice.
+ */
+static int
+dio200_inttrig_start_intr(struct comedi_device *dev, struct comedi_subdevice *s,
+			  unsigned int trignum)
+{
+	struct dio200_subdev_intr *subpriv;
+	unsigned long flags;
+	int event = 0;
+
+	if (trignum != 0)
+		return -EINVAL;
+
+	subpriv = s->private;
+
+	spin_lock_irqsave(&subpriv->spinlock, flags);
+	s->async->inttrig = NULL;
+	if (subpriv->active)
+		event = dio200_start_intr(dev, s);
+
+	spin_unlock_irqrestore(&subpriv->spinlock, flags);
+
+	if (event)
+		comedi_event(dev, s);
+
+	return 1;
+}
+
+static void dio200_read_scan_intr(struct comedi_device *dev,
+				  struct comedi_subdevice *s,
+				  unsigned int triggered)
+{
+	struct dio200_subdev_intr *subpriv = s->private;
+	unsigned short val;
+	unsigned int n, ch, len;
+
+	val = 0;
+	len = s->async->cmd.chanlist_len;
+	for (n = 0; n < len; n++) {
+		ch = CR_CHAN(s->async->cmd.chanlist[n]);
+		if (triggered & (1U << ch))
+			val |= (1U << n);
+	}
+	/* Write the scan to the buffer. */
+	if (comedi_buf_put(s->async, val)) {
+		s->async->events |= (COMEDI_CB_BLOCK | COMEDI_CB_EOS);
+	} else {
+		/* Error!  Stop acquisition.  */
+		dio200_stop_intr(dev, s);
+		s->async->events |= COMEDI_CB_ERROR | COMEDI_CB_OVERFLOW;
+		comedi_error(dev, "buffer overflow");
+	}
+
+	/* Check for end of acquisition. */
+	if (!subpriv->continuous) {
+		/* stop_src == TRIG_COUNT */
+		if (subpriv->stopcount > 0) {
+			subpriv->stopcount--;
+			if (subpriv->stopcount == 0) {
+				s->async->events |= COMEDI_CB_EOA;
+				dio200_stop_intr(dev, s);
+			}
+		}
+	}
+}
+
+/*
+ * This is called from the interrupt service routine to handle a read
+ * scan on an 'INTERRUPT' subdevice.
+ */
+static int dio200_handle_read_intr(struct comedi_device *dev,
+				   struct comedi_subdevice *s)
+{
+	const struct dio200_layout *layout = dio200_dev_layout(dev);
+	struct dio200_subdev_intr *subpriv = s->private;
+	unsigned triggered;
+	unsigned intstat;
+	unsigned cur_enabled;
+	unsigned int oldevents;
+	unsigned long flags;
+
+	triggered = 0;
+
+	spin_lock_irqsave(&subpriv->spinlock, flags);
+	oldevents = s->async->events;
+	if (layout->has_int_sce) {
+		/*
+		 * Collect interrupt sources that have triggered and disable
+		 * them temporarily.  Loop around until no extra interrupt
+		 * sources have triggered, at which point, the valid part of
+		 * the interrupt status register will read zero, clearing the
+		 * cause of the interrupt.
+		 *
+		 * Mask off interrupt sources already seen to avoid infinite
+		 * loop in case of misconfiguration.
+		 */
+		cur_enabled = subpriv->enabled_isns;
+		while ((intstat = (dio200_read8(dev, subpriv->ofs) &
+				   subpriv->valid_isns & ~triggered)) != 0) {
+			triggered |= intstat;
+			cur_enabled &= ~triggered;
+			dio200_write8(dev, subpriv->ofs, cur_enabled);
+		}
+	} else {
+		/*
+		 * No interrupt status register.  Assume the single interrupt
+		 * source has triggered.
+		 */
+		triggered = subpriv->enabled_isns;
+	}
+
+	if (triggered) {
+		/*
+		 * Some interrupt sources have triggered and have been
+		 * temporarily disabled to clear the cause of the interrupt.
+		 *
+		 * Reenable them NOW to minimize the time they are disabled.
+		 */
+		cur_enabled = subpriv->enabled_isns;
+		if (layout->has_int_sce)
+			dio200_write8(dev, subpriv->ofs, cur_enabled);
+
+		if (subpriv->active) {
+			/*
+			 * The command is still active.
+			 *
+			 * Ignore interrupt sources that the command isn't
+			 * interested in (just in case there's a race
+			 * condition).
+			 */
+			if (triggered & subpriv->enabled_isns)
+				/* Collect scan data. */
+				dio200_read_scan_intr(dev, s, triggered);
+		}
+	}
+	spin_unlock_irqrestore(&subpriv->spinlock, flags);
+
+	if (oldevents != s->async->events)
+		comedi_event(dev, s);
+
+	return (triggered != 0);
+}
+
+/*
+ * 'cancel' function for an 'INTERRUPT' subdevice.
+ */
+static int dio200_subdev_intr_cancel(struct comedi_device *dev,
+				     struct comedi_subdevice *s)
+{
+	struct dio200_subdev_intr *subpriv = s->private;
+	unsigned long flags;
+
+	spin_lock_irqsave(&subpriv->spinlock, flags);
+	if (subpriv->active)
+		dio200_stop_intr(dev, s);
+
+	spin_unlock_irqrestore(&subpriv->spinlock, flags);
+
+	return 0;
+}
+
+/*
+ * 'do_cmdtest' function for an 'INTERRUPT' subdevice.
+ */
+static int
+dio200_subdev_intr_cmdtest(struct comedi_device *dev,
+			   struct comedi_subdevice *s, struct comedi_cmd *cmd)
+{
+	int err = 0;
+
+	/* Step 1 : check if triggers are trivially valid */
+
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_INT);
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
+	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_NOW);
+	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+
+	if (err)
+		return 1;
+
+	/* Step 2a : make sure trigger sources are unique */
+
+	err |= cfc_check_trigger_is_unique(cmd->start_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
+
+	if (err)
+		return 2;
+
+	/* Step 3: check if arguments are trivially valid */
+
+	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
+	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+
+	switch (cmd->stop_src) {
+	case TRIG_COUNT:
+		/* any count allowed */
+		break;
+	case TRIG_NONE:
+		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		break;
+	default:
+		break;
+	}
+
+	if (err)
+		return 3;
+
+	/* step 4: fix up any arguments */
+
+	/* if (err) return 4; */
+
+	return 0;
+}
+
+/*
+ * 'do_cmd' function for an 'INTERRUPT' subdevice.
+ */
+static int dio200_subdev_intr_cmd(struct comedi_device *dev,
+				  struct comedi_subdevice *s)
+{
+	struct comedi_cmd *cmd = &s->async->cmd;
+	struct dio200_subdev_intr *subpriv = s->private;
+	unsigned long flags;
+	int event = 0;
+
+	spin_lock_irqsave(&subpriv->spinlock, flags);
+	subpriv->active = 1;
+
+	/* Set up end of acquisition. */
+	switch (cmd->stop_src) {
+	case TRIG_COUNT:
+		subpriv->continuous = false;
+		subpriv->stopcount = cmd->stop_arg;
+		break;
+	default:
+		/* TRIG_NONE */
+		subpriv->continuous = true;
+		subpriv->stopcount = 0;
+		break;
+	}
+
+	/* Set up start of acquisition. */
+	switch (cmd->start_src) {
+	case TRIG_INT:
+		s->async->inttrig = dio200_inttrig_start_intr;
+		break;
+	default:
+		/* TRIG_NOW */
+		event = dio200_start_intr(dev, s);
+		break;
+	}
+	spin_unlock_irqrestore(&subpriv->spinlock, flags);
+
+	if (event)
+		comedi_event(dev, s);
+
+	return 0;
+}
+
+/*
+ * This function initializes an 'INTERRUPT' subdevice.
+ */
+static int
+dio200_subdev_intr_init(struct comedi_device *dev, struct comedi_subdevice *s,
+			unsigned int offset, unsigned valid_isns)
+{
+	const struct dio200_layout *layout = dio200_dev_layout(dev);
+	struct dio200_subdev_intr *subpriv;
+
+	subpriv = kzalloc(sizeof(*subpriv), GFP_KERNEL);
+	if (!subpriv)
+		return -ENOMEM;
+
+	subpriv->ofs = offset;
+	subpriv->valid_isns = valid_isns;
+	spin_lock_init(&subpriv->spinlock);
+
+	if (layout->has_int_sce)
+		/* Disable interrupt sources. */
+		dio200_write8(dev, subpriv->ofs, 0);
+
+	s->private = subpriv;
+	s->type = COMEDI_SUBD_DI;
+	s->subdev_flags = SDF_READABLE | SDF_CMD_READ;
+	if (layout->has_int_sce) {
+		s->n_chan = DIO200_MAX_ISNS;
+		s->len_chanlist = DIO200_MAX_ISNS;
+	} else {
+		/* No interrupt source register.  Support single channel. */
+		s->n_chan = 1;
+		s->len_chanlist = 1;
+	}
+	s->range_table = &range_digital;
+	s->maxdata = 1;
+	s->insn_bits = dio200_subdev_intr_insn_bits;
+	s->do_cmdtest = dio200_subdev_intr_cmdtest;
+	s->do_cmd = dio200_subdev_intr_cmd;
+	s->cancel = dio200_subdev_intr_cancel;
+
+	return 0;
+}
+
+/*
+ * Interrupt service routine.
+ */
+static irqreturn_t dio200_interrupt(int irq, void *d)
+{
+	struct comedi_device *dev = d;
+	struct dio200_private *devpriv = dev->private;
+	struct comedi_subdevice *s;
+	int handled;
+
+	if (!dev->attached)
+		return IRQ_NONE;
+
+	if (devpriv->intr_sd >= 0) {
+		s = &dev->subdevices[devpriv->intr_sd];
+		handled = dio200_handle_read_intr(dev, s);
+	} else {
+		handled = 0;
+	}
+
+	return IRQ_RETVAL(handled);
+}
+
+/*
+ * Read an '8254' counter subdevice channel.
+ */
+static unsigned int
+dio200_subdev_8254_read_chan(struct comedi_device *dev,
+			     struct comedi_subdevice *s, unsigned int chan)
+{
+	struct dio200_subdev_8254 *subpriv = s->private;
+	unsigned int val;
+
+	/* latch counter */
+	val = chan << 6;
+	dio200_write8(dev, subpriv->ofs + i8254_control_reg, val);
+	/* read lsb, msb */
+	val = dio200_read8(dev, subpriv->ofs + chan);
+	val += dio200_read8(dev, subpriv->ofs + chan) << 8;
+	return val;
+}
+
+/*
+ * Write an '8254' subdevice channel.
+ */
+static void
+dio200_subdev_8254_write_chan(struct comedi_device *dev,
+			      struct comedi_subdevice *s, unsigned int chan,
+			      unsigned int count)
+{
+	struct dio200_subdev_8254 *subpriv = s->private;
+
+	/* write lsb, msb */
+	dio200_write8(dev, subpriv->ofs + chan, count & 0xff);
+	dio200_write8(dev, subpriv->ofs + chan, (count >> 8) & 0xff);
+}
+
+/*
+ * Set mode of an '8254' subdevice channel.
+ */
+static void
+dio200_subdev_8254_set_mode(struct comedi_device *dev,
+			    struct comedi_subdevice *s, unsigned int chan,
+			    unsigned int mode)
+{
+	struct dio200_subdev_8254 *subpriv = s->private;
+	unsigned int byte;
+
+	byte = chan << 6;
+	byte |= 0x30;		/* access order: lsb, msb */
+	byte |= (mode & 0xf);	/* counter mode and BCD|binary */
+	dio200_write8(dev, subpriv->ofs + i8254_control_reg, byte);
+}
+
+/*
+ * Read status byte of an '8254' counter subdevice channel.
+ */
+static unsigned int
+dio200_subdev_8254_status(struct comedi_device *dev,
+			  struct comedi_subdevice *s, unsigned int chan)
+{
+	struct dio200_subdev_8254 *subpriv = s->private;
+
+	/* latch status */
+	dio200_write8(dev, subpriv->ofs + i8254_control_reg,
+		      0xe0 | (2 << chan));
+	/* read status */
+	return dio200_read8(dev, subpriv->ofs + chan);
+}
+
+/*
+ * Handle 'insn_read' for an '8254' counter subdevice.
+ */
+static int
+dio200_subdev_8254_read(struct comedi_device *dev, struct comedi_subdevice *s,
+			struct comedi_insn *insn, unsigned int *data)
+{
+	struct dio200_subdev_8254 *subpriv = s->private;
+	int chan = CR_CHAN(insn->chanspec);
+	unsigned int n;
+	unsigned long flags;
+
+	for (n = 0; n < insn->n; n++) {
+		spin_lock_irqsave(&subpriv->spinlock, flags);
+		data[n] = dio200_subdev_8254_read_chan(dev, s, chan);
+		spin_unlock_irqrestore(&subpriv->spinlock, flags);
+	}
+	return insn->n;
+}
+
+/*
+ * Handle 'insn_write' for an '8254' counter subdevice.
+ */
+static int
+dio200_subdev_8254_write(struct comedi_device *dev, struct comedi_subdevice *s,
+			 struct comedi_insn *insn, unsigned int *data)
+{
+	struct dio200_subdev_8254 *subpriv = s->private;
+	int chan = CR_CHAN(insn->chanspec);
+	unsigned int n;
+	unsigned long flags;
+
+	for (n = 0; n < insn->n; n++) {
+		spin_lock_irqsave(&subpriv->spinlock, flags);
+		dio200_subdev_8254_write_chan(dev, s, chan, data[n]);
+		spin_unlock_irqrestore(&subpriv->spinlock, flags);
+	}
+	return insn->n;
+}
+
+/*
+ * Set gate source for an '8254' counter subdevice channel.
+ */
+static int
+dio200_subdev_8254_set_gate_src(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				unsigned int counter_number,
+				unsigned int gate_src)
+{
+	const struct dio200_layout *layout = dio200_dev_layout(dev);
+	struct dio200_subdev_8254 *subpriv = s->private;
+	unsigned char byte;
+
+	if (!layout->has_clk_gat_sce)
+		return -1;
+	if (counter_number > 2)
+		return -1;
+	if (gate_src > (layout->has_enhancements ? 31 : 7))
+		return -1;
+
+	subpriv->gate_src[counter_number] = gate_src;
+	byte = gat_sce(subpriv->which, counter_number, gate_src);
+	dio200_write8(dev, subpriv->gat_sce_ofs, byte);
+
+	return 0;
+}
+
+/*
+ * Get gate source for an '8254' counter subdevice channel.
+ */
+static int
+dio200_subdev_8254_get_gate_src(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				unsigned int counter_number)
+{
+	const struct dio200_layout *layout = dio200_dev_layout(dev);
+	struct dio200_subdev_8254 *subpriv = s->private;
+
+	if (!layout->has_clk_gat_sce)
+		return -1;
+	if (counter_number > 2)
+		return -1;
+
+	return subpriv->gate_src[counter_number];
+}
+
+/*
+ * Set clock source for an '8254' counter subdevice channel.
+ */
+static int
+dio200_subdev_8254_set_clock_src(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 unsigned int counter_number,
+				 unsigned int clock_src)
+{
+	const struct dio200_layout *layout = dio200_dev_layout(dev);
+	struct dio200_subdev_8254 *subpriv = s->private;
+	unsigned char byte;
+
+	if (!layout->has_clk_gat_sce)
+		return -1;
+	if (counter_number > 2)
+		return -1;
+	if (clock_src > (layout->has_enhancements ? 31 : 7))
+		return -1;
+
+	subpriv->clock_src[counter_number] = clock_src;
+	byte = clk_sce(subpriv->which, counter_number, clock_src);
+	dio200_write8(dev, subpriv->clk_sce_ofs, byte);
+
+	return 0;
+}
+
+/*
+ * Get clock source for an '8254' counter subdevice channel.
+ */
+static int
+dio200_subdev_8254_get_clock_src(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 unsigned int counter_number,
+				 unsigned int *period_ns)
+{
+	const struct dio200_layout *layout = dio200_dev_layout(dev);
+	struct dio200_subdev_8254 *subpriv = s->private;
+	unsigned clock_src;
+
+	if (!layout->has_clk_gat_sce)
+		return -1;
+	if (counter_number > 2)
+		return -1;
+
+	clock_src = subpriv->clock_src[counter_number];
+	*period_ns = clock_period[clock_src];
+	return clock_src;
+}
+
+/*
+ * Handle 'insn_config' for an '8254' counter subdevice.
+ */
+static int
+dio200_subdev_8254_config(struct comedi_device *dev, struct comedi_subdevice *s,
+			  struct comedi_insn *insn, unsigned int *data)
+{
+	struct dio200_subdev_8254 *subpriv = s->private;
+	int ret = 0;
+	int chan = CR_CHAN(insn->chanspec);
+	unsigned long flags;
+
+	spin_lock_irqsave(&subpriv->spinlock, flags);
+	switch (data[0]) {
+	case INSN_CONFIG_SET_COUNTER_MODE:
+		if (data[1] > (I8254_MODE5 | I8254_BINARY))
+			ret = -EINVAL;
+		else
+			dio200_subdev_8254_set_mode(dev, s, chan, data[1]);
+		break;
+	case INSN_CONFIG_8254_READ_STATUS:
+		data[1] = dio200_subdev_8254_status(dev, s, chan);
+		break;
+	case INSN_CONFIG_SET_GATE_SRC:
+		ret = dio200_subdev_8254_set_gate_src(dev, s, chan, data[2]);
+		if (ret < 0)
+			ret = -EINVAL;
+		break;
+	case INSN_CONFIG_GET_GATE_SRC:
+		ret = dio200_subdev_8254_get_gate_src(dev, s, chan);
+		if (ret < 0) {
+			ret = -EINVAL;
+			break;
+		}
+		data[2] = ret;
+		break;
+	case INSN_CONFIG_SET_CLOCK_SRC:
+		ret = dio200_subdev_8254_set_clock_src(dev, s, chan, data[1]);
+		if (ret < 0)
+			ret = -EINVAL;
+		break;
+	case INSN_CONFIG_GET_CLOCK_SRC:
+		ret = dio200_subdev_8254_get_clock_src(dev, s, chan, &data[2]);
+		if (ret < 0) {
+			ret = -EINVAL;
+			break;
+		}
+		data[1] = ret;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+	spin_unlock_irqrestore(&subpriv->spinlock, flags);
+	return ret < 0 ? ret : insn->n;
+}
+
+/*
+ * This function initializes an '8254' counter subdevice.
+ */
+static int
+dio200_subdev_8254_init(struct comedi_device *dev, struct comedi_subdevice *s,
+			unsigned int offset)
+{
+	const struct dio200_layout *layout = dio200_dev_layout(dev);
+	struct dio200_subdev_8254 *subpriv;
+	unsigned int chan;
+
+	subpriv = kzalloc(sizeof(*subpriv), GFP_KERNEL);
+	if (!subpriv)
+		return -ENOMEM;
+
+	s->private = subpriv;
+	s->type = COMEDI_SUBD_COUNTER;
+	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
+	s->n_chan = 3;
+	s->maxdata = 0xFFFF;
+	s->insn_read = dio200_subdev_8254_read;
+	s->insn_write = dio200_subdev_8254_write;
+	s->insn_config = dio200_subdev_8254_config;
+
+	spin_lock_init(&subpriv->spinlock);
+	subpriv->ofs = offset;
+	if (layout->has_clk_gat_sce) {
+		/* Derive CLK_SCE and GAT_SCE register offsets from
+		 * 8254 offset. */
+		subpriv->clk_sce_ofs = DIO200_XCLK_SCE + (offset >> 3);
+		subpriv->gat_sce_ofs = DIO200_XGAT_SCE + (offset >> 3);
+		subpriv->which = (offset >> 2) & 1;
+	}
+
+	/* Initialize channels. */
+	for (chan = 0; chan < 3; chan++) {
+		dio200_subdev_8254_set_mode(dev, s, chan,
+					    I8254_MODE0 | I8254_BINARY);
+		if (layout->has_clk_gat_sce) {
+			/* Gate source 0 is VCC (logic 1). */
+			dio200_subdev_8254_set_gate_src(dev, s, chan, 0);
+			/* Clock source 0 is the dedicated clock input. */
+			dio200_subdev_8254_set_clock_src(dev, s, chan, 0);
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * This function sets I/O directions for an '8255' DIO subdevice.
+ */
+static void dio200_subdev_8255_set_dir(struct comedi_device *dev,
+				       struct comedi_subdevice *s)
+{
+	struct dio200_subdev_8255 *subpriv = s->private;
+	int config;
+
+	config = CR_CW;
+	/* 1 in io_bits indicates output, 1 in config indicates input */
+	if (!(s->io_bits & 0x0000ff))
+		config |= CR_A_IO;
+	if (!(s->io_bits & 0x00ff00))
+		config |= CR_B_IO;
+	if (!(s->io_bits & 0x0f0000))
+		config |= CR_C_LO_IO;
+	if (!(s->io_bits & 0xf00000))
+		config |= CR_C_HI_IO;
+	dio200_write8(dev, subpriv->ofs + 3, config);
+}
+
+/*
+ * Handle 'insn_bits' for an '8255' DIO subdevice.
+ */
+static int dio200_subdev_8255_bits(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   struct comedi_insn *insn, unsigned int *data)
+{
+	struct dio200_subdev_8255 *subpriv = s->private;
+
+	if (data[0]) {
+		s->state &= ~data[0];
+		s->state |= (data[0] & data[1]);
+		if (data[0] & 0xff)
+			dio200_write8(dev, subpriv->ofs, s->state & 0xff);
+		if (data[0] & 0xff00)
+			dio200_write8(dev, subpriv->ofs + 1,
+				      (s->state >> 8) & 0xff);
+		if (data[0] & 0xff0000)
+			dio200_write8(dev, subpriv->ofs + 2,
+				      (s->state >> 16) & 0xff);
+	}
+	data[1] = dio200_read8(dev, subpriv->ofs);
+	data[1] |= dio200_read8(dev, subpriv->ofs + 1) << 8;
+	data[1] |= dio200_read8(dev, subpriv->ofs + 2) << 16;
+	return 2;
+}
+
+/*
+ * Handle 'insn_config' for an '8255' DIO subdevice.
+ */
+static int dio200_subdev_8255_config(struct comedi_device *dev,
+				     struct comedi_subdevice *s,
+				     struct comedi_insn *insn,
+				     unsigned int *data)
+{
+	unsigned int mask;
+	unsigned int bits;
+
+	mask = 1 << CR_CHAN(insn->chanspec);
+	if (mask & 0x0000ff)
+		bits = 0x0000ff;
+	else if (mask & 0x00ff00)
+		bits = 0x00ff00;
+	else if (mask & 0x0f0000)
+		bits = 0x0f0000;
+	else
+		bits = 0xf00000;
+	switch (data[0]) {
+	case INSN_CONFIG_DIO_INPUT:
+		s->io_bits &= ~bits;
+		break;
+	case INSN_CONFIG_DIO_OUTPUT:
+		s->io_bits |= bits;
+		break;
+	case INSN_CONFIG_DIO_QUERY:
+		data[1] = (s->io_bits & bits) ? COMEDI_OUTPUT : COMEDI_INPUT;
+		return insn->n;
+		break;
+	default:
+		return -EINVAL;
+	}
+	dio200_subdev_8255_set_dir(dev, s);
+	return 1;
+}
+
+/*
+ * This function initializes an '8255' DIO subdevice.
+ *
+ * offset is the offset to the 8255 chip.
+ */
+static int dio200_subdev_8255_init(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   unsigned int offset)
+{
+	struct dio200_subdev_8255 *subpriv;
+
+	subpriv = kzalloc(sizeof(*subpriv), GFP_KERNEL);
+	if (!subpriv)
+		return -ENOMEM;
+	subpriv->ofs = offset;
+	s->private = subpriv;
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->n_chan = 24;
+	s->range_table = &range_digital;
+	s->maxdata = 1;
+	s->insn_bits = dio200_subdev_8255_bits;
+	s->insn_config = dio200_subdev_8255_config;
+	s->state = 0;
+	s->io_bits = 0;
+	dio200_subdev_8255_set_dir(dev, s);
+	return 0;
+}
+
+/*
+ * Handle 'insn_read' for a timer subdevice.
+ */
+static int dio200_subdev_timer_read(struct comedi_device *dev,
+				    struct comedi_subdevice *s,
+				    struct comedi_insn *insn,
+				    unsigned int *data)
+{
+	unsigned int n;
+
+	for (n = 0; n < insn->n; n++)
+		data[n] = dio200_read32(dev, DIO200_TS_COUNT);
+	return n;
+}
+
+/*
+ * Reset timer subdevice.
+ */
+static void dio200_subdev_timer_reset(struct comedi_device *dev,
+				      struct comedi_subdevice *s)
+{
+	unsigned int clock;
+
+	clock = dio200_read32(dev, DIO200_TS_CONFIG) & TS_CONFIG_CLK_SRC_MASK;
+	dio200_write32(dev, DIO200_TS_CONFIG, clock | TS_CONFIG_RESET);
+	dio200_write32(dev, DIO200_TS_CONFIG, clock);
+}
+
+/*
+ * Get timer subdevice clock source and period.
+ */
+static void dio200_subdev_timer_get_clock_src(struct comedi_device *dev,
+					      struct comedi_subdevice *s,
+					      unsigned int *src,
+					      unsigned int *period)
+{
+	unsigned int clk;
+
+	clk = dio200_read32(dev, DIO200_TS_CONFIG) & TS_CONFIG_CLK_SRC_MASK;
+	*src = clk;
+	*period = (clk < ARRAY_SIZE(ts_clock_period)) ?
+		  ts_clock_period[clk] : 0;
+}
+
+/*
+ * Set timer subdevice clock source.
+ */
+static int dio200_subdev_timer_set_clock_src(struct comedi_device *dev,
+					     struct comedi_subdevice *s,
+					     unsigned int src)
+{
+	if (src > TS_CONFIG_MAX_CLK_SRC)
+		return -EINVAL;
+	dio200_write32(dev, DIO200_TS_CONFIG, src);
+	return 0;
+}
+
+/*
+ * Handle 'insn_config' for a timer subdevice.
+ */
+static int dio200_subdev_timer_config(struct comedi_device *dev,
+				      struct comedi_subdevice *s,
+				      struct comedi_insn *insn,
+				      unsigned int *data)
+{
+	int ret = 0;
+
+	switch (data[0]) {
+	case INSN_CONFIG_RESET:
+		dio200_subdev_timer_reset(dev, s);
+		break;
+	case INSN_CONFIG_SET_CLOCK_SRC:
+		ret = dio200_subdev_timer_set_clock_src(dev, s, data[1]);
+		if (ret < 0)
+			ret = -EINVAL;
+		break;
+	case INSN_CONFIG_GET_CLOCK_SRC:
+		dio200_subdev_timer_get_clock_src(dev, s, &data[1], &data[2]);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+	return ret < 0 ? ret : insn->n;
+}
+
+/*
+ * This function initializes a timer subdevice.
+ *
+ * Uses the timestamp timer registers.  There is only one timestamp timer.
+ */
+static int dio200_subdev_timer_init(struct comedi_device *dev,
+				    struct comedi_subdevice *s)
+{
+	s->type = COMEDI_SUBD_TIMER;
+	s->subdev_flags = SDF_READABLE | SDF_LSAMPL;
+	s->n_chan = 1;
+	s->maxdata = 0xFFFFFFFF;
+	s->insn_read = dio200_subdev_timer_read;
+	s->insn_config = dio200_subdev_timer_config;
+	return 0;
+}
+
+void amplc_dio200_set_enhance(struct comedi_device *dev, unsigned char val)
+{
+	dio200_write8(dev, DIO200_ENHANCE, val);
+}
+EXPORT_SYMBOL_GPL(amplc_dio200_set_enhance);
+
+int amplc_dio200_common_attach(struct comedi_device *dev, unsigned int irq,
+			       unsigned long req_irq_flags)
+{
+	const struct dio200_board *thisboard = comedi_board(dev);
+	struct dio200_private *devpriv = dev->private;
+	const struct dio200_layout *layout = dio200_board_layout(thisboard);
+	struct comedi_subdevice *s;
+	int sdx;
+	unsigned int n;
+	int ret;
+
+	devpriv->intr_sd = -1;
+
+	ret = comedi_alloc_subdevices(dev, layout->n_subdevs);
+	if (ret)
+		return ret;
+
+	for (n = 0; n < dev->n_subdevices; n++) {
+		s = &dev->subdevices[n];
+		switch (layout->sdtype[n]) {
+		case sd_8254:
+			/* counter subdevice (8254) */
+			ret = dio200_subdev_8254_init(dev, s,
+						      layout->sdinfo[n]);
+			if (ret < 0)
+				return ret;
+			break;
+		case sd_8255:
+			/* digital i/o subdevice (8255) */
+			ret = dio200_subdev_8255_init(dev, s,
+						      layout->sdinfo[n]);
+			if (ret < 0)
+				return ret;
+			break;
+		case sd_intr:
+			/* 'INTERRUPT' subdevice */
+			if (irq) {
+				ret = dio200_subdev_intr_init(dev, s,
+							      DIO200_INT_SCE,
+							      layout->sdinfo[n]
+							     );
+				if (ret < 0)
+					return ret;
+				devpriv->intr_sd = n;
+			} else {
+				s->type = COMEDI_SUBD_UNUSED;
+			}
+			break;
+		case sd_timer:
+			ret = dio200_subdev_timer_init(dev, s);
+			if (ret < 0)
+				return ret;
+			break;
+		default:
+			s->type = COMEDI_SUBD_UNUSED;
+			break;
+		}
+	}
+	sdx = devpriv->intr_sd;
+	if (sdx >= 0 && sdx < dev->n_subdevices)
+		dev->read_subdev = &dev->subdevices[sdx];
+	if (irq) {
+		if (request_irq(irq, dio200_interrupt, req_irq_flags,
+				dev->board_name, dev) >= 0) {
+			dev->irq = irq;
+		} else {
+			dev_warn(dev->class_dev,
+				 "warning! irq %u unavailable!\n", irq);
+		}
+	}
+	dev_info(dev->class_dev, "attached\n");
+	return 0;
+}
+EXPORT_SYMBOL_GPL(amplc_dio200_common_attach);
+
+void amplc_dio200_common_detach(struct comedi_device *dev)
+{
+	const struct dio200_board *thisboard = comedi_board(dev);
+	struct dio200_private *devpriv = dev->private;
+	const struct dio200_layout *layout;
+	unsigned n;
+
+	if (!thisboard || !devpriv)
+		return;
+	if (dev->irq)
+		free_irq(dev->irq, dev);
+	if (dev->subdevices) {
+		layout = dio200_board_layout(thisboard);
+		for (n = 0; n < dev->n_subdevices; n++) {
+			switch (layout->sdtype[n]) {
+			case sd_8254:
+			case sd_8255:
+			case sd_intr:
+				comedi_spriv_free(dev, n);
+				break;
+			case sd_timer:
+			default:
+				break;
+			}
+		}
+	}
+}
+EXPORT_SYMBOL_GPL(amplc_dio200_common_detach);
+
+static int __init amplc_dio200_common_init(void)
+{
+	return 0;
+}
+module_init(amplc_dio200_common_init);
+
+static void __exit amplc_dio200_common_exit(void)
+{
+}
+module_exit(amplc_dio200_common_exit);
+
+MODULE_AUTHOR("Comedi http://www.comedi.org");
+MODULE_DESCRIPTION("Comedi helper for amplc_dio200 and amplc_dio200_pci");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/amplc_dio200_pci.c b/drivers/staging/comedi/drivers/amplc_dio200_pci.c
new file mode 100644
index 00000000000..4be44e87737
--- /dev/null
+++ b/drivers/staging/comedi/drivers/amplc_dio200_pci.c
@@ -0,0 +1,486 @@
+/* comedi/drivers/amplc_dio200_pci.c
+
+    Driver for Amplicon PCI215, PCI272, PCIe215, PCIe236, PCIe296.
+
+    Copyright (C) 2005-2013 MEV Ltd. <http://www.mev.co.uk/>
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1998,2000 David A. Schleef <ds@schleef.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.
+
+*/
+/*
+ * Driver: amplc_dio200_pci
+ * Description: Amplicon 200 Series PCI Digital I/O
+ * Author: Ian Abbott <abbotti@mev.co.uk>
+ * Devices: [Amplicon] PCI215 (amplc_dio200_pci), PCIe215, PCIe236,
+ *   PCI272, PCIe296
+ * Updated: Mon, 18 Mar 2013 15:03:50 +0000
+ * Status: works
+ *
+ * Configuration options:
+ *   none
+ *
+ * Manual configuration of PCI(e) cards is not supported; they are configured
+ * automatically.
+ *
+ * SUBDEVICES
+ *
+ *                     PCI215         PCIe215        PCIe236
+ *                  -------------  -------------  -------------
+ *   Subdevices           5              8              8
+ *    0                 PPI-X          PPI-X          PPI-X
+ *    1                 PPI-Y          UNUSED         UNUSED
+ *    2                 CTR-Z1         PPI-Y          UNUSED
+ *    3                 CTR-Z2         UNUSED         UNUSED
+ *    4               INTERRUPT        CTR-Z1         CTR-Z1
+ *    5                                CTR-Z2         CTR-Z2
+ *    6                                TIMER          TIMER
+ *    7                              INTERRUPT      INTERRUPT
+ *
+ *
+ *                     PCI272         PCIe296
+ *                  -------------  -------------
+ *   Subdevices           4              8
+ *    0                 PPI-X          PPI-X1
+ *    1                 PPI-Y          PPI-X2
+ *    2                 PPI-Z          PPI-Y1
+ *    3               INTERRUPT        PPI-Y2
+ *    4                                CTR-Z1
+ *    5                                CTR-Z2
+ *    6                                TIMER
+ *    7                              INTERRUPT
+ *
+ * Each PPI is a 8255 chip providing 24 DIO channels.  The DIO channels
+ * are configurable as inputs or outputs in four groups:
+ *
+ *   Port A  - channels  0 to  7
+ *   Port B  - channels  8 to 15
+ *   Port CL - channels 16 to 19
+ *   Port CH - channels 20 to 23
+ *
+ * Only mode 0 of the 8255 chips is supported.
+ *
+ * Each CTR is a 8254 chip providing 3 16-bit counter channels.  Each
+ * channel is configured individually with INSN_CONFIG instructions.  The
+ * specific type of configuration instruction is specified in data[0].
+ * Some configuration instructions expect an additional parameter in
+ * data[1]; others return a value in data[1].  The following configuration
+ * instructions are supported:
+ *
+ *   INSN_CONFIG_SET_COUNTER_MODE.  Sets the counter channel's mode and
+ *     BCD/binary setting specified in data[1].
+ *
+ *   INSN_CONFIG_8254_READ_STATUS.  Reads the status register value for the
+ *     counter channel into data[1].
+ *
+ *   INSN_CONFIG_SET_CLOCK_SRC.  Sets the counter channel's clock source as
+ *     specified in data[1] (this is a hardware-specific value).  Not
+ *     supported on PC214E.  For the other boards, valid clock sources are
+ *     0 to 7 as follows:
+ *
+ *       0.  CLK n, the counter channel's dedicated CLK input from the SK1
+ *         connector.  (N.B. for other values, the counter channel's CLKn
+ *         pin on the SK1 connector is an output!)
+ *       1.  Internal 10 MHz clock.
+ *       2.  Internal 1 MHz clock.
+ *       3.  Internal 100 kHz clock.
+ *       4.  Internal 10 kHz clock.
+ *       5.  Internal 1 kHz clock.
+ *       6.  OUT n-1, the output of counter channel n-1 (see note 1 below).
+ *       7.  Ext Clock, the counter chip's dedicated Ext Clock input from
+ *         the SK1 connector.  This pin is shared by all three counter
+ *         channels on the chip.
+ *
+ *     For the PCIe boards, clock sources in the range 0 to 31 are allowed
+ *     and the following additional clock sources are defined:
+ *
+ *       8.  HIGH logic level.
+ *       9.  LOW logic level.
+ *      10.  "Pattern present" signal.
+ *      11.  Internal 20 MHz clock.
+ *
+ *   INSN_CONFIG_GET_CLOCK_SRC.  Returns the counter channel's current
+ *     clock source in data[1].  For internal clock sources, data[2] is set
+ *     to the period in ns.
+ *
+ *   INSN_CONFIG_SET_GATE_SRC.  Sets the counter channel's gate source as
+ *     specified in data[2] (this is a hardware-specific value).  Not
+ *     supported on PC214E.  For the other boards, valid gate sources are 0
+ *     to 7 as follows:
+ *
+ *       0.  VCC (internal +5V d.c.), i.e. gate permanently enabled.
+ *       1.  GND (internal 0V d.c.), i.e. gate permanently disabled.
+ *       2.  GAT n, the counter channel's dedicated GAT input from the SK1
+ *         connector.  (N.B. for other values, the counter channel's GATn
+ *         pin on the SK1 connector is an output!)
+ *       3.  /OUT n-2, the inverted output of counter channel n-2 (see note
+ *         2 below).
+ *       4.  Reserved.
+ *       5.  Reserved.
+ *       6.  Reserved.
+ *       7.  Reserved.
+ *
+ *     For the PCIe boards, gate sources in the range 0 to 31 are allowed;
+ *     the following additional clock sources and clock sources 6 and 7 are
+ *     (re)defined:
+ *
+ *       6.  /GAT n, negated version of the counter channel's dedicated
+ *         GAT input (negated version of gate source 2).
+ *       7.  OUT n-2, the non-inverted output of counter channel n-2
+ *         (negated version of gate source 3).
+ *       8.  "Pattern present" signal, HIGH while pattern present.
+ *       9.  "Pattern occurred" latched signal, latches HIGH when pattern
+ *         occurs.
+ *      10.  "Pattern gone away" latched signal, latches LOW when pattern
+ *         goes away after it occurred.
+ *      11.  Negated "pattern present" signal, LOW while pattern present
+ *         (negated version of gate source 8).
+ *      12.  Negated "pattern occurred" latched signal, latches LOW when
+ *         pattern occurs (negated version of gate source 9).
+ *      13.  Negated "pattern gone away" latched signal, latches LOW when
+ *         pattern goes away after it occurred (negated version of gate
+ *         source 10).
+ *
+ *   INSN_CONFIG_GET_GATE_SRC.  Returns the counter channel's current gate
+ *     source in data[2].
+ *
+ * Clock and gate interconnection notes:
+ *
+ *   1.  Clock source OUT n-1 is the output of the preceding channel on the
+ *   same counter subdevice if n > 0, or the output of channel 2 on the
+ *   preceding counter subdevice (see note 3) if n = 0.
+ *
+ *   2.  Gate source /OUT n-2 is the inverted output of channel 0 on the
+ *   same counter subdevice if n = 2, or the inverted output of channel n+1
+ *   on the preceding counter subdevice (see note 3) if n < 2.
+ *
+ *   3.  The counter subdevices are connected in a ring, so the highest
+ *   counter subdevice precedes the lowest.
+ *
+ * The 'TIMER' subdevice is a free-running 32-bit timer subdevice.
+ *
+ * The 'INTERRUPT' subdevice pretends to be a digital input subdevice.  The
+ * digital inputs come from the interrupt status register.  The number of
+ * channels matches the number of interrupt sources.  The PC214E does not
+ * have an interrupt status register; see notes on 'INTERRUPT SOURCES'
+ * below.
+ *
+ * INTERRUPT SOURCES
+ *
+ *                     PCI215         PCIe215        PCIe236
+ *                  -------------  -------------  -------------
+ *   Sources              6              6              6
+ *    0               PPI-X-C0       PPI-X-C0       PPI-X-C0
+ *    1               PPI-X-C3       PPI-X-C3       PPI-X-C3
+ *    2               PPI-Y-C0       PPI-Y-C0        unused
+ *    3               PPI-Y-C3       PPI-Y-C3        unused
+ *    4              CTR-Z1-OUT1    CTR-Z1-OUT1    CTR-Z1-OUT1
+ *    5              CTR-Z2-OUT1    CTR-Z2-OUT1    CTR-Z2-OUT1
+ *
+ *                     PCI272         PCIe296
+ *                  -------------  -------------
+ *   Sources              6              6
+ *    0               PPI-X-C0       PPI-X1-C0
+ *    1               PPI-X-C3       PPI-X1-C3
+ *    2               PPI-Y-C0       PPI-Y1-C0
+ *    3               PPI-Y-C3       PPI-Y1-C3
+ *    4               PPI-Z-C0      CTR-Z1-OUT1
+ *    5               PPI-Z-C3      CTR-Z2-OUT1
+ *
+ * When an interrupt source is enabled in the interrupt source enable
+ * register, a rising edge on the source signal latches the corresponding
+ * bit to 1 in the interrupt status register.
+ *
+ * When the interrupt status register value as a whole (actually, just the
+ * 6 least significant bits) goes from zero to non-zero, the board will
+ * generate an interrupt.  The interrupt will remain asserted until the
+ * interrupt status register is cleared to zero.  To clear a bit to zero in
+ * the interrupt status register, the corresponding interrupt source must
+ * be disabled in the interrupt source enable register (there is no
+ * separate interrupt clear register).
+ *
+ * COMMANDS
+ *
+ * The driver supports a read streaming acquisition command on the
+ * 'INTERRUPT' subdevice.  The channel list selects the interrupt sources
+ * to be enabled.  All channels will be sampled together (convert_src ==
+ * TRIG_NOW).  The scan begins a short time after the hardware interrupt
+ * occurs, subject to interrupt latencies (scan_begin_src == TRIG_EXT,
+ * scan_begin_arg == 0).  The value read from the interrupt status register
+ * is packed into a short value, one bit per requested channel, in the
+ * order they appear in the channel list.
+ */
+
+#include <linux/pci.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+
+#include "../comedidev.h"
+
+#include "amplc_dio200.h"
+
+/* PCI IDs */
+#define PCI_DEVICE_ID_AMPLICON_PCI272 0x000a
+#define PCI_DEVICE_ID_AMPLICON_PCI215 0x000b
+#define PCI_DEVICE_ID_AMPLICON_PCIE236 0x0011
+#define PCI_DEVICE_ID_AMPLICON_PCIE215 0x0012
+#define PCI_DEVICE_ID_AMPLICON_PCIE296 0x0014
+
+/*
+ * Board descriptions.
+ */
+
+enum dio200_pci_model {
+	pci215_model,
+	pci272_model,
+	pcie215_model,
+	pcie236_model,
+	pcie296_model
+};
+
+static const struct dio200_board dio200_pci_boards[] = {
+	[pci215_model] = {
+		.name = "pci215",
+		.bustype = pci_bustype,
+		.mainbar = 2,
+		.mainsize = DIO200_IO_SIZE,
+		.layout = {
+			.n_subdevs = 5,
+			.sdtype = {sd_8255, sd_8255, sd_8254, sd_8254, sd_intr},
+			.sdinfo = {0x00, 0x08, 0x10, 0x14, 0x3F},
+			.has_int_sce = true,
+			.has_clk_gat_sce = true,
+		},
+	},
+	[pci272_model] = {
+		.name = "pci272",
+		.bustype = pci_bustype,
+		.mainbar = 2,
+		.mainsize = DIO200_IO_SIZE,
+		.layout = {
+			.n_subdevs = 4,
+			.sdtype = {sd_8255, sd_8255, sd_8255, sd_intr},
+			.sdinfo = {0x00, 0x08, 0x10, 0x3F},
+			.has_int_sce = true,
+		},
+	},
+	[pcie215_model] = {
+		.name = "pcie215",
+		.bustype = pci_bustype,
+		.mainbar = 1,
+		.mainshift = 3,
+		.mainsize = DIO200_PCIE_IO_SIZE,
+		.layout = {
+			.n_subdevs = 8,
+			.sdtype = {sd_8255, sd_none, sd_8255, sd_none,
+				   sd_8254, sd_8254, sd_timer, sd_intr},
+			.sdinfo = {0x00, 0x00, 0x08, 0x00,
+				   0x10, 0x14, 0x00, 0x3F},
+			.has_int_sce = true,
+			.has_clk_gat_sce = true,
+			.has_enhancements = true,
+		},
+	},
+	[pcie236_model] = {
+		.name = "pcie236",
+		.bustype = pci_bustype,
+		.mainbar = 1,
+		.mainshift = 3,
+		.mainsize = DIO200_PCIE_IO_SIZE,
+		.layout = {
+			.n_subdevs = 8,
+			.sdtype = {sd_8255, sd_none, sd_none, sd_none,
+				   sd_8254, sd_8254, sd_timer, sd_intr},
+			.sdinfo = {0x00, 0x00, 0x00, 0x00,
+				   0x10, 0x14, 0x00, 0x3F},
+			.has_int_sce = true,
+			.has_clk_gat_sce = true,
+			.has_enhancements = true,
+		},
+	},
+	[pcie296_model] = {
+		.name = "pcie296",
+		.bustype = pci_bustype,
+		.mainbar = 1,
+		.mainshift = 3,
+		.mainsize = DIO200_PCIE_IO_SIZE,
+		.layout = {
+			.n_subdevs = 8,
+			.sdtype = {sd_8255, sd_8255, sd_8255, sd_8255,
+				   sd_8254, sd_8254, sd_timer, sd_intr},
+			.sdinfo = {0x00, 0x04, 0x08, 0x0C,
+				   0x10, 0x14, 0x00, 0x3F},
+			.has_int_sce = true,
+			.has_clk_gat_sce = true,
+			.has_enhancements = true,
+		},
+	},
+};
+
+/*
+ * This function does some special set-up for the PCIe boards
+ * PCIe215, PCIe236, PCIe296.
+ */
+static int dio200_pcie_board_setup(struct comedi_device *dev)
+{
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	void __iomem *brbase;
+
+	/*
+	 * The board uses Altera Cyclone IV with PCI-Express hard IP.
+	 * The FPGA configuration has the PCI-Express Avalon-MM Bridge
+	 * Control registers in PCI BAR 0, offset 0, and the length of
+	 * these registers is 0x4000.
+	 *
+	 * We need to write 0x80 to the "Avalon-MM to PCI-Express Interrupt
+	 * Enable" register at offset 0x50 to allow generation of PCIe
+	 * interrupts when RXmlrq_i is asserted in the SOPC Builder system.
+	 */
+	if (pci_resource_len(pcidev, 0) < 0x4000) {
+		dev_err(dev->class_dev, "error! bad PCI region!\n");
+		return -EINVAL;
+	}
+	brbase = pci_ioremap_bar(pcidev, 0);
+	if (!brbase) {
+		dev_err(dev->class_dev, "error! failed to map registers!\n");
+		return -ENOMEM;
+	}
+	writel(0x80, brbase + 0x50);
+	iounmap(brbase);
+	/* Enable "enhanced" features of board. */
+	amplc_dio200_set_enhance(dev, 1);
+	return 0;
+}
+
+static int dio200_pci_auto_attach(struct comedi_device *dev,
+				  unsigned long context_model)
+{
+	struct pci_dev *pci_dev = comedi_to_pci_dev(dev);
+	const struct dio200_board *thisboard = NULL;
+	struct dio200_private *devpriv;
+	unsigned int bar;
+	int ret;
+
+	if (context_model < ARRAY_SIZE(dio200_pci_boards))
+		thisboard = &dio200_pci_boards[context_model];
+	if (!thisboard)
+		return -EINVAL;
+	dev->board_ptr = thisboard;
+	dev->board_name = thisboard->name;
+
+	dev_info(dev->class_dev, "%s: attach pci %s (%s)\n",
+		 dev->driver->driver_name, pci_name(pci_dev), dev->board_name);
+
+	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
+	if (!devpriv)
+		return -ENOMEM;
+	dev->private = devpriv;
+
+	ret = comedi_pci_enable(dev);
+	if (ret)
+		return ret;
+
+	bar = thisboard->mainbar;
+	if (pci_resource_len(pci_dev, bar) < thisboard->mainsize) {
+		dev_err(dev->class_dev, "error! PCI region size too small!\n");
+		return -EINVAL;
+	}
+	if (pci_resource_flags(pci_dev, bar) & IORESOURCE_MEM) {
+		devpriv->io.u.membase = pci_ioremap_bar(pci_dev, bar);
+		if (!devpriv->io.u.membase) {
+			dev_err(dev->class_dev,
+				"error! cannot remap registers\n");
+			return -ENOMEM;
+		}
+		devpriv->io.regtype = mmio_regtype;
+	} else {
+		devpriv->io.u.iobase = pci_resource_start(pci_dev, bar);
+		devpriv->io.regtype = io_regtype;
+	}
+	switch (context_model) {
+	case pcie215_model:
+	case pcie236_model:
+	case pcie296_model:
+		ret = dio200_pcie_board_setup(dev);
+		if (ret < 0)
+			return ret;
+		break;
+	default:
+		break;
+	}
+	return amplc_dio200_common_attach(dev, pci_dev->irq, IRQF_SHARED);
+}
+
+static void dio200_pci_detach(struct comedi_device *dev)
+{
+	const struct dio200_board *thisboard = comedi_board(dev);
+	struct dio200_private *devpriv = dev->private;
+
+	if (!thisboard || !devpriv)
+		return;
+	amplc_dio200_common_detach(dev);
+	if (devpriv->io.regtype == mmio_regtype)
+		iounmap(devpriv->io.u.membase);
+	comedi_pci_disable(dev);
+}
+
+static struct comedi_driver dio200_pci_comedi_driver = {
+	.driver_name = "amplc_dio200_pci",
+	.module = THIS_MODULE,
+	.auto_attach = dio200_pci_auto_attach,
+	.detach = dio200_pci_detach,
+};
+
+static DEFINE_PCI_DEVICE_TABLE(dio200_pci_table) = {
+	{
+		PCI_VDEVICE(AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI215),
+		pci215_model
+	}, {
+		PCI_VDEVICE(AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI272),
+		pci272_model
+	}, {
+		PCI_VDEVICE(AMPLICON, PCI_DEVICE_ID_AMPLICON_PCIE236),
+		pcie236_model
+	}, {
+		PCI_VDEVICE(AMPLICON, PCI_DEVICE_ID_AMPLICON_PCIE215),
+		pcie215_model
+	}, {
+		PCI_VDEVICE(AMPLICON, PCI_DEVICE_ID_AMPLICON_PCIE296),
+		pcie296_model
+	},
+	{0}
+};
+
+MODULE_DEVICE_TABLE(pci, dio200_pci_table);
+
+static int dio200_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
+{
+	return comedi_pci_auto_config(dev, &dio200_pci_comedi_driver,
+				      id->driver_data);
+}
+
+static struct pci_driver dio200_pci_pci_driver = {
+	.name = "amplc_dio200_pci",
+	.id_table = dio200_pci_table,
+	.probe = dio200_pci_probe,
+	.remove	= comedi_pci_auto_unconfig,
+};
+module_comedi_pci_driver(dio200_pci_comedi_driver, dio200_pci_pci_driver);
+
+MODULE_AUTHOR("Comedi http://www.comedi.org");
+MODULE_DESCRIPTION("Comedi driver for Amplicon 200 Series PCI(e) DIO boards");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/amplc_pc236.c b/drivers/staging/comedi/drivers/amplc_pc236.c
index 479e10fddd2..115ecd51677 100644
--- a/drivers/staging/comedi/drivers/amplc_pc236.c
+++ b/drivers/staging/comedi/drivers/amplc_pc236.c
@@ -75,23 +75,19 @@ unused.
 #define PC236_IO_SIZE		4
 #define PC236_LCR_IO_SIZE	128
 
-/*
- * INTCSR values for PCI236.
- */
-/* Disable interrupt, also clear any interrupt there */
-#define PCI236_INTR_DISABLE (PLX9052_INTCSR_LI1ENAB_DISABLED \
-	| PLX9052_INTCSR_LI1POL_HIGH \
-	| PLX9052_INTCSR_LI2POL_HIGH \
-	| PLX9052_INTCSR_PCIENAB_DISABLED \
-	| PLX9052_INTCSR_LI1SEL_EDGE \
-	| PLX9052_INTCSR_LI1CLRINT_ASSERTED)
-/* Enable interrupt, also clear any interrupt there. */
-#define PCI236_INTR_ENABLE (PLX9052_INTCSR_LI1ENAB_ENABLED \
-	| PLX9052_INTCSR_LI1POL_HIGH \
-	| PLX9052_INTCSR_LI2POL_HIGH \
-	| PLX9052_INTCSR_PCIENAB_ENABLED \
-	| PLX9052_INTCSR_LI1SEL_EDGE \
-	| PLX9052_INTCSR_LI1CLRINT_ASSERTED)
+/* Disable, and clear, interrupts */
+#define PCI236_INTR_DISABLE	(PLX9052_INTCSR_LI1POL |	\
+				 PLX9052_INTCSR_LI2POL |	\
+				 PLX9052_INTCSR_LI1SEL |	\
+				 PLX9052_INTCSR_LI1CLRINT)
+
+/* Enable, and clear, interrupts */
+#define PCI236_INTR_ENABLE	(PLX9052_INTCSR_LI1ENAB |	\
+				 PLX9052_INTCSR_LI1POL |	\
+				 PLX9052_INTCSR_LI2POL |	\
+				 PLX9052_INTCSR_PCIENAB |	\
+				 PLX9052_INTCSR_LI1SEL |	\
+				 PLX9052_INTCSR_LI1CLRINT)
 
 /*
  * Board descriptions for Amplicon PC36AT and PCI236.
@@ -209,21 +205,6 @@ static struct pci_dev *pc236_find_pci_dev(struct comedi_device *dev,
 }
 
 /*
- * This function checks and requests an I/O region, reporting an error
- * if there is a conflict.
- */
-static int pc236_request_region(struct comedi_device *dev, unsigned long from,
-				unsigned long extent)
-{
-	if (!from || !request_region(from, extent, PC236_DRIVER_NAME)) {
-		dev_err(dev->class_dev, "I/O port conflict (%#lx,%lu)!\n",
-		       from, extent);
-		return -EIO;
-	}
-	return 0;
-}
-
-/*
  * This function is called to mark the interrupt as disabled (no command
  * configured on subdevice 1) and to physically disable the interrupt
  * (not possible on the PC36AT, except by removing the IRQ jumper!).
@@ -272,14 +253,14 @@ static int pc236_intr_check(struct comedi_device *dev)
 	struct pc236_private *devpriv = dev->private;
 	int retval = 0;
 	unsigned long flags;
+	unsigned int intcsr;
 
 	spin_lock_irqsave(&dev->spinlock, flags);
 	if (devpriv->enable_irq) {
 		retval = 1;
 		if (is_pci_board(thisboard)) {
-			if ((inl(devpriv->lcr_iobase + PLX9052_INTCSR)
-			     & PLX9052_INTCSR_LI1STAT_MASK)
-			    == PLX9052_INTCSR_LI1STAT_INACTIVE) {
+			intcsr = inl(devpriv->lcr_iobase + PLX9052_INTCSR);
+			if (!(intcsr & PLX9052_INTCSR_LI1STAT)) {
 				retval = 0;
 			} else {
 				/* Clear interrupt and keep it enabled. */
@@ -470,12 +451,10 @@ static int pc236_pci_common_attach(struct comedi_device *dev,
 
 	comedi_set_hw_dev(dev, &pci_dev->dev);
 
-	ret = comedi_pci_enable(pci_dev, PC236_DRIVER_NAME);
-	if (ret < 0) {
-		dev_err(dev->class_dev,
-			"error! cannot enable PCI device and request regions!\n");
+	ret = comedi_pci_enable(dev);
+	if (ret)
 		return ret;
-	}
+
 	devpriv->lcr_iobase = pci_resource_start(pci_dev, 1);
 	iobase = pci_resource_start(pci_dev, 2);
 	return pc236_common_attach(dev, iobase, pci_dev->irq, IRQF_SHARED);
@@ -493,8 +472,6 @@ static int pc236_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	struct pc236_private *devpriv;
 	int ret;
 
-	dev_info(dev->class_dev, PC236_DRIVER_NAME ": attach\n");
-
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
@@ -502,12 +479,11 @@ static int pc236_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	/* Process options according to bus type. */
 	if (is_isa_board(thisboard)) {
-		unsigned long iobase = it->options[0];
-		unsigned int irq = it->options[1];
-		ret = pc236_request_region(dev, iobase, PC236_IO_SIZE);
-		if (ret < 0)
+		ret = comedi_request_region(dev, it->options[0], PC236_IO_SIZE);
+		if (ret)
 			return ret;
-		return pc236_common_attach(dev, iobase, irq, 0);
+
+		return pc236_common_attach(dev, dev->iobase, it->options[1], 0);
 	} else if (is_pci_board(thisboard)) {
 		struct pci_dev *pci_dev;
 
@@ -567,20 +543,16 @@ static void pc236_detach(struct comedi_device *dev)
 		return;
 	if (dev->iobase)
 		pc236_intr_disable(dev);
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (dev->subdevices)
-		subdev_8255_cleanup(dev, &dev->subdevices[0]);
+	comedi_spriv_free(dev, 0);
 	if (is_isa_board(thisboard)) {
-		if (dev->iobase)
-			release_region(dev->iobase, PC236_IO_SIZE);
+		comedi_legacy_detach(dev);
 	} else if (is_pci_board(thisboard)) {
 		struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-		if (pcidev) {
-			if (dev->iobase)
-				comedi_pci_disable(pcidev);
+		if (dev->irq)
+			free_irq(dev->irq, dev);
+		comedi_pci_disable(dev);
+		if (pcidev)
 			pci_dev_put(pcidev);
-		}
 	}
 }
 
@@ -610,9 +582,10 @@ static DEFINE_PCI_DEVICE_TABLE(pc236_pci_table) = {
 MODULE_DEVICE_TABLE(pci, pc236_pci_table);
 
 static int amplc_pc236_pci_probe(struct pci_dev *dev,
-					   const struct pci_device_id *ent)
+				 const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &amplc_pc236_driver);
+	return comedi_pci_auto_config(dev, &amplc_pc236_driver,
+				      id->driver_data);
 }
 
 static struct pci_driver amplc_pc236_pci_driver = {
diff --git a/drivers/staging/comedi/drivers/amplc_pc263.c b/drivers/staging/comedi/drivers/amplc_pc263.c
index 11c1f4764ea..94a752d852b 100644
--- a/drivers/staging/comedi/drivers/amplc_pc263.c
+++ b/drivers/staging/comedi/drivers/amplc_pc263.c
@@ -24,163 +24,41 @@
 */
 /*
 Driver: amplc_pc263
-Description: Amplicon PC263, PCI263
+Description: Amplicon PC263
 Author: Ian Abbott <abbotti@mev.co.uk>
-Devices: [Amplicon] PC263 (pc263), PCI263 (pci263 or amplc_pc263)
-Updated: Wed, 22 Oct 2008 14:10:53 +0100
+Devices: [Amplicon] PC263 (pc263)
+Updated: Fri, 12 Apr 2013 15:19:36 +0100
 Status: works
 
-Configuration options - PC263:
+Configuration options:
   [0] - I/O port base address
 
-Configuration options - PCI263:
-  [0] - PCI bus of device (optional)
-  [1] - PCI slot of device (optional)
-  If bus/slot is not specified, the first available PCI device will be
-  used.
-
-Each board appears as one subdevice, with 16 digital outputs, each
+The board appears as one subdevice, with 16 digital outputs, each
 connected to a reed-relay. Relay contacts are closed when output is 1.
 The state of the outputs can be read.
 */
 
-#include <linux/pci.h>
-
 #include "../comedidev.h"
 
 #define PC263_DRIVER_NAME	"amplc_pc263"
 
-#define DO_ISA	IS_ENABLED(CONFIG_COMEDI_AMPLC_PC263_ISA)
-#define DO_PCI	IS_ENABLED(CONFIG_COMEDI_AMPLC_PC263_PCI)
-
-/* PCI263 PCI configuration register information */
-#define PCI_DEVICE_ID_AMPLICON_PCI263 0x000c
-#define PCI_DEVICE_ID_INVALID 0xffff
-
-/* PC263 / PCI263 registers */
+/* PC263 registers */
 #define PC263_IO_SIZE	2
 
 /*
- * Board descriptions for Amplicon PC263 / PCI263.
+ * Board descriptions for Amplicon PC263.
  */
 
-enum pc263_bustype { isa_bustype, pci_bustype };
-enum pc263_model { pc263_model, pci263_model, anypci_model };
-
 struct pc263_board {
 	const char *name;
-	unsigned short devid;
-	enum pc263_bustype bustype;
-	enum pc263_model model;
 };
+
 static const struct pc263_board pc263_boards[] = {
-#if DO_ISA
 	{
 		.name = "pc263",
-		.bustype = isa_bustype,
-		.model = pc263_model,
 	},
-#endif
-#if DO_PCI
-	{
-		.name = "pci263",
-		.devid = PCI_DEVICE_ID_AMPLICON_PCI263,
-		.bustype = pci_bustype,
-		.model = pci263_model,
-	},
-	{
-		.name = PC263_DRIVER_NAME,
-		.devid = PCI_DEVICE_ID_INVALID,
-		.bustype = pci_bustype,
-		.model = anypci_model,	/* wildcard */
-	},
-#endif
 };
 
-/* test if ISA supported and this is an ISA board */
-static inline bool is_isa_board(const struct pc263_board *board)
-{
-	return DO_ISA && board->bustype == isa_bustype;
-}
-
-/* test if PCI supported and this is a PCI board */
-static inline bool is_pci_board(const struct pc263_board *board)
-{
-	return DO_PCI && board->bustype == pci_bustype;
-}
-
-/*
- * This function looks for a board matching the supplied PCI device.
- */
-static const struct pc263_board *pc263_find_pci_board(struct pci_dev *pci_dev)
-{
-	unsigned int i;
-
-	for (i = 0; i < ARRAY_SIZE(pc263_boards); i++)
-		if (is_pci_board(&pc263_boards[i]) &&
-		    pci_dev->device == pc263_boards[i].devid)
-			return &pc263_boards[i];
-	return NULL;
-}
-
-
-/*
- * This function looks for a PCI device matching the requested board name,
- * bus and slot.
- */
-static struct pci_dev *pc263_find_pci_dev(struct comedi_device *dev,
-					  struct comedi_devconfig *it)
-{
-	const struct pc263_board *thisboard = comedi_board(dev);
-	struct pci_dev *pci_dev = NULL;
-	int bus = it->options[0];
-	int slot = it->options[1];
-
-	for_each_pci_dev(pci_dev) {
-		if (bus || slot) {
-			if (bus != pci_dev->bus->number ||
-			    slot != PCI_SLOT(pci_dev->devfn))
-				continue;
-		}
-		if (pci_dev->vendor != PCI_VENDOR_ID_AMPLICON)
-			continue;
-
-		if (thisboard->model == anypci_model) {
-			/* Wildcard board matches any supported PCI board. */
-			const struct pc263_board *foundboard;
-
-			foundboard = pc263_find_pci_board(pci_dev);
-			if (foundboard == NULL)
-				continue;
-			/* Replace wildcard board_ptr. */
-			dev->board_ptr = thisboard = foundboard;
-		} else {
-			/* Match specific model name. */
-			if (pci_dev->device != thisboard->devid)
-				continue;
-		}
-		return pci_dev;
-	}
-	dev_err(dev->class_dev,
-		"No supported board found! (req. bus %d, slot %d)\n",
-		bus, slot);
-	return NULL;
-}
-/*
- * This function checks and requests an I/O region, reporting an error
- * if there is a conflict.
- */
-static int pc263_request_region(struct comedi_device *dev, unsigned long from,
-				unsigned long extent)
-{
-	if (!from || !request_region(from, extent, PC263_DRIVER_NAME)) {
-		dev_err(dev->class_dev, "I/O port conflict (%#lx,%lu)!\n",
-			from, extent);
-		return -EIO;
-	}
-	return 0;
-}
-
 static int pc263_do_insn_bits(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_insn *insn, unsigned int *data)
@@ -197,30 +75,14 @@ static int pc263_do_insn_bits(struct comedi_device *dev,
 	return insn->n;
 }
 
-static void pc263_report_attach(struct comedi_device *dev)
-{
-	const struct pc263_board *thisboard = comedi_board(dev);
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	char tmpbuf[40];
-
-	if (is_isa_board(thisboard))
-		snprintf(tmpbuf, sizeof(tmpbuf), "(base %#lx) ", dev->iobase);
-	else if (is_pci_board(thisboard))
-		snprintf(tmpbuf, sizeof(tmpbuf), "(pci %s) ",
-			 pci_name(pcidev));
-	else
-		tmpbuf[0] = '\0';
-	dev_info(dev->class_dev, "%s %sattached\n", dev->board_name, tmpbuf);
-}
-
-static int pc263_common_attach(struct comedi_device *dev, unsigned long iobase)
+static int pc263_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	const struct pc263_board *thisboard = comedi_board(dev);
 	struct comedi_subdevice *s;
 	int ret;
 
-	dev->board_name = thisboard->name;
-	dev->iobase = iobase;
+	ret = comedi_request_region(dev, it->options[0], PC263_IO_SIZE);
+	if (ret)
+		return ret;
 
 	ret = comedi_alloc_subdevices(dev, 1);
 	if (ret)
@@ -237,154 +99,23 @@ static int pc263_common_attach(struct comedi_device *dev, unsigned long iobase)
 	/* read initial relay state */
 	s->state = inb(dev->iobase) | (inb(dev->iobase + 1) << 8);
 
-	pc263_report_attach(dev);
-	return 1;
-}
-
-static int pc263_pci_common_attach(struct comedi_device *dev,
-				   struct pci_dev *pci_dev)
-{
-	unsigned long iobase;
-	int ret;
-
-	comedi_set_hw_dev(dev, &pci_dev->dev);
-
-	ret = comedi_pci_enable(pci_dev, PC263_DRIVER_NAME);
-	if (ret < 0) {
-		dev_err(dev->class_dev,
-			"error! cannot enable PCI device and request regions!\n");
-		return ret;
-	}
-	iobase = pci_resource_start(pci_dev, 2);
-	return pc263_common_attach(dev, iobase);
-}
-
-/*
- * Attach is called by the Comedi core to configure the driver
- * for a particular board.  If you specified a board_name array
- * in the driver structure, dev->board_ptr contains that
- * address.
- */
-static int pc263_attach(struct comedi_device *dev, struct comedi_devconfig *it)
-{
-	const struct pc263_board *thisboard = comedi_board(dev);
-	int ret;
-
-	dev_info(dev->class_dev, PC263_DRIVER_NAME ": attach\n");
-
-	/* Process options and reserve resources according to bus type. */
-	if (is_isa_board(thisboard)) {
-		unsigned long iobase = it->options[0];
-		ret = pc263_request_region(dev, iobase, PC263_IO_SIZE);
-		if (ret < 0)
-			return ret;
-		return pc263_common_attach(dev, iobase);
-	} else if (is_pci_board(thisboard)) {
-		struct pci_dev *pci_dev;
-
-		pci_dev = pc263_find_pci_dev(dev, it);
-		if (!pci_dev)
-			return -EIO;
-		return pc263_pci_common_attach(dev, pci_dev);
-	} else {
-		dev_err(dev->class_dev, PC263_DRIVER_NAME
-			": BUG! cannot determine board type!\n");
-		return -EINVAL;
-	}
-}
-
-/*
- * The auto_attach hook is called at PCI probe time via
- * comedi_pci_auto_config().  dev->board_ptr is NULL on entry.
- * There should be a board entry matching the supplied PCI device.
- */
-static int pc263_auto_attach(struct comedi_device *dev,
-				       unsigned long context_unused)
-{
-	struct pci_dev *pci_dev;
-
-	if (!DO_PCI)
-		return -EINVAL;
-
-	pci_dev = comedi_to_pci_dev(dev);
-	dev_info(dev->class_dev, PC263_DRIVER_NAME ": attach pci %s\n",
-		 pci_name(pci_dev));
-	dev->board_ptr = pc263_find_pci_board(pci_dev);
-	if (dev->board_ptr == NULL) {
-		dev_err(dev->class_dev, "BUG! cannot determine board type!\n");
-		return -EINVAL;
-	}
-	/*
-	 * Need to 'get' the PCI device to match the 'put' in pc263_detach().
-	 * TODO: Remove the pci_dev_get() and matching pci_dev_put() once
-	 * support for manual attachment of PCI devices via pc263_attach()
-	 * has been removed.
-	 */
-	pci_dev_get(pci_dev);
-	return pc263_pci_common_attach(dev, pci_dev);
-}
-
-static void pc263_detach(struct comedi_device *dev)
-{
-	const struct pc263_board *thisboard = comedi_board(dev);
-
-	if (!thisboard)
-		return;
-	if (is_isa_board(thisboard)) {
-		if (dev->iobase)
-			release_region(dev->iobase, PC263_IO_SIZE);
-	} else if (is_pci_board(thisboard)) {
-		struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-		if (pcidev) {
-			if (dev->iobase)
-				comedi_pci_disable(pcidev);
-			pci_dev_put(pcidev);
-		}
-	}
+	dev_info(dev->class_dev, "%s (base %#lx) attached\n", dev->board_name,
+		 dev->iobase);
+	return 0;
 }
 
-/*
- * The struct comedi_driver structure tells the Comedi core module
- * which functions to call to configure/deconfigure (attach/detach)
- * the board, and also about the kernel module that contains
- * the device code.
- */
 static struct comedi_driver amplc_pc263_driver = {
 	.driver_name = PC263_DRIVER_NAME,
 	.module = THIS_MODULE,
 	.attach = pc263_attach,
-	.auto_attach = pc263_auto_attach,
-	.detach = pc263_detach,
+	.detach = comedi_legacy_detach,
 	.board_name = &pc263_boards[0].name,
 	.offset = sizeof(struct pc263_board),
 	.num_names = ARRAY_SIZE(pc263_boards),
 };
 
-#if DO_PCI
-static DEFINE_PCI_DEVICE_TABLE(pc263_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI263) },
-	{0}
-};
-MODULE_DEVICE_TABLE(pci, pc263_pci_table);
-
-static int amplc_pc263_pci_probe(struct pci_dev *dev,
-						  const struct pci_device_id
-						  *ent)
-{
-	return comedi_pci_auto_config(dev, &amplc_pc263_driver);
-}
-
-static struct pci_driver amplc_pc263_pci_driver = {
-	.name = PC263_DRIVER_NAME,
-	.id_table = pc263_pci_table,
-	.probe = &amplc_pc263_pci_probe,
-	.remove		= comedi_pci_auto_unconfig,
-};
-module_comedi_pci_driver(amplc_pc263_driver, amplc_pc263_pci_driver);
-#else
 module_comedi_driver(amplc_pc263_driver);
-#endif
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
+MODULE_DESCRIPTION("Comedi driver for Amplicon PC263 relay board");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/amplc_pci224.c b/drivers/staging/comedi/drivers/amplc_pci224.c
index c9da4cd74ba..4d7eab9b556 100644
--- a/drivers/staging/comedi/drivers/amplc_pci224.c
+++ b/drivers/staging/comedi/drivers/amplc_pci224.c
@@ -1280,13 +1280,10 @@ static int pci224_attach_common(struct comedi_device *dev,
 
 	comedi_set_hw_dev(dev, &pci_dev->dev);
 
-	ret = comedi_pci_enable(pci_dev, DRIVER_NAME);
-	if (ret < 0) {
-		dev_err(dev->class_dev,
-			"error! cannot enable PCI device and request regions!\n"
-			);
+	ret = comedi_pci_enable(dev);
+	if (ret)
 		return ret;
-	}
+
 	spin_lock_init(&devpriv->ao_spinlock);
 
 	devpriv->iobase1 = pci_resource_start(pci_dev, 2);
@@ -1488,11 +1485,9 @@ static void pci224_detach(struct comedi_device *dev)
 		kfree(devpriv->ao_scan_vals);
 		kfree(devpriv->ao_scan_order);
 	}
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
+	comedi_pci_disable(dev);
+	if (pcidev)
 		pci_dev_put(pcidev);
-	}
 }
 
 static struct comedi_driver amplc_pci224_driver = {
@@ -1507,10 +1502,10 @@ static struct comedi_driver amplc_pci224_driver = {
 };
 
 static int amplc_pci224_pci_probe(struct pci_dev *dev,
-						   const struct pci_device_id
-						   *ent)
+				  const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &amplc_pci224_driver);
+	return comedi_pci_auto_config(dev, &amplc_pci224_driver,
+				      id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(amplc_pci224_pci_table) = {
diff --git a/drivers/staging/comedi/drivers/amplc_pci230.c b/drivers/staging/comedi/drivers/amplc_pci230.c
index e2244c6e536..49200fbd60b 100644
--- a/drivers/staging/comedi/drivers/amplc_pci230.c
+++ b/drivers/staging/comedi/drivers/amplc_pci230.c
@@ -2645,12 +2645,11 @@ static int pci230_attach_common(struct comedi_device *dev,
 	comedi_set_hw_dev(dev, &pci_dev->dev);
 
 	dev->board_name = thisboard->name;
-	/* Enable PCI device and reserve I/O spaces. */
-	if (comedi_pci_enable(pci_dev, "amplc_pci230") < 0) {
-		dev_err(dev->class_dev,
-			"failed to enable PCI device and request regions\n");
-		return -EIO;
-	}
+
+	rc = comedi_pci_enable(dev);
+	if (rc)
+		return rc;
+
 	/* Read base addresses of the PCI230's two I/O regions from PCI
 	 * configuration register. */
 	iobase1 = pci_resource_start(pci_dev, 2);
@@ -2833,18 +2832,14 @@ static int pci230_auto_attach(struct comedi_device *dev,
 
 static void pci230_detach(struct comedi_device *dev)
 {
-	const struct pci230_board *thisboard = comedi_board(dev);
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 
-	if (dev->subdevices && thisboard->have_dio)
-		subdev_8255_cleanup(dev, &dev->subdevices[2]);
+	comedi_spriv_free(dev, 2);
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
+	comedi_pci_disable(dev);
+	if (pcidev)
 		pci_dev_put(pcidev);
-	}
 }
 
 static struct comedi_driver amplc_pci230_driver = {
@@ -2859,9 +2854,10 @@ static struct comedi_driver amplc_pci230_driver = {
 };
 
 static int amplc_pci230_pci_probe(struct pci_dev *dev,
-					    const struct pci_device_id *ent)
+				  const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &amplc_pci230_driver);
+	return comedi_pci_auto_config(dev, &amplc_pci230_driver,
+				      id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(amplc_pci230_pci_table) = {
diff --git a/drivers/staging/comedi/drivers/amplc_pci263.c b/drivers/staging/comedi/drivers/amplc_pci263.c
new file mode 100644
index 00000000000..8b57533bf40
--- /dev/null
+++ b/drivers/staging/comedi/drivers/amplc_pci263.c
@@ -0,0 +1,127 @@
+/*
+    comedi/drivers/amplc_pci263.c
+    Driver for Amplicon PCI263 relay board.
+
+    Copyright (C) 2002 MEV Ltd. <http://www.mev.co.uk/>
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 2000 David A. Schleef <ds@schleef.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.
+
+*/
+/*
+Driver: amplc_pci263
+Description: Amplicon PCI263
+Author: Ian Abbott <abbotti@mev.co.uk>
+Devices: [Amplicon] PCI263 (amplc_pci263)
+Updated: Fri, 12 Apr 2013 15:19:36 +0100
+Status: works
+
+Configuration options: not applicable, uses PCI auto config
+
+The board appears as one subdevice, with 16 digital outputs, each
+connected to a reed-relay. Relay contacts are closed when output is 1.
+The state of the outputs can be read.
+*/
+
+#include <linux/pci.h>
+
+#include "../comedidev.h"
+
+#define PCI263_DRIVER_NAME	"amplc_pci263"
+
+/* PCI263 PCI configuration register information */
+#define PCI_DEVICE_ID_AMPLICON_PCI263 0x000c
+
+static int pci263_do_insn_bits(struct comedi_device *dev,
+			       struct comedi_subdevice *s,
+			       struct comedi_insn *insn, unsigned int *data)
+{
+	/* The insn data is a mask in data[0] and the new data
+	 * in data[1], each channel cooresponding to a bit. */
+	if (data[0]) {
+		s->state &= ~data[0];
+		s->state |= data[0] & data[1];
+		/* Write out the new digital output lines */
+		outb(s->state & 0xFF, dev->iobase);
+		outb(s->state >> 8, dev->iobase + 1);
+	}
+	return insn->n;
+}
+
+static int pci263_auto_attach(struct comedi_device *dev,
+			      unsigned long context_unused)
+{
+	struct pci_dev *pci_dev = comedi_to_pci_dev(dev);
+	struct comedi_subdevice *s;
+	int ret;
+
+	ret = comedi_pci_enable(dev);
+	if (ret)
+		return ret;
+
+	dev->iobase = pci_resource_start(pci_dev, 2);
+	ret = comedi_alloc_subdevices(dev, 1);
+	if (ret)
+		return ret;
+
+	s = &dev->subdevices[0];
+	/* digital output subdevice */
+	s->type = COMEDI_SUBD_DO;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->n_chan = 16;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_bits = pci263_do_insn_bits;
+	/* read initial relay state */
+	s->state = inb(dev->iobase) | (inb(dev->iobase + 1) << 8);
+
+	dev_info(dev->class_dev, "%s (pci %s) attached\n", dev->board_name,
+		 pci_name(pci_dev));
+	return 0;
+}
+
+static struct comedi_driver amplc_pci263_driver = {
+	.driver_name	= PCI263_DRIVER_NAME,
+	.module		= THIS_MODULE,
+	.auto_attach	= pci263_auto_attach,
+	.detach		= comedi_pci_disable,
+};
+
+static DEFINE_PCI_DEVICE_TABLE(pci263_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI263) },
+	{0}
+};
+MODULE_DEVICE_TABLE(pci, pci263_pci_table);
+
+static int amplc_pci263_pci_probe(struct pci_dev *dev,
+				  const struct pci_device_id *id)
+{
+	return comedi_pci_auto_config(dev, &amplc_pci263_driver,
+				      id->driver_data);
+}
+
+static struct pci_driver amplc_pci263_pci_driver = {
+	.name		= PCI263_DRIVER_NAME,
+	.id_table	= pci263_pci_table,
+	.probe		= &amplc_pci263_pci_probe,
+	.remove		= comedi_pci_auto_unconfig,
+};
+module_comedi_pci_driver(amplc_pci263_driver, amplc_pci263_pci_driver);
+
+MODULE_AUTHOR("Comedi http://www.comedi.org");
+MODULE_DESCRIPTION("Comedi driver for Amplicon PCI263 relay board");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/c6xdigio.c b/drivers/staging/comedi/drivers/c6xdigio.c
index 070037c22db..92376dc86dd 100644
--- a/drivers/staging/comedi/drivers/c6xdigio.c
+++ b/drivers/staging/comedi/drivers/c6xdigio.c
@@ -397,12 +397,6 @@ static void board_init(struct comedi_device *dev)
 
 }
 
-/*
-   options[0] - I/O port
-   options[1] - irq
-   options[2] - number of encoder chips installed
- */
-
 static const struct pnp_device_id c6xdigio_pnp_tbl[] = {
 	/* Standard LPT Printer Port */
 	{.id = "PNP0400", .driver_data = 0},
@@ -419,34 +413,20 @@ static struct pnp_driver c6xdigio_pnp_driver = {
 static int c6xdigio_attach(struct comedi_device *dev,
 			   struct comedi_devconfig *it)
 {
-	int result = 0;
-	unsigned long iobase;
-	unsigned int irq;
 	struct comedi_subdevice *s;
+	int ret;
 
-	iobase = it->options[0];
-	printk(KERN_DEBUG "comedi%d: c6xdigio: 0x%04lx\n", dev->minor, iobase);
-	if (!request_region(iobase, C6XDIGIO_SIZE, "c6xdigio")) {
-		printk(KERN_ERR "comedi%d: I/O port conflict\n", dev->minor);
-		return -EIO;
-	}
-	dev->iobase = iobase;
-	dev->board_name = "c6xdigio";
+	ret = comedi_request_region(dev, it->options[0], C6XDIGIO_SIZE);
+	if (ret)
+		return ret;
 
-	result = comedi_alloc_subdevices(dev, 2);
-	if (result)
-		return result;
+	ret = comedi_alloc_subdevices(dev, 2);
+	if (ret)
+		return ret;
 
 	/*  Make sure that PnP ports get activated */
 	pnp_register_driver(&c6xdigio_pnp_driver);
 
-	irq = it->options[1];
-	if (irq > 0)
-		printk(KERN_DEBUG "comedi%d: irq = %u ignored\n",
-				dev->minor, irq);
-	else if (irq == 0)
-		printk(KERN_DEBUG "comedi%d: no irq\n", dev->minor);
-
 	s = &dev->subdevices[0];
 	/* pwm output subdevice */
 	s->type = COMEDI_SUBD_AO;	/*  Not sure what to put here */
@@ -488,10 +468,7 @@ static int c6xdigio_attach(struct comedi_device *dev,
 
 static void c6xdigio_detach(struct comedi_device *dev)
 {
-	if (dev->iobase)
-		release_region(dev->iobase, C6XDIGIO_SIZE);
-	if (dev->irq)
-		free_irq(dev->irq, dev);
+	comedi_legacy_detach(dev);
 	pnp_unregister_driver(&c6xdigio_pnp_driver);
 }
 
diff --git a/drivers/staging/comedi/drivers/cb_pcidas.c b/drivers/staging/comedi/drivers/cb_pcidas.c
index 79c72118a09..53dd298d2b5 100644
--- a/drivers/staging/comedi/drivers/cb_pcidas.c
+++ b/drivers/staging/comedi/drivers/cb_pcidas.c
@@ -231,9 +231,19 @@ enum trimpot_model {
 	AD8402,
 };
 
+enum cb_pcidas_boardid {
+	BOARD_PCIDAS1602_16,
+	BOARD_PCIDAS1200,
+	BOARD_PCIDAS1602_12,
+	BOARD_PCIDAS1200_JR,
+	BOARD_PCIDAS1602_16_JR,
+	BOARD_PCIDAS1000,
+	BOARD_PCIDAS1001,
+	BOARD_PCIDAS1002,
+};
+
 struct cb_pcidas_board {
 	const char *name;
-	unsigned short device_id;
 	int ai_nchan;		/*  Inputs in single-ended mode */
 	int ai_bits;		/*  analog input resolution */
 	int ai_speed;		/*  fastest conversion period in ns */
@@ -248,9 +258,8 @@ struct cb_pcidas_board {
 };
 
 static const struct cb_pcidas_board cb_pcidas_boards[] = {
-	{
+	[BOARD_PCIDAS1602_16] = {
 		.name		= "pci-das1602/16",
-		.device_id	= 0x1,
 		.ai_nchan	= 16,
 		.ai_bits	= 16,
 		.ai_speed	= 5000,
@@ -262,9 +271,9 @@ static const struct cb_pcidas_board cb_pcidas_boards[] = {
 		.trimpot	= AD8402,
 		.has_dac08	= 1,
 		.is_1602	= 1,
-	}, {
+	},
+	[BOARD_PCIDAS1200] = {
 		.name		= "pci-das1200",
-		.device_id	= 0xF,
 		.ai_nchan	= 16,
 		.ai_bits	= 12,
 		.ai_speed	= 3200,
@@ -272,9 +281,9 @@ static const struct cb_pcidas_board cb_pcidas_boards[] = {
 		.fifo_size	= 1024,
 		.ranges		= &cb_pcidas_ranges,
 		.trimpot	= AD7376,
-	}, {
+	},
+	[BOARD_PCIDAS1602_12] = {
 		.name		= "pci-das1602/12",
-		.device_id	= 0x10,
 		.ai_nchan	= 16,
 		.ai_bits	= 12,
 		.ai_speed	= 3200,
@@ -285,18 +294,18 @@ static const struct cb_pcidas_board cb_pcidas_boards[] = {
 		.ranges		= &cb_pcidas_ranges,
 		.trimpot	= AD7376,
 		.is_1602	= 1,
-	}, {
+	},
+	[BOARD_PCIDAS1200_JR] = {
 		.name		= "pci-das1200/jr",
-		.device_id	= 0x19,
 		.ai_nchan	= 16,
 		.ai_bits	= 12,
 		.ai_speed	= 3200,
 		.fifo_size	= 1024,
 		.ranges		= &cb_pcidas_ranges,
 		.trimpot	= AD7376,
-	}, {
+	},
+	[BOARD_PCIDAS1602_16_JR] = {
 		.name		= "pci-das1602/16/jr",
-		.device_id	= 0x1C,
 		.ai_nchan	= 16,
 		.ai_bits	= 16,
 		.ai_speed	= 5000,
@@ -305,18 +314,18 @@ static const struct cb_pcidas_board cb_pcidas_boards[] = {
 		.trimpot	= AD8402,
 		.has_dac08	= 1,
 		.is_1602	= 1,
-	}, {
+	},
+	[BOARD_PCIDAS1000] = {
 		.name		= "pci-das1000",
-		.device_id	= 0x4C,
 		.ai_nchan	= 16,
 		.ai_bits	= 12,
 		.ai_speed	= 4000,
 		.fifo_size	= 1024,
 		.ranges		= &cb_pcidas_ranges,
 		.trimpot	= AD7376,
-	}, {
+	},
+	[BOARD_PCIDAS1001] = {
 		.name		= "pci-das1001",
-		.device_id	= 0x1a,
 		.ai_nchan	= 16,
 		.ai_bits	= 12,
 		.ai_speed	= 6800,
@@ -324,9 +333,9 @@ static const struct cb_pcidas_board cb_pcidas_boards[] = {
 		.fifo_size	= 1024,
 		.ranges		= &cb_pcidas_alt_ranges,
 		.trimpot	= AD7376,
-	}, {
+	},
+	[BOARD_PCIDAS1002] = {
 		.name		= "pci-das1002",
-		.device_id	= 0x1b,
 		.ai_nchan	= 16,
 		.ai_bits	= 12,
 		.ai_speed	= 6800,
@@ -1332,7 +1341,7 @@ static irqreturn_t cb_pcidas_interrupt(int irq, void *d)
 	static const int timeout = 10000;
 	unsigned long flags;
 
-	if (dev->attached == 0)
+	if (!dev->attached)
 		return IRQ_NONE;
 
 	async = s->async;
@@ -1424,31 +1433,18 @@ static irqreturn_t cb_pcidas_interrupt(int irq, void *d)
 	return IRQ_HANDLED;
 }
 
-static const void *cb_pcidas_find_boardinfo(struct comedi_device *dev,
-					    struct pci_dev *pcidev)
-{
-	const struct cb_pcidas_board *thisboard;
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(cb_pcidas_boards); i++) {
-		thisboard = &cb_pcidas_boards[i];
-		if (thisboard->device_id == pcidev->device)
-			return thisboard;
-	}
-	return NULL;
-}
-
 static int cb_pcidas_auto_attach(struct comedi_device *dev,
-					   unsigned long context_unused)
+				 unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct cb_pcidas_board *thisboard;
+	const struct cb_pcidas_board *thisboard = NULL;
 	struct cb_pcidas_private *devpriv;
 	struct comedi_subdevice *s;
 	int i;
 	int ret;
 
-	thisboard = cb_pcidas_find_boardinfo(dev, pcidev);
+	if (context < ARRAY_SIZE(cb_pcidas_boards))
+		thisboard = &cb_pcidas_boards[context];
 	if (!thisboard)
 		return -ENODEV;
 	dev->board_ptr  = thisboard;
@@ -1459,7 +1455,7 @@ static int cb_pcidas_auto_attach(struct comedi_device *dev,
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
 
@@ -1603,7 +1599,6 @@ static int cb_pcidas_auto_attach(struct comedi_device *dev,
 static void cb_pcidas_detach(struct comedi_device *dev)
 {
 	struct cb_pcidas_private *devpriv = dev->private;
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 
 	if (devpriv) {
 		if (devpriv->s5933_config) {
@@ -1613,12 +1608,8 @@ static void cb_pcidas_detach(struct comedi_device *dev)
 	}
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-	if (dev->subdevices)
-		subdev_8255_cleanup(dev, &dev->subdevices[2]);
-	if (pcidev) {
-		if (devpriv->s5933_config)
-			comedi_pci_disable(pcidev);
-	}
+	comedi_spriv_free(dev, 2);
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver cb_pcidas_driver = {
@@ -1629,20 +1620,21 @@ static struct comedi_driver cb_pcidas_driver = {
 };
 
 static int cb_pcidas_pci_probe(struct pci_dev *dev,
-					 const struct pci_device_id *ent)
+			       const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &cb_pcidas_driver);
+	return comedi_pci_auto_config(dev, &cb_pcidas_driver,
+				      id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(cb_pcidas_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0001) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x000f) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0010) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0019) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x001c) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x004c) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x001a) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x001b) },
+	{ PCI_VDEVICE(CB, 0x0001), BOARD_PCIDAS1602_16 },
+	{ PCI_VDEVICE(CB, 0x000f), BOARD_PCIDAS1200 },
+	{ PCI_VDEVICE(CB, 0x0010), BOARD_PCIDAS1602_12 },
+	{ PCI_VDEVICE(CB, 0x0019), BOARD_PCIDAS1200_JR },
+	{ PCI_VDEVICE(CB, 0x001c), BOARD_PCIDAS1602_16_JR },
+	{ PCI_VDEVICE(CB, 0x004c), BOARD_PCIDAS1000 },
+	{ PCI_VDEVICE(CB, 0x001a), BOARD_PCIDAS1001 },
+	{ PCI_VDEVICE(CB, 0x001b), BOARD_PCIDAS1002 },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, cb_pcidas_pci_table);
diff --git a/drivers/staging/comedi/drivers/cb_pcidas64.c b/drivers/staging/comedi/drivers/cb_pcidas64.c
index 9f3112cb7a2..c3e5495b4f0 100644
--- a/drivers/staging/comedi/drivers/cb_pcidas64.c
+++ b/drivers/staging/comedi/drivers/cb_pcidas64.c
@@ -118,13 +118,6 @@ static const int max_counter_value = 0xffffff;
 
 /* PCI-DAS64xxx base addresses */
 
-/* indices of base address regions */
-enum base_address_regions {
-	PLX9080_BADDRINDEX = 0,
-	MAIN_BADDRINDEX = 2,
-	DIO_COUNTER_BADDRINDEX = 3,
-};
-
 /* devpriv->main_iobase registers */
 enum write_only_registers {
 	INTR_ENABLE_REG = 0x0,	/*  interrupt enable register */
@@ -543,13 +536,6 @@ static const int ao_range_code_64xx[] = {
 	0x3,
 };
 
-static const struct comedi_lrange ao_ranges_60xx = {
-	1,
-	{
-	 BIP_RANGE(10),
-	 }
-};
-
 static const int ao_range_code_60xx[] = {
 	0x0,
 };
@@ -593,9 +579,39 @@ struct hw_fifo_info {
 	uint16_t fifo_size_reg_mask;
 };
 
+enum pcidas64_boardid {
+	BOARD_PCIDAS6402_16,
+	BOARD_PCIDAS6402_12,
+	BOARD_PCIDAS64_M1_16,
+	BOARD_PCIDAS64_M2_16,
+	BOARD_PCIDAS64_M3_16,
+	BOARD_PCIDAS6013,
+	BOARD_PCIDAS6014,
+	BOARD_PCIDAS6023,
+	BOARD_PCIDAS6025,
+	BOARD_PCIDAS6030,
+	BOARD_PCIDAS6031,
+	BOARD_PCIDAS6032,
+	BOARD_PCIDAS6033,
+	BOARD_PCIDAS6034,
+	BOARD_PCIDAS6035,
+	BOARD_PCIDAS6036,
+	BOARD_PCIDAS6040,
+	BOARD_PCIDAS6052,
+	BOARD_PCIDAS6070,
+	BOARD_PCIDAS6071,
+	BOARD_PCIDAS4020_12,
+	BOARD_PCIDAS6402_16_JR,
+	BOARD_PCIDAS64_M1_16_JR,
+	BOARD_PCIDAS64_M2_16_JR,
+	BOARD_PCIDAS64_M3_16_JR,
+	BOARD_PCIDAS64_M1_14,
+	BOARD_PCIDAS64_M2_14,
+	BOARD_PCIDAS64_M3_14,
+};
+
 struct pcidas64_board {
 	const char *name;
-	int device_id;		/*  pci device id */
 	int ai_se_chans;	/*  number of ai inputs in single-ended mode */
 	int ai_bits;		/*  analog input resolution */
 	int ai_speed;		/*  fastest conversion period in ns */
@@ -648,421 +664,397 @@ static inline unsigned int ai_dma_ring_count(const struct pcidas64_board *board)
 static const int bytes_in_sample = 2;
 
 static const struct pcidas64_board pcidas64_boards[] = {
-	{
-	 .name = "pci-das6402/16",
-	 .device_id = 0x1d,
-	 .ai_se_chans = 64,
-	 .ai_bits = 16,
-	 .ai_speed = 5000,
-	 .ao_nchan = 2,
-	 .ao_bits = 16,
-	 .ao_scan_speed = 10000,
-	 .layout = LAYOUT_64XX,
-	 .ai_range_table = &ai_ranges_64xx,
-	 .ao_range_table = &ao_ranges_64xx,
-	 .ao_range_code = ao_range_code_64xx,
-	 .ai_fifo = &ai_fifo_64xx,
-	 .has_8255 = 1,
-	 },
-	{
-	 .name = "pci-das6402/12",	/*  XXX check */
-	 .device_id = 0x1e,
-	 .ai_se_chans = 64,
-	 .ai_bits = 12,
-	 .ai_speed = 5000,
-	 .ao_nchan = 2,
-	 .ao_bits = 12,
-	 .ao_scan_speed = 10000,
-	 .layout = LAYOUT_64XX,
-	 .ai_range_table = &ai_ranges_64xx,
-	 .ao_range_table = &ao_ranges_64xx,
-	 .ao_range_code = ao_range_code_64xx,
-	 .ai_fifo = &ai_fifo_64xx,
-	 .has_8255 = 1,
-	 },
-	{
-	 .name = "pci-das64/m1/16",
-	 .device_id = 0x35,
-	 .ai_se_chans = 64,
-	 .ai_bits = 16,
-	 .ai_speed = 1000,
-	 .ao_nchan = 2,
-	 .ao_bits = 16,
-	 .ao_scan_speed = 10000,
-	 .layout = LAYOUT_64XX,
-	 .ai_range_table = &ai_ranges_64xx,
-	 .ao_range_table = &ao_ranges_64xx,
-	 .ao_range_code = ao_range_code_64xx,
-	 .ai_fifo = &ai_fifo_64xx,
-	 .has_8255 = 1,
-	 },
-	{
-	 .name = "pci-das64/m2/16",
-	 .device_id = 0x36,
-	 .ai_se_chans = 64,
-	 .ai_bits = 16,
-	 .ai_speed = 500,
-	 .ao_nchan = 2,
-	 .ao_bits = 16,
-	 .ao_scan_speed = 10000,
-	 .layout = LAYOUT_64XX,
-	 .ai_range_table = &ai_ranges_64xx,
-	 .ao_range_table = &ao_ranges_64xx,
-	 .ao_range_code = ao_range_code_64xx,
-	 .ai_fifo = &ai_fifo_64xx,
-	 .has_8255 = 1,
-	 },
-	{
-	 .name = "pci-das64/m3/16",
-	 .device_id = 0x37,
-	 .ai_se_chans = 64,
-	 .ai_bits = 16,
-	 .ai_speed = 333,
-	 .ao_nchan = 2,
-	 .ao_bits = 16,
-	 .ao_scan_speed = 10000,
-	 .layout = LAYOUT_64XX,
-	 .ai_range_table = &ai_ranges_64xx,
-	 .ao_range_table = &ao_ranges_64xx,
-	 .ao_range_code = ao_range_code_64xx,
-	 .ai_fifo = &ai_fifo_64xx,
-	 .has_8255 = 1,
-	 },
-	{
-	 .name = "pci-das6013",
-	 .device_id = 0x78,
-	 .ai_se_chans = 16,
-	 .ai_bits = 16,
-	 .ai_speed = 5000,
-	 .ao_nchan = 0,
-	 .ao_bits = 16,
-	 .layout = LAYOUT_60XX,
-	 .ai_range_table = &ai_ranges_60xx,
-	 .ao_range_table = &ao_ranges_60xx,
-	 .ao_range_code = ao_range_code_60xx,
-	 .ai_fifo = &ai_fifo_60xx,
-	 .has_8255 = 0,
-	 },
-	{
-	 .name = "pci-das6014",
-	 .device_id = 0x79,
-	 .ai_se_chans = 16,
-	 .ai_bits = 16,
-	 .ai_speed = 5000,
-	 .ao_nchan = 2,
-	 .ao_bits = 16,
-	 .ao_scan_speed = 100000,
-	 .layout = LAYOUT_60XX,
-	 .ai_range_table = &ai_ranges_60xx,
-	 .ao_range_table = &ao_ranges_60xx,
-	 .ao_range_code = ao_range_code_60xx,
-	 .ai_fifo = &ai_fifo_60xx,
-	 .has_8255 = 0,
-	 },
-	{
-	 .name = "pci-das6023",
-	 .device_id = 0x5d,
-	 .ai_se_chans = 16,
-	 .ai_bits = 12,
-	 .ai_speed = 5000,
-	 .ao_nchan = 0,
-	 .ao_scan_speed = 100000,
-	 .layout = LAYOUT_60XX,
-	 .ai_range_table = &ai_ranges_60xx,
-	 .ao_range_table = &ao_ranges_60xx,
-	 .ao_range_code = ao_range_code_60xx,
-	 .ai_fifo = &ai_fifo_60xx,
-	 .has_8255 = 1,
-	 },
-	{
-	 .name = "pci-das6025",
-	 .device_id = 0x5e,
-	 .ai_se_chans = 16,
-	 .ai_bits = 12,
-	 .ai_speed = 5000,
-	 .ao_nchan = 2,
-	 .ao_bits = 12,
-	 .ao_scan_speed = 100000,
-	 .layout = LAYOUT_60XX,
-	 .ai_range_table = &ai_ranges_60xx,
-	 .ao_range_table = &ao_ranges_60xx,
-	 .ao_range_code = ao_range_code_60xx,
-	 .ai_fifo = &ai_fifo_60xx,
-	 .has_8255 = 1,
-	 },
-	{
-	 .name = "pci-das6030",
-	 .device_id = 0x5f,
-	 .ai_se_chans = 16,
-	 .ai_bits = 16,
-	 .ai_speed = 10000,
-	 .ao_nchan = 2,
-	 .ao_bits = 16,
-	 .ao_scan_speed = 10000,
-	 .layout = LAYOUT_60XX,
-	 .ai_range_table = &ai_ranges_6030,
-	 .ao_range_table = &ao_ranges_6030,
-	 .ao_range_code = ao_range_code_6030,
-	 .ai_fifo = &ai_fifo_60xx,
-	 .has_8255 = 0,
-	 },
-	{
-	 .name = "pci-das6031",
-	 .device_id = 0x60,
-	 .ai_se_chans = 64,
-	 .ai_bits = 16,
-	 .ai_speed = 10000,
-	 .ao_nchan = 2,
-	 .ao_bits = 16,
-	 .ao_scan_speed = 10000,
-	 .layout = LAYOUT_60XX,
-	 .ai_range_table = &ai_ranges_6030,
-	 .ao_range_table = &ao_ranges_6030,
-	 .ao_range_code = ao_range_code_6030,
-	 .ai_fifo = &ai_fifo_60xx,
-	 .has_8255 = 0,
-	 },
-	{
-	 .name = "pci-das6032",
-	 .device_id = 0x61,
-	 .ai_se_chans = 16,
-	 .ai_bits = 16,
-	 .ai_speed = 10000,
-	 .ao_nchan = 0,
-	 .layout = LAYOUT_60XX,
-	 .ai_range_table = &ai_ranges_6030,
-	 .ai_fifo = &ai_fifo_60xx,
-	 .has_8255 = 0,
-	 },
-	{
-	 .name = "pci-das6033",
-	 .device_id = 0x62,
-	 .ai_se_chans = 64,
-	 .ai_bits = 16,
-	 .ai_speed = 10000,
-	 .ao_nchan = 0,
-	 .layout = LAYOUT_60XX,
-	 .ai_range_table = &ai_ranges_6030,
-	 .ai_fifo = &ai_fifo_60xx,
-	 .has_8255 = 0,
-	 },
-	{
-	 .name = "pci-das6034",
-	 .device_id = 0x63,
-	 .ai_se_chans = 16,
-	 .ai_bits = 16,
-	 .ai_speed = 5000,
-	 .ao_nchan = 0,
-	 .ao_scan_speed = 0,
-	 .layout = LAYOUT_60XX,
-	 .ai_range_table = &ai_ranges_60xx,
-	 .ai_fifo = &ai_fifo_60xx,
-	 .has_8255 = 0,
-	 },
-	{
-	 .name = "pci-das6035",
-	 .device_id = 0x64,
-	 .ai_se_chans = 16,
-	 .ai_bits = 16,
-	 .ai_speed = 5000,
-	 .ao_nchan = 2,
-	 .ao_bits = 12,
-	 .ao_scan_speed = 100000,
-	 .layout = LAYOUT_60XX,
-	 .ai_range_table = &ai_ranges_60xx,
-	 .ao_range_table = &ao_ranges_60xx,
-	 .ao_range_code = ao_range_code_60xx,
-	 .ai_fifo = &ai_fifo_60xx,
-	 .has_8255 = 0,
-	 },
-	{
-	 .name = "pci-das6036",
-	 .device_id = 0x6f,
-	 .ai_se_chans = 16,
-	 .ai_bits = 16,
-	 .ai_speed = 5000,
-	 .ao_nchan = 2,
-	 .ao_bits = 16,
-	 .ao_scan_speed = 100000,
-	 .layout = LAYOUT_60XX,
-	 .ai_range_table = &ai_ranges_60xx,
-	 .ao_range_table = &ao_ranges_60xx,
-	 .ao_range_code = ao_range_code_60xx,
-	 .ai_fifo = &ai_fifo_60xx,
-	 .has_8255 = 0,
-	 },
-	{
-	 .name = "pci-das6040",
-	 .device_id = 0x65,
-	 .ai_se_chans = 16,
-	 .ai_bits = 12,
-	 .ai_speed = 2000,
-	 .ao_nchan = 2,
-	 .ao_bits = 12,
-	 .ao_scan_speed = 1000,
-	 .layout = LAYOUT_60XX,
-	 .ai_range_table = &ai_ranges_6052,
-	 .ao_range_table = &ao_ranges_6030,
-	 .ao_range_code = ao_range_code_6030,
-	 .ai_fifo = &ai_fifo_60xx,
-	 .has_8255 = 0,
-	 },
-	{
-	 .name = "pci-das6052",
-	 .device_id = 0x66,
-	 .ai_se_chans = 16,
-	 .ai_bits = 16,
-	 .ai_speed = 3333,
-	 .ao_nchan = 2,
-	 .ao_bits = 16,
-	 .ao_scan_speed = 3333,
-	 .layout = LAYOUT_60XX,
-	 .ai_range_table = &ai_ranges_6052,
-	 .ao_range_table = &ao_ranges_6030,
-	 .ao_range_code = ao_range_code_6030,
-	 .ai_fifo = &ai_fifo_60xx,
-	 .has_8255 = 0,
-	 },
-	{
-	 .name = "pci-das6070",
-	 .device_id = 0x67,
-	 .ai_se_chans = 16,
-	 .ai_bits = 12,
-	 .ai_speed = 800,
-	 .ao_nchan = 2,
-	 .ao_bits = 12,
-	 .ao_scan_speed = 1000,
-	 .layout = LAYOUT_60XX,
-	 .ai_range_table = &ai_ranges_6052,
-	 .ao_range_table = &ao_ranges_6030,
-	 .ao_range_code = ao_range_code_6030,
-	 .ai_fifo = &ai_fifo_60xx,
-	 .has_8255 = 0,
-	 },
-	{
-	 .name = "pci-das6071",
-	 .device_id = 0x68,
-	 .ai_se_chans = 64,
-	 .ai_bits = 12,
-	 .ai_speed = 800,
-	 .ao_nchan = 2,
-	 .ao_bits = 12,
-	 .ao_scan_speed = 1000,
-	 .layout = LAYOUT_60XX,
-	 .ai_range_table = &ai_ranges_6052,
-	 .ao_range_table = &ao_ranges_6030,
-	 .ao_range_code = ao_range_code_6030,
-	 .ai_fifo = &ai_fifo_60xx,
-	 .has_8255 = 0,
-	 },
-	{
-	 .name = "pci-das4020/12",
-	 .device_id = 0x52,
-	 .ai_se_chans = 4,
-	 .ai_bits = 12,
-	 .ai_speed = 50,
-	 .ao_bits = 12,
-	 .ao_nchan = 2,
-	 .ao_scan_speed = 0,	/*  no hardware pacing on ao */
-	 .layout = LAYOUT_4020,
-	 .ai_range_table = &ai_ranges_4020,
-	 .ao_range_table = &ao_ranges_4020,
-	 .ao_range_code = ao_range_code_4020,
-	 .ai_fifo = &ai_fifo_4020,
-	 .has_8255 = 1,
-	 },
+	[BOARD_PCIDAS6402_16] = {
+		.name		= "pci-das6402/16",
+		.ai_se_chans	= 64,
+		.ai_bits	= 16,
+		.ai_speed	= 5000,
+		.ao_nchan	= 2,
+		.ao_bits	= 16,
+		.ao_scan_speed	= 10000,
+		.layout		= LAYOUT_64XX,
+		.ai_range_table	= &ai_ranges_64xx,
+		.ao_range_table	= &ao_ranges_64xx,
+		.ao_range_code	= ao_range_code_64xx,
+		.ai_fifo	= &ai_fifo_64xx,
+		.has_8255	= 1,
+	},
+	[BOARD_PCIDAS6402_12] = {
+		.name		= "pci-das6402/12",	/*  XXX check */
+		.ai_se_chans	= 64,
+		.ai_bits	= 12,
+		.ai_speed	= 5000,
+		.ao_nchan	= 2,
+		.ao_bits	= 12,
+		.ao_scan_speed	= 10000,
+		.layout		= LAYOUT_64XX,
+		.ai_range_table	= &ai_ranges_64xx,
+		.ao_range_table	= &ao_ranges_64xx,
+		.ao_range_code	= ao_range_code_64xx,
+		.ai_fifo	= &ai_fifo_64xx,
+		.has_8255	= 1,
+	},
+	[BOARD_PCIDAS64_M1_16] = {
+		.name		= "pci-das64/m1/16",
+		.ai_se_chans	= 64,
+		.ai_bits	= 16,
+		.ai_speed	= 1000,
+		.ao_nchan	= 2,
+		.ao_bits	= 16,
+		.ao_scan_speed	= 10000,
+		.layout		= LAYOUT_64XX,
+		.ai_range_table	= &ai_ranges_64xx,
+		.ao_range_table	= &ao_ranges_64xx,
+		.ao_range_code	= ao_range_code_64xx,
+		.ai_fifo	= &ai_fifo_64xx,
+		.has_8255	= 1,
+	},
+	[BOARD_PCIDAS64_M2_16] = {
+		.name = "pci-das64/m2/16",
+		.ai_se_chans	= 64,
+		.ai_bits	= 16,
+		.ai_speed	= 500,
+		.ao_nchan	= 2,
+		.ao_bits	= 16,
+		.ao_scan_speed	= 10000,
+		.layout		= LAYOUT_64XX,
+		.ai_range_table	= &ai_ranges_64xx,
+		.ao_range_table	= &ao_ranges_64xx,
+		.ao_range_code	= ao_range_code_64xx,
+		.ai_fifo	= &ai_fifo_64xx,
+		.has_8255	= 1,
+	},
+	[BOARD_PCIDAS64_M3_16] = {
+		.name		= "pci-das64/m3/16",
+		.ai_se_chans	= 64,
+		.ai_bits	= 16,
+		.ai_speed	= 333,
+		.ao_nchan	= 2,
+		.ao_bits	= 16,
+		.ao_scan_speed	= 10000,
+		.layout		= LAYOUT_64XX,
+		.ai_range_table	= &ai_ranges_64xx,
+		.ao_range_table	= &ao_ranges_64xx,
+		.ao_range_code	= ao_range_code_64xx,
+		.ai_fifo	= &ai_fifo_64xx,
+		.has_8255	= 1,
+	},
+	[BOARD_PCIDAS6013] = {
+		.name		= "pci-das6013",
+		.ai_se_chans	= 16,
+		.ai_bits	= 16,
+		.ai_speed	= 5000,
+		.ao_nchan	= 0,
+		.ao_bits	= 16,
+		.layout		= LAYOUT_60XX,
+		.ai_range_table	= &ai_ranges_60xx,
+		.ao_range_table	= &range_bipolar10,
+		.ao_range_code	= ao_range_code_60xx,
+		.ai_fifo	= &ai_fifo_60xx,
+		.has_8255	= 0,
+	},
+	[BOARD_PCIDAS6014] = {
+		.name		= "pci-das6014",
+		.ai_se_chans	= 16,
+		.ai_bits	= 16,
+		.ai_speed	= 5000,
+		.ao_nchan	= 2,
+		.ao_bits	= 16,
+		.ao_scan_speed	= 100000,
+		.layout		= LAYOUT_60XX,
+		.ai_range_table	= &ai_ranges_60xx,
+		.ao_range_table	= &range_bipolar10,
+		.ao_range_code	= ao_range_code_60xx,
+		.ai_fifo	= &ai_fifo_60xx,
+		.has_8255	= 0,
+	},
+	[BOARD_PCIDAS6023] = {
+		.name		= "pci-das6023",
+		.ai_se_chans	= 16,
+		.ai_bits	= 12,
+		.ai_speed	= 5000,
+		.ao_nchan	= 0,
+		.ao_scan_speed	= 100000,
+		.layout		= LAYOUT_60XX,
+		.ai_range_table	= &ai_ranges_60xx,
+		.ao_range_table	= &range_bipolar10,
+		.ao_range_code	= ao_range_code_60xx,
+		.ai_fifo	= &ai_fifo_60xx,
+		.has_8255	= 1,
+	},
+	[BOARD_PCIDAS6025] = {
+		.name		= "pci-das6025",
+		.ai_se_chans	= 16,
+		.ai_bits	= 12,
+		.ai_speed	= 5000,
+		.ao_nchan	= 2,
+		.ao_bits	= 12,
+		.ao_scan_speed	= 100000,
+		.layout		= LAYOUT_60XX,
+		.ai_range_table	= &ai_ranges_60xx,
+		.ao_range_table	= &range_bipolar10,
+		.ao_range_code	= ao_range_code_60xx,
+		.ai_fifo	= &ai_fifo_60xx,
+		.has_8255	= 1,
+	},
+	[BOARD_PCIDAS6030] = {
+		.name		= "pci-das6030",
+		.ai_se_chans	= 16,
+		.ai_bits	= 16,
+		.ai_speed	= 10000,
+		.ao_nchan	= 2,
+		.ao_bits	= 16,
+		.ao_scan_speed	= 10000,
+		.layout		= LAYOUT_60XX,
+		.ai_range_table	= &ai_ranges_6030,
+		.ao_range_table	= &ao_ranges_6030,
+		.ao_range_code	= ao_range_code_6030,
+		.ai_fifo	= &ai_fifo_60xx,
+		.has_8255	= 0,
+	},
+	[BOARD_PCIDAS6031] = {
+		.name		= "pci-das6031",
+		.ai_se_chans	= 64,
+		.ai_bits	= 16,
+		.ai_speed	= 10000,
+		.ao_nchan	= 2,
+		.ao_bits	= 16,
+		.ao_scan_speed	= 10000,
+		.layout		= LAYOUT_60XX,
+		.ai_range_table	= &ai_ranges_6030,
+		.ao_range_table	= &ao_ranges_6030,
+		.ao_range_code	= ao_range_code_6030,
+		.ai_fifo	= &ai_fifo_60xx,
+		.has_8255	= 0,
+	},
+	[BOARD_PCIDAS6032] = {
+		.name		= "pci-das6032",
+		.ai_se_chans	= 16,
+		.ai_bits	= 16,
+		.ai_speed	= 10000,
+		.ao_nchan	= 0,
+		.layout		= LAYOUT_60XX,
+		.ai_range_table	= &ai_ranges_6030,
+		.ai_fifo	= &ai_fifo_60xx,
+		.has_8255	= 0,
+	},
+	[BOARD_PCIDAS6033] = {
+		.name		= "pci-das6033",
+		.ai_se_chans	= 64,
+		.ai_bits	= 16,
+		.ai_speed	= 10000,
+		.ao_nchan	= 0,
+		.layout		= LAYOUT_60XX,
+		.ai_range_table	= &ai_ranges_6030,
+		.ai_fifo	= &ai_fifo_60xx,
+		.has_8255	= 0,
+	},
+	[BOARD_PCIDAS6034] = {
+		.name		= "pci-das6034",
+		.ai_se_chans	= 16,
+		.ai_bits	= 16,
+		.ai_speed	= 5000,
+		.ao_nchan	= 0,
+		.ao_scan_speed	= 0,
+		.layout		= LAYOUT_60XX,
+		.ai_range_table	= &ai_ranges_60xx,
+		.ai_fifo	= &ai_fifo_60xx,
+		.has_8255	= 0,
+	},
+	[BOARD_PCIDAS6035] = {
+		.name		= "pci-das6035",
+		.ai_se_chans	= 16,
+		.ai_bits	= 16,
+		.ai_speed	= 5000,
+		.ao_nchan	= 2,
+		.ao_bits	= 12,
+		.ao_scan_speed	= 100000,
+		.layout		= LAYOUT_60XX,
+		.ai_range_table	= &ai_ranges_60xx,
+		.ao_range_table	= &range_bipolar10,
+		.ao_range_code	= ao_range_code_60xx,
+		.ai_fifo	= &ai_fifo_60xx,
+		.has_8255	= 0,
+	},
+	[BOARD_PCIDAS6036] = {
+		.name		= "pci-das6036",
+		.ai_se_chans	= 16,
+		.ai_bits	= 16,
+		.ai_speed	= 5000,
+		.ao_nchan	= 2,
+		.ao_bits	= 16,
+		.ao_scan_speed	= 100000,
+		.layout		= LAYOUT_60XX,
+		.ai_range_table	= &ai_ranges_60xx,
+		.ao_range_table	= &range_bipolar10,
+		.ao_range_code	= ao_range_code_60xx,
+		.ai_fifo	= &ai_fifo_60xx,
+		.has_8255	= 0,
+	},
+	[BOARD_PCIDAS6040] = {
+		.name		= "pci-das6040",
+		.ai_se_chans	= 16,
+		.ai_bits	= 12,
+		.ai_speed	= 2000,
+		.ao_nchan	= 2,
+		.ao_bits	= 12,
+		.ao_scan_speed	= 1000,
+		.layout		= LAYOUT_60XX,
+		.ai_range_table	= &ai_ranges_6052,
+		.ao_range_table	= &ao_ranges_6030,
+		.ao_range_code	= ao_range_code_6030,
+		.ai_fifo	= &ai_fifo_60xx,
+		.has_8255	= 0,
+	},
+	[BOARD_PCIDAS6052] = {
+		.name		= "pci-das6052",
+		.ai_se_chans	= 16,
+		.ai_bits	= 16,
+		.ai_speed	= 3333,
+		.ao_nchan	= 2,
+		.ao_bits	= 16,
+		.ao_scan_speed	= 3333,
+		.layout		= LAYOUT_60XX,
+		.ai_range_table	= &ai_ranges_6052,
+		.ao_range_table	= &ao_ranges_6030,
+		.ao_range_code	= ao_range_code_6030,
+		.ai_fifo	= &ai_fifo_60xx,
+		.has_8255	= 0,
+	},
+	[BOARD_PCIDAS6070] = {
+		.name		= "pci-das6070",
+		.ai_se_chans	= 16,
+		.ai_bits	= 12,
+		.ai_speed	= 800,
+		.ao_nchan	= 2,
+		.ao_bits	= 12,
+		.ao_scan_speed	= 1000,
+		.layout		= LAYOUT_60XX,
+		.ai_range_table	= &ai_ranges_6052,
+		.ao_range_table	= &ao_ranges_6030,
+		.ao_range_code	= ao_range_code_6030,
+		.ai_fifo	= &ai_fifo_60xx,
+		.has_8255	= 0,
+	},
+	[BOARD_PCIDAS6071] = {
+		.name		= "pci-das6071",
+		.ai_se_chans	= 64,
+		.ai_bits	= 12,
+		.ai_speed	= 800,
+		.ao_nchan	= 2,
+		.ao_bits	= 12,
+		.ao_scan_speed	= 1000,
+		.layout		= LAYOUT_60XX,
+		.ai_range_table	= &ai_ranges_6052,
+		.ao_range_table	= &ao_ranges_6030,
+		.ao_range_code	= ao_range_code_6030,
+		.ai_fifo	= &ai_fifo_60xx,
+		.has_8255	= 0,
+	},
+	[BOARD_PCIDAS4020_12] = {
+		.name		= "pci-das4020/12",
+		.ai_se_chans	= 4,
+		.ai_bits	= 12,
+		.ai_speed	= 50,
+		.ao_bits	= 12,
+		.ao_nchan	= 2,
+		.ao_scan_speed	= 0,	/*  no hardware pacing on ao */
+		.layout		= LAYOUT_4020,
+		.ai_range_table	= &ai_ranges_4020,
+		.ao_range_table	= &ao_ranges_4020,
+		.ao_range_code	= ao_range_code_4020,
+		.ai_fifo	= &ai_fifo_4020,
+		.has_8255	= 1,
+	},
 #if 0
-	{
-	 .name = "pci-das6402/16/jr",
-	 .device_id = 0		/*  XXX, */
-	 .ai_se_chans = 64,
-	 .ai_bits = 16,
-	 .ai_speed = 5000,
-	 .ao_nchan = 0,
-	 .ao_scan_speed = 10000,
-	 .layout = LAYOUT_64XX,
-	 .ai_range_table = &ai_ranges_64xx,
-	 .ai_fifo = ai_fifo_64xx,
-	 .has_8255 = 1,
-	 },
-	{
-	 .name = "pci-das64/m1/16/jr",
-	 .device_id = 0		/*  XXX, */
-	 .ai_se_chans = 64,
-	 .ai_bits = 16,
-	 .ai_speed = 1000,
-	 .ao_nchan = 0,
-	 .ao_scan_speed = 10000,
-	 .layout = LAYOUT_64XX,
-	 .ai_range_table = &ai_ranges_64xx,
-	 .ai_fifo = ai_fifo_64xx,
-	 .has_8255 = 1,
-	 },
-	{
-	 .name = "pci-das64/m2/16/jr",
-	 .device_id = 0		/*  XXX, */
-	 .ai_se_chans = 64,
-	 .ai_bits = 16,
-	 .ai_speed = 500,
-	 .ao_nchan = 0,
-	 .ao_scan_speed = 10000,
-	 .layout = LAYOUT_64XX,
-	 .ai_range_table = &ai_ranges_64xx,
-	 .ai_fifo = ai_fifo_64xx,
-	 .has_8255 = 1,
-	 },
-	{
-	 .name = "pci-das64/m3/16/jr",
-	 .device_id = 0		/*  XXX, */
-	 .ai_se_chans = 64,
-	 .ai_bits = 16,
-	 .ai_speed = 333,
-	 .ao_nchan = 0,
-	 .ao_scan_speed = 10000,
-	 .layout = LAYOUT_64XX,
-	 .ai_range_table = &ai_ranges_64xx,
-	 .ai_fifo = ai_fifo_64xx,
-	 .has_8255 = 1,
-	 },
-	{
-	 .name = "pci-das64/m1/14",
-	 .device_id = 0,	/*  XXX */
-	 .ai_se_chans = 64,
-	 .ai_bits = 14,
-	 .ai_speed = 1000,
-	 .ao_nchan = 2,
-	 .ao_scan_speed = 10000,
-	 .layout = LAYOUT_64XX,
-	 .ai_range_table = &ai_ranges_64xx,
-	 .ai_fifo = ai_fifo_64xx,
-	 .has_8255 = 1,
-	 },
-	{
-	 .name = "pci-das64/m2/14",
-	 .device_id = 0,	/*  XXX */
-	 .ai_se_chans = 64,
-	 .ai_bits = 14,
-	 .ai_speed = 500,
-	 .ao_nchan = 2,
-	 .ao_scan_speed = 10000,
-	 .layout = LAYOUT_64XX,
-	 .ai_range_table = &ai_ranges_64xx,
-	 .ai_fifo = ai_fifo_64xx,
-	 .has_8255 = 1,
-	 },
-	{
-	 .name = "pci-das64/m3/14",
-	 .device_id = 0,	/*  XXX */
-	 .ai_se_chans = 64,
-	 .ai_bits = 14,
-	 .ai_speed = 333,
-	 .ao_nchan = 2,
-	 .ao_scan_speed = 10000,
-	 .layout = LAYOUT_64XX,
-	 .ai_range_table = &ai_ranges_64xx,
-	 .ai_fifo = ai_fifo_64xx,
-	 .has_8255 = 1,
-	 },
+	/*
+	 * The device id for these boards is unknown
+	 */
+
+	[BOARD_PCIDAS6402_16_JR] = {
+		.name		= "pci-das6402/16/jr",
+		.ai_se_chans	= 64,
+		.ai_bits	= 16,
+		.ai_speed	= 5000,
+		.ao_nchan	= 0,
+		.ao_scan_speed	= 10000,
+		.layout		= LAYOUT_64XX,
+		.ai_range_table	= &ai_ranges_64xx,
+		.ai_fifo	= ai_fifo_64xx,
+		.has_8255	= 1,
+	},
+	[BOARD_PCIDAS64_M1_16_JR] = {
+		.name		= "pci-das64/m1/16/jr",
+		.ai_se_chans	= 64,
+		.ai_bits	= 16,
+		.ai_speed	= 1000,
+		.ao_nchan	= 0,
+		.ao_scan_speed	= 10000,
+		.layout		= LAYOUT_64XX,
+		.ai_range_table	= &ai_ranges_64xx,
+		.ai_fifo	= ai_fifo_64xx,
+		.has_8255	= 1,
+	},
+	[BOARD_PCIDAS64_M2_16_JR] = {
+		.name = "pci-das64/m2/16/jr",
+		.ai_se_chans	= 64,
+		.ai_bits	= 16,
+		.ai_speed	= 500,
+		.ao_nchan	= 0,
+		.ao_scan_speed	= 10000,
+		.layout		= LAYOUT_64XX,
+		.ai_range_table	= &ai_ranges_64xx,
+		.ai_fifo	= ai_fifo_64xx,
+		.has_8255	= 1,
+	},
+	[BOARD_PCIDAS64_M3_16_JR] = {
+		.name		= "pci-das64/m3/16/jr",
+		.ai_se_chans	= 64,
+		.ai_bits	= 16,
+		.ai_speed	= 333,
+		.ao_nchan	= 0,
+		.ao_scan_speed	= 10000,
+		.layout		= LAYOUT_64XX,
+		.ai_range_table	= &ai_ranges_64xx,
+		.ai_fifo	= ai_fifo_64xx,
+		.has_8255	= 1,
+	},
+	[BOARD_PCIDAS64_M1_14] = {
+		.name		= "pci-das64/m1/14",
+		.ai_se_chans	= 64,
+		.ai_bits	= 14,
+		.ai_speed	= 1000,
+		.ao_nchan	= 2,
+		.ao_scan_speed	= 10000,
+		.layout		= LAYOUT_64XX,
+		.ai_range_table	= &ai_ranges_64xx,
+		.ai_fifo	= ai_fifo_64xx,
+		.has_8255	= 1,
+	},
+	[BOARD_PCIDAS64_M2_14] = {
+		.name		= "pci-das64/m2/14",
+		.ai_se_chans	= 64,
+		.ai_bits	= 14,
+		.ai_speed	= 500,
+		.ao_nchan	= 2,
+		.ao_scan_speed	= 10000,
+		.layout		= LAYOUT_64XX,
+		.ai_range_table	= &ai_ranges_64xx,
+		.ai_fifo	= ai_fifo_64xx,
+		.has_8255	= 1,
+	},
+	[BOARD_PCIDAS64_M3_14] = {
+		.name		= "pci-das64/m3/14",
+		.ai_se_chans	= 64,
+		.ai_bits	= 14,
+		.ai_speed	= 333,
+		.ao_nchan	= 2,
+		.ao_scan_speed	= 10000,
+		.layout		= LAYOUT_64XX,
+		.ai_range_table	= &ai_ranges_64xx,
+		.ai_fifo	= ai_fifo_64xx,
+		.has_8255	= 1,
+	},
 #endif
 };
 
@@ -1088,7 +1080,6 @@ struct ext_clock_info {
 /* this structure is for data unique to this hardware driver. */
 struct pcidas64_private {
 	/*  base addresses (physical) */
-	resource_size_t plx9080_phys_iobase;
 	resource_size_t main_phys_iobase;
 	resource_size_t dio_counter_phys_iobase;
 	/*  base addresses (ioremapped) */
@@ -1523,7 +1514,7 @@ static int alloc_and_init_dma_members(struct comedi_device *dev)
 	struct pcidas64_private *devpriv = dev->private;
 	int i;
 
-	/*  alocate pci dma buffers */
+	/*  allocate pci dma buffers */
 	for (i = 0; i < ai_dma_ring_count(thisboard); i++) {
 		devpriv->ai_buffer[i] =
 			pci_alloc_consistent(pcidev, DMA_BUFFER_SIZE,
@@ -3107,7 +3098,7 @@ static irqreturn_t handle_interrupt(int irq, void *d)
 	/* an interrupt before all the postconfig stuff gets done could
 	 * cause a NULL dereference if we continue through the
 	 * interrupt handler */
-	if (dev->attached == 0) {
+	if (!dev->attached) {
 		DEBUG_PRINT("premature interrupt, ignoring\n");
 		return IRQ_HANDLED;
 	}
@@ -4033,68 +4024,40 @@ static int setup_subdevices(struct comedi_device *dev)
 	return 0;
 }
 
-static const struct pcidas64_board
-*cb_pcidas64_find_pci_board(struct pci_dev *pcidev)
-{
-	unsigned int i;
-
-	for (i = 0; i < ARRAY_SIZE(pcidas64_boards); i++)
-		if (pcidev->device == pcidas64_boards[i].device_id)
-			return &pcidas64_boards[i];
-	return NULL;
-}
-
 static int auto_attach(struct comedi_device *dev,
-				 unsigned long context_unused)
+		       unsigned long context)
 {
-	const struct pcidas64_board *thisboard;
-	struct pcidas64_private *devpriv;
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	const struct pcidas64_board *thisboard = NULL;
+	struct pcidas64_private *devpriv;
 	uint32_t local_range, local_decode;
 	int retval;
 
-	dev->board_ptr = cb_pcidas64_find_pci_board(pcidev);
-	if (!dev->board_ptr) {
-		dev_err(dev->class_dev,
-			"cb_pcidas64: does not support pci %s\n",
-			pci_name(pcidev));
-		return -EINVAL;
-	}
-	thisboard = comedi_board(dev);
+	if (context < ARRAY_SIZE(pcidas64_boards))
+		thisboard = &pcidas64_boards[context];
+	if (!thisboard)
+		return -ENODEV;
+	dev->board_ptr = thisboard;
 
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	if (comedi_pci_enable(pcidev, dev->driver->driver_name)) {
-		dev_warn(dev->class_dev,
-			 "failed to enable PCI device and request regions\n");
-		return -EIO;
-	}
+	retval = comedi_pci_enable(dev);
+	if (retval)
+		return retval;
 	pci_set_master(pcidev);
 
 	/* Initialize dev->board_name */
 	dev->board_name = thisboard->name;
 
-	dev->iobase = pci_resource_start(pcidev, MAIN_BADDRINDEX);
-
-	devpriv->plx9080_phys_iobase =
-		pci_resource_start(pcidev, PLX9080_BADDRINDEX);
-	devpriv->main_phys_iobase = dev->iobase;
-	devpriv->dio_counter_phys_iobase =
-		pci_resource_start(pcidev, DIO_COUNTER_BADDRINDEX);
-
-	/*  remap, won't work with 2.0 kernels but who cares */
-	devpriv->plx9080_iobase =
-		ioremap(devpriv->plx9080_phys_iobase,
-			pci_resource_len(pcidev, PLX9080_BADDRINDEX));
-	devpriv->main_iobase =
-		ioremap(devpriv->main_phys_iobase,
-			pci_resource_len(pcidev, MAIN_BADDRINDEX));
-	devpriv->dio_counter_iobase =
-		ioremap(devpriv->dio_counter_phys_iobase,
-			pci_resource_len(pcidev, DIO_COUNTER_BADDRINDEX));
+	devpriv->main_phys_iobase = pci_resource_start(pcidev, 2);
+	devpriv->dio_counter_phys_iobase = pci_resource_start(pcidev, 3);
+
+	devpriv->plx9080_iobase = pci_ioremap_bar(pcidev, 0);
+	devpriv->main_iobase = pci_ioremap_bar(pcidev, 2);
+	devpriv->dio_counter_iobase = pci_ioremap_bar(pcidev, 3);
 
 	if (!devpriv->plx9080_iobase || !devpriv->main_iobase
 	    || !devpriv->dio_counter_iobase) {
@@ -4200,12 +4163,8 @@ static void detach(struct comedi_device *dev)
 					devpriv->ao_dma_desc_bus_addr);
 		}
 	}
-	if (dev->subdevices)
-		subdev_8255_cleanup(dev, &dev->subdevices[4]);
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-	}
+	comedi_spriv_free(dev, 4);
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver cb_pcidas64_driver = {
@@ -4216,31 +4175,34 @@ static struct comedi_driver cb_pcidas64_driver = {
 };
 
 static int cb_pcidas64_pci_probe(struct pci_dev *dev,
-					   const struct pci_device_id *ent)
+				 const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &cb_pcidas64_driver);
+	return comedi_pci_auto_config(dev, &cb_pcidas64_driver,
+				      id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(cb_pcidas64_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x001d) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x001e) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0035) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0036) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0037) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0052) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x005d) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x005e) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x005f) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0061) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0062) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0063) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0064) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0066) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0067) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0068) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x006f) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0078) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0079) },
+	{ PCI_VDEVICE(CB, 0x001d), BOARD_PCIDAS6402_16 },
+	{ PCI_VDEVICE(CB, 0x001e), BOARD_PCIDAS6402_12 },
+	{ PCI_VDEVICE(CB, 0x0035), BOARD_PCIDAS64_M1_16 },
+	{ PCI_VDEVICE(CB, 0x0036), BOARD_PCIDAS64_M2_16 },
+	{ PCI_VDEVICE(CB, 0x0037), BOARD_PCIDAS64_M3_16 },
+	{ PCI_VDEVICE(CB, 0x0052), BOARD_PCIDAS4020_12 },
+	{ PCI_VDEVICE(CB, 0x005d), BOARD_PCIDAS6023 },
+	{ PCI_VDEVICE(CB, 0x005e), BOARD_PCIDAS6025 },
+	{ PCI_VDEVICE(CB, 0x005f), BOARD_PCIDAS6030 },
+	{ PCI_VDEVICE(CB, 0x0060), BOARD_PCIDAS6031 },
+	{ PCI_VDEVICE(CB, 0x0061), BOARD_PCIDAS6032 },
+	{ PCI_VDEVICE(CB, 0x0062), BOARD_PCIDAS6033 },
+	{ PCI_VDEVICE(CB, 0x0063), BOARD_PCIDAS6034 },
+	{ PCI_VDEVICE(CB, 0x0064), BOARD_PCIDAS6035 },
+	{ PCI_VDEVICE(CB, 0x0065), BOARD_PCIDAS6040 },
+	{ PCI_VDEVICE(CB, 0x0066), BOARD_PCIDAS6052 },
+	{ PCI_VDEVICE(CB, 0x0067), BOARD_PCIDAS6070 },
+	{ PCI_VDEVICE(CB, 0x0068), BOARD_PCIDAS6071 },
+	{ PCI_VDEVICE(CB, 0x006f), BOARD_PCIDAS6036 },
+	{ PCI_VDEVICE(CB, 0x0078), BOARD_PCIDAS6013 },
+	{ PCI_VDEVICE(CB, 0x0079), BOARD_PCIDAS6014 },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, cb_pcidas64_pci_table);
diff --git a/drivers/staging/comedi/drivers/cb_pcidda.c b/drivers/staging/comedi/drivers/cb_pcidda.c
index e2cadc72845..f9b459888b8 100644
--- a/drivers/staging/comedi/drivers/cb_pcidda.c
+++ b/drivers/staging/comedi/drivers/cb_pcidda.c
@@ -48,16 +48,6 @@
 #include "comedi_fc.h"
 #include "8255.h"
 
-/*
- * ComputerBoards PCI Device ID's supported by this driver
- */
-#define PCI_DEVICE_ID_DDA02_12		0x0020
-#define PCI_DEVICE_ID_DDA04_12		0x0021
-#define PCI_DEVICE_ID_DDA08_12		0x0022
-#define PCI_DEVICE_ID_DDA02_16		0x0023
-#define PCI_DEVICE_ID_DDA04_16		0x0024
-#define PCI_DEVICE_ID_DDA08_16		0x0025
-
 #define EEPROM_SIZE	128	/*  number of entries in eeprom */
 /* maximum number of ao channels for supported boards */
 #define MAX_AO_CHANNELS 8
@@ -118,42 +108,49 @@ static const struct comedi_lrange cb_pcidda_ranges = {
 	}
 };
 
+enum cb_pcidda_boardid {
+	BOARD_DDA02_12,
+	BOARD_DDA04_12,
+	BOARD_DDA08_12,
+	BOARD_DDA02_16,
+	BOARD_DDA04_16,
+	BOARD_DDA08_16,
+};
+
 struct cb_pcidda_board {
 	const char *name;
-	unsigned short device_id;
 	int ao_chans;
 	int ao_bits;
 };
 
 static const struct cb_pcidda_board cb_pcidda_boards[] = {
-	{
+	[BOARD_DDA02_12] = {
 		.name		= "pci-dda02/12",
-		.device_id	= PCI_DEVICE_ID_DDA02_12,
 		.ao_chans	= 2,
 		.ao_bits	= 12,
-	}, {
+	},
+	[BOARD_DDA04_12] = {
 		.name		= "pci-dda04/12",
-		.device_id	= PCI_DEVICE_ID_DDA04_12,
 		.ao_chans	= 4,
 		.ao_bits	= 12,
-	}, {
+	},
+	[BOARD_DDA08_12] = {
 		.name		= "pci-dda08/12",
-		.device_id	= PCI_DEVICE_ID_DDA08_12,
 		.ao_chans	= 8,
 		.ao_bits	= 12,
-	}, {
+	},
+	[BOARD_DDA02_16] = {
 		.name		= "pci-dda02/16",
-		.device_id	= PCI_DEVICE_ID_DDA02_16,
 		.ao_chans	= 2,
 		.ao_bits	= 16,
-	}, {
+	},
+	[BOARD_DDA04_16] = {
 		.name		= "pci-dda04/16",
-		.device_id	= PCI_DEVICE_ID_DDA04_16,
 		.ao_chans	= 4,
 		.ao_bits	= 16,
-	}, {
+	},
+	[BOARD_DDA08_16] = {
 		.name		= "pci-dda08/16",
-		.device_id	= PCI_DEVICE_ID_DDA08_16,
 		.ao_chans	= 8,
 		.ao_bits	= 16,
 	},
@@ -337,32 +334,19 @@ static int cb_pcidda_ao_insn_write(struct comedi_device *dev,
 	return insn->n;
 }
 
-static const void *cb_pcidda_find_boardinfo(struct comedi_device *dev,
-					    struct pci_dev *pcidev)
-{
-	const struct cb_pcidda_board *thisboard;
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(cb_pcidda_boards); i++) {
-		thisboard = &cb_pcidda_boards[i];
-		if (thisboard->device_id != pcidev->device)
-			return thisboard;
-	}
-	return NULL;
-}
-
 static int cb_pcidda_auto_attach(struct comedi_device *dev,
-					   unsigned long context_unused)
+				 unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct cb_pcidda_board *thisboard;
+	const struct cb_pcidda_board *thisboard = NULL;
 	struct cb_pcidda_private *devpriv;
 	struct comedi_subdevice *s;
 	unsigned long iobase_8255;
 	int i;
 	int ret;
 
-	thisboard = cb_pcidda_find_boardinfo(dev, pcidev);
+	if (context < ARRAY_SIZE(cb_pcidda_boards))
+		thisboard = &cb_pcidda_boards[context];
 	if (!thisboard)
 		return -ENODEV;
 	dev->board_ptr = thisboard;
@@ -373,7 +357,7 @@ static int cb_pcidda_auto_attach(struct comedi_device *dev,
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
 	dev->iobase = pci_resource_start(pcidev, 3);
@@ -415,16 +399,9 @@ static int cb_pcidda_auto_attach(struct comedi_device *dev,
 
 static void cb_pcidda_detach(struct comedi_device *dev)
 {
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-
-	if (dev->subdevices) {
-		subdev_8255_cleanup(dev, &dev->subdevices[1]);
-		subdev_8255_cleanup(dev, &dev->subdevices[2]);
-	}
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-	}
+	comedi_spriv_free(dev, 1);
+	comedi_spriv_free(dev, 2);
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver cb_pcidda_driver = {
@@ -435,18 +412,19 @@ static struct comedi_driver cb_pcidda_driver = {
 };
 
 static int cb_pcidda_pci_probe(struct pci_dev *dev,
-					 const struct pci_device_id *ent)
+			       const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &cb_pcidda_driver);
+	return comedi_pci_auto_config(dev, &cb_pcidda_driver,
+				      id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(cb_pcidda_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, PCI_DEVICE_ID_DDA02_12) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, PCI_DEVICE_ID_DDA04_12) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, PCI_DEVICE_ID_DDA08_12) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, PCI_DEVICE_ID_DDA02_16) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, PCI_DEVICE_ID_DDA04_16) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, PCI_DEVICE_ID_DDA08_16) },
+	{ PCI_VDEVICE(CB, 0x0020), BOARD_DDA02_12 },
+	{ PCI_VDEVICE(CB, 0x0021), BOARD_DDA04_12 },
+	{ PCI_VDEVICE(CB, 0x0022), BOARD_DDA08_12 },
+	{ PCI_VDEVICE(CB, 0x0023), BOARD_DDA02_16 },
+	{ PCI_VDEVICE(CB, 0x0024), BOARD_DDA04_16 },
+	{ PCI_VDEVICE(CB, 0x0025), BOARD_DDA08_16 },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, cb_pcidda_pci_table);
diff --git a/drivers/staging/comedi/drivers/cb_pcimdas.c b/drivers/staging/comedi/drivers/cb_pcimdas.c
index aae063ca85a..29813c9d4a2 100644
--- a/drivers/staging/comedi/drivers/cb_pcimdas.c
+++ b/drivers/staging/comedi/drivers/cb_pcimdas.c
@@ -215,14 +215,12 @@ static int cb_pcimdas_auto_attach(struct comedi_device *dev,
 	unsigned long iobase_8255;
 	int ret;
 
-	dev->board_name = dev->driver->driver_name;
-
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
 
@@ -277,14 +275,9 @@ static int cb_pcimdas_auto_attach(struct comedi_device *dev,
 
 static void cb_pcimdas_detach(struct comedi_device *dev)
 {
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-	}
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver cb_pcimdas_driver = {
@@ -295,9 +288,10 @@ static struct comedi_driver cb_pcimdas_driver = {
 };
 
 static int cb_pcimdas_pci_probe(struct pci_dev *dev,
-					  const struct pci_device_id *ent)
+				const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &cb_pcimdas_driver);
+	return comedi_pci_auto_config(dev, &cb_pcimdas_driver,
+				      id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(cb_pcimdas_pci_table) = {
diff --git a/drivers/staging/comedi/drivers/cb_pcimdda.c b/drivers/staging/comedi/drivers/cb_pcimdda.c
index 63cfbaf3a3f..88f03ae6f3e 100644
--- a/drivers/staging/comedi/drivers/cb_pcimdda.c
+++ b/drivers/staging/comedi/drivers/cb_pcimdda.c
@@ -161,14 +161,12 @@ static int cb_pcimdda_auto_attach(struct comedi_device *dev,
 	struct comedi_subdevice *s;
 	int ret;
 
-	dev->board_name = dev->driver->driver_name;
-
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
 	dev->iobase = pci_resource_start(pcidev, 3);
@@ -201,14 +199,8 @@ static int cb_pcimdda_auto_attach(struct comedi_device *dev,
 
 static void cb_pcimdda_detach(struct comedi_device *dev)
 {
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-
-	if (dev->subdevices)
-		subdev_8255_cleanup(dev, &dev->subdevices[1]);
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-	}
+	comedi_spriv_free(dev, 1);
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver cb_pcimdda_driver = {
@@ -219,9 +211,10 @@ static struct comedi_driver cb_pcimdda_driver = {
 };
 
 static int cb_pcimdda_pci_probe(struct pci_dev *dev,
-					  const struct pci_device_id *ent)
+				const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &cb_pcimdda_driver);
+	return comedi_pci_auto_config(dev, &cb_pcimdda_driver,
+				      id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(cb_pcimdda_pci_table) = {
diff --git a/drivers/staging/comedi/drivers/comedi_fc.c b/drivers/staging/comedi/drivers/comedi_fc.c
index 83728298eef..37dc79637d2 100644
--- a/drivers/staging/comedi/drivers/comedi_fc.c
+++ b/drivers/staging/comedi/drivers/comedi_fc.c
@@ -65,7 +65,7 @@ unsigned int cfc_write_array_to_buffer(struct comedi_subdevice *subd,
 
 	return num_bytes;
 }
-EXPORT_SYMBOL(cfc_write_array_to_buffer);
+EXPORT_SYMBOL_GPL(cfc_write_array_to_buffer);
 
 unsigned int cfc_read_array_from_buffer(struct comedi_subdevice *subd,
 					void *data, unsigned int num_bytes)
@@ -83,7 +83,7 @@ unsigned int cfc_read_array_from_buffer(struct comedi_subdevice *subd,
 
 	return num_bytes;
 }
-EXPORT_SYMBOL(cfc_read_array_from_buffer);
+EXPORT_SYMBOL_GPL(cfc_read_array_from_buffer);
 
 unsigned int cfc_handle_events(struct comedi_device *dev,
 			       struct comedi_subdevice *subd)
@@ -100,7 +100,7 @@ unsigned int cfc_handle_events(struct comedi_device *dev,
 
 	return events;
 }
-EXPORT_SYMBOL(cfc_handle_events);
+EXPORT_SYMBOL_GPL(cfc_handle_events);
 
 MODULE_AUTHOR("Frank Mori Hess <fmhess@users.sourceforge.net>");
 MODULE_DESCRIPTION("Shared functions for Comedi low-level drivers");
diff --git a/drivers/staging/comedi/drivers/comedi_parport.c b/drivers/staging/comedi/drivers/comedi_parport.c
index 76d59dcbea0..3e061cc9b48 100644
--- a/drivers/staging/comedi/drivers/comedi_parport.c
+++ b/drivers/staging/comedi/drivers/comedi_parport.c
@@ -261,19 +261,13 @@ static int parport_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
 	struct parport_private *devpriv;
-	int ret;
-	unsigned int irq;
-	unsigned long iobase;
 	struct comedi_subdevice *s;
+	unsigned int irq;
+	int ret;
 
-	dev->board_name = dev->driver->driver_name;
-
-	iobase = it->options[0];
-	if (!request_region(iobase, PARPORT_SIZE, dev->board_name)) {
-		dev_err(dev->class_dev, "I/O port conflict\n");
-		return -EIO;
-	}
-	dev->iobase = iobase;
+	ret = comedi_request_region(dev, it->options[0], PARPORT_SIZE);
+	if (ret)
+		return ret;
 
 	irq = it->options[1];
 	if (irq) {
@@ -341,25 +335,14 @@ static int parport_attach(struct comedi_device *dev,
 	devpriv->c_data = 0;
 	outb(devpriv->c_data, dev->iobase + PARPORT_C);
 
-	dev_info(dev->class_dev, "%s: iobase=0x%04lx, irq %sabled",
-		dev->board_name, dev->iobase, dev->irq ? "en" : "dis");
-
 	return 0;
 }
 
-static void parport_detach(struct comedi_device *dev)
-{
-	if (dev->iobase)
-		release_region(dev->iobase, PARPORT_SIZE);
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-}
-
 static struct comedi_driver parport_driver = {
 	.driver_name	= "comedi_parport",
 	.module		= THIS_MODULE,
 	.attach		= parport_attach,
-	.detach		= parport_detach,
+	.detach		= comedi_legacy_detach,
 };
 module_comedi_driver(parport_driver);
 
diff --git a/drivers/staging/comedi/drivers/comedi_test.c b/drivers/staging/comedi/drivers/comedi_test.c
index 270fea5c6b5..c1d8e86f53a 100644
--- a/drivers/staging/comedi/drivers/comedi_test.c
+++ b/drivers/staging/comedi/drivers/comedi_test.c
@@ -384,8 +384,6 @@ static int waveform_attach(struct comedi_device *dev,
 	int i;
 	int ret;
 
-	dev->board_name = dev->driver->driver_name;
-
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
diff --git a/drivers/staging/comedi/drivers/contec_pci_dio.c b/drivers/staging/comedi/drivers/contec_pci_dio.c
index 182dea669ef..f2230bfd4eb 100644
--- a/drivers/staging/comedi/drivers/contec_pci_dio.c
+++ b/drivers/staging/comedi/drivers/contec_pci_dio.c
@@ -77,9 +77,7 @@ static int contec_auto_attach(struct comedi_device *dev,
 	struct comedi_subdevice *s;
 	int ret;
 
-	dev->board_name = dev->driver->driver_name;
-
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
 	dev->iobase = pci_resource_start(pcidev, 0);
@@ -109,27 +107,18 @@ static int contec_auto_attach(struct comedi_device *dev,
 	return 0;
 }
 
-static void contec_detach(struct comedi_device *dev)
-{
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-	}
-}
-
 static struct comedi_driver contec_pci_dio_driver = {
 	.driver_name	= "contec_pci_dio",
 	.module		= THIS_MODULE,
 	.auto_attach	= contec_auto_attach,
-	.detach		= contec_detach,
+	.detach		= comedi_pci_disable,
 };
 
 static int contec_pci_dio_pci_probe(struct pci_dev *dev,
-					      const struct pci_device_id *ent)
+				    const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &contec_pci_dio_driver);
+	return comedi_pci_auto_config(dev, &contec_pci_dio_driver,
+				      id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(contec_pci_dio_pci_table) = {
diff --git a/drivers/staging/comedi/drivers/daqboard2000.c b/drivers/staging/comedi/drivers/daqboard2000.c
index 50b450f09c6..b87f95c3e17 100644
--- a/drivers/staging/comedi/drivers/daqboard2000.c
+++ b/drivers/staging/comedi/drivers/daqboard2000.c
@@ -462,9 +462,9 @@ static void daqboard2000_resetLocalBus(struct comedi_device *dev)
 	struct daqboard2000_private *devpriv = dev->private;
 
 	writel(DAQBOARD2000_SECRLocalBusHi, devpriv->plx + 0x6c);
-	udelay(10000);
+	mdelay(10);
 	writel(DAQBOARD2000_SECRLocalBusLo, devpriv->plx + 0x6c);
-	udelay(10000);
+	mdelay(10);
 }
 
 static void daqboard2000_reloadPLX(struct comedi_device *dev)
@@ -472,11 +472,11 @@ static void daqboard2000_reloadPLX(struct comedi_device *dev)
 	struct daqboard2000_private *devpriv = dev->private;
 
 	writel(DAQBOARD2000_SECRReloadLo, devpriv->plx + 0x6c);
-	udelay(10000);
+	mdelay(10);
 	writel(DAQBOARD2000_SECRReloadHi, devpriv->plx + 0x6c);
-	udelay(10000);
+	mdelay(10);
 	writel(DAQBOARD2000_SECRReloadLo, devpriv->plx + 0x6c);
-	udelay(10000);
+	mdelay(10);
 }
 
 static void daqboard2000_pulseProgPin(struct comedi_device *dev)
@@ -484,9 +484,9 @@ static void daqboard2000_pulseProgPin(struct comedi_device *dev)
 	struct daqboard2000_private *devpriv = dev->private;
 
 	writel(DAQBOARD2000_SECRProgPinHi, devpriv->plx + 0x6c);
-	udelay(10000);
+	mdelay(10);
 	writel(DAQBOARD2000_SECRProgPinLo, devpriv->plx + 0x6c);
-	udelay(10000);	/* Not in the original code, but I like symmetry... */
+	mdelay(10);	/* Not in the original code, but I like symmetry... */
 }
 
 static int daqboard2000_pollCPLD(struct comedi_device *dev, int mask)
@@ -709,15 +709,12 @@ static int daqboard2000_auto_attach(struct comedi_device *dev,
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	result = comedi_pci_enable(pcidev, dev->driver->driver_name);
-	if (result < 0)
+	result = comedi_pci_enable(dev);
+	if (result)
 		return result;
-	dev->iobase = 1;	/* the "detach" needs this */
 
-	devpriv->plx = ioremap(pci_resource_start(pcidev, 0),
-			       pci_resource_len(pcidev, 0));
-	devpriv->daq = ioremap(pci_resource_start(pcidev, 2),
-			       pci_resource_len(pcidev, 2));
+	devpriv->plx = pci_ioremap_bar(pcidev, 0);
+	devpriv->daq = pci_ioremap_bar(pcidev, 2);
 	if (!devpriv->plx || !devpriv->daq)
 		return -ENOMEM;
 
@@ -767,11 +764,9 @@ static int daqboard2000_auto_attach(struct comedi_device *dev,
 
 static void daqboard2000_detach(struct comedi_device *dev)
 {
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct daqboard2000_private *devpriv = dev->private;
 
-	if (dev->subdevices)
-		subdev_8255_cleanup(dev, &dev->subdevices[2]);
+	comedi_spriv_free(dev, 2);
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 	if (devpriv) {
@@ -780,11 +775,7 @@ static void daqboard2000_detach(struct comedi_device *dev)
 		if (devpriv->plx)
 			iounmap(devpriv->plx);
 	}
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-		pci_dev_put(pcidev);
-	}
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver daqboard2000_driver = {
@@ -795,9 +786,10 @@ static struct comedi_driver daqboard2000_driver = {
 };
 
 static int daqboard2000_pci_probe(struct pci_dev *dev,
-					    const struct pci_device_id *ent)
+				  const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &daqboard2000_driver);
+	return comedi_pci_auto_config(dev, &daqboard2000_driver,
+				      id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(daqboard2000_pci_table) = {
diff --git a/drivers/staging/comedi/drivers/das08.c b/drivers/staging/comedi/drivers/das08.c
index 9823aa06787..ba12c1d605f 100644
--- a/drivers/staging/comedi/drivers/das08.c
+++ b/drivers/staging/comedi/drivers/das08.c
@@ -568,8 +568,7 @@ EXPORT_SYMBOL_GPL(das08_common_attach);
 
 void das08_common_detach(struct comedi_device *dev)
 {
-	if (dev->subdevices)
-		subdev_8255_cleanup(dev, &dev->subdevices[4]);
+	comedi_spriv_free(dev, 4);
 }
 EXPORT_SYMBOL_GPL(das08_common_detach);
 
diff --git a/drivers/staging/comedi/drivers/das08.h b/drivers/staging/comedi/drivers/das08.h
index b102ad4918c..89bb8d6fdfc 100644
--- a/drivers/staging/comedi/drivers/das08.h
+++ b/drivers/staging/comedi/drivers/das08.h
@@ -32,7 +32,6 @@ enum das08_lrange { das08_pg_none, das08_bipolar5, das08_pgh, das08_pgl,
 
 struct das08_board_struct {
 	const char *name;
-	unsigned int id;	/*  id for pci/pcmcia boards */
 	bool is_jr;		/* true for 'JR' boards */
 	unsigned int ai_nbits;
 	enum das08_lrange ai_pg;
diff --git a/drivers/staging/comedi/drivers/das08_cs.c b/drivers/staging/comedi/drivers/das08_cs.c
index cfeebe4d1dd..d9f3e92317d 100644
--- a/drivers/staging/comedi/drivers/das08_cs.c
+++ b/drivers/staging/comedi/drivers/das08_cs.c
@@ -59,7 +59,6 @@ Command support does not exist, but could be added for this board.
 static const struct das08_board_struct das08_cs_boards[] = {
 	{
 		.name		= "pcm-das08",
-		.id		= 0x0,	/*  XXX */
 		.ai_nbits	= 12,
 		.ai_pg		= das08_bipolar5,
 		.ai_encoding	= das08_pcm_encode12,
diff --git a/drivers/staging/comedi/drivers/das08_isa.c b/drivers/staging/comedi/drivers/das08_isa.c
index f1207824716..f09f6966ed6 100644
--- a/drivers/staging/comedi/drivers/das08_isa.c
+++ b/drivers/staging/comedi/drivers/das08_isa.c
@@ -179,26 +179,24 @@ static int das08_isa_attach(struct comedi_device *dev,
 {
 	const struct das08_board_struct *thisboard = comedi_board(dev);
 	struct das08_private_struct *devpriv;
+	int ret;
 
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	if (!request_region(it->options[0], thisboard->iosize,
-			    thisboard->name))
-		return -EIO;
+	ret = comedi_request_region(dev, it->options[0], thisboard->iosize);
+	if (ret)
+		return ret;
 
-	return das08_common_attach(dev, it->options[0]);
+	return das08_common_attach(dev, dev->iobase);
 }
 
 static void das08_isa_detach(struct comedi_device *dev)
 {
-	const struct das08_board_struct *thisboard = comedi_board(dev);
-
 	das08_common_detach(dev);
-	if (dev->iobase)
-		release_region(dev->iobase, thisboard->iosize);
+	comedi_legacy_detach(dev);
 }
 
 static struct comedi_driver das08_isa_driver = {
diff --git a/drivers/staging/comedi/drivers/das08_pci.c b/drivers/staging/comedi/drivers/das08_pci.c
index c405876ddcf..53fa943dd0b 100644
--- a/drivers/staging/comedi/drivers/das08_pci.c
+++ b/drivers/staging/comedi/drivers/das08_pci.c
@@ -46,7 +46,6 @@
 static const struct das08_board_struct das08_pci_boards[] = {
 	{
 		.name		= "pci-das08",
-		.id		= PCI_DEVICE_ID_PCIDAS08,
 		.ai_nbits	= 12,
 		.ai_pg		= das08_bipolar5,
 		.ai_encoding	= das08_encode12,
@@ -72,7 +71,7 @@ static int das08_pci_auto_attach(struct comedi_device *dev,
 	/* The das08 driver needs the board_ptr */
 	dev->board_ptr = &das08_pci_boards[0];
 
-	ret = comedi_pci_enable(pdev, dev->driver->driver_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
 	dev->iobase = pci_resource_start(pdev, 2);
@@ -82,11 +81,8 @@ static int das08_pci_auto_attach(struct comedi_device *dev,
 
 static void das08_pci_detach(struct comedi_device *dev)
 {
-	struct pci_dev *pdev = comedi_to_pci_dev(dev);
-
 	das08_common_detach(dev);
-	if (dev->iobase)
-		comedi_pci_disable(pdev);
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver das08_pci_comedi_driver = {
@@ -97,9 +93,10 @@ static struct comedi_driver das08_pci_comedi_driver = {
 };
 
 static int das08_pci_probe(struct pci_dev *dev,
-			   const struct pci_device_id *ent)
+			   const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &das08_pci_comedi_driver);
+	return comedi_pci_auto_config(dev, &das08_pci_comedi_driver,
+				      id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(das08_pci_table) = {
diff --git a/drivers/staging/comedi/drivers/das16.c b/drivers/staging/comedi/drivers/das16.c
index f238a1fbccb..762b5a6eac5 100644
--- a/drivers/staging/comedi/drivers/das16.c
+++ b/drivers/staging/comedi/drivers/das16.c
@@ -393,6 +393,8 @@ struct das16_private_struct {
 	struct timer_list timer;	/*  for timed interrupt */
 	volatile short timer_running;
 	volatile short timer_mode;	/*  true if using timer mode */
+
+	unsigned long extra_iobase;
 };
 
 static int das16_cmd_test(struct comedi_device *dev, struct comedi_subdevice *s,
@@ -876,7 +878,7 @@ static void das16_interrupt(struct comedi_device *dev)
 	int num_bytes, residue;
 	int buffer_index;
 
-	if (dev->attached == 0) {
+	if (!dev->attached) {
 		comedi_error(dev, "premature interrupt");
 		return;
 	}
@@ -1079,13 +1081,11 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	struct comedi_subdevice *s;
 	int ret;
 	unsigned int irq;
-	unsigned long iobase;
 	unsigned int dma_chan;
 	int timer_mode;
 	unsigned long flags;
 	struct comedi_krange *user_ai_range, *user_ao_range;
 
-	iobase = it->options[0];
 #if 0
 	irq = it->options[1];
 	timer_mode = it->options[8];
@@ -1097,8 +1097,6 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (timer_mode)
 		irq = 0;
 
-	printk(KERN_INFO "comedi%d: das16:", dev->minor);
-
 	/*  check that clock setting is valid */
 	if (it->options[3]) {
 		if (it->options[3] != 0 &&
@@ -1116,39 +1114,26 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	dev->private = devpriv;
 
 	if (board->size < 0x400) {
-		printk(" 0x%04lx-0x%04lx\n", iobase, iobase + board->size);
-		if (!request_region(iobase, board->size, "das16")) {
-			printk(KERN_ERR " I/O port conflict\n");
-			return -EIO;
-		}
+		ret = comedi_request_region(dev, it->options[0], board->size);
+		if (ret)
+			return ret;
 	} else {
-		printk(KERN_INFO " 0x%04lx-0x%04lx 0x%04lx-0x%04lx\n",
-		       iobase, iobase + 0x0f,
-		       iobase + 0x400,
-		       iobase + 0x400 + (board->size & 0x3ff));
-		if (!request_region(iobase, 0x10, "das16")) {
-			printk(KERN_ERR " I/O port conflict:  0x%04lx-0x%04lx\n",
-			       iobase, iobase + 0x0f);
-			return -EIO;
-		}
-		if (!request_region(iobase + 0x400, board->size & 0x3ff,
-				    "das16")) {
-			release_region(iobase, 0x10);
-			printk(KERN_ERR " I/O port conflict:  0x%04lx-0x%04lx\n",
-			       iobase + 0x400,
-			       iobase + 0x400 + (board->size & 0x3ff));
-			return -EIO;
-		}
+		ret = comedi_request_region(dev, it->options[0], 0x10);
+		if (ret)
+			return ret;
+		/* Request an additional region for the 8255 */
+		ret = __comedi_request_region(dev, dev->iobase + 0x400,
+					      board->size & 0x3ff);
+		if (ret)
+			return ret;
+		devpriv->extra_iobase = dev->iobase + 0x400;
 	}
 
-	dev->iobase = iobase;
-
 	/*  probe id bits to make sure they are consistent */
 	if (das16_probe(dev, it)) {
 		printk(KERN_ERR " id bits do not match selected board, aborting\n");
 		return -EINVAL;
 	}
-	dev->board_name = board->name;
 
 	/*  get master clock speed */
 	if (board->size < 0x400) {
@@ -1162,7 +1147,8 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	/* now for the irq */
 	if (irq > 1 && irq < 8) {
-		ret = request_irq(irq, das16_dma_interrupt, 0, "das16", dev);
+		ret = request_irq(irq, das16_dma_interrupt, 0,
+				  dev->board_name, dev);
 
 		if (ret < 0)
 			return ret;
@@ -1188,7 +1174,7 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 			if (devpriv->dma_buffer[i] == NULL)
 				return -ENOMEM;
 		}
-		if (request_dma(dma_chan, "das16")) {
+		if (request_dma(dma_chan, dev->board_name)) {
 			printk(KERN_ERR " failed to allocate dma channel %i\n",
 			       dma_chan);
 			return -EINVAL;
@@ -1353,8 +1339,7 @@ static void das16_detach(struct comedi_device *dev)
 	struct das16_private_struct *devpriv = dev->private;
 
 	das16_reset(dev);
-	if (dev->subdevices)
-		subdev_8255_cleanup(dev, &dev->subdevices[4]);
+	comedi_spriv_free(dev, 4);
 	if (devpriv) {
 		int i;
 		for (i = 0; i < 2; i++) {
@@ -1369,17 +1354,9 @@ static void das16_detach(struct comedi_device *dev)
 		kfree(devpriv->user_ai_range_table);
 		kfree(devpriv->user_ao_range_table);
 	}
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (dev->iobase) {
-		if (board->size < 0x400) {
-			release_region(dev->iobase, board->size);
-		} else {
-			release_region(dev->iobase, 0x10);
-			release_region(dev->iobase + 0x400,
-				       board->size & 0x3ff);
-		}
-	}
+	if (devpriv->extra_iobase)
+		release_region(devpriv->extra_iobase, board->size & 0x3ff);
+	comedi_legacy_detach(dev);
 }
 
 static const struct das16_board das16_boards[] = {
diff --git a/drivers/staging/comedi/drivers/das16m1.c b/drivers/staging/comedi/drivers/das16m1.c
index b0a861a779b..9cb9c3b0479 100644
--- a/drivers/staging/comedi/drivers/das16m1.c
+++ b/drivers/staging/comedi/drivers/das16m1.c
@@ -143,6 +143,7 @@ struct das16m1_private_struct {
 	unsigned int do_bits;	/*  saves status of digital output bits */
 	unsigned int divisor1;	/*  divides master clock to obtain conversion speed */
 	unsigned int divisor2;	/*  divides master clock to obtain conversion speed */
+	unsigned long extra_iobase;
 };
 
 static inline short munge_sample(short data)
@@ -499,7 +500,7 @@ static irqreturn_t das16m1_interrupt(int irq, void *d)
 	int status;
 	struct comedi_device *dev = d;
 
-	if (dev->attached == 0) {
+	if (!dev->attached) {
 		comedi_error(dev, "premature interrupt");
 		return IRQ_HANDLED;
 	}
@@ -571,28 +572,21 @@ static int das16m1_attach(struct comedi_device *dev,
 	struct comedi_subdevice *s;
 	int ret;
 	unsigned int irq;
-	unsigned long iobase;
-
-	dev->board_name = dev->driver->driver_name;
-
-	iobase = it->options[0];
 
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	if (!request_region(iobase, DAS16M1_SIZE, dev->board_name)) {
-		comedi_error(dev, "I/O port conflict\n");
-		return -EIO;
-	}
-	if (!request_region(iobase + DAS16M1_82C55, DAS16M1_SIZE2,
-			    dev->board_name)) {
-		release_region(iobase, DAS16M1_SIZE);
-		comedi_error(dev, "I/O port conflict\n");
-		return -EIO;
-	}
-	dev->iobase = iobase;
+	ret = comedi_request_region(dev, it->options[0], DAS16M1_SIZE);
+	if (ret)
+		return ret;
+	/* Request an additional region for the 8255 */
+	ret = __comedi_request_region(dev, dev->iobase + DAS16M1_82C55,
+				      DAS16M1_SIZE2);
+	if (ret)
+		return ret;
+	devpriv->extra_iobase = dev->iobase + DAS16M1_82C55;
 
 	/* now for the irq */
 	irq = it->options[1];
@@ -654,7 +648,9 @@ static int das16m1_attach(struct comedi_device *dev,
 
 	s = &dev->subdevices[3];
 	/* 8255 */
-	subdev_8255_init(dev, s, NULL, dev->iobase + DAS16M1_82C55);
+	ret = subdev_8255_init(dev, s, NULL, devpriv->extra_iobase);
+	if (ret)
+		return ret;
 
 	/*  disable upper half of hardware conversion counter so it doesn't mess with us */
 	outb(TOTAL_CLEAR, dev->iobase + DAS16M1_8254_FIRST_CNTRL);
@@ -674,14 +670,12 @@ static int das16m1_attach(struct comedi_device *dev,
 
 static void das16m1_detach(struct comedi_device *dev)
 {
-	if (dev->subdevices)
-		subdev_8255_cleanup(dev, &dev->subdevices[3]);
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (dev->iobase) {
-		release_region(dev->iobase, DAS16M1_SIZE);
-		release_region(dev->iobase + DAS16M1_82C55, DAS16M1_SIZE2);
-	}
+	struct das16m1_private_struct *devpriv = dev->private;
+
+	comedi_spriv_free(dev, 3);
+	if (devpriv && devpriv->extra_iobase)
+		release_region(devpriv->extra_iobase, DAS16M1_SIZE2);
+	comedi_legacy_detach(dev);
 }
 
 static struct comedi_driver das16m1_driver = {
diff --git a/drivers/staging/comedi/drivers/das1800.c b/drivers/staging/comedi/drivers/das1800.c
index 7900f959555..abf7638a9f7 100644
--- a/drivers/staging/comedi/drivers/das1800.c
+++ b/drivers/staging/comedi/drivers/das1800.c
@@ -429,11 +429,6 @@ static const struct das1800_board das1800_boards[] = {
 	 },
 };
 
-/*
- * Useful for shorthand access to the particular board structure
- */
-#define thisboard ((const struct das1800_board *)dev->board_ptr)
-
 struct das1800_private {
 	volatile unsigned int count;	/* number of data points left to be taken */
 	unsigned int divisor1;	/* value to load into board's counter 1 for timed conversions */
@@ -454,14 +449,6 @@ struct das1800_private {
 	short ao_update_bits;	/* remembers the last write to the 'update' dac */
 };
 
-/* analog out range for boards with basic analog out */
-static const struct comedi_lrange range_ao_1 = {
-	1,
-	{
-	 RANGE(-10, 10),
-	 }
-};
-
 /* analog out range for 'ao' boards */
 /*
 static const struct comedi_lrange range_ao_2 = {
@@ -476,6 +463,8 @@ static const struct comedi_lrange range_ao_2 = {
 static inline uint16_t munge_bipolar_sample(const struct comedi_device *dev,
 					    uint16_t sample)
 {
+	const struct das1800_board *thisboard = comedi_board(dev);
+
 	sample += 1 << (thisboard->resolution - 1);
 	return sample;
 }
@@ -731,7 +720,7 @@ static irqreturn_t das1800_interrupt(int irq, void *d)
 	struct comedi_device *dev = d;
 	unsigned int status;
 
-	if (dev->attached == 0) {
+	if (!dev->attached) {
 		comedi_error(dev, "premature interrupt");
 		return IRQ_HANDLED;
 	}
@@ -789,6 +778,7 @@ static int das1800_ai_do_cmdtest(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
 				 struct comedi_cmd *cmd)
 {
+	const struct das1800_board *thisboard = comedi_board(dev);
 	struct das1800_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int tmp_arg;
@@ -1232,6 +1222,7 @@ static int das1800_ai_rinsn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
 {
+	const struct das1800_board *thisboard = comedi_board(dev);
 	int i, n;
 	int chan, range, aref, chan_range;
 	int timeout = 1000;
@@ -1295,6 +1286,7 @@ static int das1800_ao_winsn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
 {
+	const struct das1800_board *thisboard = comedi_board(dev);
 	struct das1800_private *devpriv = dev->private;
 	int chan = CR_CHAN(insn->chanspec);
 /* int range = CR_RANGE(insn->chanspec); */
@@ -1516,46 +1508,23 @@ static int das1800_probe(struct comedi_device *dev)
 static int das1800_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
+	const struct das1800_board *thisboard = comedi_board(dev);
 	struct das1800_private *devpriv;
 	struct comedi_subdevice *s;
-	unsigned long iobase = it->options[0];
 	unsigned int irq = it->options[1];
 	unsigned int dma0 = it->options[2];
 	unsigned int dma1 = it->options[3];
-	unsigned long iobase2;
 	int board;
-	int retval;
+	int ret;
 
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	printk(KERN_DEBUG "comedi%d: %s: io 0x%lx", dev->minor,
-	       dev->driver->driver_name, iobase);
-	if (irq) {
-		printk(KERN_CONT ", irq %u", irq);
-		if (dma0) {
-			printk(KERN_CONT ", dma %u", dma0);
-			if (dma1)
-				printk(KERN_CONT " and %u", dma1);
-		}
-	}
-	printk(KERN_CONT "\n");
-
-	if (iobase == 0) {
-		dev_err(dev->class_dev, "io base address required\n");
-		return -EINVAL;
-	}
-
-	/* check if io addresses are available */
-	if (!request_region(iobase, DAS1800_SIZE, dev->driver->driver_name)) {
-		printk
-		    (" I/O port conflict: failed to allocate ports 0x%lx to 0x%lx\n",
-		     iobase, iobase + DAS1800_SIZE - 1);
-		return -EIO;
-	}
-	dev->iobase = iobase;
+	ret = comedi_request_region(dev, it->options[0], DAS1800_SIZE);
+	if (ret)
+		return ret;
 
 	board = das1800_probe(dev);
 	if (board < 0) {
@@ -1564,18 +1533,16 @@ static int das1800_attach(struct comedi_device *dev,
 	}
 
 	dev->board_ptr = das1800_boards + board;
+	thisboard = comedi_board(dev);
 	dev->board_name = thisboard->name;
 
 	/*  if it is an 'ao' board with fancy analog out then we need extra io ports */
 	if (thisboard->ao_ability == 2) {
-		iobase2 = iobase + IOBASE2;
-		if (!request_region(iobase2, DAS1800_SIZE,
-				    dev->driver->driver_name)) {
-			printk
-			    (" I/O port conflict: failed to allocate ports 0x%lx to 0x%lx\n",
-			     iobase2, iobase2 + DAS1800_SIZE - 1);
-			return -EIO;
-		}
+		unsigned long iobase2 = dev->iobase + IOBASE2;
+
+		ret = __comedi_request_region(dev, iobase2, DAS1800_SIZE);
+		if (ret)
+			return ret;
 		devpriv->iobase2 = iobase2;
 	}
 
@@ -1618,9 +1585,9 @@ static int das1800_attach(struct comedi_device *dev,
 		break;
 	}
 
-	retval = das1800_init_dma(dev, dma0, dma1);
-	if (retval < 0)
-		return retval;
+	ret = das1800_init_dma(dev, dma0, dma1);
+	if (ret < 0)
+		return ret;
 
 	if (devpriv->ai_buf0 == NULL) {
 		devpriv->ai_buf0 =
@@ -1629,9 +1596,9 @@ static int das1800_attach(struct comedi_device *dev,
 			return -ENOMEM;
 	}
 
-	retval = comedi_alloc_subdevices(dev, 4);
-	if (retval)
-		return retval;
+	ret = comedi_alloc_subdevices(dev, 4);
+	if (ret)
+		return ret;
 
 	/* analog input subdevice */
 	s = &dev->subdevices[0];
@@ -1657,7 +1624,7 @@ static int das1800_attach(struct comedi_device *dev,
 		s->subdev_flags = SDF_WRITABLE;
 		s->n_chan = thisboard->ao_n_chan;
 		s->maxdata = (1 << thisboard->resolution) - 1;
-		s->range_table = &range_ao_1;
+		s->range_table = &range_bipolar10;
 		s->insn_write = das1800_ao_winsn;
 	} else {
 		s->type = COMEDI_SUBD_UNUSED;
@@ -1701,21 +1668,18 @@ static void das1800_detach(struct comedi_device *dev)
 {
 	struct das1800_private *devpriv = dev->private;
 
-	if (dev->iobase)
-		release_region(dev->iobase, DAS1800_SIZE);
-	if (dev->irq)
-		free_irq(dev->irq, dev);
 	if (devpriv) {
-		if (devpriv->iobase2)
-			release_region(devpriv->iobase2, DAS1800_SIZE);
 		if (devpriv->dma0)
 			free_dma(devpriv->dma0);
 		if (devpriv->dma1)
 			free_dma(devpriv->dma1);
 		kfree(devpriv->ai_buf0);
 		kfree(devpriv->ai_buf1);
+		if (devpriv->iobase2)
+			release_region(devpriv->iobase2, DAS1800_SIZE);
 	}
-};
+	comedi_legacy_detach(dev);
+}
 
 static struct comedi_driver das1800_driver = {
 	.driver_name	= "das1800",
diff --git a/drivers/staging/comedi/drivers/das6402.c b/drivers/staging/comedi/drivers/das6402.c
index 2efddb89bbc..11424fb5b4d 100644
--- a/drivers/staging/comedi/drivers/das6402.c
+++ b/drivers/staging/comedi/drivers/das6402.c
@@ -284,23 +284,12 @@ static int das6402_attach(struct comedi_device *dev,
 {
 	struct das6402_private *devpriv;
 	unsigned int irq;
-	unsigned long iobase;
 	int ret;
 	struct comedi_subdevice *s;
 
-	dev->board_name = "das6402";
-
-	iobase = it->options[0];
-	if (iobase == 0)
-		iobase = 0x300;
-
-	if (!request_region(iobase, DAS6402_SIZE, "das6402")) {
-		dev_err(dev->class_dev, "I/O port conflict\n");
-		return -EIO;
-	}
-	dev->iobase = iobase;
-
-	/* should do a probe here */
+	ret = comedi_request_region(dev, it->options[0], DAS6402_SIZE);
+	if (ret)
+		return ret;
 
 	irq = it->options[0];
 	dev_dbg(dev->class_dev, "( irq = %u )\n", irq);
@@ -335,19 +324,11 @@ static int das6402_attach(struct comedi_device *dev,
 	return 0;
 }
 
-static void das6402_detach(struct comedi_device *dev)
-{
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (dev->iobase)
-		release_region(dev->iobase, DAS6402_SIZE);
-}
-
 static struct comedi_driver das6402_driver = {
 	.driver_name	= "das6402",
 	.module		= THIS_MODULE,
 	.attach		= das6402_attach,
-	.detach		= das6402_detach,
+	.detach		= comedi_legacy_detach,
 };
 module_comedi_driver(das6402_driver)
 
diff --git a/drivers/staging/comedi/drivers/das800.c b/drivers/staging/comedi/drivers/das800.c
index 38f625be812..9ce6cbcc7ee 100644
--- a/drivers/staging/comedi/drivers/das800.c
+++ b/drivers/staging/comedi/drivers/das800.c
@@ -95,8 +95,8 @@ cmd triggers supported:
 #define   IRQ                   0x8
 #define   BUSY                  0x80
 #define DAS800_GAIN           3
-#define   CIO_FFOV              0x8	/*  fifo overflow for cio-das802/16 */
-#define   CIO_ENHF              0x90	/*  interrupt fifo half full for cio-das802/16 */
+#define   CIO_FFOV              0x8   /* cio-das802/16 fifo overflow */
+#define   CIO_ENHF              0x90  /* cio-das802/16 fifo half full int ena */
 #define   CONTROL1              0x80
 #define   CONV_CONTROL          0xa0
 #define   SCAN_LIMITS           0xc0
@@ -107,6 +107,8 @@ cmd triggers supported:
 #define   STATUS2_INTE          0X20
 #define DAS800_ID             7
 
+#define DAS802_16_HALF_FIFO_SZ	128
+
 struct das800_board {
 	const char *name;
 	int ai_speed;
@@ -114,491 +116,204 @@ struct das800_board {
 	int resolution;
 };
 
-/* analog input ranges */
-static const struct comedi_lrange range_das800_ai = {
-	1,
-	{
-	 RANGE(-5, 5),
-	 }
-};
-
 static const struct comedi_lrange range_das801_ai = {
-	9,
-	{
-	 RANGE(-5, 5),
-	 RANGE(-10, 10),
-	 RANGE(0, 10),
-	 RANGE(-0.5, 0.5),
-	 RANGE(0, 1),
-	 RANGE(-0.05, 0.05),
-	 RANGE(0, 0.1),
-	 RANGE(-0.01, 0.01),
-	 RANGE(0, 0.02),
-	 }
+	9, {
+		BIP_RANGE(5),
+		BIP_RANGE(10),
+		UNI_RANGE(10),
+		BIP_RANGE(0.5),
+		UNI_RANGE(1),
+		BIP_RANGE(0.05),
+		UNI_RANGE(0.1),
+		BIP_RANGE(0.01),
+		UNI_RANGE(0.02)
+	}
 };
 
 static const struct comedi_lrange range_cio_das801_ai = {
-	9,
-	{
-	 RANGE(-5, 5),
-	 RANGE(-10, 10),
-	 RANGE(0, 10),
-	 RANGE(-0.5, 0.5),
-	 RANGE(0, 1),
-	 RANGE(-0.05, 0.05),
-	 RANGE(0, 0.1),
-	 RANGE(-0.005, 0.005),
-	 RANGE(0, 0.01),
-	 }
+	9, {
+		BIP_RANGE(5),
+		BIP_RANGE(10),
+		UNI_RANGE(10),
+		BIP_RANGE(0.5),
+		UNI_RANGE(1),
+		BIP_RANGE(0.05),
+		UNI_RANGE(0.1),
+		BIP_RANGE(0.005),
+		UNI_RANGE(0.01)
+	}
 };
 
 static const struct comedi_lrange range_das802_ai = {
-	9,
-	{
-	 RANGE(-5, 5),
-	 RANGE(-10, 10),
-	 RANGE(0, 10),
-	 RANGE(-2.5, 2.5),
-	 RANGE(0, 5),
-	 RANGE(-1.25, 1.25),
-	 RANGE(0, 2.5),
-	 RANGE(-0.625, 0.625),
-	 RANGE(0, 1.25),
-	 }
+	9, {
+		BIP_RANGE(5),
+		BIP_RANGE(10),
+		UNI_RANGE(10),
+		BIP_RANGE(2.5),
+		UNI_RANGE(5),
+		BIP_RANGE(1.25),
+		UNI_RANGE(2.5),
+		BIP_RANGE(0.625),
+		UNI_RANGE(1.25)
+	}
 };
 
 static const struct comedi_lrange range_das80216_ai = {
-	8,
-	{
-	 RANGE(-10, 10),
-	 RANGE(0, 10),
-	 RANGE(-5, 5),
-	 RANGE(0, 5),
-	 RANGE(-2.5, 2.5),
-	 RANGE(0, 2.5),
-	 RANGE(-1.25, 1.25),
-	 RANGE(0, 1.25),
-	 }
+	8, {
+		BIP_RANGE(10),
+		UNI_RANGE(10),
+		BIP_RANGE(5),
+		UNI_RANGE(5),
+		BIP_RANGE(2.5),
+		UNI_RANGE(2.5),
+		BIP_RANGE(1.25),
+		UNI_RANGE(1.25)
+	}
 };
 
-enum { das800, ciodas800, das801, ciodas801, das802, ciodas802, ciodas80216 };
+enum das800_boardinfo {
+	BOARD_DAS800,
+	BOARD_CIODAS800,
+	BOARD_DAS801,
+	BOARD_CIODAS801,
+	BOARD_DAS802,
+	BOARD_CIODAS802,
+	BOARD_CIODAS80216,
+};
 
 static const struct das800_board das800_boards[] = {
-	{
-	 .name = "das-800",
-	 .ai_speed = 25000,
-	 .ai_range = &range_das800_ai,
-	 .resolution = 12,
-	 },
-	{
-	 .name = "cio-das800",
-	 .ai_speed = 20000,
-	 .ai_range = &range_das800_ai,
-	 .resolution = 12,
-	 },
-	{
-	 .name = "das-801",
-	 .ai_speed = 25000,
-	 .ai_range = &range_das801_ai,
-	 .resolution = 12,
-	 },
-	{
-	 .name = "cio-das801",
-	 .ai_speed = 20000,
-	 .ai_range = &range_cio_das801_ai,
-	 .resolution = 12,
-	 },
-	{
-	 .name = "das-802",
-	 .ai_speed = 25000,
-	 .ai_range = &range_das802_ai,
-	 .resolution = 12,
-	 },
-	{
-	 .name = "cio-das802",
-	 .ai_speed = 20000,
-	 .ai_range = &range_das802_ai,
-	 .resolution = 12,
-	 },
-	{
-	 .name = "cio-das802/16",
-	 .ai_speed = 10000,
-	 .ai_range = &range_das80216_ai,
-	 .resolution = 16,
-	 },
+	[BOARD_DAS800] = {
+		.name		= "das-800",
+		.ai_speed	= 25000,
+		.ai_range	= &range_bipolar5,
+		.resolution	= 12,
+	},
+	[BOARD_CIODAS800] = {
+		.name		= "cio-das800",
+		.ai_speed	= 20000,
+		.ai_range	= &range_bipolar5,
+		.resolution	= 12,
+	},
+	[BOARD_DAS801] = {
+		.name		= "das-801",
+		.ai_speed	= 25000,
+		.ai_range	= &range_das801_ai,
+		.resolution	= 12,
+	},
+	[BOARD_CIODAS801] = {
+		.name		= "cio-das801",
+		.ai_speed	= 20000,
+		.ai_range	= &range_cio_das801_ai,
+		.resolution	= 12,
+	},
+	[BOARD_DAS802] = {
+		.name		= "das-802",
+		.ai_speed	= 25000,
+		.ai_range	= &range_das802_ai,
+		.resolution	= 12,
+	},
+	[BOARD_CIODAS802] = {
+		.name		= "cio-das802",
+		.ai_speed	= 20000,
+		.ai_range	= &range_das802_ai,
+		.resolution	= 12,
+	},
+	[BOARD_CIODAS80216] = {
+		.name		= "cio-das802/16",
+		.ai_speed	= 10000,
+		.ai_range	= &range_das80216_ai,
+		.resolution	= 16,
+	},
 };
 
-/*
- * Useful for shorthand access to the particular board structure
- */
-#define thisboard ((const struct das800_board *)dev->board_ptr)
-
 struct das800_private {
-	volatile unsigned int count;	/* number of data points left to be taken */
-	volatile int forever;	/* flag indicating whether we should take data forever */
-	unsigned int divisor1;	/* value to load into board's counter 1 for timed conversions */
-	unsigned int divisor2;	/* value to load into board's counter 2 for timed conversions */
-	volatile int do_bits;	/* digital output bits */
-};
-
-static int das800_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static void das800_detach(struct comedi_device *dev);
-static int das800_cancel(struct comedi_device *dev, struct comedi_subdevice *s);
-
-static struct comedi_driver driver_das800 = {
-	.driver_name = "das800",
-	.module = THIS_MODULE,
-	.attach = das800_attach,
-	.detach = das800_detach,
-	.num_names = ARRAY_SIZE(das800_boards),
-	.board_name = &das800_boards[0].name,
-	.offset = sizeof(struct das800_board),
+	unsigned int count;	/* number of data points left to be taken */
+	unsigned int divisor1;	/* counter 1 value for timed conversions */
+	unsigned int divisor2;	/* counter 2 value for timed conversions */
+	unsigned int do_bits;	/* digital output bits */
+	bool forever;		/* flag that we should take data forever */
 };
 
-static irqreturn_t das800_interrupt(int irq, void *d);
-static void enable_das800(struct comedi_device *dev);
-static void disable_das800(struct comedi_device *dev);
-static int das800_ai_do_cmdtest(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_cmd *cmd);
-static int das800_ai_do_cmd(struct comedi_device *dev,
-			    struct comedi_subdevice *s);
-static int das800_ai_rinsn(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_insn *insn,
-			   unsigned int *data);
-static int das800_di_rbits(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_insn *insn,
-			   unsigned int *data);
-static int das800_do_wbits(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_insn *insn,
-			   unsigned int *data);
-static int das800_probe(struct comedi_device *dev);
-static int das800_set_frequency(struct comedi_device *dev);
-
-/* checks and probes das-800 series board type */
-static int das800_probe(struct comedi_device *dev)
+static void das800_ind_write(struct comedi_device *dev,
+			     unsigned val, unsigned reg)
 {
-	int id_bits;
-	unsigned long irq_flags;
-	int board;
-
-	/*  'comedi spin lock irqsave' disables even rt interrupts, we use them to protect indirect addressing */
-	spin_lock_irqsave(&dev->spinlock, irq_flags);
-	outb(ID, dev->iobase + DAS800_GAIN);	/* select base address + 7 to be ID register */
-	id_bits = inb(dev->iobase + DAS800_ID) & 0x3;	/* get id bits */
-	spin_unlock_irqrestore(&dev->spinlock, irq_flags);
-
-	board = thisboard - das800_boards;
-
-	switch (id_bits) {
-	case 0x0:
-		if (board == das800) {
-			dev_dbg(dev->class_dev, "Board model: DAS-800\n");
-			return board;
-		}
-		if (board == ciodas800) {
-			dev_dbg(dev->class_dev, "Board model: CIO-DAS800\n");
-			return board;
-		}
-		dev_dbg(dev->class_dev, "Board model (probed): DAS-800\n");
-		return das800;
-		break;
-	case 0x2:
-		if (board == das801) {
-			dev_dbg(dev->class_dev, "Board model: DAS-801\n");
-			return board;
-		}
-		if (board == ciodas801) {
-			dev_dbg(dev->class_dev, "Board model: CIO-DAS801\n");
-			return board;
-		}
-		dev_dbg(dev->class_dev, "Board model (probed): DAS-801\n");
-		return das801;
-		break;
-	case 0x3:
-		if (board == das802) {
-			dev_dbg(dev->class_dev, "Board model: DAS-802\n");
-			return board;
-		}
-		if (board == ciodas802) {
-			dev_dbg(dev->class_dev, "Board model: CIO-DAS802\n");
-			return board;
-		}
-		if (board == ciodas80216) {
-			dev_dbg(dev->class_dev, "Board model: CIO-DAS802/16\n");
-			return board;
-		}
-		dev_dbg(dev->class_dev, "Board model (probed): DAS-802\n");
-		return das802;
-		break;
-	default:
-		dev_dbg(dev->class_dev,
-			"Board model: probe returned 0x%x (unknown)\n",
-			id_bits);
-		return board;
-		break;
-	}
-	return -1;
+	/*
+	 * Select dev->iobase + 2 to be desired register
+	 * then write to that register.
+	 */
+	outb(reg, dev->iobase + DAS800_GAIN);
+	outb(val, dev->iobase + 2);
 }
 
-module_comedi_driver(driver_das800);
+static unsigned das800_ind_read(struct comedi_device *dev, unsigned reg)
+{
+	/*
+	 * Select dev->iobase + 7 to be desired register
+	 * then read from that register.
+	 */
+	outb(reg, dev->iobase + DAS800_GAIN);
+	return inb(dev->iobase + 7);
+}
 
-/* interrupt service routine */
-static irqreturn_t das800_interrupt(int irq, void *d)
+static void das800_enable(struct comedi_device *dev)
 {
-	short i;		/* loop index */
-	short dataPoint = 0;
-	struct comedi_device *dev = d;
+	const struct das800_board *thisboard = comedi_board(dev);
 	struct das800_private *devpriv = dev->private;
-	struct comedi_subdevice *s = dev->read_subdev;	/* analog input subdevice */
-	struct comedi_async *async;
-	int status;
 	unsigned long irq_flags;
-	static const int max_loops = 128;	/*  half-fifo size for cio-das802/16 */
-	/*  flags */
-	int fifo_empty = 0;
-	int fifo_overflow = 0;
 
-	status = inb(dev->iobase + DAS800_STATUS);
-	/* if interrupt was not generated by board or driver not attached, quit */
-	if (!(status & IRQ))
-		return IRQ_NONE;
-	if (!(dev->attached))
-		return IRQ_HANDLED;
-
-	/* wait until here to initialize async, since we will get null dereference
-	 * if interrupt occurs before driver is fully attached!
-	 */
-	async = s->async;
-
-	/*  if hardware conversions are not enabled, then quit */
 	spin_lock_irqsave(&dev->spinlock, irq_flags);
-	outb(CONTROL1, dev->iobase + DAS800_GAIN);	/* select base address + 7 to be STATUS2 register */
-	status = inb(dev->iobase + DAS800_STATUS2) & STATUS2_HCEN;
-	/* don't release spinlock yet since we want to make sure no one else disables hardware conversions */
-	if (status == 0) {
-		spin_unlock_irqrestore(&dev->spinlock, irq_flags);
-		return IRQ_HANDLED;
-	}
-
-	/* loop while card's fifo is not empty (and limit to half fifo for cio-das802/16) */
-	for (i = 0; i < max_loops; i++) {
-		/* read 16 bits from dev->iobase and dev->iobase + 1 */
-		dataPoint = inb(dev->iobase + DAS800_LSB);
-		dataPoint += inb(dev->iobase + DAS800_MSB) << 8;
-		if (thisboard->resolution == 12) {
-			fifo_empty = dataPoint & FIFO_EMPTY;
-			fifo_overflow = dataPoint & FIFO_OVF;
-			if (fifo_overflow)
-				break;
-		} else {
-			fifo_empty = 0;	/*  cio-das802/16 has no fifo empty status bit */
-		}
-		if (fifo_empty)
-			break;
-		/* strip off extraneous bits for 12 bit cards */
-		if (thisboard->resolution == 12)
-			dataPoint = (dataPoint >> 4) & 0xfff;
-		/* if there are more data points to collect */
-		if (devpriv->count > 0 || devpriv->forever == 1) {
-			/* write data point to buffer */
-			cfc_write_to_buffer(s, dataPoint);
-			if (devpriv->count > 0)
-				devpriv->count--;
-		}
-	}
-	async->events |= COMEDI_CB_BLOCK;
-	/* check for fifo overflow */
-	if (thisboard->resolution == 12) {
-		fifo_overflow = dataPoint & FIFO_OVF;
-		/*  else cio-das802/16 */
-	} else {
-		fifo_overflow = inb(dev->iobase + DAS800_GAIN) & CIO_FFOV;
-	}
-	if (fifo_overflow) {
-		spin_unlock_irqrestore(&dev->spinlock, irq_flags);
-		comedi_error(dev, "DAS800 FIFO overflow");
-		das800_cancel(dev, s);
-		async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
-		comedi_event(dev, s);
-		async->events = 0;
-		return IRQ_HANDLED;
-	}
-	if (devpriv->count > 0 || devpriv->forever == 1) {
-		/* Re-enable card's interrupt.
-		 * We already have spinlock, so indirect addressing is safe */
-		outb(CONTROL1, dev->iobase + DAS800_GAIN);	/* select dev->iobase + 2 to be control register 1 */
-		outb(CONTROL1_INTE | devpriv->do_bits,
-		     dev->iobase + DAS800_CONTROL1);
-		spin_unlock_irqrestore(&dev->spinlock, irq_flags);
-		/* otherwise, stop taking data */
-	} else {
-		spin_unlock_irqrestore(&dev->spinlock, irq_flags);
-		disable_das800(dev);	/* disable hardware triggered conversions */
-		async->events |= COMEDI_CB_EOA;
-	}
-	comedi_event(dev, s);
-	async->events = 0;
-	return IRQ_HANDLED;
+	/*  enable fifo-half full interrupts for cio-das802/16 */
+	if (thisboard->resolution == 16)
+		outb(CIO_ENHF, dev->iobase + DAS800_GAIN);
+	/* enable hardware triggering */
+	das800_ind_write(dev, CONV_HCEN, CONV_CONTROL);
+	/* enable card's interrupt */
+	das800_ind_write(dev, CONTROL1_INTE | devpriv->do_bits, CONTROL1);
+	spin_unlock_irqrestore(&dev->spinlock, irq_flags);
 }
 
-static int das800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+static void das800_disable(struct comedi_device *dev)
 {
-	struct das800_private *devpriv;
-	struct comedi_subdevice *s;
-	unsigned long iobase = it->options[0];
-	unsigned int irq = it->options[1];
 	unsigned long irq_flags;
-	int board;
-	int ret;
-
-	dev_info(dev->class_dev, "das800: io 0x%lx\n", iobase);
-	if (irq)
-		dev_dbg(dev->class_dev, "irq %u\n", irq);
 
-	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
-	if (!devpriv)
-		return -ENOMEM;
-	dev->private = devpriv;
-
-	if (iobase == 0) {
-		dev_err(dev->class_dev,
-			"io base address required for das800\n");
-		return -EINVAL;
-	}
-
-	/* check if io addresses are available */
-	if (!request_region(iobase, DAS800_SIZE, "das800")) {
-		dev_err(dev->class_dev, "I/O port conflict\n");
-		return -EIO;
-	}
-	dev->iobase = iobase;
-
-	board = das800_probe(dev);
-	if (board < 0) {
-		dev_dbg(dev->class_dev, "unable to determine board type\n");
-		return -ENODEV;
-	}
-	dev->board_ptr = das800_boards + board;
-
-	/* grab our IRQ */
-	if (irq == 1 || irq > 7) {
-		dev_err(dev->class_dev, "irq out of range\n");
-		return -EINVAL;
-	}
-	if (irq) {
-		if (request_irq(irq, das800_interrupt, 0, "das800", dev)) {
-			dev_err(dev->class_dev, "unable to allocate irq %u\n",
-				irq);
-			return -EINVAL;
-		}
-	}
-	dev->irq = irq;
-
-	dev->board_name = thisboard->name;
-
-	ret = comedi_alloc_subdevices(dev, 3);
-	if (ret)
-		return ret;
-
-	/* analog input subdevice */
-	s = &dev->subdevices[0];
-	dev->read_subdev = s;
-	s->type = COMEDI_SUBD_AI;
-	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_CMD_READ;
-	s->n_chan = 8;
-	s->len_chanlist = 8;
-	s->maxdata = (1 << thisboard->resolution) - 1;
-	s->range_table = thisboard->ai_range;
-	s->do_cmd = das800_ai_do_cmd;
-	s->do_cmdtest = das800_ai_do_cmdtest;
-	s->insn_read = das800_ai_rinsn;
-	s->cancel = das800_cancel;
-
-	/* di */
-	s = &dev->subdevices[1];
-	s->type = COMEDI_SUBD_DI;
-	s->subdev_flags = SDF_READABLE;
-	s->n_chan = 3;
-	s->maxdata = 1;
-	s->range_table = &range_digital;
-	s->insn_bits = das800_di_rbits;
-
-	/* do */
-	s = &dev->subdevices[2];
-	s->type = COMEDI_SUBD_DO;
-	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
-	s->n_chan = 4;
-	s->maxdata = 1;
-	s->range_table = &range_digital;
-	s->insn_bits = das800_do_wbits;
-
-	disable_das800(dev);
-
-	/* initialize digital out channels */
 	spin_lock_irqsave(&dev->spinlock, irq_flags);
-	outb(CONTROL1, dev->iobase + DAS800_GAIN);	/* select dev->iobase + 2 to be control register 1 */
-	outb(CONTROL1_INTE | devpriv->do_bits, dev->iobase + DAS800_CONTROL1);
+	/* disable hardware triggering of conversions */
+	das800_ind_write(dev, 0x0, CONV_CONTROL);
 	spin_unlock_irqrestore(&dev->spinlock, irq_flags);
+}
 
-	return 0;
-};
-
-static void das800_detach(struct comedi_device *dev)
-{
-	if (dev->iobase)
-		release_region(dev->iobase, DAS800_SIZE);
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-};
-
-static int das800_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
+static int das800_set_frequency(struct comedi_device *dev)
 {
 	struct das800_private *devpriv = dev->private;
+	int err = 0;
+
+	if (i8254_load(dev->iobase + DAS800_8254, 0, 1, devpriv->divisor1, 2))
+		err++;
+	if (i8254_load(dev->iobase + DAS800_8254, 0, 2, devpriv->divisor2, 2))
+		err++;
+	if (err)
+		return -1;
 
-	devpriv->forever = 0;
-	devpriv->count = 0;
-	disable_das800(dev);
 	return 0;
 }
 
-/* enable_das800 makes the card start taking hardware triggered conversions */
-static void enable_das800(struct comedi_device *dev)
+static int das800_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 {
 	struct das800_private *devpriv = dev->private;
-	unsigned long irq_flags;
 
-	spin_lock_irqsave(&dev->spinlock, irq_flags);
-	/*  enable fifo-half full interrupts for cio-das802/16 */
-	if (thisboard->resolution == 16)
-		outb(CIO_ENHF, dev->iobase + DAS800_GAIN);
-	outb(CONV_CONTROL, dev->iobase + DAS800_GAIN);	/* select dev->iobase + 2 to be conversion control register */
-	outb(CONV_HCEN, dev->iobase + DAS800_CONV_CONTROL);	/* enable hardware triggering */
-	outb(CONTROL1, dev->iobase + DAS800_GAIN);	/* select dev->iobase + 2 to be control register 1 */
-	outb(CONTROL1_INTE | devpriv->do_bits, dev->iobase + DAS800_CONTROL1);	/* enable card's interrupt */
-	spin_unlock_irqrestore(&dev->spinlock, irq_flags);
-}
-
-/* disable_das800 stops hardware triggered conversions */
-static void disable_das800(struct comedi_device *dev)
-{
-	unsigned long irq_flags;
-	spin_lock_irqsave(&dev->spinlock, irq_flags);
-	outb(CONV_CONTROL, dev->iobase + DAS800_GAIN);	/* select dev->iobase + 2 to be conversion control register */
-	outb(0x0, dev->iobase + DAS800_CONV_CONTROL);	/* disable hardware triggering of conversions */
-	spin_unlock_irqrestore(&dev->spinlock, irq_flags);
+	devpriv->forever = false;
+	devpriv->count = 0;
+	das800_disable(dev);
+	return 0;
 }
 
 static int das800_ai_do_cmdtest(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_cmd *cmd)
 {
+	const struct das800_board *thisboard = comedi_board(dev);
 	struct das800_private *devpriv = dev->private;
 	int err = 0;
-	int tmp;
-	int gain, startChan;
-	int i;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -644,11 +359,13 @@ static int das800_ai_do_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
+		int tmp = cmd->convert_arg;
+
 		/* calculate counter values that give desired timing */
-		i8253_cascade_ns_to_timer_2div(TIMER_BASE, &(devpriv->divisor1),
-					       &(devpriv->divisor2),
-					       &(cmd->convert_arg),
+		i8253_cascade_ns_to_timer_2div(TIMER_BASE,
+					       &devpriv->divisor1,
+					       &devpriv->divisor2,
+					       &cmd->convert_arg,
 					       cmd->flags & TRIG_ROUND_MASK);
 		if (tmp != cmd->convert_arg)
 			err++;
@@ -659,18 +376,21 @@ static int das800_ai_do_cmdtest(struct comedi_device *dev,
 
 	/*  check channel/gain list against card's limitations */
 	if (cmd->chanlist) {
-		gain = CR_RANGE(cmd->chanlist[0]);
-		startChan = CR_CHAN(cmd->chanlist[0]);
+		unsigned int chan = CR_CHAN(cmd->chanlist[0]);
+		unsigned int range = CR_RANGE(cmd->chanlist[0]);
+		unsigned int next;
+		int i;
+
 		for (i = 1; i < cmd->chanlist_len; i++) {
-			if (CR_CHAN(cmd->chanlist[i]) !=
-			    (startChan + i) % N_CHAN_AI) {
-				comedi_error(dev,
-					     "entries in chanlist must be consecutive channels, counting upwards\n");
+			next = cmd->chanlist[i];
+			if (CR_CHAN(next) != (chan + i) % N_CHAN_AI) {
+				dev_err(dev->class_dev,
+					"chanlist must be consecutive, counting upwards\n");
 				err++;
 			}
-			if (CR_RANGE(cmd->chanlist[i]) != gain) {
-				comedi_error(dev,
-					     "entries in chanlist must all have the same gain\n");
+			if (CR_RANGE(next) != range) {
+				dev_err(dev->class_dev,
+					"chanlist must all have the same gain\n");
 				err++;
 			}
 		}
@@ -685,32 +405,24 @@ static int das800_ai_do_cmdtest(struct comedi_device *dev,
 static int das800_ai_do_cmd(struct comedi_device *dev,
 			    struct comedi_subdevice *s)
 {
+	const struct das800_board *thisboard = comedi_board(dev);
 	struct das800_private *devpriv = dev->private;
-	int startChan, endChan, scan, gain;
+	struct comedi_async *async = s->async;
+	unsigned int gain = CR_RANGE(async->cmd.chanlist[0]);
+	unsigned int start_chan = CR_CHAN(async->cmd.chanlist[0]);
+	unsigned int end_chan = (start_chan + async->cmd.chanlist_len - 1) % 8;
+	unsigned int scan_chans = (end_chan << 3) | start_chan;
 	int conv_bits;
 	unsigned long irq_flags;
-	struct comedi_async *async = s->async;
 
-	if (!dev->irq) {
-		comedi_error(dev,
-			     "no irq assigned for das-800, cannot do hardware conversions");
-		return -1;
-	}
-
-	disable_das800(dev);
-
-	/* set channel scan limits */
-	startChan = CR_CHAN(async->cmd.chanlist[0]);
-	endChan = (startChan + async->cmd.chanlist_len - 1) % 8;
-	scan = (endChan << 3) | startChan;
+	das800_disable(dev);
 
 	spin_lock_irqsave(&dev->spinlock, irq_flags);
-	outb(SCAN_LIMITS, dev->iobase + DAS800_GAIN);	/* select base address + 2 to be scan limits register */
-	outb(scan, dev->iobase + DAS800_SCAN_LIMITS);	/* set scan limits */
+	/* set scan limits */
+	das800_ind_write(dev, scan_chans, SCAN_LIMITS);
 	spin_unlock_irqrestore(&dev->spinlock, irq_flags);
 
 	/* set gain */
-	gain = CR_RANGE(async->cmd.chanlist[0]);
 	if (thisboard->resolution == 12 && gain > 0)
 		gain += 0x7;
 	gain &= 0xf;
@@ -719,10 +431,10 @@ static int das800_ai_do_cmd(struct comedi_device *dev,
 	switch (async->cmd.stop_src) {
 	case TRIG_COUNT:
 		devpriv->count = async->cmd.stop_arg * async->cmd.chanlist_len;
-		devpriv->forever = 0;
+		devpriv->forever = false;
 		break;
 	case TRIG_NONE:
-		devpriv->forever = 1;
+		devpriv->forever = true;
 		devpriv->count = 0;
 		break;
 	default:
@@ -740,11 +452,6 @@ static int das800_ai_do_cmd(struct comedi_device *dev,
 	case TRIG_TIMER:
 		conv_bits |= CASC | ITE;
 		/* set conversion frequency */
-		i8253_cascade_ns_to_timer_2div(TIMER_BASE, &(devpriv->divisor1),
-					       &(devpriv->divisor2),
-					       &(async->cmd.convert_arg),
-					       async->cmd.
-					       flags & TRIG_ROUND_MASK);
 		if (das800_set_frequency(dev) < 0) {
 			comedi_error(dev, "Error setting up counters");
 			return -1;
@@ -757,125 +464,333 @@ static int das800_ai_do_cmd(struct comedi_device *dev,
 	}
 
 	spin_lock_irqsave(&dev->spinlock, irq_flags);
-	outb(CONV_CONTROL, dev->iobase + DAS800_GAIN);	/* select dev->iobase + 2 to be conversion control register */
-	outb(conv_bits, dev->iobase + DAS800_CONV_CONTROL);
+	das800_ind_write(dev, conv_bits, CONV_CONTROL);
 	spin_unlock_irqrestore(&dev->spinlock, irq_flags);
+
 	async->events = 0;
-	enable_das800(dev);
+	das800_enable(dev);
 	return 0;
 }
 
-static int das800_ai_rinsn(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_insn *insn,
-			   unsigned int *data)
+static unsigned int das800_ai_get_sample(struct comedi_device *dev)
 {
+	unsigned int lsb = inb(dev->iobase + DAS800_LSB);
+	unsigned int msb = inb(dev->iobase + DAS800_MSB);
+
+	return (msb << 8) | lsb;
+}
+
+static irqreturn_t das800_interrupt(int irq, void *d)
+{
+	struct comedi_device *dev = d;
 	struct das800_private *devpriv = dev->private;
-	int i, n;
-	int chan;
-	int range;
-	int lsb, msb;
-	int timeout = 1000;
+	struct comedi_subdevice *s = dev->read_subdev;
+	struct comedi_async *async = s ? s->async : NULL;
 	unsigned long irq_flags;
+	unsigned int status;
+	unsigned int val;
+	bool fifo_empty;
+	bool fifo_overflow;
+	int i;
 
-	disable_das800(dev);	/* disable hardware conversions (enables software conversions) */
+	status = inb(dev->iobase + DAS800_STATUS);
+	if (!(status & IRQ))
+		return IRQ_NONE;
+	if (!dev->attached)
+		return IRQ_HANDLED;
 
-	/* set multiplexer */
-	chan = CR_CHAN(insn->chanspec);
+	spin_lock_irqsave(&dev->spinlock, irq_flags);
+	status = das800_ind_read(dev, CONTROL1) & STATUS2_HCEN;
+	/*
+	 * Don't release spinlock yet since we want to make sure
+	 * no one else disables hardware conversions.
+	 */
 
+	/* if hardware conversions are not enabled, then quit */
+	if (status == 0) {
+		spin_unlock_irqrestore(&dev->spinlock, irq_flags);
+		return IRQ_HANDLED;
+	}
+
+	for (i = 0; i < DAS802_16_HALF_FIFO_SZ; i++) {
+		val = das800_ai_get_sample(dev);
+		if (s->maxdata == 0x0fff) {
+			fifo_empty = !!(val & FIFO_EMPTY);
+			fifo_overflow = !!(val & FIFO_OVF);
+		} else {
+			/* cio-das802/16 has no fifo empty status bit */
+			fifo_empty = false;
+			fifo_overflow = !!(inb(dev->iobase + DAS800_GAIN) &
+						CIO_FFOV);
+		}
+		if (fifo_empty || fifo_overflow)
+			break;
+
+		if (s->maxdata == 0x0fff)
+			val >>= 4;	/* 12-bit sample */
+
+		/* if there are more data points to collect */
+		if (devpriv->count > 0 || devpriv->forever) {
+			/* write data point to buffer */
+			cfc_write_to_buffer(s, val & s->maxdata);
+			devpriv->count--;
+		}
+	}
+	async->events |= COMEDI_CB_BLOCK;
+
+	if (fifo_overflow) {
+		spin_unlock_irqrestore(&dev->spinlock, irq_flags);
+		das800_cancel(dev, s);
+		async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
+		comedi_event(dev, s);
+		async->events = 0;
+		return IRQ_HANDLED;
+	}
+
+	if (devpriv->count > 0 || devpriv->forever) {
+		/* Re-enable card's interrupt.
+		 * We already have spinlock, so indirect addressing is safe */
+		das800_ind_write(dev, CONTROL1_INTE | devpriv->do_bits,
+				 CONTROL1);
+		spin_unlock_irqrestore(&dev->spinlock, irq_flags);
+	} else {
+		/* otherwise, stop taking data */
+		spin_unlock_irqrestore(&dev->spinlock, irq_flags);
+		das800_disable(dev);
+		async->events |= COMEDI_CB_EOA;
+	}
+	comedi_event(dev, s);
+	async->events = 0;
+	return IRQ_HANDLED;
+}
+
+static int das800_wait_for_conv(struct comedi_device *dev, int timeout)
+{
+	int i;
+
+	for (i = 0; i < timeout; i++) {
+		if (!(inb(dev->iobase + DAS800_STATUS) & BUSY))
+			return 0;
+	}
+	return -ETIME;
+}
+
+static int das800_ai_insn_read(struct comedi_device *dev,
+			       struct comedi_subdevice *s,
+			       struct comedi_insn *insn,
+			       unsigned int *data)
+{
+	struct das800_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int range = CR_RANGE(insn->chanspec);
+	unsigned long irq_flags;
+	unsigned int val;
+	int ret;
+	int i;
+
+	das800_disable(dev);
+
+	/* set multiplexer */
 	spin_lock_irqsave(&dev->spinlock, irq_flags);
-	outb(CONTROL1, dev->iobase + DAS800_GAIN);	/* select dev->iobase + 2 to be control register 1 */
-	outb(chan | devpriv->do_bits, dev->iobase + DAS800_CONTROL1);
+	das800_ind_write(dev, chan | devpriv->do_bits, CONTROL1);
 	spin_unlock_irqrestore(&dev->spinlock, irq_flags);
 
 	/* set gain / range */
-	range = CR_RANGE(insn->chanspec);
-	if (thisboard->resolution == 12 && range)
+	if (s->maxdata == 0x0fff && range)
 		range += 0x7;
 	range &= 0xf;
 	outb(range, dev->iobase + DAS800_GAIN);
 
 	udelay(5);
 
-	for (n = 0; n < insn->n; n++) {
+	for (i = 0; i < insn->n; i++) {
 		/* trigger conversion */
 		outb_p(0, dev->iobase + DAS800_MSB);
 
-		for (i = 0; i < timeout; i++) {
-			if (!(inb(dev->iobase + DAS800_STATUS) & BUSY))
-				break;
-		}
-		if (i == timeout) {
-			comedi_error(dev, "timeout");
-			return -ETIME;
-		}
-		lsb = inb(dev->iobase + DAS800_LSB);
-		msb = inb(dev->iobase + DAS800_MSB);
-		if (thisboard->resolution == 12) {
-			data[n] = (lsb >> 4) & 0xff;
-			data[n] |= (msb << 4);
-		} else {
-			data[n] = (msb << 8) | lsb;
-		}
+		ret = das800_wait_for_conv(dev, 1000);
+		if (ret)
+			return ret;
+
+		val = das800_ai_get_sample(dev);
+		if (s->maxdata == 0x0fff)
+			val >>= 4;	/* 12-bit sample */
+		data[i] = val & s->maxdata;
 	}
 
-	return n;
+	return insn->n;
 }
 
-static int das800_di_rbits(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_insn *insn,
-			   unsigned int *data)
+static int das800_di_insn_bits(struct comedi_device *dev,
+			       struct comedi_subdevice *s,
+			       struct comedi_insn *insn,
+			       unsigned int *data)
 {
-	unsigned int bits;
-
-	bits = inb(dev->iobase + DAS800_STATUS) >> 4;
-	bits &= 0x7;
-	data[1] = bits;
-	data[0] = 0;
+	data[1] = (inb(dev->iobase + DAS800_STATUS) >> 4) & 0x7;
 
 	return insn->n;
 }
 
-static int das800_do_wbits(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_insn *insn,
-			   unsigned int *data)
+static int das800_do_insn_bits(struct comedi_device *dev,
+			       struct comedi_subdevice *s,
+			       struct comedi_insn *insn,
+			       unsigned int *data)
 {
 	struct das800_private *devpriv = dev->private;
-	int wbits;
+	unsigned int mask = data[0];
+	unsigned int bits = data[1];
 	unsigned long irq_flags;
 
-	/*  only set bits that have been masked */
-	data[0] &= 0xf;
-	wbits = devpriv->do_bits >> 4;
-	wbits &= ~data[0];
-	wbits |= data[0] & data[1];
-	devpriv->do_bits = wbits << 4;
+	if (mask) {
+		s->state &= ~mask;
+		s->state |= (bits & mask);
+		devpriv->do_bits = s->state << 4;
 
-	spin_lock_irqsave(&dev->spinlock, irq_flags);
-	outb(CONTROL1, dev->iobase + DAS800_GAIN);	/* select dev->iobase + 2 to be control register 1 */
-	outb(devpriv->do_bits | CONTROL1_INTE, dev->iobase + DAS800_CONTROL1);
-	spin_unlock_irqrestore(&dev->spinlock, irq_flags);
+		spin_lock_irqsave(&dev->spinlock, irq_flags);
+		das800_ind_write(dev, CONTROL1_INTE | devpriv->do_bits,
+				 CONTROL1);
+		spin_unlock_irqrestore(&dev->spinlock, irq_flags);
+	}
 
-	data[1] = wbits;
+	data[1] = s->state;
 
 	return insn->n;
 }
 
-/* loads counters with divisor1, divisor2 from private structure */
-static int das800_set_frequency(struct comedi_device *dev)
+static int das800_probe(struct comedi_device *dev)
 {
-	struct das800_private *devpriv = dev->private;
-	int err = 0;
+	const struct das800_board *thisboard = comedi_board(dev);
+	int board = thisboard ? thisboard - das800_boards : -EINVAL;
+	int id_bits;
+	unsigned long irq_flags;
 
-	if (i8254_load(dev->iobase + DAS800_8254, 0, 1, devpriv->divisor1, 2))
-		err++;
-	if (i8254_load(dev->iobase + DAS800_8254, 0, 2, devpriv->divisor2, 2))
-		err++;
-	if (err)
-		return -1;
+	spin_lock_irqsave(&dev->spinlock, irq_flags);
+	id_bits = das800_ind_read(dev, ID) & 0x3;
+	spin_unlock_irqrestore(&dev->spinlock, irq_flags);
 
-	return 0;
+	switch (id_bits) {
+	case 0x0:
+		if (board == BOARD_DAS800 || board == BOARD_CIODAS800)
+			break;
+		dev_dbg(dev->class_dev, "Board model (probed): DAS-800\n");
+		board = BOARD_DAS800;
+		break;
+	case 0x2:
+		if (board == BOARD_DAS801 || board == BOARD_CIODAS801)
+			break;
+		dev_dbg(dev->class_dev, "Board model (probed): DAS-801\n");
+		board = BOARD_DAS801;
+		break;
+	case 0x3:
+		if (board == BOARD_DAS802 || board == BOARD_CIODAS802 ||
+		    board == BOARD_CIODAS80216)
+			break;
+		dev_dbg(dev->class_dev, "Board model (probed): DAS-802\n");
+		board = BOARD_DAS802;
+		break;
+	default:
+		dev_dbg(dev->class_dev, "Board model: 0x%x (unknown)\n",
+			id_bits);
+		board = -EINVAL;
+		break;
+	}
+	return board;
 }
 
+static int das800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+{
+	const struct das800_board *thisboard = comedi_board(dev);
+	struct das800_private *devpriv;
+	struct comedi_subdevice *s;
+	unsigned int irq = it->options[1];
+	unsigned long irq_flags;
+	int board;
+	int ret;
+
+	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
+	if (!devpriv)
+		return -ENOMEM;
+	dev->private = devpriv;
+
+	ret = comedi_request_region(dev, it->options[0], DAS800_SIZE);
+	if (ret)
+		return ret;
+
+	board = das800_probe(dev);
+	if (board < 0) {
+		dev_dbg(dev->class_dev, "unable to determine board type\n");
+		return -ENODEV;
+	}
+	dev->board_ptr = das800_boards + board;
+	thisboard = comedi_board(dev);
+	dev->board_name = thisboard->name;
+
+	if (irq > 1 && irq <= 7) {
+		ret = request_irq(irq, das800_interrupt, 0, dev->board_name,
+				  dev);
+		if (ret == 0)
+			dev->irq = irq;
+	}
+
+	ret = comedi_alloc_subdevices(dev, 3);
+	if (ret)
+		return ret;
+
+	/* Analog Input subdevice */
+	s = &dev->subdevices[0];
+	dev->read_subdev = s;
+	s->type		= COMEDI_SUBD_AI;
+	s->subdev_flags	= SDF_READABLE | SDF_GROUND;
+	s->n_chan	= 8;
+	s->maxdata	= (1 << thisboard->resolution) - 1;
+	s->range_table	= thisboard->ai_range;
+	s->insn_read	= das800_ai_insn_read;
+	if (dev->irq) {
+		s->subdev_flags	|= SDF_CMD_READ;
+		s->len_chanlist	= 8;
+		s->do_cmdtest	= das800_ai_do_cmdtest;
+		s->do_cmd	= das800_ai_do_cmd;
+		s->cancel	= das800_cancel;
+	}
+
+	/* Digital Input subdevice */
+	s = &dev->subdevices[1];
+	s->type		= COMEDI_SUBD_DI;
+	s->subdev_flags	= SDF_READABLE;
+	s->n_chan	= 3;
+	s->maxdata	= 1;
+	s->range_table	= &range_digital;
+	s->insn_bits	= das800_di_insn_bits;
+
+	/* Digital Output subdevice */
+	s = &dev->subdevices[2];
+	s->type		= COMEDI_SUBD_DO;
+	s->subdev_flags	= SDF_WRITABLE | SDF_READABLE;
+	s->n_chan	= 4;
+	s->maxdata	= 1;
+	s->range_table	= &range_digital;
+	s->insn_bits	= das800_do_insn_bits;
+
+	das800_disable(dev);
+
+	/* initialize digital out channels */
+	spin_lock_irqsave(&dev->spinlock, irq_flags);
+	das800_ind_write(dev, CONTROL1_INTE | devpriv->do_bits, CONTROL1);
+	spin_unlock_irqrestore(&dev->spinlock, irq_flags);
+
+	return 0;
+};
+
+static struct comedi_driver driver_das800 = {
+	.driver_name	= "das800",
+	.module		= THIS_MODULE,
+	.attach		= das800_attach,
+	.detach		= comedi_legacy_detach,
+	.num_names	= ARRAY_SIZE(das800_boards),
+	.board_name	= &das800_boards[0].name,
+	.offset		= sizeof(struct das800_board),
+};
+module_comedi_driver(driver_das800);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/dmm32at.c b/drivers/staging/comedi/drivers/dmm32at.c
index 9e2124179a0..6c85dd2d549 100644
--- a/drivers/staging/comedi/drivers/dmm32at.c
+++ b/drivers/staging/comedi/drivers/dmm32at.c
@@ -695,25 +695,13 @@ static int dmm32at_attach(struct comedi_device *dev,
 	int ret;
 	struct comedi_subdevice *s;
 	unsigned char aihi, ailo, fifostat, aistat, intstat, airback;
-	unsigned long iobase;
 	unsigned int irq;
 
-	dev->board_name = dev->driver->driver_name;
-
-	iobase = it->options[0];
 	irq = it->options[1];
 
-	printk(KERN_INFO "comedi%d: dmm32at: attaching\n", dev->minor);
-	printk(KERN_DEBUG "dmm32at: probing at address 0x%04lx, irq %u\n",
-	       iobase, irq);
-
-	/* register address space */
-	if (!request_region(iobase, DMM32AT_MEMSIZE, dev->board_name)) {
-		printk(KERN_ERR "comedi%d: dmm32at: I/O port conflict\n",
-		       dev->minor);
-		return -EIO;
-	}
-	dev->iobase = iobase;
+	ret = comedi_request_region(dev, it->options[0], DMM32AT_MEMSIZE);
+	if (ret)
+		return ret;
 
 	/* the following just makes sure the board is there and gets
 	   it to a known state */
@@ -832,19 +820,11 @@ static int dmm32at_attach(struct comedi_device *dev,
 
 }
 
-static void dmm32at_detach(struct comedi_device *dev)
-{
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (dev->iobase)
-		release_region(dev->iobase, DMM32AT_MEMSIZE);
-}
-
 static struct comedi_driver dmm32at_driver = {
 	.driver_name	= "dmm32at",
 	.module		= THIS_MODULE,
 	.attach		= dmm32at_attach,
-	.detach		= dmm32at_detach,
+	.detach		= comedi_legacy_detach,
 };
 module_comedi_driver(dmm32at_driver);
 
diff --git a/drivers/staging/comedi/drivers/dt2801.c b/drivers/staging/comedi/drivers/dt2801.c
index f6942aaf0ec..8f5006d70da 100644
--- a/drivers/staging/comedi/drivers/dt2801.c
+++ b/drivers/staging/comedi/drivers/dt2801.c
@@ -225,8 +225,6 @@ static const struct dt2801_board boardtypes[] = {
 	 .dabits = 12},
 };
 
-#define boardtype (*(const struct dt2801_board *)dev->board_ptr)
-
 struct dt2801_private {
 
 	const struct comedi_lrange *dac_range_types[2];
@@ -592,19 +590,16 @@ static int dt2801_dio_insn_config(struct comedi_device *dev,
 */
 static int dt2801_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
+	const struct dt2801_board *board = comedi_board(dev);
 	struct dt2801_private *devpriv;
 	struct comedi_subdevice *s;
-	unsigned long iobase;
 	int board_code, type;
 	int ret = 0;
 	int n_ai_chans;
 
-	iobase = it->options[0];
-	if (!request_region(iobase, DT2801_IOSIZE, "dt2801")) {
-		comedi_error(dev, "I/O port conflict");
-		return -EIO;
-	}
-	dev->iobase = iobase;
+	ret = comedi_request_region(dev, it->options[0], DT2801_IOSIZE);
+	if (ret)
+		return ret;
 
 	/* do some checking */
 
@@ -624,10 +619,9 @@ static int dt2801_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 havetype:
 	dev->board_ptr = boardtypes + type;
-	printk("dt2801: %s at port 0x%lx", boardtype.name, iobase);
+	board = comedi_board(dev);
 
 	n_ai_chans = probe_number_of_ai_chans(dev);
-	printk(" (ai channels = %d)\n", n_ai_chans);
 
 	ret = comedi_alloc_subdevices(dev, 4);
 	if (ret)
@@ -638,7 +632,7 @@ havetype:
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	dev->board_name = boardtype.name;
+	dev->board_name = board->name;
 
 	s = &dev->subdevices[0];
 	/* ai subdevice */
@@ -648,12 +642,12 @@ havetype:
 	s->n_chan = n_ai_chans;
 #else
 	if (it->options[2])
-		s->n_chan = boardtype.ad_chan;
+		s->n_chan = board->ad_chan;
 	else
-		s->n_chan = boardtype.ad_chan / 2;
+		s->n_chan = board->ad_chan / 2;
 #endif
-	s->maxdata = (1 << boardtype.adbits) - 1;
-	s->range_table = ai_range_lkup(boardtype.adrangetype, it->options[3]);
+	s->maxdata = (1 << board->adbits) - 1;
+	s->range_table = ai_range_lkup(board->adrangetype, it->options[3]);
 	s->insn_read = dt2801_ai_insn_read;
 
 	s = &dev->subdevices[1];
@@ -661,7 +655,7 @@ havetype:
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE;
 	s->n_chan = 2;
-	s->maxdata = (1 << boardtype.dabits) - 1;
+	s->maxdata = (1 << board->dabits) - 1;
 	s->range_table_list = devpriv->dac_range_types;
 	devpriv->dac_range_types[0] = dac_range_lkup(it->options[4]);
 	devpriv->dac_range_types[1] = dac_range_lkup(it->options[5]);
@@ -693,17 +687,11 @@ out:
 	return ret;
 }
 
-static void dt2801_detach(struct comedi_device *dev)
-{
-	if (dev->iobase)
-		release_region(dev->iobase, DT2801_IOSIZE);
-}
-
 static struct comedi_driver dt2801_driver = {
 	.driver_name	= "dt2801",
 	.module		= THIS_MODULE,
 	.attach		= dt2801_attach,
-	.detach		= dt2801_detach,
+	.detach		= comedi_legacy_detach,
 };
 module_comedi_driver(dt2801_driver);
 
diff --git a/drivers/staging/comedi/drivers/dt2811.c b/drivers/staging/comedi/drivers/dt2811.c
index f90ecf494aa..8757b54ad4a 100644
--- a/drivers/staging/comedi/drivers/dt2811.c
+++ b/drivers/staging/comedi/drivers/dt2811.c
@@ -50,8 +50,6 @@ Configuration options:
 
 #include <linux/ioport.h>
 
-static const char *driver_name = "dt2811";
-
 static const struct comedi_lrange range_dt2811_pgh_ai_5_unipolar = {
 	4, {
 		RANGE(0, 5),
@@ -401,19 +399,10 @@ static int dt2811_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	struct dt2811_private *devpriv;
 	int ret;
 	struct comedi_subdevice *s;
-	unsigned long iobase;
-
-	iobase = it->options[0];
-
-	printk(KERN_INFO "comedi%d: dt2811:base=0x%04lx\n", dev->minor, iobase);
-
-	if (!request_region(iobase, DT2811_SIZE, driver_name)) {
-		printk(KERN_ERR "I/O port conflict\n");
-		return -EIO;
-	}
 
-	dev->iobase = iobase;
-	dev->board_name = board->name;
+	ret = comedi_request_region(dev, it->options[0], DT2811_SIZE);
+	if (ret)
+		return ret;
 
 #if 0
 	outb(0, dev->iobase + DT2811_ADCSR);
@@ -449,7 +438,7 @@ static int dt2811_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 			i = inb(dev->iobase + DT2811_ADDATHI);
 			printk(KERN_INFO "(irq = %d)\n", irq);
 			ret = request_irq(irq, dt2811_interrupt, 0,
-					  driver_name, dev);
+					  dev->board_name, dev);
 			if (ret < 0)
 				return -EIO;
 			dev->irq = irq;
@@ -567,14 +556,6 @@ static int dt2811_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static void dt2811_detach(struct comedi_device *dev)
-{
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (dev->iobase)
-		release_region(dev->iobase, DT2811_SIZE);
-}
-
 static const struct dt2811_board boardtypes[] = {
 	{
 		.name		= "dt2811-pgh",
@@ -593,7 +574,7 @@ static struct comedi_driver dt2811_driver = {
 	.driver_name	= "dt2811",
 	.module		= THIS_MODULE,
 	.attach		= dt2811_attach,
-	.detach		= dt2811_detach,
+	.detach		= comedi_legacy_detach,
 	.board_name	= &boardtypes[0].name,
 	.num_names	= ARRAY_SIZE(boardtypes),
 	.offset		= sizeof(struct dt2811_board),
diff --git a/drivers/staging/comedi/drivers/dt2814.c b/drivers/staging/comedi/drivers/dt2814.c
index e520dbaaa19..7c95b3b6813 100644
--- a/drivers/staging/comedi/drivers/dt2814.c
+++ b/drivers/staging/comedi/drivers/dt2814.c
@@ -246,16 +246,10 @@ static int dt2814_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	int i, irq;
 	int ret;
 	struct comedi_subdevice *s;
-	unsigned long iobase;
 
-	iobase = it->options[0];
-	printk(KERN_INFO "comedi%d: dt2814: 0x%04lx ", dev->minor, iobase);
-	if (!request_region(iobase, DT2814_SIZE, "dt2814")) {
-		printk(KERN_ERR "I/O port conflict\n");
-		return -EIO;
-	}
-	dev->iobase = iobase;
-	dev->board_name = "dt2814";
+	ret = comedi_request_region(dev, it->options[0], DT2814_SIZE);
+	if (ret)
+		return ret;
 
 	outb(0, dev->iobase + DT2814_CSR);
 	udelay(100);
@@ -329,19 +323,11 @@ static int dt2814_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static void dt2814_detach(struct comedi_device *dev)
-{
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (dev->iobase)
-		release_region(dev->iobase, DT2814_SIZE);
-}
-
 static struct comedi_driver dt2814_driver = {
 	.driver_name	= "dt2814",
 	.module		= THIS_MODULE,
 	.attach		= dt2814_attach,
-	.detach		= dt2814_detach,
+	.detach		= comedi_legacy_detach,
 };
 module_comedi_driver(dt2814_driver);
 
diff --git a/drivers/staging/comedi/drivers/dt2815.c b/drivers/staging/comedi/drivers/dt2815.c
index 1e0cfe4972a..b24e87681fe 100644
--- a/drivers/staging/comedi/drivers/dt2815.c
+++ b/drivers/staging/comedi/drivers/dt2815.c
@@ -61,12 +61,6 @@ Configuration options:
 #include <linux/ioport.h>
 #include <linux/delay.h>
 
-static const struct comedi_lrange
-	range_dt2815_ao_32_current = {1, {RANGE_mA(0, 32)} };
-
-static const struct comedi_lrange
-	range_dt2815_ao_20_current = {1, {RANGE_mA(4, 20)} };
-
 #define DT2815_SIZE 2
 
 #define DT2815_DATA 0
@@ -166,18 +160,11 @@ static int dt2815_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	struct comedi_subdevice *s;
 	int i;
 	const struct comedi_lrange *current_range_type, *voltage_range_type;
-	unsigned long iobase;
 	int ret;
 
-	iobase = it->options[0];
-	printk(KERN_INFO "comedi%d: dt2815: 0x%04lx ", dev->minor, iobase);
-	if (!request_region(iobase, DT2815_SIZE, "dt2815")) {
-		printk(KERN_WARNING "I/O port conflict\n");
-		return -EIO;
-	}
-
-	dev->iobase = iobase;
-	dev->board_name = "dt2815";
+	ret = comedi_request_region(dev, it->options[0], DT2815_SIZE);
+	if (ret)
+		return ret;
 
 	ret = comedi_alloc_subdevices(dev, 1);
 	if (ret)
@@ -199,7 +186,7 @@ static int dt2815_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->range_table_list = devpriv->range_type_list;
 
 	current_range_type = (it->options[3])
-	    ? &range_dt2815_ao_20_current : &range_dt2815_ao_32_current;
+	    ? &range_4_20mA : &range_0_32mA;
 	voltage_range_type = (it->options[2])
 	    ? &range_bipolar5 : &range_unipolar5;
 	for (i = 0; i < 8; i++) {
@@ -233,17 +220,11 @@ static int dt2815_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static void dt2815_detach(struct comedi_device *dev)
-{
-	if (dev->iobase)
-		release_region(dev->iobase, DT2815_SIZE);
-}
-
 static struct comedi_driver dt2815_driver = {
 	.driver_name	= "dt2815",
 	.module		= THIS_MODULE,
 	.attach		= dt2815_attach,
-	.detach		= dt2815_detach,
+	.detach		= comedi_legacy_detach,
 };
 module_comedi_driver(dt2815_driver);
 
diff --git a/drivers/staging/comedi/drivers/dt2817.c b/drivers/staging/comedi/drivers/dt2817.c
index 502e42e0753..b5c8e8213fa 100644
--- a/drivers/staging/comedi/drivers/dt2817.c
+++ b/drivers/staging/comedi/drivers/dt2817.c
@@ -126,16 +126,10 @@ static int dt2817_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	int ret;
 	struct comedi_subdevice *s;
-	unsigned long iobase;
 
-	iobase = it->options[0];
-	printk(KERN_INFO "comedi%d: dt2817: 0x%04lx ", dev->minor, iobase);
-	if (!request_region(iobase, DT2817_SIZE, "dt2817")) {
-		printk("I/O port conflict\n");
-		return -EIO;
-	}
-	dev->iobase = iobase;
-	dev->board_name = "dt2817";
+	ret = comedi_request_region(dev, it->options[0], DT2817_SIZE);
+	if (ret)
+		return ret;
 
 	ret = comedi_alloc_subdevices(dev, 1);
 	if (ret)
@@ -154,22 +148,14 @@ static int dt2817_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->state = 0;
 	outb(0, dev->iobase + DT2817_CR);
 
-	printk(KERN_INFO "\n");
-
 	return 0;
 }
 
-static void dt2817_detach(struct comedi_device *dev)
-{
-	if (dev->iobase)
-		release_region(dev->iobase, DT2817_SIZE);
-}
-
 static struct comedi_driver dt2817_driver = {
 	.driver_name	= "dt2817",
 	.module		= THIS_MODULE,
 	.attach		= dt2817_attach,
-	.detach		= dt2817_detach,
+	.detach		= comedi_legacy_detach,
 };
 module_comedi_driver(dt2817_driver);
 
diff --git a/drivers/staging/comedi/drivers/dt282x.c b/drivers/staging/comedi/drivers/dt282x.c
index 122d980a95a..90f2de9bc40 100644
--- a/drivers/staging/comedi/drivers/dt282x.c
+++ b/drivers/staging/comedi/drivers/dt282x.c
@@ -248,8 +248,6 @@ struct dt282x_private {
 	int dma_dir;
 };
 
-#define boardtype (*(const struct dt282x_board *)dev->board_ptr)
-
 /*
  *    Some useless abstractions
  */
@@ -289,14 +287,15 @@ static int dt282x_grab_dma(struct comedi_device *dev, int dma1, int dma2);
 static void dt282x_munge(struct comedi_device *dev, short *buf,
 			 unsigned int nbytes)
 {
+	const struct dt282x_board *board = comedi_board(dev);
 	struct dt282x_private *devpriv = dev->private;
 	unsigned int i;
-	unsigned short mask = (1 << boardtype.adbits) - 1;
-	unsigned short sign = 1 << (boardtype.adbits - 1);
+	unsigned short mask = (1 << board->adbits) - 1;
+	unsigned short sign = 1 << (board->adbits - 1);
 	int n;
 
 	if (devpriv->ad_2scomp)
-		sign = 1 << (boardtype.adbits - 1);
+		sign = 1 << (board->adbits - 1);
 	else
 		sign = 0;
 
@@ -501,10 +500,10 @@ static irqreturn_t dt282x_interrupt(int irq, void *d)
 		short data;
 
 		data = (short)inw(dev->iobase + DT2821_ADDAT);
-		data &= (1 << boardtype.adbits) - 1;
+		data &= (1 << board->adbits) - 1;
 
 		if (devpriv->ad_2scomp)
-			data ^= 1 << (boardtype.adbits - 1);
+			data ^= 1 << (board->adbits - 1);
 		ret = comedi_buf_put(s->async, data);
 
 		if (ret == 0)
@@ -554,6 +553,7 @@ static int dt282x_ai_insn_read(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
+	const struct dt282x_board *board = comedi_board(dev);
 	struct dt282x_private *devpriv = dev->private;
 	int i;
 
@@ -574,9 +574,9 @@ static int dt282x_ai_insn_read(struct comedi_device *dev,
 
 		data[i] =
 		    inw(dev->iobase +
-			DT2821_ADDAT) & ((1 << boardtype.adbits) - 1);
+			DT2821_ADDAT) & ((1 << board->adbits) - 1);
 		if (devpriv->ad_2scomp)
-			data[i] ^= (1 << (boardtype.adbits - 1));
+			data[i] ^= (1 << (board->adbits - 1));
 	}
 
 	return i;
@@ -795,13 +795,14 @@ static int dt282x_ao_insn_write(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn, unsigned int *data)
 {
+	const struct dt282x_board *board = comedi_board(dev);
 	struct dt282x_private *devpriv = dev->private;
 	short d;
 	unsigned int chan;
 
 	chan = CR_CHAN(insn->chanspec);
 	d = data[0];
-	d &= (1 << boardtype.dabits) - 1;
+	d &= (1 << board->dabits) - 1;
 	devpriv->ao[chan] = d;
 
 	devpriv->dacsr |= DT2821_SSEL;
@@ -810,11 +811,11 @@ static int dt282x_ao_insn_write(struct comedi_device *dev,
 		/* select channel */
 		devpriv->dacsr |= DT2821_YSEL;
 		if (devpriv->da0_2scomp)
-			d ^= (1 << (boardtype.dabits - 1));
+			d ^= (1 << (board->dabits - 1));
 	} else {
 		devpriv->dacsr &= ~DT2821_YSEL;
 		if (devpriv->da1_2scomp)
-			d ^= (1 << (boardtype.dabits - 1));
+			d ^= (1 << (board->dabits - 1));
 	}
 
 	outw(devpriv->dacsr, dev->iobase + DT2821_DACSR);
@@ -1122,20 +1123,10 @@ static int dt282x_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	int i, irq;
 	int ret;
 	struct comedi_subdevice *s;
-	unsigned long iobase;
-
-	dev->board_name = board->name;
 
-	iobase = it->options[opt_iobase];
-	if (!iobase)
-		iobase = 0x240;
-
-	printk(KERN_INFO "comedi%d: dt282x: 0x%04lx", dev->minor, iobase);
-	if (!request_region(iobase, DT2821_SIZE, "dt282x")) {
-		printk(KERN_INFO " I/O port conflict\n");
-		return -EBUSY;
-	}
-	dev->iobase = iobase;
+	ret = comedi_request_region(dev, it->options[0], DT2821_SIZE);
+	if (ret)
+		return ret;
 
 	outw(DT2821_BDINIT, dev->iobase + DT2821_SUPCSR);
 	i = inw(dev->iobase + DT2821_ADCSR);
@@ -1185,7 +1176,8 @@ static int dt282x_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 #endif
 	if (irq > 0) {
 		printk(KERN_INFO " ( irq = %d )", irq);
-		ret = request_irq(irq, dt282x_interrupt, 0, "dt282x", dev);
+		ret = request_irq(irq, dt282x_interrupt, 0,
+				  dev->board_name, dev);
 		if (ret < 0) {
 			printk(KERN_ERR " failed to get irq\n");
 			return -EIO;
@@ -1223,20 +1215,20 @@ static int dt282x_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->subdev_flags = SDF_READABLE | SDF_CMD_READ |
 	    ((it->options[opt_diff]) ? SDF_DIFF : SDF_COMMON);
 	s->n_chan =
-	    (it->options[opt_diff]) ? boardtype.adchan_di : boardtype.adchan_se;
+	    (it->options[opt_diff]) ? board->adchan_di : board->adchan_se;
 	s->insn_read = dt282x_ai_insn_read;
 	s->do_cmdtest = dt282x_ai_cmdtest;
 	s->do_cmd = dt282x_ai_cmd;
 	s->cancel = dt282x_ai_cancel;
-	s->maxdata = (1 << boardtype.adbits) - 1;
+	s->maxdata = (1 << board->adbits) - 1;
 	s->len_chanlist = 16;
 	s->range_table =
-	    opt_ai_range_lkup(boardtype.ispgl, it->options[opt_ai_range]);
+	    opt_ai_range_lkup(board->ispgl, it->options[opt_ai_range]);
 	devpriv->ad_2scomp = it->options[opt_ai_twos];
 
 	s = &dev->subdevices[1];
 
-	s->n_chan = boardtype.dachan;
+	s->n_chan = board->dachan;
 	if (s->n_chan) {
 		/* ao subsystem */
 		s->type = COMEDI_SUBD_AO;
@@ -1247,7 +1239,7 @@ static int dt282x_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		s->do_cmdtest = dt282x_ao_cmdtest;
 		s->do_cmd = dt282x_ao_cmd;
 		s->cancel = dt282x_ao_cancel;
-		s->maxdata = (1 << boardtype.dabits) - 1;
+		s->maxdata = (1 << board->dabits) - 1;
 		s->len_chanlist = 2;
 		s->range_table_list = devpriv->darangelist;
 		devpriv->darangelist[0] =
@@ -1279,10 +1271,6 @@ static void dt282x_detach(struct comedi_device *dev)
 {
 	struct dt282x_private *devpriv = dev->private;
 
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (dev->iobase)
-		release_region(dev->iobase, DT2821_SIZE);
 	if (dev->private) {
 		if (devpriv->dma[0].chan)
 			free_dma(devpriv->dma[0].chan);
@@ -1293,6 +1281,7 @@ static void dt282x_detach(struct comedi_device *dev)
 		if (devpriv->dma[1].buf)
 			free_page((unsigned long)devpriv->dma[1].buf);
 	}
+	comedi_legacy_detach(dev);
 }
 
 static const struct dt282x_board boardtypes[] = {
diff --git a/drivers/staging/comedi/drivers/dt3000.c b/drivers/staging/comedi/drivers/dt3000.c
index 3ce499fa5db..7e03929c9a1 100644
--- a/drivers/staging/comedi/drivers/dt3000.c
+++ b/drivers/staging/comedi/drivers/dt3000.c
@@ -63,17 +63,6 @@ AO commands are not supported.
 
 #include "comedi_fc.h"
 
-/*
- * PCI device id's supported by this driver
- */
-#define PCI_DEVICE_ID_DT3001		0x0022
-#define PCI_DEVICE_ID_DT3002		0x0023
-#define PCI_DEVICE_ID_DT3003		0x0024
-#define PCI_DEVICE_ID_DT3004		0x0025
-#define PCI_DEVICE_ID_DT3005		0x0026
-#define PCI_DEVICE_ID_DT3001_PGL	0x0027
-#define PCI_DEVICE_ID_DT3003_PGL	0x0028
-
 static const struct comedi_lrange range_dt3000_ai = {
 	4, {
 		BIP_RANGE(10),
@@ -92,9 +81,18 @@ static const struct comedi_lrange range_dt3000_ai_pgl = {
 	}
 };
 
+enum dt3k_boardid {
+	BOARD_DT3001,
+	BOARD_DT3001_PGL,
+	BOARD_DT3002,
+	BOARD_DT3003,
+	BOARD_DT3003_PGL,
+	BOARD_DT3004,
+	BOARD_DT3005,
+};
+
 struct dt3k_boardtype {
 	const char *name;
-	unsigned int device_id;
 	int adchan;
 	int adbits;
 	int ai_speed;
@@ -104,61 +102,60 @@ struct dt3k_boardtype {
 };
 
 static const struct dt3k_boardtype dt3k_boardtypes[] = {
-	{
+	[BOARD_DT3001] = {
 		.name		= "dt3001",
-		.device_id	= PCI_DEVICE_ID_DT3001,
 		.adchan		= 16,
 		.adbits		= 12,
 		.adrange	= &range_dt3000_ai,
 		.ai_speed	= 3000,
 		.dachan		= 2,
 		.dabits		= 12,
-	}, {
+	},
+	[BOARD_DT3001_PGL] = {
 		.name		= "dt3001-pgl",
-		.device_id	= PCI_DEVICE_ID_DT3001_PGL,
 		.adchan		= 16,
 		.adbits		= 12,
 		.adrange	= &range_dt3000_ai_pgl,
 		.ai_speed	= 3000,
 		.dachan		= 2,
 		.dabits		= 12,
-	}, {
+	},
+	[BOARD_DT3002] = {
 		.name		= "dt3002",
-		.device_id	= PCI_DEVICE_ID_DT3002,
 		.adchan		= 32,
 		.adbits		= 12,
 		.adrange	= &range_dt3000_ai,
 		.ai_speed	= 3000,
-	}, {
+	},
+	[BOARD_DT3003] = {
 		.name		= "dt3003",
-		.device_id	= PCI_DEVICE_ID_DT3003,
 		.adchan		= 64,
 		.adbits		= 12,
 		.adrange	= &range_dt3000_ai,
 		.ai_speed	= 3000,
 		.dachan		= 2,
 		.dabits		= 12,
-	}, {
+	},
+	[BOARD_DT3003_PGL] = {
 		.name		= "dt3003-pgl",
-		.device_id	= PCI_DEVICE_ID_DT3003_PGL,
 		.adchan		= 64,
 		.adbits		= 12,
 		.adrange	= &range_dt3000_ai_pgl,
 		.ai_speed	= 3000,
 		.dachan		= 2,
 		.dabits		= 12,
-	}, {
+	},
+	[BOARD_DT3004] = {
 		.name		= "dt3004",
-		.device_id	= PCI_DEVICE_ID_DT3004,
 		.adchan		= 16,
 		.adbits		= 16,
 		.adrange	= &range_dt3000_ai,
 		.ai_speed	= 10000,
 		.dachan		= 2,
 		.dabits		= 12,
-	}, {
+	},
+	[BOARD_DT3005] = {
 		.name		= "dt3005",	/* a.k.a. 3004-200 */
-		.device_id 	= PCI_DEVICE_ID_DT3005,
 		.adchan		= 16,
 		.adbits		= 16,
 		.adrange	= &range_dt3000_ai,
@@ -168,8 +165,6 @@ static const struct dt3k_boardtype dt3k_boardtypes[] = {
 	},
 };
 
-#define DT3000_SIZE		(4*0x1000)
-
 /* dual-ported RAM location definitions */
 
 #define DPR_DAC_buffer		(4*0x000)
@@ -716,31 +711,17 @@ static int dt3k_mem_insn_read(struct comedi_device *dev,
 	return i;
 }
 
-static const void *dt3000_find_boardinfo(struct comedi_device *dev,
-					 struct pci_dev *pcidev)
-{
-	const struct dt3k_boardtype *this_board;
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(dt3k_boardtypes); i++) {
-		this_board = &dt3k_boardtypes[i];
-		if (this_board->device_id == pcidev->device)
-			return this_board;
-	}
-	return NULL;
-}
-
 static int dt3000_auto_attach(struct comedi_device *dev,
-					unsigned long context_unused)
+			      unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct dt3k_boardtype *this_board;
+	const struct dt3k_boardtype *this_board = NULL;
 	struct dt3k_private *devpriv;
 	struct comedi_subdevice *s;
-	resource_size_t pci_base;
 	int ret = 0;
 
-	this_board = dt3000_find_boardinfo(dev, pcidev);
+	if (context < ARRAY_SIZE(dt3k_boardtypes))
+		this_board = &dt3k_boardtypes[context];
 	if (!this_board)
 		return -ENODEV;
 	dev->board_ptr = this_board;
@@ -751,13 +732,11 @@ static int dt3000_auto_attach(struct comedi_device *dev,
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret < 0)
 		return ret;
-	dev->iobase = 1;	/* the "detach" needs this */
 
-	pci_base  = pci_resource_start(pcidev, 0);
-	devpriv->io_addr = ioremap(pci_base, DT3000_SIZE);
+	devpriv->io_addr = pci_ioremap_bar(pcidev, 0);
 	if (!devpriv->io_addr)
 		return -ENOMEM;
 
@@ -830,7 +809,6 @@ static int dt3000_auto_attach(struct comedi_device *dev,
 
 static void dt3000_detach(struct comedi_device *dev)
 {
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct dt3k_private *devpriv = dev->private;
 
 	if (dev->irq)
@@ -839,10 +817,7 @@ static void dt3000_detach(struct comedi_device *dev)
 		if (devpriv->io_addr)
 			iounmap(devpriv->io_addr);
 	}
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-	}
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver dt3000_driver = {
@@ -853,19 +828,19 @@ static struct comedi_driver dt3000_driver = {
 };
 
 static int dt3000_pci_probe(struct pci_dev *dev,
-				      const struct pci_device_id *ent)
+			    const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &dt3000_driver);
+	return comedi_pci_auto_config(dev, &dt3000_driver, id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(dt3000_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_DT, PCI_DEVICE_ID_DT3001) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_DT, PCI_DEVICE_ID_DT3001_PGL) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_DT, PCI_DEVICE_ID_DT3002) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_DT, PCI_DEVICE_ID_DT3003) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_DT, PCI_DEVICE_ID_DT3003_PGL) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_DT, PCI_DEVICE_ID_DT3004) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_DT, PCI_DEVICE_ID_DT3005) },
+	{ PCI_VDEVICE(DT, 0x0022), BOARD_DT3001 },
+	{ PCI_VDEVICE(DT, 0x0023), BOARD_DT3002 },
+	{ PCI_VDEVICE(DT, 0x0024), BOARD_DT3003 },
+	{ PCI_VDEVICE(DT, 0x0025), BOARD_DT3004 },
+	{ PCI_VDEVICE(DT, 0x0026), BOARD_DT3005 },
+	{ PCI_VDEVICE(DT, 0x0027), BOARD_DT3001_PGL },
+	{ PCI_VDEVICE(DT, 0x0028), BOARD_DT3003_PGL },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, dt3000_pci_table);
diff --git a/drivers/staging/comedi/drivers/dt9812.c b/drivers/staging/comedi/drivers/dt9812.c
index 57b45190479..81eb5ed6ec9 100644
--- a/drivers/staging/comedi/drivers/dt9812.c
+++ b/drivers/staging/comedi/drivers/dt9812.c
@@ -303,26 +303,6 @@ struct slot_dt9812 {
 	struct comedi_dt9812 *comedi;
 };
 
-static const struct comedi_lrange dt9812_10_ain_range = { 1, {
-							      BIP_RANGE(10),
-							      }
-};
-
-static const struct comedi_lrange dt9812_2pt5_ain_range = { 1, {
-								UNI_RANGE(2.5),
-								}
-};
-
-static const struct comedi_lrange dt9812_10_aout_range = { 1, {
-							       BIP_RANGE(10),
-							       }
-};
-
-static const struct comedi_lrange dt9812_2pt5_aout_range = { 1, {
-								 UNI_RANGE(2.5),
-								 }
-};
-
 static struct slot_dt9812 dt9812[DT9812_NUM_SLOTS];
 
 static inline struct usb_dt9812 *to_dt9812_dev(struct kref *d)
@@ -912,12 +892,12 @@ static int dt9812_comedi_open(struct comedi_device *dev)
 		switch (devpriv->slot->usb->device) {
 		case 0:{
 				s->maxdata = 4095;
-				s->range_table = &dt9812_10_ain_range;
+				s->range_table = &range_bipolar10;
 			}
 			break;
 		case 1:{
 				s->maxdata = 4095;
-				s->range_table = &dt9812_2pt5_ain_range;
+				s->range_table = &range_unipolar2_5;
 			}
 			break;
 		}
@@ -927,12 +907,12 @@ static int dt9812_comedi_open(struct comedi_device *dev)
 		switch (devpriv->slot->usb->device) {
 		case 0:{
 				s->maxdata = 4095;
-				s->range_table = &dt9812_10_aout_range;
+				s->range_table = &range_bipolar10;
 			}
 			break;
 		case 1:{
 				s->maxdata = 4095;
-				s->range_table = &dt9812_2pt5_aout_range;
+				s->range_table = &range_unipolar2_5;
 			}
 			break;
 		}
@@ -1032,8 +1012,6 @@ static int dt9812_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	struct comedi_subdevice *s;
 	int ret;
 
-	dev->board_name = "dt9812";
-
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
diff --git a/drivers/staging/comedi/drivers/dyna_pci10xx.c b/drivers/staging/comedi/drivers/dyna_pci10xx.c
index decc17f1867..93ec8e492cc 100644
--- a/drivers/staging/comedi/drivers/dyna_pci10xx.c
+++ b/drivers/staging/comedi/drivers/dyna_pci10xx.c
@@ -187,14 +187,12 @@ static int dyna_pci10xx_auto_attach(struct comedi_device *dev,
 	struct comedi_subdevice *s;
 	int ret;
 
-	dev->board_name = dev->driver->driver_name;
-
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
 	dev->iobase = pci_resource_start(pcidev, 2);
@@ -254,15 +252,11 @@ static int dyna_pci10xx_auto_attach(struct comedi_device *dev,
 
 static void dyna_pci10xx_detach(struct comedi_device *dev)
 {
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct dyna_pci10xx_private *devpriv = dev->private;
 
 	if (devpriv)
 		mutex_destroy(&devpriv->mutex);
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-	}
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver dyna_pci10xx_driver = {
@@ -273,9 +267,10 @@ static struct comedi_driver dyna_pci10xx_driver = {
 };
 
 static int dyna_pci10xx_pci_probe(struct pci_dev *dev,
-					    const struct pci_device_id *ent)
+				  const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &dyna_pci10xx_driver);
+	return comedi_pci_auto_config(dev, &dyna_pci10xx_driver,
+				      id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(dyna_pci10xx_pci_table) = {
diff --git a/drivers/staging/comedi/drivers/fl512.c b/drivers/staging/comedi/drivers/fl512.c
index 019c96eda6f..ff6f0bd7c86 100644
--- a/drivers/staging/comedi/drivers/fl512.c
+++ b/drivers/staging/comedi/drivers/fl512.c
@@ -111,31 +111,18 @@ static int fl512_ao_insn_readback(struct comedi_device *dev,
 static int fl512_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	struct fl512_private *devpriv;
-	unsigned long iobase;
-	int ret;
-
-	/* pointer to the subdevice: Analog in, Analog out,
-	   (not made ->and Digital IO) */
 	struct comedi_subdevice *s;
+	int ret;
 
-	iobase = it->options[0];
-	printk(KERN_INFO "comedi:%d fl512: 0x%04lx", dev->minor, iobase);
-	if (!request_region(iobase, FL512_SIZE, "fl512")) {
-		printk(KERN_WARNING " I/O port conflict\n");
-		return -EIO;
-	}
-	dev->iobase = iobase;
-	dev->board_name = "fl512";
+	ret = comedi_request_region(dev, it->options[0], FL512_SIZE);
+	if (ret)
+		return ret;
 
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
 	dev->private = devpriv;
 
-#if DEBUG
-	printk(KERN_DEBUG "malloc ok\n");
-#endif
-
 	ret = comedi_alloc_subdevices(dev, 2);
 	if (ret)
 		return ret;
@@ -157,7 +144,6 @@ static int fl512_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->range_table = &range_fl512;
 	/* function to call when read AD */
 	s->insn_read = fl512_ai_insn;
-	printk(KERN_INFO "comedi: fl512: subdevice 0 initialized\n");
 
 	/* Analog output */
 	s = &dev->subdevices[1];
@@ -175,22 +161,15 @@ static int fl512_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->insn_write = fl512_ao_insn;
 	/* function to call when reading DA */
 	s->insn_read = fl512_ao_insn_readback;
-	printk(KERN_INFO "comedi: fl512: subdevice 1 initialized\n");
 
 	return 1;
 }
 
-static void fl512_detach(struct comedi_device *dev)
-{
-	if (dev->iobase)
-		release_region(dev->iobase, FL512_SIZE);
-}
-
 static struct comedi_driver fl512_driver = {
 	.driver_name	= "fl512",
 	.module		= THIS_MODULE,
 	.attach		= fl512_attach,
-	.detach		= fl512_detach,
+	.detach		= comedi_legacy_detach,
 };
 module_comedi_driver(fl512_driver);
 
diff --git a/drivers/staging/comedi/drivers/gsc_hpdi.c b/drivers/staging/comedi/drivers/gsc_hpdi.c
index b60c9756267..0c061df0978 100644
--- a/drivers/staging/comedi/drivers/gsc_hpdi.c
+++ b/drivers/staging/comedi/drivers/gsc_hpdi.c
@@ -78,12 +78,6 @@ static int dio_config_block_size(struct comedi_device *dev, unsigned int *data);
 #define NUM_DMA_BUFFERS 4
 #define NUM_DMA_DESCRIPTORS 256
 
-/* indices of base address regions */
-enum base_address_regions {
-	PLX9080_BADDRINDEX = 0,
-	HPDI_BADDRINDEX = 2,
-};
-
 enum hpdi_registers {
 	FIRMWARE_REV_REG = 0x0,
 	BOARD_CONTROL_REG = 0x4,
@@ -499,20 +493,13 @@ static int hpdi_auto_attach(struct comedi_device *dev,
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	if (comedi_pci_enable(pcidev, dev->board_name)) {
-		dev_warn(dev->class_dev,
-			 "failed enable PCI device and request regions\n");
-		return -EIO;
-	}
-	dev->iobase = 1;	/* the "detach" needs this */
+	retval = comedi_pci_enable(dev);
+	if (retval)
+		return retval;
 	pci_set_master(pcidev);
 
-	devpriv->plx9080_iobase =
-		ioremap(pci_resource_start(pcidev, PLX9080_BADDRINDEX),
-			pci_resource_len(pcidev, PLX9080_BADDRINDEX));
-	devpriv->hpdi_iobase =
-		ioremap(pci_resource_start(pcidev, HPDI_BADDRINDEX),
-			pci_resource_len(pcidev, HPDI_BADDRINDEX));
+	devpriv->plx9080_iobase = pci_ioremap_bar(pcidev, 0);
+	devpriv->hpdi_iobase = pci_ioremap_bar(pcidev, 2);
 	if (!devpriv->plx9080_iobase || !devpriv->hpdi_iobase) {
 		dev_warn(dev->class_dev, "failed to remap io memory\n");
 		return -ENOMEM;
@@ -596,9 +583,8 @@ static void hpdi_detach(struct comedi_device *dev)
 					    NUM_DMA_DESCRIPTORS,
 					    devpriv->dma_desc,
 					    devpriv->dma_desc_phys_addr);
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
 	}
+	comedi_pci_disable(dev);
 }
 
 static int dio_config_block_size(struct comedi_device *dev, unsigned int *data)
@@ -943,9 +929,9 @@ static struct comedi_driver gsc_hpdi_driver = {
 };
 
 static int gsc_hpdi_pci_probe(struct pci_dev *dev,
-					const struct pci_device_id *ent)
+			      const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &gsc_hpdi_driver);
+	return comedi_pci_auto_config(dev, &gsc_hpdi_driver, id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(gsc_hpdi_pci_table) = {
diff --git a/drivers/staging/comedi/drivers/icp_multi.c b/drivers/staging/comedi/drivers/icp_multi.c
index 1e08f9141fa..08ab9d6e719 100644
--- a/drivers/staging/comedi/drivers/icp_multi.c
+++ b/drivers/staging/comedi/drivers/icp_multi.c
@@ -68,8 +68,6 @@ Configuration options: not applicable, uses PCI auto config
 #define ICP_MULTI_CNTR2		0x14	/* R/W: Counter 2 */
 #define ICP_MULTI_CNTR3		0x16	/* R/W: Counter 3 */
 
-#define ICP_MULTI_SIZE		0x20	/* 32 bytes */
-
 /*  Define bits from ADC command/status register */
 #define	ADC_ST		0x0001	/* Start ADC */
 #define	ADC_BSY		0x0001	/* ADC busy */
@@ -120,7 +118,7 @@ struct icp_multi_private {
 	unsigned int DacCmdStatus;	/*  DAC Command/Status register */
 	unsigned int IntEnable;	/*  Interrupt Enable register */
 	unsigned int IntStatus;	/*  Interrupt Status register */
-	unsigned int act_chanlist[32];	/*  list of scaned channel */
+	unsigned int act_chanlist[32];	/*  list of scanned channel */
 	unsigned char act_chanlist_len;	/*  len of scanlist */
 	unsigned char act_chanlist_pos;	/*  actual position in MUX list */
 	unsigned int *ai_chanlist;	/*  actaul chanlist */
@@ -500,23 +498,18 @@ static int icp_multi_auto_attach(struct comedi_device *dev,
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct icp_multi_private *devpriv;
 	struct comedi_subdevice *s;
-	resource_size_t iobase;
 	int ret;
 
-	dev->board_name = dev->driver->driver_name;
-
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
-	iobase = pci_resource_start(pcidev, 2);
-	dev->iobase = iobase;
 
-	devpriv->io_addr = ioremap(iobase, ICP_MULTI_SIZE);
+	devpriv->io_addr = pci_ioremap_bar(pcidev, 2);
 	if (!devpriv->io_addr)
 		return -ENOMEM;
 
@@ -594,7 +587,6 @@ static int icp_multi_auto_attach(struct comedi_device *dev,
 
 static void icp_multi_detach(struct comedi_device *dev)
 {
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct icp_multi_private *devpriv = dev->private;
 
 	if (devpriv)
@@ -604,10 +596,7 @@ static void icp_multi_detach(struct comedi_device *dev)
 		free_irq(dev->irq, dev);
 	if (devpriv && devpriv->io_addr)
 		iounmap(devpriv->io_addr);
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-	}
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver icp_multi_driver = {
@@ -618,9 +607,9 @@ static struct comedi_driver icp_multi_driver = {
 };
 
 static int icp_multi_pci_probe(struct pci_dev *dev,
-					   const struct pci_device_id *ent)
+			       const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &icp_multi_driver);
+	return comedi_pci_auto_config(dev, &icp_multi_driver, id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(icp_multi_pci_table) = {
diff --git a/drivers/staging/comedi/drivers/ii_pci20kc.c b/drivers/staging/comedi/drivers/ii_pci20kc.c
index 93584e2be35..ee7537daf47 100644
--- a/drivers/staging/comedi/drivers/ii_pci20kc.c
+++ b/drivers/staging/comedi/drivers/ii_pci20kc.c
@@ -77,9 +77,6 @@ options for PCI-20341M:
 	     3  200
 */
 
-/* XXX needs to use ioremap() for compatibility with 2.4 kernels.  Should also
- * check_mem_region() etc. - fmhess */
-
 #include "../comedidev.h"
 
 #define PCI20000_ID			0x1d
@@ -212,7 +209,6 @@ static int pci20xxx_attach(struct comedi_device *dev,
 	dev->private = devpriv;
 
 	devpriv->ioaddr = (void __iomem *)(unsigned long)it->options[0];
-	dev->board_name = "pci20kc";
 
 	/* Check PCI-20001 C-2A Carrier Board ID */
 	if ((readb(devpriv->ioaddr) & PCI20000_ID) != PCI20000_ID) {
diff --git a/drivers/staging/comedi/drivers/jr3_pci.c b/drivers/staging/comedi/drivers/jr3_pci.c
index 17ba75e0ab8..90b303ab230 100644
--- a/drivers/staging/comedi/drivers/jr3_pci.c
+++ b/drivers/staging/comedi/drivers/jr3_pci.c
@@ -700,16 +700,12 @@ static int jr3_pci_auto_attach(struct comedi_device *dev,
 		return -EINVAL;
 		break;
 	}
-	dev->board_name = "jr3_pci";
 
-	result = comedi_pci_enable(pcidev, "jr3_pci");
-	if (result < 0)
+	result = comedi_pci_enable(dev);
+	if (result)
 		return result;
 
-	dev->iobase = 1;	/* the "detach" needs this */
-	devpriv->iobase = ioremap(pci_resource_start(pcidev, 0),
-				  offsetof(struct jr3_t,
-					   channel[devpriv->n_channels]));
+	devpriv->iobase = pci_ioremap_bar(pcidev, 0);
 	if (!devpriv->iobase)
 		return -ENOMEM;
 
@@ -816,7 +812,6 @@ static int jr3_pci_auto_attach(struct comedi_device *dev,
 static void jr3_pci_detach(struct comedi_device *dev)
 {
 	int i;
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct jr3_pci_dev_private *devpriv = dev->private;
 
 	if (devpriv) {
@@ -828,9 +823,8 @@ static void jr3_pci_detach(struct comedi_device *dev)
 		}
 		if (devpriv->iobase)
 			iounmap(devpriv->iobase);
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
 	}
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver jr3_pci_driver = {
@@ -841,9 +835,9 @@ static struct comedi_driver jr3_pci_driver = {
 };
 
 static int jr3_pci_pci_probe(struct pci_dev *dev,
-				       const struct pci_device_id *ent)
+			     const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &jr3_pci_driver);
+	return comedi_pci_auto_config(dev, &jr3_pci_driver, id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(jr3_pci_pci_table) = {
diff --git a/drivers/staging/comedi/drivers/ke_counter.c b/drivers/staging/comedi/drivers/ke_counter.c
index 8c09c026508..e0e64752e31 100644
--- a/drivers/staging/comedi/drivers/ke_counter.c
+++ b/drivers/staging/comedi/drivers/ke_counter.c
@@ -96,9 +96,7 @@ static int cnt_auto_attach(struct comedi_device *dev,
 	struct comedi_subdevice *s;
 	int ret;
 
-	dev->board_name = dev->driver->driver_name;
-
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
 	dev->iobase = pci_resource_start(pcidev, 0);
@@ -131,27 +129,18 @@ static int cnt_auto_attach(struct comedi_device *dev,
 	return 0;
 }
 
-static void cnt_detach(struct comedi_device *dev)
-{
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-	}
-}
-
 static struct comedi_driver ke_counter_driver = {
 	.driver_name	= "ke_counter",
 	.module		= THIS_MODULE,
 	.auto_attach	= cnt_auto_attach,
-	.detach		= cnt_detach,
+	.detach		= comedi_pci_disable,
 };
 
 static int ke_counter_pci_probe(struct pci_dev *dev,
-					  const struct pci_device_id *ent)
+				const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &ke_counter_driver);
+	return comedi_pci_auto_config(dev, &ke_counter_driver,
+				      id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(ke_counter_pci_table) = {
diff --git a/drivers/staging/comedi/drivers/me4000.c b/drivers/staging/comedi/drivers/me4000.c
index b766bb93efd..641e693d5d0 100644
--- a/drivers/staging/comedi/drivers/me4000.c
+++ b/drivers/staging/comedi/drivers/me4000.c
@@ -55,26 +55,13 @@ broken.
 
 #include "comedi_fc.h"
 #include "8253.h"
+#include "plx9052.h"
 
 #if 0
 /* file removed due to GPL incompatibility */
 #include "me4000_fw.h"
 #endif
 
-#define PCI_DEVICE_ID_MEILHAUS_ME4650	0x4650
-#define PCI_DEVICE_ID_MEILHAUS_ME4660	0x4660
-#define PCI_DEVICE_ID_MEILHAUS_ME4660I	0x4661
-#define PCI_DEVICE_ID_MEILHAUS_ME4660S	0x4662
-#define PCI_DEVICE_ID_MEILHAUS_ME4660IS	0x4663
-#define PCI_DEVICE_ID_MEILHAUS_ME4670	0x4670
-#define PCI_DEVICE_ID_MEILHAUS_ME4670I	0x4671
-#define PCI_DEVICE_ID_MEILHAUS_ME4670S	0x4672
-#define PCI_DEVICE_ID_MEILHAUS_ME4670IS	0x4673
-#define PCI_DEVICE_ID_MEILHAUS_ME4680	0x4680
-#define PCI_DEVICE_ID_MEILHAUS_ME4680I	0x4681
-#define PCI_DEVICE_ID_MEILHAUS_ME4680S	0x4682
-#define PCI_DEVICE_ID_MEILHAUS_ME4680IS	0x4683
-
 /*
  * ME4000 Register map and bit defines
  */
@@ -183,28 +170,6 @@ broken.
 #define ME4000_AO_DEMUX_ADJUST_VALUE		0x4c
 #define ME4000_AI_SAMPLE_COUNTER_REG		0xc0
 
-/*
- * PLX Register map and bit defines
- */
-#define PLX_INTCSR				0x4c
-#define PLX_INTCSR_LOCAL_INT1_EN		(1 << 0)
-#define PLX_INTCSR_LOCAL_INT1_POL		(1 << 1)
-#define PLX_INTCSR_LOCAL_INT1_STATE		(1 << 2)
-#define PLX_INTCSR_LOCAL_INT2_EN		(1 << 3)
-#define PLX_INTCSR_LOCAL_INT2_POL		(1 << 4)
-#define PLX_INTCSR_LOCAL_INT2_STATE		(1 << 5)
-#define PLX_INTCSR_PCI_INT_EN			(1 << 6)
-#define PLX_INTCSR_SOFT_INT			(1 << 7)
-#define PLX_ICR					0x50
-#define PLX_ICR_BIT_EEPROM_CLOCK_SET		(1 << 24)
-#define PLX_ICR_BIT_EEPROM_CHIP_SELECT		(1 << 25)
-#define PLX_ICR_BIT_EEPROM_WRITE		(1 << 26)
-#define PLX_ICR_BIT_EEPROM_READ			(1 << 27)
-#define PLX_ICR_BIT_EEPROM_VALID		(1 << 28)
-#define PLX_ICR_MASK_EEPROM			(0x1f << 24)
-
-#define EEPROM_DELAY				1
-
 #define ME4000_AI_FIFO_COUNT			2048
 
 #define ME4000_AI_MIN_TICKS			66
@@ -220,9 +185,24 @@ struct me4000_info {
 	unsigned int ao_readback[4];
 };
 
+enum me4000_boardid {
+	BOARD_ME4650,
+	BOARD_ME4660,
+	BOARD_ME4660I,
+	BOARD_ME4660S,
+	BOARD_ME4660IS,
+	BOARD_ME4670,
+	BOARD_ME4670I,
+	BOARD_ME4670S,
+	BOARD_ME4670IS,
+	BOARD_ME4680,
+	BOARD_ME4680I,
+	BOARD_ME4680S,
+	BOARD_ME4680IS,
+};
+
 struct me4000_board {
 	const char *name;
-	unsigned short device_id;
 	int ao_nchan;
 	int ao_fifo;
 	int ai_nchan;
@@ -234,62 +214,61 @@ struct me4000_board {
 };
 
 static const struct me4000_board me4000_boards[] = {
-	{
+	[BOARD_ME4650] = {
 		.name		= "ME-4650",
-		.device_id	= PCI_DEVICE_ID_MEILHAUS_ME4650,
 		.ai_nchan	= 16,
 		.dio_nchan	= 32,
-	}, {
+	},
+	[BOARD_ME4660] = {
 		.name		= "ME-4660",
-		.device_id	= PCI_DEVICE_ID_MEILHAUS_ME4660,
 		.ai_nchan	= 32,
 		.ai_diff_nchan	= 16,
 		.dio_nchan	= 32,
 		.has_counter	= 1,
-	}, {
+	},
+	[BOARD_ME4660I] = {
 		.name		= "ME-4660i",
-		.device_id	= PCI_DEVICE_ID_MEILHAUS_ME4660I,
 		.ai_nchan	= 32,
 		.ai_diff_nchan	= 16,
 		.dio_nchan	= 32,
 		.has_counter	= 1,
-	}, {
+	},
+	[BOARD_ME4660S] = {
 		.name		= "ME-4660s",
-		.device_id	= PCI_DEVICE_ID_MEILHAUS_ME4660S,
 		.ai_nchan	= 32,
 		.ai_diff_nchan	= 16,
 		.ai_sh_nchan	= 8,
 		.dio_nchan	= 32,
 		.has_counter	= 1,
-	}, {
+	},
+	[BOARD_ME4660IS] = {
 		.name		= "ME-4660is",
-		.device_id	= PCI_DEVICE_ID_MEILHAUS_ME4660IS,
 		.ai_nchan	= 32,
 		.ai_diff_nchan	= 16,
 		.ai_sh_nchan	= 8,
 		.dio_nchan	= 32,
 		.has_counter	= 1,
-	}, {
+	},
+	[BOARD_ME4670] = {
 		.name		= "ME-4670",
-		.device_id	= PCI_DEVICE_ID_MEILHAUS_ME4670,
 		.ao_nchan	= 4,
 		.ai_nchan	= 32,
 		.ai_diff_nchan	= 16,
 		.ex_trig_analog	= 1,
 		.dio_nchan	= 32,
 		.has_counter	= 1,
-	}, {
+	},
+	[BOARD_ME4670I] = {
 		.name		= "ME-4670i",
-		.device_id	= PCI_DEVICE_ID_MEILHAUS_ME4670I,
 		.ao_nchan	= 4,
 		.ai_nchan	= 32,
 		.ai_diff_nchan	= 16,
 		.ex_trig_analog	= 1,
 		.dio_nchan	= 32,
 		.has_counter	= 1,
-	}, {
+	},
+	[BOARD_ME4670S] = {
 		.name		= "ME-4670s",
-		.device_id	= PCI_DEVICE_ID_MEILHAUS_ME4670S,
 		.ao_nchan	= 4,
 		.ai_nchan	= 32,
 		.ai_diff_nchan	= 16,
@@ -297,9 +276,9 @@ static const struct me4000_board me4000_boards[] = {
 		.ex_trig_analog	= 1,
 		.dio_nchan	= 32,
 		.has_counter	= 1,
-	}, {
+	},
+	[BOARD_ME4670IS] = {
 		.name		= "ME-4670is",
-		.device_id	= PCI_DEVICE_ID_MEILHAUS_ME4670IS,
 		.ao_nchan	= 4,
 		.ai_nchan	= 32,
 		.ai_diff_nchan	= 16,
@@ -307,9 +286,9 @@ static const struct me4000_board me4000_boards[] = {
 		.ex_trig_analog	= 1,
 		.dio_nchan	= 32,
 		.has_counter	= 1,
-	}, {
+	},
+	[BOARD_ME4680] = {
 		.name		= "ME-4680",
-		.device_id	= PCI_DEVICE_ID_MEILHAUS_ME4680,
 		.ao_nchan	= 4,
 		.ao_fifo	= 4,
 		.ai_nchan	= 32,
@@ -317,9 +296,9 @@ static const struct me4000_board me4000_boards[] = {
 		.ex_trig_analog	= 1,
 		.dio_nchan	= 32,
 		.has_counter	= 1,
-	}, {
+	},
+	[BOARD_ME4680I] = {
 		.name		= "ME-4680i",
-		.device_id	= PCI_DEVICE_ID_MEILHAUS_ME4680I,
 		.ao_nchan	= 4,
 		.ao_fifo	= 4,
 		.ai_nchan	= 32,
@@ -327,9 +306,9 @@ static const struct me4000_board me4000_boards[] = {
 		.ex_trig_analog	= 1,
 		.dio_nchan	= 32,
 		.has_counter	= 1,
-	}, {
+	},
+	[BOARD_ME4680S] = {
 		.name		= "ME-4680s",
-		.device_id	= PCI_DEVICE_ID_MEILHAUS_ME4680S,
 		.ao_nchan	= 4,
 		.ao_fifo	= 4,
 		.ai_nchan	= 32,
@@ -338,9 +317,9 @@ static const struct me4000_board me4000_boards[] = {
 		.ex_trig_analog	= 1,
 		.dio_nchan	= 32,
 		.has_counter	= 1,
-	}, {
+	},
+	[BOARD_ME4680IS] = {
 		.name		= "ME-4680is",
-		.device_id	= PCI_DEVICE_ID_MEILHAUS_ME4680IS,
 		.ao_nchan	= 4,
 		.ao_fifo	= 4,
 		.ai_nchan	= 32,
@@ -376,6 +355,7 @@ static int xilinx_download(struct comedi_device *dev)
 	wait_queue_head_t queue;
 	int idx = 0;
 	int size = 0;
+	unsigned int intcsr;
 
 	if (!xilinx_iobase)
 		return -ENODEV;
@@ -386,27 +366,28 @@ static int xilinx_download(struct comedi_device *dev)
 	 * Set PLX local interrupt 2 polarity to high.
 	 * Interrupt is thrown by init pin of xilinx.
 	 */
-	outl(0x10, info->plx_regbase + PLX_INTCSR);
+	outl(PLX9052_INTCSR_LI2POL, info->plx_regbase + PLX9052_INTCSR);
 
 	/* Set /CS and /WRITE of the Xilinx */
-	value = inl(info->plx_regbase + PLX_ICR);
-	value |= 0x100;
-	outl(value, info->plx_regbase + PLX_ICR);
+	value = inl(info->plx_regbase + PLX9052_CNTRL);
+	value |= PLX9052_CNTRL_UIO2_DATA;
+	outl(value, info->plx_regbase + PLX9052_CNTRL);
 
 	/* Init Xilinx with CS1 */
 	inb(xilinx_iobase + 0xC8);
 
 	/* Wait until /INIT pin is set */
 	udelay(20);
-	if (!(inl(info->plx_regbase + PLX_INTCSR) & 0x20)) {
+	intcsr = inl(info->plx_regbase + PLX9052_INTCSR);
+	if (!(intcsr & PLX9052_INTCSR_LI2STAT)) {
 		dev_err(dev->class_dev, "Can't init Xilinx\n");
 		return -EIO;
 	}
 
 	/* Reset /CS and /WRITE of the Xilinx */
-	value = inl(info->plx_regbase + PLX_ICR);
-	value &= ~0x100;
-	outl(value, info->plx_regbase + PLX_ICR);
+	value = inl(info->plx_regbase + PLX9052_CNTRL);
+	value &= ~PLX9052_CNTRL_UIO2_DATA;
+	outl(value, info->plx_regbase + PLX9052_CNTRL);
 	if (FIRMWARE_NOT_AVAILABLE) {
 		dev_err(dev->class_dev,
 			"xilinx firmware unavailable due to licensing, aborting");
@@ -422,7 +403,7 @@ static int xilinx_download(struct comedi_device *dev)
 			udelay(10);
 
 			/* Check if BUSY flag is low */
-			if (inl(info->plx_regbase + PLX_ICR) & 0x20) {
+			if (inl(info->plx_regbase + PLX9052_CNTRL) & PLX9052_CNTRL_UIO1_DATA) {
 				dev_err(dev->class_dev,
 					"Xilinx is still busy (idx = %d)\n",
 					idx);
@@ -432,7 +413,7 @@ static int xilinx_download(struct comedi_device *dev)
 	}
 
 	/* If done flag is high download was successful */
-	if (inl(info->plx_regbase + PLX_ICR) & 0x4) {
+	if (inl(info->plx_regbase + PLX9052_CNTRL) & PLX9052_CNTRL_UIO0_DATA) {
 	} else {
 		dev_err(dev->class_dev, "DONE flag is not set\n");
 		dev_err(dev->class_dev, "Download not successful\n");
@@ -440,9 +421,9 @@ static int xilinx_download(struct comedi_device *dev)
 	}
 
 	/* Set /CS and /WRITE */
-	value = inl(info->plx_regbase + PLX_ICR);
-	value |= 0x100;
-	outl(value, info->plx_regbase + PLX_ICR);
+	value = inl(info->plx_regbase + PLX9052_CNTRL);
+	value |= PLX9052_CNTRL_UIO2_DATA;
+	outl(value, info->plx_regbase + PLX9052_CNTRL);
 
 	return 0;
 }
@@ -454,11 +435,11 @@ static void me4000_reset(struct comedi_device *dev)
 	int chan;
 
 	/* Make a hardware reset */
-	val = inl(info->plx_regbase + PLX_ICR);
-	val |= 0x40000000;
-	outl(val, info->plx_regbase + PLX_ICR);
-	val &= ~0x40000000;
-	outl(val , info->plx_regbase + PLX_ICR);
+	val = inl(info->plx_regbase + PLX9052_CNTRL);
+	val |= PLX9052_CNTRL_PCI_RESET;
+	outl(val, info->plx_regbase + PLX9052_CNTRL);
+	val &= ~PLX9052_CNTRL_PCI_RESET;
+	outl(val , info->plx_regbase + PLX9052_CNTRL);
 
 	/* 0x8000 to the DACs means an output voltage of 0V */
 	for (chan = 0; chan < 4; chan++)
@@ -474,7 +455,9 @@ static void me4000_reset(struct comedi_device *dev)
 		outl(val, dev->iobase + ME4000_AO_CTRL_REG(chan));
 
 	/* Enable interrupts on the PLX */
-	outl(0x43, info->plx_regbase + PLX_INTCSR);
+	outl(PLX9052_INTCSR_LI1ENAB |
+	     PLX9052_INTCSR_LI1POL |
+	     PLX9052_INTCSR_PCIENAB, info->plx_regbase + PLX9052_INTCSR);
 
 	/* Set the adustment register for AO demux */
 	outl(ME4000_AO_DEMUX_ADJUST_VALUE,
@@ -1550,30 +1533,17 @@ static int me4000_cnt_insn_write(struct comedi_device *dev,
 	return 1;
 }
 
-static const void *me4000_find_boardinfo(struct comedi_device *dev,
-					 struct pci_dev *pcidev)
-{
-	const struct me4000_board *thisboard;
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(me4000_boards); i++) {
-		thisboard = &me4000_boards[i];
-		if (thisboard->device_id == pcidev->device)
-			return thisboard;
-	}
-	return NULL;
-}
-
 static int me4000_auto_attach(struct comedi_device *dev,
-					unsigned long context_unused)
+			      unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct me4000_board *thisboard;
+	const struct me4000_board *thisboard = NULL;
 	struct me4000_info *info;
 	struct comedi_subdevice *s;
 	int result;
 
-	thisboard = me4000_find_boardinfo(dev, pcidev);
+	if (context < ARRAY_SIZE(me4000_boards))
+		thisboard = &me4000_boards[context];
 	if (!thisboard)
 		return -ENODEV;
 	dev->board_ptr = thisboard;
@@ -1584,7 +1554,7 @@ static int me4000_auto_attach(struct comedi_device *dev,
 		return -ENOMEM;
 	dev->private = info;
 
-	result = comedi_pci_enable(pcidev, dev->board_name);
+	result = comedi_pci_enable(dev);
 	if (result)
 		return result;
 
@@ -1710,16 +1680,11 @@ static int me4000_auto_attach(struct comedi_device *dev,
 
 static void me4000_detach(struct comedi_device *dev)
 {
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-	if (pcidev) {
-		if (dev->iobase) {
-			me4000_reset(dev);
-			comedi_pci_disable(pcidev);
-		}
-	}
+	if (dev->iobase)
+		me4000_reset(dev);
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver me4000_driver = {
@@ -1730,26 +1695,26 @@ static struct comedi_driver me4000_driver = {
 };
 
 static int me4000_pci_probe(struct pci_dev *dev,
-				      const struct pci_device_id *ent)
+			    const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &me4000_driver);
+	return comedi_pci_auto_config(dev, &me4000_driver, id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(me4000_pci_table) = {
-	{PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4650)},
-	{PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4660)},
-	{PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4660I)},
-	{PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4660S)},
-	{PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4660IS)},
-	{PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4670)},
-	{PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4670I)},
-	{PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4670S)},
-	{PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4670IS)},
-	{PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4680)},
-	{PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4680I)},
-	{PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4680S)},
-	{PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4680IS)},
-	{0}
+	{ PCI_VDEVICE(MEILHAUS, 0x4650), BOARD_ME4650 },
+	{ PCI_VDEVICE(MEILHAUS, 0x4660), BOARD_ME4660 },
+	{ PCI_VDEVICE(MEILHAUS, 0x4661), BOARD_ME4660I },
+	{ PCI_VDEVICE(MEILHAUS, 0x4662), BOARD_ME4660S },
+	{ PCI_VDEVICE(MEILHAUS, 0x4663), BOARD_ME4660IS },
+	{ PCI_VDEVICE(MEILHAUS, 0x4670), BOARD_ME4670 },
+	{ PCI_VDEVICE(MEILHAUS, 0x4671), BOARD_ME4670I },
+	{ PCI_VDEVICE(MEILHAUS, 0x4672), BOARD_ME4670S },
+	{ PCI_VDEVICE(MEILHAUS, 0x4673), BOARD_ME4670IS },
+	{ PCI_VDEVICE(MEILHAUS, 0x4680), BOARD_ME4680 },
+	{ PCI_VDEVICE(MEILHAUS, 0x4681), BOARD_ME4680I },
+	{ PCI_VDEVICE(MEILHAUS, 0x4682), BOARD_ME4680S },
+	{ PCI_VDEVICE(MEILHAUS, 0x4683), BOARD_ME4680IS },
+	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, me4000_pci_table);
 
diff --git a/drivers/staging/comedi/drivers/me_daq.c b/drivers/staging/comedi/drivers/me_daq.c
index 06490ebc8cc..09f2a9feaf7 100644
--- a/drivers/staging/comedi/drivers/me_daq.c
+++ b/drivers/staging/comedi/drivers/me_daq.c
@@ -41,12 +41,10 @@
 
 #include "../comedidev.h"
 
-#define ME2600_FIRMWARE		"me2600_firmware.bin"
+#include "plx9052.h"
 
-#define ME2000_DEVICE_ID	0x2000
-#define ME2600_DEVICE_ID	0x2600
+#define ME2600_FIRMWARE		"me2600_firmware.bin"
 
-#define PLX_INTCSR		0x4C	/* PLX interrupt status register */
 #define XILINX_DOWNLOAD_RESET	0x42	/* Xilinx registers */
 
 #define ME_CONTROL_1			0x0000	/* - | W */
@@ -149,21 +147,26 @@ static const struct comedi_lrange me_ao_range = {
 	}
 };
 
+enum me_boardid {
+	BOARD_ME2600,
+	BOARD_ME2000,
+};
+
 struct me_board {
 	const char *name;
-	int device_id;
+	int needs_firmware;
 	int has_ao;
 };
 
 static const struct me_board me_boards[] = {
-	{
+	[BOARD_ME2600] = {
 		.name		= "me-2600i",
-		.device_id	= ME2600_DEVICE_ID,
+		.needs_firmware	= 1,
 		.has_ao		= 1,
-	}, {
+	},
+	[BOARD_ME2000] = {
 		.name		= "me-2000i",
-		.device_id	= ME2000_DEVICE_ID,
-	}
+	},
 };
 
 struct me_private_data {
@@ -396,7 +399,7 @@ static int me2600_xilinx_download(struct comedi_device *dev,
 	unsigned int i;
 
 	/* disable irq's on PLX */
-	writel(0x00, dev_private->plx_regbase + PLX_INTCSR);
+	writel(0x00, dev_private->plx_regbase + PLX9052_INTCSR);
 
 	/* First, make a dummy read to reset xilinx */
 	value = readw(dev_private->me_regbase + XILINX_DOWNLOAD_RESET);
@@ -426,7 +429,7 @@ static int me2600_xilinx_download(struct comedi_device *dev,
 
 	/*
 	 * Loop for writing firmware byte by byte to xilinx
-	 * Firmware data start at offfset 16
+	 * Firmware data start at offset 16
 	 */
 	for (i = 0; i < file_length; i++)
 		writeb((data[16 + i] & 0xff),
@@ -437,10 +440,10 @@ static int me2600_xilinx_download(struct comedi_device *dev,
 		writeb(0x00, dev_private->me_regbase + 0x0);
 
 	/* Test if there was an error during download -> INTB was thrown */
-	value = readl(dev_private->plx_regbase + PLX_INTCSR);
-	if (value & 0x20) {
+	value = readl(dev_private->plx_regbase + PLX9052_INTCSR);
+	if (value & PLX9052_INTCSR_LI2STAT) {
 		/* Disable interrupt */
-		writel(0x00, dev_private->plx_regbase + PLX_INTCSR);
+		writel(0x00, dev_private->plx_regbase + PLX9052_INTCSR);
 		dev_err(dev->class_dev, "Xilinx download failed\n");
 		return -EIO;
 	}
@@ -449,7 +452,10 @@ static int me2600_xilinx_download(struct comedi_device *dev,
 	sleep(1);
 
 	/* Enable PLX-Interrupts */
-	writel(0x43, dev_private->plx_regbase + PLX_INTCSR);
+	writel(PLX9052_INTCSR_LI1ENAB |
+	       PLX9052_INTCSR_LI1POL |
+	       PLX9052_INTCSR_PCIENAB,
+	       dev_private->plx_regbase + PLX9052_INTCSR);
 
 	return 0;
 }
@@ -488,30 +494,17 @@ static int me_reset(struct comedi_device *dev)
 	return 0;
 }
 
-static const void *me_find_boardinfo(struct comedi_device *dev,
-				     struct pci_dev *pcidev)
-{
-	const struct me_board *board;
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(me_boards); i++) {
-		board = &me_boards[i];
-		if (board->device_id == pcidev->device)
-			return board;
-	}
-	return NULL;
-}
-
 static int me_auto_attach(struct comedi_device *dev,
-				    unsigned long context_unused)
+			  unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct me_board *board;
+	const struct me_board *board = NULL;
 	struct me_private_data *dev_private;
 	struct comedi_subdevice *s;
 	int ret;
 
-	board = me_find_boardinfo(dev, pcidev);
+	if (context < ARRAY_SIZE(me_boards))
+		board = &me_boards[context];
 	if (!board)
 		return -ENODEV;
 	dev->board_ptr = board;
@@ -522,23 +515,20 @@ static int me_auto_attach(struct comedi_device *dev,
 		return -ENOMEM;
 	dev->private = dev_private;
 
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
-	dev->iobase = 1;	/* detach needs this */
 
-	dev_private->plx_regbase = ioremap(pci_resource_start(pcidev, 0),
-					   pci_resource_len(pcidev, 0));
+	dev_private->plx_regbase = pci_ioremap_bar(pcidev, 0);
 	if (!dev_private->plx_regbase)
 		return -ENOMEM;
 
-	dev_private->me_regbase = ioremap(pci_resource_start(pcidev, 2),
-					  pci_resource_len(pcidev, 2));
+	dev_private->me_regbase = pci_ioremap_bar(pcidev, 2);
 	if (!dev_private->me_regbase)
 		return -ENOMEM;
 
 	/* Download firmware and reset card */
-	if (board->device_id == ME2600_DEVICE_ID) {
+	if (board->needs_firmware) {
 		ret = me2600_upload_firmware(dev);
 		if (ret < 0)
 			return ret;
@@ -591,7 +581,6 @@ static int me_auto_attach(struct comedi_device *dev,
 
 static void me_detach(struct comedi_device *dev)
 {
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct me_private_data *dev_private = dev->private;
 
 	if (dev_private) {
@@ -602,10 +591,7 @@ static void me_detach(struct comedi_device *dev)
 		if (dev_private->plx_regbase)
 			iounmap(dev_private->plx_regbase);
 	}
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-	}
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver me_daq_driver = {
@@ -616,14 +602,14 @@ static struct comedi_driver me_daq_driver = {
 };
 
 static int me_daq_pci_probe(struct pci_dev *dev,
-				      const struct pci_device_id *ent)
+			    const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &me_daq_driver);
+	return comedi_pci_auto_config(dev, &me_daq_driver, id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(me_daq_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, ME2600_DEVICE_ID) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, ME2000_DEVICE_ID) },
+	{ PCI_VDEVICE(MEILHAUS, 0x2600), BOARD_ME2600 },
+	{ PCI_VDEVICE(MEILHAUS, 0x2000), BOARD_ME2000 },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, me_daq_pci_table);
diff --git a/drivers/staging/comedi/drivers/mite.c b/drivers/staging/comedi/drivers/mite.c
index be2c15f8461..523c6564ffc 100644
--- a/drivers/staging/comedi/drivers/mite.c
+++ b/drivers/staging/comedi/drivers/mite.c
@@ -58,10 +58,6 @@
 #include "comedi_fc.h"
 #include "mite.h"
 
-#define PCI_MITE_SIZE		4096
-#define PCI_DAQ_SIZE		4096
-#define PCI_DAQ_SIZE_660X       8192
-
 #define TOP_OF_PAGE(x) ((x)|(~(PAGE_MASK)))
 
 struct mite_struct *mite_alloc(struct pci_dev *pcidev)
@@ -81,7 +77,7 @@ struct mite_struct *mite_alloc(struct pci_dev *pcidev)
 	}
 	return mite;
 }
-EXPORT_SYMBOL(mite_alloc);
+EXPORT_SYMBOL_GPL(mite_alloc);
 
 static void dump_chip_signature(u32 csigr_bits)
 {
@@ -104,40 +100,28 @@ static unsigned mite_fifo_size(struct mite_struct *mite, unsigned channel)
 int mite_setup2(struct mite_struct *mite, unsigned use_iodwbsr_1)
 {
 	unsigned long length;
-	resource_size_t addr;
 	int i;
 	u32 csigr_bits;
 	unsigned unknown_dma_burst_bits;
 
-	if (comedi_pci_enable(mite->pcidev, "mite")) {
-		dev_err(&mite->pcidev->dev,
-			"error enabling mite and requesting io regions\n");
-		return -EIO;
-	}
 	pci_set_master(mite->pcidev);
 
-	addr = pci_resource_start(mite->pcidev, 0);
-	mite->mite_phys_addr = addr;
-	mite->mite_io_addr = ioremap(addr, PCI_MITE_SIZE);
+	mite->mite_io_addr = pci_ioremap_bar(mite->pcidev, 0);
 	if (!mite->mite_io_addr) {
 		dev_err(&mite->pcidev->dev,
 			"Failed to remap mite io memory address\n");
 		return -ENOMEM;
 	}
+	mite->mite_phys_addr = pci_resource_start(mite->pcidev, 0);
 
-	addr = pci_resource_start(mite->pcidev, 1);
-	mite->daq_phys_addr = addr;
-	length = pci_resource_len(mite->pcidev, 1);
-	/*
-	 * In case of a 660x board, DAQ size is 8k instead of 4k
-	 * (see as shown by lspci output)
-	 */
-	mite->daq_io_addr = ioremap(mite->daq_phys_addr, length);
+	mite->daq_io_addr = pci_ioremap_bar(mite->pcidev, 1);
 	if (!mite->daq_io_addr) {
 		dev_err(&mite->pcidev->dev,
 			"Failed to remap daq io memory address\n");
 		return -ENOMEM;
 	}
+	mite->daq_phys_addr = pci_resource_start(mite->pcidev, 1);
+	length = pci_resource_len(mite->pcidev, 1);
 
 	if (use_iodwbsr_1) {
 		writel(0, mite->mite_io_addr + MITE_IODWBSR);
@@ -185,13 +169,13 @@ int mite_setup2(struct mite_struct *mite, unsigned use_iodwbsr_1)
 	dev_info(&mite->pcidev->dev, "fifo size is %i.\n", mite->fifo_size);
 	return 0;
 }
-EXPORT_SYMBOL(mite_setup2);
+EXPORT_SYMBOL_GPL(mite_setup2);
 
 int mite_setup(struct mite_struct *mite)
 {
 	return mite_setup2(mite, 0);
 }
-EXPORT_SYMBOL(mite_setup);
+EXPORT_SYMBOL_GPL(mite_setup);
 
 void mite_unsetup(struct mite_struct *mite)
 {
@@ -208,12 +192,10 @@ void mite_unsetup(struct mite_struct *mite)
 		iounmap(mite->daq_io_addr);
 		mite->daq_io_addr = NULL;
 	}
-	if (mite->mite_phys_addr) {
-		comedi_pci_disable(mite->pcidev);
+	if (mite->mite_phys_addr)
 		mite->mite_phys_addr = 0;
-	}
 }
-EXPORT_SYMBOL(mite_unsetup);
+EXPORT_SYMBOL_GPL(mite_unsetup);
 
 struct mite_dma_descriptor_ring *mite_alloc_ring(struct mite_struct *mite)
 {
@@ -232,7 +214,7 @@ struct mite_dma_descriptor_ring *mite_alloc_ring(struct mite_struct *mite)
 	ring->descriptors_dma_addr = 0;
 	return ring;
 };
-EXPORT_SYMBOL(mite_alloc_ring);
+EXPORT_SYMBOL_GPL(mite_alloc_ring);
 
 void mite_free_ring(struct mite_dma_descriptor_ring *ring)
 {
@@ -248,7 +230,7 @@ void mite_free_ring(struct mite_dma_descriptor_ring *ring)
 		kfree(ring);
 	}
 };
-EXPORT_SYMBOL(mite_free_ring);
+EXPORT_SYMBOL_GPL(mite_free_ring);
 
 struct mite_channel *mite_request_channel_in_range(struct mite_struct *mite,
 						   struct
@@ -275,7 +257,7 @@ struct mite_channel *mite_request_channel_in_range(struct mite_struct *mite,
 	spin_unlock_irqrestore(&mite->lock, flags);
 	return channel;
 }
-EXPORT_SYMBOL(mite_request_channel_in_range);
+EXPORT_SYMBOL_GPL(mite_request_channel_in_range);
 
 void mite_release_channel(struct mite_channel *mite_chan)
 {
@@ -302,7 +284,7 @@ void mite_release_channel(struct mite_channel *mite_chan)
 	}
 	spin_unlock_irqrestore(&mite->lock, flags);
 }
-EXPORT_SYMBOL(mite_release_channel);
+EXPORT_SYMBOL_GPL(mite_release_channel);
 
 void mite_dma_arm(struct mite_channel *mite_chan)
 {
@@ -325,7 +307,7 @@ void mite_dma_arm(struct mite_channel *mite_chan)
 	spin_unlock_irqrestore(&mite->lock, flags);
 /*       mite_dma_tcr(mite, channel); */
 }
-EXPORT_SYMBOL(mite_dma_arm);
+EXPORT_SYMBOL_GPL(mite_dma_arm);
 
 /**************************************/
 
@@ -382,7 +364,7 @@ int mite_buf_change(struct mite_dma_descriptor_ring *ring,
 	smp_wmb();
 	return 0;
 }
-EXPORT_SYMBOL(mite_buf_change);
+EXPORT_SYMBOL_GPL(mite_buf_change);
 
 void mite_prep_dma(struct mite_channel *mite_chan,
 		   unsigned int num_device_bits, unsigned int num_memory_bits)
@@ -473,7 +455,7 @@ void mite_prep_dma(struct mite_channel *mite_chan,
 
 	MDPRINTK("exit mite_prep_dma\n");
 }
-EXPORT_SYMBOL(mite_prep_dma);
+EXPORT_SYMBOL_GPL(mite_prep_dma);
 
 static u32 mite_device_bytes_transferred(struct mite_channel *mite_chan)
 {
@@ -487,7 +469,7 @@ u32 mite_bytes_in_transit(struct mite_channel *mite_chan)
 	return readl(mite->mite_io_addr +
 		     MITE_FCR(mite_chan->channel)) & 0x000000FF;
 }
-EXPORT_SYMBOL(mite_bytes_in_transit);
+EXPORT_SYMBOL_GPL(mite_bytes_in_transit);
 
 /* returns lower bound for number of bytes transferred from device to memory */
 u32 mite_bytes_written_to_memory_lb(struct mite_channel *mite_chan)
@@ -497,7 +479,7 @@ u32 mite_bytes_written_to_memory_lb(struct mite_channel *mite_chan)
 	device_byte_count = mite_device_bytes_transferred(mite_chan);
 	return device_byte_count - mite_bytes_in_transit(mite_chan);
 }
-EXPORT_SYMBOL(mite_bytes_written_to_memory_lb);
+EXPORT_SYMBOL_GPL(mite_bytes_written_to_memory_lb);
 
 /* returns upper bound for number of bytes transferred from device to memory */
 u32 mite_bytes_written_to_memory_ub(struct mite_channel *mite_chan)
@@ -507,7 +489,7 @@ u32 mite_bytes_written_to_memory_ub(struct mite_channel *mite_chan)
 	in_transit_count = mite_bytes_in_transit(mite_chan);
 	return mite_device_bytes_transferred(mite_chan) - in_transit_count;
 }
-EXPORT_SYMBOL(mite_bytes_written_to_memory_ub);
+EXPORT_SYMBOL_GPL(mite_bytes_written_to_memory_ub);
 
 /* returns lower bound for number of bytes read from memory to device */
 u32 mite_bytes_read_from_memory_lb(struct mite_channel *mite_chan)
@@ -517,7 +499,7 @@ u32 mite_bytes_read_from_memory_lb(struct mite_channel *mite_chan)
 	device_byte_count = mite_device_bytes_transferred(mite_chan);
 	return device_byte_count + mite_bytes_in_transit(mite_chan);
 }
-EXPORT_SYMBOL(mite_bytes_read_from_memory_lb);
+EXPORT_SYMBOL_GPL(mite_bytes_read_from_memory_lb);
 
 /* returns upper bound for number of bytes read from memory to device */
 u32 mite_bytes_read_from_memory_ub(struct mite_channel *mite_chan)
@@ -527,7 +509,7 @@ u32 mite_bytes_read_from_memory_ub(struct mite_channel *mite_chan)
 	in_transit_count = mite_bytes_in_transit(mite_chan);
 	return mite_device_bytes_transferred(mite_chan) + in_transit_count;
 }
-EXPORT_SYMBOL(mite_bytes_read_from_memory_ub);
+EXPORT_SYMBOL_GPL(mite_bytes_read_from_memory_ub);
 
 unsigned mite_dma_tcr(struct mite_channel *mite_chan)
 {
@@ -542,7 +524,7 @@ unsigned mite_dma_tcr(struct mite_channel *mite_chan)
 
 	return tcr;
 }
-EXPORT_SYMBOL(mite_dma_tcr);
+EXPORT_SYMBOL_GPL(mite_dma_tcr);
 
 void mite_dma_disarm(struct mite_channel *mite_chan)
 {
@@ -553,7 +535,7 @@ void mite_dma_disarm(struct mite_channel *mite_chan)
 	chor = CHOR_ABORT;
 	writel(chor, mite->mite_io_addr + MITE_CHOR(mite_chan->channel));
 }
-EXPORT_SYMBOL(mite_dma_disarm);
+EXPORT_SYMBOL_GPL(mite_dma_disarm);
 
 int mite_sync_input_dma(struct mite_channel *mite_chan,
 			struct comedi_async *async)
@@ -591,7 +573,7 @@ int mite_sync_input_dma(struct mite_channel *mite_chan,
 	async->events |= COMEDI_CB_BLOCK;
 	return 0;
 }
-EXPORT_SYMBOL(mite_sync_input_dma);
+EXPORT_SYMBOL_GPL(mite_sync_input_dma);
 
 int mite_sync_output_dma(struct mite_channel *mite_chan,
 			 struct comedi_async *async)
@@ -629,7 +611,7 @@ int mite_sync_output_dma(struct mite_channel *mite_chan,
 	}
 	return 0;
 }
-EXPORT_SYMBOL(mite_sync_output_dma);
+EXPORT_SYMBOL_GPL(mite_sync_output_dma);
 
 unsigned mite_get_status(struct mite_channel *mite_chan)
 {
@@ -648,7 +630,7 @@ unsigned mite_get_status(struct mite_channel *mite_chan)
 	spin_unlock_irqrestore(&mite->lock, flags);
 	return status;
 }
-EXPORT_SYMBOL(mite_get_status);
+EXPORT_SYMBOL_GPL(mite_get_status);
 
 int mite_done(struct mite_channel *mite_chan)
 {
@@ -662,7 +644,7 @@ int mite_done(struct mite_channel *mite_chan)
 	spin_unlock_irqrestore(&mite->lock, flags);
 	return done;
 }
-EXPORT_SYMBOL(mite_done);
+EXPORT_SYMBOL_GPL(mite_done);
 
 #ifdef DEBUG_MITE
 
@@ -795,7 +777,7 @@ void mite_dump_regs(struct mite_channel *mite_chan)
 	value = readl(mite_io_addr + offset);
 	pr_debug("mite status[FCR] at 0x%08x =0x%08x\n", offset, value);
 }
-EXPORT_SYMBOL(mite_dump_regs);
+EXPORT_SYMBOL_GPL(mite_dump_regs);
 #endif
 
 static int __init mite_module_init(void)
diff --git a/drivers/staging/comedi/drivers/mpc624.c b/drivers/staging/comedi/drivers/mpc624.c
index 67dc5ad81b0..4717be4ad26 100644
--- a/drivers/staging/comedi/drivers/mpc624.c
+++ b/drivers/staging/comedi/drivers/mpc624.c
@@ -285,18 +285,11 @@ static int mpc624_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	struct mpc624_private *devpriv;
 	struct comedi_subdevice *s;
-	unsigned long iobase;
 	int ret;
 
-	iobase = it->options[0];
-	printk(KERN_INFO "comedi%d: mpc624 [0x%04lx, ", dev->minor, iobase);
-	if (request_region(iobase, MPC624_SIZE, "mpc624") == NULL) {
-		printk(KERN_ERR "I/O port(s) in use\n");
-		return -EIO;
-	}
-
-	dev->iobase = iobase;
-	dev->board_name = "mpc624";
+	ret = comedi_request_region(dev, it->options[0], MPC624_SIZE);
+	if (ret)
+		return ret;
 
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
@@ -306,48 +299,35 @@ static int mpc624_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	switch (it->options[1]) {
 	case 0:
 		devpriv->ulConvertionRate = MPC624_SPEED_3_52_kHz;
-		printk(KERN_INFO "3.52 kHz, ");
 		break;
 	case 1:
 		devpriv->ulConvertionRate = MPC624_SPEED_1_76_kHz;
-		printk(KERN_INFO "1.76 kHz, ");
 		break;
 	case 2:
 		devpriv->ulConvertionRate = MPC624_SPEED_880_Hz;
-		printk(KERN_INFO "880 Hz, ");
 		break;
 	case 3:
 		devpriv->ulConvertionRate = MPC624_SPEED_440_Hz;
-		printk(KERN_INFO "440 Hz, ");
 		break;
 	case 4:
 		devpriv->ulConvertionRate = MPC624_SPEED_220_Hz;
-		printk(KERN_INFO "220 Hz, ");
 		break;
 	case 5:
 		devpriv->ulConvertionRate = MPC624_SPEED_110_Hz;
-		printk(KERN_INFO "110 Hz, ");
 		break;
 	case 6:
 		devpriv->ulConvertionRate = MPC624_SPEED_55_Hz;
-		printk(KERN_INFO "55 Hz, ");
 		break;
 	case 7:
 		devpriv->ulConvertionRate = MPC624_SPEED_27_5_Hz;
-		printk(KERN_INFO "27.5 Hz, ");
 		break;
 	case 8:
 		devpriv->ulConvertionRate = MPC624_SPEED_13_75_Hz;
-		printk(KERN_INFO "13.75 Hz, ");
 		break;
 	case 9:
 		devpriv->ulConvertionRate = MPC624_SPEED_6_875_Hz;
-		printk(KERN_INFO "6.875 Hz, ");
 		break;
 	default:
-		printk
-		    (KERN_ERR "illegal conversion rate setting!"
-			" Valid numbers are 0..9. Using 9 => 6.875 Hz, ");
 		devpriv->ulConvertionRate = MPC624_SPEED_3_52_kHz;
 	}
 
@@ -362,37 +342,26 @@ static int mpc624_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	switch (it->options[1]) {
 	default:
 		s->maxdata = 0x3FFFFFFF;
-		printk(KERN_INFO "30 bit, ");
 	}
 
 	switch (it->options[1]) {
 	case 0:
 		s->range_table = &range_mpc624_bipolar1;
-		printk(KERN_INFO "1.01V]: ");
 		break;
 	default:
 		s->range_table = &range_mpc624_bipolar10;
-		printk(KERN_INFO "10.1V]: ");
 	}
 	s->len_chanlist = 1;
 	s->insn_read = mpc624_ai_rinsn;
 
-	printk(KERN_INFO "attached\n");
-
 	return 1;
 }
 
-static void mpc624_detach(struct comedi_device *dev)
-{
-	if (dev->iobase)
-		release_region(dev->iobase, MPC624_SIZE);
-}
-
 static struct comedi_driver mpc624_driver = {
 	.driver_name	= "mpc624",
 	.module		= THIS_MODULE,
 	.attach		= mpc624_attach,
-	.detach		= mpc624_detach
+	.detach		= comedi_legacy_detach,
 };
 module_comedi_driver(mpc624_driver);
 
diff --git a/drivers/staging/comedi/drivers/multiq3.c b/drivers/staging/comedi/drivers/multiq3.c
index 1f5f402f3d1..7a8292086e1 100644
--- a/drivers/staging/comedi/drivers/multiq3.c
+++ b/drivers/staging/comedi/drivers/multiq3.c
@@ -222,41 +222,20 @@ static void encoder_reset(struct comedi_device *dev)
 	}
 }
 
-/*
-   options[0] - I/O port
-   options[1] - irq
-   options[2] - number of encoder chips installed
- */
-
 static int multiq3_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
 	struct multiq3_private *devpriv;
-	int result = 0;
-	unsigned long iobase;
-	unsigned int irq;
 	struct comedi_subdevice *s;
+	int ret;
 
-	iobase = it->options[0];
-	printk(KERN_INFO "comedi%d: multiq3: 0x%04lx ", dev->minor, iobase);
-	if (!request_region(iobase, MULTIQ3_SIZE, "multiq3")) {
-		printk(KERN_ERR "comedi%d: I/O port conflict\n", dev->minor);
-		return -EIO;
-	}
-
-	dev->iobase = iobase;
+	ret = comedi_request_region(dev, it->options[0], MULTIQ3_SIZE);
+	if (ret)
+		return ret;
 
-	irq = it->options[1];
-	if (irq)
-		printk(KERN_WARNING "comedi%d: irq = %u ignored\n",
-			dev->minor, irq);
-	else
-		printk(KERN_WARNING "comedi%d: no irq\n", dev->minor);
-	dev->board_name = "multiq3";
-
-	result = comedi_alloc_subdevices(dev, 5);
-	if (result)
-		return result;
+	ret = comedi_alloc_subdevices(dev, 5);
+	if (ret)
+		return ret;
 
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
@@ -315,19 +294,11 @@ static int multiq3_attach(struct comedi_device *dev,
 	return 0;
 }
 
-static void multiq3_detach(struct comedi_device *dev)
-{
-	if (dev->iobase)
-		release_region(dev->iobase, MULTIQ3_SIZE);
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-}
-
 static struct comedi_driver multiq3_driver = {
 	.driver_name	= "multiq3",
 	.module		= THIS_MODULE,
 	.attach		= multiq3_attach,
-	.detach		= multiq3_detach,
+	.detach		= comedi_legacy_detach,
 };
 module_comedi_driver(multiq3_driver);
 
diff --git a/drivers/staging/comedi/drivers/ni_6527.c b/drivers/staging/comedi/drivers/ni_6527.c
index bcd4df290ec..d10f777b7f1 100644
--- a/drivers/staging/comedi/drivers/ni_6527.c
+++ b/drivers/staging/comedi/drivers/ni_6527.c
@@ -81,33 +81,24 @@ Updated: Sat, 25 Jan 2003 13:24:40 -0800
 #define Rising_Edge_Detection_Enable(x)		(0x018+(x))
 #define Falling_Edge_Detection_Enable(x)	(0x020+(x))
 
-struct ni6527_board {
+enum ni6527_boardid {
+	BOARD_PCI6527,
+	BOARD_PXI6527,
+};
 
-	int dev_id;
+struct ni6527_board {
 	const char *name;
 };
 
 static const struct ni6527_board ni6527_boards[] = {
-	{
-	 .dev_id = 0x2b20,
-	 .name = "pci-6527",
-	 },
-	{
-	 .dev_id = 0x2b10,
-	 .name = "pxi-6527",
-	 },
-};
-
-#define this_board ((const struct ni6527_board *)dev->board_ptr)
-
-static DEFINE_PCI_DEVICE_TABLE(ni6527_pci_table) = {
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2b10)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2b20)},
-	{0}
+	[BOARD_PCI6527] = {
+		.name		= "pci-6527",
+	},
+	[BOARD_PXI6527] = {
+		.name		= "pxi-6527",
+	},
 };
 
-MODULE_DEVICE_TABLE(pci, ni6527_pci_table);
-
 struct ni6527_private {
 	struct mite_struct *mite;
 	unsigned int filter_interval;
@@ -329,37 +320,31 @@ static int ni6527_intr_insn_config(struct comedi_device *dev,
 	return 2;
 }
 
-static const struct ni6527_board *
-ni6527_find_boardinfo(struct pci_dev *pcidev)
-{
-	unsigned int dev_id = pcidev->device;
-	unsigned int n;
-
-	for (n = 0; n < ARRAY_SIZE(ni6527_boards); n++) {
-		const struct ni6527_board *board = &ni6527_boards[n];
-		if (board->dev_id == dev_id)
-			return board;
-	}
-	return NULL;
-}
-
 static int ni6527_auto_attach(struct comedi_device *dev,
-					unsigned long context_unused)
+			      unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	const struct ni6527_board *board = NULL;
 	struct ni6527_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret;
 
+	if (context < ARRAY_SIZE(ni6527_boards))
+		board = &ni6527_boards[context];
+	if (!board)
+		return -ENODEV;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
+
+	ret = comedi_pci_enable(dev);
+	if (ret)
+		return ret;
+
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	dev->board_ptr = ni6527_find_boardinfo(pcidev);
-	if (!dev->board_ptr)
-		return -ENODEV;
-
 	devpriv->mite = mite_alloc(pcidev);
 	if (!devpriv->mite)
 		return -ENOMEM;
@@ -370,7 +355,6 @@ static int ni6527_auto_attach(struct comedi_device *dev,
 		return ret;
 	}
 
-	dev->board_name = this_board->name;
 	dev_info(dev->class_dev, "board: %s, ID=0x%02x\n", dev->board_name,
 		 readb(devpriv->mite->daq_io_addr + ID_Register));
 
@@ -439,6 +423,7 @@ static void ni6527_detach(struct comedi_device *dev)
 		mite_unsetup(devpriv->mite);
 		mite_free(devpriv->mite);
 	}
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver ni6527_driver = {
@@ -449,15 +434,22 @@ static struct comedi_driver ni6527_driver = {
 };
 
 static int ni6527_pci_probe(struct pci_dev *dev,
-				      const struct pci_device_id *ent)
+			    const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &ni6527_driver);
+	return comedi_pci_auto_config(dev, &ni6527_driver, id->driver_data);
 }
 
+static DEFINE_PCI_DEVICE_TABLE(ni6527_pci_table) = {
+	{ PCI_VDEVICE(NI, 0x2b10), BOARD_PXI6527 },
+	{ PCI_VDEVICE(NI, 0x2b20), BOARD_PCI6527 },
+	{ 0 }
+};
+MODULE_DEVICE_TABLE(pci, ni6527_pci_table);
+
 static struct pci_driver ni6527_pci_driver = {
-	.name = DRIVER_NAME,
-	.id_table = ni6527_pci_table,
-	.probe = ni6527_pci_probe,
+	.name		= DRIVER_NAME,
+	.id_table	= ni6527_pci_table,
+	.probe		= ni6527_pci_probe,
 	.remove		= comedi_pci_auto_unconfig,
 };
 module_comedi_pci_driver(ni6527_driver, ni6527_pci_driver);
diff --git a/drivers/staging/comedi/drivers/ni_65xx.c b/drivers/staging/comedi/drivers/ni_65xx.c
index bfa790ecf41..3f71f0f54d3 100644
--- a/drivers/staging/comedi/drivers/ni_65xx.c
+++ b/drivers/staging/comedi/drivers/ni_65xx.c
@@ -112,8 +112,32 @@ static inline unsigned Filter_Enable(unsigned port)
 #define OverflowIntEnable		0x02
 #define EdgeIntEnable			0x01
 
+enum ni_65xx_boardid {
+	BOARD_PCI6509,
+	BOARD_PXI6509,
+	BOARD_PCI6510,
+	BOARD_PCI6511,
+	BOARD_PXI6511,
+	BOARD_PCI6512,
+	BOARD_PXI6512,
+	BOARD_PCI6513,
+	BOARD_PXI6513,
+	BOARD_PCI6514,
+	BOARD_PXI6514,
+	BOARD_PCI6515,
+	BOARD_PXI6515,
+	BOARD_PCI6516,
+	BOARD_PCI6517,
+	BOARD_PCI6518,
+	BOARD_PCI6519,
+	BOARD_PCI6520,
+	BOARD_PCI6521,
+	BOARD_PXI6521,
+	BOARD_PCI6528,
+	BOARD_PXI6528,
+};
+
 struct ni_65xx_board {
-	int dev_id;
 	const char *name;
 	unsigned num_dio_ports;
 	unsigned num_di_ports;
@@ -122,130 +146,117 @@ struct ni_65xx_board {
 };
 
 static const struct ni_65xx_board ni_65xx_boards[] = {
-	{
-	 .dev_id = 0x7085,
-	 .name = "pci-6509",
-	 .num_dio_ports = 12,
-	 .invert_outputs = 0},
-	{
-	 .dev_id = 0x1710,
-	 .name = "pxi-6509",
-	 .num_dio_ports = 12,
-	 .invert_outputs = 0},
-	{
-	 .dev_id = 0x7124,
-	 .name = "pci-6510",
-	 .num_di_ports = 4},
-	{
-	 .dev_id = 0x70c3,
-	 .name = "pci-6511",
-	 .num_di_ports = 8},
-	{
-	 .dev_id = 0x70d3,
-	 .name = "pxi-6511",
-	 .num_di_ports = 8},
-	{
-	 .dev_id = 0x70cc,
-	 .name = "pci-6512",
-	 .num_do_ports = 8},
-	{
-	 .dev_id = 0x70d2,
-	 .name = "pxi-6512",
-	 .num_do_ports = 8},
-	{
-	 .dev_id = 0x70c8,
-	 .name = "pci-6513",
-	 .num_do_ports = 8,
-	 .invert_outputs = 1},
-	{
-	 .dev_id = 0x70d1,
-	 .name = "pxi-6513",
-	 .num_do_ports = 8,
-	 .invert_outputs = 1},
-	{
-	 .dev_id = 0x7088,
-	 .name = "pci-6514",
-	 .num_di_ports = 4,
-	 .num_do_ports = 4,
-	 .invert_outputs = 1},
-	{
-	 .dev_id = 0x70CD,
-	 .name = "pxi-6514",
-	 .num_di_ports = 4,
-	 .num_do_ports = 4,
-	 .invert_outputs = 1},
-	{
-	 .dev_id = 0x7087,
-	 .name = "pci-6515",
-	 .num_di_ports = 4,
-	 .num_do_ports = 4,
-	 .invert_outputs = 1},
-	{
-	 .dev_id = 0x70c9,
-	 .name = "pxi-6515",
-	 .num_di_ports = 4,
-	 .num_do_ports = 4,
-	 .invert_outputs = 1},
-	{
-	 .dev_id = 0x7125,
-	 .name = "pci-6516",
-	 .num_do_ports = 4,
-	 .invert_outputs = 1},
-	{
-	 .dev_id = 0x7126,
-	 .name = "pci-6517",
-	 .num_do_ports = 4,
-	 .invert_outputs = 1},
-	{
-	 .dev_id = 0x7127,
-	 .name = "pci-6518",
-	 .num_di_ports = 2,
-	 .num_do_ports = 2,
-	 .invert_outputs = 1},
-	{
-	 .dev_id = 0x7128,
-	 .name = "pci-6519",
-	 .num_di_ports = 2,
-	 .num_do_ports = 2,
-	 .invert_outputs = 1},
-	{
-	 .dev_id = 0x71c5,
-	 .name = "pci-6520",
-	 .num_di_ports = 1,
-	 .num_do_ports = 1,
-	 },
-	{
-	 .dev_id = 0x718b,
-	 .name = "pci-6521",
-	 .num_di_ports = 1,
-	 .num_do_ports = 1,
-	 },
-	{
-	 .dev_id = 0x718c,
-	 .name = "pxi-6521",
-	 .num_di_ports = 1,
-	 .num_do_ports = 1,
-	 },
-	{
-	 .dev_id = 0x70a9,
-	 .name = "pci-6528",
-	 .num_di_ports = 3,
-	 .num_do_ports = 3,
-	 },
-	{
-	 .dev_id = 0x7086,
-	 .name = "pxi-6528",
-	 .num_di_ports = 3,
-	 .num_do_ports = 3,
-	 },
+	[BOARD_PCI6509] = {
+		.name		= "pci-6509",
+		.num_dio_ports	= 12,
+	},
+	[BOARD_PXI6509] = {
+		.name		= "pxi-6509",
+		.num_dio_ports	= 12,
+	},
+	[BOARD_PCI6510] = {
+		.name		= "pci-6510",
+		.num_di_ports	= 4,
+	},
+	[BOARD_PCI6511] = {
+		.name		= "pci-6511",
+		.num_di_ports	= 8,
+	},
+	[BOARD_PXI6511] = {
+		.name		= "pxi-6511",
+		.num_di_ports	= 8,
+	},
+	[BOARD_PCI6512] = {
+		.name		= "pci-6512",
+		.num_do_ports	= 8,
+	},
+	[BOARD_PXI6512] = {
+		.name		= "pxi-6512",
+		.num_do_ports	= 8,
+	},
+	[BOARD_PCI6513] = {
+		.name		= "pci-6513",
+		.num_do_ports	= 8,
+		.invert_outputs	= 1,
+	},
+	[BOARD_PXI6513] = {
+		.name		= "pxi-6513",
+		.num_do_ports	= 8,
+		.invert_outputs	= 1,
+	},
+	[BOARD_PCI6514] = {
+		.name		= "pci-6514",
+		.num_di_ports	= 4,
+		.num_do_ports	= 4,
+		.invert_outputs	= 1,
+	},
+	[BOARD_PXI6514] = {
+		.name		= "pxi-6514",
+		.num_di_ports	= 4,
+		.num_do_ports	= 4,
+		.invert_outputs	= 1,
+	},
+	[BOARD_PCI6515] = {
+		.name		= "pci-6515",
+		.num_di_ports	= 4,
+		.num_do_ports	= 4,
+		.invert_outputs	= 1,
+	},
+	[BOARD_PXI6515] = {
+		.name		= "pxi-6515",
+		.num_di_ports	= 4,
+		.num_do_ports	= 4,
+		.invert_outputs	= 1,
+	},
+	[BOARD_PCI6516] = {
+		.name		= "pci-6516",
+		.num_do_ports	= 4,
+		.invert_outputs	= 1,
+	},
+	[BOARD_PCI6517] = {
+		.name		= "pci-6517",
+		.num_do_ports	= 4,
+		.invert_outputs	= 1,
+	},
+	[BOARD_PCI6518] = {
+		.name		= "pci-6518",
+		.num_di_ports	= 2,
+		.num_do_ports	= 2,
+		.invert_outputs	= 1,
+	},
+	[BOARD_PCI6519] = {
+		.name		= "pci-6519",
+		.num_di_ports	= 2,
+		.num_do_ports	= 2,
+		.invert_outputs	= 1,
+	},
+	[BOARD_PCI6520] = {
+		.name		= "pci-6520",
+		.num_di_ports	= 1,
+		.num_do_ports	= 1,
+	},
+	[BOARD_PCI6521] = {
+		.name		= "pci-6521",
+		.num_di_ports	= 1,
+		.num_do_ports	= 1,
+	},
+	[BOARD_PXI6521] = {
+		.name		= "pxi-6521",
+		.num_di_ports	= 1,
+		.num_do_ports	= 1,
+	},
+	[BOARD_PCI6528] = {
+		.name		= "pci-6528",
+		.num_di_ports	= 3,
+		.num_do_ports	= 3,
+	},
+	[BOARD_PXI6528] = {
+		.name		= "pxi-6528",
+		.num_di_ports	= 3,
+		.num_do_ports	= 3,
+	},
 };
 
-#define n_ni_65xx_boards ARRAY_SIZE(ni_65xx_boards)
-static inline const struct ni_65xx_board *board(struct comedi_device *dev)
-{
-	return dev->board_ptr;
-}
-
 static inline unsigned ni_65xx_port_by_channel(unsigned channel)
 {
 	return channel / ni_65xx_channels_per_port;
@@ -257,34 +268,6 @@ static inline unsigned ni_65xx_total_num_ports(const struct ni_65xx_board
 	return board->num_dio_ports + board->num_di_ports + board->num_do_ports;
 }
 
-static DEFINE_PCI_DEVICE_TABLE(ni_65xx_pci_table) = {
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1710)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x7085)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x7086)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x7087)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x7088)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70a9)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70c3)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70c8)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70c9)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70cc)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70CD)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70d1)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70d2)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70d3)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x7124)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x7125)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x7126)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x7127)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x7128)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x718b)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x718c)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x71c5)},
-	{0}
-};
-
-MODULE_DEVICE_TABLE(pci, ni_65xx_pci_table);
-
 struct ni_65xx_private {
 	struct mite_struct *mite;
 	unsigned int filter_interval;
@@ -398,6 +381,7 @@ static int ni_65xx_dio_insn_bits(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
 				 struct comedi_insn *insn, unsigned int *data)
 {
+	const struct ni_65xx_board *board = comedi_board(dev);
 	struct ni_65xx_private *devpriv = dev->private;
 	unsigned base_bitfield_channel;
 	const unsigned max_ports_per_bitfield = 5;
@@ -413,7 +397,7 @@ static int ni_65xx_dio_insn_bits(struct comedi_device *dev,
 		unsigned base_port_channel;
 		unsigned port_mask, port_data, port_read_bits;
 		int bitshift;
-		if (port >= ni_65xx_total_num_ports(board(dev)))
+		if (port >= ni_65xx_total_num_ports(board))
 			break;
 		base_port_channel = port_offset * ni_65xx_channels_per_port;
 		port_mask = data[0];
@@ -436,7 +420,7 @@ static int ni_65xx_dio_insn_bits(struct comedi_device *dev,
 			devpriv->output_bits[port] |=
 			    port_data & port_mask;
 			bits = devpriv->output_bits[port];
-			if (board(dev)->invert_outputs)
+			if (board->invert_outputs)
 				bits = ~bits;
 			writeb(bits,
 			       devpriv->mite->daq_io_addr +
@@ -444,7 +428,7 @@ static int ni_65xx_dio_insn_bits(struct comedi_device *dev,
 		}
 		port_read_bits =
 		    readb(devpriv->mite->daq_io_addr + Port_Data(port));
-		if (s->type == COMEDI_SUBD_DO && board(dev)->invert_outputs) {
+		if (s->type == COMEDI_SUBD_DO && board->invert_outputs) {
 			/* Outputs inverted, so invert value read back from
 			 * DO subdevice.  (Does not apply to boards with DIO
 			 * subdevice.) */
@@ -599,38 +583,32 @@ static int ni_65xx_intr_insn_config(struct comedi_device *dev,
 	return 2;
 }
 
-static const struct ni_65xx_board *
-ni_65xx_find_boardinfo(struct pci_dev *pcidev)
-{
-	unsigned int dev_id = pcidev->device;
-	unsigned int n;
-
-	for (n = 0; n < ARRAY_SIZE(ni_65xx_boards); n++) {
-		const struct ni_65xx_board *board = &ni_65xx_boards[n];
-		if (board->dev_id == dev_id)
-			return board;
-	}
-	return NULL;
-}
-
 static int ni_65xx_auto_attach(struct comedi_device *dev,
-					 unsigned long context_unused)
+			       unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	const struct ni_65xx_board *board = NULL;
 	struct ni_65xx_private *devpriv;
 	struct comedi_subdevice *s;
 	unsigned i;
 	int ret;
 
+	if (context < ARRAY_SIZE(ni_65xx_boards))
+		board = &ni_65xx_boards[context];
+	if (!board)
+		return -ENODEV;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
+
+	ret = comedi_pci_enable(dev);
+	if (ret)
+		return ret;
+
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	dev->board_ptr = ni_65xx_find_boardinfo(pcidev);
-	if (!dev->board_ptr)
-		return -ENODEV;
-
 	devpriv->mite = mite_alloc(pcidev);
 	if (!devpriv->mite)
 		return -ENOMEM;
@@ -641,7 +619,6 @@ static int ni_65xx_auto_attach(struct comedi_device *dev,
 		return ret;
 	}
 
-	dev->board_name = board(dev)->name;
 	dev->irq = mite_irq(devpriv->mite);
 	dev_info(dev->class_dev, "board: %s, ID=0x%02x", dev->board_name,
 	       readb(devpriv->mite->daq_io_addr + ID_Register));
@@ -651,11 +628,11 @@ static int ni_65xx_auto_attach(struct comedi_device *dev,
 		return ret;
 
 	s = &dev->subdevices[0];
-	if (board(dev)->num_di_ports) {
+	if (board->num_di_ports) {
 		s->type = COMEDI_SUBD_DI;
 		s->subdev_flags = SDF_READABLE;
 		s->n_chan =
-		    board(dev)->num_di_ports * ni_65xx_channels_per_port;
+		    board->num_di_ports * ni_65xx_channels_per_port;
 		s->range_table = &range_digital;
 		s->maxdata = 1;
 		s->insn_config = ni_65xx_dio_insn_config;
@@ -669,28 +646,28 @@ static int ni_65xx_auto_attach(struct comedi_device *dev,
 	}
 
 	s = &dev->subdevices[1];
-	if (board(dev)->num_do_ports) {
+	if (board->num_do_ports) {
 		s->type = COMEDI_SUBD_DO;
 		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
 		s->n_chan =
-		    board(dev)->num_do_ports * ni_65xx_channels_per_port;
+		    board->num_do_ports * ni_65xx_channels_per_port;
 		s->range_table = &range_digital;
 		s->maxdata = 1;
 		s->insn_bits = ni_65xx_dio_insn_bits;
 		s->private = ni_65xx_alloc_subdevice_private();
 		if (s->private == NULL)
 			return -ENOMEM;
-		sprivate(s)->base_port = board(dev)->num_di_ports;
+		sprivate(s)->base_port = board->num_di_ports;
 	} else {
 		s->type = COMEDI_SUBD_UNUSED;
 	}
 
 	s = &dev->subdevices[2];
-	if (board(dev)->num_dio_ports) {
+	if (board->num_dio_ports) {
 		s->type = COMEDI_SUBD_DIO;
 		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
 		s->n_chan =
-		    board(dev)->num_dio_ports * ni_65xx_channels_per_port;
+		    board->num_dio_ports * ni_65xx_channels_per_port;
 		s->range_table = &range_digital;
 		s->maxdata = 1;
 		s->insn_config = ni_65xx_dio_insn_config;
@@ -699,7 +676,7 @@ static int ni_65xx_auto_attach(struct comedi_device *dev,
 		if (s->private == NULL)
 			return -ENOMEM;
 		sprivate(s)->base_port = 0;
-		for (i = 0; i < board(dev)->num_dio_ports; ++i) {
+		for (i = 0; i < board->num_dio_ports; ++i) {
 			/*  configure all ports for input */
 			writeb(0x1,
 			       devpriv->mite->daq_io_addr +
@@ -722,10 +699,10 @@ static int ni_65xx_auto_attach(struct comedi_device *dev,
 	s->insn_bits = ni_65xx_intr_insn_bits;
 	s->insn_config = ni_65xx_intr_insn_config;
 
-	for (i = 0; i < ni_65xx_total_num_ports(board(dev)); ++i) {
+	for (i = 0; i < ni_65xx_total_num_ports(board); ++i) {
 		writeb(0x00,
 		       devpriv->mite->daq_io_addr + Filter_Enable(i));
-		if (board(dev)->invert_outputs)
+		if (board->invert_outputs)
 			writeb(0x01,
 			       devpriv->mite->daq_io_addr + Port_Data(i));
 		else
@@ -753,6 +730,7 @@ static int ni_65xx_auto_attach(struct comedi_device *dev,
 static void ni_65xx_detach(struct comedi_device *dev)
 {
 	struct ni_65xx_private *devpriv = dev->private;
+	int i;
 
 	if (devpriv && devpriv->mite && devpriv->mite->daq_io_addr) {
 		writeb(0x00,
@@ -761,20 +739,15 @@ static void ni_65xx_detach(struct comedi_device *dev)
 	}
 	if (dev->irq)
 		free_irq(dev->irq, dev);
+	for (i = 0; i < dev->n_subdevices; ++i)
+		comedi_spriv_free(dev, i);
 	if (devpriv) {
-		struct comedi_subdevice *s;
-		unsigned i;
-
-		for (i = 0; i < dev->n_subdevices; ++i) {
-			s = &dev->subdevices[i];
-			kfree(s->private);
-			s->private = NULL;
-		}
 		if (devpriv->mite) {
 			mite_unsetup(devpriv->mite);
 			mite_free(devpriv->mite);
 		}
 	}
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver ni_65xx_driver = {
@@ -785,15 +758,42 @@ static struct comedi_driver ni_65xx_driver = {
 };
 
 static int ni_65xx_pci_probe(struct pci_dev *dev,
-				       const struct pci_device_id *ent)
+			     const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &ni_65xx_driver);
+	return comedi_pci_auto_config(dev, &ni_65xx_driver, id->driver_data);
 }
 
+static DEFINE_PCI_DEVICE_TABLE(ni_65xx_pci_table) = {
+	{ PCI_VDEVICE(NI, 0x1710), BOARD_PXI6509 },
+	{ PCI_VDEVICE(NI, 0x7085), BOARD_PCI6509 },
+	{ PCI_VDEVICE(NI, 0x7086), BOARD_PXI6528 },
+	{ PCI_VDEVICE(NI, 0x7087), BOARD_PCI6515 },
+	{ PCI_VDEVICE(NI, 0x7088), BOARD_PCI6514 },
+	{ PCI_VDEVICE(NI, 0x70a9), BOARD_PCI6528 },
+	{ PCI_VDEVICE(NI, 0x70c3), BOARD_PCI6511 },
+	{ PCI_VDEVICE(NI, 0x70c8), BOARD_PCI6513 },
+	{ PCI_VDEVICE(NI, 0x70c9), BOARD_PXI6515 },
+	{ PCI_VDEVICE(NI, 0x70cc), BOARD_PCI6512 },
+	{ PCI_VDEVICE(NI, 0x70cd), BOARD_PXI6514 },
+	{ PCI_VDEVICE(NI, 0x70d1), BOARD_PXI6513 },
+	{ PCI_VDEVICE(NI, 0x70d2), BOARD_PXI6512 },
+	{ PCI_VDEVICE(NI, 0x70d3), BOARD_PXI6511 },
+	{ PCI_VDEVICE(NI, 0x7124), BOARD_PCI6510 },
+	{ PCI_VDEVICE(NI, 0x7125), BOARD_PCI6516 },
+	{ PCI_VDEVICE(NI, 0x7126), BOARD_PCI6517 },
+	{ PCI_VDEVICE(NI, 0x7127), BOARD_PCI6518 },
+	{ PCI_VDEVICE(NI, 0x7128), BOARD_PCI6519 },
+	{ PCI_VDEVICE(NI, 0x718b), BOARD_PCI6521 },
+	{ PCI_VDEVICE(NI, 0x718c), BOARD_PXI6521 },
+	{ PCI_VDEVICE(NI, 0x71c5), BOARD_PCI6520 },
+	{ 0 }
+};
+MODULE_DEVICE_TABLE(pci, ni_65xx_pci_table);
+
 static struct pci_driver ni_65xx_pci_driver = {
-	.name = "ni_65xx",
-	.id_table = ni_65xx_pci_table,
-	.probe = ni_65xx_pci_probe,
+	.name		= "ni_65xx",
+	.id_table	= ni_65xx_pci_table,
+	.probe		= ni_65xx_pci_probe,
 	.remove		= comedi_pci_auto_unconfig,
 };
 module_comedi_pci_driver(ni_65xx_driver, ni_65xx_pci_driver);
diff --git a/drivers/staging/comedi/drivers/ni_660x.c b/drivers/staging/comedi/drivers/ni_660x.c
index e46dd7a1a72..5cdda7fe97a 100644
--- a/drivers/staging/comedi/drivers/ni_660x.c
+++ b/drivers/staging/comedi/drivers/ni_660x.c
@@ -18,27 +18,25 @@
 */
 
 /*
-Driver: ni_660x
-Description: National Instruments 660x counter/timer boards
-Devices:
-[National Instruments] PCI-6601 (ni_660x), PCI-6602, PXI-6602,
-	PXI-6608
-Author: J.P. Mellor <jpmellor@rose-hulman.edu>,
-	Herman.Bruyninckx@mech.kuleuven.ac.be,
-	Wim.Meeussen@mech.kuleuven.ac.be,
-	Klaas.Gadeyne@mech.kuleuven.ac.be,
-	Frank Mori Hess <fmhess@users.sourceforge.net>
-Updated: Thu Oct 18 12:56:06 EDT 2007
-Status: experimental
-
-Encoders work.  PulseGeneration (both single pulse and pulse train)
-works. Buffered commands work for input but not output.
-
-References:
-DAQ 660x Register-Level Programmer Manual  (NI 370505A-01)
-DAQ 6601/6602 User Manual (NI 322137B-01)
-
-*/
+ * Driver: ni_660x
+ * Description: National Instruments 660x counter/timer boards
+ * Devices: [National Instruments] PCI-6601 (ni_660x), PCI-6602, PXI-6602,
+ *   PXI-6608, PXI-6624
+ * Author: J.P. Mellor <jpmellor@rose-hulman.edu>,
+ *   Herman.Bruyninckx@mech.kuleuven.ac.be,
+ *   Wim.Meeussen@mech.kuleuven.ac.be,
+ *   Klaas.Gadeyne@mech.kuleuven.ac.be,
+ *   Frank Mori Hess <fmhess@users.sourceforge.net>
+ * Updated: Fri, 15 Mar 2013 10:47:56 +0000
+ * Status: experimental
+ *
+ * Encoders work.  PulseGeneration (both single pulse and pulse train)
+ * works.  Buffered commands work for input but not output.
+ * 
+ * References:
+ * DAQ 660x Register-Level Programmer Manual  (NI 370505A-01)
+ * DAQ 6601/6602 User Manual (NI 322137B-01)
+ */
 
 #include <linux/pci.h>
 #include <linux/interrupt.h>
@@ -389,34 +387,40 @@ enum global_interrupt_config_register_bits {
 /* First chip is at base-address + 0x00, etc. */
 static const unsigned GPCT_OFFSET[2] = { 0x0, 0x800 };
 
-/* Board description*/
+enum ni_660x_boardid {
+	BOARD_PCI6601,
+	BOARD_PCI6602,
+	BOARD_PXI6602,
+	BOARD_PXI6608,
+	BOARD_PXI6624
+};
+
 struct ni_660x_board {
-	unsigned short dev_id;	/* `lspci` will show you this */
 	const char *name;
 	unsigned n_chips;	/* total number of TIO chips */
 };
 
 static const struct ni_660x_board ni_660x_boards[] = {
-	{
-	 .dev_id = 0x2c60,
-	 .name = "PCI-6601",
-	 .n_chips = 1,
-	 },
-	{
-	 .dev_id = 0x1310,
-	 .name = "PCI-6602",
-	 .n_chips = 2,
-	 },
-	{
-	 .dev_id = 0x1360,
-	 .name = "PXI-6602",
-	 .n_chips = 2,
-	 },
-	{
-	 .dev_id = 0x2cc0,
-	 .name = "PXI-6608",
-	 .n_chips = 2,
-	 },
+	[BOARD_PCI6601] = {
+		.name		= "PCI-6601",
+		.n_chips	= 1,
+	},
+	[BOARD_PCI6602] = {
+		.name		= "PCI-6602",
+		.n_chips	= 2,
+	},
+	[BOARD_PXI6602] = {
+		.name		= "PXI-6602",
+		.n_chips	= 2,
+	},
+	[BOARD_PXI6608] = {
+		.name		= "PXI-6608",
+		.n_chips	= 2,
+	},
+	[BOARD_PXI6624] = {
+		.name		= "PXI-6624",
+		.n_chips	= 2,
+	},
 };
 
 #define NI_660X_MAX_NUM_CHIPS 2
@@ -883,7 +887,7 @@ static irqreturn_t ni_660x_interrupt(int irq, void *d)
 	unsigned i;
 	unsigned long flags;
 
-	if (dev->attached == 0)
+	if (!dev->attached)
 		return IRQ_NONE;
 	/* lock to avoid race with comedi_poll */
 	spin_lock_irqsave(&devpriv->interrupt_lock, flags);
@@ -974,20 +978,6 @@ static void ni_660x_free_mite_rings(struct comedi_device *dev)
 	}
 }
 
-static const struct ni_660x_board *
-ni_660x_find_boardinfo(struct pci_dev *pcidev)
-{
-	unsigned int dev_id = pcidev->device;
-	unsigned int n;
-
-	for (n = 0; n < ARRAY_SIZE(ni_660x_boards); n++) {
-		const struct ni_660x_board *board = &ni_660x_boards[n];
-		if (board->dev_id == dev_id)
-			return board;
-	}
-	return NULL;
-}
-
 static int
 ni_660x_GPCT_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 		   struct comedi_insn *insn, unsigned int *data)
@@ -1170,32 +1160,36 @@ static int ni_660x_dio_insn_config(struct comedi_device *dev,
 }
 
 static int ni_660x_auto_attach(struct comedi_device *dev,
-					 unsigned long context_unused)
+			       unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct ni_660x_board *board;
+	const struct ni_660x_board *board = NULL;
 	struct ni_660x_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret;
 	unsigned i;
 	unsigned global_interrupt_config_bits;
 
+	if (context < ARRAY_SIZE(ni_660x_boards))
+		board = &ni_660x_boards[context];
+	if (!board)
+		return -ENODEV;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
+
+	ret = comedi_pci_enable(dev);
+	if (ret)
+		return ret;
+
 	ret = ni_660x_allocate_private(dev);
 	if (ret < 0)
 		return ret;
 	devpriv = dev->private;
 
-	dev->board_ptr = ni_660x_find_boardinfo(pcidev);
-	if (!dev->board_ptr)
-		return -ENODEV;
-	board = comedi_board(dev);
-
 	devpriv->mite = mite_alloc(pcidev);
 	if (!devpriv->mite)
 		return -ENOMEM;
 
-	dev->board_name = board->name;
-
 	ret = mite_setup2(devpriv->mite, 1);
 	if (ret < 0) {
 		dev_warn(dev->class_dev, "error setting up mite\n");
@@ -1315,6 +1309,7 @@ static void ni_660x_detach(struct comedi_device *dev)
 			mite_free(devpriv->mite);
 		}
 	}
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver ni_660x_driver = {
@@ -1325,17 +1320,18 @@ static struct comedi_driver ni_660x_driver = {
 };
 
 static int ni_660x_pci_probe(struct pci_dev *dev,
-				       const struct pci_device_id *ent)
+			     const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &ni_660x_driver);
+	return comedi_pci_auto_config(dev, &ni_660x_driver, id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(ni_660x_pci_table) = {
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2c60)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1310)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1360)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2cc0)},
-	{0}
+	{ PCI_VDEVICE(NI, 0x1310), BOARD_PCI6602 },
+	{ PCI_VDEVICE(NI, 0x1360), BOARD_PXI6602 },
+	{ PCI_VDEVICE(NI, 0x2c60), BOARD_PCI6601 },
+	{ PCI_VDEVICE(NI, 0x2cc0), BOARD_PXI6608 },
+	{ PCI_VDEVICE(NI, 0x1e40), BOARD_PXI6624 },
+	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, ni_660x_pci_table);
 
diff --git a/drivers/staging/comedi/drivers/ni_670x.c b/drivers/staging/comedi/drivers/ni_670x.c
index 2faf86c83dc..42ab6dbf9d3 100644
--- a/drivers/staging/comedi/drivers/ni_670x.c
+++ b/drivers/staging/comedi/drivers/ni_670x.c
@@ -60,26 +60,28 @@ Commands are not supported.
 #define	MISC_STATUS_OFFSET		0x14
 #define	MISC_CONTROL_OFFSET		0x14
 
-/* Board description*/
+enum ni_670x_boardid {
+	BOARD_PCI6703,
+	BOARD_PXI6704,
+	BOARD_PCI6704,
+};
 
 struct ni_670x_board {
 	const char *name;
-	unsigned short dev_id;
 	unsigned short ao_chans;
 };
 
 static const struct ni_670x_board ni_670x_boards[] = {
-	{
+	[BOARD_PCI6703] = {
 		.name		= "PCI-6703",
-		.dev_id		= 0x2c90,
 		.ao_chans	= 16,
-	}, {
+	},
+	[BOARD_PXI6704] = {
 		.name		= "PXI-6704",
-		.dev_id		= 0x1920,
 		.ao_chans	= 32,
-	}, {
+	},
+	[BOARD_PCI6704] = {
 		.name		= "PCI-6704",
-		.dev_id		= 0x1290,
 		.ao_chans	= 32,
 	},
 };
@@ -92,8 +94,6 @@ struct ni_670x_private {
 	unsigned int ao_readback[32];
 };
 
-static struct comedi_lrange range_0_20mA = { 1, {RANGE_mA(0, 20)} };
-
 static int ni_670x_ao_winsn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
@@ -189,49 +189,41 @@ static int ni_670x_dio_insn_config(struct comedi_device *dev,
 	return insn->n;
 }
 
-static const struct ni_670x_board *
-ni_670x_find_boardinfo(struct pci_dev *pcidev)
-{
-	unsigned int dev_id = pcidev->device;
-	unsigned int n;
-
-	for (n = 0; n < ARRAY_SIZE(ni_670x_boards); n++) {
-		const struct ni_670x_board *board = &ni_670x_boards[n];
-		if (board->dev_id == dev_id)
-			return board;
-	}
-	return NULL;
-}
-
 static int ni_670x_auto_attach(struct comedi_device *dev,
-					 unsigned long context_unused)
+			       unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct ni_670x_board *thisboard;
+	const struct ni_670x_board *thisboard = NULL;
 	struct ni_670x_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret;
 	int i;
 
+	if (context < ARRAY_SIZE(ni_670x_boards))
+		thisboard = &ni_670x_boards[context];
+	if (!thisboard)
+		return -ENODEV;
+	dev->board_ptr = thisboard;
+	dev->board_name = thisboard->name;
+
+	ret = comedi_pci_enable(dev);
+	if (ret)
+		return ret;
+
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	dev->board_ptr = ni_670x_find_boardinfo(pcidev);
-	if (!dev->board_ptr)
-		return -ENODEV;
 	devpriv->mite = mite_alloc(pcidev);
 	if (!devpriv->mite)
 		return -ENOMEM;
-	thisboard = comedi_board(dev);
 
 	ret = mite_setup(devpriv->mite);
 	if (ret < 0) {
 		dev_warn(dev->class_dev, "error setting up mite\n");
 		return ret;
 	}
-	dev->board_name = thisboard->name;
 
 	ret = comedi_alloc_subdevices(dev, 2);
 	if (ret)
@@ -296,6 +288,7 @@ static void ni_670x_detach(struct comedi_device *dev)
 		mite_unsetup(devpriv->mite);
 		mite_free(devpriv->mite);
 	}
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver ni_670x_driver = {
@@ -306,14 +299,15 @@ static struct comedi_driver ni_670x_driver = {
 };
 
 static int ni_670x_pci_probe(struct pci_dev *dev,
-				       const struct pci_device_id *ent)
+			     const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &ni_670x_driver);
+	return comedi_pci_auto_config(dev, &ni_670x_driver, id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(ni_670x_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2c90) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1920) },
+	{ PCI_VDEVICE(NI, 0x1290), BOARD_PCI6704 },
+	{ PCI_VDEVICE(NI, 0x1920), BOARD_PXI6704 },
+	{ PCI_VDEVICE(NI, 0x2c90), BOARD_PCI6703 },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, ni_670x_pci_table);
diff --git a/drivers/staging/comedi/drivers/ni_at_a2150.c b/drivers/staging/comedi/drivers/ni_at_a2150.c
index 06de25bb2f5..2d375168f36 100644
--- a/drivers/staging/comedi/drivers/ni_at_a2150.c
+++ b/drivers/staging/comedi/drivers/ni_at_a2150.c
@@ -154,11 +154,6 @@ static const struct a2150_board a2150_boards[] = {
 	 },
 };
 
-/*
- * Useful for shorthand access to the particular board structure
- */
-#define thisboard ((const struct a2150_board *)dev->board_ptr)
-
 struct a2150_private {
 
 	volatile unsigned int count;	/* number of data points left to be taken */
@@ -204,7 +199,7 @@ static irqreturn_t a2150_interrupt(int irq, void *d)
 	short dpnt;
 	static const int sample_size = sizeof(devpriv->dma_buffer[0]);
 
-	if (dev->attached == 0) {
+	if (!dev->attached) {
 		comedi_error(dev, "premature interrupt");
 		return IRQ_HANDLED;
 	}
@@ -319,6 +314,7 @@ static int a2150_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 static int a2150_ai_cmdtest(struct comedi_device *dev,
 			    struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
+	const struct a2150_board *thisboard = comedi_board(dev);
 	int err = 0;
 	int tmp;
 	int startChan;
@@ -604,6 +600,7 @@ static int a2150_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 static int a2150_get_timing(struct comedi_device *dev, unsigned int *period,
 			    int flags)
 {
+	const struct a2150_board *thisboard = comedi_board(dev);
 	struct a2150_private *devpriv = dev->private;
 	int lub, glb, temp;
 	int lub_divisor_shift, lub_index, glb_divisor_shift, glb_index;
@@ -719,45 +716,23 @@ static int a2150_probe(struct comedi_device *dev)
 
 static int a2150_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
+	const struct a2150_board *thisboard = comedi_board(dev);
 	struct a2150_private *devpriv;
 	struct comedi_subdevice *s;
-	unsigned long iobase = it->options[0];
 	unsigned int irq = it->options[1];
 	unsigned int dma = it->options[2];
 	static const int timeout = 2000;
 	int i;
 	int ret;
 
-	printk("comedi%d: %s: io 0x%lx", dev->minor, dev->driver->driver_name,
-	       iobase);
-	if (irq) {
-		printk(", irq %u", irq);
-	} else {
-		printk(", no irq");
-	}
-	if (dma) {
-		printk(", dma %u", dma);
-	} else {
-		printk(", no dma");
-	}
-	printk("\n");
-
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	if (iobase == 0) {
-		printk(" io base address required\n");
-		return -EINVAL;
-	}
-
-	/* check if io addresses are available */
-	if (!request_region(iobase, A2150_SIZE, dev->driver->driver_name)) {
-		printk(" I/O port conflict\n");
-		return -EIO;
-	}
-	dev->iobase = iobase;
+	ret = comedi_request_region(dev, it->options[0], A2150_SIZE);
+	if (ret)
+		return ret;
 
 	/* grab our IRQ */
 	if (irq) {
@@ -797,6 +772,7 @@ static int a2150_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	}
 
 	dev->board_ptr = a2150_boards + a2150_probe(dev);
+	thisboard = comedi_board(dev);
 	dev->board_name = thisboard->name;
 
 	ret = comedi_alloc_subdevices(dev, 1);
@@ -851,17 +827,14 @@ static void a2150_detach(struct comedi_device *dev)
 {
 	struct a2150_private *devpriv = dev->private;
 
-	if (dev->iobase) {
+	if (dev->iobase)
 		outw(APD_BIT | DPD_BIT, dev->iobase + CONFIG_REG);
-		release_region(dev->iobase, A2150_SIZE);
-	}
-	if (dev->irq)
-		free_irq(dev->irq, dev);
 	if (devpriv) {
 		if (devpriv->dma)
 			free_dma(devpriv->dma);
 		kfree(devpriv->dma_buffer);
 	}
+	comedi_legacy_detach(dev);
 };
 
 static struct comedi_driver ni_at_a2150_driver = {
diff --git a/drivers/staging/comedi/drivers/ni_at_ao.c b/drivers/staging/comedi/drivers/ni_at_ao.c
index 907f65cdbdc..7e5783a4f4e 100644
--- a/drivers/staging/comedi/drivers/ni_at_ao.c
+++ b/drivers/staging/comedi/drivers/ni_at_ao.c
@@ -337,24 +337,14 @@ static int atao_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	const struct atao_board *board = comedi_board(dev);
 	struct atao_private *devpriv;
 	struct comedi_subdevice *s;
-	unsigned long iobase;
 	int ao_unipolar;
 	int ret;
 
-	iobase = it->options[0];
-	if (iobase == 0)
-		iobase = 0x1c0;
 	ao_unipolar = it->options[3];
 
-	printk(KERN_INFO "comedi%d: ni_at_ao: 0x%04lx", dev->minor, iobase);
-
-	if (!request_region(iobase, ATAO_SIZE, "ni_at_ao")) {
-		printk(" I/O port conflict\n");
-		return -EIO;
-	}
-	dev->iobase = iobase;
-
-	dev->board_name = board->name;
+	ret = comedi_request_region(dev, it->options[0], ATAO_SIZE);
+	if (ret)
+		return ret;
 
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
@@ -409,12 +399,6 @@ static int atao_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static void atao_detach(struct comedi_device *dev)
-{
-	if (dev->iobase)
-		release_region(dev->iobase, ATAO_SIZE);
-}
-
 static const struct atao_board atao_boards[] = {
 	{
 		.name		= "ai-ao-6",
@@ -429,7 +413,7 @@ static struct comedi_driver ni_at_ao_driver = {
 	.driver_name	= "ni_at_ao",
 	.module		= THIS_MODULE,
 	.attach		= atao_attach,
-	.detach		= atao_detach,
+	.detach		= comedi_legacy_detach,
 	.board_name	= &atao_boards[0].name,
 	.offset		= sizeof(struct atao_board),
 	.num_names	= ARRAY_SIZE(atao_boards),
diff --git a/drivers/staging/comedi/drivers/ni_atmio.c b/drivers/staging/comedi/drivers/ni_atmio.c
index 2cc29965e15..4ced7ba119b 100644
--- a/drivers/staging/comedi/drivers/ni_atmio.c
+++ b/drivers/staging/comedi/drivers/ni_atmio.c
@@ -350,7 +350,7 @@ static int ni_isapnp_find_board(struct pnp_dev **dev)
 	struct pnp_dev *isapnp_dev = NULL;
 	int i;
 
-	for (i = 0; i < n_ni_boards; i++) {
+	for (i = 0; i < ARRAY_SIZE(ni_boards); i++) {
 		isapnp_dev = pnp_find_dev(NULL,
 					  ISAPNP_VENDOR('N', 'I', 'C'),
 					  ISAPNP_FUNCTION(ni_boards[i].
@@ -377,7 +377,7 @@ static int ni_isapnp_find_board(struct pnp_dev **dev)
 		}
 		break;
 	}
-	if (i == n_ni_boards)
+	if (i == ARRAY_SIZE(ni_boards))
 		return -ENODEV;
 	*dev = isapnp_dev;
 	return 0;
@@ -388,7 +388,7 @@ static int ni_getboardtype(struct comedi_device *dev)
 	int device_id = ni_read_eeprom(dev, 511);
 	int i;
 
-	for (i = 0; i < n_ni_boards; i++) {
+	for (i = 0; i < ARRAY_SIZE(ni_boards); i++) {
 		if (ni_boards[i].device_id == device_id)
 			return i;
 
@@ -406,6 +406,7 @@ static int ni_getboardtype(struct comedi_device *dev)
 static int ni_atmio_attach(struct comedi_device *dev,
 			   struct comedi_devconfig *it)
 {
+	const struct ni_board_struct *boardtype;
 	struct ni_private *devpriv;
 	struct pnp_dev *isapnp_dev;
 	int ret;
@@ -436,15 +437,9 @@ static int ni_atmio_attach(struct comedi_device *dev,
 		devpriv->isapnp_dev = isapnp_dev;
 	}
 
-	/* reserve our I/O region */
-
-	printk("comedi%d: ni_atmio: 0x%04lx", dev->minor, iobase);
-	if (!request_region(iobase, NI_SIZE, "ni_atmio")) {
-		printk(" I/O port conflict\n");
-		return -EIO;
-	}
-
-	dev->iobase = iobase;
+	ret = comedi_request_region(dev, iobase, NI_SIZE);
+	if (ret)
+		return ret;
 
 #ifdef DEBUG
 	/* board existence sanity check */
@@ -466,9 +461,10 @@ static int ni_atmio_attach(struct comedi_device *dev,
 		return -EIO;
 
 	dev->board_ptr = ni_boards + board;
+	boardtype = comedi_board(dev);
 
-	printk(" %s", boardtype.name);
-	dev->board_name = boardtype.name;
+	printk(" %s", boardtype->name);
+	dev->board_name = boardtype->name;
 
 	/* irq stuff */
 
@@ -503,10 +499,7 @@ static void ni_atmio_detach(struct comedi_device *dev)
 	struct ni_private *devpriv = dev->private;
 
 	mio_common_detach(dev);
-	if (dev->iobase)
-		release_region(dev->iobase, NI_SIZE);
-	if (dev->irq)
-		free_irq(dev->irq, dev);
+	comedi_legacy_detach(dev);
 	if (devpriv->isapnp_dev)
 		pnp_device_detach(devpriv->isapnp_dev);
 }
diff --git a/drivers/staging/comedi/drivers/ni_atmio16d.c b/drivers/staging/comedi/drivers/ni_atmio16d.c
index 4a17494f55e..6c97a0925aa 100644
--- a/drivers/staging/comedi/drivers/ni_atmio16d.c
+++ b/drivers/staging/comedi/drivers/ni_atmio16d.c
@@ -638,22 +638,13 @@ static int atmio16d_attach(struct comedi_device *dev,
 {
 	const struct atmio16_board_t *board = comedi_board(dev);
 	struct atmio16d_private *devpriv;
+	struct comedi_subdevice *s;
 	unsigned int irq;
-	unsigned long iobase;
 	int ret;
 
-	struct comedi_subdevice *s;
-
-	/* make sure the address range is free and allocate it */
-	iobase = it->options[0];
-	printk(KERN_INFO "comedi%d: atmio16d: 0x%04lx ", dev->minor, iobase);
-	if (!request_region(iobase, ATMIO16D_SIZE, "ni_atmio16d")) {
-		printk("I/O port conflict\n");
-		return -EIO;
-	}
-	dev->iobase = iobase;
-
-	dev->board_name = board->name;
+	ret = comedi_request_region(dev, it->options[0], ATMIO16D_SIZE);
+	if (ret)
+		return ret;
 
 	ret = comedi_alloc_subdevices(dev, 4);
 	if (ret)
@@ -776,18 +767,9 @@ static int atmio16d_attach(struct comedi_device *dev,
 
 static void atmio16d_detach(struct comedi_device *dev)
 {
-	const struct atmio16_board_t *board = comedi_board(dev);
-	struct comedi_subdevice *s;
-
-	if (dev->subdevices && board->has_8255) {
-		s = &dev->subdevices[3];
-		subdev_8255_cleanup(dev, s);
-	}
-	if (dev->irq)
-		free_irq(dev->irq, dev);
+	comedi_spriv_free(dev, 3);
 	reset_atmio16d(dev);
-	if (dev->iobase)
-		release_region(dev->iobase, ATMIO16D_SIZE);
+	comedi_legacy_detach(dev);
 }
 
 static const struct atmio16_board_t atmio16_boards[] = {
diff --git a/drivers/staging/comedi/drivers/ni_daq_700.c b/drivers/staging/comedi/drivers/ni_daq_700.c
index 9cc6092eacd..d067ef70e19 100644
--- a/drivers/staging/comedi/drivers/ni_daq_700.c
+++ b/drivers/staging/comedi/drivers/ni_daq_700.c
@@ -202,8 +202,6 @@ static int daq700_auto_attach(struct comedi_device *dev,
 	struct comedi_subdevice *s;
 	int ret;
 
-	dev->board_name = dev->driver->driver_name;
-
 	link->config_flags |= CONF_AUTO_SET_IO;
 	ret = comedi_pcmcia_enable(dev, NULL);
 	if (ret)
diff --git a/drivers/staging/comedi/drivers/ni_daq_dio24.c b/drivers/staging/comedi/drivers/ni_daq_dio24.c
index e1cc9d01f20..9b7805fda93 100644
--- a/drivers/staging/comedi/drivers/ni_daq_dio24.c
+++ b/drivers/staging/comedi/drivers/ni_daq_dio24.c
@@ -52,8 +52,6 @@ static int dio24_auto_attach(struct comedi_device *dev,
 	struct comedi_subdevice *s;
 	int ret;
 
-	dev->board_name = dev->driver->driver_name;
-
 	link->config_flags |= CONF_AUTO_SET_IO;
 	ret = comedi_pcmcia_enable(dev, NULL);
 	if (ret)
@@ -75,8 +73,7 @@ static int dio24_auto_attach(struct comedi_device *dev,
 
 static void dio24_detach(struct comedi_device *dev)
 {
-	if (dev->subdevices)
-		subdev_8255_cleanup(dev, &dev->subdevices[0]);
+	comedi_spriv_free(dev, 0);
 	comedi_pcmcia_disable(dev);
 }
 
diff --git a/drivers/staging/comedi/drivers/ni_labpc.c b/drivers/staging/comedi/drivers/ni_labpc.c
index f957b8859b3..3d978f34d21 100644
--- a/drivers/staging/comedi/drivers/ni_labpc.c
+++ b/drivers/staging/comedi/drivers/ni_labpc.c
@@ -1,79 +1,66 @@
 /*
-    comedi/drivers/ni_labpc.c
-    Driver for National Instruments Lab-PC series boards and compatibles
-    Copyright (C) 2001, 2002, 2003 Frank Mori Hess <fmhess@users.sourceforge.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.
-
-************************************************************************
-*/
-/*
-Driver: ni_labpc
-Description: National Instruments Lab-PC (& compatibles)
-Author: Frank Mori Hess <fmhess@users.sourceforge.net>
-Devices: [National Instruments] Lab-PC-1200 (labpc-1200),
-  Lab-PC-1200AI (labpc-1200ai), Lab-PC+ (lab-pc+), PCI-1200 (ni_labpc)
-Status: works
-
-Tested with lab-pc-1200.  For the older Lab-PC+, not all input ranges
-and analog references will work, the available ranges/arefs will
-depend on how you have configured the jumpers on your board
-(see your owner's manual).
-
-Kernel-level ISA plug-and-play support for the lab-pc-1200
-boards has not
-yet been added to the driver, mainly due to the fact that
-I don't know the device id numbers.  If you have one
-of these boards,
-please file a bug report at http://comedi.org/ 
-so I can get the necessary information from you.
-
-The 1200 series boards have onboard calibration dacs for correcting
-analog input/output offsets and gains.  The proper settings for these
-caldacs are stored on the board's eeprom.  To read the caldac values
-from the eeprom and store them into a file that can be then be used by
-comedilib, use the comedi_calibrate program.
-
-Configuration options - ISA boards:
-  [0] - I/O port base address
-  [1] - IRQ (optional, required for timed or externally triggered conversions)
-  [2] - DMA channel (optional)
-
-Configuration options - PCI boards:
-  [0] - bus (optional)
-  [1] - slot (optional)
-
-The Lab-pc+ has quirky chanlist requirements
-when scanning multiple channels.  Multiple channel scan
-sequence must start at highest channel, then decrement down to
-channel 0.  The rest of the cards can scan down like lab-pc+ or scan
-up from channel zero.  Chanlists consisting of all one channel
-are also legal, and allow you to pace conversions in bursts.
-
-*/
+ * comedi/drivers/ni_labpc.c
+ * Driver for National Instruments Lab-PC series boards and compatibles
+ * Copyright (C) 2001-2003 Frank Mori Hess <fmhess@users.sourceforge.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.
+ */
 
 /*
+ * Driver: ni_labpc
+ * Description: National Instruments Lab-PC (& compatibles)
+ * Devices: (National Instruments) Lab-PC-1200 [lab-pc-1200]
+ *	    (National Instruments) Lab-PC-1200AI [lab-pc-1200ai]
+ *	    (National Instruments) Lab-PC+ [lab-pc+]
+ * Author: Frank Mori Hess <fmhess@users.sourceforge.net>
+ * Status: works
+ *
+ * Configuration options - ISA boards:
+ *   [0] - I/O port base address
+ *   [1] - IRQ (optional, required for timed or externally triggered
+ *		conversions)
+ *   [2] - DMA channel (optional)
+ *
+ * Tested with lab-pc-1200.  For the older Lab-PC+, not all input
+ * ranges and analog references will work, the available ranges/arefs
+ * will depend on how you have configured the jumpers on your board
+ * (see your owner's manual).
+ *
+ * Kernel-level ISA plug-and-play support for the lab-pc-1200 boards
+ * has not yet been added to the driver, mainly due to the fact that
+ * I don't know the device id numbers. If you have one of these boards,
+ * please file a bug report at http://comedi.org/ so I can get the
+ * necessary information from you.
+ *
+ * The 1200 series boards have onboard calibration dacs for correcting
+ * analog input/output offsets and gains. The proper settings for these
+ * caldacs are stored on the board's eeprom. To read the caldac values
+ * from the eeprom and store them into a file that can be then be used
+ * by comedilib, use the comedi_calibrate program.
+ *
+ * The Lab-pc+ has quirky chanlist requirements when scanning multiple
+ * channels. Multiple channel scan sequence must start at highest channel,
+ * then decrement down to channel 0. The rest of the cards can scan down
+ * like lab-pc+ or scan up from channel zero. Chanlists consisting of all
+ * one channel are also legal, and allow you to pace conversions in bursts.
+ *
+ * NI manuals:
+ * 341309a (labpc-1200 register manual)
+ * 320502b (lab-pc+)
+ */
 
-NI manuals:
-341309a (labpc-1200 register manual)
-340914a (pci-1200)
-320502b (lab-pc+)
-
-*/
-
-#include <linux/pci.h>
 #include <linux/interrupt.h>
 #include <linux/slab.h>
 #include <linux/io.h>
@@ -85,128 +72,79 @@ NI manuals:
 
 #include "8253.h"
 #include "8255.h"
-#include "mite.h"
 #include "comedi_fc.h"
 #include "ni_labpc.h"
 
-#define DRV_NAME "ni_labpc"
-
-/* size of io region used by board */
-#define LABPC_SIZE           32
-/* 2 MHz master clock */
-#define LABPC_TIMER_BASE            500
-
-/* Registers for the lab-pc+ */
-
-/* write-only registers */
-#define COMMAND1_REG	0x0
-#define   ADC_GAIN_MASK	(0x7 << 4)
-#define   ADC_CHAN_BITS(x)	((x) & 0x7)
-/* enables multi channel scans */
-#define   ADC_SCAN_EN_BIT	0x80
-#define COMMAND2_REG	0x1
-/* enable pretriggering (used in conjunction with SWTRIG) */
-#define   PRETRIG_BIT	0x1
-/* enable paced conversions on external trigger */
-#define   HWTRIG_BIT	0x2
-/* enable paced conversions */
-#define   SWTRIG_BIT	0x4
-/* use two cascaded counters for pacing */
-#define   CASCADE_BIT	0x8
-#define   DAC_PACED_BIT(channel)	(0x40 << ((channel) & 0x1))
-#define COMMAND3_REG	0x2
-/* enable dma transfers */
-#define   DMA_EN_BIT	0x1
-/* enable interrupts for 8255 */
-#define   DIO_INTR_EN_BIT	0x2
-/* enable dma terminal count interrupt */
-#define   DMATC_INTR_EN_BIT	0x4
-/* enable timer interrupt */
-#define   TIMER_INTR_EN_BIT	0x8
-/* enable error interrupt */
-#define   ERR_INTR_EN_BIT	0x10
-/* enable fifo not empty interrupt */
-#define   ADC_FNE_INTR_EN_BIT	0x20
-#define ADC_CONVERT_REG	0x3
-#define DAC_LSB_REG(channel)	(0x4 + 2 * ((channel) & 0x1))
-#define DAC_MSB_REG(channel)	(0x5 + 2 * ((channel) & 0x1))
-#define ADC_CLEAR_REG	0x8
-#define DMATC_CLEAR_REG	0xa
-#define TIMER_CLEAR_REG	0xc
-/* 1200 boards only */
-#define COMMAND6_REG	0xe
-/* select ground or common-mode reference */
-#define   ADC_COMMON_BIT	0x1
-/*  adc unipolar */
-#define   ADC_UNIP_BIT	0x2
-/*  dac unipolar */
-#define   DAC_UNIP_BIT(channel)	(0x4 << ((channel) & 0x1))
-/* enable fifo half full interrupt */
-#define   ADC_FHF_INTR_EN_BIT	0x20
-/* enable interrupt on end of hardware count */
-#define   A1_INTR_EN_BIT	0x40
-/* scan up from channel zero instead of down to zero */
-#define   ADC_SCAN_UP_BIT 0x80
-#define COMMAND4_REG	0xf
-/* enables 'interval' scanning */
-#define   INTERVAL_SCAN_EN_BIT	0x1
-/* enables external signal on counter b1 output to trigger scan */
-#define   EXT_SCAN_EN_BIT	0x2
-/* chooses direction (output or input) for EXTCONV* line */
-#define   EXT_CONVERT_OUT_BIT	0x4
-/* chooses differential inputs for adc (in conjunction with board jumper) */
-#define   ADC_DIFF_BIT	0x8
-#define   EXT_CONVERT_DISABLE_BIT	0x10
-/* 1200 boards only, calibration stuff */
-#define COMMAND5_REG	0x1c
-/* enable eeprom for write */
-#define   EEPROM_WRITE_UNPROTECT_BIT	0x4
-/* enable dithering */
-#define   DITHER_EN_BIT	0x8
-/* load calibration dac */
-#define   CALDAC_LOAD_BIT	0x10
-/* serial clock - rising edge writes, falling edge reads */
-#define   SCLOCK_BIT	0x20
-/* serial data bit for writing to eeprom or calibration dacs */
-#define   SDATA_BIT	0x40
-/* enable eeprom for read/write */
-#define   EEPROM_EN_BIT	0x80
-#define INTERVAL_COUNT_REG	0x1e
-#define INTERVAL_LOAD_REG	0x1f
-#define   INTERVAL_LOAD_BITS	0x1
-
-/* read-only registers */
-#define STATUS1_REG	0x0
-/* data is available in fifo */
-#define   DATA_AVAIL_BIT	0x1
-/* overrun has occurred */
-#define   OVERRUN_BIT	0x2
-/* fifo overflow */
-#define   OVERFLOW_BIT	0x4
-/* timer interrupt has occurred */
-#define   TIMER_BIT	0x8
-/* dma terminal count has occurred */
-#define   DMATC_BIT	0x10
-/* external trigger has occurred */
-#define   EXT_TRIG_BIT	0x40
-/* 1200 boards only */
-#define STATUS2_REG	0x1d
-/* programmable eeprom serial output */
-#define   EEPROM_OUT_BIT	0x1
-/* counter A1 terminal count */
-#define   A1_TC_BIT	0x2
-/* fifo not half full */
-#define   FNHF_BIT	0x4
-#define ADC_FIFO_REG	0xa
-
-#define DIO_BASE_REG	0x10
-#define COUNTER_A_BASE_REG	0x14
-#define COUNTER_A_CONTROL_REG	(COUNTER_A_BASE_REG + 0x3)
-/* check modes put conversion pacer output in harmless state (a0 mode 2) */
-#define   INIT_A0_BITS	0x14
-/* put hardware conversion counter output in harmless state (a1 mode 0) */
-#define   INIT_A1_BITS	0x70
-#define COUNTER_B_BASE_REG	0x18
+/*
+ * Register map (all registers are 8-bit)
+ */
+#define STAT1_REG		0x00	/* R: Status 1 reg */
+#define STAT1_DAVAIL		(1 << 0)
+#define STAT1_OVERRUN		(1 << 1)
+#define STAT1_OVERFLOW		(1 << 2)
+#define STAT1_CNTINT		(1 << 3)
+#define STAT1_GATA0		(1 << 5)
+#define STAT1_EXTGATA0		(1 << 6)
+#define CMD1_REG		0x00	/* W: Command 1 reg */
+#define CMD1_MA(x)		(((x) & 0x7) << 0)
+#define CMD1_TWOSCMP		(1 << 3)
+#define CMD1_GAIN_MASK		(7 << 4)
+#define CMD1_SCANEN		(1 << 7)
+#define CMD2_REG		0x01	/* W: Command 2 reg */
+#define CMD2_PRETRIG		(1 << 0)
+#define CMD2_HWTRIG		(1 << 1)
+#define CMD2_SWTRIG		(1 << 2)
+#define CMD2_TBSEL		(1 << 3)
+#define CMD2_2SDAC0		(1 << 4)
+#define CMD2_2SDAC1		(1 << 5)
+#define CMD2_LDAC(x)		(1 << (6 + (x)))
+#define CMD3_REG		0x02	/* W: Command 3 reg */
+#define CMD3_DMAEN		(1 << 0)
+#define CMD3_DIOINTEN		(1 << 1)
+#define CMD3_DMATCINTEN		(1 << 2)
+#define CMD3_CNTINTEN		(1 << 3)
+#define CMD3_ERRINTEN		(1 << 4)
+#define CMD3_FIFOINTEN		(1 << 5)
+#define ADC_START_CONVERT_REG	0x03	/* W: Start Convert reg */
+#define DAC_LSB_REG(x)		(0x04 + 2 * (x)) /* W: DAC0/1 LSB reg */
+#define DAC_MSB_REG(x)		(0x05 + 2 * (x)) /* W: DAC0/1 MSB reg */
+#define ADC_FIFO_CLEAR_REG	0x08	/* W: A/D FIFO Clear reg */
+#define ADC_FIFO_REG		0x0a	/* R: A/D FIFO reg */
+#define DMATC_CLEAR_REG		0x0a	/* W: DMA Interrupt Clear reg */
+#define TIMER_CLEAR_REG		0x0c	/* W: Timer Interrupt Clear reg */
+#define CMD6_REG		0x0e	/* W: Command 6 reg */
+#define CMD6_NRSE		(1 << 0)
+#define CMD6_ADCUNI		(1 << 1)
+#define CMD6_DACUNI(x)		(1 << (2 + (x)))
+#define CMD6_HFINTEN		(1 << 5)
+#define CMD6_DQINTEN		(1 << 6)
+#define CMD6_SCANUP		(1 << 7)
+#define CMD4_REG		0x0f	/* W: Command 3 reg */
+#define CMD4_INTSCAN		(1 << 0)
+#define CMD4_EOIRCV		(1 << 1)
+#define CMD4_ECLKDRV		(1 << 2)
+#define CMD4_SEDIFF		(1 << 3)
+#define CMD4_ECLKRCV		(1 << 4)
+#define DIO_BASE_REG		0x10	/* R/W: 8255 DIO base reg */
+#define COUNTER_A_BASE_REG	0x14	/* R/W: 8253 Counter A base reg */
+#define COUNTER_B_BASE_REG	0x18	/* R/W: 8253 Counter B base reg */
+#define CMD5_REG		0x1c	/* W: Command 5 reg */
+#define CMD5_WRTPRT		(1 << 2)
+#define CMD5_DITHEREN		(1 << 3)
+#define CMD5_CALDACLD		(1 << 4)
+#define CMD5_SCLK		(1 << 5)
+#define CMD5_SDATA		(1 << 6)
+#define CMD5_EEPROMCS		(1 << 7)
+#define STAT2_REG		0x1d	/* R: Status 2 reg */
+#define STAT2_PROMOUT		(1 << 0)
+#define STAT2_OUTA1		(1 << 1)
+#define STAT2_FIFONHF		(1 << 2)
+#define INTERVAL_COUNT_REG	0x1e	/* W: Interval Counter Data reg */
+#define INTERVAL_STROBE_REG	0x1f	/* W: Interval Counter Strobe reg */
+
+#define LABPC_SIZE		0x20	/* size of ISA io region */
+#define LABPC_TIMER_BASE	500	/* 2 MHz master clock */
+#define LABPC_ADC_TIMEOUT	1000
 
 enum scan_mode {
 	MODE_SINGLE_CHAN,
@@ -215,187 +153,63 @@ enum scan_mode {
 	MODE_MULT_CHAN_DOWN,
 };
 
-static int labpc_cancel(struct comedi_device *dev, struct comedi_subdevice *s);
-static irqreturn_t labpc_interrupt(int irq, void *d);
-static int labpc_drain_fifo(struct comedi_device *dev);
-#ifdef CONFIG_ISA_DMA_API
-static void labpc_drain_dma(struct comedi_device *dev);
-static void handle_isa_dma(struct comedi_device *dev);
-#endif
-static void labpc_drain_dregs(struct comedi_device *dev);
-static int labpc_ai_cmdtest(struct comedi_device *dev,
-			    struct comedi_subdevice *s, struct comedi_cmd *cmd);
-static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s);
-static int labpc_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data);
-static int labpc_ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data);
-static int labpc_ao_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data);
-static int labpc_calib_read_insn(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn, unsigned int *data);
-static int labpc_calib_write_insn(struct comedi_device *dev,
-				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn, unsigned int *data);
-static int labpc_eeprom_read_insn(struct comedi_device *dev,
-				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn, unsigned int *data);
-static int labpc_eeprom_write_insn(struct comedi_device *dev,
-				   struct comedi_subdevice *s,
-				   struct comedi_insn *insn,
-				   unsigned int *data);
-static void labpc_adc_timing(struct comedi_device *dev, struct comedi_cmd *cmd,
-			     enum scan_mode scan_mode);
-#ifdef CONFIG_ISA_DMA_API
-static unsigned int labpc_suggest_transfer_size(const struct comedi_cmd *cmd);
-#endif
-static int labpc_dio_mem_callback(int dir, int port, int data,
-				  unsigned long arg);
-static void labpc_serial_out(struct comedi_device *dev, unsigned int value,
-			     unsigned int num_bits);
-static unsigned int labpc_serial_in(struct comedi_device *dev);
-static unsigned int labpc_eeprom_read(struct comedi_device *dev,
-				      unsigned int address);
-static unsigned int labpc_eeprom_read_status(struct comedi_device *dev);
-static int labpc_eeprom_write(struct comedi_device *dev,
-				       unsigned int address,
-				       unsigned int value);
-static void write_caldac(struct comedi_device *dev, unsigned int channel,
-			 unsigned int value);
-
-/* analog input ranges */
-#define NUM_LABPC_PLUS_AI_RANGES 16
-/* indicates unipolar ranges */
-static const int labpc_plus_is_unipolar[NUM_LABPC_PLUS_AI_RANGES] = {
-	0,
-	0,
-	0,
-	0,
-	0,
-	0,
-	0,
-	0,
-	1,
-	1,
-	1,
-	1,
-	1,
-	1,
-	1,
-	1,
-};
-
-/* map range index to gain bits */
-static const int labpc_plus_ai_gain_bits[NUM_LABPC_PLUS_AI_RANGES] = {
-	0x00,
-	0x10,
-	0x20,
-	0x30,
-	0x40,
-	0x50,
-	0x60,
-	0x70,
-	0x00,
-	0x10,
-	0x20,
-	0x30,
-	0x40,
-	0x50,
-	0x60,
-	0x70,
+static const int labpc_plus_ai_gain_bits[] = {
+	0x00, 0x10, 0x20, 0x30, 0x40, 0x50, 0x60, 0x70,
+	0x00, 0x10, 0x20, 0x30, 0x40, 0x50, 0x60, 0x70,
 };
 
 static const struct comedi_lrange range_labpc_plus_ai = {
-	NUM_LABPC_PLUS_AI_RANGES,
-	{
-	 BIP_RANGE(5),
-	 BIP_RANGE(4),
-	 BIP_RANGE(2.5),
-	 BIP_RANGE(1),
-	 BIP_RANGE(0.5),
-	 BIP_RANGE(0.25),
-	 BIP_RANGE(0.1),
-	 BIP_RANGE(0.05),
-	 UNI_RANGE(10),
-	 UNI_RANGE(8),
-	 UNI_RANGE(5),
-	 UNI_RANGE(2),
-	 UNI_RANGE(1),
-	 UNI_RANGE(0.5),
-	 UNI_RANGE(0.2),
-	 UNI_RANGE(0.1),
-	 }
+	16, {
+		BIP_RANGE(5),
+		BIP_RANGE(4),
+		BIP_RANGE(2.5),
+		BIP_RANGE(1),
+		BIP_RANGE(0.5),
+		BIP_RANGE(0.25),
+		BIP_RANGE(0.1),
+		BIP_RANGE(0.05),
+		UNI_RANGE(10),
+		UNI_RANGE(8),
+		UNI_RANGE(5),
+		UNI_RANGE(2),
+		UNI_RANGE(1),
+		UNI_RANGE(0.5),
+		UNI_RANGE(0.2),
+		UNI_RANGE(0.1)
+	}
 };
 
-#define NUM_LABPC_1200_AI_RANGES 14
-/* indicates unipolar ranges */
-const int labpc_1200_is_unipolar[NUM_LABPC_1200_AI_RANGES] = {
-	0,
-	0,
-	0,
-	0,
-	0,
-	0,
-	0,
-	1,
-	1,
-	1,
-	1,
-	1,
-	1,
-	1,
-};
-EXPORT_SYMBOL_GPL(labpc_1200_is_unipolar);
-
-/* map range index to gain bits */
-const int labpc_1200_ai_gain_bits[NUM_LABPC_1200_AI_RANGES] = {
-	0x00,
-	0x20,
-	0x30,
-	0x40,
-	0x50,
-	0x60,
-	0x70,
-	0x00,
-	0x20,
-	0x30,
-	0x40,
-	0x50,
-	0x60,
-	0x70,
+const int labpc_1200_ai_gain_bits[] = {
+	0x00, 0x20, 0x30, 0x40, 0x50, 0x60, 0x70,
+	0x00, 0x20, 0x30, 0x40, 0x50, 0x60, 0x70,
 };
 EXPORT_SYMBOL_GPL(labpc_1200_ai_gain_bits);
 
 const struct comedi_lrange range_labpc_1200_ai = {
-	NUM_LABPC_1200_AI_RANGES,
-	{
-	 BIP_RANGE(5),
-	 BIP_RANGE(2.5),
-	 BIP_RANGE(1),
-	 BIP_RANGE(0.5),
-	 BIP_RANGE(0.25),
-	 BIP_RANGE(0.1),
-	 BIP_RANGE(0.05),
-	 UNI_RANGE(10),
-	 UNI_RANGE(5),
-	 UNI_RANGE(2),
-	 UNI_RANGE(1),
-	 UNI_RANGE(0.5),
-	 UNI_RANGE(0.2),
-	 UNI_RANGE(0.1),
-	 }
+	14, {
+		BIP_RANGE(5),
+		BIP_RANGE(2.5),
+		BIP_RANGE(1),
+		BIP_RANGE(0.5),
+		BIP_RANGE(0.25),
+		BIP_RANGE(0.1),
+		BIP_RANGE(0.05),
+		UNI_RANGE(10),
+		UNI_RANGE(5),
+		UNI_RANGE(2),
+		UNI_RANGE(1),
+		UNI_RANGE(0.5),
+		UNI_RANGE(0.2),
+		UNI_RANGE(0.1)
+	}
 };
 EXPORT_SYMBOL_GPL(range_labpc_1200_ai);
 
-/* analog output ranges */
-#define AO_RANGE_IS_UNIPOLAR 0x1
 static const struct comedi_lrange range_labpc_ao = {
-	2,
-	{
-	 BIP_RANGE(5),
-	 UNI_RANGE(10),
-	 }
+	2, {
+		BIP_RANGE(5),
+		UNI_RANGE(10)
+	}
 };
 
 /* functions that do inb/outb and readb/writeb so we can use
@@ -420,413 +234,417 @@ static inline void labpc_writeb(unsigned int byte, unsigned long address)
 	writeb(byte, (void __iomem *)address);
 }
 
-static const struct labpc_board_struct labpc_boards[] = {
-	{
-	 .name = "lab-pc-1200",
-	 .ai_speed = 10000,
-	 .bustype = isa_bustype,
-	 .register_layout = labpc_1200_layout,
-	 .has_ao = 1,
-	 .ai_range_table = &range_labpc_1200_ai,
-	 .ai_range_code = labpc_1200_ai_gain_bits,
-	 .ai_range_is_unipolar = labpc_1200_is_unipolar,
-	 .ai_scan_up = 1,
-	 .memory_mapped_io = 0,
-	 },
+#if IS_ENABLED(CONFIG_COMEDI_NI_LABPC_ISA)
+static const struct labpc_boardinfo labpc_boards[] = {
 	{
-	 .name = "lab-pc-1200ai",
-	 .ai_speed = 10000,
-	 .bustype = isa_bustype,
-	 .register_layout = labpc_1200_layout,
-	 .has_ao = 0,
-	 .ai_range_table = &range_labpc_1200_ai,
-	 .ai_range_code = labpc_1200_ai_gain_bits,
-	 .ai_range_is_unipolar = labpc_1200_is_unipolar,
-	 .ai_scan_up = 1,
-	 .memory_mapped_io = 0,
-	 },
-	{
-	 .name = "lab-pc+",
-	 .ai_speed = 12000,
-	 .bustype = isa_bustype,
-	 .register_layout = labpc_plus_layout,
-	 .has_ao = 1,
-	 .ai_range_table = &range_labpc_plus_ai,
-	 .ai_range_code = labpc_plus_ai_gain_bits,
-	 .ai_range_is_unipolar = labpc_plus_is_unipolar,
-	 .ai_scan_up = 0,
-	 .memory_mapped_io = 0,
-	 },
-#ifdef CONFIG_COMEDI_PCI_DRIVERS
-	{
-	 .name = "pci-1200",
-	 .device_id = 0x161,
-	 .ai_speed = 10000,
-	 .bustype = pci_bustype,
-	 .register_layout = labpc_1200_layout,
-	 .has_ao = 1,
-	 .ai_range_table = &range_labpc_1200_ai,
-	 .ai_range_code = labpc_1200_ai_gain_bits,
-	 .ai_range_is_unipolar = labpc_1200_is_unipolar,
-	 .ai_scan_up = 1,
-	 .memory_mapped_io = 1,
-	 },
-/* dummy entry so pci board works when comedi_config is passed driver name */
-	{
-	 .name = DRV_NAME,
-	 .bustype = pci_bustype,
-	 },
-#endif
+		.name			= "lab-pc-1200",
+		.ai_speed		= 10000,
+		.register_layout	= labpc_1200_layout,
+		.has_ao			= 1,
+		.ai_range_table		= &range_labpc_1200_ai,
+		.ai_range_code		= labpc_1200_ai_gain_bits,
+		.ai_scan_up		= 1,
+	}, {
+		.name			= "lab-pc-1200ai",
+		.ai_speed		= 10000,
+		.register_layout	= labpc_1200_layout,
+		.ai_range_table		= &range_labpc_1200_ai,
+		.ai_range_code		= labpc_1200_ai_gain_bits,
+		.ai_scan_up		= 1,
+	}, {
+		.name			= "lab-pc+",
+		.ai_speed		= 12000,
+		.register_layout	= labpc_plus_layout,
+		.has_ao			= 1,
+		.ai_range_table		= &range_labpc_plus_ai,
+		.ai_range_code		= labpc_plus_ai_gain_bits,
+	},
 };
-
-/*
- * Useful for shorthand access to the particular board structure
- */
-#define thisboard ((struct labpc_board_struct *)dev->board_ptr)
+#endif
 
 /* size in bytes of dma buffer */
 static const int dma_buffer_size = 0xff00;
 /* 2 bytes per sample */
 static const int sample_size = 2;
 
-static inline int labpc_counter_load(struct comedi_device *dev,
-				     unsigned long base_address,
-				     unsigned int counter_number,
-				     unsigned int count, unsigned int mode)
+static int labpc_counter_load(struct comedi_device *dev,
+			      unsigned long base_address,
+			      unsigned int counter_number,
+			      unsigned int count, unsigned int mode)
 {
-	if (thisboard->memory_mapped_io)
+	const struct labpc_boardinfo *board = comedi_board(dev);
+
+	if (board->has_mmio)
 		return i8254_mm_load((void __iomem *)base_address, 0,
 				     counter_number, count, mode);
 	else
 		return i8254_load(base_address, 0, counter_number, count, mode);
 }
 
-int labpc_common_attach(struct comedi_device *dev, unsigned long iobase,
-			unsigned int irq, unsigned int dma_chan)
+static int labpc_counter_set_mode(struct comedi_device *dev,
+				  unsigned long base_address,
+				  unsigned int counter_number,
+				  unsigned int mode)
+{
+	const struct labpc_boardinfo *board = comedi_board(dev);
+
+	if (board->has_mmio)
+		return i8254_mm_set_mode((void __iomem *)base_address, 0,
+					 counter_number, mode);
+	else
+		return i8254_set_mode(base_address, 0, counter_number, mode);
+}
+
+static bool labpc_range_is_unipolar(struct comedi_subdevice *s,
+				    unsigned int range)
+{
+	return s->range_table->range[range].min >= 0;
+}
+
+static int labpc_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 {
 	struct labpc_private *devpriv = dev->private;
-	struct comedi_subdevice *s;
-	int i;
-	unsigned long isr_flags;
-#ifdef CONFIG_ISA_DMA_API
-	unsigned long dma_flags;
-#endif
-	short lsb, msb;
-	int ret;
+	unsigned long flags;
 
-	dev_info(dev->class_dev, "ni_labpc: %s\n", thisboard->name);
-	if (iobase == 0) {
-		dev_err(dev->class_dev, "io base address is zero!\n");
-		return -EINVAL;
-	}
-	/*  request io regions for isa boards */
-	if (thisboard->bustype == isa_bustype) {
-		/* check if io addresses are available */
-		if (!request_region(iobase, LABPC_SIZE, DRV_NAME)) {
-			dev_err(dev->class_dev, "I/O port conflict\n");
-			return -EIO;
-		}
-	}
-	dev->iobase = iobase;
+	spin_lock_irqsave(&dev->spinlock, flags);
+	devpriv->cmd2 &= ~(CMD2_SWTRIG | CMD2_HWTRIG | CMD2_PRETRIG);
+	devpriv->write_byte(devpriv->cmd2, dev->iobase + CMD2_REG);
+	spin_unlock_irqrestore(&dev->spinlock, flags);
 
-	if (thisboard->memory_mapped_io) {
-		devpriv->read_byte = labpc_readb;
-		devpriv->write_byte = labpc_writeb;
-	} else {
-		devpriv->read_byte = labpc_inb;
-		devpriv->write_byte = labpc_outb;
-	}
-	/* initialize board's command registers */
-	devpriv->write_byte(devpriv->command1_bits, dev->iobase + COMMAND1_REG);
-	devpriv->write_byte(devpriv->command2_bits, dev->iobase + COMMAND2_REG);
-	devpriv->write_byte(devpriv->command3_bits, dev->iobase + COMMAND3_REG);
-	devpriv->write_byte(devpriv->command4_bits, dev->iobase + COMMAND4_REG);
-	if (thisboard->register_layout == labpc_1200_layout) {
-		devpriv->write_byte(devpriv->command5_bits,
-				    dev->iobase + COMMAND5_REG);
-		devpriv->write_byte(devpriv->command6_bits,
-				    dev->iobase + COMMAND6_REG);
-	}
+	devpriv->cmd3 = 0;
+	devpriv->write_byte(devpriv->cmd3, dev->iobase + CMD3_REG);
 
-	/* grab our IRQ */
-	if (irq) {
-		isr_flags = 0;
-		if (thisboard->bustype == pci_bustype
-		    || thisboard->bustype == pcmcia_bustype)
-			isr_flags |= IRQF_SHARED;
-		if (request_irq(irq, labpc_interrupt, isr_flags,
-				DRV_NAME, dev)) {
-			dev_err(dev->class_dev, "unable to allocate irq %u\n",
-				irq);
-			return -EINVAL;
-		}
-	}
-	dev->irq = irq;
+	return 0;
+}
 
-#ifdef CONFIG_ISA_DMA_API
-	/* grab dma channel */
-	if (dma_chan > 3) {
-		dev_err(dev->class_dev, "invalid dma channel %u\n", dma_chan);
-		return -EINVAL;
-	} else if (dma_chan) {
-		/* allocate dma buffer */
-		devpriv->dma_buffer = kmalloc(dma_buffer_size,
-					      GFP_KERNEL | GFP_DMA);
-		if (devpriv->dma_buffer == NULL)
-			return -ENOMEM;
+static void labpc_ai_set_chan_and_gain(struct comedi_device *dev,
+				       enum scan_mode mode,
+				       unsigned int chan,
+				       unsigned int range,
+				       unsigned int aref)
+{
+	const struct labpc_boardinfo *board = comedi_board(dev);
+	struct labpc_private *devpriv = dev->private;
 
-		if (request_dma(dma_chan, DRV_NAME)) {
-			dev_err(dev->class_dev,
-				"failed to allocate dma channel %u\n",
-				dma_chan);
-			return -EINVAL;
-		}
-		devpriv->dma_chan = dma_chan;
-		dma_flags = claim_dma_lock();
-		disable_dma(devpriv->dma_chan);
-		set_dma_mode(devpriv->dma_chan, DMA_MODE_READ);
-		release_dma_lock(dma_flags);
-	}
-#endif
+	/* munge channel bits for differential/scan disabled mode */
+	if ((mode == MODE_SINGLE_CHAN || mode == MODE_SINGLE_CHAN_INTERVAL) &&
+	    aref == AREF_DIFF)
+		chan *= 2;
+	devpriv->cmd1 = CMD1_MA(chan);
+	devpriv->cmd1 |= board->ai_range_code[range];
 
-	dev->board_name = thisboard->name;
+	devpriv->write_byte(devpriv->cmd1, dev->iobase + CMD1_REG);
+}
 
-	ret = comedi_alloc_subdevices(dev, 5);
-	if (ret)
-		return ret;
+static void labpc_setup_cmd6_reg(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 enum scan_mode mode,
+				 enum transfer_type xfer,
+				 unsigned int range,
+				 unsigned int aref,
+				 bool ena_intr)
+{
+	const struct labpc_boardinfo *board = comedi_board(dev);
+	struct labpc_private *devpriv = dev->private;
 
-	/* analog input subdevice */
-	s = &dev->subdevices[0];
-	dev->read_subdev = s;
-	s->type = COMEDI_SUBD_AI;
-	s->subdev_flags =
-	    SDF_READABLE | SDF_GROUND | SDF_COMMON | SDF_DIFF | SDF_CMD_READ;
-	s->n_chan = 8;
-	s->len_chanlist = 8;
-	s->maxdata = (1 << 12) - 1;	/* 12 bit resolution */
-	s->range_table = thisboard->ai_range_table;
-	s->do_cmd = labpc_ai_cmd;
-	s->do_cmdtest = labpc_ai_cmdtest;
-	s->insn_read = labpc_ai_rinsn;
-	s->cancel = labpc_cancel;
+	if (board->register_layout != labpc_1200_layout)
+		return;
 
-	/* analog output */
-	s = &dev->subdevices[1];
-	if (thisboard->has_ao) {
-		/*
-		 * Could provide command support, except it only has a
-		 * one sample hardware buffer for analog output and no
-		 * underrun flag.
-		 */
-		s->type = COMEDI_SUBD_AO;
-		s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_GROUND;
-		s->n_chan = NUM_AO_CHAN;
-		s->maxdata = (1 << 12) - 1;	/*  12 bit resolution */
-		s->range_table = &range_labpc_ao;
-		s->insn_read = labpc_ao_rinsn;
-		s->insn_write = labpc_ao_winsn;
-		/* initialize analog outputs to a known value */
-		for (i = 0; i < s->n_chan; i++) {
-			devpriv->ao_value[i] = s->maxdata / 2;
-			lsb = devpriv->ao_value[i] & 0xff;
-			msb = (devpriv->ao_value[i] >> 8) & 0xff;
-			devpriv->write_byte(lsb, dev->iobase + DAC_LSB_REG(i));
-			devpriv->write_byte(msb, dev->iobase + DAC_MSB_REG(i));
-		}
-	} else {
-		s->type = COMEDI_SUBD_UNUSED;
-	}
+	/* reference inputs to ground or common? */
+	if (aref != AREF_GROUND)
+		devpriv->cmd6 |= CMD6_NRSE;
+	else
+		devpriv->cmd6 &= ~CMD6_NRSE;
 
-	/* 8255 dio */
-	s = &dev->subdevices[2];
-	/*  if board uses io memory we have to give a custom callback
-	 * function to the 8255 driver */
-	if (thisboard->memory_mapped_io)
-		subdev_8255_init(dev, s, labpc_dio_mem_callback,
-				 (unsigned long)(dev->iobase + DIO_BASE_REG));
+	/* bipolar or unipolar range? */
+	if (labpc_range_is_unipolar(s, range))
+		devpriv->cmd6 |= CMD6_ADCUNI;
 	else
-		subdev_8255_init(dev, s, NULL, dev->iobase + DIO_BASE_REG);
+		devpriv->cmd6 &= ~CMD6_ADCUNI;
 
-	/*  calibration subdevices for boards that have one */
-	s = &dev->subdevices[3];
-	if (thisboard->register_layout == labpc_1200_layout) {
-		s->type = COMEDI_SUBD_CALIB;
-		s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
-		s->n_chan = 16;
-		s->maxdata = 0xff;
-		s->insn_read = labpc_calib_read_insn;
-		s->insn_write = labpc_calib_write_insn;
+	/*  interrupt on fifo half full? */
+	if (xfer == fifo_half_full_transfer)
+		devpriv->cmd6 |= CMD6_HFINTEN;
+	else
+		devpriv->cmd6 &= ~CMD6_HFINTEN;
 
-		for (i = 0; i < s->n_chan; i++)
-			write_caldac(dev, i, s->maxdata / 2);
-	} else
-		s->type = COMEDI_SUBD_UNUSED;
+	/* enable interrupt on counter a1 terminal count? */
+	if (ena_intr)
+		devpriv->cmd6 |= CMD6_DQINTEN;
+	else
+		devpriv->cmd6 &= ~CMD6_DQINTEN;
 
-	/* EEPROM */
-	s = &dev->subdevices[4];
-	if (thisboard->register_layout == labpc_1200_layout) {
-		s->type = COMEDI_SUBD_MEMORY;
-		s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
-		s->n_chan = EEPROM_SIZE;
-		s->maxdata = 0xff;
-		s->insn_read = labpc_eeprom_read_insn;
-		s->insn_write = labpc_eeprom_write_insn;
-
-		for (i = 0; i < EEPROM_SIZE; i++)
-			devpriv->eeprom_data[i] = labpc_eeprom_read(dev, i);
-	} else
-		s->type = COMEDI_SUBD_UNUSED;
+	/* are we scanning up or down through channels? */
+	if (mode == MODE_MULT_CHAN_UP)
+		devpriv->cmd6 |= CMD6_SCANUP;
+	else
+		devpriv->cmd6 &= ~CMD6_SCANUP;
 
-	return 0;
+	devpriv->write_byte(devpriv->cmd6, dev->iobase + CMD6_REG);
 }
-EXPORT_SYMBOL_GPL(labpc_common_attach);
 
-static const struct labpc_board_struct *
-labpc_pci_find_boardinfo(struct pci_dev *pcidev)
+static unsigned int labpc_read_adc_fifo(struct comedi_device *dev)
+{
+	struct labpc_private *devpriv = dev->private;
+	unsigned int lsb = devpriv->read_byte(dev->iobase + ADC_FIFO_REG);
+	unsigned int msb = devpriv->read_byte(dev->iobase + ADC_FIFO_REG);
+
+	return (msb << 8) | lsb;
+}
+
+static void labpc_clear_adc_fifo(struct comedi_device *dev)
+{
+	struct labpc_private *devpriv = dev->private;
+
+	devpriv->write_byte(0x1, dev->iobase + ADC_FIFO_CLEAR_REG);
+	labpc_read_adc_fifo(dev);
+}
+
+static int labpc_ai_wait_for_data(struct comedi_device *dev,
+				  int timeout)
 {
-	unsigned int device_id = pcidev->device;
-	unsigned int n;
-
-	for (n = 0; n < ARRAY_SIZE(labpc_boards); n++) {
-		const struct labpc_board_struct *board = &labpc_boards[n];
-		if (board->bustype == pci_bustype &&
-		    board->device_id == device_id)
-			return board;
+	struct labpc_private *devpriv = dev->private;
+	int i;
+
+	for (i = 0; i < timeout; i++) {
+		devpriv->stat1 = devpriv->read_byte(dev->iobase + STAT1_REG);
+		if (devpriv->stat1 & STAT1_DAVAIL)
+			return 0;
+		udelay(1);
 	}
-	return NULL;
+	return -ETIME;
 }
 
-static int labpc_auto_attach(struct comedi_device *dev,
-				       unsigned long context_unused)
+static int labpc_ai_insn_read(struct comedi_device *dev,
+			      struct comedi_subdevice *s,
+			      struct comedi_insn *insn,
+			      unsigned int *data)
 {
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	struct labpc_private *devpriv;
-	unsigned long iobase;
-	unsigned int irq;
+	struct labpc_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int range = CR_RANGE(insn->chanspec);
+	unsigned int aref = CR_AREF(insn->chanspec);
 	int ret;
+	int i;
 
-	if (!IS_ENABLED(CONFIG_COMEDI_PCI_DRIVERS))
-		return -ENODEV;
+	/* disable timed conversions, interrupt generation and dma */
+	labpc_cancel(dev, s);
 
-	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
-	if (!devpriv)
-		return -ENOMEM;
-	dev->private = devpriv;
+	labpc_ai_set_chan_and_gain(dev, MODE_SINGLE_CHAN, chan, range, aref);
 
-	dev->board_ptr = labpc_pci_find_boardinfo(pcidev);
-	if (!dev->board_ptr)
-		return -ENODEV;
-	devpriv->mite = mite_alloc(pcidev);
-	if (!devpriv->mite)
-		return -ENOMEM;
-	ret = mite_setup(devpriv->mite);
-	if (ret < 0)
+	labpc_setup_cmd6_reg(dev, s, MODE_SINGLE_CHAN, fifo_not_empty_transfer,
+			     range, aref, false);
+
+	/* setup cmd4 register */
+	devpriv->cmd4 = 0;
+	devpriv->cmd4 |= CMD4_ECLKRCV;
+	/* single-ended/differential */
+	if (aref == AREF_DIFF)
+		devpriv->cmd4 |= CMD4_SEDIFF;
+	devpriv->write_byte(devpriv->cmd4, dev->iobase + CMD4_REG);
+
+	/* initialize pacer counter to prevent any problems */
+	ret = labpc_counter_set_mode(dev, dev->iobase + COUNTER_A_BASE_REG,
+				     0, I8254_MODE2);
+	if (ret)
 		return ret;
-	iobase = (unsigned long)devpriv->mite->daq_io_addr;
-	irq = mite_irq(devpriv->mite);
-	return labpc_common_attach(dev, iobase, irq, 0);
+
+	labpc_clear_adc_fifo(dev);
+
+	for (i = 0; i < insn->n; i++) {
+		/* trigger conversion */
+		devpriv->write_byte(0x1, dev->iobase + ADC_START_CONVERT_REG);
+
+		ret = labpc_ai_wait_for_data(dev, LABPC_ADC_TIMEOUT);
+		if (ret)
+			return ret;
+
+		data[i] = labpc_read_adc_fifo(dev);
+	}
+
+	return insn->n;
 }
 
-static int labpc_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+#ifdef CONFIG_ISA_DMA_API
+/* utility function that suggests a dma transfer size in bytes */
+static unsigned int labpc_suggest_transfer_size(const struct comedi_cmd *cmd)
 {
-	struct labpc_private *devpriv;
-	unsigned long iobase = 0;
-	unsigned int irq = 0;
-	unsigned int dma_chan = 0;
+	unsigned int size;
+	unsigned int freq;
 
-	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
-	if (!devpriv)
-		return -ENOMEM;
-	dev->private = devpriv;
+	if (cmd->convert_src == TRIG_TIMER)
+		freq = 1000000000 / cmd->convert_arg;
+	/* return some default value */
+	else
+		freq = 0xffffffff;
 
-	/* get base address, irq etc. based on bustype */
-	switch (thisboard->bustype) {
-	case isa_bustype:
-#ifdef CONFIG_ISA_DMA_API
-		iobase = it->options[0];
-		irq = it->options[1];
-		dma_chan = it->options[2];
-#else
-		dev_err(dev->class_dev,
-			"ni_labpc driver has not been built with ISA DMA support.\n");
-		return -EINVAL;
-#endif
-		break;
-	case pci_bustype:
-#ifdef CONFIG_COMEDI_PCI_DRIVERS
-		dev_err(dev->class_dev,
-			"manual configuration of PCI board '%s' is not supported\n",
-			thisboard->name);
-		return -EINVAL;
-#else
-		dev_err(dev->class_dev,
-			"ni_labpc driver has not been built with PCI support.\n");
-		return -EINVAL;
+	/* make buffer fill in no more than 1/3 second */
+	size = (freq / 3) * sample_size;
+
+	/* set a minimum and maximum size allowed */
+	if (size > dma_buffer_size)
+		size = dma_buffer_size - dma_buffer_size % sample_size;
+	else if (size < sample_size)
+		size = sample_size;
+
+	return size;
+}
 #endif
-		break;
-	default:
-		dev_err(dev->class_dev,
-			"ni_labpc: bug! couldn't determine board type\n");
-		return -EINVAL;
-		break;
-	}
 
-	return labpc_common_attach(dev, iobase, irq, dma_chan);
+static bool labpc_use_continuous_mode(const struct comedi_cmd *cmd,
+				      enum scan_mode mode)
+{
+	if (mode == MODE_SINGLE_CHAN || cmd->scan_begin_src == TRIG_FOLLOW)
+		return true;
+
+	return false;
 }
 
-void labpc_common_detach(struct comedi_device *dev)
+static unsigned int labpc_ai_convert_period(const struct comedi_cmd *cmd,
+					    enum scan_mode mode)
 {
-	struct labpc_private *devpriv = dev->private;
-	struct comedi_subdevice *s;
+	if (cmd->convert_src != TRIG_TIMER)
+		return 0;
+
+	if (mode == MODE_SINGLE_CHAN && cmd->scan_begin_src == TRIG_TIMER)
+		return cmd->scan_begin_arg;
 
-	if (!thisboard)
+	return cmd->convert_arg;
+}
+
+static void labpc_set_ai_convert_period(struct comedi_cmd *cmd,
+					enum scan_mode mode, unsigned int ns)
+{
+	if (cmd->convert_src != TRIG_TIMER)
 		return;
-	if (dev->subdevices) {
-		s = &dev->subdevices[2];
-		subdev_8255_cleanup(dev, s);
-	}
-#ifdef CONFIG_ISA_DMA_API
-	/* only free stuff if it has been allocated by _attach */
-	kfree(devpriv->dma_buffer);
-	if (devpriv->dma_chan)
-		free_dma(devpriv->dma_chan);
-#endif
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (thisboard->bustype == isa_bustype && dev->iobase)
-		release_region(dev->iobase, LABPC_SIZE);
-#ifdef CONFIG_COMEDI_PCI_DRIVERS
-	if (devpriv->mite) {
-		mite_unsetup(devpriv->mite);
-		mite_free(devpriv->mite);
-	}
-#endif
-};
-EXPORT_SYMBOL_GPL(labpc_common_detach);
 
-static void labpc_clear_adc_fifo(const struct comedi_device *dev)
+	if (mode == MODE_SINGLE_CHAN &&
+	    cmd->scan_begin_src == TRIG_TIMER) {
+		cmd->scan_begin_arg = ns;
+		if (cmd->convert_arg > cmd->scan_begin_arg)
+			cmd->convert_arg = cmd->scan_begin_arg;
+	} else
+		cmd->convert_arg = ns;
+}
+
+static unsigned int labpc_ai_scan_period(const struct comedi_cmd *cmd,
+					enum scan_mode mode)
 {
-	struct labpc_private *devpriv = dev->private;
+	if (cmd->scan_begin_src != TRIG_TIMER)
+		return 0;
 
-	devpriv->write_byte(0x1, dev->iobase + ADC_CLEAR_REG);
-	devpriv->read_byte(dev->iobase + ADC_FIFO_REG);
-	devpriv->read_byte(dev->iobase + ADC_FIFO_REG);
+	if (mode == MODE_SINGLE_CHAN && cmd->convert_src == TRIG_TIMER)
+		return 0;
+
+	return cmd->scan_begin_arg;
 }
 
-static int labpc_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
+static void labpc_set_ai_scan_period(struct comedi_cmd *cmd,
+				     enum scan_mode mode, unsigned int ns)
+{
+	if (cmd->scan_begin_src != TRIG_TIMER)
+		return;
+
+	if (mode == MODE_SINGLE_CHAN && cmd->convert_src == TRIG_TIMER)
+		return;
+
+	cmd->scan_begin_arg = ns;
+}
+
+/* figures out what counter values to use based on command */
+static void labpc_adc_timing(struct comedi_device *dev, struct comedi_cmd *cmd,
+			     enum scan_mode mode)
 {
 	struct labpc_private *devpriv = dev->private;
-	unsigned long flags;
+	/* max value for 16 bit counter in mode 2 */
+	const int max_counter_value = 0x10000;
+	/* min value for 16 bit counter in mode 2 */
+	const int min_counter_value = 2;
+	unsigned int base_period;
+	unsigned int scan_period;
+	unsigned int convert_period;
 
-	spin_lock_irqsave(&dev->spinlock, flags);
-	devpriv->command2_bits &= ~SWTRIG_BIT & ~HWTRIG_BIT & ~PRETRIG_BIT;
-	devpriv->write_byte(devpriv->command2_bits, dev->iobase + COMMAND2_REG);
-	spin_unlock_irqrestore(&dev->spinlock, flags);
+	/*
+	 * if both convert and scan triggers are TRIG_TIMER, then they
+	 * both rely on counter b0
+	 */
+	convert_period = labpc_ai_convert_period(cmd, mode);
+	scan_period = labpc_ai_scan_period(cmd, mode);
+	if (convert_period && scan_period) {
+		/*
+		 * pick the lowest b0 divisor value we can (for maximum input
+		 * clock speed on convert and scan counters)
+		 */
+		devpriv->divisor_b0 = (scan_period - 1) /
+		    (LABPC_TIMER_BASE * max_counter_value) + 1;
+		if (devpriv->divisor_b0 < min_counter_value)
+			devpriv->divisor_b0 = min_counter_value;
+		if (devpriv->divisor_b0 > max_counter_value)
+			devpriv->divisor_b0 = max_counter_value;
 
-	devpriv->command3_bits = 0;
-	devpriv->write_byte(devpriv->command3_bits, dev->iobase + COMMAND3_REG);
+		base_period = LABPC_TIMER_BASE * devpriv->divisor_b0;
 
-	return 0;
+		/*  set a0 for conversion frequency and b1 for scan frequency */
+		switch (cmd->flags & TRIG_ROUND_MASK) {
+		default:
+		case TRIG_ROUND_NEAREST:
+			devpriv->divisor_a0 =
+			    (convert_period + (base_period / 2)) / base_period;
+			devpriv->divisor_b1 =
+			    (scan_period + (base_period / 2)) / base_period;
+			break;
+		case TRIG_ROUND_UP:
+			devpriv->divisor_a0 =
+			    (convert_period + (base_period - 1)) / base_period;
+			devpriv->divisor_b1 =
+			    (scan_period + (base_period - 1)) / base_period;
+			break;
+		case TRIG_ROUND_DOWN:
+			devpriv->divisor_a0 = convert_period / base_period;
+			devpriv->divisor_b1 = scan_period / base_period;
+			break;
+		}
+		/*  make sure a0 and b1 values are acceptable */
+		if (devpriv->divisor_a0 < min_counter_value)
+			devpriv->divisor_a0 = min_counter_value;
+		if (devpriv->divisor_a0 > max_counter_value)
+			devpriv->divisor_a0 = max_counter_value;
+		if (devpriv->divisor_b1 < min_counter_value)
+			devpriv->divisor_b1 = min_counter_value;
+		if (devpriv->divisor_b1 > max_counter_value)
+			devpriv->divisor_b1 = max_counter_value;
+		/*  write corrected timings to command */
+		labpc_set_ai_convert_period(cmd, mode,
+					    base_period * devpriv->divisor_a0);
+		labpc_set_ai_scan_period(cmd, mode,
+					 base_period * devpriv->divisor_b1);
+		/*
+		 * if only one TRIG_TIMER is used, we can employ the generic
+		 * cascaded timing functions
+		 */
+	} else if (scan_period) {
+		/*
+		 * calculate cascaded counter values
+		 * that give desired scan timing
+		 */
+		i8253_cascade_ns_to_timer_2div(LABPC_TIMER_BASE,
+					       &(devpriv->divisor_b1),
+					       &(devpriv->divisor_b0),
+					       &scan_period,
+					       cmd->flags & TRIG_ROUND_MASK);
+		labpc_set_ai_scan_period(cmd, mode, scan_period);
+	} else if (convert_period) {
+		/*
+		 * calculate cascaded counter values
+		 * that give desired conversion timing
+		 */
+		i8253_cascade_ns_to_timer_2div(LABPC_TIMER_BASE,
+					       &(devpriv->divisor_a0),
+					       &(devpriv->divisor_b0),
+					       &convert_period,
+					       cmd->flags & TRIG_ROUND_MASK);
+		labpc_set_ai_convert_period(cmd, mode, convert_period);
+	}
 }
 
 static enum scan_mode labpc_ai_scan_mode(const struct comedi_cmd *cmd)
@@ -923,72 +741,10 @@ static int labpc_ai_chanlist_invalid(const struct comedi_device *dev,
 	return 0;
 }
 
-static int labpc_use_continuous_mode(const struct comedi_cmd *cmd,
-				     enum scan_mode mode)
-{
-	if (mode == MODE_SINGLE_CHAN)
-		return 1;
-
-	if (cmd->scan_begin_src == TRIG_FOLLOW)
-		return 1;
-
-	return 0;
-}
-
-static unsigned int labpc_ai_convert_period(const struct comedi_cmd *cmd,
-					    enum scan_mode mode)
-{
-	if (cmd->convert_src != TRIG_TIMER)
-		return 0;
-
-	if (mode == MODE_SINGLE_CHAN && cmd->scan_begin_src == TRIG_TIMER)
-		return cmd->scan_begin_arg;
-
-	return cmd->convert_arg;
-}
-
-static void labpc_set_ai_convert_period(struct comedi_cmd *cmd,
-					enum scan_mode mode, unsigned int ns)
-{
-	if (cmd->convert_src != TRIG_TIMER)
-		return;
-
-	if (mode == MODE_SINGLE_CHAN &&
-	    cmd->scan_begin_src == TRIG_TIMER) {
-		cmd->scan_begin_arg = ns;
-		if (cmd->convert_arg > cmd->scan_begin_arg)
-			cmd->convert_arg = cmd->scan_begin_arg;
-	} else
-		cmd->convert_arg = ns;
-}
-
-static unsigned int labpc_ai_scan_period(const struct comedi_cmd *cmd,
-					enum scan_mode mode)
-{
-	if (cmd->scan_begin_src != TRIG_TIMER)
-		return 0;
-
-	if (mode == MODE_SINGLE_CHAN && cmd->convert_src == TRIG_TIMER)
-		return 0;
-
-	return cmd->scan_begin_arg;
-}
-
-static void labpc_set_ai_scan_period(struct comedi_cmd *cmd,
-				     enum scan_mode mode, unsigned int ns)
-{
-	if (cmd->scan_begin_src != TRIG_TIMER)
-		return;
-
-	if (mode == MODE_SINGLE_CHAN && cmd->convert_src == TRIG_TIMER)
-		return;
-
-	cmd->scan_begin_arg = ns;
-}
-
 static int labpc_ai_cmdtest(struct comedi_device *dev,
 			    struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
+	const struct labpc_boardinfo *board = comedi_board(dev);
 	int err = 0;
 	int tmp, tmp2;
 	unsigned int stop_mask;
@@ -1003,7 +759,7 @@ static int labpc_ai_cmdtest(struct comedi_device *dev,
 	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
 
 	stop_mask = TRIG_COUNT | TRIG_NONE;
-	if (thisboard->register_layout == labpc_1200_layout)
+	if (board->register_layout == labpc_1200_layout)
 		stop_mask |= TRIG_EXT;
 	err |= cfc_check_trigger_src(&cmd->stop_src, stop_mask);
 
@@ -1037,7 +793,7 @@ static int labpc_ai_cmdtest(struct comedi_device *dev,
 
 	if (cmd->convert_src == TRIG_TIMER)
 		err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
-						 thisboard->ai_speed);
+						 board->ai_speed);
 
 	/* make sure scan timing is not too fast */
 	if (cmd->scan_begin_src == TRIG_TIMER) {
@@ -1045,7 +801,7 @@ static int labpc_ai_cmdtest(struct comedi_device *dev,
 			err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
 					cmd->convert_arg * cmd->chanlist_len);
 		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-				thisboard->ai_speed * cmd->chanlist_len);
+				board->ai_speed * cmd->chanlist_len);
 	}
 
 	switch (cmd->stop_src) {
@@ -1086,34 +842,23 @@ static int labpc_ai_cmdtest(struct comedi_device *dev,
 
 static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
+	const struct labpc_boardinfo *board = comedi_board(dev);
 	struct labpc_private *devpriv = dev->private;
-	int channel, range, aref;
-#ifdef CONFIG_ISA_DMA_API
-	unsigned long irq_flags;
-#endif
-	int ret;
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
+	enum scan_mode mode = labpc_ai_scan_mode(cmd);
+	unsigned int chanspec = (mode == MODE_MULT_CHAN_UP)
+				? cmd->chanlist[cmd->chanlist_len - 1]
+				: cmd->chanlist[0];
+	unsigned int chan = CR_CHAN(chanspec);
+	unsigned int range = CR_RANGE(chanspec);
+	unsigned int aref = CR_AREF(chanspec);
 	enum transfer_type xfer;
-	enum scan_mode mode;
 	unsigned long flags;
-
-	if (!dev->irq) {
-		comedi_error(dev, "no irq assigned, cannot perform command");
-		return -1;
-	}
-
-	range = CR_RANGE(cmd->chanlist[0]);
-	aref = CR_AREF(cmd->chanlist[0]);
+	int ret;
 
 	/* make sure board is disabled before setting up acquisition */
-	spin_lock_irqsave(&dev->spinlock, flags);
-	devpriv->command2_bits &= ~SWTRIG_BIT & ~HWTRIG_BIT & ~PRETRIG_BIT;
-	devpriv->write_byte(devpriv->command2_bits, dev->iobase + COMMAND2_REG);
-	spin_unlock_irqrestore(&dev->spinlock, flags);
-
-	devpriv->command3_bits = 0;
-	devpriv->write_byte(devpriv->command3_bits, dev->iobase + COMMAND3_REG);
+	labpc_cancel(dev, s);
 
 	/*  initialize software conversion count */
 	if (cmd->stop_src == TRIG_COUNT)
@@ -1126,17 +871,17 @@ static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		 * (pc+ manual says this is minimum allowed) using mode 0
 		 */
 		ret = labpc_counter_load(dev, dev->iobase + COUNTER_A_BASE_REG,
-					 1, 3, 0);
-		if (ret < 0) {
-			comedi_error(dev, "error loading counter a1");
-			return -1;
-		}
-	} else			/*
-				 * otherwise, just put a1 in mode 0
-				 * with no count to set its output low
-				 */
-		devpriv->write_byte(INIT_A1_BITS,
-				    dev->iobase + COUNTER_A_CONTROL_REG);
+					 1, 3, I8254_MODE0);
+	} else	{
+		/* just put counter a1 in mode 0 to set its output low */
+		ret = labpc_counter_set_mode(dev,
+					     dev->iobase + COUNTER_A_BASE_REG,
+					     1, I8254_MODE0);
+	}
+	if (ret) {
+		comedi_error(dev, "error loading counter a1");
+		return ret;
+	}
 
 #ifdef CONFIG_ISA_DMA_API
 	/*  figure out what method we will use to transfer data */
@@ -1145,14 +890,12 @@ static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		 * dma unsafe at RT priority,
 		 * and too much setup time for TRIG_WAKE_EOS for
 		 */
-	    (cmd->flags & (TRIG_WAKE_EOS | TRIG_RT)) == 0 &&
-	    /*  only available on the isa boards */
-	    thisboard->bustype == isa_bustype) {
+	    (cmd->flags & (TRIG_WAKE_EOS | TRIG_RT)) == 0) {
 		xfer = isa_dma_transfer;
 		/* pc-plus has no fifo-half full interrupt */
 	} else
 #endif
-	if (thisboard->register_layout == labpc_1200_layout &&
+	if (board->register_layout == labpc_1200_layout &&
 		   /*  wake-end-of-scan should interrupt on fifo not empty */
 		   (cmd->flags & TRIG_WAKE_EOS) == 0 &&
 		   /*  make sure we are taking more than just a few points */
@@ -1161,76 +904,34 @@ static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	} else
 		xfer = fifo_not_empty_transfer;
 	devpriv->current_transfer = xfer;
-	mode = labpc_ai_scan_mode(cmd);
 
-	/*  setup command6 register for 1200 boards */
-	if (thisboard->register_layout == labpc_1200_layout) {
-		/*  reference inputs to ground or common? */
-		if (aref != AREF_GROUND)
-			devpriv->command6_bits |= ADC_COMMON_BIT;
-		else
-			devpriv->command6_bits &= ~ADC_COMMON_BIT;
-		/*  bipolar or unipolar range? */
-		if (thisboard->ai_range_is_unipolar[range])
-			devpriv->command6_bits |= ADC_UNIP_BIT;
-		else
-			devpriv->command6_bits &= ~ADC_UNIP_BIT;
-		/*  interrupt on fifo half full? */
-		if (xfer == fifo_half_full_transfer)
-			devpriv->command6_bits |= ADC_FHF_INTR_EN_BIT;
-		else
-			devpriv->command6_bits &= ~ADC_FHF_INTR_EN_BIT;
-		/*  enable interrupt on counter a1 terminal count? */
-		if (cmd->stop_src == TRIG_EXT)
-			devpriv->command6_bits |= A1_INTR_EN_BIT;
-		else
-			devpriv->command6_bits &= ~A1_INTR_EN_BIT;
-		/*  are we scanning up or down through channels? */
-		if (mode == MODE_MULT_CHAN_UP)
-			devpriv->command6_bits |= ADC_SCAN_UP_BIT;
-		else
-			devpriv->command6_bits &= ~ADC_SCAN_UP_BIT;
-		/*  write to register */
-		devpriv->write_byte(devpriv->command6_bits,
-				    dev->iobase + COMMAND6_REG);
-	}
+	labpc_ai_set_chan_and_gain(dev, mode, chan, range, aref);
+
+	labpc_setup_cmd6_reg(dev, s, mode, xfer, range, aref,
+			     (cmd->stop_src == TRIG_EXT));
 
-	/* setup channel list, etc (command1 register) */
-	devpriv->command1_bits = 0;
-	if (mode == MODE_MULT_CHAN_UP)
-		channel = CR_CHAN(cmd->chanlist[cmd->chanlist_len - 1]);
-	else
-		channel = CR_CHAN(cmd->chanlist[0]);
-	/* munge channel bits for differential / scan disabled mode */
-	if ((mode == MODE_SINGLE_CHAN || mode == MODE_SINGLE_CHAN_INTERVAL) &&
-	    aref == AREF_DIFF)
-		channel *= 2;
-	devpriv->command1_bits |= ADC_CHAN_BITS(channel);
-	devpriv->command1_bits |= thisboard->ai_range_code[range];
-	devpriv->write_byte(devpriv->command1_bits, dev->iobase + COMMAND1_REG);
 	/* manual says to set scan enable bit on second pass */
 	if (mode == MODE_MULT_CHAN_UP || mode == MODE_MULT_CHAN_DOWN) {
-		devpriv->command1_bits |= ADC_SCAN_EN_BIT;
+		devpriv->cmd1 |= CMD1_SCANEN;
 		/* need a brief delay before enabling scan, or scan
 		 * list will get screwed when you switch
 		 * between scan up to scan down mode - dunno why */
 		udelay(1);
-		devpriv->write_byte(devpriv->command1_bits,
-				    dev->iobase + COMMAND1_REG);
+		devpriv->write_byte(devpriv->cmd1, dev->iobase + CMD1_REG);
 	}
 
 	devpriv->write_byte(cmd->chanlist_len,
 			    dev->iobase + INTERVAL_COUNT_REG);
 	/*  load count */
-	devpriv->write_byte(INTERVAL_LOAD_BITS,
-			    dev->iobase + INTERVAL_LOAD_REG);
+	devpriv->write_byte(0x1, dev->iobase + INTERVAL_STROBE_REG);
 
-	if (cmd->convert_src == TRIG_TIMER || cmd->scan_begin_src == TRIG_TIMER) {
+	if (cmd->convert_src == TRIG_TIMER ||
+	    cmd->scan_begin_src == TRIG_TIMER) {
 		/*  set up pacing */
 		labpc_adc_timing(dev, cmd, mode);
 		/*  load counter b0 in mode 3 */
 		ret = labpc_counter_load(dev, dev->iobase + COUNTER_B_BASE_REG,
-					 0, devpriv->divisor_b0, 3);
+					 0, devpriv->divisor_b0, I8254_MODE3);
 		if (ret < 0) {
 			comedi_error(dev, "error loading counter b0");
 			return -1;
@@ -1240,20 +941,23 @@ static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	if (labpc_ai_convert_period(cmd, mode)) {
 		/*  load counter a0 in mode 2 */
 		ret = labpc_counter_load(dev, dev->iobase + COUNTER_A_BASE_REG,
-					 0, devpriv->divisor_a0, 2);
-		if (ret < 0) {
-			comedi_error(dev, "error loading counter a0");
-			return -1;
-		}
-	} else
-		devpriv->write_byte(INIT_A0_BITS,
-				    dev->iobase + COUNTER_A_CONTROL_REG);
+					 0, devpriv->divisor_a0, I8254_MODE2);
+	} else {
+		/* initialize pacer counter to prevent any problems */
+		ret = labpc_counter_set_mode(dev,
+					     dev->iobase + COUNTER_A_BASE_REG,
+					     0, I8254_MODE2);
+	}
+	if (ret) {
+		comedi_error(dev, "error loading counter a0");
+		return ret;
+	}
 
 	/*  set up scan pacing */
 	if (labpc_ai_scan_period(cmd, mode)) {
 		/*  load counter b1 in mode 2 */
 		ret = labpc_counter_load(dev, dev->iobase + COUNTER_B_BASE_REG,
-					 1, devpriv->divisor_b1, 2);
+					 1, devpriv->divisor_b1, I8254_MODE2);
 		if (ret < 0) {
 			comedi_error(dev, "error loading counter b1");
 			return -1;
@@ -1265,6 +969,8 @@ static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 #ifdef CONFIG_ISA_DMA_API
 	/*  set up dma transfer */
 	if (xfer == isa_dma_transfer) {
+		unsigned long irq_flags;
+
 		irq_flags = claim_dma_lock();
 		disable_dma(devpriv->dma_chan);
 		/* clear flip-flop to make sure 2-byte registers for
@@ -1283,197 +989,54 @@ static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		enable_dma(devpriv->dma_chan);
 		release_dma_lock(irq_flags);
 		/*  enable board's dma */
-		devpriv->command3_bits |= DMA_EN_BIT | DMATC_INTR_EN_BIT;
+		devpriv->cmd3 |= (CMD3_DMAEN | CMD3_DMATCINTEN);
 	} else
-		devpriv->command3_bits &= ~DMA_EN_BIT & ~DMATC_INTR_EN_BIT;
+		devpriv->cmd3 &= ~(CMD3_DMAEN | CMD3_DMATCINTEN);
 #endif
 
 	/*  enable error interrupts */
-	devpriv->command3_bits |= ERR_INTR_EN_BIT;
+	devpriv->cmd3 |= CMD3_ERRINTEN;
 	/*  enable fifo not empty interrupt? */
 	if (xfer == fifo_not_empty_transfer)
-		devpriv->command3_bits |= ADC_FNE_INTR_EN_BIT;
+		devpriv->cmd3 |= CMD3_FIFOINTEN;
 	else
-		devpriv->command3_bits &= ~ADC_FNE_INTR_EN_BIT;
-	devpriv->write_byte(devpriv->command3_bits, dev->iobase + COMMAND3_REG);
+		devpriv->cmd3 &= ~CMD3_FIFOINTEN;
+	devpriv->write_byte(devpriv->cmd3, dev->iobase + CMD3_REG);
 
-	/*  setup any external triggering/pacing (command4 register) */
-	devpriv->command4_bits = 0;
+	/*  setup any external triggering/pacing (cmd4 register) */
+	devpriv->cmd4 = 0;
 	if (cmd->convert_src != TRIG_EXT)
-		devpriv->command4_bits |= EXT_CONVERT_DISABLE_BIT;
+		devpriv->cmd4 |= CMD4_ECLKRCV;
 	/* XXX should discard first scan when using interval scanning
 	 * since manual says it is not synced with scan clock */
-	if (labpc_use_continuous_mode(cmd, mode) == 0) {
-		devpriv->command4_bits |= INTERVAL_SCAN_EN_BIT;
+	if (!labpc_use_continuous_mode(cmd, mode)) {
+		devpriv->cmd4 |= CMD4_INTSCAN;
 		if (cmd->scan_begin_src == TRIG_EXT)
-			devpriv->command4_bits |= EXT_SCAN_EN_BIT;
+			devpriv->cmd4 |= CMD4_EOIRCV;
 	}
 	/*  single-ended/differential */
 	if (aref == AREF_DIFF)
-		devpriv->command4_bits |= ADC_DIFF_BIT;
-	devpriv->write_byte(devpriv->command4_bits, dev->iobase + COMMAND4_REG);
+		devpriv->cmd4 |= CMD4_SEDIFF;
+	devpriv->write_byte(devpriv->cmd4, dev->iobase + CMD4_REG);
 
 	/*  startup acquisition */
 
-	/*  command2 reg */
-	/*  use 2 cascaded counters for pacing */
 	spin_lock_irqsave(&dev->spinlock, flags);
-	devpriv->command2_bits |= CASCADE_BIT;
-	switch (cmd->start_src) {
-	case TRIG_EXT:
-		devpriv->command2_bits |= HWTRIG_BIT;
-		devpriv->command2_bits &= ~PRETRIG_BIT & ~SWTRIG_BIT;
-		break;
-	case TRIG_NOW:
-		devpriv->command2_bits |= SWTRIG_BIT;
-		devpriv->command2_bits &= ~PRETRIG_BIT & ~HWTRIG_BIT;
-		break;
-	default:
-		comedi_error(dev, "bug with start_src");
-		spin_unlock_irqrestore(&dev->spinlock, flags);
-		return -1;
-		break;
-	}
-	switch (cmd->stop_src) {
-	case TRIG_EXT:
-		devpriv->command2_bits |= HWTRIG_BIT | PRETRIG_BIT;
-		break;
-	case TRIG_COUNT:
-	case TRIG_NONE:
-		break;
-	default:
-		comedi_error(dev, "bug with stop_src");
-		spin_unlock_irqrestore(&dev->spinlock, flags);
-		return -1;
-	}
-	devpriv->write_byte(devpriv->command2_bits, dev->iobase + COMMAND2_REG);
-	spin_unlock_irqrestore(&dev->spinlock, flags);
-
-	return 0;
-}
-
-/* interrupt service routine */
-static irqreturn_t labpc_interrupt(int irq, void *d)
-{
-	struct comedi_device *dev = d;
-	struct labpc_private *devpriv = dev->private;
-	struct comedi_subdevice *s = dev->read_subdev;
-	struct comedi_async *async;
-	struct comedi_cmd *cmd;
-
-	if (dev->attached == 0) {
-		comedi_error(dev, "premature interrupt");
-		return IRQ_HANDLED;
-	}
-
-	async = s->async;
-	cmd = &async->cmd;
-	async->events = 0;
-
-	/* read board status */
-	devpriv->status1_bits = devpriv->read_byte(dev->iobase + STATUS1_REG);
-	if (thisboard->register_layout == labpc_1200_layout)
-		devpriv->status2_bits =
-		    devpriv->read_byte(dev->iobase + STATUS2_REG);
-
-	if ((devpriv->status1_bits & (DMATC_BIT | TIMER_BIT | OVERFLOW_BIT |
-				      OVERRUN_BIT | DATA_AVAIL_BIT)) == 0
-	    && (devpriv->status2_bits & A1_TC_BIT) == 0
-	    && (devpriv->status2_bits & FNHF_BIT)) {
-		return IRQ_NONE;
-	}
-
-	if (devpriv->status1_bits & OVERRUN_BIT) {
-		/* clear error interrupt */
-		devpriv->write_byte(0x1, dev->iobase + ADC_CLEAR_REG);
-		async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
-		comedi_event(dev, s);
-		comedi_error(dev, "overrun");
-		return IRQ_HANDLED;
-	}
-
-#ifdef CONFIG_ISA_DMA_API
-	if (devpriv->current_transfer == isa_dma_transfer) {
-		/*
-		 * if a dma terminal count of external stop trigger
-		 * has occurred
-		 */
-		if (devpriv->status1_bits & DMATC_BIT ||
-		    (thisboard->register_layout == labpc_1200_layout
-		     && devpriv->status2_bits & A1_TC_BIT)) {
-			handle_isa_dma(dev);
-		}
-	} else
-#endif
-		labpc_drain_fifo(dev);
-
-	if (devpriv->status1_bits & TIMER_BIT) {
-		comedi_error(dev, "handled timer interrupt?");
-		/*  clear it */
-		devpriv->write_byte(0x1, dev->iobase + TIMER_CLEAR_REG);
-	}
-
-	if (devpriv->status1_bits & OVERFLOW_BIT) {
-		/*  clear error interrupt */
-		devpriv->write_byte(0x1, dev->iobase + ADC_CLEAR_REG);
-		async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
-		comedi_event(dev, s);
-		comedi_error(dev, "overflow");
-		return IRQ_HANDLED;
-	}
-	/*  handle external stop trigger */
-	if (cmd->stop_src == TRIG_EXT) {
-		if (devpriv->status2_bits & A1_TC_BIT) {
-			labpc_drain_dregs(dev);
-			labpc_cancel(dev, s);
-			async->events |= COMEDI_CB_EOA;
-		}
-	}
-
-	/* TRIG_COUNT end of acquisition */
-	if (cmd->stop_src == TRIG_COUNT) {
-		if (devpriv->count == 0) {
-			labpc_cancel(dev, s);
-			async->events |= COMEDI_CB_EOA;
-		}
-	}
 
-	comedi_event(dev, s);
-	return IRQ_HANDLED;
-}
+	/* use 2 cascaded counters for pacing */
+	devpriv->cmd2 |= CMD2_TBSEL;
 
-/* read all available samples from ai fifo */
-static int labpc_drain_fifo(struct comedi_device *dev)
-{
-	struct labpc_private *devpriv = dev->private;
-	unsigned int lsb, msb;
-	short data;
-	struct comedi_async *async = dev->read_subdev->async;
-	const int timeout = 10000;
-	unsigned int i;
+	devpriv->cmd2 &= ~(CMD2_SWTRIG | CMD2_HWTRIG | CMD2_PRETRIG);
+	if (cmd->start_src == TRIG_EXT)
+		devpriv->cmd2 |= CMD2_HWTRIG;
+	else
+		devpriv->cmd2 |= CMD2_SWTRIG;
+	if (cmd->stop_src == TRIG_EXT)
+		devpriv->cmd2 |= (CMD2_HWTRIG | CMD2_PRETRIG);
 
-	devpriv->status1_bits = devpriv->read_byte(dev->iobase + STATUS1_REG);
+	devpriv->write_byte(devpriv->cmd2, dev->iobase + CMD2_REG);
 
-	for (i = 0; (devpriv->status1_bits & DATA_AVAIL_BIT) && i < timeout;
-	     i++) {
-		/*  quit if we have all the data we want */
-		if (async->cmd.stop_src == TRIG_COUNT) {
-			if (devpriv->count == 0)
-				break;
-			devpriv->count--;
-		}
-		lsb = devpriv->read_byte(dev->iobase + ADC_FIFO_REG);
-		msb = devpriv->read_byte(dev->iobase + ADC_FIFO_REG);
-		data = (msb << 8) | lsb;
-		cfc_write_to_buffer(dev->read_subdev, data);
-		devpriv->status1_bits =
-		    devpriv->read_byte(dev->iobase + STATUS1_REG);
-	}
-	if (i == timeout) {
-		comedi_error(dev, "ai timeout, fifo never empties");
-		async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
-		return -1;
-	}
+	spin_unlock_irqrestore(&dev->spinlock, flags);
 
 	return 0;
 }
@@ -1489,7 +1052,7 @@ static void labpc_drain_dma(struct comedi_device *dev)
 	unsigned int max_points, num_points, residue, leftover;
 	int i;
 
-	status = devpriv->status1_bits;
+	status = devpriv->stat1;
 
 	flags = claim_dma_lock();
 	disable_dma(devpriv->dma_chan);
@@ -1546,6 +1109,38 @@ static void handle_isa_dma(struct comedi_device *dev)
 }
 #endif
 
+/* read all available samples from ai fifo */
+static int labpc_drain_fifo(struct comedi_device *dev)
+{
+	struct labpc_private *devpriv = dev->private;
+	short data;
+	struct comedi_async *async = dev->read_subdev->async;
+	const int timeout = 10000;
+	unsigned int i;
+
+	devpriv->stat1 = devpriv->read_byte(dev->iobase + STAT1_REG);
+
+	for (i = 0; (devpriv->stat1 & STAT1_DAVAIL) && i < timeout;
+	     i++) {
+		/*  quit if we have all the data we want */
+		if (async->cmd.stop_src == TRIG_COUNT) {
+			if (devpriv->count == 0)
+				break;
+			devpriv->count--;
+		}
+		data = labpc_read_adc_fifo(dev);
+		cfc_write_to_buffer(dev->read_subdev, data);
+		devpriv->stat1 = devpriv->read_byte(dev->iobase + STAT1_REG);
+	}
+	if (i == timeout) {
+		comedi_error(dev, "ai timeout, fifo never empties");
+		async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
+		return -1;
+	}
+
+	return 0;
+}
+
 /* makes sure all data acquired by board is transferred to comedi (used
  * when acquisition is terminated by stop_src == TRIG_EXT). */
 static void labpc_drain_dregs(struct comedi_device *dev)
@@ -1560,99 +1155,102 @@ static void labpc_drain_dregs(struct comedi_device *dev)
 	labpc_drain_fifo(dev);
 }
 
-static int labpc_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data)
+/* interrupt service routine */
+static irqreturn_t labpc_interrupt(int irq, void *d)
 {
+	struct comedi_device *dev = d;
+	const struct labpc_boardinfo *board = comedi_board(dev);
 	struct labpc_private *devpriv = dev->private;
-	int i, n;
-	int chan, range;
-	int lsb, msb;
-	int timeout = 1000;
-	unsigned long flags;
+	struct comedi_subdevice *s = dev->read_subdev;
+	struct comedi_async *async;
+	struct comedi_cmd *cmd;
 
-	/*  disable timed conversions */
-	spin_lock_irqsave(&dev->spinlock, flags);
-	devpriv->command2_bits &= ~SWTRIG_BIT & ~HWTRIG_BIT & ~PRETRIG_BIT;
-	devpriv->write_byte(devpriv->command2_bits, dev->iobase + COMMAND2_REG);
-	spin_unlock_irqrestore(&dev->spinlock, flags);
+	if (!dev->attached) {
+		comedi_error(dev, "premature interrupt");
+		return IRQ_HANDLED;
+	}
 
-	/*  disable interrupt generation and dma */
-	devpriv->command3_bits = 0;
-	devpriv->write_byte(devpriv->command3_bits, dev->iobase + COMMAND3_REG);
+	async = s->async;
+	cmd = &async->cmd;
+	async->events = 0;
 
-	/* set gain and channel */
-	devpriv->command1_bits = 0;
-	chan = CR_CHAN(insn->chanspec);
-	range = CR_RANGE(insn->chanspec);
-	devpriv->command1_bits |= thisboard->ai_range_code[range];
-	/* munge channel bits for differential/scan disabled mode */
-	if (CR_AREF(insn->chanspec) == AREF_DIFF)
-		chan *= 2;
-	devpriv->command1_bits |= ADC_CHAN_BITS(chan);
-	devpriv->write_byte(devpriv->command1_bits, dev->iobase + COMMAND1_REG);
-
-	/* setup command6 register for 1200 boards */
-	if (thisboard->register_layout == labpc_1200_layout) {
-		/*  reference inputs to ground or common? */
-		if (CR_AREF(insn->chanspec) != AREF_GROUND)
-			devpriv->command6_bits |= ADC_COMMON_BIT;
-		else
-			devpriv->command6_bits &= ~ADC_COMMON_BIT;
-		/* bipolar or unipolar range? */
-		if (thisboard->ai_range_is_unipolar[range])
-			devpriv->command6_bits |= ADC_UNIP_BIT;
-		else
-			devpriv->command6_bits &= ~ADC_UNIP_BIT;
-		/* don't interrupt on fifo half full */
-		devpriv->command6_bits &= ~ADC_FHF_INTR_EN_BIT;
-		/* don't enable interrupt on counter a1 terminal count? */
-		devpriv->command6_bits &= ~A1_INTR_EN_BIT;
-		/* write to register */
-		devpriv->write_byte(devpriv->command6_bits,
-				    dev->iobase + COMMAND6_REG);
+	/* read board status */
+	devpriv->stat1 = devpriv->read_byte(dev->iobase + STAT1_REG);
+	if (board->register_layout == labpc_1200_layout)
+		devpriv->stat2 = devpriv->read_byte(dev->iobase + STAT2_REG);
+
+	if ((devpriv->stat1 & (STAT1_GATA0 | STAT1_CNTINT | STAT1_OVERFLOW |
+			       STAT1_OVERRUN | STAT1_DAVAIL)) == 0
+	    && (devpriv->stat2 & STAT2_OUTA1) == 0
+	    && (devpriv->stat2 & STAT2_FIFONHF)) {
+		return IRQ_NONE;
 	}
-	/* setup command4 register */
-	devpriv->command4_bits = 0;
-	devpriv->command4_bits |= EXT_CONVERT_DISABLE_BIT;
-	/* single-ended/differential */
-	if (CR_AREF(insn->chanspec) == AREF_DIFF)
-		devpriv->command4_bits |= ADC_DIFF_BIT;
-	devpriv->write_byte(devpriv->command4_bits, dev->iobase + COMMAND4_REG);
 
-	/*
-	 * initialize pacer counter output to make sure it doesn't
-	 * cause any problems
-	 */
-	devpriv->write_byte(INIT_A0_BITS, dev->iobase + COUNTER_A_CONTROL_REG);
+	if (devpriv->stat1 & STAT1_OVERRUN) {
+		/* clear error interrupt */
+		devpriv->write_byte(0x1, dev->iobase + ADC_FIFO_CLEAR_REG);
+		async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
+		comedi_event(dev, s);
+		comedi_error(dev, "overrun");
+		return IRQ_HANDLED;
+	}
 
-	labpc_clear_adc_fifo(dev);
+#ifdef CONFIG_ISA_DMA_API
+	if (devpriv->current_transfer == isa_dma_transfer) {
+		/*
+		 * if a dma terminal count of external stop trigger
+		 * has occurred
+		 */
+		if (devpriv->stat1 & STAT1_GATA0 ||
+		    (board->register_layout == labpc_1200_layout
+		     && devpriv->stat2 & STAT2_OUTA1)) {
+			handle_isa_dma(dev);
+		}
+	} else
+#endif
+		labpc_drain_fifo(dev);
 
-	for (n = 0; n < insn->n; n++) {
-		/* trigger conversion */
-		devpriv->write_byte(0x1, dev->iobase + ADC_CONVERT_REG);
+	if (devpriv->stat1 & STAT1_CNTINT) {
+		comedi_error(dev, "handled timer interrupt?");
+		/*  clear it */
+		devpriv->write_byte(0x1, dev->iobase + TIMER_CLEAR_REG);
+	}
 
-		for (i = 0; i < timeout; i++) {
-			if (devpriv->read_byte(dev->iobase +
-					       STATUS1_REG) & DATA_AVAIL_BIT)
-				break;
-			udelay(1);
+	if (devpriv->stat1 & STAT1_OVERFLOW) {
+		/*  clear error interrupt */
+		devpriv->write_byte(0x1, dev->iobase + ADC_FIFO_CLEAR_REG);
+		async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
+		comedi_event(dev, s);
+		comedi_error(dev, "overflow");
+		return IRQ_HANDLED;
+	}
+	/*  handle external stop trigger */
+	if (cmd->stop_src == TRIG_EXT) {
+		if (devpriv->stat2 & STAT2_OUTA1) {
+			labpc_drain_dregs(dev);
+			labpc_cancel(dev, s);
+			async->events |= COMEDI_CB_EOA;
 		}
-		if (i == timeout) {
-			comedi_error(dev, "timeout");
-			return -ETIME;
+	}
+
+	/* TRIG_COUNT end of acquisition */
+	if (cmd->stop_src == TRIG_COUNT) {
+		if (devpriv->count == 0) {
+			labpc_cancel(dev, s);
+			async->events |= COMEDI_CB_EOA;
 		}
-		lsb = devpriv->read_byte(dev->iobase + ADC_FIFO_REG);
-		msb = devpriv->read_byte(dev->iobase + ADC_FIFO_REG);
-		data[n] = (msb << 8) | lsb;
 	}
 
-	return n;
+	comedi_event(dev, s);
+	return IRQ_HANDLED;
 }
 
-/* analog output insn */
-static int labpc_ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data)
+static int labpc_ao_insn_write(struct comedi_device *dev,
+			       struct comedi_subdevice *s,
+			       struct comedi_insn *insn,
+			       unsigned int *data)
 {
+	const struct labpc_boardinfo *board = comedi_board(dev);
 	struct labpc_private *devpriv = dev->private;
 	int channel, range;
 	unsigned long flags;
@@ -1664,20 +1262,19 @@ static int labpc_ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
 	/* note: hardware bug in daqcard-1200 means pacing cannot
 	 * be independently enabled/disabled for its the two channels */
 	spin_lock_irqsave(&dev->spinlock, flags);
-	devpriv->command2_bits &= ~DAC_PACED_BIT(channel);
-	devpriv->write_byte(devpriv->command2_bits, dev->iobase + COMMAND2_REG);
+	devpriv->cmd2 &= ~CMD2_LDAC(channel);
+	devpriv->write_byte(devpriv->cmd2, dev->iobase + CMD2_REG);
 	spin_unlock_irqrestore(&dev->spinlock, flags);
 
 	/* set range */
-	if (thisboard->register_layout == labpc_1200_layout) {
+	if (board->register_layout == labpc_1200_layout) {
 		range = CR_RANGE(insn->chanspec);
-		if (range & AO_RANGE_IS_UNIPOLAR)
-			devpriv->command6_bits |= DAC_UNIP_BIT(channel);
+		if (labpc_range_is_unipolar(s, range))
+			devpriv->cmd6 |= CMD6_DACUNI(channel);
 		else
-			devpriv->command6_bits &= ~DAC_UNIP_BIT(channel);
+			devpriv->cmd6 &= ~CMD6_DACUNI(channel);
 		/*  write to register */
-		devpriv->write_byte(devpriv->command6_bits,
-				    dev->iobase + COMMAND6_REG);
+		devpriv->write_byte(devpriv->cmd6, dev->iobase + CMD6_REG);
 	}
 	/* send data */
 	lsb = data[0] & 0xff;
@@ -1691,9 +1288,10 @@ static int labpc_ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
 	return 1;
 }
 
-/* analog output readback insn */
-static int labpc_ao_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data)
+static int labpc_ao_insn_read(struct comedi_device *dev,
+			      struct comedi_subdevice *s,
+			      struct comedi_insn *insn,
+			      unsigned int *data)
 {
 	struct labpc_private *devpriv = dev->private;
 
@@ -1702,183 +1300,7 @@ static int labpc_ao_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 	return 1;
 }
 
-static int labpc_calib_read_insn(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn, unsigned int *data)
-{
-	struct labpc_private *devpriv = dev->private;
-
-	data[0] = devpriv->caldac[CR_CHAN(insn->chanspec)];
-
-	return 1;
-}
-
-static int labpc_calib_write_insn(struct comedi_device *dev,
-				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn, unsigned int *data)
-{
-	int channel = CR_CHAN(insn->chanspec);
-
-	write_caldac(dev, channel, data[0]);
-	return 1;
-}
-
-static int labpc_eeprom_read_insn(struct comedi_device *dev,
-				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn, unsigned int *data)
-{
-	struct labpc_private *devpriv = dev->private;
-
-	data[0] = devpriv->eeprom_data[CR_CHAN(insn->chanspec)];
-
-	return 1;
-}
-
-static int labpc_eeprom_write_insn(struct comedi_device *dev,
-				   struct comedi_subdevice *s,
-				   struct comedi_insn *insn, unsigned int *data)
-{
-	int channel = CR_CHAN(insn->chanspec);
-	int ret;
-
-	/*  only allow writes to user area of eeprom */
-	if (channel < 16 || channel > 127) {
-		dev_dbg(dev->class_dev,
-			"eeprom writes are only allowed to channels 16 through 127 (the pointer and user areas)\n");
-		return -EINVAL;
-	}
-
-	ret = labpc_eeprom_write(dev, channel, data[0]);
-	if (ret < 0)
-		return ret;
-
-	return 1;
-}
-
-#ifdef CONFIG_ISA_DMA_API
-/* utility function that suggests a dma transfer size in bytes */
-static unsigned int labpc_suggest_transfer_size(const struct comedi_cmd *cmd)
-{
-	unsigned int size;
-	unsigned int freq;
-
-	if (cmd->convert_src == TRIG_TIMER)
-		freq = 1000000000 / cmd->convert_arg;
-	/* return some default value */
-	else
-		freq = 0xffffffff;
-
-	/* make buffer fill in no more than 1/3 second */
-	size = (freq / 3) * sample_size;
-
-	/* set a minimum and maximum size allowed */
-	if (size > dma_buffer_size)
-		size = dma_buffer_size - dma_buffer_size % sample_size;
-	else if (size < sample_size)
-		size = sample_size;
-
-	return size;
-}
-#endif
-
-/* figures out what counter values to use based on command */
-static void labpc_adc_timing(struct comedi_device *dev, struct comedi_cmd *cmd,
-			     enum scan_mode mode)
-{
-	struct labpc_private *devpriv = dev->private;
-	/* max value for 16 bit counter in mode 2 */
-	const int max_counter_value = 0x10000;
-	/* min value for 16 bit counter in mode 2 */
-	const int min_counter_value = 2;
-	unsigned int base_period;
-	unsigned int scan_period;
-	unsigned int convert_period;
-
-	/*
-	 * if both convert and scan triggers are TRIG_TIMER, then they
-	 * both rely on counter b0
-	 */
-	convert_period = labpc_ai_convert_period(cmd, mode);
-	scan_period = labpc_ai_scan_period(cmd, mode);
-	if (convert_period && scan_period) {
-		/*
-		 * pick the lowest b0 divisor value we can (for maximum input
-		 * clock speed on convert and scan counters)
-		 */
-		devpriv->divisor_b0 = (scan_period - 1) /
-		    (LABPC_TIMER_BASE * max_counter_value) + 1;
-		if (devpriv->divisor_b0 < min_counter_value)
-			devpriv->divisor_b0 = min_counter_value;
-		if (devpriv->divisor_b0 > max_counter_value)
-			devpriv->divisor_b0 = max_counter_value;
-
-		base_period = LABPC_TIMER_BASE * devpriv->divisor_b0;
-
-		/*  set a0 for conversion frequency and b1 for scan frequency */
-		switch (cmd->flags & TRIG_ROUND_MASK) {
-		default:
-		case TRIG_ROUND_NEAREST:
-			devpriv->divisor_a0 =
-			    (convert_period + (base_period / 2)) / base_period;
-			devpriv->divisor_b1 =
-			    (scan_period + (base_period / 2)) / base_period;
-			break;
-		case TRIG_ROUND_UP:
-			devpriv->divisor_a0 =
-			    (convert_period + (base_period - 1)) / base_period;
-			devpriv->divisor_b1 =
-			    (scan_period + (base_period - 1)) / base_period;
-			break;
-		case TRIG_ROUND_DOWN:
-			devpriv->divisor_a0 = convert_period / base_period;
-			devpriv->divisor_b1 = scan_period / base_period;
-			break;
-		}
-		/*  make sure a0 and b1 values are acceptable */
-		if (devpriv->divisor_a0 < min_counter_value)
-			devpriv->divisor_a0 = min_counter_value;
-		if (devpriv->divisor_a0 > max_counter_value)
-			devpriv->divisor_a0 = max_counter_value;
-		if (devpriv->divisor_b1 < min_counter_value)
-			devpriv->divisor_b1 = min_counter_value;
-		if (devpriv->divisor_b1 > max_counter_value)
-			devpriv->divisor_b1 = max_counter_value;
-		/*  write corrected timings to command */
-		labpc_set_ai_convert_period(cmd, mode,
-					    base_period * devpriv->divisor_a0);
-		labpc_set_ai_scan_period(cmd, mode,
-					 base_period * devpriv->divisor_b1);
-		/*
-		 * if only one TRIG_TIMER is used, we can employ the generic
-		 * cascaded timing functions
-		 */
-	} else if (scan_period) {
-		/*
-		 * calculate cascaded counter values
-		 * that give desired scan timing
-		 */
-		i8253_cascade_ns_to_timer_2div(LABPC_TIMER_BASE,
-					       &(devpriv->divisor_b1),
-					       &(devpriv->divisor_b0),
-					       &scan_period,
-					       cmd->flags & TRIG_ROUND_MASK);
-		labpc_set_ai_scan_period(cmd, mode, scan_period);
-	} else if (convert_period) {
-		/*
-		 * calculate cascaded counter values
-		 * that give desired conversion timing
-		 */
-		i8253_cascade_ns_to_timer_2div(LABPC_TIMER_BASE,
-					       &(devpriv->divisor_a0),
-					       &(devpriv->divisor_b0),
-					       &convert_period,
-					       cmd->flags & TRIG_ROUND_MASK);
-		labpc_set_ai_convert_period(cmd, mode, convert_period);
-	}
-}
-
-static int labpc_dio_mem_callback(int dir, int port, int data,
-				  unsigned long iobase)
+static int labpc_8255_mmio(int dir, int port, int data, unsigned long iobase)
 {
 	if (dir) {
 		writeb(data, (void __iomem *)(iobase + port));
@@ -1897,20 +1319,18 @@ static void labpc_serial_out(struct comedi_device *dev, unsigned int value,
 
 	for (i = 1; i <= value_width; i++) {
 		/*  clear serial clock */
-		devpriv->command5_bits &= ~SCLOCK_BIT;
+		devpriv->cmd5 &= ~CMD5_SCLK;
 		/*  send bits most significant bit first */
 		if (value & (1 << (value_width - i)))
-			devpriv->command5_bits |= SDATA_BIT;
+			devpriv->cmd5 |= CMD5_SDATA;
 		else
-			devpriv->command5_bits &= ~SDATA_BIT;
+			devpriv->cmd5 &= ~CMD5_SDATA;
 		udelay(1);
-		devpriv->write_byte(devpriv->command5_bits,
-				    dev->iobase + COMMAND5_REG);
+		devpriv->write_byte(devpriv->cmd5, dev->iobase + CMD5_REG);
 		/*  set clock to load bit */
-		devpriv->command5_bits |= SCLOCK_BIT;
+		devpriv->cmd5 |= CMD5_SCLK;
 		udelay(1);
-		devpriv->write_byte(devpriv->command5_bits,
-				    dev->iobase + COMMAND5_REG);
+		devpriv->write_byte(devpriv->cmd5, dev->iobase + CMD5_REG);
 	}
 }
 
@@ -1924,20 +1344,17 @@ static unsigned int labpc_serial_in(struct comedi_device *dev)
 
 	for (i = 1; i <= value_width; i++) {
 		/*  set serial clock */
-		devpriv->command5_bits |= SCLOCK_BIT;
+		devpriv->cmd5 |= CMD5_SCLK;
 		udelay(1);
-		devpriv->write_byte(devpriv->command5_bits,
-				    dev->iobase + COMMAND5_REG);
+		devpriv->write_byte(devpriv->cmd5, dev->iobase + CMD5_REG);
 		/*  clear clock bit */
-		devpriv->command5_bits &= ~SCLOCK_BIT;
+		devpriv->cmd5 &= ~CMD5_SCLK;
 		udelay(1);
-		devpriv->write_byte(devpriv->command5_bits,
-				    dev->iobase + COMMAND5_REG);
+		devpriv->write_byte(devpriv->cmd5, dev->iobase + CMD5_REG);
 		/*  read bits most significant bit first */
 		udelay(1);
-		devpriv->status2_bits =
-		    devpriv->read_byte(dev->iobase + STATUS2_REG);
-		if (devpriv->status2_bits & EEPROM_OUT_BIT)
+		devpriv->stat2 = devpriv->read_byte(dev->iobase + STAT2_REG);
+		if (devpriv->stat2 & STAT2_PROMOUT)
 			value |= 1 << (value_width - i);
 	}
 
@@ -1955,12 +1372,12 @@ static unsigned int labpc_eeprom_read(struct comedi_device *dev,
 	const int write_length = 8;
 
 	/*  enable read/write to eeprom */
-	devpriv->command5_bits &= ~EEPROM_EN_BIT;
+	devpriv->cmd5 &= ~CMD5_EEPROMCS;
 	udelay(1);
-	devpriv->write_byte(devpriv->command5_bits, dev->iobase + COMMAND5_REG);
-	devpriv->command5_bits |= EEPROM_EN_BIT | EEPROM_WRITE_UNPROTECT_BIT;
+	devpriv->write_byte(devpriv->cmd5, dev->iobase + CMD5_REG);
+	devpriv->cmd5 |= (CMD5_EEPROMCS | CMD5_WRTPRT);
 	udelay(1);
-	devpriv->write_byte(devpriv->command5_bits, dev->iobase + COMMAND5_REG);
+	devpriv->write_byte(devpriv->cmd5, dev->iobase + CMD5_REG);
 
 	/*  send read instruction */
 	labpc_serial_out(dev, read_instruction, write_length);
@@ -1970,9 +1387,37 @@ static unsigned int labpc_eeprom_read(struct comedi_device *dev,
 	value = labpc_serial_in(dev);
 
 	/*  disable read/write to eeprom */
-	devpriv->command5_bits &= ~EEPROM_EN_BIT & ~EEPROM_WRITE_UNPROTECT_BIT;
+	devpriv->cmd5 &= ~(CMD5_EEPROMCS | CMD5_WRTPRT);
+	udelay(1);
+	devpriv->write_byte(devpriv->cmd5, dev->iobase + CMD5_REG);
+
+	return value;
+}
+
+static unsigned int labpc_eeprom_read_status(struct comedi_device *dev)
+{
+	struct labpc_private *devpriv = dev->private;
+	unsigned int value;
+	const int read_status_instruction = 0x5;
+	const int write_length = 8;	/*  8 bit write lengths to eeprom */
+
+	/*  enable read/write to eeprom */
+	devpriv->cmd5 &= ~CMD5_EEPROMCS;
+	udelay(1);
+	devpriv->write_byte(devpriv->cmd5, dev->iobase + CMD5_REG);
+	devpriv->cmd5 |= (CMD5_EEPROMCS | CMD5_WRTPRT);
+	udelay(1);
+	devpriv->write_byte(devpriv->cmd5, dev->iobase + CMD5_REG);
+
+	/*  send read status instruction */
+	labpc_serial_out(dev, read_status_instruction, write_length);
+	/*  read result */
+	value = labpc_serial_in(dev);
+
+	/*  disable read/write to eeprom */
+	devpriv->cmd5 &= ~(CMD5_EEPROMCS | CMD5_WRTPRT);
 	udelay(1);
-	devpriv->write_byte(devpriv->command5_bits, dev->iobase + COMMAND5_REG);
+	devpriv->write_byte(devpriv->cmd5, dev->iobase + CMD5_REG);
 
 	return value;
 }
@@ -2002,68 +1447,40 @@ static int labpc_eeprom_write(struct comedi_device *dev,
 	devpriv->eeprom_data[address] = value;
 
 	/*  enable read/write to eeprom */
-	devpriv->command5_bits &= ~EEPROM_EN_BIT;
+	devpriv->cmd5 &= ~CMD5_EEPROMCS;
 	udelay(1);
-	devpriv->write_byte(devpriv->command5_bits, dev->iobase + COMMAND5_REG);
-	devpriv->command5_bits |= EEPROM_EN_BIT | EEPROM_WRITE_UNPROTECT_BIT;
+	devpriv->write_byte(devpriv->cmd5, dev->iobase + CMD5_REG);
+	devpriv->cmd5 |= (CMD5_EEPROMCS | CMD5_WRTPRT);
 	udelay(1);
-	devpriv->write_byte(devpriv->command5_bits, dev->iobase + COMMAND5_REG);
+	devpriv->write_byte(devpriv->cmd5, dev->iobase + CMD5_REG);
 
 	/*  send write_enable instruction */
 	labpc_serial_out(dev, write_enable_instruction, write_length);
-	devpriv->command5_bits &= ~EEPROM_EN_BIT;
+	devpriv->cmd5 &= ~CMD5_EEPROMCS;
 	udelay(1);
-	devpriv->write_byte(devpriv->command5_bits, dev->iobase + COMMAND5_REG);
+	devpriv->write_byte(devpriv->cmd5, dev->iobase + CMD5_REG);
 
 	/*  send write instruction */
-	devpriv->command5_bits |= EEPROM_EN_BIT;
+	devpriv->cmd5 |= CMD5_EEPROMCS;
 	udelay(1);
-	devpriv->write_byte(devpriv->command5_bits, dev->iobase + COMMAND5_REG);
+	devpriv->write_byte(devpriv->cmd5, dev->iobase + CMD5_REG);
 	labpc_serial_out(dev, write_instruction, write_length);
 	/*  send 8 bit address to write to */
 	labpc_serial_out(dev, address, write_length);
 	/*  write value */
 	labpc_serial_out(dev, value, write_length);
-	devpriv->command5_bits &= ~EEPROM_EN_BIT;
+	devpriv->cmd5 &= ~CMD5_EEPROMCS;
 	udelay(1);
-	devpriv->write_byte(devpriv->command5_bits, dev->iobase + COMMAND5_REG);
+	devpriv->write_byte(devpriv->cmd5, dev->iobase + CMD5_REG);
 
 	/*  disable read/write to eeprom */
-	devpriv->command5_bits &= ~EEPROM_EN_BIT & ~EEPROM_WRITE_UNPROTECT_BIT;
+	devpriv->cmd5 &= ~(CMD5_EEPROMCS | CMD5_WRTPRT);
 	udelay(1);
-	devpriv->write_byte(devpriv->command5_bits, dev->iobase + COMMAND5_REG);
+	devpriv->write_byte(devpriv->cmd5, dev->iobase + CMD5_REG);
 
 	return 0;
 }
 
-static unsigned int labpc_eeprom_read_status(struct comedi_device *dev)
-{
-	struct labpc_private *devpriv = dev->private;
-	unsigned int value;
-	const int read_status_instruction = 0x5;
-	const int write_length = 8;	/*  8 bit write lengths to eeprom */
-
-	/*  enable read/write to eeprom */
-	devpriv->command5_bits &= ~EEPROM_EN_BIT;
-	udelay(1);
-	devpriv->write_byte(devpriv->command5_bits, dev->iobase + COMMAND5_REG);
-	devpriv->command5_bits |= EEPROM_EN_BIT | EEPROM_WRITE_UNPROTECT_BIT;
-	udelay(1);
-	devpriv->write_byte(devpriv->command5_bits, dev->iobase + COMMAND5_REG);
-
-	/*  send read status instruction */
-	labpc_serial_out(dev, read_status_instruction, write_length);
-	/*  read result */
-	value = labpc_serial_in(dev);
-
-	/*  disable read/write to eeprom */
-	devpriv->command5_bits &= ~EEPROM_EN_BIT & ~EEPROM_WRITE_UNPROTECT_BIT;
-	udelay(1);
-	devpriv->write_byte(devpriv->command5_bits, dev->iobase + COMMAND5_REG);
-
-	return value;
-}
-
 /* writes to 8 bit calibration dacs */
 static void write_caldac(struct comedi_device *dev, unsigned int channel,
 			 unsigned int value)
@@ -2075,10 +1492,9 @@ static void write_caldac(struct comedi_device *dev, unsigned int channel,
 	devpriv->caldac[channel] = value;
 
 	/*  clear caldac load bit and make sure we don't write to eeprom */
-	devpriv->command5_bits &=
-	    ~CALDAC_LOAD_BIT & ~EEPROM_EN_BIT & ~EEPROM_WRITE_UNPROTECT_BIT;
+	devpriv->cmd5 &= ~(CMD5_CALDACLD | CMD5_EEPROMCS | CMD5_WRTPRT);
 	udelay(1);
-	devpriv->write_byte(devpriv->command5_bits, dev->iobase + COMMAND5_REG);
+	devpriv->write_byte(devpriv->cmd5, dev->iobase + CMD5_REG);
 
 	/*  write 4 bit channel */
 	labpc_serial_out(dev, channel, 4);
@@ -2086,49 +1502,295 @@ static void write_caldac(struct comedi_device *dev, unsigned int channel,
 	labpc_serial_out(dev, value, 8);
 
 	/*  set and clear caldac bit to load caldac value */
-	devpriv->command5_bits |= CALDAC_LOAD_BIT;
+	devpriv->cmd5 |= CMD5_CALDACLD;
 	udelay(1);
-	devpriv->write_byte(devpriv->command5_bits, dev->iobase + COMMAND5_REG);
-	devpriv->command5_bits &= ~CALDAC_LOAD_BIT;
+	devpriv->write_byte(devpriv->cmd5, dev->iobase + CMD5_REG);
+	devpriv->cmd5 &= ~CMD5_CALDACLD;
 	udelay(1);
-	devpriv->write_byte(devpriv->command5_bits, dev->iobase + COMMAND5_REG);
+	devpriv->write_byte(devpriv->cmd5, dev->iobase + CMD5_REG);
 }
 
-static struct comedi_driver labpc_driver = {
-	.driver_name = DRV_NAME,
-	.module = THIS_MODULE,
-	.attach = labpc_attach,
-	.auto_attach = labpc_auto_attach,
-	.detach = labpc_common_detach,
-	.num_names = ARRAY_SIZE(labpc_boards),
-	.board_name = &labpc_boards[0].name,
-	.offset = sizeof(struct labpc_board_struct),
-};
+static int labpc_calib_insn_write(struct comedi_device *dev,
+				  struct comedi_subdevice *s,
+				  struct comedi_insn *insn,
+				  unsigned int *data)
+{
+	unsigned int chan = CR_CHAN(insn->chanspec);
 
-#ifdef CONFIG_COMEDI_PCI_DRIVERS
-static DEFINE_PCI_DEVICE_TABLE(labpc_pci_table) = {
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x161)},
-	{0}
-};
-MODULE_DEVICE_TABLE(pci, labpc_pci_table);
+	/*
+	 * Only write the last data value to the caldac. Preceding
+	 * data would be overwritten anyway.
+	 */
+	if (insn->n > 0)
+		write_caldac(dev, chan, data[insn->n - 1]);
+
+	return insn->n;
+}
 
-static int labpc_pci_probe(struct pci_dev *dev,
-				     const struct pci_device_id *ent)
+static int labpc_calib_insn_read(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 struct comedi_insn *insn,
+				 unsigned int *data)
 {
-	return comedi_pci_auto_config(dev, &labpc_driver);
+	struct labpc_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	int i;
+
+	for (i = 0; i < insn->n; i++)
+		data[i] = devpriv->caldac[chan];
+
+	return insn->n;
 }
 
-static struct pci_driver labpc_pci_driver = {
-	.name = DRV_NAME,
-	.id_table = labpc_pci_table,
-	.probe = labpc_pci_probe,
-	.remove		= comedi_pci_auto_unconfig,
+static int labpc_eeprom_insn_write(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   struct comedi_insn *insn,
+				   unsigned int *data)
+{
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	int ret;
+
+	/* only allow writes to user area of eeprom */
+	if (chan < 16 || chan > 127)
+		return -EINVAL;
+
+	/*
+	 * Only write the last data value to the eeprom. Preceding
+	 * data would be overwritten anyway.
+	 */
+	if (insn->n > 0) {
+		ret = labpc_eeprom_write(dev, chan, data[insn->n - 1]);
+		if (ret)
+			return ret;
+	}
+
+	return insn->n;
+}
+
+static int labpc_eeprom_insn_read(struct comedi_device *dev,
+				  struct comedi_subdevice *s,
+				  struct comedi_insn *insn,
+				  unsigned int *data)
+{
+	struct labpc_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	int i;
+
+	for (i = 0; i < insn->n; i++)
+		data[i] = devpriv->eeprom_data[chan];
+
+	return insn->n;
+}
+
+int labpc_common_attach(struct comedi_device *dev,
+			unsigned int irq, unsigned long isr_flags)
+{
+	const struct labpc_boardinfo *board = comedi_board(dev);
+	struct labpc_private *devpriv = dev->private;
+	struct comedi_subdevice *s;
+	int ret;
+	int i;
+
+	if (board->has_mmio) {
+		devpriv->read_byte = labpc_readb;
+		devpriv->write_byte = labpc_writeb;
+	} else {
+		devpriv->read_byte = labpc_inb;
+		devpriv->write_byte = labpc_outb;
+	}
+
+	/* initialize board's command registers */
+	devpriv->write_byte(devpriv->cmd1, dev->iobase + CMD1_REG);
+	devpriv->write_byte(devpriv->cmd2, dev->iobase + CMD2_REG);
+	devpriv->write_byte(devpriv->cmd3, dev->iobase + CMD3_REG);
+	devpriv->write_byte(devpriv->cmd4, dev->iobase + CMD4_REG);
+	if (board->register_layout == labpc_1200_layout) {
+		devpriv->write_byte(devpriv->cmd5, dev->iobase + CMD5_REG);
+		devpriv->write_byte(devpriv->cmd6, dev->iobase + CMD6_REG);
+	}
+
+	if (irq) {
+		ret = request_irq(irq, labpc_interrupt, isr_flags,
+				  dev->board_name, dev);
+		if (ret == 0)
+			dev->irq = irq;
+	}
+
+	ret = comedi_alloc_subdevices(dev, 5);
+	if (ret)
+		return ret;
+
+	/* analog input subdevice */
+	s = &dev->subdevices[0];
+	s->type		= COMEDI_SUBD_AI;
+	s->subdev_flags	= SDF_READABLE | SDF_GROUND | SDF_COMMON | SDF_DIFF;
+	s->n_chan	= 8;
+	s->len_chanlist	= 8;
+	s->maxdata	= 0x0fff;
+	s->range_table	= board->ai_range_table;
+	s->insn_read	= labpc_ai_insn_read;
+	if (dev->irq) {
+		dev->read_subdev = s;
+		s->subdev_flags	|= SDF_CMD_READ;
+		s->do_cmd	= labpc_ai_cmd;
+		s->do_cmdtest	= labpc_ai_cmdtest;
+		s->cancel	= labpc_cancel;
+	}
+
+	/* analog output */
+	s = &dev->subdevices[1];
+	if (board->has_ao) {
+		s->type		= COMEDI_SUBD_AO;
+		s->subdev_flags	= SDF_READABLE | SDF_WRITABLE | SDF_GROUND;
+		s->n_chan	= NUM_AO_CHAN;
+		s->maxdata	= 0x0fff;
+		s->range_table	= &range_labpc_ao;
+		s->insn_read	= labpc_ao_insn_read;
+		s->insn_write	= labpc_ao_insn_write;
+
+		/* initialize analog outputs to a known value */
+		for (i = 0; i < s->n_chan; i++) {
+			short lsb, msb;
+
+			devpriv->ao_value[i] = s->maxdata / 2;
+			lsb = devpriv->ao_value[i] & 0xff;
+			msb = (devpriv->ao_value[i] >> 8) & 0xff;
+			devpriv->write_byte(lsb, dev->iobase + DAC_LSB_REG(i));
+			devpriv->write_byte(msb, dev->iobase + DAC_MSB_REG(i));
+		}
+	} else {
+		s->type		= COMEDI_SUBD_UNUSED;
+	}
+
+	/* 8255 dio */
+	s = &dev->subdevices[2];
+	ret = subdev_8255_init(dev, s,
+			       (board->has_mmio) ? labpc_8255_mmio : NULL,
+			       dev->iobase + DIO_BASE_REG);
+	if (ret)
+		return ret;
+
+	/*  calibration subdevices for boards that have one */
+	s = &dev->subdevices[3];
+	if (board->register_layout == labpc_1200_layout) {
+		s->type		= COMEDI_SUBD_CALIB;
+		s->subdev_flags	= SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
+		s->n_chan	= 16;
+		s->maxdata	= 0xff;
+		s->insn_read	= labpc_calib_insn_read;
+		s->insn_write	= labpc_calib_insn_write;
+
+		for (i = 0; i < s->n_chan; i++)
+			write_caldac(dev, i, s->maxdata / 2);
+	} else
+		s->type		= COMEDI_SUBD_UNUSED;
+
+	/* EEPROM */
+	s = &dev->subdevices[4];
+	if (board->register_layout == labpc_1200_layout) {
+		s->type		= COMEDI_SUBD_MEMORY;
+		s->subdev_flags	= SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
+		s->n_chan	= EEPROM_SIZE;
+		s->maxdata	= 0xff;
+		s->insn_read	= labpc_eeprom_insn_read;
+		s->insn_write	= labpc_eeprom_insn_write;
+
+		for (i = 0; i < s->n_chan; i++)
+			devpriv->eeprom_data[i] = labpc_eeprom_read(dev, i);
+	} else
+		s->type		= COMEDI_SUBD_UNUSED;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(labpc_common_attach);
+
+void labpc_common_detach(struct comedi_device *dev)
+{
+	comedi_spriv_free(dev, 2);
+}
+EXPORT_SYMBOL_GPL(labpc_common_detach);
+
+#if IS_ENABLED(CONFIG_COMEDI_NI_LABPC_ISA)
+static int labpc_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+{
+	struct labpc_private *devpriv;
+	unsigned int irq = it->options[1];
+	unsigned int dma_chan = it->options[2];
+	int ret;
+
+	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
+	if (!devpriv)
+		return -ENOMEM;
+	dev->private = devpriv;
+
+	ret = comedi_request_region(dev, it->options[0], LABPC_SIZE);
+	if (ret)
+		return ret;
+
+	ret = labpc_common_attach(dev, irq, 0);
+	if (ret)
+		return ret;
+
+#ifdef CONFIG_ISA_DMA_API
+	if (dev->irq && (dma_chan == 1 || dma_chan == 3)) {
+		devpriv->dma_buffer = kmalloc(dma_buffer_size,
+					      GFP_KERNEL | GFP_DMA);
+		if (devpriv->dma_buffer) {
+			ret = request_dma(dma_chan, dev->board_name);
+			if (ret == 0) {
+				unsigned long dma_flags;
+
+				devpriv->dma_chan = dma_chan;
+				dma_flags = claim_dma_lock();
+				disable_dma(devpriv->dma_chan);
+				set_dma_mode(devpriv->dma_chan, DMA_MODE_READ);
+				release_dma_lock(dma_flags);
+			} else {
+				kfree(devpriv->dma_buffer);
+			}
+		}
+	}
+#endif
+
+	return 0;
+}
+
+static void labpc_detach(struct comedi_device *dev)
+{
+	struct labpc_private *devpriv = dev->private;
+
+	labpc_common_detach(dev);
+
+	if (devpriv) {
+		kfree(devpriv->dma_buffer);
+		if (devpriv->dma_chan)
+			free_dma(devpriv->dma_chan);
+	}
+	comedi_legacy_detach(dev);
+}
+
+static struct comedi_driver labpc_driver = {
+	.driver_name	= "ni_labpc",
+	.module		= THIS_MODULE,
+	.attach		= labpc_attach,
+	.detach		= labpc_detach,
+	.num_names	= ARRAY_SIZE(labpc_boards),
+	.board_name	= &labpc_boards[0].name,
+	.offset		= sizeof(struct labpc_boardinfo),
 };
-module_comedi_pci_driver(labpc_driver, labpc_pci_driver);
-#else
 module_comedi_driver(labpc_driver);
-#endif
+#else
+static int __init labpc_common_init(void)
+{
+	return 0;
+}
+module_init(labpc_common_init);
 
+static void __exit labpc_common_exit(void)
+{
+}
+module_exit(labpc_common_exit);
+#endif
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/ni_labpc.h b/drivers/staging/comedi/drivers/ni_labpc.h
index e052ed3ba54..615f16f271c 100644
--- a/drivers/staging/comedi/drivers/ni_labpc.h
+++ b/drivers/staging/comedi/drivers/ni_labpc.h
@@ -27,48 +27,45 @@
 #define EEPROM_SIZE	256	/*  256 byte eeprom */
 #define NUM_AO_CHAN	2	/*  boards have two analog output channels */
 
-enum labpc_bustype { isa_bustype, pci_bustype, pcmcia_bustype };
 enum labpc_register_layout { labpc_plus_layout, labpc_1200_layout };
 enum transfer_type { fifo_not_empty_transfer, fifo_half_full_transfer,
 	isa_dma_transfer
 };
 
-struct labpc_board_struct {
+struct labpc_boardinfo {
 	const char *name;
 	int device_id;		/*  device id for pci and pcmcia boards */
 	int ai_speed;		/*  maximum input speed in nanoseconds */
-	enum labpc_bustype bustype;	/*  ISA/PCI/etc. */
 
 	/*  1200 has extra registers compared to pc+ */
 	enum labpc_register_layout register_layout;
 	int has_ao;		/*  has analog output true/false */
 	const struct comedi_lrange *ai_range_table;
 	const int *ai_range_code;
-	const int *ai_range_is_unipolar;
 
 	/*  board can auto scan up in ai channels, not just down */
 	unsigned ai_scan_up:1;
 
 	/* uses memory mapped io instead of ioports */
-	unsigned memory_mapped_io:1;
+	unsigned has_mmio:1;
 };
 
 struct labpc_private {
 	struct mite_struct *mite;	/*  for mite chip on pci-1200 */
 	/*  number of data points left to be taken */
-	volatile unsigned long long count;
+	unsigned long long count;
 	/*  software copy of analog output values */
 	unsigned int ao_value[NUM_AO_CHAN];
 	/*  software copys of bits written to command registers */
-	volatile unsigned int command1_bits;
-	volatile unsigned int command2_bits;
-	volatile unsigned int command3_bits;
-	volatile unsigned int command4_bits;
-	volatile unsigned int command5_bits;
-	volatile unsigned int command6_bits;
+	unsigned int cmd1;
+	unsigned int cmd2;
+	unsigned int cmd3;
+	unsigned int cmd4;
+	unsigned int cmd5;
+	unsigned int cmd6;
 	/*  store last read of board status registers */
-	volatile unsigned int status1_bits;
-	volatile unsigned int status2_bits;
+	unsigned int stat1;
+	unsigned int stat2;
 	/*
 	 * value to load into board's counter a0 (conversion pacing) for timed
 	 * conversions
@@ -101,11 +98,10 @@ struct labpc_private {
 	void (*write_byte) (unsigned int byte, unsigned long address);
 };
 
-int labpc_common_attach(struct comedi_device *dev, unsigned long iobase,
-			unsigned int irq, unsigned int dma);
+int labpc_common_attach(struct comedi_device *dev,
+			unsigned int irq, unsigned long isr_flags);
 void labpc_common_detach(struct comedi_device *dev);
 
-extern const int labpc_1200_is_unipolar[];
 extern const int labpc_1200_ai_gain_bits[];
 extern const struct comedi_lrange range_labpc_1200_ai;
 
diff --git a/drivers/staging/comedi/drivers/ni_labpc_cs.c b/drivers/staging/comedi/drivers/ni_labpc_cs.c
index be7d1413b2e..9e3737c6918 100644
--- a/drivers/staging/comedi/drivers/ni_labpc_cs.c
+++ b/drivers/staging/comedi/drivers/ni_labpc_cs.c
@@ -73,17 +73,15 @@ NI manuals:
 #include <pcmcia/cisreg.h>
 #include <pcmcia/ds.h>
 
-static const struct labpc_board_struct labpc_cs_boards[] = {
+static const struct labpc_boardinfo labpc_cs_boards[] = {
 	{
 		.name			= "daqcard-1200",
 		.device_id		= 0x103,
 		.ai_speed		= 10000,
-		.bustype		= pcmcia_bustype,
 		.register_layout	= labpc_1200_layout,
 		.has_ao			= 1,
 		.ai_range_table		= &range_labpc_1200_ai,
 		.ai_range_code		= labpc_1200_ai_gain_bits,
-		.ai_range_is_unipolar	= labpc_1200_is_unipolar,
 	},
 };
 
@@ -112,7 +110,7 @@ static int labpc_auto_attach(struct comedi_device *dev,
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	return labpc_common_attach(dev, dev->iobase, link->irq, 0);
+	return labpc_common_attach(dev, link->irq, IRQF_SHARED);
 }
 
 static void labpc_detach(struct comedi_device *dev)
diff --git a/drivers/staging/comedi/drivers/ni_labpc_pci.c b/drivers/staging/comedi/drivers/ni_labpc_pci.c
new file mode 100644
index 00000000000..8e916f86cce
--- /dev/null
+++ b/drivers/staging/comedi/drivers/ni_labpc_pci.c
@@ -0,0 +1,142 @@
+/*
+ * comedi/drivers/ni_labpc_pci.c
+ * Driver for National Instruments Lab-PC PCI-1200
+ * Copyright (C) 2001, 2002, 2003 Frank Mori Hess <fmhess@users.sourceforge.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.
+ */
+
+/*
+ * Driver: ni_labpc_pci
+ * Description: National Instruments Lab-PC PCI-1200
+ * Devices: (National Instruments) PCI-1200 [ni_pci-1200]
+ * Author: Frank Mori Hess <fmhess@users.sourceforge.net>
+ * Status: works
+ *
+ * This is the PCI-specific support split off from the ni_labpc driver.
+ *
+ * Configuration Options: not applicable, uses PCI auto config
+ *
+ * NI manuals:
+ * 340914a (pci-1200)
+ */
+
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/pci.h>
+
+#include "../comedidev.h"
+
+#include "mite.h"
+#include "ni_labpc.h"
+
+enum labpc_pci_boardid {
+	BOARD_NI_PCI1200,
+};
+
+static const struct labpc_boardinfo labpc_pci_boards[] = {
+	[BOARD_NI_PCI1200] = {
+		.name			= "ni_pci-1200",
+		.ai_speed		= 10000,
+		.register_layout	= labpc_1200_layout,
+		.has_ao			= 1,
+		.ai_range_table		= &range_labpc_1200_ai,
+		.ai_range_code		= labpc_1200_ai_gain_bits,
+		.ai_scan_up		= 1,
+		.has_mmio		= 1,
+	},
+};
+
+static int labpc_pci_auto_attach(struct comedi_device *dev,
+				 unsigned long context)
+{
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	const struct labpc_boardinfo *board = NULL;
+	struct labpc_private *devpriv;
+	int ret;
+
+	if (context < ARRAY_SIZE(labpc_pci_boards))
+		board = &labpc_pci_boards[context];
+	if (!board)
+		return -ENODEV;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
+
+	ret = comedi_pci_enable(dev);
+	if (ret)
+		return ret;
+
+	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
+	if (!devpriv)
+		return -ENOMEM;
+	dev->private = devpriv;
+
+	devpriv->mite = mite_alloc(pcidev);
+	if (!devpriv->mite)
+		return -ENOMEM;
+	ret = mite_setup(devpriv->mite);
+	if (ret < 0)
+		return ret;
+	dev->iobase = (unsigned long)devpriv->mite->daq_io_addr;
+
+	return labpc_common_attach(dev, mite_irq(devpriv->mite), IRQF_SHARED);
+}
+
+static void labpc_pci_detach(struct comedi_device *dev)
+{
+	struct labpc_private *devpriv = dev->private;
+
+	labpc_common_detach(dev);
+
+	if (devpriv && devpriv->mite) {
+		mite_unsetup(devpriv->mite);
+		mite_free(devpriv->mite);
+	}
+	if (dev->irq)
+		free_irq(dev->irq, dev);
+	comedi_pci_disable(dev);
+}
+
+static struct comedi_driver labpc_pci_comedi_driver = {
+	.driver_name	= "labpc_pci",
+	.module		= THIS_MODULE,
+	.auto_attach	= labpc_pci_auto_attach,
+	.detach		= labpc_pci_detach,
+};
+
+static DEFINE_PCI_DEVICE_TABLE(labpc_pci_table) = {
+	{ PCI_VDEVICE(NI, 0x161), BOARD_NI_PCI1200 },
+	{ 0 }
+};
+MODULE_DEVICE_TABLE(pci, labpc_pci_table);
+
+static int labpc_pci_probe(struct pci_dev *dev,
+			   const struct pci_device_id *id)
+{
+	return comedi_pci_auto_config(dev, &labpc_pci_comedi_driver,
+				      id->driver_data);
+}
+
+static struct pci_driver labpc_pci_driver = {
+	.name		= "labpc_pci",
+	.id_table	= labpc_pci_table,
+	.probe		= labpc_pci_probe,
+	.remove		= comedi_pci_auto_unconfig,
+};
+module_comedi_pci_driver(labpc_pci_comedi_driver, labpc_pci_driver);
+
+MODULE_DESCRIPTION("Comedi: National Instruments Lab-PC PCI-1200 driver");
+MODULE_AUTHOR("Comedi http://www.comedi.org");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/ni_mio_common.c b/drivers/staging/comedi/drivers/ni_mio_common.c
index b7403597e90..a46d579016d 100644
--- a/drivers/staging/comedi/drivers/ni_mio_common.c
+++ b/drivers/staging/comedi/drivers/ni_mio_common.c
@@ -189,11 +189,6 @@ static const struct comedi_lrange range_ni_M_ai_628x = { 7, {
 							     }
 };
 
-static const struct comedi_lrange range_ni_S_ai_6143 = { 1, {
-							     RANGE(-5, +5),
-							     }
-};
-
 static const struct comedi_lrange range_ni_E_ao_ext = { 4, {
 							    RANGE(-10, 10),
 							    RANGE(0, 10),
@@ -210,7 +205,7 @@ static const struct comedi_lrange *const ni_range_lkup[] = {
 	[ai_gain_611x] = &range_ni_E_ai_611x,
 	[ai_gain_622x] = &range_ni_M_ai_622x,
 	[ai_gain_628x] = &range_ni_M_ai_628x,
-	[ai_gain_6143] = &range_ni_S_ai_6143
+	[ai_gain_6143] = &range_bipolar5
 };
 
 static int ni_dio_insn_config(struct comedi_device *dev,
@@ -696,9 +691,10 @@ static void ni_release_cdo_mite_channel(struct comedi_device *dev)
 static void ni_e_series_enable_second_irq(struct comedi_device *dev,
 					  unsigned gpct_index, short enable)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 
-	if (boardtype.reg_type & ni_reg_m_series_mask)
+	if (board->reg_type & ni_reg_m_series_mask)
 		return;
 	switch (gpct_index) {
 	case 0:
@@ -728,16 +724,17 @@ static void ni_e_series_enable_second_irq(struct comedi_device *dev,
 
 static void ni_clear_ai_fifo(struct comedi_device *dev)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 
-	if (boardtype.reg_type == ni_reg_6143) {
+	if (board->reg_type == ni_reg_6143) {
 		/*  Flush the 6143 data FIFO */
 		ni_writel(0x10, AIFIFO_Control_6143);	/*  Flush fifo */
 		ni_writel(0x00, AIFIFO_Control_6143);	/*  Flush fifo */
 		while (ni_readl(AIFIFO_Status_6143) & 0x10) ;	/*  Wait for complete */
 	} else {
 		devpriv->stc_writew(dev, 1, ADC_FIFO_Clear);
-		if (boardtype.reg_type == ni_reg_625x) {
+		if (board->reg_type == ni_reg_625x) {
 			ni_writeb(0, M_Offset_Static_AI_Control(0));
 			ni_writeb(1, M_Offset_Static_AI_Control(0));
 #if 0
@@ -845,7 +842,7 @@ static irqreturn_t ni_E_interrupt(int irq, void *d)
 	struct mite_struct *mite = devpriv->mite;
 #endif
 
-	if (dev->attached == 0)
+	if (!dev->attached)
 		return IRQ_NONE;
 	smp_mb();		/*  make sure dev->attached is checked before handler does anything else. */
 
@@ -1292,6 +1289,7 @@ static void ni_mio_print_status_b(int status)
 static void ni_ao_fifo_load(struct comedi_device *dev,
 			    struct comedi_subdevice *s, int n)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
 	int chan;
@@ -1309,10 +1307,10 @@ static void ni_ao_fifo_load(struct comedi_device *dev,
 
 		range = CR_RANGE(cmd->chanlist[chan]);
 
-		if (boardtype.reg_type & ni_reg_6xxx_mask) {
+		if (board->reg_type & ni_reg_6xxx_mask) {
 			packed_data = d & 0xffff;
 			/* 6711 only has 16 bit wide ao fifo */
-			if (boardtype.reg_type != ni_reg_6711) {
+			if (board->reg_type != ni_reg_6711) {
 				err &= comedi_buf_get(async, &d);
 				if (err == 0)
 					break;
@@ -1352,6 +1350,7 @@ static void ni_ao_fifo_load(struct comedi_device *dev,
 static int ni_ao_fifo_half_empty(struct comedi_device *dev,
 				 struct comedi_subdevice *s)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	int n;
 
 	n = comedi_buf_read_n_available(s->async);
@@ -1361,8 +1360,8 @@ static int ni_ao_fifo_half_empty(struct comedi_device *dev,
 	}
 
 	n /= sizeof(short);
-	if (n > boardtype.ao_fifo_depth / 2)
-		n = boardtype.ao_fifo_depth / 2;
+	if (n > board->ao_fifo_depth / 2)
+		n = board->ao_fifo_depth / 2;
 
 	ni_ao_fifo_load(dev, s, n);
 
@@ -1374,12 +1373,13 @@ static int ni_ao_fifo_half_empty(struct comedi_device *dev,
 static int ni_ao_prep_fifo(struct comedi_device *dev,
 			   struct comedi_subdevice *s)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 	int n;
 
 	/* reset fifo */
 	devpriv->stc_writew(dev, 1, DAC_FIFO_Clear);
-	if (boardtype.reg_type & ni_reg_6xxx_mask)
+	if (board->reg_type & ni_reg_6xxx_mask)
 		ni_ao_win_outl(dev, 0x6, AO_FIFO_Offset_Load_611x);
 
 	/* load some data */
@@ -1388,8 +1388,8 @@ static int ni_ao_prep_fifo(struct comedi_device *dev,
 		return 0;
 
 	n /= sizeof(short);
-	if (n > boardtype.ao_fifo_depth)
-		n = boardtype.ao_fifo_depth;
+	if (n > board->ao_fifo_depth)
+		n = board->ao_fifo_depth;
 
 	ni_ao_fifo_load(dev, s, n);
 
@@ -1399,11 +1399,12 @@ static int ni_ao_prep_fifo(struct comedi_device *dev,
 static void ni_ai_fifo_read(struct comedi_device *dev,
 			    struct comedi_subdevice *s, int n)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 	struct comedi_async *async = s->async;
 	int i;
 
-	if (boardtype.reg_type == ni_reg_611x) {
+	if (board->reg_type == ni_reg_611x) {
 		short data[2];
 		u32 dl;
 
@@ -1420,7 +1421,7 @@ static void ni_ai_fifo_read(struct comedi_device *dev,
 			data[0] = dl & 0xffff;
 			cfc_write_to_buffer(s, data[0]);
 		}
-	} else if (boardtype.reg_type == ni_reg_6143) {
+	} else if (board->reg_type == ni_reg_6143) {
 		short data[2];
 		u32 dl;
 
@@ -1458,10 +1459,11 @@ static void ni_ai_fifo_read(struct comedi_device *dev,
 
 static void ni_handle_fifo_half_full(struct comedi_device *dev)
 {
-	int n;
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct comedi_subdevice *s = &dev->subdevices[NI_AI_SUBDEV];
+	int n;
 
-	n = boardtype.ai_fifo_depth / 2;
+	n = board->ai_fifo_depth / 2;
 
 	ni_ai_fifo_read(dev, s, n);
 }
@@ -1508,6 +1510,7 @@ static int ni_ai_drain_dma(struct comedi_device *dev)
 */
 static void ni_handle_fifo_dregs(struct comedi_device *dev)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 	struct comedi_subdevice *s = &dev->subdevices[NI_AI_SUBDEV];
 	short data[2];
@@ -1515,7 +1518,7 @@ static void ni_handle_fifo_dregs(struct comedi_device *dev)
 	short fifo_empty;
 	int i;
 
-	if (boardtype.reg_type == ni_reg_611x) {
+	if (board->reg_type == ni_reg_611x) {
 		while ((devpriv->stc_readw(dev,
 					   AI_Status_1_Register) &
 			AI_FIFO_Empty_St) == 0) {
@@ -1526,7 +1529,7 @@ static void ni_handle_fifo_dregs(struct comedi_device *dev)
 			data[1] = (dl & 0xffff);
 			cfc_write_array_to_buffer(s, data, sizeof(data));
 		}
-	} else if (boardtype.reg_type == ni_reg_6143) {
+	} else if (board->reg_type == ni_reg_6143) {
 		i = 0;
 		while (ni_readl(AIFIFO_Status_6143) & 0x04) {
 			dl = ni_readl(AIFIFO_Data_6143);
@@ -1573,12 +1576,13 @@ static void ni_handle_fifo_dregs(struct comedi_device *dev)
 
 static void get_last_sample_611x(struct comedi_device *dev)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv __maybe_unused = dev->private;
 	struct comedi_subdevice *s = &dev->subdevices[NI_AI_SUBDEV];
 	short data;
 	u32 dl;
 
-	if (boardtype.reg_type != ni_reg_611x)
+	if (board->reg_type != ni_reg_611x)
 		return;
 
 	/* Check if there's a single sample stuck in the FIFO */
@@ -1591,12 +1595,13 @@ static void get_last_sample_611x(struct comedi_device *dev)
 
 static void get_last_sample_6143(struct comedi_device *dev)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv __maybe_unused = dev->private;
 	struct comedi_subdevice *s = &dev->subdevices[NI_AI_SUBDEV];
 	short data;
 	u32 dl;
 
-	if (boardtype.reg_type != ni_reg_6143)
+	if (board->reg_type != ni_reg_6143)
 		return;
 
 	/* Check if there's a single sample stuck in the FIFO */
@@ -1641,6 +1646,7 @@ static void ni_ai_munge(struct comedi_device *dev, struct comedi_subdevice *s,
 
 static int ni_ai_setup_MITE_dma(struct comedi_device *dev)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 	struct comedi_subdevice *s = &dev->subdevices[NI_AI_SUBDEV];
 	int retval;
@@ -1660,7 +1666,7 @@ static int ni_ai_setup_MITE_dma(struct comedi_device *dev)
 		return -EIO;
 	}
 
-	switch (boardtype.reg_type) {
+	switch (board->reg_type) {
 	case ni_reg_611x:
 	case ni_reg_6143:
 		mite_prep_dma(devpriv->ai_mite_chan, 32, 16);
@@ -1681,6 +1687,7 @@ static int ni_ai_setup_MITE_dma(struct comedi_device *dev)
 
 static int ni_ao_setup_MITE_dma(struct comedi_device *dev)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 	struct comedi_subdevice *s = &dev->subdevices[NI_AO_SUBDEV];
 	int retval;
@@ -1695,7 +1702,7 @@ static int ni_ao_setup_MITE_dma(struct comedi_device *dev)
 
 	spin_lock_irqsave(&devpriv->mite_channel_lock, flags);
 	if (devpriv->ao_mite_chan) {
-		if (boardtype.reg_type & (ni_reg_611x | ni_reg_6713)) {
+		if (board->reg_type & (ni_reg_611x | ni_reg_6713)) {
 			mite_prep_dma(devpriv->ao_mite_chan, 32, 32);
 		} else {
 			/* doing 32 instead of 16 bit wide transfers from memory
@@ -1720,6 +1727,7 @@ static int ni_ao_setup_MITE_dma(struct comedi_device *dev)
 
 static int ni_ai_reset(struct comedi_device *dev, struct comedi_subdevice *s)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 
 	ni_release_ai_mite_channel(dev);
@@ -1735,7 +1743,7 @@ static int ni_ai_reset(struct comedi_device *dev, struct comedi_subdevice *s)
 
 	ni_clear_ai_fifo(dev);
 
-	if (boardtype.reg_type != ni_reg_6143)
+	if (board->reg_type != ni_reg_6143)
 		ni_writeb(0, Misc_Command);
 
 	devpriv->stc_writew(dev, AI_Disarm, AI_Command_1_Register);	/* reset pulses */
@@ -1746,7 +1754,7 @@ static int ni_ai_reset(struct comedi_device *dev, struct comedi_subdevice *s)
 	devpriv->stc_writew(dev, 0x0000, AI_Mode_2_Register);
 	/* generate FIFO interrupts on non-empty */
 	devpriv->stc_writew(dev, (0 << 6) | 0x0000, AI_Mode_3_Register);
-	if (boardtype.reg_type == ni_reg_611x) {
+	if (board->reg_type == ni_reg_611x) {
 		devpriv->stc_writew(dev, AI_SHIFTIN_Pulse_Width |
 				    AI_SOC_Polarity |
 				    AI_LOCALMUX_CLK_Pulse_Width,
@@ -1759,7 +1767,7 @@ static int ni_ai_reset(struct comedi_device *dev, struct comedi_subdevice *s)
 				    AI_CONVERT_Output_Select
 				    (AI_CONVERT_Output_Enable_High),
 				    AI_Output_Control_Register);
-	} else if (boardtype.reg_type == ni_reg_6143) {
+	} else if (board->reg_type == ni_reg_6143) {
 		devpriv->stc_writew(dev, AI_SHIFTIN_Pulse_Width |
 				    AI_SOC_Polarity |
 				    AI_LOCALMUX_CLK_Pulse_Width,
@@ -1784,7 +1792,7 @@ static int ni_ai_reset(struct comedi_device *dev, struct comedi_subdevice *s)
 		    AI_EXTMUX_CLK_Output_Select(0) |
 		    AI_LOCALMUX_CLK_Output_Select(2) |
 		    AI_SC_TC_Output_Select(3);
-		if (boardtype.reg_type == ni_reg_622x)
+		if (board->reg_type == ni_reg_622x)
 			ai_output_control_bits |=
 			    AI_CONVERT_Output_Select
 			    (AI_CONVERT_Output_Enable_High);
@@ -1832,9 +1840,10 @@ static int ni_ai_insn_read(struct comedi_device *dev,
 			   struct comedi_subdevice *s, struct comedi_insn *insn,
 			   unsigned int *data)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 	int i, n;
-	const unsigned int mask = (1 << boardtype.adbits) - 1;
+	const unsigned int mask = (1 << board->adbits) - 1;
 	unsigned signbits;
 	unsigned short d;
 	unsigned long dl;
@@ -1844,7 +1853,7 @@ static int ni_ai_insn_read(struct comedi_device *dev,
 	ni_clear_ai_fifo(dev);
 
 	signbits = devpriv->ai_offset[0];
-	if (boardtype.reg_type == ni_reg_611x) {
+	if (board->reg_type == ni_reg_611x) {
 		for (n = 0; n < num_adc_stages_611x; n++) {
 			devpriv->stc_writew(dev, AI_CONVERT_Pulse,
 					    AI_Command_1_Register);
@@ -1877,7 +1886,7 @@ static int ni_ai_insn_read(struct comedi_device *dev,
 			d += signbits;
 			data[n] = d;
 		}
-	} else if (boardtype.reg_type == ni_reg_6143) {
+	} else if (board->reg_type == ni_reg_6143) {
 		for (n = 0; n < insn->n; n++) {
 			devpriv->stc_writew(dev, AI_CONVERT_Pulse,
 					    AI_Command_1_Register);
@@ -1913,7 +1922,7 @@ static int ni_ai_insn_read(struct comedi_device *dev,
 				    ("ni_mio_common: timeout in ni_ai_insn_read\n");
 				return -ETIME;
 			}
-			if (boardtype.reg_type & ni_reg_m_series_mask) {
+			if (board->reg_type & ni_reg_m_series_mask) {
 				data[n] =
 				    ni_readl(M_Offset_AI_FIFO_Data) & mask;
 			} else {
@@ -1948,6 +1957,7 @@ static void ni_m_series_load_channelgain_list(struct comedi_device *dev,
 					      unsigned int n_chan,
 					      unsigned int *list)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 	unsigned int chan, range, aref;
 	unsigned int i;
@@ -1957,12 +1967,12 @@ static void ni_m_series_load_channelgain_list(struct comedi_device *dev,
 
 	devpriv->stc_writew(dev, 1, Configuration_Memory_Clear);
 
-/* offset = 1 << (boardtype.adbits - 1); */
+/* offset = 1 << (board->adbits - 1); */
 	if ((list[0] & CR_ALT_SOURCE)) {
 		unsigned bypass_bits;
 		chan = CR_CHAN(list[0]);
 		range = CR_RANGE(list[0]);
-		range_code = ni_gainlkup[boardtype.gainlkup][range];
+		range_code = ni_gainlkup[board->gainlkup][range];
 		dither = ((list[0] & CR_ALT_FILTER) != 0);
 		bypass_bits = MSeries_AI_Bypass_Config_FIFO_Bit;
 		bypass_bits |= chan;
@@ -1989,7 +1999,7 @@ static void ni_m_series_load_channelgain_list(struct comedi_device *dev,
 		range = CR_RANGE(list[i]);
 		dither = ((list[i] & CR_ALT_FILTER) != 0);
 
-		range_code = ni_gainlkup[boardtype.gainlkup][range];
+		range_code = ni_gainlkup[board->gainlkup][range];
 		devpriv->ai_offset[i] = offset;
 		switch (aref) {
 		case AREF_DIFF:
@@ -2009,7 +2019,7 @@ static void ni_m_series_load_channelgain_list(struct comedi_device *dev,
 		}
 		config_bits |= MSeries_AI_Config_Channel_Bits(chan);
 		config_bits |=
-		    MSeries_AI_Config_Bank_Bits(boardtype.reg_type, chan);
+		    MSeries_AI_Config_Bank_Bits(board->reg_type, chan);
 		config_bits |= MSeries_AI_Config_Gain_Bits(range_code);
 		if (i == n_chan - 1)
 			config_bits |= MSeries_AI_Config_Last_Channel_Bit;
@@ -2054,6 +2064,7 @@ static void ni_m_series_load_channelgain_list(struct comedi_device *dev,
 static void ni_load_channelgain_list(struct comedi_device *dev,
 				     unsigned int n_chan, unsigned int *list)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 	unsigned int chan, range, aref;
 	unsigned int i;
@@ -2061,12 +2072,12 @@ static void ni_load_channelgain_list(struct comedi_device *dev,
 	unsigned offset;
 	unsigned int dither;
 
-	if (boardtype.reg_type & ni_reg_m_series_mask) {
+	if (board->reg_type & ni_reg_m_series_mask) {
 		ni_m_series_load_channelgain_list(dev, n_chan, list);
 		return;
 	}
-	if (n_chan == 1 && (boardtype.reg_type != ni_reg_611x)
-	    && (boardtype.reg_type != ni_reg_6143)) {
+	if (n_chan == 1 && (board->reg_type != ni_reg_611x)
+	    && (board->reg_type != ni_reg_6143)) {
 		if (devpriv->changain_state
 		    && devpriv->changain_spec == list[0]) {
 			/*  ready to go. */
@@ -2081,7 +2092,7 @@ static void ni_load_channelgain_list(struct comedi_device *dev,
 	devpriv->stc_writew(dev, 1, Configuration_Memory_Clear);
 
 	/*  Set up Calibration mode if required */
-	if (boardtype.reg_type == ni_reg_6143) {
+	if (board->reg_type == ni_reg_6143) {
 		if ((list[0] & CR_ALT_SOURCE)
 		    && !devpriv->ai_calib_source_enabled) {
 			/*  Strobe Relay enable bit */
@@ -2105,9 +2116,9 @@ static void ni_load_channelgain_list(struct comedi_device *dev,
 		}
 	}
 
-	offset = 1 << (boardtype.adbits - 1);
+	offset = 1 << (board->adbits - 1);
 	for (i = 0; i < n_chan; i++) {
-		if ((boardtype.reg_type != ni_reg_6143)
+		if ((board->reg_type != ni_reg_6143)
 		    && (list[i] & CR_ALT_SOURCE)) {
 			chan = devpriv->ai_calib_source;
 		} else {
@@ -2118,21 +2129,21 @@ static void ni_load_channelgain_list(struct comedi_device *dev,
 		dither = ((list[i] & CR_ALT_FILTER) != 0);
 
 		/* fix the external/internal range differences */
-		range = ni_gainlkup[boardtype.gainlkup][range];
-		if (boardtype.reg_type == ni_reg_611x)
+		range = ni_gainlkup[board->gainlkup][range];
+		if (board->reg_type == ni_reg_611x)
 			devpriv->ai_offset[i] = offset;
 		else
 			devpriv->ai_offset[i] = (range & 0x100) ? 0 : offset;
 
 		hi = 0;
 		if ((list[i] & CR_ALT_SOURCE)) {
-			if (boardtype.reg_type == ni_reg_611x)
+			if (board->reg_type == ni_reg_611x)
 				ni_writew(CR_CHAN(list[i]) & 0x0003,
 					  Calibration_Channel_Select_611x);
 		} else {
-			if (boardtype.reg_type == ni_reg_611x)
+			if (board->reg_type == ni_reg_611x)
 				aref = AREF_DIFF;
-			else if (boardtype.reg_type == ni_reg_6143)
+			else if (board->reg_type == ni_reg_6143)
 				aref = AREF_OTHER;
 			switch (aref) {
 			case AREF_DIFF:
@@ -2152,7 +2163,7 @@ static void ni_load_channelgain_list(struct comedi_device *dev,
 
 		ni_writew(hi, Configuration_Memory_High);
 
-		if (boardtype.reg_type != ni_reg_6143) {
+		if (board->reg_type != ni_reg_6143) {
 			lo = range;
 			if (i == n_chan - 1)
 				lo |= AI_LAST_CHANNEL;
@@ -2164,8 +2175,8 @@ static void ni_load_channelgain_list(struct comedi_device *dev,
 	}
 
 	/* prime the channel/gain list */
-	if ((boardtype.reg_type != ni_reg_611x)
-	    && (boardtype.reg_type != ni_reg_6143)) {
+	if ((board->reg_type != ni_reg_611x)
+	    && (board->reg_type != ni_reg_6143)) {
 		ni_prime_channelgain_list(dev);
 	}
 }
@@ -2201,22 +2212,25 @@ static unsigned ni_timer_to_ns(const struct comedi_device *dev, int timer)
 static unsigned ni_min_ai_scan_period_ns(struct comedi_device *dev,
 					 unsigned num_channels)
 {
-	switch (boardtype.reg_type) {
+	const struct ni_board_struct *board = comedi_board(dev);
+
+	switch (board->reg_type) {
 	case ni_reg_611x:
 	case ni_reg_6143:
 		/*  simultaneously-sampled inputs */
-		return boardtype.ai_speed;
+		return board->ai_speed;
 		break;
 	default:
 		/*  multiplexed inputs */
 		break;
 	}
-	return boardtype.ai_speed * num_channels;
+	return board->ai_speed * num_channels;
 }
 
 static int ni_ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 			 struct comedi_cmd *cmd)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 	int err = 0;
 	int tmp;
@@ -2233,8 +2247,8 @@ static int ni_ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 					TRIG_TIMER | TRIG_EXT);
 
 	sources = TRIG_TIMER | TRIG_EXT;
-	if (boardtype.reg_type == ni_reg_611x ||
-	    boardtype.reg_type == ni_reg_6143)
+	if (board->reg_type == ni_reg_611x ||
+	    board->reg_type == ni_reg_6143)
 		sources |= TRIG_NOW;
 	err |= cfc_check_trigger_src(&cmd->convert_src, sources);
 
@@ -2289,12 +2303,12 @@ static int ni_ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	}
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		if ((boardtype.reg_type == ni_reg_611x)
-		    || (boardtype.reg_type == ni_reg_6143)) {
+		if ((board->reg_type == ni_reg_611x)
+		    || (board->reg_type == ni_reg_6143)) {
 			err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
 		} else {
 			err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
-							 boardtype.ai_speed);
+							 board->ai_speed);
 			err |= cfc_check_trigger_arg_max(&cmd->convert_arg,
 						devpriv->clock_ns * 0xffff);
 		}
@@ -2315,7 +2329,7 @@ static int ni_ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	if (cmd->stop_src == TRIG_COUNT) {
 		unsigned int max_count = 0x01000000;
 
-		if (boardtype.reg_type == ni_reg_611x)
+		if (board->reg_type == ni_reg_611x)
 			max_count -= num_adc_stages_611x;
 		err |= cfc_check_trigger_arg_max(&cmd->stop_arg, max_count);
 		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
@@ -2341,8 +2355,8 @@ static int ni_ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 			err++;
 	}
 	if (cmd->convert_src == TRIG_TIMER) {
-		if ((boardtype.reg_type != ni_reg_611x)
-		    && (boardtype.reg_type != ni_reg_6143)) {
+		if ((board->reg_type != ni_reg_611x)
+		    && (board->reg_type != ni_reg_6143)) {
 			tmp = cmd->convert_arg;
 			cmd->convert_arg =
 			    ni_timer_to_ns(dev, ni_ns_to_timer(dev,
@@ -2370,6 +2384,7 @@ static int ni_ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 static int ni_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 	const struct comedi_cmd *cmd = &s->async->cmd;
 	int timer;
@@ -2426,8 +2441,8 @@ static int ni_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	mode2 &= ~AI_SC_Reload_Mode;
 	devpriv->stc_writew(dev, mode2, AI_Mode_2_Register);
 
-	if (cmd->chanlist_len == 1 || (boardtype.reg_type == ni_reg_611x)
-	    || (boardtype.reg_type == ni_reg_6143)) {
+	if (cmd->chanlist_len == 1 || (board->reg_type == ni_reg_611x)
+	    || (board->reg_type == ni_reg_6143)) {
 		start_stop_select |= AI_STOP_Polarity;
 		start_stop_select |= AI_STOP_Select(31);	/*  logic low */
 		start_stop_select |= AI_STOP_Sync;
@@ -2442,7 +2457,7 @@ static int ni_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	case TRIG_COUNT:
 		stop_count = cmd->stop_arg - 1;
 
-		if (boardtype.reg_type == ni_reg_611x) {
+		if (board->reg_type == ni_reg_611x) {
 			/*  have to take 3 stage adc pipeline into account */
 			stop_count += num_adc_stages_611x;
 		}
@@ -2698,6 +2713,7 @@ static int ni_ai_insn_config(struct comedi_device *dev,
 			     struct comedi_subdevice *s,
 			     struct comedi_insn *insn, unsigned int *data)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 
 	if (insn->n < 1)
@@ -2707,7 +2723,7 @@ static int ni_ai_insn_config(struct comedi_device *dev,
 	case INSN_CONFIG_ANALOG_TRIG:
 		return ni_ai_config_analog_trig(dev, s, insn, data);
 	case INSN_CONFIG_ALT_SOURCE:
-		if (boardtype.reg_type & ni_reg_m_series_mask) {
+		if (board->reg_type & ni_reg_m_series_mask) {
 			if (data[1] & ~(MSeries_AI_Bypass_Cal_Sel_Pos_Mask |
 					MSeries_AI_Bypass_Cal_Sel_Neg_Mask |
 					MSeries_AI_Bypass_Mode_Mux_Mask |
@@ -2715,7 +2731,7 @@ static int ni_ai_insn_config(struct comedi_device *dev,
 				return -EINVAL;
 			}
 			devpriv->ai_calib_source = data[1];
-		} else if (boardtype.reg_type == ni_reg_6143) {
+		} else if (board->reg_type == ni_reg_6143) {
 			unsigned int calib_source;
 
 			calib_source = data[1] & 0xf;
@@ -2735,7 +2751,7 @@ static int ni_ai_insn_config(struct comedi_device *dev,
 			if (calib_source >= 8)
 				return -EINVAL;
 			devpriv->ai_calib_source = calib_source;
-			if (boardtype.reg_type == ni_reg_611x) {
+			if (board->reg_type == ni_reg_611x) {
 				ni_writeb(calib_source_adjust,
 					  Cal_Gain_Select_611x);
 			}
@@ -2753,6 +2769,7 @@ static int ni_ai_config_analog_trig(struct comedi_device *dev,
 				    struct comedi_insn *insn,
 				    unsigned int *data)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 	unsigned int a, b, modebits;
 	int err = 0;
@@ -2761,14 +2778,14 @@ static int ni_ai_config_analog_trig(struct comedi_device *dev,
 	 * data[2] is analog line
 	 * data[3] is set level
 	 * data[4] is reset level */
-	if (!boardtype.has_analog_trig)
+	if (!board->has_analog_trig)
 		return -EINVAL;
 	if ((data[1] & 0xffff0000) != COMEDI_EV_SCAN_BEGIN) {
 		data[1] &= (COMEDI_EV_SCAN_BEGIN | 0xffff);
 		err++;
 	}
-	if (data[2] >= boardtype.n_adchan) {
-		data[2] = boardtype.n_adchan - 1;
+	if (data[2] >= board->n_adchan) {
+		data[2] = board->n_adchan - 1;
 		err++;
 	}
 	if (data[3] > 255) {	/* a */
@@ -2852,6 +2869,7 @@ static void ni_ao_munge(struct comedi_device *dev, struct comedi_subdevice *s,
 			void *data, unsigned int num_bytes,
 			unsigned int chan_index)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct comedi_async *async = s->async;
 	unsigned int range;
 	unsigned int i;
@@ -2859,10 +2877,10 @@ static void ni_ao_munge(struct comedi_device *dev, struct comedi_subdevice *s,
 	unsigned int length = num_bytes / sizeof(short);
 	short *array = data;
 
-	offset = 1 << (boardtype.aobits - 1);
+	offset = 1 << (board->aobits - 1);
 	for (i = 0; i < length; i++) {
 		range = CR_RANGE(async->cmd.chanlist[chan_index]);
-		if (boardtype.ao_unipolar == 0 || (range & 1) == 0)
+		if (board->ao_unipolar == 0 || (range & 1) == 0)
 			array[i] -= offset;
 #ifdef PCIDMA
 		array[i] = cpu_to_le16(array[i]);
@@ -2877,6 +2895,7 @@ static int ni_m_series_ao_config_chanlist(struct comedi_device *dev,
 					  unsigned int chanspec[],
 					  unsigned int n_chans, int timed)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 	unsigned int range;
 	unsigned int chan;
@@ -2885,7 +2904,7 @@ static int ni_m_series_ao_config_chanlist(struct comedi_device *dev,
 	int invert = 0;
 
 	if (timed) {
-		for (i = 0; i < boardtype.n_aochan; ++i) {
+		for (i = 0; i < board->n_aochan; ++i) {
 			devpriv->ao_conf[i] &= ~MSeries_AO_Update_Timed_Bit;
 			ni_writeb(devpriv->ao_conf[i],
 				  M_Offset_AO_Config_Bank(i));
@@ -2949,6 +2968,7 @@ static int ni_old_ao_config_chanlist(struct comedi_device *dev,
 				     unsigned int chanspec[],
 				     unsigned int n_chans)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 	unsigned int range;
 	unsigned int chan;
@@ -2961,10 +2981,10 @@ static int ni_old_ao_config_chanlist(struct comedi_device *dev,
 		range = CR_RANGE(chanspec[i]);
 		conf = AO_Channel(chan);
 
-		if (boardtype.ao_unipolar) {
+		if (board->ao_unipolar) {
 			if ((range & 1) == 0) {
 				conf |= AO_Bipolar;
-				invert = (1 << (boardtype.aobits - 1));
+				invert = (1 << (board->aobits - 1));
 			} else {
 				invert = 0;
 			}
@@ -2972,7 +2992,7 @@ static int ni_old_ao_config_chanlist(struct comedi_device *dev,
 				conf |= AO_Ext_Ref;
 		} else {
 			conf |= AO_Bipolar;
-			invert = (1 << (boardtype.aobits - 1));
+			invert = (1 << (board->aobits - 1));
 		}
 
 		/* not all boards can deglitch, but this shouldn't hurt */
@@ -2995,7 +3015,9 @@ static int ni_ao_config_chanlist(struct comedi_device *dev,
 				 unsigned int chanspec[], unsigned int n_chans,
 				 int timed)
 {
-	if (boardtype.reg_type & ni_reg_m_series_mask)
+	const struct ni_board_struct *board = comedi_board(dev);
+
+	if (board->reg_type & ni_reg_m_series_mask)
 		return ni_m_series_ao_config_chanlist(dev, s, chanspec, n_chans,
 						      timed);
 	else
@@ -3017,6 +3039,7 @@ static int ni_ao_insn_write(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
 	unsigned int invert;
@@ -3025,7 +3048,7 @@ static int ni_ao_insn_write(struct comedi_device *dev,
 
 	devpriv->ao[chan] = data[0];
 
-	if (boardtype.reg_type & ni_reg_m_series_mask) {
+	if (board->reg_type & ni_reg_m_series_mask) {
 		ni_writew(data[0], M_Offset_DAC_Direct_Data(chan));
 	} else
 		ni_writew(data[0] ^ invert,
@@ -3038,12 +3061,13 @@ static int ni_ao_insn_write_671x(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
 				 struct comedi_insn *insn, unsigned int *data)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
 	unsigned int invert;
 
 	ao_win_out(1 << chan, AO_Immediate_671x);
-	invert = 1 << (boardtype.aobits - 1);
+	invert = 1 << (board->aobits - 1);
 
 	ni_ao_config_chanlist(dev, s, &insn->chanspec, 1, 0);
 
@@ -3057,13 +3081,14 @@ static int ni_ao_insn_config(struct comedi_device *dev,
 			     struct comedi_subdevice *s,
 			     struct comedi_insn *insn, unsigned int *data)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 
 	switch (data[0]) {
 	case INSN_CONFIG_GET_HARDWARE_BUFFER_SIZE:
 		switch (data[1]) {
 		case COMEDI_OUTPUT:
-			data[2] = 1 + boardtype.ao_fifo_depth * sizeof(short);
+			data[2] = 1 + board->ao_fifo_depth * sizeof(short);
 			if (devpriv->mite)
 				data[2] += devpriv->mite->fifo_size;
 			break;
@@ -3085,6 +3110,7 @@ static int ni_ao_insn_config(struct comedi_device *dev,
 static int ni_ao_inttrig(struct comedi_device *dev, struct comedi_subdevice *s,
 			 unsigned int trignum)
 {
+	const struct ni_board_struct *board __maybe_unused = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 	int ret;
 	int interrupt_b_bits;
@@ -3104,7 +3130,7 @@ static int ni_ao_inttrig(struct comedi_device *dev, struct comedi_subdevice *s,
 	interrupt_b_bits = AO_Error_Interrupt_Enable;
 #ifdef PCIDMA
 	devpriv->stc_writew(dev, 1, DAC_FIFO_Clear);
-	if (boardtype.reg_type & ni_reg_6xxx_mask)
+	if (board->reg_type & ni_reg_6xxx_mask)
 		ni_ao_win_outl(dev, 0x6, AO_FIFO_Offset_Load_611x);
 	ret = ni_ao_setup_MITE_dma(dev);
 	if (ret)
@@ -3155,6 +3181,7 @@ static int ni_ao_inttrig(struct comedi_device *dev, struct comedi_subdevice *s,
 
 static int ni_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 	const struct comedi_cmd *cmd = &s->async->cmd;
 	int bits;
@@ -3170,7 +3197,7 @@ static int ni_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 	devpriv->stc_writew(dev, AO_Disarm, AO_Command_1_Register);
 
-	if (boardtype.reg_type & ni_reg_6xxx_mask) {
+	if (board->reg_type & ni_reg_6xxx_mask) {
 		ao_win_out(CLEAR_WG, AO_Misc_611x);
 
 		bits = 0;
@@ -3233,7 +3260,7 @@ static int ni_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	devpriv->stc_writew(dev, devpriv->ao_mode2, AO_Mode_2_Register);
 	switch (cmd->stop_src) {
 	case TRIG_COUNT:
-		if (boardtype.reg_type & ni_reg_m_series_mask) {
+		if (board->reg_type & ni_reg_m_series_mask) {
 			/*  this is how the NI example code does it for m-series boards, verified correct with 6259 */
 			devpriv->stc_writel(dev, cmd->stop_arg - 1,
 					    AO_UC_Load_A_Register);
@@ -3301,8 +3328,8 @@ static int ni_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		unsigned bits;
 		devpriv->ao_mode1 &= ~AO_Multiple_Channels;
 		bits = AO_UPDATE_Output_Select(AO_Update_Output_High_Z);
-		if (boardtype.
-		    reg_type & (ni_reg_m_series_mask | ni_reg_6xxx_mask)) {
+		if (board->reg_type &
+		    (ni_reg_m_series_mask | ni_reg_6xxx_mask)) {
 			bits |= AO_Number_Of_Channels(0);
 		} else {
 			bits |=
@@ -3329,14 +3356,14 @@ static int ni_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 	bits = AO_BC_Source_Select | AO_UPDATE_Pulse_Width |
 	    AO_TMRDACWR_Pulse_Width;
-	if (boardtype.ao_fifo_depth)
+	if (board->ao_fifo_depth)
 		bits |= AO_FIFO_Enable;
 	else
 		bits |= AO_DMA_PIO_Control;
 #if 0
 	/* F Hess: windows driver does not set AO_Number_Of_DAC_Packages bit for 6281,
 	   verified with bus analyzer. */
-	if (boardtype.reg_type & ni_reg_m_series_mask)
+	if (board->reg_type & ni_reg_m_series_mask)
 		bits |= AO_Number_Of_DAC_Packages;
 #endif
 	devpriv->stc_writew(dev, bits, AO_Personal_Register);
@@ -3360,6 +3387,7 @@ static int ni_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 static int ni_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 			 struct comedi_cmd *cmd)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 	int err = 0;
 	int tmp;
@@ -3407,7 +3435,7 @@ static int ni_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-						 boardtype.ao_speed);
+						 board->ao_speed);
 		err |= cfc_check_trigger_arg_max(&cmd->scan_begin_arg,
 						 devpriv->clock_ns * 0xffffff);
 	}
@@ -3448,6 +3476,7 @@ static int ni_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 static int ni_ao_reset(struct comedi_device *dev, struct comedi_subdevice *s)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 
 	/* devpriv->ao0p=0x0000; */
@@ -3475,7 +3504,7 @@ static int ni_ao_reset(struct comedi_device *dev, struct comedi_subdevice *s)
 	devpriv->stc_writew(dev, devpriv->ao_mode1, AO_Mode_1_Register);
 	devpriv->ao_mode2 = 0;
 	devpriv->stc_writew(dev, devpriv->ao_mode2, AO_Mode_2_Register);
-	if (boardtype.reg_type & ni_reg_m_series_mask)
+	if (board->reg_type & ni_reg_m_series_mask)
 		devpriv->ao_mode3 = AO_Last_Gate_Disable;
 	else
 		devpriv->ao_mode3 = 0;
@@ -3483,7 +3512,7 @@ static int ni_ao_reset(struct comedi_device *dev, struct comedi_subdevice *s)
 	devpriv->ao_trigger_select = 0;
 	devpriv->stc_writew(dev, devpriv->ao_trigger_select,
 			    AO_Trigger_Select_Register);
-	if (boardtype.reg_type & ni_reg_6xxx_mask) {
+	if (board->reg_type & ni_reg_6xxx_mask) {
 		unsigned immediate_bits = 0;
 		unsigned i;
 		for (i = 0; i < s->n_chan; ++i) {
@@ -3784,6 +3813,7 @@ static int ni_cdio_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 
 static void handle_cdio_interrupt(struct comedi_device *dev)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv __maybe_unused = dev->private;
 	unsigned cdio_status;
 	struct comedi_subdevice *s = &dev->subdevices[NI_DIO_SUBDEV];
@@ -3791,7 +3821,7 @@ static void handle_cdio_interrupt(struct comedi_device *dev)
 	unsigned long flags;
 #endif
 
-	if ((boardtype.reg_type & ni_reg_m_series_mask) == 0) {
+	if ((board->reg_type & ni_reg_m_series_mask) == 0) {
 		return;
 	}
 #ifdef PCIDMA
@@ -4039,17 +4069,13 @@ static int ni_serial_sw_readwrite8(struct comedi_device *dev,
 static void mio_common_detach(struct comedi_device *dev)
 {
 	struct ni_private *devpriv = dev->private;
-	struct comedi_subdevice *s;
 
 	if (devpriv) {
 		if (devpriv->counter_dev) {
 			ni_gpct_device_destroy(devpriv->counter_dev);
 		}
 	}
-	if (dev->subdevices && boardtype.has_8255) {
-		s = &dev->subdevices[NI_8255_DIO_SUBDEV];
-		subdev_8255_cleanup(dev, s);
-	}
+	comedi_spriv_free(dev, NI_8255_DIO_SUBDEV);
 }
 
 static void init_ao_67xx(struct comedi_device *dev, struct comedi_subdevice *s)
@@ -4355,14 +4381,15 @@ static int ni_alloc_private(struct comedi_device *dev)
 
 static int ni_E_init(struct comedi_device *dev)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 	struct comedi_subdevice *s;
 	unsigned j;
 	enum ni_gpct_variant counter_variant;
 	int ret;
 
-	if (boardtype.n_aochan > MAX_N_AO_CHAN) {
-		printk("bug! boardtype.n_aochan > MAX_N_AO_CHAN\n");
+	if (board->n_aochan > MAX_N_AO_CHAN) {
+		printk("bug! n_aochan > MAX_N_AO_CHAN\n");
 		return -EINVAL;
 	}
 
@@ -4374,20 +4401,20 @@ static int ni_E_init(struct comedi_device *dev)
 
 	s = &dev->subdevices[NI_AI_SUBDEV];
 	dev->read_subdev = s;
-	if (boardtype.n_adchan) {
+	if (board->n_adchan) {
 		s->type = COMEDI_SUBD_AI;
 		s->subdev_flags =
 		    SDF_READABLE | SDF_DIFF | SDF_DITHER | SDF_CMD_READ;
-		if (boardtype.reg_type != ni_reg_611x)
+		if (board->reg_type != ni_reg_611x)
 			s->subdev_flags |= SDF_GROUND | SDF_COMMON | SDF_OTHER;
-		if (boardtype.adbits > 16)
+		if (board->adbits > 16)
 			s->subdev_flags |= SDF_LSAMPL;
-		if (boardtype.reg_type & ni_reg_m_series_mask)
+		if (board->reg_type & ni_reg_m_series_mask)
 			s->subdev_flags |= SDF_SOFT_CALIBRATED;
-		s->n_chan = boardtype.n_adchan;
+		s->n_chan = board->n_adchan;
 		s->len_chanlist = 512;
-		s->maxdata = (1 << boardtype.adbits) - 1;
-		s->range_table = ni_range_lkup[boardtype.gainlkup];
+		s->maxdata = (1 << board->adbits) - 1;
+		s->range_table = ni_range_lkup[board->gainlkup];
 		s->insn_read = &ni_ai_insn_read;
 		s->insn_config = &ni_ai_insn_config;
 		s->do_cmdtest = &ni_ai_cmdtest;
@@ -4405,40 +4432,40 @@ static int ni_E_init(struct comedi_device *dev)
 	/* analog output subdevice */
 
 	s = &dev->subdevices[NI_AO_SUBDEV];
-	if (boardtype.n_aochan) {
+	if (board->n_aochan) {
 		s->type = COMEDI_SUBD_AO;
 		s->subdev_flags = SDF_WRITABLE | SDF_DEGLITCH | SDF_GROUND;
-		if (boardtype.reg_type & ni_reg_m_series_mask)
+		if (board->reg_type & ni_reg_m_series_mask)
 			s->subdev_flags |= SDF_SOFT_CALIBRATED;
-		s->n_chan = boardtype.n_aochan;
-		s->maxdata = (1 << boardtype.aobits) - 1;
-		s->range_table = boardtype.ao_range_table;
+		s->n_chan = board->n_aochan;
+		s->maxdata = (1 << board->aobits) - 1;
+		s->range_table = board->ao_range_table;
 		s->insn_read = &ni_ao_insn_read;
-		if (boardtype.reg_type & ni_reg_6xxx_mask) {
+		if (board->reg_type & ni_reg_6xxx_mask) {
 			s->insn_write = &ni_ao_insn_write_671x;
 		} else {
 			s->insn_write = &ni_ao_insn_write;
 		}
 		s->insn_config = &ni_ao_insn_config;
 #ifdef PCIDMA
-		if (boardtype.n_aochan) {
+		if (board->n_aochan) {
 			s->async_dma_dir = DMA_TO_DEVICE;
 #else
-		if (boardtype.ao_fifo_depth) {
+		if (board->ao_fifo_depth) {
 #endif
 			dev->write_subdev = s;
 			s->subdev_flags |= SDF_CMD_WRITE;
 			s->do_cmd = &ni_ao_cmd;
 			s->do_cmdtest = &ni_ao_cmdtest;
-			s->len_chanlist = boardtype.n_aochan;
-			if ((boardtype.reg_type & ni_reg_m_series_mask) == 0)
+			s->len_chanlist = board->n_aochan;
+			if ((board->reg_type & ni_reg_m_series_mask) == 0)
 				s->munge = ni_ao_munge;
 		}
 		s->cancel = &ni_ao_reset;
 	} else {
 		s->type = COMEDI_SUBD_UNUSED;
 	}
-	if ((boardtype.reg_type & ni_reg_67xx_mask))
+	if ((board->reg_type & ni_reg_67xx_mask))
 		init_ao_67xx(dev, s);
 
 	/* digital i/o subdevice */
@@ -4449,8 +4476,8 @@ static int ni_E_init(struct comedi_device *dev)
 	s->maxdata = 1;
 	s->io_bits = 0;		/* all bits input */
 	s->range_table = &range_digital;
-	s->n_chan = boardtype.num_p0_dio_channels;
-	if (boardtype.reg_type & ni_reg_m_series_mask) {
+	s->n_chan = board->num_p0_dio_channels;
+	if (board->reg_type & ni_reg_m_series_mask) {
 		s->subdev_flags |=
 		    SDF_LSAMPL | SDF_CMD_WRITE /* | SDF_CMD_READ */ ;
 		s->insn_bits = &ni_m_series_dio_insn_bits;
@@ -4472,7 +4499,7 @@ static int ni_E_init(struct comedi_device *dev)
 
 	/* 8255 device */
 	s = &dev->subdevices[NI_8255_DIO_SUBDEV];
-	if (boardtype.has_8255) {
+	if (board->has_8255) {
 		subdev_8255_init(dev, s, ni_8255_callback, (unsigned long)dev);
 	} else {
 		s->type = COMEDI_SUBD_UNUSED;
@@ -4485,14 +4512,14 @@ static int ni_E_init(struct comedi_device *dev)
 	/* calibration subdevice -- ai and ao */
 	s = &dev->subdevices[NI_CALIBRATION_SUBDEV];
 	s->type = COMEDI_SUBD_CALIB;
-	if (boardtype.reg_type & ni_reg_m_series_mask) {
+	if (board->reg_type & ni_reg_m_series_mask) {
 		/*  internal PWM analog output used for AI nonlinearity calibration */
 		s->subdev_flags = SDF_INTERNAL;
 		s->insn_config = &ni_m_series_pwm_config;
 		s->n_chan = 1;
 		s->maxdata = 0;
 		ni_writel(0x0, M_Offset_Cal_PWM);
-	} else if (boardtype.reg_type == ni_reg_6143) {
+	} else if (board->reg_type == ni_reg_6143) {
 		/*  internal PWM analog output used for AI nonlinearity calibration */
 		s->subdev_flags = SDF_INTERNAL;
 		s->insn_config = &ni_6143_pwm_config;
@@ -4510,7 +4537,7 @@ static int ni_E_init(struct comedi_device *dev)
 	s->type = COMEDI_SUBD_MEMORY;
 	s->subdev_flags = SDF_READABLE | SDF_INTERNAL;
 	s->maxdata = 0xff;
-	if (boardtype.reg_type & ni_reg_m_series_mask) {
+	if (board->reg_type & ni_reg_m_series_mask) {
 		s->n_chan = M_SERIES_EEPROM_SIZE;
 		s->insn_read = &ni_m_series_eeprom_insn_read;
 	} else {
@@ -4522,7 +4549,7 @@ static int ni_E_init(struct comedi_device *dev)
 	s = &dev->subdevices[NI_PFI_DIO_SUBDEV];
 	s->type = COMEDI_SUBD_DIO;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
-	if (boardtype.reg_type & ni_reg_m_series_mask) {
+	if (board->reg_type & ni_reg_m_series_mask) {
 		unsigned i;
 		s->n_chan = 16;
 		ni_writew(s->state, M_Offset_PFI_DO);
@@ -4534,7 +4561,7 @@ static int ni_E_init(struct comedi_device *dev)
 		s->n_chan = 10;
 	}
 	s->maxdata = 1;
-	if (boardtype.reg_type & ni_reg_m_series_mask) {
+	if (board->reg_type & ni_reg_m_series_mask) {
 		s->insn_bits = &ni_pfi_insn_bits;
 	}
 	s->insn_config = &ni_pfi_insn_config;
@@ -4542,11 +4569,11 @@ static int ni_E_init(struct comedi_device *dev)
 
 	/* cs5529 calibration adc */
 	s = &dev->subdevices[NI_CS5529_CALIBRATION_SUBDEV];
-	if (boardtype.reg_type & ni_reg_67xx_mask) {
+	if (board->reg_type & ni_reg_67xx_mask) {
 		s->type = COMEDI_SUBD_AI;
 		s->subdev_flags = SDF_READABLE | SDF_DIFF | SDF_INTERNAL;
 		/*  one channel for each analog output channel */
-		s->n_chan = boardtype.n_aochan;
+		s->n_chan = board->n_aochan;
 		s->maxdata = (1 << 16) - 1;
 		s->range_table = &range_unknown;	/* XXX */
 		s->insn_read = cs5529_ai_insn_read;
@@ -4576,7 +4603,7 @@ static int ni_E_init(struct comedi_device *dev)
 	s->insn_config = ni_rtsi_insn_config;
 	ni_rtsi_init(dev);
 
-	if (boardtype.reg_type & ni_reg_m_series_mask) {
+	if (board->reg_type & ni_reg_m_series_mask) {
 		counter_variant = ni_gpct_variant_m_series;
 	} else {
 		counter_variant = ni_gpct_variant_e_series;
@@ -4594,7 +4621,7 @@ static int ni_E_init(struct comedi_device *dev)
 		    SDF_READABLE | SDF_WRITABLE | SDF_LSAMPL | SDF_CMD_READ
 		    /* | SDF_CMD_WRITE */ ;
 		s->n_chan = 3;
-		if (boardtype.reg_type & ni_reg_m_series_mask)
+		if (board->reg_type & ni_reg_m_series_mask)
 			s->maxdata = 0xffffffff;
 		else
 			s->maxdata = 0xffffff;
@@ -4626,7 +4653,7 @@ static int ni_E_init(struct comedi_device *dev)
 	/* ai configuration */
 	s = &dev->subdevices[NI_AI_SUBDEV];
 	ni_ai_reset(dev, s);
-	if ((boardtype.reg_type & ni_reg_6xxx_mask) == 0) {
+	if ((board->reg_type & ni_reg_6xxx_mask) == 0) {
 		/*  BEAM is this needed for PCI-6143 ?? */
 		devpriv->clock_and_fout =
 		    Slow_Internal_Time_Divide_By_2 |
@@ -4663,11 +4690,11 @@ static int ni_E_init(struct comedi_device *dev)
 	ni_writeb(devpriv->ai_ao_select_reg, AI_AO_Select);
 	ni_writeb(devpriv->g0_g1_select_reg, G0_G1_Select);
 
-	if (boardtype.reg_type & ni_reg_6xxx_mask) {
+	if (board->reg_type & ni_reg_6xxx_mask) {
 		ni_writeb(0, Magic_611x);
-	} else if (boardtype.reg_type & ni_reg_m_series_mask) {
+	} else if (board->reg_type & ni_reg_m_series_mask) {
 		int channel;
-		for (channel = 0; channel < boardtype.n_aochan; ++channel) {
+		for (channel = 0; channel < board->n_aochan; ++channel) {
 			ni_writeb(0xf, M_Offset_AO_Waveform_Order(channel));
 			ni_writeb(0x0,
 				  M_Offset_AO_Reference_Attenuation(channel));
@@ -4938,6 +4965,7 @@ static struct caldac_struct caldacs[] = {
 
 static void caldac_setup(struct comedi_device *dev, struct comedi_subdevice *s)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 	int i, j;
 	int n_dacs;
@@ -4947,12 +4975,12 @@ static void caldac_setup(struct comedi_device *dev, struct comedi_subdevice *s)
 	int type;
 	int chan;
 
-	type = boardtype.caldac[0];
+	type = board->caldac[0];
 	if (type == caldac_none)
 		return;
 	n_bits = caldacs[type].n_bits;
 	for (i = 0; i < 3; i++) {
-		type = boardtype.caldac[i];
+		type = board->caldac[i];
 		if (type == caldac_none)
 			break;
 		if (caldacs[type].n_bits != n_bits)
@@ -4971,7 +4999,7 @@ static void caldac_setup(struct comedi_device *dev, struct comedi_subdevice *s)
 		s->maxdata_list = maxdata_list = devpriv->caldac_maxdata_list;
 		chan = 0;
 		for (i = 0; i < n_dacs; i++) {
-			type = boardtype.caldac[i];
+			type = board->caldac[i];
 			for (j = 0; j < caldacs[type].n_chans; j++) {
 				maxdata_list[chan] =
 				    (1 << caldacs[type].n_bits) - 1;
@@ -4982,7 +5010,7 @@ static void caldac_setup(struct comedi_device *dev, struct comedi_subdevice *s)
 		for (chan = 0; chan < s->n_chan; chan++)
 			ni_write_caldac(dev, i, s->maxdata_list[i] / 2);
 	} else {
-		type = boardtype.caldac[0];
+		type = board->caldac[0];
 		s->maxdata = (1 << caldacs[type].n_bits) - 1;
 
 		for (chan = 0; chan < s->n_chan; chan++)
@@ -4992,6 +5020,7 @@ static void caldac_setup(struct comedi_device *dev, struct comedi_subdevice *s)
 
 static void ni_write_caldac(struct comedi_device *dev, int addr, int val)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 	unsigned int loadbit = 0, bits = 0, bit, bitstring = 0;
 	int i;
@@ -5003,7 +5032,7 @@ static void ni_write_caldac(struct comedi_device *dev, int addr, int val)
 	devpriv->caldacs[addr] = val;
 
 	for (i = 0; i < 3; i++) {
-		type = boardtype.caldac[i];
+		type = board->caldac[i];
 		if (type == caldac_none)
 			break;
 		if (addr < caldacs[type].n_chans) {
@@ -5275,7 +5304,9 @@ static int ni_old_set_pfi_routing(struct comedi_device *dev, unsigned chan,
 static int ni_set_pfi_routing(struct comedi_device *dev, unsigned chan,
 			      unsigned source)
 {
-	if (boardtype.reg_type & ni_reg_m_series_mask)
+	const struct ni_board_struct *board = comedi_board(dev);
+
+	if (board->reg_type & ni_reg_m_series_mask)
 		return ni_m_series_set_pfi_routing(dev, chan, source);
 	else
 		return ni_old_set_pfi_routing(dev, chan, source);
@@ -5336,7 +5367,9 @@ static unsigned ni_old_get_pfi_routing(struct comedi_device *dev, unsigned chan)
 
 static unsigned ni_get_pfi_routing(struct comedi_device *dev, unsigned chan)
 {
-	if (boardtype.reg_type & ni_reg_m_series_mask)
+	const struct ni_board_struct *board = comedi_board(dev);
+
+	if (board->reg_type & ni_reg_m_series_mask)
 		return ni_m_series_get_pfi_routing(dev, chan);
 	else
 		return ni_old_get_pfi_routing(dev, chan);
@@ -5345,10 +5378,11 @@ static unsigned ni_get_pfi_routing(struct comedi_device *dev, unsigned chan)
 static int ni_config_filter(struct comedi_device *dev, unsigned pfi_channel,
 			    enum ni_pfi_filter_select filter)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv __maybe_unused = dev->private;
 	unsigned bits;
 
-	if ((boardtype.reg_type & ni_reg_m_series_mask) == 0) {
+	if ((board->reg_type & ni_reg_m_series_mask) == 0) {
 		return -ENOTSUPP;
 	}
 	bits = ni_readl(M_Offset_PFI_Filter);
@@ -5362,9 +5396,10 @@ static int ni_pfi_insn_bits(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv __maybe_unused = dev->private;
 
-	if ((boardtype.reg_type & ni_reg_m_series_mask) == 0) {
+	if ((board->reg_type & ni_reg_m_series_mask) == 0) {
 		return -ENOTSUPP;
 	}
 	if (data[0]) {
@@ -5423,6 +5458,7 @@ static int ni_pfi_insn_config(struct comedi_device *dev,
  */
 static void ni_rtsi_init(struct comedi_device *dev)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 
 	/*  Initialises the RTSI bus signal switch to a default state */
@@ -5449,7 +5485,7 @@ static void ni_rtsi_init(struct comedi_device *dev)
 	    RTSI_Trig_Output_Bits(5,
 				  NI_RTSI_OUTPUT_G_SRC0) |
 	    RTSI_Trig_Output_Bits(6, NI_RTSI_OUTPUT_G_GATE0);
-	if (boardtype.reg_type & ni_reg_m_series_mask)
+	if (board->reg_type & ni_reg_m_series_mask)
 		devpriv->rtsi_trig_b_output_reg |=
 		    RTSI_Trig_Output_Bits(7, NI_RTSI_OUTPUT_RTSI_OSC);
 	devpriv->stc_writew(dev, devpriv->rtsi_trig_b_output_reg,
@@ -5517,7 +5553,9 @@ static int ni_mseries_get_pll_parameters(unsigned reference_period_ns,
 
 static inline unsigned num_configurable_rtsi_channels(struct comedi_device *dev)
 {
-	if (boardtype.reg_type & ni_reg_m_series_mask)
+	const struct ni_board_struct *board = comedi_board(dev);
+
+	if (board->reg_type & ni_reg_m_series_mask)
 		return 8;
 	else
 		return 7;
@@ -5629,6 +5667,7 @@ static int ni_mseries_set_pll_master_clock(struct comedi_device *dev,
 static int ni_set_master_clock(struct comedi_device *dev, unsigned source,
 			       unsigned period_ns)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 
 	if (source == NI_MIO_INTERNAL_CLOCK) {
@@ -5636,7 +5675,7 @@ static int ni_set_master_clock(struct comedi_device *dev, unsigned source,
 		devpriv->stc_writew(dev, devpriv->rtsi_trig_direction_reg,
 				    RTSI_Trig_Direction_Register);
 		devpriv->clock_ns = TIMEBASE_1_NS;
-		if (boardtype.reg_type & ni_reg_m_series_mask) {
+		if (board->reg_type & ni_reg_m_series_mask) {
 			devpriv->clock_and_fout2 &=
 			    ~(MSeries_Timebase1_Select_Bit |
 			      MSeries_Timebase3_Select_Bit);
@@ -5646,7 +5685,7 @@ static int ni_set_master_clock(struct comedi_device *dev, unsigned source,
 		}
 		devpriv->clock_source = source;
 	} else {
-		if (boardtype.reg_type & ni_reg_m_series_mask) {
+		if (board->reg_type & ni_reg_m_series_mask) {
 			return ni_mseries_set_pll_master_clock(dev, source,
 							       period_ns);
 		} else {
@@ -5676,6 +5715,8 @@ static int ni_set_master_clock(struct comedi_device *dev, unsigned source,
 static int ni_valid_rtsi_output_source(struct comedi_device *dev, unsigned chan,
 				       unsigned source)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
+
 	if (chan >= num_configurable_rtsi_channels(dev)) {
 		if (chan == old_RTSI_clock_channel) {
 			if (source == NI_RTSI_OUTPUT_RTSI_OSC)
@@ -5702,7 +5743,7 @@ static int ni_valid_rtsi_output_source(struct comedi_device *dev, unsigned chan,
 		return 1;
 		break;
 	case NI_RTSI_OUTPUT_RTSI_OSC:
-		if (boardtype.reg_type & ni_reg_m_series_mask)
+		if (board->reg_type & ni_reg_m_series_mask)
 			return 1;
 		else
 			return 0;
@@ -5758,6 +5799,7 @@ static int ni_rtsi_insn_config(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
 
@@ -5766,9 +5808,7 @@ static int ni_rtsi_insn_config(struct comedi_device *dev,
 		if (chan < num_configurable_rtsi_channels(dev)) {
 			devpriv->rtsi_trig_direction_reg |=
 			    RTSI_Output_Bit(chan,
-					    (boardtype.
-					     reg_type & ni_reg_m_series_mask) !=
-					    0);
+				(board->reg_type & ni_reg_m_series_mask) != 0);
 		} else if (chan == old_RTSI_clock_channel) {
 			devpriv->rtsi_trig_direction_reg |=
 			    Drive_RTSI_Clock_Bit;
@@ -5780,9 +5820,7 @@ static int ni_rtsi_insn_config(struct comedi_device *dev,
 		if (chan < num_configurable_rtsi_channels(dev)) {
 			devpriv->rtsi_trig_direction_reg &=
 			    ~RTSI_Output_Bit(chan,
-					     (boardtype.
-					      reg_type & ni_reg_m_series_mask)
-					     != 0);
+				(board->reg_type & ni_reg_m_series_mask) != 0);
 		} else if (chan == old_RTSI_clock_channel) {
 			devpriv->rtsi_trig_direction_reg &=
 			    ~Drive_RTSI_Clock_Bit;
@@ -5795,10 +5833,9 @@ static int ni_rtsi_insn_config(struct comedi_device *dev,
 			data[1] =
 			    (devpriv->rtsi_trig_direction_reg &
 			     RTSI_Output_Bit(chan,
-					     (boardtype.reg_type &
-					      ni_reg_m_series_mask)
-					     != 0)) ? INSN_CONFIG_DIO_OUTPUT :
-			    INSN_CONFIG_DIO_INPUT;
+				(board->reg_type & ni_reg_m_series_mask) != 0))
+				? INSN_CONFIG_DIO_OUTPUT
+				: INSN_CONFIG_DIO_INPUT;
 		} else if (chan == old_RTSI_clock_channel) {
 			data[1] =
 			    (devpriv->rtsi_trig_direction_reg &
diff --git a/drivers/staging/comedi/drivers/ni_pcidio.c b/drivers/staging/comedi/drivers/ni_pcidio.c
index 0a00260d11f..b5f340c186e 100644
--- a/drivers/staging/comedi/drivers/ni_pcidio.c
+++ b/drivers/staging/comedi/drivers/ni_pcidio.c
@@ -280,29 +280,30 @@ enum FPGA_Control_Bits {
 static int ni_pcidio_cancel(struct comedi_device *dev,
 			    struct comedi_subdevice *s);
 
+enum nidio_boardid {
+	BOARD_PCIDIO_32HS,
+	BOARD_PXI6533,
+	BOARD_PCI6534,
+};
+
 struct nidio_board {
-	int dev_id;
 	const char *name;
 	unsigned int uses_firmware:1;
 };
 
 static const struct nidio_board nidio_boards[] = {
-	{
-		.dev_id		= 0x1150,
+	[BOARD_PCIDIO_32HS] = {
 		.name		= "pci-dio-32hs",
-	}, {
-		.dev_id		= 0x1320,
+	},
+	[BOARD_PXI6533] = {
 		.name		= "pxi-6533",
-	}, {
-		.dev_id		= 0x12b0,
+	},
+	[BOARD_PCI6534] = {
 		.name		= "pci-6534",
 		.uses_firmware	= 1,
 	},
 };
 
-#define n_nidio_boards ARRAY_SIZE(nidio_boards)
-#define this_board ((const struct nidio_board *)dev->board_ptr)
-
 struct nidio96_private {
 	struct mite_struct *mite;
 	int boardtype;
@@ -419,7 +420,7 @@ static irqreturn_t nidio_interrupt(int irq, void *d)
 	unsigned int m_status = 0;
 
 	/* interrupcions parasites */
-	if (dev->attached == 0) {
+	if (!dev->attached) {
 		/* assume it's from another card */
 		return IRQ_NONE;
 	}
@@ -1094,29 +1095,27 @@ static int pci_6534_upload_firmware(struct comedi_device *dev)
 	return ret;
 }
 
-static const struct nidio_board *
-nidio_find_boardinfo(struct pci_dev *pcidev)
-{
-	unsigned int dev_id = pcidev->device;
-	unsigned int n;
-
-	for (n = 0; n < ARRAY_SIZE(nidio_boards); n++) {
-		const struct nidio_board *board = &nidio_boards[n];
-		if (board->dev_id == dev_id)
-			return board;
-	}
-	return NULL;
-}
-
 static int nidio_auto_attach(struct comedi_device *dev,
-				       unsigned long context_unused)
+			     unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	const struct nidio_board *board = NULL;
 	struct nidio96_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret;
 	unsigned int irq;
 
+	if (context < ARRAY_SIZE(nidio_boards))
+		board = &nidio_boards[context];
+	if (!board)
+		return -ENODEV;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
+
+	ret = comedi_pci_enable(dev);
+	if (ret)
+		return ret;
+
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
@@ -1124,9 +1123,6 @@ static int nidio_auto_attach(struct comedi_device *dev,
 
 	spin_lock_init(&devpriv->mite_channel_lock);
 
-	dev->board_ptr = nidio_find_boardinfo(pcidev);
-	if (!dev->board_ptr)
-		return -ENODEV;
 	devpriv->mite = mite_alloc(pcidev);
 	if (!devpriv->mite)
 		return -ENOMEM;
@@ -1141,9 +1137,8 @@ static int nidio_auto_attach(struct comedi_device *dev,
 	if (devpriv->di_mite_ring == NULL)
 		return -ENOMEM;
 
-	dev->board_name = this_board->name;
 	irq = mite_irq(devpriv->mite);
-	if (this_board->uses_firmware) {
+	if (board->uses_firmware) {
 		ret = pci_6534_upload_firmware(dev);
 		if (ret < 0)
 			return ret;
@@ -1211,6 +1206,7 @@ static void nidio_detach(struct comedi_device *dev)
 			mite_free(devpriv->mite);
 		}
 	}
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver ni_pcidio_driver = {
@@ -1221,15 +1217,15 @@ static struct comedi_driver ni_pcidio_driver = {
 };
 
 static int ni_pcidio_pci_probe(struct pci_dev *dev,
-					 const struct pci_device_id *ent)
+			       const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &ni_pcidio_driver);
+	return comedi_pci_auto_config(dev, &ni_pcidio_driver, id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(ni_pcidio_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1150) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1320) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x12b0) },
+	{ PCI_VDEVICE(NI, 0x1150), BOARD_PCIDIO_32HS },
+	{ PCI_VDEVICE(NI, 0x12b0), BOARD_PCI6534 },
+	{ PCI_VDEVICE(NI, 0x1320), BOARD_PXI6533 },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, ni_pcidio_pci_table);
diff --git a/drivers/staging/comedi/drivers/ni_pcimio.c b/drivers/staging/comedi/drivers/ni_pcimio.c
index 98b43f2fc65..634d02303aa 100644
--- a/drivers/staging/comedi/drivers/ni_pcimio.c
+++ b/drivers/staging/comedi/drivers/ni_pcimio.c
@@ -111,7 +111,6 @@ Bugs:
 */
 
 #include <linux/delay.h>
-#include <linux/delay.h>
 
 #include "../comedidev.h"
 
@@ -158,1036 +157,894 @@ static const struct comedi_lrange range_ni_M_625x_ao = { 3, {
 							     }
 };
 
-static const struct comedi_lrange range_ni_M_622x_ao = { 1, {
-							     RANGE(-10, 10),
-							     }
+enum ni_pcimio_boardid {
+	BOARD_PCIMIO_16XE_50,
+	BOARD_PCIMIO_16XE_10,
+	BOARD_PCI6014,
+	BOARD_PXI6030E,
+	BOARD_PCIMIO_16E_1,
+	BOARD_PCIMIO_16E_4,
+	BOARD_PXI6040E,
+	BOARD_PCI6031E,
+	BOARD_PCI6032E,
+	BOARD_PCI6033E,
+	BOARD_PCI6071E,
+	BOARD_PCI6023E,
+	BOARD_PCI6024E,
+	BOARD_PCI6025E,
+	BOARD_PXI6025E,
+	BOARD_PCI6034E,
+	BOARD_PCI6035E,
+	BOARD_PCI6052E,
+	BOARD_PCI6110,
+	BOARD_PCI6111,
+	/* BOARD_PCI6115, */
+	/* BOARD_PXI6115, */
+	BOARD_PCI6711,
+	BOARD_PXI6711,
+	BOARD_PCI6713,
+	BOARD_PXI6713,
+	BOARD_PCI6731,
+	/* BOARD_PXI6731, */
+	BOARD_PCI6733,
+	BOARD_PXI6733,
+	BOARD_PXI6071E,
+	BOARD_PXI6070E,
+	BOARD_PXI6052E,
+	BOARD_PXI6031E,
+	BOARD_PCI6036E,
+	BOARD_PCI6220,
+	BOARD_PCI6221,
+	BOARD_PCI6221_37PIN,
+	BOARD_PCI6224,
+	BOARD_PXI6224,
+	BOARD_PCI6225,
+	BOARD_PXI6225,
+	BOARD_PCI6229,
+	BOARD_PCI6250,
+	BOARD_PCI6251,
+	BOARD_PCIE6251,
+	BOARD_PXIE6251,
+	BOARD_PCI6254,
+	BOARD_PCI6259,
+	BOARD_PCIE6259,
+	BOARD_PCI6280,
+	BOARD_PCI6281,
+	BOARD_PXI6281,
+	BOARD_PCI6284,
+	BOARD_PCI6289,
+	BOARD_PCI6143,
+	BOARD_PXI6143,
 };
 
 static const struct ni_board_struct ni_boards[] = {
-	{
-	 .device_id = 0x0162,	/*  NI also says 0x1620.  typo? */
-	 .name = "pci-mio-16xe-50",
-	 .n_adchan = 16,
-	 .adbits = 16,
-	 .ai_fifo_depth = 2048,
-	 .alwaysdither = 1,
-	 .gainlkup = ai_gain_8,
-	 .ai_speed = 50000,
-	 .n_aochan = 2,
-	 .aobits = 12,
-	 .ao_fifo_depth = 0,
-	 .ao_range_table = &range_bipolar10,
-	 .ao_unipolar = 0,
-	 .ao_speed = 50000,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {dac8800, dac8043},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x1170,
-	 .name = "pci-mio-16xe-10",	/*  aka pci-6030E */
-	 .n_adchan = 16,
-	 .adbits = 16,
-	 .ai_fifo_depth = 512,
-	 .alwaysdither = 1,
-	 .gainlkup = ai_gain_14,
-	 .ai_speed = 10000,
-	 .n_aochan = 2,
-	 .aobits = 16,
-	 .ao_fifo_depth = 2048,
-	 .ao_range_table = &range_ni_E_ao_ext,
-	 .ao_unipolar = 1,
-	 .ao_speed = 10000,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {dac8800, dac8043, ad8522},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x28c0,
-	 .name = "pci-6014",
-	 .n_adchan = 16,
-	 .adbits = 16,
-	 .ai_fifo_depth = 512,
-	 .alwaysdither = 1,
-	 .gainlkup = ai_gain_4,
-	 .ai_speed = 5000,
-	 .n_aochan = 2,
-	 .aobits = 16,
-	 .ao_fifo_depth = 0,
-	 .ao_range_table = &range_bipolar10,
-	 .ao_unipolar = 0,
-	 .ao_speed = 100000,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {ad8804_debug},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x11d0,
-	 .name = "pxi-6030e",
-	 .n_adchan = 16,
-	 .adbits = 16,
-	 .ai_fifo_depth = 512,
-	 .alwaysdither = 1,
-	 .gainlkup = ai_gain_14,
-	 .ai_speed = 10000,
-	 .n_aochan = 2,
-	 .aobits = 16,
-	 .ao_fifo_depth = 2048,
-	 .ao_range_table = &range_ni_E_ao_ext,
-	 .ao_unipolar = 1,
-	 .ao_speed = 10000,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {dac8800, dac8043, ad8522},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x1180,
-	 .name = "pci-mio-16e-1",	/* aka pci-6070e */
-	 .n_adchan = 16,
-	 .adbits = 12,
-	 .ai_fifo_depth = 512,
-	 .alwaysdither = 0,
-	 .gainlkup = ai_gain_16,
-	 .ai_speed = 800,
-	 .n_aochan = 2,
-	 .aobits = 12,
-	 .ao_fifo_depth = 2048,
-	 .ao_range_table = &range_ni_E_ao_ext,
-	 .ao_unipolar = 1,
-	 .ao_speed = 1000,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {mb88341},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x1190,
-	 .name = "pci-mio-16e-4",	/* aka pci-6040e */
-	 .n_adchan = 16,
-	 .adbits = 12,
-	 .ai_fifo_depth = 512,
-	 .alwaysdither = 0,
-	 .gainlkup = ai_gain_16,
-	 /*      .Note = there have been reported problems with full speed
-	  * on this board */
-	 .ai_speed = 2000,
-	 .n_aochan = 2,
-	 .aobits = 12,
-	 .ao_fifo_depth = 512,
-	 .ao_range_table = &range_ni_E_ao_ext,
-	 .ao_unipolar = 1,
-	 .ao_speed = 1000,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {ad8804_debug},	/*  doc says mb88341 */
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x11c0,
-	 .name = "pxi-6040e",
-	 .n_adchan = 16,
-	 .adbits = 12,
-	 .ai_fifo_depth = 512,
-	 .alwaysdither = 0,
-	 .gainlkup = ai_gain_16,
-	 .ai_speed = 2000,
-	 .n_aochan = 2,
-	 .aobits = 12,
-	 .ao_fifo_depth = 512,
-	 .ao_range_table = &range_ni_E_ao_ext,
-	 .ao_unipolar = 1,
-	 .ao_speed = 1000,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {mb88341},
-	 .has_8255 = 0,
-	 },
-
-	{
-	 .device_id = 0x1330,
-	 .name = "pci-6031e",
-	 .n_adchan = 64,
-	 .adbits = 16,
-	 .ai_fifo_depth = 512,
-	 .alwaysdither = 1,
-	 .gainlkup = ai_gain_14,
-	 .ai_speed = 10000,
-	 .n_aochan = 2,
-	 .aobits = 16,
-	 .ao_fifo_depth = 2048,
-	 .ao_range_table = &range_ni_E_ao_ext,
-	 .ao_unipolar = 1,
-	 .ao_speed = 10000,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {dac8800, dac8043, ad8522},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x1270,
-	 .name = "pci-6032e",
-	 .n_adchan = 16,
-	 .adbits = 16,
-	 .ai_fifo_depth = 512,
-	 .alwaysdither = 1,
-	 .gainlkup = ai_gain_14,
-	 .ai_speed = 10000,
-	 .n_aochan = 0,
-	 .aobits = 0,
-	 .ao_fifo_depth = 0,
-	 .ao_unipolar = 0,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {dac8800, dac8043, ad8522},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x1340,
-	 .name = "pci-6033e",
-	 .n_adchan = 64,
-	 .adbits = 16,
-	 .ai_fifo_depth = 512,
-	 .alwaysdither = 1,
-	 .gainlkup = ai_gain_14,
-	 .ai_speed = 10000,
-	 .n_aochan = 0,
-	 .aobits = 0,
-	 .ao_fifo_depth = 0,
-	 .ao_unipolar = 0,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {dac8800, dac8043, ad8522},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x1350,
-	 .name = "pci-6071e",
-	 .n_adchan = 64,
-	 .adbits = 12,
-	 .ai_fifo_depth = 512,
-	 .alwaysdither = 1,
-	 .gainlkup = ai_gain_16,
-	 .ai_speed = 800,
-	 .n_aochan = 2,
-	 .aobits = 12,
-	 .ao_fifo_depth = 2048,
-	 .ao_range_table = &range_ni_E_ao_ext,
-	 .ao_unipolar = 1,
-	 .ao_speed = 1000,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {ad8804_debug},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x2a60,
-	 .name = "pci-6023e",
-	 .n_adchan = 16,
-	 .adbits = 12,
-	 .ai_fifo_depth = 512,
-	 .alwaysdither = 0,
-	 .gainlkup = ai_gain_4,
-	 .ai_speed = 5000,
-	 .n_aochan = 0,
-	 .aobits = 0,
-	 .ao_unipolar = 0,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {ad8804_debug},	/* manual is wrong */
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x2a70,
-	 .name = "pci-6024e",
-	 .n_adchan = 16,
-	 .adbits = 12,
-	 .ai_fifo_depth = 512,
-	 .alwaysdither = 0,
-	 .gainlkup = ai_gain_4,
-	 .ai_speed = 5000,
-	 .n_aochan = 2,
-	 .aobits = 12,
-	 .ao_fifo_depth = 0,
-	 .ao_range_table = &range_bipolar10,
-	 .ao_unipolar = 0,
-	 .ao_speed = 100000,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {ad8804_debug},	/* manual is wrong */
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x2a80,
-	 .name = "pci-6025e",
-	 .n_adchan = 16,
-	 .adbits = 12,
-	 .ai_fifo_depth = 512,
-	 .alwaysdither = 0,
-	 .gainlkup = ai_gain_4,
-	 .ai_speed = 5000,
-	 .n_aochan = 2,
-	 .aobits = 12,
-	 .ao_fifo_depth = 0,
-	 .ao_range_table = &range_bipolar10,
-	 .ao_unipolar = 0,
-	 .ao_speed = 100000,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {ad8804_debug},	/* manual is wrong */
-	 .has_8255 = 1,
-	 },
-	{
-	 .device_id = 0x2ab0,
-	 .name = "pxi-6025e",
-	 .n_adchan = 16,
-	 .adbits = 12,
-	 .ai_fifo_depth = 512,
-	 .alwaysdither = 0,
-	 .gainlkup = ai_gain_4,
-	 .ai_speed = 5000,
-	 .n_aochan = 2,
-	 .aobits = 12,
-	 .ao_fifo_depth = 0,
-	 .ao_range_table = &range_ni_E_ao_ext,
-	 .ao_unipolar = 1,
-	 .ao_speed = 100000,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {ad8804_debug},	/* manual is wrong */
-	 .has_8255 = 1,
-	 },
-
-	{
-	 .device_id = 0x2ca0,
-	 .name = "pci-6034e",
-	 .n_adchan = 16,
-	 .adbits = 16,
-	 .ai_fifo_depth = 512,
-	 .alwaysdither = 1,
-	 .gainlkup = ai_gain_4,
-	 .ai_speed = 5000,
-	 .n_aochan = 0,
-	 .aobits = 0,
-	 .ao_fifo_depth = 0,
-	 .ao_unipolar = 0,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {ad8804_debug},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x2c80,
-	 .name = "pci-6035e",
-	 .n_adchan = 16,
-	 .adbits = 16,
-	 .ai_fifo_depth = 512,
-	 .alwaysdither = 1,
-	 .gainlkup = ai_gain_4,
-	 .ai_speed = 5000,
-	 .n_aochan = 2,
-	 .aobits = 12,
-	 .ao_fifo_depth = 0,
-	 .ao_range_table = &range_bipolar10,
-	 .ao_unipolar = 0,
-	 .ao_speed = 100000,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {ad8804_debug},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x18b0,
-	 .name = "pci-6052e",
-	 .n_adchan = 16,
-	 .adbits = 16,
-	 .ai_fifo_depth = 512,
-	 .alwaysdither = 1,
-	 .gainlkup = ai_gain_16,
-	 .ai_speed = 3000,
-	 .n_aochan = 2,
-	 .aobits = 16,
-	 .ao_unipolar = 1,
-	 .ao_fifo_depth = 2048,
-	 .ao_range_table = &range_ni_E_ao_ext,
-	 .ao_speed = 3000,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {ad8804_debug, ad8804_debug, ad8522},	/* manual is wrong */
-	 },
-	{.device_id = 0x14e0,
-	 .name = "pci-6110",
-	 .n_adchan = 4,
-	 .adbits = 12,
-	 .ai_fifo_depth = 8192,
-	 .alwaysdither = 0,
-	 .gainlkup = ai_gain_611x,
-	 .ai_speed = 200,
-	 .n_aochan = 2,
-	 .aobits = 16,
-	 .reg_type = ni_reg_611x,
-	 .ao_range_table = &range_bipolar10,
-	 .ao_unipolar = 0,
-	 .ao_fifo_depth = 2048,
-	 .ao_speed = 250,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {ad8804, ad8804},
-	 },
-	{
-	 .device_id = 0x14f0,
-	 .name = "pci-6111",
-	 .n_adchan = 2,
-	 .adbits = 12,
-	 .ai_fifo_depth = 8192,
-	 .alwaysdither = 0,
-	 .gainlkup = ai_gain_611x,
-	 .ai_speed = 200,
-	 .n_aochan = 2,
-	 .aobits = 16,
-	 .reg_type = ni_reg_611x,
-	 .ao_range_table = &range_bipolar10,
-	 .ao_unipolar = 0,
-	 .ao_fifo_depth = 2048,
-	 .ao_speed = 250,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {ad8804, ad8804},
-	 },
+	[BOARD_PCIMIO_16XE_50] = {
+		.name		= "pci-mio-16xe-50",
+		.n_adchan	= 16,
+		.adbits		= 16,
+		.ai_fifo_depth	= 2048,
+		.alwaysdither	= 1,
+		.gainlkup	= ai_gain_8,
+		.ai_speed	= 50000,
+		.n_aochan	= 2,
+		.aobits		= 12,
+		.ao_range_table	= &range_bipolar10,
+		.ao_speed	= 50000,
+		.num_p0_dio_channels = 8,
+		.caldac		= { dac8800, dac8043 },
+	},
+	[BOARD_PCIMIO_16XE_10] = {
+		.name		= "pci-mio-16xe-10",	/*  aka pci-6030E */
+		.n_adchan	= 16,
+		.adbits		= 16,
+		.ai_fifo_depth	= 512,
+		.alwaysdither	= 1,
+		.gainlkup	= ai_gain_14,
+		.ai_speed	= 10000,
+		.n_aochan	= 2,
+		.aobits		= 16,
+		.ao_fifo_depth	= 2048,
+		.ao_range_table	= &range_ni_E_ao_ext,
+		.ao_unipolar	= 1,
+		.ao_speed	= 10000,
+		.num_p0_dio_channels = 8,
+		.caldac		= { dac8800, dac8043, ad8522 },
+	},
+	[BOARD_PCI6014] = {
+		.name		= "pci-6014",
+		.n_adchan	= 16,
+		.adbits		= 16,
+		.ai_fifo_depth	= 512,
+		.alwaysdither	= 1,
+		.gainlkup	= ai_gain_4,
+		.ai_speed	= 5000,
+		.n_aochan	= 2,
+		.aobits		= 16,
+		.ao_range_table	= &range_bipolar10,
+		.ao_speed	= 100000,
+		.num_p0_dio_channels = 8,
+		.caldac		= { ad8804_debug },
+	},
+	[BOARD_PXI6030E] = {
+		.name		= "pxi-6030e",
+		.n_adchan	= 16,
+		.adbits		= 16,
+		.ai_fifo_depth	= 512,
+		.alwaysdither	= 1,
+		.gainlkup	= ai_gain_14,
+		.ai_speed	= 10000,
+		.n_aochan	= 2,
+		.aobits		= 16,
+		.ao_fifo_depth	= 2048,
+		.ao_range_table	= &range_ni_E_ao_ext,
+		.ao_unipolar	= 1,
+		.ao_speed	= 10000,
+		.num_p0_dio_channels = 8,
+		.caldac		= { dac8800, dac8043, ad8522 },
+	},
+	[BOARD_PCIMIO_16E_1] = {
+		.name		= "pci-mio-16e-1",	/* aka pci-6070e */
+		.n_adchan	= 16,
+		.adbits		= 12,
+		.ai_fifo_depth	= 512,
+		.gainlkup	= ai_gain_16,
+		.ai_speed	= 800,
+		.n_aochan	= 2,
+		.aobits		= 12,
+		.ao_fifo_depth	= 2048,
+		.ao_range_table	= &range_ni_E_ao_ext,
+		.ao_unipolar	= 1,
+		.ao_speed	= 1000,
+		.num_p0_dio_channels = 8,
+		.caldac		= { mb88341 },
+	},
+	[BOARD_PCIMIO_16E_4] = {
+		.name		= "pci-mio-16e-4",	/* aka pci-6040e */
+		.n_adchan	= 16,
+		.adbits		= 12,
+		.ai_fifo_depth	= 512,
+		.gainlkup	= ai_gain_16,
+		/*
+		 * there have been reported problems with
+		 * full speed on this board
+		 */
+		.ai_speed	= 2000,
+		.n_aochan	= 2,
+		.aobits		= 12,
+		.ao_fifo_depth	= 512,
+		.ao_range_table	= &range_ni_E_ao_ext,
+		.ao_unipolar	= 1,
+		.ao_speed	= 1000,
+		.num_p0_dio_channels = 8,
+		.caldac		= { ad8804_debug },	/* doc says mb88341 */
+	},
+	[BOARD_PXI6040E] = {
+		.name		= "pxi-6040e",
+		.n_adchan	= 16,
+		.adbits		= 12,
+		.ai_fifo_depth	= 512,
+		.gainlkup	= ai_gain_16,
+		.ai_speed	= 2000,
+		.n_aochan	= 2,
+		.aobits		= 12,
+		.ao_fifo_depth	= 512,
+		.ao_range_table	= &range_ni_E_ao_ext,
+		.ao_unipolar	= 1,
+		.ao_speed	= 1000,
+		.num_p0_dio_channels = 8,
+		.caldac		= { mb88341 },
+	},
+	[BOARD_PCI6031E] = {
+		.name		= "pci-6031e",
+		.n_adchan	= 64,
+		.adbits		= 16,
+		.ai_fifo_depth	= 512,
+		.alwaysdither	= 1,
+		.gainlkup	= ai_gain_14,
+		.ai_speed	= 10000,
+		.n_aochan	= 2,
+		.aobits		= 16,
+		.ao_fifo_depth	= 2048,
+		.ao_range_table	= &range_ni_E_ao_ext,
+		.ao_unipolar	= 1,
+		.ao_speed	= 10000,
+		.num_p0_dio_channels = 8,
+		.caldac		= { dac8800, dac8043, ad8522 },
+	},
+	[BOARD_PCI6032E] = {
+		.name		= "pci-6032e",
+		.n_adchan	= 16,
+		.adbits		= 16,
+		.ai_fifo_depth	= 512,
+		.alwaysdither	= 1,
+		.gainlkup	= ai_gain_14,
+		.ai_speed	= 10000,
+		.num_p0_dio_channels = 8,
+		.caldac		= { dac8800, dac8043, ad8522 },
+	},
+	[BOARD_PCI6033E] = {
+		.name		= "pci-6033e",
+		.n_adchan	= 64,
+		.adbits		= 16,
+		.ai_fifo_depth	= 512,
+		.alwaysdither	= 1,
+		.gainlkup	= ai_gain_14,
+		.ai_speed	= 10000,
+		.num_p0_dio_channels = 8,
+		.caldac		= { dac8800, dac8043, ad8522 },
+	},
+	[BOARD_PCI6071E] = {
+		.name		= "pci-6071e",
+		.n_adchan	= 64,
+		.adbits		= 12,
+		.ai_fifo_depth	= 512,
+		.alwaysdither	= 1,
+		.gainlkup	= ai_gain_16,
+		.ai_speed	= 800,
+		.n_aochan	= 2,
+		.aobits		= 12,
+		.ao_fifo_depth	= 2048,
+		.ao_range_table	= &range_ni_E_ao_ext,
+		.ao_unipolar	= 1,
+		.ao_speed	= 1000,
+		.num_p0_dio_channels = 8,
+		.caldac		= { ad8804_debug },
+	},
+	[BOARD_PCI6023E] = {
+		.name		= "pci-6023e",
+		.n_adchan	= 16,
+		.adbits		= 12,
+		.ai_fifo_depth	= 512,
+		.gainlkup	= ai_gain_4,
+		.ai_speed	= 5000,
+		.num_p0_dio_channels = 8,
+		.caldac		= { ad8804_debug },	/* manual is wrong */
+	},
+	[BOARD_PCI6024E] = {
+		.name		= "pci-6024e",
+		.n_adchan	= 16,
+		.adbits		= 12,
+		.ai_fifo_depth	= 512,
+		.gainlkup	= ai_gain_4,
+		.ai_speed	= 5000,
+		.n_aochan	= 2,
+		.aobits		= 12,
+		.ao_range_table	= &range_bipolar10,
+		.ao_speed	= 100000,
+		.num_p0_dio_channels = 8,
+		.caldac		= { ad8804_debug },	/* manual is wrong */
+	},
+	[BOARD_PCI6025E] = {
+		.name		= "pci-6025e",
+		.n_adchan	= 16,
+		.adbits		= 12,
+		.ai_fifo_depth	= 512,
+		.gainlkup	= ai_gain_4,
+		.ai_speed	= 5000,
+		.n_aochan	= 2,
+		.aobits		= 12,
+		.ao_range_table	= &range_bipolar10,
+		.ao_speed	= 100000,
+		.num_p0_dio_channels = 8,
+		.caldac		= { ad8804_debug },	/* manual is wrong */
+		.has_8255	= 1,
+	},
+	[BOARD_PXI6025E] = {
+		.name		= "pxi-6025e",
+		.n_adchan	= 16,
+		.adbits		= 12,
+		.ai_fifo_depth	= 512,
+		.gainlkup	= ai_gain_4,
+		.ai_speed	= 5000,
+		.n_aochan	= 2,
+		.aobits		= 12,
+		.ao_range_table	= &range_ni_E_ao_ext,
+		.ao_unipolar	= 1,
+		.ao_speed	= 100000,
+		.num_p0_dio_channels = 8,
+		.caldac		= { ad8804_debug },	/* manual is wrong */
+		.has_8255	= 1,
+	},
+	[BOARD_PCI6034E] = {
+		.name		= "pci-6034e",
+		.n_adchan	= 16,
+		.adbits		= 16,
+		.ai_fifo_depth	= 512,
+		.alwaysdither	= 1,
+		.gainlkup	= ai_gain_4,
+		.ai_speed	= 5000,
+		.num_p0_dio_channels = 8,
+		.caldac		= { ad8804_debug },
+	},
+	[BOARD_PCI6035E] = {
+		.name		= "pci-6035e",
+		.n_adchan	= 16,
+		.adbits		= 16,
+		.ai_fifo_depth	= 512,
+		.alwaysdither	= 1,
+		.gainlkup	= ai_gain_4,
+		.ai_speed	= 5000,
+		.n_aochan	= 2,
+		.aobits		= 12,
+		.ao_range_table	= &range_bipolar10,
+		.ao_speed	= 100000,
+		.num_p0_dio_channels = 8,
+		.caldac		= { ad8804_debug },
+	},
+	[BOARD_PCI6052E] = {
+		.name		= "pci-6052e",
+		.n_adchan	= 16,
+		.adbits		= 16,
+		.ai_fifo_depth	= 512,
+		.alwaysdither	= 1,
+		.gainlkup	= ai_gain_16,
+		.ai_speed	= 3000,
+		.n_aochan	= 2,
+		.aobits		= 16,
+		.ao_unipolar	= 1,
+		.ao_fifo_depth	= 2048,
+		.ao_range_table	= &range_ni_E_ao_ext,
+		.ao_speed	= 3000,
+		.num_p0_dio_channels = 8,
+		/* manual is wrong */
+		.caldac		= { ad8804_debug, ad8804_debug, ad8522 },
+	},
+	[BOARD_PCI6110] = {
+		.name		= "pci-6110",
+		.n_adchan	= 4,
+		.adbits		= 12,
+		.ai_fifo_depth	= 8192,
+		.alwaysdither	= 0,
+		.gainlkup	= ai_gain_611x,
+		.ai_speed	= 200,
+		.n_aochan	= 2,
+		.aobits		= 16,
+		.reg_type	= ni_reg_611x,
+		.ao_range_table	= &range_bipolar10,
+		.ao_fifo_depth	= 2048,
+		.ao_speed	= 250,
+		.num_p0_dio_channels = 8,
+		.caldac		= { ad8804, ad8804 },
+	},
+	[BOARD_PCI6111] = {
+		.name		= "pci-6111",
+		.n_adchan	= 2,
+		.adbits		= 12,
+		.ai_fifo_depth	= 8192,
+		.gainlkup	= ai_gain_611x,
+		.ai_speed	= 200,
+		.n_aochan	= 2,
+		.aobits		= 16,
+		.reg_type	= ni_reg_611x,
+		.ao_range_table	= &range_bipolar10,
+		.ao_fifo_depth	= 2048,
+		.ao_speed	= 250,
+		.num_p0_dio_channels = 8,
+		.caldac		= { ad8804, ad8804 },
+	},
 #if 0
 	/* The 6115 boards probably need their own driver */
-	{
-	 .device_id = 0x2ed0,
-	 .name = "pci-6115",
-	 .n_adchan = 4,
-	 .adbits = 12,
-	 .ai_fifo_depth = 8192,
-	 .alwaysdither = 0,
-	 .gainlkup = ai_gain_611x,
-	 .ai_speed = 100,
-	 .n_aochan = 2,
-	 .aobits = 16,
-	 .ao_671x = 1,
-	 .ao_unipolar = 0,
-	 .ao_fifo_depth = 2048,
-	 .ao_speed = 250,
-	 .num_p0_dio_channels = 8,
-	 .reg_611x = 1,
-	 .caldac = {ad8804_debug, ad8804_debug, ad8804_debug},	/* XXX */
-	 },
+	[BOARD_PCI6115] = {	/* .device_id = 0x2ed0, */
+		.name		= "pci-6115",
+		.n_adchan	= 4,
+		.adbits		= 12,
+		.ai_fifo_depth	= 8192,
+		.gainlkup	= ai_gain_611x,
+		.ai_speed	= 100,
+		.n_aochan	= 2,
+		.aobits		= 16,
+		.ao_671x	= 1,
+		.ao_fifo_depth	= 2048,
+		.ao_speed	= 250,
+		.num_p0_dio_channels = 8,
+		.reg_611x	= 1,
+		/* XXX */
+		.caldac		= { ad8804_debug, ad8804_debug, ad8804_debug },
+	},
 #endif
 #if 0
-	{
-	 .device_id = 0x0000,
-	 .name = "pxi-6115",
-	 .n_adchan = 4,
-	 .adbits = 12,
-	 .ai_fifo_depth = 8192,
-	 .alwaysdither = 0,
-	 .gainlkup = ai_gain_611x,
-	 .ai_speed = 100,
-	 .n_aochan = 2,
-	 .aobits = 16,
-	 .ao_671x = 1,
-	 .ao_unipolar = 0,
-	 .ao_fifo_depth = 2048,
-	 .ao_speed = 250,
-	 .reg_611x = 1,
-	 .num_p0_dio_channels = 8,
-	 caldac = {ad8804_debug, ad8804_debug, ad8804_debug},	/* XXX */
-	 },
+	[BOARD_PXI6115] = {	/* .device_id = ????, */
+		.name		= "pxi-6115",
+		.n_adchan	= 4,
+		.adbits		= 12,
+		.ai_fifo_depth	= 8192,
+		.gainlkup	= ai_gain_611x,
+		.ai_speed	= 100,
+		.n_aochan	= 2,
+		.aobits		= 16,
+		.ao_671x	= 1,
+		.ao_fifo_depth	= 2048,
+		.ao_speed	= 250,
+		.reg_611x	= 1,
+		.num_p0_dio_channels = 8,
+		/* XXX */
+		.caldac		= { ad8804_debug, ad8804_debug, ad8804_debug },
+	},
 #endif
-	{
-	 .device_id = 0x1880,
-	 .name = "pci-6711",
-	 .n_adchan = 0,		/* no analog input */
-	 .n_aochan = 4,
-	 .aobits = 12,
-	 .ao_unipolar = 0,
-	 .ao_fifo_depth = 16384,
-	 /* data sheet says 8192, but fifo really holds 16384 samples */
-	 .ao_range_table = &range_bipolar10,
-	 .ao_speed = 1000,
-	 .num_p0_dio_channels = 8,
-	 .reg_type = ni_reg_6711,
-	 .caldac = {ad8804_debug},
-	 },
-	{
-	 .device_id = 0x2b90,
-	 .name = "pxi-6711",
-	 .n_adchan = 0,		/* no analog input */
-	 .n_aochan = 4,
-	 .aobits = 12,
-	 .ao_unipolar = 0,
-	 .ao_fifo_depth = 16384,
-	 .ao_range_table = &range_bipolar10,
-	 .ao_speed = 1000,
-	 .num_p0_dio_channels = 8,
-	 .reg_type = ni_reg_6711,
-	 .caldac = {ad8804_debug},
-	 },
-	{
-	 .device_id = 0x1870,
-	 .name = "pci-6713",
-	 .n_adchan = 0,		/* no analog input */
-	 .n_aochan = 8,
-	 .aobits = 12,
-	 .ao_unipolar = 0,
-	 .ao_fifo_depth = 16384,
-	 .ao_range_table = &range_bipolar10,
-	 .ao_speed = 1000,
-	 .num_p0_dio_channels = 8,
-	 .reg_type = ni_reg_6713,
-	 .caldac = {ad8804_debug, ad8804_debug},
-	 },
-	{
-	 .device_id = 0x2b80,
-	 .name = "pxi-6713",
-	 .n_adchan = 0,		/* no analog input */
-	 .n_aochan = 8,
-	 .aobits = 12,
-	 .ao_unipolar = 0,
-	 .ao_fifo_depth = 16384,
-	 .ao_range_table = &range_bipolar10,
-	 .ao_speed = 1000,
-	 .num_p0_dio_channels = 8,
-	 .reg_type = ni_reg_6713,
-	 .caldac = {ad8804_debug, ad8804_debug},
-	 },
-	{
-	 .device_id = 0x2430,
-	 .name = "pci-6731",
-	 .n_adchan = 0,		/* no analog input */
-	 .n_aochan = 4,
-	 .aobits = 16,
-	 .ao_unipolar = 0,
-	 .ao_fifo_depth = 8192,
-	 .ao_range_table = &range_bipolar10,
-	 .ao_speed = 1000,
-	 .num_p0_dio_channels = 8,
-	 .reg_type = ni_reg_6711,
-	 .caldac = {ad8804_debug},
-	 },
-#if 0				/* need device ids */
-	{
-	 .device_id = 0x0,
-	 .name = "pxi-6731",
-	 .n_adchan = 0,		/* no analog input */
-	 .n_aochan = 4,
-	 .aobits = 16,
-	 .ao_unipolar = 0,
-	 .ao_fifo_depth = 8192,
-	 .ao_range_table = &range_bipolar10,
-	 .num_p0_dio_channels = 8,
-	 .reg_type = ni_reg_6711,
-	 .caldac = {ad8804_debug},
-	 },
+	[BOARD_PCI6711] = {
+		.name = "pci-6711",
+		.n_aochan	= 4,
+		.aobits		= 12,
+		/* data sheet says 8192, but fifo really holds 16384 samples */
+		.ao_fifo_depth	= 16384,
+		.ao_range_table	= &range_bipolar10,
+		.ao_speed	= 1000,
+		.num_p0_dio_channels = 8,
+		.reg_type	= ni_reg_6711,
+		.caldac		= { ad8804_debug },
+	},
+	[BOARD_PXI6711] = {
+		.name		= "pxi-6711",
+		.n_aochan	= 4,
+		.aobits		= 12,
+		.ao_fifo_depth	= 16384,
+		.ao_range_table	= &range_bipolar10,
+		.ao_speed	= 1000,
+		.num_p0_dio_channels = 8,
+		.reg_type	= ni_reg_6711,
+		.caldac		= { ad8804_debug },
+	},
+	[BOARD_PCI6713] = {
+		.name		= "pci-6713",
+		.n_aochan	= 8,
+		.aobits		= 12,
+		.ao_fifo_depth	= 16384,
+		.ao_range_table	= &range_bipolar10,
+		.ao_speed	= 1000,
+		.num_p0_dio_channels = 8,
+		.reg_type	= ni_reg_6713,
+		.caldac		= { ad8804_debug, ad8804_debug },
+	},
+	[BOARD_PXI6713] = {
+		.name		= "pxi-6713",
+		.n_aochan	= 8,
+		.aobits		= 12,
+		.ao_fifo_depth	= 16384,
+		.ao_range_table	= &range_bipolar10,
+		.ao_speed	= 1000,
+		.num_p0_dio_channels = 8,
+		.reg_type	= ni_reg_6713,
+		.caldac		= { ad8804_debug, ad8804_debug },
+	},
+	[BOARD_PCI6731] = {
+		.name		= "pci-6731",
+		.n_aochan	= 4,
+		.aobits		= 16,
+		.ao_fifo_depth	= 8192,
+		.ao_range_table	= &range_bipolar10,
+		.ao_speed	= 1000,
+		.num_p0_dio_channels = 8,
+		.reg_type	= ni_reg_6711,
+		.caldac		= { ad8804_debug },
+	},
+#if 0
+	[BOARD_PXI6731] = {	/* .device_id = ????, */
+		.name		= "pxi-6731",
+		.n_aochan	= 4,
+		.aobits		= 16,
+		.ao_fifo_depth	= 8192,
+		.ao_range_table	= &range_bipolar10,
+		.num_p0_dio_channels = 8,
+		.reg_type	= ni_reg_6711,
+		.caldac		= { ad8804_debug },
+	},
 #endif
-	{
-	 .device_id = 0x2410,
-	 .name = "pci-6733",
-	 .n_adchan = 0,		/* no analog input */
-	 .n_aochan = 8,
-	 .aobits = 16,
-	 .ao_unipolar = 0,
-	 .ao_fifo_depth = 16384,
-	 .ao_range_table = &range_bipolar10,
-	 .ao_speed = 1000,
-	 .num_p0_dio_channels = 8,
-	 .reg_type = ni_reg_6713,
-	 .caldac = {ad8804_debug, ad8804_debug},
-	 },
-	{
-	 .device_id = 0x2420,
-	 .name = "pxi-6733",
-	 .n_adchan = 0,		/* no analog input */
-	 .n_aochan = 8,
-	 .aobits = 16,
-	 .ao_unipolar = 0,
-	 .ao_fifo_depth = 16384,
-	 .ao_range_table = &range_bipolar10,
-	 .ao_speed = 1000,
-	 .num_p0_dio_channels = 8,
-	 .reg_type = ni_reg_6713,
-	 .caldac = {ad8804_debug, ad8804_debug},
-	 },
-	{
-	 .device_id = 0x15b0,
-	 .name = "pxi-6071e",
-	 .n_adchan = 64,
-	 .adbits = 12,
-	 .ai_fifo_depth = 512,
-	 .alwaysdither = 1,
-	 .gainlkup = ai_gain_16,
-	 .ai_speed = 800,
-	 .n_aochan = 2,
-	 .aobits = 12,
-	 .ao_fifo_depth = 2048,
-	 .ao_range_table = &range_ni_E_ao_ext,
-	 .ao_unipolar = 1,
-	 .ao_speed = 1000,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {ad8804_debug},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x11b0,
-	 .name = "pxi-6070e",
-	 .n_adchan = 16,
-	 .adbits = 12,
-	 .ai_fifo_depth = 512,
-	 .alwaysdither = 1,
-	 .gainlkup = ai_gain_16,
-	 .ai_speed = 800,
-	 .n_aochan = 2,
-	 .aobits = 12,
-	 .ao_fifo_depth = 2048,
-	 .ao_range_table = &range_ni_E_ao_ext,
-	 .ao_unipolar = 1,
-	 .ao_speed = 1000,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {ad8804_debug},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x18c0,
-	 .name = "pxi-6052e",
-	 .n_adchan = 16,
-	 .adbits = 16,
-	 .ai_fifo_depth = 512,
-	 .alwaysdither = 1,
-	 .gainlkup = ai_gain_16,
-	 .ai_speed = 3000,
-	 .n_aochan = 2,
-	 .aobits = 16,
-	 .ao_unipolar = 1,
-	 .ao_fifo_depth = 2048,
-	 .ao_range_table = &range_ni_E_ao_ext,
-	 .ao_speed = 3000,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {mb88341, mb88341, ad8522},
-	 },
-	{
-	 .device_id = 0x1580,
-	 .name = "pxi-6031e",
-	 .n_adchan = 64,
-	 .adbits = 16,
-	 .ai_fifo_depth = 512,
-	 .alwaysdither = 1,
-	 .gainlkup = ai_gain_14,
-	 .ai_speed = 10000,
-	 .n_aochan = 2,
-	 .aobits = 16,
-	 .ao_fifo_depth = 2048,
-	 .ao_range_table = &range_ni_E_ao_ext,
-	 .ao_unipolar = 1,
-	 .ao_speed = 10000,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {dac8800, dac8043, ad8522},
-	 },
-	{
-	 .device_id = 0x2890,
-	 .name = "pci-6036e",
-	 .n_adchan = 16,
-	 .adbits = 16,
-	 .ai_fifo_depth = 512,
-	 .alwaysdither = 1,
-	 .gainlkup = ai_gain_4,
-	 .ai_speed = 5000,
-	 .n_aochan = 2,
-	 .aobits = 16,
-	 .ao_fifo_depth = 0,
-	 .ao_range_table = &range_bipolar10,
-	 .ao_unipolar = 0,
-	 .ao_speed = 100000,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {ad8804_debug},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x70b0,
-	 .name = "pci-6220",
-	 .n_adchan = 16,
-	 .adbits = 16,
-	 .ai_fifo_depth = 512,
-	 /*      .FIXME = guess */
-	 .gainlkup = ai_gain_622x,
-	 .ai_speed = 4000,
-	 .n_aochan = 0,
-	 .aobits = 0,
-	 .ao_fifo_depth = 0,
-	 .num_p0_dio_channels = 8,
-	 .reg_type = ni_reg_622x,
-	 .ao_unipolar = 0,
-	 .caldac = {caldac_none},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x70af,
-	 .name = "pci-6221",
-	 .n_adchan = 16,
-	 .adbits = 16,
-	 .ai_fifo_depth = 4095,
-	 .gainlkup = ai_gain_622x,
-	 .ai_speed = 4000,
-	 .n_aochan = 2,
-	 .aobits = 16,
-	 .ao_fifo_depth = 8191,
-	 .ao_range_table = &range_ni_M_622x_ao,
-	 .reg_type = ni_reg_622x,
-	 .ao_unipolar = 0,
-	 .ao_speed = 1200,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {caldac_none},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x71bc,
-	 .name = "pci-6221_37pin",
-	 .n_adchan = 16,
-	 .adbits = 16,
-	 .ai_fifo_depth = 4095,
-	 .gainlkup = ai_gain_622x,
-	 .ai_speed = 4000,
-	 .n_aochan = 2,
-	 .aobits = 16,
-	 .ao_fifo_depth = 8191,
-	 .ao_range_table = &range_ni_M_622x_ao,
-	 .reg_type = ni_reg_622x,
-	 .ao_unipolar = 0,
-	 .ao_speed = 1200,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {caldac_none},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x70f2,
-	 .name = "pci-6224",
-	 .n_adchan = 32,
-	 .adbits = 16,
-	 .ai_fifo_depth = 4095,
-	 .gainlkup = ai_gain_622x,
-	 .ai_speed = 4000,
-	 .n_aochan = 0,
-	 .aobits = 0,
-	 .ao_fifo_depth = 0,
-	 .reg_type = ni_reg_622x,
-	 .ao_unipolar = 0,
-	 .num_p0_dio_channels = 32,
-	 .caldac = {caldac_none},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x70f3,
-	 .name = "pxi-6224",
-	 .n_adchan = 32,
-	 .adbits = 16,
-	 .ai_fifo_depth = 4095,
-	 .gainlkup = ai_gain_622x,
-	 .ai_speed = 4000,
-	 .n_aochan = 0,
-	 .aobits = 0,
-	 .ao_fifo_depth = 0,
-	 .reg_type = ni_reg_622x,
-	 .ao_unipolar = 0,
-	 .num_p0_dio_channels = 32,
-	 .caldac = {caldac_none},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x716c,
-	 .name = "pci-6225",
-	 .n_adchan = 80,
-	 .adbits = 16,
-	 .ai_fifo_depth = 4095,
-	 .gainlkup = ai_gain_622x,
-	 .ai_speed = 4000,
-	 .n_aochan = 2,
-	 .aobits = 16,
-	 .ao_fifo_depth = 8191,
-	 .ao_range_table = &range_ni_M_622x_ao,
-	 .reg_type = ni_reg_622x,
-	 .ao_unipolar = 0,
-	 .ao_speed = 1200,
-	 .num_p0_dio_channels = 32,
-	 .caldac = {caldac_none},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x716d,
-	 .name = "pxi-6225",
-	 .n_adchan = 80,
-	 .adbits = 16,
-	 .ai_fifo_depth = 4095,
-	 .gainlkup = ai_gain_622x,
-	 .ai_speed = 4000,
-	 .n_aochan = 2,
-	 .aobits = 16,
-	 .ao_fifo_depth = 8191,
-	 .ao_range_table = &range_ni_M_622x_ao,
-	 .reg_type = ni_reg_622x,
-	 .ao_unipolar = 0,
-	 .ao_speed = 1200,
-	 .num_p0_dio_channels = 32,
-	 .caldac = {caldac_none},
-	 .has_8255 = 0,
+	[BOARD_PCI6733] = {
+		.name		= "pci-6733",
+		.n_aochan	= 8,
+		.aobits		= 16,
+		.ao_fifo_depth	= 16384,
+		.ao_range_table	= &range_bipolar10,
+		.ao_speed	= 1000,
+		.num_p0_dio_channels = 8,
+		.reg_type	= ni_reg_6713,
+		.caldac		= { ad8804_debug, ad8804_debug },
+	},
+	[BOARD_PXI6733] = {
+		.name		= "pxi-6733",
+		.n_aochan	= 8,
+		.aobits		= 16,
+		.ao_fifo_depth	= 16384,
+		.ao_range_table	= &range_bipolar10,
+		.ao_speed	= 1000,
+		.num_p0_dio_channels = 8,
+		.reg_type	= ni_reg_6713,
+		.caldac		= { ad8804_debug, ad8804_debug },
+	},
+	[BOARD_PXI6071E] = {
+		.name		= "pxi-6071e",
+		.n_adchan	= 64,
+		.adbits		= 12,
+		.ai_fifo_depth	= 512,
+		.alwaysdither	= 1,
+		.gainlkup	= ai_gain_16,
+		.ai_speed	= 800,
+		.n_aochan	= 2,
+		.aobits		= 12,
+		.ao_fifo_depth	= 2048,
+		.ao_range_table	= &range_ni_E_ao_ext,
+		.ao_unipolar	= 1,
+		.ao_speed	= 1000,
+		.num_p0_dio_channels = 8,
+		.caldac		= { ad8804_debug },
+	},
+	[BOARD_PXI6070E] = {
+		.name		= "pxi-6070e",
+		.n_adchan	= 16,
+		.adbits		= 12,
+		.ai_fifo_depth	= 512,
+		.alwaysdither	= 1,
+		.gainlkup	= ai_gain_16,
+		.ai_speed	= 800,
+		.n_aochan	= 2,
+		.aobits		= 12,
+		.ao_fifo_depth	= 2048,
+		.ao_range_table	= &range_ni_E_ao_ext,
+		.ao_unipolar	= 1,
+		.ao_speed	= 1000,
+		.num_p0_dio_channels = 8,
+		.caldac		= { ad8804_debug },
+	},
+	[BOARD_PXI6052E] = {
+		.name		= "pxi-6052e",
+		.n_adchan	= 16,
+		.adbits		= 16,
+		.ai_fifo_depth	= 512,
+		.alwaysdither	= 1,
+		.gainlkup	= ai_gain_16,
+		.ai_speed	= 3000,
+		.n_aochan	= 2,
+		.aobits		= 16,
+		.ao_unipolar	= 1,
+		.ao_fifo_depth	= 2048,
+		.ao_range_table	= &range_ni_E_ao_ext,
+		.ao_speed	= 3000,
+		.num_p0_dio_channels = 8,
+		.caldac		= { mb88341, mb88341, ad8522 },
+	},
+	[BOARD_PXI6031E] = {
+		.name		= "pxi-6031e",
+		.n_adchan	= 64,
+		.adbits		= 16,
+		.ai_fifo_depth	= 512,
+		.alwaysdither	= 1,
+		.gainlkup	= ai_gain_14,
+		.ai_speed	= 10000,
+		.n_aochan	= 2,
+		.aobits		= 16,
+		.ao_fifo_depth	= 2048,
+		.ao_range_table	= &range_ni_E_ao_ext,
+		.ao_unipolar	= 1,
+		.ao_speed	= 10000,
+		.num_p0_dio_channels = 8,
+		.caldac		= { dac8800, dac8043, ad8522 },
+	},
+	[BOARD_PCI6036E] = {
+		.name = "pci-6036e",
+		.n_adchan	= 16,
+		.adbits		= 16,
+		.ai_fifo_depth	= 512,
+		.alwaysdither	= 1,
+		.gainlkup	= ai_gain_4,
+		.ai_speed	= 5000,
+		.n_aochan	= 2,
+		.aobits		= 16,
+		.ao_range_table	= &range_bipolar10,
+		.ao_speed	= 100000,
+		.num_p0_dio_channels = 8,
+		.caldac		= { ad8804_debug },
+	},
+	[BOARD_PCI6220] = {
+		.name		= "pci-6220",
+		.n_adchan	= 16,
+		.adbits		= 16,
+		.ai_fifo_depth	= 512,		/* FIXME: guess */
+		.gainlkup	= ai_gain_622x,
+		.ai_speed	= 4000,
+		.num_p0_dio_channels = 8,
+		.reg_type	= ni_reg_622x,
+		.caldac		= { caldac_none },
+	},
+	[BOARD_PCI6221] = {
+		.name		= "pci-6221",
+		.n_adchan	= 16,
+		.adbits		= 16,
+		.ai_fifo_depth	= 4095,
+		.gainlkup	= ai_gain_622x,
+		.ai_speed	= 4000,
+		.n_aochan	= 2,
+		.aobits		= 16,
+		.ao_fifo_depth	= 8191,
+		.ao_range_table	= &range_bipolar10,
+		.reg_type	= ni_reg_622x,
+		.ao_speed	= 1200,
+		.num_p0_dio_channels = 8,
+		.caldac		= { caldac_none },
+	},
+	[BOARD_PCI6221_37PIN] = {
+		.name		= "pci-6221_37pin",
+		.n_adchan	= 16,
+		.adbits		= 16,
+		.ai_fifo_depth	= 4095,
+		.gainlkup	= ai_gain_622x,
+		.ai_speed	= 4000,
+		.n_aochan	= 2,
+		.aobits		= 16,
+		.ao_fifo_depth	= 8191,
+		.ao_range_table	= &range_bipolar10,
+		.reg_type	= ni_reg_622x,
+		.ao_speed	= 1200,
+		.num_p0_dio_channels = 8,
+		.caldac		= { caldac_none },
+	},
+	[BOARD_PCI6224] = {
+		.name		= "pci-6224",
+		.n_adchan	= 32,
+		.adbits		= 16,
+		.ai_fifo_depth	= 4095,
+		.gainlkup	= ai_gain_622x,
+		.ai_speed	= 4000,
+		.reg_type	= ni_reg_622x,
+		.num_p0_dio_channels = 32,
+		.caldac		= { caldac_none },
+	},
+	[BOARD_PXI6224] = {
+		.name		= "pxi-6224",
+		.n_adchan	= 32,
+		.adbits		= 16,
+		.ai_fifo_depth	= 4095,
+		.gainlkup	= ai_gain_622x,
+		.ai_speed	= 4000,
+		.reg_type	= ni_reg_622x,
+		.num_p0_dio_channels = 32,
+		.caldac		= { caldac_none },
+	},
+	[BOARD_PCI6225] = {
+		.name		= "pci-6225",
+		.n_adchan	= 80,
+		.adbits		= 16,
+		.ai_fifo_depth	= 4095,
+		.gainlkup	= ai_gain_622x,
+		.ai_speed	= 4000,
+		.n_aochan	= 2,
+		.aobits		= 16,
+		.ao_fifo_depth	= 8191,
+		.ao_range_table	= &range_bipolar10,
+		.reg_type	= ni_reg_622x,
+		.ao_speed	= 1200,
+		.num_p0_dio_channels = 32,
+		.caldac		= { caldac_none },
+	},
+	[BOARD_PXI6225] = {
+		.name		= "pxi-6225",
+		.n_adchan	= 80,
+		.adbits		= 16,
+		.ai_fifo_depth	= 4095,
+		.gainlkup	= ai_gain_622x,
+		.ai_speed	= 4000,
+		.n_aochan	= 2,
+		.aobits		= 16,
+		.ao_fifo_depth	= 8191,
+		.ao_range_table	= &range_bipolar10,
+		.reg_type	= ni_reg_622x,
+		.ao_speed	= 1200,
+		.num_p0_dio_channels = 32,
+		.caldac		= { caldac_none },
+	},
+	[BOARD_PCI6229] = {
+		.name		= "pci-6229",
+		.n_adchan	= 32,
+		.adbits		= 16,
+		.ai_fifo_depth	= 4095,
+		.gainlkup	= ai_gain_622x,
+		.ai_speed	= 4000,
+		.n_aochan	= 4,
+		.aobits		= 16,
+		.ao_fifo_depth	= 8191,
+		.ao_range_table	= &range_bipolar10,
+		.reg_type	= ni_reg_622x,
+		.ao_speed	= 1200,
+		.num_p0_dio_channels = 32,
+		.caldac		= { caldac_none },
+	},
+	[BOARD_PCI6250] = {
+		.name		= "pci-6250",
+		.n_adchan	= 16,
+		.adbits		= 16,
+		.ai_fifo_depth	= 4095,
+		.gainlkup	= ai_gain_628x,
+		.ai_speed	= 800,
+		.reg_type	= ni_reg_625x,
+		.num_p0_dio_channels = 8,
+		.caldac		= { caldac_none },
+	},
+	[BOARD_PCI6251] = {
+		.name		= "pci-6251",
+		.n_adchan	= 16,
+		.adbits		= 16,
+		.ai_fifo_depth	= 4095,
+		.gainlkup	= ai_gain_628x,
+		.ai_speed	= 800,
+		.n_aochan	= 2,
+		.aobits		= 16,
+		.ao_fifo_depth	= 8191,
+		.ao_range_table	= &range_ni_M_625x_ao,
+		.reg_type	= ni_reg_625x,
+		.ao_speed	= 350,
+		.num_p0_dio_channels = 8,
+		.caldac		= { caldac_none },
+	},
+	[BOARD_PCIE6251] = {
+		.name		= "pcie-6251",
+		.n_adchan	= 16,
+		.adbits		= 16,
+		.ai_fifo_depth	= 4095,
+		.gainlkup	= ai_gain_628x,
+		.ai_speed	= 800,
+		.n_aochan	= 2,
+		.aobits		= 16,
+		.ao_fifo_depth	= 8191,
+		.ao_range_table	= &range_ni_M_625x_ao,
+		.reg_type	= ni_reg_625x,
+		.ao_speed	= 350,
+		.num_p0_dio_channels = 8,
+		.caldac		= { caldac_none },
+	},
+	[BOARD_PXIE6251] = {
+		.name		= "pxie-6251",
+		.n_adchan	= 16,
+		.adbits		= 16,
+		.ai_fifo_depth	= 4095,
+		.gainlkup	= ai_gain_628x,
+		.ai_speed	= 800,
+		.n_aochan	= 2,
+		.aobits		= 16,
+		.ao_fifo_depth	= 8191,
+		.ao_range_table	= &range_ni_M_625x_ao,
+		.reg_type	= ni_reg_625x,
+		.ao_speed	= 350,
+		.num_p0_dio_channels = 8,
+		.caldac		= { caldac_none },
+	},
+	[BOARD_PCI6254] = {
+		.name		= "pci-6254",
+		.n_adchan	= 32,
+		.adbits		= 16,
+		.ai_fifo_depth	= 4095,
+		.gainlkup	= ai_gain_628x,
+		.ai_speed	= 800,
+		.reg_type	= ni_reg_625x,
+		.num_p0_dio_channels = 32,
+		.caldac		= { caldac_none },
+	},
+	[BOARD_PCI6259] = {
+		.name		= "pci-6259",
+		.n_adchan	= 32,
+		.adbits		= 16,
+		.ai_fifo_depth	= 4095,
+		.gainlkup	= ai_gain_628x,
+		.ai_speed	= 800,
+		.n_aochan	= 4,
+		.aobits		= 16,
+		.ao_fifo_depth	= 8191,
+		.ao_range_table	= &range_ni_M_625x_ao,
+		.reg_type	= ni_reg_625x,
+		.ao_speed	= 350,
+		.num_p0_dio_channels = 32,
+		.caldac		= { caldac_none },
+	},
+	[BOARD_PCIE6259] = {
+		.name		= "pcie-6259",
+		.n_adchan	= 32,
+		.adbits		= 16,
+		.ai_fifo_depth	= 4095,
+		.gainlkup	= ai_gain_628x,
+		.ai_speed	= 800,
+		.n_aochan	= 4,
+		.aobits		= 16,
+		.ao_fifo_depth	= 8191,
+		.ao_range_table	= &range_ni_M_625x_ao,
+		.reg_type	= ni_reg_625x,
+		.ao_speed	= 350,
+		.num_p0_dio_channels = 32,
+		.caldac		= { caldac_none },
+	},
+	[BOARD_PCI6280] = {
+		.name		= "pci-6280",
+		.n_adchan	= 16,
+		.adbits		= 18,
+		.ai_fifo_depth	= 2047,
+		.gainlkup	= ai_gain_628x,
+		.ai_speed	= 1600,
+		.ao_fifo_depth	= 8191,
+		.reg_type	= ni_reg_628x,
+		.num_p0_dio_channels = 8,
+		.caldac		= { caldac_none },
+	},
+	[BOARD_PCI6281] = {
+		.name		= "pci-6281",
+		.n_adchan	= 16,
+		.adbits		= 18,
+		.ai_fifo_depth	= 2047,
+		.gainlkup	= ai_gain_628x,
+		.ai_speed	= 1600,
+		.n_aochan	= 2,
+		.aobits		= 16,
+		.ao_fifo_depth	= 8191,
+		.ao_range_table = &range_ni_M_628x_ao,
+		.reg_type	= ni_reg_628x,
+		.ao_unipolar	= 1,
+		.ao_speed	= 350,
+		.num_p0_dio_channels = 8,
+		.caldac		= { caldac_none },
+	},
+	[BOARD_PXI6281] = {
+		.name		= "pxi-6281",
+		.n_adchan	= 16,
+		.adbits		= 18,
+		.ai_fifo_depth	= 2047,
+		.gainlkup	= ai_gain_628x,
+		.ai_speed	= 1600,
+		.n_aochan	= 2,
+		.aobits		= 16,
+		.ao_fifo_depth	= 8191,
+		.ao_range_table	= &range_ni_M_628x_ao,
+		.reg_type	= ni_reg_628x,
+		.ao_unipolar	= 1,
+		.ao_speed	= 350,
+		.num_p0_dio_channels = 8,
+		.caldac		= { caldac_none },
+	},
+	[BOARD_PCI6284] = {
+		.name		= "pci-6284",
+		.n_adchan	= 32,
+		.adbits		= 18,
+		.ai_fifo_depth	= 2047,
+		.gainlkup	= ai_gain_628x,
+		.ai_speed	= 1600,
+		.reg_type	= ni_reg_628x,
+		.num_p0_dio_channels = 32,
+		.caldac		= { caldac_none },
+	},
+	[BOARD_PCI6289] = {
+		.name		= "pci-6289",
+		.n_adchan	= 32,
+		.adbits		= 18,
+		.ai_fifo_depth	= 2047,
+		.gainlkup	= ai_gain_628x,
+		.ai_speed	= 1600,
+		.n_aochan	= 4,
+		.aobits		= 16,
+		.ao_fifo_depth	= 8191,
+		.ao_range_table	= &range_ni_M_628x_ao,
+		.reg_type	= ni_reg_628x,
+		.ao_unipolar	= 1,
+		.ao_speed	= 350,
+		.num_p0_dio_channels = 32,
+		.caldac		= { caldac_none },
+	},
+	[BOARD_PCI6143] = {
+		.name		= "pci-6143",
+		.n_adchan	= 8,
+		.adbits		= 16,
+		.ai_fifo_depth	= 1024,
+		.gainlkup	= ai_gain_6143,
+		.ai_speed	= 4000,
+		.reg_type	= ni_reg_6143,
+		.num_p0_dio_channels = 8,
+		.caldac		= { ad8804_debug, ad8804_debug },
+	},
+	[BOARD_PXI6143] = {
+		.name		= "pxi-6143",
+		.n_adchan	= 8,
+		.adbits		= 16,
+		.ai_fifo_depth	= 1024,
+		.gainlkup	= ai_gain_6143,
+		.ai_speed	= 4000,
+		.reg_type	= ni_reg_6143,
+		.num_p0_dio_channels = 8,
+		.caldac		= { ad8804_debug, ad8804_debug },
 	},
-	{
-	 .device_id = 0x70aa,
-	 .name = "pci-6229",
-	 .n_adchan = 32,
-	 .adbits = 16,
-	 .ai_fifo_depth = 4095,
-	 .gainlkup = ai_gain_622x,
-	 .ai_speed = 4000,
-	 .n_aochan = 4,
-	 .aobits = 16,
-	 .ao_fifo_depth = 8191,
-	 .ao_range_table = &range_ni_M_622x_ao,
-	 .reg_type = ni_reg_622x,
-	 .ao_unipolar = 0,
-	 .ao_speed = 1200,
-	 .num_p0_dio_channels = 32,
-	 .caldac = {caldac_none},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x70b4,
-	 .name = "pci-6250",
-	 .n_adchan = 16,
-	 .adbits = 16,
-	 .ai_fifo_depth = 4095,
-	 .gainlkup = ai_gain_628x,
-	 .ai_speed = 800,
-	 .n_aochan = 0,
-	 .aobits = 0,
-	 .ao_fifo_depth = 0,
-	 .reg_type = ni_reg_625x,
-	 .ao_unipolar = 0,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {caldac_none},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x70b8,
-	 .name = "pci-6251",
-	 .n_adchan = 16,
-	 .adbits = 16,
-	 .ai_fifo_depth = 4095,
-	 .gainlkup = ai_gain_628x,
-	 .ai_speed = 800,
-	 .n_aochan = 2,
-	 .aobits = 16,
-	 .ao_fifo_depth = 8191,
-	 .ao_range_table = &range_ni_M_625x_ao,
-	 .reg_type = ni_reg_625x,
-	 .ao_unipolar = 0,
-	 .ao_speed = 350,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {caldac_none},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x717d,
-	 .name = "pcie-6251",
-	 .n_adchan = 16,
-	 .adbits = 16,
-	 .ai_fifo_depth = 4095,
-	 .gainlkup = ai_gain_628x,
-	 .ai_speed = 800,
-	 .n_aochan = 2,
-	 .aobits = 16,
-	 .ao_fifo_depth = 8191,
-	 .ao_range_table = &range_ni_M_625x_ao,
-	 .reg_type = ni_reg_625x,
-	 .ao_unipolar = 0,
-	 .ao_speed = 350,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {caldac_none},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x72e8,
-	 .name = "pxie-6251",
-	 .n_adchan = 16,
-	 .adbits = 16,
-	 .ai_fifo_depth = 4095,
-	 .gainlkup = ai_gain_628x,
-	 .ai_speed = 800,
-	 .n_aochan = 2,
-	 .aobits = 16,
-	 .ao_fifo_depth = 8191,
-	 .ao_range_table = &range_ni_M_625x_ao,
-	 .reg_type = ni_reg_625x,
-	 .ao_unipolar = 0,
-	 .ao_speed = 350,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {caldac_none},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x70b7,
-	 .name = "pci-6254",
-	 .n_adchan = 32,
-	 .adbits = 16,
-	 .ai_fifo_depth = 4095,
-	 .gainlkup = ai_gain_628x,
-	 .ai_speed = 800,
-	 .n_aochan = 0,
-	 .aobits = 0,
-	 .ao_fifo_depth = 0,
-	 .reg_type = ni_reg_625x,
-	 .ao_unipolar = 0,
-	 .num_p0_dio_channels = 32,
-	 .caldac = {caldac_none},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x70ab,
-	 .name = "pci-6259",
-	 .n_adchan = 32,
-	 .adbits = 16,
-	 .ai_fifo_depth = 4095,
-	 .gainlkup = ai_gain_628x,
-	 .ai_speed = 800,
-	 .n_aochan = 4,
-	 .aobits = 16,
-	 .ao_fifo_depth = 8191,
-	 .ao_range_table = &range_ni_M_625x_ao,
-	 .reg_type = ni_reg_625x,
-	 .ao_unipolar = 0,
-	 .ao_speed = 350,
-	 .num_p0_dio_channels = 32,
-	 .caldac = {caldac_none},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x717f,
-	 .name = "pcie-6259",
-	 .n_adchan = 32,
-	 .adbits = 16,
-	 .ai_fifo_depth = 4095,
-	 .gainlkup = ai_gain_628x,
-	 .ai_speed = 800,
-	 .n_aochan = 4,
-	 .aobits = 16,
-	 .ao_fifo_depth = 8191,
-	 .ao_range_table = &range_ni_M_625x_ao,
-	 .reg_type = ni_reg_625x,
-	 .ao_unipolar = 0,
-	 .ao_speed = 350,
-	 .num_p0_dio_channels = 32,
-	 .caldac = {caldac_none},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x70b6,
-	 .name = "pci-6280",
-	 .n_adchan = 16,
-	 .adbits = 18,
-	 .ai_fifo_depth = 2047,
-	 .gainlkup = ai_gain_628x,
-	 .ai_speed = 1600,
-	 .n_aochan = 0,
-	 .aobits = 0,
-	 .ao_fifo_depth = 8191,
-	 .reg_type = ni_reg_628x,
-	 .ao_unipolar = 0,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {caldac_none},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x70bd,
-	 .name = "pci-6281",
-	 .n_adchan = 16,
-	 .adbits = 18,
-	 .ai_fifo_depth = 2047,
-	 .gainlkup = ai_gain_628x,
-	 .ai_speed = 1600,
-	 .n_aochan = 2,
-	 .aobits = 16,
-	 .ao_fifo_depth = 8191,
-	 .ao_range_table = &range_ni_M_628x_ao,
-	 .reg_type = ni_reg_628x,
-	 .ao_unipolar = 1,
-	 .ao_speed = 350,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {caldac_none},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x70bf,
-	 .name = "pxi-6281",
-	 .n_adchan = 16,
-	 .adbits = 18,
-	 .ai_fifo_depth = 2047,
-	 .gainlkup = ai_gain_628x,
-	 .ai_speed = 1600,
-	 .n_aochan = 2,
-	 .aobits = 16,
-	 .ao_fifo_depth = 8191,
-	 .ao_range_table = &range_ni_M_628x_ao,
-	 .reg_type = ni_reg_628x,
-	 .ao_unipolar = 1,
-	 .ao_speed = 350,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {caldac_none},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x70bc,
-	 .name = "pci-6284",
-	 .n_adchan = 32,
-	 .adbits = 18,
-	 .ai_fifo_depth = 2047,
-	 .gainlkup = ai_gain_628x,
-	 .ai_speed = 1600,
-	 .n_aochan = 0,
-	 .aobits = 0,
-	 .ao_fifo_depth = 0,
-	 .reg_type = ni_reg_628x,
-	 .ao_unipolar = 0,
-	 .num_p0_dio_channels = 32,
-	 .caldac = {caldac_none},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x70ac,
-	 .name = "pci-6289",
-	 .n_adchan = 32,
-	 .adbits = 18,
-	 .ai_fifo_depth = 2047,
-	 .gainlkup = ai_gain_628x,
-	 .ai_speed = 1600,
-	 .n_aochan = 4,
-	 .aobits = 16,
-	 .ao_fifo_depth = 8191,
-	 .ao_range_table = &range_ni_M_628x_ao,
-	 .reg_type = ni_reg_628x,
-	 .ao_unipolar = 1,
-	 .ao_speed = 350,
-	 .num_p0_dio_channels = 32,
-	 .caldac = {caldac_none},
-	 .has_8255 = 0,
-	 },
-	{
-	 .device_id = 0x70C0,
-	 .name = "pci-6143",
-	 .n_adchan = 8,
-	 .adbits = 16,
-	 .ai_fifo_depth = 1024,
-	 .alwaysdither = 0,
-	 .gainlkup = ai_gain_6143,
-	 .ai_speed = 4000,
-	 .n_aochan = 0,
-	 .aobits = 0,
-	 .reg_type = ni_reg_6143,
-	 .ao_unipolar = 0,
-	 .ao_fifo_depth = 0,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {ad8804_debug, ad8804_debug},
-	 },
-	{
-	 .device_id = 0x710D,
-	 .name = "pxi-6143",
-	 .n_adchan = 8,
-	 .adbits = 16,
-	 .ai_fifo_depth = 1024,
-	 .alwaysdither = 0,
-	 .gainlkup = ai_gain_6143,
-	 .ai_speed = 4000,
-	 .n_aochan = 0,
-	 .aobits = 0,
-	 .reg_type = ni_reg_6143,
-	 .ao_unipolar = 0,
-	 .ao_fifo_depth = 0,
-	 .num_p0_dio_channels = 8,
-	 .caldac = {ad8804_debug, ad8804_debug},
-	 },
 };
 
 struct ni_private {
@@ -1569,6 +1426,7 @@ static void m_series_init_eeprom_buffer(struct comedi_device *dev)
 
 static void init_6143(struct comedi_device *dev)
 {
+	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 
 	/*  Disable interrupts */
@@ -1579,7 +1437,8 @@ static void init_6143(struct comedi_device *dev)
 	ni_writeb(0x80, PipelineDelay_6143);	/*  Set EOCMode, ADCMode and pipelinedelay */
 	ni_writeb(0x00, EOC_Set_6143);	/*  Set EOC Delay */
 
-	ni_writel(boardtype.ai_fifo_depth / 2, AIFIFO_Flag_6143);	/*  Set the FIFO half full level */
+	/* Set the FIFO half full level */
+	ni_writel(board->ai_fifo_depth / 2, AIFIFO_Flag_6143);
 
 	/*  Strobe Relay disable bit */
 	devpriv->ai_calib_source_enabled = 0;
@@ -1606,48 +1465,38 @@ static void pcimio_detach(struct comedi_device *dev)
 			mite_free(devpriv->mite);
 		}
 	}
-}
-
-static const struct ni_board_struct *
-pcimio_find_boardinfo(struct pci_dev *pcidev)
-{
-	unsigned int device_id = pcidev->device;
-	unsigned int n;
-
-	for (n = 0; n < ARRAY_SIZE(ni_boards); n++) {
-		const struct ni_board_struct *board = &ni_boards[n];
-		if (board->device_id == device_id)
-			return board;
-	}
-	return NULL;
+	comedi_pci_disable(dev);
 }
 
 static int pcimio_auto_attach(struct comedi_device *dev,
-					unsigned long context_unused)
+			      unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	const struct ni_board_struct *board = NULL;
 	struct ni_private *devpriv;
 	int ret;
 
-	dev_info(dev->class_dev, "ni_pcimio: attach %s\n", pci_name(pcidev));
+	if (context < ARRAY_SIZE(ni_boards))
+		board = &ni_boards[context];
+	if (!board)
+		return -ENODEV;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
+
+	ret = comedi_pci_enable(dev);
+	if (ret)
+		return ret;
 
 	ret = ni_alloc_private(dev);
 	if (ret)
 		return ret;
 	devpriv = dev->private;
 
-	dev->board_ptr = pcimio_find_boardinfo(pcidev);
-	if (!dev->board_ptr)
-		return -ENODEV;
-
 	devpriv->mite = mite_alloc(pcidev);
 	if (!devpriv->mite)
 		return -ENOMEM;
 
-	dev_dbg(dev->class_dev, "%s\n", boardtype.name);
-	dev->board_name = boardtype.name;
-
-	if (boardtype.reg_type & ni_reg_m_series_mask) {
+	if (board->reg_type & ni_reg_m_series_mask) {
 		devpriv->stc_writew = &m_series_stc_writew;
 		devpriv->stc_readw = &m_series_stc_readw;
 		devpriv->stc_writel = &m_series_stc_writel;
@@ -1681,9 +1530,9 @@ static int pcimio_auto_attach(struct comedi_device *dev,
 	if (devpriv->gpct_mite_ring[1] == NULL)
 		return -ENOMEM;
 
-	if (boardtype.reg_type & ni_reg_m_series_mask)
+	if (board->reg_type & ni_reg_m_series_mask)
 		m_series_init_eeprom_buffer(dev);
-	if (boardtype.reg_type == ni_reg_6143)
+	if (board->reg_type == ni_reg_6143)
 		init_6143(dev);
 
 	dev->irq = mite_irq(devpriv->mite);
@@ -1788,65 +1637,66 @@ static struct comedi_driver ni_pcimio_driver = {
 };
 
 static int ni_pcimio_pci_probe(struct pci_dev *dev,
-					 const struct pci_device_id *ent)
+			       const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &ni_pcimio_driver);
+	return comedi_pci_auto_config(dev, &ni_pcimio_driver, id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(ni_pcimio_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x0162) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1170) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1180) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1190) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x11b0) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x11c0) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x11d0) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1270) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1330) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1340) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1350) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x14e0) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x14f0) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1580) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x15b0) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1880) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1870) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x18b0) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x18c0) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2410) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2420) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2430) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2890) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x28c0) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2a60) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2a70) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2a80) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2ab0) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2b80) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2b90) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2c80) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2ca0) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70aa) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70ab) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70ac) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70af) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70b0) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70b4) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70b6) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70b7) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70b8) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70bc) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70bd) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70bf) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70c0) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70f2) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x710d) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x716c) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x716d) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x717f) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x71bc) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x717d) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x72e8) },
+	{ PCI_VDEVICE(NI, 0x0162), BOARD_PCIMIO_16XE_50 },	/* 0x1620? */
+	{ PCI_VDEVICE(NI, 0x1170), BOARD_PCIMIO_16XE_10 },
+	{ PCI_VDEVICE(NI, 0x1180), BOARD_PCIMIO_16E_1 },
+	{ PCI_VDEVICE(NI, 0x1190), BOARD_PCIMIO_16E_4 },
+	{ PCI_VDEVICE(NI, 0x11b0), BOARD_PXI6070E },
+	{ PCI_VDEVICE(NI, 0x11c0), BOARD_PXI6040E },
+	{ PCI_VDEVICE(NI, 0x11d0), BOARD_PXI6030E },
+	{ PCI_VDEVICE(NI, 0x1270), BOARD_PCI6032E },
+	{ PCI_VDEVICE(NI, 0x1330), BOARD_PCI6031E },
+	{ PCI_VDEVICE(NI, 0x1340), BOARD_PCI6033E },
+	{ PCI_VDEVICE(NI, 0x1350), BOARD_PCI6071E },
+	{ PCI_VDEVICE(NI, 0x14e0), BOARD_PCI6110 },
+	{ PCI_VDEVICE(NI, 0x14f0), BOARD_PCI6111 },
+	{ PCI_VDEVICE(NI, 0x1580), BOARD_PXI6031E },
+	{ PCI_VDEVICE(NI, 0x15b0), BOARD_PXI6071E },
+	{ PCI_VDEVICE(NI, 0x1880), BOARD_PCI6711 },
+	{ PCI_VDEVICE(NI, 0x1870), BOARD_PCI6713 },
+	{ PCI_VDEVICE(NI, 0x18b0), BOARD_PCI6052E },
+	{ PCI_VDEVICE(NI, 0x18c0), BOARD_PXI6052E },
+	{ PCI_VDEVICE(NI, 0x2410), BOARD_PCI6733 },
+	{ PCI_VDEVICE(NI, 0x2420), BOARD_PXI6733 },
+	{ PCI_VDEVICE(NI, 0x2430), BOARD_PCI6731 },
+	{ PCI_VDEVICE(NI, 0x2890), BOARD_PCI6036E },
+	{ PCI_VDEVICE(NI, 0x28c0), BOARD_PCI6014 },
+	{ PCI_VDEVICE(NI, 0x2a60), BOARD_PCI6023E },
+	{ PCI_VDEVICE(NI, 0x2a70), BOARD_PCI6024E },
+	{ PCI_VDEVICE(NI, 0x2a80), BOARD_PCI6025E },
+	{ PCI_VDEVICE(NI, 0x2ab0), BOARD_PXI6025E },
+	{ PCI_VDEVICE(NI, 0x2b80), BOARD_PXI6713 },
+	{ PCI_VDEVICE(NI, 0x2b90), BOARD_PXI6711 },
+	{ PCI_VDEVICE(NI, 0x2c80), BOARD_PCI6035E },
+	{ PCI_VDEVICE(NI, 0x2ca0), BOARD_PCI6034E },
+	{ PCI_VDEVICE(NI, 0x70aa), BOARD_PCI6229 },
+	{ PCI_VDEVICE(NI, 0x70ab), BOARD_PCI6259 },
+	{ PCI_VDEVICE(NI, 0x70ac), BOARD_PCI6289 },
+	{ PCI_VDEVICE(NI, 0x70af), BOARD_PCI6221 },
+	{ PCI_VDEVICE(NI, 0x70b0), BOARD_PCI6220 },
+	{ PCI_VDEVICE(NI, 0x70b4), BOARD_PCI6250 },
+	{ PCI_VDEVICE(NI, 0x70b6), BOARD_PCI6280 },
+	{ PCI_VDEVICE(NI, 0x70b7), BOARD_PCI6254 },
+	{ PCI_VDEVICE(NI, 0x70b8), BOARD_PCI6251 },
+	{ PCI_VDEVICE(NI, 0x70bc), BOARD_PCI6284 },
+	{ PCI_VDEVICE(NI, 0x70bd), BOARD_PCI6281 },
+	{ PCI_VDEVICE(NI, 0x70bf), BOARD_PXI6281 },
+	{ PCI_VDEVICE(NI, 0x70c0), BOARD_PCI6143 },
+	{ PCI_VDEVICE(NI, 0x70f2), BOARD_PCI6224 },
+	{ PCI_VDEVICE(NI, 0x70f3), BOARD_PXI6224 },
+	{ PCI_VDEVICE(NI, 0x710d), BOARD_PXI6143 },
+	{ PCI_VDEVICE(NI, 0x716c), BOARD_PCI6225 },
+	{ PCI_VDEVICE(NI, 0x716d), BOARD_PXI6225 },
+	{ PCI_VDEVICE(NI, 0x717f), BOARD_PCIE6259 },
+	{ PCI_VDEVICE(NI, 0x71bc), BOARD_PCI6221_37PIN },
+	{ PCI_VDEVICE(NI, 0x717d), BOARD_PCIE6251 },
+	{ PCI_VDEVICE(NI, 0x72e8), BOARD_PXIE6251 },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, ni_pcimio_pci_table);
diff --git a/drivers/staging/comedi/drivers/ni_stc.h b/drivers/staging/comedi/drivers/ni_stc.h
index 504ea715533..0a613c07760 100644
--- a/drivers/staging/comedi/drivers/ni_stc.h
+++ b/drivers/staging/comedi/drivers/ni_stc.h
@@ -1421,10 +1421,6 @@ struct ni_board_struct {
 	enum caldac_enum caldac[3];
 };
 
-#define n_ni_boards  (sizeof(ni_boards)/sizeof(struct ni_board_struct))
-
-#define boardtype (*(struct ni_board_struct *)dev->board_ptr)
-
 #define MAX_N_AO_CHAN 8
 #define NUM_GPCT 2
 
diff --git a/drivers/staging/comedi/drivers/pcl711.c b/drivers/staging/comedi/drivers/pcl711.c
index 6ee5da24a96..8be2a4c503c 100644
--- a/drivers/staging/comedi/drivers/pcl711.c
+++ b/drivers/staging/comedi/drivers/pcl711.c
@@ -451,23 +451,12 @@ static int pcl711_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	const struct pcl711_board *board = comedi_board(dev);
 	struct pcl711_private *devpriv;
 	int ret;
-	unsigned long iobase;
 	unsigned int irq;
 	struct comedi_subdevice *s;
 
-	/* claim our I/O space */
-
-	iobase = it->options[0];
-	printk(KERN_INFO "comedi%d: pcl711: 0x%04lx ", dev->minor, iobase);
-	if (!request_region(iobase, PCL711_SIZE, "pcl711")) {
-		printk("I/O port conflict\n");
-		return -EIO;
-	}
-	dev->iobase = iobase;
-
-	/* there should be a sanity check here */
-
-	dev->board_name = board->name;
+	ret = comedi_request_region(dev, it->options[0], PCL711_SIZE);
+	if (ret)
+		return ret;
 
 	/* grab our IRQ */
 	irq = it->options[1];
@@ -476,7 +465,8 @@ static int pcl711_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		return -EINVAL;
 	}
 	if (irq) {
-		if (request_irq(irq, pcl711_interrupt, 0, "pcl711", dev)) {
+		if (request_irq(irq, pcl711_interrupt, 0, dev->board_name,
+			        dev)) {
 			printk(KERN_ERR "unable to allocate irq %u\n", irq);
 			return -EINVAL;
 		} else {
@@ -560,14 +550,6 @@ static int pcl711_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static void pcl711_detach(struct comedi_device *dev)
-{
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (dev->iobase)
-		release_region(dev->iobase, PCL711_SIZE);
-}
-
 static const struct pcl711_board boardtypes[] = {
 	{ "pcl711", 0, 0, 0, 5, 8, 1, 0, &range_bipolar5 },
 	{ "pcl711b", 1, 0, 0, 5, 8, 1, 7, &range_pcl711b_ai },
@@ -579,7 +561,7 @@ static struct comedi_driver pcl711_driver = {
 	.driver_name	= "pcl711",
 	.module		= THIS_MODULE,
 	.attach		= pcl711_attach,
-	.detach		= pcl711_detach,
+	.detach		= comedi_legacy_detach,
 	.board_name	= &boardtypes[0].name,
 	.num_names	= ARRAY_SIZE(boardtypes),
 	.offset		= sizeof(struct pcl711_board),
diff --git a/drivers/staging/comedi/drivers/pcl724.c b/drivers/staging/comedi/drivers/pcl724.c
index 7b3c4293c01..4f033d88eec 100644
--- a/drivers/staging/comedi/drivers/pcl724.c
+++ b/drivers/staging/comedi/drivers/pcl724.c
@@ -100,28 +100,19 @@ static int pcl724_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	const struct pcl724_board *board = comedi_board(dev);
 	struct comedi_subdevice *s;
-	unsigned long iobase;
 	unsigned int iorange;
 	int ret, i, n_subdevices;
 #ifdef PCL724_IRQ
 	unsigned int irq;
 #endif
 
-	iobase = it->options[0];
 	iorange = board->io_range;
-	if ((board->can_have96) && ((it->options[1] == 1)
-					 || (it->options[1] == 96)))
+	if ((board->can_have96) &&
+	    ((it->options[1] == 1) || (it->options[1] == 96)))
 		iorange = PCL722_96_SIZE; /* PCL-724 in 96 DIO configuration */
-	printk(KERN_INFO "comedi%d: pcl724: board=%s, 0x%03lx ", dev->minor,
-	       board->name, iobase);
-	if (!request_region(iobase, iorange, "pcl724")) {
-		printk("I/O port conflict\n");
-		return -EIO;
-	}
-
-	dev->iobase = iobase;
-
-	dev->board_name = board->name;
+	ret = comedi_request_region(dev, it->options[0], iorange);
+	if (ret)
+		return ret;
 
 #ifdef PCL724_IRQ
 	irq = 0;
@@ -134,8 +125,8 @@ static int pcl724_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 				       "DISABLING IT", irq);
 				irq = 0;	/* Bad IRQ */
 			} else {
-				if (request_irq
-				    (irq, interrupt_pcl724, 0, "pcl724", dev)) {
+				if (request_irq(irq, interrupt_pcl724, 0,
+					        dev->board_name, dev)) {
 					printk(KERN_WARNING
 					       ", unable to allocate IRQ %u, "
 					       "DISABLING IT", irq);
@@ -178,19 +169,11 @@ static int pcl724_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 static void pcl724_detach(struct comedi_device *dev)
 {
-	const struct pcl724_board *board = comedi_board(dev);
-	struct comedi_subdevice *s;
 	int i;
 
-	for (i = 0; i < dev->n_subdevices; i++) {
-		s = &dev->subdevices[i];
-		subdev_8255_cleanup(dev, s);
-	}
-#ifdef PCL724_IRQ
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-#endif
-	release_region(dev->iobase, board->io_range);
+	for (i = 0; i < dev->n_subdevices; i++)
+		comedi_spriv_free(dev, i);
+	comedi_legacy_detach(dev);
 }
 
 static const struct pcl724_board boardtypes[] = {
diff --git a/drivers/staging/comedi/drivers/pcl725.c b/drivers/staging/comedi/drivers/pcl725.c
index 21fbc1ae1e7..6b02f0631b4 100644
--- a/drivers/staging/comedi/drivers/pcl725.c
+++ b/drivers/staging/comedi/drivers/pcl725.c
@@ -45,18 +45,11 @@ static int pcl725_di_insn(struct comedi_device *dev, struct comedi_subdevice *s,
 static int pcl725_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	struct comedi_subdevice *s;
-	unsigned long iobase;
 	int ret;
 
-	iobase = it->options[0];
-	printk(KERN_INFO "comedi%d: pcl725: 0x%04lx ", dev->minor, iobase);
-	if (!request_region(iobase, PCL725_SIZE, "pcl725")) {
-		printk("I/O port conflict\n");
-		return -EIO;
-	}
-	dev->board_name = "pcl725";
-	dev->iobase = iobase;
-	dev->irq = 0;
+	ret = comedi_request_region(dev, it->options[0], PCL725_SIZE);
+	if (ret)
+		return ret;
 
 	ret = comedi_alloc_subdevices(dev, 2);
 	if (ret)
@@ -85,17 +78,11 @@ static int pcl725_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static void pcl725_detach(struct comedi_device *dev)
-{
-	if (dev->iobase)
-		release_region(dev->iobase, PCL725_SIZE);
-}
-
 static struct comedi_driver pcl725_driver = {
 	.driver_name	= "pcl725",
 	.module		= THIS_MODULE,
 	.attach		= pcl725_attach,
-	.detach		= pcl725_detach,
+	.detach		= comedi_legacy_detach,
 };
 module_comedi_driver(pcl725_driver);
 
diff --git a/drivers/staging/comedi/drivers/pcl726.c b/drivers/staging/comedi/drivers/pcl726.c
index 50e01968f19..4aa994393fa 100644
--- a/drivers/staging/comedi/drivers/pcl726.c
+++ b/drivers/staging/comedi/drivers/pcl726.c
@@ -90,9 +90,6 @@ Interrupts are not supported.
 #define PCL727_DI_HI  0
 #define PCL727_DI_LO  1
 
-static const struct comedi_lrange range_4_20mA = { 1, {RANGE_mA(4, 20)} };
-static const struct comedi_lrange range_0_20mA = { 1, {RANGE_mA(0, 20)} };
-
 static const struct comedi_lrange *const rangelist_726[] = {
 	&range_unipolar5, &range_unipolar10,
 	&range_bipolar5, &range_bipolar10,
@@ -228,25 +225,14 @@ static int pcl726_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	const struct pcl726_board *board = comedi_board(dev);
 	struct pcl726_private *devpriv;
 	struct comedi_subdevice *s;
-	unsigned long iobase;
-	unsigned int iorange;
 	int ret, i;
 #ifdef ACL6126_IRQ
 	unsigned int irq;
 #endif
 
-	iobase = it->options[0];
-	iorange = board->io_range;
-	printk(KERN_WARNING "comedi%d: pcl726: board=%s, 0x%03lx ", dev->minor,
-	       board->name, iobase);
-	if (!request_region(iobase, iorange, "pcl726")) {
-		printk(KERN_WARNING "I/O port conflict\n");
-		return -EIO;
-	}
-
-	dev->iobase = iobase;
-
-	dev->board_name = board->name;
+	ret = comedi_request_region(dev, it->options[0], board->io_range);
+	if (ret)
+		return ret;
 
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
@@ -271,7 +257,7 @@ static int pcl726_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 				irq = 0;	/* Bad IRQ */
 			} else {
 				if (request_irq(irq, interrupt_pcl818, 0,
-						"pcl726", dev)) {
+						dev->board_name, dev)) {
 					printk(KERN_WARNING
 						", unable to allocate IRQ %d,"
 						" DISABLING IT", irq);
@@ -349,23 +335,11 @@ static int pcl726_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static void pcl726_detach(struct comedi_device *dev)
-{
-	const struct pcl726_board *board = comedi_board(dev);
-
-#ifdef ACL6126_IRQ
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-#endif
-	if (dev->iobase)
-		release_region(dev->iobase, board->io_range);
-}
-
 static struct comedi_driver pcl726_driver = {
 	.driver_name	= "pcl726",
 	.module		= THIS_MODULE,
 	.attach		= pcl726_attach,
-	.detach		= pcl726_detach,
+	.detach		= comedi_legacy_detach,
 	.board_name	= &boardtypes[0].name,
 	.num_names	= ARRAY_SIZE(boardtypes),
 	.offset		= sizeof(struct pcl726_board),
diff --git a/drivers/staging/comedi/drivers/pcl730.c b/drivers/staging/comedi/drivers/pcl730.c
index e3de4993745..2879db75da3 100644
--- a/drivers/staging/comedi/drivers/pcl730.c
+++ b/drivers/staging/comedi/drivers/pcl730.c
@@ -64,21 +64,11 @@ static int pcl730_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	const struct pcl730_board *board = comedi_board(dev);
 	struct comedi_subdevice *s;
-	unsigned long iobase;
-	unsigned int iorange;
 	int ret;
 
-	iobase = it->options[0];
-	iorange = board->io_range;
-	printk(KERN_INFO "comedi%d: pcl730: board=%s 0x%04lx ", dev->minor,
-	       board->name, iobase);
-	if (!request_region(iobase, iorange, "pcl730")) {
-		printk("I/O port conflict\n");
-		return -EIO;
-	}
-	dev->board_name = board->name;
-	dev->iobase = iobase;
-	dev->irq = 0;
+	ret = comedi_request_region(dev, it->options[0], board->io_range);
+	if (ret)
+		return ret;
 
 	ret = comedi_alloc_subdevices(dev, 4);
 	if (ret)
@@ -129,14 +119,6 @@ static int pcl730_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static void pcl730_detach(struct comedi_device *dev)
-{
-	const struct pcl730_board *board = comedi_board(dev);
-
-	if (dev->iobase)
-		release_region(dev->iobase, board->io_range);
-}
-
 static const struct pcl730_board boardtypes[] = {
 	{ "pcl730", PCL730_SIZE, },
 	{ "iso730", PCL730_SIZE, },
@@ -147,7 +129,7 @@ static struct comedi_driver pcl730_driver = {
 	.driver_name	= "pcl730",
 	.module		= THIS_MODULE,
 	.attach		= pcl730_attach,
-	.detach		= pcl730_detach,
+	.detach		= comedi_legacy_detach,
 	.board_name	= &boardtypes[0].name,
 	.num_names	= ARRAY_SIZE(boardtypes),
 	.offset		= sizeof(struct pcl730_board),
diff --git a/drivers/staging/comedi/drivers/pcl812.c b/drivers/staging/comedi/drivers/pcl812.c
index 560930e6a8e..cd02786702c 100644
--- a/drivers/staging/comedi/drivers/pcl812.c
+++ b/drivers/staging/comedi/drivers/pcl812.c
@@ -1041,28 +1041,6 @@ static void start_pacer(struct comedi_device *dev, int mode,
 /*
 ==============================================================================
 */
-static void free_resources(struct comedi_device *dev)
-{
-	const struct pcl812_board *board = comedi_board(dev);
-	struct pcl812_private *devpriv = dev->private;
-
-	if (devpriv) {
-		if (devpriv->dmabuf[0])
-			free_pages(devpriv->dmabuf[0], devpriv->dmapages[0]);
-		if (devpriv->dmabuf[1])
-			free_pages(devpriv->dmabuf[1], devpriv->dmapages[1]);
-		if (devpriv->dma)
-			free_dma(devpriv->dma);
-	}
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (dev->iobase)
-		release_region(dev->iobase, board->io_range);
-}
-
-/*
-==============================================================================
-*/
 static int pcl812_ai_cancel(struct comedi_device *dev,
 			    struct comedi_subdevice *s)
 {
@@ -1122,32 +1100,21 @@ static int pcl812_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	const struct pcl812_board *board = comedi_board(dev);
 	struct pcl812_private *devpriv;
 	int ret, subdev;
-	unsigned long iobase;
 	unsigned int irq;
 	unsigned int dma;
 	unsigned long pages;
 	struct comedi_subdevice *s;
 	int n_subdevices;
 
-	iobase = it->options[0];
-	printk(KERN_INFO "comedi%d: pcl812:  board=%s, ioport=0x%03lx",
-	       dev->minor, board->name, iobase);
-
-	if (!request_region(iobase, board->io_range, "pcl812")) {
-		printk("I/O port conflict\n");
-		return -EIO;
-	}
-	dev->iobase = iobase;
+	ret = comedi_request_region(dev, it->options[0], board->io_range);
+	if (ret)
+		return ret;
 
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
-	if (!devpriv) {
-		free_resources(dev);
+	if (!devpriv)
 		return -ENOMEM;
-	}
 	dev->private = devpriv;
 
-	dev->board_name = board->name;
-
 	irq = 0;
 	if (board->IRQbits != 0) {	/* board support IRQ */
 		irq = it->options[1];
@@ -1158,8 +1125,8 @@ static int pcl812_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 				     "DISABLING IT", irq);
 				irq = 0;	/* Bad IRQ */
 			} else {
-				if (request_irq
-				    (irq, interrupt_pcl812, 0, "pcl812", dev)) {
+				if (request_irq(irq, interrupt_pcl812, 0,
+						dev->board_name, dev)) {
 					printk
 					    (", unable to allocate IRQ %u, "
 					     "DISABLING IT", irq);
@@ -1183,7 +1150,7 @@ static int pcl812_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 			printk(", DMA is out of allowed range, FAIL!\n");
 			return -EINVAL;	/* Bad DMA */
 		}
-		ret = request_dma(dma, "pcl812");
+		ret = request_dma(dma, dev->board_name);
 		if (ret) {
 			printk(KERN_ERR ", unable to allocate DMA %u, FAIL!\n",
 			       dma);
@@ -1199,7 +1166,6 @@ static int pcl812_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 			 * maybe experiment with try_to_free_pages()
 			 * will help ....
 			 */
-			free_resources(dev);
 			return -EBUSY;	/* no buffer :-( */
 		}
 		devpriv->dmapages[0] = pages;
@@ -1208,7 +1174,6 @@ static int pcl812_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		devpriv->dmabuf[1] = __get_dma_pages(GFP_KERNEL, pages);
 		if (!devpriv->dmabuf[1]) {
 			printk(KERN_ERR ", unable to allocate DMA buffer, FAIL!\n");
-			free_resources(dev);
 			return -EBUSY;
 		}
 		devpriv->dmapages[1] = pages;
@@ -1228,10 +1193,8 @@ no_dma:
 		n_subdevices++;
 
 	ret = comedi_alloc_subdevices(dev, n_subdevices);
-	if (ret) {
-		free_resources(dev);
+	if (ret)
 		return ret;
-	}
 
 	subdev = 0;
 
@@ -1465,7 +1428,17 @@ no_dma:
 
 static void pcl812_detach(struct comedi_device *dev)
 {
-	free_resources(dev);
+	struct pcl812_private *devpriv = dev->private;
+
+	if (devpriv) {
+		if (devpriv->dmabuf[0])
+			free_pages(devpriv->dmabuf[0], devpriv->dmapages[0]);
+		if (devpriv->dmabuf[1])
+			free_pages(devpriv->dmabuf[1], devpriv->dmapages[1]);
+		if (devpriv->dma)
+			free_dma(devpriv->dma);
+	}
+	comedi_legacy_detach(dev);
 }
 
 static const struct pcl812_board boardtypes[] = {
diff --git a/drivers/staging/comedi/drivers/pcl816.c b/drivers/staging/comedi/drivers/pcl816.c
index f625fdab335..91bd2071f57 100644
--- a/drivers/staging/comedi/drivers/pcl816.c
+++ b/drivers/staging/comedi/drivers/pcl816.c
@@ -35,10 +35,10 @@ Configuration Options:
 #include "../comedidev.h"
 
 #include <linux/ioport.h>
-#include <linux/mc146818rtc.h>
 #include <linux/gfp.h>
 #include <linux/delay.h>
 #include <linux/io.h>
+#include <linux/interrupt.h>
 #include <asm/dma.h>
 
 #include "comedi_fc.h"
@@ -82,14 +82,6 @@ Configuration Options:
 #define INT_TYPE_AI1_DMA 2
 #define INT_TYPE_AI3_INT 4
 #define INT_TYPE_AI3_DMA 5
-#ifdef unused
-#define INT_TYPE_AI1_DMA_RTC 9
-#define INT_TYPE_AI3_DMA_RTC 10
-
-/* RTC stuff... */
-#define RTC_IRQ		8
-#define RTC_IO_EXTENT	0x10
-#endif
 
 #define MAGIC_DMA_WORD 0x5a5a
 
@@ -126,26 +118,14 @@ struct pcl816_board {
 	int i8254_osc_base;	/*  1/frequency of on board oscilator in ns */
 };
 
-#ifdef unused
-static int RTC_lock;	/* RTC lock */
-static int RTC_timer_lock;	/* RTC int lock */
-#endif
-
 struct pcl816_private {
 
 	unsigned int dma;	/*  used DMA, 0=don't use DMA */
-	int dma_rtc;		/*  1=RTC used with DMA, 0=no RTC alloc */
-#ifdef unused
-	unsigned long rtc_iobase;	/*  RTC port region */
-	unsigned int rtc_iosize;
-	unsigned int rtc_irq;
-#endif
 	unsigned long dmabuf[2];	/*  pointers to begin of DMA buffers */
 	unsigned int dmapages[2];	/*  len of DMA buffers in PAGE_SIZEs */
 	unsigned int hwdmaptr[2];	/*  hardware address of DMA buffers */
 	unsigned int hwdmasize[2];	/*  len of DMA buffers in Bytes */
 	unsigned int dmasamplsize;	/*  size in samples hwdmasize[0]/2 */
-	unsigned int last_top_dma;	/*  DMA pointer in last RTC int */
 	int next_dma_buf;	/*  which DMA buffer will be used next round */
 	long dma_runs_to_end;	/*  how many we must permorm DMA transfer to end of record */
 	unsigned long last_dma_run;	/*  how many bytes we must transfer on last DMA page */
@@ -154,9 +134,6 @@ struct pcl816_private {
 	unsigned char ai_neverending;	/*  if=1, then we do neverending record (you must use cancel()) */
 	int irq_free;		/*  1=have allocated IRQ */
 	int irq_blocked;	/*  1=IRQ now uses any subdev */
-#ifdef unused
-	int rtc_irq_blocked;	/*  1=we now do AI with DMA&RTC */
-#endif
 	int irq_was_now_closed;	/*  when IRQ finish, there's stored int816_mode for last interrupt */
 	int int816_mode;	/*  who now uses IRQ - 1=AI1 int, 2=AI1 dma, 3=AI3 int, 4AI3 dma */
 	struct comedi_subdevice *last_int_sub;	/*  ptr to subdevice which now finish */
@@ -167,10 +144,6 @@ struct pcl816_private {
 	unsigned int ai_n_chan;		/*  how many channels per scan */
 	unsigned int ai_poll_ptr;	/*  how many sampes transfer poll */
 	struct comedi_subdevice *sub_ai;	/*  ptr to AI subdevice */
-#ifdef unused
-	struct timer_list rtc_irq_timer;	/*  timer for RTC sanity check */
-	unsigned long rtc_freq;	/*  RTC int freq */
-#endif
 };
 
 /*
@@ -186,9 +159,6 @@ static int pcl816_ai_cancel(struct comedi_device *dev,
 			    struct comedi_subdevice *s);
 static void start_pacer(struct comedi_device *dev, int mode,
 			unsigned int divisor1, unsigned int divisor2);
-#ifdef unused
-static int set_rtc_irq_bit(unsigned char bit);
-#endif
 
 static int pcl816_ai_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s,
@@ -718,12 +688,6 @@ static int pcl816_ai_cancel(struct comedi_device *dev,
 
 	if (devpriv->irq_blocked > 0) {
 		switch (devpriv->int816_mode) {
-#ifdef unused
-		case INT_TYPE_AI1_DMA_RTC:
-		case INT_TYPE_AI3_DMA_RTC:
-			set_rtc_irq_bit(0);	/*  stop RTC */
-			del_timer(&devpriv->rtc_irq_timer);
-#endif
 		case INT_TYPE_AI1_DMA:
 		case INT_TYPE_AI3_DMA:
 			disable_dma(devpriv->dma);
@@ -837,7 +801,7 @@ start_pacer(struct comedi_device *dev, int mode, unsigned int divisor1,
 
 /*
 ==============================================================================
- Check if channel list from user is builded correctly
+ Check if channel list from user is built correctly
  If it's ok, then return non-zero length of repeated segment of channel list
 */
 static int
@@ -939,67 +903,21 @@ setup_channel_list(struct comedi_device *dev,
 	     dev->iobase + PCL816_MUX);
 }
 
-#ifdef unused
-/*
-==============================================================================
-  Enable(1)/disable(0) periodic interrupts from RTC
-*/
-static int set_rtc_irq_bit(unsigned char bit)
-{
-	unsigned char val;
-	unsigned long flags;
-
-	if (bit == 1) {
-		RTC_timer_lock++;
-		if (RTC_timer_lock > 1)
-			return 0;
-	} else {
-		RTC_timer_lock--;
-		if (RTC_timer_lock < 0)
-			RTC_timer_lock = 0;
-		if (RTC_timer_lock > 0)
-			return 0;
-	}
-
-	save_flags(flags);
-	cli();
-	val = CMOS_READ(RTC_CONTROL);
-	if (bit)
-		val |= RTC_PIE;
-	else
-		val &= ~RTC_PIE;
-
-	CMOS_WRITE(val, RTC_CONTROL);
-	CMOS_READ(RTC_INTR_FLAGS);
-	restore_flags(flags);
-	return 0;
-}
-#endif
-
 static int pcl816_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	const struct pcl816_board *board = comedi_board(dev);
 	struct pcl816_private *devpriv;
 	int ret;
-	unsigned long iobase;
 	unsigned int irq, dma;
 	unsigned long pages;
 	/* int i; */
 	struct comedi_subdevice *s;
 
-	/* claim our I/O space */
-	iobase = it->options[0];
-	printk("comedi%d: pcl816:  board=%s, ioport=0x%03lx", dev->minor,
-	       board->name, iobase);
-
-	if (!request_region(iobase, board->io_range, "pcl816")) {
-		printk("I/O port conflict\n");
-		return -EIO;
-	}
-
-	dev->iobase = iobase;
+	ret = comedi_request_region(dev, it->options[0], board->io_range);
+	if (ret)
+		return ret;
 
-	if (pcl816_check(iobase)) {
+	if (pcl816_check(dev->iobase)) {
 		printk(KERN_ERR ", I cann't detect board. FAIL!\n");
 		return -EIO;
 	}
@@ -1009,8 +927,6 @@ static int pcl816_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	dev->board_name = board->name;
-
 	/* grab our IRQ */
 	irq = 0;
 	if (board->IRQbits != 0) {	/* board support IRQ */
@@ -1022,8 +938,8 @@ static int pcl816_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 				     "DISABLING IT", irq);
 				irq = 0;	/* Bad IRQ */
 			} else {
-				if (request_irq
-				    (irq, interrupt_pcl816, 0, "pcl816", dev)) {
+				if (request_irq(irq, interrupt_pcl816, 0,
+						dev->board_name, dev)) {
 					printk
 					    (", unable to allocate IRQ %u, "
 					     "DISABLING IT", irq);
@@ -1044,46 +960,10 @@ static int pcl816_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	devpriv->irq_blocked = 0;	/* number of subdevice which use IRQ */
 	devpriv->int816_mode = 0;	/* mode of irq */
 
-#ifdef unused
-	/* grab RTC for DMA operations */
-	devpriv->dma_rtc = 0;
-	if (it->options[2] > 0) {	/*  we want to use DMA */
-		if (RTC_lock == 0) {
-			if (!request_region(RTC_PORT(0), RTC_IO_EXTENT,
-					    "pcl816 (RTC)"))
-				goto no_rtc;
-		}
-		devpriv->rtc_iobase = RTC_PORT(0);
-		devpriv->rtc_iosize = RTC_IO_EXTENT;
-		RTC_lock++;
-#ifdef UNTESTED_CODE
-		if (!request_irq(RTC_IRQ, interrupt_pcl816_ai_mode13_dma_rtc, 0,
-				 "pcl816 DMA (RTC)", dev)) {
-			devpriv->dma_rtc = 1;
-			devpriv->rtc_irq = RTC_IRQ;
-			printk(", dma_irq=%u", devpriv->rtc_irq);
-		} else {
-			RTC_lock--;
-			if (RTC_lock == 0) {
-				if (devpriv->rtc_iobase)
-					release_region(devpriv->rtc_iobase,
-						       devpriv->rtc_iosize);
-			}
-			devpriv->rtc_iobase = 0;
-			devpriv->rtc_iosize = 0;
-		}
-#else
-		printk("pcl816: RTC code missing");
-#endif
-
-	}
-
-no_rtc:
-#endif
 	/* grab our DMA */
 	dma = 0;
 	devpriv->dma = dma;
-	if ((devpriv->irq_free == 0) && (devpriv->dma_rtc == 0))
+	if (!devpriv->irq_free)
 		goto no_dma;	/* if we haven't IRQ, we can't use DMA */
 
 	if (board->DMAbits != 0) {	/* board support DMA */
@@ -1095,7 +975,7 @@ no_rtc:
 			printk(", DMA is out of allowed range, FAIL!\n");
 			return -EINVAL;	/* Bad DMA */
 		}
-		ret = request_dma(dma, "pcl816");
+		ret = request_dma(dma, dev->board_name);
 		if (ret) {
 			printk(KERN_ERR
 			       ", unable to allocate DMA %u, FAIL!\n", dma);
@@ -1120,19 +1000,16 @@ no_rtc:
 		devpriv->hwdmasize[0] = (1 << pages) * PAGE_SIZE;
 		/* printk("%d %d %ld, ",devpriv->dmapages[0],devpriv->hwdmasize[0],PAGE_SIZE); */
 
-		if (devpriv->dma_rtc == 0) {	/*  we must do duble buff :-( */
-			devpriv->dmabuf[1] = __get_dma_pages(GFP_KERNEL, pages);
-			if (!devpriv->dmabuf[1]) {
-				printk(KERN_ERR
-				       ", unable to allocate DMA buffer, "
-				       "FAIL!\n");
-				return -EBUSY;
-			}
-			devpriv->dmapages[1] = pages;
-			devpriv->hwdmaptr[1] =
-			    virt_to_bus((void *)devpriv->dmabuf[1]);
-			devpriv->hwdmasize[1] = (1 << pages) * PAGE_SIZE;
+		devpriv->dmabuf[1] = __get_dma_pages(GFP_KERNEL, pages);
+		if (!devpriv->dmabuf[1]) {
+			printk(KERN_ERR
+				", unable to allocate DMA buffer, "
+				"FAIL!\n");
+			return -EBUSY;
 		}
+		devpriv->dmapages[1] = pages;
+		devpriv->hwdmaptr[1] = virt_to_bus((void *)devpriv->dmabuf[1]);
+		devpriv->hwdmasize[1] = (1 << pages) * PAGE_SIZE;
 	}
 
 no_dma:
@@ -1205,7 +1082,6 @@ case COMEDI_SUBD_DO:
 
 static void pcl816_detach(struct comedi_device *dev)
 {
-	const struct pcl816_board *board = comedi_board(dev);
 	struct pcl816_private *devpriv = dev->private;
 
 	if (dev->private) {
@@ -1217,24 +1093,8 @@ static void pcl816_detach(struct comedi_device *dev)
 			free_pages(devpriv->dmabuf[0], devpriv->dmapages[0]);
 		if (devpriv->dmabuf[1])
 			free_pages(devpriv->dmabuf[1], devpriv->dmapages[1]);
-#ifdef unused
-		if (devpriv->rtc_irq)
-			free_irq(devpriv->rtc_irq, dev);
-		if ((devpriv->dma_rtc) && (RTC_lock == 1)) {
-			if (devpriv->rtc_iobase)
-				release_region(devpriv->rtc_iobase,
-					       devpriv->rtc_iosize);
-		}
-#endif
 	}
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (dev->iobase)
-		release_region(dev->iobase, board->io_range);
-#ifdef unused
-	if (devpriv->dma_rtc)
-		RTC_lock--;
-#endif
+	comedi_legacy_detach(dev);
 }
 
 static const struct pcl816_board boardtypes[] = {
diff --git a/drivers/staging/comedi/drivers/pcl818.c b/drivers/staging/comedi/drivers/pcl818.c
index b5af22eb7c3..91cb1bd6717 100644
--- a/drivers/staging/comedi/drivers/pcl818.c
+++ b/drivers/staging/comedi/drivers/pcl818.c
@@ -98,15 +98,15 @@ A word or two about DMA. Driver support DMA operations at two ways:
 
 */
 
-#include "../comedidev.h"
-
 #include <linux/ioport.h>
-#include <linux/mc146818rtc.h>
 #include <linux/gfp.h>
 #include <linux/delay.h>
 #include <linux/io.h>
+#include <linux/interrupt.h>
 #include <asm/dma.h>
 
+#include "../comedidev.h"
+
 #include "comedi_fc.h"
 #include "8253.h"
 
@@ -186,15 +186,6 @@ A word or two about DMA. Driver support DMA operations at two ways:
 #define INT_TYPE_AO3_INT 8
 #endif
 
-#ifdef unused
-/* RTC stuff... */
-#define INT_TYPE_AI1_DMA_RTC 9
-#define INT_TYPE_AI3_DMA_RTC 10
-
-#define RTC_IRQ 	8
-#define RTC_IO_EXTENT	0x10
-#endif
-
 #define MAGIC_DMA_WORD 0x5a5a
 
 static const struct comedi_lrange range_pcl818h_ai = { 9, {
@@ -248,11 +239,6 @@ static const struct comedi_lrange range718_bipolar0_5 = {
 static const struct comedi_lrange range718_unipolar2 = { 1, {UNI_RANGE(2),} };
 static const struct comedi_lrange range718_unipolar1 = { 1, {BIP_RANGE(1),} };
 
-#ifdef unused
-static int RTC_lock;	/* RTC lock */
-static int RTC_timer_lock;	/* RTC int lock */
-#endif
-
 struct pcl818_board {
 
 	const char *name;	/*  driver name */
@@ -277,22 +263,11 @@ struct pcl818_board {
 struct pcl818_private {
 
 	unsigned int dma;	/*  used DMA, 0=don't use DMA */
-	int dma_rtc;		/*  1=RTC used with DMA, 0=no RTC alloc */
 	unsigned int io_range;
-#ifdef unused
-	unsigned long rtc_iobase;	/*  RTC port region */
-	unsigned int rtc_iosize;
-	unsigned int rtc_irq;
-	struct timer_list rtc_irq_timer;	/*  timer for RTC sanity check */
-	unsigned long rtc_freq;	/*  RTC int freq */
-	int rtc_irq_blocked;	/*  1=we now do AI with DMA&RTC */
-#endif
 	unsigned long dmabuf[2];	/*  pointers to begin of DMA buffers */
 	unsigned int dmapages[2];	/*  len of DMA buffers in PAGE_SIZEs */
 	unsigned int hwdmaptr[2];	/*  hardware address of DMA buffers */
 	unsigned int hwdmasize[2];	/*  len of DMA buffers in Bytes */
-	unsigned int dmasamplsize;	/*  size in samples hwdmasize[0]/2 */
-	unsigned int last_top_dma;	/*  DMA pointer in last RTC int */
 	int next_dma_buf;	/*  which DMA buffer will be used next round */
 	long dma_runs_to_end;	/*  how many we must permorm DMA transfer to end of record */
 	unsigned long last_dma_run;	/*  how many bytes we must transfer on last DMA page */
@@ -342,12 +317,6 @@ static int pcl818_ai_cancel(struct comedi_device *dev,
 static void start_pacer(struct comedi_device *dev, int mode,
 			unsigned int divisor1, unsigned int divisor2);
 
-#ifdef unused
-static int set_rtc_irq_bit(unsigned char bit);
-static void rtc_dropped_irq(unsigned long data);
-static int rtc_setfreq_irq(int freq);
-#endif
-
 /*
 ==============================================================================
    ANALOG INPUT MODE0, 818 cards, slow version
@@ -610,113 +579,6 @@ static irqreturn_t interrupt_pcl818_ai_mode13_dma(int irq, void *d)
 	return IRQ_HANDLED;
 }
 
-#ifdef unused
-/*
-==============================================================================
-   analog input dma mode 1 & 3 over RTC, 818 cards
-*/
-static irqreturn_t interrupt_pcl818_ai_mode13_dma_rtc(int irq, void *d)
-{
-	struct comedi_device *dev = d;
-	struct pcl818_private *devpriv = dev->private;
-	struct comedi_subdevice *s = &dev->subdevices[0];
-	unsigned long tmp;
-	unsigned int top1, top2, i, bufptr;
-	long ofs_dats;
-	short *dmabuf = (short *)devpriv->dmabuf[0];
-
-	/* outb(2,0x378); */
-	switch (devpriv->ai_mode) {
-	case INT_TYPE_AI1_DMA_RTC:
-	case INT_TYPE_AI3_DMA_RTC:
-		tmp = (CMOS_READ(RTC_INTR_FLAGS) & 0xF0);
-		mod_timer(&devpriv->rtc_irq_timer,
-			  jiffies + HZ / devpriv->rtc_freq + 2 * HZ / 100);
-
-		for (i = 0; i < 10; i++) {
-			top1 = get_dma_residue(devpriv->dma);
-			top2 = get_dma_residue(devpriv->dma);
-			if (top1 == top2)
-				break;
-		}
-
-		if (top1 != top2)
-			return IRQ_HANDLED;
-		top1 = devpriv->hwdmasize[0] - top1;	/*  where is now DMA in buffer */
-		top1 >>= 1;
-		ofs_dats = top1 - devpriv->last_top_dma;	/*  new samples from last call */
-		if (ofs_dats < 0)
-			ofs_dats = (devpriv->dmasamplsize) + ofs_dats;
-		if (!ofs_dats)
-			return IRQ_HANDLED;	/*  exit=no new samples from last call */
-		/*  obsluz data */
-		i = devpriv->last_top_dma - 1;
-		i &= (devpriv->dmasamplsize - 1);
-
-		if (dmabuf[i] != MAGIC_DMA_WORD) {	/*  DMA overflow! */
-			comedi_error(dev, "A/D mode1/3 DMA buffer overflow!");
-			/* printk("I %d dmabuf[i] %d %d\n",i,dmabuf[i],devpriv->dmasamplsize); */
-			pcl818_ai_cancel(dev, s);
-			s->async->events |= COMEDI_CB_EOA | COMEDI_CB_ERROR;
-			comedi_event(dev, s);
-			return IRQ_HANDLED;
-		}
-		/* printk("r %ld ",ofs_dats); */
-
-		bufptr = devpriv->last_top_dma;
-
-		for (i = 0; i < ofs_dats; i++) {
-			if ((dmabuf[bufptr] & 0xf) != devpriv->act_chanlist[devpriv->act_chanlist_pos]) {	/*  dropout! */
-				printk
-				    ("comedi: A/D mode1/3 DMA - channel dropout %d!=%d !\n",
-				     (dmabuf[bufptr] & 0xf),
-				     devpriv->
-				     act_chanlist[devpriv->act_chanlist_pos]);
-				pcl818_ai_cancel(dev, s);
-				s->async->events |=
-				    COMEDI_CB_EOA | COMEDI_CB_ERROR;
-				comedi_event(dev, s);
-				return IRQ_HANDLED;
-			}
-
-			comedi_buf_put(s->async, dmabuf[bufptr++] >> 4);	/*  get one sample */
-			bufptr &= (devpriv->dmasamplsize - 1);
-
-			devpriv->act_chanlist_pos++;
-			if (devpriv->act_chanlist_pos >=
-					devpriv->act_chanlist_len) {
-				devpriv->act_chanlist_pos = 0;
-			}
-			s->async->cur_chan++;
-			if (s->async->cur_chan >= devpriv->ai_n_chan) {
-				s->async->cur_chan = 0;
-				devpriv->ai_act_scan--;
-			}
-
-			if (!devpriv->neverending_ai)
-				if (devpriv->ai_act_scan == 0) {	/* all data sampled */
-					pcl818_ai_cancel(dev, s);
-					s->async->events |= COMEDI_CB_EOA;
-					comedi_event(dev, s);
-					/* printk("done int ai13 dma\n"); */
-					return IRQ_HANDLED;
-				}
-		}
-
-		devpriv->last_top_dma = bufptr;
-		bufptr--;
-		bufptr &= (devpriv->dmasamplsize - 1);
-		dmabuf[bufptr] = MAGIC_DMA_WORD;
-		comedi_event(dev, s);
-		/* outb(0,0x378); */
-		return IRQ_HANDLED;
-	}
-
-	/* outb(0,0x378); */
-	return IRQ_HANDLED;
-}
-#endif
-
 /*
 ==============================================================================
    analog input interrupt mode 1 & 3, 818HD/HG cards
@@ -900,49 +762,6 @@ static void pcl818_ai_mode13dma_int(int mode, struct comedi_device *dev,
 	};
 }
 
-#ifdef unused
-/*
-==============================================================================
-   ANALOG INPUT MODE 1 or 3 DMA rtc, 818 cards
-*/
-static void pcl818_ai_mode13dma_rtc(int mode, struct comedi_device *dev,
-				    struct comedi_subdevice *s)
-{
-	struct pcl818_private *devpriv = dev->private;
-	unsigned int flags;
-	short *pole;
-
-	set_dma_mode(devpriv->dma, DMA_MODE_READ | DMA_AUTOINIT);
-	flags = claim_dma_lock();
-	clear_dma_ff(devpriv->dma);
-	set_dma_addr(devpriv->dma, devpriv->hwdmaptr[0]);
-	set_dma_count(devpriv->dma, devpriv->hwdmasize[0]);
-	release_dma_lock(flags);
-	enable_dma(devpriv->dma);
-	devpriv->last_top_dma = 0;	/* devpriv->hwdmasize[0]; */
-	pole = (short *)devpriv->dmabuf[0];
-	devpriv->dmasamplsize = devpriv->hwdmasize[0] / 2;
-	pole[devpriv->dmasamplsize - 1] = MAGIC_DMA_WORD;
-#ifdef unused
-	devpriv->rtc_freq = rtc_setfreq_irq(2048);
-	devpriv->rtc_irq_timer.expires =
-	    jiffies + HZ / devpriv->rtc_freq + 2 * HZ / 100;
-	devpriv->rtc_irq_timer.data = (unsigned long)dev;
-	devpriv->rtc_irq_timer.function = rtc_dropped_irq;
-
-	add_timer(&devpriv->rtc_irq_timer);
-#endif
-
-	if (mode == 1) {
-		devpriv->int818_mode = INT_TYPE_AI1_DMA_RTC;
-		outb(0x07 | (dev->irq << 4), dev->iobase + PCL818_CONTROL);	/* Pacer+DMA */
-	} else {
-		devpriv->int818_mode = INT_TYPE_AI3_DMA_RTC;
-		outb(0x06 | (dev->irq << 4), dev->iobase + PCL818_CONTROL);	/* Ext trig+DMA */
-	};
-}
-#endif
-
 /*
 ==============================================================================
    ANALOG INPUT MODE 1 or 3, 818 cards
@@ -956,7 +775,7 @@ static int pcl818_ai_cmd_mode(int mode, struct comedi_device *dev,
 	unsigned int seglen;
 
 	dev_dbg(dev->class_dev, "pcl818_ai_cmd_mode()\n");
-	if ((!dev->irq) && (!devpriv->dma_rtc)) {
+	if (!dev->irq) {
 		comedi_error(dev, "IRQ not defined!");
 		return -EINVAL;
 	}
@@ -1005,15 +824,7 @@ static int pcl818_ai_cmd_mode(int mode, struct comedi_device *dev,
 	switch (devpriv->dma) {
 	case 1:		/*  DMA */
 	case 3:
-		if (devpriv->dma_rtc == 0)
-			pcl818_ai_mode13dma_int(mode, dev, s);
-#ifdef unused
-		else
-			pcl818_ai_mode13dma_rtc(mode, dev, s);
-#else
-		else
-			return -EINVAL;
-#endif
+		pcl818_ai_mode13dma_int(mode, dev, s);
 		break;
 	case 0:
 		if (!devpriv->usefifo) {
@@ -1047,97 +858,10 @@ static int pcl818_ai_cmd_mode(int mode, struct comedi_device *dev,
 
 	start_pacer(dev, mode, divisor1, divisor2);
 
-#ifdef unused
-	switch (devpriv->ai_mode) {
-	case INT_TYPE_AI1_DMA_RTC:
-	case INT_TYPE_AI3_DMA_RTC:
-		set_rtc_irq_bit(1);	/* start RTC */
-		break;
-	}
-#endif
 	dev_dbg(dev->class_dev, "pcl818_ai_cmd_mode() end\n");
 	return 0;
 }
 
-#ifdef unused
-/*
-==============================================================================
-   ANALOG OUTPUT MODE 1 or 3, 818 cards
-*/
-#ifdef PCL818_MODE13_AO
-static int pcl818_ao_mode13(int mode, struct comedi_device *dev,
-			    struct comedi_subdevice *s, comedi_trig *it)
-{
-	struct pcl818_private *devpriv = dev->private;
-	int divisor1 = 0, divisor2 = 0;
-
-	if (!dev->irq) {
-		comedi_error(dev, "IRQ not defined!");
-		return -EINVAL;
-	}
-
-	if (devpriv->irq_blocked)
-		return -EBUSY;
-
-	start_pacer(dev, -1, 0, 0);	/*  stop pacer */
-
-	devpriv->int13_act_scan = it->n;
-	devpriv->int13_act_chan = 0;
-	devpriv->irq_blocked = 1;
-	devpriv->irq_was_now_closed = 0;
-	devpriv->neverending_ai = 0;
-	devpriv->act_chanlist_pos = 0;
-
-	if (mode == 1) {
-		i8253_cascade_ns_to_timer(devpriv->i8253_osc_base, &divisor1,
-					  &divisor2, &it->trigvar,
-					  TRIG_ROUND_NEAREST);
-		if (divisor1 == 1) {	/* PCL818 crash if any divisor is set to 1 */
-			divisor1 = 2;
-			divisor2 /= 2;
-		}
-		if (divisor2 == 1) {
-			divisor2 = 2;
-			divisor1 /= 2;
-		}
-	}
-
-	outb(0, dev->iobase + PCL818_CNTENABLE);	/* enable pacer */
-	if (mode == 1) {
-		devpriv->int818_mode = INT_TYPE_AO1_INT;
-		outb(0x83 | (dev->irq << 4), dev->iobase + PCL818_CONTROL);	/* Pacer+IRQ */
-	} else {
-		devpriv->int818_mode = INT_TYPE_AO3_INT;
-		outb(0x82 | (dev->irq << 4), dev->iobase + PCL818_CONTROL);	/* Ext trig+IRQ */
-	};
-
-	start_pacer(dev, mode, divisor1, divisor2);
-
-	return 0;
-}
-
-/*
-==============================================================================
-   ANALOG OUTPUT MODE 1, 818 cards
-*/
-static int pcl818_ao_mode1(struct comedi_device *dev,
-			   struct comedi_subdevice *s, comedi_trig *it)
-{
-	return pcl818_ao_mode13(1, dev, s, it);
-}
-
-/*
-==============================================================================
-   ANALOG OUTPUT MODE 3, 818 cards
-*/
-static int pcl818_ao_mode3(struct comedi_device *dev,
-			   struct comedi_subdevice *s, comedi_trig *it)
-{
-	return pcl818_ao_mode13(3, dev, s, it);
-}
-#endif
-#endif
-
 /*
 ==============================================================================
  Start/stop pacer onboard pacer
@@ -1391,12 +1115,6 @@ static int pcl818_ai_cancel(struct comedi_device *dev,
 		devpriv->irq_was_now_closed = 1;
 
 		switch (devpriv->ai_mode) {
-#ifdef unused
-		case INT_TYPE_AI1_DMA_RTC:
-		case INT_TYPE_AI3_DMA_RTC:
-			set_rtc_irq_bit(0);	/*  stop RTC */
-			del_timer(&devpriv->rtc_irq_timer);
-#endif
 		case INT_TYPE_AI1_DMA:
 		case INT_TYPE_AI3_DMA:
 			if (devpriv->neverending_ai ||
@@ -1499,102 +1217,11 @@ static void pcl818_reset(struct comedi_device *dev)
 	}
 }
 
-#ifdef unused
-/*
-==============================================================================
-  Enable(1)/disable(0) periodic interrupts from RTC
-*/
-static int set_rtc_irq_bit(unsigned char bit)
-{
-	unsigned char val;
-	unsigned long flags;
-
-	if (bit == 1) {
-		RTC_timer_lock++;
-		if (RTC_timer_lock > 1)
-			return 0;
-	} else {
-		RTC_timer_lock--;
-		if (RTC_timer_lock < 0)
-			RTC_timer_lock = 0;
-		if (RTC_timer_lock > 0)
-			return 0;
-	}
-
-	save_flags(flags);
-	cli();
-	val = CMOS_READ(RTC_CONTROL);
-	if (bit)
-		val |= RTC_PIE;
-	else
-		val &= ~RTC_PIE;
-
-	CMOS_WRITE(val, RTC_CONTROL);
-	CMOS_READ(RTC_INTR_FLAGS);
-	restore_flags(flags);
-	return 0;
-}
-
-/*
-==============================================================================
-  Restart RTC if something stop it (xntpd every 11 mins or large IDE transfers)
-*/
-static void rtc_dropped_irq(unsigned long data)
-{
-	struct comedi_device *dev = (void *)data;
-	struct pcl818_private *devpriv = dev->private;
-	unsigned long flags, tmp;
-
-	switch (devpriv->int818_mode) {
-	case INT_TYPE_AI1_DMA_RTC:
-	case INT_TYPE_AI3_DMA_RTC:
-		mod_timer(&devpriv->rtc_irq_timer,
-			  jiffies + HZ / devpriv->rtc_freq + 2 * HZ / 100);
-		save_flags(flags);
-		cli();
-		tmp = (CMOS_READ(RTC_INTR_FLAGS) & 0xF0);	/* restart */
-		restore_flags(flags);
-		break;
-	}
-}
-
-/*
-==============================================================================
-  Set frequency of interrupts from RTC
-*/
-static int rtc_setfreq_irq(int freq)
-{
-	int tmp = 0;
-	int rtc_freq;
-	unsigned char val;
-	unsigned long flags;
-
-	if (freq < 2)
-		freq = 2;
-	if (freq > 8192)
-		freq = 8192;
-
-	while (freq > (1 << tmp))
-		tmp++;
-
-	rtc_freq = 1 << tmp;
-
-	save_flags(flags);
-	cli();
-	val = CMOS_READ(RTC_FREQ_SELECT) & 0xf0;
-	val |= (16 - tmp);
-	CMOS_WRITE(val, RTC_FREQ_SELECT);
-	restore_flags(flags);
-	return rtc_freq;
-}
-#endif
-
 static int pcl818_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	const struct pcl818_board *board = comedi_board(dev);
 	struct pcl818_private *devpriv;
 	int ret;
-	unsigned long iobase;
 	unsigned int irq;
 	int dma;
 	unsigned long pages;
@@ -1605,31 +1232,21 @@ static int pcl818_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	/* claim our I/O space */
-	iobase = it->options[0];
-	printk
-	    ("comedi%d: pcl818:  board=%s, ioport=0x%03lx",
-	     dev->minor, board->name, iobase);
 	devpriv->io_range = board->io_range;
 	if ((board->fifo) && (it->options[2] == -1)) {
 		/*  we've board with FIFO and we want to use FIFO */
 		devpriv->io_range = PCLx1xFIFO_RANGE;
 		devpriv->usefifo = 1;
 	}
-	if (!request_region(iobase, devpriv->io_range, "pcl818")) {
-		comedi_error(dev, "I/O port conflict\n");
-		return -EIO;
-	}
-
-	dev->iobase = iobase;
+	ret = comedi_request_region(dev, it->options[0], devpriv->io_range);
+	if (ret)
+		return ret;
 
-	if (pcl818_check(iobase)) {
+	if (pcl818_check(dev->iobase)) {
 		comedi_error(dev, "I can't detect board. FAIL!\n");
 		return -EIO;
 	}
 
-	dev->board_name = board->name;
-
 	/* grab our IRQ */
 	irq = 0;
 	if (board->IRQbits != 0) {	/* board support IRQ */
@@ -1641,8 +1258,8 @@ static int pcl818_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 				     irq);
 				irq = 0;	/* Bad IRQ */
 			} else {
-				if (request_irq
-				    (irq, interrupt_pcl818, 0, "pcl818", dev)) {
+				if (request_irq(irq, interrupt_pcl818, 0,
+						dev->board_name, dev)) {
 					printk
 					    (", unable to allocate IRQ %u, DISABLING IT",
 					     irq);
@@ -1663,41 +1280,10 @@ static int pcl818_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	devpriv->irq_blocked = 0;	/* number of subdevice which use IRQ */
 	devpriv->ai_mode = 0;	/* mode of irq */
 
-#ifdef unused
-	/* grab RTC for DMA operations */
-	devpriv->dma_rtc = 0;
-	if (it->options[2] > 0) {	/*  we want to use DMA */
-		if (RTC_lock == 0) {
-			if (!request_region(RTC_PORT(0), RTC_IO_EXTENT,
-					    "pcl818 (RTC)"))
-				goto no_rtc;
-		}
-		devpriv->rtc_iobase = RTC_PORT(0);
-		devpriv->rtc_iosize = RTC_IO_EXTENT;
-		RTC_lock++;
-		if (!request_irq(RTC_IRQ, interrupt_pcl818_ai_mode13_dma_rtc, 0,
-				 "pcl818 DMA (RTC)", dev)) {
-			devpriv->dma_rtc = 1;
-			devpriv->rtc_irq = RTC_IRQ;
-			printk(KERN_DEBUG "dma_irq=%u", devpriv->rtc_irq);
-		} else {
-			RTC_lock--;
-			if (RTC_lock == 0) {
-				if (devpriv->rtc_iobase)
-					release_region(devpriv->rtc_iobase,
-						       devpriv->rtc_iosize);
-			}
-			devpriv->rtc_iobase = 0;
-			devpriv->rtc_iosize = 0;
-		}
-	}
-
-no_rtc:
-#endif
 	/* grab our DMA */
 	dma = 0;
 	devpriv->dma = dma;
-	if ((devpriv->irq_free == 0) && (devpriv->dma_rtc == 0))
+	if (!devpriv->irq_free)
 		goto no_dma;	/* if we haven't IRQ, we can't use DMA */
 	if (board->DMAbits != 0) {	/* board support DMA */
 		dma = it->options[2];
@@ -1707,7 +1293,7 @@ no_rtc:
 			printk(KERN_ERR "DMA is out of allowed range, FAIL!\n");
 			return -EINVAL;	/* Bad DMA */
 		}
-		ret = request_dma(dma, "pcl818");
+		ret = request_dma(dma, dev->board_name);
 		if (ret)
 			return -EBUSY;	/* DMA isn't free */
 		devpriv->dma = dma;
@@ -1720,15 +1306,12 @@ no_rtc:
 		devpriv->hwdmaptr[0] = virt_to_bus((void *)devpriv->dmabuf[0]);
 		devpriv->hwdmasize[0] = (1 << pages) * PAGE_SIZE;
 		/* printk("%d %d %ld, ",devpriv->dmapages[0],devpriv->hwdmasize[0],PAGE_SIZE); */
-		if (devpriv->dma_rtc == 0) {	/*  we must do duble buff :-( */
-			devpriv->dmabuf[1] = __get_dma_pages(GFP_KERNEL, pages);
-			if (!devpriv->dmabuf[1])
-				return -EBUSY;
-			devpriv->dmapages[1] = pages;
-			devpriv->hwdmaptr[1] =
-			    virt_to_bus((void *)devpriv->dmabuf[1]);
-			devpriv->hwdmasize[1] = (1 << pages) * PAGE_SIZE;
-		}
+		devpriv->dmabuf[1] = __get_dma_pages(GFP_KERNEL, pages);
+		if (!devpriv->dmabuf[1])
+			return -EBUSY;
+		devpriv->dmapages[1] = pages;
+		devpriv->hwdmaptr[1] = virt_to_bus((void *)devpriv->dmabuf[1]);
+		devpriv->hwdmasize[1] = (1 << pages) * PAGE_SIZE;
 	}
 
 no_dma:
@@ -1758,7 +1341,7 @@ no_dma:
 		s->range_table = board->ai_range_type;
 		s->cancel = pcl818_ai_cancel;
 		s->insn_read = pcl818_ai_insn_read;
-		if ((irq) || (devpriv->dma_rtc)) {
+		if (irq) {
 			dev->read_subdev = s;
 			s->subdev_flags |= SDF_CMD_READ;
 			s->do_cmdtest = ai_cmdtest;
@@ -1815,14 +1398,6 @@ no_dma:
 		s->range_table = board->ao_range_type;
 		s->insn_read = pcl818_ao_insn_read;
 		s->insn_write = pcl818_ao_insn_write;
-#ifdef unused
-#ifdef PCL818_MODE13_AO
-		if (irq) {
-			s->trig[1] = pcl818_ao_mode1;
-			s->trig[3] = pcl818_ao_mode3;
-		}
-#endif
-#endif
 		if (board->is_818) {
 			if ((it->options[4] == 1) || (it->options[4] == 10))
 				s->range_table = &range_unipolar10;
@@ -1896,22 +1471,8 @@ static void pcl818_detach(struct comedi_device *dev)
 			free_pages(devpriv->dmabuf[0], devpriv->dmapages[0]);
 		if (devpriv->dmabuf[1])
 			free_pages(devpriv->dmabuf[1], devpriv->dmapages[1]);
-#ifdef unused
-		if (devpriv->rtc_irq)
-			free_irq(devpriv->rtc_irq, dev);
-		if ((devpriv->dma_rtc) && (RTC_lock == 1)) {
-			if (devpriv->rtc_iobase)
-				release_region(devpriv->rtc_iobase,
-					       devpriv->rtc_iosize);
-		}
-		if (devpriv->dma_rtc)
-			RTC_lock--;
-#endif
 	}
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (dev->iobase)
-		release_region(dev->iobase, devpriv->io_range);
+	comedi_legacy_detach(dev);
 }
 
 static const struct pcl818_board boardtypes[] = {
diff --git a/drivers/staging/comedi/drivers/pcm3724.c b/drivers/staging/comedi/drivers/pcm3724.c
index 5f062df1ead..4ef0df30b07 100644
--- a/drivers/staging/comedi/drivers/pcm3724.c
+++ b/drivers/staging/comedi/drivers/pcm3724.c
@@ -226,29 +226,16 @@ static int pcm3724_attach(struct comedi_device *dev,
 {
 	struct priv_pcm3724 *priv;
 	struct comedi_subdevice *s;
-	unsigned long iobase;
-	unsigned int iorange;
 	int ret, i;
 
-	dev->board_name = dev->driver->driver_name;
-
-	iobase = it->options[0];
-	iorange = PCM3724_SIZE;
-
 	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
 	dev->private = priv;
 
-	printk(KERN_INFO "comedi%d: pcm3724: board=%s, 0x%03lx ", dev->minor,
-	       dev->board_name, iobase);
-	if (!iobase || !request_region(iobase, iorange, "pcm3724")) {
-		printk("I/O port conflict\n");
-		return -EIO;
-	}
-
-	dev->iobase = iobase;
-	printk(KERN_INFO "\n");
+	ret = comedi_request_region(dev, it->options[0], PCM3724_SIZE);
+	if (ret)
+		return ret;
 
 	ret = comedi_alloc_subdevices(dev, 2);
 	if (ret)
@@ -265,17 +252,11 @@ static int pcm3724_attach(struct comedi_device *dev,
 
 static void pcm3724_detach(struct comedi_device *dev)
 {
-	struct comedi_subdevice *s;
 	int i;
 
-	if (dev->subdevices) {
-		for (i = 0; i < dev->n_subdevices; i++) {
-			s = &dev->subdevices[i];
-			subdev_8255_cleanup(dev, s);
-		}
-	}
-	if (dev->iobase)
-		release_region(dev->iobase, PCM3724_SIZE);
+	for (i = 0; i < dev->n_subdevices; i++)
+		comedi_spriv_free(dev, i);
+	comedi_legacy_detach(dev);
 }
 
 static struct comedi_driver pcm3724_driver = {
diff --git a/drivers/staging/comedi/drivers/pcm3730.c b/drivers/staging/comedi/drivers/pcm3730.c
index 067f14d2261..3a3ce2c769a 100644
--- a/drivers/staging/comedi/drivers/pcm3730.c
+++ b/drivers/staging/comedi/drivers/pcm3730.c
@@ -54,19 +54,11 @@ static int pcm3730_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
 	struct comedi_subdevice *s;
-	unsigned long iobase;
 	int ret;
 
-	iobase = it->options[0];
-	printk(KERN_INFO "comedi%d: pcm3730: 0x%04lx ", dev->minor, iobase);
-	if (!request_region(iobase, PCM3730_SIZE, "pcm3730")) {
-		printk("I/O port conflict\n");
-		return -EIO;
-	}
-	dev->iobase = iobase;
-	dev->board_name = "pcm3730";
-	dev->iobase = dev->iobase;
-	dev->irq = 0;
+	ret = comedi_request_region(dev, it->options[0], PCM3730_SIZE);
+	if (ret)
+		return ret;
 
 	ret = comedi_alloc_subdevices(dev, 6);
 	if (ret)
@@ -131,17 +123,11 @@ static int pcm3730_attach(struct comedi_device *dev,
 	return 0;
 }
 
-static void pcm3730_detach(struct comedi_device *dev)
-{
-	if (dev->iobase)
-		release_region(dev->iobase, PCM3730_SIZE);
-}
-
 static struct comedi_driver pcm3730_driver = {
 	.driver_name	= "pcm3730",
 	.module		= THIS_MODULE,
 	.attach		= pcm3730_attach,
-	.detach		= pcm3730_detach,
+	.detach		= comedi_legacy_detach,
 };
 module_comedi_driver(pcm3730_driver);
 
diff --git a/drivers/staging/comedi/drivers/pcmad.c b/drivers/staging/comedi/drivers/pcmad.c
index 13e84215fac..b7c932e152e 100644
--- a/drivers/staging/comedi/drivers/pcmad.c
+++ b/drivers/staging/comedi/drivers/pcmad.c
@@ -105,18 +105,12 @@ static int pcmad_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	const struct pcmad_board_struct *board = comedi_board(dev);
 	struct pcmad_priv_struct *devpriv;
-	int ret;
 	struct comedi_subdevice *s;
-	unsigned long iobase;
+	int ret;
 
-	iobase = it->options[0];
-	printk(KERN_INFO "comedi%d: pcmad: 0x%04lx ", dev->minor, iobase);
-	if (!request_region(iobase, PCMAD_SIZE, "pcmad")) {
-		printk(KERN_CONT "I/O port conflict\n");
-		return -EIO;
-	}
-	printk(KERN_CONT "\n");
-	dev->iobase = iobase;
+	ret = comedi_request_region(dev, it->options[0], PCMAD_SIZE);
+	if (ret)
+		return ret;
 
 	ret = comedi_alloc_subdevices(dev, 1);
 	if (ret)
@@ -127,8 +121,6 @@ static int pcmad_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	dev->board_name = board->name;
-
 	s = &dev->subdevices[0];
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | AREF_GROUND;
@@ -141,14 +133,6 @@ static int pcmad_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static void pcmad_detach(struct comedi_device *dev)
-{
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (dev->iobase)
-		release_region(dev->iobase, PCMAD_SIZE);
-}
-
 static const struct pcmad_board_struct pcmad_boards[] = {
 	{
 		.name		= "pcmad12",
@@ -162,7 +146,7 @@ static struct comedi_driver pcmad_driver = {
 	.driver_name	= "pcmad",
 	.module		= THIS_MODULE,
 	.attach		= pcmad_attach,
-	.detach		= pcmad_detach,
+	.detach		= comedi_legacy_detach,
 	.board_name	= &pcmad_boards[0].name,
 	.num_names	= ARRAY_SIZE(pcmad_boards),
 	.offset		= sizeof(pcmad_boards[0]),
diff --git a/drivers/staging/comedi/drivers/pcmda12.c b/drivers/staging/comedi/drivers/pcmda12.c
index 13f79f49748..61e7fd14a1e 100644
--- a/drivers/staging/comedi/drivers/pcmda12.c
+++ b/drivers/staging/comedi/drivers/pcmda12.c
@@ -154,21 +154,11 @@ static int pcmda12_attach(struct comedi_device *dev,
 {
 	struct pcmda12_private *devpriv;
 	struct comedi_subdevice *s;
-	unsigned long iobase;
 	int ret;
 
-	iobase = it->options[0];
-	printk(KERN_INFO
-	       "comedi%d: %s: io: %lx %s ", dev->minor, dev->driver->driver_name,
-	       iobase, it->options[1] ? "simultaneous xfer mode enabled" : "");
-
-	if (!request_region(iobase, IOSIZE, dev->driver->driver_name)) {
-		printk("I/O port conflict\n");
-		return -EIO;
-	}
-	dev->iobase = iobase;
-
-	dev->board_name = dev->driver->driver_name;
+	ret = comedi_request_region(dev, it->options[0], IOSIZE);
+	if (ret)
+		return ret;
 
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
@@ -193,22 +183,14 @@ static int pcmda12_attach(struct comedi_device *dev,
 
 	zero_chans(dev);	/* clear out all the registers, basically */
 
-	printk(KERN_INFO "attached\n");
-
 	return 1;
 }
 
-static void pcmda12_detach(struct comedi_device *dev)
-{
-	if (dev->iobase)
-		release_region(dev->iobase, IOSIZE);
-}
-
 static struct comedi_driver pcmda12_driver = {
 	.driver_name	= "pcmda12",
 	.module		= THIS_MODULE,
 	.attach		= pcmda12_attach,
-	.detach		= pcmda12_detach,
+	.detach		= comedi_legacy_detach,
 };
 module_comedi_driver(pcmda12_driver);
 
diff --git a/drivers/staging/comedi/drivers/pcmmio.c b/drivers/staging/comedi/drivers/pcmmio.c
index 5fa1fe08eb9..5a236cd5b33 100644
--- a/drivers/staging/comedi/drivers/pcmmio.c
+++ b/drivers/staging/comedi/drivers/pcmmio.c
@@ -1034,24 +1034,14 @@ static int pcmmio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	struct comedi_subdevice *s;
 	int sdev_no, chans_left, n_dio_subdevs, n_subdevs, port, asic,
 	    thisasic_chanct = 0;
-	unsigned long iobase;
 	unsigned int irq[MAX_ASICS];
 	int ret;
 
-	dev->board_name = dev->driver->driver_name;
-
-	iobase = it->options[0];
 	irq[0] = it->options[1];
 
-	printk(KERN_INFO "comedi%d: %s: io: %lx attaching...\n", dev->minor,
-			dev->board_name, iobase);
-
-	dev->iobase = iobase;
-
-	if (!iobase || !request_region(iobase, 32, dev->board_name)) {
-		printk(KERN_ERR "comedi%d: I/O port conflict\n", dev->minor);
-		return -EIO;
-	}
+	ret = comedi_request_region(dev, it->options[0], 32);
+	if (ret)
+		return ret;
 
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
@@ -1203,13 +1193,6 @@ static int pcmmio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		devpriv->asics[asic].irq = irq[asic];
 	}
 
-	dev->irq = irq[0];	/*
-				 * grr.. wish comedi dev struct supported
-				 * multiple irqs..
-				 */
-
-	printk(KERN_INFO "comedi%d: attached\n", dev->minor);
-
 	return 1;
 }
 
@@ -1218,14 +1201,13 @@ static void pcmmio_detach(struct comedi_device *dev)
 	struct pcmmio_private *devpriv = dev->private;
 	int i;
 
-	if (dev->iobase)
-		release_region(dev->iobase, 32);
 	for (i = 0; i < MAX_ASICS; ++i) {
 		if (devpriv && devpriv->asics[i].irq)
 			free_irq(devpriv->asics[i].irq, dev);
 	}
 	if (devpriv && devpriv->sprivs)
 		kfree(devpriv->sprivs);
+	comedi_legacy_detach(dev);
 }
 
 static struct comedi_driver pcmmio_driver = {
diff --git a/drivers/staging/comedi/drivers/pcmuio.c b/drivers/staging/comedi/drivers/pcmuio.c
index 433270ceda4..0c98e26bbba 100644
--- a/drivers/staging/comedi/drivers/pcmuio.c
+++ b/drivers/staging/comedi/drivers/pcmuio.c
@@ -800,27 +800,16 @@ static int pcmuio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	struct pcmuio_private *devpriv;
 	struct comedi_subdevice *s;
 	int sdev_no, chans_left, n_subdevs, port, asic, thisasic_chanct = 0;
-	unsigned long iobase;
 	unsigned int irq[MAX_ASICS];
 	int ret;
 
-	iobase = it->options[0];
 	irq[0] = it->options[1];
 	irq[1] = it->options[2];
 
-	dev_dbg(dev->class_dev, "%s: io: %lx attach\n",
-		dev->driver->driver_name, iobase);
-
-	dev->iobase = iobase;
-
-	if (!iobase || !request_region(iobase,
-				       board->num_asics * ASIC_IOSIZE,
-				       dev->driver->driver_name)) {
-		dev_err(dev->class_dev, "I/O port conflict\n");
-		return -EIO;
-	}
-
-	dev->board_name = board->name;
+	ret = comedi_request_region(dev, it->options[0],
+				    board->num_asics * ASIC_IOSIZE);
+	if (ret)
+		return ret;
 
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
@@ -928,9 +917,6 @@ static int pcmuio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		devpriv->asics[asic].irq = irq[asic];
 	}
 
-	dev->irq = irq[0];	/* grr.. wish comedi dev struct supported multiple
-				   irqs.. */
-
 	if (irq[0]) {
 		dev_dbg(dev->class_dev, "irq: %u\n", irq[0]);
 		if (irq[1] && board->num_asics == 2)
@@ -946,18 +932,16 @@ static int pcmuio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 static void pcmuio_detach(struct comedi_device *dev)
 {
-	const struct pcmuio_board *board = comedi_board(dev);
 	struct pcmuio_private *devpriv = dev->private;
 	int i;
 
-	if (dev->iobase)
-		release_region(dev->iobase, ASIC_IOSIZE * board->num_asics);
 	for (i = 0; i < MAX_ASICS; ++i) {
 		if (devpriv->asics[i].irq)
 			free_irq(devpriv->asics[i].irq, dev);
 	}
 	if (devpriv && devpriv->sprivs)
 		kfree(devpriv->sprivs);
+	comedi_legacy_detach(dev);
 }
 
 static const struct pcmuio_board pcmuio_boards[] = {
diff --git a/drivers/staging/comedi/drivers/plx9052.h b/drivers/staging/comedi/drivers/plx9052.h
index 5894739ff42..ff76fbb4b3e 100644
--- a/drivers/staging/comedi/drivers/plx9052.h
+++ b/drivers/staging/comedi/drivers/plx9052.h
@@ -27,60 +27,58 @@
 #define _PLX9052_H_
 
 /*
- * PLX PCI9052 INTCSR register.
+ * INTCSR - Interrupt Control/Status register
  */
-#define PLX9052_INTCSR	0x4C	/* Offset in Local Configuration Registers */
-/* Local Interrupt 1 Enable */
-#define PLX9052_INTCSR_LI1ENAB_MASK		0x0001
-#define PLX9052_INTCSR_LI1ENAB_DISABLED		0x0000
-#define PLX9052_INTCSR_LI1ENAB_ENABLED		0x0001
-/* Local Interrupt 1 Polarity */
-#define PLX9052_INTCSR_LI1POL_MASK		0x0002
-#define PLX9052_INTCSR_LI1POL_LOW		0x0000
-#define PLX9052_INTCSR_LI1POL_HIGH		0x0002
-/* Local Interrupt 1 Status (read-only) */
-#define PLX9052_INTCSR_LI1STAT_MASK		0x0004
-#define PLX9052_INTCSR_LI1STAT_INACTIVE		0x0000
-#define PLX9052_INTCSR_LI1STAT_ACTIVE		0x0004
-/* Local Interrupt 2 Enable */
-#define PLX9052_INTCSR_LI2ENAB_MASK		0x0008
-#define PLX9052_INTCSR_LI2ENAB_DISABLED		0x0000
-#define PLX9052_INTCSR_LI2ENAB_ENABLED		0x0008
-/* Local Interrupt 2 Polarity */
-#define PLX9052_INTCSR_LI2POL_MASK		0x0010
-#define PLX9052_INTCSR_LI2POL_LOW		0x0000
-#define PLX9052_INTCSR_LI2POL_HIGH		0x0010
-/* Local Interrupt 2 Status (read-only) */
-#define PLX9052_INTCSR_LI2STAT_MASK		0x0020
-#define PLX9052_INTCSR_LI2STAT_INACTIVE		0x0000
-#define PLX9052_INTCSR_LI2STAT_ACTIVE		0x0020
-/* PCI Interrupt Enable */
-#define PLX9052_INTCSR_PCIENAB_MASK		0x0040
-#define PLX9052_INTCSR_PCIENAB_DISABLED		0x0000
-#define PLX9052_INTCSR_PCIENAB_ENABLED		0x0040
-/* Software Interrupt */
-#define PLX9052_INTCSR_SOFTINT_MASK		0x0080
-#define PLX9052_INTCSR_SOFTINT_UNASSERTED	0x0000
-#define PLX9052_INTCSR_SOFTINT_ASSERTED		0x0080
-/* Local Interrupt 1 Select Enable */
-#define PLX9052_INTCSR_LI1SEL_MASK		0x0100
-#define PLX9052_INTCSR_LI1SEL_LEVEL		0x0000
-#define PLX9052_INTCSR_LI1SEL_EDGE		0x0100
-/* Local Interrupt 2 Select Enable */
-#define PLX9052_INTCSR_LI2SEL_MASK		0x0200
-#define PLX9052_INTCSR_LI2SEL_LEVEL		0x0000
-#define PLX9052_INTCSR_LI2SEL_EDGE		0x0200
-/* Local Edge Triggerable Interrupt 1 Clear Bit */
-#define PLX9052_INTCSR_LI1CLRINT_MASK		0x0400
-#define PLX9052_INTCSR_LI1CLRINT_UNASSERTED	0x0000
-#define PLX9052_INTCSR_LI1CLRINT_ASSERTED	0x0400
-/* Local Edge Triggerable Interrupt 2 Clear Bit */
-#define PLX9052_INTCSR_LI2CLRINT_MASK		0x0800
-#define PLX9052_INTCSR_LI2CLRINT_UNASSERTED	0x0000
-#define PLX9052_INTCSR_LI2CLRINT_ASSERTED	0x0800
-/* ISA Interface Mode Enable (read-only over PCI bus) */
-#define PLX9052_INTCSR_ISAMODE_MASK		0x1000
-#define PLX9052_INTCSR_ISAMODE_DISABLED		0x0000
-#define PLX9052_INTCSR_ISAMODE_ENABLED		0x1000
+#define PLX9052_INTCSR			0x4c
+#define PLX9052_INTCSR_LI1ENAB		(1 << 0)  /* LI1 enabled */
+#define PLX9052_INTCSR_LI1POL		(1 << 1)  /* LI1 active high */
+#define PLX9052_INTCSR_LI1STAT		(1 << 2)  /* LI1 active */
+#define PLX9052_INTCSR_LI2ENAB		(1 << 3)  /* LI2 enabled */
+#define PLX9052_INTCSR_LI2POL		(1 << 4)  /* LI2 active high */
+#define PLX9052_INTCSR_LI2STAT		(1 << 5)  /* LI2 active */
+#define PLX9052_INTCSR_PCIENAB		(1 << 6)  /* PCIINT enabled */
+#define PLX9052_INTCSR_SOFTINT		(1 << 7)  /* generate soft int */
+#define PLX9052_INTCSR_LI1SEL		(1 << 8)  /* LI1 edge */
+#define PLX9052_INTCSR_LI2SEL		(1 << 9)  /* LI2 edge */
+#define PLX9052_INTCSR_LI1CLRINT	(1 << 10) /* LI1 clear int */
+#define PLX9052_INTCSR_LI2CLRINT	(1 << 11) /* LI2 clear int */
+#define PLX9052_INTCSR_ISAMODE		(1 << 12) /* ISA interface mode */
+
+/*
+ * CNTRL - User I/O, Direct Slave Response, Serial EEPROM, and
+ * Initialization Control register
+ */
+#define PLX9052_CNTRL			0x50
+#define PLX9052_CNTRL_WAITO		(1 << 0)  /* UIO0 or WAITO# select */
+#define PLX9052_CNTRL_UIO0_DIR		(1 << 1)  /* UIO0 direction */
+#define PLX9052_CNTRL_UIO0_DATA		(1 << 2)  /* UIO0 data */
+#define PLX9052_CNTRL_LLOCKO		(1 << 3)  /* UIO1 or LLOCKo# select */
+#define PLX9052_CNTRL_UIO1_DIR		(1 << 4)  /* UIO1 direction */
+#define PLX9052_CNTRL_UIO1_DATA		(1 << 5)  /* UIO1 data */
+#define PLX9052_CNTRL_CS2		(1 << 6)  /* UIO2 or CS2# select */
+#define PLX9052_CNTRL_UIO2_DIR		(1 << 7)  /* UIO2 direction */
+#define PLX9052_CNTRL_UIO2_DATA		(1 << 8)  /* UIO2 data */
+#define PLX9052_CNTRL_CS3		(1 << 9)  /* UIO3 or CS3# select */
+#define PLX9052_CNTRL_UIO3_DIR		(1 << 10) /* UIO3 direction */
+#define PLX9052_CNTRL_UIO3_DATA		(1 << 11) /* UIO3 data */
+#define PLX9052_CNTRL_PCIBAR01		(0 << 12) /* bar 0 (mem) and 1 (I/O) */
+#define PLX9052_CNTRL_PCIBAR0		(1 << 12) /* bar 0 (mem) only */
+#define PLX9052_CNTRL_PCIBAR1		(2 << 12) /* bar 1 (I/O) only */
+#define PLX9052_CNTRL_PCI2_1_FEATURES	(1 << 14) /* PCI r2.1 features enabled */
+#define PLX9052_CNTRL_PCI_R_W_FLUSH	(1 << 15) /* read w/write flush mode */
+#define PLX9052_CNTRL_PCI_R_NO_FLUSH	(1 << 16) /* read no flush mode */
+#define PLX9052_CNTRL_PCI_R_NO_WRITE	(1 << 17) /* read no write mode */
+#define PLX9052_CNTRL_PCI_W_RELEASE	(1 << 18) /* write release bus mode */
+#define PLX9052_CNTRL_RETRY_CLKS(x)	(((x) & 0xf) << 19) /* slave retry clks */
+#define PLX9052_CNTRL_LOCK_ENAB		(1 << 23) /* slave LOCK# enable */
+#define PLX9052_CNTRL_EEPROM_MASK	(0x1f << 24) /* EEPROM bits */
+#define PLX9052_CNTRL_EEPROM_CLK	(1 << 24) /* EEPROM clock */
+#define PLX9052_CNTRL_EEPROM_CS		(1 << 25) /* EEPROM chip select */
+#define PLX9052_CNTRL_EEPROM_DOUT	(1 << 26) /* EEPROM write bit */
+#define PLX9052_CNTRL_EEPROM_DIN	(1 << 27) /* EEPROM read bit */
+#define PLX9052_CNTRL_EEPROM_PRESENT	(1 << 28) /* EEPROM present */
+#define PLX9052_CNTRL_RELOAD_CFG	(1 << 29) /* reload configuration */
+#define PLX9052_CNTRL_PCI_RESET		(1 << 30) /* PCI adapter reset */
+#define PLX9052_CNTRL_MASK_REV		(1 << 31) /* mask revision */
 
 #endif /* _PLX9052_H_ */
diff --git a/drivers/staging/comedi/drivers/poc.c b/drivers/staging/comedi/drivers/poc.c
index d7842c95d98..b55a16baeb1 100644
--- a/drivers/staging/comedi/drivers/poc.c
+++ b/drivers/staging/comedi/drivers/poc.c
@@ -139,29 +139,11 @@ static int poc_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	const struct boarddef_struct *board = comedi_board(dev);
 	struct poc_private *devpriv;
 	struct comedi_subdevice *s;
-	unsigned long iobase;
-	unsigned int iosize;
 	int ret;
 
-	iobase = it->options[0];
-	printk(KERN_INFO "comedi%d: poc: using %s iobase 0x%lx\n", dev->minor,
-	       board->name, iobase);
-
-	dev->board_name = board->name;
-
-	if (iobase == 0) {
-		printk(KERN_ERR "io base address required\n");
-		return -EINVAL;
-	}
-
-	iosize = board->iosize;
-	/* check if io addresses are available */
-	if (!request_region(iobase, iosize, "dac02")) {
-		printk(KERN_ERR "I/O port conflict: failed to allocate ports "
-			"0x%lx to 0x%lx\n", iobase, iobase + iosize - 1);
-		return -EIO;
-	}
-	dev->iobase = iobase;
+	ret = comedi_request_region(dev, it->options[0], board->iosize);
+	if (ret)
+		return ret;
 
 	ret = comedi_alloc_subdevices(dev, 1);
 	if (ret)
@@ -187,14 +169,6 @@ static int poc_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static void poc_detach(struct comedi_device *dev)
-{
-	const struct boarddef_struct *board = comedi_board(dev);
-
-	if (dev->iobase)
-		release_region(dev->iobase, board->iosize);
-}
-
 static const struct boarddef_struct boards[] = {
 	{
 		.name		= "dac02",
@@ -229,7 +203,7 @@ static struct comedi_driver poc_driver = {
 	.driver_name	= "poc",
 	.module		= THIS_MODULE,
 	.attach		= poc_attach,
-	.detach		= poc_detach,
+	.detach		= comedi_legacy_detach,
 	.board_name	= &boards[0].name,
 	.num_names	= ARRAY_SIZE(boards),
 	.offset		= sizeof(boards[0]),
diff --git a/drivers/staging/comedi/drivers/quatech_daqp_cs.c b/drivers/staging/comedi/drivers/quatech_daqp_cs.c
index 911eb6b3229..e092ce87722 100644
--- a/drivers/staging/comedi/drivers/quatech_daqp_cs.c
+++ b/drivers/staging/comedi/drivers/quatech_daqp_cs.c
@@ -715,8 +715,6 @@ static int daqp_auto_attach(struct comedi_device *dev,
 	struct comedi_subdevice *s;
 	int ret;
 
-	dev->board_name = dev->driver->driver_name;
-
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
diff --git a/drivers/staging/comedi/drivers/rtd520.c b/drivers/staging/comedi/drivers/rtd520.c
index 6a5c914fa50..30a17284fac 100644
--- a/drivers/staging/comedi/drivers/rtd520.c
+++ b/drivers/staging/comedi/drivers/rtd520.c
@@ -1,105 +1,103 @@
 /*
-    comedi/drivers/rtd520.c
-    Comedi driver for Real Time Devices (RTD) PCI4520/DM7520
-
-    COMEDI - Linux Control and Measurement Device Interface
-    Copyright (C) 2001 David A. Schleef <ds@schleef.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.
+ * comedi/drivers/rtd520.c
+ * Comedi driver for Real Time Devices (RTD) PCI4520/DM7520
+ *
+ * COMEDI - Linux Control and Measurement Device Interface
+ * Copyright (C) 2001 David A. Schleef <ds@schleef.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.
+ */
 
-    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.
-*/
-/*
-Driver: rtd520
-Description: Real Time Devices PCI4520/DM7520
-Author: Dan Christian
-Devices: [Real Time Devices] DM7520HR-1 (rtd520), DM7520HR-8,
-  PCI4520, PCI4520-8
-Status: Works.  Only tested on DM7520-8.  Not SMP safe.
-
-Configuration options:
-  [0] - PCI bus of device (optional)
-	If bus / slot is not specified, the first available PCI
-	device will be used.
-  [1] - PCI slot of device (optional)
-*/
 /*
-    Created by Dan Christian, NASA Ames Research Center.
-
-    The PCI4520 is a PCI card.  The DM7520 is a PC/104-plus card.
-    Both have:
-    8/16 12 bit ADC with FIFO and channel gain table
-    8 bits high speed digital out (for external MUX) (or 8 in or 8 out)
-    8 bits high speed digital in with FIFO and interrupt on change (or 8 IO)
-    2 12 bit DACs with FIFOs
-    2 bits output
-    2 bits input
-    bus mastering DMA
-    timers: ADC sample, pacer, burst, about, delay, DA1, DA2
-    sample counter
-    3 user timer/counters (8254)
-    external interrupt
-
-    The DM7520 has slightly fewer features (fewer gain steps).
-
-    These boards can support external multiplexors and multi-board
-    synchronization, but this driver doesn't support that.
-
-    Board docs: http://www.rtdusa.com/PC104/DM/analog%20IO/dm7520.htm
-    Data sheet: http://www.rtdusa.com/pdf/dm7520.pdf
-    Example source: http://www.rtdusa.com/examples/dm/dm7520.zip
-    Call them and ask for the register level manual.
-    PCI chip: http://www.plxtech.com/products/io/pci9080
-
-    Notes:
-    This board is memory mapped.  There is some IO stuff, but it isn't needed.
-
-    I use a pretty loose naming style within the driver (rtd_blah).
-    All externally visible names should be rtd520_blah.
-    I use camelCase for structures (and inside them).
-    I may also use upper CamelCase for function names (old habit).
-
-    This board is somewhat related to the RTD PCI4400 board.
-
-    I borrowed heavily from the ni_mio_common, ni_atmio16d, mite, and
-    das1800, since they have the best documented code.  Driver
-    cb_pcidas64.c uses the same DMA controller.
-
-    As far as I can tell, the About interrupt doesn't work if Sample is
-    also enabled.  It turns out that About really isn't needed, since
-    we always count down samples read.
-
-    There was some timer/counter code, but it didn't follow the right API.
-
-*/
+ * Driver: rtd520
+ * Description: Real Time Devices PCI4520/DM7520
+ * Devices: (Real Time Devices) DM7520HR-1 [DM7520]
+ *	    (Real Time Devices) DM7520HR-8 [DM7520]
+ *	    (Real Time Devices) PCI4520 [PCI4520]
+ *	    (Real Time Devices) PCI4520-8 [PCI4520]
+ * Author: Dan Christian
+ * Status: Works. Only tested on DM7520-8. Not SMP safe.
+ *
+ * Configuration options: not applicable, uses PCI auto config
+ */
 
 /*
-  driver status:
-
-  Analog-In supports instruction and command mode.
-
-  With DMA, you can sample at 1.15Mhz with 70% idle on a 400Mhz K6-2
-  (single channel, 64K read buffer).  I get random system lockups when
-  using DMA with ALI-15xx based systems.  I haven't been able to test
-  any other chipsets.  The lockups happen soon after the start of an
-  acquistion, not in the middle of a long run.
-
-  Without DMA, you can do 620Khz sampling with 20% idle on a 400Mhz K6-2
-  (with a 256K read buffer).
-
-  Digital-IO and Analog-Out only support instruction mode.
+ * Created by Dan Christian, NASA Ames Research Center.
+ *
+ * The PCI4520 is a PCI card. The DM7520 is a PC/104-plus card.
+ * Both have:
+ *   8/16 12 bit ADC with FIFO and channel gain table
+ *   8 bits high speed digital out (for external MUX) (or 8 in or 8 out)
+ *   8 bits high speed digital in with FIFO and interrupt on change (or 8 IO)
+ *   2 12 bit DACs with FIFOs
+ *   2 bits output
+ *   2 bits input
+ *   bus mastering DMA
+ *   timers: ADC sample, pacer, burst, about, delay, DA1, DA2
+ *   sample counter
+ *   3 user timer/counters (8254)
+ *   external interrupt
+ *
+ * The DM7520 has slightly fewer features (fewer gain steps).
+ *
+ * These boards can support external multiplexors and multi-board
+ * synchronization, but this driver doesn't support that.
+ *
+ * Board docs: http://www.rtdusa.com/PC104/DM/analog%20IO/dm7520.htm
+ * Data sheet: http://www.rtdusa.com/pdf/dm7520.pdf
+ * Example source: http://www.rtdusa.com/examples/dm/dm7520.zip
+ * Call them and ask for the register level manual.
+ * PCI chip: http://www.plxtech.com/products/io/pci9080
+ *
+ * Notes:
+ * This board is memory mapped. There is some IO stuff, but it isn't needed.
+ *
+ * I use a pretty loose naming style within the driver (rtd_blah).
+ * All externally visible names should be rtd520_blah.
+ * I use camelCase for structures (and inside them).
+ * I may also use upper CamelCase for function names (old habit).
+ *
+ * This board is somewhat related to the RTD PCI4400 board.
+ *
+ * I borrowed heavily from the ni_mio_common, ni_atmio16d, mite, and
+ * das1800, since they have the best documented code. Driver cb_pcidas64.c
+ * uses the same DMA controller.
+ *
+ * As far as I can tell, the About interrupt doesn't work if Sample is
+ * also enabled. It turns out that About really isn't needed, since
+ * we always count down samples read.
+ *
+ * There was some timer/counter code, but it didn't follow the right API.
+ */
 
-*/
+/*
+ * driver status:
+ *
+ * Analog-In supports instruction and command mode.
+ *
+ * With DMA, you can sample at 1.15Mhz with 70% idle on a 400Mhz K6-2
+ * (single channel, 64K read buffer). I get random system lockups when
+ * using DMA with ALI-15xx based systems. I haven't been able to test
+ * any other chipsets. The lockups happen soon after the start of an
+ * acquistion, not in the middle of a long run.
+ *
+ * Without DMA, you can do 620Khz sampling with 20% idle on a 400Mhz K6-2
+ * (with a 256K read buffer).
+ *
+ * Digital-IO and Analog-Out only support instruction mode.
+ */
 
 #include <linux/pci.h>
 #include <linux/delay.h>
@@ -108,9 +106,123 @@ Configuration options:
 #include "../comedidev.h"
 
 #include "comedi_fc.h"
-#include "rtd520.h"
 #include "plx9080.h"
 
+/*
+ * Local Address Space 0 Offsets
+ */
+#define LAS0_USER_IO		0x0008	/* User I/O */
+#define LAS0_ADC		0x0010	/* FIFO Status/Software A/D Start */
+#define FS_DAC1_NOT_EMPTY	(1 << 0)	/* DAC1 FIFO not empty */
+#define FS_DAC1_HEMPTY		(1 << 1)	/* DAC1 FIFO half empty */
+#define FS_DAC1_NOT_FULL	(1 << 2)	/* DAC1 FIFO not full */
+#define FS_DAC2_NOT_EMPTY	(1 << 4)	/* DAC2 FIFO not empty */
+#define FS_DAC2_HEMPTY		(1 << 5)	/* DAC2 FIFO half empty */
+#define FS_DAC2_NOT_FULL	(1 << 6)	/* DAC2 FIFO not full */
+#define FS_ADC_NOT_EMPTY	(1 << 8)	/* ADC FIFO not empty */
+#define FS_ADC_HEMPTY		(1 << 9)	/* ADC FIFO half empty */
+#define FS_ADC_NOT_FULL		(1 << 10)	/* ADC FIFO not full */
+#define FS_DIN_NOT_EMPTY	(1 << 12)	/* DIN FIFO not empty */
+#define FS_DIN_HEMPTY		(1 << 13)	/* DIN FIFO half empty */
+#define FS_DIN_NOT_FULL		(1 << 14)	/* DIN FIFO not full */
+#define LAS0_DAC1		0x0014	/* Software D/A1 Update (w) */
+#define LAS0_DAC2		0x0018	/* Software D/A2 Update (w) */
+#define LAS0_DAC		0x0024	/* Software Simultaneous Update (w) */
+#define LAS0_PACER		0x0028	/* Software Pacer Start/Stop */
+#define LAS0_TIMER		0x002c	/* Timer Status/HDIN Software Trig. */
+#define LAS0_IT			0x0030	/* Interrupt Status/Enable */
+#define IRQM_ADC_FIFO_WRITE	(1 << 0)	/* ADC FIFO Write */
+#define IRQM_CGT_RESET		(1 << 1)	/* Reset CGT */
+#define IRQM_CGT_PAUSE		(1 << 3)	/* Pause CGT */
+#define IRQM_ADC_ABOUT_CNT	(1 << 4)	/* About Counter out */
+#define IRQM_ADC_DELAY_CNT	(1 << 5)	/* Delay Counter out */
+#define IRQM_ADC_SAMPLE_CNT	(1 << 6)	/* ADC Sample Counter */
+#define IRQM_DAC1_UCNT		(1 << 7)	/* DAC1 Update Counter */
+#define IRQM_DAC2_UCNT		(1 << 8)	/* DAC2 Update Counter */
+#define IRQM_UTC1		(1 << 9)	/* User TC1 out */
+#define IRQM_UTC1_INV		(1 << 10)	/* User TC1 out, inverted */
+#define IRQM_UTC2		(1 << 11)	/* User TC2 out */
+#define IRQM_DIGITAL_IT		(1 << 12)	/* Digital Interrupt */
+#define IRQM_EXTERNAL_IT	(1 << 13)	/* External Interrupt */
+#define IRQM_ETRIG_RISING	(1 << 14)	/* Ext Trigger rising-edge */
+#define IRQM_ETRIG_FALLING	(1 << 15)	/* Ext Trigger falling-edge */
+#define LAS0_CLEAR		0x0034	/* Clear/Set Interrupt Clear Mask */
+#define LAS0_OVERRUN		0x0038	/* Pending interrupts/Clear Overrun */
+#define LAS0_PCLK		0x0040	/* Pacer Clock (24bit) */
+#define LAS0_BCLK		0x0044	/* Burst Clock (10bit) */
+#define LAS0_ADC_SCNT		0x0048	/* A/D Sample counter (10bit) */
+#define LAS0_DAC1_UCNT		0x004c	/* D/A1 Update counter (10 bit) */
+#define LAS0_DAC2_UCNT		0x0050	/* D/A2 Update counter (10 bit) */
+#define LAS0_DCNT		0x0054	/* Delay counter (16 bit) */
+#define LAS0_ACNT		0x0058	/* About counter (16 bit) */
+#define LAS0_DAC_CLK		0x005c	/* DAC clock (16bit) */
+#define LAS0_UTC0		0x0060	/* 8254 TC Counter 0 */
+#define LAS0_UTC1		0x0064	/* 8254 TC Counter 1 */
+#define LAS0_UTC2		0x0068	/* 8254 TC Counter 2 */
+#define LAS0_UTC_CTRL		0x006c	/* 8254 TC Control */
+#define LAS0_DIO0		0x0070	/* Digital I/O Port 0 */
+#define LAS0_DIO1		0x0074	/* Digital I/O Port 1 */
+#define LAS0_DIO0_CTRL		0x0078	/* Digital I/O Control */
+#define LAS0_DIO_STATUS		0x007c	/* Digital I/O Status */
+#define LAS0_BOARD_RESET	0x0100	/* Board reset */
+#define LAS0_DMA0_SRC		0x0104	/* DMA 0 Sources select */
+#define LAS0_DMA1_SRC		0x0108	/* DMA 1 Sources select */
+#define LAS0_ADC_CONVERSION	0x010c	/* A/D Conversion Signal select */
+#define LAS0_BURST_START	0x0110	/* Burst Clock Start Trigger select */
+#define LAS0_PACER_START	0x0114	/* Pacer Clock Start Trigger select */
+#define LAS0_PACER_STOP		0x0118	/* Pacer Clock Stop Trigger select */
+#define LAS0_ACNT_STOP_ENABLE	0x011c	/* About Counter Stop Enable */
+#define LAS0_PACER_REPEAT	0x0120	/* Pacer Start Trigger Mode select */
+#define LAS0_DIN_START		0x0124	/* HiSpd DI Sampling Signal select */
+#define LAS0_DIN_FIFO_CLEAR	0x0128	/* Digital Input FIFO Clear */
+#define LAS0_ADC_FIFO_CLEAR	0x012c	/* A/D FIFO Clear */
+#define LAS0_CGT_WRITE		0x0130	/* Channel Gain Table Write */
+#define LAS0_CGL_WRITE		0x0134	/* Channel Gain Latch Write */
+#define LAS0_CG_DATA		0x0138	/* Digital Table Write */
+#define LAS0_CGT_ENABLE		0x013c	/* Channel Gain Table Enable */
+#define LAS0_CG_ENABLE		0x0140	/* Digital Table Enable */
+#define LAS0_CGT_PAUSE		0x0144	/* Table Pause Enable */
+#define LAS0_CGT_RESET		0x0148	/* Reset Channel Gain Table */
+#define LAS0_CGT_CLEAR		0x014c	/* Clear Channel Gain Table */
+#define LAS0_DAC1_CTRL		0x0150	/* D/A1 output type/range */
+#define LAS0_DAC1_SRC		0x0154	/* D/A1 update source */
+#define LAS0_DAC1_CYCLE		0x0158	/* D/A1 cycle mode */
+#define LAS0_DAC1_RESET		0x015c	/* D/A1 FIFO reset */
+#define LAS0_DAC1_FIFO_CLEAR	0x0160	/* D/A1 FIFO clear */
+#define LAS0_DAC2_CTRL		0x0164	/* D/A2 output type/range */
+#define LAS0_DAC2_SRC		0x0168	/* D/A2 update source */
+#define LAS0_DAC2_CYCLE		0x016c	/* D/A2 cycle mode */
+#define LAS0_DAC2_RESET		0x0170	/* D/A2 FIFO reset */
+#define LAS0_DAC2_FIFO_CLEAR	0x0174	/* D/A2 FIFO clear */
+#define LAS0_ADC_SCNT_SRC	0x0178	/* A/D Sample Counter Source select */
+#define LAS0_PACER_SELECT	0x0180	/* Pacer Clock select */
+#define LAS0_SBUS0_SRC		0x0184	/* SyncBus 0 Source select */
+#define LAS0_SBUS0_ENABLE	0x0188	/* SyncBus 0 enable */
+#define LAS0_SBUS1_SRC		0x018c	/* SyncBus 1 Source select */
+#define LAS0_SBUS1_ENABLE	0x0190	/* SyncBus 1 enable */
+#define LAS0_SBUS2_SRC		0x0198	/* SyncBus 2 Source select */
+#define LAS0_SBUS2_ENABLE	0x019c	/* SyncBus 2 enable */
+#define LAS0_ETRG_POLARITY	0x01a4	/* Ext. Trigger polarity select */
+#define LAS0_EINT_POLARITY	0x01a8	/* Ext. Interrupt polarity select */
+#define LAS0_UTC0_CLOCK		0x01ac	/* UTC0 Clock select */
+#define LAS0_UTC0_GATE		0x01b0	/* UTC0 Gate select */
+#define LAS0_UTC1_CLOCK		0x01b4	/* UTC1 Clock select */
+#define LAS0_UTC1_GATE		0x01b8	/* UTC1 Gate select */
+#define LAS0_UTC2_CLOCK		0x01bc	/* UTC2 Clock select */
+#define LAS0_UTC2_GATE		0x01c0	/* UTC2 Gate select */
+#define LAS0_UOUT0_SELECT	0x01c4	/* User Output 0 source select */
+#define LAS0_UOUT1_SELECT	0x01c8	/* User Output 1 source select */
+#define LAS0_DMA0_RESET		0x01cc	/* DMA0 Request state machine reset */
+#define LAS0_DMA1_RESET		0x01d0	/* DMA1 Request state machine reset */
+
+/*
+ * Local Address Space 1 Offsets
+ */
+#define LAS1_ADC_FIFO		0x0000	/* A/D FIFO (16bit) */
+#define LAS1_HDIO_FIFO		0x0004	/* HiSpd DI FIFO (16bit) */
+#define LAS1_DAC1_FIFO		0x0008	/* D/A1 FIFO (16bit) */
+#define LAS1_DAC2_FIFO		0x000c	/* D/A2 FIFO (16bit) */
+
 /*======================================================================
   Driver specific stuff (tunable)
 ======================================================================*/
@@ -249,61 +361,48 @@ static const struct comedi_lrange rtd_ao_range = {
 	}
 };
 
-struct rtdBoard {
+enum rtd_boardid {
+	BOARD_DM7520,
+	BOARD_PCI4520,
+};
+
+struct rtd_boardinfo {
 	const char *name;
-	int device_id;
-	int range10Start;	/* start of +-10V range */
-	int rangeUniStart;	/* start of +10V range */
+	int range_bip10;	/* start of +-10V range */
+	int range_uni10;	/* start of +10V range */
 	const struct comedi_lrange *ai_range;
 };
 
-static const struct rtdBoard rtd520Boards[] = {
-	{
+static const struct rtd_boardinfo rtd520Boards[] = {
+	[BOARD_DM7520] = {
 		.name		= "DM7520",
-		.device_id	= 0x7520,
-		.range10Start	= 6,
-		.rangeUniStart	= 12,
+		.range_bip10	= 6,
+		.range_uni10	= 12,
 		.ai_range	= &rtd_ai_7520_range,
-	}, {
+	},
+	[BOARD_PCI4520] = {
 		.name		= "PCI4520",
-		.device_id	= 0x4520,
-		.range10Start	= 8,
-		.rangeUniStart	= 16,
+		.range_bip10	= 8,
+		.range_uni10	= 16,
 		.ai_range	= &rtd_ai_4520_range,
 	},
 };
 
-/*
-   This structure is for data unique to this hardware driver.
-   This is also unique for each board in the system.
-*/
-struct rtdPrivate {
+struct rtd_private {
 	/* memory mapped board structures */
 	void __iomem *las0;
 	void __iomem *las1;
 	void __iomem *lcfg;
 
-	long aiCount;		/* total transfer size (samples) */
-	int transCount;		/* # to transfer data. 0->1/2FIFO */
+	long ai_count;		/* total transfer size (samples) */
+	int xfer_count;		/* # to transfer data. 0->1/2FIFO */
 	int flags;		/* flag event modes */
 
-	/* channel list info */
-	/* chanBipolar tracks whether a channel is bipolar (and needs +2048) */
-	unsigned char chanBipolar[RTD_MAX_CHANLIST / 8];	/* bit array */
+	unsigned char chan_is_bipolar[RTD_MAX_CHANLIST / 8];	/* bit array */
 
-	/* read back data */
-	unsigned int aoValue[2];	/* Used for AO read back */
+	unsigned int ao_readback[2];
 
-	/* timer gate (when enabled) */
-	u8 utcGate[4];		/* 1 extra allows simple range check */
-
-	/* shadow registers affect other registers, but can't be read back */
-	/* The macros below update these on writes */
-	u16 intMask;		/* interrupt mask */
-	u16 intClearMask;	/* interrupt clear mask */
-	u8 utcCtrl[4];		/* crtl mode for 3 utc + read back */
-	u8 dioStatus;		/* could be read back (dio0Ctrl) */
-	unsigned fifoLen;
+	unsigned fifosz;
 };
 
 /* bit defines for "flags" */
@@ -365,36 +464,34 @@ static int rtd_ns_to_timer(unsigned int *ns, int round_mode)
 /*
   Convert a single comedi channel-gain entry to a RTD520 table entry
 */
-static unsigned short rtdConvertChanGain(struct comedi_device *dev,
-					 unsigned int comediChan, int chanIndex)
-{				/* index in channel list */
-	const struct rtdBoard *thisboard = comedi_board(dev);
-	struct rtdPrivate *devpriv = dev->private;
-	unsigned int chan, range, aref;
+static unsigned short rtd_convert_chan_gain(struct comedi_device *dev,
+					    unsigned int chanspec, int index)
+{
+	const struct rtd_boardinfo *board = comedi_board(dev);
+	struct rtd_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(chanspec);
+	unsigned int range = CR_RANGE(chanspec);
+	unsigned int aref = CR_AREF(chanspec);
 	unsigned short r = 0;
 
-	chan = CR_CHAN(comediChan);
-	range = CR_RANGE(comediChan);
-	aref = CR_AREF(comediChan);
-
 	r |= chan & 0xf;
 
 	/* Note: we also setup the channel list bipolar flag array */
-	if (range < thisboard->range10Start) {
+	if (range < board->range_bip10) {
 		/* +-5 range */
 		r |= 0x000;
 		r |= (range & 0x7) << 4;
-		CHAN_ARRAY_SET(devpriv->chanBipolar, chanIndex);
-	} else if (range < thisboard->rangeUniStart) {
+		CHAN_ARRAY_SET(devpriv->chan_is_bipolar, index);
+	} else if (range < board->range_uni10) {
 		/* +-10 range */
 		r |= 0x100;
-		r |= ((range - thisboard->range10Start) & 0x7) << 4;
-		CHAN_ARRAY_SET(devpriv->chanBipolar, chanIndex);
+		r |= ((range - board->range_bip10) & 0x7) << 4;
+		CHAN_ARRAY_SET(devpriv->chan_is_bipolar, index);
 	} else {
 		/* +10 range */
 		r |= 0x200;
-		r |= ((range - thisboard->rangeUniStart) & 0x7) << 4;
-		CHAN_ARRAY_CLEAR(devpriv->chanBipolar, chanIndex);
+		r |= ((range - board->range_uni10) & 0x7) << 4;
+		CHAN_ARRAY_CLEAR(devpriv->chan_is_bipolar, index);
 	}
 
 	switch (aref) {
@@ -423,7 +520,7 @@ static unsigned short rtdConvertChanGain(struct comedi_device *dev,
 static void rtd_load_channelgain_list(struct comedi_device *dev,
 				      unsigned int n_chan, unsigned int *list)
 {
-	struct rtdPrivate *devpriv = dev->private;
+	struct rtd_private *devpriv = dev->private;
 
 	if (n_chan > 1) {	/* setup channel gain table */
 		int ii;
@@ -431,12 +528,12 @@ static void rtd_load_channelgain_list(struct comedi_device *dev,
 		writel(0, devpriv->las0 + LAS0_CGT_CLEAR);
 		writel(1, devpriv->las0 + LAS0_CGT_ENABLE);
 		for (ii = 0; ii < n_chan; ii++) {
-			writel(rtdConvertChanGain(dev, list[ii], ii),
+			writel(rtd_convert_chan_gain(dev, list[ii], ii),
 				devpriv->las0 + LAS0_CGT_WRITE);
 		}
 	} else {		/* just use the channel gain latch */
 		writel(0, devpriv->las0 + LAS0_CGT_ENABLE);
-		writel(rtdConvertChanGain(dev, list[0], 0),
+		writel(rtd_convert_chan_gain(dev, list[0], 0),
 			devpriv->las0 + LAS0_CGL_WRITE);
 	}
 }
@@ -445,7 +542,7 @@ static void rtd_load_channelgain_list(struct comedi_device *dev,
 empty status flag clears */
 static int rtd520_probe_fifo_depth(struct comedi_device *dev)
 {
-	struct rtdPrivate *devpriv = dev->private;
+	struct rtd_private *devpriv = dev->private;
 	unsigned int chanspec = CR_PACK(0, 0, AREF_GROUND);
 	unsigned i;
 	static const unsigned limit = 0x2000;
@@ -493,7 +590,7 @@ static int rtd_ai_rinsn(struct comedi_device *dev,
 			struct comedi_subdevice *s, struct comedi_insn *insn,
 			unsigned int *data)
 {
-	struct rtdPrivate *devpriv = dev->private;
+	struct rtd_private *devpriv = dev->private;
 	int n, ii;
 	int stat;
 
@@ -525,7 +622,7 @@ static int rtd_ai_rinsn(struct comedi_device *dev,
 		d = readw(devpriv->las1 + LAS1_ADC_FIFO);
 		/*printk ("rtd520: Got 0x%x after %d usec\n", d, ii+1); */
 		d = d >> 3;	/* low 3 bits are marker lines */
-		if (CHAN_ARRAY_TEST(devpriv->chanBipolar, 0))
+		if (CHAN_ARRAY_TEST(devpriv->chan_is_bipolar, 0))
 			/* convert to comedi unsigned data */
 			data[n] = d + 2048;
 		else
@@ -545,21 +642,22 @@ static int rtd_ai_rinsn(struct comedi_device *dev,
 static int ai_read_n(struct comedi_device *dev, struct comedi_subdevice *s,
 		     int count)
 {
-	struct rtdPrivate *devpriv = dev->private;
+	struct rtd_private *devpriv = dev->private;
 	int ii;
 
 	for (ii = 0; ii < count; ii++) {
 		short sample;
 		s16 d;
 
-		if (0 == devpriv->aiCount) {	/* done */
+		if (0 == devpriv->ai_count) {	/* done */
 			d = readw(devpriv->las1 + LAS1_ADC_FIFO);
 			continue;
 		}
 
 		d = readw(devpriv->las1 + LAS1_ADC_FIFO);
 		d = d >> 3;	/* low 3 bits are marker lines */
-		if (CHAN_ARRAY_TEST(devpriv->chanBipolar, s->async->cur_chan)) {
+		if (CHAN_ARRAY_TEST(devpriv->chan_is_bipolar,
+				    s->async->cur_chan)) {
 			/* convert to comedi unsigned data */
 			sample = d + 2048;
 		} else
@@ -568,8 +666,8 @@ static int ai_read_n(struct comedi_device *dev, struct comedi_subdevice *s,
 		if (!comedi_buf_put(s->async, sample))
 			return -1;
 
-		if (devpriv->aiCount > 0)	/* < 0, means read forever */
-			devpriv->aiCount--;
+		if (devpriv->ai_count > 0)	/* < 0, means read forever */
+			devpriv->ai_count--;
 	}
 	return 0;
 }
@@ -579,18 +677,19 @@ static int ai_read_n(struct comedi_device *dev, struct comedi_subdevice *s,
 */
 static int ai_read_dregs(struct comedi_device *dev, struct comedi_subdevice *s)
 {
-	struct rtdPrivate *devpriv = dev->private;
+	struct rtd_private *devpriv = dev->private;
 
 	while (readl(devpriv->las0 + LAS0_ADC) & FS_ADC_NOT_EMPTY) {
 		short sample;
 		s16 d = readw(devpriv->las1 + LAS1_ADC_FIFO);
 
-		if (0 == devpriv->aiCount) {	/* done */
+		if (0 == devpriv->ai_count) {	/* done */
 			continue;	/* read rest */
 		}
 
 		d = d >> 3;	/* low 3 bits are marker lines */
-		if (CHAN_ARRAY_TEST(devpriv->chanBipolar, s->async->cur_chan)) {
+		if (CHAN_ARRAY_TEST(devpriv->chan_is_bipolar,
+				    s->async->cur_chan)) {
 			/* convert to comedi unsigned data */
 			sample = d + 2048;
 		} else
@@ -599,8 +698,8 @@ static int ai_read_dregs(struct comedi_device *dev, struct comedi_subdevice *s)
 		if (!comedi_buf_put(s->async, sample))
 			return -1;
 
-		if (devpriv->aiCount > 0)	/* < 0, means read forever */
-			devpriv->aiCount--;
+		if (devpriv->ai_count > 0)	/* < 0, means read forever */
+			devpriv->ai_count--;
 	}
 	return 0;
 }
@@ -611,23 +710,22 @@ static int ai_read_dregs(struct comedi_device *dev, struct comedi_subdevice *s)
   This is a "slow handler";  other interrupts may be active.
   The data conversion may someday happen in a "bottom half".
 */
-static irqreturn_t rtd_interrupt(int irq,	/* interrupt number (ignored) */
-				 void *d)
-{				/* our data *//* cpu context (ignored) */
+static irqreturn_t rtd_interrupt(int irq, void *d)
+{
 	struct comedi_device *dev = d;
 	struct comedi_subdevice *s = &dev->subdevices[0];
-	struct rtdPrivate *devpriv = dev->private;
+	struct rtd_private *devpriv = dev->private;
 	u32 overrun;
 	u16 status;
-	u16 fifoStatus;
+	u16 fifo_status;
 
 	if (!dev->attached)
 		return IRQ_NONE;
 
-	fifoStatus = readl(devpriv->las0 + LAS0_ADC);
+	fifo_status = readl(devpriv->las0 + LAS0_ADC);
 	/* check for FIFO full, this automatically halts the ADC! */
-	if (!(fifoStatus & FS_ADC_NOT_FULL))	/* 0 -> full */
-		goto abortTransfer;
+	if (!(fifo_status & FS_ADC_NOT_FULL))	/* 0 -> full */
+		goto xfer_abort;
 
 	status = readw(devpriv->las0 + LAS0_IT);
 	/* if interrupt was not caused by our board, or handled above */
@@ -641,23 +739,23 @@ static irqreturn_t rtd_interrupt(int irq,	/* interrupt number (ignored) */
 		 * finished, we must handle the possibility that there is
 		 * no data here
 		 */
-		if (!(fifoStatus & FS_ADC_HEMPTY)) {
+		if (!(fifo_status & FS_ADC_HEMPTY)) {
 			/* FIFO half full */
-			if (ai_read_n(dev, s, devpriv->fifoLen / 2) < 0)
-				goto abortTransfer;
+			if (ai_read_n(dev, s, devpriv->fifosz / 2) < 0)
+				goto xfer_abort;
 
-			if (0 == devpriv->aiCount)
-				goto transferDone;
+			if (0 == devpriv->ai_count)
+				goto xfer_done;
 
 			comedi_event(dev, s);
-		} else if (devpriv->transCount > 0) {
-			if (fifoStatus & FS_ADC_NOT_EMPTY) {
+		} else if (devpriv->xfer_count > 0) {
+			if (fifo_status & FS_ADC_NOT_EMPTY) {
 				/* FIFO not empty */
-				if (ai_read_n(dev, s, devpriv->transCount) < 0)
-					goto abortTransfer;
+				if (ai_read_n(dev, s, devpriv->xfer_count) < 0)
+					goto xfer_abort;
 
-				if (0 == devpriv->aiCount)
-					goto transferDone;
+				if (0 == devpriv->ai_count)
+					goto xfer_done;
 
 				comedi_event(dev, s);
 			}
@@ -666,30 +764,28 @@ static irqreturn_t rtd_interrupt(int irq,	/* interrupt number (ignored) */
 
 	overrun = readl(devpriv->las0 + LAS0_OVERRUN) & 0xffff;
 	if (overrun)
-		goto abortTransfer;
+		goto xfer_abort;
 
 	/* clear the interrupt */
-	devpriv->intClearMask = status;
-	writew(devpriv->intClearMask, devpriv->las0 + LAS0_CLEAR);
+	writew(status, devpriv->las0 + LAS0_CLEAR);
 	readw(devpriv->las0 + LAS0_CLEAR);
 	return IRQ_HANDLED;
 
-abortTransfer:
+xfer_abort:
 	writel(0, devpriv->las0 + LAS0_ADC_FIFO_CLEAR);
 	s->async->events |= COMEDI_CB_ERROR;
-	devpriv->aiCount = 0;	/* stop and don't transfer any more */
-	/* fall into transferDone */
+	devpriv->ai_count = 0;	/* stop and don't transfer any more */
+	/* fall into xfer_done */
 
-transferDone:
+xfer_done:
 	/* pacer stop source: SOFTWARE */
 	writel(0, devpriv->las0 + LAS0_PACER_STOP);
 	writel(0, devpriv->las0 + LAS0_PACER);	/* stop pacer */
 	writel(0, devpriv->las0 + LAS0_ADC_CONVERSION);
-	devpriv->intMask = 0;
-	writew(devpriv->intMask, devpriv->las0 + LAS0_IT);
+	writew(0, devpriv->las0 + LAS0_IT);
 
-	if (devpriv->aiCount > 0) {	/* there shouldn't be anything left */
-		fifoStatus = readl(devpriv->las0 + LAS0_ADC);
+	if (devpriv->ai_count > 0) {	/* there shouldn't be anything left */
+		fifo_status = readl(devpriv->las0 + LAS0_ADC);
 		ai_read_dregs(dev, s);	/* read anything left in FIFO */
 	}
 
@@ -698,11 +794,10 @@ transferDone:
 
 	/* clear the interrupt */
 	status = readw(devpriv->las0 + LAS0_IT);
-	devpriv->intClearMask = status;
-	writew(devpriv->intClearMask, devpriv->las0 + LAS0_CLEAR);
+	writew(status, devpriv->las0 + LAS0_CLEAR);
 	readw(devpriv->las0 + LAS0_CLEAR);
 
-	fifoStatus = readl(devpriv->las0 + LAS0_ADC);
+	fifo_status = readl(devpriv->las0 + LAS0_ADC);
 	overrun = readl(devpriv->las0 + LAS0_OVERRUN) & 0xffff;
 
 	return IRQ_HANDLED;
@@ -875,7 +970,7 @@ static int rtd_ai_cmdtest(struct comedi_device *dev,
 */
 static int rtd_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
-	struct rtdPrivate *devpriv = dev->private;
+	struct rtd_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	int timer;
 
@@ -884,8 +979,7 @@ static int rtd_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	writel(0, devpriv->las0 + LAS0_PACER_STOP);
 	writel(0, devpriv->las0 + LAS0_PACER);	/* stop pacer */
 	writel(0, devpriv->las0 + LAS0_ADC_CONVERSION);
-	devpriv->intMask = 0;
-	writew(devpriv->intMask, devpriv->las0 + LAS0_IT);
+	writew(0, devpriv->las0 + LAS0_IT);
 	writel(0, devpriv->las0 + LAS0_ADC_FIFO_CLEAR);
 	writel(0, devpriv->las0 + LAS0_OVERRUN);
 
@@ -907,7 +1001,7 @@ static int rtd_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		/* ADC conversion trigger source: PACER */
 		writel(1, devpriv->las0 + LAS0_ADC_CONVERSION);
 	}
-	writel((devpriv->fifoLen / 2 - 1) & 0xffff, devpriv->las0 + LAS0_ACNT);
+	writel((devpriv->fifosz / 2 - 1) & 0xffff, devpriv->las0 + LAS0_ACNT);
 
 	if (TRIG_TIMER == cmd->scan_begin_src) {
 		/* scan_begin_arg is in nanoseconds */
@@ -918,37 +1012,36 @@ static int rtd_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 			 * the application is responsible for doing the
 			 * right thing
 			 */
-			devpriv->transCount = cmd->chanlist_len;
+			devpriv->xfer_count = cmd->chanlist_len;
 			devpriv->flags |= SEND_EOS;
 		} else {
 			/* arrange to transfer data periodically */
-			devpriv->transCount
-			    =
+			devpriv->xfer_count =
 			    (TRANS_TARGET_PERIOD * cmd->chanlist_len) /
 			    cmd->scan_begin_arg;
-			if (devpriv->transCount < cmd->chanlist_len) {
+			if (devpriv->xfer_count < cmd->chanlist_len) {
 				/* transfer after each scan (and avoid 0) */
-				devpriv->transCount = cmd->chanlist_len;
+				devpriv->xfer_count = cmd->chanlist_len;
 			} else {	/* make a multiple of scan length */
-				devpriv->transCount =
-				    (devpriv->transCount +
+				devpriv->xfer_count =
+				    (devpriv->xfer_count +
 				     cmd->chanlist_len - 1)
 				    / cmd->chanlist_len;
-				devpriv->transCount *= cmd->chanlist_len;
+				devpriv->xfer_count *= cmd->chanlist_len;
 			}
 			devpriv->flags |= SEND_EOS;
 		}
-		if (devpriv->transCount >= (devpriv->fifoLen / 2)) {
+		if (devpriv->xfer_count >= (devpriv->fifosz / 2)) {
 			/* out of counter range, use 1/2 fifo instead */
-			devpriv->transCount = 0;
+			devpriv->xfer_count = 0;
 			devpriv->flags &= ~SEND_EOS;
 		} else {
 			/* interrupt for each transfer */
-			writel((devpriv->transCount - 1) & 0xffff,
+			writel((devpriv->xfer_count - 1) & 0xffff,
 				devpriv->las0 + LAS0_ACNT);
 		}
 	} else {		/* unknown timing, just use 1/2 FIFO */
-		devpriv->transCount = 0;
+		devpriv->xfer_count = 0;
 		devpriv->flags &= ~SEND_EOS;
 	}
 	/* pacer clock source: INTERNAL 8MHz */
@@ -961,15 +1054,15 @@ static int rtd_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	/* First, setup when to stop */
 	switch (cmd->stop_src) {
 	case TRIG_COUNT:	/* stop after N scans */
-		devpriv->aiCount = cmd->stop_arg * cmd->chanlist_len;
-		if ((devpriv->transCount > 0)
-		    && (devpriv->transCount > devpriv->aiCount)) {
-			devpriv->transCount = devpriv->aiCount;
+		devpriv->ai_count = cmd->stop_arg * cmd->chanlist_len;
+		if ((devpriv->xfer_count > 0)
+		    && (devpriv->xfer_count > devpriv->ai_count)) {
+			devpriv->xfer_count = devpriv->ai_count;
 		}
 		break;
 
 	case TRIG_NONE:	/* stop when cancel is called */
-		devpriv->aiCount = -1;	/* read forever */
+		devpriv->ai_count = -1;	/* read forever */
 		break;
 	}
 
@@ -1011,17 +1104,14 @@ static int rtd_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 	/* This doesn't seem to work.  There is no way to clear an interrupt
 	   that the priority controller has queued! */
-	devpriv->intClearMask = ~0;
-	writew(devpriv->intClearMask, devpriv->las0 + LAS0_CLEAR);
+	writew(~0, devpriv->las0 + LAS0_CLEAR);
 	readw(devpriv->las0 + LAS0_CLEAR);
 
 	/* TODO: allow multiple interrupt sources */
-	if (devpriv->transCount > 0) {	/* transfer every N samples */
-		devpriv->intMask = IRQM_ADC_ABOUT_CNT;
-		writew(devpriv->intMask, devpriv->las0 + LAS0_IT);
+	if (devpriv->xfer_count > 0) {	/* transfer every N samples */
+		writew(IRQM_ADC_ABOUT_CNT, devpriv->las0 + LAS0_IT);
 	} else {		/* 1/2 FIFO transfers */
-		devpriv->intMask = IRQM_ADC_ABOUT_CNT;
-		writew(devpriv->intMask, devpriv->las0 + LAS0_IT);
+		writew(IRQM_ADC_ABOUT_CNT, devpriv->las0 + LAS0_IT);
 	}
 
 	/* BUG: start_src is ASSUMED to be TRIG_NOW */
@@ -1035,7 +1125,7 @@ static int rtd_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 */
 static int rtd_ai_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 {
-	struct rtdPrivate *devpriv = dev->private;
+	struct rtd_private *devpriv = dev->private;
 	u32 overrun;
 	u16 status;
 
@@ -1043,9 +1133,8 @@ static int rtd_ai_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 	writel(0, devpriv->las0 + LAS0_PACER_STOP);
 	writel(0, devpriv->las0 + LAS0_PACER);	/* stop pacer */
 	writel(0, devpriv->las0 + LAS0_ADC_CONVERSION);
-	devpriv->intMask = 0;
-	writew(devpriv->intMask, devpriv->las0 + LAS0_IT);
-	devpriv->aiCount = 0;	/* stop and don't transfer any more */
+	writew(0, devpriv->las0 + LAS0_IT);
+	devpriv->ai_count = 0;	/* stop and don't transfer any more */
 	status = readw(devpriv->las0 + LAS0_IT);
 	overrun = readl(devpriv->las0 + LAS0_OVERRUN) & 0xffff;
 	return 0;
@@ -1058,7 +1147,7 @@ static int rtd_ao_winsn(struct comedi_device *dev,
 			struct comedi_subdevice *s, struct comedi_insn *insn,
 			unsigned int *data)
 {
-	struct rtdPrivate *devpriv = dev->private;
+	struct rtd_private *devpriv = dev->private;
 	int i;
 	int chan = CR_CHAN(insn->chanspec);
 	int range = CR_RANGE(insn->chanspec);
@@ -1090,7 +1179,7 @@ static int rtd_ao_winsn(struct comedi_device *dev,
 		writew(0, devpriv->las0 +
 			((chan == 0) ? LAS0_DAC1 : LAS0_DAC2));
 
-		devpriv->aoValue[chan] = data[i];	/* save for read back */
+		devpriv->ao_readback[chan] = data[i];
 
 		for (ii = 0; ii < RTD_DAC_TIMEOUT; ++ii) {
 			stat = readl(devpriv->las0 + LAS0_ADC);
@@ -1114,73 +1203,56 @@ static int rtd_ao_rinsn(struct comedi_device *dev,
 			struct comedi_subdevice *s, struct comedi_insn *insn,
 			unsigned int *data)
 {
-	struct rtdPrivate *devpriv = dev->private;
+	struct rtd_private *devpriv = dev->private;
 	int i;
 	int chan = CR_CHAN(insn->chanspec);
 
 	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->aoValue[chan];
+		data[i] = devpriv->ao_readback[chan];
 
 
 	return i;
 }
 
-/*
-   Write a masked set of bits and the read back the port.
-   We track what the bits should be (i.e. we don't read the port first).
-
-   DIO devices are slightly special.  Although it is possible to
- * implement the insn_read/insn_write interface, it is much more
- * useful to applications if you implement the insn_bits interface.
- * This allows packed reading/writing of the DIO channels.  The
- * comedi core can convert between insn_bits and insn_read/write
- */
 static int rtd_dio_insn_bits(struct comedi_device *dev,
 			     struct comedi_subdevice *s,
-			     struct comedi_insn *insn, unsigned int *data)
+			     struct comedi_insn *insn,
+			     unsigned int *data)
 {
-	struct rtdPrivate *devpriv = dev->private;
+	struct rtd_private *devpriv = dev->private;
+	unsigned int mask = data[0];
+	unsigned int bits = data[1];
 
-	/* The insn data is a mask in data[0] and the new data
-	 * in data[1], each channel cooresponding to a bit. */
-	if (data[0]) {
-		s->state &= ~data[0];
-		s->state |= data[0] & data[1];
+	if (mask) {
+		s->state &= ~mask;
+		s->state |= (bits & mask);
 
-		/* Write out the new digital output lines */
 		writew(s->state & 0xff, devpriv->las0 + LAS0_DIO0);
 	}
-	/* on return, data[1] contains the value of the digital
-	 * input lines. */
+
 	data[1] = readw(devpriv->las0 + LAS0_DIO0) & 0xff;
 
 	return insn->n;
 }
 
-/*
-  Configure one bit on a IO port as Input or Output (hence the name :-).
-*/
 static int rtd_dio_insn_config(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
+			       struct comedi_insn *insn,
+			       unsigned int *data)
 {
-	struct rtdPrivate *devpriv = dev->private;
-	int chan = CR_CHAN(insn->chanspec);
+	struct rtd_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int mask = 1 << chan;
 
-	/* The input or output configuration of each digital line is
-	 * configured by a special insn_config instruction.  chanspec
-	 * contains the channel to be changed, and data[0] contains the
-	 * value COMEDI_INPUT or COMEDI_OUTPUT. */
 	switch (data[0]) {
 	case INSN_CONFIG_DIO_OUTPUT:
-		s->io_bits |= 1 << chan;	/* 1 means Out */
+		s->io_bits |= mask;
 		break;
 	case INSN_CONFIG_DIO_INPUT:
-		s->io_bits &= ~(1 << chan);
+		s->io_bits &= ~mask;
 		break;
 	case INSN_CONFIG_DIO_QUERY:
-		data[1] =
-		    (s->io_bits & (1 << chan)) ? COMEDI_OUTPUT : COMEDI_INPUT;
+		data[1] = (s->io_bits & mask) ? COMEDI_OUTPUT : COMEDI_INPUT;
 		return insn->n;
 		break;
 	default:
@@ -1188,30 +1260,30 @@ static int rtd_dio_insn_config(struct comedi_device *dev,
 	}
 
 	/* TODO support digital match interrupts and strobes */
-	devpriv->dioStatus = 0x01;	/* set direction */
-	writew(devpriv->dioStatus, devpriv->las0 + LAS0_DIO_STATUS);
+
+	/* set direction */
+	writew(0x01, devpriv->las0 + LAS0_DIO_STATUS);
 	writew(s->io_bits & 0xff, devpriv->las0 + LAS0_DIO0_CTRL);
-	devpriv->dioStatus = 0x00;	/* clear interrupts */
-	writew(devpriv->dioStatus, devpriv->las0 + LAS0_DIO_STATUS);
+
+	/* clear interrupts */
+	writew(0x00, devpriv->las0 + LAS0_DIO_STATUS);
 
 	/* port1 can only be all input or all output */
 
 	/* there are also 2 user input lines and 2 user output lines */
 
-	return 1;
+	return insn->n;
 }
 
 static void rtd_reset(struct comedi_device *dev)
 {
-	struct rtdPrivate *devpriv = dev->private;
+	struct rtd_private *devpriv = dev->private;
 
 	writel(0, devpriv->las0 + LAS0_BOARD_RESET);
 	udelay(100);		/* needed? */
-	writel(0, devpriv->lcfg + LCFG_ITCSR);
-	devpriv->intMask = 0;
-	writew(devpriv->intMask, devpriv->las0 + LAS0_IT);
-	devpriv->intClearMask = ~0;
-	writew(devpriv->intClearMask, devpriv->las0 + LAS0_CLEAR);
+	writel(0, devpriv->lcfg + PLX_INTRCS_REG);
+	writew(0, devpriv->las0 + LAS0_IT);
+	writew(~0, devpriv->las0 + LAS0_CLEAR);
 	readw(devpriv->las0 + LAS0_CLEAR);
 }
 
@@ -1221,7 +1293,7 @@ static void rtd_reset(struct comedi_device *dev)
  */
 static void rtd_init_board(struct comedi_device *dev)
 {
-	struct rtdPrivate *devpriv = dev->private;
+	struct rtd_private *devpriv = dev->private;
 
 	rtd_reset(dev);
 
@@ -1231,16 +1303,11 @@ static void rtd_init_board(struct comedi_device *dev)
 	writel(0, devpriv->las0 + LAS0_DAC1_RESET);
 	writel(0, devpriv->las0 + LAS0_DAC2_RESET);
 	/* clear digital IO fifo */
-	devpriv->dioStatus = 0;
-	writew(devpriv->dioStatus, devpriv->las0 + LAS0_DIO_STATUS);
-	devpriv->utcCtrl[0] = (0 << 6) | 0x30;
-	devpriv->utcCtrl[1] = (1 << 6) | 0x30;
-	devpriv->utcCtrl[2] = (2 << 6) | 0x30;
-	devpriv->utcCtrl[3] = (3 << 6) | 0x00;
-	writeb(devpriv->utcCtrl[0], devpriv->las0 + LAS0_UTC_CTRL);
-	writeb(devpriv->utcCtrl[1], devpriv->las0 + LAS0_UTC_CTRL);
-	writeb(devpriv->utcCtrl[2], devpriv->las0 + LAS0_UTC_CTRL);
-	writeb(devpriv->utcCtrl[3], devpriv->las0 + LAS0_UTC_CTRL);
+	writew(0, devpriv->las0 + LAS0_DIO_STATUS);
+	writeb((0 << 6) | 0x30, devpriv->las0 + LAS0_UTC_CTRL);
+	writeb((1 << 6) | 0x30, devpriv->las0 + LAS0_UTC_CTRL);
+	writeb((2 << 6) | 0x30, devpriv->las0 + LAS0_UTC_CTRL);
+	writeb((3 << 6) | 0x00, devpriv->las0 + LAS0_UTC_CTRL);
 	/* TODO: set user out source ??? */
 }
 
@@ -1259,51 +1326,34 @@ static void rtd_pci_latency_quirk(struct comedi_device *dev,
 	}
 }
 
-static const void *rtd_find_boardinfo(struct comedi_device *dev,
-				      struct pci_dev *pcidev)
-{
-	const struct rtdBoard *thisboard;
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(rtd520Boards); i++) {
-		thisboard = &rtd520Boards[i];
-		if (pcidev->device == thisboard->device_id)
-			return thisboard;
-	}
-	return NULL;
-}
-
 static int rtd_auto_attach(struct comedi_device *dev,
-				     unsigned long context_unused)
+			   unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct rtdBoard *thisboard;
-	struct rtdPrivate *devpriv;
+	const struct rtd_boardinfo *board = NULL;
+	struct rtd_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret;
 
-	thisboard = rtd_find_boardinfo(dev, pcidev);
-	if (!thisboard)
+	if (context < ARRAY_SIZE(rtd520Boards))
+		board = &rtd520Boards[context];
+	if (!board)
 		return -ENODEV;
-	dev->board_ptr = thisboard;
-	dev->board_name = thisboard->name;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
 
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
-	dev->iobase = 1;	/* the "detach" needs this */
-
-	devpriv->las0 = ioremap_nocache(pci_resource_start(pcidev, 2),
-					pci_resource_len(pcidev, 2));
-	devpriv->las1 = ioremap_nocache(pci_resource_start(pcidev, 3),
-					pci_resource_len(pcidev, 3));
-	devpriv->lcfg = ioremap_nocache(pci_resource_start(pcidev, 0),
-					pci_resource_len(pcidev, 0));
+
+	devpriv->las0 = pci_ioremap_bar(pcidev, 2);
+	devpriv->las1 = pci_ioremap_bar(pcidev, 3);
+	devpriv->lcfg = pci_ioremap_bar(pcidev, 0);
 	if (!devpriv->las0 || !devpriv->las1 || !devpriv->lcfg)
 		return -ENOMEM;
 
@@ -1326,7 +1376,7 @@ static int rtd_auto_attach(struct comedi_device *dev,
 	s->subdev_flags	= SDF_READABLE | SDF_GROUND | SDF_COMMON | SDF_DIFF;
 	s->n_chan	= 16;
 	s->maxdata	= 0x0fff;
-	s->range_table	= thisboard->ai_range;
+	s->range_table	= board->ai_range;
 	s->len_chanlist	= RTD_MAX_CHANLIST;
 	s->insn_read	= rtd_ai_rinsn;
 	if (dev->irq) {
@@ -1370,10 +1420,10 @@ static int rtd_auto_attach(struct comedi_device *dev,
 	ret = rtd520_probe_fifo_depth(dev);
 	if (ret < 0)
 		return ret;
-	devpriv->fifoLen = ret;
+	devpriv->fifosz = ret;
 
 	if (dev->irq)
-		writel(ICS_PIE | ICS_PLIE, devpriv->lcfg + LCFG_ITCSR);
+		writel(ICS_PIE | ICS_PLIE, devpriv->lcfg + PLX_INTRCS_REG);
 
 	dev_info(dev->class_dev, "%s attached\n", dev->board_name);
 
@@ -1382,17 +1432,16 @@ static int rtd_auto_attach(struct comedi_device *dev,
 
 static void rtd_detach(struct comedi_device *dev)
 {
-	struct rtdPrivate *devpriv = dev->private;
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	struct rtd_private *devpriv = dev->private;
 
 	if (devpriv) {
 		/* Shut down any board ops by resetting it */
 		if (devpriv->las0 && devpriv->lcfg)
 			rtd_reset(dev);
 		if (dev->irq) {
-			writel(readl(devpriv->lcfg + LCFG_ITCSR) &
+			writel(readl(devpriv->lcfg + PLX_INTRCS_REG) &
 				~(ICS_PLIE | ICS_DMA0_E | ICS_DMA1_E),
-				devpriv->lcfg + LCFG_ITCSR);
+				devpriv->lcfg + PLX_INTRCS_REG);
 			free_irq(dev->irq, dev);
 		}
 		if (devpriv->las0)
@@ -1402,10 +1451,7 @@ static void rtd_detach(struct comedi_device *dev)
 		if (devpriv->lcfg)
 			iounmap(devpriv->lcfg);
 	}
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-	}
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver rtd520_driver = {
@@ -1416,14 +1462,14 @@ static struct comedi_driver rtd520_driver = {
 };
 
 static int rtd520_pci_probe(struct pci_dev *dev,
-				      const struct pci_device_id *ent)
+			    const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &rtd520_driver);
+	return comedi_pci_auto_config(dev, &rtd520_driver, id->driver_data);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(rtd520_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_RTD, 0x7520) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_RTD, 0x4520) },
+	{ PCI_VDEVICE(RTD, 0x7520), BOARD_DM7520 },
+	{ PCI_VDEVICE(RTD, 0x4520), BOARD_PCI4520 },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, rtd520_pci_table);
diff --git a/drivers/staging/comedi/drivers/rtd520.h b/drivers/staging/comedi/drivers/rtd520.h
deleted file mode 100644
index 25188a58145..00000000000
--- a/drivers/staging/comedi/drivers/rtd520.h
+++ /dev/null
@@ -1,381 +0,0 @@
-/*
-    comedi/drivers/rtd520.h
-    Comedi driver defines for Real Time Devices (RTD) PCI4520/DM7520
-
-    COMEDI - Linux Control and Measurement Device Interface
-    Copyright (C) 2001 David A. Schleef <ds@schleef.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.
-*/
-
-/*
-    Created by Dan Christian, NASA Ames Research Center.
-    See board notes in rtd520.c
-*/
-
-/*
- * Local Address Space 0 Offsets
- */
-#define LAS0_USER_IO		0x0008	/* User I/O */
-#define LAS0_ADC		0x0010	/* FIFO Status/Software A/D Start */
-#define LAS0_DAC1		0x0014	/* Software D/A1 Update (w) */
-#define LAS0_DAC2		0x0018	/* Software D/A2 Update (w) */
-#define LAS0_DAC		0x0024	/* Software Simultaneous Update (w) */
-#define LAS0_PACER		0x0028	/* Software Pacer Start/Stop */
-#define LAS0_TIMER		0x002c	/* Timer Status/HDIN Software Trig. */
-#define LAS0_IT			0x0030	/* Interrupt Status/Enable */
-#define LAS0_CLEAR		0x0034	/* Clear/Set Interrupt Clear Mask */
-#define LAS0_OVERRUN		0x0038	/* Pending interrupts/Clear Overrun */
-#define LAS0_PCLK		0x0040	/* Pacer Clock (24bit) */
-#define LAS0_BCLK		0x0044	/* Burst Clock (10bit) */
-#define LAS0_ADC_SCNT		0x0048	/* A/D Sample counter (10bit) */
-#define LAS0_DAC1_UCNT		0x004c	/* D/A1 Update counter (10 bit) */
-#define LAS0_DAC2_UCNT		0x0050	/* D/A2 Update counter (10 bit) */
-#define LAS0_DCNT		0x0054	/* Delay counter (16 bit) */
-#define LAS0_ACNT		0x0058	/* About counter (16 bit) */
-#define LAS0_DAC_CLK		0x005c	/* DAC clock (16bit) */
-#define LAS0_UTC0		0x0060	/* 8254 TC Counter 0 */
-#define LAS0_UTC1		0x0064	/* 8254 TC Counter 1 */
-#define LAS0_UTC2		0x0068	/* 8254 TC Counter 2 */
-#define LAS0_UTC_CTRL		0x006c	/* 8254 TC Control */
-#define LAS0_DIO0		0x0070	/* Digital I/O Port 0 */
-#define LAS0_DIO1		0x0074	/* Digital I/O Port 1 */
-#define LAS0_DIO0_CTRL		0x0078	/* Digital I/O Control */
-#define LAS0_DIO_STATUS		0x007c	/* Digital I/O Status */
-#define LAS0_BOARD_RESET	0x0100	/* Board reset */
-#define LAS0_DMA0_SRC		0x0104	/* DMA 0 Sources select */
-#define LAS0_DMA1_SRC		0x0108	/* DMA 1 Sources select */
-#define LAS0_ADC_CONVERSION	0x010c	/* A/D Conversion Signal select */
-#define LAS0_BURST_START	0x0110	/* Burst Clock Start Trigger select */
-#define LAS0_PACER_START	0x0114	/* Pacer Clock Start Trigger select */
-#define LAS0_PACER_STOP		0x0118	/* Pacer Clock Stop Trigger select */
-#define LAS0_ACNT_STOP_ENABLE	0x011c	/* About Counter Stop Enable */
-#define LAS0_PACER_REPEAT	0x0120	/* Pacer Start Trigger Mode select */
-#define LAS0_DIN_START		0x0124	/* HiSpd DI Sampling Signal select */
-#define LAS0_DIN_FIFO_CLEAR	0x0128	/* Digital Input FIFO Clear */
-#define LAS0_ADC_FIFO_CLEAR	0x012c	/* A/D FIFO Clear */
-#define LAS0_CGT_WRITE		0x0130	/* Channel Gain Table Write */
-#define LAS0_CGL_WRITE		0x0134	/* Channel Gain Latch Write */
-#define LAS0_CG_DATA		0x0138	/* Digital Table Write */
-#define LAS0_CGT_ENABLE		0x013c	/* Channel Gain Table Enable */
-#define LAS0_CG_ENABLE		0x0140	/* Digital Table Enable */
-#define LAS0_CGT_PAUSE		0x0144	/* Table Pause Enable */
-#define LAS0_CGT_RESET		0x0148	/* Reset Channel Gain Table */
-#define LAS0_CGT_CLEAR		0x014c	/* Clear Channel Gain Table */
-#define LAS0_DAC1_CTRL		0x0150	/* D/A1 output type/range */
-#define LAS0_DAC1_SRC		0x0154	/* D/A1 update source */
-#define LAS0_DAC1_CYCLE		0x0158	/* D/A1 cycle mode */
-#define LAS0_DAC1_RESET		0x015c	/* D/A1 FIFO reset */
-#define LAS0_DAC1_FIFO_CLEAR	0x0160	/* D/A1 FIFO clear */
-#define LAS0_DAC2_CTRL		0x0164	/* D/A2 output type/range */
-#define LAS0_DAC2_SRC		0x0168	/* D/A2 update source */
-#define LAS0_DAC2_CYCLE		0x016c	/* D/A2 cycle mode */
-#define LAS0_DAC2_RESET		0x0170	/* D/A2 FIFO reset */
-#define LAS0_DAC2_FIFO_CLEAR	0x0174	/* D/A2 FIFO clear */
-#define LAS0_ADC_SCNT_SRC	0x0178	/* A/D Sample Counter Source select */
-#define LAS0_PACER_SELECT	0x0180	/* Pacer Clock select */
-#define LAS0_SBUS0_SRC		0x0184	/* SyncBus 0 Source select */
-#define LAS0_SBUS0_ENABLE	0x0188	/* SyncBus 0 enable */
-#define LAS0_SBUS1_SRC		0x018c	/* SyncBus 1 Source select */
-#define LAS0_SBUS1_ENABLE	0x0190	/* SyncBus 1 enable */
-#define LAS0_SBUS2_SRC		0x0198	/* SyncBus 2 Source select */
-#define LAS0_SBUS2_ENABLE	0x019c	/* SyncBus 2 enable */
-#define LAS0_ETRG_POLARITY	0x01a4	/* Ext. Trigger polarity select */
-#define LAS0_EINT_POLARITY	0x01a8	/* Ext. Interrupt polarity select */
-#define LAS0_UTC0_CLOCK		0x01ac	/* UTC0 Clock select */
-#define LAS0_UTC0_GATE		0x01b0	/* UTC0 Gate select */
-#define LAS0_UTC1_CLOCK		0x01b4	/* UTC1 Clock select */
-#define LAS0_UTC1_GATE		0x01b8	/* UTC1 Gate select */
-#define LAS0_UTC2_CLOCK		0x01bc	/* UTC2 Clock select */
-#define LAS0_UTC2_GATE		0x01c0	/* UTC2 Gate select */
-#define LAS0_UOUT0_SELECT	0x01c4	/* User Output 0 source select */
-#define LAS0_UOUT1_SELECT	0x01c8	/* User Output 1 source select */
-#define LAS0_DMA0_RESET		0x01cc	/* DMA0 Request state machine reset */
-#define LAS0_DMA1_RESET		0x01d0	/* DMA1 Request state machine reset */
-
-/*
- * Local Address Space 1 Offsets
- */
-#define LAS1_ADC_FIFO		0x0000	/* A/D FIFO (16bit) */
-#define LAS1_HDIO_FIFO		0x0004	/* HiSpd DI FIFO (16bit) */
-#define LAS1_DAC1_FIFO		0x0008	/* D/A1 FIFO (16bit) */
-#define LAS1_DAC2_FIFO		0x000c	/* D/A2 FIFO (16bit) */
-
-/*
- * PLX 9080 local config & runtime registers
- */
-#define LCFG_ITCSR		0x0068	/* Interrupt Control/Status */
-#define LCFG_DMAMODE0		0x0080	/* DMA0 Mode */
-#define LCFG_DMAPADR0		0x0084	/* DMA0 PCI Address */
-#define LCFG_DMALADR0		0x0088	/* DMA0 Local Address */
-#define LCFG_DMASIZ0		0x008c	/* DMA0 Transfer Size (Bytes) */
-#define LCFG_DMADPR0		0x0090	/* DMA0 Descriptor Pointer */
-#define LCFG_DMAMODE1		0x0094	/* DMA1 Mode */
-#define LCFG_DMAPADR1		0x0098	/* DMA1 PCI Address */
-#define LCFG_DMALADR1		0x009c	/* DMA1 Local Address */
-#define LCFG_DMASIZ1		0x00a0	/* DMA1 Transfer Size (Bytes) */
-#define LCFG_DMADPR1		0x00a4	/* DMA1 Descriptor Pointer */
-#define LCFG_DMACSR0		0x00a8	/* DMA0 Command/Status */
-#define LCFG_DMACSR1		0x00a9	/* DMA0 Command/Status */
-#define LCFG_DMAARB		0x00ac	/* DMA Arbitration */
-#define LCFG_DMATHR		0x00b0	/* DMA Threshold */
-
-/*  FIFO Status Word Bits (RtdFifoStatus) */
-#define FS_DAC1_NOT_EMPTY	(1 << 0)  /* DAC1 FIFO not empty */
-#define FS_DAC1_HEMPTY		(1 << 1)  /* DAC1 FIFO half empty */
-#define FS_DAC1_NOT_FULL	(1 << 2)  /* DAC1 FIFO not full */
-#define FS_DAC2_NOT_EMPTY	(1 << 4)  /* DAC2 FIFO not empty */
-#define FS_DAC2_HEMPTY		(1 << 5)  /* DAC2 FIFO half empty */
-#define FS_DAC2_NOT_FULL	(1 << 6)  /* DAC2 FIFO not full */
-#define FS_ADC_NOT_EMPTY	(1 << 8)  /* ADC FIFO not empty */
-#define FS_ADC_HEMPTY		(1 << 9)  /* ADC FIFO half empty */
-#define FS_ADC_NOT_FULL		(1 << 10) /* ADC FIFO not full */
-#define FS_DIN_NOT_EMPTY	(1 << 12) /* DIN FIFO not empty */
-#define FS_DIN_HEMPTY		(1 << 13) /* DIN FIFO half empty */
-#define FS_DIN_NOT_FULL		(1 << 14) /* DIN FIFO not full */
-
-/*  Timer Status Word Bits (GetTimerStatus) */
-#define TS_PCLK_GATE		(1 << 0)  /* Pacer Clock Gate enabled */
-#define TS_BCLK_GATE		(1 << 1)  /* Burst Clock Gate running */
-#define TS_DCNT_GATE		(1 << 2)  /* Pacer Clock Delayed Start Trig. */
-#define TS_ACNT_GATE		(1 << 3)  /* Pacer Clock About Trig. */
-#define TS_PCLK_RUN		(1 << 4)  /* Pacer Clock Shutdown Flag */
-
-/*  External Trigger polarity select */
-/*  External Interrupt polarity select */
-#define POL_POSITIVE         0x0	/*  positive edge */
-#define POL_NEGATIVE         0x1	/*  negative edge */
-
-/*  User Output Signal select (SetUout0Source, SetUout1Source) */
-#define UOUT_ADC                0x0	/*  A/D Conversion Signal */
-#define UOUT_DAC1               0x1	/*  D/A1 Update */
-#define UOUT_DAC2               0x2	/*  D/A2 Update */
-#define UOUT_SOFTWARE           0x3	/*  Software Programmable */
-
-/*  Pacer clock select (SetPacerSource) */
-#define PCLK_INTERNAL           1	/*  Internal Pacer Clock */
-#define PCLK_EXTERNAL           0	/*  External Pacer Clock */
-
-/*  A/D Sample Counter Sources (SetAdcntSource, SetupSampleCounter) */
-#define ADC_SCNT_CGT_RESET         0x0	/*  needs restart with StartPacer */
-#define ADC_SCNT_FIFO_WRITE        0x1
-
-/*  A/D Conversion Signal Select (for SetConversionSelect) */
-#define ADC_START_SOFTWARE         0x0	/*  Software A/D Start */
-#define ADC_START_PCLK             0x1	/*  Pacer Clock (Ext. Int. see Func.509) */
-#define ADC_START_BCLK             0x2	/*  Burst Clock */
-#define ADC_START_DIGITAL_IT       0x3	/*  Digital Interrupt */
-#define ADC_START_DAC1_MARKER1     0x4	/*  D/A 1 Data Marker 1 */
-#define ADC_START_DAC2_MARKER1     0x5	/*  D/A 2 Data Marker 1 */
-#define ADC_START_SBUS0            0x6	/*  SyncBus 0 */
-#define ADC_START_SBUS1            0x7	/*  SyncBus 1 */
-#define ADC_START_SBUS2            0x8	/*  SyncBus 2 */
-
-/*  Burst Clock start trigger select (SetBurstStart) */
-#define BCLK_START_SOFTWARE        0x0	/*  Software A/D Start (StartBurst) */
-#define BCLK_START_PCLK            0x1	/*  Pacer Clock */
-#define BCLK_START_ETRIG           0x2	/*  External Trigger */
-#define BCLK_START_DIGITAL_IT      0x3	/*  Digital Interrupt */
-#define BCLK_START_SBUS0           0x4	/*  SyncBus 0 */
-#define BCLK_START_SBUS1           0x5	/*  SyncBus 1 */
-#define BCLK_START_SBUS2           0x6	/*  SyncBus 2 */
-
-/*  Pacer Clock start trigger select (SetPacerStart) */
-#define PCLK_START_SOFTWARE        0x0	/*  Software Pacer Start (StartPacer) */
-#define PCLK_START_ETRIG           0x1	/*  External trigger */
-#define PCLK_START_DIGITAL_IT      0x2	/*  Digital interrupt */
-#define PCLK_START_UTC2            0x3	/*  User TC 2 out */
-#define PCLK_START_SBUS0           0x4	/*  SyncBus 0 */
-#define PCLK_START_SBUS1           0x5	/*  SyncBus 1 */
-#define PCLK_START_SBUS2           0x6	/*  SyncBus 2 */
-#define PCLK_START_D_SOFTWARE      0x8	/*  Delayed Software Pacer Start */
-#define PCLK_START_D_ETRIG         0x9	/*  Delayed external trigger */
-#define PCLK_START_D_DIGITAL_IT    0xA	/*  Delayed digital interrupt */
-#define PCLK_START_D_UTC2          0xB	/*  Delayed User TC 2 out */
-#define PCLK_START_D_SBUS0         0xC	/*  Delayed SyncBus 0 */
-#define PCLK_START_D_SBUS1         0xD	/*  Delayed SyncBus 1 */
-#define PCLK_START_D_SBUS2         0xE	/*  Delayed SyncBus 2 */
-#define PCLK_START_ETRIG_GATED     0xF	/*  External Trigger Gated controlled mode */
-
-/*  Pacer Clock Stop Trigger select (SetPacerStop) */
-#define PCLK_STOP_SOFTWARE         0x0	/*  Software Pacer Stop (StopPacer) */
-#define PCLK_STOP_ETRIG            0x1	/*  External Trigger */
-#define PCLK_STOP_DIGITAL_IT       0x2	/*  Digital Interrupt */
-#define PCLK_STOP_ACNT             0x3	/*  About Counter */
-#define PCLK_STOP_UTC2             0x4	/*  User TC2 out */
-#define PCLK_STOP_SBUS0            0x5	/*  SyncBus 0 */
-#define PCLK_STOP_SBUS1            0x6	/*  SyncBus 1 */
-#define PCLK_STOP_SBUS2            0x7	/*  SyncBus 2 */
-#define PCLK_STOP_A_SOFTWARE       0x8	/*  About Software Pacer Stop */
-#define PCLK_STOP_A_ETRIG          0x9	/*  About External Trigger */
-#define PCLK_STOP_A_DIGITAL_IT     0xA	/*  About Digital Interrupt */
-#define PCLK_STOP_A_UTC2           0xC	/*  About User TC2 out */
-#define PCLK_STOP_A_SBUS0          0xD	/*  About SyncBus 0 */
-#define PCLK_STOP_A_SBUS1          0xE	/*  About SyncBus 1 */
-#define PCLK_STOP_A_SBUS2          0xF	/*  About SyncBus 2 */
-
-/*  About Counter Stop Enable */
-#define ACNT_STOP                  0x0	/*  stop enable */
-#define ACNT_NO_STOP               0x1	/*  stop disabled */
-
-/*  DAC update source (SetDAC1Start & SetDAC2Start) */
-#define DAC_START_SOFTWARE         0x0	/*  Software Update */
-#define DAC_START_CGT              0x1	/*  CGT controlled Update */
-#define DAC_START_DAC_CLK          0x2	/*  D/A Clock */
-#define DAC_START_EPCLK            0x3	/*  External Pacer Clock */
-#define DAC_START_SBUS0            0x4	/*  SyncBus 0 */
-#define DAC_START_SBUS1            0x5	/*  SyncBus 1 */
-#define DAC_START_SBUS2            0x6	/*  SyncBus 2 */
-
-/*  DAC Cycle Mode (SetDAC1Cycle, SetDAC2Cycle, SetupDAC) */
-#define DAC_CYCLE_SINGLE           0x0	/*  not cycle */
-#define DAC_CYCLE_MULTI            0x1	/*  cycle */
-
-/*  8254 Operation Modes (Set8254Mode, SetupTimerCounter) */
-#define M8254_EVENT_COUNTER        0	/*  Event Counter */
-#define M8254_HW_ONE_SHOT          1	/*  Hardware-Retriggerable One-Shot */
-#define M8254_RATE_GENERATOR       2	/*  Rate Generator */
-#define M8254_SQUARE_WAVE          3	/*  Square Wave Mode */
-#define M8254_SW_STROBE            4	/*  Software Triggered Strobe */
-#define M8254_HW_STROBE            5	/*  Hardware Triggered Strobe (Retriggerable) */
-
-/*  User Timer/Counter 0 Clock Select (SetUtc0Clock) */
-#define CUTC0_8MHZ                 0x0	/*  8MHz */
-#define CUTC0_EXT_TC_CLOCK1        0x1	/*  Ext. TC Clock 1 */
-#define CUTC0_EXT_TC_CLOCK2        0x2	/*  Ext. TC Clock 2 */
-#define CUTC0_EXT_PCLK             0x3	/*  Ext. Pacer Clock */
-
-/*  User Timer/Counter 1 Clock Select (SetUtc1Clock) */
-#define CUTC1_8MHZ                 0x0	/*  8MHz */
-#define CUTC1_EXT_TC_CLOCK1        0x1	/*  Ext. TC Clock 1 */
-#define CUTC1_EXT_TC_CLOCK2        0x2	/*  Ext. TC Clock 2 */
-#define CUTC1_EXT_PCLK             0x3	/*  Ext. Pacer Clock */
-#define CUTC1_UTC0_OUT             0x4	/*  User Timer/Counter 0 out */
-#define CUTC1_DIN_SIGNAL           0x5	/*  High-Speed Digital Input   Sampling signal */
-
-/*  User Timer/Counter 2 Clock Select (SetUtc2Clock) */
-#define CUTC2_8MHZ                 0x0	/*  8MHz */
-#define CUTC2_EXT_TC_CLOCK1        0x1	/*  Ext. TC Clock 1 */
-#define CUTC2_EXT_TC_CLOCK2        0x2	/*  Ext. TC Clock 2 */
-#define CUTC2_EXT_PCLK             0x3	/*  Ext. Pacer Clock */
-#define CUTC2_UTC1_OUT             0x4	/*  User Timer/Counter 1 out */
-
-/*  User Timer/Counter 0 Gate Select (SetUtc0Gate) */
-#define GUTC0_NOT_GATED            0x0	/*  Not gated */
-#define GUTC0_GATED                0x1	/*  Gated */
-#define GUTC0_EXT_TC_GATE1         0x2	/*  Ext. TC Gate 1 */
-#define GUTC0_EXT_TC_GATE2         0x3	/*  Ext. TC Gate 2 */
-
-/*  User Timer/Counter 1 Gate Select (SetUtc1Gate) */
-#define GUTC1_NOT_GATED            0x0	/*  Not gated */
-#define GUTC1_GATED                0x1	/*  Gated */
-#define GUTC1_EXT_TC_GATE1         0x2	/*  Ext. TC Gate 1 */
-#define GUTC1_EXT_TC_GATE2         0x3	/*  Ext. TC Gate 2 */
-#define GUTC1_UTC0_OUT             0x4	/*  User Timer/Counter 0 out */
-
-/*  User Timer/Counter 2 Gate Select (SetUtc2Gate) */
-#define GUTC2_NOT_GATED            0x0	/*  Not gated */
-#define GUTC2_GATED                0x1	/*  Gated */
-#define GUTC2_EXT_TC_GATE1         0x2	/*  Ext. TC Gate 1 */
-#define GUTC2_EXT_TC_GATE2         0x3	/*  Ext. TC Gate 2 */
-#define GUTC2_UTC1_OUT             0x4	/*  User Timer/Counter 1 out */
-
-/*  Interrupt Source Masks (SetITMask, ClearITMask, GetITStatus) */
-#define IRQM_ADC_FIFO_WRITE        0x0001	/*  ADC FIFO Write */
-#define IRQM_CGT_RESET             0x0002	/*  Reset CGT */
-#define IRQM_CGT_PAUSE             0x0008	/*  Pause CGT */
-#define IRQM_ADC_ABOUT_CNT         0x0010	/*  About Counter out */
-#define IRQM_ADC_DELAY_CNT         0x0020	/*  Delay Counter out */
-#define IRQM_ADC_SAMPLE_CNT	   0x0040	/*  ADC Sample Counter */
-#define IRQM_DAC1_UCNT             0x0080	/*  DAC1 Update Counter */
-#define IRQM_DAC2_UCNT             0x0100	/*  DAC2 Update Counter */
-#define IRQM_UTC1                  0x0200	/*  User TC1 out */
-#define IRQM_UTC1_INV              0x0400	/*  User TC1 out, inverted */
-#define IRQM_UTC2                  0x0800	/*  User TC2 out */
-#define IRQM_DIGITAL_IT            0x1000	/*  Digital Interrupt */
-#define IRQM_EXTERNAL_IT           0x2000	/*  External Interrupt */
-#define IRQM_ETRIG_RISING          0x4000	/*  External Trigger rising-edge */
-#define IRQM_ETRIG_FALLING         0x8000	/*  External Trigger falling-edge */
-
-/*  DMA Request Sources (LAS0) */
-#define DMAS_DISABLED              0x0	/*  DMA Disabled */
-#define DMAS_ADC_SCNT              0x1	/*  ADC Sample Counter */
-#define DMAS_DAC1_UCNT             0x2	/*  D/A1 Update Counter */
-#define DMAS_DAC2_UCNT             0x3	/*  D/A2 Update Counter */
-#define DMAS_UTC1                  0x4	/*  User TC1 out */
-#define DMAS_ADFIFO_HALF_FULL      0x8	/*  A/D FIFO half full */
-#define DMAS_DAC1_FIFO_HALF_EMPTY  0x9	/*  D/A1 FIFO half empty */
-#define DMAS_DAC2_FIFO_HALF_EMPTY  0xA	/*  D/A2 FIFO half empty */
-
-/*  DMA Local Addresses   (0x40000000+LAS1 offset) */
-#define DMALADDR_ADC       0x40000000	/*  A/D FIFO */
-#define DMALADDR_HDIN      0x40000004	/*  High Speed Digital Input FIFO */
-#define DMALADDR_DAC1      0x40000008	/*  D/A1 FIFO */
-#define DMALADDR_DAC2      0x4000000C	/*  D/A2 FIFO */
-
-/*  Port 0 compare modes (SetDIO0CompareMode) */
-#define DIO_MODE_EVENT     0	/*  Event Mode */
-#define DIO_MODE_MATCH     1	/*  Match Mode */
-
-/*  Digital Table Enable (Port 1 disable) */
-#define DTBL_DISABLE       0	/*  Enable Digital Table */
-#define DTBL_ENABLE        1	/*  Disable Digital Table */
-
-/*  Sampling Signal for High Speed Digital Input (SetHdinStart) */
-#define HDIN_SOFTWARE      0x0	/*  Software Trigger */
-#define HDIN_ADC           0x1	/*  A/D Conversion Signal */
-#define HDIN_UTC0          0x2	/*  User TC out 0 */
-#define HDIN_UTC1          0x3	/*  User TC out 1 */
-#define HDIN_UTC2          0x4	/*  User TC out 2 */
-#define HDIN_EPCLK         0x5	/*  External Pacer Clock */
-#define HDIN_ETRG          0x6	/*  External Trigger */
-
-/*  Channel Gain Table / Channel Gain Latch */
-#define CSC_LATCH          0	/*  Channel Gain Latch mode */
-#define CSC_CGT            1	/*  Channel Gain Table mode */
-
-/*  Channel Gain Table Pause Enable */
-#define CGT_PAUSE_DISABLE  0	/*  Channel Gain Table Pause Disable */
-#define CGT_PAUSE_ENABLE   1	/*  Channel Gain Table Pause Enable */
-
-/*  DAC output type/range (p63) */
-#define AOUT_UNIP5         0	/*  0..+5 Volt */
-#define AOUT_UNIP10        1	/*  0..+10 Volt */
-#define AOUT_BIP5          2	/*  -5..+5 Volt */
-#define AOUT_BIP10         3	/*  -10..+10 Volt */
-
-/*  Ghannel Gain Table field definitions (p61) */
-/*  Gain */
-#define GAIN1              0
-#define GAIN2              1
-#define GAIN4              2
-#define GAIN8              3
-#define GAIN16             4
-#define GAIN32             5
-#define GAIN64             6
-#define GAIN128            7
-
-/*  Input range/polarity */
-#define AIN_BIP5           0	/*  -5..+5 Volt */
-#define AIN_BIP10          1	/*  -10..+10 Volt */
-#define AIN_UNIP10         2	/*  0..+10 Volt */
-
-/*  non referenced single ended select bit */
-#define NRSE_AGND          0	/*  AGND referenced SE input */
-#define NRSE_AINS          1	/*  AIN SENSE referenced SE input */
-
-/*  single ended vs differential */
-#define GND_SE		0	/*  Single-Ended */
-#define GND_DIFF	1	/*  Differential */
diff --git a/drivers/staging/comedi/drivers/rti800.c b/drivers/staging/comedi/drivers/rti800.c
index 7e577e44490..f4163fd35a0 100644
--- a/drivers/staging/comedi/drivers/rti800.c
+++ b/drivers/staging/comedi/drivers/rti800.c
@@ -1,186 +1,184 @@
 /*
-   comedi/drivers/rti800.c
-   Hardware driver for Analog Devices RTI-800/815 board
-
-   COMEDI - Linux Control and Measurement Device Interface
-   Copyright (C) 1998 David A. Schleef <ds@schleef.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.
-
+ * comedi/drivers/rti800.c
+ * Hardware driver for Analog Devices RTI-800/815 board
+ *
+ * COMEDI - Linux Control and Measurement Device Interface
+ * Copyright (C) 1998 David A. Schleef <ds@schleef.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.
  */
+
 /*
-Driver: rti800
-Description: Analog Devices RTI-800/815
-Author: ds
-Status: unknown
-Updated: Fri, 05 Sep 2008 14:50:44 +0100
-Devices: [Analog Devices] RTI-800 (rti800), RTI-815 (rti815)
-
-Configuration options:
-  [0] - I/O port base address
-  [1] - IRQ
-  [2] - A/D reference
-	0 = differential
-	1 = pseudodifferential (common)
-	2 = single-ended
-  [3] - A/D range
-	0 = [-10,10]
-	1 = [-5,5]
-	2 = [0,10]
-  [4] - A/D encoding
-	0 = two's complement
-	1 = straight binary
-  [5] - DAC 0 range
-	0 = [-10,10]
-	1 = [0,10]
-  [6] - DAC 0 encoding
-	0 = two's complement
-	1 = straight binary
-  [7] - DAC 1 range (same as DAC 0)
-  [8] - DAC 1 encoding (same as DAC 0)
-*/
+ * Driver: rti800
+ * Description: Analog Devices RTI-800/815
+ * Devices: (Analog Devices) RTI-800 [rti800]
+ *	    (Analog Devices) RTI-815 [rti815]
+ * Author: David A. Schleef <ds@schleef.org>
+ * Status: unknown
+ * Updated: Fri, 05 Sep 2008 14:50:44 +0100
+ *
+ * Configuration options:
+ *   [0] - I/O port base address
+ *   [1] - IRQ (not supported / unused)
+ *   [2] - A/D mux/reference (number of channels)
+ *	   0 = differential
+ *	   1 = pseudodifferential (common)
+ *	   2 = single-ended
+ *   [3] - A/D range
+ *	   0 = [-10,10]
+ *	   1 = [-5,5]
+ *	   2 = [0,10]
+ *   [4] - A/D encoding
+ *	   0 = two's complement
+ *	   1 = straight binary
+ *   [5] - DAC 0 range
+ *	   0 = [-10,10]
+ *	   1 = [0,10]
+ *   [6] - DAC 0 encoding
+ *	   0 = two's complement
+ *	   1 = straight binary
+ *   [7] - DAC 1 range (same as DAC 0)
+ *   [8] - DAC 1 encoding (same as DAC 0)
+ */
 
 #include <linux/interrupt.h>
 #include "../comedidev.h"
 
 #include <linux/ioport.h>
 
-#define RTI800_SIZE 16
-
-#define RTI800_CSR 0
-#define RTI800_MUXGAIN 1
-#define RTI800_CONVERT 2
-#define RTI800_ADCLO 3
-#define RTI800_ADCHI 4
-#define RTI800_DAC0LO 5
-#define RTI800_DAC0HI 6
-#define RTI800_DAC1LO 7
-#define RTI800_DAC1HI 8
-#define RTI800_CLRFLAGS 9
-#define RTI800_DI 10
-#define RTI800_DO 11
-#define RTI800_9513A_DATA 12
-#define RTI800_9513A_CNTRL 13
-#define RTI800_9513A_STATUS 13
-
 /*
- * flags for CSR register
+ * Register map
  */
+#define RTI800_CSR		0x00
+#define RTI800_CSR_BUSY		(1 << 7)
+#define RTI800_CSR_DONE		(1 << 6)
+#define RTI800_CSR_OVERRUN	(1 << 5)
+#define RTI800_CSR_TCR		(1 << 4)
+#define RTI800_CSR_DMA_ENAB	(1 << 3)
+#define RTI800_CSR_INTR_TC	(1 << 2)
+#define RTI800_CSR_INTR_EC	(1 << 1)
+#define RTI800_CSR_INTR_OVRN	(1 << 0)
+#define RTI800_MUXGAIN		0x01
+#define RTI800_CONVERT		0x02
+#define RTI800_ADCLO		0x03
+#define RTI800_ADCHI		0x04
+#define RTI800_DAC0LO		0x05
+#define RTI800_DAC0HI		0x06
+#define RTI800_DAC1LO		0x07
+#define RTI800_DAC1HI		0x08
+#define RTI800_CLRFLAGS		0x09
+#define RTI800_DI		0x0a
+#define RTI800_DO		0x0b
+#define RTI800_9513A_DATA	0x0c
+#define RTI800_9513A_CNTRL	0x0d
+#define RTI800_9513A_STATUS	0x0d
+
+#define RTI800_IOSIZE		0x10
+
+#define RTI800_AI_TIMEOUT	100
+
+static const struct comedi_lrange range_rti800_ai_10_bipolar = {
+	4, {
+		BIP_RANGE(10),
+		BIP_RANGE(1),
+		BIP_RANGE(0.1),
+		BIP_RANGE(0.02)
+	}
+};
 
-#define RTI800_BUSY		0x80
-#define RTI800_DONE		0x40
-#define RTI800_OVERRUN		0x20
-#define RTI800_TCR		0x10
-#define RTI800_DMA_ENAB		0x08
-#define RTI800_INTR_TC		0x04
-#define RTI800_INTR_EC		0x02
-#define RTI800_INTR_OVRN	0x01
-
-#define Am9513_8BITBUS
-
-#define Am9513_output_control(a)	outb(a, dev->iobase+RTI800_9513A_CNTRL)
-#define Am9513_output_data(a)		outb(a, dev->iobase+RTI800_9513A_DATA)
-#define Am9513_input_data()		inb(dev->iobase+RTI800_9513A_DATA)
-#define Am9513_input_status()		inb(dev->iobase+RTI800_9513A_STATUS)
-
-#include "am9513.h"
-
-static const struct comedi_lrange range_rti800_ai_10_bipolar = { 4, {
-								     BIP_RANGE
-								     (10),
-								     BIP_RANGE
-								     (1),
-								     BIP_RANGE
-								     (0.1),
-								     BIP_RANGE
-								     (0.02)
-								     }
+static const struct comedi_lrange range_rti800_ai_5_bipolar = {
+	4, {
+		BIP_RANGE(5),
+		BIP_RANGE(0.5),
+		BIP_RANGE(0.05),
+		BIP_RANGE(0.01)
+	}
 };
 
-static const struct comedi_lrange range_rti800_ai_5_bipolar = { 4, {
-								    BIP_RANGE
-								    (5),
-								    BIP_RANGE
-								    (0.5),
-								    BIP_RANGE
-								    (0.05),
-								    BIP_RANGE
-								    (0.01)
-								    }
+static const struct comedi_lrange range_rti800_ai_unipolar = {
+	4, {
+		UNI_RANGE(10),
+		UNI_RANGE(1),
+		UNI_RANGE(0.1),
+		UNI_RANGE(0.02)
+	}
 };
 
-static const struct comedi_lrange range_rti800_ai_unipolar = { 4, {
-								   UNI_RANGE
-								   (10),
-								   UNI_RANGE(1),
-								   UNI_RANGE
-								   (0.1),
-								   UNI_RANGE
-								   (0.02)
-								   }
+static const struct comedi_lrange *const rti800_ai_ranges[] = {
+	&range_rti800_ai_10_bipolar,
+	&range_rti800_ai_5_bipolar,
+	&range_rti800_ai_unipolar,
 };
 
-struct rti800_board {
+static const struct comedi_lrange *const rti800_ao_ranges[] = {
+	&range_bipolar10,
+	&range_unipolar10,
+};
 
+struct rti800_board {
 	const char *name;
 	int has_ao;
 };
 
-static irqreturn_t rti800_interrupt(int irq, void *dev);
+static const struct rti800_board rti800_boardtypes[] = {
+	{
+		.name		= "rti800",
+	}, {
+		.name		= "rti815",
+		.has_ao		= 1,
+	},
+};
 
 struct rti800_private {
-	enum {
-		adc_diff, adc_pseudodiff, adc_singleended
-	} adc_mux;
-	enum {
-		adc_bipolar10, adc_bipolar5, adc_unipolar10
-	} adc_range;
-	enum {
-		adc_2comp, adc_straight
-	} adc_coding;
-	enum {
-		dac_bipolar10, dac_unipolar10
-	} dac0_range, dac1_range;
-	enum {
-		dac_2comp, dac_straight
-	} dac0_coding, dac1_coding;
+	bool adc_2comp;
+	bool dac_2comp[2];
 	const struct comedi_lrange *ao_range_type_list[2];
 	unsigned int ao_readback[2];
-	int muxgain_bits;
+	unsigned char muxgain_bits;
 };
 
-#define RTI800_TIMEOUT 100
-
-static irqreturn_t rti800_interrupt(int irq, void *dev)
+static int rti800_ai_wait_for_conversion(struct comedi_device *dev,
+					 int timeout)
 {
-	return IRQ_HANDLED;
-}
+	unsigned char status;
+	int i;
 
-/* settling delay times in usec for different gains */
-static const int gaindelay[] = { 10, 20, 40, 80 };
+	for (i = 0; i < timeout; i++) {
+		status = inb(dev->iobase + RTI800_CSR);
+		if (status & RTI800_CSR_OVERRUN) {
+			outb(0, dev->iobase + RTI800_CLRFLAGS);
+			return -EIO;
+		}
+		if (status & RTI800_CSR_DONE)
+			return 0;
+		udelay(1);
+	}
+	return -ETIME;
+}
 
 static int rti800_ai_insn_read(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
+			       struct comedi_insn *insn,
+			       unsigned int *data)
 {
 	struct rti800_private *devpriv = dev->private;
-	int i, t;
-	int status;
-	int chan = CR_CHAN(insn->chanspec);
-	unsigned gain = CR_RANGE(insn->chanspec);
-	unsigned muxgain_bits;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int gain = CR_RANGE(insn->chanspec);
+	unsigned char muxgain_bits;
+	int ret;
+	int i;
 
 	inb(dev->iobase + RTI800_ADCHI);
 	outb(0, dev->iobase + RTI800_CLRFLAGS);
@@ -189,80 +187,80 @@ static int rti800_ai_insn_read(struct comedi_device *dev,
 	if (muxgain_bits != devpriv->muxgain_bits) {
 		devpriv->muxgain_bits = muxgain_bits;
 		outb(devpriv->muxgain_bits, dev->iobase + RTI800_MUXGAIN);
-		/* without a delay here, the RTI_OVERRUN bit
-		 * gets set, and you will have an error. */
+		/*
+		 * Without a delay here, the RTI_CSR_OVERRUN bit
+		 * gets set, and you will have an error.
+		 */
 		if (insn->n > 0) {
-			BUG_ON(gain >= ARRAY_SIZE(gaindelay));
-			udelay(gaindelay[gain]);
+			int delay = (gain == 0) ? 10 :
+				    (gain == 1) ? 20 :
+				    (gain == 2) ? 40 : 80;
+
+			udelay(delay);
 		}
 	}
 
 	for (i = 0; i < insn->n; i++) {
 		outb(0, dev->iobase + RTI800_CONVERT);
-		for (t = RTI800_TIMEOUT; t; t--) {
-			status = inb(dev->iobase + RTI800_CSR);
-			if (status & RTI800_OVERRUN) {
-				printk(KERN_WARNING "rti800: a/d overrun\n");
-				outb(0, dev->iobase + RTI800_CLRFLAGS);
-				return -EIO;
-			}
-			if (status & RTI800_DONE)
-				break;
-			udelay(1);
-		}
-		if (t == 0) {
-			printk(KERN_WARNING "rti800: timeout\n");
-			return -ETIME;
-		}
+		ret = rti800_ai_wait_for_conversion(dev, RTI800_AI_TIMEOUT);
+		if (ret)
+			return ret;
+
 		data[i] = inb(dev->iobase + RTI800_ADCLO);
-		data[i] |= (0xf & inb(dev->iobase + RTI800_ADCHI)) << 8;
+		data[i] |= (inb(dev->iobase + RTI800_ADCHI) & 0xf) << 8;
 
-		if (devpriv->adc_coding == adc_2comp)
+		if (devpriv->adc_2comp)
 			data[i] ^= 0x800;
 	}
 
-	return i;
+	return insn->n;
 }
 
 static int rti800_ao_insn_read(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
+			       struct comedi_insn *insn,
+			       unsigned int *data)
 {
 	struct rti800_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
 	int i;
-	int chan = CR_CHAN(insn->chanspec);
 
 	for (i = 0; i < insn->n; i++)
 		data[i] = devpriv->ao_readback[chan];
 
-	return i;
+	return insn->n;
 }
 
 static int rti800_ao_insn_write(struct comedi_device *dev,
 				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data)
+				struct comedi_insn *insn,
+				unsigned int *data)
 {
 	struct rti800_private *devpriv = dev->private;
-	int chan = CR_CHAN(insn->chanspec);
-	int d;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	int reg_lo = chan ? RTI800_DAC1LO : RTI800_DAC0LO;
+	int reg_hi = chan ? RTI800_DAC1HI : RTI800_DAC0HI;
+	int val = devpriv->ao_readback[chan];
 	int i;
 
 	for (i = 0; i < insn->n; i++) {
-		devpriv->ao_readback[chan] = d = data[i];
-		if (devpriv->dac0_coding == dac_2comp)
-			d ^= 0x800;
-
-		outb(d & 0xff,
-		     dev->iobase + (chan ? RTI800_DAC1LO : RTI800_DAC0LO));
-		outb(d >> 8,
-		     dev->iobase + (chan ? RTI800_DAC1HI : RTI800_DAC0HI));
+		val = data[i];
+		if (devpriv->dac_2comp[chan])
+			val ^= 0x800;
+
+		outb(val & 0xff, dev->iobase + reg_lo);
+		outb((val >> 8) & 0xff, dev->iobase + reg_hi);
 	}
-	return i;
+
+	devpriv->ao_readback[chan] = val;
+
+	return insn->n;
 }
 
 static int rti800_di_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
+			       struct comedi_insn *insn,
+			       unsigned int *data)
 {
 	data[1] = inb(dev->iobase + RTI800_DI);
 	return insn->n;
@@ -270,11 +268,16 @@ static int rti800_di_insn_bits(struct comedi_device *dev,
 
 static int rti800_do_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
+			       struct comedi_insn *insn,
+			       unsigned int *data)
 {
-	if (data[0]) {
-		s->state &= ~data[0];
-		s->state |= data[0] & data[1];
+	unsigned int mask = data[0];
+	unsigned int bits = data[1];
+
+	if (mask) {
+		s->state &= ~mask;
+		s->state |= (bits & mask);
+
 		/* Outputs are inverted... */
 		outb(s->state ^ 0xff, dev->iobase + RTI800_DO);
 	}
@@ -284,186 +287,106 @@ static int rti800_do_insn_bits(struct comedi_device *dev,
 	return insn->n;
 }
 
-/*
-   options[0] - I/O port
-   options[1] - irq
-   options[2] - a/d mux
-	0=differential, 1=pseudodiff, 2=single
-   options[3] - a/d range
-	0=bipolar10, 1=bipolar5, 2=unipolar10
-   options[4] - a/d coding
-	0=2's comp, 1=straight binary
-   options[5] - dac0 range
-	0=bipolar10, 1=unipolar10
-   options[6] - dac0 coding
-	0=2's comp, 1=straight binary
-   options[7] - dac1 range
-   options[8] - dac1 coding
- */
-
 static int rti800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	const struct rti800_board *board = comedi_board(dev);
 	struct rti800_private *devpriv;
-	unsigned int irq;
-	unsigned long iobase;
-	int ret;
 	struct comedi_subdevice *s;
+	int ret;
 
-	iobase = it->options[0];
-	printk(KERN_INFO "comedi%d: rti800: 0x%04lx\n", dev->minor, iobase);
-	if (!request_region(iobase, RTI800_SIZE, "rti800")) {
-		printk(KERN_WARNING "I/O port conflict\n");
-		return -EIO;
-	}
-	dev->iobase = iobase;
-
-#ifdef DEBUG
-	printk(KERN_DEBUG "fingerprint=%x,%x,%x,%x,%x ",
-	       inb(dev->iobase + 0),
-	       inb(dev->iobase + 1),
-	       inb(dev->iobase + 2),
-	       inb(dev->iobase + 3), inb(dev->iobase + 4));
-#endif
+	ret = comedi_request_region(dev, it->options[0], RTI800_IOSIZE);
+	if (ret)
+		return ret;
 
 	outb(0, dev->iobase + RTI800_CSR);
 	inb(dev->iobase + RTI800_ADCHI);
 	outb(0, dev->iobase + RTI800_CLRFLAGS);
 
-	irq = it->options[1];
-	if (irq) {
-		printk(KERN_INFO "( irq = %u )\n", irq);
-		ret = request_irq(irq, rti800_interrupt, 0, "rti800", dev);
-		if (ret < 0) {
-			printk(KERN_WARNING " Failed to allocate IRQ\n");
-			return ret;
-		}
-		dev->irq = irq;
-	} else {
-		printk(KERN_INFO "( no irq )\n");
-	}
-
-	dev->board_name = board->name;
-
-	ret = comedi_alloc_subdevices(dev, 4);
-	if (ret)
-		return ret;
-
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	devpriv->adc_mux = it->options[2];
-	devpriv->adc_range = it->options[3];
-	devpriv->adc_coding = it->options[4];
-	devpriv->dac0_range = it->options[5];
-	devpriv->dac0_coding = it->options[6];
-	devpriv->dac1_range = it->options[7];
-	devpriv->dac1_coding = it->options[8];
-	devpriv->muxgain_bits = -1;
+	devpriv->adc_2comp = (it->options[4] == 0);
+	devpriv->dac_2comp[0] = (it->options[6] == 0);
+	devpriv->dac_2comp[1] = (it->options[8] == 0);
+	/* invalid, forces the MUXGAIN register to be set when first used */
+	devpriv->muxgain_bits = 0xff;
+
+	ret = comedi_alloc_subdevices(dev, 4);
+	if (ret)
+		return ret;
 
 	s = &dev->subdevices[0];
 	/* ai subdevice */
-	s->type = COMEDI_SUBD_AI;
-	s->subdev_flags = SDF_READABLE | SDF_GROUND;
-	s->n_chan = (devpriv->adc_mux ? 16 : 8);
-	s->insn_read = rti800_ai_insn_read;
-	s->maxdata = 0xfff;
-	switch (devpriv->adc_range) {
-	case adc_bipolar10:
-		s->range_table = &range_rti800_ai_10_bipolar;
-		break;
-	case adc_bipolar5:
-		s->range_table = &range_rti800_ai_5_bipolar;
-		break;
-	case adc_unipolar10:
-		s->range_table = &range_rti800_ai_unipolar;
-		break;
-	}
+	s->type		= COMEDI_SUBD_AI;
+	s->subdev_flags	= SDF_READABLE | SDF_GROUND;
+	s->n_chan	= (it->options[2] ? 16 : 8);
+	s->insn_read	= rti800_ai_insn_read;
+	s->maxdata	= 0x0fff;
+	s->range_table	= (it->options[3] < ARRAY_SIZE(rti800_ai_ranges))
+				? rti800_ai_ranges[it->options[3]]
+				: &range_unknown;
 
 	s = &dev->subdevices[1];
 	if (board->has_ao) {
 		/* ao subdevice (only on rti815) */
-		s->type = COMEDI_SUBD_AO;
-		s->subdev_flags = SDF_WRITABLE;
-		s->n_chan = 2;
-		s->insn_read = rti800_ao_insn_read;
-		s->insn_write = rti800_ao_insn_write;
-		s->maxdata = 0xfff;
+		s->type		= COMEDI_SUBD_AO;
+		s->subdev_flags	= SDF_WRITABLE;
+		s->n_chan	= 2;
+		s->insn_read	= rti800_ao_insn_read;
+		s->insn_write	= rti800_ao_insn_write;
+		s->maxdata	= 0x0fff;
 		s->range_table_list = devpriv->ao_range_type_list;
-		switch (devpriv->dac0_range) {
-		case dac_bipolar10:
-			devpriv->ao_range_type_list[0] = &range_bipolar10;
-			break;
-		case dac_unipolar10:
-			devpriv->ao_range_type_list[0] = &range_unipolar10;
-			break;
-		}
-		switch (devpriv->dac1_range) {
-		case dac_bipolar10:
-			devpriv->ao_range_type_list[1] = &range_bipolar10;
-			break;
-		case dac_unipolar10:
-			devpriv->ao_range_type_list[1] = &range_unipolar10;
-			break;
-		}
+		devpriv->ao_range_type_list[0] =
+			(it->options[5] < ARRAY_SIZE(rti800_ao_ranges))
+				? rti800_ao_ranges[it->options[5]]
+				: &range_unknown;
+		devpriv->ao_range_type_list[1] =
+			(it->options[7] < ARRAY_SIZE(rti800_ao_ranges))
+				? rti800_ao_ranges[it->options[7]]
+				: &range_unknown;
 	} else {
-		s->type = COMEDI_SUBD_UNUSED;
+		s->type		= COMEDI_SUBD_UNUSED;
 	}
 
 	s = &dev->subdevices[2];
 	/* di */
-	s->type = COMEDI_SUBD_DI;
-	s->subdev_flags = SDF_READABLE;
-	s->n_chan = 8;
-	s->insn_bits = rti800_di_insn_bits;
-	s->maxdata = 1;
-	s->range_table = &range_digital;
+	s->type		= COMEDI_SUBD_DI;
+	s->subdev_flags	= SDF_READABLE;
+	s->n_chan	= 8;
+	s->insn_bits	= rti800_di_insn_bits;
+	s->maxdata	= 1;
+	s->range_table	= &range_digital;
 
 	s = &dev->subdevices[3];
 	/* do */
-	s->type = COMEDI_SUBD_DO;
-	s->subdev_flags = SDF_WRITABLE;
-	s->n_chan = 8;
-	s->insn_bits = rti800_do_insn_bits;
-	s->maxdata = 1;
-	s->range_table = &range_digital;
-
-/* don't yet know how to deal with counter/timers */
-#if 0
-	s = &dev->subdevices[4];
-	/* do */
-	s->type = COMEDI_SUBD_TIMER;
-#endif
+	s->type		= COMEDI_SUBD_DO;
+	s->subdev_flags	= SDF_WRITABLE;
+	s->n_chan	= 8;
+	s->insn_bits	= rti800_do_insn_bits;
+	s->maxdata	= 1;
+	s->range_table	= &range_digital;
+
+	/*
+	 * There is also an Am9513 timer on these boards. This subdevice
+	 * is not currently supported.
+	 */
 
 	return 0;
 }
 
-static void rti800_detach(struct comedi_device *dev)
-{
-	if (dev->iobase)
-		release_region(dev->iobase, RTI800_SIZE);
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-}
-
-static const struct rti800_board boardtypes[] = {
-	{ "rti800", 0 },
-	{ "rti815", 1 },
-};
-
 static struct comedi_driver rti800_driver = {
 	.driver_name	= "rti800",
 	.module		= THIS_MODULE,
 	.attach		= rti800_attach,
-	.detach		= rti800_detach,
-	.num_names	= ARRAY_SIZE(boardtypes),
-	.board_name	= &boardtypes[0].name,
+	.detach		= comedi_legacy_detach,
+	.num_names	= ARRAY_SIZE(rti800_boardtypes),
+	.board_name	= &rti800_boardtypes[0].name,
 	.offset		= sizeof(struct rti800_board),
 };
 module_comedi_driver(rti800_driver);
 
+MODULE_DESCRIPTION("Comedi: RTI-800 Multifunction Analog/Digital board");
 MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/rti802.c b/drivers/staging/comedi/drivers/rti802.c
index 2185ca1bcf0..46dbbe6cdd7 100644
--- a/drivers/staging/comedi/drivers/rti802.c
+++ b/drivers/staging/comedi/drivers/rti802.c
@@ -92,18 +92,11 @@ static int rti802_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	struct rti802_private *devpriv;
 	struct comedi_subdevice *s;
 	int i;
-	unsigned long iobase;
 	int ret;
 
-	iobase = it->options[0];
-	printk(KERN_INFO "comedi%d: rti802: 0x%04lx ", dev->minor, iobase);
-	if (!request_region(iobase, RTI802_SIZE, "rti802")) {
-		printk(KERN_WARNING "I/O port conflict\n");
-		return -EIO;
-	}
-	dev->iobase = iobase;
-
-	dev->board_name = "rti802";
+	ret = comedi_request_region(dev, it->options[0], RTI802_SIZE);
+	if (ret)
+		return ret;
 
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
@@ -135,17 +128,11 @@ static int rti802_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static void rti802_detach(struct comedi_device *dev)
-{
-	if (dev->iobase)
-		release_region(dev->iobase, RTI802_SIZE);
-}
-
 static struct comedi_driver rti802_driver = {
 	.driver_name	= "rti802",
 	.module		= THIS_MODULE,
 	.attach		= rti802_attach,
-	.detach		= rti802_detach,
+	.detach		= comedi_legacy_detach,
 };
 module_comedi_driver(rti802_driver);
 
diff --git a/drivers/staging/comedi/drivers/s526.c b/drivers/staging/comedi/drivers/s526.c
index 39232b35945..d240ce87bd6 100644
--- a/drivers/staging/comedi/drivers/s526.c
+++ b/drivers/staging/comedi/drivers/s526.c
@@ -552,17 +552,11 @@ static int s526_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	struct s526_private *devpriv;
 	struct comedi_subdevice *s;
-	int iobase;
 	int ret;
 
-	dev->board_name = dev->driver->driver_name;
-
-	iobase = it->options[0];
-	if (!iobase || !request_region(iobase, S526_IOSIZE, dev->board_name)) {
-		comedi_error(dev, "I/O port conflict");
-		return -EIO;
-	}
-	dev->iobase = iobase;
+	ret = comedi_request_region(dev, it->options[0], S526_IOSIZE);
+	if (ret)
+		return ret;
 
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
@@ -616,22 +610,14 @@ static int s526_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->insn_bits = s526_dio_insn_bits;
 	s->insn_config = s526_dio_insn_config;
 
-	dev_info(dev->class_dev, "%s attached\n", dev->board_name);
-
 	return 1;
 }
 
-static void s526_detach(struct comedi_device *dev)
-{
-	if (dev->iobase > 0)
-		release_region(dev->iobase, S526_IOSIZE);
-}
-
 static struct comedi_driver s526_driver = {
 	.driver_name	= "s526",
 	.module		= THIS_MODULE,
 	.attach		= s526_attach,
-	.detach		= s526_detach,
+	.detach		= comedi_legacy_detach,
 };
 module_comedi_driver(s526_driver);
 
diff --git a/drivers/staging/comedi/drivers/s626.c b/drivers/staging/comedi/drivers/s626.c
index 81a1fe66157..0cf4b3d1279 100644
--- a/drivers/staging/comedi/drivers/s626.c
+++ b/drivers/staging/comedi/drivers/s626.c
@@ -80,7 +80,7 @@ INSN_CONFIG instructions:
 #define PCI_SUBDEVICE_ID_S626 0x0272
 
 struct s626_private {
-	void __iomem *base_addr;
+	void __iomem *mmio;
 	uint8_t ai_cmd_running;	/*  ai_cmd is running */
 	uint8_t ai_continous;	/*  continous acquisition */
 	int ai_sample_count;	/*  number of samples to acquire */
@@ -106,64 +106,6 @@ struct s626_private {
 	unsigned int ao_readback[S626_DAC_CHANNELS];
 };
 
-struct dio_private {
-	uint16_t RDDIn;
-	uint16_t WRDOut;
-	uint16_t RDEdgSel;
-	uint16_t WREdgSel;
-	uint16_t RDCapSel;
-	uint16_t WRCapSel;
-	uint16_t RDCapFlg;
-	uint16_t RDIntSel;
-	uint16_t WRIntSel;
-};
-
-static struct dio_private dio_private_A = {
-	.RDDIn = LP_RDDINA,
-	.WRDOut = LP_WRDOUTA,
-	.RDEdgSel = LP_RDEDGSELA,
-	.WREdgSel = LP_WREDGSELA,
-	.RDCapSel = LP_RDCAPSELA,
-	.WRCapSel = LP_WRCAPSELA,
-	.RDCapFlg = LP_RDCAPFLGA,
-	.RDIntSel = LP_RDINTSELA,
-	.WRIntSel = LP_WRINTSELA,
-};
-
-static struct dio_private dio_private_B = {
-	.RDDIn = LP_RDDINB,
-	.WRDOut = LP_WRDOUTB,
-	.RDEdgSel = LP_RDEDGSELB,
-	.WREdgSel = LP_WREDGSELB,
-	.RDCapSel = LP_RDCAPSELB,
-	.WRCapSel = LP_WRCAPSELB,
-	.RDCapFlg = LP_RDCAPFLGB,
-	.RDIntSel = LP_RDINTSELB,
-	.WRIntSel = LP_WRINTSELB,
-};
-
-static struct dio_private dio_private_C = {
-	.RDDIn = LP_RDDINC,
-	.WRDOut = LP_WRDOUTC,
-	.RDEdgSel = LP_RDEDGSELC,
-	.WREdgSel = LP_WREDGSELC,
-	.RDCapSel = LP_RDCAPSELC,
-	.WRCapSel = LP_WRCAPSELC,
-	.RDCapFlg = LP_RDCAPFLGC,
-	.RDIntSel = LP_RDINTSELC,
-	.WRIntSel = LP_WRINTSELC,
-};
-
-/* to group dio devices (48 bits mask and data are not allowed ???)
-static struct dio_private *dio_private_word[]={
-  &dio_private_A,
-  &dio_private_B,
-  &dio_private_C,
-};
-*/
-
-#define diopriv ((struct dio_private *)s->private)
-
 /*  COUNTER OBJECT ------------------------------------------------ */
 struct enc_private {
 	/*  Pointers to functions that differ for A and B counters: */
@@ -195,37 +137,53 @@ struct enc_private {
 /*  Translation table to map IntSrc into equivalent RDMISC2 event flag  bits. */
 /* static const uint16_t EventBits[][4] = { EVBITS(0), EVBITS(1), EVBITS(2), EVBITS(3), EVBITS(4), EVBITS(5) }; */
 
-/*  enab/disable a function or test status bit(s) that are accessed */
-/*  through Main Control Registers 1 or 2. */
-#define MC_ENABLE(REGADRS, CTRLWORD)	writel(((uint32_t)(CTRLWORD) << 16) | (uint32_t)(CTRLWORD), devpriv->base_addr+(REGADRS))
+/*
+ * Enable/disable a function or test status bit(s) that are accessed
+ * through Main Control Registers 1 or 2.
+ */
+static void s626_mc_enable(struct comedi_device *dev,
+			   unsigned int cmd, unsigned int reg)
+{
+	struct s626_private *devpriv = dev->private;
+	unsigned int val = (cmd << 16) | cmd;
 
-#define MC_DISABLE(REGADRS, CTRLWORD)	writel((uint32_t)(CTRLWORD) << 16 , devpriv->base_addr+(REGADRS))
+	writel(val, devpriv->mmio + reg);
+}
 
-#define MC_TEST(REGADRS, CTRLWORD)	((readl(devpriv->base_addr+(REGADRS)) & CTRLWORD) != 0)
+static void s626_mc_disable(struct comedi_device *dev,
+			    unsigned int cmd, unsigned int reg)
+{
+	struct s626_private *devpriv = dev->private;
 
-/* #define WR7146(REGARDS,CTRLWORD)
-    writel(CTRLWORD,(uint32_t)(devpriv->base_addr+(REGARDS))) */
-#define WR7146(REGARDS, CTRLWORD) writel(CTRLWORD, devpriv->base_addr+(REGARDS))
+	writel(cmd << 16 , devpriv->mmio + reg);
+}
 
-/* #define RR7146(REGARDS)
-    readl((uint32_t)(devpriv->base_addr+(REGARDS))) */
-#define RR7146(REGARDS)		readl(devpriv->base_addr+(REGARDS))
+static bool s626_mc_test(struct comedi_device *dev,
+			 unsigned int cmd, unsigned int reg)
+{
+	struct s626_private *devpriv = dev->private;
+	unsigned int val;
+
+	val = readl(devpriv->mmio + reg);
+
+	return (val & cmd) ? true : false;
+}
 
 #define BUGFIX_STREG(REGADRS)   (REGADRS - 4)
 
 /*  Write a time slot control record to TSL2. */
 #define VECTPORT(VECTNUM)		(P_TSL2 + ((VECTNUM) << 2))
-#define SETVECT(VECTNUM, VECTVAL)	WR7146(VECTPORT(VECTNUM), (VECTVAL))
 
 /*  Code macros used for constructing I2C command bytes. */
 #define I2C_B2(ATTR, VAL)	(((ATTR) << 6) | ((VAL) << 24))
 #define I2C_B1(ATTR, VAL)	(((ATTR) << 4) | ((VAL) << 16))
 #define I2C_B0(ATTR, VAL)	(((ATTR) << 2) | ((VAL) <<  8))
 
-static const struct comedi_lrange s626_range_table = { 2, {
-							   RANGE(-5, 5),
-							   RANGE(-10, 10),
-							   }
+static const struct comedi_lrange s626_range_table = {
+	2, {
+		BIP_RANGE(5),
+		BIP_RANGE(10),
+	}
 };
 
 /*  Execute a DEBI transfer.  This must be called from within a */
@@ -234,16 +192,18 @@ static void DEBItransfer(struct comedi_device *dev)
 {
 	struct s626_private *devpriv = dev->private;
 
-	/*  Initiate upload of shadow RAM to DEBI control register. */
-	MC_ENABLE(P_MC2, MC2_UPLD_DEBI);
+	/* Initiate upload of shadow RAM to DEBI control register */
+	s626_mc_enable(dev, MC2_UPLD_DEBI, P_MC2);
 
-	/*  Wait for completion of upload from shadow RAM to DEBI control */
-	/*  register. */
-	while (!MC_TEST(P_MC2, MC2_UPLD_DEBI))
+	/*
+	 * Wait for completion of upload from shadow RAM to
+	 * DEBI control register.
+	 */
+	while (!s626_mc_test(dev, MC2_UPLD_DEBI, P_MC2))
 		;
 
-	/*  Wait until DEBI transfer is done. */
-	while (RR7146(P_PSR) & PSR_DEBI_S)
+	/* Wait until DEBI transfer is done */
+	while (readl(devpriv->mmio + P_PSR) & PSR_DEBI_S)
 		;
 }
 
@@ -252,19 +212,14 @@ static void DEBItransfer(struct comedi_device *dev)
 static uint16_t DEBIread(struct comedi_device *dev, uint16_t addr)
 {
 	struct s626_private *devpriv = dev->private;
-	uint16_t retval;
 
-	/*  Set up DEBI control register value in shadow RAM. */
-	WR7146(P_DEBICMD, DEBI_CMD_RDWORD | addr);
+	/* Set up DEBI control register value in shadow RAM */
+	writel(DEBI_CMD_RDWORD | addr, devpriv->mmio + P_DEBICMD);
 
 	/*  Execute the DEBI transfer. */
 	DEBItransfer(dev);
 
-	/*  Fetch target register value. */
-	retval = (uint16_t) RR7146(P_DEBIAD);
-
-	/*  Return register value. */
-	return retval;
+	return readl(devpriv->mmio + P_DEBIAD);
 }
 
 /*  Write a value to a gate array register. */
@@ -272,9 +227,9 @@ static void DEBIwrite(struct comedi_device *dev, uint16_t addr, uint16_t wdata)
 {
 	struct s626_private *devpriv = dev->private;
 
-	/*  Set up DEBI control register value in shadow RAM. */
-	WR7146(P_DEBICMD, DEBI_CMD_WRWORD | addr);
-	WR7146(P_DEBIAD, wdata);
+	/* Set up DEBI control register value in shadow RAM */
+	writel(DEBI_CMD_WRWORD | addr, devpriv->mmio + P_DEBICMD);
+	writel(wdata, devpriv->mmio + P_DEBIAD);
 
 	/*  Execute the DEBI transfer. */
 	DEBItransfer(dev);
@@ -284,23 +239,22 @@ static void DEBIwrite(struct comedi_device *dev, uint16_t addr, uint16_t wdata)
  * specifies bits that are to be preserved, wdata is new value to be
  * or'd with the masked original.
  */
-static void DEBIreplace(struct comedi_device *dev, uint16_t addr, uint16_t mask,
-			uint16_t wdata)
+static void DEBIreplace(struct comedi_device *dev, unsigned int addr,
+			unsigned int mask, unsigned int wdata)
 {
 	struct s626_private *devpriv = dev->private;
+	unsigned int val;
 
-	/*  Copy target gate array register into P_DEBIAD register. */
-	WR7146(P_DEBICMD, DEBI_CMD_RDWORD | addr);
-	/* Set up DEBI control reg value in shadow RAM. */
-	DEBItransfer(dev);	/*  Execute the DEBI Read transfer. */
-
-	/*  Write back the modified image. */
-	WR7146(P_DEBICMD, DEBI_CMD_WRWORD | addr);
-	/* Set up DEBI control reg value in shadow  RAM. */
+	addr &= 0xffff;
+	writel(DEBI_CMD_RDWORD | addr, devpriv->mmio + P_DEBICMD);
+	DEBItransfer(dev);
 
-	WR7146(P_DEBIAD, wdata | ((uint16_t) RR7146(P_DEBIAD) & mask));
-	/* Modify the register image. */
-	DEBItransfer(dev);	/*  Execute the DEBI Write transfer. */
+	writel(DEBI_CMD_WRWORD | addr, devpriv->mmio + P_DEBICMD);
+	val = readl(devpriv->mmio + P_DEBIAD);
+	val &= mask;
+	val |= wdata;
+	writel(val & 0xffff, devpriv->mmio + P_DEBIAD);
+	DEBItransfer(dev);
 }
 
 /* **************  EEPROM ACCESS FUNCTIONS  ************** */
@@ -308,31 +262,32 @@ static void DEBIreplace(struct comedi_device *dev, uint16_t addr, uint16_t mask,
 static uint32_t I2Chandshake(struct comedi_device *dev, uint32_t val)
 {
 	struct s626_private *devpriv = dev->private;
+	unsigned int ctrl;
 
-	/*  Write I2C command to I2C Transfer Control shadow register. */
-	WR7146(P_I2CCTRL, val);
-
-	/*  Upload I2C shadow registers into working registers and wait for */
-	/*  upload confirmation. */
-
-	MC_ENABLE(P_MC2, MC2_UPLD_IIC);
-	while (!MC_TEST(P_MC2, MC2_UPLD_IIC))
-		;
+	/* Write I2C command to I2C Transfer Control shadow register */
+	writel(val, devpriv->mmio + P_I2CCTRL);
 
-	/*  Wait until I2C bus transfer is finished or an error occurs. */
-	while ((RR7146(P_I2CCTRL) & (I2C_BUSY | I2C_ERR)) == I2C_BUSY)
+	/*
+	 * Upload I2C shadow registers into working registers and
+	 * wait for upload confirmation.
+	 */
+	s626_mc_enable(dev, MC2_UPLD_IIC, P_MC2);
+	while (!s626_mc_test(dev, MC2_UPLD_IIC, P_MC2))
 		;
 
-	/*  Return non-zero if I2C error occurred. */
-	return RR7146(P_I2CCTRL) & I2C_ERR;
+	/* Wait until I2C bus transfer is finished or an error occurs */
+	do {
+		ctrl = readl(devpriv->mmio + P_I2CCTRL);
+	} while ((ctrl & (I2C_BUSY | I2C_ERR)) == I2C_BUSY);
 
+	/* Return non-zero if I2C error occurred */
+	return ctrl & I2C_ERR;
 }
 
 /*  Read uint8_t from EEPROM. */
 static uint8_t I2Cread(struct comedi_device *dev, uint8_t addr)
 {
 	struct s626_private *devpriv = dev->private;
-	uint8_t rtnval;
 
 	/*  Send EEPROM target address. */
 	if (I2Chandshake(dev, I2C_B2(I2C_ATTRSTART, I2CW)
@@ -360,9 +315,8 @@ static uint8_t I2Cread(struct comedi_device *dev, uint8_t addr)
 		/*  Abort function and declare error if handshake failed. */
 		return 0;
 	}
-	/*  Return copy of EEPROM value. */
-	rtnval = (uint8_t) (RR7146(P_I2CCTRL) >> 16);
-	return rtnval;
+
+	return (readl(devpriv->mmio + P_I2CCTRL) >> 16) & 0xff;
 }
 
 /* ***********  DAC FUNCTIONS *********** */
@@ -402,23 +356,25 @@ static void SendDAC(struct comedi_device *dev, uint32_t val)
 
 	/* Copy DAC setpoint value to DAC's output DMA buffer. */
 
-	/* WR7146( (uint32_t)devpriv->pDacWBuf, val ); */
+	/* writel(val, devpriv->mmio + (uint32_t)devpriv->pDacWBuf); */
 	*devpriv->pDacWBuf = val;
 
-	/* enab the output DMA transfer.  This will cause the DMAC to copy
-	 * the DAC's data value to A2's output FIFO.  The DMA transfer will
+	/*
+	 * Enable the output DMA transfer. This will cause the DMAC to copy
+	 * the DAC's data value to A2's output FIFO. The DMA transfer will
 	 * then immediately terminate because the protection address is
 	 * reached upon transfer of the first DWORD value.
 	 */
-	MC_ENABLE(P_MC1, MC1_A2OUT);
+	s626_mc_enable(dev, MC1_A2OUT, P_MC1);
 
 	/*  While the DMA transfer is executing ... */
 
-	/* Reset Audio2 output FIFO's underflow flag (along with any other
-	 * FIFO underflow/overflow flags).  When set, this flag will
-	 * indicate that we have emerged from slot 0.
+	/*
+	 * Reset Audio2 output FIFO's underflow flag (along with any
+	 * other FIFO underflow/overflow flags). When set, this flag
+	 * will indicate that we have emerged from slot 0.
 	 */
-	WR7146(P_ISR, ISR_AFOU);
+	writel(ISR_AFOU, devpriv->mmio + P_ISR);
 
 	/* Wait for the DMA transfer to finish so that there will be data
 	 * available in the FIFO when time slot 1 tries to transfer a DWORD
@@ -426,7 +382,7 @@ static void SendDAC(struct comedi_device *dev, uint32_t val)
 	 * Done by polling the DMAC enable flag; this flag is automatically
 	 * cleared when the transfer has finished.
 	 */
-	while ((RR7146(P_MC1) & MC1_A2OUT) != 0)
+	while (readl(devpriv->mmio + P_MC1) & MC1_A2OUT)
 		;
 
 	/* START THE OUTPUT STREAM TO THE TARGET DAC -------------------- */
@@ -436,7 +392,7 @@ static void SendDAC(struct comedi_device *dev, uint32_t val)
 	 * will be shifted in and stored in FB_BUFFER2 for end-of-slot-list
 	 * detection.
 	 */
-	SETVECT(0, XSD2 | RSD3 | SIB_A2);
+	writel(XSD2 | RSD3 | SIB_A2, devpriv->mmio + VECTPORT(0));
 
 	/* Wait for slot 1 to execute to ensure that the Packet will be
 	 * transmitted.  This is detected by polling the Audio2 output FIFO
@@ -444,7 +400,7 @@ static void SendDAC(struct comedi_device *dev, uint32_t val)
 	 * finished transferring the DAC's data DWORD from the output FIFO
 	 * to the output buffer register.
 	 */
-	while ((RR7146(P_SSR) & SSR_AF2_OUT) == 0)
+	while (!(readl(devpriv->mmio + P_SSR) & SSR_AF2_OUT))
 		;
 
 	/* Set up to trap execution at slot 0 when the TSL sequencer cycles
@@ -453,7 +409,8 @@ static void SendDAC(struct comedi_device *dev, uint32_t val)
 	 * stored in the last byte to be shifted out of the FIFO's DWORD
 	 * buffer register.
 	 */
-	SETVECT(0, XSD2 | XFIFO_2 | RSD2 | SIB_A2 | EOS);
+	writel(XSD2 | XFIFO_2 | RSD2 | SIB_A2 | EOS,
+	       devpriv->mmio + VECTPORT(0));
 
 	/* WAIT FOR THE TRANSACTION TO FINISH ----------------------- */
 
@@ -474,14 +431,14 @@ static void SendDAC(struct comedi_device *dev, uint32_t val)
 	 *    we test for the FB_BUFFER2 MSB contents to be equal to 0xFF.  If
 	 *    the TSL has not yet finished executing slot 5 ...
 	 */
-	if ((RR7146(P_FB_BUFFER2) & 0xFF000000) != 0) {
+	if (readl(devpriv->mmio + P_FB_BUFFER2) & 0xff000000) {
 		/* The trap was set on time and we are still executing somewhere
 		 * in slots 2-5, so we now wait for slot 0 to execute and trap
 		 * TSL execution.  This is detected when FB_BUFFER2 MSB changes
 		 * from 0xFF to 0x00, which slot 0 causes to happen by shifting
 		 * out/in on SD2 the 0x00 that is always referenced by slot 5.
 		 */
-		while ((RR7146(P_FB_BUFFER2) & 0xFF000000) != 0)
+		while (readl(devpriv->mmio + P_FB_BUFFER2) & 0xff000000)
 			;
 	}
 	/* Either (1) we were too late setting the slot 0 trap; the TSL
@@ -492,13 +449,13 @@ static void SendDAC(struct comedi_device *dev, uint32_t val)
 	 * In order to do this, we reprogram slot 0 so that it will shift in
 	 * SD3, which is driven only by a pull-up resistor.
 	 */
-	SETVECT(0, RSD3 | SIB_A2 | EOS);
+	writel(RSD3 | SIB_A2 | EOS, devpriv->mmio + VECTPORT(0));
 
 	/* Wait for slot 0 to execute, at which time the TSL is setup for
 	 * the next DAC write.  This is detected when FB_BUFFER2 MSB changes
 	 * from 0x00 to 0xFF.
 	 */
-	while ((RR7146(P_FB_BUFFER2) & 0xFF000000) == 0)
+	while (!(readl(devpriv->mmio + P_FB_BUFFER2) & 0xff000000))
 		;
 }
 
@@ -532,16 +489,16 @@ static void SetDAC(struct comedi_device *dev, uint16_t chan, short dacdata)
 	 * disables gating for the DAC clock and all DAC chip selects.
 	 */
 
+	/* Choose DAC chip select to be asserted */
 	WSImage = (chan & 2) ? WS1 : WS2;
-	/* Choose DAC chip select to be asserted. */
-	SETVECT(2, XSD2 | XFIFO_1 | WSImage);
-	/* Slot 2: Transmit high data byte to target DAC. */
-	SETVECT(3, XSD2 | XFIFO_0 | WSImage);
-	/* Slot 3: Transmit low data byte to target DAC. */
-	SETVECT(4, XSD2 | XFIFO_3 | WS3);
+	/* Slot 2: Transmit high data byte to target DAC */
+	writel(XSD2 | XFIFO_1 | WSImage, devpriv->mmio + VECTPORT(2));
+	/* Slot 3: Transmit low data byte to target DAC */
+	writel(XSD2 | XFIFO_0 | WSImage, devpriv->mmio + VECTPORT(3));
 	/* Slot 4: Transmit to non-existent TrimDac channel to keep clock */
-	SETVECT(5, XSD2 | XFIFO_2 | WS3 | EOS);
-	/* Slot 5: running after writing target DAC's low data byte. */
+	writel(XSD2 | XFIFO_3 | WS3, devpriv->mmio + VECTPORT(4));
+	/* Slot 5: running after writing target DAC's low data byte */
+	writel(XSD2 | XFIFO_2 | WS3 | EOS, devpriv->mmio + VECTPORT(5));
 
 	/*  Construct and transmit target DAC's serial packet:
 	 * ( A10D DDDD ),( DDDD DDDD ),( 0x0F ),( 0x00 ) where A is chan<0>,
@@ -577,14 +534,14 @@ static void WriteTrimDAC(struct comedi_device *dev, uint8_t LogicalChan,
 	 * can be detected.
 	 */
 
-	SETVECT(2, XSD2 | XFIFO_1 | WS3);
-	/* Slot 2: Send high uint8_t to target TrimDac. */
-	SETVECT(3, XSD2 | XFIFO_0 | WS3);
-	/* Slot 3: Send low uint8_t to target TrimDac. */
-	SETVECT(4, XSD2 | XFIFO_3 | WS1);
-	/* Slot 4: Send NOP high uint8_t to DAC0 to keep clock running. */
-	SETVECT(5, XSD2 | XFIFO_2 | WS1 | EOS);
-	/* Slot 5: Send NOP low  uint8_t to DAC0. */
+	/* Slot 2: Send high uint8_t to target TrimDac */
+	writel(XSD2 | XFIFO_1 | WS3, devpriv->mmio + VECTPORT(2));
+	/* Slot 3: Send low uint8_t to target TrimDac */
+	writel(XSD2 | XFIFO_0 | WS3, devpriv->mmio + VECTPORT(3));
+	/* Slot 4: Send NOP high uint8_t to DAC0 to keep clock running */
+	writel(XSD2 | XFIFO_3 | WS1, devpriv->mmio + VECTPORT(4));
+	/* Slot 5: Send NOP low  uint8_t to DAC0 */
+	writel(XSD2 | XFIFO_2 | WS1 | EOS, devpriv->mmio + VECTPORT(5));
 
 	/* Construct and transmit target DAC's serial packet:
 	 * ( 0000 AAAA ), ( DDDD DDDD ),( 0x00 ),( 0x00 ) where A<3:0> is the
@@ -638,8 +595,8 @@ static void SetLatchSource(struct comedi_device *dev, struct enc_private *k,
 			   uint16_t value)
 {
 	DEBIreplace(dev, k->MyCRB,
-		    (uint16_t) (~(CRBMSK_INTCTRL | CRBMSK_LATCHSRC)),
-		    (uint16_t) (value << CRBBIT_LATCHSRC));
+		    ~(CRBMSK_INTCTRL | CRBMSK_LATCHSRC),
+		    value << CRBBIT_LATCHSRC);
 }
 
 /*  Write value into counter preload register. */
@@ -670,43 +627,24 @@ static unsigned int s626_ai_reg_to_uint(int data)
 
 static int s626_dio_set_irq(struct comedi_device *dev, unsigned int chan)
 {
-	unsigned int group;
-	unsigned int bitmask;
+	unsigned int group = chan / 16;
+	unsigned int mask = 1 << (chan - (16 * group));
 	unsigned int status;
 
-	/* select dio bank */
-	group = chan / 16;
-	bitmask = 1 << (chan - (16 * group));
-
 	/* set channel to capture positive edge */
-	status = DEBIread(dev,
-			  ((struct dio_private *)(dev->subdevices + 2 +
-						  group)->private)->RDEdgSel);
-	DEBIwrite(dev,
-		  ((struct dio_private *)(dev->subdevices + 2 +
-					  group)->private)->WREdgSel,
-		  bitmask | status);
+	status = DEBIread(dev, LP_RDEDGSEL(group));
+	DEBIwrite(dev, LP_WREDGSEL(group), mask | status);
 
 	/* enable interrupt on selected channel */
-	status = DEBIread(dev,
-			  ((struct dio_private *)(dev->subdevices + 2 +
-						  group)->private)->RDIntSel);
-	DEBIwrite(dev,
-		  ((struct dio_private *)(dev->subdevices + 2 +
-					  group)->private)->WRIntSel,
-		  bitmask | status);
+	status = DEBIread(dev, LP_RDINTSEL(group));
+	DEBIwrite(dev, LP_WRINTSEL(group), mask | status);
 
 	/* enable edge capture write command */
 	DEBIwrite(dev, LP_MISC1, MISC1_EDCAP);
 
 	/* enable edge capture on selected channel */
-	status = DEBIread(dev,
-			  ((struct dio_private *)(dev->subdevices + 2 +
-						  group)->private)->RDCapSel);
-	DEBIwrite(dev,
-		  ((struct dio_private *)(dev->subdevices + 2 +
-					  group)->private)->WRCapSel,
-		  bitmask | status);
+	status = DEBIread(dev, LP_RDCAPSEL(group));
+	DEBIwrite(dev, LP_WRCAPSEL(group), mask | status);
 
 	return 0;
 }
@@ -718,9 +656,7 @@ static int s626_dio_reset_irq(struct comedi_device *dev, unsigned int group,
 	DEBIwrite(dev, LP_MISC1, MISC1_NOEDCAP);
 
 	/* enable edge capture on selected channel */
-	DEBIwrite(dev,
-		  ((struct dio_private *)(dev->subdevices + 2 +
-					  group)->private)->WRCapSel, mask);
+	DEBIwrite(dev, LP_WRCAPSEL(group), mask);
 
 	return 0;
 }
@@ -732,244 +668,249 @@ static int s626_dio_clear_irq(struct comedi_device *dev)
 	/* disable edge capture write command */
 	DEBIwrite(dev, LP_MISC1, MISC1_NOEDCAP);
 
-	for (group = 0; group < S626_DIO_BANKS; group++) {
-		/* clear pending events and interrupt */
-		DEBIwrite(dev,
-			  ((struct dio_private *)(dev->subdevices + 2 +
-						  group)->private)->WRCapSel,
-			  0xffff);
-	}
+	/* clear all dio pending events and interrupt */
+	for (group = 0; group < S626_DIO_BANKS; group++)
+		DEBIwrite(dev, LP_WRCAPSEL(group), 0xffff);
 
 	return 0;
 }
 
-static irqreturn_t s626_irq_handler(int irq, void *d)
+static void handle_dio_interrupt(struct comedi_device *dev,
+				 uint16_t irqbit, uint8_t group)
 {
-	struct comedi_device *dev = d;
 	struct s626_private *devpriv = dev->private;
-	struct comedi_subdevice *s;
-	struct comedi_cmd *cmd;
-	struct enc_private *k;
-	unsigned long flags;
-	int32_t *readaddr;
-	uint32_t irqtype, irqstatus;
-	int i = 0;
-	short tempdata;
-	uint8_t group;
-	uint16_t irqbit;
+	struct comedi_subdevice *s = dev->read_subdev;
+	struct comedi_cmd *cmd = &s->async->cmd;
 
-	if (dev->attached == 0)
-		return IRQ_NONE;
-	/*  lock to avoid race with comedi_poll */
-	spin_lock_irqsave(&dev->spinlock, flags);
+	s626_dio_reset_irq(dev, group, irqbit);
 
-	/* save interrupt enable register state */
-	irqstatus = readl(devpriv->base_addr + P_IER);
+	if (devpriv->ai_cmd_running) {
+		/* check if interrupt is an ai acquisition start trigger */
+		if ((irqbit >> (cmd->start_arg - (16 * group))) == 1 &&
+		    cmd->start_src == TRIG_EXT) {
+			/* Start executing the RPS program */
+			s626_mc_enable(dev, MC1_ERPS1, P_MC1);
+
+			if (cmd->scan_begin_src == TRIG_EXT)
+				s626_dio_set_irq(dev, cmd->scan_begin_arg);
+		}
+		if ((irqbit >> (cmd->scan_begin_arg - (16 * group))) == 1 &&
+		    cmd->scan_begin_src == TRIG_EXT) {
+			/* Trigger ADC scan loop start */
+			s626_mc_enable(dev, MC2_ADC_RPS, P_MC2);
 
-	/* read interrupt type */
-	irqtype = readl(devpriv->base_addr + P_ISR);
+			if (cmd->convert_src == TRIG_EXT) {
+				devpriv->ai_convert_count = cmd->chanlist_len;
 
-	/* disable master interrupt */
-	writel(0, devpriv->base_addr + P_IER);
+				s626_dio_set_irq(dev, cmd->convert_arg);
+			}
 
-	/* clear interrupt */
-	writel(irqtype, devpriv->base_addr + P_ISR);
+			if (cmd->convert_src == TRIG_TIMER) {
+				struct enc_private *k = &encpriv[5];
 
-	switch (irqtype) {
-	case IRQ_RPS1:		/*  end_of_scan occurs */
-		/*  manage ai subdevice */
-		s = dev->subdevices;
-		cmd = &(s->async->cmd);
+				devpriv->ai_convert_count = cmd->chanlist_len;
+				k->SetEnable(dev, k, CLKENAB_ALWAYS);
+			}
+		}
+		if ((irqbit >> (cmd->convert_arg - (16 * group))) == 1 &&
+		    cmd->convert_src == TRIG_EXT) {
+			/* Trigger ADC scan loop start */
+			s626_mc_enable(dev, MC2_ADC_RPS, P_MC2);
+
+			devpriv->ai_convert_count--;
+			if (devpriv->ai_convert_count > 0)
+				s626_dio_set_irq(dev, cmd->convert_arg);
+		}
+	}
+}
 
-		/* Init ptr to DMA buffer that holds new ADC data.  We skip the
-		 * first uint16_t in the buffer because it contains junk data from
-		 * the final ADC of the previous poll list scan.
-		 */
-		readaddr = (int32_t *) devpriv->ANABuf.LogicalBase + 1;
-
-		/*  get the data and hand it over to comedi */
-		for (i = 0; i < (s->async->cmd.chanlist_len); i++) {
-			/*  Convert ADC data to 16-bit integer values and copy to application */
-			/*  buffer. */
-			tempdata = s626_ai_reg_to_uint((int)*readaddr);
-			readaddr++;
-
-			/* put data into read buffer */
-			/*  comedi_buf_put(s->async, tempdata); */
-			if (cfc_write_to_buffer(s, tempdata) == 0)
-				printk
-				    ("s626_irq_handler: cfc_write_to_buffer error!\n");
+static void check_dio_interrupts(struct comedi_device *dev)
+{
+	uint16_t irqbit;
+	uint8_t group;
+
+	for (group = 0; group < S626_DIO_BANKS; group++) {
+		irqbit = 0;
+		/* read interrupt type */
+		irqbit = DEBIread(dev, LP_RDCAPFLG(group));
+
+		/* check if interrupt is generated from dio channels */
+		if (irqbit) {
+			handle_dio_interrupt(dev, irqbit, group);
+			return;
 		}
+	}
+}
 
-		/* end of scan occurs */
-		s->async->events |= COMEDI_CB_EOS;
+static void check_counter_interrupts(struct comedi_device *dev)
+{
+	struct s626_private *devpriv = dev->private;
+	struct comedi_subdevice *s = dev->read_subdev;
+	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &async->cmd;
+	struct enc_private *k;
+	uint16_t irqbit;
 
-		if (!(devpriv->ai_continous))
-			devpriv->ai_sample_count--;
-		if (devpriv->ai_sample_count <= 0) {
-			devpriv->ai_cmd_running = 0;
+	/* read interrupt type */
+	irqbit = DEBIread(dev, LP_RDMISC2);
 
-			/*  Stop RPS program. */
-			MC_DISABLE(P_MC1, MC1_ERPS1);
+	/* check interrupt on counters */
+	if (irqbit & IRQ_COINT1A) {
+		k = &encpriv[0];
 
-			/* send end of acquisition */
-			s->async->events |= COMEDI_CB_EOA;
+		/* clear interrupt capture flag */
+		k->ResetCapFlags(dev, k);
+	}
+	if (irqbit & IRQ_COINT2A) {
+		k = &encpriv[1];
 
-			/* disable master interrupt */
-			irqstatus = 0;
-		}
+		/* clear interrupt capture flag */
+		k->ResetCapFlags(dev, k);
+	}
+	if (irqbit & IRQ_COINT3A) {
+		k = &encpriv[2];
 
-		if (devpriv->ai_cmd_running && cmd->scan_begin_src == TRIG_EXT)
-			s626_dio_set_irq(dev, cmd->scan_begin_arg);
-		/*  tell comedi that data is there */
-		comedi_event(dev, s);
-		break;
-	case IRQ_GPIO3:	/* check dio and conter interrupt */
-		/*  manage ai subdevice */
-		s = dev->subdevices;
-		cmd = &(s->async->cmd);
+		/* clear interrupt capture flag */
+		k->ResetCapFlags(dev, k);
+	}
+	if (irqbit & IRQ_COINT1B) {
+		k = &encpriv[3];
 
-		/* s626_dio_clear_irq(dev); */
+		/* clear interrupt capture flag */
+		k->ResetCapFlags(dev, k);
+	}
+	if (irqbit & IRQ_COINT2B) {
+		k = &encpriv[4];
+
+		/* clear interrupt capture flag */
+		k->ResetCapFlags(dev, k);
+
+		if (devpriv->ai_convert_count > 0) {
+			devpriv->ai_convert_count--;
+			if (devpriv->ai_convert_count == 0)
+				k->SetEnable(dev, k, CLKENAB_INDEX);
 
-		for (group = 0; group < S626_DIO_BANKS; group++) {
-			irqbit = 0;
-			/* read interrupt type */
-			irqbit = DEBIread(dev,
-					  ((struct dio_private *)(dev->
-								  subdevices +
-								  2 +
-								  group)->
-					   private)->RDCapFlg);
-
-			/* check if interrupt is generated from dio channels */
-			if (irqbit) {
-				s626_dio_reset_irq(dev, group, irqbit);
-				if (devpriv->ai_cmd_running) {
-					/* check if interrupt is an ai acquisition start trigger */
-					if ((irqbit >> (cmd->start_arg -
-							(16 * group)))
-					    == 1 && cmd->start_src == TRIG_EXT) {
-						/*  Start executing the RPS program. */
-						MC_ENABLE(P_MC1, MC1_ERPS1);
-
-						if (cmd->scan_begin_src ==
-						    TRIG_EXT) {
-							s626_dio_set_irq(dev,
-									 cmd->scan_begin_arg);
-						}
-					}
-					if ((irqbit >> (cmd->scan_begin_arg -
-							(16 * group)))
-					    == 1
-					    && cmd->scan_begin_src ==
-					    TRIG_EXT) {
-						/*  Trigger ADC scan loop start by setting RPS Signal 0. */
-						MC_ENABLE(P_MC2, MC2_ADC_RPS);
-
-						if (cmd->convert_src ==
-						    TRIG_EXT) {
-							devpriv->ai_convert_count
-							    = cmd->chanlist_len;
-
-							s626_dio_set_irq(dev,
-									 cmd->convert_arg);
-						}
-
-						if (cmd->convert_src ==
-						    TRIG_TIMER) {
-							k = &encpriv[5];
-							devpriv->ai_convert_count
-							    = cmd->chanlist_len;
-							k->SetEnable(dev, k,
-								     CLKENAB_ALWAYS);
-						}
-					}
-					if ((irqbit >> (cmd->convert_arg -
-							(16 * group)))
-					    == 1
-					    && cmd->convert_src == TRIG_EXT) {
-						/*  Trigger ADC scan loop start by setting RPS Signal 0. */
-						MC_ENABLE(P_MC2, MC2_ADC_RPS);
-
-						devpriv->ai_convert_count--;
-
-						if (devpriv->ai_convert_count >
-						    0) {
-							s626_dio_set_irq(dev,
-									 cmd->convert_arg);
-						}
-					}
-				}
-				break;
+			if (cmd->convert_src == TRIG_TIMER) {
+				/* Trigger ADC scan loop start */
+				s626_mc_enable(dev, MC2_ADC_RPS, P_MC2);
 			}
 		}
+	}
+	if (irqbit & IRQ_COINT3B) {
+		k = &encpriv[5];
 
-		/* read interrupt type */
-		irqbit = DEBIread(dev, LP_RDMISC2);
-
-		/* check interrupt on counters */
-		if (irqbit & IRQ_COINT1A) {
-			k = &encpriv[0];
+		/* clear interrupt capture flag */
+		k->ResetCapFlags(dev, k);
 
-			/* clear interrupt capture flag */
-			k->ResetCapFlags(dev, k);
+		if (cmd->scan_begin_src == TRIG_TIMER) {
+			/* Trigger ADC scan loop start */
+			s626_mc_enable(dev, MC2_ADC_RPS, P_MC2);
 		}
-		if (irqbit & IRQ_COINT2A) {
-			k = &encpriv[1];
 
-			/* clear interrupt capture flag */
-			k->ResetCapFlags(dev, k);
+		if (cmd->convert_src == TRIG_TIMER) {
+			k = &encpriv[4];
+			devpriv->ai_convert_count = cmd->chanlist_len;
+			k->SetEnable(dev, k, CLKENAB_ALWAYS);
 		}
-		if (irqbit & IRQ_COINT3A) {
-			k = &encpriv[2];
+	}
+}
 
-			/* clear interrupt capture flag */
-			k->ResetCapFlags(dev, k);
-		}
-		if (irqbit & IRQ_COINT1B) {
-			k = &encpriv[3];
+static bool handle_eos_interrupt(struct comedi_device *dev)
+{
+	struct s626_private *devpriv = dev->private;
+	struct comedi_subdevice *s = dev->read_subdev;
+	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &async->cmd;
+	/*
+	 * Init ptr to DMA buffer that holds new ADC data.  We skip the
+	 * first uint16_t in the buffer because it contains junk data
+	 * from the final ADC of the previous poll list scan.
+	 */
+	int32_t *readaddr = (int32_t *)devpriv->ANABuf.LogicalBase + 1;
+	bool finished = false;
+	int i;
 
-			/* clear interrupt capture flag */
-			k->ResetCapFlags(dev, k);
-		}
-		if (irqbit & IRQ_COINT2B) {
-			k = &encpriv[4];
+	/* get the data and hand it over to comedi */
+	for (i = 0; i < cmd->chanlist_len; i++) {
+		short tempdata;
 
-			/* clear interrupt capture flag */
-			k->ResetCapFlags(dev, k);
+		/*
+		 * Convert ADC data to 16-bit integer values and copy
+		 * to application buffer.
+		 */
+		tempdata = s626_ai_reg_to_uint((int)*readaddr);
+		readaddr++;
 
-			if (devpriv->ai_convert_count > 0) {
-				devpriv->ai_convert_count--;
-				if (devpriv->ai_convert_count == 0)
-					k->SetEnable(dev, k, CLKENAB_INDEX);
+		/* put data into read buffer */
+		/* comedi_buf_put(async, tempdata); */
+		cfc_write_to_buffer(s, tempdata);
+	}
 
-				if (cmd->convert_src == TRIG_TIMER) {
-					/*  Trigger ADC scan loop start by setting RPS Signal 0. */
-					MC_ENABLE(P_MC2, MC2_ADC_RPS);
-				}
-			}
-		}
-		if (irqbit & IRQ_COINT3B) {
-			k = &encpriv[5];
+	/* end of scan occurs */
+	async->events |= COMEDI_CB_EOS;
 
-			/* clear interrupt capture flag */
-			k->ResetCapFlags(dev, k);
+	if (!devpriv->ai_continous)
+		devpriv->ai_sample_count--;
+	if (devpriv->ai_sample_count <= 0) {
+		devpriv->ai_cmd_running = 0;
 
-			if (cmd->scan_begin_src == TRIG_TIMER) {
-				/*  Trigger ADC scan loop start by setting RPS Signal 0. */
-				MC_ENABLE(P_MC2, MC2_ADC_RPS);
-			}
+		/* Stop RPS program */
+		s626_mc_disable(dev, MC1_ERPS1, P_MC1);
 
-			if (cmd->convert_src == TRIG_TIMER) {
-				k = &encpriv[4];
-				devpriv->ai_convert_count = cmd->chanlist_len;
-				k->SetEnable(dev, k, CLKENAB_ALWAYS);
-			}
-		}
+		/* send end of acquisition */
+		async->events |= COMEDI_CB_EOA;
+
+		/* disable master interrupt */
+		finished = true;
+	}
+
+	if (devpriv->ai_cmd_running && cmd->scan_begin_src == TRIG_EXT)
+		s626_dio_set_irq(dev, cmd->scan_begin_arg);
+
+	/* tell comedi that data is there */
+	comedi_event(dev, s);
+
+	return finished;
+}
+
+static irqreturn_t s626_irq_handler(int irq, void *d)
+{
+	struct comedi_device *dev = d;
+	struct s626_private *devpriv = dev->private;
+	unsigned long flags;
+	uint32_t irqtype, irqstatus;
+
+	if (!dev->attached)
+		return IRQ_NONE;
+	/*  lock to avoid race with comedi_poll */
+	spin_lock_irqsave(&dev->spinlock, flags);
+
+	/* save interrupt enable register state */
+	irqstatus = readl(devpriv->mmio + P_IER);
+
+	/* read interrupt type */
+	irqtype = readl(devpriv->mmio + P_ISR);
+
+	/* disable master interrupt */
+	writel(0, devpriv->mmio + P_IER);
+
+	/* clear interrupt */
+	writel(irqtype, devpriv->mmio + P_ISR);
+
+	switch (irqtype) {
+	case IRQ_RPS1:		/*  end_of_scan occurs */
+		if (handle_eos_interrupt(dev))
+			irqstatus = 0;
+		break;
+	case IRQ_GPIO3:	/* check dio and conter interrupt */
+		/* s626_dio_clear_irq(dev); */
+		check_dio_interrupts(dev);
+		check_counter_interrupts(dev);
+		break;
 	}
 
 	/* enable interrupt */
-	writel(irqstatus, devpriv->base_addr + P_IER);
+	writel(irqstatus, devpriv->mmio + P_IER);
 
 	spin_unlock_irqrestore(&dev->spinlock, flags);
 	return IRQ_HANDLED;
@@ -988,14 +929,15 @@ static void ResetADC(struct comedi_device *dev, uint8_t *ppl)
 	uint32_t LocalPPL;
 	struct comedi_cmd *cmd = &(dev->subdevices->async->cmd);
 
-	/*  Stop RPS program in case it is currently running. */
-	MC_DISABLE(P_MC1, MC1_ERPS1);
+	/* Stop RPS program in case it is currently running */
+	s626_mc_disable(dev, MC1_ERPS1, P_MC1);
 
 	/*  Set starting logical address to write RPS commands. */
 	pRPS = (uint32_t *) devpriv->RPSBuf.LogicalBase;
 
-	/*  Initialize RPS instruction pointer. */
-	WR7146(P_RPSADDR1, (uint32_t) devpriv->RPSBuf.PhysicalBase);
+	/* Initialize RPS instruction pointer */
+	writel((uint32_t)devpriv->RPSBuf.PhysicalBase,
+	       devpriv->mmio + P_RPSADDR1);
 
 	/*  Construct RPS program in RPSBuf DMA buffer */
 
@@ -1165,41 +1107,42 @@ static void ResetADC(struct comedi_device *dev, uint8_t *ppl)
 	/*  End of RPS program build */
 }
 
-/* TO COMPLETE, IF NECESSARY */
-static int s626_ai_insn_config(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
+#ifdef unused_code
+static int s626_ai_rinsn(struct comedi_device *dev,
+			 struct comedi_subdevice *s,
+			 struct comedi_insn *insn,
+			 unsigned int *data)
 {
+	struct s626_private *devpriv = dev->private;
+	register uint8_t i;
+	register int32_t *readaddr;
 
-	return -EINVAL;
-}
-
-/* static int s626_ai_rinsn(struct comedi_device *dev,struct comedi_subdevice *s,struct comedi_insn *insn,unsigned int *data) */
-/* { */
-/*   struct s626_private *devpriv = dev->private; */
-/*   register uint8_t	i; */
-/*   register int32_t	*readaddr; */
-
-/*   Trigger ADC scan loop start by setting RPS Signal 0. */
-/*   MC_ENABLE( P_MC2, MC2_ADC_RPS ); */
+	/* Trigger ADC scan loop start */
+	s626_mc_enable(dev, MC2_ADC_RPS, P_MC2);
 
-/*   Wait until ADC scan loop is finished (RPS Signal 0 reset). */
-/*   while ( MC_TEST( P_MC2, MC2_ADC_RPS ) ); */
+	/* Wait until ADC scan loop is finished (RPS Signal 0 reset) */
+	while (s626_mc_test(dev, MC2_ADC_RPS, P_MC2))
+		;
 
-/* Init ptr to DMA buffer that holds new ADC data.  We skip the
- * first uint16_t in the buffer because it contains junk data from
- * the final ADC of the previous poll list scan.
- */
-/*   readaddr = (uint32_t *)devpriv->ANABuf.LogicalBase + 1; */
+	/*
+	 * Init ptr to DMA buffer that holds new ADC data.  We skip the
+	 * first uint16_t in the buffer because it contains junk data from
+	 * the final ADC of the previous poll list scan.
+	 */
+	readaddr = (uint32_t *)devpriv->ANABuf.LogicalBase + 1;
 
-/*  Convert ADC data to 16-bit integer values and copy to application buffer. */
-/*   for ( i = 0; i < devpriv->AdcItems; i++ ) { */
-/*     *data = s626_ai_reg_to_uint( *readaddr++ ); */
-/*     data++; */
-/*   } */
+	/*
+	 * Convert ADC data to 16-bit integer values and
+	 * copy to application buffer.
+	 */
+	for (i = 0; i < devpriv->AdcItems; i++) {
+		*data = s626_ai_reg_to_uint(*readaddr++);
+		data++;
+	}
 
-/*   return i; */
-/* } */
+	return i;
+}
+#endif
 
 static int s626_ai_insn_read(struct comedi_device *dev,
 			     struct comedi_subdevice *s,
@@ -1210,14 +1153,9 @@ static int s626_ai_insn_read(struct comedi_device *dev,
 	uint16_t range = CR_RANGE(insn->chanspec);
 	uint16_t AdcSpec = 0;
 	uint32_t GpioImage;
+	int tmp;
 	int n;
 
-	/* interrupt call test  */
-/*   writel(IRQ_GPIO3,devpriv->base_addr+P_PSR); */
-	/* Writing a logical 1 into any of the RPS_PSR bits causes the
-	 * corresponding interrupt to be generated if enabled
-	 */
-
 	/* Convert application's ADC specification into form
 	 *  appropriate for register programming.
 	 */
@@ -1237,27 +1175,29 @@ static int s626_ai_insn_read(struct comedi_device *dev,
 		/*  Delay 10 microseconds for analog input settling. */
 		udelay(10);
 
-		/*  Start ADC by pulsing GPIO1 low. */
-		GpioImage = RR7146(P_GPIO);
-		/*  Assert ADC Start command */
-		WR7146(P_GPIO, GpioImage & ~GPIO1_HI);
-		/*    and stretch it out. */
-		WR7146(P_GPIO, GpioImage & ~GPIO1_HI);
-		WR7146(P_GPIO, GpioImage & ~GPIO1_HI);
-		/*  Negate ADC Start command. */
-		WR7146(P_GPIO, GpioImage | GPIO1_HI);
+		/* Start ADC by pulsing GPIO1 low */
+		GpioImage = readl(devpriv->mmio + P_GPIO);
+		/* Assert ADC Start command */
+		writel(GpioImage & ~GPIO1_HI, devpriv->mmio + P_GPIO);
+		/* and stretch it out */
+		writel(GpioImage & ~GPIO1_HI, devpriv->mmio + P_GPIO);
+		writel(GpioImage & ~GPIO1_HI, devpriv->mmio + P_GPIO);
+		/* Negate ADC Start command */
+		writel(GpioImage | GPIO1_HI, devpriv->mmio + P_GPIO);
 
 		/*  Wait for ADC to complete (GPIO2 is asserted high when */
 		/*  ADC not busy) and for data from previous conversion to */
 		/*  shift into FB BUFFER 1 register. */
 
-		/*  Wait for ADC done. */
-		while (!(RR7146(P_PSR) & PSR_GPIO2))
+		/* Wait for ADC done */
+		while (!(readl(devpriv->mmio + P_PSR) & PSR_GPIO2))
 			;
 
-		/*  Fetch ADC data. */
-		if (n != 0)
-			data[n - 1] = s626_ai_reg_to_uint(RR7146(P_FB_BUFFER1));
+		/* Fetch ADC data */
+		if (n != 0) {
+			tmp = readl(devpriv->mmio + P_FB_BUFFER1);
+			data[n - 1] = s626_ai_reg_to_uint(tmp);
+		}
 
 		/* Allow the ADC to stabilize for 4 microseconds before
 		 * starting the next (final) conversion.  This delay is
@@ -1272,27 +1212,28 @@ static int s626_ai_insn_read(struct comedi_device *dev,
 
 	/* Start a dummy conversion to cause the data from the
 	 * previous conversion to be shifted in. */
-	GpioImage = RR7146(P_GPIO);
-
+	GpioImage = readl(devpriv->mmio + P_GPIO);
 	/* Assert ADC Start command */
-	WR7146(P_GPIO, GpioImage & ~GPIO1_HI);
-	/*    and stretch it out. */
-	WR7146(P_GPIO, GpioImage & ~GPIO1_HI);
-	WR7146(P_GPIO, GpioImage & ~GPIO1_HI);
-	/*  Negate ADC Start command. */
-	WR7146(P_GPIO, GpioImage | GPIO1_HI);
+	writel(GpioImage & ~GPIO1_HI, devpriv->mmio + P_GPIO);
+	/* and stretch it out */
+	writel(GpioImage & ~GPIO1_HI, devpriv->mmio + P_GPIO);
+	writel(GpioImage & ~GPIO1_HI, devpriv->mmio + P_GPIO);
+	/* Negate ADC Start command */
+	writel(GpioImage | GPIO1_HI, devpriv->mmio + P_GPIO);
 
 	/*  Wait for the data to arrive in FB BUFFER 1 register. */
 
-	/*  Wait for ADC done. */
-	while (!(RR7146(P_PSR) & PSR_GPIO2))
+	/* Wait for ADC done */
+	while (!(readl(devpriv->mmio + P_PSR) & PSR_GPIO2))
 		;
 
 	/*  Fetch ADC data from audio interface's input shift register. */
 
-	/*  Fetch ADC data. */
-	if (n != 0)
-		data[n - 1] = s626_ai_reg_to_uint(RR7146(P_FB_BUFFER1));
+	/* Fetch ADC data */
+	if (n != 0) {
+		tmp = readl(devpriv->mmio + P_FB_BUFFER1);
+		data[n - 1] = s626_ai_reg_to_uint(tmp);
+	}
 
 	return n;
 }
@@ -1317,13 +1258,11 @@ static int s626_ai_load_polllist(uint8_t *ppl, struct comedi_cmd *cmd)
 static int s626_ai_inttrig(struct comedi_device *dev,
 			   struct comedi_subdevice *s, unsigned int trignum)
 {
-	struct s626_private *devpriv = dev->private;
-
 	if (trignum != 0)
 		return -EINVAL;
 
-	/*  Start executing the RPS program. */
-	MC_ENABLE(P_MC1, MC1_ERPS1);
+	/* Start executing the RPS program */
+	s626_mc_enable(dev, MC1_ERPS1, P_MC1);
 
 	s->async->inttrig = NULL;
 
@@ -1407,10 +1346,10 @@ static int s626_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		return -EBUSY;
 	}
 	/* disable interrupt */
-	writel(0, devpriv->base_addr + P_IER);
+	writel(0, devpriv->mmio + P_IER);
 
 	/* clear interrupt request */
-	writel(IRQ_RPS1 | IRQ_GPIO3, devpriv->base_addr + P_ISR);
+	writel(IRQ_RPS1 | IRQ_GPIO3, devpriv->mmio + P_ISR);
 
 	/* clear any pending interrupt */
 	s626_dio_clear_irq(dev);
@@ -1483,7 +1422,7 @@ static int s626_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	case TRIG_NONE:
 		/*  continous acquisition */
 		devpriv->ai_continous = 1;
-		devpriv->ai_sample_count = 0;
+		devpriv->ai_sample_count = 1;
 		break;
 	}
 
@@ -1491,11 +1430,11 @@ static int s626_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 	switch (cmd->start_src) {
 	case TRIG_NOW:
-		/*  Trigger ADC scan loop start by setting RPS Signal 0. */
-		/*  MC_ENABLE( P_MC2, MC2_ADC_RPS ); */
+		/* Trigger ADC scan loop start */
+		/* s626_mc_enable(dev, MC2_ADC_RPS, P_MC2); */
 
-		/*  Start executing the RPS program. */
-		MC_ENABLE(P_MC1, MC1_ERPS1);
+		/* Start executing the RPS program */
+		s626_mc_enable(dev, MC1_ERPS1, P_MC1);
 
 		s->async->inttrig = NULL;
 		break;
@@ -1511,7 +1450,7 @@ static int s626_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	}
 
 	/* enable interrupt */
-	writel(IRQ_GPIO3 | IRQ_RPS1, devpriv->base_addr + P_IER);
+	writel(IRQ_GPIO3 | IRQ_RPS1, devpriv->mmio + P_IER);
 
 	return 0;
 }
@@ -1628,11 +1567,11 @@ static int s626_ai_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 {
 	struct s626_private *devpriv = dev->private;
 
-	/*  Stop RPS program in case it is currently running. */
-	MC_DISABLE(P_MC1, MC1_ERPS1);
+	/* Stop RPS program in case it is currently running */
+	s626_mc_disable(dev, MC1_ERPS1, P_MC1);
 
 	/* disable master interrupt */
-	writel(0, devpriv->base_addr + P_IER);
+	writel(0, devpriv->mmio + P_IER);
 
 	devpriv->ai_cmd_running = 0;
 
@@ -1679,84 +1618,74 @@ static int s626_ao_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 static void s626_dio_init(struct comedi_device *dev)
 {
 	uint16_t group;
-	struct comedi_subdevice *s;
 
 	/*  Prepare to treat writes to WRCapSel as capture disables. */
 	DEBIwrite(dev, LP_MISC1, MISC1_NOEDCAP);
 
 	/*  For each group of sixteen channels ... */
 	for (group = 0; group < S626_DIO_BANKS; group++) {
-		s = dev->subdevices + 2 + group;
-		DEBIwrite(dev, diopriv->WRIntSel, 0);	/*  Disable all interrupts. */
-		DEBIwrite(dev, diopriv->WRCapSel, 0xFFFF);	/*  Disable all event */
-		/*  captures. */
-		DEBIwrite(dev, diopriv->WREdgSel, 0);	/*  Init all DIOs to */
-		/*  default edge */
-		/*  polarity. */
-		DEBIwrite(dev, diopriv->WRDOut, 0);	/*  Program all outputs */
-		/*  to inactive state. */
+		/* Disable all interrupts */
+		DEBIwrite(dev, LP_WRINTSEL(group), 0);
+		/* Disable all event captures */
+		DEBIwrite(dev, LP_WRCAPSEL(group), 0xffff);
+		/* Init all DIOs to default edge polarity */
+		DEBIwrite(dev, LP_WREDGSEL(group), 0);
+		/* Program all outputs to inactive state */
+		DEBIwrite(dev, LP_WRDOUT(group), 0);
 	}
 }
 
-/* DIO devices are slightly special.  Although it is possible to
- * implement the insn_read/insn_write interface, it is much more
- * useful to applications if you implement the insn_bits interface.
- * This allows packed reading/writing of the DIO channels.  The comedi
- * core can convert between insn_bits and insn_read/write */
-
 static int s626_dio_insn_bits(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
-			      struct comedi_insn *insn, unsigned int *data)
+			      struct comedi_insn *insn,
+			      unsigned int *data)
 {
-	/*
-	 * The insn data consists of a mask in data[0] and the new data in
-	 * data[1]. The mask defines which bits we are concerning about.
-	 * The new data must be anded with the mask.  Each channel
-	 * corresponds to a bit.
-	 */
-	if (data[0]) {
-		/* Check if requested ports are configured for output */
-		if ((s->io_bits & data[0]) != data[0])
-			return -EIO;
+	unsigned long group = (unsigned long)s->private;
+	unsigned long mask = data[0];
+	unsigned long bits = data[1];
 
-		s->state &= ~data[0];
-		s->state |= data[0] & data[1];
+	if (mask) {
+		/* Check if requested channels are configured for output */
+		if ((s->io_bits & mask) != mask)
+			return -EIO;
 
-		/* Write out the new digital output lines */
+		s->state &= ~mask;
+		s->state |= (bits & mask);
 
-		DEBIwrite(dev, diopriv->WRDOut, s->state);
+		DEBIwrite(dev, LP_WRDOUT(group), s->state);
 	}
-	data[1] = DEBIread(dev, diopriv->RDDIn);
+	data[1] = DEBIread(dev, LP_RDDIN(group));
 
 	return insn->n;
 }
 
 static int s626_dio_insn_config(struct comedi_device *dev,
 				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data)
+				struct comedi_insn *insn,
+				unsigned int *data)
 {
+	unsigned long group = (unsigned long)s->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int mask = 1 << chan;
 
 	switch (data[0]) {
 	case INSN_CONFIG_DIO_QUERY:
-		data[1] =
-		    (s->
-		     io_bits & (1 << CR_CHAN(insn->chanspec))) ? COMEDI_OUTPUT :
-		    COMEDI_INPUT;
+		data[1] = (s->io_bits & mask) ? COMEDI_OUTPUT : COMEDI_INPUT;
 		return insn->n;
 		break;
 	case COMEDI_INPUT:
-		s->io_bits &= ~(1 << CR_CHAN(insn->chanspec));
+		s->io_bits &= ~mask;
 		break;
 	case COMEDI_OUTPUT:
-		s->io_bits |= 1 << CR_CHAN(insn->chanspec);
+		s->io_bits |= mask;
 		break;
 	default:
 		return -EINVAL;
 		break;
 	}
-	DEBIwrite(dev, diopriv->WRDOut, s->io_bits);
+	DEBIwrite(dev, LP_WRDOUT(group), s->io_bits);
 
-	return 1;
+	return insn->n;
 }
 
 /* Now this function initializes the value of the counter (data[0])
@@ -1861,13 +1790,13 @@ static void CloseDMAB(struct comedi_device *dev, struct bufferDMA *pdma,
 
 static void ResetCapFlags_A(struct comedi_device *dev, struct enc_private *k)
 {
-	DEBIreplace(dev, k->MyCRB, (uint16_t) (~CRBMSK_INTCTRL),
+	DEBIreplace(dev, k->MyCRB, ~CRBMSK_INTCTRL,
 		    CRBMSK_INTRESETCMD | CRBMSK_INTRESET_A);
 }
 
 static void ResetCapFlags_B(struct comedi_device *dev, struct enc_private *k)
 {
-	DEBIreplace(dev, k->MyCRB, (uint16_t) (~CRBMSK_INTCTRL),
+	DEBIreplace(dev, k->MyCRB, ~CRBMSK_INTCTRL,
 		    CRBMSK_INTRESETCMD | CRBMSK_INTRESET_B);
 }
 
@@ -2012,8 +1941,7 @@ static void SetMode_A(struct comedi_device *dev, struct enc_private *k,
 	/*  While retaining CounterB and LatchSrc configurations, program the */
 	/*  new counter operating mode. */
 	DEBIreplace(dev, k->MyCRA, CRAMSK_INDXSRC_B | CRAMSK_CLKSRC_B, cra);
-	DEBIreplace(dev, k->MyCRB,
-		    (uint16_t) (~(CRBMSK_INTCTRL | CRBMSK_CLKENAB_A)), crb);
+	DEBIreplace(dev, k->MyCRB, ~(CRBMSK_INTCTRL | CRBMSK_CLKENAB_A), crb);
 }
 
 static void SetMode_B(struct comedi_device *dev, struct enc_private *k,
@@ -2074,8 +2002,7 @@ static void SetMode_B(struct comedi_device *dev, struct enc_private *k,
 
 	/*  While retaining CounterA and LatchSrc configurations, program the */
 	/*  new counter operating mode. */
-	DEBIreplace(dev, k->MyCRA,
-		    (uint16_t) (~(CRAMSK_INDXSRC_B | CRAMSK_CLKSRC_B)), cra);
+	DEBIreplace(dev, k->MyCRA, ~(CRAMSK_INDXSRC_B | CRAMSK_CLKSRC_B), cra);
 	DEBIreplace(dev, k->MyCRB, CRBMSK_CLKENAB_A | CRBMSK_LATCHSRC, crb);
 }
 
@@ -2084,17 +2011,15 @@ static void SetMode_B(struct comedi_device *dev, struct enc_private *k,
 static void SetEnable_A(struct comedi_device *dev, struct enc_private *k,
 			uint16_t enab)
 {
-	DEBIreplace(dev, k->MyCRB,
-		    (uint16_t) (~(CRBMSK_INTCTRL | CRBMSK_CLKENAB_A)),
-		    (uint16_t) (enab << CRBBIT_CLKENAB_A));
+	DEBIreplace(dev, k->MyCRB, ~(CRBMSK_INTCTRL | CRBMSK_CLKENAB_A),
+		    enab << CRBBIT_CLKENAB_A);
 }
 
 static void SetEnable_B(struct comedi_device *dev, struct enc_private *k,
 			uint16_t enab)
 {
-	DEBIreplace(dev, k->MyCRB,
-		    (uint16_t) (~(CRBMSK_INTCTRL | CRBMSK_CLKENAB_B)),
-		    (uint16_t) (enab << CRBBIT_CLKENAB_B));
+	DEBIreplace(dev, k->MyCRB, ~(CRBMSK_INTCTRL | CRBMSK_CLKENAB_B),
+		    enab << CRBBIT_CLKENAB_B);
 }
 
 static uint16_t GetEnable_A(struct comedi_device *dev, struct enc_private *k)
@@ -2123,16 +2048,15 @@ static uint16_t GetEnable_B(struct comedi_device *dev, struct enc_private *k)
 static void SetLoadTrig_A(struct comedi_device *dev, struct enc_private *k,
 			  uint16_t Trig)
 {
-	DEBIreplace(dev, k->MyCRA, (uint16_t) (~CRAMSK_LOADSRC_A),
-		    (uint16_t) (Trig << CRABIT_LOADSRC_A));
+	DEBIreplace(dev, k->MyCRA, ~CRAMSK_LOADSRC_A,
+		    Trig << CRABIT_LOADSRC_A);
 }
 
 static void SetLoadTrig_B(struct comedi_device *dev, struct enc_private *k,
 			  uint16_t Trig)
 {
-	DEBIreplace(dev, k->MyCRB,
-		    (uint16_t) (~(CRBMSK_LOADSRC_B | CRBMSK_INTCTRL)),
-		    (uint16_t) (Trig << CRBBIT_LOADSRC_B));
+	DEBIreplace(dev, k->MyCRB, ~(CRBMSK_LOADSRC_B | CRBMSK_INTCTRL),
+		    Trig << CRBBIT_LOADSRC_B);
 }
 
 static uint16_t GetLoadTrig_A(struct comedi_device *dev, struct enc_private *k)
@@ -2156,12 +2080,12 @@ static void SetIntSrc_A(struct comedi_device *dev, struct enc_private *k,
 	struct s626_private *devpriv = dev->private;
 
 	/*  Reset any pending counter overflow or index captures. */
-	DEBIreplace(dev, k->MyCRB, (uint16_t) (~CRBMSK_INTCTRL),
+	DEBIreplace(dev, k->MyCRB, ~CRBMSK_INTCTRL,
 		    CRBMSK_INTRESETCMD | CRBMSK_INTRESET_A);
 
 	/*  Program counter interrupt source. */
 	DEBIreplace(dev, k->MyCRA, ~CRAMSK_INTSRC_A,
-		    (uint16_t) (IntSource << CRABIT_INTSRC_A));
+		    IntSource << CRABIT_INTSRC_A);
 
 	/*  Update MISC2 interrupt enable mask. */
 	devpriv->CounterIntEnabs =
@@ -2430,7 +2354,7 @@ static void s626_initialize(struct comedi_device *dev)
 	int i;
 
 	/* Enable DEBI and audio pins, enable I2C interface */
-	MC_ENABLE(P_MC1, MC1_DEBI | MC1_AUDIO | MC1_I2C);
+	s626_mc_enable(dev, MC1_DEBI | MC1_AUDIO | MC1_I2C, P_MC1);
 
 	/*
 	 *  Configure DEBI operating mode
@@ -2440,15 +2364,16 @@ static void s626_initialize(struct comedi_device *dev)
 	 *   Set up byte lane steering
 	 *   Intel-compatible local bus (DEBI never times out)
 	 */
-	WR7146(P_DEBICFG, DEBI_CFG_SLAVE16 |
-			  (DEBI_TOUT << DEBI_CFG_TOUT_BIT) |
-			  DEBI_SWAP | DEBI_CFG_INTEL);
+	writel(DEBI_CFG_SLAVE16 |
+	       (DEBI_TOUT << DEBI_CFG_TOUT_BIT) |
+	       DEBI_SWAP | DEBI_CFG_INTEL,
+	       devpriv->mmio + P_DEBICFG);
 
 	/* Disable MMU paging */
-	WR7146(P_DEBIPAGE, DEBI_PAGE_DISABLE);
+	writel(DEBI_PAGE_DISABLE, devpriv->mmio + P_DEBIPAGE);
 
 	/* Init GPIO so that ADC Start* is negated */
-	WR7146(P_GPIO, GPIO_BASE | GPIO1_HI);
+	writel(GPIO_BASE | GPIO1_HI, devpriv->mmio + P_GPIO);
 
 	/* I2C device address for onboard eeprom (revb) */
 	devpriv->I2CAdrs = 0xA0;
@@ -2457,9 +2382,9 @@ static void s626_initialize(struct comedi_device *dev)
 	 * Issue an I2C ABORT command to halt any I2C
 	 * operation in progress and reset BUSY flag.
 	 */
-	WR7146(P_I2CSTAT, I2C_CLKSEL | I2C_ABORT);
-	MC_ENABLE(P_MC2, MC2_UPLD_IIC);
-	while ((RR7146(P_MC2) & MC2_UPLD_IIC) == 0)
+	writel(I2C_CLKSEL | I2C_ABORT, devpriv->mmio + P_I2CSTAT);
+	s626_mc_enable(dev, MC2_UPLD_IIC, P_MC2);
+	while (!(readl(devpriv->mmio + P_MC2) & MC2_UPLD_IIC))
 		;
 
 	/*
@@ -2467,9 +2392,9 @@ static void s626_initialize(struct comedi_device *dev)
 	 * reg twice to reset all  I2C error flags.
 	 */
 	for (i = 0; i < 2; i++) {
-		WR7146(P_I2CSTAT, I2C_CLKSEL);
-		MC_ENABLE(P_MC2, MC2_UPLD_IIC);
-		while (!MC_TEST(P_MC2, MC2_UPLD_IIC))
+		writel(I2C_CLKSEL, devpriv->mmio + P_I2CSTAT);
+		s626_mc_enable(dev, MC2_UPLD_IIC, P_MC2);
+		while (!s626_mc_test(dev, MC2_UPLD_IIC, P_MC2))
 			;
 	}
 
@@ -2479,7 +2404,7 @@ static void s626_initialize(struct comedi_device *dev)
 	 * DAC data setup times are satisfied, enable DAC serial
 	 * clock out.
 	 */
-	WR7146(P_ACON2, ACON2_INIT);
+	writel(ACON2_INIT, devpriv->mmio + P_ACON2);
 
 	/*
 	 * Set up TSL1 slot list, which is used to control the
@@ -2487,22 +2412,23 @@ static void s626_initialize(struct comedi_device *dev)
 	 * SIB_A1  = store data uint8_t at next available location
 	 * in FB BUFFER1 register.
 	 */
-	WR7146(P_TSL1, RSD1 | SIB_A1);
-	WR7146(P_TSL1 + 4, RSD1 | SIB_A1 | EOS);
+	writel(RSD1 | SIB_A1, devpriv->mmio + P_TSL1);
+	writel(RSD1 | SIB_A1 | EOS, devpriv->mmio + P_TSL1 + 4);
 
 	/* Enable TSL1 slot list so that it executes all the time */
-	WR7146(P_ACON1, ACON1_ADCSTART);
+	writel(ACON1_ADCSTART, devpriv->mmio + P_ACON1);
 
 	/*
 	 * Initialize RPS registers used for ADC
 	 */
 
 	/* Physical start of RPS program */
-	WR7146(P_RPSADDR1, (uint32_t)devpriv->RPSBuf.PhysicalBase);
+	writel((uint32_t)devpriv->RPSBuf.PhysicalBase,
+	       devpriv->mmio + P_RPSADDR1);
 	/* RPS program performs no explicit mem writes */
-	WR7146(P_RPSPAGE1, 0);
+	writel(0, devpriv->mmio + P_RPSPAGE1);
 	/* Disable RPS timeouts */
-	WR7146(P_RPS1_TOUT, 0);
+	writel(0, devpriv->mmio + P_RPS1_TOUT);
 
 #if 0
 	/*
@@ -2558,7 +2484,7 @@ static void s626_initialize(struct comedi_device *dev)
 	 *   burst length = 1 DWORD
 	 *   threshold = 1 DWORD.
 	 */
-	WR7146(P_PCI_BT_A, 0);
+	writel(0, devpriv->mmio + P_PCI_BT_A);
 
 	/*
 	 * Init Audio2's output DMA physical addresses.  The protection
@@ -2568,8 +2494,9 @@ static void s626_initialize(struct comedi_device *dev)
 	 */
 	pPhysBuf = devpriv->ANABuf.PhysicalBase +
 		   (DAC_WDMABUF_OS * sizeof(uint32_t));
-	WR7146(P_BASEA2_OUT, (uint32_t) pPhysBuf);
-	WR7146(P_PROTA2_OUT, (uint32_t) (pPhysBuf + sizeof(uint32_t)));
+	writel((uint32_t)pPhysBuf, devpriv->mmio + P_BASEA2_OUT);
+	writel((uint32_t)(pPhysBuf + sizeof(uint32_t)),
+	       devpriv->mmio + P_PROTA2_OUT);
 
 	/*
 	 * Cache Audio2's output DMA buffer logical address.  This is
@@ -2584,7 +2511,7 @@ static void s626_initialize(struct comedi_device *dev)
 	 * DMAC will automatically halt and its PCI address pointer
 	 * will be reset when the protection address is reached.
 	 */
-	WR7146(P_PAGEA2_OUT, 8);
+	writel(8, devpriv->mmio + P_PAGEA2_OUT);
 
 	/*
 	 * Initialize time slot list 2 (TSL2), which is used to control
@@ -2599,7 +2526,7 @@ static void s626_initialize(struct comedi_device *dev)
 	 */
 
 	/* Slot 0: Trap TSL execution, shift 0xFF into FB_BUFFER2 */
-	SETVECT(0, XSD2 | RSD3 | SIB_A2 | EOS);
+	writel(XSD2 | RSD3 | SIB_A2 | EOS, devpriv->mmio + VECTPORT(0));
 
 	/*
 	 * Initialize slot 1, which is constant.  Slot 1 causes a
@@ -2611,10 +2538,10 @@ static void s626_initialize(struct comedi_device *dev)
 	 */
 
 	/* Slot 1: Fetch DWORD from Audio2's output FIFO */
-	SETVECT(1, LF_A2);
+	writel(LF_A2, devpriv->mmio + VECTPORT(1));
 
 	/* Start DAC's audio interface (TSL2) running */
-	WR7146(P_ACON1, ACON1_DACSTART);
+	writel(ACON1_DACSTART, devpriv->mmio + P_ACON1);
 
 	/*
 	 * Init Trim DACs to calibrated values.  Do it twice because the
@@ -2653,9 +2580,6 @@ static void s626_initialize(struct comedi_device *dev)
 
 	/* Initialize the digital I/O subsystem */
 	s626_dio_init(dev);
-
-	/* enable interrupt test */
-	/* writel(IRQ_GPIO3 | IRQ_RPS1, devpriv->base_addr + P_IER); */
 }
 
 static int s626_auto_attach(struct comedi_device *dev,
@@ -2666,28 +2590,24 @@ static int s626_auto_attach(struct comedi_device *dev,
 	struct comedi_subdevice *s;
 	int ret;
 
-	dev->board_name = dev->driver->driver_name;
-
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
-	dev->iobase = 1;	/* detach needs this */
 
-	devpriv->base_addr = ioremap(pci_resource_start(pcidev, 0),
-				     pci_resource_len(pcidev, 0));
-	if (!devpriv->base_addr)
+	devpriv->mmio = pci_ioremap_bar(pcidev, 0);
+	if (!devpriv->mmio)
 		return -ENOMEM;
 
 	/* disable master interrupt */
-	writel(0, devpriv->base_addr + P_IER);
+	writel(0, devpriv->mmio + P_IER);
 
 	/* soft reset */
-	writel(MC1_SOFT_RESET, devpriv->base_addr + P_MC1);
+	writel(MC1_SOFT_RESET, devpriv->mmio + P_MC1);
 
 	/* DMA FIXME DMA// */
 
@@ -2707,79 +2627,79 @@ static int s626_auto_attach(struct comedi_device *dev,
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* analog input subdevice */
-	dev->read_subdev = s;
-	/* we support single-ended (ground) and differential */
-	s->type = COMEDI_SUBD_AI;
-	s->subdev_flags = SDF_READABLE | SDF_DIFF | SDF_CMD_READ;
-	s->n_chan = S626_ADC_CHANNELS;
-	s->maxdata = (0xffff >> 2);
-	s->range_table = &s626_range_table;
-	s->len_chanlist = S626_ADC_CHANNELS;
-	s->insn_config = s626_ai_insn_config;
-	s->insn_read = s626_ai_insn_read;
-	s->do_cmd = s626_ai_cmd;
-	s->do_cmdtest = s626_ai_cmdtest;
-	s->cancel = s626_ai_cancel;
-
-	s = dev->subdevices + 1;
+	s->type		= COMEDI_SUBD_AI;
+	s->subdev_flags	= SDF_READABLE | SDF_DIFF | SDF_CMD_READ;
+	s->n_chan	= S626_ADC_CHANNELS;
+	s->maxdata	= 0x3fff;
+	s->range_table	= &s626_range_table;
+	s->len_chanlist	= S626_ADC_CHANNELS;
+	s->insn_read	= s626_ai_insn_read;
+	if (dev->irq) {
+		dev->read_subdev = s;
+		s->do_cmd	= s626_ai_cmd;
+		s->do_cmdtest	= s626_ai_cmdtest;
+		s->cancel	= s626_ai_cancel;
+	}
+
+	s = &dev->subdevices[1];
 	/* analog output subdevice */
-	s->type = COMEDI_SUBD_AO;
-	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
-	s->n_chan = S626_DAC_CHANNELS;
-	s->maxdata = (0x3fff);
-	s->range_table = &range_bipolar10;
-	s->insn_write = s626_ao_winsn;
-	s->insn_read = s626_ao_rinsn;
-
-	s = dev->subdevices + 2;
+	s->type		= COMEDI_SUBD_AO;
+	s->subdev_flags	= SDF_WRITABLE | SDF_READABLE;
+	s->n_chan	= S626_DAC_CHANNELS;
+	s->maxdata	= 0x3fff;
+	s->range_table	= &range_bipolar10;
+	s->insn_write	= s626_ao_winsn;
+	s->insn_read	= s626_ao_rinsn;
+
+	s = &dev->subdevices[2];
 	/* digital I/O subdevice */
-	s->type = COMEDI_SUBD_DIO;
-	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
-	s->n_chan = 16;
-	s->maxdata = 1;
-	s->io_bits = 0xffff;
-	s->private = &dio_private_A;
-	s->range_table = &range_digital;
-	s->insn_config = s626_dio_insn_config;
-	s->insn_bits = s626_dio_insn_bits;
-
-	s = dev->subdevices + 3;
+	s->type		= COMEDI_SUBD_DIO;
+	s->subdev_flags	= SDF_WRITABLE | SDF_READABLE;
+	s->n_chan	= 16;
+	s->maxdata	= 1;
+	s->io_bits	= 0xffff;
+	s->private	= (void *)0;	/* DIO group 0 */
+	s->range_table	= &range_digital;
+	s->insn_config	= s626_dio_insn_config;
+	s->insn_bits	= s626_dio_insn_bits;
+
+	s = &dev->subdevices[3];
 	/* digital I/O subdevice */
-	s->type = COMEDI_SUBD_DIO;
-	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
-	s->n_chan = 16;
-	s->maxdata = 1;
-	s->io_bits = 0xffff;
-	s->private = &dio_private_B;
-	s->range_table = &range_digital;
-	s->insn_config = s626_dio_insn_config;
-	s->insn_bits = s626_dio_insn_bits;
-
-	s = dev->subdevices + 4;
+	s->type		= COMEDI_SUBD_DIO;
+	s->subdev_flags	= SDF_WRITABLE | SDF_READABLE;
+	s->n_chan	= 16;
+	s->maxdata	= 1;
+	s->io_bits	= 0xffff;
+	s->private	= (void *)1;	/* DIO group 1 */
+	s->range_table	= &range_digital;
+	s->insn_config	= s626_dio_insn_config;
+	s->insn_bits	= s626_dio_insn_bits;
+
+	s = &dev->subdevices[4];
 	/* digital I/O subdevice */
-	s->type = COMEDI_SUBD_DIO;
-	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
-	s->n_chan = 16;
-	s->maxdata = 1;
-	s->io_bits = 0xffff;
-	s->private = &dio_private_C;
-	s->range_table = &range_digital;
-	s->insn_config = s626_dio_insn_config;
-	s->insn_bits = s626_dio_insn_bits;
-
-	s = dev->subdevices + 5;
+	s->type		= COMEDI_SUBD_DIO;
+	s->subdev_flags	= SDF_WRITABLE | SDF_READABLE;
+	s->n_chan	= 16;
+	s->maxdata	= 1;
+	s->io_bits	= 0xffff;
+	s->private	= (void *)2;	/* DIO group 2 */
+	s->range_table	= &range_digital;
+	s->insn_config 	= s626_dio_insn_config;
+	s->insn_bits	= s626_dio_insn_bits;
+
+	s = &dev->subdevices[5];
 	/* encoder (counter) subdevice */
-	s->type = COMEDI_SUBD_COUNTER;
-	s->subdev_flags = SDF_WRITABLE | SDF_READABLE | SDF_LSAMPL;
-	s->n_chan = S626_ENCODER_CHANNELS;
-	s->private = enc_private_data;
-	s->insn_config = s626_enc_insn_config;
-	s->insn_read = s626_enc_insn_read;
-	s->insn_write = s626_enc_insn_write;
-	s->maxdata = 0xffffff;
-	s->range_table = &range_unknown;
+	s->type		= COMEDI_SUBD_COUNTER;
+	s->subdev_flags	= SDF_WRITABLE | SDF_READABLE | SDF_LSAMPL;
+	s->n_chan	= S626_ENCODER_CHANNELS;
+	s->maxdata	= 0xffffff;
+	s->private	= enc_private_data;
+	s->range_table	= &range_unknown;
+	s->insn_config	= s626_enc_insn_config;
+	s->insn_read	= s626_enc_insn_read;
+	s->insn_write	= s626_enc_insn_write;
 
 	s626_initialize(dev);
 
@@ -2790,24 +2710,26 @@ static int s626_auto_attach(struct comedi_device *dev,
 
 static void s626_detach(struct comedi_device *dev)
 {
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct s626_private *devpriv = dev->private;
 
 	if (devpriv) {
 		/* stop ai_command */
 		devpriv->ai_cmd_running = 0;
 
-		if (devpriv->base_addr) {
+		if (devpriv->mmio) {
 			/* interrupt mask */
-			WR7146(P_IER, 0);	/*  Disable master interrupt. */
-			WR7146(P_ISR, IRQ_GPIO3 | IRQ_RPS1);	/*  Clear board's IRQ status flag. */
+			/* Disable master interrupt */
+			writel(0, devpriv->mmio + P_IER);
+			/* Clear board's IRQ status flag */
+			writel(IRQ_GPIO3 | IRQ_RPS1,
+			       devpriv->mmio + P_ISR);
 
 			/*  Disable the watchdog timer and battery charger. */
 			WriteMISC2(dev, 0);
 
-			/*  Close all interfaces on 7146 device. */
-			WR7146(P_MC1, MC1_SHUTDOWN);
-			WR7146(P_ACON1, ACON1_BASE);
+			/* Close all interfaces on 7146 device */
+			writel(MC1_SHUTDOWN, devpriv->mmio + P_MC1);
+			writel(ACON1_BASE, devpriv->mmio + P_ACON1);
 
 			CloseDMAB(dev, &devpriv->RPSBuf, DMABUF_SIZE);
 			CloseDMAB(dev, &devpriv->ANABuf, DMABUF_SIZE);
@@ -2815,13 +2737,10 @@ static void s626_detach(struct comedi_device *dev)
 
 		if (dev->irq)
 			free_irq(dev->irq, dev);
-		if (devpriv->base_addr)
-			iounmap(devpriv->base_addr);
-	}
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
+		if (devpriv->mmio)
+			iounmap(devpriv->mmio);
 	}
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver s626_driver = {
@@ -2832,9 +2751,9 @@ static struct comedi_driver s626_driver = {
 };
 
 static int s626_pci_probe(struct pci_dev *dev,
-				    const struct pci_device_id *ent)
+			  const struct pci_device_id *id)
 {
-	return comedi_pci_auto_config(dev, &s626_driver);
+	return comedi_pci_auto_config(dev, &s626_driver, id->driver_data);
 }
 
 /*
diff --git a/drivers/staging/comedi/drivers/s626.h b/drivers/staging/comedi/drivers/s626.h
index ff4b3a5e4e5..99cd57b092e 100644
--- a/drivers/staging/comedi/drivers/s626.h
+++ b/drivers/staging/comedi/drivers/s626.h
@@ -267,36 +267,17 @@
 #define LP_DACPOL		0x0082	/*   Write DAC polarity. */
 #define LP_GSEL			0x0084	/*   Write ADC gain. */
 #define LP_ISEL			0x0086	/*   Write ADC channel select. */
-/*  Digital I/O (write only): */
-#define LP_WRINTSELA		0x0042	/*   Write A interrupt enable. */
-#define LP_WREDGSELA		0x0044	/*   Write A edge selection. */
-#define LP_WRCAPSELA		0x0046	/*   Write A capture enable. */
-#define LP_WRDOUTA		0x0048	/*   Write A digital output. */
-#define LP_WRINTSELB		0x0052	/*   Write B interrupt enable. */
-#define LP_WREDGSELB		0x0054	/*   Write B edge selection. */
-#define LP_WRCAPSELB		0x0056	/*   Write B capture enable. */
-#define LP_WRDOUTB		0x0058	/*   Write B digital output. */
-#define LP_WRINTSELC		0x0062	/*   Write C interrupt enable. */
-#define LP_WREDGSELC		0x0064	/*   Write C edge selection. */
-#define LP_WRCAPSELC		0x0066	/*   Write C capture enable. */
-#define LP_WRDOUTC		0x0068	/*   Write C digital output. */
-
-/*  Digital I/O (read only): */
-#define LP_RDDINA		0x0040	/*   Read digital input. */
-#define LP_RDCAPFLGA		0x0048	/*   Read edges captured. */
-#define LP_RDINTSELA		0x004A	/*   Read interrupt enable register. */
-#define LP_RDEDGSELA		0x004C	/*   Read edge selection register. */
-#define LP_RDCAPSELA		0x004E	/*   Read capture enable register. */
-#define LP_RDDINB		0x0050	/*   Read digital input. */
-#define LP_RDCAPFLGB		0x0058	/*   Read edges captured. */
-#define LP_RDINTSELB		0x005A	/*   Read interrupt enable register. */
-#define LP_RDEDGSELB		0x005C	/*   Read edge selection register. */
-#define LP_RDCAPSELB		0x005E	/*   Read capture enable register. */
-#define LP_RDDINC		0x0060	/*   Read digital input. */
-#define LP_RDCAPFLGC		0x0068	/*   Read edges captured. */
-#define LP_RDINTSELC		0x006A	/*   Read interrupt enable register. */
-#define LP_RDEDGSELC		0x006C	/*   Read edge selection register. */
-#define LP_RDCAPSELC		0x006E	/*   Read capture enable register. */
+
+/* Digital I/O registers */
+#define LP_RDDIN(x)		(0x0040 + (x) * 0x10)	/* R: digital input */
+#define LP_WRINTSEL(x)		(0x0042 + (x) * 0x10)	/* W: int enable */
+#define LP_WREDGSEL(x)		(0x0044 + (x) * 0x10)	/* W: edge selection */
+#define LP_WRCAPSEL(x)		(0x0046 + (x) * 0x10)	/* W: capture enable */
+#define LP_RDCAPFLG(x)		(0x0048 + (x) * 0x10)	/* R: edges captured */
+#define LP_WRDOUT(x)		(0x0048 + (x) * 0x10)	/* W: digital output */
+#define LP_RDINTSEL(x)		(0x004a + (x) * 0x10)	/* R: int enable */
+#define LP_RDEDGSEL(x)		(0x004c + (x) * 0x10)	/* R: edge selection */
+#define LP_RDCAPSEL(x)		(0x004e + (x) * 0x10)	/* R: capture enable */
 
 /*  Counter Registers (read/write): */
 #define LP_CR0A			0x0000	/*   0A setup register. */
@@ -661,27 +642,27 @@
 
 /*  Bit field masks for CRA and CRB. */
 
-#define CRAMSK_INDXSRC_B	((uint16_t)(3 << CRABIT_INDXSRC_B))
-#define CRAMSK_CLKSRC_B		((uint16_t)(3 << CRABIT_CLKSRC_B))
-#define CRAMSK_INDXPOL_A	((uint16_t)(1 << CRABIT_INDXPOL_A))
-#define CRAMSK_LOADSRC_A	((uint16_t)(3 << CRABIT_LOADSRC_A))
-#define CRAMSK_CLKMULT_A	((uint16_t)(3 << CRABIT_CLKMULT_A))
-#define CRAMSK_INTSRC_A		((uint16_t)(3 << CRABIT_INTSRC_A))
-#define CRAMSK_CLKPOL_A		((uint16_t)(3 << CRABIT_CLKPOL_A))
-#define CRAMSK_INDXSRC_A	((uint16_t)(3 << CRABIT_INDXSRC_A))
-#define CRAMSK_CLKSRC_A		((uint16_t)(3 << CRABIT_CLKSRC_A))
-
-#define CRBMSK_INTRESETCMD	((uint16_t)(1 << CRBBIT_INTRESETCMD))
-#define CRBMSK_INTRESET_B	((uint16_t)(1 << CRBBIT_INTRESET_B))
-#define CRBMSK_INTRESET_A	((uint16_t)(1 << CRBBIT_INTRESET_A))
-#define CRBMSK_CLKENAB_A	((uint16_t)(1 << CRBBIT_CLKENAB_A))
-#define CRBMSK_INTSRC_B		((uint16_t)(3 << CRBBIT_INTSRC_B))
-#define CRBMSK_LATCHSRC		((uint16_t)(3 << CRBBIT_LATCHSRC))
-#define CRBMSK_LOADSRC_B	((uint16_t)(3 << CRBBIT_LOADSRC_B))
-#define CRBMSK_CLKMULT_B	((uint16_t)(3 << CRBBIT_CLKMULT_B))
-#define CRBMSK_CLKENAB_B	((uint16_t)(1 << CRBBIT_CLKENAB_B))
-#define CRBMSK_INDXPOL_B	((uint16_t)(1 << CRBBIT_INDXPOL_B))
-#define CRBMSK_CLKPOL_B		((uint16_t)(1 << CRBBIT_CLKPOL_B))
+#define CRAMSK_INDXSRC_B	(3 << CRABIT_INDXSRC_B)
+#define CRAMSK_CLKSRC_B		(3 << CRABIT_CLKSRC_B)
+#define CRAMSK_INDXPOL_A	(1 << CRABIT_INDXPOL_A)
+#define CRAMSK_LOADSRC_A	(3 << CRABIT_LOADSRC_A)
+#define CRAMSK_CLKMULT_A	(3 << CRABIT_CLKMULT_A)
+#define CRAMSK_INTSRC_A		(3 << CRABIT_INTSRC_A)
+#define CRAMSK_CLKPOL_A		(3 << CRABIT_CLKPOL_A)
+#define CRAMSK_INDXSRC_A	(3 << CRABIT_INDXSRC_A)
+#define CRAMSK_CLKSRC_A		(3 << CRABIT_CLKSRC_A)
+
+#define CRBMSK_INTRESETCMD	(1 << CRBBIT_INTRESETCMD)
+#define CRBMSK_INTRESET_B	(1 << CRBBIT_INTRESET_B)
+#define CRBMSK_INTRESET_A	(1 << CRBBIT_INTRESET_A)
+#define CRBMSK_CLKENAB_A	(1 << CRBBIT_CLKENAB_A)
+#define CRBMSK_INTSRC_B		(3 << CRBBIT_INTSRC_B)
+#define CRBMSK_LATCHSRC		(3 << CRBBIT_LATCHSRC)
+#define CRBMSK_LOADSRC_B	(3 << CRBBIT_LOADSRC_B)
+#define CRBMSK_CLKMULT_B	(3 << CRBBIT_CLKMULT_B)
+#define CRBMSK_CLKENAB_B	(1 << CRBBIT_CLKENAB_B)
+#define CRBMSK_INDXPOL_B	(1 << CRBBIT_INDXPOL_B)
+#define CRBMSK_CLKPOL_B		(1 << CRBBIT_CLKPOL_B)
 
 #define CRBMSK_INTCTRL		(CRBMSK_INTRESETCMD | CRBMSK_INTRESET_A | CRBMSK_INTRESET_B)	/*  Interrupt reset control bits. */
 
@@ -699,15 +680,15 @@
 
 /*  Bit field masks for standardized SETUP structure. */
 
-#define STDMSK_INTSRC		((uint16_t)(3 << STDBIT_INTSRC))
-#define STDMSK_LATCHSRC		((uint16_t)(3 << STDBIT_LATCHSRC))
-#define STDMSK_LOADSRC		((uint16_t)(3 << STDBIT_LOADSRC))
-#define STDMSK_INDXSRC		((uint16_t)(1 << STDBIT_INDXSRC))
-#define STDMSK_INDXPOL		((uint16_t)(1 << STDBIT_INDXPOL))
-#define STDMSK_CLKSRC		((uint16_t)(3 << STDBIT_CLKSRC))
-#define STDMSK_CLKPOL		((uint16_t)(1 << STDBIT_CLKPOL))
-#define STDMSK_CLKMULT		((uint16_t)(3 << STDBIT_CLKMULT))
-#define STDMSK_CLKENAB		((uint16_t)(1 << STDBIT_CLKENAB))
+#define STDMSK_INTSRC		(3 << STDBIT_INTSRC)
+#define STDMSK_LATCHSRC		(3 << STDBIT_LATCHSRC)
+#define STDMSK_LOADSRC		(3 << STDBIT_LOADSRC)
+#define STDMSK_INDXSRC		(1 << STDBIT_INDXSRC)
+#define STDMSK_INDXPOL		(1 << STDBIT_INDXPOL)
+#define STDMSK_CLKSRC		(3 << STDBIT_CLKSRC)
+#define STDMSK_CLKPOL		(1 << STDBIT_CLKPOL)
+#define STDMSK_CLKMULT		(3 << STDBIT_CLKMULT)
+#define STDMSK_CLKENAB		(1 << STDBIT_CLKENAB)
 
 struct bufferDMA {
 	dma_addr_t PhysicalBase;
diff --git a/drivers/staging/comedi/drivers/serial2002.c b/drivers/staging/comedi/drivers/serial2002.c
index e6177b48cca..8900086374d 100644
--- a/drivers/staging/comedi/drivers/serial2002.c
+++ b/drivers/staging/comedi/drivers/serial2002.c
@@ -31,8 +31,6 @@ Status: in development
 
 */
 
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
 #include "../comedidev.h"
 
 #include <linux/delay.h>
@@ -72,7 +70,40 @@ struct serial_data {
 	unsigned long value;
 };
 
-static long tty_ioctl(struct file *f, unsigned op, unsigned long param)
+/*
+ * The configuration serial_data.value read from the device is
+ * a bitmask that defines specific options of a channel:
+ *
+ * 4:0 - the channel to configure
+ * 7:5 - the kind of channel
+ * 9:8 - the command used to configure the channel
+ *
+ * The remaining bits vary in use depending on the command:
+ *
+ * BITS     15:10 - the channel bits (maxdata)
+ * MIN/MAX  12:10 - the units multiplier for the scale
+ *          13    - the sign of the scale
+ *          33:14 - the base value for the range
+ */
+#define S2002_CFG_CHAN(x)		((x) & 0x1f)
+#define S2002_CFG_KIND(x)		(((x) >> 5) & 0x7)
+#define S2002_CFG_KIND_INVALID		0
+#define S2002_CFG_KIND_DIGITAL_IN	1
+#define S2002_CFG_KIND_DIGITAL_OUT	2
+#define S2002_CFG_KIND_ANALOG_IN	3
+#define S2002_CFG_KIND_ANALOG_OUT	4
+#define S2002_CFG_KIND_ENCODER_IN	5
+#define S2002_CFG_CMD(x)		(((x) >> 8) & 0x3)
+#define S2002_CFG_CMD_BITS		0
+#define S2002_CFG_CMD_MIN		1
+#define S2002_CFG_CMD_MAX		2
+#define S2002_CFG_BITS(x)		(((x) >> 10) & 0x3f)
+#define S2002_CFG_UNITS(x)		(((x) >> 10) & 0x7)
+#define S2002_CFG_SIGN(x)		(((x) >> 13) & 0x1)
+#define S2002_CFG_BASE(x)		(((x) >> 14) & 0xfffff)
+
+static long serial2002_tty_ioctl(struct file *f, unsigned op,
+				 unsigned long param)
 {
 	if (f->f_op->unlocked_ioctl)
 		return f->f_op->unlocked_ioctl(f, op, param);
@@ -80,39 +111,58 @@ static long tty_ioctl(struct file *f, unsigned op, unsigned long param)
 	return -ENOSYS;
 }
 
-static int tty_write(struct file *f, unsigned char *buf, int count)
+static int serial2002_tty_write(struct file *f, unsigned char *buf, int count)
 {
+	const char __user *p = (__force const char __user *)buf;
 	int result;
 	mm_segment_t oldfs;
 
 	oldfs = get_fs();
 	set_fs(KERNEL_DS);
 	f->f_pos = 0;
-	result = f->f_op->write(f, buf, count, &f->f_pos);
+	result = f->f_op->write(f, p, count, &f->f_pos);
 	set_fs(oldfs);
 	return result;
 }
 
-#if 0
-/*
- * On 2.6.26.3 this occaisonally gave me page faults, worked around by
- * settings.c_cc[VMIN] = 0; settings.c_cc[VTIME] = 0
- */
-static int tty_available(struct file *f)
+static int serial2002_tty_readb(struct file *f, unsigned char *buf)
 {
-	long result = 0;
-	mm_segment_t oldfs;
+	char __user *p = (__force char __user *)buf;
 
-	oldfs = get_fs();
-	set_fs(KERNEL_DS);
-	tty_ioctl(f, FIONREAD, (unsigned long)&result);
-	set_fs(oldfs);
-	return result;
+	f->f_pos = 0;
+	return f->f_op->read(f, p, 1, &f->f_pos);
+}
+
+static void serial2002_tty_read_poll_wait(struct file *f, int timeout)
+{
+	struct poll_wqueues table;
+	struct timeval start, now;
+
+	do_gettimeofday(&start);
+	poll_initwait(&table);
+	while (1) {
+		long elapsed;
+		int mask;
+
+		mask = f->f_op->poll(f, &table.pt);
+		if (mask & (POLLRDNORM | POLLRDBAND | POLLIN |
+			    POLLHUP | POLLERR)) {
+			break;
+		}
+		do_gettimeofday(&now);
+		elapsed = (1000000 * (now.tv_sec - start.tv_sec) +
+			  now.tv_usec - start.tv_usec);
+		if (elapsed > timeout)
+			break;
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(((timeout - elapsed) * HZ) / 10000);
+	}
+	poll_freewait(&table);
 }
-#endif
 
-static int tty_read(struct file *f, int timeout)
+static int serial2002_tty_read(struct file *f, int timeout)
 {
+	unsigned char ch;
 	int result;
 
 	result = -1;
@@ -122,50 +172,19 @@ static int tty_read(struct file *f, int timeout)
 		oldfs = get_fs();
 		set_fs(KERNEL_DS);
 		if (f->f_op->poll) {
-			struct poll_wqueues table;
-			struct timeval start, now;
-
-			do_gettimeofday(&start);
-			poll_initwait(&table);
-			while (1) {
-				long elapsed;
-				int mask;
-
-				mask = f->f_op->poll(f, &table.pt);
-				if (mask & (POLLRDNORM | POLLRDBAND | POLLIN |
-					    POLLHUP | POLLERR)) {
-					break;
-				}
-				do_gettimeofday(&now);
-				elapsed =
-				    (1000000 * (now.tv_sec - start.tv_sec) +
-				     now.tv_usec - start.tv_usec);
-				if (elapsed > timeout)
-					break;
-				set_current_state(TASK_INTERRUPTIBLE);
-				schedule_timeout(((timeout -
-						   elapsed) * HZ) / 10000);
-			}
-			poll_freewait(&table);
-			{
-				unsigned char ch;
+			serial2002_tty_read_poll_wait(f, timeout);
 
-				f->f_pos = 0;
-				if (f->f_op->read(f, &ch, 1, &f->f_pos) == 1)
-					result = ch;
-			}
+			if (serial2002_tty_readb(f, &ch) == 1)
+				result = ch;
 		} else {
 			/* Device does not support poll, busy wait */
 			int retries = 0;
 			while (1) {
-				unsigned char ch;
-
 				retries++;
 				if (retries >= timeout)
 					break;
 
-				f->f_pos = 0;
-				if (f->f_op->read(f, &ch, 1, &f->f_pos) == 1) {
+				if (serial2002_tty_readb(f, &ch) == 1) {
 					result = ch;
 					break;
 				}
@@ -177,90 +196,76 @@ static int tty_read(struct file *f, int timeout)
 	return result;
 }
 
-static void tty_setspeed(struct file *f, int speed)
+static void serial2002_tty_setspeed(struct file *f, int speed)
 {
+	struct termios termios;
+	struct serial_struct serial;
 	mm_segment_t oldfs;
 
 	oldfs = get_fs();
 	set_fs(KERNEL_DS);
-	{
-		/*  Set speed */
-		struct termios settings;
-
-		tty_ioctl(f, TCGETS, (unsigned long)&settings);
-/* printk("Speed: %d\n", settings.c_cflag & (CBAUD | CBAUDEX)); */
-		settings.c_iflag = 0;
-		settings.c_oflag = 0;
-		settings.c_lflag = 0;
-		settings.c_cflag = CLOCAL | CS8 | CREAD;
-		settings.c_cc[VMIN] = 0;
-		settings.c_cc[VTIME] = 0;
-		switch (speed) {
-		case 2400:{
-				settings.c_cflag |= B2400;
-			}
-			break;
-		case 4800:{
-				settings.c_cflag |= B4800;
-			}
-			break;
-		case 9600:{
-				settings.c_cflag |= B9600;
-			}
-			break;
-		case 19200:{
-				settings.c_cflag |= B19200;
-			}
-			break;
-		case 38400:{
-				settings.c_cflag |= B38400;
-			}
-			break;
-		case 57600:{
-				settings.c_cflag |= B57600;
-			}
-			break;
-		case 115200:{
-				settings.c_cflag |= B115200;
-			}
-			break;
-		default:{
-				settings.c_cflag |= B9600;
-			}
-			break;
-		}
-		tty_ioctl(f, TCSETS, (unsigned long)&settings);
-/* printk("Speed: %d\n", settings.c_cflag & (CBAUD | CBAUDEX)); */
-	}
-	{
-		/*  Set low latency */
-		struct serial_struct settings;
 
-		tty_ioctl(f, TIOCGSERIAL, (unsigned long)&settings);
-		settings.flags |= ASYNC_LOW_LATENCY;
-		tty_ioctl(f, TIOCSSERIAL, (unsigned long)&settings);
+	/* Set speed */
+	serial2002_tty_ioctl(f, TCGETS, (unsigned long)&termios);
+	termios.c_iflag = 0;
+	termios.c_oflag = 0;
+	termios.c_lflag = 0;
+	termios.c_cflag = CLOCAL | CS8 | CREAD;
+	termios.c_cc[VMIN] = 0;
+	termios.c_cc[VTIME] = 0;
+	switch (speed) {
+	case 2400:
+		termios.c_cflag |= B2400;
+		break;
+	case 4800:
+		termios.c_cflag |= B4800;
+		break;
+	case 9600:
+		termios.c_cflag |= B9600;
+		break;
+	case 19200:
+		termios.c_cflag |= B19200;
+		break;
+	case 38400:
+		termios.c_cflag |= B38400;
+		break;
+	case 57600:
+		termios.c_cflag |= B57600;
+		break;
+	case 115200:
+		termios.c_cflag |= B115200;
+		break;
+	default:
+		termios.c_cflag |= B9600;
+		break;
 	}
+	serial2002_tty_ioctl(f, TCSETS, (unsigned long)&termios);
+
+	/* Set low latency */
+	serial2002_tty_ioctl(f, TIOCGSERIAL, (unsigned long)&serial);
+	serial.flags |= ASYNC_LOW_LATENCY;
+	serial2002_tty_ioctl(f, TIOCSSERIAL, (unsigned long)&serial);
 
 	set_fs(oldfs);
 }
 
-static void poll_digital(struct file *f, int channel)
+static void serial2002_poll_digital(struct file *f, int channel)
 {
 	char cmd;
 
 	cmd = 0x40 | (channel & 0x1f);
-	tty_write(f, &cmd, 1);
+	serial2002_tty_write(f, &cmd, 1);
 }
 
-static void poll_channel(struct file *f, int channel)
+static void serial2002_poll_channel(struct file *f, int channel)
 {
 	char cmd;
 
 	cmd = 0x60 | (channel & 0x1f);
-	tty_write(f, &cmd, 1);
+	serial2002_tty_write(f, &cmd, 1);
 }
 
-static struct serial_data serial_read(struct file *f, int timeout)
+static struct serial_data serial2002_read(struct file *f, int timeout)
 {
 	struct serial_data result;
 	int length;
@@ -270,26 +275,23 @@ static struct serial_data serial_read(struct file *f, int timeout)
 	result.value = 0;
 	length = 0;
 	while (1) {
-		int data = tty_read(f, timeout);
+		int data = serial2002_tty_read(f, timeout);
 
 		length++;
 		if (data < 0) {
-			pr_err("Failed to read serial.\n");
 			break;
 		} else if (data & 0x80) {
 			result.value = (result.value << 7) | (data & 0x7f);
 		} else {
 			if (length == 1) {
 				switch ((data >> 5) & 0x03) {
-				case 0:{
-						result.value = 0;
-						result.kind = is_digital;
-					}
+				case 0:
+					result.value = 0;
+					result.kind = is_digital;
 					break;
-				case 1:{
-						result.value = 1;
-						result.kind = is_digital;
-					}
+				case 1:
+					result.value = 1;
+					result.kind = is_digital;
 					break;
 				}
 			} else {
@@ -305,12 +307,12 @@ static struct serial_data serial_read(struct file *f, int timeout)
 
 }
 
-static void serial_write(struct file *f, struct serial_data data)
+static void serial2002_write(struct file *f, struct serial_data data)
 {
 	if (data.kind == is_digital) {
 		unsigned char ch =
 		    ((data.value << 5) & 0x20) | (data.index & 0x1f);
-		tty_write(f, &ch, 1);
+		serial2002_tty_write(f, &ch, 1);
 	} else {
 		unsigned char ch[6];
 		int i = 0;
@@ -334,319 +336,250 @@ static void serial_write(struct file *f, struct serial_data data)
 		i++;
 		ch[i] = ((data.value << 5) & 0x60) | (data.index & 0x1f);
 		i++;
-		tty_write(f, ch, i);
+		serial2002_tty_write(f, ch, i);
 	}
 }
 
-static int serial_2002_open(struct comedi_device *dev)
+struct config_t {
+	short int kind;
+	short int bits;
+	int min;
+	int max;
+};
+
+static int serial2002_setup_subdevice(struct comedi_subdevice *s,
+				      struct config_t *cfg,
+				      struct serial2002_range_table_t *range,
+				      unsigned char *mapping,
+				      int kind)
 {
-	struct serial2002_private *devpriv = dev->private;
-	int result;
-	char port[20];
+	const struct comedi_lrange **range_table_list = NULL;
+	unsigned int *maxdata_list;
+	int j, chan;
 
-	sprintf(port, "/dev/ttyS%d", devpriv->port);
-	devpriv->tty = filp_open(port, O_RDWR, 0);
-	if (IS_ERR(devpriv->tty)) {
-		result = (int)PTR_ERR(devpriv->tty);
-		dev_err(dev->class_dev, "file open error = %d\n", result);
-	} else {
-		struct config_t {
-
-			short int kind;
-			short int bits;
-			int min;
-			int max;
-		};
-
-		struct config_t *dig_in_config;
-		struct config_t *dig_out_config;
-		struct config_t *chan_in_config;
-		struct config_t *chan_out_config;
-		int i;
-
-		result = 0;
-		dig_in_config = kcalloc(32, sizeof(struct config_t),
-				GFP_KERNEL);
-		dig_out_config = kcalloc(32, sizeof(struct config_t),
-				GFP_KERNEL);
-		chan_in_config = kcalloc(32, sizeof(struct config_t),
-				GFP_KERNEL);
-		chan_out_config = kcalloc(32, sizeof(struct config_t),
-				GFP_KERNEL);
-		if (!dig_in_config || !dig_out_config
-		    || !chan_in_config || !chan_out_config) {
-			result = -ENOMEM;
-			goto err_alloc_configs;
+	for (chan = 0, j = 0; j < 32; j++) {
+		if (cfg[j].kind == kind)
+			chan++;
+	}
+	s->n_chan = chan;
+	s->maxdata = 0;
+	kfree(s->maxdata_list);
+	maxdata_list = kmalloc(sizeof(unsigned int) * s->n_chan, GFP_KERNEL);
+	if (!maxdata_list)
+		return -ENOMEM;
+	s->maxdata_list = maxdata_list;
+	kfree(s->range_table_list);
+	s->range_table = NULL;
+	s->range_table_list = NULL;
+	if (kind == 1 || kind == 2) {
+		s->range_table = &range_digital;
+	} else if (range) {
+		range_table_list =
+			kmalloc(sizeof(struct serial2002_range_table_t) *
+				s->n_chan, GFP_KERNEL);
+		if (!range_table_list)
+			return -ENOMEM;
+		s->range_table_list = range_table_list;
+	}
+	for (chan = 0, j = 0; j < 32; j++) {
+		if (cfg[j].kind == kind) {
+			if (mapping)
+				mapping[chan] = j;
+			if (range) {
+				range[j].length = 1;
+				range[j].range.min = cfg[j].min;
+				range[j].range.max = cfg[j].max;
+				range_table_list[chan] =
+				    (const struct comedi_lrange *)&range[j];
+			}
+			maxdata_list[chan] = ((long long)1 << cfg[j].bits) - 1;
+			chan++;
 		}
+	}
+	return 0;
+}
 
-		tty_setspeed(devpriv->tty, devpriv->speed);
-		poll_channel(devpriv->tty, 31);	/*  Start reading configuration */
-		while (1) {
-			struct serial_data data;
+static int serial2002_setup_subdevs(struct comedi_device *dev)
+{
+	struct serial2002_private *devpriv = dev->private;
+	struct config_t *di_cfg;
+	struct config_t *do_cfg;
+	struct config_t *ai_cfg;
+	struct config_t *ao_cfg;
+	struct config_t *cfg;
+	struct comedi_subdevice *s;
+	int result = 0;
+	int i;
 
-			data = serial_read(devpriv->tty, 1000);
-			if (data.kind != is_channel || data.index != 31
-			    || !(data.value & 0xe0)) {
-				break;
-			} else {
-				int command, channel, kind;
-				struct config_t *cur_config = NULL;
-
-				channel = data.value & 0x1f;
-				kind = (data.value >> 5) & 0x7;
-				command = (data.value >> 8) & 0x3;
-				switch (kind) {
-				case 1:{
-						cur_config = dig_in_config;
-					}
-					break;
-				case 2:{
-						cur_config = dig_out_config;
-					}
-					break;
-				case 3:{
-						cur_config = chan_in_config;
-					}
-					break;
-				case 4:{
-						cur_config = chan_out_config;
-					}
-					break;
-				case 5:{
-						cur_config = chan_in_config;
-					}
-					break;
-				}
+	/* Allocate the temporary structs to hold the configuration data */
+	di_cfg = kcalloc(32, sizeof(*cfg), GFP_KERNEL);
+	do_cfg = kcalloc(32, sizeof(*cfg), GFP_KERNEL);
+	ai_cfg = kcalloc(32, sizeof(*cfg), GFP_KERNEL);
+	ao_cfg = kcalloc(32, sizeof(*cfg), GFP_KERNEL);
+	if (!di_cfg || !do_cfg || !ai_cfg || !ao_cfg) {
+		result = -ENOMEM;
+		goto err_alloc_configs;
+	}
 
-				if (cur_config) {
-					cur_config[channel].kind = kind;
-					switch (command) {
-					case 0:{
-							cur_config[channel].bits
-							    =
-							    (data.value >> 10) &
-							    0x3f;
-						}
-						break;
-					case 1:{
-							int unit, sign, min;
-							unit =
-							    (data.value >> 10) &
-							    0x7;
-							sign =
-							    (data.value >> 13) &
-							    0x1;
-							min =
-							    (data.value >> 14) &
-							    0xfffff;
-
-							switch (unit) {
-							case 0:{
-									min =
-									    min
-									    *
-									    1000000;
-								}
-								break;
-							case 1:{
-									min =
-									    min
-									    *
-									    1000;
-								}
-								break;
-							case 2:{
-									min =
-									    min
-									    * 1;
-								}
-								break;
-							}
-							if (sign)
-								min = -min;
-							cur_config[channel].min
-							    = min;
-						}
-						break;
-					case 2:{
-							int unit, sign, max;
-							unit =
-							    (data.value >> 10) &
-							    0x7;
-							sign =
-							    (data.value >> 13) &
-							    0x1;
-							max =
-							    (data.value >> 14) &
-							    0xfffff;
-
-							switch (unit) {
-							case 0:{
-									max =
-									    max
-									    *
-									    1000000;
-								}
-								break;
-							case 1:{
-									max =
-									    max
-									    *
-									    1000;
-								}
-								break;
-							case 2:{
-									max =
-									    max
-									    * 1;
-								}
-								break;
-							}
-							if (sign)
-								max = -max;
-							cur_config[channel].max
-							    = max;
-						}
-						break;
-					}
-				}
-			}
-		}
-		for (i = 0; i <= 4; i++) {
-			/*  Fill in subdev data */
-			struct config_t *c;
-			unsigned char *mapping = NULL;
-			struct serial2002_range_table_t *range = NULL;
-			int kind = 0;
-
-			switch (i) {
-			case 0:{
-					c = dig_in_config;
-					mapping = devpriv->digital_in_mapping;
-					kind = 1;
-				}
+	/* Read the configuration from the connected device */
+	serial2002_tty_setspeed(devpriv->tty, devpriv->speed);
+	serial2002_poll_channel(devpriv->tty, 31);
+	while (1) {
+		struct serial_data data;
+
+		data = serial2002_read(devpriv->tty, 1000);
+		if (data.kind != is_channel || data.index != 31 ||
+		    S2002_CFG_KIND(data.value) == S2002_CFG_KIND_INVALID) {
+			break;
+		} else {
+			int channel = S2002_CFG_CHAN(data.value);
+			int range = S2002_CFG_BASE(data.value);
+
+			switch (S2002_CFG_KIND(data.value)) {
+			case S2002_CFG_KIND_DIGITAL_IN:
+				cfg = di_cfg;
 				break;
-			case 1:{
-					c = dig_out_config;
-					mapping = devpriv->digital_out_mapping;
-					kind = 2;
-				}
+			case S2002_CFG_KIND_DIGITAL_OUT:
+				cfg = do_cfg;
 				break;
-			case 2:{
-					c = chan_in_config;
-					mapping = devpriv->analog_in_mapping;
-					range = devpriv->in_range;
-					kind = 3;
-				}
+			case S2002_CFG_KIND_ANALOG_IN:
+				cfg = ai_cfg;
 				break;
-			case 3:{
-					c = chan_out_config;
-					mapping = devpriv->analog_out_mapping;
-					range = devpriv->out_range;
-					kind = 4;
-				}
+			case S2002_CFG_KIND_ANALOG_OUT:
+				cfg = ao_cfg;
 				break;
-			case 4:{
-					c = chan_in_config;
-					mapping = devpriv->encoder_in_mapping;
-					range = devpriv->in_range;
-					kind = 5;
-				}
+			case S2002_CFG_KIND_ENCODER_IN:
+				cfg = ai_cfg;
 				break;
-			default:{
-					c = NULL;
-				}
+			default:
+				cfg = NULL;
 				break;
 			}
-			if (c) {
-				struct comedi_subdevice *s;
-				const struct comedi_lrange **range_table_list =
-				    NULL;
-				unsigned int *maxdata_list;
-				int j, chan;
-
-				for (chan = 0, j = 0; j < 32; j++) {
-					if (c[j].kind == kind)
-						chan++;
-				}
-				s = &dev->subdevices[i];
-				s->n_chan = chan;
-				s->maxdata = 0;
-				kfree(s->maxdata_list);
-				s->maxdata_list = maxdata_list =
-				    kmalloc(sizeof(unsigned int) * s->n_chan,
-					    GFP_KERNEL);
-				if (!s->maxdata_list)
-					break;	/* error handled below */
-				kfree(s->range_table_list);
-				s->range_table = NULL;
-				s->range_table_list = NULL;
-				if (kind == 1 || kind == 2) {
-					s->range_table = &range_digital;
-				} else if (range) {
-					s->range_table_list = range_table_list =
-					    kmalloc(sizeof
-						    (struct
-						     serial2002_range_table_t) *
-						    s->n_chan, GFP_KERNEL);
-					if (!s->range_table_list)
-						break;	/* err handled below */
-				}
-				for (chan = 0, j = 0; j < 32; j++) {
-					if (c[j].kind == kind) {
-						if (mapping)
-							mapping[chan] = j;
-						if (range) {
-							range[j].length = 1;
-							range[j].range.min =
-							    c[j].min;
-							range[j].range.max =
-							    c[j].max;
-							range_table_list[chan] =
-							    (const struct
-							     comedi_lrange *)
-							    &range[j];
-						}
-						maxdata_list[chan] =
-						    ((long long)1 << c[j].bits)
-						    - 1;
-						chan++;
-					}
+			if (!cfg)
+				continue;	/* unknown kind, skip it */
+
+			cfg[channel].kind = S2002_CFG_KIND(data.value);
+
+			switch (S2002_CFG_CMD(data.value)) {
+			case S2002_CFG_CMD_BITS:
+				cfg[channel].bits = S2002_CFG_BITS(data.value);
+				break;
+			case S2002_CFG_CMD_MIN:
+			case S2002_CFG_CMD_MAX:
+				switch (S2002_CFG_UNITS(data.value)) {
+				case 0:
+					range *= 1000000;
+					break;
+				case 1:
+					range *= 1000;
+					break;
+				case 2:
+					range *= 1;
+					break;
 				}
+				if (S2002_CFG_SIGN(data.value))
+					range = -range;
+				if (S2002_CFG_CMD(data.value) ==
+				    S2002_CFG_CMD_MIN)
+					cfg[channel].min = range;
+				else
+					cfg[channel].max = range;
+				break;
 			}
 		}
-		if (i <= 4) {
-			/* Failed to allocate maxdata_list or range_table_list
-			 * for a subdevice that needed it.  */
-			result = -ENOMEM;
-			for (i = 0; i <= 4; i++) {
-				struct comedi_subdevice *s;
-
-				s = &dev->subdevices[i];
-				kfree(s->maxdata_list);
-				s->maxdata_list = NULL;
-				kfree(s->range_table_list);
-				s->range_table_list = NULL;
-			}
+	}
+
+	/* Fill in subdevice data */
+	for (i = 0; i <= 4; i++) {
+		unsigned char *mapping = NULL;
+		struct serial2002_range_table_t *range = NULL;
+		int kind = 0;
+
+		s = &dev->subdevices[i];
+
+		switch (i) {
+		case 0:
+			cfg = di_cfg;
+			mapping = devpriv->digital_in_mapping;
+			kind = S2002_CFG_KIND_DIGITAL_IN;
+			break;
+		case 1:
+			cfg = do_cfg;
+			mapping = devpriv->digital_out_mapping;
+			kind = S2002_CFG_KIND_DIGITAL_OUT;
+			break;
+		case 2:
+			cfg = ai_cfg;
+			mapping = devpriv->analog_in_mapping;
+			range = devpriv->in_range;
+			kind = S2002_CFG_KIND_ANALOG_IN;
+			break;
+		case 3:
+			cfg = ao_cfg;
+			mapping = devpriv->analog_out_mapping;
+			range = devpriv->out_range;
+			kind = S2002_CFG_KIND_ANALOG_OUT;
+			break;
+		case 4:
+			cfg = ai_cfg;
+			mapping = devpriv->encoder_in_mapping;
+			range = devpriv->in_range;
+			kind = S2002_CFG_KIND_ENCODER_IN;
+			break;
 		}
 
+		if (serial2002_setup_subdevice(s, cfg, range, mapping, kind))
+			break;	/* err handled below */
+	}
+	if (i <= 4) {
+		/*
+		 * Failed to allocate maxdata_list or range_table_list
+		 * for a subdevice that needed it.
+		 */
+		result = -ENOMEM;
+		for (i = 0; i <= 4; i++) {
+			s = &dev->subdevices[i];
+			kfree(s->maxdata_list);
+			s->maxdata_list = NULL;
+			kfree(s->range_table_list);
+			s->range_table_list = NULL;
+		}
+	}
+
 err_alloc_configs:
-		kfree(dig_in_config);
-		kfree(dig_out_config);
-		kfree(chan_in_config);
-		kfree(chan_out_config);
-
-		if (result) {
-			if (devpriv->tty) {
-				filp_close(devpriv->tty, NULL);
-				devpriv->tty = NULL;
-			}
+	kfree(di_cfg);
+	kfree(do_cfg);
+	kfree(ai_cfg);
+	kfree(ao_cfg);
+
+	if (result) {
+		if (devpriv->tty) {
+			filp_close(devpriv->tty, NULL);
+			devpriv->tty = NULL;
 		}
 	}
+
+	return result;
+}
+
+static int serial2002_open(struct comedi_device *dev)
+{
+	struct serial2002_private *devpriv = dev->private;
+	int result;
+	char port[20];
+
+	sprintf(port, "/dev/ttyS%d", devpriv->port);
+	devpriv->tty = filp_open(port, O_RDWR, 0);
+	if (IS_ERR(devpriv->tty)) {
+		result = (int)PTR_ERR(devpriv->tty);
+		dev_err(dev->class_dev, "file open error = %d\n", result);
+	} else {
+		result = serial2002_setup_subdevs(dev);
+	}
 	return result;
 }
 
-static void serial_2002_close(struct comedi_device *dev)
+static void serial2002_close(struct comedi_device *dev)
 {
 	struct serial2002_private *devpriv = dev->private;
 
@@ -654,9 +587,10 @@ static void serial_2002_close(struct comedi_device *dev)
 		filp_close(devpriv->tty, NULL);
 }
 
-static int serial2002_di_rinsn(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
+static int serial2002_di_insn_read(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   struct comedi_insn *insn,
+				   unsigned int *data)
 {
 	struct serial2002_private *devpriv = dev->private;
 	int n;
@@ -666,9 +600,9 @@ static int serial2002_di_rinsn(struct comedi_device *dev,
 	for (n = 0; n < insn->n; n++) {
 		struct serial_data read;
 
-		poll_digital(devpriv->tty, chan);
+		serial2002_poll_digital(devpriv->tty, chan);
 		while (1) {
-			read = serial_read(devpriv->tty, 1000);
+			read = serial2002_read(devpriv->tty, 1000);
 			if (read.kind != is_digital || read.index == chan)
 				break;
 		}
@@ -677,9 +611,10 @@ static int serial2002_di_rinsn(struct comedi_device *dev,
 	return n;
 }
 
-static int serial2002_do_winsn(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
+static int serial2002_do_insn_write(struct comedi_device *dev,
+				    struct comedi_subdevice *s,
+				    struct comedi_insn *insn,
+				    unsigned int *data)
 {
 	struct serial2002_private *devpriv = dev->private;
 	int n;
@@ -692,14 +627,15 @@ static int serial2002_do_winsn(struct comedi_device *dev,
 		write.kind = is_digital;
 		write.index = chan;
 		write.value = data[n];
-		serial_write(devpriv->tty, write);
+		serial2002_write(devpriv->tty, write);
 	}
 	return n;
 }
 
-static int serial2002_ai_rinsn(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
+static int serial2002_ai_insn_read(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   struct comedi_insn *insn,
+				   unsigned int *data)
 {
 	struct serial2002_private *devpriv = dev->private;
 	int n;
@@ -709,9 +645,9 @@ static int serial2002_ai_rinsn(struct comedi_device *dev,
 	for (n = 0; n < insn->n; n++) {
 		struct serial_data read;
 
-		poll_channel(devpriv->tty, chan);
+		serial2002_poll_channel(devpriv->tty, chan);
 		while (1) {
-			read = serial_read(devpriv->tty, 1000);
+			read = serial2002_read(devpriv->tty, 1000);
 			if (read.kind != is_channel || read.index == chan)
 				break;
 		}
@@ -720,9 +656,10 @@ static int serial2002_ai_rinsn(struct comedi_device *dev,
 	return n;
 }
 
-static int serial2002_ao_winsn(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
+static int serial2002_ao_insn_write(struct comedi_device *dev,
+				    struct comedi_subdevice *s,
+				    struct comedi_insn *insn,
+				    unsigned int *data)
 {
 	struct serial2002_private *devpriv = dev->private;
 	int n;
@@ -735,15 +672,16 @@ static int serial2002_ao_winsn(struct comedi_device *dev,
 		write.kind = is_channel;
 		write.index = chan;
 		write.value = data[n];
-		serial_write(devpriv->tty, write);
+		serial2002_write(devpriv->tty, write);
 		devpriv->ao_readback[chan] = data[n];
 	}
 	return n;
 }
 
-static int serial2002_ao_rinsn(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
+static int serial2002_ao_insn_read(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   struct comedi_insn *insn,
+				   unsigned int *data)
 {
 	struct serial2002_private *devpriv = dev->private;
 	int n;
@@ -755,9 +693,10 @@ static int serial2002_ao_rinsn(struct comedi_device *dev,
 	return n;
 }
 
-static int serial2002_ei_rinsn(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
+static int serial2002_encoder_insn_read(struct comedi_device *dev,
+					struct comedi_subdevice *s,
+					struct comedi_insn *insn,
+					unsigned int *data)
 {
 	struct serial2002_private *devpriv = dev->private;
 	int n;
@@ -767,9 +706,9 @@ static int serial2002_ei_rinsn(struct comedi_device *dev,
 	for (n = 0; n < insn->n; n++) {
 		struct serial_data read;
 
-		poll_channel(devpriv->tty, chan);
+		serial2002_poll_channel(devpriv->tty, chan);
 		while (1) {
-			read = serial_read(devpriv->tty, 1000);
+			read = serial2002_read(devpriv->tty, 1000);
 			if (read.kind != is_channel || read.index == chan)
 				break;
 		}
@@ -785,20 +724,13 @@ static int serial2002_attach(struct comedi_device *dev,
 	struct comedi_subdevice *s;
 	int ret;
 
-	dev_dbg(dev->class_dev, "serial2002: attach\n");
-	dev->board_name = dev->driver->driver_name;
-
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
 	if (!devpriv)
 		return -ENOMEM;
 	dev->private = devpriv;
 
-	dev->open = serial_2002_open;
-	dev->close = serial_2002_close;
 	devpriv->port = it->options[0];
 	devpriv->speed = it->options[1];
-	dev_dbg(dev->class_dev, "/dev/ttyS%d @ %d\n", devpriv->port,
-		devpriv->speed);
 
 	ret = comedi_alloc_subdevices(dev, 5);
 	if (ret)
@@ -806,51 +738,54 @@ static int serial2002_attach(struct comedi_device *dev,
 
 	/* digital input subdevice */
 	s = &dev->subdevices[0];
-	s->type = COMEDI_SUBD_DI;
-	s->subdev_flags = SDF_READABLE;
-	s->n_chan = 0;
-	s->maxdata = 1;
-	s->range_table = &range_digital;
-	s->insn_read = &serial2002_di_rinsn;
+	s->type		= COMEDI_SUBD_DI;
+	s->subdev_flags	= SDF_READABLE;
+	s->n_chan	= 0;
+	s->maxdata	= 1;
+	s->range_table	= &range_digital;
+	s->insn_read	= serial2002_di_insn_read;
 
 	/* digital output subdevice */
 	s = &dev->subdevices[1];
-	s->type = COMEDI_SUBD_DO;
-	s->subdev_flags = SDF_WRITEABLE;
-	s->n_chan = 0;
-	s->maxdata = 1;
-	s->range_table = &range_digital;
-	s->insn_write = &serial2002_do_winsn;
+	s->type		= COMEDI_SUBD_DO;
+	s->subdev_flags	= SDF_WRITEABLE;
+	s->n_chan	= 0;
+	s->maxdata	= 1;
+	s->range_table	= &range_digital;
+	s->insn_write	= serial2002_do_insn_write;
 
 	/* analog input subdevice */
 	s = &dev->subdevices[2];
-	s->type = COMEDI_SUBD_AI;
-	s->subdev_flags = SDF_READABLE | SDF_GROUND;
-	s->n_chan = 0;
-	s->maxdata = 1;
-	s->range_table = NULL;
-	s->insn_read = &serial2002_ai_rinsn;
+	s->type		= COMEDI_SUBD_AI;
+	s->subdev_flags	= SDF_READABLE | SDF_GROUND;
+	s->n_chan	= 0;
+	s->maxdata	= 1;
+	s->range_table	= NULL;
+	s->insn_read	= serial2002_ai_insn_read;
 
 	/* analog output subdevice */
 	s = &dev->subdevices[3];
-	s->type = COMEDI_SUBD_AO;
-	s->subdev_flags = SDF_WRITEABLE;
-	s->n_chan = 0;
-	s->maxdata = 1;
-	s->range_table = NULL;
-	s->insn_write = &serial2002_ao_winsn;
-	s->insn_read = &serial2002_ao_rinsn;
+	s->type		= COMEDI_SUBD_AO;
+	s->subdev_flags	= SDF_WRITEABLE;
+	s->n_chan	= 0;
+	s->maxdata	= 1;
+	s->range_table	= NULL;
+	s->insn_write	= serial2002_ao_insn_write;
+	s->insn_read	= serial2002_ao_insn_read;
 
 	/* encoder input subdevice */
 	s = &dev->subdevices[4];
-	s->type = COMEDI_SUBD_COUNTER;
-	s->subdev_flags = SDF_READABLE | SDF_LSAMPL;
-	s->n_chan = 0;
-	s->maxdata = 1;
-	s->range_table = NULL;
-	s->insn_read = &serial2002_ei_rinsn;
+	s->type		= COMEDI_SUBD_COUNTER;
+	s->subdev_flags	= SDF_READABLE | SDF_LSAMPL;
+	s->n_chan	= 0;
+	s->maxdata	= 1;
+	s->range_table	= NULL;
+	s->insn_read	= serial2002_encoder_insn_read;
+
+	dev->open	= serial2002_open;
+	dev->close	= serial2002_close;
 
-	return 1;
+	return 0;
 }
 
 static void serial2002_detach(struct comedi_device *dev)
@@ -858,7 +793,7 @@ static void serial2002_detach(struct comedi_device *dev)
 	struct comedi_subdevice *s;
 	int i;
 
-	for (i = 0; i < 5; i++) {
+	for (i = 0; i < dev->n_subdevices; i++) {
 		s = &dev->subdevices[i];
 		kfree(s->maxdata_list);
 		kfree(s->range_table_list);
diff --git a/drivers/staging/comedi/drivers/skel.c b/drivers/staging/comedi/drivers/skel.c
index cb83f6ae48b..dbc8c54d6da 100644
--- a/drivers/staging/comedi/drivers/skel.c
+++ b/drivers/staging/comedi/drivers/skel.c
@@ -90,29 +90,30 @@ Configuration Options:
  * boards in this way is optional, and completely driver-dependent.
  * Some drivers use arrays such as this, other do not.
  */
+enum skel_boardid {
+	BOARD_SKEL100,
+	BOARD_SKEL200,
+};
+
 struct skel_board {
 	const char *name;
-	unsigned int devid;
 	int ai_chans;
 	int ai_bits;
 	int have_dio;
 };
 
 static const struct skel_board skel_boards[] = {
-	{
-	 .name = "skel-100",
-	 .devid = 0x100,
-	 .ai_chans = 16,
-	 .ai_bits = 12,
-	 .have_dio = 1,
-	 },
-	{
-	 .name = "skel-200",
-	 .devid = 0x200,
-	 .ai_chans = 8,
-	 .ai_bits = 16,
-	 .have_dio = 0,
-	 },
+	[BOARD_SKEL100] = {
+		.name		= "skel-100",
+		.ai_chans	= 16,
+		.ai_bits	= 12,
+		.have_dio	= 1,
+	},
+	[BOARD_SKEL200] = {
+		.name		= "skel-200",
+		.ai_chans	= 8,
+		.ai_bits	= 16,
+	},
 };
 
 /* this structure is for data unique to this hardware driver.  If
@@ -394,22 +395,6 @@ static int skel_dio_insn_config(struct comedi_device *dev,
 	return insn->n;
 }
 
-static const struct skel_board *skel_find_pci_board(struct pci_dev *pcidev)
-{
-	unsigned int i;
-
-/*
- * This example code assumes all the entries in skel_boards[] are PCI boards
- * and all use the same PCI vendor ID.  If skel_boards[] contains a mixture
- * of PCI and non-PCI boards, this loop should skip over the non-PCI boards.
- */
-	for (i = 0; i < ARRAY_SIZE(skel_boards); i++)
-		if (/* skel_boards[i].bustype == pci_bustype && */
-		    pcidev->device == skel_boards[i].devid)
-			return &skel_boards[i];
-	return NULL;
-}
-
 /*
  * Handle common part of skel_attach() and skel_auto_attach().
  */
@@ -496,10 +481,12 @@ static int skel_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	thisboard = comedi_board(dev);
 
-/*
- * Initialize dev->board_name.
- */
-	dev->board_name = thisboard->name;
+	/*
+	 * The dev->board_name is initialized by the comedi core before
+	 * calling the (*attach) function. It can be optionally set by
+	 * the driver if additional probing has been done.
+	 */
+	/* dev->board_name = thisboard->name; */
 
 	/* Allocate the private data */
 	devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
@@ -541,16 +528,13 @@ static int skel_attach(struct comedi_device *dev, struct comedi_devconfig *it)
  * comedi_usb_auto_config(), etc.) to handle devices that can be attached
  * to the Comedi core automatically without the COMEDI_DEVCONFIG ioctl.
  *
- * The context parameter is usually unused, but if the driver called
- * comedi_auto_config() directly instead of the comedi_pci_auto_config()
- * wrapper function, this will be a copy of the context passed to
- * comedi_auto_config().
+ * The context parameter is driver dependent.
  */
 static int skel_auto_attach(struct comedi_device *dev,
-				      unsigned long context)
+			    unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct skel_board *thisboard;
+	const struct skel_board *thisboard = NULL;
 	struct skel_private *devpriv;
 	int ret;
 
@@ -558,12 +542,18 @@ static int skel_auto_attach(struct comedi_device *dev,
 	if (!IS_ENABLED(CONFIG_COMEDI_PCI_DRIVERS))
 		return -EINVAL;
 
-	/* Find a matching board in skel_boards[]. */
-	thisboard = skel_find_pci_board(pcidev);
-	if (!thisboard) {
-		dev_err(dev->class_dev, "BUG! cannot determine board type!\n");
-		return -EINVAL;
-	}
+	/*
+	 * In this example, the _auto_attach is for a PCI device.
+	 *
+	 * The 'context' passed to this function is the id->driver_data
+	 * associated with the PCI device found in the id_table during
+	 * the modprobe. This 'context' is the index of the entry in
+	 * skel_boards[i] that contains the boardinfo for the PCI device.
+	 */
+	if (context < ARRAY_SIZE(skel_boards))
+		thisboard = &skel_boards[context];
+	if (!thisboard)
+		return -ENODEV;
 
 	/*
 	 * Point the struct comedi_device to the matching board info
@@ -579,7 +569,7 @@ static int skel_auto_attach(struct comedi_device *dev,
 	dev->private = devpriv;
 
 	/* Enable the PCI device. */
-	ret = comedi_pci_enable(pcidev, dev->board_name);
+	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
 
@@ -618,7 +608,6 @@ static void skel_detach(struct comedi_device *dev)
 {
 	const struct skel_board *thisboard = comedi_board(dev);
 	struct skel_private *devpriv = dev->private;
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 
 	if (!thisboard || !devpriv)
 		return;
@@ -638,17 +627,17 @@ static void skel_detach(struct comedi_device *dev)
 		 * If PCI device enabled by _auto_attach() (or _attach()),
 		 * disable it here.
 		 */
-		if (pcidev && dev->iobase)
-			comedi_pci_disable(pcidev);
+		comedi_pci_disable(dev);
 	} else {
 		/*
 		 * ISA board
 		 *
-		 * If I/O regions successfully requested by _attach(),
-		 * release them here.
+		 * Release the first I/O region requested during the
+		 * _attach(). This is safe to call even if the request
+		 * failed. If any additional I/O regions are requested
+		 * they need to be released by the driver.
 		 */
-		if (dev->iobase)
-			release_region(dev->iobase, SKEL_SIZE);
+		comedi_legacy_detach(dev);
 	}
 }
 
@@ -689,28 +678,33 @@ static struct comedi_driver skel_driver = {
 
 #ifdef CONFIG_COMEDI_PCI_DRIVERS
 
-/* This is used by modprobe to translate PCI IDs to drivers.  Should
- * only be used for PCI and ISA-PnP devices */
-/* Please add your PCI vendor ID to comedidev.h, and it will be forwarded
- * upstream. */
-#define PCI_VENDOR_ID_SKEL 0xdafe
+static int skel_pci_probe(struct pci_dev *dev,
+			  const struct pci_device_id *id)
+{
+	return comedi_pci_auto_config(dev, &skel_driver, id->driver_data);
+}
+
+/*
+ * Please add your PCI vendor ID to comedidev.h, and it will
+ * be forwarded upstream.
+ */
+#define PCI_VENDOR_ID_SKEL	0xdafe
+
+/*
+ * This is used by modprobe to translate PCI IDs to drivers.
+ * Should only be used for PCI and ISA-PnP devices
+ */
 static DEFINE_PCI_DEVICE_TABLE(skel_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_SKEL, 0x0100) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_SKEL, 0x0200) },
+	{ PCI_VDEVICE(SKEL, 0x0100), BOARD_SKEL100 },
+	{ PCI_VDEVICE(SKEL, 0x0200), BOARD_SKEL200 },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, skel_pci_table);
 
-static int skel_pci_probe(struct pci_dev *dev,
-					   const struct pci_device_id *ent)
-{
-	return comedi_pci_auto_config(dev, &skel_driver);
-}
-
 static struct pci_driver skel_pci_driver = {
-	.name = "dummy",
-	.id_table = skel_pci_table,
-	.probe = &skel_pci_probe,
+	.name		= "dummy",
+	.id_table	= skel_pci_table,
+	.probe		= skel_pci_probe,
 	.remove		= comedi_pci_auto_unconfig,
 };
 module_comedi_pci_driver(skel_driver, skel_pci_driver);
diff --git a/drivers/staging/comedi/drivers/ssv_dnp.c b/drivers/staging/comedi/drivers/ssv_dnp.c
index afa4016f906..a76df092a57 100644
--- a/drivers/staging/comedi/drivers/ssv_dnp.c
+++ b/drivers/staging/comedi/drivers/ssv_dnp.c
@@ -167,8 +167,6 @@ static int dnp_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	struct comedi_subdevice *s;
 	int ret;
 
-	dev->board_name = dev->driver->driver_name;
-
 	ret = comedi_alloc_subdevices(dev, 1);
 	if (ret)
 		return ret;
diff --git a/drivers/staging/comedi/drivers/unioxx5.c b/drivers/staging/comedi/drivers/unioxx5.c
index 74b974bf103..0c243477cbe 100644
--- a/drivers/staging/comedi/drivers/unioxx5.c
+++ b/drivers/staging/comedi/drivers/unioxx5.c
@@ -367,24 +367,24 @@ static int unioxx5_insn_config(struct comedi_device *dev,
 }
 
 /* initializing subdevice with given address */
-static int __unioxx5_subdev_init(struct comedi_subdevice *subdev,
-				 int subdev_iobase, int minor)
+static int __unioxx5_subdev_init(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 int iobase)
 {
 	struct unioxx5_subd_priv *usp;
 	int i, to, ndef_flag = 0;
-
-	if (!request_region(subdev_iobase, UNIOXX5_SIZE, DRIVER_NAME)) {
-		dev_err(subdev->class_dev,
-			"comedi%d: I/O port conflict\n", minor);
-		return -EIO;
-	}
+	int ret;
 
 	usp = kzalloc(sizeof(*usp), GFP_KERNEL);
 	if (usp == NULL)
-		return -1;
+		return -ENOMEM;
 
-	usp->usp_iobase = subdev_iobase;
-	dev_info(subdev->class_dev, "comedi%d: |", minor);
+	ret = __comedi_request_region(dev, iobase, UNIOXX5_SIZE);
+	if (ret) {
+		kfree(usp);
+		return ret;
+	}
+	usp->usp_iobase = iobase;
 
 	/* defining modules types */
 	for (i = 0; i < 12; i++) {
@@ -392,14 +392,14 @@ static int __unioxx5_subdev_init(struct comedi_subdevice *subdev,
 
 		__unioxx5_analog_config(usp, i * 2);
 		/* sends channel number to card */
-		outb(i + 1, subdev_iobase + 5);
-		outb('H', subdev_iobase + 6);	/* requests EEPROM world */
-		while (!(inb(subdev_iobase + 0) & TxBE))
+		outb(i + 1, iobase + 5);
+		outb('H', iobase + 6);	/* requests EEPROM world */
+		while (!(inb(iobase + 0) & TxBE))
 			;	/* waits while writting will be allowed */
-		outb(0, subdev_iobase + 6);
+		outb(0, iobase + 6);
 
 		/* waits while reading of two bytes will be allowed */
-		while (!(inb(subdev_iobase + 0) & Rx2CA)) {
+		while (!(inb(iobase + 0) & Rx2CA)) {
 			if (--to <= 0) {
 				ndef_flag = 1;
 				break;
@@ -410,25 +410,22 @@ static int __unioxx5_subdev_init(struct comedi_subdevice *subdev,
 			usp->usp_module_type[i] = 0;
 			ndef_flag = 0;
 		} else
-			usp->usp_module_type[i] = inb(subdev_iobase + 6);
+			usp->usp_module_type[i] = inb(iobase + 6);
 
-		printk(" [%d] 0x%02x |", i, usp->usp_module_type[i]);
 		udelay(1);
 	}
 
-	printk("\n");
-
 	/* initial subdevice for digital or analog i/o */
-	subdev->type = COMEDI_SUBD_DIO;
-	subdev->private = usp;
-	subdev->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-	subdev->n_chan = UNIOXX5_NUM_OF_CHANS;
-	subdev->maxdata = 0xFFF;
-	subdev->range_table = &range_digital;
-	subdev->insn_read = unioxx5_subdev_read;
-	subdev->insn_write = unioxx5_subdev_write;
+	s->type = COMEDI_SUBD_DIO;
+	s->private = usp;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->n_chan = UNIOXX5_NUM_OF_CHANS;
+	s->maxdata = 0xFFF;
+	s->range_table = &range_digital;
+	s->insn_read = unioxx5_subdev_read;
+	s->insn_write = unioxx5_subdev_write;
 	/* for digital modules only!!! */
-	subdev->insn_config = unioxx5_insn_config;
+	s->insn_config = unioxx5_insn_config;
 
 	return 0;
 }
@@ -436,13 +433,13 @@ static int __unioxx5_subdev_init(struct comedi_subdevice *subdev,
 static int unioxx5_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
+	struct comedi_subdevice *s;
 	int iobase, i, n_subd;
 	int id, num, ba;
 	int ret;
 
 	iobase = it->options[0];
 
-	dev->board_name = DRIVER_NAME;
 	dev->iobase = iobase;
 	iobase += UNIOXX5_SUBDEV_BASE;
 
@@ -470,9 +467,10 @@ static int unioxx5_attach(struct comedi_device *dev,
 
 	/* initializing each of for same subdevices */
 	for (i = 0; i < n_subd; i++, iobase += UNIOXX5_SUBDEV_ODDS) {
-		if (__unioxx5_subdev_init(&dev->subdevices[i], iobase,
-					  dev->minor) < 0)
-			return -1;
+		s = &dev->subdevices[i];
+		ret = __unioxx5_subdev_init(dev, s, iobase);
+		if (ret)
+			return ret;
 	}
 
 	return 0;
diff --git a/drivers/staging/comedi/drivers/usbdux.c b/drivers/staging/comedi/drivers/usbdux.c
index 6aac1f60bc4..6f5da67e26c 100644
--- a/drivers/staging/comedi/drivers/usbdux.c
+++ b/drivers/staging/comedi/drivers/usbdux.c
@@ -56,7 +56,7 @@ sampling rate. If you sample two channels you get 4kHz and so on.
  *       functions firmware upload is by fxload and no longer by comedi (due to
  *       enumeration)
  * 0.97: USB IDs received, adjusted table
- * 0.98: SMP, locking, memroy alloc: moved all usb memory alloc
+ * 0.98: SMP, locking, memory alloc: moved all usb memory alloc
  *       to the usb subsystem and moved all comedi related memory
  *       alloc to comedi.
  *       | kernel | registration | usbdux-usb | usbdux-comedi | comedi |
@@ -250,26 +250,26 @@ struct usbduxsub {
 	/* pointer to the usb-device */
 	struct usb_device *usbdev;
 	/* actual number of in-buffers */
-	int numOfInBuffers;
+	int num_in_buffers;
 	/* actual number of out-buffers */
-	int numOfOutBuffers;
+	int num_out_buffers;
 	/* ISO-transfer handling: buffers */
-	struct urb **urbIn;
-	struct urb **urbOut;
+	struct urb **urb_in;
+	struct urb **urb_out;
 	/* pwm-transfer handling */
-	struct urb *urbPwm;
+	struct urb *urb_pwm;
 	/* PWM period */
-	unsigned int pwmPeriod;
+	unsigned int pwm_period;
 	/* PWM internal delay for the GPIF in the FX2 */
-	int8_t pwmDelay;
+	int8_t pwn_delay;
 	/* size of the PWM buffer which holds the bit pattern */
-	int sizePwmBuf;
+	int size_pwm_buf;
 	/* input buffer for the ISO-transfer */
-	int16_t *inBuffer;
+	int16_t *in_buffer;
 	/* input buffer for single insn */
-	int16_t *insnBuffer;
+	int16_t *insn_buffer;
 	/* output buffer for single DA outputs */
-	int16_t *outBuffer;
+	int16_t *out_buffer;
 	/* interface number */
 	int ifnum;
 	/* interface structure in 2.6 */
@@ -319,17 +319,17 @@ static DEFINE_SEMAPHORE(start_stop_sem);
  * Stops the data acquision
  * It should be safe to call this function from any context
  */
-static int usbduxsub_unlink_InURBs(struct usbduxsub *usbduxsub_tmp)
+static int usbduxsub_unlink_inurbs(struct usbduxsub *usbduxsub_tmp)
 {
 	int i = 0;
 	int err = 0;
 
-	if (usbduxsub_tmp && usbduxsub_tmp->urbIn) {
-		for (i = 0; i < usbduxsub_tmp->numOfInBuffers; i++) {
-			if (usbduxsub_tmp->urbIn[i]) {
+	if (usbduxsub_tmp && usbduxsub_tmp->urb_in) {
+		for (i = 0; i < usbduxsub_tmp->num_in_buffers; i++) {
+			if (usbduxsub_tmp->urb_in[i]) {
 				/* We wait here until all transfers have been
 				 * cancelled. */
-				usb_kill_urb(usbduxsub_tmp->urbIn[i]);
+				usb_kill_urb(usbduxsub_tmp->urb_in[i]);
 			}
 			dev_dbg(&usbduxsub_tmp->interface->dev,
 				"comedi: usbdux: unlinked InURB %d, err=%d\n",
@@ -356,7 +356,7 @@ static int usbdux_ai_stop(struct usbduxsub *this_usbduxsub, int do_unlink)
 
 	if (do_unlink) {
 		/* stop aquistion */
-		ret = usbduxsub_unlink_InURBs(this_usbduxsub);
+		ret = usbduxsub_unlink_inurbs(this_usbduxsub);
 	}
 
 	this_usbduxsub->ai_cmd_running = 0;
@@ -394,7 +394,7 @@ static int usbdux_ai_cancel(struct comedi_device *dev,
 }
 
 /* analogue IN - interrupt service routine */
-static void usbduxsub_ai_IsocIrq(struct urb *urb)
+static void usbduxsub_ai_isoc_irq(struct urb *urb)
 {
 	int i, err, n;
 	struct usbduxsub *this_usbduxsub;
@@ -412,7 +412,7 @@ static void usbduxsub_ai_IsocIrq(struct urb *urb)
 	switch (urb->status) {
 	case 0:
 		/* copy the result in the transfer buffer */
-		memcpy(this_usbduxsub->inBuffer,
+		memcpy(this_usbduxsub->in_buffer,
 		       urb->transfer_buffer, SIZEINBUF);
 		break;
 	case -EILSEQ:
@@ -517,11 +517,11 @@ static void usbduxsub_ai_IsocIrq(struct urb *urb)
 		if (CR_RANGE(s->async->cmd.chanlist[i]) <= 1) {
 			err = comedi_buf_put
 			    (s->async,
-			     le16_to_cpu(this_usbduxsub->inBuffer[i]) ^ 0x800);
+			     le16_to_cpu(this_usbduxsub->in_buffer[i]) ^ 0x800);
 		} else {
 			err = comedi_buf_put
 			    (s->async,
-			     le16_to_cpu(this_usbduxsub->inBuffer[i]));
+			     le16_to_cpu(this_usbduxsub->in_buffer[i]));
 		}
 		if (unlikely(err == 0)) {
 			/* buffer overflow */
@@ -534,15 +534,15 @@ static void usbduxsub_ai_IsocIrq(struct urb *urb)
 	comedi_event(this_usbduxsub->comedidev, s);
 }
 
-static int usbduxsub_unlink_OutURBs(struct usbduxsub *usbduxsub_tmp)
+static int usbduxsub_unlink_outurbs(struct usbduxsub *usbduxsub_tmp)
 {
 	int i = 0;
 	int err = 0;
 
-	if (usbduxsub_tmp && usbduxsub_tmp->urbOut) {
-		for (i = 0; i < usbduxsub_tmp->numOfOutBuffers; i++) {
-			if (usbduxsub_tmp->urbOut[i])
-				usb_kill_urb(usbduxsub_tmp->urbOut[i]);
+	if (usbduxsub_tmp && usbduxsub_tmp->urb_out) {
+		for (i = 0; i < usbduxsub_tmp->num_out_buffers; i++) {
+			if (usbduxsub_tmp->urb_out[i])
+				usb_kill_urb(usbduxsub_tmp->urb_out[i]);
 
 			dev_dbg(&usbduxsub_tmp->interface->dev,
 				"comedi: usbdux: unlinked OutURB %d: res=%d\n",
@@ -564,7 +564,7 @@ static int usbdux_ao_stop(struct usbduxsub *this_usbduxsub, int do_unlink)
 	dev_dbg(&this_usbduxsub->interface->dev, "comedi: usbdux_ao_cancel\n");
 
 	if (do_unlink)
-		ret = usbduxsub_unlink_OutURBs(this_usbduxsub);
+		ret = usbduxsub_unlink_outurbs(this_usbduxsub);
 
 	this_usbduxsub->ao_cmd_running = 0;
 
@@ -593,7 +593,7 @@ static int usbdux_ao_cancel(struct comedi_device *dev,
 	return res;
 }
 
-static void usbduxsub_ao_IsocIrq(struct urb *urb)
+static void usbduxsub_ao_isoc_irq(struct urb *urb)
 {
 	int i, ret;
 	int8_t *datap;
@@ -798,7 +798,7 @@ static int usbduxsub_stop(struct usbduxsub *usbduxsub)
 
 static int usbduxsub_upload(struct usbduxsub *usbduxsub,
 			    uint8_t *local_transfer_buffer,
-			    unsigned int startAddr, unsigned int len)
+			    unsigned int start_addr, unsigned int len)
 {
 	int errcode;
 
@@ -809,7 +809,7 @@ static int usbduxsub_upload(struct usbduxsub *usbduxsub,
 				  /* bmRequestType */
 				  VENDOR_DIR_OUT,
 				  /* value */
-				  startAddr,
+				  start_addr,
 				  /* index */
 				  0x0000,
 				  /* our local safe buffer */
@@ -828,24 +828,24 @@ static int usbduxsub_upload(struct usbduxsub *usbduxsub,
 
 #define FIRMWARE_MAX_LEN 0x2000
 
-static int firmwareUpload(struct usbduxsub *usbduxsub,
-			  const u8 *firmwareBinary, int sizeFirmware)
+static int firmware_upload(struct usbduxsub *usbduxsub,
+			  const u8 *firmware_binary, int size_firmware)
 {
 	int ret;
-	uint8_t *fwBuf;
+	uint8_t *fw_buf;
 
-	if (!firmwareBinary)
+	if (!firmware_binary)
 		return 0;
 
-	if (sizeFirmware > FIRMWARE_MAX_LEN) {
+	if (size_firmware > FIRMWARE_MAX_LEN) {
 		dev_err(&usbduxsub->interface->dev,
 			"usbdux firmware binary it too large for FX2.\n");
 		return -ENOMEM;
 	}
 
 	/* we generate a local buffer for the firmware */
-	fwBuf = kmemdup(firmwareBinary, sizeFirmware, GFP_KERNEL);
-	if (!fwBuf) {
+	fw_buf = kmemdup(firmware_binary, size_firmware, GFP_KERNEL);
+	if (!fw_buf) {
 		dev_err(&usbduxsub->interface->dev,
 			"comedi_: mem alloc for firmware failed\n");
 		return -ENOMEM;
@@ -855,81 +855,81 @@ static int firmwareUpload(struct usbduxsub *usbduxsub,
 	if (ret < 0) {
 		dev_err(&usbduxsub->interface->dev,
 			"comedi_: can not stop firmware\n");
-		kfree(fwBuf);
+		kfree(fw_buf);
 		return ret;
 	}
 
-	ret = usbduxsub_upload(usbduxsub, fwBuf, 0, sizeFirmware);
+	ret = usbduxsub_upload(usbduxsub, fw_buf, 0, size_firmware);
 	if (ret < 0) {
 		dev_err(&usbduxsub->interface->dev,
 			"comedi_: firmware upload failed\n");
-		kfree(fwBuf);
+		kfree(fw_buf);
 		return ret;
 	}
 	ret = usbduxsub_start(usbduxsub);
 	if (ret < 0) {
 		dev_err(&usbduxsub->interface->dev,
 			"comedi_: can not start firmware\n");
-		kfree(fwBuf);
+		kfree(fw_buf);
 		return ret;
 	}
-	kfree(fwBuf);
+	kfree(fw_buf);
 	return 0;
 }
 
-static int usbduxsub_submit_InURBs(struct usbduxsub *usbduxsub)
+static int usbduxsub_submit_inurbs(struct usbduxsub *usbduxsub)
 {
-	int i, errFlag;
+	int i, err_flag;
 
 	if (!usbduxsub)
 		return -EFAULT;
 
 	/* Submit all URBs and start the transfer on the bus */
-	for (i = 0; i < usbduxsub->numOfInBuffers; i++) {
+	for (i = 0; i < usbduxsub->num_in_buffers; i++) {
 		/* in case of a resubmission after an unlink... */
-		usbduxsub->urbIn[i]->interval = usbduxsub->ai_interval;
-		usbduxsub->urbIn[i]->context = usbduxsub->comedidev;
-		usbduxsub->urbIn[i]->dev = usbduxsub->usbdev;
-		usbduxsub->urbIn[i]->status = 0;
-		usbduxsub->urbIn[i]->transfer_flags = URB_ISO_ASAP;
+		usbduxsub->urb_in[i]->interval = usbduxsub->ai_interval;
+		usbduxsub->urb_in[i]->context = usbduxsub->comedidev;
+		usbduxsub->urb_in[i]->dev = usbduxsub->usbdev;
+		usbduxsub->urb_in[i]->status = 0;
+		usbduxsub->urb_in[i]->transfer_flags = URB_ISO_ASAP;
 		dev_dbg(&usbduxsub->interface->dev,
 			"comedi%d: submitting in-urb[%d]: %p,%p intv=%d\n",
 			usbduxsub->comedidev->minor, i,
-			(usbduxsub->urbIn[i]->context),
-			(usbduxsub->urbIn[i]->dev),
-			(usbduxsub->urbIn[i]->interval));
-		errFlag = usb_submit_urb(usbduxsub->urbIn[i], GFP_ATOMIC);
-		if (errFlag) {
+			(usbduxsub->urb_in[i]->context),
+			(usbduxsub->urb_in[i]->dev),
+			(usbduxsub->urb_in[i]->interval));
+		err_flag = usb_submit_urb(usbduxsub->urb_in[i], GFP_ATOMIC);
+		if (err_flag) {
 			dev_err(&usbduxsub->interface->dev,
 				"comedi_: ai: usb_submit_urb(%d) error %d\n",
-				i, errFlag);
-			return errFlag;
+				i, err_flag);
+			return err_flag;
 		}
 	}
 	return 0;
 }
 
-static int usbduxsub_submit_OutURBs(struct usbduxsub *usbduxsub)
+static int usbduxsub_submit_outurbs(struct usbduxsub *usbduxsub)
 {
-	int i, errFlag;
+	int i, err_flag;
 
 	if (!usbduxsub)
 		return -EFAULT;
 
-	for (i = 0; i < usbduxsub->numOfOutBuffers; i++) {
+	for (i = 0; i < usbduxsub->num_out_buffers; i++) {
 		dev_dbg(&usbduxsub->interface->dev,
 			"comedi_: submitting out-urb[%d]\n", i);
 		/* in case of a resubmission after an unlink... */
-		usbduxsub->urbOut[i]->context = usbduxsub->comedidev;
-		usbduxsub->urbOut[i]->dev = usbduxsub->usbdev;
-		usbduxsub->urbOut[i]->status = 0;
-		usbduxsub->urbOut[i]->transfer_flags = URB_ISO_ASAP;
-		errFlag = usb_submit_urb(usbduxsub->urbOut[i], GFP_ATOMIC);
-		if (errFlag) {
+		usbduxsub->urb_out[i]->context = usbduxsub->comedidev;
+		usbduxsub->urb_out[i]->dev = usbduxsub->usbdev;
+		usbduxsub->urb_out[i]->status = 0;
+		usbduxsub->urb_out[i]->transfer_flags = URB_ISO_ASAP;
+		err_flag = usb_submit_urb(usbduxsub->urb_out[i], GFP_ATOMIC);
+		if (err_flag) {
 			dev_err(&usbduxsub->interface->dev,
 				"comedi_: ao: usb_submit_urb(%d) error %d\n",
-				i, errFlag);
-			return errFlag;
+				i, err_flag);
+			return err_flag;
 		}
 	}
 	return 0;
@@ -940,7 +940,7 @@ static int usbdux_ai_cmdtest(struct comedi_device *dev,
 {
 	struct usbduxsub *this_usbduxsub = dev->private;
 	int err = 0, i;
-	unsigned int tmpTimer;
+	unsigned int tmp_timer;
 
 	if (!(this_usbduxsub->probed))
 		return -ENODEV;
@@ -990,7 +990,7 @@ static int usbdux_ai_cmdtest(struct comedi_device *dev,
 							 1000000 / 8 * i);
 			/* now calc the real sampling rate with all the
 			 * rounding errors */
-			tmpTimer =
+			tmp_timer =
 			    ((unsigned int)(cmd->scan_begin_arg / 125000)) *
 			    125000;
 		} else {
@@ -1001,11 +1001,11 @@ static int usbdux_ai_cmdtest(struct comedi_device *dev,
 			/*
 			 * calc the real sampling rate with the rounding errors
 			 */
-			tmpTimer = ((unsigned int)(cmd->scan_begin_arg /
+			tmp_timer = ((unsigned int)(cmd->scan_begin_arg /
 						   1000000)) * 1000000;
 		}
 		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg,
-						tmpTimer);
+						tmp_timer);
 	}
 
 	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
@@ -1081,7 +1081,7 @@ static int receive_dux_commands(struct usbduxsub *this_usbduxsub, int command)
 		result = usb_bulk_msg(this_usbduxsub->usbdev,
 				      usb_rcvbulkpipe(this_usbduxsub->usbdev,
 						      COMMAND_IN_EP),
-				      this_usbduxsub->insnBuffer, SIZEINSNBUF,
+				      this_usbduxsub->insn_buffer, SIZEINSNBUF,
 				      &nrec, BULK_TIMEOUT);
 		if (result < 0) {
 			dev_err(&this_usbduxsub->interface->dev, "comedi%d: "
@@ -1089,7 +1089,7 @@ static int receive_dux_commands(struct usbduxsub *this_usbduxsub, int command)
 				"\n", this_usbduxsub->comedidev->minor, result);
 			return result;
 		}
-		if (le16_to_cpu(this_usbduxsub->insnBuffer[0]) == command)
+		if (le16_to_cpu(this_usbduxsub->insn_buffer[0]) == command)
 			return result;
 	}
 	/* this is only reached if the data has been requested a couple of
@@ -1097,7 +1097,7 @@ static int receive_dux_commands(struct usbduxsub *this_usbduxsub, int command)
 	dev_err(&this_usbduxsub->interface->dev, "comedi%d: insn: "
 		"wrong data returned from firmware: want cmd %d, got cmd %d.\n",
 		this_usbduxsub->comedidev->minor, command,
-		le16_to_cpu(this_usbduxsub->insnBuffer[0]));
+		le16_to_cpu(this_usbduxsub->insn_buffer[0]));
 	return -EFAULT;
 }
 
@@ -1126,7 +1126,7 @@ static int usbdux_ai_inttrig(struct comedi_device *dev,
 	}
 	if (!(this_usbduxsub->ai_cmd_running)) {
 		this_usbduxsub->ai_cmd_running = 1;
-		ret = usbduxsub_submit_InURBs(this_usbduxsub);
+		ret = usbduxsub_submit_inurbs(this_usbduxsub);
 		if (ret < 0) {
 			dev_err(&this_usbduxsub->interface->dev,
 				"comedi%d: usbdux_ai_inttrig: "
@@ -1243,7 +1243,7 @@ static int usbdux_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	if (cmd->start_src == TRIG_NOW) {
 		/* enable this acquisition operation */
 		this_usbduxsub->ai_cmd_running = 1;
-		ret = usbduxsub_submit_InURBs(this_usbduxsub);
+		ret = usbduxsub_submit_inurbs(this_usbduxsub);
 		if (ret < 0) {
 			this_usbduxsub->ai_cmd_running = 0;
 			/* fixme: unlink here?? */
@@ -1311,7 +1311,7 @@ static int usbdux_ai_insn_read(struct comedi_device *dev,
 			up(&this_usbduxsub->sem);
 			return 0;
 		}
-		one = le16_to_cpu(this_usbduxsub->insnBuffer[1]);
+		one = le16_to_cpu(this_usbduxsub->insn_buffer[1]);
 		if (CR_RANGE(insn->chanspec) <= 1)
 			one = one ^ 0x800;
 
@@ -1341,7 +1341,7 @@ static int usbdux_ao_insn_read(struct comedi_device *dev,
 		return -ENODEV;
 	}
 	for (i = 0; i < insn->n; i++)
-		data[i] = this_usbduxsub->outBuffer[chan];
+		data[i] = this_usbduxsub->out_buffer[chan];
 
 	up(&this_usbduxsub->sem);
 	return i;
@@ -1384,7 +1384,7 @@ static int usbdux_ao_insn_write(struct comedi_device *dev,
 		/* one 16 bit value */
 		*((int16_t *) (this_usbduxsub->dux_commands + 2)) =
 		    cpu_to_le16(data[i]);
-		this_usbduxsub->outBuffer[chan] = data[i];
+		this_usbduxsub->out_buffer[chan] = data[i];
 		/* channel number */
 		this_usbduxsub->dux_commands[4] = (chan << 6);
 		err = send_dux_commands(this_usbduxsub, SENDDACOMMANDS);
@@ -1421,7 +1421,7 @@ static int usbdux_ao_inttrig(struct comedi_device *dev,
 	}
 	if (!(this_usbduxsub->ao_cmd_running)) {
 		this_usbduxsub->ao_cmd_running = 1;
-		ret = usbduxsub_submit_OutURBs(this_usbduxsub);
+		ret = usbduxsub_submit_outurbs(this_usbduxsub);
 		if (ret < 0) {
 			dev_err(&this_usbduxsub->interface->dev,
 				"comedi%d: usbdux_ao_inttrig: submitURB: "
@@ -1616,7 +1616,7 @@ static int usbdux_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	if (cmd->start_src == TRIG_NOW) {
 		/* enable this acquisition operation */
 		this_usbduxsub->ao_cmd_running = 1;
-		ret = usbduxsub_submit_OutURBs(this_usbduxsub);
+		ret = usbduxsub_submit_outurbs(this_usbduxsub);
 		if (ret < 0) {
 			this_usbduxsub->ao_cmd_running = 0;
 			/* fixme: unlink here?? */
@@ -1704,7 +1704,7 @@ static int usbdux_dio_insn_bits(struct comedi_device *dev,
 		return err;
 	}
 
-	data[1] = le16_to_cpu(this_usbduxsub->insnBuffer[1]);
+	data[1] = le16_to_cpu(this_usbduxsub->insn_buffer[1]);
 	up(&this_usbduxsub->sem);
 	return insn->n;
 }
@@ -1740,7 +1740,7 @@ static int usbdux_counter_read(struct comedi_device *dev,
 		return err;
 	}
 
-	data[0] = le16_to_cpu(this_usbduxsub->insnBuffer[chan + 1]);
+	data[0] = le16_to_cpu(this_usbduxsub->insn_buffer[chan + 1]);
 	up(&this_usbduxsub->sem);
 	return 1;
 }
@@ -1787,13 +1787,13 @@ static int usbdux_counter_config(struct comedi_device *dev,
 /***********************************/
 /* PWM */
 
-static int usbduxsub_unlink_PwmURBs(struct usbduxsub *usbduxsub_tmp)
+static int usbduxsub_unlink_pwm_urbs(struct usbduxsub *usbduxsub_tmp)
 {
 	int err = 0;
 
-	if (usbduxsub_tmp && usbduxsub_tmp->urbPwm) {
-		if (usbduxsub_tmp->urbPwm)
-			usb_kill_urb(usbduxsub_tmp->urbPwm);
+	if (usbduxsub_tmp && usbduxsub_tmp->urb_pwm) {
+		if (usbduxsub_tmp->urb_pwm)
+			usb_kill_urb(usbduxsub_tmp->urb_pwm);
 		dev_dbg(&usbduxsub_tmp->interface->dev,
 			"comedi: unlinked PwmURB: res=%d\n", err);
 	}
@@ -1812,7 +1812,7 @@ static int usbdux_pwm_stop(struct usbduxsub *this_usbduxsub, int do_unlink)
 
 	dev_dbg(&this_usbduxsub->interface->dev, "comedi: %s\n", __func__);
 	if (do_unlink)
-		ret = usbduxsub_unlink_PwmURBs(this_usbduxsub);
+		ret = usbduxsub_unlink_pwm_urbs(this_usbduxsub);
 
 	this_usbduxsub->pwm_cmd_running = 0;
 
@@ -1885,7 +1885,7 @@ static void usbduxsub_pwm_irq(struct urb *urb)
 	if (!(this_usbduxsub->pwm_cmd_running))
 		return;
 
-	urb->transfer_buffer_length = this_usbduxsub->sizePwmBuf;
+	urb->transfer_buffer_length = this_usbduxsub->size_pwm_buf;
 	urb->dev = this_usbduxsub->usbdev;
 	urb->status = 0;
 	if (this_usbduxsub->pwm_cmd_running) {
@@ -1905,9 +1905,9 @@ static void usbduxsub_pwm_irq(struct urb *urb)
 	}
 }
 
-static int usbduxsub_submit_PwmURBs(struct usbduxsub *usbduxsub)
+static int usbduxsub_submit_pwm_urbs(struct usbduxsub *usbduxsub)
 {
-	int errFlag;
+	int err_flag;
 
 	if (!usbduxsub)
 		return -EFAULT;
@@ -1915,19 +1915,19 @@ static int usbduxsub_submit_PwmURBs(struct usbduxsub *usbduxsub)
 	dev_dbg(&usbduxsub->interface->dev, "comedi_: submitting pwm-urb\n");
 
 	/* in case of a resubmission after an unlink... */
-	usb_fill_bulk_urb(usbduxsub->urbPwm,
+	usb_fill_bulk_urb(usbduxsub->urb_pwm,
 			  usbduxsub->usbdev,
 			  usb_sndbulkpipe(usbduxsub->usbdev, PWM_EP),
-			  usbduxsub->urbPwm->transfer_buffer,
-			  usbduxsub->sizePwmBuf, usbduxsub_pwm_irq,
+			  usbduxsub->urb_pwm->transfer_buffer,
+			  usbduxsub->size_pwm_buf, usbduxsub_pwm_irq,
 			  usbduxsub->comedidev);
 
-	errFlag = usb_submit_urb(usbduxsub->urbPwm, GFP_ATOMIC);
-	if (errFlag) {
+	err_flag = usb_submit_urb(usbduxsub->urb_pwm, GFP_ATOMIC);
+	if (err_flag) {
 		dev_err(&usbduxsub->interface->dev,
 			"comedi_: usbdux: pwm: usb_submit_urb error %d\n",
-			errFlag);
-		return errFlag;
+			err_flag);
+		return err_flag;
 	}
 	return 0;
 }
@@ -1952,8 +1952,8 @@ static int usbdux_pwm_period(struct comedi_device *dev,
 			return -EAGAIN;
 		}
 	}
-	this_usbduxsub->pwmDelay = fx2delay;
-	this_usbduxsub->pwmPeriod = period;
+	this_usbduxsub->pwn_delay = fx2delay;
+	this_usbduxsub->pwm_period = period;
 	dev_dbg(&this_usbduxsub->interface->dev, "%s: frequ=%d, period=%d\n",
 		__func__, period, fx2delay);
 	return 0;
@@ -1974,17 +1974,17 @@ static int usbdux_pwm_start(struct comedi_device *dev,
 		return 0;
 	}
 
-	this_usbduxsub->dux_commands[1] = ((int8_t) this_usbduxsub->pwmDelay);
+	this_usbduxsub->dux_commands[1] = ((int8_t) this_usbduxsub->pwn_delay);
 	ret = send_dux_commands(this_usbduxsub, SENDPWMON);
 	if (ret < 0)
 		return ret;
 
 	/* initialise the buffer */
-	for (i = 0; i < this_usbduxsub->sizePwmBuf; i++)
-		((char *)(this_usbduxsub->urbPwm->transfer_buffer))[i] = 0;
+	for (i = 0; i < this_usbduxsub->size_pwm_buf; i++)
+		((char *)(this_usbduxsub->urb_pwm->transfer_buffer))[i] = 0;
 
 	this_usbduxsub->pwm_cmd_running = 1;
-	ret = usbduxsub_submit_PwmURBs(this_usbduxsub);
+	ret = usbduxsub_submit_pwm_urbs(this_usbduxsub);
 	if (ret < 0) {
 		this_usbduxsub->pwm_cmd_running = 0;
 		return ret;
@@ -1999,7 +1999,7 @@ static int usbdux_pwm_pattern(struct comedi_device *dev,
 {
 	struct usbduxsub *this_usbduxsub = dev->private;
 	int i, szbuf;
-	char *pBuf;
+	char *p_buf;
 	char pwm_mask;
 	char sgn_mask;
 	char c;
@@ -2013,10 +2013,10 @@ static int usbdux_pwm_pattern(struct comedi_device *dev,
 	sgn_mask = (16 << channel);
 	/* this is the buffer which will be filled with the with bit */
 	/* pattern for one period */
-	szbuf = this_usbduxsub->sizePwmBuf;
-	pBuf = (char *)(this_usbduxsub->urbPwm->transfer_buffer);
+	szbuf = this_usbduxsub->size_pwm_buf;
+	p_buf = (char *)(this_usbduxsub->urb_pwm->transfer_buffer);
 	for (i = 0; i < szbuf; i++) {
-		c = *pBuf;
+		c = *p_buf;
 		/* reset bits */
 		c = c & (~pwm_mask);
 		/* set the bit as long as the index is lower than the value */
@@ -2030,7 +2030,7 @@ static int usbdux_pwm_pattern(struct comedi_device *dev,
 			/* negative value */
 			c = c | sgn_mask;
 		}
-		*(pBuf++) = c;
+		*(p_buf++) = c;
 	}
 	return 1;
 }
@@ -2102,7 +2102,7 @@ static int usbdux_pwm_config(struct comedi_device *dev,
 			"comedi%d: %s: setting period\n", dev->minor, __func__);
 		return usbdux_pwm_period(dev, s, data[1]);
 	case INSN_CONFIG_PWM_GET_PERIOD:
-		data[1] = this_usbduxsub->pwmPeriod;
+		data[1] = this_usbduxsub->pwm_period;
 		return 0;
 	case INSN_CONFIG_PWM_SET_H_BRIDGE:
 		/* value in the first byte and the sign in the second for a
@@ -2138,55 +2138,55 @@ static void tidy_up(struct usbduxsub *usbduxsub_tmp)
 
 	usbduxsub_tmp->probed = 0;
 
-	if (usbduxsub_tmp->urbIn) {
+	if (usbduxsub_tmp->urb_in) {
 		if (usbduxsub_tmp->ai_cmd_running) {
 			usbduxsub_tmp->ai_cmd_running = 0;
-			usbduxsub_unlink_InURBs(usbduxsub_tmp);
+			usbduxsub_unlink_inurbs(usbduxsub_tmp);
 		}
-		for (i = 0; i < usbduxsub_tmp->numOfInBuffers; i++) {
-			kfree(usbduxsub_tmp->urbIn[i]->transfer_buffer);
-			usbduxsub_tmp->urbIn[i]->transfer_buffer = NULL;
-			usb_kill_urb(usbduxsub_tmp->urbIn[i]);
-			usb_free_urb(usbduxsub_tmp->urbIn[i]);
-			usbduxsub_tmp->urbIn[i] = NULL;
+		for (i = 0; i < usbduxsub_tmp->num_in_buffers; i++) {
+			kfree(usbduxsub_tmp->urb_in[i]->transfer_buffer);
+			usbduxsub_tmp->urb_in[i]->transfer_buffer = NULL;
+			usb_kill_urb(usbduxsub_tmp->urb_in[i]);
+			usb_free_urb(usbduxsub_tmp->urb_in[i]);
+			usbduxsub_tmp->urb_in[i] = NULL;
 		}
-		kfree(usbduxsub_tmp->urbIn);
-		usbduxsub_tmp->urbIn = NULL;
+		kfree(usbduxsub_tmp->urb_in);
+		usbduxsub_tmp->urb_in = NULL;
 	}
-	if (usbduxsub_tmp->urbOut) {
+	if (usbduxsub_tmp->urb_out) {
 		if (usbduxsub_tmp->ao_cmd_running) {
 			usbduxsub_tmp->ao_cmd_running = 0;
-			usbduxsub_unlink_OutURBs(usbduxsub_tmp);
+			usbduxsub_unlink_outurbs(usbduxsub_tmp);
 		}
-		for (i = 0; i < usbduxsub_tmp->numOfOutBuffers; i++) {
-			kfree(usbduxsub_tmp->urbOut[i]->transfer_buffer);
-			usbduxsub_tmp->urbOut[i]->transfer_buffer = NULL;
-			if (usbduxsub_tmp->urbOut[i]) {
-				usb_kill_urb(usbduxsub_tmp->urbOut[i]);
-				usb_free_urb(usbduxsub_tmp->urbOut[i]);
-				usbduxsub_tmp->urbOut[i] = NULL;
+		for (i = 0; i < usbduxsub_tmp->num_out_buffers; i++) {
+			kfree(usbduxsub_tmp->urb_out[i]->transfer_buffer);
+			usbduxsub_tmp->urb_out[i]->transfer_buffer = NULL;
+			if (usbduxsub_tmp->urb_out[i]) {
+				usb_kill_urb(usbduxsub_tmp->urb_out[i]);
+				usb_free_urb(usbduxsub_tmp->urb_out[i]);
+				usbduxsub_tmp->urb_out[i] = NULL;
 			}
 		}
-		kfree(usbduxsub_tmp->urbOut);
-		usbduxsub_tmp->urbOut = NULL;
+		kfree(usbduxsub_tmp->urb_out);
+		usbduxsub_tmp->urb_out = NULL;
 	}
-	if (usbduxsub_tmp->urbPwm) {
+	if (usbduxsub_tmp->urb_pwm) {
 		if (usbduxsub_tmp->pwm_cmd_running) {
 			usbduxsub_tmp->pwm_cmd_running = 0;
-			usbduxsub_unlink_PwmURBs(usbduxsub_tmp);
+			usbduxsub_unlink_pwm_urbs(usbduxsub_tmp);
 		}
-		kfree(usbduxsub_tmp->urbPwm->transfer_buffer);
-		usbduxsub_tmp->urbPwm->transfer_buffer = NULL;
-		usb_kill_urb(usbduxsub_tmp->urbPwm);
-		usb_free_urb(usbduxsub_tmp->urbPwm);
-		usbduxsub_tmp->urbPwm = NULL;
-	}
-	kfree(usbduxsub_tmp->inBuffer);
-	usbduxsub_tmp->inBuffer = NULL;
-	kfree(usbduxsub_tmp->insnBuffer);
-	usbduxsub_tmp->insnBuffer = NULL;
-	kfree(usbduxsub_tmp->outBuffer);
-	usbduxsub_tmp->outBuffer = NULL;
+		kfree(usbduxsub_tmp->urb_pwm->transfer_buffer);
+		usbduxsub_tmp->urb_pwm->transfer_buffer = NULL;
+		usb_kill_urb(usbduxsub_tmp->urb_pwm);
+		usb_free_urb(usbduxsub_tmp->urb_pwm);
+		usbduxsub_tmp->urb_pwm = NULL;
+	}
+	kfree(usbduxsub_tmp->in_buffer);
+	usbduxsub_tmp->in_buffer = NULL;
+	kfree(usbduxsub_tmp->insn_buffer);
+	usbduxsub_tmp->insn_buffer = NULL;
+	kfree(usbduxsub_tmp->out_buffer);
+	usbduxsub_tmp->out_buffer = NULL;
 	kfree(usbduxsub_tmp->dac_commands);
 	usbduxsub_tmp->dac_commands = NULL;
 	kfree(usbduxsub_tmp->dux_commands);
@@ -2207,8 +2207,6 @@ static int usbdux_attach_common(struct comedi_device *dev,
 	/* pointer back to the corresponding comedi device */
 	udev->comedidev = dev;
 
-	dev->board_name = "usbdux";
-
 	/* set number of subdevices */
 	if (udev->high_speed) {
 		/* with pwm */
@@ -2309,7 +2307,7 @@ static int usbdux_attach_common(struct comedi_device *dev,
 		s->subdev_flags = SDF_WRITABLE | SDF_PWM_HBRIDGE;
 		s->n_chan = 8;
 		/* this defines the max duty cycle resolution */
-		s->maxdata = udev->sizePwmBuf;
+		s->maxdata = udev->size_pwm_buf;
 		s->insn_write = usbdux_pwm_write;
 		s->insn_read = usbdux_pwm_read;
 		s->insn_config = usbdux_pwm_config;
@@ -2388,7 +2386,7 @@ static void usbdux_firmware_request_complete_handler(const struct firmware *fw,
 	 * we need to upload the firmware here because fw will be
 	 * freed once we've left this function
 	 */
-	ret = firmwareUpload(usbduxsub_tmp, fw->data, fw->size);
+	ret = firmware_upload(usbduxsub_tmp, fw->data, fw->size);
 
 	if (ret) {
 		dev_err(&uinterf->dev,
@@ -2464,22 +2462,22 @@ static int usbdux_usb_probe(struct usb_interface *uinterf,
 		return -ENOMEM;
 	}
 	/* create space for the in buffer and set it to zero */
-	usbduxsub[index].inBuffer = kzalloc(SIZEINBUF, GFP_KERNEL);
-	if (!(usbduxsub[index].inBuffer)) {
+	usbduxsub[index].in_buffer = kzalloc(SIZEINBUF, GFP_KERNEL);
+	if (!(usbduxsub[index].in_buffer)) {
 		tidy_up(&(usbduxsub[index]));
 		up(&start_stop_sem);
 		return -ENOMEM;
 	}
 	/* create space of the instruction buffer */
-	usbduxsub[index].insnBuffer = kzalloc(SIZEINSNBUF, GFP_KERNEL);
-	if (!(usbduxsub[index].insnBuffer)) {
+	usbduxsub[index].insn_buffer = kzalloc(SIZEINSNBUF, GFP_KERNEL);
+	if (!(usbduxsub[index].insn_buffer)) {
 		tidy_up(&(usbduxsub[index]));
 		up(&start_stop_sem);
 		return -ENOMEM;
 	}
 	/* create space for the outbuffer */
-	usbduxsub[index].outBuffer = kzalloc(SIZEOUTBUF, GFP_KERNEL);
-	if (!(usbduxsub[index].outBuffer)) {
+	usbduxsub[index].out_buffer = kzalloc(SIZEOUTBUF, GFP_KERNEL);
+	if (!(usbduxsub[index].out_buffer)) {
 		tidy_up(&(usbduxsub[index]));
 		up(&start_stop_sem);
 		return -ENOMEM;
@@ -2496,124 +2494,124 @@ static int usbdux_usb_probe(struct usb_interface *uinterf,
 		return -ENODEV;
 	}
 	if (usbduxsub[index].high_speed)
-		usbduxsub[index].numOfInBuffers = NUMOFINBUFFERSHIGH;
+		usbduxsub[index].num_in_buffers = NUMOFINBUFFERSHIGH;
 	else
-		usbduxsub[index].numOfInBuffers = NUMOFINBUFFERSFULL;
+		usbduxsub[index].num_in_buffers = NUMOFINBUFFERSFULL;
 
-	usbduxsub[index].urbIn =
-		kcalloc(usbduxsub[index].numOfInBuffers, sizeof(struct urb *),
+	usbduxsub[index].urb_in =
+		kcalloc(usbduxsub[index].num_in_buffers, sizeof(struct urb *),
 			GFP_KERNEL);
-	if (!(usbduxsub[index].urbIn)) {
+	if (!(usbduxsub[index].urb_in)) {
 		tidy_up(&(usbduxsub[index]));
 		up(&start_stop_sem);
 		return -ENOMEM;
 	}
-	for (i = 0; i < usbduxsub[index].numOfInBuffers; i++) {
+	for (i = 0; i < usbduxsub[index].num_in_buffers; i++) {
 		/* one frame: 1ms */
-		usbduxsub[index].urbIn[i] = usb_alloc_urb(1, GFP_KERNEL);
-		if (usbduxsub[index].urbIn[i] == NULL) {
+		usbduxsub[index].urb_in[i] = usb_alloc_urb(1, GFP_KERNEL);
+		if (usbduxsub[index].urb_in[i] == NULL) {
 			dev_err(dev, "comedi_: usbdux%d: "
 				"Could not alloc. urb(%d)\n", index, i);
 			tidy_up(&(usbduxsub[index]));
 			up(&start_stop_sem);
 			return -ENOMEM;
 		}
-		usbduxsub[index].urbIn[i]->dev = usbduxsub[index].usbdev;
+		usbduxsub[index].urb_in[i]->dev = usbduxsub[index].usbdev;
 		/* will be filled later with a pointer to the comedi-device */
 		/* and ONLY then the urb should be submitted */
-		usbduxsub[index].urbIn[i]->context = NULL;
-		usbduxsub[index].urbIn[i]->pipe =
+		usbduxsub[index].urb_in[i]->context = NULL;
+		usbduxsub[index].urb_in[i]->pipe =
 		    usb_rcvisocpipe(usbduxsub[index].usbdev, ISOINEP);
-		usbduxsub[index].urbIn[i]->transfer_flags = URB_ISO_ASAP;
-		usbduxsub[index].urbIn[i]->transfer_buffer =
+		usbduxsub[index].urb_in[i]->transfer_flags = URB_ISO_ASAP;
+		usbduxsub[index].urb_in[i]->transfer_buffer =
 		    kzalloc(SIZEINBUF, GFP_KERNEL);
-		if (!(usbduxsub[index].urbIn[i]->transfer_buffer)) {
+		if (!(usbduxsub[index].urb_in[i]->transfer_buffer)) {
 			tidy_up(&(usbduxsub[index]));
 			up(&start_stop_sem);
 			return -ENOMEM;
 		}
-		usbduxsub[index].urbIn[i]->complete = usbduxsub_ai_IsocIrq;
-		usbduxsub[index].urbIn[i]->number_of_packets = 1;
-		usbduxsub[index].urbIn[i]->transfer_buffer_length = SIZEINBUF;
-		usbduxsub[index].urbIn[i]->iso_frame_desc[0].offset = 0;
-		usbduxsub[index].urbIn[i]->iso_frame_desc[0].length = SIZEINBUF;
+		usbduxsub[index].urb_in[i]->complete = usbduxsub_ai_isoc_irq;
+		usbduxsub[index].urb_in[i]->number_of_packets = 1;
+		usbduxsub[index].urb_in[i]->transfer_buffer_length = SIZEINBUF;
+		usbduxsub[index].urb_in[i]->iso_frame_desc[0].offset = 0;
+		usbduxsub[index].urb_in[i]->iso_frame_desc[0].length = SIZEINBUF;
 	}
 
 	/* out */
 	if (usbduxsub[index].high_speed)
-		usbduxsub[index].numOfOutBuffers = NUMOFOUTBUFFERSHIGH;
+		usbduxsub[index].num_out_buffers = NUMOFOUTBUFFERSHIGH;
 	else
-		usbduxsub[index].numOfOutBuffers = NUMOFOUTBUFFERSFULL;
+		usbduxsub[index].num_out_buffers = NUMOFOUTBUFFERSFULL;
 
-	usbduxsub[index].urbOut =
-		kcalloc(usbduxsub[index].numOfOutBuffers, sizeof(struct urb *),
+	usbduxsub[index].urb_out =
+		kcalloc(usbduxsub[index].num_out_buffers, sizeof(struct urb *),
 			GFP_KERNEL);
-	if (!(usbduxsub[index].urbOut)) {
+	if (!(usbduxsub[index].urb_out)) {
 		tidy_up(&(usbduxsub[index]));
 		up(&start_stop_sem);
 		return -ENOMEM;
 	}
-	for (i = 0; i < usbduxsub[index].numOfOutBuffers; i++) {
+	for (i = 0; i < usbduxsub[index].num_out_buffers; i++) {
 		/* one frame: 1ms */
-		usbduxsub[index].urbOut[i] = usb_alloc_urb(1, GFP_KERNEL);
-		if (usbduxsub[index].urbOut[i] == NULL) {
+		usbduxsub[index].urb_out[i] = usb_alloc_urb(1, GFP_KERNEL);
+		if (usbduxsub[index].urb_out[i] == NULL) {
 			dev_err(dev, "comedi_: usbdux%d: "
 				"Could not alloc. urb(%d)\n", index, i);
 			tidy_up(&(usbduxsub[index]));
 			up(&start_stop_sem);
 			return -ENOMEM;
 		}
-		usbduxsub[index].urbOut[i]->dev = usbduxsub[index].usbdev;
+		usbduxsub[index].urb_out[i]->dev = usbduxsub[index].usbdev;
 		/* will be filled later with a pointer to the comedi-device */
 		/* and ONLY then the urb should be submitted */
-		usbduxsub[index].urbOut[i]->context = NULL;
-		usbduxsub[index].urbOut[i]->pipe =
+		usbduxsub[index].urb_out[i]->context = NULL;
+		usbduxsub[index].urb_out[i]->pipe =
 		    usb_sndisocpipe(usbduxsub[index].usbdev, ISOOUTEP);
-		usbduxsub[index].urbOut[i]->transfer_flags = URB_ISO_ASAP;
-		usbduxsub[index].urbOut[i]->transfer_buffer =
+		usbduxsub[index].urb_out[i]->transfer_flags = URB_ISO_ASAP;
+		usbduxsub[index].urb_out[i]->transfer_buffer =
 		    kzalloc(SIZEOUTBUF, GFP_KERNEL);
-		if (!(usbduxsub[index].urbOut[i]->transfer_buffer)) {
+		if (!(usbduxsub[index].urb_out[i]->transfer_buffer)) {
 			tidy_up(&(usbduxsub[index]));
 			up(&start_stop_sem);
 			return -ENOMEM;
 		}
-		usbduxsub[index].urbOut[i]->complete = usbduxsub_ao_IsocIrq;
-		usbduxsub[index].urbOut[i]->number_of_packets = 1;
-		usbduxsub[index].urbOut[i]->transfer_buffer_length = SIZEOUTBUF;
-		usbduxsub[index].urbOut[i]->iso_frame_desc[0].offset = 0;
-		usbduxsub[index].urbOut[i]->iso_frame_desc[0].length =
+		usbduxsub[index].urb_out[i]->complete = usbduxsub_ao_isoc_irq;
+		usbduxsub[index].urb_out[i]->number_of_packets = 1;
+		usbduxsub[index].urb_out[i]->transfer_buffer_length = SIZEOUTBUF;
+		usbduxsub[index].urb_out[i]->iso_frame_desc[0].offset = 0;
+		usbduxsub[index].urb_out[i]->iso_frame_desc[0].length =
 		    SIZEOUTBUF;
 		if (usbduxsub[index].high_speed) {
 			/* uframes */
-			usbduxsub[index].urbOut[i]->interval = 8;
+			usbduxsub[index].urb_out[i]->interval = 8;
 		} else {
 			/* frames */
-			usbduxsub[index].urbOut[i]->interval = 1;
+			usbduxsub[index].urb_out[i]->interval = 1;
 		}
 	}
 
 	/* pwm */
 	if (usbduxsub[index].high_speed) {
 		/* max bulk ep size in high speed */
-		usbduxsub[index].sizePwmBuf = 512;
-		usbduxsub[index].urbPwm = usb_alloc_urb(0, GFP_KERNEL);
-		if (usbduxsub[index].urbPwm == NULL) {
+		usbduxsub[index].size_pwm_buf = 512;
+		usbduxsub[index].urb_pwm = usb_alloc_urb(0, GFP_KERNEL);
+		if (usbduxsub[index].urb_pwm == NULL) {
 			dev_err(dev, "comedi_: usbdux%d: "
 				"Could not alloc. pwm urb\n", index);
 			tidy_up(&(usbduxsub[index]));
 			up(&start_stop_sem);
 			return -ENOMEM;
 		}
-		usbduxsub[index].urbPwm->transfer_buffer =
-		    kzalloc(usbduxsub[index].sizePwmBuf, GFP_KERNEL);
-		if (!(usbduxsub[index].urbPwm->transfer_buffer)) {
+		usbduxsub[index].urb_pwm->transfer_buffer =
+		    kzalloc(usbduxsub[index].size_pwm_buf, GFP_KERNEL);
+		if (!(usbduxsub[index].urb_pwm->transfer_buffer)) {
 			tidy_up(&(usbduxsub[index]));
 			up(&start_stop_sem);
 			return -ENOMEM;
 		}
 	} else {
-		usbduxsub[index].urbPwm = NULL;
-		usbduxsub[index].sizePwmBuf = 0;
+		usbduxsub[index].urb_pwm = NULL;
+		usbduxsub[index].size_pwm_buf = 0;
 	}
 
 	usbduxsub[index].ai_cmd_running = 0;
diff --git a/drivers/staging/comedi/drivers/usbduxfast.c b/drivers/staging/comedi/drivers/usbduxfast.c
index 1ba0e3df492..7f95af33085 100644
--- a/drivers/staging/comedi/drivers/usbduxfast.c
+++ b/drivers/staging/comedi/drivers/usbduxfast.c
@@ -1387,7 +1387,7 @@ static int usbduxfast_attach_common(struct comedi_device *dev,
 	down(&udfs->sem);
 	/* pointer back to the corresponding comedi device */
 	udfs->comedidev = dev;
-	dev->board_name = "usbduxfast";
+
 	ret = comedi_alloc_subdevices(dev, 1);
 	if (ret) {
 		up(&udfs->sem);
diff --git a/drivers/staging/comedi/drivers/usbduxsigma.c b/drivers/staging/comedi/drivers/usbduxsigma.c
index a728c8fc32a..d3bc1b9910a 100644
--- a/drivers/staging/comedi/drivers/usbduxsigma.c
+++ b/drivers/staging/comedi/drivers/usbduxsigma.c
@@ -176,12 +176,6 @@ static const struct comedi_lrange range_usbdux_ai_range = { 1, {
 								}
 };
 
-static const struct comedi_lrange range_usbdux_ao_range = { 1, {
-								UNI_RANGE
-								(2.5),
-							       }
-};
-
 /*
  * private structure of one subdevice
  */
@@ -1376,7 +1370,7 @@ static int usbdux_getstatusinfo(struct comedi_device *dev, int chan)
 
 	/* 32 bits big endian from the A/D converter */
 	one = be32_to_cpu(*((int32_t *)((this_usbduxsub->insnBuffer)+1)));
-	/* mask out the staus byte */
+	/* mask out the status byte */
 	one = one & 0x00ffffff;
 	one = one ^ 0x00800000;
 
@@ -2211,7 +2205,7 @@ static int usbduxsigma_attach_common(struct comedi_device *dev,
 	down(&uds->sem);
 	/* pointer back to the corresponding comedi device */
 	uds->comedidev = dev;
-	dev->board_name = "usbduxsigma";
+
 	/* set number of subdevices */
 	if (uds->high_speed)
 		n_subdevs = 4;	/* with pwm */
@@ -2269,7 +2263,7 @@ static int usbduxsigma_attach_common(struct comedi_device *dev,
 	/* 8 bit resolution */
 	s->maxdata = 0x00ff;
 	/* unipolar range */
-	s->range_table = (&range_usbdux_ao_range);
+	s->range_table = &range_unipolar2_5;
 	/* callback */
 	s->do_cmdtest = usbdux_ao_cmdtest;
 	s->do_cmd = usbdux_ao_cmd;
diff --git a/drivers/staging/comedi/kcomedilib/kcomedilib_main.c b/drivers/staging/comedi/kcomedilib/kcomedilib_main.c
index 8932a510d96..3231a483f56 100644
--- a/drivers/staging/comedi/kcomedilib/kcomedilib_main.c
+++ b/drivers/staging/comedi/kcomedilib/kcomedilib_main.c
@@ -63,7 +63,7 @@ struct comedi_device *comedi_open(const char *filename)
 
 	return dev;
 }
-EXPORT_SYMBOL(comedi_open);
+EXPORT_SYMBOL_GPL(comedi_open);
 
 int comedi_close(struct comedi_device *d)
 {
@@ -73,7 +73,7 @@ int comedi_close(struct comedi_device *d)
 
 	return 0;
 }
-EXPORT_SYMBOL(comedi_close);
+EXPORT_SYMBOL_GPL(comedi_close);
 
 static int comedi_do_insn(struct comedi_device *dev,
 			  struct comedi_insn *insn,
@@ -143,7 +143,7 @@ int comedi_dio_config(struct comedi_device *dev, unsigned int subdev,
 
 	return comedi_do_insn(dev, &insn, &io);
 }
-EXPORT_SYMBOL(comedi_dio_config);
+EXPORT_SYMBOL_GPL(comedi_dio_config);
 
 int comedi_dio_bitfield(struct comedi_device *dev, unsigned int subdev,
 			unsigned int mask, unsigned int *bits)
@@ -166,7 +166,7 @@ int comedi_dio_bitfield(struct comedi_device *dev, unsigned int subdev,
 
 	return ret;
 }
-EXPORT_SYMBOL(comedi_dio_bitfield);
+EXPORT_SYMBOL_GPL(comedi_dio_bitfield);
 
 int comedi_find_subdevice_by_type(struct comedi_device *dev, int type,
 				  unsigned int subd)
@@ -183,7 +183,7 @@ int comedi_find_subdevice_by_type(struct comedi_device *dev, int type,
 	}
 	return -1;
 }
-EXPORT_SYMBOL(comedi_find_subdevice_by_type);
+EXPORT_SYMBOL_GPL(comedi_find_subdevice_by_type);
 
 int comedi_get_n_channels(struct comedi_device *dev, unsigned int subdevice)
 {
@@ -191,4 +191,4 @@ int comedi_get_n_channels(struct comedi_device *dev, unsigned int subdevice)
 
 	return s->n_chan;
 }
-EXPORT_SYMBOL(comedi_get_n_channels);
+EXPORT_SYMBOL_GPL(comedi_get_n_channels);
diff --git a/drivers/staging/comedi/range.c b/drivers/staging/comedi/range.c
index 59ff0cf7338..1dc391b7644 100644
--- a/drivers/staging/comedi/range.c
+++ b/drivers/staging/comedi/range.c
@@ -26,17 +26,25 @@
 #include "comedi_internal.h"
 
 const struct comedi_lrange range_bipolar10 = { 1, {BIP_RANGE(10)} };
-EXPORT_SYMBOL(range_bipolar10);
+EXPORT_SYMBOL_GPL(range_bipolar10);
 const struct comedi_lrange range_bipolar5 = { 1, {BIP_RANGE(5)} };
-EXPORT_SYMBOL(range_bipolar5);
+EXPORT_SYMBOL_GPL(range_bipolar5);
 const struct comedi_lrange range_bipolar2_5 = { 1, {BIP_RANGE(2.5)} };
-EXPORT_SYMBOL(range_bipolar2_5);
+EXPORT_SYMBOL_GPL(range_bipolar2_5);
 const struct comedi_lrange range_unipolar10 = { 1, {UNI_RANGE(10)} };
-EXPORT_SYMBOL(range_unipolar10);
+EXPORT_SYMBOL_GPL(range_unipolar10);
 const struct comedi_lrange range_unipolar5 = { 1, {UNI_RANGE(5)} };
-EXPORT_SYMBOL(range_unipolar5);
+EXPORT_SYMBOL_GPL(range_unipolar5);
+const struct comedi_lrange range_unipolar2_5 = { 1, {UNI_RANGE(2.5)} };
+EXPORT_SYMBOL_GPL(range_unipolar2_5);
+const struct comedi_lrange range_0_20mA = { 1, {RANGE_mA(0, 20)} };
+EXPORT_SYMBOL_GPL(range_0_20mA);
+const struct comedi_lrange range_4_20mA = { 1, {RANGE_mA(4, 20)} };
+EXPORT_SYMBOL_GPL(range_4_20mA);
+const struct comedi_lrange range_0_32mA = { 1, {RANGE_mA(0, 32)} };
+EXPORT_SYMBOL_GPL(range_0_32mA);
 const struct comedi_lrange range_unknown = { 1, {{0, 1000000, UNIT_none} } };
-EXPORT_SYMBOL(range_unknown);
+EXPORT_SYMBOL_GPL(range_unknown);
 
 /*
 	COMEDI_RANGEINFO
@@ -165,4 +173,4 @@ int comedi_check_chanlist(struct comedi_subdevice *s, int n,
 	}
 	return 0;
 }
-EXPORT_SYMBOL(comedi_check_chanlist);
+EXPORT_SYMBOL_GPL(comedi_check_chanlist);
diff --git a/drivers/staging/cptm1217/clearpad_tm1217.c b/drivers/staging/cptm1217/clearpad_tm1217.c
index 31fb5d31bb3..e96eee3ca89 100644
--- a/drivers/staging/cptm1217/clearpad_tm1217.c
+++ b/drivers/staging/cptm1217/clearpad_tm1217.c
@@ -557,12 +557,15 @@ fail:
 
 }
 
+#ifdef CONFIG_PM_SLEEP
+
 /*
  * cp_tm1217 suspend
  *
  */
-static int cp_tm1217_suspend(struct i2c_client *client, pm_message_t mesg)
+static int cp_tm1217_suspend(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
 	struct cp_tm1217_device *ts = i2c_get_clientdata(client);
 	u8 req[2];
 	int retval;
@@ -583,8 +586,9 @@ static int cp_tm1217_suspend(struct i2c_client *client, pm_message_t mesg)
  * cp_tm1217_resume
  *
  */
-static int cp_tm1217_resume(struct i2c_client *client)
+static int cp_tm1217_resume(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
 	struct cp_tm1217_device *ts = i2c_get_clientdata(client);
 	u8 req[2];
 	int retval;
@@ -618,6 +622,11 @@ static int cp_tm1217_resume(struct i2c_client *client)
 	return 0;
 }
 
+#endif
+
+static SIMPLE_DEV_PM_OPS(cp_tm1217_pm_ops, cp_tm1217_suspend,
+	cp_tm1217_resume);
+
 /*
  * cp_tm1217_remove
  *
@@ -647,12 +656,11 @@ static struct i2c_driver cp_tm1217_driver = {
 	.driver = {
 		.owner	= THIS_MODULE,
 		.name	= CPTM1217_DRIVER_NAME,
+		.pm	= &cp_tm1217_pm_ops,
 	},
 	.id_table	= cp_tm1217_idtable,
 	.probe		= cp_tm1217_probe,
 	.remove		= cp_tm1217_remove,
-	.suspend    = cp_tm1217_suspend,
-	.resume     = cp_tm1217_resume,
 };
 
 module_i2c_driver(cp_tm1217_driver);
diff --git a/drivers/staging/csr/bh.c b/drivers/staging/csr/bh.c
index 7b133597e92..b53a9e29a97 100644
--- a/drivers/staging/csr/bh.c
+++ b/drivers/staging/csr/bh.c
@@ -373,7 +373,7 @@ CsrResult unifi_run_bh(void *ospriv)
     unifi_priv_t *priv = ospriv;
 
     /*
-     * If an error has occured, we discard silently all messages from the bh
+     * If an error has occurred, we discard silently all messages from the bh
      * until the error has been processed and the unifi has been reinitialised.
      */
     if (priv->bh_thread.block_thread == 1) {
diff --git a/drivers/staging/csr/csr_log.h b/drivers/staging/csr/csr_log.h
index 5de5650767d..982941043dd 100644
--- a/drivers/staging/csr/csr_log.h
+++ b/drivers/staging/csr/csr_log.h
@@ -34,7 +34,7 @@ typedef u32 CsrLogLevelEnvironment;
 #define CSR_LOG_LEVEL_ENVIRONMENT_BGINT_START  ((CsrLogLevelEnvironment) 0x00000100) /* Background Interrupt start events are logged */
 #define CSR_LOG_LEVEL_ENVIRONMENT_BGINT_DONE   ((CsrLogLevelEnvironment) 0x00000200) /* Background Interrupt done events are logged */
 #define CSR_LOG_LEVEL_ENVIRONMENT_PROTO        ((CsrLogLevelEnvironment) 0x00000400) /* Transport protocol events are logged */
-#define CSR_LOG_LEVEL_ENVIRONMENT_PROTO_LOC    ((CsrLogLevelEnvironment) 0x00000800) /* The Location where the transport protocol event occured are logged NB: This is a supplement to CSR_LOG_LEVEL_ENVIRONMENT_PROTO, it has no effect without it */
+#define CSR_LOG_LEVEL_ENVIRONMENT_PROTO_LOC    ((CsrLogLevelEnvironment) 0x00000800) /* The Location where the transport protocol event occurred are logged NB: This is a supplement to CSR_LOG_LEVEL_ENVIRONMENT_PROTO, it has no effect without it */
 /* The bit masks between here are reserved for future usage */
 #define CSR_LOG_LEVEL_ENVIRONMENT_ALL          ((CsrLogLevelEnvironment) 0xFFFFFFFF) /* All possible environment data/events are logged WARNING: By using this define the application also accepts future possible environment data/events in the logs */
 
@@ -44,10 +44,10 @@ typedef u32 CsrLogLevelEnvironment;
 typedef u32 CsrLogLevelTask;
 #define CSR_LOG_LEVEL_TASK_OFF                 ((CsrLogLevelTask) 0x00000000) /* No events are logged for this task */
 #define CSR_LOG_LEVEL_TASK_TEXT                ((CsrLogLevelTask) 0x00000001) /* Text strings printed by a task are logged NB: This bit does not affect the CSR_LOG_TEXT_LEVEL interface. This has to be configured separately */
-#define CSR_LOG_LEVEL_TASK_TEXT_LOC            ((CsrLogLevelTask) 0x00000002) /* The locaction where the text string call occured are logged. NB: This is a supplement to CSR_LOG_LEVEL_TASK_TEXT, it has no effect without it */
+#define CSR_LOG_LEVEL_TASK_TEXT_LOC            ((CsrLogLevelTask) 0x00000002) /* The locaction where the text string call occurred are logged. NB: This is a supplement to CSR_LOG_LEVEL_TASK_TEXT, it has no effect without it */
 #define CSR_LOG_LEVEL_TASK_STATE               ((CsrLogLevelTask) 0x00000004) /* FSM state transitions in a task are logged */
 #define CSR_LOG_LEVEL_TASK_STATE_NAME          ((CsrLogLevelTask) 0x00000008) /* The name of each state in a FSM state transition are logged. NB: This is a supplement to CSR_LOG_LEVEL_TASK_STATE, it has no effect without it */
-#define CSR_LOG_LEVEL_TASK_STATE_LOC           ((CsrLogLevelTask) 0x00000010) /* The location where the FSM state transition occured are logged. NB: This is a supplement to CSR_LOG_LEVEL_TASK_STATE, it has no effect without it */
+#define CSR_LOG_LEVEL_TASK_STATE_LOC           ((CsrLogLevelTask) 0x00000010) /* The location where the FSM state transition occurred are logged. NB: This is a supplement to CSR_LOG_LEVEL_TASK_STATE, it has no effect without it */
 #define CSR_LOG_LEVEL_TASK_TASK_SWITCH         ((CsrLogLevelTask) 0x00000020) /* Activation and deactiation of a task are logged */
 #define CSR_LOG_LEVEL_TASK_MESSAGE_PUT         ((CsrLogLevelTask) 0x00000080) /* Message put operations are logged */
 #define CSR_LOG_LEVEL_TASK_MESSAGE_PUT_LOC     ((CsrLogLevelTask) 0x00000100) /* The location where a message was sent are logged. NB: This is a supplement to CSR_LOG_LEVEL_TASK_MESSAGE_PUT, it has no effect without it */
diff --git a/drivers/staging/csr/csr_sdio.h b/drivers/staging/csr/csr_sdio.h
index 624a53fa1d7..0971d135abf 100644
--- a/drivers/staging/csr/csr_sdio.h
+++ b/drivers/staging/csr/csr_sdio.h
@@ -257,7 +257,7 @@ void CsrSdioFunctionDriverUnregister(CsrSdioFunctionDriver *functionDriver);
  *      CSR_RESULT_SUCCESS - The specified function was enabled/disabled.
  *      CSR_RESULT_FAILURE - Unspecified/unknown error.
  *      CSR_SDIO_RESULT_NO_DEVICE - The device does not exist anymore.
- *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occured. The state of the
+ *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occurred. The state of the
  *                                  related bit in the I/O Enable register is
  *                                  undefined.
  *      CSR_SDIO_RESULT_TIMEOUT - No response from the device, or the related
@@ -295,7 +295,7 @@ CsrResult CsrSdioFunctionDisable(CsrSdioFunction *function);
  *      CSR_RESULT_SUCCESS - The specified function was enabled/disabled.
  *      CSR_RESULT_FAILURE - Unspecified/unknown error.
  *      CSR_SDIO_RESULT_NO_DEVICE - The device does not exist anymore.
- *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occured. The state of the
+ *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occurred. The state of the
  *                                  related bit in the INT Enable register is
  *                                  unchanged.
  *      CSR_SDIO_RESULT_INVALID_VALUE - The specified function cannot be
@@ -408,7 +408,7 @@ void CsrSdioResumeAcknowledge(CsrSdioFunction *function, CsrResult result);
  *      CSR_RESULT_FAILURE - Unspecified/unknown error.
  *      CSR_SDIO_RESULT_INVALID_VALUE - One or more arguments were invalid.
  *      CSR_SDIO_RESULT_NO_DEVICE - The device does not exist anymore.
- *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occured. The configured block
+ *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occurred. The configured block
  *                                  size is undefined.
  *      CSR_SDIO_RESULT_TIMEOUT - No response from the device.
  *
@@ -456,7 +456,7 @@ CsrResult CsrSdioBlockSizeSet(CsrSdioFunction *function, u16 blockSize);
  *      maxFrequency - The maximum clock frequency for the function in Hertz.
  *
  *  RETURNS
- *      CSR_RESULT_SUCCESS - The maximum clock frequency was succesfully
+ *      CSR_RESULT_SUCCESS - The maximum clock frequency was successfully
  *                                set for the function.
  *      CSR_RESULT_FAILURE - Unspecified/unknown error.
  *      CSR_SDIO_RESULT_INVALID_VALUE - One or more arguments were invalid.
@@ -494,7 +494,7 @@ CsrResult CsrSdioMaxBusClockFrequencySet(CsrSdioFunction *function, u32 maxFrequ
  *      CSR_RESULT_FAILURE - Unspecified/unknown error.
  *      CSR_SDIO_RESULT_INVALID_VALUE - One or more arguments were invalid.
  *      CSR_SDIO_RESULT_NO_DEVICE - The device does not exist anymore.
- *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occured. No data read/written.
+ *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occurred. No data read/written.
  *      CSR_SDIO_RESULT_TIMEOUT - No response from the device.
  *
  *      NOTE: If the SDIO R5 response is available, and either of the
@@ -537,7 +537,7 @@ void CsrSdioWrite8Async(CsrSdioFunction *function, u32 address, u8 data, CsrSdio
  *      CSR_RESULT_FAILURE - Unspecified/unknown error.
  *      CSR_SDIO_RESULT_INVALID_VALUE - One or more arguments were invalid.
  *      CSR_SDIO_RESULT_NO_DEVICE - The device does not exist anymore.
- *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occured. Data may have been
+ *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occurred. Data may have been
  *                                  partially read/written.
  *      CSR_SDIO_RESULT_TIMEOUT - No response from the device.
  *
@@ -583,7 +583,7 @@ void CsrSdioWrite16Async(CsrSdioFunction *function, u32 address, u16 data, CsrSd
  *      CSR_RESULT_FAILURE - Unspecified/unknown error.
  *      CSR_SDIO_RESULT_INVALID_VALUE - One or more arguments were invalid.
  *      CSR_SDIO_RESULT_NO_DEVICE - The device does not exist anymore.
- *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occured. No data read/written.
+ *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occurred. No data read/written.
  *      CSR_SDIO_RESULT_TIMEOUT - No response from the device.
  *
  *      NOTE: If the SDIO R5 response is available, and either of the
@@ -628,7 +628,7 @@ void CsrSdioF0Write8Async(CsrSdioFunction *function, u32 address, u8 data, CsrSd
  *      CSR_RESULT_FAILURE - Unspecified/unknown error.
  *      CSR_SDIO_RESULT_INVALID_VALUE - One or more arguments were invalid.
  *      CSR_SDIO_RESULT_NO_DEVICE - The device does not exist anymore.
- *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occured. Data may have been
+ *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occurred. Data may have been
  *                                  partially read/written.
  *      CSR_SDIO_RESULT_TIMEOUT - No response from the device.
  *
@@ -666,11 +666,11 @@ void CsrSdioWriteAsync(CsrSdioFunction *function, u32 address, const void *data,
  *                 the device to power on/off.
  *
  *  RETURNS (only CsrSdioPowerOn)
- *      CSR_RESULT_SUCCESS - Power was succesfully reapplied and the device
+ *      CSR_RESULT_SUCCESS - Power was successfully reapplied and the device
  *                                has been reinitialised.
  *      CSR_RESULT_FAILURE - Unspecified/unknown error.
  *      CSR_SDIO_RESULT_NO_DEVICE - The device does not exist anymore.
- *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occured during reinitialisation.
+ *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occurred during reinitialisation.
  *      CSR_SDIO_RESULT_TIMEOUT - No response from the device during
  *                                reinitialisation.
  *      CSR_SDIO_RESULT_NOT_RESET - The power was not removed by the
@@ -693,11 +693,11 @@ void CsrSdioPowerOff(CsrSdioFunction *function);
  *                 the device to hard reset.
  *
  *  RETURNS
- *      CSR_RESULT_SUCCESS - Reset was succesfully performed and the device
+ *      CSR_RESULT_SUCCESS - Reset was successfully performed and the device
  *                                has been reinitialised.
  *      CSR_RESULT_FAILURE - Unspecified/unknown error.
  *      CSR_SDIO_RESULT_NO_DEVICE - The device does not exist anymore.
- *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occured during reinitialisation.
+ *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occurred during reinitialisation.
  *      CSR_SDIO_RESULT_TIMEOUT - No response from the device during
  *                                reinitialisation.
  *      CSR_SDIO_RESULT_NOT_RESET - The reset was not applied because it is not
diff --git a/drivers/staging/csr/csr_time.c b/drivers/staging/csr/csr_time.c
index f3f4a9c9c67..01179e46f47 100644
--- a/drivers/staging/csr/csr_time.c
+++ b/drivers/staging/csr/csr_time.c
@@ -1,10 +1,10 @@
 /*****************************************************************************
 
-            (c) Cambridge Silicon Radio Limited 2010
-            All rights reserved and confidential information of CSR
+	(c) Cambridge Silicon Radio Limited 2010
+	All rights reserved and confidential information of CSR
 
-            Refer to LICENSE.txt included with this source for details
-            on the license terms.
+	Refer to LICENSE.txt included with this source for details
+	on the license terms.
 
 *****************************************************************************/
 
diff --git a/drivers/staging/csr/csr_wifi_fsm.h b/drivers/staging/csr/csr_wifi_fsm.h
index fde1508c1ea..fc5c5aa6a3c 100644
--- a/drivers/staging/csr/csr_wifi_fsm.h
+++ b/drivers/staging/csr/csr_wifi_fsm.h
@@ -70,7 +70,7 @@ extern CsrWifiFsmContext* CsrWifiFsmInit(void *applicationContext, void *externa
  *   This function is used to free any dynamic resources allocated for the
  *   given context by CsrWifiFsmInit().
  *   The FSM's reset function is called to cleanup any fsm specific memory
- *   The reset funtion does NOT need to free the fsm data pointer as
+ *   The reset function does NOT need to free the fsm data pointer as
  *   CsrWifiFsmShutdown() will do it.
  *   the FSM's init function is call again to reinitialise the FSM context.
  *   CsrWifiFsmReset() should NEVER be called when CsrWifiFsmExecute() is running.
@@ -91,7 +91,7 @@ extern void CsrWifiFsmReset(CsrWifiFsmContext *context);
  *   given context by CsrWifiFsmInit(), prior to complete termination of
  *   the program.
  *   The FSM's reset function is called to cleanup any fsm specific memory.
- *   The reset funtion does NOT need to free the fsm data pointer as
+ *   The reset function does NOT need to free the fsm data pointer as
  *   CsrWifiFsmShutdown() will do it.
  *   CsrWifiFsmShutdown() should NEVER be called when CsrWifiFsmExecute() is running.
  *
@@ -203,7 +203,7 @@ extern void CsrWifiFsmFastForward(CsrWifiFsmContext *context, u16 ms);
  *   shift the current time of day by ms amount
  *
  * @par Description
- *   usefull to speed up tests where time needs to pass
+ *   useful to speed up tests where time needs to pass
  *
  * @param[in]    context  : FSM context
  * @param[in]    ms       : ms to adjust time by
diff --git a/drivers/staging/csr/csr_wifi_hip_card_sdio.c b/drivers/staging/csr/csr_wifi_hip_card_sdio.c
index 25cabf3234c..d5425325894 100644
--- a/drivers/staging/csr/csr_wifi_hip_card_sdio.c
+++ b/drivers/staging/csr/csr_wifi_hip_card_sdio.c
@@ -559,7 +559,7 @@ static void _build_sdio_config_data(sdio_config_data_t *cfg_data,
  *      padding bytes. Every read from this memory has to be transformed in
  *      host (cpu specific) format, before it is stored in driver's parameters
  *      or/and structures. Athough unifi_card_read16() and unifi_read32() do perform
- *      the convertion internally, unifi_readn() does not.
+ *      the conversion internally, unifi_readn() does not.
  * ---------------------------------------------------------------------------
  */
 static CsrResult card_hw_init(card_t *card)
@@ -1332,7 +1332,7 @@ static CsrResult card_access_panic(card_t *card)
     s32 i;
     CsrResult r, sr;
 
-    /* A chip version of zero means that the version never got succesfully read
+    /* A chip version of zero means that the version never got successfully read
      * during reset. In this case give up because it will not be possible to
      * verify the chip version.
      */
@@ -2446,7 +2446,7 @@ static CsrResult unifi_prepare_hw(card_t *card)
 
     if (old_state == UNIFI_HOST_STATE_TORPID)
     {
-        /* Ensure the initial clock rate is set; if a reset occured when the chip was
+        /* Ensure the initial clock rate is set; if a reset occurred when the chip was
          * TORPID, unifi_set_host_state() may have raised it to MAX.
          */
         csrResult = CsrSdioMaxBusClockFrequencySet(card->sdio_if, UNIFI_SDIO_CLOCK_INIT_HZ);
@@ -2567,7 +2567,7 @@ static CsrResult unifi_reset_hardware(card_t *card)
     csrResult = CsrSdioHardReset(card->sdio_if);
     if (csrResult == CSR_RESULT_SUCCESS)
     {
-        unifi_info(card->ospriv, "CsrSdioHardReset succeeded on reseting UniFi\n");
+        unifi_info(card->ospriv, "CsrSdioHardReset succeeded on resetting UniFi\n");
         r = unifi_prepare_hw(card);
         if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
         {
diff --git a/drivers/staging/csr/csr_wifi_hip_card_sdio_intr.c b/drivers/staging/csr/csr_wifi_hip_card_sdio_intr.c
index 97f645c0681..cfe186e0707 100644
--- a/drivers/staging/csr/csr_wifi_hip_card_sdio_intr.c
+++ b/drivers/staging/csr/csr_wifi_hip_card_sdio_intr.c
@@ -883,7 +883,7 @@ static CsrResult handle_host_protocol(card_t *card, u8 *processed_something)
     r = read_to_host_signals(card, &done);
     if (r != CSR_RESULT_SUCCESS)
     {
-        unifi_error(card->ospriv, "Error occured reading to-host signals\n");
+        unifi_error(card->ospriv, "Error occurred reading to-host signals\n");
         return r;
     }
     if (done > 0)
@@ -899,7 +899,7 @@ static CsrResult handle_host_protocol(card_t *card, u8 *processed_something)
     r = process_to_host_signals(card, &done);
     if (r != CSR_RESULT_SUCCESS)
     {
-        unifi_error(card->ospriv, "Error occured processing to-host signals\n");
+        unifi_error(card->ospriv, "Error occurred processing to-host signals\n");
         return r;
     }
 
@@ -908,7 +908,7 @@ static CsrResult handle_host_protocol(card_t *card, u8 *processed_something)
     r = process_fh_cmd_queue(card, &done);
     if (r != CSR_RESULT_SUCCESS)
     {
-        unifi_error(card->ospriv, "Error occured processing from-host signals\n");
+        unifi_error(card->ospriv, "Error occurred processing from-host signals\n");
         return r;
     }
     if (done > 0)
@@ -919,7 +919,7 @@ static CsrResult handle_host_protocol(card_t *card, u8 *processed_something)
     r = process_fh_traffic_queue(card, &done);
     if (r != CSR_RESULT_SUCCESS)
     {
-        unifi_error(card->ospriv, "Error occured processing from-host data signals\n");
+        unifi_error(card->ospriv, "Error occurred processing from-host data signals\n");
         return r;
     }
     if (done > 0)
@@ -1953,7 +1953,7 @@ static CsrResult check_fh_sig_slots(card_t *card, u16 needed, s32 *space_fh)
  *      in the from-host queue using the wire-format structures, as they arrive.
  *      All other requests are stored in the from-host queue using the host
  *      (cpu specific) structures. We use the is_packed member of the card_signal_t
- *      structure that describes the queue to make the distiction.
+ *      structure that describes the queue to make the distinction.
  * ---------------------------------------------------------------------------
  */
 static CsrResult process_fh_cmd_queue(card_t *card, s32 *processed)
diff --git a/drivers/staging/csr/csr_wifi_hip_chiphelper.h b/drivers/staging/csr/csr_wifi_hip_chiphelper.h
index b6b67ee11db..09b3aefcbce 100644
--- a/drivers/staging/csr/csr_wifi_hip_chiphelper.h
+++ b/drivers/staging/csr/csr_wifi_hip_chiphelper.h
@@ -354,7 +354,7 @@ CHIP_HELPER_LIST(C_DEC)
     map that is part of the window and the size is the number of
     visible words.
 
-    Some of the windows have thier lowest portion covered by
+    Some of the windows have their lowest portion covered by
     registers.  For these windows address is the first address
     after the registers and size is the siave excluding the part
     covered by registers.
@@ -375,7 +375,7 @@ CHIP_HELPER_LIST(C_DEC)
     will be needed if the device is being booted from cold.  These
     register writes enable the clocks and setup the PLL to a basic
     working state.  SPI access might be unreliable until these writes
-    have occured (And they may take mulitple goes).
+    have occurred (And they may take mulitple goes).
 */
 /*  HostResetSequence
 
diff --git a/drivers/staging/csr/csr_wifi_hip_unifi.h b/drivers/staging/csr/csr_wifi_hip_unifi.h
index c2a2231680f..1160a0e25c7 100644
--- a/drivers/staging/csr/csr_wifi_hip_unifi.h
+++ b/drivers/staging/csr/csr_wifi_hip_unifi.h
@@ -117,9 +117,9 @@ typedef enum
     UNIFI_TRAFFIC_Q_BE,
     UNIFI_TRAFFIC_Q_VI,
     UNIFI_TRAFFIC_Q_VO,
-    UNIFI_TRAFFIC_Q_EAPOL,    /* Non existant in HIP */
-    UNIFI_TRAFFIC_Q_MAX,      /* Non existant */
-    UNIFI_TRAFFIC_Q_MLME      /* Non existant */
+    UNIFI_TRAFFIC_Q_EAPOL,    /* Non existent in HIP */
+    UNIFI_TRAFFIC_Q_MAX,      /* Non existent */
+    UNIFI_TRAFFIC_Q_MLME      /* Non existent */
 } unifi_TrafficQueue;
 
 /*
@@ -263,7 +263,7 @@ card_t* unifi_alloc_card(CsrSdioFunction *sdiopriv, void *ospriv);
  *
  * @return \b 0 if UniFi is initialized.
  *
- * @return \b -CSR_EIO if an I/O error occured while initializing UniFi
+ * @return \b -CSR_EIO if an I/O error occurred while initializing UniFi
  *
  * @return \b -CSR_ENODEV if the card is no longer present.
  *
@@ -311,7 +311,7 @@ void unifi_cancel_pending_signals(card_t *card);
  *
  * @return \b 0 signal is sent.
  *
- * @return \b -CSR_EIO if an error occured while sending the signal
+ * @return \b -CSR_EIO if an error occurred while sending the signal
  *
  * @return \b -CSR_ENODEV if the card is no longer present.
  *
@@ -361,7 +361,7 @@ void unifi_card_info(card_t *card, card_info_t *card_info);
  *
  * @return \b 0 if the check was performed.
  *
- * @return \b -CSR_EIO if an error occured while checking the status.
+ * @return \b -CSR_EIO if an error occurred while checking the status.
  *
  * @return \b -CSR_ENODEV if the card is no longer present.
  *
@@ -383,11 +383,11 @@ CsrResult unifi_check_io_status(card_t *card, s32 *status);
  * is required. If unifi_bh() is called before the timeout expires,
  * the caller must pass in the remaining time.
  *
- * @return \b 0 if no error occured.
+ * @return \b 0 if no error occurred.
  *
  * @return \b -CSR_ENODEV if the card is no longer present.
  *
- * @return \b -CSR_E* if an error occured while running the bottom half.
+ * @return \b -CSR_E* if an error occurred while running the bottom half.
  *
  * @ingroup upperedge
  */
@@ -439,7 +439,7 @@ enum unifi_periodic_wake_mode
  *
  * @param periodic_wake_mode the Periodic Wake Mode.
  *
- * @return \b 0 if no error occured.
+ * @return \b 0 if no error occurred.
  *
  * @return \b -CSR_E* if the request failed.
  *
@@ -458,7 +458,7 @@ CsrResult unifi_configure_low_power_mode(card_t                       *card,
  *
  * @param card the HIP core lib API context.
  *
- * @return \b 0 if no error occured.
+ * @return \b 0 if no error occurred.
  *
  * @return \b -CSR_ENODEV if the card is no longer present.
  *
diff --git a/drivers/staging/csr/drv.c b/drivers/staging/csr/drv.c
index 3bd52fdeac3..5520d6539f7 100644
--- a/drivers/staging/csr/drv.c
+++ b/drivers/staging/csr/drv.c
@@ -1815,7 +1815,7 @@ udi_log_event(ul_client_t *pcli,
     }
 
     /* Allocate log structure plus actual signal. */
-    logptr = (udi_log_t *)kmalloc(sizeof(udi_log_t) + total_len, GFP_KERNEL);
+    logptr = kmalloc(sizeof(udi_log_t) + total_len, GFP_KERNEL);
 
     if (logptr == NULL) {
         printk(KERN_ERR
@@ -1890,7 +1890,7 @@ uf_sme_queue_message(unifi_priv_t *priv, u8 *buffer, int length)
     }
 
     /* Allocate log structure plus actual signal. */
-    logptr = (udi_log_t *)kmalloc(sizeof(udi_log_t) + length, GFP_ATOMIC);
+    logptr = kmalloc(sizeof(udi_log_t) + length, GFP_ATOMIC);
     if (logptr == NULL) {
         unifi_error(priv, "Failed to allocate %d bytes for an SME message\n",
                     sizeof(udi_log_t) + length);
diff --git a/drivers/staging/csr/netdev.c b/drivers/staging/csr/netdev.c
index 7dad26f7017..a0177d99897 100644
--- a/drivers/staging/csr/netdev.c
+++ b/drivers/staging/csr/netdev.c
@@ -2365,7 +2365,7 @@ unifi_rx(unifi_priv_t *priv, CSR_SIGNAL *signal, bulk_data_param_t *bulkdata)
         rx_buffered_packets_t *rx_q_item;
         struct list_head *rx_list;
 
-        rx_q_item = (rx_buffered_packets_t *)kmalloc(sizeof(rx_buffered_packets_t),
+        rx_q_item = kmalloc(sizeof(rx_buffered_packets_t),
                 GFP_KERNEL);
         if (rx_q_item == NULL) {
             unifi_error(priv, "%s: Failed to allocate %d bytes for rx packet record\n",
diff --git a/drivers/staging/csr/sdio_mmc.c b/drivers/staging/csr/sdio_mmc.c
index b6a16de08f4..30271d35af5 100644
--- a/drivers/staging/csr/sdio_mmc.c
+++ b/drivers/staging/csr/sdio_mmc.c
@@ -1031,8 +1031,7 @@ uf_glue_sdio_probe(struct sdio_func *func,
            sdio_func_id(func), instance);
 
     /* Allocate context */
-    sdio_ctx = (CsrSdioFunction *)kmalloc(sizeof(CsrSdioFunction),
-                                          GFP_KERNEL);
+    sdio_ctx = kmalloc(sizeof(CsrSdioFunction), GFP_KERNEL);
     if (sdio_ctx == NULL) {
         sdio_release_host(func);
         return -ENOMEM;
diff --git a/drivers/staging/csr/sme_native.c b/drivers/staging/csr/sme_native.c
index 525fe1bce0e..ca55249bde3 100644
--- a/drivers/staging/csr/sme_native.c
+++ b/drivers/staging/csr/sme_native.c
@@ -273,7 +273,7 @@ sme_native_log_event(ul_client_t *pcli,
     }
 
     /* Allocate log structure plus actual signal. */
-    logptr = (udi_log_t *)kmalloc(sizeof(udi_log_t) + total_len, GFP_KERNEL);
+    logptr = kmalloc(sizeof(udi_log_t) + total_len, GFP_KERNEL);
 
     if (logptr == NULL) {
         unifi_error(priv,
diff --git a/drivers/staging/csr/sme_wext.c b/drivers/staging/csr/sme_wext.c
index 5e06a380b40..4129a6436b7 100644
--- a/drivers/staging/csr/sme_wext.c
+++ b/drivers/staging/csr/sme_wext.c
@@ -1273,7 +1273,6 @@ unifi_siwscan(struct net_device *dev, struct iw_request_info *info,
 {
     netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
     unifi_priv_t *priv = interfacePriv->privPtr;
-    int scantype;
     int r;
     CsrWifiSsid scan_ssid;
     unsigned char *channel_list = NULL;
@@ -1293,8 +1292,6 @@ unifi_siwscan(struct net_device *dev, struct iw_request_info *info,
     }
 
 
-    scantype = UNIFI_SCAN_ACTIVE;
-
 #if WIRELESS_EXT > 17
     /* Providing a valid channel list will force an active scan */
     if (req) {
diff --git a/drivers/staging/csr/unifi_pdu_processing.c b/drivers/staging/csr/unifi_pdu_processing.c
index 95efc360cc2..f9b421b5aa3 100644
--- a/drivers/staging/csr/unifi_pdu_processing.c
+++ b/drivers/staging/csr/unifi_pdu_processing.c
@@ -403,7 +403,7 @@ CsrResult enque_tx_data_pdu(unifi_priv_t *priv, bulk_data_param_t *bulkdata,
 
 
 
-    tx_q_item = (tx_buffered_packets_t *)kmalloc(sizeof(tx_buffered_packets_t), GFP_ATOMIC);
+    tx_q_item = kmalloc(sizeof(tx_buffered_packets_t), GFP_ATOMIC);
     if (tx_q_item == NULL) {
         unifi_error(priv,
                 "Failed to allocate %d bytes for tx packet record\n",
@@ -1468,7 +1468,7 @@ static int update_macheader(unifi_priv_t *priv, struct sk_buff *skb,
             }
     }
 
-    /* prepare the complete skb, by pushing the MAC header to the begining of the skb->data */
+    /* prepare the complete skb, by pushing the MAC header to the beginning of the skb->data */
     unifi_trace(priv, UDBG5, "updated Mac Header: %d \n",macHeaderLengthInBytes);
     memcpy(bufPtr, macHeaderBuf, macHeaderLengthInBytes);
 
@@ -1546,7 +1546,7 @@ uf_ap_process_data_pdu(unifi_priv_t *priv, struct sk_buff *skb,
         return -1;
     }
 
-    /* fetch the destination record from staion record database */
+    /* fetch the destination record from station record database */
     dstStaInfo = CsrWifiRouterCtrlGetStationRecordFromPeerMacAddress(priv, ehdr->h_dest, interfaceTag);
 
     /* AP mode processing, & if packet is unicast */
@@ -1878,13 +1878,13 @@ CsrResult uf_process_ma_packet_req(unifi_priv_t *priv,
         }
         else if ((pktType == CSR_WIFI_MULTICAST_PDU) && (!status))
         {
-            /* If broadcast Tim is set && queuing is successfull, then only update TIM */
+            /* If broadcast Tim is set && queuing is successful, then only update TIM */
             spin_lock_irqsave(&priv->staRecord_lock,lock_flags);
             interfacePriv->noOfbroadcastPktQueued++;
             spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
         }
     }
-    /* If broadcast Tim is set && queuing is successfull, then only update TIM */
+    /* If broadcast Tim is set && queuing is successful, then only update TIM */
     if(setBcTim && !status) {
         unifi_trace(priv, UDBG3, "tim set due to broadcast pkt\n");
         if (!interfacePriv->bcTimSetReqPendingFlag)
@@ -1995,7 +1995,7 @@ s8 uf_get_protection_bit_from_interfacemode(unifi_priv_t *priv, u16 interfaceTag
                      */
                     protection = interfacePriv->protect;
                 } else {
-                    /* fetch the destination record from staion record database */
+                    /* fetch the destination record from station record database */
                     dstStaInfo = CsrWifiRouterCtrlGetStationRecordFromPeerMacAddress(priv, daddr, interfaceTag);
                     if (!dstStaInfo) {
                         unifi_trace(priv, UDBG3, "peer not found in station record in send_ma_pkt_request\n");
@@ -3170,7 +3170,7 @@ void uf_process_ps_poll(unifi_priv_t *priv,u8* sa,u8* da,u8 pmBit,u16 interfaceT
         /*Send Data From Management Frames*/
         /* Priority orders for delivering the buffered packets are
          * 1. Deliver the Management frames if there
-         * 2. Other access catagory frames which are non deliver enable including UNIFI_TRAFFIC_Q_VO
+         * 2. Other access category frames which are non deliver enable including UNIFI_TRAFFIC_Q_VO
          * priority is from VO->BK
          */
 
@@ -3282,7 +3282,7 @@ void add_to_send_cfm_list(unifi_priv_t * priv,
 {
     tx_buffered_packets_t *send_cfm_list_item = NULL;
 
-    send_cfm_list_item = (tx_buffered_packets_t *) kmalloc(sizeof(tx_buffered_packets_t), GFP_ATOMIC);
+    send_cfm_list_item = kmalloc(sizeof(tx_buffered_packets_t), GFP_ATOMIC);
 
     if(send_cfm_list_item == NULL){
         unifi_warning(priv, "%s: Failed to allocate memory for new list item \n");
@@ -3439,7 +3439,7 @@ CsrWifiRouterCtrlStaInfo_t *CsrWifiRouterCtrlGetStationRecordFromPeerMacAddress(
 
     interfacePriv = priv->interfacePriv[interfaceTag];
 
-    /* disable the preemption untill station record is fetched */
+    /* disable the preemption until station record is fetched */
     spin_lock_irqsave(&priv->staRecord_lock,lock_flags);
 
     for (i = 0; i < UNIFI_MAX_CONNECTIONS; i++) {
@@ -3665,7 +3665,7 @@ void update_eosp_to_head_of_broadcast_list_head(unifi_priv_t *priv,u16 interface
     if (interfacePriv->noOfbroadcastPktQueued) {
 
         /* Update the EOSP to the HEAD of b/c list
-         * beacuse we have received any mgmt packet so it should not hold for long time
+         * because we have received any mgmt packet so it should not hold for long time
          * peer may time out.
          */
         spin_lock_irqsave(&priv->tx_q_lock,lock_flags);
diff --git a/drivers/staging/csr/unifi_sme.c b/drivers/staging/csr/unifi_sme.c
index 7d19e632a5e..90295035621 100644
--- a/drivers/staging/csr/unifi_sme.c
+++ b/drivers/staging/csr/unifi_sme.c
@@ -876,6 +876,8 @@ int unifi_cfg_get_info(unifi_priv_t *priv, unsigned char *arg)
             {
 #ifdef CSR_SUPPORT_WEXT_AP
                 uf_cfg_ap_config_t cfg_ap_config;
+
+		memset(&cfg_ap_config, 0, sizeof(cfg_ap_config));
                 cfg_ap_config.channel = priv->ap_config.channel;
                 cfg_ap_config.beaconInterval = priv->ap_mac_config.beaconInterval;
                 cfg_ap_config.wmmEnabled = priv->ap_mac_config.wmmEnabled;
diff --git a/drivers/staging/cxt1e1/pmcc4_drv.c b/drivers/staging/cxt1e1/pmcc4_drv.c
index 8a7b3a64645..8d8a22be5b2 100644
--- a/drivers/staging/cxt1e1/pmcc4_drv.c
+++ b/drivers/staging/cxt1e1/pmcc4_drv.c
@@ -1545,6 +1545,7 @@ c4_get_iidinfo (ci_t * ci, struct sbe_iid_info * iip)
 
     np = dev->name;
     strncpy (iip->iname, np, CHNM_STRLEN - 1);
+    iip->iname[CHNM_STRLEN - 1] = '\0';
     return 0;
 }
 
diff --git a/drivers/staging/dgrp/dgrp_common.h b/drivers/staging/dgrp/dgrp_common.h
index 0583fe9c7b0..2832b8e26c4 100644
--- a/drivers/staging/dgrp/dgrp_common.h
+++ b/drivers/staging/dgrp/dgrp_common.h
@@ -66,7 +66,7 @@ extern void dgrp_register_dpa_hook(struct proc_dir_entry *de);
 extern void dgrp_dpa_data(struct nd_struct *, int, u8 *, int);
 
 /* from dgrp_sysfs.c */
-extern void dgrp_create_class_sysfs_files(void);
+extern int dgrp_create_class_sysfs_files(void);
 extern void dgrp_remove_class_sysfs_files(void);
 
 extern void dgrp_create_node_class_sysfs_files(struct nd_struct *nd);
diff --git a/drivers/staging/dgrp/dgrp_dpa_ops.c b/drivers/staging/dgrp/dgrp_dpa_ops.c
index 021cca498f2..ca10a3362e2 100644
--- a/drivers/staging/dgrp/dgrp_dpa_ops.c
+++ b/drivers/staging/dgrp/dgrp_dpa_ops.c
@@ -116,7 +116,7 @@ void dgrp_register_dpa_hook(struct proc_dir_entry *de)
 	struct nd_struct *node = de->data;
 
 	de->proc_iops = &dpa_inode_ops;
-	de->proc_fops = &dpa_ops;
+	rcu_assign_pointer(de->proc_fops, &dpa_ops);
 
 	node->nd_dpa_de = de;
 	spin_lock_init(&node->nd_dpa_lock);
@@ -432,6 +432,7 @@ static long dgrp_dpa_ioctl(struct file *file, unsigned int cmd,
 
 
 	case DIGI_GETVPD:
+		memset(&vpd, 0, sizeof(vpd));
 		if (nd->nd_vpd_len > 0) {
 			vpd.vpd_len = nd->nd_vpd_len;
 			memcpy(&vpd.vpd_data, &nd->nd_vpd, nd->nd_vpd_len);
diff --git a/drivers/staging/dgrp/dgrp_driver.c b/drivers/staging/dgrp/dgrp_driver.c
index aa262588e9b..e456dc6cb36 100644
--- a/drivers/staging/dgrp/dgrp_driver.c
+++ b/drivers/staging/dgrp/dgrp_driver.c
@@ -66,6 +66,8 @@ module_exit(dgrp_cleanup_module);
  */
 static int dgrp_init_module(void)
 {
+	int ret;
+
 	INIT_LIST_HEAD(&nd_struct_list);
 
 	spin_lock_init(&dgrp_poll_data.poll_lock);
@@ -74,7 +76,9 @@ static int dgrp_init_module(void)
 	dgrp_poll_data.timer.function = dgrp_poll_handler;
 	dgrp_poll_data.timer.data = (unsigned long) &dgrp_poll_data;
 
-	dgrp_create_class_sysfs_files();
+	ret = dgrp_create_class_sysfs_files();
+	if (ret)
+		return ret;
 
 	dgrp_register_proc();
 
diff --git a/drivers/staging/dgrp/dgrp_mon_ops.c b/drivers/staging/dgrp/dgrp_mon_ops.c
index 4792d056a36..b484fccb494 100644
--- a/drivers/staging/dgrp/dgrp_mon_ops.c
+++ b/drivers/staging/dgrp/dgrp_mon_ops.c
@@ -66,7 +66,7 @@ void dgrp_register_mon_hook(struct proc_dir_entry *de)
 	struct nd_struct *node = de->data;
 
 	de->proc_iops = &mon_inode_ops;
-	de->proc_fops = &mon_ops;
+	rcu_assign_pointer(de->proc_fops, &mon_ops);
 	node->nd_mon_de = de;
 	sema_init(&node->nd_mon_semaphore, 1);
 }
diff --git a/drivers/staging/dgrp/dgrp_net_ops.c b/drivers/staging/dgrp/dgrp_net_ops.c
index e6018823b9d..64f48ffb9d4 100644
--- a/drivers/staging/dgrp/dgrp_net_ops.c
+++ b/drivers/staging/dgrp/dgrp_net_ops.c
@@ -91,7 +91,7 @@ void dgrp_register_net_hook(struct proc_dir_entry *de)
 	struct nd_struct *node = de->data;
 
 	de->proc_iops = &net_inode_ops;
-	de->proc_fops = &net_ops;
+	rcu_assign_pointer(de->proc_fops, &net_ops);
 	node->nd_net_de = de;
 	sema_init(&node->nd_net_semaphore, 1);
 	node->nd_state = NS_CLOSED;
@@ -3405,7 +3405,7 @@ static long dgrp_net_ioctl(struct file *file, unsigned int cmd,
 		if (size != sizeof(struct link_struct))
 			return -EINVAL;
 
-		if (copy_from_user((void *)(&link), (void __user *) arg, size))
+		if (copy_from_user(&link, (void __user *)arg, size))
 			return -EFAULT;
 
 		if (link.lk_fast_rate < 9600)
diff --git a/drivers/staging/dgrp/dgrp_ports_ops.c b/drivers/staging/dgrp/dgrp_ports_ops.c
index cd1fc208862..f93dc1f262f 100644
--- a/drivers/staging/dgrp/dgrp_ports_ops.c
+++ b/drivers/staging/dgrp/dgrp_ports_ops.c
@@ -65,7 +65,7 @@ void dgrp_register_ports_hook(struct proc_dir_entry *de)
 	struct nd_struct *node = de->data;
 
 	de->proc_iops = &ports_inode_ops;
-	de->proc_fops = &ports_ops;
+	rcu_assign_pointer(de->proc_fops, &ports_ops);
 	node->nd_ports_de = de;
 }
 
diff --git a/drivers/staging/dgrp/dgrp_specproc.c b/drivers/staging/dgrp/dgrp_specproc.c
index 73f287f9660..d66712c8aa9 100644
--- a/drivers/staging/dgrp/dgrp_specproc.c
+++ b/drivers/staging/dgrp/dgrp_specproc.c
@@ -271,9 +271,11 @@ static void register_proc_table(struct dgrp_proc_entry *table,
 			if (!table->child) {
 				de->proc_iops = &proc_inode_ops;
 				if (table->proc_file_ops)
-					de->proc_fops = table->proc_file_ops;
+					rcu_assign_pointer(de->proc_fops,
+							table->proc_file_ops);
 				else
-					de->proc_fops = &dgrp_proc_file_ops;
+					rcu_assign_pointer(de->proc_fops,
+							 &dgrp_proc_file_ops);
 			}
 		}
 		table->de = de;
diff --git a/drivers/staging/dgrp/dgrp_sysfs.c b/drivers/staging/dgrp/dgrp_sysfs.c
index be179adfb7c..7d1b36d1e75 100644
--- a/drivers/staging/dgrp/dgrp_sysfs.c
+++ b/drivers/staging/dgrp/dgrp_sysfs.c
@@ -85,30 +85,50 @@ static struct attribute_group dgrp_global_settings_attribute_group = {
 
 
 
-void dgrp_create_class_sysfs_files(void)
+int dgrp_create_class_sysfs_files(void)
 {
 	int ret = 0;
 	int max_majors = 1U << (32 - MINORBITS);
 
 	dgrp_class = class_create(THIS_MODULE, "digi_realport");
+	if (IS_ERR(dgrp_class))
+		return PTR_ERR(dgrp_class);
 	ret = class_create_file(dgrp_class, &class_attr_driver_version);
+	if (ret)
+		goto err_class;
 
 	dgrp_class_global_settings_dev = device_create(dgrp_class, NULL,
 		MKDEV(0, max_majors + 1), NULL, "driver_settings");
-
+	if (IS_ERR(dgrp_class_global_settings_dev)) {
+		ret = PTR_ERR(dgrp_class_global_settings_dev);
+		goto err_file;
+	}
 	ret = sysfs_create_group(&dgrp_class_global_settings_dev->kobj,
 		&dgrp_global_settings_attribute_group);
 	if (ret) {
 		pr_alert("%s: failed to create sysfs global settings device attributes.\n",
 			__func__);
-		sysfs_remove_group(&dgrp_class_global_settings_dev->kobj,
-			&dgrp_global_settings_attribute_group);
-		return;
+		goto err_dev1;
 	}
 
 	dgrp_class_nodes_dev = device_create(dgrp_class, NULL,
 		MKDEV(0, max_majors + 2), NULL, "nodes");
+	if (IS_ERR(dgrp_class_nodes_dev)) {
+		ret = PTR_ERR(dgrp_class_nodes_dev);
+		goto err_group;
+	}
 
+	return 0;
+err_group:
+	sysfs_remove_group(&dgrp_class_global_settings_dev->kobj,
+		&dgrp_global_settings_attribute_group);
+err_dev1:
+	device_destroy(dgrp_class, MKDEV(0, max_majors + 1));
+err_file:
+	class_remove_file(dgrp_class, &class_attr_driver_version);
+err_class:
+	class_destroy(dgrp_class);
+	return ret;
 }
 
 
diff --git a/drivers/staging/dwc2/Kconfig b/drivers/staging/dwc2/Kconfig
new file mode 100644
index 00000000000..f0b4739c65a
--- /dev/null
+++ b/drivers/staging/dwc2/Kconfig
@@ -0,0 +1,54 @@
+config USB_DWC2
+	tristate "DesignWare USB2 DRD Core Support"
+	depends on USB
+	depends on VIRT_TO_BUS
+	select USB_OTG_UTILS
+	help
+	  Say Y or M here if your system has a Dual Role HighSpeed
+	  USB controller based on the DesignWare HSOTG IP Core.
+
+	  If you choose to build this driver as dynamically linked
+	  modules, the core module will be called dwc2.ko, the
+	  PCI bus interface module (if you have a PCI bus system)
+	  will be called dwc2_pci.ko and the platform interface module
+	  (for controllers directly connected to the CPU) will be called
+	  dwc2_platform.ko.
+
+	  NOTE: This driver at present only implements the Host mode
+	  of the controller. The existing s3c-hsotg driver supports
+	  Peripheral mode, but only for the Samsung S3C platforms.
+	  There are plans to merge the s3c-hsotg driver with this
+	  driver in the near future to create a dual-role driver.
+
+if USB_DWC2
+
+config USB_DWC2_DEBUG
+	bool "Enable Debugging Messages"
+	help
+	  Say Y here to enable debugging messages in the DWC2 Driver.
+
+config USB_DWC2_VERBOSE
+	bool "Enable Verbose Debugging Messages"
+	depends on USB_DWC2_DEBUG
+	help
+	  Say Y here to enable verbose debugging messages in the DWC2 Driver.
+	  WARNING: Enabling this will quickly fill your message log.
+	  If in doubt, say N.
+
+config USB_DWC2_TRACK_MISSED_SOFS
+	bool "Enable Missed SOF Tracking"
+	help
+	  Say Y here to enable logging of missed SOF events to the dmesg log.
+	  If in doubt, say N.
+
+config USB_DWC2_DEBUG_PERIODIC
+	bool "Enable Debugging Messages For Periodic Transfers"
+	depends on USB_DWC2_DEBUG || USB_DWC2_VERBOSE
+	default y
+	help
+	  Say N here to disable (verbose) debugging messages to be
+	  logged for periodic transfers. This allows better debugging of
+	  non-periodic transfers, but of course the debug logs will be
+	  incomplete. Note that this also disables some debug messages
+	  for which the transfer type cannot be deduced.
+endif
diff --git a/drivers/staging/dwc2/Makefile b/drivers/staging/dwc2/Makefile
new file mode 100644
index 00000000000..11529d3439b
--- /dev/null
+++ b/drivers/staging/dwc2/Makefile
@@ -0,0 +1,25 @@
+ccflags-$(CONFIG_USB_DWC2_DEBUG)	+= -DDEBUG
+ccflags-$(CONFIG_USB_DWC2_VERBOSE)	+= -DVERBOSE_DEBUG
+
+obj-$(CONFIG_USB_DWC2)			+= dwc2.o
+
+dwc2-y					+= core.o core_intr.o
+
+# NOTE: This driver at present only implements the Host mode
+# of the controller. The existing s3c-hsotg driver supports
+# Peripheral mode, but only for the Samsung S3C platforms.
+# There are plans to merge the s3c-hsotg driver with this
+# driver in the near future to create a dual-role driver. Once
+# that is done, Host mode will become an optional feature that
+# is selected with a config option.
+
+dwc2-y					+= hcd.o hcd_intr.o
+dwc2-y					+= hcd_queue.o hcd_ddma.o
+
+ifneq ($(CONFIG_PCI),)
+	obj-$(CONFIG_USB_DWC2)		+= dwc2_pci.o
+endif
+obj-$(CONFIG_USB_DWC2)			+= dwc2_platform.o
+
+dwc2_pci-y				+= pci.o
+dwc2_platform-y				+= platform.o
diff --git a/drivers/staging/dwc2/core.c b/drivers/staging/dwc2/core.c
new file mode 100644
index 00000000000..3177db2380b
--- /dev/null
+++ b/drivers/staging/dwc2/core.c
@@ -0,0 +1,2759 @@
+/*
+ * core.c - DesignWare HS OTG Controller common routines
+ *
+ * Copyright (C) 2004-2013 Synopsys, Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions, and the following disclaimer,
+ *    without modification.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The names of the above-listed copyright holders may not be used
+ *    to endorse or promote products derived from this software without
+ *    specific prior written permission.
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation; either version 2 of the License, or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+ * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * The Core code provides basic services for accessing and managing the
+ * DWC_otg hardware. These services are used by both the Host Controller
+ * Driver and the Peripheral Controller Driver.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/spinlock.h>
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/usb.h>
+
+#include <linux/usb/hcd.h>
+#include <linux/usb/ch11.h>
+
+#include "core.h"
+#include "hcd.h"
+
+/**
+ * dwc2_enable_common_interrupts() - Initializes the commmon interrupts,
+ * used in both device and host modes
+ *
+ * @hsotg: Programming view of the DWC_otg controller
+ */
+static void dwc2_enable_common_interrupts(struct dwc2_hsotg *hsotg)
+{
+	u32 intmsk;
+
+	/* Clear any pending OTG Interrupts */
+	writel(0xffffffff, hsotg->regs + GOTGINT);
+
+	/* Clear any pending interrupts */
+	writel(0xffffffff, hsotg->regs + GINTSTS);
+
+	/* Enable the interrupts in the GINTMSK */
+	intmsk = GINTSTS_MODEMIS | GINTSTS_OTGINT;
+
+	if (hsotg->core_params->dma_enable <= 0)
+		intmsk |= GINTSTS_RXFLVL;
+
+	intmsk |= GINTSTS_CONIDSTSCHNG | GINTSTS_WKUPINT | GINTSTS_USBSUSP |
+		  GINTSTS_SESSREQINT;
+
+	writel(intmsk, hsotg->regs + GINTMSK);
+}
+
+/*
+ * Initializes the FSLSPClkSel field of the HCFG register depending on the
+ * PHY type
+ */
+static void dwc2_init_fs_ls_pclk_sel(struct dwc2_hsotg *hsotg)
+{
+	u32 hs_phy_type = hsotg->hwcfg2 & GHWCFG2_HS_PHY_TYPE_MASK;
+	u32 fs_phy_type = hsotg->hwcfg2 & GHWCFG2_FS_PHY_TYPE_MASK;
+	u32 hcfg, val;
+
+	if ((hs_phy_type == GHWCFG2_HS_PHY_TYPE_ULPI &&
+	     fs_phy_type == GHWCFG2_FS_PHY_TYPE_DEDICATED &&
+	     hsotg->core_params->ulpi_fs_ls > 0) ||
+	    hsotg->core_params->phy_type == DWC2_PHY_TYPE_PARAM_FS) {
+		/* Full speed PHY */
+		val = HCFG_FSLSPCLKSEL_48_MHZ;
+	} else {
+		/* High speed PHY running at full speed or high speed */
+		val = HCFG_FSLSPCLKSEL_30_60_MHZ;
+	}
+
+	dev_dbg(hsotg->dev, "Initializing HCFG.FSLSPClkSel to %08x\n", val);
+	hcfg = readl(hsotg->regs + HCFG);
+	hcfg &= ~HCFG_FSLSPCLKSEL_MASK;
+	hcfg |= val;
+	writel(hcfg, hsotg->regs + HCFG);
+}
+
+/*
+ * Do core a soft reset of the core.  Be careful with this because it
+ * resets all the internal state machines of the core.
+ */
+static void dwc2_core_reset(struct dwc2_hsotg *hsotg)
+{
+	u32 greset;
+	int count = 0;
+
+	dev_vdbg(hsotg->dev, "%s()\n", __func__);
+
+	/* Wait for AHB master IDLE state */
+	do {
+		usleep_range(20000, 40000);
+		greset = readl(hsotg->regs + GRSTCTL);
+		if (++count > 50) {
+			dev_warn(hsotg->dev,
+				 "%s() HANG! AHB Idle GRSTCTL=%0x\n",
+				 __func__, greset);
+			return;
+		}
+	} while (!(greset & GRSTCTL_AHBIDLE));
+
+	/* Core Soft Reset */
+	count = 0;
+	greset |= GRSTCTL_CSFTRST;
+	writel(greset, hsotg->regs + GRSTCTL);
+	do {
+		usleep_range(20000, 40000);
+		greset = readl(hsotg->regs + GRSTCTL);
+		if (++count > 50) {
+			dev_warn(hsotg->dev,
+				 "%s() HANG! Soft Reset GRSTCTL=%0x\n",
+				 __func__, greset);
+			break;
+		}
+	} while (greset & GRSTCTL_CSFTRST);
+
+	/*
+	 * NOTE: This long sleep is _very_ important, otherwise the core will
+	 * not stay in host mode after a connector ID change!
+	 */
+	usleep_range(150000, 200000);
+}
+
+static void dwc2_fs_phy_init(struct dwc2_hsotg *hsotg, bool select_phy)
+{
+	u32 usbcfg, i2cctl;
+
+	/*
+	 * core_init() is now called on every switch so only call the
+	 * following for the first time through
+	 */
+	if (select_phy) {
+		dev_dbg(hsotg->dev, "FS PHY selected\n");
+		usbcfg = readl(hsotg->regs + GUSBCFG);
+		usbcfg |= GUSBCFG_PHYSEL;
+		writel(usbcfg, hsotg->regs + GUSBCFG);
+
+		/* Reset after a PHY select */
+		dwc2_core_reset(hsotg);
+	}
+
+	/*
+	 * Program DCFG.DevSpd or HCFG.FSLSPclkSel to 48Mhz in FS. Also
+	 * do this on HNP Dev/Host mode switches (done in dev_init and
+	 * host_init).
+	 */
+	if (dwc2_is_host_mode(hsotg))
+		dwc2_init_fs_ls_pclk_sel(hsotg);
+
+	if (hsotg->core_params->i2c_enable > 0) {
+		dev_dbg(hsotg->dev, "FS PHY enabling I2C\n");
+
+		/* Program GUSBCFG.OtgUtmiFsSel to I2C */
+		usbcfg = readl(hsotg->regs + GUSBCFG);
+		usbcfg |= GUSBCFG_OTG_UTMI_FS_SEL;
+		writel(usbcfg, hsotg->regs + GUSBCFG);
+
+		/* Program GI2CCTL.I2CEn */
+		i2cctl = readl(hsotg->regs + GI2CCTL);
+		i2cctl &= ~GI2CCTL_I2CDEVADDR_MASK;
+		i2cctl |= 1 << GI2CCTL_I2CDEVADDR_SHIFT;
+		i2cctl &= ~GI2CCTL_I2CEN;
+		writel(i2cctl, hsotg->regs + GI2CCTL);
+		i2cctl |= GI2CCTL_I2CEN;
+		writel(i2cctl, hsotg->regs + GI2CCTL);
+	}
+}
+
+static void dwc2_hs_phy_init(struct dwc2_hsotg *hsotg, bool select_phy)
+{
+	u32 usbcfg;
+
+	if (!select_phy)
+		return;
+
+	usbcfg = readl(hsotg->regs + GUSBCFG);
+
+	/*
+	 * HS PHY parameters. These parameters are preserved during soft reset
+	 * so only program the first time. Do a soft reset immediately after
+	 * setting phyif.
+	 */
+	switch (hsotg->core_params->phy_type) {
+	case DWC2_PHY_TYPE_PARAM_ULPI:
+		/* ULPI interface */
+		dev_dbg(hsotg->dev, "HS ULPI PHY selected\n");
+		usbcfg |= GUSBCFG_ULPI_UTMI_SEL;
+		usbcfg &= ~(GUSBCFG_PHYIF16 | GUSBCFG_DDRSEL);
+		if (hsotg->core_params->phy_ulpi_ddr > 0)
+			usbcfg |= GUSBCFG_DDRSEL;
+		break;
+	case DWC2_PHY_TYPE_PARAM_UTMI:
+		/* UTMI+ interface */
+		dev_dbg(hsotg->dev, "HS UTMI+ PHY selected\n");
+		usbcfg &= ~(GUSBCFG_ULPI_UTMI_SEL | GUSBCFG_PHYIF16);
+		if (hsotg->core_params->phy_utmi_width == 16)
+			usbcfg |= GUSBCFG_PHYIF16;
+		break;
+	default:
+		dev_err(hsotg->dev, "FS PHY selected at HS!\n");
+		break;
+	}
+
+	writel(usbcfg, hsotg->regs + GUSBCFG);
+
+	/* Reset after setting the PHY parameters */
+	dwc2_core_reset(hsotg);
+}
+
+static void dwc2_phy_init(struct dwc2_hsotg *hsotg, bool select_phy)
+{
+	u32 usbcfg, hs_phy_type, fs_phy_type;
+
+	if (hsotg->core_params->speed == DWC2_SPEED_PARAM_FULL &&
+	    hsotg->core_params->phy_type == DWC2_PHY_TYPE_PARAM_FS) {
+		/* If FS mode with FS PHY */
+		dwc2_fs_phy_init(hsotg, select_phy);
+	} else {
+		/* High speed PHY */
+		dwc2_hs_phy_init(hsotg, select_phy);
+	}
+
+	hs_phy_type = hsotg->hwcfg2 & GHWCFG2_HS_PHY_TYPE_MASK;
+	fs_phy_type = hsotg->hwcfg2 & GHWCFG2_FS_PHY_TYPE_MASK;
+
+	if (hs_phy_type == GHWCFG2_HS_PHY_TYPE_ULPI &&
+	    fs_phy_type == GHWCFG2_FS_PHY_TYPE_DEDICATED &&
+	    hsotg->core_params->ulpi_fs_ls > 0) {
+		dev_dbg(hsotg->dev, "Setting ULPI FSLS\n");
+		usbcfg = readl(hsotg->regs + GUSBCFG);
+		usbcfg |= GUSBCFG_ULPI_FS_LS;
+		usbcfg |= GUSBCFG_ULPI_CLK_SUSP_M;
+		writel(usbcfg, hsotg->regs + GUSBCFG);
+	} else {
+		usbcfg = readl(hsotg->regs + GUSBCFG);
+		usbcfg &= ~GUSBCFG_ULPI_FS_LS;
+		usbcfg &= ~GUSBCFG_ULPI_CLK_SUSP_M;
+		writel(usbcfg, hsotg->regs + GUSBCFG);
+	}
+}
+
+static int dwc2_gahbcfg_init(struct dwc2_hsotg *hsotg)
+{
+	u32 ahbcfg = 0;
+
+	switch (hsotg->hwcfg2 & GHWCFG2_ARCHITECTURE_MASK) {
+	case GHWCFG2_EXT_DMA_ARCH:
+		dev_err(hsotg->dev, "External DMA Mode not supported\n");
+		return -EINVAL;
+
+	case GHWCFG2_INT_DMA_ARCH:
+		dev_dbg(hsotg->dev, "Internal DMA Mode\n");
+		/*
+		 * Old value was GAHBCFG_HBSTLEN_INCR - done for
+		 * Host mode ISOC in issue fix - vahrama
+		 */
+		ahbcfg |= GAHBCFG_HBSTLEN_INCR4;
+		break;
+
+	case GHWCFG2_SLAVE_ONLY_ARCH:
+	default:
+		dev_dbg(hsotg->dev, "Slave Only Mode\n");
+		break;
+	}
+
+	dev_dbg(hsotg->dev, "dma_enable:%d dma_desc_enable:%d\n",
+		hsotg->core_params->dma_enable,
+		hsotg->core_params->dma_desc_enable);
+
+	if (hsotg->core_params->dma_enable > 0) {
+		if (hsotg->core_params->dma_desc_enable > 0)
+			dev_dbg(hsotg->dev, "Using Descriptor DMA mode\n");
+		else
+			dev_dbg(hsotg->dev, "Using Buffer DMA mode\n");
+	} else {
+		dev_dbg(hsotg->dev, "Using Slave mode\n");
+		hsotg->core_params->dma_desc_enable = 0;
+	}
+
+	if (hsotg->core_params->ahb_single > 0)
+		ahbcfg |= GAHBCFG_AHB_SINGLE;
+
+	if (hsotg->core_params->dma_enable > 0)
+		ahbcfg |= GAHBCFG_DMA_EN;
+
+	writel(ahbcfg, hsotg->regs + GAHBCFG);
+
+	return 0;
+}
+
+static void dwc2_gusbcfg_init(struct dwc2_hsotg *hsotg)
+{
+	u32 usbcfg;
+
+	usbcfg = readl(hsotg->regs + GUSBCFG);
+	usbcfg &= ~(GUSBCFG_HNPCAP | GUSBCFG_SRPCAP);
+
+	switch (hsotg->hwcfg2 & GHWCFG2_OP_MODE_MASK) {
+	case GHWCFG2_OP_MODE_HNP_SRP_CAPABLE:
+		if (hsotg->core_params->otg_cap ==
+				DWC2_CAP_PARAM_HNP_SRP_CAPABLE)
+			usbcfg |= GUSBCFG_HNPCAP;
+		if (hsotg->core_params->otg_cap !=
+				DWC2_CAP_PARAM_NO_HNP_SRP_CAPABLE)
+			usbcfg |= GUSBCFG_SRPCAP;
+		break;
+
+	case GHWCFG2_OP_MODE_SRP_ONLY_CAPABLE:
+	case GHWCFG2_OP_MODE_SRP_CAPABLE_DEVICE:
+	case GHWCFG2_OP_MODE_SRP_CAPABLE_HOST:
+		if (hsotg->core_params->otg_cap !=
+				DWC2_CAP_PARAM_NO_HNP_SRP_CAPABLE)
+			usbcfg |= GUSBCFG_SRPCAP;
+		break;
+
+	case GHWCFG2_OP_MODE_NO_HNP_SRP_CAPABLE:
+	case GHWCFG2_OP_MODE_NO_SRP_CAPABLE_DEVICE:
+	case GHWCFG2_OP_MODE_NO_SRP_CAPABLE_HOST:
+	default:
+		break;
+	}
+
+	writel(usbcfg, hsotg->regs + GUSBCFG);
+}
+
+/**
+ * dwc2_core_init() - Initializes the DWC_otg controller registers and
+ * prepares the core for device mode or host mode operation
+ *
+ * @hsotg:      Programming view of the DWC_otg controller
+ * @select_phy: If true then also set the Phy type
+ * @irq:        If >= 0, the irq to register
+ */
+int dwc2_core_init(struct dwc2_hsotg *hsotg, bool select_phy, int irq)
+{
+	u32 usbcfg, otgctl;
+	int retval;
+
+	dev_dbg(hsotg->dev, "%s(%p)\n", __func__, hsotg);
+
+	usbcfg = readl(hsotg->regs + GUSBCFG);
+
+	/* Set ULPI External VBUS bit if needed */
+	usbcfg &= ~GUSBCFG_ULPI_EXT_VBUS_DRV;
+	if (hsotg->core_params->phy_ulpi_ext_vbus ==
+				DWC2_PHY_ULPI_EXTERNAL_VBUS)
+		usbcfg |= GUSBCFG_ULPI_EXT_VBUS_DRV;
+
+	/* Set external TS Dline pulsing bit if needed */
+	usbcfg &= ~GUSBCFG_TERMSELDLPULSE;
+	if (hsotg->core_params->ts_dline > 0)
+		usbcfg |= GUSBCFG_TERMSELDLPULSE;
+
+	writel(usbcfg, hsotg->regs + GUSBCFG);
+
+	/* Reset the Controller */
+	dwc2_core_reset(hsotg);
+
+	dev_dbg(hsotg->dev, "num_dev_perio_in_ep=%d\n",
+		hsotg->hwcfg4 >> GHWCFG4_NUM_DEV_PERIO_IN_EP_SHIFT &
+		GHWCFG4_NUM_DEV_PERIO_IN_EP_MASK >>
+				GHWCFG4_NUM_DEV_PERIO_IN_EP_SHIFT);
+
+	hsotg->total_fifo_size = hsotg->hwcfg3 >> GHWCFG3_DFIFO_DEPTH_SHIFT &
+			GHWCFG3_DFIFO_DEPTH_MASK >> GHWCFG3_DFIFO_DEPTH_SHIFT;
+	hsotg->rx_fifo_size = readl(hsotg->regs + GRXFSIZ);
+	hsotg->nperio_tx_fifo_size =
+			readl(hsotg->regs + GNPTXFSIZ) >> 16 & 0xffff;
+
+	dev_dbg(hsotg->dev, "Total FIFO SZ=%d\n", hsotg->total_fifo_size);
+	dev_dbg(hsotg->dev, "RxFIFO SZ=%d\n", hsotg->rx_fifo_size);
+	dev_dbg(hsotg->dev, "NP TxFIFO SZ=%d\n", hsotg->nperio_tx_fifo_size);
+
+	/*
+	 * This needs to happen in FS mode before any other programming occurs
+	 */
+	dwc2_phy_init(hsotg, select_phy);
+
+	/* Program the GAHBCFG Register */
+	retval = dwc2_gahbcfg_init(hsotg);
+	if (retval)
+		return retval;
+
+	/* Program the GUSBCFG register */
+	dwc2_gusbcfg_init(hsotg);
+
+	/* Program the GOTGCTL register */
+	otgctl = readl(hsotg->regs + GOTGCTL);
+	otgctl &= ~GOTGCTL_OTGVER;
+	if (hsotg->core_params->otg_ver > 0)
+		otgctl |= GOTGCTL_OTGVER;
+	writel(otgctl, hsotg->regs + GOTGCTL);
+	dev_dbg(hsotg->dev, "OTG VER PARAM: %d\n", hsotg->core_params->otg_ver);
+
+	/* Clear the SRP success bit for FS-I2c */
+	hsotg->srp_success = 0;
+
+	if (irq >= 0) {
+		dev_dbg(hsotg->dev, "registering common handler for irq%d\n",
+			irq);
+		retval = devm_request_irq(hsotg->dev, irq,
+					  dwc2_handle_common_intr, IRQF_SHARED,
+					  dev_name(hsotg->dev), hsotg);
+		if (retval)
+			return retval;
+	}
+
+	/* Enable common interrupts */
+	dwc2_enable_common_interrupts(hsotg);
+
+	/*
+	 * Do device or host intialization based on mode during PCD and
+	 * HCD initialization
+	 */
+	if (dwc2_is_host_mode(hsotg)) {
+		dev_dbg(hsotg->dev, "Host Mode\n");
+		hsotg->op_state = OTG_STATE_A_HOST;
+	} else {
+		dev_dbg(hsotg->dev, "Device Mode\n");
+		hsotg->op_state = OTG_STATE_B_PERIPHERAL;
+	}
+
+	return 0;
+}
+
+/**
+ * dwc2_enable_host_interrupts() - Enables the Host mode interrupts
+ *
+ * @hsotg: Programming view of DWC_otg controller
+ */
+void dwc2_enable_host_interrupts(struct dwc2_hsotg *hsotg)
+{
+	u32 intmsk;
+
+	dev_dbg(hsotg->dev, "%s()\n", __func__);
+
+	/* Disable all interrupts */
+	writel(0, hsotg->regs + GINTMSK);
+	writel(0, hsotg->regs + HAINTMSK);
+
+	/* Clear any pending interrupts */
+	writel(0xffffffff, hsotg->regs + GINTSTS);
+
+	/* Enable the common interrupts */
+	dwc2_enable_common_interrupts(hsotg);
+
+	/* Enable host mode interrupts without disturbing common interrupts */
+	intmsk = readl(hsotg->regs + GINTMSK);
+	intmsk |= GINTSTS_DISCONNINT | GINTSTS_PRTINT | GINTSTS_HCHINT;
+	writel(intmsk, hsotg->regs + GINTMSK);
+}
+
+/**
+ * dwc2_disable_host_interrupts() - Disables the Host Mode interrupts
+ *
+ * @hsotg: Programming view of DWC_otg controller
+ */
+void dwc2_disable_host_interrupts(struct dwc2_hsotg *hsotg)
+{
+	u32 intmsk = readl(hsotg->regs + GINTMSK);
+
+	/* Disable host mode interrupts without disturbing common interrupts */
+	intmsk &= ~(GINTSTS_SOF | GINTSTS_PRTINT | GINTSTS_HCHINT |
+		    GINTSTS_PTXFEMP | GINTSTS_NPTXFEMP);
+	writel(intmsk, hsotg->regs + GINTMSK);
+}
+
+static void dwc2_config_fifos(struct dwc2_hsotg *hsotg)
+{
+	struct dwc2_core_params *params = hsotg->core_params;
+	u32 rxfsiz, nptxfsiz, ptxfsiz, hptxfsiz, dfifocfg;
+
+	if (!(hsotg->hwcfg2 & GHWCFG2_DYNAMIC_FIFO) ||
+	    !params->enable_dynamic_fifo)
+		return;
+
+	dev_dbg(hsotg->dev, "Total FIFO Size=%d\n", hsotg->total_fifo_size);
+	dev_dbg(hsotg->dev, "Rx FIFO Size=%d\n", params->host_rx_fifo_size);
+	dev_dbg(hsotg->dev, "NP Tx FIFO Size=%d\n",
+		params->host_nperio_tx_fifo_size);
+	dev_dbg(hsotg->dev, "P Tx FIFO Size=%d\n",
+		params->host_perio_tx_fifo_size);
+
+	/* Rx FIFO */
+	dev_dbg(hsotg->dev, "initial grxfsiz=%08x\n",
+		readl(hsotg->regs + GRXFSIZ));
+	writel(params->host_rx_fifo_size, hsotg->regs + GRXFSIZ);
+	dev_dbg(hsotg->dev, "new grxfsiz=%08x\n", readl(hsotg->regs + GRXFSIZ));
+
+	/* Non-periodic Tx FIFO */
+	dev_dbg(hsotg->dev, "initial gnptxfsiz=%08x\n",
+		readl(hsotg->regs + GNPTXFSIZ));
+	nptxfsiz = params->host_nperio_tx_fifo_size <<
+		   FIFOSIZE_DEPTH_SHIFT & FIFOSIZE_DEPTH_MASK;
+	nptxfsiz |= params->host_rx_fifo_size <<
+		    FIFOSIZE_STARTADDR_SHIFT & FIFOSIZE_STARTADDR_MASK;
+	writel(nptxfsiz, hsotg->regs + GNPTXFSIZ);
+	dev_dbg(hsotg->dev, "new gnptxfsiz=%08x\n",
+		readl(hsotg->regs + GNPTXFSIZ));
+
+	/* Periodic Tx FIFO */
+	dev_dbg(hsotg->dev, "initial hptxfsiz=%08x\n",
+		readl(hsotg->regs + HPTXFSIZ));
+	ptxfsiz = params->host_perio_tx_fifo_size <<
+		  FIFOSIZE_DEPTH_SHIFT & FIFOSIZE_DEPTH_MASK;
+	ptxfsiz |= (params->host_rx_fifo_size +
+		    params->host_nperio_tx_fifo_size) <<
+		   FIFOSIZE_STARTADDR_SHIFT & FIFOSIZE_STARTADDR_MASK;
+	writel(ptxfsiz, hsotg->regs + HPTXFSIZ);
+	dev_dbg(hsotg->dev, "new hptxfsiz=%08x\n",
+		readl(hsotg->regs + HPTXFSIZ));
+
+	if (hsotg->core_params->en_multiple_tx_fifo > 0 &&
+	    hsotg->snpsid <= DWC2_CORE_REV_2_94a) {
+		/*
+		 * Global DFIFOCFG calculation for Host mode -
+		 * include RxFIFO, NPTXFIFO and HPTXFIFO
+		 */
+		dfifocfg = readl(hsotg->regs + GDFIFOCFG);
+		rxfsiz = readl(hsotg->regs + GRXFSIZ) & 0x0000ffff;
+		nptxfsiz = readl(hsotg->regs + GNPTXFSIZ) >> 16 & 0xffff;
+		hptxfsiz = readl(hsotg->regs + HPTXFSIZ) >> 16 & 0xffff;
+		dfifocfg &= ~GDFIFOCFG_EPINFOBASE_MASK;
+		dfifocfg |= (rxfsiz + nptxfsiz + hptxfsiz) <<
+			    GDFIFOCFG_EPINFOBASE_SHIFT &
+			    GDFIFOCFG_EPINFOBASE_MASK;
+		writel(dfifocfg, hsotg->regs + GDFIFOCFG);
+	}
+}
+
+/**
+ * dwc2_core_host_init() - Initializes the DWC_otg controller registers for
+ * Host mode
+ *
+ * @hsotg: Programming view of DWC_otg controller
+ *
+ * This function flushes the Tx and Rx FIFOs and flushes any entries in the
+ * request queues. Host channels are reset to ensure that they are ready for
+ * performing transfers.
+ */
+void dwc2_core_host_init(struct dwc2_hsotg *hsotg)
+{
+	u32 hcfg, hfir, otgctl;
+
+	dev_dbg(hsotg->dev, "%s(%p)\n", __func__, hsotg);
+
+	/* Restart the Phy Clock */
+	writel(0, hsotg->regs + PCGCTL);
+
+	/* Initialize Host Configuration Register */
+	dwc2_init_fs_ls_pclk_sel(hsotg);
+	if (hsotg->core_params->speed == DWC2_SPEED_PARAM_FULL) {
+		hcfg = readl(hsotg->regs + HCFG);
+		hcfg |= HCFG_FSLSSUPP;
+		writel(hcfg, hsotg->regs + HCFG);
+	}
+
+	/*
+	 * This bit allows dynamic reloading of the HFIR register during
+	 * runtime. This bit needs to be programmed during inital configuration
+	 * and its value must not be changed during runtime.
+	 */
+	if (hsotg->core_params->reload_ctl > 0) {
+		hfir = readl(hsotg->regs + HFIR);
+		hfir |= HFIR_RLDCTRL;
+		writel(hfir, hsotg->regs + HFIR);
+	}
+
+	if (hsotg->core_params->dma_desc_enable > 0) {
+		u32 op_mode = hsotg->hwcfg2 & GHWCFG2_OP_MODE_MASK;
+
+		if (hsotg->snpsid < DWC2_CORE_REV_2_90a ||
+		    !(hsotg->hwcfg4 & GHWCFG4_DESC_DMA) ||
+		    op_mode == GHWCFG2_OP_MODE_SRP_CAPABLE_DEVICE ||
+		    op_mode == GHWCFG2_OP_MODE_NO_SRP_CAPABLE_DEVICE ||
+		    op_mode == GHWCFG2_OP_MODE_UNDEFINED) {
+			dev_err(hsotg->dev,
+				"Hardware does not support descriptor DMA mode -\n");
+			dev_err(hsotg->dev,
+				"falling back to buffer DMA mode.\n");
+			hsotg->core_params->dma_desc_enable = 0;
+		} else {
+			hcfg = readl(hsotg->regs + HCFG);
+			hcfg |= HCFG_DESCDMA;
+			writel(hcfg, hsotg->regs + HCFG);
+		}
+	}
+
+	/* Configure data FIFO sizes */
+	dwc2_config_fifos(hsotg);
+
+	/* TODO - check this */
+	/* Clear Host Set HNP Enable in the OTG Control Register */
+	otgctl = readl(hsotg->regs + GOTGCTL);
+	otgctl &= ~GOTGCTL_HSTSETHNPEN;
+	writel(otgctl, hsotg->regs + GOTGCTL);
+
+	/* Make sure the FIFOs are flushed */
+	dwc2_flush_tx_fifo(hsotg, 0x10 /* all TX FIFOs */);
+	dwc2_flush_rx_fifo(hsotg);
+
+	/* Clear Host Set HNP Enable in the OTG Control Register */
+	otgctl = readl(hsotg->regs + GOTGCTL);
+	otgctl &= ~GOTGCTL_HSTSETHNPEN;
+	writel(otgctl, hsotg->regs + GOTGCTL);
+
+	if (hsotg->core_params->dma_desc_enable <= 0) {
+		int num_channels, i;
+		u32 hcchar;
+
+		/* Flush out any leftover queued requests */
+		num_channels = hsotg->core_params->host_channels;
+		for (i = 0; i < num_channels; i++) {
+			hcchar = readl(hsotg->regs + HCCHAR(i));
+			hcchar &= ~HCCHAR_CHENA;
+			hcchar |= HCCHAR_CHDIS;
+			hcchar &= ~HCCHAR_EPDIR;
+			writel(hcchar, hsotg->regs + HCCHAR(i));
+		}
+
+		/* Halt all channels to put them into a known state */
+		for (i = 0; i < num_channels; i++) {
+			int count = 0;
+
+			hcchar = readl(hsotg->regs + HCCHAR(i));
+			hcchar |= HCCHAR_CHENA | HCCHAR_CHDIS;
+			hcchar &= ~HCCHAR_EPDIR;
+			writel(hcchar, hsotg->regs + HCCHAR(i));
+			dev_dbg(hsotg->dev, "%s: Halt channel %d\n",
+				__func__, i);
+			do {
+				hcchar = readl(hsotg->regs + HCCHAR(i));
+				if (++count > 1000) {
+					dev_err(hsotg->dev,
+						"Unable to clear enable on channel %d\n",
+						i);
+					break;
+				}
+				udelay(1);
+			} while (hcchar & HCCHAR_CHENA);
+		}
+	}
+
+	/* Turn on the vbus power */
+	dev_dbg(hsotg->dev, "Init: Port Power? op_state=%d\n", hsotg->op_state);
+	if (hsotg->op_state == OTG_STATE_A_HOST) {
+		u32 hprt0 = dwc2_read_hprt0(hsotg);
+
+		dev_dbg(hsotg->dev, "Init: Power Port (%d)\n",
+			!!(hprt0 & HPRT0_PWR));
+		if (!(hprt0 & HPRT0_PWR)) {
+			hprt0 |= HPRT0_PWR;
+			writel(hprt0, hsotg->regs + HPRT0);
+		}
+	}
+
+	dwc2_enable_host_interrupts(hsotg);
+}
+
+static void dwc2_hc_enable_slave_ints(struct dwc2_hsotg *hsotg,
+				      struct dwc2_host_chan *chan)
+{
+	u32 hcintmsk = HCINTMSK_CHHLTD;
+
+	switch (chan->ep_type) {
+	case USB_ENDPOINT_XFER_CONTROL:
+	case USB_ENDPOINT_XFER_BULK:
+		dev_vdbg(hsotg->dev, "control/bulk\n");
+		hcintmsk |= HCINTMSK_XFERCOMPL;
+		hcintmsk |= HCINTMSK_STALL;
+		hcintmsk |= HCINTMSK_XACTERR;
+		hcintmsk |= HCINTMSK_DATATGLERR;
+		if (chan->ep_is_in) {
+			hcintmsk |= HCINTMSK_BBLERR;
+		} else {
+			hcintmsk |= HCINTMSK_NAK;
+			hcintmsk |= HCINTMSK_NYET;
+			if (chan->do_ping)
+				hcintmsk |= HCINTMSK_ACK;
+		}
+
+		if (chan->do_split) {
+			hcintmsk |= HCINTMSK_NAK;
+			if (chan->complete_split)
+				hcintmsk |= HCINTMSK_NYET;
+			else
+				hcintmsk |= HCINTMSK_ACK;
+		}
+
+		if (chan->error_state)
+			hcintmsk |= HCINTMSK_ACK;
+		break;
+
+	case USB_ENDPOINT_XFER_INT:
+		if (dbg_perio())
+			dev_vdbg(hsotg->dev, "intr\n");
+		hcintmsk |= HCINTMSK_XFERCOMPL;
+		hcintmsk |= HCINTMSK_NAK;
+		hcintmsk |= HCINTMSK_STALL;
+		hcintmsk |= HCINTMSK_XACTERR;
+		hcintmsk |= HCINTMSK_DATATGLERR;
+		hcintmsk |= HCINTMSK_FRMOVRUN;
+
+		if (chan->ep_is_in)
+			hcintmsk |= HCINTMSK_BBLERR;
+		if (chan->error_state)
+			hcintmsk |= HCINTMSK_ACK;
+		if (chan->do_split) {
+			if (chan->complete_split)
+				hcintmsk |= HCINTMSK_NYET;
+			else
+				hcintmsk |= HCINTMSK_ACK;
+		}
+		break;
+
+	case USB_ENDPOINT_XFER_ISOC:
+		if (dbg_perio())
+			dev_vdbg(hsotg->dev, "isoc\n");
+		hcintmsk |= HCINTMSK_XFERCOMPL;
+		hcintmsk |= HCINTMSK_FRMOVRUN;
+		hcintmsk |= HCINTMSK_ACK;
+
+		if (chan->ep_is_in) {
+			hcintmsk |= HCINTMSK_XACTERR;
+			hcintmsk |= HCINTMSK_BBLERR;
+		}
+		break;
+	default:
+		dev_err(hsotg->dev, "## Unknown EP type ##\n");
+		break;
+	}
+
+	writel(hcintmsk, hsotg->regs + HCINTMSK(chan->hc_num));
+	if (dbg_hc(chan))
+		dev_vdbg(hsotg->dev, "set HCINTMSK to %08x\n", hcintmsk);
+}
+
+static void dwc2_hc_enable_dma_ints(struct dwc2_hsotg *hsotg,
+				    struct dwc2_host_chan *chan)
+{
+	u32 hcintmsk = HCINTMSK_CHHLTD;
+
+	/*
+	 * For Descriptor DMA mode core halts the channel on AHB error.
+	 * Interrupt is not required.
+	 */
+	if (hsotg->core_params->dma_desc_enable <= 0) {
+		if (dbg_hc(chan))
+			dev_vdbg(hsotg->dev, "desc DMA disabled\n");
+		hcintmsk |= HCINTMSK_AHBERR;
+	} else {
+		if (dbg_hc(chan))
+			dev_vdbg(hsotg->dev, "desc DMA enabled\n");
+		if (chan->ep_type == USB_ENDPOINT_XFER_ISOC)
+			hcintmsk |= HCINTMSK_XFERCOMPL;
+	}
+
+	if (chan->error_state && !chan->do_split &&
+	    chan->ep_type != USB_ENDPOINT_XFER_ISOC) {
+		if (dbg_hc(chan))
+			dev_vdbg(hsotg->dev, "setting ACK\n");
+		hcintmsk |= HCINTMSK_ACK;
+		if (chan->ep_is_in) {
+			hcintmsk |= HCINTMSK_DATATGLERR;
+			if (chan->ep_type != USB_ENDPOINT_XFER_INT)
+				hcintmsk |= HCINTMSK_NAK;
+		}
+	}
+
+	writel(hcintmsk, hsotg->regs + HCINTMSK(chan->hc_num));
+	if (dbg_hc(chan))
+		dev_vdbg(hsotg->dev, "set HCINTMSK to %08x\n", hcintmsk);
+}
+
+static void dwc2_hc_enable_ints(struct dwc2_hsotg *hsotg,
+				struct dwc2_host_chan *chan)
+{
+	u32 intmsk;
+
+	if (hsotg->core_params->dma_enable > 0) {
+		if (dbg_hc(chan))
+			dev_vdbg(hsotg->dev, "DMA enabled\n");
+		dwc2_hc_enable_dma_ints(hsotg, chan);
+	} else {
+		if (dbg_hc(chan))
+			dev_vdbg(hsotg->dev, "DMA disabled\n");
+		dwc2_hc_enable_slave_ints(hsotg, chan);
+	}
+
+	/* Enable the top level host channel interrupt */
+	intmsk = readl(hsotg->regs + HAINTMSK);
+	intmsk |= 1 << chan->hc_num;
+	writel(intmsk, hsotg->regs + HAINTMSK);
+	if (dbg_hc(chan))
+		dev_vdbg(hsotg->dev, "set HAINTMSK to %08x\n", intmsk);
+
+	/* Make sure host channel interrupts are enabled */
+	intmsk = readl(hsotg->regs + GINTMSK);
+	intmsk |= GINTSTS_HCHINT;
+	writel(intmsk, hsotg->regs + GINTMSK);
+	if (dbg_hc(chan))
+		dev_vdbg(hsotg->dev, "set GINTMSK to %08x\n", intmsk);
+}
+
+/**
+ * dwc2_hc_init() - Prepares a host channel for transferring packets to/from
+ * a specific endpoint
+ *
+ * @hsotg: Programming view of DWC_otg controller
+ * @chan:  Information needed to initialize the host channel
+ *
+ * The HCCHARn register is set up with the characteristics specified in chan.
+ * Host channel interrupts that may need to be serviced while this transfer is
+ * in progress are enabled.
+ */
+void dwc2_hc_init(struct dwc2_hsotg *hsotg, struct dwc2_host_chan *chan)
+{
+	u8 hc_num = chan->hc_num;
+	u32 hcintmsk;
+	u32 hcchar;
+	u32 hcsplt = 0;
+
+	if (dbg_hc(chan))
+		dev_vdbg(hsotg->dev, "%s()\n", __func__);
+
+	/* Clear old interrupt conditions for this host channel */
+	hcintmsk = 0xffffffff;
+	hcintmsk &= ~HCINTMSK_RESERVED14_31;
+	writel(hcintmsk, hsotg->regs + HCINT(hc_num));
+
+	/* Enable channel interrupts required for this transfer */
+	dwc2_hc_enable_ints(hsotg, chan);
+
+	/*
+	 * Program the HCCHARn register with the endpoint characteristics for
+	 * the current transfer
+	 */
+	hcchar = chan->dev_addr << HCCHAR_DEVADDR_SHIFT & HCCHAR_DEVADDR_MASK;
+	hcchar |= chan->ep_num << HCCHAR_EPNUM_SHIFT & HCCHAR_EPNUM_MASK;
+	if (chan->ep_is_in)
+		hcchar |= HCCHAR_EPDIR;
+	if (chan->speed == USB_SPEED_LOW)
+		hcchar |= HCCHAR_LSPDDEV;
+	hcchar |= chan->ep_type << HCCHAR_EPTYPE_SHIFT & HCCHAR_EPTYPE_MASK;
+	hcchar |= chan->max_packet << HCCHAR_MPS_SHIFT & HCCHAR_MPS_MASK;
+	writel(hcchar, hsotg->regs + HCCHAR(hc_num));
+	if (dbg_hc(chan)) {
+		dev_vdbg(hsotg->dev, "set HCCHAR(%d) to %08x\n",
+			 hc_num, hcchar);
+
+		dev_vdbg(hsotg->dev, "%s: Channel %d\n", __func__, hc_num);
+		dev_vdbg(hsotg->dev, "	 Dev Addr: %d\n",
+			 hcchar >> HCCHAR_DEVADDR_SHIFT &
+			 HCCHAR_DEVADDR_MASK >> HCCHAR_DEVADDR_SHIFT);
+		dev_vdbg(hsotg->dev, "	 Ep Num: %d\n",
+			 hcchar >> HCCHAR_EPNUM_SHIFT &
+			 HCCHAR_EPNUM_MASK >> HCCHAR_EPNUM_SHIFT);
+		dev_vdbg(hsotg->dev, "	 Is In: %d\n",
+			 !!(hcchar & HCCHAR_EPDIR));
+		dev_vdbg(hsotg->dev, "	 Is Low Speed: %d\n",
+			 !!(hcchar & HCCHAR_LSPDDEV));
+		dev_vdbg(hsotg->dev, "	 Ep Type: %d\n",
+			 hcchar >> HCCHAR_EPTYPE_SHIFT &
+			 HCCHAR_EPTYPE_MASK >> HCCHAR_EPTYPE_SHIFT);
+		dev_vdbg(hsotg->dev, "	 Max Pkt: %d\n",
+			 hcchar >> HCCHAR_MPS_SHIFT &
+			 HCCHAR_MPS_MASK >> HCCHAR_MPS_SHIFT);
+		dev_vdbg(hsotg->dev, "	 Multi Cnt: %d\n",
+			 hcchar >> HCCHAR_MULTICNT_SHIFT &
+			 HCCHAR_MULTICNT_MASK >> HCCHAR_MULTICNT_SHIFT);
+	}
+
+	/* Program the HCSPLT register for SPLITs */
+	if (chan->do_split) {
+		if (dbg_hc(chan))
+			dev_vdbg(hsotg->dev,
+				 "Programming HC %d with split --> %s\n",
+				 hc_num,
+				 chan->complete_split ? "CSPLIT" : "SSPLIT");
+		if (chan->complete_split)
+			hcsplt |= HCSPLT_COMPSPLT;
+		hcsplt |= chan->xact_pos << HCSPLT_XACTPOS_SHIFT &
+			  HCSPLT_XACTPOS_MASK;
+		hcsplt |= chan->hub_addr << HCSPLT_HUBADDR_SHIFT &
+			  HCSPLT_HUBADDR_MASK;
+		hcsplt |= chan->hub_port << HCSPLT_PRTADDR_SHIFT &
+			  HCSPLT_PRTADDR_MASK;
+		if (dbg_hc(chan)) {
+			dev_vdbg(hsotg->dev, "	  comp split %d\n",
+				 chan->complete_split);
+			dev_vdbg(hsotg->dev, "	  xact pos %d\n",
+				 chan->xact_pos);
+			dev_vdbg(hsotg->dev, "	  hub addr %d\n",
+				 chan->hub_addr);
+			dev_vdbg(hsotg->dev, "	  hub port %d\n",
+				 chan->hub_port);
+			dev_vdbg(hsotg->dev, "	  is_in %d\n",
+				 chan->ep_is_in);
+			dev_vdbg(hsotg->dev, "	  Max Pkt %d\n",
+				 hcchar >> HCCHAR_MPS_SHIFT &
+				 HCCHAR_MPS_MASK >> HCCHAR_MPS_SHIFT);
+			dev_vdbg(hsotg->dev, "	  xferlen %d\n",
+				 chan->xfer_len);
+		}
+	}
+
+	writel(hcsplt, hsotg->regs + HCSPLT(hc_num));
+}
+
+/**
+ * dwc2_hc_halt() - Attempts to halt a host channel
+ *
+ * @hsotg:       Controller register interface
+ * @chan:        Host channel to halt
+ * @halt_status: Reason for halting the channel
+ *
+ * This function should only be called in Slave mode or to abort a transfer in
+ * either Slave mode or DMA mode. Under normal circumstances in DMA mode, the
+ * controller halts the channel when the transfer is complete or a condition
+ * occurs that requires application intervention.
+ *
+ * In slave mode, checks for a free request queue entry, then sets the Channel
+ * Enable and Channel Disable bits of the Host Channel Characteristics
+ * register of the specified channel to intiate the halt. If there is no free
+ * request queue entry, sets only the Channel Disable bit of the HCCHARn
+ * register to flush requests for this channel. In the latter case, sets a
+ * flag to indicate that the host channel needs to be halted when a request
+ * queue slot is open.
+ *
+ * In DMA mode, always sets the Channel Enable and Channel Disable bits of the
+ * HCCHARn register. The controller ensures there is space in the request
+ * queue before submitting the halt request.
+ *
+ * Some time may elapse before the core flushes any posted requests for this
+ * host channel and halts. The Channel Halted interrupt handler completes the
+ * deactivation of the host channel.
+ */
+void dwc2_hc_halt(struct dwc2_hsotg *hsotg, struct dwc2_host_chan *chan,
+		  enum dwc2_halt_status halt_status)
+{
+	u32 nptxsts, hptxsts, hcchar;
+
+	if (dbg_hc(chan))
+		dev_vdbg(hsotg->dev, "%s()\n", __func__);
+	if (halt_status == DWC2_HC_XFER_NO_HALT_STATUS)
+		dev_err(hsotg->dev, "!!! halt_status = %d !!!\n", halt_status);
+
+	if (halt_status == DWC2_HC_XFER_URB_DEQUEUE ||
+	    halt_status == DWC2_HC_XFER_AHB_ERR) {
+		/*
+		 * Disable all channel interrupts except Ch Halted. The QTD
+		 * and QH state associated with this transfer has been cleared
+		 * (in the case of URB_DEQUEUE), so the channel needs to be
+		 * shut down carefully to prevent crashes.
+		 */
+		u32 hcintmsk = HCINTMSK_CHHLTD;
+
+		dev_vdbg(hsotg->dev, "dequeue/error\n");
+		writel(hcintmsk, hsotg->regs + HCINTMSK(chan->hc_num));
+
+		/*
+		 * Make sure no other interrupts besides halt are currently
+		 * pending. Handling another interrupt could cause a crash due
+		 * to the QTD and QH state.
+		 */
+		writel(~hcintmsk, hsotg->regs + HCINT(chan->hc_num));
+
+		/*
+		 * Make sure the halt status is set to URB_DEQUEUE or AHB_ERR
+		 * even if the channel was already halted for some other
+		 * reason
+		 */
+		chan->halt_status = halt_status;
+
+		hcchar = readl(hsotg->regs + HCCHAR(chan->hc_num));
+		if (!(hcchar & HCCHAR_CHENA)) {
+			/*
+			 * The channel is either already halted or it hasn't
+			 * started yet. In DMA mode, the transfer may halt if
+			 * it finishes normally or a condition occurs that
+			 * requires driver intervention. Don't want to halt
+			 * the channel again. In either Slave or DMA mode,
+			 * it's possible that the transfer has been assigned
+			 * to a channel, but not started yet when an URB is
+			 * dequeued. Don't want to halt a channel that hasn't
+			 * started yet.
+			 */
+			return;
+		}
+	}
+	if (chan->halt_pending) {
+		/*
+		 * A halt has already been issued for this channel. This might
+		 * happen when a transfer is aborted by a higher level in
+		 * the stack.
+		 */
+		dev_vdbg(hsotg->dev,
+			 "*** %s: Channel %d, chan->halt_pending already set ***\n",
+			 __func__, chan->hc_num);
+		return;
+	}
+
+	hcchar = readl(hsotg->regs + HCCHAR(chan->hc_num));
+
+	/* No need to set the bit in DDMA for disabling the channel */
+	/* TODO check it everywhere channel is disabled */
+	if (hsotg->core_params->dma_desc_enable <= 0) {
+		if (dbg_hc(chan))
+			dev_vdbg(hsotg->dev, "desc DMA disabled\n");
+		hcchar |= HCCHAR_CHENA;
+	} else {
+		if (dbg_hc(chan))
+			dev_dbg(hsotg->dev, "desc DMA enabled\n");
+	}
+	hcchar |= HCCHAR_CHDIS;
+
+	if (hsotg->core_params->dma_enable <= 0) {
+		if (dbg_hc(chan))
+			dev_vdbg(hsotg->dev, "DMA not enabled\n");
+		hcchar |= HCCHAR_CHENA;
+
+		/* Check for space in the request queue to issue the halt */
+		if (chan->ep_type == USB_ENDPOINT_XFER_CONTROL ||
+		    chan->ep_type == USB_ENDPOINT_XFER_BULK) {
+			dev_vdbg(hsotg->dev, "control/bulk\n");
+			nptxsts = readl(hsotg->regs + GNPTXSTS);
+			if ((nptxsts & TXSTS_QSPCAVAIL_MASK) == 0) {
+				dev_vdbg(hsotg->dev, "Disabling channel\n");
+				hcchar &= ~HCCHAR_CHENA;
+			}
+		} else {
+			if (dbg_perio())
+				dev_vdbg(hsotg->dev, "isoc/intr\n");
+			hptxsts = readl(hsotg->regs + HPTXSTS);
+			if ((hptxsts & TXSTS_QSPCAVAIL_MASK) == 0 ||
+			    hsotg->queuing_high_bandwidth) {
+				if (dbg_perio())
+					dev_vdbg(hsotg->dev, "Disabling channel\n");
+				hcchar &= ~HCCHAR_CHENA;
+			}
+		}
+	} else {
+		if (dbg_hc(chan))
+			dev_vdbg(hsotg->dev, "DMA enabled\n");
+	}
+
+	writel(hcchar, hsotg->regs + HCCHAR(chan->hc_num));
+	chan->halt_status = halt_status;
+
+	if (hcchar & HCCHAR_CHENA) {
+		if (dbg_hc(chan))
+			dev_vdbg(hsotg->dev, "Channel enabled\n");
+		chan->halt_pending = 1;
+		chan->halt_on_queue = 0;
+	} else {
+		if (dbg_hc(chan))
+			dev_vdbg(hsotg->dev, "Channel disabled\n");
+		chan->halt_on_queue = 1;
+	}
+
+	if (dbg_hc(chan)) {
+		dev_vdbg(hsotg->dev, "%s: Channel %d\n", __func__,
+			 chan->hc_num);
+		dev_vdbg(hsotg->dev, "	 hcchar: 0x%08x\n",
+			 hcchar);
+		dev_vdbg(hsotg->dev, "	 halt_pending: %d\n",
+			 chan->halt_pending);
+		dev_vdbg(hsotg->dev, "	 halt_on_queue: %d\n",
+			 chan->halt_on_queue);
+		dev_vdbg(hsotg->dev, "	 halt_status: %d\n",
+			 chan->halt_status);
+	}
+}
+
+/**
+ * dwc2_hc_cleanup() - Clears the transfer state for a host channel
+ *
+ * @hsotg: Programming view of DWC_otg controller
+ * @chan:  Identifies the host channel to clean up
+ *
+ * This function is normally called after a transfer is done and the host
+ * channel is being released
+ */
+void dwc2_hc_cleanup(struct dwc2_hsotg *hsotg, struct dwc2_host_chan *chan)
+{
+	u32 hcintmsk;
+
+	chan->xfer_started = 0;
+
+	/*
+	 * Clear channel interrupt enables and any unhandled channel interrupt
+	 * conditions
+	 */
+	writel(0, hsotg->regs + HCINTMSK(chan->hc_num));
+	hcintmsk = 0xffffffff;
+	hcintmsk &= ~HCINTMSK_RESERVED14_31;
+	writel(hcintmsk, hsotg->regs + HCINT(chan->hc_num));
+}
+
+/**
+ * dwc2_hc_set_even_odd_frame() - Sets the channel property that indicates in
+ * which frame a periodic transfer should occur
+ *
+ * @hsotg:  Programming view of DWC_otg controller
+ * @chan:   Identifies the host channel to set up and its properties
+ * @hcchar: Current value of the HCCHAR register for the specified host channel
+ *
+ * This function has no effect on non-periodic transfers
+ */
+static void dwc2_hc_set_even_odd_frame(struct dwc2_hsotg *hsotg,
+				       struct dwc2_host_chan *chan, u32 *hcchar)
+{
+	u32 hfnum, frnum;
+
+	if (chan->ep_type == USB_ENDPOINT_XFER_INT ||
+	    chan->ep_type == USB_ENDPOINT_XFER_ISOC) {
+		hfnum = readl(hsotg->regs + HFNUM);
+		frnum = hfnum >> HFNUM_FRNUM_SHIFT &
+			HFNUM_FRNUM_MASK >> HFNUM_FRNUM_SHIFT;
+
+		/* 1 if _next_ frame is odd, 0 if it's even */
+		if (frnum & 0x1)
+			*hcchar |= HCCHAR_ODDFRM;
+	}
+}
+
+static void dwc2_set_pid_isoc(struct dwc2_host_chan *chan)
+{
+	/* Set up the initial PID for the transfer */
+	if (chan->speed == USB_SPEED_HIGH) {
+		if (chan->ep_is_in) {
+			if (chan->multi_count == 1)
+				chan->data_pid_start = DWC2_HC_PID_DATA0;
+			else if (chan->multi_count == 2)
+				chan->data_pid_start = DWC2_HC_PID_DATA1;
+			else
+				chan->data_pid_start = DWC2_HC_PID_DATA2;
+		} else {
+			if (chan->multi_count == 1)
+				chan->data_pid_start = DWC2_HC_PID_DATA0;
+			else
+				chan->data_pid_start = DWC2_HC_PID_MDATA;
+		}
+	} else {
+		chan->data_pid_start = DWC2_HC_PID_DATA0;
+	}
+}
+
+/**
+ * dwc2_hc_write_packet() - Writes a packet into the Tx FIFO associated with
+ * the Host Channel
+ *
+ * @hsotg: Programming view of DWC_otg controller
+ * @chan:  Information needed to initialize the host channel
+ *
+ * This function should only be called in Slave mode. For a channel associated
+ * with a non-periodic EP, the non-periodic Tx FIFO is written. For a channel
+ * associated with a periodic EP, the periodic Tx FIFO is written.
+ *
+ * Upon return the xfer_buf and xfer_count fields in chan are incremented by
+ * the number of bytes written to the Tx FIFO.
+ */
+static void dwc2_hc_write_packet(struct dwc2_hsotg *hsotg,
+				 struct dwc2_host_chan *chan)
+{
+	u32 i;
+	u32 remaining_count;
+	u32 byte_count;
+	u32 dword_count;
+	u32 __iomem *data_fifo;
+	u32 *data_buf = (u32 *)chan->xfer_buf;
+
+	if (dbg_hc(chan))
+		dev_vdbg(hsotg->dev, "%s()\n", __func__);
+
+	data_fifo = (u32 __iomem *)(hsotg->regs + HCFIFO(chan->hc_num));
+
+	remaining_count = chan->xfer_len - chan->xfer_count;
+	if (remaining_count > chan->max_packet)
+		byte_count = chan->max_packet;
+	else
+		byte_count = remaining_count;
+
+	dword_count = (byte_count + 3) / 4;
+
+	if (((unsigned long)data_buf & 0x3) == 0) {
+		/* xfer_buf is DWORD aligned */
+		for (i = 0; i < dword_count; i++, data_buf++)
+			writel(*data_buf, data_fifo);
+	} else {
+		/* xfer_buf is not DWORD aligned */
+		for (i = 0; i < dword_count; i++, data_buf++) {
+			u32 data = data_buf[0] | data_buf[1] << 8 |
+				   data_buf[2] << 16 | data_buf[3] << 24;
+			writel(data, data_fifo);
+		}
+	}
+
+	chan->xfer_count += byte_count;
+	chan->xfer_buf += byte_count;
+}
+
+/**
+ * dwc2_hc_start_transfer() - Does the setup for a data transfer for a host
+ * channel and starts the transfer
+ *
+ * @hsotg: Programming view of DWC_otg controller
+ * @chan:  Information needed to initialize the host channel. The xfer_len value
+ *         may be reduced to accommodate the max widths of the XferSize and
+ *         PktCnt fields in the HCTSIZn register. The multi_count value may be
+ *         changed to reflect the final xfer_len value.
+ *
+ * This function may be called in either Slave mode or DMA mode. In Slave mode,
+ * the caller must ensure that there is sufficient space in the request queue
+ * and Tx Data FIFO.
+ *
+ * For an OUT transfer in Slave mode, it loads a data packet into the
+ * appropriate FIFO. If necessary, additional data packets are loaded in the
+ * Host ISR.
+ *
+ * For an IN transfer in Slave mode, a data packet is requested. The data
+ * packets are unloaded from the Rx FIFO in the Host ISR. If necessary,
+ * additional data packets are requested in the Host ISR.
+ *
+ * For a PING transfer in Slave mode, the Do Ping bit is set in the HCTSIZ
+ * register along with a packet count of 1 and the channel is enabled. This
+ * causes a single PING transaction to occur. Other fields in HCTSIZ are
+ * simply set to 0 since no data transfer occurs in this case.
+ *
+ * For a PING transfer in DMA mode, the HCTSIZ register is initialized with
+ * all the information required to perform the subsequent data transfer. In
+ * addition, the Do Ping bit is set in the HCTSIZ register. In this case, the
+ * controller performs the entire PING protocol, then starts the data
+ * transfer.
+ */
+void dwc2_hc_start_transfer(struct dwc2_hsotg *hsotg,
+			    struct dwc2_host_chan *chan)
+{
+	u32 max_hc_xfer_size = hsotg->core_params->max_transfer_size;
+	u16 max_hc_pkt_count = hsotg->core_params->max_packet_count;
+	u32 hcchar;
+	u32 hctsiz = 0;
+	u16 num_packets;
+
+	if (dbg_hc(chan))
+		dev_vdbg(hsotg->dev, "%s()\n", __func__);
+
+	if (chan->do_ping) {
+		if (hsotg->core_params->dma_enable <= 0) {
+			if (dbg_hc(chan))
+				dev_vdbg(hsotg->dev, "ping, no DMA\n");
+			dwc2_hc_do_ping(hsotg, chan);
+			chan->xfer_started = 1;
+			return;
+		} else {
+			if (dbg_hc(chan))
+				dev_vdbg(hsotg->dev, "ping, DMA\n");
+			hctsiz |= TSIZ_DOPNG;
+		}
+	}
+
+	if (chan->do_split) {
+		if (dbg_hc(chan))
+			dev_vdbg(hsotg->dev, "split\n");
+		num_packets = 1;
+
+		if (chan->complete_split && !chan->ep_is_in)
+			/*
+			 * For CSPLIT OUT Transfer, set the size to 0 so the
+			 * core doesn't expect any data written to the FIFO
+			 */
+			chan->xfer_len = 0;
+		else if (chan->ep_is_in || chan->xfer_len > chan->max_packet)
+			chan->xfer_len = chan->max_packet;
+		else if (!chan->ep_is_in && chan->xfer_len > 188)
+			chan->xfer_len = 188;
+
+		hctsiz |= chan->xfer_len << TSIZ_XFERSIZE_SHIFT &
+			  TSIZ_XFERSIZE_MASK;
+	} else {
+		if (dbg_hc(chan))
+			dev_vdbg(hsotg->dev, "no split\n");
+		/*
+		 * Ensure that the transfer length and packet count will fit
+		 * in the widths allocated for them in the HCTSIZn register
+		 */
+		if (chan->ep_type == USB_ENDPOINT_XFER_INT ||
+		    chan->ep_type == USB_ENDPOINT_XFER_ISOC) {
+			/*
+			 * Make sure the transfer size is no larger than one
+			 * (micro)frame's worth of data. (A check was done
+			 * when the periodic transfer was accepted to ensure
+			 * that a (micro)frame's worth of data can be
+			 * programmed into a channel.)
+			 */
+			u32 max_periodic_len =
+				chan->multi_count * chan->max_packet;
+
+			if (chan->xfer_len > max_periodic_len)
+				chan->xfer_len = max_periodic_len;
+		} else if (chan->xfer_len > max_hc_xfer_size) {
+			/*
+			 * Make sure that xfer_len is a multiple of max packet
+			 * size
+			 */
+			chan->xfer_len =
+				max_hc_xfer_size - chan->max_packet + 1;
+		}
+
+		if (chan->xfer_len > 0) {
+			num_packets = (chan->xfer_len + chan->max_packet - 1) /
+					chan->max_packet;
+			if (num_packets > max_hc_pkt_count) {
+				num_packets = max_hc_pkt_count;
+				chan->xfer_len = num_packets * chan->max_packet;
+			}
+		} else {
+			/* Need 1 packet for transfer length of 0 */
+			num_packets = 1;
+		}
+
+		if (chan->ep_is_in)
+			/*
+			 * Always program an integral # of max packets for IN
+			 * transfers
+			 */
+			chan->xfer_len = num_packets * chan->max_packet;
+
+		if (chan->ep_type == USB_ENDPOINT_XFER_INT ||
+		    chan->ep_type == USB_ENDPOINT_XFER_ISOC)
+			/*
+			 * Make sure that the multi_count field matches the
+			 * actual transfer length
+			 */
+			chan->multi_count = num_packets;
+
+		if (chan->ep_type == USB_ENDPOINT_XFER_ISOC)
+			dwc2_set_pid_isoc(chan);
+
+		hctsiz |= chan->xfer_len << TSIZ_XFERSIZE_SHIFT &
+			  TSIZ_XFERSIZE_MASK;
+	}
+
+	chan->start_pkt_count = num_packets;
+	hctsiz |= num_packets << TSIZ_PKTCNT_SHIFT & TSIZ_PKTCNT_MASK;
+	hctsiz |= chan->data_pid_start << TSIZ_SC_MC_PID_SHIFT &
+		  TSIZ_SC_MC_PID_MASK;
+	writel(hctsiz, hsotg->regs + HCTSIZ(chan->hc_num));
+	if (dbg_hc(chan)) {
+		dev_vdbg(hsotg->dev, "Wrote %08x to HCTSIZ(%d)\n",
+			 hctsiz, chan->hc_num);
+
+		dev_vdbg(hsotg->dev, "%s: Channel %d\n", __func__,
+			 chan->hc_num);
+		dev_vdbg(hsotg->dev, "	 Xfer Size: %d\n",
+			 hctsiz >> TSIZ_XFERSIZE_SHIFT &
+			 TSIZ_XFERSIZE_MASK >> TSIZ_XFERSIZE_SHIFT);
+		dev_vdbg(hsotg->dev, "	 Num Pkts: %d\n",
+			 hctsiz >> TSIZ_PKTCNT_SHIFT &
+			 TSIZ_PKTCNT_MASK >> TSIZ_PKTCNT_SHIFT);
+		dev_vdbg(hsotg->dev, "	 Start PID: %d\n",
+			 hctsiz >> TSIZ_SC_MC_PID_SHIFT &
+			 TSIZ_SC_MC_PID_MASK >> TSIZ_SC_MC_PID_SHIFT);
+	}
+
+	if (hsotg->core_params->dma_enable > 0) {
+		dma_addr_t dma_addr;
+
+		if (chan->align_buf) {
+			if (dbg_hc(chan))
+				dev_vdbg(hsotg->dev, "align_buf\n");
+			dma_addr = chan->align_buf;
+		} else {
+			dma_addr = chan->xfer_dma;
+		}
+		writel((u32)dma_addr, hsotg->regs + HCDMA(chan->hc_num));
+		if (dbg_hc(chan))
+			dev_vdbg(hsotg->dev, "Wrote %08lx to HCDMA(%d)\n",
+				 (unsigned long)dma_addr, chan->hc_num);
+	}
+
+	/* Start the split */
+	if (chan->do_split) {
+		u32 hcsplt = readl(hsotg->regs + HCSPLT(chan->hc_num));
+
+		hcsplt |= HCSPLT_SPLTENA;
+		writel(hcsplt, hsotg->regs + HCSPLT(chan->hc_num));
+	}
+
+	hcchar = readl(hsotg->regs + HCCHAR(chan->hc_num));
+	hcchar &= ~HCCHAR_MULTICNT_MASK;
+	hcchar |= chan->multi_count << HCCHAR_MULTICNT_SHIFT &
+		  HCCHAR_MULTICNT_MASK;
+	dwc2_hc_set_even_odd_frame(hsotg, chan, &hcchar);
+
+	if (hcchar & HCCHAR_CHDIS)
+		dev_warn(hsotg->dev,
+			 "%s: chdis set, channel %d, hcchar 0x%08x\n",
+			 __func__, chan->hc_num, hcchar);
+
+	/* Set host channel enable after all other setup is complete */
+	hcchar |= HCCHAR_CHENA;
+	hcchar &= ~HCCHAR_CHDIS;
+
+	if (dbg_hc(chan))
+		dev_vdbg(hsotg->dev, "	 Multi Cnt: %d\n",
+			 hcchar >> HCCHAR_MULTICNT_SHIFT &
+			 HCCHAR_MULTICNT_MASK >> HCCHAR_MULTICNT_SHIFT);
+
+	writel(hcchar, hsotg->regs + HCCHAR(chan->hc_num));
+	if (dbg_hc(chan))
+		dev_vdbg(hsotg->dev, "Wrote %08x to HCCHAR(%d)\n", hcchar,
+			 chan->hc_num);
+
+	chan->xfer_started = 1;
+	chan->requests++;
+
+	if (hsotg->core_params->dma_enable <= 0 &&
+	    !chan->ep_is_in && chan->xfer_len > 0)
+		/* Load OUT packet into the appropriate Tx FIFO */
+		dwc2_hc_write_packet(hsotg, chan);
+}
+
+/**
+ * dwc2_hc_start_transfer_ddma() - Does the setup for a data transfer for a
+ * host channel and starts the transfer in Descriptor DMA mode
+ *
+ * @hsotg: Programming view of DWC_otg controller
+ * @chan:  Information needed to initialize the host channel
+ *
+ * Initializes HCTSIZ register. For a PING transfer the Do Ping bit is set.
+ * Sets PID and NTD values. For periodic transfers initializes SCHED_INFO field
+ * with micro-frame bitmap.
+ *
+ * Initializes HCDMA register with descriptor list address and CTD value then
+ * starts the transfer via enabling the channel.
+ */
+void dwc2_hc_start_transfer_ddma(struct dwc2_hsotg *hsotg,
+				 struct dwc2_host_chan *chan)
+{
+	u32 hcchar;
+	u32 hc_dma;
+	u32 hctsiz = 0;
+
+	if (chan->do_ping)
+		hctsiz |= TSIZ_DOPNG;
+
+	if (chan->ep_type == USB_ENDPOINT_XFER_ISOC)
+		dwc2_set_pid_isoc(chan);
+
+	/* Packet Count and Xfer Size are not used in Descriptor DMA mode */
+	hctsiz |= chan->data_pid_start << TSIZ_SC_MC_PID_SHIFT &
+		  TSIZ_SC_MC_PID_MASK;
+
+	/* 0 - 1 descriptor, 1 - 2 descriptors, etc */
+	hctsiz |= (chan->ntd - 1) << TSIZ_NTD_SHIFT & TSIZ_NTD_MASK;
+
+	/* Non-zero only for high-speed interrupt endpoints */
+	hctsiz |= chan->schinfo << TSIZ_SCHINFO_SHIFT & TSIZ_SCHINFO_MASK;
+
+	if (dbg_hc(chan)) {
+		dev_vdbg(hsotg->dev, "%s: Channel %d\n", __func__,
+			 chan->hc_num);
+		dev_vdbg(hsotg->dev, "	 Start PID: %d\n",
+			 chan->data_pid_start);
+		dev_vdbg(hsotg->dev, "	 NTD: %d\n", chan->ntd - 1);
+	}
+
+	writel(hctsiz, hsotg->regs + HCTSIZ(chan->hc_num));
+
+	hc_dma = (u32)chan->desc_list_addr & HCDMA_DMA_ADDR_MASK;
+
+	/* Always start from first descriptor */
+	hc_dma &= ~HCDMA_CTD_MASK;
+	writel(hc_dma, hsotg->regs + HCDMA(chan->hc_num));
+	if (dbg_hc(chan))
+		dev_vdbg(hsotg->dev, "Wrote %08x to HCDMA(%d)\n",
+			 hc_dma, chan->hc_num);
+
+	hcchar = readl(hsotg->regs + HCCHAR(chan->hc_num));
+	hcchar &= ~HCCHAR_MULTICNT_MASK;
+	hcchar |= chan->multi_count << HCCHAR_MULTICNT_SHIFT &
+		  HCCHAR_MULTICNT_MASK;
+
+	if (hcchar & HCCHAR_CHDIS)
+		dev_warn(hsotg->dev,
+			 "%s: chdis set, channel %d, hcchar 0x%08x\n",
+			 __func__, chan->hc_num, hcchar);
+
+	/* Set host channel enable after all other setup is complete */
+	hcchar |= HCCHAR_CHENA;
+	hcchar &= ~HCCHAR_CHDIS;
+
+	if (dbg_hc(chan))
+		dev_vdbg(hsotg->dev, "	 Multi Cnt: %d\n",
+			 hcchar >> HCCHAR_MULTICNT_SHIFT &
+			 HCCHAR_MULTICNT_MASK >> HCCHAR_MULTICNT_SHIFT);
+
+	writel(hcchar, hsotg->regs + HCCHAR(chan->hc_num));
+	if (dbg_hc(chan))
+		dev_vdbg(hsotg->dev, "Wrote %08x to HCCHAR(%d)\n", hcchar,
+			 chan->hc_num);
+
+	chan->xfer_started = 1;
+	chan->requests++;
+}
+
+/**
+ * dwc2_hc_continue_transfer() - Continues a data transfer that was started by
+ * a previous call to dwc2_hc_start_transfer()
+ *
+ * @hsotg: Programming view of DWC_otg controller
+ * @chan:  Information needed to initialize the host channel
+ *
+ * The caller must ensure there is sufficient space in the request queue and Tx
+ * Data FIFO. This function should only be called in Slave mode. In DMA mode,
+ * the controller acts autonomously to complete transfers programmed to a host
+ * channel.
+ *
+ * For an OUT transfer, a new data packet is loaded into the appropriate FIFO
+ * if there is any data remaining to be queued. For an IN transfer, another
+ * data packet is always requested. For the SETUP phase of a control transfer,
+ * this function does nothing.
+ *
+ * Return: 1 if a new request is queued, 0 if no more requests are required
+ * for this transfer
+ */
+int dwc2_hc_continue_transfer(struct dwc2_hsotg *hsotg,
+			      struct dwc2_host_chan *chan)
+{
+	if (dbg_hc(chan))
+		dev_vdbg(hsotg->dev, "%s: Channel %d\n", __func__,
+			 chan->hc_num);
+
+	if (chan->do_split)
+		/* SPLITs always queue just once per channel */
+		return 0;
+
+	if (chan->data_pid_start == DWC2_HC_PID_SETUP)
+		/* SETUPs are queued only once since they can't be NAK'd */
+		return 0;
+
+	if (chan->ep_is_in) {
+		/*
+		 * Always queue another request for other IN transfers. If
+		 * back-to-back INs are issued and NAKs are received for both,
+		 * the driver may still be processing the first NAK when the
+		 * second NAK is received. When the interrupt handler clears
+		 * the NAK interrupt for the first NAK, the second NAK will
+		 * not be seen. So we can't depend on the NAK interrupt
+		 * handler to requeue a NAK'd request. Instead, IN requests
+		 * are issued each time this function is called. When the
+		 * transfer completes, the extra requests for the channel will
+		 * be flushed.
+		 */
+		u32 hcchar = readl(hsotg->regs + HCCHAR(chan->hc_num));
+
+		dwc2_hc_set_even_odd_frame(hsotg, chan, &hcchar);
+		hcchar |= HCCHAR_CHENA;
+		hcchar &= ~HCCHAR_CHDIS;
+		if (dbg_hc(chan))
+			dev_vdbg(hsotg->dev, "	 IN xfer: hcchar = 0x%08x\n",
+				 hcchar);
+		writel(hcchar, hsotg->regs + HCCHAR(chan->hc_num));
+		chan->requests++;
+		return 1;
+	}
+
+	/* OUT transfers */
+
+	if (chan->xfer_count < chan->xfer_len) {
+		if (chan->ep_type == USB_ENDPOINT_XFER_INT ||
+		    chan->ep_type == USB_ENDPOINT_XFER_ISOC) {
+			u32 hcchar = readl(hsotg->regs +
+					   HCCHAR(chan->hc_num));
+
+			dwc2_hc_set_even_odd_frame(hsotg, chan,
+						   &hcchar);
+		}
+
+		/* Load OUT packet into the appropriate Tx FIFO */
+		dwc2_hc_write_packet(hsotg, chan);
+		chan->requests++;
+		return 1;
+	}
+
+	return 0;
+}
+
+/**
+ * dwc2_hc_do_ping() - Starts a PING transfer
+ *
+ * @hsotg: Programming view of DWC_otg controller
+ * @chan:  Information needed to initialize the host channel
+ *
+ * This function should only be called in Slave mode. The Do Ping bit is set in
+ * the HCTSIZ register, then the channel is enabled.
+ */
+void dwc2_hc_do_ping(struct dwc2_hsotg *hsotg, struct dwc2_host_chan *chan)
+{
+	u32 hcchar;
+	u32 hctsiz;
+
+	if (dbg_hc(chan))
+		dev_vdbg(hsotg->dev, "%s: Channel %d\n", __func__,
+			 chan->hc_num);
+
+
+	hctsiz = TSIZ_DOPNG;
+	hctsiz |= 1 << TSIZ_PKTCNT_SHIFT;
+	writel(hctsiz, hsotg->regs + HCTSIZ(chan->hc_num));
+
+	hcchar = readl(hsotg->regs + HCCHAR(chan->hc_num));
+	hcchar |= HCCHAR_CHENA;
+	hcchar &= ~HCCHAR_CHDIS;
+	writel(hcchar, hsotg->regs + HCCHAR(chan->hc_num));
+}
+
+/**
+ * dwc2_calc_frame_interval() - Calculates the correct frame Interval value for
+ * the HFIR register according to PHY type and speed
+ *
+ * @hsotg: Programming view of DWC_otg controller
+ *
+ * NOTE: The caller can modify the value of the HFIR register only after the
+ * Port Enable bit of the Host Port Control and Status register (HPRT.EnaPort)
+ * has been set
+ */
+u32 dwc2_calc_frame_interval(struct dwc2_hsotg *hsotg)
+{
+	u32 usbcfg;
+	u32 hwcfg2;
+	u32 hprt0;
+	int clock = 60;	/* default value */
+
+	usbcfg = readl(hsotg->regs + GUSBCFG);
+	hwcfg2 = readl(hsotg->regs + GHWCFG2);
+	hprt0 = readl(hsotg->regs + HPRT0);
+
+	if (!(usbcfg & GUSBCFG_PHYSEL) && (usbcfg & GUSBCFG_ULPI_UTMI_SEL) &&
+	    !(usbcfg & GUSBCFG_PHYIF16))
+		clock = 60;
+	if ((usbcfg & GUSBCFG_PHYSEL) && (hwcfg2 & GHWCFG2_FS_PHY_TYPE_MASK) ==
+	    GHWCFG2_FS_PHY_TYPE_SHARED_ULPI)
+		clock = 48;
+	if (!(usbcfg & GUSBCFG_PHY_LP_CLK_SEL) && !(usbcfg & GUSBCFG_PHYSEL) &&
+	    !(usbcfg & GUSBCFG_ULPI_UTMI_SEL) && (usbcfg & GUSBCFG_PHYIF16))
+		clock = 30;
+	if (!(usbcfg & GUSBCFG_PHY_LP_CLK_SEL) && !(usbcfg & GUSBCFG_PHYSEL) &&
+	    !(usbcfg & GUSBCFG_ULPI_UTMI_SEL) && !(usbcfg & GUSBCFG_PHYIF16))
+		clock = 60;
+	if ((usbcfg & GUSBCFG_PHY_LP_CLK_SEL) && !(usbcfg & GUSBCFG_PHYSEL) &&
+	    !(usbcfg & GUSBCFG_ULPI_UTMI_SEL) && (usbcfg & GUSBCFG_PHYIF16))
+		clock = 48;
+	if ((usbcfg & GUSBCFG_PHYSEL) && !(usbcfg & GUSBCFG_PHYIF16) &&
+	    (hwcfg2 & GHWCFG2_FS_PHY_TYPE_MASK) ==
+	    GHWCFG2_FS_PHY_TYPE_SHARED_UTMI)
+		clock = 48;
+	if ((usbcfg & GUSBCFG_PHYSEL) && (hwcfg2 & GHWCFG2_FS_PHY_TYPE_MASK) ==
+	    GHWCFG2_FS_PHY_TYPE_DEDICATED)
+		clock = 48;
+
+	if ((hprt0 & HPRT0_SPD_MASK) == 0)
+		/* High speed case */
+		return 125 * clock;
+	else
+		/* FS/LS case */
+		return 1000 * clock;
+}
+
+/**
+ * dwc2_read_packet() - Reads a packet from the Rx FIFO into the destination
+ * buffer
+ *
+ * @core_if: Programming view of DWC_otg controller
+ * @dest:    Destination buffer for the packet
+ * @bytes:   Number of bytes to copy to the destination
+ */
+void dwc2_read_packet(struct dwc2_hsotg *hsotg, u8 *dest, u16 bytes)
+{
+	u32 __iomem *fifo = hsotg->regs + HCFIFO(0);
+	u32 *data_buf = (u32 *)dest;
+	int word_count = (bytes + 3) / 4;
+	int i;
+
+	/*
+	 * Todo: Account for the case where dest is not dword aligned. This
+	 * requires reading data from the FIFO into a u32 temp buffer, then
+	 * moving it into the data buffer.
+	 */
+
+	dev_vdbg(hsotg->dev, "%s(%p,%p,%d)\n", __func__, hsotg, dest, bytes);
+
+	for (i = 0; i < word_count; i++, data_buf++)
+		*data_buf = readl(fifo);
+}
+
+/**
+ * dwc2_dump_host_registers() - Prints the host registers
+ *
+ * @hsotg: Programming view of DWC_otg controller
+ *
+ * NOTE: This function will be removed once the peripheral controller code
+ * is integrated and the driver is stable
+ */
+void dwc2_dump_host_registers(struct dwc2_hsotg *hsotg)
+{
+#ifdef DEBUG
+	u32 __iomem *addr;
+	int i;
+
+	dev_dbg(hsotg->dev, "Host Global Registers\n");
+	addr = hsotg->regs + HCFG;
+	dev_dbg(hsotg->dev, "HCFG	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + HFIR;
+	dev_dbg(hsotg->dev, "HFIR	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + HFNUM;
+	dev_dbg(hsotg->dev, "HFNUM	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + HPTXSTS;
+	dev_dbg(hsotg->dev, "HPTXSTS	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + HAINT;
+	dev_dbg(hsotg->dev, "HAINT	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + HAINTMSK;
+	dev_dbg(hsotg->dev, "HAINTMSK	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	if (hsotg->core_params->dma_desc_enable > 0) {
+		addr = hsotg->regs + HFLBADDR;
+		dev_dbg(hsotg->dev, "HFLBADDR @0x%08lX : 0x%08X\n",
+			(unsigned long)addr, readl(addr));
+	}
+
+	addr = hsotg->regs + HPRT0;
+	dev_dbg(hsotg->dev, "HPRT0	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+
+	for (i = 0; i < hsotg->core_params->host_channels; i++) {
+		dev_dbg(hsotg->dev, "Host Channel %d Specific Registers\n", i);
+		addr = hsotg->regs + HCCHAR(i);
+		dev_dbg(hsotg->dev, "HCCHAR	 @0x%08lX : 0x%08X\n",
+			(unsigned long)addr, readl(addr));
+		addr = hsotg->regs + HCSPLT(i);
+		dev_dbg(hsotg->dev, "HCSPLT	 @0x%08lX : 0x%08X\n",
+			(unsigned long)addr, readl(addr));
+		addr = hsotg->regs + HCINT(i);
+		dev_dbg(hsotg->dev, "HCINT	 @0x%08lX : 0x%08X\n",
+			(unsigned long)addr, readl(addr));
+		addr = hsotg->regs + HCINTMSK(i);
+		dev_dbg(hsotg->dev, "HCINTMSK	 @0x%08lX : 0x%08X\n",
+			(unsigned long)addr, readl(addr));
+		addr = hsotg->regs + HCTSIZ(i);
+		dev_dbg(hsotg->dev, "HCTSIZ	 @0x%08lX : 0x%08X\n",
+			(unsigned long)addr, readl(addr));
+		addr = hsotg->regs + HCDMA(i);
+		dev_dbg(hsotg->dev, "HCDMA	 @0x%08lX : 0x%08X\n",
+			(unsigned long)addr, readl(addr));
+		if (hsotg->core_params->dma_desc_enable > 0) {
+			addr = hsotg->regs + HCDMAB(i);
+			dev_dbg(hsotg->dev, "HCDMAB	 @0x%08lX : 0x%08X\n",
+				(unsigned long)addr, readl(addr));
+		}
+	}
+#endif
+}
+
+/**
+ * dwc2_dump_global_registers() - Prints the core global registers
+ *
+ * @hsotg: Programming view of DWC_otg controller
+ *
+ * NOTE: This function will be removed once the peripheral controller code
+ * is integrated and the driver is stable
+ */
+void dwc2_dump_global_registers(struct dwc2_hsotg *hsotg)
+{
+#ifdef DEBUG
+	u32 __iomem *addr;
+	int i, ep_num;
+	char *txfsiz;
+
+	dev_dbg(hsotg->dev, "Core Global Registers\n");
+	addr = hsotg->regs + GOTGCTL;
+	dev_dbg(hsotg->dev, "GOTGCTL	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + GOTGINT;
+	dev_dbg(hsotg->dev, "GOTGINT	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + GAHBCFG;
+	dev_dbg(hsotg->dev, "GAHBCFG	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + GUSBCFG;
+	dev_dbg(hsotg->dev, "GUSBCFG	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + GRSTCTL;
+	dev_dbg(hsotg->dev, "GRSTCTL	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + GINTSTS;
+	dev_dbg(hsotg->dev, "GINTSTS	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + GINTMSK;
+	dev_dbg(hsotg->dev, "GINTMSK	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + GRXSTSR;
+	dev_dbg(hsotg->dev, "GRXSTSR	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + GRXFSIZ;
+	dev_dbg(hsotg->dev, "GRXFSIZ	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + GNPTXFSIZ;
+	dev_dbg(hsotg->dev, "GNPTXFSIZ	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + GNPTXSTS;
+	dev_dbg(hsotg->dev, "GNPTXSTS	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + GI2CCTL;
+	dev_dbg(hsotg->dev, "GI2CCTL	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + GPVNDCTL;
+	dev_dbg(hsotg->dev, "GPVNDCTL	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + GGPIO;
+	dev_dbg(hsotg->dev, "GGPIO	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + GUID;
+	dev_dbg(hsotg->dev, "GUID	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + GSNPSID;
+	dev_dbg(hsotg->dev, "GSNPSID	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + GHWCFG1;
+	dev_dbg(hsotg->dev, "GHWCFG1	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + GHWCFG2;
+	dev_dbg(hsotg->dev, "GHWCFG2	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + GHWCFG3;
+	dev_dbg(hsotg->dev, "GHWCFG3	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + GHWCFG4;
+	dev_dbg(hsotg->dev, "GHWCFG4	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + GLPMCFG;
+	dev_dbg(hsotg->dev, "GLPMCFG	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + GPWRDN;
+	dev_dbg(hsotg->dev, "GPWRDN	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + GDFIFOCFG;
+	dev_dbg(hsotg->dev, "GDFIFOCFG	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+	addr = hsotg->regs + HPTXFSIZ;
+	dev_dbg(hsotg->dev, "HPTXFSIZ	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+
+	if (hsotg->core_params->en_multiple_tx_fifo <= 0) {
+		ep_num = hsotg->hwcfg4 >> GHWCFG4_NUM_DEV_PERIO_IN_EP_SHIFT &
+			 GHWCFG4_NUM_DEV_PERIO_IN_EP_MASK >>
+					 GHWCFG4_NUM_DEV_PERIO_IN_EP_SHIFT;
+		txfsiz = "DPTXFSIZ";
+	} else {
+		ep_num = hsotg->hwcfg4 >> GHWCFG4_NUM_IN_EPS_SHIFT &
+			 GHWCFG4_NUM_IN_EPS_MASK >> GHWCFG4_NUM_IN_EPS_SHIFT;
+		txfsiz = "DIENPTXF";
+	}
+
+	for (i = 0; i < ep_num; i++) {
+		addr = hsotg->regs + DPTXFSIZN(i + 1);
+		dev_dbg(hsotg->dev, "%s[%d] @0x%08lX : 0x%08X\n", txfsiz, i + 1,
+			(unsigned long)addr, readl(addr));
+	}
+
+	addr = hsotg->regs + PCGCTL;
+	dev_dbg(hsotg->dev, "PCGCTL	 @0x%08lX : 0x%08X\n",
+		(unsigned long)addr, readl(addr));
+#endif
+}
+
+/**
+ * dwc2_flush_tx_fifo() - Flushes a Tx FIFO
+ *
+ * @hsotg: Programming view of DWC_otg controller
+ * @num:   Tx FIFO to flush
+ */
+void dwc2_flush_tx_fifo(struct dwc2_hsotg *hsotg, const int num)
+{
+	u32 greset;
+	int count = 0;
+
+	dev_vdbg(hsotg->dev, "Flush Tx FIFO %d\n", num);
+
+	greset = GRSTCTL_TXFFLSH;
+	greset |= num << GRSTCTL_TXFNUM_SHIFT & GRSTCTL_TXFNUM_MASK;
+	writel(greset, hsotg->regs + GRSTCTL);
+
+	do {
+		greset = readl(hsotg->regs + GRSTCTL);
+		if (++count > 10000) {
+			dev_warn(hsotg->dev,
+				 "%s() HANG! GRSTCTL=%0x GNPTXSTS=0x%08x\n",
+				 __func__, greset,
+				 readl(hsotg->regs + GNPTXSTS));
+			break;
+		}
+		udelay(1);
+	} while (greset & GRSTCTL_TXFFLSH);
+
+	/* Wait for at least 3 PHY Clocks */
+	udelay(1);
+}
+
+/**
+ * dwc2_flush_rx_fifo() - Flushes the Rx FIFO
+ *
+ * @hsotg: Programming view of DWC_otg controller
+ */
+void dwc2_flush_rx_fifo(struct dwc2_hsotg *hsotg)
+{
+	u32 greset;
+	int count = 0;
+
+	dev_vdbg(hsotg->dev, "%s()\n", __func__);
+
+	greset = GRSTCTL_RXFFLSH;
+	writel(greset, hsotg->regs + GRSTCTL);
+
+	do {
+		greset = readl(hsotg->regs + GRSTCTL);
+		if (++count > 10000) {
+			dev_warn(hsotg->dev, "%s() HANG! GRSTCTL=%0x\n",
+				 __func__, greset);
+			break;
+		}
+		udelay(1);
+	} while (greset & GRSTCTL_RXFFLSH);
+
+	/* Wait for at least 3 PHY Clocks */
+	udelay(1);
+}
+
+#define DWC2_PARAM_TEST(a, b, c)	((a) < (b) || (a) > (c))
+
+/* Parameter access functions */
+int dwc2_set_param_otg_cap(struct dwc2_hsotg *hsotg, int val)
+{
+	int valid = 1;
+	int retval = 0;
+	u32 op_mode;
+
+	op_mode = hsotg->hwcfg2 & GHWCFG2_OP_MODE_MASK;
+
+	switch (val) {
+	case DWC2_CAP_PARAM_HNP_SRP_CAPABLE:
+		if (op_mode != GHWCFG2_OP_MODE_HNP_SRP_CAPABLE)
+			valid = 0;
+		break;
+	case DWC2_CAP_PARAM_SRP_ONLY_CAPABLE:
+		switch (op_mode) {
+		case GHWCFG2_OP_MODE_HNP_SRP_CAPABLE:
+		case GHWCFG2_OP_MODE_SRP_ONLY_CAPABLE:
+		case GHWCFG2_OP_MODE_SRP_CAPABLE_DEVICE:
+		case GHWCFG2_OP_MODE_SRP_CAPABLE_HOST:
+			break;
+		default:
+			valid = 0;
+			break;
+		}
+		break;
+	case DWC2_CAP_PARAM_NO_HNP_SRP_CAPABLE:
+		/* always valid */
+		break;
+	default:
+		valid = 0;
+		break;
+	}
+
+	if (!valid) {
+		if (val >= 0)
+			dev_err(hsotg->dev,
+				"%d invalid for otg_cap parameter. Check HW configuration.\n",
+				val);
+		switch (op_mode) {
+		case GHWCFG2_OP_MODE_HNP_SRP_CAPABLE:
+			val = DWC2_CAP_PARAM_HNP_SRP_CAPABLE;
+			break;
+		case GHWCFG2_OP_MODE_SRP_ONLY_CAPABLE:
+		case GHWCFG2_OP_MODE_SRP_CAPABLE_DEVICE:
+		case GHWCFG2_OP_MODE_SRP_CAPABLE_HOST:
+			val = DWC2_CAP_PARAM_SRP_ONLY_CAPABLE;
+			break;
+		default:
+			val = DWC2_CAP_PARAM_NO_HNP_SRP_CAPABLE;
+			break;
+		}
+		dev_dbg(hsotg->dev, "Setting otg_cap to %d\n", val);
+		retval = -EINVAL;
+	}
+
+	hsotg->core_params->otg_cap = val;
+	return retval;
+}
+
+int dwc2_set_param_dma_enable(struct dwc2_hsotg *hsotg, int val)
+{
+	int valid = 1;
+	int retval = 0;
+
+	if (val > 0 && (hsotg->hwcfg2 & GHWCFG2_ARCHITECTURE_MASK) ==
+	    GHWCFG2_SLAVE_ONLY_ARCH)
+		valid = 0;
+	if (val < 0)
+		valid = 0;
+
+	if (!valid) {
+		if (val >= 0)
+			dev_err(hsotg->dev,
+				"%d invalid for dma_enable parameter. Check HW configuration.\n",
+				val);
+		val = (hsotg->hwcfg2 & GHWCFG2_ARCHITECTURE_MASK) !=
+			GHWCFG2_SLAVE_ONLY_ARCH;
+		dev_dbg(hsotg->dev, "Setting dma_enable to %d\n", val);
+		retval = -EINVAL;
+	}
+
+	hsotg->core_params->dma_enable = val;
+	return retval;
+}
+
+int dwc2_set_param_dma_desc_enable(struct dwc2_hsotg *hsotg, int val)
+{
+	int valid = 1;
+	int retval = 0;
+
+	if (val > 0 && (hsotg->core_params->dma_enable <= 0 ||
+			!(hsotg->hwcfg4 & GHWCFG4_DESC_DMA)))
+		valid = 0;
+	if (val < 0)
+		valid = 0;
+
+	if (!valid) {
+		if (val >= 0)
+			dev_err(hsotg->dev,
+				"%d invalid for dma_desc_enable parameter. Check HW configuration.\n",
+				val);
+		val = (hsotg->core_params->dma_enable > 0 &&
+			(hsotg->hwcfg4 & GHWCFG4_DESC_DMA));
+		dev_dbg(hsotg->dev, "Setting dma_desc_enable to %d\n", val);
+		retval = -EINVAL;
+	}
+
+	hsotg->core_params->dma_desc_enable = val;
+	return retval;
+}
+
+int dwc2_set_param_host_support_fs_ls_low_power(struct dwc2_hsotg *hsotg,
+						int val)
+{
+	int retval = 0;
+
+	if (DWC2_PARAM_TEST(val, 0, 1)) {
+		if (val >= 0) {
+			dev_err(hsotg->dev,
+				"Wrong value for host_support_fs_low_power\n");
+			dev_err(hsotg->dev,
+				"host_support_fs_low_power must be 0 or 1\n");
+		}
+		val = 0;
+		dev_dbg(hsotg->dev,
+			"Setting host_support_fs_low_power to %d\n", val);
+		retval = -EINVAL;
+	}
+
+	hsotg->core_params->host_support_fs_ls_low_power = val;
+	return retval;
+}
+
+int dwc2_set_param_enable_dynamic_fifo(struct dwc2_hsotg *hsotg, int val)
+{
+	int valid = 1;
+	int retval = 0;
+
+	if (val > 0 && !(hsotg->hwcfg2 & GHWCFG2_DYNAMIC_FIFO))
+		valid = 0;
+	if (val < 0)
+		valid = 0;
+
+	if (!valid) {
+		if (val >= 0)
+			dev_err(hsotg->dev,
+				"%d invalid for enable_dynamic_fifo parameter. Check HW configuration.\n",
+				val);
+		val = !!(hsotg->hwcfg2 & GHWCFG2_DYNAMIC_FIFO);
+		dev_dbg(hsotg->dev, "Setting enable_dynamic_fifo to %d\n", val);
+		retval = -EINVAL;
+	}
+
+	hsotg->core_params->enable_dynamic_fifo = val;
+	return retval;
+}
+
+int dwc2_set_param_host_rx_fifo_size(struct dwc2_hsotg *hsotg, int val)
+{
+	int valid = 1;
+	int retval = 0;
+
+	if (val < 16 || val > readl(hsotg->regs + GRXFSIZ))
+		valid = 0;
+
+	if (!valid) {
+		if (val >= 0)
+			dev_err(hsotg->dev,
+				"%d invalid for host_rx_fifo_size. Check HW configuration.\n",
+				val);
+		val = readl(hsotg->regs + GRXFSIZ);
+		dev_dbg(hsotg->dev, "Setting host_rx_fifo_size to %d\n", val);
+		retval = -EINVAL;
+	}
+
+	hsotg->core_params->host_rx_fifo_size = val;
+	return retval;
+}
+
+int dwc2_set_param_host_nperio_tx_fifo_size(struct dwc2_hsotg *hsotg, int val)
+{
+	int valid = 1;
+	int retval = 0;
+
+	if (val < 16 || val > (readl(hsotg->regs + GNPTXFSIZ) >> 16 & 0xffff))
+		valid = 0;
+
+	if (!valid) {
+		if (val >= 0)
+			dev_err(hsotg->dev,
+				"%d invalid for host_nperio_tx_fifo_size. Check HW configuration.\n",
+				val);
+		val = readl(hsotg->regs + GNPTXFSIZ) >> 16 & 0xffff;
+		dev_dbg(hsotg->dev, "Setting host_nperio_tx_fifo_size to %d\n",
+			val);
+		retval = -EINVAL;
+	}
+
+	hsotg->core_params->host_nperio_tx_fifo_size = val;
+	return retval;
+}
+
+int dwc2_set_param_host_perio_tx_fifo_size(struct dwc2_hsotg *hsotg, int val)
+{
+	int valid = 1;
+	int retval = 0;
+
+	if (val < 16 || val > (hsotg->hptxfsiz >> 16))
+		valid = 0;
+
+	if (!valid) {
+		if (val >= 0)
+			dev_err(hsotg->dev,
+				"%d invalid for host_perio_tx_fifo_size. Check HW configuration.\n",
+				val);
+		val = hsotg->hptxfsiz >> 16;
+		dev_dbg(hsotg->dev, "Setting host_perio_tx_fifo_size to %d\n",
+			val);
+		retval = -EINVAL;
+	}
+
+	hsotg->core_params->host_perio_tx_fifo_size = val;
+	return retval;
+}
+
+int dwc2_set_param_max_transfer_size(struct dwc2_hsotg *hsotg, int val)
+{
+	int valid = 1;
+	int retval = 0;
+	int width = hsotg->hwcfg3 >> GHWCFG3_XFER_SIZE_CNTR_WIDTH_SHIFT &
+		    GHWCFG3_XFER_SIZE_CNTR_WIDTH_MASK >>
+				GHWCFG3_XFER_SIZE_CNTR_WIDTH_SHIFT;
+
+	if (val < 2047 || val >= (1 << (width + 11)))
+		valid = 0;
+
+	if (!valid) {
+		if (val >= 0)
+			dev_err(hsotg->dev,
+				"%d invalid for max_transfer_size. Check HW configuration.\n",
+				val);
+		val = (1 << (width + 11)) - 1;
+		dev_dbg(hsotg->dev, "Setting max_transfer_size to %d\n", val);
+		retval = -EINVAL;
+	}
+
+	hsotg->core_params->max_transfer_size = val;
+	return retval;
+}
+
+int dwc2_set_param_max_packet_count(struct dwc2_hsotg *hsotg, int val)
+{
+	int valid = 1;
+	int retval = 0;
+	int width = hsotg->hwcfg3 >> GHWCFG3_PACKET_SIZE_CNTR_WIDTH_SHIFT &
+		    GHWCFG3_PACKET_SIZE_CNTR_WIDTH_MASK >>
+				GHWCFG3_PACKET_SIZE_CNTR_WIDTH_SHIFT;
+
+	if (val < 15 || val > (1 << (width + 4)))
+		valid = 0;
+
+	if (!valid) {
+		if (val >= 0)
+			dev_err(hsotg->dev,
+				"%d invalid for max_packet_count. Check HW configuration.\n",
+				val);
+		val = (1 << (width + 4)) - 1;
+		dev_dbg(hsotg->dev, "Setting max_packet_count to %d\n", val);
+		retval = -EINVAL;
+	}
+
+	hsotg->core_params->max_packet_count = val;
+	return retval;
+}
+
+int dwc2_set_param_host_channels(struct dwc2_hsotg *hsotg, int val)
+{
+	int valid = 1;
+	int retval = 0;
+	int num_chan = hsotg->hwcfg2 >> GHWCFG2_NUM_HOST_CHAN_SHIFT &
+		GHWCFG2_NUM_HOST_CHAN_MASK >> GHWCFG2_NUM_HOST_CHAN_SHIFT;
+
+	if (val < 1 || val > num_chan + 1)
+		valid = 0;
+
+	if (!valid) {
+		if (val >= 0)
+			dev_err(hsotg->dev,
+				"%d invalid for host_channels. Check HW configuration.\n",
+				val);
+		val = num_chan + 1;
+		dev_dbg(hsotg->dev, "Setting host_channels to %d\n", val);
+		retval = -EINVAL;
+	}
+
+	hsotg->core_params->host_channels = val;
+	return retval;
+}
+
+int dwc2_set_param_phy_type(struct dwc2_hsotg *hsotg, int val)
+{
+#ifndef NO_FS_PHY_HW_CHECKS
+	int valid = 0;
+	u32 hs_phy_type;
+	u32 fs_phy_type;
+#endif
+	int retval = 0;
+
+	if (DWC2_PARAM_TEST(val, DWC2_PHY_TYPE_PARAM_FS,
+			    DWC2_PHY_TYPE_PARAM_ULPI)) {
+		if (val >= 0) {
+			dev_err(hsotg->dev, "Wrong value for phy_type\n");
+			dev_err(hsotg->dev, "phy_type must be 0, 1 or 2\n");
+		}
+
+#ifndef NO_FS_PHY_HW_CHECKS
+		valid = 0;
+#else
+		val = 0;
+		dev_dbg(hsotg->dev, "Setting phy_type to %d\n", val);
+		retval = -EINVAL;
+#endif
+	}
+
+#ifndef NO_FS_PHY_HW_CHECKS
+	hs_phy_type = hsotg->hwcfg2 & GHWCFG2_HS_PHY_TYPE_MASK;
+	fs_phy_type = hsotg->hwcfg2 & GHWCFG2_FS_PHY_TYPE_MASK;
+
+	if (val == DWC2_PHY_TYPE_PARAM_UTMI &&
+	    (hs_phy_type == GHWCFG2_HS_PHY_TYPE_UTMI ||
+	     hs_phy_type == GHWCFG2_HS_PHY_TYPE_UTMI_ULPI))
+		valid = 1;
+	else if (val == DWC2_PHY_TYPE_PARAM_ULPI &&
+		 (hs_phy_type == GHWCFG2_HS_PHY_TYPE_ULPI ||
+		  hs_phy_type == GHWCFG2_HS_PHY_TYPE_UTMI_ULPI))
+		valid = 1;
+	else if (val == DWC2_PHY_TYPE_PARAM_FS &&
+		 fs_phy_type == GHWCFG2_FS_PHY_TYPE_DEDICATED)
+		valid = 1;
+
+	if (!valid) {
+		if (val >= 0)
+			dev_err(hsotg->dev,
+				"%d invalid for phy_type. Check HW configuration.\n",
+				val);
+		val = 0;
+		if (hs_phy_type != GHWCFG2_HS_PHY_TYPE_NOT_SUPPORTED) {
+			if (hs_phy_type == GHWCFG2_HS_PHY_TYPE_UTMI ||
+			    hs_phy_type == GHWCFG2_HS_PHY_TYPE_UTMI_ULPI)
+				val = DWC2_PHY_TYPE_PARAM_UTMI;
+			else
+				val = DWC2_PHY_TYPE_PARAM_ULPI;
+		}
+		dev_dbg(hsotg->dev, "Setting phy_type to %d\n", val);
+		retval = -EINVAL;
+	}
+#endif
+
+	hsotg->core_params->phy_type = val;
+	return retval;
+}
+
+static int dwc2_get_param_phy_type(struct dwc2_hsotg *hsotg)
+{
+	return hsotg->core_params->phy_type;
+}
+
+int dwc2_set_param_speed(struct dwc2_hsotg *hsotg, int val)
+{
+	int valid = 1;
+	int retval = 0;
+
+	if (DWC2_PARAM_TEST(val, 0, 1)) {
+		if (val >= 0) {
+			dev_err(hsotg->dev, "Wrong value for speed parameter\n");
+			dev_err(hsotg->dev, "max_speed parameter must be 0 or 1\n");
+		}
+		valid = 0;
+	}
+
+	if (val == 0 && dwc2_get_param_phy_type(hsotg) ==
+					DWC2_PHY_TYPE_PARAM_FS)
+		valid = 0;
+
+	if (!valid) {
+		if (val >= 0)
+			dev_err(hsotg->dev,
+				"%d invalid for speed parameter. Check HW configuration.\n",
+				val);
+		val = dwc2_get_param_phy_type(hsotg) == DWC2_PHY_TYPE_PARAM_FS ?
+				1 : 0;
+		dev_dbg(hsotg->dev, "Setting speed to %d\n", val);
+		retval = -EINVAL;
+	}
+
+	hsotg->core_params->speed = val;
+	return retval;
+}
+
+int dwc2_set_param_host_ls_low_power_phy_clk(struct dwc2_hsotg *hsotg, int val)
+{
+	int valid = 1;
+	int retval = 0;
+
+	if (DWC2_PARAM_TEST(val, DWC2_HOST_LS_LOW_POWER_PHY_CLK_PARAM_48MHZ,
+			    DWC2_HOST_LS_LOW_POWER_PHY_CLK_PARAM_6MHZ)) {
+		if (val >= 0) {
+			dev_err(hsotg->dev,
+				"Wrong value for host_ls_low_power_phy_clk parameter\n");
+			dev_err(hsotg->dev,
+				"host_ls_low_power_phy_clk must be 0 or 1\n");
+		}
+		valid = 0;
+	}
+
+	if (val == DWC2_HOST_LS_LOW_POWER_PHY_CLK_PARAM_48MHZ &&
+	    dwc2_get_param_phy_type(hsotg) == DWC2_PHY_TYPE_PARAM_FS)
+		valid = 0;
+
+	if (!valid) {
+		if (val >= 0)
+			dev_err(hsotg->dev,
+				"%d invalid for host_ls_low_power_phy_clk. Check HW configuration.\n",
+				val);
+		val = dwc2_get_param_phy_type(hsotg) == DWC2_PHY_TYPE_PARAM_FS
+			? DWC2_HOST_LS_LOW_POWER_PHY_CLK_PARAM_6MHZ
+			: DWC2_HOST_LS_LOW_POWER_PHY_CLK_PARAM_48MHZ;
+		dev_dbg(hsotg->dev, "Setting host_ls_low_power_phy_clk to %d\n",
+			val);
+		retval = -EINVAL;
+	}
+
+	hsotg->core_params->host_ls_low_power_phy_clk = val;
+	return retval;
+}
+
+int dwc2_set_param_phy_ulpi_ddr(struct dwc2_hsotg *hsotg, int val)
+{
+	int retval = 0;
+
+	if (DWC2_PARAM_TEST(val, 0, 1)) {
+		if (val >= 0) {
+			dev_err(hsotg->dev, "Wrong value for phy_ulpi_ddr\n");
+			dev_err(hsotg->dev, "phy_upli_ddr must be 0 or 1\n");
+		}
+		val = 0;
+		dev_dbg(hsotg->dev, "Setting phy_upli_ddr to %d\n", val);
+		retval = -EINVAL;
+	}
+
+	hsotg->core_params->phy_ulpi_ddr = val;
+	return retval;
+}
+
+int dwc2_set_param_phy_ulpi_ext_vbus(struct dwc2_hsotg *hsotg, int val)
+{
+	int retval = 0;
+
+	if (DWC2_PARAM_TEST(val, 0, 1)) {
+		if (val >= 0) {
+			dev_err(hsotg->dev,
+				"Wrong value for phy_ulpi_ext_vbus\n");
+			dev_err(hsotg->dev,
+				"phy_ulpi_ext_vbus must be 0 or 1\n");
+		}
+		val = 0;
+		dev_dbg(hsotg->dev, "Setting phy_ulpi_ext_vbus to %d\n", val);
+		retval = -EINVAL;
+	}
+
+	hsotg->core_params->phy_ulpi_ext_vbus = val;
+	return retval;
+}
+
+int dwc2_set_param_phy_utmi_width(struct dwc2_hsotg *hsotg, int val)
+{
+	int retval = 0;
+
+	if (DWC2_PARAM_TEST(val, 8, 8) && DWC2_PARAM_TEST(val, 16, 16)) {
+		if (val >= 0) {
+			dev_err(hsotg->dev, "Wrong value for phy_utmi_width\n");
+			dev_err(hsotg->dev, "phy_utmi_width must be 8 or 16\n");
+		}
+		val = 8;
+		dev_dbg(hsotg->dev, "Setting phy_utmi_width to %d\n", val);
+		retval = -EINVAL;
+	}
+
+	hsotg->core_params->phy_utmi_width = val;
+	return retval;
+}
+
+int dwc2_set_param_ulpi_fs_ls(struct dwc2_hsotg *hsotg, int val)
+{
+	int retval = 0;
+
+	if (DWC2_PARAM_TEST(val, 0, 1)) {
+		if (val >= 0) {
+			dev_err(hsotg->dev, "Wrong value for ulpi_fs_ls\n");
+			dev_err(hsotg->dev, "ulpi_fs_ls must be 0 or 1\n");
+		}
+		val = 0;
+		dev_dbg(hsotg->dev, "Setting ulpi_fs_ls to %d\n", val);
+		retval = -EINVAL;
+	}
+
+	hsotg->core_params->ulpi_fs_ls = val;
+	return retval;
+}
+
+int dwc2_set_param_ts_dline(struct dwc2_hsotg *hsotg, int val)
+{
+	int retval = 0;
+
+	if (DWC2_PARAM_TEST(val, 0, 1)) {
+		if (val >= 0) {
+			dev_err(hsotg->dev, "Wrong value for ts_dline\n");
+			dev_err(hsotg->dev, "ts_dline must be 0 or 1\n");
+		}
+		val = 0;
+		dev_dbg(hsotg->dev, "Setting ts_dline to %d\n", val);
+		retval = -EINVAL;
+	}
+
+	hsotg->core_params->ts_dline = val;
+	return retval;
+}
+
+int dwc2_set_param_i2c_enable(struct dwc2_hsotg *hsotg, int val)
+{
+#ifndef NO_FS_PHY_HW_CHECKS
+	int valid = 1;
+#endif
+	int retval = 0;
+
+	if (DWC2_PARAM_TEST(val, 0, 1)) {
+		if (val >= 0) {
+			dev_err(hsotg->dev, "Wrong value for i2c_enable\n");
+			dev_err(hsotg->dev, "i2c_enable must be 0 or 1\n");
+		}
+
+#ifndef NO_FS_PHY_HW_CHECKS
+		valid = 0;
+#else
+		val = 0;
+		dev_dbg(hsotg->dev, "Setting i2c_enable to %d\n", val);
+		retval = -EINVAL;
+#endif
+	}
+
+#ifndef NO_FS_PHY_HW_CHECKS
+	if (val == 1 && !(hsotg->hwcfg3 & GHWCFG3_I2C))
+		valid = 0;
+
+	if (!valid) {
+		if (val >= 0)
+			dev_err(hsotg->dev,
+				"%d invalid for i2c_enable. Check HW configuration.\n",
+				val);
+		val = !!(hsotg->hwcfg3 & GHWCFG3_I2C);
+		dev_dbg(hsotg->dev, "Setting i2c_enable to %d\n", val);
+		retval = -EINVAL;
+	}
+#endif
+
+	hsotg->core_params->i2c_enable = val;
+	return retval;
+}
+
+int dwc2_set_param_en_multiple_tx_fifo(struct dwc2_hsotg *hsotg, int val)
+{
+	int valid = 1;
+	int retval = 0;
+
+	if (DWC2_PARAM_TEST(val, 0, 1)) {
+		if (val >= 0) {
+			dev_err(hsotg->dev,
+				"Wrong value for en_multiple_tx_fifo,\n");
+			dev_err(hsotg->dev,
+				"en_multiple_tx_fifo must be 0 or 1\n");
+		}
+		valid = 0;
+	}
+
+	if (val == 1 && !(hsotg->hwcfg4 & GHWCFG4_DED_FIFO_EN))
+		valid = 0;
+
+	if (!valid) {
+		if (val >= 0)
+			dev_err(hsotg->dev,
+				"%d invalid for parameter en_multiple_tx_fifo. Check HW configuration.\n",
+				val);
+		val = !!(hsotg->hwcfg4 & GHWCFG4_DED_FIFO_EN);
+		dev_dbg(hsotg->dev, "Setting en_multiple_tx_fifo to %d\n", val);
+		retval = -EINVAL;
+	}
+
+	hsotg->core_params->en_multiple_tx_fifo = val;
+	return retval;
+}
+
+int dwc2_set_param_reload_ctl(struct dwc2_hsotg *hsotg, int val)
+{
+	int valid = 1;
+	int retval = 0;
+
+	if (DWC2_PARAM_TEST(val, 0, 1)) {
+		if (val >= 0) {
+			dev_err(hsotg->dev,
+				"'%d' invalid for parameter reload_ctl\n", val);
+			dev_err(hsotg->dev, "reload_ctl must be 0 or 1\n");
+		}
+		valid = 0;
+	}
+
+	if (val == 1 && hsotg->snpsid < DWC2_CORE_REV_2_92a)
+		valid = 0;
+
+	if (!valid) {
+		if (val >= 0)
+			dev_err(hsotg->dev,
+				"%d invalid for parameter reload_ctl. Check HW configuration.\n",
+				val);
+		val = hsotg->snpsid >= DWC2_CORE_REV_2_92a;
+		dev_dbg(hsotg->dev, "Setting reload_ctl to %d\n", val);
+		retval = -EINVAL;
+	}
+
+	hsotg->core_params->reload_ctl = val;
+	return retval;
+}
+
+int dwc2_set_param_ahb_single(struct dwc2_hsotg *hsotg, int val)
+{
+	int valid = 1;
+	int retval = 0;
+
+	if (DWC2_PARAM_TEST(val, 0, 1)) {
+		if (val >= 0) {
+			dev_err(hsotg->dev,
+				"'%d' invalid for parameter ahb_single\n", val);
+			dev_err(hsotg->dev, "ahb_single must be 0 or 1\n");
+		}
+		valid = 0;
+	}
+
+	if (val > 0 && hsotg->snpsid < DWC2_CORE_REV_2_94a)
+		valid = 0;
+
+	if (!valid) {
+		if (val >= 0)
+			dev_err(hsotg->dev,
+				"%d invalid for parameter ahb_single. Check HW configuration.\n",
+				val);
+		val = 0;
+		dev_dbg(hsotg->dev, "Setting ahb_single to %d\n", val);
+		retval = -EINVAL;
+	}
+
+	hsotg->core_params->ahb_single = val;
+	return retval;
+}
+
+int dwc2_set_param_otg_ver(struct dwc2_hsotg *hsotg, int val)
+{
+	int retval = 0;
+
+	if (DWC2_PARAM_TEST(val, 0, 1)) {
+		if (val >= 0) {
+			dev_err(hsotg->dev,
+				"'%d' invalid for parameter otg_ver\n", val);
+			dev_err(hsotg->dev,
+				"otg_ver must be 0 (for OTG 1.3 support) or 1 (for OTG 2.0 support)\n");
+		}
+		val = 0;
+		dev_dbg(hsotg->dev, "Setting otg_ver to %d\n", val);
+		retval = -EINVAL;
+	}
+
+	hsotg->core_params->otg_ver = val;
+	return retval;
+}
+
+/*
+ * This function is called during module intialization to pass module parameters
+ * for the DWC_otg core. It returns non-0 if any parameters are invalid.
+ */
+int dwc2_set_parameters(struct dwc2_hsotg *hsotg,
+			struct dwc2_core_params *params)
+{
+	int retval = 0;
+
+	dev_dbg(hsotg->dev, "%s()\n", __func__);
+
+	retval |= dwc2_set_param_otg_cap(hsotg, params->otg_cap);
+	retval |= dwc2_set_param_dma_enable(hsotg, params->dma_enable);
+	retval |= dwc2_set_param_dma_desc_enable(hsotg,
+						 params->dma_desc_enable);
+	retval |= dwc2_set_param_host_support_fs_ls_low_power(hsotg,
+			params->host_support_fs_ls_low_power);
+	retval |= dwc2_set_param_enable_dynamic_fifo(hsotg,
+			params->enable_dynamic_fifo);
+	retval |= dwc2_set_param_host_rx_fifo_size(hsotg,
+			params->host_rx_fifo_size);
+	retval |= dwc2_set_param_host_nperio_tx_fifo_size(hsotg,
+			params->host_nperio_tx_fifo_size);
+	retval |= dwc2_set_param_host_perio_tx_fifo_size(hsotg,
+			params->host_perio_tx_fifo_size);
+	retval |= dwc2_set_param_max_transfer_size(hsotg,
+			params->max_transfer_size);
+	retval |= dwc2_set_param_max_packet_count(hsotg,
+			params->max_packet_count);
+	retval |= dwc2_set_param_host_channels(hsotg, params->host_channels);
+	retval |= dwc2_set_param_phy_type(hsotg, params->phy_type);
+	retval |= dwc2_set_param_speed(hsotg, params->speed);
+	retval |= dwc2_set_param_host_ls_low_power_phy_clk(hsotg,
+			params->host_ls_low_power_phy_clk);
+	retval |= dwc2_set_param_phy_ulpi_ddr(hsotg, params->phy_ulpi_ddr);
+	retval |= dwc2_set_param_phy_ulpi_ext_vbus(hsotg,
+			params->phy_ulpi_ext_vbus);
+	retval |= dwc2_set_param_phy_utmi_width(hsotg, params->phy_utmi_width);
+	retval |= dwc2_set_param_ulpi_fs_ls(hsotg, params->ulpi_fs_ls);
+	retval |= dwc2_set_param_ts_dline(hsotg, params->ts_dline);
+	retval |= dwc2_set_param_i2c_enable(hsotg, params->i2c_enable);
+	retval |= dwc2_set_param_en_multiple_tx_fifo(hsotg,
+			params->en_multiple_tx_fifo);
+	retval |= dwc2_set_param_reload_ctl(hsotg, params->reload_ctl);
+	retval |= dwc2_set_param_ahb_single(hsotg, params->ahb_single);
+	retval |= dwc2_set_param_otg_ver(hsotg, params->otg_ver);
+
+	return retval;
+}
+
+u16 dwc2_get_otg_version(struct dwc2_hsotg *hsotg)
+{
+	return (u16)(hsotg->core_params->otg_ver == 1 ? 0x0200 : 0x0103);
+}
+
+int dwc2_check_core_status(struct dwc2_hsotg *hsotg)
+{
+	if (readl(hsotg->regs + GSNPSID) == 0xffffffff)
+		return -1;
+	else
+		return 0;
+}
+
+/**
+ * dwc2_enable_global_interrupts() - Enables the controller's Global
+ * Interrupt in the AHB Config register
+ *
+ * @hsotg: Programming view of DWC_otg controller
+ */
+void dwc2_enable_global_interrupts(struct dwc2_hsotg *hsotg)
+{
+	u32 ahbcfg = readl(hsotg->regs + GAHBCFG);
+
+	ahbcfg |= GAHBCFG_GLBL_INTR_EN;
+	writel(ahbcfg, hsotg->regs + GAHBCFG);
+}
+
+/**
+ * dwc2_disable_global_interrupts() - Disables the controller's Global
+ * Interrupt in the AHB Config register
+ *
+ * @hsotg: Programming view of DWC_otg controller
+ */
+void dwc2_disable_global_interrupts(struct dwc2_hsotg *hsotg)
+{
+	u32 ahbcfg = readl(hsotg->regs + GAHBCFG);
+
+	ahbcfg &= ~GAHBCFG_GLBL_INTR_EN;
+	writel(ahbcfg, hsotg->regs + GAHBCFG);
+}
+
+MODULE_DESCRIPTION("DESIGNWARE HS OTG Core");
+MODULE_AUTHOR("Synopsys, Inc.");
+MODULE_LICENSE("Dual BSD/GPL");
diff --git a/drivers/staging/dwc2/core.h b/drivers/staging/dwc2/core.h
new file mode 100644
index 00000000000..fc075a7c1de
--- /dev/null
+++ b/drivers/staging/dwc2/core.h
@@ -0,0 +1,662 @@
+/*
+ * core.h - DesignWare HS OTG Controller common declarations
+ *
+ * Copyright (C) 2004-2013 Synopsys, Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions, and the following disclaimer,
+ *    without modification.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The names of the above-listed copyright holders may not be used
+ *    to endorse or promote products derived from this software without
+ *    specific prior written permission.
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation; either version 2 of the License, or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+ * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __DWC2_CORE_H__
+#define __DWC2_CORE_H__
+
+#include <linux/usb/phy.h>
+#include "hw.h"
+
+#ifdef DWC2_LOG_WRITES
+static inline void do_write(u32 value, void *addr)
+{
+	writel(value, addr);
+	pr_info("INFO:: wrote %08x to %p\n", value, addr);
+}
+
+#undef writel
+#define writel(v, a)	do_write(v, a)
+#endif
+
+/* Maximum number of Endpoints/HostChannels */
+#define MAX_EPS_CHANNELS	16
+
+struct dwc2_hsotg;
+struct dwc2_host_chan;
+
+/* Device States */
+enum dwc2_lx_state {
+	DWC2_L0,	/* On state */
+	DWC2_L1,	/* LPM sleep state */
+	DWC2_L2,	/* USB suspend state */
+	DWC2_L3,	/* Off state */
+};
+
+/**
+ * struct dwc2_core_params - Parameters for configuring the core
+ *
+ * @otg_cap:            Specifies the OTG capabilities. The driver will
+ *                      automatically detect the value for this parameter if
+ *                      none is specified.
+ *                       0 - HNP and SRP capable (default)
+ *                       1 - SRP Only capable
+ *                       2 - No HNP/SRP capable
+ * @dma_enable:         Specifies whether to use slave or DMA mode for accessing
+ *                      the data FIFOs. The driver will automatically detect the
+ *                      value for this parameter if none is specified.
+ *                       0 - Slave
+ *                       1 - DMA (default, if available)
+ * @dma_desc_enable:    When DMA mode is enabled, specifies whether to use
+ *                      address DMA mode or descriptor DMA mode for accessing
+ *                      the data FIFOs. The driver will automatically detect the
+ *                      value for this if none is specified.
+ *                       0 - Address DMA
+ *                       1 - Descriptor DMA (default, if available)
+ * @speed:              Specifies the maximum speed of operation in host and
+ *                      device mode. The actual speed depends on the speed of
+ *                      the attached device and the value of phy_type.
+ *                       0 - High Speed (default)
+ *                       1 - Full Speed
+ * @host_support_fs_ls_low_power: Specifies whether low power mode is supported
+ *                      when attached to a Full Speed or Low Speed device in
+ *                      host mode.
+ *                       0 - Don't support low power mode (default)
+ *                       1 - Support low power mode
+ * @host_ls_low_power_phy_clk: Specifies the PHY clock rate in low power mode
+ *                      when connected to a Low Speed device in host mode. This
+ *                      parameter is applicable only if
+ *                      host_support_fs_ls_low_power is enabled. If phy_type is
+ *                      set to FS then defaults to 6 MHZ otherwise 48 MHZ.
+ *                       0 - 48 MHz
+ *                       1 - 6 MHz
+ * @enable_dynamic_fifo: 0 - Use coreConsultant-specified FIFO size parameters
+ *                       1 - Allow dynamic FIFO sizing (default)
+ * @host_rx_fifo_size:  Number of 4-byte words in the Rx FIFO in host mode when
+ *                      dynamic FIFO sizing is enabled
+ *                       16 to 32768 (default 1024)
+ * @host_nperio_tx_fifo_size: Number of 4-byte words in the non-periodic Tx FIFO
+ *                      in host mode when dynamic FIFO sizing is enabled
+ *                       16 to 32768 (default 1024)
+ * @host_perio_tx_fifo_size: Number of 4-byte words in the periodic Tx FIFO in
+ *                      host mode when dynamic FIFO sizing is enabled
+ *                       16 to 32768 (default 1024)
+ * @max_transfer_size:  The maximum transfer size supported, in bytes
+ *                       2047 to 65,535 (default 65,535)
+ * @max_packet_count:   The maximum number of packets in a transfer
+ *                       15 to 511 (default 511)
+ * @host_channels:      The number of host channel registers to use
+ *                       1 to 16 (default 12)
+ * @phy_type:           Specifies the type of PHY interface to use. By default,
+ *                      the driver will automatically detect the phy_type.
+ * @phy_utmi_width:     Specifies the UTMI+ Data Width (in bits). This parameter
+ *                      is applicable for a phy_type of UTMI+ or ULPI. (For a
+ *                      ULPI phy_type, this parameter indicates the data width
+ *                      between the MAC and the ULPI Wrapper.) Also, this
+ *                      parameter is applicable only if the OTG_HSPHY_WIDTH cC
+ *                      parameter was set to "8 and 16 bits", meaning that the
+ *                      core has been configured to work at either data path
+ *                      width.
+ *                       8 or 16 (default 16)
+ * @phy_ulpi_ddr:       Specifies whether the ULPI operates at double or single
+ *                      data rate. This parameter is only applicable if phy_type
+ *                      is ULPI.
+ *                       0 - single data rate ULPI interface with 8 bit wide
+ *                           data bus (default)
+ *                       1 - double data rate ULPI interface with 4 bit wide
+ *                           data bus
+ * @phy_ulpi_ext_vbus:  For a ULPI phy, specifies whether to use the internal or
+ *                      external supply to drive the VBus
+ * @i2c_enable:         Specifies whether to use the I2Cinterface for a full
+ *                      speed PHY. This parameter is only applicable if phy_type
+ *                      is FS.
+ *                       0 - No (default)
+ *                       1 - Yes
+ * @ulpi_fs_ls:         True to make ULPI phy operate in FS/LS mode only
+ * @ts_dline:           True to enable Term Select Dline pulsing
+ * @en_multiple_tx_fifo: Specifies whether dedicated per-endpoint transmit FIFOs
+ *                      are enabled
+ * @reload_ctl:         True to allow dynamic reloading of HFIR register during
+ *                      runtime
+ * @ahb_single:         This bit enables SINGLE transfers for remainder data in
+ *                      a transfer for DMA mode of operation.
+ *                       0 - remainder data will be sent using INCR burst size
+ *                       1 - remainder data will be sent using SINGLE burst size
+ * @otg_ver:            OTG version supported
+ *                       0 - 1.3
+ *                       1 - 2.0
+ *
+ * The following parameters may be specified when starting the module. These
+ * parameters define how the DWC_otg controller should be configured.
+ */
+struct dwc2_core_params {
+	/*
+	 * Don't add any non-int members here, this will break
+	 * dwc2_set_all_params!
+	 */
+	int otg_cap;
+	int otg_ver;
+	int dma_enable;
+	int dma_desc_enable;
+	int speed;
+	int enable_dynamic_fifo;
+	int en_multiple_tx_fifo;
+	int host_rx_fifo_size;
+	int host_nperio_tx_fifo_size;
+	int host_perio_tx_fifo_size;
+	int max_transfer_size;
+	int max_packet_count;
+	int host_channels;
+	int phy_type;
+	int phy_utmi_width;
+	int phy_ulpi_ddr;
+	int phy_ulpi_ext_vbus;
+	int i2c_enable;
+	int ulpi_fs_ls;
+	int host_support_fs_ls_low_power;
+	int host_ls_low_power_phy_clk;
+	int ts_dline;
+	int reload_ctl;
+	int ahb_single;
+};
+
+/**
+ * struct dwc2_hsotg - Holds the state of the driver, including the non-periodic
+ * and periodic schedules
+ *
+ * @dev:                The struct device pointer
+ * @regs:		Pointer to controller regs
+ * @core_params:        Parameters that define how the core should be configured
+ * @hwcfg1:             Hardware Configuration - stored here for convenience
+ * @hwcfg2:             Hardware Configuration - stored here for convenience
+ * @hwcfg3:             Hardware Configuration - stored here for convenience
+ * @hwcfg4:             Hardware Configuration - stored here for convenience
+ * @hptxfsiz:           Hardware Configuration - stored here for convenience
+ * @snpsid:             Value from SNPSID register
+ * @total_fifo_size:    Total internal RAM for FIFOs (bytes)
+ * @rx_fifo_size:       Size of Rx FIFO (bytes)
+ * @nperio_tx_fifo_size: Size of Non-periodic Tx FIFO (Bytes)
+ * @op_state:           The operational State, during transitions (a_host=>
+ *                      a_peripheral and b_device=>b_host) this may not match
+ *                      the core, but allows the software to determine
+ *                      transitions
+ * @queuing_high_bandwidth: True if multiple packets of a high-bandwidth
+ *                      transfer are in process of being queued
+ * @srp_success:        Stores status of SRP request in the case of a FS PHY
+ *                      with an I2C interface
+ * @wq_otg:             Workqueue object used for handling of some interrupts
+ * @wf_otg:             Work object for handling Connector ID Status Change
+ *                      interrupt
+ * @wkp_timer:          Timer object for handling Wakeup Detected interrupt
+ * @lx_state:           Lx state of connected device
+ * @flags:              Flags for handling root port state changes
+ * @non_periodic_sched_inactive: Inactive QHs in the non-periodic schedule.
+ *                      Transfers associated with these QHs are not currently
+ *                      assigned to a host channel.
+ * @non_periodic_sched_active: Active QHs in the non-periodic schedule.
+ *                      Transfers associated with these QHs are currently
+ *                      assigned to a host channel.
+ * @non_periodic_qh_ptr: Pointer to next QH to process in the active
+ *                      non-periodic schedule
+ * @periodic_sched_inactive: Inactive QHs in the periodic schedule. This is a
+ *                      list of QHs for periodic transfers that are _not_
+ *                      scheduled for the next frame. Each QH in the list has an
+ *                      interval counter that determines when it needs to be
+ *                      scheduled for execution. This scheduling mechanism
+ *                      allows only a simple calculation for periodic bandwidth
+ *                      used (i.e. must assume that all periodic transfers may
+ *                      need to execute in the same frame). However, it greatly
+ *                      simplifies scheduling and should be sufficient for the
+ *                      vast majority of OTG hosts, which need to connect to a
+ *                      small number of peripherals at one time. Items move from
+ *                      this list to periodic_sched_ready when the QH interval
+ *                      counter is 0 at SOF.
+ * @periodic_sched_ready:  List of periodic QHs that are ready for execution in
+ *                      the next frame, but have not yet been assigned to host
+ *                      channels. Items move from this list to
+ *                      periodic_sched_assigned as host channels become
+ *                      available during the current frame.
+ * @periodic_sched_assigned: List of periodic QHs to be executed in the next
+ *                      frame that are assigned to host channels. Items move
+ *                      from this list to periodic_sched_queued as the
+ *                      transactions for the QH are queued to the DWC_otg
+ *                      controller.
+ * @periodic_sched_queued: List of periodic QHs that have been queued for
+ *                      execution. Items move from this list to either
+ *                      periodic_sched_inactive or periodic_sched_ready when the
+ *                      channel associated with the transfer is released. If the
+ *                      interval for the QH is 1, the item moves to
+ *                      periodic_sched_ready because it must be rescheduled for
+ *                      the next frame. Otherwise, the item moves to
+ *                      periodic_sched_inactive.
+ * @periodic_usecs:     Total bandwidth claimed so far for periodic transfers.
+ *                      This value is in microseconds per (micro)frame. The
+ *                      assumption is that all periodic transfers may occur in
+ *                      the same (micro)frame.
+ * @frame_number:       Frame number read from the core at SOF. The value ranges
+ *                      from 0 to HFNUM_MAX_FRNUM.
+ * @periodic_qh_count:  Count of periodic QHs, if using several eps. Used for
+ *                      SOF enable/disable.
+ * @free_hc_list:       Free host channels in the controller. This is a list of
+ *                      struct dwc2_host_chan items.
+ * @periodic_channels:  Number of host channels assigned to periodic transfers.
+ *                      Currently assuming that there is a dedicated host
+ *                      channel for each periodic transaction and at least one
+ *                      host channel is available for non-periodic transactions.
+ * @non_periodic_channels: Number of host channels assigned to non-periodic
+ *                      transfers
+ * @hc_ptr_array:       Array of pointers to the host channel descriptors.
+ *                      Allows accessing a host channel descriptor given the
+ *                      host channel number. This is useful in interrupt
+ *                      handlers.
+ * @status_buf:         Buffer used for data received during the status phase of
+ *                      a control transfer.
+ * @status_buf_dma:     DMA address for status_buf
+ * @start_work:         Delayed work for handling host A-cable connection
+ * @reset_work:         Delayed work for handling a port reset
+ * @lock:               Spinlock that protects all the driver data structures
+ * @priv:               Stores a pointer to the struct usb_hcd
+ * @otg_port:           OTG port number
+ * @frame_list:         Frame list
+ * @frame_list_dma:     Frame list DMA address
+ */
+struct dwc2_hsotg {
+	struct device *dev;
+	void __iomem *regs;
+	struct dwc2_core_params *core_params;
+	u32 hwcfg1;
+	u32 hwcfg2;
+	u32 hwcfg3;
+	u32 hwcfg4;
+	u32 hptxfsiz;
+	u32 snpsid;
+	u16 total_fifo_size;
+	u16 rx_fifo_size;
+	u16 nperio_tx_fifo_size;
+	enum usb_otg_state op_state;
+
+	unsigned int queuing_high_bandwidth:1;
+	unsigned int srp_success:1;
+
+	struct workqueue_struct *wq_otg;
+	struct work_struct wf_otg;
+	struct timer_list wkp_timer;
+	enum dwc2_lx_state lx_state;
+
+	union dwc2_hcd_internal_flags {
+		u32 d32;
+		struct {
+			unsigned port_connect_status_change:1;
+			unsigned port_connect_status:1;
+			unsigned port_reset_change:1;
+			unsigned port_enable_change:1;
+			unsigned port_suspend_change:1;
+			unsigned port_over_current_change:1;
+			unsigned port_l1_change:1;
+			unsigned reserved:26;
+		} b;
+	} flags;
+
+	struct list_head non_periodic_sched_inactive;
+	struct list_head non_periodic_sched_active;
+	struct list_head *non_periodic_qh_ptr;
+	struct list_head periodic_sched_inactive;
+	struct list_head periodic_sched_ready;
+	struct list_head periodic_sched_assigned;
+	struct list_head periodic_sched_queued;
+	u16 periodic_usecs;
+	u16 frame_number;
+	u16 periodic_qh_count;
+
+#ifdef CONFIG_USB_DWC2_TRACK_MISSED_SOFS
+#define FRAME_NUM_ARRAY_SIZE 1000
+	u16 last_frame_num;
+	u16 *frame_num_array;
+	u16 *last_frame_num_array;
+	int frame_num_idx;
+	int dumped_frame_num_array;
+#endif
+
+	struct list_head free_hc_list;
+	int periodic_channels;
+	int non_periodic_channels;
+	struct dwc2_host_chan *hc_ptr_array[MAX_EPS_CHANNELS];
+	u8 *status_buf;
+	dma_addr_t status_buf_dma;
+#define DWC2_HCD_STATUS_BUF_SIZE 64
+
+	struct delayed_work start_work;
+	struct delayed_work reset_work;
+	spinlock_t lock;
+	void *priv;
+	u8 otg_port;
+	u32 *frame_list;
+	dma_addr_t frame_list_dma;
+
+	/* DWC OTG HW Release versions */
+#define DWC2_CORE_REV_2_71a	0x4f54271a
+#define DWC2_CORE_REV_2_90a	0x4f54290a
+#define DWC2_CORE_REV_2_92a	0x4f54292a
+#define DWC2_CORE_REV_2_94a	0x4f54294a
+#define DWC2_CORE_REV_3_00a	0x4f54300a
+
+#ifdef DEBUG
+	u32 frrem_samples;
+	u64 frrem_accum;
+
+	u32 hfnum_7_samples_a;
+	u64 hfnum_7_frrem_accum_a;
+	u32 hfnum_0_samples_a;
+	u64 hfnum_0_frrem_accum_a;
+	u32 hfnum_other_samples_a;
+	u64 hfnum_other_frrem_accum_a;
+
+	u32 hfnum_7_samples_b;
+	u64 hfnum_7_frrem_accum_b;
+	u32 hfnum_0_samples_b;
+	u64 hfnum_0_frrem_accum_b;
+	u32 hfnum_other_samples_b;
+	u64 hfnum_other_frrem_accum_b;
+#endif
+};
+
+/* Reasons for halting a host channel */
+enum dwc2_halt_status {
+	DWC2_HC_XFER_NO_HALT_STATUS,
+	DWC2_HC_XFER_COMPLETE,
+	DWC2_HC_XFER_URB_COMPLETE,
+	DWC2_HC_XFER_ACK,
+	DWC2_HC_XFER_NAK,
+	DWC2_HC_XFER_NYET,
+	DWC2_HC_XFER_STALL,
+	DWC2_HC_XFER_XACT_ERR,
+	DWC2_HC_XFER_FRAME_OVERRUN,
+	DWC2_HC_XFER_BABBLE_ERR,
+	DWC2_HC_XFER_DATA_TOGGLE_ERR,
+	DWC2_HC_XFER_AHB_ERR,
+	DWC2_HC_XFER_PERIODIC_INCOMPLETE,
+	DWC2_HC_XFER_URB_DEQUEUE,
+};
+
+/*
+ * The following functions support initialization of the core driver component
+ * and the DWC_otg controller
+ */
+extern void dwc2_core_host_init(struct dwc2_hsotg *hsotg);
+
+/*
+ * Host core Functions.
+ * The following functions support managing the DWC_otg controller in host
+ * mode.
+ */
+extern void dwc2_hc_init(struct dwc2_hsotg *hsotg, struct dwc2_host_chan *chan);
+extern void dwc2_hc_halt(struct dwc2_hsotg *hsotg, struct dwc2_host_chan *chan,
+			 enum dwc2_halt_status halt_status);
+extern void dwc2_hc_cleanup(struct dwc2_hsotg *hsotg,
+			    struct dwc2_host_chan *chan);
+extern void dwc2_hc_start_transfer(struct dwc2_hsotg *hsotg,
+				   struct dwc2_host_chan *chan);
+extern void dwc2_hc_start_transfer_ddma(struct dwc2_hsotg *hsotg,
+					struct dwc2_host_chan *chan);
+extern int dwc2_hc_continue_transfer(struct dwc2_hsotg *hsotg,
+				     struct dwc2_host_chan *chan);
+extern void dwc2_hc_do_ping(struct dwc2_hsotg *hsotg,
+			    struct dwc2_host_chan *chan);
+extern void dwc2_enable_host_interrupts(struct dwc2_hsotg *hsotg);
+extern void dwc2_disable_host_interrupts(struct dwc2_hsotg *hsotg);
+
+extern u32 dwc2_calc_frame_interval(struct dwc2_hsotg *hsotg);
+extern int dwc2_check_core_status(struct dwc2_hsotg *hsotg);
+
+/*
+ * Common core Functions.
+ * The following functions support managing the DWC_otg controller in either
+ * device or host mode.
+ */
+extern void dwc2_read_packet(struct dwc2_hsotg *hsotg, u8 *dest, u16 bytes);
+extern void dwc2_flush_tx_fifo(struct dwc2_hsotg *hsotg, const int num);
+extern void dwc2_flush_rx_fifo(struct dwc2_hsotg *hsotg);
+
+extern int dwc2_core_init(struct dwc2_hsotg *hsotg, bool select_phy, int irq);
+extern void dwc2_enable_global_interrupts(struct dwc2_hsotg *hcd);
+extern void dwc2_disable_global_interrupts(struct dwc2_hsotg *hcd);
+
+/* This function should be called on every hardware interrupt. */
+extern irqreturn_t dwc2_handle_common_intr(int irq, void *dev);
+
+/* OTG Core Parameters */
+
+/*
+ * Specifies the OTG capabilities. The driver will automatically
+ * detect the value for this parameter if none is specified.
+ * 0 - HNP and SRP capable (default)
+ * 1 - SRP Only capable
+ * 2 - No HNP/SRP capable
+ */
+extern int dwc2_set_param_otg_cap(struct dwc2_hsotg *hsotg, int val);
+#define DWC2_CAP_PARAM_HNP_SRP_CAPABLE		0
+#define DWC2_CAP_PARAM_SRP_ONLY_CAPABLE		1
+#define DWC2_CAP_PARAM_NO_HNP_SRP_CAPABLE	2
+
+/*
+ * Specifies whether to use slave or DMA mode for accessing the data
+ * FIFOs. The driver will automatically detect the value for this
+ * parameter if none is specified.
+ * 0 - Slave
+ * 1 - DMA (default, if available)
+ */
+extern int dwc2_set_param_dma_enable(struct dwc2_hsotg *hsotg, int val);
+
+/*
+ * When DMA mode is enabled specifies whether to use
+ * address DMA or DMA Descritor mode for accessing the data
+ * FIFOs in device mode. The driver will automatically detect
+ * the value for this parameter if none is specified.
+ * 0 - address DMA
+ * 1 - DMA Descriptor(default, if available)
+ */
+extern int dwc2_set_param_dma_desc_enable(struct dwc2_hsotg *hsotg, int val);
+
+/*
+ * Specifies the maximum speed of operation in host and device mode.
+ * The actual speed depends on the speed of the attached device and
+ * the value of phy_type. The actual speed depends on the speed of the
+ * attached device.
+ * 0 - High Speed (default)
+ * 1 - Full Speed
+ */
+extern int dwc2_set_param_speed(struct dwc2_hsotg *hsotg, int val);
+#define DWC2_SPEED_PARAM_HIGH	0
+#define DWC2_SPEED_PARAM_FULL	1
+
+/*
+ * Specifies whether low power mode is supported when attached
+ * to a Full Speed or Low Speed device in host mode.
+ *
+ * 0 - Don't support low power mode (default)
+ * 1 - Support low power mode
+ */
+extern int dwc2_set_param_host_support_fs_ls_low_power(struct dwc2_hsotg *hsotg,
+						       int val);
+
+/*
+ * Specifies the PHY clock rate in low power mode when connected to a
+ * Low Speed device in host mode. This parameter is applicable only if
+ * HOST_SUPPORT_FS_LS_LOW_POWER is enabled. If PHY_TYPE is set to FS
+ * then defaults to 6 MHZ otherwise 48 MHZ.
+ *
+ * 0 - 48 MHz
+ * 1 - 6 MHz
+ */
+extern int dwc2_set_param_host_ls_low_power_phy_clk(struct dwc2_hsotg *hsotg,
+						    int val);
+#define DWC2_HOST_LS_LOW_POWER_PHY_CLK_PARAM_48MHZ	0
+#define DWC2_HOST_LS_LOW_POWER_PHY_CLK_PARAM_6MHZ	1
+
+/*
+ * 0 - Use cC FIFO size parameters
+ * 1 - Allow dynamic FIFO sizing (default)
+ */
+extern int dwc2_set_param_enable_dynamic_fifo(struct dwc2_hsotg *hsotg,
+					      int val);
+
+/*
+ * Number of 4-byte words in the Rx FIFO in host mode when dynamic
+ * FIFO sizing is enabled.
+ * 16 to 32768 (default 1024)
+ */
+extern int dwc2_set_param_host_rx_fifo_size(struct dwc2_hsotg *hsotg, int val);
+
+/*
+ * Number of 4-byte words in the non-periodic Tx FIFO in host mode
+ * when Dynamic FIFO sizing is enabled in the core.
+ * 16 to 32768 (default 256)
+ */
+extern int dwc2_set_param_host_nperio_tx_fifo_size(struct dwc2_hsotg *hsotg,
+						   int val);
+
+/*
+ * Number of 4-byte words in the host periodic Tx FIFO when dynamic
+ * FIFO sizing is enabled.
+ * 16 to 32768 (default 256)
+ */
+extern int dwc2_set_param_host_perio_tx_fifo_size(struct dwc2_hsotg *hsotg,
+						  int val);
+
+/*
+ * The maximum transfer size supported in bytes.
+ * 2047 to 65,535  (default 65,535)
+ */
+extern int dwc2_set_param_max_transfer_size(struct dwc2_hsotg *hsotg, int val);
+
+/*
+ * The maximum number of packets in a transfer.
+ * 15 to 511  (default 511)
+ */
+extern int dwc2_set_param_max_packet_count(struct dwc2_hsotg *hsotg, int val);
+
+/*
+ * The number of host channel registers to use.
+ * 1 to 16 (default 11)
+ * Note: The FPGA configuration supports a maximum of 11 host channels.
+ */
+extern int dwc2_set_param_host_channels(struct dwc2_hsotg *hsotg, int val);
+
+/*
+ * Specifies the type of PHY interface to use. By default, the driver
+ * will automatically detect the phy_type.
+ *
+ * 0 - Full Speed PHY
+ * 1 - UTMI+ (default)
+ * 2 - ULPI
+ */
+extern int dwc2_set_param_phy_type(struct dwc2_hsotg *hsotg, int val);
+#define DWC2_PHY_TYPE_PARAM_FS		0
+#define DWC2_PHY_TYPE_PARAM_UTMI	1
+#define DWC2_PHY_TYPE_PARAM_ULPI	2
+
+/*
+ * Specifies the UTMI+ Data Width. This parameter is
+ * applicable for a PHY_TYPE of UTMI+ or ULPI. (For a ULPI
+ * PHY_TYPE, this parameter indicates the data width between
+ * the MAC and the ULPI Wrapper.) Also, this parameter is
+ * applicable only if the OTG_HSPHY_WIDTH cC parameter was set
+ * to "8 and 16 bits", meaning that the core has been
+ * configured to work at either data path width.
+ *
+ * 8 or 16 bits (default 16)
+ */
+extern int dwc2_set_param_phy_utmi_width(struct dwc2_hsotg *hsotg, int val);
+
+/*
+ * Specifies whether the ULPI operates at double or single
+ * data rate. This parameter is only applicable if PHY_TYPE is
+ * ULPI.
+ *
+ * 0 - single data rate ULPI interface with 8 bit wide data
+ * bus (default)
+ * 1 - double data rate ULPI interface with 4 bit wide data
+ * bus
+ */
+extern int dwc2_set_param_phy_ulpi_ddr(struct dwc2_hsotg *hsotg, int val);
+
+/*
+ * Specifies whether to use the internal or external supply to
+ * drive the vbus with a ULPI phy.
+ */
+extern int dwc2_set_param_phy_ulpi_ext_vbus(struct dwc2_hsotg *hsotg, int val);
+#define DWC2_PHY_ULPI_INTERNAL_VBUS	0
+#define DWC2_PHY_ULPI_EXTERNAL_VBUS	1
+
+/*
+ * Specifies whether to use the I2Cinterface for full speed PHY. This
+ * parameter is only applicable if PHY_TYPE is FS.
+ * 0 - No (default)
+ * 1 - Yes
+ */
+extern int dwc2_set_param_i2c_enable(struct dwc2_hsotg *hsotg, int val);
+
+extern int dwc2_set_param_ulpi_fs_ls(struct dwc2_hsotg *hsotg, int val);
+
+extern int dwc2_set_param_ts_dline(struct dwc2_hsotg *hsotg, int val);
+
+/*
+ * Specifies whether dedicated transmit FIFOs are
+ * enabled for non periodic IN endpoints in device mode
+ * 0 - No
+ * 1 - Yes
+ */
+extern int dwc2_set_param_en_multiple_tx_fifo(struct dwc2_hsotg *hsotg,
+					      int val);
+
+extern int dwc2_set_param_reload_ctl(struct dwc2_hsotg *hsotg, int val);
+
+extern int dwc2_set_param_ahb_single(struct dwc2_hsotg *hsotg, int val);
+
+extern int dwc2_set_param_otg_ver(struct dwc2_hsotg *hsotg, int val);
+
+/*
+ * Dump core registers and SPRAM
+ */
+extern void dwc2_dump_dev_registers(struct dwc2_hsotg *hsotg);
+extern void dwc2_dump_host_registers(struct dwc2_hsotg *hsotg);
+extern void dwc2_dump_global_registers(struct dwc2_hsotg *hsotg);
+
+/*
+ * Return OTG version - either 1.3 or 2.0
+ */
+extern u16 dwc2_get_otg_version(struct dwc2_hsotg *hsotg);
+
+#endif /* __DWC2_CORE_H__ */
diff --git a/drivers/staging/dwc2/core_intr.c b/drivers/staging/dwc2/core_intr.c
new file mode 100644
index 00000000000..4c9ad14e90e
--- /dev/null
+++ b/drivers/staging/dwc2/core_intr.c
@@ -0,0 +1,505 @@
+/*
+ * core_intr.c - DesignWare HS OTG Controller common interrupt handling
+ *
+ * Copyright (C) 2004-2013 Synopsys, Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions, and the following disclaimer,
+ *    without modification.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The names of the above-listed copyright holders may not be used
+ *    to endorse or promote products derived from this software without
+ *    specific prior written permission.
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation; either version 2 of the License, or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+ * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * This file contains the common interrupt handlers
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/spinlock.h>
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/usb.h>
+
+#include <linux/usb/hcd.h>
+#include <linux/usb/ch11.h>
+
+#include "core.h"
+#include "hcd.h"
+
+static const char *dwc2_op_state_str(struct dwc2_hsotg *hsotg)
+{
+#ifdef DEBUG
+	switch (hsotg->op_state) {
+	case OTG_STATE_A_HOST:
+		return "a_host";
+	case OTG_STATE_A_SUSPEND:
+		return "a_suspend";
+	case OTG_STATE_A_PERIPHERAL:
+		return "a_peripheral";
+	case OTG_STATE_B_PERIPHERAL:
+		return "b_peripheral";
+	case OTG_STATE_B_HOST:
+		return "b_host";
+	default:
+		return "unknown";
+	}
+#else
+	return "";
+#endif
+}
+
+/**
+ * dwc2_handle_mode_mismatch_intr() - Logs a mode mismatch warning message
+ *
+ * @hsotg: Programming view of DWC_otg controller
+ */
+static void dwc2_handle_mode_mismatch_intr(struct dwc2_hsotg *hsotg)
+{
+	dev_warn(hsotg->dev, "Mode Mismatch Interrupt: currently in %s mode\n",
+		 dwc2_is_host_mode(hsotg) ? "Host" : "Device");
+
+	/* Clear interrupt */
+	writel(GINTSTS_MODEMIS, hsotg->regs + GINTSTS);
+}
+
+/**
+ * dwc2_handle_otg_intr() - Handles the OTG Interrupts. It reads the OTG
+ * Interrupt Register (GOTGINT) to determine what interrupt has occurred.
+ *
+ * @hsotg: Programming view of DWC_otg controller
+ */
+static void dwc2_handle_otg_intr(struct dwc2_hsotg *hsotg)
+{
+	u32 gotgint;
+	u32 gotgctl;
+	u32 gintmsk;
+
+	gotgint = readl(hsotg->regs + GOTGINT);
+	gotgctl = readl(hsotg->regs + GOTGCTL);
+	dev_dbg(hsotg->dev, "++OTG Interrupt gotgint=%0x [%s]\n", gotgint,
+		dwc2_op_state_str(hsotg));
+
+	if (gotgint & GOTGINT_SES_END_DET) {
+		dev_dbg(hsotg->dev,
+			" ++OTG Interrupt: Session End Detected++ (%s)\n",
+			dwc2_op_state_str(hsotg));
+		gotgctl = readl(hsotg->regs + GOTGCTL);
+
+		if (hsotg->op_state == OTG_STATE_B_HOST) {
+			hsotg->op_state = OTG_STATE_B_PERIPHERAL;
+		} else {
+			/*
+			 * If not B_HOST and Device HNP still set, HNP did
+			 * not succeed!
+			 */
+			if (gotgctl & GOTGCTL_DEVHNPEN) {
+				dev_dbg(hsotg->dev, "Session End Detected\n");
+				dev_err(hsotg->dev,
+					"Device Not Connected/Responding!\n");
+			}
+
+			/*
+			 * If Session End Detected the B-Cable has been
+			 * disconnected
+			 */
+			/* Reset to a clean state */
+			hsotg->lx_state = DWC2_L0;
+		}
+
+		gotgctl = readl(hsotg->regs + GOTGCTL);
+		gotgctl &= ~GOTGCTL_DEVHNPEN;
+		writel(gotgctl, hsotg->regs + GOTGCTL);
+	}
+
+	if (gotgint & GOTGINT_SES_REQ_SUC_STS_CHNG) {
+		dev_dbg(hsotg->dev,
+			" ++OTG Interrupt: Session Request Success Status Change++\n");
+		gotgctl = readl(hsotg->regs + GOTGCTL);
+		if (gotgctl & GOTGCTL_SESREQSCS) {
+			if (hsotg->core_params->phy_type ==
+					DWC2_PHY_TYPE_PARAM_FS
+			    && hsotg->core_params->i2c_enable > 0) {
+				hsotg->srp_success = 1;
+			} else {
+				/* Clear Session Request */
+				gotgctl = readl(hsotg->regs + GOTGCTL);
+				gotgctl &= ~GOTGCTL_SESREQ;
+				writel(gotgctl, hsotg->regs + GOTGCTL);
+			}
+		}
+	}
+
+	if (gotgint & GOTGINT_HST_NEG_SUC_STS_CHNG) {
+		/*
+		 * Print statements during the HNP interrupt handling
+		 * can cause it to fail
+		 */
+		gotgctl = readl(hsotg->regs + GOTGCTL);
+		/*
+		 * WA for 3.00a- HW is not setting cur_mode, even sometimes
+		 * this does not help
+		 */
+		if (hsotg->snpsid >= DWC2_CORE_REV_3_00a)
+			udelay(100);
+		if (gotgctl & GOTGCTL_HSTNEGSCS) {
+			if (dwc2_is_host_mode(hsotg)) {
+				hsotg->op_state = OTG_STATE_B_HOST;
+				/*
+				 * Need to disable SOF interrupt immediately.
+				 * When switching from device to host, the PCD
+				 * interrupt handler won't handle the interrupt
+				 * if host mode is already set. The HCD
+				 * interrupt handler won't get called if the
+				 * HCD state is HALT. This means that the
+				 * interrupt does not get handled and Linux
+				 * complains loudly.
+				 */
+				gintmsk = readl(hsotg->regs + GINTMSK);
+				gintmsk &= ~GINTSTS_SOF;
+				writel(gintmsk, hsotg->regs + GINTMSK);
+
+				/*
+				 * Call callback function with spin lock
+				 * released
+				 */
+				spin_unlock(&hsotg->lock);
+
+				/* Initialize the Core for Host mode */
+				dwc2_hcd_start(hsotg);
+				spin_lock(&hsotg->lock);
+				hsotg->op_state = OTG_STATE_B_HOST;
+			}
+		} else {
+			gotgctl = readl(hsotg->regs + GOTGCTL);
+			gotgctl &= ~(GOTGCTL_HNPREQ | GOTGCTL_DEVHNPEN);
+			writel(gotgctl, hsotg->regs + GOTGCTL);
+			dev_dbg(hsotg->dev, "HNP Failed\n");
+			dev_err(hsotg->dev,
+				"Device Not Connected/Responding\n");
+		}
+	}
+
+	if (gotgint & GOTGINT_HST_NEG_DET) {
+		/*
+		 * The disconnect interrupt is set at the same time as
+		 * Host Negotiation Detected. During the mode switch all
+		 * interrupts are cleared so the disconnect interrupt
+		 * handler will not get executed.
+		 */
+		dev_dbg(hsotg->dev,
+			" ++OTG Interrupt: Host Negotiation Detected++ (%s)\n",
+			(dwc2_is_host_mode(hsotg) ? "Host" : "Device"));
+		if (dwc2_is_device_mode(hsotg)) {
+			dev_dbg(hsotg->dev, "a_suspend->a_peripheral (%d)\n",
+				hsotg->op_state);
+			spin_unlock(&hsotg->lock);
+			dwc2_hcd_disconnect(hsotg);
+			spin_lock(&hsotg->lock);
+			hsotg->op_state = OTG_STATE_A_PERIPHERAL;
+		} else {
+			/* Need to disable SOF interrupt immediately */
+			gintmsk = readl(hsotg->regs + GINTMSK);
+			gintmsk &= ~GINTSTS_SOF;
+			writel(gintmsk, hsotg->regs + GINTMSK);
+			spin_unlock(&hsotg->lock);
+			dwc2_hcd_start(hsotg);
+			spin_lock(&hsotg->lock);
+			hsotg->op_state = OTG_STATE_A_HOST;
+		}
+	}
+
+	if (gotgint & GOTGINT_A_DEV_TOUT_CHG)
+		dev_dbg(hsotg->dev,
+			" ++OTG Interrupt: A-Device Timeout Change++\n");
+	if (gotgint & GOTGINT_DBNCE_DONE)
+		dev_dbg(hsotg->dev, " ++OTG Interrupt: Debounce Done++\n");
+
+	/* Clear GOTGINT */
+	writel(gotgint, hsotg->regs + GOTGINT);
+}
+
+/**
+ * dwc2_handle_conn_id_status_change_intr() - Handles the Connector ID Status
+ * Change Interrupt
+ *
+ * @hsotg: Programming view of DWC_otg controller
+ *
+ * Reads the OTG Interrupt Register (GOTCTL) to determine whether this is a
+ * Device to Host Mode transition or a Host to Device Mode transition. This only
+ * occurs when the cable is connected/removed from the PHY connector.
+ */
+static void dwc2_handle_conn_id_status_change_intr(struct dwc2_hsotg *hsotg)
+{
+	u32 gintmsk = readl(hsotg->regs + GINTMSK);
+
+	/* Need to disable SOF interrupt immediately */
+	gintmsk &= ~GINTSTS_SOF;
+	writel(gintmsk, hsotg->regs + GINTMSK);
+
+	dev_dbg(hsotg->dev, " ++Connector ID Status Change Interrupt++  (%s)\n",
+		dwc2_is_host_mode(hsotg) ? "Host" : "Device");
+
+	/*
+	 * Need to schedule a work, as there are possible DELAY function calls.
+	 * Release lock before scheduling workq as it holds spinlock during
+	 * scheduling.
+	 */
+	spin_unlock(&hsotg->lock);
+	queue_work(hsotg->wq_otg, &hsotg->wf_otg);
+	spin_lock(&hsotg->lock);
+
+	/* Clear interrupt */
+	writel(GINTSTS_CONIDSTSCHNG, hsotg->regs + GINTSTS);
+}
+
+/**
+ * dwc2_handle_session_req_intr() - This interrupt indicates that a device is
+ * initiating the Session Request Protocol to request the host to turn on bus
+ * power so a new session can begin
+ *
+ * @hsotg: Programming view of DWC_otg controller
+ *
+ * This handler responds by turning on bus power. If the DWC_otg controller is
+ * in low power mode, this handler brings the controller out of low power mode
+ * before turning on bus power.
+ */
+static void dwc2_handle_session_req_intr(struct dwc2_hsotg *hsotg)
+{
+	dev_dbg(hsotg->dev, "++Session Request Interrupt++\n");
+
+	/* Clear interrupt */
+	writel(GINTSTS_SESSREQINT, hsotg->regs + GINTSTS);
+}
+
+/*
+ * This interrupt indicates that the DWC_otg controller has detected a
+ * resume or remote wakeup sequence. If the DWC_otg controller is in
+ * low power mode, the handler must brings the controller out of low
+ * power mode. The controller automatically begins resume signaling.
+ * The handler schedules a time to stop resume signaling.
+ */
+static void dwc2_handle_wakeup_detected_intr(struct dwc2_hsotg *hsotg)
+{
+	dev_dbg(hsotg->dev, "++Resume or Remote Wakeup Detected Interrupt++\n");
+	dev_dbg(hsotg->dev, "%s lxstate = %d\n", __func__, hsotg->lx_state);
+
+	if (dwc2_is_device_mode(hsotg)) {
+		dev_dbg(hsotg->dev, "DSTS=0x%0x\n", readl(hsotg->regs + DSTS));
+		if (hsotg->lx_state == DWC2_L2) {
+			u32 dctl = readl(hsotg->regs + DCTL);
+
+			/* Clear Remote Wakeup Signaling */
+			dctl &= ~DCTL_RMTWKUPSIG;
+			writel(dctl, hsotg->regs + DCTL);
+		}
+		/* Change to L0 state */
+		hsotg->lx_state = DWC2_L0;
+	} else {
+		if (hsotg->lx_state != DWC2_L1) {
+			u32 pcgcctl = readl(hsotg->regs + PCGCTL);
+
+			/* Restart the Phy Clock */
+			pcgcctl &= ~PCGCTL_STOPPCLK;
+			writel(pcgcctl, hsotg->regs + PCGCTL);
+			mod_timer(&hsotg->wkp_timer,
+				  jiffies + msecs_to_jiffies(71));
+		} else {
+			/* Change to L0 state */
+			hsotg->lx_state = DWC2_L0;
+		}
+	}
+
+	/* Clear interrupt */
+	writel(GINTSTS_WKUPINT, hsotg->regs + GINTSTS);
+}
+
+/*
+ * This interrupt indicates that a device has been disconnected from the
+ * root port
+ */
+static void dwc2_handle_disconnect_intr(struct dwc2_hsotg *hsotg)
+{
+	dev_dbg(hsotg->dev, "++Disconnect Detected Interrupt++ (%s) %s\n",
+		dwc2_is_host_mode(hsotg) ? "Host" : "Device",
+		dwc2_op_state_str(hsotg));
+
+	/* Change to L3 (OFF) state */
+	hsotg->lx_state = DWC2_L3;
+
+	writel(GINTSTS_DISCONNINT, hsotg->regs + GINTSTS);
+}
+
+/*
+ * This interrupt indicates that SUSPEND state has been detected on the USB.
+ *
+ * For HNP the USB Suspend interrupt signals the change from "a_peripheral"
+ * to "a_host".
+ *
+ * When power management is enabled the core will be put in low power mode.
+ */
+static void dwc2_handle_usb_suspend_intr(struct dwc2_hsotg *hsotg)
+{
+	u32 dsts;
+
+	dev_dbg(hsotg->dev, "USB SUSPEND\n");
+
+	if (dwc2_is_device_mode(hsotg)) {
+		/*
+		 * Check the Device status register to determine if the Suspend
+		 * state is active
+		 */
+		dsts = readl(hsotg->regs + DSTS);
+		dev_dbg(hsotg->dev, "DSTS=0x%0x\n", dsts);
+		dev_dbg(hsotg->dev,
+			"DSTS.Suspend Status=%d HWCFG4.Power Optimize=%d\n",
+			!!(dsts & DSTS_SUSPSTS),
+			!!(hsotg->hwcfg4 & GHWCFG4_POWER_OPTIMIZ));
+	} else {
+		if (hsotg->op_state == OTG_STATE_A_PERIPHERAL) {
+			dev_dbg(hsotg->dev, "a_peripheral->a_host\n");
+
+			/* Clear the a_peripheral flag, back to a_host */
+			spin_unlock(&hsotg->lock);
+			dwc2_hcd_start(hsotg);
+			spin_lock(&hsotg->lock);
+			hsotg->op_state = OTG_STATE_A_HOST;
+		}
+	}
+
+	/* Change to L2 (suspend) state */
+	hsotg->lx_state = DWC2_L2;
+
+	/* Clear interrupt */
+	writel(GINTSTS_USBSUSP, hsotg->regs + GINTSTS);
+}
+
+#define GINTMSK_COMMON	(GINTSTS_WKUPINT | GINTSTS_SESSREQINT |		\
+			 GINTSTS_CONIDSTSCHNG | GINTSTS_OTGINT |	\
+			 GINTSTS_MODEMIS | GINTSTS_DISCONNINT |		\
+			 GINTSTS_USBSUSP | GINTSTS_RESTOREDONE |	\
+			 GINTSTS_PRTINT)
+
+/*
+ * This function returns the Core Interrupt register
+ */
+static u32 dwc2_read_common_intr(struct dwc2_hsotg *hsotg)
+{
+	u32 gintsts;
+	u32 gintmsk;
+	u32 gahbcfg;
+	u32 gintmsk_common = GINTMSK_COMMON;
+
+	gintsts = readl(hsotg->regs + GINTSTS);
+	gintmsk = readl(hsotg->regs + GINTMSK);
+	gahbcfg = readl(hsotg->regs + GAHBCFG);
+
+#ifdef DEBUG
+	/* If any common interrupts set */
+	if (gintsts & gintmsk_common)
+		dev_dbg(hsotg->dev, "gintsts=%08x  gintmsk=%08x\n",
+			gintsts, gintmsk);
+#endif
+
+	if (gahbcfg & GAHBCFG_GLBL_INTR_EN)
+		return gintsts & gintmsk & gintmsk_common;
+	else
+		return 0;
+}
+
+/*
+ * Common interrupt handler
+ *
+ * The common interrupts are those that occur in both Host and Device mode.
+ * This handler handles the following interrupts:
+ * - Mode Mismatch Interrupt
+ * - OTG Interrupt
+ * - Connector ID Status Change Interrupt
+ * - Disconnect Interrupt
+ * - Session Request Interrupt
+ * - Resume / Remote Wakeup Detected Interrupt
+ * - Suspend Interrupt
+ */
+irqreturn_t dwc2_handle_common_intr(int irq, void *dev)
+{
+	struct dwc2_hsotg *hsotg = dev;
+	u32 gintsts;
+	int retval = 0;
+
+	if (dwc2_check_core_status(hsotg) < 0) {
+		dev_warn(hsotg->dev, "Controller is disconnected\n");
+		goto out;
+	}
+
+	spin_lock(&hsotg->lock);
+
+	gintsts = dwc2_read_common_intr(hsotg);
+	if (gintsts & ~GINTSTS_PRTINT)
+		retval = 1;
+
+	if (gintsts & GINTSTS_MODEMIS)
+		dwc2_handle_mode_mismatch_intr(hsotg);
+	if (gintsts & GINTSTS_OTGINT)
+		dwc2_handle_otg_intr(hsotg);
+	if (gintsts & GINTSTS_CONIDSTSCHNG)
+		dwc2_handle_conn_id_status_change_intr(hsotg);
+	if (gintsts & GINTSTS_DISCONNINT)
+		dwc2_handle_disconnect_intr(hsotg);
+	if (gintsts & GINTSTS_SESSREQINT)
+		dwc2_handle_session_req_intr(hsotg);
+	if (gintsts & GINTSTS_WKUPINT)
+		dwc2_handle_wakeup_detected_intr(hsotg);
+	if (gintsts & GINTSTS_USBSUSP)
+		dwc2_handle_usb_suspend_intr(hsotg);
+
+	if (gintsts & GINTSTS_RESTOREDONE) {
+		gintsts = GINTSTS_RESTOREDONE;
+		writel(gintsts, hsotg->regs + GINTSTS);
+		dev_dbg(hsotg->dev, " --Restore done interrupt received--\n");
+	}
+
+	if (gintsts & GINTSTS_PRTINT) {
+		/*
+		 * The port interrupt occurs while in device mode with HPRT0
+		 * Port Enable/Disable
+		 */
+		if (dwc2_is_device_mode(hsotg)) {
+			dev_dbg(hsotg->dev,
+				" --Port interrupt received in Device mode--\n");
+			gintsts = GINTSTS_PRTINT;
+			writel(gintsts, hsotg->regs + GINTSTS);
+			retval = 1;
+		}
+	}
+
+	spin_unlock(&hsotg->lock);
+out:
+	return IRQ_RETVAL(retval);
+}
+EXPORT_SYMBOL_GPL(dwc2_handle_common_intr);
diff --git a/drivers/staging/dwc2/hcd.c b/drivers/staging/dwc2/hcd.c
new file mode 100644
index 00000000000..827ab781ae9
--- /dev/null
+++ b/drivers/staging/dwc2/hcd.c
@@ -0,0 +1,2981 @@
+/*
+ * hcd.c - DesignWare HS OTG Controller host-mode routines
+ *
+ * Copyright (C) 2004-2013 Synopsys, Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions, and the following disclaimer,
+ *    without modification.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The names of the above-listed copyright holders may not be used
+ *    to endorse or promote products derived from this software without
+ *    specific prior written permission.
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation; either version 2 of the License, or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+ * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * This file contains the core HCD code, and implements the Linux hc_driver
+ * API
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/spinlock.h>
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/usb.h>
+
+#include <linux/usb/hcd.h>
+#include <linux/usb/ch11.h>
+
+#include "core.h"
+#include "hcd.h"
+
+/**
+ * dwc2_dump_channel_info() - Prints the state of a host channel
+ *
+ * @hsotg: Programming view of DWC_otg controller
+ * @chan:  Pointer to the channel to dump
+ *
+ * Must be called with interrupt disabled and spinlock held
+ *
+ * NOTE: This function will be removed once the peripheral controller code
+ * is integrated and the driver is stable
+ */
+static void dwc2_dump_channel_info(struct dwc2_hsotg *hsotg,
+				   struct dwc2_host_chan *chan)
+{
+#ifdef VERBOSE_DEBUG
+	int num_channels = hsotg->core_params->host_channels;
+	struct dwc2_qh *qh;
+	u32 hcchar;
+	u32 hcsplt;
+	u32 hctsiz;
+	u32 hc_dma;
+	int i;
+
+	if (chan == NULL)
+		return;
+
+	hcchar = readl(hsotg->regs + HCCHAR(chan->hc_num));
+	hcsplt = readl(hsotg->regs + HCSPLT(chan->hc_num));
+	hctsiz = readl(hsotg->regs + HCTSIZ(chan->hc_num));
+	hc_dma = readl(hsotg->regs + HCDMA(chan->hc_num));
+
+	dev_dbg(hsotg->dev, "  Assigned to channel %p:\n", chan);
+	dev_dbg(hsotg->dev, "    hcchar 0x%08x, hcsplt 0x%08x\n",
+		hcchar, hcsplt);
+	dev_dbg(hsotg->dev, "    hctsiz 0x%08x, hc_dma 0x%08x\n",
+		hctsiz, hc_dma);
+	dev_dbg(hsotg->dev, "    dev_addr: %d, ep_num: %d, ep_is_in: %d\n",
+		chan->dev_addr, chan->ep_num, chan->ep_is_in);
+	dev_dbg(hsotg->dev, "    ep_type: %d\n", chan->ep_type);
+	dev_dbg(hsotg->dev, "    max_packet: %d\n", chan->max_packet);
+	dev_dbg(hsotg->dev, "    data_pid_start: %d\n", chan->data_pid_start);
+	dev_dbg(hsotg->dev, "    xfer_started: %d\n", chan->xfer_started);
+	dev_dbg(hsotg->dev, "    halt_status: %d\n", chan->halt_status);
+	dev_dbg(hsotg->dev, "    xfer_buf: %p\n", chan->xfer_buf);
+	dev_dbg(hsotg->dev, "    xfer_dma: %08lx\n",
+		(unsigned long)chan->xfer_dma);
+	dev_dbg(hsotg->dev, "    xfer_len: %d\n", chan->xfer_len);
+	dev_dbg(hsotg->dev, "    qh: %p\n", chan->qh);
+	dev_dbg(hsotg->dev, "  NP inactive sched:\n");
+	list_for_each_entry(qh, &hsotg->non_periodic_sched_inactive,
+			    qh_list_entry)
+		dev_dbg(hsotg->dev, "    %p\n", qh);
+	dev_dbg(hsotg->dev, "  NP active sched:\n");
+	list_for_each_entry(qh, &hsotg->non_periodic_sched_active,
+			    qh_list_entry)
+		dev_dbg(hsotg->dev, "    %p\n", qh);
+	dev_dbg(hsotg->dev, "  Channels:\n");
+	for (i = 0; i < num_channels; i++) {
+		struct dwc2_host_chan *chan = hsotg->hc_ptr_array[i];
+
+		dev_dbg(hsotg->dev, "    %2d: %p\n", i, chan);
+	}
+#endif /* VERBOSE_DEBUG */
+}
+
+/*
+ * Processes all the URBs in a single list of QHs. Completes them with
+ * -ETIMEDOUT and frees the QTD.
+ *
+ * Must be called with interrupt disabled and spinlock held
+ */
+static void dwc2_kill_urbs_in_qh_list(struct dwc2_hsotg *hsotg,
+				      struct list_head *qh_list)
+{
+	struct dwc2_qh *qh, *qh_tmp;
+	struct dwc2_qtd *qtd, *qtd_tmp;
+
+	list_for_each_entry_safe(qh, qh_tmp, qh_list, qh_list_entry) {
+		list_for_each_entry_safe(qtd, qtd_tmp, &qh->qtd_list,
+					 qtd_list_entry) {
+			if (qtd->urb != NULL) {
+				dwc2_host_complete(hsotg, qtd->urb->priv,
+						   qtd->urb, -ETIMEDOUT);
+				dwc2_hcd_qtd_unlink_and_free(hsotg, qtd, qh);
+			}
+		}
+	}
+}
+
+static void dwc2_qh_list_free(struct dwc2_hsotg *hsotg,
+			      struct list_head *qh_list)
+{
+	struct dwc2_qtd *qtd, *qtd_tmp;
+	struct dwc2_qh *qh, *qh_tmp;
+	unsigned long flags;
+
+	if (!qh_list->next)
+		/* The list hasn't been initialized yet */
+		return;
+
+	spin_lock_irqsave(&hsotg->lock, flags);
+
+	/* Ensure there are no QTDs or URBs left */
+	dwc2_kill_urbs_in_qh_list(hsotg, qh_list);
+
+	list_for_each_entry_safe(qh, qh_tmp, qh_list, qh_list_entry) {
+		dwc2_hcd_qh_unlink(hsotg, qh);
+
+		/* Free each QTD in the QH's QTD list */
+		list_for_each_entry_safe(qtd, qtd_tmp, &qh->qtd_list,
+					 qtd_list_entry)
+			dwc2_hcd_qtd_unlink_and_free(hsotg, qtd, qh);
+
+		spin_unlock_irqrestore(&hsotg->lock, flags);
+		dwc2_hcd_qh_free(hsotg, qh);
+		spin_lock_irqsave(&hsotg->lock, flags);
+	}
+
+	spin_unlock_irqrestore(&hsotg->lock, flags);
+}
+
+/*
+ * Responds with an error status of -ETIMEDOUT to all URBs in the non-periodic
+ * and periodic schedules. The QTD associated with each URB is removed from
+ * the schedule and freed. This function may be called when a disconnect is
+ * detected or when the HCD is being stopped.
+ *
+ * Must be called with interrupt disabled and spinlock held
+ */
+static void dwc2_kill_all_urbs(struct dwc2_hsotg *hsotg)
+{
+	dwc2_kill_urbs_in_qh_list(hsotg, &hsotg->non_periodic_sched_inactive);
+	dwc2_kill_urbs_in_qh_list(hsotg, &hsotg->non_periodic_sched_active);
+	dwc2_kill_urbs_in_qh_list(hsotg, &hsotg->periodic_sched_inactive);
+	dwc2_kill_urbs_in_qh_list(hsotg, &hsotg->periodic_sched_ready);
+	dwc2_kill_urbs_in_qh_list(hsotg, &hsotg->periodic_sched_assigned);
+	dwc2_kill_urbs_in_qh_list(hsotg, &hsotg->periodic_sched_queued);
+}
+
+/**
+ * dwc2_hcd_start() - Starts the HCD when switching to Host mode
+ *
+ * @hsotg: Pointer to struct dwc2_hsotg
+ */
+void dwc2_hcd_start(struct dwc2_hsotg *hsotg)
+{
+	u32 hprt0;
+
+	if (hsotg->op_state == OTG_STATE_B_HOST) {
+		/*
+		 * Reset the port. During a HNP mode switch the reset
+		 * needs to occur within 1ms and have a duration of at
+		 * least 50ms.
+		 */
+		hprt0 = dwc2_read_hprt0(hsotg);
+		hprt0 |= HPRT0_RST;
+		writel(hprt0, hsotg->regs + HPRT0);
+	}
+
+	queue_delayed_work(hsotg->wq_otg, &hsotg->start_work,
+			   msecs_to_jiffies(50));
+}
+
+/* Must be called with interrupt disabled and spinlock held */
+static void dwc2_hcd_cleanup_channels(struct dwc2_hsotg *hsotg)
+{
+	int num_channels = hsotg->core_params->host_channels;
+	struct dwc2_host_chan *channel;
+	u32 hcchar;
+	int i;
+
+	if (hsotg->core_params->dma_enable <= 0) {
+		/* Flush out any channel requests in slave mode */
+		for (i = 0; i < num_channels; i++) {
+			channel = hsotg->hc_ptr_array[i];
+			if (!list_empty(&channel->hc_list_entry))
+				continue;
+			hcchar = readl(hsotg->regs + HCCHAR(i));
+			if (hcchar & HCCHAR_CHENA) {
+				hcchar &= ~(HCCHAR_CHENA | HCCHAR_EPDIR);
+				hcchar |= HCCHAR_CHDIS;
+				writel(hcchar, hsotg->regs + HCCHAR(i));
+			}
+		}
+	}
+
+	for (i = 0; i < num_channels; i++) {
+		channel = hsotg->hc_ptr_array[i];
+		if (!list_empty(&channel->hc_list_entry))
+			continue;
+		hcchar = readl(hsotg->regs + HCCHAR(i));
+		if (hcchar & HCCHAR_CHENA) {
+			/* Halt the channel */
+			hcchar |= HCCHAR_CHDIS;
+			writel(hcchar, hsotg->regs + HCCHAR(i));
+		}
+
+		dwc2_hc_cleanup(hsotg, channel);
+		list_add_tail(&channel->hc_list_entry, &hsotg->free_hc_list);
+		/*
+		 * Added for Descriptor DMA to prevent channel double cleanup in
+		 * release_channel_ddma(), which is called from ep_disable when
+		 * device disconnects
+		 */
+		channel->qh = NULL;
+	}
+}
+
+/**
+ * dwc2_hcd_disconnect() - Handles disconnect of the HCD
+ *
+ * @hsotg: Pointer to struct dwc2_hsotg
+ *
+ * Must be called with interrupt disabled and spinlock held
+ */
+void dwc2_hcd_disconnect(struct dwc2_hsotg *hsotg)
+{
+	u32 intr;
+
+	/* Set status flags for the hub driver */
+	hsotg->flags.b.port_connect_status_change = 1;
+	hsotg->flags.b.port_connect_status = 0;
+
+	/*
+	 * Shutdown any transfers in process by clearing the Tx FIFO Empty
+	 * interrupt mask and status bits and disabling subsequent host
+	 * channel interrupts.
+	 */
+	intr = readl(hsotg->regs + GINTMSK);
+	intr &= ~(GINTSTS_NPTXFEMP | GINTSTS_PTXFEMP | GINTSTS_HCHINT);
+	writel(intr, hsotg->regs + GINTMSK);
+	intr = GINTSTS_NPTXFEMP | GINTSTS_PTXFEMP | GINTSTS_HCHINT;
+	writel(intr, hsotg->regs + GINTSTS);
+
+	/*
+	 * Turn off the vbus power only if the core has transitioned to device
+	 * mode. If still in host mode, need to keep power on to detect a
+	 * reconnection.
+	 */
+	if (dwc2_is_device_mode(hsotg)) {
+		if (hsotg->op_state != OTG_STATE_A_SUSPEND) {
+			dev_dbg(hsotg->dev, "Disconnect: PortPower off\n");
+			writel(0, hsotg->regs + HPRT0);
+		}
+
+		dwc2_disable_host_interrupts(hsotg);
+	}
+
+	/* Respond with an error status to all URBs in the schedule */
+	dwc2_kill_all_urbs(hsotg);
+
+	if (dwc2_is_host_mode(hsotg))
+		/* Clean up any host channels that were in use */
+		dwc2_hcd_cleanup_channels(hsotg);
+
+	dwc2_host_disconnect(hsotg);
+}
+
+/**
+ * dwc2_hcd_rem_wakeup() - Handles Remote Wakeup
+ *
+ * @hsotg: Pointer to struct dwc2_hsotg
+ */
+static void dwc2_hcd_rem_wakeup(struct dwc2_hsotg *hsotg)
+{
+	if (hsotg->lx_state == DWC2_L2)
+		hsotg->flags.b.port_suspend_change = 1;
+	else
+		hsotg->flags.b.port_l1_change = 1;
+}
+
+/**
+ * dwc2_hcd_stop() - Halts the DWC_otg host mode operations in a clean manner
+ *
+ * @hsotg: Pointer to struct dwc2_hsotg
+ *
+ * Must be called with interrupt disabled and spinlock held
+ */
+void dwc2_hcd_stop(struct dwc2_hsotg *hsotg)
+{
+	dev_dbg(hsotg->dev, "DWC OTG HCD STOP\n");
+
+	/*
+	 * The root hub should be disconnected before this function is called.
+	 * The disconnect will clear the QTD lists (via ..._hcd_urb_dequeue)
+	 * and the QH lists (via ..._hcd_endpoint_disable).
+	 */
+
+	/* Turn off all host-specific interrupts */
+	dwc2_disable_host_interrupts(hsotg);
+
+	/* Turn off the vbus power */
+	dev_dbg(hsotg->dev, "PortPower off\n");
+	writel(0, hsotg->regs + HPRT0);
+}
+
+static int dwc2_hcd_urb_enqueue(struct dwc2_hsotg *hsotg,
+				struct dwc2_hcd_urb *urb, void **ep_handle,
+				gfp_t mem_flags)
+{
+	struct dwc2_qtd *qtd;
+	unsigned long flags;
+	u32 intr_mask;
+	int retval;
+
+	if (!hsotg->flags.b.port_connect_status) {
+		/* No longer connected */
+		dev_err(hsotg->dev, "Not connected\n");
+		return -ENODEV;
+	}
+
+	qtd = kzalloc(sizeof(*qtd), mem_flags);
+	if (!qtd)
+		return -ENOMEM;
+
+	dwc2_hcd_qtd_init(qtd, urb);
+	retval = dwc2_hcd_qtd_add(hsotg, qtd, (struct dwc2_qh **)ep_handle,
+				  mem_flags);
+	if (retval < 0) {
+		dev_err(hsotg->dev,
+			"DWC OTG HCD URB Enqueue failed adding QTD. Error status %d\n",
+			retval);
+		kfree(qtd);
+		return retval;
+	}
+
+	intr_mask = readl(hsotg->regs + GINTMSK);
+	if (!(intr_mask & GINTSTS_SOF) && retval == 0) {
+		enum dwc2_transaction_type tr_type;
+
+		if (qtd->qh->ep_type == USB_ENDPOINT_XFER_BULK &&
+		    !(qtd->urb->flags & URB_GIVEBACK_ASAP))
+			/*
+			 * Do not schedule SG transactions until qtd has
+			 * URB_GIVEBACK_ASAP set
+			 */
+			return 0;
+
+		spin_lock_irqsave(&hsotg->lock, flags);
+		tr_type = dwc2_hcd_select_transactions(hsotg);
+		if (tr_type != DWC2_TRANSACTION_NONE)
+			dwc2_hcd_queue_transactions(hsotg, tr_type);
+		spin_unlock_irqrestore(&hsotg->lock, flags);
+	}
+
+	return retval;
+}
+
+/* Must be called with interrupt disabled and spinlock held */
+static int dwc2_hcd_urb_dequeue(struct dwc2_hsotg *hsotg,
+				struct dwc2_hcd_urb *urb)
+{
+	struct dwc2_qh *qh;
+	struct dwc2_qtd *urb_qtd;
+
+	urb_qtd = urb->qtd;
+	if (!urb_qtd) {
+		dev_dbg(hsotg->dev, "## Urb QTD is NULL ##\n");
+		return -EINVAL;
+	}
+
+	qh = urb_qtd->qh;
+	if (!qh) {
+		dev_dbg(hsotg->dev, "## Urb QTD QH is NULL ##\n");
+		return -EINVAL;
+	}
+
+	if (urb_qtd->in_process && qh->channel) {
+		dwc2_dump_channel_info(hsotg, qh->channel);
+
+		/* The QTD is in process (it has been assigned to a channel) */
+		if (hsotg->flags.b.port_connect_status)
+			/*
+			 * If still connected (i.e. in host mode), halt the
+			 * channel so it can be used for other transfers. If
+			 * no longer connected, the host registers can't be
+			 * written to halt the channel since the core is in
+			 * device mode.
+			 */
+			dwc2_hc_halt(hsotg, qh->channel,
+				     DWC2_HC_XFER_URB_DEQUEUE);
+	}
+
+	/*
+	 * Free the QTD and clean up the associated QH. Leave the QH in the
+	 * schedule if it has any remaining QTDs.
+	 */
+	if (hsotg->core_params->dma_desc_enable <= 0) {
+		u8 in_process = urb_qtd->in_process;
+
+		dwc2_hcd_qtd_unlink_and_free(hsotg, urb_qtd, qh);
+		if (in_process) {
+			dwc2_hcd_qh_deactivate(hsotg, qh, 0);
+			qh->channel = NULL;
+		} else if (list_empty(&qh->qtd_list)) {
+			dwc2_hcd_qh_unlink(hsotg, qh);
+		}
+	} else {
+		dwc2_hcd_qtd_unlink_and_free(hsotg, urb_qtd, qh);
+	}
+
+	return 0;
+}
+
+/* Must NOT be called with interrupt disabled or spinlock held */
+static int dwc2_hcd_endpoint_disable(struct dwc2_hsotg *hsotg,
+				     struct usb_host_endpoint *ep, int retry)
+{
+	struct dwc2_qtd *qtd, *qtd_tmp;
+	struct dwc2_qh *qh;
+	unsigned long flags;
+	int rc;
+
+	spin_lock_irqsave(&hsotg->lock, flags);
+
+	qh = ep->hcpriv;
+	if (!qh) {
+		rc = -EINVAL;
+		goto err;
+	}
+
+	while (!list_empty(&qh->qtd_list) && retry--) {
+		if (retry == 0) {
+			dev_err(hsotg->dev,
+				"## timeout in dwc2_hcd_endpoint_disable() ##\n");
+			rc = -EBUSY;
+			goto err;
+		}
+
+		spin_unlock_irqrestore(&hsotg->lock, flags);
+		usleep_range(20000, 40000);
+		spin_lock_irqsave(&hsotg->lock, flags);
+		qh = ep->hcpriv;
+		if (!qh) {
+			rc = -EINVAL;
+			goto err;
+		}
+	}
+
+	dwc2_hcd_qh_unlink(hsotg, qh);
+
+	/* Free each QTD in the QH's QTD list */
+	list_for_each_entry_safe(qtd, qtd_tmp, &qh->qtd_list, qtd_list_entry)
+		dwc2_hcd_qtd_unlink_and_free(hsotg, qtd, qh);
+
+	ep->hcpriv = NULL;
+	spin_unlock_irqrestore(&hsotg->lock, flags);
+	dwc2_hcd_qh_free(hsotg, qh);
+
+	return 0;
+
+err:
+	ep->hcpriv = NULL;
+	spin_unlock_irqrestore(&hsotg->lock, flags);
+
+	return rc;
+}
+
+/* Must be called with interrupt disabled and spinlock held */
+static int dwc2_hcd_endpoint_reset(struct dwc2_hsotg *hsotg,
+				   struct usb_host_endpoint *ep)
+{
+	struct dwc2_qh *qh = ep->hcpriv;
+
+	if (!qh)
+		return -EINVAL;
+
+	qh->data_toggle = DWC2_HC_PID_DATA0;
+
+	return 0;
+}
+
+/*
+ * Initializes dynamic portions of the DWC_otg HCD state
+ *
+ * Must be called with interrupt disabled and spinlock held
+ */
+static void dwc2_hcd_reinit(struct dwc2_hsotg *hsotg)
+{
+	struct dwc2_host_chan *chan, *chan_tmp;
+	int num_channels;
+	int i;
+
+	hsotg->flags.d32 = 0;
+
+	hsotg->non_periodic_qh_ptr = &hsotg->non_periodic_sched_active;
+	hsotg->non_periodic_channels = 0;
+	hsotg->periodic_channels = 0;
+
+	/*
+	 * Put all channels in the free channel list and clean up channel
+	 * states
+	 */
+	list_for_each_entry_safe(chan, chan_tmp, &hsotg->free_hc_list,
+				 hc_list_entry)
+		list_del_init(&chan->hc_list_entry);
+
+	num_channels = hsotg->core_params->host_channels;
+	for (i = 0; i < num_channels; i++) {
+		chan = hsotg->hc_ptr_array[i];
+		list_add_tail(&chan->hc_list_entry, &hsotg->free_hc_list);
+		dwc2_hc_cleanup(hsotg, chan);
+	}
+
+	/* Initialize the DWC core for host mode operation */
+	dwc2_core_host_init(hsotg);
+}
+
+static void dwc2_hc_init_split(struct dwc2_hsotg *hsotg,
+			       struct dwc2_host_chan *chan,
+			       struct dwc2_qtd *qtd, struct dwc2_hcd_urb *urb)
+{
+	int hub_addr, hub_port;
+
+	chan->do_split = 1;
+	chan->xact_pos = qtd->isoc_split_pos;
+	chan->complete_split = qtd->complete_split;
+	dwc2_host_hub_info(hsotg, urb->priv, &hub_addr, &hub_port);
+	chan->hub_addr = (u8)hub_addr;
+	chan->hub_port = (u8)hub_port;
+}
+
+static void *dwc2_hc_init_xfer(struct dwc2_hsotg *hsotg,
+			       struct dwc2_host_chan *chan,
+			       struct dwc2_qtd *qtd, void *bufptr)
+{
+	struct dwc2_hcd_urb *urb = qtd->urb;
+	struct dwc2_hcd_iso_packet_desc *frame_desc;
+
+	switch (dwc2_hcd_get_pipe_type(&urb->pipe_info)) {
+	case USB_ENDPOINT_XFER_CONTROL:
+		chan->ep_type = USB_ENDPOINT_XFER_CONTROL;
+
+		switch (qtd->control_phase) {
+		case DWC2_CONTROL_SETUP:
+			dev_vdbg(hsotg->dev, "  Control setup transaction\n");
+			chan->do_ping = 0;
+			chan->ep_is_in = 0;
+			chan->data_pid_start = DWC2_HC_PID_SETUP;
+			if (hsotg->core_params->dma_enable > 0)
+				chan->xfer_dma = urb->setup_dma;
+			else
+				chan->xfer_buf = urb->setup_packet;
+			chan->xfer_len = 8;
+			bufptr = NULL;
+			break;
+
+		case DWC2_CONTROL_DATA:
+			dev_vdbg(hsotg->dev, "  Control data transaction\n");
+			chan->data_pid_start = qtd->data_toggle;
+			break;
+
+		case DWC2_CONTROL_STATUS:
+			/*
+			 * Direction is opposite of data direction or IN if no
+			 * data
+			 */
+			dev_vdbg(hsotg->dev, "  Control status transaction\n");
+			if (urb->length == 0)
+				chan->ep_is_in = 1;
+			else
+				chan->ep_is_in =
+					dwc2_hcd_is_pipe_out(&urb->pipe_info);
+			if (chan->ep_is_in)
+				chan->do_ping = 0;
+			chan->data_pid_start = DWC2_HC_PID_DATA1;
+			chan->xfer_len = 0;
+			if (hsotg->core_params->dma_enable > 0)
+				chan->xfer_dma = hsotg->status_buf_dma;
+			else
+				chan->xfer_buf = hsotg->status_buf;
+			bufptr = NULL;
+			break;
+		}
+		break;
+
+	case USB_ENDPOINT_XFER_BULK:
+		chan->ep_type = USB_ENDPOINT_XFER_BULK;
+		break;
+
+	case USB_ENDPOINT_XFER_INT:
+		chan->ep_type = USB_ENDPOINT_XFER_INT;
+		break;
+
+	case USB_ENDPOINT_XFER_ISOC:
+		chan->ep_type = USB_ENDPOINT_XFER_ISOC;
+		if (hsotg->core_params->dma_desc_enable > 0)
+			break;
+
+		frame_desc = &urb->iso_descs[qtd->isoc_frame_index];
+		frame_desc->status = 0;
+
+		if (hsotg->core_params->dma_enable > 0) {
+			chan->xfer_dma = urb->dma;
+			chan->xfer_dma += frame_desc->offset +
+					qtd->isoc_split_offset;
+		} else {
+			chan->xfer_buf = urb->buf;
+			chan->xfer_buf += frame_desc->offset +
+					qtd->isoc_split_offset;
+		}
+
+		chan->xfer_len = frame_desc->length - qtd->isoc_split_offset;
+
+		/* For non-dword aligned buffers */
+		if (hsotg->core_params->dma_enable > 0 &&
+		    (chan->xfer_dma & 0x3))
+			bufptr = (u8 *)urb->buf + frame_desc->offset +
+					qtd->isoc_split_offset;
+		else
+			bufptr = NULL;
+
+		if (chan->xact_pos == DWC2_HCSPLT_XACTPOS_ALL) {
+			if (chan->xfer_len <= 188)
+				chan->xact_pos = DWC2_HCSPLT_XACTPOS_ALL;
+			else
+				chan->xact_pos = DWC2_HCSPLT_XACTPOS_BEGIN;
+		}
+		break;
+	}
+
+	return bufptr;
+}
+
+static int dwc2_hc_setup_align_buf(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh,
+				   struct dwc2_host_chan *chan, void *bufptr)
+{
+	u32 buf_size;
+
+	if (chan->ep_type != USB_ENDPOINT_XFER_ISOC)
+		buf_size = hsotg->core_params->max_transfer_size;
+	else
+		buf_size = 4096;
+
+	if (!qh->dw_align_buf) {
+		qh->dw_align_buf = dma_alloc_coherent(hsotg->dev, buf_size,
+						      &qh->dw_align_buf_dma,
+						      GFP_ATOMIC);
+		if (!qh->dw_align_buf)
+			return -ENOMEM;
+	}
+
+	if (!chan->ep_is_in && chan->xfer_len) {
+		dma_sync_single_for_cpu(hsotg->dev, chan->xfer_dma, buf_size,
+					DMA_TO_DEVICE);
+		memcpy(qh->dw_align_buf, bufptr, chan->xfer_len);
+		dma_sync_single_for_device(hsotg->dev, chan->xfer_dma, buf_size,
+					   DMA_TO_DEVICE);
+	}
+
+	chan->align_buf = qh->dw_align_buf_dma;
+	return 0;
+}
+
+/**
+ * dwc2_assign_and_init_hc() - Assigns transactions from a QTD to a free host
+ * channel and initializes the host channel to perform the transactions. The
+ * host channel is removed from the free list.
+ *
+ * @hsotg: The HCD state structure
+ * @qh:    Transactions from the first QTD for this QH are selected and assigned
+ *         to a free host channel
+ */
+static void dwc2_assign_and_init_hc(struct dwc2_hsotg *hsotg,
+				    struct dwc2_qh *qh)
+{
+	struct dwc2_host_chan *chan;
+	struct dwc2_hcd_urb *urb;
+	struct dwc2_qtd *qtd;
+	void *bufptr = NULL;
+
+	if (dbg_qh(qh))
+		dev_vdbg(hsotg->dev, "%s(%p,%p)\n", __func__, hsotg, qh);
+
+	if (list_empty(&qh->qtd_list)) {
+		dev_dbg(hsotg->dev, "No QTDs in QH list\n");
+		return;
+	}
+
+	if (list_empty(&hsotg->free_hc_list)) {
+		dev_dbg(hsotg->dev, "No free channel to assign\n");
+		return;
+	}
+
+	chan = list_first_entry(&hsotg->free_hc_list, struct dwc2_host_chan,
+				hc_list_entry);
+
+	/* Remove the host channel from the free list */
+	list_del_init(&chan->hc_list_entry);
+
+	qtd = list_first_entry(&qh->qtd_list, struct dwc2_qtd, qtd_list_entry);
+	urb = qtd->urb;
+	qh->channel = chan;
+	qtd->in_process = 1;
+
+	/*
+	 * Use usb_pipedevice to determine device address. This address is
+	 * 0 before the SET_ADDRESS command and the correct address afterward.
+	 */
+	chan->dev_addr = dwc2_hcd_get_dev_addr(&urb->pipe_info);
+	chan->ep_num = dwc2_hcd_get_ep_num(&urb->pipe_info);
+	chan->speed = qh->dev_speed;
+	chan->max_packet = dwc2_max_packet(qh->maxp);
+
+	chan->xfer_started = 0;
+	chan->halt_status = DWC2_HC_XFER_NO_HALT_STATUS;
+	chan->error_state = (qtd->error_count > 0);
+	chan->halt_on_queue = 0;
+	chan->halt_pending = 0;
+	chan->requests = 0;
+
+	/*
+	 * The following values may be modified in the transfer type section
+	 * below. The xfer_len value may be reduced when the transfer is
+	 * started to accommodate the max widths of the XferSize and PktCnt
+	 * fields in the HCTSIZn register.
+	 */
+
+	chan->ep_is_in = (dwc2_hcd_is_pipe_in(&urb->pipe_info) != 0);
+	if (chan->ep_is_in)
+		chan->do_ping = 0;
+	else
+		chan->do_ping = qh->ping_state;
+
+	chan->data_pid_start = qh->data_toggle;
+	chan->multi_count = 1;
+
+	if (hsotg->core_params->dma_enable > 0) {
+		chan->xfer_dma = urb->dma + urb->actual_length;
+
+		/* For non-dword aligned case */
+		if (hsotg->core_params->dma_desc_enable <= 0 &&
+		    (chan->xfer_dma & 0x3))
+			bufptr = (u8 *)urb->buf + urb->actual_length;
+	} else {
+		chan->xfer_buf = (u8 *)urb->buf + urb->actual_length;
+	}
+
+	chan->xfer_len = urb->length - urb->actual_length;
+	chan->xfer_count = 0;
+
+	/* Set the split attributes if required */
+	if (qh->do_split)
+		dwc2_hc_init_split(hsotg, chan, qtd, urb);
+	else
+		chan->do_split = 0;
+
+	/* Set the transfer attributes */
+	bufptr = dwc2_hc_init_xfer(hsotg, chan, qtd, bufptr);
+
+	/* Non DWORD-aligned buffer case */
+	if (bufptr) {
+		dev_vdbg(hsotg->dev, "Non-aligned buffer\n");
+		if (dwc2_hc_setup_align_buf(hsotg, qh, chan, bufptr)) {
+			dev_err(hsotg->dev,
+				"%s: Failed to allocate memory to handle non-dword aligned buffer\n",
+				__func__);
+			/* Add channel back to free list */
+			chan->align_buf = 0;
+			chan->multi_count = 0;
+			list_add_tail(&chan->hc_list_entry,
+				      &hsotg->free_hc_list);
+			qtd->in_process = 0;
+			qh->channel = NULL;
+			return;
+		}
+	} else {
+		chan->align_buf = 0;
+	}
+
+	if (chan->ep_type == USB_ENDPOINT_XFER_INT ||
+	    chan->ep_type == USB_ENDPOINT_XFER_ISOC)
+		/*
+		 * This value may be modified when the transfer is started
+		 * to reflect the actual transfer length
+		 */
+		chan->multi_count = dwc2_hb_mult(qh->maxp);
+
+	if (hsotg->core_params->dma_desc_enable > 0)
+		chan->desc_list_addr = qh->desc_list_dma;
+
+	dwc2_hc_init(hsotg, chan);
+	chan->qh = qh;
+}
+
+/**
+ * dwc2_hcd_select_transactions() - Selects transactions from the HCD transfer
+ * schedule and assigns them to available host channels. Called from the HCD
+ * interrupt handler functions.
+ *
+ * @hsotg: The HCD state structure
+ *
+ * Return: The types of new transactions that were assigned to host channels
+ */
+enum dwc2_transaction_type dwc2_hcd_select_transactions(
+		struct dwc2_hsotg *hsotg)
+{
+	enum dwc2_transaction_type ret_val = DWC2_TRANSACTION_NONE;
+	struct list_head *qh_ptr;
+	struct dwc2_qh *qh;
+	int num_channels;
+
+#ifdef DWC2_DEBUG_SOF
+	dev_vdbg(hsotg->dev, "  Select Transactions\n");
+#endif
+
+	/* Process entries in the periodic ready list */
+	qh_ptr = hsotg->periodic_sched_ready.next;
+	while (qh_ptr != &hsotg->periodic_sched_ready) {
+		if (list_empty(&hsotg->free_hc_list))
+			break;
+		qh = list_entry(qh_ptr, struct dwc2_qh, qh_list_entry);
+		dwc2_assign_and_init_hc(hsotg, qh);
+
+		/*
+		 * Move the QH from the periodic ready schedule to the
+		 * periodic assigned schedule
+		 */
+		qh_ptr = qh_ptr->next;
+		list_move(&qh->qh_list_entry, &hsotg->periodic_sched_assigned);
+		ret_val = DWC2_TRANSACTION_PERIODIC;
+	}
+
+	/*
+	 * Process entries in the inactive portion of the non-periodic
+	 * schedule. Some free host channels may not be used if they are
+	 * reserved for periodic transfers.
+	 */
+	num_channels = hsotg->core_params->host_channels;
+	qh_ptr = hsotg->non_periodic_sched_inactive.next;
+	while (qh_ptr != &hsotg->non_periodic_sched_inactive) {
+		if (hsotg->non_periodic_channels >= num_channels -
+						hsotg->periodic_channels)
+			break;
+		if (list_empty(&hsotg->free_hc_list))
+			break;
+		qh = list_entry(qh_ptr, struct dwc2_qh, qh_list_entry);
+		dwc2_assign_and_init_hc(hsotg, qh);
+
+		/*
+		 * Move the QH from the non-periodic inactive schedule to the
+		 * non-periodic active schedule
+		 */
+		qh_ptr = qh_ptr->next;
+		list_move(&qh->qh_list_entry,
+			  &hsotg->non_periodic_sched_active);
+
+		if (ret_val == DWC2_TRANSACTION_NONE)
+			ret_val = DWC2_TRANSACTION_NON_PERIODIC;
+		else
+			ret_val = DWC2_TRANSACTION_ALL;
+
+		hsotg->non_periodic_channels++;
+	}
+
+	return ret_val;
+}
+
+/**
+ * dwc2_queue_transaction() - Attempts to queue a single transaction request for
+ * a host channel associated with either a periodic or non-periodic transfer
+ *
+ * @hsotg: The HCD state structure
+ * @chan:  Host channel descriptor associated with either a periodic or
+ *         non-periodic transfer
+ * @fifo_dwords_avail: Number of DWORDs available in the periodic Tx FIFO
+ *                     for periodic transfers or the non-periodic Tx FIFO
+ *                     for non-periodic transfers
+ *
+ * Return: 1 if a request is queued and more requests may be needed to
+ * complete the transfer, 0 if no more requests are required for this
+ * transfer, -1 if there is insufficient space in the Tx FIFO
+ *
+ * This function assumes that there is space available in the appropriate
+ * request queue. For an OUT transfer or SETUP transaction in Slave mode,
+ * it checks whether space is available in the appropriate Tx FIFO.
+ *
+ * Must be called with interrupt disabled and spinlock held
+ */
+static int dwc2_queue_transaction(struct dwc2_hsotg *hsotg,
+				  struct dwc2_host_chan *chan,
+				  u16 fifo_dwords_avail)
+{
+	int retval = 0;
+
+	if (hsotg->core_params->dma_enable > 0) {
+		if (hsotg->core_params->dma_desc_enable > 0) {
+			if (!chan->xfer_started ||
+			    chan->ep_type == USB_ENDPOINT_XFER_ISOC) {
+				dwc2_hcd_start_xfer_ddma(hsotg, chan->qh);
+				chan->qh->ping_state = 0;
+			}
+		} else if (!chan->xfer_started) {
+			dwc2_hc_start_transfer(hsotg, chan);
+			chan->qh->ping_state = 0;
+		}
+	} else if (chan->halt_pending) {
+		/* Don't queue a request if the channel has been halted */
+	} else if (chan->halt_on_queue) {
+		dwc2_hc_halt(hsotg, chan, chan->halt_status);
+	} else if (chan->do_ping) {
+		if (!chan->xfer_started)
+			dwc2_hc_start_transfer(hsotg, chan);
+	} else if (!chan->ep_is_in ||
+		   chan->data_pid_start == DWC2_HC_PID_SETUP) {
+		if ((fifo_dwords_avail * 4) >= chan->max_packet) {
+			if (!chan->xfer_started) {
+				dwc2_hc_start_transfer(hsotg, chan);
+				retval = 1;
+			} else {
+				retval = dwc2_hc_continue_transfer(hsotg, chan);
+			}
+		} else {
+			retval = -1;
+		}
+	} else {
+		if (!chan->xfer_started) {
+			dwc2_hc_start_transfer(hsotg, chan);
+			retval = 1;
+		} else {
+			retval = dwc2_hc_continue_transfer(hsotg, chan);
+		}
+	}
+
+	return retval;
+}
+
+/*
+ * Processes periodic channels for the next frame and queues transactions for
+ * these channels to the DWC_otg controller. After queueing transactions, the
+ * Periodic Tx FIFO Empty interrupt is enabled if there are more transactions
+ * to queue as Periodic Tx FIFO or request queue space becomes available.
+ * Otherwise, the Periodic Tx FIFO Empty interrupt is disabled.
+ *
+ * Must be called with interrupt disabled and spinlock held
+ */
+static void dwc2_process_periodic_channels(struct dwc2_hsotg *hsotg)
+{
+	struct list_head *qh_ptr;
+	struct dwc2_qh *qh;
+	u32 tx_status;
+	u32 fspcavail;
+	u32 gintmsk;
+	int status;
+	int no_queue_space = 0;
+	int no_fifo_space = 0;
+	u32 qspcavail;
+
+	if (dbg_perio())
+		dev_vdbg(hsotg->dev, "Queue periodic transactions\n");
+
+	tx_status = readl(hsotg->regs + HPTXSTS);
+	qspcavail = tx_status >> TXSTS_QSPCAVAIL_SHIFT &
+		    TXSTS_QSPCAVAIL_MASK >> TXSTS_QSPCAVAIL_SHIFT;
+	fspcavail = tx_status >> TXSTS_FSPCAVAIL_SHIFT &
+		    TXSTS_FSPCAVAIL_MASK >> TXSTS_FSPCAVAIL_SHIFT;
+
+	if (dbg_perio()) {
+		dev_vdbg(hsotg->dev, "  P Tx Req Queue Space Avail (before queue): %d\n",
+			 qspcavail);
+		dev_vdbg(hsotg->dev, "  P Tx FIFO Space Avail (before queue): %d\n",
+			 fspcavail);
+	}
+
+	qh_ptr = hsotg->periodic_sched_assigned.next;
+	while (qh_ptr != &hsotg->periodic_sched_assigned) {
+		tx_status = readl(hsotg->regs + HPTXSTS);
+		if ((tx_status & TXSTS_QSPCAVAIL_MASK) == 0) {
+			no_queue_space = 1;
+			break;
+		}
+
+		qh = list_entry(qh_ptr, struct dwc2_qh, qh_list_entry);
+		if (!qh->channel) {
+			qh_ptr = qh_ptr->next;
+			continue;
+		}
+
+		/* Make sure EP's TT buffer is clean before queueing qtds */
+		if (qh->tt_buffer_dirty) {
+			qh_ptr = qh_ptr->next;
+			continue;
+		}
+
+		/*
+		 * Set a flag if we're queuing high-bandwidth in slave mode.
+		 * The flag prevents any halts to get into the request queue in
+		 * the middle of multiple high-bandwidth packets getting queued.
+		 */
+		if (hsotg->core_params->dma_enable <= 0 &&
+				qh->channel->multi_count > 1)
+			hsotg->queuing_high_bandwidth = 1;
+
+		fspcavail = tx_status >> TXSTS_FSPCAVAIL_SHIFT &
+			    TXSTS_FSPCAVAIL_MASK >> TXSTS_FSPCAVAIL_SHIFT;
+		status = dwc2_queue_transaction(hsotg, qh->channel, fspcavail);
+		if (status < 0) {
+			no_fifo_space = 1;
+			break;
+		}
+
+		/*
+		 * In Slave mode, stay on the current transfer until there is
+		 * nothing more to do or the high-bandwidth request count is
+		 * reached. In DMA mode, only need to queue one request. The
+		 * controller automatically handles multiple packets for
+		 * high-bandwidth transfers.
+		 */
+		if (hsotg->core_params->dma_enable > 0 || status == 0 ||
+		    qh->channel->requests == qh->channel->multi_count) {
+			qh_ptr = qh_ptr->next;
+			/*
+			 * Move the QH from the periodic assigned schedule to
+			 * the periodic queued schedule
+			 */
+			list_move(&qh->qh_list_entry,
+				  &hsotg->periodic_sched_queued);
+
+			/* done queuing high bandwidth */
+			hsotg->queuing_high_bandwidth = 0;
+		}
+	}
+
+	if (hsotg->core_params->dma_enable <= 0) {
+		tx_status = readl(hsotg->regs + HPTXSTS);
+		qspcavail = tx_status >> TXSTS_QSPCAVAIL_SHIFT &
+			    TXSTS_QSPCAVAIL_MASK >> TXSTS_QSPCAVAIL_SHIFT;
+		fspcavail = tx_status >> TXSTS_FSPCAVAIL_SHIFT &
+			    TXSTS_FSPCAVAIL_MASK >> TXSTS_FSPCAVAIL_SHIFT;
+		if (dbg_perio()) {
+			dev_vdbg(hsotg->dev,
+				 "  P Tx Req Queue Space Avail (after queue): %d\n",
+				 qspcavail);
+			dev_vdbg(hsotg->dev,
+				 "  P Tx FIFO Space Avail (after queue): %d\n",
+				 fspcavail);
+		}
+
+		if (!list_empty(&hsotg->periodic_sched_assigned) ||
+		    no_queue_space || no_fifo_space) {
+			/*
+			 * May need to queue more transactions as the request
+			 * queue or Tx FIFO empties. Enable the periodic Tx
+			 * FIFO empty interrupt. (Always use the half-empty
+			 * level to ensure that new requests are loaded as
+			 * soon as possible.)
+			 */
+			gintmsk = readl(hsotg->regs + GINTMSK);
+			gintmsk |= GINTSTS_PTXFEMP;
+			writel(gintmsk, hsotg->regs + GINTMSK);
+		} else {
+			/*
+			 * Disable the Tx FIFO empty interrupt since there are
+			 * no more transactions that need to be queued right
+			 * now. This function is called from interrupt
+			 * handlers to queue more transactions as transfer
+			 * states change.
+			 */
+			gintmsk = readl(hsotg->regs + GINTMSK);
+			gintmsk &= ~GINTSTS_PTXFEMP;
+			writel(gintmsk, hsotg->regs + GINTMSK);
+		}
+	}
+}
+
+/*
+ * Processes active non-periodic channels and queues transactions for these
+ * channels to the DWC_otg controller. After queueing transactions, the NP Tx
+ * FIFO Empty interrupt is enabled if there are more transactions to queue as
+ * NP Tx FIFO or request queue space becomes available. Otherwise, the NP Tx
+ * FIFO Empty interrupt is disabled.
+ *
+ * Must be called with interrupt disabled and spinlock held
+ */
+static void dwc2_process_non_periodic_channels(struct dwc2_hsotg *hsotg)
+{
+	struct list_head *orig_qh_ptr;
+	struct dwc2_qh *qh;
+	u32 tx_status;
+	u32 qspcavail;
+	u32 fspcavail;
+	u32 gintmsk;
+	int status;
+	int no_queue_space = 0;
+	int no_fifo_space = 0;
+	int more_to_do = 0;
+
+	dev_vdbg(hsotg->dev, "Queue non-periodic transactions\n");
+
+	tx_status = readl(hsotg->regs + GNPTXSTS);
+	qspcavail = tx_status >> TXSTS_QSPCAVAIL_SHIFT &
+		    TXSTS_QSPCAVAIL_MASK >> TXSTS_QSPCAVAIL_SHIFT;
+	fspcavail = tx_status >> TXSTS_FSPCAVAIL_SHIFT &
+		    TXSTS_FSPCAVAIL_MASK >> TXSTS_FSPCAVAIL_SHIFT;
+	dev_vdbg(hsotg->dev, "  NP Tx Req Queue Space Avail (before queue): %d\n",
+		 qspcavail);
+	dev_vdbg(hsotg->dev, "  NP Tx FIFO Space Avail (before queue): %d\n",
+		 fspcavail);
+
+	/*
+	 * Keep track of the starting point. Skip over the start-of-list
+	 * entry.
+	 */
+	if (hsotg->non_periodic_qh_ptr == &hsotg->non_periodic_sched_active)
+		hsotg->non_periodic_qh_ptr = hsotg->non_periodic_qh_ptr->next;
+	orig_qh_ptr = hsotg->non_periodic_qh_ptr;
+
+	/*
+	 * Process once through the active list or until no more space is
+	 * available in the request queue or the Tx FIFO
+	 */
+	do {
+		tx_status = readl(hsotg->regs + GNPTXSTS);
+		qspcavail = tx_status >> TXSTS_QSPCAVAIL_SHIFT &
+			    TXSTS_QSPCAVAIL_MASK >> TXSTS_QSPCAVAIL_SHIFT;
+		if (hsotg->core_params->dma_enable <= 0 && qspcavail == 0) {
+			no_queue_space = 1;
+			break;
+		}
+
+		qh = list_entry(hsotg->non_periodic_qh_ptr, struct dwc2_qh,
+				qh_list_entry);
+		if (!qh->channel)
+			goto next;
+
+		/* Make sure EP's TT buffer is clean before queueing qtds */
+		if (qh->tt_buffer_dirty)
+			goto next;
+
+		fspcavail = tx_status >> TXSTS_FSPCAVAIL_SHIFT &
+			    TXSTS_FSPCAVAIL_MASK >> TXSTS_FSPCAVAIL_SHIFT;
+		status = dwc2_queue_transaction(hsotg, qh->channel, fspcavail);
+
+		if (status > 0) {
+			more_to_do = 1;
+		} else if (status < 0) {
+			no_fifo_space = 1;
+			break;
+		}
+next:
+		/* Advance to next QH, skipping start-of-list entry */
+		hsotg->non_periodic_qh_ptr = hsotg->non_periodic_qh_ptr->next;
+		if (hsotg->non_periodic_qh_ptr ==
+				&hsotg->non_periodic_sched_active)
+			hsotg->non_periodic_qh_ptr =
+					hsotg->non_periodic_qh_ptr->next;
+	} while (hsotg->non_periodic_qh_ptr != orig_qh_ptr);
+
+	if (hsotg->core_params->dma_enable <= 0) {
+		tx_status = readl(hsotg->regs + GNPTXSTS);
+		qspcavail = tx_status >> TXSTS_QSPCAVAIL_SHIFT &
+			    TXSTS_QSPCAVAIL_MASK >> TXSTS_QSPCAVAIL_SHIFT;
+		fspcavail = tx_status >> TXSTS_FSPCAVAIL_SHIFT &
+			    TXSTS_FSPCAVAIL_MASK >> TXSTS_FSPCAVAIL_SHIFT;
+		dev_vdbg(hsotg->dev,
+			 "  NP Tx Req Queue Space Avail (after queue): %d\n",
+			 qspcavail);
+		dev_vdbg(hsotg->dev,
+			 "  NP Tx FIFO Space Avail (after queue): %d\n",
+			 fspcavail);
+
+		if (more_to_do || no_queue_space || no_fifo_space) {
+			/*
+			 * May need to queue more transactions as the request
+			 * queue or Tx FIFO empties. Enable the non-periodic
+			 * Tx FIFO empty interrupt. (Always use the half-empty
+			 * level to ensure that new requests are loaded as
+			 * soon as possible.)
+			 */
+			gintmsk = readl(hsotg->regs + GINTMSK);
+			gintmsk |= GINTSTS_NPTXFEMP;
+			writel(gintmsk, hsotg->regs + GINTMSK);
+		} else {
+			/*
+			 * Disable the Tx FIFO empty interrupt since there are
+			 * no more transactions that need to be queued right
+			 * now. This function is called from interrupt
+			 * handlers to queue more transactions as transfer
+			 * states change.
+			 */
+			gintmsk = readl(hsotg->regs + GINTMSK);
+			gintmsk &= ~GINTSTS_NPTXFEMP;
+			writel(gintmsk, hsotg->regs + GINTMSK);
+		}
+	}
+}
+
+/**
+ * dwc2_hcd_queue_transactions() - Processes the currently active host channels
+ * and queues transactions for these channels to the DWC_otg controller. Called
+ * from the HCD interrupt handler functions.
+ *
+ * @hsotg:   The HCD state structure
+ * @tr_type: The type(s) of transactions to queue (non-periodic, periodic,
+ *           or both)
+ *
+ * Must be called with interrupt disabled and spinlock held
+ */
+void dwc2_hcd_queue_transactions(struct dwc2_hsotg *hsotg,
+				 enum dwc2_transaction_type tr_type)
+{
+#ifdef DWC2_DEBUG_SOF
+	dev_vdbg(hsotg->dev, "Queue Transactions\n");
+#endif
+	/* Process host channels associated with periodic transfers */
+	if ((tr_type == DWC2_TRANSACTION_PERIODIC ||
+	     tr_type == DWC2_TRANSACTION_ALL) &&
+	    !list_empty(&hsotg->periodic_sched_assigned))
+		dwc2_process_periodic_channels(hsotg);
+
+	/* Process host channels associated with non-periodic transfers */
+	if (tr_type == DWC2_TRANSACTION_NON_PERIODIC ||
+	    tr_type == DWC2_TRANSACTION_ALL) {
+		if (!list_empty(&hsotg->non_periodic_sched_active)) {
+			dwc2_process_non_periodic_channels(hsotg);
+		} else {
+			/*
+			 * Ensure NP Tx FIFO empty interrupt is disabled when
+			 * there are no non-periodic transfers to process
+			 */
+			u32 gintmsk = readl(hsotg->regs + GINTMSK);
+
+			gintmsk &= ~GINTSTS_NPTXFEMP;
+			writel(gintmsk, hsotg->regs + GINTMSK);
+		}
+	}
+}
+
+static void dwc2_conn_id_status_change(struct work_struct *work)
+{
+	struct dwc2_hsotg *hsotg = container_of(work, struct dwc2_hsotg,
+						wf_otg);
+	u32 count = 0;
+	u32 gotgctl;
+
+	dev_dbg(hsotg->dev, "%s()\n", __func__);
+
+	gotgctl = readl(hsotg->regs + GOTGCTL);
+	dev_dbg(hsotg->dev, "gotgctl=%0x\n", gotgctl);
+	dev_dbg(hsotg->dev, "gotgctl.b.conidsts=%d\n",
+		!!(gotgctl & GOTGCTL_CONID_B));
+
+	/* B-Device connector (Device Mode) */
+	if (gotgctl & GOTGCTL_CONID_B) {
+		/* Wait for switch to device mode */
+		dev_dbg(hsotg->dev, "connId B\n");
+		while (!dwc2_is_device_mode(hsotg)) {
+			dev_info(hsotg->dev,
+				 "Waiting for Peripheral Mode, Mode=%s\n",
+				 dwc2_is_host_mode(hsotg) ? "Host" :
+				 "Peripheral");
+			usleep_range(20000, 40000);
+			if (++count > 250)
+				break;
+		}
+		if (count > 250)
+			dev_err(hsotg->dev,
+				"Connection id status change timed out\n");
+		hsotg->op_state = OTG_STATE_B_PERIPHERAL;
+		dwc2_core_init(hsotg, false, -1);
+		dwc2_enable_global_interrupts(hsotg);
+	} else {
+		/* A-Device connector (Host Mode) */
+		dev_dbg(hsotg->dev, "connId A\n");
+		while (!dwc2_is_host_mode(hsotg)) {
+			dev_info(hsotg->dev, "Waiting for Host Mode, Mode=%s\n",
+				 dwc2_is_host_mode(hsotg) ?
+				 "Host" : "Peripheral");
+			usleep_range(20000, 40000);
+			if (++count > 250)
+				break;
+		}
+		if (count > 250)
+			dev_err(hsotg->dev,
+				"Connection id status change timed out\n");
+		hsotg->op_state = OTG_STATE_A_HOST;
+
+		/* Initialize the Core for Host mode */
+		dwc2_core_init(hsotg, false, -1);
+		dwc2_enable_global_interrupts(hsotg);
+		dwc2_hcd_start(hsotg);
+	}
+}
+
+static void dwc2_wakeup_detected(unsigned long data)
+{
+	struct dwc2_hsotg *hsotg = (struct dwc2_hsotg *)data;
+	u32 hprt0;
+
+	dev_dbg(hsotg->dev, "%s()\n", __func__);
+
+	/*
+	 * Clear the Resume after 70ms. (Need 20 ms minimum. Use 70 ms
+	 * so that OPT tests pass with all PHYs.)
+	 */
+	hprt0 = dwc2_read_hprt0(hsotg);
+	dev_dbg(hsotg->dev, "Resume: HPRT0=%0x\n", hprt0);
+	hprt0 &= ~HPRT0_RES;
+	writel(hprt0, hsotg->regs + HPRT0);
+	dev_dbg(hsotg->dev, "Clear Resume: HPRT0=%0x\n",
+		readl(hsotg->regs + HPRT0));
+
+	dwc2_hcd_rem_wakeup(hsotg);
+
+	/* Change to L0 state */
+	hsotg->lx_state = DWC2_L0;
+}
+
+static int dwc2_host_is_b_hnp_enabled(struct dwc2_hsotg *hsotg)
+{
+	struct usb_hcd *hcd = dwc2_hsotg_to_hcd(hsotg);
+
+	return hcd->self.b_hnp_enable;
+}
+
+/* Must NOT be called with interrupt disabled or spinlock held */
+static void dwc2_port_suspend(struct dwc2_hsotg *hsotg, u16 windex)
+{
+	unsigned long flags;
+	u32 hprt0;
+	u32 pcgctl;
+	u32 gotgctl;
+
+	dev_dbg(hsotg->dev, "%s()\n", __func__);
+
+	spin_lock_irqsave(&hsotg->lock, flags);
+
+	if (windex == hsotg->otg_port && dwc2_host_is_b_hnp_enabled(hsotg)) {
+		gotgctl = readl(hsotg->regs + GOTGCTL);
+		gotgctl |= GOTGCTL_HSTSETHNPEN;
+		writel(gotgctl, hsotg->regs + GOTGCTL);
+		hsotg->op_state = OTG_STATE_A_SUSPEND;
+	}
+
+	hprt0 = dwc2_read_hprt0(hsotg);
+	hprt0 |= HPRT0_SUSP;
+	writel(hprt0, hsotg->regs + HPRT0);
+
+	/* Update lx_state */
+	hsotg->lx_state = DWC2_L2;
+
+	/* Suspend the Phy Clock */
+	pcgctl = readl(hsotg->regs + PCGCTL);
+	pcgctl |= PCGCTL_STOPPCLK;
+	writel(pcgctl, hsotg->regs + PCGCTL);
+	udelay(10);
+
+	/* For HNP the bus must be suspended for at least 200ms */
+	if (dwc2_host_is_b_hnp_enabled(hsotg)) {
+		pcgctl = readl(hsotg->regs + PCGCTL);
+		pcgctl &= ~PCGCTL_STOPPCLK;
+		writel(pcgctl, hsotg->regs + PCGCTL);
+
+		spin_unlock_irqrestore(&hsotg->lock, flags);
+
+		usleep_range(200000, 250000);
+	} else {
+		spin_unlock_irqrestore(&hsotg->lock, flags);
+	}
+}
+
+/* Handles hub class-specific requests */
+static int dwc2_hcd_hub_control(struct dwc2_hsotg *hsotg, u16 typereq,
+				u16 wvalue, u16 windex, char *buf, u16 wlength)
+{
+	struct usb_hub_descriptor *hub_desc;
+	int retval = 0;
+	u32 hprt0;
+	u32 port_status;
+	u32 speed;
+	u32 pcgctl;
+
+	switch (typereq) {
+	case ClearHubFeature:
+		dev_dbg(hsotg->dev, "ClearHubFeature %1xh\n", wvalue);
+
+		switch (wvalue) {
+		case C_HUB_LOCAL_POWER:
+		case C_HUB_OVER_CURRENT:
+			/* Nothing required here */
+			break;
+
+		default:
+			retval = -EINVAL;
+			dev_err(hsotg->dev,
+				"ClearHubFeature request %1xh unknown\n",
+				wvalue);
+		}
+		break;
+
+	case ClearPortFeature:
+		if (wvalue != USB_PORT_FEAT_L1)
+			if (!windex || windex > 1)
+				goto error;
+		switch (wvalue) {
+		case USB_PORT_FEAT_ENABLE:
+			dev_dbg(hsotg->dev,
+				"ClearPortFeature USB_PORT_FEAT_ENABLE\n");
+			hprt0 = dwc2_read_hprt0(hsotg);
+			hprt0 |= HPRT0_ENA;
+			writel(hprt0, hsotg->regs + HPRT0);
+			break;
+
+		case USB_PORT_FEAT_SUSPEND:
+			dev_dbg(hsotg->dev,
+				"ClearPortFeature USB_PORT_FEAT_SUSPEND\n");
+			writel(0, hsotg->regs + PCGCTL);
+			usleep_range(20000, 40000);
+
+			hprt0 = dwc2_read_hprt0(hsotg);
+			hprt0 |= HPRT0_RES;
+			writel(hprt0, hsotg->regs + HPRT0);
+			hprt0 &= ~HPRT0_SUSP;
+			usleep_range(100000, 150000);
+
+			hprt0 &= ~HPRT0_RES;
+			writel(hprt0, hsotg->regs + HPRT0);
+			break;
+
+		case USB_PORT_FEAT_POWER:
+			dev_dbg(hsotg->dev,
+				"ClearPortFeature USB_PORT_FEAT_POWER\n");
+			hprt0 = dwc2_read_hprt0(hsotg);
+			hprt0 &= ~HPRT0_PWR;
+			writel(hprt0, hsotg->regs + HPRT0);
+			break;
+
+		case USB_PORT_FEAT_INDICATOR:
+			dev_dbg(hsotg->dev,
+				"ClearPortFeature USB_PORT_FEAT_INDICATOR\n");
+			/* Port indicator not supported */
+			break;
+
+		case USB_PORT_FEAT_C_CONNECTION:
+			/*
+			 * Clears driver's internal Connect Status Change flag
+			 */
+			dev_dbg(hsotg->dev,
+				"ClearPortFeature USB_PORT_FEAT_C_CONNECTION\n");
+			hsotg->flags.b.port_connect_status_change = 0;
+			break;
+
+		case USB_PORT_FEAT_C_RESET:
+			/* Clears driver's internal Port Reset Change flag */
+			dev_dbg(hsotg->dev,
+				"ClearPortFeature USB_PORT_FEAT_C_RESET\n");
+			hsotg->flags.b.port_reset_change = 0;
+			break;
+
+		case USB_PORT_FEAT_C_ENABLE:
+			/*
+			 * Clears the driver's internal Port Enable/Disable
+			 * Change flag
+			 */
+			dev_dbg(hsotg->dev,
+				"ClearPortFeature USB_PORT_FEAT_C_ENABLE\n");
+			hsotg->flags.b.port_enable_change = 0;
+			break;
+
+		case USB_PORT_FEAT_C_SUSPEND:
+			/*
+			 * Clears the driver's internal Port Suspend Change
+			 * flag, which is set when resume signaling on the host
+			 * port is complete
+			 */
+			dev_dbg(hsotg->dev,
+				"ClearPortFeature USB_PORT_FEAT_C_SUSPEND\n");
+			hsotg->flags.b.port_suspend_change = 0;
+			break;
+
+		case USB_PORT_FEAT_C_PORT_L1:
+			dev_dbg(hsotg->dev,
+				"ClearPortFeature USB_PORT_FEAT_C_PORT_L1\n");
+			hsotg->flags.b.port_l1_change = 0;
+			break;
+
+		case USB_PORT_FEAT_C_OVER_CURRENT:
+			dev_dbg(hsotg->dev,
+				"ClearPortFeature USB_PORT_FEAT_C_OVER_CURRENT\n");
+			hsotg->flags.b.port_over_current_change = 0;
+			break;
+
+		default:
+			retval = -EINVAL;
+			dev_err(hsotg->dev,
+				"ClearPortFeature request %1xh unknown or unsupported\n",
+				wvalue);
+		}
+		break;
+
+	case GetHubDescriptor:
+		dev_dbg(hsotg->dev, "GetHubDescriptor\n");
+		hub_desc = (struct usb_hub_descriptor *)buf;
+		hub_desc->bDescLength = 9;
+		hub_desc->bDescriptorType = 0x29;
+		hub_desc->bNbrPorts = 1;
+		hub_desc->wHubCharacteristics = cpu_to_le16(0x08);
+		hub_desc->bPwrOn2PwrGood = 1;
+		hub_desc->bHubContrCurrent = 0;
+		hub_desc->u.hs.DeviceRemovable[0] = 0;
+		hub_desc->u.hs.DeviceRemovable[1] = 0xff;
+		break;
+
+	case GetHubStatus:
+		dev_dbg(hsotg->dev, "GetHubStatus\n");
+		memset(buf, 0, 4);
+		break;
+
+	case GetPortStatus:
+		dev_dbg(hsotg->dev,
+			"GetPortStatus wIndex=0x%04x flags=0x%08x\n", windex,
+			hsotg->flags.d32);
+		if (!windex || windex > 1)
+			goto error;
+
+		port_status = 0;
+		if (hsotg->flags.b.port_connect_status_change)
+			port_status |= USB_PORT_STAT_C_CONNECTION << 16;
+		if (hsotg->flags.b.port_enable_change)
+			port_status |= USB_PORT_STAT_C_ENABLE << 16;
+		if (hsotg->flags.b.port_suspend_change)
+			port_status |= USB_PORT_STAT_C_SUSPEND << 16;
+		if (hsotg->flags.b.port_l1_change)
+			port_status |= USB_PORT_STAT_C_L1 << 16;
+		if (hsotg->flags.b.port_reset_change)
+			port_status |= USB_PORT_STAT_C_RESET << 16;
+		if (hsotg->flags.b.port_over_current_change) {
+			dev_warn(hsotg->dev, "Overcurrent change detected\n");
+			port_status |= USB_PORT_STAT_C_OVERCURRENT << 16;
+		}
+
+		if (!hsotg->flags.b.port_connect_status) {
+			/*
+			 * The port is disconnected, which means the core is
+			 * either in device mode or it soon will be. Just
+			 * return 0's for the remainder of the port status
+			 * since the port register can't be read if the core
+			 * is in device mode.
+			 */
+			*(__le32 *)buf = cpu_to_le32(port_status);
+			break;
+		}
+
+		hprt0 = readl(hsotg->regs + HPRT0);
+		dev_dbg(hsotg->dev, "  HPRT0: 0x%08x\n", hprt0);
+
+		if (hprt0 & HPRT0_CONNSTS)
+			port_status |= USB_PORT_STAT_CONNECTION;
+		if (hprt0 & HPRT0_ENA)
+			port_status |= USB_PORT_STAT_ENABLE;
+		if (hprt0 & HPRT0_SUSP)
+			port_status |= USB_PORT_STAT_SUSPEND;
+		if (hprt0 & HPRT0_OVRCURRACT)
+			port_status |= USB_PORT_STAT_OVERCURRENT;
+		if (hprt0 & HPRT0_RST)
+			port_status |= USB_PORT_STAT_RESET;
+		if (hprt0 & HPRT0_PWR)
+			port_status |= USB_PORT_STAT_POWER;
+
+		speed = hprt0 & HPRT0_SPD_MASK;
+		if (speed == HPRT0_SPD_HIGH_SPEED)
+			port_status |= USB_PORT_STAT_HIGH_SPEED;
+		else if (speed == HPRT0_SPD_LOW_SPEED)
+			port_status |= USB_PORT_STAT_LOW_SPEED;
+
+		if (hprt0 & HPRT0_TSTCTL_MASK)
+			port_status |= USB_PORT_STAT_TEST;
+		/* USB_PORT_FEAT_INDICATOR unsupported always 0 */
+
+		dev_dbg(hsotg->dev, "port_status=%08x\n", port_status);
+		*(__le32 *)buf = cpu_to_le32(port_status);
+		break;
+
+	case SetHubFeature:
+		dev_dbg(hsotg->dev, "SetHubFeature\n");
+		/* No HUB features supported */
+		break;
+
+	case SetPortFeature:
+		dev_dbg(hsotg->dev, "SetPortFeature\n");
+		if (wvalue != USB_PORT_FEAT_TEST && (!windex || windex > 1))
+			goto error;
+
+		if (!hsotg->flags.b.port_connect_status) {
+			/*
+			 * The port is disconnected, which means the core is
+			 * either in device mode or it soon will be. Just
+			 * return without doing anything since the port
+			 * register can't be written if the core is in device
+			 * mode.
+			 */
+			break;
+		}
+
+		switch (wvalue) {
+		case USB_PORT_FEAT_SUSPEND:
+			dev_dbg(hsotg->dev,
+				"SetPortFeature - USB_PORT_FEAT_SUSPEND\n");
+			if (windex != hsotg->otg_port)
+				goto error;
+			dwc2_port_suspend(hsotg, windex);
+			break;
+
+		case USB_PORT_FEAT_POWER:
+			dev_dbg(hsotg->dev,
+				"SetPortFeature - USB_PORT_FEAT_POWER\n");
+			hprt0 = dwc2_read_hprt0(hsotg);
+			hprt0 |= HPRT0_PWR;
+			writel(hprt0, hsotg->regs + HPRT0);
+			break;
+
+		case USB_PORT_FEAT_RESET:
+			hprt0 = dwc2_read_hprt0(hsotg);
+			dev_dbg(hsotg->dev,
+				"SetPortFeature - USB_PORT_FEAT_RESET\n");
+			pcgctl = readl(hsotg->regs + PCGCTL);
+			pcgctl &= ~(PCGCTL_ENBL_SLEEP_GATING | PCGCTL_STOPPCLK);
+			writel(pcgctl, hsotg->regs + PCGCTL);
+			/* ??? Original driver does this */
+			writel(0, hsotg->regs + PCGCTL);
+
+			hprt0 = dwc2_read_hprt0(hsotg);
+			/* Clear suspend bit if resetting from suspend state */
+			hprt0 &= ~HPRT0_SUSP;
+
+			/*
+			 * When B-Host the Port reset bit is set in the Start
+			 * HCD Callback function, so that the reset is started
+			 * within 1ms of the HNP success interrupt
+			 */
+			if (!dwc2_hcd_is_b_host(hsotg)) {
+				hprt0 |= HPRT0_PWR | HPRT0_RST;
+				dev_dbg(hsotg->dev,
+					"In host mode, hprt0=%08x\n", hprt0);
+				writel(hprt0, hsotg->regs + HPRT0);
+			}
+
+			/* Clear reset bit in 10ms (FS/LS) or 50ms (HS) */
+			usleep_range(50000, 70000);
+			hprt0 &= ~HPRT0_RST;
+			writel(hprt0, hsotg->regs + HPRT0);
+			hsotg->lx_state = DWC2_L0; /* Now back to On state */
+			break;
+
+		case USB_PORT_FEAT_INDICATOR:
+			dev_dbg(hsotg->dev,
+				"SetPortFeature - USB_PORT_FEAT_INDICATOR\n");
+			/* Not supported */
+			break;
+
+		default:
+			retval = -EINVAL;
+			dev_err(hsotg->dev,
+				"SetPortFeature %1xh unknown or unsupported\n",
+				wvalue);
+			break;
+		}
+		break;
+
+	default:
+error:
+		retval = -EINVAL;
+		dev_dbg(hsotg->dev,
+			"Unknown hub control request: %1xh wIndex: %1xh wValue: %1xh\n",
+			typereq, windex, wvalue);
+		break;
+	}
+
+	return retval;
+}
+
+static int dwc2_hcd_is_status_changed(struct dwc2_hsotg *hsotg, int port)
+{
+	int retval;
+
+	if (port != 1)
+		return -EINVAL;
+
+	retval = (hsotg->flags.b.port_connect_status_change ||
+		  hsotg->flags.b.port_reset_change ||
+		  hsotg->flags.b.port_enable_change ||
+		  hsotg->flags.b.port_suspend_change ||
+		  hsotg->flags.b.port_over_current_change);
+
+	if (retval) {
+		dev_dbg(hsotg->dev,
+			"DWC OTG HCD HUB STATUS DATA: Root port status changed\n");
+		dev_dbg(hsotg->dev, "  port_connect_status_change: %d\n",
+			hsotg->flags.b.port_connect_status_change);
+		dev_dbg(hsotg->dev, "  port_reset_change: %d\n",
+			hsotg->flags.b.port_reset_change);
+		dev_dbg(hsotg->dev, "  port_enable_change: %d\n",
+			hsotg->flags.b.port_enable_change);
+		dev_dbg(hsotg->dev, "  port_suspend_change: %d\n",
+			hsotg->flags.b.port_suspend_change);
+		dev_dbg(hsotg->dev, "  port_over_current_change: %d\n",
+			hsotg->flags.b.port_over_current_change);
+	}
+
+	return retval;
+}
+
+int dwc2_hcd_get_frame_number(struct dwc2_hsotg *hsotg)
+{
+	u32 hfnum = readl(hsotg->regs + HFNUM);
+
+#ifdef DWC2_DEBUG_SOF
+	dev_vdbg(hsotg->dev, "DWC OTG HCD GET FRAME NUMBER %d\n",
+		 hfnum >> HFNUM_FRNUM_SHIFT &
+		 HFNUM_FRNUM_MASK >> HFNUM_FRNUM_SHIFT);
+#endif
+	return hfnum >> HFNUM_FRNUM_SHIFT &
+	       HFNUM_FRNUM_MASK >> HFNUM_FRNUM_SHIFT;
+}
+
+int dwc2_hcd_is_b_host(struct dwc2_hsotg *hsotg)
+{
+	return (hsotg->op_state == OTG_STATE_B_HOST);
+}
+
+static struct dwc2_hcd_urb *dwc2_hcd_urb_alloc(struct dwc2_hsotg *hsotg,
+					       int iso_desc_count,
+					       gfp_t mem_flags)
+{
+	struct dwc2_hcd_urb *urb;
+	u32 size = sizeof(*urb) + iso_desc_count *
+		   sizeof(struct dwc2_hcd_iso_packet_desc);
+
+	urb = kzalloc(size, mem_flags);
+	if (urb)
+		urb->packet_count = iso_desc_count;
+	return urb;
+}
+
+static void dwc2_hcd_urb_set_pipeinfo(struct dwc2_hsotg *hsotg,
+				      struct dwc2_hcd_urb *urb, u8 dev_addr,
+				      u8 ep_num, u8 ep_type, u8 ep_dir, u16 mps)
+{
+	if (dbg_perio() ||
+	    ep_type == USB_ENDPOINT_XFER_BULK ||
+	    ep_type == USB_ENDPOINT_XFER_CONTROL)
+		dev_vdbg(hsotg->dev,
+			 "addr=%d, ep_num=%d, ep_dir=%1x, ep_type=%1x, mps=%d\n",
+			 dev_addr, ep_num, ep_dir, ep_type, mps);
+	urb->pipe_info.dev_addr = dev_addr;
+	urb->pipe_info.ep_num = ep_num;
+	urb->pipe_info.pipe_type = ep_type;
+	urb->pipe_info.pipe_dir = ep_dir;
+	urb->pipe_info.mps = mps;
+}
+
+/*
+ * NOTE: This function will be removed once the peripheral controller code
+ * is integrated and the driver is stable
+ */
+void dwc2_hcd_dump_state(struct dwc2_hsotg *hsotg)
+{
+#ifdef DEBUG
+	struct dwc2_host_chan *chan;
+	struct dwc2_hcd_urb *urb;
+	struct dwc2_qtd *qtd;
+	int num_channels;
+	u32 np_tx_status;
+	u32 p_tx_status;
+	int i;
+
+	num_channels = hsotg->core_params->host_channels;
+	dev_dbg(hsotg->dev, "\n");
+	dev_dbg(hsotg->dev,
+		"************************************************************\n");
+	dev_dbg(hsotg->dev, "HCD State:\n");
+	dev_dbg(hsotg->dev, "  Num channels: %d\n", num_channels);
+
+	for (i = 0; i < num_channels; i++) {
+		chan = hsotg->hc_ptr_array[i];
+		dev_dbg(hsotg->dev, "  Channel %d:\n", i);
+		dev_dbg(hsotg->dev,
+			"    dev_addr: %d, ep_num: %d, ep_is_in: %d\n",
+			chan->dev_addr, chan->ep_num, chan->ep_is_in);
+		dev_dbg(hsotg->dev, "    speed: %d\n", chan->speed);
+		dev_dbg(hsotg->dev, "    ep_type: %d\n", chan->ep_type);
+		dev_dbg(hsotg->dev, "    max_packet: %d\n", chan->max_packet);
+		dev_dbg(hsotg->dev, "    data_pid_start: %d\n",
+			chan->data_pid_start);
+		dev_dbg(hsotg->dev, "    multi_count: %d\n", chan->multi_count);
+		dev_dbg(hsotg->dev, "    xfer_started: %d\n",
+			chan->xfer_started);
+		dev_dbg(hsotg->dev, "    xfer_buf: %p\n", chan->xfer_buf);
+		dev_dbg(hsotg->dev, "    xfer_dma: %08lx\n",
+			(unsigned long)chan->xfer_dma);
+		dev_dbg(hsotg->dev, "    xfer_len: %d\n", chan->xfer_len);
+		dev_dbg(hsotg->dev, "    xfer_count: %d\n", chan->xfer_count);
+		dev_dbg(hsotg->dev, "    halt_on_queue: %d\n",
+			chan->halt_on_queue);
+		dev_dbg(hsotg->dev, "    halt_pending: %d\n",
+			chan->halt_pending);
+		dev_dbg(hsotg->dev, "    halt_status: %d\n", chan->halt_status);
+		dev_dbg(hsotg->dev, "    do_split: %d\n", chan->do_split);
+		dev_dbg(hsotg->dev, "    complete_split: %d\n",
+			chan->complete_split);
+		dev_dbg(hsotg->dev, "    hub_addr: %d\n", chan->hub_addr);
+		dev_dbg(hsotg->dev, "    hub_port: %d\n", chan->hub_port);
+		dev_dbg(hsotg->dev, "    xact_pos: %d\n", chan->xact_pos);
+		dev_dbg(hsotg->dev, "    requests: %d\n", chan->requests);
+		dev_dbg(hsotg->dev, "    qh: %p\n", chan->qh);
+
+		if (chan->xfer_started) {
+			u32 hfnum, hcchar, hctsiz, hcint, hcintmsk;
+
+			hfnum = readl(hsotg->regs + HFNUM);
+			hcchar = readl(hsotg->regs + HCCHAR(i));
+			hctsiz = readl(hsotg->regs + HCTSIZ(i));
+			hcint = readl(hsotg->regs + HCINT(i));
+			hcintmsk = readl(hsotg->regs + HCINTMSK(i));
+			dev_dbg(hsotg->dev, "    hfnum: 0x%08x\n", hfnum);
+			dev_dbg(hsotg->dev, "    hcchar: 0x%08x\n", hcchar);
+			dev_dbg(hsotg->dev, "    hctsiz: 0x%08x\n", hctsiz);
+			dev_dbg(hsotg->dev, "    hcint: 0x%08x\n", hcint);
+			dev_dbg(hsotg->dev, "    hcintmsk: 0x%08x\n", hcintmsk);
+		}
+
+		if (!(chan->xfer_started && chan->qh))
+			continue;
+
+		list_for_each_entry(qtd, &chan->qh->qtd_list, qtd_list_entry) {
+			if (!qtd->in_process)
+				break;
+			urb = qtd->urb;
+			dev_dbg(hsotg->dev, "    URB Info:\n");
+			dev_dbg(hsotg->dev, "      qtd: %p, urb: %p\n",
+				qtd, urb);
+			if (urb) {
+				dev_dbg(hsotg->dev,
+					"      Dev: %d, EP: %d %s\n",
+					dwc2_hcd_get_dev_addr(&urb->pipe_info),
+					dwc2_hcd_get_ep_num(&urb->pipe_info),
+					dwc2_hcd_is_pipe_in(&urb->pipe_info) ?
+					"IN" : "OUT");
+				dev_dbg(hsotg->dev,
+					"      Max packet size: %d\n",
+					dwc2_hcd_get_mps(&urb->pipe_info));
+				dev_dbg(hsotg->dev,
+					"      transfer_buffer: %p\n",
+					urb->buf);
+				dev_dbg(hsotg->dev,
+					"      transfer_dma: %08lx\n",
+					(unsigned long)urb->dma);
+				dev_dbg(hsotg->dev,
+					"      transfer_buffer_length: %d\n",
+					urb->length);
+				dev_dbg(hsotg->dev, "      actual_length: %d\n",
+					urb->actual_length);
+			}
+		}
+	}
+
+	dev_dbg(hsotg->dev, "  non_periodic_channels: %d\n",
+		hsotg->non_periodic_channels);
+	dev_dbg(hsotg->dev, "  periodic_channels: %d\n",
+		hsotg->periodic_channels);
+	dev_dbg(hsotg->dev, "  periodic_usecs: %d\n", hsotg->periodic_usecs);
+	np_tx_status = readl(hsotg->regs + GNPTXSTS);
+	dev_dbg(hsotg->dev, "  NP Tx Req Queue Space Avail: %d\n",
+		np_tx_status >> TXSTS_QSPCAVAIL_SHIFT &
+		TXSTS_QSPCAVAIL_MASK >> TXSTS_QSPCAVAIL_SHIFT);
+	dev_dbg(hsotg->dev, "  NP Tx FIFO Space Avail: %d\n",
+		np_tx_status >> TXSTS_FSPCAVAIL_SHIFT &
+		TXSTS_FSPCAVAIL_MASK >> TXSTS_FSPCAVAIL_SHIFT);
+	p_tx_status = readl(hsotg->regs + HPTXSTS);
+	dev_dbg(hsotg->dev, "  P Tx Req Queue Space Avail: %d\n",
+		p_tx_status >> TXSTS_QSPCAVAIL_SHIFT &
+		TXSTS_QSPCAVAIL_MASK >> TXSTS_QSPCAVAIL_SHIFT);
+	dev_dbg(hsotg->dev, "  P Tx FIFO Space Avail: %d\n",
+		p_tx_status >> TXSTS_FSPCAVAIL_SHIFT &
+		TXSTS_FSPCAVAIL_MASK >> TXSTS_FSPCAVAIL_SHIFT);
+	dwc2_hcd_dump_frrem(hsotg);
+	dwc2_dump_global_registers(hsotg);
+	dwc2_dump_host_registers(hsotg);
+	dev_dbg(hsotg->dev,
+		"************************************************************\n");
+	dev_dbg(hsotg->dev, "\n");
+#endif
+}
+
+/*
+ * NOTE: This function will be removed once the peripheral controller code
+ * is integrated and the driver is stable
+ */
+void dwc2_hcd_dump_frrem(struct dwc2_hsotg *hsotg)
+{
+#ifdef DWC2_DUMP_FRREM
+	dev_dbg(hsotg->dev, "Frame remaining at SOF:\n");
+	dev_dbg(hsotg->dev, "  samples %u, accum %llu, avg %llu\n",
+		hsotg->frrem_samples, hsotg->frrem_accum,
+		hsotg->frrem_samples > 0 ?
+		hsotg->frrem_accum / hsotg->frrem_samples : 0);
+	dev_dbg(hsotg->dev, "\n");
+	dev_dbg(hsotg->dev, "Frame remaining at start_transfer (uframe 7):\n");
+	dev_dbg(hsotg->dev, "  samples %u, accum %llu, avg %llu\n",
+		hsotg->hfnum_7_samples,
+		hsotg->hfnum_7_frrem_accum,
+		hsotg->hfnum_7_samples > 0 ?
+		hsotg->hfnum_7_frrem_accum / hsotg->hfnum_7_samples : 0);
+	dev_dbg(hsotg->dev, "Frame remaining at start_transfer (uframe 0):\n");
+	dev_dbg(hsotg->dev, "  samples %u, accum %llu, avg %llu\n",
+		hsotg->hfnum_0_samples,
+		hsotg->hfnum_0_frrem_accum,
+		hsotg->hfnum_0_samples > 0 ?
+		hsotg->hfnum_0_frrem_accum / hsotg->hfnum_0_samples : 0);
+	dev_dbg(hsotg->dev, "Frame remaining at start_transfer (uframe 1-6):\n");
+	dev_dbg(hsotg->dev, "  samples %u, accum %llu, avg %llu\n",
+		hsotg->hfnum_other_samples,
+		hsotg->hfnum_other_frrem_accum,
+		hsotg->hfnum_other_samples > 0 ?
+		hsotg->hfnum_other_frrem_accum / hsotg->hfnum_other_samples :
+		0);
+	dev_dbg(hsotg->dev, "\n");
+	dev_dbg(hsotg->dev, "Frame remaining at sample point A (uframe 7):\n");
+	dev_dbg(hsotg->dev, "  samples %u, accum %llu, avg %llu\n",
+		hsotg->hfnum_7_samples_a, hsotg->hfnum_7_frrem_accum_a,
+		hsotg->hfnum_7_samples_a > 0 ?
+		hsotg->hfnum_7_frrem_accum_a / hsotg->hfnum_7_samples_a : 0);
+	dev_dbg(hsotg->dev, "Frame remaining at sample point A (uframe 0):\n");
+	dev_dbg(hsotg->dev, "  samples %u, accum %llu, avg %llu\n",
+		hsotg->hfnum_0_samples_a, hsotg->hfnum_0_frrem_accum_a,
+		hsotg->hfnum_0_samples_a > 0 ?
+		hsotg->hfnum_0_frrem_accum_a / hsotg->hfnum_0_samples_a : 0);
+	dev_dbg(hsotg->dev, "Frame remaining at sample point A (uframe 1-6):\n");
+	dev_dbg(hsotg->dev, "  samples %u, accum %llu, avg %llu\n",
+		hsotg->hfnum_other_samples_a, hsotg->hfnum_other_frrem_accum_a,
+		hsotg->hfnum_other_samples_a > 0 ?
+		hsotg->hfnum_other_frrem_accum_a / hsotg->hfnum_other_samples_a
+		: 0);
+	dev_dbg(hsotg->dev, "\n");
+	dev_dbg(hsotg->dev, "Frame remaining at sample point B (uframe 7):\n");
+	dev_dbg(hsotg->dev, "  samples %u, accum %llu, avg %llu\n",
+		hsotg->hfnum_7_samples_b, hsotg->hfnum_7_frrem_accum_b,
+		hsotg->hfnum_7_samples_b > 0 ?
+		hsotg->hfnum_7_frrem_accum_b / hsotg->hfnum_7_samples_b : 0);
+	dev_dbg(hsotg->dev, "Frame remaining at sample point B (uframe 0):\n");
+	dev_dbg(hsotg->dev, "  samples %u, accum %llu, avg %llu\n",
+		hsotg->hfnum_0_samples_b, hsotg->hfnum_0_frrem_accum_b,
+		(hsotg->hfnum_0_samples_b > 0) ?
+		hsotg->hfnum_0_frrem_accum_b / hsotg->hfnum_0_samples_b : 0);
+	dev_dbg(hsotg->dev, "Frame remaining at sample point B (uframe 1-6):\n");
+	dev_dbg(hsotg->dev, "  samples %u, accum %llu, avg %llu\n",
+		hsotg->hfnum_other_samples_b, hsotg->hfnum_other_frrem_accum_b,
+		(hsotg->hfnum_other_samples_b > 0) ?
+		hsotg->hfnum_other_frrem_accum_b / hsotg->hfnum_other_samples_b
+		: 0);
+#endif
+}
+
+struct wrapper_priv_data {
+	struct dwc2_hsotg *hsotg;
+};
+
+/* Gets the dwc2_hsotg from a usb_hcd */
+static struct dwc2_hsotg *dwc2_hcd_to_hsotg(struct usb_hcd *hcd)
+{
+	struct wrapper_priv_data *p;
+
+	p = (struct wrapper_priv_data *) &hcd->hcd_priv;
+	return p->hsotg;
+}
+
+static int _dwc2_hcd_start(struct usb_hcd *hcd);
+
+void dwc2_host_start(struct dwc2_hsotg *hsotg)
+{
+	struct usb_hcd *hcd = dwc2_hsotg_to_hcd(hsotg);
+
+	hcd->self.is_b_host = dwc2_hcd_is_b_host(hsotg);
+	_dwc2_hcd_start(hcd);
+}
+
+void dwc2_host_disconnect(struct dwc2_hsotg *hsotg)
+{
+	struct usb_hcd *hcd = dwc2_hsotg_to_hcd(hsotg);
+
+	hcd->self.is_b_host = 0;
+}
+
+void dwc2_host_hub_info(struct dwc2_hsotg *hsotg, void *context, int *hub_addr,
+			int *hub_port)
+{
+	struct urb *urb = context;
+
+	if (urb->dev->tt)
+		*hub_addr = urb->dev->tt->hub->devnum;
+	else
+		*hub_addr = 0;
+	*hub_port = urb->dev->ttport;
+}
+
+int dwc2_host_get_speed(struct dwc2_hsotg *hsotg, void *context)
+{
+	struct urb *urb = context;
+
+	return urb->dev->speed;
+}
+
+static void dwc2_allocate_bus_bandwidth(struct usb_hcd *hcd, u16 bw,
+					struct urb *urb)
+{
+	struct usb_bus *bus = hcd_to_bus(hcd);
+
+	if (urb->interval)
+		bus->bandwidth_allocated += bw / urb->interval;
+	if (usb_pipetype(urb->pipe) == PIPE_ISOCHRONOUS)
+		bus->bandwidth_isoc_reqs++;
+	else
+		bus->bandwidth_int_reqs++;
+}
+
+static void dwc2_free_bus_bandwidth(struct usb_hcd *hcd, u16 bw,
+				    struct urb *urb)
+{
+	struct usb_bus *bus = hcd_to_bus(hcd);
+
+	if (urb->interval)
+		bus->bandwidth_allocated -= bw / urb->interval;
+	if (usb_pipetype(urb->pipe) == PIPE_ISOCHRONOUS)
+		bus->bandwidth_isoc_reqs--;
+	else
+		bus->bandwidth_int_reqs--;
+}
+
+/*
+ * Sets the final status of an URB and returns it to the upper layer. Any
+ * required cleanup of the URB is performed.
+ *
+ * Must be called with interrupt disabled and spinlock held
+ */
+void dwc2_host_complete(struct dwc2_hsotg *hsotg, void *context,
+			struct dwc2_hcd_urb *dwc2_urb, int status)
+{
+	struct urb *urb = context;
+	int i;
+
+	if (!urb) {
+		dev_dbg(hsotg->dev, "## %s: context is NULL ##\n", __func__);
+		return;
+	}
+
+	if (!dwc2_urb) {
+		dev_dbg(hsotg->dev, "## %s: dwc2_urb is NULL ##\n", __func__);
+		return;
+	}
+
+	urb->actual_length = dwc2_hcd_urb_get_actual_length(dwc2_urb);
+
+	if (dbg_urb(urb))
+		dev_vdbg(hsotg->dev,
+			 "%s: urb %p device %d ep %d-%s status %d actual %d\n",
+			 __func__, urb, usb_pipedevice(urb->pipe),
+			 usb_pipeendpoint(urb->pipe),
+			 usb_pipein(urb->pipe) ? "IN" : "OUT", status,
+			 urb->actual_length);
+
+	if (usb_pipetype(urb->pipe) == PIPE_ISOCHRONOUS && dbg_perio()) {
+		for (i = 0; i < urb->number_of_packets; i++)
+			dev_vdbg(hsotg->dev, " ISO Desc %d status %d\n",
+				 i, urb->iso_frame_desc[i].status);
+	}
+
+	if (usb_pipetype(urb->pipe) == PIPE_ISOCHRONOUS) {
+		urb->error_count = dwc2_hcd_urb_get_error_count(dwc2_urb);
+		for (i = 0; i < urb->number_of_packets; ++i) {
+			urb->iso_frame_desc[i].actual_length =
+				dwc2_hcd_urb_get_iso_desc_actual_length(
+						dwc2_urb, i);
+			urb->iso_frame_desc[i].status =
+				dwc2_hcd_urb_get_iso_desc_status(dwc2_urb, i);
+		}
+	}
+
+	urb->status = status;
+	urb->hcpriv = NULL;
+	if (!status) {
+		if ((urb->transfer_flags & URB_SHORT_NOT_OK) &&
+		    urb->actual_length < urb->transfer_buffer_length)
+			urb->status = -EREMOTEIO;
+	}
+
+	if (usb_pipetype(urb->pipe) == PIPE_ISOCHRONOUS ||
+	    usb_pipetype(urb->pipe) == PIPE_INTERRUPT) {
+		struct usb_host_endpoint *ep = urb->ep;
+
+		if (ep)
+			dwc2_free_bus_bandwidth(dwc2_hsotg_to_hcd(hsotg),
+					dwc2_hcd_get_ep_bandwidth(hsotg, ep),
+					urb);
+	}
+
+	kfree(dwc2_urb);
+
+	spin_unlock(&hsotg->lock);
+	usb_hcd_giveback_urb(dwc2_hsotg_to_hcd(hsotg), urb, status);
+	spin_lock(&hsotg->lock);
+}
+
+/*
+ * Work queue function for starting the HCD when A-Cable is connected
+ */
+static void dwc2_hcd_start_func(struct work_struct *work)
+{
+	struct dwc2_hsotg *hsotg = container_of(work, struct dwc2_hsotg,
+						start_work.work);
+
+	dev_dbg(hsotg->dev, "%s() %p\n", __func__, hsotg);
+	dwc2_host_start(hsotg);
+}
+
+/*
+ * Reset work queue function
+ */
+static void dwc2_hcd_reset_func(struct work_struct *work)
+{
+	struct dwc2_hsotg *hsotg = container_of(work, struct dwc2_hsotg,
+						reset_work.work);
+	u32 hprt0;
+
+	dev_dbg(hsotg->dev, "USB RESET function called\n");
+	hprt0 = dwc2_read_hprt0(hsotg);
+	hprt0 &= ~HPRT0_RST;
+	writel(hprt0, hsotg->regs + HPRT0);
+	hsotg->flags.b.port_reset_change = 1;
+}
+
+/*
+ * =========================================================================
+ *  Linux HC Driver Functions
+ * =========================================================================
+ */
+
+/*
+ * Initializes the DWC_otg controller and its root hub and prepares it for host
+ * mode operation. Activates the root port. Returns 0 on success and a negative
+ * error code on failure.
+ */
+static int _dwc2_hcd_start(struct usb_hcd *hcd)
+{
+	struct dwc2_hsotg *hsotg = dwc2_hcd_to_hsotg(hcd);
+	struct usb_bus *bus = hcd_to_bus(hcd);
+	unsigned long flags;
+
+	dev_dbg(hsotg->dev, "DWC OTG HCD START\n");
+
+	spin_lock_irqsave(&hsotg->lock, flags);
+
+	hcd->state = HC_STATE_RUNNING;
+
+	if (dwc2_is_device_mode(hsotg)) {
+		spin_unlock_irqrestore(&hsotg->lock, flags);
+		return 0;	/* why 0 ?? */
+	}
+
+	dwc2_hcd_reinit(hsotg);
+
+	/* Initialize and connect root hub if one is not already attached */
+	if (bus->root_hub) {
+		dev_dbg(hsotg->dev, "DWC OTG HCD Has Root Hub\n");
+		/* Inform the HUB driver to resume */
+		usb_hcd_resume_root_hub(hcd);
+	}
+
+	spin_unlock_irqrestore(&hsotg->lock, flags);
+	return 0;
+}
+
+/*
+ * Halts the DWC_otg host mode operations in a clean manner. USB transfers are
+ * stopped.
+ */
+static void _dwc2_hcd_stop(struct usb_hcd *hcd)
+{
+	struct dwc2_hsotg *hsotg = dwc2_hcd_to_hsotg(hcd);
+	unsigned long flags;
+
+	spin_lock_irqsave(&hsotg->lock, flags);
+	dwc2_hcd_stop(hsotg);
+	spin_unlock_irqrestore(&hsotg->lock, flags);
+
+	usleep_range(1000, 3000);
+}
+
+/* Returns the current frame number */
+static int _dwc2_hcd_get_frame_number(struct usb_hcd *hcd)
+{
+	struct dwc2_hsotg *hsotg = dwc2_hcd_to_hsotg(hcd);
+
+	return dwc2_hcd_get_frame_number(hsotg);
+}
+
+static void dwc2_dump_urb_info(struct usb_hcd *hcd, struct urb *urb,
+			       char *fn_name)
+{
+#ifdef VERBOSE_DEBUG
+	struct dwc2_hsotg *hsotg = dwc2_hcd_to_hsotg(hcd);
+	char *pipetype;
+	char *speed;
+
+	dev_vdbg(hsotg->dev, "%s, urb %p\n", fn_name, urb);
+	dev_vdbg(hsotg->dev, "  Device address: %d\n",
+		 usb_pipedevice(urb->pipe));
+	dev_vdbg(hsotg->dev, "  Endpoint: %d, %s\n",
+		 usb_pipeendpoint(urb->pipe),
+		 usb_pipein(urb->pipe) ? "IN" : "OUT");
+
+	switch (usb_pipetype(urb->pipe)) {
+	case PIPE_CONTROL:
+		pipetype = "CONTROL";
+		break;
+	case PIPE_BULK:
+		pipetype = "BULK";
+		break;
+	case PIPE_INTERRUPT:
+		pipetype = "INTERRUPT";
+		break;
+	case PIPE_ISOCHRONOUS:
+		pipetype = "ISOCHRONOUS";
+		break;
+	default:
+		pipetype = "UNKNOWN";
+		break;
+	}
+
+	dev_vdbg(hsotg->dev, "  Endpoint type: %s %s (%s)\n", pipetype,
+		 usb_urb_dir_in(urb) ? "IN" : "OUT", usb_pipein(urb->pipe) ?
+		 "IN" : "OUT");
+
+	switch (urb->dev->speed) {
+	case USB_SPEED_HIGH:
+		speed = "HIGH";
+		break;
+	case USB_SPEED_FULL:
+		speed = "FULL";
+		break;
+	case USB_SPEED_LOW:
+		speed = "LOW";
+		break;
+	default:
+		speed = "UNKNOWN";
+		break;
+	}
+
+	dev_vdbg(hsotg->dev, "  Speed: %s\n", speed);
+	dev_vdbg(hsotg->dev, "  Max packet size: %d\n",
+		 usb_maxpacket(urb->dev, urb->pipe, usb_pipeout(urb->pipe)));
+	dev_vdbg(hsotg->dev, "  Data buffer length: %d\n",
+		 urb->transfer_buffer_length);
+	dev_vdbg(hsotg->dev, "  Transfer buffer: %p, Transfer DMA: %08lx\n",
+		 urb->transfer_buffer, (unsigned long)urb->transfer_dma);
+	dev_vdbg(hsotg->dev, "  Setup buffer: %p, Setup DMA: %08lx\n",
+		 urb->setup_packet, (unsigned long)urb->setup_dma);
+	dev_vdbg(hsotg->dev, "  Interval: %d\n", urb->interval);
+
+	if (usb_pipetype(urb->pipe) == PIPE_ISOCHRONOUS) {
+		int i;
+
+		for (i = 0; i < urb->number_of_packets; i++) {
+			dev_vdbg(hsotg->dev, "  ISO Desc %d:\n", i);
+			dev_vdbg(hsotg->dev, "    offset: %d, length %d\n",
+				 urb->iso_frame_desc[i].offset,
+				 urb->iso_frame_desc[i].length);
+		}
+	}
+#endif
+}
+
+/*
+ * Starts processing a USB transfer request specified by a USB Request Block
+ * (URB). mem_flags indicates the type of memory allocation to use while
+ * processing this URB.
+ */
+static int _dwc2_hcd_urb_enqueue(struct usb_hcd *hcd, struct urb *urb,
+				 gfp_t mem_flags)
+{
+	struct dwc2_hsotg *hsotg = dwc2_hcd_to_hsotg(hcd);
+	struct usb_host_endpoint *ep = urb->ep;
+	struct dwc2_hcd_urb *dwc2_urb;
+	int i;
+	int alloc_bandwidth = 0;
+	int retval = 0;
+	u8 ep_type = 0;
+	u32 tflags = 0;
+	void *buf;
+	unsigned long flags;
+
+	if (dbg_urb(urb)) {
+		dev_vdbg(hsotg->dev, "DWC OTG HCD URB Enqueue\n");
+		dwc2_dump_urb_info(hcd, urb, "urb_enqueue");
+	}
+
+	if (ep == NULL)
+		return -EINVAL;
+
+	if (usb_pipetype(urb->pipe) == PIPE_ISOCHRONOUS ||
+	    usb_pipetype(urb->pipe) == PIPE_INTERRUPT) {
+		spin_lock_irqsave(&hsotg->lock, flags);
+		if (!dwc2_hcd_is_bandwidth_allocated(hsotg, ep))
+			alloc_bandwidth = 1;
+		spin_unlock_irqrestore(&hsotg->lock, flags);
+	}
+
+	switch (usb_pipetype(urb->pipe)) {
+	case PIPE_CONTROL:
+		ep_type = USB_ENDPOINT_XFER_CONTROL;
+		break;
+	case PIPE_ISOCHRONOUS:
+		ep_type = USB_ENDPOINT_XFER_ISOC;
+		break;
+	case PIPE_BULK:
+		ep_type = USB_ENDPOINT_XFER_BULK;
+		break;
+	case PIPE_INTERRUPT:
+		ep_type = USB_ENDPOINT_XFER_INT;
+		break;
+	default:
+		dev_warn(hsotg->dev, "Wrong ep type\n");
+	}
+
+	dwc2_urb = dwc2_hcd_urb_alloc(hsotg, urb->number_of_packets,
+				      mem_flags);
+	if (!dwc2_urb)
+		return -ENOMEM;
+
+	dwc2_hcd_urb_set_pipeinfo(hsotg, dwc2_urb, usb_pipedevice(urb->pipe),
+				  usb_pipeendpoint(urb->pipe), ep_type,
+				  usb_pipein(urb->pipe),
+				  usb_maxpacket(urb->dev, urb->pipe,
+						!(usb_pipein(urb->pipe))));
+
+	buf = urb->transfer_buffer;
+	if (hcd->self.uses_dma) {
+		/*
+		 * Calculate virtual address from physical address, because
+		 * some class driver may not fill transfer_buffer.
+		 * In Buffer DMA mode virtual address is used, when handling
+		 * non-DWORD aligned buffers.
+		 */
+		buf = bus_to_virt(urb->transfer_dma);
+	}
+
+	if (!(urb->transfer_flags & URB_NO_INTERRUPT))
+		tflags |= URB_GIVEBACK_ASAP;
+	if (urb->transfer_flags & URB_ZERO_PACKET)
+		tflags |= URB_SEND_ZERO_PACKET;
+
+	dwc2_urb->priv = urb;
+	dwc2_urb->buf = buf;
+	dwc2_urb->dma = urb->transfer_dma;
+	dwc2_urb->length = urb->transfer_buffer_length;
+	dwc2_urb->setup_packet = urb->setup_packet;
+	dwc2_urb->setup_dma = urb->setup_dma;
+	dwc2_urb->flags = tflags;
+	dwc2_urb->interval = urb->interval;
+	dwc2_urb->status = -EINPROGRESS;
+
+	for (i = 0; i < urb->number_of_packets; ++i)
+		dwc2_hcd_urb_set_iso_desc_params(dwc2_urb, i,
+						 urb->iso_frame_desc[i].offset,
+						 urb->iso_frame_desc[i].length);
+
+	urb->hcpriv = dwc2_urb;
+	retval = dwc2_hcd_urb_enqueue(hsotg, dwc2_urb, &ep->hcpriv,
+				      mem_flags);
+	if (retval) {
+		urb->hcpriv = NULL;
+		kfree(dwc2_urb);
+	} else {
+		if (alloc_bandwidth) {
+			spin_lock_irqsave(&hsotg->lock, flags);
+			dwc2_allocate_bus_bandwidth(hcd,
+					dwc2_hcd_get_ep_bandwidth(hsotg, ep),
+					urb);
+			spin_unlock_irqrestore(&hsotg->lock, flags);
+		}
+	}
+
+	return retval;
+}
+
+/*
+ * Aborts/cancels a USB transfer request. Always returns 0 to indicate success.
+ */
+static int _dwc2_hcd_urb_dequeue(struct usb_hcd *hcd, struct urb *urb,
+				 int status)
+{
+	struct dwc2_hsotg *hsotg = dwc2_hcd_to_hsotg(hcd);
+	int rc = 0;
+	unsigned long flags;
+
+	dev_dbg(hsotg->dev, "DWC OTG HCD URB Dequeue\n");
+	dwc2_dump_urb_info(hcd, urb, "urb_dequeue");
+
+	spin_lock_irqsave(&hsotg->lock, flags);
+
+	if (!urb->hcpriv) {
+		dev_dbg(hsotg->dev, "## urb->hcpriv is NULL ##\n");
+		goto out;
+	}
+
+	rc = dwc2_hcd_urb_dequeue(hsotg, urb->hcpriv);
+
+	kfree(urb->hcpriv);
+	urb->hcpriv = NULL;
+
+	/* Higher layer software sets URB status */
+	spin_unlock(&hsotg->lock);
+	usb_hcd_giveback_urb(hcd, urb, status);
+	spin_lock(&hsotg->lock);
+
+	dev_dbg(hsotg->dev, "Called usb_hcd_giveback_urb()\n");
+	dev_dbg(hsotg->dev, "  urb->status = %d\n", urb->status);
+out:
+	spin_unlock_irqrestore(&hsotg->lock, flags);
+
+	return rc;
+}
+
+/*
+ * Frees resources in the DWC_otg controller related to a given endpoint. Also
+ * clears state in the HCD related to the endpoint. Any URBs for the endpoint
+ * must already be dequeued.
+ */
+static void _dwc2_hcd_endpoint_disable(struct usb_hcd *hcd,
+				       struct usb_host_endpoint *ep)
+{
+	struct dwc2_hsotg *hsotg = dwc2_hcd_to_hsotg(hcd);
+
+	dev_dbg(hsotg->dev,
+		"DWC OTG HCD EP DISABLE: bEndpointAddress=0x%02x, ep->hcpriv=%p\n",
+		ep->desc.bEndpointAddress, ep->hcpriv);
+	dwc2_hcd_endpoint_disable(hsotg, ep, 250);
+}
+
+/*
+ * Resets endpoint specific parameter values, in current version used to reset
+ * the data toggle (as a WA). This function can be called from usb_clear_halt
+ * routine.
+ */
+static void _dwc2_hcd_endpoint_reset(struct usb_hcd *hcd,
+				     struct usb_host_endpoint *ep)
+{
+	struct dwc2_hsotg *hsotg = dwc2_hcd_to_hsotg(hcd);
+	int is_control = usb_endpoint_xfer_control(&ep->desc);
+	int is_out = usb_endpoint_dir_out(&ep->desc);
+	int epnum = usb_endpoint_num(&ep->desc);
+	struct usb_device *udev;
+	unsigned long flags;
+
+	dev_dbg(hsotg->dev,
+		"DWC OTG HCD EP RESET: bEndpointAddress=0x%02x\n",
+		ep->desc.bEndpointAddress);
+
+	udev = to_usb_device(hsotg->dev);
+
+	spin_lock_irqsave(&hsotg->lock, flags);
+
+	usb_settoggle(udev, epnum, is_out, 0);
+	if (is_control)
+		usb_settoggle(udev, epnum, !is_out, 0);
+	dwc2_hcd_endpoint_reset(hsotg, ep);
+
+	spin_unlock_irqrestore(&hsotg->lock, flags);
+}
+
+/*
+ * Handles host mode interrupts for the DWC_otg controller. Returns IRQ_NONE if
+ * there was no interrupt to handle. Returns IRQ_HANDLED if there was a valid
+ * interrupt.
+ *
+ * This function is called by the USB core when an interrupt occurs
+ */
+static irqreturn_t _dwc2_hcd_irq(struct usb_hcd *hcd)
+{
+	struct dwc2_hsotg *hsotg = dwc2_hcd_to_hsotg(hcd);
+	int retval = dwc2_hcd_intr(hsotg);
+
+	return IRQ_RETVAL(retval);
+}
+
+/*
+ * Creates Status Change bitmap for the root hub and root port. The bitmap is
+ * returned in buf. Bit 0 is the status change indicator for the root hub. Bit 1
+ * is the status change indicator for the single root port. Returns 1 if either
+ * change indicator is 1, otherwise returns 0.
+ */
+static int _dwc2_hcd_hub_status_data(struct usb_hcd *hcd, char *buf)
+{
+	struct dwc2_hsotg *hsotg = dwc2_hcd_to_hsotg(hcd);
+
+	buf[0] = dwc2_hcd_is_status_changed(hsotg, 1) << 1;
+	return buf[0] != 0;
+}
+
+/* Handles hub class-specific requests */
+static int _dwc2_hcd_hub_control(struct usb_hcd *hcd, u16 typereq, u16 wvalue,
+				 u16 windex, char *buf, u16 wlength)
+{
+	int retval = dwc2_hcd_hub_control(dwc2_hcd_to_hsotg(hcd), typereq,
+					  wvalue, windex, buf, wlength);
+	return retval;
+}
+
+/* Handles hub TT buffer clear completions */
+static void _dwc2_hcd_clear_tt_buffer_complete(struct usb_hcd *hcd,
+					       struct usb_host_endpoint *ep)
+{
+	struct dwc2_hsotg *hsotg = dwc2_hcd_to_hsotg(hcd);
+	struct dwc2_qh *qh;
+	unsigned long flags;
+
+	qh = ep->hcpriv;
+	if (!qh)
+		return;
+
+	spin_lock_irqsave(&hsotg->lock, flags);
+	qh->tt_buffer_dirty = 0;
+
+	if (hsotg->flags.b.port_connect_status)
+		dwc2_hcd_queue_transactions(hsotg, DWC2_TRANSACTION_ALL);
+
+	spin_unlock_irqrestore(&hsotg->lock, flags);
+}
+
+static struct hc_driver dwc2_hc_driver = {
+	.description = "dwc2_hsotg",
+	.product_desc = "DWC OTG Controller",
+	.hcd_priv_size = sizeof(struct wrapper_priv_data),
+
+	.irq = _dwc2_hcd_irq,
+	.flags = HCD_MEMORY | HCD_USB2,
+
+	.start = _dwc2_hcd_start,
+	.stop = _dwc2_hcd_stop,
+	.urb_enqueue = _dwc2_hcd_urb_enqueue,
+	.urb_dequeue = _dwc2_hcd_urb_dequeue,
+	.endpoint_disable = _dwc2_hcd_endpoint_disable,
+	.endpoint_reset = _dwc2_hcd_endpoint_reset,
+	.get_frame_number = _dwc2_hcd_get_frame_number,
+
+	.hub_status_data = _dwc2_hcd_hub_status_data,
+	.hub_control = _dwc2_hcd_hub_control,
+	.clear_tt_buffer_complete = _dwc2_hcd_clear_tt_buffer_complete,
+};
+
+/*
+ * Frees secondary storage associated with the dwc2_hsotg structure contained
+ * in the struct usb_hcd field
+ */
+static void dwc2_hcd_free(struct dwc2_hsotg *hsotg)
+{
+	u32 ahbcfg;
+	u32 dctl;
+	int i;
+
+	dev_dbg(hsotg->dev, "DWC OTG HCD FREE\n");
+
+	/* Free memory for QH/QTD lists */
+	dwc2_qh_list_free(hsotg, &hsotg->non_periodic_sched_inactive);
+	dwc2_qh_list_free(hsotg, &hsotg->non_periodic_sched_active);
+	dwc2_qh_list_free(hsotg, &hsotg->periodic_sched_inactive);
+	dwc2_qh_list_free(hsotg, &hsotg->periodic_sched_ready);
+	dwc2_qh_list_free(hsotg, &hsotg->periodic_sched_assigned);
+	dwc2_qh_list_free(hsotg, &hsotg->periodic_sched_queued);
+
+	/* Free memory for the host channels */
+	for (i = 0; i < MAX_EPS_CHANNELS; i++) {
+		struct dwc2_host_chan *chan = hsotg->hc_ptr_array[i];
+
+		if (chan != NULL) {
+			dev_dbg(hsotg->dev, "HCD Free channel #%i, chan=%p\n",
+				i, chan);
+			hsotg->hc_ptr_array[i] = NULL;
+			kfree(chan);
+		}
+	}
+
+	if (hsotg->core_params->dma_enable > 0) {
+		if (hsotg->status_buf) {
+			dma_free_coherent(hsotg->dev, DWC2_HCD_STATUS_BUF_SIZE,
+					  hsotg->status_buf,
+					  hsotg->status_buf_dma);
+			hsotg->status_buf = NULL;
+		}
+	} else {
+		kfree(hsotg->status_buf);
+		hsotg->status_buf = NULL;
+	}
+
+	ahbcfg = readl(hsotg->regs + GAHBCFG);
+
+	/* Disable all interrupts */
+	ahbcfg &= ~GAHBCFG_GLBL_INTR_EN;
+	writel(ahbcfg, hsotg->regs + GAHBCFG);
+	writel(0, hsotg->regs + GINTMSK);
+
+	if (hsotg->snpsid >= DWC2_CORE_REV_3_00a) {
+		dctl = readl(hsotg->regs + DCTL);
+		dctl |= DCTL_SFTDISCON;
+		writel(dctl, hsotg->regs + DCTL);
+	}
+
+	if (hsotg->wq_otg) {
+		if (!cancel_work_sync(&hsotg->wf_otg))
+			flush_workqueue(hsotg->wq_otg);
+		destroy_workqueue(hsotg->wq_otg);
+	}
+
+	kfree(hsotg->core_params);
+	hsotg->core_params = NULL;
+	del_timer(&hsotg->wkp_timer);
+}
+
+static void dwc2_hcd_release(struct dwc2_hsotg *hsotg)
+{
+	/* Turn off all host-specific interrupts */
+	dwc2_disable_host_interrupts(hsotg);
+
+	dwc2_hcd_free(hsotg);
+}
+
+/*
+ * Sets all parameters to the given value.
+ *
+ * Assumes that the dwc2_core_params struct contains only integers.
+ */
+void dwc2_set_all_params(struct dwc2_core_params *params, int value)
+{
+	int *p = (int *)params;
+	size_t size = sizeof(*params) / sizeof(*p);
+	int i;
+
+	for (i = 0; i < size; i++)
+		p[i] = -1;
+}
+EXPORT_SYMBOL_GPL(dwc2_set_all_params);
+
+/*
+ * Initializes the HCD. This function allocates memory for and initializes the
+ * static parts of the usb_hcd and dwc2_hsotg structures. It also registers the
+ * USB bus with the core and calls the hc_driver->start() function. It returns
+ * a negative error on failure.
+ */
+int dwc2_hcd_init(struct dwc2_hsotg *hsotg, int irq,
+		  struct dwc2_core_params *params)
+{
+	struct usb_hcd *hcd;
+	struct dwc2_host_chan *channel;
+	u32 snpsid, gusbcfg, hcfg;
+	int i, num_channels;
+	int retval = -ENOMEM;
+
+	dev_dbg(hsotg->dev, "DWC OTG HCD INIT\n");
+
+	/*
+	 * Attempt to ensure this device is really a DWC_otg Controller.
+	 * Read and verify the GSNPSID register contents. The value should be
+	 * 0x45f42xxx or 0x45f43xxx, which corresponds to either "OT2" or "OT3",
+	 * as in "OTG version 2.xx" or "OTG version 3.xx".
+	 */
+	snpsid = readl(hsotg->regs + GSNPSID);
+	if ((snpsid & 0xfffff000) != 0x4f542000 &&
+	    (snpsid & 0xfffff000) != 0x4f543000) {
+		dev_err(hsotg->dev, "Bad value for GSNPSID: 0x%08x\n", snpsid);
+		retval = -ENODEV;
+		goto error1;
+	}
+
+	/*
+	 * Store the contents of the hardware configuration registers here for
+	 * easy access later
+	 */
+	hsotg->hwcfg1 = readl(hsotg->regs + GHWCFG1);
+	hsotg->hwcfg2 = readl(hsotg->regs + GHWCFG2);
+	hsotg->hwcfg3 = readl(hsotg->regs + GHWCFG3);
+	hsotg->hwcfg4 = readl(hsotg->regs + GHWCFG4);
+
+	dev_dbg(hsotg->dev, "hwcfg1=%08x\n", hsotg->hwcfg1);
+	dev_dbg(hsotg->dev, "hwcfg2=%08x\n", hsotg->hwcfg2);
+	dev_dbg(hsotg->dev, "hwcfg3=%08x\n", hsotg->hwcfg3);
+	dev_dbg(hsotg->dev, "hwcfg4=%08x\n", hsotg->hwcfg4);
+
+	/* Force host mode to get HPTXFSIZ exact power on value */
+	gusbcfg = readl(hsotg->regs + GUSBCFG);
+	gusbcfg |= GUSBCFG_FORCEHOSTMODE;
+	writel(gusbcfg, hsotg->regs + GUSBCFG);
+	usleep_range(100000, 150000);
+
+	hsotg->hptxfsiz = readl(hsotg->regs + HPTXFSIZ);
+	dev_dbg(hsotg->dev, "hptxfsiz=%08x\n", hsotg->hptxfsiz);
+	gusbcfg = readl(hsotg->regs + GUSBCFG);
+	gusbcfg &= ~GUSBCFG_FORCEHOSTMODE;
+	writel(gusbcfg, hsotg->regs + GUSBCFG);
+	usleep_range(100000, 150000);
+
+	hcfg = readl(hsotg->regs + HCFG);
+	dev_dbg(hsotg->dev, "hcfg=%08x\n", hcfg);
+	dev_dbg(hsotg->dev, "op_mode=%0x\n",
+		hsotg->hwcfg2 >> GHWCFG2_OP_MODE_SHIFT &
+		GHWCFG2_OP_MODE_MASK >> GHWCFG2_OP_MODE_SHIFT);
+	dev_dbg(hsotg->dev, "arch=%0x\n",
+		hsotg->hwcfg2 >> GHWCFG2_ARCHITECTURE_SHIFT &
+		GHWCFG2_ARCHITECTURE_MASK >> GHWCFG2_ARCHITECTURE_SHIFT);
+	dev_dbg(hsotg->dev, "num_dev_ep=%d\n",
+		hsotg->hwcfg2 >> GHWCFG2_NUM_DEV_EP_SHIFT &
+		GHWCFG2_NUM_DEV_EP_MASK >> GHWCFG2_NUM_DEV_EP_SHIFT);
+	dev_dbg(hsotg->dev, "max_host_chan=%d\n",
+		hsotg->hwcfg2 >> GHWCFG2_NUM_HOST_CHAN_SHIFT &
+		GHWCFG2_NUM_HOST_CHAN_MASK >> GHWCFG2_NUM_HOST_CHAN_SHIFT);
+	dev_dbg(hsotg->dev, "nonperio_tx_q_depth=0x%0x\n",
+		hsotg->hwcfg2 >> GHWCFG2_NONPERIO_TX_Q_DEPTH_SHIFT &
+		GHWCFG2_NONPERIO_TX_Q_DEPTH_MASK >>
+				GHWCFG2_NONPERIO_TX_Q_DEPTH_SHIFT);
+	dev_dbg(hsotg->dev, "host_perio_tx_q_depth=0x%0x\n",
+		hsotg->hwcfg2 >> GHWCFG2_HOST_PERIO_TX_Q_DEPTH_SHIFT &
+		GHWCFG2_HOST_PERIO_TX_Q_DEPTH_MASK >>
+				GHWCFG2_HOST_PERIO_TX_Q_DEPTH_SHIFT);
+	dev_dbg(hsotg->dev, "dev_token_q_depth=0x%0x\n",
+		hsotg->hwcfg2 >> GHWCFG2_DEV_TOKEN_Q_DEPTH_SHIFT &
+		GHWCFG3_XFER_SIZE_CNTR_WIDTH_MASK >>
+				GHWCFG3_XFER_SIZE_CNTR_WIDTH_SHIFT);
+
+#ifdef CONFIG_USB_DWC2_TRACK_MISSED_SOFS
+	hsotg->frame_num_array = kzalloc(sizeof(*hsotg->frame_num_array) *
+					 FRAME_NUM_ARRAY_SIZE, GFP_KERNEL);
+	if (!hsotg->frame_num_array)
+		goto error1;
+	hsotg->last_frame_num_array = kzalloc(
+			sizeof(*hsotg->last_frame_num_array) *
+			FRAME_NUM_ARRAY_SIZE, GFP_KERNEL);
+	if (!hsotg->last_frame_num_array)
+		goto error1;
+	hsotg->last_frame_num = HFNUM_MAX_FRNUM;
+#endif
+
+	hsotg->core_params = kzalloc(sizeof(*hsotg->core_params), GFP_KERNEL);
+	if (!hsotg->core_params)
+		goto error1;
+
+	dwc2_set_all_params(hsotg->core_params, -1);
+
+	/* Validate parameter values */
+	dwc2_set_parameters(hsotg, params);
+
+	/* Set device flags indicating whether the HCD supports DMA */
+	if (hsotg->core_params->dma_enable > 0) {
+		if (dma_set_mask(hsotg->dev, DMA_BIT_MASK(31)) < 0)
+			dev_warn(hsotg->dev,
+				 "can't enable workaround for >2GB RAM\n");
+		if (dma_set_coherent_mask(hsotg->dev, DMA_BIT_MASK(31)) < 0)
+			dev_warn(hsotg->dev,
+				 "can't enable workaround for >2GB RAM\n");
+	} else {
+		dma_set_mask(hsotg->dev, 0);
+		dma_set_coherent_mask(hsotg->dev, 0);
+	}
+
+	hcd = usb_create_hcd(&dwc2_hc_driver, hsotg->dev, dev_name(hsotg->dev));
+	if (!hcd)
+		goto error1;
+
+	hcd->has_tt = 1;
+
+	spin_lock_init(&hsotg->lock);
+	((struct wrapper_priv_data *) &hcd->hcd_priv)->hsotg = hsotg;
+	hsotg->priv = hcd;
+
+	/*
+	 * Disable the global interrupt until all the interrupt handlers are
+	 * installed
+	 */
+	dwc2_disable_global_interrupts(hsotg);
+
+	/* Initialize the DWC_otg core, and select the Phy type */
+	retval = dwc2_core_init(hsotg, true, irq);
+	if (retval)
+		goto error2;
+
+	/* Create new workqueue and init work */
+	retval = -ENOMEM;
+	hsotg->wq_otg = create_singlethread_workqueue("dwc2");
+	if (!hsotg->wq_otg) {
+		dev_err(hsotg->dev, "Failed to create workqueue\n");
+		goto error2;
+	}
+	INIT_WORK(&hsotg->wf_otg, dwc2_conn_id_status_change);
+
+	hsotg->snpsid = readl(hsotg->regs + GSNPSID);
+	dev_dbg(hsotg->dev, "Core Release: %1x.%1x%1x%1x\n",
+		hsotg->snpsid >> 12 & 0xf, hsotg->snpsid >> 8 & 0xf,
+		hsotg->snpsid >> 4 & 0xf, hsotg->snpsid & 0xf);
+
+	setup_timer(&hsotg->wkp_timer, dwc2_wakeup_detected,
+		    (unsigned long)hsotg);
+
+	/* Initialize the non-periodic schedule */
+	INIT_LIST_HEAD(&hsotg->non_periodic_sched_inactive);
+	INIT_LIST_HEAD(&hsotg->non_periodic_sched_active);
+
+	/* Initialize the periodic schedule */
+	INIT_LIST_HEAD(&hsotg->periodic_sched_inactive);
+	INIT_LIST_HEAD(&hsotg->periodic_sched_ready);
+	INIT_LIST_HEAD(&hsotg->periodic_sched_assigned);
+	INIT_LIST_HEAD(&hsotg->periodic_sched_queued);
+
+	/*
+	 * Create a host channel descriptor for each host channel implemented
+	 * in the controller. Initialize the channel descriptor array.
+	 */
+	INIT_LIST_HEAD(&hsotg->free_hc_list);
+	num_channels = hsotg->core_params->host_channels;
+	memset(&hsotg->hc_ptr_array[0], 0, sizeof(hsotg->hc_ptr_array));
+
+	for (i = 0; i < num_channels; i++) {
+		channel = kzalloc(sizeof(*channel), GFP_KERNEL);
+		if (channel == NULL)
+			goto error3;
+		channel->hc_num = i;
+		hsotg->hc_ptr_array[i] = channel;
+	}
+
+	/* Initialize hsotg start work */
+	INIT_DELAYED_WORK(&hsotg->start_work, dwc2_hcd_start_func);
+
+	/* Initialize port reset work */
+	INIT_DELAYED_WORK(&hsotg->reset_work, dwc2_hcd_reset_func);
+
+	/*
+	 * Allocate space for storing data on status transactions. Normally no
+	 * data is sent, but this space acts as a bit bucket. This must be
+	 * done after usb_add_hcd since that function allocates the DMA buffer
+	 * pool.
+	 */
+	if (hsotg->core_params->dma_enable > 0)
+		hsotg->status_buf = dma_alloc_coherent(hsotg->dev,
+					DWC2_HCD_STATUS_BUF_SIZE,
+					&hsotg->status_buf_dma, GFP_KERNEL);
+	else
+		hsotg->status_buf = kzalloc(DWC2_HCD_STATUS_BUF_SIZE,
+					  GFP_KERNEL);
+
+	if (!hsotg->status_buf)
+		goto error3;
+
+	hsotg->otg_port = 1;
+	hsotg->frame_list = NULL;
+	hsotg->frame_list_dma = 0;
+	hsotg->periodic_qh_count = 0;
+
+	/* Initiate lx_state to L3 disconnected state */
+	hsotg->lx_state = DWC2_L3;
+
+	hcd->self.otg_port = hsotg->otg_port;
+
+	/* Don't support SG list at this point */
+	hcd->self.sg_tablesize = 0;
+
+	/*
+	 * Finish generic HCD initialization and start the HCD. This function
+	 * allocates the DMA buffer pool, registers the USB bus, requests the
+	 * IRQ line, and calls hcd_start method.
+	 */
+	retval = usb_add_hcd(hcd, irq, IRQF_SHARED | IRQF_DISABLED);
+	if (retval < 0)
+		goto error3;
+
+	dwc2_dump_global_registers(hsotg);
+	dwc2_dump_host_registers(hsotg);
+	dwc2_hcd_dump_state(hsotg);
+
+	dwc2_enable_global_interrupts(hsotg);
+
+	return 0;
+
+error3:
+	dwc2_hcd_release(hsotg);
+error2:
+	usb_put_hcd(hcd);
+error1:
+	kfree(hsotg->core_params);
+
+#ifdef CONFIG_USB_DWC2_TRACK_MISSED_SOFS
+	kfree(hsotg->last_frame_num_array);
+	kfree(hsotg->frame_num_array);
+#endif
+
+	dev_err(hsotg->dev, "%s() FAILED, returning %d\n", __func__, retval);
+	return retval;
+}
+EXPORT_SYMBOL_GPL(dwc2_hcd_init);
+
+/*
+ * Removes the HCD.
+ * Frees memory and resources associated with the HCD and deregisters the bus.
+ */
+void dwc2_hcd_remove(struct dwc2_hsotg *hsotg)
+{
+	struct usb_hcd *hcd;
+
+	dev_dbg(hsotg->dev, "DWC OTG HCD REMOVE\n");
+
+	hcd = dwc2_hsotg_to_hcd(hsotg);
+	dev_dbg(hsotg->dev, "hsotg->hcd = %p\n", hcd);
+
+	if (!hcd) {
+		dev_dbg(hsotg->dev, "%s: dwc2_hsotg_to_hcd(hsotg) NULL!\n",
+			__func__);
+		return;
+	}
+
+	usb_remove_hcd(hcd);
+	hsotg->priv = NULL;
+	dwc2_hcd_release(hsotg);
+	usb_put_hcd(hcd);
+
+#ifdef CONFIG_USB_DWC2_TRACK_MISSED_SOFS
+	kfree(hsotg->last_frame_num_array);
+	kfree(hsotg->frame_num_array);
+#endif
+}
+EXPORT_SYMBOL_GPL(dwc2_hcd_remove);
diff --git a/drivers/staging/dwc2/hcd.h b/drivers/staging/dwc2/hcd.h
new file mode 100644
index 00000000000..d071f1a05df
--- /dev/null
+++ b/drivers/staging/dwc2/hcd.h
@@ -0,0 +1,765 @@
+/*
+ * hcd.h - DesignWare HS OTG Controller host-mode declarations
+ *
+ * Copyright (C) 2004-2013 Synopsys, Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions, and the following disclaimer,
+ *    without modification.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The names of the above-listed copyright holders may not be used
+ *    to endorse or promote products derived from this software without
+ *    specific prior written permission.
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation; either version 2 of the License, or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+ * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef __DWC2_HCD_H__
+#define __DWC2_HCD_H__
+
+/*
+ * This file contains the structures, constants, and interfaces for the
+ * Host Contoller Driver (HCD)
+ *
+ * The Host Controller Driver (HCD) is responsible for translating requests
+ * from the USB Driver into the appropriate actions on the DWC_otg controller.
+ * It isolates the USBD from the specifics of the controller by providing an
+ * API to the USBD.
+ */
+
+struct dwc2_qh;
+
+/**
+ * struct dwc2_host_chan - Software host channel descriptor
+ *
+ * @hc_num:             Host channel number, used for register address lookup
+ * @dev_addr:           Address of the device
+ * @ep_num:             Endpoint of the device
+ * @ep_is_in:           Endpoint direction
+ * @speed:              Device speed. One of the following values:
+ *                       - USB_SPEED_LOW
+ *                       - USB_SPEED_FULL
+ *                       - USB_SPEED_HIGH
+ * @ep_type:            Endpoint type. One of the following values:
+ *                       - USB_ENDPOINT_XFER_CONTROL: 0
+ *                       - USB_ENDPOINT_XFER_ISOC:    1
+ *                       - USB_ENDPOINT_XFER_BULK:    2
+ *                       - USB_ENDPOINT_XFER_INTR:    3
+ * @max_packet:         Max packet size in bytes
+ * @data_pid_start:     PID for initial transaction.
+ *                       0: DATA0
+ *                       1: DATA2
+ *                       2: DATA1
+ *                       3: MDATA (non-Control EP),
+ *                          SETUP (Control EP)
+ * @multi_count:        Number of additional periodic transactions per
+ *                      (micro)frame
+ * @xfer_buf:           Pointer to current transfer buffer position
+ * @xfer_dma:           DMA address of xfer_buf
+ * @align_buf:          In Buffer DMA mode this will be used if xfer_buf is not
+ *                      DWORD aligned
+ * @xfer_len:           Total number of bytes to transfer
+ * @xfer_count:         Number of bytes transferred so far
+ * @start_pkt_count:    Packet count at start of transfer
+ * @xfer_started:       True if the transfer has been started
+ * @ping:               True if a PING request should be issued on this channel
+ * @error_state:        True if the error count for this transaction is non-zero
+ * @halt_on_queue:      True if this channel should be halted the next time a
+ *                      request is queued for the channel. This is necessary in
+ *                      slave mode if no request queue space is available when
+ *                      an attempt is made to halt the channel.
+ * @halt_pending:       True if the host channel has been halted, but the core
+ *                      is not finished flushing queued requests
+ * @do_split:           Enable split for the channel
+ * @complete_split:     Enable complete split
+ * @hub_addr:           Address of high speed hub for the split
+ * @hub_port:           Port of the low/full speed device for the split
+ * @xact_pos:           Split transaction position. One of the following values:
+ *                       - DWC2_HCSPLT_XACTPOS_MID
+ *                       - DWC2_HCSPLT_XACTPOS_BEGIN
+ *                       - DWC2_HCSPLT_XACTPOS_END
+ *                       - DWC2_HCSPLT_XACTPOS_ALL
+ * @requests:           Number of requests issued for this channel since it was
+ *                      assigned to the current transfer (not counting PINGs)
+ * @schinfo:            Scheduling micro-frame bitmap
+ * @ntd:                Number of transfer descriptors for the transfer
+ * @halt_status:        Reason for halting the host channel
+ * @hcint               Contents of the HCINT register when the interrupt came
+ * @qh:                 QH for the transfer being processed by this channel
+ * @hc_list_entry:      For linking to list of host channels
+ * @desc_list_addr:     Current QH's descriptor list DMA address
+ *
+ * This structure represents the state of a single host channel when acting in
+ * host mode. It contains the data items needed to transfer packets to an
+ * endpoint via a host channel.
+ */
+struct dwc2_host_chan {
+	u8 hc_num;
+
+	unsigned dev_addr:7;
+	unsigned ep_num:4;
+	unsigned ep_is_in:1;
+	unsigned speed:4;
+	unsigned ep_type:2;
+	unsigned max_packet:11;
+	unsigned data_pid_start:2;
+#define DWC2_HC_PID_DATA0	(TSIZ_SC_MC_PID_DATA0 >> TSIZ_SC_MC_PID_SHIFT)
+#define DWC2_HC_PID_DATA2	(TSIZ_SC_MC_PID_DATA2 >> TSIZ_SC_MC_PID_SHIFT)
+#define DWC2_HC_PID_DATA1	(TSIZ_SC_MC_PID_DATA1 >> TSIZ_SC_MC_PID_SHIFT)
+#define DWC2_HC_PID_MDATA	(TSIZ_SC_MC_PID_MDATA >> TSIZ_SC_MC_PID_SHIFT)
+#define DWC2_HC_PID_SETUP	(TSIZ_SC_MC_PID_SETUP >> TSIZ_SC_MC_PID_SHIFT)
+
+	unsigned multi_count:2;
+
+	u8 *xfer_buf;
+	dma_addr_t xfer_dma;
+	dma_addr_t align_buf;
+	u32 xfer_len;
+	u32 xfer_count;
+	u16 start_pkt_count;
+	u8 xfer_started;
+	u8 do_ping;
+	u8 error_state;
+	u8 halt_on_queue;
+	u8 halt_pending;
+	u8 do_split;
+	u8 complete_split;
+	u8 hub_addr;
+	u8 hub_port;
+	u8 xact_pos;
+#define DWC2_HCSPLT_XACTPOS_MID	(HCSPLT_XACTPOS_MID >> HCSPLT_XACTPOS_SHIFT)
+#define DWC2_HCSPLT_XACTPOS_END	(HCSPLT_XACTPOS_END >> HCSPLT_XACTPOS_SHIFT)
+#define DWC2_HCSPLT_XACTPOS_BEGIN (HCSPLT_XACTPOS_BEGIN >> HCSPLT_XACTPOS_SHIFT)
+#define DWC2_HCSPLT_XACTPOS_ALL	(HCSPLT_XACTPOS_ALL >> HCSPLT_XACTPOS_SHIFT)
+
+	u8 requests;
+	u8 schinfo;
+	u16 ntd;
+	enum dwc2_halt_status halt_status;
+	u32 hcint;
+	struct dwc2_qh *qh;
+	struct list_head hc_list_entry;
+	dma_addr_t desc_list_addr;
+};
+
+struct dwc2_hcd_pipe_info {
+	u8 dev_addr;
+	u8 ep_num;
+	u8 pipe_type;
+	u8 pipe_dir;
+	u16 mps;
+};
+
+struct dwc2_hcd_iso_packet_desc {
+	u32 offset;
+	u32 length;
+	u32 actual_length;
+	u32 status;
+};
+
+struct dwc2_qtd;
+
+struct dwc2_hcd_urb {
+	void *priv;
+	struct dwc2_qtd *qtd;
+	void *buf;
+	dma_addr_t dma;
+	void *setup_packet;
+	dma_addr_t setup_dma;
+	u32 length;
+	u32 actual_length;
+	u32 status;
+	u32 error_count;
+	u32 packet_count;
+	u32 flags;
+	u16 interval;
+	struct dwc2_hcd_pipe_info pipe_info;
+	struct dwc2_hcd_iso_packet_desc iso_descs[0];
+};
+
+/* Phases for control transfers */
+enum dwc2_control_phase {
+	DWC2_CONTROL_SETUP,
+	DWC2_CONTROL_DATA,
+	DWC2_CONTROL_STATUS,
+};
+
+/* Transaction types */
+enum dwc2_transaction_type {
+	DWC2_TRANSACTION_NONE,
+	DWC2_TRANSACTION_PERIODIC,
+	DWC2_TRANSACTION_NON_PERIODIC,
+	DWC2_TRANSACTION_ALL,
+};
+
+/**
+ * struct dwc2_qh - Software queue head structure
+ *
+ * @ep_type:            Endpoint type. One of the following values:
+ *                       - USB_ENDPOINT_XFER_CONTROL
+ *                       - USB_ENDPOINT_XFER_BULK
+ *                       - USB_ENDPOINT_XFER_INT
+ *                       - USB_ENDPOINT_XFER_ISOC
+ * @ep_is_in:           Endpoint direction
+ * @maxp:               Value from wMaxPacketSize field of Endpoint Descriptor
+ * @dev_speed:          Device speed. One of the following values:
+ *                       - USB_SPEED_LOW
+ *                       - USB_SPEED_FULL
+ *                       - USB_SPEED_HIGH
+ * @data_toggle:        Determines the PID of the next data packet for
+ *                      non-controltransfers. Ignored for control transfers.
+ *                      One of the following values:
+ *                       - DWC2_HC_PID_DATA0
+ *                       - DWC2_HC_PID_DATA1
+ * @ping_state:         Ping state
+ * @do_split:           Full/low speed endpoint on high-speed hub requires split
+ * @qtd_list:           List of QTDs for this QH
+ * @channel:            Host channel currently processing transfers for this QH
+ * @usecs:              Bandwidth in microseconds per (micro)frame
+ * @interval:           Interval between transfers in (micro)frames
+ * @sched_frame:        (micro)frame to initialize a periodic transfer.
+ *                      The transfer executes in the following (micro)frame.
+ * @start_split_frame:  (Micro)frame at which last start split was initialized
+ * @dw_align_buf:       Used instead of original buffer if its physical address
+ *                      is not dword-aligned
+ * @dw_align_buf_dma:   DMA address for align_buf
+ * @qh_list_entry:      Entry for QH in either the periodic or non-periodic
+ *                      schedule
+ * @desc_list:          List of transfer descriptors
+ * @desc_list_dma:      Physical address of desc_list
+ * @n_bytes:            Xfer Bytes array. Each element corresponds to a transfer
+ *                      descriptor and indicates original XferSize value for the
+ *                      descriptor
+ * @ntd:                Actual number of transfer descriptors in a list
+ * @td_first:           Index of first activated isochronous transfer descriptor
+ * @td_last:            Index of last activated isochronous transfer descriptor
+ * @tt_buffer_dirty     True if clear_tt_buffer_complete is pending
+ *
+ * A Queue Head (QH) holds the static characteristics of an endpoint and
+ * maintains a list of transfers (QTDs) for that endpoint. A QH structure may
+ * be entered in either the non-periodic or periodic schedule.
+ */
+struct dwc2_qh {
+	u8 ep_type;
+	u8 ep_is_in;
+	u16 maxp;
+	u8 dev_speed;
+	u8 data_toggle;
+	u8 ping_state;
+	u8 do_split;
+	struct list_head qtd_list;
+	struct dwc2_host_chan *channel;
+	u16 usecs;
+	u16 interval;
+	u16 sched_frame;
+	u16 start_split_frame;
+	u8 *dw_align_buf;
+	dma_addr_t dw_align_buf_dma;
+	struct list_head qh_list_entry;
+	struct dwc2_hcd_dma_desc *desc_list;
+	dma_addr_t desc_list_dma;
+	u32 *n_bytes;
+	u16 ntd;
+	u8 td_first;
+	u8 td_last;
+	unsigned tt_buffer_dirty:1;
+};
+
+/**
+ * struct dwc2_qtd - Software queue transfer descriptor (QTD)
+ *
+ * @control_phase:      Current phase for control transfers (Setup, Data, or
+ *                      Status)
+ * @in_process:         Indicates if this QTD is currently processed by HW
+ * @data_toggle:        Determines the PID of the next data packet for the
+ *                      data phase of control transfers. Ignored for other
+ *                      transfer types. One of the following values:
+ *                       - DWC2_HC_PID_DATA0
+ *                       - DWC2_HC_PID_DATA1
+ * @complete_split:     Keeps track of the current split type for FS/LS
+ *                      endpoints on a HS Hub
+ * @isoc_split_pos:     Position of the ISOC split in full/low speed
+ * @isoc_frame_index:   Index of the next frame descriptor for an isochronous
+ *                      transfer. A frame descriptor describes the buffer
+ *                      position and length of the data to be transferred in the
+ *                      next scheduled (micro)frame of an isochronous transfer.
+ *                      It also holds status for that transaction. The frame
+ *                      index starts at 0.
+ * @isoc_split_offset:  Position of the ISOC split in the buffer for the
+ *                      current frame
+ * @ssplit_out_xfer_count: How many bytes transferred during SSPLIT OUT
+ * @error_count:        Holds the number of bus errors that have occurred for
+ *                      a transaction within this transfer
+ * @n_desc:             Number of DMA descriptors for this QTD
+ * @isoc_frame_index_last: Last activated frame (packet) index, used in
+ *                      descriptor DMA mode only
+ * @urb:                URB for this transfer
+ * @qh:                 Queue head for this QTD
+ * @qtd_list_entry:     For linking to the QH's list of QTDs
+ *
+ * A Queue Transfer Descriptor (QTD) holds the state of a bulk, control,
+ * interrupt, or isochronous transfer. A single QTD is created for each URB
+ * (of one of these types) submitted to the HCD. The transfer associated with
+ * a QTD may require one or multiple transactions.
+ *
+ * A QTD is linked to a Queue Head, which is entered in either the
+ * non-periodic or periodic schedule for execution. When a QTD is chosen for
+ * execution, some or all of its transactions may be executed. After
+ * execution, the state of the QTD is updated. The QTD may be retired if all
+ * its transactions are complete or if an error occurred. Otherwise, it
+ * remains in the schedule so more transactions can be executed later.
+ */
+struct dwc2_qtd {
+	enum dwc2_control_phase control_phase;
+	u8 in_process;
+	u8 data_toggle;
+	u8 complete_split;
+	u8 isoc_split_pos;
+	u16 isoc_frame_index;
+	u16 isoc_split_offset;
+	u32 ssplit_out_xfer_count;
+	u8 error_count;
+	u8 n_desc;
+	u16 isoc_frame_index_last;
+	struct dwc2_hcd_urb *urb;
+	struct dwc2_qh *qh;
+	struct list_head qtd_list_entry;
+};
+
+#ifdef DEBUG
+struct hc_xfer_info {
+	struct dwc2_hsotg *hsotg;
+	struct dwc2_host_chan *chan;
+};
+#endif
+
+/* Gets the struct usb_hcd that contains a struct dwc2_hsotg */
+static inline struct usb_hcd *dwc2_hsotg_to_hcd(struct dwc2_hsotg *hsotg)
+{
+	return (struct usb_hcd *)hsotg->priv;
+}
+
+/*
+ * Inline used to disable one channel interrupt. Channel interrupts are
+ * disabled when the channel is halted or released by the interrupt handler.
+ * There is no need to handle further interrupts of that type until the
+ * channel is re-assigned. In fact, subsequent handling may cause crashes
+ * because the channel structures are cleaned up when the channel is released.
+ */
+static inline void disable_hc_int(struct dwc2_hsotg *hsotg, int chnum, u32 intr)
+{
+	u32 mask = readl(hsotg->regs + HCINTMSK(chnum));
+
+	mask &= ~intr;
+	writel(mask, hsotg->regs + HCINTMSK(chnum));
+}
+
+/*
+ * Returns the mode of operation, host or device
+ */
+static inline int dwc2_is_host_mode(struct dwc2_hsotg *hsotg)
+{
+	return (readl(hsotg->regs + GINTSTS) & GINTSTS_CURMODE_HOST) != 0;
+}
+static inline int dwc2_is_device_mode(struct dwc2_hsotg *hsotg)
+{
+	return (readl(hsotg->regs + GINTSTS) & GINTSTS_CURMODE_HOST) == 0;
+}
+
+/*
+ * Reads HPRT0 in preparation to modify. It keeps the WC bits 0 so that if they
+ * are read as 1, they won't clear when written back.
+ */
+static inline u32 dwc2_read_hprt0(struct dwc2_hsotg *hsotg)
+{
+	u32 hprt0 = readl(hsotg->regs + HPRT0);
+
+	hprt0 &= ~(HPRT0_ENA | HPRT0_CONNDET | HPRT0_ENACHG | HPRT0_OVRCURRCHG);
+	return hprt0;
+}
+
+static inline u8 dwc2_hcd_get_ep_num(struct dwc2_hcd_pipe_info *pipe)
+{
+	return pipe->ep_num;
+}
+
+static inline u8 dwc2_hcd_get_pipe_type(struct dwc2_hcd_pipe_info *pipe)
+{
+	return pipe->pipe_type;
+}
+
+static inline u16 dwc2_hcd_get_mps(struct dwc2_hcd_pipe_info *pipe)
+{
+	return pipe->mps;
+}
+
+static inline u8 dwc2_hcd_get_dev_addr(struct dwc2_hcd_pipe_info *pipe)
+{
+	return pipe->dev_addr;
+}
+
+static inline u8 dwc2_hcd_is_pipe_isoc(struct dwc2_hcd_pipe_info *pipe)
+{
+	return pipe->pipe_type == USB_ENDPOINT_XFER_ISOC;
+}
+
+static inline u8 dwc2_hcd_is_pipe_int(struct dwc2_hcd_pipe_info *pipe)
+{
+	return pipe->pipe_type == USB_ENDPOINT_XFER_INT;
+}
+
+static inline u8 dwc2_hcd_is_pipe_bulk(struct dwc2_hcd_pipe_info *pipe)
+{
+	return pipe->pipe_type == USB_ENDPOINT_XFER_BULK;
+}
+
+static inline u8 dwc2_hcd_is_pipe_control(struct dwc2_hcd_pipe_info *pipe)
+{
+	return pipe->pipe_type == USB_ENDPOINT_XFER_CONTROL;
+}
+
+static inline u8 dwc2_hcd_is_pipe_in(struct dwc2_hcd_pipe_info *pipe)
+{
+	return pipe->pipe_dir == USB_DIR_IN;
+}
+
+static inline u8 dwc2_hcd_is_pipe_out(struct dwc2_hcd_pipe_info *pipe)
+{
+	return !dwc2_hcd_is_pipe_in(pipe);
+}
+
+extern int dwc2_hcd_init(struct dwc2_hsotg *hsotg, int irq,
+			 struct dwc2_core_params *params);
+extern void dwc2_hcd_remove(struct dwc2_hsotg *hsotg);
+extern int dwc2_set_parameters(struct dwc2_hsotg *hsotg,
+			       struct dwc2_core_params *params);
+extern void dwc2_set_all_params(struct dwc2_core_params *params, int value);
+
+/* Transaction Execution Functions */
+extern enum dwc2_transaction_type dwc2_hcd_select_transactions(
+						struct dwc2_hsotg *hsotg);
+extern void dwc2_hcd_queue_transactions(struct dwc2_hsotg *hsotg,
+					enum dwc2_transaction_type tr_type);
+
+/* Schedule Queue Functions */
+/* Implemented in hcd_queue.c */
+extern void dwc2_hcd_qh_free(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh);
+extern int dwc2_hcd_qh_add(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh);
+extern void dwc2_hcd_qh_unlink(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh);
+extern void dwc2_hcd_qh_deactivate(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh,
+				   int sched_csplit);
+
+extern void dwc2_hcd_qtd_init(struct dwc2_qtd *qtd, struct dwc2_hcd_urb *urb);
+extern int dwc2_hcd_qtd_add(struct dwc2_hsotg *hsotg, struct dwc2_qtd *qtd,
+			    struct dwc2_qh **qh, gfp_t mem_flags);
+
+/* Unlinks and frees a QTD */
+static inline void dwc2_hcd_qtd_unlink_and_free(struct dwc2_hsotg *hsotg,
+						struct dwc2_qtd *qtd,
+						struct dwc2_qh *qh)
+{
+	list_del(&qtd->qtd_list_entry);
+	kfree(qtd);
+}
+
+/* Descriptor DMA support functions */
+extern void dwc2_hcd_start_xfer_ddma(struct dwc2_hsotg *hsotg,
+				     struct dwc2_qh *qh);
+extern void dwc2_hcd_complete_xfer_ddma(struct dwc2_hsotg *hsotg,
+					struct dwc2_host_chan *chan, int chnum,
+					enum dwc2_halt_status halt_status);
+
+extern int dwc2_hcd_qh_init_ddma(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh,
+				 gfp_t mem_flags);
+extern void dwc2_hcd_qh_free_ddma(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh);
+
+/* Check if QH is non-periodic */
+#define dwc2_qh_is_non_per(_qh_ptr_) \
+	((_qh_ptr_)->ep_type == USB_ENDPOINT_XFER_BULK || \
+	 (_qh_ptr_)->ep_type == USB_ENDPOINT_XFER_CONTROL)
+
+#ifdef CONFIG_USB_DWC2_DEBUG_PERIODIC
+static inline bool dbg_hc(struct dwc2_host_chan *hc) { return true; }
+static inline bool dbg_qh(struct dwc2_qh *qh) { return true; }
+static inline bool dbg_urb(struct urb *urb) { return true; }
+static inline bool dbg_perio(void) { return true; }
+#else /* !CONFIG_USB_DWC2_DEBUG_PERIODIC */
+static inline bool dbg_hc(struct dwc2_host_chan *hc)
+{
+	return hc->ep_type == USB_ENDPOINT_XFER_BULK ||
+	       hc->ep_type == USB_ENDPOINT_XFER_CONTROL;
+}
+
+static inline bool dbg_qh(struct dwc2_qh *qh)
+{
+	return qh->ep_type == USB_ENDPOINT_XFER_BULK ||
+	       qh->ep_type == USB_ENDPOINT_XFER_CONTROL;
+}
+
+static inline bool dbg_urb(struct urb *urb)
+{
+	return usb_pipetype(urb->pipe) == PIPE_BULK ||
+	       usb_pipetype(urb->pipe) == PIPE_CONTROL;
+}
+
+static inline bool dbg_perio(void) { return false; }
+#endif
+
+/* High bandwidth multiplier as encoded in highspeed endpoint descriptors */
+#define dwc2_hb_mult(wmaxpacketsize) (1 + (((wmaxpacketsize) >> 11) & 0x03))
+
+/* Packet size for any kind of endpoint descriptor */
+#define dwc2_max_packet(wmaxpacketsize) ((wmaxpacketsize) & 0x07ff)
+
+/*
+ * Returns true if frame1 is less than or equal to frame2. The comparison is
+ * done modulo HFNUM_MAX_FRNUM. This accounts for the rollover of the
+ * frame number when the max frame number is reached.
+ */
+static inline int dwc2_frame_num_le(u16 frame1, u16 frame2)
+{
+	return ((frame2 - frame1) & HFNUM_MAX_FRNUM) <= (HFNUM_MAX_FRNUM >> 1);
+}
+
+/*
+ * Returns true if frame1 is greater than frame2. The comparison is done
+ * modulo HFNUM_MAX_FRNUM. This accounts for the rollover of the frame
+ * number when the max frame number is reached.
+ */
+static inline int dwc2_frame_num_gt(u16 frame1, u16 frame2)
+{
+	return (frame1 != frame2) &&
+	       ((frame1 - frame2) & HFNUM_MAX_FRNUM) < (HFNUM_MAX_FRNUM >> 1);
+}
+
+/*
+ * Increments frame by the amount specified by inc. The addition is done
+ * modulo HFNUM_MAX_FRNUM. Returns the incremented value.
+ */
+static inline u16 dwc2_frame_num_inc(u16 frame, u16 inc)
+{
+	return (frame + inc) & HFNUM_MAX_FRNUM;
+}
+
+static inline u16 dwc2_full_frame_num(u16 frame)
+{
+	return (frame & HFNUM_MAX_FRNUM) >> 3;
+}
+
+static inline u16 dwc2_micro_frame_num(u16 frame)
+{
+	return frame & 0x7;
+}
+
+/*
+ * Returns the Core Interrupt Status register contents, ANDed with the Core
+ * Interrupt Mask register contents
+ */
+static inline u32 dwc2_read_core_intr(struct dwc2_hsotg *hsotg)
+{
+	return readl(hsotg->regs + GINTSTS) & readl(hsotg->regs + GINTMSK);
+}
+
+static inline u32 dwc2_hcd_urb_get_status(struct dwc2_hcd_urb *dwc2_urb)
+{
+	return dwc2_urb->status;
+}
+
+static inline u32 dwc2_hcd_urb_get_actual_length(
+		struct dwc2_hcd_urb *dwc2_urb)
+{
+	return dwc2_urb->actual_length;
+}
+
+static inline u32 dwc2_hcd_urb_get_error_count(struct dwc2_hcd_urb *dwc2_urb)
+{
+	return dwc2_urb->error_count;
+}
+
+static inline void dwc2_hcd_urb_set_iso_desc_params(
+		struct dwc2_hcd_urb *dwc2_urb, int desc_num, u32 offset,
+		u32 length)
+{
+	dwc2_urb->iso_descs[desc_num].offset = offset;
+	dwc2_urb->iso_descs[desc_num].length = length;
+}
+
+static inline u32 dwc2_hcd_urb_get_iso_desc_status(
+		struct dwc2_hcd_urb *dwc2_urb, int desc_num)
+{
+	return dwc2_urb->iso_descs[desc_num].status;
+}
+
+static inline u32 dwc2_hcd_urb_get_iso_desc_actual_length(
+		struct dwc2_hcd_urb *dwc2_urb, int desc_num)
+{
+	return dwc2_urb->iso_descs[desc_num].actual_length;
+}
+
+static inline int dwc2_hcd_is_bandwidth_allocated(struct dwc2_hsotg *hsotg,
+						  struct usb_host_endpoint *ep)
+{
+	struct dwc2_qh *qh = ep->hcpriv;
+
+	if (qh && !list_empty(&qh->qh_list_entry))
+		return 1;
+
+	return 0;
+}
+
+static inline u16 dwc2_hcd_get_ep_bandwidth(struct dwc2_hsotg *hsotg,
+					    struct usb_host_endpoint *ep)
+{
+	struct dwc2_qh *qh = ep->hcpriv;
+
+	if (!qh) {
+		WARN_ON(1);
+		return 0;
+	}
+
+	return qh->usecs;
+}
+
+extern void dwc2_hcd_save_data_toggle(struct dwc2_hsotg *hsotg,
+				      struct dwc2_host_chan *chan, int chnum,
+				      struct dwc2_qtd *qtd);
+
+/* HCD Core API */
+
+/**
+ * dwc2_hcd_intr() - Called on every hardware interrupt
+ *
+ * @hsotg: The DWC2 HCD
+ *
+ * Returns non zero if interrupt is handled
+ * Return 0 if interrupt is not handled
+ */
+extern int dwc2_hcd_intr(struct dwc2_hsotg *hsotg);
+
+/**
+ * dwc2_hcd_stop() - Halts the DWC_otg host mode operation
+ *
+ * @hsotg: The DWC2 HCD
+ */
+extern void dwc2_hcd_stop(struct dwc2_hsotg *hsotg);
+
+extern void dwc2_hcd_start(struct dwc2_hsotg *hsotg);
+extern void dwc2_hcd_disconnect(struct dwc2_hsotg *hsotg);
+
+/**
+ * dwc2_hcd_is_b_host() - Returns 1 if core currently is acting as B host,
+ * and 0 otherwise
+ *
+ * @hsotg: The DWC2 HCD
+ */
+extern int dwc2_hcd_is_b_host(struct dwc2_hsotg *hsotg);
+
+/**
+ * dwc2_hcd_get_frame_number() - Returns current frame number
+ *
+ * @hsotg: The DWC2 HCD
+ */
+extern int dwc2_hcd_get_frame_number(struct dwc2_hsotg *hsotg);
+
+/**
+ * dwc2_hcd_dump_state() - Dumps hsotg state
+ *
+ * @hsotg: The DWC2 HCD
+ *
+ * NOTE: This function will be removed once the peripheral controller code
+ * is integrated and the driver is stable
+ */
+extern void dwc2_hcd_dump_state(struct dwc2_hsotg *hsotg);
+
+/**
+ * dwc2_hcd_dump_frrem() - Dumps the average frame remaining at SOF
+ *
+ * @hsotg: The DWC2 HCD
+ *
+ * This can be used to determine average interrupt latency. Frame remaining is
+ * also shown for start transfer and two additional sample points.
+ *
+ * NOTE: This function will be removed once the peripheral controller code
+ * is integrated and the driver is stable
+ */
+extern void dwc2_hcd_dump_frrem(struct dwc2_hsotg *hsotg);
+
+/* URB interface */
+
+/* Transfer flags */
+#define URB_GIVEBACK_ASAP	0x1
+#define URB_SEND_ZERO_PACKET	0x2
+
+/* Host driver callbacks */
+
+extern void dwc2_host_start(struct dwc2_hsotg *hsotg);
+extern void dwc2_host_disconnect(struct dwc2_hsotg *hsotg);
+extern void dwc2_host_hub_info(struct dwc2_hsotg *hsotg, void *context,
+			       int *hub_addr, int *hub_port);
+extern int dwc2_host_get_speed(struct dwc2_hsotg *hsotg, void *context);
+extern void dwc2_host_complete(struct dwc2_hsotg *hsotg, void *context,
+			       struct dwc2_hcd_urb *dwc2_urb, int status);
+
+#ifdef DEBUG
+/*
+ * Macro to sample the remaining PHY clocks left in the current frame. This
+ * may be used during debugging to determine the average time it takes to
+ * execute sections of code. There are two possible sample points, "a" and
+ * "b", so the _letter_ argument must be one of these values.
+ *
+ * To dump the average sample times, read the "hcd_frrem" sysfs attribute. For
+ * example, "cat /sys/devices/lm0/hcd_frrem".
+ */
+#define dwc2_sample_frrem(_hcd_, _qh_, _letter_)			\
+do {									\
+	struct hfnum_data _hfnum_;					\
+	struct dwc2_qtd *_qtd_;						\
+									\
+	_qtd_ = list_entry((_qh_)->qtd_list.next, struct dwc2_qtd,	\
+			   qtd_list_entry);				\
+	if (usb_pipeint(_qtd_->urb->pipe) &&				\
+	    (_qh_)->start_split_frame != 0 && !_qtd_->complete_split) {	\
+		_hfnum_.d32 = readl((_hcd_)->regs + HFNUM);		\
+		switch (_hfnum_.b.frnum & 0x7) {			\
+		case 7:							\
+			(_hcd_)->hfnum_7_samples_##_letter_++;		\
+			(_hcd_)->hfnum_7_frrem_accum_##_letter_ +=	\
+				_hfnum_.b.frrem;			\
+			break;						\
+		case 0:							\
+			(_hcd_)->hfnum_0_samples_##_letter_++;		\
+			(_hcd_)->hfnum_0_frrem_accum_##_letter_ +=	\
+				_hfnum_.b.frrem;			\
+			break;						\
+		default:						\
+			(_hcd_)->hfnum_other_samples_##_letter_++;	\
+			(_hcd_)->hfnum_other_frrem_accum_##_letter_ +=	\
+				_hfnum_.b.frrem;			\
+			break;						\
+		}							\
+	}								\
+} while (0)
+#else
+#define dwc2_sample_frrem(_hcd_, _qh_, _letter_)	do {} while (0)
+#endif
+
+#endif /* __DWC2_HCD_H__ */
diff --git a/drivers/staging/dwc2/hcd_ddma.c b/drivers/staging/dwc2/hcd_ddma.c
new file mode 100644
index 00000000000..5c0fd273a7b
--- /dev/null
+++ b/drivers/staging/dwc2/hcd_ddma.c
@@ -0,0 +1,1196 @@
+/*
+ * hcd_ddma.c - DesignWare HS OTG Controller descriptor DMA routines
+ *
+ * Copyright (C) 2004-2013 Synopsys, Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions, and the following disclaimer,
+ *    without modification.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The names of the above-listed copyright holders may not be used
+ *    to endorse or promote products derived from this software without
+ *    specific prior written permission.
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation; either version 2 of the License, or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+ * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * This file contains the Descriptor DMA implementation for Host mode
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/spinlock.h>
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/usb.h>
+
+#include <linux/usb/hcd.h>
+#include <linux/usb/ch11.h>
+
+#include "core.h"
+#include "hcd.h"
+
+static u16 dwc2_frame_list_idx(u16 frame)
+{
+	return frame & (FRLISTEN_64_SIZE - 1);
+}
+
+static u16 dwc2_desclist_idx_inc(u16 idx, u16 inc, u8 speed)
+{
+	return (idx + inc) &
+		((speed == USB_SPEED_HIGH ? MAX_DMA_DESC_NUM_HS_ISOC :
+		  MAX_DMA_DESC_NUM_GENERIC) - 1);
+}
+
+static u16 dwc2_desclist_idx_dec(u16 idx, u16 inc, u8 speed)
+{
+	return (idx - inc) &
+		((speed == USB_SPEED_HIGH ? MAX_DMA_DESC_NUM_HS_ISOC :
+		  MAX_DMA_DESC_NUM_GENERIC) - 1);
+}
+
+static u16 dwc2_max_desc_num(struct dwc2_qh *qh)
+{
+	return (qh->ep_type == USB_ENDPOINT_XFER_ISOC &&
+		qh->dev_speed == USB_SPEED_HIGH) ?
+		MAX_DMA_DESC_NUM_HS_ISOC : MAX_DMA_DESC_NUM_GENERIC;
+}
+
+static u16 dwc2_frame_incr_val(struct dwc2_qh *qh)
+{
+	return qh->dev_speed == USB_SPEED_HIGH ?
+	       (qh->interval + 8 - 1) / 8 : qh->interval;
+}
+
+static int dwc2_desc_list_alloc(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh,
+				gfp_t flags)
+{
+	qh->desc_list = dma_alloc_coherent(hsotg->dev,
+				sizeof(struct dwc2_hcd_dma_desc) *
+				dwc2_max_desc_num(qh), &qh->desc_list_dma,
+				flags);
+
+	if (!qh->desc_list)
+		return -ENOMEM;
+
+	memset(qh->desc_list, 0,
+	       sizeof(struct dwc2_hcd_dma_desc) * dwc2_max_desc_num(qh));
+
+	qh->n_bytes = kzalloc(sizeof(u32) * dwc2_max_desc_num(qh), flags);
+	if (!qh->n_bytes) {
+		dma_free_coherent(hsotg->dev, sizeof(struct dwc2_hcd_dma_desc)
+				  * dwc2_max_desc_num(qh), qh->desc_list,
+				  qh->desc_list_dma);
+		qh->desc_list = NULL;
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static void dwc2_desc_list_free(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh)
+{
+	if (qh->desc_list) {
+		dma_free_coherent(hsotg->dev, sizeof(struct dwc2_hcd_dma_desc)
+				  * dwc2_max_desc_num(qh), qh->desc_list,
+				  qh->desc_list_dma);
+		qh->desc_list = NULL;
+	}
+
+	kfree(qh->n_bytes);
+	qh->n_bytes = NULL;
+}
+
+static int dwc2_frame_list_alloc(struct dwc2_hsotg *hsotg, gfp_t mem_flags)
+{
+	if (hsotg->frame_list)
+		return 0;
+
+	hsotg->frame_list = dma_alloc_coherent(hsotg->dev,
+					       4 * FRLISTEN_64_SIZE,
+					       &hsotg->frame_list_dma,
+					       mem_flags);
+	if (!hsotg->frame_list)
+		return -ENOMEM;
+
+	memset(hsotg->frame_list, 0, 4 * FRLISTEN_64_SIZE);
+	return 0;
+}
+
+static void dwc2_frame_list_free(struct dwc2_hsotg *hsotg)
+{
+	u32 *frame_list;
+	dma_addr_t frame_list_dma;
+	unsigned long flags;
+
+	spin_lock_irqsave(&hsotg->lock, flags);
+
+	if (!hsotg->frame_list) {
+		spin_unlock_irqrestore(&hsotg->lock, flags);
+		return;
+	}
+
+	frame_list = hsotg->frame_list;
+	frame_list_dma = hsotg->frame_list_dma;
+	hsotg->frame_list = NULL;
+
+	spin_unlock_irqrestore(&hsotg->lock, flags);
+
+	dma_free_coherent(hsotg->dev, 4 * FRLISTEN_64_SIZE, frame_list,
+			  frame_list_dma);
+}
+
+static void dwc2_per_sched_enable(struct dwc2_hsotg *hsotg, u32 fr_list_en)
+{
+	u32 hcfg;
+	unsigned long flags;
+
+	spin_lock_irqsave(&hsotg->lock, flags);
+
+	hcfg = readl(hsotg->regs + HCFG);
+	if (hcfg & HCFG_PERSCHEDENA) {
+		/* already enabled */
+		spin_unlock_irqrestore(&hsotg->lock, flags);
+		return;
+	}
+
+	writel(hsotg->frame_list_dma, hsotg->regs + HFLBADDR);
+
+	hcfg &= ~HCFG_FRLISTEN_MASK;
+	hcfg |= fr_list_en | HCFG_PERSCHEDENA;
+	dev_vdbg(hsotg->dev, "Enabling Periodic schedule\n");
+	writel(hcfg, hsotg->regs + HCFG);
+
+	spin_unlock_irqrestore(&hsotg->lock, flags);
+}
+
+static void dwc2_per_sched_disable(struct dwc2_hsotg *hsotg)
+{
+	u32 hcfg;
+	unsigned long flags;
+
+	spin_lock_irqsave(&hsotg->lock, flags);
+
+	hcfg = readl(hsotg->regs + HCFG);
+	if (!(hcfg & HCFG_PERSCHEDENA)) {
+		/* already disabled */
+		spin_unlock_irqrestore(&hsotg->lock, flags);
+		return;
+	}
+
+	hcfg &= ~HCFG_PERSCHEDENA;
+	dev_vdbg(hsotg->dev, "Disabling Periodic schedule\n");
+	writel(hcfg, hsotg->regs + HCFG);
+
+	spin_unlock_irqrestore(&hsotg->lock, flags);
+}
+
+/*
+ * Activates/Deactivates FrameList entries for the channel based on endpoint
+ * servicing period
+ */
+static void dwc2_update_frame_list(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh,
+				   int enable)
+{
+	struct dwc2_host_chan *chan;
+	u16 i, j, inc;
+
+	if (!hsotg) {
+		pr_err("hsotg = %p\n", hsotg);
+		return;
+	}
+
+	if (!qh->channel) {
+		dev_err(hsotg->dev, "qh->channel = %p\n", qh->channel);
+		return;
+	}
+
+	if (!hsotg->frame_list) {
+		dev_err(hsotg->dev, "hsotg->frame_list = %p\n",
+			hsotg->frame_list);
+		return;
+	}
+
+	chan = qh->channel;
+	inc = dwc2_frame_incr_val(qh);
+	if (qh->ep_type == USB_ENDPOINT_XFER_ISOC)
+		i = dwc2_frame_list_idx(qh->sched_frame);
+	else
+		i = 0;
+
+	j = i;
+	do {
+		if (enable)
+			hsotg->frame_list[j] |= 1 << chan->hc_num;
+		else
+			hsotg->frame_list[j] &= ~(1 << chan->hc_num);
+		j = (j + inc) & (FRLISTEN_64_SIZE - 1);
+	} while (j != i);
+
+	if (!enable)
+		return;
+
+	chan->schinfo = 0;
+	if (chan->speed == USB_SPEED_HIGH && qh->interval) {
+		j = 1;
+		/* TODO - check this */
+		inc = (8 + qh->interval - 1) / qh->interval;
+		for (i = 0; i < inc; i++) {
+			chan->schinfo |= j;
+			j = j << qh->interval;
+		}
+	} else {
+		chan->schinfo = 0xff;
+	}
+}
+
+static void dwc2_release_channel_ddma(struct dwc2_hsotg *hsotg,
+				      struct dwc2_qh *qh)
+{
+	struct dwc2_host_chan *chan = qh->channel;
+
+	if (dwc2_qh_is_non_per(qh))
+		hsotg->non_periodic_channels--;
+	else
+		dwc2_update_frame_list(hsotg, qh, 0);
+
+	/*
+	 * The condition is added to prevent double cleanup try in case of
+	 * device disconnect. See channel cleanup in dwc2_hcd_disconnect().
+	 */
+	if (chan->qh) {
+		if (!list_empty(&chan->hc_list_entry))
+			list_del(&chan->hc_list_entry);
+		dwc2_hc_cleanup(hsotg, chan);
+		list_add_tail(&chan->hc_list_entry, &hsotg->free_hc_list);
+		chan->qh = NULL;
+	}
+
+	qh->channel = NULL;
+	qh->ntd = 0;
+
+	if (qh->desc_list)
+		memset(qh->desc_list, 0, sizeof(struct dwc2_hcd_dma_desc) *
+		       dwc2_max_desc_num(qh));
+}
+
+/**
+ * dwc2_hcd_qh_init_ddma() - Initializes a QH structure's Descriptor DMA
+ * related members
+ *
+ * @hsotg: The HCD state structure for the DWC OTG controller
+ * @qh:    The QH to init
+ *
+ * Return: 0 if successful, negative error code otherwise
+ *
+ * Allocates memory for the descriptor list. For the first periodic QH,
+ * allocates memory for the FrameList and enables periodic scheduling.
+ */
+int dwc2_hcd_qh_init_ddma(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh,
+			  gfp_t mem_flags)
+{
+	int retval;
+
+	if (qh->do_split) {
+		dev_err(hsotg->dev,
+			"SPLIT Transfers are not supported in Descriptor DMA mode.\n");
+		retval = -EINVAL;
+		goto err0;
+	}
+
+	retval = dwc2_desc_list_alloc(hsotg, qh, mem_flags);
+	if (retval)
+		goto err0;
+
+	if (qh->ep_type == USB_ENDPOINT_XFER_ISOC ||
+	    qh->ep_type == USB_ENDPOINT_XFER_INT) {
+		if (!hsotg->frame_list) {
+			retval = dwc2_frame_list_alloc(hsotg, mem_flags);
+			if (retval)
+				goto err1;
+			/* Enable periodic schedule on first periodic QH */
+			dwc2_per_sched_enable(hsotg, HCFG_FRLISTEN_64);
+		}
+	}
+
+	qh->ntd = 0;
+	return 0;
+
+err1:
+	dwc2_desc_list_free(hsotg, qh);
+err0:
+	return retval;
+}
+
+/**
+ * dwc2_hcd_qh_free_ddma() - Frees a QH structure's Descriptor DMA related
+ * members
+ *
+ * @hsotg: The HCD state structure for the DWC OTG controller
+ * @qh:    The QH to free
+ *
+ * Frees descriptor list memory associated with the QH. If QH is periodic and
+ * the last, frees FrameList memory and disables periodic scheduling.
+ */
+void dwc2_hcd_qh_free_ddma(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh)
+{
+	dwc2_desc_list_free(hsotg, qh);
+
+	/*
+	 * Channel still assigned due to some reasons.
+	 * Seen on Isoc URB dequeue. Channel halted but no subsequent
+	 * ChHalted interrupt to release the channel. Afterwards
+	 * when it comes here from endpoint disable routine
+	 * channel remains assigned.
+	 */
+	if (qh->channel)
+		dwc2_release_channel_ddma(hsotg, qh);
+
+	if ((qh->ep_type == USB_ENDPOINT_XFER_ISOC ||
+	     qh->ep_type == USB_ENDPOINT_XFER_INT) &&
+	    !hsotg->periodic_channels && hsotg->frame_list) {
+		dwc2_per_sched_disable(hsotg);
+		dwc2_frame_list_free(hsotg);
+	}
+}
+
+static u8 dwc2_frame_to_desc_idx(struct dwc2_qh *qh, u16 frame_idx)
+{
+	if (qh->dev_speed == USB_SPEED_HIGH)
+		/* Descriptor set (8 descriptors) index which is 8-aligned */
+		return (frame_idx & ((MAX_DMA_DESC_NUM_HS_ISOC / 8) - 1)) * 8;
+	else
+		return frame_idx & (MAX_DMA_DESC_NUM_GENERIC - 1);
+}
+
+/*
+ * Determine starting frame for Isochronous transfer.
+ * Few frames skipped to prevent race condition with HC.
+ */
+static u16 dwc2_calc_starting_frame(struct dwc2_hsotg *hsotg,
+				    struct dwc2_qh *qh, u16 *skip_frames)
+{
+	u16 frame;
+
+	hsotg->frame_number = dwc2_hcd_get_frame_number(hsotg);
+
+	/* sched_frame is always frame number (not uFrame) both in FS and HS! */
+
+	/*
+	 * skip_frames is used to limit activated descriptors number
+	 * to avoid the situation when HC services the last activated
+	 * descriptor firstly.
+	 * Example for FS:
+	 * Current frame is 1, scheduled frame is 3. Since HC always fetches
+	 * the descriptor corresponding to curr_frame+1, the descriptor
+	 * corresponding to frame 2 will be fetched. If the number of
+	 * descriptors is max=64 (or greather) the list will be fully programmed
+	 * with Active descriptors and it is possible case (rare) that the
+	 * latest descriptor(considering rollback) corresponding to frame 2 will
+	 * be serviced first. HS case is more probable because, in fact, up to
+	 * 11 uframes (16 in the code) may be skipped.
+	 */
+	if (qh->dev_speed == USB_SPEED_HIGH) {
+		/*
+		 * Consider uframe counter also, to start xfer asap. If half of
+		 * the frame elapsed skip 2 frames otherwise just 1 frame.
+		 * Starting descriptor index must be 8-aligned, so if the
+		 * current frame is near to complete the next one is skipped as
+		 * well.
+		 */
+		if (dwc2_micro_frame_num(hsotg->frame_number) >= 5) {
+			*skip_frames = 2 * 8;
+			frame = dwc2_frame_num_inc(hsotg->frame_number,
+						   *skip_frames);
+		} else {
+			*skip_frames = 1 * 8;
+			frame = dwc2_frame_num_inc(hsotg->frame_number,
+						   *skip_frames);
+		}
+
+		frame = dwc2_full_frame_num(frame);
+	} else {
+		/*
+		 * Two frames are skipped for FS - the current and the next.
+		 * But for descriptor programming, 1 frame (descriptor) is
+		 * enough, see example above.
+		 */
+		*skip_frames = 1;
+		frame = dwc2_frame_num_inc(hsotg->frame_number, 2);
+	}
+
+	return frame;
+}
+
+/*
+ * Calculate initial descriptor index for isochronous transfer based on
+ * scheduled frame
+ */
+static u16 dwc2_recalc_initial_desc_idx(struct dwc2_hsotg *hsotg,
+					struct dwc2_qh *qh)
+{
+	u16 frame, fr_idx, fr_idx_tmp, skip_frames;
+
+	/*
+	 * With current ISOC processing algorithm the channel is being released
+	 * when no more QTDs in the list (qh->ntd == 0). Thus this function is
+	 * called only when qh->ntd == 0 and qh->channel == 0.
+	 *
+	 * So qh->channel != NULL branch is not used and just not removed from
+	 * the source file. It is required for another possible approach which
+	 * is, do not disable and release the channel when ISOC session
+	 * completed, just move QH to inactive schedule until new QTD arrives.
+	 * On new QTD, the QH moved back to 'ready' schedule, starting frame and
+	 * therefore starting desc_index are recalculated. In this case channel
+	 * is released only on ep_disable.
+	 */
+
+	/*
+	 * Calculate starting descriptor index. For INTERRUPT endpoint it is
+	 * always 0.
+	 */
+	if (qh->channel) {
+		frame = dwc2_calc_starting_frame(hsotg, qh, &skip_frames);
+		/*
+		 * Calculate initial descriptor index based on FrameList current
+		 * bitmap and servicing period
+		 */
+		fr_idx_tmp = dwc2_frame_list_idx(frame);
+		fr_idx = (FRLISTEN_64_SIZE +
+			  dwc2_frame_list_idx(qh->sched_frame) - fr_idx_tmp)
+			 % dwc2_frame_incr_val(qh);
+		fr_idx = (fr_idx + fr_idx_tmp) % FRLISTEN_64_SIZE;
+	} else {
+		qh->sched_frame = dwc2_calc_starting_frame(hsotg, qh,
+							   &skip_frames);
+		fr_idx = dwc2_frame_list_idx(qh->sched_frame);
+	}
+
+	qh->td_first = qh->td_last = dwc2_frame_to_desc_idx(qh, fr_idx);
+
+	return skip_frames;
+}
+
+#define ISOC_URB_GIVEBACK_ASAP
+
+#define MAX_ISOC_XFER_SIZE_FS	1023
+#define MAX_ISOC_XFER_SIZE_HS	3072
+#define DESCNUM_THRESHOLD	4
+
+static void dwc2_fill_host_isoc_dma_desc(struct dwc2_hsotg *hsotg,
+					 struct dwc2_qtd *qtd,
+					 struct dwc2_qh *qh, u32 max_xfer_size,
+					 u16 idx)
+{
+	struct dwc2_hcd_dma_desc *dma_desc = &qh->desc_list[idx];
+	struct dwc2_hcd_iso_packet_desc *frame_desc;
+
+	memset(dma_desc, 0, sizeof(*dma_desc));
+	frame_desc = &qtd->urb->iso_descs[qtd->isoc_frame_index_last];
+
+	if (frame_desc->length > max_xfer_size)
+		qh->n_bytes[idx] = max_xfer_size;
+	else
+		qh->n_bytes[idx] = frame_desc->length;
+
+	dma_desc->buf = (u32)(qtd->urb->dma + frame_desc->offset);
+	dma_desc->status = qh->n_bytes[idx] << HOST_DMA_ISOC_NBYTES_SHIFT &
+			   HOST_DMA_ISOC_NBYTES_MASK;
+
+#ifdef ISOC_URB_GIVEBACK_ASAP
+	/* Set IOC for each descriptor corresponding to last frame of URB */
+	if (qtd->isoc_frame_index_last == qtd->urb->packet_count)
+		dma_desc->status |= HOST_DMA_IOC;
+#endif
+
+	qh->ntd++;
+	qtd->isoc_frame_index_last++;
+}
+
+static void dwc2_init_isoc_dma_desc(struct dwc2_hsotg *hsotg,
+				    struct dwc2_qh *qh, u16 skip_frames)
+{
+	struct dwc2_qtd *qtd;
+	u32 max_xfer_size;
+	u16 idx, inc, n_desc, ntd_max = 0;
+
+	idx = qh->td_last;
+	inc = qh->interval;
+	n_desc = 0;
+
+	if (qh->interval) {
+		ntd_max = (dwc2_max_desc_num(qh) + qh->interval - 1) /
+				qh->interval;
+		if (skip_frames && !qh->channel)
+			ntd_max -= skip_frames / qh->interval;
+	}
+
+	max_xfer_size = qh->dev_speed == USB_SPEED_HIGH ?
+			MAX_ISOC_XFER_SIZE_HS : MAX_ISOC_XFER_SIZE_FS;
+
+	list_for_each_entry(qtd, &qh->qtd_list, qtd_list_entry) {
+		while (qh->ntd < ntd_max && qtd->isoc_frame_index_last <
+						qtd->urb->packet_count) {
+			if (n_desc > 1)
+				qh->desc_list[n_desc - 1].status |= HOST_DMA_A;
+			dwc2_fill_host_isoc_dma_desc(hsotg, qtd, qh,
+						     max_xfer_size, idx);
+			idx = dwc2_desclist_idx_inc(idx, inc, qh->dev_speed);
+			n_desc++;
+		}
+		qtd->in_process = 1;
+	}
+
+	qh->td_last = idx;
+
+#ifdef ISOC_URB_GIVEBACK_ASAP
+	/* Set IOC for last descriptor if descriptor list is full */
+	if (qh->ntd == ntd_max) {
+		idx = dwc2_desclist_idx_dec(qh->td_last, inc, qh->dev_speed);
+		qh->desc_list[idx].status |= HOST_DMA_IOC;
+	}
+#else
+	/*
+	 * Set IOC bit only for one descriptor. Always try to be ahead of HW
+	 * processing, i.e. on IOC generation driver activates next descriptor
+	 * but core continues to process descriptors following the one with IOC
+	 * set.
+	 */
+
+	if (n_desc > DESCNUM_THRESHOLD)
+		/*
+		 * Move IOC "up". Required even if there is only one QTD
+		 * in the list, because QTDs might continue to be queued,
+		 * but during the activation it was only one queued.
+		 * Actually more than one QTD might be in the list if this
+		 * function called from XferCompletion - QTDs was queued during
+		 * HW processing of the previous descriptor chunk.
+		 */
+		idx = dwc2_desclist_idx_dec(idx, inc * ((qh->ntd + 1) / 2),
+					    qh->dev_speed);
+	else
+		/*
+		 * Set the IOC for the latest descriptor if either number of
+		 * descriptors is not greater than threshold or no more new
+		 * descriptors activated
+		 */
+		idx = dwc2_desclist_idx_dec(qh->td_last, inc, qh->dev_speed);
+
+	qh->desc_list[idx].status |= HOST_DMA_IOC;
+#endif
+
+	if (n_desc) {
+		qh->desc_list[n_desc - 1].status |= HOST_DMA_A;
+		if (n_desc > 1)
+			qh->desc_list[0].status |= HOST_DMA_A;
+	}
+}
+
+static void dwc2_fill_host_dma_desc(struct dwc2_hsotg *hsotg,
+				    struct dwc2_host_chan *chan,
+				    struct dwc2_qtd *qtd, struct dwc2_qh *qh,
+				    int n_desc)
+{
+	struct dwc2_hcd_dma_desc *dma_desc = &qh->desc_list[n_desc];
+	int len = chan->xfer_len;
+
+	if (len > MAX_DMA_DESC_SIZE)
+		len = MAX_DMA_DESC_SIZE - chan->max_packet + 1;
+
+	if (chan->ep_is_in) {
+		int num_packets;
+
+		if (len > 0 && chan->max_packet)
+			num_packets = (len + chan->max_packet - 1)
+					/ chan->max_packet;
+		else
+			/* Need 1 packet for transfer length of 0 */
+			num_packets = 1;
+
+		/* Always program an integral # of packets for IN transfers */
+		len = num_packets * chan->max_packet;
+	}
+
+	dma_desc->status = len << HOST_DMA_NBYTES_SHIFT & HOST_DMA_NBYTES_MASK;
+	qh->n_bytes[n_desc] = len;
+
+	if (qh->ep_type == USB_ENDPOINT_XFER_CONTROL &&
+	    qtd->control_phase == DWC2_CONTROL_SETUP)
+		dma_desc->status |= HOST_DMA_SUP;
+
+	dma_desc->buf = (u32)chan->xfer_dma;
+
+	/*
+	 * Last (or only) descriptor of IN transfer with actual size less
+	 * than MaxPacket
+	 */
+	if (len > chan->xfer_len) {
+		chan->xfer_len = 0;
+	} else {
+		chan->xfer_dma += len;
+		chan->xfer_len -= len;
+	}
+}
+
+static void dwc2_init_non_isoc_dma_desc(struct dwc2_hsotg *hsotg,
+					struct dwc2_qh *qh)
+{
+	struct dwc2_qtd *qtd;
+	struct dwc2_host_chan *chan = qh->channel;
+	int n_desc = 0;
+
+	dev_vdbg(hsotg->dev, "%s(): qh=%p dma=%08lx len=%d\n", __func__, qh,
+		 (unsigned long)chan->xfer_dma, chan->xfer_len);
+
+	/*
+	 * Start with chan->xfer_dma initialized in assign_and_init_hc(), then
+	 * if SG transfer consists of multiple URBs, this pointer is re-assigned
+	 * to the buffer of the currently processed QTD. For non-SG request
+	 * there is always one QTD active.
+	 */
+
+	list_for_each_entry(qtd, &qh->qtd_list, qtd_list_entry) {
+		dev_vdbg(hsotg->dev, "qtd=%p\n", qtd);
+
+		if (n_desc) {
+			/* SG request - more than 1 QTD */
+			chan->xfer_dma = qtd->urb->dma +
+					qtd->urb->actual_length;
+			chan->xfer_len = qtd->urb->length -
+					qtd->urb->actual_length;
+			dev_vdbg(hsotg->dev, "buf=%08lx len=%d\n",
+				 (unsigned long)chan->xfer_dma, chan->xfer_len);
+		}
+
+		qtd->n_desc = 0;
+		do {
+			if (n_desc > 1) {
+				qh->desc_list[n_desc - 1].status |= HOST_DMA_A;
+				dev_vdbg(hsotg->dev,
+					 "set A bit in desc %d (%p)\n",
+					 n_desc - 1,
+					 &qh->desc_list[n_desc - 1]);
+			}
+			dwc2_fill_host_dma_desc(hsotg, chan, qtd, qh, n_desc);
+			dev_vdbg(hsotg->dev,
+				 "desc %d (%p) buf=%08x status=%08x\n",
+				 n_desc, &qh->desc_list[n_desc],
+				 qh->desc_list[n_desc].buf,
+				 qh->desc_list[n_desc].status);
+			qtd->n_desc++;
+			n_desc++;
+		} while (chan->xfer_len > 0 &&
+			 n_desc != MAX_DMA_DESC_NUM_GENERIC);
+
+		dev_vdbg(hsotg->dev, "n_desc=%d\n", n_desc);
+		qtd->in_process = 1;
+		if (qh->ep_type == USB_ENDPOINT_XFER_CONTROL)
+			break;
+		if (n_desc == MAX_DMA_DESC_NUM_GENERIC)
+			break;
+	}
+
+	if (n_desc) {
+		qh->desc_list[n_desc - 1].status |=
+				HOST_DMA_IOC | HOST_DMA_EOL | HOST_DMA_A;
+		dev_vdbg(hsotg->dev, "set IOC/EOL/A bits in desc %d (%p)\n",
+			 n_desc - 1, &qh->desc_list[n_desc - 1]);
+		if (n_desc > 1) {
+			qh->desc_list[0].status |= HOST_DMA_A;
+			dev_vdbg(hsotg->dev, "set A bit in desc 0 (%p)\n",
+				 &qh->desc_list[0]);
+		}
+		chan->ntd = n_desc;
+	}
+}
+
+/**
+ * dwc2_hcd_start_xfer_ddma() - Starts a transfer in Descriptor DMA mode
+ *
+ * @hsotg: The HCD state structure for the DWC OTG controller
+ * @qh:    The QH to init
+ *
+ * Return: 0 if successful, negative error code otherwise
+ *
+ * For Control and Bulk endpoints, initializes descriptor list and starts the
+ * transfer. For Interrupt and Isochronous endpoints, initializes descriptor
+ * list then updates FrameList, marking appropriate entries as active.
+ *
+ * For Isochronous endpoints the starting descriptor index is calculated based
+ * on the scheduled frame, but only on the first transfer descriptor within a
+ * session. Then the transfer is started via enabling the channel.
+ *
+ * For Isochronous endpoints the channel is not halted on XferComplete
+ * interrupt so remains assigned to the endpoint(QH) until session is done.
+ */
+void dwc2_hcd_start_xfer_ddma(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh)
+{
+	/* Channel is already assigned */
+	struct dwc2_host_chan *chan = qh->channel;
+	u16 skip_frames = 0;
+
+	switch (chan->ep_type) {
+	case USB_ENDPOINT_XFER_CONTROL:
+	case USB_ENDPOINT_XFER_BULK:
+		dwc2_init_non_isoc_dma_desc(hsotg, qh);
+		dwc2_hc_start_transfer_ddma(hsotg, chan);
+		break;
+	case USB_ENDPOINT_XFER_INT:
+		dwc2_init_non_isoc_dma_desc(hsotg, qh);
+		dwc2_update_frame_list(hsotg, qh, 1);
+		dwc2_hc_start_transfer_ddma(hsotg, chan);
+		break;
+	case USB_ENDPOINT_XFER_ISOC:
+		if (!qh->ntd)
+			skip_frames = dwc2_recalc_initial_desc_idx(hsotg, qh);
+		dwc2_init_isoc_dma_desc(hsotg, qh, skip_frames);
+
+		if (!chan->xfer_started) {
+			dwc2_update_frame_list(hsotg, qh, 1);
+
+			/*
+			 * Always set to max, instead of actual size. Otherwise
+			 * ntd will be changed with channel being enabled. Not
+			 * recommended.
+			 */
+			chan->ntd = dwc2_max_desc_num(qh);
+
+			/* Enable channel only once for ISOC */
+			dwc2_hc_start_transfer_ddma(hsotg, chan);
+		}
+
+		break;
+	default:
+		break;
+	}
+}
+
+#define DWC2_CMPL_DONE		1
+#define DWC2_CMPL_STOP		2
+
+static int dwc2_cmpl_host_isoc_dma_desc(struct dwc2_hsotg *hsotg,
+					struct dwc2_host_chan *chan,
+					struct dwc2_qtd *qtd,
+					struct dwc2_qh *qh, u16 idx)
+{
+	struct dwc2_hcd_dma_desc *dma_desc = &qh->desc_list[idx];
+	struct dwc2_hcd_iso_packet_desc *frame_desc;
+	u16 remain = 0;
+	int rc = 0;
+
+	frame_desc = &qtd->urb->iso_descs[qtd->isoc_frame_index_last];
+	dma_desc->buf = (u32)(qtd->urb->dma + frame_desc->offset);
+	if (chan->ep_is_in)
+		remain = dma_desc->status >> HOST_DMA_ISOC_NBYTES_SHIFT &
+			HOST_DMA_ISOC_NBYTES_MASK >> HOST_DMA_ISOC_NBYTES_SHIFT;
+
+	if ((dma_desc->status & HOST_DMA_STS_MASK) == HOST_DMA_STS_PKTERR) {
+		/*
+		 * XactError, or unable to complete all the transactions
+		 * in the scheduled micro-frame/frame, both indicated by
+		 * HOST_DMA_STS_PKTERR
+		 */
+		qtd->urb->error_count++;
+		frame_desc->actual_length = qh->n_bytes[idx] - remain;
+		frame_desc->status = -EPROTO;
+	} else {
+		/* Success */
+		frame_desc->actual_length = qh->n_bytes[idx] - remain;
+		frame_desc->status = 0;
+	}
+
+	if (++qtd->isoc_frame_index == qtd->urb->packet_count) {
+		/*
+		 * urb->status is not used for isoc transfers here. The
+		 * individual frame_desc status are used instead.
+		 */
+		dwc2_host_complete(hsotg, qtd->urb->priv, qtd->urb, 0);
+		dwc2_hcd_qtd_unlink_and_free(hsotg, qtd, qh);
+
+		/*
+		 * This check is necessary because urb_dequeue can be called
+		 * from urb complete callback (sound driver for example). All
+		 * pending URBs are dequeued there, so no need for further
+		 * processing.
+		 */
+		if (chan->halt_status == DWC2_HC_XFER_URB_DEQUEUE)
+			return -1;
+		rc = DWC2_CMPL_DONE;
+	}
+
+	qh->ntd--;
+
+	/* Stop if IOC requested descriptor reached */
+	if (dma_desc->status & HOST_DMA_IOC)
+		rc = DWC2_CMPL_STOP;
+
+	return rc;
+}
+
+static void dwc2_complete_isoc_xfer_ddma(struct dwc2_hsotg *hsotg,
+					 struct dwc2_host_chan *chan,
+					 enum dwc2_halt_status halt_status)
+{
+	struct dwc2_hcd_iso_packet_desc *frame_desc;
+	struct dwc2_qtd *qtd, *qtd_tmp;
+	struct dwc2_qh *qh;
+	u16 idx;
+	int rc;
+
+	qh = chan->qh;
+	idx = qh->td_first;
+
+	if (chan->halt_status == DWC2_HC_XFER_URB_DEQUEUE) {
+		list_for_each_entry(qtd, &qh->qtd_list, qtd_list_entry)
+			qtd->in_process = 0;
+		return;
+	}
+
+	if (halt_status == DWC2_HC_XFER_AHB_ERR ||
+	    halt_status == DWC2_HC_XFER_BABBLE_ERR) {
+		/*
+		 * Channel is halted in these error cases, considered as serious
+		 * issues.
+		 * Complete all URBs marking all frames as failed, irrespective
+		 * whether some of the descriptors (frames) succeeded or not.
+		 * Pass error code to completion routine as well, to update
+		 * urb->status, some of class drivers might use it to stop
+		 * queing transfer requests.
+		 */
+		int err = halt_status == DWC2_HC_XFER_AHB_ERR ?
+			  -EIO : -EOVERFLOW;
+
+		list_for_each_entry_safe(qtd, qtd_tmp, &qh->qtd_list,
+					 qtd_list_entry) {
+			for (idx = 0; idx < qtd->urb->packet_count; idx++) {
+				frame_desc = &qtd->urb->iso_descs[idx];
+				frame_desc->status = err;
+			}
+
+			dwc2_host_complete(hsotg, qtd->urb->priv, qtd->urb,
+					   err);
+			dwc2_hcd_qtd_unlink_and_free(hsotg, qtd, qh);
+		}
+
+		return;
+	}
+
+	list_for_each_entry_safe(qtd, qtd_tmp, &qh->qtd_list, qtd_list_entry) {
+		if (!qtd->in_process)
+			break;
+		do {
+			rc = dwc2_cmpl_host_isoc_dma_desc(hsotg, chan, qtd, qh,
+							  idx);
+			if (rc < 0)
+				return;
+			idx = dwc2_desclist_idx_inc(idx, qh->interval,
+						    chan->speed);
+			if (rc == DWC2_CMPL_STOP)
+				goto stop_scan;
+			if (rc == DWC2_CMPL_DONE)
+				break;
+		} while (idx != qh->td_first);
+	}
+
+stop_scan:
+	qh->td_first = idx;
+}
+
+static int dwc2_update_non_isoc_urb_state_ddma(struct dwc2_hsotg *hsotg,
+					struct dwc2_host_chan *chan,
+					struct dwc2_qtd *qtd,
+					struct dwc2_hcd_dma_desc *dma_desc,
+					enum dwc2_halt_status halt_status,
+					u32 n_bytes, int *xfer_done)
+{
+	struct dwc2_hcd_urb *urb = qtd->urb;
+	u16 remain = 0;
+
+	if (chan->ep_is_in)
+		remain = dma_desc->status >> HOST_DMA_NBYTES_SHIFT &
+			 HOST_DMA_NBYTES_MASK >> HOST_DMA_NBYTES_SHIFT;
+
+	dev_vdbg(hsotg->dev, "remain=%d dwc2_urb=%p\n", remain, urb);
+
+	if (halt_status == DWC2_HC_XFER_AHB_ERR) {
+		dev_err(hsotg->dev, "EIO\n");
+		urb->status = -EIO;
+		return 1;
+	}
+
+	if ((dma_desc->status & HOST_DMA_STS_MASK) == HOST_DMA_STS_PKTERR) {
+		switch (halt_status) {
+		case DWC2_HC_XFER_STALL:
+			dev_vdbg(hsotg->dev, "Stall\n");
+			urb->status = -EPIPE;
+			break;
+		case DWC2_HC_XFER_BABBLE_ERR:
+			dev_err(hsotg->dev, "Babble\n");
+			urb->status = -EOVERFLOW;
+			break;
+		case DWC2_HC_XFER_XACT_ERR:
+			dev_err(hsotg->dev, "XactErr\n");
+			urb->status = -EPROTO;
+			break;
+		default:
+			dev_err(hsotg->dev,
+				"%s: Unhandled descriptor error status (%d)\n",
+				__func__, halt_status);
+			break;
+		}
+		return 1;
+	}
+
+	if (dma_desc->status & HOST_DMA_A) {
+		dev_vdbg(hsotg->dev,
+			 "Active descriptor encountered on channel %d\n",
+			 chan->hc_num);
+		return 0;
+	}
+
+	if (chan->ep_type == USB_ENDPOINT_XFER_CONTROL) {
+		if (qtd->control_phase == DWC2_CONTROL_DATA) {
+			urb->actual_length += n_bytes - remain;
+			if (remain || urb->actual_length >= urb->length) {
+				/*
+				 * For Control Data stage do not set urb->status
+				 * to 0, to prevent URB callback. Set it when
+				 * Status phase is done. See below.
+				 */
+				*xfer_done = 1;
+			}
+		} else if (qtd->control_phase == DWC2_CONTROL_STATUS) {
+			urb->status = 0;
+			*xfer_done = 1;
+		}
+		/* No handling for SETUP stage */
+	} else {
+		/* BULK and INTR */
+		urb->actual_length += n_bytes - remain;
+		dev_vdbg(hsotg->dev, "length=%d actual=%d\n", urb->length,
+			 urb->actual_length);
+		if (remain || urb->actual_length >= urb->length) {
+			urb->status = 0;
+			*xfer_done = 1;
+		}
+	}
+
+	return 0;
+}
+
+static int dwc2_process_non_isoc_desc(struct dwc2_hsotg *hsotg,
+				      struct dwc2_host_chan *chan,
+				      int chnum, struct dwc2_qtd *qtd,
+				      int desc_num,
+				      enum dwc2_halt_status halt_status,
+				      int *xfer_done)
+{
+	struct dwc2_qh *qh = chan->qh;
+	struct dwc2_hcd_urb *urb = qtd->urb;
+	struct dwc2_hcd_dma_desc *dma_desc;
+	u32 n_bytes;
+	int failed;
+
+	dev_vdbg(hsotg->dev, "%s()\n", __func__);
+
+	dma_desc = &qh->desc_list[desc_num];
+	n_bytes = qh->n_bytes[desc_num];
+	dev_vdbg(hsotg->dev,
+		 "qtd=%p dwc2_urb=%p desc_num=%d desc=%p n_bytes=%d\n",
+		 qtd, urb, desc_num, dma_desc, n_bytes);
+	failed = dwc2_update_non_isoc_urb_state_ddma(hsotg, chan, qtd, dma_desc,
+						     halt_status, n_bytes,
+						     xfer_done);
+	if (failed || (*xfer_done && urb->status != -EINPROGRESS)) {
+		dwc2_host_complete(hsotg, urb->priv, urb, urb->status);
+		dwc2_hcd_qtd_unlink_and_free(hsotg, qtd, qh);
+		dev_vdbg(hsotg->dev, "failed=%1x xfer_done=%1x status=%08x\n",
+			 failed, *xfer_done, urb->status);
+		return failed;
+	}
+
+	if (qh->ep_type == USB_ENDPOINT_XFER_CONTROL) {
+		switch (qtd->control_phase) {
+		case DWC2_CONTROL_SETUP:
+			if (urb->length > 0)
+				qtd->control_phase = DWC2_CONTROL_DATA;
+			else
+				qtd->control_phase = DWC2_CONTROL_STATUS;
+			dev_vdbg(hsotg->dev,
+				 "  Control setup transaction done\n");
+			break;
+		case DWC2_CONTROL_DATA:
+			if (*xfer_done) {
+				qtd->control_phase = DWC2_CONTROL_STATUS;
+				dev_vdbg(hsotg->dev,
+					 "  Control data transfer done\n");
+			} else if (desc_num + 1 == qtd->n_desc) {
+				/*
+				 * Last descriptor for Control data stage which
+				 * is not completed yet
+				 */
+				dwc2_hcd_save_data_toggle(hsotg, chan, chnum,
+							  qtd);
+			}
+			break;
+		default:
+			break;
+		}
+	}
+
+	return 0;
+}
+
+static void dwc2_complete_non_isoc_xfer_ddma(struct dwc2_hsotg *hsotg,
+					     struct dwc2_host_chan *chan,
+					     int chnum,
+					     enum dwc2_halt_status halt_status)
+{
+	struct list_head *qtd_item, *qtd_tmp;
+	struct dwc2_qh *qh = chan->qh;
+	struct dwc2_qtd *qtd = NULL;
+	int xfer_done;
+	int desc_num = 0;
+
+	if (chan->halt_status == DWC2_HC_XFER_URB_DEQUEUE) {
+		list_for_each_entry(qtd, &qh->qtd_list, qtd_list_entry)
+			qtd->in_process = 0;
+		return;
+	}
+
+	list_for_each_safe(qtd_item, qtd_tmp, &qh->qtd_list) {
+		int i;
+
+		qtd = list_entry(qtd_item, struct dwc2_qtd, qtd_list_entry);
+		xfer_done = 0;
+
+		for (i = 0; i < qtd->n_desc; i++) {
+			if (dwc2_process_non_isoc_desc(hsotg, chan, chnum, qtd,
+						       desc_num, halt_status,
+						       &xfer_done))
+				break;
+			desc_num++;
+		}
+	}
+
+	if (qh->ep_type != USB_ENDPOINT_XFER_CONTROL) {
+		/*
+		 * Resetting the data toggle for bulk and interrupt endpoints
+		 * in case of stall. See handle_hc_stall_intr().
+		 */
+		if (halt_status == DWC2_HC_XFER_STALL)
+			qh->data_toggle = DWC2_HC_PID_DATA0;
+		else if (qtd)
+			dwc2_hcd_save_data_toggle(hsotg, chan, chnum, qtd);
+	}
+
+	if (halt_status == DWC2_HC_XFER_COMPLETE) {
+		if (chan->hcint & HCINTMSK_NYET) {
+			/*
+			 * Got a NYET on the last transaction of the transfer.
+			 * It means that the endpoint should be in the PING
+			 * state at the beginning of the next transfer.
+			 */
+			qh->ping_state = 1;
+		}
+	}
+}
+
+/**
+ * dwc2_hcd_complete_xfer_ddma() - Scans the descriptor list, updates URB's
+ * status and calls completion routine for the URB if it's done. Called from
+ * interrupt handlers.
+ *
+ * @hsotg:       The HCD state structure for the DWC OTG controller
+ * @chan:        Host channel the transfer is completed on
+ * @chnum:       Index of Host channel registers
+ * @halt_status: Reason the channel is being halted or just XferComplete
+ *               for isochronous transfers
+ *
+ * Releases the channel to be used by other transfers.
+ * In case of Isochronous endpoint the channel is not halted until the end of
+ * the session, i.e. QTD list is empty.
+ * If periodic channel released the FrameList is updated accordingly.
+ * Calls transaction selection routines to activate pending transfers.
+ */
+void dwc2_hcd_complete_xfer_ddma(struct dwc2_hsotg *hsotg,
+				 struct dwc2_host_chan *chan, int chnum,
+				 enum dwc2_halt_status halt_status)
+{
+	struct dwc2_qh *qh = chan->qh;
+	int continue_isoc_xfer = 0;
+	enum dwc2_transaction_type tr_type;
+
+	if (chan->ep_type == USB_ENDPOINT_XFER_ISOC) {
+		dwc2_complete_isoc_xfer_ddma(hsotg, chan, halt_status);
+
+		/* Release the channel if halted or session completed */
+		if (halt_status != DWC2_HC_XFER_COMPLETE ||
+		    list_empty(&qh->qtd_list)) {
+			/* Halt the channel if session completed */
+			if (halt_status == DWC2_HC_XFER_COMPLETE)
+				dwc2_hc_halt(hsotg, chan, halt_status);
+			dwc2_release_channel_ddma(hsotg, qh);
+			dwc2_hcd_qh_unlink(hsotg, qh);
+		} else {
+			/* Keep in assigned schedule to continue transfer */
+			list_move(&qh->qh_list_entry,
+				  &hsotg->periodic_sched_assigned);
+			continue_isoc_xfer = 1;
+		}
+		/*
+		 * Todo: Consider the case when period exceeds FrameList size.
+		 * Frame Rollover interrupt should be used.
+		 */
+	} else {
+		/*
+		 * Scan descriptor list to complete the URB(s), then release
+		 * the channel
+		 */
+		dwc2_complete_non_isoc_xfer_ddma(hsotg, chan, chnum,
+						 halt_status);
+		dwc2_release_channel_ddma(hsotg, qh);
+		dwc2_hcd_qh_unlink(hsotg, qh);
+
+		if (!list_empty(&qh->qtd_list)) {
+			/*
+			 * Add back to inactive non-periodic schedule on normal
+			 * completion
+			 */
+			dwc2_hcd_qh_add(hsotg, qh);
+		}
+	}
+
+	tr_type = dwc2_hcd_select_transactions(hsotg);
+	if (tr_type != DWC2_TRANSACTION_NONE || continue_isoc_xfer) {
+		if (continue_isoc_xfer) {
+			if (tr_type == DWC2_TRANSACTION_NONE)
+				tr_type = DWC2_TRANSACTION_PERIODIC;
+			else if (tr_type == DWC2_TRANSACTION_NON_PERIODIC)
+				tr_type = DWC2_TRANSACTION_ALL;
+		}
+		dwc2_hcd_queue_transactions(hsotg, tr_type);
+	}
+}
diff --git a/drivers/staging/dwc2/hcd_intr.c b/drivers/staging/dwc2/hcd_intr.c
new file mode 100644
index 00000000000..6e5dbed6cce
--- /dev/null
+++ b/drivers/staging/dwc2/hcd_intr.c
@@ -0,0 +1,2130 @@
+/*
+ * hcd_intr.c - DesignWare HS OTG Controller host-mode interrupt handling
+ *
+ * Copyright (C) 2004-2013 Synopsys, Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions, and the following disclaimer,
+ *    without modification.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The names of the above-listed copyright holders may not be used
+ *    to endorse or promote products derived from this software without
+ *    specific prior written permission.
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation; either version 2 of the License, or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+ * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * This file contains the interrupt handlers for Host mode
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/spinlock.h>
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/usb.h>
+
+#include <linux/usb/hcd.h>
+#include <linux/usb/ch11.h>
+
+#include "core.h"
+#include "hcd.h"
+
+/* This function is for debug only */
+static void dwc2_track_missed_sofs(struct dwc2_hsotg *hsotg)
+{
+#ifdef CONFIG_USB_DWC2_TRACK_MISSED_SOFS
+#warning Compiling code to track missed SOFs
+
+	u16 curr_frame_number = hsotg->frame_number;
+
+	if (hsotg->frame_num_idx < FRAME_NUM_ARRAY_SIZE) {
+		if (((hsotg->last_frame_num + 1) & HFNUM_MAX_FRNUM) !=
+		    curr_frame_number) {
+			hsotg->frame_num_array[hsotg->frame_num_idx] =
+					curr_frame_number;
+			hsotg->last_frame_num_array[hsotg->frame_num_idx] =
+					hsotg->last_frame_num;
+			hsotg->frame_num_idx++;
+		}
+	} else if (!hsotg->dumped_frame_num_array) {
+		int i;
+
+		dev_info(hsotg->dev, "Frame     Last Frame\n");
+		dev_info(hsotg->dev, "-----     ----------\n");
+		for (i = 0; i < FRAME_NUM_ARRAY_SIZE; i++) {
+			dev_info(hsotg->dev, "0x%04x    0x%04x\n",
+				 hsotg->frame_num_array[i],
+				 hsotg->last_frame_num_array[i]);
+		}
+		hsotg->dumped_frame_num_array = 1;
+	}
+	hsotg->last_frame_num = curr_frame_number;
+#endif
+}
+
+static void dwc2_hc_handle_tt_clear(struct dwc2_hsotg *hsotg,
+				    struct dwc2_host_chan *chan,
+				    struct dwc2_qtd *qtd)
+{
+	struct urb *usb_urb;
+
+	if (!chan->qh || !qtd->urb)
+		return;
+
+	usb_urb = qtd->urb->priv;
+	if (!usb_urb || !usb_urb->dev)
+		return;
+
+	if (chan->qh->dev_speed != USB_SPEED_HIGH &&
+	    qtd->urb->status != -EPIPE && qtd->urb->status != -EREMOTEIO) {
+		chan->qh->tt_buffer_dirty = 1;
+		if (usb_hub_clear_tt_buffer(usb_urb))
+			/* Clear failed; let's hope things work anyway */
+			chan->qh->tt_buffer_dirty = 0;
+	}
+}
+
+/*
+ * Handles the start-of-frame interrupt in host mode. Non-periodic
+ * transactions may be queued to the DWC_otg controller for the current
+ * (micro)frame. Periodic transactions may be queued to the controller
+ * for the next (micro)frame.
+ */
+static void dwc2_sof_intr(struct dwc2_hsotg *hsotg)
+{
+	struct list_head *qh_entry;
+	struct dwc2_qh *qh;
+	u32 hfnum;
+	enum dwc2_transaction_type tr_type;
+
+#ifdef DEBUG_SOF
+	dev_vdbg(hsotg->dev, "--Start of Frame Interrupt--\n");
+#endif
+
+	hfnum = readl(hsotg->regs + HFNUM);
+	hsotg->frame_number = hfnum >> HFNUM_FRNUM_SHIFT &
+			    HFNUM_FRNUM_MASK >> HFNUM_FRNUM_SHIFT;
+
+	dwc2_track_missed_sofs(hsotg);
+
+	/* Determine whether any periodic QHs should be executed */
+	qh_entry = hsotg->periodic_sched_inactive.next;
+	while (qh_entry != &hsotg->periodic_sched_inactive) {
+		qh = list_entry(qh_entry, struct dwc2_qh, qh_list_entry);
+		qh_entry = qh_entry->next;
+		if (dwc2_frame_num_le(qh->sched_frame, hsotg->frame_number))
+			/*
+			 * Move QH to the ready list to be executed next
+			 * (micro)frame
+			 */
+			list_move(&qh->qh_list_entry,
+				  &hsotg->periodic_sched_ready);
+	}
+	tr_type = dwc2_hcd_select_transactions(hsotg);
+	if (tr_type != DWC2_TRANSACTION_NONE)
+		dwc2_hcd_queue_transactions(hsotg, tr_type);
+
+	/* Clear interrupt */
+	writel(GINTSTS_SOF, hsotg->regs + GINTSTS);
+}
+
+/*
+ * Handles the Rx FIFO Level Interrupt, which indicates that there is
+ * at least one packet in the Rx FIFO. The packets are moved from the FIFO to
+ * memory if the DWC_otg controller is operating in Slave mode.
+ */
+static void dwc2_rx_fifo_level_intr(struct dwc2_hsotg *hsotg)
+{
+	u32 grxsts, chnum, bcnt, dpid, pktsts;
+	struct dwc2_host_chan *chan;
+
+	if (dbg_perio())
+		dev_vdbg(hsotg->dev, "--RxFIFO Level Interrupt--\n");
+
+	grxsts = readl(hsotg->regs + GRXSTSP);
+	chnum = grxsts >> GRXSTS_HCHNUM_SHIFT &
+		GRXSTS_HCHNUM_MASK >> GRXSTS_HCHNUM_SHIFT;
+	chan = hsotg->hc_ptr_array[chnum];
+	if (!chan) {
+		dev_err(hsotg->dev, "Unable to get corresponding channel\n");
+		return;
+	}
+
+	bcnt = grxsts >> GRXSTS_BYTECNT_SHIFT &
+	       GRXSTS_BYTECNT_MASK >> GRXSTS_BYTECNT_SHIFT;
+	dpid = grxsts >> GRXSTS_DPID_SHIFT &
+	       GRXSTS_DPID_MASK >> GRXSTS_DPID_SHIFT;
+	pktsts = grxsts & GRXSTS_PKTSTS_MASK;
+
+	/* Packet Status */
+	if (dbg_perio()) {
+		dev_vdbg(hsotg->dev, "    Ch num = %d\n", chnum);
+		dev_vdbg(hsotg->dev, "    Count = %d\n", bcnt);
+		dev_vdbg(hsotg->dev, "    DPID = %d, chan.dpid = %d\n", dpid,
+			 chan->data_pid_start);
+		dev_vdbg(hsotg->dev, "    PStatus = %d\n",
+			 pktsts >> GRXSTS_PKTSTS_SHIFT &
+			 GRXSTS_PKTSTS_MASK >> GRXSTS_PKTSTS_SHIFT);
+	}
+
+	switch (pktsts) {
+	case GRXSTS_PKTSTS_HCHIN:
+		/* Read the data into the host buffer */
+		if (bcnt > 0) {
+			dwc2_read_packet(hsotg, chan->xfer_buf, bcnt);
+
+			/* Update the HC fields for the next packet received */
+			chan->xfer_count += bcnt;
+			chan->xfer_buf += bcnt;
+		}
+		break;
+	case GRXSTS_PKTSTS_HCHIN_XFER_COMP:
+	case GRXSTS_PKTSTS_DATATOGGLEERR:
+	case GRXSTS_PKTSTS_HCHHALTED:
+		/* Handled in interrupt, just ignore data */
+		break;
+	default:
+		dev_err(hsotg->dev,
+			"RxFIFO Level Interrupt: Unknown status %d\n", pktsts);
+		break;
+	}
+}
+
+/*
+ * This interrupt occurs when the non-periodic Tx FIFO is half-empty. More
+ * data packets may be written to the FIFO for OUT transfers. More requests
+ * may be written to the non-periodic request queue for IN transfers. This
+ * interrupt is enabled only in Slave mode.
+ */
+static void dwc2_np_tx_fifo_empty_intr(struct dwc2_hsotg *hsotg)
+{
+	dev_vdbg(hsotg->dev, "--Non-Periodic TxFIFO Empty Interrupt--\n");
+	dwc2_hcd_queue_transactions(hsotg, DWC2_TRANSACTION_NON_PERIODIC);
+}
+
+/*
+ * This interrupt occurs when the periodic Tx FIFO is half-empty. More data
+ * packets may be written to the FIFO for OUT transfers. More requests may be
+ * written to the periodic request queue for IN transfers. This interrupt is
+ * enabled only in Slave mode.
+ */
+static void dwc2_perio_tx_fifo_empty_intr(struct dwc2_hsotg *hsotg)
+{
+	if (dbg_perio())
+		dev_vdbg(hsotg->dev, "--Periodic TxFIFO Empty Interrupt--\n");
+	dwc2_hcd_queue_transactions(hsotg, DWC2_TRANSACTION_PERIODIC);
+}
+
+static void dwc2_hprt0_enable(struct dwc2_hsotg *hsotg, u32 hprt0,
+			      u32 *hprt0_modify)
+{
+	struct dwc2_core_params *params = hsotg->core_params;
+	int do_reset = 0;
+	u32 usbcfg;
+	u32 prtspd;
+	u32 hcfg;
+	u32 hfir;
+
+	dev_vdbg(hsotg->dev, "%s(%p)\n", __func__, hsotg);
+
+	/* Every time when port enables calculate HFIR.FrInterval */
+	hfir = readl(hsotg->regs + HFIR);
+	hfir &= ~HFIR_FRINT_MASK;
+	hfir |= dwc2_calc_frame_interval(hsotg) << HFIR_FRINT_SHIFT &
+		HFIR_FRINT_MASK;
+	writel(hfir, hsotg->regs + HFIR);
+
+	/* Check if we need to adjust the PHY clock speed for low power */
+	if (!params->host_support_fs_ls_low_power) {
+		/* Port has been enabled, set the reset change flag */
+		hsotg->flags.b.port_reset_change = 1;
+		return;
+	}
+
+	usbcfg = readl(hsotg->regs + GUSBCFG);
+	prtspd = hprt0 & HPRT0_SPD_MASK;
+
+	if (prtspd == HPRT0_SPD_LOW_SPEED || prtspd == HPRT0_SPD_FULL_SPEED) {
+		/* Low power */
+		if (!(usbcfg & GUSBCFG_PHY_LP_CLK_SEL)) {
+			/* Set PHY low power clock select for FS/LS devices */
+			usbcfg |= GUSBCFG_PHY_LP_CLK_SEL;
+			writel(usbcfg, hsotg->regs + GUSBCFG);
+			do_reset = 1;
+		}
+
+		hcfg = readl(hsotg->regs + HCFG);
+
+		if (prtspd == HPRT0_SPD_LOW_SPEED &&
+		    params->host_ls_low_power_phy_clk ==
+		    DWC2_HOST_LS_LOW_POWER_PHY_CLK_PARAM_6MHZ) {
+			/* 6 MHZ */
+			dev_vdbg(hsotg->dev,
+				 "FS_PHY programming HCFG to 6 MHz\n");
+			if ((hcfg & HCFG_FSLSPCLKSEL_MASK) !=
+			    HCFG_FSLSPCLKSEL_6_MHZ) {
+				hcfg &= ~HCFG_FSLSPCLKSEL_MASK;
+				hcfg |= HCFG_FSLSPCLKSEL_6_MHZ;
+				writel(hcfg, hsotg->regs + HCFG);
+				do_reset = 1;
+			}
+		} else {
+			/* 48 MHZ */
+			dev_vdbg(hsotg->dev,
+				 "FS_PHY programming HCFG to 48 MHz\n");
+			if ((hcfg & HCFG_FSLSPCLKSEL_MASK) !=
+			    HCFG_FSLSPCLKSEL_48_MHZ) {
+				hcfg &= ~HCFG_FSLSPCLKSEL_MASK;
+				hcfg |= HCFG_FSLSPCLKSEL_48_MHZ;
+				writel(hcfg, hsotg->regs + HCFG);
+				do_reset = 1;
+			}
+		}
+	} else {
+		/* Not low power */
+		if (usbcfg & GUSBCFG_PHY_LP_CLK_SEL) {
+			usbcfg &= ~GUSBCFG_PHY_LP_CLK_SEL;
+			writel(usbcfg, hsotg->regs + GUSBCFG);
+			do_reset = 1;
+		}
+	}
+
+	if (do_reset) {
+		*hprt0_modify |= HPRT0_RST;
+		queue_delayed_work(hsotg->wq_otg, &hsotg->reset_work,
+				   msecs_to_jiffies(60));
+	} else {
+		/* Port has been enabled, set the reset change flag */
+		hsotg->flags.b.port_reset_change = 1;
+	}
+}
+
+/*
+ * There are multiple conditions that can cause a port interrupt. This function
+ * determines which interrupt conditions have occurred and handles them
+ * appropriately.
+ */
+static void dwc2_port_intr(struct dwc2_hsotg *hsotg)
+{
+	u32 hprt0;
+	u32 hprt0_modify;
+
+	dev_vdbg(hsotg->dev, "--Port Interrupt--\n");
+
+	hprt0 = readl(hsotg->regs + HPRT0);
+	hprt0_modify = hprt0;
+
+	/*
+	 * Clear appropriate bits in HPRT0 to clear the interrupt bit in
+	 * GINTSTS
+	 */
+	hprt0_modify &= ~(HPRT0_ENA | HPRT0_CONNDET | HPRT0_ENACHG |
+			  HPRT0_OVRCURRCHG);
+
+	/*
+	 * Port Connect Detected
+	 * Set flag and clear if detected
+	 */
+	if (hprt0 & HPRT0_CONNDET) {
+		dev_vdbg(hsotg->dev,
+			 "--Port Interrupt HPRT0=0x%08x Port Connect Detected--\n",
+			 hprt0);
+		hsotg->flags.b.port_connect_status_change = 1;
+		hsotg->flags.b.port_connect_status = 1;
+		hprt0_modify |= HPRT0_CONNDET;
+
+		/*
+		 * The Hub driver asserts a reset when it sees port connect
+		 * status change flag
+		 */
+	}
+
+	/*
+	 * Port Enable Changed
+	 * Clear if detected - Set internal flag if disabled
+	 */
+	if (hprt0 & HPRT0_ENACHG) {
+		dev_vdbg(hsotg->dev,
+			 "  --Port Interrupt HPRT0=0x%08x Port Enable Changed (now %d)--\n",
+			 hprt0, !!(hprt0 & HPRT0_ENA));
+		hprt0_modify |= HPRT0_ENACHG;
+		if (hprt0 & HPRT0_ENA)
+			dwc2_hprt0_enable(hsotg, hprt0, &hprt0_modify);
+		else
+			hsotg->flags.b.port_enable_change = 1;
+	}
+
+	/* Overcurrent Change Interrupt */
+	if (hprt0 & HPRT0_OVRCURRCHG) {
+		dev_vdbg(hsotg->dev,
+			 "  --Port Interrupt HPRT0=0x%08x Port Overcurrent Changed--\n",
+			 hprt0);
+		hsotg->flags.b.port_over_current_change = 1;
+		hprt0_modify |= HPRT0_OVRCURRCHG;
+	}
+
+	/* Clear Port Interrupts */
+	writel(hprt0_modify, hsotg->regs + HPRT0);
+}
+
+/*
+ * Gets the actual length of a transfer after the transfer halts. halt_status
+ * holds the reason for the halt.
+ *
+ * For IN transfers where halt_status is DWC2_HC_XFER_COMPLETE, *short_read
+ * is set to 1 upon return if less than the requested number of bytes were
+ * transferred. short_read may also be NULL on entry, in which case it remains
+ * unchanged.
+ */
+static u32 dwc2_get_actual_xfer_length(struct dwc2_hsotg *hsotg,
+				       struct dwc2_host_chan *chan, int chnum,
+				       struct dwc2_qtd *qtd,
+				       enum dwc2_halt_status halt_status,
+				       int *short_read)
+{
+	u32 hctsiz, count, length;
+
+	hctsiz = readl(hsotg->regs + HCTSIZ(chnum));
+
+	if (halt_status == DWC2_HC_XFER_COMPLETE) {
+		if (chan->ep_is_in) {
+			count = hctsiz >> TSIZ_XFERSIZE_SHIFT &
+				TSIZ_XFERSIZE_MASK >> TSIZ_XFERSIZE_SHIFT;
+			length = chan->xfer_len - count;
+			if (short_read != NULL)
+				*short_read = (count != 0);
+		} else if (chan->qh->do_split) {
+			length = qtd->ssplit_out_xfer_count;
+		} else {
+			length = chan->xfer_len;
+		}
+	} else {
+		/*
+		 * Must use the hctsiz.pktcnt field to determine how much data
+		 * has been transferred. This field reflects the number of
+		 * packets that have been transferred via the USB. This is
+		 * always an integral number of packets if the transfer was
+		 * halted before its normal completion. (Can't use the
+		 * hctsiz.xfersize field because that reflects the number of
+		 * bytes transferred via the AHB, not the USB).
+		 */
+		count = hctsiz >> TSIZ_PKTCNT_SHIFT &
+			TSIZ_PKTCNT_MASK >> TSIZ_PKTCNT_SHIFT;
+		length = (chan->start_pkt_count - count) * chan->max_packet;
+	}
+
+	return length;
+}
+
+/**
+ * dwc2_update_urb_state() - Updates the state of the URB after a Transfer
+ * Complete interrupt on the host channel. Updates the actual_length field
+ * of the URB based on the number of bytes transferred via the host channel.
+ * Sets the URB status if the data transfer is finished.
+ *
+ * Return: 1 if the data transfer specified by the URB is completely finished,
+ * 0 otherwise
+ */
+static int dwc2_update_urb_state(struct dwc2_hsotg *hsotg,
+				 struct dwc2_host_chan *chan, int chnum,
+				 struct dwc2_hcd_urb *urb,
+				 struct dwc2_qtd *qtd)
+{
+	u32 hctsiz;
+	int xfer_done = 0;
+	int short_read = 0;
+	int xfer_length = dwc2_get_actual_xfer_length(hsotg, chan, chnum, qtd,
+						      DWC2_HC_XFER_COMPLETE,
+						      &short_read);
+
+	if (urb->actual_length + xfer_length > urb->length) {
+		dev_warn(hsotg->dev, "%s(): trimming xfer length\n", __func__);
+		xfer_length = urb->length - urb->actual_length;
+	}
+
+	/* Non DWORD-aligned buffer case handling */
+	if (chan->align_buf && xfer_length && chan->ep_is_in) {
+		dev_dbg(hsotg->dev, "%s(): non-aligned buffer\n", __func__);
+		dma_sync_single_for_cpu(hsotg->dev, urb->dma, urb->length,
+					DMA_FROM_DEVICE);
+		memcpy(urb->buf + urb->actual_length, chan->qh->dw_align_buf,
+		       xfer_length);
+		dma_sync_single_for_device(hsotg->dev, urb->dma, urb->length,
+					   DMA_FROM_DEVICE);
+	}
+
+	dev_vdbg(hsotg->dev, "urb->actual_length=%d xfer_length=%d\n",
+		 urb->actual_length, xfer_length);
+	urb->actual_length += xfer_length;
+
+	if (xfer_length && chan->ep_type == USB_ENDPOINT_XFER_BULK &&
+	    (urb->flags & URB_SEND_ZERO_PACKET) &&
+	    urb->actual_length >= urb->length &&
+	    !(urb->length % chan->max_packet)) {
+		xfer_done = 0;
+	} else if (short_read || urb->actual_length >= urb->length) {
+		xfer_done = 1;
+		urb->status = 0;
+	}
+
+	hctsiz = readl(hsotg->regs + HCTSIZ(chnum));
+	dev_vdbg(hsotg->dev, "DWC_otg: %s: %s, channel %d\n",
+		 __func__, (chan->ep_is_in ? "IN" : "OUT"), chnum);
+	dev_vdbg(hsotg->dev, "  chan->xfer_len %d\n", chan->xfer_len);
+	dev_vdbg(hsotg->dev, "  hctsiz.xfersize %d\n",
+		 hctsiz >> TSIZ_XFERSIZE_SHIFT &
+		 TSIZ_XFERSIZE_MASK >> TSIZ_XFERSIZE_SHIFT);
+	dev_vdbg(hsotg->dev, "  urb->transfer_buffer_length %d\n", urb->length);
+	dev_vdbg(hsotg->dev, "  urb->actual_length %d\n", urb->actual_length);
+	dev_vdbg(hsotg->dev, "  short_read %d, xfer_done %d\n", short_read,
+		 xfer_done);
+
+	return xfer_done;
+}
+
+/*
+ * Save the starting data toggle for the next transfer. The data toggle is
+ * saved in the QH for non-control transfers and it's saved in the QTD for
+ * control transfers.
+ */
+void dwc2_hcd_save_data_toggle(struct dwc2_hsotg *hsotg,
+			       struct dwc2_host_chan *chan, int chnum,
+			       struct dwc2_qtd *qtd)
+{
+	u32 hctsiz = readl(hsotg->regs + HCTSIZ(chnum));
+	u32 pid = hctsiz & TSIZ_SC_MC_PID_MASK;
+
+	if (chan->ep_type != USB_ENDPOINT_XFER_CONTROL) {
+		if (pid == TSIZ_SC_MC_PID_DATA0)
+			chan->qh->data_toggle = DWC2_HC_PID_DATA0;
+		else
+			chan->qh->data_toggle = DWC2_HC_PID_DATA1;
+	} else {
+		if (pid == TSIZ_SC_MC_PID_DATA0)
+			qtd->data_toggle = DWC2_HC_PID_DATA0;
+		else
+			qtd->data_toggle = DWC2_HC_PID_DATA1;
+	}
+}
+
+/**
+ * dwc2_update_isoc_urb_state() - Updates the state of an Isochronous URB when
+ * the transfer is stopped for any reason. The fields of the current entry in
+ * the frame descriptor array are set based on the transfer state and the input
+ * halt_status. Completes the Isochronous URB if all the URB frames have been
+ * completed.
+ *
+ * Return: DWC2_HC_XFER_COMPLETE if there are more frames remaining to be
+ * transferred in the URB. Otherwise return DWC2_HC_XFER_URB_COMPLETE.
+ */
+static enum dwc2_halt_status dwc2_update_isoc_urb_state(
+		struct dwc2_hsotg *hsotg, struct dwc2_host_chan *chan,
+		int chnum, struct dwc2_qtd *qtd,
+		enum dwc2_halt_status halt_status)
+{
+	struct dwc2_hcd_iso_packet_desc *frame_desc;
+	struct dwc2_hcd_urb *urb = qtd->urb;
+
+	if (!urb)
+		return DWC2_HC_XFER_NO_HALT_STATUS;
+
+	frame_desc = &urb->iso_descs[qtd->isoc_frame_index];
+
+	switch (halt_status) {
+	case DWC2_HC_XFER_COMPLETE:
+		frame_desc->status = 0;
+		frame_desc->actual_length = dwc2_get_actual_xfer_length(hsotg,
+					chan, chnum, qtd, halt_status, NULL);
+
+		/* Non DWORD-aligned buffer case handling */
+		if (chan->align_buf && frame_desc->actual_length &&
+		    chan->ep_is_in) {
+			dev_dbg(hsotg->dev, "%s(): non-aligned buffer\n",
+				__func__);
+			dma_sync_single_for_cpu(hsotg->dev, urb->dma,
+						urb->length, DMA_FROM_DEVICE);
+			memcpy(urb->buf + frame_desc->offset +
+			       qtd->isoc_split_offset, chan->qh->dw_align_buf,
+			       frame_desc->actual_length);
+			dma_sync_single_for_device(hsotg->dev, urb->dma,
+						   urb->length,
+						   DMA_FROM_DEVICE);
+		}
+		break;
+	case DWC2_HC_XFER_FRAME_OVERRUN:
+		urb->error_count++;
+		if (chan->ep_is_in)
+			frame_desc->status = -ENOSR;
+		else
+			frame_desc->status = -ECOMM;
+		frame_desc->actual_length = 0;
+		break;
+	case DWC2_HC_XFER_BABBLE_ERR:
+		urb->error_count++;
+		frame_desc->status = -EOVERFLOW;
+		/* Don't need to update actual_length in this case */
+		break;
+	case DWC2_HC_XFER_XACT_ERR:
+		urb->error_count++;
+		frame_desc->status = -EPROTO;
+		frame_desc->actual_length = dwc2_get_actual_xfer_length(hsotg,
+					chan, chnum, qtd, halt_status, NULL);
+
+		/* Non DWORD-aligned buffer case handling */
+		if (chan->align_buf && frame_desc->actual_length &&
+		    chan->ep_is_in) {
+			dev_dbg(hsotg->dev, "%s(): non-aligned buffer\n",
+				__func__);
+			dma_sync_single_for_cpu(hsotg->dev, urb->dma,
+						urb->length, DMA_FROM_DEVICE);
+			memcpy(urb->buf + frame_desc->offset +
+			       qtd->isoc_split_offset, chan->qh->dw_align_buf,
+			       frame_desc->actual_length);
+			dma_sync_single_for_device(hsotg->dev, urb->dma,
+						   urb->length,
+						   DMA_FROM_DEVICE);
+		}
+
+		/* Skip whole frame */
+		if (chan->qh->do_split &&
+		    chan->ep_type == USB_ENDPOINT_XFER_ISOC && chan->ep_is_in &&
+		    hsotg->core_params->dma_enable > 0) {
+			qtd->complete_split = 0;
+			qtd->isoc_split_offset = 0;
+		}
+
+		break;
+	default:
+		dev_err(hsotg->dev, "Unhandled halt_status (%d)\n",
+			halt_status);
+		break;
+	}
+
+	if (++qtd->isoc_frame_index == urb->packet_count) {
+		/*
+		 * urb->status is not used for isoc transfers. The individual
+		 * frame_desc statuses are used instead.
+		 */
+		dwc2_host_complete(hsotg, urb->priv, urb, 0);
+		halt_status = DWC2_HC_XFER_URB_COMPLETE;
+	} else {
+		halt_status = DWC2_HC_XFER_COMPLETE;
+	}
+
+	return halt_status;
+}
+
+/*
+ * Frees the first QTD in the QH's list if free_qtd is 1. For non-periodic
+ * QHs, removes the QH from the active non-periodic schedule. If any QTDs are
+ * still linked to the QH, the QH is added to the end of the inactive
+ * non-periodic schedule. For periodic QHs, removes the QH from the periodic
+ * schedule if no more QTDs are linked to the QH.
+ */
+static void dwc2_deactivate_qh(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh,
+			       int free_qtd)
+{
+	int continue_split = 0;
+	struct dwc2_qtd *qtd;
+
+	if (dbg_qh(qh))
+		dev_vdbg(hsotg->dev, "  %s(%p,%p,%d)\n", __func__,
+			 hsotg, qh, free_qtd);
+
+	if (list_empty(&qh->qtd_list)) {
+		dev_dbg(hsotg->dev, "## QTD list empty ##\n");
+		goto no_qtd;
+	}
+
+	qtd = list_first_entry(&qh->qtd_list, struct dwc2_qtd, qtd_list_entry);
+
+	if (qtd->complete_split)
+		continue_split = 1;
+	else if (qtd->isoc_split_pos == DWC2_HCSPLT_XACTPOS_MID ||
+		 qtd->isoc_split_pos == DWC2_HCSPLT_XACTPOS_END)
+		continue_split = 1;
+
+	if (free_qtd) {
+		dwc2_hcd_qtd_unlink_and_free(hsotg, qtd, qh);
+		continue_split = 0;
+	}
+
+no_qtd:
+	if (qh->channel)
+		qh->channel->align_buf = 0;
+	qh->channel = NULL;
+	dwc2_hcd_qh_deactivate(hsotg, qh, continue_split);
+}
+
+/**
+ * dwc2_release_channel() - Releases a host channel for use by other transfers
+ *
+ * @hsotg:       The HCD state structure
+ * @chan:        The host channel to release
+ * @qtd:         The QTD associated with the host channel. This QTD may be
+ *               freed if the transfer is complete or an error has occurred.
+ * @halt_status: Reason the channel is being released. This status
+ *               determines the actions taken by this function.
+ *
+ * Also attempts to select and queue more transactions since at least one host
+ * channel is available.
+ */
+static void dwc2_release_channel(struct dwc2_hsotg *hsotg,
+				 struct dwc2_host_chan *chan,
+				 struct dwc2_qtd *qtd,
+				 enum dwc2_halt_status halt_status)
+{
+	enum dwc2_transaction_type tr_type;
+	u32 haintmsk;
+	int free_qtd = 0;
+
+	if (dbg_hc(chan))
+		dev_vdbg(hsotg->dev, "  %s: channel %d, halt_status %d\n",
+			 __func__, chan->hc_num, halt_status);
+
+	switch (halt_status) {
+	case DWC2_HC_XFER_URB_COMPLETE:
+		free_qtd = 1;
+		break;
+	case DWC2_HC_XFER_AHB_ERR:
+	case DWC2_HC_XFER_STALL:
+	case DWC2_HC_XFER_BABBLE_ERR:
+		free_qtd = 1;
+		break;
+	case DWC2_HC_XFER_XACT_ERR:
+		if (qtd && qtd->error_count >= 3) {
+			dev_vdbg(hsotg->dev,
+				 "  Complete URB with transaction error\n");
+			free_qtd = 1;
+			if (qtd->urb) {
+				qtd->urb->status = -EPROTO;
+				dwc2_host_complete(hsotg, qtd->urb->priv,
+						   qtd->urb, -EPROTO);
+			}
+		}
+		break;
+	case DWC2_HC_XFER_URB_DEQUEUE:
+		/*
+		 * The QTD has already been removed and the QH has been
+		 * deactivated. Don't want to do anything except release the
+		 * host channel and try to queue more transfers.
+		 */
+		goto cleanup;
+	case DWC2_HC_XFER_PERIODIC_INCOMPLETE:
+		dev_vdbg(hsotg->dev, "  Complete URB with I/O error\n");
+		free_qtd = 1;
+		if (qtd && qtd->urb) {
+			qtd->urb->status = -EIO;
+			dwc2_host_complete(hsotg, qtd->urb->priv, qtd->urb,
+					   -EIO);
+		}
+		break;
+	case DWC2_HC_XFER_NO_HALT_STATUS:
+	default:
+		break;
+	}
+
+	dwc2_deactivate_qh(hsotg, chan->qh, free_qtd);
+
+cleanup:
+	/*
+	 * Release the host channel for use by other transfers. The cleanup
+	 * function clears the channel interrupt enables and conditions, so
+	 * there's no need to clear the Channel Halted interrupt separately.
+	 */
+	if (!list_empty(&chan->hc_list_entry))
+		list_del(&chan->hc_list_entry);
+	dwc2_hc_cleanup(hsotg, chan);
+	list_add_tail(&chan->hc_list_entry, &hsotg->free_hc_list);
+
+	switch (chan->ep_type) {
+	case USB_ENDPOINT_XFER_CONTROL:
+	case USB_ENDPOINT_XFER_BULK:
+		hsotg->non_periodic_channels--;
+		break;
+	default:
+		/*
+		 * Don't release reservations for periodic channels here.
+		 * That's done when a periodic transfer is descheduled (i.e.
+		 * when the QH is removed from the periodic schedule).
+		 */
+		break;
+	}
+
+	haintmsk = readl(hsotg->regs + HAINTMSK);
+	haintmsk &= ~(1 << chan->hc_num);
+	writel(haintmsk, hsotg->regs + HAINTMSK);
+
+	/* Try to queue more transfers now that there's a free channel */
+	tr_type = dwc2_hcd_select_transactions(hsotg);
+	if (tr_type != DWC2_TRANSACTION_NONE)
+		dwc2_hcd_queue_transactions(hsotg, tr_type);
+}
+
+/*
+ * Halts a host channel. If the channel cannot be halted immediately because
+ * the request queue is full, this function ensures that the FIFO empty
+ * interrupt for the appropriate queue is enabled so that the halt request can
+ * be queued when there is space in the request queue.
+ *
+ * This function may also be called in DMA mode. In that case, the channel is
+ * simply released since the core always halts the channel automatically in
+ * DMA mode.
+ */
+static void dwc2_halt_channel(struct dwc2_hsotg *hsotg,
+			      struct dwc2_host_chan *chan, struct dwc2_qtd *qtd,
+			      enum dwc2_halt_status halt_status)
+{
+	if (dbg_hc(chan))
+		dev_vdbg(hsotg->dev, "%s()\n", __func__);
+
+	if (hsotg->core_params->dma_enable > 0) {
+		if (dbg_hc(chan))
+			dev_vdbg(hsotg->dev, "DMA enabled\n");
+		dwc2_release_channel(hsotg, chan, qtd, halt_status);
+		return;
+	}
+
+	/* Slave mode processing */
+	dwc2_hc_halt(hsotg, chan, halt_status);
+
+	if (chan->halt_on_queue) {
+		u32 gintmsk;
+
+		dev_vdbg(hsotg->dev, "Halt on queue\n");
+		if (chan->ep_type == USB_ENDPOINT_XFER_CONTROL ||
+		    chan->ep_type == USB_ENDPOINT_XFER_BULK) {
+			dev_vdbg(hsotg->dev, "control/bulk\n");
+			/*
+			 * Make sure the Non-periodic Tx FIFO empty interrupt
+			 * is enabled so that the non-periodic schedule will
+			 * be processed
+			 */
+			gintmsk = readl(hsotg->regs + GINTMSK);
+			gintmsk |= GINTSTS_NPTXFEMP;
+			writel(gintmsk, hsotg->regs + GINTMSK);
+		} else {
+			dev_vdbg(hsotg->dev, "isoc/intr\n");
+			/*
+			 * Move the QH from the periodic queued schedule to
+			 * the periodic assigned schedule. This allows the
+			 * halt to be queued when the periodic schedule is
+			 * processed.
+			 */
+			list_move(&chan->qh->qh_list_entry,
+				  &hsotg->periodic_sched_assigned);
+
+			/*
+			 * Make sure the Periodic Tx FIFO Empty interrupt is
+			 * enabled so that the periodic schedule will be
+			 * processed
+			 */
+			gintmsk = readl(hsotg->regs + GINTMSK);
+			gintmsk |= GINTSTS_PTXFEMP;
+			writel(gintmsk, hsotg->regs + GINTMSK);
+		}
+	}
+}
+
+/*
+ * Performs common cleanup for non-periodic transfers after a Transfer
+ * Complete interrupt. This function should be called after any endpoint type
+ * specific handling is finished to release the host channel.
+ */
+static void dwc2_complete_non_periodic_xfer(struct dwc2_hsotg *hsotg,
+					    struct dwc2_host_chan *chan,
+					    int chnum, struct dwc2_qtd *qtd,
+					    enum dwc2_halt_status halt_status)
+{
+	dev_vdbg(hsotg->dev, "%s()\n", __func__);
+
+	qtd->error_count = 0;
+
+	if (chan->hcint & HCINTMSK_NYET) {
+		/*
+		 * Got a NYET on the last transaction of the transfer. This
+		 * means that the endpoint should be in the PING state at the
+		 * beginning of the next transfer.
+		 */
+		dev_vdbg(hsotg->dev, "got NYET\n");
+		chan->qh->ping_state = 1;
+	}
+
+	/*
+	 * Always halt and release the host channel to make it available for
+	 * more transfers. There may still be more phases for a control
+	 * transfer or more data packets for a bulk transfer at this point,
+	 * but the host channel is still halted. A channel will be reassigned
+	 * to the transfer when the non-periodic schedule is processed after
+	 * the channel is released. This allows transactions to be queued
+	 * properly via dwc2_hcd_queue_transactions, which also enables the
+	 * Tx FIFO Empty interrupt if necessary.
+	 */
+	if (chan->ep_is_in) {
+		/*
+		 * IN transfers in Slave mode require an explicit disable to
+		 * halt the channel. (In DMA mode, this call simply releases
+		 * the channel.)
+		 */
+		dwc2_halt_channel(hsotg, chan, qtd, halt_status);
+	} else {
+		/*
+		 * The channel is automatically disabled by the core for OUT
+		 * transfers in Slave mode
+		 */
+		dwc2_release_channel(hsotg, chan, qtd, halt_status);
+	}
+}
+
+/*
+ * Performs common cleanup for periodic transfers after a Transfer Complete
+ * interrupt. This function should be called after any endpoint type specific
+ * handling is finished to release the host channel.
+ */
+static void dwc2_complete_periodic_xfer(struct dwc2_hsotg *hsotg,
+					struct dwc2_host_chan *chan, int chnum,
+					struct dwc2_qtd *qtd,
+					enum dwc2_halt_status halt_status)
+{
+	u32 hctsiz = readl(hsotg->regs + HCTSIZ(chnum));
+
+	qtd->error_count = 0;
+
+	if (!chan->ep_is_in || (hctsiz & TSIZ_PKTCNT_MASK) == 0)
+		/* Core halts channel in these cases */
+		dwc2_release_channel(hsotg, chan, qtd, halt_status);
+	else
+		/* Flush any outstanding requests from the Tx queue */
+		dwc2_halt_channel(hsotg, chan, qtd, halt_status);
+}
+
+static int dwc2_xfercomp_isoc_split_in(struct dwc2_hsotg *hsotg,
+				       struct dwc2_host_chan *chan, int chnum,
+				       struct dwc2_qtd *qtd)
+{
+	struct dwc2_hcd_iso_packet_desc *frame_desc;
+	u32 len;
+
+	if (!qtd->urb)
+		return 0;
+
+	frame_desc = &qtd->urb->iso_descs[qtd->isoc_frame_index];
+	len = dwc2_get_actual_xfer_length(hsotg, chan, chnum, qtd,
+					  DWC2_HC_XFER_COMPLETE, NULL);
+	if (!len) {
+		qtd->complete_split = 0;
+		qtd->isoc_split_offset = 0;
+		return 0;
+	}
+
+	frame_desc->actual_length += len;
+
+	if (chan->align_buf && len) {
+		dev_dbg(hsotg->dev, "%s(): non-aligned buffer\n", __func__);
+		dma_sync_single_for_cpu(hsotg->dev, qtd->urb->dma,
+					qtd->urb->length, DMA_FROM_DEVICE);
+		memcpy(qtd->urb->buf + frame_desc->offset +
+		       qtd->isoc_split_offset, chan->qh->dw_align_buf, len);
+		dma_sync_single_for_device(hsotg->dev, qtd->urb->dma,
+					   qtd->urb->length, DMA_FROM_DEVICE);
+	}
+
+	qtd->isoc_split_offset += len;
+
+	if (frame_desc->actual_length >= frame_desc->length) {
+		frame_desc->status = 0;
+		qtd->isoc_frame_index++;
+		qtd->complete_split = 0;
+		qtd->isoc_split_offset = 0;
+	}
+
+	if (qtd->isoc_frame_index == qtd->urb->packet_count) {
+		dwc2_host_complete(hsotg, qtd->urb->priv, qtd->urb, 0);
+		dwc2_release_channel(hsotg, chan, qtd,
+				     DWC2_HC_XFER_URB_COMPLETE);
+	} else {
+		dwc2_release_channel(hsotg, chan, qtd,
+				     DWC2_HC_XFER_NO_HALT_STATUS);
+	}
+
+	return 1;	/* Indicates that channel released */
+}
+
+/*
+ * Handles a host channel Transfer Complete interrupt. This handler may be
+ * called in either DMA mode or Slave mode.
+ */
+static void dwc2_hc_xfercomp_intr(struct dwc2_hsotg *hsotg,
+				  struct dwc2_host_chan *chan, int chnum,
+				  struct dwc2_qtd *qtd)
+{
+	struct dwc2_hcd_urb *urb = qtd->urb;
+	int pipe_type = dwc2_hcd_get_pipe_type(&urb->pipe_info);
+	enum dwc2_halt_status halt_status = DWC2_HC_XFER_COMPLETE;
+	int urb_xfer_done;
+
+	if (dbg_hc(chan))
+		dev_vdbg(hsotg->dev,
+			 "--Host Channel %d Interrupt: Transfer Complete--\n",
+			 chnum);
+
+	if (hsotg->core_params->dma_desc_enable > 0) {
+		dwc2_hcd_complete_xfer_ddma(hsotg, chan, chnum, halt_status);
+		if (pipe_type == USB_ENDPOINT_XFER_ISOC)
+			/* Do not disable the interrupt, just clear it */
+			return;
+		goto handle_xfercomp_done;
+	}
+
+	/* Handle xfer complete on CSPLIT */
+	if (chan->qh->do_split) {
+		if (chan->ep_type == USB_ENDPOINT_XFER_ISOC && chan->ep_is_in &&
+		    hsotg->core_params->dma_enable > 0) {
+			if (qtd->complete_split &&
+			    dwc2_xfercomp_isoc_split_in(hsotg, chan, chnum,
+							qtd))
+				goto handle_xfercomp_done;
+		} else {
+			qtd->complete_split = 0;
+		}
+	}
+
+	if (!urb)
+		goto handle_xfercomp_done;
+
+	/* Update the QTD and URB states */
+	switch (pipe_type) {
+	case USB_ENDPOINT_XFER_CONTROL:
+		switch (qtd->control_phase) {
+		case DWC2_CONTROL_SETUP:
+			if (urb->length > 0)
+				qtd->control_phase = DWC2_CONTROL_DATA;
+			else
+				qtd->control_phase = DWC2_CONTROL_STATUS;
+			dev_vdbg(hsotg->dev,
+				 "  Control setup transaction done\n");
+			halt_status = DWC2_HC_XFER_COMPLETE;
+			break;
+		case DWC2_CONTROL_DATA:
+			urb_xfer_done = dwc2_update_urb_state(hsotg, chan,
+							      chnum, urb, qtd);
+			if (urb_xfer_done) {
+				qtd->control_phase = DWC2_CONTROL_STATUS;
+				dev_vdbg(hsotg->dev,
+					 "  Control data transfer done\n");
+			} else {
+				dwc2_hcd_save_data_toggle(hsotg, chan, chnum,
+							  qtd);
+			}
+			halt_status = DWC2_HC_XFER_COMPLETE;
+			break;
+		case DWC2_CONTROL_STATUS:
+			dev_vdbg(hsotg->dev, "  Control transfer complete\n");
+			if (urb->status == -EINPROGRESS)
+				urb->status = 0;
+			dwc2_host_complete(hsotg, urb->priv, urb, urb->status);
+			halt_status = DWC2_HC_XFER_URB_COMPLETE;
+			break;
+		}
+
+		dwc2_complete_non_periodic_xfer(hsotg, chan, chnum, qtd,
+						halt_status);
+		break;
+	case USB_ENDPOINT_XFER_BULK:
+		dev_vdbg(hsotg->dev, "  Bulk transfer complete\n");
+		urb_xfer_done = dwc2_update_urb_state(hsotg, chan, chnum, urb,
+						      qtd);
+		if (urb_xfer_done) {
+			dwc2_host_complete(hsotg, urb->priv, urb, urb->status);
+			halt_status = DWC2_HC_XFER_URB_COMPLETE;
+		} else {
+			halt_status = DWC2_HC_XFER_COMPLETE;
+		}
+
+		dwc2_hcd_save_data_toggle(hsotg, chan, chnum, qtd);
+		dwc2_complete_non_periodic_xfer(hsotg, chan, chnum, qtd,
+						halt_status);
+		break;
+	case USB_ENDPOINT_XFER_INT:
+		dev_vdbg(hsotg->dev, "  Interrupt transfer complete\n");
+		urb_xfer_done = dwc2_update_urb_state(hsotg, chan, chnum, urb,
+						      qtd);
+
+		/*
+		 * Interrupt URB is done on the first transfer complete
+		 * interrupt
+		 */
+		if (urb_xfer_done) {
+				dwc2_host_complete(hsotg, urb->priv, urb,
+						   urb->status);
+				halt_status = DWC2_HC_XFER_URB_COMPLETE;
+		} else {
+				halt_status = DWC2_HC_XFER_COMPLETE;
+		}
+
+		dwc2_hcd_save_data_toggle(hsotg, chan, chnum, qtd);
+		dwc2_complete_periodic_xfer(hsotg, chan, chnum, qtd,
+					    halt_status);
+		break;
+	case USB_ENDPOINT_XFER_ISOC:
+		if (dbg_perio())
+			dev_vdbg(hsotg->dev, "  Isochronous transfer complete\n");
+		if (qtd->isoc_split_pos == DWC2_HCSPLT_XACTPOS_ALL)
+			halt_status = dwc2_update_isoc_urb_state(hsotg, chan,
+					chnum, qtd, DWC2_HC_XFER_COMPLETE);
+		dwc2_complete_periodic_xfer(hsotg, chan, chnum, qtd,
+					    halt_status);
+		break;
+	}
+
+handle_xfercomp_done:
+	disable_hc_int(hsotg, chnum, HCINTMSK_XFERCOMPL);
+}
+
+/*
+ * Handles a host channel STALL interrupt. This handler may be called in
+ * either DMA mode or Slave mode.
+ */
+static void dwc2_hc_stall_intr(struct dwc2_hsotg *hsotg,
+			       struct dwc2_host_chan *chan, int chnum,
+			       struct dwc2_qtd *qtd)
+{
+	struct dwc2_hcd_urb *urb = qtd->urb;
+	int pipe_type = dwc2_hcd_get_pipe_type(&urb->pipe_info);
+
+	dev_dbg(hsotg->dev, "--Host Channel %d Interrupt: STALL Received--\n",
+		chnum);
+
+	if (hsotg->core_params->dma_desc_enable > 0) {
+		dwc2_hcd_complete_xfer_ddma(hsotg, chan, chnum,
+					    DWC2_HC_XFER_STALL);
+		goto handle_stall_done;
+	}
+
+	if (!urb)
+		goto handle_stall_halt;
+
+	if (pipe_type == USB_ENDPOINT_XFER_CONTROL)
+		dwc2_host_complete(hsotg, urb->priv, urb, -EPIPE);
+
+	if (pipe_type == USB_ENDPOINT_XFER_BULK ||
+	    pipe_type == USB_ENDPOINT_XFER_INT) {
+		dwc2_host_complete(hsotg, urb->priv, urb, -EPIPE);
+		/*
+		 * USB protocol requires resetting the data toggle for bulk
+		 * and interrupt endpoints when a CLEAR_FEATURE(ENDPOINT_HALT)
+		 * setup command is issued to the endpoint. Anticipate the
+		 * CLEAR_FEATURE command since a STALL has occurred and reset
+		 * the data toggle now.
+		 */
+		chan->qh->data_toggle = 0;
+	}
+
+handle_stall_halt:
+	dwc2_halt_channel(hsotg, chan, qtd, DWC2_HC_XFER_STALL);
+
+handle_stall_done:
+	disable_hc_int(hsotg, chnum, HCINTMSK_STALL);
+}
+
+/*
+ * Updates the state of the URB when a transfer has been stopped due to an
+ * abnormal condition before the transfer completes. Modifies the
+ * actual_length field of the URB to reflect the number of bytes that have
+ * actually been transferred via the host channel.
+ */
+static void dwc2_update_urb_state_abn(struct dwc2_hsotg *hsotg,
+				      struct dwc2_host_chan *chan, int chnum,
+				      struct dwc2_hcd_urb *urb,
+				      struct dwc2_qtd *qtd,
+				      enum dwc2_halt_status halt_status)
+{
+	u32 xfer_length = dwc2_get_actual_xfer_length(hsotg, chan, chnum,
+						      qtd, halt_status, NULL);
+	u32 hctsiz;
+
+	if (urb->actual_length + xfer_length > urb->length) {
+		dev_warn(hsotg->dev, "%s(): trimming xfer length\n", __func__);
+		xfer_length = urb->length - urb->actual_length;
+	}
+
+	/* Non DWORD-aligned buffer case handling */
+	if (chan->align_buf && xfer_length && chan->ep_is_in) {
+		dev_dbg(hsotg->dev, "%s(): non-aligned buffer\n", __func__);
+		dma_sync_single_for_cpu(hsotg->dev, urb->dma, urb->length,
+					DMA_FROM_DEVICE);
+		memcpy(urb->buf + urb->actual_length, chan->qh->dw_align_buf,
+		       xfer_length);
+		dma_sync_single_for_device(hsotg->dev, urb->dma, urb->length,
+					   DMA_FROM_DEVICE);
+	}
+
+	urb->actual_length += xfer_length;
+
+	hctsiz = readl(hsotg->regs + HCTSIZ(chnum));
+	dev_vdbg(hsotg->dev, "DWC_otg: %s: %s, channel %d\n",
+		 __func__, (chan->ep_is_in ? "IN" : "OUT"), chnum);
+	dev_vdbg(hsotg->dev, "  chan->start_pkt_count %d\n",
+		 chan->start_pkt_count);
+	dev_vdbg(hsotg->dev, "  hctsiz.pktcnt %d\n",
+		 hctsiz >> TSIZ_PKTCNT_SHIFT &
+		 TSIZ_PKTCNT_MASK >> TSIZ_PKTCNT_SHIFT);
+	dev_vdbg(hsotg->dev, "  chan->max_packet %d\n", chan->max_packet);
+	dev_vdbg(hsotg->dev, "  bytes_transferred %d\n",
+		 xfer_length);
+	dev_vdbg(hsotg->dev, "  urb->actual_length %d\n",
+		 urb->actual_length);
+	dev_vdbg(hsotg->dev, "  urb->transfer_buffer_length %d\n",
+		 urb->length);
+}
+
+/*
+ * Handles a host channel NAK interrupt. This handler may be called in either
+ * DMA mode or Slave mode.
+ */
+static void dwc2_hc_nak_intr(struct dwc2_hsotg *hsotg,
+			     struct dwc2_host_chan *chan, int chnum,
+			     struct dwc2_qtd *qtd)
+{
+	if (dbg_hc(chan))
+		dev_vdbg(hsotg->dev, "--Host Channel %d Interrupt: NAK Received--\n",
+			 chnum);
+
+	/*
+	 * Handle NAK for IN/OUT SSPLIT/CSPLIT transfers, bulk, control, and
+	 * interrupt. Re-start the SSPLIT transfer.
+	 */
+	if (chan->do_split) {
+		if (chan->complete_split)
+			qtd->error_count = 0;
+		qtd->complete_split = 0;
+		dwc2_halt_channel(hsotg, chan, qtd, DWC2_HC_XFER_NAK);
+		goto handle_nak_done;
+	}
+
+	switch (dwc2_hcd_get_pipe_type(&qtd->urb->pipe_info)) {
+	case USB_ENDPOINT_XFER_CONTROL:
+	case USB_ENDPOINT_XFER_BULK:
+		if (hsotg->core_params->dma_enable > 0 && chan->ep_is_in) {
+			/*
+			 * NAK interrupts are enabled on bulk/control IN
+			 * transfers in DMA mode for the sole purpose of
+			 * resetting the error count after a transaction error
+			 * occurs. The core will continue transferring data.
+			 */
+			qtd->error_count = 0;
+			break;
+		}
+
+		/*
+		 * NAK interrupts normally occur during OUT transfers in DMA
+		 * or Slave mode. For IN transfers, more requests will be
+		 * queued as request queue space is available.
+		 */
+		qtd->error_count = 0;
+
+		if (!chan->qh->ping_state) {
+			dwc2_update_urb_state_abn(hsotg, chan, chnum, qtd->urb,
+						  qtd, DWC2_HC_XFER_NAK);
+			dwc2_hcd_save_data_toggle(hsotg, chan, chnum, qtd);
+
+			if (chan->speed == USB_SPEED_HIGH)
+				chan->qh->ping_state = 1;
+		}
+
+		/*
+		 * Halt the channel so the transfer can be re-started from
+		 * the appropriate point or the PING protocol will
+		 * start/continue
+		 */
+		dwc2_halt_channel(hsotg, chan, qtd, DWC2_HC_XFER_NAK);
+		break;
+	case USB_ENDPOINT_XFER_INT:
+		qtd->error_count = 0;
+		dwc2_halt_channel(hsotg, chan, qtd, DWC2_HC_XFER_NAK);
+		break;
+	case USB_ENDPOINT_XFER_ISOC:
+		/* Should never get called for isochronous transfers */
+		dev_err(hsotg->dev, "NACK interrupt for ISOC transfer\n");
+		break;
+	}
+
+handle_nak_done:
+	disable_hc_int(hsotg, chnum, HCINTMSK_NAK);
+}
+
+/*
+ * Handles a host channel ACK interrupt. This interrupt is enabled when
+ * performing the PING protocol in Slave mode, when errors occur during
+ * either Slave mode or DMA mode, and during Start Split transactions.
+ */
+static void dwc2_hc_ack_intr(struct dwc2_hsotg *hsotg,
+			     struct dwc2_host_chan *chan, int chnum,
+			     struct dwc2_qtd *qtd)
+{
+	struct dwc2_hcd_iso_packet_desc *frame_desc;
+
+	if (dbg_hc(chan))
+		dev_vdbg(hsotg->dev, "--Host Channel %d Interrupt: ACK Received--\n",
+			 chnum);
+
+	if (chan->do_split) {
+		/* Handle ACK on SSPLIT. ACK should not occur in CSPLIT. */
+		if (!chan->ep_is_in &&
+		    chan->data_pid_start != DWC2_HC_PID_SETUP)
+			qtd->ssplit_out_xfer_count = chan->xfer_len;
+
+		if (chan->ep_type != USB_ENDPOINT_XFER_ISOC || chan->ep_is_in) {
+			qtd->complete_split = 1;
+			dwc2_halt_channel(hsotg, chan, qtd, DWC2_HC_XFER_ACK);
+		} else {
+			/* ISOC OUT */
+			switch (chan->xact_pos) {
+			case DWC2_HCSPLT_XACTPOS_ALL:
+				break;
+			case DWC2_HCSPLT_XACTPOS_END:
+				qtd->isoc_split_pos = DWC2_HCSPLT_XACTPOS_ALL;
+				qtd->isoc_split_offset = 0;
+				break;
+			case DWC2_HCSPLT_XACTPOS_BEGIN:
+			case DWC2_HCSPLT_XACTPOS_MID:
+				/*
+				 * For BEGIN or MID, calculate the length for
+				 * the next microframe to determine the correct
+				 * SSPLIT token, either MID or END
+				 */
+				frame_desc = &qtd->urb->iso_descs[
+						qtd->isoc_frame_index];
+				qtd->isoc_split_offset += 188;
+
+				if (frame_desc->length - qtd->isoc_split_offset
+							<= 188)
+					qtd->isoc_split_pos =
+							DWC2_HCSPLT_XACTPOS_END;
+				else
+					qtd->isoc_split_pos =
+							DWC2_HCSPLT_XACTPOS_MID;
+				break;
+			}
+		}
+	} else {
+		qtd->error_count = 0;
+
+		if (chan->qh->ping_state) {
+			chan->qh->ping_state = 0;
+			/*
+			 * Halt the channel so the transfer can be re-started
+			 * from the appropriate point. This only happens in
+			 * Slave mode. In DMA mode, the ping_state is cleared
+			 * when the transfer is started because the core
+			 * automatically executes the PING, then the transfer.
+			 */
+			dwc2_halt_channel(hsotg, chan, qtd, DWC2_HC_XFER_ACK);
+		}
+	}
+
+	/*
+	 * If the ACK occurred when _not_ in the PING state, let the channel
+	 * continue transferring data after clearing the error count
+	 */
+	disable_hc_int(hsotg, chnum, HCINTMSK_ACK);
+}
+
+/*
+ * Handles a host channel NYET interrupt. This interrupt should only occur on
+ * Bulk and Control OUT endpoints and for complete split transactions. If a
+ * NYET occurs at the same time as a Transfer Complete interrupt, it is
+ * handled in the xfercomp interrupt handler, not here. This handler may be
+ * called in either DMA mode or Slave mode.
+ */
+static void dwc2_hc_nyet_intr(struct dwc2_hsotg *hsotg,
+			      struct dwc2_host_chan *chan, int chnum,
+			      struct dwc2_qtd *qtd)
+{
+	if (dbg_hc(chan))
+		dev_vdbg(hsotg->dev, "--Host Channel %d Interrupt: NYET Received--\n",
+			 chnum);
+
+	/*
+	 * NYET on CSPLIT
+	 * re-do the CSPLIT immediately on non-periodic
+	 */
+	if (chan->do_split && chan->complete_split) {
+		if (chan->ep_is_in && chan->ep_type == USB_ENDPOINT_XFER_ISOC &&
+		    hsotg->core_params->dma_enable > 0) {
+			qtd->complete_split = 0;
+			qtd->isoc_split_offset = 0;
+			if (qtd->urb &&
+			    ++qtd->isoc_frame_index == qtd->urb->packet_count) {
+				dwc2_host_complete(hsotg, qtd->urb->priv,
+						   qtd->urb, 0);
+				dwc2_release_channel(hsotg, chan, qtd,
+						     DWC2_HC_XFER_URB_COMPLETE);
+			} else {
+				dwc2_release_channel(hsotg, chan, qtd,
+						DWC2_HC_XFER_NO_HALT_STATUS);
+			}
+			goto handle_nyet_done;
+		}
+
+		if (chan->ep_type == USB_ENDPOINT_XFER_INT ||
+		    chan->ep_type == USB_ENDPOINT_XFER_ISOC) {
+			int frnum = dwc2_hcd_get_frame_number(hsotg);
+
+			if (dwc2_full_frame_num(frnum) !=
+			    dwc2_full_frame_num(chan->qh->sched_frame)) {
+				/*
+				 * No longer in the same full speed frame.
+				 * Treat this as a transaction error.
+				 */
+#if 0
+				/*
+				 * Todo: Fix system performance so this can
+				 * be treated as an error. Right now complete
+				 * splits cannot be scheduled precisely enough
+				 * due to other system activity, so this error
+				 * occurs regularly in Slave mode.
+				 */
+				qtd->error_count++;
+#endif
+				qtd->complete_split = 0;
+				dwc2_halt_channel(hsotg, chan, qtd,
+						  DWC2_HC_XFER_XACT_ERR);
+				/* Todo: add support for isoc release */
+				goto handle_nyet_done;
+			}
+		}
+
+		dwc2_halt_channel(hsotg, chan, qtd, DWC2_HC_XFER_NYET);
+		goto handle_nyet_done;
+	}
+
+	chan->qh->ping_state = 1;
+	qtd->error_count = 0;
+
+	dwc2_update_urb_state_abn(hsotg, chan, chnum, qtd->urb, qtd,
+				  DWC2_HC_XFER_NYET);
+	dwc2_hcd_save_data_toggle(hsotg, chan, chnum, qtd);
+
+	/*
+	 * Halt the channel and re-start the transfer so the PING protocol
+	 * will start
+	 */
+	dwc2_halt_channel(hsotg, chan, qtd, DWC2_HC_XFER_NYET);
+
+handle_nyet_done:
+	disable_hc_int(hsotg, chnum, HCINTMSK_NYET);
+}
+
+/*
+ * Handles a host channel babble interrupt. This handler may be called in
+ * either DMA mode or Slave mode.
+ */
+static void dwc2_hc_babble_intr(struct dwc2_hsotg *hsotg,
+				struct dwc2_host_chan *chan, int chnum,
+				struct dwc2_qtd *qtd)
+{
+	dev_dbg(hsotg->dev, "--Host Channel %d Interrupt: Babble Error--\n",
+		chnum);
+
+	if (hsotg->core_params->dma_desc_enable > 0) {
+		dwc2_hcd_complete_xfer_ddma(hsotg, chan, chnum,
+					    DWC2_HC_XFER_BABBLE_ERR);
+		goto handle_babble_done;
+	}
+
+	if (chan->ep_type != USB_ENDPOINT_XFER_ISOC) {
+		if (qtd->urb)
+			dwc2_host_complete(hsotg, qtd->urb->priv, qtd->urb,
+					   -EOVERFLOW);
+		dwc2_halt_channel(hsotg, chan, qtd, DWC2_HC_XFER_BABBLE_ERR);
+	} else {
+		enum dwc2_halt_status halt_status;
+
+		halt_status = dwc2_update_isoc_urb_state(hsotg, chan, chnum,
+						qtd, DWC2_HC_XFER_BABBLE_ERR);
+		dwc2_halt_channel(hsotg, chan, qtd, halt_status);
+	}
+
+handle_babble_done:
+	dwc2_hc_handle_tt_clear(hsotg, chan, qtd);
+	disable_hc_int(hsotg, chnum, HCINTMSK_BBLERR);
+}
+
+/*
+ * Handles a host channel AHB error interrupt. This handler is only called in
+ * DMA mode.
+ */
+static void dwc2_hc_ahberr_intr(struct dwc2_hsotg *hsotg,
+				struct dwc2_host_chan *chan, int chnum,
+				struct dwc2_qtd *qtd)
+{
+	struct dwc2_hcd_urb *urb = qtd->urb;
+	char *pipetype, *speed;
+	u32 hcchar;
+	u32 hcsplt;
+	u32 hctsiz;
+	u32 hc_dma;
+
+	dev_dbg(hsotg->dev, "--Host Channel %d Interrupt: AHB Error--\n",
+		chnum);
+
+	if (!urb)
+		goto handle_ahberr_halt;
+
+	hcchar = readl(hsotg->regs + HCCHAR(chnum));
+	hcsplt = readl(hsotg->regs + HCSPLT(chnum));
+	hctsiz = readl(hsotg->regs + HCTSIZ(chnum));
+	hc_dma = readl(hsotg->regs + HCDMA(chnum));
+
+	dev_err(hsotg->dev, "AHB ERROR, Channel %d\n", chnum);
+	dev_err(hsotg->dev, "  hcchar 0x%08x, hcsplt 0x%08x\n", hcchar, hcsplt);
+	dev_err(hsotg->dev, "  hctsiz 0x%08x, hc_dma 0x%08x\n", hctsiz, hc_dma);
+	dev_err(hsotg->dev, "  Device address: %d\n",
+		dwc2_hcd_get_dev_addr(&urb->pipe_info));
+	dev_err(hsotg->dev, "  Endpoint: %d, %s\n",
+		dwc2_hcd_get_ep_num(&urb->pipe_info),
+		dwc2_hcd_is_pipe_in(&urb->pipe_info) ? "IN" : "OUT");
+
+	switch (dwc2_hcd_get_pipe_type(&urb->pipe_info)) {
+	case USB_ENDPOINT_XFER_CONTROL:
+		pipetype = "CONTROL";
+		break;
+	case USB_ENDPOINT_XFER_BULK:
+		pipetype = "BULK";
+		break;
+	case USB_ENDPOINT_XFER_INT:
+		pipetype = "INTERRUPT";
+		break;
+	case USB_ENDPOINT_XFER_ISOC:
+		pipetype = "ISOCHRONOUS";
+		break;
+	default:
+		pipetype = "UNKNOWN";
+		break;
+	}
+
+	dev_err(hsotg->dev, "  Endpoint type: %s\n", pipetype);
+
+	switch (chan->speed) {
+	case USB_SPEED_HIGH:
+		speed = "HIGH";
+		break;
+	case USB_SPEED_FULL:
+		speed = "FULL";
+		break;
+	case USB_SPEED_LOW:
+		speed = "LOW";
+		break;
+	default:
+		speed = "UNKNOWN";
+		break;
+	}
+
+	dev_err(hsotg->dev, "  Speed: %s\n", speed);
+
+	dev_err(hsotg->dev, "  Max packet size: %d\n",
+		dwc2_hcd_get_mps(&urb->pipe_info));
+	dev_err(hsotg->dev, "  Data buffer length: %d\n", urb->length);
+	dev_err(hsotg->dev, "  Transfer buffer: %p, Transfer DMA: %08lx\n",
+		urb->buf, (unsigned long)urb->dma);
+	dev_err(hsotg->dev, "  Setup buffer: %p, Setup DMA: %08lx\n",
+		urb->setup_packet, (unsigned long)urb->setup_dma);
+	dev_err(hsotg->dev, "  Interval: %d\n", urb->interval);
+
+	/* Core halts the channel for Descriptor DMA mode */
+	if (hsotg->core_params->dma_desc_enable > 0) {
+		dwc2_hcd_complete_xfer_ddma(hsotg, chan, chnum,
+					    DWC2_HC_XFER_AHB_ERR);
+		goto handle_ahberr_done;
+	}
+
+	dwc2_host_complete(hsotg, urb->priv, urb, -EIO);
+
+handle_ahberr_halt:
+	/*
+	 * Force a channel halt. Don't call dwc2_halt_channel because that won't
+	 * write to the HCCHARn register in DMA mode to force the halt.
+	 */
+	dwc2_hc_halt(hsotg, chan, DWC2_HC_XFER_AHB_ERR);
+
+handle_ahberr_done:
+	dwc2_hc_handle_tt_clear(hsotg, chan, qtd);
+	disable_hc_int(hsotg, chnum, HCINTMSK_AHBERR);
+}
+
+/*
+ * Handles a host channel transaction error interrupt. This handler may be
+ * called in either DMA mode or Slave mode.
+ */
+static void dwc2_hc_xacterr_intr(struct dwc2_hsotg *hsotg,
+				 struct dwc2_host_chan *chan, int chnum,
+				 struct dwc2_qtd *qtd)
+{
+	dev_dbg(hsotg->dev,
+		"--Host Channel %d Interrupt: Transaction Error--\n", chnum);
+
+	if (hsotg->core_params->dma_desc_enable > 0) {
+		dwc2_hcd_complete_xfer_ddma(hsotg, chan, chnum,
+					    DWC2_HC_XFER_XACT_ERR);
+		goto handle_xacterr_done;
+	}
+
+	switch (dwc2_hcd_get_pipe_type(&qtd->urb->pipe_info)) {
+	case USB_ENDPOINT_XFER_CONTROL:
+	case USB_ENDPOINT_XFER_BULK:
+		qtd->error_count++;
+		if (!chan->qh->ping_state) {
+
+			dwc2_update_urb_state_abn(hsotg, chan, chnum, qtd->urb,
+						  qtd, DWC2_HC_XFER_XACT_ERR);
+			dwc2_hcd_save_data_toggle(hsotg, chan, chnum, qtd);
+			if (!chan->ep_is_in && chan->speed == USB_SPEED_HIGH)
+				chan->qh->ping_state = 1;
+		}
+
+		/*
+		 * Halt the channel so the transfer can be re-started from
+		 * the appropriate point or the PING protocol will start
+		 */
+		dwc2_halt_channel(hsotg, chan, qtd, DWC2_HC_XFER_XACT_ERR);
+		break;
+	case USB_ENDPOINT_XFER_INT:
+		qtd->error_count++;
+		if (chan->do_split && chan->complete_split)
+			qtd->complete_split = 0;
+		dwc2_halt_channel(hsotg, chan, qtd, DWC2_HC_XFER_XACT_ERR);
+		break;
+	case USB_ENDPOINT_XFER_ISOC:
+		{
+			enum dwc2_halt_status halt_status;
+
+			halt_status = dwc2_update_isoc_urb_state(hsotg, chan,
+					chnum, qtd, DWC2_HC_XFER_XACT_ERR);
+			dwc2_halt_channel(hsotg, chan, qtd, halt_status);
+		}
+		break;
+	}
+
+handle_xacterr_done:
+	dwc2_hc_handle_tt_clear(hsotg, chan, qtd);
+	disable_hc_int(hsotg, chnum, HCINTMSK_XACTERR);
+}
+
+/*
+ * Handles a host channel frame overrun interrupt. This handler may be called
+ * in either DMA mode or Slave mode.
+ */
+static void dwc2_hc_frmovrun_intr(struct dwc2_hsotg *hsotg,
+				  struct dwc2_host_chan *chan, int chnum,
+				  struct dwc2_qtd *qtd)
+{
+	enum dwc2_halt_status halt_status;
+
+	if (dbg_hc(chan))
+		dev_dbg(hsotg->dev, "--Host Channel %d Interrupt: Frame Overrun--\n",
+			chnum);
+
+	switch (dwc2_hcd_get_pipe_type(&qtd->urb->pipe_info)) {
+	case USB_ENDPOINT_XFER_CONTROL:
+	case USB_ENDPOINT_XFER_BULK:
+		break;
+	case USB_ENDPOINT_XFER_INT:
+		dwc2_halt_channel(hsotg, chan, qtd, DWC2_HC_XFER_FRAME_OVERRUN);
+		break;
+	case USB_ENDPOINT_XFER_ISOC:
+		halt_status = dwc2_update_isoc_urb_state(hsotg, chan, chnum,
+					qtd, DWC2_HC_XFER_FRAME_OVERRUN);
+		dwc2_halt_channel(hsotg, chan, qtd, halt_status);
+		break;
+	}
+
+	dwc2_hc_handle_tt_clear(hsotg, chan, qtd);
+	disable_hc_int(hsotg, chnum, HCINTMSK_FRMOVRUN);
+}
+
+/*
+ * Handles a host channel data toggle error interrupt. This handler may be
+ * called in either DMA mode or Slave mode.
+ */
+static void dwc2_hc_datatglerr_intr(struct dwc2_hsotg *hsotg,
+				    struct dwc2_host_chan *chan, int chnum,
+				    struct dwc2_qtd *qtd)
+{
+	dev_dbg(hsotg->dev,
+		"--Host Channel %d Interrupt: Data Toggle Error--\n", chnum);
+
+	if (chan->ep_is_in)
+		qtd->error_count = 0;
+	else
+		dev_err(hsotg->dev,
+			"Data Toggle Error on OUT transfer, channel %d\n",
+			chnum);
+
+	dwc2_hc_handle_tt_clear(hsotg, chan, qtd);
+	disable_hc_int(hsotg, chnum, HCINTMSK_DATATGLERR);
+}
+
+/*
+ * For debug only. It checks that a valid halt status is set and that
+ * HCCHARn.chdis is clear. If there's a problem, corrective action is
+ * taken and a warning is issued.
+ *
+ * Return: true if halt status is ok, false otherwise
+ */
+static bool dwc2_halt_status_ok(struct dwc2_hsotg *hsotg,
+				struct dwc2_host_chan *chan, int chnum,
+				struct dwc2_qtd *qtd)
+{
+#ifdef DEBUG
+	u32 hcchar;
+	u32 hctsiz;
+	u32 hcintmsk;
+	u32 hcsplt;
+
+	if (chan->halt_status == DWC2_HC_XFER_NO_HALT_STATUS) {
+		/*
+		 * This code is here only as a check. This condition should
+		 * never happen. Ignore the halt if it does occur.
+		 */
+		hcchar = readl(hsotg->regs + HCCHAR(chnum));
+		hctsiz = readl(hsotg->regs + HCTSIZ(chnum));
+		hcintmsk = readl(hsotg->regs + HCINTMSK(chnum));
+		hcsplt = readl(hsotg->regs + HCSPLT(chnum));
+		dev_dbg(hsotg->dev,
+			"%s: chan->halt_status DWC2_HC_XFER_NO_HALT_STATUS,\n",
+			 __func__);
+		dev_dbg(hsotg->dev,
+			"channel %d, hcchar 0x%08x, hctsiz 0x%08x,\n",
+			chnum, hcchar, hctsiz);
+		dev_dbg(hsotg->dev,
+			"hcint 0x%08x, hcintmsk 0x%08x, hcsplt 0x%08x,\n",
+			chan->hcint, hcintmsk, hcsplt);
+		if (qtd)
+			dev_dbg(hsotg->dev, "qtd->complete_split %d\n",
+				qtd->complete_split);
+		dev_warn(hsotg->dev,
+			 "%s: no halt status, channel %d, ignoring interrupt\n",
+			 __func__, chnum);
+		return false;
+	}
+
+	/*
+	 * This code is here only as a check. hcchar.chdis should never be set
+	 * when the halt interrupt occurs. Halt the channel again if it does
+	 * occur.
+	 */
+	hcchar = readl(hsotg->regs + HCCHAR(chnum));
+	if (hcchar & HCCHAR_CHDIS) {
+		dev_warn(hsotg->dev,
+			 "%s: hcchar.chdis set unexpectedly, hcchar 0x%08x, trying to halt again\n",
+			 __func__, hcchar);
+		chan->halt_pending = 0;
+		dwc2_halt_channel(hsotg, chan, qtd, chan->halt_status);
+		return false;
+	}
+#endif
+
+	return true;
+}
+
+/*
+ * Handles a host Channel Halted interrupt in DMA mode. This handler
+ * determines the reason the channel halted and proceeds accordingly.
+ */
+static void dwc2_hc_chhltd_intr_dma(struct dwc2_hsotg *hsotg,
+				    struct dwc2_host_chan *chan, int chnum,
+				    struct dwc2_qtd *qtd)
+{
+	u32 hcintmsk;
+	int out_nak_enh = 0;
+
+	if (dbg_hc(chan))
+		dev_vdbg(hsotg->dev,
+			 "--Host Channel %d Interrupt: DMA Channel Halted--\n",
+			 chnum);
+
+	/*
+	 * For core with OUT NAK enhancement, the flow for high-speed
+	 * CONTROL/BULK OUT is handled a little differently
+	 */
+	if (hsotg->snpsid >= DWC2_CORE_REV_2_71a) {
+		if (chan->speed == USB_SPEED_HIGH && !chan->ep_is_in &&
+		    (chan->ep_type == USB_ENDPOINT_XFER_CONTROL ||
+		     chan->ep_type == USB_ENDPOINT_XFER_BULK)) {
+			out_nak_enh = 1;
+		}
+	}
+
+	if (chan->halt_status == DWC2_HC_XFER_URB_DEQUEUE ||
+	    (chan->halt_status == DWC2_HC_XFER_AHB_ERR &&
+	     hsotg->core_params->dma_desc_enable <= 0)) {
+		if (hsotg->core_params->dma_desc_enable > 0)
+			dwc2_hcd_complete_xfer_ddma(hsotg, chan, chnum,
+						    chan->halt_status);
+		else
+			/*
+			 * Just release the channel. A dequeue can happen on a
+			 * transfer timeout. In the case of an AHB Error, the
+			 * channel was forced to halt because there's no way to
+			 * gracefully recover.
+			 */
+			dwc2_release_channel(hsotg, chan, qtd,
+					     chan->halt_status);
+		return;
+	}
+
+	hcintmsk = readl(hsotg->regs + HCINTMSK(chnum));
+
+	if (chan->hcint & HCINTMSK_XFERCOMPL) {
+		/*
+		 * Todo: This is here because of a possible hardware bug. Spec
+		 * says that on SPLIT-ISOC OUT transfers in DMA mode that a HALT
+		 * interrupt w/ACK bit set should occur, but I only see the
+		 * XFERCOMP bit, even with it masked out. This is a workaround
+		 * for that behavior. Should fix this when hardware is fixed.
+		 */
+		if (chan->ep_type == USB_ENDPOINT_XFER_ISOC && !chan->ep_is_in)
+			dwc2_hc_ack_intr(hsotg, chan, chnum, qtd);
+		dwc2_hc_xfercomp_intr(hsotg, chan, chnum, qtd);
+	} else if (chan->hcint & HCINTMSK_STALL) {
+		dwc2_hc_stall_intr(hsotg, chan, chnum, qtd);
+	} else if ((chan->hcint & HCINTMSK_XACTERR) &&
+		   hsotg->core_params->dma_desc_enable <= 0) {
+		if (out_nak_enh) {
+			if (chan->hcint &
+			    (HCINTMSK_NYET | HCINTMSK_NAK | HCINTMSK_ACK)) {
+				dev_vdbg(hsotg->dev,
+					 "XactErr with NYET/NAK/ACK\n");
+				qtd->error_count = 0;
+			} else {
+				dev_vdbg(hsotg->dev,
+					 "XactErr without NYET/NAK/ACK\n");
+			}
+		}
+
+		/*
+		 * Must handle xacterr before nak or ack. Could get a xacterr
+		 * at the same time as either of these on a BULK/CONTROL OUT
+		 * that started with a PING. The xacterr takes precedence.
+		 */
+		dwc2_hc_xacterr_intr(hsotg, chan, chnum, qtd);
+	} else if ((chan->hcint & HCINTMSK_XCS_XACT) &&
+		   hsotg->core_params->dma_desc_enable > 0) {
+		dwc2_hc_xacterr_intr(hsotg, chan, chnum, qtd);
+	} else if ((chan->hcint & HCINTMSK_AHBERR) &&
+		   hsotg->core_params->dma_desc_enable > 0) {
+		dwc2_hc_ahberr_intr(hsotg, chan, chnum, qtd);
+	} else if (chan->hcint & HCINTMSK_BBLERR) {
+		dwc2_hc_babble_intr(hsotg, chan, chnum, qtd);
+	} else if (chan->hcint & HCINTMSK_FRMOVRUN) {
+		dwc2_hc_frmovrun_intr(hsotg, chan, chnum, qtd);
+	} else if (!out_nak_enh) {
+		if (chan->hcint & HCINTMSK_NYET) {
+			/*
+			 * Must handle nyet before nak or ack. Could get a nyet
+			 * at the same time as either of those on a BULK/CONTROL
+			 * OUT that started with a PING. The nyet takes
+			 * precedence.
+			 */
+			dwc2_hc_nyet_intr(hsotg, chan, chnum, qtd);
+		} else if ((chan->hcint & HCINTMSK_NAK) &&
+			   !(hcintmsk & HCINTMSK_NAK)) {
+			/*
+			 * If nak is not masked, it's because a non-split IN
+			 * transfer is in an error state. In that case, the nak
+			 * is handled by the nak interrupt handler, not here.
+			 * Handle nak here for BULK/CONTROL OUT transfers, which
+			 * halt on a NAK to allow rewinding the buffer pointer.
+			 */
+			dwc2_hc_nak_intr(hsotg, chan, chnum, qtd);
+		} else if ((chan->hcint & HCINTMSK_ACK) &&
+			   !(hcintmsk & HCINTMSK_ACK)) {
+			/*
+			 * If ack is not masked, it's because a non-split IN
+			 * transfer is in an error state. In that case, the ack
+			 * is handled by the ack interrupt handler, not here.
+			 * Handle ack here for split transfers. Start splits
+			 * halt on ACK.
+			 */
+			dwc2_hc_ack_intr(hsotg, chan, chnum, qtd);
+		} else {
+			if (chan->ep_type == USB_ENDPOINT_XFER_INT ||
+			    chan->ep_type == USB_ENDPOINT_XFER_ISOC) {
+				/*
+				 * A periodic transfer halted with no other
+				 * channel interrupts set. Assume it was halted
+				 * by the core because it could not be completed
+				 * in its scheduled (micro)frame.
+				 */
+				dev_dbg(hsotg->dev,
+					"%s: Halt channel %d (assume incomplete periodic transfer)\n",
+					__func__, chnum);
+				dwc2_halt_channel(hsotg, chan, qtd,
+					DWC2_HC_XFER_PERIODIC_INCOMPLETE);
+			} else {
+				dev_err(hsotg->dev,
+					"%s: Channel %d - ChHltd set, but reason is unknown\n",
+					__func__, chnum);
+				dev_err(hsotg->dev,
+					"hcint 0x%08x, intsts 0x%08x\n",
+					chan->hcint,
+					readl(hsotg->regs + GINTSTS));
+			}
+		}
+	} else {
+		dev_info(hsotg->dev,
+			 "NYET/NAK/ACK/other in non-error case, 0x%08x\n",
+			 chan->hcint);
+	}
+}
+
+/*
+ * Handles a host channel Channel Halted interrupt
+ *
+ * In slave mode, this handler is called only when the driver specifically
+ * requests a halt. This occurs during handling other host channel interrupts
+ * (e.g. nak, xacterr, stall, nyet, etc.).
+ *
+ * In DMA mode, this is the interrupt that occurs when the core has finished
+ * processing a transfer on a channel. Other host channel interrupts (except
+ * ahberr) are disabled in DMA mode.
+ */
+static void dwc2_hc_chhltd_intr(struct dwc2_hsotg *hsotg,
+				struct dwc2_host_chan *chan, int chnum,
+				struct dwc2_qtd *qtd)
+{
+	if (dbg_hc(chan))
+		dev_vdbg(hsotg->dev, "--Host Channel %d Interrupt: Channel Halted--\n",
+			 chnum);
+
+	if (hsotg->core_params->dma_enable > 0) {
+		dwc2_hc_chhltd_intr_dma(hsotg, chan, chnum, qtd);
+	} else {
+		if (!dwc2_halt_status_ok(hsotg, chan, chnum, qtd))
+			return;
+		dwc2_release_channel(hsotg, chan, qtd, chan->halt_status);
+	}
+}
+
+/* Handles interrupt for a specific Host Channel */
+static void dwc2_hc_n_intr(struct dwc2_hsotg *hsotg, int chnum)
+{
+	struct dwc2_qtd *qtd;
+	struct dwc2_host_chan *chan;
+	u32 hcint, hcintmsk;
+
+	chan = hsotg->hc_ptr_array[chnum];
+
+	if (dbg_hc(chan))
+		dev_vdbg(hsotg->dev, "--Host Channel Interrupt--, Channel %d\n",
+			 chnum);
+
+	hcint = readl(hsotg->regs + HCINT(chnum));
+	hcintmsk = readl(hsotg->regs + HCINTMSK(chnum));
+	if (dbg_hc(chan))
+		dev_vdbg(hsotg->dev,
+			 "  hcint 0x%08x, hcintmsk 0x%08x, hcint&hcintmsk 0x%08x\n",
+			 hcint, hcintmsk, hcint & hcintmsk);
+
+	if (!chan) {
+		dev_err(hsotg->dev, "## hc_ptr_array for channel is NULL ##\n");
+		writel(hcint, hsotg->regs + HCINT(chnum));
+		return;
+	}
+
+	writel(hcint, hsotg->regs + HCINT(chnum));
+	chan->hcint = hcint;
+	hcint &= hcintmsk;
+
+	/*
+	 * If the channel was halted due to a dequeue, the qtd list might
+	 * be empty or at least the first entry will not be the active qtd.
+	 * In this case, take a shortcut and just release the channel.
+	 */
+	if (chan->halt_status == DWC2_HC_XFER_URB_DEQUEUE) {
+		/*
+		 * If the channel was halted, this should be the only
+		 * interrupt unmasked
+		 */
+		WARN_ON(hcint != HCINTMSK_CHHLTD);
+		if (hsotg->core_params->dma_desc_enable > 0)
+			dwc2_hcd_complete_xfer_ddma(hsotg, chan, chnum,
+						    chan->halt_status);
+		else
+			dwc2_release_channel(hsotg, chan, NULL,
+					     chan->halt_status);
+		return;
+	}
+
+	if (list_empty(&chan->qh->qtd_list)) {
+		/*
+		 * TODO: Will this ever happen with the
+		 * DWC2_HC_XFER_URB_DEQUEUE handling above?
+		 */
+		dev_dbg(hsotg->dev, "## no QTD queued for channel %d ##\n",
+			chnum);
+		dev_dbg(hsotg->dev,
+			"  hcint 0x%08x, hcintmsk 0x%08x, hcint&hcintmsk 0x%08x\n",
+			chan->hcint, hcintmsk, hcint);
+		chan->halt_status = DWC2_HC_XFER_NO_HALT_STATUS;
+		disable_hc_int(hsotg, chnum, HCINTMSK_CHHLTD);
+		chan->hcint = 0;
+		return;
+	}
+
+	qtd = list_first_entry(&chan->qh->qtd_list, struct dwc2_qtd,
+			       qtd_list_entry);
+
+	if (hsotg->core_params->dma_enable <= 0) {
+		if ((hcint & HCINTMSK_CHHLTD) && hcint != HCINTMSK_CHHLTD)
+			hcint &= ~HCINTMSK_CHHLTD;
+	}
+
+	if (hcint & HCINTMSK_XFERCOMPL) {
+		dwc2_hc_xfercomp_intr(hsotg, chan, chnum, qtd);
+		/*
+		 * If NYET occurred at same time as Xfer Complete, the NYET is
+		 * handled by the Xfer Complete interrupt handler. Don't want
+		 * to call the NYET interrupt handler in this case.
+		 */
+		hcint &= ~HCINTMSK_NYET;
+	}
+	if (hcint & HCINTMSK_CHHLTD)
+		dwc2_hc_chhltd_intr(hsotg, chan, chnum, qtd);
+	if (hcint & HCINTMSK_AHBERR)
+		dwc2_hc_ahberr_intr(hsotg, chan, chnum, qtd);
+	if (hcint & HCINTMSK_STALL)
+		dwc2_hc_stall_intr(hsotg, chan, chnum, qtd);
+	if (hcint & HCINTMSK_NAK)
+		dwc2_hc_nak_intr(hsotg, chan, chnum, qtd);
+	if (hcint & HCINTMSK_ACK)
+		dwc2_hc_ack_intr(hsotg, chan, chnum, qtd);
+	if (hcint & HCINTMSK_NYET)
+		dwc2_hc_nyet_intr(hsotg, chan, chnum, qtd);
+	if (hcint & HCINTMSK_XACTERR)
+		dwc2_hc_xacterr_intr(hsotg, chan, chnum, qtd);
+	if (hcint & HCINTMSK_BBLERR)
+		dwc2_hc_babble_intr(hsotg, chan, chnum, qtd);
+	if (hcint & HCINTMSK_FRMOVRUN)
+		dwc2_hc_frmovrun_intr(hsotg, chan, chnum, qtd);
+	if (hcint & HCINTMSK_DATATGLERR)
+		dwc2_hc_datatglerr_intr(hsotg, chan, chnum, qtd);
+
+	chan->hcint = 0;
+}
+
+/*
+ * This interrupt indicates that one or more host channels has a pending
+ * interrupt. There are multiple conditions that can cause each host channel
+ * interrupt. This function determines which conditions have occurred for each
+ * host channel interrupt and handles them appropriately.
+ */
+static void dwc2_hc_intr(struct dwc2_hsotg *hsotg)
+{
+	u32 haint;
+	int i;
+
+	haint = readl(hsotg->regs + HAINT);
+	if (dbg_perio()) {
+		dev_vdbg(hsotg->dev, "%s()\n", __func__);
+
+		dev_vdbg(hsotg->dev, "HAINT=%08x\n", haint);
+	}
+
+	for (i = 0; i < hsotg->core_params->host_channels; i++) {
+		if (haint & (1 << i))
+			dwc2_hc_n_intr(hsotg, i);
+	}
+}
+
+/* This function handles interrupts for the HCD */
+int dwc2_hcd_intr(struct dwc2_hsotg *hsotg)
+{
+	u32 gintsts, dbg_gintsts;
+	int retval = 0;
+
+	if (dwc2_check_core_status(hsotg) < 0) {
+		dev_warn(hsotg->dev, "Controller is disconnected\n");
+		return 0;
+	}
+
+	spin_lock(&hsotg->lock);
+
+	/* Check if HOST Mode */
+	if (dwc2_is_host_mode(hsotg)) {
+		gintsts = dwc2_read_core_intr(hsotg);
+		if (!gintsts) {
+			spin_unlock(&hsotg->lock);
+			return 0;
+		}
+
+		retval = 1;
+
+		dbg_gintsts = gintsts;
+#ifndef DEBUG_SOF
+		dbg_gintsts &= ~GINTSTS_SOF;
+#endif
+		if (!dbg_perio())
+			dbg_gintsts &= ~(GINTSTS_HCHINT | GINTSTS_RXFLVL |
+					 GINTSTS_PTXFEMP);
+
+		/* Only print if there are any non-suppressed interrupts left */
+		if (dbg_gintsts)
+			dev_vdbg(hsotg->dev,
+				 "DWC OTG HCD Interrupt Detected gintsts&gintmsk=0x%08x\n",
+				 gintsts);
+
+		if (gintsts & GINTSTS_SOF)
+			dwc2_sof_intr(hsotg);
+		if (gintsts & GINTSTS_RXFLVL)
+			dwc2_rx_fifo_level_intr(hsotg);
+		if (gintsts & GINTSTS_NPTXFEMP)
+			dwc2_np_tx_fifo_empty_intr(hsotg);
+		if (gintsts & GINTSTS_I2CINT)
+			/* Todo: Implement i2cintr handler */
+			writel(GINTSTS_I2CINT, hsotg->regs + GINTSTS);
+		if (gintsts & GINTSTS_PRTINT)
+			dwc2_port_intr(hsotg);
+		if (gintsts & GINTSTS_HCHINT)
+			dwc2_hc_intr(hsotg);
+		if (gintsts & GINTSTS_PTXFEMP)
+			dwc2_perio_tx_fifo_empty_intr(hsotg);
+
+		if (dbg_gintsts) {
+			dev_vdbg(hsotg->dev,
+				 "DWC OTG HCD Finished Servicing Interrupts\n");
+			dev_vdbg(hsotg->dev,
+				 "DWC OTG HCD gintsts=0x%08x gintmsk=0x%08x\n",
+				 readl(hsotg->regs + GINTSTS),
+				 readl(hsotg->regs + GINTMSK));
+		}
+	}
+
+	spin_unlock(&hsotg->lock);
+
+	return retval;
+}
diff --git a/drivers/staging/dwc2/hcd_queue.c b/drivers/staging/dwc2/hcd_queue.c
new file mode 100644
index 00000000000..b36f783dd3e
--- /dev/null
+++ b/drivers/staging/dwc2/hcd_queue.c
@@ -0,0 +1,677 @@
+/*
+ * hcd_queue.c - DesignWare HS OTG Controller host queuing routines
+ *
+ * Copyright (C) 2004-2013 Synopsys, Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions, and the following disclaimer,
+ *    without modification.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The names of the above-listed copyright holders may not be used
+ *    to endorse or promote products derived from this software without
+ *    specific prior written permission.
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation; either version 2 of the License, or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+ * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * This file contains the functions to manage Queue Heads and Queue
+ * Transfer Descriptors for Host mode
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/spinlock.h>
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/usb.h>
+
+#include <linux/usb/hcd.h>
+#include <linux/usb/ch11.h>
+
+#include "core.h"
+#include "hcd.h"
+
+/**
+ * dwc2_qh_init() - Initializes a QH structure
+ *
+ * @hsotg: The HCD state structure for the DWC OTG controller
+ * @qh:    The QH to init
+ * @urb:   Holds the information about the device/endpoint needed to initialize
+ *         the QH
+ */
+#define SCHEDULE_SLOP 10
+static void dwc2_qh_init(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh,
+			 struct dwc2_hcd_urb *urb)
+{
+	int dev_speed, hub_addr, hub_port;
+	char *speed, *type;
+
+	dev_vdbg(hsotg->dev, "%s()\n", __func__);
+
+	/* Initialize QH */
+	qh->ep_type = dwc2_hcd_get_pipe_type(&urb->pipe_info);
+	qh->ep_is_in = dwc2_hcd_is_pipe_in(&urb->pipe_info) ? 1 : 0;
+
+	qh->data_toggle = DWC2_HC_PID_DATA0;
+	qh->maxp = dwc2_hcd_get_mps(&urb->pipe_info);
+	INIT_LIST_HEAD(&qh->qtd_list);
+	INIT_LIST_HEAD(&qh->qh_list_entry);
+
+	/* FS/LS Endpoint on HS Hub, NOT virtual root hub */
+	dev_speed = dwc2_host_get_speed(hsotg, urb->priv);
+
+	dwc2_host_hub_info(hsotg, urb->priv, &hub_addr, &hub_port);
+
+	if ((dev_speed == USB_SPEED_LOW || dev_speed == USB_SPEED_FULL) &&
+	    hub_addr != 0 && hub_addr != 1) {
+		dev_vdbg(hsotg->dev,
+			 "QH init: EP %d: TT found at hub addr %d, for port %d\n",
+			 dwc2_hcd_get_ep_num(&urb->pipe_info), hub_addr,
+			 hub_port);
+		qh->do_split = 1;
+	}
+
+	if (qh->ep_type == USB_ENDPOINT_XFER_INT ||
+	    qh->ep_type == USB_ENDPOINT_XFER_ISOC) {
+		/* Compute scheduling parameters once and save them */
+		u32 hprt, prtspd;
+
+		/* Todo: Account for split transfers in the bus time */
+		int bytecount =
+			dwc2_hb_mult(qh->maxp) * dwc2_max_packet(qh->maxp);
+
+		qh->usecs = NS_TO_US(usb_calc_bus_time(qh->do_split ?
+				USB_SPEED_HIGH : dev_speed, qh->ep_is_in,
+				qh->ep_type == USB_ENDPOINT_XFER_ISOC,
+				bytecount));
+		/* Start in a slightly future (micro)frame */
+		qh->sched_frame = dwc2_frame_num_inc(hsotg->frame_number,
+						     SCHEDULE_SLOP);
+		qh->interval = urb->interval;
+#if 0
+		/* Increase interrupt polling rate for debugging */
+		if (qh->ep_type == USB_ENDPOINT_XFER_INT)
+			qh->interval = 8;
+#endif
+		hprt = readl(hsotg->regs + HPRT0);
+		prtspd = hprt & HPRT0_SPD_MASK;
+		if (prtspd == HPRT0_SPD_HIGH_SPEED &&
+		    (dev_speed == USB_SPEED_LOW ||
+		     dev_speed == USB_SPEED_FULL)) {
+			qh->interval *= 8;
+			qh->sched_frame |= 0x7;
+			qh->start_split_frame = qh->sched_frame;
+		}
+		dev_dbg(hsotg->dev, "interval=%d\n", qh->interval);
+	}
+
+	dev_vdbg(hsotg->dev, "DWC OTG HCD QH Initialized\n");
+	dev_vdbg(hsotg->dev, "DWC OTG HCD QH - qh = %p\n", qh);
+	dev_vdbg(hsotg->dev, "DWC OTG HCD QH - Device Address = %d\n",
+		 dwc2_hcd_get_dev_addr(&urb->pipe_info));
+	dev_vdbg(hsotg->dev, "DWC OTG HCD QH - Endpoint %d, %s\n",
+		 dwc2_hcd_get_ep_num(&urb->pipe_info),
+		 dwc2_hcd_is_pipe_in(&urb->pipe_info) ? "IN" : "OUT");
+
+	qh->dev_speed = dev_speed;
+
+	switch (dev_speed) {
+	case USB_SPEED_LOW:
+		speed = "low";
+		break;
+	case USB_SPEED_FULL:
+		speed = "full";
+		break;
+	case USB_SPEED_HIGH:
+		speed = "high";
+		break;
+	default:
+		speed = "?";
+		break;
+	}
+	dev_vdbg(hsotg->dev, "DWC OTG HCD QH - Speed = %s\n", speed);
+
+	switch (qh->ep_type) {
+	case USB_ENDPOINT_XFER_ISOC:
+		type = "isochronous";
+		break;
+	case USB_ENDPOINT_XFER_INT:
+		type = "interrupt";
+		break;
+	case USB_ENDPOINT_XFER_CONTROL:
+		type = "control";
+		break;
+	case USB_ENDPOINT_XFER_BULK:
+		type = "bulk";
+		break;
+	default:
+		type = "?";
+		break;
+	}
+
+	dev_vdbg(hsotg->dev, "DWC OTG HCD QH - Type = %s\n", type);
+
+	if (qh->ep_type == USB_ENDPOINT_XFER_INT) {
+		dev_vdbg(hsotg->dev, "DWC OTG HCD QH - usecs = %d\n",
+			 qh->usecs);
+		dev_vdbg(hsotg->dev, "DWC OTG HCD QH - interval = %d\n",
+			 qh->interval);
+	}
+}
+
+/**
+ * dwc2_hcd_qh_create() - Allocates and initializes a QH
+ *
+ * @hsotg:        The HCD state structure for the DWC OTG controller
+ * @urb:          Holds the information about the device/endpoint needed
+ *                to initialize the QH
+ * @atomic_alloc: Flag to do atomic allocation if needed
+ *
+ * Return: Pointer to the newly allocated QH, or NULL on error
+ */
+static struct dwc2_qh *dwc2_hcd_qh_create(struct dwc2_hsotg *hsotg,
+					  struct dwc2_hcd_urb *urb,
+					  gfp_t mem_flags)
+{
+	struct dwc2_qh *qh;
+
+	/* Allocate memory */
+	qh = kzalloc(sizeof(*qh), mem_flags);
+	if (!qh)
+		return NULL;
+
+	dwc2_qh_init(hsotg, qh, urb);
+
+	if (hsotg->core_params->dma_desc_enable > 0 &&
+	    dwc2_hcd_qh_init_ddma(hsotg, qh, mem_flags) < 0) {
+		dwc2_hcd_qh_free(hsotg, qh);
+		return NULL;
+	}
+
+	return qh;
+}
+
+/**
+ * dwc2_hcd_qh_free() - Frees the QH
+ *
+ * @hsotg: HCD instance
+ * @qh:    The QH to free
+ *
+ * QH should already be removed from the list. QTD list should already be empty
+ * if called from URB Dequeue.
+ *
+ * Must NOT be called with interrupt disabled or spinlock held
+ */
+void dwc2_hcd_qh_free(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh)
+{
+	u32 buf_size;
+
+	if (hsotg->core_params->dma_desc_enable > 0) {
+		dwc2_hcd_qh_free_ddma(hsotg, qh);
+	} else if (qh->dw_align_buf) {
+		if (qh->ep_type == USB_ENDPOINT_XFER_ISOC)
+			buf_size = 4096;
+		else
+			buf_size = hsotg->core_params->max_transfer_size;
+		dma_free_coherent(hsotg->dev, buf_size, qh->dw_align_buf,
+				  qh->dw_align_buf_dma);
+	}
+
+	kfree(qh);
+}
+
+/**
+ * dwc2_periodic_channel_available() - Checks that a channel is available for a
+ * periodic transfer
+ *
+ * @hsotg: The HCD state structure for the DWC OTG controller
+ *
+ * Return: 0 if successful, negative error code otherise
+ */
+static int dwc2_periodic_channel_available(struct dwc2_hsotg *hsotg)
+{
+	/*
+	 * Currently assuming that there is a dedicated host channnel for
+	 * each periodic transaction plus at least one host channel for
+	 * non-periodic transactions
+	 */
+	int status;
+	int num_channels;
+
+	num_channels = hsotg->core_params->host_channels;
+	if (hsotg->periodic_channels + hsotg->non_periodic_channels <
+								num_channels
+	    && hsotg->periodic_channels < num_channels - 1) {
+		status = 0;
+	} else {
+		dev_dbg(hsotg->dev,
+			"%s: Total channels: %d, Periodic: %d, "
+			"Non-periodic: %d\n", __func__, num_channels,
+			hsotg->periodic_channels, hsotg->non_periodic_channels);
+		status = -ENOSPC;
+	}
+
+	return status;
+}
+
+/**
+ * dwc2_check_periodic_bandwidth() - Checks that there is sufficient bandwidth
+ * for the specified QH in the periodic schedule
+ *
+ * @hsotg: The HCD state structure for the DWC OTG controller
+ * @qh:    QH containing periodic bandwidth required
+ *
+ * Return: 0 if successful, negative error code otherwise
+ *
+ * For simplicity, this calculation assumes that all the transfers in the
+ * periodic schedule may occur in the same (micro)frame
+ */
+static int dwc2_check_periodic_bandwidth(struct dwc2_hsotg *hsotg,
+					 struct dwc2_qh *qh)
+{
+	int status;
+	s16 max_claimed_usecs;
+
+	status = 0;
+
+	if (qh->dev_speed == USB_SPEED_HIGH || qh->do_split) {
+		/*
+		 * High speed mode
+		 * Max periodic usecs is 80% x 125 usec = 100 usec
+		 */
+		max_claimed_usecs = 100 - qh->usecs;
+	} else {
+		/*
+		 * Full speed mode
+		 * Max periodic usecs is 90% x 1000 usec = 900 usec
+		 */
+		max_claimed_usecs = 900 - qh->usecs;
+	}
+
+	if (hsotg->periodic_usecs > max_claimed_usecs) {
+		dev_err(hsotg->dev,
+			"%s: already claimed usecs %d, required usecs %d\n",
+			__func__, hsotg->periodic_usecs, qh->usecs);
+		status = -ENOSPC;
+	}
+
+	return status;
+}
+
+/**
+ * dwc2_check_max_xfer_size() - Checks that the max transfer size allowed in a
+ * host channel is large enough to handle the maximum data transfer in a single
+ * (micro)frame for a periodic transfer
+ *
+ * @hsotg: The HCD state structure for the DWC OTG controller
+ * @qh:    QH for a periodic endpoint
+ *
+ * Return: 0 if successful, negative error code otherwise
+ */
+static int dwc2_check_max_xfer_size(struct dwc2_hsotg *hsotg,
+				    struct dwc2_qh *qh)
+{
+	u32 max_xfer_size;
+	u32 max_channel_xfer_size;
+	int status = 0;
+
+	max_xfer_size = dwc2_max_packet(qh->maxp) * dwc2_hb_mult(qh->maxp);
+	max_channel_xfer_size = hsotg->core_params->max_transfer_size;
+
+	if (max_xfer_size > max_channel_xfer_size) {
+		dev_err(hsotg->dev,
+			"%s: Periodic xfer length %d > max xfer length for channel %d\n",
+			__func__, max_xfer_size, max_channel_xfer_size);
+		status = -ENOSPC;
+	}
+
+	return status;
+}
+
+/**
+ * dwc2_schedule_periodic() - Schedules an interrupt or isochronous transfer in
+ * the periodic schedule
+ *
+ * @hsotg: The HCD state structure for the DWC OTG controller
+ * @qh:    QH for the periodic transfer. The QH should already contain the
+ *         scheduling information.
+ *
+ * Return: 0 if successful, negative error code otherwise
+ */
+static int dwc2_schedule_periodic(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh)
+{
+	int status;
+
+	status = dwc2_periodic_channel_available(hsotg);
+	if (status) {
+		dev_dbg(hsotg->dev,
+			"%s: No host channel available for periodic transfer\n",
+			__func__);
+		return status;
+	}
+
+	status = dwc2_check_periodic_bandwidth(hsotg, qh);
+	if (status) {
+		dev_dbg(hsotg->dev,
+			"%s: Insufficient periodic bandwidth for periodic transfer\n",
+			__func__);
+		return status;
+	}
+
+	status = dwc2_check_max_xfer_size(hsotg, qh);
+	if (status) {
+		dev_dbg(hsotg->dev,
+			"%s: Channel max transfer size too small for periodic transfer\n",
+			__func__);
+		return status;
+	}
+
+	if (hsotg->core_params->dma_desc_enable > 0)
+		/* Don't rely on SOF and start in ready schedule */
+		list_add_tail(&qh->qh_list_entry, &hsotg->periodic_sched_ready);
+	else
+		/* Always start in inactive schedule */
+		list_add_tail(&qh->qh_list_entry,
+			      &hsotg->periodic_sched_inactive);
+
+	/* Reserve periodic channel */
+	hsotg->periodic_channels++;
+
+	/* Update claimed usecs per (micro)frame */
+	hsotg->periodic_usecs += qh->usecs;
+
+	return status;
+}
+
+/**
+ * dwc2_deschedule_periodic() - Removes an interrupt or isochronous transfer
+ * from the periodic schedule
+ *
+ * @hsotg: The HCD state structure for the DWC OTG controller
+ * @qh:	   QH for the periodic transfer
+ */
+static void dwc2_deschedule_periodic(struct dwc2_hsotg *hsotg,
+				     struct dwc2_qh *qh)
+{
+	list_del_init(&qh->qh_list_entry);
+
+	/* Release periodic channel reservation */
+	hsotg->periodic_channels--;
+
+	/* Update claimed usecs per (micro)frame */
+	hsotg->periodic_usecs -= qh->usecs;
+}
+
+/**
+ * dwc2_hcd_qh_add() - Adds a QH to either the non periodic or periodic
+ * schedule if it is not already in the schedule. If the QH is already in
+ * the schedule, no action is taken.
+ *
+ * @hsotg: The HCD state structure for the DWC OTG controller
+ * @qh:    The QH to add
+ *
+ * Return: 0 if successful, negative error code otherwise
+ */
+int dwc2_hcd_qh_add(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh)
+{
+	int status = 0;
+	u32 intr_mask;
+
+	if (dbg_qh(qh))
+		dev_vdbg(hsotg->dev, "%s()\n", __func__);
+
+	if (!list_empty(&qh->qh_list_entry))
+		/* QH already in a schedule */
+		return status;
+
+	/* Add the new QH to the appropriate schedule */
+	if (dwc2_qh_is_non_per(qh)) {
+		/* Always start in inactive schedule */
+		list_add_tail(&qh->qh_list_entry,
+			      &hsotg->non_periodic_sched_inactive);
+	} else {
+		status = dwc2_schedule_periodic(hsotg, qh);
+		if (status == 0) {
+			if (!hsotg->periodic_qh_count) {
+				intr_mask = readl(hsotg->regs + GINTMSK);
+				intr_mask |= GINTSTS_SOF;
+				writel(intr_mask, hsotg->regs + GINTMSK);
+			}
+			hsotg->periodic_qh_count++;
+		}
+	}
+
+	return status;
+}
+
+/**
+ * dwc2_hcd_qh_unlink() - Removes a QH from either the non-periodic or periodic
+ * schedule. Memory is not freed.
+ *
+ * @hsotg: The HCD state structure
+ * @qh:    QH to remove from schedule
+ */
+void dwc2_hcd_qh_unlink(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh)
+{
+	u32 intr_mask;
+
+	dev_vdbg(hsotg->dev, "%s()\n", __func__);
+
+	if (list_empty(&qh->qh_list_entry))
+		/* QH is not in a schedule */
+		return;
+
+	if (dwc2_qh_is_non_per(qh)) {
+		if (hsotg->non_periodic_qh_ptr == &qh->qh_list_entry)
+			hsotg->non_periodic_qh_ptr =
+					hsotg->non_periodic_qh_ptr->next;
+		list_del_init(&qh->qh_list_entry);
+	} else {
+		dwc2_deschedule_periodic(hsotg, qh);
+		hsotg->periodic_qh_count--;
+		if (!hsotg->periodic_qh_count) {
+			intr_mask = readl(hsotg->regs + GINTMSK);
+			intr_mask &= ~GINTSTS_SOF;
+			writel(intr_mask, hsotg->regs + GINTMSK);
+		}
+	}
+}
+
+/*
+ * Schedule the next continuing periodic split transfer
+ */
+static void dwc2_sched_periodic_split(struct dwc2_hsotg *hsotg,
+				      struct dwc2_qh *qh, u16 frame_number,
+				      int sched_next_periodic_split)
+{
+	u16 incr;
+
+	if (sched_next_periodic_split) {
+		qh->sched_frame = frame_number;
+		incr = dwc2_frame_num_inc(qh->start_split_frame, 1);
+		if (dwc2_frame_num_le(frame_number, incr)) {
+			/*
+			 * Allow one frame to elapse after start split
+			 * microframe before scheduling complete split, but
+			 * DON'T if we are doing the next start split in the
+			 * same frame for an ISOC out
+			 */
+			if (qh->ep_type != USB_ENDPOINT_XFER_ISOC ||
+			    qh->ep_is_in != 0) {
+				qh->sched_frame =
+					dwc2_frame_num_inc(qh->sched_frame, 1);
+			}
+		}
+	} else {
+		qh->sched_frame = dwc2_frame_num_inc(qh->start_split_frame,
+						     qh->interval);
+		if (dwc2_frame_num_le(qh->sched_frame, frame_number))
+			qh->sched_frame = frame_number;
+		qh->sched_frame |= 0x7;
+		qh->start_split_frame = qh->sched_frame;
+	}
+}
+
+/*
+ * Deactivates a QH. For non-periodic QHs, removes the QH from the active
+ * non-periodic schedule. The QH is added to the inactive non-periodic
+ * schedule if any QTDs are still attached to the QH.
+ *
+ * For periodic QHs, the QH is removed from the periodic queued schedule. If
+ * there are any QTDs still attached to the QH, the QH is added to either the
+ * periodic inactive schedule or the periodic ready schedule and its next
+ * scheduled frame is calculated. The QH is placed in the ready schedule if
+ * the scheduled frame has been reached already. Otherwise it's placed in the
+ * inactive schedule. If there are no QTDs attached to the QH, the QH is
+ * completely removed from the periodic schedule.
+ */
+void dwc2_hcd_qh_deactivate(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh,
+			    int sched_next_periodic_split)
+{
+	if (dbg_qh(qh))
+		dev_vdbg(hsotg->dev, "%s()\n", __func__);
+
+	if (dwc2_qh_is_non_per(qh)) {
+		dwc2_hcd_qh_unlink(hsotg, qh);
+		if (!list_empty(&qh->qtd_list))
+			/* Add back to inactive non-periodic schedule */
+			dwc2_hcd_qh_add(hsotg, qh);
+	} else {
+		u16 frame_number = dwc2_hcd_get_frame_number(hsotg);
+
+		if (qh->do_split) {
+			dwc2_sched_periodic_split(hsotg, qh, frame_number,
+						  sched_next_periodic_split);
+		} else {
+			qh->sched_frame = dwc2_frame_num_inc(qh->sched_frame,
+							     qh->interval);
+			if (dwc2_frame_num_le(qh->sched_frame, frame_number))
+				qh->sched_frame = frame_number;
+		}
+
+		if (list_empty(&qh->qtd_list)) {
+			dwc2_hcd_qh_unlink(hsotg, qh);
+		} else {
+			/*
+			 * Remove from periodic_sched_queued and move to
+			 * appropriate queue
+			 */
+			if (qh->sched_frame == frame_number)
+				list_move(&qh->qh_list_entry,
+					  &hsotg->periodic_sched_ready);
+			else
+				list_move(&qh->qh_list_entry,
+					  &hsotg->periodic_sched_inactive);
+		}
+	}
+}
+
+/**
+ * dwc2_hcd_qtd_init() - Initializes a QTD structure
+ *
+ * @qtd: The QTD to initialize
+ * @urb: The associated URB
+ */
+void dwc2_hcd_qtd_init(struct dwc2_qtd *qtd, struct dwc2_hcd_urb *urb)
+{
+	qtd->urb = urb;
+	if (dwc2_hcd_get_pipe_type(&urb->pipe_info) ==
+			USB_ENDPOINT_XFER_CONTROL) {
+		/*
+		 * The only time the QTD data toggle is used is on the data
+		 * phase of control transfers. This phase always starts with
+		 * DATA1.
+		 */
+		qtd->data_toggle = DWC2_HC_PID_DATA1;
+		qtd->control_phase = DWC2_CONTROL_SETUP;
+	}
+
+	/* Start split */
+	qtd->complete_split = 0;
+	qtd->isoc_split_pos = DWC2_HCSPLT_XACTPOS_ALL;
+	qtd->isoc_split_offset = 0;
+	qtd->in_process = 0;
+
+	/* Store the qtd ptr in the urb to reference the QTD */
+	urb->qtd = qtd;
+}
+
+/**
+ * dwc2_hcd_qtd_add() - Adds a QTD to the QTD-list of a QH
+ *
+ * @hsotg:        The DWC HCD structure
+ * @qtd:          The QTD to add
+ * @qh:           Out parameter to return queue head
+ * @atomic_alloc: Flag to do atomic alloc if needed
+ *
+ * Return: 0 if successful, negative error code otherwise
+ *
+ * Finds the correct QH to place the QTD into. If it does not find a QH, it
+ * will create a new QH. If the QH to which the QTD is added is not currently
+ * scheduled, it is placed into the proper schedule based on its EP type.
+ */
+int dwc2_hcd_qtd_add(struct dwc2_hsotg *hsotg, struct dwc2_qtd *qtd,
+		     struct dwc2_qh **qh, gfp_t mem_flags)
+{
+	struct dwc2_hcd_urb *urb = qtd->urb;
+	unsigned long flags;
+	int allocated = 0;
+	int retval = 0;
+
+	/*
+	 * Get the QH which holds the QTD-list to insert to. Create QH if it
+	 * doesn't exist.
+	 */
+	if (*qh == NULL) {
+		*qh = dwc2_hcd_qh_create(hsotg, urb, mem_flags);
+		if (*qh == NULL)
+			return -ENOMEM;
+		allocated = 1;
+	}
+
+	spin_lock_irqsave(&hsotg->lock, flags);
+	retval = dwc2_hcd_qh_add(hsotg, *qh);
+	if (retval && allocated) {
+		struct dwc2_qtd *qtd2, *qtd2_tmp;
+		struct dwc2_qh *qh_tmp = *qh;
+
+		*qh = NULL;
+		dwc2_hcd_qh_unlink(hsotg, qh_tmp);
+
+		/* Free each QTD in the QH's QTD list */
+		list_for_each_entry_safe(qtd2, qtd2_tmp, &qh_tmp->qtd_list,
+					 qtd_list_entry)
+			dwc2_hcd_qtd_unlink_and_free(hsotg, qtd2, qh_tmp);
+
+		spin_unlock_irqrestore(&hsotg->lock, flags);
+		dwc2_hcd_qh_free(hsotg, qh_tmp);
+	} else {
+		qtd->qh = *qh;
+		list_add_tail(&qtd->qtd_list_entry, &(*qh)->qtd_list);
+		spin_unlock_irqrestore(&hsotg->lock, flags);
+	}
+
+	return retval;
+}
diff --git a/drivers/staging/dwc2/hw.h b/drivers/staging/dwc2/hw.h
new file mode 100644
index 00000000000..382a1d74865
--- /dev/null
+++ b/drivers/staging/dwc2/hw.h
@@ -0,0 +1,811 @@
+/*
+ * hw.h - DesignWare HS OTG Controller hardware definitions
+ *
+ * Copyright 2004-2013 Synopsys, Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions, and the following disclaimer,
+ *    without modification.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The names of the above-listed copyright holders may not be used
+ *    to endorse or promote products derived from this software without
+ *    specific prior written permission.
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation; either version 2 of the License, or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+ * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __DWC2_HW_H__
+#define __DWC2_HW_H__
+
+#define HSOTG_REG(x)	(x)
+
+#define GOTGCTL				HSOTG_REG(0x000)
+#define GOTGCTL_CHIRPEN			(1 << 27)
+#define GOTGCTL_MULT_VALID_BC_MASK	(0x1f << 22)
+#define GOTGCTL_MULT_VALID_BC_SHIFT	22
+#define GOTGCTL_OTGVER			(1 << 20)
+#define GOTGCTL_BSESVLD			(1 << 19)
+#define GOTGCTL_ASESVLD			(1 << 18)
+#define GOTGCTL_DBNC_SHORT		(1 << 17)
+#define GOTGCTL_CONID_B			(1 << 16)
+#define GOTGCTL_DEVHNPEN		(1 << 11)
+#define GOTGCTL_HSTSETHNPEN		(1 << 10)
+#define GOTGCTL_HNPREQ			(1 << 9)
+#define GOTGCTL_HSTNEGSCS		(1 << 8)
+#define GOTGCTL_SESREQ			(1 << 1)
+#define GOTGCTL_SESREQSCS		(1 << 0)
+
+#define GOTGINT				HSOTG_REG(0x004)
+#define GOTGINT_DBNCE_DONE		(1 << 19)
+#define GOTGINT_A_DEV_TOUT_CHG		(1 << 18)
+#define GOTGINT_HST_NEG_DET		(1 << 17)
+#define GOTGINT_HST_NEG_SUC_STS_CHNG	(1 << 9)
+#define GOTGINT_SES_REQ_SUC_STS_CHNG	(1 << 8)
+#define GOTGINT_SES_END_DET		(1 << 2)
+
+#define GAHBCFG				HSOTG_REG(0x008)
+#define GAHBCFG_AHB_SINGLE		(1 << 23)
+#define GAHBCFG_NOTI_ALL_DMA_WRIT	(1 << 22)
+#define GAHBCFG_REM_MEM_SUPP		(1 << 21)
+#define GAHBCFG_P_TXF_EMP_LVL		(1 << 8)
+#define GAHBCFG_NP_TXF_EMP_LVL		(1 << 7)
+#define GAHBCFG_DMA_EN			(1 << 5)
+#define GAHBCFG_HBSTLEN_MASK		(0xf << 1)
+#define GAHBCFG_HBSTLEN_SHIFT		1
+#define GAHBCFG_HBSTLEN_SINGLE			(0 << 1)
+#define GAHBCFG_HBSTLEN_INCR			(1 << 1)
+#define GAHBCFG_HBSTLEN_INCR4			(3 << 1)
+#define GAHBCFG_HBSTLEN_INCR8			(5 << 1)
+#define GAHBCFG_HBSTLEN_INCR16			(7 << 1)
+#define GAHBCFG_GLBL_INTR_EN		(1 << 0)
+
+#define GUSBCFG				HSOTG_REG(0x00C)
+#define GUSBCFG_FORCEDEVMODE		(1 << 30)
+#define GUSBCFG_FORCEHOSTMODE		(1 << 29)
+#define GUSBCFG_TXENDDELAY		(1 << 28)
+#define GUSBCFG_ICTRAFFICPULLREMOVE	(1 << 27)
+#define GUSBCFG_ICUSBCAP		(1 << 26)
+#define GUSBCFG_ULPI_INT_PROT_DIS	(1 << 25)
+#define GUSBCFG_INDICATORPASSTHROUGH	(1 << 24)
+#define GUSBCFG_INDICATORCOMPLEMENT	(1 << 23)
+#define GUSBCFG_TERMSELDLPULSE		(1 << 22)
+#define GUSBCFG_ULPI_INT_VBUS_IND	(1 << 21)
+#define GUSBCFG_ULPI_EXT_VBUS_DRV	(1 << 20)
+#define GUSBCFG_ULPI_CLK_SUSP_M		(1 << 19)
+#define GUSBCFG_ULPI_AUTO_RES		(1 << 18)
+#define GUSBCFG_ULPI_FS_LS		(1 << 17)
+#define GUSBCFG_OTG_UTMI_FS_SEL		(1 << 16)
+#define GUSBCFG_PHY_LP_CLK_SEL		(1 << 15)
+#define GUSBCFG_USBTRDTIM_MASK		(0xf << 10)
+#define GUSBCFG_USBTRDTIM_SHIFT		10
+#define GUSBCFG_HNPCAP			(1 << 9)
+#define GUSBCFG_SRPCAP			(1 << 8)
+#define GUSBCFG_DDRSEL			(1 << 7)
+#define GUSBCFG_PHYSEL			(1 << 6)
+#define GUSBCFG_FSINTF			(1 << 5)
+#define GUSBCFG_ULPI_UTMI_SEL		(1 << 4)
+#define GUSBCFG_PHYIF16			(1 << 3)
+#define GUSBCFG_TOUTCAL_MASK		(0x7 << 0)
+#define GUSBCFG_TOUTCAL_SHIFT		0
+#define GUSBCFG_TOUTCAL_LIMIT		0x7
+#define GUSBCFG_TOUTCAL(_x)		((_x) << 0)
+
+#define GRSTCTL				HSOTG_REG(0x010)
+#define GRSTCTL_AHBIDLE			(1 << 31)
+#define GRSTCTL_DMAREQ			(1 << 30)
+#define GRSTCTL_TXFNUM_MASK		(0x1f << 6)
+#define GRSTCTL_TXFNUM_SHIFT		6
+#define GRSTCTL_TXFNUM_LIMIT		0x1f
+#define GRSTCTL_TXFNUM(_x)		((_x) << 6)
+#define GRSTCTL_TXFFLSH			(1 << 5)
+#define GRSTCTL_RXFFLSH			(1 << 4)
+#define GRSTCTL_IN_TKNQ_FLSH		(1 << 3)
+#define GRSTCTL_FRMCNTRRST		(1 << 2)
+#define GRSTCTL_HSFTRST			(1 << 1)
+#define GRSTCTL_CSFTRST			(1 << 0)
+
+#define GINTSTS				HSOTG_REG(0x014)
+#define GINTMSK				HSOTG_REG(0x018)
+#define GINTSTS_WKUPINT			(1 << 31)
+#define GINTSTS_SESSREQINT		(1 << 30)
+#define GINTSTS_DISCONNINT		(1 << 29)
+#define GINTSTS_CONIDSTSCHNG		(1 << 28)
+#define GINTSTS_LPMTRANRCVD		(1 << 27)
+#define GINTSTS_PTXFEMP			(1 << 26)
+#define GINTSTS_HCHINT			(1 << 25)
+#define GINTSTS_PRTINT			(1 << 24)
+#define GINTSTS_RESETDET		(1 << 23)
+#define GINTSTS_FET_SUSP		(1 << 22)
+#define GINTSTS_INCOMPL_IP		(1 << 21)
+#define GINTSTS_INCOMPL_SOIN		(1 << 20)
+#define GINTSTS_OEPINT			(1 << 19)
+#define GINTSTS_IEPINT			(1 << 18)
+#define GINTSTS_EPMIS			(1 << 17)
+#define GINTSTS_RESTOREDONE		(1 << 16)
+#define GINTSTS_EOPF			(1 << 15)
+#define GINTSTS_ISOUTDROP		(1 << 14)
+#define GINTSTS_ENUMDONE		(1 << 13)
+#define GINTSTS_USBRST			(1 << 12)
+#define GINTSTS_USBSUSP			(1 << 11)
+#define GINTSTS_ERLYSUSP		(1 << 10)
+#define GINTSTS_I2CINT			(1 << 9)
+#define GINTSTS_ULPI_CK_INT		(1 << 8)
+#define GINTSTS_GOUTNAKEFF		(1 << 7)
+#define GINTSTS_GINNAKEFF		(1 << 6)
+#define GINTSTS_NPTXFEMP		(1 << 5)
+#define GINTSTS_RXFLVL			(1 << 4)
+#define GINTSTS_SOF			(1 << 3)
+#define GINTSTS_OTGINT			(1 << 2)
+#define GINTSTS_MODEMIS			(1 << 1)
+#define GINTSTS_CURMODE_HOST		(1 << 0)
+
+#define GRXSTSR				HSOTG_REG(0x01C)
+#define GRXSTSP				HSOTG_REG(0x020)
+#define GRXSTS_FN_MASK			(0x7f << 25)
+#define GRXSTS_FN_SHIFT			25
+#define GRXSTS_PKTSTS_MASK		(0xf << 17)
+#define GRXSTS_PKTSTS_SHIFT		17
+#define GRXSTS_PKTSTS_GLOBALOUTNAK		(1 << 17)
+#define GRXSTS_PKTSTS_OUTRX			(2 << 17)
+#define GRXSTS_PKTSTS_HCHIN			(2 << 17)
+#define GRXSTS_PKTSTS_OUTDONE			(3 << 17)
+#define GRXSTS_PKTSTS_HCHIN_XFER_COMP		(3 << 17)
+#define GRXSTS_PKTSTS_SETUPDONE			(4 << 17)
+#define GRXSTS_PKTSTS_DATATOGGLEERR		(5 << 17)
+#define GRXSTS_PKTSTS_SETUPRX			(6 << 17)
+#define GRXSTS_PKTSTS_HCHHALTED			(7 << 17)
+#define GRXSTS_HCHNUM_MASK		(0xf << 0)
+#define GRXSTS_HCHNUM_SHIFT		0
+#define GRXSTS_DPID_MASK		(0x3 << 15)
+#define GRXSTS_DPID_SHIFT		15
+#define GRXSTS_BYTECNT_MASK		(0x7ff << 4)
+#define GRXSTS_BYTECNT_SHIFT		4
+#define GRXSTS_EPNUM_MASK		(0xf << 0)
+#define GRXSTS_EPNUM_SHIFT		0
+
+#define GRXFSIZ				HSOTG_REG(0x024)
+
+#define GNPTXFSIZ			HSOTG_REG(0x028)
+#define GNPTXFSIZ_NP_TXF_DEP_MASK	(0xffff << 16)
+#define GNPTXFSIZ_NP_TXF_DEP_SHIFT	16
+#define GNPTXFSIZ_NP_TXF_DEP_LIMIT	0xffff
+#define GNPTXFSIZ_NP_TXF_DEP(_x)	((_x) << 16)
+#define GNPTXFSIZ_NP_TXF_ST_ADDR_MASK	(0xffff << 0)
+#define GNPTXFSIZ_NP_TXF_ST_ADDR_SHIFT	0
+#define GNPTXFSIZ_NP_TXF_ST_ADDR_LIMIT	0xffff
+#define GNPTXFSIZ_NP_TXF_ST_ADDR(_x)	((_x) << 0)
+
+#define GNPTXSTS			HSOTG_REG(0x02C)
+#define GNPTXSTS_NP_TXQ_TOP_MASK		(0x7f << 24)
+#define GNPTXSTS_NP_TXQ_TOP_SHIFT		24
+#define GNPTXSTS_NP_TXQ_SPC_AVAIL_MASK		(0xff << 16)
+#define GNPTXSTS_NP_TXQ_SPC_AVAIL_SHIFT		16
+#define GNPTXSTS_NP_TXQ_SPC_AVAIL_GET(_v)	(((_v) >> 16) & 0xff)
+#define GNPTXSTS_NP_TXF_SPC_AVAIL_MASK		(0xffff << 0)
+#define GNPTXSTS_NP_TXF_SPC_AVAIL_SHIFT		0
+#define GNPTXSTS_NP_TXF_SPC_AVAIL_GET(_v)	(((_v) >> 0) & 0xffff)
+
+#define GI2CCTL				HSOTG_REG(0x0030)
+#define GI2CCTL_BSYDNE			(1 << 31)
+#define GI2CCTL_RW			(1 << 30)
+#define GI2CCTL_I2CDATSE0		(1 << 28)
+#define GI2CCTL_I2CDEVADDR_MASK		(0x3 << 26)
+#define GI2CCTL_I2CDEVADDR_SHIFT	26
+#define GI2CCTL_I2CSUSPCTL		(1 << 25)
+#define GI2CCTL_ACK			(1 << 24)
+#define GI2CCTL_I2CEN			(1 << 23)
+#define GI2CCTL_ADDR_MASK		(0x7f << 16)
+#define GI2CCTL_ADDR_SHIFT		16
+#define GI2CCTL_REGADDR_MASK		(0xff << 8)
+#define GI2CCTL_REGADDR_SHIFT		8
+#define GI2CCTL_RWDATA_MASK		(0xff << 0)
+#define GI2CCTL_RWDATA_SHIFT		0
+
+#define GPVNDCTL			HSOTG_REG(0x0034)
+#define GGPIO				HSOTG_REG(0x0038)
+#define GUID				HSOTG_REG(0x003c)
+#define GSNPSID				HSOTG_REG(0x0040)
+#define GHWCFG1				HSOTG_REG(0x0044)
+
+#define GHWCFG2				HSOTG_REG(0x0048)
+#define GHWCFG2_OTG_ENABLE_IC_USB		(1 << 31)
+#define GHWCFG2_DEV_TOKEN_Q_DEPTH_MASK		(0x1f << 26)
+#define GHWCFG2_DEV_TOKEN_Q_DEPTH_SHIFT		26
+#define GHWCFG2_HOST_PERIO_TX_Q_DEPTH_MASK	(0x3 << 24)
+#define GHWCFG2_HOST_PERIO_TX_Q_DEPTH_SHIFT	24
+#define GHWCFG2_NONPERIO_TX_Q_DEPTH_MASK	(0x3 << 22)
+#define GHWCFG2_NONPERIO_TX_Q_DEPTH_SHIFT	22
+#define GHWCFG2_MULTI_PROC_INT			(1 << 20)
+#define GHWCFG2_DYNAMIC_FIFO			(1 << 19)
+#define GHWCFG2_PERIO_EP_SUPPORTED		(1 << 18)
+#define GHWCFG2_NUM_HOST_CHAN_MASK		(0xf << 14)
+#define GHWCFG2_NUM_HOST_CHAN_SHIFT		14
+#define GHWCFG2_NUM_DEV_EP_MASK			(0xf << 10)
+#define GHWCFG2_NUM_DEV_EP_SHIFT		10
+#define GHWCFG2_FS_PHY_TYPE_MASK		(0x3 << 8)
+#define GHWCFG2_FS_PHY_TYPE_SHIFT		8
+#define GHWCFG2_FS_PHY_TYPE_NOT_SUPPORTED		(0 << 8)
+#define GHWCFG2_FS_PHY_TYPE_DEDICATED			(1 << 8)
+#define GHWCFG2_FS_PHY_TYPE_SHARED_UTMI			(2 << 8)
+#define GHWCFG2_FS_PHY_TYPE_SHARED_ULPI			(3 << 8)
+#define GHWCFG2_HS_PHY_TYPE_MASK		(0x3 << 6)
+#define GHWCFG2_HS_PHY_TYPE_SHIFT		6
+#define GHWCFG2_HS_PHY_TYPE_NOT_SUPPORTED		(0 << 6)
+#define GHWCFG2_HS_PHY_TYPE_UTMI			(1 << 6)
+#define GHWCFG2_HS_PHY_TYPE_ULPI			(2 << 6)
+#define GHWCFG2_HS_PHY_TYPE_UTMI_ULPI			(3 << 6)
+#define GHWCFG2_POINT2POINT			(1 << 5)
+#define GHWCFG2_ARCHITECTURE_MASK		(0x3 << 3)
+#define GHWCFG2_ARCHITECTURE_SHIFT		3
+#define GHWCFG2_SLAVE_ONLY_ARCH				(0 << 3)
+#define GHWCFG2_EXT_DMA_ARCH				(1 << 3)
+#define GHWCFG2_INT_DMA_ARCH				(2 << 3)
+#define GHWCFG2_OP_MODE_MASK			(0x7 << 0)
+#define GHWCFG2_OP_MODE_SHIFT			0
+#define GHWCFG2_OP_MODE_HNP_SRP_CAPABLE			(0 << 0)
+#define GHWCFG2_OP_MODE_SRP_ONLY_CAPABLE		(1 << 0)
+#define GHWCFG2_OP_MODE_NO_HNP_SRP_CAPABLE		(2 << 0)
+#define GHWCFG2_OP_MODE_SRP_CAPABLE_DEVICE		(3 << 0)
+#define GHWCFG2_OP_MODE_NO_SRP_CAPABLE_DEVICE		(4 << 0)
+#define GHWCFG2_OP_MODE_SRP_CAPABLE_HOST		(5 << 0)
+#define GHWCFG2_OP_MODE_NO_SRP_CAPABLE_HOST		(6 << 0)
+#define GHWCFG2_OP_MODE_UNDEFINED			(7 << 0)
+
+#define GHWCFG3				HSOTG_REG(0x004c)
+#define GHWCFG3_DFIFO_DEPTH_MASK		(0xffff << 16)
+#define GHWCFG3_DFIFO_DEPTH_SHIFT		16
+#define GHWCFG3_OTG_LPM_EN			(1 << 15)
+#define GHWCFG3_BC_SUPPORT			(1 << 14)
+#define GHWCFG3_OTG_ENABLE_HSIC			(1 << 13)
+#define GHWCFG3_ADP_SUPP			(1 << 12)
+#define GHWCFG3_SYNCH_RESET_TYPE		(1 << 11)
+#define GHWCFG3_OPTIONAL_FEATURES		(1 << 10)
+#define GHWCFG3_VENDOR_CTRL_IF			(1 << 9)
+#define GHWCFG3_I2C				(1 << 8)
+#define GHWCFG3_OTG_FUNC			(1 << 7)
+#define GHWCFG3_PACKET_SIZE_CNTR_WIDTH_MASK	(0x7 << 4)
+#define GHWCFG3_PACKET_SIZE_CNTR_WIDTH_SHIFT	4
+#define GHWCFG3_XFER_SIZE_CNTR_WIDTH_MASK	(0xf << 0)
+#define GHWCFG3_XFER_SIZE_CNTR_WIDTH_SHIFT	0
+
+#define GHWCFG4				HSOTG_REG(0x0050)
+#define GHWCFG4_DESC_DMA_DYN			(1 << 31)
+#define GHWCFG4_DESC_DMA			(1 << 30)
+#define GHWCFG4_NUM_IN_EPS_MASK			(0xf << 26)
+#define GHWCFG4_NUM_IN_EPS_SHIFT		26
+#define GHWCFG4_DED_FIFO_EN			(1 << 25)
+#define GHWCFG4_SESSION_END_FILT_EN		(1 << 24)
+#define GHWCFG4_B_VALID_FILT_EN			(1 << 23)
+#define GHWCFG4_A_VALID_FILT_EN			(1 << 22)
+#define GHWCFG4_VBUS_VALID_FILT_EN		(1 << 21)
+#define GHWCFG4_IDDIG_FILT_EN			(1 << 20)
+#define GHWCFG4_NUM_DEV_MODE_CTRL_EP_MASK	(0xf << 16)
+#define GHWCFG4_NUM_DEV_MODE_CTRL_EP_SHIFT	16
+#define GHWCFG4_UTMI_PHY_DATA_WIDTH_MASK	(0x3 << 14)
+#define GHWCFG4_UTMI_PHY_DATA_WIDTH_SHIFT	14
+#define GHWCFG4_XHIBER				(1 << 7)
+#define GHWCFG4_HIBER				(1 << 6)
+#define GHWCFG4_MIN_AHB_FREQ			(1 << 5)
+#define GHWCFG4_POWER_OPTIMIZ			(1 << 4)
+#define GHWCFG4_NUM_DEV_PERIO_IN_EP_MASK	(0xf << 0)
+#define GHWCFG4_NUM_DEV_PERIO_IN_EP_SHIFT	0
+
+#define GLPMCFG				HSOTG_REG(0x0054)
+#define GLPMCFG_INV_SEL_HSIC		(1 << 31)
+#define GLPMCFG_HSIC_CONNECT		(1 << 30)
+#define GLPMCFG_RETRY_COUNT_STS_MASK	(0x7 << 25)
+#define GLPMCFG_RETRY_COUNT_STS_SHIFT	25
+#define GLPMCFG_SEND_LPM		(1 << 24)
+#define GLPMCFG_RETRY_COUNT_MASK	(0x7 << 21)
+#define GLPMCFG_RETRY_COUNT_SHIFT	21
+#define GLPMCFG_LPM_CHAN_INDEX_MASK	(0xf << 17)
+#define GLPMCFG_LPM_CHAN_INDEX_SHIFT	17
+#define GLPMCFG_SLEEP_STATE_RESUMEOK	(1 << 16)
+#define GLPMCFG_PRT_SLEEP_STS		(1 << 15)
+#define GLPMCFG_LPM_RESP_MASK		(0x3 << 13)
+#define GLPMCFG_LPM_RESP_SHIFT		13
+#define GLPMCFG_HIRD_THRES_MASK		(0x1f << 8)
+#define GLPMCFG_HIRD_THRES_SHIFT	8
+#define GLPMCFG_HIRD_THRES_EN			(0x10 << 8)
+#define GLPMCFG_EN_UTMI_SLEEP		(1 << 7)
+#define GLPMCFG_REM_WKUP_EN		(1 << 6)
+#define GLPMCFG_HIRD_MASK		(0xf << 2)
+#define GLPMCFG_HIRD_SHIFT		2
+#define GLPMCFG_APPL_RESP		(1 << 1)
+#define GLPMCFG_LPM_CAP_EN		(1 << 0)
+
+#define GPWRDN				HSOTG_REG(0x0058)
+#define GPWRDN_MULT_VAL_ID_BC_MASK	(0x1f << 24)
+#define GPWRDN_MULT_VAL_ID_BC_SHIFT	24
+#define GPWRDN_ADP_INT			(1 << 23)
+#define GPWRDN_BSESSVLD			(1 << 22)
+#define GPWRDN_IDSTS			(1 << 21)
+#define GPWRDN_LINESTATE_MASK		(0x3 << 19)
+#define GPWRDN_LINESTATE_SHIFT		19
+#define GPWRDN_STS_CHGINT_MSK		(1 << 18)
+#define GPWRDN_STS_CHGINT		(1 << 17)
+#define GPWRDN_SRP_DET_MSK		(1 << 16)
+#define GPWRDN_SRP_DET			(1 << 15)
+#define GPWRDN_CONNECT_DET_MSK		(1 << 14)
+#define GPWRDN_CONNECT_DET		(1 << 13)
+#define GPWRDN_DISCONN_DET_MSK		(1 << 12)
+#define GPWRDN_DISCONN_DET		(1 << 11)
+#define GPWRDN_RST_DET_MSK		(1 << 10)
+#define GPWRDN_RST_DET			(1 << 9)
+#define GPWRDN_LNSTSCHG_MSK		(1 << 8)
+#define GPWRDN_LNSTSCHG			(1 << 7)
+#define GPWRDN_DIS_VBUS			(1 << 6)
+#define GPWRDN_PWRDNSWTCH		(1 << 5)
+#define GPWRDN_PWRDNRSTN		(1 << 4)
+#define GPWRDN_PWRDNCLMP		(1 << 3)
+#define GPWRDN_RESTORE			(1 << 2)
+#define GPWRDN_PMUACTV			(1 << 1)
+#define GPWRDN_PMUINTSEL		(1 << 0)
+
+#define GDFIFOCFG			HSOTG_REG(0x005c)
+#define GDFIFOCFG_EPINFOBASE_MASK	(0xffff << 16)
+#define GDFIFOCFG_EPINFOBASE_SHIFT	16
+#define GDFIFOCFG_GDFIFOCFG_MASK	(0xffff << 0)
+#define GDFIFOCFG_GDFIFOCFG_SHIFT	0
+
+#define ADPCTL				HSOTG_REG(0x0060)
+#define ADPCTL_AR_MASK			(0x3 << 27)
+#define ADPCTL_AR_SHIFT			27
+#define ADPCTL_ADP_TMOUT_INT_MSK	(1 << 26)
+#define ADPCTL_ADP_SNS_INT_MSK		(1 << 25)
+#define ADPCTL_ADP_PRB_INT_MSK		(1 << 24)
+#define ADPCTL_ADP_TMOUT_INT		(1 << 23)
+#define ADPCTL_ADP_SNS_INT		(1 << 22)
+#define ADPCTL_ADP_PRB_INT		(1 << 21)
+#define ADPCTL_ADPENA			(1 << 20)
+#define ADPCTL_ADPRES			(1 << 19)
+#define ADPCTL_ENASNS			(1 << 18)
+#define ADPCTL_ENAPRB			(1 << 17)
+#define ADPCTL_RTIM_MASK		(0x7ff << 6)
+#define ADPCTL_RTIM_SHIFT		6
+#define ADPCTL_PRB_PER_MASK		(0x3 << 4)
+#define ADPCTL_PRB_PER_SHIFT		4
+#define ADPCTL_PRB_DELTA_MASK		(0x3 << 2)
+#define ADPCTL_PRB_DELTA_SHIFT		2
+#define ADPCTL_PRB_DSCHRG_MASK		(0x3 << 0)
+#define ADPCTL_PRB_DSCHRG_SHIFT		0
+
+#define HPTXFSIZ			HSOTG_REG(0x100)
+
+#define DPTXFSIZN(_a)			HSOTG_REG(0x104 + (((_a) - 1) * 4))
+#define DPTXFSIZN_DP_TXF_SIZE_MASK	(0xffff << 16)
+#define DPTXFSIZN_DP_TXF_SIZE_SHIFT	16
+#define DPTXFSIZN_DP_TXF_SIZE_GET(_v)	(((_v) >> 16) & 0xffff)
+#define DPTXFSIZN_DP_TXF_SIZE_LIMIT	0xffff
+#define DPTXFSIZN_DP_TXF_SIZE(_x)	((_x) << 16)
+#define DPTXFSIZN_DP_TXF_ST_ADDR_MASK	(0xffff << 0)
+#define DPTXFSIZN_DP_TXF_ST_ADDR_SHIFT	0
+
+#define FIFOSIZE_DEPTH_MASK		(0xffff << 16)
+#define FIFOSIZE_DEPTH_SHIFT		16
+#define FIFOSIZE_STARTADDR_MASK		(0xffff << 0)
+#define FIFOSIZE_STARTADDR_SHIFT	0
+
+/* Device mode registers */
+
+#define DCFG				HSOTG_REG(0x800)
+#define DCFG_EPMISCNT_MASK		(0x1f << 18)
+#define DCFG_EPMISCNT_SHIFT		18
+#define DCFG_EPMISCNT_LIMIT		0x1f
+#define DCFG_EPMISCNT(_x)		((_x) << 18)
+#define DCFG_PERFRINT_MASK		(0x3 << 11)
+#define DCFG_PERFRINT_SHIFT		11
+#define DCFG_PERFRINT_LIMIT		0x3
+#define DCFG_PERFRINT(_x)		((_x) << 11)
+#define DCFG_DEVADDR_MASK		(0x7f << 4)
+#define DCFG_DEVADDR_SHIFT		4
+#define DCFG_DEVADDR_LIMIT		0x7f
+#define DCFG_DEVADDR(_x)		((_x) << 4)
+#define DCFG_NZ_STS_OUT_HSHK		(1 << 2)
+#define DCFG_DEVSPD_MASK		(0x3 << 0)
+#define DCFG_DEVSPD_SHIFT		0
+#define DCFG_DEVSPD_HS				(0 << 0)
+#define DCFG_DEVSPD_FS				(1 << 0)
+#define DCFG_DEVSPD_LS				(2 << 0)
+#define DCFG_DEVSPD_FS48			(3 << 0)
+
+#define DCTL				HSOTG_REG(0x804)
+#define DCTL_PWRONPRGDONE		(1 << 11)
+#define DCTL_CGOUTNAK			(1 << 10)
+#define DCTL_SGOUTNAK			(1 << 9)
+#define DCTL_CGNPINNAK			(1 << 8)
+#define DCTL_SGNPINNAK			(1 << 7)
+#define DCTL_TSTCTL_MASK		(0x7 << 4)
+#define DCTL_TSTCTL_SHIFT		4
+#define DCTL_GOUTNAKSTS			(1 << 3)
+#define DCTL_GNPINNAKSTS		(1 << 2)
+#define DCTL_SFTDISCON			(1 << 1)
+#define DCTL_RMTWKUPSIG			(1 << 0)
+
+#define DSTS				HSOTG_REG(0x808)
+#define DSTS_SOFFN_MASK			(0x3fff << 8)
+#define DSTS_SOFFN_SHIFT		8
+#define DSTS_SOFFN_LIMIT		0x3fff
+#define DSTS_SOFFN(_x)			((_x) << 8)
+#define DSTS_ERRATICERR			(1 << 3)
+#define DSTS_ENUMSPD_MASK		(0x3 << 1)
+#define DSTS_ENUMSPD_SHIFT		1
+#define DSTS_ENUMSPD_HS				(0 << 1)
+#define DSTS_ENUMSPD_FS				(1 << 1)
+#define DSTS_ENUMSPD_LS				(2 << 1)
+#define DSTS_ENUMSPD_FS48			(3 << 1)
+#define DSTS_SUSPSTS			(1 << 0)
+
+#define DIEPMSK				HSOTG_REG(0x810)
+#define DIEPMSK_TXFIFOEMPTY		(1 << 7)
+#define DIEPMSK_INEPNAKEFFMSK		(1 << 6)
+#define DIEPMSK_INTKNEPMISMSK		(1 << 5)
+#define DIEPMSK_INTKNTXFEMPMSK		(1 << 4)
+#define DIEPMSK_TIMEOUTMSK		(1 << 3)
+#define DIEPMSK_AHBERRMSK		(1 << 2)
+#define DIEPMSK_EPDISBLDMSK		(1 << 1)
+#define DIEPMSK_XFERCOMPLMSK		(1 << 0)
+
+#define DOEPMSK				HSOTG_REG(0x814)
+#define DOEPMSK_BACK2BACKSETUP		(1 << 6)
+#define DOEPMSK_OUTTKNEPDISMSK		(1 << 4)
+#define DOEPMSK_SETUPMSK		(1 << 3)
+#define DOEPMSK_AHBERRMSK		(1 << 2)
+#define DOEPMSK_EPDISBLDMSK		(1 << 1)
+#define DOEPMSK_XFERCOMPLMSK		(1 << 0)
+
+#define DAINT				HSOTG_REG(0x818)
+#define DAINTMSK			HSOTG_REG(0x81C)
+#define DAINT_OUTEP_SHIFT		16
+#define DAINT_OUTEP(_x)			(1 << ((_x) + 16))
+#define DAINT_INEP(_x)			(1 << (_x))
+
+#define DTKNQR1				HSOTG_REG(0x820)
+#define DTKNQR2				HSOTG_REG(0x824)
+#define DTKNQR3				HSOTG_REG(0x830)
+#define DTKNQR4				HSOTG_REG(0x834)
+
+#define DVBUSDIS			HSOTG_REG(0x828)
+#define DVBUSPULSE			HSOTG_REG(0x82C)
+
+#define DIEPCTL0			HSOTG_REG(0x900)
+#define DIEPCTL(_a)			HSOTG_REG(0x900 + ((_a) * 0x20))
+
+#define DOEPCTL0			HSOTG_REG(0xB00)
+#define DOEPCTL(_a)			HSOTG_REG(0xB00 + ((_a) * 0x20))
+
+/* EP0 specialness:
+ * bits[29..28] - reserved (no SetD0PID, SetD1PID)
+ * bits[25..22] - should always be zero, this isn't a periodic endpoint
+ * bits[10..0]  - MPS setting different for EP0
+ */
+#define D0EPCTL_MPS_MASK		(0x3 << 0)
+#define D0EPCTL_MPS_SHIFT		0
+#define D0EPCTL_MPS_64				(0 << 0)
+#define D0EPCTL_MPS_32				(1 << 0)
+#define D0EPCTL_MPS_16				(2 << 0)
+#define D0EPCTL_MPS_8				(3 << 0)
+
+#define DXEPCTL_EPENA			(1 << 31)
+#define DXEPCTL_EPDIS			(1 << 30)
+#define DXEPCTL_SETD1PID		(1 << 29)
+#define DXEPCTL_SETODDFR		(1 << 29)
+#define DXEPCTL_SETD0PID		(1 << 28)
+#define DXEPCTL_SETEVENFR		(1 << 28)
+#define DXEPCTL_SNAK			(1 << 27)
+#define DXEPCTL_CNAK			(1 << 26)
+#define DXEPCTL_TXFNUM_MASK		(0xf << 22)
+#define DXEPCTL_TXFNUM_SHIFT		22
+#define DXEPCTL_TXFNUM_LIMIT		0xf
+#define DXEPCTL_TXFNUM(_x)		((_x) << 22)
+#define DXEPCTL_STALL			(1 << 21)
+#define DXEPCTL_SNP			(1 << 20)
+#define DXEPCTL_EPTYPE_MASK		(0x3 << 18)
+#define DXEPCTL_EPTYPE_SHIFT		18
+#define DXEPCTL_EPTYPE_CONTROL			(0 << 18)
+#define DXEPCTL_EPTYPE_ISO			(1 << 18)
+#define DXEPCTL_EPTYPE_BULK			(2 << 18)
+#define DXEPCTL_EPTYPE_INTTERUPT		(3 << 18)
+#define DXEPCTL_NAKSTS			(1 << 17)
+#define DXEPCTL_DPID			(1 << 16)
+#define DXEPCTL_EOFRNUM			(1 << 16)
+#define DXEPCTL_USBACTEP		(1 << 15)
+#define DXEPCTL_NEXTEP_MASK		(0xf << 11)
+#define DXEPCTL_NEXTEP_SHIFT		11
+#define DXEPCTL_NEXTEP_LIMIT		0xf
+#define DXEPCTL_NEXTEP(_x)		((_x) << 11)
+#define DXEPCTL_MPS_MASK		(0x7ff << 0)
+#define DXEPCTL_MPS_SHIFT		0
+#define DXEPCTL_MPS_LIMIT		0x7ff
+#define DXEPCTL_MPS(_x)			((_x) << 0)
+
+#define DIEPINT(_a)			HSOTG_REG(0x908 + ((_a) * 0x20))
+#define DOEPINT(_a)			HSOTG_REG(0xB08 + ((_a) * 0x20))
+#define DXEPINT_INEPNAKEFF		(1 << 6)
+#define DXEPINT_BACK2BACKSETUP		(1 << 6)
+#define DXEPINT_INTKNEPMIS		(1 << 5)
+#define DXEPINT_INTKNTXFEMP		(1 << 4)
+#define DXEPINT_OUTTKNEPDIS		(1 << 4)
+#define DXEPINT_TIMEOUT			(1 << 3)
+#define DXEPINT_SETUP			(1 << 3)
+#define DXEPINT_AHBERR			(1 << 2)
+#define DXEPINT_EPDISBLD		(1 << 1)
+#define DXEPINT_XFERCOMPL		(1 << 0)
+
+#define DIEPTSIZ0			HSOTG_REG(0x910)
+#define DIEPTSIZ0_PKTCNT_MASK		(0x3 << 19)
+#define DIEPTSIZ0_PKTCNT_SHIFT		19
+#define DIEPTSIZ0_PKTCNT_LIMIT		0x3
+#define DIEPTSIZ0_PKTCNT(_x)		((_x) << 19)
+#define DIEPTSIZ0_XFERSIZE_MASK		(0x7f << 0)
+#define DIEPTSIZ0_XFERSIZE_SHIFT	0
+#define DIEPTSIZ0_XFERSIZE_LIMIT	0x7f
+#define DIEPTSIZ0_XFERSIZE(_x)		((_x) << 0)
+
+#define DOEPTSIZ0			HSOTG_REG(0xB10)
+#define DOEPTSIZ0_SUPCNT_MASK		(0x3 << 29)
+#define DOEPTSIZ0_SUPCNT_SHIFT		29
+#define DOEPTSIZ0_SUPCNT_LIMIT		0x3
+#define DOEPTSIZ0_SUPCNT(_x)		((_x) << 29)
+#define DOEPTSIZ0_PKTCNT		(1 << 19)
+#define DOEPTSIZ0_XFERSIZE_MASK		(0x7f << 0)
+#define DOEPTSIZ0_XFERSIZE_SHIFT	0
+
+#define DIEPTSIZ(_a)			HSOTG_REG(0x910 + ((_a) * 0x20))
+#define DOEPTSIZ(_a)			HSOTG_REG(0xB10 + ((_a) * 0x20))
+#define DXEPTSIZ_MC_MASK		(0x3 << 29)
+#define DXEPTSIZ_MC_SHIFT		29
+#define DXEPTSIZ_MC_LIMIT		0x3
+#define DXEPTSIZ_MC(_x)			((_x) << 29)
+#define DXEPTSIZ_PKTCNT_MASK		(0x3ff << 19)
+#define DXEPTSIZ_PKTCNT_SHIFT		19
+#define DXEPTSIZ_PKTCNT_LIMIT		0x3ff
+#define DXEPTSIZ_PKTCNT_GET(_v)		(((_v) >> 19) & 0x3ff)
+#define DXEPTSIZ_PKTCNT(_x)		((_x) << 19)
+#define DXEPTSIZ_XFERSIZE_MASK		(0x7ffff << 0)
+#define DXEPTSIZ_XFERSIZE_SHIFT		0
+#define DXEPTSIZ_XFERSIZE_LIMIT		0x7ffff
+#define DXEPTSIZ_XFERSIZE_GET(_v)	(((_v) >> 0) & 0x7ffff)
+#define DXEPTSIZ_XFERSIZE(_x)		((_x) << 0)
+
+#define DIEPDMA(_a)			HSOTG_REG(0x914 + ((_a) * 0x20))
+#define DOEPDMA(_a)			HSOTG_REG(0xB14 + ((_a) * 0x20))
+
+#define DTXFSTS(_a)			HSOTG_REG(0x918 + ((_a) * 0x20))
+
+#define PCGCTL				HSOTG_REG(0x0e00)
+#define PCGCTL_IF_DEV_MODE		(1 << 31)
+#define PCGCTL_P2HD_PRT_SPD_MASK	(0x3 << 29)
+#define PCGCTL_P2HD_PRT_SPD_SHIFT	29
+#define PCGCTL_P2HD_DEV_ENUM_SPD_MASK	(0x3 << 27)
+#define PCGCTL_P2HD_DEV_ENUM_SPD_SHIFT	27
+#define PCGCTL_MAC_DEV_ADDR_MASK	(0x7f << 20)
+#define PCGCTL_MAC_DEV_ADDR_SHIFT	20
+#define PCGCTL_MAX_TERMSEL		(1 << 19)
+#define PCGCTL_MAX_XCVRSELECT_MASK	(0x3 << 17)
+#define PCGCTL_MAX_XCVRSELECT_SHIFT	17
+#define PCGCTL_PORT_POWER		(1 << 16)
+#define PCGCTL_PRT_CLK_SEL_MASK		(0x3 << 14)
+#define PCGCTL_PRT_CLK_SEL_SHIFT	14
+#define PCGCTL_ESS_REG_RESTORED		(1 << 13)
+#define PCGCTL_EXTND_HIBER_SWITCH	(1 << 12)
+#define PCGCTL_EXTND_HIBER_PWRCLMP	(1 << 11)
+#define PCGCTL_ENBL_EXTND_HIBER		(1 << 10)
+#define PCGCTL_RESTOREMODE		(1 << 9)
+#define PCGCTL_RESETAFTSUSP		(1 << 8)
+#define PCGCTL_DEEP_SLEEP		(1 << 7)
+#define PCGCTL_PHY_IN_SLEEP		(1 << 6)
+#define PCGCTL_ENBL_SLEEP_GATING	(1 << 5)
+#define PCGCTL_RSTPDWNMODULE		(1 << 3)
+#define PCGCTL_PWRCLMP			(1 << 2)
+#define PCGCTL_GATEHCLK			(1 << 1)
+#define PCGCTL_STOPPCLK			(1 << 0)
+
+#define EPFIFO(_a)			HSOTG_REG(0x1000 + ((_a) * 0x1000))
+
+/* Host Mode Registers */
+
+#define HCFG				HSOTG_REG(0x0400)
+#define HCFG_MODECHTIMEN		(1 << 31)
+#define HCFG_PERSCHEDENA		(1 << 26)
+#define HCFG_FRLISTEN_MASK		(0x3 << 24)
+#define HCFG_FRLISTEN_SHIFT		24
+#define HCFG_FRLISTEN_8				(0 << 24)
+#define FRLISTEN_8_SIZE				8
+#define HCFG_FRLISTEN_16			(1 << 24)
+#define FRLISTEN_16_SIZE			16
+#define HCFG_FRLISTEN_32			(2 << 24)
+#define FRLISTEN_32_SIZE			32
+#define HCFG_FRLISTEN_64			(3 << 24)
+#define FRLISTEN_64_SIZE			64
+#define HCFG_DESCDMA			(1 << 23)
+#define HCFG_RESVALID_MASK		(0xff << 8)
+#define HCFG_RESVALID_SHIFT		8
+#define HCFG_ENA32KHZ			(1 << 7)
+#define HCFG_FSLSSUPP			(1 << 2)
+#define HCFG_FSLSPCLKSEL_MASK		(0x3 << 0)
+#define HCFG_FSLSPCLKSEL_SHIFT		0
+#define HCFG_FSLSPCLKSEL_30_60_MHZ		(0 << 0)
+#define HCFG_FSLSPCLKSEL_48_MHZ			(1 << 0)
+#define HCFG_FSLSPCLKSEL_6_MHZ			(2 << 0)
+
+#define HFIR				HSOTG_REG(0x0404)
+#define HFIR_FRINT_MASK			(0xffff << 0)
+#define HFIR_FRINT_SHIFT		0
+#define HFIR_RLDCTRL			(1 << 16)
+
+#define HFNUM				HSOTG_REG(0x0408)
+#define HFNUM_FRREM_MASK		(0xffff << 16)
+#define HFNUM_FRREM_SHIFT		16
+#define HFNUM_FRNUM_MASK		(0xffff << 0)
+#define HFNUM_FRNUM_SHIFT		0
+#define HFNUM_MAX_FRNUM			0x3fff
+
+#define HPTXSTS				HSOTG_REG(0x0410)
+#define TXSTS_QTOP_ODD			(1 << 31)
+#define TXSTS_QTOP_CHNEP_MASK		(0xf << 27)
+#define TXSTS_QTOP_CHNEP_SHIFT		27
+#define TXSTS_QTOP_TOKEN_MASK		(0x3 << 25)
+#define TXSTS_QTOP_TOKEN_SHIFT		25
+#define TXSTS_QTOP_TERMINATE		(1 << 24)
+#define TXSTS_QSPCAVAIL_MASK		(0xff << 16)
+#define TXSTS_QSPCAVAIL_SHIFT		16
+#define TXSTS_FSPCAVAIL_MASK		(0xffff << 0)
+#define TXSTS_FSPCAVAIL_SHIFT		0
+
+#define HAINT				HSOTG_REG(0x0414)
+#define HAINTMSK			HSOTG_REG(0x0418)
+#define HFLBADDR			HSOTG_REG(0x041c)
+
+#define HPRT0				HSOTG_REG(0x0440)
+#define HPRT0_SPD_MASK			(0x3 << 17)
+#define HPRT0_SPD_SHIFT			17
+#define HPRT0_SPD_HIGH_SPEED		(0 << 17)
+#define HPRT0_SPD_FULL_SPEED		(1 << 17)
+#define HPRT0_SPD_LOW_SPEED		(2 << 17)
+#define HPRT0_TSTCTL_MASK		(0xf << 13)
+#define HPRT0_TSTCTL_SHIFT		13
+#define HPRT0_PWR			(1 << 12)
+#define HPRT0_LNSTS_MASK		(0x3 << 10)
+#define HPRT0_LNSTS_SHIFT		10
+#define HPRT0_RST			(1 << 8)
+#define HPRT0_SUSP			(1 << 7)
+#define HPRT0_RES			(1 << 6)
+#define HPRT0_OVRCURRCHG		(1 << 5)
+#define HPRT0_OVRCURRACT		(1 << 4)
+#define HPRT0_ENACHG			(1 << 3)
+#define HPRT0_ENA			(1 << 2)
+#define HPRT0_CONNDET			(1 << 1)
+#define HPRT0_CONNSTS			(1 << 0)
+
+#define HCCHAR(_ch)			HSOTG_REG(0x0500 + 0x20 * (_ch))
+#define HCCHAR_CHENA			(1 << 31)
+#define HCCHAR_CHDIS			(1 << 30)
+#define HCCHAR_ODDFRM			(1 << 29)
+#define HCCHAR_DEVADDR_MASK		(0x7f << 22)
+#define HCCHAR_DEVADDR_SHIFT		22
+#define HCCHAR_MULTICNT_MASK		(0x3 << 20)
+#define HCCHAR_MULTICNT_SHIFT		20
+#define HCCHAR_EPTYPE_MASK		(0x3 << 18)
+#define HCCHAR_EPTYPE_SHIFT		18
+#define HCCHAR_LSPDDEV			(1 << 17)
+#define HCCHAR_EPDIR			(1 << 15)
+#define HCCHAR_EPNUM_MASK		(0xf << 11)
+#define HCCHAR_EPNUM_SHIFT		11
+#define HCCHAR_MPS_MASK			(0x7ff << 0)
+#define HCCHAR_MPS_SHIFT		0
+
+#define HCSPLT(_ch)			HSOTG_REG(0x0504 + 0x20 * (_ch))
+#define HCSPLT_SPLTENA			(1 << 31)
+#define HCSPLT_COMPSPLT			(1 << 16)
+#define HCSPLT_XACTPOS_MASK		(0x3 << 14)
+#define HCSPLT_XACTPOS_SHIFT		14
+#define HCSPLT_XACTPOS_MID		(0 << 14)
+#define HCSPLT_XACTPOS_END		(1 << 14)
+#define HCSPLT_XACTPOS_BEGIN		(2 << 14)
+#define HCSPLT_XACTPOS_ALL		(3 << 14)
+#define HCSPLT_HUBADDR_MASK		(0x7f << 7)
+#define HCSPLT_HUBADDR_SHIFT		7
+#define HCSPLT_PRTADDR_MASK		(0x7f << 0)
+#define HCSPLT_PRTADDR_SHIFT		0
+
+#define HCINT(_ch)			HSOTG_REG(0x0508 + 0x20 * (_ch))
+#define HCINTMSK(_ch)			HSOTG_REG(0x050c + 0x20 * (_ch))
+#define HCINTMSK_RESERVED14_31		(0x3ffff << 14)
+#define HCINTMSK_FRM_LIST_ROLL		(1 << 13)
+#define HCINTMSK_XCS_XACT		(1 << 12)
+#define HCINTMSK_BNA			(1 << 11)
+#define HCINTMSK_DATATGLERR		(1 << 10)
+#define HCINTMSK_FRMOVRUN		(1 << 9)
+#define HCINTMSK_BBLERR			(1 << 8)
+#define HCINTMSK_XACTERR		(1 << 7)
+#define HCINTMSK_NYET			(1 << 6)
+#define HCINTMSK_ACK			(1 << 5)
+#define HCINTMSK_NAK			(1 << 4)
+#define HCINTMSK_STALL			(1 << 3)
+#define HCINTMSK_AHBERR			(1 << 2)
+#define HCINTMSK_CHHLTD			(1 << 1)
+#define HCINTMSK_XFERCOMPL		(1 << 0)
+
+#define HCTSIZ(_ch)			HSOTG_REG(0x0510 + 0x20 * (_ch))
+#define TSIZ_DOPNG			(1 << 31)
+#define TSIZ_SC_MC_PID_MASK		(0x3 << 29)
+#define TSIZ_SC_MC_PID_SHIFT		29
+#define TSIZ_SC_MC_PID_DATA0		(0 << 29)
+#define TSIZ_SC_MC_PID_DATA2		(1 << 29)
+#define TSIZ_SC_MC_PID_DATA1		(2 << 29)
+#define TSIZ_SC_MC_PID_MDATA		(3 << 29)
+#define TSIZ_SC_MC_PID_SETUP		(3 << 29)
+#define TSIZ_PKTCNT_MASK		(0x3ff << 19)
+#define TSIZ_PKTCNT_SHIFT		19
+#define TSIZ_NTD_MASK			(0xff << 8)
+#define TSIZ_NTD_SHIFT			8
+#define TSIZ_SCHINFO_MASK		(0xff << 0)
+#define TSIZ_SCHINFO_SHIFT		0
+#define TSIZ_XFERSIZE_MASK		(0x7ffff << 0)
+#define TSIZ_XFERSIZE_SHIFT		0
+
+#define HCDMA(_ch)			HSOTG_REG(0x0514 + 0x20 * (_ch))
+#define HCDMA_DMA_ADDR_MASK		(0x1fffff << 11)
+#define HCDMA_DMA_ADDR_SHIFT		11
+#define HCDMA_CTD_MASK			(0xff << 3)
+#define HCDMA_CTD_SHIFT			3
+
+#define HCDMAB(_ch)			HSOTG_REG(0x051c + 0x20 * (_ch))
+
+#define HCFIFO(_ch)			HSOTG_REG(0x1000 + 0x1000 * (_ch))
+
+/**
+ * struct dwc2_hcd_dma_desc - Host-mode DMA descriptor structure
+ *
+ * @status: DMA descriptor status quadlet
+ * @buf:    DMA descriptor data buffer pointer
+ *
+ * DMA Descriptor structure contains two quadlets:
+ * Status quadlet and Data buffer pointer.
+ */
+struct dwc2_hcd_dma_desc {
+	u32 status;
+	u32 buf;
+};
+
+#define HOST_DMA_A			(1 << 31)
+#define HOST_DMA_STS_MASK		(0x3 << 28)
+#define HOST_DMA_STS_SHIFT		28
+#define HOST_DMA_STS_PKTERR		(1 << 28)
+#define HOST_DMA_EOL			(1 << 26)
+#define HOST_DMA_IOC			(1 << 25)
+#define HOST_DMA_SUP			(1 << 24)
+#define HOST_DMA_ALT_QTD		(1 << 23)
+#define HOST_DMA_QTD_OFFSET_MASK	(0x3f << 17)
+#define HOST_DMA_QTD_OFFSET_SHIFT	17
+#define HOST_DMA_ISOC_NBYTES_MASK	(0xfff << 0)
+#define HOST_DMA_ISOC_NBYTES_SHIFT	0
+#define HOST_DMA_NBYTES_MASK		(0x1ffff << 0)
+#define HOST_DMA_NBYTES_SHIFT		0
+
+#define MAX_DMA_DESC_SIZE		131071
+#define MAX_DMA_DESC_NUM_GENERIC	64
+#define MAX_DMA_DESC_NUM_HS_ISOC	256
+
+#endif /* __DWC2_HW_H__ */
diff --git a/drivers/staging/dwc2/pci.c b/drivers/staging/dwc2/pci.c
new file mode 100644
index 00000000000..69c65eb8683
--- /dev/null
+++ b/drivers/staging/dwc2/pci.c
@@ -0,0 +1,180 @@
+/*
+ * pci.c - DesignWare HS OTG Controller PCI driver
+ *
+ * Copyright (C) 2004-2013 Synopsys, Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions, and the following disclaimer,
+ *    without modification.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The names of the above-listed copyright holders may not be used
+ *    to endorse or promote products derived from this software without
+ *    specific prior written permission.
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation; either version 2 of the License, or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+ * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * Provides the initialization and cleanup entry points for the DWC_otg PCI
+ * driver
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/spinlock.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/pci.h>
+#include <linux/usb.h>
+
+#include <linux/usb/hcd.h>
+#include <linux/usb/ch11.h>
+
+#include "core.h"
+#include "hcd.h"
+
+#define PCI_VENDOR_ID_SYNOPSYS		0x16c3
+#define PCI_PRODUCT_ID_HAPS_HSOTG	0xabc0
+
+static const char dwc2_driver_name[] = "dwc2";
+
+static struct dwc2_core_params dwc2_module_params = {
+	.otg_cap			= -1,
+	.otg_ver			= -1,
+	.dma_enable			= -1,
+	.dma_desc_enable		= 0,
+	.speed				= -1,
+	.enable_dynamic_fifo		= -1,
+	.en_multiple_tx_fifo		= -1,
+	.host_rx_fifo_size		= 1024,
+	.host_nperio_tx_fifo_size	= 256,
+	.host_perio_tx_fifo_size	= 1024,
+	.max_transfer_size		= 65535,
+	.max_packet_count		= 511,
+	.host_channels			= -1,
+	.phy_type			= -1,
+	.phy_utmi_width			= 16,	/* 16 bits - NOT DETECTABLE */
+	.phy_ulpi_ddr			= -1,
+	.phy_ulpi_ext_vbus		= -1,
+	.i2c_enable			= -1,
+	.ulpi_fs_ls			= -1,
+	.host_support_fs_ls_low_power	= -1,
+	.host_ls_low_power_phy_clk	= -1,
+	.ts_dline			= -1,
+	.reload_ctl			= -1,
+	.ahb_single			= -1,
+};
+
+/**
+ * dwc2_driver_remove() - Called when the DWC_otg core is unregistered with the
+ * DWC_otg driver
+ *
+ * @dev: Bus device
+ *
+ * This routine is called, for example, when the rmmod command is executed. The
+ * device may or may not be electrically present. If it is present, the driver
+ * stops device processing. Any resources used on behalf of this device are
+ * freed.
+ */
+static void dwc2_driver_remove(struct pci_dev *dev)
+{
+	struct dwc2_hsotg *hsotg = pci_get_drvdata(dev);
+
+	dev_dbg(&dev->dev, "%s(%p)\n", __func__, dev);
+
+	dwc2_hcd_remove(hsotg);
+	pci_disable_device(dev);
+}
+
+/**
+ * dwc2_driver_probe() - Called when the DWC_otg core is bound to the DWC_otg
+ * driver
+ *
+ * @dev: Bus device
+ *
+ * This routine creates the driver components required to control the device
+ * (core, HCD, and PCD) and initializes the device. The driver components are
+ * stored in a dwc2_hsotg structure. A reference to the dwc2_hsotg is saved
+ * in the device private data. This allows the driver to access the dwc2_hsotg
+ * structure on subsequent calls to driver methods for this device.
+ */
+static int dwc2_driver_probe(struct pci_dev *dev,
+			     const struct pci_device_id *id)
+{
+	struct dwc2_hsotg *hsotg;
+	int retval;
+
+	dev_dbg(&dev->dev, "%s(%p)\n", __func__, dev);
+
+	hsotg = devm_kzalloc(&dev->dev, sizeof(*hsotg), GFP_KERNEL);
+	if (!hsotg)
+		return -ENOMEM;
+
+	pci_set_power_state(dev, PCI_D0);
+
+	hsotg->dev = &dev->dev;
+	hsotg->regs = devm_request_and_ioremap(&dev->dev, &dev->resource[0]);
+	if (!hsotg->regs)
+		return -ENOMEM;
+
+	dev_dbg(&dev->dev, "mapped PA %08lx to VA %p\n",
+		(unsigned long)pci_resource_start(dev, 0), hsotg->regs);
+
+	if (pci_enable_device(dev) < 0)
+		return -ENODEV;
+
+	pci_set_master(dev);
+
+	retval = dwc2_hcd_init(hsotg, dev->irq, &dwc2_module_params);
+	if (retval) {
+		pci_disable_device(dev);
+		return retval;
+	}
+
+	pci_set_drvdata(dev, hsotg);
+	dev_dbg(&dev->dev, "hsotg=%p\n", hsotg);
+
+	return retval;
+}
+
+static DEFINE_PCI_DEVICE_TABLE(dwc2_pci_ids) = {
+	{
+		PCI_DEVICE(PCI_VENDOR_ID_SYNOPSYS, PCI_PRODUCT_ID_HAPS_HSOTG),
+	},
+	{ /* end: all zeroes */ }
+};
+MODULE_DEVICE_TABLE(pci, dwc2_pci_ids);
+
+static struct pci_driver dwc2_pci_driver = {
+	.name = dwc2_driver_name,
+	.id_table = dwc2_pci_ids,
+	.probe = dwc2_driver_probe,
+	.remove = dwc2_driver_remove,
+};
+
+module_pci_driver(dwc2_pci_driver);
+
+MODULE_DESCRIPTION("DESIGNWARE HS OTG PCI Bus Glue");
+MODULE_AUTHOR("Synopsys, Inc.");
+MODULE_LICENSE("Dual BSD/GPL");
diff --git a/drivers/staging/dwc2/platform.c b/drivers/staging/dwc2/platform.c
new file mode 100644
index 00000000000..1f3d581a107
--- /dev/null
+++ b/drivers/staging/dwc2/platform.c
@@ -0,0 +1,145 @@
+/*
+ * platform.c - DesignWare HS OTG Controller platform driver
+ *
+ * Copyright (C) Matthijs Kooijman <matthijs@stdin.nl>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions, and the following disclaimer,
+ *    without modification.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The names of the above-listed copyright holders may not be used
+ *    to endorse or promote products derived from this software without
+ *    specific prior written permission.
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation; either version 2 of the License, or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+ * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/platform_device.h>
+
+#include "core.h"
+#include "hcd.h"
+
+static const char dwc2_driver_name[] = "dwc2";
+
+/**
+ * dwc2_driver_remove() - Called when the DWC_otg core is unregistered with the
+ * DWC_otg driver
+ *
+ * @dev: Platform device
+ *
+ * This routine is called, for example, when the rmmod command is executed. The
+ * device may or may not be electrically present. If it is present, the driver
+ * stops device processing. Any resources used on behalf of this device are
+ * freed.
+ */
+static int dwc2_driver_remove(struct platform_device *dev)
+{
+	struct dwc2_hsotg *hsotg = platform_get_drvdata(dev);
+
+	dwc2_hcd_remove(hsotg);
+
+	return 0;
+}
+
+/**
+ * dwc2_driver_probe() - Called when the DWC_otg core is bound to the DWC_otg
+ * driver
+ *
+ * @dev: Platform device
+ *
+ * This routine creates the driver components required to control the device
+ * (core, HCD, and PCD) and initializes the device. The driver components are
+ * stored in a dwc2_hsotg structure. A reference to the dwc2_hsotg is saved
+ * in the device private data. This allows the driver to access the dwc2_hsotg
+ * structure on subsequent calls to driver methods for this device.
+ */
+static int dwc2_driver_probe(struct platform_device *dev)
+{
+	struct dwc2_hsotg *hsotg;
+	struct resource *res;
+	int retval;
+	int irq;
+	struct dwc2_core_params params;
+
+	/* Default all params to autodetect */
+	dwc2_set_all_params(&params, -1);
+
+	hsotg = devm_kzalloc(&dev->dev, sizeof(*hsotg), GFP_KERNEL);
+	if (!hsotg)
+		return -ENOMEM;
+
+	hsotg->dev = &dev->dev;
+
+	irq = platform_get_irq(dev, 0);
+	if (irq < 0) {
+		dev_err(&dev->dev, "missing IRQ resource\n");
+		return -EINVAL;
+	}
+
+	res = platform_get_resource(dev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&dev->dev, "missing memory base resource\n");
+		return -EINVAL;
+	}
+
+	hsotg->regs = devm_ioremap_resource(&dev->dev, res);
+	if (IS_ERR(hsotg->regs))
+		return PTR_ERR(hsotg->regs);
+
+	dev_dbg(&dev->dev, "mapped PA %08lx to VA %p\n",
+		(unsigned long)res->start, hsotg->regs);
+
+	retval = dwc2_hcd_init(hsotg, irq, &params);
+	if (retval)
+		return retval;
+
+	platform_set_drvdata(dev, hsotg);
+
+	return retval;
+}
+
+static const struct of_device_id dwc2_of_match_table[] = {
+	{ .compatible = "snps,dwc2" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, dwc2_of_match_table);
+
+static struct platform_driver dwc2_platform_driver = {
+	.driver = {
+		.name = (char *)dwc2_driver_name,
+		.of_match_table = dwc2_of_match_table,
+	},
+	.probe = dwc2_driver_probe,
+	.remove = dwc2_driver_remove,
+};
+
+module_platform_driver(dwc2_platform_driver);
+
+MODULE_DESCRIPTION("DESIGNWARE HS OTG Platform Glue");
+MODULE_AUTHOR("Matthijs Kooijman <matthijs@stdin.nl>");
+MODULE_LICENSE("Dual BSD/GPL");
diff --git a/drivers/staging/et131x/et131x.c b/drivers/staging/et131x/et131x.c
index 42ae5e83f90..f73e58f5ef8 100644
--- a/drivers/staging/et131x/et131x.c
+++ b/drivers/staging/et131x/et131x.c
@@ -595,7 +595,7 @@ static int eeprom_write(struct et131x_adapter *adapter, u32 addr, u8 data)
 	 */
 
 	err = eeprom_wait_ready(pdev, NULL);
-	if (err)
+	if (err < 0)
 		return err;
 
 	 /* 2. Write to the LBCIF Control Register:  bit 7=1, bit 6=1, bit 3=0,
@@ -709,7 +709,7 @@ static int eeprom_read(struct et131x_adapter *adapter, u32 addr, u8 *pdata)
 	 */
 
 	err = eeprom_wait_ready(pdev, NULL);
-	if (err)
+	if (err < 0)
 		return err;
 	/* Write to the LBCIF Control Register:  bit 7=1, bit 6=0, bit 3=0,
 	 * and bits 1:0 both =0.  Bit 5 should be set according to the type
@@ -3953,6 +3953,7 @@ static void et131x_pci_remove(struct pci_dev *pdev)
 	unregister_netdev(netdev);
 	phy_disconnect(adapter->phydev);
 	mdiobus_unregister(adapter->mii_bus);
+	cancel_work_sync(&adapter->task);
 	kfree(adapter->mii_bus->irq);
 	mdiobus_free(adapter->mii_bus);
 
diff --git a/drivers/staging/fwserial/fwserial.c b/drivers/staging/fwserial/fwserial.c
index 5a6fb44f38a..e5818a1c226 100644
--- a/drivers/staging/fwserial/fwserial.c
+++ b/drivers/staging/fwserial/fwserial.c
@@ -744,7 +744,6 @@ static void fwtty_tx_complete(struct fw_card *card, int rcode,
 			      struct fwtty_transaction *txn)
 {
 	struct fwtty_port *port = txn->port;
-	struct tty_struct *tty;
 	int len;
 
 	fwtty_dbg(port, "rcode: %d", rcode);
@@ -769,13 +768,8 @@ static void fwtty_tx_complete(struct fw_card *card, int rcode,
 		port->stats.dropped += txn->dma_pended.len;
 	}
 
-	if (len < WAKEUP_CHARS) {
-		tty = tty_port_tty_get(&port->port);
-		if (tty) {
-			tty_wakeup(tty);
-			tty_kref_put(tty);
-		}
-	}
+	if (len < WAKEUP_CHARS)
+		tty_port_tty_wakeup(&port->port);
 }
 
 static int fwtty_tx(struct fwtty_port *port, bool drain)
@@ -1527,7 +1521,7 @@ static void fwtty_debugfs_show_port(struct seq_file *m, struct fwtty_port *port)
 		   stats.watermark);
 
 	if (port->port.console) {
-		seq_printf(m, "\n    ");
+		seq_puts(m, "\n    ");
 		(*port->fwcon_ops->proc_show)(m, port->con_data);
 	}
 
@@ -1559,7 +1553,7 @@ static int fwtty_proc_show(struct seq_file *m, void *v)
 		if (capable(CAP_SYS_ADMIN))
 			fwtty_proc_show_port(m, port);
 		fwtty_port_put(port);
-		seq_printf(m, "\n");
+		seq_puts(m, "\n");
 	}
 	return 0;
 }
@@ -1577,7 +1571,7 @@ static int fwtty_debugfs_stats_show(struct seq_file *m, void *v)
 			fwtty_proc_show_port(m, port);
 			fwtty_debugfs_show_port(m, port);
 			fwtty_port_put(port);
-			seq_printf(m, "\n");
+			seq_puts(m, "\n");
 		}
 	}
 	return 0;
diff --git a/drivers/staging/gdm72xx/gdm_qos.c b/drivers/staging/gdm72xx/gdm_qos.c
index 1e630312372..b795353e834 100644
--- a/drivers/staging/gdm72xx/gdm_qos.c
+++ b/drivers/staging/gdm72xx/gdm_qos.c
@@ -337,7 +337,6 @@ void gdm_recv_qos_hci_packet(void *nic_ptr, u8 *buf, int size)
 	struct nic *nic = nic_ptr;
 	u32 i, SFID, index, pos;
 	u8 subCmdEvt;
-	u8 len;
 	struct qos_cb_s *qcb = &nic->qos;
 	struct qos_entry_s *entry, *n;
 	struct list_head send_list;
@@ -347,8 +346,6 @@ void gdm_recv_qos_hci_packet(void *nic_ptr, u8 *buf, int size)
 	subCmdEvt = (u8)buf[4];
 
 	if (subCmdEvt == QOS_REPORT) {
-		len = (u8)buf[5];
-
 		spin_lock_irqsave(&qcb->qos_lock, flags);
 		for (i = 0; i < qcb->qos_list_cnt; i++) {
 			SFID = ((buf[(i*5)+6]<<24)&0xff000000);
@@ -368,21 +365,24 @@ void gdm_recv_qos_hci_packet(void *nic_ptr, u8 *buf, int size)
 		spin_unlock_irqrestore(&qcb->qos_lock, flags);
 		send_qos_list(nic, &send_list);
 		return;
-	} else if (subCmdEvt == QOS_ADD) {
-		pos = 5;
-		len = (u8)buf[pos++];
-
-		SFID = ((buf[pos++]<<24)&0xff000000);
-		SFID += ((buf[pos++]<<16)&0xff0000);
-		SFID += ((buf[pos++]<<8)&0xff00);
-		SFID += (buf[pos++]);
-
-		index = get_csr(qcb, SFID, 1);
-		if (index == -1) {
-			netdev_err(nic->netdev, "QoS ERROR: csr Update Error\n");
-			return;
-		}
+	}
+
+	/* subCmdEvt == QOS_ADD || subCmdEvt == QOS_CHANG_DEL */
+	pos = 6;
+	SFID = ((buf[pos++]<<24)&0xff000000);
+	SFID += ((buf[pos++]<<16)&0xff0000);
+	SFID += ((buf[pos++]<<8)&0xff00);
+	SFID += (buf[pos++]);
+
+	index = get_csr(qcb, SFID, 1);
+	if (index == -1) {
+		netdev_err(nic->netdev,
+			   "QoS ERROR: csr Update Error / Wrong index (%d) \n",
+			   index);
+		return;
+	}
 
+	if (subCmdEvt == QOS_ADD) {
 		netdev_dbg(nic->netdev, "QOS_ADD SFID = 0x%x, index=%d\n",
 			   SFID, index);
 
@@ -424,19 +424,6 @@ void gdm_recv_qos_hci_packet(void *nic_ptr, u8 *buf, int size)
 		qcb->qos_limit_size = 254/qcb->qos_list_cnt;
 		spin_unlock_irqrestore(&qcb->qos_lock, flags);
 	} else if (subCmdEvt == QOS_CHANGE_DEL) {
-		pos = 5;
-		len = (u8)buf[pos++];
-		SFID = ((buf[pos++]<<24)&0xff000000);
-		SFID += ((buf[pos++]<<16)&0xff0000);
-		SFID += ((buf[pos++]<<8)&0xff00);
-		SFID += (buf[pos++]);
-		index = get_csr(qcb, SFID, 1);
-		if (index == -1) {
-			netdev_err(nic->netdev, "QoS ERROR: Wrong index(%d)\n",
-				   index);
-			return;
-		}
-
 		netdev_dbg(nic->netdev, "QOS_CHANGE_DEL SFID = 0x%x, index=%d\n",
 			   SFID, index);
 
diff --git a/drivers/staging/gdm72xx/gdm_sdio.c b/drivers/staging/gdm72xx/gdm_sdio.c
index 695762b0e94..047a4d77f5e 100644
--- a/drivers/staging/gdm72xx/gdm_sdio.c
+++ b/drivers/staging/gdm72xx/gdm_sdio.c
@@ -689,6 +689,7 @@ static void sdio_wimax_remove(struct sdio_func *func)
 	struct phy_dev *phy_dev = sdio_get_drvdata(func);
 	struct sdiowm_dev *sdev = phy_dev->priv_dev;
 
+	cancel_work_sync(&sdev->ws);
 	if (phy_dev->netdev)
 		unregister_wimax_device(phy_dev);
 	sdio_claim_host(func);
diff --git a/drivers/staging/gdm72xx/netlink_k.c b/drivers/staging/gdm72xx/netlink_k.c
index 52c25ba5831..8a92605adbf 100644
--- a/drivers/staging/gdm72xx/netlink_k.c
+++ b/drivers/staging/gdm72xx/netlink_k.c
@@ -18,6 +18,7 @@
 #include <linux/netlink.h>
 #include <asm/byteorder.h>
 #include <net/sock.h>
+#include "netlink_k.h"
 
 #if !defined(NLMSG_HDRLEN)
 #define NLMSG_HDRLEN	 ((int) NLMSG_ALIGN(sizeof(struct nlmsghdr)))
diff --git a/drivers/staging/gdm72xx/sdio_boot.c b/drivers/staging/gdm72xx/sdio_boot.c
index 93046dda78f..4302fcbdfdc 100644
--- a/drivers/staging/gdm72xx/sdio_boot.c
+++ b/drivers/staging/gdm72xx/sdio_boot.c
@@ -27,6 +27,7 @@
 #include <linux/firmware.h>
 
 #include "gdm_sdio.h"
+#include "sdio_boot.h"
 
 #define TYPE_A_HEADER_SIZE	4
 #define TYPE_A_LOOKAHEAD_SIZE   16
diff --git a/drivers/staging/iio/Documentation/trigger.txt b/drivers/staging/iio/Documentation/trigger.txt
index 75cc37ff1ed..64e2e08fb4d 100644
--- a/drivers/staging/iio/Documentation/trigger.txt
+++ b/drivers/staging/iio/Documentation/trigger.txt
@@ -10,9 +10,6 @@ struct iio_trig *trig = iio_trigger_alloc("<trigger format string>", ...);
 allocates a trigger structure.  The key elements to then fill in within
 a driver are:
 
-trig->private_data
-	Device specific private data.
-
 trig->owner
 	Typically set to THIS_MODULE. Used to ensure correct
 	ownership of core allocated resources.
diff --git a/drivers/staging/iio/Kconfig b/drivers/staging/iio/Kconfig
index dc267175a2b..db4d6dc0324 100644
--- a/drivers/staging/iio/Kconfig
+++ b/drivers/staging/iio/Kconfig
@@ -4,14 +4,6 @@
 menu "IIO staging drivers"
 	depends on IIO
 
-config IIO_ST_HWMON
-	tristate "Hwmon driver that uses channels specified via iio maps"
-	depends on HWMON
-	help
-	  This is a platform driver that in combination with a suitable
-	  map allows IIO devices to provide  basic hwmon functionality
-	  for those channels specified in the map.
-
 source "drivers/staging/iio/accel/Kconfig"
 source "drivers/staging/iio/adc/Kconfig"
 source "drivers/staging/iio/addac/Kconfig"
diff --git a/drivers/staging/iio/Makefile b/drivers/staging/iio/Makefile
index 158e0a017e7..d87106135b2 100644
--- a/drivers/staging/iio/Makefile
+++ b/drivers/staging/iio/Makefile
@@ -9,8 +9,6 @@ iio_dummy-$(CONFIG_IIO_SIMPLE_DUMMY_BUFFER) += iio_simple_dummy_buffer.o
 
 obj-$(CONFIG_IIO_DUMMY_EVGEN) += iio_dummy_evgen.o
 
-obj-$(CONFIG_IIO_ST_HWMON) += iio_hwmon.o
-
 obj-y += accel/
 obj-y += adc/
 obj-y += addac/
diff --git a/drivers/staging/iio/accel/adis16201_core.c b/drivers/staging/iio/accel/adis16201_core.c
index 9e5791ff2a0..ab8ec7af88b 100644
--- a/drivers/staging/iio/accel/adis16201_core.c
+++ b/drivers/staging/iio/accel/adis16201_core.c
@@ -134,14 +134,14 @@ static const struct iio_chan_spec adis16201_channels[] = {
 	ADIS_SUPPLY_CHAN(ADIS16201_SUPPLY_OUT, ADIS16201_SCAN_SUPPLY, 12),
 	ADIS_TEMP_CHAN(ADIS16201_TEMP_OUT, ADIS16201_SCAN_TEMP, 12),
 	ADIS_ACCEL_CHAN(X, ADIS16201_XACCL_OUT, ADIS16201_SCAN_ACC_X,
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT, 14),
+		BIT(IIO_CHAN_INFO_CALIBBIAS), 14),
 	ADIS_ACCEL_CHAN(Y, ADIS16201_YACCL_OUT, ADIS16201_SCAN_ACC_Y,
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT, 14),
+		BIT(IIO_CHAN_INFO_CALIBBIAS), 14),
 	ADIS_AUX_ADC_CHAN(ADIS16201_AUX_ADC, ADIS16201_SCAN_AUX_ADC, 12),
 	ADIS_INCLI_CHAN(X, ADIS16201_XINCL_OUT, ADIS16201_SCAN_INCLI_X,
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT, 14),
+		BIT(IIO_CHAN_INFO_CALIBBIAS), 14),
 	ADIS_INCLI_CHAN(X, ADIS16201_YINCL_OUT, ADIS16201_SCAN_INCLI_Y,
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT, 14),
+		BIT(IIO_CHAN_INFO_CALIBBIAS), 14),
 	IIO_CHAN_SOFT_TIMESTAMP(7)
 };
 
diff --git a/drivers/staging/iio/accel/adis16203_core.c b/drivers/staging/iio/accel/adis16203_core.c
index 8c235273ff1..b08ac8fdeee 100644
--- a/drivers/staging/iio/accel/adis16203_core.c
+++ b/drivers/staging/iio/accel/adis16203_core.c
@@ -102,7 +102,7 @@ static const struct iio_chan_spec adis16203_channels[] = {
 	ADIS_SUPPLY_CHAN(ADIS16203_SUPPLY_OUT, ADIS16203_SCAN_SUPPLY, 12),
 	ADIS_AUX_ADC_CHAN(ADIS16203_AUX_ADC, ADIS16203_SCAN_AUX_ADC, 12),
 	ADIS_INCLI_CHAN(X, ADIS16203_XINCL_OUT, ADIS16203_SCAN_INCLI_X,
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT, 14),
+		BIT(IIO_CHAN_INFO_CALIBBIAS), 14),
 	/* Fixme: Not what it appears to be - see data sheet */
 	ADIS_INCLI_CHAN(Y, ADIS16203_YINCL_OUT, ADIS16203_SCAN_INCLI_Y, 0, 14),
 	ADIS_TEMP_CHAN(ADIS16203_TEMP_OUT, ADIS16203_SCAN_TEMP, 12),
diff --git a/drivers/staging/iio/accel/adis16204_core.c b/drivers/staging/iio/accel/adis16204_core.c
index f3592668e06..792ec25a50d 100644
--- a/drivers/staging/iio/accel/adis16204_core.c
+++ b/drivers/staging/iio/accel/adis16204_core.c
@@ -140,13 +140,11 @@ static const struct iio_chan_spec adis16204_channels[] = {
 	ADIS_AUX_ADC_CHAN(ADIS16204_AUX_ADC, ADIS16204_SCAN_AUX_ADC, 12),
 	ADIS_TEMP_CHAN(ADIS16204_TEMP_OUT, ADIS16204_SCAN_TEMP, 12),
 	ADIS_ACCEL_CHAN(X, ADIS16204_XACCL_OUT, ADIS16204_SCAN_ACC_X,
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
-		IIO_CHAN_INFO_PEAK_SEPARATE_BIT, 14),
+		BIT(IIO_CHAN_INFO_CALIBBIAS) | BIT(IIO_CHAN_INFO_PEAK), 14),
 	ADIS_ACCEL_CHAN(Y, ADIS16204_YACCL_OUT, ADIS16204_SCAN_ACC_Y,
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
-		IIO_CHAN_INFO_PEAK_SEPARATE_BIT, 14),
+		BIT(IIO_CHAN_INFO_CALIBBIAS) | BIT(IIO_CHAN_INFO_PEAK), 14),
 	ADIS_ACCEL_CHAN(ROOT_SUM_SQUARED_X_Y, ADIS16204_XY_RSS_OUT,
-		ADIS16204_SCAN_ACC_XY, IIO_CHAN_INFO_PEAK_SEPARATE_BIT, 14),
+		ADIS16204_SCAN_ACC_XY, BIT(IIO_CHAN_INFO_PEAK), 14),
 	IIO_CHAN_SOFT_TIMESTAMP(5),
 };
 
diff --git a/drivers/staging/iio/accel/adis16209_core.c b/drivers/staging/iio/accel/adis16209_core.c
index 69c50ee44ce..323c169d699 100644
--- a/drivers/staging/iio/accel/adis16209_core.c
+++ b/drivers/staging/iio/accel/adis16209_core.c
@@ -133,9 +133,9 @@ static const struct iio_chan_spec adis16209_channels[] = {
 	ADIS_SUPPLY_CHAN(ADIS16209_SUPPLY_OUT, ADIS16209_SCAN_SUPPLY, 14),
 	ADIS_TEMP_CHAN(ADIS16209_TEMP_OUT, ADIS16209_SCAN_TEMP, 12),
 	ADIS_ACCEL_CHAN(X, ADIS16209_XACCL_OUT, ADIS16209_SCAN_ACC_X,
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT, 14),
+		BIT(IIO_CHAN_INFO_CALIBBIAS), 14),
 	ADIS_ACCEL_CHAN(Y, ADIS16209_YACCL_OUT, ADIS16209_SCAN_ACC_Y,
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT, 14),
+		BIT(IIO_CHAN_INFO_CALIBBIAS), 14),
 	ADIS_AUX_ADC_CHAN(ADIS16209_AUX_ADC, ADIS16209_SCAN_AUX_ADC, 12),
 	ADIS_INCLI_CHAN(X, ADIS16209_XINCL_OUT, ADIS16209_SCAN_INCLI_X, 0, 14),
 	ADIS_INCLI_CHAN(Y, ADIS16209_YINCL_OUT, ADIS16209_SCAN_INCLI_Y, 0, 14),
diff --git a/drivers/staging/iio/accel/adis16220_core.c b/drivers/staging/iio/accel/adis16220_core.c
index 370b01aa767..0e72f795ed0 100644
--- a/drivers/staging/iio/accel/adis16220_core.c
+++ b/drivers/staging/iio/accel/adis16220_core.c
@@ -344,37 +344,37 @@ static const struct iio_chan_spec adis16220_channels[] = {
 		.indexed = 1,
 		.channel = 0,
 		.extend_name = "supply",
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			     IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_SCALE),
 		.address = in_supply,
 	}, {
 		.type = IIO_ACCEL,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			     IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
-			     IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
-			     IIO_CHAN_INFO_PEAK_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_OFFSET) |
+			BIT(IIO_CHAN_INFO_SCALE) |
+			BIT(IIO_CHAN_INFO_PEAK),
 		.address = accel,
 	}, {
 		.type = IIO_TEMP,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			     IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
-			     IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_OFFSET) |
+			BIT(IIO_CHAN_INFO_SCALE),
 		.address = temp,
 	}, {
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
 		.channel = 1,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			     IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
-			     IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_OFFSET) |
+			BIT(IIO_CHAN_INFO_SCALE),
 		.address = in_1,
 	}, {
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
 		.channel = 2,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.address = in_2,
 	}
 };
diff --git a/drivers/staging/iio/accel/adis16240_core.c b/drivers/staging/iio/accel/adis16240_core.c
index e97fa0b0233..fd1f0fd0fba 100644
--- a/drivers/staging/iio/accel/adis16240_core.c
+++ b/drivers/staging/iio/accel/adis16240_core.c
@@ -176,14 +176,11 @@ static const struct iio_chan_spec adis16240_channels[] = {
 	ADIS_SUPPLY_CHAN(ADIS16240_SUPPLY_OUT, ADIS16240_SCAN_SUPPLY, 10),
 	ADIS_AUX_ADC_CHAN(ADIS16240_AUX_ADC, ADIS16240_SCAN_AUX_ADC, 10),
 	ADIS_ACCEL_CHAN(X, ADIS16240_XACCL_OUT, ADIS16240_SCAN_ACC_X,
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
-		IIO_CHAN_INFO_PEAK_SEPARATE_BIT, 10),
+		BIT(IIO_CHAN_INFO_CALIBBIAS) | BIT(IIO_CHAN_INFO_PEAK), 10),
 	ADIS_ACCEL_CHAN(Y, ADIS16240_YACCL_OUT, ADIS16240_SCAN_ACC_Y,
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
-		IIO_CHAN_INFO_PEAK_SEPARATE_BIT, 10),
+		BIT(IIO_CHAN_INFO_CALIBBIAS) | BIT(IIO_CHAN_INFO_PEAK), 10),
 	ADIS_ACCEL_CHAN(Z, ADIS16240_ZACCL_OUT, ADIS16240_SCAN_ACC_Z,
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
-		IIO_CHAN_INFO_PEAK_SEPARATE_BIT, 10),
+		BIT(IIO_CHAN_INFO_CALIBBIAS) | BIT(IIO_CHAN_INFO_PEAK), 10),
 	ADIS_TEMP_CHAN(ADIS16240_TEMP_OUT, ADIS16240_SCAN_TEMP, 10),
 	IIO_CHAN_SOFT_TIMESTAMP(6)
 };
diff --git a/drivers/staging/iio/accel/lis3l02dq_core.c b/drivers/staging/iio/accel/lis3l02dq_core.c
index 0e019306439..1bfe5d81792 100644
--- a/drivers/staging/iio/accel/lis3l02dq_core.c
+++ b/drivers/staging/iio/accel/lis3l02dq_core.c
@@ -501,12 +501,6 @@ static irqreturn_t lis3l02dq_event_handler(int irq, void *private)
 	return IRQ_HANDLED;
 }
 
-#define LIS3L02DQ_INFO_MASK				\
-	(IIO_CHAN_INFO_RAW_SEPARATE_BIT |		\
-	 IIO_CHAN_INFO_SCALE_SHARED_BIT |		\
-	 IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |	\
-	 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT)
-
 #define LIS3L02DQ_EVENT_MASK					\
 	(IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING) |	\
 	 IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_FALLING))
@@ -516,7 +510,10 @@ static irqreturn_t lis3l02dq_event_handler(int irq, void *private)
 		.type = IIO_ACCEL,				\
 		.modified = 1,					\
 		.channel2 = mod,				\
-		.info_mask =  LIS3L02DQ_INFO_MASK,		\
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |	\
+			BIT(IIO_CHAN_INFO_CALIBSCALE) |		\
+			BIT(IIO_CHAN_INFO_CALIBBIAS),		\
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \
 		.address = index,				\
 		.scan_index = index,				\
 		.scan_type = {					\
diff --git a/drivers/staging/iio/accel/lis3l02dq_ring.c b/drivers/staging/iio/accel/lis3l02dq_ring.c
index e676403ea3e..5b8f0f6c993 100644
--- a/drivers/staging/iio/accel/lis3l02dq_ring.c
+++ b/drivers/staging/iio/accel/lis3l02dq_ring.c
@@ -228,7 +228,7 @@ error_ret:
 static int lis3l02dq_data_rdy_trigger_set_state(struct iio_trigger *trig,
 						bool state)
 {
-	struct iio_dev *indio_dev = trig->private_data;
+	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
 	int ret = 0;
 	u8 t;
 
@@ -252,7 +252,7 @@ static int lis3l02dq_data_rdy_trigger_set_state(struct iio_trigger *trig,
  */
 static int lis3l02dq_trig_try_reen(struct iio_trigger *trig)
 {
-	struct iio_dev *indio_dev = trig->private_data;
+	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
 	struct lis3l02dq_state *st = iio_priv(indio_dev);
 	int i;
 
@@ -290,7 +290,7 @@ int lis3l02dq_probe_trigger(struct iio_dev *indio_dev)
 
 	st->trig->dev.parent = &st->us->dev;
 	st->trig->ops = &lis3l02dq_trigger_ops;
-	st->trig->private_data = indio_dev;
+	iio_trigger_set_drvdata(st->trig, indio_dev);
 	ret = iio_trigger_register(st->trig);
 	if (ret)
 		goto error_free_trig;
diff --git a/drivers/staging/iio/accel/sca3000_core.c b/drivers/staging/iio/accel/sca3000_core.c
index 14683f583cc..32950ad9485 100644
--- a/drivers/staging/iio/accel/sca3000_core.c
+++ b/drivers/staging/iio/accel/sca3000_core.c
@@ -419,8 +419,6 @@ static IIO_DEVICE_ATTR(measurement_mode, S_IRUGO | S_IWUSR,
 
 static IIO_DEVICE_ATTR(revision, S_IRUGO, sca3000_show_rev, NULL, 0);
 
-#define SCA3000_INFO_MASK			\
-	IIO_CHAN_INFO_RAW_SEPARATE_BIT | IIO_CHAN_INFO_SCALE_SHARED_BIT
 #define SCA3000_EVENT_MASK					\
 	(IIO_EV_BIT(IIO_EV_TYPE_MAG, IIO_EV_DIR_RISING))
 
@@ -429,7 +427,8 @@ static IIO_DEVICE_ATTR(revision, S_IRUGO, sca3000_show_rev, NULL, 0);
 		.type = IIO_ACCEL,				\
 		.modified = 1,					\
 		.channel2 = mod,				\
-		.info_mask = SCA3000_INFO_MASK,			\
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),	\
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),\
 		.address = index,				\
 		.scan_index = index,				\
 		.scan_type = {					\
diff --git a/drivers/staging/iio/adc/Kconfig b/drivers/staging/iio/adc/Kconfig
index 7b2a01d64f5..d990829008f 100644
--- a/drivers/staging/iio/adc/Kconfig
+++ b/drivers/staging/iio/adc/Kconfig
@@ -90,13 +90,6 @@ config AD7192
 	  To compile this driver as a module, choose M here: the
 	  module will be called ad7192.
 
-config ADT7410
-	tristate "Analog Devices ADT7310/ADT7410 temperature sensor driver"
-	depends on I2C || SPI_MASTER
-	help
-	  Say yes here to build support for Analog Devices ADT7310/ADT7410
-	  temperature sensors.
-
 config AD7280
 	tristate "Analog Devices AD7280A Lithium Ion Battery Monitoring System"
 	depends on SPI
diff --git a/drivers/staging/iio/adc/Makefile b/drivers/staging/iio/adc/Makefile
index d285596272a..3e9fb143d25 100644
--- a/drivers/staging/iio/adc/Makefile
+++ b/drivers/staging/iio/adc/Makefile
@@ -16,7 +16,6 @@ obj-$(CONFIG_AD7291) += ad7291.o
 obj-$(CONFIG_AD7780) += ad7780.o
 obj-$(CONFIG_AD7816) += ad7816.o
 obj-$(CONFIG_AD7192) += ad7192.o
-obj-$(CONFIG_ADT7410) += adt7410.o
 obj-$(CONFIG_AD7280) += ad7280a.o
 obj-$(CONFIG_LPC32XX_ADC) += lpc32xx_adc.o
 obj-$(CONFIG_MXS_LRADC) += mxs-lradc.o
diff --git a/drivers/staging/iio/adc/ad7280a.c b/drivers/staging/iio/adc/ad7280a.c
index 1f190c1b12a..2fd6ee3c190 100644
--- a/drivers/staging/iio/adc/ad7280a.c
+++ b/drivers/staging/iio/adc/ad7280a.c
@@ -503,9 +503,10 @@ static int ad7280_channel_init(struct ad7280_state *st)
 				st->channels[cnt].channel = (dev * 6) + ch - 6;
 			}
 			st->channels[cnt].indexed = 1;
-			st->channels[cnt].info_mask =
-				IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-				IIO_CHAN_INFO_SCALE_SHARED_BIT;
+			st->channels[cnt].info_mask_separate =
+				BIT(IIO_CHAN_INFO_RAW);
+			st->channels[cnt].info_mask_shared_by_type =
+				BIT(IIO_CHAN_INFO_SCALE);
 			st->channels[cnt].address =
 				AD7280A_DEVADDR(dev) << 8 | ch;
 			st->channels[cnt].scan_index = cnt;
@@ -521,9 +522,8 @@ static int ad7280_channel_init(struct ad7280_state *st)
 	st->channels[cnt].channel2 = dev * 6;
 	st->channels[cnt].address = AD7280A_ALL_CELLS;
 	st->channels[cnt].indexed = 1;
-	st->channels[cnt].info_mask =
-		IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-		IIO_CHAN_INFO_SCALE_SHARED_BIT;
+	st->channels[cnt].info_mask_separate = BIT(IIO_CHAN_INFO_RAW);
+	st->channels[cnt].info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE);
 	st->channels[cnt].scan_index = cnt;
 	st->channels[cnt].scan_type.sign = 'u';
 	st->channels[cnt].scan_type.realbits = 32;
diff --git a/drivers/staging/iio/adc/ad7291.c b/drivers/staging/iio/adc/ad7291.c
index 6e58e36d242..d088c662d5c 100644
--- a/drivers/staging/iio/adc/ad7291.c
+++ b/drivers/staging/iio/adc/ad7291.c
@@ -536,8 +536,8 @@ static int ad7291_read_raw(struct iio_dev *indio_dev,
 #define AD7291_VOLTAGE_CHAN(_chan)					\
 {									\
 	.type = IIO_VOLTAGE,						\
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |			\
-	IIO_CHAN_INFO_SCALE_SHARED_BIT,					\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),			\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),		\
 	.indexed = 1,							\
 	.channel = _chan,						\
 	.event_mask = IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING)|\
@@ -555,9 +555,9 @@ static const struct iio_chan_spec ad7291_channels[] = {
 	AD7291_VOLTAGE_CHAN(7),
 	{
 		.type = IIO_TEMP,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-				IIO_CHAN_INFO_AVERAGE_RAW_SEPARATE_BIT |
-				IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+				BIT(IIO_CHAN_INFO_AVERAGE_RAW) |
+				BIT(IIO_CHAN_INFO_SCALE),
 		.indexed = 1,
 		.channel = 0,
 		.event_mask =
diff --git a/drivers/staging/iio/adc/ad7606_core.c b/drivers/staging/iio/adc/ad7606_core.c
index bae61cbe921..d104b437842 100644
--- a/drivers/staging/iio/adc/ad7606_core.c
+++ b/drivers/staging/iio/adc/ad7606_core.c
@@ -235,8 +235,8 @@ static const struct attribute_group ad7606_attribute_group_range = {
 		.indexed = 1,					\
 		.channel = num,					\
 		.address = num,					\
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |	\
-				IIO_CHAN_INFO_SCALE_SHARED_BIT, \
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),	\
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),\
 		.scan_index = num,				\
 		.scan_type = IIO_ST('s', 16, 16, 0),		\
 	}
diff --git a/drivers/staging/iio/adc/ad799x.h b/drivers/staging/iio/adc/ad799x.h
index 3e363c4ba21..b51680c1c33 100644
--- a/drivers/staging/iio/adc/ad799x.h
+++ b/drivers/staging/iio/adc/ad799x.h
@@ -87,7 +87,6 @@ struct ad799x_state;
  * struct ad799x_chip_info - chip specifc information
  * @channel:		channel specification
  * @num_channels:	number of channels
- * @int_vref_mv:	the internal reference voltage
  * @monitor_mode:	whether the chip supports monitor interrupts
  * @default_config:	device default configuration
  * @event_attrs:	pointer to the monitor event attribute group
@@ -96,7 +95,6 @@ struct ad799x_state;
 struct ad799x_chip_info {
 	struct iio_chan_spec		channel[9];
 	int				num_channels;
-	u16				int_vref_mv;
 	u16				default_config;
 	const struct iio_info		*info;
 };
@@ -104,12 +102,13 @@ struct ad799x_chip_info {
 struct ad799x_state {
 	struct i2c_client		*client;
 	const struct ad799x_chip_info	*chip_info;
-	struct iio_trigger		*trig;
 	struct regulator		*reg;
 	u16				int_vref_mv;
 	unsigned			id;
-	char				*name;
 	u16				config;
+
+	u8				*rx_buf;
+	unsigned int			transfer_size;
 };
 
 /*
diff --git a/drivers/staging/iio/adc/ad799x_core.c b/drivers/staging/iio/adc/ad799x_core.c
index 077eedbd0a0..8dc97b36e05 100644
--- a/drivers/staging/iio/adc/ad799x_core.c
+++ b/drivers/staging/iio/adc/ad799x_core.c
@@ -48,13 +48,13 @@ static int ad799x_i2c_read16(struct ad799x_state *st, u8 reg, u16 *data)
 	struct i2c_client *client = st->client;
 	int ret = 0;
 
-	ret = i2c_smbus_read_word_data(client, reg);
+	ret = i2c_smbus_read_word_swapped(client, reg);
 	if (ret < 0) {
 		dev_err(&client->dev, "I2C read error\n");
 		return ret;
 	}
 
-	*data = swab16((u16)ret);
+	*data = (u16)ret;
 
 	return 0;
 }
@@ -80,7 +80,7 @@ static int ad799x_i2c_write16(struct ad799x_state *st, u8 reg, u16 data)
 	struct i2c_client *client = st->client;
 	int ret = 0;
 
-	ret = i2c_smbus_write_word_data(client, reg, swab16(data));
+	ret = i2c_smbus_write_word_swapped(client, reg, data);
 	if (ret < 0)
 		dev_err(&client->dev, "I2C write error\n");
 
@@ -104,6 +104,13 @@ static int ad7997_8_update_scan_mode(struct iio_dev *indio_dev,
 {
 	struct ad799x_state *st = iio_priv(indio_dev);
 
+	kfree(st->rx_buf);
+	st->rx_buf = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
+	if (!st->rx_buf)
+		return -ENOMEM;
+
+	st->transfer_size = bitmap_weight(scan_mask, indio_dev->masklength) * 2;
+
 	switch (st->id) {
 	case ad7997:
 	case ad7998:
@@ -460,395 +467,114 @@ static const struct iio_info ad7993_4_7_8_info = {
 #define AD799X_EV_MASK (IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING) | \
 			IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_FALLING))
 
+#define AD799X_CHANNEL(_index, _realbits, _evmask) { \
+	.type = IIO_VOLTAGE, \
+	.indexed = 1, \
+	.channel = (_index), \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \
+	.scan_index = (_index), \
+	.scan_type = IIO_ST('u', _realbits, 16, 12 - (_realbits)), \
+	.event_mask = (_evmask), \
+}
+
 static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 	[ad7991] = {
 		.channel = {
-			[0] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 0,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 0,
-				.scan_type = IIO_ST('u', 12, 16, 0),
-			},
-			[1] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 1,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 1,
-				.scan_type = IIO_ST('u', 12, 16, 0),
-			},
-			[2] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 2,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 2,
-				.scan_type = IIO_ST('u', 12, 16, 0),
-			},
-			[3] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 3,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 3,
-				.scan_type = IIO_ST('u', 12, 16, 0),
-			},
-			[4] = IIO_CHAN_SOFT_TIMESTAMP(4),
+			AD799X_CHANNEL(0, 12, 0),
+			AD799X_CHANNEL(1, 12, 0),
+			AD799X_CHANNEL(2, 12, 0),
+			AD799X_CHANNEL(3, 12, 0),
+			IIO_CHAN_SOFT_TIMESTAMP(4),
 		},
 		.num_channels = 5,
-		.int_vref_mv = 4096,
 		.info = &ad7991_info,
 	},
 	[ad7995] = {
 		.channel = {
-			[0] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 0,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 0,
-				.scan_type = IIO_ST('u', 10, 16, 2),
-			},
-			[1] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 1,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 1,
-				.scan_type = IIO_ST('u', 10, 16, 2),
-			},
-			[2] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 2,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 2,
-				.scan_type = IIO_ST('u', 10, 16, 2),
-			},
-			[3] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 3,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 3,
-				.scan_type = IIO_ST('u', 10, 16, 2),
-			},
-			[4] = IIO_CHAN_SOFT_TIMESTAMP(4),
+			AD799X_CHANNEL(0, 10, 0),
+			AD799X_CHANNEL(1, 10, 0),
+			AD799X_CHANNEL(2, 10, 0),
+			AD799X_CHANNEL(3, 10, 0),
+			IIO_CHAN_SOFT_TIMESTAMP(4),
 		},
 		.num_channels = 5,
-		.int_vref_mv = 1024,
 		.info = &ad7991_info,
 	},
 	[ad7999] = {
 		.channel = {
-			[0] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 0,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 0,
-				.scan_type = IIO_ST('u', 8, 16, 4),
-			},
-			[1] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 1,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 1,
-				.scan_type = IIO_ST('u', 8, 16, 4),
-			},
-			[2] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 2,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 2,
-				.scan_type = IIO_ST('u', 8, 16, 4),
-			},
-			[3] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 3,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 3,
-				.scan_type = IIO_ST('u', 8, 16, 4),
-			},
-			[4] = IIO_CHAN_SOFT_TIMESTAMP(4),
+			AD799X_CHANNEL(0, 8, 0),
+			AD799X_CHANNEL(1, 8, 0),
+			AD799X_CHANNEL(2, 8, 0),
+			AD799X_CHANNEL(3, 8, 0),
+			IIO_CHAN_SOFT_TIMESTAMP(4),
 		},
 		.num_channels = 5,
-		.int_vref_mv = 1024,
 		.info = &ad7991_info,
 	},
 	[ad7992] = {
 		.channel = {
-			[0] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 0,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 0,
-				.scan_type = IIO_ST('u', 12, 16, 0),
-				.event_mask = AD799X_EV_MASK,
-			},
-			[1] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 1,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 1,
-				.scan_type = IIO_ST('u', 12, 16, 0),
-				.event_mask = AD799X_EV_MASK,
-			},
-			[2] = IIO_CHAN_SOFT_TIMESTAMP(2),
+			AD799X_CHANNEL(0, 12, AD799X_EV_MASK),
+			AD799X_CHANNEL(1, 12, AD799X_EV_MASK),
+			IIO_CHAN_SOFT_TIMESTAMP(3),
 		},
 		.num_channels = 3,
-		.int_vref_mv = 4096,
 		.default_config = AD7998_ALERT_EN,
 		.info = &ad7992_info,
 	},
 	[ad7993] = {
 		.channel = {
-			[0] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 0,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 0,
-				.scan_type = IIO_ST('u', 10, 16, 2),
-				.event_mask = AD799X_EV_MASK,
-			},
-			[1] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 1,
-				.scan_index = 1,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_type = IIO_ST('u', 10, 16, 2),
-				.event_mask = AD799X_EV_MASK,
-			},
-			[2] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 2,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 2,
-				.scan_type = IIO_ST('u', 10, 16, 2),
-				.event_mask = AD799X_EV_MASK,
-			},
-			[3] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 3,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 3,
-				.scan_type = IIO_ST('u', 10, 16, 2),
-				.event_mask = AD799X_EV_MASK,
-			},
-			[4] = IIO_CHAN_SOFT_TIMESTAMP(4),
+			AD799X_CHANNEL(0, 10, AD799X_EV_MASK),
+			AD799X_CHANNEL(1, 10, AD799X_EV_MASK),
+			AD799X_CHANNEL(2, 10, AD799X_EV_MASK),
+			AD799X_CHANNEL(3, 10, AD799X_EV_MASK),
+			IIO_CHAN_SOFT_TIMESTAMP(4),
 		},
 		.num_channels = 5,
-		.int_vref_mv = 1024,
 		.default_config = AD7998_ALERT_EN,
 		.info = &ad7993_4_7_8_info,
 	},
 	[ad7994] = {
 		.channel = {
-			[0] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 0,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 0,
-				.scan_type = IIO_ST('u', 12, 16, 0),
-				.event_mask = AD799X_EV_MASK,
-			},
-			[1] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 1,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 1,
-				.scan_type = IIO_ST('u', 12, 16, 0),
-				.event_mask = AD799X_EV_MASK,
-			},
-			[2] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 2,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 2,
-				.scan_type = IIO_ST('u', 12, 16, 0),
-				.event_mask = AD799X_EV_MASK,
-			},
-			[3] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 3,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 3,
-				.scan_type = IIO_ST('u', 12, 16, 0),
-				.event_mask = AD799X_EV_MASK,
-			},
-			[4] = IIO_CHAN_SOFT_TIMESTAMP(4),
+			AD799X_CHANNEL(0, 12, AD799X_EV_MASK),
+			AD799X_CHANNEL(1, 12, AD799X_EV_MASK),
+			AD799X_CHANNEL(2, 12, AD799X_EV_MASK),
+			AD799X_CHANNEL(3, 12, AD799X_EV_MASK),
+			IIO_CHAN_SOFT_TIMESTAMP(4),
 		},
 		.num_channels = 5,
-		.int_vref_mv = 4096,
 		.default_config = AD7998_ALERT_EN,
 		.info = &ad7993_4_7_8_info,
 	},
 	[ad7997] = {
 		.channel = {
-			[0] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 0,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 0,
-				.scan_type = IIO_ST('u', 10, 16, 2),
-				.event_mask = AD799X_EV_MASK,
-			},
-			[1] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 1,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 1,
-				.scan_type = IIO_ST('u', 10, 16, 2),
-				.event_mask = AD799X_EV_MASK,
-			},
-			[2] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 2,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 2,
-				.scan_type = IIO_ST('u', 10, 16, 2),
-				.event_mask = AD799X_EV_MASK,
-			},
-			[3] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 3,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 3,
-				.scan_type = IIO_ST('u', 10, 16, 2),
-				.event_mask = AD799X_EV_MASK,
-			},
-			[4] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 4,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 4,
-				.scan_type = IIO_ST('u', 10, 16, 2),
-			},
-			[5] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 5,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 5,
-				.scan_type = IIO_ST('u', 10, 16, 2),
-			},
-			[6] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 6,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 6,
-				.scan_type = IIO_ST('u', 10, 16, 2),
-			},
-			[7] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 7,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 7,
-				.scan_type = IIO_ST('u', 10, 16, 2),
-			},
-			[8] = IIO_CHAN_SOFT_TIMESTAMP(8),
+			AD799X_CHANNEL(0, 10, AD799X_EV_MASK),
+			AD799X_CHANNEL(1, 10, AD799X_EV_MASK),
+			AD799X_CHANNEL(2, 10, AD799X_EV_MASK),
+			AD799X_CHANNEL(3, 10, AD799X_EV_MASK),
+			AD799X_CHANNEL(4, 10, 0),
+			AD799X_CHANNEL(5, 10, 0),
+			AD799X_CHANNEL(6, 10, 0),
+			AD799X_CHANNEL(7, 10, 0),
+			IIO_CHAN_SOFT_TIMESTAMP(8),
 		},
 		.num_channels = 9,
-		.int_vref_mv = 1024,
 		.default_config = AD7998_ALERT_EN,
 		.info = &ad7993_4_7_8_info,
 	},
 	[ad7998] = {
 		.channel = {
-			[0] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 0,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 0,
-				.scan_type = IIO_ST('u', 12, 16, 0),
-				.event_mask = AD799X_EV_MASK,
-			},
-			[1] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 1,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 1,
-				.scan_type = IIO_ST('u', 12, 16, 0),
-				.event_mask = AD799X_EV_MASK,
-			},
-			[2] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 2,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 2,
-				.scan_type = IIO_ST('u', 12, 16, 0),
-				.event_mask = AD799X_EV_MASK,
-			},
-			[3] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 3,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 3,
-				.scan_type = IIO_ST('u', 12, 16, 0),
-				.event_mask = AD799X_EV_MASK,
-			},
-			[4] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 4,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 4,
-				.scan_type = IIO_ST('u', 12, 16, 0),
-			},
-			[5] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 5,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 5,
-				.scan_type = IIO_ST('u', 12, 16, 0),
-			},
-			[6] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 6,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 6,
-				.scan_type = IIO_ST('u', 12, 16, 0),
-			},
-			[7] = {
-				.type = IIO_VOLTAGE,
-				.indexed = 1,
-				.channel = 7,
-				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
-				.scan_index = 7,
-				.scan_type = IIO_ST('u', 12, 16, 0),
-			},
-			[8] = IIO_CHAN_SOFT_TIMESTAMP(8),
+			AD799X_CHANNEL(0, 12, AD799X_EV_MASK),
+			AD799X_CHANNEL(1, 12, AD799X_EV_MASK),
+			AD799X_CHANNEL(2, 12, AD799X_EV_MASK),
+			AD799X_CHANNEL(3, 12, AD799X_EV_MASK),
+			AD799X_CHANNEL(4, 12, 0),
+			AD799X_CHANNEL(5, 12, 0),
+			AD799X_CHANNEL(6, 12, 0),
+			AD799X_CHANNEL(7, 12, 0),
+			IIO_CHAN_SOFT_TIMESTAMP(8),
 		},
 		.num_channels = 9,
-		.int_vref_mv = 4096,
 		.default_config = AD7998_ALERT_EN,
 		.info = &ad7993_4_7_8_info,
 	},
@@ -875,10 +601,10 @@ static int ad799x_probe(struct i2c_client *client,
 
 	/* TODO: Add pdata options for filtering and bit delay */
 
-	if (pdata)
-		st->int_vref_mv = pdata->vref_mv;
-	else
-		st->int_vref_mv = st->chip_info->int_vref_mv;
+	if (!pdata)
+		return -EINVAL;
+
+	st->int_vref_mv = pdata->vref_mv;
 
 	st->reg = regulator_get(&client->dev, "vcc");
 	if (!IS_ERR(st->reg)) {
@@ -946,6 +672,7 @@ static int ad799x_remove(struct i2c_client *client)
 		regulator_disable(st->reg);
 		regulator_put(st->reg);
 	}
+	kfree(st->rx_buf);
 	iio_device_free(indio_dev);
 
 	return 0;
diff --git a/drivers/staging/iio/adc/ad799x_ring.c b/drivers/staging/iio/adc/ad799x_ring.c
index 2c5f38475a8..c2ebae12ee1 100644
--- a/drivers/staging/iio/adc/ad799x_ring.c
+++ b/drivers/staging/iio/adc/ad799x_ring.c
@@ -36,14 +36,9 @@ static irqreturn_t ad799x_trigger_handler(int irq, void *p)
 	struct iio_dev *indio_dev = pf->indio_dev;
 	struct ad799x_state *st = iio_priv(indio_dev);
 	s64 time_ns;
-	__u8 *rxbuf;
 	int b_sent;
 	u8 cmd;
 
-	rxbuf = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
-	if (rxbuf == NULL)
-		goto out;
-
 	switch (st->id) {
 	case ad7991:
 	case ad7995:
@@ -66,20 +61,17 @@ static irqreturn_t ad799x_trigger_handler(int irq, void *p)
 	}
 
 	b_sent = i2c_smbus_read_i2c_block_data(st->client,
-			cmd, bitmap_weight(indio_dev->active_scan_mask,
-					   indio_dev->masklength) * 2, rxbuf);
+			cmd, st->transfer_size, st->rx_buf);
 	if (b_sent < 0)
-		goto done;
+		goto out;
 
 	time_ns = iio_get_time_ns();
 
 	if (indio_dev->scan_timestamp)
-		memcpy(rxbuf + indio_dev->scan_bytes - sizeof(s64),
+		memcpy(st->rx_buf + indio_dev->scan_bytes - sizeof(s64),
 			&time_ns, sizeof(time_ns));
 
-	iio_push_to_buffers(indio_dev, rxbuf);
-done:
-	kfree(rxbuf);
+	iio_push_to_buffers(indio_dev, st->rx_buf);
 out:
 	iio_trigger_notify_done(indio_dev->trig);
 
diff --git a/drivers/staging/iio/adc/adt7410.c b/drivers/staging/iio/adc/adt7410.c
deleted file mode 100644
index 35455e16094..00000000000
--- a/drivers/staging/iio/adc/adt7410.c
+++ /dev/null
@@ -1,1102 +0,0 @@
-/*
- * ADT7410 digital temperature sensor driver supporting ADT7310/ADT7410
- *
- * Copyright 2010 Analog Devices Inc.
- *
- * Licensed under the GPL-2 or later.
- */
-
-#include <linux/interrupt.h>
-#include <linux/device.h>
-#include <linux/kernel.h>
-#include <linux/slab.h>
-#include <linux/sysfs.h>
-#include <linux/list.h>
-#include <linux/i2c.h>
-#include <linux/spi/spi.h>
-#include <linux/module.h>
-
-#include <linux/iio/iio.h>
-#include <linux/iio/sysfs.h>
-#include <linux/iio/events.h>
-
-/*
- * ADT7410 registers definition
- */
-
-#define ADT7410_TEMPERATURE		0
-#define ADT7410_STATUS			2
-#define ADT7410_CONFIG			3
-#define ADT7410_T_ALARM_HIGH		4
-#define ADT7410_T_ALARM_LOW		6
-#define ADT7410_T_CRIT			8
-#define ADT7410_T_HYST			0xA
-#define ADT7410_ID			0xB
-#define ADT7410_RESET			0x2F
-
-/*
- * ADT7310 registers definition
- */
-
-#define ADT7310_STATUS			0
-#define ADT7310_CONFIG			1
-#define ADT7310_TEMPERATURE		2
-#define ADT7310_ID			3
-#define ADT7310_T_CRIT			4
-#define ADT7310_T_HYST			5
-#define ADT7310_T_ALARM_HIGH		6
-#define ADT7310_T_ALARM_LOW		7
-
-/*
- * ADT7410 status
- */
-#define ADT7410_STAT_T_LOW		0x10
-#define ADT7410_STAT_T_HIGH		0x20
-#define ADT7410_STAT_T_CRIT		0x40
-#define ADT7410_STAT_NOT_RDY		0x80
-
-/*
- * ADT7410 config
- */
-#define ADT7410_FAULT_QUEUE_MASK	0x3
-#define ADT7410_CT_POLARITY		0x4
-#define ADT7410_INT_POLARITY		0x8
-#define ADT7410_EVENT_MODE		0x10
-#define ADT7410_MODE_MASK		0x60
-#define ADT7410_ONESHOT			0x20
-#define ADT7410_SPS			0x40
-#define ADT7410_PD			0x60
-#define ADT7410_RESOLUTION		0x80
-
-/*
- * ADT7410 masks
- */
-#define ADT7410_T16_VALUE_SIGN			0x8000
-#define ADT7410_T16_VALUE_FLOAT_OFFSET		7
-#define ADT7410_T16_VALUE_FLOAT_MASK		0x7F
-#define ADT7410_T13_VALUE_SIGN			0x1000
-#define ADT7410_T13_VALUE_OFFSET		3
-#define ADT7410_T13_VALUE_FLOAT_OFFSET		4
-#define ADT7410_T13_VALUE_FLOAT_MASK		0xF
-#define ADT7410_T_HYST_MASK			0xF
-#define ADT7410_DEVICE_ID_MASK			0xF
-#define ADT7410_MANUFACTORY_ID_MASK		0xF0
-#define ADT7410_MANUFACTORY_ID_OFFSET		4
-
-
-#define ADT7310_CMD_REG_MASK			0x28
-#define ADT7310_CMD_REG_OFFSET			3
-#define ADT7310_CMD_READ			0x40
-#define ADT7310_CMD_CON_READ			0x4
-
-#define ADT7410_IRQS				2
-
-/*
- * struct adt7410_chip_info - chip specifc information
- */
-
-struct adt7410_chip_info;
-
-struct adt7410_ops {
-	int (*read_word)(struct adt7410_chip_info *, u8 reg, u16 *data);
-	int (*write_word)(struct adt7410_chip_info *, u8 reg, u16 data);
-	int (*read_byte)(struct adt7410_chip_info *, u8 reg, u8 *data);
-	int (*write_byte)(struct adt7410_chip_info *, u8 reg, u8 data);
-};
-
-struct adt7410_chip_info {
-	struct device *dev;
-	u8  config;
-
-	const struct adt7410_ops *ops;
-};
-
-static int adt7410_read_word(struct adt7410_chip_info *chip, u8 reg, u16 *data)
-{
-	return chip->ops->read_word(chip, reg, data);
-}
-
-static int adt7410_write_word(struct adt7410_chip_info *chip, u8 reg, u16 data)
-{
-	return chip->ops->write_word(chip, reg, data);
-}
-
-static int adt7410_read_byte(struct adt7410_chip_info *chip, u8 reg, u8 *data)
-{
-	return chip->ops->read_byte(chip, reg, data);
-}
-
-static int adt7410_write_byte(struct adt7410_chip_info *chip, u8 reg, u8 data)
-{
-	return chip->ops->write_byte(chip, reg, data);
-}
-
-static ssize_t adt7410_show_mode(struct device *dev,
-		struct device_attribute *attr,
-		char *buf)
-{
-	struct iio_dev *dev_info = dev_to_iio_dev(dev);
-	struct adt7410_chip_info *chip = iio_priv(dev_info);
-	u8 config;
-
-	config = chip->config & ADT7410_MODE_MASK;
-
-	switch (config) {
-	case ADT7410_PD:
-		return sprintf(buf, "power-down\n");
-	case ADT7410_ONESHOT:
-		return sprintf(buf, "one-shot\n");
-	case ADT7410_SPS:
-		return sprintf(buf, "sps\n");
-	default:
-		return sprintf(buf, "full\n");
-	}
-}
-
-static ssize_t adt7410_store_mode(struct device *dev,
-		struct device_attribute *attr,
-		const char *buf,
-		size_t len)
-{
-	struct iio_dev *dev_info = dev_to_iio_dev(dev);
-	struct adt7410_chip_info *chip = iio_priv(dev_info);
-	u16 config;
-	int ret;
-
-	ret = adt7410_read_byte(chip, ADT7410_CONFIG, &chip->config);
-	if (ret)
-		return -EIO;
-
-	config = chip->config & (~ADT7410_MODE_MASK);
-	if (strcmp(buf, "power-down"))
-		config |= ADT7410_PD;
-	else if (strcmp(buf, "one-shot"))
-		config |= ADT7410_ONESHOT;
-	else if (strcmp(buf, "sps"))
-		config |= ADT7410_SPS;
-
-	ret = adt7410_write_byte(chip, ADT7410_CONFIG, config);
-	if (ret)
-		return -EIO;
-
-	chip->config = config;
-
-	return len;
-}
-
-static IIO_DEVICE_ATTR(mode, S_IRUGO | S_IWUSR,
-		adt7410_show_mode,
-		adt7410_store_mode,
-		0);
-
-static ssize_t adt7410_show_available_modes(struct device *dev,
-		struct device_attribute *attr,
-		char *buf)
-{
-	return sprintf(buf, "full\none-shot\nsps\npower-down\n");
-}
-
-static IIO_DEVICE_ATTR(available_modes, S_IRUGO, adt7410_show_available_modes, NULL, 0);
-
-static ssize_t adt7410_show_resolution(struct device *dev,
-		struct device_attribute *attr,
-		char *buf)
-{
-	struct iio_dev *dev_info = dev_to_iio_dev(dev);
-	struct adt7410_chip_info *chip = iio_priv(dev_info);
-	int ret;
-	int bits;
-
-	ret = adt7410_read_byte(chip, ADT7410_CONFIG, &chip->config);
-	if (ret)
-		return -EIO;
-
-	if (chip->config & ADT7410_RESOLUTION)
-		bits = 16;
-	else
-		bits = 13;
-
-	return sprintf(buf, "%d bits\n", bits);
-}
-
-static ssize_t adt7410_store_resolution(struct device *dev,
-		struct device_attribute *attr,
-		const char *buf,
-		size_t len)
-{
-	struct iio_dev *dev_info = dev_to_iio_dev(dev);
-	struct adt7410_chip_info *chip = iio_priv(dev_info);
-	unsigned long data;
-	u16 config;
-	int ret;
-
-	ret = strict_strtoul(buf, 10, &data);
-	if (ret)
-		return -EINVAL;
-
-	ret = adt7410_read_byte(chip, ADT7410_CONFIG, &chip->config);
-	if (ret)
-		return -EIO;
-
-	config = chip->config & (~ADT7410_RESOLUTION);
-	if (data)
-		config |= ADT7410_RESOLUTION;
-
-	ret = adt7410_write_byte(chip, ADT7410_CONFIG, config);
-	if (ret)
-		return -EIO;
-
-	chip->config = config;
-
-	return len;
-}
-
-static IIO_DEVICE_ATTR(resolution, S_IRUGO | S_IWUSR,
-		adt7410_show_resolution,
-		adt7410_store_resolution,
-		0);
-
-static ssize_t adt7410_show_id(struct device *dev,
-		struct device_attribute *attr,
-		char *buf)
-{
-	struct iio_dev *dev_info = dev_to_iio_dev(dev);
-	struct adt7410_chip_info *chip = iio_priv(dev_info);
-	u8 id;
-	int ret;
-
-	ret = adt7410_read_byte(chip, ADT7410_ID, &id);
-	if (ret)
-		return -EIO;
-
-	return sprintf(buf, "device id: 0x%x\nmanufactory id: 0x%x\n",
-			id & ADT7410_DEVICE_ID_MASK,
-			(id & ADT7410_MANUFACTORY_ID_MASK) >> ADT7410_MANUFACTORY_ID_OFFSET);
-}
-
-static IIO_DEVICE_ATTR(id, S_IRUGO | S_IWUSR,
-		adt7410_show_id,
-		NULL,
-		0);
-
-static ssize_t adt7410_convert_temperature(struct adt7410_chip_info *chip,
-		u16 data, char *buf)
-{
-	char sign = ' ';
-
-	if (!(chip->config & ADT7410_RESOLUTION))
-		data &= ~0x7;
-
-	if (data & ADT7410_T16_VALUE_SIGN) {
-		/* convert supplement to positive value */
-		data = (u16)((ADT7410_T16_VALUE_SIGN << 1) - (u32)data);
-		sign = '-';
-	}
-	return sprintf(buf, "%c%d.%.7d\n", sign,
-			(data >> ADT7410_T16_VALUE_FLOAT_OFFSET),
-			(data & ADT7410_T16_VALUE_FLOAT_MASK) * 78125);
-}
-
-static ssize_t adt7410_show_value(struct device *dev,
-		struct device_attribute *attr,
-		char *buf)
-{
-	struct iio_dev *dev_info = dev_to_iio_dev(dev);
-	struct adt7410_chip_info *chip = iio_priv(dev_info);
-	u8 status;
-	u16 data;
-	int ret, i = 0;
-
-	do {
-		ret = adt7410_read_byte(chip, ADT7410_STATUS, &status);
-		if (ret)
-			return -EIO;
-		i++;
-		if (i == 10000)
-			return -EIO;
-	} while (status & ADT7410_STAT_NOT_RDY);
-
-	ret = adt7410_read_word(chip, ADT7410_TEMPERATURE, &data);
-	if (ret)
-		return -EIO;
-
-	return adt7410_convert_temperature(chip, data, buf);
-}
-
-static IIO_DEVICE_ATTR(value, S_IRUGO, adt7410_show_value, NULL, 0);
-
-static struct attribute *adt7410_attributes[] = {
-	&iio_dev_attr_available_modes.dev_attr.attr,
-	&iio_dev_attr_mode.dev_attr.attr,
-	&iio_dev_attr_resolution.dev_attr.attr,
-	&iio_dev_attr_id.dev_attr.attr,
-	&iio_dev_attr_value.dev_attr.attr,
-	NULL,
-};
-
-static const struct attribute_group adt7410_attribute_group = {
-	.attrs = adt7410_attributes,
-};
-
-static irqreturn_t adt7410_event_handler(int irq, void *private)
-{
-	struct iio_dev *indio_dev = private;
-	struct adt7410_chip_info *chip = iio_priv(indio_dev);
-	s64 timestamp = iio_get_time_ns();
-	u8 status;
-
-	if (adt7410_read_byte(chip, ADT7410_STATUS, &status))
-		return IRQ_HANDLED;
-
-	if (status & ADT7410_STAT_T_HIGH)
-		iio_push_event(indio_dev,
-			       IIO_UNMOD_EVENT_CODE(IIO_TEMP, 0,
-						    IIO_EV_TYPE_THRESH,
-						    IIO_EV_DIR_RISING),
-			       timestamp);
-	if (status & ADT7410_STAT_T_LOW)
-		iio_push_event(indio_dev,
-			       IIO_UNMOD_EVENT_CODE(IIO_TEMP, 0,
-						    IIO_EV_TYPE_THRESH,
-						    IIO_EV_DIR_FALLING),
-			       timestamp);
-	if (status & ADT7410_STAT_T_CRIT)
-		iio_push_event(indio_dev,
-			       IIO_UNMOD_EVENT_CODE(IIO_TEMP, 0,
-						    IIO_EV_TYPE_THRESH,
-						    IIO_EV_DIR_RISING),
-			       timestamp);
-
-	return IRQ_HANDLED;
-}
-
-static ssize_t adt7410_show_event_mode(struct device *dev,
-		struct device_attribute *attr,
-		char *buf)
-{
-	struct iio_dev *dev_info = dev_to_iio_dev(dev);
-	struct adt7410_chip_info *chip = iio_priv(dev_info);
-	int ret;
-
-	ret = adt7410_read_byte(chip, ADT7410_CONFIG, &chip->config);
-	if (ret)
-		return -EIO;
-
-	if (chip->config & ADT7410_EVENT_MODE)
-		return sprintf(buf, "interrupt\n");
-	else
-		return sprintf(buf, "comparator\n");
-}
-
-static ssize_t adt7410_set_event_mode(struct device *dev,
-		struct device_attribute *attr,
-		const char *buf,
-		size_t len)
-{
-	struct iio_dev *dev_info = dev_to_iio_dev(dev);
-	struct adt7410_chip_info *chip = iio_priv(dev_info);
-	u16 config;
-	int ret;
-
-	ret = adt7410_read_byte(chip, ADT7410_CONFIG, &chip->config);
-	if (ret)
-		return -EIO;
-
-	config = chip->config &= ~ADT7410_EVENT_MODE;
-	if (strcmp(buf, "comparator") != 0)
-		config |= ADT7410_EVENT_MODE;
-
-	ret = adt7410_write_byte(chip, ADT7410_CONFIG, config);
-	if (ret)
-		return -EIO;
-
-	chip->config = config;
-
-	return ret;
-}
-
-static ssize_t adt7410_show_available_event_modes(struct device *dev,
-		struct device_attribute *attr,
-		char *buf)
-{
-	return sprintf(buf, "comparator\ninterrupt\n");
-}
-
-static ssize_t adt7410_show_fault_queue(struct device *dev,
-		struct device_attribute *attr,
-		char *buf)
-{
-	struct iio_dev *dev_info = dev_to_iio_dev(dev);
-	struct adt7410_chip_info *chip = iio_priv(dev_info);
-	int ret;
-
-	ret = adt7410_read_byte(chip, ADT7410_CONFIG, &chip->config);
-	if (ret)
-		return -EIO;
-
-	return sprintf(buf, "%d\n", chip->config & ADT7410_FAULT_QUEUE_MASK);
-}
-
-static ssize_t adt7410_set_fault_queue(struct device *dev,
-		struct device_attribute *attr,
-		const char *buf,
-		size_t len)
-{
-	struct iio_dev *dev_info = dev_to_iio_dev(dev);
-	struct adt7410_chip_info *chip = iio_priv(dev_info);
-	unsigned long data;
-	int ret;
-	u8 config;
-
-	ret = strict_strtoul(buf, 10, &data);
-	if (ret || data > 3)
-		return -EINVAL;
-
-	ret = adt7410_read_byte(chip, ADT7410_CONFIG, &chip->config);
-	if (ret)
-		return -EIO;
-
-	config = chip->config & ~ADT7410_FAULT_QUEUE_MASK;
-	config |= data;
-	ret = adt7410_write_byte(chip, ADT7410_CONFIG, config);
-	if (ret)
-		return -EIO;
-
-	chip->config = config;
-
-	return ret;
-}
-
-static inline ssize_t adt7410_show_t_bound(struct device *dev,
-		struct device_attribute *attr,
-		u8 bound_reg,
-		char *buf)
-{
-	struct iio_dev *dev_info = dev_to_iio_dev(dev);
-	struct adt7410_chip_info *chip = iio_priv(dev_info);
-	u16 data;
-	int ret;
-
-	ret = adt7410_read_word(chip, bound_reg, &data);
-	if (ret)
-		return -EIO;
-
-	return adt7410_convert_temperature(chip, data, buf);
-}
-
-static inline ssize_t adt7410_set_t_bound(struct device *dev,
-		struct device_attribute *attr,
-		u8 bound_reg,
-		const char *buf,
-		size_t len)
-{
-	struct iio_dev *dev_info = dev_to_iio_dev(dev);
-	struct adt7410_chip_info *chip = iio_priv(dev_info);
-	long tmp1, tmp2;
-	u16 data;
-	char *pos;
-	int ret;
-
-	pos = strchr(buf, '.');
-
-	ret = strict_strtol(buf, 10, &tmp1);
-
-	if (ret || tmp1 > 127 || tmp1 < -128)
-		return -EINVAL;
-
-	if (pos) {
-		len = strlen(pos);
-
-		if (chip->config & ADT7410_RESOLUTION) {
-			if (len > ADT7410_T16_VALUE_FLOAT_OFFSET)
-				len = ADT7410_T16_VALUE_FLOAT_OFFSET;
-			pos[len] = 0;
-			ret = strict_strtol(pos, 10, &tmp2);
-
-			if (!ret)
-				tmp2 = (tmp2 / 78125) * 78125;
-		} else {
-			if (len > ADT7410_T13_VALUE_FLOAT_OFFSET)
-				len = ADT7410_T13_VALUE_FLOAT_OFFSET;
-			pos[len] = 0;
-			ret = strict_strtol(pos, 10, &tmp2);
-
-			if (!ret)
-				tmp2 = (tmp2 / 625) * 625;
-		}
-	}
-
-	if (tmp1 < 0)
-		data = (u16)(-tmp1);
-	else
-		data = (u16)tmp1;
-
-	if (chip->config & ADT7410_RESOLUTION) {
-		data = (data << ADT7410_T16_VALUE_FLOAT_OFFSET) |
-			(tmp2 & ADT7410_T16_VALUE_FLOAT_MASK);
-
-		if (tmp1 < 0)
-			/* convert positive value to supplyment */
-			data = (u16)((ADT7410_T16_VALUE_SIGN << 1) - (u32)data);
-	} else {
-		data = (data << ADT7410_T13_VALUE_FLOAT_OFFSET) |
-			(tmp2 & ADT7410_T13_VALUE_FLOAT_MASK);
-
-		if (tmp1 < 0)
-			/* convert positive value to supplyment */
-			data = (ADT7410_T13_VALUE_SIGN << 1) - data;
-		data <<= ADT7410_T13_VALUE_OFFSET;
-	}
-
-	ret = adt7410_write_word(chip, bound_reg, data);
-	if (ret)
-		return -EIO;
-
-	return ret;
-}
-
-static ssize_t adt7410_show_t_alarm_high(struct device *dev,
-		struct device_attribute *attr,
-		char *buf)
-{
-	return adt7410_show_t_bound(dev, attr,
-			ADT7410_T_ALARM_HIGH, buf);
-}
-
-static inline ssize_t adt7410_set_t_alarm_high(struct device *dev,
-		struct device_attribute *attr,
-		const char *buf,
-		size_t len)
-{
-	return adt7410_set_t_bound(dev, attr,
-			ADT7410_T_ALARM_HIGH, buf, len);
-}
-
-static ssize_t adt7410_show_t_alarm_low(struct device *dev,
-		struct device_attribute *attr,
-		char *buf)
-{
-	return adt7410_show_t_bound(dev, attr,
-			ADT7410_T_ALARM_LOW, buf);
-}
-
-static inline ssize_t adt7410_set_t_alarm_low(struct device *dev,
-		struct device_attribute *attr,
-		const char *buf,
-		size_t len)
-{
-	return adt7410_set_t_bound(dev, attr,
-			ADT7410_T_ALARM_LOW, buf, len);
-}
-
-static ssize_t adt7410_show_t_crit(struct device *dev,
-		struct device_attribute *attr,
-		char *buf)
-{
-	return adt7410_show_t_bound(dev, attr,
-			ADT7410_T_CRIT, buf);
-}
-
-static inline ssize_t adt7410_set_t_crit(struct device *dev,
-		struct device_attribute *attr,
-		const char *buf,
-		size_t len)
-{
-	return adt7410_set_t_bound(dev, attr,
-			ADT7410_T_CRIT, buf, len);
-}
-
-static ssize_t adt7410_show_t_hyst(struct device *dev,
-		struct device_attribute *attr,
-		char *buf)
-{
-	struct iio_dev *dev_info = dev_to_iio_dev(dev);
-	struct adt7410_chip_info *chip = iio_priv(dev_info);
-	int ret;
-	u8 t_hyst;
-
-	ret = adt7410_read_byte(chip, ADT7410_T_HYST, &t_hyst);
-	if (ret)
-		return -EIO;
-
-	return sprintf(buf, "%d\n", t_hyst & ADT7410_T_HYST_MASK);
-}
-
-static inline ssize_t adt7410_set_t_hyst(struct device *dev,
-		struct device_attribute *attr,
-		const char *buf,
-		size_t len)
-{
-	struct iio_dev *dev_info = dev_to_iio_dev(dev);
-	struct adt7410_chip_info *chip = iio_priv(dev_info);
-	int ret;
-	unsigned long data;
-	u8 t_hyst;
-
-	ret = strict_strtol(buf, 10, &data);
-
-	if (ret || data > ADT7410_T_HYST_MASK)
-		return -EINVAL;
-
-	t_hyst = (u8)data;
-
-	ret = adt7410_write_byte(chip, ADT7410_T_HYST, t_hyst);
-	if (ret)
-		return -EIO;
-
-	return ret;
-}
-
-static IIO_DEVICE_ATTR(event_mode,
-		       S_IRUGO | S_IWUSR,
-		       adt7410_show_event_mode, adt7410_set_event_mode, 0);
-static IIO_DEVICE_ATTR(available_event_modes,
-		       S_IRUGO,
-		       adt7410_show_available_event_modes, NULL, 0);
-static IIO_DEVICE_ATTR(fault_queue,
-		       S_IRUGO | S_IWUSR,
-		       adt7410_show_fault_queue, adt7410_set_fault_queue, 0);
-static IIO_DEVICE_ATTR(t_alarm_high,
-		       S_IRUGO | S_IWUSR,
-		       adt7410_show_t_alarm_high, adt7410_set_t_alarm_high, 0);
-static IIO_DEVICE_ATTR(t_alarm_low,
-		       S_IRUGO | S_IWUSR,
-		       adt7410_show_t_alarm_low, adt7410_set_t_alarm_low, 0);
-static IIO_DEVICE_ATTR(t_crit,
-		       S_IRUGO | S_IWUSR,
-		       adt7410_show_t_crit, adt7410_set_t_crit, 0);
-static IIO_DEVICE_ATTR(t_hyst,
-		       S_IRUGO | S_IWUSR,
-		       adt7410_show_t_hyst, adt7410_set_t_hyst, 0);
-
-static struct attribute *adt7410_event_int_attributes[] = {
-	&iio_dev_attr_event_mode.dev_attr.attr,
-	&iio_dev_attr_available_event_modes.dev_attr.attr,
-	&iio_dev_attr_fault_queue.dev_attr.attr,
-	&iio_dev_attr_t_alarm_high.dev_attr.attr,
-	&iio_dev_attr_t_alarm_low.dev_attr.attr,
-	&iio_dev_attr_t_crit.dev_attr.attr,
-	&iio_dev_attr_t_hyst.dev_attr.attr,
-	NULL,
-};
-
-static struct attribute_group adt7410_event_attribute_group = {
-	.attrs = adt7410_event_int_attributes,
-	.name = "events",
-};
-
-static const struct iio_info adt7410_info = {
-	.attrs = &adt7410_attribute_group,
-	.event_attrs = &adt7410_event_attribute_group,
-	.driver_module = THIS_MODULE,
-};
-
-/*
- * device probe and remove
- */
-
-static int adt7410_probe(struct device *dev, int irq,
-	const char *name, const struct adt7410_ops *ops)
-{
-	unsigned long *adt7410_platform_data = dev->platform_data;
-	unsigned long local_pdata[] = {0, 0};
-	struct adt7410_chip_info *chip;
-	struct iio_dev *indio_dev;
-	int ret = 0;
-
-	indio_dev = iio_device_alloc(sizeof(*chip));
-	if (indio_dev == NULL) {
-		ret = -ENOMEM;
-		goto error_ret;
-	}
-	chip = iio_priv(indio_dev);
-	/* this is only used for device removal purposes */
-	dev_set_drvdata(dev, indio_dev);
-
-	chip->dev = dev;
-	chip->ops = ops;
-
-	indio_dev->name = name;
-	indio_dev->dev.parent = dev;
-	indio_dev->info = &adt7410_info;
-	indio_dev->modes = INDIO_DIRECT_MODE;
-
-	if (!adt7410_platform_data)
-		adt7410_platform_data = local_pdata;
-
-	/* CT critcal temperature event. line 0 */
-	if (irq) {
-		ret = request_threaded_irq(irq,
-					   NULL,
-					   &adt7410_event_handler,
-					   IRQF_TRIGGER_LOW | IRQF_ONESHOT,
-					   name,
-					   indio_dev);
-		if (ret)
-			goto error_free_dev;
-	}
-
-	/* INT bound temperature alarm event. line 1 */
-	if (adt7410_platform_data[0]) {
-		ret = request_threaded_irq(adt7410_platform_data[0],
-					   NULL,
-					   &adt7410_event_handler,
-					   adt7410_platform_data[1] |
-					   IRQF_ONESHOT,
-					   name,
-					   indio_dev);
-		if (ret)
-			goto error_unreg_ct_irq;
-	}
-
-	ret = adt7410_read_byte(chip, ADT7410_CONFIG, &chip->config);
-	if (ret) {
-		ret = -EIO;
-		goto error_unreg_int_irq;
-	}
-
-	chip->config |= ADT7410_RESOLUTION;
-
-	if (irq && adt7410_platform_data[0]) {
-
-		/* set irq polarity low level */
-		chip->config &= ~ADT7410_CT_POLARITY;
-
-		if (adt7410_platform_data[1] & IRQF_TRIGGER_HIGH)
-			chip->config |= ADT7410_INT_POLARITY;
-		else
-			chip->config &= ~ADT7410_INT_POLARITY;
-	}
-
-	ret = adt7410_write_byte(chip, ADT7410_CONFIG, chip->config);
-	if (ret) {
-		ret = -EIO;
-		goto error_unreg_int_irq;
-	}
-	ret = iio_device_register(indio_dev);
-	if (ret)
-		goto error_unreg_int_irq;
-
-	dev_info(dev, "%s temperature sensor registered.\n",
-			 name);
-
-	return 0;
-
-error_unreg_int_irq:
-	free_irq(adt7410_platform_data[0], indio_dev);
-error_unreg_ct_irq:
-	free_irq(irq, indio_dev);
-error_free_dev:
-	iio_device_free(indio_dev);
-error_ret:
-	return ret;
-}
-
-static int adt7410_remove(struct device *dev, int irq)
-{
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
-	unsigned long *adt7410_platform_data = dev->platform_data;
-
-	iio_device_unregister(indio_dev);
-	if (adt7410_platform_data[0])
-		free_irq(adt7410_platform_data[0], indio_dev);
-	if (irq)
-		free_irq(irq, indio_dev);
-	iio_device_free(indio_dev);
-
-	return 0;
-}
-
-#if IS_ENABLED(CONFIG_I2C)
-
-static int adt7410_i2c_read_word(struct adt7410_chip_info *chip, u8 reg,
-	u16 *data)
-{
-	struct i2c_client *client = to_i2c_client(chip->dev);
-	int ret = 0;
-
-	ret = i2c_smbus_read_word_data(client, reg);
-	if (ret < 0) {
-		dev_err(&client->dev, "I2C read error\n");
-		return ret;
-	}
-
-	*data = swab16((u16)ret);
-
-	return 0;
-}
-
-static int adt7410_i2c_write_word(struct adt7410_chip_info *chip, u8 reg,
-	u16 data)
-{
-	struct i2c_client *client = to_i2c_client(chip->dev);
-	int ret = 0;
-
-	ret = i2c_smbus_write_word_data(client, reg, swab16(data));
-	if (ret < 0)
-		dev_err(&client->dev, "I2C write error\n");
-
-	return ret;
-}
-
-static int adt7410_i2c_read_byte(struct adt7410_chip_info *chip, u8 reg,
-	u8 *data)
-{
-	struct i2c_client *client = to_i2c_client(chip->dev);
-	int ret = 0;
-
-	ret = i2c_smbus_read_byte_data(client, reg);
-	if (ret < 0) {
-		dev_err(&client->dev, "I2C read error\n");
-		return ret;
-	}
-
-	*data = (u8)ret;
-
-	return 0;
-}
-
-static int adt7410_i2c_write_byte(struct adt7410_chip_info *chip, u8 reg,
-	u8 data)
-{
-	struct i2c_client *client = to_i2c_client(chip->dev);
-	int ret = 0;
-
-	ret = i2c_smbus_write_byte_data(client, reg, data);
-	if (ret < 0)
-		dev_err(&client->dev, "I2C write error\n");
-
-	return ret;
-}
-
-static const struct adt7410_ops adt7410_i2c_ops = {
-	.read_word = adt7410_i2c_read_word,
-	.write_word = adt7410_i2c_write_word,
-	.read_byte = adt7410_i2c_read_byte,
-	.write_byte = adt7410_i2c_write_byte,
-};
-
-static int adt7410_i2c_probe(struct i2c_client *client,
-	const struct i2c_device_id *id)
-{
-	return adt7410_probe(&client->dev, client->irq, id->name,
-		&adt7410_i2c_ops);
-}
-
-static int adt7410_i2c_remove(struct i2c_client *client)
-{
-	return adt7410_remove(&client->dev, client->irq);
-}
-
-static const struct i2c_device_id adt7410_id[] = {
-	{ "adt7410", 0 },
-	{}
-};
-
-MODULE_DEVICE_TABLE(i2c, adt7410_id);
-
-static struct i2c_driver adt7410_driver = {
-	.driver = {
-		.name = "adt7410",
-	},
-	.probe = adt7410_i2c_probe,
-	.remove = adt7410_i2c_remove,
-	.id_table = adt7410_id,
-};
-
-static int __init adt7410_i2c_init(void)
-{
-	return i2c_add_driver(&adt7410_driver);
-}
-
-static void __exit adt7410_i2c_exit(void)
-{
-	i2c_del_driver(&adt7410_driver);
-}
-
-#else
-
-static int  __init adt7410_i2c_init(void) { return 0; };
-static void __exit adt7410_i2c_exit(void) {};
-
-#endif
-
-#if IS_ENABLED(CONFIG_SPI_MASTER)
-
-static const u8 adt7371_reg_table[] = {
-	[ADT7410_TEMPERATURE]   = ADT7310_TEMPERATURE,
-	[ADT7410_STATUS]	= ADT7310_STATUS,
-	[ADT7410_CONFIG]	= ADT7310_CONFIG,
-	[ADT7410_T_ALARM_HIGH]	= ADT7310_T_ALARM_HIGH,
-	[ADT7410_T_ALARM_LOW]	= ADT7310_T_ALARM_LOW,
-	[ADT7410_T_CRIT]	= ADT7310_T_CRIT,
-	[ADT7410_T_HYST]	= ADT7310_T_HYST,
-	[ADT7410_ID]		= ADT7310_ID,
-};
-
-#define AD7310_COMMAND(reg) (adt7371_reg_table[(reg)] << ADT7310_CMD_REG_OFFSET)
-
-static int adt7310_spi_read_word(struct adt7410_chip_info *chip,
-	u8 reg, u16 *data)
-{
-	struct spi_device *spi = to_spi_device(chip->dev);
-	u8 command = AD7310_COMMAND(reg);
-	int ret = 0;
-
-	command |= ADT7310_CMD_READ;
-	ret = spi_write(spi, &command, sizeof(command));
-	if (ret < 0) {
-		dev_err(&spi->dev, "SPI write command error\n");
-		return ret;
-	}
-
-	ret = spi_read(spi, (u8 *)data, sizeof(*data));
-	if (ret < 0) {
-		dev_err(&spi->dev, "SPI read word error\n");
-		return ret;
-	}
-
-	*data = be16_to_cpu(*data);
-
-	return 0;
-}
-
-static int adt7310_spi_write_word(struct adt7410_chip_info *chip, u8 reg,
-	u16 data)
-{
-	struct spi_device *spi = to_spi_device(chip->dev);
-	u8 buf[3];
-	int ret = 0;
-
-	buf[0] = AD7310_COMMAND(reg);
-	buf[1] = (u8)(data >> 8);
-	buf[2] = (u8)(data & 0xFF);
-
-	ret = spi_write(spi, buf, 3);
-	if (ret < 0) {
-		dev_err(&spi->dev, "SPI write word error\n");
-		return ret;
-	}
-
-	return ret;
-}
-
-static int adt7310_spi_read_byte(struct adt7410_chip_info *chip, u8 reg,
-	u8 *data)
-{
-	struct spi_device *spi = to_spi_device(chip->dev);
-	u8 command = AD7310_COMMAND(reg);
-	int ret = 0;
-
-	command |= ADT7310_CMD_READ;
-	ret = spi_write(spi, &command, sizeof(command));
-	if (ret < 0) {
-		dev_err(&spi->dev, "SPI write command error\n");
-		return ret;
-	}
-
-	ret = spi_read(spi, data, sizeof(*data));
-	if (ret < 0) {
-		dev_err(&spi->dev, "SPI read byte error\n");
-		return ret;
-	}
-
-	return 0;
-}
-
-static int adt7310_spi_write_byte(struct adt7410_chip_info *chip, u8 reg,
-	u8 data)
-{
-	struct spi_device *spi = to_spi_device(chip->dev);
-	u8 buf[2];
-	int ret = 0;
-
-	buf[0] = AD7310_COMMAND(reg);
-	buf[1] = data;
-
-	ret = spi_write(spi, buf, 2);
-	if (ret < 0) {
-		dev_err(&spi->dev, "SPI write byte error\n");
-		return ret;
-	}
-
-	return ret;
-}
-
-static const struct adt7410_ops adt7310_spi_ops = {
-	.read_word = adt7310_spi_read_word,
-	.write_word = adt7310_spi_write_word,
-	.read_byte = adt7310_spi_read_byte,
-	.write_byte = adt7310_spi_write_byte,
-};
-
-static int adt7310_spi_probe(struct spi_device *spi)
-{
-	return adt7410_probe(&spi->dev, spi->irq,
-		spi_get_device_id(spi)->name, &adt7310_spi_ops);
-}
-
-static int adt7310_spi_remove(struct spi_device *spi)
-{
-	return adt7410_remove(&spi->dev, spi->irq);
-}
-
-static const struct spi_device_id adt7310_id[] = {
-	{ "adt7310", 0 },
-	{}
-};
-MODULE_DEVICE_TABLE(spi, adt7310_id);
-
-static struct spi_driver adt7310_driver = {
-	.driver = {
-		.name = "adt7310",
-		.owner = THIS_MODULE,
-	},
-	.probe = adt7310_spi_probe,
-	.remove = adt7310_spi_remove,
-	.id_table = adt7310_id,
-};
-
-static int __init adt7310_spi_init(void)
-{
-	return spi_register_driver(&adt7310_driver);
-}
-
-static void adt7310_spi_exit(void)
-{
-	spi_unregister_driver(&adt7310_driver);
-}
-
-#else
-
-static int __init adt7310_spi_init(void) { return 0; };
-static void adt7310_spi_exit(void) {};
-
-#endif
-
-static int __init adt7410_init(void)
-{
-	int ret;
-
-	ret = adt7310_spi_init();
-	if (ret)
-		return ret;
-
-	ret = adt7410_i2c_init();
-	if (ret)
-		adt7310_spi_exit();
-
-	return ret;
-}
-module_init(adt7410_init);
-
-static void __exit adt7410_exit(void)
-{
-	adt7410_i2c_exit();
-	adt7310_spi_exit();
-}
-module_exit(adt7410_exit);
-
-MODULE_AUTHOR("Sonic Zhang <sonic.zhang@analog.com>");
-MODULE_DESCRIPTION("Analog Devices ADT7310/ADT7410 digital temperature sensor driver");
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/adc/lpc32xx_adc.c b/drivers/staging/iio/adc/lpc32xx_adc.c
index 0bf2a6cc79e..2f2f7fdd069 100644
--- a/drivers/staging/iio/adc/lpc32xx_adc.c
+++ b/drivers/staging/iio/adc/lpc32xx_adc.c
@@ -103,7 +103,7 @@ static const struct iio_info lpc32xx_adc_iio_info = {
 	.type = IIO_VOLTAGE,				\
 	.indexed = 1,					\
 	.channel = _index,				\
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,	\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),	\
 	.address = AD_IN * _index,			\
 	.scan_index = _index,				\
 }
diff --git a/drivers/staging/iio/adc/mxs-lradc.c b/drivers/staging/iio/adc/mxs-lradc.c
index 55a459b6190..6044e1717b2 100644
--- a/drivers/staging/iio/adc/mxs-lradc.c
+++ b/drivers/staging/iio/adc/mxs-lradc.c
@@ -646,7 +646,7 @@ static irqreturn_t mxs_lradc_trigger_handler(int irq, void *p)
 
 static int mxs_lradc_configure_trigger(struct iio_trigger *trig, bool state)
 {
-	struct iio_dev *iio = trig->private_data;
+	struct iio_dev *iio = iio_trigger_get_drvdata(trig);
 	struct mxs_lradc *lradc = iio_priv(iio);
 	const uint32_t st = state ? STMP_OFFSET_REG_SET : STMP_OFFSET_REG_CLR;
 
@@ -670,7 +670,7 @@ static int mxs_lradc_trigger_init(struct iio_dev *iio)
 		return -ENOMEM;
 
 	trig->dev.parent = iio->dev.parent;
-	trig->private_data = iio;
+	iio_trigger_set_drvdata(trig, iio);
 	trig->ops = &mxs_lradc_trigger_ops;
 
 	ret = iio_trigger_register(trig);
@@ -822,7 +822,7 @@ static const struct iio_buffer_setup_ops mxs_lradc_buffer_ops = {
 	.type = (chan_type),					\
 	.indexed = 1,						\
 	.scan_index = (idx),					\
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,		\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
 	.channel = (idx),					\
 	.scan_type = {						\
 		.sign = 'u',					\
@@ -983,6 +983,9 @@ static int mxs_lradc_probe(struct platform_device *pdev)
 	if (ret)
 		goto err_trig;
 
+	/* Configure the hardware. */
+	mxs_lradc_hw_init(lradc);
+
 	/* Register the touchscreen input device. */
 	ret = mxs_lradc_ts_register(lradc);
 	if (ret)
@@ -995,9 +998,6 @@ static int mxs_lradc_probe(struct platform_device *pdev)
 		goto err_ts;
 	}
 
-	/* Configure the hardware. */
-	mxs_lradc_hw_init(lradc);
-
 	return 0;
 
 err_ts:
diff --git a/drivers/staging/iio/adc/spear_adc.c b/drivers/staging/iio/adc/spear_adc.c
index 13052ceb2f2..f45da426695 100644
--- a/drivers/staging/iio/adc/spear_adc.c
+++ b/drivers/staging/iio/adc/spear_adc.c
@@ -180,8 +180,8 @@ static int spear_read_raw(struct iio_dev *indio_dev,
 #define SPEAR_ADC_CHAN(idx) {				\
 	.type = IIO_VOLTAGE,				\
 	.indexed = 1,					\
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |	\
-	IIO_CHAN_INFO_SCALE_SHARED_BIT,			\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),	\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),	\
 	.channel = idx,					\
 	.scan_type = {					\
 		.sign = 'u',				\
diff --git a/drivers/staging/iio/addac/adt7316.c b/drivers/staging/iio/addac/adt7316.c
index 0b431bc4f62..506b5a7d96c 100644
--- a/drivers/staging/iio/addac/adt7316.c
+++ b/drivers/staging/iio/addac/adt7316.c
@@ -256,7 +256,7 @@ static ssize_t adt7316_store_enabled(struct device *dev,
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	int enable;
 
-	if (!memcmp(buf, "1", 1))
+	if (buf[0] == '1')
 		enable = 1;
 	else
 		enable = 0;
@@ -299,7 +299,7 @@ static ssize_t adt7316_store_select_ex_temp(struct device *dev,
 		return -EPERM;
 
 	config1 = chip->config1 & (~ADT7516_SEL_EX_TEMP);
-	if (!memcmp(buf, "1", 1))
+	if (buf[0] == '1')
 		config1 |= ADT7516_SEL_EX_TEMP;
 
 	ret = chip->bus.write(chip->bus.client, ADT7316_CONFIG1, config1);
@@ -495,7 +495,7 @@ static ssize_t adt7316_store_disable_averaging(struct device *dev,
 	int ret;
 
 	config2 = chip->config2 & (~ADT7316_DISABLE_AVERAGING);
-	if (!memcmp(buf, "1", 1))
+	if (buf[0] == '1')
 		config2 |= ADT7316_DISABLE_AVERAGING;
 
 	ret = chip->bus.write(chip->bus.client, ADT7316_CONFIG2, config2);
@@ -534,7 +534,7 @@ static ssize_t adt7316_store_enable_smbus_timeout(struct device *dev,
 	int ret;
 
 	config2 = chip->config2 & (~ADT7316_EN_SMBUS_TIMEOUT);
-	if (!memcmp(buf, "1", 1))
+	if (buf[0] == '1')
 		config2 |= ADT7316_EN_SMBUS_TIMEOUT;
 
 	ret = chip->bus.write(chip->bus.client, ADT7316_CONFIG2, config2);
@@ -597,7 +597,7 @@ static ssize_t adt7316_store_powerdown(struct device *dev,
 	int ret;
 
 	config1 = chip->config1 & (~ADT7316_PD);
-	if (!memcmp(buf, "1", 1))
+	if (buf[0] == '1')
 		config1 |= ADT7316_PD;
 
 	ret = chip->bus.write(chip->bus.client, ADT7316_CONFIG1, config1);
@@ -635,7 +635,7 @@ static ssize_t adt7316_store_fast_ad_clock(struct device *dev,
 	int ret;
 
 	config3 = chip->config3 & (~ADT7316_ADCLK_22_5);
-	if (!memcmp(buf, "1", 1))
+	if (buf[0] == '1')
 		config3 |= ADT7316_ADCLK_22_5;
 
 	ret = chip->bus.write(chip->bus.client, ADT7316_CONFIG3, config3);
@@ -681,7 +681,7 @@ static ssize_t adt7316_store_da_high_resolution(struct device *dev,
 
 	chip->dac_bits = 8;
 
-	if (!memcmp(buf, "1", 1)) {
+	if (buf[0] == '1') {
 		config3 = chip->config3 | ADT7316_DA_HIGH_RESOLUTION;
 		if (chip->id == ID_ADT7316 || chip->id == ID_ADT7516)
 			chip->dac_bits = 12;
@@ -731,7 +731,7 @@ static ssize_t adt7316_store_AIN_internal_Vref(struct device *dev,
 	if ((chip->id & ID_FAMILY_MASK) != ID_ADT75XX)
 		return -EPERM;
 
-	if (memcmp(buf, "1", 1))
+	if (buf[0] != '1')
 		config3 = chip->config3 & (~ADT7516_AIN_IN_VREF);
 	else
 		config3 = chip->config3 | ADT7516_AIN_IN_VREF;
@@ -773,7 +773,7 @@ static ssize_t adt7316_store_enable_prop_DACA(struct device *dev,
 	int ret;
 
 	config3 = chip->config3 & (~ADT7316_EN_IN_TEMP_PROP_DACA);
-	if (!memcmp(buf, "1", 1))
+	if (buf[0] == '1')
 		config3 |= ADT7316_EN_IN_TEMP_PROP_DACA;
 
 	ret = chip->bus.write(chip->bus.client, ADT7316_CONFIG3, config3);
@@ -812,7 +812,7 @@ static ssize_t adt7316_store_enable_prop_DACB(struct device *dev,
 	int ret;
 
 	config3 = chip->config3 & (~ADT7316_EN_EX_TEMP_PROP_DACB);
-	if (!memcmp(buf, "1", 1))
+	if (buf[0] == '1')
 		config3 |= ADT7316_EN_EX_TEMP_PROP_DACB;
 
 	ret = chip->bus.write(chip->bus.client, ADT7316_CONFIG3, config3);
@@ -1018,7 +1018,7 @@ static ssize_t adt7316_store_DA_AB_Vref_bypass(struct device *dev,
 		return -EPERM;
 
 	dac_config = chip->dac_config & (~ADT7316_VREF_BYPASS_DAC_AB);
-	if (!memcmp(buf, "1", 1))
+	if (buf[0] == '1')
 		dac_config |= ADT7316_VREF_BYPASS_DAC_AB;
 
 	ret = chip->bus.write(chip->bus.client, ADT7316_DAC_CONFIG, dac_config);
@@ -1063,7 +1063,7 @@ static ssize_t adt7316_store_DA_CD_Vref_bypass(struct device *dev,
 		return -EPERM;
 
 	dac_config = chip->dac_config & (~ADT7316_VREF_BYPASS_DAC_CD);
-	if (!memcmp(buf, "1", 1))
+	if (buf[0] == '1')
 		dac_config |= ADT7316_VREF_BYPASS_DAC_CD;
 
 	ret = chip->bus.write(chip->bus.client, ADT7316_DAC_CONFIG, dac_config);
@@ -1982,7 +1982,7 @@ static ssize_t adt7316_set_int_enabled(struct device *dev,
 	int ret;
 
 	config1 = chip->config1 & (~ADT7316_INT_EN);
-	if (!memcmp(buf, "1", 1))
+	if (buf[0] == '1')
 		config1 |= ADT7316_INT_EN;
 
 	ret = chip->bus.write(chip->bus.client, ADT7316_CONFIG1, config1);
diff --git a/drivers/staging/iio/cdc/ad7150.c b/drivers/staging/iio/cdc/ad7150.c
index 3c608c14dd9..687dd2c9143 100644
--- a/drivers/staging/iio/cdc/ad7150.c
+++ b/drivers/staging/iio/cdc/ad7150.c
@@ -429,8 +429,8 @@ static const struct iio_chan_spec ad7150_channels[] = {
 		.type = IIO_CAPACITANCE,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-		IIO_CHAN_INFO_AVERAGE_RAW_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+		BIT(IIO_CHAN_INFO_AVERAGE_RAW),
 		.event_mask =
 		IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING) |
 		IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_FALLING) |
@@ -442,8 +442,8 @@ static const struct iio_chan_spec ad7150_channels[] = {
 		.type = IIO_CAPACITANCE,
 		.indexed = 1,
 		.channel = 1,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-		IIO_CHAN_INFO_AVERAGE_RAW_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+		BIT(IIO_CHAN_INFO_AVERAGE_RAW),
 		.event_mask =
 		IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING) |
 		IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_FALLING) |
diff --git a/drivers/staging/iio/cdc/ad7152.c b/drivers/staging/iio/cdc/ad7152.c
index 3c92ba3722a..1d7c5283a85 100644
--- a/drivers/staging/iio/cdc/ad7152.c
+++ b/drivers/staging/iio/cdc/ad7152.c
@@ -436,38 +436,38 @@ static const struct iio_chan_spec ad7152_channels[] = {
 		.type = IIO_CAPACITANCE,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
-		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+		BIT(IIO_CHAN_INFO_CALIBSCALE) |
+		BIT(IIO_CHAN_INFO_CALIBBIAS) |
+		BIT(IIO_CHAN_INFO_SCALE),
 	}, {
 		.type = IIO_CAPACITANCE,
 		.differential = 1,
 		.indexed = 1,
 		.channel = 0,
 		.channel2 = 2,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
-		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+		BIT(IIO_CHAN_INFO_CALIBSCALE) |
+		BIT(IIO_CHAN_INFO_CALIBBIAS) |
+		BIT(IIO_CHAN_INFO_SCALE),
 	}, {
 		.type = IIO_CAPACITANCE,
 		.indexed = 1,
 		.channel = 1,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
-		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+		BIT(IIO_CHAN_INFO_CALIBSCALE) |
+		BIT(IIO_CHAN_INFO_CALIBBIAS) |
+		BIT(IIO_CHAN_INFO_SCALE),
 	}, {
 		.type = IIO_CAPACITANCE,
 		.differential = 1,
 		.indexed = 1,
 		.channel = 1,
 		.channel2 = 3,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
-		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+		BIT(IIO_CHAN_INFO_CALIBSCALE) |
+		BIT(IIO_CHAN_INFO_CALIBBIAS) |
+		BIT(IIO_CHAN_INFO_SCALE),
 	}
 };
 /*
diff --git a/drivers/staging/iio/cdc/ad7746.c b/drivers/staging/iio/cdc/ad7746.c
index 466b82ecfbe..94f9ca726d1 100644
--- a/drivers/staging/iio/cdc/ad7746.c
+++ b/drivers/staging/iio/cdc/ad7746.c
@@ -123,8 +123,8 @@ static const struct iio_chan_spec ad7746_channels[] = {
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
 		.address = AD7746_REG_VT_DATA_HIGH << 8 |
 			AD7746_VTSETUP_VTMD_EXT_VIN,
 	},
@@ -133,8 +133,8 @@ static const struct iio_chan_spec ad7746_channels[] = {
 		.indexed = 1,
 		.channel = 1,
 		.extend_name = "supply",
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
 		.address = AD7746_REG_VT_DATA_HIGH << 8 |
 			AD7746_VTSETUP_VTMD_VDD_MON,
 	},
@@ -142,7 +142,7 @@ static const struct iio_chan_spec ad7746_channels[] = {
 		.type = IIO_TEMP,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_PROCESSED_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED),
 		.address = AD7746_REG_VT_DATA_HIGH << 8 |
 			AD7746_VTSETUP_VTMD_INT_TEMP,
 	},
@@ -150,7 +150,7 @@ static const struct iio_chan_spec ad7746_channels[] = {
 		.type = IIO_TEMP,
 		.indexed = 1,
 		.channel = 1,
-		.info_mask = IIO_CHAN_INFO_PROCESSED_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED),
 		.address = AD7746_REG_VT_DATA_HIGH << 8 |
 			AD7746_VTSETUP_VTMD_EXT_TEMP,
 	},
@@ -158,11 +158,10 @@ static const struct iio_chan_spec ad7746_channels[] = {
 		.type = IIO_CAPACITANCE,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
-		IIO_CHAN_INFO_CALIBBIAS_SHARED_BIT |
-		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+		BIT(IIO_CHAN_INFO_CALIBSCALE) | BIT(IIO_CHAN_INFO_OFFSET),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_CALIBBIAS) |
+		BIT(IIO_CHAN_INFO_SCALE),
 		.address = AD7746_REG_CAP_DATA_HIGH << 8,
 	},
 	[CIN1_DIFF] = {
@@ -171,11 +170,10 @@ static const struct iio_chan_spec ad7746_channels[] = {
 		.indexed = 1,
 		.channel = 0,
 		.channel2 = 2,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
-		IIO_CHAN_INFO_CALIBBIAS_SHARED_BIT |
-		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+		BIT(IIO_CHAN_INFO_CALIBSCALE) | BIT(IIO_CHAN_INFO_OFFSET),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_CALIBBIAS) |
+		BIT(IIO_CHAN_INFO_SCALE),
 		.address = AD7746_REG_CAP_DATA_HIGH << 8 |
 			AD7746_CAPSETUP_CAPDIFF
 	},
@@ -183,11 +181,10 @@ static const struct iio_chan_spec ad7746_channels[] = {
 		.type = IIO_CAPACITANCE,
 		.indexed = 1,
 		.channel = 1,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
-		IIO_CHAN_INFO_CALIBBIAS_SHARED_BIT |
-		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+		BIT(IIO_CHAN_INFO_CALIBSCALE) | BIT(IIO_CHAN_INFO_OFFSET),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_CALIBBIAS) |
+		BIT(IIO_CHAN_INFO_SCALE),
 		.address = AD7746_REG_CAP_DATA_HIGH << 8 |
 			AD7746_CAPSETUP_CIN2,
 	},
@@ -197,11 +194,10 @@ static const struct iio_chan_spec ad7746_channels[] = {
 		.indexed = 1,
 		.channel = 1,
 		.channel2 = 3,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
-		IIO_CHAN_INFO_CALIBBIAS_SHARED_BIT |
-		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+		BIT(IIO_CHAN_INFO_CALIBSCALE) | BIT(IIO_CHAN_INFO_OFFSET),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_CALIBBIAS) |
+		BIT(IIO_CHAN_INFO_SCALE),
 		.address = AD7746_REG_CAP_DATA_HIGH << 8 |
 			AD7746_CAPSETUP_CAPDIFF | AD7746_CAPSETUP_CIN2,
 	}
diff --git a/drivers/staging/iio/gyro/adis16060_core.c b/drivers/staging/iio/gyro/adis16060_core.c
index 687c151f984..c67d3a832ae 100644
--- a/drivers/staging/iio/gyro/adis16060_core.c
+++ b/drivers/staging/iio/gyro/adis16060_core.c
@@ -120,27 +120,26 @@ static const struct iio_chan_spec adis16060_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.address = ADIS16060_GYRO,
 	}, {
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.address = ADIS16060_AIN1,
 	}, {
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
 		.channel = 1,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.address = ADIS16060_AIN2,
 	}, {
 		.type = IIO_TEMP,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
-		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+		BIT(IIO_CHAN_INFO_OFFSET) | BIT(IIO_CHAN_INFO_SCALE),
 		.address = ADIS16060_TEMP_OUT,
 	}
 };
diff --git a/drivers/staging/iio/gyro/adis16130_core.c b/drivers/staging/iio/gyro/adis16130_core.c
index 835801ee7e8..531b803cb2a 100644
--- a/drivers/staging/iio/gyro/adis16130_core.c
+++ b/drivers/staging/iio/gyro/adis16130_core.c
@@ -100,13 +100,13 @@ static const struct iio_chan_spec adis16130_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.address = ADIS16130_RATEDATA,
 	}, {
 		.type = IIO_TEMP,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.address = ADIS16130_TEMPDATA,
 	}
 };
diff --git a/drivers/staging/iio/gyro/adis16260_core.c b/drivers/staging/iio/gyro/adis16260_core.c
index 6e80b8c768a..620d63fd099 100644
--- a/drivers/staging/iio/gyro/adis16260_core.c
+++ b/drivers/staging/iio/gyro/adis16260_core.c
@@ -124,8 +124,8 @@ static IIO_DEVICE_ATTR(sampling_frequency_available,
 #define ADIS16260_GYRO_CHANNEL_SET(axis, mod)				\
 struct iio_chan_spec adis16260_channels_##axis[] = {		\
 	ADIS_GYRO_CHAN(mod, ADIS16260_GYRO_OUT, ADIS16260_SCAN_GYRO, \
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT | \
-		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT, 14), \
+		BIT(IIO_CHAN_INFO_CALIBBIAS) | \
+		BIT(IIO_CHAN_INFO_CALIBSCALE), 14), \
 	ADIS_INCLI_CHAN(mod, ADIS16260_ANGL_OUT, ADIS16260_SCAN_ANGL, 0, 14), \
 	ADIS_TEMP_CHAN(ADIS16260_TEMP_OUT, ADIS16260_SCAN_TEMP, 12), \
 	ADIS_SUPPLY_CHAN(ADIS16260_SUPPLY_OUT, ADIS16260_SCAN_SUPPLY, 12), \
diff --git a/drivers/staging/iio/iio_simple_dummy.c b/drivers/staging/iio/iio_simple_dummy.c
index aee76c710a3..0e8e02a3cf5 100644
--- a/drivers/staging/iio/iio_simple_dummy.c
+++ b/drivers/staging/iio/iio_simple_dummy.c
@@ -71,25 +71,25 @@ static const struct iio_chan_spec iio_dummy_channels[] = {
 		.indexed = 1,
 		.channel = 0,
 		/* What other information is available? */
-		.info_mask =
+		.info_mask_separate =
 		/*
 		 * in_voltage0_raw
 		 * Raw (unscaled no bias removal etc) measurement
 		 * from the device.
 		 */
-		IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		BIT(IIO_CHAN_INFO_RAW) |
 		/*
 		 * in_voltage0_offset
 		 * Offset for userspace to apply prior to scale
 		 * when converting to standard units (microvolts)
 		 */
-		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
+		BIT(IIO_CHAN_INFO_OFFSET) |
 		/*
 		 * in_voltage0_scale
 		 * Multipler for userspace to apply post offset
 		 * when converting to standard units (microvolts)
 		 */
-		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		BIT(IIO_CHAN_INFO_SCALE),
 		/* The ordering of elements in the buffer via an enum */
 		.scan_index = voltage0,
 		.scan_type = { /* Description of storage in buffer */
@@ -118,19 +118,18 @@ static const struct iio_chan_spec iio_dummy_channels[] = {
 		.indexed = 1,
 		.channel = 1,
 		.channel2 = 2,
-		.info_mask =
 		/*
 		 * in_voltage1-voltage2_raw
 		 * Raw (unscaled no bias removal etc) measurement
 		 * from the device.
 		 */
-		IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		/*
 		 * in_voltage-voltage_scale
 		 * Shared version of scale - shared by differential
 		 * input channels of type IIO_VOLTAGE.
 		 */
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
 		.scan_index = diffvoltage1m2,
 		.scan_type = { /* Description of storage in buffer */
 			.sign = 's', /* signed */
@@ -146,9 +145,8 @@ static const struct iio_chan_spec iio_dummy_channels[] = {
 		.indexed = 1,
 		.channel = 3,
 		.channel2 = 4,
-		.info_mask =
-		IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
 		.scan_index = diffvoltage3m4,
 		.scan_type = {
 			.sign = 's',
@@ -166,15 +164,15 @@ static const struct iio_chan_spec iio_dummy_channels[] = {
 		.modified = 1,
 		/* Channel 2 is use for modifiers */
 		.channel2 = IIO_MOD_X,
-		.info_mask =
-		IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
 		/*
-		 * Internal bias correction value. Applied
+		 * Internal bias and gain correction values. Applied
 		 * by the hardware or driver prior to userspace
 		 * seeing the readings. Typically part of hardware
 		 * calibration.
 		 */
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
+		BIT(IIO_CHAN_INFO_CALIBSCALE) |
+		BIT(IIO_CHAN_INFO_CALIBBIAS),
 		.scan_index = accelx,
 		.scan_type = { /* Description of storage in buffer */
 			.sign = 's', /* signed */
@@ -191,7 +189,7 @@ static const struct iio_chan_spec iio_dummy_channels[] = {
 	/* DAC channel out_voltage0_raw */
 	{
 		.type = IIO_VOLTAGE,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.output = 1,
 		.indexed = 1,
 		.channel = 0,
@@ -204,8 +202,8 @@ static const struct iio_chan_spec iio_dummy_channels[] = {
  * @chan:	the channel whose data is to be read
  * @val:	first element of returned value (typically INT)
  * @val2:	second element of returned value (typically MICRO)
- * @mask:	what we actually want to read. 0 is the channel, everything else
- *		is as per the info_mask in iio_chan_spec.
+ * @mask:	what we actually want to read as per the info_mask_*
+ *		in iio_chan_spec.
  */
 static int iio_dummy_read_raw(struct iio_dev *indio_dev,
 			      struct iio_chan_spec const *chan,
@@ -287,8 +285,8 @@ static int iio_dummy_read_raw(struct iio_dev *indio_dev,
  * @chan:	the channel whose data is to be written
  * @val:	first element of value to set (typically INT)
  * @val2:	second element of value to set (typically MICRO)
- * @mask:	what we actually want to write. 0 is the channel, everything else
- *		is as per the info_mask in iio_chan_spec.
+ * @mask:	what we actually want to write as per the info_mask_*
+ *		in iio_chan_spec.
  *
  * Note that all raw writes are assumed IIO_VAL_INT and info mask elements
  * are assumed to be IIO_INT_PLUS_MICRO unless the callback write_raw_get_fmt
@@ -314,7 +312,7 @@ static int iio_dummy_write_raw(struct iio_dev *indio_dev,
 		st->dac_val = val;
 		mutex_unlock(&st->lock);
 		return 0;
-	case IIO_CHAN_INFO_CALIBBIAS:
+	case IIO_CHAN_INFO_CALIBSCALE:
 		mutex_lock(&st->lock);
 		/* Compare against table - hard matching here */
 		for (i = 0; i < ARRAY_SIZE(dummy_scales); i++)
@@ -327,6 +325,12 @@ static int iio_dummy_write_raw(struct iio_dev *indio_dev,
 			st->accel_calibscale = &dummy_scales[i];
 		mutex_unlock(&st->lock);
 		return ret;
+	case IIO_CHAN_INFO_CALIBBIAS:
+		mutex_lock(&st->lock);
+		st->accel_calibbias = val;
+		mutex_unlock(&st->lock);
+		return 0;
+
 	default:
 		return -EINVAL;
 	}
diff --git a/drivers/staging/iio/impedance-analyzer/ad5933.c b/drivers/staging/iio/impedance-analyzer/ad5933.c
index 440e2261e8c..6330af656a0 100644
--- a/drivers/staging/iio/impedance-analyzer/ad5933.c
+++ b/drivers/staging/iio/impedance-analyzer/ad5933.c
@@ -113,7 +113,7 @@ static const struct iio_chan_spec ad5933_channels[] = {
 		.type = IIO_TEMP,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_PROCESSED_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED),
 		.address = AD5933_REG_TEMP_DATA,
 		.scan_type = {
 			.sign = 's',
@@ -125,8 +125,8 @@ static const struct iio_chan_spec ad5933_channels[] = {
 		.indexed = 1,
 		.channel = 0,
 		.extend_name = "real_raw",
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+		BIT(IIO_CHAN_INFO_SCALE),
 		.address = AD5933_REG_REAL_DATA,
 		.scan_index = 0,
 		.scan_type = {
@@ -139,8 +139,8 @@ static const struct iio_chan_spec ad5933_channels[] = {
 		.indexed = 1,
 		.channel = 0,
 		.extend_name = "imag_raw",
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+		BIT(IIO_CHAN_INFO_SCALE),
 		.address = AD5933_REG_IMAG_DATA,
 		.scan_index = 1,
 		.scan_type = {
diff --git a/drivers/staging/iio/light/isl29018.c b/drivers/staging/iio/light/isl29018.c
index b0adac0bf5d..82478a59e42 100644
--- a/drivers/staging/iio/light/isl29018.c
+++ b/drivers/staging/iio/light/isl29018.c
@@ -412,17 +412,17 @@ static const struct iio_chan_spec isl29018_channels[] = {
 		.type = IIO_LIGHT,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_PROCESSED_SEPARATE_BIT |
-		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED) |
+		BIT(IIO_CHAN_INFO_CALIBSCALE),
 	}, {
 		.type = IIO_INTENSITY,
 		.modified = 1,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.channel2 = IIO_MOD_LIGHT_IR,
 	}, {
 		/* Unindexed in current ABI.  But perhaps it should be. */
 		.type = IIO_PROXIMITY,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 	}
 };
 
diff --git a/drivers/staging/iio/light/isl29028.c b/drivers/staging/iio/light/isl29028.c
index e52af77f778..8bb0d03627f 100644
--- a/drivers/staging/iio/light/isl29028.c
+++ b/drivers/staging/iio/light/isl29028.c
@@ -391,15 +391,15 @@ static const struct attribute_group isl29108_group = {
 static const struct iio_chan_spec isl29028_channels[] = {
 	{
 		.type = IIO_LIGHT,
-		.info_mask = IIO_CHAN_INFO_PROCESSED_SEPARATE_BIT |
-		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED) |
+		BIT(IIO_CHAN_INFO_SCALE),
 	}, {
 		.type = IIO_INTENSITY,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 	}, {
 		.type = IIO_PROXIMITY,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-		IIO_CHAN_INFO_SAMP_FREQ_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+		BIT(IIO_CHAN_INFO_SAMP_FREQ),
 	}
 };
 
diff --git a/drivers/staging/iio/light/tsl2x7x_core.c b/drivers/staging/iio/light/tsl2x7x_core.c
index a58731e70bb..d060f257251 100644
--- a/drivers/staging/iio/light/tsl2x7x_core.c
+++ b/drivers/staging/iio/light/tsl2x7x_core.c
@@ -1733,14 +1733,14 @@ static const struct tsl2x7x_chip_info tsl2x7x_chip_info_tbl[] = {
 			.type = IIO_LIGHT,
 			.indexed = 1,
 			.channel = 0,
-			.info_mask = IIO_CHAN_INFO_PROCESSED_SEPARATE_BIT,
+			.info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED),
 			}, {
 			.type = IIO_INTENSITY,
 			.indexed = 1,
 			.channel = 0,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-				IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
-				IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
+			.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+				BIT(IIO_CHAN_INFO_CALIBSCALE) |
+				BIT(IIO_CHAN_INFO_CALIBBIAS),
 			.event_mask = TSL2X7X_EVENT_MASK
 			}, {
 			.type = IIO_INTENSITY,
@@ -1757,7 +1757,7 @@ static const struct tsl2x7x_chip_info tsl2x7x_chip_info_tbl[] = {
 			.type = IIO_PROXIMITY,
 			.indexed = 1,
 			.channel = 0,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+			.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 			.event_mask = TSL2X7X_EVENT_MASK
 			},
 		},
@@ -1770,25 +1770,25 @@ static const struct tsl2x7x_chip_info tsl2x7x_chip_info_tbl[] = {
 			.type = IIO_LIGHT,
 			.indexed = 1,
 			.channel = 0,
-			.info_mask = IIO_CHAN_INFO_PROCESSED_SEPARATE_BIT
+			.info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED)
 			}, {
 			.type = IIO_INTENSITY,
 			.indexed = 1,
 			.channel = 0,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-				IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
-				IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
+			.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+				BIT(IIO_CHAN_INFO_CALIBSCALE) |
+				BIT(IIO_CHAN_INFO_CALIBBIAS),
 			.event_mask = TSL2X7X_EVENT_MASK
 			}, {
 			.type = IIO_INTENSITY,
 			.indexed = 1,
 			.channel = 1,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+			.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 			}, {
 			.type = IIO_PROXIMITY,
 			.indexed = 1,
 			.channel = 0,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+			.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 			.event_mask = TSL2X7X_EVENT_MASK
 			},
 		},
@@ -1801,8 +1801,8 @@ static const struct tsl2x7x_chip_info tsl2x7x_chip_info_tbl[] = {
 			.type = IIO_PROXIMITY,
 			.indexed = 1,
 			.channel = 0,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-				IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT,
+			.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+				BIT(IIO_CHAN_INFO_CALIBSCALE),
 			.event_mask = TSL2X7X_EVENT_MASK
 			},
 		},
@@ -1815,26 +1815,26 @@ static const struct tsl2x7x_chip_info tsl2x7x_chip_info_tbl[] = {
 			.type = IIO_LIGHT,
 			.indexed = 1,
 			.channel = 0,
-			.info_mask = IIO_CHAN_INFO_PROCESSED_SEPARATE_BIT,
+			.info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED),
 			}, {
 			.type = IIO_INTENSITY,
 			.indexed = 1,
 			.channel = 0,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-				IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
-				IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
+			.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+				BIT(IIO_CHAN_INFO_CALIBSCALE) |
+				BIT(IIO_CHAN_INFO_CALIBBIAS),
 			.event_mask = TSL2X7X_EVENT_MASK
 			}, {
 			.type = IIO_INTENSITY,
 			.indexed = 1,
 			.channel = 1,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+			.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 			}, {
 			.type = IIO_PROXIMITY,
 			.indexed = 1,
 			.channel = 0,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-				IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT,
+			.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+				BIT(IIO_CHAN_INFO_CALIBSCALE),
 			.event_mask = TSL2X7X_EVENT_MASK
 			},
 		},
diff --git a/drivers/staging/iio/magnetometer/Kconfig b/drivers/staging/iio/magnetometer/Kconfig
index df5e0d4ea29..a3ea69e9d80 100644
--- a/drivers/staging/iio/magnetometer/Kconfig
+++ b/drivers/staging/iio/magnetometer/Kconfig
@@ -3,17 +3,6 @@
 #
 menu "Magnetometer sensors"
 
-config SENSORS_AK8975
-	tristate "Asahi Kasei AK8975 3-Axis Magnetometer"
-	depends on I2C
-	depends on GPIOLIB
-	help
-	  Say yes here to build support for Asahi Kasei AK8975 3-Axis
-	  Magnetometer.
-
-	  To compile this driver as a module, choose M here: the module
-	  will be called ak8975.
-
 config SENSORS_HMC5843
 	tristate "Honeywell HMC5843/5883/5883L 3-Axis Magnetometer"
 	depends on I2C
diff --git a/drivers/staging/iio/magnetometer/Makefile b/drivers/staging/iio/magnetometer/Makefile
index f2a753f8079..f9bfb2e11d7 100644
--- a/drivers/staging/iio/magnetometer/Makefile
+++ b/drivers/staging/iio/magnetometer/Makefile
@@ -2,5 +2,4 @@
 # Makefile for industrial I/O Magnetometer sensors
 #
 
-obj-$(CONFIG_SENSORS_AK8975)	+= ak8975.o
 obj-$(CONFIG_SENSORS_HMC5843)	+= hmc5843.o
diff --git a/drivers/staging/iio/magnetometer/hmc5843.c b/drivers/staging/iio/magnetometer/hmc5843.c
index 1a520ecfa3e..86c6bf9d5dd 100644
--- a/drivers/staging/iio/magnetometer/hmc5843.c
+++ b/drivers/staging/iio/magnetometer/hmc5843.c
@@ -564,8 +564,8 @@ static int hmc5843_read_raw(struct iio_dev *indio_dev,
 		.type = IIO_MAGN,					\
 		.modified = 1,						\
 		.channel2 = IIO_MOD_##axis,				\
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |		\
-			     IIO_CHAN_INFO_SCALE_SHARED_BIT,		\
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),	\
 		.address = add						\
 	}
 
diff --git a/drivers/staging/iio/meter/ade7758_core.c b/drivers/staging/iio/meter/ade7758_core.c
index 53c68dcc454..8f5bcfab356 100644
--- a/drivers/staging/iio/meter/ade7758_core.c
+++ b/drivers/staging/iio/meter/ade7758_core.c
@@ -649,8 +649,8 @@ static const struct iio_chan_spec ade7758_channels[] = {
 		.indexed = 1,
 		.channel = 0,
 		.extend_name = "raw",
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
 		.address = AD7758_WT(AD7758_PHASE_A, AD7758_VOLTAGE),
 		.scan_index = 0,
 		.scan_type = {
@@ -663,8 +663,8 @@ static const struct iio_chan_spec ade7758_channels[] = {
 		.indexed = 1,
 		.channel = 0,
 		.extend_name = "raw",
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
 		.address = AD7758_WT(AD7758_PHASE_A, AD7758_CURRENT),
 		.scan_index = 1,
 		.scan_type = {
@@ -677,8 +677,8 @@ static const struct iio_chan_spec ade7758_channels[] = {
 		.indexed = 1,
 		.channel = 0,
 		.extend_name = "apparent_raw",
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
 		.address = AD7758_WT(AD7758_PHASE_A, AD7758_APP_PWR),
 		.scan_index = 2,
 		.scan_type = {
@@ -691,8 +691,8 @@ static const struct iio_chan_spec ade7758_channels[] = {
 		.indexed = 1,
 		.channel = 0,
 		.extend_name = "active_raw",
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
 		.address = AD7758_WT(AD7758_PHASE_A, AD7758_ACT_PWR),
 		.scan_index = 3,
 		.scan_type = {
@@ -705,8 +705,8 @@ static const struct iio_chan_spec ade7758_channels[] = {
 		.indexed = 1,
 		.channel = 0,
 		.extend_name = "reactive_raw",
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
 		.address = AD7758_WT(AD7758_PHASE_A, AD7758_REACT_PWR),
 		.scan_index = 4,
 		.scan_type = {
@@ -719,8 +719,8 @@ static const struct iio_chan_spec ade7758_channels[] = {
 		.indexed = 1,
 		.channel = 1,
 		.extend_name = "raw",
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
 		.address = AD7758_WT(AD7758_PHASE_B, AD7758_VOLTAGE),
 		.scan_index = 5,
 		.scan_type = {
@@ -733,8 +733,8 @@ static const struct iio_chan_spec ade7758_channels[] = {
 		.indexed = 1,
 		.channel = 1,
 		.extend_name = "raw",
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
 		.address = AD7758_WT(AD7758_PHASE_B, AD7758_CURRENT),
 		.scan_index = 6,
 		.scan_type = {
@@ -747,8 +747,8 @@ static const struct iio_chan_spec ade7758_channels[] = {
 		.indexed = 1,
 		.channel = 1,
 		.extend_name = "apparent_raw",
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
 		.address = AD7758_WT(AD7758_PHASE_B, AD7758_APP_PWR),
 		.scan_index = 7,
 		.scan_type = {
@@ -761,8 +761,8 @@ static const struct iio_chan_spec ade7758_channels[] = {
 		.indexed = 1,
 		.channel = 1,
 		.extend_name = "active_raw",
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
 		.address = AD7758_WT(AD7758_PHASE_B, AD7758_ACT_PWR),
 		.scan_index = 8,
 		.scan_type = {
@@ -775,8 +775,8 @@ static const struct iio_chan_spec ade7758_channels[] = {
 		.indexed = 1,
 		.channel = 1,
 		.extend_name = "reactive_raw",
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
 		.address = AD7758_WT(AD7758_PHASE_B, AD7758_REACT_PWR),
 		.scan_index = 9,
 		.scan_type = {
@@ -789,8 +789,8 @@ static const struct iio_chan_spec ade7758_channels[] = {
 		.indexed = 1,
 		.channel = 2,
 		.extend_name = "raw",
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
 		.address = AD7758_WT(AD7758_PHASE_C, AD7758_VOLTAGE),
 		.scan_index = 10,
 		.scan_type = {
@@ -803,8 +803,8 @@ static const struct iio_chan_spec ade7758_channels[] = {
 		.indexed = 1,
 		.channel = 2,
 		.extend_name = "raw",
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
 		.address = AD7758_WT(AD7758_PHASE_C, AD7758_CURRENT),
 		.scan_index = 11,
 		.scan_type = {
@@ -817,8 +817,8 @@ static const struct iio_chan_spec ade7758_channels[] = {
 		.indexed = 1,
 		.channel = 2,
 		.extend_name = "apparent_raw",
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
 		.address = AD7758_WT(AD7758_PHASE_C, AD7758_APP_PWR),
 		.scan_index = 12,
 		.scan_type = {
@@ -831,8 +831,8 @@ static const struct iio_chan_spec ade7758_channels[] = {
 		.indexed = 1,
 		.channel = 2,
 		.extend_name = "active_raw",
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
 		.address = AD7758_WT(AD7758_PHASE_C, AD7758_ACT_PWR),
 		.scan_index = 13,
 		.scan_type = {
@@ -845,8 +845,8 @@ static const struct iio_chan_spec ade7758_channels[] = {
 		.indexed = 1,
 		.channel = 2,
 		.extend_name = "reactive_raw",
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
 		.address = AD7758_WT(AD7758_PHASE_C, AD7758_REACT_PWR),
 		.scan_index = 14,
 		.scan_type = {
diff --git a/drivers/staging/iio/meter/ade7758_trigger.c b/drivers/staging/iio/meter/ade7758_trigger.c
index f9c6a340092..7a94ddd42f5 100644
--- a/drivers/staging/iio/meter/ade7758_trigger.c
+++ b/drivers/staging/iio/meter/ade7758_trigger.c
@@ -32,7 +32,7 @@ static irqreturn_t ade7758_data_rdy_trig_poll(int irq, void *private)
 static int ade7758_data_rdy_trigger_set_state(struct iio_trigger *trig,
 						bool state)
 {
-	struct iio_dev *indio_dev = trig->private_data;
+	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
 
 	dev_dbg(&indio_dev->dev, "%s (%d)\n", __func__, state);
 	return ade7758_set_irq(&indio_dev->dev, state);
@@ -44,7 +44,7 @@ static int ade7758_data_rdy_trigger_set_state(struct iio_trigger *trig,
  **/
 static int ade7758_trig_try_reen(struct iio_trigger *trig)
 {
-	struct iio_dev *indio_dev = trig->private_data;
+	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
 	struct ade7758_state *st = iio_priv(indio_dev);
 
 	enable_irq(st->us->irq);
@@ -81,7 +81,7 @@ int ade7758_probe_trigger(struct iio_dev *indio_dev)
 
 	st->trig->dev.parent = &st->us->dev;
 	st->trig->ops = &ade7758_trigger_ops;
-	st->trig->private_data = indio_dev;
+	iio_trigger_set_drvdata(st->trig, indio_dev);
 	ret = iio_trigger_register(st->trig);
 
 	/* select default trigger */
diff --git a/drivers/staging/iio/resolver/ad2s1200.c b/drivers/staging/iio/resolver/ad2s1200.c
index 4fe349914f9..71221161aa6 100644
--- a/drivers/staging/iio/resolver/ad2s1200.c
+++ b/drivers/staging/iio/resolver/ad2s1200.c
@@ -85,12 +85,12 @@ static const struct iio_chan_spec ad2s1200_channels[] = {
 		.type = IIO_ANGL,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 	}, {
 		.type = IIO_ANGL_VEL,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 	}
 };
 
diff --git a/drivers/staging/iio/resolver/ad2s1210.c b/drivers/staging/iio/resolver/ad2s1210.c
index 53110b6a3c7..0d3356d4b7d 100644
--- a/drivers/staging/iio/resolver/ad2s1210.c
+++ b/drivers/staging/iio/resolver/ad2s1210.c
@@ -577,12 +577,12 @@ static const struct iio_chan_spec ad2s1210_channels[] = {
 		.type = IIO_ANGL,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 	}, {
 		.type = IIO_ANGL_VEL,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 	}
 };
 
diff --git a/drivers/staging/iio/resolver/ad2s90.c b/drivers/staging/iio/resolver/ad2s90.c
index 0aecfbcdb99..40b825286d4 100644
--- a/drivers/staging/iio/resolver/ad2s90.c
+++ b/drivers/staging/iio/resolver/ad2s90.c
@@ -55,7 +55,7 @@ static const struct iio_chan_spec ad2s90_chan = {
 	.type = IIO_ANGL,
 	.indexed = 1,
 	.channel = 0,
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 };
 
 static int ad2s90_probe(struct spi_device *spi)
diff --git a/drivers/staging/iio/trigger/iio-trig-bfin-timer.c b/drivers/staging/iio/trigger/iio-trig-bfin-timer.c
index 42798da575c..38a158b77b1 100644
--- a/drivers/staging/iio/trigger/iio-trig-bfin-timer.c
+++ b/drivers/staging/iio/trigger/iio-trig-bfin-timer.c
@@ -65,7 +65,7 @@ struct bfin_tmr_state {
 
 static int iio_bfin_tmr_set_state(struct iio_trigger *trig, bool state)
 {
-	struct bfin_tmr_state *st = trig->private_data;
+	struct bfin_tmr_state *st = iio_trigger_get_drvdata(trig);
 
 	if (get_gptimer_period(st->t->id) == 0)
 		return -EINVAL;
@@ -82,7 +82,7 @@ static ssize_t iio_bfin_tmr_frequency_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t count)
 {
 	struct iio_trigger *trig = to_iio_trigger(dev);
-	struct bfin_tmr_state *st = trig->private_data;
+	struct bfin_tmr_state *st = iio_trigger_get_drvdata(trig);
 	unsigned long val;
 	bool enabled;
 	int ret;
@@ -125,7 +125,7 @@ static ssize_t iio_bfin_tmr_frequency_show(struct device *dev,
 				 char *buf)
 {
 	struct iio_trigger *trig = to_iio_trigger(dev);
-	struct bfin_tmr_state *st = trig->private_data;
+	struct bfin_tmr_state *st = iio_trigger_get_drvdata(trig);
 	unsigned int period = get_gptimer_period(st->t->id);
 	unsigned long val;
 
@@ -213,9 +213,9 @@ static int iio_bfin_tmr_trigger_probe(struct platform_device *pdev)
 		goto out1;
 	}
 
-	st->trig->private_data = st;
 	st->trig->ops = &iio_bfin_tmr_trigger_ops;
 	st->trig->dev.groups = iio_bfin_tmr_trigger_attr_groups;
+	iio_trigger_set_drvdata(st->trig, st);
 	ret = iio_trigger_register(st->trig);
 	if (ret)
 		goto out2;
diff --git a/drivers/staging/iio/trigger/iio-trig-gpio.c b/drivers/staging/iio/trigger/iio-trig-gpio.c
index fcc4cb048c9..7c593d18a91 100644
--- a/drivers/staging/iio/trigger/iio-trig-gpio.c
+++ b/drivers/staging/iio/trigger/iio-trig-gpio.c
@@ -83,7 +83,7 @@ static int iio_gpio_trigger_probe(struct platform_device *pdev)
 				ret = -ENOMEM;
 				goto error_put_trigger;
 			}
-			trig->private_data = trig_info;
+			iio_trigger_set_drvdata(trig, trig_info);
 			trig_info->irq = irq;
 			trig->ops = &iio_gpio_trigger_ops;
 			ret = request_irq(irq, iio_gpio_trigger_poll,
@@ -121,7 +121,7 @@ error_free_completed_registrations:
 				 trig2,
 				 &iio_gpio_trigger_list,
 				 alloc_list) {
-		trig_info = trig->private_data;
+		trig_info = iio_trigger_get_drvdata(trig);
 		free_irq(gpio_to_irq(trig_info->irq), trig);
 		kfree(trig_info);
 		iio_trigger_unregister(trig);
@@ -140,7 +140,7 @@ static int iio_gpio_trigger_remove(struct platform_device *pdev)
 				 trig2,
 				 &iio_gpio_trigger_list,
 				 alloc_list) {
-		trig_info = trig->private_data;
+		trig_info = iio_trigger_get_drvdata(trig);
 		iio_trigger_unregister(trig);
 		free_irq(trig_info->irq, trig);
 		kfree(trig_info);
diff --git a/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c b/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
index 9102b1ba253..79695974b1d 100644
--- a/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
+++ b/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
@@ -30,7 +30,7 @@ struct iio_prtc_trigger_info {
 
 static int iio_trig_periodic_rtc_set_state(struct iio_trigger *trig, bool state)
 {
-	struct iio_prtc_trigger_info *trig_info = trig->private_data;
+	struct iio_prtc_trigger_info *trig_info = iio_trigger_get_drvdata(trig);
 	if (trig_info->frequency == 0)
 		return -EINVAL;
 	printk(KERN_INFO "trigger frequency is %d\n", trig_info->frequency);
@@ -42,7 +42,7 @@ static ssize_t iio_trig_periodic_read_freq(struct device *dev,
 					   char *buf)
 {
 	struct iio_trigger *trig = to_iio_trigger(dev);
-	struct iio_prtc_trigger_info *trig_info = trig->private_data;
+	struct iio_prtc_trigger_info *trig_info = iio_trigger_get_drvdata(trig);
 	return sprintf(buf, "%u\n", trig_info->frequency);
 }
 
@@ -52,7 +52,7 @@ static ssize_t iio_trig_periodic_write_freq(struct device *dev,
 					    size_t len)
 {
 	struct iio_trigger *trig = to_iio_trigger(dev);
-	struct iio_prtc_trigger_info *trig_info = trig->private_data;
+	struct iio_prtc_trigger_info *trig_info = iio_trigger_get_drvdata(trig);
 	unsigned long val;
 	int ret;
 
@@ -124,7 +124,7 @@ static int iio_trig_periodic_rtc_probe(struct platform_device *dev)
 			ret = -ENOMEM;
 			goto error_put_trigger_and_remove_from_list;
 		}
-		trig->private_data = trig_info;
+		iio_trigger_set_drvdata(trig, trig_info);
 		trig->ops = &iio_prtc_trigger_ops;
 		/* RTC access */
 		trig_info->rtc
@@ -158,7 +158,7 @@ error_free_completed_registrations:
 				 trig2,
 				 &iio_prtc_trigger_list,
 				 alloc_list) {
-		trig_info = trig->private_data;
+		trig_info = iio_trigger_get_drvdata(trig);
 		rtc_irq_unregister(trig_info->rtc, &trig_info->task);
 		rtc_class_close(trig_info->rtc);
 		kfree(trig_info);
@@ -176,7 +176,7 @@ static int iio_trig_periodic_rtc_remove(struct platform_device *dev)
 				 trig2,
 				 &iio_prtc_trigger_list,
 				 alloc_list) {
-		trig_info = trig->private_data;
+		trig_info = iio_trigger_get_drvdata(trig);
 		rtc_irq_unregister(trig_info->rtc, &trig_info->task);
 		rtc_class_close(trig_info->rtc);
 		kfree(trig_info);
diff --git a/drivers/staging/iio/trigger/iio-trig-sysfs.c b/drivers/staging/iio/trigger/iio-trig-sysfs.c
index 3bac97224bf..b727bde8b7f 100644
--- a/drivers/staging/iio/trigger/iio-trig-sysfs.c
+++ b/drivers/staging/iio/trigger/iio-trig-sysfs.c
@@ -103,7 +103,7 @@ static ssize_t iio_sysfs_trigger_poll(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t count)
 {
 	struct iio_trigger *trig = to_iio_trigger(dev);
-	struct iio_sysfs_trig *sysfs_trig = trig->private_data;
+	struct iio_sysfs_trig *sysfs_trig = iio_trigger_get_drvdata(trig);
 
 	irq_work_queue(&sysfs_trig->work);
 
@@ -160,7 +160,7 @@ static int iio_sysfs_trigger_probe(int id)
 	t->trig->dev.groups = iio_sysfs_trigger_attr_groups;
 	t->trig->ops = &iio_sysfs_trigger_ops;
 	t->trig->dev.parent = &iio_sysfs_trig_dev;
-	t->trig->private_data = t;
+	iio_trigger_set_drvdata(t->trig, t);
 
 	init_irq_work(&t->work, iio_sysfs_trigger_work);
 
diff --git a/drivers/staging/imx-drm/Kconfig b/drivers/staging/imx-drm/Kconfig
index be7e2e30ac1..8c9e40390f4 100644
--- a/drivers/staging/imx-drm/Kconfig
+++ b/drivers/staging/imx-drm/Kconfig
@@ -20,6 +20,13 @@ config DRM_IMX_PARALLEL_DISPLAY
 	tristate "Support for parallel displays"
 	depends on DRM_IMX
 
+config DRM_IMX_TVE
+	tristate "Support for TV and VGA displays"
+	depends on DRM_IMX
+	help
+	  Choose this to enable the internal Television Encoder (TVe)
+	  found on i.MX53 processors.
+
 config DRM_IMX_IPUV3_CORE
 	tristate "IPUv3 core support"
 	depends on DRM_IMX
diff --git a/drivers/staging/imx-drm/Makefile b/drivers/staging/imx-drm/Makefile
index 83a9056546e..7e50184523c 100644
--- a/drivers/staging/imx-drm/Makefile
+++ b/drivers/staging/imx-drm/Makefile
@@ -4,6 +4,7 @@ imxdrm-objs := imx-drm-core.o imx-fb.o
 obj-$(CONFIG_DRM_IMX) += imxdrm.o
 
 obj-$(CONFIG_DRM_IMX_PARALLEL_DISPLAY) += parallel-display.o
+obj-$(CONFIG_DRM_IMX_TVE) += imx-tve.o
 obj-$(CONFIG_DRM_IMX_FB_HELPER) += imx-fbdev.o
 obj-$(CONFIG_DRM_IMX_IPUV3_CORE) += ipu-v3/
 obj-$(CONFIG_DRM_IMX_IPUV3)	+= ipuv3-crtc.o
diff --git a/drivers/staging/imx-drm/TODO b/drivers/staging/imx-drm/TODO
index e52adc44e60..123acbe9b37 100644
--- a/drivers/staging/imx-drm/TODO
+++ b/drivers/staging/imx-drm/TODO
@@ -1,5 +1,8 @@
 TODO:
 - get DRM Maintainer review for this code
+- Wait for common display framework to hit mainline and update the IPU
+  driver to use it. This will most probably make changes to the devicetree
+  bindings necessary.
 - Factor out more code to common helper functions
 - decide where to put the base driver. It is not specific to a subsystem
   and would be used by DRM/KMS and media/V4L2
diff --git a/drivers/staging/imx-drm/imx-drm-core.c b/drivers/staging/imx-drm/imx-drm-core.c
index cec19f1cf56..64553058b67 100644
--- a/drivers/staging/imx-drm/imx-drm-core.c
+++ b/drivers/staging/imx-drm/imx-drm-core.c
@@ -112,8 +112,8 @@ static struct imx_drm_crtc *imx_drm_crtc_by_num(struct imx_drm_device *imxdrm,
 	return NULL;
 }
 
-int imx_drm_crtc_panel_format(struct drm_crtc *crtc, u32 encoder_type,
-		u32 interface_pix_fmt)
+int imx_drm_crtc_panel_format_pins(struct drm_crtc *crtc, u32 encoder_type,
+		u32 interface_pix_fmt, int hsync_pin, int vsync_pin)
 {
 	struct imx_drm_device *imxdrm = crtc->dev->dev_private;
 	struct imx_drm_crtc *imx_crtc;
@@ -134,9 +134,18 @@ found:
 	helper = &imx_crtc->imx_drm_helper_funcs;
 	if (helper->set_interface_pix_fmt)
 		return helper->set_interface_pix_fmt(crtc,
-				encoder_type, interface_pix_fmt);
+				encoder_type, interface_pix_fmt,
+				hsync_pin, vsync_pin);
 	return 0;
 }
+EXPORT_SYMBOL_GPL(imx_drm_crtc_panel_format_pins);
+
+int imx_drm_crtc_panel_format(struct drm_crtc *crtc, u32 encoder_type,
+		u32 interface_pix_fmt)
+{
+	return imx_drm_crtc_panel_format_pins(crtc, encoder_type,
+					      interface_pix_fmt, 0, 0);
+}
 EXPORT_SYMBOL_GPL(imx_drm_crtc_panel_format);
 
 int imx_drm_crtc_vblank_get(struct imx_drm_crtc *imx_drm_crtc)
diff --git a/drivers/staging/imx-drm/imx-drm.h b/drivers/staging/imx-drm/imx-drm.h
index ae28a490c44..f2aac91ddf5 100644
--- a/drivers/staging/imx-drm/imx-drm.h
+++ b/drivers/staging/imx-drm/imx-drm.h
@@ -1,14 +1,24 @@
 #ifndef _IMX_DRM_H_
 #define _IMX_DRM_H_
 
+#include <linux/videodev2.h>
+
+#define IPU_PIX_FMT_GBR24	v4l2_fourcc('G', 'B', 'R', '3')
+
+struct drm_crtc;
+struct drm_connector;
+struct drm_device;
+struct drm_encoder;
 struct imx_drm_crtc;
 struct drm_fbdev_cma;
+struct drm_framebuffer;
+struct platform_device;
 
 struct imx_drm_crtc_helper_funcs {
 	int (*enable_vblank)(struct drm_crtc *crtc);
 	void (*disable_vblank)(struct drm_crtc *crtc);
 	int (*set_interface_pix_fmt)(struct drm_crtc *crtc, u32 encoder_type,
-			u32 pix_fmt);
+			u32 pix_fmt, int hsync_pin, int vsync_pin);
 	const struct drm_crtc_helper_funcs *crtc_helper_funcs;
 	const struct drm_crtc_funcs *crtc_funcs;
 };
@@ -44,6 +54,8 @@ struct drm_gem_cma_object *imx_drm_fb_get_obj(struct drm_framebuffer *fb);
 
 struct drm_device *imx_drm_device_get(void);
 void imx_drm_device_put(void);
+int imx_drm_crtc_panel_format_pins(struct drm_crtc *crtc, u32 encoder_type,
+		u32 interface_pix_fmt, int hsync_pin, int vsync_pin);
 int imx_drm_crtc_panel_format(struct drm_crtc *crtc, u32 encoder_type,
 		u32 interface_pix_fmt);
 void imx_drm_fb_helper_set(struct drm_fbdev_cma *fbdev_helper);
diff --git a/drivers/staging/imx-drm/imx-tve.c b/drivers/staging/imx-drm/imx-tve.c
new file mode 100644
index 00000000000..ac163446440
--- /dev/null
+++ b/drivers/staging/imx-drm/imx-tve.c
@@ -0,0 +1,755 @@
+/*
+ * i.MX drm driver - Television Encoder (TVEv2)
+ *
+ * Copyright (C) 2013 Philipp Zabel, Pengutronix
+ *
+ * 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/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/module.h>
+#include <linux/of_i2c.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+#include <linux/spinlock.h>
+#include <linux/videodev2.h>
+#include <drm/drmP.h>
+#include <drm/drm_fb_helper.h>
+#include <drm/drm_crtc_helper.h>
+
+#include "imx-drm.h"
+
+#define TVE_COM_CONF_REG	0x00
+#define TVE_TVDAC0_CONT_REG	0x28
+#define TVE_TVDAC1_CONT_REG	0x2c
+#define TVE_TVDAC2_CONT_REG	0x30
+#define TVE_CD_CONT_REG		0x34
+#define TVE_INT_CONT_REG	0x64
+#define TVE_STAT_REG		0x68
+#define TVE_TST_MODE_REG	0x6c
+#define TVE_MV_CONT_REG		0xdc
+
+/* TVE_COM_CONF_REG */
+#define TVE_SYNC_CH_2_EN	BIT(22)
+#define TVE_SYNC_CH_1_EN	BIT(21)
+#define TVE_SYNC_CH_0_EN	BIT(20)
+#define TVE_TV_OUT_MODE_MASK	(0x7 << 12)
+#define TVE_TV_OUT_DISABLE	(0x0 << 12)
+#define TVE_TV_OUT_CVBS_0	(0x1 << 12)
+#define TVE_TV_OUT_CVBS_2	(0x2 << 12)
+#define TVE_TV_OUT_CVBS_0_2	(0x3 << 12)
+#define TVE_TV_OUT_SVIDEO_0_1	(0x4 << 12)
+#define TVE_TV_OUT_SVIDEO_0_1_CVBS2_2	(0x5 << 12)
+#define TVE_TV_OUT_YPBPR	(0x6 << 12)
+#define TVE_TV_OUT_RGB		(0x7 << 12)
+#define TVE_TV_STAND_MASK	(0xf << 8)
+#define TVE_TV_STAND_HD_1080P30	(0xc << 8)
+#define TVE_P2I_CONV_EN		BIT(7)
+#define TVE_INP_VIDEO_FORM	BIT(6)
+#define TVE_INP_YCBCR_422	(0x0 << 6)
+#define TVE_INP_YCBCR_444	(0x1 << 6)
+#define TVE_DATA_SOURCE_MASK	(0x3 << 4)
+#define TVE_DATA_SOURCE_BUS1	(0x0 << 4)
+#define TVE_DATA_SOURCE_BUS2	(0x1 << 4)
+#define TVE_DATA_SOURCE_EXT	(0x2 << 4)
+#define TVE_DATA_SOURCE_TESTGEN	(0x3 << 4)
+#define TVE_IPU_CLK_EN_OFS	3
+#define TVE_IPU_CLK_EN		BIT(3)
+#define TVE_DAC_SAMP_RATE_OFS	1
+#define TVE_DAC_SAMP_RATE_WIDTH	2
+#define TVE_DAC_SAMP_RATE_MASK	(0x3 << 1)
+#define TVE_DAC_FULL_RATE	(0x0 << 1)
+#define TVE_DAC_DIV2_RATE	(0x1 << 1)
+#define TVE_DAC_DIV4_RATE	(0x2 << 1)
+#define TVE_EN			BIT(0)
+
+/* TVE_TVDACx_CONT_REG */
+#define TVE_TVDAC_GAIN_MASK	(0x3f << 0)
+
+/* TVE_CD_CONT_REG */
+#define TVE_CD_CH_2_SM_EN	BIT(22)
+#define TVE_CD_CH_1_SM_EN	BIT(21)
+#define TVE_CD_CH_0_SM_EN	BIT(20)
+#define TVE_CD_CH_2_LM_EN	BIT(18)
+#define TVE_CD_CH_1_LM_EN	BIT(17)
+#define TVE_CD_CH_0_LM_EN	BIT(16)
+#define TVE_CD_CH_2_REF_LVL	BIT(10)
+#define TVE_CD_CH_1_REF_LVL	BIT(9)
+#define TVE_CD_CH_0_REF_LVL	BIT(8)
+#define TVE_CD_EN		BIT(0)
+
+/* TVE_INT_CONT_REG */
+#define TVE_FRAME_END_IEN	BIT(13)
+#define TVE_CD_MON_END_IEN	BIT(2)
+#define TVE_CD_SM_IEN		BIT(1)
+#define TVE_CD_LM_IEN		BIT(0)
+
+/* TVE_TST_MODE_REG */
+#define TVE_TVDAC_TEST_MODE_MASK	(0x7 << 0)
+
+#define con_to_tve(x) container_of(x, struct imx_tve, connector)
+#define enc_to_tve(x) container_of(x, struct imx_tve, encoder)
+
+enum {
+	TVE_MODE_TVOUT,
+	TVE_MODE_VGA,
+};
+
+struct imx_tve {
+	struct drm_connector connector;
+	struct imx_drm_connector *imx_drm_connector;
+	struct drm_encoder encoder;
+	struct imx_drm_encoder *imx_drm_encoder;
+	struct device *dev;
+	spinlock_t enable_lock;	/* serializes tve_enable/disable */
+	spinlock_t lock;	/* register lock */
+	bool enabled;
+	int mode;
+
+	struct regmap *regmap;
+	struct regulator *dac_reg;
+	struct i2c_adapter *ddc;
+	struct clk *clk;
+	struct clk *di_sel_clk;
+	struct clk_hw clk_hw_di;
+	struct clk *di_clk;
+	int vsync_pin;
+	int hsync_pin;
+};
+
+static void tve_lock(void *__tve)
+{
+	struct imx_tve *tve = __tve;
+	spin_lock(&tve->lock);
+}
+
+static void tve_unlock(void *__tve)
+{
+	struct imx_tve *tve = __tve;
+	spin_unlock(&tve->lock);
+}
+
+static void tve_enable(struct imx_tve *tve)
+{
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&tve->enable_lock, flags);
+	if (!tve->enabled) {
+		tve->enabled = 1;
+		clk_prepare_enable(tve->clk);
+		ret = regmap_update_bits(tve->regmap, TVE_COM_CONF_REG,
+					 TVE_IPU_CLK_EN | TVE_EN,
+					 TVE_IPU_CLK_EN | TVE_EN);
+	}
+
+	/* clear interrupt status register */
+	regmap_write(tve->regmap, TVE_STAT_REG, 0xffffffff);
+
+	/* cable detection irq disabled in VGA mode, enabled in TVOUT mode */
+	if (tve->mode == TVE_MODE_VGA)
+		regmap_write(tve->regmap, TVE_INT_CONT_REG, 0);
+	else
+		regmap_write(tve->regmap, TVE_INT_CONT_REG,
+			     TVE_CD_SM_IEN | TVE_CD_LM_IEN | TVE_CD_MON_END_IEN);
+	spin_unlock_irqrestore(&tve->enable_lock, flags);
+}
+
+static void tve_disable(struct imx_tve *tve)
+{
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&tve->enable_lock, flags);
+	if (tve->enabled) {
+		tve->enabled = 0;
+		ret = regmap_update_bits(tve->regmap, TVE_COM_CONF_REG,
+					 TVE_IPU_CLK_EN | TVE_EN, 0);
+		clk_disable_unprepare(tve->clk);
+	}
+	spin_unlock_irqrestore(&tve->enable_lock, flags);
+}
+
+static int tve_setup_tvout(struct imx_tve *tve)
+{
+	return -ENOTSUPP;
+}
+
+static int tve_setup_vga(struct imx_tve *tve)
+{
+	unsigned int mask;
+	unsigned int val;
+	int ret;
+
+	/* set gain to (1 + 10/128) to provide 0.7V peak-to-peak amplitude */
+	ret = regmap_update_bits(tve->regmap, TVE_TVDAC0_CONT_REG,
+				 TVE_TVDAC_GAIN_MASK, 0x0a);
+	ret = regmap_update_bits(tve->regmap, TVE_TVDAC1_CONT_REG,
+				 TVE_TVDAC_GAIN_MASK, 0x0a);
+	ret = regmap_update_bits(tve->regmap, TVE_TVDAC2_CONT_REG,
+				 TVE_TVDAC_GAIN_MASK, 0x0a);
+
+	/* set configuration register */
+	mask = TVE_DATA_SOURCE_MASK | TVE_INP_VIDEO_FORM;
+	val  = TVE_DATA_SOURCE_BUS2 | TVE_INP_YCBCR_444;
+	mask |= TVE_TV_STAND_MASK       | TVE_P2I_CONV_EN;
+	val  |= TVE_TV_STAND_HD_1080P30 | 0;
+	mask |= TVE_TV_OUT_MODE_MASK | TVE_SYNC_CH_0_EN;
+	val  |= TVE_TV_OUT_RGB       | TVE_SYNC_CH_0_EN;
+	ret = regmap_update_bits(tve->regmap, TVE_COM_CONF_REG, mask, val);
+	if (ret < 0) {
+		dev_err(tve->dev, "failed to set configuration: %d\n", ret);
+		return ret;
+	}
+
+	/* set test mode (as documented) */
+	ret = regmap_update_bits(tve->regmap, TVE_TST_MODE_REG,
+				 TVE_TVDAC_TEST_MODE_MASK, 1);
+
+	return 0;
+}
+
+static enum drm_connector_status imx_tve_connector_detect(
+				struct drm_connector *connector, bool force)
+{
+	return connector_status_connected;
+}
+
+static void imx_tve_connector_destroy(struct drm_connector *connector)
+{
+	/* do not free here */
+}
+
+static int imx_tve_connector_get_modes(struct drm_connector *connector)
+{
+	struct imx_tve *tve = con_to_tve(connector);
+	struct edid *edid;
+	int ret = 0;
+
+	if (!tve->ddc)
+		return 0;
+
+	edid = drm_get_edid(connector, tve->ddc);
+	if (edid) {
+		drm_mode_connector_update_edid_property(connector, edid);
+		ret = drm_add_edid_modes(connector, edid);
+		kfree(edid);
+	}
+
+	return ret;
+}
+
+static int imx_tve_connector_mode_valid(struct drm_connector *connector,
+					struct drm_display_mode *mode)
+{
+	struct imx_tve *tve = con_to_tve(connector);
+	unsigned long rate;
+
+	/* pixel clock with 2x oversampling */
+	rate = clk_round_rate(tve->clk, 2000UL * mode->clock) / 2000;
+	if (rate == mode->clock)
+		return MODE_OK;
+
+	/* pixel clock without oversampling */
+	rate = clk_round_rate(tve->clk, 1000UL * mode->clock) / 1000;
+	if (rate == mode->clock)
+		return MODE_OK;
+
+	dev_warn(tve->dev, "ignoring mode %dx%d\n",
+		 mode->hdisplay, mode->vdisplay);
+
+	return MODE_BAD;
+}
+
+static struct drm_encoder *imx_tve_connector_best_encoder(
+		struct drm_connector *connector)
+{
+	struct imx_tve *tve = con_to_tve(connector);
+
+	return &tve->encoder;
+}
+
+static void imx_tve_encoder_dpms(struct drm_encoder *encoder, int mode)
+{
+	struct imx_tve *tve = enc_to_tve(encoder);
+	int ret;
+
+	ret = regmap_update_bits(tve->regmap, TVE_COM_CONF_REG,
+				 TVE_TV_OUT_MODE_MASK, TVE_TV_OUT_DISABLE);
+	if (ret < 0)
+		dev_err(tve->dev, "failed to disable TVOUT: %d\n", ret);
+}
+
+static bool imx_tve_encoder_mode_fixup(struct drm_encoder *encoder,
+				       const struct drm_display_mode *mode,
+				       struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+static void imx_tve_encoder_prepare(struct drm_encoder *encoder)
+{
+	struct imx_tve *tve = enc_to_tve(encoder);
+
+	tve_disable(tve);
+
+	switch (tve->mode) {
+	case TVE_MODE_VGA:
+		imx_drm_crtc_panel_format_pins(encoder->crtc,
+				DRM_MODE_ENCODER_DAC, IPU_PIX_FMT_GBR24,
+				tve->hsync_pin, tve->vsync_pin);
+		break;
+	case TVE_MODE_TVOUT:
+		imx_drm_crtc_panel_format(encoder->crtc, DRM_MODE_ENCODER_TVDAC,
+					  V4L2_PIX_FMT_YUV444);
+		break;
+	}
+}
+
+static void imx_tve_encoder_mode_set(struct drm_encoder *encoder,
+				     struct drm_display_mode *mode,
+				     struct drm_display_mode *adjusted_mode)
+{
+	struct imx_tve *tve = enc_to_tve(encoder);
+	unsigned long rounded_rate;
+	unsigned long rate;
+	int div = 1;
+	int ret;
+
+	/*
+	 * FIXME
+	 * we should try 4k * mode->clock first,
+	 * and enable 4x oversampling for lower resolutions
+	 */
+	rate = 2000UL * mode->clock;
+	clk_set_rate(tve->clk, rate);
+	rounded_rate = clk_get_rate(tve->clk);
+	if (rounded_rate >= rate)
+		div = 2;
+	clk_set_rate(tve->di_clk, rounded_rate / div);
+
+	ret = clk_set_parent(tve->di_sel_clk, tve->di_clk);
+	if (ret < 0) {
+		dev_err(tve->dev, "failed to set di_sel parent to tve_di: %d\n",
+			ret);
+	}
+
+	if (tve->mode == TVE_MODE_VGA)
+		tve_setup_vga(tve);
+	else
+		tve_setup_tvout(tve);
+}
+
+static void imx_tve_encoder_commit(struct drm_encoder *encoder)
+{
+	struct imx_tve *tve = enc_to_tve(encoder);
+
+	tve_enable(tve);
+}
+
+static void imx_tve_encoder_disable(struct drm_encoder *encoder)
+{
+	struct imx_tve *tve = enc_to_tve(encoder);
+
+	tve_disable(tve);
+}
+
+static void imx_tve_encoder_destroy(struct drm_encoder *encoder)
+{
+	/* do not free here */
+}
+
+static struct drm_connector_funcs imx_tve_connector_funcs = {
+	.dpms = drm_helper_connector_dpms,
+	.fill_modes = drm_helper_probe_single_connector_modes,
+	.detect = imx_tve_connector_detect,
+	.destroy = imx_tve_connector_destroy,
+};
+
+static struct drm_connector_helper_funcs imx_tve_connector_helper_funcs = {
+	.get_modes = imx_tve_connector_get_modes,
+	.best_encoder = imx_tve_connector_best_encoder,
+	.mode_valid = imx_tve_connector_mode_valid,
+};
+
+static struct drm_encoder_funcs imx_tve_encoder_funcs = {
+	.destroy = imx_tve_encoder_destroy,
+};
+
+static struct drm_encoder_helper_funcs imx_tve_encoder_helper_funcs = {
+	.dpms = imx_tve_encoder_dpms,
+	.mode_fixup = imx_tve_encoder_mode_fixup,
+	.prepare = imx_tve_encoder_prepare,
+	.mode_set = imx_tve_encoder_mode_set,
+	.commit = imx_tve_encoder_commit,
+	.disable = imx_tve_encoder_disable,
+};
+
+static irqreturn_t imx_tve_irq_handler(int irq, void *data)
+{
+	struct imx_tve *tve = data;
+	unsigned int val;
+
+	regmap_read(tve->regmap, TVE_STAT_REG, &val);
+
+	/* clear interrupt status register */
+	regmap_write(tve->regmap, TVE_STAT_REG, 0xffffffff);
+
+	return IRQ_HANDLED;
+}
+
+static unsigned long clk_tve_di_recalc_rate(struct clk_hw *hw,
+					    unsigned long parent_rate)
+{
+	struct imx_tve *tve = container_of(hw, struct imx_tve, clk_hw_di);
+	unsigned int val;
+	int ret;
+
+	ret = regmap_read(tve->regmap, TVE_COM_CONF_REG, &val);
+	if (ret < 0)
+		return 0;
+
+	switch (val & TVE_DAC_SAMP_RATE_MASK) {
+	case TVE_DAC_DIV4_RATE:
+		return parent_rate / 4;
+	case TVE_DAC_DIV2_RATE:
+		return parent_rate / 2;
+	case TVE_DAC_FULL_RATE:
+	default:
+		return parent_rate;
+	}
+
+	return 0;
+}
+
+static long clk_tve_di_round_rate(struct clk_hw *hw, unsigned long rate,
+				  unsigned long *prate)
+{
+	unsigned long div;
+
+	div = *prate / rate;
+	if (div >= 4)
+		return *prate / 4;
+	else if (div >= 2)
+		return *prate / 2;
+	else
+		return *prate;
+}
+
+static int clk_tve_di_set_rate(struct clk_hw *hw, unsigned long rate,
+			       unsigned long parent_rate)
+{
+	struct imx_tve *tve = container_of(hw, struct imx_tve, clk_hw_di);
+	unsigned long div;
+	u32 val;
+	int ret;
+
+	div = parent_rate / rate;
+	if (div >= 4)
+		val = TVE_DAC_DIV4_RATE;
+	else if (div >= 2)
+		val = TVE_DAC_DIV2_RATE;
+	else
+		val = TVE_DAC_FULL_RATE;
+
+	ret = regmap_update_bits(tve->regmap, TVE_COM_CONF_REG, TVE_DAC_SAMP_RATE_MASK, val);
+	if (ret < 0) {
+		dev_err(tve->dev, "failed to set divider: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static struct clk_ops clk_tve_di_ops = {
+	.round_rate = clk_tve_di_round_rate,
+	.set_rate = clk_tve_di_set_rate,
+	.recalc_rate = clk_tve_di_recalc_rate,
+};
+
+static int tve_clk_init(struct imx_tve *tve, void __iomem *base)
+{
+	const char *tve_di_parent[1];
+	struct clk_init_data init = {
+		.name = "tve_di",
+		.ops = &clk_tve_di_ops,
+		.num_parents = 1,
+		.flags = 0,
+	};
+
+	tve_di_parent[0] = __clk_get_name(tve->clk);
+	init.parent_names = (const char **)&tve_di_parent;
+
+	tve->clk_hw_di.init = &init;
+	tve->di_clk = clk_register(tve->dev, &tve->clk_hw_di);
+	if (IS_ERR(tve->di_clk)) {
+		dev_err(tve->dev, "failed to register TVE output clock: %ld\n",
+			PTR_ERR(tve->di_clk));
+		return PTR_ERR(tve->di_clk);
+	}
+
+	return 0;
+}
+
+static int imx_tve_register(struct imx_tve *tve)
+{
+	int ret;
+
+	tve->connector.funcs = &imx_tve_connector_funcs;
+	tve->encoder.funcs = &imx_tve_encoder_funcs;
+
+	tve->encoder.encoder_type = DRM_MODE_ENCODER_NONE;
+	tve->connector.connector_type = DRM_MODE_CONNECTOR_VGA;
+
+	drm_encoder_helper_add(&tve->encoder, &imx_tve_encoder_helper_funcs);
+	ret = imx_drm_add_encoder(&tve->encoder, &tve->imx_drm_encoder,
+			THIS_MODULE);
+	if (ret) {
+		dev_err(tve->dev, "adding encoder failed with %d\n", ret);
+		return ret;
+	}
+
+	drm_connector_helper_add(&tve->connector,
+			&imx_tve_connector_helper_funcs);
+
+	ret = imx_drm_add_connector(&tve->connector,
+			&tve->imx_drm_connector, THIS_MODULE);
+	if (ret) {
+		imx_drm_remove_encoder(tve->imx_drm_encoder);
+		dev_err(tve->dev, "adding connector failed with %d\n", ret);
+		return ret;
+	}
+
+	drm_mode_connector_attach_encoder(&tve->connector, &tve->encoder);
+
+	return 0;
+}
+
+static bool imx_tve_readable_reg(struct device *dev, unsigned int reg)
+{
+	return (reg % 4 == 0) && (reg <= 0xdc);
+}
+
+static struct regmap_config tve_regmap_config = {
+	.reg_bits = 32,
+	.val_bits = 32,
+	.reg_stride = 4,
+
+	.readable_reg = imx_tve_readable_reg,
+
+	.lock = tve_lock,
+	.unlock = tve_unlock,
+
+	.max_register = 0xdc,
+};
+
+static const char *imx_tve_modes[] = {
+	[TVE_MODE_TVOUT]  = "tvout",
+	[TVE_MODE_VGA] = "vga",
+};
+
+const int of_get_tve_mode(struct device_node *np)
+{
+	const char *bm;
+	int ret, i;
+
+	ret = of_property_read_string(np, "fsl,tve-mode", &bm);
+	if (ret < 0)
+		return ret;
+
+	for (i = 0; i < ARRAY_SIZE(imx_tve_modes); i++)
+		if (!strcasecmp(bm, imx_tve_modes[i]))
+			return i;
+
+	return -EINVAL;
+}
+
+static int imx_tve_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct device_node *ddc_node;
+	struct imx_tve *tve;
+	struct resource *res;
+	void __iomem *base;
+	unsigned int val;
+	int irq;
+	int ret;
+
+	tve = devm_kzalloc(&pdev->dev, sizeof(*tve), GFP_KERNEL);
+	if (!tve)
+		return -ENOMEM;
+
+	tve->dev = &pdev->dev;
+	spin_lock_init(&tve->lock);
+	spin_lock_init(&tve->enable_lock);
+
+	ddc_node = of_parse_phandle(np, "ddc", 0);
+	if (ddc_node) {
+		tve->ddc = of_find_i2c_adapter_by_node(ddc_node);
+		of_node_put(ddc_node);
+	}
+
+	tve->mode = of_get_tve_mode(np);
+	if (tve->mode != TVE_MODE_VGA) {
+		dev_err(&pdev->dev, "only VGA mode supported, currently\n");
+		return -EINVAL;
+	}
+
+	if (tve->mode == TVE_MODE_VGA) {
+		struct pinctrl *pinctrl;
+
+		pinctrl = devm_pinctrl_get_select_default(&pdev->dev);
+		if (IS_ERR(pinctrl)) {
+			ret = PTR_ERR(pinctrl);
+			dev_warn(&pdev->dev, "failed to setup pinctrl: %d", ret);
+			return ret;
+		}
+
+		ret = of_property_read_u32(np, "fsl,hsync-pin", &tve->hsync_pin);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "failed to get vsync pin\n");
+			return ret;
+		}
+
+		ret |= of_property_read_u32(np, "fsl,vsync-pin", &tve->vsync_pin);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "failed to get vsync pin\n");
+			return ret;
+		}
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "failed to get memory region\n");
+		return -ENOENT;
+	}
+
+	base = devm_request_and_ioremap(&pdev->dev, res);
+	if (!base) {
+		dev_err(&pdev->dev, "failed to remap memory region\n");
+		return -ENOENT;
+	}
+
+	tve_regmap_config.lock_arg = tve;
+	tve->regmap = devm_regmap_init_mmio_clk(&pdev->dev, "tve", base,
+						&tve_regmap_config);
+	if (IS_ERR(tve->regmap)) {
+		dev_err(&pdev->dev, "failed to init regmap: %ld\n",
+			PTR_ERR(tve->regmap));
+		return PTR_ERR(tve->regmap);
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "failed to get irq\n");
+		return irq;
+	}
+
+	ret = devm_request_threaded_irq(&pdev->dev, irq, NULL,
+					imx_tve_irq_handler, IRQF_ONESHOT,
+					"imx-tve", tve);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to request irq: %d\n", ret);
+		return ret;
+	}
+
+	tve->dac_reg = devm_regulator_get(&pdev->dev, "dac");
+	if (!IS_ERR(tve->dac_reg)) {
+		regulator_set_voltage(tve->dac_reg, 2750000, 2750000);
+		regulator_enable(tve->dac_reg);
+	}
+
+	tve->clk = devm_clk_get(&pdev->dev, "tve");
+	if (IS_ERR(tve->clk)) {
+		dev_err(&pdev->dev, "failed to get high speed tve clock: %ld\n",
+			PTR_ERR(tve->clk));
+		return PTR_ERR(tve->clk);
+	}
+
+	/* this is the IPU DI clock input selector, can be parented to tve_di */
+	tve->di_sel_clk = devm_clk_get(&pdev->dev, "di_sel");
+	if (IS_ERR(tve->di_sel_clk)) {
+		dev_err(&pdev->dev, "failed to get ipu di mux clock: %ld\n",
+			PTR_ERR(tve->di_sel_clk));
+		return PTR_ERR(tve->di_sel_clk);
+	}
+
+	ret = tve_clk_init(tve, base);
+	if (ret < 0)
+		return ret;
+
+	ret = regmap_read(tve->regmap, TVE_COM_CONF_REG, &val);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to read configuration register: %d\n", ret);
+		return ret;
+	}
+	if (val != 0x00100000) {
+		dev_err(&pdev->dev, "configuration register default value indicates this is not a TVEv2\n");
+		return -ENODEV;
+	};
+
+	/* disable cable detection for VGA mode */
+	ret = regmap_write(tve->regmap, TVE_CD_CONT_REG, 0);
+
+	ret = imx_tve_register(tve);
+	if (ret)
+		return ret;
+
+	ret = imx_drm_encoder_add_possible_crtcs(tve->imx_drm_encoder, np);
+
+	platform_set_drvdata(pdev, tve);
+
+	return 0;
+}
+
+static int imx_tve_remove(struct platform_device *pdev)
+{
+	struct imx_tve *tve = platform_get_drvdata(pdev);
+	struct drm_connector *connector = &tve->connector;
+	struct drm_encoder *encoder = &tve->encoder;
+
+	drm_mode_connector_detach_encoder(connector, encoder);
+
+	imx_drm_remove_connector(tve->imx_drm_connector);
+	imx_drm_remove_encoder(tve->imx_drm_encoder);
+
+	if (!IS_ERR(tve->dac_reg))
+		regulator_disable(tve->dac_reg);
+
+	return 0;
+}
+
+static const struct of_device_id imx_tve_dt_ids[] = {
+	{ .compatible = "fsl,imx53-tve", },
+	{ /* sentinel */ }
+};
+
+static struct platform_driver imx_tve_driver = {
+	.probe		= imx_tve_probe,
+	.remove		= imx_tve_remove,
+	.driver		= {
+		.of_match_table = imx_tve_dt_ids,
+		.name	= "imx-tve",
+		.owner	= THIS_MODULE,
+	},
+};
+
+module_platform_driver(imx_tve_driver);
+
+MODULE_DESCRIPTION("i.MX Television Encoder driver");
+MODULE_AUTHOR("Philipp Zabel, Pengutronix");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/imx-drm/ipu-v3/imx-ipu-v3.h b/drivers/staging/imx-drm/ipu-v3/imx-ipu-v3.h
index 99d1cceaa3d..74c022e2a53 100644
--- a/drivers/staging/imx-drm/ipu-v3/imx-ipu-v3.h
+++ b/drivers/staging/imx-drm/ipu-v3/imx-ipu-v3.h
@@ -54,6 +54,9 @@ struct ipu_di_signal_cfg {
 #define IPU_DI_CLKMODE_SYNC	(1 << 0)
 #define IPU_DI_CLKMODE_EXT	(1 << 1)
 	unsigned long clkflags;
+
+	u8 hsync_pin;
+	u8 vsync_pin;
 };
 
 enum ipu_color_space {
@@ -292,7 +295,8 @@ static inline void ipu_cpmem_interlaced_scan(struct ipu_ch_param *p,
 
 void ipu_cpmem_set_yuv_planar(struct ipu_ch_param __iomem *p, u32 pixel_format,
 			int stride, int height);
-void ipu_cpmem_set_yuv_interleaved(struct ipu_ch_param *p, u32 pixel_format);
+void ipu_cpmem_set_yuv_interleaved(struct ipu_ch_param __iomem *p,
+				   u32 pixel_format);
 void ipu_cpmem_set_yuv_planar_full(struct ipu_ch_param __iomem *p,
 		u32 pixel_format, int stride, int u_offset, int v_offset);
 int ipu_cpmem_set_fmt(struct ipu_ch_param __iomem *cpmem, u32 pixelformat);
diff --git a/drivers/staging/imx-drm/ipu-v3/ipu-common.c b/drivers/staging/imx-drm/ipu-v3/ipu-common.c
index 366f259e375..055b99db50f 100644
--- a/drivers/staging/imx-drm/ipu-v3/ipu-common.c
+++ b/drivers/staging/imx-drm/ipu-v3/ipu-common.c
@@ -225,7 +225,8 @@ int ipu_cpmem_set_format_passthrough(struct ipu_ch_param __iomem *p,
 }
 EXPORT_SYMBOL_GPL(ipu_cpmem_set_format_passthrough);
 
-void ipu_cpmem_set_yuv_interleaved(struct ipu_ch_param *p, u32 pixel_format)
+void ipu_cpmem_set_yuv_interleaved(struct ipu_ch_param __iomem *p,
+				   u32 pixel_format)
 {
 	switch (pixel_format) {
 	case V4L2_PIX_FMT_UYVY:
diff --git a/drivers/staging/imx-drm/ipu-v3/ipu-dc.c b/drivers/staging/imx-drm/ipu-v3/ipu-dc.c
index 93c7579417b..59f03f9aefc 100644
--- a/drivers/staging/imx-drm/ipu-v3/ipu-dc.c
+++ b/drivers/staging/imx-drm/ipu-v3/ipu-dc.c
@@ -20,6 +20,7 @@
 #include <linux/delay.h>
 #include <linux/io.h>
 
+#include "../imx-drm.h"
 #include "imx-ipu-v3.h"
 #include "ipu-prv.h"
 
@@ -60,8 +61,10 @@
 
 #define WROD(lf)		(0x18 | ((lf) << 1))
 #define WRG			0x01
+#define WCLK			0xc9
 
 #define SYNC_WAVE 0
+#define NULL_WAVE (-1)
 
 #define DC_GEN_SYNC_1_6_SYNC	(2 << 1)
 #define DC_GEN_SYNC_PRIORITY_1	(1 << 7)
@@ -86,6 +89,8 @@ struct ipu_dc_priv;
 enum ipu_dc_map {
 	IPU_DC_MAP_RGB24,
 	IPU_DC_MAP_RGB565,
+	IPU_DC_MAP_GBR24, /* TVEv2 */
+	IPU_DC_MAP_BGR666,
 };
 
 struct ipu_dc {
@@ -117,16 +122,23 @@ static void dc_link_event(struct ipu_dc *dc, int event, int addr, int priority)
 }
 
 static void dc_write_tmpl(struct ipu_dc *dc, int word, u32 opcode, u32 operand,
-		int map, int wave, int glue, int sync)
+		int map, int wave, int glue, int sync, int stop)
 {
 	struct ipu_dc_priv *priv = dc->priv;
-	u32 reg;
-	int stop = 1;
-
-	reg = sync | glue << 4 | ++wave << 11 | ++map << 15 | ((operand << 20) & 0xfff00000);
-	writel(reg, priv->dc_tmpl_reg + word * 8);
-	reg = operand >> 12 | opcode << 4 | stop << 9;
-	writel(reg, priv->dc_tmpl_reg + word * 8 + 4);
+	u32 reg1, reg2;
+
+	if (opcode == WCLK) {
+		reg1 = (operand << 20) & 0xfff00000;
+		reg2 = operand >> 12 | opcode << 1 | stop << 9;
+	} else if (opcode == WRG) {
+		reg1 = sync | glue << 4 | ++wave << 11 | ((operand << 15) & 0xffff8000);
+		reg2 = operand >> 17 | opcode << 7 | stop << 9;
+	} else {
+		reg1 = sync | glue << 4 | ++wave << 11 | ++map << 15 | ((operand << 20) & 0xfff00000);
+		reg2 = operand >> 12 | opcode << 4 | stop << 9;
+	}
+	writel(reg1, priv->dc_tmpl_reg + word * 8);
+	writel(reg2, priv->dc_tmpl_reg + word * 8 + 4);
 }
 
 static int ipu_pixfmt_to_map(u32 fmt)
@@ -136,6 +148,10 @@ static int ipu_pixfmt_to_map(u32 fmt)
 		return IPU_DC_MAP_RGB24;
 	case V4L2_PIX_FMT_RGB565:
 		return IPU_DC_MAP_RGB565;
+	case IPU_PIX_FMT_GBR24:
+		return IPU_DC_MAP_GBR24;
+	case V4L2_PIX_FMT_BGR666:
+		return IPU_DC_MAP_BGR666;
 	default:
 		return -EINVAL;
 	}
@@ -161,24 +177,26 @@ int ipu_dc_init_sync(struct ipu_dc *dc, struct ipu_di *di, bool interlaced,
 		dc_link_event(dc, DC_EVT_NEW_DATA, 0, 1);
 
 		/* Init template microcode */
-		dc_write_tmpl(dc, 0, WROD(0), 0, map, SYNC_WAVE, 0, 8);
+		dc_write_tmpl(dc, 0, WROD(0), 0, map, SYNC_WAVE, 0, 8, 1);
 	} else {
 		if (dc->di) {
 			dc_link_event(dc, DC_EVT_NL, 2, 3);
 			dc_link_event(dc, DC_EVT_EOL, 3, 2);
-			dc_link_event(dc, DC_EVT_NEW_DATA, 4, 1);
+			dc_link_event(dc, DC_EVT_NEW_DATA, 1, 1);
 			/* Init template microcode */
-			dc_write_tmpl(dc, 2, WROD(0), 0, map, SYNC_WAVE, 8, 5);
-			dc_write_tmpl(dc, 3, WROD(0), 0, map, SYNC_WAVE, 4, 5);
-			dc_write_tmpl(dc, 4, WROD(0), 0, map, SYNC_WAVE, 0, 5);
+			dc_write_tmpl(dc, 2, WROD(0), 0, map, SYNC_WAVE, 8, 5, 1);
+			dc_write_tmpl(dc, 3, WROD(0), 0, map, SYNC_WAVE, 4, 5, 0);
+			dc_write_tmpl(dc, 4, WRG, 0, map, NULL_WAVE, 0, 0, 1);
+			dc_write_tmpl(dc, 1, WROD(0), 0, map, SYNC_WAVE, 0, 5, 1);
 		} else {
 			dc_link_event(dc, DC_EVT_NL, 5, 3);
 			dc_link_event(dc, DC_EVT_EOL, 6, 2);
-			dc_link_event(dc, DC_EVT_NEW_DATA, 7, 1);
+			dc_link_event(dc, DC_EVT_NEW_DATA, 8, 1);
 			/* Init template microcode */
-			dc_write_tmpl(dc, 5, WROD(0), 0, map, SYNC_WAVE, 8, 5);
-			dc_write_tmpl(dc, 6, WROD(0), 0, map, SYNC_WAVE, 4, 5);
-			dc_write_tmpl(dc, 7, WROD(0), 0, map, SYNC_WAVE, 0, 5);
+			dc_write_tmpl(dc, 5, WROD(0), 0, map, SYNC_WAVE, 8, 5, 1);
+			dc_write_tmpl(dc, 6, WROD(0), 0, map, SYNC_WAVE, 4, 5, 0);
+			dc_write_tmpl(dc, 7, WRG, 0, map, NULL_WAVE, 0, 0, 1);
+			dc_write_tmpl(dc, 8, WROD(0), 0, map, SYNC_WAVE, 0, 5, 1);
 		}
 	}
 	dc_link_event(dc, DC_EVT_NF, 0, 0);
@@ -364,6 +382,18 @@ int ipu_dc_init(struct ipu_soc *ipu, struct device *dev,
 	ipu_dc_map_config(priv, IPU_DC_MAP_RGB565, 1, 10, 0xfc); /* green */
 	ipu_dc_map_config(priv, IPU_DC_MAP_RGB565, 2, 15, 0xf8); /* red */
 
+	/* gbr24 */
+	ipu_dc_map_clear(priv, IPU_DC_MAP_GBR24);
+	ipu_dc_map_config(priv, IPU_DC_MAP_GBR24, 2, 15, 0xff); /* green */
+	ipu_dc_map_config(priv, IPU_DC_MAP_GBR24, 1, 7, 0xff); /* blue */
+	ipu_dc_map_config(priv, IPU_DC_MAP_GBR24, 0, 23, 0xff); /* red */
+
+	/* bgr666 */
+	ipu_dc_map_clear(priv, IPU_DC_MAP_BGR666);
+	ipu_dc_map_config(priv, IPU_DC_MAP_BGR666, 0, 5, 0xfc); /* blue */
+	ipu_dc_map_config(priv, IPU_DC_MAP_BGR666, 1, 11, 0xfc); /* green */
+	ipu_dc_map_config(priv, IPU_DC_MAP_BGR666, 2, 17, 0xfc); /* red */
+
 	return 0;
 }
 
diff --git a/drivers/staging/imx-drm/ipu-v3/ipu-di.c b/drivers/staging/imx-drm/ipu-v3/ipu-di.c
index ec340da968a..19d777e39d0 100644
--- a/drivers/staging/imx-drm/ipu-v3/ipu-di.c
+++ b/drivers/staging/imx-drm/ipu-v3/ipu-di.c
@@ -413,9 +413,11 @@ static void ipu_di_sync_config_noninterlaced(struct ipu_di *di,
 		sig->v_end_width;
 	struct di_sync_config cfg[] = {
 		{
+			/* 1: INT_HSYNC */
 			.run_count = h_total - 1,
 			.run_src = DI_SYNC_CLK,
 		} , {
+			/* PIN2: HSYNC */
 			.run_count = h_total - 1,
 			.run_src = DI_SYNC_CLK,
 			.offset_count = div * sig->v_to_h_sync,
@@ -424,36 +426,106 @@ static void ipu_di_sync_config_noninterlaced(struct ipu_di *di,
 			.cnt_polarity_trigger_src = DI_SYNC_CLK,
 			.cnt_down = sig->h_sync_width * 2,
 		} , {
+			/* PIN3: VSYNC */
 			.run_count = v_total - 1,
 			.run_src = DI_SYNC_INT_HSYNC,
 			.cnt_polarity_gen_en = 1,
 			.cnt_polarity_trigger_src = DI_SYNC_INT_HSYNC,
 			.cnt_down = sig->v_sync_width * 2,
 		} , {
+			/* 4: Line Active */
 			.run_src = DI_SYNC_HSYNC,
 			.offset_count = sig->v_sync_width + sig->v_start_width,
 			.offset_src = DI_SYNC_HSYNC,
 			.repeat_count = sig->height,
 			.cnt_clr_src = DI_SYNC_VSYNC,
 		} , {
+			/* 5: Pixel Active, referenced by DC */
 			.run_src = DI_SYNC_CLK,
 			.offset_count = sig->h_sync_width + sig->h_start_width,
 			.offset_src = DI_SYNC_CLK,
 			.repeat_count = sig->width,
-			.cnt_clr_src = 5,
+			.cnt_clr_src = 5, /* Line Active */
+		} , {
+			/* unused */
 		} , {
 			/* unused */
 		} , {
 			/* unused */
 		} , {
 			/* unused */
+		},
+	};
+	/* can't use #7 and #8 for line active and pixel active counters */
+	struct di_sync_config cfg_vga[] = {
+		{
+			/* 1: INT_HSYNC */
+			.run_count = h_total - 1,
+			.run_src = DI_SYNC_CLK,
+		} , {
+			/* 2: VSYNC */
+			.run_count = v_total - 1,
+			.run_src = DI_SYNC_INT_HSYNC,
+		} , {
+			/* 3: Line Active */
+			.run_src = DI_SYNC_INT_HSYNC,
+			.offset_count = sig->v_sync_width + sig->v_start_width,
+			.offset_src = DI_SYNC_INT_HSYNC,
+			.repeat_count = sig->height,
+			.cnt_clr_src = 3 /* VSYNC */,
+		} , {
+			/* PIN4: HSYNC for VGA via TVEv2 on TQ MBa53 */
+			.run_count = h_total - 1,
+			.run_src = DI_SYNC_CLK,
+			.offset_count = div * sig->v_to_h_sync + 18, /* magic value from Freescale TVE driver */
+			.offset_src = DI_SYNC_CLK,
+			.cnt_polarity_gen_en = 1,
+			.cnt_polarity_trigger_src = DI_SYNC_CLK,
+			.cnt_down = sig->h_sync_width * 2,
+		} , {
+			/* 5: Pixel Active signal to DC */
+			.run_src = DI_SYNC_CLK,
+			.offset_count = sig->h_sync_width + sig->h_start_width,
+			.offset_src = DI_SYNC_CLK,
+			.repeat_count = sig->width,
+			.cnt_clr_src = 4, /* Line Active */
+		} , {
+			/* PIN6: VSYNC for VGA via TVEv2 on TQ MBa53 */
+			.run_count = v_total - 1,
+			.run_src = DI_SYNC_INT_HSYNC,
+			.offset_count = 1, /* magic value from Freescale TVE driver */
+			.offset_src = DI_SYNC_INT_HSYNC,
+			.cnt_polarity_gen_en = 1,
+			.cnt_polarity_trigger_src = DI_SYNC_INT_HSYNC,
+			.cnt_down = sig->v_sync_width * 2,
+		} , {
+			/* PIN4: HSYNC for VGA via TVEv2 on i.MX53-QSB */
+			.run_count = h_total - 1,
+			.run_src = DI_SYNC_CLK,
+			.offset_count = div * sig->v_to_h_sync + 18, /* magic value from Freescale TVE driver */
+			.offset_src = DI_SYNC_CLK,
+			.cnt_polarity_gen_en = 1,
+			.cnt_polarity_trigger_src = DI_SYNC_CLK,
+			.cnt_down = sig->h_sync_width * 2,
+		} , {
+			/* PIN6: VSYNC for VGA via TVEv2 on i.MX53-QSB */
+			.run_count = v_total - 1,
+			.run_src = DI_SYNC_INT_HSYNC,
+			.offset_count = 1, /* magic value from Freescale TVE driver */
+			.offset_src = DI_SYNC_INT_HSYNC,
+			.cnt_polarity_gen_en = 1,
+			.cnt_polarity_trigger_src = DI_SYNC_INT_HSYNC,
+			.cnt_down = sig->v_sync_width * 2,
 		} , {
 			/* unused */
 		},
 	};
 
 	ipu_di_write(di, v_total - 1, DI_SCR_CONF);
-	ipu_di_sync_config(di, cfg, 0, ARRAY_SIZE(cfg));
+	if (sig->hsync_pin == 2 && sig->vsync_pin == 3)
+		ipu_di_sync_config(di, cfg, 0, ARRAY_SIZE(cfg));
+	else
+		ipu_di_sync_config(di, cfg_vga, 0, ARRAY_SIZE(cfg_vga));
 }
 
 int ipu_di_init_sync_panel(struct ipu_di *di, struct ipu_di_signal_cfg *sig)
@@ -530,11 +602,25 @@ int ipu_di_init_sync_panel(struct ipu_di *di, struct ipu_di_signal_cfg *sig)
 		ipu_di_sync_config_noninterlaced(di, sig, div);
 
 		vsync_cnt = 3;
-
-		if (sig->Hsync_pol)
-			di_gen |= DI_GEN_POLARITY_2;
-		if (sig->Vsync_pol)
-			di_gen |= DI_GEN_POLARITY_3;
+		if (di->id == 1)
+			vsync_cnt = 6;
+
+		if (sig->Hsync_pol) {
+			if (sig->hsync_pin == 2)
+				di_gen |= DI_GEN_POLARITY_2;
+			else if (sig->hsync_pin == 4)
+				di_gen |= DI_GEN_POLARITY_4;
+			else if (sig->hsync_pin == 7)
+				di_gen |= DI_GEN_POLARITY_7;
+		}
+		if (sig->Vsync_pol) {
+			if (sig->hsync_pin == 3)
+				di_gen |= DI_GEN_POLARITY_3;
+			else if (sig->hsync_pin == 6)
+				di_gen |= DI_GEN_POLARITY_6;
+			else if (sig->hsync_pin == 8)
+				di_gen |= DI_GEN_POLARITY_8;
+		}
 	}
 
 	if (!sig->clk_pol)
diff --git a/drivers/staging/imx-drm/ipu-v3/ipu-dp.c b/drivers/staging/imx-drm/ipu-v3/ipu-dp.c
index 26aecaf9677..113b046c022 100644
--- a/drivers/staging/imx-drm/ipu-v3/ipu-dp.c
+++ b/drivers/staging/imx-drm/ipu-v3/ipu-dp.c
@@ -316,7 +316,6 @@ int ipu_dp_init(struct ipu_soc *ipu, struct device *dev, unsigned long base)
 
 	priv->base = devm_ioremap(dev, base, PAGE_SIZE);
 	if (!priv->base) {
-		kfree(priv);
 		return -ENOMEM;
 	}
 
diff --git a/drivers/staging/imx-drm/ipuv3-crtc.c b/drivers/staging/imx-drm/ipuv3-crtc.c
index b028b0d1317..ea61c869110 100644
--- a/drivers/staging/imx-drm/ipuv3-crtc.c
+++ b/drivers/staging/imx-drm/ipuv3-crtc.c
@@ -60,6 +60,8 @@ struct ipu_crtc {
 	int			irq;
 	u32			interface_pix_fmt;
 	unsigned long		di_clkflags;
+	int			di_hsync_pin;
+	int			di_vsync_pin;
 };
 
 #define to_ipu_crtc(x) container_of(x, struct ipu_crtc, base)
@@ -255,6 +257,9 @@ static int ipu_crtc_mode_set(struct drm_crtc *crtc,
 
 	sig_cfg.v_to_h_sync = 0;
 
+	sig_cfg.hsync_pin = ipu_crtc->di_hsync_pin;
+	sig_cfg.vsync_pin = ipu_crtc->di_vsync_pin;
+
 	if (ipu_crtc->dp) {
 		ret = ipu_dp_setup_channel(ipu_crtc->dp, IPUV3_COLORSPACE_RGB,
 				IPUV3_COLORSPACE_RGB);
@@ -406,13 +411,17 @@ static void ipu_disable_vblank(struct drm_crtc *crtc)
 }
 
 static int ipu_set_interface_pix_fmt(struct drm_crtc *crtc, u32 encoder_type,
-		u32 pixfmt)
+		u32 pixfmt, int hsync_pin, int vsync_pin)
 {
 	struct ipu_crtc *ipu_crtc = to_ipu_crtc(crtc);
 
 	ipu_crtc->interface_pix_fmt = pixfmt;
+	ipu_crtc->di_hsync_pin = hsync_pin;
+	ipu_crtc->di_vsync_pin = vsync_pin;
 
 	switch (encoder_type) {
+	case DRM_MODE_ENCODER_DAC:
+	case DRM_MODE_ENCODER_TVDAC:
 	case DRM_MODE_ENCODER_LVDS:
 		ipu_crtc->di_clkflags = IPU_DI_CLKMODE_SYNC |
 			IPU_DI_CLKMODE_EXT;
diff --git a/drivers/staging/imx-drm/parallel-display.c b/drivers/staging/imx-drm/parallel-display.c
index a8064fcc03d..e7fba62c10e 100644
--- a/drivers/staging/imx-drm/parallel-display.c
+++ b/drivers/staging/imx-drm/parallel-display.c
@@ -57,6 +57,7 @@ static void imx_pd_connector_destroy(struct drm_connector *connector)
 static int imx_pd_connector_get_modes(struct drm_connector *connector)
 {
 	struct imx_parallel_display *imxpd = con_to_imxpd(connector);
+	struct device_node *np = imxpd->dev->of_node;
 	int num_modes = 0;
 
 	if (imxpd->edid) {
@@ -72,6 +73,15 @@ static int imx_pd_connector_get_modes(struct drm_connector *connector)
 		num_modes++;
 	}
 
+	if (np) {
+		struct drm_display_mode *mode = drm_mode_create(connector->dev);
+		of_get_drm_display_mode(np, &imxpd->mode, 0);
+		drm_mode_copy(mode, &imxpd->mode);
+		mode->type |= DRM_MODE_TYPE_DRIVER | DRM_MODE_TYPE_PREFERRED,
+		drm_mode_probed_add(connector, mode);
+		num_modes++;
+	}
+
 	return num_modes;
 }
 
@@ -220,6 +230,8 @@ static int imx_pd_probe(struct platform_device *pdev)
 			imxpd->interface_pix_fmt = V4L2_PIX_FMT_RGB24;
 		else if (!strcmp(fmt, "rgb565"))
 			imxpd->interface_pix_fmt = V4L2_PIX_FMT_RGB565;
+		else if (!strcmp(fmt, "bgr666"))
+			imxpd->interface_pix_fmt = V4L2_PIX_FMT_BGR666;
 	}
 
 	imxpd->dev = &pdev->dev;
diff --git a/drivers/staging/line6/pod.c b/drivers/staging/line6/pod.c
index 74898c3c9f9..699b2172506 100644
--- a/drivers/staging/line6/pod.c
+++ b/drivers/staging/line6/pod.c
@@ -148,9 +148,8 @@ void line6_pod_process_message(struct usb_line6_pod *pod)
 	    buf[0] != (LINE6_SYSEX_BEGIN | LINE6_CHANNEL_UNKNOWN)) {
 		return;
 	}
-	if (memcmp(buf + 1, line6_midi_id, sizeof(line6_midi_id)) != 0) {
+	if (memcmp(buf + 1, line6_midi_id, sizeof(line6_midi_id)) != 0)
 		return;
-	}
 
 	if (buf[5] == POD_SYSEX_SYSTEM && buf[6] == POD_MONITOR_LEVEL) {
 		short value = ((int)buf[7] << 12) | ((int)buf[8] << 8) |
diff --git a/drivers/staging/media/as102/Makefile b/drivers/staging/media/as102/Makefile
index d8dfb757f1e..8916d8a909b 100644
--- a/drivers/staging/media/as102/Makefile
+++ b/drivers/staging/media/as102/Makefile
@@ -3,4 +3,4 @@ dvb-as102-objs := as102_drv.o as102_fw.o as10x_cmd.o as10x_cmd_stream.o \
 
 obj-$(CONFIG_DVB_AS102) += dvb-as102.o
 
-EXTRA_CFLAGS += -Idrivers/media/dvb-core
+ccflags-y += -Idrivers/media/dvb-core
diff --git a/drivers/staging/media/davinci_vpfe/davinci-vpfe-mc.txt b/drivers/staging/media/davinci_vpfe/davinci-vpfe-mc.txt
index 1dbd5641870..a1e91778aa9 100644
--- a/drivers/staging/media/davinci_vpfe/davinci-vpfe-mc.txt
+++ b/drivers/staging/media/davinci_vpfe/davinci-vpfe-mc.txt
@@ -38,7 +38,7 @@ interface to userspace.
 	DAVINCI RESIZER A
 	DAVINCI RESIZER B
 
-Each possible link in the VPFE is modeled by a link in the Media controller
+Each possible link in the VPFE is modelled by a link in the Media controller
 interface. For an example program see [1].
 
 
diff --git a/drivers/staging/media/davinci_vpfe/dm365_ipipe.c b/drivers/staging/media/davinci_vpfe/dm365_ipipe.c
index 92853539cc0..05673ed45ce 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_ipipe.c
+++ b/drivers/staging/media/davinci_vpfe/dm365_ipipe.c
@@ -1859,5 +1859,5 @@ void vpfe_ipipe_cleanup(struct vpfe_ipipe_device *ipipe,
 	iounmap(ipipe->isp5_base_addr);
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 4);
 	if (res)
-		release_mem_region(res->start, res->end - res->start + 1);
+		release_mem_region(res->start, resource_size(res));
 }
diff --git a/drivers/staging/media/davinci_vpfe/dm365_ipipeif.c b/drivers/staging/media/davinci_vpfe/dm365_ipipeif.c
index c8cae51d3d0..b2f4ef84f3d 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_ipipeif.c
+++ b/drivers/staging/media/davinci_vpfe/dm365_ipipeif.c
@@ -1065,7 +1065,6 @@ vpfe_ipipeif_cleanup(struct vpfe_ipipeif_device *ipipeif,
 	iounmap(ipipeif->ipipeif_base_addr);
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 3);
 	if (res)
-		release_mem_region(res->start,
-					res->end - res->start + 1);
+		release_mem_region(res->start, resource_size(res));
 
 }
diff --git a/drivers/staging/media/davinci_vpfe/dm365_isif.c b/drivers/staging/media/davinci_vpfe/dm365_isif.c
index ebeea72e176..5829360f74c 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_isif.c
+++ b/drivers/staging/media/davinci_vpfe/dm365_isif.c
@@ -685,7 +685,7 @@ static void isif_config_bclamp(struct vpfe_isif_device *isif,
 	val = (bc->bc_mode_color & ISIF_BC_MODE_COLOR_MASK) <<
 		ISIF_BC_MODE_COLOR_SHIFT;
 
-	/* Enable BC and horizontal clamp caculation paramaters */
+	/* Enable BC and horizontal clamp calculation paramaters */
 	val = val | 1 | ((bc->horz.mode & ISIF_HORZ_BC_MODE_MASK) <<
 	      ISIF_HORZ_BC_MODE_SHIFT);
 
@@ -722,7 +722,7 @@ static void isif_config_bclamp(struct vpfe_isif_device *isif,
 		isif_write(isif->isif_cfg.base_addr, val, CLHWIN2);
 	}
 
-	/* vertical clamp caculation paramaters */
+	/* vertical clamp calculation paramaters */
 	/* OB H Valid */
 	val = bc->vert.ob_h_sz_calc & ISIF_VERT_BC_OB_H_SZ_MASK;
 
@@ -1569,7 +1569,7 @@ isif_pad_set_crop(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
 		crop->rect.width = format->width;
 		crop->rect.height = format->height;
 	}
-	/* adjust the width to 16 pixel boundry */
+	/* adjust the width to 16 pixel boundary */
 	crop->rect.width = ((crop->rect.width + 15) & ~0xf);
 	vpfe_isif->crop = crop->rect;
 	if (crop->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
@@ -1953,7 +1953,7 @@ static void isif_remove(struct vpfe_isif_device *isif,
 		res = platform_get_resource(pdev, IORESOURCE_MEM, i);
 		if (res)
 			release_mem_region(res->start,
-					   res->end - res->start + 1);
+					   resource_size(res));
 		i++;
 	}
 }
@@ -2003,7 +2003,7 @@ int vpfe_isif_init(struct vpfe_isif_device *isif, struct platform_device *pdev)
 			status = -ENOENT;
 			goto fail_nobase_res;
 		}
-		res_len = res->end - res->start + 1;
+		res_len = resource_size(res);
 		res = request_mem_region(res->start, res_len, res->name);
 		if (!res) {
 			status = -EBUSY;
diff --git a/drivers/staging/media/davinci_vpfe/dm365_resizer.c b/drivers/staging/media/davinci_vpfe/dm365_resizer.c
index 9cb0262b9b3..126f84c4cb6 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_resizer.c
+++ b/drivers/staging/media/davinci_vpfe/dm365_resizer.c
@@ -1995,5 +1995,5 @@ vpfe_resizer_cleanup(struct vpfe_resizer_device *vpfe_rsz,
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 5);
 	if (res)
 		release_mem_region(res->start,
-					res->end - res->start + 1);
+					resource_size(res));
 }
diff --git a/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c b/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c
index 7b351717bcb..b88e1ddce22 100644
--- a/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c
+++ b/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c
@@ -243,7 +243,7 @@ static int vpfe_enable_clock(struct vpfe_device *vpfe_dev)
 
 		vpfe_dev->clks[i] =
 				clk_get(vpfe_dev->pdev, vpfe_cfg->clocks[i]);
-		if (vpfe_dev->clks[i] == NULL) {
+		if (IS_ERR(vpfe_dev->clks[i])) {
 			v4l2_err(vpfe_dev->pdev->driver,
 				"Failed to get clock %s\n",
 				vpfe_cfg->clocks[i]);
@@ -264,7 +264,7 @@ static int vpfe_enable_clock(struct vpfe_device *vpfe_dev)
 	return 0;
 out:
 	for (i = 0; i < vpfe_cfg->num_clocks; i++)
-		if (vpfe_dev->clks[i]) {
+		if (!IS_ERR(vpfe_dev->clks[i])) {
 			clk_disable_unprepare(vpfe_dev->clks[i]);
 			clk_put(vpfe_dev->clks[i]);
 		}
@@ -719,22 +719,4 @@ static struct platform_driver vpfe_driver = {
 	.remove = vpfe_remove,
 };
 
-/**
- * vpfe_init : This function registers device driver
- */
-static __init int vpfe_init(void)
-{
-	/* Register driver to the kernel */
-	return platform_driver_register(&vpfe_driver);
-}
-
-/**
- * vpfe_cleanup : This function un-registers device driver
- */
-static void vpfe_cleanup(void)
-{
-	platform_driver_unregister(&vpfe_driver);
-}
-
-module_init(vpfe_init);
-module_exit(vpfe_cleanup);
+module_platform_driver(vpfe_driver);
diff --git a/drivers/staging/media/davinci_vpfe/vpfe_video.c b/drivers/staging/media/davinci_vpfe/vpfe_video.c
index 99ccbebea59..ba913f1d955 100644
--- a/drivers/staging/media/davinci_vpfe/vpfe_video.c
+++ b/drivers/staging/media/davinci_vpfe/vpfe_video.c
@@ -357,7 +357,7 @@ static int vpfe_pipeline_disable(struct vpfe_pipeline *pipe)
  *
  * Set the pipeline to the given stream state.
  *
- * Return 0 if successfull, or the return value of the failed video::s_stream
+ * Return 0 if successful, or the return value of the failed video::s_stream
  * operation otherwise.
  */
 static int vpfe_pipeline_set_stream(struct vpfe_pipeline *pipe,
@@ -644,7 +644,7 @@ static int vpfe_g_fmt(struct file *file, void *priv,
  * fills v4l2_fmtdesc structure with output format set on adjacent subdev,
  * only one format is enumearted as subdevs are already configured
  *
- * Return 0 if successfull, error code otherwise
+ * Return 0 if successful, error code otherwise
  */
 static int vpfe_enum_fmt(struct file *file, void  *priv,
 				   struct v4l2_fmtdesc *fmt)
@@ -769,7 +769,7 @@ static int vpfe_try_fmt(struct file *file, void *priv,
  * fills v4l2_input structure with input available on media chain,
  * only one input is enumearted as media chain is setup by this time
  *
- * Return 0 if successfull, -EINVAL is media chain is invalid
+ * Return 0 if successful, -EINVAL is media chain is invalid
  */
 static int vpfe_enum_input(struct file *file, void *priv,
 				 struct v4l2_input *inp)
@@ -779,7 +779,7 @@ static int vpfe_enum_input(struct file *file, void *priv,
 	struct vpfe_device *vpfe_dev = video->vpfe_dev;
 
 	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_enum_input\n");
-	/* enumerate from the subdev user has choosen through mc */
+	/* enumerate from the subdev user has chosen through mc */
 	if (inp->index < sdinfo->num_inputs) {
 		memcpy(inp, &sdinfo->inputs[inp->index],
 		       sizeof(struct v4l2_input));
@@ -924,7 +924,7 @@ static int vpfe_querystd(struct file *file, void *priv, v4l2_std_id *std_id)
  *
  * Return 0 on success, error code otherwise
  */
-static int vpfe_s_std(struct file *file, void *priv, v4l2_std_id *std_id)
+static int vpfe_s_std(struct file *file, void *priv, v4l2_std_id std_id)
 {
 	struct vpfe_video_device *video = video_drvdata(file);
 	struct vpfe_device *vpfe_dev = video->vpfe_dev;
@@ -945,13 +945,13 @@ static int vpfe_s_std(struct file *file, void *priv, v4l2_std_id *std_id)
 		goto unlock_out;
 	}
 	ret = v4l2_device_call_until_err(&vpfe_dev->v4l2_dev, sdinfo->grp_id,
-					 core, s_std, *std_id);
+					 core, s_std, std_id);
 	if (ret < 0) {
 		v4l2_err(&vpfe_dev->v4l2_dev, "Failed to set standard\n");
 		video->stdid = V4L2_STD_UNKNOWN;
 		goto unlock_out;
 	}
-	video->stdid = *std_id;
+	video->stdid = std_id;
 unlock_out:
 	mutex_unlock(&video->lock);
 	return ret;
@@ -1016,12 +1016,12 @@ vpfe_query_dv_timings(struct file *file, void *fh,
 }
 
 /*
- * vpfe_s_dv_timings() - set dv_preset on external subdev
+ * vpfe_s_dv_timings() - set dv_timings on external subdev
  * @file: file pointer
  * @priv: void pointer
  * @timings: pointer to v4l2_dv_timings structure
  *
- * set dv_timings pointed by preset on external subdev through
+ * set dv_timings pointed by timings on external subdev through
  * v4l2_device_call_until_err, this configures amplifier also
  *
  * Return 0 on success, error code otherwise
@@ -1042,12 +1042,12 @@ vpfe_s_dv_timings(struct file *file, void *fh,
 }
 
 /*
- * vpfe_g_dv_timings() - get dv_preset which is set on external subdev
+ * vpfe_g_dv_timings() - get dv_timings which is set on external subdev
  * @file: file pointer
  * @priv: void pointer
  * @timings: pointer to v4l2_dv_timings structure
  *
- * get dv_preset which is set on external subdev through
+ * get dv_timings which is set on external subdev through
  * v4l2_subdev_call
  *
  * Return 0 on success, error code otherwise
@@ -1423,7 +1423,7 @@ static int vpfe_dqbuf(struct file *file, void *priv,
 }
 
 /*
- * vpfe_streamon() - get dv_preset which is set on external subdev
+ * vpfe_streamon() - start streaming
  * @file: file pointer
  * @priv: void pointer
  * @buf_type: enum v4l2_buf_type
@@ -1472,7 +1472,7 @@ static int vpfe_streamon(struct file *file, void *priv,
 }
 
 /*
- * vpfe_streamoff() - get dv_preset which is set on external subdev
+ * vpfe_streamoff() - stop streaming
  * @file: file pointer
  * @priv: void pointer
  * @buf_type: enum v4l2_buf_type
diff --git a/drivers/staging/media/davinci_vpfe/vpfe_video.h b/drivers/staging/media/davinci_vpfe/vpfe_video.h
index bf8af01d4a1..df0aeec8b58 100644
--- a/drivers/staging/media/davinci_vpfe/vpfe_video.h
+++ b/drivers/staging/media/davinci_vpfe/vpfe_video.h
@@ -138,7 +138,7 @@ struct vpfe_video_device {
 	v4l2_std_id				stdid;
 	/*
 	 * offset where second field starts from the starting of the
-	 * buffer for field seperated YCbCr formats
+	 * buffer for field separated YCbCr formats
 	 */
 	u32					field_off;
 };
diff --git a/drivers/staging/media/dt3155v4l/dt3155v4l.c b/drivers/staging/media/dt3155v4l/dt3155v4l.c
index e33b7f55d84..c32e0acde4f 100644
--- a/drivers/staging/media/dt3155v4l/dt3155v4l.c
+++ b/drivers/staging/media/dt3155v4l/dt3155v4l.c
@@ -26,6 +26,7 @@
 #include <linux/slab.h>
 #include <media/v4l2-dev.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-common.h>
 #include <media/videobuf2-dma-contig.h>
 
 #include "dt3155v4l.h"
@@ -341,7 +342,7 @@ dt3155_irq_handler_even(int irq, void *dev_id)
 
 	spin_lock(&ipd->lock);
 	if (ipd->curr_buf) {
-		do_gettimeofday(&ipd->curr_buf->v4l2_buf.timestamp);
+		v4l2_get_timestamp(&ipd->curr_buf->v4l2_buf.timestamp);
 		ipd->curr_buf->v4l2_buf.sequence = (ipd->field_count) >> 1;
 		vb2_buffer_done(ipd->curr_buf, VB2_BUF_STATE_DONE);
 	}
@@ -390,6 +391,7 @@ dt3155_open(struct file *filp)
 			goto err_alloc_queue;
 		}
 		pd->q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		pd->q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 		pd->q->io_modes = VB2_READ | VB2_MMAP;
 		pd->q->ops = &q_ops;
 		pd->q->mem_ops = &vb2_dma_contig_memops;
@@ -398,7 +400,7 @@ dt3155_open(struct file *filp)
 		pd->field_count = 0;
 		ret = vb2_queue_init(pd->q);
 		if (ret < 0)
-			return ret;
+			goto err_request_irq;
 		INIT_LIST_HEAD(&pd->dmaq);
 		spin_lock_init(&pd->lock);
 		/* disable all irqs, clear all irq flags */
@@ -410,6 +412,7 @@ dt3155_open(struct file *filp)
 			goto err_request_irq;
 	}
 	pd->users++;
+	mutex_unlock(&pd->mux);
 	return 0; /* success */
 err_request_irq:
 	kfree(pd->q);
@@ -612,9 +615,9 @@ dt3155_ioc_g_std(struct file *filp, void *p, v4l2_std_id *norm)
 }
 
 static int
-dt3155_ioc_s_std(struct file *filp, void *p, v4l2_std_id *norm)
+dt3155_ioc_s_std(struct file *filp, void *p, v4l2_std_id norm)
 {
-	if (*norm & DT3155_CURRENT_NORM)
+	if (norm & DT3155_CURRENT_NORM)
 		return 0;
 	return -EINVAL;
 }
diff --git a/drivers/staging/media/go7007/Kconfig b/drivers/staging/media/go7007/Kconfig
index 7dfb2815b9e..04bd0fba7b1 100644
--- a/drivers/staging/media/go7007/Kconfig
+++ b/drivers/staging/media/go7007/Kconfig
@@ -1,20 +1,25 @@
 config VIDEO_GO7007
 	tristate "WIS GO7007 MPEG encoder support"
-	depends on VIDEO_DEV && PCI && I2C
-	depends on SND
-	select VIDEOBUF_DMA_SG
-	depends on RC_CORE
+	depends on VIDEO_DEV && I2C
+	depends on SND && USB
+	select VIDEOBUF2_VMALLOC
 	select VIDEO_TUNER
-	select VIDEO_TVEEPROM
+	select CYPRESS_FIRMWARE
 	select SND_PCM
-	select CRC32
+	select VIDEO_SONY_BTF_MPX if MEDIA_SUBDRV_AUTOSELECT
+	select VIDEO_SAA711X if MEDIA_SUBDRV_AUTOSELECT
+	select VIDEO_TW2804 if MEDIA_SUBDRV_AUTOSELECT
+	select VIDEO_TW9903 if MEDIA_SUBDRV_AUTOSELECT
+	select VIDEO_TW9906 if MEDIA_SUBDRV_AUTOSELECT
+	select VIDEO_OV7640 if MEDIA_SUBDRV_AUTOSELECT
+	select VIDEO_UDA1342 if MEDIA_SUBDRV_AUTOSELECT
 	default N
 	---help---
 	  This is a video4linux driver for the WIS GO7007 MPEG
 	  encoder chip.
 
 	  To compile this driver as a module, choose M here: the
-	  module will be called go7007
+	  module will be called go7007.
 
 config VIDEO_GO7007_USB
 	tristate "WIS GO7007 USB support"
@@ -25,85 +30,25 @@ config VIDEO_GO7007_USB
 	  encoder chip over USB.
 
 	  To compile this driver as a module, choose M here: the
-	  module will be called go7007-usb
+	  module will be called go7007-usb.
 
-config VIDEO_GO7007_USB_S2250_BOARD
-	tristate "Sensoray 2250/2251 support"
-	depends on VIDEO_GO7007_USB && DVB_USB
-	default N
-	---help---
-	  This is a video4linux driver for the Sensoray 2250/2251 device.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called s2250
-
-config VIDEO_GO7007_OV7640
-	tristate "OV7640 subdev support"
-	depends on VIDEO_GO7007
-	default N
-	---help---
-	  This is a video4linux driver for the OV7640 sub-device.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called wis-ov7640
-
-config VIDEO_GO7007_SAA7113
-	tristate "SAA7113 subdev support"
-	depends on VIDEO_GO7007
-	default N
-	---help---
-	  This is a video4linux driver for the SAA7113 sub-device.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called wis-saa7113
-
-config VIDEO_GO7007_SAA7115
-	tristate "SAA7115 subdev support"
-	depends on VIDEO_GO7007
-	default N
-	---help---
-	  This is a video4linux driver for the SAA7115 sub-device.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called wis-saa7115
-
-config VIDEO_GO7007_TW9903
-	tristate "TW9903 subdev support"
-	depends on VIDEO_GO7007
-	default N
-	---help---
-	  This is a video4linux driver for the TW9903 sub-device.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called wis-tw9903
-
-config VIDEO_GO7007_UDA1342
-	tristate "UDA1342 subdev support"
-	depends on VIDEO_GO7007
-	default N
-	---help---
-	  This is a video4linux driver for the UDA1342 sub-device.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called wis-uda1342
-
-config VIDEO_GO7007_SONY_TUNER
-	tristate "Sony tuner subdev support"
+config VIDEO_GO7007_LOADER
+	tristate "WIS GO7007 Loader support"
 	depends on VIDEO_GO7007
-	default N
+	default y
 	---help---
-	  This is a video4linux driver for the Sony Tuner sub-device.
+	  This is a go7007 firmware loader driver for the WIS GO7007
+	  MPEG encoder chip over USB.
 
 	  To compile this driver as a module, choose M here: the
-	  module will be called wis-sony-tuner
+	  module will be called go7007-loader.
 
-config VIDEO_GO7007_TW2804
-	tristate "TW2804 subdev support"
-	depends on VIDEO_GO7007
+config VIDEO_GO7007_USB_S2250_BOARD
+	tristate "Sensoray 2250/2251 support"
+	depends on VIDEO_GO7007_USB && USB
 	default N
 	---help---
-	  This is a video4linux driver for the TW2804 sub-device.
+	  This is a video4linux driver for the Sensoray 2250/2251 device.
 
 	  To compile this driver as a module, choose M here: the
-	  module will be called wis-tw2804
-
+	  module will be called s2250.
diff --git a/drivers/staging/media/go7007/Makefile b/drivers/staging/media/go7007/Makefile
index 3fdbef5306a..9c6ad4a263e 100644
--- a/drivers/staging/media/go7007/Makefile
+++ b/drivers/staging/media/go7007/Makefile
@@ -1,18 +1,7 @@
-#obj-m += go7007.o go7007-usb.o snd-go7007.o wis-saa7115.o wis-tw9903.o \
-		wis-uda1342.o wis-sony-tuner.o wis-saa7113.o wis-ov7640.o \
-		wis-tw2804.o
-
-
 obj-$(CONFIG_VIDEO_GO7007) += go7007.o
 obj-$(CONFIG_VIDEO_GO7007_USB) += go7007-usb.o
-obj-$(CONFIG_VIDEO_GO7007_USB_S2250_BOARD) += s2250.o s2250-loader.o
-obj-$(CONFIG_VIDEO_GO7007_SAA7113) += wis-saa7113.o
-obj-$(CONFIG_VIDEO_GO7007_OV7640) += wis-ov7640.o
-obj-$(CONFIG_VIDEO_GO7007_SAA7115) += wis-saa7115.o
-obj-$(CONFIG_VIDEO_GO7007_TW9903) += wis-tw9903.o
-obj-$(CONFIG_VIDEO_GO7007_UDA1342) += wis-uda1342.o
-obj-$(CONFIG_VIDEO_GO7007_SONY_TUNER) += wis-sony-tuner.o
-obj-$(CONFIG_VIDEO_GO7007_TW2804) += wis-tw2804.o
+obj-$(CONFIG_VIDEO_GO7007_LOADER) += go7007-loader.o
+obj-$(CONFIG_VIDEO_GO7007_USB_S2250_BOARD) += s2250.o
 
 go7007-y := go7007-v4l2.o go7007-driver.o go7007-i2c.o go7007-fw.o \
 		snd-go7007.o
@@ -21,10 +10,6 @@ s2250-y := s2250-board.o
 
 # Uncomment when the saa7134 patches get into upstream
 #obj-$(CONFIG_VIDEO_SAA7134) += saa7134-go7007.o
-#ccflags-$(CONFIG_VIDEO_SAA7134:m=y) += -Idrivers/media/video/saa7134 -DSAA7134_MPEG_GO7007=3
-
-# S2250 needs cypress ezusb loader from dvb-usb
-ccflags-$(CONFIG_VIDEO_GO7007_USB_S2250_BOARD:m=y) += -Idrivers/media/usb/dvb-usb
+#ccflags-$(CONFIG_VIDEO_SAA7134:m=y) += -Idrivers/media/pci/saa7134
 
-ccflags-y += -Idrivers/media/dvb-frontends
-ccflags-y += -Idrivers/media/dvb-core
+ccflags-$(CONFIG_VIDEO_GO7007_LOADER:m=y) += -Idrivers/media/common
diff --git a/drivers/staging/media/go7007/README b/drivers/staging/media/go7007/README
index aeba1324a9c..3af0d906281 100644
--- a/drivers/staging/media/go7007/README
+++ b/drivers/staging/media/go7007/README
@@ -1,11 +1,137 @@
 Todo:
-	- checkpatch.pl cleanups
-	- sparse cleanups
-	- lots of little modules, should be merged together
-	  and added to the build.
-	- testing?
-	- handle churn in v4l layer.
+	- create an API for motion detection
+	- let s2250-board use i2c subdevs as well instead of hardcoding
+	  support for the i2c devices.
+	- when the driver is moved out of staging, support for saa7134-go7007
+	  should be added to the saa7134 driver. The patch for that is
+	  included below.
 
-Please send patches to Greg Kroah-Hartman <greg@linuxfoundation.org> and Cc: Ross
-Cohen <rcohen@snurgle.org> as well.
+Patch for saa7134:
 
+diff --git a/drivers/media/pci/saa7134/saa7134-cards.c b/drivers/media/pci/saa7134/saa7134-cards.c
+index dc68cf1..9a53794 100644
+--- a/drivers/media/pci/saa7134/saa7134-cards.c
++++ b/drivers/media/pci/saa7134/saa7134-cards.c
+@@ -5790,6 +5790,29 @@ struct saa7134_board saa7134_boards[] = {
+ 			.gpio = 0x6010000,
+ 		} },
+ 	},
++	[SAA7134_BOARD_WIS_VOYAGER] = {
++		.name           = "WIS Voyager or compatible",
++		.audio_clock    = 0x00200000,
++		.tuner_type	= TUNER_PHILIPS_TDA8290,
++		.radio_type     = UNSET,
++		.tuner_addr     = ADDR_UNSET,
++		.radio_addr     = ADDR_UNSET,
++		.mpeg		= SAA7134_MPEG_GO7007,
++		.inputs		= { {
++			.name = name_comp1,
++			.vmux = 0,
++			.amux = LINE2,
++		}, {
++			.name = name_tv,
++			.vmux = 3,
++			.amux = TV,
++			.tv   = 1,
++		}, {
++			.name = name_svideo,
++			.vmux = 6,
++		.amux = LINE1,
++		} },
++	},
+ 
+ };
+ 
+@@ -7037,6 +7060,12 @@ struct pci_device_id saa7134_pci_tbl[] = {
+ 		.subdevice    = 0x0911,
+ 		.driver_data  = SAA7134_BOARD_SENSORAY811_911,
+ 	}, {
++		.vendor       = PCI_VENDOR_ID_PHILIPS,
++		.device       = PCI_DEVICE_ID_PHILIPS_SAA7133,
++		.subvendor    = 0x1905, /* WIS */
++		.subdevice    = 0x7007,
++		.driver_data  = SAA7134_BOARD_WIS_VOYAGER,
++	}, {
+ 		/* --- boards without eeprom + subsystem ID --- */
+ 		.vendor       = PCI_VENDOR_ID_PHILIPS,
+ 		.device       = PCI_DEVICE_ID_PHILIPS_SAA7134,
+diff --git a/drivers/media/pci/saa7134/saa7134-core.c b/drivers/media/pci/saa7134/saa7134-core.c
+index 8fd24e7..0a849ea 100644
+--- a/drivers/media/pci/saa7134/saa7134-core.c
++++ b/drivers/media/pci/saa7134/saa7134-core.c
+@@ -156,6 +156,8 @@ static void request_module_async(struct work_struct *work){
+ 		request_module("saa7134-empress");
+ 	if (card_is_dvb(dev))
+ 		request_module("saa7134-dvb");
++	if (card_is_go7007(dev))
++		request_module("saa7134-go7007");
+ 	if (alsa) {
+ 		if (dev->pci->device != PCI_DEVICE_ID_PHILIPS_SAA7130)
+ 			request_module("saa7134-alsa");
+@@ -557,8 +559,12 @@ static irqreturn_t saa7134_irq(int irq, void *dev_id)
+ 			saa7134_irq_vbi_done(dev,status);
+ 
+ 		if ((report & SAA7134_IRQ_REPORT_DONE_RA2) &&
+-		    card_has_mpeg(dev))
+-			saa7134_irq_ts_done(dev,status);
++		    card_has_mpeg(dev)) {
++			if (dev->mops->irq_ts_done != NULL)
++				dev->mops->irq_ts_done(dev, status);
++			else
++				saa7134_irq_ts_done(dev, status);
++		}
+ 
+ 		if (report & SAA7134_IRQ_REPORT_GPIO16) {
+ 			switch (dev->has_remote) {
+diff --git a/drivers/media/pci/saa7134/saa7134.h b/drivers/media/pci/saa7134/saa7134.h
+index 62169dd..5fad39a 100644
+--- a/drivers/media/pci/saa7134/saa7134.h
++++ b/drivers/media/pci/saa7134/saa7134.h
+@@ -334,6 +334,7 @@ struct saa7134_card_ir {
+ #define SAA7134_BOARD_KWORLD_PC150U         189
+ #define SAA7134_BOARD_ASUSTeK_PS3_100      190
+ #define SAA7134_BOARD_HAWELL_HW_9004V1      191
++#define SAA7134_BOARD_WIS_VOYAGER           192
+ 
+ #define SAA7134_MAXBOARDS 32
+ #define SAA7134_INPUT_MAX 8
+@@ -364,6 +365,7 @@ enum saa7134_mpeg_type {
+ 	SAA7134_MPEG_UNUSED,
+ 	SAA7134_MPEG_EMPRESS,
+ 	SAA7134_MPEG_DVB,
++	SAA7134_MPEG_GO7007,
+ };
+ 
+ enum saa7134_mpeg_ts_type {
+@@ -403,6 +405,7 @@ struct saa7134_board {
+ #define card_has_radio(dev)   (NULL != saa7134_boards[dev->board].radio.name)
+ #define card_is_empress(dev)  (SAA7134_MPEG_EMPRESS == saa7134_boards[dev->board].mpeg)
+ #define card_is_dvb(dev)      (SAA7134_MPEG_DVB     == saa7134_boards[dev->board].mpeg)
++#define card_is_go7007(dev)   (SAA7134_MPEG_GO7007  == saa7134_boards[dev->board].mpeg)
+ #define card_has_mpeg(dev)    (SAA7134_MPEG_UNUSED  != saa7134_boards[dev->board].mpeg)
+ #define card(dev)             (saa7134_boards[dev->board])
+ #define card_in(dev,n)        (saa7134_boards[dev->board].inputs[n])
+@@ -535,6 +538,8 @@ struct saa7134_mpeg_ops {
+ 	int                        (*init)(struct saa7134_dev *dev);
+ 	int                        (*fini)(struct saa7134_dev *dev);
+ 	void                       (*signal_change)(struct saa7134_dev *dev);
++	void                       (*irq_ts_done)(struct saa7134_dev *dev,
++						  unsigned long status);
+ };
+ 
+ /* global device status */
+diff --git a/drivers/staging/media/go7007/Makefile b/drivers/staging/media/go7007/Makefile
+index 9c6ad4a..1b23689 100644
+--- a/drivers/staging/media/go7007/Makefile
++++ b/drivers/staging/media/go7007/Makefile
+@@ -8,8 +8,7 @@ go7007-y := go7007-v4l2.o go7007-driver.o go7007-i2c.o go7007-fw.o \
+ 
+ s2250-y := s2250-board.o
+ 
+-# Uncomment when the saa7134 patches get into upstream
+-#obj-$(CONFIG_VIDEO_SAA7134) += saa7134-go7007.o
+-#ccflags-$(CONFIG_VIDEO_SAA7134:m=y) += -Idrivers/media/pci/saa7134
++obj-$(CONFIG_VIDEO_SAA7134) += saa7134-go7007.o
++ccflags-$(CONFIG_VIDEO_SAA7134:m=y) += -Idrivers/media/pci/saa7134
+ 
+ ccflags-$(CONFIG_VIDEO_GO7007_LOADER:m=y) += -Idrivers/media/common
diff --git a/drivers/staging/media/go7007/go7007-driver.c b/drivers/staging/media/go7007/go7007-driver.c
index 66950916df2..a5ca99d7c0b 100644
--- a/drivers/staging/media/go7007/go7007-driver.c
+++ b/drivers/staging/media/go7007/go7007-driver.c
@@ -35,7 +35,6 @@
 #include <media/v4l2-common.h>
 
 #include "go7007-priv.h"
-#include "wis-i2c.h"
 
 /*
  * Wait for an interrupt to be delivered from the GO7007SB and return
@@ -91,43 +90,43 @@ EXPORT_SYMBOL(go7007_read_addr);
 static int go7007_load_encoder(struct go7007 *go)
 {
 	const struct firmware *fw_entry;
-	char fw_name[] = "go7007fw.bin";
+	char fw_name[] = "go7007/go7007fw.bin";
 	void *bounce;
 	int fw_len, rv = 0;
 	u16 intr_val, intr_data;
 
-	if (request_firmware(&fw_entry, fw_name, go->dev)) {
-		v4l2_err(go, "unable to load firmware from file "
-			"\"%s\"\n", fw_name);
-		return -1;
-	}
-	if (fw_entry->size < 16 || memcmp(fw_entry->data, "WISGO7007FW", 11)) {
-		v4l2_err(go, "file \"%s\" does not appear to be "
-				"go7007 firmware\n", fw_name);
-		release_firmware(fw_entry);
-		return -1;
-	}
-	fw_len = fw_entry->size - 16;
-	bounce = kmemdup(fw_entry->data + 16, fw_len, GFP_KERNEL);
-	if (bounce == NULL) {
-		v4l2_err(go, "unable to allocate %d bytes for "
-				"firmware transfer\n", fw_len);
+	if (go->boot_fw == NULL) {
+		if (request_firmware(&fw_entry, fw_name, go->dev)) {
+			v4l2_err(go, "unable to load firmware from file \"%s\"\n", fw_name);
+			return -1;
+		}
+		if (fw_entry->size < 16 || memcmp(fw_entry->data, "WISGO7007FW", 11)) {
+			v4l2_err(go, "file \"%s\" does not appear to be go7007 firmware\n", fw_name);
+			release_firmware(fw_entry);
+			return -1;
+		}
+		fw_len = fw_entry->size - 16;
+		bounce = kmemdup(fw_entry->data + 16, fw_len, GFP_KERNEL);
+		if (bounce == NULL) {
+			v4l2_err(go, "unable to allocate %d bytes for firmware transfer\n", fw_len);
+			release_firmware(fw_entry);
+			return -1;
+		}
 		release_firmware(fw_entry);
-		return -1;
+		go->boot_fw_len = fw_len;
+		go->boot_fw = bounce;
 	}
-	release_firmware(fw_entry);
 	if (go7007_interface_reset(go) < 0 ||
-			go7007_send_firmware(go, bounce, fw_len) < 0 ||
-			go7007_read_interrupt(go, &intr_val, &intr_data) < 0 ||
+	    go7007_send_firmware(go, go->boot_fw, go->boot_fw_len) < 0 ||
+	    go7007_read_interrupt(go, &intr_val, &intr_data) < 0 ||
 			(intr_val & ~0x1) != 0x5a5a) {
 		v4l2_err(go, "error transferring firmware\n");
 		rv = -1;
 	}
-	kfree(bounce);
 	return rv;
 }
 
-MODULE_FIRMWARE("go7007fw.bin");
+MODULE_FIRMWARE("go7007/go7007fw.bin");
 
 /*
  * Boot the encoder and register the I2C adapter if requested.  Do the
@@ -167,15 +166,24 @@ static int go7007_init_encoder(struct go7007 *go)
 		go7007_write_addr(go, 0x1000, 0x0811);
 		go7007_write_addr(go, 0x1000, 0x0c11);
 	}
-	if (go->board_id == GO7007_BOARDID_MATRIX_REV) {
+	switch (go->board_id) {
+	case GO7007_BOARDID_MATRIX_REV:
 		/* Set GPIO pin 0 to be an output (audio clock control) */
 		go7007_write_addr(go, 0x3c82, 0x0001);
 		go7007_write_addr(go, 0x3c80, 0x00fe);
-	}
-	if (go->board_id == GO7007_BOARDID_ADLINK_MPG24) {
+		break;
+	case GO7007_BOARDID_ADLINK_MPG24:
 		/* set GPIO5 to be an output, currently low */
 		go7007_write_addr(go, 0x3c82, 0x0000);
 		go7007_write_addr(go, 0x3c80, 0x00df);
+		break;
+	case GO7007_BOARDID_ADS_USBAV_709:
+		/* GPIO pin 0: audio clock control */
+		/*      pin 2: TW9906 reset */
+		/*      pin 3: capture LED */
+		go7007_write_addr(go, 0x3c82, 0x000d);
+		go7007_write_addr(go, 0x3c80, 0x00f2);
+		break;
 	}
 	return 0;
 }
@@ -196,18 +204,54 @@ int go7007_reset_encoder(struct go7007 *go)
 /*
  * Attempt to instantiate an I2C client by ID, probably loading a module.
  */
-static int init_i2c_module(struct i2c_adapter *adapter, const char *type,
-			   int addr)
+static int init_i2c_module(struct i2c_adapter *adapter, const struct go_i2c *const i2c)
 {
 	struct go7007 *go = i2c_get_adapdata(adapter);
 	struct v4l2_device *v4l2_dev = &go->v4l2_dev;
-
-	if (v4l2_i2c_new_subdev(v4l2_dev, adapter, type, addr, NULL))
+	struct v4l2_subdev *sd;
+	struct i2c_board_info info;
+
+	memset(&info, 0, sizeof(info));
+	strlcpy(info.type, i2c->type, sizeof(info.type));
+	info.addr = i2c->addr;
+	info.flags = i2c->flags;
+
+	sd = v4l2_i2c_new_subdev_board(v4l2_dev, adapter, &info, NULL);
+	if (sd) {
+		if (i2c->is_video)
+			go->sd_video = sd;
+		if (i2c->is_audio)
+			go->sd_audio = sd;
 		return 0;
+	}
 
-	dev_info(&adapter->dev,
-		 "go7007: probing for module i2c:%s failed\n", type);
-	return -1;
+	printk(KERN_INFO "go7007: probing for module i2c:%s failed\n", i2c->type);
+	return -EINVAL;
+}
+
+/*
+ * Detach and unregister the encoder.  The go7007 struct won't be freed
+ * until v4l2 finishes releasing its resources and all associated fds are
+ * closed by applications.
+ */
+static void go7007_remove(struct v4l2_device *v4l2_dev)
+{
+	struct go7007 *go = container_of(v4l2_dev, struct go7007, v4l2_dev);
+
+	v4l2_device_unregister(v4l2_dev);
+	if (go->hpi_ops->release)
+		go->hpi_ops->release(go);
+	if (go->i2c_adapter_online) {
+		if (i2c_del_adapter(&go->i2c_adapter) == 0)
+			go->i2c_adapter_online = 0;
+		else
+			v4l2_err(&go->v4l2_dev,
+				"error removing I2C adapter!\n");
+	}
+
+	kfree(go->boot_fw);
+	go7007_v4l2_remove(go);
+	kfree(go);
 }
 
 /*
@@ -218,38 +262,63 @@ static int init_i2c_module(struct i2c_adapter *adapter, const char *type,
  *
  * Must NOT be called with the hw_lock held.
  */
-int go7007_register_encoder(struct go7007 *go)
+int go7007_register_encoder(struct go7007 *go, unsigned num_i2c_devs)
 {
 	int i, ret;
 
 	dev_info(go->dev, "go7007: registering new %s\n", go->name);
 
+	go->v4l2_dev.release = go7007_remove;
+	ret = v4l2_device_register(go->dev, &go->v4l2_dev);
+	if (ret < 0)
+		return ret;
+
 	mutex_lock(&go->hw_lock);
 	ret = go7007_init_encoder(go);
 	mutex_unlock(&go->hw_lock);
 	if (ret < 0)
-		return -1;
+		return ret;
 
-	/* v4l2 init must happen before i2c subdevs */
-	ret = go7007_v4l2_init(go);
+	ret = go7007_v4l2_ctrl_init(go);
 	if (ret < 0)
 		return ret;
 
 	if (!go->i2c_adapter_online &&
 			go->board_info->flags & GO7007_BOARD_USE_ONBOARD_I2C) {
-		if (go7007_i2c_init(go) < 0)
-			return -1;
+		ret = go7007_i2c_init(go);
+		if (ret < 0)
+			return ret;
 		go->i2c_adapter_online = 1;
 	}
 	if (go->i2c_adapter_online) {
-		for (i = 0; i < go->board_info->num_i2c_devs; ++i)
-			init_i2c_module(&go->i2c_adapter,
-					go->board_info->i2c_devs[i].type,
-					go->board_info->i2c_devs[i].addr);
+		if (go->board_id == GO7007_BOARDID_ADS_USBAV_709) {
+			/* Reset the TW9906 */
+			go7007_write_addr(go, 0x3c82, 0x0009);
+			msleep(50);
+			go7007_write_addr(go, 0x3c82, 0x000d);
+		}
+		for (i = 0; i < num_i2c_devs; ++i)
+			init_i2c_module(&go->i2c_adapter, &go->board_info->i2c_devs[i]);
+
+		if (go->tuner_type >= 0) {
+			struct tuner_setup setup = {
+				.addr = ADDR_UNSET,
+				.type = go->tuner_type,
+				.mode_mask = T_ANALOG_TV,
+			};
+
+			v4l2_device_call_all(&go->v4l2_dev, 0, tuner,
+				s_type_addr, &setup);
+		}
 		if (go->board_id == GO7007_BOARDID_ADLINK_MPG24)
-			i2c_clients_command(&go->i2c_adapter,
-				DECODER_SET_CHANNEL, &go->channel_number);
+			v4l2_subdev_call(go->sd_video, video, s_routing,
+					0, 0, go->channel_number + 1);
 	}
+
+	ret = go7007_v4l2_init(go);
+	if (ret < 0)
+		return ret;
+
 	if (go->board_info->flags & GO7007_BOARD_HAS_AUDIO) {
 		go->audio_enabled = 1;
 		go7007_snd_init(go);
@@ -309,48 +378,54 @@ start_error:
 /*
  * Store a byte in the current video buffer, if there is one.
  */
-static inline void store_byte(struct go7007_buffer *gobuf, u8 byte)
+static inline void store_byte(struct go7007_buffer *vb, u8 byte)
 {
-	if (gobuf != NULL && gobuf->bytesused < GO7007_BUF_SIZE) {
-		unsigned int pgidx = gobuf->offset >> PAGE_SHIFT;
-		unsigned int pgoff = gobuf->offset & ~PAGE_MASK;
+	if (vb && vb->vb.v4l2_planes[0].bytesused < GO7007_BUF_SIZE) {
+		u8 *ptr = vb2_plane_vaddr(&vb->vb, 0);
 
-		*((u8 *)page_address(gobuf->pages[pgidx]) + pgoff) = byte;
-		++gobuf->offset;
-		++gobuf->bytesused;
+		ptr[vb->vb.v4l2_planes[0].bytesused++] = byte;
 	}
 }
 
 /*
  * Deliver the last video buffer and get a new one to start writing to.
  */
-static void frame_boundary(struct go7007 *go)
+static struct go7007_buffer *frame_boundary(struct go7007 *go, struct go7007_buffer *vb)
 {
-	struct go7007_buffer *gobuf;
+	struct go7007_buffer *vb_tmp = NULL;
+	u32 *bytesused = &vb->vb.v4l2_planes[0].bytesused;
 	int i;
 
-	if (go->active_buf) {
-		if (go->active_buf->modet_active) {
-			if (go->active_buf->bytesused + 216 < GO7007_BUF_SIZE) {
+	if (vb) {
+		if (vb->modet_active) {
+			if (*bytesused + 216 < GO7007_BUF_SIZE) {
 				for (i = 0; i < 216; ++i)
-					store_byte(go->active_buf,
-							go->active_map[i]);
-				go->active_buf->bytesused -= 216;
+					store_byte(vb, go->active_map[i]);
+				*bytesused -= 216;
 			} else
-				go->active_buf->modet_active = 0;
+				vb->modet_active = 0;
 		}
-		go->active_buf->state = BUF_STATE_DONE;
-		wake_up_interruptible(&go->frame_waitq);
-		go->active_buf = NULL;
+		vb->vb.v4l2_buf.sequence = go->next_seq++;
+		v4l2_get_timestamp(&vb->vb.v4l2_buf.timestamp);
+		vb_tmp = vb;
+		spin_lock(&go->spinlock);
+		list_del(&vb->list);
+		if (list_empty(&go->vidq_active))
+			vb = NULL;
+		else
+			vb = list_first_entry(&go->vidq_active, struct go7007_buffer, list);
+		go->active_buf = vb;
+		spin_unlock(&go->spinlock);
+		vb2_buffer_done(&vb_tmp->vb, VB2_BUF_STATE_DONE);
+		return vb;
 	}
-	list_for_each_entry(gobuf, &go->stream, stream)
-		if (gobuf->state == BUF_STATE_QUEUED) {
-			gobuf->seq = go->next_seq;
-			do_gettimeofday(&gobuf->timestamp);
-			go->active_buf = gobuf;
-			break;
-		}
-	++go->next_seq;
+	spin_lock(&go->spinlock);
+	if (!list_empty(&go->vidq_active))
+		vb = go->active_buf =
+			list_first_entry(&go->vidq_active, struct go7007_buffer, list);
+	spin_unlock(&go->spinlock);
+	go->next_seq++;
+	return vb;
 }
 
 static void write_bitmap_word(struct go7007 *go)
@@ -374,30 +449,30 @@ static void write_bitmap_word(struct go7007 *go)
  */
 void go7007_parse_video_stream(struct go7007 *go, u8 *buf, int length)
 {
-	int i, seq_start_code = -1, frame_start_code = -1;
-
-	spin_lock(&go->spinlock);
+	struct go7007_buffer *vb = go->active_buf;
+	int i, seq_start_code = -1, gop_start_code = -1, frame_start_code = -1;
 
 	switch (go->format) {
-	case GO7007_FORMAT_MPEG4:
+	case V4L2_PIX_FMT_MPEG4:
 		seq_start_code = 0xB0;
+		gop_start_code = 0xB3;
 		frame_start_code = 0xB6;
 		break;
-	case GO7007_FORMAT_MPEG1:
-	case GO7007_FORMAT_MPEG2:
+	case V4L2_PIX_FMT_MPEG1:
+	case V4L2_PIX_FMT_MPEG2:
 		seq_start_code = 0xB3;
+		gop_start_code = 0xB8;
 		frame_start_code = 0x00;
 		break;
 	}
 
 	for (i = 0; i < length; ++i) {
-		if (go->active_buf != NULL &&
-			    go->active_buf->bytesused >= GO7007_BUF_SIZE - 3) {
+		if (vb && vb->vb.v4l2_planes[0].bytesused >= GO7007_BUF_SIZE - 3) {
 			v4l2_info(&go->v4l2_dev, "dropping oversized frame\n");
-			go->active_buf->offset -= go->active_buf->bytesused;
-			go->active_buf->bytesused = 0;
-			go->active_buf->modet_active = 0;
-			go->active_buf = NULL;
+			vb->vb.v4l2_planes[0].bytesused = 0;
+			vb->frame_offset = 0;
+			vb->modet_active = 0;
+			vb = go->active_buf = NULL;
 		}
 
 		switch (go->state) {
@@ -410,7 +485,7 @@ void go7007_parse_video_stream(struct go7007 *go, u8 *buf, int length)
 				go->state = STATE_FF;
 				break;
 			default:
-				store_byte(go->active_buf, buf[i]);
+				store_byte(vb, buf[i]);
 				break;
 			}
 			break;
@@ -420,12 +495,12 @@ void go7007_parse_video_stream(struct go7007 *go, u8 *buf, int length)
 				go->state = STATE_00_00;
 				break;
 			case 0xFF:
-				store_byte(go->active_buf, 0x00);
+				store_byte(vb, 0x00);
 				go->state = STATE_FF;
 				break;
 			default:
-				store_byte(go->active_buf, 0x00);
-				store_byte(go->active_buf, buf[i]);
+				store_byte(vb, 0x00);
+				store_byte(vb, buf[i]);
 				go->state = STATE_DATA;
 				break;
 			}
@@ -433,21 +508,21 @@ void go7007_parse_video_stream(struct go7007 *go, u8 *buf, int length)
 		case STATE_00_00:
 			switch (buf[i]) {
 			case 0x00:
-				store_byte(go->active_buf, 0x00);
+				store_byte(vb, 0x00);
 				/* go->state remains STATE_00_00 */
 				break;
 			case 0x01:
 				go->state = STATE_00_00_01;
 				break;
 			case 0xFF:
-				store_byte(go->active_buf, 0x00);
-				store_byte(go->active_buf, 0x00);
+				store_byte(vb, 0x00);
+				store_byte(vb, 0x00);
 				go->state = STATE_FF;
 				break;
 			default:
-				store_byte(go->active_buf, 0x00);
-				store_byte(go->active_buf, 0x00);
-				store_byte(go->active_buf, buf[i]);
+				store_byte(vb, 0x00);
+				store_byte(vb, 0x00);
+				store_byte(vb, buf[i]);
 				go->state = STATE_DATA;
 				break;
 			}
@@ -455,31 +530,26 @@ void go7007_parse_video_stream(struct go7007 *go, u8 *buf, int length)
 		case STATE_00_00_01:
 			if (buf[i] == 0xF8 && go->modet_enable == 0) {
 				/* MODET start code, but MODET not enabled */
-				store_byte(go->active_buf, 0x00);
-				store_byte(go->active_buf, 0x00);
-				store_byte(go->active_buf, 0x01);
-				store_byte(go->active_buf, 0xF8);
+				store_byte(vb, 0x00);
+				store_byte(vb, 0x00);
+				store_byte(vb, 0x01);
+				store_byte(vb, 0xF8);
 				go->state = STATE_DATA;
 				break;
 			}
 			/* If this is the start of a new MPEG frame,
 			 * get a new buffer */
-			if ((go->format == GO7007_FORMAT_MPEG1 ||
-					go->format == GO7007_FORMAT_MPEG2 ||
-					go->format == GO7007_FORMAT_MPEG4) &&
-					(buf[i] == seq_start_code ||
-						buf[i] == 0xB8 || /* GOP code */
-						buf[i] == frame_start_code)) {
-				if (go->active_buf == NULL || go->seen_frame)
-					frame_boundary(go);
-				if (buf[i] == frame_start_code) {
-					if (go->active_buf != NULL)
-						go->active_buf->frame_offset =
-							go->active_buf->offset;
-					go->seen_frame = 1;
-				} else {
-					go->seen_frame = 0;
-				}
+			if ((go->format == V4L2_PIX_FMT_MPEG1 ||
+			     go->format == V4L2_PIX_FMT_MPEG2 ||
+			     go->format == V4L2_PIX_FMT_MPEG4) &&
+			    (buf[i] == seq_start_code ||
+			     buf[i] == gop_start_code ||
+			     buf[i] == frame_start_code)) {
+				if (vb == NULL || go->seen_frame)
+					vb = frame_boundary(go, vb);
+				go->seen_frame = buf[i] == frame_start_code;
+				if (vb && go->seen_frame)
+					vb->frame_offset = vb->vb.v4l2_planes[0].bytesused;
 			}
 			/* Handle any special chunk types, or just write the
 			 * start code to the (potentially new) buffer */
@@ -498,16 +568,16 @@ void go7007_parse_video_stream(struct go7007 *go, u8 *buf, int length)
 				go->state = STATE_MODET_MAP;
 				break;
 			case 0xFF: /* Potential JPEG start code */
-				store_byte(go->active_buf, 0x00);
-				store_byte(go->active_buf, 0x00);
-				store_byte(go->active_buf, 0x01);
+				store_byte(vb, 0x00);
+				store_byte(vb, 0x00);
+				store_byte(vb, 0x01);
 				go->state = STATE_FF;
 				break;
 			default:
-				store_byte(go->active_buf, 0x00);
-				store_byte(go->active_buf, 0x00);
-				store_byte(go->active_buf, 0x01);
-				store_byte(go->active_buf, buf[i]);
+				store_byte(vb, 0x00);
+				store_byte(vb, 0x00);
+				store_byte(vb, 0x01);
+				store_byte(vb, buf[i]);
 				go->state = STATE_DATA;
 				break;
 			}
@@ -515,20 +585,20 @@ void go7007_parse_video_stream(struct go7007 *go, u8 *buf, int length)
 		case STATE_FF:
 			switch (buf[i]) {
 			case 0x00:
-				store_byte(go->active_buf, 0xFF);
+				store_byte(vb, 0xFF);
 				go->state = STATE_00;
 				break;
 			case 0xFF:
-				store_byte(go->active_buf, 0xFF);
+				store_byte(vb, 0xFF);
 				/* go->state remains STATE_FF */
 				break;
 			case 0xD8:
-				if (go->format == GO7007_FORMAT_MJPEG)
-					frame_boundary(go);
+				if (go->format == V4L2_PIX_FMT_MJPEG)
+					vb = frame_boundary(go, vb);
 				/* fall through */
 			default:
-				store_byte(go->active_buf, 0xFF);
-				store_byte(go->active_buf, buf[i]);
+				store_byte(vb, 0xFF);
+				store_byte(vb, buf[i]);
 				go->state = STATE_DATA;
 				break;
 			}
@@ -551,8 +621,8 @@ void go7007_parse_video_stream(struct go7007 *go, u8 *buf, int length)
 					write_bitmap_word(go);
 				} else
 					go->modet_word = buf[i] << 8;
-			} else if (go->parse_length == 207 && go->active_buf) {
-				go->active_buf->modet_active = buf[i];
+			} else if (go->parse_length == 207 && vb) {
+				vb->modet_active = buf[i];
 			}
 			if (++go->parse_length == 208)
 				go->state = STATE_DATA;
@@ -563,15 +633,14 @@ void go7007_parse_video_stream(struct go7007 *go, u8 *buf, int length)
 			break;
 		}
 	}
-
-	spin_unlock(&go->spinlock);
 }
 EXPORT_SYMBOL(go7007_parse_video_stream);
 
 /*
  * Allocate a new go7007 struct.  Used by the hardware-specific probe.
  */
-struct go7007 *go7007_alloc(struct go7007_board_info *board, struct device *dev)
+struct go7007 *go7007_alloc(const struct go7007_board_info *board,
+						struct device *dev)
 {
 	struct go7007 *go;
 	int i;
@@ -588,33 +657,17 @@ struct go7007 *go7007_alloc(struct go7007_board_info *board, struct device *dev)
 	mutex_init(&go->hw_lock);
 	init_waitqueue_head(&go->frame_waitq);
 	spin_lock_init(&go->spinlock);
-	go->video_dev = NULL;
-	go->ref_count = 0;
 	go->status = STATUS_INIT;
 	memset(&go->i2c_adapter, 0, sizeof(go->i2c_adapter));
 	go->i2c_adapter_online = 0;
 	go->interrupt_available = 0;
 	init_waitqueue_head(&go->interrupt_waitq);
-	go->in_use = 0;
 	go->input = 0;
-	if (board->sensor_flags & GO7007_SENSOR_TV) {
-		go->standard = GO7007_STD_NTSC;
-		go->width = 720;
-		go->height = 480;
-		go->sensor_framerate = 30000;
-	} else {
-		go->standard = GO7007_STD_OTHER;
-		go->width = board->sensor_width;
-		go->height = board->sensor_height;
-		go->sensor_framerate = board->sensor_framerate;
-	}
-	go->encoder_v_offset = board->sensor_v_offset;
-	go->encoder_h_offset = board->sensor_h_offset;
+	go7007_update_board(go);
 	go->encoder_h_halve = 0;
 	go->encoder_v_halve = 0;
 	go->encoder_subsample = 0;
-	go->streaming = 0;
-	go->format = GO7007_FORMAT_MJPEG;
+	go->format = V4L2_PIX_FMT_MJPEG;
 	go->bitrate = 1500000;
 	go->fps_scale = 1;
 	go->pali = 0;
@@ -633,31 +686,30 @@ struct go7007 *go7007_alloc(struct go7007_board_info *board, struct device *dev)
 		go->modet_map[i] = 0;
 	go->audio_deliver = NULL;
 	go->audio_enabled = 0;
-	INIT_LIST_HEAD(&go->stream);
 
 	return go;
 }
 EXPORT_SYMBOL(go7007_alloc);
 
-/*
- * Detach and unregister the encoder.  The go7007 struct won't be freed
- * until v4l2 finishes releasing its resources and all associated fds are
- * closed by applications.
- */
-void go7007_remove(struct go7007 *go)
+void go7007_update_board(struct go7007 *go)
 {
-	if (go->i2c_adapter_online) {
-		if (i2c_del_adapter(&go->i2c_adapter) == 0)
-			go->i2c_adapter_online = 0;
-		else
-			v4l2_err(&go->v4l2_dev,
-				"error removing I2C adapter!\n");
-	}
+	const struct go7007_board_info *board = go->board_info;
 
-	if (go->audio_enabled)
-		go7007_snd_remove(go);
-	go7007_v4l2_remove(go);
+	if (board->sensor_flags & GO7007_SENSOR_TV) {
+		go->standard = GO7007_STD_NTSC;
+		go->std = V4L2_STD_NTSC_M;
+		go->width = 720;
+		go->height = 480;
+		go->sensor_framerate = 30000;
+	} else {
+		go->standard = GO7007_STD_OTHER;
+		go->width = board->sensor_width;
+		go->height = board->sensor_height;
+		go->sensor_framerate = board->sensor_framerate;
+	}
+	go->encoder_v_offset = board->sensor_v_offset;
+	go->encoder_h_offset = board->sensor_h_offset;
 }
-EXPORT_SYMBOL(go7007_remove);
+EXPORT_SYMBOL(go7007_update_board);
 
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/media/go7007/go7007-fw.c b/drivers/staging/media/go7007/go7007-fw.c
index a5ede1c109d..c2d0e58afc3 100644
--- a/drivers/staging/media/go7007/go7007-fw.c
+++ b/drivers/staging/media/go7007/go7007-fw.c
@@ -36,6 +36,8 @@
 
 #include "go7007-priv.h"
 
+#define GO7007_FW_NAME "go7007/go7007tv.bin"
+
 /* Constants used in the source firmware image to describe code segments */
 
 #define	FLAG_MODE_MJPEG		(1)
@@ -455,9 +457,9 @@ static int mpeg1_frame_header(struct go7007 *go, unsigned char *buf,
 
 	CODE_ADD(c, frame == PFRAME ? 0x2 : 0x3, 13);
 	CODE_ADD(c, 0xffff, 16);
-	CODE_ADD(c, go->format == GO7007_FORMAT_MPEG2 ? 0x7 : 0x4, 4);
+	CODE_ADD(c, go->format == V4L2_PIX_FMT_MPEG2 ? 0x7 : 0x4, 4);
 	if (frame != PFRAME)
-		CODE_ADD(c, go->format == GO7007_FORMAT_MPEG2 ? 0x7 : 0x4, 4);
+		CODE_ADD(c, go->format == V4L2_PIX_FMT_MPEG2 ? 0x7 : 0x4, 4);
 	else
 		CODE_ADD(c, 0, 4); /* Is this supposed to be here?? */
 	CODE_ADD(c, 0, 3); /* What is this?? */
@@ -466,7 +468,7 @@ static int mpeg1_frame_header(struct go7007 *go, unsigned char *buf,
 	if (j != 8)
 		CODE_ADD(c, 0, j);
 
-	if (go->format == GO7007_FORMAT_MPEG2) {
+	if (go->format == V4L2_PIX_FMT_MPEG2) {
 		CODE_ADD(c, 0x1, 24);
 		CODE_ADD(c, 0xb5, 8);
 		CODE_ADD(c, 0x844, 12);
@@ -537,7 +539,7 @@ static int mpeg1_sequence_header(struct go7007 *go, unsigned char *buf, int ext)
 	int i, aspect_ratio, picture_rate;
 	CODE_GEN(c, buf + 6);
 
-	if (go->format == GO7007_FORMAT_MPEG1) {
+	if (go->format == V4L2_PIX_FMT_MPEG1) {
 		switch (go->aspect_ratio) {
 		case GO7007_RATIO_4_3:
 			aspect_ratio = go->standard == GO7007_STD_NTSC ? 3 : 2;
@@ -587,9 +589,9 @@ static int mpeg1_sequence_header(struct go7007 *go, unsigned char *buf, int ext)
 	CODE_ADD(c, go->height, 12);
 	CODE_ADD(c, aspect_ratio, 4);
 	CODE_ADD(c, picture_rate, 4);
-	CODE_ADD(c, go->format == GO7007_FORMAT_MPEG2 ? 20000 : 0x3ffff, 18);
+	CODE_ADD(c, go->format == V4L2_PIX_FMT_MPEG2 ? 20000 : 0x3ffff, 18);
 	CODE_ADD(c, 1, 1);
-	CODE_ADD(c, go->format == GO7007_FORMAT_MPEG2 ? 112 : 20, 10);
+	CODE_ADD(c, go->format == V4L2_PIX_FMT_MPEG2 ? 112 : 20, 10);
 	CODE_ADD(c, 0, 3);
 
 	/* Byte-align with zeros */
@@ -597,7 +599,7 @@ static int mpeg1_sequence_header(struct go7007 *go, unsigned char *buf, int ext)
 	if (i != 8)
 		CODE_ADD(c, 0, i);
 
-	if (go->format == GO7007_FORMAT_MPEG2) {
+	if (go->format == V4L2_PIX_FMT_MPEG2) {
 		CODE_ADD(c, 0x1, 24);
 		CODE_ADD(c, 0xb5, 8);
 		CODE_ADD(c, 0x148, 12);
@@ -930,10 +932,10 @@ static int brctrl_to_package(struct go7007 *go,
 					__le16 *code, int space, int *framelen)
 {
 	int converge_speed = 0;
-	int lambda = (go->format == GO7007_FORMAT_MJPEG || go->dvd_mode) ?
+	int lambda = (go->format == V4L2_PIX_FMT_MJPEG || go->dvd_mode) ?
 				100 : 0;
 	int peak_rate = 6 * go->bitrate / 5;
-	int vbv_buffer = go->format == GO7007_FORMAT_MJPEG ?
+	int vbv_buffer = go->format == V4L2_PIX_FMT_MJPEG ?
 				go->bitrate :
 				(go->dvd_mode ? 900000 : peak_rate);
 	int fps = go->sensor_framerate / go->fps_scale;
@@ -1096,10 +1098,10 @@ static int config_package(struct go7007 *go, __le16 *code, int space)
 		0xc003,		0x28b4,
 		0xc004,		0x3c5a,
 		0xdc05,		0x2a77,
-		0xc6c3,		go->format == GO7007_FORMAT_MPEG4 ? 0 :
-				(go->format == GO7007_FORMAT_H263 ? 0 : 1),
-		0xc680,		go->format == GO7007_FORMAT_MPEG4 ? 0xf1 :
-				(go->format == GO7007_FORMAT_H263 ? 0x61 :
+		0xc6c3,		go->format == V4L2_PIX_FMT_MPEG4 ? 0 :
+				(go->format == V4L2_PIX_FMT_H263 ? 0 : 1),
+		0xc680,		go->format == V4L2_PIX_FMT_MPEG4 ? 0xf1 :
+				(go->format == V4L2_PIX_FMT_H263 ? 0x61 :
 									0xd3),
 		0xc780,		0x0140,
 		0xe009,		0x0001,
@@ -1123,15 +1125,15 @@ static int config_package(struct go7007 *go, __le16 *code, int space)
 						(!go->interlace_coding) ?
 					0x0008 : 0x0009,
 		0xc404,		go->interlace_coding ? 0x44 :
-				(go->format == GO7007_FORMAT_MPEG4 ? 0x11 :
-				(go->format == GO7007_FORMAT_MPEG1 ? 0x02 :
-				(go->format == GO7007_FORMAT_MPEG2 ? 0x04 :
-				(go->format == GO7007_FORMAT_H263  ? 0x08 :
+				(go->format == V4L2_PIX_FMT_MPEG4 ? 0x11 :
+				(go->format == V4L2_PIX_FMT_MPEG1 ? 0x02 :
+				(go->format == V4L2_PIX_FMT_MPEG2 ? 0x04 :
+				(go->format == V4L2_PIX_FMT_H263  ? 0x08 :
 								     0x20)))),
-		0xbf0a,		(go->format == GO7007_FORMAT_MPEG4 ? 8 :
-				(go->format == GO7007_FORMAT_MPEG1 ? 1 :
-				(go->format == GO7007_FORMAT_MPEG2 ? 2 :
-				(go->format == GO7007_FORMAT_H263 ? 4 : 16)))) |
+		0xbf0a,		(go->format == V4L2_PIX_FMT_MPEG4 ? 8 :
+				(go->format == V4L2_PIX_FMT_MPEG1 ? 1 :
+				(go->format == V4L2_PIX_FMT_MPEG2 ? 2 :
+				(go->format == V4L2_PIX_FMT_H263 ? 4 : 16)))) |
 				((go->repeat_seqhead ? 1 : 0) << 6) |
 				((go->dvd_mode ? 1 : 0) << 9) |
 				((go->gop_header_enable ? 1 : 0) << 10),
@@ -1348,19 +1350,19 @@ static int final_package(struct go7007 *go, __le16 *code, int space)
 			0x41,
 		go->ipb ? 0xd4c : 0x36b,
 		(rows << 8) | (go->width >> 4),
-		go->format == GO7007_FORMAT_MPEG4 ? 0x0404 : 0,
+		go->format == V4L2_PIX_FMT_MPEG4 ? 0x0404 : 0,
 		(1 << 15) | ((go->interlace_coding ? 1 : 0) << 13) |
 			((go->closed_gop ? 1 : 0) << 12) |
-			((go->format == GO7007_FORMAT_MPEG4 ? 1 : 0) << 11) |
+			((go->format == V4L2_PIX_FMT_MPEG4 ? 1 : 0) << 11) |
 		/*	(1 << 9) |   */
 			((go->ipb ? 3 : 0) << 7) |
 			((go->modet_enable ? 1 : 0) << 2) |
 			((go->dvd_mode ? 1 : 0) << 1) | 1,
-		(go->format == GO7007_FORMAT_MPEG1 ? 0x89a0 :
-			(go->format == GO7007_FORMAT_MPEG2 ? 0x89a0 :
-			(go->format == GO7007_FORMAT_MJPEG ? 0x89a0 :
-			(go->format == GO7007_FORMAT_MPEG4 ? 0x8920 :
-			(go->format == GO7007_FORMAT_H263 ? 0x8920 : 0))))),
+		(go->format == V4L2_PIX_FMT_MPEG1 ? 0x89a0 :
+			(go->format == V4L2_PIX_FMT_MPEG2 ? 0x89a0 :
+			(go->format == V4L2_PIX_FMT_MJPEG ? 0x89a0 :
+			(go->format == V4L2_PIX_FMT_MPEG4 ? 0x8920 :
+			(go->format == V4L2_PIX_FMT_H263 ? 0x8920 : 0))))),
 		go->ipb ? 0x1f15 : 0x1f0b,
 		go->ipb ? 0x0015 : 0x000b,
 		go->ipb ? 0xa800 : 0x5800,
@@ -1503,13 +1505,13 @@ static int do_special(struct go7007 *go, u16 type, __le16 *code, int space,
 	switch (type) {
 	case SPECIAL_FRM_HEAD:
 		switch (go->format) {
-		case GO7007_FORMAT_MJPEG:
+		case V4L2_PIX_FMT_MJPEG:
 			return gen_mjpeghdr_to_package(go, code, space);
-		case GO7007_FORMAT_MPEG1:
-		case GO7007_FORMAT_MPEG2:
+		case V4L2_PIX_FMT_MPEG1:
+		case V4L2_PIX_FMT_MPEG2:
 			return gen_mpeg1hdr_to_package(go, code, space,
 								framelen);
-		case GO7007_FORMAT_MPEG4:
+		case V4L2_PIX_FMT_MPEG4:
 			return gen_mpeg4hdr_to_package(go, code, space,
 								framelen);
 		}
@@ -1519,11 +1521,11 @@ static int do_special(struct go7007 *go, u16 type, __le16 *code, int space,
 		return config_package(go, code, space);
 	case SPECIAL_SEQHEAD:
 		switch (go->format) {
-		case GO7007_FORMAT_MPEG1:
-		case GO7007_FORMAT_MPEG2:
+		case V4L2_PIX_FMT_MPEG1:
+		case V4L2_PIX_FMT_MPEG2:
 			return seqhead_to_package(go, code, space,
 					mpeg1_sequence_header);
-		case GO7007_FORMAT_MPEG4:
+		case V4L2_PIX_FMT_MPEG4:
 			return seqhead_to_package(go, code, space,
 					mpeg4_sequence_header);
 		default:
@@ -1553,25 +1555,25 @@ int go7007_construct_fw_image(struct go7007 *go, u8 **fw, int *fwlen)
 	int ret;
 
 	switch (go->format) {
-	case GO7007_FORMAT_MJPEG:
+	case V4L2_PIX_FMT_MJPEG:
 		mode_flag = FLAG_MODE_MJPEG;
 		break;
-	case GO7007_FORMAT_MPEG1:
+	case V4L2_PIX_FMT_MPEG1:
 		mode_flag = FLAG_MODE_MPEG1;
 		break;
-	case GO7007_FORMAT_MPEG2:
+	case V4L2_PIX_FMT_MPEG2:
 		mode_flag = FLAG_MODE_MPEG2;
 		break;
-	case GO7007_FORMAT_MPEG4:
+	case V4L2_PIX_FMT_MPEG4:
 		mode_flag = FLAG_MODE_MPEG4;
 		break;
 	default:
 		return -1;
 	}
-	if (request_firmware(&fw_entry, go->board_info->firmware, go->dev)) {
+	if (request_firmware(&fw_entry, GO7007_FW_NAME, go->dev)) {
 		dev_err(go->dev,
 			"unable to load firmware from file \"%s\"\n",
-			go->board_info->firmware);
+			GO7007_FW_NAME);
 		return -1;
 	}
 	code = kzalloc(codespace * 2, GFP_KERNEL);
@@ -1586,7 +1588,7 @@ int go7007_construct_fw_image(struct go7007 *go, u8 **fw, int *fwlen)
 		if (chunk_len + 2 > srclen) {
 			dev_err(go->dev,
 				"firmware file \"%s\" appears to be corrupted\n",
-				go->board_info->firmware);
+				GO7007_FW_NAME);
 			goto fw_failed;
 		}
 		if (chunk_flags & mode_flag) {
@@ -1622,3 +1624,5 @@ fw_failed:
 	release_firmware(fw_entry);
 	return -1;
 }
+
+MODULE_FIRMWARE(GO7007_FW_NAME);
diff --git a/drivers/staging/media/go7007/go7007-i2c.c b/drivers/staging/media/go7007/go7007-i2c.c
index 39456a36b2c..74f25e03c32 100644
--- a/drivers/staging/media/go7007/go7007-i2c.c
+++ b/drivers/staging/media/go7007/go7007-i2c.c
@@ -28,7 +28,6 @@
 #include <linux/uaccess.h>
 
 #include "go7007-priv.h"
-#include "wis-i2c.h"
 
 /********************* Driver for on-board I2C adapter *********************/
 
@@ -52,11 +51,11 @@ static DEFINE_MUTEX(adlink_mpg24_i2c_lock);
 static int go7007_i2c_xfer(struct go7007 *go, u16 addr, int read,
 		u16 command, int flags, u8 *data)
 {
-	int i, ret = -1;
+	int i, ret = -EIO;
 	u16 val;
 
 	if (go->status == STATUS_SHUTDOWN)
-		return -1;
+		return -ENODEV;
 
 #ifdef GO7007_I2C_DEBUG
 	if (read)
@@ -146,7 +145,7 @@ static int go7007_smbus_xfer(struct i2c_adapter *adapter, u16 addr,
 	struct go7007 *go = i2c_get_adapdata(adapter);
 
 	if (size != I2C_SMBUS_BYTE_DATA)
-		return -1;
+		return -EIO;
 	return go7007_i2c_xfer(go, addr, read_write == I2C_SMBUS_READ, command,
 			flags & I2C_CLIENT_SCCB ? 0x10 : 0x00, &data->byte);
 }
@@ -170,26 +169,26 @@ static int go7007_i2c_master_xfer(struct i2c_adapter *adapter,
 					(msgs[i].flags & I2C_M_RD) ||
 					!(msgs[i + 1].flags & I2C_M_RD) ||
 					msgs[i + 1].len != 1)
-				return -1;
+				return -EIO;
 			if (go7007_i2c_xfer(go, msgs[i].addr, 1,
 					(msgs[i].buf[0] << 8) | msgs[i].buf[1],
 					0x01, &msgs[i + 1].buf[0]) < 0)
-				return -1;
+				return -EIO;
 			++i;
 		} else if (msgs[i].len == 3) {
 			if (msgs[i].flags & I2C_M_RD)
-				return -1;
+				return -EIO;
 			if (msgs[i].len != 3)
-				return -1;
+				return -EIO;
 			if (go7007_i2c_xfer(go, msgs[i].addr, 0,
 					(msgs[i].buf[0] << 8) | msgs[i].buf[1],
 					0x01, &msgs[i].buf[2]) < 0)
-				return -1;
+				return -EIO;
 		} else
-			return -1;
+			return -EIO;
 	}
 
-	return 0;
+	return num;
 }
 
 static u32 go7007_functionality(struct i2c_adapter *adapter)
diff --git a/drivers/staging/media/go7007/go7007-loader.c b/drivers/staging/media/go7007/go7007-loader.c
new file mode 100644
index 00000000000..f846ad5819d
--- /dev/null
+++ b/drivers/staging/media/go7007/go7007-loader.c
@@ -0,0 +1,144 @@
+/*
+ * Copyright (C) 2008 Sensoray Company 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.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/usb.h>
+#include <linux/firmware.h>
+#include <cypress_firmware.h>
+
+struct fw_config {
+	u16 vendor;
+	u16 product;
+	const char * const fw_name1;
+	const char * const fw_name2;
+};
+
+struct fw_config fw_configs[] = {
+	{ 0x1943, 0xa250, "go7007/s2250-1.fw", "go7007/s2250-2.fw" },
+	{ 0x093b, 0xa002, "go7007/px-m402u.fw", NULL },
+	{ 0x093b, 0xa004, "go7007/px-tv402u.fw", NULL },
+	{ 0x0eb1, 0x6666, "go7007/lr192.fw", NULL },
+	{ 0x0eb1, 0x6668, "go7007/wis-startrek.fw", NULL },
+	{ 0, 0, NULL, NULL }
+};
+MODULE_FIRMWARE("go7007/s2250-1.fw");
+MODULE_FIRMWARE("go7007/s2250-2.fw");
+MODULE_FIRMWARE("go7007/px-m402u.fw");
+MODULE_FIRMWARE("go7007/px-tv402u.fw");
+MODULE_FIRMWARE("go7007/lr192.fw");
+MODULE_FIRMWARE("go7007/wis-startrek.fw");
+
+static int go7007_loader_probe(struct usb_interface *interface,
+				const struct usb_device_id *id)
+{
+	struct usb_device *usbdev;
+	const struct firmware *fw;
+	u16 vendor, product;
+	const char *fw1, *fw2;
+	int ret;
+	int i;
+
+	usbdev = usb_get_dev(interface_to_usbdev(interface));
+	if (!usbdev)
+		goto failed2;
+
+	if (usbdev->descriptor.bNumConfigurations != 1) {
+		dev_err(&interface->dev, "can't handle multiple config\n");
+		return -ENODEV;
+	}
+
+	vendor = le16_to_cpu(usbdev->descriptor.idVendor);
+	product = le16_to_cpu(usbdev->descriptor.idProduct);
+
+	for (i = 0; fw_configs[i].fw_name1; i++)
+		if (fw_configs[i].vendor == vendor &&
+		    fw_configs[i].product == product)
+			break;
+
+	/* Should never happen */
+	if (fw_configs[i].fw_name1 == NULL)
+		goto failed2;
+
+	fw1 = fw_configs[i].fw_name1;
+	fw2 = fw_configs[i].fw_name2;
+
+	dev_info(&interface->dev, "loading firmware %s\n", fw1);
+
+	if (request_firmware(&fw, fw1, &usbdev->dev)) {
+		dev_err(&interface->dev,
+			"unable to load firmware from file \"%s\"\n", fw1);
+		goto failed2;
+	}
+	ret = cypress_load_firmware(usbdev, fw, CYPRESS_FX2);
+	release_firmware(fw);
+	if (0 != ret) {
+		dev_err(&interface->dev, "loader download failed\n");
+		goto failed2;
+	}
+
+	if (fw2 == NULL)
+		return 0;
+
+	if (request_firmware(&fw, fw2, &usbdev->dev)) {
+		dev_err(&interface->dev,
+			"unable to load firmware from file \"%s\"\n", fw2);
+		goto failed2;
+	}
+	ret = cypress_load_firmware(usbdev, fw, CYPRESS_FX2);
+	release_firmware(fw);
+	if (0 != ret) {
+		dev_err(&interface->dev, "firmware download failed\n");
+		goto failed2;
+	}
+	return 0;
+
+failed2:
+	dev_err(&interface->dev, "probe failed\n");
+	return -ENODEV;
+}
+
+static void go7007_loader_disconnect(struct usb_interface *interface)
+{
+	dev_info(&interface->dev, "disconnect\n");
+	usb_set_intfdata(interface, NULL);
+}
+
+static const struct usb_device_id go7007_loader_ids[] = {
+	{ USB_DEVICE(0x1943, 0xa250) },
+	{ USB_DEVICE(0x093b, 0xa002) },
+	{ USB_DEVICE(0x093b, 0xa004) },
+	{ USB_DEVICE(0x0eb1, 0x6666) },
+	{ USB_DEVICE(0x0eb1, 0x6668) },
+	{}                          /* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, go7007_loader_ids);
+
+static struct usb_driver go7007_loader_driver = {
+	.name		= "go7007-loader",
+	.probe		= go7007_loader_probe,
+	.disconnect	= go7007_loader_disconnect,
+	.id_table	= go7007_loader_ids,
+};
+
+module_usb_driver(go7007_loader_driver);
+
+MODULE_AUTHOR("");
+MODULE_DESCRIPTION("firmware loader for go7007-usb");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/media/go7007/go7007-priv.h b/drivers/staging/media/go7007/go7007-priv.h
index b58c394c655..6e16af72050 100644
--- a/drivers/staging/media/go7007/go7007-priv.h
+++ b/drivers/staging/media/go7007/go7007-priv.h
@@ -22,6 +22,9 @@
  */
 
 #include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fh.h>
+#include <media/videobuf2-core.h>
 
 struct go7007;
 
@@ -34,15 +37,13 @@ struct go7007;
 #define GO7007_BOARDID_XMEN_II		5
 #define GO7007_BOARDID_XMEN_III		6
 #define GO7007_BOARDID_MATRIX_REV	7
-#define GO7007_BOARDID_PX_M402U		16
-#define GO7007_BOARDID_PX_TV402U_ANY	17 /* need to check tuner model */
-#define GO7007_BOARDID_PX_TV402U_NA	18 /* detected NTSC tuner */
-#define GO7007_BOARDID_PX_TV402U_EU	19 /* detected PAL tuner */
-#define GO7007_BOARDID_PX_TV402U_JP	20 /* detected NTSC-J tuner */
-#define GO7007_BOARDID_LIFEVIEW_LR192	21 /* TV Walker Ultra */
-#define GO7007_BOARDID_ENDURA		22
-#define GO7007_BOARDID_ADLINK_MPG24	23
-#define GO7007_BOARDID_SENSORAY_2250	24 /* Sensoray 2250/2251 */
+#define GO7007_BOARDID_PX_M402U		8
+#define GO7007_BOARDID_PX_TV402U	9
+#define GO7007_BOARDID_LIFEVIEW_LR192	10 /* TV Walker Ultra */
+#define GO7007_BOARDID_ENDURA		11
+#define GO7007_BOARDID_ADLINK_MPG24	12
+#define GO7007_BOARDID_SENSORAY_2250	13 /* Sensoray 2250/2251 */
+#define GO7007_BOARDID_ADS_USBAV_709    14
 
 /* Various characteristics of each board */
 #define GO7007_BOARD_HAS_AUDIO		(1<<0)
@@ -61,6 +62,7 @@ struct go7007;
 #define GO7007_SENSOR_TV		(1<<7)
 #define GO7007_SENSOR_VBI		(1<<8)
 #define GO7007_SENSOR_SCALING		(1<<9)
+#define GO7007_SENSOR_SAA7115		(1<<10)
 
 /* Characteristics of audio sensor devices */
 #define GO7007_AUDIO_I2S_MODE_1		(1)
@@ -74,7 +76,6 @@ struct go7007;
 #define GO7007_AUDIO_OKI_MODE		(1<<17)
 
 struct go7007_board_info {
-	char *firmware;
 	unsigned int flags;
 	int hpi_buffer_cap;
 	unsigned int sensor_flags;
@@ -88,17 +89,25 @@ struct go7007_board_info {
 	int audio_bclk_div;
 	int audio_main_div;
 	int num_i2c_devs;
-	struct {
+	struct go_i2c {
 		const char *type;
-		int id;
+		unsigned int is_video:1;
+		unsigned int is_audio:1;
 		int addr;
-	} i2c_devs[4];
+		u32 flags;
+	} i2c_devs[5];
 	int num_inputs;
 	struct {
 		int video_input;
-		int audio_input;
+		int audio_index;
 		char *name;
 	} inputs[4];
+	int video_config;
+	int num_aud_inputs;
+	struct {
+		int audio_input;
+		char *name;
+	} aud_inputs[3];
 };
 
 struct go7007_hpi_ops {
@@ -109,6 +118,7 @@ struct go7007_hpi_ops {
 	int (*stream_stop)(struct go7007 *go);
 	int (*send_firmware)(struct go7007 *go, u8 *data, int len);
 	int (*send_command)(struct go7007 *go, unsigned int cmd, void *arg);
+	void (*release)(struct go7007 *go);
 };
 
 /* The video buffer size must be a multiple of PAGE_SIZE */
@@ -116,35 +126,12 @@ struct go7007_hpi_ops {
 #define	GO7007_BUF_SIZE		(GO7007_BUF_PAGES << PAGE_SHIFT)
 
 struct go7007_buffer {
-	struct go7007 *go; /* Reverse reference for VMA ops */
-	int index; /* Reverse reference for DQBUF */
-	enum { BUF_STATE_IDLE, BUF_STATE_QUEUED, BUF_STATE_DONE } state;
-	u32 seq;
-	struct timeval timestamp;
-	struct list_head stream;
-	struct page *pages[GO7007_BUF_PAGES + 1]; /* extra for straddling */
-	unsigned long user_addr;
-	unsigned int page_count;
-	unsigned int offset;
-	unsigned int bytesused;
+	struct vb2_buffer vb;
+	struct list_head list;
 	unsigned int frame_offset;
 	u32 modet_active;
-	int mapped;
 };
 
-struct go7007_file {
-	struct go7007 *go;
-	struct mutex lock;
-	int buf_count;
-	struct go7007_buffer *bufs;
-};
-
-#define	GO7007_FORMAT_MJPEG	0
-#define GO7007_FORMAT_MPEG4	1
-#define GO7007_FORMAT_MPEG1	2
-#define GO7007_FORMAT_MPEG2	3
-#define GO7007_FORMAT_H263	4
-
 #define GO7007_RATIO_1_1	0
 #define GO7007_RATIO_4_3	1
 #define GO7007_RATIO_16_9	2
@@ -163,24 +150,38 @@ enum go7007_parser_state {
 
 struct go7007 {
 	struct device *dev;
-	struct go7007_board_info *board_info;
+	u8 bus_info[32];
+	const struct go7007_board_info *board_info;
 	unsigned int board_id;
 	int tuner_type;
 	int channel_number; /* for multi-channel boards like Adlink PCI-MPG24 */
 	char name[64];
-	struct video_device *video_dev;
+	struct video_device vdev;
+	void *boot_fw;
+	unsigned boot_fw_len;
 	struct v4l2_device v4l2_dev;
-	int ref_count;
+	struct v4l2_ctrl_handler hdl;
+	struct v4l2_ctrl *mpeg_video_encoding;
+	struct v4l2_ctrl *mpeg_video_gop_size;
+	struct v4l2_ctrl *mpeg_video_gop_closure;
+	struct v4l2_ctrl *mpeg_video_bitrate;
+	struct v4l2_ctrl *mpeg_video_aspect_ratio;
+	struct v4l2_ctrl *mpeg_video_b_frames;
+	struct v4l2_ctrl *mpeg_video_rep_seqheader;
 	enum { STATUS_INIT, STATUS_ONLINE, STATUS_SHUTDOWN } status;
 	spinlock_t spinlock;
 	struct mutex hw_lock;
-	int streaming;
-	int in_use;
+	struct mutex serialize_lock;
 	int audio_enabled;
+	struct v4l2_subdev *sd_video;
+	struct v4l2_subdev *sd_audio;
+	u8 usb_buf[16];
 
 	/* Video input */
 	int input;
+	int aud_input;
 	enum { GO7007_STD_NTSC, GO7007_STD_PAL, GO7007_STD_OTHER } standard;
+	v4l2_std_id std;
 	int sensor_framerate;
 	int width;
 	int height;
@@ -191,7 +192,7 @@ struct go7007 {
 	unsigned int encoder_subsample:1;
 
 	/* Encoder config */
-	int format;
+	u32 format;
 	int bitrate;
 	int fps_scale;
 	int pali;
@@ -217,14 +218,16 @@ struct go7007 {
 	unsigned char active_map[216];
 
 	/* Video streaming */
-	struct go7007_buffer *active_buf;
+	struct mutex queue_lock;
+	struct vb2_queue vidq;
 	enum go7007_parser_state state;
 	int parse_length;
 	u16 modet_word;
 	int seen_frame;
 	u32 next_seq;
-	struct list_head stream;
+	struct list_head vidq_active;
 	wait_queue_head_t frame_waitq;
+	struct go7007_buffer *active_buf;
 
 	/* Audio streaming */
 	void (*audio_deliver)(struct go7007 *go, u8 *buf, int length);
@@ -267,12 +270,12 @@ int go7007_read_addr(struct go7007 *go, u16 addr, u16 *data);
 int go7007_read_interrupt(struct go7007 *go, u16 *value, u16 *data);
 int go7007_boot_encoder(struct go7007 *go, int init_i2c);
 int go7007_reset_encoder(struct go7007 *go);
-int go7007_register_encoder(struct go7007 *go);
+int go7007_register_encoder(struct go7007 *go, unsigned num_i2c_devs);
 int go7007_start_encoder(struct go7007 *go);
 void go7007_parse_video_stream(struct go7007 *go, u8 *buf, int length);
-struct go7007 *go7007_alloc(struct go7007_board_info *board,
+struct go7007 *go7007_alloc(const struct go7007_board_info *board,
 					struct device *dev);
-void go7007_remove(struct go7007 *go);
+void go7007_update_board(struct go7007 *go);
 
 /* go7007-fw.c */
 int go7007_construct_fw_image(struct go7007 *go, u8 **fw, int *fwlen);
@@ -283,6 +286,7 @@ int go7007_i2c_remove(struct go7007 *go);
 
 /* go7007-v4l2.c */
 int go7007_v4l2_init(struct go7007 *go);
+int go7007_v4l2_ctrl_init(struct go7007 *go);
 void go7007_v4l2_remove(struct go7007 *go);
 
 /* snd-go7007.c */
diff --git a/drivers/staging/media/go7007/go7007-usb.c b/drivers/staging/media/go7007/go7007-usb.c
index 9dbf5ecd05a..50066e01a6e 100644
--- a/drivers/staging/media/go7007/go7007-usb.c
+++ b/drivers/staging/media/go7007/go7007-usb.c
@@ -26,10 +26,11 @@
 #include <linux/usb.h>
 #include <linux/i2c.h>
 #include <asm/byteorder.h>
-#include <media/tvaudio.h>
+#include <media/saa7115.h>
+#include <media/tuner.h>
+#include <media/uda1342.h>
 
 #include "go7007-priv.h"
-#include "wis-i2c.h"
 
 static unsigned int assume_endura;
 module_param(assume_endura, int, 0644);
@@ -62,7 +63,7 @@ struct go7007_usb_board {
 };
 
 struct go7007_usb {
-	struct go7007_usb_board *board;
+	const struct go7007_usb_board *board;
 	struct mutex i2c_lock;
 	struct usb_device *usbdev;
 	struct urb *video_urbs[8];
@@ -72,10 +73,9 @@ struct go7007_usb {
 
 /*********************** Product specification data ***********************/
 
-static struct go7007_usb_board board_matrix_ii = {
+static const struct go7007_usb_board board_matrix_ii = {
 	.flags		= GO7007_USB_EZUSB,
 	.main_info	= {
-		.firmware	 = "go7007tv.bin",
 		.flags		 = GO7007_BOARD_HAS_AUDIO |
 					GO7007_BOARD_USE_ONBOARD_I2C,
 		.audio_flags	 = GO7007_AUDIO_I2S_MODE_1 |
@@ -87,14 +87,15 @@ static struct go7007_usb_board board_matrix_ii = {
 		.sensor_flags	 = GO7007_SENSOR_656 |
 					GO7007_SENSOR_VALID_ENABLE |
 					GO7007_SENSOR_TV |
+					GO7007_SENSOR_SAA7115 |
 					GO7007_SENSOR_VBI |
 					GO7007_SENSOR_SCALING,
 		.num_i2c_devs	 = 1,
 		.i2c_devs	 = {
 			{
-				.type	= "wis_saa7115",
-				.id	= I2C_DRIVERID_WIS_SAA7115,
+				.type	= "saa7115",
 				.addr	= 0x20,
+				.is_video = 1,
 			},
 		},
 		.num_inputs	 = 2,
@@ -108,13 +109,13 @@ static struct go7007_usb_board board_matrix_ii = {
 				.name		= "S-Video",
 			},
 		},
+		.video_config	= SAA7115_IDQ_IS_DEFAULT,
 	},
 };
 
-static struct go7007_usb_board board_matrix_reload = {
+static const struct go7007_usb_board board_matrix_reload = {
 	.flags		= GO7007_USB_EZUSB,
 	.main_info	= {
-		.firmware	 = "go7007tv.bin",
 		.flags		 = GO7007_BOARD_HAS_AUDIO |
 					GO7007_BOARD_USE_ONBOARD_I2C,
 		.audio_flags	 = GO7007_AUDIO_I2S_MODE_1 |
@@ -129,9 +130,9 @@ static struct go7007_usb_board board_matrix_reload = {
 		.num_i2c_devs	 = 1,
 		.i2c_devs	 = {
 			{
-				.type	= "wis_saa7113",
-				.id	= I2C_DRIVERID_WIS_SAA7113,
+				.type	= "saa7113",
 				.addr	= 0x25,
+				.is_video = 1,
 			},
 		},
 		.num_inputs	 = 2,
@@ -145,18 +146,19 @@ static struct go7007_usb_board board_matrix_reload = {
 				.name		= "S-Video",
 			},
 		},
+		.video_config	= SAA7115_IDQ_IS_DEFAULT,
 	},
 };
 
-static struct go7007_usb_board board_star_trek = {
+static const struct go7007_usb_board board_star_trek = {
 	.flags		= GO7007_USB_EZUSB | GO7007_USB_EZUSB_I2C,
 	.main_info	= {
-		.firmware	 = "go7007tv.bin",
 		.flags		 = GO7007_BOARD_HAS_AUDIO, /* |
 					GO7007_BOARD_HAS_TUNER, */
 		.sensor_flags	 = GO7007_SENSOR_656 |
 					GO7007_SENSOR_VALID_ENABLE |
 					GO7007_SENSOR_TV |
+					GO7007_SENSOR_SAA7115 |
 					GO7007_SENSOR_VBI |
 					GO7007_SENSOR_SCALING,
 		.audio_flags	 = GO7007_AUDIO_I2S_MODE_1 |
@@ -167,42 +169,43 @@ static struct go7007_usb_board board_star_trek = {
 		.num_i2c_devs	 = 1,
 		.i2c_devs	 = {
 			{
-				.type	= "wis_saa7115",
-				.id	= I2C_DRIVERID_WIS_SAA7115,
+				.type	= "saa7115",
 				.addr	= 0x20,
+				.is_video = 1,
 			},
 		},
 		.num_inputs	 = 2,
 		.inputs		 = {
+		/*	{
+		 *		.video_input	= 3,
+		 *		.audio_index	= AUDIO_TUNER,
+		 *		.name		= "Tuner",
+		 *	},
+		 */
 			{
 				.video_input	= 1,
-			/*	.audio_input	= AUDIO_EXTERN, */
+			/*	.audio_index	= AUDIO_EXTERN, */
 				.name		= "Composite",
 			},
 			{
 				.video_input	= 8,
-			/*	.audio_input	= AUDIO_EXTERN, */
+			/*	.audio_index	= AUDIO_EXTERN, */
 				.name		= "S-Video",
 			},
-		/*	{
-		 *		.video_input	= 3,
-		 *		.audio_input	= AUDIO_TUNER,
-		 *		.name		= "Tuner",
-		 *	},
-		 */
 		},
+		.video_config	= SAA7115_IDQ_IS_DEFAULT,
 	},
 };
 
-static struct go7007_usb_board board_px_tv402u = {
+static const struct go7007_usb_board board_px_tv402u = {
 	.flags		= GO7007_USB_EZUSB | GO7007_USB_EZUSB_I2C,
 	.main_info	= {
-		.firmware	 = "go7007tv.bin",
 		.flags		 = GO7007_BOARD_HAS_AUDIO |
 					GO7007_BOARD_HAS_TUNER,
 		.sensor_flags	 = GO7007_SENSOR_656 |
 					GO7007_SENSOR_VALID_ENABLE |
 					GO7007_SENSOR_TV |
+					GO7007_SENSOR_SAA7115 |
 					GO7007_SENSOR_VBI |
 					GO7007_SENSOR_SCALING,
 		.audio_flags	 = GO7007_AUDIO_I2S_MODE_1 |
@@ -210,49 +213,67 @@ static struct go7007_usb_board board_px_tv402u = {
 		.audio_bclk_div	 = 8,
 		.audio_main_div	 = 2,
 		.hpi_buffer_cap  = 7,
-		.num_i2c_devs	 = 3,
+		.num_i2c_devs	 = 5,
 		.i2c_devs	 = {
 			{
-				.type	= "wis_saa7115",
-				.id	= I2C_DRIVERID_WIS_SAA7115,
+				.type	= "saa7115",
 				.addr	= 0x20,
+				.is_video = 1,
 			},
 			{
-				.type	= "wis_uda1342",
-				.id	= I2C_DRIVERID_WIS_UDA1342,
+				.type	= "uda1342",
 				.addr	= 0x1a,
+				.is_audio = 1,
 			},
 			{
-				.type	= "wis_sony_tuner",
-				.id	= I2C_DRIVERID_WIS_SONY_TUNER,
+				.type	= "tuner",
 				.addr	= 0x60,
 			},
+			{
+				.type	= "tuner",
+				.addr	= 0x43,
+			},
+			{
+				.type	= "sony-btf-mpx",
+				.addr	= 0x44,
+			},
 		},
 		.num_inputs	 = 3,
 		.inputs		 = {
 			{
+				.video_input	= 3,
+				.audio_index	= 0,
+				.name		= "Tuner",
+			},
+			{
 				.video_input	= 1,
-				.audio_input	= TVAUDIO_INPUT_EXTERN,
+				.audio_index	= 1,
 				.name		= "Composite",
 			},
 			{
 				.video_input	= 8,
-				.audio_input	= TVAUDIO_INPUT_EXTERN,
+				.audio_index	= 1,
 				.name		= "S-Video",
 			},
+		},
+		.video_config	= SAA7115_IDQ_IS_DEFAULT,
+		.num_aud_inputs	 = 2,
+		.aud_inputs	 = {
 			{
-				.video_input	= 3,
-				.audio_input	= TVAUDIO_INPUT_TUNER,
+				.audio_input	= UDA1342_IN2,
 				.name		= "Tuner",
 			},
+			{
+				.audio_input	= UDA1342_IN1,
+				.name		= "Line In",
+			},
 		},
 	},
 };
 
-static struct go7007_usb_board board_xmen = {
+static const struct go7007_usb_board board_xmen = {
 	.flags		= 0,
 	.main_info	= {
-		.firmware	  = "go7007tv.bin",
 		.flags		  = GO7007_BOARD_USE_ONBOARD_I2C,
 		.hpi_buffer_cap   = 0,
 		.sensor_flags	  = GO7007_SENSOR_VREF_POLAR,
@@ -271,8 +292,7 @@ static struct go7007_usb_board board_xmen = {
 		.num_i2c_devs	  = 1,
 		.i2c_devs	  = {
 			{
-				.type	= "wis_ov7640",
-				.id	= I2C_DRIVERID_WIS_OV7640,
+				.type	= "ov7640",
 				.addr	= 0x21,
 			},
 		},
@@ -285,10 +305,9 @@ static struct go7007_usb_board board_xmen = {
 	},
 };
 
-static struct go7007_usb_board board_matrix_revolution = {
+static const struct go7007_usb_board board_matrix_revolution = {
 	.flags		= GO7007_USB_EZUSB,
 	.main_info	= {
-		.firmware	 = "go7007tv.bin",
 		.flags		 = GO7007_BOARD_HAS_AUDIO |
 					GO7007_BOARD_USE_ONBOARD_I2C,
 		.audio_flags	 = GO7007_AUDIO_I2S_MODE_1 |
@@ -304,8 +323,8 @@ static struct go7007_usb_board board_matrix_revolution = {
 		.num_i2c_devs	 = 1,
 		.i2c_devs	 = {
 			{
-				.type	= "wis_tw9903",
-				.id	= I2C_DRIVERID_WIS_TW9903,
+				.type	= "tw9903",
+				.is_video = 1,
 				.addr	= 0x44,
 			},
 		},
@@ -323,10 +342,9 @@ static struct go7007_usb_board board_matrix_revolution = {
 	},
 };
 
-static struct go7007_usb_board board_lifeview_lr192 = {
+static const struct go7007_usb_board board_lifeview_lr192 = {
 	.flags		= GO7007_USB_EZUSB,
 	.main_info	= {
-		.firmware	 = "go7007tv.bin",
 		.flags		 = GO7007_BOARD_HAS_AUDIO |
 					GO7007_BOARD_USE_ONBOARD_I2C,
 		.audio_flags	 = GO7007_AUDIO_I2S_MODE_1 |
@@ -351,10 +369,9 @@ static struct go7007_usb_board board_lifeview_lr192 = {
 	},
 };
 
-static struct go7007_usb_board board_endura = {
+static const struct go7007_usb_board board_endura = {
 	.flags		= 0,
 	.main_info	= {
-		.firmware	 = "go7007tv.bin",
 		.flags		 = 0,
 		.audio_flags	 = GO7007_AUDIO_I2S_MODE_1 |
 					GO7007_AUDIO_I2S_MASTER |
@@ -376,10 +393,9 @@ static struct go7007_usb_board board_endura = {
 	},
 };
 
-static struct go7007_usb_board board_adlink_mpg24 = {
+static const struct go7007_usb_board board_adlink_mpg24 = {
 	.flags		= 0,
 	.main_info	= {
-		.firmware	 = "go7007tv.bin",
 		.flags		 = GO7007_BOARD_USE_ONBOARD_I2C,
 		.audio_flags	 = GO7007_AUDIO_I2S_MODE_1 |
 					GO7007_AUDIO_I2S_MASTER |
@@ -394,9 +410,10 @@ static struct go7007_usb_board board_adlink_mpg24 = {
 		.num_i2c_devs	 = 1,
 		.i2c_devs	 = {
 			{
-				.type	= "wis_tw2804",
-				.id	= I2C_DRIVERID_WIS_TW2804,
+				.type	= "tw2804",
 				.addr	= 0x00, /* yes, really */
+				.flags  = I2C_CLIENT_TEN,
+				.is_video = 1,
 			},
 		},
 		.num_inputs	 = 1,
@@ -408,10 +425,9 @@ static struct go7007_usb_board board_adlink_mpg24 = {
 	},
 };
 
-static struct go7007_usb_board board_sensoray_2250 = {
+static const struct go7007_usb_board board_sensoray_2250 = {
 	.flags		= GO7007_USB_EZUSB | GO7007_USB_EZUSB_I2C,
 	.main_info	= {
-		.firmware	 = "go7007tv.bin",
 		.audio_flags	 = GO7007_AUDIO_I2S_MODE_1 |
 					GO7007_AUDIO_I2S_MASTER |
 					GO7007_AUDIO_WORD_16,
@@ -426,8 +442,9 @@ static struct go7007_usb_board board_sensoray_2250 = {
 		.i2c_devs	 = {
 			{
 				.type	= "s2250",
-				.id	= I2C_DRIVERID_S2250,
 				.addr	= 0x43,
+				.is_video = 1,
+				.is_audio = 1,
 			},
 		},
 		.num_inputs	 = 2,
@@ -441,10 +458,60 @@ static struct go7007_usb_board board_sensoray_2250 = {
 				.name		= "S-Video",
 			},
 		},
+		.num_aud_inputs	 = 3,
+		.aud_inputs	 = {
+			{
+				.audio_input	= 0,
+				.name		= "Line In",
+			},
+			{
+				.audio_input	= 1,
+				.name		= "Mic",
+			},
+			{
+				.audio_input	= 2,
+				.name		= "Mic Boost",
+			},
+		},
 	},
 };
 
-MODULE_FIRMWARE("go7007tv.bin");
+static const struct go7007_usb_board board_ads_usbav_709 = {
+	.flags		= GO7007_USB_EZUSB,
+	.main_info	= {
+		.flags		 = GO7007_BOARD_HAS_AUDIO |
+					GO7007_BOARD_USE_ONBOARD_I2C,
+		.audio_flags	 = GO7007_AUDIO_I2S_MODE_1 |
+					GO7007_AUDIO_I2S_MASTER |
+					GO7007_AUDIO_WORD_16,
+		.audio_rate	 = 48000,
+		.audio_bclk_div	 = 8,
+		.audio_main_div	 = 2,
+		.hpi_buffer_cap  = 7,
+		.sensor_flags	 = GO7007_SENSOR_656 |
+					GO7007_SENSOR_TV |
+					GO7007_SENSOR_VBI,
+		.num_i2c_devs	 = 1,
+		.i2c_devs	 = {
+			{
+				.type	= "tw9906",
+				.is_video = 1,
+				.addr	= 0x44,
+			},
+		},
+		.num_inputs	 = 2,
+		.inputs		 = {
+			{
+				.video_input	= 0,
+				.name		= "Composite",
+			},
+			{
+				.video_input	= 10,
+				.name		= "S-Video",
+			},
+		},
+	},
+};
 
 static const struct usb_device_id go7007_usb_id_table[] = {
 	{
@@ -529,7 +596,7 @@ static const struct usb_device_id go7007_usb_id_table[] = {
 		.idProduct	= 0xa104,  /* Product ID of TV402U */
 		.bcdDevice_lo	= 0x1,
 		.bcdDevice_hi	= 0x1,
-		.driver_info	= (kernel_ulong_t)GO7007_BOARDID_PX_TV402U_ANY,
+		.driver_info	= (kernel_ulong_t)GO7007_BOARDID_PX_TV402U,
 	},
 	{
 		.match_flags	= USB_DEVICE_ID_MATCH_DEVICE_AND_VERSION,
@@ -547,6 +614,14 @@ static const struct usb_device_id go7007_usb_id_table[] = {
 		.bcdDevice_hi	= 0x1,
 		.driver_info	= (kernel_ulong_t)GO7007_BOARDID_SENSORAY_2250,
 	},
+	{
+		.match_flags	= USB_DEVICE_ID_MATCH_DEVICE_AND_VERSION,
+		.idVendor	= 0x06e1,  /* Vendor ID of ADS Technologies */
+		.idProduct	= 0x0709,  /* Product ID of DVD Xpress DX2 */
+		.bcdDevice_lo	= 0x204,
+		.bcdDevice_hi	= 0x204,
+		.driver_info	= (kernel_ulong_t)GO7007_BOARDID_ADS_USBAV_709,
+	},
 	{ }					/* Terminating entry */
 };
 
@@ -578,6 +653,8 @@ static int go7007_usb_interface_reset(struct go7007 *go)
 	struct go7007_usb *usb = go->hpi_context;
 	u16 intr_val, intr_data;
 
+	if (go->status == STATUS_SHUTDOWN)
+		return -1;
 	/* Reset encoder */
 	if (go7007_write_interrupt(go, 0x0001, 0x0001) < 0)
 		return -1;
@@ -613,7 +690,7 @@ static int go7007_usb_ezusb_write_interrupt(struct go7007 *go,
 {
 	struct go7007_usb *usb = go->hpi_context;
 	int i, r;
-	u16 status_reg;
+	u16 status_reg = 0;
 	int timeout = 500;
 
 #ifdef GO7007_USB_DEBUG
@@ -625,15 +702,17 @@ static int go7007_usb_ezusb_write_interrupt(struct go7007 *go,
 		r = usb_control_msg(usb->usbdev,
 				usb_rcvctrlpipe(usb->usbdev, 0), 0x14,
 				USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
-				0, HPI_STATUS_ADDR, &status_reg,
+				0, HPI_STATUS_ADDR, go->usb_buf,
 				sizeof(status_reg), timeout);
 		if (r < 0)
-			goto write_int_error;
-		__le16_to_cpus(&status_reg);
+			break;
+		status_reg = le16_to_cpu(*((u16 *)go->usb_buf));
 		if (!(status_reg & 0x0010))
 			break;
 		msleep(10);
 	}
+	if (r < 0)
+		goto write_int_error;
 	if (i == 100) {
 		printk(KERN_ERR
 			"go7007-usb: device is hung, status reg = 0x%04x\n",
@@ -661,7 +740,6 @@ static int go7007_usb_onboard_write_interrupt(struct go7007 *go,
 						int addr, int data)
 {
 	struct go7007_usb *usb = go->hpi_context;
-	u8 *tbuf;
 	int r;
 	int timeout = 500;
 
@@ -670,17 +748,14 @@ static int go7007_usb_onboard_write_interrupt(struct go7007 *go,
 		"go7007-usb: WriteInterrupt: %04x %04x\n", addr, data);
 #endif
 
-	tbuf = kzalloc(8, GFP_KERNEL);
-	if (tbuf == NULL)
-		return -ENOMEM;
-	tbuf[0] = data & 0xff;
-	tbuf[1] = data >> 8;
-	tbuf[2] = addr & 0xff;
-	tbuf[3] = addr >> 8;
+	go->usb_buf[0] = data & 0xff;
+	go->usb_buf[1] = data >> 8;
+	go->usb_buf[2] = addr & 0xff;
+	go->usb_buf[3] = addr >> 8;
+	go->usb_buf[4] = go->usb_buf[5] = go->usb_buf[6] = go->usb_buf[7] = 0;
 	r = usb_control_msg(usb->usbdev, usb_sndctrlpipe(usb->usbdev, 2), 0x00,
 			USB_TYPE_VENDOR | USB_RECIP_ENDPOINT, 0x55aa,
-			0xf0f0, tbuf, 8, timeout);
-	kfree(tbuf);
+			0xf0f0, go->usb_buf, 8, timeout);
 	if (r < 0) {
 		printk(KERN_ERR "go7007-usb: error in WriteInterrupt: %d\n", r);
 		return r;
@@ -738,7 +813,7 @@ static void go7007_usb_read_video_pipe_complete(struct urb *urb)
 	struct go7007 *go = (struct go7007 *)urb->context;
 	int r, status = urb->status;
 
-	if (!go->streaming) {
+	if (!vb2_is_streaming(&go->vidq)) {
 		wake_up_interruptible(&go->frame_waitq);
 		return;
 	}
@@ -762,7 +837,7 @@ static void go7007_usb_read_audio_pipe_complete(struct urb *urb)
 	struct go7007 *go = (struct go7007 *)urb->context;
 	int r, status = urb->status;
 
-	if (!go->streaming)
+	if (!vb2_is_streaming(&go->vidq))
 		return;
 	if (status) {
 		printk(KERN_ERR "go7007-usb: error in audio pipe: %d\n",
@@ -849,6 +924,37 @@ static int go7007_usb_send_firmware(struct go7007 *go, u8 *data, int len)
 					&transferred, timeout);
 }
 
+static void go7007_usb_release(struct go7007 *go)
+{
+	struct go7007_usb *usb = go->hpi_context;
+	struct urb *vurb, *aurb;
+	int i;
+
+	if (usb->intr_urb) {
+		usb_kill_urb(usb->intr_urb);
+		kfree(usb->intr_urb->transfer_buffer);
+		usb_free_urb(usb->intr_urb);
+	}
+
+	/* Free USB-related structs */
+	for (i = 0; i < 8; ++i) {
+		vurb = usb->video_urbs[i];
+		if (vurb) {
+			usb_kill_urb(vurb);
+			kfree(vurb->transfer_buffer);
+			usb_free_urb(vurb);
+		}
+		aurb = usb->audio_urbs[i];
+		if (aurb) {
+			usb_kill_urb(aurb);
+			kfree(aurb->transfer_buffer);
+			usb_free_urb(aurb);
+		}
+	}
+
+	kfree(go->hpi_context);
+}
+
 static struct go7007_hpi_ops go7007_usb_ezusb_hpi_ops = {
 	.interface_reset	= go7007_usb_interface_reset,
 	.write_interrupt	= go7007_usb_ezusb_write_interrupt,
@@ -856,6 +962,7 @@ static struct go7007_hpi_ops go7007_usb_ezusb_hpi_ops = {
 	.stream_start		= go7007_usb_stream_start,
 	.stream_stop		= go7007_usb_stream_stop,
 	.send_firmware		= go7007_usb_send_firmware,
+	.release		= go7007_usb_release,
 };
 
 static struct go7007_hpi_ops go7007_usb_onboard_hpi_ops = {
@@ -865,6 +972,7 @@ static struct go7007_hpi_ops go7007_usb_onboard_hpi_ops = {
 	.stream_start		= go7007_usb_stream_start,
 	.stream_stop		= go7007_usb_stream_stop,
 	.send_firmware		= go7007_usb_send_firmware,
+	.release		= go7007_usb_release,
 };
 
 /********************* Driver for EZ-USB I2C adapter *********************/
@@ -874,12 +982,12 @@ static int go7007_usb_i2c_master_xfer(struct i2c_adapter *adapter,
 {
 	struct go7007 *go = i2c_get_adapdata(adapter);
 	struct go7007_usb *usb = go->hpi_context;
-	u8 buf[16];
+	u8 *buf = go->usb_buf;
 	int buf_len, i;
-	int ret = -1;
+	int ret = -EIO;
 
 	if (go->status == STATUS_SHUTDOWN)
-		return -1;
+		return -ENODEV;
 
 	mutex_lock(&usb->i2c_lock);
 
@@ -929,14 +1037,14 @@ static int go7007_usb_i2c_master_xfer(struct i2c_adapter *adapter,
 						buf, buf_len, 0) < 0)
 			goto i2c_done;
 		if (msgs[i].flags & I2C_M_RD) {
-			memset(buf, 0, sizeof(buf));
+			memset(buf, 0, msgs[i].len + 1);
 			if (go7007_usb_vendor_request(go, 0x25, 0, 0, buf,
 						msgs[i].len + 1, 1) < 0)
 				goto i2c_done;
 			memcpy(msgs[i].buf, buf + 1, msgs[i].len);
 		}
 	}
-	ret = 0;
+	ret = num;
 
 i2c_done:
 	mutex_unlock(&usb->i2c_lock);
@@ -968,8 +1076,9 @@ static int go7007_usb_probe(struct usb_interface *intf,
 {
 	struct go7007 *go;
 	struct go7007_usb *usb;
-	struct go7007_usb_board *board;
+	const struct go7007_usb_board *board;
 	struct usb_device *usbdev = interface_to_usbdev(intf);
+	unsigned num_i2c_devs;
 	char *name;
 	int video_pipe, i, v_urb_len;
 
@@ -1008,7 +1117,7 @@ static int go7007_usb_probe(struct usb_interface *intf,
 		name = "Plextor PX-M402U";
 		board = &board_matrix_ii;
 		break;
-	case GO7007_BOARDID_PX_TV402U_ANY:
+	case GO7007_BOARDID_PX_TV402U:
 		name = "Plextor PX-TV402U (unknown tuner)";
 		board = &board_px_tv402u;
 		break;
@@ -1024,29 +1133,29 @@ static int go7007_usb_probe(struct usb_interface *intf,
 		name = "Sensoray 2250/2251";
 		board = &board_sensoray_2250;
 		break;
+	case GO7007_BOARDID_ADS_USBAV_709:
+		name = "ADS Tech DVD Xpress DX2";
+		board = &board_ads_usbav_709;
+		break;
 	default:
 		printk(KERN_ERR "go7007-usb: unknown board ID %d!\n",
 				(unsigned int)id->driver_info);
 		return 0;
 	}
 
-	usb = kzalloc(sizeof(struct go7007_usb), GFP_KERNEL);
-	if (usb == NULL)
+	go = go7007_alloc(&board->main_info, &intf->dev);
+	if (go == NULL)
 		return -ENOMEM;
 
-	/* Allocate the URB and buffer for receiving incoming interrupts */
-	usb->intr_urb = usb_alloc_urb(0, GFP_KERNEL);
-	if (usb->intr_urb == NULL)
-		goto allocfail;
-	usb->intr_urb->transfer_buffer = kmalloc(2*sizeof(u16), GFP_KERNEL);
-	if (usb->intr_urb->transfer_buffer == NULL)
-		goto allocfail;
+	usb = kzalloc(sizeof(struct go7007_usb), GFP_KERNEL);
+	if (usb == NULL) {
+		kfree(go);
+		return -ENOMEM;
+	}
 
-	go = go7007_alloc(&board->main_info, &intf->dev);
-	if (go == NULL)
-		goto allocfail;
 	usb->board = board;
 	usb->usbdev = usbdev;
+	usb_make_path(usbdev, go->bus_info, sizeof(go->bus_info));
 	go->board_id = id->driver_info;
 	strncpy(go->name, name, sizeof(go->name));
 	if (board->flags & GO7007_USB_EZUSB)
@@ -1054,6 +1163,15 @@ static int go7007_usb_probe(struct usb_interface *intf,
 	else
 		go->hpi_ops = &go7007_usb_onboard_hpi_ops;
 	go->hpi_context = usb;
+
+	/* Allocate the URB and buffer for receiving incoming interrupts */
+	usb->intr_urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (usb->intr_urb == NULL)
+		goto allocfail;
+	usb->intr_urb->transfer_buffer = kmalloc(2*sizeof(u16), GFP_KERNEL);
+	if (usb->intr_urb->transfer_buffer == NULL)
+		goto allocfail;
+
 	if (go->board_id == GO7007_BOARDID_SENSORAY_2250)
 		usb_fill_bulk_urb(usb->intr_urb, usb->usbdev,
 			usb_rcvbulkpipe(usb->usbdev, 4),
@@ -1069,7 +1187,7 @@ static int go7007_usb_probe(struct usb_interface *intf,
 	/* Boot the GO7007 */
 	if (go7007_boot_encoder(go, go->board_info->flags &
 					GO7007_BOARD_USE_ONBOARD_I2C) < 0)
-		goto initfail;
+		goto allocfail;
 
 	/* Register the EZ-USB I2C adapter, if we're using it */
 	if (board->flags & GO7007_USB_EZUSB_I2C) {
@@ -1081,7 +1199,7 @@ static int go7007_usb_probe(struct usb_interface *intf,
 		if (i2c_add_adapter(&go->i2c_adapter) < 0) {
 			printk(KERN_ERR
 				"go7007-usb: error: i2c_add_adapter failed\n");
-			goto initfail;
+			goto allocfail;
 		}
 		go->i2c_adapter_online = 1;
 	}
@@ -1121,34 +1239,36 @@ static int go7007_usb_probe(struct usb_interface *intf,
 					"Adlink PCI-MPG24, channel #%d",
 					channel);
 			}
+			go7007_update_board(go);
 		}
 	}
 
-	/* Probe the tuner model on the TV402U */
-	if (go->board_id == GO7007_BOARDID_PX_TV402U_ANY) {
-		u8 data[3];
+	num_i2c_devs = go->board_info->num_i2c_devs;
 
+	/* Probe the tuner model on the TV402U */
+	if (go->board_id == GO7007_BOARDID_PX_TV402U) {
 		/* Board strapping indicates tuner model */
-		if (go7007_usb_vendor_request(go, 0x41, 0, 0, data, 3, 1) < 0) {
+		if (go7007_usb_vendor_request(go, 0x41, 0, 0, go->usb_buf, 3, 1) < 0) {
 			printk(KERN_ERR "go7007-usb: GPIO read failed!\n");
-			goto initfail;
+			goto allocfail;
 		}
-		switch (data[0] >> 6) {
+		switch (go->usb_buf[0] >> 6) {
 		case 1:
-			go->board_id = GO7007_BOARDID_PX_TV402U_EU;
 			go->tuner_type = TUNER_SONY_BTF_PG472Z;
+			go->std = V4L2_STD_PAL;
 			strncpy(go->name, "Plextor PX-TV402U-EU",
 					sizeof(go->name));
 			break;
 		case 2:
-			go->board_id = GO7007_BOARDID_PX_TV402U_JP;
 			go->tuner_type = TUNER_SONY_BTF_PK467Z;
+			go->std = V4L2_STD_NTSC_M_JP;
+			num_i2c_devs -= 2;
 			strncpy(go->name, "Plextor PX-TV402U-JP",
 					sizeof(go->name));
 			break;
 		case 3:
-			go->board_id = GO7007_BOARDID_PX_TV402U_NA;
 			go->tuner_type = TUNER_SONY_BTF_PB463Z;
+			num_i2c_devs -= 2;
 			strncpy(go->name, "Plextor PX-TV402U-NA",
 					sizeof(go->name));
 			break;
@@ -1162,7 +1282,7 @@ static int go7007_usb_probe(struct usb_interface *intf,
 		if (go7007_usb_vendor_request(go, 0x40, 0x7f02, 0,
 					NULL, 0, 0) < 0) {
 			printk(KERN_ERR "go7007-usb: GPIO write failed!\n");
-			goto initfail;
+			goto allocfail;
 		}
 	}
 
@@ -1176,11 +1296,6 @@ static int go7007_usb_probe(struct usb_interface *intf,
 				"port will result in stream corruption, even "
 				"at low bitrates!\n");
 
-	/* Do any final GO7007 initialization, then register the
-	 * V4L2 and ALSA interfaces */
-	if (go7007_register_encoder(go) < 0)
-		goto initfail;
-
 	/* Allocate the URBs and buffers for receiving the video stream */
 	if (board->flags & GO7007_USB_EZUSB) {
 		v_urb_len = 1024;
@@ -1192,80 +1307,65 @@ static int go7007_usb_probe(struct usb_interface *intf,
 	for (i = 0; i < 8; ++i) {
 		usb->video_urbs[i] = usb_alloc_urb(0, GFP_KERNEL);
 		if (usb->video_urbs[i] == NULL)
-			goto initfail;
+			goto allocfail;
 		usb->video_urbs[i]->transfer_buffer =
 						kmalloc(v_urb_len, GFP_KERNEL);
 		if (usb->video_urbs[i]->transfer_buffer == NULL)
-			goto initfail;
+			goto allocfail;
 		usb_fill_bulk_urb(usb->video_urbs[i], usb->usbdev, video_pipe,
 				usb->video_urbs[i]->transfer_buffer, v_urb_len,
 				go7007_usb_read_video_pipe_complete, go);
 	}
 
 	/* Allocate the URBs and buffers for receiving the audio stream */
-	if ((board->flags & GO7007_USB_EZUSB) && go->audio_enabled)
+	if ((board->flags & GO7007_USB_EZUSB) &&
+	    (board->flags & GO7007_BOARD_HAS_AUDIO)) {
 		for (i = 0; i < 8; ++i) {
 			usb->audio_urbs[i] = usb_alloc_urb(0, GFP_KERNEL);
 			if (usb->audio_urbs[i] == NULL)
-				goto initfail;
+				goto allocfail;
 			usb->audio_urbs[i]->transfer_buffer = kmalloc(4096,
 								GFP_KERNEL);
 			if (usb->audio_urbs[i]->transfer_buffer == NULL)
-				goto initfail;
+				goto allocfail;
 			usb_fill_bulk_urb(usb->audio_urbs[i], usb->usbdev,
 				usb_rcvbulkpipe(usb->usbdev, 8),
 				usb->audio_urbs[i]->transfer_buffer, 4096,
 				go7007_usb_read_audio_pipe_complete, go);
 		}
+	}
 
+	/* Do any final GO7007 initialization, then register the
+	 * V4L2 and ALSA interfaces */
+	if (go7007_register_encoder(go, num_i2c_devs) < 0)
+		goto allocfail;
 
 	go->status = STATUS_ONLINE;
 	return 0;
 
-initfail:
-	go->status = STATUS_SHUTDOWN;
-	return 0;
-
 allocfail:
-	if (usb->intr_urb) {
-		kfree(usb->intr_urb->transfer_buffer);
-		usb_free_urb(usb->intr_urb);
-	}
-	kfree(usb);
+	go7007_usb_release(go);
+	kfree(go);
 	return -ENOMEM;
 }
 
 static void go7007_usb_disconnect(struct usb_interface *intf)
 {
 	struct go7007 *go = to_go7007(usb_get_intfdata(intf));
-	struct go7007_usb *usb = go->hpi_context;
-	struct urb *vurb, *aurb;
-	int i;
 
-	usb_kill_urb(usb->intr_urb);
+	mutex_lock(&go->queue_lock);
+	mutex_lock(&go->serialize_lock);
 
-	/* Free USB-related structs */
-	for (i = 0; i < 8; ++i) {
-		vurb = usb->video_urbs[i];
-		if (vurb) {
-			usb_kill_urb(vurb);
-			kfree(vurb->transfer_buffer);
-			usb_free_urb(vurb);
-		}
-		aurb = usb->audio_urbs[i];
-		if (aurb) {
-			usb_kill_urb(aurb);
-			kfree(aurb->transfer_buffer);
-			usb_free_urb(aurb);
-		}
-	}
-	kfree(usb->intr_urb->transfer_buffer);
-	usb_free_urb(usb->intr_urb);
-
-	kfree(go->hpi_context);
+	if (go->audio_enabled)
+		go7007_snd_remove(go);
 
-	go7007_remove(go);
 	go->status = STATUS_SHUTDOWN;
+	v4l2_device_disconnect(&go->v4l2_dev);
+	video_unregister_device(&go->vdev);
+	mutex_unlock(&go->serialize_lock);
+	mutex_unlock(&go->queue_lock);
+
+	v4l2_device_put(&go->v4l2_dev);
 }
 
 static struct usb_driver go7007_usb_driver = {
diff --git a/drivers/staging/media/go7007/go7007-v4l2.c b/drivers/staging/media/go7007/go7007-v4l2.c
index cb9fe33050c..50eb69a8ef0 100644
--- a/drivers/staging/media/go7007/go7007-v4l2.c
+++ b/drivers/staging/media/go7007/go7007-v4l2.c
@@ -17,7 +17,6 @@
 
 #include <linux/module.h>
 #include <linux/init.h>
-#include <linux/version.h>
 #include <linux/delay.h>
 #include <linux/sched.h>
 #include <linux/spinlock.h>
@@ -27,115 +26,45 @@
 #include <linux/time.h>
 #include <linux/vmalloc.h>
 #include <linux/pagemap.h>
+#include <linux/i2c.h>
+#include <linux/mutex.h>
+#include <linux/uaccess.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-subdev.h>
-#include <linux/i2c.h>
-#include <linux/mutex.h>
-#include <linux/uaccess.h>
+#include <media/v4l2-event.h>
+#include <media/videobuf2-vmalloc.h>
+#include <media/saa7115.h>
 
 #include "go7007.h"
 #include "go7007-priv.h"
-#include "wis-i2c.h"
-
-/* Temporary defines until accepted in v4l-dvb */
-#ifndef V4L2_MPEG_STREAM_TYPE_MPEG_ELEM
-#define	V4L2_MPEG_STREAM_TYPE_MPEG_ELEM   6 /* MPEG elementary stream */
-#endif
-#ifndef V4L2_MPEG_VIDEO_ENCODING_MPEG_4
-#define	V4L2_MPEG_VIDEO_ENCODING_MPEG_4   3
-#endif
 
 #define call_all(dev, o, f, args...) \
 	v4l2_device_call_until_err(dev, 0, o, f, ##args)
 
-static void deactivate_buffer(struct go7007_buffer *gobuf)
-{
-	int i;
-
-	if (gobuf->state != BUF_STATE_IDLE) {
-		list_del(&gobuf->stream);
-		gobuf->state = BUF_STATE_IDLE;
-	}
-	if (gobuf->page_count > 0) {
-		for (i = 0; i < gobuf->page_count; ++i)
-			page_cache_release(gobuf->pages[i]);
-		gobuf->page_count = 0;
-	}
-}
-
-static void abort_queued(struct go7007 *go)
-{
-	struct go7007_buffer *gobuf, *next;
-
-	list_for_each_entry_safe(gobuf, next, &go->stream, stream) {
-		deactivate_buffer(gobuf);
-	}
-}
-
-static int go7007_streamoff(struct go7007 *go)
-{
-	unsigned long flags;
-
-	mutex_lock(&go->hw_lock);
-	if (go->streaming) {
-		go->streaming = 0;
-		go7007_stream_stop(go);
-		spin_lock_irqsave(&go->spinlock, flags);
-		abort_queued(go);
-		spin_unlock_irqrestore(&go->spinlock, flags);
-		go7007_reset_encoder(go);
-	}
-	mutex_unlock(&go->hw_lock);
-	return 0;
-}
-
-static int go7007_open(struct file *file)
-{
-	struct go7007 *go = video_get_drvdata(video_devdata(file));
-	struct go7007_file *gofh;
-
-	if (go->status != STATUS_ONLINE)
-		return -EBUSY;
-	gofh = kzalloc(sizeof(struct go7007_file), GFP_KERNEL);
-	if (gofh == NULL)
-		return -ENOMEM;
-	++go->ref_count;
-	gofh->go = go;
-	mutex_init(&gofh->lock);
-	gofh->buf_count = 0;
-	file->private_data = gofh;
-	return 0;
-}
-
-static int go7007_release(struct file *file)
+static bool valid_pixelformat(u32 pixelformat)
 {
-	struct go7007_file *gofh = file->private_data;
-	struct go7007 *go = gofh->go;
-
-	if (gofh->buf_count > 0) {
-		go7007_streamoff(go);
-		go->in_use = 0;
-		kfree(gofh->bufs);
-		gofh->buf_count = 0;
+	switch (pixelformat) {
+	case V4L2_PIX_FMT_MJPEG:
+	case V4L2_PIX_FMT_MPEG1:
+	case V4L2_PIX_FMT_MPEG2:
+	case V4L2_PIX_FMT_MPEG4:
+		return true;
+	default:
+		return false;
 	}
-	kfree(gofh);
-	if (--go->ref_count == 0)
-		kfree(go);
-	file->private_data = NULL;
-	return 0;
 }
 
-static u32 get_frame_type_flag(struct go7007_buffer *gobuf, int format)
+static u32 get_frame_type_flag(struct go7007_buffer *vb, int format)
 {
-	u8 *f = page_address(gobuf->pages[0]);
+	u8 *ptr = vb2_plane_vaddr(&vb->vb, 0);
 
 	switch (format) {
-	case GO7007_FORMAT_MJPEG:
+	case V4L2_PIX_FMT_MJPEG:
 		return V4L2_BUF_FLAG_KEYFRAME;
-	case GO7007_FORMAT_MPEG4:
-		switch ((f[gobuf->frame_offset + 4] >> 6) & 0x3) {
+	case V4L2_PIX_FMT_MPEG4:
+		switch ((ptr[vb->frame_offset + 4] >> 6) & 0x3) {
 		case 0:
 			return V4L2_BUF_FLAG_KEYFRAME;
 		case 1:
@@ -145,9 +74,9 @@ static u32 get_frame_type_flag(struct go7007_buffer *gobuf, int format)
 		default:
 			return 0;
 		}
-	case GO7007_FORMAT_MPEG1:
-	case GO7007_FORMAT_MPEG2:
-		switch ((f[gobuf->frame_offset + 5] >> 3) & 0x7) {
+	case V4L2_PIX_FMT_MPEG1:
+	case V4L2_PIX_FMT_MPEG2:
+		switch ((ptr[vb->frame_offset + 5] >> 3) & 0x7) {
 		case 1:
 			return V4L2_BUF_FLAG_KEYFRAME;
 		case 2:
@@ -162,30 +91,111 @@ static u32 get_frame_type_flag(struct go7007_buffer *gobuf, int format)
 	return 0;
 }
 
-static int set_capture_size(struct go7007 *go, struct v4l2_format *fmt, int try)
+static void get_resolution(struct go7007 *go, int *width, int *height)
 {
-	int sensor_height = 0, sensor_width = 0;
-	int width, height, i;
-
-	if (fmt != NULL && fmt->fmt.pix.pixelformat != V4L2_PIX_FMT_MJPEG &&
-			fmt->fmt.pix.pixelformat != V4L2_PIX_FMT_MPEG &&
-			fmt->fmt.pix.pixelformat != V4L2_PIX_FMT_MPEG4)
-		return -EINVAL;
-
 	switch (go->standard) {
 	case GO7007_STD_NTSC:
-		sensor_width = 720;
-		sensor_height = 480;
+		*width = 720;
+		*height = 480;
 		break;
 	case GO7007_STD_PAL:
-		sensor_width = 720;
-		sensor_height = 576;
+		*width = 720;
+		*height = 576;
 		break;
 	case GO7007_STD_OTHER:
-		sensor_width = go->board_info->sensor_width;
-		sensor_height = go->board_info->sensor_height;
+	default:
+		*width = go->board_info->sensor_width;
+		*height = go->board_info->sensor_height;
+		break;
+	}
+}
+
+static void set_formatting(struct go7007 *go)
+{
+	if (go->format == V4L2_PIX_FMT_MJPEG) {
+		go->pali = 0;
+		go->aspect_ratio = GO7007_RATIO_1_1;
+		go->gop_size = 0;
+		go->ipb = 0;
+		go->closed_gop = 0;
+		go->repeat_seqhead = 0;
+		go->seq_header_enable = 0;
+		go->gop_header_enable = 0;
+		go->dvd_mode = 0;
+		return;
+	}
+
+	switch (go->format) {
+	case V4L2_PIX_FMT_MPEG1:
+		go->pali = 0;
+		break;
+	default:
+	case V4L2_PIX_FMT_MPEG2:
+		go->pali = 0x48;
+		break;
+	case V4L2_PIX_FMT_MPEG4:
+		/* For future reference: this is the list of MPEG4
+		 * profiles that are available, although they are
+		 * untested:
+		 *
+		 * Profile		pali
+		 * --------------	----
+		 * PROFILE_S_L0		0x08
+		 * PROFILE_S_L1		0x01
+		 * PROFILE_S_L2		0x02
+		 * PROFILE_S_L3		0x03
+		 * PROFILE_ARTS_L1	0x91
+		 * PROFILE_ARTS_L2	0x92
+		 * PROFILE_ARTS_L3	0x93
+		 * PROFILE_ARTS_L4	0x94
+		 * PROFILE_AS_L0	0xf0
+		 * PROFILE_AS_L1	0xf1
+		 * PROFILE_AS_L2	0xf2
+		 * PROFILE_AS_L3	0xf3
+		 * PROFILE_AS_L4	0xf4
+		 * PROFILE_AS_L5	0xf5
+		 */
+		go->pali = 0xf5;
+		break;
+	}
+	go->gop_size = v4l2_ctrl_g_ctrl(go->mpeg_video_gop_size);
+	go->closed_gop = v4l2_ctrl_g_ctrl(go->mpeg_video_gop_closure);
+	go->ipb = v4l2_ctrl_g_ctrl(go->mpeg_video_b_frames) != 0;
+	go->bitrate = v4l2_ctrl_g_ctrl(go->mpeg_video_bitrate);
+	go->repeat_seqhead = v4l2_ctrl_g_ctrl(go->mpeg_video_rep_seqheader);
+	go->gop_header_enable = 1;
+	go->dvd_mode = 0;
+	if (go->format == V4L2_PIX_FMT_MPEG2)
+		go->dvd_mode =
+			go->bitrate == 9800000 &&
+			go->gop_size == 15 &&
+			go->ipb == 0 &&
+			go->repeat_seqhead == 1 &&
+			go->closed_gop;
+
+	switch (v4l2_ctrl_g_ctrl(go->mpeg_video_aspect_ratio)) {
+	default:
+	case V4L2_MPEG_VIDEO_ASPECT_1x1:
+		go->aspect_ratio = GO7007_RATIO_1_1;
+		break;
+	case V4L2_MPEG_VIDEO_ASPECT_4x3:
+		go->aspect_ratio = GO7007_RATIO_4_3;
+		break;
+	case V4L2_MPEG_VIDEO_ASPECT_16x9:
+		go->aspect_ratio = GO7007_RATIO_16_9;
 		break;
 	}
+}
+
+static int set_capture_size(struct go7007 *go, struct v4l2_format *fmt, int try)
+{
+	int sensor_height = 0, sensor_width = 0;
+	int width, height, i;
+
+	if (fmt != NULL && !valid_pixelformat(fmt->fmt.pix.pixelformat))
+		return -EINVAL;
+
+	get_resolution(go, &sensor_width, &sensor_height);
 
 	if (fmt == NULL) {
 		width = sensor_width;
@@ -205,13 +215,12 @@ static int set_capture_size(struct go7007 *go, struct v4l2_format *fmt, int try)
 		else
 			height = fmt->fmt.pix.height & ~0x0f;
 	} else {
-		int requested_size = fmt->fmt.pix.width * fmt->fmt.pix.height;
-		int sensor_size = sensor_width * sensor_height;
+		width = fmt->fmt.pix.width;
 
-		if (64 * requested_size < 9 * sensor_size) {
+		if (width <= sensor_width / 4) {
 			width = sensor_width / 4;
 			height = sensor_height / 4;
-		} else if (64 * requested_size < 36 * sensor_size) {
+		} else if (width <= sensor_width / 2) {
 			width = sensor_width / 2;
 			height = sensor_height / 2;
 		} else {
@@ -233,12 +242,14 @@ static int set_capture_size(struct go7007 *go, struct v4l2_format *fmt, int try)
 		fmt->fmt.pix.field = V4L2_FIELD_NONE;
 		fmt->fmt.pix.bytesperline = 0;
 		fmt->fmt.pix.sizeimage = GO7007_BUF_SIZE;
-		fmt->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M; /* ?? */
+		fmt->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 	}
 
 	if (try)
 		return 0;
 
+	if (fmt)
+		go->format = fmt->fmt.pix.pixelformat;
 	go->width = width;
 	go->height = height;
 	go->encoder_h_offset = go->board_info->sensor_h_offset;
@@ -252,18 +263,11 @@ static int set_capture_size(struct go7007 *go, struct v4l2_format *fmt, int try)
 		struct v4l2_mbus_framefmt mbus_fmt;
 
 		mbus_fmt.code = V4L2_MBUS_FMT_FIXED;
-		if (fmt != NULL)
-			mbus_fmt.width = fmt->fmt.pix.width;
-		else
-			mbus_fmt.width = width;
-
-		if (height > sensor_height / 2) {
-			mbus_fmt.height = height / 2;
-			go->encoder_v_halve = 0;
-		} else {
-			mbus_fmt.height = height;
-			go->encoder_v_halve = 1;
-		}
+		mbus_fmt.width = fmt ? fmt->fmt.pix.width : width;
+		mbus_fmt.height = height;
+		go->encoder_h_halve = 0;
+		go->encoder_v_halve = 0;
+		go->encoder_subsample = 0;
 		call_all(&go->v4l2_dev, video, s_mbus_fmt, &mbus_fmt);
 	} else {
 		if (width <= sensor_width / 4) {
@@ -280,55 +284,6 @@ static int set_capture_size(struct go7007 *go, struct v4l2_format *fmt, int try)
 			go->encoder_subsample = 0;
 		}
 	}
-
-	if (fmt == NULL)
-		return 0;
-
-	switch (fmt->fmt.pix.pixelformat) {
-	case V4L2_PIX_FMT_MPEG:
-		if (go->format == GO7007_FORMAT_MPEG1 ||
-				go->format == GO7007_FORMAT_MPEG2 ||
-				go->format == GO7007_FORMAT_MPEG4)
-			break;
-		go->format = GO7007_FORMAT_MPEG1;
-		go->pali = 0;
-		go->aspect_ratio = GO7007_RATIO_1_1;
-		go->gop_size = go->sensor_framerate / 1000;
-		go->ipb = 0;
-		go->closed_gop = 1;
-		go->repeat_seqhead = 1;
-		go->seq_header_enable = 1;
-		go->gop_header_enable = 1;
-		go->dvd_mode = 0;
-		break;
-	/* Backwards compatibility only! */
-	case V4L2_PIX_FMT_MPEG4:
-		if (go->format == GO7007_FORMAT_MPEG4)
-			break;
-		go->format = GO7007_FORMAT_MPEG4;
-		go->pali = 0xf5;
-		go->aspect_ratio = GO7007_RATIO_1_1;
-		go->gop_size = go->sensor_framerate / 1000;
-		go->ipb = 0;
-		go->closed_gop = 1;
-		go->repeat_seqhead = 1;
-		go->seq_header_enable = 1;
-		go->gop_header_enable = 1;
-		go->dvd_mode = 0;
-		break;
-	case V4L2_PIX_FMT_MJPEG:
-		go->format = GO7007_FORMAT_MJPEG;
-		go->pali = 0;
-		go->aspect_ratio = GO7007_RATIO_1_1;
-		go->gop_size = 0;
-		go->ipb = 0;
-		go->closed_gop = 0;
-		go->repeat_seqhead = 0;
-		go->seq_header_enable = 0;
-		go->gop_header_enable = 0;
-		go->dvd_mode = 0;
-		break;
-	}
 	return 0;
 }
 
@@ -390,230 +345,23 @@ static int clip_to_modet_map(struct go7007 *go, int region,
 }
 #endif
 
-static int mpeg_query_ctrl(struct v4l2_queryctrl *ctrl)
-{
-	static const u32 mpeg_ctrls[] = {
-		V4L2_CID_MPEG_CLASS,
-		V4L2_CID_MPEG_STREAM_TYPE,
-		V4L2_CID_MPEG_VIDEO_ENCODING,
-		V4L2_CID_MPEG_VIDEO_ASPECT,
-		V4L2_CID_MPEG_VIDEO_GOP_SIZE,
-		V4L2_CID_MPEG_VIDEO_GOP_CLOSURE,
-		V4L2_CID_MPEG_VIDEO_BITRATE,
-		0
-	};
-	static const u32 *ctrl_classes[] = {
-		mpeg_ctrls,
-		NULL
-	};
-
-	ctrl->id = v4l2_ctrl_next(ctrl_classes, ctrl->id);
-
-	switch (ctrl->id) {
-	case V4L2_CID_MPEG_CLASS:
-		return v4l2_ctrl_query_fill(ctrl, 0, 0, 0, 0);
-	case V4L2_CID_MPEG_STREAM_TYPE:
-		return v4l2_ctrl_query_fill(ctrl,
-				V4L2_MPEG_STREAM_TYPE_MPEG2_DVD,
-				V4L2_MPEG_STREAM_TYPE_MPEG_ELEM, 1,
-				V4L2_MPEG_STREAM_TYPE_MPEG_ELEM);
-	case V4L2_CID_MPEG_VIDEO_ENCODING:
-		return v4l2_ctrl_query_fill(ctrl,
-				V4L2_MPEG_VIDEO_ENCODING_MPEG_1,
-				V4L2_MPEG_VIDEO_ENCODING_MPEG_4, 1,
-				V4L2_MPEG_VIDEO_ENCODING_MPEG_2);
-	case V4L2_CID_MPEG_VIDEO_ASPECT:
-		return v4l2_ctrl_query_fill(ctrl,
-				V4L2_MPEG_VIDEO_ASPECT_1x1,
-				V4L2_MPEG_VIDEO_ASPECT_16x9, 1,
-				V4L2_MPEG_VIDEO_ASPECT_1x1);
-	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
-		return v4l2_ctrl_query_fill(ctrl, 0, 34, 1, 15);
-	case V4L2_CID_MPEG_VIDEO_GOP_CLOSURE:
-		return v4l2_ctrl_query_fill(ctrl, 0, 1, 1, 0);
-	case V4L2_CID_MPEG_VIDEO_BITRATE:
-		return v4l2_ctrl_query_fill(ctrl,
-				64000,
-				10000000, 1,
-				1500000);
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static int mpeg_s_ctrl(struct v4l2_control *ctrl, struct go7007 *go)
-{
-	/* pretty sure we can't change any of these while streaming */
-	if (go->streaming)
-		return -EBUSY;
-
-	switch (ctrl->id) {
-	case V4L2_CID_MPEG_STREAM_TYPE:
-		switch (ctrl->value) {
-		case V4L2_MPEG_STREAM_TYPE_MPEG2_DVD:
-			go->format = GO7007_FORMAT_MPEG2;
-			go->bitrate = 9800000;
-			go->gop_size = 15;
-			go->pali = 0x48;
-			go->closed_gop = 1;
-			go->repeat_seqhead = 0;
-			go->seq_header_enable = 1;
-			go->gop_header_enable = 1;
-			go->dvd_mode = 1;
-			break;
-		case V4L2_MPEG_STREAM_TYPE_MPEG_ELEM:
-			/* todo: */
-			break;
-		default:
-			return -EINVAL;
-		}
-		break;
-	case V4L2_CID_MPEG_VIDEO_ENCODING:
-		switch (ctrl->value) {
-		case V4L2_MPEG_VIDEO_ENCODING_MPEG_1:
-			go->format = GO7007_FORMAT_MPEG1;
-			go->pali = 0;
-			break;
-		case V4L2_MPEG_VIDEO_ENCODING_MPEG_2:
-			go->format = GO7007_FORMAT_MPEG2;
-			/*if (mpeg->pali >> 24 == 2)
-				go->pali = mpeg->pali & 0xff;
-			else*/
-				go->pali = 0x48;
-			break;
-		case V4L2_MPEG_VIDEO_ENCODING_MPEG_4:
-			go->format = GO7007_FORMAT_MPEG4;
-			/*if (mpeg->pali >> 24 == 4)
-				go->pali = mpeg->pali & 0xff;
-			else*/
-				go->pali = 0xf5;
-			break;
-		default:
-			return -EINVAL;
-		}
-		go->gop_header_enable =
-			/*mpeg->flags & GO7007_MPEG_OMIT_GOP_HEADER
-			? 0 :*/ 1;
-		/*if (mpeg->flags & GO7007_MPEG_REPEAT_SEQHEADER)
-			go->repeat_seqhead = 1;
-		else*/
-			go->repeat_seqhead = 0;
-		go->dvd_mode = 0;
-		break;
-	case V4L2_CID_MPEG_VIDEO_ASPECT:
-		if (go->format == GO7007_FORMAT_MJPEG)
-			return -EINVAL;
-		switch (ctrl->value) {
-		case V4L2_MPEG_VIDEO_ASPECT_1x1:
-			go->aspect_ratio = GO7007_RATIO_1_1;
-			break;
-		case V4L2_MPEG_VIDEO_ASPECT_4x3:
-			go->aspect_ratio = GO7007_RATIO_4_3;
-			break;
-		case V4L2_MPEG_VIDEO_ASPECT_16x9:
-			go->aspect_ratio = GO7007_RATIO_16_9;
-			break;
-		case V4L2_MPEG_VIDEO_ASPECT_221x100:
-		default:
-			return -EINVAL;
-		}
-		break;
-	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
-		if (ctrl->value < 0 || ctrl->value > 34)
-			return -EINVAL;
-		go->gop_size = ctrl->value;
-		break;
-	case V4L2_CID_MPEG_VIDEO_GOP_CLOSURE:
-		if (ctrl->value != 0 && ctrl->value != 1)
-			return -EINVAL;
-		go->closed_gop = ctrl->value;
-		break;
-	case V4L2_CID_MPEG_VIDEO_BITRATE:
-		/* Upper bound is kind of arbitrary here */
-		if (ctrl->value < 64000 || ctrl->value > 10000000)
-			return -EINVAL;
-		go->bitrate = ctrl->value;
-		break;
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static int mpeg_g_ctrl(struct v4l2_control *ctrl, struct go7007 *go)
-{
-	switch (ctrl->id) {
-	case V4L2_CID_MPEG_STREAM_TYPE:
-		if (go->dvd_mode)
-			ctrl->value = V4L2_MPEG_STREAM_TYPE_MPEG2_DVD;
-		else
-			ctrl->value = V4L2_MPEG_STREAM_TYPE_MPEG_ELEM;
-		break;
-	case V4L2_CID_MPEG_VIDEO_ENCODING:
-		switch (go->format) {
-		case GO7007_FORMAT_MPEG1:
-			ctrl->value = V4L2_MPEG_VIDEO_ENCODING_MPEG_1;
-			break;
-		case GO7007_FORMAT_MPEG2:
-			ctrl->value = V4L2_MPEG_VIDEO_ENCODING_MPEG_2;
-			break;
-		case GO7007_FORMAT_MPEG4:
-			ctrl->value = V4L2_MPEG_VIDEO_ENCODING_MPEG_4;
-			break;
-		default:
-			return -EINVAL;
-		}
-		break;
-	case V4L2_CID_MPEG_VIDEO_ASPECT:
-		switch (go->aspect_ratio) {
-		case GO7007_RATIO_1_1:
-			ctrl->value = V4L2_MPEG_VIDEO_ASPECT_1x1;
-			break;
-		case GO7007_RATIO_4_3:
-			ctrl->value = V4L2_MPEG_VIDEO_ASPECT_4x3;
-			break;
-		case GO7007_RATIO_16_9:
-			ctrl->value = V4L2_MPEG_VIDEO_ASPECT_16x9;
-			break;
-		default:
-			return -EINVAL;
-		}
-		break;
-	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
-		ctrl->value = go->gop_size;
-		break;
-	case V4L2_CID_MPEG_VIDEO_GOP_CLOSURE:
-		ctrl->value = go->closed_gop;
-		break;
-	case V4L2_CID_MPEG_VIDEO_BITRATE:
-		ctrl->value = go->bitrate;
-		break;
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
 static int vidioc_querycap(struct file *file, void  *priv,
 					struct v4l2_capability *cap)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
 	strlcpy(cap->driver, "go7007", sizeof(cap->driver));
 	strlcpy(cap->card, go->name, sizeof(cap->card));
-#if 0
-	strlcpy(cap->bus_info, dev_name(&dev->udev->dev), sizeof(cap->bus_info));
-#endif
-
-	cap->version = KERNEL_VERSION(0, 9, 8);
+	strlcpy(cap->bus_info, go->bus_info, sizeof(cap->bus_info));
 
-	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE |
-			    V4L2_CAP_STREAMING; /* | V4L2_CAP_AUDIO; */
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE |
+				V4L2_CAP_STREAMING;
 
+	if (go->board_info->num_aud_inputs)
+		cap->device_caps |= V4L2_CAP_AUDIO;
 	if (go->board_info->flags & GO7007_BOARD_HAS_TUNER)
-		cap->capabilities |= V4L2_CAP_TUNER;
-
+		cap->device_caps |= V4L2_CAP_TUNER;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
@@ -625,11 +373,19 @@ static int vidioc_enum_fmt_vid_cap(struct file *file, void  *priv,
 	switch (fmt->index) {
 	case 0:
 		fmt->pixelformat = V4L2_PIX_FMT_MJPEG;
-		desc = "Motion-JPEG";
+		desc = "Motion JPEG";
 		break;
 	case 1:
-		fmt->pixelformat = V4L2_PIX_FMT_MPEG;
-		desc = "MPEG1/MPEG2/MPEG4";
+		fmt->pixelformat = V4L2_PIX_FMT_MPEG1;
+		desc = "MPEG-1 ES";
+		break;
+	case 2:
+		fmt->pixelformat = V4L2_PIX_FMT_MPEG2;
+		desc = "MPEG-2 ES";
+		break;
+	case 3:
+		fmt->pixelformat = V4L2_PIX_FMT_MPEG4;
+		desc = "MPEG-4 ES";
 		break;
 	default:
 		return -EINVAL;
@@ -645,13 +401,12 @@ 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 go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
 	fmt->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 	fmt->fmt.pix.width = go->width;
 	fmt->fmt.pix.height = go->height;
-	fmt->fmt.pix.pixelformat = (go->format == GO7007_FORMAT_MJPEG) ?
-				   V4L2_PIX_FMT_MJPEG : V4L2_PIX_FMT_MPEG;
+	fmt->fmt.pix.pixelformat = go->format;
 	fmt->fmt.pix.field = V4L2_FIELD_NONE;
 	fmt->fmt.pix.bytesperline = 0;
 	fmt->fmt.pix.sizeimage = GO7007_BUF_SIZE;
@@ -663,7 +418,7 @@ static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 			struct v4l2_format *fmt)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
 	return set_capture_size(go, fmt, 1);
 }
@@ -671,348 +426,137 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 			struct v4l2_format *fmt)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
-	if (go->streaming)
+	if (vb2_is_busy(&go->vidq))
 		return -EBUSY;
 
 	return set_capture_size(go, fmt, 0);
 }
 
-static int vidioc_reqbufs(struct file *file, void *priv,
-			  struct v4l2_requestbuffers *req)
+static int go7007_queue_setup(struct vb2_queue *q, const struct v4l2_format *fmt,
+		unsigned int *num_buffers, unsigned int *num_planes,
+		unsigned int sizes[], void *alloc_ctxs[])
 {
-	struct go7007_file *gofh = priv;
-	struct go7007 *go = gofh->go;
-	int retval = -EBUSY;
-	unsigned int count, i;
-
-	if (go->streaming)
-		return retval;
-
-	if (req->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
-			req->memory != V4L2_MEMORY_MMAP)
-		return -EINVAL;
-
-	mutex_lock(&gofh->lock);
-	for (i = 0; i < gofh->buf_count; ++i)
-		if (gofh->bufs[i].mapped > 0)
-			goto unlock_and_return;
-
-	mutex_lock(&go->hw_lock);
-	if (go->in_use > 0 && gofh->buf_count == 0) {
-		mutex_unlock(&go->hw_lock);
-		goto unlock_and_return;
-	}
+	sizes[0] = GO7007_BUF_SIZE;
+	*num_planes = 1;
 
-	if (gofh->buf_count > 0)
-		kfree(gofh->bufs);
-
-	retval = -ENOMEM;
-	count = req->count;
-	if (count > 0) {
-		if (count < 2)
-			count = 2;
-		if (count > 32)
-			count = 32;
-
-		gofh->bufs = kcalloc(count, sizeof(struct go7007_buffer),
-				     GFP_KERNEL);
-
-		if (!gofh->bufs) {
-			mutex_unlock(&go->hw_lock);
-			goto unlock_and_return;
-		}
-
-		for (i = 0; i < count; ++i) {
-			gofh->bufs[i].go = go;
-			gofh->bufs[i].index = i;
-			gofh->bufs[i].state = BUF_STATE_IDLE;
-			gofh->bufs[i].mapped = 0;
-		}
-
-		go->in_use = 1;
-	} else {
-		go->in_use = 0;
-	}
-
-	gofh->buf_count = count;
-	mutex_unlock(&go->hw_lock);
-	mutex_unlock(&gofh->lock);
-
-	memset(req, 0, sizeof(*req));
-
-	req->count = count;
-	req->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	req->memory = V4L2_MEMORY_MMAP;
-
-	return 0;
-
-unlock_and_return:
-	mutex_unlock(&gofh->lock);
-	return retval;
-}
-
-static int vidioc_querybuf(struct file *file, void *priv,
-			   struct v4l2_buffer *buf)
-{
-	struct go7007_file *gofh = priv;
-	int retval = -EINVAL;
-	unsigned int index;
-
-	if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return retval;
-
-	index = buf->index;
-
-	mutex_lock(&gofh->lock);
-	if (index >= gofh->buf_count)
-		goto unlock_and_return;
-
-	memset(buf, 0, sizeof(*buf));
-	buf->index = index;
-	buf->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
-	switch (gofh->bufs[index].state) {
-	case BUF_STATE_QUEUED:
-		buf->flags = V4L2_BUF_FLAG_QUEUED;
-		break;
-	case BUF_STATE_DONE:
-		buf->flags = V4L2_BUF_FLAG_DONE;
-		break;
-	default:
-		buf->flags = 0;
-	}
-
-	if (gofh->bufs[index].mapped)
-		buf->flags |= V4L2_BUF_FLAG_MAPPED;
-	buf->memory = V4L2_MEMORY_MMAP;
-	buf->m.offset = index * GO7007_BUF_SIZE;
-	buf->length = GO7007_BUF_SIZE;
-	mutex_unlock(&gofh->lock);
+	if (*num_buffers < 2)
+		*num_buffers = 2;
 
 	return 0;
-
-unlock_and_return:
-	mutex_unlock(&gofh->lock);
-	return retval;
 }
 
-static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
+static void go7007_buf_queue(struct vb2_buffer *vb)
 {
-	struct go7007_file *gofh = priv;
-	struct go7007 *go = gofh->go;
-	struct go7007_buffer *gobuf;
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct go7007 *go = vb2_get_drv_priv(vq);
+	struct go7007_buffer *go7007_vb =
+		container_of(vb, struct go7007_buffer, vb);
 	unsigned long flags;
-	int retval = -EINVAL;
-	int ret;
 
-	if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
-			buf->memory != V4L2_MEMORY_MMAP)
-		return retval;
-
-	mutex_lock(&gofh->lock);
-	if (buf->index >= gofh->buf_count)
-		goto unlock_and_return;
-
-	gobuf = &gofh->bufs[buf->index];
-	if (!gobuf->mapped)
-		goto unlock_and_return;
-
-	retval = -EBUSY;
-	if (gobuf->state != BUF_STATE_IDLE)
-		goto unlock_and_return;
-
-	/* offset will be 0 until we really support USERPTR streaming */
-	gobuf->offset = gobuf->user_addr & ~PAGE_MASK;
-	gobuf->bytesused = 0;
-	gobuf->frame_offset = 0;
-	gobuf->modet_active = 0;
-	if (gobuf->offset > 0)
-		gobuf->page_count = GO7007_BUF_PAGES + 1;
-	else
-		gobuf->page_count = GO7007_BUF_PAGES;
-
-	retval = -ENOMEM;
-	down_read(&current->mm->mmap_sem);
-	ret = get_user_pages(current, current->mm,
-			gobuf->user_addr & PAGE_MASK, gobuf->page_count,
-			1, 1, gobuf->pages, NULL);
-	up_read(&current->mm->mmap_sem);
-
-	if (ret != gobuf->page_count) {
-		int i;
-		for (i = 0; i < ret; ++i)
-			page_cache_release(gobuf->pages[i]);
-		gobuf->page_count = 0;
-		goto unlock_and_return;
-	}
-
-	gobuf->state = BUF_STATE_QUEUED;
 	spin_lock_irqsave(&go->spinlock, flags);
-	list_add_tail(&gobuf->stream, &go->stream);
+	list_add_tail(&go7007_vb->list, &go->vidq_active);
 	spin_unlock_irqrestore(&go->spinlock, flags);
-	mutex_unlock(&gofh->lock);
+}
 
-	return 0;
+static int go7007_buf_prepare(struct vb2_buffer *vb)
+{
+	struct go7007_buffer *go7007_vb =
+		container_of(vb, struct go7007_buffer, vb);
 
-unlock_and_return:
-	mutex_unlock(&gofh->lock);
-	return retval;
+	go7007_vb->modet_active = 0;
+	go7007_vb->frame_offset = 0;
+	vb->v4l2_planes[0].bytesused = 0;
+	return 0;
 }
 
-
-static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
+static int go7007_buf_finish(struct vb2_buffer *vb)
 {
-	struct go7007_file *gofh = priv;
-	struct go7007 *go = gofh->go;
-	struct go7007_buffer *gobuf;
-	int retval = -EINVAL;
-	unsigned long flags;
-	u32 frame_type_flag;
-	DEFINE_WAIT(wait);
-
-	if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return retval;
-	if (buf->memory != V4L2_MEMORY_MMAP)
-		return retval;
-
-	mutex_lock(&gofh->lock);
-	if (list_empty(&go->stream))
-		goto unlock_and_return;
-	gobuf = list_entry(go->stream.next,
-			struct go7007_buffer, stream);
-
-	retval = -EAGAIN;
-	if (gobuf->state != BUF_STATE_DONE &&
-			!(file->f_flags & O_NONBLOCK)) {
-		for (;;) {
-			prepare_to_wait(&go->frame_waitq, &wait,
-					TASK_INTERRUPTIBLE);
-			if (gobuf->state == BUF_STATE_DONE)
-				break;
-			if (signal_pending(current)) {
-				retval = -ERESTARTSYS;
-				break;
-			}
-			schedule();
-		}
-		finish_wait(&go->frame_waitq, &wait);
-	}
-	if (gobuf->state != BUF_STATE_DONE)
-		goto unlock_and_return;
-
-	spin_lock_irqsave(&go->spinlock, flags);
-	deactivate_buffer(gobuf);
-	spin_unlock_irqrestore(&go->spinlock, flags);
-	frame_type_flag = get_frame_type_flag(gobuf, go->format);
-	gobuf->state = BUF_STATE_IDLE;
-
-	memset(buf, 0, sizeof(*buf));
-	buf->index = gobuf->index;
-	buf->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	buf->bytesused = gobuf->bytesused;
-	buf->flags = V4L2_BUF_FLAG_MAPPED | frame_type_flag;
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct go7007 *go = vb2_get_drv_priv(vq);
+	struct go7007_buffer *go7007_vb =
+		container_of(vb, struct go7007_buffer, vb);
+	u32 frame_type_flag = get_frame_type_flag(go7007_vb, go->format);
+	struct v4l2_buffer *buf = &vb->v4l2_buf;
+
+	buf->flags &= ~(V4L2_BUF_FLAG_KEYFRAME | V4L2_BUF_FLAG_BFRAME |
+			V4L2_BUF_FLAG_PFRAME);
+	buf->flags |= frame_type_flag;
 	buf->field = V4L2_FIELD_NONE;
-	buf->timestamp = gobuf->timestamp;
-	buf->sequence = gobuf->seq;
-	buf->memory = V4L2_MEMORY_MMAP;
-	buf->m.offset = gobuf->index * GO7007_BUF_SIZE;
-	buf->length = GO7007_BUF_SIZE;
-	buf->reserved = gobuf->modet_active;
-
-	mutex_unlock(&gofh->lock);
 	return 0;
-
-unlock_and_return:
-	mutex_unlock(&gofh->lock);
-	return retval;
 }
 
-static int vidioc_streamon(struct file *file, void *priv,
-					enum v4l2_buf_type type)
+static int go7007_start_streaming(struct vb2_queue *q, unsigned int count)
 {
-	struct go7007_file *gofh = priv;
-	struct go7007 *go = gofh->go;
-	int retval = 0;
-
-	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
+	struct go7007 *go = vb2_get_drv_priv(q);
+	int ret;
 
-	mutex_lock(&gofh->lock);
+	set_formatting(go);
 	mutex_lock(&go->hw_lock);
-
-	if (!go->streaming) {
-		go->streaming = 1;
-		go->next_seq = 0;
-		go->active_buf = NULL;
-		if (go7007_start_encoder(go) < 0)
-			retval = -EIO;
-		else
-			retval = 0;
-	}
+	go->next_seq = 0;
+	go->active_buf = NULL;
+	q->streaming = 1;
+	if (go7007_start_encoder(go) < 0)
+		ret = -EIO;
+	else
+		ret = 0;
 	mutex_unlock(&go->hw_lock);
-	mutex_unlock(&gofh->lock);
+	if (ret) {
+		q->streaming = 0;
+		return ret;
+	}
 	call_all(&go->v4l2_dev, video, s_stream, 1);
-
-	return retval;
+	v4l2_ctrl_grab(go->mpeg_video_gop_size, true);
+	v4l2_ctrl_grab(go->mpeg_video_gop_closure, true);
+	v4l2_ctrl_grab(go->mpeg_video_bitrate, true);
+	v4l2_ctrl_grab(go->mpeg_video_aspect_ratio, true);
+	/* Turn on Capture LED */
+	if (go->board_id == GO7007_BOARDID_ADS_USBAV_709)
+		go7007_write_addr(go, 0x3c82, 0x0005);
+	return ret;
 }
 
-static int vidioc_streamoff(struct file *file, void *priv,
-					enum v4l2_buf_type type)
+static int go7007_stop_streaming(struct vb2_queue *q)
 {
-	struct go7007_file *gofh = priv;
-	struct go7007 *go = gofh->go;
+	struct go7007 *go = vb2_get_drv_priv(q);
+	unsigned long flags;
 
-	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-	mutex_lock(&gofh->lock);
-	go7007_streamoff(go);
-	mutex_unlock(&gofh->lock);
+	q->streaming = 0;
+	go7007_stream_stop(go);
+	mutex_lock(&go->hw_lock);
+	go7007_reset_encoder(go);
+	mutex_unlock(&go->hw_lock);
 	call_all(&go->v4l2_dev, video, s_stream, 0);
 
+	spin_lock_irqsave(&go->spinlock, flags);
+	INIT_LIST_HEAD(&go->vidq_active);
+	spin_unlock_irqrestore(&go->spinlock, flags);
+	v4l2_ctrl_grab(go->mpeg_video_gop_size, false);
+	v4l2_ctrl_grab(go->mpeg_video_gop_closure, false);
+	v4l2_ctrl_grab(go->mpeg_video_bitrate, false);
+	v4l2_ctrl_grab(go->mpeg_video_aspect_ratio, false);
+	/* Turn on Capture LED */
+	if (go->board_id == GO7007_BOARDID_ADS_USBAV_709)
+		go7007_write_addr(go, 0x3c82, 0x000d);
 	return 0;
 }
 
-static int vidioc_queryctrl(struct file *file, void *priv,
-			   struct v4l2_queryctrl *query)
-{
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
-	int id = query->id;
-
-	if (0 == call_all(&go->v4l2_dev, core, queryctrl, query))
-		return 0;
-
-	query->id = id;
-	return mpeg_query_ctrl(query);
-}
-
-static int vidioc_g_ctrl(struct file *file, void *priv,
-				struct v4l2_control *ctrl)
-{
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
-
-	if (0 == call_all(&go->v4l2_dev, core, g_ctrl, ctrl))
-		return 0;
-
-	return mpeg_g_ctrl(ctrl, go);
-}
-
-static int vidioc_s_ctrl(struct file *file, void *priv,
-				struct v4l2_control *ctrl)
-{
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
-
-	if (0 == call_all(&go->v4l2_dev, core, s_ctrl, ctrl))
-		return 0;
-
-	return mpeg_s_ctrl(ctrl, go);
-}
+static struct vb2_ops go7007_video_qops = {
+	.queue_setup    = go7007_queue_setup,
+	.buf_queue      = go7007_buf_queue,
+	.buf_prepare    = go7007_buf_prepare,
+	.buf_finish     = go7007_buf_finish,
+	.start_streaming = go7007_start_streaming,
+	.stop_streaming = go7007_stop_streaming,
+	.wait_prepare   = vb2_ops_wait_prepare,
+	.wait_finish    = vb2_ops_wait_finish,
+};
 
 static int vidioc_g_parm(struct file *filp, void *priv,
 		struct v4l2_streamparm *parm)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(filp);
 	struct v4l2_fract timeperframe = {
 		.numerator = 1001 *  go->fps_scale,
 		.denominator = go->sensor_framerate,
@@ -1021,7 +565,8 @@ static int vidioc_g_parm(struct file *filp, void *priv,
 	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
 
-	parm->parm.capture.capability |= V4L2_CAP_TIMEPERFRAME;
+	parm->parm.capture.readbuffers = 2;
+	parm->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
 	parm->parm.capture.timeperframe = timeperframe;
 
 	return 0;
@@ -1030,13 +575,11 @@ static int vidioc_g_parm(struct file *filp, void *priv,
 static int vidioc_s_parm(struct file *filp, void *priv,
 		struct v4l2_streamparm *parm)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(filp);
 	unsigned int n, d;
 
 	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
-	if (parm->parm.capture.capturemode != 0)
-		return -EINVAL;
 
 	n = go->sensor_framerate *
 		parm->parm.capture.timeperframe.numerator;
@@ -1046,7 +589,7 @@ static int vidioc_s_parm(struct file *filp, void *priv,
 	else
 		go->fps_scale = 1;
 
-	return 0;
+	return vidioc_g_parm(filp, priv, parm);
 }
 
 /* VIDIOC_ENUMSTD on go7007 were used for enumerating the supported fps and
@@ -1062,121 +605,96 @@ static int vidioc_s_parm(struct file *filp, void *priv,
 static int vidioc_enum_framesizes(struct file *filp, void *priv,
 				  struct v4l2_frmsizeenum *fsize)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(filp);
+	int width, height;
 
-	/* Return -EINVAL, if it is a TV board */
-	if ((go->board_info->flags & GO7007_BOARD_HAS_TUNER) ||
-	    (go->board_info->sensor_flags & GO7007_SENSOR_TV))
+	if (fsize->index > 2)
 		return -EINVAL;
 
-	if (fsize->index > 0)
+	if (!valid_pixelformat(fsize->pixel_format))
 		return -EINVAL;
 
+	get_resolution(go, &width, &height);
 	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
-	fsize->discrete.width = go->board_info->sensor_width;
-	fsize->discrete.height = go->board_info->sensor_height;
-
+	fsize->discrete.width = (width >> fsize->index) & ~0xf;
+	fsize->discrete.height = (height >> fsize->index) & ~0xf;
 	return 0;
 }
 
 static int vidioc_enum_frameintervals(struct file *filp, void *priv,
 				      struct v4l2_frmivalenum *fival)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(filp);
+	int width, height;
+	int i;
 
-	/* Return -EINVAL, if it is a TV board */
-	if ((go->board_info->flags & GO7007_BOARD_HAS_TUNER) ||
-	    (go->board_info->sensor_flags & GO7007_SENSOR_TV))
+	if (fival->index > 4)
 		return -EINVAL;
 
-	if (fival->index > 0)
+	if (!valid_pixelformat(fival->pixel_format))
 		return -EINVAL;
 
+	if (!(go->board_info->sensor_flags & GO7007_SENSOR_SCALING)) {
+		get_resolution(go, &width, &height);
+		for (i = 0; i <= 2; i++)
+			if (fival->width == ((width >> i) & ~0xf) &&
+			    fival->height == ((height >> i) & ~0xf))
+				break;
+		if (i > 2)
+			return -EINVAL;
+	}
 	fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
-	fival->discrete.numerator = 1001;
-	fival->discrete.denominator = go->board_info->sensor_framerate;
-
+	fival->discrete.numerator = 1001 * (fival->index + 1);
+	fival->discrete.denominator = go->sensor_framerate;
 	return 0;
 }
 
 static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *std)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
-	switch (go->standard) {
-	case GO7007_STD_NTSC:
-		*std = V4L2_STD_NTSC;
-		break;
-	case GO7007_STD_PAL:
-		*std = V4L2_STD_PAL;
-		break;
-	default:
-		return -EINVAL;
+	*std = go->std;
+	return 0;
+}
+
+static int go7007_s_std(struct go7007 *go)
+{
+	if (go->std & V4L2_STD_625_50) {
+		go->standard = GO7007_STD_PAL;
+		go->sensor_framerate = 25025;
+	} else {
+		go->standard = GO7007_STD_NTSC;
+		go->sensor_framerate = 30000;
 	}
 
+	call_all(&go->v4l2_dev, core, s_std, go->std);
+	set_capture_size(go, NULL, 0);
 	return 0;
 }
 
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *std)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id std)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
-	if (go->streaming)
+	if (vb2_is_busy(&go->vidq))
 		return -EBUSY;
 
-	if (!(go->board_info->sensor_flags & GO7007_SENSOR_TV) && *std != 0)
-		return -EINVAL;
-
-	if (*std == 0)
-		return -EINVAL;
-
-	if ((go->board_info->flags & GO7007_BOARD_HAS_TUNER) &&
-			go->input == go->board_info->num_inputs - 1) {
-		if (!go->i2c_adapter_online)
-			return -EIO;
-		if (call_all(&go->v4l2_dev, core, s_std, *std) < 0)
-			return -EINVAL;
-	}
-
-	if (*std & V4L2_STD_NTSC) {
-		go->standard = GO7007_STD_NTSC;
-		go->sensor_framerate = 30000;
-	} else if (*std & V4L2_STD_PAL) {
-		go->standard = GO7007_STD_PAL;
-		go->sensor_framerate = 25025;
-	} else if (*std & V4L2_STD_SECAM) {
-		go->standard = GO7007_STD_PAL;
-		go->sensor_framerate = 25025;
-	} else
-		return -EINVAL;
-
-	call_all(&go->v4l2_dev, core, s_std, *std);
-	set_capture_size(go, NULL, 0);
+	go->std = std;
 
-	return 0;
+	return go7007_s_std(go);
 }
 
 static int vidioc_querystd(struct file *file, void *priv, v4l2_std_id *std)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
-
-	if ((go->board_info->flags & GO7007_BOARD_HAS_TUNER) &&
-			go->input == go->board_info->num_inputs - 1) {
-		if (!go->i2c_adapter_online)
-			return -EIO;
-		return call_all(&go->v4l2_dev, video, querystd, std);
-	} else if (go->board_info->sensor_flags & GO7007_SENSOR_TV)
-		*std = V4L2_STD_NTSC | V4L2_STD_PAL | V4L2_STD_SECAM;
-	else
-		*std = 0;
+	struct go7007 *go = video_drvdata(file);
 
-	return 0;
+	return call_all(&go->v4l2_dev, video, querystd, std);
 }
 
 static int vidioc_enum_input(struct file *file, void *priv,
 				struct v4l2_input *inp)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
 	if (inp->index >= go->board_info->num_inputs)
 		return -EINVAL;
@@ -1184,18 +702,20 @@ static int vidioc_enum_input(struct file *file, void *priv,
 	strncpy(inp->name, go->board_info->inputs[inp->index].name,
 			sizeof(inp->name));
 
-	/* If this board has a tuner, it will be the last input */
+	/* If this board has a tuner, it will be the first input */
 	if ((go->board_info->flags & GO7007_BOARD_HAS_TUNER) &&
-			inp->index == go->board_info->num_inputs - 1)
+			inp->index == 0)
 		inp->type = V4L2_INPUT_TYPE_TUNER;
 	else
 		inp->type = V4L2_INPUT_TYPE_CAMERA;
 
-	inp->audioset = 0;
+	if (go->board_info->num_aud_inputs)
+		inp->audioset = (1 << go->board_info->num_aud_inputs) - 1;
+	else
+		inp->audioset = 0;
 	inp->tuner = 0;
 	if (go->board_info->sensor_flags & GO7007_SENSOR_TV)
-		inp->std = V4L2_STD_NTSC | V4L2_STD_PAL |
-						V4L2_STD_SECAM;
+		inp->std = video_devdata(file)->tvnorms;
 	else
 		inp->std = 0;
 
@@ -1205,203 +725,128 @@ static int vidioc_enum_input(struct file *file, void *priv,
 
 static int vidioc_g_input(struct file *file, void *priv, unsigned int *input)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
 	*input = go->input;
 
 	return 0;
 }
 
-static int vidioc_s_input(struct file *file, void *priv, unsigned int input)
+static int vidioc_enumaudio(struct file *file, void *fh, struct v4l2_audio *a)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
-	if (input >= go->board_info->num_inputs)
+	if (a->index >= go->board_info->num_aud_inputs)
 		return -EINVAL;
-	if (go->streaming)
-		return -EBUSY;
+	strlcpy(a->name, go->board_info->aud_inputs[a->index].name, sizeof(a->name));
+	a->capability = V4L2_AUDCAP_STEREO;
+	return 0;
+}
 
-	go->input = input;
+static int vidioc_g_audio(struct file *file, void *fh, struct v4l2_audio *a)
+{
+	struct go7007 *go = video_drvdata(file);
 
-	return call_all(&go->v4l2_dev, video, s_routing, input, 0, 0);
+	a->index = go->aud_input;
+	strlcpy(a->name, go->board_info->aud_inputs[go->aud_input].name, sizeof(a->name));
+	a->capability = V4L2_AUDCAP_STEREO;
+	return 0;
 }
 
-static int vidioc_g_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *t)
+static int vidioc_s_audio(struct file *file, void *fh, const struct v4l2_audio *a)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
-	if (!(go->board_info->flags & GO7007_BOARD_HAS_TUNER))
-		return -EINVAL;
-	if (t->index != 0)
+	if (a->index >= go->board_info->num_aud_inputs)
 		return -EINVAL;
-	if (!go->i2c_adapter_online)
-		return -EIO;
-
-	return call_all(&go->v4l2_dev, tuner, g_tuner, t);
+	go->aud_input = a->index;
+	v4l2_subdev_call(go->sd_audio, audio, s_routing,
+			go->board_info->aud_inputs[go->aud_input].audio_input, 0, 0);
+	return 0;
 }
 
-static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *t)
+static void go7007_s_input(struct go7007 *go)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	unsigned int input = go->input;
 
-	if (!(go->board_info->flags & GO7007_BOARD_HAS_TUNER))
-		return -EINVAL;
-	if (t->index != 0)
-		return -EINVAL;
-	if (!go->i2c_adapter_online)
-		return -EIO;
-
-	switch (go->board_id) {
-	case GO7007_BOARDID_PX_TV402U_NA:
-	case GO7007_BOARDID_PX_TV402U_JP:
-		/* No selectable options currently */
-		if (t->audmode != V4L2_TUNER_MODE_STEREO)
-			return -EINVAL;
-		break;
-	}
+	v4l2_subdev_call(go->sd_video, video, s_routing,
+			go->board_info->inputs[input].video_input, 0,
+			go->board_info->video_config);
+	if (go->board_info->num_aud_inputs) {
+		int aud_input = go->board_info->inputs[input].audio_index;
 
-	return call_all(&go->v4l2_dev, tuner, s_tuner, t);
+		v4l2_subdev_call(go->sd_audio, audio, s_routing,
+			go->board_info->aud_inputs[aud_input].audio_input, 0, 0);
+		go->aud_input = aud_input;
+	}
 }
 
-static int vidioc_g_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
+static int vidioc_s_input(struct file *file, void *priv, unsigned int input)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
-	if (!(go->board_info->flags & GO7007_BOARD_HAS_TUNER))
+	if (input >= go->board_info->num_inputs)
 		return -EINVAL;
-	if (!go->i2c_adapter_online)
-		return -EIO;
+	if (vb2_is_busy(&go->vidq))
+		return -EBUSY;
 
-	f->type = V4L2_TUNER_ANALOG_TV;
+	go->input = input;
+	go7007_s_input(go);
 
-	return call_all(&go->v4l2_dev, tuner, g_frequency, f);
+	return 0;
 }
 
-static int vidioc_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
+static int vidioc_g_tuner(struct file *file, void *priv,
+				struct v4l2_tuner *t)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
-	if (!(go->board_info->flags & GO7007_BOARD_HAS_TUNER))
+	if (t->index != 0)
 		return -EINVAL;
-	if (!go->i2c_adapter_online)
-		return -EIO;
 
-	return call_all(&go->v4l2_dev, tuner, s_frequency, f);
+	strlcpy(t->name, "Tuner", sizeof(t->name));
+	return call_all(&go->v4l2_dev, tuner, g_tuner, t);
 }
 
-static int vidioc_cropcap(struct file *file, void *priv,
-					struct v4l2_cropcap *cropcap)
+static int vidioc_s_tuner(struct file *file, void *priv,
+				const struct v4l2_tuner *t)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
-	if (cropcap->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+	if (t->index != 0)
 		return -EINVAL;
 
-	/* These specify the raw input of the sensor */
-	switch (go->standard) {
-	case GO7007_STD_NTSC:
-		cropcap->bounds.top = 0;
-		cropcap->bounds.left = 0;
-		cropcap->bounds.width = 720;
-		cropcap->bounds.height = 480;
-		cropcap->defrect.top = 0;
-		cropcap->defrect.left = 0;
-		cropcap->defrect.width = 720;
-		cropcap->defrect.height = 480;
-		break;
-	case GO7007_STD_PAL:
-		cropcap->bounds.top = 0;
-		cropcap->bounds.left = 0;
-		cropcap->bounds.width = 720;
-		cropcap->bounds.height = 576;
-		cropcap->defrect.top = 0;
-		cropcap->defrect.left = 0;
-		cropcap->defrect.width = 720;
-		cropcap->defrect.height = 576;
-		break;
-	case GO7007_STD_OTHER:
-		cropcap->bounds.top = 0;
-		cropcap->bounds.left = 0;
-		cropcap->bounds.width = go->board_info->sensor_width;
-		cropcap->bounds.height = go->board_info->sensor_height;
-		cropcap->defrect.top = 0;
-		cropcap->defrect.left = 0;
-		cropcap->defrect.width = go->board_info->sensor_width;
-		cropcap->defrect.height = go->board_info->sensor_height;
-		break;
-	}
-
-	return 0;
+	return call_all(&go->v4l2_dev, tuner, s_tuner, t);
 }
 
-static int vidioc_g_crop(struct file *file, void *priv, struct v4l2_crop *crop)
+static int vidioc_g_frequency(struct file *file, void *priv,
+				struct v4l2_frequency *f)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
-	if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+	if (f->tuner)
 		return -EINVAL;
 
-	crop->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
-	/* These specify the raw input of the sensor */
-	switch (go->standard) {
-	case GO7007_STD_NTSC:
-		crop->c.top = 0;
-		crop->c.left = 0;
-		crop->c.width = 720;
-		crop->c.height = 480;
-		break;
-	case GO7007_STD_PAL:
-		crop->c.top = 0;
-		crop->c.left = 0;
-		crop->c.width = 720;
-		crop->c.height = 576;
-		break;
-	case GO7007_STD_OTHER:
-		crop->c.top = 0;
-		crop->c.left = 0;
-		crop->c.width = go->board_info->sensor_width;
-		crop->c.height = go->board_info->sensor_height;
-		break;
-	}
-
-	return 0;
+	return call_all(&go->v4l2_dev, tuner, g_frequency, f);
 }
 
-/* FIXME: vidioc_s_crop is not really implemented!!!
- */
-static int vidioc_s_crop(struct file *file, void *priv, const struct v4l2_crop *crop)
+static int vidioc_s_frequency(struct file *file, void *priv,
+				const struct v4l2_frequency *f)
 {
-	if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	return 0;
-}
+	struct go7007 *go = video_drvdata(file);
 
-static int vidioc_g_jpegcomp(struct file *file, void *priv,
-			 struct v4l2_jpegcompression *params)
-{
-	memset(params, 0, sizeof(*params));
-	params->quality = 50; /* ?? */
-	params->jpeg_markers = V4L2_JPEG_MARKER_DHT |
-				V4L2_JPEG_MARKER_DQT;
+	if (f->tuner)
+		return -EINVAL;
 
-	return 0;
+	return call_all(&go->v4l2_dev, tuner, s_frequency, f);
 }
 
-static int vidioc_s_jpegcomp(struct file *file, void *priv,
-			 const struct v4l2_jpegcompression *params)
+static int vidioc_log_status(struct file *file, void *priv)
 {
-	if (params->quality != 50 ||
-			params->jpeg_markers != (V4L2_JPEG_MARKER_DHT |
-						V4L2_JPEG_MARKER_DQT))
-		return -EINVAL;
+	struct go7007 *go = video_drvdata(file);
 
-	return 0;
+	v4l2_ctrl_log_status(file, priv);
+	return call_all(&go->v4l2_dev, core, log_status);
 }
 
 /* FIXME:
@@ -1412,180 +857,6 @@ static int vidioc_s_jpegcomp(struct file *file, void *priv,
  */
 
 #if 0
-	/* Temporary ioctls for controlling compression characteristics */
-	case GO7007IOC_S_BITRATE:
-	{
-		int *bitrate = arg;
-
-		if (go->streaming)
-			return -EINVAL;
-		/* Upper bound is kind of arbitrary here */
-		if (*bitrate < 64000 || *bitrate > 10000000)
-			return -EINVAL;
-		go->bitrate = *bitrate;
-		return 0;
-	}
-	case GO7007IOC_G_BITRATE:
-	{
-		int *bitrate = arg;
-
-		*bitrate = go->bitrate;
-		return 0;
-	}
-	case GO7007IOC_S_COMP_PARAMS:
-	{
-		struct go7007_comp_params *comp = arg;
-
-		if (go->format == GO7007_FORMAT_MJPEG)
-			return -EINVAL;
-		if (comp->gop_size > 0)
-			go->gop_size = comp->gop_size;
-		else
-			go->gop_size = go->sensor_framerate / 1000;
-		if (go->gop_size != 15)
-			go->dvd_mode = 0;
-		/*go->ipb = comp->max_b_frames > 0;*/ /* completely untested */
-		if (go->board_info->sensor_flags & GO7007_SENSOR_TV) {
-			switch (comp->aspect_ratio) {
-			case GO7007_ASPECT_RATIO_4_3_NTSC:
-			case GO7007_ASPECT_RATIO_4_3_PAL:
-				go->aspect_ratio = GO7007_RATIO_4_3;
-				break;
-			case GO7007_ASPECT_RATIO_16_9_NTSC:
-			case GO7007_ASPECT_RATIO_16_9_PAL:
-				go->aspect_ratio = GO7007_RATIO_16_9;
-				break;
-			default:
-				go->aspect_ratio = GO7007_RATIO_1_1;
-				break;
-			}
-		}
-		if (comp->flags & GO7007_COMP_OMIT_SEQ_HEADER) {
-			go->dvd_mode = 0;
-			go->seq_header_enable = 0;
-		} else {
-			go->seq_header_enable = 1;
-		}
-		/* fall-through */
-	}
-	case GO7007IOC_G_COMP_PARAMS:
-	{
-		struct go7007_comp_params *comp = arg;
-
-		if (go->format == GO7007_FORMAT_MJPEG)
-			return -EINVAL;
-		memset(comp, 0, sizeof(*comp));
-		comp->gop_size = go->gop_size;
-		comp->max_b_frames = go->ipb ? 2 : 0;
-		switch (go->aspect_ratio) {
-		case GO7007_RATIO_4_3:
-			if (go->standard == GO7007_STD_NTSC)
-				comp->aspect_ratio =
-					GO7007_ASPECT_RATIO_4_3_NTSC;
-			else
-				comp->aspect_ratio =
-					GO7007_ASPECT_RATIO_4_3_PAL;
-			break;
-		case GO7007_RATIO_16_9:
-			if (go->standard == GO7007_STD_NTSC)
-				comp->aspect_ratio =
-					GO7007_ASPECT_RATIO_16_9_NTSC;
-			else
-				comp->aspect_ratio =
-					GO7007_ASPECT_RATIO_16_9_PAL;
-			break;
-		default:
-			comp->aspect_ratio = GO7007_ASPECT_RATIO_1_1;
-			break;
-		}
-		if (go->closed_gop)
-			comp->flags |= GO7007_COMP_CLOSED_GOP;
-		if (!go->seq_header_enable)
-			comp->flags |= GO7007_COMP_OMIT_SEQ_HEADER;
-		return 0;
-	}
-	case GO7007IOC_S_MPEG_PARAMS:
-	{
-		struct go7007_mpeg_params *mpeg = arg;
-
-		if (go->format != GO7007_FORMAT_MPEG1 &&
-				go->format != GO7007_FORMAT_MPEG2 &&
-				go->format != GO7007_FORMAT_MPEG4)
-			return -EINVAL;
-
-		if (mpeg->flags & GO7007_MPEG_FORCE_DVD_MODE) {
-			go->format = GO7007_FORMAT_MPEG2;
-			go->bitrate = 9800000;
-			go->gop_size = 15;
-			go->pali = 0x48;
-			go->closed_gop = 1;
-			go->repeat_seqhead = 0;
-			go->seq_header_enable = 1;
-			go->gop_header_enable = 1;
-			go->dvd_mode = 1;
-		} else {
-			switch (mpeg->mpeg_video_standard) {
-			case GO7007_MPEG_VIDEO_MPEG1:
-				go->format = GO7007_FORMAT_MPEG1;
-				go->pali = 0;
-				break;
-			case GO7007_MPEG_VIDEO_MPEG2:
-				go->format = GO7007_FORMAT_MPEG2;
-				if (mpeg->pali >> 24 == 2)
-					go->pali = mpeg->pali & 0xff;
-				else
-					go->pali = 0x48;
-				break;
-			case GO7007_MPEG_VIDEO_MPEG4:
-				go->format = GO7007_FORMAT_MPEG4;
-				if (mpeg->pali >> 24 == 4)
-					go->pali = mpeg->pali & 0xff;
-				else
-					go->pali = 0xf5;
-				break;
-			default:
-				return -EINVAL;
-			}
-			go->gop_header_enable =
-				mpeg->flags & GO7007_MPEG_OMIT_GOP_HEADER
-				? 0 : 1;
-			if (mpeg->flags & GO7007_MPEG_REPEAT_SEQHEADER)
-				go->repeat_seqhead = 1;
-			else
-				go->repeat_seqhead = 0;
-			go->dvd_mode = 0;
-		}
-		/* fall-through */
-	}
-	case GO7007IOC_G_MPEG_PARAMS:
-	{
-		struct go7007_mpeg_params *mpeg = arg;
-
-		memset(mpeg, 0, sizeof(*mpeg));
-		switch (go->format) {
-		case GO7007_FORMAT_MPEG1:
-			mpeg->mpeg_video_standard = GO7007_MPEG_VIDEO_MPEG1;
-			mpeg->pali = 0;
-			break;
-		case GO7007_FORMAT_MPEG2:
-			mpeg->mpeg_video_standard = GO7007_MPEG_VIDEO_MPEG2;
-			mpeg->pali = GO7007_MPEG_PROFILE(2, go->pali);
-			break;
-		case GO7007_FORMAT_MPEG4:
-			mpeg->mpeg_video_standard = GO7007_MPEG_VIDEO_MPEG4;
-			mpeg->pali = GO7007_MPEG_PROFILE(4, go->pali);
-			break;
-		default:
-			return -EINVAL;
-		}
-		if (!go->gop_header_enable)
-			mpeg->flags |= GO7007_MPEG_OMIT_GOP_HEADER;
-		if (go->repeat_seqhead)
-			mpeg->flags |= GO7007_MPEG_REPEAT_SEQHEADER;
-		if (go->dvd_mode)
-			mpeg->flags |= GO7007_MPEG_FORCE_DVD_MODE;
-		return 0;
-	}
 	case GO7007IOC_S_MD_PARAMS:
 	{
 		struct go7007_md_params *mdp = arg;
@@ -1604,25 +875,6 @@ static int vidioc_s_jpegcomp(struct file *file, void *priv,
 			go->modet[mdp->region].enable = 0;
 		/* fall-through */
 	}
-	case GO7007IOC_G_MD_PARAMS:
-	{
-		struct go7007_md_params *mdp = arg;
-		int region = mdp->region;
-
-		if (mdp->region > 3)
-			return -EINVAL;
-		memset(mdp, 0, sizeof(struct go7007_md_params));
-		mdp->region = region;
-		if (!go->modet[region].enable)
-			return 0;
-		mdp->pixel_threshold =
-			(go->modet[region].pixel_threshold << 1) + 1;
-		mdp->motion_threshold =
-			(go->modet[region].motion_threshold << 1) + 1;
-		mdp->trigger =
-			(go->modet[region].mb_threshold << 1) + 1;
-		return 0;
-	}
 	case GO7007IOC_S_MD_REGION:
 	{
 		struct go7007_md_region *region = arg;
@@ -1633,116 +885,14 @@ static int vidioc_s_jpegcomp(struct file *file, void *priv,
 	}
 #endif
 
-static ssize_t go7007_read(struct file *file, char __user *data,
-		size_t count, loff_t *ppos)
-{
-	return -EINVAL;
-}
-
-static void go7007_vm_open(struct vm_area_struct *vma)
-{
-	struct go7007_buffer *gobuf = vma->vm_private_data;
-
-	++gobuf->mapped;
-}
-
-static void go7007_vm_close(struct vm_area_struct *vma)
-{
-	struct go7007_buffer *gobuf = vma->vm_private_data;
-	unsigned long flags;
-
-	if (--gobuf->mapped == 0) {
-		spin_lock_irqsave(&gobuf->go->spinlock, flags);
-		deactivate_buffer(gobuf);
-		spin_unlock_irqrestore(&gobuf->go->spinlock, flags);
-	}
-}
-
-/* Copied from videobuf-dma-sg.c */
-static int go7007_vm_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
-{
-	struct page *page;
-
-	page = alloc_page(GFP_USER | __GFP_DMA32);
-	if (!page)
-		return VM_FAULT_OOM;
-	clear_user_highpage(page, (unsigned long)vmf->virtual_address);
-	vmf->page = page;
-	return 0;
-}
-
-static struct vm_operations_struct go7007_vm_ops = {
-	.open	= go7007_vm_open,
-	.close	= go7007_vm_close,
-	.fault	= go7007_vm_fault,
-};
-
-static int go7007_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	struct go7007_file *gofh = file->private_data;
-	unsigned int index;
-
-	if (gofh->go->status != STATUS_ONLINE)
-		return -EIO;
-	if (!(vma->vm_flags & VM_SHARED))
-		return -EINVAL; /* only support VM_SHARED mapping */
-	if (vma->vm_end - vma->vm_start != GO7007_BUF_SIZE)
-		return -EINVAL; /* must map exactly one full buffer */
-	mutex_lock(&gofh->lock);
-	index = vma->vm_pgoff / GO7007_BUF_PAGES;
-	if (index >= gofh->buf_count) {
-		mutex_unlock(&gofh->lock);
-		return -EINVAL; /* trying to map beyond requested buffers */
-	}
-	if (index * GO7007_BUF_PAGES != vma->vm_pgoff) {
-		mutex_unlock(&gofh->lock);
-		return -EINVAL; /* offset is not aligned on buffer boundary */
-	}
-	if (gofh->bufs[index].mapped > 0) {
-		mutex_unlock(&gofh->lock);
-		return -EBUSY;
-	}
-	gofh->bufs[index].mapped = 1;
-	gofh->bufs[index].user_addr = vma->vm_start;
-	vma->vm_ops = &go7007_vm_ops;
-	vma->vm_flags |= VM_DONTEXPAND;
-	vma->vm_flags &= ~VM_IO;
-	vma->vm_private_data = &gofh->bufs[index];
-	mutex_unlock(&gofh->lock);
-	return 0;
-}
-
-static unsigned int go7007_poll(struct file *file, poll_table *wait)
-{
-	struct go7007_file *gofh = file->private_data;
-	struct go7007_buffer *gobuf;
-
-	if (list_empty(&gofh->go->stream))
-		return POLLERR;
-	gobuf = list_entry(gofh->go->stream.next, struct go7007_buffer, stream);
-	poll_wait(file, &gofh->go->frame_waitq, wait);
-	if (gobuf->state == BUF_STATE_DONE)
-		return POLLIN | POLLRDNORM;
-	return 0;
-}
-
-static void go7007_vfl_release(struct video_device *vfd)
-{
-	struct go7007 *go = video_get_drvdata(vfd);
-
-	video_device_release(vfd);
-	if (--go->ref_count == 0)
-		kfree(go);
-}
-
 static struct v4l2_file_operations go7007_fops = {
 	.owner		= THIS_MODULE,
-	.open		= go7007_open,
-	.release	= go7007_release,
-	.ioctl		= video_ioctl2,
-	.read		= go7007_read,
-	.mmap		= go7007_mmap,
-	.poll		= go7007_poll,
+	.open		= v4l2_fh_open,
+	.release	= vb2_fop_release,
+	.unlocked_ioctl	= video_ioctl2,
+	.read		= vb2_fop_read,
+	.mmap		= vb2_fop_mmap,
+	.poll		= vb2_fop_poll,
 };
 
 static const struct v4l2_ioctl_ops video_ioctl_ops = {
@@ -1751,21 +901,21 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_g_fmt_vid_cap     = vidioc_g_fmt_vid_cap,
 	.vidioc_try_fmt_vid_cap   = vidioc_try_fmt_vid_cap,
 	.vidioc_s_fmt_vid_cap     = vidioc_s_fmt_vid_cap,
-	.vidioc_reqbufs           = vidioc_reqbufs,
-	.vidioc_querybuf          = vidioc_querybuf,
-	.vidioc_qbuf              = vidioc_qbuf,
-	.vidioc_dqbuf             = vidioc_dqbuf,
+	.vidioc_reqbufs           = vb2_ioctl_reqbufs,
+	.vidioc_querybuf          = vb2_ioctl_querybuf,
+	.vidioc_qbuf              = vb2_ioctl_qbuf,
+	.vidioc_dqbuf             = vb2_ioctl_dqbuf,
 	.vidioc_g_std             = vidioc_g_std,
 	.vidioc_s_std             = vidioc_s_std,
 	.vidioc_querystd          = vidioc_querystd,
 	.vidioc_enum_input        = vidioc_enum_input,
 	.vidioc_g_input           = vidioc_g_input,
 	.vidioc_s_input           = vidioc_s_input,
-	.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_enumaudio         = vidioc_enumaudio,
+	.vidioc_g_audio           = vidioc_g_audio,
+	.vidioc_s_audio           = vidioc_s_audio,
+	.vidioc_streamon          = vb2_ioctl_streamon,
+	.vidioc_streamoff         = vb2_ioctl_streamoff,
 	.vidioc_g_tuner           = vidioc_g_tuner,
 	.vidioc_s_tuner           = vidioc_s_tuner,
 	.vidioc_g_frequency       = vidioc_g_frequency,
@@ -1774,66 +924,129 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_s_parm            = vidioc_s_parm,
 	.vidioc_enum_framesizes   = vidioc_enum_framesizes,
 	.vidioc_enum_frameintervals = vidioc_enum_frameintervals,
-	.vidioc_cropcap           = vidioc_cropcap,
-	.vidioc_g_crop            = vidioc_g_crop,
-	.vidioc_s_crop            = vidioc_s_crop,
-	.vidioc_g_jpegcomp        = vidioc_g_jpegcomp,
-	.vidioc_s_jpegcomp        = vidioc_s_jpegcomp,
+	.vidioc_log_status        = vidioc_log_status,
+	.vidioc_subscribe_event   = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 static struct video_device go7007_template = {
 	.name		= "go7007",
 	.fops		= &go7007_fops,
-	.release	= go7007_vfl_release,
+	.release	= video_device_release_empty,
 	.ioctl_ops	= &video_ioctl_ops,
 	.tvnorms	= V4L2_STD_ALL,
-	.current_norm	= V4L2_STD_NTSC,
 };
 
+int go7007_v4l2_ctrl_init(struct go7007 *go)
+{
+	struct v4l2_ctrl_handler *hdl = &go->hdl;
+	struct v4l2_ctrl *ctrl;
+
+	v4l2_ctrl_handler_init(hdl, 13);
+	go->mpeg_video_gop_size = v4l2_ctrl_new_std(hdl, NULL,
+			V4L2_CID_MPEG_VIDEO_GOP_SIZE, 0, 34, 1, 15);
+	go->mpeg_video_gop_closure = v4l2_ctrl_new_std(hdl, NULL,
+			V4L2_CID_MPEG_VIDEO_GOP_CLOSURE, 0, 1, 1, 1);
+	go->mpeg_video_bitrate = v4l2_ctrl_new_std(hdl, NULL,
+			V4L2_CID_MPEG_VIDEO_BITRATE,
+			64000, 10000000, 1, 9800000);
+	go->mpeg_video_b_frames = v4l2_ctrl_new_std(hdl, NULL,
+			V4L2_CID_MPEG_VIDEO_B_FRAMES, 0, 2, 2, 0);
+	go->mpeg_video_rep_seqheader = v4l2_ctrl_new_std(hdl, NULL,
+			V4L2_CID_MPEG_VIDEO_REPEAT_SEQ_HEADER, 0, 1, 1, 1);
+
+	go->mpeg_video_aspect_ratio = v4l2_ctrl_new_std_menu(hdl, NULL,
+			V4L2_CID_MPEG_VIDEO_ASPECT,
+			V4L2_MPEG_VIDEO_ASPECT_16x9, 0,
+			V4L2_MPEG_VIDEO_ASPECT_1x1);
+	ctrl = v4l2_ctrl_new_std(hdl, NULL,
+			V4L2_CID_JPEG_ACTIVE_MARKER, 0,
+			V4L2_JPEG_ACTIVE_MARKER_DQT | V4L2_JPEG_ACTIVE_MARKER_DHT, 0,
+			V4L2_JPEG_ACTIVE_MARKER_DQT | V4L2_JPEG_ACTIVE_MARKER_DHT);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	if (hdl->error) {
+		int rv = hdl->error;
+
+		v4l2_err(&go->v4l2_dev, "Could not register controls\n");
+		return rv;
+	}
+	go->v4l2_dev.ctrl_handler = hdl;
+	return 0;
+}
+
 int go7007_v4l2_init(struct go7007 *go)
 {
+	struct video_device *vdev = &go->vdev;
 	int rv;
 
-	go->video_dev = video_device_alloc();
-	if (go->video_dev == NULL)
-		return -ENOMEM;
-	*go->video_dev = go7007_template;
-	go->video_dev->parent = go->dev;
-	rv = video_register_device(go->video_dev, VFL_TYPE_GRABBER, -1);
-	if (rv < 0) {
-		video_device_release(go->video_dev);
-		go->video_dev = NULL;
+	mutex_init(&go->serialize_lock);
+	mutex_init(&go->queue_lock);
+
+	INIT_LIST_HEAD(&go->vidq_active);
+	go->vidq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	go->vidq.io_modes = VB2_MMAP | VB2_USERPTR | VB2_READ;
+	go->vidq.ops = &go7007_video_qops;
+	go->vidq.mem_ops = &vb2_vmalloc_memops;
+	go->vidq.drv_priv = go;
+	go->vidq.buf_struct_size = sizeof(struct go7007_buffer);
+	go->vidq.timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	go->vidq.lock = &go->queue_lock;
+	rv = vb2_queue_init(&go->vidq);
+	if (rv)
 		return rv;
+	*vdev = go7007_template;
+	vdev->lock = &go->serialize_lock;
+	vdev->queue = &go->vidq;
+	set_bit(V4L2_FL_USE_FH_PRIO, &vdev->flags);
+	video_set_drvdata(vdev, go);
+	vdev->v4l2_dev = &go->v4l2_dev;
+	if (!v4l2_device_has_op(&go->v4l2_dev, video, querystd))
+		v4l2_disable_ioctl(vdev, VIDIOC_QUERYSTD);
+	if (!(go->board_info->flags & GO7007_BOARD_HAS_TUNER)) {
+		v4l2_disable_ioctl(vdev, VIDIOC_S_FREQUENCY);
+		v4l2_disable_ioctl(vdev, VIDIOC_G_FREQUENCY);
+		v4l2_disable_ioctl(vdev, VIDIOC_S_TUNER);
+		v4l2_disable_ioctl(vdev, VIDIOC_G_TUNER);
+	} else {
+		struct v4l2_frequency f = {
+			.type = V4L2_TUNER_ANALOG_TV,
+			.frequency = 980,
+		};
+
+		call_all(&go->v4l2_dev, tuner, s_frequency, &f);
 	}
-	rv = v4l2_device_register(go->dev, &go->v4l2_dev);
-	if (rv < 0) {
-		video_device_release(go->video_dev);
-		go->video_dev = NULL;
-		return rv;
+	if (!(go->board_info->sensor_flags & GO7007_SENSOR_TV)) {
+		v4l2_disable_ioctl(vdev, VIDIOC_G_STD);
+		v4l2_disable_ioctl(vdev, VIDIOC_S_STD);
+		vdev->tvnorms = 0;
+	}
+	if (go->board_info->sensor_flags & GO7007_SENSOR_SCALING)
+		v4l2_disable_ioctl(vdev, VIDIOC_ENUM_FRAMESIZES);
+	if (go->board_info->num_aud_inputs == 0) {
+		v4l2_disable_ioctl(vdev, VIDIOC_G_AUDIO);
+		v4l2_disable_ioctl(vdev, VIDIOC_S_AUDIO);
+		v4l2_disable_ioctl(vdev, VIDIOC_ENUMAUDIO);
 	}
-	video_set_drvdata(go->video_dev, go);
-	++go->ref_count;
+	/* Setup correct crystal frequency on this board */
+	if (go->board_info->sensor_flags & GO7007_SENSOR_SAA7115)
+		v4l2_subdev_call(go->sd_video, video, s_crystal_freq,
+				SAA7115_FREQ_24_576_MHZ,
+				SAA7115_FREQ_FL_APLL | SAA7115_FREQ_FL_UCGC |
+				SAA7115_FREQ_FL_DOUBLE_ASCLK);
+	go7007_s_input(go);
+	if (go->board_info->sensor_flags & GO7007_SENSOR_TV)
+		go7007_s_std(go);
+	rv = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
+	if (rv < 0)
+		return rv;
 	dev_info(go->dev, "registered device %s [v4l2]\n",
-		 video_device_node_name(go->video_dev));
+		 video_device_node_name(vdev));
 
 	return 0;
 }
 
 void go7007_v4l2_remove(struct go7007 *go)
 {
-	unsigned long flags;
-
-	mutex_lock(&go->hw_lock);
-	if (go->streaming) {
-		go->streaming = 0;
-		go7007_stream_stop(go);
-		spin_lock_irqsave(&go->spinlock, flags);
-		abort_queued(go);
-		spin_unlock_irqrestore(&go->spinlock, flags);
-	}
-	mutex_unlock(&go->hw_lock);
-	if (go->video_dev)
-		video_unregister_device(go->video_dev);
-	if (go->status != STATUS_SHUTDOWN)
-		v4l2_device_unregister(&go->v4l2_dev);
+	v4l2_ctrl_handler_free(&go->hdl);
 }
diff --git a/drivers/staging/media/go7007/go7007.h b/drivers/staging/media/go7007/go7007.h
index 7399c915a93..54b98973898 100644
--- a/drivers/staging/media/go7007/go7007.h
+++ b/drivers/staging/media/go7007/go7007.h
@@ -17,72 +17,6 @@
  * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  */
 
-/* DEPRECATED -- use V4L2_PIX_FMT_MPEG and then call GO7007IOC_S_MPEG_PARAMS
- * to select between MPEG1, MPEG2, and MPEG4 */
-#define V4L2_PIX_FMT_MPEG4     v4l2_fourcc('M', 'P', 'G', '4') /* MPEG4 */
-
-/* These will be replaced with a better interface
- * soon, so don't get too attached to them */
-#define	GO7007IOC_S_BITRATE	_IOW('V', BASE_VIDIOC_PRIVATE + 0, int)
-#define	GO7007IOC_G_BITRATE	_IOR('V', BASE_VIDIOC_PRIVATE + 1, int)
-
-enum go7007_aspect_ratio {
-	GO7007_ASPECT_RATIO_1_1 = 0,
-	GO7007_ASPECT_RATIO_4_3_NTSC = 1,
-	GO7007_ASPECT_RATIO_4_3_PAL = 2,
-	GO7007_ASPECT_RATIO_16_9_NTSC = 3,
-	GO7007_ASPECT_RATIO_16_9_PAL = 4,
-};
-
-/* Used to set generic compression parameters */
-struct go7007_comp_params {
-	__u32 gop_size;
-	__u32 max_b_frames;
-	enum go7007_aspect_ratio aspect_ratio;
-	__u32 flags;
-	__u32 reserved[8];
-};
-
-#define GO7007_COMP_CLOSED_GOP		0x00000001
-#define GO7007_COMP_OMIT_SEQ_HEADER	0x00000002
-
-enum go7007_mpeg_video_standard {
-	GO7007_MPEG_VIDEO_MPEG1 = 0,
-	GO7007_MPEG_VIDEO_MPEG2 = 1,
-	GO7007_MPEG_VIDEO_MPEG4 = 2,
-};
-
-/* Used to set parameters for V4L2_PIX_FMT_MPEG format */
-struct go7007_mpeg_params {
-	enum go7007_mpeg_video_standard mpeg_video_standard;
-	__u32 flags;
-	__u32 pali;
-	__u32 reserved[8];
-};
-
-#define GO7007_MPEG_FORCE_DVD_MODE	0x00000001
-#define GO7007_MPEG_OMIT_GOP_HEADER	0x00000002
-#define GO7007_MPEG_REPEAT_SEQHEADER	0x00000004
-
-#define GO7007_MPEG_PROFILE(format, pali)	(((format)<<24)|(pali))
-
-#define GO7007_MPEG2_PROFILE_MAIN_MAIN		GO7007_MPEG_PROFILE(2, 0x48)
-
-#define GO7007_MPEG4_PROFILE_S_L0		GO7007_MPEG_PROFILE(4, 0x08)
-#define GO7007_MPEG4_PROFILE_S_L1		GO7007_MPEG_PROFILE(4, 0x01)
-#define GO7007_MPEG4_PROFILE_S_L2		GO7007_MPEG_PROFILE(4, 0x02)
-#define GO7007_MPEG4_PROFILE_S_L3		GO7007_MPEG_PROFILE(4, 0x03)
-#define GO7007_MPEG4_PROFILE_ARTS_L1		GO7007_MPEG_PROFILE(4, 0x91)
-#define GO7007_MPEG4_PROFILE_ARTS_L2		GO7007_MPEG_PROFILE(4, 0x92)
-#define GO7007_MPEG4_PROFILE_ARTS_L3		GO7007_MPEG_PROFILE(4, 0x93)
-#define GO7007_MPEG4_PROFILE_ARTS_L4		GO7007_MPEG_PROFILE(4, 0x94)
-#define GO7007_MPEG4_PROFILE_AS_L0		GO7007_MPEG_PROFILE(4, 0xf0)
-#define GO7007_MPEG4_PROFILE_AS_L1		GO7007_MPEG_PROFILE(4, 0xf1)
-#define GO7007_MPEG4_PROFILE_AS_L2		GO7007_MPEG_PROFILE(4, 0xf2)
-#define GO7007_MPEG4_PROFILE_AS_L3		GO7007_MPEG_PROFILE(4, 0xf3)
-#define GO7007_MPEG4_PROFILE_AS_L4		GO7007_MPEG_PROFILE(4, 0xf4)
-#define GO7007_MPEG4_PROFILE_AS_L5		GO7007_MPEG_PROFILE(4, 0xf5)
-
 struct go7007_md_params {
 	__u16 region;
 	__u16 trigger;
@@ -98,14 +32,6 @@ struct go7007_md_region {
 	__u32 reserved[8];
 };
 
-#define	GO7007IOC_S_MPEG_PARAMS	_IOWR('V', BASE_VIDIOC_PRIVATE + 2, \
-					struct go7007_mpeg_params)
-#define	GO7007IOC_G_MPEG_PARAMS	_IOR('V', BASE_VIDIOC_PRIVATE + 3, \
-					struct go7007_mpeg_params)
-#define	GO7007IOC_S_COMP_PARAMS	_IOWR('V', BASE_VIDIOC_PRIVATE + 4, \
-					struct go7007_comp_params)
-#define	GO7007IOC_G_COMP_PARAMS	_IOR('V', BASE_VIDIOC_PRIVATE + 5, \
-					struct go7007_comp_params)
 #define	GO7007IOC_S_MD_PARAMS	_IOWR('V', BASE_VIDIOC_PRIVATE + 6, \
 					struct go7007_md_params)
 #define	GO7007IOC_G_MD_PARAMS	_IOR('V', BASE_VIDIOC_PRIVATE + 7, \
diff --git a/drivers/staging/media/go7007/s2250-board.c b/drivers/staging/media/go7007/s2250-board.c
index 37400bfa6cc..beaa98b9c85 100644
--- a/drivers/staging/media/go7007/s2250-board.c
+++ b/drivers/staging/media/go7007/s2250-board.c
@@ -29,6 +29,13 @@
 MODULE_DESCRIPTION("Sensoray 2250/2251 i2c v4l2 subdev driver");
 MODULE_LICENSE("GPL v2");
 
+/*
+ * Note: this board has two i2c devices: a vpx3226f and a tlv320aic23b.
+ * Due to the unusual way these are accessed on this device we do not
+ * reuse the i2c drivers, but instead they are implemented in this
+ * driver. It would be nice to improve on this, though.
+ */
+
 #define TLV320_ADDRESS      0x34
 #define VPX322_ADDR_ANALOGCONTROL1	0x02
 #define VPX322_ADDR_BRIGHTNESS0		0x0127
@@ -116,6 +123,7 @@ static u16 vid_regs_fp_pal[] = {
 
 struct s2250 {
 	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
 	v4l2_std_id std;
 	int input;
 	int brightness;
@@ -353,127 +361,48 @@ static int s2250_s_std(struct v4l2_subdev *sd, v4l2_std_id norm)
 	u16 vidsource;
 
 	vidsource = (state->input == 1) ? 0x040 : 0x020;
-	switch (norm) {
-	case V4L2_STD_NTSC:
-		write_regs_fp(client, vid_regs_fp);
-		write_reg_fp(client, 0x20, vidsource | 1);
-		break;
-	case V4L2_STD_PAL:
+	if (norm & V4L2_STD_625_50) {
 		write_regs_fp(client, vid_regs_fp);
 		write_regs_fp(client, vid_regs_fp_pal);
 		write_reg_fp(client, 0x20, vidsource);
-		break;
-	default:
-		return -EINVAL;
+	} else {
+		write_regs_fp(client, vid_regs_fp);
+		write_reg_fp(client, 0x20, vidsource | 1);
 	}
 	state->std = norm;
 	return 0;
 }
 
-static int s2250_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *query)
+static int s2250_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	switch (query->id) {
-	case V4L2_CID_BRIGHTNESS:
-		return v4l2_ctrl_query_fill(query, 0, 100, 1, 50);
-	case V4L2_CID_CONTRAST:
-		return v4l2_ctrl_query_fill(query, 0, 100, 1, 50);
-	case V4L2_CID_SATURATION:
-		return v4l2_ctrl_query_fill(query, 0, 100, 1, 50);
-	case V4L2_CID_HUE:
-		return v4l2_ctrl_query_fill(query, -50, 50, 1, 0);
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static int s2250_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-	struct s2250 *state = to_state(sd);
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	int value1;
+	struct s2250 *state = container_of(ctrl->handler, struct s2250, hdl);
+	struct i2c_client *client = v4l2_get_subdevdata(&state->sd);
 	u16 oldvalue;
 
 	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		if (ctrl->value > 100)
-			state->brightness = 100;
-		else if (ctrl->value < 0)
-			state->brightness = 0;
-		else
-			state->brightness = ctrl->value;
-		value1 = (state->brightness - 50) * 255 / 100;
 		read_reg_fp(client, VPX322_ADDR_BRIGHTNESS0, &oldvalue);
 		write_reg_fp(client, VPX322_ADDR_BRIGHTNESS0,
-			     value1 | (oldvalue & ~0xff));
+			     ctrl->val | (oldvalue & ~0xff));
 		read_reg_fp(client, VPX322_ADDR_BRIGHTNESS1, &oldvalue);
 		write_reg_fp(client, VPX322_ADDR_BRIGHTNESS1,
-			     value1 | (oldvalue & ~0xff));
+			     ctrl->val | (oldvalue & ~0xff));
 		write_reg_fp(client, 0x140, 0x60);
 		break;
 	case V4L2_CID_CONTRAST:
-		if (ctrl->value > 100)
-			state->contrast = 100;
-		else if (ctrl->value < 0)
-			state->contrast = 0;
-		else
-			state->contrast = ctrl->value;
-		value1 = state->contrast * 0x40 / 100;
-		if (value1 > 0x3f)
-			value1 = 0x3f; /* max */
 		read_reg_fp(client, VPX322_ADDR_CONTRAST0, &oldvalue);
 		write_reg_fp(client, VPX322_ADDR_CONTRAST0,
-			     value1 | (oldvalue & ~0x3f));
+			     ctrl->val | (oldvalue & ~0x3f));
 		read_reg_fp(client, VPX322_ADDR_CONTRAST1, &oldvalue);
 		write_reg_fp(client, VPX322_ADDR_CONTRAST1,
-			     value1 | (oldvalue & ~0x3f));
+			     ctrl->val | (oldvalue & ~0x3f));
 		write_reg_fp(client, 0x140, 0x60);
 		break;
 	case V4L2_CID_SATURATION:
-		if (ctrl->value > 100)
-			state->saturation = 100;
-		else if (ctrl->value < 0)
-			state->saturation = 0;
-		else
-			state->saturation = ctrl->value;
-		value1 = state->saturation * 4140 / 100;
-		if (value1 > 4094)
-			value1 = 4094;
-		write_reg_fp(client, VPX322_ADDR_SAT, value1);
-		break;
-	case V4L2_CID_HUE:
-		if (ctrl->value > 50)
-			state->hue = 50;
-		else if (ctrl->value < -50)
-			state->hue = -50;
-		else
-			state->hue = ctrl->value;
-		/* clamp the hue range */
-		value1 = state->hue * 280 / 50;
-		write_reg_fp(client, VPX322_ADDR_HUE, value1);
-		break;
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static int s2250_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-	struct s2250 *state = to_state(sd);
-
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		ctrl->value = state->brightness;
-		break;
-	case V4L2_CID_CONTRAST:
-		ctrl->value = state->contrast;
-		break;
-	case V4L2_CID_SATURATION:
-		ctrl->value = state->saturation;
+		write_reg_fp(client, VPX322_ADDR_SAT, ctrl->val);
 		break;
 	case V4L2_CID_HUE:
-		ctrl->value = state->hue;
+		write_reg_fp(client, VPX322_ADDR_HUE, ctrl->val);
 		break;
 	default:
 		return -EINVAL;
@@ -531,24 +460,21 @@ static int s2250_log_status(struct v4l2_subdev *sd)
 	v4l2_info(sd, "Input: %s\n", state->input == 0 ? "Composite" :
 					state->input == 1 ? "S-video" :
 					"error");
-	v4l2_info(sd, "Brightness: %d\n", state->brightness);
-	v4l2_info(sd, "Contrast: %d\n", state->contrast);
-	v4l2_info(sd, "Saturation: %d\n", state->saturation);
-	v4l2_info(sd, "Hue: %d\n", state->hue);
 	v4l2_info(sd, "Audio input: %s\n", state->audio_input == 0 ? "Line In" :
 					state->audio_input == 1 ? "Mic" :
 					state->audio_input == 2 ? "Mic Boost" :
 					"error");
-	return 0;
+	return v4l2_ctrl_subdev_log_status(sd);
 }
 
 /* --------------------------------------------------------------------------*/
 
+static const struct v4l2_ctrl_ops s2250_ctrl_ops = {
+	.s_ctrl = s2250_s_ctrl,
+};
+
 static const struct v4l2_subdev_core_ops s2250_core_ops = {
 	.log_status = s2250_log_status,
-	.g_ctrl = s2250_g_ctrl,
-	.s_ctrl = s2250_s_ctrl,
-	.queryctrl = s2250_queryctrl,
 	.s_std = s2250_s_std,
 };
 
@@ -584,7 +510,7 @@ static int s2250_probe(struct i2c_client *client,
 	if (audio == NULL)
 		return -ENOMEM;
 
-	state = kmalloc(sizeof(struct s2250), GFP_KERNEL);
+	state = kzalloc(sizeof(struct s2250), GFP_KERNEL);
 	if (state == NULL) {
 		i2c_unregister_device(audio);
 		return -ENOMEM;
@@ -596,6 +522,24 @@ static int s2250_probe(struct i2c_client *client,
 	v4l2_info(sd, "initializing %s at address 0x%x on %s\n",
 	       "Sensoray 2250/2251", client->addr, client->adapter->name);
 
+	v4l2_ctrl_handler_init(&state->hdl, 4);
+	v4l2_ctrl_new_std(&state->hdl, &s2250_ctrl_ops,
+		V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
+	v4l2_ctrl_new_std(&state->hdl, &s2250_ctrl_ops,
+		V4L2_CID_CONTRAST, 0, 0x3f, 1, 0x32);
+	v4l2_ctrl_new_std(&state->hdl, &s2250_ctrl_ops,
+		V4L2_CID_SATURATION, 0, 4094, 1, 2070);
+	v4l2_ctrl_new_std(&state->hdl, &s2250_ctrl_ops,
+		V4L2_CID_HUE, -512, 511, 1, 0);
+	sd->ctrl_handler = &state->hdl;
+	if (state->hdl.error) {
+		int err = state->hdl.error;
+
+		v4l2_ctrl_handler_free(&state->hdl);
+		kfree(state);
+		return err;
+	}
+
 	state->std = V4L2_STD_NTSC;
 	state->brightness = 50;
 	state->contrast = 50;
@@ -606,22 +550,16 @@ static int s2250_probe(struct i2c_client *client,
 	/* initialize the audio */
 	if (write_regs(audio, aud_regs) < 0) {
 		dev_err(&client->dev, "error initializing audio\n");
-		i2c_unregister_device(audio);
-		kfree(state);
-		return 0;
+		goto fail;
 	}
 
 	if (write_regs(client, vid_regs) < 0) {
 		dev_err(&client->dev, "error initializing decoder\n");
-		i2c_unregister_device(audio);
-		kfree(state);
-		return 0;
+		goto fail;
 	}
 	if (write_regs_fp(client, vid_regs_fp) < 0) {
 		dev_err(&client->dev, "error initializing decoder\n");
-		i2c_unregister_device(audio);
-		kfree(state);
-		return 0;
+		goto fail;
 	}
 	/* set default channel */
 	/* composite */
@@ -657,14 +595,21 @@ static int s2250_probe(struct i2c_client *client,
 
 	v4l2_info(sd, "initialized successfully\n");
 	return 0;
+
+fail:
+	i2c_unregister_device(audio);
+	v4l2_ctrl_handler_free(&state->hdl);
+	kfree(state);
+	return -EIO;
 }
 
 static int s2250_remove(struct i2c_client *client)
 {
-	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct s2250 *state = to_state(i2c_get_clientdata(client));
 
-	v4l2_device_unregister_subdev(sd);
-	kfree(to_state(sd));
+	v4l2_device_unregister_subdev(&state->sd);
+	v4l2_ctrl_handler_free(&state->hdl);
+	kfree(state);
 	return 0;
 }
 
diff --git a/drivers/staging/media/go7007/s2250-loader.c b/drivers/staging/media/go7007/s2250-loader.c
deleted file mode 100644
index 72e5175fe7e..00000000000
--- a/drivers/staging/media/go7007/s2250-loader.c
+++ /dev/null
@@ -1,169 +0,0 @@
-/*
- * Copyright (C) 2008 Sensoray Company 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.
- */
-
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/slab.h>
-#include <linux/usb.h>
-#include <dvb-usb.h>
-
-#define S2250_LOADER_FIRMWARE	"s2250_loader.fw"
-#define S2250_FIRMWARE		"s2250.fw"
-
-typedef struct device_extension_s {
-    struct kref     kref;
-    int minor;
-    struct usb_device *usbdev;
-} device_extension_t, *pdevice_extension_t;
-
-#define USB_s2250loader_MAJOR 240
-#define USB_s2250loader_MINOR_BASE 0
-#define MAX_DEVICES 256
-
-static pdevice_extension_t s2250_dev_table[MAX_DEVICES];
-static DEFINE_MUTEX(s2250_dev_table_mutex);
-
-#define to_s2250loader_dev_common(d) container_of(d, device_extension_t, kref)
-static void s2250loader_delete(struct kref *kref)
-{
-	pdevice_extension_t s = to_s2250loader_dev_common(kref);
-	s2250_dev_table[s->minor] = NULL;
-	kfree(s);
-}
-
-static int s2250loader_probe(struct usb_interface *interface,
-				const struct usb_device_id *id)
-{
-	struct usb_device *usbdev;
-	int minor, ret;
-	pdevice_extension_t s = NULL;
-	const struct firmware *fw;
-
-	usbdev = usb_get_dev(interface_to_usbdev(interface));
-	if (!usbdev) {
-		dev_err(&interface->dev, "Enter s2250loader_probe failed\n");
-		return -1;
-	}
-	dev_info(&interface->dev, "Enter s2250loader_probe 2.6 kernel\n");
-	dev_info(&interface->dev, "vendor id 0x%x, device id 0x%x devnum:%d\n",
-		 usbdev->descriptor.idVendor, usbdev->descriptor.idProduct,
-		 usbdev->devnum);
-
-	if (usbdev->descriptor.bNumConfigurations != 1) {
-		dev_err(&interface->dev, "can't handle multiple config\n");
-		return -1;
-	}
-	mutex_lock(&s2250_dev_table_mutex);
-
-	for (minor = 0; minor < MAX_DEVICES; minor++) {
-		if (s2250_dev_table[minor] == NULL)
-			break;
-	}
-
-	if (minor < 0 || minor >= MAX_DEVICES) {
-		dev_err(&interface->dev, "Invalid minor: %d\n", minor);
-		goto failed;
-	}
-
-	/* Allocate dev data structure */
-	s = kmalloc(sizeof(device_extension_t), GFP_KERNEL);
-	if (s == NULL)
-		goto failed;
-
-	s2250_dev_table[minor] = s;
-
-	dev_info(&interface->dev,
-		 "s2250loader_probe: Device %d on Bus %d Minor %d\n",
-		 usbdev->devnum, usbdev->bus->busnum, minor);
-
-	memset(s, 0, sizeof(device_extension_t));
-	s->usbdev = usbdev;
-	dev_info(&interface->dev, "loading 2250 loader\n");
-
-	kref_init(&(s->kref));
-
-	mutex_unlock(&s2250_dev_table_mutex);
-
-	if (request_firmware(&fw, S2250_LOADER_FIRMWARE, &usbdev->dev)) {
-		dev_err(&interface->dev,
-			"s2250: unable to load firmware from file \"%s\"\n",
-			S2250_LOADER_FIRMWARE);
-		goto failed2;
-	}
-	ret = usb_cypress_load_firmware(usbdev, fw, CYPRESS_FX2);
-	release_firmware(fw);
-	if (0 != ret) {
-		dev_err(&interface->dev, "loader download failed\n");
-		goto failed2;
-	}
-
-	if (request_firmware(&fw, S2250_FIRMWARE, &usbdev->dev)) {
-		dev_err(&interface->dev,
-			"s2250: unable to load firmware from file \"%s\"\n",
-			S2250_FIRMWARE);
-		goto failed2;
-	}
-	ret = usb_cypress_load_firmware(usbdev, fw, CYPRESS_FX2);
-	release_firmware(fw);
-	if (0 != ret) {
-		dev_err(&interface->dev, "firmware_s2250 download failed\n");
-		goto failed2;
-	}
-
-	usb_set_intfdata(interface, s);
-	return 0;
-
-failed:
-	mutex_unlock(&s2250_dev_table_mutex);
-failed2:
-	if (s)
-		kref_put(&(s->kref), s2250loader_delete);
-
-	dev_err(&interface->dev, "probe failed\n");
-	return -1;
-}
-
-static void s2250loader_disconnect(struct usb_interface *interface)
-{
-	pdevice_extension_t s;
-	dev_info(&interface->dev, "s2250: disconnect\n");
-	s = usb_get_intfdata(interface);
-	usb_set_intfdata(interface, NULL);
-	kref_put(&(s->kref), s2250loader_delete);
-}
-
-static const struct usb_device_id s2250loader_ids[] = {
-	{USB_DEVICE(0x1943, 0xa250)},
-	{}                          /* Terminating entry */
-};
-
-MODULE_DEVICE_TABLE(usb, s2250loader_ids);
-
-static struct usb_driver s2250loader_driver = {
-	.name		= "s2250-loader",
-	.probe		= s2250loader_probe,
-	.disconnect	= s2250loader_disconnect,
-	.id_table	= s2250loader_ids,
-};
-
-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/go7007/s2250-loader.h b/drivers/staging/media/go7007/s2250-loader.h
deleted file mode 100644
index b7c301af16c..00000000000
--- a/drivers/staging/media/go7007/s2250-loader.h
+++ /dev/null
@@ -1,24 +0,0 @@
-/*
- * Copyright (C) 2005-2006 Micronas USA 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.
- */
-
-#ifndef _S2250_LOADER_H_
-#define _S2250_LOADER_H_
-
-extern int s2250loader_init(void);
-extern void s2250loader_cleanup(void);
-
-#endif
diff --git a/drivers/staging/media/go7007/saa7134-go7007.c b/drivers/staging/media/go7007/saa7134-go7007.c
index cf7c34a9945..d80b235d72e 100644
--- a/drivers/staging/media/go7007/saa7134-go7007.c
+++ b/drivers/staging/media/go7007/saa7134-go7007.c
@@ -28,12 +28,15 @@
 #include <linux/i2c.h>
 #include <asm/byteorder.h>
 #include <media/v4l2-common.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
 
-#include "saa7134-reg.h"
 #include "saa7134.h"
+#include "saa7134-reg.h"
+#include "go7007.h"
 #include "go7007-priv.h"
 
-#define GO7007_HPI_DEBUG
+/*#define GO7007_HPI_DEBUG*/
 
 enum hpi_address {
 	HPI_ADDR_VIDEO_BUFFER = 0xe4,
@@ -57,6 +60,7 @@ enum gpio_command {
 };
 
 struct saa7134_go7007 {
+	struct v4l2_subdev sd;
 	struct saa7134_dev *dev;
 	u8 *top;
 	u8 *bottom;
@@ -64,8 +68,12 @@ struct saa7134_go7007 {
 	dma_addr_t bottom_dma;
 };
 
-static struct go7007_board_info board_voyager = {
-	.firmware	 = "go7007tv.bin",
+static inline struct saa7134_go7007 *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct saa7134_go7007, sd);
+}
+
+static const struct go7007_board_info board_voyager = {
 	.flags		 = 0,
 	.sensor_flags	 = GO7007_SENSOR_656 |
 				GO7007_SENSOR_VALID_ENABLE |
@@ -84,7 +92,6 @@ static struct go7007_board_info board_voyager = {
 		},
 	},
 };
-MODULE_FIRMWARE("go7007tv.bin");
 
 /********************* Driver for GPIO HPI interface *********************/
 
@@ -236,7 +243,7 @@ static void saa7134_go7007_irq_ts_done(struct saa7134_dev *dev,
 	struct go7007 *go = video_get_drvdata(dev->empress_dev);
 	struct saa7134_go7007 *saa = go->hpi_context;
 
-	if (!go->streaming)
+	if (!vb2_is_streaming(&go->vidq))
 		return;
 	if (0 != (status & 0x000f0000))
 		printk(KERN_DEBUG "saa7134-go7007: irq: lost %ld\n",
@@ -261,12 +268,12 @@ static int saa7134_go7007_stream_start(struct go7007 *go)
 
 	saa->top_dma = dma_map_page(&dev->pci->dev, virt_to_page(saa->top),
 			0, PAGE_SIZE, DMA_FROM_DEVICE);
-	if (!saa->top_dma)
+	if (dma_mapping_error(&dev->pci->dev, saa->top_dma))
 		return -ENOMEM;
 	saa->bottom_dma = dma_map_page(&dev->pci->dev,
 			virt_to_page(saa->bottom),
 			0, PAGE_SIZE, DMA_FROM_DEVICE);
-	if (!saa->bottom_dma) {
+	if (dma_mapping_error(&dev->pci->dev, saa->bottom_dma)) {
 		dma_unmap_page(&dev->pci->dev, saa->top_dma, PAGE_SIZE,
 				DMA_FROM_DEVICE);
 		return -ENOMEM;
@@ -380,47 +387,6 @@ static int saa7134_go7007_send_firmware(struct go7007 *go, u8 *data, int len)
 	return 0;
 }
 
-static int saa7134_go7007_send_command(struct go7007 *go, unsigned int cmd,
-					void *arg)
-{
-	struct saa7134_go7007 *saa = go->hpi_context;
-	struct saa7134_dev *dev = saa->dev;
-
-	switch (cmd) {
-	case VIDIOC_S_STD:
-	{
-		v4l2_std_id *std = arg;
-		return saa7134_s_std_internal(dev, NULL, std);
-	}
-	case VIDIOC_G_STD:
-	{
-		v4l2_std_id *std = arg;
-		*std = dev->tvnorm->id;
-		return 0;
-	}
-	case VIDIOC_QUERYCTRL:
-	{
-		struct v4l2_queryctrl *ctrl = arg;
-		if (V4L2_CTRL_ID2CLASS(ctrl->id) == V4L2_CTRL_CLASS_USER)
-			return saa7134_queryctrl(NULL, NULL, ctrl);
-	}
-	case VIDIOC_G_CTRL:
-	{
-		struct v4l2_control *ctrl = arg;
-		if (V4L2_CTRL_ID2CLASS(ctrl->id) == V4L2_CTRL_CLASS_USER)
-			return saa7134_g_ctrl_internal(dev, NULL, ctrl);
-	}
-	case VIDIOC_S_CTRL:
-	{
-		struct v4l2_control *ctrl = arg;
-		if (V4L2_CTRL_ID2CLASS(ctrl->id) == V4L2_CTRL_CLASS_USER)
-			return saa7134_s_ctrl_internal(dev, NULL, ctrl);
-	}
-	}
-	return -EINVAL;
-
-}
-
 static struct go7007_hpi_ops saa7134_go7007_hpi_ops = {
 	.interface_reset	= saa7134_go7007_interface_reset,
 	.write_interrupt	= saa7134_go7007_write_interrupt,
@@ -428,21 +394,88 @@ static struct go7007_hpi_ops saa7134_go7007_hpi_ops = {
 	.stream_start		= saa7134_go7007_stream_start,
 	.stream_stop		= saa7134_go7007_stream_stop,
 	.send_firmware		= saa7134_go7007_send_firmware,
-	.send_command		= saa7134_go7007_send_command,
+};
+MODULE_FIRMWARE("go7007/go7007tv.bin");
+
+/* --------------------------------------------------------------------------*/
+
+static int saa7134_go7007_s_std(struct v4l2_subdev *sd, v4l2_std_id norm)
+{
+	struct saa7134_go7007 *saa = to_state(sd);
+	struct saa7134_dev *dev = saa->dev;
+
+	return saa7134_s_std_internal(dev, NULL, norm);
+}
+
+static int saa7134_go7007_queryctrl(struct v4l2_subdev *sd,
+				    struct v4l2_queryctrl *query)
+{
+	return saa7134_queryctrl(NULL, NULL, query);
+}
+static int saa7134_go7007_s_ctrl(struct v4l2_subdev *sd,
+				 struct v4l2_control *ctrl)
+{
+	struct saa7134_go7007 *saa = to_state(sd);
+	struct saa7134_dev *dev = saa->dev;
+	return saa7134_s_ctrl_internal(dev, NULL, ctrl);
+}
+
+static int saa7134_go7007_g_ctrl(struct v4l2_subdev *sd,
+				 struct v4l2_control *ctrl)
+{
+	struct saa7134_go7007 *saa = to_state(sd);
+	struct saa7134_dev *dev = saa->dev;
+	return saa7134_g_ctrl_internal(dev, NULL, ctrl);
+}
+
+/* --------------------------------------------------------------------------*/
+
+static const struct v4l2_subdev_core_ops saa7134_go7007_core_ops = {
+	.g_ctrl = saa7134_go7007_g_ctrl,
+	.s_ctrl = saa7134_go7007_s_ctrl,
+	.queryctrl = saa7134_go7007_queryctrl,
+	.s_std = saa7134_go7007_s_std,
+};
+
+static const struct v4l2_subdev_ops saa7134_go7007_sd_ops = {
+	.core = &saa7134_go7007_core_ops,
 };
 
+/* --------------------------------------------------------------------------*/
+
+
 /********************* Add/remove functions *********************/
 
 static int saa7134_go7007_init(struct saa7134_dev *dev)
 {
 	struct go7007 *go;
 	struct saa7134_go7007 *saa;
+	struct v4l2_subdev *sd;
 
 	printk(KERN_DEBUG "saa7134-go7007: probing new SAA713X board\n");
 
+	go = go7007_alloc(&board_voyager, &dev->pci->dev);
+	if (go == NULL)
+		return -ENOMEM;
+
 	saa = kzalloc(sizeof(struct saa7134_go7007), GFP_KERNEL);
-	if (saa == NULL)
+	if (saa == NULL) {
+		kfree(go);
 		return -ENOMEM;
+	}
+
+	go->board_id = GO7007_BOARDID_PCI_VOYAGER;
+	snprintf(go->bus_info, sizeof(go->bus_info), "PCI:%s", pci_name(dev->pci));
+	strlcpy(go->name, saa7134_boards[dev->board].name, sizeof(go->name));
+	go->hpi_ops = &saa7134_go7007_hpi_ops;
+	go->hpi_context = saa;
+	saa->dev = dev;
+
+	/* Init the subdevice interface */
+	sd = &saa->sd;
+	v4l2_subdev_init(sd, &saa7134_go7007_sd_ops);
+	v4l2_set_subdevdata(sd, saa);
+	strncpy(sd->name, "saa7134-go7007", sizeof(sd->name));
 
 	/* Allocate a couple pages for receiving the compressed stream */
 	saa->top = (u8 *)get_zeroed_page(GFP_KERNEL);
@@ -452,32 +485,23 @@ static int saa7134_go7007_init(struct saa7134_dev *dev)
 	if (!saa->bottom)
 		goto allocfail;
 
-	go = go7007_alloc(&board_voyager, &dev->pci->dev);
-	if (go == NULL)
-		goto allocfail;
-	go->board_id = GO7007_BOARDID_PCI_VOYAGER;
-	strncpy(go->name, saa7134_boards[dev->board].name, sizeof(go->name));
-	go->hpi_ops = &saa7134_go7007_hpi_ops;
-	go->hpi_context = saa;
-	saa->dev = dev;
-
 	/* Boot the GO7007 */
 	if (go7007_boot_encoder(go, go->board_info->flags &
 					GO7007_BOARD_USE_ONBOARD_I2C) < 0)
-		goto initfail;
+		goto allocfail;
 
 	/* Do any final GO7007 initialization, then register the
 	 * V4L2 and ALSA interfaces */
-	if (go7007_register_encoder(go) < 0)
-		goto initfail;
-	dev->empress_dev = go->video_dev;
-	video_set_drvdata(dev->empress_dev, go);
+	if (go7007_register_encoder(go, go->board_info->num_i2c_devs) < 0)
+		goto allocfail;
 
-	go->status = STATUS_ONLINE;
-	return 0;
+	/* Register the subdevice interface with the go7007 device */
+	if (v4l2_device_register_subdev(&go->v4l2_dev, sd) < 0)
+		printk(KERN_INFO "saa7134-go7007: register subdev failed\n");
 
-initfail:
-	go->status = STATUS_SHUTDOWN;
+	dev->empress_dev = &go->vdev;
+
+	go->status = STATUS_ONLINE;
 	return 0;
 
 allocfail:
@@ -486,6 +510,7 @@ allocfail:
 	if (saa->bottom)
 		free_page((unsigned long)saa->bottom);
 	kfree(saa);
+	kfree(go);
 	return -ENOMEM;
 }
 
@@ -498,12 +523,18 @@ static int saa7134_go7007_fini(struct saa7134_dev *dev)
 		return 0;
 
 	go = video_get_drvdata(dev->empress_dev);
+	if (go->audio_enabled)
+		go7007_snd_remove(go);
+
 	saa = go->hpi_context;
 	go->status = STATUS_SHUTDOWN;
 	free_page((unsigned long)saa->top);
 	free_page((unsigned long)saa->bottom);
+	v4l2_device_unregister_subdev(&saa->sd);
 	kfree(saa);
-	go7007_remove(go);
+	video_unregister_device(&go->vdev);
+
+	v4l2_device_put(&go->v4l2_dev);
 	dev->empress_dev = NULL;
 
 	return 0;
diff --git a/drivers/staging/media/go7007/snd-go7007.c b/drivers/staging/media/go7007/snd-go7007.c
index 5af29ff68bf..4be0fa40a39 100644
--- a/drivers/staging/media/go7007/snd-go7007.c
+++ b/drivers/staging/media/go7007/snd-go7007.c
@@ -221,8 +221,6 @@ static int go7007_snd_free(struct snd_device *device)
 
 	kfree(go->snd_context);
 	go->snd_context = NULL;
-	if (--go->ref_count == 0)
-		kfree(go);
 	return 0;
 }
 
@@ -267,9 +265,9 @@ int go7007_snd_init(struct go7007 *go)
 		kfree(gosnd);
 		return ret;
 	}
-	strncpy(gosnd->card->driver, "go7007", sizeof(gosnd->card->driver));
-	strncpy(gosnd->card->shortname, go->name, sizeof(gosnd->card->driver));
-	strncpy(gosnd->card->longname, gosnd->card->shortname,
+	strlcpy(gosnd->card->driver, "go7007", sizeof(gosnd->card->driver));
+	strlcpy(gosnd->card->shortname, go->name, sizeof(gosnd->card->driver));
+	strlcpy(gosnd->card->longname, gosnd->card->shortname,
 			sizeof(gosnd->card->longname));
 
 	gosnd->pcm->private_data = go;
@@ -285,8 +283,8 @@ int go7007_snd_init(struct go7007 *go)
 
 	gosnd->substream = NULL;
 	go->snd_context = gosnd;
+	v4l2_device_get(&go->v4l2_dev);
 	++dev;
-	++go->ref_count;
 
 	return 0;
 }
@@ -298,6 +296,7 @@ int go7007_snd_remove(struct go7007 *go)
 
 	snd_card_disconnect(gosnd->card);
 	snd_card_free_when_closed(gosnd->card);
+	v4l2_device_put(&go->v4l2_dev);
 	return 0;
 }
 EXPORT_SYMBOL(go7007_snd_remove);
diff --git a/drivers/staging/media/go7007/wis-i2c.h b/drivers/staging/media/go7007/wis-i2c.h
deleted file mode 100644
index 6d09c06c856..00000000000
--- a/drivers/staging/media/go7007/wis-i2c.h
+++ /dev/null
@@ -1,42 +0,0 @@
-/*
- * Copyright (C) 2005-2006 Micronas USA 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.
- */
-
-/* Temporary I2C IDs -- these need to be replaced with real registered IDs */
-#define	I2C_DRIVERID_WIS_SAA7115	0xf0f0
-#define	I2C_DRIVERID_WIS_UDA1342	0xf0f1
-#define	I2C_DRIVERID_WIS_SONY_TUNER	0xf0f2
-#define	I2C_DRIVERID_WIS_TW9903		0xf0f3
-#define	I2C_DRIVERID_WIS_SAA7113	0xf0f4
-#define	I2C_DRIVERID_WIS_OV7640		0xf0f5
-#define	I2C_DRIVERID_WIS_TW2804		0xf0f6
-#define	I2C_DRIVERID_S2250		0xf0f7
-
-/* Definitions for new video decoder commands */
-
-struct video_decoder_resolution {
-	unsigned int width;
-	unsigned int height;
-};
-
-#define	DECODER_SET_RESOLUTION	_IOW('d', 200, struct video_decoder_resolution)
-#define	DECODER_SET_CHANNEL	_IOW('d', 201, int)
-
-/* Sony tuner types */
-
-#define TUNER_SONY_BTF_PG472Z		200
-#define TUNER_SONY_BTF_PK467Z		201
-#define TUNER_SONY_BTF_PB463Z		202
diff --git a/drivers/staging/media/go7007/wis-saa7113.c b/drivers/staging/media/go7007/wis-saa7113.c
deleted file mode 100644
index 891cde713a4..00000000000
--- a/drivers/staging/media/go7007/wis-saa7113.c
+++ /dev/null
@@ -1,324 +0,0 @@
-/*
- * Copyright (C) 2005-2006 Micronas USA 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.
- */
-
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/i2c.h>
-#include <linux/videodev2.h>
-#include <linux/ioctl.h>
-#include <linux/slab.h>
-
-#include "wis-i2c.h"
-
-struct wis_saa7113 {
-	int norm;
-	int brightness;
-	int contrast;
-	int saturation;
-	int hue;
-};
-
-static u8 initial_registers[] = {
-	0x01, 0x08,
-	0x02, 0xc0,
-	0x03, 0x33,
-	0x04, 0x00,
-	0x05, 0x00,
-	0x06, 0xe9,
-	0x07, 0x0d,
-	0x08, 0xd8,
-	0x09, 0x40,
-	0x0a, 0x80,
-	0x0b, 0x47,
-	0x0c, 0x40,
-	0x0d, 0x00,
-	0x0e, 0x01,
-	0x0f, 0x2a,
-	0x10, 0x40,
-	0x11, 0x0c,
-	0x12, 0xfe,
-	0x13, 0x00,
-	0x14, 0x00,
-	0x15, 0x04,
-	0x16, 0x00,
-	0x17, 0x00,
-	0x18, 0x00,
-	0x19, 0x00,
-	0x1a, 0x00,
-	0x1b, 0x00,
-	0x1c, 0x00,
-	0x1d, 0x00,
-	0x1e, 0x00,
-	0x1f, 0xc8,
-	0x40, 0x00,
-	0x41, 0xff,
-	0x42, 0xff,
-	0x43, 0xff,
-	0x44, 0xff,
-	0x45, 0xff,
-	0x46, 0xff,
-	0x47, 0xff,
-	0x48, 0xff,
-	0x49, 0xff,
-	0x4a, 0xff,
-	0x4b, 0xff,
-	0x4c, 0xff,
-	0x4d, 0xff,
-	0x4e, 0xff,
-	0x4f, 0xff,
-	0x50, 0xff,
-	0x51, 0xff,
-	0x52, 0xff,
-	0x53, 0xff,
-	0x54, 0xff,
-	0x55, 0xff,
-	0x56, 0xff,
-	0x57, 0xff,
-	0x58, 0x00,
-	0x59, 0x54,
-	0x5a, 0x07,
-	0x5b, 0x83,
-	0x5c, 0x00,
-	0x5d, 0x00,
-	0x5e, 0x00,
-	0x5f, 0x00,
-	0x60, 0x00,
-	0x61, 0x00,
-	0x00, 0x00, /* Terminator (reg 0x00 is read-only) */
-};
-
-static int write_reg(struct i2c_client *client, u8 reg, u8 value)
-{
-	return i2c_smbus_write_byte_data(client, reg, value);
-}
-
-static int write_regs(struct i2c_client *client, u8 *regs)
-{
-	int i;
-
-	for (i = 0; regs[i] != 0x00; i += 2)
-		if (i2c_smbus_write_byte_data(client, regs[i], regs[i + 1]) < 0)
-			return -1;
-	return 0;
-}
-
-static int wis_saa7113_command(struct i2c_client *client,
-				unsigned int cmd, void *arg)
-{
-	struct wis_saa7113 *dec = i2c_get_clientdata(client);
-
-	switch (cmd) {
-	case VIDIOC_S_INPUT:
-	{
-		int *input = arg;
-
-		i2c_smbus_write_byte_data(client, 0x02, 0xC0 | *input);
-		i2c_smbus_write_byte_data(client, 0x09,
-				*input < 6 ? 0x40 : 0x80);
-		break;
-	}
-	case VIDIOC_S_STD:
-	{
-		v4l2_std_id *input = arg;
-		dec->norm = *input;
-		if (dec->norm & V4L2_STD_NTSC) {
-			write_reg(client, 0x0e, 0x01);
-			write_reg(client, 0x10, 0x40);
-		} else if (dec->norm & V4L2_STD_PAL) {
-			write_reg(client, 0x0e, 0x01);
-			write_reg(client, 0x10, 0x48);
-		} else if (dec->norm & V4L2_STD_SECAM) {
-			write_reg(client, 0x0e, 0x50);
-			write_reg(client, 0x10, 0x48);
-		}
-		break;
-	}
-	case VIDIOC_QUERYCTRL:
-	{
-		struct v4l2_queryctrl *ctrl = arg;
-
-		switch (ctrl->id) {
-		case V4L2_CID_BRIGHTNESS:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Brightness", sizeof(ctrl->name));
-			ctrl->minimum = 0;
-			ctrl->maximum = 255;
-			ctrl->step = 1;
-			ctrl->default_value = 128;
-			ctrl->flags = 0;
-			break;
-		case V4L2_CID_CONTRAST:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Contrast", sizeof(ctrl->name));
-			ctrl->minimum = 0;
-			ctrl->maximum = 127;
-			ctrl->step = 1;
-			ctrl->default_value = 71;
-			ctrl->flags = 0;
-			break;
-		case V4L2_CID_SATURATION:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Saturation", sizeof(ctrl->name));
-			ctrl->minimum = 0;
-			ctrl->maximum = 127;
-			ctrl->step = 1;
-			ctrl->default_value = 64;
-			ctrl->flags = 0;
-			break;
-		case V4L2_CID_HUE:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Hue", sizeof(ctrl->name));
-			ctrl->minimum = -128;
-			ctrl->maximum = 127;
-			ctrl->step = 1;
-			ctrl->default_value = 0;
-			ctrl->flags = 0;
-			break;
-		}
-		break;
-	}
-	case VIDIOC_S_CTRL:
-	{
-		struct v4l2_control *ctrl = arg;
-
-		switch (ctrl->id) {
-		case V4L2_CID_BRIGHTNESS:
-			if (ctrl->value > 255)
-				dec->brightness = 255;
-			else if (ctrl->value < 0)
-				dec->brightness = 0;
-			else
-				dec->brightness = ctrl->value;
-			write_reg(client, 0x0a, dec->brightness);
-			break;
-		case V4L2_CID_CONTRAST:
-			if (ctrl->value > 127)
-				dec->contrast = 127;
-			else if (ctrl->value < 0)
-				dec->contrast = 0;
-			else
-				dec->contrast = ctrl->value;
-			write_reg(client, 0x0b, dec->contrast);
-			break;
-		case V4L2_CID_SATURATION:
-			if (ctrl->value > 127)
-				dec->saturation = 127;
-			else if (ctrl->value < 0)
-				dec->saturation = 0;
-			else
-				dec->saturation = ctrl->value;
-			write_reg(client, 0x0c, dec->saturation);
-			break;
-		case V4L2_CID_HUE:
-			if (ctrl->value > 127)
-				dec->hue = 127;
-			else if (ctrl->value < -128)
-				dec->hue = -128;
-			else
-				dec->hue = ctrl->value;
-			write_reg(client, 0x0d, dec->hue);
-			break;
-		}
-		break;
-	}
-	case VIDIOC_G_CTRL:
-	{
-		struct v4l2_control *ctrl = arg;
-
-		switch (ctrl->id) {
-		case V4L2_CID_BRIGHTNESS:
-			ctrl->value = dec->brightness;
-			break;
-		case V4L2_CID_CONTRAST:
-			ctrl->value = dec->contrast;
-			break;
-		case V4L2_CID_SATURATION:
-			ctrl->value = dec->saturation;
-			break;
-		case V4L2_CID_HUE:
-			ctrl->value = dec->hue;
-			break;
-		}
-		break;
-	}
-	default:
-		break;
-	}
-	return 0;
-}
-
-static int wis_saa7113_probe(struct i2c_client *client,
-			     const struct i2c_device_id *id)
-{
-	struct i2c_adapter *adapter = client->adapter;
-	struct wis_saa7113 *dec;
-
-	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
-		return -ENODEV;
-
-	dec = kmalloc(sizeof(struct wis_saa7113), GFP_KERNEL);
-	if (dec == NULL)
-		return -ENOMEM;
-
-	dec->norm = V4L2_STD_NTSC;
-	dec->brightness = 128;
-	dec->contrast = 71;
-	dec->saturation = 64;
-	dec->hue = 0;
-	i2c_set_clientdata(client, dec);
-
-	dev_dbg(&client->dev,
-		"wis-saa7113: initializing SAA7113 at address %d on %s\n",
-		client->addr, adapter->name);
-
-	if (write_regs(client, initial_registers) < 0) {
-		dev_err(&client->dev,
-			"wis-saa7113: error initializing SAA7113\n");
-		kfree(dec);
-		return -ENODEV;
-	}
-
-	return 0;
-}
-
-static int wis_saa7113_remove(struct i2c_client *client)
-{
-	struct wis_saa7113 *dec = i2c_get_clientdata(client);
-
-	kfree(dec);
-	return 0;
-}
-
-static const struct i2c_device_id wis_saa7113_id[] = {
-	{ "wis_saa7113", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, wis_saa7113_id);
-
-static struct i2c_driver wis_saa7113_driver = {
-	.driver = {
-		.name	= "WIS SAA7113 I2C driver",
-	},
-	.probe		= wis_saa7113_probe,
-	.remove		= wis_saa7113_remove,
-	.command	= wis_saa7113_command,
-	.id_table	= wis_saa7113_id,
-};
-
-module_i2c_driver(wis_saa7113_driver);
-
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/media/go7007/wis-saa7115.c b/drivers/staging/media/go7007/wis-saa7115.c
deleted file mode 100644
index fa86acd3fdf..00000000000
--- a/drivers/staging/media/go7007/wis-saa7115.c
+++ /dev/null
@@ -1,457 +0,0 @@
-/*
- * Copyright (C) 2005-2006 Micronas USA 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.
- */
-
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/i2c.h>
-#include <linux/videodev2.h>
-#include <linux/ioctl.h>
-#include <linux/slab.h>
-
-#include "wis-i2c.h"
-
-struct wis_saa7115 {
-	int norm;
-	int brightness;
-	int contrast;
-	int saturation;
-	int hue;
-};
-
-static u8 initial_registers[] = {
-	0x01, 0x08,
-	0x02, 0xc0,
-	0x03, 0x20,
-	0x04, 0x80,
-	0x05, 0x80,
-	0x06, 0xeb,
-	0x07, 0xe0,
-	0x08, 0xf0,	/* always toggle FID */
-	0x09, 0x40,
-	0x0a, 0x80,
-	0x0b, 0x40,
-	0x0c, 0x40,
-	0x0d, 0x00,
-	0x0e, 0x03,
-	0x0f, 0x2a,
-	0x10, 0x0e,
-	0x11, 0x00,
-	0x12, 0x8d,
-	0x13, 0x00,
-	0x14, 0x00,
-	0x15, 0x11,
-	0x16, 0x01,
-	0x17, 0xda,
-	0x18, 0x40,
-	0x19, 0x80,
-	0x1a, 0x00,
-	0x1b, 0x42,
-	0x1c, 0xa9,
-	0x30, 0x66,
-	0x31, 0x90,
-	0x32, 0x01,
-	0x34, 0x00,
-	0x35, 0x00,
-	0x36, 0x20,
-	0x38, 0x03,
-	0x39, 0x20,
-	0x3a, 0x88,
-	0x40, 0x00,
-	0x41, 0xff,
-	0x42, 0xff,
-	0x43, 0xff,
-	0x44, 0xff,
-	0x45, 0xff,
-	0x46, 0xff,
-	0x47, 0xff,
-	0x48, 0xff,
-	0x49, 0xff,
-	0x4a, 0xff,
-	0x4b, 0xff,
-	0x4c, 0xff,
-	0x4d, 0xff,
-	0x4e, 0xff,
-	0x4f, 0xff,
-	0x50, 0xff,
-	0x51, 0xff,
-	0x52, 0xff,
-	0x53, 0xff,
-	0x54, 0xf4 /*0xff*/,
-	0x55, 0xff,
-	0x56, 0xff,
-	0x57, 0xff,
-	0x58, 0x40,
-	0x59, 0x47,
-	0x5a, 0x06 /*0x03*/,
-	0x5b, 0x83,
-	0x5d, 0x06,
-	0x5e, 0x00,
-	0x80, 0x30, /* window defined scaler operation, task A and B enabled */
-	0x81, 0x03, /* use scaler datapath generated V */
-	0x83, 0x00,
-	0x84, 0x00,
-	0x85, 0x00,
-	0x86, 0x45,
-	0x87, 0x31,
-	0x88, 0xc0,
-	0x90, 0x02, /* task A process top field */
-	0x91, 0x08,
-	0x92, 0x09,
-	0x93, 0x80,
-	0x94, 0x06,
-	0x95, 0x00,
-	0x96, 0xc0,
-	0x97, 0x02,
-	0x98, 0x12,
-	0x99, 0x00,
-	0x9a, 0xf2,
-	0x9b, 0x00,
-	0x9c, 0xd0,
-	0x9d, 0x02,
-	0x9e, 0xf2,
-	0x9f, 0x00,
-	0xa0, 0x01,
-	0xa1, 0x01,
-	0xa2, 0x01,
-	0xa4, 0x80,
-	0xa5, 0x40,
-	0xa6, 0x40,
-	0xa8, 0x00,
-	0xa9, 0x04,
-	0xaa, 0x00,
-	0xac, 0x00,
-	0xad, 0x02,
-	0xae, 0x00,
-	0xb0, 0x00,
-	0xb1, 0x04,
-	0xb2, 0x00,
-	0xb3, 0x04,
-	0xb4, 0x00,
-	0xb8, 0x00,
-	0xbc, 0x00,
-	0xc0, 0x03,	/* task B process bottom field */
-	0xc1, 0x08,
-	0xc2, 0x09,
-	0xc3, 0x80,
-	0xc4, 0x06,
-	0xc5, 0x00,
-	0xc6, 0xc0,
-	0xc7, 0x02,
-	0xc8, 0x12,
-	0xc9, 0x00,
-	0xca, 0xf2,
-	0xcb, 0x00,
-	0xcc, 0xd0,
-	0xcd, 0x02,
-	0xce, 0xf2,
-	0xcf, 0x00,
-	0xd0, 0x01,
-	0xd1, 0x01,
-	0xd2, 0x01,
-	0xd4, 0x80,
-	0xd5, 0x40,
-	0xd6, 0x40,
-	0xd8, 0x00,
-	0xd9, 0x04,
-	0xda, 0x00,
-	0xdc, 0x00,
-	0xdd, 0x02,
-	0xde, 0x00,
-	0xe0, 0x00,
-	0xe1, 0x04,
-	0xe2, 0x00,
-	0xe3, 0x04,
-	0xe4, 0x00,
-	0xe8, 0x00,
-	0x88, 0xf0, /* End of original static list */
-	0x00, 0x00, /* Terminator (reg 0x00 is read-only) */
-};
-
-static int write_reg(struct i2c_client *client, u8 reg, u8 value)
-{
-	return i2c_smbus_write_byte_data(client, reg, value);
-}
-
-static int write_regs(struct i2c_client *client, u8 *regs)
-{
-	int i;
-
-	for (i = 0; regs[i] != 0x00; i += 2)
-		if (i2c_smbus_write_byte_data(client, regs[i], regs[i + 1]) < 0)
-			return -1;
-	return 0;
-}
-
-static int wis_saa7115_command(struct i2c_client *client,
-				unsigned int cmd, void *arg)
-{
-	struct wis_saa7115 *dec = i2c_get_clientdata(client);
-
-	switch (cmd) {
-	case VIDIOC_S_INPUT:
-	{
-		int *input = arg;
-
-		i2c_smbus_write_byte_data(client, 0x02, 0xC0 | *input);
-		i2c_smbus_write_byte_data(client, 0x09,
-				*input < 6 ? 0x40 : 0xC0);
-		break;
-	}
-	case DECODER_SET_RESOLUTION:
-	{
-		struct video_decoder_resolution *res = arg;
-		/* Course-grained scaler */
-		int h_integer_scaler = res->width < 704 ? 704 / res->width : 1;
-		/* Fine-grained scaler to take care of remainder */
-		int h_scaling_increment = (704 / h_integer_scaler) *
-					1024 / res->width;
-		/* Fine-grained scaler only */
-		int v_scaling_increment = (dec->norm & V4L2_STD_NTSC ?
-				240 : 288) * 1024 / res->height;
-		u8 regs[] = {
-			0x88,	0xc0,
-			0x9c,	res->width & 0xff,
-			0x9d,	res->width >> 8,
-			0x9e,	res->height & 0xff,
-			0x9f,	res->height >> 8,
-			0xa0,	h_integer_scaler,
-			0xa1,	1,
-			0xa2,	1,
-			0xa8,	h_scaling_increment & 0xff,
-			0xa9,	h_scaling_increment >> 8,
-			0xac,	(h_scaling_increment / 2) & 0xff,
-			0xad,	(h_scaling_increment / 2) >> 8,
-			0xb0,	v_scaling_increment & 0xff,
-			0xb1,	v_scaling_increment >> 8,
-			0xb2,	v_scaling_increment & 0xff,
-			0xb3,	v_scaling_increment >> 8,
-			0xcc,	res->width & 0xff,
-			0xcd,	res->width >> 8,
-			0xce,	res->height & 0xff,
-			0xcf,	res->height >> 8,
-			0xd0,	h_integer_scaler,
-			0xd1,	1,
-			0xd2,	1,
-			0xd8,	h_scaling_increment & 0xff,
-			0xd9,	h_scaling_increment >> 8,
-			0xdc,	(h_scaling_increment / 2) & 0xff,
-			0xdd,	(h_scaling_increment / 2) >> 8,
-			0xe0,	v_scaling_increment & 0xff,
-			0xe1,	v_scaling_increment >> 8,
-			0xe2,	v_scaling_increment & 0xff,
-			0xe3,	v_scaling_increment >> 8,
-			0x88,	0xf0,
-			0,	0,
-		};
-		write_regs(client, regs);
-		break;
-	}
-	case VIDIOC_S_STD:
-	{
-		v4l2_std_id *input = arg;
-		u8 regs[] = {
-			0x88,	0xc0,
-			0x98,	*input & V4L2_STD_NTSC ? 0x12 : 0x16,
-			0x9a,	*input & V4L2_STD_NTSC ? 0xf2 : 0x20,
-			0x9b,	*input & V4L2_STD_NTSC ? 0x00 : 0x01,
-			0xc8,	*input & V4L2_STD_NTSC ? 0x12 : 0x16,
-			0xca,	*input & V4L2_STD_NTSC ? 0xf2 : 0x20,
-			0xcb,	*input & V4L2_STD_NTSC ? 0x00 : 0x01,
-			0x88,	0xf0,
-			0x30,	*input & V4L2_STD_NTSC ? 0x66 : 0x00,
-			0x31,	*input & V4L2_STD_NTSC ? 0x90 : 0xe0,
-			0,	0,
-		};
-		write_regs(client, regs);
-		dec->norm = *input;
-		break;
-	}
-	case VIDIOC_QUERYCTRL:
-	{
-		struct v4l2_queryctrl *ctrl = arg;
-
-		switch (ctrl->id) {
-		case V4L2_CID_BRIGHTNESS:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Brightness", sizeof(ctrl->name));
-			ctrl->minimum = 0;
-			ctrl->maximum = 255;
-			ctrl->step = 1;
-			ctrl->default_value = 128;
-			ctrl->flags = 0;
-			break;
-		case V4L2_CID_CONTRAST:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Contrast", sizeof(ctrl->name));
-			ctrl->minimum = 0;
-			ctrl->maximum = 127;
-			ctrl->step = 1;
-			ctrl->default_value = 64;
-			ctrl->flags = 0;
-			break;
-		case V4L2_CID_SATURATION:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Saturation", sizeof(ctrl->name));
-			ctrl->minimum = 0;
-			ctrl->maximum = 127;
-			ctrl->step = 1;
-			ctrl->default_value = 64;
-			ctrl->flags = 0;
-			break;
-		case V4L2_CID_HUE:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Hue", sizeof(ctrl->name));
-			ctrl->minimum = -128;
-			ctrl->maximum = 127;
-			ctrl->step = 1;
-			ctrl->default_value = 0;
-			ctrl->flags = 0;
-			break;
-		}
-		break;
-	}
-	case VIDIOC_S_CTRL:
-	{
-		struct v4l2_control *ctrl = arg;
-
-		switch (ctrl->id) {
-		case V4L2_CID_BRIGHTNESS:
-			if (ctrl->value > 255)
-				dec->brightness = 255;
-			else if (ctrl->value < 0)
-				dec->brightness = 0;
-			else
-				dec->brightness = ctrl->value;
-			write_reg(client, 0x0a, dec->brightness);
-			break;
-		case V4L2_CID_CONTRAST:
-			if (ctrl->value > 127)
-				dec->contrast = 127;
-			else if (ctrl->value < 0)
-				dec->contrast = 0;
-			else
-				dec->contrast = ctrl->value;
-			write_reg(client, 0x0b, dec->contrast);
-			break;
-		case V4L2_CID_SATURATION:
-			if (ctrl->value > 127)
-				dec->saturation = 127;
-			else if (ctrl->value < 0)
-				dec->saturation = 0;
-			else
-				dec->saturation = ctrl->value;
-			write_reg(client, 0x0c, dec->saturation);
-			break;
-		case V4L2_CID_HUE:
-			if (ctrl->value > 127)
-				dec->hue = 127;
-			else if (ctrl->value < -128)
-				dec->hue = -128;
-			else
-				dec->hue = ctrl->value;
-			write_reg(client, 0x0d, dec->hue);
-			break;
-		}
-		break;
-	}
-	case VIDIOC_G_CTRL:
-	{
-		struct v4l2_control *ctrl = arg;
-
-		switch (ctrl->id) {
-		case V4L2_CID_BRIGHTNESS:
-			ctrl->value = dec->brightness;
-			break;
-		case V4L2_CID_CONTRAST:
-			ctrl->value = dec->contrast;
-			break;
-		case V4L2_CID_SATURATION:
-			ctrl->value = dec->saturation;
-			break;
-		case V4L2_CID_HUE:
-			ctrl->value = dec->hue;
-			break;
-		}
-		break;
-	}
-	default:
-		break;
-	}
-	return 0;
-}
-
-static int wis_saa7115_probe(struct i2c_client *client,
-			     const struct i2c_device_id *id)
-{
-	struct i2c_adapter *adapter = client->adapter;
-	struct wis_saa7115 *dec;
-
-	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
-		return -ENODEV;
-
-	dec = kmalloc(sizeof(struct wis_saa7115), GFP_KERNEL);
-	if (dec == NULL)
-		return -ENOMEM;
-
-	dec->norm = V4L2_STD_NTSC;
-	dec->brightness = 128;
-	dec->contrast = 64;
-	dec->saturation = 64;
-	dec->hue = 0;
-	i2c_set_clientdata(client, dec);
-
-	dev_dbg(&client->dev,
-		"wis-saa7115: initializing SAA7115 at address %d on %s\n",
-		client->addr, adapter->name);
-
-	if (write_regs(client, initial_registers) < 0) {
-		dev_err(&client->dev,
-			"wis-saa7115: error initializing SAA7115\n");
-		kfree(dec);
-		return -ENODEV;
-	}
-
-	return 0;
-}
-
-static int wis_saa7115_remove(struct i2c_client *client)
-{
-	struct wis_saa7115 *dec = i2c_get_clientdata(client);
-
-	kfree(dec);
-	return 0;
-}
-
-static const struct i2c_device_id wis_saa7115_id[] = {
-	{ "wis_saa7115", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, wis_saa7115_id);
-
-static struct i2c_driver wis_saa7115_driver = {
-	.driver = {
-		.name	= "WIS SAA7115 I2C driver",
-	},
-	.probe		= wis_saa7115_probe,
-	.remove		= wis_saa7115_remove,
-	.command	= wis_saa7115_command,
-	.id_table	= wis_saa7115_id,
-};
-
-module_i2c_driver(wis_saa7115_driver);
-
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/media/go7007/wis-sony-tuner.c b/drivers/staging/media/go7007/wis-sony-tuner.c
deleted file mode 100644
index 5d7ff8c81d6..00000000000
--- a/drivers/staging/media/go7007/wis-sony-tuner.c
+++ /dev/null
@@ -1,707 +0,0 @@
-/*
- * Copyright (C) 2005-2006 Micronas USA 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.
- */
-
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/i2c.h>
-#include <linux/videodev2.h>
-#include <linux/slab.h>
-#include <media/tuner.h>
-#include <media/v4l2-common.h>
-#include <media/v4l2-ioctl.h>
-
-#include "wis-i2c.h"
-
-/* #define MPX_DEBUG */
-
-/* AS(IF/MPX) pin:      LOW      HIGH/OPEN
- * IF/MPX address:   0x42/0x40   0x43/0x44
- */
-#define IF_I2C_ADDR	0x43
-#define MPX_I2C_ADDR	0x44
-
-static v4l2_std_id force_band;
-static char force_band_str[] = "-";
-module_param_string(force_band, force_band_str, sizeof(force_band_str), 0644);
-static int force_mpx_mode = -1;
-module_param(force_mpx_mode, int, 0644);
-
-/* Store tuner info in the same format as tuner.c, so maybe we can put the
- * Sony tuner support in there. */
-struct sony_tunertype {
-	char *name;
-	unsigned char Vendor; /* unused here */
-	unsigned char Type; /* unused here */
-
-	unsigned short thresh1; /*  band switch VHF_LO <=> VHF_HI */
-	unsigned short thresh2; /*  band switch VHF_HI <=> UHF */
-	unsigned char VHF_L;
-	unsigned char VHF_H;
-	unsigned char UHF;
-	unsigned char config;
-	unsigned short IFPCoff;
-};
-
-/* This array is indexed by (tuner_type - 200) */
-static struct sony_tunertype sony_tuners[] = {
-	{ "Sony PAL+SECAM (BTF-PG472Z)", 0, 0,
-	  16*144.25, 16*427.25, 0x01, 0x02, 0x04, 0xc6, 623},
-	{ "Sony NTSC_JP (BTF-PK467Z)", 0, 0,
-	  16*220.25, 16*467.25, 0x01, 0x02, 0x04, 0xc6, 940},
-	{ "Sony NTSC (BTF-PB463Z)", 0, 0,
-	  16*130.25, 16*364.25, 0x01, 0x02, 0x04, 0xc6, 732},
-};
-
-struct wis_sony_tuner {
-	int type;
-	v4l2_std_id std;
-	unsigned int freq;
-	int mpxmode;
-	u32 audmode;
-};
-
-/* Basically the same as default_set_tv_freq() in tuner.c */
-static int set_freq(struct i2c_client *client, int freq)
-{
-	struct wis_sony_tuner *t = i2c_get_clientdata(client);
-	char *band_name;
-	int n;
-	int band_select;
-	struct sony_tunertype *tun;
-	u8 buffer[4];
-
-	tun = &sony_tuners[t->type - 200];
-	if (freq < tun->thresh1) {
-		band_name = "VHF_L";
-		band_select = tun->VHF_L;
-	} else if (freq < tun->thresh2) {
-		band_name = "VHF_H";
-		band_select = tun->VHF_H;
-	} else {
-		band_name = "UHF";
-		band_select = tun->UHF;
-	}
-	dev_dbg(&client->dev, "tuning to frequency %d.%04d (%s)\n",
-		freq / 16, (freq % 16) * 625, band_name);
-	n = freq + tun->IFPCoff;
-
-	buffer[0] = n >> 8;
-	buffer[1] = n & 0xff;
-	buffer[2] = tun->config;
-	buffer[3] = band_select;
-	i2c_master_send(client, buffer, 4);
-
-	return 0;
-}
-
-static int mpx_write(struct i2c_client *client, int dev, int addr, int val)
-{
-	u8 buffer[5];
-	struct i2c_msg msg;
-
-	buffer[0] = dev;
-	buffer[1] = addr >> 8;
-	buffer[2] = addr & 0xff;
-	buffer[3] = val >> 8;
-	buffer[4] = val & 0xff;
-	msg.addr = MPX_I2C_ADDR;
-	msg.flags = 0;
-	msg.len = 5;
-	msg.buf = buffer;
-	i2c_transfer(client->adapter, &msg, 1);
-	return 0;
-}
-
-/*
- * MPX register values for the BTF-PG472Z:
- *
- *                                 FM_     NICAM_  SCART_
- *          MODUS  SOURCE    ACB   PRESCAL PRESCAL PRESCAL SYSTEM  VOLUME
- *         10/0030 12/0008 12/0013 12/000E 12/0010 12/0000 10/0020 12/0000
- *         ---------------------------------------------------------------
- * Auto     1003    0020    0100    2603    5000    XXXX    0001    7500
- *
- * B/G
- *  Mono    1003    0020    0100    2603    5000    XXXX    0003    7500
- *  A2      1003    0020    0100    2601    5000    XXXX    0003    7500
- *  NICAM   1003    0120    0100    2603    5000    XXXX    0008    7500
- *
- * I
- *  Mono    1003    0020    0100    2603    7900    XXXX    000A    7500
- *  NICAM   1003    0120    0100    2603    7900    XXXX    000A    7500
- *
- * D/K
- *  Mono    1003    0020    0100    2603    5000    XXXX    0004    7500
- *  A2-1    1003    0020    0100    2601    5000    XXXX    0004    7500
- *  A2-2    1003    0020    0100    2601    5000    XXXX    0005    7500
- *  A2-3    1003    0020    0100    2601    5000    XXXX    0007    7500
- *  NICAM   1003    0120    0100    2603    5000    XXXX    000B    7500
- *
- * L/L'
- *  Mono    0003    0200    0100    7C03    5000    2200    0009    7500
- *  NICAM   0003    0120    0100    7C03    5000    XXXX    0009    7500
- *
- * M
- *  Mono    1003    0200    0100    2B03    5000    2B00    0002    7500
- *
- * For Asia, replace the 0x26XX in FM_PRESCALE with 0x14XX.
- *
- * Bilingual selection in A2/NICAM:
- *
- *         High byte of SOURCE     Left chan   Right chan
- *                 0x01              MAIN         SUB
- *                 0x03              MAIN         MAIN
- *                 0x04              SUB          SUB
- *
- * Force mono in NICAM by setting the high byte of SOURCE to 0x02 (L/L') or
- * 0x00 (all other bands).  Force mono in A2 with FMONO_A2:
- *
- *                      FMONO_A2
- *                      10/0022
- *                      --------
- *     Forced mono ON     07F0
- *     Forced mono OFF    0190
- */
-
-static struct {
-	enum { AUD_MONO, AUD_A2, AUD_NICAM, AUD_NICAM_L } audio_mode;
-	u16 modus;
-	u16 source;
-	u16 acb;
-	u16 fm_prescale;
-	u16 nicam_prescale;
-	u16 scart_prescale;
-	u16 system;
-	u16 volume;
-} mpx_audio_modes[] = {
-	/* Auto */	{ AUD_MONO,	0x1003, 0x0020, 0x0100, 0x2603,
-					0x5000, 0x0000, 0x0001, 0x7500 },
-	/* B/G Mono */	{ AUD_MONO,	0x1003, 0x0020, 0x0100, 0x2603,
-					0x5000, 0x0000, 0x0003, 0x7500 },
-	/* B/G A2 */	{ AUD_A2,	0x1003, 0x0020, 0x0100, 0x2601,
-					0x5000, 0x0000, 0x0003, 0x7500 },
-	/* B/G NICAM */ { AUD_NICAM,	0x1003, 0x0120, 0x0100, 0x2603,
-					0x5000, 0x0000, 0x0008, 0x7500 },
-	/* I Mono */	{ AUD_MONO,	0x1003, 0x0020, 0x0100, 0x2603,
-					0x7900, 0x0000, 0x000A, 0x7500 },
-	/* I NICAM */	{ AUD_NICAM,	0x1003, 0x0120, 0x0100, 0x2603,
-					0x7900, 0x0000, 0x000A, 0x7500 },
-	/* D/K Mono */	{ AUD_MONO,	0x1003, 0x0020, 0x0100, 0x2603,
-					0x5000, 0x0000, 0x0004, 0x7500 },
-	/* D/K A2-1 */	{ AUD_A2,	0x1003, 0x0020, 0x0100, 0x2601,
-					0x5000, 0x0000, 0x0004, 0x7500 },
-	/* D/K A2-2 */	{ AUD_A2,	0x1003, 0x0020, 0x0100, 0x2601,
-					0x5000, 0x0000, 0x0005, 0x7500 },
-	/* D/K A2-3 */	{ AUD_A2,	0x1003, 0x0020, 0x0100, 0x2601,
-					0x5000, 0x0000, 0x0007, 0x7500 },
-	/* D/K NICAM */	{ AUD_NICAM,	0x1003, 0x0120, 0x0100, 0x2603,
-					0x5000, 0x0000, 0x000B, 0x7500 },
-	/* L/L' Mono */	{ AUD_MONO,	0x0003, 0x0200, 0x0100, 0x7C03,
-					0x5000, 0x2200, 0x0009, 0x7500 },
-	/* L/L' NICAM */{ AUD_NICAM_L,	0x0003, 0x0120, 0x0100, 0x7C03,
-					0x5000, 0x0000, 0x0009, 0x7500 },
-};
-
-#define MPX_NUM_MODES	ARRAY_SIZE(mpx_audio_modes)
-
-static int mpx_setup(struct i2c_client *client)
-{
-	struct wis_sony_tuner *t = i2c_get_clientdata(client);
-	u16 source = 0;
-	u8 buffer[3];
-	struct i2c_msg msg;
-
-	/* reset MPX */
-	buffer[0] = 0x00;
-	buffer[1] = 0x80;
-	buffer[2] = 0x00;
-	msg.addr = MPX_I2C_ADDR;
-	msg.flags = 0;
-	msg.len = 3;
-	msg.buf = buffer;
-	i2c_transfer(client->adapter, &msg, 1);
-	buffer[1] = 0x00;
-	i2c_transfer(client->adapter, &msg, 1);
-
-	if (mpx_audio_modes[t->mpxmode].audio_mode != AUD_MONO) {
-		switch (t->audmode) {
-		case V4L2_TUNER_MODE_MONO:
-			switch (mpx_audio_modes[t->mpxmode].audio_mode) {
-			case AUD_A2:
-				source = mpx_audio_modes[t->mpxmode].source;
-				break;
-			case AUD_NICAM:
-				source = 0x0000;
-				break;
-			case AUD_NICAM_L:
-				source = 0x0200;
-				break;
-			default:
-				break;
-			}
-			break;
-		case V4L2_TUNER_MODE_STEREO:
-			source = mpx_audio_modes[t->mpxmode].source;
-			break;
-		case V4L2_TUNER_MODE_LANG1:
-			source = 0x0300;
-			break;
-		case V4L2_TUNER_MODE_LANG2:
-			source = 0x0400;
-			break;
-		}
-		source |= mpx_audio_modes[t->mpxmode].source & 0x00ff;
-	} else
-		source = mpx_audio_modes[t->mpxmode].source;
-
-	mpx_write(client, 0x10, 0x0030, mpx_audio_modes[t->mpxmode].modus);
-	mpx_write(client, 0x12, 0x0008, source);
-	mpx_write(client, 0x12, 0x0013, mpx_audio_modes[t->mpxmode].acb);
-	mpx_write(client, 0x12, 0x000e,
-			mpx_audio_modes[t->mpxmode].fm_prescale);
-	mpx_write(client, 0x12, 0x0010,
-			mpx_audio_modes[t->mpxmode].nicam_prescale);
-	mpx_write(client, 0x12, 0x000d,
-			mpx_audio_modes[t->mpxmode].scart_prescale);
-	mpx_write(client, 0x10, 0x0020, mpx_audio_modes[t->mpxmode].system);
-	mpx_write(client, 0x12, 0x0000, mpx_audio_modes[t->mpxmode].volume);
-	if (mpx_audio_modes[t->mpxmode].audio_mode == AUD_A2)
-		mpx_write(client, 0x10, 0x0022,
-			t->audmode == V4L2_TUNER_MODE_MONO ?  0x07f0 : 0x0190);
-
-#ifdef MPX_DEBUG
-	{
-		u8 buf1[3], buf2[2];
-		struct i2c_msg msgs[2];
-
-		dev_dbg(&client->dev,
-			"MPX registers: %04x %04x %04x %04x %04x %04x %04x %04x\n",
-			mpx_audio_modes[t->mpxmode].modus,
-			source,
-			mpx_audio_modes[t->mpxmode].acb,
-			mpx_audio_modes[t->mpxmode].fm_prescale,
-			mpx_audio_modes[t->mpxmode].nicam_prescale,
-			mpx_audio_modes[t->mpxmode].scart_prescale,
-			mpx_audio_modes[t->mpxmode].system,
-			mpx_audio_modes[t->mpxmode].volume);
-		buf1[0] = 0x11;
-		buf1[1] = 0x00;
-		buf1[2] = 0x7e;
-		msgs[0].addr = MPX_I2C_ADDR;
-		msgs[0].flags = 0;
-		msgs[0].len = 3;
-		msgs[0].buf = buf1;
-		msgs[1].addr = MPX_I2C_ADDR;
-		msgs[1].flags = I2C_M_RD;
-		msgs[1].len = 2;
-		msgs[1].buf = buf2;
-		i2c_transfer(client->adapter, msgs, 2);
-		dev_dbg(&client->dev, "MPX system: %02x%02x\n",
-			buf2[0], buf2[1]);
-		buf1[0] = 0x11;
-		buf1[1] = 0x02;
-		buf1[2] = 0x00;
-		i2c_transfer(client->adapter, msgs, 2);
-		dev_dbg(&client->dev, "MPX status: %02x%02x\n",
-			buf2[0], buf2[1]);
-	}
-#endif
-	return 0;
-}
-
-/*
- * IF configuration values for the BTF-PG472Z:
- *
- *	B/G: 0x94 0x70 0x49
- *	I:   0x14 0x70 0x4a
- *	D/K: 0x14 0x70 0x4b
- *	L:   0x04 0x70 0x4b
- *	L':  0x44 0x70 0x53
- *	M:   0x50 0x30 0x4c
- */
-
-static int set_if(struct i2c_client *client)
-{
-	struct wis_sony_tuner *t = i2c_get_clientdata(client);
-	u8 buffer[4];
-	struct i2c_msg msg;
-	int default_mpx_mode = 0;
-
-	/* configure IF */
-	buffer[0] = 0;
-	if (t->std & V4L2_STD_PAL_BG) {
-		buffer[1] = 0x94;
-		buffer[2] = 0x70;
-		buffer[3] = 0x49;
-		default_mpx_mode = 1;
-	} else if (t->std & V4L2_STD_PAL_I) {
-		buffer[1] = 0x14;
-		buffer[2] = 0x70;
-		buffer[3] = 0x4a;
-		default_mpx_mode = 4;
-	} else if (t->std & V4L2_STD_PAL_DK) {
-		buffer[1] = 0x14;
-		buffer[2] = 0x70;
-		buffer[3] = 0x4b;
-		default_mpx_mode = 6;
-	} else if (t->std & V4L2_STD_SECAM_L) {
-		buffer[1] = 0x04;
-		buffer[2] = 0x70;
-		buffer[3] = 0x4b;
-		default_mpx_mode = 11;
-	}
-	msg.addr = IF_I2C_ADDR;
-	msg.flags = 0;
-	msg.len = 4;
-	msg.buf = buffer;
-	i2c_transfer(client->adapter, &msg, 1);
-
-	/* Select MPX mode if not forced by the user */
-	if (force_mpx_mode >= 0 && force_mpx_mode < MPX_NUM_MODES)
-		t->mpxmode = force_mpx_mode;
-	else
-		t->mpxmode = default_mpx_mode;
-	dev_dbg(&client->dev, "setting MPX to mode %d\n", t->mpxmode);
-	mpx_setup(client);
-
-	return 0;
-}
-
-static int tuner_command(struct i2c_client *client, unsigned int cmd, void *arg)
-{
-	struct wis_sony_tuner *t = i2c_get_clientdata(client);
-
-	switch (cmd) {
-#if 0
-#ifdef TUNER_SET_TYPE_ADDR
-	case TUNER_SET_TYPE_ADDR:
-	{
-		struct tuner_setup *tun_setup = arg;
-		int *type = &tun_setup->type;
-#else
-	case TUNER_SET_TYPE:
-	{
-		int *type = arg;
-#endif
-
-		if (t->type >= 0) {
-			if (t->type != *type)
-				dev_err(&client->dev,
-					"type already set to %d, ignoring request for %d\n",
-					t->type, *type);
-			break;
-		}
-		t->type = *type;
-		switch (t->type) {
-		case TUNER_SONY_BTF_PG472Z:
-			switch (force_band_str[0]) {
-			case 'b':
-			case 'B':
-			case 'g':
-			case 'G':
-				dev_info(&client->dev,
-					 "forcing tuner to PAL-B/G bands\n");
-				force_band = V4L2_STD_PAL_BG;
-				break;
-			case 'i':
-			case 'I':
-				dev_info(&client->dev,
-					 "forcing tuner to PAL-I band\n");
-				force_band = V4L2_STD_PAL_I;
-				break;
-			case 'd':
-			case 'D':
-			case 'k':
-			case 'K':
-				dev_info(&client->dev,
-					 "forcing tuner to PAL-D/K bands\n");
-				force_band = V4L2_STD_PAL_I;
-				break;
-			case 'l':
-			case 'L':
-				dev_info(&client->dev,
-					 "forcing tuner to SECAM-L band\n");
-				force_band = V4L2_STD_SECAM_L;
-				break;
-			default:
-				force_band = 0;
-				break;
-			}
-			if (force_band)
-				t->std = force_band;
-			else
-				t->std = V4L2_STD_PAL_BG;
-			set_if(client);
-			break;
-		case TUNER_SONY_BTF_PK467Z:
-			t->std = V4L2_STD_NTSC_M_JP;
-			break;
-		case TUNER_SONY_BTF_PB463Z:
-			t->std = V4L2_STD_NTSC_M;
-			break;
-		default:
-			dev_err(&client->dev,
-				"tuner type %d is not supported by this module\n",
-				*type);
-			break;
-		}
-		if (type >= 0)
-			dev_info(&clinet->dev,
-				 "type set to %d (%s)\n",
-				 t->type, sony_tuners[t->type - 200].name);
-		break;
-	}
-#endif
-	case VIDIOC_G_FREQUENCY:
-	{
-		struct v4l2_frequency *f = arg;
-
-		f->frequency = t->freq;
-		break;
-	}
-	case VIDIOC_S_FREQUENCY:
-	{
-		struct v4l2_frequency *f = arg;
-
-		t->freq = f->frequency;
-		set_freq(client, t->freq);
-		break;
-	}
-	case VIDIOC_ENUMSTD:
-	{
-		struct v4l2_standard *std = arg;
-
-		switch (t->type) {
-		case TUNER_SONY_BTF_PG472Z:
-			switch (std->index) {
-			case 0:
-				v4l2_video_std_construct(std,
-						V4L2_STD_PAL_BG, "PAL-B/G");
-				break;
-			case 1:
-				v4l2_video_std_construct(std,
-						V4L2_STD_PAL_I, "PAL-I");
-				break;
-			case 2:
-				v4l2_video_std_construct(std,
-						V4L2_STD_PAL_DK, "PAL-D/K");
-				break;
-			case 3:
-				v4l2_video_std_construct(std,
-						V4L2_STD_SECAM_L, "SECAM-L");
-				break;
-			default:
-				std->id = 0; /* hack to indicate EINVAL */
-				break;
-			}
-			break;
-		case TUNER_SONY_BTF_PK467Z:
-			if (std->index != 0) {
-				std->id = 0; /* hack to indicate EINVAL */
-				break;
-			}
-			v4l2_video_std_construct(std,
-					V4L2_STD_NTSC_M_JP, "NTSC-J");
-			break;
-		case TUNER_SONY_BTF_PB463Z:
-			if (std->index != 0) {
-				std->id = 0; /* hack to indicate EINVAL */
-				break;
-			}
-			v4l2_video_std_construct(std, V4L2_STD_NTSC_M, "NTSC");
-			break;
-		}
-		break;
-	}
-	case VIDIOC_G_STD:
-	{
-		v4l2_std_id *std = arg;
-
-		*std = t->std;
-		break;
-	}
-	case VIDIOC_S_STD:
-	{
-		v4l2_std_id *std = arg;
-		v4l2_std_id old = t->std;
-
-		switch (t->type) {
-		case TUNER_SONY_BTF_PG472Z:
-			if (force_band && (*std & force_band) != *std &&
-					*std != V4L2_STD_PAL &&
-					*std != V4L2_STD_SECAM) {
-				dev_dbg(&client->dev,
-					"ignoring requested TV standard in favor of force_band value\n");
-				t->std = force_band;
-			} else if (*std & V4L2_STD_PAL_BG) { /* default */
-				t->std = V4L2_STD_PAL_BG;
-			} else if (*std & V4L2_STD_PAL_I) {
-				t->std = V4L2_STD_PAL_I;
-			} else if (*std & V4L2_STD_PAL_DK) {
-				t->std = V4L2_STD_PAL_DK;
-			} else if (*std & V4L2_STD_SECAM_L) {
-				t->std = V4L2_STD_SECAM_L;
-			} else {
-				dev_err(&client->dev,
-					"TV standard not supported\n");
-				*std = 0; /* hack to indicate EINVAL */
-				break;
-			}
-			if (old != t->std)
-				set_if(client);
-			break;
-		case TUNER_SONY_BTF_PK467Z:
-			if (!(*std & V4L2_STD_NTSC_M_JP)) {
-				dev_err(&client->dev,
-					"TV standard not supported\n");
-				*std = 0; /* hack to indicate EINVAL */
-			}
-			break;
-		case TUNER_SONY_BTF_PB463Z:
-			if (!(*std & V4L2_STD_NTSC_M)) {
-				dev_err(&client->dev,
-					"TV standard not supported\n");
-				*std = 0; /* hack to indicate EINVAL */
-			}
-			break;
-		}
-		break;
-	}
-	case VIDIOC_QUERYSTD:
-	{
-		v4l2_std_id *std = arg;
-
-		switch (t->type) {
-		case TUNER_SONY_BTF_PG472Z:
-			if (force_band)
-				*std = force_band;
-			else
-				*std = V4L2_STD_PAL_BG | V4L2_STD_PAL_I |
-					V4L2_STD_PAL_DK | V4L2_STD_SECAM_L;
-			break;
-		case TUNER_SONY_BTF_PK467Z:
-			*std = V4L2_STD_NTSC_M_JP;
-			break;
-		case TUNER_SONY_BTF_PB463Z:
-			*std = V4L2_STD_NTSC_M;
-			break;
-		}
-		break;
-	}
-	case VIDIOC_G_TUNER:
-	{
-		struct v4l2_tuner *tun = arg;
-
-		memset(tun, 0, sizeof(*tun));
-		strcpy(tun->name, "Television");
-		tun->type = V4L2_TUNER_ANALOG_TV;
-		tun->rangelow = 0UL; /* does anything use these? */
-		tun->rangehigh = 0xffffffffUL;
-		switch (t->type) {
-		case TUNER_SONY_BTF_PG472Z:
-			tun->capability = V4L2_TUNER_CAP_NORM |
-				V4L2_TUNER_CAP_STEREO | V4L2_TUNER_CAP_LANG1 |
-				V4L2_TUNER_CAP_LANG2;
-			tun->rxsubchans = V4L2_TUNER_SUB_MONO |
-				V4L2_TUNER_SUB_STEREO | V4L2_TUNER_SUB_LANG1 |
-				V4L2_TUNER_SUB_LANG2;
-			break;
-		case TUNER_SONY_BTF_PK467Z:
-		case TUNER_SONY_BTF_PB463Z:
-			tun->capability = V4L2_TUNER_CAP_STEREO;
-			tun->rxsubchans = V4L2_TUNER_SUB_MONO |
-						V4L2_TUNER_SUB_STEREO;
-			break;
-		}
-		tun->audmode = t->audmode;
-		return 0;
-	}
-	case VIDIOC_S_TUNER:
-	{
-		struct v4l2_tuner *tun = arg;
-
-		switch (t->type) {
-		case TUNER_SONY_BTF_PG472Z:
-			if (tun->audmode != t->audmode) {
-				t->audmode = tun->audmode;
-				mpx_setup(client);
-			}
-			break;
-		case TUNER_SONY_BTF_PK467Z:
-		case TUNER_SONY_BTF_PB463Z:
-			break;
-		}
-		return 0;
-	}
-	default:
-		break;
-	}
-	return 0;
-}
-
-static int wis_sony_tuner_probe(struct i2c_client *client,
-				const struct i2c_device_id *id)
-{
-	struct i2c_adapter *adapter = client->adapter;
-	struct wis_sony_tuner *t;
-
-	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_I2C_BLOCK))
-		return -ENODEV;
-
-	t = kmalloc(sizeof(struct wis_sony_tuner), GFP_KERNEL);
-	if (t == NULL)
-		return -ENOMEM;
-
-	t->type = -1;
-	t->freq = 0;
-	t->mpxmode = 0;
-	t->audmode = V4L2_TUNER_MODE_STEREO;
-	i2c_set_clientdata(client, t);
-
-	dev_dbg(&client->dev, "initializing tuner at address %d on %s\n",
-		client->addr, adapter->name);
-
-	return 0;
-}
-
-static int wis_sony_tuner_remove(struct i2c_client *client)
-{
-	struct wis_sony_tuner *t = i2c_get_clientdata(client);
-
-	kfree(t);
-	return 0;
-}
-
-static const struct i2c_device_id wis_sony_tuner_id[] = {
-	{ "wis_sony_tuner", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, wis_sony_tuner_id);
-
-static struct i2c_driver wis_sony_tuner_driver = {
-	.driver = {
-		.name	= "WIS Sony TV Tuner I2C driver",
-	},
-	.probe		= wis_sony_tuner_probe,
-	.remove		= wis_sony_tuner_remove,
-	.command	= tuner_command,
-	.id_table	= wis_sony_tuner_id,
-};
-
-module_i2c_driver(wis_sony_tuner_driver);
-
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/media/go7007/wis-tw2804.c b/drivers/staging/media/go7007/wis-tw2804.c
deleted file mode 100644
index 290fd8c7bfe..00000000000
--- a/drivers/staging/media/go7007/wis-tw2804.c
+++ /dev/null
@@ -1,348 +0,0 @@
-/*
- * Copyright (C) 2005-2006 Micronas USA 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.
- */
-
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/i2c.h>
-#include <linux/videodev2.h>
-#include <linux/ioctl.h>
-#include <linux/slab.h>
-
-#include "wis-i2c.h"
-
-struct wis_tw2804 {
-	int channel;
-	int norm;
-	int brightness;
-	int contrast;
-	int saturation;
-	int hue;
-};
-
-static u8 global_registers[] = {
-	0x39, 0x00,
-	0x3a, 0xff,
-	0x3b, 0x84,
-	0x3c, 0x80,
-	0x3d, 0x80,
-	0x3e, 0x82,
-	0x3f, 0x82,
-	0xff, 0xff, /* Terminator (reg 0xff does not exist) */
-};
-
-static u8 channel_registers[] = {
-	0x01, 0xc4,
-	0x02, 0xa5,
-	0x03, 0x20,
-	0x04, 0xd0,
-	0x05, 0x20,
-	0x06, 0xd0,
-	0x07, 0x88,
-	0x08, 0x20,
-	0x09, 0x07,
-	0x0a, 0xf0,
-	0x0b, 0x07,
-	0x0c, 0xf0,
-	0x0d, 0x40,
-	0x0e, 0xd2,
-	0x0f, 0x80,
-	0x10, 0x80,
-	0x11, 0x80,
-	0x12, 0x80,
-	0x13, 0x1f,
-	0x14, 0x00,
-	0x15, 0x00,
-	0x16, 0x00,
-	0x17, 0x00,
-	0x18, 0xff,
-	0x19, 0xff,
-	0x1a, 0xff,
-	0x1b, 0xff,
-	0x1c, 0xff,
-	0x1d, 0xff,
-	0x1e, 0xff,
-	0x1f, 0xff,
-	0x20, 0x07,
-	0x21, 0x07,
-	0x22, 0x00,
-	0x23, 0x91,
-	0x24, 0x51,
-	0x25, 0x03,
-	0x26, 0x00,
-	0x27, 0x00,
-	0x28, 0x00,
-	0x29, 0x00,
-	0x2a, 0x00,
-	0x2b, 0x00,
-	0x2c, 0x00,
-	0x2d, 0x00,
-	0x2e, 0x00,
-	0x2f, 0x00,
-	0x30, 0x00,
-	0x31, 0x00,
-	0x32, 0x00,
-	0x33, 0x00,
-	0x34, 0x00,
-	0x35, 0x00,
-	0x36, 0x00,
-	0x37, 0x00,
-	0xff, 0xff, /* Terminator (reg 0xff does not exist) */
-};
-
-static int write_reg(struct i2c_client *client, u8 reg, u8 value, int channel)
-{
-	return i2c_smbus_write_byte_data(client, reg | (channel << 6), value);
-}
-
-static int write_regs(struct i2c_client *client, u8 *regs, int channel)
-{
-	int i;
-
-	for (i = 0; regs[i] != 0xff; i += 2)
-		if (i2c_smbus_write_byte_data(client,
-				regs[i] | (channel << 6), regs[i + 1]) < 0)
-			return -1;
-	return 0;
-}
-
-static int wis_tw2804_command(struct i2c_client *client,
-				unsigned int cmd, void *arg)
-{
-	struct wis_tw2804 *dec = i2c_get_clientdata(client);
-
-	if (cmd == DECODER_SET_CHANNEL) {
-		int *input = arg;
-
-		if (*input < 0 || *input > 3) {
-			dev_err(&client->dev,
-				"channel %d is not between 0 and 3!\n", *input);
-			return 0;
-		}
-		dec->channel = *input;
-		dev_dbg(&client->dev, "initializing TW2804 channel %d\n",
-			dec->channel);
-		if (dec->channel == 0 &&
-				write_regs(client, global_registers, 0) < 0) {
-			dev_err(&client->dev,
-				"error initializing TW2804 global registers\n");
-			return 0;
-		}
-		if (write_regs(client, channel_registers, dec->channel) < 0) {
-			dev_err(&client->dev,
-				"error initializing TW2804 channel %d\n",
-				dec->channel);
-			return 0;
-		}
-		return 0;
-	}
-
-	if (dec->channel < 0) {
-		dev_dbg(&client->dev,
-			"ignoring command %08x until channel number is set\n",
-			cmd);
-		return 0;
-	}
-
-	switch (cmd) {
-	case VIDIOC_S_STD:
-	{
-		v4l2_std_id *input = arg;
-		u8 regs[] = {
-			0x01, *input & V4L2_STD_NTSC ? 0xc4 : 0x84,
-			0x09, *input & V4L2_STD_NTSC ? 0x07 : 0x04,
-			0x0a, *input & V4L2_STD_NTSC ? 0xf0 : 0x20,
-			0x0b, *input & V4L2_STD_NTSC ? 0x07 : 0x04,
-			0x0c, *input & V4L2_STD_NTSC ? 0xf0 : 0x20,
-			0x0d, *input & V4L2_STD_NTSC ? 0x40 : 0x4a,
-			0x16, *input & V4L2_STD_NTSC ? 0x00 : 0x40,
-			0x17, *input & V4L2_STD_NTSC ? 0x00 : 0x40,
-			0x20, *input & V4L2_STD_NTSC ? 0x07 : 0x0f,
-			0x21, *input & V4L2_STD_NTSC ? 0x07 : 0x0f,
-			0xff,	0xff,
-		};
-		write_regs(client, regs, dec->channel);
-		dec->norm = *input;
-		break;
-	}
-	case VIDIOC_QUERYCTRL:
-	{
-		struct v4l2_queryctrl *ctrl = arg;
-
-		switch (ctrl->id) {
-		case V4L2_CID_BRIGHTNESS:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Brightness", sizeof(ctrl->name));
-			ctrl->minimum = 0;
-			ctrl->maximum = 255;
-			ctrl->step = 1;
-			ctrl->default_value = 128;
-			ctrl->flags = 0;
-			break;
-		case V4L2_CID_CONTRAST:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Contrast", sizeof(ctrl->name));
-			ctrl->minimum = 0;
-			ctrl->maximum = 255;
-			ctrl->step = 1;
-			ctrl->default_value = 128;
-			ctrl->flags = 0;
-			break;
-		case V4L2_CID_SATURATION:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Saturation", sizeof(ctrl->name));
-			ctrl->minimum = 0;
-			ctrl->maximum = 255;
-			ctrl->step = 1;
-			ctrl->default_value = 128;
-			ctrl->flags = 0;
-			break;
-		case V4L2_CID_HUE:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Hue", sizeof(ctrl->name));
-			ctrl->minimum = 0;
-			ctrl->maximum = 255;
-			ctrl->step = 1;
-			ctrl->default_value = 128;
-			ctrl->flags = 0;
-			break;
-		}
-		break;
-	}
-	case VIDIOC_S_CTRL:
-	{
-		struct v4l2_control *ctrl = arg;
-
-		switch (ctrl->id) {
-		case V4L2_CID_BRIGHTNESS:
-			if (ctrl->value > 255)
-				dec->brightness = 255;
-			else if (ctrl->value < 0)
-				dec->brightness = 0;
-			else
-				dec->brightness = ctrl->value;
-			write_reg(client, 0x12, dec->brightness, dec->channel);
-			break;
-		case V4L2_CID_CONTRAST:
-			if (ctrl->value > 255)
-				dec->contrast = 255;
-			else if (ctrl->value < 0)
-				dec->contrast = 0;
-			else
-				dec->contrast = ctrl->value;
-			write_reg(client, 0x11, dec->contrast, dec->channel);
-			break;
-		case V4L2_CID_SATURATION:
-			if (ctrl->value > 255)
-				dec->saturation = 255;
-			else if (ctrl->value < 0)
-				dec->saturation = 0;
-			else
-				dec->saturation = ctrl->value;
-			write_reg(client, 0x10, dec->saturation, dec->channel);
-			break;
-		case V4L2_CID_HUE:
-			if (ctrl->value > 255)
-				dec->hue = 255;
-			else if (ctrl->value < 0)
-				dec->hue = 0;
-			else
-				dec->hue = ctrl->value;
-			write_reg(client, 0x0f, dec->hue, dec->channel);
-			break;
-		}
-		break;
-	}
-	case VIDIOC_G_CTRL:
-	{
-		struct v4l2_control *ctrl = arg;
-
-		switch (ctrl->id) {
-		case V4L2_CID_BRIGHTNESS:
-			ctrl->value = dec->brightness;
-			break;
-		case V4L2_CID_CONTRAST:
-			ctrl->value = dec->contrast;
-			break;
-		case V4L2_CID_SATURATION:
-			ctrl->value = dec->saturation;
-			break;
-		case V4L2_CID_HUE:
-			ctrl->value = dec->hue;
-			break;
-		}
-		break;
-	}
-	default:
-		break;
-	}
-	return 0;
-}
-
-static int wis_tw2804_probe(struct i2c_client *client,
-			    const struct i2c_device_id *id)
-{
-	struct i2c_adapter *adapter = client->adapter;
-	struct wis_tw2804 *dec;
-
-	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
-		return -ENODEV;
-
-	dec = kmalloc(sizeof(struct wis_tw2804), GFP_KERNEL);
-	if (dec == NULL)
-		return -ENOMEM;
-
-	dec->channel = -1;
-	dec->norm = V4L2_STD_NTSC;
-	dec->brightness = 128;
-	dec->contrast = 128;
-	dec->saturation = 128;
-	dec->hue = 128;
-	i2c_set_clientdata(client, dec);
-
-	dev_dbg(&client->dev, "creating TW2804 at address %d on %s\n",
-		client->addr, adapter->name);
-
-	return 0;
-}
-
-static int wis_tw2804_remove(struct i2c_client *client)
-{
-	struct wis_tw2804 *dec = i2c_get_clientdata(client);
-
-	kfree(dec);
-	return 0;
-}
-
-static const struct i2c_device_id wis_tw2804_id[] = {
-	{ "wis_tw2804", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, wis_tw2804_id);
-
-static struct i2c_driver wis_tw2804_driver = {
-	.driver = {
-		.name	= "WIS TW2804 I2C driver",
-	},
-	.probe		= wis_tw2804_probe,
-	.remove		= wis_tw2804_remove,
-	.command	= wis_tw2804_command,
-	.id_table	= wis_tw2804_id,
-};
-
-module_i2c_driver(wis_tw2804_driver);
-
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/media/go7007/wis-tw9903.c b/drivers/staging/media/go7007/wis-tw9903.c
deleted file mode 100644
index 684ca37f038..00000000000
--- a/drivers/staging/media/go7007/wis-tw9903.c
+++ /dev/null
@@ -1,328 +0,0 @@
-/*
- * Copyright (C) 2005-2006 Micronas USA 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.
- */
-
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/i2c.h>
-#include <linux/videodev2.h>
-#include <linux/ioctl.h>
-#include <linux/slab.h>
-
-#include "wis-i2c.h"
-
-struct wis_tw9903 {
-	int norm;
-	int brightness;
-	int contrast;
-	int hue;
-};
-
-static u8 initial_registers[] = {
-	0x02, 0x44, /* input 1, composite */
-	0x03, 0x92, /* correct digital format */
-	0x04, 0x00,
-	0x05, 0x80, /* or 0x00 for PAL */
-	0x06, 0x40, /* second internal current reference */
-	0x07, 0x02, /* window */
-	0x08, 0x14, /* window */
-	0x09, 0xf0, /* window */
-	0x0a, 0x81, /* window */
-	0x0b, 0xd0, /* window */
-	0x0c, 0x8c,
-	0x0d, 0x00, /* scaling */
-	0x0e, 0x11, /* scaling */
-	0x0f, 0x00, /* scaling */
-	0x10, 0x00, /* brightness */
-	0x11, 0x60, /* contrast */
-	0x12, 0x01, /* sharpness */
-	0x13, 0x7f, /* U gain */
-	0x14, 0x5a, /* V gain */
-	0x15, 0x00, /* hue */
-	0x16, 0xc3, /* sharpness */
-	0x18, 0x00,
-	0x19, 0x58, /* vbi */
-	0x1a, 0x80,
-	0x1c, 0x0f, /* video norm */
-	0x1d, 0x7f, /* video norm */
-	0x20, 0xa0, /* clamping gain (working 0x50) */
-	0x21, 0x22,
-	0x22, 0xf0,
-	0x23, 0xfe,
-	0x24, 0x3c,
-	0x25, 0x38,
-	0x26, 0x44,
-	0x27, 0x20,
-	0x28, 0x00,
-	0x29, 0x15,
-	0x2a, 0xa0,
-	0x2b, 0x44,
-	0x2c, 0x37,
-	0x2d, 0x00,
-	0x2e, 0xa5, /* burst PLL control (working: a9) */
-	0x2f, 0xe0, /* 0xea is blue test frame -- 0xe0 for normal */
-	0x31, 0x00,
-	0x33, 0x22,
-	0x34, 0x11,
-	0x35, 0x35,
-	0x3b, 0x05,
-	0x06, 0xc0, /* reset device */
-	0x00, 0x00, /* Terminator (reg 0x00 is read-only) */
-};
-
-static int write_reg(struct i2c_client *client, u8 reg, u8 value)
-{
-	return i2c_smbus_write_byte_data(client, reg, value);
-}
-
-static int write_regs(struct i2c_client *client, u8 *regs)
-{
-	int i;
-
-	for (i = 0; regs[i] != 0x00; i += 2)
-		if (i2c_smbus_write_byte_data(client, regs[i], regs[i + 1]) < 0)
-			return -1;
-	return 0;
-}
-
-static int wis_tw9903_command(struct i2c_client *client,
-				unsigned int cmd, void *arg)
-{
-	struct wis_tw9903 *dec = i2c_get_clientdata(client);
-
-	switch (cmd) {
-	case VIDIOC_S_INPUT:
-	{
-		int *input = arg;
-
-		i2c_smbus_write_byte_data(client, 0x02, 0x40 | (*input << 1));
-		break;
-	}
-#if 0
-	/* The scaler on this thing seems to be horribly broken */
-	case DECODER_SET_RESOLUTION:
-	{
-		struct video_decoder_resolution *res = arg;
-		/*int hscale = 256 * 720 / res->width;*/
-		int hscale = 256 * 720 / (res->width - (res->width > 704 ? 0 : 8));
-		int vscale = 256 * (dec->norm & V4L2_STD_NTSC ?  240 : 288)
-				/ res->height;
-		u8 regs[] = {
-			0x0d, vscale & 0xff,
-			0x0f, hscale & 0xff,
-			0x0e, ((vscale & 0xf00) >> 4) | ((hscale & 0xf00) >> 8),
-			0x06, 0xc0, /* reset device */
-			0,	0,
-		};
-		dev_dbg(&client->dev, "vscale is %04x, hscale is %04x\n",
-			vscale, hscale);
-		/*write_regs(client, regs);*/
-		break;
-	}
-#endif
-	case VIDIOC_S_STD:
-	{
-		v4l2_std_id *input = arg;
-		u8 regs[] = {
-			0x05, *input & V4L2_STD_NTSC ? 0x80 : 0x00,
-			0x07, *input & V4L2_STD_NTSC ? 0x02 : 0x12,
-			0x08, *input & V4L2_STD_NTSC ? 0x14 : 0x18,
-			0x09, *input & V4L2_STD_NTSC ? 0xf0 : 0x20,
-			0,	0,
-		};
-		write_regs(client, regs);
-		dec->norm = *input;
-		break;
-	}
-	case VIDIOC_QUERYCTRL:
-	{
-		struct v4l2_queryctrl *ctrl = arg;
-
-		switch (ctrl->id) {
-		case V4L2_CID_BRIGHTNESS:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Brightness", sizeof(ctrl->name));
-			ctrl->minimum = -128;
-			ctrl->maximum = 127;
-			ctrl->step = 1;
-			ctrl->default_value = 0x00;
-			ctrl->flags = 0;
-			break;
-		case V4L2_CID_CONTRAST:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Contrast", sizeof(ctrl->name));
-			ctrl->minimum = 0;
-			ctrl->maximum = 255;
-			ctrl->step = 1;
-			ctrl->default_value = 0x60;
-			ctrl->flags = 0;
-			break;
-#if 0
-		/* I don't understand how the Chroma Gain registers work... */
-		case V4L2_CID_SATURATION:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Saturation", sizeof(ctrl->name));
-			ctrl->minimum = 0;
-			ctrl->maximum = 127;
-			ctrl->step = 1;
-			ctrl->default_value = 64;
-			ctrl->flags = 0;
-			break;
-#endif
-		case V4L2_CID_HUE:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Hue", sizeof(ctrl->name));
-			ctrl->minimum = -128;
-			ctrl->maximum = 127;
-			ctrl->step = 1;
-			ctrl->default_value = 0;
-			ctrl->flags = 0;
-			break;
-		}
-		break;
-	}
-	case VIDIOC_S_CTRL:
-	{
-		struct v4l2_control *ctrl = arg;
-
-		switch (ctrl->id) {
-		case V4L2_CID_BRIGHTNESS:
-			if (ctrl->value > 127)
-				dec->brightness = 127;
-			else if (ctrl->value < -128)
-				dec->brightness = -128;
-			else
-				dec->brightness = ctrl->value;
-			write_reg(client, 0x10, dec->brightness);
-			break;
-		case V4L2_CID_CONTRAST:
-			if (ctrl->value > 255)
-				dec->contrast = 255;
-			else if (ctrl->value < 0)
-				dec->contrast = 0;
-			else
-				dec->contrast = ctrl->value;
-			write_reg(client, 0x11, dec->contrast);
-			break;
-#if 0
-		case V4L2_CID_SATURATION:
-			if (ctrl->value > 127)
-				dec->saturation = 127;
-			else if (ctrl->value < 0)
-				dec->saturation = 0;
-			else
-				dec->saturation = ctrl->value;
-			/*write_reg(client, 0x0c, dec->saturation);*/
-			break;
-#endif
-		case V4L2_CID_HUE:
-			if (ctrl->value > 127)
-				dec->hue = 127;
-			else if (ctrl->value < -128)
-				dec->hue = -128;
-			else
-				dec->hue = ctrl->value;
-			write_reg(client, 0x15, dec->hue);
-			break;
-		}
-		break;
-	}
-	case VIDIOC_G_CTRL:
-	{
-		struct v4l2_control *ctrl = arg;
-
-		switch (ctrl->id) {
-		case V4L2_CID_BRIGHTNESS:
-			ctrl->value = dec->brightness;
-			break;
-		case V4L2_CID_CONTRAST:
-			ctrl->value = dec->contrast;
-			break;
-#if 0
-		case V4L2_CID_SATURATION:
-			ctrl->value = dec->saturation;
-			break;
-#endif
-		case V4L2_CID_HUE:
-			ctrl->value = dec->hue;
-			break;
-		}
-		break;
-	}
-	default:
-		break;
-	}
-	return 0;
-}
-
-static int wis_tw9903_probe(struct i2c_client *client,
-			    const struct i2c_device_id *id)
-{
-	struct i2c_adapter *adapter = client->adapter;
-	struct wis_tw9903 *dec;
-
-	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
-		return -ENODEV;
-
-	dec = kmalloc(sizeof(struct wis_tw9903), GFP_KERNEL);
-	if (dec == NULL)
-		return -ENOMEM;
-
-	dec->norm = V4L2_STD_NTSC;
-	dec->brightness = 0;
-	dec->contrast = 0x60;
-	dec->hue = 0;
-	i2c_set_clientdata(client, dec);
-
-	dev_dbg(&client->dev, "initializing TW9903 at address %d on %s\n",
-		client->addr, adapter->name);
-
-	if (write_regs(client, initial_registers) < 0) {
-		dev_err(&client->dev, "error initializing TW9903\n");
-		kfree(dec);
-		return -ENODEV;
-	}
-
-	return 0;
-}
-
-static int wis_tw9903_remove(struct i2c_client *client)
-{
-	struct wis_tw9903 *dec = i2c_get_clientdata(client);
-
-	kfree(dec);
-	return 0;
-}
-
-static const struct i2c_device_id wis_tw9903_id[] = {
-	{ "wis_tw9903", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, wis_tw9903_id);
-
-static struct i2c_driver wis_tw9903_driver = {
-	.driver = {
-		.name	= "WIS TW9903 I2C driver",
-	},
-	.probe		= wis_tw9903_probe,
-	.remove		= wis_tw9903_remove,
-	.command	= wis_tw9903_command,
-	.id_table	= wis_tw9903_id,
-};
-
-module_i2c_driver(wis_tw9903_driver);
-
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/media/lirc/lirc_sir.c b/drivers/staging/media/lirc/lirc_sir.c
index 63a554c36f7..f781c532b3c 100644
--- a/drivers/staging/media/lirc/lirc_sir.c
+++ b/drivers/staging/media/lirc/lirc_sir.c
@@ -787,12 +787,6 @@ static int init_hardware(void)
 	spin_lock_irqsave(&hardware_lock, flags);
 	/* reset UART */
 #ifdef LIRC_ON_SA1100
-#ifdef CONFIG_SA1100_BITSY
-	if (machine_is_bitsy()) {
-		pr_info("Power on IR module\n");
-		set_bitsy_egpio(EGPIO_BITSY_IR_ON);
-	}
-#endif
 #ifdef CONFIG_SA1100_COLLIE
 	sa1100_irda_set_power_collie(3);	/* power on */
 #endif
@@ -942,10 +936,6 @@ static void drop_hardware(void)
 	Ser2UTCR3 = sr.utcr3;
 
 	Ser2HSCR0 = sr.hscr0;
-#ifdef CONFIG_SA1100_BITSY
-	if (machine_is_bitsy())
-		clr_bitsy_egpio(EGPIO_BITSY_IR_ON);
-#endif
 #ifdef CONFIG_SA1100_COLLIE
 	sa1100_irda_set_power_collie(0);	/* power off */
 #endif
diff --git a/drivers/staging/media/solo6x10/Kconfig b/drivers/staging/media/solo6x10/Kconfig
index 63352de5eab..ec32776ff54 100644
--- a/drivers/staging/media/solo6x10/Kconfig
+++ b/drivers/staging/media/solo6x10/Kconfig
@@ -1,7 +1,8 @@
 config SOLO6X10
 	tristate "Softlogic 6x10 MPEG codec cards"
 	depends on PCI && VIDEO_DEV && SND && I2C
-	select VIDEOBUF_DMA_SG
+	select VIDEOBUF2_DMA_SG
+	select VIDEOBUF2_DMA_CONTIG
 	select SND_PCM
 	---help---
 	  This driver supports the Softlogic based MPEG-4 and h.264 codec
diff --git a/drivers/staging/media/solo6x10/Makefile b/drivers/staging/media/solo6x10/Makefile
index 337e38c3a0f..7aae118947b 100644
--- a/drivers/staging/media/solo6x10/Makefile
+++ b/drivers/staging/media/solo6x10/Makefile
@@ -1,3 +1,5 @@
-solo6x10-y := core.o i2c.o p2m.o v4l2.o tw28.o gpio.o disp.o enc.o v4l2-enc.o g723.o
+solo6x10-y := solo6x10-core.o solo6x10-i2c.o solo6x10-p2m.o solo6x10-v4l2.o \
+		solo6x10-tw28.o solo6x10-gpio.o solo6x10-disp.o solo6x10-enc.o \
+		solo6x10-v4l2-enc.o solo6x10-g723.o solo6x10-eeprom.o
 
 obj-$(CONFIG_SOLO6X10) += solo6x10.o
diff --git a/drivers/staging/media/solo6x10/TODO b/drivers/staging/media/solo6x10/TODO
index 539f739fe9e..7b8db75b1ac 100644
--- a/drivers/staging/media/solo6x10/TODO
+++ b/drivers/staging/media/solo6x10/TODO
@@ -1,24 +1,15 @@
-TODO (staging => main):
-
-	* Motion detection flags need to be moved to v4l2
-	* Some private CIDs need to be moved to v4l2
-
-TODO (general):
-
-	* encoder on/off controls
-	* mpeg cid bitrate mode (vbr/cbr)
-	* mpeg cid bitrate/bitrate-peak
-	* mpeg encode of user data
-	* mpeg decode of user data
-	* switch between 4 frames/irq to 1 when using mjpeg (and then back
-	  when not)
-	* implement a CID control for motion areas/thresholds
-	* implement CID controls for mozaic areas
-	* allow for higher level of interval (for < 1 fps)
-	* sound:
-	  - implement playback via external sound jack
-	  - implement loopback of external sound jack with incoming audio?
-	  - implement pause/resume
-
-Plase send patches to Mauro Carvalho Chehab <mchehab@redhat.com> and Cc Ben Collins
-<bcollins@bluecherry.net>
+- batch up desc requests for more efficient use of p2m?
+- encoder on/off controls
+- mpeg cid bitrate mode (vbr/cbr)
+- mpeg cid bitrate/bitrate-peak
+- mpeg encode of user data
+- mpeg decode of user data
+- implement CID controls for mozaic areas
+
+- sound
+ - implement playback via external sound jack
+ - implement loopback of external sound jack with incoming audio?
+ - implement pause/resume (make use of in bc-server)
+
+Please send patches to the linux media list <linux-media@vger.kernel.org> and
+Cc Ismael Luceno <ismael.luceno@corp.bluecherry.net>.
diff --git a/drivers/staging/media/solo6x10/core.c b/drivers/staging/media/solo6x10/core.c
deleted file mode 100644
index fd83d6d028b..00000000000
--- a/drivers/staging/media/solo6x10/core.c
+++ /dev/null
@@ -1,321 +0,0 @@
-/*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.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., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- */
-
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/pci.h>
-#include <linux/interrupt.h>
-#include <linux/slab.h>
-#include <linux/videodev2.h>
-#include "solo6x10.h"
-#include "tw28.h"
-
-MODULE_DESCRIPTION("Softlogic 6x10 MP4/H.264 Encoder/Decoder V4L2/ALSA Driver");
-MODULE_AUTHOR("Ben Collins <bcollins@bluecherry.net>");
-MODULE_VERSION(SOLO6X10_VERSION);
-MODULE_LICENSE("GPL");
-
-void solo_irq_on(struct solo_dev *solo_dev, u32 mask)
-{
-	solo_dev->irq_mask |= mask;
-	solo_reg_write(solo_dev, SOLO_IRQ_ENABLE, solo_dev->irq_mask);
-}
-
-void solo_irq_off(struct solo_dev *solo_dev, u32 mask)
-{
-	solo_dev->irq_mask &= ~mask;
-	solo_reg_write(solo_dev, SOLO_IRQ_ENABLE, solo_dev->irq_mask);
-}
-
-/* XXX We should check the return value of the sub-device ISR's */
-static irqreturn_t solo_isr(int irq, void *data)
-{
-	struct solo_dev *solo_dev = data;
-	u32 status;
-	int i;
-
-	status = solo_reg_read(solo_dev, SOLO_IRQ_STAT);
-	if (!status)
-		return IRQ_NONE;
-
-	if (status & ~solo_dev->irq_mask) {
-		solo_reg_write(solo_dev, SOLO_IRQ_STAT,
-			       status & ~solo_dev->irq_mask);
-		status &= solo_dev->irq_mask;
-	}
-
-	if (status & SOLO_IRQ_PCI_ERR) {
-		u32 err = solo_reg_read(solo_dev, SOLO_PCI_ERR);
-		solo_p2m_error_isr(solo_dev, err);
-		solo_reg_write(solo_dev, SOLO_IRQ_STAT, SOLO_IRQ_PCI_ERR);
-	}
-
-	for (i = 0; i < SOLO_NR_P2M; i++)
-		if (status & SOLO_IRQ_P2M(i))
-			solo_p2m_isr(solo_dev, i);
-
-	if (status & SOLO_IRQ_IIC)
-		solo_i2c_isr(solo_dev);
-
-	if (status & SOLO_IRQ_VIDEO_IN)
-		solo_video_in_isr(solo_dev);
-
-	/* Call this first so enc gets detected flag set */
-	if (status & SOLO_IRQ_MOTION)
-		solo_motion_isr(solo_dev);
-
-	if (status & SOLO_IRQ_ENCODER)
-		solo_enc_v4l2_isr(solo_dev);
-
-	if (status & SOLO_IRQ_G723)
-		solo_g723_isr(solo_dev);
-
-	return IRQ_HANDLED;
-}
-
-static void free_solo_dev(struct solo_dev *solo_dev)
-{
-	struct pci_dev *pdev;
-
-	if (!solo_dev)
-		return;
-
-	pdev = solo_dev->pdev;
-
-	/* If we never initialized the PCI device, then nothing else
-	 * below here needs cleanup */
-	if (!pdev) {
-		kfree(solo_dev);
-		return;
-	}
-
-	/* Bring down the sub-devices first */
-	solo_g723_exit(solo_dev);
-	solo_enc_v4l2_exit(solo_dev);
-	solo_enc_exit(solo_dev);
-	solo_v4l2_exit(solo_dev);
-	solo_disp_exit(solo_dev);
-	solo_gpio_exit(solo_dev);
-	solo_p2m_exit(solo_dev);
-	solo_i2c_exit(solo_dev);
-
-	/* Now cleanup the PCI device */
-	if (solo_dev->reg_base) {
-		solo_irq_off(solo_dev, ~0);
-		pci_iounmap(pdev, solo_dev->reg_base);
-		free_irq(pdev->irq, solo_dev);
-	}
-
-	pci_release_regions(pdev);
-	pci_disable_device(pdev);
-	pci_set_drvdata(pdev, NULL);
-
-	kfree(solo_dev);
-}
-
-static int solo_pci_probe(struct pci_dev *pdev,
-				    const struct pci_device_id *id)
-{
-	struct solo_dev *solo_dev;
-	int ret;
-	int sdram;
-	u8 chip_id;
-	u32 reg;
-
-	solo_dev = kzalloc(sizeof(*solo_dev), GFP_KERNEL);
-	if (solo_dev == NULL)
-		return -ENOMEM;
-
-	solo_dev->pdev = pdev;
-	spin_lock_init(&solo_dev->reg_io_lock);
-	pci_set_drvdata(pdev, solo_dev);
-
-	ret = pci_enable_device(pdev);
-	if (ret)
-		goto fail_probe;
-
-	pci_set_master(pdev);
-
-	ret = pci_request_regions(pdev, SOLO6X10_NAME);
-	if (ret)
-		goto fail_probe;
-
-	solo_dev->reg_base = pci_ioremap_bar(pdev, 0);
-	if (solo_dev->reg_base == NULL) {
-		ret = -ENOMEM;
-		goto fail_probe;
-	}
-
-	chip_id = solo_reg_read(solo_dev, SOLO_CHIP_OPTION) &
-					SOLO_CHIP_ID_MASK;
-	switch (chip_id) {
-	case 7:
-		solo_dev->nr_chans = 16;
-		solo_dev->nr_ext = 5;
-		break;
-	case 6:
-		solo_dev->nr_chans = 8;
-		solo_dev->nr_ext = 2;
-		break;
-	default:
-		dev_warn(&pdev->dev, "Invalid chip_id 0x%02x, "
-			 "defaulting to 4 channels\n",
-			 chip_id);
-	case 5:
-		solo_dev->nr_chans = 4;
-		solo_dev->nr_ext = 1;
-	}
-
-	solo_dev->flags = id->driver_data;
-
-	/* Disable all interrupts to start */
-	solo_irq_off(solo_dev, ~0);
-
-	reg = SOLO_SYS_CFG_SDRAM64BIT;
-	/* Initial global settings */
-	if (!(solo_dev->flags & FLAGS_6110))
-		reg |= SOLO6010_SYS_CFG_INPUTDIV(25) |
-			SOLO6010_SYS_CFG_FEEDBACKDIV((SOLO_CLOCK_MHZ * 2) - 2) |
-			SOLO6010_SYS_CFG_OUTDIV(3);
-	solo_reg_write(solo_dev, SOLO_SYS_CFG, reg);
-
-	if (solo_dev->flags & FLAGS_6110) {
-		u32 sys_clock_MHz = SOLO_CLOCK_MHZ;
-		u32 pll_DIVQ;
-		u32 pll_DIVF;
-
-		if (sys_clock_MHz < 125) {
-			pll_DIVQ = 3;
-			pll_DIVF = (sys_clock_MHz * 4) / 3;
-		} else {
-			pll_DIVQ = 2;
-			pll_DIVF = (sys_clock_MHz * 2) / 3;
-		}
-
-		solo_reg_write(solo_dev, SOLO6110_PLL_CONFIG,
-			       SOLO6110_PLL_RANGE_5_10MHZ |
-			       SOLO6110_PLL_DIVR(9) |
-			       SOLO6110_PLL_DIVQ_EXP(pll_DIVQ) |
-			       SOLO6110_PLL_DIVF(pll_DIVF) | SOLO6110_PLL_FSEN);
-		mdelay(1);      /* PLL Locking time (1ms) */
-
-		solo_reg_write(solo_dev, SOLO_DMA_CTRL1, 3 << 8); /* ? */
-	} else
-		solo_reg_write(solo_dev, SOLO_DMA_CTRL1, 1 << 8); /* ? */
-
-	solo_reg_write(solo_dev, SOLO_TIMER_CLOCK_NUM, SOLO_CLOCK_MHZ - 1);
-
-	/* PLL locking time of 1ms */
-	mdelay(1);
-
-	ret = request_irq(pdev->irq, solo_isr, IRQF_SHARED, SOLO6X10_NAME,
-			  solo_dev);
-	if (ret)
-		goto fail_probe;
-
-	/* Handle this from the start */
-	solo_irq_on(solo_dev, SOLO_IRQ_PCI_ERR);
-
-	ret = solo_i2c_init(solo_dev);
-	if (ret)
-		goto fail_probe;
-
-	/* Setup the DMA engine */
-	sdram = (solo_dev->nr_chans >= 8) ? 2 : 1;
-	solo_reg_write(solo_dev, SOLO_DMA_CTRL,
-		       SOLO_DMA_CTRL_REFRESH_CYCLE(1) |
-		       SOLO_DMA_CTRL_SDRAM_SIZE(sdram) |
-		       SOLO_DMA_CTRL_SDRAM_CLK_INVERT |
-		       SOLO_DMA_CTRL_READ_CLK_SELECT |
-		       SOLO_DMA_CTRL_LATENCY(1));
-
-	ret = solo_p2m_init(solo_dev);
-	if (ret)
-		goto fail_probe;
-
-	ret = solo_disp_init(solo_dev);
-	if (ret)
-		goto fail_probe;
-
-	ret = solo_gpio_init(solo_dev);
-	if (ret)
-		goto fail_probe;
-
-	ret = solo_tw28_init(solo_dev);
-	if (ret)
-		goto fail_probe;
-
-	ret = solo_v4l2_init(solo_dev);
-	if (ret)
-		goto fail_probe;
-
-	ret = solo_enc_init(solo_dev);
-	if (ret)
-		goto fail_probe;
-
-	ret = solo_enc_v4l2_init(solo_dev);
-	if (ret)
-		goto fail_probe;
-
-	ret = solo_g723_init(solo_dev);
-	if (ret)
-		goto fail_probe;
-
-	return 0;
-
-fail_probe:
-	free_solo_dev(solo_dev);
-	return ret;
-}
-
-static void solo_pci_remove(struct pci_dev *pdev)
-{
-	struct solo_dev *solo_dev = pci_get_drvdata(pdev);
-
-	free_solo_dev(solo_dev);
-}
-
-static struct pci_device_id solo_id_table[] = {
-	/* 6010 based cards */
-	{PCI_DEVICE(PCI_VENDOR_ID_SOFTLOGIC, PCI_DEVICE_ID_SOLO6010)},
-	{PCI_DEVICE(PCI_VENDOR_ID_SOFTLOGIC, PCI_DEVICE_ID_SOLO6110),
-	 .driver_data = FLAGS_6110},
-	{PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_NEUSOLO_4)},
-	{PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_NEUSOLO_9)},
-	{PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_NEUSOLO_16)},
-	{PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_BC_SOLO_4)},
-	{PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_BC_SOLO_9)},
-	{PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_BC_SOLO_16)},
-	/* 6110 based cards */
-	{PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_BC_6110_4)},
-	{PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_BC_6110_8)},
-	{PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_BC_6110_16)},
-	{0,}
-};
-
-MODULE_DEVICE_TABLE(pci, solo_id_table);
-
-static struct pci_driver solo_pci_driver = {
-	.name = SOLO6X10_NAME,
-	.id_table = solo_id_table,
-	.probe = solo_pci_probe,
-	.remove = solo_pci_remove,
-};
-
-module_pci_driver(solo_pci_driver);
diff --git a/drivers/staging/media/solo6x10/offsets.h b/drivers/staging/media/solo6x10/offsets.h
deleted file mode 100644
index 3d7e569f1cf..00000000000
--- a/drivers/staging/media/solo6x10/offsets.h
+++ /dev/null
@@ -1,74 +0,0 @@
-/*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.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., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- */
-
-#ifndef __SOLO6X10_OFFSETS_H
-#define __SOLO6X10_OFFSETS_H
-
-/* Offsets and sizes of the external address */
-#define SOLO_DISP_EXT_ADDR			0x00000000
-#define SOLO_DISP_EXT_SIZE			0x00480000
-
-#define SOLO_DEC2LIVE_EXT_ADDR (SOLO_DISP_EXT_ADDR + SOLO_DISP_EXT_SIZE)
-#define SOLO_DEC2LIVE_EXT_SIZE			0x00240000
-
-#define SOLO_OSG_EXT_ADDR (SOLO_DEC2LIVE_EXT_ADDR + SOLO_DEC2LIVE_EXT_SIZE)
-#define SOLO_OSG_EXT_SIZE			0x00120000
-
-#define SOLO_EOSD_EXT_ADDR (SOLO_OSG_EXT_ADDR + SOLO_OSG_EXT_SIZE)
-#define SOLO_EOSD_EXT_SIZE			0x00010000
-
-#define SOLO_MOTION_EXT_ADDR(__solo) (SOLO_EOSD_EXT_ADDR +	\
-				      (SOLO_EOSD_EXT_SIZE * __solo->nr_chans))
-#define SOLO_MOTION_EXT_SIZE			0x00080000
-
-#define SOLO_G723_EXT_ADDR(__solo) \
-		(SOLO_MOTION_EXT_ADDR(__solo) + SOLO_MOTION_EXT_SIZE)
-#define SOLO_G723_EXT_SIZE			0x00010000
-
-#define SOLO_CAP_EXT_ADDR(__solo) \
-		(SOLO_G723_EXT_ADDR(__solo) + SOLO_G723_EXT_SIZE)
-#define SOLO_CAP_EXT_MAX_PAGE			(18 + 15)
-#define SOLO_CAP_EXT_SIZE			(SOLO_CAP_EXT_MAX_PAGE * 65536)
-
-/* This +1 is very important -- Why?! -- BenC */
-#define SOLO_EREF_EXT_ADDR(__solo) \
-		(SOLO_CAP_EXT_ADDR(__solo) + \
-		 (SOLO_CAP_EXT_SIZE * (__solo->nr_chans + 1)))
-#define SOLO_EREF_EXT_SIZE			0x00140000
-
-#define SOLO_MP4E_EXT_ADDR(__solo) \
-		(SOLO_EREF_EXT_ADDR(__solo) + \
-		 (SOLO_EREF_EXT_SIZE * __solo->nr_chans))
-#define SOLO_MP4E_EXT_SIZE(__solo)		(0x00080000 * __solo->nr_chans)
-
-#define SOLO_DREF_EXT_ADDR(__solo) \
-		(SOLO_MP4E_EXT_ADDR(__solo) + SOLO_MP4E_EXT_SIZE(__solo))
-#define SOLO_DREF_EXT_SIZE			0x00140000
-
-#define SOLO_MP4D_EXT_ADDR(__solo) \
-		(SOLO_DREF_EXT_ADDR(__solo) + \
-		 (SOLO_DREF_EXT_SIZE * __solo->nr_chans))
-#define SOLO_MP4D_EXT_SIZE			0x00080000
-
-#define SOLO_JPEG_EXT_ADDR(__solo) \
-		(SOLO_MP4D_EXT_ADDR(__solo) + \
-		 (SOLO_MP4D_EXT_SIZE * __solo->nr_chans))
-#define SOLO_JPEG_EXT_SIZE(__solo)		(0x00080000 * __solo->nr_chans)
-
-#endif /* __SOLO6X10_OFFSETS_H */
diff --git a/drivers/staging/media/solo6x10/osd-font.h b/drivers/staging/media/solo6x10/osd-font.h
deleted file mode 100644
index 591e0e82e0e..00000000000
--- a/drivers/staging/media/solo6x10/osd-font.h
+++ /dev/null
@@ -1,154 +0,0 @@
-/*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.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., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- */
-
-#ifndef __SOLO6X10_OSD_FONT_H
-#define __SOLO6X10_OSD_FONT_H
-
-static const unsigned int solo_osd_font[] = {
-	0x00000000, 0x0000c0c8, 0xccfefe0c, 0x08000000,
-	0x00000000, 0x10103838, 0x7c7cfefe, 0x00000000,	/* 0 */
-	0x00000000, 0xfefe7c7c, 0x38381010, 0x10000000,
-	0x00000000, 0x7c82fefe, 0xfefefe7c, 0x00000000,
-	0x00000000, 0x00001038, 0x10000000, 0x00000000,
-	0x00000000, 0x0010387c, 0xfe7c3810, 0x00000000,
-	0x00000000, 0x00384444, 0x44380000, 0x00000000,
-	0x00000000, 0x38448282, 0x82443800, 0x00000000,
-	0x00000000, 0x007c7c7c, 0x7c7c0000, 0x00000000,
-	0x00000000, 0x6c6c6c6c, 0x6c6c6c6c, 0x00000000,
-	0x00000000, 0x061e7efe, 0xfe7e1e06, 0x00000000,
-	0x00000000, 0xc0f0fcfe, 0xfefcf0c0, 0x00000000,
-	0x00000000, 0xc6cedefe, 0xfedecec6, 0x00000000,
-	0x00000000, 0xc6e6f6fe, 0xfef6e6c6, 0x00000000,
-	0x00000000, 0x12367efe, 0xfe7e3612, 0x00000000,
-	0x00000000, 0x90d8fcfe, 0xfefcd890, 0x00000000,
-	0x00000038, 0x7cc692ba, 0x92c67c38, 0x00000000,
-	0x00000038, 0x7cc6aa92, 0xaac67c38, 0x00000000,
-	0x00000038, 0x7830107c, 0xbaa8680c, 0x00000000,
-	0x00000038, 0x3c18127c, 0xb8382c60, 0x00000000,
-	0x00000044, 0xaa6c8254, 0x38eec67c, 0x00000000,
-	0x00000082, 0x44288244, 0x38c6827c, 0x00000000,
-	0x00000038, 0x444444fe, 0xfeeec6fe, 0x00000000,
-	0x00000018, 0x78187818, 0x3c7e7e3c, 0x00000000,
-	0x00000000, 0x3854929a, 0x82443800, 0x00000000,
-	0x00000000, 0x00c0c8cc, 0xfefe0c08, 0x00000000,
-	0x0000e0a0, 0xe040e00e, 0x8a0ea40e, 0x00000000,
-	0x0000e0a0, 0xe040e00e, 0x0a8e440e, 0x00000000,
-	0x0000007c, 0x82829292, 0x929282fe, 0x00000000,
-	0x000000f8, 0xfc046494, 0x946404fc, 0x00000000,
-	0x0000003f, 0x7f404c52, 0x524c407f, 0x00000000,
-	0x0000007c, 0x82ba82ba, 0x82ba82fe, 0x00000000,
-	0x00000000, 0x00000000, 0x00000000, 0x00000000,
-	0x00000000, 0x183c3c3c, 0x18180018, 0x18000000,	/* 32   ! */
-	0x00000066, 0x66240000, 0x00000000, 0x00000000,
-	0x00000000, 0x6c6cfe6c, 0x6c6cfe6c, 0x6c000000,	/* 34 " # */
-	0x00001010, 0x7cd6d616, 0x7cd0d6d6, 0x7c101000,
-	0x00000000, 0x0086c660, 0x30180cc6, 0xc2000000,	/* 36 $ % */
-	0x00000000, 0x386c6c38, 0xdc766666, 0xdc000000,
-	0x0000000c, 0x0c0c0600, 0x00000000, 0x00000000,	/* 38 & ' */
-	0x00000000, 0x30180c0c, 0x0c0c0c18, 0x30000000,
-	0x00000000, 0x0c183030, 0x30303018, 0x0c000000,	/* 40 ( ) */
-	0x00000000, 0x0000663c, 0xff3c6600, 0x00000000,
-	0x00000000, 0x00001818, 0x7e181800, 0x00000000,	/* 42 * + */
-	0x00000000, 0x00000000, 0x00000e0e, 0x0c060000,
-	0x00000000, 0x00000000, 0x7e000000, 0x00000000,	/* 44 , - */
-	0x00000000, 0x00000000, 0x00000006, 0x06000000,
-	0x00000000, 0x80c06030, 0x180c0602, 0x00000000,	/* 46 . / */
-	0x0000007c, 0xc6e6f6de, 0xcec6c67c, 0x00000000,
-	0x00000030, 0x383c3030, 0x303030fc, 0x00000000,	/* 48 0 1 */
-	0x0000007c, 0xc6c06030, 0x180cc6fe, 0x00000000,
-	0x0000007c, 0xc6c0c07c, 0xc0c0c67c, 0x00000000,	/* 50 2 3 */
-	0x00000060, 0x70786c66, 0xfe6060f0, 0x00000000,
-	0x000000fe, 0x0606067e, 0xc0c0c67c, 0x00000000,	/* 52 4 5 */
-	0x00000038, 0x0c06067e, 0xc6c6c67c, 0x00000000,
-	0x000000fe, 0xc6c06030, 0x18181818, 0x00000000,	/* 54 6 7 */
-	0x0000007c, 0xc6c6c67c, 0xc6c6c67c, 0x00000000,
-	0x0000007c, 0xc6c6c6fc, 0xc0c06038, 0x00000000,	/* 56 8 9 */
-	0x00000000, 0x18180000, 0x00181800, 0x00000000,
-	0x00000000, 0x18180000, 0x0018180c, 0x00000000,	/* 58 : ; */
-	0x00000060, 0x30180c06, 0x0c183060, 0x00000000,
-	0x00000000, 0x007e0000, 0x007e0000, 0x00000000,
-	0x00000006, 0x0c183060, 0x30180c06, 0x00000000,
-	0x0000007c, 0xc6c66030, 0x30003030, 0x00000000,
-	0x0000007c, 0xc6f6d6d6, 0x7606067c, 0x00000000,
-	0x00000010, 0x386cc6c6, 0xfec6c6c6, 0x00000000,	/* 64 @ A */
-	0x0000007e, 0xc6c6c67e, 0xc6c6c67e, 0x00000000,
-	0x00000078, 0xcc060606, 0x0606cc78, 0x00000000,	/* 66 */
-	0x0000003e, 0x66c6c6c6, 0xc6c6663e, 0x00000000,
-	0x000000fe, 0x0606063e, 0x060606fe, 0x00000000,	/* 68 */
-	0x000000fe, 0x0606063e, 0x06060606, 0x00000000,
-	0x00000078, 0xcc060606, 0xf6c6ccb8, 0x00000000,	/* 70 */
-	0x000000c6, 0xc6c6c6fe, 0xc6c6c6c6, 0x00000000,
-	0x0000003c, 0x18181818, 0x1818183c, 0x00000000,	/* 72 */
-	0x00000060, 0x60606060, 0x6066663c, 0x00000000,
-	0x000000c6, 0xc666361e, 0x3666c6c6, 0x00000000,	/* 74 */
-	0x00000006, 0x06060606, 0x060606fe, 0x00000000,
-	0x000000c6, 0xeefed6c6, 0xc6c6c6c6, 0x00000000,	/* 76 */
-	0x000000c6, 0xcedefef6, 0xe6c6c6c6, 0x00000000,
-	0x00000038, 0x6cc6c6c6, 0xc6c66c38, 0x00000000,	/* 78 */
-	0x0000007e, 0xc6c6c67e, 0x06060606, 0x00000000,
-	0x00000038, 0x6cc6c6c6, 0xc6d67c38, 0x60000000,	/* 80 */
-	0x0000007e, 0xc6c6c67e, 0x66c6c6c6, 0x00000000,
-	0x0000007c, 0xc6c60c38, 0x60c6c67c, 0x00000000,	/* 82 */
-	0x0000007e, 0x18181818, 0x18181818, 0x00000000,
-	0x000000c6, 0xc6c6c6c6, 0xc6c6c67c, 0x00000000,	/* 84 */
-	0x000000c6, 0xc6c6c6c6, 0xc66c3810, 0x00000000,
-	0x000000c6, 0xc6c6c6c6, 0xd6d6fe6c, 0x00000000,	/* 86 */
-	0x000000c6, 0xc6c66c38, 0x6cc6c6c6, 0x00000000,
-	0x00000066, 0x66666666, 0x3c181818, 0x00000000,	/* 88 */
-	0x000000fe, 0xc0603018, 0x0c0606fe, 0x00000000,
-	0x0000003c, 0x0c0c0c0c, 0x0c0c0c3c, 0x00000000,	/* 90 */
-	0x00000002, 0x060c1830, 0x60c08000, 0x00000000,
-	0x0000003c, 0x30303030, 0x3030303c, 0x00000000,	/* 92 */
-	0x00001038, 0x6cc60000, 0x00000000, 0x00000000,
-	0x00000000, 0x00000000, 0x00000000, 0x00fe0000,
-	0x00001818, 0x30000000, 0x00000000, 0x00000000,
-	0x00000000, 0x00003c60, 0x7c66667c, 0x00000000,
-	0x0000000c, 0x0c0c7ccc, 0xcccccc7c, 0x00000000,
-	0x00000000, 0x00007cc6, 0x0606c67c, 0x00000000,
-	0x00000060, 0x60607c66, 0x6666667c, 0x00000000,
-	0x00000000, 0x00007cc6, 0xfe06c67c, 0x00000000,
-	0x00000078, 0x0c0c0c3e, 0x0c0c0c0c, 0x00000000,
-	0x00000000, 0x00007c66, 0x6666667c, 0x60603e00,
-	0x0000000c, 0x0c0c7ccc, 0xcccccccc, 0x00000000,
-	0x00000030, 0x30003830, 0x30303078, 0x00000000,
-	0x00000030, 0x30003c30, 0x30303030, 0x30301f00,
-	0x0000000c, 0x0c0ccc6c, 0x3c6ccccc, 0x00000000,
-	0x00000030, 0x30303030, 0x30303030, 0x00000000,
-	0x00000000, 0x000066fe, 0xd6d6d6d6, 0x00000000,
-	0x00000000, 0x000078cc, 0xcccccccc, 0x00000000,
-	0x00000000, 0x00007cc6, 0xc6c6c67c, 0x00000000,
-	0x00000000, 0x00007ccc, 0xcccccc7c, 0x0c0c0c00,
-	0x00000000, 0x00007c66, 0x6666667c, 0x60606000,
-	0x00000000, 0x000076dc, 0x0c0c0c0c, 0x00000000,
-	0x00000000, 0x00007cc6, 0x1c70c67c, 0x00000000,
-	0x00000000, 0x1818fe18, 0x18181870, 0x00000000,
-	0x00000000, 0x00006666, 0x6666663c, 0x00000000,
-	0x00000000, 0x0000c6c6, 0xc66c3810, 0x00000000,
-	0x00000000, 0x0000c6d6, 0xd6d6fe6c, 0x00000000,
-	0x00000000, 0x0000c66c, 0x38386cc6, 0x00000000,
-	0x00000000, 0x00006666, 0x6666667c, 0x60603e00,
-	0x00000000, 0x0000fe60, 0x30180cfe, 0x00000000,
-	0x00000070, 0x1818180e, 0x18181870, 0x00000000,
-	0x00000018, 0x18181800, 0x18181818, 0x00000000,
-	0x0000000e, 0x18181870, 0x1818180e, 0x00000000,
-	0x000000dc, 0x76000000, 0x00000000, 0x00000000,
-	0x00000000, 0x0010386c, 0xc6c6fe00, 0x00000000
-};
-
-#endif /* __SOLO6X10_OSD_FONT_H */
diff --git a/drivers/staging/media/solo6x10/p2m.c b/drivers/staging/media/solo6x10/p2m.c
deleted file mode 100644
index 58ab61b1f1d..00000000000
--- a/drivers/staging/media/solo6x10/p2m.c
+++ /dev/null
@@ -1,306 +0,0 @@
-/*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.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., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- */
-
-#include <linux/kernel.h>
-#include <linux/slab.h>
-#include <linux/scatterlist.h>
-#include "solo6x10.h"
-
-/* #define SOLO_TEST_P2M */
-
-int solo_p2m_dma(struct solo_dev *solo_dev, u8 id, int wr,
-		 void *sys_addr, u32 ext_addr, u32 size)
-{
-	dma_addr_t dma_addr;
-	int ret;
-
-	WARN_ON(!size);
-	BUG_ON(id >= SOLO_NR_P2M);
-
-	if (!size)
-		return -EINVAL;
-
-	dma_addr = pci_map_single(solo_dev->pdev, sys_addr, size,
-				  wr ? PCI_DMA_TODEVICE : PCI_DMA_FROMDEVICE);
-
-	ret = solo_p2m_dma_t(solo_dev, id, wr, dma_addr, ext_addr, size);
-
-	pci_unmap_single(solo_dev->pdev, dma_addr, size,
-			 wr ? PCI_DMA_TODEVICE : PCI_DMA_FROMDEVICE);
-
-	return ret;
-}
-
-int solo_p2m_dma_t(struct solo_dev *solo_dev, u8 id, int wr,
-		   dma_addr_t dma_addr, u32 ext_addr, u32 size)
-{
-	struct p2m_desc *desc = kzalloc(sizeof(*desc) * 2, GFP_DMA);
-	int ret;
-
-	if (desc == NULL)
-		return -ENOMEM;
-
-	solo_p2m_push_desc(&desc[1], wr, dma_addr, ext_addr, size, 0, 0);
-	ret = solo_p2m_dma_desc(solo_dev, id, desc, 2);
-	kfree(desc);
-
-	return ret;
-}
-
-void solo_p2m_push_desc(struct p2m_desc *desc, int wr, dma_addr_t dma_addr,
-			u32 ext_addr, u32 size, int repeat, u32 ext_size)
-{
-	desc->ta = cpu_to_le32(dma_addr);
-	desc->fa = cpu_to_le32(ext_addr);
-
-	desc->ext = cpu_to_le32(SOLO_P2M_COPY_SIZE(size >> 2));
-	desc->ctrl = cpu_to_le32(SOLO_P2M_BURST_SIZE(SOLO_P2M_BURST_256) |
-				 (wr ? SOLO_P2M_WRITE : 0) | SOLO_P2M_TRANS_ON);
-
-	/* Ext size only matters when we're repeating */
-	if (repeat) {
-		desc->ext |= cpu_to_le32(SOLO_P2M_EXT_INC(ext_size >> 2));
-		desc->ctrl |=  cpu_to_le32(SOLO_P2M_PCI_INC(size >> 2) |
-					   SOLO_P2M_REPEAT(repeat));
-	}
-}
-
-int solo_p2m_dma_desc(struct solo_dev *solo_dev, u8 id,
-		      struct p2m_desc *desc, int desc_count)
-{
-	struct solo_p2m_dev *p2m_dev;
-	unsigned int timeout;
-	int ret = 0;
-	u32 config = 0;
-	dma_addr_t desc_dma = 0;
-
-	BUG_ON(id >= SOLO_NR_P2M);
-	BUG_ON(!desc_count || desc_count > SOLO_NR_P2M_DESC);
-
-	p2m_dev = &solo_dev->p2m_dev[id];
-
-	mutex_lock(&p2m_dev->mutex);
-
-	solo_reg_write(solo_dev, SOLO_P2M_CONTROL(id), 0);
-
-	INIT_COMPLETION(p2m_dev->completion);
-	p2m_dev->error = 0;
-
-	/* Enable the descriptors */
-	config = solo_reg_read(solo_dev, SOLO_P2M_CONFIG(id));
-	desc_dma = pci_map_single(solo_dev->pdev, desc,
-				  desc_count * sizeof(*desc),
-				  PCI_DMA_TODEVICE);
-	solo_reg_write(solo_dev, SOLO_P2M_DES_ADR(id), desc_dma);
-	solo_reg_write(solo_dev, SOLO_P2M_DESC_ID(id), desc_count - 1);
-	solo_reg_write(solo_dev, SOLO_P2M_CONFIG(id), config |
-		       SOLO_P2M_DESC_MODE);
-
-	/* Should have all descriptors completed from one interrupt */
-	timeout = wait_for_completion_timeout(&p2m_dev->completion, HZ);
-
-	solo_reg_write(solo_dev, SOLO_P2M_CONTROL(id), 0);
-
-	/* Reset back to non-descriptor mode */
-	solo_reg_write(solo_dev, SOLO_P2M_CONFIG(id), config);
-	solo_reg_write(solo_dev, SOLO_P2M_DESC_ID(id), 0);
-	solo_reg_write(solo_dev, SOLO_P2M_DES_ADR(id), 0);
-	pci_unmap_single(solo_dev->pdev, desc_dma,
-			 desc_count * sizeof(*desc),
-			 PCI_DMA_TODEVICE);
-
-	if (p2m_dev->error)
-		ret = -EIO;
-	else if (timeout == 0)
-		ret = -EAGAIN;
-
-	mutex_unlock(&p2m_dev->mutex);
-
-	WARN_ON_ONCE(ret);
-
-	return ret;
-}
-
-int solo_p2m_dma_sg(struct solo_dev *solo_dev, u8 id,
-		    struct p2m_desc *pdesc, int wr,
-		    struct scatterlist *sg, u32 sg_off,
-		    u32 ext_addr, u32 size)
-{
-	int i;
-	int idx;
-
-	BUG_ON(id >= SOLO_NR_P2M);
-
-	if (WARN_ON_ONCE(!size))
-		return -EINVAL;
-
-	memset(pdesc, 0, sizeof(*pdesc));
-
-	/* Should rewrite this to handle > SOLO_NR_P2M_DESC transactions */
-	for (i = 0, idx = 1; idx < SOLO_NR_P2M_DESC && sg && size > 0;
-	     i++, sg = sg_next(sg)) {
-		struct p2m_desc *desc = &pdesc[idx];
-		u32 sg_len = sg_dma_len(sg);
-		u32 len;
-
-		if (sg_off >= sg_len) {
-			sg_off -= sg_len;
-			continue;
-		}
-
-		sg_len -= sg_off;
-		len = min(sg_len, size);
-
-		solo_p2m_push_desc(desc, wr, sg_dma_address(sg) + sg_off,
-				   ext_addr, len, 0, 0);
-
-		size -= len;
-		ext_addr += len;
-		idx++;
-
-		sg_off = 0;
-	}
-
-	WARN_ON_ONCE(size || i >= SOLO_NR_P2M_DESC);
-
-	return solo_p2m_dma_desc(solo_dev, id, pdesc, idx);
-}
-
-#ifdef SOLO_TEST_P2M
-
-#define P2M_TEST_CHAR		0xbe
-
-static unsigned long long p2m_test(struct solo_dev *solo_dev, u8 id,
-				   u32 base, int size)
-{
-	u8 *wr_buf;
-	u8 *rd_buf;
-	int i;
-	unsigned long long err_cnt = 0;
-
-	wr_buf = kmalloc(size, GFP_KERNEL);
-	if (!wr_buf) {
-		printk(SOLO6X10_NAME ": Failed to malloc for p2m_test\n");
-		return size;
-	}
-
-	rd_buf = kmalloc(size, GFP_KERNEL);
-	if (!rd_buf) {
-		printk(SOLO6X10_NAME ": Failed to malloc for p2m_test\n");
-		kfree(wr_buf);
-		return size;
-	}
-
-	memset(wr_buf, P2M_TEST_CHAR, size);
-	memset(rd_buf, P2M_TEST_CHAR + 1, size);
-
-	solo_p2m_dma(solo_dev, id, 1, wr_buf, base, size);
-	solo_p2m_dma(solo_dev, id, 0, rd_buf, base, size);
-
-	for (i = 0; i < size; i++)
-		if (wr_buf[i] != rd_buf[i])
-			err_cnt++;
-
-	kfree(wr_buf);
-	kfree(rd_buf);
-
-	return err_cnt;
-}
-
-#define TEST_CHUNK_SIZE		(8 * 1024)
-
-static void run_p2m_test(struct solo_dev *solo_dev)
-{
-	unsigned long long errs = 0;
-	u32 size = SOLO_JPEG_EXT_ADDR(solo_dev) + SOLO_JPEG_EXT_SIZE(solo_dev);
-	int i, d;
-
-	dev_warn(&solo_dev->pdev->dev, "Testing %u bytes of external ram\n",
-		 size);
-
-	for (i = 0; i < size; i += TEST_CHUNK_SIZE)
-		for (d = 0; d < 4; d++)
-			errs += p2m_test(solo_dev, d, i, TEST_CHUNK_SIZE);
-
-	dev_warn(&solo_dev->pdev->dev, "Found %llu errors during p2m test\n",
-		 errs);
-
-	return;
-}
-#else
-#define run_p2m_test(__solo)	do {} while (0)
-#endif
-
-void solo_p2m_isr(struct solo_dev *solo_dev, int id)
-{
-	struct solo_p2m_dev *p2m_dev = &solo_dev->p2m_dev[id];
-
-	solo_reg_write(solo_dev, SOLO_IRQ_STAT, SOLO_IRQ_P2M(id));
-
-	complete(&p2m_dev->completion);
-}
-
-void solo_p2m_error_isr(struct solo_dev *solo_dev, u32 status)
-{
-	struct solo_p2m_dev *p2m_dev;
-	int i;
-
-	if (!(status & SOLO_PCI_ERR_P2M))
-		return;
-
-	for (i = 0; i < SOLO_NR_P2M; i++) {
-		p2m_dev = &solo_dev->p2m_dev[i];
-		p2m_dev->error = 1;
-		solo_reg_write(solo_dev, SOLO_P2M_CONTROL(i), 0);
-		complete(&p2m_dev->completion);
-	}
-}
-
-void solo_p2m_exit(struct solo_dev *solo_dev)
-{
-	int i;
-
-	for (i = 0; i < SOLO_NR_P2M; i++)
-		solo_irq_off(solo_dev, SOLO_IRQ_P2M(i));
-}
-
-int solo_p2m_init(struct solo_dev *solo_dev)
-{
-	struct solo_p2m_dev *p2m_dev;
-	int i;
-
-	for (i = 0; i < SOLO_NR_P2M; i++) {
-		p2m_dev = &solo_dev->p2m_dev[i];
-
-		mutex_init(&p2m_dev->mutex);
-		init_completion(&p2m_dev->completion);
-
-		solo_reg_write(solo_dev, SOLO_P2M_CONTROL(i), 0);
-		solo_reg_write(solo_dev, SOLO_P2M_CONFIG(i),
-			       SOLO_P2M_CSC_16BIT_565 |
-			       SOLO_P2M_DMA_INTERVAL(3) |
-			       SOLO_P2M_DESC_INTR_OPT |
-			       SOLO_P2M_PCI_MASTER_MODE);
-		solo_irq_on(solo_dev, SOLO_IRQ_P2M(i));
-	}
-
-	run_p2m_test(solo_dev);
-
-	return 0;
-}
diff --git a/drivers/staging/media/solo6x10/solo6x10-core.c b/drivers/staging/media/solo6x10/solo6x10-core.c
new file mode 100644
index 00000000000..36750205d23
--- /dev/null
+++ b/drivers/staging/media/solo6x10/solo6x10-core.c
@@ -0,0 +1,709 @@
+/*
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.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., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/interrupt.h>
+#include <linux/videodev2.h>
+#include <linux/delay.h>
+#include <linux/sysfs.h>
+#include <linux/ktime.h>
+#include <linux/slab.h>
+
+#include "solo6x10.h"
+#include "solo6x10-tw28.h"
+
+MODULE_DESCRIPTION("Softlogic 6x10 MPEG4/H.264/G.723 CODEC V4L2/ALSA Driver");
+MODULE_AUTHOR("Bluecherry <maintainers@bluecherrydvr.com>");
+MODULE_VERSION(SOLO6X10_VERSION);
+MODULE_LICENSE("GPL");
+
+unsigned video_nr = -1;
+module_param(video_nr, uint, 0644);
+MODULE_PARM_DESC(video_nr, "videoX start number, -1 is autodetect (default)");
+
+static int full_eeprom; /* default is only top 64B */
+module_param(full_eeprom, uint, 0644);
+MODULE_PARM_DESC(full_eeprom, "Allow access to full 128B EEPROM (dangerous)");
+
+
+static void solo_set_time(struct solo_dev *solo_dev)
+{
+	struct timespec ts;
+
+	ktime_get_ts(&ts);
+
+	solo_reg_write(solo_dev, SOLO_TIMER_SEC, ts.tv_sec);
+	solo_reg_write(solo_dev, SOLO_TIMER_USEC, ts.tv_nsec / NSEC_PER_USEC);
+}
+
+static void solo_timer_sync(struct solo_dev *solo_dev)
+{
+	u32 sec, usec;
+	struct timespec ts;
+	long diff;
+
+	if (solo_dev->type != SOLO_DEV_6110)
+		return;
+
+	if (++solo_dev->time_sync < 60)
+		return;
+
+	solo_dev->time_sync = 0;
+
+	sec = solo_reg_read(solo_dev, SOLO_TIMER_SEC);
+	usec = solo_reg_read(solo_dev, SOLO_TIMER_USEC);
+
+	ktime_get_ts(&ts);
+
+	diff = (long)ts.tv_sec - (long)sec;
+	diff = (diff * 1000000)
+		+ ((long)(ts.tv_nsec / NSEC_PER_USEC) - (long)usec);
+
+	if (diff > 1000 || diff < -1000) {
+		solo_set_time(solo_dev);
+	} else if (diff) {
+		long usec_lsb = solo_dev->usec_lsb;
+
+		usec_lsb -= diff / 4;
+		if (usec_lsb < 0)
+			usec_lsb = 0;
+		else if (usec_lsb > 255)
+			usec_lsb = 255;
+
+		solo_dev->usec_lsb = usec_lsb;
+		solo_reg_write(solo_dev, SOLO_TIMER_USEC_LSB,
+			       solo_dev->usec_lsb);
+	}
+}
+
+static irqreturn_t solo_isr(int irq, void *data)
+{
+	struct solo_dev *solo_dev = data;
+	u32 status;
+	int i;
+
+	status = solo_reg_read(solo_dev, SOLO_IRQ_STAT);
+	if (!status)
+		return IRQ_NONE;
+
+	if (status & ~solo_dev->irq_mask) {
+		solo_reg_write(solo_dev, SOLO_IRQ_STAT,
+			       status & ~solo_dev->irq_mask);
+		status &= solo_dev->irq_mask;
+	}
+
+	if (status & SOLO_IRQ_PCI_ERR)
+		solo_p2m_error_isr(solo_dev);
+
+	for (i = 0; i < SOLO_NR_P2M; i++)
+		if (status & SOLO_IRQ_P2M(i))
+			solo_p2m_isr(solo_dev, i);
+
+	if (status & SOLO_IRQ_IIC)
+		solo_i2c_isr(solo_dev);
+
+	if (status & SOLO_IRQ_VIDEO_IN) {
+		solo_video_in_isr(solo_dev);
+		solo_timer_sync(solo_dev);
+	}
+
+	if (status & SOLO_IRQ_ENCODER)
+		solo_enc_v4l2_isr(solo_dev);
+
+	if (status & SOLO_IRQ_G723)
+		solo_g723_isr(solo_dev);
+
+	/* Clear all interrupts handled */
+	solo_reg_write(solo_dev, SOLO_IRQ_STAT, status);
+
+	return IRQ_HANDLED;
+}
+
+static void free_solo_dev(struct solo_dev *solo_dev)
+{
+	struct pci_dev *pdev;
+
+	if (!solo_dev)
+		return;
+
+	if (solo_dev->dev.parent)
+		device_unregister(&solo_dev->dev);
+
+	pdev = solo_dev->pdev;
+
+	/* If we never initialized the PCI device, then nothing else
+	 * below here needs cleanup */
+	if (!pdev) {
+		kfree(solo_dev);
+		return;
+	}
+
+	if (solo_dev->reg_base) {
+		/* Bring down the sub-devices first */
+		solo_g723_exit(solo_dev);
+		solo_enc_v4l2_exit(solo_dev);
+		solo_enc_exit(solo_dev);
+		solo_v4l2_exit(solo_dev);
+		solo_disp_exit(solo_dev);
+		solo_gpio_exit(solo_dev);
+		solo_p2m_exit(solo_dev);
+		solo_i2c_exit(solo_dev);
+
+		/* Now cleanup the PCI device */
+		solo_irq_off(solo_dev, ~0);
+		pci_iounmap(pdev, solo_dev->reg_base);
+		if (pdev->irq)
+			free_irq(pdev->irq, solo_dev);
+	}
+
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+	v4l2_device_unregister(&solo_dev->v4l2_dev);
+	pci_set_drvdata(pdev, NULL);
+
+	kfree(solo_dev);
+}
+
+static ssize_t eeprom_store(struct device *dev, struct device_attribute *attr,
+			    const char *buf, size_t count)
+{
+	struct solo_dev *solo_dev =
+		container_of(dev, struct solo_dev, dev);
+	unsigned short *p = (unsigned short *)buf;
+	int i;
+
+	if (count & 0x1)
+		dev_warn(dev, "EEPROM Write not aligned (truncating)\n");
+
+	if (!full_eeprom && count > 64) {
+		dev_warn(dev, "EEPROM Write truncated to 64 bytes\n");
+		count = 64;
+	} else if (full_eeprom && count > 128) {
+		dev_warn(dev, "EEPROM Write truncated to 128 bytes\n");
+		count = 128;
+	}
+
+	solo_eeprom_ewen(solo_dev, 1);
+
+	for (i = full_eeprom ? 0 : 32; i < min((int)(full_eeprom ? 64 : 32),
+					       (int)(count / 2)); i++)
+		solo_eeprom_write(solo_dev, i, cpu_to_be16(p[i]));
+
+	solo_eeprom_ewen(solo_dev, 0);
+
+	return count;
+}
+
+static ssize_t eeprom_show(struct device *dev, struct device_attribute *attr,
+			   char *buf)
+{
+	struct solo_dev *solo_dev =
+		container_of(dev, struct solo_dev, dev);
+	unsigned short *p = (unsigned short *)buf;
+	int count = (full_eeprom ? 128 : 64);
+	int i;
+
+	for (i = (full_eeprom ? 0 : 32); i < (count / 2); i++)
+		p[i] = be16_to_cpu(solo_eeprom_read(solo_dev, i));
+
+	return count;
+}
+
+static ssize_t p2m_timeouts_show(struct device *dev,
+				 struct device_attribute *attr,
+				 char *buf)
+{
+	struct solo_dev *solo_dev =
+		container_of(dev, struct solo_dev, dev);
+
+	return sprintf(buf, "%d\n", solo_dev->p2m_timeouts);
+}
+
+static ssize_t sdram_size_show(struct device *dev,
+			       struct device_attribute *attr,
+			       char *buf)
+{
+	struct solo_dev *solo_dev =
+		container_of(dev, struct solo_dev, dev);
+
+	return sprintf(buf, "%dMegs\n", solo_dev->sdram_size >> 20);
+}
+
+static ssize_t tw28xx_show(struct device *dev,
+			   struct device_attribute *attr,
+			   char *buf)
+{
+	struct solo_dev *solo_dev =
+		container_of(dev, struct solo_dev, dev);
+
+	return sprintf(buf, "tw2815[%d] tw2864[%d] tw2865[%d]\n",
+		       hweight32(solo_dev->tw2815),
+		       hweight32(solo_dev->tw2864),
+		       hweight32(solo_dev->tw2865));
+}
+
+static ssize_t input_map_show(struct device *dev,
+			      struct device_attribute *attr,
+			      char *buf)
+{
+	struct solo_dev *solo_dev =
+		container_of(dev, struct solo_dev, dev);
+	unsigned int val;
+	char *out = buf;
+
+	val = solo_reg_read(solo_dev, SOLO_VI_CH_SWITCH_0);
+	out += sprintf(out, "Channel 0   => Input %d\n", val & 0x1f);
+	out += sprintf(out, "Channel 1   => Input %d\n", (val >> 5) & 0x1f);
+	out += sprintf(out, "Channel 2   => Input %d\n", (val >> 10) & 0x1f);
+	out += sprintf(out, "Channel 3   => Input %d\n", (val >> 15) & 0x1f);
+	out += sprintf(out, "Channel 4   => Input %d\n", (val >> 20) & 0x1f);
+	out += sprintf(out, "Channel 5   => Input %d\n", (val >> 25) & 0x1f);
+
+	val = solo_reg_read(solo_dev, SOLO_VI_CH_SWITCH_1);
+	out += sprintf(out, "Channel 6   => Input %d\n", val & 0x1f);
+	out += sprintf(out, "Channel 7   => Input %d\n", (val >> 5) & 0x1f);
+	out += sprintf(out, "Channel 8   => Input %d\n", (val >> 10) & 0x1f);
+	out += sprintf(out, "Channel 9   => Input %d\n", (val >> 15) & 0x1f);
+	out += sprintf(out, "Channel 10  => Input %d\n", (val >> 20) & 0x1f);
+	out += sprintf(out, "Channel 11  => Input %d\n", (val >> 25) & 0x1f);
+
+	val = solo_reg_read(solo_dev, SOLO_VI_CH_SWITCH_2);
+	out += sprintf(out, "Channel 12  => Input %d\n", val & 0x1f);
+	out += sprintf(out, "Channel 13  => Input %d\n", (val >> 5) & 0x1f);
+	out += sprintf(out, "Channel 14  => Input %d\n", (val >> 10) & 0x1f);
+	out += sprintf(out, "Channel 15  => Input %d\n", (val >> 15) & 0x1f);
+	out += sprintf(out, "Spot Output => Input %d\n", (val >> 20) & 0x1f);
+
+	return out - buf;
+}
+
+static ssize_t p2m_timeout_store(struct device *dev,
+				 struct device_attribute *attr,
+				 const char *buf, size_t count)
+{
+	struct solo_dev *solo_dev =
+		container_of(dev, struct solo_dev, dev);
+	unsigned long ms;
+
+	int ret = kstrtoul(buf, 10, &ms);
+	if (ret < 0 || ms > 200)
+		return -EINVAL;
+	solo_dev->p2m_jiffies = msecs_to_jiffies(ms);
+
+	return count;
+}
+
+static ssize_t p2m_timeout_show(struct device *dev,
+				struct device_attribute *attr,
+				char *buf)
+{
+	struct solo_dev *solo_dev =
+		container_of(dev, struct solo_dev, dev);
+
+	return sprintf(buf, "%ums\n", jiffies_to_msecs(solo_dev->p2m_jiffies));
+}
+
+static ssize_t intervals_show(struct device *dev,
+			      struct device_attribute *attr,
+			      char *buf)
+{
+	struct solo_dev *solo_dev =
+		container_of(dev, struct solo_dev, dev);
+	char *out = buf;
+	int fps = solo_dev->fps;
+	int i;
+
+	for (i = 0; i < solo_dev->nr_chans; i++) {
+		out += sprintf(out, "Channel %d: %d/%d (0x%08x)\n",
+			       i, solo_dev->v4l2_enc[i]->interval, fps,
+			       solo_reg_read(solo_dev, SOLO_CAP_CH_INTV(i)));
+	}
+
+	return out - buf;
+}
+
+static ssize_t sdram_offsets_show(struct device *dev,
+				  struct device_attribute *attr,
+				  char *buf)
+{
+	struct solo_dev *solo_dev =
+		container_of(dev, struct solo_dev, dev);
+	char *out = buf;
+
+	out += sprintf(out, "DISP: 0x%08x @ 0x%08x\n",
+		       SOLO_DISP_EXT_ADDR,
+		       SOLO_DISP_EXT_SIZE);
+
+	out += sprintf(out, "EOSD: 0x%08x @ 0x%08x (0x%08x * %d)\n",
+		       SOLO_EOSD_EXT_ADDR,
+		       SOLO_EOSD_EXT_AREA(solo_dev),
+		       SOLO_EOSD_EXT_SIZE(solo_dev),
+		       SOLO_EOSD_EXT_AREA(solo_dev) /
+		       SOLO_EOSD_EXT_SIZE(solo_dev));
+
+	out += sprintf(out, "MOTI: 0x%08x @ 0x%08x\n",
+		       SOLO_MOTION_EXT_ADDR(solo_dev),
+		       SOLO_MOTION_EXT_SIZE);
+
+	out += sprintf(out, "G723: 0x%08x @ 0x%08x\n",
+		       SOLO_G723_EXT_ADDR(solo_dev),
+		       SOLO_G723_EXT_SIZE);
+
+	out += sprintf(out, "CAPT: 0x%08x @ 0x%08x (0x%08x * %d)\n",
+		       SOLO_CAP_EXT_ADDR(solo_dev),
+		       SOLO_CAP_EXT_SIZE(solo_dev),
+		       SOLO_CAP_PAGE_SIZE,
+		       SOLO_CAP_EXT_SIZE(solo_dev) / SOLO_CAP_PAGE_SIZE);
+
+	out += sprintf(out, "EREF: 0x%08x @ 0x%08x (0x%08x * %d)\n",
+		       SOLO_EREF_EXT_ADDR(solo_dev),
+		       SOLO_EREF_EXT_AREA(solo_dev),
+		       SOLO_EREF_EXT_SIZE,
+		       SOLO_EREF_EXT_AREA(solo_dev) / SOLO_EREF_EXT_SIZE);
+
+	out += sprintf(out, "MPEG: 0x%08x @ 0x%08x\n",
+		       SOLO_MP4E_EXT_ADDR(solo_dev),
+		       SOLO_MP4E_EXT_SIZE(solo_dev));
+
+	out += sprintf(out, "JPEG: 0x%08x @ 0x%08x\n",
+		       SOLO_JPEG_EXT_ADDR(solo_dev),
+		       SOLO_JPEG_EXT_SIZE(solo_dev));
+
+	return out - buf;
+}
+
+static ssize_t sdram_show(struct file *file, struct kobject *kobj,
+			  struct bin_attribute *a, char *buf,
+			  loff_t off, size_t count)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct solo_dev *solo_dev =
+		container_of(dev, struct solo_dev, dev);
+	const int size = solo_dev->sdram_size;
+
+	if (off >= size)
+		return 0;
+
+	if (off + count > size)
+		count = size - off;
+
+	if (solo_p2m_dma(solo_dev, 0, buf, off, count, 0, 0))
+		return -EIO;
+
+	return count;
+}
+
+static const struct device_attribute solo_dev_attrs[] = {
+	__ATTR(eeprom, 0640, eeprom_show, eeprom_store),
+	__ATTR(p2m_timeout, 0644, p2m_timeout_show, p2m_timeout_store),
+	__ATTR_RO(p2m_timeouts),
+	__ATTR_RO(sdram_size),
+	__ATTR_RO(tw28xx),
+	__ATTR_RO(input_map),
+	__ATTR_RO(intervals),
+	__ATTR_RO(sdram_offsets),
+};
+
+static void solo_device_release(struct device *dev)
+{
+	/* Do nothing */
+}
+
+static int solo_sysfs_init(struct solo_dev *solo_dev)
+{
+	struct bin_attribute *sdram_attr = &solo_dev->sdram_attr;
+	struct device *dev = &solo_dev->dev;
+	const char *driver;
+	int i;
+
+	if (solo_dev->type == SOLO_DEV_6110)
+		driver = "solo6110";
+	else
+		driver = "solo6010";
+
+	dev->release = solo_device_release;
+	dev->parent = &solo_dev->pdev->dev;
+	set_dev_node(dev, dev_to_node(&solo_dev->pdev->dev));
+	dev_set_name(dev, "%s-%d-%d", driver, solo_dev->vfd->num,
+		     solo_dev->nr_chans);
+
+	if (device_register(dev)) {
+		dev->parent = NULL;
+		return -ENOMEM;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(solo_dev_attrs); i++) {
+		if (device_create_file(dev, &solo_dev_attrs[i])) {
+			device_unregister(dev);
+			return -ENOMEM;
+		}
+	}
+
+	sysfs_attr_init(&sdram_attr->attr);
+	sdram_attr->attr.name = "sdram";
+	sdram_attr->attr.mode = 0440;
+	sdram_attr->read = sdram_show;
+	sdram_attr->size = solo_dev->sdram_size;
+
+	if (device_create_bin_file(dev, sdram_attr)) {
+		device_unregister(dev);
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static int solo_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
+{
+	struct solo_dev *solo_dev;
+	int ret;
+	u8 chip_id;
+
+	solo_dev = kzalloc(sizeof(*solo_dev), GFP_KERNEL);
+	if (solo_dev == NULL)
+		return -ENOMEM;
+
+	if (id->driver_data == SOLO_DEV_6010)
+		dev_info(&pdev->dev, "Probing Softlogic 6010\n");
+	else
+		dev_info(&pdev->dev, "Probing Softlogic 6110\n");
+
+	solo_dev->type = id->driver_data;
+	solo_dev->pdev = pdev;
+	spin_lock_init(&solo_dev->reg_io_lock);
+	ret = v4l2_device_register(&pdev->dev, &solo_dev->v4l2_dev);
+	if (ret)
+		goto fail_probe;
+
+	/* Only for during init */
+	solo_dev->p2m_jiffies = msecs_to_jiffies(100);
+
+	ret = pci_enable_device(pdev);
+	if (ret)
+		goto fail_probe;
+
+	pci_set_master(pdev);
+
+	/* RETRY/TRDY Timeout disabled */
+	pci_write_config_byte(pdev, 0x40, 0x00);
+	pci_write_config_byte(pdev, 0x41, 0x00);
+
+	ret = pci_request_regions(pdev, SOLO6X10_NAME);
+	if (ret)
+		goto fail_probe;
+
+	solo_dev->reg_base = pci_ioremap_bar(pdev, 0);
+	if (solo_dev->reg_base == NULL) {
+		ret = -ENOMEM;
+		goto fail_probe;
+	}
+
+	chip_id = solo_reg_read(solo_dev, SOLO_CHIP_OPTION) &
+				SOLO_CHIP_ID_MASK;
+	switch (chip_id) {
+	case 7:
+		solo_dev->nr_chans = 16;
+		solo_dev->nr_ext = 5;
+		break;
+	case 6:
+		solo_dev->nr_chans = 8;
+		solo_dev->nr_ext = 2;
+		break;
+	default:
+		dev_warn(&pdev->dev, "Invalid chip_id 0x%02x, assuming 4 ch\n",
+			 chip_id);
+	case 5:
+		solo_dev->nr_chans = 4;
+		solo_dev->nr_ext = 1;
+	}
+
+	/* Disable all interrupts to start */
+	solo_irq_off(solo_dev, ~0);
+
+	/* Initial global settings */
+	if (solo_dev->type == SOLO_DEV_6010) {
+		solo_dev->clock_mhz = 108;
+		solo_dev->sys_config = SOLO_SYS_CFG_SDRAM64BIT
+			| SOLO_SYS_CFG_INPUTDIV(25)
+			| SOLO_SYS_CFG_FEEDBACKDIV(solo_dev->clock_mhz * 2 - 2)
+			| SOLO_SYS_CFG_OUTDIV(3);
+		solo_reg_write(solo_dev, SOLO_SYS_CFG, solo_dev->sys_config);
+	} else {
+		u32 divq, divf;
+
+		solo_dev->clock_mhz = 135;
+
+		if (solo_dev->clock_mhz < 125) {
+			divq = 3;
+			divf = (solo_dev->clock_mhz * 4) / 3 - 1;
+		} else {
+			divq = 2;
+			divf = (solo_dev->clock_mhz * 2) / 3 - 1;
+		}
+
+		solo_reg_write(solo_dev, SOLO_PLL_CONFIG,
+			       (1 << 20) | /* PLL_RANGE */
+			       (8 << 15) | /* PLL_DIVR  */
+			       (divq << 12) |
+			       (divf <<  4) |
+			       (1 <<  1)   /* PLL_FSEN */);
+
+		solo_dev->sys_config = SOLO_SYS_CFG_SDRAM64BIT;
+	}
+
+	solo_reg_write(solo_dev, SOLO_SYS_CFG, solo_dev->sys_config);
+	solo_reg_write(solo_dev, SOLO_TIMER_CLOCK_NUM,
+		       solo_dev->clock_mhz - 1);
+
+	/* PLL locking time of 1ms */
+	mdelay(1);
+
+	ret = request_irq(pdev->irq, solo_isr, IRQF_SHARED, SOLO6X10_NAME,
+			  solo_dev);
+	if (ret)
+		goto fail_probe;
+
+	/* Handle this from the start */
+	solo_irq_on(solo_dev, SOLO_IRQ_PCI_ERR);
+
+	ret = solo_i2c_init(solo_dev);
+	if (ret)
+		goto fail_probe;
+
+	/* Setup the DMA engine */
+	solo_reg_write(solo_dev, SOLO_DMA_CTRL,
+		       SOLO_DMA_CTRL_REFRESH_CYCLE(1) |
+		       SOLO_DMA_CTRL_SDRAM_SIZE(2) |
+		       SOLO_DMA_CTRL_SDRAM_CLK_INVERT |
+		       SOLO_DMA_CTRL_READ_CLK_SELECT |
+		       SOLO_DMA_CTRL_LATENCY(1));
+
+	/* Undocumented crap */
+	solo_reg_write(solo_dev, SOLO_DMA_CTRL1,
+		       solo_dev->type == SOLO_DEV_6010 ? 0x100 : 0x300);
+
+	if (solo_dev->type != SOLO_DEV_6010) {
+		solo_dev->usec_lsb = 0x3f;
+		solo_set_time(solo_dev);
+	}
+
+	/* Disable watchdog */
+	solo_reg_write(solo_dev, SOLO_WATCHDOG, 0);
+
+	/* Initialize sub components */
+
+	ret = solo_p2m_init(solo_dev);
+	if (ret)
+		goto fail_probe;
+
+	ret = solo_disp_init(solo_dev);
+	if (ret)
+		goto fail_probe;
+
+	ret = solo_gpio_init(solo_dev);
+	if (ret)
+		goto fail_probe;
+
+	ret = solo_tw28_init(solo_dev);
+	if (ret)
+		goto fail_probe;
+
+	ret = solo_v4l2_init(solo_dev, video_nr);
+	if (ret)
+		goto fail_probe;
+
+	ret = solo_enc_init(solo_dev);
+	if (ret)
+		goto fail_probe;
+
+	ret = solo_enc_v4l2_init(solo_dev, video_nr);
+	if (ret)
+		goto fail_probe;
+
+	ret = solo_g723_init(solo_dev);
+	if (ret)
+		goto fail_probe;
+
+	ret = solo_sysfs_init(solo_dev);
+	if (ret)
+		goto fail_probe;
+
+	/* Now that init is over, set this lower */
+	solo_dev->p2m_jiffies = msecs_to_jiffies(20);
+
+	return 0;
+
+fail_probe:
+	free_solo_dev(solo_dev);
+	return ret;
+}
+
+static void solo_pci_remove(struct pci_dev *pdev)
+{
+	struct v4l2_device *v4l2_dev = pci_get_drvdata(pdev);
+	struct solo_dev *solo_dev = container_of(v4l2_dev, struct solo_dev, v4l2_dev);
+
+	free_solo_dev(solo_dev);
+}
+
+static DEFINE_PCI_DEVICE_TABLE(solo_id_table) = {
+	/* 6010 based cards */
+	{ PCI_DEVICE(PCI_VENDOR_ID_SOFTLOGIC, PCI_DEVICE_ID_SOLO6010),
+	  .driver_data = SOLO_DEV_6010 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_NEUSOLO_4),
+	  .driver_data = SOLO_DEV_6010 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_NEUSOLO_9),
+	  .driver_data = SOLO_DEV_6010 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_NEUSOLO_16),
+	  .driver_data = SOLO_DEV_6010 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_BC_SOLO_4),
+	  .driver_data = SOLO_DEV_6010 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_BC_SOLO_9),
+	  .driver_data = SOLO_DEV_6010 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_BC_SOLO_16),
+	  .driver_data = SOLO_DEV_6010 },
+	/* 6110 based cards */
+	{ PCI_DEVICE(PCI_VENDOR_ID_SOFTLOGIC, PCI_DEVICE_ID_SOLO6110),
+	  .driver_data = SOLO_DEV_6110 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_BC_6110_4),
+	  .driver_data = SOLO_DEV_6110 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_BC_6110_8),
+	  .driver_data = SOLO_DEV_6110 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_BC_6110_16),
+	  .driver_data = SOLO_DEV_6110 },
+	{0,}
+};
+
+MODULE_DEVICE_TABLE(pci, solo_id_table);
+
+static struct pci_driver solo_pci_driver = {
+	.name = SOLO6X10_NAME,
+	.id_table = solo_id_table,
+	.probe = solo_pci_probe,
+	.remove = solo_pci_remove,
+};
+
+module_pci_driver(solo_pci_driver);
diff --git a/drivers/staging/media/solo6x10/disp.c b/drivers/staging/media/solo6x10/solo6x10-disp.c
index 884c0eb757c..32d9953bc36 100644
--- a/drivers/staging/media/solo6x10/disp.c
+++ b/drivers/staging/media/solo6x10/solo6x10-disp.c
@@ -1,6 +1,11 @@
 /*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.net>
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.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
@@ -21,6 +26,7 @@
 #include <linux/module.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-ioctl.h>
+
 #include "solo6x10.h"
 
 #define SOLO_VCLK_DELAY			3
@@ -30,12 +36,8 @@
 #define SOLO_MOT_THRESH_H		64
 #define SOLO_MOT_THRESH_SIZE		8192
 #define SOLO_MOT_THRESH_REAL		(SOLO_MOT_THRESH_W * SOLO_MOT_THRESH_H)
-#define SOLO_MOT_FLAG_SIZE		512
-#define SOLO_MOT_FLAG_AREA		(SOLO_MOT_FLAG_SIZE * 32)
-
-static unsigned video_type;
-module_param(video_type, uint, 0644);
-MODULE_PARM_DESC(video_type, "video_type (0 = NTSC/Default, 1 = PAL)");
+#define SOLO_MOT_FLAG_SIZE		1024
+#define SOLO_MOT_FLAG_AREA		(SOLO_MOT_FLAG_SIZE * 16)
 
 static void solo_vin_config(struct solo_dev *solo_dev)
 {
@@ -73,7 +75,12 @@ static void solo_vin_config(struct solo_dev *solo_dev)
 	solo_reg_write(solo_dev, SOLO_VI_CH_FORMAT,
 		       SOLO_VI_FD_SEL_MASK(0) | SOLO_VI_PROG_MASK(0));
 
-	solo_reg_write(solo_dev, SOLO_VI_FMT_CFG, 0);
+	/* On 6110, initialize mozaic darkness stength */
+	if (solo_dev->type == SOLO_DEV_6010)
+		solo_reg_write(solo_dev, SOLO_VI_FMT_CFG, 0);
+	else
+		solo_reg_write(solo_dev, SOLO_VI_FMT_CFG, 16 << 22);
+
 	solo_reg_write(solo_dev, SOLO_VI_PAGE_SW, 2);
 
 	if (solo_dev->video_type == SOLO_VO_FMT_TYPE_NTSC) {
@@ -97,21 +104,30 @@ static void solo_vin_config(struct solo_dev *solo_dev)
 		       SOLO_VI_PB_HSTOP(16 + 720));
 }
 
-static void solo_disp_config(struct solo_dev *solo_dev)
+static void solo_vout_config_cursor(struct solo_dev *dev)
+{
+	int i;
+
+	/* Load (blank) cursor bitmap mask (2bpp) */
+	for (i = 0; i < 20; i++)
+		solo_reg_write(dev, SOLO_VO_CURSOR_MASK(i), 0);
+
+	solo_reg_write(dev, SOLO_VO_CURSOR_POS, 0);
+
+	solo_reg_write(dev, SOLO_VO_CURSOR_CLR,
+		       (0x80 << 24) | (0x80 << 16) | (0x10 << 8) | 0x80);
+	solo_reg_write(dev, SOLO_VO_CURSOR_CLR2, (0xe0 << 8) | 0x80);
+}
+
+static void solo_vout_config(struct solo_dev *solo_dev)
 {
 	solo_dev->vout_hstart = 6;
 	solo_dev->vout_vstart = 8;
 
-	solo_reg_write(solo_dev, SOLO_VO_BORDER_LINE_COLOR,
-		       (0xa0 << 24) | (0x88 << 16) | (0xa0 << 8) | 0x88);
-	solo_reg_write(solo_dev, SOLO_VO_BORDER_FILL_COLOR,
-		       (0x10 << 24) | (0x8f << 16) | (0x10 << 8) | 0x8f);
-	solo_reg_write(solo_dev, SOLO_VO_BKG_COLOR,
-		       (16 << 24) | (128 << 16) | (16 << 8) | 128);
-
 	solo_reg_write(solo_dev, SOLO_VO_FMT_ENC,
 		       solo_dev->video_type |
 		       SOLO_VO_USER_COLOR_SET_NAV |
+		       SOLO_VO_USER_COLOR_SET_NAH |
 		       SOLO_VO_NA_COLOR_Y(0) |
 		       SOLO_VO_NA_COLOR_CB(0) |
 		       SOLO_VO_NA_COLOR_CR(0));
@@ -130,19 +146,31 @@ static void solo_disp_config(struct solo_dev *solo_dev)
 		       SOLO_VO_H_LEN(solo_dev->video_hsize) |
 		       SOLO_VO_V_LEN(solo_dev->video_vsize));
 
-	solo_reg_write(solo_dev, SOLO_VI_WIN_SW, 5);
+	/* Border & background colors */
+	solo_reg_write(solo_dev, SOLO_VO_BORDER_LINE_COLOR,
+		       (0xa0 << 24) | (0x88 << 16) | (0xa0 << 8) | 0x88);
+	solo_reg_write(solo_dev, SOLO_VO_BORDER_FILL_COLOR,
+		       (0x10 << 24) | (0x8f << 16) | (0x10 << 8) | 0x8f);
+	solo_reg_write(solo_dev, SOLO_VO_BKG_COLOR,
+		       (16 << 24) | (128 << 16) | (16 << 8) | 128);
+
+	solo_reg_write(solo_dev, SOLO_VO_DISP_ERASE, SOLO_VO_DISP_ERASE_ON);
+
+	solo_reg_write(solo_dev, SOLO_VI_WIN_SW, 0);
+
+	solo_reg_write(solo_dev, SOLO_VO_ZOOM_CTRL, 0);
+	solo_reg_write(solo_dev, SOLO_VO_FREEZE_CTRL, 0);
 
 	solo_reg_write(solo_dev, SOLO_VO_DISP_CTRL, SOLO_VO_DISP_ON |
 		       SOLO_VO_DISP_ERASE_COUNT(8) |
 		       SOLO_VO_DISP_BASE(SOLO_DISP_EXT_ADDR));
 
-	solo_reg_write(solo_dev, SOLO_VO_DISP_ERASE, SOLO_VO_DISP_ERASE_ON);
 
-	/* Enable channels we support */
-	solo_reg_write(solo_dev, SOLO_VI_CH_ENA, (1 << solo_dev->nr_chans) - 1);
+	solo_vout_config_cursor(solo_dev);
 
-	/* Disable the watchdog */
-	solo_reg_write(solo_dev, SOLO_WATCHDOG, 0);
+	/* Enable channels we support */
+	solo_reg_write(solo_dev, SOLO_VI_CH_ENA,
+		       (1 << solo_dev->nr_chans) - 1);
 }
 
 static int solo_dma_vin_region(struct solo_dev *solo_dev, u32 off,
@@ -153,29 +181,48 @@ static int solo_dma_vin_region(struct solo_dev *solo_dev, u32 off,
 	int ret = 0;
 
 	for (i = 0; i < sizeof(buf) >> 1; i++)
-		buf[i] = val;
+		buf[i] = cpu_to_le16(val);
 
 	for (i = 0; i < reg_size; i += sizeof(buf))
-		ret |= solo_p2m_dma(solo_dev, SOLO_P2M_DMA_ID_VIN, 1, buf,
+		ret |= solo_p2m_dma(solo_dev, 1, buf,
 				    SOLO_MOTION_EXT_ADDR(solo_dev) + off + i,
-				    sizeof(buf));
+				    sizeof(buf), 0, 0);
 
 	return ret;
 }
 
-void solo_set_motion_threshold(struct solo_dev *solo_dev, u8 ch, u16 val)
+int solo_set_motion_threshold(struct solo_dev *solo_dev, u8 ch, u16 val)
 {
 	if (ch > solo_dev->nr_chans)
-		return;
+		return -EINVAL;
 
-	solo_dma_vin_region(solo_dev, SOLO_MOT_FLAG_AREA +
-			    (ch * SOLO_MOT_THRESH_SIZE * 2),
-			    val, SOLO_MOT_THRESH_REAL);
+	return solo_dma_vin_region(solo_dev, SOLO_MOT_FLAG_AREA +
+				   (ch * SOLO_MOT_THRESH_SIZE * 2),
+				   val, SOLO_MOT_THRESH_SIZE);
+}
+
+int solo_set_motion_block(struct solo_dev *solo_dev, u8 ch,
+		const struct solo_motion_thresholds *thresholds)
+{
+	u32 off = SOLO_MOT_FLAG_AREA + ch * SOLO_MOT_THRESH_SIZE * 2;
+	u16 buf[64];
+	int x, y;
+	int ret = 0;
+
+	memset(buf, 0, sizeof(buf));
+	for (y = 0; y < SOLO_MOTION_SZ; y++) {
+		for (x = 0; x < SOLO_MOTION_SZ; x++)
+			buf[x] = cpu_to_le16(thresholds->thresholds[y][x]);
+		ret |= solo_p2m_dma(solo_dev, 1, buf,
+			SOLO_MOTION_EXT_ADDR(solo_dev) + off + y * sizeof(buf),
+			sizeof(buf), 0, 0);
+	}
+	return ret;
 }
 
 /* First 8k is motion flag (512 bytes * 16). Following that is an 8k+8k
  * threshold and working table for each channel. Atleast that's what the
- * spec says. However, this code (take from rdk) has some mystery 8k
+ * spec says. However, this code (taken from rdk) has some mystery 8k
  * block right after the flag area, before the first thresh table. */
 static void solo_motion_config(struct solo_dev *solo_dev)
 {
@@ -188,9 +235,9 @@ static void solo_motion_config(struct solo_dev *solo_dev)
 
 		/* Clear working cache table */
 		solo_dma_vin_region(solo_dev, SOLO_MOT_FLAG_AREA +
-				    SOLO_MOT_THRESH_SIZE +
-				    (i * SOLO_MOT_THRESH_SIZE * 2),
-				    0x0000, SOLO_MOT_THRESH_REAL);
+				    (i * SOLO_MOT_THRESH_SIZE * 2) +
+				    SOLO_MOT_THRESH_SIZE, 0x0000,
+				    SOLO_MOT_THRESH_SIZE);
 
 		/* Set default threshold table */
 		solo_set_motion_threshold(solo_dev, i, SOLO_DEF_MOT_THRESH);
@@ -202,8 +249,8 @@ static void solo_motion_config(struct solo_dev *solo_dev)
 	solo_reg_write(solo_dev, SOLO_VI_MOT_CTRL,
 		       SOLO_VI_MOTION_FRAME_COUNT(3) |
 		       SOLO_VI_MOTION_SAMPLE_LENGTH(solo_dev->video_hsize / 16)
-		       | /* SOLO_VI_MOTION_INTR_START_STOP | */
-		       SOLO_VI_MOTION_SAMPLE_COUNT(10));
+		       /* | SOLO_VI_MOTION_INTR_START_STOP */
+		       | SOLO_VI_MOTION_SAMPLE_COUNT(10));
 
 	solo_reg_write(solo_dev, SOLO_VI_MOTION_BORDER, 0);
 	solo_reg_write(solo_dev, SOLO_VI_MOTION_BAR, 0);
@@ -214,19 +261,17 @@ int solo_disp_init(struct solo_dev *solo_dev)
 	int i;
 
 	solo_dev->video_hsize = 704;
-	if (video_type == 0) {
-		solo_dev->video_type = SOLO_VO_FMT_TYPE_NTSC;
+	if (solo_dev->video_type == SOLO_VO_FMT_TYPE_NTSC) {
 		solo_dev->video_vsize = 240;
 		solo_dev->fps = 30;
 	} else {
-		solo_dev->video_type = SOLO_VO_FMT_TYPE_PAL;
 		solo_dev->video_vsize = 288;
 		solo_dev->fps = 25;
 	}
 
 	solo_vin_config(solo_dev);
 	solo_motion_config(solo_dev);
-	solo_disp_config(solo_dev);
+	solo_vout_config(solo_dev);
 
 	for (i = 0; i < solo_dev->nr_chans; i++)
 		solo_reg_write(solo_dev, SOLO_VI_WIN_ON(i), 1);
@@ -238,8 +283,6 @@ void solo_disp_exit(struct solo_dev *solo_dev)
 {
 	int i;
 
-	solo_irq_off(solo_dev, SOLO_IRQ_MOTION);
-
 	solo_reg_write(solo_dev, SOLO_VO_DISP_CTRL, 0);
 	solo_reg_write(solo_dev, SOLO_VO_ZOOM_CTRL, 0);
 	solo_reg_write(solo_dev, SOLO_VO_FREEZE_CTRL, 0);
diff --git a/drivers/staging/media/solo6x10/solo6x10-eeprom.c b/drivers/staging/media/solo6x10/solo6x10-eeprom.c
new file mode 100644
index 00000000000..9d1c9bb53d6
--- /dev/null
+++ b/drivers/staging/media/solo6x10/solo6x10-eeprom.c
@@ -0,0 +1,154 @@
+/*
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.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., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/delay.h>
+
+#include "solo6x10.h"
+
+/* Control */
+#define EE_SHIFT_CLK	0x04
+#define EE_CS		0x08
+#define EE_DATA_WRITE	0x02
+#define EE_DATA_READ	0x01
+#define EE_ENB		(0x80 | EE_CS)
+
+#define eeprom_delay()	udelay(100)
+#if 0
+#define eeprom_delay()	solo_reg_read(solo_dev, SOLO_EEPROM_CTRL)
+#define eeprom_delay()	({				\
+	int i, ret;					\
+	udelay(100);					\
+	for (i = ret = 0; i < 1000 && !ret; i++)	\
+		ret = solo_eeprom_reg_read(solo_dev);	\
+})
+#endif
+#define ADDR_LEN	6
+
+/* Commands */
+#define EE_EWEN_CMD	4
+#define EE_EWDS_CMD	4
+#define EE_WRITE_CMD	5
+#define EE_READ_CMD	6
+#define EE_ERASE_CMD	7
+
+static unsigned int solo_eeprom_reg_read(struct solo_dev *solo_dev)
+{
+	return solo_reg_read(solo_dev, SOLO_EEPROM_CTRL) & EE_DATA_READ;
+}
+
+static void solo_eeprom_reg_write(struct solo_dev *solo_dev, u32 data)
+{
+	solo_reg_write(solo_dev, SOLO_EEPROM_CTRL, data);
+	eeprom_delay();
+}
+
+static void solo_eeprom_cmd(struct solo_dev *solo_dev, int cmd)
+{
+	int i;
+
+	solo_eeprom_reg_write(solo_dev, SOLO_EEPROM_ACCESS_EN);
+	solo_eeprom_reg_write(solo_dev, SOLO_EEPROM_ENABLE);
+
+	for (i = 4 + ADDR_LEN; i >= 0; i--) {
+		int dataval = (cmd & (1 << i)) ? EE_DATA_WRITE : 0;
+
+		solo_eeprom_reg_write(solo_dev, SOLO_EEPROM_ENABLE | dataval);
+		solo_eeprom_reg_write(solo_dev, SOLO_EEPROM_ENABLE |
+				      EE_SHIFT_CLK | dataval);
+	}
+
+	solo_eeprom_reg_write(solo_dev, SOLO_EEPROM_ENABLE);
+}
+
+unsigned int solo_eeprom_ewen(struct solo_dev *solo_dev, int w_en)
+{
+	int ewen_cmd = (w_en ? 0x3f : 0) | (EE_EWEN_CMD << ADDR_LEN);
+	unsigned int retval = 0;
+	int i;
+
+	solo_eeprom_cmd(solo_dev, ewen_cmd);
+
+	for (i = 0; i < 16; i++) {
+		solo_eeprom_reg_write(solo_dev, SOLO_EEPROM_ENABLE |
+				      EE_SHIFT_CLK);
+		retval = (retval << 1) | solo_eeprom_reg_read(solo_dev);
+		solo_eeprom_reg_write(solo_dev, SOLO_EEPROM_ENABLE);
+		retval = (retval << 1) | solo_eeprom_reg_read(solo_dev);
+	}
+
+	solo_eeprom_reg_write(solo_dev, ~EE_CS);
+	retval = (retval << 1) | solo_eeprom_reg_read(solo_dev);
+
+	return retval;
+}
+
+unsigned short solo_eeprom_read(struct solo_dev *solo_dev, int loc)
+{
+	int read_cmd = loc | (EE_READ_CMD << ADDR_LEN);
+	unsigned short retval = 0;
+	int i;
+
+	solo_eeprom_cmd(solo_dev, read_cmd);
+
+	for (i = 0; i < 16; i++) {
+		solo_eeprom_reg_write(solo_dev, SOLO_EEPROM_ENABLE |
+				      EE_SHIFT_CLK);
+		retval = (retval << 1) | solo_eeprom_reg_read(solo_dev);
+		solo_eeprom_reg_write(solo_dev, SOLO_EEPROM_ENABLE);
+	}
+
+	solo_eeprom_reg_write(solo_dev, ~EE_CS);
+
+	return retval;
+}
+
+int solo_eeprom_write(struct solo_dev *solo_dev, int loc,
+		      unsigned short data)
+{
+	int write_cmd = loc | (EE_WRITE_CMD << ADDR_LEN);
+	unsigned int retval;
+	int i;
+
+	solo_eeprom_cmd(solo_dev, write_cmd);
+
+	for (i = 15; i >= 0; i--) {
+		unsigned int dataval = (data >> i) & 1;
+
+		solo_eeprom_reg_write(solo_dev, EE_ENB);
+		solo_eeprom_reg_write(solo_dev,
+				      EE_ENB | (dataval << 1) | EE_SHIFT_CLK);
+	}
+
+	solo_eeprom_reg_write(solo_dev, EE_ENB);
+	solo_eeprom_reg_write(solo_dev, ~EE_CS);
+	solo_eeprom_reg_write(solo_dev, EE_ENB);
+
+	for (i = retval = 0; i < 10000 && !retval; i++)
+		retval = solo_eeprom_reg_read(solo_dev);
+
+	solo_eeprom_reg_write(solo_dev, ~EE_CS);
+
+	return !retval;
+}
diff --git a/drivers/staging/media/solo6x10/enc.c b/drivers/staging/media/solo6x10/solo6x10-enc.c
index de502599bb1..94d5735abf8 100644
--- a/drivers/staging/media/solo6x10/enc.c
+++ b/drivers/staging/media/solo6x10/solo6x10-enc.c
@@ -1,6 +1,11 @@
 /*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.net>
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.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
@@ -18,30 +23,41 @@
  */
 
 #include <linux/kernel.h>
+#include <linux/font.h>
+#include <linux/bitrev.h>
 #include <linux/slab.h>
-#include "solo6x10.h"
-#include "osd-font.h"
 
-#define CAPTURE_MAX_BANDWIDTH		32	/* D1 4channel (D1 == 4) */
-#define OSG_BUFFER_SIZE			1024
+#include "solo6x10.h"
 
 #define VI_PROG_HSIZE			(1280 - 16)
 #define VI_PROG_VSIZE			(1024 - 16)
 
+#define IRQ_LEVEL			2
+
 static void solo_capture_config(struct solo_dev *solo_dev)
 {
-	int i, j;
 	unsigned long height;
 	unsigned long width;
-	unsigned char *buf;
+	void *buf;
+	int i;
 
 	solo_reg_write(solo_dev, SOLO_CAP_BASE,
-		       SOLO_CAP_MAX_PAGE(SOLO_CAP_EXT_MAX_PAGE *
-					 solo_dev->nr_chans) |
-		       SOLO_CAP_BASE_ADDR(SOLO_CAP_EXT_ADDR(solo_dev) >> 16));
-	solo_reg_write(solo_dev, SOLO_CAP_BTW,
-		       (1 << 17) | SOLO_CAP_PROG_BANDWIDTH(2) |
-		       SOLO_CAP_MAX_BANDWIDTH(CAPTURE_MAX_BANDWIDTH));
+		       SOLO_CAP_MAX_PAGE((SOLO_CAP_EXT_SIZE(solo_dev)
+					  - SOLO_CAP_PAGE_SIZE) >> 16)
+		       | SOLO_CAP_BASE_ADDR(SOLO_CAP_EXT_ADDR(solo_dev) >> 16));
+
+	/* XXX: Undocumented bits at b17 and b24 */
+	if (solo_dev->type == SOLO_DEV_6110) {
+		/* NOTE: Ref driver has (62 << 24) here as well, but it causes
+		 * wacked out frame timing on 4-port 6110. */
+		solo_reg_write(solo_dev, SOLO_CAP_BTW,
+			       (1 << 17) | SOLO_CAP_PROG_BANDWIDTH(2) |
+			       SOLO_CAP_MAX_BANDWIDTH(36));
+	} else {
+		solo_reg_write(solo_dev, SOLO_CAP_BTW,
+			       (1 << 17) | SOLO_CAP_PROG_BANDWIDTH(2) |
+			       SOLO_CAP_MAX_BANDWIDTH(32));
+	}
 
 	/* Set scale 1, 9 dimension */
 	width = solo_dev->video_hsize;
@@ -96,115 +112,212 @@ static void solo_capture_config(struct solo_dev *solo_dev)
 	solo_reg_write(solo_dev, SOLO_VE_OSD_BASE, SOLO_EOSD_EXT_ADDR >> 16);
 	solo_reg_write(solo_dev, SOLO_VE_OSD_CLR,
 		       0xF0 << 16 | 0x80 << 8 | 0x80);
-	solo_reg_write(solo_dev, SOLO_VE_OSD_OPT, 0);
+
+	if (solo_dev->type == SOLO_DEV_6010)
+		solo_reg_write(solo_dev, SOLO_VE_OSD_OPT,
+			       SOLO_VE_OSD_H_SHADOW | SOLO_VE_OSD_V_SHADOW);
+	else
+		solo_reg_write(solo_dev, SOLO_VE_OSD_OPT, SOLO_VE_OSD_V_DOUBLE
+			       | SOLO_VE_OSD_H_SHADOW | SOLO_VE_OSD_V_SHADOW);
 
 	/* Clear OSG buffer */
-	buf = kzalloc(OSG_BUFFER_SIZE, GFP_KERNEL);
+	buf = kzalloc(SOLO_EOSD_EXT_SIZE(solo_dev), GFP_KERNEL);
 	if (!buf)
 		return;
 
 	for (i = 0; i < solo_dev->nr_chans; i++) {
-		for (j = 0; j < SOLO_EOSD_EXT_SIZE; j += OSG_BUFFER_SIZE) {
-			solo_p2m_dma(solo_dev, SOLO_P2M_DMA_ID_MP4E, 1, buf,
-				     SOLO_EOSD_EXT_ADDR +
-				     (i * SOLO_EOSD_EXT_SIZE) + j,
-				     OSG_BUFFER_SIZE);
-		}
+		solo_p2m_dma(solo_dev, 1, buf,
+			     SOLO_EOSD_EXT_ADDR +
+			     (SOLO_EOSD_EXT_SIZE(solo_dev) * i),
+			     SOLO_EOSD_EXT_SIZE(solo_dev), 0, 0);
 	}
 	kfree(buf);
 }
 
+/* Should be called with enable_lock held */
 int solo_osd_print(struct solo_enc_dev *solo_enc)
 {
 	struct solo_dev *solo_dev = solo_enc->solo_dev;
-	char *str = solo_enc->osd_text;
-	u8 *buf;
+	unsigned char *str = solo_enc->osd_text;
+	u8 *buf = solo_enc->osd_buf;
 	u32 reg = solo_reg_read(solo_dev, SOLO_VE_OSD_CH);
-	int len = strlen(str);
+	const struct font_desc *vga = find_font("VGA8x16");
+	const unsigned char *vga_data;
+	int len;
 	int i, j;
-	int x = 1, y = 1;
+
+	if (WARN_ON_ONCE(!vga))
+		return -ENODEV;
+
+	len = strlen(str);
 
 	if (len == 0) {
+		/* Disable OSD on this channel */
 		reg &= ~(1 << solo_enc->ch);
 		solo_reg_write(solo_dev, SOLO_VE_OSD_CH, reg);
 		return 0;
 	}
 
-	buf = kzalloc(SOLO_EOSD_EXT_SIZE, GFP_KERNEL);
-	if (!buf)
-		return -ENOMEM;
+	memset(buf, 0, SOLO_EOSD_EXT_SIZE_MAX);
+	vga_data = (const unsigned char *)vga->data;
 
 	for (i = 0; i < len; i++) {
+		unsigned char c = str[i];
+
 		for (j = 0; j < 16; j++) {
-			buf[(j*2) + (i%2) + ((x + (i/2)) * 32) + (y * 2048)] =
-				(solo_osd_font[(str[i] * 4) + (j / 4)]
-					>> ((3 - (j % 4)) * 8)) & 0xff;
+			buf[(j * 2) + (i % 2) + (i / 2 * 32)] =
+				bitrev8(vga_data[(c * 16) + j]);
 		}
 	}
 
-	solo_p2m_dma(solo_dev, 0, 1, buf, SOLO_EOSD_EXT_ADDR +
-		     (solo_enc->ch * SOLO_EOSD_EXT_SIZE), SOLO_EOSD_EXT_SIZE);
+	solo_p2m_dma(solo_dev, 1, buf,
+		     SOLO_EOSD_EXT_ADDR +
+		     (solo_enc->ch * SOLO_EOSD_EXT_SIZE(solo_dev)),
+		     SOLO_EOSD_EXT_SIZE(solo_dev), 0, 0);
+
+	/* Enable OSD on this channel */
 	reg |= (1 << solo_enc->ch);
 	solo_reg_write(solo_dev, SOLO_VE_OSD_CH, reg);
 
-	kfree(buf);
-
 	return 0;
 }
 
+/**
+ * Set channel Quality Profile (0-3).
+ */
+void solo_s_jpeg_qp(struct solo_dev *solo_dev, unsigned int ch,
+		    unsigned int qp)
+{
+	unsigned long flags;
+	unsigned int idx, reg;
+
+	if ((ch > 31) || (qp > 3))
+		return;
+
+	if (solo_dev->type == SOLO_DEV_6010)
+		return;
+
+	if (ch < 16) {
+		idx = 0;
+		reg = SOLO_VE_JPEG_QP_CH_L;
+	} else {
+		ch -= 16;
+		idx = 1;
+		reg = SOLO_VE_JPEG_QP_CH_H;
+	}
+	ch *= 2;
+
+	spin_lock_irqsave(&solo_dev->jpeg_qp_lock, flags);
+
+	solo_dev->jpeg_qp[idx] &= ~(3 << ch);
+	solo_dev->jpeg_qp[idx] |= (qp & 3) << ch;
+
+	solo_reg_write(solo_dev, reg, solo_dev->jpeg_qp[idx]);
+
+	spin_unlock_irqrestore(&solo_dev->jpeg_qp_lock, flags);
+}
+
+int solo_g_jpeg_qp(struct solo_dev *solo_dev, unsigned int ch)
+{
+	int idx;
+
+	if (solo_dev->type == SOLO_DEV_6010)
+		return 2;
+
+	if (WARN_ON_ONCE(ch > 31))
+		return 2;
+
+	if (ch < 16) {
+		idx = 0;
+	} else {
+		ch -= 16;
+		idx = 1;
+	}
+	ch *= 2;
+
+	return (solo_dev->jpeg_qp[idx] >> ch) & 3;
+}
+
+#define SOLO_QP_INIT 0xaaaaaaaa
+
 static void solo_jpeg_config(struct solo_dev *solo_dev)
 {
-	u32 reg;
-	if (solo_dev->flags & FLAGS_6110)
-		reg = (4 << 24) | (3 << 16) | (2 << 8) | (1 << 0);
-	else
-		reg = (2 << 24) | (2 << 16) | (2 << 8) | (2 << 0);
-	solo_reg_write(solo_dev, SOLO_VE_JPEG_QP_TBL, reg);
-	solo_reg_write(solo_dev, SOLO_VE_JPEG_QP_CH_L, 0);
-	solo_reg_write(solo_dev, SOLO_VE_JPEG_QP_CH_H, 0);
+	if (solo_dev->type == SOLO_DEV_6010) {
+		solo_reg_write(solo_dev, SOLO_VE_JPEG_QP_TBL,
+			       (2 << 24) | (2 << 16) | (2 << 8) | 2);
+	} else {
+		solo_reg_write(solo_dev, SOLO_VE_JPEG_QP_TBL,
+			       (4 << 24) | (3 << 16) | (2 << 8) | 1);
+	}
+
+	spin_lock_init(&solo_dev->jpeg_qp_lock);
+
+	/* Initialize Quality Profile for all channels */
+	solo_dev->jpeg_qp[0] = solo_dev->jpeg_qp[1] = SOLO_QP_INIT;
+	solo_reg_write(solo_dev, SOLO_VE_JPEG_QP_CH_L, SOLO_QP_INIT);
+	solo_reg_write(solo_dev, SOLO_VE_JPEG_QP_CH_H, SOLO_QP_INIT);
+
 	solo_reg_write(solo_dev, SOLO_VE_JPEG_CFG,
 		(SOLO_JPEG_EXT_SIZE(solo_dev) & 0xffff0000) |
 		((SOLO_JPEG_EXT_ADDR(solo_dev) >> 16) & 0x0000ffff));
 	solo_reg_write(solo_dev, SOLO_VE_JPEG_CTRL, 0xffffffff);
-	/* que limit, samp limit, pos limit */
-	solo_reg_write(solo_dev, 0x0688, (0 << 16) | (30 << 8) | 60);
+	if (solo_dev->type == SOLO_DEV_6110) {
+		solo_reg_write(solo_dev, SOLO_VE_JPEG_CFG1,
+			       (0 << 16) | (30 << 8) | 60);
+	}
 }
 
 static void solo_mp4e_config(struct solo_dev *solo_dev)
 {
 	int i;
-	u32 reg;
+	u32 cfg;
 
-	/* We can only use VE_INTR_CTRL(0) if we want to support mjpeg */
 	solo_reg_write(solo_dev, SOLO_VE_CFG0,
-		       SOLO_VE_INTR_CTRL(0) |
+		       SOLO_VE_INTR_CTRL(IRQ_LEVEL) |
 		       SOLO_VE_BLOCK_SIZE(SOLO_MP4E_EXT_SIZE(solo_dev) >> 16) |
 		       SOLO_VE_BLOCK_BASE(SOLO_MP4E_EXT_ADDR(solo_dev) >> 16));
 
-	solo_reg_write(solo_dev, SOLO_VE_CFG1,
-		       SOLO_VE_INSERT_INDEX | SOLO_VE_MOTION_MODE(0));
+
+	cfg = SOLO_VE_BYTE_ALIGN(2) | SOLO_VE_INSERT_INDEX
+		| SOLO_VE_MOTION_MODE(0);
+	if (solo_dev->type != SOLO_DEV_6010) {
+		cfg |= SOLO_VE_MPEG_SIZE_H(
+			(SOLO_MP4E_EXT_SIZE(solo_dev) >> 24) & 0x0f);
+		cfg |= SOLO_VE_JPEG_SIZE_H(
+			(SOLO_JPEG_EXT_SIZE(solo_dev) >> 24) & 0x0f);
+	}
+	solo_reg_write(solo_dev, SOLO_VE_CFG1, cfg);
 
 	solo_reg_write(solo_dev, SOLO_VE_WMRK_POLY, 0);
 	solo_reg_write(solo_dev, SOLO_VE_VMRK_INIT_KEY, 0);
 	solo_reg_write(solo_dev, SOLO_VE_WMRK_STRL, 0);
+	if (solo_dev->type == SOLO_DEV_6110)
+		solo_reg_write(solo_dev, SOLO_VE_WMRK_ENABLE, 0);
 	solo_reg_write(solo_dev, SOLO_VE_ENCRYP_POLY, 0);
 	solo_reg_write(solo_dev, SOLO_VE_ENCRYP_INIT, 0);
 
-	reg = SOLO_VE_LITTLE_ENDIAN | SOLO_COMP_ATTR_FCODE(1) |
-		SOLO_COMP_TIME_INC(0) | SOLO_COMP_TIME_WIDTH(15);
-	if (solo_dev->flags & FLAGS_6110)
-		reg |= SOLO_DCT_INTERVAL(10);
-	else
-		reg |= SOLO_DCT_INTERVAL(36 / 4);
-	solo_reg_write(solo_dev, SOLO_VE_ATTR, reg);
+	solo_reg_write(solo_dev, SOLO_VE_ATTR,
+		       SOLO_VE_LITTLE_ENDIAN |
+		       SOLO_COMP_ATTR_FCODE(1) |
+		       SOLO_COMP_TIME_INC(0) |
+		       SOLO_COMP_TIME_WIDTH(15) |
+		       SOLO_DCT_INTERVAL(solo_dev->type == SOLO_DEV_6010 ? 9 : 10));
 
-	for (i = 0; i < solo_dev->nr_chans; i++)
+	for (i = 0; i < solo_dev->nr_chans; i++) {
 		solo_reg_write(solo_dev, SOLO_VE_CH_REF_BASE(i),
 			       (SOLO_EREF_EXT_ADDR(solo_dev) +
 			       (i * SOLO_EREF_EXT_SIZE)) >> 16);
+		solo_reg_write(solo_dev, SOLO_VE_CH_REF_BASE_E(i),
+			       (SOLO_EREF_EXT_ADDR(solo_dev) +
+			       ((i + 16) * SOLO_EREF_EXT_SIZE)) >> 16);
+	}
 
-	if (solo_dev->flags & FLAGS_6110)
-		solo_reg_write(solo_dev, 0x0634, 0x00040008); /* ? */
+	if (solo_dev->type == SOLO_DEV_6110) {
+		solo_reg_write(solo_dev, SOLO_VE_COMPT_MOT, 0x00040008);
+	} else {
+		for (i = 0; i < solo_dev->nr_chans; i++)
+			solo_reg_write(solo_dev, SOLO_VE_CH_MOT(i), 0x100);
+	}
 }
 
 int solo_enc_init(struct solo_dev *solo_dev)
@@ -220,8 +333,6 @@ int solo_enc_init(struct solo_dev *solo_dev)
 		solo_reg_write(solo_dev, SOLO_CAP_CH_COMP_ENA_E(i), 0);
 	}
 
-	solo_irq_on(solo_dev, SOLO_IRQ_ENCODER);
-
 	return 0;
 }
 
@@ -229,8 +340,6 @@ void solo_enc_exit(struct solo_dev *solo_dev)
 {
 	int i;
 
-	solo_irq_off(solo_dev, SOLO_IRQ_ENCODER);
-
 	for (i = 0; i < solo_dev->nr_chans; i++) {
 		solo_reg_write(solo_dev, SOLO_CAP_CH_SCALE(i), 0);
 		solo_reg_write(solo_dev, SOLO_CAP_CH_COMP_ENA_E(i), 0);
diff --git a/drivers/staging/media/solo6x10/g723.c b/drivers/staging/media/solo6x10/solo6x10-g723.c
index 2cd0de28a63..1db18c7972a 100644
--- a/drivers/staging/media/solo6x10/g723.c
+++ b/drivers/staging/media/solo6x10/solo6x10-g723.c
@@ -1,6 +1,11 @@
 /*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.net>
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.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
@@ -21,17 +26,18 @@
 #include <linux/mempool.h>
 #include <linux/poll.h>
 #include <linux/kthread.h>
-#include <linux/slab.h>
 #include <linux/freezer.h>
-#include <linux/export.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+
 #include <sound/core.h>
 #include <sound/initval.h>
 #include <sound/pcm.h>
 #include <sound/control.h>
+
 #include "solo6x10.h"
-#include "tw28.h"
+#include "solo6x10-tw28.h"
 
-#define G723_INTR_ORDER		0
 #define G723_FDMA_PAGES		32
 #define G723_PERIOD_BYTES	48
 #define G723_PERIOD_BLOCK	1024
@@ -46,36 +52,40 @@
 /* The solo writes to 1k byte pages, 32 pages, in the dma. Each 1k page
  * is broken down to 20 * 48 byte regions (one for each channel possible)
  * with the rest of the page being dummy data. */
-#define MAX_BUFFER		(G723_PERIOD_BYTES * PERIODS_MAX)
-#define IRQ_PAGES		4 /* 0 - 4 */
-#define PERIODS_MIN		(1 << IRQ_PAGES)
+#define G723_MAX_BUFFER		(G723_PERIOD_BYTES * PERIODS_MAX)
+#define G723_INTR_ORDER		4 /* 0 - 4 */
+#define PERIODS_MIN		(1 << G723_INTR_ORDER)
 #define PERIODS_MAX		G723_FDMA_PAGES
 
 struct solo_snd_pcm {
-	int		on;
-	spinlock_t	lock;
-	struct solo_dev	*solo_dev;
-	unsigned char	g723_buf[G723_PERIOD_BYTES];
+	int				on;
+	spinlock_t			lock;
+	struct solo_dev		*solo_dev;
+	unsigned char			*g723_buf;
+	dma_addr_t			g723_dma;
 };
 
 static void solo_g723_config(struct solo_dev *solo_dev)
 {
 	int clk_div;
 
-	clk_div = SOLO_CLOCK_MHZ / (SAMPLERATE * (BITRATE * 2) * 2);
+	clk_div = (solo_dev->clock_mhz * 1000000)
+		/ (SAMPLERATE * (BITRATE * 2) * 2);
 
 	solo_reg_write(solo_dev, SOLO_AUDIO_SAMPLE,
-		       SOLO_AUDIO_BITRATE(BITRATE) |
-		       SOLO_AUDIO_CLK_DIV(clk_div));
+		       SOLO_AUDIO_BITRATE(BITRATE)
+		       | SOLO_AUDIO_CLK_DIV(clk_div));
 
 	solo_reg_write(solo_dev, SOLO_AUDIO_FDMA_INTR,
-		      SOLO_AUDIO_FDMA_INTERVAL(IRQ_PAGES) |
-		      SOLO_AUDIO_INTR_ORDER(G723_INTR_ORDER) |
-		      SOLO_AUDIO_FDMA_BASE(SOLO_G723_EXT_ADDR(solo_dev) >> 16));
+		       SOLO_AUDIO_FDMA_INTERVAL(1)
+		       | SOLO_AUDIO_INTR_ORDER(G723_INTR_ORDER)
+		       | SOLO_AUDIO_FDMA_BASE(SOLO_G723_EXT_ADDR(solo_dev) >> 16));
 
 	solo_reg_write(solo_dev, SOLO_AUDIO_CONTROL,
-		       SOLO_AUDIO_ENABLE | SOLO_AUDIO_I2S_MODE |
-		       SOLO_AUDIO_I2S_MULTI(3) | SOLO_AUDIO_MODE(OUTMODE_MASK));
+		       SOLO_AUDIO_ENABLE
+		       | SOLO_AUDIO_I2S_MODE
+		       | SOLO_AUDIO_I2S_MULTI(3)
+		       | SOLO_AUDIO_MODE(OUTMODE_MASK));
 }
 
 void solo_g723_isr(struct solo_dev *solo_dev)
@@ -85,8 +95,6 @@ void solo_g723_isr(struct solo_dev *solo_dev)
 	struct snd_pcm_substream *ss;
 	struct solo_snd_pcm *solo_pcm;
 
-	solo_reg_write(solo_dev, SOLO_IRQ_STAT, SOLO_IRQ_G723);
-
 	for (ss = pstr->substream; ss != NULL; ss = ss->next) {
 		if (snd_pcm_substream_chip(ss) == NULL)
 			continue;
@@ -115,18 +123,18 @@ static int snd_solo_hw_free(struct snd_pcm_substream *ss)
 	return snd_pcm_lib_free_pages(ss);
 }
 
-static struct snd_pcm_hardware snd_solo_pcm_hw = {
+static const struct snd_pcm_hardware snd_solo_pcm_hw = {
 	.info			= (SNDRV_PCM_INFO_MMAP |
 				   SNDRV_PCM_INFO_INTERLEAVED |
 				   SNDRV_PCM_INFO_BLOCK_TRANSFER |
 				   SNDRV_PCM_INFO_MMAP_VALID),
 	.formats		= SNDRV_PCM_FMTBIT_U8,
 	.rates			= SNDRV_PCM_RATE_8000,
-	.rate_min		= 8000,
-	.rate_max		= 8000,
+	.rate_min		= SAMPLERATE,
+	.rate_max		= SAMPLERATE,
 	.channels_min		= 1,
 	.channels_max		= 1,
-	.buffer_bytes_max	= MAX_BUFFER,
+	.buffer_bytes_max	= G723_MAX_BUFFER,
 	.period_bytes_min	= G723_PERIOD_BYTES,
 	.period_bytes_max	= G723_PERIOD_BYTES,
 	.periods_min		= PERIODS_MIN,
@@ -140,7 +148,13 @@ static int snd_solo_pcm_open(struct snd_pcm_substream *ss)
 
 	solo_pcm = kzalloc(sizeof(*solo_pcm), GFP_KERNEL);
 	if (solo_pcm == NULL)
-		return -ENOMEM;
+		goto oom;
+
+	solo_pcm->g723_buf = pci_alloc_consistent(solo_dev->pdev,
+						  G723_PERIOD_BYTES,
+						  &solo_pcm->g723_dma);
+	if (solo_pcm->g723_buf == NULL)
+		goto oom;
 
 	spin_lock_init(&solo_pcm->lock);
 	solo_pcm->solo_dev = solo_dev;
@@ -149,6 +163,10 @@ static int snd_solo_pcm_open(struct snd_pcm_substream *ss)
 	snd_pcm_substream_chip(ss) = solo_pcm;
 
 	return 0;
+
+oom:
+	kfree(solo_pcm);
+	return -ENOMEM;
 }
 
 static int snd_solo_pcm_close(struct snd_pcm_substream *ss)
@@ -156,6 +174,8 @@ static int snd_solo_pcm_close(struct snd_pcm_substream *ss)
 	struct solo_snd_pcm *solo_pcm = snd_pcm_substream_chip(ss);
 
 	snd_pcm_substream_chip(ss) = solo_pcm->solo_dev;
+	pci_free_consistent(solo_pcm->solo_dev->pdev, G723_PERIOD_BYTES,
+			    solo_pcm->g723_buf, solo_pcm->g723_dma);
 	kfree(solo_pcm);
 
 	return 0;
@@ -220,12 +240,11 @@ static int snd_solo_pcm_copy(struct snd_pcm_substream *ss, int channel,
 	for (i = 0; i < (count / G723_FRAMES_PER_PAGE); i++) {
 		int page = (pos / G723_FRAMES_PER_PAGE) + i;
 
-		err = solo_p2m_dma(solo_dev, SOLO_P2M_DMA_ID_G723E, 0,
-				   solo_pcm->g723_buf,
-				   SOLO_G723_EXT_ADDR(solo_dev) +
-				   (page * G723_PERIOD_BLOCK) +
-				   (ss->number * G723_PERIOD_BYTES),
-				   G723_PERIOD_BYTES);
+		err = solo_p2m_dma_t(solo_dev, 0, solo_pcm->g723_dma,
+				     SOLO_G723_EXT_ADDR(solo_dev) +
+				     (page * G723_PERIOD_BLOCK) +
+				     (ss->number * G723_PERIOD_BYTES),
+				     G723_PERIOD_BYTES, 0, 0);
 		if (err)
 			return err;
 
@@ -325,7 +344,7 @@ static int solo_snd_pcm_init(struct solo_dev *solo_dev)
 	ret = snd_pcm_lib_preallocate_pages_for_all(pcm,
 					SNDRV_DMA_TYPE_CONTINUOUS,
 					snd_dma_continuous_data(GFP_KERNEL),
-					MAX_BUFFER, MAX_BUFFER);
+					G723_MAX_BUFFER, G723_MAX_BUFFER);
 	if (ret < 0)
 		return ret;
 
@@ -368,6 +387,7 @@ int solo_g723_init(struct solo_dev *solo_dev)
 	strcpy(card->mixername, "SOLO-6x10");
 	kctl = snd_solo_capture_volume;
 	kctl.count = solo_dev->nr_chans;
+
 	ret = snd_ctl_add(card, snd_ctl_new1(&kctl, solo_dev));
 	if (ret < 0)
 		return ret;
@@ -393,8 +413,12 @@ snd_error:
 
 void solo_g723_exit(struct solo_dev *solo_dev)
 {
+	if (!solo_dev->snd_card)
+		return;
+
 	solo_reg_write(solo_dev, SOLO_AUDIO_CONTROL, 0);
 	solo_irq_off(solo_dev, SOLO_IRQ_G723);
 
 	snd_card_free(solo_dev->snd_card);
+	solo_dev->snd_card = NULL;
 }
diff --git a/drivers/staging/media/solo6x10/gpio.c b/drivers/staging/media/solo6x10/solo6x10-gpio.c
index 0925e6f33a9..73276dc9287 100644
--- a/drivers/staging/media/solo6x10/gpio.c
+++ b/drivers/staging/media/solo6x10/solo6x10-gpio.c
@@ -1,6 +1,11 @@
 /*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.net>
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.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
@@ -19,7 +24,9 @@
 
 #include <linux/kernel.h>
 #include <linux/fs.h>
-#include <asm/uaccess.h>
+#include <linux/delay.h>
+#include <linux/uaccess.h>
+
 #include "solo6x10.h"
 
 static void solo_gpio_mode(struct solo_dev *solo_dev,
diff --git a/drivers/staging/media/solo6x10/i2c.c b/drivers/staging/media/solo6x10/solo6x10-i2c.c
index 398070a3d29..01aa417c925 100644
--- a/drivers/staging/media/solo6x10/i2c.c
+++ b/drivers/staging/media/solo6x10/solo6x10-i2c.c
@@ -1,6 +1,11 @@
 /*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.net>
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.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
@@ -26,6 +31,7 @@
  * thread context, ACK the interrupt, and move on. -- BenC */
 
 #include <linux/kernel.h>
+
 #include "solo6x10.h"
 
 u8 solo_i2c_readbyte(struct solo_dev *solo_dev, int id, u8 addr, u8 off)
@@ -173,10 +179,9 @@ int solo_i2c_isr(struct solo_dev *solo_dev)
 	u32 status = solo_reg_read(solo_dev, SOLO_IIC_CTRL);
 	int ret = -EINVAL;
 
-	solo_reg_write(solo_dev, SOLO_IRQ_STAT, SOLO_IRQ_IIC);
 
-	if (status & (SOLO_IIC_STATE_TRNS | SOLO_IIC_STATE_SIG_ERR) ||
-	    solo_dev->i2c_id < 0) {
+	if (CHK_FLAGS(status, SOLO_IIC_STATE_TRNS | SOLO_IIC_STATE_SIG_ERR)
+	    || solo_dev->i2c_id < 0) {
 		solo_i2c_stop(solo_dev);
 		return -ENXIO;
 	}
@@ -239,7 +244,8 @@ static int solo_i2c_master_xfer(struct i2c_adapter *adap,
 	timeout = HZ / 2;
 
 	for (;;) {
-		prepare_to_wait(&solo_dev->i2c_wait, &wait, TASK_INTERRUPTIBLE);
+		prepare_to_wait(&solo_dev->i2c_wait, &wait,
+				TASK_INTERRUPTIBLE);
 
 		if (solo_dev->i2c_state == IIC_STATE_STOP)
 			break;
@@ -267,7 +273,7 @@ static u32 solo_i2c_functionality(struct i2c_adapter *adap)
 	return I2C_FUNC_I2C;
 }
 
-static struct i2c_algorithm solo_i2c_algo = {
+static const struct i2c_algorithm solo_i2c_algo = {
 	.master_xfer	= solo_i2c_master_xfer,
 	.functionality	= solo_i2c_functionality,
 };
@@ -288,7 +294,8 @@ int solo_i2c_init(struct solo_dev *solo_dev)
 	for (i = 0; i < SOLO_I2C_ADAPTERS; i++) {
 		struct i2c_adapter *adap = &solo_dev->i2c_adap[i];
 
-		snprintf(adap->name, I2C_NAME_SIZE, "%s I2C %d", SOLO6X10_NAME, i);
+		snprintf(adap->name, I2C_NAME_SIZE, "%s I2C %d",
+			 SOLO6X10_NAME, i);
 		adap->algo = &solo_i2c_algo;
 		adap->algo_data = solo_dev;
 		adap->retries = 1;
@@ -311,9 +318,6 @@ int solo_i2c_init(struct solo_dev *solo_dev)
 		return ret;
 	}
 
-	dev_info(&solo_dev->pdev->dev, "Enabled %d i2c adapters\n",
-		 SOLO_I2C_ADAPTERS);
-
 	return 0;
 }
 
diff --git a/drivers/staging/media/solo6x10/solo6x10-jpeg.h b/drivers/staging/media/solo6x10/solo6x10-jpeg.h
index 50defec318c..c5218ceeabc 100644
--- a/drivers/staging/media/solo6x10/solo6x10-jpeg.h
+++ b/drivers/staging/media/solo6x10/solo6x10-jpeg.h
@@ -1,6 +1,11 @@
 /*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.net>
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.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
@@ -20,7 +25,7 @@
 #ifndef __SOLO6X10_JPEG_H
 #define __SOLO6X10_JPEG_H
 
-static unsigned char jpeg_header[] = {
+static const unsigned char jpeg_header[] = {
 	0xff, 0xd8, 0xff, 0xfe, 0x00, 0x0d, 0x42, 0x6c,
 	0x75, 0x65, 0x63, 0x68, 0x65, 0x72, 0x72, 0x79,
 	0x20, 0xff, 0xdb, 0x00, 0x43, 0x00, 0x20, 0x16,
@@ -102,4 +107,87 @@ static unsigned char jpeg_header[] = {
 /* This is the byte marker for the start of SOF0: 0xffc0 marker */
 #define SOF0_START	575
 
+/* This is the byte marker for the start of the DQT */
+#define DQT_START	17
+#define DQT_LEN		138
+const unsigned char jpeg_dqt[4][DQT_LEN] = {
+	{
+		0xff, 0xdb, 0x00, 0x43, 0x00,
+		0x08, 0x06, 0x06, 0x07, 0x06, 0x05, 0x08, 0x07,
+		0x07, 0x07, 0x09, 0x09, 0x08, 0x0a, 0x0c, 0x14,
+		0x0d, 0x0c, 0x0b, 0x0b, 0x0c, 0x19, 0x12, 0x13,
+		0x0f, 0x14, 0x1d, 0x1a, 0x1f, 0x1e, 0x1d, 0x1a,
+		0x1c, 0x1c, 0x20, 0x24, 0x2e, 0x27, 0x20, 0x22,
+		0x2c, 0x23, 0x1c, 0x1c, 0x28, 0x37, 0x29, 0x2c,
+		0x30, 0x31, 0x34, 0x34, 0x34, 0x1f, 0x27, 0x39,
+		0x3d, 0x38, 0x32, 0x3c, 0x2e, 0x33, 0x34, 0x32,
+		0xff, 0xdb, 0x00, 0x43, 0x01,
+		0x09, 0x09, 0x09, 0x0c, 0x0b, 0x0c, 0x18, 0x0d,
+		0x0d, 0x18, 0x32, 0x21, 0x1c, 0x21, 0x32, 0x32,
+		0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32,
+		0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32,
+		0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32,
+		0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32,
+		0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32,
+		0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32
+	}, {
+		0xff, 0xdb, 0x00, 0x43, 0x00,
+		0x10, 0x0b, 0x0c, 0x0e, 0x0c, 0x0a, 0x10, 0x0e,
+		0x0d, 0x0e, 0x12, 0x11, 0x10, 0x13, 0x18, 0x28,
+		0x1a, 0x18, 0x16, 0x16, 0x18, 0x31, 0x23, 0x25,
+		0x1d, 0x28, 0x3a, 0x33, 0x3d, 0x3c, 0x39, 0x33,
+		0x38, 0x37, 0x40, 0x48, 0x5c, 0x4e, 0x40, 0x44,
+		0x57, 0x45, 0x37, 0x38, 0x50, 0x6d, 0x51, 0x57,
+		0x5f, 0x62, 0x67, 0x68, 0x67, 0x3e, 0x4d, 0x71,
+		0x79, 0x70, 0x64, 0x78, 0x5c, 0x65, 0x67, 0x63,
+		0xff, 0xdb, 0x00, 0x43, 0x01,
+		0x11, 0x12, 0x12, 0x18, 0x15, 0x18, 0x2f, 0x1a,
+		0x1a, 0x2f, 0x63, 0x42, 0x38, 0x42, 0x63, 0x63,
+		0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
+		0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
+		0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
+		0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
+		0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
+		0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63
+	}, {
+		0xff, 0xdb, 0x00, 0x43, 0x00,
+		0x20, 0x16, 0x18, 0x1c, 0x18, 0x14, 0x20, 0x1c,
+		0x1a, 0x1c, 0x24, 0x22, 0x20, 0x26, 0x30, 0x50,
+		0x34, 0x30, 0x2c, 0x2c, 0x30, 0x62, 0x46, 0x4a,
+		0x3a, 0x50, 0x74, 0x66, 0x7a, 0x78, 0x72, 0x66,
+		0x70, 0x6e, 0x80, 0x90, 0xb8, 0x9c, 0x80, 0x88,
+		0xae, 0x8a, 0x6e, 0x70, 0xa0, 0xda, 0xa2, 0xae,
+		0xbe, 0xc4, 0xce, 0xd0, 0xce, 0x7c, 0x9a, 0xe2,
+		0xf2, 0xe0, 0xc8, 0xf0, 0xb8, 0xca, 0xce, 0xc6,
+		0xff, 0xdb, 0x00, 0x43, 0x01,
+		0x22, 0x24, 0x24, 0x30, 0x2a, 0x30, 0x5e, 0x34,
+		0x34, 0x5e, 0xc6, 0x84, 0x70, 0x84, 0xc6, 0xc6,
+		0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6,
+		0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6,
+		0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6,
+		0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6,
+		0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6,
+		0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6
+	}, {
+		0xff, 0xdb, 0x00, 0x43, 0x00,
+		0x30, 0x21, 0x24, 0x2a, 0x24, 0x1e, 0x30, 0x2a,
+		0x27, 0x2a, 0x36, 0x33, 0x30, 0x39, 0x48, 0x78,
+		0x4e, 0x48, 0x42, 0x42, 0x48, 0x93, 0x69, 0x6f,
+		0x57, 0x78, 0xae, 0x99, 0xb7, 0xb4, 0xab, 0x99,
+		0xa8, 0xa5, 0xc0, 0xd8, 0xff, 0xea, 0xc0, 0xcc,
+		0xff, 0xcf, 0xa5, 0xa8, 0xf0, 0xff, 0xf3, 0xff,
+		0xff, 0xff, 0xff, 0xff, 0xff, 0xba, 0xe7, 0xff,
+		0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+		0xff, 0xdb, 0x00, 0x43, 0x01,
+		0x33, 0x36, 0x36, 0x48, 0x3f, 0x48, 0x8d, 0x4e,
+		0x4e, 0x8d, 0xff, 0xc6, 0xa8, 0xc6, 0xff, 0xff,
+		0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+		0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+		0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+		0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+		0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+		0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff
+	}
+};
+
 #endif /* __SOLO6X10_JPEG_H */
diff --git a/drivers/staging/media/solo6x10/solo6x10-offsets.h b/drivers/staging/media/solo6x10/solo6x10-offsets.h
new file mode 100644
index 00000000000..f005dca501f
--- /dev/null
+++ b/drivers/staging/media/solo6x10/solo6x10-offsets.h
@@ -0,0 +1,85 @@
+/*
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.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., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#ifndef __SOLO6X10_OFFSETS_H
+#define __SOLO6X10_OFFSETS_H
+
+#define SOLO_DISP_EXT_ADDR			0x00000000
+#define SOLO_DISP_EXT_SIZE			0x00480000
+
+#define SOLO_EOSD_EXT_ADDR \
+	(SOLO_DISP_EXT_ADDR + SOLO_DISP_EXT_SIZE)
+#define SOLO_EOSD_EXT_SIZE(__solo) \
+	(__solo->type == SOLO_DEV_6010 ? 0x10000 : 0x20000)
+#define SOLO_EOSD_EXT_SIZE_MAX			0x20000
+#define SOLO_EOSD_EXT_AREA(__solo) \
+	(SOLO_EOSD_EXT_SIZE(__solo) * 32)
+
+#define SOLO_MOTION_EXT_ADDR(__solo) \
+	(SOLO_EOSD_EXT_ADDR + SOLO_EOSD_EXT_AREA(__solo))
+#define SOLO_MOTION_EXT_SIZE			0x00080000
+
+#define SOLO_G723_EXT_ADDR(__solo) \
+	(SOLO_MOTION_EXT_ADDR(__solo) + SOLO_MOTION_EXT_SIZE)
+#define SOLO_G723_EXT_SIZE			0x00010000
+
+#define SOLO_CAP_EXT_ADDR(__solo) \
+	(SOLO_G723_EXT_ADDR(__solo) + SOLO_G723_EXT_SIZE)
+
+/* 18 is the maximum number of pages required for PAL@D1, the largest frame
+ * possible */
+#define SOLO_CAP_PAGE_SIZE			(18 << 16)
+
+/* Always allow the encoder enough for 16 channels, even if we have less. The
+ * exception is if we have card with only 32Megs of memory. */
+#define SOLO_CAP_EXT_SIZE(__solo) \
+	((((__solo->sdram_size <= (32 << 20)) ? 4 : 16) + 1)	\
+	 * SOLO_CAP_PAGE_SIZE)
+
+#define SOLO_EREF_EXT_ADDR(__solo) \
+	(SOLO_CAP_EXT_ADDR(__solo) + SOLO_CAP_EXT_SIZE(__solo))
+#define SOLO_EREF_EXT_SIZE			0x00140000
+#define SOLO_EREF_EXT_AREA(__solo) \
+	(SOLO_EREF_EXT_SIZE * __solo->nr_chans * 2)
+
+#define __SOLO_JPEG_MIN_SIZE(__solo)		(__solo->nr_chans * 0x00080000)
+
+#define SOLO_MP4E_EXT_ADDR(__solo) \
+	(SOLO_EREF_EXT_ADDR(__solo) + SOLO_EREF_EXT_AREA(__solo))
+#define SOLO_MP4E_EXT_SIZE(__solo) \
+	max((__solo->nr_chans * 0x00080000),				\
+	    min(((__solo->sdram_size - SOLO_MP4E_EXT_ADDR(__solo)) -	\
+		 __SOLO_JPEG_MIN_SIZE(__solo)), 0x00ff0000))
+
+#define __SOLO_JPEG_MIN_SIZE(__solo)		(__solo->nr_chans * 0x00080000)
+#define SOLO_JPEG_EXT_ADDR(__solo) \
+		(SOLO_MP4E_EXT_ADDR(__solo) + SOLO_MP4E_EXT_SIZE(__solo))
+#define SOLO_JPEG_EXT_SIZE(__solo) \
+	max(__SOLO_JPEG_MIN_SIZE(__solo),				\
+	    min((__solo->sdram_size - SOLO_JPEG_EXT_ADDR(__solo)), 0x00ff0000))
+
+#define SOLO_SDRAM_END(__solo) \
+	(SOLO_JPEG_EXT_ADDR(__solo) + SOLO_JPEG_EXT_SIZE(__solo))
+
+#endif /* __SOLO6X10_OFFSETS_H */
diff --git a/drivers/staging/media/solo6x10/solo6x10-p2m.c b/drivers/staging/media/solo6x10/solo6x10-p2m.c
new file mode 100644
index 00000000000..333594189b8
--- /dev/null
+++ b/drivers/staging/media/solo6x10/solo6x10-p2m.c
@@ -0,0 +1,333 @@
+/*
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.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., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+
+#include "solo6x10.h"
+
+static int multi_p2m;
+module_param(multi_p2m, uint, 0644);
+MODULE_PARM_DESC(multi_p2m,
+		 "Use multiple P2M DMA channels (default: no, 6010-only)");
+
+static int desc_mode;
+module_param(desc_mode, uint, 0644);
+MODULE_PARM_DESC(desc_mode,
+		 "Allow use of descriptor mode DMA (default: no, 6010-only)");
+
+int solo_p2m_dma(struct solo_dev *solo_dev, int wr,
+		 void *sys_addr, u32 ext_addr, u32 size,
+		 int repeat, u32 ext_size)
+{
+	dma_addr_t dma_addr;
+	int ret;
+
+	if (WARN_ON_ONCE((unsigned long)sys_addr & 0x03))
+		return -EINVAL;
+	if (WARN_ON_ONCE(!size))
+		return -EINVAL;
+
+	dma_addr = pci_map_single(solo_dev->pdev, sys_addr, size,
+				  wr ? PCI_DMA_TODEVICE : PCI_DMA_FROMDEVICE);
+	if (pci_dma_mapping_error(solo_dev->pdev, dma_addr))
+		return -ENOMEM;
+
+	ret = solo_p2m_dma_t(solo_dev, wr, dma_addr, ext_addr, size,
+			     repeat, ext_size);
+
+	pci_unmap_single(solo_dev->pdev, dma_addr, size,
+			 wr ? PCI_DMA_TODEVICE : PCI_DMA_FROMDEVICE);
+
+	return ret;
+}
+
+/* Mutex must be held for p2m_id before calling this!! */
+int solo_p2m_dma_desc(struct solo_dev *solo_dev,
+		      struct solo_p2m_desc *desc, dma_addr_t desc_dma,
+		      int desc_cnt)
+{
+	struct solo_p2m_dev *p2m_dev;
+	unsigned int timeout;
+	unsigned int config = 0;
+	int ret = 0;
+	int p2m_id = 0;
+
+	/* Get next ID. According to Softlogic, 6110 has problems on !=0 P2M */
+	if (solo_dev->type != SOLO_DEV_6110 && multi_p2m) {
+		p2m_id = atomic_inc_return(&solo_dev->p2m_count) % SOLO_NR_P2M;
+		if (p2m_id < 0)
+			p2m_id = -p2m_id;
+	}
+
+	p2m_dev = &solo_dev->p2m_dev[p2m_id];
+
+	if (mutex_lock_interruptible(&p2m_dev->mutex))
+		return -EINTR;
+
+	INIT_COMPLETION(p2m_dev->completion);
+	p2m_dev->error = 0;
+
+	if (desc_cnt > 1 && solo_dev->type != SOLO_DEV_6110 && desc_mode) {
+		/* For 6010 with more than one desc, we can do a one-shot */
+		p2m_dev->desc_count = p2m_dev->desc_idx = 0;
+		config = solo_reg_read(solo_dev, SOLO_P2M_CONFIG(p2m_id));
+
+		solo_reg_write(solo_dev, SOLO_P2M_DES_ADR(p2m_id), desc_dma);
+		solo_reg_write(solo_dev, SOLO_P2M_DESC_ID(p2m_id), desc_cnt);
+		solo_reg_write(solo_dev, SOLO_P2M_CONFIG(p2m_id), config |
+			       SOLO_P2M_DESC_MODE);
+	} else {
+		/* For single descriptors and 6110, we need to run each desc */
+		p2m_dev->desc_count = desc_cnt;
+		p2m_dev->desc_idx = 1;
+		p2m_dev->descs = desc;
+
+		solo_reg_write(solo_dev, SOLO_P2M_TAR_ADR(p2m_id),
+			       desc[1].dma_addr);
+		solo_reg_write(solo_dev, SOLO_P2M_EXT_ADR(p2m_id),
+			       desc[1].ext_addr);
+		solo_reg_write(solo_dev, SOLO_P2M_EXT_CFG(p2m_id),
+			       desc[1].cfg);
+		solo_reg_write(solo_dev, SOLO_P2M_CONTROL(p2m_id),
+			       desc[1].ctrl);
+	}
+
+	timeout = wait_for_completion_timeout(&p2m_dev->completion,
+					      solo_dev->p2m_jiffies);
+
+	if (WARN_ON_ONCE(p2m_dev->error))
+		ret = -EIO;
+	else if (timeout == 0) {
+		solo_dev->p2m_timeouts++;
+		ret = -EAGAIN;
+	}
+
+	solo_reg_write(solo_dev, SOLO_P2M_CONTROL(p2m_id), 0);
+
+	/* Don't write here for the no_desc_mode case, because config is 0.
+	 * We can't test no_desc_mode again, it might race. */
+	if (desc_cnt > 1 && solo_dev->type != SOLO_DEV_6110 && config)
+		solo_reg_write(solo_dev, SOLO_P2M_CONFIG(p2m_id), config);
+
+	mutex_unlock(&p2m_dev->mutex);
+
+	return ret;
+}
+
+void solo_p2m_fill_desc(struct solo_p2m_desc *desc, int wr,
+			dma_addr_t dma_addr, u32 ext_addr, u32 size,
+			int repeat, u32 ext_size)
+{
+	WARN_ON_ONCE(dma_addr & 0x03);
+	WARN_ON_ONCE(!size);
+
+	desc->cfg = SOLO_P2M_COPY_SIZE(size >> 2);
+	desc->ctrl = SOLO_P2M_BURST_SIZE(SOLO_P2M_BURST_256) |
+		(wr ? SOLO_P2M_WRITE : 0) | SOLO_P2M_TRANS_ON;
+
+	if (repeat) {
+		desc->cfg |= SOLO_P2M_EXT_INC(ext_size >> 2);
+		desc->ctrl |=  SOLO_P2M_PCI_INC(size >> 2) |
+			 SOLO_P2M_REPEAT(repeat);
+	}
+
+	desc->dma_addr = dma_addr;
+	desc->ext_addr = ext_addr;
+}
+
+int solo_p2m_dma_t(struct solo_dev *solo_dev, int wr,
+		   dma_addr_t dma_addr, u32 ext_addr, u32 size,
+		   int repeat, u32 ext_size)
+{
+	struct solo_p2m_desc desc[2];
+
+	solo_p2m_fill_desc(&desc[1], wr, dma_addr, ext_addr, size, repeat,
+			   ext_size);
+
+	/* No need for desc_dma since we know it is a single-shot */
+	return solo_p2m_dma_desc(solo_dev, desc, 0, 1);
+}
+
+void solo_p2m_isr(struct solo_dev *solo_dev, int id)
+{
+	struct solo_p2m_dev *p2m_dev = &solo_dev->p2m_dev[id];
+	struct solo_p2m_desc *desc;
+
+	if (p2m_dev->desc_count <= p2m_dev->desc_idx) {
+		complete(&p2m_dev->completion);
+		return;
+	}
+
+	/* Setup next descriptor */
+	p2m_dev->desc_idx++;
+	desc = &p2m_dev->descs[p2m_dev->desc_idx];
+
+	solo_reg_write(solo_dev, SOLO_P2M_CONTROL(id), 0);
+	solo_reg_write(solo_dev, SOLO_P2M_TAR_ADR(id), desc->dma_addr);
+	solo_reg_write(solo_dev, SOLO_P2M_EXT_ADR(id), desc->ext_addr);
+	solo_reg_write(solo_dev, SOLO_P2M_EXT_CFG(id), desc->cfg);
+	solo_reg_write(solo_dev, SOLO_P2M_CONTROL(id), desc->ctrl);
+}
+
+void solo_p2m_error_isr(struct solo_dev *solo_dev)
+{
+	unsigned int err = solo_reg_read(solo_dev, SOLO_PCI_ERR);
+	struct solo_p2m_dev *p2m_dev;
+	int i;
+
+	if (!(err & (SOLO_PCI_ERR_P2M | SOLO_PCI_ERR_P2M_DESC)))
+		return;
+
+	for (i = 0; i < SOLO_NR_P2M; i++) {
+		p2m_dev = &solo_dev->p2m_dev[i];
+		p2m_dev->error = 1;
+		solo_reg_write(solo_dev, SOLO_P2M_CONTROL(i), 0);
+		complete(&p2m_dev->completion);
+	}
+}
+
+void solo_p2m_exit(struct solo_dev *solo_dev)
+{
+	int i;
+
+	for (i = 0; i < SOLO_NR_P2M; i++)
+		solo_irq_off(solo_dev, SOLO_IRQ_P2M(i));
+}
+
+static int solo_p2m_test(struct solo_dev *solo_dev, int base, int size)
+{
+	u32 *wr_buf;
+	u32 *rd_buf;
+	int i;
+	int ret = -EIO;
+	int order = get_order(size);
+
+	wr_buf = (u32 *)__get_free_pages(GFP_KERNEL, order);
+	if (wr_buf == NULL)
+		return -1;
+
+	rd_buf = (u32 *)__get_free_pages(GFP_KERNEL, order);
+	if (rd_buf == NULL) {
+		free_pages((unsigned long)wr_buf, order);
+		return -1;
+	}
+
+	for (i = 0; i < (size >> 3); i++)
+		*(wr_buf + i) = (i << 16) | (i + 1);
+
+	for (i = (size >> 3); i < (size >> 2); i++)
+		*(wr_buf + i) = ~((i << 16) | (i + 1));
+
+	memset(rd_buf, 0x55, size);
+
+	if (solo_p2m_dma(solo_dev, 1, wr_buf, base, size, 0, 0))
+		goto test_fail;
+
+	if (solo_p2m_dma(solo_dev, 0, rd_buf, base, size, 0, 0))
+		goto test_fail;
+
+	for (i = 0; i < (size >> 2); i++) {
+		if (*(wr_buf + i) != *(rd_buf + i))
+			goto test_fail;
+	}
+
+	ret = 0;
+
+test_fail:
+	free_pages((unsigned long)wr_buf, order);
+	free_pages((unsigned long)rd_buf, order);
+
+	return ret;
+}
+
+int solo_p2m_init(struct solo_dev *solo_dev)
+{
+	struct solo_p2m_dev *p2m_dev;
+	int i;
+
+	for (i = 0; i < SOLO_NR_P2M; i++) {
+		p2m_dev = &solo_dev->p2m_dev[i];
+
+		mutex_init(&p2m_dev->mutex);
+		init_completion(&p2m_dev->completion);
+
+		solo_reg_write(solo_dev, SOLO_P2M_CONTROL(i), 0);
+		solo_reg_write(solo_dev, SOLO_P2M_CONFIG(i),
+			       SOLO_P2M_CSC_16BIT_565 |
+			       SOLO_P2M_DESC_INTR_OPT |
+			       SOLO_P2M_DMA_INTERVAL(0) |
+			       SOLO_P2M_PCI_MASTER_MODE);
+		solo_irq_on(solo_dev, SOLO_IRQ_P2M(i));
+	}
+
+	/* Find correct SDRAM size */
+	for (solo_dev->sdram_size = 0, i = 2; i >= 0; i--) {
+		solo_reg_write(solo_dev, SOLO_DMA_CTRL,
+			       SOLO_DMA_CTRL_REFRESH_CYCLE(1) |
+			       SOLO_DMA_CTRL_SDRAM_SIZE(i) |
+			       SOLO_DMA_CTRL_SDRAM_CLK_INVERT |
+			       SOLO_DMA_CTRL_READ_CLK_SELECT |
+			       SOLO_DMA_CTRL_LATENCY(1));
+
+		solo_reg_write(solo_dev, SOLO_SYS_CFG, solo_dev->sys_config |
+			       SOLO_SYS_CFG_RESET);
+		solo_reg_write(solo_dev, SOLO_SYS_CFG, solo_dev->sys_config);
+
+		switch (i) {
+		case 2:
+			if (solo_p2m_test(solo_dev, 0x07ff0000, 0x00010000) ||
+			    solo_p2m_test(solo_dev, 0x05ff0000, 0x00010000))
+				continue;
+			break;
+
+		case 1:
+			if (solo_p2m_test(solo_dev, 0x03ff0000, 0x00010000))
+				continue;
+			break;
+
+		default:
+			if (solo_p2m_test(solo_dev, 0x01ff0000, 0x00010000))
+				continue;
+		}
+
+		solo_dev->sdram_size = (32 << 20) << i;
+		break;
+	}
+
+	if (!solo_dev->sdram_size) {
+		dev_err(&solo_dev->pdev->dev, "Error detecting SDRAM size\n");
+		return -EIO;
+	}
+
+	if (SOLO_SDRAM_END(solo_dev) > solo_dev->sdram_size) {
+		dev_err(&solo_dev->pdev->dev,
+			"SDRAM is not large enough (%u < %u)\n",
+			solo_dev->sdram_size, SOLO_SDRAM_END(solo_dev));
+		return -EIO;
+	}
+
+	return 0;
+}
diff --git a/drivers/staging/media/solo6x10/registers.h b/drivers/staging/media/solo6x10/solo6x10-regs.h
index aca544472c9..428f6c95118 100644
--- a/drivers/staging/media/solo6x10/registers.h
+++ b/drivers/staging/media/solo6x10/solo6x10-regs.h
@@ -1,6 +1,11 @@
 /*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.net>
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.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
@@ -20,20 +25,20 @@
 #ifndef __SOLO6X10_REGISTERS_H
 #define __SOLO6X10_REGISTERS_H
 
-#include "offsets.h"
+#include "solo6x10-offsets.h"
 
-/* Global 6X10 system configuration */
+/* Global 6010 system configuration */
 #define SOLO_SYS_CFG				0x0000
-#define   SOLO6010_SYS_CFG_FOUT_EN		0x00000001 /* 6010 only */
-#define   SOLO6010_SYS_CFG_PLL_BYPASS		0x00000002 /* 6010 only */
-#define   SOLO6010_SYS_CFG_PLL_PWDN		0x00000004 /* 6010 only */
-#define   SOLO6010_SYS_CFG_OUTDIV(__n)		(((__n) & 0x003) << 3) /* 6010 only */
-#define   SOLO6010_SYS_CFG_FEEDBACKDIV(__n)	(((__n) & 0x1ff) << 5) /* 6010 only */
-#define   SOLO6010_SYS_CFG_INPUTDIV(__n)	(((__n) & 0x01f) << 14) /* 6010 only */
+#define   SOLO_SYS_CFG_FOUT_EN			0x00000001
+#define   SOLO_SYS_CFG_PLL_BYPASS		0x00000002
+#define   SOLO_SYS_CFG_PLL_PWDN			0x00000004
+#define   SOLO_SYS_CFG_OUTDIV(__n)		(((__n) & 0x003) << 3)
+#define   SOLO_SYS_CFG_FEEDBACKDIV(__n)		(((__n) & 0x1ff) << 5)
+#define   SOLO_SYS_CFG_INPUTDIV(__n)		(((__n) & 0x01f) << 14)
 #define   SOLO_SYS_CFG_CLOCK_DIV		0x00080000
 #define   SOLO_SYS_CFG_NCLK_DELAY(__n)		(((__n) & 0x003) << 24)
 #define   SOLO_SYS_CFG_PCLK_DELAY(__n)		(((__n) & 0x00f) << 26)
-#define   SOLO_SYS_CFG_SDRAM64BIT		0x40000000 /* 6110: must be set */
+#define   SOLO_SYS_CFG_SDRAM64BIT		0x40000000
 #define   SOLO_SYS_CFG_RESET			0x80000000
 
 #define	SOLO_DMA_CTRL				0x0004
@@ -45,7 +50,9 @@
 #define	  SOLO_DMA_CTRL_READ_DATA_SELECT	(1<<3)
 #define	  SOLO_DMA_CTRL_READ_CLK_SELECT		(1<<2)
 #define	  SOLO_DMA_CTRL_LATENCY(n)		((n)<<0)
-#define	SOLO_DMA_CTRL1				0x0008
+
+/* Some things we set in this are undocumented. Why Softlogic?!?! */
+#define SOLO_DMA_CTRL1				0x0008
 
 #define SOLO_SYS_VCLK				0x000C
 #define	  SOLO_VCLK_INVERT			(1<<22)
@@ -61,7 +68,7 @@
 #define	  SOLO_VCLK_VIN0001_DELAY(n)		((n)<<0)
 
 #define SOLO_IRQ_STAT				0x0010
-#define SOLO_IRQ_ENABLE				0x0014
+#define SOLO_IRQ_MASK				0x0014
 #define	  SOLO_IRQ_P2M(n)			(1<<((n)+17))
 #define	  SOLO_IRQ_GPIO				(1<<16)
 #define	  SOLO_IRQ_VIDEO_LOSS			(1<<15)
@@ -82,22 +89,7 @@
 #define SOLO_CHIP_OPTION			0x001C
 #define   SOLO_CHIP_ID_MASK			0x00000007
 
-#define SOLO6110_PLL_CONFIG			0x0020
-#define   SOLO6110_PLL_RANGE_BYPASS		(0 << 20)
-#define   SOLO6110_PLL_RANGE_5_10MHZ		(1 << 20)
-#define   SOLO6110_PLL_RANGE_8_16MHZ		(2 << 20)
-#define   SOLO6110_PLL_RANGE_13_26MHZ		(3 << 20)
-#define   SOLO6110_PLL_RANGE_21_42MHZ		(4 << 20)
-#define   SOLO6110_PLL_RANGE_34_68MHZ		(5 << 20)
-#define   SOLO6110_PLL_RANGE_54_108MHZ		(6 << 20)
-#define   SOLO6110_PLL_RANGE_88_200MHZ		(7 << 20)
-#define   SOLO6110_PLL_DIVR(x)			(((x) - 1) << 15)
-#define   SOLO6110_PLL_DIVQ_EXP(x)		((x) << 12)
-#define   SOLO6110_PLL_DIVF(x)			(((x) - 1) << 4)
-#define   SOLO6110_PLL_RESET			(1 << 3)
-#define   SOLO6110_PLL_BYPASS			(1 << 2)
-#define   SOLO6110_PLL_FSEN			(1 << 1)
-#define   SOLO6110_PLL_FB			(1 << 0)
+#define SOLO_PLL_CONFIG				0x0020 /* 6110 Only */
 
 #define SOLO_EEPROM_CTRL			0x0060
 #define	  SOLO_EEPROM_ACCESS_EN			(1<<7)
@@ -105,7 +97,7 @@
 #define	  SOLO_EEPROM_CLK			(1<<2)
 #define	  SOLO_EEPROM_DO			(1<<1)
 #define	  SOLO_EEPROM_DI			(1<<0)
-#define	  SOLO_EEPROM_ENABLE			(EEPROM_ACCESS_EN | EEPROM_CS)
+#define	  SOLO_EEPROM_ENABLE (SOLO_EEPROM_ACCESS_EN | SOLO_EEPROM_CS)
 
 #define SOLO_PCI_ERR				0x0070
 #define   SOLO_PCI_ERR_FATAL			0x00000001
@@ -274,8 +266,8 @@
 #define	  SOLO_VO_FI_CHANGE			(1<<20)
 #define	  SOLO_VO_USER_COLOR_SET_VSYNC		(1<<19)
 #define	  SOLO_VO_USER_COLOR_SET_HSYNC		(1<<18)
-#define	  SOLO_VO_USER_COLOR_SET_NAV		(1<<17)
-#define	  SOLO_VO_USER_COLOR_SET_NAH		(1<<16)
+#define	  SOLO_VO_USER_COLOR_SET_NAH		(1<<17)
+#define	  SOLO_VO_USER_COLOR_SET_NAV		(1<<16)
 #define	  SOLO_VO_NA_COLOR_Y(Y)			((Y)<<8)
 #define	  SOLO_VO_NA_COLOR_CB(CB)		(((CB)/16)<<4)
 #define	  SOLO_VO_NA_COLOR_CR(CR)		(((CR)/16)<<0)
@@ -401,12 +393,13 @@
 #define	  SOLO_VE_BLOCK_BASE(n)			((n)<<0)
 
 #define SOLO_VE_CFG1				0x0614
-#define   SOLO6110_VE_MPEG_SIZE_H(n)		((n)<<28) /* 6110 only */
-#define	  SOLO6010_VE_BYTE_ALIGN(n)		((n)<<24) /* 6010 only */
-#define   SOLO6110_VE_JPEG_SIZE_H(n)		((n)<<20) /* 6110 only */
+#define	  SOLO_VE_BYTE_ALIGN(n)			((n)<<24)
 #define	  SOLO_VE_INSERT_INDEX			(1<<18)
 #define	  SOLO_VE_MOTION_MODE(n)		((n)<<16)
 #define	  SOLO_VE_MOTION_BASE(n)		((n)<<0)
+#define   SOLO_VE_MPEG_SIZE_H(n)		((n)<<28) /* 6110 Only */
+#define   SOLO_VE_JPEG_SIZE_H(n)		((n)<<20) /* 6110 Only */
+#define   SOLO_VE_INSERT_INDEX_JPEG		(1<<19)   /* 6110 Only */
 
 #define SOLO_VE_WMRK_POLY			0x061C
 #define SOLO_VE_VMRK_INIT_KEY			0x0620
@@ -420,6 +413,7 @@
 #define	  SOLO_COMP_TIME_INC(n)			((n)<<25)
 #define	  SOLO_COMP_TIME_WIDTH(n)		((n)<<21)
 #define	  SOLO_DCT_INTERVAL(n)			((n)<<16)
+#define SOLO_VE_COMPT_MOT			0x0634 /* 6110 Only */
 
 #define SOLO_VE_STATE(n)			(0x0640+((n)*4))
 
@@ -428,14 +422,21 @@
 #define SOLO_VE_JPEG_QP_CH_H			0x0678
 #define SOLO_VE_JPEG_CFG			0x067C
 #define SOLO_VE_JPEG_CTRL			0x0680
-
+#define SOLO_VE_CODE_ENCRYPT			0x0684 /* 6110 Only */
+#define SOLO_VE_JPEG_CFG1			0x0688 /* 6110 Only */
+#define SOLO_VE_WMRK_ENABLE			0x068C /* 6110 Only */
 #define SOLO_VE_OSD_CH				0x0690
 #define SOLO_VE_OSD_BASE			0x0694
 #define SOLO_VE_OSD_CLR				0x0698
 #define SOLO_VE_OSD_OPT				0x069C
+#define   SOLO_VE_OSD_V_DOUBLE			(1<<16) /* 6110 Only */
+#define   SOLO_VE_OSD_H_SHADOW			(1<<15)
+#define   SOLO_VE_OSD_V_SHADOW			(1<<14)
+#define   SOLO_VE_OSD_H_OFFSET(n)		((n & 0x7f)<<7)
+#define   SOLO_VE_OSD_V_OFFSET(n)		(n & 0x7f)
 
 #define SOLO_VE_CH_INTL(ch)			(0x0700+((ch)*4))
-#define SOLO6010_VE_CH_MOT(ch)			(0x0740+((ch)*4)) /* 6010 only */
+#define SOLO_VE_CH_MOT(ch)			(0x0740+((ch)*4))
 #define SOLO_VE_CH_QP(ch)			(0x0780+((ch)*4))
 #define SOLO_VE_CH_QP_E(ch)			(0x07C0+((ch)*4))
 #define SOLO_VE_CH_GOP(ch)			(0x0800+((ch)*4))
@@ -447,7 +448,7 @@
 #define SOLO_VE_JPEG_QUE(n)			(0x0A04+((n)*8))
 
 #define SOLO_VD_CFG0				0x0900
-#define	  SOLO6010_VD_CFG_NO_WRITE_NO_WINDOW	(1<<24) /* 6010 only */
+#define	  SOLO_VD_CFG_NO_WRITE_NO_WINDOW	(1<<24)
 #define	  SOLO_VD_CFG_BUSY_WIAT_CODE		(1<<23)
 #define	  SOLO_VD_CFG_BUSY_WIAT_REF		(1<<22)
 #define	  SOLO_VD_CFG_BUSY_WIAT_RES		(1<<21)
@@ -599,9 +600,9 @@
 #define	  SOLO_UART_RX_DATA_POP			(1<<8)
 
 #define SOLO_TIMER_CLOCK_NUM			0x0be0
-#define SOLO_TIMER_WATCHDOG			0x0be4
 #define SOLO_TIMER_USEC				0x0be8
 #define SOLO_TIMER_SEC				0x0bec
+#define SOLO_TIMER_USEC_LSB			0x0d20 /* 6110 Only */
 
 #define SOLO_AUDIO_CONTROL			0x0D00
 #define	  SOLO_AUDIO_ENABLE			(1<<31)
@@ -629,9 +630,10 @@
 #define	  SOLO_AUDIO_EVOL(ch, value)		((value)<<((ch)%10))
 #define SOLO_AUDIO_STA				0x0D14
 
-
-#define SOLO_WATCHDOG				0x0BE4
-#define WATCHDOG_STAT(status)			(status<<8)
-#define WATCHDOG_TIME(sec)			(sec&0xff)
+/*
+ * Watchdog configuration
+ */
+#define SOLO_WATCHDOG				0x0be4
+#define SOLO_WATCHDOG_SET(status, sec)		(status << 8 | (sec & 0xff))
 
 #endif /* __SOLO6X10_REGISTERS_H */
diff --git a/drivers/staging/media/solo6x10/tw28.c b/drivers/staging/media/solo6x10/solo6x10-tw28.c
index db56b42c56c..ad00e2b6032 100644
--- a/drivers/staging/media/solo6x10/tw28.c
+++ b/drivers/staging/media/solo6x10/solo6x10-tw28.c
@@ -1,6 +1,11 @@
 /*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.net>
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.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
@@ -18,12 +23,12 @@
  */
 
 #include <linux/kernel.h>
+#include <linux/delay.h>
+
 #include "solo6x10.h"
-#include "tw28.h"
+#include "solo6x10-tw28.h"
 
-/* XXX: Some of these values are masked into an 8-bit regs, and shifted
- * around for other 8-bit regs. What are the magic bits in these values? */
-#define DEFAULT_HDELAY_NTSC		(32 - 4)
+#define DEFAULT_HDELAY_NTSC		(32 - 8)
 #define DEFAULT_HACTIVE_NTSC		(720 + 16)
 #define DEFAULT_VDELAY_NTSC		(7 - 2)
 #define DEFAULT_VACTIVE_NTSC		(240 + 4)
@@ -33,15 +38,16 @@
 #define DEFAULT_VDELAY_PAL		(6)
 #define DEFAULT_VACTIVE_PAL		(312-DEFAULT_VDELAY_PAL)
 
-static u8 tbl_tw2864_template[] = {
-	0x00, 0x00, 0x80, 0x10, 0x80, 0x80, 0x00, 0x02, /* 0x00 */
-	0x12, 0xf5, 0x09, 0xd0, 0x00, 0x00, 0x00, 0x7f,
-	0x00, 0x00, 0x80, 0x10, 0x80, 0x80, 0x00, 0x02, /* 0x10 */
-	0x12, 0xf5, 0x09, 0xd0, 0x00, 0x00, 0x00, 0x7f,
-	0x00, 0x00, 0x80, 0x10, 0x80, 0x80, 0x00, 0x02, /* 0x20 */
-	0x12, 0xf5, 0x09, 0xd0, 0x00, 0x00, 0x00, 0x7f,
-	0x00, 0x00, 0x80, 0x10, 0x80, 0x80, 0x00, 0x02, /* 0x30 */
-	0x12, 0xf5, 0x09, 0xd0, 0x00, 0x00, 0x00, 0x7f,
+
+static const u8 tbl_tw2864_ntsc_template[] = {
+	0x00, 0xf0, 0x70, 0x30, 0x80, 0x80, 0x00, 0x02, /* 0x00 */
+	0x12, 0xf5, 0x0c, 0xd0, 0x00, 0x00, 0x00, 0x7f,
+	0x00, 0xf0, 0x70, 0x30, 0x80, 0x80, 0x00, 0x02, /* 0x10 */
+	0x12, 0xf5, 0x0c, 0xd0, 0x00, 0x00, 0x00, 0x7f,
+	0x00, 0xf0, 0x70, 0x30, 0x80, 0x80, 0x00, 0x02, /* 0x20 */
+	0x12, 0xf5, 0x0c, 0xd0, 0x00, 0x00, 0x00, 0x7f,
+	0x00, 0xf0, 0x70, 0x30, 0x80, 0x80, 0x00, 0x02, /* 0x30 */
+	0x12, 0xf5, 0x0c, 0xd0, 0x00, 0x00, 0x00, 0x7f,
 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 0x40 */
 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 0x50 */
@@ -61,14 +67,49 @@ static u8 tbl_tw2864_template[] = {
 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 0xc0 */
 	0x00, 0x00, 0x55, 0x00, 0xb1, 0xe4, 0x40, 0x00,
 	0x77, 0x77, 0x01, 0x13, 0x57, 0x9b, 0xdf, 0x20, /* 0xd0 */
-	0x64, 0xa8, 0xec, 0xd1, 0x0f, 0x11, 0x11, 0x81,
-	0x10, 0xe0, 0xbb, 0xbb, 0x00, 0x11, 0x00, 0x00, /* 0xe0 */
+	0x64, 0xa8, 0xec, 0xc1, 0x0f, 0x11, 0x11, 0x81,
+	0x00, 0xe0, 0xbb, 0xbb, 0x00, 0x11, 0x00, 0x00, /* 0xe0 */
 	0x11, 0x00, 0x00, 0x11, 0x00, 0x00, 0x11, 0x00,
 	0x83, 0xb5, 0x09, 0x78, 0x85, 0x00, 0x01, 0x20, /* 0xf0 */
 	0x64, 0x11, 0x40, 0xaf, 0xff, 0x00, 0x00, 0x00,
 };
 
-static u8 tbl_tw2865_ntsc_template[] = {
+static const u8 tbl_tw2864_pal_template[] = {
+	0x00, 0xf0, 0x70, 0x30, 0x80, 0x80, 0x00, 0x12, /* 0x00 */
+	0x18, 0xf5, 0x0c, 0xd0, 0x00, 0x00, 0x01, 0x7f,
+	0x00, 0xf0, 0x70, 0x30, 0x80, 0x80, 0x00, 0x12, /* 0x10 */
+	0x18, 0xf5, 0x0c, 0xd0, 0x00, 0x00, 0x01, 0x7f,
+	0x00, 0xf0, 0x70, 0x30, 0x80, 0x80, 0x00, 0x12, /* 0x20 */
+	0x18, 0xf5, 0x0c, 0xd0, 0x00, 0x00, 0x01, 0x7f,
+	0x00, 0xf0, 0x70, 0x30, 0x80, 0x80, 0x00, 0x12, /* 0x30 */
+	0x18, 0xf5, 0x0c, 0xd0, 0x00, 0x00, 0x01, 0x7f,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 0x40 */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 0x50 */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 0x60 */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 0x70 */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xA3, 0x00,
+	0x00, 0x02, 0x00, 0xcc, 0x00, 0x80, 0x44, 0x50, /* 0x80 */
+	0x22, 0x01, 0xd8, 0xbc, 0xb8, 0x44, 0x38, 0x00,
+	0x00, 0x78, 0x72, 0x3e, 0x14, 0xa5, 0xe4, 0x05, /* 0x90 */
+	0x00, 0x28, 0x44, 0x44, 0xa0, 0x90, 0x5a, 0x01,
+	0x0a, 0x0a, 0x0a, 0x0a, 0x1a, 0x1a, 0x1a, 0x1a, /* 0xa0 */
+	0x00, 0x00, 0x00, 0xf0, 0xf0, 0xf0, 0xf0, 0x44,
+	0x44, 0x0a, 0x00, 0xff, 0xef, 0xef, 0xef, 0xef, /* 0xb0 */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 0xc0 */
+	0x00, 0x00, 0x55, 0x00, 0xb1, 0xe4, 0x40, 0x00,
+	0x77, 0x77, 0x01, 0x13, 0x57, 0x9b, 0xdf, 0x20, /* 0xd0 */
+	0x64, 0xa8, 0xec, 0xc1, 0x0f, 0x11, 0x11, 0x81,
+	0x00, 0xe0, 0xbb, 0xbb, 0x00, 0x11, 0x00, 0x00, /* 0xe0 */
+	0x11, 0x00, 0x00, 0x11, 0x00, 0x00, 0x11, 0x00,
+	0x83, 0xb5, 0x09, 0x00, 0xa0, 0x00, 0x01, 0x20, /* 0xf0 */
+	0x64, 0x11, 0x40, 0xaf, 0xff, 0x00, 0x00, 0x00,
+};
+
+static const u8 tbl_tw2865_ntsc_template[] = {
 	0x00, 0xf0, 0x70, 0x30, 0x80, 0x80, 0x00, 0x02, /* 0x00 */
 	0x12, 0xff, 0x09, 0xd0, 0x00, 0x00, 0x00, 0x7f,
 	0x00, 0xf0, 0x70, 0x30, 0x80, 0x80, 0x00, 0x02, /* 0x10 */
@@ -103,7 +144,7 @@ static u8 tbl_tw2865_ntsc_template[] = {
 	0x64, 0x51, 0x40, 0xaf, 0xFF, 0xF0, 0x00, 0xC0,
 };
 
-static u8 tbl_tw2865_pal_template[] = {
+static const u8 tbl_tw2865_pal_template[] = {
 	0x00, 0xf0, 0x70, 0x30, 0x80, 0x80, 0x00, 0x12, /* 0x00 */
 	0x11, 0xff, 0x01, 0xc3, 0x00, 0x00, 0x01, 0x7f,
 	0x00, 0xf0, 0x70, 0x30, 0x80, 0x80, 0x00, 0x12, /* 0x10 */
@@ -180,8 +221,8 @@ static void tw_write_and_verify(struct solo_dev *solo_dev, u8 addr, u8 off,
 		msleep_interruptible(1);
 	}
 
-/*	printk("solo6x10/tw28: Error writing register: %02x->%02x [%02x]\n",
-		addr, off, val); */
+/*	printk("solo6x10/tw28: Error writing register: %02x->%02x [%02x]\n", */
+/*		addr, off, val); */
 }
 
 static int tw2865_setup(struct solo_dev *solo_dev, u8 dev_addr)
@@ -216,16 +257,17 @@ static int tw2865_setup(struct solo_dev *solo_dev, u8 dev_addr)
 
 	for (i = 0; i < 0xff; i++) {
 		/* Skip read only registers */
-		if (i >= 0xb8 && i <= 0xc1)
-			continue;
-		if ((i & ~0x30) == 0x00 ||
-		    (i & ~0x30) == 0x0c ||
-		    (i & ~0x30) == 0x0d)
-			continue;
-		if (i >= 0xc4 && i <= 0xc7)
+		switch (i) {
+		case 0xb8 ... 0xc1:
+		case 0xc4 ... 0xc7:
+		case 0xfd:
 			continue;
-		if (i == 0xfd)
+		}
+		switch (i & ~0x30) {
+		case 0x00:
+		case 0x0c ... 0x0d:
 			continue;
+		}
 
 		tw_write_and_verify(solo_dev, dev_addr, i,
 				    tbl_tw2865_common[i]);
@@ -236,11 +278,15 @@ static int tw2865_setup(struct solo_dev *solo_dev, u8 dev_addr)
 
 static int tw2864_setup(struct solo_dev *solo_dev, u8 dev_addr)
 {
-	u8 tbl_tw2864_common[sizeof(tbl_tw2864_template)];
+	u8 tbl_tw2864_common[256];
 	int i;
 
-	memcpy(tbl_tw2864_common, tbl_tw2864_template,
-	       sizeof(tbl_tw2864_common));
+	if (solo_dev->video_type == SOLO_VO_FMT_TYPE_PAL)
+		memcpy(tbl_tw2864_common, tbl_tw2864_pal_template,
+		       sizeof(tbl_tw2864_common));
+	else
+		memcpy(tbl_tw2864_common, tbl_tw2864_ntsc_template,
+		       sizeof(tbl_tw2864_common));
 
 	if (solo_dev->tw2865 == 0) {
 		/* IRQ Mode */
@@ -285,33 +331,19 @@ static int tw2864_setup(struct solo_dev *solo_dev, u8 dev_addr)
 		}
 	}
 
-	/* NTSC or PAL */
-	if (solo_dev->video_type == SOLO_VO_FMT_TYPE_PAL) {
-		for (i = 0; i < 4; i++) {
-			tbl_tw2864_common[0x07 | (i << 4)] |= 0x10;
-			tbl_tw2864_common[0x08 | (i << 4)] |= 0x06;
-			tbl_tw2864_common[0x0a | (i << 4)] |= 0x08;
-			tbl_tw2864_common[0x0b | (i << 4)] |= 0x13;
-			tbl_tw2864_common[0x0e | (i << 4)] |= 0x01;
-		}
-		tbl_tw2864_common[0x9d] = 0x90;
-		tbl_tw2864_common[0xf3] = 0x00;
-		tbl_tw2864_common[0xf4] = 0xa0;
-	}
-
 	for (i = 0; i < 0xff; i++) {
 		/* Skip read only registers */
-		if (i >= 0xb8 && i <= 0xc1)
+		switch (i) {
+		case 0xb8 ... 0xc1:
+		case 0xfd:
 			continue;
-		if ((i & ~0x30) == 0x00 ||
-		    (i & ~0x30) == 0x0c ||
-		    (i & ~0x30) == 0x0d)
-			continue;
-		if (i == 0x74 || i == 0x77 || i == 0x78 ||
-		    i == 0x79 || i == 0x7a)
-			continue;
-		if (i == 0xfd)
+		}
+		switch (i & ~0x30) {
+		case 0x00:
+		case 0x0c:
+		case 0x0d:
 			continue;
+		}
 
 		tw_write_and_verify(solo_dev, dev_addr, i,
 				    tbl_tw2864_common[i]);
@@ -544,8 +576,10 @@ int solo_tw28_init(struct solo_dev *solo_dev)
 	int i;
 	u8 value;
 
-	/* Detect techwell chip type */
-	for (i = 0; i < TW_NUM_CHIP; i++) {
+	solo_dev->tw28_cnt = 0;
+
+	/* Detect techwell chip type(s) */
+	for (i = 0; i < solo_dev->nr_chans / 4; i++) {
 		value = solo_i2c_readbyte(solo_dev, SOLO_I2C_TW,
 					  TW_CHIP_OFFSET_ADDR(i), 0xFF);
 
@@ -560,7 +594,8 @@ int solo_tw28_init(struct solo_dev *solo_dev)
 			break;
 		default:
 			value = solo_i2c_readbyte(solo_dev, SOLO_I2C_TW,
-						  TW_CHIP_OFFSET_ADDR(i), 0x59);
+						  TW_CHIP_OFFSET_ADDR(i),
+						  0x59);
 			if ((value >> 3) == 0x04) {
 				solo_dev->tw2815 |= 1 << i;
 				solo_dev->tw28_cnt++;
@@ -568,8 +603,11 @@ int solo_tw28_init(struct solo_dev *solo_dev)
 		}
 	}
 
-	if (!solo_dev->tw28_cnt)
+	if (solo_dev->tw28_cnt != (solo_dev->nr_chans >> 2)) {
+		dev_err(&solo_dev->pdev->dev,
+			"Could not initialize any techwell chips\n");
 		return -EINVAL;
+	}
 
 	saa7128_setup(solo_dev);
 
@@ -582,17 +620,6 @@ int solo_tw28_init(struct solo_dev *solo_dev)
 			tw2815_setup(solo_dev, TW_CHIP_OFFSET_ADDR(i));
 	}
 
-	dev_info(&solo_dev->pdev->dev, "Initialized %d tw28xx chip%s:",
-		 solo_dev->tw28_cnt, solo_dev->tw28_cnt == 1 ? "" : "s");
-
-	if (solo_dev->tw2865)
-		printk(" tw2865[%d]", hweight32(solo_dev->tw2865));
-	if (solo_dev->tw2864)
-		printk(" tw2864[%d]", hweight32(solo_dev->tw2864));
-	if (solo_dev->tw2815)
-		printk(" tw2815[%d]", hweight32(solo_dev->tw2815));
-	printk("\n");
-
 	return 0;
 }
 
@@ -610,7 +637,7 @@ int tw28_get_video_status(struct solo_dev *solo_dev, u8 ch)
 	chip_num = ch / 4;
 	ch %= 4;
 
-	val = tw_readbyte(solo_dev, chip_num, TW286X_AV_STAT_ADDR,
+	val = tw_readbyte(solo_dev, chip_num, TW286x_AV_STAT_ADDR,
 			  TW_AV_STAT_ADDR) & 0x0f;
 
 	return val & (1 << ch) ? 1 : 0;
@@ -626,7 +653,7 @@ u16 tw28_get_audio_status(struct solo_dev *solo_dev)
 	int i;
 
 	for (i = 0; i < solo_dev->tw28_cnt; i++) {
-		val = (tw_readbyte(solo_dev, i, TW286X_AV_STAT_ADDR,
+		val = (tw_readbyte(solo_dev, i, TW286x_AV_STAT_ADDR,
 				   TW_AV_STAT_ADDR) & 0xf0) >> 4;
 		status |= val << (i * 4);
 	}
@@ -635,7 +662,13 @@ u16 tw28_get_audio_status(struct solo_dev *solo_dev)
 }
 #endif
 
-int tw28_set_ctrl_val(struct solo_dev *solo_dev, u32 ctrl, u8 ch, s32 val)
+bool tw28_has_sharpness(struct solo_dev *solo_dev, u8 ch)
+{
+	return is_tw286x(solo_dev, ch / 4);
+}
+
+int tw28_set_ctrl_val(struct solo_dev *solo_dev, u32 ctrl, u8 ch,
+		      s32 val)
 {
 	char sval;
 	u8 chip_num;
@@ -650,8 +683,6 @@ int tw28_set_ctrl_val(struct solo_dev *solo_dev, u32 ctrl, u8 ch, s32 val)
 	switch (ctrl) {
 	case V4L2_CID_SHARPNESS:
 		/* Only 286x has sharpness */
-		if (val > 0x0f || val < 0)
-			return -ERANGE;
 		if (is_tw286x(solo_dev, chip_num)) {
 			u8 v = solo_i2c_readbyte(solo_dev, SOLO_I2C_TW,
 						 TW_CHIP_OFFSET_ADDR(chip_num),
@@ -661,8 +692,9 @@ int tw28_set_ctrl_val(struct solo_dev *solo_dev, u32 ctrl, u8 ch, s32 val)
 			solo_i2c_writebyte(solo_dev, SOLO_I2C_TW,
 					   TW_CHIP_OFFSET_ADDR(chip_num),
 					   TW286x_SHARPNESS(chip_num), v);
-		} else if (val != 0)
-			return -ERANGE;
+		} else {
+			return -EINVAL;
+		}
 		break;
 
 	case V4L2_CID_HUE:
@@ -676,6 +708,7 @@ int tw28_set_ctrl_val(struct solo_dev *solo_dev, u32 ctrl, u8 ch, s32 val)
 		break;
 
 	case V4L2_CID_SATURATION:
+		/* 286x chips have a U and V component for saturation */
 		if (is_tw286x(solo_dev, chip_num)) {
 			solo_i2c_writebyte(solo_dev, SOLO_I2C_TW,
 					   TW_CHIP_OFFSET_ADDR(chip_num),
diff --git a/drivers/staging/media/solo6x10/tw28.h b/drivers/staging/media/solo6x10/solo6x10-tw28.h
index a44a03afbd3..1a02c87d4cf 100644
--- a/drivers/staging/media/solo6x10/tw28.h
+++ b/drivers/staging/media/solo6x10/solo6x10-tw28.h
@@ -1,6 +1,11 @@
 /*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.net>
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.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
@@ -36,7 +41,7 @@
 #define TW_AUDIO_INPUT_GAIN_ADDR(n)		(0x60 + ((n > 1) ? 1 : 0))
 
 /* tw286x */
-#define TW286X_AV_STAT_ADDR			0xfd
+#define TW286x_AV_STAT_ADDR			0xfd
 #define TW286x_HUE_ADDR(n)			(0x06 | ((n) << 4))
 #define TW286x_SATURATIONU_ADDR(n)		(0x04 | ((n) << 4))
 #define TW286x_SATURATIONV_ADDR(n)		(0x05 | ((n) << 4))
@@ -50,6 +55,7 @@ int solo_tw28_init(struct solo_dev *solo_dev);
 
 int tw28_set_ctrl_val(struct solo_dev *solo_dev, u32 ctrl, u8 ch, s32 val);
 int tw28_get_ctrl_val(struct solo_dev *solo_dev, u32 ctrl, u8 ch, s32 *val);
+bool tw28_has_sharpness(struct solo_dev *solo_dev, u8 ch);
 
 u8 tw28_get_audio_gain(struct solo_dev *solo_dev, u8 ch);
 void tw28_set_audio_gain(struct solo_dev *solo_dev, u8 ch, u8 val);
diff --git a/drivers/staging/media/solo6x10/solo6x10-v4l2-enc.c b/drivers/staging/media/solo6x10/solo6x10-v4l2-enc.c
new file mode 100644
index 00000000000..98e2902afd7
--- /dev/null
+++ b/drivers/staging/media/solo6x10/solo6x10-v4l2-enc.c
@@ -0,0 +1,1385 @@
+/*
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.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., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/kthread.h>
+#include <linux/freezer.h>
+
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-event.h>
+#include <media/videobuf2-dma-sg.h>
+
+#include "solo6x10.h"
+#include "solo6x10-tw28.h"
+#include "solo6x10-jpeg.h"
+
+#define MIN_VID_BUFFERS		2
+#define FRAME_BUF_SIZE		(196 * 1024)
+#define MP4_QS			16
+#define DMA_ALIGN		4096
+
+/* 6010 M4V */
+static unsigned char vop_6010_ntsc_d1[] = {
+	0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 0x20,
+	0x02, 0x48, 0x1d, 0xc0, 0x00, 0x40, 0x00, 0x40,
+	0x00, 0x40, 0x00, 0x80, 0x00, 0x97, 0x53, 0x04,
+	0x1f, 0x4c, 0x58, 0x10, 0xf0, 0x71, 0x18, 0x3f,
+};
+
+static unsigned char vop_6010_ntsc_cif[] = {
+	0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 0x20,
+	0x02, 0x48, 0x1d, 0xc0, 0x00, 0x40, 0x00, 0x40,
+	0x00, 0x40, 0x00, 0x80, 0x00, 0x97, 0x53, 0x04,
+	0x1f, 0x4c, 0x2c, 0x10, 0x78, 0x51, 0x18, 0x3f,
+};
+
+static unsigned char vop_6010_pal_d1[] = {
+	0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 0x20,
+	0x02, 0x48, 0x15, 0xc0, 0x00, 0x40, 0x00, 0x40,
+	0x00, 0x40, 0x00, 0x80, 0x00, 0x97, 0x53, 0x04,
+	0x1f, 0x4c, 0x58, 0x11, 0x20, 0x71, 0x18, 0x3f,
+};
+
+static unsigned char vop_6010_pal_cif[] = {
+	0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 0x20,
+	0x02, 0x48, 0x15, 0xc0, 0x00, 0x40, 0x00, 0x40,
+	0x00, 0x40, 0x00, 0x80, 0x00, 0x97, 0x53, 0x04,
+	0x1f, 0x4c, 0x2c, 0x10, 0x90, 0x51, 0x18, 0x3f,
+};
+
+/* 6110 h.264 */
+static unsigned char vop_6110_ntsc_d1[] = {
+	0x00, 0x00, 0x00, 0x01, 0x67, 0x42, 0x00, 0x1e,
+	0x9a, 0x74, 0x05, 0x81, 0xec, 0x80, 0x00, 0x00,
+	0x00, 0x01, 0x68, 0xce, 0x32, 0x28, 0x00, 0x00,
+};
+
+static unsigned char vop_6110_ntsc_cif[] = {
+	0x00, 0x00, 0x00, 0x01, 0x67, 0x42, 0x00, 0x1e,
+	0x9a, 0x74, 0x0b, 0x0f, 0xc8, 0x00, 0x00, 0x00,
+	0x01, 0x68, 0xce, 0x32, 0x28, 0x00, 0x00, 0x00,
+};
+
+static unsigned char vop_6110_pal_d1[] = {
+	0x00, 0x00, 0x00, 0x01, 0x67, 0x42, 0x00, 0x1e,
+	0x9a, 0x74, 0x05, 0x80, 0x93, 0x20, 0x00, 0x00,
+	0x00, 0x01, 0x68, 0xce, 0x32, 0x28, 0x00, 0x00,
+};
+
+static unsigned char vop_6110_pal_cif[] = {
+	0x00, 0x00, 0x00, 0x01, 0x67, 0x42, 0x00, 0x1e,
+	0x9a, 0x74, 0x0b, 0x04, 0xb2, 0x00, 0x00, 0x00,
+	0x01, 0x68, 0xce, 0x32, 0x28, 0x00, 0x00, 0x00,
+};
+
+struct vop_header {
+	/* VE_STATUS0 */
+	u32 mpeg_size:20, sad_motion_flag:1, video_motion_flag:1, vop_type:2,
+		channel:5, source_fl:1, interlace:1, progressive:1;
+
+	/* VE_STATUS1 */
+	u32 vsize:8, hsize:8, last_queue:4, nop0:8, scale:4;
+
+	/* VE_STATUS2 */
+	u32 mpeg_off;
+
+	/* VE_STATUS3 */
+	u32 jpeg_off;
+
+	/* VE_STATUS4 */
+	u32 jpeg_size:20, interval:10, nop1:2;
+
+	/* VE_STATUS5/6 */
+	u32 sec, usec;
+
+	/* VE_STATUS7/8/9 */
+	u32 nop2[3];
+
+	/* VE_STATUS10 */
+	u32 mpeg_size_alt:20, nop3:12;
+
+	u32 end_nops[5];
+} __packed;
+
+struct solo_enc_buf {
+	enum solo_enc_types	type;
+	struct vop_header	*vh;
+	int			motion;
+};
+
+static int solo_is_motion_on(struct solo_enc_dev *solo_enc)
+{
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+
+	return (solo_dev->motion_mask >> solo_enc->ch) & 1;
+}
+
+static int solo_motion_detected(struct solo_enc_dev *solo_enc)
+{
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+	unsigned long flags;
+	u32 ch_mask = 1 << solo_enc->ch;
+	int ret = 0;
+
+	spin_lock_irqsave(&solo_enc->motion_lock, flags);
+	if (solo_reg_read(solo_dev, SOLO_VI_MOT_STATUS) & ch_mask) {
+		solo_reg_write(solo_dev, SOLO_VI_MOT_CLEAR, ch_mask);
+		ret = 1;
+	}
+	spin_unlock_irqrestore(&solo_enc->motion_lock, flags);
+
+	return ret;
+}
+
+static void solo_motion_toggle(struct solo_enc_dev *solo_enc, int on)
+{
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+	u32 mask = 1 << solo_enc->ch;
+	unsigned long flags;
+
+	spin_lock_irqsave(&solo_enc->motion_lock, flags);
+
+	if (on)
+		solo_dev->motion_mask |= mask;
+	else
+		solo_dev->motion_mask &= ~mask;
+
+	solo_reg_write(solo_dev, SOLO_VI_MOT_CLEAR, mask);
+
+	solo_reg_write(solo_dev, SOLO_VI_MOT_ADR,
+		       SOLO_VI_MOTION_EN(solo_dev->motion_mask) |
+		       (SOLO_MOTION_EXT_ADDR(solo_dev) >> 16));
+
+	spin_unlock_irqrestore(&solo_enc->motion_lock, flags);
+}
+
+void solo_update_mode(struct solo_enc_dev *solo_enc)
+{
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+	int vop_len;
+	unsigned char *vop;
+
+	solo_enc->interlaced = (solo_enc->mode & 0x08) ? 1 : 0;
+	solo_enc->bw_weight = max(solo_dev->fps / solo_enc->interval, 1);
+
+	if (solo_enc->mode == SOLO_ENC_MODE_CIF) {
+		solo_enc->width = solo_dev->video_hsize >> 1;
+		solo_enc->height = solo_dev->video_vsize;
+		if (solo_dev->type == SOLO_DEV_6110) {
+			if (solo_dev->video_type == SOLO_VO_FMT_TYPE_NTSC) {
+				vop = vop_6110_ntsc_cif;
+				vop_len = sizeof(vop_6110_ntsc_cif);
+			} else {
+				vop = vop_6110_pal_cif;
+				vop_len = sizeof(vop_6110_pal_cif);
+			}
+		} else {
+			if (solo_dev->video_type == SOLO_VO_FMT_TYPE_NTSC) {
+				vop = vop_6010_ntsc_cif;
+				vop_len = sizeof(vop_6010_ntsc_cif);
+			} else {
+				vop = vop_6010_pal_cif;
+				vop_len = sizeof(vop_6010_pal_cif);
+			}
+		}
+	} else {
+		solo_enc->width = solo_dev->video_hsize;
+		solo_enc->height = solo_dev->video_vsize << 1;
+		solo_enc->bw_weight <<= 2;
+		if (solo_dev->type == SOLO_DEV_6110) {
+			if (solo_dev->video_type == SOLO_VO_FMT_TYPE_NTSC) {
+				vop = vop_6110_ntsc_d1;
+				vop_len = sizeof(vop_6110_ntsc_d1);
+			} else {
+				vop = vop_6110_pal_d1;
+				vop_len = sizeof(vop_6110_pal_d1);
+			}
+		} else {
+			if (solo_dev->video_type == SOLO_VO_FMT_TYPE_NTSC) {
+				vop = vop_6010_ntsc_d1;
+				vop_len = sizeof(vop_6010_ntsc_d1);
+			} else {
+				vop = vop_6010_pal_d1;
+				vop_len = sizeof(vop_6010_pal_d1);
+			}
+		}
+	}
+
+	memcpy(solo_enc->vop, vop, vop_len);
+
+	/* Some fixups for 6010/M4V */
+	if (solo_dev->type == SOLO_DEV_6010) {
+		u16 fps = solo_dev->fps * 1000;
+		u16 interval = solo_enc->interval * 1000;
+
+		vop = solo_enc->vop;
+
+		/* Frame rate and interval */
+		vop[22] = fps >> 4;
+		vop[23] = ((fps << 4) & 0xf0) | 0x0c
+			| ((interval >> 13) & 0x3);
+		vop[24] = (interval >> 5) & 0xff;
+		vop[25] = ((interval << 3) & 0xf8) | 0x04;
+	}
+
+	solo_enc->vop_len = vop_len;
+
+	/* Now handle the jpeg header */
+	vop = solo_enc->jpeg_header;
+	vop[SOF0_START + 5] = 0xff & (solo_enc->height >> 8);
+	vop[SOF0_START + 6] = 0xff & solo_enc->height;
+	vop[SOF0_START + 7] = 0xff & (solo_enc->width >> 8);
+	vop[SOF0_START + 8] = 0xff & solo_enc->width;
+
+	memcpy(vop + DQT_START,
+	       jpeg_dqt[solo_g_jpeg_qp(solo_dev, solo_enc->ch)], DQT_LEN);
+}
+
+static int solo_enc_on(struct solo_enc_dev *solo_enc)
+{
+	u8 ch = solo_enc->ch;
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+	u8 interval;
+
+	solo_update_mode(solo_enc);
+
+	/* Make sure to do a bandwidth check */
+	if (solo_enc->bw_weight > solo_dev->enc_bw_remain)
+		return -EBUSY;
+	solo_enc->sequence = 0;
+	solo_dev->enc_bw_remain -= solo_enc->bw_weight;
+
+	if (solo_enc->type == SOLO_ENC_TYPE_EXT)
+		solo_reg_write(solo_dev, SOLO_CAP_CH_COMP_ENA_E(ch), 1);
+
+	/* Disable all encoding for this channel */
+	solo_reg_write(solo_dev, SOLO_CAP_CH_SCALE(ch), 0);
+
+	/* Common for both std and ext encoding */
+	solo_reg_write(solo_dev, SOLO_VE_CH_INTL(ch),
+		       solo_enc->interlaced ? 1 : 0);
+
+	if (solo_enc->interlaced)
+		interval = solo_enc->interval - 1;
+	else
+		interval = solo_enc->interval;
+
+	/* Standard encoding only */
+	solo_reg_write(solo_dev, SOLO_VE_CH_GOP(ch), solo_enc->gop);
+	solo_reg_write(solo_dev, SOLO_VE_CH_QP(ch), solo_enc->qp);
+	solo_reg_write(solo_dev, SOLO_CAP_CH_INTV(ch), interval);
+
+	/* Extended encoding only */
+	solo_reg_write(solo_dev, SOLO_VE_CH_GOP_E(ch), solo_enc->gop);
+	solo_reg_write(solo_dev, SOLO_VE_CH_QP_E(ch), solo_enc->qp);
+	solo_reg_write(solo_dev, SOLO_CAP_CH_INTV_E(ch), interval);
+
+	/* Enables the standard encoder */
+	solo_reg_write(solo_dev, SOLO_CAP_CH_SCALE(ch), solo_enc->mode);
+
+	return 0;
+}
+
+static void solo_enc_off(struct solo_enc_dev *solo_enc)
+{
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+
+	solo_dev->enc_bw_remain += solo_enc->bw_weight;
+
+	solo_reg_write(solo_dev, SOLO_CAP_CH_SCALE(solo_enc->ch), 0);
+	solo_reg_write(solo_dev, SOLO_CAP_CH_COMP_ENA_E(solo_enc->ch), 0);
+}
+
+static int enc_get_mpeg_dma(struct solo_dev *solo_dev, dma_addr_t dma,
+			      unsigned int off, unsigned int size)
+{
+	int ret;
+
+	if (off > SOLO_MP4E_EXT_SIZE(solo_dev))
+		return -EINVAL;
+
+	/* Single shot */
+	if (off + size <= SOLO_MP4E_EXT_SIZE(solo_dev)) {
+		return solo_p2m_dma_t(solo_dev, 0, dma,
+				      SOLO_MP4E_EXT_ADDR(solo_dev) + off, size,
+				      0, 0);
+	}
+
+	/* Buffer wrap */
+	ret = solo_p2m_dma_t(solo_dev, 0, dma,
+			     SOLO_MP4E_EXT_ADDR(solo_dev) + off,
+			     SOLO_MP4E_EXT_SIZE(solo_dev) - off, 0, 0);
+
+	if (!ret) {
+		ret = solo_p2m_dma_t(solo_dev, 0,
+			     dma + SOLO_MP4E_EXT_SIZE(solo_dev) - off,
+			     SOLO_MP4E_EXT_ADDR(solo_dev),
+			     size + off - SOLO_MP4E_EXT_SIZE(solo_dev), 0, 0);
+	}
+
+	return ret;
+}
+
+/* Build a descriptor queue out of an SG list and send it to the P2M for
+ * processing. */
+static int solo_send_desc(struct solo_enc_dev *solo_enc, int skip,
+			  struct vb2_dma_sg_desc *vbuf, int off, int size,
+			  unsigned int base, unsigned int base_size)
+{
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+	struct scatterlist *sg;
+	int i;
+	int ret;
+
+	if (WARN_ON_ONCE(size > FRAME_BUF_SIZE))
+		return -EINVAL;
+
+	solo_enc->desc_count = 1;
+
+	for_each_sg(vbuf->sglist, sg, vbuf->num_pages, i) {
+		struct solo_p2m_desc *desc;
+		dma_addr_t dma;
+		int len;
+		int left = base_size - off;
+
+		desc = &solo_enc->desc_items[solo_enc->desc_count++];
+		dma = sg_dma_address(sg);
+		len = sg_dma_len(sg);
+
+		/* We assume this is smaller than the scatter size */
+		BUG_ON(skip >= len);
+		if (skip) {
+			len -= skip;
+			dma += skip;
+			size -= skip;
+			skip = 0;
+		}
+
+		len = min(len, size);
+
+		if (len <= left) {
+			/* Single descriptor */
+			solo_p2m_fill_desc(desc, 0, dma, base + off,
+					   len, 0, 0);
+		} else {
+			/* Buffer wrap */
+			/* XXX: Do these as separate DMA requests, to avoid
+			   timeout errors triggered by awkwardly sized
+			   descriptors. See
+			   <https://github.com/bluecherrydvr/solo6x10/issues/8>
+			 */
+			ret = solo_p2m_dma_t(solo_dev, 0, dma, base + off,
+					     left, 0, 0);
+			if (ret)
+				return ret;
+
+			ret = solo_p2m_dma_t(solo_dev, 0, dma + left, base,
+					     len - left, 0, 0);
+			if (ret)
+				return ret;
+
+			solo_enc->desc_count--;
+		}
+
+		size -= len;
+		if (size <= 0)
+			break;
+
+		off += len;
+		if (off >= base_size)
+			off -= base_size;
+
+		/* Because we may use two descriptors per loop */
+		if (solo_enc->desc_count >= (solo_enc->desc_nelts - 1)) {
+			ret = solo_p2m_dma_desc(solo_dev, solo_enc->desc_items,
+						solo_enc->desc_dma,
+						solo_enc->desc_count - 1);
+			if (ret)
+				return ret;
+			solo_enc->desc_count = 1;
+		}
+	}
+
+	if (solo_enc->desc_count <= 1)
+		return 0;
+
+	return solo_p2m_dma_desc(solo_dev, solo_enc->desc_items, solo_enc->desc_dma,
+				 solo_enc->desc_count - 1);
+}
+
+static int solo_fill_jpeg(struct solo_enc_dev *solo_enc,
+		struct vb2_buffer *vb, struct vop_header *vh)
+{
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+	struct vb2_dma_sg_desc *vbuf = vb2_dma_sg_plane_desc(vb, 0);
+	int frame_size;
+	int ret;
+
+	vb->v4l2_buf.flags |= V4L2_BUF_FLAG_KEYFRAME;
+
+	if (vb2_plane_size(vb, 0) < vh->jpeg_size + solo_enc->jpeg_len)
+		return -EIO;
+
+	sg_copy_from_buffer(vbuf->sglist, vbuf->num_pages,
+			solo_enc->jpeg_header,
+			solo_enc->jpeg_len);
+
+	frame_size = (vh->jpeg_size + solo_enc->jpeg_len + (DMA_ALIGN - 1))
+		& ~(DMA_ALIGN - 1);
+	vb2_set_plane_payload(vb, 0, vh->jpeg_size + solo_enc->jpeg_len);
+
+	dma_map_sg(&solo_dev->pdev->dev, vbuf->sglist, vbuf->num_pages,
+			DMA_FROM_DEVICE);
+	ret = solo_send_desc(solo_enc, solo_enc->jpeg_len, vbuf, vh->jpeg_off,
+			frame_size, SOLO_JPEG_EXT_ADDR(solo_dev),
+			SOLO_JPEG_EXT_SIZE(solo_dev));
+	dma_unmap_sg(&solo_dev->pdev->dev, vbuf->sglist, vbuf->num_pages,
+			DMA_FROM_DEVICE);
+	return ret;
+}
+
+static int solo_fill_mpeg(struct solo_enc_dev *solo_enc,
+		struct vb2_buffer *vb, struct vop_header *vh)
+{
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+	struct vb2_dma_sg_desc *vbuf = vb2_dma_sg_plane_desc(vb, 0);
+	int frame_off, frame_size;
+	int skip = 0;
+	int ret;
+
+	if (vb2_plane_size(vb, 0) < vh->mpeg_size)
+		return -EIO;
+
+	/* If this is a key frame, add extra header */
+	if (!vh->vop_type) {
+		sg_copy_from_buffer(vbuf->sglist, vbuf->num_pages,
+				solo_enc->vop,
+				solo_enc->vop_len);
+
+		skip = solo_enc->vop_len;
+
+		vb->v4l2_buf.flags |= V4L2_BUF_FLAG_KEYFRAME;
+		vb2_set_plane_payload(vb, 0, vh->mpeg_size + solo_enc->vop_len);
+	} else {
+		vb->v4l2_buf.flags |= V4L2_BUF_FLAG_PFRAME;
+		vb2_set_plane_payload(vb, 0, vh->mpeg_size);
+	}
+
+	/* Now get the actual mpeg payload */
+	frame_off = (vh->mpeg_off + sizeof(*vh))
+		% SOLO_MP4E_EXT_SIZE(solo_dev);
+	frame_size = (vh->mpeg_size + skip + (DMA_ALIGN - 1))
+		& ~(DMA_ALIGN - 1);
+
+	dma_map_sg(&solo_dev->pdev->dev, vbuf->sglist, vbuf->num_pages,
+			DMA_FROM_DEVICE);
+	ret = solo_send_desc(solo_enc, skip, vbuf, frame_off, frame_size,
+			SOLO_MP4E_EXT_ADDR(solo_dev),
+			SOLO_MP4E_EXT_SIZE(solo_dev));
+	dma_unmap_sg(&solo_dev->pdev->dev, vbuf->sglist, vbuf->num_pages,
+			DMA_FROM_DEVICE);
+	return ret;
+}
+
+static int solo_enc_fillbuf(struct solo_enc_dev *solo_enc,
+			    struct vb2_buffer *vb, struct solo_enc_buf *enc_buf)
+{
+	struct vop_header *vh = enc_buf->vh;
+	int ret;
+
+	/* Check for motion flags */
+	vb->v4l2_buf.flags &= ~(V4L2_BUF_FLAG_MOTION_ON |
+				V4L2_BUF_FLAG_MOTION_DETECTED);
+	if (solo_is_motion_on(solo_enc)) {
+		vb->v4l2_buf.flags |= V4L2_BUF_FLAG_MOTION_ON;
+		if (enc_buf->motion)
+			vb->v4l2_buf.flags |= V4L2_BUF_FLAG_MOTION_DETECTED;
+	}
+
+	switch (solo_enc->fmt) {
+	case V4L2_PIX_FMT_MPEG4:
+	case V4L2_PIX_FMT_H264:
+		ret = solo_fill_mpeg(solo_enc, vb, vh);
+		break;
+	default: /* V4L2_PIX_FMT_MJPEG */
+		ret = solo_fill_jpeg(solo_enc, vb, vh);
+		break;
+	}
+
+	if (!ret) {
+		vb->v4l2_buf.sequence = solo_enc->sequence++;
+		vb->v4l2_buf.timestamp.tv_sec = vh->sec;
+		vb->v4l2_buf.timestamp.tv_usec = vh->usec;
+	}
+
+	vb2_buffer_done(vb, ret ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+
+	return ret;
+}
+
+static void solo_enc_handle_one(struct solo_enc_dev *solo_enc,
+				struct solo_enc_buf *enc_buf)
+{
+	struct solo_vb2_buf *vb;
+	unsigned long flags;
+
+	mutex_lock(&solo_enc->lock);
+	if (solo_enc->type != enc_buf->type)
+		goto unlock;
+
+	spin_lock_irqsave(&solo_enc->av_lock, flags);
+	if (list_empty(&solo_enc->vidq_active)) {
+		spin_unlock_irqrestore(&solo_enc->av_lock, flags);
+		goto unlock;
+	}
+	vb = list_first_entry(&solo_enc->vidq_active, struct solo_vb2_buf, list);
+	list_del(&vb->list);
+	spin_unlock_irqrestore(&solo_enc->av_lock, flags);
+
+	solo_enc_fillbuf(solo_enc, &vb->vb, enc_buf);
+unlock:
+	mutex_unlock(&solo_enc->lock);
+}
+
+void solo_enc_v4l2_isr(struct solo_dev *solo_dev)
+{
+	wake_up_interruptible_all(&solo_dev->ring_thread_wait);
+}
+
+static void solo_handle_ring(struct solo_dev *solo_dev)
+{
+	for (;;) {
+		struct solo_enc_dev *solo_enc;
+		struct solo_enc_buf enc_buf;
+		u32 mpeg_current, off;
+		u8 ch;
+		u8 cur_q;
+
+		/* Check if the hardware has any new ones in the queue */
+		cur_q = solo_reg_read(solo_dev, SOLO_VE_STATE(11)) & 0xff;
+		if (cur_q == solo_dev->enc_idx)
+			break;
+
+		mpeg_current = solo_reg_read(solo_dev,
+					SOLO_VE_MPEG4_QUE(solo_dev->enc_idx));
+		solo_dev->enc_idx = (solo_dev->enc_idx + 1) % MP4_QS;
+
+		ch = (mpeg_current >> 24) & 0x1f;
+		off = mpeg_current & 0x00ffffff;
+
+		if (ch >= SOLO_MAX_CHANNELS) {
+			ch -= SOLO_MAX_CHANNELS;
+			enc_buf.type = SOLO_ENC_TYPE_EXT;
+		} else
+			enc_buf.type = SOLO_ENC_TYPE_STD;
+
+		solo_enc = solo_dev->v4l2_enc[ch];
+		if (solo_enc == NULL) {
+			dev_err(&solo_dev->pdev->dev,
+				"Got spurious packet for channel %d\n", ch);
+			continue;
+		}
+
+		/* FAIL... */
+		if (enc_get_mpeg_dma(solo_dev, solo_dev->vh_dma, off,
+				     sizeof(struct vop_header)))
+			continue;
+
+		enc_buf.vh = (struct vop_header *)solo_dev->vh_buf;
+		enc_buf.vh->mpeg_off -= SOLO_MP4E_EXT_ADDR(solo_dev);
+		enc_buf.vh->jpeg_off -= SOLO_JPEG_EXT_ADDR(solo_dev);
+
+		/* Sanity check */
+		if (enc_buf.vh->mpeg_off != off)
+			continue;
+
+		if (solo_motion_detected(solo_enc))
+			enc_buf.motion = 1;
+		else
+			enc_buf.motion = 0;
+
+		solo_enc_handle_one(solo_enc, &enc_buf);
+	}
+}
+
+static int solo_ring_thread(void *data)
+{
+	struct solo_dev *solo_dev = data;
+	DECLARE_WAITQUEUE(wait, current);
+
+	set_freezable();
+	add_wait_queue(&solo_dev->ring_thread_wait, &wait);
+
+	for (;;) {
+		long timeout = schedule_timeout_interruptible(HZ);
+		if (timeout == -ERESTARTSYS || kthread_should_stop())
+			break;
+		solo_irq_off(solo_dev, SOLO_IRQ_ENCODER);
+		solo_handle_ring(solo_dev);
+		solo_irq_on(solo_dev, SOLO_IRQ_ENCODER);
+		try_to_freeze();
+	}
+
+	remove_wait_queue(&solo_dev->ring_thread_wait, &wait);
+
+	return 0;
+}
+
+static int solo_enc_queue_setup(struct vb2_queue *q, const struct v4l2_format *fmt,
+			   unsigned int *num_buffers, unsigned int *num_planes,
+			   unsigned int sizes[], void *alloc_ctxs[])
+{
+	sizes[0] = FRAME_BUF_SIZE;
+	*num_planes = 1;
+
+	if (*num_buffers < MIN_VID_BUFFERS)
+		*num_buffers = MIN_VID_BUFFERS;
+
+	return 0;
+}
+
+static void solo_enc_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct solo_enc_dev *solo_enc = vb2_get_drv_priv(vq);
+	struct solo_vb2_buf *solo_vb =
+		container_of(vb, struct solo_vb2_buf, vb);
+
+	spin_lock(&solo_enc->av_lock);
+	list_add_tail(&solo_vb->list, &solo_enc->vidq_active);
+	spin_unlock(&solo_enc->av_lock);
+}
+
+static int solo_ring_start(struct solo_dev *solo_dev)
+{
+	solo_dev->ring_thread = kthread_run(solo_ring_thread, solo_dev,
+					    SOLO6X10_NAME "_ring");
+	if (IS_ERR(solo_dev->ring_thread)) {
+		int err = PTR_ERR(solo_dev->ring_thread);
+		solo_dev->ring_thread = NULL;
+		return err;
+	}
+
+	solo_irq_on(solo_dev, SOLO_IRQ_ENCODER);
+
+	return 0;
+}
+
+static void solo_ring_stop(struct solo_dev *solo_dev)
+{
+	if (solo_dev->ring_thread) {
+		kthread_stop(solo_dev->ring_thread);
+		solo_dev->ring_thread = NULL;
+	}
+
+	solo_irq_off(solo_dev, SOLO_IRQ_ENCODER);
+}
+
+static int solo_enc_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct solo_enc_dev *solo_enc = vb2_get_drv_priv(q);
+	int ret;
+
+	ret = solo_enc_on(solo_enc);
+	if (ret)
+		return ret;
+	return solo_ring_start(solo_enc->solo_dev);
+}
+
+static int solo_enc_stop_streaming(struct vb2_queue *q)
+{
+	struct solo_enc_dev *solo_enc = vb2_get_drv_priv(q);
+
+	solo_enc_off(solo_enc);
+	INIT_LIST_HEAD(&solo_enc->vidq_active);
+	solo_ring_stop(solo_enc->solo_dev);
+	return 0;
+}
+
+static struct vb2_ops solo_enc_video_qops = {
+	.queue_setup	= solo_enc_queue_setup,
+	.buf_queue	= solo_enc_buf_queue,
+	.start_streaming = solo_enc_start_streaming,
+	.stop_streaming = solo_enc_stop_streaming,
+	.wait_prepare	= vb2_ops_wait_prepare,
+	.wait_finish	= vb2_ops_wait_finish,
+};
+
+static int solo_enc_querycap(struct file *file, void  *priv,
+			     struct v4l2_capability *cap)
+{
+	struct solo_enc_dev *solo_enc = video_drvdata(file);
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+
+	strcpy(cap->driver, SOLO6X10_NAME);
+	snprintf(cap->card, sizeof(cap->card), "Softlogic 6x10 Enc %d",
+		 solo_enc->ch);
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "PCI:%s",
+		 pci_name(solo_dev->pdev));
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE |
+			V4L2_CAP_READWRITE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+static int solo_enc_enum_input(struct file *file, void *priv,
+			       struct v4l2_input *input)
+{
+	struct solo_enc_dev *solo_enc = video_drvdata(file);
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+
+	if (input->index)
+		return -EINVAL;
+
+	snprintf(input->name, sizeof(input->name), "Encoder %d",
+		 solo_enc->ch + 1);
+	input->type = V4L2_INPUT_TYPE_CAMERA;
+	input->std = solo_enc->vfd->tvnorms;
+
+	if (!tw28_get_video_status(solo_dev, solo_enc->ch))
+		input->status = V4L2_IN_ST_NO_SIGNAL;
+
+	return 0;
+}
+
+static int solo_enc_set_input(struct file *file, void *priv,
+			      unsigned int index)
+{
+	if (index)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int solo_enc_get_input(struct file *file, void *priv,
+			      unsigned int *index)
+{
+	*index = 0;
+
+	return 0;
+}
+
+static int solo_enc_enum_fmt_cap(struct file *file, void *priv,
+				 struct v4l2_fmtdesc *f)
+{
+	struct solo_enc_dev *solo_enc = video_drvdata(file);
+	int dev_type = solo_enc->solo_dev->type;
+
+	switch (f->index) {
+	case 0:
+		switch (dev_type) {
+		case SOLO_DEV_6010:
+			f->pixelformat = V4L2_PIX_FMT_MPEG4;
+			strcpy(f->description, "MPEG-4 part 2");
+			break;
+		case SOLO_DEV_6110:
+			f->pixelformat = V4L2_PIX_FMT_H264;
+			strcpy(f->description, "H.264");
+			break;
+		}
+		break;
+	case 1:
+		f->pixelformat = V4L2_PIX_FMT_MJPEG;
+		strcpy(f->description, "MJPEG");
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	f->flags = V4L2_FMT_FLAG_COMPRESSED;
+
+	return 0;
+}
+
+static inline int solo_valid_pixfmt(u32 pixfmt, int dev_type)
+{
+	return (pixfmt == V4L2_PIX_FMT_H264 && dev_type == SOLO_DEV_6110)
+		|| (pixfmt == V4L2_PIX_FMT_MPEG4 && dev_type == SOLO_DEV_6010)
+		|| pixfmt == V4L2_PIX_FMT_MJPEG ? 0 : -EINVAL;
+}
+
+static int solo_enc_try_fmt_cap(struct file *file, void *priv,
+			    struct v4l2_format *f)
+{
+	struct solo_enc_dev *solo_enc = video_drvdata(file);
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+
+	if (solo_valid_pixfmt(pix->pixelformat, solo_dev->type))
+		return -EINVAL;
+
+	if (pix->width < solo_dev->video_hsize ||
+	    pix->height < solo_dev->video_vsize << 1) {
+		/* Default to CIF 1/2 size */
+		pix->width = solo_dev->video_hsize >> 1;
+		pix->height = solo_dev->video_vsize;
+	} else {
+		/* Full frame */
+		pix->width = solo_dev->video_hsize;
+		pix->height = solo_dev->video_vsize << 1;
+	}
+
+	switch (pix->field) {
+	case V4L2_FIELD_NONE:
+	case V4L2_FIELD_INTERLACED:
+		break;
+	case V4L2_FIELD_ANY:
+	default:
+		pix->field = V4L2_FIELD_INTERLACED;
+		break;
+	}
+
+	/* Just set these */
+	pix->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	pix->sizeimage = FRAME_BUF_SIZE;
+	pix->bytesperline = 0;
+	pix->priv = 0;
+
+	return 0;
+}
+
+static int solo_enc_set_fmt_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct solo_enc_dev *solo_enc = video_drvdata(file);
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	int ret;
+
+	if (vb2_is_busy(&solo_enc->vidq))
+		return -EBUSY;
+
+	ret = solo_enc_try_fmt_cap(file, priv, f);
+	if (ret)
+		return ret;
+
+	if (pix->width == solo_dev->video_hsize)
+		solo_enc->mode = SOLO_ENC_MODE_D1;
+	else
+		solo_enc->mode = SOLO_ENC_MODE_CIF;
+
+	/* This does not change the encoder at all */
+	solo_enc->fmt = pix->pixelformat;
+
+	/*
+	 * More information is needed about these 'extended' types. As far
+	 * as I can tell these are basically additional video streams with
+	 * different MPEG encoding attributes that can run in parallel with
+	 * the main stream. If so, then this should be implemented as a
+	 * second video node. Abusing priv like this is certainly not the
+	 * right approach.
+	if (pix->priv)
+		solo_enc->type = SOLO_ENC_TYPE_EXT;
+	 */
+	solo_update_mode(solo_enc);
+	return 0;
+}
+
+static int solo_enc_get_fmt_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct solo_enc_dev *solo_enc = video_drvdata(file);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+
+	pix->width = solo_enc->width;
+	pix->height = solo_enc->height;
+	pix->pixelformat = solo_enc->fmt;
+	pix->field = solo_enc->interlaced ? V4L2_FIELD_INTERLACED :
+		     V4L2_FIELD_NONE;
+	pix->sizeimage = FRAME_BUF_SIZE;
+	pix->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	pix->priv = 0;
+
+	return 0;
+}
+
+static int solo_enc_g_std(struct file *file, void *priv, v4l2_std_id *i)
+{
+	struct solo_enc_dev *solo_enc = video_drvdata(file);
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+
+	if (solo_dev->video_type == SOLO_VO_FMT_TYPE_NTSC)
+		*i = V4L2_STD_NTSC_M;
+	else
+		*i = V4L2_STD_PAL;
+	return 0;
+}
+
+static int solo_enc_s_std(struct file *file, void *priv, v4l2_std_id std)
+{
+	struct solo_enc_dev *solo_enc = video_drvdata(file);
+
+	return solo_set_video_type(solo_enc->solo_dev, std & V4L2_STD_PAL);
+}
+
+static int solo_enum_framesizes(struct file *file, void *priv,
+				struct v4l2_frmsizeenum *fsize)
+{
+	struct solo_enc_dev *solo_enc = video_drvdata(file);
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+
+	if (solo_valid_pixfmt(fsize->pixel_format, solo_dev->type))
+		return -EINVAL;
+
+	switch (fsize->index) {
+	case 0:
+		fsize->discrete.width = solo_dev->video_hsize >> 1;
+		fsize->discrete.height = solo_dev->video_vsize;
+		break;
+	case 1:
+		fsize->discrete.width = solo_dev->video_hsize;
+		fsize->discrete.height = solo_dev->video_vsize << 1;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+
+	return 0;
+}
+
+static int solo_enum_frameintervals(struct file *file, void *priv,
+				    struct v4l2_frmivalenum *fintv)
+{
+	struct solo_enc_dev *solo_enc = video_drvdata(file);
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+
+	if (solo_valid_pixfmt(fintv->pixel_format, solo_dev->type))
+		return -EINVAL;
+	if (fintv->index)
+		return -EINVAL;
+	if ((fintv->width != solo_dev->video_hsize >> 1 ||
+	     fintv->height != solo_dev->video_vsize) &&
+	    (fintv->width != solo_dev->video_hsize ||
+	     fintv->height != solo_dev->video_vsize << 1))
+		return -EINVAL;
+
+	fintv->type = V4L2_FRMIVAL_TYPE_STEPWISE;
+
+	fintv->stepwise.min.numerator = 1;
+	fintv->stepwise.min.denominator = solo_dev->fps;
+
+	fintv->stepwise.max.numerator = 15;
+	fintv->stepwise.max.denominator = solo_dev->fps;
+
+	fintv->stepwise.step.numerator = 1;
+	fintv->stepwise.step.denominator = solo_dev->fps;
+
+	return 0;
+}
+
+static int solo_g_parm(struct file *file, void *priv,
+		       struct v4l2_streamparm *sp)
+{
+	struct solo_enc_dev *solo_enc = video_drvdata(file);
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+	struct v4l2_captureparm *cp = &sp->parm.capture;
+
+	cp->capability = V4L2_CAP_TIMEPERFRAME;
+	cp->timeperframe.numerator = solo_enc->interval;
+	cp->timeperframe.denominator = solo_dev->fps;
+	cp->capturemode = 0;
+	/* XXX: Shouldn't we be able to get/set this from videobuf? */
+	cp->readbuffers = 2;
+
+	return 0;
+}
+
+static int solo_s_parm(struct file *file, void *priv,
+		       struct v4l2_streamparm *sp)
+{
+	struct solo_enc_dev *solo_enc = video_drvdata(file);
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+	struct v4l2_captureparm *cp = &sp->parm.capture;
+
+	if (vb2_is_streaming(&solo_enc->vidq))
+		return -EBUSY;
+
+	if ((cp->timeperframe.numerator == 0) ||
+	    (cp->timeperframe.denominator == 0)) {
+		/* reset framerate */
+		cp->timeperframe.numerator = 1;
+		cp->timeperframe.denominator = solo_dev->fps;
+	}
+
+	if (cp->timeperframe.denominator != solo_dev->fps)
+		cp->timeperframe.denominator = solo_dev->fps;
+
+	if (cp->timeperframe.numerator > 15)
+		cp->timeperframe.numerator = 15;
+
+	solo_enc->interval = cp->timeperframe.numerator;
+
+	cp->capability = V4L2_CAP_TIMEPERFRAME;
+	cp->readbuffers = 2;
+
+	solo_update_mode(solo_enc);
+	return 0;
+}
+
+static long solo_enc_default(struct file *file, void *fh,
+			bool valid_prio, unsigned int cmd, void *arg)
+{
+	struct solo_enc_dev *solo_enc = video_drvdata(file);
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+	struct solo_motion_thresholds *thresholds = arg;
+
+	switch (cmd) {
+	case SOLO_IOC_G_MOTION_THRESHOLDS:
+		*thresholds = solo_enc->motion_thresholds;
+		return 0;
+
+	case SOLO_IOC_S_MOTION_THRESHOLDS:
+		if (!valid_prio)
+			return -EBUSY;
+		solo_enc->motion_thresholds = *thresholds;
+		if (solo_enc->motion_enabled && !solo_enc->motion_global)
+			return solo_set_motion_block(solo_dev, solo_enc->ch,
+						&solo_enc->motion_thresholds);
+		return 0;
+	default:
+		return -ENOTTY;
+	}
+}
+
+static int solo_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct solo_enc_dev *solo_enc =
+		container_of(ctrl->handler, struct solo_enc_dev, hdl);
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+	int err;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+	case V4L2_CID_CONTRAST:
+	case V4L2_CID_SATURATION:
+	case V4L2_CID_HUE:
+	case V4L2_CID_SHARPNESS:
+		return tw28_set_ctrl_val(solo_dev, ctrl->id, solo_enc->ch,
+					 ctrl->val);
+	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
+		solo_enc->gop = ctrl->val;
+		return 0;
+	case V4L2_CID_MOTION_THRESHOLD:
+		solo_enc->motion_thresh = ctrl->val;
+		if (!solo_enc->motion_global || !solo_enc->motion_enabled)
+			return 0;
+		return solo_set_motion_threshold(solo_dev, solo_enc->ch, ctrl->val);
+	case V4L2_CID_MOTION_MODE:
+		solo_enc->motion_global = ctrl->val == 1;
+		solo_enc->motion_enabled = ctrl->val > 0;
+		if (ctrl->val) {
+			if (solo_enc->motion_global)
+				solo_set_motion_threshold(solo_dev, solo_enc->ch,
+						solo_enc->motion_thresh);
+			else
+				solo_set_motion_block(solo_dev, solo_enc->ch,
+						&solo_enc->motion_thresholds);
+		}
+		solo_motion_toggle(solo_enc, ctrl->val);
+		return 0;
+	case V4L2_CID_OSD_TEXT:
+		strcpy(solo_enc->osd_text, ctrl->string);
+		err = solo_osd_print(solo_enc);
+		return err;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_file_operations solo_enc_fops = {
+	.owner			= THIS_MODULE,
+	.open			= v4l2_fh_open,
+	.release		= vb2_fop_release,
+	.read			= vb2_fop_read,
+	.poll			= vb2_fop_poll,
+	.mmap			= vb2_fop_mmap,
+	.unlocked_ioctl		= video_ioctl2,
+};
+
+static const struct v4l2_ioctl_ops solo_enc_ioctl_ops = {
+	.vidioc_querycap		= solo_enc_querycap,
+	.vidioc_s_std			= solo_enc_s_std,
+	.vidioc_g_std			= solo_enc_g_std,
+	/* Input callbacks */
+	.vidioc_enum_input		= solo_enc_enum_input,
+	.vidioc_s_input			= solo_enc_set_input,
+	.vidioc_g_input			= solo_enc_get_input,
+	/* Video capture format callbacks */
+	.vidioc_enum_fmt_vid_cap	= solo_enc_enum_fmt_cap,
+	.vidioc_try_fmt_vid_cap		= solo_enc_try_fmt_cap,
+	.vidioc_s_fmt_vid_cap		= solo_enc_set_fmt_cap,
+	.vidioc_g_fmt_vid_cap		= solo_enc_get_fmt_cap,
+	/* Streaming I/O */
+	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+	.vidioc_streamon		= vb2_ioctl_streamon,
+	.vidioc_streamoff		= vb2_ioctl_streamoff,
+	/* Frame size and interval */
+	.vidioc_enum_framesizes		= solo_enum_framesizes,
+	.vidioc_enum_frameintervals	= solo_enum_frameintervals,
+	/* Video capture parameters */
+	.vidioc_s_parm			= solo_s_parm,
+	.vidioc_g_parm			= solo_g_parm,
+	/* Logging and events */
+	.vidioc_log_status		= v4l2_ctrl_log_status,
+	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+	.vidioc_default			= solo_enc_default,
+};
+
+static const struct video_device solo_enc_template = {
+	.name			= SOLO6X10_NAME,
+	.fops			= &solo_enc_fops,
+	.ioctl_ops		= &solo_enc_ioctl_ops,
+	.minor			= -1,
+	.release		= video_device_release,
+	.tvnorms		= V4L2_STD_NTSC_M | V4L2_STD_PAL,
+};
+
+static const struct v4l2_ctrl_ops solo_ctrl_ops = {
+	.s_ctrl = solo_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config solo_motion_threshold_ctrl = {
+	.ops = &solo_ctrl_ops,
+	.id = V4L2_CID_MOTION_THRESHOLD,
+	.name = "Motion Detection Threshold",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.max = 0xffff,
+	.def = SOLO_DEF_MOT_THRESH,
+	.step = 1,
+	.flags = V4L2_CTRL_FLAG_SLIDER,
+};
+
+static const char * const solo_motion_mode_menu[] = {
+	"Disabled",
+	"Global Threshold",
+	"Regional Threshold",
+	NULL
+};
+
+static const struct v4l2_ctrl_config solo_motion_enable_ctrl = {
+	.ops = &solo_ctrl_ops,
+	.id = V4L2_CID_MOTION_MODE,
+	.name = "Motion Detection Mode",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.qmenu = solo_motion_mode_menu,
+	.max = 2,
+};
+
+static const struct v4l2_ctrl_config solo_osd_text_ctrl = {
+	.ops = &solo_ctrl_ops,
+	.id = V4L2_CID_OSD_TEXT,
+	.name = "OSD Text",
+	.type = V4L2_CTRL_TYPE_STRING,
+	.max = OSD_TEXT_MAX,
+	.step = 1,
+};
+
+static struct solo_enc_dev *solo_enc_alloc(struct solo_dev *solo_dev,
+					   u8 ch, unsigned nr)
+{
+	struct solo_enc_dev *solo_enc;
+	struct v4l2_ctrl_handler *hdl;
+	int ret;
+	int x, y;
+
+	solo_enc = kzalloc(sizeof(*solo_enc), GFP_KERNEL);
+	if (!solo_enc)
+		return ERR_PTR(-ENOMEM);
+
+	hdl = &solo_enc->hdl;
+	v4l2_ctrl_handler_init(hdl, 10);
+	v4l2_ctrl_new_std(hdl, &solo_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(hdl, &solo_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(hdl, &solo_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(hdl, &solo_ctrl_ops,
+			V4L2_CID_HUE, 0, 255, 1, 128);
+	if (tw28_has_sharpness(solo_dev, ch))
+		v4l2_ctrl_new_std(hdl, &solo_ctrl_ops,
+			V4L2_CID_SHARPNESS, 0, 15, 1, 0);
+	v4l2_ctrl_new_std(hdl, &solo_ctrl_ops,
+			V4L2_CID_MPEG_VIDEO_GOP_SIZE, 1, 255, 1, solo_dev->fps);
+	v4l2_ctrl_new_custom(hdl, &solo_motion_threshold_ctrl, NULL);
+	v4l2_ctrl_new_custom(hdl, &solo_motion_enable_ctrl, NULL);
+	v4l2_ctrl_new_custom(hdl, &solo_osd_text_ctrl, NULL);
+	if (hdl->error) {
+		ret = hdl->error;
+		goto hdl_free;
+	}
+
+	solo_enc->solo_dev = solo_dev;
+	solo_enc->ch = ch;
+	mutex_init(&solo_enc->lock);
+	spin_lock_init(&solo_enc->av_lock);
+	INIT_LIST_HEAD(&solo_enc->vidq_active);
+	solo_enc->fmt = (solo_dev->type == SOLO_DEV_6010) ?
+		V4L2_PIX_FMT_MPEG4 : V4L2_PIX_FMT_H264;
+	solo_enc->type = SOLO_ENC_TYPE_STD;
+
+	solo_enc->qp = SOLO_DEFAULT_QP;
+	solo_enc->gop = solo_dev->fps;
+	solo_enc->interval = 1;
+	solo_enc->mode = SOLO_ENC_MODE_CIF;
+	solo_enc->motion_global = true;
+	solo_enc->motion_thresh = SOLO_DEF_MOT_THRESH;
+	for (y = 0; y < SOLO_MOTION_SZ; y++)
+		for (x = 0; x < SOLO_MOTION_SZ; x++)
+			solo_enc->motion_thresholds.thresholds[y][x] =
+							SOLO_DEF_MOT_THRESH;
+
+	solo_enc->vidq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	solo_enc->vidq.io_modes = VB2_MMAP | VB2_USERPTR | VB2_READ;
+	solo_enc->vidq.ops = &solo_enc_video_qops;
+	solo_enc->vidq.mem_ops = &vb2_dma_sg_memops;
+	solo_enc->vidq.drv_priv = solo_enc;
+	solo_enc->vidq.gfp_flags = __GFP_DMA32;
+	solo_enc->vidq.timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	solo_enc->vidq.buf_struct_size = sizeof(struct solo_vb2_buf);
+	solo_enc->vidq.lock = &solo_enc->lock;
+	ret = vb2_queue_init(&solo_enc->vidq);
+	if (ret)
+		goto hdl_free;
+	solo_update_mode(solo_enc);
+
+	spin_lock_init(&solo_enc->motion_lock);
+
+	/* Initialize this per encoder */
+	solo_enc->jpeg_len = sizeof(jpeg_header);
+	memcpy(solo_enc->jpeg_header, jpeg_header, solo_enc->jpeg_len);
+
+	solo_enc->desc_nelts = 32;
+	solo_enc->desc_items = pci_alloc_consistent(solo_dev->pdev,
+				      sizeof(struct solo_p2m_desc) *
+				      solo_enc->desc_nelts, &solo_enc->desc_dma);
+	ret = -ENOMEM;
+	if (solo_enc->desc_items == NULL)
+		goto hdl_free;
+
+	solo_enc->vfd = video_device_alloc();
+	if (!solo_enc->vfd)
+		goto pci_free;
+
+	*solo_enc->vfd = solo_enc_template;
+	solo_enc->vfd->v4l2_dev = &solo_dev->v4l2_dev;
+	solo_enc->vfd->ctrl_handler = hdl;
+	solo_enc->vfd->queue = &solo_enc->vidq;
+	solo_enc->vfd->lock = &solo_enc->lock;
+	set_bit(V4L2_FL_USE_FH_PRIO, &solo_enc->vfd->flags);
+	video_set_drvdata(solo_enc->vfd, solo_enc);
+	ret = video_register_device(solo_enc->vfd, VFL_TYPE_GRABBER, nr);
+	if (ret < 0)
+		goto vdev_release;
+
+	snprintf(solo_enc->vfd->name, sizeof(solo_enc->vfd->name),
+		 "%s-enc (%i/%i)", SOLO6X10_NAME, solo_dev->vfd->num,
+		 solo_enc->vfd->num);
+
+	return solo_enc;
+
+vdev_release:
+	video_device_release(solo_enc->vfd);
+pci_free:
+	pci_free_consistent(solo_enc->solo_dev->pdev,
+			sizeof(struct solo_p2m_desc) * solo_enc->desc_nelts,
+			solo_enc->desc_items, solo_enc->desc_dma);
+hdl_free:
+	v4l2_ctrl_handler_free(hdl);
+	kfree(solo_enc);
+	return ERR_PTR(ret);
+}
+
+static void solo_enc_free(struct solo_enc_dev *solo_enc)
+{
+	if (solo_enc == NULL)
+		return;
+
+	video_unregister_device(solo_enc->vfd);
+	v4l2_ctrl_handler_free(&solo_enc->hdl);
+	kfree(solo_enc);
+}
+
+int solo_enc_v4l2_init(struct solo_dev *solo_dev, unsigned nr)
+{
+	int i;
+
+	init_waitqueue_head(&solo_dev->ring_thread_wait);
+
+	solo_dev->vh_size = sizeof(struct vop_header);
+	solo_dev->vh_buf = pci_alloc_consistent(solo_dev->pdev,
+						solo_dev->vh_size,
+						&solo_dev->vh_dma);
+	if (solo_dev->vh_buf == NULL)
+		return -ENOMEM;
+
+	for (i = 0; i < solo_dev->nr_chans; i++) {
+		solo_dev->v4l2_enc[i] = solo_enc_alloc(solo_dev, i, nr);
+		if (IS_ERR(solo_dev->v4l2_enc[i]))
+			break;
+	}
+
+	if (i != solo_dev->nr_chans) {
+		int ret = PTR_ERR(solo_dev->v4l2_enc[i]);
+		while (i--)
+			solo_enc_free(solo_dev->v4l2_enc[i]);
+		pci_free_consistent(solo_dev->pdev, solo_dev->vh_size,
+				    solo_dev->vh_buf, solo_dev->vh_dma);
+		solo_dev->vh_buf = NULL;
+		return ret;
+	}
+
+	if (solo_dev->type == SOLO_DEV_6010)
+		solo_dev->enc_bw_remain = solo_dev->fps * 4 * 4;
+	else
+		solo_dev->enc_bw_remain = solo_dev->fps * 4 * 5;
+
+	dev_info(&solo_dev->pdev->dev, "Encoders as /dev/video%d-%d\n",
+		 solo_dev->v4l2_enc[0]->vfd->num,
+		 solo_dev->v4l2_enc[solo_dev->nr_chans - 1]->vfd->num);
+
+	return 0;
+}
+
+void solo_enc_v4l2_exit(struct solo_dev *solo_dev)
+{
+	int i;
+
+	for (i = 0; i < solo_dev->nr_chans; i++)
+		solo_enc_free(solo_dev->v4l2_enc[i]);
+
+	if (solo_dev->vh_buf)
+		pci_free_consistent(solo_dev->pdev, solo_dev->vh_size,
+			    solo_dev->vh_buf, solo_dev->vh_dma);
+}
diff --git a/drivers/staging/media/solo6x10/solo6x10-v4l2.c b/drivers/staging/media/solo6x10/solo6x10-v4l2.c
new file mode 100644
index 00000000000..7b26de3488d
--- /dev/null
+++ b/drivers/staging/media/solo6x10/solo6x10-v4l2.c
@@ -0,0 +1,734 @@
+/*
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.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., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/kthread.h>
+#include <linux/freezer.h>
+
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-event.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "solo6x10.h"
+#include "solo6x10-tw28.h"
+
+/* Image size is two fields, SOLO_HW_BPL is one horizontal line in hardware */
+#define SOLO_HW_BPL		2048
+#define solo_vlines(__solo)	(__solo->video_vsize * 2)
+#define solo_image_size(__solo) (solo_bytesperline(__solo) * \
+				 solo_vlines(__solo))
+#define solo_bytesperline(__solo) (__solo->video_hsize * 2)
+
+#define MIN_VID_BUFFERS		2
+
+static inline void erase_on(struct solo_dev *solo_dev)
+{
+	solo_reg_write(solo_dev, SOLO_VO_DISP_ERASE, SOLO_VO_DISP_ERASE_ON);
+	solo_dev->erasing = 1;
+	solo_dev->frame_blank = 0;
+}
+
+static inline int erase_off(struct solo_dev *solo_dev)
+{
+	if (!solo_dev->erasing)
+		return 0;
+
+	/* First time around, assert erase off */
+	if (!solo_dev->frame_blank)
+		solo_reg_write(solo_dev, SOLO_VO_DISP_ERASE, 0);
+	/* Keep the erasing flag on for 8 frames minimum */
+	if (solo_dev->frame_blank++ >= 8)
+		solo_dev->erasing = 0;
+
+	return 1;
+}
+
+void solo_video_in_isr(struct solo_dev *solo_dev)
+{
+	wake_up_interruptible_all(&solo_dev->disp_thread_wait);
+}
+
+static void solo_win_setup(struct solo_dev *solo_dev, u8 ch,
+			   int sx, int sy, int ex, int ey, int scale)
+{
+	if (ch >= solo_dev->nr_chans)
+		return;
+
+	/* Here, we just keep window/channel the same */
+	solo_reg_write(solo_dev, SOLO_VI_WIN_CTRL0(ch),
+		       SOLO_VI_WIN_CHANNEL(ch) |
+		       SOLO_VI_WIN_SX(sx) |
+		       SOLO_VI_WIN_EX(ex) |
+		       SOLO_VI_WIN_SCALE(scale));
+
+	solo_reg_write(solo_dev, SOLO_VI_WIN_CTRL1(ch),
+		       SOLO_VI_WIN_SY(sy) |
+		       SOLO_VI_WIN_EY(ey));
+}
+
+static int solo_v4l2_ch_ext_4up(struct solo_dev *solo_dev, u8 idx, int on)
+{
+	u8 ch = idx * 4;
+
+	if (ch >= solo_dev->nr_chans)
+		return -EINVAL;
+
+	if (!on) {
+		u8 i;
+		for (i = ch; i < ch + 4; i++)
+			solo_win_setup(solo_dev, i, solo_dev->video_hsize,
+				       solo_vlines(solo_dev),
+				       solo_dev->video_hsize,
+				       solo_vlines(solo_dev), 0);
+		return 0;
+	}
+
+	/* Row 1 */
+	solo_win_setup(solo_dev, ch, 0, 0, solo_dev->video_hsize / 2,
+		       solo_vlines(solo_dev) / 2, 3);
+	solo_win_setup(solo_dev, ch + 1, solo_dev->video_hsize / 2, 0,
+		       solo_dev->video_hsize, solo_vlines(solo_dev) / 2, 3);
+	/* Row 2 */
+	solo_win_setup(solo_dev, ch + 2, 0, solo_vlines(solo_dev) / 2,
+		       solo_dev->video_hsize / 2, solo_vlines(solo_dev), 3);
+	solo_win_setup(solo_dev, ch + 3, solo_dev->video_hsize / 2,
+		       solo_vlines(solo_dev) / 2, solo_dev->video_hsize,
+		       solo_vlines(solo_dev), 3);
+
+	return 0;
+}
+
+static int solo_v4l2_ch_ext_16up(struct solo_dev *solo_dev, int on)
+{
+	int sy, ysize, hsize, i;
+
+	if (!on) {
+		for (i = 0; i < 16; i++)
+			solo_win_setup(solo_dev, i, solo_dev->video_hsize,
+				       solo_vlines(solo_dev),
+				       solo_dev->video_hsize,
+				       solo_vlines(solo_dev), 0);
+		return 0;
+	}
+
+	ysize = solo_vlines(solo_dev) / 4;
+	hsize = solo_dev->video_hsize / 4;
+
+	for (sy = 0, i = 0; i < 4; i++, sy += ysize) {
+		solo_win_setup(solo_dev, i * 4, 0, sy, hsize,
+			       sy + ysize, 5);
+		solo_win_setup(solo_dev, (i * 4) + 1, hsize, sy,
+			       hsize * 2, sy + ysize, 5);
+		solo_win_setup(solo_dev, (i * 4) + 2, hsize * 2, sy,
+			       hsize * 3, sy + ysize, 5);
+		solo_win_setup(solo_dev, (i * 4) + 3, hsize * 3, sy,
+			       solo_dev->video_hsize, sy + ysize, 5);
+	}
+
+	return 0;
+}
+
+static int solo_v4l2_ch(struct solo_dev *solo_dev, u8 ch, int on)
+{
+	u8 ext_ch;
+
+	if (ch < solo_dev->nr_chans) {
+		solo_win_setup(solo_dev, ch, on ? 0 : solo_dev->video_hsize,
+			       on ? 0 : solo_vlines(solo_dev),
+			       solo_dev->video_hsize, solo_vlines(solo_dev),
+			       on ? 1 : 0);
+		return 0;
+	}
+
+	if (ch >= solo_dev->nr_chans + solo_dev->nr_ext)
+		return -EINVAL;
+
+	ext_ch = ch - solo_dev->nr_chans;
+
+	/* 4up's first */
+	if (ext_ch < 4)
+		return solo_v4l2_ch_ext_4up(solo_dev, ext_ch, on);
+
+	/* Remaining case is 16up for 16-port */
+	return solo_v4l2_ch_ext_16up(solo_dev, on);
+}
+
+static int solo_v4l2_set_ch(struct solo_dev *solo_dev, u8 ch)
+{
+	if (ch >= solo_dev->nr_chans + solo_dev->nr_ext)
+		return -EINVAL;
+
+	erase_on(solo_dev);
+
+	solo_v4l2_ch(solo_dev, solo_dev->cur_disp_ch, 0);
+	solo_v4l2_ch(solo_dev, ch, 1);
+
+	solo_dev->cur_disp_ch = ch;
+
+	return 0;
+}
+
+static void solo_fillbuf(struct solo_dev *solo_dev,
+			 struct vb2_buffer *vb)
+{
+	dma_addr_t vbuf;
+	unsigned int fdma_addr;
+	int error = -1;
+	int i;
+
+	vbuf = vb2_dma_contig_plane_dma_addr(vb, 0);
+	if (!vbuf)
+		goto finish_buf;
+
+	if (erase_off(solo_dev)) {
+		void *p = vb2_plane_vaddr(vb, 0);
+		int image_size = solo_image_size(solo_dev);
+		for (i = 0; i < image_size; i += 2) {
+			((u8 *)p)[i] = 0x80;
+			((u8 *)p)[i + 1] = 0x00;
+		}
+		error = 0;
+	} else {
+		fdma_addr = SOLO_DISP_EXT_ADDR + (solo_dev->old_write *
+				(SOLO_HW_BPL * solo_vlines(solo_dev)));
+
+		error = solo_p2m_dma_t(solo_dev, 0, vbuf, fdma_addr,
+				       solo_bytesperline(solo_dev),
+				       solo_vlines(solo_dev), SOLO_HW_BPL);
+	}
+
+finish_buf:
+	if (!error) {
+		vb2_set_plane_payload(vb, 0,
+			solo_vlines(solo_dev) * solo_bytesperline(solo_dev));
+		vb->v4l2_buf.sequence = solo_dev->sequence++;
+		v4l2_get_timestamp(&vb->v4l2_buf.timestamp);
+	}
+
+	vb2_buffer_done(vb, error ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+}
+
+static void solo_thread_try(struct solo_dev *solo_dev)
+{
+	struct solo_vb2_buf *vb;
+
+	/* Only "break" from this loop if slock is held, otherwise
+	 * just return. */
+	for (;;) {
+		unsigned int cur_write;
+
+		cur_write = SOLO_VI_STATUS0_PAGE(
+			solo_reg_read(solo_dev, SOLO_VI_STATUS0));
+		if (cur_write == solo_dev->old_write)
+			return;
+
+		spin_lock(&solo_dev->slock);
+
+		if (list_empty(&solo_dev->vidq_active))
+			break;
+
+		vb = list_first_entry(&solo_dev->vidq_active, struct solo_vb2_buf,
+				      list);
+
+		solo_dev->old_write = cur_write;
+		list_del(&vb->list);
+
+		spin_unlock(&solo_dev->slock);
+
+		solo_fillbuf(solo_dev, &vb->vb);
+	}
+
+	assert_spin_locked(&solo_dev->slock);
+	spin_unlock(&solo_dev->slock);
+}
+
+static int solo_thread(void *data)
+{
+	struct solo_dev *solo_dev = data;
+	DECLARE_WAITQUEUE(wait, current);
+
+	set_freezable();
+	add_wait_queue(&solo_dev->disp_thread_wait, &wait);
+
+	for (;;) {
+		long timeout = schedule_timeout_interruptible(HZ);
+		if (timeout == -ERESTARTSYS || kthread_should_stop())
+			break;
+		solo_thread_try(solo_dev);
+		try_to_freeze();
+	}
+
+	remove_wait_queue(&solo_dev->disp_thread_wait, &wait);
+
+	return 0;
+}
+
+static int solo_start_thread(struct solo_dev *solo_dev)
+{
+	int ret = 0;
+
+	solo_dev->kthread = kthread_run(solo_thread, solo_dev, SOLO6X10_NAME "_disp");
+
+	if (IS_ERR(solo_dev->kthread)) {
+		ret = PTR_ERR(solo_dev->kthread);
+		solo_dev->kthread = NULL;
+		return ret;
+	}
+	solo_irq_on(solo_dev, SOLO_IRQ_VIDEO_IN);
+
+	return ret;
+}
+
+static void solo_stop_thread(struct solo_dev *solo_dev)
+{
+	if (!solo_dev->kthread)
+		return;
+
+	solo_irq_off(solo_dev, SOLO_IRQ_VIDEO_IN);
+	kthread_stop(solo_dev->kthread);
+	solo_dev->kthread = NULL;
+}
+
+static int solo_queue_setup(struct vb2_queue *q, const struct v4l2_format *fmt,
+			   unsigned int *num_buffers, unsigned int *num_planes,
+			   unsigned int sizes[], void *alloc_ctxs[])
+{
+	struct solo_dev *solo_dev = vb2_get_drv_priv(q);
+
+	sizes[0] = solo_image_size(solo_dev);
+	alloc_ctxs[0] = solo_dev->alloc_ctx;
+	*num_planes = 1;
+
+	if (*num_buffers < MIN_VID_BUFFERS)
+		*num_buffers = MIN_VID_BUFFERS;
+
+	return 0;
+}
+
+static int solo_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct solo_dev *solo_dev = vb2_get_drv_priv(q);
+
+	solo_dev->sequence = 0;
+	return solo_start_thread(solo_dev);
+}
+
+static int solo_stop_streaming(struct vb2_queue *q)
+{
+	struct solo_dev *solo_dev = vb2_get_drv_priv(q);
+
+	solo_stop_thread(solo_dev);
+	INIT_LIST_HEAD(&solo_dev->vidq_active);
+	return 0;
+}
+
+static void solo_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct solo_dev *solo_dev = vb2_get_drv_priv(vq);
+	struct solo_vb2_buf *solo_vb =
+		container_of(vb, struct solo_vb2_buf, vb);
+
+	spin_lock(&solo_dev->slock);
+	list_add_tail(&solo_vb->list, &solo_dev->vidq_active);
+	spin_unlock(&solo_dev->slock);
+	wake_up_interruptible(&solo_dev->disp_thread_wait);
+}
+
+static const struct vb2_ops solo_video_qops = {
+	.queue_setup	= solo_queue_setup,
+	.buf_queue	= solo_buf_queue,
+	.start_streaming = solo_start_streaming,
+	.stop_streaming = solo_stop_streaming,
+	.wait_prepare	= vb2_ops_wait_prepare,
+	.wait_finish	= vb2_ops_wait_finish,
+};
+
+static int solo_querycap(struct file *file, void  *priv,
+			 struct v4l2_capability *cap)
+{
+	struct solo_dev *solo_dev = video_drvdata(file);
+
+	strcpy(cap->driver, SOLO6X10_NAME);
+	strcpy(cap->card, "Softlogic 6x10");
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "PCI:%s",
+		 pci_name(solo_dev->pdev));
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE |
+			V4L2_CAP_READWRITE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+static int solo_enum_ext_input(struct solo_dev *solo_dev,
+			       struct v4l2_input *input)
+{
+	static const char * const dispnames_1[] = { "4UP" };
+	static const char * const dispnames_2[] = { "4UP-1", "4UP-2" };
+	static const char * const dispnames_5[] = {
+		"4UP-1", "4UP-2", "4UP-3", "4UP-4", "16UP"
+	};
+	const char * const *dispnames;
+
+	if (input->index >= (solo_dev->nr_chans + solo_dev->nr_ext))
+		return -EINVAL;
+
+	if (solo_dev->nr_ext == 5)
+		dispnames = dispnames_5;
+	else if (solo_dev->nr_ext == 2)
+		dispnames = dispnames_2;
+	else
+		dispnames = dispnames_1;
+
+	snprintf(input->name, sizeof(input->name), "Multi %s",
+		 dispnames[input->index - solo_dev->nr_chans]);
+
+	return 0;
+}
+
+static int solo_enum_input(struct file *file, void *priv,
+			   struct v4l2_input *input)
+{
+	struct solo_dev *solo_dev = video_drvdata(file);
+
+	if (input->index >= solo_dev->nr_chans) {
+		int ret = solo_enum_ext_input(solo_dev, input);
+		if (ret < 0)
+			return ret;
+	} else {
+		snprintf(input->name, sizeof(input->name), "Camera %d",
+			 input->index + 1);
+
+		/* We can only check this for normal inputs */
+		if (!tw28_get_video_status(solo_dev, input->index))
+			input->status = V4L2_IN_ST_NO_SIGNAL;
+	}
+
+	input->type = V4L2_INPUT_TYPE_CAMERA;
+	input->std = solo_dev->vfd->tvnorms;
+	return 0;
+}
+
+static int solo_set_input(struct file *file, void *priv, unsigned int index)
+{
+	struct solo_dev *solo_dev = video_drvdata(file);
+	int ret = solo_v4l2_set_ch(solo_dev, index);
+
+	if (!ret) {
+		while (erase_off(solo_dev))
+			/* Do nothing */;
+	}
+
+	return ret;
+}
+
+static int solo_get_input(struct file *file, void *priv, unsigned int *index)
+{
+	struct solo_dev *solo_dev = video_drvdata(file);
+
+	*index = solo_dev->cur_disp_ch;
+
+	return 0;
+}
+
+static int solo_enum_fmt_cap(struct file *file, void *priv,
+			     struct v4l2_fmtdesc *f)
+{
+	if (f->index)
+		return -EINVAL;
+
+	f->pixelformat = V4L2_PIX_FMT_UYVY;
+	strlcpy(f->description, "UYUV 4:2:2 Packed", sizeof(f->description));
+
+	return 0;
+}
+
+static int solo_try_fmt_cap(struct file *file, void *priv,
+			    struct v4l2_format *f)
+{
+	struct solo_dev *solo_dev = video_drvdata(file);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	int image_size = solo_image_size(solo_dev);
+
+	if (pix->pixelformat != V4L2_PIX_FMT_UYVY)
+		return -EINVAL;
+
+	pix->width = solo_dev->video_hsize;
+	pix->height = solo_vlines(solo_dev);
+	pix->sizeimage = image_size;
+	pix->field = V4L2_FIELD_INTERLACED;
+	pix->pixelformat = V4L2_PIX_FMT_UYVY;
+	pix->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	pix->priv = 0;
+	return 0;
+}
+
+static int solo_set_fmt_cap(struct file *file, void *priv,
+			    struct v4l2_format *f)
+{
+	struct solo_dev *solo_dev = video_drvdata(file);
+
+	if (vb2_is_busy(&solo_dev->vidq))
+		return -EBUSY;
+
+	/* For right now, if it doesn't match our running config,
+	 * then fail */
+	return solo_try_fmt_cap(file, priv, f);
+}
+
+static int solo_get_fmt_cap(struct file *file, void *priv,
+			    struct v4l2_format *f)
+{
+	struct solo_dev *solo_dev = video_drvdata(file);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+
+	pix->width = solo_dev->video_hsize;
+	pix->height = solo_vlines(solo_dev);
+	pix->pixelformat = V4L2_PIX_FMT_UYVY;
+	pix->field = V4L2_FIELD_INTERLACED;
+	pix->sizeimage = solo_image_size(solo_dev);
+	pix->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	pix->bytesperline = solo_bytesperline(solo_dev);
+	pix->priv = 0;
+
+	return 0;
+}
+
+static int solo_g_std(struct file *file, void *priv, v4l2_std_id *i)
+{
+	struct solo_dev *solo_dev = video_drvdata(file);
+
+	if (solo_dev->video_type == SOLO_VO_FMT_TYPE_NTSC)
+		*i = V4L2_STD_NTSC_M;
+	else
+		*i = V4L2_STD_PAL;
+	return 0;
+}
+
+int solo_set_video_type(struct solo_dev *solo_dev, bool type)
+{
+	int i;
+
+	/* Make sure all video nodes are idle */
+	if (vb2_is_busy(&solo_dev->vidq))
+		return -EBUSY;
+	for (i = 0; i < solo_dev->nr_chans; i++)
+		if (vb2_is_busy(&solo_dev->v4l2_enc[i]->vidq))
+			return -EBUSY;
+	solo_dev->video_type = type;
+	/* Reconfigure for the new standard */
+	solo_disp_init(solo_dev);
+	solo_enc_init(solo_dev);
+	solo_tw28_init(solo_dev);
+	for (i = 0; i < solo_dev->nr_chans; i++)
+		solo_update_mode(solo_dev->v4l2_enc[i]);
+	return solo_v4l2_set_ch(solo_dev, solo_dev->cur_disp_ch);
+}
+
+static int solo_s_std(struct file *file, void *priv, v4l2_std_id std)
+{
+	struct solo_dev *solo_dev = video_drvdata(file);
+
+	return solo_set_video_type(solo_dev, std & V4L2_STD_PAL);
+}
+
+static int solo_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct solo_dev *solo_dev =
+		container_of(ctrl->handler, struct solo_dev, disp_hdl);
+
+	switch (ctrl->id) {
+	case V4L2_CID_MOTION_TRACE:
+		if (ctrl->val) {
+			solo_reg_write(solo_dev, SOLO_VI_MOTION_BORDER,
+					SOLO_VI_MOTION_Y_ADD |
+					SOLO_VI_MOTION_Y_VALUE(0x20) |
+					SOLO_VI_MOTION_CB_VALUE(0x10) |
+					SOLO_VI_MOTION_CR_VALUE(0x10));
+			solo_reg_write(solo_dev, SOLO_VI_MOTION_BAR,
+					SOLO_VI_MOTION_CR_ADD |
+					SOLO_VI_MOTION_Y_VALUE(0x10) |
+					SOLO_VI_MOTION_CB_VALUE(0x80) |
+					SOLO_VI_MOTION_CR_VALUE(0x10));
+		} else {
+			solo_reg_write(solo_dev, SOLO_VI_MOTION_BORDER, 0);
+			solo_reg_write(solo_dev, SOLO_VI_MOTION_BAR, 0);
+		}
+		return 0;
+	default:
+		break;
+	}
+	return -EINVAL;
+}
+
+static const struct v4l2_file_operations solo_v4l2_fops = {
+	.owner			= THIS_MODULE,
+	.open			= v4l2_fh_open,
+	.release		= vb2_fop_release,
+	.read			= vb2_fop_read,
+	.poll			= vb2_fop_poll,
+	.mmap			= vb2_fop_mmap,
+	.unlocked_ioctl		= video_ioctl2,
+};
+
+static const struct v4l2_ioctl_ops solo_v4l2_ioctl_ops = {
+	.vidioc_querycap		= solo_querycap,
+	.vidioc_s_std			= solo_s_std,
+	.vidioc_g_std			= solo_g_std,
+	/* Input callbacks */
+	.vidioc_enum_input		= solo_enum_input,
+	.vidioc_s_input			= solo_set_input,
+	.vidioc_g_input			= solo_get_input,
+	/* Video capture format callbacks */
+	.vidioc_enum_fmt_vid_cap	= solo_enum_fmt_cap,
+	.vidioc_try_fmt_vid_cap		= solo_try_fmt_cap,
+	.vidioc_s_fmt_vid_cap		= solo_set_fmt_cap,
+	.vidioc_g_fmt_vid_cap		= solo_get_fmt_cap,
+	/* Streaming I/O */
+	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+	.vidioc_streamon		= vb2_ioctl_streamon,
+	.vidioc_streamoff		= vb2_ioctl_streamoff,
+	/* Logging and events */
+	.vidioc_log_status		= v4l2_ctrl_log_status,
+	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+};
+
+static struct video_device solo_v4l2_template = {
+	.name			= SOLO6X10_NAME,
+	.fops			= &solo_v4l2_fops,
+	.ioctl_ops		= &solo_v4l2_ioctl_ops,
+	.minor			= -1,
+	.release		= video_device_release,
+	.tvnorms		= V4L2_STD_NTSC_M | V4L2_STD_PAL,
+};
+
+static const struct v4l2_ctrl_ops solo_ctrl_ops = {
+	.s_ctrl = solo_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config solo_motion_trace_ctrl = {
+	.ops = &solo_ctrl_ops,
+	.id = V4L2_CID_MOTION_TRACE,
+	.name = "Motion Detection Trace",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+int solo_v4l2_init(struct solo_dev *solo_dev, unsigned nr)
+{
+	int ret;
+	int i;
+
+	init_waitqueue_head(&solo_dev->disp_thread_wait);
+	spin_lock_init(&solo_dev->slock);
+	mutex_init(&solo_dev->lock);
+	INIT_LIST_HEAD(&solo_dev->vidq_active);
+
+	solo_dev->vfd = video_device_alloc();
+	if (!solo_dev->vfd)
+		return -ENOMEM;
+
+	*solo_dev->vfd = solo_v4l2_template;
+	solo_dev->vfd->v4l2_dev = &solo_dev->v4l2_dev;
+	solo_dev->vfd->queue = &solo_dev->vidq;
+	solo_dev->vfd->lock = &solo_dev->lock;
+	v4l2_ctrl_handler_init(&solo_dev->disp_hdl, 1);
+	v4l2_ctrl_new_custom(&solo_dev->disp_hdl, &solo_motion_trace_ctrl, NULL);
+	if (solo_dev->disp_hdl.error) {
+		ret = solo_dev->disp_hdl.error;
+		goto fail;
+	}
+	solo_dev->vfd->ctrl_handler = &solo_dev->disp_hdl;
+	set_bit(V4L2_FL_USE_FH_PRIO, &solo_dev->vfd->flags);
+
+	video_set_drvdata(solo_dev->vfd, solo_dev);
+
+	solo_dev->vidq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	solo_dev->vidq.io_modes = VB2_MMAP | VB2_USERPTR | VB2_READ;
+	solo_dev->vidq.ops = &solo_video_qops;
+	solo_dev->vidq.mem_ops = &vb2_dma_contig_memops;
+	solo_dev->vidq.drv_priv = solo_dev;
+	solo_dev->vidq.timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	solo_dev->vidq.gfp_flags = __GFP_DMA32;
+	solo_dev->vidq.buf_struct_size = sizeof(struct solo_vb2_buf);
+	solo_dev->vidq.lock = &solo_dev->lock;
+	ret = vb2_queue_init(&solo_dev->vidq);
+	if (ret < 0)
+		goto fail;
+
+	solo_dev->alloc_ctx = vb2_dma_contig_init_ctx(&solo_dev->pdev->dev);
+	if (IS_ERR(solo_dev->alloc_ctx)) {
+		dev_err(&solo_dev->pdev->dev, "Can't allocate buffer context");
+		return PTR_ERR(solo_dev->alloc_ctx);
+	}
+
+	/* Cycle all the channels and clear */
+	for (i = 0; i < solo_dev->nr_chans; i++) {
+		solo_v4l2_set_ch(solo_dev, i);
+		while (erase_off(solo_dev))
+			/* Do nothing */;
+	}
+
+	/* Set the default display channel */
+	solo_v4l2_set_ch(solo_dev, 0);
+	while (erase_off(solo_dev))
+		/* Do nothing */;
+
+	ret = video_register_device(solo_dev->vfd, VFL_TYPE_GRABBER, nr);
+	if (ret < 0)
+		goto fail;
+
+	snprintf(solo_dev->vfd->name, sizeof(solo_dev->vfd->name), "%s (%i)",
+		 SOLO6X10_NAME, solo_dev->vfd->num);
+
+	dev_info(&solo_dev->pdev->dev, "Display as /dev/video%d with "
+		 "%d inputs (%d extended)\n", solo_dev->vfd->num,
+		 solo_dev->nr_chans, solo_dev->nr_ext);
+
+	return 0;
+
+fail:
+	video_device_release(solo_dev->vfd);
+	vb2_dma_contig_cleanup_ctx(solo_dev->alloc_ctx);
+	v4l2_ctrl_handler_free(&solo_dev->disp_hdl);
+	solo_dev->vfd = NULL;
+	return ret;
+}
+
+void solo_v4l2_exit(struct solo_dev *solo_dev)
+{
+	if (solo_dev->vfd == NULL)
+		return;
+
+	video_unregister_device(solo_dev->vfd);
+	vb2_dma_contig_cleanup_ctx(solo_dev->alloc_ctx);
+	v4l2_ctrl_handler_free(&solo_dev->disp_hdl);
+	solo_dev->vfd = NULL;
+}
diff --git a/drivers/staging/media/solo6x10/solo6x10.h b/drivers/staging/media/solo6x10/solo6x10.h
index abee7213202..6f91d2e34b2 100644
--- a/drivers/staging/media/solo6x10/solo6x10.h
+++ b/drivers/staging/media/solo6x10/solo6x10.h
@@ -1,6 +1,11 @@
 /*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.net>
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.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
@@ -20,21 +25,23 @@
 #ifndef __SOLO6X10_H
 #define __SOLO6X10_H
 
-#include <linux/version.h>
 #include <linux/pci.h>
 #include <linux/i2c.h>
-#include <linux/semaphore.h>
 #include <linux/mutex.h>
 #include <linux/list.h>
 #include <linux/wait.h>
-#include <linux/delay.h>
-#include <linux/slab.h>
-#include <asm/io.h>
+#include <linux/stringify.h>
+#include <linux/io.h>
 #include <linux/atomic.h>
+#include <linux/slab.h>
 #include <linux/videodev2.h>
+
 #include <media/v4l2-dev.h>
-#include <media/videobuf-core.h>
-#include "registers.h"
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/videobuf2-core.h>
+
+#include "solo6x10-regs.h"
 
 #ifndef PCI_VENDOR_ID_SOFTLOGIC
 #define PCI_VENDOR_ID_SOFTLOGIC		0x9413
@@ -58,19 +65,15 @@
 #define PCI_DEVICE_ID_BC_6110_16	0x5310
 #endif /* Bluecherry */
 
+/* Used in pci_device_id, and solo_dev->type */
+#define SOLO_DEV_6010			0
+#define SOLO_DEV_6110			1
+
 #define SOLO6X10_NAME			"solo6x10"
 
 #define SOLO_MAX_CHANNELS		16
 
-/* Make sure these two match */
-#define SOLO6X10_VERSION		"2.1.0"
-#define SOLO6X10_VER_MAJOR		2
-#define SOLO6X10_VER_MINOR		0
-#define SOLO6X10_VER_SUB		0
-#define SOLO6X10_VER_NUM \
-	KERNEL_VERSION(SOLO6X10_VER_MAJOR, SOLO6X10_VER_MINOR, SOLO6X10_VER_SUB)
-
-#define FLAGS_6110			1
+#define SOLO6X10_VERSION		"3.0.0"
 
 /*
  * The SOLO6x10 actually has 8 i2c channels, but we only use 2.
@@ -84,16 +87,7 @@
 /* DMA Engine setup */
 #define SOLO_NR_P2M			4
 #define SOLO_NR_P2M_DESC		256
-/* MPEG and JPEG share the same interrupt and locks so they must be together
- * in the same dma channel. */
-#define SOLO_P2M_DMA_ID_MP4E		0
-#define SOLO_P2M_DMA_ID_JPEG		0
-#define SOLO_P2M_DMA_ID_MP4D		1
-#define SOLO_P2M_DMA_ID_G723D		1
-#define SOLO_P2M_DMA_ID_DISP		2
-#define SOLO_P2M_DMA_ID_OSG		2
-#define SOLO_P2M_DMA_ID_G723E		3
-#define SOLO_P2M_DMA_ID_VIN		3
+#define SOLO_P2M_DESC_SIZE		(SOLO_NR_P2M_DESC * 16)
 
 /* Encoder standard modes */
 #define SOLO_ENC_MODE_CIF		2
@@ -103,23 +97,37 @@
 #define SOLO_DEFAULT_GOP		30
 #define SOLO_DEFAULT_QP			3
 
-/* There is 8MB memory available for solo to buffer MPEG4 frames.
- * This gives us 512 * 16kbyte queues. */
-#define SOLO_NR_RING_BUFS		512
-
-#define SOLO_CLOCK_MHZ			108
-
 #ifndef V4L2_BUF_FLAG_MOTION_ON
-#define V4L2_BUF_FLAG_MOTION_ON		0x0400
-#define V4L2_BUF_FLAG_MOTION_DETECTED	0x0800
-#endif
-#ifndef V4L2_CID_MOTION_ENABLE
-#define PRIVATE_CIDS
-#define V4L2_CID_MOTION_ENABLE		(V4L2_CID_PRIVATE_BASE+0)
-#define V4L2_CID_MOTION_THRESHOLD	(V4L2_CID_PRIVATE_BASE+1)
-#define V4L2_CID_MOTION_TRACE		(V4L2_CID_PRIVATE_BASE+2)
+#define V4L2_BUF_FLAG_MOTION_ON		0x10000
+#define V4L2_BUF_FLAG_MOTION_DETECTED	0x20000
 #endif
 
+#define SOLO_CID_CUSTOM_BASE		(V4L2_CID_USER_BASE | 0xf000)
+#define V4L2_CID_MOTION_MODE		(SOLO_CID_CUSTOM_BASE+0)
+#define V4L2_CID_MOTION_THRESHOLD	(SOLO_CID_CUSTOM_BASE+1)
+#define V4L2_CID_MOTION_TRACE		(SOLO_CID_CUSTOM_BASE+2)
+#define V4L2_CID_OSD_TEXT		(SOLO_CID_CUSTOM_BASE+3)
+
+/*
+ * Motion thresholds are in a table of 64x64 samples, with
+ * each sample representing 16x16 pixels of the source. In
+ * effect, 44x30 samples are used for NTSC, and 44x36 for PAL.
+ * The 5th sample on the 10th row is (10*64)+5 = 645.
+ *
+ * Using a 64x64 array will result in a problem on some architectures like
+ * the powerpc where the size of the argument is limited to 13 bits.
+ * Since both PAL and NTSC do not use the full table anyway I've chosen
+ * to limit the array to 45x45 (45*16 = 720, which is the maximum PAL/NTSC
+ * width).
+ */
+#define SOLO_MOTION_SZ (45)
+struct solo_motion_thresholds {
+	__u16	thresholds[SOLO_MOTION_SZ][SOLO_MOTION_SZ];
+};
+
+#define SOLO_IOC_G_MOTION_THRESHOLDS	_IOR('V', BASE_VIDIOC_PRIVATE+0, struct solo_motion_thresholds)
+#define SOLO_IOC_S_MOTION_THRESHOLDS	_IOW('V', BASE_VIDIOC_PRIVATE+1, struct solo_motion_thresholds)
+
 enum SOLO_I2C_STATE {
 	IIC_STATE_IDLE,
 	IIC_STATE_START,
@@ -128,20 +136,29 @@ enum SOLO_I2C_STATE {
 	IIC_STATE_STOP
 };
 
-struct p2m_desc {
-	u32 ctrl;
-	u32 ext;
-	u32 ta;
-	u32 fa;
+/* Defined in Table 4-16, Page 68-69 of the 6010 Datasheet */
+struct solo_p2m_desc {
+	u32	ctrl;
+	u32	cfg;
+	u32	dma_addr;
+	u32	ext_addr;
 };
 
 struct solo_p2m_dev {
 	struct mutex		mutex;
 	struct completion	completion;
+	int			desc_count;
+	int			desc_idx;
+	struct solo_p2m_desc	*descs;
 	int			error;
 };
 
-#define OSD_TEXT_MAX		30
+#define OSD_TEXT_MAX		44
+
+struct solo_vb2_buf {
+	struct vb2_buffer vb;
+	struct list_head list;
+};
 
 enum solo_enc_types {
 	SOLO_ENC_TYPE_STD,
@@ -149,46 +166,61 @@ enum solo_enc_types {
 };
 
 struct solo_enc_dev {
-	struct solo_dev		*solo_dev;
+	struct solo_dev	*solo_dev;
 	/* V4L2 Items */
+	struct v4l2_ctrl_handler hdl;
 	struct video_device	*vfd;
 	/* General accounting */
-	wait_queue_head_t	thread_wait;
-	spinlock_t		lock;
-	atomic_t		readers;
+	struct mutex		lock;
+	spinlock_t		motion_lock;
 	u8			ch;
 	u8			mode, gop, qp, interlaced, interval;
-	u8			reset_gop;
 	u8			bw_weight;
-	u8			motion_detected;
 	u16			motion_thresh;
+	struct solo_motion_thresholds motion_thresholds;
+	bool			motion_global;
+	bool			motion_enabled;
 	u16			width;
 	u16			height;
+
+	/* OSD buffers */
 	char			osd_text[OSD_TEXT_MAX + 1];
-};
+	u8			osd_buf[SOLO_EOSD_EXT_SIZE_MAX]
+					__aligned(4);
 
-struct solo_enc_buf {
-	u8			vop;
-	u8			ch;
+	/* VOP stuff */
+	unsigned char		vop[64];
+	int			vop_len;
+	unsigned char		jpeg_header[1024];
+	int			jpeg_len;
+
+	u32			fmt;
 	enum solo_enc_types	type;
-	u32			off;
-	u32			size;
-	u32			jpeg_off;
-	u32			jpeg_size;
-	struct timeval		ts;
+	u32			sequence;
+	struct vb2_queue	vidq;
+	struct list_head	vidq_active;
+	int			desc_count;
+	int			desc_nelts;
+	struct solo_p2m_desc	*desc_items;
+	dma_addr_t		desc_dma;
+	spinlock_t		av_lock;
 };
 
 /* The SOLO6x10 PCI Device */
 struct solo_dev {
 	/* General stuff */
 	struct pci_dev		*pdev;
+	int			type;
+	unsigned int		time_sync;
+	unsigned int		usec_lsb;
+	unsigned int		clock_mhz;
 	u8 __iomem		*reg_base;
 	int			nr_chans;
 	int			nr_ext;
-	u32			flags;
 	u32			irq_mask;
 	u32			motion_mask;
 	spinlock_t		reg_io_lock;
+	struct v4l2_device	v4l2_dev;
 
 	/* tw28xx accounting */
 	u8			tw2865, tw2864, tw2815;
@@ -206,6 +238,9 @@ struct solo_dev {
 
 	/* P2M DMA Engine */
 	struct solo_p2m_dev	p2m_dev[SOLO_NR_P2M];
+	atomic_t		p2m_count;
+	int			p2m_jiffies;
+	unsigned int		p2m_timeouts;
 
 	/* V4L2 Display items */
 	struct video_device	*vfd;
@@ -213,15 +248,13 @@ struct solo_dev {
 	unsigned int		frame_blank;
 	u8			cur_disp_ch;
 	wait_queue_head_t	disp_thread_wait;
+	struct v4l2_ctrl_handler disp_hdl;
 
 	/* V4L2 Encoder items */
 	struct solo_enc_dev	*v4l2_enc[SOLO_MAX_CHANNELS];
 	u16			enc_bw_remain;
 	/* IDX into hw mp4 encoder */
 	u8			enc_idx;
-	/* Our software ring of enc buf references */
-	u16			enc_wr_idx;
-	struct solo_enc_buf	enc_buf[SOLO_NR_RING_BUFS];
 
 	/* Current video settings */
 	u32			video_type;
@@ -230,11 +263,40 @@ struct solo_dev {
 	u16			vin_hstart, vin_vstart;
 	u8			fps;
 
+	/* JPEG Qp setting */
+	spinlock_t      jpeg_qp_lock;
+	u32		jpeg_qp[2];
+
 	/* Audio components */
 	struct snd_card		*snd_card;
 	struct snd_pcm		*snd_pcm;
 	atomic_t		snd_users;
 	int			g723_hw_idx;
+
+	/* sysfs stuffs */
+	struct device		dev;
+	int			sdram_size;
+	struct bin_attribute	sdram_attr;
+	unsigned int		sys_config;
+
+	/* Ring thread */
+	struct task_struct	*ring_thread;
+	wait_queue_head_t	ring_thread_wait;
+
+	/* VOP_HEADER handling */
+	void                    *vh_buf;
+	dma_addr_t		vh_dma;
+	int			vh_size;
+
+	/* Buffer handling */
+	struct vb2_queue	vidq;
+	struct vb2_alloc_ctx	*alloc_ctx;
+	u32			sequence;
+	struct task_struct      *kthread;
+	struct mutex		lock;
+	spinlock_t		slock;
+	int			old_write;
+	struct list_head	vidq_active;
 };
 
 static inline u32 solo_reg_read(struct solo_dev *solo_dev, int reg)
@@ -255,7 +317,8 @@ static inline u32 solo_reg_read(struct solo_dev *solo_dev, int reg)
 	return ret;
 }
 
-static inline void solo_reg_write(struct solo_dev *solo_dev, int reg, u32 data)
+static inline void solo_reg_write(struct solo_dev *solo_dev, int reg,
+				  u32 data)
 {
 	unsigned long flags;
 	u16 val;
@@ -270,10 +333,19 @@ static inline void solo_reg_write(struct solo_dev *solo_dev, int reg, u32 data)
 	spin_unlock_irqrestore(&solo_dev->reg_io_lock, flags);
 }
 
-void solo_irq_on(struct solo_dev *solo_dev, u32 mask);
-void solo_irq_off(struct solo_dev *solo_dev, u32 mask);
+static inline void solo_irq_on(struct solo_dev *dev, u32 mask)
+{
+	dev->irq_mask |= mask;
+	solo_reg_write(dev, SOLO_IRQ_MASK, dev->irq_mask);
+}
+
+static inline void solo_irq_off(struct solo_dev *dev, u32 mask)
+{
+	dev->irq_mask &= ~mask;
+	solo_reg_write(dev, SOLO_IRQ_MASK, dev->irq_mask);
+}
 
-/* Init/exit routeines for subsystems */
+/* Init/exit routines for subsystems */
 int solo_disp_init(struct solo_dev *solo_dev);
 void solo_disp_exit(struct solo_dev *solo_dev);
 
@@ -286,13 +358,13 @@ void solo_i2c_exit(struct solo_dev *solo_dev);
 int solo_p2m_init(struct solo_dev *solo_dev);
 void solo_p2m_exit(struct solo_dev *solo_dev);
 
-int solo_v4l2_init(struct solo_dev *solo_dev);
+int solo_v4l2_init(struct solo_dev *solo_dev, unsigned nr);
 void solo_v4l2_exit(struct solo_dev *solo_dev);
 
 int solo_enc_init(struct solo_dev *solo_dev);
 void solo_enc_exit(struct solo_dev *solo_dev);
 
-int solo_enc_v4l2_init(struct solo_dev *solo_dev);
+int solo_enc_v4l2_init(struct solo_dev *solo_dev, unsigned nr);
 void solo_enc_v4l2_exit(struct solo_dev *solo_dev);
 
 int solo_g723_init(struct solo_dev *solo_dev);
@@ -301,7 +373,7 @@ void solo_g723_exit(struct solo_dev *solo_dev);
 /* ISR's */
 int solo_i2c_isr(struct solo_dev *solo_dev);
 void solo_p2m_isr(struct solo_dev *solo_dev, int id);
-void solo_p2m_error_isr(struct solo_dev *solo_dev, u32 status);
+void solo_p2m_error_isr(struct solo_dev *solo_dev);
 void solo_enc_v4l2_isr(struct solo_dev *solo_dev);
 void solo_g723_isr(struct solo_dev *solo_dev);
 void solo_motion_isr(struct solo_dev *solo_dev);
@@ -313,24 +385,43 @@ void solo_i2c_writebyte(struct solo_dev *solo_dev, int id, u8 addr, u8 off,
 			u8 data);
 
 /* P2M DMA */
-int solo_p2m_dma_t(struct solo_dev *solo_dev, u8 id, int wr,
-		   dma_addr_t dma_addr, u32 ext_addr, u32 size);
-int solo_p2m_dma(struct solo_dev *solo_dev, u8 id, int wr,
-		 void *sys_addr, u32 ext_addr, u32 size);
-int solo_p2m_dma_sg(struct solo_dev *solo_dev, u8 id,
-		    struct p2m_desc *pdesc, int wr,
-		    struct scatterlist *sglist, u32 sg_off,
-		    u32 ext_addr, u32 size);
-void solo_p2m_push_desc(struct p2m_desc *desc, int wr, dma_addr_t dma_addr,
-			u32 ext_addr, u32 size, int repeat, u32 ext_size);
-int solo_p2m_dma_desc(struct solo_dev *solo_dev, u8 id,
-		      struct p2m_desc *desc, int desc_count);
+int solo_p2m_dma_t(struct solo_dev *solo_dev, int wr,
+		   dma_addr_t dma_addr, u32 ext_addr, u32 size,
+		   int repeat, u32 ext_size);
+int solo_p2m_dma(struct solo_dev *solo_dev, int wr,
+		 void *sys_addr, u32 ext_addr, u32 size,
+		 int repeat, u32 ext_size);
+void solo_p2m_fill_desc(struct solo_p2m_desc *desc, int wr,
+			dma_addr_t dma_addr, u32 ext_addr, u32 size,
+			int repeat, u32 ext_size);
+int solo_p2m_dma_desc(struct solo_dev *solo_dev,
+		      struct solo_p2m_desc *desc, dma_addr_t desc_dma,
+		      int desc_cnt);
+
+/* Global s_std ioctl */
+int solo_set_video_type(struct solo_dev *solo_dev, bool type);
+void solo_update_mode(struct solo_enc_dev *solo_enc);
 
 /* Set the threshold for motion detection */
-void solo_set_motion_threshold(struct solo_dev *solo_dev, u8 ch, u16 val);
+int solo_set_motion_threshold(struct solo_dev *solo_dev, u8 ch, u16 val);
+int solo_set_motion_block(struct solo_dev *solo_dev, u8 ch,
+		const struct solo_motion_thresholds *thresholds);
 #define SOLO_DEF_MOT_THRESH		0x0300
 
 /* Write text on OSD */
 int solo_osd_print(struct solo_enc_dev *solo_enc);
 
+/* EEPROM commands */
+unsigned int solo_eeprom_ewen(struct solo_dev *solo_dev, int w_en);
+unsigned short solo_eeprom_read(struct solo_dev *solo_dev, int loc);
+int solo_eeprom_write(struct solo_dev *solo_dev, int loc,
+		      unsigned short data);
+
+/* JPEG Qp functions */
+void solo_s_jpeg_qp(struct solo_dev *solo_dev, unsigned int ch,
+		    unsigned int qp);
+int solo_g_jpeg_qp(struct solo_dev *solo_dev, unsigned int ch);
+
+#define CHK_FLAGS(v, flags) (((v) & (flags)) == (flags))
+
 #endif /* __SOLO6X10_H */
diff --git a/drivers/staging/media/solo6x10/v4l2-enc.c b/drivers/staging/media/solo6x10/v4l2-enc.c
deleted file mode 100644
index 4977e869d5b..00000000000
--- a/drivers/staging/media/solo6x10/v4l2-enc.c
+++ /dev/null
@@ -1,1829 +0,0 @@
-/*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.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., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- */
-
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/kthread.h>
-#include <linux/freezer.h>
-#include <media/v4l2-ioctl.h>
-#include <media/v4l2-common.h>
-#include <media/videobuf-dma-sg.h>
-#include "solo6x10.h"
-#include "tw28.h"
-#include "solo6x10-jpeg.h"
-
-#define MIN_VID_BUFFERS		4
-#define FRAME_BUF_SIZE		(128 * 1024)
-#define MP4_QS			16
-
-static int solo_enc_thread(void *data);
-
-extern unsigned video_nr;
-
-struct solo_enc_fh {
-	struct			solo_enc_dev *enc;
-	u32			fmt;
-	u16			rd_idx;
-	u8			enc_on;
-	enum solo_enc_types	type;
-	struct videobuf_queue	vidq;
-	struct list_head	vidq_active;
-	struct task_struct	*kthread;
-	struct p2m_desc		desc[SOLO_NR_P2M_DESC];
-};
-
-static const u32 solo_user_ctrls[] = {
-	V4L2_CID_BRIGHTNESS,
-	V4L2_CID_CONTRAST,
-	V4L2_CID_SATURATION,
-	V4L2_CID_HUE,
-	V4L2_CID_SHARPNESS,
-	0
-};
-
-static const u32 solo_mpeg_ctrls[] = {
-	V4L2_CID_MPEG_VIDEO_ENCODING,
-	V4L2_CID_MPEG_VIDEO_GOP_SIZE,
-	0
-};
-
-static const u32 solo_private_ctrls[] = {
-	V4L2_CID_MOTION_ENABLE,
-	V4L2_CID_MOTION_THRESHOLD,
-	0
-};
-
-static const u32 solo_fmtx_ctrls[] = {
-	V4L2_CID_RDS_TX_RADIO_TEXT,
-	0
-};
-
-static const u32 *solo_ctrl_classes[] = {
-	solo_user_ctrls,
-	solo_mpeg_ctrls,
-	solo_fmtx_ctrls,
-	solo_private_ctrls,
-	NULL
-};
-
-static int solo_is_motion_on(struct solo_enc_dev *solo_enc)
-{
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-	u8 ch = solo_enc->ch;
-
-	if (solo_dev->motion_mask & (1 << ch))
-		return 1;
-	return 0;
-}
-
-static void solo_motion_toggle(struct solo_enc_dev *solo_enc, int on)
-{
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-	u8 ch = solo_enc->ch;
-
-	spin_lock(&solo_enc->lock);
-
-	if (on)
-		solo_dev->motion_mask |= (1 << ch);
-	else
-		solo_dev->motion_mask &= ~(1 << ch);
-
-	/* Do this regardless of if we are turning on or off */
-	solo_reg_write(solo_enc->solo_dev, SOLO_VI_MOT_CLEAR,
-		       1 << solo_enc->ch);
-	solo_enc->motion_detected = 0;
-
-	solo_reg_write(solo_dev, SOLO_VI_MOT_ADR,
-		       SOLO_VI_MOTION_EN(solo_dev->motion_mask) |
-		       (SOLO_MOTION_EXT_ADDR(solo_dev) >> 16));
-
-	if (solo_dev->motion_mask)
-		solo_irq_on(solo_dev, SOLO_IRQ_MOTION);
-	else
-		solo_irq_off(solo_dev, SOLO_IRQ_MOTION);
-
-	spin_unlock(&solo_enc->lock);
-}
-
-/* Should be called with solo_enc->lock held */
-static void solo_update_mode(struct solo_enc_dev *solo_enc)
-{
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-
-	assert_spin_locked(&solo_enc->lock);
-
-	solo_enc->interlaced = (solo_enc->mode & 0x08) ? 1 : 0;
-	solo_enc->bw_weight = max(solo_dev->fps / solo_enc->interval, 1);
-
-	switch (solo_enc->mode) {
-	case SOLO_ENC_MODE_CIF:
-		solo_enc->width = solo_dev->video_hsize >> 1;
-		solo_enc->height = solo_dev->video_vsize;
-		break;
-	case SOLO_ENC_MODE_D1:
-		solo_enc->width = solo_dev->video_hsize;
-		solo_enc->height = solo_dev->video_vsize << 1;
-		solo_enc->bw_weight <<= 2;
-		break;
-	default:
-		WARN(1, "mode is unknown\n");
-	}
-}
-
-/* Should be called with solo_enc->lock held */
-static int solo_enc_on(struct solo_enc_fh *fh)
-{
-	struct solo_enc_dev *solo_enc = fh->enc;
-	u8 ch = solo_enc->ch;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-	u8 interval;
-
-	assert_spin_locked(&solo_enc->lock);
-
-	if (fh->enc_on)
-		return 0;
-
-	solo_update_mode(solo_enc);
-
-	/* Make sure to bw check on first reader */
-	if (!atomic_read(&solo_enc->readers)) {
-		if (solo_enc->bw_weight > solo_dev->enc_bw_remain)
-			return -EBUSY;
-		else
-			solo_dev->enc_bw_remain -= solo_enc->bw_weight;
-	}
-
-	fh->enc_on = 1;
-	fh->rd_idx = solo_enc->solo_dev->enc_wr_idx;
-
-	if (fh->type == SOLO_ENC_TYPE_EXT)
-		solo_reg_write(solo_dev, SOLO_CAP_CH_COMP_ENA_E(ch), 1);
-
-	if (atomic_inc_return(&solo_enc->readers) > 1)
-		return 0;
-
-	/* Disable all encoding for this channel */
-	solo_reg_write(solo_dev, SOLO_CAP_CH_SCALE(ch), 0);
-
-	/* Common for both std and ext encoding */
-	solo_reg_write(solo_dev, SOLO_VE_CH_INTL(ch),
-		       solo_enc->interlaced ? 1 : 0);
-
-	if (solo_enc->interlaced)
-		interval = solo_enc->interval - 1;
-	else
-		interval = solo_enc->interval;
-
-	/* Standard encoding only */
-	solo_reg_write(solo_dev, SOLO_VE_CH_GOP(ch), solo_enc->gop);
-	solo_reg_write(solo_dev, SOLO_VE_CH_QP(ch), solo_enc->qp);
-	solo_reg_write(solo_dev, SOLO_CAP_CH_INTV(ch), interval);
-
-	/* Extended encoding only */
-	solo_reg_write(solo_dev, SOLO_VE_CH_GOP_E(ch), solo_enc->gop);
-	solo_reg_write(solo_dev, SOLO_VE_CH_QP_E(ch), solo_enc->qp);
-	solo_reg_write(solo_dev, SOLO_CAP_CH_INTV_E(ch), interval);
-
-	/* Enables the standard encoder */
-	solo_reg_write(solo_dev, SOLO_CAP_CH_SCALE(ch), solo_enc->mode);
-
-	/* Settle down Beavis... */
-	mdelay(10);
-
-	return 0;
-}
-
-static void solo_enc_off(struct solo_enc_fh *fh)
-{
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-
-	if (!fh->enc_on)
-		return;
-
-	if (fh->kthread) {
-		kthread_stop(fh->kthread);
-		fh->kthread = NULL;
-	}
-
-	solo_dev->enc_bw_remain += solo_enc->bw_weight;
-	fh->enc_on = 0;
-
-	if (atomic_dec_return(&solo_enc->readers) > 0)
-		return;
-
-	solo_reg_write(solo_dev, SOLO_CAP_CH_SCALE(solo_enc->ch), 0);
-	solo_reg_write(solo_dev, SOLO_CAP_CH_COMP_ENA_E(solo_enc->ch), 0);
-}
-
-static int solo_start_fh_thread(struct solo_enc_fh *fh)
-{
-	struct solo_enc_dev *solo_enc = fh->enc;
-
-	fh->kthread = kthread_run(solo_enc_thread, fh, SOLO6X10_NAME "_enc");
-
-	/* Oops, we had a problem */
-	if (IS_ERR(fh->kthread)) {
-		spin_lock(&solo_enc->lock);
-		solo_enc_off(fh);
-		spin_unlock(&solo_enc->lock);
-
-		return PTR_ERR(fh->kthread);
-	}
-
-	return 0;
-}
-
-static void enc_reset_gop(struct solo_dev *solo_dev, u8 ch)
-{
-	BUG_ON(ch >= solo_dev->nr_chans);
-	solo_reg_write(solo_dev, SOLO_VE_CH_GOP(ch), 1);
-	solo_dev->v4l2_enc[ch]->reset_gop = 1;
-}
-
-static int enc_gop_reset(struct solo_dev *solo_dev, u8 ch, u8 vop)
-{
-	BUG_ON(ch >= solo_dev->nr_chans);
-	if (!solo_dev->v4l2_enc[ch]->reset_gop)
-		return 0;
-	if (vop)
-		return 1;
-	solo_dev->v4l2_enc[ch]->reset_gop = 0;
-	solo_reg_write(solo_dev, SOLO_VE_CH_GOP(ch),
-		       solo_dev->v4l2_enc[ch]->gop);
-	return 0;
-}
-
-static void enc_write_sg(struct scatterlist *sglist, void *buf, int size)
-{
-	struct scatterlist *sg;
-	u8 *src = buf;
-
-	for (sg = sglist; sg && size > 0; sg = sg_next(sg)) {
-		u8 *p = sg_virt(sg);
-		size_t len = sg_dma_len(sg);
-		int i;
-
-		for (i = 0; i < len && size; i++)
-			p[i] = *(src++);
-	}
-}
-
-static int enc_get_mpeg_dma_sg(struct solo_dev *solo_dev,
-			       struct p2m_desc *desc,
-			       struct scatterlist *sglist, int skip,
-			       unsigned int off, unsigned int size)
-{
-	int ret;
-
-	if (off > SOLO_MP4E_EXT_SIZE(solo_dev))
-		return -EINVAL;
-
-	if (off + size <= SOLO_MP4E_EXT_SIZE(solo_dev)) {
-		return solo_p2m_dma_sg(solo_dev, SOLO_P2M_DMA_ID_MP4E,
-				       desc, 0, sglist, skip,
-				       SOLO_MP4E_EXT_ADDR(solo_dev) + off, size);
-	}
-
-	/* Buffer wrap */
-	ret = solo_p2m_dma_sg(solo_dev, SOLO_P2M_DMA_ID_MP4E, desc, 0,
-			      sglist, skip, SOLO_MP4E_EXT_ADDR(solo_dev) + off,
-			      SOLO_MP4E_EXT_SIZE(solo_dev) - off);
-
-	ret |= solo_p2m_dma_sg(solo_dev, SOLO_P2M_DMA_ID_MP4E, desc, 0,
-			       sglist, skip + SOLO_MP4E_EXT_SIZE(solo_dev) - off,
-			       SOLO_MP4E_EXT_ADDR(solo_dev),
-			       size + off - SOLO_MP4E_EXT_SIZE(solo_dev));
-
-	return ret;
-}
-
-static int enc_get_mpeg_dma_t(struct solo_dev *solo_dev,
-			      dma_addr_t buf, unsigned int off,
-			      unsigned int size)
-{
-	int ret;
-
-	if (off > SOLO_MP4E_EXT_SIZE(solo_dev))
-		return -EINVAL;
-
-	if (off + size <= SOLO_MP4E_EXT_SIZE(solo_dev)) {
-		return solo_p2m_dma_t(solo_dev, SOLO_P2M_DMA_ID_MP4E, 0, buf,
-				      SOLO_MP4E_EXT_ADDR(solo_dev) + off, size);
-	}
-
-	/* Buffer wrap */
-	ret = solo_p2m_dma_t(solo_dev, SOLO_P2M_DMA_ID_MP4E, 0, buf,
-			     SOLO_MP4E_EXT_ADDR(solo_dev) + off,
-			     SOLO_MP4E_EXT_SIZE(solo_dev) - off);
-
-	ret |= solo_p2m_dma_t(solo_dev, SOLO_P2M_DMA_ID_MP4E, 0,
-			      buf + SOLO_MP4E_EXT_SIZE(solo_dev) - off,
-			      SOLO_MP4E_EXT_ADDR(solo_dev),
-			      size + off - SOLO_MP4E_EXT_SIZE(solo_dev));
-
-	return ret;
-}
-
-static int enc_get_mpeg_dma(struct solo_dev *solo_dev, void *buf,
-			    unsigned int off, unsigned int size)
-{
-	int ret;
-
-	dma_addr_t dma_addr = pci_map_single(solo_dev->pdev, buf, size,
-					     PCI_DMA_FROMDEVICE);
-	ret = enc_get_mpeg_dma_t(solo_dev, dma_addr, off, size);
-	pci_unmap_single(solo_dev->pdev, dma_addr, size, PCI_DMA_FROMDEVICE);
-
-	return ret;
-}
-
-static int enc_get_jpeg_dma_sg(struct solo_dev *solo_dev,
-			       struct p2m_desc *desc,
-			       struct scatterlist *sglist, int skip,
-			       unsigned int off, unsigned int size)
-{
-	int ret;
-
-	if (off > SOLO_JPEG_EXT_SIZE(solo_dev))
-		return -EINVAL;
-
-	if (off + size <= SOLO_JPEG_EXT_SIZE(solo_dev)) {
-		return solo_p2m_dma_sg(solo_dev, SOLO_P2M_DMA_ID_JPEG,
-				       desc, 0, sglist, skip,
-				       SOLO_JPEG_EXT_ADDR(solo_dev) + off, size);
-	}
-
-	/* Buffer wrap */
-	ret = solo_p2m_dma_sg(solo_dev, SOLO_P2M_DMA_ID_JPEG, desc, 0,
-			      sglist, skip, SOLO_JPEG_EXT_ADDR(solo_dev) + off,
-			      SOLO_JPEG_EXT_SIZE(solo_dev) - off);
-
-	ret |= solo_p2m_dma_sg(solo_dev, SOLO_P2M_DMA_ID_JPEG, desc, 0,
-			       sglist, skip + SOLO_JPEG_EXT_SIZE(solo_dev) - off,
-			       SOLO_JPEG_EXT_ADDR(solo_dev),
-			       size + off - SOLO_JPEG_EXT_SIZE(solo_dev));
-
-	return ret;
-}
-
-/* Returns true of __chk is within the first __range bytes of __off */
-#define OFF_IN_RANGE(__off, __range, __chk) \
-	((__off <= __chk) && ((__off + __range) >= __chk))
-
-static void solo_jpeg_header(struct solo_enc_dev *solo_enc,
-			     struct videobuf_dmabuf *vbuf)
-{
-	struct scatterlist *sg;
-	void *src = jpeg_header;
-	size_t copied = 0;
-	size_t to_copy = sizeof(jpeg_header);
-
-	for (sg = vbuf->sglist; sg && copied < to_copy; sg = sg_next(sg)) {
-		size_t this_copy = min(sg_dma_len(sg),
-				       (unsigned int)(to_copy - copied));
-		u8 *p = sg_virt(sg);
-
-		memcpy(p, src + copied, this_copy);
-
-		if (OFF_IN_RANGE(copied, this_copy, SOF0_START + 5))
-			p[(SOF0_START + 5) - copied] =
-				0xff & (solo_enc->height >> 8);
-		if (OFF_IN_RANGE(copied, this_copy, SOF0_START + 6))
-			p[(SOF0_START + 6) - copied] = 0xff & solo_enc->height;
-		if (OFF_IN_RANGE(copied, this_copy, SOF0_START + 7))
-			p[(SOF0_START + 7) - copied] =
-				0xff & (solo_enc->width >> 8);
-		if (OFF_IN_RANGE(copied, this_copy, SOF0_START + 8))
-			p[(SOF0_START + 8) - copied] = 0xff & solo_enc->width;
-
-		copied += this_copy;
-	}
-}
-
-static int solo_fill_jpeg(struct solo_enc_fh *fh, struct solo_enc_buf *enc_buf,
-			  struct videobuf_buffer *vb,
-			  struct videobuf_dmabuf *vbuf)
-{
-	struct solo_dev *solo_dev = fh->enc->solo_dev;
-	int size = enc_buf->jpeg_size;
-
-	/* Copy the header first (direct write) */
-	solo_jpeg_header(fh->enc, vbuf);
-
-	vb->size = size + sizeof(jpeg_header);
-
-	/* Grab the jpeg frame */
-	return enc_get_jpeg_dma_sg(solo_dev, fh->desc, vbuf->sglist,
-				   sizeof(jpeg_header),
-				   enc_buf->jpeg_off, size);
-}
-
-static inline int vop_interlaced(__le32 *vh)
-{
-	return (__le32_to_cpu(vh[0]) >> 30) & 1;
-}
-
-static inline u32 vop_size(__le32 *vh)
-{
-	return __le32_to_cpu(vh[0]) & 0xFFFFF;
-}
-
-static inline u8 vop_hsize(__le32 *vh)
-{
-	return (__le32_to_cpu(vh[1]) >> 8) & 0xFF;
-}
-
-static inline u8 vop_vsize(__le32 *vh)
-{
-	return __le32_to_cpu(vh[1]) & 0xFF;
-}
-
-/* must be called with *bits % 8 = 0 */
-static void write_bytes(u8 **out, unsigned *bits, const u8 *src, unsigned count)
-{
-	memcpy(*out, src, count);
-	*out += count;
-	*bits += count * 8;
-}
-
-static void write_bits(u8 **out, unsigned *bits, u32 value, unsigned count)
-{
-
-	value <<= 32 - count; // shift to the right
-
-	while (count--) {
-		**out <<= 1;
-		**out |= !!(value & (1 << 31)); /* MSB */
-		value <<= 1;
-		if (++(*bits) % 8 == 0)
-			(*out)++;
-	}
-}
-
-static void write_ue(u8 **out, unsigned *bits, unsigned value) /* H.264 only */
-{
-	uint32_t max = 0, cnt = 0;
-
-	while (value > max) {
-		max = (max + 2) * 2 - 2;
-		cnt++;
-	}
-	write_bits(out, bits, 1, cnt + 1);
-	write_bits(out, bits, ~(max - value), cnt);
-}
-
-static void write_se(u8 **out, unsigned *bits, int value) /* H.264 only */
-{
-	if (value <= 0)
-		write_ue(out, bits, -value * 2);
-	else
-		write_ue(out, bits, value * 2 - 1);
-}
-
-static void write_mpeg4_end(u8 **out, unsigned *bits)
-{
-	write_bits(out, bits, 0, 1);
-	/* align on 32-bit boundary */
-	if (*bits % 32)
-		write_bits(out, bits, 0xFFFFFFFF, 32 - *bits % 32);
-}
-
-static void write_h264_end(u8 **out, unsigned *bits, int align)
-{
-	write_bits(out, bits, 1, 1);
-	while ((*bits) % 8)
-		write_bits(out, bits, 0, 1);
-	if (align)
-		while ((*bits) % 32)
-			write_bits(out, bits, 0, 1);
-}
-
-static void mpeg4_write_vol(u8 **out, struct solo_dev *solo_dev,
-			    __le32 *vh, unsigned fps, unsigned interval)
-{
-	static const u8 hdr[] = {
-		0, 0, 1, 0x00 /* video_object_start_code */,
-		0, 0, 1, 0x20 /* video_object_layer_start_code */
-	};
-	unsigned bits = 0;
-	unsigned width = vop_hsize(vh) << 4;
-	unsigned height = vop_vsize(vh) << 4;
-	unsigned interlaced = vop_interlaced(vh);
-
-	write_bytes(out, &bits, hdr, sizeof(hdr));
-	write_bits(out, &bits,    0,  1); /* random_accessible_vol */
-	write_bits(out, &bits, 0x04,  8); /* video_object_type_indication: main */
-	write_bits(out, &bits,    1,  1); /* is_object_layer_identifier */
-	write_bits(out, &bits,    2,  4); /* video_object_layer_verid: table V2-39 */
-	write_bits(out, &bits,    0,  3); /* video_object_layer_priority */
-	if (solo_dev->video_type == SOLO_VO_FMT_TYPE_NTSC)
-		write_bits(out, &bits,  3,  4); /* aspect_ratio_info, assuming 4:3 */
-	else
-		write_bits(out, &bits,  2,  4);
-	write_bits(out, &bits,    1,  1); /* vol_control_parameters */
-	write_bits(out, &bits,    1,  2); /* chroma_format: 4:2:0 */
-	write_bits(out, &bits,    1,  1); /* low_delay */
-	write_bits(out, &bits,    0,  1); /* vbv_parameters */
-	write_bits(out, &bits,    0,  2); /* video_object_layer_shape: rectangular */
-	write_bits(out, &bits,    1,  1); /* marker_bit */
-	write_bits(out, &bits,  fps, 16); /* vop_time_increment_resolution */
-	write_bits(out, &bits,    1,  1); /* marker_bit */
-	write_bits(out, &bits,    1,  1); /* fixed_vop_rate */
-	write_bits(out, &bits, interval, 15); /* fixed_vop_time_increment */
-	write_bits(out, &bits,    1,  1); /* marker_bit */
-	write_bits(out, &bits, width, 13); /* video_object_layer_width */
-	write_bits(out, &bits,    1,  1); /* marker_bit */
-	write_bits(out, &bits, height, 13); /* video_object_layer_height */
-	write_bits(out, &bits,    1,  1); /* marker_bit */
-	write_bits(out, &bits, interlaced, 1); /* interlaced */
-	write_bits(out, &bits,    1,  1); /* obmc_disable */
-	write_bits(out, &bits,    0,  2); /* sprite_enable */
-	write_bits(out, &bits,    0,  1); /* not_8_bit */
-	write_bits(out, &bits,    1,  0); /* quant_type */
-	write_bits(out, &bits,    0,  1); /* load_intra_quant_mat */
-	write_bits(out, &bits,    0,  1); /* load_nonintra_quant_mat */
-	write_bits(out, &bits,    0,  1); /* quarter_sample */
-	write_bits(out, &bits,    1,  1); /* complexity_estimation_disable */
-	write_bits(out, &bits,    1,  1); /* resync_marker_disable */
-	write_bits(out, &bits,    0,  1); /* data_partitioned */
-	write_bits(out, &bits,    0,  1); /* newpred_enable */
-	write_bits(out, &bits,    0,  1); /* reduced_resolution_vop_enable */
-	write_bits(out, &bits,    0,  1); /* scalability */
-	write_mpeg4_end(out, &bits);
-}
-
-static void h264_write_vol(u8 **out, struct solo_dev *solo_dev, __le32 *vh)
-{
-	static const u8 sps[] = {
-		0, 0, 0, 1 /* start code */, 0x67, 66 /* profile_idc */,
-		0 /* constraints */, 30 /* level_idc */
-	};
-	static const u8 pps[] = {
-		0, 0, 0, 1 /* start code */, 0x68
-	};
-
-	unsigned bits = 0;
-	unsigned mbs_w = vop_hsize(vh);
-	unsigned mbs_h = vop_vsize(vh);
-
-	write_bytes(out, &bits, sps, sizeof(sps));
-	write_ue(out, &bits,   0);	/* seq_parameter_set_id */
-	write_ue(out, &bits,   5);	/* log2_max_frame_num_minus4 */
-	write_ue(out, &bits,   0);	/* pic_order_cnt_type */
-	write_ue(out, &bits,   6);	/* log2_max_pic_order_cnt_lsb_minus4 */
-	write_ue(out, &bits,   1);	/* max_num_ref_frames */
-	write_bits(out, &bits, 0, 1);	/* gaps_in_frame_num_value_allowed_flag */
-	write_ue(out, &bits, mbs_w - 1);	/* pic_width_in_mbs_minus1 */
-	write_ue(out, &bits, mbs_h - 1);	/* pic_height_in_map_units_minus1 */
-	write_bits(out, &bits, 1, 1);	/* frame_mbs_only_flag */
-	write_bits(out, &bits, 1, 1);	/* direct_8x8_frame_field_flag */
-	write_bits(out, &bits, 0, 1);	/* frame_cropping_flag */
-	write_bits(out, &bits, 0, 1);	/* vui_parameters_present_flag */
-	write_h264_end(out, &bits, 0);
-
-	write_bytes(out, &bits, pps, sizeof(pps));
-	write_ue(out, &bits,   0);	/* pic_parameter_set_id */
-	write_ue(out, &bits,   0);	/* seq_parameter_set_id */
-	write_bits(out, &bits, 0, 1);	/* entropy_coding_mode_flag */
-	write_bits(out, &bits, 0, 1);	/* bottom_field_pic_order_in_frame_present_flag */
-	write_ue(out, &bits,   0);	/* num_slice_groups_minus1 */
-	write_ue(out, &bits,   0);	/* num_ref_idx_l0_default_active_minus1 */
-	write_ue(out, &bits,   0);	/* num_ref_idx_l1_default_active_minus1 */
-	write_bits(out, &bits, 0, 1);	/* weighted_pred_flag */
-	write_bits(out, &bits, 0, 2);	/* weighted_bipred_idc */
-	write_se(out, &bits,   0);	/* pic_init_qp_minus26 */
-	write_se(out, &bits,   0);	/* pic_init_qs_minus26 */
-	write_se(out, &bits,   2);	/* chroma_qp_index_offset */
-	write_bits(out, &bits, 0, 1);	/* deblocking_filter_control_present_flag */
-	write_bits(out, &bits, 1, 1);	/* constrained_intra_pred_flag */
-	write_bits(out, &bits, 0, 1);	/* redundant_pic_cnt_present_flag */
-	write_h264_end(out, &bits, 1);
-}
-
-static int solo_fill_mpeg(struct solo_enc_fh *fh, struct solo_enc_buf *enc_buf,
-			  struct videobuf_buffer *vb,
-			  struct videobuf_dmabuf *vbuf)
-{
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-
-#define VH_WORDS 16
-#define MAX_VOL_HEADER_LENGTH 64
-
-	__le32 vh[VH_WORDS];
-	int ret;
-	int frame_size, frame_off;
-	int skip = 0;
-
-	if (WARN_ON_ONCE(enc_buf->size <= sizeof(vh)))
-		return -EINVAL;
-
-	/* First get the hardware vop header (not real mpeg) */
-	ret = enc_get_mpeg_dma(solo_dev, vh, enc_buf->off, sizeof(vh));
-	if (WARN_ON_ONCE(ret))
-		return ret;
-
-	if (WARN_ON_ONCE(vop_size(vh) > enc_buf->size))
-		return -EINVAL;
-
-	vb->width = vop_hsize(vh) << 4;
-	vb->height = vop_vsize(vh) << 4;
-	vb->size = vop_size(vh);
-
-	/* If this is a key frame, add extra m4v header */
-	if (!enc_buf->vop) {
-		u8 header[MAX_VOL_HEADER_LENGTH], *out = header;
-
-		if (solo_dev->flags & FLAGS_6110)
-			h264_write_vol(&out, solo_dev, vh);
-		else
-			mpeg4_write_vol(&out, solo_dev, vh,
-					solo_dev->fps * 1000,
-					solo_enc->interval * 1000);
-		skip = out - header;
-		enc_write_sg(vbuf->sglist, header, skip);
-		/* Adjust the dma buffer past this header */
-		vb->size += skip;
-	}
-
-	/* Now get the actual mpeg payload */
-	frame_off = (enc_buf->off + sizeof(vh)) % SOLO_MP4E_EXT_SIZE(solo_dev);
-	frame_size = enc_buf->size - sizeof(vh);
-
-	ret = enc_get_mpeg_dma_sg(solo_dev, fh->desc, vbuf->sglist,
-				  skip, frame_off, frame_size);
-	WARN_ON_ONCE(ret);
-
-	return ret;
-}
-
-static void solo_enc_fillbuf(struct solo_enc_fh *fh,
-			    struct videobuf_buffer *vb)
-{
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-	struct solo_enc_buf *enc_buf = NULL;
-	struct videobuf_dmabuf *vbuf;
-	int ret;
-	int error = 1;
-	u16 idx = fh->rd_idx;
-
-	while (idx != solo_dev->enc_wr_idx) {
-		struct solo_enc_buf *ebuf = &solo_dev->enc_buf[idx];
-
-		idx = (idx + 1) % SOLO_NR_RING_BUFS;
-
-		if (ebuf->ch != solo_enc->ch)
-			continue;
-
-		if (fh->fmt == V4L2_PIX_FMT_MPEG) {
-			if (fh->type == ebuf->type) {
-				enc_buf = ebuf;
-				break;
-			}
-		} else {
-			/* For mjpeg, keep reading to the newest frame */
-			enc_buf = ebuf;
-		}
-	}
-
-	fh->rd_idx = idx;
-
-	if (WARN_ON_ONCE(!enc_buf))
-		goto buf_err;
-
-	if ((fh->fmt == V4L2_PIX_FMT_MPEG &&
-	     vb->bsize < enc_buf->size) ||
-	    (fh->fmt == V4L2_PIX_FMT_MJPEG &&
-	     vb->bsize < (enc_buf->jpeg_size + sizeof(jpeg_header)))) {
-		WARN_ON_ONCE(1);
-		goto buf_err;
-	}
-
-	vbuf = videobuf_to_dma(vb);
-	if (WARN_ON_ONCE(!vbuf))
-		goto buf_err;
-
-	if (fh->fmt == V4L2_PIX_FMT_MPEG)
-		ret = solo_fill_mpeg(fh, enc_buf, vb, vbuf);
-	else
-		ret = solo_fill_jpeg(fh, enc_buf, vb, vbuf);
-
-	if (!ret)
-		error = 0;
-
-buf_err:
-	if (error) {
-		vb->state = VIDEOBUF_ERROR;
-	} else {
-		vb->field_count++;
-		vb->ts = enc_buf->ts;
-		vb->state = VIDEOBUF_DONE;
-	}
-
-	wake_up(&vb->done);
-
-	return;
-}
-
-static void solo_enc_thread_try(struct solo_enc_fh *fh)
-{
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-	struct videobuf_buffer *vb;
-
-	for (;;) {
-		spin_lock(&solo_enc->lock);
-
-		if (fh->rd_idx == solo_dev->enc_wr_idx)
-			break;
-
-		if (list_empty(&fh->vidq_active))
-			break;
-
-		vb = list_first_entry(&fh->vidq_active,
-				      struct videobuf_buffer, queue);
-
-		if (!waitqueue_active(&vb->done))
-			break;
-
-		list_del(&vb->queue);
-
-		spin_unlock(&solo_enc->lock);
-
-		solo_enc_fillbuf(fh, vb);
-	}
-
-	assert_spin_locked(&solo_enc->lock);
-	spin_unlock(&solo_enc->lock);
-}
-
-static int solo_enc_thread(void *data)
-{
-	struct solo_enc_fh *fh = data;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	DECLARE_WAITQUEUE(wait, current);
-
-	set_freezable();
-	add_wait_queue(&solo_enc->thread_wait, &wait);
-
-	for (;;) {
-		long timeout = schedule_timeout_interruptible(HZ);
-		if (timeout == -ERESTARTSYS || kthread_should_stop())
-			break;
-		solo_enc_thread_try(fh);
-		try_to_freeze();
-	}
-
-	remove_wait_queue(&solo_enc->thread_wait, &wait);
-
-	return 0;
-}
-
-void solo_motion_isr(struct solo_dev *solo_dev)
-{
-	u32 status;
-	int i;
-
-	solo_reg_write(solo_dev, SOLO_IRQ_STAT, SOLO_IRQ_MOTION);
-
-	status = solo_reg_read(solo_dev, SOLO_VI_MOT_STATUS);
-
-	for (i = 0; i < solo_dev->nr_chans; i++) {
-		struct solo_enc_dev *solo_enc = solo_dev->v4l2_enc[i];
-
-		BUG_ON(solo_enc == NULL);
-
-		if (solo_enc->motion_detected)
-			continue;
-		if (!(status & (1 << i)))
-			continue;
-
-		solo_enc->motion_detected = 1;
-	}
-}
-
-void solo_enc_v4l2_isr(struct solo_dev *solo_dev)
-{
-	struct solo_enc_buf *enc_buf;
-	u32 mpeg_current, mpeg_next, mpeg_size;
-	u32 jpeg_current, jpeg_next, jpeg_size;
-	u32 reg_mpeg_size;
-	u8 cur_q, vop_type;
-	u8 ch;
-	enum solo_enc_types enc_type;
-
-	solo_reg_write(solo_dev, SOLO_IRQ_STAT, SOLO_IRQ_ENCODER);
-
-	cur_q = ((solo_reg_read(solo_dev, SOLO_VE_STATE(11)) & 0xF) + 1) % MP4_QS;
-
-	reg_mpeg_size = ((solo_reg_read(solo_dev, SOLO_VE_STATE(0)) & 0xFFFFF) + 64 + 8) & ~7;
-
-	while (solo_dev->enc_idx != cur_q) {
-		mpeg_current = solo_reg_read(solo_dev,
-					SOLO_VE_MPEG4_QUE(solo_dev->enc_idx));
-		jpeg_current = solo_reg_read(solo_dev,
-					SOLO_VE_JPEG_QUE(solo_dev->enc_idx));
-		solo_dev->enc_idx = (solo_dev->enc_idx + 1) % MP4_QS;
-		mpeg_next = solo_reg_read(solo_dev,
-					SOLO_VE_MPEG4_QUE(solo_dev->enc_idx));
-		jpeg_next = solo_reg_read(solo_dev,
-					SOLO_VE_JPEG_QUE(solo_dev->enc_idx));
-
-		ch = (mpeg_current >> 24) & 0x1f;
-		if (ch >= SOLO_MAX_CHANNELS) {
-			ch -= SOLO_MAX_CHANNELS;
-			enc_type = SOLO_ENC_TYPE_EXT;
-		} else
-			enc_type = SOLO_ENC_TYPE_STD;
-
-		vop_type = (mpeg_current >> 29) & 3;
-
-		mpeg_current &= 0x00ffffff;
-		mpeg_next    &= 0x00ffffff;
-		jpeg_current &= 0x00ffffff;
-		jpeg_next    &= 0x00ffffff;
-
-		mpeg_size = (SOLO_MP4E_EXT_SIZE(solo_dev) +
-			     mpeg_next - mpeg_current) %
-			    SOLO_MP4E_EXT_SIZE(solo_dev);
-
-		jpeg_size = (SOLO_JPEG_EXT_SIZE(solo_dev) +
-			     jpeg_next - jpeg_current) %
-			    SOLO_JPEG_EXT_SIZE(solo_dev);
-
-		/* XXX I think this means we had a ring overflow? */
-		if (mpeg_current > mpeg_next && mpeg_size != reg_mpeg_size) {
-			enc_reset_gop(solo_dev, ch);
-			continue;
-		}
-
-		/* When resetting the GOP, skip frames until I-frame */
-		if (enc_gop_reset(solo_dev, ch, vop_type))
-			continue;
-
-		enc_buf = &solo_dev->enc_buf[solo_dev->enc_wr_idx];
-
-		enc_buf->vop = vop_type;
-		enc_buf->ch = ch;
-		enc_buf->off = mpeg_current;
-		enc_buf->size = mpeg_size;
-		enc_buf->jpeg_off = jpeg_current;
-		enc_buf->jpeg_size = jpeg_size;
-		enc_buf->type = enc_type;
-
-		do_gettimeofday(&enc_buf->ts);
-
-		solo_dev->enc_wr_idx = (solo_dev->enc_wr_idx + 1) %
-					SOLO_NR_RING_BUFS;
-
-		wake_up_interruptible(&solo_dev->v4l2_enc[ch]->thread_wait);
-	}
-
-	return;
-}
-
-static int solo_enc_buf_setup(struct videobuf_queue *vq, unsigned int *count,
-			      unsigned int *size)
-{
-	*size = FRAME_BUF_SIZE;
-
-	if (*count < MIN_VID_BUFFERS)
-		*count = MIN_VID_BUFFERS;
-
-	return 0;
-}
-
-static int solo_enc_buf_prepare(struct videobuf_queue *vq,
-				struct videobuf_buffer *vb,
-				enum v4l2_field field)
-{
-	struct solo_enc_fh *fh = vq->priv_data;
-	struct solo_enc_dev *solo_enc = fh->enc;
-
-	vb->size = FRAME_BUF_SIZE;
-	if (vb->baddr != 0 && vb->bsize < vb->size)
-		return -EINVAL;
-
-	/* These properties only change when queue is idle */
-	vb->width = solo_enc->width;
-	vb->height = solo_enc->height;
-	vb->field  = field;
-
-	if (vb->state == VIDEOBUF_NEEDS_INIT) {
-		int rc = videobuf_iolock(vq, vb, NULL);
-		if (rc < 0) {
-			struct videobuf_dmabuf *dma = videobuf_to_dma(vb);
-			videobuf_dma_unmap(vq->dev, dma);
-			videobuf_dma_free(dma);
-			vb->state = VIDEOBUF_NEEDS_INIT;
-			return rc;
-		}
-	}
-	vb->state = VIDEOBUF_PREPARED;
-
-	return 0;
-}
-
-static void solo_enc_buf_queue(struct videobuf_queue *vq,
-			       struct videobuf_buffer *vb)
-{
-	struct solo_enc_fh *fh = vq->priv_data;
-
-	vb->state = VIDEOBUF_QUEUED;
-	list_add_tail(&vb->queue, &fh->vidq_active);
-	wake_up_interruptible(&fh->enc->thread_wait);
-}
-
-static void solo_enc_buf_release(struct videobuf_queue *vq,
-				 struct videobuf_buffer *vb)
-{
-	struct videobuf_dmabuf *dma = videobuf_to_dma(vb);
-
-	videobuf_dma_unmap(vq->dev, dma);
-	videobuf_dma_free(dma);
-	vb->state = VIDEOBUF_NEEDS_INIT;
-}
-
-static struct videobuf_queue_ops solo_enc_video_qops = {
-	.buf_setup	= solo_enc_buf_setup,
-	.buf_prepare	= solo_enc_buf_prepare,
-	.buf_queue	= solo_enc_buf_queue,
-	.buf_release	= solo_enc_buf_release,
-};
-
-static unsigned int solo_enc_poll(struct file *file,
-				  struct poll_table_struct *wait)
-{
-	struct solo_enc_fh *fh = file->private_data;
-
-	return videobuf_poll_stream(file, &fh->vidq, wait);
-}
-
-static int solo_enc_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	struct solo_enc_fh *fh = file->private_data;
-
-	return videobuf_mmap_mapper(&fh->vidq, vma);
-}
-
-static int solo_enc_open(struct file *file)
-{
-	struct solo_enc_dev *solo_enc = video_drvdata(file);
-	struct solo_enc_fh *fh;
-
-	fh = kzalloc(sizeof(*fh), GFP_KERNEL);
-	if (fh == NULL)
-		return -ENOMEM;
-
-	fh->enc = solo_enc;
-	file->private_data = fh;
-	INIT_LIST_HEAD(&fh->vidq_active);
-	fh->fmt = V4L2_PIX_FMT_MPEG;
-	fh->type = SOLO_ENC_TYPE_STD;
-
-	videobuf_queue_sg_init(&fh->vidq, &solo_enc_video_qops,
-			       &solo_enc->solo_dev->pdev->dev,
-			       &solo_enc->lock,
-			       V4L2_BUF_TYPE_VIDEO_CAPTURE,
-			       V4L2_FIELD_INTERLACED,
-			       sizeof(struct videobuf_buffer), fh, NULL);
-
-	return 0;
-}
-
-static ssize_t solo_enc_read(struct file *file, char __user *data,
-			     size_t count, loff_t *ppos)
-{
-	struct solo_enc_fh *fh = file->private_data;
-	struct solo_enc_dev *solo_enc = fh->enc;
-
-	/* Make sure the encoder is on */
-	if (!fh->enc_on) {
-		int ret;
-
-		spin_lock(&solo_enc->lock);
-		ret = solo_enc_on(fh);
-		spin_unlock(&solo_enc->lock);
-		if (ret)
-			return ret;
-
-		ret = solo_start_fh_thread(fh);
-		if (ret)
-			return ret;
-	}
-
-	return videobuf_read_stream(&fh->vidq, data, count, ppos, 0,
-				    file->f_flags & O_NONBLOCK);
-}
-
-static int solo_enc_release(struct file *file)
-{
-	struct solo_enc_fh *fh = file->private_data;
-	struct solo_enc_dev *solo_enc = fh->enc;
-
-	videobuf_stop(&fh->vidq);
-	videobuf_mmap_free(&fh->vidq);
-
-	spin_lock(&solo_enc->lock);
-	solo_enc_off(fh);
-	spin_unlock(&solo_enc->lock);
-
-	kfree(fh);
-
-	return 0;
-}
-
-static int solo_enc_querycap(struct file *file, void  *priv,
-			     struct v4l2_capability *cap)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-
-	strcpy(cap->driver, SOLO6X10_NAME);
-	snprintf(cap->card, sizeof(cap->card), "Softlogic 6x10 Enc %d",
-		 solo_enc->ch);
-	snprintf(cap->bus_info, sizeof(cap->bus_info), "PCI %s",
-		 pci_name(solo_dev->pdev));
-	cap->version = SOLO6X10_VER_NUM;
-	cap->capabilities =     V4L2_CAP_VIDEO_CAPTURE |
-				V4L2_CAP_READWRITE |
-				V4L2_CAP_STREAMING;
-	return 0;
-}
-
-static int solo_enc_enum_input(struct file *file, void *priv,
-			       struct v4l2_input *input)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-
-	if (input->index)
-		return -EINVAL;
-
-	snprintf(input->name, sizeof(input->name), "Encoder %d",
-		 solo_enc->ch + 1);
-	input->type = V4L2_INPUT_TYPE_CAMERA;
-
-	if (solo_dev->video_type == SOLO_VO_FMT_TYPE_NTSC)
-		input->std = V4L2_STD_NTSC_M;
-	else
-		input->std = V4L2_STD_PAL_B;
-
-	if (!tw28_get_video_status(solo_dev, solo_enc->ch))
-		input->status = V4L2_IN_ST_NO_SIGNAL;
-
-	return 0;
-}
-
-static int solo_enc_set_input(struct file *file, void *priv, unsigned int index)
-{
-	if (index)
-		return -EINVAL;
-
-	return 0;
-}
-
-static int solo_enc_get_input(struct file *file, void *priv,
-			      unsigned int *index)
-{
-	*index = 0;
-
-	return 0;
-}
-
-static int solo_enc_enum_fmt_cap(struct file *file, void *priv,
-				 struct v4l2_fmtdesc *f)
-{
-	switch (f->index) {
-	case 0:
-		f->pixelformat = V4L2_PIX_FMT_MPEG;
-		strcpy(f->description, "MPEG-4 AVC");
-		break;
-	case 1:
-		f->pixelformat = V4L2_PIX_FMT_MJPEG;
-		strcpy(f->description, "MJPEG");
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	f->flags = V4L2_FMT_FLAG_COMPRESSED;
-
-	return 0;
-}
-
-static int solo_enc_try_fmt_cap(struct file *file, void *priv,
-			    struct v4l2_format *f)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-	struct v4l2_pix_format *pix = &f->fmt.pix;
-
-	if (pix->pixelformat != V4L2_PIX_FMT_MPEG &&
-	    pix->pixelformat != V4L2_PIX_FMT_MJPEG)
-		return -EINVAL;
-
-	/* We cannot change width/height in mid read */
-	if (atomic_read(&solo_enc->readers) > 0) {
-		if (pix->width != solo_enc->width ||
-		    pix->height != solo_enc->height)
-			return -EBUSY;
-	}
-
-	if (pix->width < solo_dev->video_hsize ||
-	    pix->height < solo_dev->video_vsize << 1) {
-		/* Default to CIF 1/2 size */
-		pix->width = solo_dev->video_hsize >> 1;
-		pix->height = solo_dev->video_vsize;
-	} else {
-		/* Full frame */
-		pix->width = solo_dev->video_hsize;
-		pix->height = solo_dev->video_vsize << 1;
-	}
-
-	if (pix->field == V4L2_FIELD_ANY)
-		pix->field = V4L2_FIELD_INTERLACED;
-	else if (pix->field != V4L2_FIELD_INTERLACED)
-		pix->field = V4L2_FIELD_INTERLACED;
-
-	/* Just set these */
-	pix->colorspace = V4L2_COLORSPACE_SMPTE170M;
-	pix->sizeimage = FRAME_BUF_SIZE;
-
-	return 0;
-}
-
-static int solo_enc_set_fmt_cap(struct file *file, void *priv,
-				struct v4l2_format *f)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-	struct v4l2_pix_format *pix = &f->fmt.pix;
-	int ret;
-
-	spin_lock(&solo_enc->lock);
-
-	ret = solo_enc_try_fmt_cap(file, priv, f);
-	if (ret) {
-		spin_unlock(&solo_enc->lock);
-		return ret;
-	}
-
-	if (pix->width == solo_dev->video_hsize)
-		solo_enc->mode = SOLO_ENC_MODE_D1;
-	else
-		solo_enc->mode = SOLO_ENC_MODE_CIF;
-
-	/* This does not change the encoder at all */
-	fh->fmt = pix->pixelformat;
-
-	if (pix->priv)
-		fh->type = SOLO_ENC_TYPE_EXT;
-	ret = solo_enc_on(fh);
-
-	spin_unlock(&solo_enc->lock);
-
-	if (ret)
-		return ret;
-
-	return solo_start_fh_thread(fh);
-}
-
-static int solo_enc_get_fmt_cap(struct file *file, void *priv,
-				struct v4l2_format *f)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct v4l2_pix_format *pix = &f->fmt.pix;
-
-	pix->width = solo_enc->width;
-	pix->height = solo_enc->height;
-	pix->pixelformat = fh->fmt;
-	pix->field = solo_enc->interlaced ? V4L2_FIELD_INTERLACED :
-		     V4L2_FIELD_NONE;
-	pix->sizeimage = FRAME_BUF_SIZE;
-	pix->colorspace = V4L2_COLORSPACE_SMPTE170M;
-
-	return 0;
-}
-
-static int solo_enc_reqbufs(struct file *file, void *priv,
-			    struct v4l2_requestbuffers *req)
-{
-	struct solo_enc_fh *fh = priv;
-
-	return videobuf_reqbufs(&fh->vidq, req);
-}
-
-static int solo_enc_querybuf(struct file *file, void *priv,
-			     struct v4l2_buffer *buf)
-{
-	struct solo_enc_fh *fh = priv;
-
-	return videobuf_querybuf(&fh->vidq, buf);
-}
-
-static int solo_enc_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
-{
-	struct solo_enc_fh *fh = priv;
-
-	return videobuf_qbuf(&fh->vidq, buf);
-}
-
-static int solo_enc_dqbuf(struct file *file, void *priv,
-			  struct v4l2_buffer *buf)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	int ret;
-
-	/* Make sure the encoder is on */
-	if (!fh->enc_on) {
-		spin_lock(&solo_enc->lock);
-		ret = solo_enc_on(fh);
-		spin_unlock(&solo_enc->lock);
-		if (ret)
-			return ret;
-
-		ret = solo_start_fh_thread(fh);
-		if (ret)
-			return ret;
-	}
-
-	ret = videobuf_dqbuf(&fh->vidq, buf, file->f_flags & O_NONBLOCK);
-	if (ret)
-		return ret;
-
-	/* Signal motion detection */
-	if (solo_is_motion_on(solo_enc)) {
-		buf->flags |= V4L2_BUF_FLAG_MOTION_ON;
-		if (solo_enc->motion_detected) {
-			buf->flags |= V4L2_BUF_FLAG_MOTION_DETECTED;
-			solo_reg_write(solo_enc->solo_dev, SOLO_VI_MOT_CLEAR,
-				       1 << solo_enc->ch);
-			solo_enc->motion_detected = 0;
-		}
-	}
-
-	/* Check for key frame on mpeg data */
-	if (fh->fmt == V4L2_PIX_FMT_MPEG) {
-		struct videobuf_dmabuf *vbuf =
-				videobuf_to_dma(fh->vidq.bufs[buf->index]);
-
-		if (vbuf) {
-			u8 *p = sg_virt(vbuf->sglist);
-			if (p[3] == 0x00)
-				buf->flags |= V4L2_BUF_FLAG_KEYFRAME;
-			else
-				buf->flags |= V4L2_BUF_FLAG_PFRAME;
-		}
-	}
-
-	return 0;
-}
-
-static int solo_enc_streamon(struct file *file, void *priv,
-			     enum v4l2_buf_type i)
-{
-	struct solo_enc_fh *fh = priv;
-
-	if (i != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	return videobuf_streamon(&fh->vidq);
-}
-
-static int solo_enc_streamoff(struct file *file, void *priv,
-			      enum v4l2_buf_type i)
-{
-	struct solo_enc_fh *fh = priv;
-
-	if (i != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	return videobuf_streamoff(&fh->vidq);
-}
-
-static int solo_enc_s_std(struct file *file, void *priv, v4l2_std_id *i)
-{
-	return 0;
-}
-
-static int solo_enum_framesizes(struct file *file, void *priv,
-				struct v4l2_frmsizeenum *fsize)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_dev *solo_dev = fh->enc->solo_dev;
-
-	if (fsize->pixel_format != V4L2_PIX_FMT_MPEG)
-		return -EINVAL;
-
-	switch (fsize->index) {
-	case 0:
-		fsize->discrete.width = solo_dev->video_hsize >> 1;
-		fsize->discrete.height = solo_dev->video_vsize;
-		break;
-	case 1:
-		fsize->discrete.width = solo_dev->video_hsize;
-		fsize->discrete.height = solo_dev->video_vsize << 1;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
-
-	return 0;
-}
-
-static int solo_enum_frameintervals(struct file *file, void *priv,
-				    struct v4l2_frmivalenum *fintv)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_dev *solo_dev = fh->enc->solo_dev;
-
-	if (fintv->pixel_format != V4L2_PIX_FMT_MPEG || fintv->index)
-		return -EINVAL;
-
-	fintv->type = V4L2_FRMIVAL_TYPE_STEPWISE;
-
-	fintv->stepwise.min.numerator = solo_dev->fps;
-	fintv->stepwise.min.denominator = 1;
-
-	fintv->stepwise.max.numerator = solo_dev->fps;
-	fintv->stepwise.max.denominator = 15;
-
-	fintv->stepwise.step.numerator = 1;
-	fintv->stepwise.step.denominator = 1;
-
-	return 0;
-}
-
-static int solo_g_parm(struct file *file, void *priv,
-		       struct v4l2_streamparm *sp)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-	struct v4l2_captureparm *cp = &sp->parm.capture;
-
-	cp->capability = V4L2_CAP_TIMEPERFRAME;
-	cp->timeperframe.numerator = solo_enc->interval;
-	cp->timeperframe.denominator = solo_dev->fps;
-	cp->capturemode = 0;
-	/* XXX: Shouldn't we be able to get/set this from videobuf? */
-	cp->readbuffers = 2;
-
-	return 0;
-}
-
-static int solo_s_parm(struct file *file, void *priv,
-		       struct v4l2_streamparm *sp)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-	struct v4l2_captureparm *cp = &sp->parm.capture;
-
-	spin_lock(&solo_enc->lock);
-
-	if (atomic_read(&solo_enc->readers) > 0) {
-		spin_unlock(&solo_enc->lock);
-		return -EBUSY;
-	}
-
-	if ((cp->timeperframe.numerator == 0) ||
-	    (cp->timeperframe.denominator == 0)) {
-		/* reset framerate */
-		cp->timeperframe.numerator = 1;
-		cp->timeperframe.denominator = solo_dev->fps;
-	}
-
-	if (cp->timeperframe.denominator != solo_dev->fps)
-		cp->timeperframe.denominator = solo_dev->fps;
-
-	if (cp->timeperframe.numerator > 15)
-		cp->timeperframe.numerator = 15;
-
-	solo_enc->interval = cp->timeperframe.numerator;
-
-	cp->capability = V4L2_CAP_TIMEPERFRAME;
-
-	solo_enc->gop = max(solo_dev->fps / solo_enc->interval, 1);
-	solo_update_mode(solo_enc);
-
-	spin_unlock(&solo_enc->lock);
-
-	return 0;
-}
-
-static int solo_queryctrl(struct file *file, void *priv,
-			  struct v4l2_queryctrl *qc)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-
-	qc->id = v4l2_ctrl_next(solo_ctrl_classes, qc->id);
-	if (!qc->id)
-		return -EINVAL;
-
-	switch (qc->id) {
-	case V4L2_CID_BRIGHTNESS:
-	case V4L2_CID_CONTRAST:
-	case V4L2_CID_SATURATION:
-	case V4L2_CID_HUE:
-		return v4l2_ctrl_query_fill(qc, 0x00, 0xff, 1, 0x80);
-	case V4L2_CID_SHARPNESS:
-		return v4l2_ctrl_query_fill(qc, 0x00, 0x0f, 1, 0x00);
-	case V4L2_CID_MPEG_VIDEO_ENCODING:
-		return v4l2_ctrl_query_fill(
-			qc, V4L2_MPEG_VIDEO_ENCODING_MPEG_1,
-			V4L2_MPEG_VIDEO_ENCODING_MPEG_4_AVC, 1,
-			V4L2_MPEG_VIDEO_ENCODING_MPEG_4_AVC);
-	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
-		return v4l2_ctrl_query_fill(qc, 1, 255, 1, solo_dev->fps);
-#ifdef PRIVATE_CIDS
-	case V4L2_CID_MOTION_THRESHOLD:
-		qc->flags |= V4L2_CTRL_FLAG_SLIDER;
-		qc->type = V4L2_CTRL_TYPE_INTEGER;
-		qc->minimum = 0;
-		qc->maximum = 0xffff;
-		qc->step = 1;
-		qc->default_value = SOLO_DEF_MOT_THRESH;
-		strlcpy(qc->name, "Motion Detection Threshold",
-			sizeof(qc->name));
-		return 0;
-	case V4L2_CID_MOTION_ENABLE:
-		qc->type = V4L2_CTRL_TYPE_BOOLEAN;
-		qc->minimum = 0;
-		qc->maximum = qc->step = 1;
-		qc->default_value = 0;
-		strlcpy(qc->name, "Motion Detection Enable", sizeof(qc->name));
-		return 0;
-#else
-	case V4L2_CID_MOTION_THRESHOLD:
-		return v4l2_ctrl_query_fill(qc, 0, 0xffff, 1,
-					    SOLO_DEF_MOT_THRESH);
-	case V4L2_CID_MOTION_ENABLE:
-		return v4l2_ctrl_query_fill(qc, 0, 1, 1, 0);
-#endif
-	case V4L2_CID_RDS_TX_RADIO_TEXT:
-		qc->type = V4L2_CTRL_TYPE_STRING;
-		qc->minimum = 0;
-		qc->maximum = OSD_TEXT_MAX;
-		qc->step = 1;
-		qc->default_value = 0;
-		strlcpy(qc->name, "OSD Text", sizeof(qc->name));
-		return 0;
-	}
-
-	return -EINVAL;
-}
-
-static int solo_querymenu(struct file *file, void *priv,
-			  struct v4l2_querymenu *qmenu)
-{
-	struct v4l2_queryctrl qctrl;
-	int err;
-
-	qctrl.id = qmenu->id;
-	err = solo_queryctrl(file, priv, &qctrl);
-	if (err)
-		return err;
-
-	return v4l2_ctrl_query_menu(qmenu, &qctrl, NULL);
-}
-
-static int solo_g_ctrl(struct file *file, void *priv,
-		       struct v4l2_control *ctrl)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-	case V4L2_CID_CONTRAST:
-	case V4L2_CID_SATURATION:
-	case V4L2_CID_HUE:
-	case V4L2_CID_SHARPNESS:
-		return tw28_get_ctrl_val(solo_dev, ctrl->id, solo_enc->ch,
-					 &ctrl->value);
-	case V4L2_CID_MPEG_VIDEO_ENCODING:
-		ctrl->value = V4L2_MPEG_VIDEO_ENCODING_MPEG_4_AVC;
-		break;
-	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
-		ctrl->value = solo_enc->gop;
-		break;
-	case V4L2_CID_MOTION_THRESHOLD:
-		ctrl->value = solo_enc->motion_thresh;
-		break;
-	case V4L2_CID_MOTION_ENABLE:
-		ctrl->value = solo_is_motion_on(solo_enc);
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static int solo_s_ctrl(struct file *file, void *priv,
-		       struct v4l2_control *ctrl)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-	case V4L2_CID_CONTRAST:
-	case V4L2_CID_SATURATION:
-	case V4L2_CID_HUE:
-	case V4L2_CID_SHARPNESS:
-		return tw28_set_ctrl_val(solo_dev, ctrl->id, solo_enc->ch,
-					 ctrl->value);
-	case V4L2_CID_MPEG_VIDEO_ENCODING:
-		if (ctrl->value != V4L2_MPEG_VIDEO_ENCODING_MPEG_4_AVC)
-			return -ERANGE;
-		break;
-	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
-		if (ctrl->value < 1 || ctrl->value > 255)
-			return -ERANGE;
-		solo_enc->gop = ctrl->value;
-		solo_reg_write(solo_dev, SOLO_VE_CH_GOP(solo_enc->ch),
-			       solo_enc->gop);
-		solo_reg_write(solo_dev, SOLO_VE_CH_GOP_E(solo_enc->ch),
-			       solo_enc->gop);
-		break;
-	case V4L2_CID_MOTION_THRESHOLD:
-		/* TODO accept value on lower 16-bits and use high
-		 * 16-bits to assign the value to a specific block */
-		if (ctrl->value < 0 || ctrl->value > 0xffff)
-			return -ERANGE;
-		solo_enc->motion_thresh = ctrl->value;
-		solo_set_motion_threshold(solo_dev, solo_enc->ch, ctrl->value);
-		break;
-	case V4L2_CID_MOTION_ENABLE:
-		solo_motion_toggle(solo_enc, ctrl->value);
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static int solo_s_ext_ctrls(struct file *file, void *priv,
-			    struct v4l2_ext_controls *ctrls)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	int i;
-
-	for (i = 0; i < ctrls->count; i++) {
-		struct v4l2_ext_control *ctrl = (ctrls->controls + i);
-		int err;
-
-		switch (ctrl->id) {
-		case V4L2_CID_RDS_TX_RADIO_TEXT:
-			if (ctrl->size - 1 > OSD_TEXT_MAX)
-				err = -ERANGE;
-			else {
-				err = copy_from_user(solo_enc->osd_text,
-						     ctrl->string,
-						     OSD_TEXT_MAX);
-				solo_enc->osd_text[OSD_TEXT_MAX] = '\0';
-				if (!err)
-					err = solo_osd_print(solo_enc);
-				else
-					err = -EFAULT;
-			}
-			break;
-		default:
-			err = -EINVAL;
-		}
-
-		if (err < 0) {
-			ctrls->error_idx = i;
-			return err;
-		}
-	}
-
-	return 0;
-}
-
-static int solo_g_ext_ctrls(struct file *file, void *priv,
-			    struct v4l2_ext_controls *ctrls)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	int i;
-
-	for (i = 0; i < ctrls->count; i++) {
-		struct v4l2_ext_control *ctrl = (ctrls->controls + i);
-		int err;
-
-		switch (ctrl->id) {
-		case V4L2_CID_RDS_TX_RADIO_TEXT:
-			if (ctrl->size < OSD_TEXT_MAX) {
-				ctrl->size = OSD_TEXT_MAX;
-				err = -ENOSPC;
-			} else {
-				err = copy_to_user(ctrl->string,
-						   solo_enc->osd_text,
-						   OSD_TEXT_MAX);
-				if (err)
-					err = -EFAULT;
-			}
-			break;
-		default:
-			err = -EINVAL;
-		}
-
-		if (err < 0) {
-			ctrls->error_idx = i;
-			return err;
-		}
-	}
-
-	return 0;
-}
-
-static const struct v4l2_file_operations solo_enc_fops = {
-	.owner			= THIS_MODULE,
-	.open			= solo_enc_open,
-	.release		= solo_enc_release,
-	.read			= solo_enc_read,
-	.poll			= solo_enc_poll,
-	.mmap			= solo_enc_mmap,
-	.ioctl			= video_ioctl2,
-};
-
-static const struct v4l2_ioctl_ops solo_enc_ioctl_ops = {
-	.vidioc_querycap		= solo_enc_querycap,
-	.vidioc_s_std			= solo_enc_s_std,
-	/* Input callbacks */
-	.vidioc_enum_input		= solo_enc_enum_input,
-	.vidioc_s_input			= solo_enc_set_input,
-	.vidioc_g_input			= solo_enc_get_input,
-	/* Video capture format callbacks */
-	.vidioc_enum_fmt_vid_cap	= solo_enc_enum_fmt_cap,
-	.vidioc_try_fmt_vid_cap		= solo_enc_try_fmt_cap,
-	.vidioc_s_fmt_vid_cap		= solo_enc_set_fmt_cap,
-	.vidioc_g_fmt_vid_cap		= solo_enc_get_fmt_cap,
-	/* Streaming I/O */
-	.vidioc_reqbufs			= solo_enc_reqbufs,
-	.vidioc_querybuf		= solo_enc_querybuf,
-	.vidioc_qbuf			= solo_enc_qbuf,
-	.vidioc_dqbuf			= solo_enc_dqbuf,
-	.vidioc_streamon		= solo_enc_streamon,
-	.vidioc_streamoff		= solo_enc_streamoff,
-	/* Frame size and interval */
-	.vidioc_enum_framesizes		= solo_enum_framesizes,
-	.vidioc_enum_frameintervals	= solo_enum_frameintervals,
-	/* Video capture parameters */
-	.vidioc_s_parm			= solo_s_parm,
-	.vidioc_g_parm			= solo_g_parm,
-	/* Controls */
-	.vidioc_queryctrl		= solo_queryctrl,
-	.vidioc_querymenu		= solo_querymenu,
-	.vidioc_g_ctrl			= solo_g_ctrl,
-	.vidioc_s_ctrl			= solo_s_ctrl,
-	.vidioc_g_ext_ctrls		= solo_g_ext_ctrls,
-	.vidioc_s_ext_ctrls		= solo_s_ext_ctrls,
-};
-
-static struct video_device solo_enc_template = {
-	.name			= SOLO6X10_NAME,
-	.fops			= &solo_enc_fops,
-	.ioctl_ops		= &solo_enc_ioctl_ops,
-	.minor			= -1,
-	.release		= video_device_release,
-
-	.tvnorms		= V4L2_STD_NTSC_M | V4L2_STD_PAL_B,
-	.current_norm		= V4L2_STD_NTSC_M,
-};
-
-static struct solo_enc_dev *solo_enc_alloc(struct solo_dev *solo_dev, u8 ch)
-{
-	struct solo_enc_dev *solo_enc;
-	int ret;
-
-	solo_enc = kzalloc(sizeof(*solo_enc), GFP_KERNEL);
-	if (!solo_enc)
-		return ERR_PTR(-ENOMEM);
-
-	solo_enc->vfd = video_device_alloc();
-	if (!solo_enc->vfd) {
-		kfree(solo_enc);
-		return ERR_PTR(-ENOMEM);
-	}
-
-	solo_enc->solo_dev = solo_dev;
-	solo_enc->ch = ch;
-
-	*solo_enc->vfd = solo_enc_template;
-	solo_enc->vfd->parent = &solo_dev->pdev->dev;
-	ret = video_register_device(solo_enc->vfd, VFL_TYPE_GRABBER,
-				    video_nr);
-	if (ret < 0) {
-		video_device_release(solo_enc->vfd);
-		kfree(solo_enc);
-		return ERR_PTR(ret);
-	}
-
-	video_set_drvdata(solo_enc->vfd, solo_enc);
-
-	snprintf(solo_enc->vfd->name, sizeof(solo_enc->vfd->name),
-		 "%s-enc (%i/%i)", SOLO6X10_NAME, solo_dev->vfd->num,
-		 solo_enc->vfd->num);
-
-	if (video_nr != -1)
-		video_nr++;
-
-	spin_lock_init(&solo_enc->lock);
-	init_waitqueue_head(&solo_enc->thread_wait);
-	atomic_set(&solo_enc->readers, 0);
-
-	solo_enc->qp = SOLO_DEFAULT_QP;
-	solo_enc->gop = solo_dev->fps;
-	solo_enc->interval = 1;
-	solo_enc->mode = SOLO_ENC_MODE_CIF;
-	solo_enc->motion_thresh = SOLO_DEF_MOT_THRESH;
-
-	spin_lock(&solo_enc->lock);
-	solo_update_mode(solo_enc);
-	spin_unlock(&solo_enc->lock);
-
-	return solo_enc;
-}
-
-static void solo_enc_free(struct solo_enc_dev *solo_enc)
-{
-	if (solo_enc == NULL)
-		return;
-
-	video_unregister_device(solo_enc->vfd);
-	kfree(solo_enc);
-}
-
-int solo_enc_v4l2_init(struct solo_dev *solo_dev)
-{
-	int i;
-
-	for (i = 0; i < solo_dev->nr_chans; i++) {
-		solo_dev->v4l2_enc[i] = solo_enc_alloc(solo_dev, i);
-		if (IS_ERR(solo_dev->v4l2_enc[i]))
-			break;
-	}
-
-	if (i != solo_dev->nr_chans) {
-		int ret = PTR_ERR(solo_dev->v4l2_enc[i]);
-		while (i--)
-			solo_enc_free(solo_dev->v4l2_enc[i]);
-		return ret;
-	}
-
-	/* D1@MAX-FPS * 4 */
-	solo_dev->enc_bw_remain = solo_dev->fps * 4 * 4;
-
-	dev_info(&solo_dev->pdev->dev, "Encoders as /dev/video%d-%d\n",
-		 solo_dev->v4l2_enc[0]->vfd->num,
-		 solo_dev->v4l2_enc[solo_dev->nr_chans - 1]->vfd->num);
-
-	return 0;
-}
-
-void solo_enc_v4l2_exit(struct solo_dev *solo_dev)
-{
-	int i;
-
-	solo_irq_off(solo_dev, SOLO_IRQ_MOTION);
-
-	for (i = 0; i < solo_dev->nr_chans; i++)
-		solo_enc_free(solo_dev->v4l2_enc[i]);
-}
diff --git a/drivers/staging/media/solo6x10/v4l2.c b/drivers/staging/media/solo6x10/v4l2.c
deleted file mode 100644
index ca774cc5753..00000000000
--- a/drivers/staging/media/solo6x10/v4l2.c
+++ /dev/null
@@ -1,961 +0,0 @@
-/*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.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., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- */
-
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/kthread.h>
-#include <linux/freezer.h>
-#include <media/v4l2-ioctl.h>
-#include <media/v4l2-common.h>
-#include <media/videobuf-dma-sg.h>
-#include "solo6x10.h"
-#include "tw28.h"
-
-#define SOLO_HW_BPL		2048
-#define SOLO_DISP_PIX_FIELD	V4L2_FIELD_INTERLACED
-
-/* Image size is two fields, SOLO_HW_BPL is one horizontal line */
-#define solo_vlines(__solo)	(__solo->video_vsize * 2)
-#define solo_image_size(__solo) (solo_bytesperline(__solo) * \
-				 solo_vlines(__solo))
-#define solo_bytesperline(__solo) (__solo->video_hsize * 2)
-
-#define MIN_VID_BUFFERS		4
-
-/* Simple file handle */
-struct solo_filehandle {
-	struct solo_dev		*solo_dev;
-	struct videobuf_queue	vidq;
-	struct task_struct      *kthread;
-	spinlock_t		slock;
-	int			old_write;
-	struct list_head	vidq_active;
-	struct p2m_desc		desc[SOLO_NR_P2M_DESC];
-	int			desc_idx;
-};
-
-unsigned video_nr = -1;
-module_param(video_nr, uint, 0644);
-MODULE_PARM_DESC(video_nr, "videoX start number, -1 is autodetect (default)");
-
-static void erase_on(struct solo_dev *solo_dev)
-{
-	solo_reg_write(solo_dev, SOLO_VO_DISP_ERASE, SOLO_VO_DISP_ERASE_ON);
-	solo_dev->erasing = 1;
-	solo_dev->frame_blank = 0;
-}
-
-static int erase_off(struct solo_dev *solo_dev)
-{
-	if (!solo_dev->erasing)
-		return 0;
-
-	/* First time around, assert erase off */
-	if (!solo_dev->frame_blank)
-		solo_reg_write(solo_dev, SOLO_VO_DISP_ERASE, 0);
-	/* Keep the erasing flag on for 8 frames minimum */
-	if (solo_dev->frame_blank++ >= 8)
-		solo_dev->erasing = 0;
-
-	return 1;
-}
-
-void solo_video_in_isr(struct solo_dev *solo_dev)
-{
-	solo_reg_write(solo_dev, SOLO_IRQ_STAT, SOLO_IRQ_VIDEO_IN);
-	wake_up_interruptible(&solo_dev->disp_thread_wait);
-}
-
-static void solo_win_setup(struct solo_dev *solo_dev, u8 ch,
-			   int sx, int sy, int ex, int ey, int scale)
-{
-	if (ch >= solo_dev->nr_chans)
-		return;
-
-	/* Here, we just keep window/channel the same */
-	solo_reg_write(solo_dev, SOLO_VI_WIN_CTRL0(ch),
-		       SOLO_VI_WIN_CHANNEL(ch) |
-		       SOLO_VI_WIN_SX(sx) |
-		       SOLO_VI_WIN_EX(ex) |
-		       SOLO_VI_WIN_SCALE(scale));
-
-	solo_reg_write(solo_dev, SOLO_VI_WIN_CTRL1(ch),
-		       SOLO_VI_WIN_SY(sy) |
-		       SOLO_VI_WIN_EY(ey));
-}
-
-static int solo_v4l2_ch_ext_4up(struct solo_dev *solo_dev, u8 idx, int on)
-{
-	u8 ch = idx * 4;
-
-	if (ch >= solo_dev->nr_chans)
-		return -EINVAL;
-
-	if (!on) {
-		u8 i;
-		for (i = ch; i < ch + 4; i++)
-			solo_win_setup(solo_dev, i, solo_dev->video_hsize,
-				       solo_vlines(solo_dev),
-				       solo_dev->video_hsize,
-				       solo_vlines(solo_dev), 0);
-		return 0;
-	}
-
-	/* Row 1 */
-	solo_win_setup(solo_dev, ch, 0, 0, solo_dev->video_hsize / 2,
-		       solo_vlines(solo_dev) / 2, 3);
-	solo_win_setup(solo_dev, ch + 1, solo_dev->video_hsize / 2, 0,
-		       solo_dev->video_hsize, solo_vlines(solo_dev) / 2, 3);
-	/* Row 2 */
-	solo_win_setup(solo_dev, ch + 2, 0, solo_vlines(solo_dev) / 2,
-		       solo_dev->video_hsize / 2, solo_vlines(solo_dev), 3);
-	solo_win_setup(solo_dev, ch + 3, solo_dev->video_hsize / 2,
-		       solo_vlines(solo_dev) / 2, solo_dev->video_hsize,
-		       solo_vlines(solo_dev), 3);
-
-	return 0;
-}
-
-static int solo_v4l2_ch_ext_16up(struct solo_dev *solo_dev, int on)
-{
-	int sy, ysize, hsize, i;
-
-	if (!on) {
-		for (i = 0; i < 16; i++)
-			solo_win_setup(solo_dev, i, solo_dev->video_hsize,
-				       solo_vlines(solo_dev),
-				       solo_dev->video_hsize,
-				       solo_vlines(solo_dev), 0);
-		return 0;
-	}
-
-	ysize = solo_vlines(solo_dev) / 4;
-	hsize = solo_dev->video_hsize / 4;
-
-	for (sy = 0, i = 0; i < 4; i++, sy += ysize) {
-		solo_win_setup(solo_dev, i * 4, 0, sy, hsize,
-			       sy + ysize, 5);
-		solo_win_setup(solo_dev, (i * 4) + 1, hsize, sy,
-			       hsize * 2, sy + ysize, 5);
-		solo_win_setup(solo_dev, (i * 4) + 2, hsize * 2, sy,
-			       hsize * 3, sy + ysize, 5);
-		solo_win_setup(solo_dev, (i * 4) + 3, hsize * 3, sy,
-			       solo_dev->video_hsize, sy + ysize, 5);
-	}
-
-	return 0;
-}
-
-static int solo_v4l2_ch(struct solo_dev *solo_dev, u8 ch, int on)
-{
-	u8 ext_ch;
-
-	if (ch < solo_dev->nr_chans) {
-		solo_win_setup(solo_dev, ch, on ? 0 : solo_dev->video_hsize,
-			       on ? 0 : solo_vlines(solo_dev),
-			       solo_dev->video_hsize, solo_vlines(solo_dev),
-			       on ? 1 : 0);
-		return 0;
-	}
-
-	if (ch >= solo_dev->nr_chans + solo_dev->nr_ext)
-		return -EINVAL;
-
-	ext_ch = ch - solo_dev->nr_chans;
-
-	/* 4up's first */
-	if (ext_ch < 4)
-		return solo_v4l2_ch_ext_4up(solo_dev, ext_ch, on);
-
-	/* Remaining case is 16up for 16-port */
-	return solo_v4l2_ch_ext_16up(solo_dev, on);
-}
-
-static int solo_v4l2_set_ch(struct solo_dev *solo_dev, u8 ch)
-{
-	if (ch >= solo_dev->nr_chans + solo_dev->nr_ext)
-		return -EINVAL;
-
-	erase_on(solo_dev);
-
-	solo_v4l2_ch(solo_dev, solo_dev->cur_disp_ch, 0);
-	solo_v4l2_ch(solo_dev, ch, 1);
-
-	solo_dev->cur_disp_ch = ch;
-
-	return 0;
-}
-
-static void disp_reset_desc(struct solo_filehandle *fh)
-{
-	/* We use desc mode, which ignores desc 0 */
-	memset(fh->desc, 0, sizeof(*fh->desc));
-	fh->desc_idx = 1;
-}
-
-static int disp_flush_descs(struct solo_filehandle *fh)
-{
-	int ret;
-
-	if (!fh->desc_idx)
-		return 0;
-
-	ret = solo_p2m_dma_desc(fh->solo_dev, SOLO_P2M_DMA_ID_DISP,
-				fh->desc, fh->desc_idx);
-	disp_reset_desc(fh);
-
-	return ret;
-}
-
-static int disp_push_desc(struct solo_filehandle *fh, dma_addr_t dma_addr,
-		      u32 ext_addr, int size, int repeat, int ext_size)
-{
-	if (fh->desc_idx >= SOLO_NR_P2M_DESC) {
-		int ret = disp_flush_descs(fh);
-		if (ret)
-			return ret;
-	}
-
-	solo_p2m_push_desc(&fh->desc[fh->desc_idx], 0, dma_addr, ext_addr,
-			   size, repeat, ext_size);
-	fh->desc_idx++;
-
-	return 0;
-}
-
-static void solo_fillbuf(struct solo_filehandle *fh,
-			 struct videobuf_buffer *vb)
-{
-	struct solo_dev *solo_dev = fh->solo_dev;
-	struct videobuf_dmabuf *vbuf;
-	unsigned int fdma_addr;
-	int error = 1;
-	int i;
-	struct scatterlist *sg;
-	dma_addr_t sg_dma;
-	int sg_size_left;
-
-	vbuf = videobuf_to_dma(vb);
-	if (!vbuf)
-		goto finish_buf;
-
-	if (erase_off(solo_dev)) {
-		int i;
-
-		/* Just blit to the entire sg list, ignoring size */
-		for_each_sg(vbuf->sglist, sg, vbuf->sglen, i) {
-			void *p = sg_virt(sg);
-			size_t len = sg_dma_len(sg);
-
-			for (i = 0; i < len; i += 2) {
-				((u8 *)p)[i] = 0x80;
-				((u8 *)p)[i + 1] = 0x00;
-			}
-		}
-
-		error = 0;
-		goto finish_buf;
-	}
-
-	disp_reset_desc(fh);
-	sg = vbuf->sglist;
-	sg_dma = sg_dma_address(sg);
-	sg_size_left = sg_dma_len(sg);
-
-	fdma_addr = SOLO_DISP_EXT_ADDR + (fh->old_write *
-			(SOLO_HW_BPL * solo_vlines(solo_dev)));
-
-	for (i = 0; i < solo_vlines(solo_dev); i++) {
-		int line_len = solo_bytesperline(solo_dev);
-		int lines;
-
-		if (!sg_size_left) {
-			sg = sg_next(sg);
-			if (sg == NULL)
-				goto finish_buf;
-			sg_dma = sg_dma_address(sg);
-			sg_size_left = sg_dma_len(sg);
-		}
-
-		/* No room for an entire line, so chunk it up */
-		if (sg_size_left < line_len) {
-			int this_addr = fdma_addr;
-
-			while (line_len > 0) {
-				int this_write;
-
-				if (!sg_size_left) {
-					sg = sg_next(sg);
-					if (sg == NULL)
-						goto finish_buf;
-					sg_dma = sg_dma_address(sg);
-					sg_size_left = sg_dma_len(sg);
-				}
-
-				this_write = min(sg_size_left, line_len);
-
-				if (disp_push_desc(fh, sg_dma, this_addr,
-						   this_write, 0, 0))
-					goto finish_buf;
-
-				line_len -= this_write;
-				sg_size_left -= this_write;
-				sg_dma += this_write;
-				this_addr += this_write;
-			}
-
-			fdma_addr += SOLO_HW_BPL;
-			continue;
-		}
-
-		/* Shove as many lines into a repeating descriptor as possible */
-		lines = min(sg_size_left / line_len,
-			    solo_vlines(solo_dev) - i);
-
-		if (disp_push_desc(fh, sg_dma, fdma_addr, line_len,
-				   lines - 1, SOLO_HW_BPL))
-			goto finish_buf;
-
-		i += lines - 1;
-		fdma_addr += SOLO_HW_BPL * lines;
-		sg_dma += lines * line_len;
-		sg_size_left -= lines * line_len;
-	}
-
-	error = disp_flush_descs(fh);
-
-finish_buf:
-	if (error) {
-		vb->state = VIDEOBUF_ERROR;
-	} else {
-		vb->size = solo_vlines(solo_dev) * solo_bytesperline(solo_dev);
-		vb->state = VIDEOBUF_DONE;
-		vb->field_count++;
-		do_gettimeofday(&vb->ts);
-	}
-
-	wake_up(&vb->done);
-
-	return;
-}
-
-static void solo_thread_try(struct solo_filehandle *fh)
-{
-	struct videobuf_buffer *vb;
-	unsigned int cur_write;
-
-	for (;;) {
-		spin_lock(&fh->slock);
-
-		if (list_empty(&fh->vidq_active))
-			break;
-
-		vb = list_first_entry(&fh->vidq_active, struct videobuf_buffer,
-				      queue);
-
-		if (!waitqueue_active(&vb->done))
-			break;
-
-		cur_write = SOLO_VI_STATUS0_PAGE(solo_reg_read(fh->solo_dev,
-						 SOLO_VI_STATUS0));
-		if (cur_write == fh->old_write)
-			break;
-
-		fh->old_write = cur_write;
-		list_del(&vb->queue);
-
-		spin_unlock(&fh->slock);
-
-		solo_fillbuf(fh, vb);
-	}
-
-	assert_spin_locked(&fh->slock);
-	spin_unlock(&fh->slock);
-}
-
-static int solo_thread(void *data)
-{
-	struct solo_filehandle *fh = data;
-	struct solo_dev *solo_dev = fh->solo_dev;
-	DECLARE_WAITQUEUE(wait, current);
-
-	set_freezable();
-	add_wait_queue(&solo_dev->disp_thread_wait, &wait);
-
-	for (;;) {
-		long timeout = schedule_timeout_interruptible(HZ);
-		if (timeout == -ERESTARTSYS || kthread_should_stop())
-			break;
-		solo_thread_try(fh);
-		try_to_freeze();
-	}
-
-	remove_wait_queue(&solo_dev->disp_thread_wait, &wait);
-
-	return 0;
-}
-
-static int solo_start_thread(struct solo_filehandle *fh)
-{
-	fh->kthread = kthread_run(solo_thread, fh, SOLO6X10_NAME "_disp");
-
-	return PTR_RET(fh->kthread);
-}
-
-static void solo_stop_thread(struct solo_filehandle *fh)
-{
-	if (fh->kthread) {
-		kthread_stop(fh->kthread);
-		fh->kthread = NULL;
-	}
-}
-
-static int solo_buf_setup(struct videobuf_queue *vq, unsigned int *count,
-			  unsigned int *size)
-{
-	struct solo_filehandle *fh = vq->priv_data;
-	struct solo_dev *solo_dev  = fh->solo_dev;
-
-	*size = solo_image_size(solo_dev);
-
-	if (*count < MIN_VID_BUFFERS)
-		*count = MIN_VID_BUFFERS;
-
-	return 0;
-}
-
-static int solo_buf_prepare(struct videobuf_queue *vq,
-			    struct videobuf_buffer *vb, enum v4l2_field field)
-{
-	struct solo_filehandle *fh  = vq->priv_data;
-	struct solo_dev *solo_dev = fh->solo_dev;
-
-	vb->size = solo_image_size(solo_dev);
-	if (vb->baddr != 0 && vb->bsize < vb->size)
-		return -EINVAL;
-
-	/* XXX: These properties only change when queue is idle */
-	vb->width  = solo_dev->video_hsize;
-	vb->height = solo_vlines(solo_dev);
-	vb->bytesperline = solo_bytesperline(solo_dev);
-	vb->field  = field;
-
-	if (vb->state == VIDEOBUF_NEEDS_INIT) {
-		int rc = videobuf_iolock(vq, vb, NULL);
-		if (rc < 0) {
-			struct videobuf_dmabuf *dma = videobuf_to_dma(vb);
-			videobuf_dma_unmap(vq->dev, dma);
-			videobuf_dma_free(dma);
-			vb->state = VIDEOBUF_NEEDS_INIT;
-			return rc;
-		}
-	}
-	vb->state = VIDEOBUF_PREPARED;
-
-	return 0;
-}
-
-static void solo_buf_queue(struct videobuf_queue *vq,
-			   struct videobuf_buffer *vb)
-{
-	struct solo_filehandle *fh = vq->priv_data;
-	struct solo_dev *solo_dev = fh->solo_dev;
-
-	vb->state = VIDEOBUF_QUEUED;
-	list_add_tail(&vb->queue, &fh->vidq_active);
-	wake_up_interruptible(&solo_dev->disp_thread_wait);
-}
-
-static void solo_buf_release(struct videobuf_queue *vq,
-			     struct videobuf_buffer *vb)
-{
-	struct videobuf_dmabuf *dma = videobuf_to_dma(vb);
-
-	videobuf_dma_unmap(vq->dev, dma);
-	videobuf_dma_free(dma);
-	vb->state = VIDEOBUF_NEEDS_INIT;
-}
-
-static struct videobuf_queue_ops solo_video_qops = {
-	.buf_setup	= solo_buf_setup,
-	.buf_prepare	= solo_buf_prepare,
-	.buf_queue	= solo_buf_queue,
-	.buf_release	= solo_buf_release,
-};
-
-static unsigned int solo_v4l2_poll(struct file *file,
-				   struct poll_table_struct *wait)
-{
-	struct solo_filehandle *fh = file->private_data;
-
-	return videobuf_poll_stream(file, &fh->vidq, wait);
-}
-
-static int solo_v4l2_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	struct solo_filehandle *fh = file->private_data;
-
-	return videobuf_mmap_mapper(&fh->vidq, vma);
-}
-
-static int solo_v4l2_open(struct file *file)
-{
-	struct solo_dev *solo_dev = video_drvdata(file);
-	struct solo_filehandle *fh;
-	int ret;
-
-	fh = kzalloc(sizeof(*fh), GFP_KERNEL);
-	if (fh == NULL)
-		return -ENOMEM;
-
-	spin_lock_init(&fh->slock);
-	INIT_LIST_HEAD(&fh->vidq_active);
-	fh->solo_dev = solo_dev;
-	file->private_data = fh;
-
-	ret = solo_start_thread(fh);
-	if (ret) {
-		kfree(fh);
-		return ret;
-	}
-
-	videobuf_queue_sg_init(&fh->vidq, &solo_video_qops,
-			       &solo_dev->pdev->dev, &fh->slock,
-			       V4L2_BUF_TYPE_VIDEO_CAPTURE,
-			       SOLO_DISP_PIX_FIELD,
-			       sizeof(struct videobuf_buffer), fh, NULL);
-
-	return 0;
-}
-
-static ssize_t solo_v4l2_read(struct file *file, char __user *data,
-			      size_t count, loff_t *ppos)
-{
-	struct solo_filehandle *fh = file->private_data;
-
-	return videobuf_read_stream(&fh->vidq, data, count, ppos, 0,
-				    file->f_flags & O_NONBLOCK);
-}
-
-static int solo_v4l2_release(struct file *file)
-{
-	struct solo_filehandle *fh = file->private_data;
-
-	videobuf_stop(&fh->vidq);
-	videobuf_mmap_free(&fh->vidq);
-	solo_stop_thread(fh);
-	kfree(fh);
-
-	return 0;
-}
-
-static int solo_querycap(struct file *file, void  *priv,
-			 struct v4l2_capability *cap)
-{
-	struct solo_filehandle  *fh  = priv;
-	struct solo_dev *solo_dev = fh->solo_dev;
-
-	strcpy(cap->driver, SOLO6X10_NAME);
-	strcpy(cap->card, "Softlogic 6x10");
-	snprintf(cap->bus_info, sizeof(cap->bus_info), "PCI %s",
-		 pci_name(solo_dev->pdev));
-	cap->version = SOLO6X10_VER_NUM;
-	cap->capabilities =     V4L2_CAP_VIDEO_CAPTURE |
-				V4L2_CAP_READWRITE |
-				V4L2_CAP_STREAMING;
-	return 0;
-}
-
-static int solo_enum_ext_input(struct solo_dev *solo_dev,
-			       struct v4l2_input *input)
-{
-	static const char *dispnames_1[] = { "4UP" };
-	static const char *dispnames_2[] = { "4UP-1", "4UP-2" };
-	static const char *dispnames_5[] = {
-		"4UP-1", "4UP-2", "4UP-3", "4UP-4", "16UP"
-	};
-	const char **dispnames;
-
-	if (input->index >= (solo_dev->nr_chans + solo_dev->nr_ext))
-		return -EINVAL;
-
-	if (solo_dev->nr_ext == 5)
-		dispnames = dispnames_5;
-	else if (solo_dev->nr_ext == 2)
-		dispnames = dispnames_2;
-	else
-		dispnames = dispnames_1;
-
-	snprintf(input->name, sizeof(input->name), "Multi %s",
-		 dispnames[input->index - solo_dev->nr_chans]);
-
-	return 0;
-}
-
-static int solo_enum_input(struct file *file, void *priv,
-			   struct v4l2_input *input)
-{
-	struct solo_filehandle *fh  = priv;
-	struct solo_dev *solo_dev = fh->solo_dev;
-
-	if (input->index >= solo_dev->nr_chans) {
-		int ret = solo_enum_ext_input(solo_dev, input);
-		if (ret < 0)
-			return ret;
-	} else {
-		snprintf(input->name, sizeof(input->name), "Camera %d",
-			 input->index + 1);
-
-		/* We can only check this for normal inputs */
-		if (!tw28_get_video_status(solo_dev, input->index))
-			input->status = V4L2_IN_ST_NO_SIGNAL;
-	}
-
-	input->type = V4L2_INPUT_TYPE_CAMERA;
-
-	if (solo_dev->video_type == SOLO_VO_FMT_TYPE_NTSC)
-		input->std = V4L2_STD_NTSC_M;
-	else
-		input->std = V4L2_STD_PAL_B;
-
-	return 0;
-}
-
-static int solo_set_input(struct file *file, void *priv, unsigned int index)
-{
-	struct solo_filehandle *fh = priv;
-
-	return solo_v4l2_set_ch(fh->solo_dev, index);
-}
-
-static int solo_get_input(struct file *file, void *priv, unsigned int *index)
-{
-	struct solo_filehandle *fh = priv;
-
-	*index = fh->solo_dev->cur_disp_ch;
-
-	return 0;
-}
-
-static int solo_enum_fmt_cap(struct file *file, void *priv,
-			     struct v4l2_fmtdesc *f)
-{
-	if (f->index)
-		return -EINVAL;
-
-	f->pixelformat = V4L2_PIX_FMT_UYVY;
-	strlcpy(f->description, "UYUV 4:2:2 Packed", sizeof(f->description));
-
-	return 0;
-}
-
-static int solo_try_fmt_cap(struct file *file, void *priv,
-			    struct v4l2_format *f)
-{
-	struct solo_filehandle *fh = priv;
-	struct solo_dev *solo_dev = fh->solo_dev;
-	struct v4l2_pix_format *pix = &f->fmt.pix;
-	int image_size = solo_image_size(solo_dev);
-
-	/* Check supported sizes */
-	if (pix->width != solo_dev->video_hsize)
-		pix->width = solo_dev->video_hsize;
-	if (pix->height != solo_vlines(solo_dev))
-		pix->height = solo_vlines(solo_dev);
-	if (pix->sizeimage != image_size)
-		pix->sizeimage = image_size;
-
-	/* Check formats */
-	if (pix->field == V4L2_FIELD_ANY)
-		pix->field = SOLO_DISP_PIX_FIELD;
-
-	if (pix->pixelformat != V4L2_PIX_FMT_UYVY ||
-	    pix->field       != SOLO_DISP_PIX_FIELD ||
-	    pix->colorspace  != V4L2_COLORSPACE_SMPTE170M)
-		return -EINVAL;
-
-	return 0;
-}
-
-static int solo_set_fmt_cap(struct file *file, void *priv,
-			    struct v4l2_format *f)
-{
-	struct solo_filehandle *fh = priv;
-
-	if (videobuf_queue_is_busy(&fh->vidq))
-		return -EBUSY;
-
-	/* For right now, if it doesn't match our running config,
-	 * then fail */
-	return solo_try_fmt_cap(file, priv, f);
-}
-
-static int solo_get_fmt_cap(struct file *file, void *priv,
-			    struct v4l2_format *f)
-{
-	struct solo_filehandle *fh = priv;
-	struct solo_dev *solo_dev = fh->solo_dev;
-	struct v4l2_pix_format *pix = &f->fmt.pix;
-
-	pix->width = solo_dev->video_hsize;
-	pix->height = solo_vlines(solo_dev);
-	pix->pixelformat = V4L2_PIX_FMT_UYVY;
-	pix->field = SOLO_DISP_PIX_FIELD;
-	pix->sizeimage = solo_image_size(solo_dev);
-	pix->colorspace = V4L2_COLORSPACE_SMPTE170M;
-	pix->bytesperline = solo_bytesperline(solo_dev);
-
-	return 0;
-}
-
-static int solo_reqbufs(struct file *file, void *priv,
-			struct v4l2_requestbuffers *req)
-{
-	struct solo_filehandle *fh = priv;
-
-	return videobuf_reqbufs(&fh->vidq, req);
-}
-
-static int solo_querybuf(struct file *file, void *priv, struct v4l2_buffer *buf)
-{
-	struct solo_filehandle *fh = priv;
-
-	return videobuf_querybuf(&fh->vidq, buf);
-}
-
-static int solo_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
-{
-	struct solo_filehandle *fh = priv;
-
-	return videobuf_qbuf(&fh->vidq, buf);
-}
-
-static int solo_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
-{
-	struct solo_filehandle *fh = priv;
-
-	return videobuf_dqbuf(&fh->vidq, buf, file->f_flags & O_NONBLOCK);
-}
-
-static int solo_streamon(struct file *file, void *priv, enum v4l2_buf_type i)
-{
-	struct solo_filehandle *fh = priv;
-
-	if (i != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	return videobuf_streamon(&fh->vidq);
-}
-
-static int solo_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
-{
-	struct solo_filehandle *fh = priv;
-
-	if (i != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	return videobuf_streamoff(&fh->vidq);
-}
-
-static int solo_s_std(struct file *file, void *priv, v4l2_std_id *i)
-{
-	return 0;
-}
-
-static const u32 solo_motion_ctrls[] = {
-	V4L2_CID_MOTION_TRACE,
-	0
-};
-
-static const u32 *solo_ctrl_classes[] = {
-	solo_motion_ctrls,
-	NULL
-};
-
-static int solo_disp_queryctrl(struct file *file, void *priv,
-			       struct v4l2_queryctrl *qc)
-{
-	qc->id = v4l2_ctrl_next(solo_ctrl_classes, qc->id);
-	if (!qc->id)
-		return -EINVAL;
-
-	switch (qc->id) {
-#ifdef PRIVATE_CIDS
-	case V4L2_CID_MOTION_TRACE:
-		qc->type = V4L2_CTRL_TYPE_BOOLEAN;
-		qc->minimum = 0;
-		qc->maximum = qc->step = 1;
-		qc->default_value = 0;
-		strlcpy(qc->name, "Motion Detection Trace", sizeof(qc->name));
-		return 0;
-#else
-	case V4L2_CID_MOTION_TRACE:
-		return v4l2_ctrl_query_fill(qc, 0, 1, 1, 0);
-#endif
-	}
-	return -EINVAL;
-}
-
-static int solo_disp_g_ctrl(struct file *file, void *priv,
-			    struct v4l2_control *ctrl)
-{
-	struct solo_filehandle *fh = priv;
-	struct solo_dev *solo_dev = fh->solo_dev;
-
-	switch (ctrl->id) {
-	case V4L2_CID_MOTION_TRACE:
-		ctrl->value = solo_reg_read(solo_dev, SOLO_VI_MOTION_BAR)
-			? 1 : 0;
-		return 0;
-	}
-	return -EINVAL;
-}
-
-static int solo_disp_s_ctrl(struct file *file, void *priv,
-			    struct v4l2_control *ctrl)
-{
-	struct solo_filehandle *fh = priv;
-	struct solo_dev *solo_dev = fh->solo_dev;
-
-	switch (ctrl->id) {
-	case V4L2_CID_MOTION_TRACE:
-		if (ctrl->value) {
-			solo_reg_write(solo_dev, SOLO_VI_MOTION_BORDER,
-					SOLO_VI_MOTION_Y_ADD |
-					SOLO_VI_MOTION_Y_VALUE(0x20) |
-					SOLO_VI_MOTION_CB_VALUE(0x10) |
-					SOLO_VI_MOTION_CR_VALUE(0x10));
-			solo_reg_write(solo_dev, SOLO_VI_MOTION_BAR,
-					SOLO_VI_MOTION_CR_ADD |
-					SOLO_VI_MOTION_Y_VALUE(0x10) |
-					SOLO_VI_MOTION_CB_VALUE(0x80) |
-					SOLO_VI_MOTION_CR_VALUE(0x10));
-		} else {
-			solo_reg_write(solo_dev, SOLO_VI_MOTION_BORDER, 0);
-			solo_reg_write(solo_dev, SOLO_VI_MOTION_BAR, 0);
-		}
-		return 0;
-	}
-	return -EINVAL;
-}
-
-static const struct v4l2_file_operations solo_v4l2_fops = {
-	.owner			= THIS_MODULE,
-	.open			= solo_v4l2_open,
-	.release		= solo_v4l2_release,
-	.read			= solo_v4l2_read,
-	.poll			= solo_v4l2_poll,
-	.mmap			= solo_v4l2_mmap,
-	.ioctl			= video_ioctl2,
-};
-
-static const struct v4l2_ioctl_ops solo_v4l2_ioctl_ops = {
-	.vidioc_querycap		= solo_querycap,
-	.vidioc_s_std			= solo_s_std,
-	/* Input callbacks */
-	.vidioc_enum_input		= solo_enum_input,
-	.vidioc_s_input			= solo_set_input,
-	.vidioc_g_input			= solo_get_input,
-	/* Video capture format callbacks */
-	.vidioc_enum_fmt_vid_cap	= solo_enum_fmt_cap,
-	.vidioc_try_fmt_vid_cap		= solo_try_fmt_cap,
-	.vidioc_s_fmt_vid_cap		= solo_set_fmt_cap,
-	.vidioc_g_fmt_vid_cap		= solo_get_fmt_cap,
-	/* Streaming I/O */
-	.vidioc_reqbufs			= solo_reqbufs,
-	.vidioc_querybuf		= solo_querybuf,
-	.vidioc_qbuf			= solo_qbuf,
-	.vidioc_dqbuf			= solo_dqbuf,
-	.vidioc_streamon		= solo_streamon,
-	.vidioc_streamoff		= solo_streamoff,
-	/* Controls */
-	.vidioc_queryctrl		= solo_disp_queryctrl,
-	.vidioc_g_ctrl			= solo_disp_g_ctrl,
-	.vidioc_s_ctrl			= solo_disp_s_ctrl,
-};
-
-static struct video_device solo_v4l2_template = {
-	.name			= SOLO6X10_NAME,
-	.fops			= &solo_v4l2_fops,
-	.ioctl_ops		= &solo_v4l2_ioctl_ops,
-	.minor			= -1,
-	.release		= video_device_release,
-
-	.tvnorms		= V4L2_STD_NTSC_M | V4L2_STD_PAL_B,
-	.current_norm		= V4L2_STD_NTSC_M,
-};
-
-int solo_v4l2_init(struct solo_dev *solo_dev)
-{
-	int ret;
-	int i;
-
-	init_waitqueue_head(&solo_dev->disp_thread_wait);
-
-	solo_dev->vfd = video_device_alloc();
-	if (!solo_dev->vfd)
-		return -ENOMEM;
-
-	*solo_dev->vfd = solo_v4l2_template;
-	solo_dev->vfd->parent = &solo_dev->pdev->dev;
-
-	ret = video_register_device(solo_dev->vfd, VFL_TYPE_GRABBER, video_nr);
-	if (ret < 0) {
-		video_device_release(solo_dev->vfd);
-		solo_dev->vfd = NULL;
-		return ret;
-	}
-
-	video_set_drvdata(solo_dev->vfd, solo_dev);
-
-	snprintf(solo_dev->vfd->name, sizeof(solo_dev->vfd->name), "%s (%i)",
-		 SOLO6X10_NAME, solo_dev->vfd->num);
-
-	if (video_nr != -1)
-		video_nr++;
-
-	dev_info(&solo_dev->pdev->dev, "Display as /dev/video%d with "
-		 "%d inputs (%d extended)\n", solo_dev->vfd->num,
-		 solo_dev->nr_chans, solo_dev->nr_ext);
-
-	/* Cycle all the channels and clear */
-	for (i = 0; i < solo_dev->nr_chans; i++) {
-		solo_v4l2_set_ch(solo_dev, i);
-		while (erase_off(solo_dev))
-			;/* Do nothing */
-	}
-
-	/* Set the default display channel */
-	solo_v4l2_set_ch(solo_dev, 0);
-	while (erase_off(solo_dev))
-		;/* Do nothing */
-
-	solo_irq_on(solo_dev, SOLO_IRQ_VIDEO_IN);
-
-	return 0;
-}
-
-void solo_v4l2_exit(struct solo_dev *solo_dev)
-{
-	solo_irq_off(solo_dev, SOLO_IRQ_VIDEO_IN);
-	if (solo_dev->vfd) {
-		video_unregister_device(solo_dev->vfd);
-		solo_dev->vfd = NULL;
-	}
-}
diff --git a/drivers/staging/net/Kconfig b/drivers/staging/net/Kconfig
deleted file mode 100644
index a64e56b1898..00000000000
--- a/drivers/staging/net/Kconfig
+++ /dev/null
@@ -1,38 +0,0 @@
-if NETDEVICES
-
-if WAN
-
-config PC300
-	tristate "Cyclades-PC300 support (RS-232/V.35, X.21, T1/E1 boards)"
-	depends on HDLC && PCI && BROKEN
-	---help---
-	  This driver is broken because of struct tty_driver change.
-
-	  Driver for the Cyclades-PC300 synchronous communication boards.
-
-	  These boards provide synchronous serial interfaces to your
-	  Linux box (interfaces currently available are RS-232/V.35, X.21 and
-	  T1/E1). If you wish to support Multilink PPP, please select the
-	  option later and read the file README.mlppp provided by PC300
-	  package.
-
-	  To compile this as a module, choose M here: the module
-	  will be called pc300.
-
-	  If unsure, say N.
-
-config PC300_MLPPP
-	bool "Cyclades-PC300 MLPPP support"
-	depends on PC300 && PPP_MULTILINK && PPP_SYNC_TTY && HDLC_PPP
-	help
-	  Multilink PPP over the PC300 synchronous communication boards.
-
-comment "Cyclades-PC300 MLPPP support is disabled."
-	depends on HDLC && PC300 && (PPP=n || !PPP_MULTILINK || PPP_SYNC_TTY=n || !HDLC_PPP)
-
-comment "Refer to the file README.mlppp, provided by PC300 package."
-	depends on HDLC && PC300 && (PPP=n || !PPP_MULTILINK || PPP_SYNC_TTY=n || !HDLC_PPP)
-
-endif # WAN
-
-endif # NETDEVICES
diff --git a/drivers/staging/net/Makefile b/drivers/staging/net/Makefile
deleted file mode 100644
index 0799c43d511..00000000000
--- a/drivers/staging/net/Makefile
+++ /dev/null
@@ -1,5 +0,0 @@
-pc300-y				:= pc300_drv.o
-pc300-$(CONFIG_PC300_MLPPP)	+= pc300_tty.o
-pc300-objs			:= $(pc300-y)
-
-obj-$(CONFIG_PC300)		+= pc300.o
diff --git a/drivers/staging/net/TODO b/drivers/staging/net/TODO
deleted file mode 100644
index e3446f2ad7c..00000000000
--- a/drivers/staging/net/TODO
+++ /dev/null
@@ -1,5 +0,0 @@
-PC300
-The driver is very broken and cannot work with the current TTY layer. It is
-inevitable to convert it to the new TTY API.
-
-If no one steps in to adopt the driver, it will be removed in the 3.7 release.
diff --git a/drivers/staging/net/pc300-falc-lh.h b/drivers/staging/net/pc300-falc-lh.h
deleted file mode 100644
index 01ed23ca76c..00000000000
--- a/drivers/staging/net/pc300-falc-lh.h
+++ /dev/null
@@ -1,1238 +0,0 @@
-/*
- * falc.h	Description of the Siemens FALC T1/E1 framer.
- *
- * Author:	Ivan Passos <ivan@cyclades.com>
- *
- * Copyright:	(c) 2000-2001 Cyclades Corp.
- *
- *	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.
- *
- * $Log: falc-lh.h,v $
- * Revision 3.1  2001/06/15 12:41:10  regina
- * upping major version number
- *
- * Revision 1.1.1.1  2001/06/13 20:24:47  daniela
- * PC300 initial CVS version (3.4.0-pre1)
- *
- * Revision 1.1 2000/05/15 ivan
- * Included DJA bits for the LIM2 register.
- *
- * Revision 1.0 2000/02/22 ivan
- * Initial version.
- *
- */
-
-#ifndef _FALC_LH_H
-#define _FALC_LH_H
-
-#define NUM_OF_T1_CHANNELS	24
-#define NUM_OF_E1_CHANNELS	32
-
-/*>>>>>>>>>>>>>>>>>  FALC Register Bits (Transmit Mode)  <<<<<<<<<<<<<<<<<<< */
-
-/* CMDR (Command Register)
-   ---------------- E1 & T1 ------------------------------ */
-#define CMDR_RMC	0x80
-#define CMDR_RRES	0x40
-#define CMDR_XREP	0x20
-#define CMDR_XRES	0x10
-#define CMDR_XHF	0x08
-#define CMDR_XTF	0x04
-#define CMDR_XME	0x02
-#define CMDR_SRES	0x01
-
-/* MODE (Mode Register)
-   ----------------- E1 & T1 ----------------------------- */
-#define MODE_MDS2	0x80
-#define MODE_MDS1	0x40
-#define MODE_MDS0	0x20
-#define MODE_BRAC	0x10
-#define MODE_HRAC	0x08
-
-/* IPC (Interrupt Port Configuration)
-   ----------------- E1 & T1 ----------------------------- */
-#define IPC_VIS		0x80
-#define IPC_SCI		0x04
-#define IPC_IC1		0x02
-#define IPC_IC0		0x01
-
-/* CCR1 (Common Configuration Register 1)
-   ----------------- E1 & T1 ----------------------------- */
-#define CCR1_SFLG       0x80
-#define CCR1_XTS16RA    0x40
-#define CCR1_BRM        0x40
-#define CCR1_CASSYM     0x20
-#define CCR1_EDLX       0x20
-#define CCR1_EITS       0x10
-#define CCR1_ITF        0x08
-#define CCR1_RFT1       0x02
-#define CCR1_RFT0       0x01
-
-/* CCR3 (Common Configuration Register 3)
-   ---------------- E1 & T1 ------------------------------ */
-
-#define CCR3_PRE1       0x80
-#define CCR3_PRE0       0x40
-#define CCR3_EPT        0x20
-#define CCR3_RADD       0x10
-#define CCR3_RCRC       0x04
-#define CCR3_XCRC       0x02
-
-
-/* RTR1-4 (Receive Timeslot Register 1-4)
-   ---------------- E1 & T1 ------------------------------ */
-
-#define RTR1_TS0        0x80
-#define RTR1_TS1        0x40
-#define RTR1_TS2        0x20
-#define RTR1_TS3        0x10
-#define RTR1_TS4        0x08
-#define RTR1_TS5        0x04
-#define RTR1_TS6        0x02
-#define RTR1_TS7        0x01
-
-#define RTR2_TS8        0x80
-#define RTR2_TS9        0x40
-#define RTR2_TS10       0x20
-#define RTR2_TS11       0x10
-#define RTR2_TS12       0x08
-#define RTR2_TS13       0x04
-#define RTR2_TS14       0x02
-#define RTR2_TS15       0x01
-
-#define RTR3_TS16       0x80
-#define RTR3_TS17       0x40
-#define RTR3_TS18       0x20
-#define RTR3_TS19       0x10
-#define RTR3_TS20       0x08
-#define RTR3_TS21       0x04
-#define RTR3_TS22       0x02
-#define RTR3_TS23       0x01
-
-#define RTR4_TS24       0x80
-#define RTR4_TS25       0x40
-#define RTR4_TS26       0x20
-#define RTR4_TS27       0x10
-#define RTR4_TS28       0x08
-#define RTR4_TS29       0x04
-#define RTR4_TS30       0x02
-#define RTR4_TS31       0x01
-
-
-/* TTR1-4 (Transmit Timeslot Register 1-4)
-   ---------------- E1 & T1 ------------------------------ */
-
-#define TTR1_TS0        0x80
-#define TTR1_TS1        0x40
-#define TTR1_TS2        0x20
-#define TTR1_TS3        0x10
-#define TTR1_TS4        0x08
-#define TTR1_TS5        0x04
-#define TTR1_TS6        0x02
-#define TTR1_TS7        0x01
-
-#define TTR2_TS8        0x80
-#define TTR2_TS9        0x40
-#define TTR2_TS10       0x20
-#define TTR2_TS11       0x10
-#define TTR2_TS12       0x08
-#define TTR2_TS13       0x04
-#define TTR2_TS14       0x02
-#define TTR2_TS15       0x01
-
-#define TTR3_TS16       0x80
-#define TTR3_TS17       0x40
-#define TTR3_TS18       0x20
-#define TTR3_TS19       0x10
-#define TTR3_TS20       0x08
-#define TTR3_TS21       0x04
-#define TTR3_TS22       0x02
-#define TTR3_TS23       0x01
-
-#define TTR4_TS24       0x80
-#define TTR4_TS25       0x40
-#define TTR4_TS26       0x20
-#define TTR4_TS27       0x10
-#define TTR4_TS28       0x08
-#define TTR4_TS29       0x04
-#define TTR4_TS30       0x02
-#define TTR4_TS31       0x01
-
-
-
-/* IMR0-4 (Interrupt Mask Register 0-4)
-
-   ----------------- E1 & T1 ----------------------------- */
-
-#define IMR0_RME        0x80
-#define IMR0_RFS        0x40
-#define IMR0_T8MS       0x20
-#define IMR0_ISF        0x20
-#define IMR0_RMB        0x10
-#define IMR0_CASC       0x08
-#define IMR0_RSC        0x08
-#define IMR0_CRC6       0x04
-#define IMR0_CRC4       0x04
-#define IMR0_PDEN	0x02
-#define IMR0_RPF        0x01
-
-#define IMR1_CASE       0x80
-#define IMR1_RDO        0x40
-#define IMR1_ALLS       0x20
-#define IMR1_XDU        0x10
-#define IMR1_XMB        0x08
-#define IMR1_XLSC       0x02
-#define IMR1_XPR        0x01
-#define IMR1_LLBSC	0x80
-
-#define IMR2_FAR        0x80
-#define IMR2_LFA        0x40
-#define IMR2_MFAR       0x20
-#define IMR2_T400MS     0x10
-#define IMR2_LMFA       0x10
-#define IMR2_AIS        0x08
-#define IMR2_LOS        0x04
-#define IMR2_RAR        0x02
-#define IMR2_RA         0x01
-
-#define IMR3_ES         0x80
-#define IMR3_SEC        0x40
-#define IMR3_LMFA16     0x20
-#define IMR3_AIS16      0x10
-#define IMR3_RA16       0x08
-#define IMR3_API        0x04
-#define IMR3_XSLP       0x20
-#define IMR3_XSLN       0x10
-#define IMR3_LLBSC      0x08
-#define IMR3_XRS        0x04
-#define IMR3_SLN        0x02
-#define IMR3_SLP        0x01
-
-#define IMR4_LFA        0x80
-#define IMR4_FER        0x40
-#define IMR4_CER        0x20
-#define IMR4_AIS        0x10
-#define IMR4_LOS        0x08
-#define IMR4_CVE        0x04
-#define IMR4_SLIP       0x02
-#define IMR4_EBE        0x01
-
-/* FMR0-5 for E1 and T1  (Framer Mode Register ) */
-
-#define FMR0_XC1        0x80
-#define FMR0_XC0        0x40
-#define FMR0_RC1        0x20
-#define FMR0_RC0        0x10
-#define FMR0_EXTD       0x08
-#define FMR0_ALM        0x04
-#define E1_FMR0_FRS     0x02
-#define T1_FMR0_FRS     0x08
-#define FMR0_SRAF       0x04
-#define FMR0_EXLS       0x02
-#define FMR0_SIM        0x01
-
-#define FMR1_MFCS       0x80
-#define FMR1_AFR        0x40
-#define FMR1_ENSA       0x20
-#define FMR1_CTM        0x80
-#define FMR1_SIGM       0x40
-#define FMR1_EDL        0x20
-#define FMR1_PMOD       0x10
-#define FMR1_XFS        0x08
-#define FMR1_CRC        0x08
-#define FMR1_ECM        0x04
-#define FMR1_IMOD       0x02
-#define FMR1_XAIS       0x01
-
-#define FMR2_RFS1       0x80
-#define FMR2_RFS0       0x40
-#define FMR2_MCSP	0x40
-#define FMR2_RTM        0x20
-#define FMR2_SSP        0x20
-#define FMR2_DAIS       0x10
-#define FMR2_SAIS       0x08
-#define FMR2_PLB        0x04
-#define FMR2_AXRA       0x02
-#define FMR2_ALMF       0x01
-#define FMR2_EXZE       0x01
-
-#define LOOP_RTM	0x40
-#define LOOP_SFM	0x40
-#define LOOP_ECLB	0x20
-#define LOOP_CLA	0x1f
-
-/*--------------------- E1 ----------------------------*/
-#define FMR3_XLD	0x20
-#define FMR3_XLU	0x10
-
-/*--------------------- T1 ----------------------------*/
-#define FMR4_AIS3       0x80
-#define FMR4_TM         0x40
-#define FMR4_XRA        0x20
-#define FMR4_SSC1       0x10
-#define FMR4_SSC0       0x08
-#define FMR4_AUTO       0x04
-#define FMR4_FM1        0x02
-#define FMR4_FM0        0x01
-
-#define FMR5_SRS        0x80
-#define FMR5_EIBR       0x40
-#define FMR5_XLD        0x20
-#define FMR5_XLU        0x10
-
-
-/* LOOP (Channel Loop Back)
-
-   ------------------ E1 & T1 ---------------------------- */
-
-#define LOOP_SFM        0x40
-#define LOOP_ECLB       0x20
-#define LOOP_CLA4       0x10
-#define LOOP_CLA3       0x08
-#define LOOP_CLA2       0x04
-#define LOOP_CLA1       0x02
-#define LOOP_CLA0       0x01
-
-
-
-/* XSW (Transmit Service Word Pulseframe)
-
-   ------------------- E1 --------------------------- */
-
-#define XSW_XSIS        0x80
-#define XSW_XTM         0x40
-#define XSW_XRA         0x20
-#define XSW_XY0         0x10
-#define XSW_XY1         0x08
-#define XSW_XY2         0x04
-#define XSW_XY3         0x02
-#define XSW_XY4         0x01
-
-
-/* XSP (Transmit Spare Bits)
-
-   ------------------- E1 --------------------------- */
-
-#define XSP_XAP         0x80
-#define XSP_CASEN       0x40
-#define XSP_TT0         0x20
-#define XSP_EBP         0x10
-#define XSP_AXS         0x08
-#define XSP_XSIF        0x04
-#define XSP_XS13        0x02
-#define XSP_XS15        0x01
-
-
-/* XC0/1 (Transmit Control 0/1)
-   ------------------ E1 & T1 ---------------------------- */
-
-#define XC0_SA8E        0x80
-#define XC0_SA7E        0x40
-#define XC0_SA6E        0x20
-#define XC0_SA5E        0x10
-#define XC0_SA4E        0x08
-#define XC0_BRM         0x80
-#define XC0_MFBS        0x40
-#define XC0_SFRZ        0x10
-#define XC0_XCO2        0x04
-#define XC0_XCO1        0x02
-#define XC0_XCO0        0x01
-
-#define XC1_XTO5        0x20
-#define XC1_XTO4        0x10
-#define XC1_XTO3        0x08
-#define XC1_XTO2        0x04
-#define XC1_XTO1        0x02
-#define XC1_XTO0        0x01
-
-
-/* RC0/1 (Receive Control 0/1)
-   ------------------ E1 & T1 ---------------------------- */
-
-#define RC0_SICS        0x40
-#define RC0_CRCI        0x20
-#define RC0_XCRCI       0x10
-#define RC0_RDIS        0x08
-#define RC0_RCO2        0x04
-#define RC0_RCO1        0x02
-#define RC0_RCO0        0x01
-
-#define RC1_SWD         0x80
-#define RC1_ASY4        0x40
-#define RC1_RRAM        0x40
-#define RC1_RTO5        0x20
-#define RC1_RTO4        0x10
-#define RC1_RTO3        0x08
-#define RC1_RTO2        0x04
-#define RC1_RTO1        0x02
-#define RC1_RTO0        0x01
-
-
-
-/* XPM0-2 (Transmit Pulse Mask 0-2)
-   --------------------- E1 & T1 ------------------------- */
-
-#define XPM0_XP12       0x80
-#define XPM0_XP11       0x40
-#define XPM0_XP10       0x20
-#define XPM0_XP04       0x10
-#define XPM0_XP03       0x08
-#define XPM0_XP02       0x04
-#define XPM0_XP01       0x02
-#define XPM0_XP00       0x01
-
-#define XPM1_XP30       0x80
-#define XPM1_XP24       0x40
-#define XPM1_XP23       0x20
-#define XPM1_XP22       0x10
-#define XPM1_XP21       0x08
-#define XPM1_XP20       0x04
-#define XPM1_XP14       0x02
-#define XPM1_XP13       0x01
-
-#define XPM2_XLHP       0x80
-#define XPM2_XLT        0x40
-#define XPM2_DAXLT      0x20
-#define XPM2_XP34       0x08
-#define XPM2_XP33       0x04
-#define XPM2_XP32       0x02
-#define XPM2_XP31       0x01
-
-
-/* TSWM (Transparent Service Word Mask)
-   ------------------ E1 ---------------------------- */
-
-#define TSWM_TSIS       0x80
-#define TSWM_TSIF       0x40
-#define TSWM_TRA        0x20
-#define TSWM_TSA4       0x10
-#define TSWM_TSA5       0x08
-#define TSWM_TSA6       0x04
-#define TSWM_TSA7       0x02
-#define TSWM_TSA8       0x01
-
-/* IDLE <Idle Channel Code Register>
-
-   ------------------ E1 & T1 ----------------------- */
-
-#define IDLE_IDL7       0x80
-#define IDLE_IDL6       0x40
-#define IDLE_IDL5       0x20
-#define IDLE_IDL4       0x10
-#define IDLE_IDL3       0x08
-#define IDLE_IDL2       0x04
-#define IDLE_IDL1       0x02
-#define IDLE_IDL0       0x01
-
-
-/* XSA4-8 <Transmit SA4-8 Register(Read/Write) >
-   -------------------E1 ----------------------------- */
-
-#define XSA4_XS47       0x80
-#define XSA4_XS46       0x40
-#define XSA4_XS45       0x20
-#define XSA4_XS44       0x10
-#define XSA4_XS43       0x08
-#define XSA4_XS42       0x04
-#define XSA4_XS41       0x02
-#define XSA4_XS40       0x01
-
-#define XSA5_XS57       0x80
-#define XSA5_XS56       0x40
-#define XSA5_XS55       0x20
-#define XSA5_XS54       0x10
-#define XSA5_XS53       0x08
-#define XSA5_XS52       0x04
-#define XSA5_XS51       0x02
-#define XSA5_XS50       0x01
-
-#define XSA6_XS67       0x80
-#define XSA6_XS66       0x40
-#define XSA6_XS65       0x20
-#define XSA6_XS64       0x10
-#define XSA6_XS63       0x08
-#define XSA6_XS62       0x04
-#define XSA6_XS61       0x02
-#define XSA6_XS60       0x01
-
-#define XSA7_XS77       0x80
-#define XSA7_XS76       0x40
-#define XSA7_XS75       0x20
-#define XSA7_XS74       0x10
-#define XSA7_XS73       0x08
-#define XSA7_XS72       0x04
-#define XSA7_XS71       0x02
-#define XSA7_XS70       0x01
-
-#define XSA8_XS87       0x80
-#define XSA8_XS86       0x40
-#define XSA8_XS85       0x20
-#define XSA8_XS84       0x10
-#define XSA8_XS83       0x08
-#define XSA8_XS82       0x04
-#define XSA8_XS81       0x02
-#define XSA8_XS80       0x01
-
-
-/* XDL1-3 (Transmit DL-Bit Register1-3 (read/write))
-   ----------------------- T1 --------------------- */
-
-#define XDL1_XDL17      0x80
-#define XDL1_XDL16      0x40
-#define XDL1_XDL15      0x20
-#define XDL1_XDL14      0x10
-#define XDL1_XDL13      0x08
-#define XDL1_XDL12      0x04
-#define XDL1_XDL11      0x02
-#define XDL1_XDL10      0x01
-
-#define XDL2_XDL27      0x80
-#define XDL2_XDL26      0x40
-#define XDL2_XDL25      0x20
-#define XDL2_XDL24      0x10
-#define XDL2_XDL23      0x08
-#define XDL2_XDL22      0x04
-#define XDL2_XDL21      0x02
-#define XDL2_XDL20      0x01
-
-#define XDL3_XDL37      0x80
-#define XDL3_XDL36      0x40
-#define XDL3_XDL35      0x20
-#define XDL3_XDL34      0x10
-#define XDL3_XDL33      0x08
-#define XDL3_XDL32      0x04
-#define XDL3_XDL31      0x02
-#define XDL3_XDL30      0x01
-
-
-/* ICB1-4 (Idle Channel Register 1-4)
-   ------------------ E1 ---------------------------- */
-
-#define E1_ICB1_IC0	0x80
-#define E1_ICB1_IC1	0x40
-#define E1_ICB1_IC2	0x20
-#define E1_ICB1_IC3	0x10
-#define E1_ICB1_IC4	0x08
-#define E1_ICB1_IC5	0x04
-#define E1_ICB1_IC6	0x02
-#define E1_ICB1_IC7	0x01
-
-#define E1_ICB2_IC8	0x80
-#define E1_ICB2_IC9	0x40
-#define E1_ICB2_IC10	0x20
-#define E1_ICB2_IC11	0x10
-#define E1_ICB2_IC12	0x08
-#define E1_ICB2_IC13	0x04
-#define E1_ICB2_IC14	0x02
-#define E1_ICB2_IC15	0x01
-
-#define E1_ICB3_IC16	0x80
-#define E1_ICB3_IC17	0x40
-#define E1_ICB3_IC18	0x20
-#define E1_ICB3_IC19	0x10
-#define E1_ICB3_IC20	0x08
-#define E1_ICB3_IC21	0x04
-#define E1_ICB3_IC22	0x02
-#define E1_ICB3_IC23	0x01
-
-#define E1_ICB4_IC24	0x80
-#define E1_ICB4_IC25	0x40
-#define E1_ICB4_IC26	0x20
-#define E1_ICB4_IC27	0x10
-#define E1_ICB4_IC28	0x08
-#define E1_ICB4_IC29	0x04
-#define E1_ICB4_IC30	0x02
-#define E1_ICB4_IC31	0x01
-
-/* ICB1-4 (Idle Channel Register 1-4)
-   ------------------ T1 ---------------------------- */
-
-#define T1_ICB1_IC1	0x80
-#define T1_ICB1_IC2	0x40
-#define T1_ICB1_IC3	0x20
-#define T1_ICB1_IC4	0x10
-#define T1_ICB1_IC5	0x08
-#define T1_ICB1_IC6	0x04
-#define T1_ICB1_IC7	0x02
-#define T1_ICB1_IC8	0x01
-
-#define T1_ICB2_IC9	0x80
-#define T1_ICB2_IC10	0x40
-#define T1_ICB2_IC11	0x20
-#define T1_ICB2_IC12	0x10
-#define T1_ICB2_IC13	0x08
-#define T1_ICB2_IC14	0x04
-#define T1_ICB2_IC15	0x02
-#define T1_ICB2_IC16	0x01
-
-#define T1_ICB3_IC17	0x80
-#define T1_ICB3_IC18	0x40
-#define T1_ICB3_IC19	0x20
-#define T1_ICB3_IC20	0x10
-#define T1_ICB3_IC21	0x08
-#define T1_ICB3_IC22	0x04
-#define T1_ICB3_IC23	0x02
-#define T1_ICB3_IC24	0x01
-
-/* FMR3 (Framer Mode Register 3)
-   --------------------E1------------------------ */
-
-#define FMR3_CMI        0x08
-#define FMR3_SYNSA      0x04
-#define FMR3_CFRZ       0x02
-#define FMR3_EXTIW      0x01
-
-
-
-/* CCB1-3 (Clear Channel Register)
-   ------------------- T1 ----------------------- */
-
-#define CCB1_CH1        0x80
-#define CCB1_CH2        0x40
-#define CCB1_CH3        0x20
-#define CCB1_CH4        0x10
-#define CCB1_CH5        0x08
-#define CCB1_CH6        0x04
-#define CCB1_CH7        0x02
-#define CCB1_CH8        0x01
-
-#define CCB2_CH9        0x80
-#define CCB2_CH10       0x40
-#define CCB2_CH11       0x20
-#define CCB2_CH12       0x10
-#define CCB2_CH13       0x08
-#define CCB2_CH14       0x04
-#define CCB2_CH15       0x02
-#define CCB2_CH16       0x01
-
-#define CCB3_CH17       0x80
-#define CCB3_CH18       0x40
-#define CCB3_CH19       0x20
-#define CCB3_CH20       0x10
-#define CCB3_CH21       0x08
-#define CCB3_CH22       0x04
-#define CCB3_CH23       0x02
-#define CCB3_CH24       0x01
-
-
-/* LIM0/1 (Line Interface Mode 0/1)
-   ------------------- E1 & T1 --------------------------- */
-
-#define LIM0_XFB        0x80
-#define LIM0_XDOS       0x40
-#define LIM0_SCL1       0x20
-#define LIM0_SCL0       0x10
-#define LIM0_EQON       0x08
-#define LIM0_ELOS       0x04
-#define LIM0_LL         0x02
-#define LIM0_MAS        0x01
-
-#define LIM1_EFSC       0x80
-#define LIM1_RIL2       0x40
-#define LIM1_RIL1       0x20
-#define LIM1_RIL0       0x10
-#define LIM1_DCOC       0x08
-#define LIM1_JATT       0x04
-#define LIM1_RL         0x02
-#define LIM1_DRS        0x01
-
-
-/* PCDR (Pulse Count Detection Register(Read/Write))
-   ------------------ E1 & T1 ------------------------- */
-
-#define PCDR_PCD7	0x80
-#define PCDR_PCD6	0x40
-#define PCDR_PCD5	0x20
-#define PCDR_PCD4	0x10
-#define PCDR_PCD3	0x08
-#define PCDR_PCD2	0x04
-#define PCDR_PCD1	0x02
-#define PCDR_PCD0	0x01
-
-#define PCRR_PCR7	0x80
-#define PCRR_PCR6	0x40
-#define PCRR_PCR5	0x20
-#define PCRR_PCR4	0x10
-#define PCRR_PCR3	0x08
-#define PCRR_PCR2	0x04
-#define PCRR_PCR1	0x02
-#define PCRR_PCR0	0x01
-
-
-/* LIM2 (Line Interface Mode 2)
-
-   ------------------ E1 & T1 ---------------------------- */
-
-#define LIM2_DJA2	0x20
-#define LIM2_DJA1	0x10
-#define LIM2_LOS2	0x02
-#define LIM2_LOS1	0x01
-
-/* LCR1 (Loop Code Register 1) */
-
-#define LCR1_EPRM	0x80
-#define	LCR1_XPRBS	0x40
-
-/* SIC1 (System Interface Control 1) */
-#define SIC1_SRSC	0x80
-#define SIC1_RBS1	0x20
-#define SIC1_RBS0	0x10
-#define SIC1_SXSC	0x08
-#define SIC1_XBS1	0x02
-#define SIC1_XBS0	0x01
-
-/* DEC (Disable Error Counter)
-   ------------------ E1 & T1 ---------------------------- */
-
-#define DEC_DCEC3       0x20
-#define DEC_DBEC        0x10
-#define DEC_DCEC1       0x08
-#define DEC_DCEC        0x08
-#define DEC_DEBC        0x04
-#define DEC_DCVC        0x02
-#define DEC_DFEC        0x01
-
-
-/* FALC Register Bits (Receive Mode)
-   ---------------------------------------------------------------------------- */
-
-
-/* FRS0/1 (Framer Receive Status Register 0/1)
-   ----------------- E1 & T1 ---------------------------------- */
-
-#define FRS0_LOS        0x80
-#define FRS0_AIS        0x40
-#define FRS0_LFA        0x20
-#define FRS0_RRA        0x10
-#define FRS0_API        0x08
-#define FRS0_NMF        0x04
-#define FRS0_LMFA       0x02
-#define FRS0_FSRF       0x01
-
-#define FRS1_TS16RA     0x40
-#define FRS1_TS16LOS    0x20
-#define FRS1_TS16AIS    0x10
-#define FRS1_TS16LFA    0x08
-#define FRS1_EXZD       0x80
-#define FRS1_LLBDD      0x10
-#define FRS1_LLBAD      0x08
-#define FRS1_XLS        0x02
-#define FRS1_XLO        0x01
-#define FRS1_PDEN	0x40
-
-/* FRS2/3 (Framer Receive Status Register 2/3)
-   ----------------- T1 ---------------------------------- */
-
-#define FRS2_ESC2       0x80
-#define FRS2_ESC1       0x40
-#define FRS2_ESC0       0x20
-
-#define FRS3_FEH5       0x20
-#define FRS3_FEH4       0x10
-#define FRS3_FEH3       0x08
-#define FRS3_FEH2       0x04
-#define FRS3_FEH1       0x02
-#define FRS3_FEH0       0x01
-
-
-/* RSW (Receive Service Word Pulseframe)
-   ----------------- E1 ------------------------------ */
-
-#define RSW_RSI         0x80
-#define RSW_RRA         0x20
-#define RSW_RYO         0x10
-#define RSW_RY1         0x08
-#define RSW_RY2         0x04
-#define RSW_RY3         0x02
-#define RSW_RY4         0x01
-
-
-/* RSP (Receive Spare Bits / Additional Status)
-   ---------------- E1 ------------------------------- */
-
-#define RSP_SI1         0x80
-#define RSP_SI2         0x40
-#define RSP_LLBDD	0x10
-#define RSP_LLBAD	0x08
-#define RSP_RSIF        0x04
-#define RSP_RS13        0x02
-#define RSP_RS15        0x01
-
-
-/* FECL (Framing Error Counter)
-   ---------------- E1 & T1 -------------------------- */
-
-#define FECL_FE7        0x80
-#define FECL_FE6        0x40
-#define FECL_FE5        0x20
-#define FECL_FE4        0x10
-#define FECL_FE3        0x08
-#define FECL_FE2        0x04
-#define FECL_FE1        0x02
-#define FECL_FE0        0x01
-
-#define FECH_FE15       0x80
-#define FECH_FE14       0x40
-#define FECH_FE13       0x20
-#define FECH_FE12       0x10
-#define FECH_FE11       0x08
-#define FECH_FE10       0x04
-#define FECH_FE9        0x02
-#define FECH_FE8        0x01
-
-
-/* CVCl (Code Violation Counter)
-   ----------------- E1 ------------------------- */
-
-#define CVCL_CV7        0x80
-#define CVCL_CV6        0x40
-#define CVCL_CV5        0x20
-#define CVCL_CV4        0x10
-#define CVCL_CV3        0x08
-#define CVCL_CV2        0x04
-#define CVCL_CV1        0x02
-#define CVCL_CV0        0x01
-
-#define CVCH_CV15       0x80
-#define CVCH_CV14       0x40
-#define CVCH_CV13       0x20
-#define CVCH_CV12       0x10
-#define CVCH_CV11       0x08
-#define CVCH_CV10       0x04
-#define CVCH_CV9        0x02
-#define CVCH_CV8        0x01
-
-
-/* CEC1-3L (CRC Error Counter)
-   ------------------ E1 ----------------------------- */
-
-#define CEC1L_CR7       0x80
-#define CEC1L_CR6       0x40
-#define CEC1L_CR5       0x20
-#define CEC1L_CR4       0x10
-#define CEC1L_CR3       0x08
-#define CEC1L_CR2       0x04
-#define CEC1L_CR1       0x02
-#define CEC1L_CR0       0x01
-
-#define CEC1H_CR15      0x80
-#define CEC1H_CR14      0x40
-#define CEC1H_CR13      0x20
-#define CEC1H_CR12      0x10
-#define CEC1H_CR11      0x08
-#define CEC1H_CR10      0x04
-#define CEC1H_CR9       0x02
-#define CEC1H_CR8       0x01
-
-#define CEC2L_CR7       0x80
-#define CEC2L_CR6       0x40
-#define CEC2L_CR5       0x20
-#define CEC2L_CR4       0x10
-#define CEC2L_CR3       0x08
-#define CEC2L_CR2       0x04
-#define CEC2L_CR1       0x02
-#define CEC2L_CR0       0x01
-
-#define CEC2H_CR15      0x80
-#define CEC2H_CR14      0x40
-#define CEC2H_CR13      0x20
-#define CEC2H_CR12      0x10
-#define CEC2H_CR11      0x08
-#define CEC2H_CR10      0x04
-#define CEC2H_CR9       0x02
-#define CEC2H_CR8       0x01
-
-#define CEC3L_CR7       0x80
-#define CEC3L_CR6       0x40
-#define CEC3L_CR5       0x20
-#define CEC3L_CR4       0x10
-#define CEC3L_CR3       0x08
-#define CEC3L_CR2       0x04
-#define CEC3L_CR1       0x02
-#define CEC3L_CR0       0x01
-
-#define CEC3H_CR15      0x80
-#define CEC3H_CR14      0x40
-#define CEC3H_CR13      0x20
-#define CEC3H_CR12      0x10
-#define CEC3H_CR11      0x08
-#define CEC3H_CR10      0x04
-#define CEC3H_CR9       0x02
-#define CEC3H_CR8       0x01
-
-
-/* CECL (CRC Error Counter)
-
-   ------------------ T1 ----------------------------- */
-
-#define CECL_CR7        0x80
-#define CECL_CR6        0x40
-#define CECL_CR5        0x20
-#define CECL_CR4        0x10
-#define CECL_CR3        0x08
-#define CECL_CR2        0x04
-#define CECL_CR1        0x02
-#define CECL_CR0        0x01
-
-#define CECH_CR15       0x80
-#define CECH_CR14       0x40
-#define CECH_CR13       0x20
-#define CECH_CR12       0x10
-#define CECH_CR11       0x08
-#define CECH_CR10       0x04
-#define CECH_CR9        0x02
-#define CECH_CR8        0x01
-
-/* EBCL (E Bit Error Counter)
-   ------------------- E1 & T1 ------------------------- */
-
-#define EBCL_EB7        0x80
-#define EBCL_EB6        0x40
-#define EBCL_EB5        0x20
-#define EBCL_EB4        0x10
-#define EBCL_EB3        0x08
-#define EBCL_EB2        0x04
-#define EBCL_EB1        0x02
-#define EBCL_EB0        0x01
-
-#define EBCH_EB15       0x80
-#define EBCH_EB14       0x40
-#define EBCH_EB13       0x20
-#define EBCH_EB12       0x10
-#define EBCH_EB11       0x08
-#define EBCH_EB10       0x04
-#define EBCH_EB9        0x02
-#define EBCH_EB8        0x01
-
-
-/* RSA4-8 (Receive Sa4-8-Bit Register)
-   -------------------- E1 --------------------------- */
-
-#define RSA4_RS47       0x80
-#define RSA4_RS46       0x40
-#define RSA4_RS45       0x20
-#define RSA4_RS44       0x10
-#define RSA4_RS43       0x08
-#define RSA4_RS42       0x04
-#define RSA4_RS41       0x02
-#define RSA4_RS40       0x01
-
-#define RSA5_RS57       0x80
-#define RSA5_RS56       0x40
-#define RSA5_RS55       0x20
-#define RSA5_RS54       0x10
-#define RSA5_RS53       0x08
-#define RSA5_RS52       0x04
-#define RSA5_RS51       0x02
-#define RSA5_RS50       0x01
-
-#define RSA6_RS67       0x80
-#define RSA6_RS66       0x40
-#define RSA6_RS65       0x20
-#define RSA6_RS64       0x10
-#define RSA6_RS63       0x08
-#define RSA6_RS62       0x04
-#define RSA6_RS61       0x02
-#define RSA6_RS60       0x01
-
-#define RSA7_RS77       0x80
-#define RSA7_RS76       0x40
-#define RSA7_RS75       0x20
-#define RSA7_RS74       0x10
-#define RSA7_RS73       0x08
-#define RSA7_RS72       0x04
-#define RSA7_RS71       0x02
-#define RSA7_RS70       0x01
-
-#define RSA8_RS87       0x80
-#define RSA8_RS86       0x40
-#define RSA8_RS85       0x20
-#define RSA8_RS84       0x10
-#define RSA8_RS83       0x08
-#define RSA8_RS82       0x04
-#define RSA8_RS81       0x02
-#define RSA8_RS80       0x01
-
-/* RSA6S (Receive Sa6 Bit Status Register)
-   ------------------------ T1 ------------------------- */
-
-#define RSA6S_SX        0x20
-#define RSA6S_SF        0x10
-#define RSA6S_SE        0x08
-#define RSA6S_SC        0x04
-#define RSA6S_SA        0x02
-#define RSA6S_S8        0x01
-
-
-/* RDL1-3 Receive DL-Bit Register1-3)
-   ------------------------ T1 ------------------------- */
-
-#define RDL1_RDL17      0x80
-#define RDL1_RDL16      0x40
-#define RDL1_RDL15      0x20
-#define RDL1_RDL14      0x10
-#define RDL1_RDL13      0x08
-#define RDL1_RDL12      0x04
-#define RDL1_RDL11      0x02
-#define RDL1_RDL10      0x01
-
-#define RDL2_RDL27      0x80
-#define RDL2_RDL26      0x40
-#define RDL2_RDL25      0x20
-#define RDL2_RDL24      0x10
-#define RDL2_RDL23      0x08
-#define RDL2_RDL22      0x04
-#define RDL2_RDL21      0x02
-#define RDL2_RDL20      0x01
-
-#define RDL3_RDL37      0x80
-#define RDL3_RDL36      0x40
-#define RDL3_RDL35      0x20
-#define RDL3_RDL34      0x10
-#define RDL3_RDL33      0x08
-#define RDL3_RDL32      0x04
-#define RDL3_RDL31      0x02
-#define RDL3_RDL30      0x01
-
-
-/* SIS (Signaling Status Register)
-
-   -------------------- E1 & T1 -------------------------- */
-
-#define SIS_XDOV        0x80
-#define SIS_XFW         0x40
-#define SIS_XREP        0x20
-#define SIS_RLI         0x08
-#define SIS_CEC         0x04
-#define SIS_BOM         0x01
-
-
-/* RSIS (Receive Signaling Status Register)
-
-   -------------------- E1 & T1 --------------------------- */
-
-#define RSIS_VFR        0x80
-#define RSIS_RDO        0x40
-#define RSIS_CRC16      0x20
-#define RSIS_RAB        0x10
-#define RSIS_HA1        0x08
-#define RSIS_HA0        0x04
-#define RSIS_HFR        0x02
-#define RSIS_LA         0x01
-
-
-/* RBCL/H (Receive Byte Count Low/High)
-
-   ------------------- E1 & T1 ----------------------- */
-
-#define RBCL_RBC7       0x80
-#define RBCL_RBC6       0x40
-#define RBCL_RBC5       0x20
-#define RBCL_RBC4       0x10
-#define RBCL_RBC3       0x08
-#define RBCL_RBC2       0x04
-#define RBCL_RBC1       0x02
-#define RBCL_RBC0       0x01
-
-#define RBCH_OV         0x10
-#define RBCH_RBC11      0x08
-#define RBCH_RBC10      0x04
-#define RBCH_RBC9       0x02
-#define RBCH_RBC8       0x01
-
-
-/* ISR1-3  (Interrupt Status Register 1-3)
-
-   ------------------ E1 & T1 ------------------------------ */
-
-#define  FISR0_RME	0x80
-#define  FISR0_RFS	0x40
-#define  FISR0_T8MS	0x20
-#define  FISR0_ISF	0x20
-#define  FISR0_RMB	0x10
-#define  FISR0_CASC	0x08
-#define  FISR0_RSC	0x08
-#define  FISR0_CRC6	0x04
-#define  FISR0_CRC4	0x04
-#define  FISR0_PDEN	0x02
-#define  FISR0_RPF	0x01
-
-#define  FISR1_CASE	0x80
-#define  FISR1_LLBSC	0x80
-#define  FISR1_RDO	0x40
-#define  FISR1_ALLS	0x20
-#define  FISR1_XDU	0x10
-#define  FISR1_XMB	0x08
-#define  FISR1_XLSC	0x02
-#define  FISR1_XPR	0x01
-
-#define  FISR2_FAR	0x80
-#define  FISR2_LFA	0x40
-#define  FISR2_MFAR	0x20
-#define  FISR2_T400MS	0x10
-#define  FISR2_LMFA	0x10
-#define  FISR2_AIS	0x08
-#define  FISR2_LOS	0x04
-#define  FISR2_RAR	0x02
-#define  FISR2_RA	0x01
-
-#define  FISR3_ES	0x80
-#define  FISR3_SEC	0x40
-#define  FISR3_LMFA16	0x20
-#define  FISR3_AIS16	0x10
-#define  FISR3_RA16	0x08
-#define  FISR3_API	0x04
-#define  FISR3_XSLP	0x20
-#define  FISR3_XSLN	0x10
-#define  FISR3_LLBSC	0x08
-#define  FISR3_XRS	0x04
-#define  FISR3_SLN	0x02
-#define  FISR3_SLP	0x01
-
-
-/* GIS  (Global Interrupt Status Register)
-
-   --------------------- E1 & T1 --------------------- */
-
-#define  GIS_ISR3	0x08
-#define  GIS_ISR2	0x04
-#define  GIS_ISR1	0x02
-#define  GIS_ISR0	0x01
-
-
-/* VSTR  (Version Status Register)
-
-   --------------------- E1 & T1 --------------------- */
-
-#define  VSTR_VN3	0x08
-#define  VSTR_VN2	0x04
-#define  VSTR_VN1	0x02
-#define  VSTR_VN0	0x01
-
-
-/*>>>>>>>>>>>>>>>>>>>>>  Local Control Structures  <<<<<<<<<<<<<<<<<<<<<<<<< */
-
-/* Write-only Registers (E1/T1 control mode write registers) */
-#define XFIFOH	0x00		/* Tx FIFO High Byte */
-#define XFIFOL	0x01		/* Tx FIFO Low Byte */
-#define CMDR	0x02		/* Command Reg */
-#define DEC	0x60		/* Disable Error Counter */
-#define TEST2	0x62		/* Manuf. Test Reg 2 */
-#define XS(nbr)	(0x70 + (nbr))	/* Tx CAS Reg (0 to 15) */
-
-/* Read-write Registers (E1/T1 status mode read registers) */
-#define MODE	0x03	/* Mode Reg */
-#define RAH1	0x04	/* Receive Address High 1 */
-#define RAH2	0x05	/* Receive Address High 2 */
-#define RAL1	0x06	/* Receive Address Low 1 */
-#define RAL2	0x07	/* Receive Address Low 2 */
-#define IPC	0x08	/* Interrupt Port Configuration */
-#define CCR1	0x09	/* Common Configuration Reg 1 */
-#define CCR3	0x0A	/* Common Configuration Reg 3 */
-#define PRE	0x0B	/* Preamble Reg */
-#define RTR1	0x0C	/* Receive Timeslot Reg 1 */
-#define RTR2	0x0D	/* Receive Timeslot Reg 2 */
-#define RTR3	0x0E	/* Receive Timeslot Reg 3 */
-#define RTR4	0x0F	/* Receive Timeslot Reg 4 */
-#define TTR1	0x10	/* Transmit Timeslot Reg 1 */
-#define TTR2	0x11	/* Transmit Timeslot Reg 2 */
-#define TTR3	0x12	/* Transmit Timeslot Reg 3 */
-#define TTR4	0x13	/* Transmit Timeslot Reg 4 */
-#define IMR0	0x14	/* Interrupt Mask Reg 0 */
-#define IMR1	0x15	/* Interrupt Mask Reg 1 */
-#define IMR2	0x16	/* Interrupt Mask Reg 2 */
-#define IMR3	0x17	/* Interrupt Mask Reg 3 */
-#define IMR4	0x18	/* Interrupt Mask Reg 4 */
-#define IMR5	0x19	/* Interrupt Mask Reg 5 */
-#define FMR0	0x1A	/* Framer Mode Reigster 0 */
-#define FMR1	0x1B	/* Framer Mode Reigster 1 */
-#define FMR2	0x1C	/* Framer Mode Reigster 2 */
-#define LOOP	0x1D	/* Channel Loop Back */
-#define XSW	0x1E	/* Transmit Service Word */
-#define FMR4	0x1E	/* Framer Mode Reg 4 */
-#define XSP	0x1F	/* Transmit Spare Bits */
-#define FMR5	0x1F	/* Framer Mode Reg 5 */
-#define XC0	0x20	/* Transmit Control 0 */
-#define XC1	0x21	/* Transmit Control 1 */
-#define RC0	0x22	/* Receive Control 0 */
-#define RC1	0x23	/* Receive Control 1 */
-#define XPM0	0x24	/* Transmit Pulse Mask 0 */
-#define XPM1	0x25	/* Transmit Pulse Mask 1 */
-#define XPM2	0x26	/* Transmit Pulse Mask 2 */
-#define TSWM	0x27	/* Transparent Service Word Mask */
-#define TEST1	0x28	/* Manuf. Test Reg 1 */
-#define IDLE	0x29	/* Idle Channel Code */
-#define XSA4    0x2A	/* Transmit SA4 Bit Reg */
-#define XDL1	0x2A	/* Transmit DL-Bit Reg 2 */
-#define XSA5    0x2B	/* Transmit SA4 Bit Reg */
-#define XDL2	0x2B	/* Transmit DL-Bit Reg 2 */
-#define XSA6    0x2C	/* Transmit SA4 Bit Reg */
-#define XDL3	0x2C	/* Transmit DL-Bit Reg 2 */
-#define XSA7    0x2D	/* Transmit SA4 Bit Reg */
-#define CCB1	0x2D	/* Clear Channel Reg 1 */
-#define XSA8    0x2E	/* Transmit SA4 Bit Reg */
-#define CCB2	0x2E	/* Clear Channel Reg 2 */
-#define FMR3	0x2F	/* Framer Mode Reg. 3 */
-#define CCB3	0x2F	/* Clear Channel Reg 3 */
-#define ICB1	0x30	/* Idle Channel Reg 1 */
-#define ICB2	0x31	/* Idle Channel Reg 2 */
-#define ICB3	0x32	/* Idle Channel Reg 3 */
-#define ICB4	0x33	/* Idle Channel Reg 4 */
-#define LIM0	0x34	/* Line Interface Mode 0 */
-#define LIM1	0x35	/* Line Interface Mode 1 */
-#define PCDR	0x36	/* Pulse Count Detection */
-#define PCRR	0x37	/* Pulse Count Recovery */
-#define LIM2	0x38	/* Line Interface Mode Reg 2 */
-#define LCR1	0x39	/* Loop Code Reg 1 */
-#define LCR2	0x3A	/* Loop Code Reg 2 */
-#define LCR3	0x3B	/* Loop Code Reg 3 */
-#define SIC1	0x3C	/* System Interface Control 1 */
-
-/* Read-only Registers (E1/T1 control mode read registers) */
-#define RFIFOH	0x00		/* Receive FIFO */
-#define RFIFOL	0x01		/* Receive FIFO */
-#define FRS0	0x4C		/* Framer Receive Status 0 */
-#define FRS1	0x4D		/* Framer Receive Status 1 */
-#define RSW	0x4E		/* Receive Service Word */
-#define FRS2	0x4E		/* Framer Receive Status 2 */
-#define RSP	0x4F		/* Receive Spare Bits */
-#define FRS3	0x4F		/* Framer Receive Status 3 */
-#define FECL	0x50		/* Framing Error Counter */
-#define FECH	0x51		/* Framing Error Counter */
-#define CVCL	0x52		/* Code Violation Counter */
-#define CVCH	0x53		/* Code Violation Counter */
-#define CECL	0x54		/* CRC Error Counter 1 */
-#define CECH	0x55		/* CRC Error Counter 1 */
-#define EBCL	0x56		/* E-Bit Error Counter */
-#define EBCH	0x57		/* E-Bit Error Counter */
-#define BECL	0x58		/* Bit Error Counter Low */
-#define BECH	0x59		/* Bit Error Counter Low */
-#define CEC3	0x5A		/* CRC Error Counter 3 (16-bit) */
-#define RSA4	0x5C		/* Receive SA4 Bit Reg */
-#define RDL1	0x5C		/* Receive DL-Bit Reg 1 */
-#define RSA5	0x5D		/* Receive SA5 Bit Reg */
-#define RDL2	0x5D		/* Receive DL-Bit Reg 2 */
-#define RSA6	0x5E		/* Receive SA6 Bit Reg */
-#define RDL3	0x5E		/* Receive DL-Bit Reg 3 */
-#define RSA7	0x5F		/* Receive SA7 Bit Reg */
-#define RSA8	0x60		/* Receive SA8 Bit Reg */
-#define RSA6S	0x61		/* Receive SA6 Bit Status Reg */
-#define TSR0	0x62		/* Manuf. Test Reg 0 */
-#define TSR1	0x63		/* Manuf. Test Reg 1 */
-#define SIS	0x64		/* Signaling Status Reg */
-#define RSIS	0x65		/* Receive Signaling Status Reg */
-#define RBCL	0x66		/* Receive Byte Control */
-#define RBCH	0x67		/* Receive Byte Control */
-#define FISR0	0x68		/* Interrupt Status Reg 0 */
-#define FISR1	0x69		/* Interrupt Status Reg 1 */
-#define FISR2	0x6A		/* Interrupt Status Reg 2 */
-#define FISR3	0x6B		/* Interrupt Status Reg 3 */
-#define GIS	0x6E		/* Global Interrupt Status */
-#define VSTR	0x6F		/* Version Status */
-#define RS(nbr)	(0x70 + (nbr))	/* Rx CAS Reg (0 to 15) */
-
-#endif	/* _FALC_LH_H */
-
diff --git a/drivers/staging/net/pc300.h b/drivers/staging/net/pc300.h
deleted file mode 100644
index 2e4f84f6cad..00000000000
--- a/drivers/staging/net/pc300.h
+++ /dev/null
@@ -1,436 +0,0 @@
-/*
- * pc300.h	Cyclades-PC300(tm) Kernel API Definitions.
- *
- * Author:	Ivan Passos <ivan@cyclades.com>
- *
- * Copyright:	(c) 1999-2002 Cyclades Corp.
- *
- *	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.
- *
- * $Log: pc300.h,v $
- * Revision 3.12  2002/03/07 14:17:09  henrique
- * License data fixed
- *
- * Revision 3.11  2002/01/28 21:09:39  daniela
- * Included ';' after pc300hw.bus.
- *
- * Revision 3.10  2002/01/17 17:58:52  ivan
- * Support for PC300-TE/M (PMC).
- *
- * Revision 3.9  2001/09/28 13:30:53  daniela
- * Renamed dma_start routine to rx_dma_start.
- *
- * Revision 3.8  2001/09/24 13:03:45  daniela
- * Fixed BOF interrupt treatment. Created dma_start routine.
- *
- * Revision 3.7  2001/08/10 17:19:58  daniela
- * Fixed IOCTLs defines.
- *
- * Revision 3.6  2001/07/18 19:24:42  daniela
- * Included kernel version.
- *
- * Revision 3.5  2001/07/05 18:38:08  daniela
- * DMA transmission bug fix.
- *
- * Revision 3.4  2001/06/26 17:10:40  daniela
- * New configuration parameters (line code, CRC calculation and clock).
- *
- * Revision 3.3  2001/06/22 13:13:02  regina
- * MLPPP implementation
- *
- * Revision 3.2  2001/06/18 17:56:09  daniela
- * Increased DEF_MTU and TX_QUEUE_LEN.
- *
- * Revision 3.1  2001/06/15 12:41:10  regina
- * upping major version number
- *
- * Revision 1.1.1.1  2001/06/13 20:25:06  daniela
- * PC300 initial CVS version (3.4.0-pre1)
- *
- * Revision 2.3 2001/03/05 daniela
- * Created struct pc300conf, to provide the hardware information to pc300util.
- * Inclusion of 'alloc_ramsize' field on structure 'pc300hw'.
- * 
- * Revision 2.2 2000/12/22 daniela
- * Structures and defines to support pc300util: statistics, status, 
- * loopback tests, trace.
- * 
- * Revision 2.1 2000/09/28 ivan
- * Inclusion of 'iophys' and 'iosize' fields on structure 'pc300hw', to 
- * allow release of I/O region at module unload.
- * Changed location of include files.
- *
- * Revision 2.0 2000/03/27 ivan
- * Added support for the PC300/TE cards.
- *
- * Revision 1.1 2000/01/31 ivan
- * Replaced 'pc300[drv|sca].h' former PC300 driver include files.
- *
- * Revision 1.0 1999/12/16 ivan
- * First official release.
- * Inclusion of 'nchan' field on structure 'pc300hw', to allow variable 
- * number of ports per card.
- * Inclusion of 'if_ptr' field on structure 'pc300dev'.
- *
- * Revision 0.6 1999/11/17 ivan
- * Changed X.25-specific function names to comply with adopted convention.
- *
- * Revision 0.5 1999/11/16 Daniela Squassoni
- * X.25 support.
- *
- * Revision 0.4 1999/11/15 ivan
- * Inclusion of 'clock' field on structure 'pc300hw'.
- *
- * Revision 0.3 1999/11/10 ivan
- * IOCTL name changing.
- * Inclusion of driver function prototypes.
- *
- * Revision 0.2 1999/11/03 ivan
- * Inclusion of 'tx_skb' and union 'ifu' on structure 'pc300dev'.
- *
- * Revision 0.1 1999/01/15 ivan
- * Initial version.
- *
- */
-
-#ifndef	_PC300_H
-#define	_PC300_H
-
-#include <linux/hdlc.h>
-#include "hd64572.h"
-#include "pc300-falc-lh.h"
-
-#define PC300_PROTO_MLPPP 1
-
-#define	PC300_MAXCHAN	2	/* Number of channels per card */
-
-#define	PC300_RAMSIZE	0x40000 /* RAM window size (256Kb) */
-#define	PC300_FALCSIZE	0x400	/* FALC window size (1Kb) */
-
-#define PC300_OSC_CLOCK	24576000
-#define PC300_PCI_CLOCK	33000000
-
-#define BD_DEF_LEN	0x0800	/* DMA buffer length (2KB) */
-#define DMA_TX_MEMSZ	0x8000	/* Total DMA Tx memory size (32KB/ch) */
-#define DMA_RX_MEMSZ	0x10000	/* Total DMA Rx memory size (64KB/ch) */
-
-#define N_DMA_TX_BUF	(DMA_TX_MEMSZ / BD_DEF_LEN)	/* DMA Tx buffers */
-#define N_DMA_RX_BUF	(DMA_RX_MEMSZ / BD_DEF_LEN)	/* DMA Rx buffers */
-
-/* DMA Buffer Offsets */
-#define DMA_TX_BASE	((N_DMA_TX_BUF + N_DMA_RX_BUF) *	\
-			 PC300_MAXCHAN * sizeof(pcsca_bd_t))
-#define DMA_RX_BASE	(DMA_TX_BASE + PC300_MAXCHAN*DMA_TX_MEMSZ)
-
-/* DMA Descriptor Offsets */
-#define DMA_TX_BD_BASE	0x0000
-#define DMA_RX_BD_BASE	(DMA_TX_BD_BASE + ((PC300_MAXCHAN*DMA_TX_MEMSZ / \
-				BD_DEF_LEN) * sizeof(pcsca_bd_t)))
-
-/* DMA Descriptor Macros */
-#define TX_BD_ADDR(chan, n)	(DMA_TX_BD_BASE + \
-				 ((N_DMA_TX_BUF*chan) + n) * sizeof(pcsca_bd_t))
-#define RX_BD_ADDR(chan, n)	(DMA_RX_BD_BASE + \
-				 ((N_DMA_RX_BUF*chan) + n) * sizeof(pcsca_bd_t))
-
-/* Macro to access the FALC registers (TE only) */
-#define F_REG(reg, chan)	(0x200*(chan) + ((reg)<<2))
-
-/***************************************
- * Memory access functions/macros      *
- * (required to support Alpha systems) *
- ***************************************/
-#define cpc_writeb(port,val)	{writeb((u8)(val),(port)); mb();}
-#define cpc_writew(port,val)	{writew((ushort)(val),(port)); mb();}
-#define cpc_writel(port,val)	{writel((u32)(val),(port)); mb();}
-
-#define cpc_readb(port)		readb(port)
-#define cpc_readw(port)		readw(port)
-#define cpc_readl(port)		readl(port)
-
-/****** Data Structures *****************************************************/
-
-/*
- *      RUNTIME_9050 - PLX PCI9050-1 local configuration and shared runtime
- *      registers. This structure can be used to access the 9050 registers
- *      (memory mapped).
- */
-struct RUNTIME_9050 {
-	u32 loc_addr_range[4];	/* 00-0Ch : Local Address Ranges */
-	u32 loc_rom_range;	/* 10h : Local ROM Range */
-	u32 loc_addr_base[4];	/* 14-20h : Local Address Base Addrs */
-	u32 loc_rom_base;	/* 24h : Local ROM Base */
-	u32 loc_bus_descr[4];	/* 28-34h : Local Bus Descriptors */
-	u32 rom_bus_descr;	/* 38h : ROM Bus Descriptor */
-	u32 cs_base[4];		/* 3C-48h : Chip Select Base Addrs */
-	u32 intr_ctrl_stat;	/* 4Ch : Interrupt Control/Status */
-	u32 init_ctrl;		/* 50h : EEPROM ctrl, Init Ctrl, etc */
-};
-
-#define PLX_9050_LINT1_ENABLE	0x01
-#define PLX_9050_LINT1_POL	0x02
-#define PLX_9050_LINT1_STATUS	0x04
-#define PLX_9050_LINT2_ENABLE	0x08
-#define PLX_9050_LINT2_POL	0x10
-#define PLX_9050_LINT2_STATUS	0x20
-#define PLX_9050_INTR_ENABLE	0x40
-#define PLX_9050_SW_INTR	0x80
-
-/* Masks to access the init_ctrl PLX register */
-#define	PC300_CLKSEL_MASK		(0x00000004UL)
-#define	PC300_CHMEDIA_MASK(chan)	(0x00000020UL<<(chan*3))
-#define	PC300_CTYPE_MASK		(0x00000800UL)
-
-/* CPLD Registers (base addr = falcbase, TE only) */
-/* CPLD v. 0 */
-#define CPLD_REG1	0x140	/* Chip resets, DCD/CTS status */
-#define CPLD_REG2	0x144	/* Clock enable , LED control */
-/* CPLD v. 2 or higher */
-#define CPLD_V2_REG1	0x100	/* Chip resets, DCD/CTS status */
-#define CPLD_V2_REG2	0x104	/* Clock enable , LED control */
-#define CPLD_ID_REG	0x108	/* CPLD version */
-
-/* CPLD Register bit description: for the FALC bits, they should always be 
-   set based on the channel (use (bit<<(2*ch)) to access the correct bit for 
-   that channel) */
-#define CPLD_REG1_FALC_RESET	0x01
-#define CPLD_REG1_SCA_RESET	0x02
-#define CPLD_REG1_GLOBAL_CLK	0x08
-#define CPLD_REG1_FALC_DCD	0x10
-#define CPLD_REG1_FALC_CTS	0x20
-
-#define CPLD_REG2_FALC_TX_CLK	0x01
-#define CPLD_REG2_FALC_RX_CLK	0x02
-#define CPLD_REG2_FALC_LED1	0x10
-#define CPLD_REG2_FALC_LED2	0x20
-
-/* Structure with FALC-related fields (TE only) */
-#define PC300_FALC_MAXLOOP	0x0000ffff	/* for falc_issue_cmd() */
-
-typedef struct falc {
-	u8 sync;	/* If true FALC is synchronized */
-	u8 active;	/* if TRUE then already active */
-	u8 loop_active;	/* if TRUE a line loopback UP was received */
-	u8 loop_gen;	/* if TRUE a line loopback UP was issued */
-
-	u8 num_channels;
-	u8 offset;	/* 1 for T1, 0 for E1 */
-	u8 full_bandwidth;
-
-	u8 xmb_cause;
-	u8 multiframe_mode;
-
-	/* Statistics */
-	u16 pden;	/* Pulse Density violation count */
-	u16 los;	/* Loss of Signal count */
-	u16 losr;	/* Loss of Signal recovery count */
-	u16 lfa;	/* Loss of frame alignment count */
-	u16 farec;	/* Frame Alignment Recovery count */
-	u16 lmfa;	/* Loss of multiframe alignment count */
-	u16 ais;	/* Remote Alarm indication Signal count */
-	u16 sec;	/* One-second timer */
-	u16 es;		/* Errored second */
-	u16 rai;	/* remote alarm received */
-	u16 bec;
-	u16 fec;
-	u16 cvc;
-	u16 cec;
-	u16 ebc;
-
-	/* Status */
-	u8 red_alarm;
-	u8 blue_alarm;
-	u8 loss_fa;
-	u8 yellow_alarm;
-	u8 loss_mfa;
-	u8 prbs;
-} falc_t;
-
-typedef struct falc_status {
-	u8 sync;	/* If true FALC is synchronized */
-	u8 red_alarm;
-	u8 blue_alarm;
-	u8 loss_fa;
-	u8 yellow_alarm;
-	u8 loss_mfa;
-	u8 prbs;
-} falc_status_t;
-
-typedef struct rsv_x21_status {
-	u8 dcd;
-	u8 dsr;
-	u8 cts;
-	u8 rts;
-	u8 dtr;
-} rsv_x21_status_t;
-
-typedef struct pc300stats {
-	int hw_type;
-	u32 line_on;
-	u32 line_off;
-	struct net_device_stats gen_stats;
-	falc_t te_stats;
-} pc300stats_t;
-
-typedef struct pc300status {
-	int hw_type;
-	rsv_x21_status_t gen_status;
-	falc_status_t te_status;
-} pc300status_t;
-
-typedef struct pc300loopback {
-	char loop_type;
-	char loop_on;
-} pc300loopback_t;
-
-typedef struct pc300patterntst {
-	char patrntst_on;       /* 0 - off; 1 - on; 2 - read num_errors */
-	u16 num_errors;
-} pc300patterntst_t;
-
-typedef struct pc300dev {
-	struct pc300ch *chan;
-	u8 trace_on;
-	u32 line_on;		/* DCD(X.21, RSV) / sync(TE) change counters */
-	u32 line_off;
-	char name[16];
-	struct net_device *dev;
-#ifdef CONFIG_PC300_MLPPP
-	void *cpc_tty;	/* information to PC300 TTY driver */
-#endif
-}pc300dev_t;
-
-typedef struct pc300hw {
-	int type;		/* RSV, X21, etc. */
-	int bus;		/* Bus (PCI, PMC, etc.) */
-	int nchan;		/* number of channels */
-	int irq;		/* interrupt request level */
-	u32 clock;		/* Board clock */
-	u8 cpld_id;		/* CPLD ID (TE only) */
-	u16 cpld_reg1;		/* CPLD reg 1 (TE only) */
-	u16 cpld_reg2;		/* CPLD reg 2 (TE only) */
-	u16 gpioc_reg;		/* PLX GPIOC reg */
-	u16 intctl_reg;		/* PLX Int Ctrl/Status reg */
-	u32 iophys;		/* PLX registers I/O base */
-	u32 iosize;		/* PLX registers I/O size */
-	u32 plxphys;		/* PLX registers MMIO base (physical) */
-	void __iomem * plxbase;	/* PLX registers MMIO base (virtual) */
-	u32 plxsize;		/* PLX registers MMIO size */
-	u32 scaphys;		/* SCA registers MMIO base (physical) */
-	void __iomem * scabase;	/* SCA registers MMIO base (virtual) */
-	u32 scasize;		/* SCA registers MMIO size */
-	u32 ramphys;		/* On-board RAM MMIO base (physical) */
-	void __iomem * rambase;	/* On-board RAM MMIO base (virtual) */
-	u32 alloc_ramsize;	/* RAM MMIO size allocated by the PCI bridge */
-	u32 ramsize;		/* On-board RAM MMIO size */
-	u32 falcphys;		/* FALC registers MMIO base (physical) */
-	void __iomem * falcbase;/* FALC registers MMIO base (virtual) */
-	u32 falcsize;		/* FALC registers MMIO size */
-} pc300hw_t;
-
-typedef struct pc300chconf {
-	sync_serial_settings	phys_settings;	/* Clock type/rate (in bps),
-						   loopback mode */
-	raw_hdlc_proto		proto_settings;	/* Encoding, parity (CRC) */
-	u32 media;		/* HW media (RS232, V.35, etc.) */
-	u32 proto;		/* Protocol (PPP, X.25, etc.) */
-
-	/* TE-specific parameters */
-	u8 lcode;		/* Line Code (AMI, B8ZS, etc.) */
-	u8 fr_mode;		/* Frame Mode (ESF, D4, etc.) */
-	u8 lbo;			/* Line Build Out */
-	u8 rx_sens;		/* Rx Sensitivity (long- or short-haul) */
-	u32 tslot_bitmap;	/* bit[i]=1  =>  timeslot _i_ is active */
-} pc300chconf_t;
-
-typedef struct pc300ch {
-	struct pc300 *card;
-	int channel;
-	pc300dev_t d;
-	pc300chconf_t conf;
-	u8 tx_first_bd;	/* First TX DMA block descr. w/ data */
-	u8 tx_next_bd;	/* Next free TX DMA block descriptor */
-	u8 rx_first_bd;	/* First free RX DMA block descriptor */
-	u8 rx_last_bd;	/* Last free RX DMA block descriptor */
-	u8 nfree_tx_bd;	/* Number of free TX DMA block descriptors */
-	falc_t falc;	/* FALC structure (TE only) */
-} pc300ch_t;
-
-typedef struct pc300 {
-	pc300hw_t hw;			/* hardware config. */
-	pc300ch_t chan[PC300_MAXCHAN];
-	spinlock_t card_lock;
-} pc300_t;
-
-typedef struct pc300conf {
-	pc300hw_t hw;
-	pc300chconf_t conf;
-} pc300conf_t;
-
-/* DEV ioctl() commands */
-#define	N_SPPP_IOCTLS	2
-
-enum pc300_ioctl_cmds {
-	SIOCCPCRESERVED = (SIOCDEVPRIVATE + N_SPPP_IOCTLS),
-	SIOCGPC300CONF,
-	SIOCSPC300CONF,
-	SIOCGPC300STATUS,
-	SIOCGPC300FALCSTATUS,
-	SIOCGPC300UTILSTATS,
-	SIOCGPC300UTILSTATUS,
-	SIOCSPC300TRACE,
-	SIOCSPC300LOOPBACK,
-	SIOCSPC300PATTERNTEST,
-};
-
-/* Loopback types - PC300/TE boards */
-enum pc300_loopback_cmds {
-	PC300LOCLOOP = 1,
-	PC300REMLOOP,
-	PC300PAYLOADLOOP,
-	PC300GENLOOPUP,
-	PC300GENLOOPDOWN,
-};
-
-/* Control Constant Definitions */
-#define	PC300_RSV	0x01
-#define	PC300_X21	0x02
-#define	PC300_TE	0x03
-
-#define	PC300_PCI	0x00
-#define	PC300_PMC	0x01
-
-#define PC300_LC_AMI	0x01
-#define PC300_LC_B8ZS	0x02
-#define PC300_LC_NRZ	0x03
-#define PC300_LC_HDB3	0x04
-
-/* Framing (T1) */
-#define PC300_FR_ESF		0x01
-#define PC300_FR_D4		0x02
-#define PC300_FR_ESF_JAPAN	0x03
-
-/* Framing (E1) */
-#define PC300_FR_MF_CRC4	0x04
-#define PC300_FR_MF_NON_CRC4	0x05
-#define PC300_FR_UNFRAMED	0x06
-
-#define PC300_LBO_0_DB		0x00
-#define PC300_LBO_7_5_DB	0x01
-#define PC300_LBO_15_DB		0x02
-#define PC300_LBO_22_5_DB	0x03
-
-#define PC300_RX_SENS_SH	0x01
-#define PC300_RX_SENS_LH	0x02
-
-#define PC300_TX_TIMEOUT	(2*HZ)
-#define PC300_TX_QUEUE_LEN	100
-#define	PC300_DEF_MTU		1600
-
-/* Function Prototypes */
-int cpc_open(struct net_device *dev);
-
-#endif	/* _PC300_H */
diff --git a/drivers/staging/net/pc300_drv.c b/drivers/staging/net/pc300_drv.c
deleted file mode 100644
index 7281797ffe9..00000000000
--- a/drivers/staging/net/pc300_drv.c
+++ /dev/null
@@ -1,3670 +0,0 @@
-#define	USE_PCI_CLOCK
-static const char rcsid[] =
-"Revision: 3.4.5 Date: 2002/03/07 ";
-
-/*
- * pc300.c	Cyclades-PC300(tm) Driver.
- *
- * Author:	Ivan Passos <ivan@cyclades.com>
- * Maintainer:	PC300 Maintainer <pc300@cyclades.com>
- *
- * Copyright:	(c) 1999-2003 Cyclades Corp.
- *
- *	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.
- *	
- *	Using tabstop = 4.
- * 
- * $Log: pc300_drv.c,v $
- * Revision 3.23  2002/03/20 13:58:40  henrique
- * Fixed ortographic mistakes
- *
- * Revision 3.22  2002/03/13 16:56:56  henrique
- * Take out the debug messages
- *
- * Revision 3.21  2002/03/07 14:17:09  henrique
- * License data fixed
- *
- * Revision 3.20  2002/01/17 17:58:52  ivan
- * Support for PC300-TE/M (PMC).
- *
- * Revision 3.19  2002/01/03 17:08:47  daniela
- * Enables DMA reception when the SCA-II disables it improperly.
- *
- * Revision 3.18  2001/12/03 18:47:50  daniela
- * Esthetic changes.
- *
- * Revision 3.17  2001/10/19 16:50:13  henrique
- * Patch to kernel 2.4.12 and new generic hdlc.
- *
- * Revision 3.16  2001/10/16 15:12:31  regina
- * clear statistics
- *
- * Revision 3.11 to 3.15  2001/10/11 20:26:04  daniela
- * More DMA fixes for noisy lines.
- * Return the size of bad frames in dma_get_rx_frame_size, so that the Rx buffer
- * descriptors can be cleaned by dma_buf_read (called in cpc_net_rx).
- * Renamed dma_start routine to rx_dma_start. Improved Rx statistics.
- * Fixed BOF interrupt treatment. Created dma_start routine.
- * Changed min and max to cpc_min and cpc_max.
- *
- * Revision 3.10  2001/08/06 12:01:51  regina
- * Fixed problem in DSR_DE bit.
- *
- * Revision 3.9  2001/07/18 19:27:26  daniela
- * Added some history comments.
- *
- * Revision 3.8  2001/07/12 13:11:19  regina
- * bug fix - DCD-OFF in pc300 tty driver
- *
- * Revision 3.3 to 3.7  2001/07/06 15:00:20  daniela
- * Removing kernel 2.4.3 and previous support.
- * DMA transmission bug fix.
- * MTU check in cpc_net_rx fixed.
- * Boot messages reviewed.
- * New configuration parameters (line code, CRC calculation and clock).
- *
- * Revision 3.2 2001/06/22 13:13:02  regina
- * MLPPP implementation. Changed the header of message trace to include
- * the device name. New format : "hdlcX[R/T]: ".
- * Default configuration changed.
- *
- * Revision 3.1 2001/06/15 regina
- * in cpc_queue_xmit, netif_stop_queue is called if don't have free descriptor
- * upping major version number
- *
- * Revision 1.1.1.1  2001/06/13 20:25:04  daniela
- * PC300 initial CVS version (3.4.0-pre1)
- *
- * Revision 3.0.1.2 2001/06/08 daniela
- * Did some changes in the DMA programming implementation to avoid the 
- * occurrence of a SCA-II bug when CDA is accessed during a DMA transfer.
- *
- * Revision 3.0.1.1 2001/05/02 daniela
- * Added kernel 2.4.3 support.
- * 
- * Revision 3.0.1.0 2001/03/13 daniela, henrique
- * Added Frame Relay Support.
- * Driver now uses HDLC generic driver to provide protocol support.
- * 
- * Revision 3.0.0.8 2001/03/02 daniela
- * Fixed ram size detection. 
- * Changed SIOCGPC300CONF ioctl, to give hw information to pc300util.
- * 
- * Revision 3.0.0.7 2001/02/23 daniela
- * netif_stop_queue called before the SCA-II transmition commands in 
- * cpc_queue_xmit, and with interrupts disabled to avoid race conditions with 
- * transmition interrupts.
- * Fixed falc_check_status for Unframed E1.
- * 
- * Revision 3.0.0.6 2000/12/13 daniela
- * Implemented pc300util support: trace, statistics, status and loopback
- * tests for the PC300 TE boards.
- * 
- * Revision 3.0.0.5 2000/12/12 ivan
- * Added support for Unframed E1.
- * Implemented monitor mode.
- * Fixed DCD sensitivity on the second channel.
- * Driver now complies with new PCI kernel architecture.
- *
- * Revision 3.0.0.4 2000/09/28 ivan
- * Implemented DCD sensitivity.
- * Moved hardware-specific open to the end of cpc_open, to avoid race
- * conditions with early reception interrupts.
- * Included code for [request|release]_mem_region().
- * Changed location of pc300.h .
- * Minor code revision (contrib. of Jeff Garzik).
- *
- * Revision 3.0.0.3 2000/07/03 ivan
- * Previous bugfix for the framing errors with external clock made X21
- * boards stop working. This version fixes it.
- *
- * Revision 3.0.0.2 2000/06/23 ivan
- * Revisited cpc_queue_xmit to prevent race conditions on Tx DMA buffer
- * handling when Tx timeouts occur.
- * Revisited Rx statistics.
- * Fixed a bug in the SCA-II programming that would cause framing errors
- * when external clock was configured.
- *
- * Revision 3.0.0.1 2000/05/26 ivan
- * Added logic in the SCA interrupt handler so that no board can monopolize
- * the driver.
- * Request PLX I/O region, although driver doesn't use it, to avoid
- * problems with other drivers accessing it.
- *
- * Revision 3.0.0.0 2000/05/15 ivan
- * Did some changes in the DMA programming implementation to avoid the
- * occurrence of a SCA-II bug in the second channel.
- * Implemented workaround for PLX9050 bug that would cause a system lockup
- * in certain systems, depending on the MMIO addresses allocated to the
- * board.
- * Fixed the FALC chip programming to avoid synchronization problems in the
- * second channel (TE only).
- * Implemented a cleaner and faster Tx DMA descriptor cleanup procedure in
- * cpc_queue_xmit().
- * Changed the built-in driver implementation so that the driver can use the
- * general 'hdlcN' naming convention instead of proprietary device names.
- * Driver load messages are now device-centric, instead of board-centric.
- * Dynamic allocation of net_device structures.
- * Code is now compliant with the new module interface (module_[init|exit]).
- * Make use of the PCI helper functions to access PCI resources.
- *
- * Revision 2.0.0.0 2000/04/15 ivan
- * Added support for the PC300/TE boards (T1/FT1/E1/FE1).
- *
- * Revision 1.1.0.0 2000/02/28 ivan
- * Major changes in the driver architecture.
- * Softnet compliancy implemented.
- * Driver now reports physical instead of virtual memory addresses.
- * Added cpc_change_mtu function.
- *
- * Revision 1.0.0.0 1999/12/16 ivan
- * First official release.
- * Support for 1- and 2-channel boards (which use distinct PCI Device ID's).
- * Support for monolythic installation (i.e., drv built into the kernel).
- * X.25 additional checking when lapb_[dis]connect_request returns an error.
- * SCA programming now covers X.21 as well.
- *
- * Revision 0.3.1.0 1999/11/18 ivan
- * Made X.25 support configuration-dependent (as it depends on external 
- * modules to work).
- * Changed X.25-specific function names to comply with adopted convention.
- * Fixed typos in X.25 functions that would cause compile errors (Daniela).
- * Fixed bug in ch_config that would disable interrupts on a previously 
- * enabled channel if the other channel on the same board was enabled later.
- *
- * Revision 0.3.0.0 1999/11/16 daniela
- * X.25 support.
- *
- * Revision 0.2.3.0 1999/11/15 ivan
- * Function cpc_ch_status now provides more detailed information.
- * Added support for X.21 clock configuration.
- * Changed TNR1 setting in order to prevent Tx FIFO overaccesses by the SCA.
- * Now using PCI clock instead of internal oscillator clock for the SCA.
- *
- * Revision 0.2.2.0 1999/11/10 ivan
- * Changed the *_dma_buf_check functions so that they would print only 
- * the useful info instead of the whole buffer descriptor bank.
- * Fixed bug in cpc_queue_xmit that would eventually crash the system 
- * in case of a packet drop.
- * Implemented TX underrun handling.
- * Improved SCA fine tuning to boost up its performance.
- *
- * Revision 0.2.1.0 1999/11/03 ivan
- * Added functions *dma_buf_pt_init to allow independent initialization 
- * of the next-descr. and DMA buffer pointers on the DMA descriptors.
- * Kernel buffer release and tbusy clearing is now done in the interrupt 
- * handler.
- * Fixed bug in cpc_open that would cause an interface reopen to fail.
- * Added a protocol-specific code section in cpc_net_rx.
- * Removed printk level defs (they might be added back after the beta phase).
- *
- * Revision 0.2.0.0 1999/10/28 ivan
- * Revisited the code so that new protocols can be easily added / supported. 
- *
- * Revision 0.1.0.1 1999/10/20 ivan
- * Mostly "esthetic" changes.
- *
- * Revision 0.1.0.0 1999/10/11 ivan
- * Initial version.
- *
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/mm.h>
-#include <linux/ioport.h>
-#include <linux/pci.h>
-#include <linux/errno.h>
-#include <linux/string.h>
-#include <linux/init.h>
-#include <linux/delay.h>
-#include <linux/net.h>
-#include <linux/skbuff.h>
-#include <linux/if_arp.h>
-#include <linux/netdevice.h>
-#include <linux/etherdevice.h>
-#include <linux/spinlock.h>
-#include <linux/if.h>
-#include <linux/slab.h>
-#include <net/arp.h>
-
-#include <asm/io.h>
-#include <asm/uaccess.h>
-
-#include "pc300.h"
-
-#define	CPC_LOCK(card,flags)		\
-		do {						\
-		spin_lock_irqsave(&card->card_lock, flags);	\
-		} while (0)
-
-#define CPC_UNLOCK(card,flags)			\
-		do {							\
-		spin_unlock_irqrestore(&card->card_lock, flags);	\
-		} while (0)
-
-#undef	PC300_DEBUG_PCI
-#undef	PC300_DEBUG_INTR
-#undef	PC300_DEBUG_TX
-#undef	PC300_DEBUG_RX
-#undef	PC300_DEBUG_OTHER
-
-static DEFINE_PCI_DEVICE_TABLE(cpc_pci_dev_id) = {
-	/* PC300/RSV or PC300/X21, 2 chan */
-	{0x120e, 0x300, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0x300},
-	/* PC300/RSV or PC300/X21, 1 chan */
-	{0x120e, 0x301, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0x301},
-	/* PC300/TE, 2 chan */
-	{0x120e, 0x310, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0x310},
-	/* PC300/TE, 1 chan */
-	{0x120e, 0x311, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0x311},
-	/* PC300/TE-M, 2 chan */
-	{0x120e, 0x320, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0x320},
-	/* PC300/TE-M, 1 chan */
-	{0x120e, 0x321, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0x321},
-	/* End of table */
-	{0,},
-};
-MODULE_DEVICE_TABLE(pci, cpc_pci_dev_id);
-
-#ifndef cpc_min
-#define	cpc_min(a,b)	(((a)<(b))?(a):(b))
-#endif
-#ifndef cpc_max
-#define	cpc_max(a,b)	(((a)>(b))?(a):(b))
-#endif
-
-/* prototypes */
-static void tx_dma_buf_pt_init(pc300_t *, int);
-static void tx_dma_buf_init(pc300_t *, int);
-static void rx_dma_buf_pt_init(pc300_t *, int);
-static void rx_dma_buf_init(pc300_t *, int);
-static void tx_dma_buf_check(pc300_t *, int);
-static void rx_dma_buf_check(pc300_t *, int);
-static irqreturn_t cpc_intr(int, void *);
-static int clock_rate_calc(u32, u32, int *);
-static u32 detect_ram(pc300_t *);
-static void plx_init(pc300_t *);
-static void cpc_trace(struct net_device *, struct sk_buff *, char);
-static int cpc_attach(struct net_device *, unsigned short, unsigned short);
-static int cpc_close(struct net_device *dev);
-
-#ifdef CONFIG_PC300_MLPPP
-void cpc_tty_init(pc300dev_t * dev);
-void cpc_tty_unregister_service(pc300dev_t * pc300dev);
-void cpc_tty_receive(pc300dev_t * pc300dev);
-void cpc_tty_trigger_poll(pc300dev_t * pc300dev);
-#endif
-
-/************************/
-/***   DMA Routines   ***/
-/************************/
-static void tx_dma_buf_pt_init(pc300_t * card, int ch)
-{
-	int i;
-	int ch_factor = ch * N_DMA_TX_BUF;
-	volatile pcsca_bd_t __iomem *ptdescr = (card->hw.rambase
-			               + DMA_TX_BD_BASE + ch_factor * sizeof(pcsca_bd_t));
-
-	for (i = 0; i < N_DMA_TX_BUF; i++, ptdescr++) {
-		cpc_writel(&ptdescr->next, (u32)(DMA_TX_BD_BASE +
-			(ch_factor + ((i + 1) & (N_DMA_TX_BUF - 1))) * sizeof(pcsca_bd_t)));
-		cpc_writel(&ptdescr->ptbuf,
-			   (u32)(DMA_TX_BASE + (ch_factor + i) * BD_DEF_LEN));
-	}
-}
-
-static void tx_dma_buf_init(pc300_t * card, int ch)
-{
-	int i;
-	int ch_factor = ch * N_DMA_TX_BUF;
-	volatile pcsca_bd_t __iomem *ptdescr = (card->hw.rambase
-			       + DMA_TX_BD_BASE + ch_factor * sizeof(pcsca_bd_t));
-
-	for (i = 0; i < N_DMA_TX_BUF; i++, ptdescr++) {
-		memset_io(ptdescr, 0, sizeof(pcsca_bd_t));
-		cpc_writew(&ptdescr->len, 0);
-		cpc_writeb(&ptdescr->status, DST_OSB);
-	}
-	tx_dma_buf_pt_init(card, ch);
-}
-
-static void rx_dma_buf_pt_init(pc300_t * card, int ch)
-{
-	int i;
-	int ch_factor = ch * N_DMA_RX_BUF;
-	volatile pcsca_bd_t __iomem *ptdescr = (card->hw.rambase
-				       + DMA_RX_BD_BASE + ch_factor * sizeof(pcsca_bd_t));
-
-	for (i = 0; i < N_DMA_RX_BUF; i++, ptdescr++) {
-		cpc_writel(&ptdescr->next, (u32)(DMA_RX_BD_BASE +
-			(ch_factor + ((i + 1) & (N_DMA_RX_BUF - 1))) * sizeof(pcsca_bd_t)));
-		cpc_writel(&ptdescr->ptbuf,
-			   (u32)(DMA_RX_BASE + (ch_factor + i) * BD_DEF_LEN));
-	}
-}
-
-static void rx_dma_buf_init(pc300_t * card, int ch)
-{
-	int i;
-	int ch_factor = ch * N_DMA_RX_BUF;
-	volatile pcsca_bd_t __iomem *ptdescr = (card->hw.rambase
-				       + DMA_RX_BD_BASE + ch_factor * sizeof(pcsca_bd_t));
-
-	for (i = 0; i < N_DMA_RX_BUF; i++, ptdescr++) {
-		memset_io(ptdescr, 0, sizeof(pcsca_bd_t));
-		cpc_writew(&ptdescr->len, 0);
-		cpc_writeb(&ptdescr->status, 0);
-	}
-	rx_dma_buf_pt_init(card, ch);
-}
-
-static void tx_dma_buf_check(pc300_t * card, int ch)
-{
-	volatile pcsca_bd_t __iomem *ptdescr;
-	int i;
-	u16 first_bd = card->chan[ch].tx_first_bd;
-	u16 next_bd = card->chan[ch].tx_next_bd;
-
-	printk("#CH%d: f_bd = %d(0x%08zx), n_bd = %d(0x%08zx)\n", ch,
-	       first_bd, TX_BD_ADDR(ch, first_bd),
-	       next_bd, TX_BD_ADDR(ch, next_bd));
-	for (i = first_bd,
-	     ptdescr = (card->hw.rambase + TX_BD_ADDR(ch, first_bd));
-	     i != ((next_bd + 1) & (N_DMA_TX_BUF - 1));
-	     i = (i + 1) & (N_DMA_TX_BUF - 1), 
-		 ptdescr = (card->hw.rambase + TX_BD_ADDR(ch, i))) {
-		printk("\n CH%d TX%d: next=0x%x, ptbuf=0x%x, ST=0x%x, len=%d",
-		       ch, i, cpc_readl(&ptdescr->next),
-		       cpc_readl(&ptdescr->ptbuf),
-		       cpc_readb(&ptdescr->status), cpc_readw(&ptdescr->len));
-	}
-	printk("\n");
-}
-
-#ifdef	PC300_DEBUG_OTHER
-/* Show all TX buffer descriptors */
-static void tx1_dma_buf_check(pc300_t * card, int ch)
-{
-	volatile pcsca_bd_t __iomem *ptdescr;
-	int i;
-	u16 first_bd = card->chan[ch].tx_first_bd;
-	u16 next_bd = card->chan[ch].tx_next_bd;
-	u32 scabase = card->hw.scabase;
-
-	printk ("\nnfree_tx_bd = %d\n", card->chan[ch].nfree_tx_bd);
-	printk("#CH%d: f_bd = %d(0x%08x), n_bd = %d(0x%08x)\n", ch,
-	       first_bd, TX_BD_ADDR(ch, first_bd),
-	       next_bd, TX_BD_ADDR(ch, next_bd));
-	printk("TX_CDA=0x%08x, TX_EDA=0x%08x\n",
-	       cpc_readl(scabase + DTX_REG(CDAL, ch)),
-	       cpc_readl(scabase + DTX_REG(EDAL, ch)));
-	for (i = 0; i < N_DMA_TX_BUF; i++) {
-		ptdescr = (card->hw.rambase + TX_BD_ADDR(ch, i));
-		printk("\n CH%d TX%d: next=0x%x, ptbuf=0x%x, ST=0x%x, len=%d",
-		       ch, i, cpc_readl(&ptdescr->next),
-		       cpc_readl(&ptdescr->ptbuf),
-		       cpc_readb(&ptdescr->status), cpc_readw(&ptdescr->len));
-	}
-	printk("\n");
-}
-#endif
-
-static void rx_dma_buf_check(pc300_t * card, int ch)
-{
-	volatile pcsca_bd_t __iomem *ptdescr;
-	int i;
-	u16 first_bd = card->chan[ch].rx_first_bd;
-	u16 last_bd = card->chan[ch].rx_last_bd;
-	int ch_factor;
-
-	ch_factor = ch * N_DMA_RX_BUF;
-	printk("#CH%d: f_bd = %d, l_bd = %d\n", ch, first_bd, last_bd);
-	for (i = 0, ptdescr = (card->hw.rambase +
-					      DMA_RX_BD_BASE + ch_factor * sizeof(pcsca_bd_t));
-	     i < N_DMA_RX_BUF; i++, ptdescr++) {
-		if (cpc_readb(&ptdescr->status) & DST_OSB)
-			printk ("\n CH%d RX%d: next=0x%x, ptbuf=0x%x, ST=0x%x, len=%d",
-				 ch, i, cpc_readl(&ptdescr->next),
-				 cpc_readl(&ptdescr->ptbuf),
-				 cpc_readb(&ptdescr->status),
-				 cpc_readw(&ptdescr->len));
-	}
-	printk("\n");
-}
-
-static int dma_get_rx_frame_size(pc300_t * card, int ch)
-{
-	volatile pcsca_bd_t __iomem *ptdescr;
-	u16 first_bd = card->chan[ch].rx_first_bd;
-	int rcvd = 0;
-	volatile u8 status;
-
-	ptdescr = (card->hw.rambase + RX_BD_ADDR(ch, first_bd));
-	while ((status = cpc_readb(&ptdescr->status)) & DST_OSB) {
-		rcvd += cpc_readw(&ptdescr->len);
-		first_bd = (first_bd + 1) & (N_DMA_RX_BUF - 1);
-		if ((status & DST_EOM) || (first_bd == card->chan[ch].rx_last_bd)) {
-			/* Return the size of a good frame or incomplete bad frame 
-			* (dma_buf_read will clean the buffer descriptors in this case). */
-			return rcvd;
-		}
-		ptdescr = (card->hw.rambase + cpc_readl(&ptdescr->next));
-	}
-	return -1;
-}
-
-/*
- * dma_buf_write: writes a frame to the Tx DMA buffers
- * NOTE: this function writes one frame at a time.
- */
-static int dma_buf_write(pc300_t *card, int ch, u8 *ptdata, int len)
-{
-	int i, nchar;
-	volatile pcsca_bd_t __iomem *ptdescr;
-	int tosend = len;
-	u8 nbuf = ((len - 1) / BD_DEF_LEN) + 1;
-
-	if (nbuf >= card->chan[ch].nfree_tx_bd) {
-		return -ENOMEM;
-	}
-
-	for (i = 0; i < nbuf; i++) {
-		ptdescr = (card->hw.rambase +
-					  TX_BD_ADDR(ch, card->chan[ch].tx_next_bd));
-		nchar = cpc_min(BD_DEF_LEN, tosend);
-		if (cpc_readb(&ptdescr->status) & DST_OSB) {
-			memcpy_toio((card->hw.rambase + cpc_readl(&ptdescr->ptbuf)),
-				    &ptdata[len - tosend], nchar);
-			cpc_writew(&ptdescr->len, nchar);
-			card->chan[ch].nfree_tx_bd--;
-			if ((i + 1) == nbuf) {
-				/* This must be the last BD to be used */
-				cpc_writeb(&ptdescr->status, DST_EOM);
-			} else {
-				cpc_writeb(&ptdescr->status, 0);
-			}
-		} else {
-			return -ENOMEM;
-		}
-		tosend -= nchar;
-		card->chan[ch].tx_next_bd =
-			(card->chan[ch].tx_next_bd + 1) & (N_DMA_TX_BUF - 1);
-	}
-	/* If it gets to here, it means we have sent the whole frame */
-	return 0;
-}
-
-/*
- * dma_buf_read: reads a frame from the Rx DMA buffers
- * NOTE: this function reads one frame at a time.
- */
-static int dma_buf_read(pc300_t * card, int ch, struct sk_buff *skb)
-{
-	int nchar;
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	volatile pcsca_bd_t __iomem *ptdescr;
-	int rcvd = 0;
-	volatile u8 status;
-
-	ptdescr = (card->hw.rambase +
-				  RX_BD_ADDR(ch, chan->rx_first_bd));
-	while ((status = cpc_readb(&ptdescr->status)) & DST_OSB) {
-		nchar = cpc_readw(&ptdescr->len);
-		if ((status & (DST_OVR | DST_CRC | DST_RBIT | DST_SHRT | DST_ABT)) ||
-		    (nchar > BD_DEF_LEN)) {
-
-			if (nchar > BD_DEF_LEN)
-				status |= DST_RBIT;
-			rcvd = -status;
-			/* Discard remaining descriptors used by the bad frame */
-			while (chan->rx_first_bd != chan->rx_last_bd) {
-				cpc_writeb(&ptdescr->status, 0);
-				chan->rx_first_bd = (chan->rx_first_bd+1) & (N_DMA_RX_BUF-1);
-				if (status & DST_EOM)
-					break;
-				ptdescr = (card->hw.rambase +
-							  cpc_readl(&ptdescr->next));
-				status = cpc_readb(&ptdescr->status);
-			}
-			break;
-		}
-		if (nchar != 0) {
-			if (skb) {
-				memcpy_fromio(skb_put(skb, nchar),
-				 (card->hw.rambase+cpc_readl(&ptdescr->ptbuf)),nchar);
-			}
-			rcvd += nchar;
-		}
-		cpc_writeb(&ptdescr->status, 0);
-		cpc_writeb(&ptdescr->len, 0);
-		chan->rx_first_bd = (chan->rx_first_bd + 1) & (N_DMA_RX_BUF - 1);
-
-		if (status & DST_EOM)
-			break;
-
-		ptdescr = (card->hw.rambase + cpc_readl(&ptdescr->next));
-	}
-
-	if (rcvd != 0) {
-		/* Update pointer */
-		chan->rx_last_bd = (chan->rx_first_bd - 1) & (N_DMA_RX_BUF - 1);
-		/* Update EDA */
-		cpc_writel(card->hw.scabase + DRX_REG(EDAL, ch),
-			   RX_BD_ADDR(ch, chan->rx_last_bd));
-	}
-	return rcvd;
-}
-
-static void tx_dma_stop(pc300_t * card, int ch)
-{
-	void __iomem *scabase = card->hw.scabase;
-	u8 drr_ena_bit = 1 << (5 + 2 * ch);
-	u8 drr_rst_bit = 1 << (1 + 2 * ch);
-
-	/* Disable DMA */
-	cpc_writeb(scabase + DRR, drr_ena_bit);
-	cpc_writeb(scabase + DRR, drr_rst_bit & ~drr_ena_bit);
-}
-
-static void rx_dma_stop(pc300_t * card, int ch)
-{
-	void __iomem *scabase = card->hw.scabase;
-	u8 drr_ena_bit = 1 << (4 + 2 * ch);
-	u8 drr_rst_bit = 1 << (2 * ch);
-
-	/* Disable DMA */
-	cpc_writeb(scabase + DRR, drr_ena_bit);
-	cpc_writeb(scabase + DRR, drr_rst_bit & ~drr_ena_bit);
-}
-
-static void rx_dma_start(pc300_t * card, int ch)
-{
-	void __iomem *scabase = card->hw.scabase;
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	
-	/* Start DMA */
-	cpc_writel(scabase + DRX_REG(CDAL, ch),
-		   RX_BD_ADDR(ch, chan->rx_first_bd));
-	if (cpc_readl(scabase + DRX_REG(CDAL,ch)) !=
-				  RX_BD_ADDR(ch, chan->rx_first_bd)) {
-		cpc_writel(scabase + DRX_REG(CDAL, ch),
-				   RX_BD_ADDR(ch, chan->rx_first_bd));
-	}
-	cpc_writel(scabase + DRX_REG(EDAL, ch),
-		   RX_BD_ADDR(ch, chan->rx_last_bd));
-	cpc_writew(scabase + DRX_REG(BFLL, ch), BD_DEF_LEN);
-	cpc_writeb(scabase + DSR_RX(ch), DSR_DE);
-	if (!(cpc_readb(scabase + DSR_RX(ch)) & DSR_DE)) {
-	cpc_writeb(scabase + DSR_RX(ch), DSR_DE);
-	}
-}
-
-/*************************/
-/***   FALC Routines   ***/
-/*************************/
-static void falc_issue_cmd(pc300_t *card, int ch, u8 cmd)
-{
-	void __iomem *falcbase = card->hw.falcbase;
-	unsigned long i = 0;
-
-	while (cpc_readb(falcbase + F_REG(SIS, ch)) & SIS_CEC) {
-		if (i++ >= PC300_FALC_MAXLOOP) {
-			printk("%s: FALC command locked(cmd=0x%x).\n",
-			       card->chan[ch].d.name, cmd);
-			break;
-		}
-	}
-	cpc_writeb(falcbase + F_REG(CMDR, ch), cmd);
-}
-
-static void falc_intr_enable(pc300_t * card, int ch)
-{
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	pc300chconf_t *conf = (pc300chconf_t *) & chan->conf;
-	falc_t *pfalc = (falc_t *) & chan->falc;
-	void __iomem *falcbase = card->hw.falcbase;
-
-	/* Interrupt pins are open-drain */
-	cpc_writeb(falcbase + F_REG(IPC, ch),
-		   cpc_readb(falcbase + F_REG(IPC, ch)) & ~IPC_IC0);
-	/* Conters updated each second */
-	cpc_writeb(falcbase + F_REG(FMR1, ch),
-		   cpc_readb(falcbase + F_REG(FMR1, ch)) | FMR1_ECM);
-	/* Enable SEC and ES interrupts  */
-	cpc_writeb(falcbase + F_REG(IMR3, ch),
-		   cpc_readb(falcbase + F_REG(IMR3, ch)) & ~(IMR3_SEC | IMR3_ES));
-	if (conf->fr_mode == PC300_FR_UNFRAMED) {
-		cpc_writeb(falcbase + F_REG(IMR4, ch),
-			   cpc_readb(falcbase + F_REG(IMR4, ch)) & ~(IMR4_LOS));
-	} else {
-		cpc_writeb(falcbase + F_REG(IMR4, ch),
-			   cpc_readb(falcbase + F_REG(IMR4, ch)) &
-			   ~(IMR4_LFA | IMR4_AIS | IMR4_LOS | IMR4_SLIP));
-	}
-	if (conf->media == IF_IFACE_T1) {
-		cpc_writeb(falcbase + F_REG(IMR3, ch),
-			   cpc_readb(falcbase + F_REG(IMR3, ch)) & ~IMR3_LLBSC);
-	} else {
-		cpc_writeb(falcbase + F_REG(IPC, ch),
-			   cpc_readb(falcbase + F_REG(IPC, ch)) | IPC_SCI);
-		if (conf->fr_mode == PC300_FR_UNFRAMED) {
-			cpc_writeb(falcbase + F_REG(IMR2, ch),
-				   cpc_readb(falcbase + F_REG(IMR2, ch)) & ~(IMR2_LOS));
-		} else {
-			cpc_writeb(falcbase + F_REG(IMR2, ch),
-				   cpc_readb(falcbase + F_REG(IMR2, ch)) &
-				   ~(IMR2_FAR | IMR2_LFA | IMR2_AIS | IMR2_LOS));
-			if (pfalc->multiframe_mode) {
-				cpc_writeb(falcbase + F_REG(IMR2, ch),
-					   cpc_readb(falcbase + F_REG(IMR2, ch)) & 
-					   ~(IMR2_T400MS | IMR2_MFAR));
-			} else {
-				cpc_writeb(falcbase + F_REG(IMR2, ch),
-					   cpc_readb(falcbase + F_REG(IMR2, ch)) | 
-					   IMR2_T400MS | IMR2_MFAR);
-			}
-		}
-	}
-}
-
-static void falc_open_timeslot(pc300_t * card, int ch, int timeslot)
-{
-	void __iomem *falcbase = card->hw.falcbase;
-	u8 tshf = card->chan[ch].falc.offset;
-
-	cpc_writeb(falcbase + F_REG((ICB1 + (timeslot - tshf) / 8), ch),
-		   cpc_readb(falcbase + F_REG((ICB1 + (timeslot - tshf) / 8), ch)) & 
-		   	~(0x80 >> ((timeslot - tshf) & 0x07)));
-	cpc_writeb(falcbase + F_REG((TTR1 + timeslot / 8), ch),
-		   cpc_readb(falcbase + F_REG((TTR1 + timeslot / 8), ch)) | 
-   			(0x80 >> (timeslot & 0x07)));
-	cpc_writeb(falcbase + F_REG((RTR1 + timeslot / 8), ch),
-		   cpc_readb(falcbase + F_REG((RTR1 + timeslot / 8), ch)) | 
-			(0x80 >> (timeslot & 0x07)));
-}
-
-static void falc_close_timeslot(pc300_t * card, int ch, int timeslot)
-{
-	void __iomem *falcbase = card->hw.falcbase;
-	u8 tshf = card->chan[ch].falc.offset;
-
-	cpc_writeb(falcbase + F_REG((ICB1 + (timeslot - tshf) / 8), ch),
-		   cpc_readb(falcbase + F_REG((ICB1 + (timeslot - tshf) / 8), ch)) | 
-		   (0x80 >> ((timeslot - tshf) & 0x07)));
-	cpc_writeb(falcbase + F_REG((TTR1 + timeslot / 8), ch),
-		   cpc_readb(falcbase + F_REG((TTR1 + timeslot / 8), ch)) & 
-		   ~(0x80 >> (timeslot & 0x07)));
-	cpc_writeb(falcbase + F_REG((RTR1 + timeslot / 8), ch),
-		   cpc_readb(falcbase + F_REG((RTR1 + timeslot / 8), ch)) & 
-		   ~(0x80 >> (timeslot & 0x07)));
-}
-
-static void falc_close_all_timeslots(pc300_t * card, int ch)
-{
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	pc300chconf_t *conf = (pc300chconf_t *) & chan->conf;
-	void __iomem *falcbase = card->hw.falcbase;
-
-	cpc_writeb(falcbase + F_REG(ICB1, ch), 0xff);
-	cpc_writeb(falcbase + F_REG(TTR1, ch), 0);
-	cpc_writeb(falcbase + F_REG(RTR1, ch), 0);
-	cpc_writeb(falcbase + F_REG(ICB2, ch), 0xff);
-	cpc_writeb(falcbase + F_REG(TTR2, ch), 0);
-	cpc_writeb(falcbase + F_REG(RTR2, ch), 0);
-	cpc_writeb(falcbase + F_REG(ICB3, ch), 0xff);
-	cpc_writeb(falcbase + F_REG(TTR3, ch), 0);
-	cpc_writeb(falcbase + F_REG(RTR3, ch), 0);
-	if (conf->media == IF_IFACE_E1) {
-		cpc_writeb(falcbase + F_REG(ICB4, ch), 0xff);
-		cpc_writeb(falcbase + F_REG(TTR4, ch), 0);
-		cpc_writeb(falcbase + F_REG(RTR4, ch), 0);
-	}
-}
-
-static void falc_open_all_timeslots(pc300_t * card, int ch)
-{
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	pc300chconf_t *conf = (pc300chconf_t *) & chan->conf;
-	void __iomem *falcbase = card->hw.falcbase;
-
-	cpc_writeb(falcbase + F_REG(ICB1, ch), 0);
-	if (conf->fr_mode == PC300_FR_UNFRAMED) {
-		cpc_writeb(falcbase + F_REG(TTR1, ch), 0xff);
-		cpc_writeb(falcbase + F_REG(RTR1, ch), 0xff);
-	} else {
-		/* Timeslot 0 is never enabled */
-		cpc_writeb(falcbase + F_REG(TTR1, ch), 0x7f);
-		cpc_writeb(falcbase + F_REG(RTR1, ch), 0x7f);
-	}
-	cpc_writeb(falcbase + F_REG(ICB2, ch), 0);
-	cpc_writeb(falcbase + F_REG(TTR2, ch), 0xff);
-	cpc_writeb(falcbase + F_REG(RTR2, ch), 0xff);
-	cpc_writeb(falcbase + F_REG(ICB3, ch), 0);
-	cpc_writeb(falcbase + F_REG(TTR3, ch), 0xff);
-	cpc_writeb(falcbase + F_REG(RTR3, ch), 0xff);
-	if (conf->media == IF_IFACE_E1) {
-		cpc_writeb(falcbase + F_REG(ICB4, ch), 0);
-		cpc_writeb(falcbase + F_REG(TTR4, ch), 0xff);
-		cpc_writeb(falcbase + F_REG(RTR4, ch), 0xff);
-	} else {
-		cpc_writeb(falcbase + F_REG(ICB4, ch), 0xff);
-		cpc_writeb(falcbase + F_REG(TTR4, ch), 0x80);
-		cpc_writeb(falcbase + F_REG(RTR4, ch), 0x80);
-	}
-}
-
-static void falc_init_timeslot(pc300_t * card, int ch)
-{
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	pc300chconf_t *conf = (pc300chconf_t *) & chan->conf;
-	falc_t *pfalc = (falc_t *) & chan->falc;
-	int tslot;
-
-	for (tslot = 0; tslot < pfalc->num_channels; tslot++) {
-		if (conf->tslot_bitmap & (1 << tslot)) {
-			// Channel enabled
-			falc_open_timeslot(card, ch, tslot + 1);
-		} else {
-			// Channel disabled
-			falc_close_timeslot(card, ch, tslot + 1);
-		}
-	}
-}
-
-static void falc_enable_comm(pc300_t * card, int ch)
-{
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	falc_t *pfalc = (falc_t *) & chan->falc;
-
-	if (pfalc->full_bandwidth) {
-		falc_open_all_timeslots(card, ch);
-	} else {
-		falc_init_timeslot(card, ch);
-	}
-	// CTS/DCD ON
-	cpc_writeb(card->hw.falcbase + card->hw.cpld_reg1,
-		   cpc_readb(card->hw.falcbase + card->hw.cpld_reg1) &
-		   ~((CPLD_REG1_FALC_DCD | CPLD_REG1_FALC_CTS) << (2 * ch)));
-}
-
-static void falc_disable_comm(pc300_t * card, int ch)
-{
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	falc_t *pfalc = (falc_t *) & chan->falc;
-
-	if (pfalc->loop_active != 2) {
-		falc_close_all_timeslots(card, ch);
-	}
-	// CTS/DCD OFF
-	cpc_writeb(card->hw.falcbase + card->hw.cpld_reg1,
-		   cpc_readb(card->hw.falcbase + card->hw.cpld_reg1) |
-		   ((CPLD_REG1_FALC_DCD | CPLD_REG1_FALC_CTS) << (2 * ch)));
-}
-
-static void falc_init_t1(pc300_t * card, int ch)
-{
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	pc300chconf_t *conf = (pc300chconf_t *) & chan->conf;
-	falc_t *pfalc = (falc_t *) & chan->falc;
-	void __iomem *falcbase = card->hw.falcbase;
-	u8 dja = (ch ? (LIM2_DJA2 | LIM2_DJA1) : 0);
-
-	/* Switch to T1 mode (PCM 24) */
-	cpc_writeb(falcbase + F_REG(FMR1, ch), FMR1_PMOD);
-
-	/* Wait 20 us for setup */
-	udelay(20);
-
-	/* Transmit Buffer Size (1 frame) */
-	cpc_writeb(falcbase + F_REG(SIC1, ch), SIC1_XBS0);
-
-	/* Clock mode */
-	if (conf->phys_settings.clock_type == CLOCK_INT) { /* Master mode */
-		cpc_writeb(falcbase + F_REG(LIM0, ch),
-			   cpc_readb(falcbase + F_REG(LIM0, ch)) | LIM0_MAS);
-	} else { /* Slave mode */
-		cpc_writeb(falcbase + F_REG(LIM0, ch),
-			   cpc_readb(falcbase + F_REG(LIM0, ch)) & ~LIM0_MAS);
-		cpc_writeb(falcbase + F_REG(LOOP, ch),
-			   cpc_readb(falcbase + F_REG(LOOP, ch)) & ~LOOP_RTM);
-	}
-
-	cpc_writeb(falcbase + F_REG(IPC, ch), IPC_SCI);
-	cpc_writeb(falcbase + F_REG(FMR0, ch),
-		   cpc_readb(falcbase + F_REG(FMR0, ch)) &
-		   ~(FMR0_XC0 | FMR0_XC1 | FMR0_RC0 | FMR0_RC1));
-
-	switch (conf->lcode) {
-		case PC300_LC_AMI:
-			cpc_writeb(falcbase + F_REG(FMR0, ch),
-				   cpc_readb(falcbase + F_REG(FMR0, ch)) |
-				   FMR0_XC1 | FMR0_RC1);
-			/* Clear Channel register to ON for all channels */
-			cpc_writeb(falcbase + F_REG(CCB1, ch), 0xff);
-			cpc_writeb(falcbase + F_REG(CCB2, ch), 0xff);
-			cpc_writeb(falcbase + F_REG(CCB3, ch), 0xff);
-			break;
-
-		case PC300_LC_B8ZS:
-			cpc_writeb(falcbase + F_REG(FMR0, ch),
-				   cpc_readb(falcbase + F_REG(FMR0, ch)) |
-				   FMR0_XC0 | FMR0_XC1 | FMR0_RC0 | FMR0_RC1);
-			break;
-
-		case PC300_LC_NRZ:
-			cpc_writeb(falcbase + F_REG(FMR0, ch),
-				   cpc_readb(falcbase + F_REG(FMR0, ch)) | 0x00);
-			break;
-	}
-
-	cpc_writeb(falcbase + F_REG(LIM0, ch),
-		   cpc_readb(falcbase + F_REG(LIM0, ch)) | LIM0_ELOS);
-	cpc_writeb(falcbase + F_REG(LIM0, ch),
-		   cpc_readb(falcbase + F_REG(LIM0, ch)) & ~(LIM0_SCL1 | LIM0_SCL0));
-	/* Set interface mode to 2 MBPS */
-	cpc_writeb(falcbase + F_REG(FMR1, ch),
-		   cpc_readb(falcbase + F_REG(FMR1, ch)) | FMR1_IMOD);
-
-	switch (conf->fr_mode) {
-		case PC300_FR_ESF:
-			pfalc->multiframe_mode = 0;
-			cpc_writeb(falcbase + F_REG(FMR4, ch),
-				   cpc_readb(falcbase + F_REG(FMR4, ch)) | FMR4_FM1);
-			cpc_writeb(falcbase + F_REG(FMR1, ch),
-				   cpc_readb(falcbase + F_REG(FMR1, ch)) | 
-				   FMR1_CRC | FMR1_EDL);
-			cpc_writeb(falcbase + F_REG(XDL1, ch), 0);
-			cpc_writeb(falcbase + F_REG(XDL2, ch), 0);
-			cpc_writeb(falcbase + F_REG(XDL3, ch), 0);
-			cpc_writeb(falcbase + F_REG(FMR0, ch),
-				   cpc_readb(falcbase + F_REG(FMR0, ch)) & ~FMR0_SRAF);
-			cpc_writeb(falcbase + F_REG(FMR2, ch),
-				   cpc_readb(falcbase + F_REG(FMR2,ch)) | FMR2_MCSP | FMR2_SSP);
-			break;
-
-		case PC300_FR_D4:
-			pfalc->multiframe_mode = 1;
-			cpc_writeb(falcbase + F_REG(FMR4, ch),
-				   cpc_readb(falcbase + F_REG(FMR4, ch)) &
-				   ~(FMR4_FM1 | FMR4_FM0));
-			cpc_writeb(falcbase + F_REG(FMR0, ch),
-				   cpc_readb(falcbase + F_REG(FMR0, ch)) | FMR0_SRAF);
-			cpc_writeb(falcbase + F_REG(FMR2, ch),
-				   cpc_readb(falcbase + F_REG(FMR2, ch)) & ~FMR2_SSP);
-			break;
-	}
-
-	/* Enable Automatic Resynchronization */
-	cpc_writeb(falcbase + F_REG(FMR4, ch),
-		   cpc_readb(falcbase + F_REG(FMR4, ch)) | FMR4_AUTO);
-
-	/* Transmit Automatic Remote Alarm */
-	cpc_writeb(falcbase + F_REG(FMR2, ch),
-		   cpc_readb(falcbase + F_REG(FMR2, ch)) | FMR2_AXRA);
-
-	/* Channel translation mode 1 : one to one */
-	cpc_writeb(falcbase + F_REG(FMR1, ch),
-		   cpc_readb(falcbase + F_REG(FMR1, ch)) | FMR1_CTM);
-
-	/* No signaling */
-	cpc_writeb(falcbase + F_REG(FMR1, ch),
-		   cpc_readb(falcbase + F_REG(FMR1, ch)) & ~FMR1_SIGM);
-	cpc_writeb(falcbase + F_REG(FMR5, ch),
-		   cpc_readb(falcbase + F_REG(FMR5, ch)) &
-		   ~(FMR5_EIBR | FMR5_SRS));
-	cpc_writeb(falcbase + F_REG(CCR1, ch), 0);
-
-	cpc_writeb(falcbase + F_REG(LIM1, ch),
-		   cpc_readb(falcbase + F_REG(LIM1, ch)) | LIM1_RIL0 | LIM1_RIL1);
-
-	switch (conf->lbo) {
-			/* Provides proper Line Build Out */
-		case PC300_LBO_0_DB:
-			cpc_writeb(falcbase + F_REG(LIM2, ch), (LIM2_LOS1 | dja));
-			cpc_writeb(falcbase + F_REG(XPM0, ch), 0x5a);
-			cpc_writeb(falcbase + F_REG(XPM1, ch), 0x8f);
-			cpc_writeb(falcbase + F_REG(XPM2, ch), 0x20);
-			break;
-		case PC300_LBO_7_5_DB:
-			cpc_writeb(falcbase + F_REG(LIM2, ch), (0x40 | LIM2_LOS1 | dja));
-			cpc_writeb(falcbase + F_REG(XPM0, ch), 0x11);
-			cpc_writeb(falcbase + F_REG(XPM1, ch), 0x02);
-			cpc_writeb(falcbase + F_REG(XPM2, ch), 0x20);
-			break;
-		case PC300_LBO_15_DB:
-			cpc_writeb(falcbase + F_REG(LIM2, ch), (0x80 | LIM2_LOS1 | dja));
-			cpc_writeb(falcbase + F_REG(XPM0, ch), 0x8e);
-			cpc_writeb(falcbase + F_REG(XPM1, ch), 0x01);
-			cpc_writeb(falcbase + F_REG(XPM2, ch), 0x20);
-			break;
-		case PC300_LBO_22_5_DB:
-			cpc_writeb(falcbase + F_REG(LIM2, ch), (0xc0 | LIM2_LOS1 | dja));
-			cpc_writeb(falcbase + F_REG(XPM0, ch), 0x09);
-			cpc_writeb(falcbase + F_REG(XPM1, ch), 0x01);
-			cpc_writeb(falcbase + F_REG(XPM2, ch), 0x20);
-			break;
-	}
-
-	/* Transmit Clock-Slot Offset */
-	cpc_writeb(falcbase + F_REG(XC0, ch),
-		   cpc_readb(falcbase + F_REG(XC0, ch)) | 0x01);
-	/* Transmit Time-slot Offset */
-	cpc_writeb(falcbase + F_REG(XC1, ch), 0x3e);
-	/* Receive  Clock-Slot offset */
-	cpc_writeb(falcbase + F_REG(RC0, ch), 0x05);
-	/* Receive  Time-slot offset */
-	cpc_writeb(falcbase + F_REG(RC1, ch), 0x00);
-
-	/* LOS Detection after 176 consecutive 0s */
-	cpc_writeb(falcbase + F_REG(PCDR, ch), 0x0a);
-	/* LOS Recovery after 22 ones in the time window of PCD */
-	cpc_writeb(falcbase + F_REG(PCRR, ch), 0x15);
-
-	cpc_writeb(falcbase + F_REG(IDLE, ch), 0x7f);
-
-	if (conf->fr_mode == PC300_FR_ESF_JAPAN) {
-		cpc_writeb(falcbase + F_REG(RC1, ch),
-			   cpc_readb(falcbase + F_REG(RC1, ch)) | 0x80);
-	}
-
-	falc_close_all_timeslots(card, ch);
-}
-
-static void falc_init_e1(pc300_t * card, int ch)
-{
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	pc300chconf_t *conf = (pc300chconf_t *) & chan->conf;
-	falc_t *pfalc = (falc_t *) & chan->falc;
-	void __iomem *falcbase = card->hw.falcbase;
-	u8 dja = (ch ? (LIM2_DJA2 | LIM2_DJA1) : 0);
-
-	/* Switch to E1 mode (PCM 30) */
-	cpc_writeb(falcbase + F_REG(FMR1, ch),
-		   cpc_readb(falcbase + F_REG(FMR1, ch)) & ~FMR1_PMOD);
-
-	/* Clock mode */
-	if (conf->phys_settings.clock_type == CLOCK_INT) { /* Master mode */
-		cpc_writeb(falcbase + F_REG(LIM0, ch),
-			   cpc_readb(falcbase + F_REG(LIM0, ch)) | LIM0_MAS);
-	} else { /* Slave mode */
-		cpc_writeb(falcbase + F_REG(LIM0, ch),
-			   cpc_readb(falcbase + F_REG(LIM0, ch)) & ~LIM0_MAS);
-	}
-	cpc_writeb(falcbase + F_REG(LOOP, ch),
-		   cpc_readb(falcbase + F_REG(LOOP, ch)) & ~LOOP_SFM);
-
-	cpc_writeb(falcbase + F_REG(IPC, ch), IPC_SCI);
-	cpc_writeb(falcbase + F_REG(FMR0, ch),
-		   cpc_readb(falcbase + F_REG(FMR0, ch)) &
-		   ~(FMR0_XC0 | FMR0_XC1 | FMR0_RC0 | FMR0_RC1));
-
-	switch (conf->lcode) {
-		case PC300_LC_AMI:
-			cpc_writeb(falcbase + F_REG(FMR0, ch),
-				   cpc_readb(falcbase + F_REG(FMR0, ch)) |
-				   FMR0_XC1 | FMR0_RC1);
-			break;
-
-		case PC300_LC_HDB3:
-			cpc_writeb(falcbase + F_REG(FMR0, ch),
-				   cpc_readb(falcbase + F_REG(FMR0, ch)) |
-				   FMR0_XC0 | FMR0_XC1 | FMR0_RC0 | FMR0_RC1);
-			break;
-
-		case PC300_LC_NRZ:
-			break;
-	}
-
-	cpc_writeb(falcbase + F_REG(LIM0, ch),
-		   cpc_readb(falcbase + F_REG(LIM0, ch)) & ~(LIM0_SCL1 | LIM0_SCL0));
-	/* Set interface mode to 2 MBPS */
-	cpc_writeb(falcbase + F_REG(FMR1, ch),
-		   cpc_readb(falcbase + F_REG(FMR1, ch)) | FMR1_IMOD);
-
-	cpc_writeb(falcbase + F_REG(XPM0, ch), 0x18);
-	cpc_writeb(falcbase + F_REG(XPM1, ch), 0x03);
-	cpc_writeb(falcbase + F_REG(XPM2, ch), 0x00);
-
-	switch (conf->fr_mode) {
-		case PC300_FR_MF_CRC4:
-			pfalc->multiframe_mode = 1;
-			cpc_writeb(falcbase + F_REG(FMR1, ch),
-				   cpc_readb(falcbase + F_REG(FMR1, ch)) | FMR1_XFS);
-			cpc_writeb(falcbase + F_REG(FMR2, ch),
-				   cpc_readb(falcbase + F_REG(FMR2, ch)) | FMR2_RFS1);
-			cpc_writeb(falcbase + F_REG(FMR2, ch),
-				   cpc_readb(falcbase + F_REG(FMR2, ch)) & ~FMR2_RFS0);
-			cpc_writeb(falcbase + F_REG(FMR3, ch),
-				   cpc_readb(falcbase + F_REG(FMR3, ch)) & ~FMR3_EXTIW);
-
-			/* MultiFrame Resynchronization */
-			cpc_writeb(falcbase + F_REG(FMR1, ch),
-				   cpc_readb(falcbase + F_REG(FMR1, ch)) | FMR1_MFCS);
-
-			/* Automatic Loss of Multiframe > 914 CRC errors */
-			cpc_writeb(falcbase + F_REG(FMR2, ch),
-				   cpc_readb(falcbase + F_REG(FMR2, ch)) | FMR2_ALMF);
-
-			/* S1 and SI1/SI2 spare Bits set to 1 */
-			cpc_writeb(falcbase + F_REG(XSP, ch),
-				   cpc_readb(falcbase + F_REG(XSP, ch)) & ~XSP_AXS);
-			cpc_writeb(falcbase + F_REG(XSP, ch),
-				   cpc_readb(falcbase + F_REG(XSP, ch)) | XSP_EBP);
-			cpc_writeb(falcbase + F_REG(XSP, ch),
-				   cpc_readb(falcbase + F_REG(XSP, ch)) | XSP_XS13 | XSP_XS15);
-
-			/* Automatic Force Resynchronization */
-			cpc_writeb(falcbase + F_REG(FMR1, ch),
-				   cpc_readb(falcbase + F_REG(FMR1, ch)) | FMR1_AFR);
-
-			/* Transmit Automatic Remote Alarm */
-			cpc_writeb(falcbase + F_REG(FMR2, ch),
-				   cpc_readb(falcbase + F_REG(FMR2, ch)) | FMR2_AXRA);
-
-			/* Transmit Spare Bits for National Use (Y, Sn, Sa) */
-			cpc_writeb(falcbase + F_REG(XSW, ch),
-				   cpc_readb(falcbase + F_REG(XSW, ch)) |
-				   XSW_XY0 | XSW_XY1 | XSW_XY2 | XSW_XY3 | XSW_XY4);
-			break;
-
-		case PC300_FR_MF_NON_CRC4:
-		case PC300_FR_D4:
-			pfalc->multiframe_mode = 0;
-			cpc_writeb(falcbase + F_REG(FMR1, ch),
-				   cpc_readb(falcbase + F_REG(FMR1, ch)) & ~FMR1_XFS);
-			cpc_writeb(falcbase + F_REG(FMR2, ch),
-				   cpc_readb(falcbase + F_REG(FMR2, ch)) & 
-				   ~(FMR2_RFS1 | FMR2_RFS0));
-			cpc_writeb(falcbase + F_REG(XSW, ch),
-				   cpc_readb(falcbase + F_REG(XSW, ch)) | XSW_XSIS);
-			cpc_writeb(falcbase + F_REG(XSP, ch),
-				   cpc_readb(falcbase + F_REG(XSP, ch)) | XSP_XSIF);
-
-			/* Automatic Force Resynchronization */
-			cpc_writeb(falcbase + F_REG(FMR1, ch),
-				   cpc_readb(falcbase + F_REG(FMR1, ch)) | FMR1_AFR);
-
-			/* Transmit Automatic Remote Alarm */
-			cpc_writeb(falcbase + F_REG(FMR2, ch),
-				   cpc_readb(falcbase + F_REG(FMR2, ch)) | FMR2_AXRA);
-
-			/* Transmit Spare Bits for National Use (Y, Sn, Sa) */
-			cpc_writeb(falcbase + F_REG(XSW, ch),
-				   cpc_readb(falcbase + F_REG(XSW, ch)) |
-				   XSW_XY0 | XSW_XY1 | XSW_XY2 | XSW_XY3 | XSW_XY4);
-			break;
-
-		case PC300_FR_UNFRAMED:
-			pfalc->multiframe_mode = 0;
-			cpc_writeb(falcbase + F_REG(FMR1, ch),
-				   cpc_readb(falcbase + F_REG(FMR1, ch)) & ~FMR1_XFS);
-			cpc_writeb(falcbase + F_REG(FMR2, ch),
-				   cpc_readb(falcbase + F_REG(FMR2, ch)) & 
-				   ~(FMR2_RFS1 | FMR2_RFS0));
-			cpc_writeb(falcbase + F_REG(XSP, ch),
-				   cpc_readb(falcbase + F_REG(XSP, ch)) | XSP_TT0);
-			cpc_writeb(falcbase + F_REG(XSW, ch),
-				   cpc_readb(falcbase + F_REG(XSW, ch)) & 
-				   ~(XSW_XTM|XSW_XY0|XSW_XY1|XSW_XY2|XSW_XY3|XSW_XY4));
-			cpc_writeb(falcbase + F_REG(TSWM, ch), 0xff);
-			cpc_writeb(falcbase + F_REG(FMR2, ch),
-				   cpc_readb(falcbase + F_REG(FMR2, ch)) |
-				   (FMR2_RTM | FMR2_DAIS));
-			cpc_writeb(falcbase + F_REG(FMR2, ch),
-				   cpc_readb(falcbase + F_REG(FMR2, ch)) & ~FMR2_AXRA);
-			cpc_writeb(falcbase + F_REG(FMR1, ch),
-				   cpc_readb(falcbase + F_REG(FMR1, ch)) & ~FMR1_AFR);
-			pfalc->sync = 1;
-			cpc_writeb(falcbase + card->hw.cpld_reg2,
-				   cpc_readb(falcbase + card->hw.cpld_reg2) |
-				   (CPLD_REG2_FALC_LED2 << (2 * ch)));
-			break;
-	}
-
-	/* No signaling */
-	cpc_writeb(falcbase + F_REG(XSP, ch),
-		   cpc_readb(falcbase + F_REG(XSP, ch)) & ~XSP_CASEN);
-	cpc_writeb(falcbase + F_REG(CCR1, ch), 0);
-
-	cpc_writeb(falcbase + F_REG(LIM1, ch),
-		   cpc_readb(falcbase + F_REG(LIM1, ch)) | LIM1_RIL0 | LIM1_RIL1);
-	cpc_writeb(falcbase + F_REG(LIM2, ch), (LIM2_LOS1 | dja));
-
-	/* Transmit Clock-Slot Offset */
-	cpc_writeb(falcbase + F_REG(XC0, ch),
-		   cpc_readb(falcbase + F_REG(XC0, ch)) | 0x01);
-	/* Transmit Time-slot Offset */
-	cpc_writeb(falcbase + F_REG(XC1, ch), 0x3e);
-	/* Receive  Clock-Slot offset */
-	cpc_writeb(falcbase + F_REG(RC0, ch), 0x05);
-	/* Receive  Time-slot offset */
-	cpc_writeb(falcbase + F_REG(RC1, ch), 0x00);
-
-	/* LOS Detection after 176 consecutive 0s */
-	cpc_writeb(falcbase + F_REG(PCDR, ch), 0x0a);
-	/* LOS Recovery after 22 ones in the time window of PCD */
-	cpc_writeb(falcbase + F_REG(PCRR, ch), 0x15);
-
-	cpc_writeb(falcbase + F_REG(IDLE, ch), 0x7f);
-
-	falc_close_all_timeslots(card, ch);
-}
-
-static void falc_init_hdlc(pc300_t * card, int ch)
-{
-	void __iomem *falcbase = card->hw.falcbase;
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	pc300chconf_t *conf = (pc300chconf_t *) & chan->conf;
-
-	/* Enable transparent data transfer */
-	if (conf->fr_mode == PC300_FR_UNFRAMED) {
-		cpc_writeb(falcbase + F_REG(MODE, ch), 0);
-	} else {
-		cpc_writeb(falcbase + F_REG(MODE, ch),
-			   cpc_readb(falcbase + F_REG(MODE, ch)) |
-			   (MODE_HRAC | MODE_MDS2));
-		cpc_writeb(falcbase + F_REG(RAH2, ch), 0xff);
-		cpc_writeb(falcbase + F_REG(RAH1, ch), 0xff);
-		cpc_writeb(falcbase + F_REG(RAL2, ch), 0xff);
-		cpc_writeb(falcbase + F_REG(RAL1, ch), 0xff);
-	}
-
-	/* Tx/Rx reset  */
-	falc_issue_cmd(card, ch, CMDR_RRES | CMDR_XRES | CMDR_SRES);
-
-	/* Enable interrupt sources */
-	falc_intr_enable(card, ch);
-}
-
-static void te_config(pc300_t * card, int ch)
-{
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	pc300chconf_t *conf = (pc300chconf_t *) & chan->conf;
-	falc_t *pfalc = (falc_t *) & chan->falc;
-	void __iomem *falcbase = card->hw.falcbase;
-	u8 dummy;
-	unsigned long flags;
-
-	memset(pfalc, 0, sizeof(falc_t));
-	switch (conf->media) {
-		case IF_IFACE_T1:
-			pfalc->num_channels = NUM_OF_T1_CHANNELS;
-			pfalc->offset = 1;
-			break;
-		case IF_IFACE_E1:
-			pfalc->num_channels = NUM_OF_E1_CHANNELS;
-			pfalc->offset = 0;
-			break;
-	}
-	if (conf->tslot_bitmap == 0xffffffffUL)
-		pfalc->full_bandwidth = 1;
-	else
-		pfalc->full_bandwidth = 0;
-
-	CPC_LOCK(card, flags);
-	/* Reset the FALC chip */
-	cpc_writeb(card->hw.falcbase + card->hw.cpld_reg1,
-		   cpc_readb(card->hw.falcbase + card->hw.cpld_reg1) |
-		   (CPLD_REG1_FALC_RESET << (2 * ch)));
-	udelay(10000);
-	cpc_writeb(card->hw.falcbase + card->hw.cpld_reg1,
-		   cpc_readb(card->hw.falcbase + card->hw.cpld_reg1) &
-		   ~(CPLD_REG1_FALC_RESET << (2 * ch)));
-
-	if (conf->media == IF_IFACE_T1) {
-		falc_init_t1(card, ch);
-	} else {
-		falc_init_e1(card, ch);
-	}
-	falc_init_hdlc(card, ch);
-	if (conf->rx_sens == PC300_RX_SENS_SH) {
-		cpc_writeb(falcbase + F_REG(LIM0, ch),
-			   cpc_readb(falcbase + F_REG(LIM0, ch)) & ~LIM0_EQON);
-	} else {
-		cpc_writeb(falcbase + F_REG(LIM0, ch),
-			   cpc_readb(falcbase + F_REG(LIM0, ch)) | LIM0_EQON);
-	}
-	cpc_writeb(card->hw.falcbase + card->hw.cpld_reg2,
-		   cpc_readb(card->hw.falcbase + card->hw.cpld_reg2) |
-		   ((CPLD_REG2_FALC_TX_CLK | CPLD_REG2_FALC_RX_CLK) << (2 * ch)));
-
-	/* Clear all interrupt registers */
-	dummy = cpc_readb(falcbase + F_REG(FISR0, ch)) +
-		cpc_readb(falcbase + F_REG(FISR1, ch)) +
-		cpc_readb(falcbase + F_REG(FISR2, ch)) +
-		cpc_readb(falcbase + F_REG(FISR3, ch));
-	CPC_UNLOCK(card, flags);
-}
-
-static void falc_check_status(pc300_t * card, int ch, unsigned char frs0)
-{
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	pc300chconf_t *conf = (pc300chconf_t *) & chan->conf;
-	falc_t *pfalc = (falc_t *) & chan->falc;
-	void __iomem *falcbase = card->hw.falcbase;
-
-	/* Verify LOS */
-	if (frs0 & FRS0_LOS) {
-		if (!pfalc->red_alarm) {
-			pfalc->red_alarm = 1;
-			pfalc->los++;
-			if (!pfalc->blue_alarm) {
-				// EVENT_FALC_ABNORMAL
-				if (conf->media == IF_IFACE_T1) {
-					/* Disable this interrupt as it may otherwise interfere 
-					 * with other working boards. */
-					cpc_writeb(falcbase + F_REG(IMR0, ch), 
-						   cpc_readb(falcbase + F_REG(IMR0, ch))
-						   | IMR0_PDEN);
-				}
-				falc_disable_comm(card, ch);
-				// EVENT_FALC_ABNORMAL
-			}
-		}
-	} else {
-		if (pfalc->red_alarm) {
-			pfalc->red_alarm = 0;
-			pfalc->losr++;
-		}
-	}
-
-	if (conf->fr_mode != PC300_FR_UNFRAMED) {
-		/* Verify AIS alarm */
-		if (frs0 & FRS0_AIS) {
-			if (!pfalc->blue_alarm) {
-				pfalc->blue_alarm = 1;
-				pfalc->ais++;
-				// EVENT_AIS
-				if (conf->media == IF_IFACE_T1) {
-					/* Disable this interrupt as it may otherwise interfere with                       other working boards. */
-					cpc_writeb(falcbase + F_REG(IMR0, ch),
-						   cpc_readb(falcbase + F_REG(IMR0, ch)) | IMR0_PDEN);
-				}
-				falc_disable_comm(card, ch);
-				// EVENT_AIS
-			}
-		} else {
-			pfalc->blue_alarm = 0;
-		}
-
-		/* Verify LFA */
-		if (frs0 & FRS0_LFA) {
-			if (!pfalc->loss_fa) {
-				pfalc->loss_fa = 1;
-				pfalc->lfa++;
-				if (!pfalc->blue_alarm && !pfalc->red_alarm) {
-					// EVENT_FALC_ABNORMAL
-					if (conf->media == IF_IFACE_T1) {
-						/* Disable this interrupt as it may otherwise 
-						 * interfere with other working boards. */
-						cpc_writeb(falcbase + F_REG(IMR0, ch),
-							   cpc_readb(falcbase + F_REG(IMR0, ch))
-							   | IMR0_PDEN);
-					}
-					falc_disable_comm(card, ch);
-					// EVENT_FALC_ABNORMAL
-				}
-			}
-		} else {
-			if (pfalc->loss_fa) {
-				pfalc->loss_fa = 0;
-				pfalc->farec++;
-			}
-		}
-
-		/* Verify LMFA */
-		if (pfalc->multiframe_mode && (frs0 & FRS0_LMFA)) {
-			/* D4 or CRC4 frame mode */
-			if (!pfalc->loss_mfa) {
-				pfalc->loss_mfa = 1;
-				pfalc->lmfa++;
-				if (!pfalc->blue_alarm && !pfalc->red_alarm &&
-				    !pfalc->loss_fa) {
-					// EVENT_FALC_ABNORMAL
-					if (conf->media == IF_IFACE_T1) {
-						/* Disable this interrupt as it may otherwise 
-						 * interfere with other working boards. */
-						cpc_writeb(falcbase + F_REG(IMR0, ch),
-							   cpc_readb(falcbase + F_REG(IMR0, ch))
-							   | IMR0_PDEN);
-					}
-					falc_disable_comm(card, ch);
-					// EVENT_FALC_ABNORMAL
-				}
-			}
-		} else {
-			pfalc->loss_mfa = 0;
-		}
-
-		/* Verify Remote Alarm */
-		if (frs0 & FRS0_RRA) {
-			if (!pfalc->yellow_alarm) {
-				pfalc->yellow_alarm = 1;
-				pfalc->rai++;
-				if (pfalc->sync) {
-					// EVENT_RAI
-					falc_disable_comm(card, ch);
-					// EVENT_RAI
-				}
-			}
-		} else {
-			pfalc->yellow_alarm = 0;
-		}
-	} /* if !PC300_UNFRAMED */
-
-	if (pfalc->red_alarm || pfalc->loss_fa ||
-	    pfalc->loss_mfa || pfalc->blue_alarm) {
-		if (pfalc->sync) {
-			pfalc->sync = 0;
-			chan->d.line_off++;
-			cpc_writeb(falcbase + card->hw.cpld_reg2,
-				   cpc_readb(falcbase + card->hw.cpld_reg2) &
-				   ~(CPLD_REG2_FALC_LED2 << (2 * ch)));
-		}
-	} else {
-		if (!pfalc->sync) {
-			pfalc->sync = 1;
-			chan->d.line_on++;
-			cpc_writeb(falcbase + card->hw.cpld_reg2,
-				   cpc_readb(falcbase + card->hw.cpld_reg2) |
-				   (CPLD_REG2_FALC_LED2 << (2 * ch)));
-		}
-	}
-
-	if (pfalc->sync && !pfalc->yellow_alarm) {
-		if (!pfalc->active) {
-			// EVENT_FALC_NORMAL
-			if (pfalc->loop_active) {
-				return;
-			}
-			if (conf->media == IF_IFACE_T1) {
-				cpc_writeb(falcbase + F_REG(IMR0, ch),
-					   cpc_readb(falcbase + F_REG(IMR0, ch)) & ~IMR0_PDEN);
-			}
-			falc_enable_comm(card, ch);
-			// EVENT_FALC_NORMAL
-			pfalc->active = 1;
-		}
-	} else {
-		if (pfalc->active) {
-			pfalc->active = 0;
-		}
-	}
-}
-
-static void falc_update_stats(pc300_t * card, int ch)
-{
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	pc300chconf_t *conf = (pc300chconf_t *) & chan->conf;
-	falc_t *pfalc = (falc_t *) & chan->falc;
-	void __iomem *falcbase = card->hw.falcbase;
-	u16 counter;
-
-	counter = cpc_readb(falcbase + F_REG(FECL, ch));
-	counter |= cpc_readb(falcbase + F_REG(FECH, ch)) << 8;
-	pfalc->fec += counter;
-
-	counter = cpc_readb(falcbase + F_REG(CVCL, ch));
-	counter |= cpc_readb(falcbase + F_REG(CVCH, ch)) << 8;
-	pfalc->cvc += counter;
-
-	counter = cpc_readb(falcbase + F_REG(CECL, ch));
-	counter |= cpc_readb(falcbase + F_REG(CECH, ch)) << 8;
-	pfalc->cec += counter;
-
-	counter = cpc_readb(falcbase + F_REG(EBCL, ch));
-	counter |= cpc_readb(falcbase + F_REG(EBCH, ch)) << 8;
-	pfalc->ebc += counter;
-
-	if (cpc_readb(falcbase + F_REG(LCR1, ch)) & LCR1_EPRM) {
-		mdelay(10);
-		counter = cpc_readb(falcbase + F_REG(BECL, ch));
-		counter |= cpc_readb(falcbase + F_REG(BECH, ch)) << 8;
-		pfalc->bec += counter;
-
-		if (((conf->media == IF_IFACE_T1) &&
-		     (cpc_readb(falcbase + F_REG(FRS1, ch)) & FRS1_LLBAD) &&
-		     (!(cpc_readb(falcbase + F_REG(FRS1, ch)) & FRS1_PDEN))) ||
-		    ((conf->media == IF_IFACE_E1) &&
-		     (cpc_readb(falcbase + F_REG(RSP, ch)) & RSP_LLBAD))) {
-			pfalc->prbs = 2;
-		} else {
-			pfalc->prbs = 1;
-		}
-	}
-}
-
-/*----------------------------------------------------------------------------
- * falc_remote_loop
- *----------------------------------------------------------------------------
- * Description:	In the remote loopback mode the clock and data recovered
- *		from the line inputs RL1/2 or RDIP/RDIN are routed back
- *		to the line outputs XL1/2 or XDOP/XDON via the analog
- *		transmitter. As in normal mode they are processed by
- *		the synchronizer and then sent to the system interface.
- *----------------------------------------------------------------------------
- */
-static void falc_remote_loop(pc300_t * card, int ch, int loop_on)
-{
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	pc300chconf_t *conf = (pc300chconf_t *) & chan->conf;
-	falc_t *pfalc = (falc_t *) & chan->falc;
-	void __iomem *falcbase = card->hw.falcbase;
-
-	if (loop_on) {
-		// EVENT_FALC_ABNORMAL
-		if (conf->media == IF_IFACE_T1) {
-			/* Disable this interrupt as it may otherwise interfere with 
-			 * other working boards. */
-			cpc_writeb(falcbase + F_REG(IMR0, ch),
-				   cpc_readb(falcbase + F_REG(IMR0, ch)) | IMR0_PDEN);
-		}
-		falc_disable_comm(card, ch);
-		// EVENT_FALC_ABNORMAL
-		cpc_writeb(falcbase + F_REG(LIM1, ch),
-			   cpc_readb(falcbase + F_REG(LIM1, ch)) | LIM1_RL);
-		pfalc->loop_active = 1;
-	} else {
-		cpc_writeb(falcbase + F_REG(LIM1, ch),
-			   cpc_readb(falcbase + F_REG(LIM1, ch)) & ~LIM1_RL);
-		pfalc->sync = 0;
-		cpc_writeb(falcbase + card->hw.cpld_reg2,
-			   cpc_readb(falcbase + card->hw.cpld_reg2) &
-			   ~(CPLD_REG2_FALC_LED2 << (2 * ch)));
-		pfalc->active = 0;
-		falc_issue_cmd(card, ch, CMDR_XRES);
-		pfalc->loop_active = 0;
-	}
-}
-
-/*----------------------------------------------------------------------------
- * falc_local_loop
- *----------------------------------------------------------------------------
- * Description: The local loopback mode disconnects the receive lines 
- *		RL1/RL2 resp. RDIP/RDIN from the receiver. Instead of the
- *		signals coming from the line the data provided by system
- *		interface are routed through the analog receiver back to
- *		the system interface. The unipolar bit stream will be
- *		undisturbed transmitted on the line. Receiver and transmitter
- *		coding must be identical.
- *----------------------------------------------------------------------------
- */
-static void falc_local_loop(pc300_t * card, int ch, int loop_on)
-{
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	falc_t *pfalc = (falc_t *) & chan->falc;
-	void __iomem *falcbase = card->hw.falcbase;
-
-	if (loop_on) {
-		cpc_writeb(falcbase + F_REG(LIM0, ch),
-			   cpc_readb(falcbase + F_REG(LIM0, ch)) | LIM0_LL);
-		pfalc->loop_active = 1;
-	} else {
-		cpc_writeb(falcbase + F_REG(LIM0, ch),
-			   cpc_readb(falcbase + F_REG(LIM0, ch)) & ~LIM0_LL);
-		pfalc->loop_active = 0;
-	}
-}
-
-/*----------------------------------------------------------------------------
- * falc_payload_loop
- *----------------------------------------------------------------------------
- * Description: This routine allows to enable/disable payload loopback.
- *		When the payload loop is activated, the received 192 bits
- *		of payload data will be looped back to the transmit
- *		direction. The framing bits, CRC6 and DL bits are not 
- *		looped. They are originated by the FALC-LH transmitter.
- *----------------------------------------------------------------------------
- */
-static void falc_payload_loop(pc300_t * card, int ch, int loop_on)
-{
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	pc300chconf_t *conf = (pc300chconf_t *) & chan->conf;
-	falc_t *pfalc = (falc_t *) & chan->falc;
-	void __iomem *falcbase = card->hw.falcbase;
-
-	if (loop_on) {
-		// EVENT_FALC_ABNORMAL
-		if (conf->media == IF_IFACE_T1) {
-			/* Disable this interrupt as it may otherwise interfere with 
-			 * other working boards. */
-			cpc_writeb(falcbase + F_REG(IMR0, ch),
-				   cpc_readb(falcbase + F_REG(IMR0, ch)) | IMR0_PDEN);
-		}
-		falc_disable_comm(card, ch);
-		// EVENT_FALC_ABNORMAL
-		cpc_writeb(falcbase + F_REG(FMR2, ch),
-			   cpc_readb(falcbase + F_REG(FMR2, ch)) | FMR2_PLB);
-		if (conf->media == IF_IFACE_T1) {
-			cpc_writeb(falcbase + F_REG(FMR4, ch),
-				   cpc_readb(falcbase + F_REG(FMR4, ch)) | FMR4_TM);
-		} else {
-			cpc_writeb(falcbase + F_REG(FMR5, ch),
-				   cpc_readb(falcbase + F_REG(FMR5, ch)) | XSP_TT0);
-		}
-		falc_open_all_timeslots(card, ch);
-		pfalc->loop_active = 2;
-	} else {
-		cpc_writeb(falcbase + F_REG(FMR2, ch),
-			   cpc_readb(falcbase + F_REG(FMR2, ch)) & ~FMR2_PLB);
-		if (conf->media == IF_IFACE_T1) {
-			cpc_writeb(falcbase + F_REG(FMR4, ch),
-				   cpc_readb(falcbase + F_REG(FMR4, ch)) & ~FMR4_TM);
-		} else {
-			cpc_writeb(falcbase + F_REG(FMR5, ch),
-				   cpc_readb(falcbase + F_REG(FMR5, ch)) & ~XSP_TT0);
-		}
-		pfalc->sync = 0;
-		cpc_writeb(falcbase + card->hw.cpld_reg2,
-			   cpc_readb(falcbase + card->hw.cpld_reg2) &
-			   ~(CPLD_REG2_FALC_LED2 << (2 * ch)));
-		pfalc->active = 0;
-		falc_issue_cmd(card, ch, CMDR_XRES);
-		pfalc->loop_active = 0;
-	}
-}
-
-/*----------------------------------------------------------------------------
- * turn_off_xlu
- *----------------------------------------------------------------------------
- * Description:	Turns XLU bit off in the proper register
- *----------------------------------------------------------------------------
- */
-static void turn_off_xlu(pc300_t * card, int ch)
-{
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	pc300chconf_t *conf = (pc300chconf_t *) & chan->conf;
-	void __iomem *falcbase = card->hw.falcbase;
-
-	if (conf->media == IF_IFACE_T1) {
-		cpc_writeb(falcbase + F_REG(FMR5, ch),
-			   cpc_readb(falcbase + F_REG(FMR5, ch)) & ~FMR5_XLU);
-	} else {
-		cpc_writeb(falcbase + F_REG(FMR3, ch),
-			   cpc_readb(falcbase + F_REG(FMR3, ch)) & ~FMR3_XLU);
-	}
-}
-
-/*----------------------------------------------------------------------------
- * turn_off_xld
- *----------------------------------------------------------------------------
- * Description: Turns XLD bit off in the proper register
- *----------------------------------------------------------------------------
- */
-static void turn_off_xld(pc300_t * card, int ch)
-{
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	pc300chconf_t *conf = (pc300chconf_t *) & chan->conf;
-	void __iomem *falcbase = card->hw.falcbase;
-
-	if (conf->media == IF_IFACE_T1) {
-		cpc_writeb(falcbase + F_REG(FMR5, ch),
-			   cpc_readb(falcbase + F_REG(FMR5, ch)) & ~FMR5_XLD);
-	} else {
-		cpc_writeb(falcbase + F_REG(FMR3, ch),
-			   cpc_readb(falcbase + F_REG(FMR3, ch)) & ~FMR3_XLD);
-	}
-}
-
-/*----------------------------------------------------------------------------
- * falc_generate_loop_up_code
- *----------------------------------------------------------------------------
- * Description:	This routine writes the proper FALC chip register in order
- *		to generate a LOOP activation code over a T1/E1 line.
- *----------------------------------------------------------------------------
- */
-static void falc_generate_loop_up_code(pc300_t * card, int ch)
-{
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	pc300chconf_t *conf = (pc300chconf_t *) & chan->conf;
-	falc_t *pfalc = (falc_t *) & chan->falc;
-	void __iomem *falcbase = card->hw.falcbase;
-
-	if (conf->media == IF_IFACE_T1) {
-		cpc_writeb(falcbase + F_REG(FMR5, ch),
-			   cpc_readb(falcbase + F_REG(FMR5, ch)) | FMR5_XLU);
-	} else {
-		cpc_writeb(falcbase + F_REG(FMR3, ch),
-			   cpc_readb(falcbase + F_REG(FMR3, ch)) | FMR3_XLU);
-	}
-	// EVENT_FALC_ABNORMAL
-	if (conf->media == IF_IFACE_T1) {
-		/* Disable this interrupt as it may otherwise interfere with 
-		 * other working boards. */
-		cpc_writeb(falcbase + F_REG(IMR0, ch),
-			   cpc_readb(falcbase + F_REG(IMR0, ch)) | IMR0_PDEN);
-	}
-	falc_disable_comm(card, ch);
-	// EVENT_FALC_ABNORMAL
-	pfalc->loop_gen = 1;
-}
-
-/*----------------------------------------------------------------------------
- * falc_generate_loop_down_code
- *----------------------------------------------------------------------------
- * Description:	This routine writes the proper FALC chip register in order
- *		to generate a LOOP deactivation code over a T1/E1 line.
- *----------------------------------------------------------------------------
- */
-static void falc_generate_loop_down_code(pc300_t * card, int ch)
-{
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	pc300chconf_t *conf = (pc300chconf_t *) & chan->conf;
-	falc_t *pfalc = (falc_t *) & chan->falc;
-	void __iomem *falcbase = card->hw.falcbase;
-
-	if (conf->media == IF_IFACE_T1) {
-		cpc_writeb(falcbase + F_REG(FMR5, ch),
-			   cpc_readb(falcbase + F_REG(FMR5, ch)) | FMR5_XLD);
-	} else {
-		cpc_writeb(falcbase + F_REG(FMR3, ch),
-			   cpc_readb(falcbase + F_REG(FMR3, ch)) | FMR3_XLD);
-	}
-	pfalc->sync = 0;
-	cpc_writeb(falcbase + card->hw.cpld_reg2,
-		   cpc_readb(falcbase + card->hw.cpld_reg2) &
-		   ~(CPLD_REG2_FALC_LED2 << (2 * ch)));
-	pfalc->active = 0;
-//?    falc_issue_cmd(card, ch, CMDR_XRES);
-	pfalc->loop_gen = 0;
-}
-
-/*----------------------------------------------------------------------------
- * falc_pattern_test
- *----------------------------------------------------------------------------
- * Description:	This routine generates a pattern code and checks
- *		it on the reception side.
- *----------------------------------------------------------------------------
- */
-static void falc_pattern_test(pc300_t * card, int ch, unsigned int activate)
-{
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	pc300chconf_t *conf = (pc300chconf_t *) & chan->conf;
-	falc_t *pfalc = (falc_t *) & chan->falc;
-	void __iomem *falcbase = card->hw.falcbase;
-
-	if (activate) {
-		pfalc->prbs = 1;
-		pfalc->bec = 0;
-		if (conf->media == IF_IFACE_T1) {
-			/* Disable local loop activation/deactivation detect */
-			cpc_writeb(falcbase + F_REG(IMR3, ch),
-				   cpc_readb(falcbase + F_REG(IMR3, ch)) | IMR3_LLBSC);
-		} else {
-			/* Disable local loop activation/deactivation detect */
-			cpc_writeb(falcbase + F_REG(IMR1, ch),
-				   cpc_readb(falcbase + F_REG(IMR1, ch)) | IMR1_LLBSC);
-		}
-		/* Activates generation and monitoring of PRBS 
-		 * (Pseudo Random Bit Sequence) */
-		cpc_writeb(falcbase + F_REG(LCR1, ch),
-			   cpc_readb(falcbase + F_REG(LCR1, ch)) | LCR1_EPRM | LCR1_XPRBS);
-	} else {
-		pfalc->prbs = 0;
-		/* Deactivates generation and monitoring of PRBS 
-		 * (Pseudo Random Bit Sequence) */
-		cpc_writeb(falcbase + F_REG(LCR1, ch),
-			   cpc_readb(falcbase+F_REG(LCR1,ch)) & ~(LCR1_EPRM | LCR1_XPRBS));
-		if (conf->media == IF_IFACE_T1) {
-			/* Enable local loop activation/deactivation detect */
-			cpc_writeb(falcbase + F_REG(IMR3, ch),
-				   cpc_readb(falcbase + F_REG(IMR3, ch)) & ~IMR3_LLBSC);
-		} else {
-			/* Enable local loop activation/deactivation detect */
-			cpc_writeb(falcbase + F_REG(IMR1, ch),
-				   cpc_readb(falcbase + F_REG(IMR1, ch)) & ~IMR1_LLBSC);
-		}
-	}
-}
-
-/*----------------------------------------------------------------------------
- * falc_pattern_test_error
- *----------------------------------------------------------------------------
- * Description:	This routine returns the bit error counter value
- *----------------------------------------------------------------------------
- */
-static u16 falc_pattern_test_error(pc300_t * card, int ch)
-{
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	falc_t *pfalc = (falc_t *) & chan->falc;
-
-	return pfalc->bec;
-}
-
-/**********************************/
-/***   Net Interface Routines   ***/
-/**********************************/
-
-static void
-cpc_trace(struct net_device *dev, struct sk_buff *skb_main, char rx_tx)
-{
-	struct sk_buff *skb;
-
-	if ((skb = dev_alloc_skb(10 + skb_main->len)) == NULL) {
-		printk("%s: out of memory\n", dev->name);
-		return;
-	}
-	skb_put(skb, 10 + skb_main->len);
-
-	skb->dev = dev;
-	skb->protocol = htons(ETH_P_CUST);
-	skb_reset_mac_header(skb);
-	skb->pkt_type = PACKET_HOST;
-	skb->len = 10 + skb_main->len;
-
-	skb_copy_to_linear_data(skb, dev->name, 5);
-	skb->data[5] = '[';
-	skb->data[6] = rx_tx;
-	skb->data[7] = ']';
-	skb->data[8] = ':';
-	skb->data[9] = ' ';
-	skb_copy_from_linear_data(skb_main, &skb->data[10], skb_main->len);
-
-	netif_rx(skb);
-}
-
-static void cpc_tx_timeout(struct net_device *dev)
-{
-	pc300dev_t *d = (pc300dev_t *) dev_to_hdlc(dev)->priv;
-	pc300ch_t *chan = (pc300ch_t *) d->chan;
-	pc300_t *card = (pc300_t *) chan->card;
-	int ch = chan->channel;
-	unsigned long flags;
-	u8 ilar;
-
-	dev->stats.tx_errors++;
-	dev->stats.tx_aborted_errors++;
-	CPC_LOCK(card, flags);
-	if ((ilar = cpc_readb(card->hw.scabase + ILAR)) != 0) {
-		printk("%s: ILAR=0x%x\n", dev->name, ilar);
-		cpc_writeb(card->hw.scabase + ILAR, ilar);
-		cpc_writeb(card->hw.scabase + DMER, 0x80);
-	}
-	if (card->hw.type == PC300_TE) {
-		cpc_writeb(card->hw.falcbase + card->hw.cpld_reg2,
-			   cpc_readb(card->hw.falcbase + card->hw.cpld_reg2) &
-			   ~(CPLD_REG2_FALC_LED1 << (2 * ch)));
-	}
-	dev->trans_start = jiffies; /* prevent tx timeout */
-	CPC_UNLOCK(card, flags);
-	netif_wake_queue(dev);
-}
-
-static int cpc_queue_xmit(struct sk_buff *skb, struct net_device *dev)
-{
-	pc300dev_t *d = (pc300dev_t *) dev_to_hdlc(dev)->priv;
-	pc300ch_t *chan = (pc300ch_t *) d->chan;
-	pc300_t *card = (pc300_t *) chan->card;
-	int ch = chan->channel;
-	unsigned long flags;
-#ifdef PC300_DEBUG_TX
-	int i;
-#endif
-
-	if (!netif_carrier_ok(dev)) {
-		/* DCD must be OFF: drop packet */
-		dev_kfree_skb(skb);
-		dev->stats.tx_errors++;
-		dev->stats.tx_carrier_errors++;
-		return 0;
-	} else if (cpc_readb(card->hw.scabase + M_REG(ST3, ch)) & ST3_DCD) {
-		printk("%s: DCD is OFF. Going administrative down.\n", dev->name);
-		dev->stats.tx_errors++;
-		dev->stats.tx_carrier_errors++;
-		dev_kfree_skb(skb);
-		netif_carrier_off(dev);
-		CPC_LOCK(card, flags);
-		cpc_writeb(card->hw.scabase + M_REG(CMD, ch), CMD_TX_BUF_CLR);
-		if (card->hw.type == PC300_TE) {
-			cpc_writeb(card->hw.falcbase + card->hw.cpld_reg2,
-				   cpc_readb(card->hw.falcbase + card->hw.cpld_reg2) & 
-				   			~(CPLD_REG2_FALC_LED1 << (2 * ch)));
-		}
-		CPC_UNLOCK(card, flags);
-		netif_wake_queue(dev);
-		return 0;
-	}
-
-	/* Write buffer to DMA buffers */
-	if (dma_buf_write(card, ch, (u8 *)skb->data, skb->len) != 0) {
-//		printk("%s: write error. Dropping TX packet.\n", dev->name);
-		netif_stop_queue(dev);
-		dev_kfree_skb(skb);
-		dev->stats.tx_errors++;
-		dev->stats.tx_dropped++;
-		return 0;
-	}
-#ifdef PC300_DEBUG_TX
-	printk("%s T:", dev->name);
-	for (i = 0; i < skb->len; i++)
-		printk(" %02x", *(skb->data + i));
-	printk("\n");
-#endif
-
-	if (d->trace_on) {
-		cpc_trace(dev, skb, 'T');
-	}
-
-	/* Start transmission */
-	CPC_LOCK(card, flags);
-	/* verify if it has more than one free descriptor */
-	if (card->chan[ch].nfree_tx_bd <= 1) {
-		/* don't have so stop the queue */
-		netif_stop_queue(dev);
-	}
-	cpc_writel(card->hw.scabase + DTX_REG(EDAL, ch),
-		   TX_BD_ADDR(ch, chan->tx_next_bd));
-	cpc_writeb(card->hw.scabase + M_REG(CMD, ch), CMD_TX_ENA);
-	cpc_writeb(card->hw.scabase + DSR_TX(ch), DSR_DE);
-	if (card->hw.type == PC300_TE) {
-		cpc_writeb(card->hw.falcbase + card->hw.cpld_reg2,
-			   cpc_readb(card->hw.falcbase + card->hw.cpld_reg2) |
-			   (CPLD_REG2_FALC_LED1 << (2 * ch)));
-	}
-	CPC_UNLOCK(card, flags);
-	dev_kfree_skb(skb);
-
-	return 0;
-}
-
-static void cpc_net_rx(struct net_device *dev)
-{
-	pc300dev_t *d = (pc300dev_t *) dev_to_hdlc(dev)->priv;
-	pc300ch_t *chan = (pc300ch_t *) d->chan;
-	pc300_t *card = (pc300_t *) chan->card;
-	int ch = chan->channel;
-#ifdef PC300_DEBUG_RX
-	int i;
-#endif
-	int rxb;
-	struct sk_buff *skb;
-
-	while (1) {
-		if ((rxb = dma_get_rx_frame_size(card, ch)) == -1)
-			return;
-
-		if (!netif_carrier_ok(dev)) {
-			/* DCD must be OFF: drop packet */
-		    printk("%s : DCD is OFF - drop %d rx bytes\n", dev->name, rxb); 
-			skb = NULL;
-		} else {
-			if (rxb > (dev->mtu + 40)) { /* add headers */
-				printk("%s : MTU exceeded %d\n", dev->name, rxb); 
-				skb = NULL;
-			} else {
-				skb = dev_alloc_skb(rxb);
-				if (skb == NULL) {
-					printk("%s: Memory squeeze!!\n", dev->name);
-					return;
-				}
-				skb->dev = dev;
-			}
-		}
-
-		if (((rxb = dma_buf_read(card, ch, skb)) <= 0) || (skb == NULL)) {
-#ifdef PC300_DEBUG_RX
-			printk("%s: rxb = %x\n", dev->name, rxb);
-#endif
-			if ((skb == NULL) && (rxb > 0)) {
-				/* rxb > dev->mtu */
-				dev->stats.rx_errors++;
-				dev->stats.rx_length_errors++;
-				continue;
-			}
-
-			if (rxb < 0) {	/* Invalid frame */
-				rxb = -rxb;
-				if (rxb & DST_OVR) {
-					dev->stats.rx_errors++;
-					dev->stats.rx_fifo_errors++;
-				}
-				if (rxb & DST_CRC) {
-					dev->stats.rx_errors++;
-					dev->stats.rx_crc_errors++;
-				}
-				if (rxb & (DST_RBIT | DST_SHRT | DST_ABT)) {
-					dev->stats.rx_errors++;
-					dev->stats.rx_frame_errors++;
-				}
-			}
-			if (skb) {
-				dev_kfree_skb_irq(skb);
-			}
-			continue;
-		}
-
-		dev->stats.rx_bytes += rxb;
-
-#ifdef PC300_DEBUG_RX
-		printk("%s R:", dev->name);
-		for (i = 0; i < skb->len; i++)
-			printk(" %02x", *(skb->data + i));
-		printk("\n");
-#endif
-		if (d->trace_on) {
-			cpc_trace(dev, skb, 'R');
-		}
-		dev->stats.rx_packets++;
-		skb->protocol = hdlc_type_trans(skb, dev);
-		netif_rx(skb);
-	}
-}
-
-/************************************/
-/***   PC300 Interrupt Routines   ***/
-/************************************/
-static void sca_tx_intr(pc300dev_t *dev)
-{
-	pc300ch_t *chan = (pc300ch_t *)dev->chan; 
-	pc300_t *card = (pc300_t *)chan->card; 
-	int ch = chan->channel; 
-	volatile pcsca_bd_t __iomem * ptdescr; 
-
-    /* Clean up descriptors from previous transmission */
-	ptdescr = (card->hw.rambase +
-						TX_BD_ADDR(ch,chan->tx_first_bd));
-	while ((cpc_readl(card->hw.scabase + DTX_REG(CDAL,ch)) !=
-		TX_BD_ADDR(ch,chan->tx_first_bd)) &&
-	       (cpc_readb(&ptdescr->status) & DST_OSB)) {
-		dev->dev->stats.tx_packets++;
-		dev->dev->stats.tx_bytes += cpc_readw(&ptdescr->len);
-		cpc_writeb(&ptdescr->status, DST_OSB);
-		cpc_writew(&ptdescr->len, 0);
-		chan->nfree_tx_bd++;
-		chan->tx_first_bd = (chan->tx_first_bd + 1) & (N_DMA_TX_BUF - 1);
-		ptdescr = (card->hw.rambase + TX_BD_ADDR(ch,chan->tx_first_bd));
-    }
-
-#ifdef CONFIG_PC300_MLPPP
-	if (chan->conf.proto == PC300_PROTO_MLPPP) {
-			cpc_tty_trigger_poll(dev);
-	} else {
-#endif
-	/* Tell the upper layer we are ready to transmit more packets */
-		netif_wake_queue(dev->dev);
-#ifdef CONFIG_PC300_MLPPP
-	}
-#endif
-}
-
-static void sca_intr(pc300_t * card)
-{
-	void __iomem *scabase = card->hw.scabase;
-	volatile u32 status;
-	int ch;
-	int intr_count = 0;
-	unsigned char dsr_rx;
-
-	while ((status = cpc_readl(scabase + ISR0)) != 0) {
-		for (ch = 0; ch < card->hw.nchan; ch++) {
-			pc300ch_t *chan = &card->chan[ch];
-			pc300dev_t *d = &chan->d;
-			struct net_device *dev = d->dev;
-
-			spin_lock(&card->card_lock);
-
-	    /**** Reception ****/
-			if (status & IR0_DRX((IR0_DMIA | IR0_DMIB), ch)) {
-				u8 drx_stat = cpc_readb(scabase + DSR_RX(ch));
-
-				/* Clear RX interrupts */
-				cpc_writeb(scabase + DSR_RX(ch), drx_stat | DSR_DWE);
-
-#ifdef PC300_DEBUG_INTR
-				printk ("sca_intr: RX intr chan[%d] (st=0x%08lx, dsr=0x%02x)\n",
-					 ch, status, drx_stat);
-#endif
-				if (status & IR0_DRX(IR0_DMIA, ch)) {
-					if (drx_stat & DSR_BOF) {
-#ifdef CONFIG_PC300_MLPPP
-						if (chan->conf.proto == PC300_PROTO_MLPPP) {
-							/* verify if driver is TTY */
-							if ((cpc_readb(scabase + DSR_RX(ch)) & DSR_DE)) {
-								rx_dma_stop(card, ch);
-							}
-							cpc_tty_receive(d);
-							rx_dma_start(card, ch);
-						} else 
-#endif
-						{
-							if ((cpc_readb(scabase + DSR_RX(ch)) & DSR_DE)) {
-								rx_dma_stop(card, ch);
-							}
-							cpc_net_rx(dev);
-							/* Discard invalid frames */
-							dev->stats.rx_errors++;
-							dev->stats.rx_over_errors++;
-							chan->rx_first_bd = 0;
-							chan->rx_last_bd = N_DMA_RX_BUF - 1;
-							rx_dma_start(card, ch);
-						}
-					}
-				}
-				if (status & IR0_DRX(IR0_DMIB, ch)) {
-					if (drx_stat & DSR_EOM) {
-						if (card->hw.type == PC300_TE) {
-							cpc_writeb(card->hw.falcbase +
-								   card->hw.cpld_reg2,
-								   cpc_readb (card->hw.falcbase +
-								    	card->hw.cpld_reg2) |
-								   (CPLD_REG2_FALC_LED1 << (2 * ch)));
-						}
-#ifdef CONFIG_PC300_MLPPP
-						if (chan->conf.proto == PC300_PROTO_MLPPP) {
-							/* verify if driver is TTY */
-							cpc_tty_receive(d);
-						} else {
-							cpc_net_rx(dev);
-						}
-#else
-						cpc_net_rx(dev);
-#endif
-						if (card->hw.type == PC300_TE) {
-							cpc_writeb(card->hw.falcbase +
-								   card->hw.cpld_reg2,
-								   cpc_readb (card->hw.falcbase +
-								    		card->hw.cpld_reg2) &
-								   ~ (CPLD_REG2_FALC_LED1 << (2 * ch)));
-						}
-					}
-				}
-				if (!(dsr_rx = cpc_readb(scabase + DSR_RX(ch)) & DSR_DE)) {
-#ifdef PC300_DEBUG_INTR
-		printk("%s: RX intr chan[%d] (st=0x%08lx, dsr=0x%02x, dsr2=0x%02x)\n",
-			dev->name, ch, status, drx_stat, dsr_rx);
-#endif
-					cpc_writeb(scabase + DSR_RX(ch), (dsr_rx | DSR_DE) & 0xfe);
-				}
-			}
-
-	    /**** Transmission ****/
-			if (status & IR0_DTX((IR0_EFT | IR0_DMIA | IR0_DMIB), ch)) {
-				u8 dtx_stat = cpc_readb(scabase + DSR_TX(ch));
-
-				/* Clear TX interrupts */
-				cpc_writeb(scabase + DSR_TX(ch), dtx_stat | DSR_DWE);
-
-#ifdef PC300_DEBUG_INTR
-				printk ("sca_intr: TX intr chan[%d] (st=0x%08lx, dsr=0x%02x)\n",
-					 ch, status, dtx_stat);
-#endif
-				if (status & IR0_DTX(IR0_EFT, ch)) {
-					if (dtx_stat & DSR_UDRF) {
-						if (cpc_readb (scabase + M_REG(TBN, ch)) != 0) {
-							cpc_writeb(scabase + M_REG(CMD,ch), CMD_TX_BUF_CLR);
-						}
-						if (card->hw.type == PC300_TE) {
-							cpc_writeb(card->hw.falcbase + card->hw.cpld_reg2,
-								   cpc_readb (card->hw.falcbase + 
-										   card->hw.cpld_reg2) &
-								   ~ (CPLD_REG2_FALC_LED1 << (2 * ch)));
-						}
-						dev->stats.tx_errors++;
-						dev->stats.tx_fifo_errors++;
-						sca_tx_intr(d);
-					}
-				}
-				if (status & IR0_DTX(IR0_DMIA, ch)) {
-					if (dtx_stat & DSR_BOF) {
-					}
-				}
-				if (status & IR0_DTX(IR0_DMIB, ch)) {
-					if (dtx_stat & DSR_EOM) {
-						if (card->hw.type == PC300_TE) {
-							cpc_writeb(card->hw.falcbase + card->hw.cpld_reg2,
-								   cpc_readb (card->hw.falcbase +
-								    			card->hw.cpld_reg2) &
-								   ~ (CPLD_REG2_FALC_LED1 << (2 * ch)));
-						}
-						sca_tx_intr(d);
-					}
-				}
-			}
-
-	    /**** MSCI ****/
-			if (status & IR0_M(IR0_RXINTA, ch)) {
-				u8 st1 = cpc_readb(scabase + M_REG(ST1, ch));
-
-				/* Clear MSCI interrupts */
-				cpc_writeb(scabase + M_REG(ST1, ch), st1);
-
-#ifdef PC300_DEBUG_INTR
-				printk("sca_intr: MSCI intr chan[%d] (st=0x%08lx, st1=0x%02x)\n",
-					 ch, status, st1);
-#endif
-				if (st1 & ST1_CDCD) {	/* DCD changed */
-					if (cpc_readb(scabase + M_REG(ST3, ch)) & ST3_DCD) {
-						printk ("%s: DCD is OFF. Going administrative down.\n",
-							 dev->name);
-#ifdef CONFIG_PC300_MLPPP
-						if (chan->conf.proto != PC300_PROTO_MLPPP) {
-							netif_carrier_off(dev);
-						}
-#else
-						netif_carrier_off(dev);
-
-#endif
-						card->chan[ch].d.line_off++;
-					} else {	/* DCD = 1 */
-						printk ("%s: DCD is ON. Going administrative up.\n",
-							 dev->name);
-#ifdef CONFIG_PC300_MLPPP
-						if (chan->conf.proto != PC300_PROTO_MLPPP)
-							/* verify if driver is not TTY */
-#endif
-							netif_carrier_on(dev);
-						card->chan[ch].d.line_on++;
-					}
-				}
-			}
-			spin_unlock(&card->card_lock);
-		}
-		if (++intr_count == 10)
-			/* Too much work at this board. Force exit */
-			break;
-	}
-}
-
-static void falc_t1_loop_detection(pc300_t *card, int ch, u8 frs1)
-{
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	falc_t *pfalc = (falc_t *) & chan->falc;
-	void __iomem *falcbase = card->hw.falcbase;
-
-	if (((cpc_readb(falcbase + F_REG(LCR1, ch)) & LCR1_XPRBS) == 0) &&
-	    !pfalc->loop_gen) {
-		if (frs1 & FRS1_LLBDD) {
-			// A Line Loop Back Deactivation signal detected
-			if (pfalc->loop_active) {
-				falc_remote_loop(card, ch, 0);
-			}
-		} else {
-			if ((frs1 & FRS1_LLBAD) &&
-			    ((cpc_readb(falcbase + F_REG(LCR1, ch)) & LCR1_EPRM) == 0)) {
-				// A Line Loop Back Activation signal detected  
-				if (!pfalc->loop_active) {
-					falc_remote_loop(card, ch, 1);
-				}
-			}
-		}
-	}
-}
-
-static void falc_e1_loop_detection(pc300_t *card, int ch, u8 rsp)
-{
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	falc_t *pfalc = (falc_t *) & chan->falc;
-	void __iomem *falcbase = card->hw.falcbase;
-
-	if (((cpc_readb(falcbase + F_REG(LCR1, ch)) & LCR1_XPRBS) == 0) &&
-	    !pfalc->loop_gen) {
-		if (rsp & RSP_LLBDD) {
-			// A Line Loop Back Deactivation signal detected
-			if (pfalc->loop_active) {
-				falc_remote_loop(card, ch, 0);
-			}
-		} else {
-			if ((rsp & RSP_LLBAD) &&
-			    ((cpc_readb(falcbase + F_REG(LCR1, ch)) & LCR1_EPRM) == 0)) {
-				// A Line Loop Back Activation signal detected  
-				if (!pfalc->loop_active) {
-					falc_remote_loop(card, ch, 1);
-				}
-			}
-		}
-	}
-}
-
-static void falc_t1_intr(pc300_t * card, int ch)
-{
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	falc_t *pfalc = (falc_t *) & chan->falc;
-	void __iomem *falcbase = card->hw.falcbase;
-	u8 isr0, isr3, gis;
-	u8 dummy;
-
-	while ((gis = cpc_readb(falcbase + F_REG(GIS, ch))) != 0) {
-		if (gis & GIS_ISR0) {
-			isr0 = cpc_readb(falcbase + F_REG(FISR0, ch));
-			if (isr0 & FISR0_PDEN) {
-				/* Read the bit to clear the situation */
-				if (cpc_readb(falcbase + F_REG(FRS1, ch)) &
-				    FRS1_PDEN) {
-					pfalc->pden++;
-				}
-			}
-		}
-
-		if (gis & GIS_ISR1) {
-			dummy = cpc_readb(falcbase + F_REG(FISR1, ch));
-		}
-
-		if (gis & GIS_ISR2) {
-			dummy = cpc_readb(falcbase + F_REG(FISR2, ch));
-		}
-
-		if (gis & GIS_ISR3) {
-			isr3 = cpc_readb(falcbase + F_REG(FISR3, ch));
-			if (isr3 & FISR3_SEC) {
-				pfalc->sec++;
-				falc_update_stats(card, ch);
-				falc_check_status(card, ch,
-						  cpc_readb(falcbase + F_REG(FRS0, ch)));
-			}
-			if (isr3 & FISR3_ES) {
-				pfalc->es++;
-			}
-			if (isr3 & FISR3_LLBSC) {
-				falc_t1_loop_detection(card, ch,
-						       cpc_readb(falcbase + F_REG(FRS1, ch)));
-			}
-		}
-	}
-}
-
-static void falc_e1_intr(pc300_t * card, int ch)
-{
-	pc300ch_t *chan = (pc300ch_t *) & card->chan[ch];
-	falc_t *pfalc = (falc_t *) & chan->falc;
-	void __iomem *falcbase = card->hw.falcbase;
-	u8 isr1, isr2, isr3, gis, rsp;
-	u8 dummy;
-
-	while ((gis = cpc_readb(falcbase + F_REG(GIS, ch))) != 0) {
-		rsp = cpc_readb(falcbase + F_REG(RSP, ch));
-
-		if (gis & GIS_ISR0) {
-			dummy = cpc_readb(falcbase + F_REG(FISR0, ch));
-		}
-		if (gis & GIS_ISR1) {
-			isr1 = cpc_readb(falcbase + F_REG(FISR1, ch));
-			if (isr1 & FISR1_XMB) {
-				if ((pfalc->xmb_cause & 2) &&
-				    pfalc->multiframe_mode) {
-					if (cpc_readb (falcbase + F_REG(FRS0, ch)) & 
-									(FRS0_LOS | FRS0_AIS | FRS0_LFA)) {
-						cpc_writeb(falcbase + F_REG(XSP, ch),
-							   cpc_readb(falcbase + F_REG(XSP, ch))
-							   & ~XSP_AXS);
-					} else {
-						cpc_writeb(falcbase + F_REG(XSP, ch),
-							   cpc_readb(falcbase + F_REG(XSP, ch))
-							   | XSP_AXS);
-					}
-				}
-				pfalc->xmb_cause = 0;
-				cpc_writeb(falcbase + F_REG(IMR1, ch),
-					   cpc_readb(falcbase + F_REG(IMR1, ch)) | IMR1_XMB);
-			}
-			if (isr1 & FISR1_LLBSC) {
-				falc_e1_loop_detection(card, ch, rsp);
-			}
-		}
-		if (gis & GIS_ISR2) {
-			isr2 = cpc_readb(falcbase + F_REG(FISR2, ch));
-			if (isr2 & FISR2_T400MS) {
-				cpc_writeb(falcbase + F_REG(XSW, ch),
-					   cpc_readb(falcbase + F_REG(XSW, ch)) | XSW_XRA);
-			}
-			if (isr2 & FISR2_MFAR) {
-				cpc_writeb(falcbase + F_REG(XSW, ch),
-					   cpc_readb(falcbase + F_REG(XSW, ch)) & ~XSW_XRA);
-			}
-			if (isr2 & (FISR2_FAR | FISR2_LFA | FISR2_AIS | FISR2_LOS)) {
-				pfalc->xmb_cause |= 2;
-				cpc_writeb(falcbase + F_REG(IMR1, ch),
-					   cpc_readb(falcbase + F_REG(IMR1, ch)) & ~IMR1_XMB);
-			}
-		}
-		if (gis & GIS_ISR3) {
-			isr3 = cpc_readb(falcbase + F_REG(FISR3, ch));
-			if (isr3 & FISR3_SEC) {
-				pfalc->sec++;
-				falc_update_stats(card, ch);
-				falc_check_status(card, ch,
-						  cpc_readb(falcbase + F_REG(FRS0, ch)));
-			}
-			if (isr3 & FISR3_ES) {
-				pfalc->es++;
-			}
-		}
-	}
-}
-
-static void falc_intr(pc300_t * card)
-{
-	int ch;
-
-	for (ch = 0; ch < card->hw.nchan; ch++) {
-		pc300ch_t *chan = &card->chan[ch];
-		pc300chconf_t *conf = (pc300chconf_t *) & chan->conf;
-
-		if (conf->media == IF_IFACE_T1) {
-			falc_t1_intr(card, ch);
-		} else {
-			falc_e1_intr(card, ch);
-		}
-	}
-}
-
-static irqreturn_t cpc_intr(int irq, void *dev_id)
-{
-	pc300_t *card = dev_id;
-	volatile u8 plx_status;
-
-	if (!card) {
-#ifdef PC300_DEBUG_INTR
-		printk("cpc_intr: spurious intr %d\n", irq);
-#endif
-		return IRQ_NONE;		/* spurious intr */
-	}
-
-	if (!card->hw.rambase) {
-#ifdef PC300_DEBUG_INTR
-		printk("cpc_intr: spurious intr2 %d\n", irq);
-#endif
-		return IRQ_NONE;		/* spurious intr */
-	}
-
-	switch (card->hw.type) {
-		case PC300_RSV:
-		case PC300_X21:
-			sca_intr(card);
-			break;
-
-		case PC300_TE:
-			while ( (plx_status = (cpc_readb(card->hw.plxbase + card->hw.intctl_reg) &
-				 (PLX_9050_LINT1_STATUS | PLX_9050_LINT2_STATUS))) != 0) {
-				if (plx_status & PLX_9050_LINT1_STATUS) {	/* SCA Interrupt */
-					sca_intr(card);
-				}
-				if (plx_status & PLX_9050_LINT2_STATUS) {	/* FALC Interrupt */
-					falc_intr(card);
-				}
-			}
-			break;
-	}
-	return IRQ_HANDLED;
-}
-
-static void cpc_sca_status(pc300_t * card, int ch)
-{
-	u8 ilar;
-	void __iomem *scabase = card->hw.scabase;
-	unsigned long flags;
-
-	tx_dma_buf_check(card, ch);
-	rx_dma_buf_check(card, ch);
-	ilar = cpc_readb(scabase + ILAR);
-	printk ("ILAR=0x%02x, WCRL=0x%02x, PCR=0x%02x, BTCR=0x%02x, BOLR=0x%02x\n",
-		 ilar, cpc_readb(scabase + WCRL), cpc_readb(scabase + PCR),
-		 cpc_readb(scabase + BTCR), cpc_readb(scabase + BOLR));
-	printk("TX_CDA=0x%08x, TX_EDA=0x%08x\n",
-	       cpc_readl(scabase + DTX_REG(CDAL, ch)),
-	       cpc_readl(scabase + DTX_REG(EDAL, ch)));
-	printk("RX_CDA=0x%08x, RX_EDA=0x%08x, BFL=0x%04x\n",
-	       cpc_readl(scabase + DRX_REG(CDAL, ch)),
-	       cpc_readl(scabase + DRX_REG(EDAL, ch)),
-	       cpc_readw(scabase + DRX_REG(BFLL, ch)));
-	printk("DMER=0x%02x, DSR_TX=0x%02x, DSR_RX=0x%02x\n",
-	       cpc_readb(scabase + DMER), cpc_readb(scabase + DSR_TX(ch)),
-	       cpc_readb(scabase + DSR_RX(ch)));
-	printk("DMR_TX=0x%02x, DMR_RX=0x%02x, DIR_TX=0x%02x, DIR_RX=0x%02x\n",
-	       cpc_readb(scabase + DMR_TX(ch)), cpc_readb(scabase + DMR_RX(ch)),
-	       cpc_readb(scabase + DIR_TX(ch)),
-	       cpc_readb(scabase + DIR_RX(ch)));
-	printk("DCR_TX=0x%02x, DCR_RX=0x%02x, FCT_TX=0x%02x, FCT_RX=0x%02x\n",
-	       cpc_readb(scabase + DCR_TX(ch)), cpc_readb(scabase + DCR_RX(ch)),
-	       cpc_readb(scabase + FCT_TX(ch)),
-	       cpc_readb(scabase + FCT_RX(ch)));
-	printk("MD0=0x%02x, MD1=0x%02x, MD2=0x%02x, MD3=0x%02x, IDL=0x%02x\n",
-	       cpc_readb(scabase + M_REG(MD0, ch)),
-	       cpc_readb(scabase + M_REG(MD1, ch)),
-	       cpc_readb(scabase + M_REG(MD2, ch)),
-	       cpc_readb(scabase + M_REG(MD3, ch)),
-	       cpc_readb(scabase + M_REG(IDL, ch)));
-	printk("CMD=0x%02x, SA0=0x%02x, SA1=0x%02x, TFN=0x%02x, CTL=0x%02x\n",
-	       cpc_readb(scabase + M_REG(CMD, ch)),
-	       cpc_readb(scabase + M_REG(SA0, ch)),
-	       cpc_readb(scabase + M_REG(SA1, ch)),
-	       cpc_readb(scabase + M_REG(TFN, ch)),
-	       cpc_readb(scabase + M_REG(CTL, ch)));
-	printk("ST0=0x%02x, ST1=0x%02x, ST2=0x%02x, ST3=0x%02x, ST4=0x%02x\n",
-	       cpc_readb(scabase + M_REG(ST0, ch)),
-	       cpc_readb(scabase + M_REG(ST1, ch)),
-	       cpc_readb(scabase + M_REG(ST2, ch)),
-	       cpc_readb(scabase + M_REG(ST3, ch)),
-	       cpc_readb(scabase + M_REG(ST4, ch)));
-	printk ("CST0=0x%02x, CST1=0x%02x, CST2=0x%02x, CST3=0x%02x, FST=0x%02x\n",
-		 cpc_readb(scabase + M_REG(CST0, ch)),
-		 cpc_readb(scabase + M_REG(CST1, ch)),
-		 cpc_readb(scabase + M_REG(CST2, ch)),
-		 cpc_readb(scabase + M_REG(CST3, ch)),
-		 cpc_readb(scabase + M_REG(FST, ch)));
-	printk("TRC0=0x%02x, TRC1=0x%02x, RRC=0x%02x, TBN=0x%02x, RBN=0x%02x\n",
-	       cpc_readb(scabase + M_REG(TRC0, ch)),
-	       cpc_readb(scabase + M_REG(TRC1, ch)),
-	       cpc_readb(scabase + M_REG(RRC, ch)),
-	       cpc_readb(scabase + M_REG(TBN, ch)),
-	       cpc_readb(scabase + M_REG(RBN, ch)));
-	printk("TFS=0x%02x, TNR0=0x%02x, TNR1=0x%02x, RNR=0x%02x\n",
-	       cpc_readb(scabase + M_REG(TFS, ch)),
-	       cpc_readb(scabase + M_REG(TNR0, ch)),
-	       cpc_readb(scabase + M_REG(TNR1, ch)),
-	       cpc_readb(scabase + M_REG(RNR, ch)));
-	printk("TCR=0x%02x, RCR=0x%02x, TNR1=0x%02x, RNR=0x%02x\n",
-	       cpc_readb(scabase + M_REG(TCR, ch)),
-	       cpc_readb(scabase + M_REG(RCR, ch)),
-	       cpc_readb(scabase + M_REG(TNR1, ch)),
-	       cpc_readb(scabase + M_REG(RNR, ch)));
-	printk("TXS=0x%02x, RXS=0x%02x, EXS=0x%02x, TMCT=0x%02x, TMCR=0x%02x\n",
-	       cpc_readb(scabase + M_REG(TXS, ch)),
-	       cpc_readb(scabase + M_REG(RXS, ch)),
-	       cpc_readb(scabase + M_REG(EXS, ch)),
-	       cpc_readb(scabase + M_REG(TMCT, ch)),
-	       cpc_readb(scabase + M_REG(TMCR, ch)));
-	printk("IE0=0x%02x, IE1=0x%02x, IE2=0x%02x, IE4=0x%02x, FIE=0x%02x\n",
-	       cpc_readb(scabase + M_REG(IE0, ch)),
-	       cpc_readb(scabase + M_REG(IE1, ch)),
-	       cpc_readb(scabase + M_REG(IE2, ch)),
-	       cpc_readb(scabase + M_REG(IE4, ch)),
-	       cpc_readb(scabase + M_REG(FIE, ch)));
-	printk("IER0=0x%08x\n", cpc_readl(scabase + IER0));
-
-	if (ilar != 0) {
-		CPC_LOCK(card, flags);
-		cpc_writeb(scabase + ILAR, ilar);
-		cpc_writeb(scabase + DMER, 0x80);
-		CPC_UNLOCK(card, flags);
-	}
-}
-
-static void cpc_falc_status(pc300_t * card, int ch)
-{
-	pc300ch_t *chan = &card->chan[ch];
-	falc_t *pfalc = (falc_t *) & chan->falc;
-	unsigned long flags;
-
-	CPC_LOCK(card, flags);
-	printk("CH%d:   %s %s  %d channels\n",
-	       ch, (pfalc->sync ? "SYNC" : ""), (pfalc->active ? "ACTIVE" : ""),
-	       pfalc->num_channels);
-
-	printk("        pden=%d,  los=%d,  losr=%d,  lfa=%d,  farec=%d\n",
-	       pfalc->pden, pfalc->los, pfalc->losr, pfalc->lfa, pfalc->farec);
-	printk("        lmfa=%d,  ais=%d,  sec=%d,  es=%d,  rai=%d\n",
-	       pfalc->lmfa, pfalc->ais, pfalc->sec, pfalc->es, pfalc->rai);
-	printk("        bec=%d,  fec=%d,  cvc=%d,  cec=%d,  ebc=%d\n",
-	       pfalc->bec, pfalc->fec, pfalc->cvc, pfalc->cec, pfalc->ebc);
-
-	printk("\n");
-	printk("        STATUS: %s  %s  %s  %s  %s  %s\n",
-	       (pfalc->red_alarm ? "RED" : ""),
-	       (pfalc->blue_alarm ? "BLU" : ""),
-	       (pfalc->yellow_alarm ? "YEL" : ""),
-	       (pfalc->loss_fa ? "LFA" : ""),
-	       (pfalc->loss_mfa ? "LMF" : ""), (pfalc->prbs ? "PRB" : ""));
-	CPC_UNLOCK(card, flags);
-}
-
-static int cpc_change_mtu(struct net_device *dev, int new_mtu)
-{
-	if ((new_mtu < 128) || (new_mtu > PC300_DEF_MTU))
-		return -EINVAL;
-	dev->mtu = new_mtu;
-	return 0;
-}
-
-static int cpc_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
-{
-	pc300dev_t *d = (pc300dev_t *) dev_to_hdlc(dev)->priv;
-	pc300ch_t *chan = (pc300ch_t *) d->chan;
-	pc300_t *card = (pc300_t *) chan->card;
-	pc300conf_t conf_aux;
-	pc300chconf_t *conf = (pc300chconf_t *) & chan->conf;
-	int ch = chan->channel;
-	void __user *arg = ifr->ifr_data;
-	struct if_settings *settings = &ifr->ifr_settings;
-	void __iomem *scabase = card->hw.scabase;
-
-	if (!capable(CAP_NET_ADMIN))
-		return -EPERM;
-
-	switch (cmd) {
-		case SIOCGPC300CONF:
-#ifdef CONFIG_PC300_MLPPP
-			if (conf->proto != PC300_PROTO_MLPPP) {
-				conf->proto = /* FIXME hdlc->proto.id */ 0;
-			}
-#else
-			conf->proto = /* FIXME hdlc->proto.id */ 0;
-#endif
-			memcpy(&conf_aux.conf, conf, sizeof(pc300chconf_t));
-			memcpy(&conf_aux.hw, &card->hw, sizeof(pc300hw_t));
-			if (!arg || 
-				copy_to_user(arg, &conf_aux, sizeof(pc300conf_t))) 
-				return -EINVAL;
-			return 0;
-		case SIOCSPC300CONF:
-			if (!capable(CAP_NET_ADMIN))
-				return -EPERM;
-			if (!arg || 
-				copy_from_user(&conf_aux.conf, arg, sizeof(pc300chconf_t)))
-				return -EINVAL;
-			if (card->hw.cpld_id < 0x02 &&
-			    conf_aux.conf.fr_mode == PC300_FR_UNFRAMED) {
-				/* CPLD_ID < 0x02 doesn't support Unframed E1 */
-				return -EINVAL;
-			}
-#ifdef CONFIG_PC300_MLPPP
-			if (conf_aux.conf.proto == PC300_PROTO_MLPPP) {
-				if (conf->proto != PC300_PROTO_MLPPP) {
-					memcpy(conf, &conf_aux.conf, sizeof(pc300chconf_t));
-					cpc_tty_init(d);	/* init TTY driver */
-				}
-			} else {
-				if (conf_aux.conf.proto == 0xffff) {
-					if (conf->proto == PC300_PROTO_MLPPP){ 
-						/* ifdown interface */
-						cpc_close(dev);
-					}
-				} else {
-					memcpy(conf, &conf_aux.conf, sizeof(pc300chconf_t));
-					/* FIXME hdlc->proto.id = conf->proto; */
-				}
-			}
-#else
-			memcpy(conf, &conf_aux.conf, sizeof(pc300chconf_t));
-			/* FIXME hdlc->proto.id = conf->proto; */
-#endif
-			return 0;
-		case SIOCGPC300STATUS:
-			cpc_sca_status(card, ch);
-			return 0;
-		case SIOCGPC300FALCSTATUS:
-			cpc_falc_status(card, ch);
-			return 0;
-
-		case SIOCGPC300UTILSTATS:
-			{
-				if (!arg) {	/* clear statistics */
-					memset(&dev->stats, 0, sizeof(dev->stats));
-					if (card->hw.type == PC300_TE) {
-						memset(&chan->falc, 0, sizeof(falc_t));
-					}
-				} else {
-					pc300stats_t pc300stats;
-
-					memset(&pc300stats, 0, sizeof(pc300stats_t));
-					pc300stats.hw_type = card->hw.type;
-					pc300stats.line_on = card->chan[ch].d.line_on;
-					pc300stats.line_off = card->chan[ch].d.line_off;
-					memcpy(&pc300stats.gen_stats, &dev->stats,
-					       sizeof(dev->stats));
-					if (card->hw.type == PC300_TE)
-						memcpy(&pc300stats.te_stats,&chan->falc,sizeof(falc_t));
-				    	if (copy_to_user(arg, &pc300stats, sizeof(pc300stats_t)))
-						return -EFAULT;
-				}
-				return 0;
-			}
-
-		case SIOCGPC300UTILSTATUS:
-			{
-				struct pc300status pc300status;
-
-				pc300status.hw_type = card->hw.type;
-				if (card->hw.type == PC300_TE) {
-					pc300status.te_status.sync = chan->falc.sync;
-					pc300status.te_status.red_alarm = chan->falc.red_alarm;
-					pc300status.te_status.blue_alarm = chan->falc.blue_alarm;
-					pc300status.te_status.loss_fa = chan->falc.loss_fa;
-					pc300status.te_status.yellow_alarm =chan->falc.yellow_alarm;
-					pc300status.te_status.loss_mfa = chan->falc.loss_mfa;
-					pc300status.te_status.prbs = chan->falc.prbs;
-				} else {
-					pc300status.gen_status.dcd =
-						!(cpc_readb (scabase + M_REG(ST3, ch)) & ST3_DCD);
-					pc300status.gen_status.cts =
-						!(cpc_readb (scabase + M_REG(ST3, ch)) & ST3_CTS);
-					pc300status.gen_status.rts =
-						!(cpc_readb (scabase + M_REG(CTL, ch)) & CTL_RTS);
-					pc300status.gen_status.dtr =
-						!(cpc_readb (scabase + M_REG(CTL, ch)) & CTL_DTR);
-					/* There is no DSR in HD64572 */
-				}
-				if (!arg ||
-				    copy_to_user(arg, &pc300status, sizeof(pc300status_t)))
-					return -EINVAL;
-				return 0;
-			}
-
-		case SIOCSPC300TRACE:
-			/* Sets/resets a trace_flag for the respective device */
-			if (!arg || copy_from_user(&d->trace_on, arg,sizeof(unsigned char)))
-					return -EINVAL;
-			return 0;
-
-		case SIOCSPC300LOOPBACK:
-			{
-				struct pc300loopback pc300loop;
-
-				/* TE boards only */
-				if (card->hw.type != PC300_TE)
-					return -EINVAL;
-
-				if (!arg || 
-					copy_from_user(&pc300loop, arg, sizeof(pc300loopback_t)))
-						return -EINVAL;
-				switch (pc300loop.loop_type) {
-					case PC300LOCLOOP:	/* Turn the local loop on/off */
-						falc_local_loop(card, ch, pc300loop.loop_on);
-						return 0;
-
-					case PC300REMLOOP:	/* Turn the remote loop on/off */
-						falc_remote_loop(card, ch, pc300loop.loop_on);
-						return 0;
-
-					case PC300PAYLOADLOOP:	/* Turn the payload loop on/off */
-						falc_payload_loop(card, ch, pc300loop.loop_on);
-						return 0;
-
-					case PC300GENLOOPUP:	/* Generate loop UP */
-						if (pc300loop.loop_on) {
-							falc_generate_loop_up_code (card, ch);
-						} else {
-							turn_off_xlu(card, ch);
-						}
-						return 0;
-
-					case PC300GENLOOPDOWN:	/* Generate loop DOWN */
-						if (pc300loop.loop_on) {
-							falc_generate_loop_down_code (card, ch);
-						} else {
-							turn_off_xld(card, ch);
-						}
-						return 0;
-
-					default:
-						return -EINVAL;
-				}
-			}
-
-		case SIOCSPC300PATTERNTEST:
-			/* Turn the pattern test on/off and show the errors counter */
-			{
-				struct pc300patterntst pc300patrntst;
-
-				/* TE boards only */
-				if (card->hw.type != PC300_TE)
-					return -EINVAL;
-
-				if (card->hw.cpld_id < 0x02) {
-					/* CPLD_ID < 0x02 doesn't support pattern test */
-					return -EINVAL;
-				}
-
-				if (!arg || 
-					copy_from_user(&pc300patrntst,arg,sizeof(pc300patterntst_t)))
-						return -EINVAL;
-				if (pc300patrntst.patrntst_on == 2) {
-					if (chan->falc.prbs == 0) {
-						falc_pattern_test(card, ch, 1);
-					}
-					pc300patrntst.num_errors =
-						falc_pattern_test_error(card, ch);
-					if (copy_to_user(arg, &pc300patrntst,
-							 sizeof(pc300patterntst_t)))
-							return -EINVAL;
-				} else {
-					falc_pattern_test(card, ch, pc300patrntst.patrntst_on);
-				}
-				return 0;
-			}
-
-		case SIOCWANDEV:
-			switch (ifr->ifr_settings.type) {
-				case IF_GET_IFACE:
-				{
-					const size_t size = sizeof(sync_serial_settings);
-					ifr->ifr_settings.type = conf->media;
-					if (ifr->ifr_settings.size < size) {
-						/* data size wanted */
-						ifr->ifr_settings.size = size;
-						return -ENOBUFS;
-					}
-	
-					if (copy_to_user(settings->ifs_ifsu.sync,
-							 &conf->phys_settings, size)) {
-						return -EFAULT;
-					}
-					return 0;
-				}
-
-				case IF_IFACE_V35:
-				case IF_IFACE_V24:
-				case IF_IFACE_X21:
-				{
-					const size_t size = sizeof(sync_serial_settings);
-
-					if (!capable(CAP_NET_ADMIN)) {
-						return -EPERM;
-					}
-					/* incorrect data len? */
-					if (ifr->ifr_settings.size != size) {
-						return -ENOBUFS;
-					}
-
-					if (copy_from_user(&conf->phys_settings, 
-							   settings->ifs_ifsu.sync, size)) {
-						return -EFAULT;
-					}
-
-					if (conf->phys_settings.loopback) {
-						cpc_writeb(card->hw.scabase + M_REG(MD2, ch),
-							cpc_readb(card->hw.scabase + M_REG(MD2, ch)) | 
-							MD2_LOOP_MIR);
-					}
-					conf->media = ifr->ifr_settings.type;
-					return 0;
-				}
-
-				case IF_IFACE_T1:
-				case IF_IFACE_E1:
-				{
-					const size_t te_size = sizeof(te1_settings);
-					const size_t size = sizeof(sync_serial_settings);
-
-					if (!capable(CAP_NET_ADMIN)) {
-						return -EPERM;
-					}
-
-					/* incorrect data len? */
-					if (ifr->ifr_settings.size != te_size) {
-						return -ENOBUFS;
-					}
-
-					if (copy_from_user(&conf->phys_settings, 
-							   settings->ifs_ifsu.te1, size)) {
-						return -EFAULT;
-					}/* Ignoring HDLC slot_map for a while */
-					
-					if (conf->phys_settings.loopback) {
-						cpc_writeb(card->hw.scabase + M_REG(MD2, ch),
-							cpc_readb(card->hw.scabase + M_REG(MD2, ch)) | 
-							MD2_LOOP_MIR);
-					}
-					conf->media = ifr->ifr_settings.type;
-					return 0;
-				}
-				default:
-					return hdlc_ioctl(dev, ifr, cmd);
-			}
-
-		default:
-			return hdlc_ioctl(dev, ifr, cmd);
-	}
-}
-
-static int clock_rate_calc(u32 rate, u32 clock, int *br_io)
-{
-	int br, tc;
-	int br_pwr, error;
-
-	*br_io = 0;
-
-	if (rate == 0)
-		return 0;
-
-	for (br = 0, br_pwr = 1; br <= 9; br++, br_pwr <<= 1) {
-		if ((tc = clock / br_pwr / rate) <= 0xff) {
-			*br_io = br;
-			break;
-		}
-	}
-
-	if (tc <= 0xff) {
-		error = ((rate - (clock / br_pwr / rate)) / rate) * 1000;
-		/* Errors bigger than +/- 1% won't be tolerated */
-		if (error < -10 || error > 10)
-			return -1;
-		else
-			return tc;
-	} else {
-		return -1;
-	}
-}
-
-static int ch_config(pc300dev_t * d)
-{
-	pc300ch_t *chan = (pc300ch_t *) d->chan;
-	pc300chconf_t *conf = (pc300chconf_t *) & chan->conf;
-	pc300_t *card = (pc300_t *) chan->card;
-	void __iomem *scabase = card->hw.scabase;
-	void __iomem *plxbase = card->hw.plxbase;
-	int ch = chan->channel;
-	u32 clkrate = chan->conf.phys_settings.clock_rate;
-	u32 clktype = chan->conf.phys_settings.clock_type;
-	u16 encoding = chan->conf.proto_settings.encoding;
-	u16 parity = chan->conf.proto_settings.parity;
-	u8 md0, md2;
-
-	/* Reset the channel */
-	cpc_writeb(scabase + M_REG(CMD, ch), CMD_CH_RST);
-
-	/* Configure the SCA registers */
-	switch (parity) {
-		case PARITY_NONE:
-			md0 = MD0_BIT_SYNC;
-			break;
-		case PARITY_CRC16_PR0:
-			md0 = MD0_CRC16_0|MD0_CRCC0|MD0_BIT_SYNC;
-			break;
-		case PARITY_CRC16_PR1:
-			md0 = MD0_CRC16_1|MD0_CRCC0|MD0_BIT_SYNC;
-			break;
-		case PARITY_CRC32_PR1_CCITT:
-			md0 = MD0_CRC32|MD0_CRCC0|MD0_BIT_SYNC;
-			break;
-		case PARITY_CRC16_PR1_CCITT:
-		default:
-			md0 = MD0_CRC_CCITT|MD0_CRCC0|MD0_BIT_SYNC;
-			break;
-	}
-	switch (encoding) {
-		case ENCODING_NRZI:
-			md2 = MD2_F_DUPLEX|MD2_ADPLL_X8|MD2_NRZI;
-			break;
-		case ENCODING_FM_MARK:	/* FM1 */
-			md2 = MD2_F_DUPLEX|MD2_ADPLL_X8|MD2_FM|MD2_FM1;
-			break;
-		case ENCODING_FM_SPACE:	/* FM0 */
-			md2 = MD2_F_DUPLEX|MD2_ADPLL_X8|MD2_FM|MD2_FM0;
-			break;
-		case ENCODING_MANCHESTER: /* It's not working... */
-			md2 = MD2_F_DUPLEX|MD2_ADPLL_X8|MD2_FM|MD2_MANCH;
-			break;
-		case ENCODING_NRZ:
-		default:
-			md2 = MD2_F_DUPLEX|MD2_ADPLL_X8|MD2_NRZ;
-			break;
-	}
-	cpc_writeb(scabase + M_REG(MD0, ch), md0);
-	cpc_writeb(scabase + M_REG(MD1, ch), 0);
-	cpc_writeb(scabase + M_REG(MD2, ch), md2);
- 	cpc_writeb(scabase + M_REG(IDL, ch), 0x7e);
-	cpc_writeb(scabase + M_REG(CTL, ch), CTL_URSKP | CTL_IDLC);
-
-	/* Configure HW media */
-	switch (card->hw.type) {
-		case PC300_RSV:
-			if (conf->media == IF_IFACE_V35) {
-				cpc_writel((plxbase + card->hw.gpioc_reg),
-					   cpc_readl(plxbase + card->hw.gpioc_reg) | PC300_CHMEDIA_MASK(ch));
-			} else {
-				cpc_writel((plxbase + card->hw.gpioc_reg),
-					   cpc_readl(plxbase + card->hw.gpioc_reg) & ~PC300_CHMEDIA_MASK(ch));
-			}
-			break;
-
-		case PC300_X21:
-			break;
-
-		case PC300_TE:
-			te_config(card, ch);
-			break;
-	}
-
-	switch (card->hw.type) {
-		case PC300_RSV:
-		case PC300_X21:
-			if (clktype == CLOCK_INT || clktype == CLOCK_TXINT) {
-				int tmc, br;
-
-				/* Calculate the clkrate parameters */
-				tmc = clock_rate_calc(clkrate, card->hw.clock, &br);
-				if (tmc < 0)
-					return -EIO;
-				cpc_writeb(scabase + M_REG(TMCT, ch), tmc);
-				cpc_writeb(scabase + M_REG(TXS, ch),
-					   (TXS_DTRXC | TXS_IBRG | br));
-				if (clktype == CLOCK_INT) {
-					cpc_writeb(scabase + M_REG(TMCR, ch), tmc);
-					cpc_writeb(scabase + M_REG(RXS, ch), 
-						   (RXS_IBRG | br));
-				} else {
-					cpc_writeb(scabase + M_REG(TMCR, ch), 1);
-					cpc_writeb(scabase + M_REG(RXS, ch), 0);
-				}
-	    			if (card->hw.type == PC300_X21) {
-					cpc_writeb(scabase + M_REG(GPO, ch), 1);
-					cpc_writeb(scabase + M_REG(EXS, ch), EXS_TES1 | EXS_RES1);
-				} else {
-					cpc_writeb(scabase + M_REG(EXS, ch), EXS_TES1);
-				}
-			} else {
-				cpc_writeb(scabase + M_REG(TMCT, ch), 1);
-				if (clktype == CLOCK_EXT) {
-					cpc_writeb(scabase + M_REG(TXS, ch), 
-						   TXS_DTRXC);
-				} else {
-					cpc_writeb(scabase + M_REG(TXS, ch), 
-						   TXS_DTRXC|TXS_RCLK);
-				}
-	    			cpc_writeb(scabase + M_REG(TMCR, ch), 1);
-				cpc_writeb(scabase + M_REG(RXS, ch), 0);
-				if (card->hw.type == PC300_X21) {
-					cpc_writeb(scabase + M_REG(GPO, ch), 0);
-					cpc_writeb(scabase + M_REG(EXS, ch), EXS_TES1 | EXS_RES1);
-				} else {
-					cpc_writeb(scabase + M_REG(EXS, ch), EXS_TES1);
-				}
-			}
-			break;
-
-		case PC300_TE:
-			/* SCA always receives clock from the FALC chip */
-			cpc_writeb(scabase + M_REG(TMCT, ch), 1);
-			cpc_writeb(scabase + M_REG(TXS, ch), 0);
-			cpc_writeb(scabase + M_REG(TMCR, ch), 1);
-			cpc_writeb(scabase + M_REG(RXS, ch), 0);
-			cpc_writeb(scabase + M_REG(EXS, ch), 0);
-			break;
-	}
-
-	/* Enable Interrupts */
-	cpc_writel(scabase + IER0,
-		   cpc_readl(scabase + IER0) |
-		   IR0_M(IR0_RXINTA, ch) |
-		   IR0_DRX(IR0_EFT | IR0_DMIA | IR0_DMIB, ch) |
-		   IR0_DTX(IR0_EFT | IR0_DMIA | IR0_DMIB, ch));
-	cpc_writeb(scabase + M_REG(IE0, ch),
-		   cpc_readl(scabase + M_REG(IE0, ch)) | IE0_RXINTA);
-	cpc_writeb(scabase + M_REG(IE1, ch),
-		   cpc_readl(scabase + M_REG(IE1, ch)) | IE1_CDCD);
-
-	return 0;
-}
-
-static int rx_config(pc300dev_t * d)
-{
-	pc300ch_t *chan = (pc300ch_t *) d->chan;
-	pc300_t *card = (pc300_t *) chan->card;
-	void __iomem *scabase = card->hw.scabase;
-	int ch = chan->channel;
-
-	cpc_writeb(scabase + DSR_RX(ch), 0);
-
-	/* General RX settings */
-	cpc_writeb(scabase + M_REG(RRC, ch), 0);
-	cpc_writeb(scabase + M_REG(RNR, ch), 16);
-
-	/* Enable reception */
-	cpc_writeb(scabase + M_REG(CMD, ch), CMD_RX_CRC_INIT);
-	cpc_writeb(scabase + M_REG(CMD, ch), CMD_RX_ENA);
-
-	/* Initialize DMA stuff */
-	chan->rx_first_bd = 0;
-	chan->rx_last_bd = N_DMA_RX_BUF - 1;
-	rx_dma_buf_init(card, ch);
-	cpc_writeb(scabase + DCR_RX(ch), DCR_FCT_CLR);
-	cpc_writeb(scabase + DMR_RX(ch), (DMR_TMOD | DMR_NF));
-	cpc_writeb(scabase + DIR_RX(ch), (DIR_EOM | DIR_BOF));
-
-	/* Start DMA */
-	rx_dma_start(card, ch);
-
-	return 0;
-}
-
-static int tx_config(pc300dev_t * d)
-{
-	pc300ch_t *chan = (pc300ch_t *) d->chan;
-	pc300_t *card = (pc300_t *) chan->card;
-	void __iomem *scabase = card->hw.scabase;
-	int ch = chan->channel;
-
-	cpc_writeb(scabase + DSR_TX(ch), 0);
-
-	/* General TX settings */
-	cpc_writeb(scabase + M_REG(TRC0, ch), 0);
-	cpc_writeb(scabase + M_REG(TFS, ch), 32);
-	cpc_writeb(scabase + M_REG(TNR0, ch), 20);
-	cpc_writeb(scabase + M_REG(TNR1, ch), 48);
-	cpc_writeb(scabase + M_REG(TCR, ch), 8);
-
-	/* Enable transmission */
-	cpc_writeb(scabase + M_REG(CMD, ch), CMD_TX_CRC_INIT);
-
-	/* Initialize DMA stuff */
-	chan->tx_first_bd = 0;
-	chan->tx_next_bd = 0;
-	tx_dma_buf_init(card, ch);
-	cpc_writeb(scabase + DCR_TX(ch), DCR_FCT_CLR);
-	cpc_writeb(scabase + DMR_TX(ch), (DMR_TMOD | DMR_NF));
-	cpc_writeb(scabase + DIR_TX(ch), (DIR_EOM | DIR_BOF | DIR_UDRF));
-	cpc_writel(scabase + DTX_REG(CDAL, ch), TX_BD_ADDR(ch, chan->tx_first_bd));
-	cpc_writel(scabase + DTX_REG(EDAL, ch), TX_BD_ADDR(ch, chan->tx_next_bd));
-
-	return 0;
-}
-
-static int cpc_attach(struct net_device *dev, unsigned short encoding,
-		      unsigned short parity)
-{
-	pc300dev_t *d = (pc300dev_t *)dev_to_hdlc(dev)->priv;
-	pc300ch_t *chan = (pc300ch_t *)d->chan;
-	pc300_t *card = (pc300_t *)chan->card;
-	pc300chconf_t *conf = (pc300chconf_t *)&chan->conf;
-
-	if (card->hw.type == PC300_TE) {
-		if (encoding != ENCODING_NRZ && encoding != ENCODING_NRZI) {
-			return -EINVAL;
-		}
-	} else {
-		if (encoding != ENCODING_NRZ && encoding != ENCODING_NRZI &&
-		    encoding != ENCODING_FM_MARK && encoding != ENCODING_FM_SPACE) {
-			/* Driver doesn't support ENCODING_MANCHESTER yet */
-			return -EINVAL;
-		}
-	}
-
-	if (parity != PARITY_NONE && parity != PARITY_CRC16_PR0 &&
-	    parity != PARITY_CRC16_PR1 && parity != PARITY_CRC32_PR1_CCITT &&
-	    parity != PARITY_CRC16_PR1_CCITT) {
-		return -EINVAL;
-	}
-
-	conf->proto_settings.encoding = encoding;
-	conf->proto_settings.parity = parity;
-	return 0;
-}
-
-static int cpc_opench(pc300dev_t * d)
-{
-	pc300ch_t *chan = (pc300ch_t *) d->chan;
-	pc300_t *card = (pc300_t *) chan->card;
-	int ch = chan->channel, rc;
-	void __iomem *scabase = card->hw.scabase;
-
-	rc = ch_config(d);
-	if (rc)
-		return rc;
-
-	rx_config(d);
-
-	tx_config(d);
-
-	/* Assert RTS and DTR */
-	cpc_writeb(scabase + M_REG(CTL, ch),
-		   cpc_readb(scabase + M_REG(CTL, ch)) & ~(CTL_RTS | CTL_DTR));
-
-	return 0;
-}
-
-static void cpc_closech(pc300dev_t * d)
-{
-	pc300ch_t *chan = (pc300ch_t *) d->chan;
-	pc300_t *card = (pc300_t *) chan->card;
-	falc_t *pfalc = (falc_t *) & chan->falc;
-	int ch = chan->channel;
-
-	cpc_writeb(card->hw.scabase + M_REG(CMD, ch), CMD_CH_RST);
-	rx_dma_stop(card, ch);
-	tx_dma_stop(card, ch);
-
-	if (card->hw.type == PC300_TE) {
-		memset(pfalc, 0, sizeof(falc_t));
-		cpc_writeb(card->hw.falcbase + card->hw.cpld_reg2,
-			   cpc_readb(card->hw.falcbase + card->hw.cpld_reg2) &
-			   ~((CPLD_REG2_FALC_TX_CLK | CPLD_REG2_FALC_RX_CLK |
-			      CPLD_REG2_FALC_LED2) << (2 * ch)));
-		/* Reset the FALC chip */
-		cpc_writeb(card->hw.falcbase + card->hw.cpld_reg1,
-			   cpc_readb(card->hw.falcbase + card->hw.cpld_reg1) |
-			   (CPLD_REG1_FALC_RESET << (2 * ch)));
-		udelay(10000);
-		cpc_writeb(card->hw.falcbase + card->hw.cpld_reg1,
-			   cpc_readb(card->hw.falcbase + card->hw.cpld_reg1) &
-			   ~(CPLD_REG1_FALC_RESET << (2 * ch)));
-	}
-}
-
-int cpc_open(struct net_device *dev)
-{
-	pc300dev_t *d = (pc300dev_t *) dev_to_hdlc(dev)->priv;
-	struct ifreq ifr;
-	int result;
-
-#ifdef	PC300_DEBUG_OTHER
-	printk("pc300: cpc_open");
-#endif
-
-	result = hdlc_open(dev);
-
-	if (result)
-		return result;
-
-	sprintf(ifr.ifr_name, "%s", dev->name);
-	result = cpc_opench(d);
-	if (result)
-		goto err_out;
-
-	netif_start_queue(dev);
-	return 0;
-
-err_out:
-	hdlc_close(dev);
-	return result;
-}
-
-static int cpc_close(struct net_device *dev)
-{
-	pc300dev_t *d = (pc300dev_t *) dev_to_hdlc(dev)->priv;
-	pc300ch_t *chan = (pc300ch_t *) d->chan;
-	pc300_t *card = (pc300_t *) chan->card;
-	unsigned long flags;
-
-#ifdef	PC300_DEBUG_OTHER
-	printk("pc300: cpc_close");
-#endif
-
-	netif_stop_queue(dev);
-
-	CPC_LOCK(card, flags);
-	cpc_closech(d);
-	CPC_UNLOCK(card, flags);
-
-	hdlc_close(dev);
-
-#ifdef CONFIG_PC300_MLPPP
-	if (chan->conf.proto == PC300_PROTO_MLPPP) {
-		cpc_tty_unregister_service(d);
-		chan->conf.proto = 0xffff;
-	}
-#endif
-
-	return 0;
-}
-
-static u32 detect_ram(pc300_t * card)
-{
-	u32 i;
-	u8 data;
-	void __iomem *rambase = card->hw.rambase;
-
-	card->hw.ramsize = PC300_RAMSIZE;
-	/* Let's find out how much RAM is present on this board */
-	for (i = 0; i < card->hw.ramsize; i++) {
-		data = (u8)(i & 0xff);
-		cpc_writeb(rambase + i, data);
-		if (cpc_readb(rambase + i) != data) {
-			break;
-		}
-	}
-	return i;
-}
-
-static void plx_init(pc300_t * card)
-{
-	struct RUNTIME_9050 __iomem *plx_ctl = card->hw.plxbase;
-
-	/* Reset PLX */
-	cpc_writel(&plx_ctl->init_ctrl,
-		   cpc_readl(&plx_ctl->init_ctrl) | 0x40000000);
-	udelay(10000L);
-	cpc_writel(&plx_ctl->init_ctrl,
-		   cpc_readl(&plx_ctl->init_ctrl) & ~0x40000000);
-
-	/* Reload Config. Registers from EEPROM */
-	cpc_writel(&plx_ctl->init_ctrl,
-		   cpc_readl(&plx_ctl->init_ctrl) | 0x20000000);
-	udelay(10000L);
-	cpc_writel(&plx_ctl->init_ctrl,
-		   cpc_readl(&plx_ctl->init_ctrl) & ~0x20000000);
-
-}
-
-static void show_version(void)
-{
-	char *rcsvers, *rcsdate, *tmp;
-
-	rcsvers = strchr(rcsid, ' ');
-	rcsvers++;
-	tmp = strchr(rcsvers, ' ');
-	*tmp++ = '\0';
-	rcsdate = strchr(tmp, ' ');
-	rcsdate++;
-	tmp = strrchr(rcsdate, ' ');
-	*tmp = '\0';
-	pr_info("Cyclades-PC300 driver %s %s\n", rcsvers, rcsdate);
-}				/* show_version */
-
-static const struct net_device_ops cpc_netdev_ops = {
-	.ndo_open		= cpc_open,
-	.ndo_stop		= cpc_close,
-	.ndo_tx_timeout		= cpc_tx_timeout,
-	.ndo_set_mac_address	= NULL,
-	.ndo_change_mtu		= cpc_change_mtu,
-	.ndo_do_ioctl		= cpc_ioctl,
-	.ndo_validate_addr	= eth_validate_addr,
-};
-
-static void cpc_init_card(pc300_t * card)
-{
-	int i, devcount = 0;
-	static int board_nbr = 1;
-
-	/* Enable interrupts on the PCI bridge */
-	plx_init(card);
-	cpc_writew(card->hw.plxbase + card->hw.intctl_reg,
-		   cpc_readw(card->hw.plxbase + card->hw.intctl_reg) | 0x0040);
-
-#ifdef USE_PCI_CLOCK
-	/* Set board clock to PCI clock */
-	cpc_writel(card->hw.plxbase + card->hw.gpioc_reg,
-		   cpc_readl(card->hw.plxbase + card->hw.gpioc_reg) | 0x00000004UL);
-	card->hw.clock = PC300_PCI_CLOCK;
-#else
-	/* Set board clock to internal oscillator clock */
-	cpc_writel(card->hw.plxbase + card->hw.gpioc_reg,
-		   cpc_readl(card->hw.plxbase + card->hw.gpioc_reg) & ~0x00000004UL);
-	card->hw.clock = PC300_OSC_CLOCK;
-#endif
-
-	/* Detect actual on-board RAM size */
-	card->hw.ramsize = detect_ram(card);
-
-	/* Set Global SCA-II registers */
-	cpc_writeb(card->hw.scabase + PCR, PCR_PR2);
-	cpc_writeb(card->hw.scabase + BTCR, 0x10);
-	cpc_writeb(card->hw.scabase + WCRL, 0);
-	cpc_writeb(card->hw.scabase + DMER, 0x80);
-
-	if (card->hw.type == PC300_TE) {
-		u8 reg1;
-
-		/* Check CPLD version */
-		reg1 = cpc_readb(card->hw.falcbase + CPLD_REG1);
-		cpc_writeb(card->hw.falcbase + CPLD_REG1, (reg1 + 0x5a));
-		if (cpc_readb(card->hw.falcbase + CPLD_REG1) == reg1) {
-			/* New CPLD */
-			card->hw.cpld_id = cpc_readb(card->hw.falcbase + CPLD_ID_REG);
-			card->hw.cpld_reg1 = CPLD_V2_REG1;
-			card->hw.cpld_reg2 = CPLD_V2_REG2;
-		} else {
-			/* old CPLD */
-			card->hw.cpld_id = 0;
-			card->hw.cpld_reg1 = CPLD_REG1;
-			card->hw.cpld_reg2 = CPLD_REG2;
-			cpc_writeb(card->hw.falcbase + CPLD_REG1, reg1);
-		}
-
-		/* Enable the board's global clock */
-		cpc_writeb(card->hw.falcbase + card->hw.cpld_reg1,
-			   cpc_readb(card->hw.falcbase + card->hw.cpld_reg1) |
-			   CPLD_REG1_GLOBAL_CLK);
-
-	}
-
-	for (i = 0; i < card->hw.nchan; i++) {
-		pc300ch_t *chan = &card->chan[i];
-		pc300dev_t *d = &chan->d;
-		hdlc_device *hdlc;
-		struct net_device *dev;
-
-		chan->card = card;
-		chan->channel = i;
-		chan->conf.phys_settings.clock_rate = 0;
-		chan->conf.phys_settings.clock_type = CLOCK_EXT;
-		chan->conf.proto_settings.encoding = ENCODING_NRZ;
-		chan->conf.proto_settings.parity = PARITY_CRC16_PR1_CCITT;
-		switch (card->hw.type) {
-			case PC300_TE:
-				chan->conf.media = IF_IFACE_T1;
-				chan->conf.lcode = PC300_LC_B8ZS;
-				chan->conf.fr_mode = PC300_FR_ESF;
-				chan->conf.lbo = PC300_LBO_0_DB;
-				chan->conf.rx_sens = PC300_RX_SENS_SH;
-				chan->conf.tslot_bitmap = 0xffffffffUL;
-				break;
-
-			case PC300_X21:
-				chan->conf.media = IF_IFACE_X21;
-				break;
-
-			case PC300_RSV:
-			default:
-				chan->conf.media = IF_IFACE_V35;
-				break;
-		}
-		chan->conf.proto = IF_PROTO_PPP;
-		chan->tx_first_bd = 0;
-		chan->tx_next_bd = 0;
-		chan->rx_first_bd = 0;
-		chan->rx_last_bd = N_DMA_RX_BUF - 1;
-		chan->nfree_tx_bd = N_DMA_TX_BUF;
-
-		d->chan = chan;
-		d->trace_on = 0;
-		d->line_on = 0;
-		d->line_off = 0;
-
-		dev = alloc_hdlcdev(d);
-		if (dev == NULL)
-			continue;
-
-		hdlc = dev_to_hdlc(dev);
-		hdlc->xmit = cpc_queue_xmit;
-		hdlc->attach = cpc_attach;
-		d->dev = dev;
-		dev->mem_start = card->hw.ramphys;
-		dev->mem_end = card->hw.ramphys + card->hw.ramsize - 1;
-		dev->irq = card->hw.irq;
-		dev->tx_queue_len = PC300_TX_QUEUE_LEN;
-		dev->mtu = PC300_DEF_MTU;
-
-		dev->netdev_ops = &cpc_netdev_ops;
-		dev->watchdog_timeo = PC300_TX_TIMEOUT;
-
-		if (register_hdlc_device(dev) == 0) {
-			printk("%s: Cyclades-PC300/", dev->name);
-			switch (card->hw.type) {
-				case PC300_TE:
-					if (card->hw.bus == PC300_PMC) {
-						printk("TE-M");
-					} else {
-						printk("TE  ");
-					}
-					break;
-
-				case PC300_X21:
-					printk("X21 ");
-					break;
-
-				case PC300_RSV:
-				default:
-					printk("RSV ");
-					break;
-			}
-			printk (" #%d, %dKB of RAM at 0x%08x, IRQ%d, channel %d.\n",
-				 board_nbr, card->hw.ramsize / 1024,
-				 card->hw.ramphys, card->hw.irq, i + 1);
-			devcount++;
-		} else {
-			printk ("Dev%d on card(0x%08x): unable to allocate i/f name.\n",
-				 i + 1, card->hw.ramphys);
-			free_netdev(dev);
-			continue;
-		}
-	}
-	spin_lock_init(&card->card_lock);
-
-	board_nbr++;
-}
-
-static int
-cpc_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
-{
-	int err, eeprom_outdated = 0;
-	u16 device_id;
-	pc300_t *card;
-
-	if ((err = pci_enable_device(pdev)) < 0)
-		return err;
-
-	card = kzalloc(sizeof(pc300_t), GFP_KERNEL);
-	if (card == NULL) {
-		printk("PC300 found at RAM 0x%016llx, "
-		       "but could not allocate card structure.\n",
-		       (unsigned long long)pci_resource_start(pdev, 3));
-		err = -ENOMEM;
-		goto err_disable_dev;
-	}
-
-	err = -ENODEV;
-
-	/* read PCI configuration area */
-	device_id = ent->device;
-	card->hw.irq = pdev->irq;
-	card->hw.iophys = pci_resource_start(pdev, 1);
-	card->hw.iosize = pci_resource_len(pdev, 1);
-	card->hw.scaphys = pci_resource_start(pdev, 2);
-	card->hw.scasize = pci_resource_len(pdev, 2);
-	card->hw.ramphys = pci_resource_start(pdev, 3);
-	card->hw.alloc_ramsize = pci_resource_len(pdev, 3);
-	card->hw.falcphys = pci_resource_start(pdev, 4);
-	card->hw.falcsize = pci_resource_len(pdev, 4);
-	card->hw.plxphys = pci_resource_start(pdev, 5);
-	card->hw.plxsize = pci_resource_len(pdev, 5);
-
-	switch (device_id) {
-		case PCI_DEVICE_ID_PC300_RX_1:
-		case PCI_DEVICE_ID_PC300_TE_1:
-		case PCI_DEVICE_ID_PC300_TE_M_1:
-			card->hw.nchan = 1;
-			break;
-
-		case PCI_DEVICE_ID_PC300_RX_2:
-		case PCI_DEVICE_ID_PC300_TE_2:
-		case PCI_DEVICE_ID_PC300_TE_M_2:
-		default:
-			card->hw.nchan = PC300_MAXCHAN;
-			break;
-	}
-#ifdef PC300_DEBUG_PCI
-	printk("cpc (bus=0x0%x,pci_id=0x%x,", pdev->bus->number, pdev->devfn);
-	printk("rev_id=%d) IRQ%d\n", pdev->revision, card->hw.irq);
-	printk("cpc:found  ramaddr=0x%08lx plxaddr=0x%08lx "
-	       "ctladdr=0x%08lx falcaddr=0x%08lx\n",
-	       card->hw.ramphys, card->hw.plxphys, card->hw.scaphys,
-	       card->hw.falcphys);
-#endif
-	/* Although we don't use this I/O region, we should
-	 * request it from the kernel anyway, to avoid problems
-	 * with other drivers accessing it. */
-	if (!request_region(card->hw.iophys, card->hw.iosize, "PLX Registers")) {
-		/* In case we can't allocate it, warn user */
-		printk("WARNING: couldn't allocate I/O region for PC300 board "
-		       "at 0x%08x!\n", card->hw.ramphys);
-	}
-
-	if (card->hw.plxphys) {
-		pci_write_config_dword(pdev, PCI_BASE_ADDRESS_0, card->hw.plxphys);
-	} else {
-		eeprom_outdated = 1;
-		card->hw.plxphys = pci_resource_start(pdev, 0);
-		card->hw.plxsize = pci_resource_len(pdev, 0);
-	}
-
-	if (!request_mem_region(card->hw.plxphys, card->hw.plxsize,
-				"PLX Registers")) {
-		printk("PC300 found at RAM 0x%08x, "
-		       "but could not allocate PLX mem region.\n",
-		       card->hw.ramphys);
-		goto err_release_io;
-	}
-	if (!request_mem_region(card->hw.ramphys, card->hw.alloc_ramsize,
-				"On-board RAM")) {
-		printk("PC300 found at RAM 0x%08x, "
-		       "but could not allocate RAM mem region.\n",
-		       card->hw.ramphys);
-		goto err_release_plx;
-	}
-	if (!request_mem_region(card->hw.scaphys, card->hw.scasize,
-				"SCA-II Registers")) {
-		printk("PC300 found at RAM 0x%08x, "
-		       "but could not allocate SCA mem region.\n",
-		       card->hw.ramphys);
-		goto err_release_ram;
-	}
-
-	card->hw.plxbase = ioremap(card->hw.plxphys, card->hw.plxsize);
-	card->hw.rambase = ioremap(card->hw.ramphys, card->hw.alloc_ramsize);
-	card->hw.scabase = ioremap(card->hw.scaphys, card->hw.scasize);
-	switch (device_id) {
-		case PCI_DEVICE_ID_PC300_TE_1:
-		case PCI_DEVICE_ID_PC300_TE_2:
-		case PCI_DEVICE_ID_PC300_TE_M_1:
-		case PCI_DEVICE_ID_PC300_TE_M_2:
-			request_mem_region(card->hw.falcphys, card->hw.falcsize,
-					   "FALC Registers");
-			card->hw.falcbase = ioremap(card->hw.falcphys, card->hw.falcsize);
-			break;
-
-		case PCI_DEVICE_ID_PC300_RX_1:
-		case PCI_DEVICE_ID_PC300_RX_2:
-		default:
-			card->hw.falcbase = NULL;
-			break;
-	}
-
-#ifdef PC300_DEBUG_PCI
-	printk("cpc: relocate ramaddr=0x%08lx plxaddr=0x%08lx "
-	       "ctladdr=0x%08lx falcaddr=0x%08lx\n",
-	       card->hw.rambase, card->hw.plxbase, card->hw.scabase,
-	       card->hw.falcbase);
-#endif
-
-	/* Set PCI drv pointer to the card structure */
-	pci_set_drvdata(pdev, card);
-
-	/* Set board type */
-	switch (device_id) {
-		case PCI_DEVICE_ID_PC300_TE_1:
-		case PCI_DEVICE_ID_PC300_TE_2:
-		case PCI_DEVICE_ID_PC300_TE_M_1:
-		case PCI_DEVICE_ID_PC300_TE_M_2:
-			card->hw.type = PC300_TE;
-
-			if ((device_id == PCI_DEVICE_ID_PC300_TE_M_1) ||
-			    (device_id == PCI_DEVICE_ID_PC300_TE_M_2)) {
-				card->hw.bus = PC300_PMC;
-				/* Set PLX register offsets */
-				card->hw.gpioc_reg = 0x54;
-				card->hw.intctl_reg = 0x4c;
-			} else {
-				card->hw.bus = PC300_PCI;
-				/* Set PLX register offsets */
-				card->hw.gpioc_reg = 0x50;
-				card->hw.intctl_reg = 0x4c;
-			}
-			break;
-
-		case PCI_DEVICE_ID_PC300_RX_1:
-		case PCI_DEVICE_ID_PC300_RX_2:
-		default:
-			card->hw.bus = PC300_PCI;
-			/* Set PLX register offsets */
-			card->hw.gpioc_reg = 0x50;
-			card->hw.intctl_reg = 0x4c;
-
-			if ((cpc_readl(card->hw.plxbase + card->hw.gpioc_reg) & PC300_CTYPE_MASK)) {
-				card->hw.type = PC300_X21;
-			} else {
-				card->hw.type = PC300_RSV;
-			}
-			break;
-	}
-
-	/* Allocate IRQ */
-	if (request_irq(card->hw.irq, cpc_intr, IRQF_SHARED, "Cyclades-PC300", card)) {
-		printk ("PC300 found at RAM 0x%08x, but could not allocate IRQ%d.\n",
-			 card->hw.ramphys, card->hw.irq);
-		goto err_io_unmap;
-	}
-
-	cpc_init_card(card);
-
-	if (eeprom_outdated)
-		printk("WARNING: PC300 with outdated EEPROM.\n");
-	return 0;
-
-err_io_unmap:
-	iounmap(card->hw.plxbase);
-	iounmap(card->hw.scabase);
-	iounmap(card->hw.rambase);
-	if (card->hw.type == PC300_TE) {
-		iounmap(card->hw.falcbase);
-		release_mem_region(card->hw.falcphys, card->hw.falcsize);
-	}
-	release_mem_region(card->hw.scaphys, card->hw.scasize);
-err_release_ram:
-	release_mem_region(card->hw.ramphys, card->hw.alloc_ramsize);
-err_release_plx:
-	release_mem_region(card->hw.plxphys, card->hw.plxsize);
-err_release_io:
-	release_region(card->hw.iophys, card->hw.iosize);
-	kfree(card);
-err_disable_dev:
-	pci_disable_device(pdev);
-	return err;
-}
-
-static void cpc_remove_one(struct pci_dev *pdev)
-{
-	pc300_t *card = pci_get_drvdata(pdev);
-
-	if (card->hw.rambase) {
-		int i;
-
-		/* Disable interrupts on the PCI bridge */
-		cpc_writew(card->hw.plxbase + card->hw.intctl_reg,
-			   cpc_readw(card->hw.plxbase + card->hw.intctl_reg) & ~(0x0040));
-
-		for (i = 0; i < card->hw.nchan; i++) {
-			unregister_hdlc_device(card->chan[i].d.dev);
-		}
-		iounmap(card->hw.plxbase);
-		iounmap(card->hw.scabase);
-		iounmap(card->hw.rambase);
-		release_mem_region(card->hw.plxphys, card->hw.plxsize);
-		release_mem_region(card->hw.ramphys, card->hw.alloc_ramsize);
-		release_mem_region(card->hw.scaphys, card->hw.scasize);
-		release_region(card->hw.iophys, card->hw.iosize);
-		if (card->hw.type == PC300_TE) {
-			iounmap(card->hw.falcbase);
-			release_mem_region(card->hw.falcphys, card->hw.falcsize);
-		}
-		for (i = 0; i < card->hw.nchan; i++)
-			if (card->chan[i].d.dev)
-				free_netdev(card->chan[i].d.dev);
-		if (card->hw.irq)
-			free_irq(card->hw.irq, card);
-		kfree(card);
-		pci_disable_device(pdev);
-	}
-}
-
-static struct pci_driver cpc_driver = {
-	.name           = "pc300",
-	.id_table       = cpc_pci_dev_id,
-	.probe          = cpc_init_one,
-	.remove         = cpc_remove_one,
-};
-
-static int __init cpc_init(void)
-{
-	show_version();
-	return pci_register_driver(&cpc_driver);
-}
-
-static void __exit cpc_cleanup_module(void)
-{
-	pci_unregister_driver(&cpc_driver);
-}
-
-module_init(cpc_init);
-module_exit(cpc_cleanup_module);
-
-MODULE_DESCRIPTION("Cyclades-PC300 cards driver");
-MODULE_AUTHOR(  "Author: Ivan Passos <ivan@cyclades.com>\r\n"
-                "Maintainer: PC300 Maintainer <pc300@cyclades.com");
-MODULE_LICENSE("GPL");
-
diff --git a/drivers/staging/net/pc300_tty.c b/drivers/staging/net/pc300_tty.c
deleted file mode 100644
index 4709f422856..00000000000
--- a/drivers/staging/net/pc300_tty.c
+++ /dev/null
@@ -1,1079 +0,0 @@
-/*
- * pc300_tty.c	Cyclades-PC300(tm) TTY Driver.
- *
- * Author:	Regina Kodato <reginak@cyclades.com>
- *
- * Copyright:	(c) 1999-2002 Cyclades Corp.
- *
- *	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.
- *   
- *  $Log: pc300_tty.c,v $
- *  Revision 3.7  2002/03/07 14:17:09  henrique
- *  License data fixed
- *
- *  Revision 3.6  2001/12/10 12:29:42  regina
- *  Fix the MLPPP bug
- *
- *  Revision 3.5  2001/10/31 11:20:05  regina
- *  automatic pppd starts
- *
- *  Revision 3.4  2001/08/06 12:01:51  regina
- *  problem in DSR_DE bit
- *
- *  Revision 3.3  2001/07/26 22:58:41  regina
- *  update EDA value
- *
- *  Revision 3.2  2001/07/12 13:11:20  regina
- *  bug fix - DCD-OFF in pc300 tty driver
- *
- *	DMA transmission bug fix
- *  
- *  Revision 3.1  2001/06/22 13:13:02  regina
- *  MLPPP implementation
- *
- */
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/errno.h>
-#include <linux/string.h>
-#include <linux/init.h>
-#include <linux/netdevice.h>
-#include <linux/spinlock.h>
-#include <linux/slab.h>
-#include <linux/if.h>
-#include <linux/skbuff.h>
-/* TTY includes */
-#include <linux/tty.h>
-#include <linux/tty_flip.h>
-#include <linux/serial.h>
-
-#include <asm/io.h>
-#include <asm/uaccess.h>
-
-#include "pc300.h"
-
-/* defines and macros */
-/* TTY Global definitions */
-#define	CPC_TTY_NPORTS	8	/* maximum number of the sync tty connections */
-#define	CPC_TTY_MAJOR	CYCLADES_MAJOR	
-#define CPC_TTY_MINOR_START	240	/* minor of the first PC300 interface */
-
-#define CPC_TTY_MAX_MTU	2000	
-
-/* tty interface state */
-#define	CPC_TTY_ST_IDLE	0
-#define CPC_TTY_ST_INIT	1	/* configured with MLPPP and up */
-#define CPC_TTY_ST_OPEN	2	/* opened by application */
-
-#define	CPC_TTY_LOCK(card,flags)\
-	do {\
-		spin_lock_irqsave(&card->card_lock, flags);	\
-	} while (0)
-
-#define CPC_TTY_UNLOCK(card,flags)	\
-	do {\
-		spin_unlock_irqrestore(&card->card_lock, flags);	\
-	} while (0)
-
-//#define	CPC_TTY_DBG(format,a...)	printk(format,##a)
-#define	CPC_TTY_DBG(format,a...)
-
-/* data structures */
-typedef struct _st_cpc_rx_buf {
-	struct _st_cpc_rx_buf	*next;
-	int		size;
-	unsigned char	data[1];
-} st_cpc_rx_buf;
-
-struct st_cpc_rx_list {
-	st_cpc_rx_buf	*first;
-	st_cpc_rx_buf	*last;
-};
-
-typedef	struct _st_cpc_tty_area {
-	int		state;		/* state of the TTY interface */
-	int		num_open;	
-	unsigned int 	tty_minor;	/* minor this interface */
-	volatile struct st_cpc_rx_list buf_rx;	/* ptr. to reception buffer */
-	unsigned char*	buf_tx;		/* ptr. to transmission buffer */
-	pc300dev_t*	pc300dev;	/* ptr. to info struct in PC300 driver */
-	unsigned char	name[20];	/* interf. name + "-tty" */
-	struct tty_struct *tty;		
-	struct work_struct tty_tx_work; /* tx work - tx interrupt */
-	struct work_struct tty_rx_work; /* rx work - rx interrupt */
-	} st_cpc_tty_area;
-
-/* TTY data structures */
-static struct tty_driver serial_drv;
-
-/* local variables */
-static st_cpc_tty_area	cpc_tty_area[CPC_TTY_NPORTS];
-
-static int cpc_tty_cnt = 0;	/* number of intrfaces configured with MLPPP */
-static int cpc_tty_unreg_flag = 0;
-
-/* TTY functions prototype */
-static int cpc_tty_open(struct tty_struct *tty, struct file *flip);
-static void cpc_tty_close(struct tty_struct *tty, struct file *flip);
-static int cpc_tty_write(struct tty_struct *tty, const unsigned char *buf, int count);
-static int cpc_tty_write_room(struct tty_struct *tty);
-static int cpc_tty_chars_in_buffer(struct tty_struct *tty);
-static void cpc_tty_flush_buffer(struct tty_struct *tty);
-static void cpc_tty_hangup(struct tty_struct *tty);
-static void cpc_tty_rx_work(struct work_struct *work);
-static void cpc_tty_tx_work(struct work_struct *work);
-static int cpc_tty_send_to_card(pc300dev_t *dev,void *buf, int len);
-static void cpc_tty_trace(pc300dev_t *dev, char* buf, int len, char rxtx);
-static void cpc_tty_signal_off(pc300dev_t *pc300dev, unsigned char);
-static void cpc_tty_signal_on(pc300dev_t *pc300dev, unsigned char);
-
-static int pc300_tiocmset(struct tty_struct *, unsigned int, unsigned int);
-static int pc300_tiocmget(struct tty_struct *);
-
-/* functions called by PC300 driver */
-void cpc_tty_init(pc300dev_t *dev);
-void cpc_tty_unregister_service(pc300dev_t *pc300dev);
-void cpc_tty_receive(pc300dev_t *pc300dev);
-void cpc_tty_trigger_poll(pc300dev_t *pc300dev);
-
-/*
- * PC300 TTY clear "signal"
- */
-static void cpc_tty_signal_off(pc300dev_t *pc300dev, unsigned char signal)
-{
-	pc300ch_t *pc300chan = (pc300ch_t *)pc300dev->chan; 
-	pc300_t *card = (pc300_t *) pc300chan->card; 
-	int ch = pc300chan->channel; 
-	unsigned long flags; 
-
-	CPC_TTY_DBG("%s-tty: Clear signal %x\n",
-		pc300dev->dev->name, signal);
-	CPC_TTY_LOCK(card, flags); 
-	cpc_writeb(card->hw.scabase + M_REG(CTL,ch), 
-		cpc_readb(card->hw.scabase+M_REG(CTL,ch))& signal);
-	CPC_TTY_UNLOCK(card,flags); 
-}
-
-/*
- * PC300 TTY set "signal" to ON
- */
-static void cpc_tty_signal_on(pc300dev_t *pc300dev, unsigned char signal)
-{
-	pc300ch_t *pc300chan = (pc300ch_t *)pc300dev->chan; 
-	pc300_t *card = (pc300_t *) pc300chan->card; 
-	int ch = pc300chan->channel; 
-	unsigned long flags; 
-
-	CPC_TTY_DBG("%s-tty: Set signal %x\n",
-		pc300dev->dev->name, signal);
-	CPC_TTY_LOCK(card, flags); 
-	cpc_writeb(card->hw.scabase + M_REG(CTL,ch), 
-		cpc_readb(card->hw.scabase+M_REG(CTL,ch))& ~signal);
-	CPC_TTY_UNLOCK(card,flags); 
-}
-
-
-static const struct tty_operations pc300_ops = {
-	.open = cpc_tty_open,
-	.close = cpc_tty_close,
-	.write = cpc_tty_write,
-	.write_room = cpc_tty_write_room,
-	.chars_in_buffer = cpc_tty_chars_in_buffer,
-	.tiocmset = pc300_tiocmset,
-	.tiocmget = pc300_tiocmget,
-	.flush_buffer = cpc_tty_flush_buffer,
-	.hangup = cpc_tty_hangup,
-};
-
-
-/*
- * PC300 TTY initialization routine
- *
- * This routine is called by the PC300 driver during board configuration 
- * (ioctl=SIOCSP300CONF). At this point the adapter is completely
- * initialized.
- * o verify kernel version (only 2.4.x)
- * o register TTY driver
- * o init cpc_tty_area struct
- */
-void cpc_tty_init(pc300dev_t *pc300dev)
-{
-	unsigned long port;
-	int aux;
-	st_cpc_tty_area * cpc_tty;
-
-	/* hdlcX - X=interface number */
-	port = pc300dev->dev->name[4] - '0';
-	if (port >= CPC_TTY_NPORTS) {
-		printk("%s-tty: invalid interface selected (0-%i): %li",
-			pc300dev->dev->name,
-			CPC_TTY_NPORTS-1,port);
-		return;
-	}
-
-	if (cpc_tty_cnt == 0) { /* first TTY connection -> register driver */
-		CPC_TTY_DBG("%s-tty: driver init, major:%i, minor range:%i=%i\n",
-			pc300dev->dev->name,
-			CPC_TTY_MAJOR, CPC_TTY_MINOR_START,
-			CPC_TTY_MINOR_START+CPC_TTY_NPORTS);
-		/* initialize tty driver struct */
-		memset(&serial_drv,0,sizeof(struct tty_driver));
-		serial_drv.magic = TTY_DRIVER_MAGIC;
-		serial_drv.owner = THIS_MODULE;
-		serial_drv.driver_name = "pc300_tty";
-		serial_drv.name = "ttyCP";
-		serial_drv.major = CPC_TTY_MAJOR;
-		serial_drv.minor_start = CPC_TTY_MINOR_START;
-		serial_drv.num = CPC_TTY_NPORTS;
-		serial_drv.type = TTY_DRIVER_TYPE_SERIAL;
-		serial_drv.subtype = SERIAL_TYPE_NORMAL;
-
-		serial_drv.init_termios = tty_std_termios;
-		serial_drv.init_termios.c_cflag = B9600|CS8|CREAD|HUPCL|CLOCAL;
-		serial_drv.flags = TTY_DRIVER_REAL_RAW;
-
-		/* interface routines from the upper tty layer to the tty driver */
-		tty_set_operations(&serial_drv, &pc300_ops);
-
-		/* register the TTY driver */
-		if (tty_register_driver(&serial_drv)) { 
-			printk("%s-tty: Failed to register serial driver! ",
-				pc300dev->dev->name);
-		   	return;
-		} 
-
-		memset((void *)cpc_tty_area, 0,
-								sizeof(st_cpc_tty_area) * CPC_TTY_NPORTS);
-	}
-
-	cpc_tty = &cpc_tty_area[port];
-	
-	if (cpc_tty->state != CPC_TTY_ST_IDLE) {
-		CPC_TTY_DBG("%s-tty: TTY port %i, already in use.\n",
-				pc300dev->dev->name, port);
-		return;
-	}
-
-	cpc_tty_cnt++;
-	cpc_tty->state = CPC_TTY_ST_INIT; 
-	cpc_tty->num_open= 0;
-	cpc_tty->tty_minor = port + CPC_TTY_MINOR_START;
-	cpc_tty->pc300dev = pc300dev; 
-
-	INIT_WORK(&cpc_tty->tty_tx_work, cpc_tty_tx_work);
-	INIT_WORK(&cpc_tty->tty_rx_work, cpc_tty_rx_work);
-	
-	cpc_tty->buf_rx.first = cpc_tty->buf_rx.last = NULL;
-
-	pc300dev->cpc_tty = (void *)cpc_tty; 
-	
-	aux = strlen(pc300dev->dev->name);
-	memcpy(cpc_tty->name, pc300dev->dev->name, aux);
-	memcpy(&cpc_tty->name[aux], "-tty", 5);
-	
-	cpc_open(pc300dev->dev);
-	cpc_tty_signal_off(pc300dev, CTL_DTR);
-
-	CPC_TTY_DBG("%s: Initializing TTY Sync Driver, tty major#%d minor#%i\n",
-			cpc_tty->name,CPC_TTY_MAJOR,cpc_tty->tty_minor); 
-	return; 
-} 
-
-/*
- * PC300 TTY OPEN routine
- *
- * This routine is called by the tty driver to open the interface 
- * o verify minor
- * o allocate buffer to Rx and Tx
- */
-static int cpc_tty_open(struct tty_struct *tty, struct file *flip)
-{
-	int port ;
-	st_cpc_tty_area *cpc_tty;
-
-	if (!tty) { 
-		return -ENODEV;
-	} 
-
-	port = tty->index;
-
-	if ((port < 0) || (port >= CPC_TTY_NPORTS)){ 
-		CPC_TTY_DBG("pc300_tty: open invalid port %d\n", port);
-		return -ENODEV;
-	} 
-
-	cpc_tty = &cpc_tty_area[port];
-	
-	if (cpc_tty->state == CPC_TTY_ST_IDLE){
-		CPC_TTY_DBG("%s: open - invalid interface, port=%d\n",
-					cpc_tty->name, tty->index);
-		return -ENODEV;
-	}
-
-	if (cpc_tty->num_open == 0) { /* first open of this tty */
-		if (!cpc_tty_area[port].buf_tx){
-			cpc_tty_area[port].buf_tx = kmalloc(CPC_TTY_MAX_MTU,GFP_KERNEL);
-			if (!cpc_tty_area[port].buf_tx) {
-				CPC_TTY_DBG("%s: error in memory allocation\n",cpc_tty->name);
-				return -ENOMEM;
-			}
-		} 
-
-		if (cpc_tty_area[port].buf_rx.first) {
-			unsigned char * aux;
-			while (cpc_tty_area[port].buf_rx.first) {
-				aux = (unsigned char *)cpc_tty_area[port].buf_rx.first;
-				cpc_tty_area[port].buf_rx.first = cpc_tty_area[port].buf_rx.first->next;
-				kfree(aux);
-			}
-			cpc_tty_area[port].buf_rx.first = NULL;
-			cpc_tty_area[port].buf_rx.last = NULL;
-		}
-
-		cpc_tty_area[port].state = CPC_TTY_ST_OPEN;
-		cpc_tty_area[port].tty = tty;
-		tty->driver_data = &cpc_tty_area[port];
-
-		cpc_tty_signal_on(cpc_tty->pc300dev, CTL_DTR);
-	} 
-
-	cpc_tty->num_open++;
-
-	CPC_TTY_DBG("%s: opening TTY driver\n", cpc_tty->name);
-	
-	/* avisar driver PC300 */ 
-	return 0; 
-}
-
-/*
- * PC300 TTY CLOSE routine
- *
- * This routine is called by the tty driver to close the interface 
- * o call close channel in PC300 driver (cpc_closech)
- * o free Rx and Tx buffers
- */
-
-static void cpc_tty_close(struct tty_struct *tty, struct file *flip)
-{
-	st_cpc_tty_area    *cpc_tty;
-	unsigned long flags;
-	int res;
-
-	if (!tty || !tty->driver_data ) {
-		CPC_TTY_DBG("hdlx-tty: no TTY in close\n");
-		return;
-	}
-
-	cpc_tty = (st_cpc_tty_area *) tty->driver_data;
-
-	if ((cpc_tty->tty != tty)|| (cpc_tty->state != CPC_TTY_ST_OPEN)) {
-		CPC_TTY_DBG("%s: TTY is not opened\n",cpc_tty->name);
-		return;
-	}
-   	
-	if (!cpc_tty->num_open) {
-		CPC_TTY_DBG("%s: TTY is closed\n",cpc_tty->name);
-		return;
-	}
-
-	if (--cpc_tty->num_open > 0) {
-		CPC_TTY_DBG("%s: TTY closed\n",cpc_tty->name);
-		return;
-	}
-
-	cpc_tty_signal_off(cpc_tty->pc300dev, CTL_DTR);
-
-	CPC_TTY_LOCK(cpc_tty->pc300dev->chan->card, flags);  /* lock irq */ 
-	cpc_tty->tty = NULL;
-	cpc_tty->state = CPC_TTY_ST_INIT;
-	CPC_TTY_UNLOCK(cpc_tty->pc300dev->chan->card, flags); /* unlock irq */ 
-	
-	if (cpc_tty->buf_rx.first) {
-		unsigned char * aux;
-		while (cpc_tty->buf_rx.first) {
-			aux = (unsigned char *)cpc_tty->buf_rx.first;
-			cpc_tty->buf_rx.first = cpc_tty->buf_rx.first->next;
-			kfree(aux);
-		}
-		cpc_tty->buf_rx.first = NULL;
-		cpc_tty->buf_rx.last = NULL;
-	}
-	
-	kfree(cpc_tty->buf_tx);
-	cpc_tty->buf_tx = NULL;
-
-	CPC_TTY_DBG("%s: TTY closed\n",cpc_tty->name);
-	
-	if (!serial_drv.refcount && cpc_tty_unreg_flag) {
-		cpc_tty_unreg_flag = 0;
-		CPC_TTY_DBG("%s: unregister the tty driver\n", cpc_tty->name);
-		if ((res=tty_unregister_driver(&serial_drv))) { 
-			CPC_TTY_DBG("%s: ERROR ->unregister the tty driver error=%d\n",
-							cpc_tty->name,res);
-		}
-	}
-	return; 
-} 
-
-/*
- * PC300 TTY WRITE routine
- *
- * This routine is called by the tty driver to write a series of characters
- * to the tty device. The characters may come from user or kernel space.
- * o verify the DCD signal
- * o send characters to board and start the transmission
- */
-static int cpc_tty_write(struct tty_struct *tty, const unsigned char *buf, int count)
-{
-	st_cpc_tty_area    *cpc_tty; 
-	pc300ch_t *pc300chan; 
-	pc300_t *card; 
-	int ch; 
-	unsigned long flags; 
-	struct net_device_stats *stats; 
-
-	if (!tty || !tty->driver_data ) { 
-		CPC_TTY_DBG("hdlcX-tty: no TTY in write\n");
-		return -ENODEV;
-	} 
-
-	cpc_tty = (st_cpc_tty_area *) tty->driver_data; 
-
-	if ((cpc_tty->tty != tty) ||  (cpc_tty->state != CPC_TTY_ST_OPEN)) { 
-		CPC_TTY_DBG("%s: TTY is not opened\n", cpc_tty->name);
-		return -ENODEV; 
-	}
-
-	if (count > CPC_TTY_MAX_MTU) { 
-		CPC_TTY_DBG("%s: count is invalid\n",cpc_tty->name);
-		return -EINVAL;        /* frame too big */ 
-	}
-
-	CPC_TTY_DBG("%s: cpc_tty_write data len=%i\n",cpc_tty->name,count);
-	
-	pc300chan = (pc300ch_t *)((pc300dev_t*)cpc_tty->pc300dev)->chan; 
-	stats = &cpc_tty->pc300dev->dev->stats;
-	card = (pc300_t *) pc300chan->card;
-	ch = pc300chan->channel; 
-
-	/* verify DCD signal*/ 
-	if (cpc_readb(card->hw.scabase + M_REG(ST3,ch)) & ST3_DCD) { 
-		/* DCD is OFF */ 
-		CPC_TTY_DBG("%s : DCD is OFF\n", cpc_tty->name);
-		stats->tx_errors++;
-		stats->tx_carrier_errors++;
-		CPC_TTY_LOCK(card, flags); 
-		cpc_writeb(card->hw.scabase + M_REG(CMD, ch), CMD_TX_BUF_CLR); 
-		
-		if (card->hw.type == PC300_TE) { 
-			cpc_writeb(card->hw.falcbase + card->hw.cpld_reg2, 
-				cpc_readb(card->hw.falcbase + card->hw.cpld_reg2) & 
-				~(CPLD_REG2_FALC_LED1 << (2 *ch))); 
-		}
-
-		CPC_TTY_UNLOCK(card, flags); 
-
-		return -EINVAL; 
-	}
-
-	if (cpc_tty_send_to_card(cpc_tty->pc300dev, (void*)buf, count)) { 
-	   /* failed to send */
-	   CPC_TTY_DBG("%s: trasmition error\n", cpc_tty->name);
-	   return 0;
-	}
-	return count; 
-} 
-
-/*
- * PC300 TTY Write Room routine
- * 
- * This routine returns the numbers of characteres the tty driver will accept
- * for queuing to be written. 
- * o return MTU
- */
-static int cpc_tty_write_room(struct tty_struct *tty)
-{
-	st_cpc_tty_area    *cpc_tty; 
-
-	if (!tty || !tty->driver_data ) { 
-		CPC_TTY_DBG("hdlcX-tty: no TTY to write room\n");
-		return -ENODEV;
-	}
-
-	cpc_tty = (st_cpc_tty_area *) tty->driver_data; 
-
-	if ((cpc_tty->tty != tty) ||  (cpc_tty->state != CPC_TTY_ST_OPEN)) { 
-		CPC_TTY_DBG("%s: TTY is not opened\n",cpc_tty->name);
-		return -ENODEV; 
-	}
-   	
-	CPC_TTY_DBG("%s: write room\n",cpc_tty->name);
-	
-	return CPC_TTY_MAX_MTU;
-} 
-
-/*
- * PC300 TTY chars in buffer routine
- * 
- * This routine returns the chars number in the transmission buffer 
- * o returns 0
- */
-static int cpc_tty_chars_in_buffer(struct tty_struct *tty)
-{
-	st_cpc_tty_area    *cpc_tty; 
-
-	if (!tty || !tty->driver_data ) {
-		CPC_TTY_DBG("hdlcX-tty: no TTY to chars in buffer\n");
-		return -ENODEV; 
-	}
-
-	cpc_tty = (st_cpc_tty_area *) tty->driver_data; 
-
-	if ((cpc_tty->tty != tty) || (cpc_tty->state != CPC_TTY_ST_OPEN)) { 
-		CPC_TTY_DBG("%s: TTY is not opened\n",cpc_tty->name);
-		return -ENODEV; 
-	}
-   
-	return 0;
-} 
-
-static int pc300_tiocmset(struct tty_struct *tty,
-			  unsigned int set, unsigned int clear)
-{
-	st_cpc_tty_area    *cpc_tty; 
-
-	CPC_TTY_DBG("%s: set:%x clear:%x\n", __func__, set, clear);
-
-	if (!tty || !tty->driver_data ) {
-	   	CPC_TTY_DBG("hdlcX-tty: no TTY to chars in buffer\n");	
-		return -ENODEV; 
-	}
-
-	cpc_tty = (st_cpc_tty_area *) tty->driver_data; 
-
-	if (set & TIOCM_RTS)
-		cpc_tty_signal_on(cpc_tty->pc300dev, CTL_RTS);
-	if (set & TIOCM_DTR)
-		cpc_tty_signal_on(cpc_tty->pc300dev, CTL_DTR);
-
-	if (clear & TIOCM_RTS)
-		cpc_tty_signal_off(cpc_tty->pc300dev, CTL_RTS);
-	if (clear & TIOCM_DTR)
-		cpc_tty_signal_off(cpc_tty->pc300dev, CTL_DTR);
-
-	return 0;
-}
-
-static int pc300_tiocmget(struct tty_struct *tty)
-{
-	unsigned int result;
-	unsigned char status;
-	unsigned long flags;
-	st_cpc_tty_area  *cpc_tty = (st_cpc_tty_area *) tty->driver_data;
-	pc300dev_t *pc300dev = cpc_tty->pc300dev;
-	pc300ch_t *pc300chan = (pc300ch_t *)pc300dev->chan;
-	pc300_t *card = (pc300_t *) pc300chan->card;
-	int ch = pc300chan->channel;
-
-	cpc_tty = (st_cpc_tty_area *) tty->driver_data;
-
-	CPC_TTY_DBG("%s-tty: tiocmget\n",
-		((struct net_device*)(pc300dev->hdlc))->name);
-
-	CPC_TTY_LOCK(card, flags);
-	status = cpc_readb(card->hw.scabase+M_REG(CTL,ch));
-	CPC_TTY_UNLOCK(card,flags);
-
-	result = ((status & CTL_DTR) ? TIOCM_DTR : 0) |
-		 ((status & CTL_RTS) ? TIOCM_RTS : 0);
-
-	return result;
-}
-
-/*
- * PC300 TTY Flush Buffer routine
- *
- * This routine resets the transmission buffer 
- */
-static void cpc_tty_flush_buffer(struct tty_struct *tty)
-{ 
-	st_cpc_tty_area    *cpc_tty; 
-	
-	if (!tty || !tty->driver_data ) {
-	   	CPC_TTY_DBG("hdlcX-tty: no TTY to flush buffer\n");	
-		return; 
-	}
-
-	cpc_tty = (st_cpc_tty_area *) tty->driver_data; 
-
-	if ((cpc_tty->tty != tty) ||  (cpc_tty->state != CPC_TTY_ST_OPEN)) { 
-		CPC_TTY_DBG("%s: TTY is not opened\n",cpc_tty->name);
-		return; 
-	}
-
-	CPC_TTY_DBG("%s: call wake_up_interruptible\n",cpc_tty->name);
-
-	tty_wakeup(tty);	
-	return; 
-} 
-
-/*
- * PC300 TTY Hangup routine
- *
- * This routine is called by the tty driver to hangup the interface 
- * o clear DTR signal
- */
-
-static void cpc_tty_hangup(struct tty_struct *tty)
-{ 
-	st_cpc_tty_area    *cpc_tty; 
-	int res;
-
-	if (!tty || !tty->driver_data ) {
-		CPC_TTY_DBG("hdlcX-tty: no TTY to hangup\n");	
-		return ; 
-	}
-
-	cpc_tty = (st_cpc_tty_area *) tty->driver_data; 
-
-	if ((cpc_tty->tty != tty) || (cpc_tty->state != CPC_TTY_ST_OPEN)) {
-		CPC_TTY_DBG("%s: TTY is not opened\n",cpc_tty->name);
-		return ;
-	}
-	if (!serial_drv.refcount && cpc_tty_unreg_flag) {
-		cpc_tty_unreg_flag = 0;
-		CPC_TTY_DBG("%s: unregister the tty driver\n", cpc_tty->name);
-		if ((res=tty_unregister_driver(&serial_drv))) { 
-			CPC_TTY_DBG("%s: ERROR ->unregister the tty driver error=%d\n",
-							cpc_tty->name,res);
-		}
-	}
-	cpc_tty_signal_off(cpc_tty->pc300dev, CTL_DTR);
-}
-
-/*
- * PC300 TTY RX work routine
- * This routine treats RX work
- * o verify read buffer
- * o call the line disc. read
- * o free memory
- */
-static void cpc_tty_rx_work(struct work_struct *work)
-{
-	st_cpc_tty_area *cpc_tty;
-	unsigned long port;
-	int i, j;
-	volatile st_cpc_rx_buf *buf;
-	char flags=0,flg_rx=1; 
-	struct tty_ldisc *ld;
-
-	if (cpc_tty_cnt == 0) return;
-	
-	for (i=0; (i < 4) && flg_rx ; i++) {
-		flg_rx = 0;
-
-		cpc_tty = container_of(work, st_cpc_tty_area, tty_rx_work);
-		port = cpc_tty - cpc_tty_area;
-
-		for (j=0; j < CPC_TTY_NPORTS; j++) {
-			cpc_tty = &cpc_tty_area[port];
-		
-			if ((buf=cpc_tty->buf_rx.first) != NULL) {
-				if (cpc_tty->tty) {
-					ld = tty_ldisc_ref(cpc_tty->tty);
-					if (ld) {
-						if (ld->ops->receive_buf) {
-							CPC_TTY_DBG("%s: call line disc. receive_buf\n",cpc_tty->name);
-							ld->ops->receive_buf(cpc_tty->tty, (char *)(buf->data), &flags, buf->size);
-						}
-						tty_ldisc_deref(ld);
-					}
-				}	
-				cpc_tty->buf_rx.first = cpc_tty->buf_rx.first->next;
-				kfree((void *)buf);
-				buf = cpc_tty->buf_rx.first;
-				flg_rx = 1;
-			}
-			if (++port == CPC_TTY_NPORTS) port = 0;
-		}
-	}
-} 
-
-/*
- * PC300 TTY RX work routine
- *
- * This routine treats RX interrupt. 
- * o read all frames in card
- * o verify the frame size
- * o read the frame in rx buffer
- */
-static void cpc_tty_rx_disc_frame(pc300ch_t *pc300chan)
-{
-	volatile pcsca_bd_t __iomem * ptdescr; 
-	volatile unsigned char status; 
-	pc300_t *card = (pc300_t *)pc300chan->card; 
-	int ch = pc300chan->channel; 
-
-	/* dma buf read */ 
-	ptdescr = (pcsca_bd_t __iomem *)(card->hw.rambase + 
-				RX_BD_ADDR(ch, pc300chan->rx_first_bd)); 
-	while (pc300chan->rx_first_bd != pc300chan->rx_last_bd) { 
-		status = cpc_readb(&ptdescr->status); 
-		cpc_writeb(&ptdescr->status, 0); 
-		cpc_writeb(&ptdescr->len, 0); 
-		pc300chan->rx_first_bd = (pc300chan->rx_first_bd + 1) & 
-					(N_DMA_RX_BUF - 1); 
-		if (status & DST_EOM) { 
-			break; /* end of message */
-		}
-		ptdescr = (pcsca_bd_t __iomem *)(card->hw.rambase + cpc_readl(&ptdescr->next)); 
-	}
-}
-
-void cpc_tty_receive(pc300dev_t *pc300dev)
-{
-	st_cpc_tty_area *cpc_tty; 
-	pc300ch_t *pc300chan = (pc300ch_t *)pc300dev->chan; 
-	pc300_t *card = (pc300_t *)pc300chan->card; 
-	int ch = pc300chan->channel; 
-	volatile pcsca_bd_t  __iomem * ptdescr; 
-	struct net_device_stats *stats = &pc300dev->dev->stats;
-	int rx_len, rx_aux; 
-	volatile unsigned char status; 
-	unsigned short first_bd = pc300chan->rx_first_bd;
-	st_cpc_rx_buf *new = NULL;
-	unsigned char dsr_rx;
-
-	if (pc300dev->cpc_tty == NULL) { 
-		return; 
-	}
-
-	dsr_rx = cpc_readb(card->hw.scabase + DSR_RX(ch));
-
-	cpc_tty = pc300dev->cpc_tty;
-
-	while (1) { 
-		rx_len = 0;
-		ptdescr = (pcsca_bd_t  __iomem *)(card->hw.rambase + RX_BD_ADDR(ch, first_bd));
-		while ((status = cpc_readb(&ptdescr->status)) & DST_OSB) {
-			rx_len += cpc_readw(&ptdescr->len);
-			first_bd = (first_bd + 1) & (N_DMA_RX_BUF - 1);
-			if (status & DST_EOM) {
-				break;
-			}
-			ptdescr = (pcsca_bd_t __iomem *)(card->hw.rambase+cpc_readl(&ptdescr->next));
-		}
-			
-		if (!rx_len) { 
-			if (dsr_rx & DSR_BOF) {
-				/* update EDA */ 
-				cpc_writel(card->hw.scabase + DRX_REG(EDAL, ch), 
-						RX_BD_ADDR(ch, pc300chan->rx_last_bd)); 
-			}
-			kfree(new);
-			return; 
-		}
-		
-		if (rx_len > CPC_TTY_MAX_MTU) { 
-			/* Free RX descriptors */ 
-			CPC_TTY_DBG("%s: frame size is invalid.\n",cpc_tty->name);
-			stats->rx_errors++; 
-			stats->rx_frame_errors++; 
-			cpc_tty_rx_disc_frame(pc300chan);
-			continue;
-		} 
-		
-		new = kmalloc(rx_len + sizeof(st_cpc_rx_buf), GFP_ATOMIC);
-		if (!new) {
-			cpc_tty_rx_disc_frame(pc300chan);
-			continue;
-		}
-		
-		/* dma buf read */ 
-		ptdescr = (pcsca_bd_t __iomem *)(card->hw.rambase + 
-				RX_BD_ADDR(ch, pc300chan->rx_first_bd)); 
-
-		rx_len = 0;	/* counter frame size */
-		
-		while ((status = cpc_readb(&ptdescr->status)) & DST_OSB) {
-			rx_aux = cpc_readw(&ptdescr->len);
-			if ((status & (DST_OVR | DST_CRC | DST_RBIT |  DST_SHRT | DST_ABT))
-				|| (rx_aux > BD_DEF_LEN)) {
-				CPC_TTY_DBG("%s: reception error\n", cpc_tty->name);
-				stats->rx_errors++; 
-				if (status & DST_OVR) { 
-					stats->rx_fifo_errors++; 
-				}
-				if (status & DST_CRC) { 
-					stats->rx_crc_errors++; 
-				}
-				if ((status & (DST_RBIT | DST_SHRT | DST_ABT)) ||
-					(rx_aux > BD_DEF_LEN))	{ 
-					stats->rx_frame_errors++; 
-				} 
-				/* discard remainig descriptors used by the bad frame */ 
-				CPC_TTY_DBG("%s: reception error - discard descriptors",
-						cpc_tty->name);
-				cpc_tty_rx_disc_frame(pc300chan);
-				rx_len = 0;
-				kfree(new);
-				new = NULL;
-				break; /* read next frame - while(1) */
-			}
-
-			if (cpc_tty->state != CPC_TTY_ST_OPEN) {
-				/* Free RX descriptors */ 
-				cpc_tty_rx_disc_frame(pc300chan);
-				stats->rx_dropped++; 
-				rx_len = 0; 
-				kfree(new);
-				new = NULL;
-				break; /* read next frame - while(1) */
-			}
-
-			/* read the segment of the frame */
-			if (rx_aux != 0) {
-				memcpy_fromio((new->data + rx_len), 
-					(void __iomem *)(card->hw.rambase + 
-					 cpc_readl(&ptdescr->ptbuf)), rx_aux);
-				rx_len += rx_aux; 
-			}
-			cpc_writeb(&ptdescr->status,0); 
-			cpc_writeb(&ptdescr->len, 0); 
-			pc300chan->rx_first_bd = (pc300chan->rx_first_bd + 1) & 
-					(N_DMA_RX_BUF -1); 
-			if (status & DST_EOM)break;
-			
-			ptdescr = (pcsca_bd_t __iomem *) (card->hw.rambase + 
-					cpc_readl(&ptdescr->next)); 
-		}
-		/* update pointer */ 
-		pc300chan->rx_last_bd = (pc300chan->rx_first_bd - 1) & 
-					(N_DMA_RX_BUF - 1) ; 
-		if (!(dsr_rx & DSR_BOF)) {
-			/* update EDA */ 
-			cpc_writel(card->hw.scabase + DRX_REG(EDAL, ch), 
-					RX_BD_ADDR(ch, pc300chan->rx_last_bd)); 
-		}
-		if (rx_len != 0) { 
-			stats->rx_bytes += rx_len; 
-		
-			if (pc300dev->trace_on) { 
-				cpc_tty_trace(pc300dev, new->data,rx_len, 'R'); 
-			} 
-			new->size = rx_len;
-			new->next = NULL;
-			if (cpc_tty->buf_rx.first == NULL) {
-				cpc_tty->buf_rx.first = new;
-				cpc_tty->buf_rx.last = new;
-			} else {
-				cpc_tty->buf_rx.last->next = new;
-				cpc_tty->buf_rx.last = new;
-			}
-			schedule_work(&(cpc_tty->tty_rx_work));
-			stats->rx_packets++;
-		}
-	} 
-} 
-
-/*
- * PC300 TTY TX work routine
- * 
- * This routine treats TX interrupt. 
- * o if need call line discipline wakeup
- * o call wake_up_interruptible
- */
-static void cpc_tty_tx_work(struct work_struct *work)
-{
-	st_cpc_tty_area *cpc_tty =
-		container_of(work, st_cpc_tty_area, tty_tx_work);
-	struct tty_struct *tty; 
-
-	CPC_TTY_DBG("%s: cpc_tty_tx_work init\n",cpc_tty->name);
-	
-	if ((tty = cpc_tty->tty) == NULL) { 
-		CPC_TTY_DBG("%s: the interface is not opened\n",cpc_tty->name);
-		return; 
-	}
-	tty_wakeup(tty);
-}
-
-/*
- * PC300 TTY send to card routine
- * 
- * This routine send data to card. 
- * o clear descriptors
- * o write data to DMA buffers
- * o start the transmission
- */
-static int cpc_tty_send_to_card(pc300dev_t *dev,void* buf, int len)
-{
-	pc300ch_t *chan = (pc300ch_t *)dev->chan; 
-	pc300_t *card = (pc300_t *)chan->card; 
-	int ch = chan->channel; 
-	struct net_device_stats *stats = &dev->dev->stats;
-	unsigned long flags; 
-	volatile pcsca_bd_t __iomem *ptdescr; 
-	int i, nchar;
-	int tosend = len;
-	int nbuf = ((len - 1)/BD_DEF_LEN) + 1;
-	unsigned char *pdata=buf;
-
-	CPC_TTY_DBG("%s:cpc_tty_send_to_cars len=%i", 
-			(st_cpc_tty_area *)dev->cpc_tty->name,len);	
-
-	if (nbuf >= card->chan[ch].nfree_tx_bd) {
-		return 1;
-	}
-	
-	/* write buffer to DMA buffers */ 
-	CPC_TTY_DBG("%s: call dma_buf_write\n",
-			(st_cpc_tty_area *)dev->cpc_tty->name);	
-	for (i = 0 ; i < nbuf ; i++) {
-		ptdescr = (pcsca_bd_t __iomem *)(card->hw.rambase + 
-			TX_BD_ADDR(ch, card->chan[ch].tx_next_bd));
-		nchar = (BD_DEF_LEN > tosend) ? tosend : BD_DEF_LEN;
-		if (cpc_readb(&ptdescr->status) & DST_OSB) {
-			memcpy_toio((void __iomem *)(card->hw.rambase + 
-				cpc_readl(&ptdescr->ptbuf)), 
-				&pdata[len - tosend], 
-				nchar);
-			card->chan[ch].nfree_tx_bd--;
-			if ((i + 1) == nbuf) {
-				/* This must be the last BD to be used */
-				cpc_writeb(&ptdescr->status, DST_EOM);
-			} else {
-				cpc_writeb(&ptdescr->status, 0);
-			}
-			cpc_writew(&ptdescr->len, nchar);
-		} else {
-			CPC_TTY_DBG("%s: error in dma_buf_write\n",
-					(st_cpc_tty_area *)dev->cpc_tty->name);	
-			stats->tx_dropped++;
-			return 1; 
-		}
-		tosend -= nchar;
-		card->chan[ch].tx_next_bd = 
-			(card->chan[ch].tx_next_bd + 1) & (N_DMA_TX_BUF - 1);
-	}
-
-	if (dev->trace_on) { 
-		cpc_tty_trace(dev, buf, len,'T'); 
-	}
-
-	/* start transmission */ 
-	CPC_TTY_DBG("%s: start transmission\n",
-		(st_cpc_tty_area *)dev->cpc_tty->name);	
-	
-	CPC_TTY_LOCK(card, flags); 
-	cpc_writeb(card->hw.scabase + DTX_REG(EDAL, ch), 
-			TX_BD_ADDR(ch, chan->tx_next_bd)); 
-	cpc_writeb(card->hw.scabase + M_REG(CMD, ch), CMD_TX_ENA); 
-	cpc_writeb(card->hw.scabase + DSR_TX(ch), DSR_DE); 
-
-	if (card->hw.type == PC300_TE) { 
-		cpc_writeb(card->hw.falcbase + card->hw.cpld_reg2, 
-			cpc_readb(card->hw.falcbase + card->hw.cpld_reg2) |
-			(CPLD_REG2_FALC_LED1 << (2 * ch))); 
-	}
-	CPC_TTY_UNLOCK(card, flags); 
-	return 0; 
-} 
-
-/*
- *	PC300 TTY trace routine
- *
- *  This routine send trace of connection to application. 
- *  o clear descriptors
- *  o write data to DMA buffers
- *  o start the transmission
- */
-
-static void cpc_tty_trace(pc300dev_t *dev, char* buf, int len, char rxtx)
-{
-	struct sk_buff *skb; 
-
-	if ((skb = dev_alloc_skb(10 + len)) == NULL) { 
-		/* out of memory */ 
-		CPC_TTY_DBG("%s: tty_trace - out of memory\n", dev->dev->name);
-		return; 
-	}
-
-	skb_put (skb, 10 + len); 
-	skb->dev = dev->dev; 
-	skb->protocol = htons(ETH_P_CUST); 
-	skb_reset_mac_header(skb);
-	skb->pkt_type = PACKET_HOST; 
-	skb->len = 10 + len; 
-
-	skb_copy_to_linear_data(skb, dev->dev->name, 5);
-	skb->data[5] = '['; 
-	skb->data[6] = rxtx; 
-	skb->data[7] = ']'; 
-	skb->data[8] = ':'; 
-	skb->data[9] = ' '; 
-	skb_copy_to_linear_data_offset(skb, 10, buf, len);
-	netif_rx(skb); 
-} 	
-
-/*
- *	PC300 TTY unregister service routine
- *
- *	This routine unregister one interface. 
- */
-void cpc_tty_unregister_service(pc300dev_t *pc300dev)
-{
-	st_cpc_tty_area *cpc_tty; 
-	ulong flags;
-	int res;
-
-	if ((cpc_tty= (st_cpc_tty_area *) pc300dev->cpc_tty) == NULL) {
-		CPC_TTY_DBG("%s: interface is not TTY\n", pc300dev->dev->name);
-		return; 
-	}
-	CPC_TTY_DBG("%s: cpc_tty_unregister_service", cpc_tty->name);
-
-	if (cpc_tty->pc300dev != pc300dev) { 
-		CPC_TTY_DBG("%s: invalid tty ptr=%s\n", 
-		pc300dev->dev->name, cpc_tty->name);
-		return; 
-	}
-
-	if (--cpc_tty_cnt == 0) { 
-		if (serial_drv.refcount) {
-			CPC_TTY_DBG("%s: unregister is not possible, refcount=%d",
-							cpc_tty->name, serial_drv.refcount);
-			cpc_tty_cnt++;
-			cpc_tty_unreg_flag = 1;
-			return;
-		} else { 
-			CPC_TTY_DBG("%s: unregister the tty driver\n", cpc_tty->name);
-			if ((res=tty_unregister_driver(&serial_drv))) { 
-				CPC_TTY_DBG("%s: ERROR ->unregister the tty driver error=%d\n",
-								cpc_tty->name,res);
-			}
-		}
-	}
-	CPC_TTY_LOCK(pc300dev->chan->card,flags);
-	cpc_tty->tty = NULL; 
-	CPC_TTY_UNLOCK(pc300dev->chan->card, flags);
-	cpc_tty->tty_minor = 0; 
-	cpc_tty->state = CPC_TTY_ST_IDLE; 
-} 
-
-/*
- * PC300 TTY trigger poll routine
- * This routine is called by pc300driver to treats Tx interrupt. 
- */
-void cpc_tty_trigger_poll(pc300dev_t *pc300dev)
-{
-	st_cpc_tty_area *cpc_tty = (st_cpc_tty_area *)pc300dev->cpc_tty; 
-	if (!cpc_tty) {
-		return;
-	}
-	schedule_work(&(cpc_tty->tty_tx_work)); 
-} 
diff --git a/drivers/staging/netlogic/Kconfig b/drivers/staging/netlogic/Kconfig
new file mode 100644
index 00000000000..d660de51b54
--- /dev/null
+++ b/drivers/staging/netlogic/Kconfig
@@ -0,0 +1,7 @@
+config NETLOGIC_XLR_NET
+	tristate "Netlogic XLR/XLS network device"
+	depends on CPU_XLR
+	select PHYLIB
+	---help---
+	This driver support Netlogic XLR/XLS on chip gigabit
+	Ethernet.
diff --git a/drivers/staging/netlogic/Makefile b/drivers/staging/netlogic/Makefile
new file mode 100644
index 00000000000..f7355e3e9c4
--- /dev/null
+++ b/drivers/staging/netlogic/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_NETLOGIC_XLR_NET) += xlr_net.o platform_net.o
diff --git a/drivers/staging/netlogic/TODO b/drivers/staging/netlogic/TODO
new file mode 100644
index 00000000000..08e6d5218b3
--- /dev/null
+++ b/drivers/staging/netlogic/TODO
@@ -0,0 +1,12 @@
+* Implementing 64bit stat counter in software
+* All memory allocation should be changed to DMA allocations
+* All the netdev should be linked to single pdev as parent
+* Changing comments in to linux standred format
+
+Please send patches
+To:
+Ganesan Ramalingam <ganesanr@broadcom.com>
+Greg Kroah-Hartman <gregkh@linuxfoundation.org>
+Cc:
+Jayachandran Chandrashekaran Nair <jchandra@broadcom.com>
+
diff --git a/drivers/staging/netlogic/platform_net.c b/drivers/staging/netlogic/platform_net.c
new file mode 100644
index 00000000000..61f20e10d63
--- /dev/null
+++ b/drivers/staging/netlogic/platform_net.c
@@ -0,0 +1,223 @@
+/*
+ * Copyright (c) 2003-2012 Broadcom Corporation
+ * All Rights Reserved
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * 1. Redistributions of source code must retain the above copyright
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the Broadcom
+ * license below:
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY BROADCOM ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL BROADCOM OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
+ * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
+ * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
+ * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/ioport.h>
+#include <linux/resource.h>
+#include <linux/phy.h>
+
+#include <asm/netlogic/haldefs.h>
+#include <asm/netlogic/common.h>
+#include <asm/netlogic/xlr/fmn.h>
+#include <asm/netlogic/xlr/xlr.h>
+#include <asm/netlogic/psb-bootinfo.h>
+#include <asm/netlogic/xlr/pic.h>
+#include <asm/netlogic/xlr/iomap.h>
+
+#include "platform_net.h"
+
+/* Linux Net */
+#define MAX_NUM_GMAC		8
+#define MAX_NUM_XLS_GMAC	8
+#define MAX_NUM_XLR_GMAC	4
+
+
+static u32 xlr_gmac_offsets[] = {
+	NETLOGIC_IO_GMAC_0_OFFSET, NETLOGIC_IO_GMAC_1_OFFSET,
+	NETLOGIC_IO_GMAC_2_OFFSET, NETLOGIC_IO_GMAC_3_OFFSET,
+	NETLOGIC_IO_GMAC_4_OFFSET, NETLOGIC_IO_GMAC_5_OFFSET,
+	NETLOGIC_IO_GMAC_6_OFFSET, NETLOGIC_IO_GMAC_7_OFFSET
+};
+
+static u32 xlr_gmac_irqs[] = { PIC_GMAC_0_IRQ, PIC_GMAC_1_IRQ,
+	PIC_GMAC_2_IRQ, PIC_GMAC_3_IRQ,
+	PIC_GMAC_4_IRQ, PIC_GMAC_5_IRQ,
+	PIC_GMAC_6_IRQ, PIC_GMAC_7_IRQ
+};
+
+static struct xlr_net_data ndata[MAX_NUM_GMAC];
+static struct resource xlr_net_res[8][2];
+static struct platform_device xlr_net_dev[8];
+static u32 __iomem *gmac0_addr;
+static u32 __iomem *gmac4_addr;
+static u32 __iomem *gpio_addr;
+
+static void config_mac(struct xlr_net_data *nd, int phy, u32 __iomem *serdes,
+		u32 __iomem *pcs, int rfr, int tx, int *bkt_size,
+		struct xlr_fmn_info *gmac_fmn_info, int phy_addr)
+{
+	nd->cpu_mask = nlm_current_node()->coremask;
+	nd->phy_interface = phy;
+	nd->rfr_station = rfr;
+	nd->tx_stnid = tx;
+	nd->mii_addr = gmac0_addr;
+	nd->serdes_addr = serdes;
+	nd->pcs_addr = pcs;
+	nd->gpio_addr = gpio_addr;
+
+	nd->bucket_size = bkt_size;
+	nd->gmac_fmn_info = gmac_fmn_info;
+	nd->phy_addr = phy_addr;
+}
+
+static void net_device_init(int id, struct resource *res, int offset, int irq)
+{
+	res[0].name = "gmac";
+	res[0].start = CPHYSADDR(nlm_mmio_base(offset));
+	res[0].end = res[0].start + 0xfff;
+	res[0].flags = IORESOURCE_MEM;
+
+	res[1].name = "gmac";
+	res[1].start = irq;
+	res[1].end = irq;
+	res[1].flags = IORESOURCE_IRQ;
+
+	xlr_net_dev[id].name = "xlr-net";
+	xlr_net_dev[id].id = id;
+	xlr_net_dev[id].num_resources = 2;
+	xlr_net_dev[id].resource = res;
+	xlr_net_dev[id].dev.platform_data = &ndata[id];
+}
+
+static void xls_gmac_init(void)
+{
+	int mac;
+
+	gmac4_addr = ioremap(CPHYSADDR(
+		nlm_mmio_base(NETLOGIC_IO_GMAC_4_OFFSET)), 0xfff);
+	/* Passing GPIO base for serdes init. Only needed on sgmii ports*/
+	gpio_addr = ioremap(CPHYSADDR(
+		nlm_mmio_base(NETLOGIC_IO_GPIO_OFFSET)), 0xfff);
+
+	switch (nlm_prom_info.board_major_version) {
+	case 12:
+		/* first block RGMII or XAUI, use RGMII */
+		config_mac(&ndata[0],
+			PHY_INTERFACE_MODE_RGMII,
+			gmac0_addr,	/* serdes */
+			gmac0_addr,	/* pcs */
+			FMN_STNID_GMACRFR_0,
+			FMN_STNID_GMAC0_TX0,
+			xlr_board_fmn_config.bucket_size,
+			&xlr_board_fmn_config.gmac[0],
+			0);
+
+		net_device_init(0, xlr_net_res[0], xlr_gmac_offsets[0],
+				xlr_gmac_irqs[0]);
+		platform_device_register(&xlr_net_dev[0]);
+
+		/* second block is XAUI, not supported yet */
+		break;
+	default:
+		/* default XLS config, all ports SGMII */
+		for (mac = 0; mac < 4; mac++) {
+			config_mac(&ndata[mac],
+				PHY_INTERFACE_MODE_SGMII,
+				gmac0_addr,	/* serdes */
+				gmac0_addr,	/* pcs */
+				FMN_STNID_GMACRFR_0,
+				FMN_STNID_GMAC0_TX0 + mac,
+				xlr_board_fmn_config.bucket_size,
+				&xlr_board_fmn_config.gmac[0],
+				/* PHY address according to chip/board */
+				mac + 0x10);
+
+			net_device_init(mac, xlr_net_res[mac],
+					xlr_gmac_offsets[mac],
+					xlr_gmac_irqs[mac]);
+			platform_device_register(&xlr_net_dev[mac]);
+		}
+
+		for (mac = 4; mac < MAX_NUM_XLS_GMAC; mac++) {
+			config_mac(&ndata[mac],
+				PHY_INTERFACE_MODE_SGMII,
+				gmac4_addr,	/* serdes */
+				gmac4_addr,	/* pcs */
+				FMN_STNID_GMAC1_FR_0,
+				FMN_STNID_GMAC1_TX0 + mac - 4,
+				xlr_board_fmn_config.bucket_size,
+				&xlr_board_fmn_config.gmac[1],
+				/* PHY address according to chip/board */
+				mac + 0x10);
+
+			net_device_init(mac, xlr_net_res[mac],
+					xlr_gmac_offsets[mac],
+					xlr_gmac_irqs[mac]);
+			platform_device_register(&xlr_net_dev[mac]);
+		}
+	}
+}
+
+static void xlr_gmac_init(void)
+{
+	int mac;
+
+	/* assume all GMACs for now */
+	for (mac = 0; mac < MAX_NUM_XLR_GMAC; mac++) {
+		config_mac(&ndata[mac],
+			PHY_INTERFACE_MODE_RGMII,
+			0,
+			0,
+			FMN_STNID_GMACRFR_0,
+			FMN_STNID_GMAC0_TX0,
+			xlr_board_fmn_config.bucket_size,
+			&xlr_board_fmn_config.gmac[0],
+			mac);
+
+		net_device_init(mac, xlr_net_res[mac], xlr_gmac_offsets[mac],
+				xlr_gmac_irqs[mac]);
+		platform_device_register(&xlr_net_dev[mac]);
+	}
+}
+
+static int __init xlr_net_init(void)
+{
+	gmac0_addr = ioremap(CPHYSADDR(
+		nlm_mmio_base(NETLOGIC_IO_GMAC_0_OFFSET)), 0xfff);
+
+	if (nlm_chip_is_xls())
+		xls_gmac_init();
+	else
+		xlr_gmac_init();
+
+	return 0;
+}
+
+arch_initcall(xlr_net_init);
diff --git a/drivers/staging/netlogic/platform_net.h b/drivers/staging/netlogic/platform_net.h
new file mode 100644
index 00000000000..29deeea72ca
--- /dev/null
+++ b/drivers/staging/netlogic/platform_net.h
@@ -0,0 +1,46 @@
+/*
+ * Copyright (c) 2003-2012 Broadcom Corporation
+ * All Rights Reserved
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the Broadcom
+ * license below:
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY BROADCOM ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL BROADCOM OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
+ * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
+ * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
+ * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+struct xlr_net_data {
+	int cpu_mask;
+	u32 __iomem *mii_addr;
+	u32 __iomem *serdes_addr;
+	u32 __iomem *pcs_addr;
+	u32 __iomem *gpio_addr;
+	int phy_interface;
+	int rfr_station;
+	int tx_stnid;
+	int *bucket_size;
+	int phy_addr;
+	struct xlr_fmn_info *gmac_fmn_info;
+};
diff --git a/drivers/staging/netlogic/xlr_net.c b/drivers/staging/netlogic/xlr_net.c
new file mode 100644
index 00000000000..dd98cb1468a
--- /dev/null
+++ b/drivers/staging/netlogic/xlr_net.c
@@ -0,0 +1,1114 @@
+/*
+ * Copyright (c) 2003-2012 Broadcom Corporation
+ * All Rights Reserved
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the Broadcom
+ * license below:
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY BROADCOM ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL BROADCOM OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
+ * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
+ * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
+ * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <linux/phy.h>
+#include <linux/delay.h>
+#include <linux/netdevice.h>
+#include <linux/smp.h>
+#include <linux/ethtool.h>
+#include <linux/module.h>
+#include <linux/etherdevice.h>
+#include <linux/skbuff.h>
+#include <linux/jiffies.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+
+#include <asm/mipsregs.h>
+
+/* fmn.h - For FMN credit configuration and registering fmn_handler.
+ * FMN is communication mechanism that allows processing agents within
+ * XLR/XLS to communicate each other.
+ */
+#include <asm/netlogic/xlr/fmn.h>
+
+#include "platform_net.h"
+#include "xlr_net.h"
+
+/*
+ * The readl/writel implementation byteswaps on XLR/XLS, so
+ * we need to use __raw_ IO to read the NAE registers
+ * because they are in the big-endian MMIO area on the SoC.
+ */
+static inline void xlr_nae_wreg(u32 __iomem *base, unsigned int reg, u32 val)
+{
+	__raw_writel(val, base + reg);
+}
+
+static inline u32 xlr_nae_rdreg(u32 __iomem *base, unsigned int reg)
+{
+	return __raw_readl(base + reg);
+}
+
+static inline void xlr_reg_update(u32 *base_addr,
+		u32 off, u32 val, u32 mask)
+{
+	u32 tmp;
+
+	tmp = xlr_nae_rdreg(base_addr, off);
+	xlr_nae_wreg(base_addr, off, (tmp & ~mask) | (val & mask));
+}
+
+/*
+ * Table of net_device pointers indexed by port, this will be used to
+ * lookup the net_device corresponding to a port by the message ring handler.
+ *
+ * Maximum ports in XLR/XLS is 8(8 GMAC on XLS, 4 GMAC + 2 XGMAC on XLR)
+ */
+static struct net_device *mac_to_ndev[8];
+
+static inline struct sk_buff *mac_get_skb_back_ptr(void *addr)
+{
+	struct sk_buff **back_ptr;
+
+	/* this function should be used only for newly allocated packets.
+	 * It assumes the first cacheline is for the back pointer related
+	 * book keeping info.
+	 */
+	back_ptr = (struct sk_buff **)(addr - MAC_SKB_BACK_PTR_SIZE);
+	return *back_ptr;
+}
+
+static inline void mac_put_skb_back_ptr(struct sk_buff *skb)
+{
+	struct sk_buff **back_ptr = (struct sk_buff **)skb->data;
+
+	/* this function should be used only for newly allocated packets.
+	 * It assumes the first cacheline is for the back pointer related
+	 * book keeping info.
+	 */
+	skb_reserve(skb, MAC_SKB_BACK_PTR_SIZE);
+	*back_ptr = skb;
+}
+
+static int send_to_rfr_fifo(struct xlr_net_priv *priv, void *addr)
+{
+	struct nlm_fmn_msg msg;
+	int ret = 0, num_try = 0, stnid;
+	unsigned long paddr, mflags;
+
+	paddr = virt_to_bus(addr);
+	msg.msg0 = (u64)paddr & 0xffffffffe0ULL;
+	msg.msg1 = 0;
+	msg.msg2 = 0;
+	msg.msg3 = 0;
+	stnid = priv->nd->rfr_station;
+	do {
+		mflags = nlm_cop2_enable();
+		ret = nlm_fmn_send(1, 0, stnid, &msg);
+		nlm_cop2_restore(mflags);
+		if (ret == 0)
+			return 0;
+	} while (++num_try < 10000);
+
+	pr_err("Send to RFR failed in RX path\n");
+	return ret;
+}
+
+static inline struct sk_buff *xlr_alloc_skb(void)
+{
+	struct sk_buff *skb;
+
+	/* skb->data is cache aligned */
+	skb = alloc_skb(XLR_RX_BUF_SIZE, GFP_ATOMIC);
+	if (!skb) {
+		pr_err("SKB allocation failed\n");
+		return NULL;
+	}
+	mac_put_skb_back_ptr(skb);
+	return skb;
+}
+
+static void xlr_net_fmn_handler(int bkt, int src_stnid, int size,
+		int code, struct nlm_fmn_msg *msg, void *arg)
+{
+	struct sk_buff *skb, *skb_new = NULL;
+	struct net_device *ndev;
+	struct xlr_net_priv *priv;
+	u64 length, port;
+	void *addr;
+
+	length = (msg->msg0 >> 40) & 0x3fff;
+	if (length == 0) {
+		addr = bus_to_virt(msg->msg0 & 0xffffffffffULL);
+		dev_kfree_skb_any(addr);
+	} else if (length) {
+		addr = bus_to_virt(msg->msg0 & 0xffffffffe0ULL);
+		length = length - BYTE_OFFSET - MAC_CRC_LEN;
+		port = msg->msg0 & 0x0f;
+		if (src_stnid == FMN_STNID_GMAC1)
+			port = port + 4;
+		skb = mac_get_skb_back_ptr(addr);
+		skb->dev = mac_to_ndev[port];
+		ndev = skb->dev;
+		priv = netdev_priv(ndev);
+
+		/* 16 byte IP header align */
+		skb_reserve(skb, BYTE_OFFSET);
+		skb_put(skb, length);
+		skb->protocol = eth_type_trans(skb, skb->dev);
+		skb->dev->last_rx = jiffies;
+		netif_rx(skb);
+		/* Fill rx ring */
+		skb_new = xlr_alloc_skb();
+		if (skb_new)
+			send_to_rfr_fifo(priv, skb_new->data);
+	}
+	return;
+}
+
+/* Ethtool operation */
+static int xlr_get_settings(struct net_device *ndev, struct ethtool_cmd *ecmd)
+{
+	struct xlr_net_priv *priv = netdev_priv(ndev);
+	struct phy_device *phydev = priv->mii_bus->phy_map[priv->phy_addr];
+
+	if (!phydev)
+		return -ENODEV;
+	return phy_ethtool_gset(phydev, ecmd);
+}
+
+static int xlr_set_settings(struct net_device *ndev, struct ethtool_cmd *ecmd)
+{
+	struct xlr_net_priv *priv = netdev_priv(ndev);
+	struct phy_device *phydev = priv->mii_bus->phy_map[priv->phy_addr];
+
+	if (!phydev)
+		return -ENODEV;
+	return phy_ethtool_sset(phydev, ecmd);
+}
+
+static struct ethtool_ops xlr_ethtool_ops = {
+	.get_settings = xlr_get_settings,
+	.set_settings = xlr_set_settings,
+};
+
+/* Net operations */
+static int xlr_net_fill_rx_ring(struct net_device *ndev)
+{
+	struct sk_buff *skb;
+	struct xlr_net_priv *priv = netdev_priv(ndev);
+	int i;
+
+	for (i = 0; i < MAX_FRIN_SPILL/2; i++) {
+		skb = xlr_alloc_skb();
+		if (!skb)
+			return -ENOMEM;
+		send_to_rfr_fifo(priv, skb->data);
+	}
+	pr_info("Rx ring setup done\n");
+	return 0;
+}
+
+static int xlr_net_open(struct net_device *ndev)
+{
+	u32 err;
+	struct xlr_net_priv *priv = netdev_priv(ndev);
+	struct phy_device *phydev = priv->mii_bus->phy_map[priv->phy_addr];
+
+	/* schedule a link state check */
+	phy_start(phydev);
+
+	err = phy_start_aneg(phydev);
+	if (err) {
+		pr_err("Autoneg failed\n");
+		return err;
+	}
+
+	/* Setup the speed from PHY to internal reg*/
+	xlr_set_gmac_speed(priv);
+	netif_tx_start_all_queues(ndev);
+	return 0;
+}
+
+static int xlr_net_stop(struct net_device *ndev)
+{
+	struct xlr_net_priv *priv = netdev_priv(ndev);
+	struct phy_device *phydev = priv->mii_bus->phy_map[priv->phy_addr];
+
+	phy_stop(phydev);
+	netif_tx_stop_all_queues(ndev);
+	return 0;
+}
+
+static void xlr_make_tx_desc(struct nlm_fmn_msg *msg, unsigned long addr,
+		struct sk_buff *skb)
+{
+	unsigned long physkb = virt_to_phys(skb);
+	int cpu_core = nlm_core_id();
+	int fr_stn_id = cpu_core * 8 + XLR_FB_STN;	/* FB to 6th bucket */
+	msg->msg0 = (((u64)1 << 63)	|	/* End of packet descriptor */
+		((u64)127 << 54)	|	/* No Free back */
+		(u64)skb->len << 40	|	/* Length of data */
+		((u64)addr));
+	msg->msg1 = (((u64)1 << 63)	|
+		((u64)fr_stn_id << 54)	|	/* Free back id */
+		(u64)0 << 40		|	/* Set len to 0 */
+		((u64)physkb  & 0xffffffff));	/* 32bit address */
+	msg->msg2 = msg->msg3 = 0;
+}
+
+static void __maybe_unused xlr_wakeup_queue(unsigned long dev)
+{
+	struct net_device *ndev = (struct net_device *) dev;
+	struct xlr_net_priv *priv = netdev_priv(ndev);
+	struct phy_device *phydev = priv->mii_bus->phy_map[priv->phy_addr];
+
+	if (phydev->link)
+		netif_tx_wake_queue(netdev_get_tx_queue(ndev, priv->wakeup_q));
+}
+
+static netdev_tx_t xlr_net_start_xmit(struct sk_buff *skb,
+		struct net_device *ndev)
+{
+	struct nlm_fmn_msg msg;
+	struct xlr_net_priv *priv = netdev_priv(ndev);
+	int ret;
+	u32 flags;
+
+	xlr_make_tx_desc(&msg, virt_to_phys(skb->data), skb);
+	flags = nlm_cop2_enable();
+	ret = nlm_fmn_send(2, 0, priv->nd->tx_stnid, &msg);
+	nlm_cop2_restore(flags);
+	if (ret)
+		dev_kfree_skb_any(skb);
+	return NETDEV_TX_OK;
+}
+
+static u16 xlr_net_select_queue(struct net_device *ndev, struct sk_buff *skb)
+{
+	return (u16)smp_processor_id();
+}
+
+static void xlr_hw_set_mac_addr(struct net_device *ndev)
+{
+	struct xlr_net_priv *priv = netdev_priv(ndev);
+
+	/* set mac station address */
+	xlr_nae_wreg(priv->base_addr, R_MAC_ADDR0,
+		((ndev->dev_addr[5] << 24) | (ndev->dev_addr[4] << 16) |
+		(ndev->dev_addr[3] << 8) | (ndev->dev_addr[2])));
+	xlr_nae_wreg(priv->base_addr, R_MAC_ADDR0 + 1,
+		((ndev->dev_addr[1] << 24) | (ndev->dev_addr[0] << 16)));
+
+	xlr_nae_wreg(priv->base_addr, R_MAC_ADDR_MASK2, 0xffffffff);
+	xlr_nae_wreg(priv->base_addr, R_MAC_ADDR_MASK2 + 1, 0xffffffff);
+	xlr_nae_wreg(priv->base_addr, R_MAC_ADDR_MASK3, 0xffffffff);
+	xlr_nae_wreg(priv->base_addr, R_MAC_ADDR_MASK3 + 1, 0xffffffff);
+
+	xlr_nae_wreg(priv->base_addr, R_MAC_FILTER_CONFIG,
+		(1 << O_MAC_FILTER_CONFIG__BROADCAST_EN) |
+		(1 << O_MAC_FILTER_CONFIG__ALL_MCAST_EN) |
+		(1 << O_MAC_FILTER_CONFIG__MAC_ADDR0_VALID));
+
+	if (priv->nd->phy_interface == PHY_INTERFACE_MODE_RGMII ||
+			priv->nd->phy_interface == PHY_INTERFACE_MODE_SGMII)
+		xlr_reg_update(priv->base_addr, R_IPG_IFG, MAC_B2B_IPG, 0x7f);
+}
+
+static int xlr_net_set_mac_addr(struct net_device *ndev, void *data)
+{
+	int err;
+
+	err = eth_mac_addr(ndev, data);
+	if (err)
+		return err;
+	xlr_hw_set_mac_addr(ndev);
+	return 0;
+}
+
+static void xlr_set_rx_mode(struct net_device *ndev)
+{
+	struct xlr_net_priv *priv = netdev_priv(ndev);
+	u32 regval;
+
+	regval = xlr_nae_rdreg(priv->base_addr, R_MAC_FILTER_CONFIG);
+
+	if (ndev->flags & IFF_PROMISC) {
+		regval |= (1 << O_MAC_FILTER_CONFIG__BROADCAST_EN) |
+		(1 << O_MAC_FILTER_CONFIG__PAUSE_FRAME_EN) |
+		(1 << O_MAC_FILTER_CONFIG__ALL_MCAST_EN) |
+		(1 << O_MAC_FILTER_CONFIG__ALL_UCAST_EN);
+	} else {
+		regval &= ~((1 << O_MAC_FILTER_CONFIG__PAUSE_FRAME_EN) |
+		(1 << O_MAC_FILTER_CONFIG__ALL_UCAST_EN));
+	}
+
+	xlr_nae_wreg(priv->base_addr, R_MAC_FILTER_CONFIG, regval);
+}
+
+static void xlr_stats(struct net_device *ndev, struct rtnl_link_stats64 *stats)
+{
+	struct xlr_net_priv *priv = netdev_priv(ndev);
+
+	stats->rx_packets = xlr_nae_rdreg(priv->base_addr, RX_PACKET_COUNTER);
+	stats->tx_packets = xlr_nae_rdreg(priv->base_addr, TX_PACKET_COUNTER);
+	stats->rx_bytes = xlr_nae_rdreg(priv->base_addr, RX_BYTE_COUNTER);
+	stats->tx_bytes = xlr_nae_rdreg(priv->base_addr, TX_BYTE_COUNTER);
+	stats->tx_errors = xlr_nae_rdreg(priv->base_addr, TX_FCS_ERROR_COUNTER);
+	stats->rx_dropped = xlr_nae_rdreg(priv->base_addr,
+			RX_DROP_PACKET_COUNTER);
+	stats->tx_dropped = xlr_nae_rdreg(priv->base_addr,
+			TX_DROP_FRAME_COUNTER);
+
+	stats->multicast = xlr_nae_rdreg(priv->base_addr,
+			RX_MULTICAST_PACKET_COUNTER);
+	stats->collisions = xlr_nae_rdreg(priv->base_addr,
+			TX_TOTAL_COLLISION_COUNTER);
+
+	stats->rx_length_errors = xlr_nae_rdreg(priv->base_addr,
+			RX_FRAME_LENGTH_ERROR_COUNTER);
+	stats->rx_over_errors = xlr_nae_rdreg(priv->base_addr,
+			RX_DROP_PACKET_COUNTER);
+	stats->rx_crc_errors = xlr_nae_rdreg(priv->base_addr,
+			RX_FCS_ERROR_COUNTER);
+	stats->rx_frame_errors = xlr_nae_rdreg(priv->base_addr,
+			RX_ALIGNMENT_ERROR_COUNTER);
+
+	stats->rx_fifo_errors = xlr_nae_rdreg(priv->base_addr,
+			RX_DROP_PACKET_COUNTER);
+	stats->rx_missed_errors = xlr_nae_rdreg(priv->base_addr,
+			RX_CARRIER_SENSE_ERROR_COUNTER);
+
+	stats->rx_errors = (stats->rx_over_errors + stats->rx_crc_errors +
+			stats->rx_frame_errors + stats->rx_fifo_errors +
+			stats->rx_missed_errors);
+
+	stats->tx_aborted_errors = xlr_nae_rdreg(priv->base_addr,
+			TX_EXCESSIVE_COLLISION_PACKET_COUNTER);
+	stats->tx_carrier_errors = xlr_nae_rdreg(priv->base_addr,
+			TX_DROP_FRAME_COUNTER);
+	stats->tx_fifo_errors = xlr_nae_rdreg(priv->base_addr,
+			TX_DROP_FRAME_COUNTER);
+}
+
+static struct rtnl_link_stats64 *xlr_get_stats64(struct net_device *ndev,
+		struct rtnl_link_stats64 *stats)
+{
+	xlr_stats(ndev, stats);
+	return stats;
+}
+
+static struct net_device_ops xlr_netdev_ops = {
+	.ndo_open = xlr_net_open,
+	.ndo_stop = xlr_net_stop,
+	.ndo_start_xmit = xlr_net_start_xmit,
+	.ndo_select_queue = xlr_net_select_queue,
+	.ndo_set_mac_address = xlr_net_set_mac_addr,
+	.ndo_set_rx_mode = xlr_set_rx_mode,
+	.ndo_get_stats64 = xlr_get_stats64,
+};
+
+/* Gmac init */
+static void *xlr_config_spill(struct xlr_net_priv *priv, int reg_start_0,
+		int reg_start_1, int reg_size, int size)
+{
+	void *spill;
+	u32 *base;
+	unsigned long phys_addr;
+	u32 spill_size;
+
+	base = priv->base_addr;
+	spill_size = size;
+	spill = kmalloc(spill_size + SMP_CACHE_BYTES, GFP_ATOMIC);
+	if (!spill)
+		pr_err("Unable to allocate memory for spill area!\n");
+
+	spill = PTR_ALIGN(spill, SMP_CACHE_BYTES);
+	phys_addr = virt_to_phys(spill);
+	dev_dbg(&priv->ndev->dev, "Allocated spill %d bytes at %lx\n",
+			size, phys_addr);
+	xlr_nae_wreg(base, reg_start_0, (phys_addr >> 5) & 0xffffffff);
+	xlr_nae_wreg(base, reg_start_1, ((u64)phys_addr >> 37) & 0x07);
+	xlr_nae_wreg(base, reg_size, spill_size);
+
+	return spill;
+}
+
+/*
+ * Configure the 6 FIFO's that are used by the network accelarator to
+ * communicate with the rest of the XLx device. 4 of the FIFO's are for
+ * packets from NA --> cpu (called Class FIFO's) and 2 are for feeding
+ * the NA with free descriptors.
+ */
+static void xlr_config_fifo_spill_area(struct xlr_net_priv *priv)
+{
+	priv->frin_spill = xlr_config_spill(priv,
+			R_REG_FRIN_SPILL_MEM_START_0,
+			R_REG_FRIN_SPILL_MEM_START_1,
+			R_REG_FRIN_SPILL_MEM_SIZE,
+			MAX_FRIN_SPILL *
+			sizeof(u64));
+	priv->frout_spill = xlr_config_spill(priv,
+			R_FROUT_SPILL_MEM_START_0,
+			R_FROUT_SPILL_MEM_START_1,
+			R_FROUT_SPILL_MEM_SIZE,
+			MAX_FROUT_SPILL *
+			sizeof(u64));
+	priv->class_0_spill = xlr_config_spill(priv,
+			R_CLASS0_SPILL_MEM_START_0,
+			R_CLASS0_SPILL_MEM_START_1,
+			R_CLASS0_SPILL_MEM_SIZE,
+			MAX_CLASS_0_SPILL *
+			sizeof(u64));
+	priv->class_1_spill = xlr_config_spill(priv,
+			R_CLASS1_SPILL_MEM_START_0,
+			R_CLASS1_SPILL_MEM_START_1,
+			R_CLASS1_SPILL_MEM_SIZE,
+			MAX_CLASS_1_SPILL *
+			sizeof(u64));
+	priv->class_2_spill = xlr_config_spill(priv,
+			R_CLASS2_SPILL_MEM_START_0,
+			R_CLASS2_SPILL_MEM_START_1,
+			R_CLASS2_SPILL_MEM_SIZE,
+			MAX_CLASS_2_SPILL *
+			sizeof(u64));
+	priv->class_3_spill = xlr_config_spill(priv,
+			R_CLASS3_SPILL_MEM_START_0,
+			R_CLASS3_SPILL_MEM_START_1,
+			R_CLASS3_SPILL_MEM_SIZE,
+			MAX_CLASS_3_SPILL *
+			sizeof(u64));
+}
+
+/* Configure PDE to Round-Robin distribution of packets to the
+ * available cpu */
+static void xlr_config_pde(struct xlr_net_priv *priv)
+{
+	int i = 0;
+	u64 bkt_map = 0;
+
+	/* Each core has 8 buckets(station) */
+	for (i = 0; i < hweight32(priv->nd->cpu_mask); i++)
+		bkt_map |= (0xff << (i * 8));
+
+	xlr_nae_wreg(priv->base_addr, R_PDE_CLASS_0, (bkt_map & 0xffffffff));
+	xlr_nae_wreg(priv->base_addr, R_PDE_CLASS_0 + 1,
+			((bkt_map >> 32) & 0xffffffff));
+
+	xlr_nae_wreg(priv->base_addr, R_PDE_CLASS_1, (bkt_map & 0xffffffff));
+	xlr_nae_wreg(priv->base_addr, R_PDE_CLASS_1 + 1,
+			((bkt_map >> 32) & 0xffffffff));
+
+	xlr_nae_wreg(priv->base_addr, R_PDE_CLASS_2, (bkt_map & 0xffffffff));
+	xlr_nae_wreg(priv->base_addr, R_PDE_CLASS_2 + 1,
+			((bkt_map >> 32) & 0xffffffff));
+
+	xlr_nae_wreg(priv->base_addr, R_PDE_CLASS_3, (bkt_map & 0xffffffff));
+	xlr_nae_wreg(priv->base_addr, R_PDE_CLASS_3 + 1,
+			((bkt_map >> 32) & 0xffffffff));
+}
+
+/* Setup the Message ring credits, bucket size and other
+ * common configuration */
+static void xlr_config_common(struct xlr_net_priv *priv)
+{
+	struct xlr_fmn_info *gmac = priv->nd->gmac_fmn_info;
+	int start_stn_id = gmac->start_stn_id;
+	int end_stn_id = gmac->end_stn_id;
+	int *bucket_size = priv->nd->bucket_size;
+	int i, j;
+
+	/* Setting non-core MsgBktSize(0x321 - 0x325) */
+	for (i = start_stn_id; i <= end_stn_id; i++) {
+		xlr_nae_wreg(priv->base_addr,
+				R_GMAC_RFR0_BUCKET_SIZE + i - start_stn_id,
+				bucket_size[i]);
+	}
+
+	/* Setting non-core Credit counter register
+	 * Distributing Gmac's credit to CPU's*/
+	for (i = 0; i < 8; i++) {
+		for (j = 0; j < 8; j++)
+			xlr_nae_wreg(priv->base_addr,
+					(R_CC_CPU0_0 + (i * 8)) + j,
+					gmac->credit_config[(i * 8) + j]);
+	}
+
+	xlr_nae_wreg(priv->base_addr, R_MSG_TX_THRESHOLD, 3);
+	xlr_nae_wreg(priv->base_addr, R_DMACR0, 0xffffffff);
+	xlr_nae_wreg(priv->base_addr, R_DMACR1, 0xffffffff);
+	xlr_nae_wreg(priv->base_addr, R_DMACR2, 0xffffffff);
+	xlr_nae_wreg(priv->base_addr, R_DMACR3, 0xffffffff);
+	xlr_nae_wreg(priv->base_addr, R_FREEQCARVE, 0);
+
+	xlr_net_fill_rx_ring(priv->ndev);
+	nlm_register_fmn_handler(start_stn_id, end_stn_id, xlr_net_fmn_handler,
+					NULL);
+}
+
+static void xlr_config_translate_table(struct xlr_net_priv *priv)
+{
+	u32 cpu_mask;
+	u32 val;
+	int bkts[32]; /* one bucket is assumed for each cpu */
+	int b1, b2, c1, c2, i, j, k;
+	int use_bkt;
+
+	use_bkt = 0;
+	cpu_mask = priv->nd->cpu_mask;
+
+	pr_info("Using %s-based distribution\n",
+			(use_bkt) ? "bucket" : "class");
+	j = 0;
+	for (i = 0; i < 32; i++) {
+		if ((1 << i) & cpu_mask) {
+			/* for each cpu, mark the 4+threadid bucket */
+			bkts[j] = ((i / 4) * 8) + (i % 4);
+			j++;
+		}
+	}
+
+	/*configure the 128 * 9 Translation table to send to available buckets*/
+	k = 0;
+	c1 = 3;
+	c2 = 0;
+	for (i = 0; i < 64; i++) {
+		/* On use_bkt set the b0, b1 are used, else
+		 * the 4 classes are used, here implemented
+		 * a logic to distribute the packets to the
+		 * buckets equally or based on the class
+		 */
+		c1 = (c1 + 1) & 3;
+		c2 = (c1 + 1) & 3;
+		b1 = bkts[k];
+		k = (k + 1) % j;
+		b2 = bkts[k];
+		k = (k + 1) % j;
+		val = ((c1 << 23) | (b1 << 17) | (use_bkt << 16) |
+				(c2 << 7) | (b2 << 1) | (use_bkt << 0));
+
+		val = ((c1 << 23) | (b1 << 17) | (use_bkt << 16) |
+				(c2 << 7) | (b2 << 1) | (use_bkt << 0));
+		dev_dbg(&priv->ndev->dev, "Table[%d] b1=%d b2=%d c1=%d c2=%d\n",
+				i, b1, b2, c1, c2);
+		xlr_nae_wreg(priv->base_addr, R_TRANSLATETABLE + i, val);
+		c1 = c2;
+	}
+}
+
+static void xlr_config_parser(struct xlr_net_priv *priv)
+{
+	u32 val;
+
+	/* Mark it as ETHERNET type */
+	xlr_nae_wreg(priv->base_addr, R_L2TYPE_0, 0x01);
+
+	/* Use 7bit CRChash for flow classification with 127 as CRC polynomial*/
+	xlr_nae_wreg(priv->base_addr, R_PARSERCONFIGREG,
+			((0x7f << 8) | (1 << 1)));
+
+	/* configure the parser : L2 Type is configured in the bootloader */
+	/* extract IP: src, dest protocol */
+	xlr_nae_wreg(priv->base_addr, R_L3CTABLE,
+			(9 << 20) | (1 << 19) | (1 << 18) | (0x01 << 16) |
+			(0x0800 << 0));
+	xlr_nae_wreg(priv->base_addr, R_L3CTABLE + 1,
+			(9 << 25) | (1 << 21) | (12 << 14) | (4 << 10) |
+			(16 << 4) | 4);
+
+	/* Configure to extract SRC port and Dest port for TCP and UDP pkts */
+	xlr_nae_wreg(priv->base_addr, R_L4CTABLE, 6);
+	xlr_nae_wreg(priv->base_addr, R_L4CTABLE + 2, 17);
+	val = ((0 << 21) | (2 << 17) | (2 << 11) | (2 << 7));
+	xlr_nae_wreg(priv->base_addr, R_L4CTABLE + 1, val);
+	xlr_nae_wreg(priv->base_addr, R_L4CTABLE + 3, val);
+
+	xlr_config_translate_table(priv);
+}
+
+static int xlr_phy_write(u32 *base_addr, int phy_addr, int regnum, u16 val)
+{
+	unsigned long timeout, stoptime, checktime;
+	int timedout;
+
+	/* 100ms timeout*/
+	timeout = msecs_to_jiffies(100);
+	stoptime = jiffies + timeout;
+	timedout = 0;
+
+	xlr_nae_wreg(base_addr, R_MII_MGMT_ADDRESS, (phy_addr << 8) | regnum);
+
+	/* Write the data which starts the write cycle */
+	xlr_nae_wreg(base_addr, R_MII_MGMT_WRITE_DATA, (u32) val);
+
+	/* poll for the read cycle to complete */
+	while (!timedout) {
+		checktime = jiffies;
+		if (xlr_nae_rdreg(base_addr, R_MII_MGMT_INDICATORS) == 0)
+			break;
+		timedout = time_after(checktime, stoptime);
+	}
+	if (timedout) {
+		pr_info("Phy device write err: device busy");
+		return -EBUSY;
+	}
+
+	return 0;
+}
+
+static int xlr_phy_read(u32 *base_addr, int phy_addr, int regnum)
+{
+	unsigned long timeout, stoptime, checktime;
+	int timedout;
+
+	/* 100ms timeout*/
+	timeout = msecs_to_jiffies(100);
+	stoptime = jiffies + timeout;
+	timedout = 0;
+
+	/* setup the phy reg to be used */
+	xlr_nae_wreg(base_addr, R_MII_MGMT_ADDRESS,
+			(phy_addr << 8) | (regnum << 0));
+
+	/* Issue the read command */
+	xlr_nae_wreg(base_addr, R_MII_MGMT_COMMAND,
+			(1 << O_MII_MGMT_COMMAND__rstat));
+
+
+	/* poll for the read cycle to complete */
+	while (!timedout) {
+		checktime = jiffies;
+		if (xlr_nae_rdreg(base_addr, R_MII_MGMT_INDICATORS) == 0)
+			break;
+		timedout = time_after(checktime, stoptime);
+	}
+	if (timedout) {
+		pr_info("Phy device read err: device busy");
+		return -EBUSY;
+	}
+
+	/* clear the read cycle */
+	xlr_nae_wreg(base_addr, R_MII_MGMT_COMMAND, 0);
+
+	/* Read the data */
+	return xlr_nae_rdreg(base_addr, R_MII_MGMT_STATUS);
+}
+
+static int xlr_mii_write(struct mii_bus *bus, int phy_addr, int regnum, u16 val)
+{
+	struct xlr_net_priv *priv = bus->priv;
+	int ret;
+
+	ret = xlr_phy_write(priv->mii_addr, phy_addr, regnum, val);
+	dev_dbg(&priv->ndev->dev, "mii_write phy %d : %d <- %x [%x]\n",
+			phy_addr, regnum, val, ret);
+	return ret;
+}
+
+static int xlr_mii_read(struct mii_bus *bus, int phy_addr, int regnum)
+{
+	struct xlr_net_priv *priv = bus->priv;
+	int ret;
+
+	ret =  xlr_phy_read(priv->mii_addr, phy_addr, regnum);
+	dev_dbg(&priv->ndev->dev, "mii_read phy %d : %d [%x]\n",
+			phy_addr, regnum, ret);
+	return ret;
+}
+
+/* XLR ports are RGMII. XLS ports are SGMII mostly except the port0,
+ * which can be configured either SGMII or RGMII, considered SGMII
+ * by default, if board setup to RGMII the port_type need to set
+ * accordingly.Serdes and PCS layer need to configured for SGMII
+ */
+static void xlr_sgmii_init(struct xlr_net_priv *priv)
+{
+	int phy;
+
+	xlr_phy_write(priv->serdes_addr, 26, 0, 0x6DB0);
+	xlr_phy_write(priv->serdes_addr, 26, 1, 0xFFFF);
+	xlr_phy_write(priv->serdes_addr, 26, 2, 0xB6D0);
+	xlr_phy_write(priv->serdes_addr, 26, 3, 0x00FF);
+	xlr_phy_write(priv->serdes_addr, 26, 4, 0x0000);
+	xlr_phy_write(priv->serdes_addr, 26, 5, 0x0000);
+	xlr_phy_write(priv->serdes_addr, 26, 6, 0x0005);
+	xlr_phy_write(priv->serdes_addr, 26, 7, 0x0001);
+	xlr_phy_write(priv->serdes_addr, 26, 8, 0x0000);
+	xlr_phy_write(priv->serdes_addr, 26, 9, 0x0000);
+	xlr_phy_write(priv->serdes_addr, 26, 10, 0x0000);
+
+	/* program  GPIO values for serdes init parameters */
+	xlr_nae_wreg(priv->gpio_addr, 0x20, 0x7e6802);
+	xlr_nae_wreg(priv->gpio_addr, 0x10, 0x7104);
+
+	xlr_nae_wreg(priv->gpio_addr, 0x22, 0x7e6802);
+	xlr_nae_wreg(priv->gpio_addr, 0x21, 0x7104);
+
+	/* enable autoneg - more magic */
+	phy = priv->port_id % 4 + 27;
+	xlr_phy_write(priv->pcs_addr, phy, 0, 0x1000);
+	xlr_phy_write(priv->pcs_addr, phy, 0, 0x0200);
+}
+
+void xlr_set_gmac_speed(struct xlr_net_priv *priv)
+{
+	struct phy_device *phydev = priv->mii_bus->phy_map[priv->phy_addr];
+	int speed;
+
+	if (phydev->interface == PHY_INTERFACE_MODE_SGMII)
+		xlr_sgmii_init(priv);
+
+	if (phydev->speed != priv->phy_speed) {
+		pr_info("change %d to %d\n", priv->phy_speed, phydev->speed);
+		speed = phydev->speed;
+		if (speed == SPEED_1000) {
+			/* Set interface to Byte mode */
+			xlr_nae_wreg(priv->base_addr, R_MAC_CONFIG_2, 0x7217);
+			priv->phy_speed = speed;
+		} else if (speed == SPEED_100 || speed == SPEED_10) {
+			/* Set interface to Nibble mode */
+			xlr_nae_wreg(priv->base_addr, R_MAC_CONFIG_2, 0x7117);
+			priv->phy_speed = speed;
+		}
+		/* Set SGMII speed in Interface controll reg */
+		if (phydev->interface == PHY_INTERFACE_MODE_SGMII) {
+			if (speed == SPEED_10)
+				xlr_nae_wreg(priv->base_addr,
+					R_INTERFACE_CONTROL, SGMII_SPEED_10);
+			if (speed == SPEED_100)
+				xlr_nae_wreg(priv->base_addr,
+					R_INTERFACE_CONTROL, SGMII_SPEED_100);
+			if (speed == SPEED_1000)
+				xlr_nae_wreg(priv->base_addr,
+					R_INTERFACE_CONTROL, SGMII_SPEED_1000);
+		}
+		if (speed == SPEED_10)
+			xlr_nae_wreg(priv->base_addr, R_CORECONTROL, 0x2);
+		if (speed == SPEED_100)
+			xlr_nae_wreg(priv->base_addr, R_CORECONTROL, 0x1);
+		if (speed == SPEED_1000)
+			xlr_nae_wreg(priv->base_addr, R_CORECONTROL, 0x0);
+	}
+	pr_info("gmac%d : %dMbps\n", priv->port_id, priv->phy_speed);
+}
+
+static void xlr_gmac_link_adjust(struct net_device *ndev)
+{
+	struct xlr_net_priv *priv = netdev_priv(ndev);
+	struct phy_device *phydev = priv->mii_bus->phy_map[priv->phy_addr];
+	u32 intreg;
+
+	intreg = xlr_nae_rdreg(priv->base_addr, R_INTREG);
+	if (phydev->link) {
+		if (phydev->speed != priv->phy_speed) {
+			pr_info("gmac%d : Link up\n", priv->port_id);
+			xlr_set_gmac_speed(priv);
+		}
+	} else {
+		pr_info("gmac%d : Link down\n", priv->port_id);
+		xlr_set_gmac_speed(priv);
+	}
+}
+
+static int xlr_mii_probe(struct xlr_net_priv *priv)
+{
+	struct phy_device *phydev = priv->mii_bus->phy_map[priv->phy_addr];
+
+	if (!phydev) {
+		pr_err("no PHY found on phy_addr %d\n", priv->phy_addr);
+		return -ENODEV;
+	}
+
+	/* Attach MAC to PHY */
+	phydev = phy_connect(priv->ndev, dev_name(&phydev->dev),
+			&xlr_gmac_link_adjust, priv->nd->phy_interface);
+
+	if (IS_ERR(phydev)) {
+		pr_err("could not attach PHY\n");
+		return PTR_ERR(phydev);
+	}
+	phydev->supported &= (ADVERTISED_10baseT_Full
+				| ADVERTISED_10baseT_Half
+				| ADVERTISED_100baseT_Full
+				| ADVERTISED_100baseT_Half
+				| ADVERTISED_1000baseT_Full
+				| ADVERTISED_Autoneg
+				| ADVERTISED_MII);
+
+	phydev->advertising = phydev->supported;
+	pr_info("attached PHY driver [%s] (mii_bus:phy_addr=%s\n",
+		phydev->drv->name, dev_name(&phydev->dev));
+	return 0;
+}
+
+static int xlr_setup_mdio(struct xlr_net_priv *priv,
+		struct platform_device *pdev)
+{
+	int err;
+
+	priv->phy_addr = priv->nd->phy_addr;
+	priv->mii_bus = mdiobus_alloc();
+	if (!priv->mii_bus) {
+		pr_err("mdiobus alloc failed\n");
+		return -ENOMEM;
+	}
+
+	priv->mii_bus->priv = priv;
+	priv->mii_bus->name = "xlr-mdio";
+	snprintf(priv->mii_bus->id, MII_BUS_ID_SIZE, "%s-%d",
+			priv->mii_bus->name, priv->port_id);
+	priv->mii_bus->read = xlr_mii_read;
+	priv->mii_bus->write = xlr_mii_write;
+	priv->mii_bus->parent = &pdev->dev;
+	priv->mii_bus->irq = kmalloc(sizeof(int)*PHY_MAX_ADDR, GFP_KERNEL);
+	priv->mii_bus->irq[priv->phy_addr] = priv->ndev->irq;
+
+	/* Scan only the enabled address */
+	priv->mii_bus->phy_mask = ~(1 << priv->phy_addr);
+
+	/* setting clock divisor to 54 */
+	xlr_nae_wreg(priv->base_addr, R_MII_MGMT_CONFIG, 0x7);
+
+	err = mdiobus_register(priv->mii_bus);
+	if (err) {
+		mdiobus_free(priv->mii_bus);
+		pr_err("mdio bus registration failed\n");
+		return err;
+	}
+
+	pr_info("Registerd mdio bus id : %s\n", priv->mii_bus->id);
+	err = xlr_mii_probe(priv);
+	if (err) {
+		mdiobus_free(priv->mii_bus);
+		return err;
+	}
+	return 0;
+}
+
+static void xlr_port_enable(struct xlr_net_priv *priv)
+{
+	u32 prid = (read_c0_prid() & 0xf000);
+
+	/* Setup MAC_CONFIG reg if (xls & rgmii) */
+	if ((prid == 0x8000 || prid == 0x4000 || prid == 0xc000) &&
+			priv->nd->phy_interface == PHY_INTERFACE_MODE_RGMII)
+		xlr_reg_update(priv->base_addr, R_RX_CONTROL,
+			(1 << O_RX_CONTROL__RGMII), (1 << O_RX_CONTROL__RGMII));
+
+	/* Rx Tx enable */
+	xlr_reg_update(priv->base_addr, R_MAC_CONFIG_1,
+		((1 << O_MAC_CONFIG_1__rxen) | (1 << O_MAC_CONFIG_1__txen) |
+		(1 << O_MAC_CONFIG_1__rxfc) | (1 << O_MAC_CONFIG_1__txfc)),
+		((1 << O_MAC_CONFIG_1__rxen) | (1 << O_MAC_CONFIG_1__txen) |
+		(1 << O_MAC_CONFIG_1__rxfc) | (1 << O_MAC_CONFIG_1__txfc)));
+
+	/* Setup tx control reg */
+	xlr_reg_update(priv->base_addr, R_TX_CONTROL,
+		((1 << O_TX_CONTROL__TxEnable) |
+		(512 << O_TX_CONTROL__TxThreshold)), 0x3fff);
+
+	/* Setup rx control reg */
+	xlr_reg_update(priv->base_addr, R_RX_CONTROL,
+		1 << O_RX_CONTROL__RxEnable, 1 << O_RX_CONTROL__RxEnable);
+}
+
+static void xlr_port_disable(struct xlr_net_priv *priv)
+{
+	/* Setup MAC_CONFIG reg */
+	/* Rx Tx disable*/
+	xlr_reg_update(priv->base_addr, R_MAC_CONFIG_1,
+		((1 << O_MAC_CONFIG_1__rxen) | (1 << O_MAC_CONFIG_1__txen) |
+		(1 << O_MAC_CONFIG_1__rxfc) | (1 << O_MAC_CONFIG_1__txfc)),
+		0x0);
+
+	/* Setup tx control reg */
+	xlr_reg_update(priv->base_addr, R_TX_CONTROL,
+		((1 << O_TX_CONTROL__TxEnable) |
+		(512 << O_TX_CONTROL__TxThreshold)), 0);
+
+	/* Setup rx control reg */
+	xlr_reg_update(priv->base_addr, R_RX_CONTROL,
+		1 << O_RX_CONTROL__RxEnable, 0);
+}
+
+/* Initialization of gmac */
+static int xlr_gmac_init(struct xlr_net_priv *priv,
+		struct platform_device *pdev)
+{
+	int ret;
+
+	pr_info("Initializing the gmac%d\n", priv->port_id);
+
+	xlr_port_disable(priv);
+	xlr_nae_wreg(priv->base_addr, R_DESC_PACK_CTRL,
+			(1 << O_DESC_PACK_CTRL__MaxEntry)
+			| (BYTE_OFFSET << O_DESC_PACK_CTRL__ByteOffset)
+			| (1600 << O_DESC_PACK_CTRL__RegularSize));
+
+	ret = xlr_setup_mdio(priv, pdev);
+	if (ret)
+		return ret;
+	xlr_port_enable(priv);
+
+	/* Enable Full-duplex/1000Mbps/CRC */
+	xlr_nae_wreg(priv->base_addr, R_MAC_CONFIG_2, 0x7217);
+	/* speed 2.5Mhz */
+	xlr_nae_wreg(priv->base_addr, R_CORECONTROL, 0x02);
+	/* Setup Interrupt mask reg */
+	xlr_nae_wreg(priv->base_addr, R_INTMASK,
+		(1 << O_INTMASK__TxIllegal)	|
+		(1 << O_INTMASK__MDInt)		|
+		(1 << O_INTMASK__TxFetchError)	|
+		(1 << O_INTMASK__P2PSpillEcc)	|
+		(1 << O_INTMASK__TagFull)	|
+		(1 << O_INTMASK__Underrun)	|
+		(1 << O_INTMASK__Abort)
+		);
+
+	/* Clear all stats */
+	xlr_reg_update(priv->base_addr, R_STATCTRL,
+		0, 1 << O_STATCTRL__ClrCnt);
+	xlr_reg_update(priv->base_addr, R_STATCTRL,
+		1 << O_STATCTRL__ClrCnt, 1 << O_STATCTRL__ClrCnt);
+	return 0;
+}
+
+static int xlr_net_probe(struct platform_device *pdev)
+{
+	struct xlr_net_priv *priv = NULL;
+	struct net_device *ndev;
+	struct resource *res;
+	int mac, err;
+
+	mac = pdev->id;
+	ndev = alloc_etherdev_mq(sizeof(struct xlr_net_priv), 32);
+	if (!ndev) {
+		pr_err("Allocation of Ethernet device failed\n");
+		return -ENOMEM;
+	}
+
+	priv = netdev_priv(ndev);
+	priv->pdev = pdev;
+	priv->ndev = ndev;
+	priv->port_id = mac;
+	priv->nd = (struct xlr_net_data *)pdev->dev.platform_data;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL) {
+		pr_err("No memory resource for MAC %d\n", mac);
+		err = -ENODEV;
+		goto err_gmac;
+	}
+
+	ndev->base_addr = (unsigned long) devm_request_and_ioremap
+		(&pdev->dev, res);
+	if (!ndev->base_addr) {
+		dev_err(&pdev->dev,
+				"devm_request_and_ioremap failed\n");
+		return -EBUSY;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (res == NULL) {
+		pr_err("No irq resource for MAC %d\n", mac);
+		err = -ENODEV;
+		goto err_gmac;
+	}
+	ndev->irq = res->start;
+
+	priv->mii_addr = priv->nd->mii_addr;
+	priv->serdes_addr = priv->nd->serdes_addr;
+	priv->pcs_addr = priv->nd->pcs_addr;
+	priv->gpio_addr = priv->nd->gpio_addr;
+	priv->base_addr = (u32 *) ndev->base_addr;
+
+	mac_to_ndev[mac] = ndev;
+	ndev->netdev_ops = &xlr_netdev_ops;
+	ndev->watchdog_timeo = HZ;
+
+	/* Setup Mac address and Rx mode */
+	eth_hw_addr_random(ndev);
+	xlr_hw_set_mac_addr(ndev);
+	xlr_set_rx_mode(ndev);
+
+	priv->num_rx_desc += MAX_NUM_DESC_SPILL;
+	SET_ETHTOOL_OPS(ndev, &xlr_ethtool_ops);
+	SET_NETDEV_DEV(ndev, &pdev->dev);
+
+	/* Common registers, do one time initialization */
+	if (mac == 0 || mac == 4) {
+		xlr_config_fifo_spill_area(priv);
+		/* Configure PDE to Round-Robin pkt distribution */
+		xlr_config_pde(priv);
+		xlr_config_parser(priv);
+	}
+	/* Call init with respect to port */
+	if (strcmp(res->name, "gmac") == 0) {
+		err = xlr_gmac_init(priv, pdev);
+		if (err) {
+			pr_err("gmac%d init failed\n", mac);
+			goto err_gmac;
+		}
+	}
+
+	if (mac == 0 || mac == 4)
+		xlr_config_common(priv);
+
+	err = register_netdev(ndev);
+	if (err)
+		goto err_netdev;
+	platform_set_drvdata(pdev, priv);
+	return 0;
+
+err_netdev:
+	mdiobus_free(priv->mii_bus);
+err_gmac:
+	free_netdev(ndev);
+	return err;
+}
+
+static int xlr_net_remove(struct platform_device *pdev)
+{
+	struct xlr_net_priv *priv = platform_get_drvdata(pdev);
+	unregister_netdev(priv->ndev);
+	mdiobus_unregister(priv->mii_bus);
+	mdiobus_free(priv->mii_bus);
+	free_netdev(priv->ndev);
+	return 0;
+}
+
+static struct platform_driver xlr_net_driver = {
+	.probe		= xlr_net_probe,
+	.remove		= xlr_net_remove,
+	.driver		= {
+		.name	= "xlr-net",
+		.owner	= THIS_MODULE,
+	},
+};
+
+module_platform_driver(xlr_net_driver);
+
+MODULE_AUTHOR("Ganesan Ramalingam <ganesanr@broadcom.com>");
+MODULE_DESCRIPTION("Ethernet driver for Netlogic XLR/XLS");
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_ALIAS("platform:xlr-net");
diff --git a/drivers/staging/netlogic/xlr_net.h b/drivers/staging/netlogic/xlr_net.h
new file mode 100644
index 00000000000..f91d27e9d7c
--- /dev/null
+++ b/drivers/staging/netlogic/xlr_net.h
@@ -0,0 +1,1099 @@
+/*
+ * Copyright (c) 2003-2012 Broadcom Corporation
+ * All Rights Reserved
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the Broadcom
+ * license below:
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY BROADCOM ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL BROADCOM OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
+ * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
+ * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
+ * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+/* #define MAC_SPLIT_MODE */
+
+#define MAC_SPACING                 0x400
+#define XGMAC_SPACING               0x400
+
+/* PE-MCXMAC register and bit field definitions */
+#define R_MAC_CONFIG_1                                              0x00
+#define   O_MAC_CONFIG_1__srst                                      31
+#define   O_MAC_CONFIG_1__simr                                      30
+#define   O_MAC_CONFIG_1__hrrmc                                     18
+#define   W_MAC_CONFIG_1__hrtmc                                      2
+#define   O_MAC_CONFIG_1__hrrfn                                     16
+#define   W_MAC_CONFIG_1__hrtfn                                      2
+#define   O_MAC_CONFIG_1__intlb                                      8
+#define   O_MAC_CONFIG_1__rxfc                                       5
+#define   O_MAC_CONFIG_1__txfc                                       4
+#define   O_MAC_CONFIG_1__srxen                                      3
+#define   O_MAC_CONFIG_1__rxen                                       2
+#define   O_MAC_CONFIG_1__stxen                                      1
+#define   O_MAC_CONFIG_1__txen                                       0
+#define R_MAC_CONFIG_2                                              0x01
+#define   O_MAC_CONFIG_2__prlen                                     12
+#define   W_MAC_CONFIG_2__prlen                                      4
+#define   O_MAC_CONFIG_2__speed                                      8
+#define   W_MAC_CONFIG_2__speed                                      2
+#define   O_MAC_CONFIG_2__hugen                                      5
+#define   O_MAC_CONFIG_2__flchk                                      4
+#define   O_MAC_CONFIG_2__crce                                       1
+#define   O_MAC_CONFIG_2__fulld                                      0
+#define R_IPG_IFG                                                   0x02
+#define   O_IPG_IFG__ipgr1                                          24
+#define   W_IPG_IFG__ipgr1                                           7
+#define   O_IPG_IFG__ipgr2                                          16
+#define   W_IPG_IFG__ipgr2                                           7
+#define   O_IPG_IFG__mifg                                            8
+#define   W_IPG_IFG__mifg                                            8
+#define   O_IPG_IFG__ipgt                                            0
+#define   W_IPG_IFG__ipgt                                            7
+#define R_HALF_DUPLEX                                               0x03
+#define   O_HALF_DUPLEX__abebt                                      24
+#define   W_HALF_DUPLEX__abebt                                       4
+#define   O_HALF_DUPLEX__abebe                                      19
+#define   O_HALF_DUPLEX__bpnb                                       18
+#define   O_HALF_DUPLEX__nobo                                       17
+#define   O_HALF_DUPLEX__edxsdfr                                    16
+#define   O_HALF_DUPLEX__retry                                      12
+#define   W_HALF_DUPLEX__retry                                       4
+#define   O_HALF_DUPLEX__lcol                                        0
+#define   W_HALF_DUPLEX__lcol                                       10
+#define R_MAXIMUM_FRAME_LENGTH                                      0x04
+#define   O_MAXIMUM_FRAME_LENGTH__maxf                               0
+#define   W_MAXIMUM_FRAME_LENGTH__maxf                              16
+#define R_TEST                                                      0x07
+#define   O_TEST__mbof                                               3
+#define   O_TEST__rthdf                                              2
+#define   O_TEST__tpause                                             1
+#define   O_TEST__sstct                                              0
+#define R_MII_MGMT_CONFIG                                           0x08
+#define   O_MII_MGMT_CONFIG__scinc                                   5
+#define   O_MII_MGMT_CONFIG__spre                                    4
+#define   O_MII_MGMT_CONFIG__clks                                    3
+#define   W_MII_MGMT_CONFIG__clks                                    3
+#define R_MII_MGMT_COMMAND                                          0x09
+#define   O_MII_MGMT_COMMAND__scan                                   1
+#define   O_MII_MGMT_COMMAND__rstat                                  0
+#define R_MII_MGMT_ADDRESS                                          0x0A
+#define   O_MII_MGMT_ADDRESS__fiad                                   8
+#define   W_MII_MGMT_ADDRESS__fiad                                   5
+#define   O_MII_MGMT_ADDRESS__fgad                                   5
+#define   W_MII_MGMT_ADDRESS__fgad                                   0
+#define R_MII_MGMT_WRITE_DATA                                       0x0B
+#define   O_MII_MGMT_WRITE_DATA__ctld                                0
+#define   W_MII_MGMT_WRITE_DATA__ctld                               16
+#define R_MII_MGMT_STATUS                                           0x0C
+#define R_MII_MGMT_INDICATORS                                       0x0D
+#define   O_MII_MGMT_INDICATORS__nvalid                              2
+#define   O_MII_MGMT_INDICATORS__scan                                1
+#define   O_MII_MGMT_INDICATORS__busy                                0
+#define R_INTERFACE_CONTROL                                         0x0E
+#define   O_INTERFACE_CONTROL__hrstint                              31
+#define   O_INTERFACE_CONTROL__tbimode                              27
+#define   O_INTERFACE_CONTROL__ghdmode                              26
+#define   O_INTERFACE_CONTROL__lhdmode                              25
+#define   O_INTERFACE_CONTROL__phymod                               24
+#define   O_INTERFACE_CONTROL__hrrmi                                23
+#define   O_INTERFACE_CONTROL__rspd                                 16
+#define   O_INTERFACE_CONTROL__hr100                                15
+#define   O_INTERFACE_CONTROL__frcq                                 10
+#define   O_INTERFACE_CONTROL__nocfr                                 9
+#define   O_INTERFACE_CONTROL__dlfct                                 8
+#define   O_INTERFACE_CONTROL__enjab                                 0
+#define R_INTERFACE_STATUS                                         0x0F
+#define   O_INTERFACE_STATUS__xsdfr                                  9
+#define   O_INTERFACE_STATUS__ssrr                                   8
+#define   W_INTERFACE_STATUS__ssrr                                   5
+#define   O_INTERFACE_STATUS__miilf                                  3
+#define   O_INTERFACE_STATUS__locar                                  2
+#define   O_INTERFACE_STATUS__sqerr                                  1
+#define   O_INTERFACE_STATUS__jabber                                 0
+#define R_STATION_ADDRESS_LS                                       0x10
+#define R_STATION_ADDRESS_MS                                       0x11
+
+/* A-XGMAC register and bit field definitions */
+#define R_XGMAC_CONFIG_0    0x00
+#define   O_XGMAC_CONFIG_0__hstmacrst               31
+#define   O_XGMAC_CONFIG_0__hstrstrctl              23
+#define   O_XGMAC_CONFIG_0__hstrstrfn               22
+#define   O_XGMAC_CONFIG_0__hstrsttctl              18
+#define   O_XGMAC_CONFIG_0__hstrsttfn               17
+#define   O_XGMAC_CONFIG_0__hstrstmiim              16
+#define   O_XGMAC_CONFIG_0__hstloopback             8
+#define R_XGMAC_CONFIG_1    0x01
+#define   O_XGMAC_CONFIG_1__hsttctlen               31
+#define   O_XGMAC_CONFIG_1__hsttfen                 30
+#define   O_XGMAC_CONFIG_1__hstrctlen               29
+#define   O_XGMAC_CONFIG_1__hstrfen                 28
+#define   O_XGMAC_CONFIG_1__tfen                    26
+#define   O_XGMAC_CONFIG_1__rfen                    24
+#define   O_XGMAC_CONFIG_1__hstrctlshrtp            12
+#define   O_XGMAC_CONFIG_1__hstdlyfcstx             10
+#define   W_XGMAC_CONFIG_1__hstdlyfcstx              2
+#define   O_XGMAC_CONFIG_1__hstdlyfcsrx              8
+#define   W_XGMAC_CONFIG_1__hstdlyfcsrx              2
+#define   O_XGMAC_CONFIG_1__hstppen                  7
+#define   O_XGMAC_CONFIG_1__hstbytswp                6
+#define   O_XGMAC_CONFIG_1__hstdrplt64               5
+#define   O_XGMAC_CONFIG_1__hstprmscrx               4
+#define   O_XGMAC_CONFIG_1__hstlenchk                3
+#define   O_XGMAC_CONFIG_1__hstgenfcs                2
+#define   O_XGMAC_CONFIG_1__hstpadmode               0
+#define   W_XGMAC_CONFIG_1__hstpadmode               2
+#define R_XGMAC_CONFIG_2    0x02
+#define   O_XGMAC_CONFIG_2__hsttctlfrcp             31
+#define   O_XGMAC_CONFIG_2__hstmlnkflth             27
+#define   O_XGMAC_CONFIG_2__hstalnkflth             26
+#define   O_XGMAC_CONFIG_2__rflnkflt                24
+#define   W_XGMAC_CONFIG_2__rflnkflt                 2
+#define   O_XGMAC_CONFIG_2__hstipgextmod            16
+#define   W_XGMAC_CONFIG_2__hstipgextmod             5
+#define   O_XGMAC_CONFIG_2__hstrctlfrcp             15
+#define   O_XGMAC_CONFIG_2__hstipgexten              5
+#define   O_XGMAC_CONFIG_2__hstmipgext               0
+#define   W_XGMAC_CONFIG_2__hstmipgext               5
+#define R_XGMAC_CONFIG_3    0x03
+#define   O_XGMAC_CONFIG_3__hstfltrfrm              31
+#define   W_XGMAC_CONFIG_3__hstfltrfrm              16
+#define   O_XGMAC_CONFIG_3__hstfltrfrmdc            15
+#define   W_XGMAC_CONFIG_3__hstfltrfrmdc            16
+#define R_XGMAC_STATION_ADDRESS_LS      0x04
+#define   O_XGMAC_STATION_ADDRESS_LS__hstmacadr0    0
+#define   W_XGMAC_STATION_ADDRESS_LS__hstmacadr0    32
+#define R_XGMAC_STATION_ADDRESS_MS      0x05
+#define R_XGMAC_MAX_FRAME_LEN           0x08
+#define   O_XGMAC_MAX_FRAME_LEN__hstmxfrmwctx       16
+#define   W_XGMAC_MAX_FRAME_LEN__hstmxfrmwctx       14
+#define   O_XGMAC_MAX_FRAME_LEN__hstmxfrmbcrx        0
+#define   W_XGMAC_MAX_FRAME_LEN__hstmxfrmbcrx       16
+#define R_XGMAC_REV_LEVEL               0x0B
+#define   O_XGMAC_REV_LEVEL__revlvl                  0
+#define   W_XGMAC_REV_LEVEL__revlvl                 15
+#define R_XGMAC_MIIM_COMMAND            0x10
+#define   O_XGMAC_MIIM_COMMAND__hstldcmd             3
+#define   O_XGMAC_MIIM_COMMAND__hstmiimcmd           0
+#define   W_XGMAC_MIIM_COMMAND__hstmiimcmd           3
+#define R_XGMAC_MIIM_FILED              0x11
+#define   O_XGMAC_MIIM_FILED__hststfield            30
+#define   W_XGMAC_MIIM_FILED__hststfield             2
+#define   O_XGMAC_MIIM_FILED__hstopfield            28
+#define   W_XGMAC_MIIM_FILED__hstopfield             2
+#define   O_XGMAC_MIIM_FILED__hstphyadx             23
+#define   W_XGMAC_MIIM_FILED__hstphyadx              5
+#define   O_XGMAC_MIIM_FILED__hstregadx             18
+#define   W_XGMAC_MIIM_FILED__hstregadx              5
+#define   O_XGMAC_MIIM_FILED__hsttafield            16
+#define   W_XGMAC_MIIM_FILED__hsttafield             2
+#define   O_XGMAC_MIIM_FILED__miimrddat              0
+#define   W_XGMAC_MIIM_FILED__miimrddat             16
+#define R_XGMAC_MIIM_CONFIG             0x12
+#define   O_XGMAC_MIIM_CONFIG__hstnopram             7
+#define   O_XGMAC_MIIM_CONFIG__hstclkdiv             0
+#define   W_XGMAC_MIIM_CONFIG__hstclkdiv             7
+#define R_XGMAC_MIIM_LINK_FAIL_VECTOR   0x13
+#define   O_XGMAC_MIIM_LINK_FAIL_VECTOR__miimlfvec   0
+#define   W_XGMAC_MIIM_LINK_FAIL_VECTOR__miimlfvec  32
+#define R_XGMAC_MIIM_INDICATOR          0x14
+#define   O_XGMAC_MIIM_INDICATOR__miimphylf          4
+#define   O_XGMAC_MIIM_INDICATOR__miimmoncplt        3
+#define   O_XGMAC_MIIM_INDICATOR__miimmonvld         2
+#define   O_XGMAC_MIIM_INDICATOR__miimmon            1
+#define   O_XGMAC_MIIM_INDICATOR__miimbusy           0
+
+/* GMAC stats registers */
+#define R_RBYT							    0x27
+#define R_RPKT							    0x28
+#define R_RFCS							    0x29
+#define R_RMCA							    0x2A
+#define R_RBCA							    0x2B
+#define R_RXCF							    0x2C
+#define R_RXPF							    0x2D
+#define R_RXUO							    0x2E
+#define R_RALN							    0x2F
+#define R_RFLR							    0x30
+#define R_RCDE							    0x31
+#define R_RCSE							    0x32
+#define R_RUND							    0x33
+#define R_ROVR							    0x34
+#define R_TBYT							    0x38
+#define R_TPKT							    0x39
+#define R_TMCA							    0x3A
+#define R_TBCA							    0x3B
+#define R_TXPF							    0x3C
+#define R_TDFR							    0x3D
+#define R_TEDF							    0x3E
+#define R_TSCL							    0x3F
+#define R_TMCL							    0x40
+#define R_TLCL							    0x41
+#define R_TXCL							    0x42
+#define R_TNCL							    0x43
+#define R_TJBR							    0x46
+#define R_TFCS							    0x47
+#define R_TXCF							    0x48
+#define R_TOVR							    0x49
+#define R_TUND							    0x4A
+#define R_TFRG							    0x4B
+
+/* Glue logic register and bit field definitions */
+#define R_MAC_ADDR0                                                 0x50
+#define R_MAC_ADDR1                                                 0x52
+#define R_MAC_ADDR2                                                 0x54
+#define R_MAC_ADDR3                                                 0x56
+#define R_MAC_ADDR_MASK2                                            0x58
+#define R_MAC_ADDR_MASK3                                            0x5A
+#define R_MAC_FILTER_CONFIG                                         0x5C
+#define   O_MAC_FILTER_CONFIG__BROADCAST_EN                         10
+#define   O_MAC_FILTER_CONFIG__PAUSE_FRAME_EN                       9
+#define   O_MAC_FILTER_CONFIG__ALL_MCAST_EN                         8
+#define   O_MAC_FILTER_CONFIG__ALL_UCAST_EN                         7
+#define   O_MAC_FILTER_CONFIG__HASH_MCAST_EN                        6
+#define   O_MAC_FILTER_CONFIG__HASH_UCAST_EN                        5
+#define   O_MAC_FILTER_CONFIG__ADDR_MATCH_DISC                      4
+#define   O_MAC_FILTER_CONFIG__MAC_ADDR3_VALID                      3
+#define   O_MAC_FILTER_CONFIG__MAC_ADDR2_VALID                      2
+#define   O_MAC_FILTER_CONFIG__MAC_ADDR1_VALID                      1
+#define   O_MAC_FILTER_CONFIG__MAC_ADDR0_VALID                      0
+#define R_HASH_TABLE_VECTOR                                         0x30
+#define R_TX_CONTROL                                                 0x0A0
+#define   O_TX_CONTROL__Tx15Halt                                     31
+#define   O_TX_CONTROL__Tx14Halt                                     30
+#define   O_TX_CONTROL__Tx13Halt                                     29
+#define   O_TX_CONTROL__Tx12Halt                                     28
+#define   O_TX_CONTROL__Tx11Halt                                     27
+#define   O_TX_CONTROL__Tx10Halt                                     26
+#define   O_TX_CONTROL__Tx9Halt                                      25
+#define   O_TX_CONTROL__Tx8Halt                                      24
+#define   O_TX_CONTROL__Tx7Halt                                      23
+#define   O_TX_CONTROL__Tx6Halt                                      22
+#define   O_TX_CONTROL__Tx5Halt                                      21
+#define   O_TX_CONTROL__Tx4Halt                                      20
+#define   O_TX_CONTROL__Tx3Halt                                      19
+#define   O_TX_CONTROL__Tx2Halt                                      18
+#define   O_TX_CONTROL__Tx1Halt                                      17
+#define   O_TX_CONTROL__Tx0Halt                                      16
+#define   O_TX_CONTROL__TxIdle                                       15
+#define   O_TX_CONTROL__TxEnable                                     14
+#define   O_TX_CONTROL__TxThreshold                                  0
+#define   W_TX_CONTROL__TxThreshold                                  14
+#define R_RX_CONTROL                                                 0x0A1
+#define   O_RX_CONTROL__RGMII                                        10
+#define   O_RX_CONTROL__SoftReset			             2
+#define   O_RX_CONTROL__RxHalt                                       1
+#define   O_RX_CONTROL__RxEnable                                     0
+#define R_DESC_PACK_CTRL                                            0x0A2
+#define   O_DESC_PACK_CTRL__ByteOffset                              17
+#define   W_DESC_PACK_CTRL__ByteOffset                              3
+#define   O_DESC_PACK_CTRL__PrePadEnable                            16
+#define   O_DESC_PACK_CTRL__MaxEntry                                14
+#define   W_DESC_PACK_CTRL__MaxEntry                                2
+#define   O_DESC_PACK_CTRL__RegularSize                             0
+#define   W_DESC_PACK_CTRL__RegularSize                             14
+#define R_STATCTRL                                                  0x0A3
+#define   O_STATCTRL__OverFlowEn                                    4
+#define   O_STATCTRL__GIG                                           3
+#define   O_STATCTRL__Sten                                          2
+#define   O_STATCTRL__ClrCnt                                        1
+#define   O_STATCTRL__AutoZ                                         0
+#define R_L2ALLOCCTRL                                               0x0A4
+#define   O_L2ALLOCCTRL__TxL2Allocate                               9
+#define   W_L2ALLOCCTRL__TxL2Allocate                               9
+#define   O_L2ALLOCCTRL__RxL2Allocate                               0
+#define   W_L2ALLOCCTRL__RxL2Allocate                               9
+#define R_INTMASK                                                   0x0A5
+#define   O_INTMASK__Spi4TxError                                     28
+#define   O_INTMASK__Spi4RxError                                     27
+#define   O_INTMASK__RGMIIHalfDupCollision                           27
+#define   O_INTMASK__Abort                                           26
+#define   O_INTMASK__Underrun                                        25
+#define   O_INTMASK__DiscardPacket                                   24
+#define   O_INTMASK__AsyncFifoFull                                   23
+#define   O_INTMASK__TagFull                                         22
+#define   O_INTMASK__Class3Full                                      21
+#define   O_INTMASK__C3EarlyFull                                     20
+#define   O_INTMASK__Class2Full                                      19
+#define   O_INTMASK__C2EarlyFull                                     18
+#define   O_INTMASK__Class1Full                                      17
+#define   O_INTMASK__C1EarlyFull                                     16
+#define   O_INTMASK__Class0Full                                      15
+#define   O_INTMASK__C0EarlyFull                                     14
+#define   O_INTMASK__RxDataFull                                      13
+#define   O_INTMASK__RxEarlyFull                                     12
+#define   O_INTMASK__RFreeEmpty                                      9
+#define   O_INTMASK__RFEarlyEmpty                                    8
+#define   O_INTMASK__P2PSpillEcc                                     7
+#define   O_INTMASK__FreeDescFull                                    5
+#define   O_INTMASK__FreeEarlyFull                                   4
+#define   O_INTMASK__TxFetchError                                    3
+#define   O_INTMASK__StatCarry                                       2
+#define   O_INTMASK__MDInt                                           1
+#define   O_INTMASK__TxIllegal                                       0
+#define R_INTREG                                                    0x0A6
+#define   O_INTREG__Spi4TxError                                     28
+#define   O_INTREG__Spi4RxError                                     27
+#define   O_INTREG__RGMIIHalfDupCollision                           27
+#define   O_INTREG__Abort                                           26
+#define   O_INTREG__Underrun                                        25
+#define   O_INTREG__DiscardPacket                                   24
+#define   O_INTREG__AsyncFifoFull                                   23
+#define   O_INTREG__TagFull                                         22
+#define   O_INTREG__Class3Full                                      21
+#define   O_INTREG__C3EarlyFull                                     20
+#define   O_INTREG__Class2Full                                      19
+#define   O_INTREG__C2EarlyFull                                     18
+#define   O_INTREG__Class1Full                                      17
+#define   O_INTREG__C1EarlyFull                                     16
+#define   O_INTREG__Class0Full                                      15
+#define   O_INTREG__C0EarlyFull                                     14
+#define   O_INTREG__RxDataFull                                      13
+#define   O_INTREG__RxEarlyFull                                     12
+#define   O_INTREG__RFreeEmpty                                      9
+#define   O_INTREG__RFEarlyEmpty                                    8
+#define   O_INTREG__P2PSpillEcc                                     7
+#define   O_INTREG__FreeDescFull                                    5
+#define   O_INTREG__FreeEarlyFull                                   4
+#define   O_INTREG__TxFetchError                                    3
+#define   O_INTREG__StatCarry                                       2
+#define   O_INTREG__MDInt                                           1
+#define   O_INTREG__TxIllegal                                       0
+#define R_TXRETRY                                                   0x0A7
+#define   O_TXRETRY__CollisionRetry                                 6
+#define   O_TXRETRY__BusErrorRetry                                  5
+#define   O_TXRETRY__UnderRunRetry                                  4
+#define   O_TXRETRY__Retries                                        0
+#define   W_TXRETRY__Retries                                        4
+#define R_CORECONTROL                                               0x0A8
+#define   O_CORECONTROL__ErrorThread                                4
+#define   W_CORECONTROL__ErrorThread                                7
+#define   O_CORECONTROL__Shutdown                                   2
+#define   O_CORECONTROL__Speed                                      0
+#define   W_CORECONTROL__Speed                                      2
+#define R_BYTEOFFSET0                                               0x0A9
+#define R_BYTEOFFSET1                                               0x0AA
+#define R_L2TYPE_0                                                  0x0F0
+#define   O_L2TYPE__ExtraHdrProtoSize                               26
+#define   W_L2TYPE__ExtraHdrProtoSize                               5
+#define   O_L2TYPE__ExtraHdrProtoOffset                             20
+#define   W_L2TYPE__ExtraHdrProtoOffset                             6
+#define   O_L2TYPE__ExtraHeaderSize                                 14
+#define   W_L2TYPE__ExtraHeaderSize                                 6
+#define   O_L2TYPE__ProtoOffset                                     8
+#define   W_L2TYPE__ProtoOffset                                     6
+#define   O_L2TYPE__L2HdrOffset                                     2
+#define   W_L2TYPE__L2HdrOffset                                     6
+#define   O_L2TYPE__L2Proto                                         0
+#define   W_L2TYPE__L2Proto                                         2
+#define R_L2TYPE_1                                                  0xF0
+#define R_L2TYPE_2                                                  0xF0
+#define R_L2TYPE_3                                                  0xF0
+#define R_PARSERCONFIGREG                                           0x100
+#define   O_PARSERCONFIGREG__CRCHashPoly                            8
+#define   W_PARSERCONFIGREG__CRCHashPoly                            7
+#define   O_PARSERCONFIGREG__PrePadOffset                           4
+#define   W_PARSERCONFIGREG__PrePadOffset                           4
+#define   O_PARSERCONFIGREG__UseCAM                                 2
+#define   O_PARSERCONFIGREG__UseHASH                                1
+#define   O_PARSERCONFIGREG__UseProto                               0
+#define R_L3CTABLE                                                  0x140
+#define   O_L3CTABLE__Offset0                                       25
+#define   W_L3CTABLE__Offset0                                       7
+#define   O_L3CTABLE__Len0                                          21
+#define   W_L3CTABLE__Len0                                          4
+#define   O_L3CTABLE__Offset1                                       14
+#define   W_L3CTABLE__Offset1                                       7
+#define   O_L3CTABLE__Len1                                          10
+#define   W_L3CTABLE__Len1                                          4
+#define   O_L3CTABLE__Offset2                                       4
+#define   W_L3CTABLE__Offset2                                       6
+#define   O_L3CTABLE__Len2                                          0
+#define   W_L3CTABLE__Len2                                          4
+#define   O_L3CTABLE__L3HdrOffset                                   26
+#define   W_L3CTABLE__L3HdrOffset                                   6
+#define   O_L3CTABLE__L4ProtoOffset                                 20
+#define   W_L3CTABLE__L4ProtoOffset                                 6
+#define   O_L3CTABLE__IPChksumCompute                               19
+#define   O_L3CTABLE__L4Classify                                    18
+#define   O_L3CTABLE__L2Proto                                       16
+#define   W_L3CTABLE__L2Proto                                       2
+#define   O_L3CTABLE__L3ProtoKey                                    0
+#define   W_L3CTABLE__L3ProtoKey                                    16
+#define R_L4CTABLE                                                  0x160
+#define   O_L4CTABLE__Offset0                                       21
+#define   W_L4CTABLE__Offset0                                       6
+#define   O_L4CTABLE__Len0                                          17
+#define   W_L4CTABLE__Len0                                          4
+#define   O_L4CTABLE__Offset1                                       11
+#define   W_L4CTABLE__Offset1                                       6
+#define   O_L4CTABLE__Len1                                          7
+#define   W_L4CTABLE__Len1                                          4
+#define   O_L4CTABLE__TCPChksumEnable                               0
+#define R_CAM4X128TABLE                                             0x172
+#define   O_CAM4X128TABLE__ClassId                                  7
+#define   W_CAM4X128TABLE__ClassId                                  2
+#define   O_CAM4X128TABLE__BucketId                                 1
+#define   W_CAM4X128TABLE__BucketId                                 6
+#define   O_CAM4X128TABLE__UseBucket                                0
+#define R_CAM4X128KEY                                               0x180
+#define R_TRANSLATETABLE                                            0x1A0
+#define R_DMACR0                                                    0x200
+#define   O_DMACR0__Data0WrMaxCr                                    27
+#define   W_DMACR0__Data0WrMaxCr                                    3
+#define   O_DMACR0__Data0RdMaxCr                                    24
+#define   W_DMACR0__Data0RdMaxCr                                    3
+#define   O_DMACR0__Data1WrMaxCr                                    21
+#define   W_DMACR0__Data1WrMaxCr                                    3
+#define   O_DMACR0__Data1RdMaxCr                                    18
+#define   W_DMACR0__Data1RdMaxCr                                    3
+#define   O_DMACR0__Data2WrMaxCr                                    15
+#define   W_DMACR0__Data2WrMaxCr                                    3
+#define   O_DMACR0__Data2RdMaxCr                                    12
+#define   W_DMACR0__Data2RdMaxCr                                    3
+#define   O_DMACR0__Data3WrMaxCr                                    9
+#define   W_DMACR0__Data3WrMaxCr                                    3
+#define   O_DMACR0__Data3RdMaxCr                                    6
+#define   W_DMACR0__Data3RdMaxCr                                    3
+#define   O_DMACR0__Data4WrMaxCr                                    3
+#define   W_DMACR0__Data4WrMaxCr                                    3
+#define   O_DMACR0__Data4RdMaxCr                                    0
+#define   W_DMACR0__Data4RdMaxCr                                    3
+#define R_DMACR1                                                    0x201
+#define   O_DMACR1__Data5WrMaxCr                                    27
+#define   W_DMACR1__Data5WrMaxCr                                    3
+#define   O_DMACR1__Data5RdMaxCr                                    24
+#define   W_DMACR1__Data5RdMaxCr                                    3
+#define   O_DMACR1__Data6WrMaxCr                                    21
+#define   W_DMACR1__Data6WrMaxCr                                    3
+#define   O_DMACR1__Data6RdMaxCr                                    18
+#define   W_DMACR1__Data6RdMaxCr                                    3
+#define   O_DMACR1__Data7WrMaxCr                                    15
+#define   W_DMACR1__Data7WrMaxCr                                    3
+#define   O_DMACR1__Data7RdMaxCr                                    12
+#define   W_DMACR1__Data7RdMaxCr                                    3
+#define   O_DMACR1__Data8WrMaxCr                                    9
+#define   W_DMACR1__Data8WrMaxCr                                    3
+#define   O_DMACR1__Data8RdMaxCr                                    6
+#define   W_DMACR1__Data8RdMaxCr                                    3
+#define   O_DMACR1__Data9WrMaxCr                                    3
+#define   W_DMACR1__Data9WrMaxCr                                    3
+#define   O_DMACR1__Data9RdMaxCr                                    0
+#define   W_DMACR1__Data9RdMaxCr                                    3
+#define R_DMACR2                                                    0x202
+#define   O_DMACR2__Data10WrMaxCr                                   27
+#define   W_DMACR2__Data10WrMaxCr                                   3
+#define   O_DMACR2__Data10RdMaxCr                                   24
+#define   W_DMACR2__Data10RdMaxCr                                   3
+#define   O_DMACR2__Data11WrMaxCr                                   21
+#define   W_DMACR2__Data11WrMaxCr                                   3
+#define   O_DMACR2__Data11RdMaxCr                                   18
+#define   W_DMACR2__Data11RdMaxCr                                   3
+#define   O_DMACR2__Data12WrMaxCr                                   15
+#define   W_DMACR2__Data12WrMaxCr                                   3
+#define   O_DMACR2__Data12RdMaxCr                                   12
+#define   W_DMACR2__Data12RdMaxCr                                   3
+#define   O_DMACR2__Data13WrMaxCr                                   9
+#define   W_DMACR2__Data13WrMaxCr                                   3
+#define   O_DMACR2__Data13RdMaxCr                                   6
+#define   W_DMACR2__Data13RdMaxCr                                   3
+#define   O_DMACR2__Data14WrMaxCr                                   3
+#define   W_DMACR2__Data14WrMaxCr                                   3
+#define   O_DMACR2__Data14RdMaxCr                                   0
+#define   W_DMACR2__Data14RdMaxCr                                   3
+#define R_DMACR3                                                    0x203
+#define   O_DMACR3__Data15WrMaxCr                                   27
+#define   W_DMACR3__Data15WrMaxCr                                   3
+#define   O_DMACR3__Data15RdMaxCr                                   24
+#define   W_DMACR3__Data15RdMaxCr                                   3
+#define   O_DMACR3__SpClassWrMaxCr                                  21
+#define   W_DMACR3__SpClassWrMaxCr                                  3
+#define   O_DMACR3__SpClassRdMaxCr                                  18
+#define   W_DMACR3__SpClassRdMaxCr                                  3
+#define   O_DMACR3__JumFrInWrMaxCr                                  15
+#define   W_DMACR3__JumFrInWrMaxCr                                  3
+#define   O_DMACR3__JumFrInRdMaxCr                                  12
+#define   W_DMACR3__JumFrInRdMaxCr                                  3
+#define   O_DMACR3__RegFrInWrMaxCr                                  9
+#define   W_DMACR3__RegFrInWrMaxCr                                  3
+#define   O_DMACR3__RegFrInRdMaxCr                                  6
+#define   W_DMACR3__RegFrInRdMaxCr                                  3
+#define   O_DMACR3__FrOutWrMaxCr                                    3
+#define   W_DMACR3__FrOutWrMaxCr                                    3
+#define   O_DMACR3__FrOutRdMaxCr                                    0
+#define   W_DMACR3__FrOutRdMaxCr                                    3
+#define R_REG_FRIN_SPILL_MEM_START_0                                0x204
+#define   O_REG_FRIN_SPILL_MEM_START_0__RegFrInSpillMemStart0        0
+#define   W_REG_FRIN_SPILL_MEM_START_0__RegFrInSpillMemStart0       32
+#define R_REG_FRIN_SPILL_MEM_START_1                                0x205
+#define   O_REG_FRIN_SPILL_MEM_START_1__RegFrInSpillMemStart1        0
+#define   W_REG_FRIN_SPILL_MEM_START_1__RegFrInSpillMemStart1        3
+#define R_REG_FRIN_SPILL_MEM_SIZE                                   0x206
+#define   O_REG_FRIN_SPILL_MEM_SIZE__RegFrInSpillMemSize             0
+#define   W_REG_FRIN_SPILL_MEM_SIZE__RegFrInSpillMemSize            32
+#define R_FROUT_SPILL_MEM_START_0                                   0x207
+#define   O_FROUT_SPILL_MEM_START_0__FrOutSpillMemStart0             0
+#define   W_FROUT_SPILL_MEM_START_0__FrOutSpillMemStart0            32
+#define R_FROUT_SPILL_MEM_START_1                                   0x208
+#define   O_FROUT_SPILL_MEM_START_1__FrOutSpillMemStart1             0
+#define   W_FROUT_SPILL_MEM_START_1__FrOutSpillMemStart1             3
+#define R_FROUT_SPILL_MEM_SIZE                                      0x209
+#define   O_FROUT_SPILL_MEM_SIZE__FrOutSpillMemSize                  0
+#define   W_FROUT_SPILL_MEM_SIZE__FrOutSpillMemSize                 32
+#define R_CLASS0_SPILL_MEM_START_0                                  0x20A
+#define   O_CLASS0_SPILL_MEM_START_0__Class0SpillMemStart0           0
+#define   W_CLASS0_SPILL_MEM_START_0__Class0SpillMemStart0          32
+#define R_CLASS0_SPILL_MEM_START_1                                  0x20B
+#define   O_CLASS0_SPILL_MEM_START_1__Class0SpillMemStart1           0
+#define   W_CLASS0_SPILL_MEM_START_1__Class0SpillMemStart1           3
+#define R_CLASS0_SPILL_MEM_SIZE                                     0x20C
+#define   O_CLASS0_SPILL_MEM_SIZE__Class0SpillMemSize                0
+#define   W_CLASS0_SPILL_MEM_SIZE__Class0SpillMemSize               32
+#define R_JUMFRIN_SPILL_MEM_START_0                                 0x20D
+#define   O_JUMFRIN_SPILL_MEM_START_0__JumFrInSpillMemStar0          0
+#define   W_JUMFRIN_SPILL_MEM_START_0__JumFrInSpillMemStar0         32
+#define R_JUMFRIN_SPILL_MEM_START_1                                 0x20E
+#define   O_JUMFRIN_SPILL_MEM_START_1__JumFrInSpillMemStart1         0
+#define   W_JUMFRIN_SPILL_MEM_START_1__JumFrInSpillMemStart1         3
+#define R_JUMFRIN_SPILL_MEM_SIZE                                    0x20F
+#define   O_JUMFRIN_SPILL_MEM_SIZE__JumFrInSpillMemSize              0
+#define   W_JUMFRIN_SPILL_MEM_SIZE__JumFrInSpillMemSize             32
+#define R_CLASS1_SPILL_MEM_START_0                                  0x210
+#define   O_CLASS1_SPILL_MEM_START_0__Class1SpillMemStart0           0
+#define   W_CLASS1_SPILL_MEM_START_0__Class1SpillMemStart0          32
+#define R_CLASS1_SPILL_MEM_START_1                                  0x211
+#define   O_CLASS1_SPILL_MEM_START_1__Class1SpillMemStart1           0
+#define   W_CLASS1_SPILL_MEM_START_1__Class1SpillMemStart1           3
+#define R_CLASS1_SPILL_MEM_SIZE                                     0x212
+#define   O_CLASS1_SPILL_MEM_SIZE__Class1SpillMemSize                0
+#define   W_CLASS1_SPILL_MEM_SIZE__Class1SpillMemSize               32
+#define R_CLASS2_SPILL_MEM_START_0                                  0x213
+#define   O_CLASS2_SPILL_MEM_START_0__Class2SpillMemStart0           0
+#define   W_CLASS2_SPILL_MEM_START_0__Class2SpillMemStart0          32
+#define R_CLASS2_SPILL_MEM_START_1                                  0x214
+#define   O_CLASS2_SPILL_MEM_START_1__Class2SpillMemStart1           0
+#define   W_CLASS2_SPILL_MEM_START_1__Class2SpillMemStart1           3
+#define R_CLASS2_SPILL_MEM_SIZE                                     0x215
+#define   O_CLASS2_SPILL_MEM_SIZE__Class2SpillMemSize                0
+#define   W_CLASS2_SPILL_MEM_SIZE__Class2SpillMemSize               32
+#define R_CLASS3_SPILL_MEM_START_0                                  0x216
+#define   O_CLASS3_SPILL_MEM_START_0__Class3SpillMemStart0           0
+#define   W_CLASS3_SPILL_MEM_START_0__Class3SpillMemStart0          32
+#define R_CLASS3_SPILL_MEM_START_1                                  0x217
+#define   O_CLASS3_SPILL_MEM_START_1__Class3SpillMemStart1           0
+#define   W_CLASS3_SPILL_MEM_START_1__Class3SpillMemStart1           3
+#define R_CLASS3_SPILL_MEM_SIZE                                     0x218
+#define   O_CLASS3_SPILL_MEM_SIZE__Class3SpillMemSize                0
+#define   W_CLASS3_SPILL_MEM_SIZE__Class3SpillMemSize               32
+#define R_REG_FRIN1_SPILL_MEM_START_0                               0x219
+#define R_REG_FRIN1_SPILL_MEM_START_1                               0x21a
+#define R_REG_FRIN1_SPILL_MEM_SIZE                                  0x21b
+#define R_SPIHNGY0                                                  0x219
+#define   O_SPIHNGY0__EG_HNGY_THRESH_0                              24
+#define   W_SPIHNGY0__EG_HNGY_THRESH_0                              7
+#define   O_SPIHNGY0__EG_HNGY_THRESH_1                              16
+#define   W_SPIHNGY0__EG_HNGY_THRESH_1                              7
+#define   O_SPIHNGY0__EG_HNGY_THRESH_2                              8
+#define   W_SPIHNGY0__EG_HNGY_THRESH_2                              7
+#define   O_SPIHNGY0__EG_HNGY_THRESH_3                              0
+#define   W_SPIHNGY0__EG_HNGY_THRESH_3                              7
+#define R_SPIHNGY1                                                  0x21A
+#define   O_SPIHNGY1__EG_HNGY_THRESH_4                              24
+#define   W_SPIHNGY1__EG_HNGY_THRESH_4                              7
+#define   O_SPIHNGY1__EG_HNGY_THRESH_5                              16
+#define   W_SPIHNGY1__EG_HNGY_THRESH_5                              7
+#define   O_SPIHNGY1__EG_HNGY_THRESH_6                              8
+#define   W_SPIHNGY1__EG_HNGY_THRESH_6                              7
+#define   O_SPIHNGY1__EG_HNGY_THRESH_7                              0
+#define   W_SPIHNGY1__EG_HNGY_THRESH_7                              7
+#define R_SPIHNGY2                                                  0x21B
+#define   O_SPIHNGY2__EG_HNGY_THRESH_8                              24
+#define   W_SPIHNGY2__EG_HNGY_THRESH_8                              7
+#define   O_SPIHNGY2__EG_HNGY_THRESH_9                              16
+#define   W_SPIHNGY2__EG_HNGY_THRESH_9                              7
+#define   O_SPIHNGY2__EG_HNGY_THRESH_10                             8
+#define   W_SPIHNGY2__EG_HNGY_THRESH_10                             7
+#define   O_SPIHNGY2__EG_HNGY_THRESH_11                             0
+#define   W_SPIHNGY2__EG_HNGY_THRESH_11                             7
+#define R_SPIHNGY3                                                  0x21C
+#define   O_SPIHNGY3__EG_HNGY_THRESH_12                             24
+#define   W_SPIHNGY3__EG_HNGY_THRESH_12                             7
+#define   O_SPIHNGY3__EG_HNGY_THRESH_13                             16
+#define   W_SPIHNGY3__EG_HNGY_THRESH_13                             7
+#define   O_SPIHNGY3__EG_HNGY_THRESH_14                             8
+#define   W_SPIHNGY3__EG_HNGY_THRESH_14                             7
+#define   O_SPIHNGY3__EG_HNGY_THRESH_15                             0
+#define   W_SPIHNGY3__EG_HNGY_THRESH_15                             7
+#define R_SPISTRV0                                                  0x21D
+#define   O_SPISTRV0__EG_STRV_THRESH_0                              24
+#define   W_SPISTRV0__EG_STRV_THRESH_0                              7
+#define   O_SPISTRV0__EG_STRV_THRESH_1                              16
+#define   W_SPISTRV0__EG_STRV_THRESH_1                              7
+#define   O_SPISTRV0__EG_STRV_THRESH_2                              8
+#define   W_SPISTRV0__EG_STRV_THRESH_2                              7
+#define   O_SPISTRV0__EG_STRV_THRESH_3                              0
+#define   W_SPISTRV0__EG_STRV_THRESH_3                              7
+#define R_SPISTRV1                                                  0x21E
+#define   O_SPISTRV1__EG_STRV_THRESH_4                              24
+#define   W_SPISTRV1__EG_STRV_THRESH_4                              7
+#define   O_SPISTRV1__EG_STRV_THRESH_5                              16
+#define   W_SPISTRV1__EG_STRV_THRESH_5                              7
+#define   O_SPISTRV1__EG_STRV_THRESH_6                              8
+#define   W_SPISTRV1__EG_STRV_THRESH_6                              7
+#define   O_SPISTRV1__EG_STRV_THRESH_7                              0
+#define   W_SPISTRV1__EG_STRV_THRESH_7                              7
+#define R_SPISTRV2                                                  0x21F
+#define   O_SPISTRV2__EG_STRV_THRESH_8                              24
+#define   W_SPISTRV2__EG_STRV_THRESH_8                              7
+#define   O_SPISTRV2__EG_STRV_THRESH_9                              16
+#define   W_SPISTRV2__EG_STRV_THRESH_9                              7
+#define   O_SPISTRV2__EG_STRV_THRESH_10                             8
+#define   W_SPISTRV2__EG_STRV_THRESH_10                             7
+#define   O_SPISTRV2__EG_STRV_THRESH_11                             0
+#define   W_SPISTRV2__EG_STRV_THRESH_11                             7
+#define R_SPISTRV3                                                  0x220
+#define   O_SPISTRV3__EG_STRV_THRESH_12                             24
+#define   W_SPISTRV3__EG_STRV_THRESH_12                             7
+#define   O_SPISTRV3__EG_STRV_THRESH_13                             16
+#define   W_SPISTRV3__EG_STRV_THRESH_13                             7
+#define   O_SPISTRV3__EG_STRV_THRESH_14                             8
+#define   W_SPISTRV3__EG_STRV_THRESH_14                             7
+#define   O_SPISTRV3__EG_STRV_THRESH_15                             0
+#define   W_SPISTRV3__EG_STRV_THRESH_15                             7
+#define R_TXDATAFIFO0                                               0x221
+#define   O_TXDATAFIFO0__Tx0DataFifoStart                           24
+#define   W_TXDATAFIFO0__Tx0DataFifoStart                           7
+#define   O_TXDATAFIFO0__Tx0DataFifoSize                            16
+#define   W_TXDATAFIFO0__Tx0DataFifoSize                            7
+#define   O_TXDATAFIFO0__Tx1DataFifoStart                           8
+#define   W_TXDATAFIFO0__Tx1DataFifoStart                           7
+#define   O_TXDATAFIFO0__Tx1DataFifoSize                            0
+#define   W_TXDATAFIFO0__Tx1DataFifoSize                            7
+#define R_TXDATAFIFO1                                               0x222
+#define   O_TXDATAFIFO1__Tx2DataFifoStart                           24
+#define   W_TXDATAFIFO1__Tx2DataFifoStart                           7
+#define   O_TXDATAFIFO1__Tx2DataFifoSize                            16
+#define   W_TXDATAFIFO1__Tx2DataFifoSize                            7
+#define   O_TXDATAFIFO1__Tx3DataFifoStart                           8
+#define   W_TXDATAFIFO1__Tx3DataFifoStart                           7
+#define   O_TXDATAFIFO1__Tx3DataFifoSize                            0
+#define   W_TXDATAFIFO1__Tx3DataFifoSize                            7
+#define R_TXDATAFIFO2                                               0x223
+#define   O_TXDATAFIFO2__Tx4DataFifoStart                           24
+#define   W_TXDATAFIFO2__Tx4DataFifoStart                           7
+#define   O_TXDATAFIFO2__Tx4DataFifoSize                            16
+#define   W_TXDATAFIFO2__Tx4DataFifoSize                            7
+#define   O_TXDATAFIFO2__Tx5DataFifoStart                           8
+#define   W_TXDATAFIFO2__Tx5DataFifoStart                           7
+#define   O_TXDATAFIFO2__Tx5DataFifoSize                            0
+#define   W_TXDATAFIFO2__Tx5DataFifoSize                            7
+#define R_TXDATAFIFO3                                               0x224
+#define   O_TXDATAFIFO3__Tx6DataFifoStart                           24
+#define   W_TXDATAFIFO3__Tx6DataFifoStart                           7
+#define   O_TXDATAFIFO3__Tx6DataFifoSize                            16
+#define   W_TXDATAFIFO3__Tx6DataFifoSize                            7
+#define   O_TXDATAFIFO3__Tx7DataFifoStart                           8
+#define   W_TXDATAFIFO3__Tx7DataFifoStart                           7
+#define   O_TXDATAFIFO3__Tx7DataFifoSize                            0
+#define   W_TXDATAFIFO3__Tx7DataFifoSize                            7
+#define R_TXDATAFIFO4                                               0x225
+#define   O_TXDATAFIFO4__Tx8DataFifoStart                           24
+#define   W_TXDATAFIFO4__Tx8DataFifoStart                           7
+#define   O_TXDATAFIFO4__Tx8DataFifoSize                            16
+#define   W_TXDATAFIFO4__Tx8DataFifoSize                            7
+#define   O_TXDATAFIFO4__Tx9DataFifoStart                           8
+#define   W_TXDATAFIFO4__Tx9DataFifoStart                           7
+#define   O_TXDATAFIFO4__Tx9DataFifoSize                            0
+#define   W_TXDATAFIFO4__Tx9DataFifoSize                            7
+#define R_TXDATAFIFO5                                               0x226
+#define   O_TXDATAFIFO5__Tx10DataFifoStart                          24
+#define   W_TXDATAFIFO5__Tx10DataFifoStart                          7
+#define   O_TXDATAFIFO5__Tx10DataFifoSize                           16
+#define   W_TXDATAFIFO5__Tx10DataFifoSize                           7
+#define   O_TXDATAFIFO5__Tx11DataFifoStart                          8
+#define   W_TXDATAFIFO5__Tx11DataFifoStart                          7
+#define   O_TXDATAFIFO5__Tx11DataFifoSize                           0
+#define   W_TXDATAFIFO5__Tx11DataFifoSize                           7
+#define R_TXDATAFIFO6                                               0x227
+#define   O_TXDATAFIFO6__Tx12DataFifoStart                          24
+#define   W_TXDATAFIFO6__Tx12DataFifoStart                          7
+#define   O_TXDATAFIFO6__Tx12DataFifoSize                           16
+#define   W_TXDATAFIFO6__Tx12DataFifoSize                           7
+#define   O_TXDATAFIFO6__Tx13DataFifoStart                          8
+#define   W_TXDATAFIFO6__Tx13DataFifoStart                          7
+#define   O_TXDATAFIFO6__Tx13DataFifoSize                           0
+#define   W_TXDATAFIFO6__Tx13DataFifoSize                           7
+#define R_TXDATAFIFO7                                               0x228
+#define   O_TXDATAFIFO7__Tx14DataFifoStart                          24
+#define   W_TXDATAFIFO7__Tx14DataFifoStart                          7
+#define   O_TXDATAFIFO7__Tx14DataFifoSize                           16
+#define   W_TXDATAFIFO7__Tx14DataFifoSize                           7
+#define   O_TXDATAFIFO7__Tx15DataFifoStart                          8
+#define   W_TXDATAFIFO7__Tx15DataFifoStart                          7
+#define   O_TXDATAFIFO7__Tx15DataFifoSize                           0
+#define   W_TXDATAFIFO7__Tx15DataFifoSize                           7
+#define R_RXDATAFIFO0                                               0x229
+#define   O_RXDATAFIFO0__Rx0DataFifoStart                           24
+#define   W_RXDATAFIFO0__Rx0DataFifoStart                           7
+#define   O_RXDATAFIFO0__Rx0DataFifoSize                            16
+#define   W_RXDATAFIFO0__Rx0DataFifoSize                            7
+#define   O_RXDATAFIFO0__Rx1DataFifoStart                           8
+#define   W_RXDATAFIFO0__Rx1DataFifoStart                           7
+#define   O_RXDATAFIFO0__Rx1DataFifoSize                            0
+#define   W_RXDATAFIFO0__Rx1DataFifoSize                            7
+#define R_RXDATAFIFO1                                               0x22A
+#define   O_RXDATAFIFO1__Rx2DataFifoStart                           24
+#define   W_RXDATAFIFO1__Rx2DataFifoStart                           7
+#define   O_RXDATAFIFO1__Rx2DataFifoSize                            16
+#define   W_RXDATAFIFO1__Rx2DataFifoSize                            7
+#define   O_RXDATAFIFO1__Rx3DataFifoStart                           8
+#define   W_RXDATAFIFO1__Rx3DataFifoStart                           7
+#define   O_RXDATAFIFO1__Rx3DataFifoSize                            0
+#define   W_RXDATAFIFO1__Rx3DataFifoSize                            7
+#define R_RXDATAFIFO2                                               0x22B
+#define   O_RXDATAFIFO2__Rx4DataFifoStart                           24
+#define   W_RXDATAFIFO2__Rx4DataFifoStart                           7
+#define   O_RXDATAFIFO2__Rx4DataFifoSize                            16
+#define   W_RXDATAFIFO2__Rx4DataFifoSize                            7
+#define   O_RXDATAFIFO2__Rx5DataFifoStart                           8
+#define   W_RXDATAFIFO2__Rx5DataFifoStart                           7
+#define   O_RXDATAFIFO2__Rx5DataFifoSize                            0
+#define   W_RXDATAFIFO2__Rx5DataFifoSize                            7
+#define R_RXDATAFIFO3                                               0x22C
+#define   O_RXDATAFIFO3__Rx6DataFifoStart                           24
+#define   W_RXDATAFIFO3__Rx6DataFifoStart                           7
+#define   O_RXDATAFIFO3__Rx6DataFifoSize                            16
+#define   W_RXDATAFIFO3__Rx6DataFifoSize                            7
+#define   O_RXDATAFIFO3__Rx7DataFifoStart                           8
+#define   W_RXDATAFIFO3__Rx7DataFifoStart                           7
+#define   O_RXDATAFIFO3__Rx7DataFifoSize                            0
+#define   W_RXDATAFIFO3__Rx7DataFifoSize                            7
+#define R_RXDATAFIFO4                                               0x22D
+#define   O_RXDATAFIFO4__Rx8DataFifoStart                           24
+#define   W_RXDATAFIFO4__Rx8DataFifoStart                           7
+#define   O_RXDATAFIFO4__Rx8DataFifoSize                            16
+#define   W_RXDATAFIFO4__Rx8DataFifoSize                            7
+#define   O_RXDATAFIFO4__Rx9DataFifoStart                           8
+#define   W_RXDATAFIFO4__Rx9DataFifoStart                           7
+#define   O_RXDATAFIFO4__Rx9DataFifoSize                            0
+#define   W_RXDATAFIFO4__Rx9DataFifoSize                            7
+#define R_RXDATAFIFO5                                               0x22E
+#define   O_RXDATAFIFO5__Rx10DataFifoStart                          24
+#define   W_RXDATAFIFO5__Rx10DataFifoStart                          7
+#define   O_RXDATAFIFO5__Rx10DataFifoSize                           16
+#define   W_RXDATAFIFO5__Rx10DataFifoSize                           7
+#define   O_RXDATAFIFO5__Rx11DataFifoStart                          8
+#define   W_RXDATAFIFO5__Rx11DataFifoStart                          7
+#define   O_RXDATAFIFO5__Rx11DataFifoSize                           0
+#define   W_RXDATAFIFO5__Rx11DataFifoSize                           7
+#define R_RXDATAFIFO6                                               0x22F
+#define   O_RXDATAFIFO6__Rx12DataFifoStart                          24
+#define   W_RXDATAFIFO6__Rx12DataFifoStart                          7
+#define   O_RXDATAFIFO6__Rx12DataFifoSize                           16
+#define   W_RXDATAFIFO6__Rx12DataFifoSize                           7
+#define   O_RXDATAFIFO6__Rx13DataFifoStart                          8
+#define   W_RXDATAFIFO6__Rx13DataFifoStart                          7
+#define   O_RXDATAFIFO6__Rx13DataFifoSize                           0
+#define   W_RXDATAFIFO6__Rx13DataFifoSize                           7
+#define R_RXDATAFIFO7                                               0x230
+#define   O_RXDATAFIFO7__Rx14DataFifoStart                          24
+#define   W_RXDATAFIFO7__Rx14DataFifoStart                          7
+#define   O_RXDATAFIFO7__Rx14DataFifoSize                           16
+#define   W_RXDATAFIFO7__Rx14DataFifoSize                           7
+#define   O_RXDATAFIFO7__Rx15DataFifoStart                          8
+#define   W_RXDATAFIFO7__Rx15DataFifoStart                          7
+#define   O_RXDATAFIFO7__Rx15DataFifoSize                           0
+#define   W_RXDATAFIFO7__Rx15DataFifoSize                           7
+#define R_XGMACPADCALIBRATION                                       0x231
+#define R_FREEQCARVE                                                0x233
+#define R_SPI4STATICDELAY0                                          0x240
+#define   O_SPI4STATICDELAY0__DataLine7                             28
+#define   W_SPI4STATICDELAY0__DataLine7                             4
+#define   O_SPI4STATICDELAY0__DataLine6                             24
+#define   W_SPI4STATICDELAY0__DataLine6                             4
+#define   O_SPI4STATICDELAY0__DataLine5                             20
+#define   W_SPI4STATICDELAY0__DataLine5                             4
+#define   O_SPI4STATICDELAY0__DataLine4                             16
+#define   W_SPI4STATICDELAY0__DataLine4                             4
+#define   O_SPI4STATICDELAY0__DataLine3                             12
+#define   W_SPI4STATICDELAY0__DataLine3                             4
+#define   O_SPI4STATICDELAY0__DataLine2                             8
+#define   W_SPI4STATICDELAY0__DataLine2                             4
+#define   O_SPI4STATICDELAY0__DataLine1                             4
+#define   W_SPI4STATICDELAY0__DataLine1                             4
+#define   O_SPI4STATICDELAY0__DataLine0                             0
+#define   W_SPI4STATICDELAY0__DataLine0                             4
+#define R_SPI4STATICDELAY1                                          0x241
+#define   O_SPI4STATICDELAY1__DataLine15                            28
+#define   W_SPI4STATICDELAY1__DataLine15                            4
+#define   O_SPI4STATICDELAY1__DataLine14                            24
+#define   W_SPI4STATICDELAY1__DataLine14                            4
+#define   O_SPI4STATICDELAY1__DataLine13                            20
+#define   W_SPI4STATICDELAY1__DataLine13                            4
+#define   O_SPI4STATICDELAY1__DataLine12                            16
+#define   W_SPI4STATICDELAY1__DataLine12                            4
+#define   O_SPI4STATICDELAY1__DataLine11                            12
+#define   W_SPI4STATICDELAY1__DataLine11                            4
+#define   O_SPI4STATICDELAY1__DataLine10                            8
+#define   W_SPI4STATICDELAY1__DataLine10                            4
+#define   O_SPI4STATICDELAY1__DataLine9                             4
+#define   W_SPI4STATICDELAY1__DataLine9                             4
+#define   O_SPI4STATICDELAY1__DataLine8                             0
+#define   W_SPI4STATICDELAY1__DataLine8                             4
+#define R_SPI4STATICDELAY2                                          0x242
+#define   O_SPI4STATICDELAY0__TxStat1                               8
+#define   W_SPI4STATICDELAY0__TxStat1                               4
+#define   O_SPI4STATICDELAY0__TxStat0                               4
+#define   W_SPI4STATICDELAY0__TxStat0                               4
+#define   O_SPI4STATICDELAY0__RxControl                             0
+#define   W_SPI4STATICDELAY0__RxControl                             4
+#define R_SPI4CONTROL                                               0x243
+#define   O_SPI4CONTROL__StaticDelay                                2
+#define   O_SPI4CONTROL__LVDS_LVTTL                                 1
+#define   O_SPI4CONTROL__SPI4Enable                                 0
+#define R_CLASSWATERMARKS                                           0x244
+#define   O_CLASSWATERMARKS__Class0Watermark                        24
+#define   W_CLASSWATERMARKS__Class0Watermark                        5
+#define   O_CLASSWATERMARKS__Class1Watermark                        16
+#define   W_CLASSWATERMARKS__Class1Watermark                        5
+#define   O_CLASSWATERMARKS__Class3Watermark                        0
+#define   W_CLASSWATERMARKS__Class3Watermark                        5
+#define R_RXWATERMARKS1                                              0x245
+#define   O_RXWATERMARKS__Rx0DataWatermark                          24
+#define   W_RXWATERMARKS__Rx0DataWatermark                          7
+#define   O_RXWATERMARKS__Rx1DataWatermark                          16
+#define   W_RXWATERMARKS__Rx1DataWatermark                          7
+#define   O_RXWATERMARKS__Rx3DataWatermark                          0
+#define   W_RXWATERMARKS__Rx3DataWatermark                          7
+#define R_RXWATERMARKS2                                              0x246
+#define   O_RXWATERMARKS__Rx4DataWatermark                          24
+#define   W_RXWATERMARKS__Rx4DataWatermark                          7
+#define   O_RXWATERMARKS__Rx5DataWatermark                          16
+#define   W_RXWATERMARKS__Rx5DataWatermark                          7
+#define   O_RXWATERMARKS__Rx6DataWatermark                          8
+#define   W_RXWATERMARKS__Rx6DataWatermark                          7
+#define   O_RXWATERMARKS__Rx7DataWatermark                          0
+#define   W_RXWATERMARKS__Rx7DataWatermark                          7
+#define R_RXWATERMARKS3                                              0x247
+#define   O_RXWATERMARKS__Rx8DataWatermark                          24
+#define   W_RXWATERMARKS__Rx8DataWatermark                          7
+#define   O_RXWATERMARKS__Rx9DataWatermark                          16
+#define   W_RXWATERMARKS__Rx9DataWatermark                          7
+#define   O_RXWATERMARKS__Rx10DataWatermark                         8
+#define   W_RXWATERMARKS__Rx10DataWatermark                         7
+#define   O_RXWATERMARKS__Rx11DataWatermark                         0
+#define   W_RXWATERMARKS__Rx11DataWatermark                         7
+#define R_RXWATERMARKS4                                              0x248
+#define   O_RXWATERMARKS__Rx12DataWatermark                         24
+#define   W_RXWATERMARKS__Rx12DataWatermark                         7
+#define   O_RXWATERMARKS__Rx13DataWatermark                         16
+#define   W_RXWATERMARKS__Rx13DataWatermark                         7
+#define   O_RXWATERMARKS__Rx14DataWatermark                         8
+#define   W_RXWATERMARKS__Rx14DataWatermark                         7
+#define   O_RXWATERMARKS__Rx15DataWatermark                         0
+#define   W_RXWATERMARKS__Rx15DataWatermark                         7
+#define R_FREEWATERMARKS                                            0x249
+#define   O_FREEWATERMARKS__FreeOutWatermark                        16
+#define   W_FREEWATERMARKS__FreeOutWatermark                        16
+#define   O_FREEWATERMARKS__JumFrWatermark                          8
+#define   W_FREEWATERMARKS__JumFrWatermark                          7
+#define   O_FREEWATERMARKS__RegFrWatermark                          0
+#define   W_FREEWATERMARKS__RegFrWatermark                          7
+#define R_EGRESSFIFOCARVINGSLOTS                                    0x24a
+
+#define CTRL_RES0           0
+#define CTRL_RES1           1
+#define CTRL_REG_FREE       2
+#define CTRL_JUMBO_FREE     3
+#define CTRL_CONT           4
+#define CTRL_EOP            5
+#define CTRL_START          6
+#define CTRL_SNGL           7
+
+#define CTRL_B0_NOT_EOP     0
+#define CTRL_B0_EOP         1
+
+#define R_ROUND_ROBIN_TABLE                 0
+#define R_PDE_CLASS_0                       0x300
+#define R_PDE_CLASS_1                       0x302
+#define R_PDE_CLASS_2                       0x304
+#define R_PDE_CLASS_3                       0x306
+
+#define R_MSG_TX_THRESHOLD                  0x308
+
+#define R_GMAC_JFR0_BUCKET_SIZE              0x320
+#define R_GMAC_RFR0_BUCKET_SIZE              0x321
+#define R_GMAC_TX0_BUCKET_SIZE              0x322
+#define R_GMAC_TX1_BUCKET_SIZE              0x323
+#define R_GMAC_TX2_BUCKET_SIZE              0x324
+#define R_GMAC_TX3_BUCKET_SIZE              0x325
+#define R_GMAC_JFR1_BUCKET_SIZE              0x326
+#define R_GMAC_RFR1_BUCKET_SIZE              0x327
+
+#define R_XGS_TX0_BUCKET_SIZE               0x320
+#define R_XGS_TX1_BUCKET_SIZE               0x321
+#define R_XGS_TX2_BUCKET_SIZE               0x322
+#define R_XGS_TX3_BUCKET_SIZE               0x323
+#define R_XGS_TX4_BUCKET_SIZE               0x324
+#define R_XGS_TX5_BUCKET_SIZE               0x325
+#define R_XGS_TX6_BUCKET_SIZE               0x326
+#define R_XGS_TX7_BUCKET_SIZE               0x327
+#define R_XGS_TX8_BUCKET_SIZE               0x328
+#define R_XGS_TX9_BUCKET_SIZE               0x329
+#define R_XGS_TX10_BUCKET_SIZE              0x32A
+#define R_XGS_TX11_BUCKET_SIZE              0x32B
+#define R_XGS_TX12_BUCKET_SIZE              0x32C
+#define R_XGS_TX13_BUCKET_SIZE              0x32D
+#define R_XGS_TX14_BUCKET_SIZE              0x32E
+#define R_XGS_TX15_BUCKET_SIZE              0x32F
+#define R_XGS_JFR_BUCKET_SIZE               0x330
+#define R_XGS_RFR_BUCKET_SIZE               0x331
+
+#define R_CC_CPU0_0                         0x380
+#define R_CC_CPU1_0                         0x388
+#define R_CC_CPU2_0                         0x390
+#define R_CC_CPU3_0                         0x398
+#define R_CC_CPU4_0                         0x3a0
+#define R_CC_CPU5_0                         0x3a8
+#define R_CC_CPU6_0                         0x3b0
+#define R_CC_CPU7_0                         0x3b8
+
+#define XLR_GMAC_BLK_SZ		            (XLR_IO_GMAC_1_OFFSET - \
+		XLR_IO_GMAC_0_OFFSET)
+
+/* Constants used for configuring the devices */
+
+#define XLR_FB_STN			6 /* Bucket used for Tx freeback */
+
+#define MAC_B2B_IPG                     88
+
+#define	XLR_NET_PREPAD_LEN		32
+
+/* frame sizes need to be cacheline aligned */
+#define MAX_FRAME_SIZE                  (1536 + XLR_NET_PREPAD_LEN)
+#define MAX_FRAME_SIZE_JUMBO            9216
+
+#define MAC_SKB_BACK_PTR_SIZE           SMP_CACHE_BYTES
+#define MAC_PREPAD                      0
+#define BYTE_OFFSET                     2
+#define XLR_RX_BUF_SIZE                 (MAX_FRAME_SIZE + BYTE_OFFSET + \
+		MAC_PREPAD + MAC_SKB_BACK_PTR_SIZE + SMP_CACHE_BYTES)
+#define MAC_CRC_LEN                     4
+#define MAX_NUM_MSGRNG_STN_CC           128
+#define MAX_MSG_SND_ATTEMPTS		100	/* 13 stns x 4 entry msg/stn +
+						   headroom */
+
+#define MAC_FRIN_TO_BE_SENT_THRESHOLD   16
+
+#define MAX_NUM_DESC_SPILL		1024
+#define MAX_FRIN_SPILL                  (MAX_NUM_DESC_SPILL << 2)
+#define MAX_FROUT_SPILL                 (MAX_NUM_DESC_SPILL << 2)
+#define MAX_CLASS_0_SPILL               (MAX_NUM_DESC_SPILL << 2)
+#define MAX_CLASS_1_SPILL               (MAX_NUM_DESC_SPILL << 2)
+#define MAX_CLASS_2_SPILL               (MAX_NUM_DESC_SPILL << 2)
+#define MAX_CLASS_3_SPILL               (MAX_NUM_DESC_SPILL << 2)
+
+enum {
+	SGMII_SPEED_10 = 0x00000000,
+	SGMII_SPEED_100 = 0x02000000,
+	SGMII_SPEED_1000 = 0x04000000,
+};
+
+enum tsv_rsv_reg {
+	TX_RX_64_BYTE_FRAME = 0x20,
+	TX_RX_64_127_BYTE_FRAME,
+	TX_RX_128_255_BYTE_FRAME,
+	TX_RX_256_511_BYTE_FRAME,
+	TX_RX_512_1023_BYTE_FRAME,
+	TX_RX_1024_1518_BYTE_FRAME,
+	TX_RX_1519_1522_VLAN_BYTE_FRAME,
+
+	RX_BYTE_COUNTER = 0x27,
+	RX_PACKET_COUNTER,
+	RX_FCS_ERROR_COUNTER,
+	RX_MULTICAST_PACKET_COUNTER,
+	RX_BROADCAST_PACKET_COUNTER,
+	RX_CONTROL_FRAME_PACKET_COUNTER,
+	RX_PAUSE_FRAME_PACKET_COUNTER,
+	RX_UNKNOWN_OP_CODE_COUNTER,
+	RX_ALIGNMENT_ERROR_COUNTER,
+	RX_FRAME_LENGTH_ERROR_COUNTER,
+	RX_CODE_ERROR_COUNTER,
+	RX_CARRIER_SENSE_ERROR_COUNTER,
+	RX_UNDERSIZE_PACKET_COUNTER,
+	RX_OVERSIZE_PACKET_COUNTER,
+	RX_FRAGMENTS_COUNTER,
+	RX_JABBER_COUNTER,
+	RX_DROP_PACKET_COUNTER,
+
+	TX_BYTE_COUNTER   = 0x38,
+	TX_PACKET_COUNTER,
+	TX_MULTICAST_PACKET_COUNTER,
+	TX_BROADCAST_PACKET_COUNTER,
+	TX_PAUSE_CONTROL_FRAME_COUNTER,
+	TX_DEFERRAL_PACKET_COUNTER,
+	TX_EXCESSIVE_DEFERRAL_PACKET_COUNTER,
+	TX_SINGLE_COLLISION_PACKET_COUNTER,
+	TX_MULTI_COLLISION_PACKET_COUNTER,
+	TX_LATE_COLLISION_PACKET_COUNTER,
+	TX_EXCESSIVE_COLLISION_PACKET_COUNTER,
+	TX_TOTAL_COLLISION_COUNTER,
+	TX_PAUSE_FRAME_HONERED_COUNTER,
+	TX_DROP_FRAME_COUNTER,
+	TX_JABBER_FRAME_COUNTER,
+	TX_FCS_ERROR_COUNTER,
+	TX_CONTROL_FRAME_COUNTER,
+	TX_OVERSIZE_FRAME_COUNTER,
+	TX_UNDERSIZE_FRAME_COUNTER,
+	TX_FRAGMENT_FRAME_COUNTER,
+
+	CARRY_REG_1 = 0x4c,
+	CARRY_REG_2 = 0x4d,
+};
+
+struct xlr_net_priv {
+	u32 __iomem *base_addr;
+	struct net_device *ndev;
+	struct mii_bus *mii_bus;
+	int num_rx_desc;
+	int phy_addr;	/* PHY addr on MDIO bus */
+	int pcs_id;	/* PCS id on MDIO bus */
+	int port_id;	/* Port(gmac/xgmac) number, i.e 0-7 */
+	u32 __iomem *mii_addr;
+	u32 __iomem *serdes_addr;
+	u32 __iomem *pcs_addr;
+	u32 __iomem *gpio_addr;
+	int phy_speed;
+	int port_type;
+	struct timer_list queue_timer;
+	int wakeup_q;
+	struct platform_device *pdev;
+	struct xlr_net_data *nd;
+
+	u64 *frin_spill;
+	u64 *frout_spill;
+	u64 *class_0_spill;
+	u64 *class_1_spill;
+	u64 *class_2_spill;
+	u64 *class_3_spill;
+};
+
+extern void xlr_set_gmac_speed(struct xlr_net_priv *priv);
diff --git a/drivers/staging/nvec/Kconfig b/drivers/staging/nvec/Kconfig
index f779fdc3427..7e61adacd15 100644
--- a/drivers/staging/nvec/Kconfig
+++ b/drivers/staging/nvec/Kconfig
@@ -1,5 +1,5 @@
 config MFD_NVEC
-	bool "NV Tegra Embedded Controller SMBus Interface"
+	tristate "NV Tegra Embedded Controller SMBus Interface"
 	depends on I2C && GPIOLIB && ARCH_TEGRA
 	select MFD_CORE
 	help
@@ -7,28 +7,28 @@ config MFD_NVEC
 	    controller.
 
 config KEYBOARD_NVEC
-	bool "Keyboard on nVidia compliant EC"
+	tristate "Keyboard on nVidia compliant EC"
 	depends on MFD_NVEC && INPUT
 	help
 	  Say Y here to enable support for a keyboard connected to 
 	  a nVidia compliant embedded controller.
 
 config SERIO_NVEC_PS2
-	bool "PS2 on nVidia EC"
+	tristate "PS2 on nVidia EC"
 	depends on MFD_NVEC && SERIO
 	help
 	  Say Y here to enable support for a Touchpad / Mouse connected
 	  to a nVidia compliant embedded controller.
 
 config NVEC_POWER
-	bool "NVEC charger and battery"
+	tristate "NVEC charger and battery"
 	depends on MFD_NVEC && POWER_SUPPLY
 	help
 	  Say Y to enable support for battery and charger interface for
 	  nVidia compliant embedded controllers.
 
 config NVEC_PAZ00
-	bool "Support for OEM specific functions on Compal PAZ00 based devices"
+	tristate "Support for OEM specific functions on Compal PAZ00 based devices"
 	depends on MFD_NVEC && LEDS_CLASS
 	help
 	  Say Y to enable control of the yellow side leds on Compal PAZ00 based
diff --git a/drivers/staging/nvec/nvec.c b/drivers/staging/nvec/nvec.c
index cf159365b0e..a88959f9a07 100644
--- a/drivers/staging/nvec/nvec.c
+++ b/drivers/staging/nvec/nvec.c
@@ -352,10 +352,10 @@ static void nvec_toggle_global_events(struct nvec_chip *nvec, bool state)
  */
 static void nvec_event_mask(char *ev, u32 mask)
 {
-	ev[3] = mask >> 16 && 0xff;
-	ev[4] = mask >> 24 && 0xff;
-	ev[5] = mask >> 0  && 0xff;
-	ev[6] = mask >> 8  && 0xff;
+	ev[3] = mask >> 16 & 0xff;
+	ev[4] = mask >> 24 & 0xff;
+	ev[5] = mask >> 0  & 0xff;
+	ev[6] = mask >> 8  & 0xff;
 }
 
 /**
diff --git a/drivers/staging/olpc_dcon/olpc_dcon.c b/drivers/staging/olpc_dcon/olpc_dcon.c
index 54ed6f69e3d..193e1c68bb4 100644
--- a/drivers/staging/olpc_dcon/olpc_dcon.c
+++ b/drivers/staging/olpc_dcon/olpc_dcon.c
@@ -550,12 +550,12 @@ static int dcon_reboot_notify(struct notifier_block *nb,
 	struct dcon_priv *dcon = container_of(nb, struct dcon_priv, reboot_nb);
 
 	if (!dcon || !dcon->client)
-		return 0;
+		return NOTIFY_DONE;
 
 	/* Turn off the DCON. Entirely. */
 	dcon_write(dcon, DCON_REG_MODE, 0x39);
 	dcon_write(dcon, DCON_REG_MODE, 0x32);
-	return 0;
+	return NOTIFY_DONE;
 }
 
 static int unfreeze_on_panic(struct notifier_block *nb,
diff --git a/drivers/staging/omap-thermal/Makefile b/drivers/staging/omap-thermal/Makefile
deleted file mode 100644
index 091c4d20b14..00000000000
--- a/drivers/staging/omap-thermal/Makefile
+++ /dev/null
@@ -1,5 +0,0 @@
-obj-$(CONFIG_OMAP_BANDGAP)	+= omap-thermal.o
-omap-thermal-y			:= omap-bandgap.o
-omap-thermal-$(CONFIG_OMAP_THERMAL)	+= omap-thermal-common.o
-omap-thermal-$(CONFIG_OMAP4_THERMAL)	+= omap4-thermal.o
-omap-thermal-$(CONFIG_OMAP5_THERMAL)	+= omap5-thermal.o
diff --git a/drivers/staging/omap-thermal/TODO b/drivers/staging/omap-thermal/TODO
deleted file mode 100644
index 9e23cc4d551..00000000000
--- a/drivers/staging/omap-thermal/TODO
+++ /dev/null
@@ -1,28 +0,0 @@
-List of TODOs (by Eduardo Valentin)
-
-on omap-bandgap.c:
-- Rework locking
-- Improve driver code by adding usage of regmap-mmio
-- Test every exposed API to userland
-- Add support to hwmon
-- Review and revisit all API exposed
-- Revisit PM support
-- Revisit data structures and simplify them
-- Once SCM-core api settles, update this driver accordingly
-
-on omap-thermal-common.c/omap-thermal.h:
-- Revisit extrapolation constants for O4/O5
-- Revisit need for locking
-- Revisit trips and its definitions
-- Revisit trending
-
-on omap5-thermal.c
-- Add support for GPU cooling
-
-generally:
-- write Kconfig dependencies so that omap variants are covered
-- make checkpatch.pl and sparse happy
-- make sure this code works on OMAP4430, OMAP4460 and OMAP5430
-- update documentation
-
-Copy patches to Eduardo Valentin <eduardo.valentin@ti.com>
diff --git a/drivers/staging/omap-thermal/omap-bandgap.c b/drivers/staging/omap-thermal/omap-bandgap.c
deleted file mode 100644
index dcc1448dbf8..00000000000
--- a/drivers/staging/omap-thermal/omap-bandgap.c
+++ /dev/null
@@ -1,1174 +0,0 @@
-/*
- * OMAP4 Bandgap temperature sensor driver
- *
- * Copyright (C) 2011-2012 Texas Instruments Incorporated - http://www.ti.com/
- * Author: J Keerthy <j-keerthy@ti.com>
- * Author: Moiz Sonasath <m-sonasath@ti.com>
- * Couple of fixes, DT and MFD adaptation:
- *   Eduardo Valentin <eduardo.valentin@ti.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/module.h>
-#include <linux/export.h>
-#include <linux/init.h>
-#include <linux/kernel.h>
-#include <linux/interrupt.h>
-#include <linux/clk.h>
-#include <linux/gpio.h>
-#include <linux/platform_device.h>
-#include <linux/err.h>
-#include <linux/types.h>
-#include <linux/mutex.h>
-#include <linux/reboot.h>
-#include <linux/of_device.h>
-#include <linux/of_platform.h>
-#include <linux/of_irq.h>
-#include <linux/io.h>
-
-#include "omap-bandgap.h"
-
-static u32 omap_bandgap_readl(struct omap_bandgap *bg_ptr, u32 reg)
-{
-	return readl(bg_ptr->base + reg);
-}
-
-static void omap_bandgap_writel(struct omap_bandgap *bg_ptr, u32 val, u32 reg)
-{
-	writel(val, bg_ptr->base + reg);
-}
-
-static int omap_bandgap_power(struct omap_bandgap *bg_ptr, bool on)
-{
-	struct temp_sensor_registers *tsr;
-	int i;
-	u32 ctrl;
-
-	if (!OMAP_BANDGAP_HAS(bg_ptr, POWER_SWITCH))
-		return 0;
-
-	for (i = 0; i < bg_ptr->conf->sensor_count; i++) {
-		tsr = bg_ptr->conf->sensors[i].registers;
-		ctrl = omap_bandgap_readl(bg_ptr, tsr->temp_sensor_ctrl);
-		ctrl &= ~tsr->bgap_tempsoff_mask;
-		/* active on 0 */
-		ctrl |= !on << __ffs(tsr->bgap_tempsoff_mask);
-
-		/* write BGAP_TEMPSOFF should be reset to 0 */
-		omap_bandgap_writel(bg_ptr, ctrl, tsr->temp_sensor_ctrl);
-	}
-
-	return 0;
-}
-
-/* This is the Talert handler. Call it only if HAS(TALERT) is set */
-static irqreturn_t talert_irq_handler(int irq, void *data)
-{
-	struct omap_bandgap *bg_ptr = data;
-	struct temp_sensor_registers *tsr;
-	u32 t_hot = 0, t_cold = 0, temp, ctrl;
-	int i;
-
-	bg_ptr = data;
-	/* Read the status of t_hot */
-	for (i = 0; i < bg_ptr->conf->sensor_count; i++) {
-		tsr = bg_ptr->conf->sensors[i].registers;
-		t_hot = omap_bandgap_readl(bg_ptr, tsr->bgap_status);
-		t_hot &= tsr->status_hot_mask;
-
-		/* Read the status of t_cold */
-		t_cold = omap_bandgap_readl(bg_ptr, tsr->bgap_status);
-		t_cold &= tsr->status_cold_mask;
-
-		if (!t_cold && !t_hot)
-			continue;
-
-		ctrl = omap_bandgap_readl(bg_ptr, tsr->bgap_mask_ctrl);
-		/*
-		 * One TALERT interrupt: Two sources
-		 * If the interrupt is due to t_hot then mask t_hot and
-		 * and unmask t_cold else mask t_cold and unmask t_hot
-		 */
-		if (t_hot) {
-			ctrl &= ~tsr->mask_hot_mask;
-			ctrl |= tsr->mask_cold_mask;
-		} else if (t_cold) {
-			ctrl &= ~tsr->mask_cold_mask;
-			ctrl |= tsr->mask_hot_mask;
-		}
-
-		omap_bandgap_writel(bg_ptr, ctrl, tsr->bgap_mask_ctrl);
-
-		dev_dbg(bg_ptr->dev,
-			"%s: IRQ from %s sensor: hotevent %d coldevent %d\n",
-			__func__, bg_ptr->conf->sensors[i].domain,
-			t_hot, t_cold);
-
-		/* read temperature */
-		temp = omap_bandgap_readl(bg_ptr, tsr->temp_sensor_ctrl);
-		temp &= tsr->bgap_dtemp_mask;
-
-		/* report temperature to whom may concern */
-		if (bg_ptr->conf->report_temperature)
-			bg_ptr->conf->report_temperature(bg_ptr, i);
-	}
-
-	return IRQ_HANDLED;
-}
-
-/* This is the Tshut handler. Call it only if HAS(TSHUT) is set */
-static irqreturn_t omap_bandgap_tshut_irq_handler(int irq, void *data)
-{
-	orderly_poweroff(true);
-
-	return IRQ_HANDLED;
-}
-
-static
-int adc_to_temp_conversion(struct omap_bandgap *bg_ptr, int id, int adc_val,
-			   int *t)
-{
-	struct temp_sensor_data *ts_data = bg_ptr->conf->sensors[id].ts_data;
-
-	/* look up for temperature in the table and return the temperature */
-	if (adc_val < ts_data->adc_start_val || adc_val > ts_data->adc_end_val)
-		return -ERANGE;
-
-	*t = bg_ptr->conv_table[adc_val - ts_data->adc_start_val];
-
-	return 0;
-}
-
-static int temp_to_adc_conversion(long temp, struct omap_bandgap *bg_ptr, int i,
-				  int *adc)
-{
-	struct temp_sensor_data *ts_data = bg_ptr->conf->sensors[i].ts_data;
-	int high, low, mid;
-
-	low = 0;
-	high = ts_data->adc_end_val - ts_data->adc_start_val;
-	mid = (high + low) / 2;
-
-	if (temp < bg_ptr->conv_table[low] || temp > bg_ptr->conv_table[high])
-		return -EINVAL;
-
-	while (low < high) {
-		if (temp < bg_ptr->conv_table[mid])
-			high = mid - 1;
-		else
-			low = mid + 1;
-		mid = (low + high) / 2;
-	}
-
-	*adc = ts_data->adc_start_val + low;
-
-	return 0;
-}
-
-/* Talert masks. Call it only if HAS(TALERT) is set */
-static int temp_sensor_unmask_interrupts(struct omap_bandgap *bg_ptr, int id,
-					 u32 t_hot, u32 t_cold)
-{
-	struct temp_sensor_registers *tsr;
-	u32 temp, reg_val;
-
-	/* Read the current on die temperature */
-	tsr = bg_ptr->conf->sensors[id].registers;
-	temp = omap_bandgap_readl(bg_ptr, tsr->temp_sensor_ctrl);
-	temp &= tsr->bgap_dtemp_mask;
-
-	reg_val = omap_bandgap_readl(bg_ptr, tsr->bgap_mask_ctrl);
-	if (temp < t_hot)
-		reg_val |= tsr->mask_hot_mask;
-	else
-		reg_val &= ~tsr->mask_hot_mask;
-
-	if (t_cold < temp)
-		reg_val |= tsr->mask_cold_mask;
-	else
-		reg_val &= ~tsr->mask_cold_mask;
-	omap_bandgap_writel(bg_ptr, reg_val, tsr->bgap_mask_ctrl);
-
-	return 0;
-}
-
-static
-int add_hyst(int adc_val, int hyst_val, struct omap_bandgap *bg_ptr, int i,
-	     u32 *sum)
-{
-	int temp, ret;
-
-	ret = adc_to_temp_conversion(bg_ptr, i, adc_val, &temp);
-	if (ret < 0)
-		return ret;
-
-	temp += hyst_val;
-
-	return temp_to_adc_conversion(temp, bg_ptr, i, sum);
-}
-
-/* Talert Thot threshold. Call it only if HAS(TALERT) is set */
-static
-int temp_sensor_configure_thot(struct omap_bandgap *bg_ptr, int id, int t_hot)
-{
-	struct temp_sensor_data *ts_data = bg_ptr->conf->sensors[id].ts_data;
-	struct temp_sensor_registers *tsr;
-	u32 thresh_val, reg_val;
-	int cold, err = 0;
-
-	tsr = bg_ptr->conf->sensors[id].registers;
-
-	/* obtain the T cold value */
-	thresh_val = omap_bandgap_readl(bg_ptr, tsr->bgap_threshold);
-	cold = (thresh_val & tsr->threshold_tcold_mask) >>
-	    __ffs(tsr->threshold_tcold_mask);
-	if (t_hot <= cold) {
-		/* change the t_cold to t_hot - 5000 millidegrees */
-		err |= add_hyst(t_hot, -ts_data->hyst_val, bg_ptr, id, &cold);
-		/* write the new t_cold value */
-		reg_val = thresh_val & (~tsr->threshold_tcold_mask);
-		reg_val |= cold << __ffs(tsr->threshold_tcold_mask);
-		omap_bandgap_writel(bg_ptr, reg_val, tsr->bgap_threshold);
-		thresh_val = reg_val;
-	}
-
-	/* write the new t_hot value */
-	reg_val = thresh_val & ~tsr->threshold_thot_mask;
-	reg_val |= (t_hot << __ffs(tsr->threshold_thot_mask));
-	omap_bandgap_writel(bg_ptr, reg_val, tsr->bgap_threshold);
-	if (err) {
-		dev_err(bg_ptr->dev, "failed to reprogram thot threshold\n");
-		return -EIO;
-	}
-
-	return temp_sensor_unmask_interrupts(bg_ptr, id, t_hot, cold);
-}
-
-/* Talert Thot and Tcold thresholds. Call it only if HAS(TALERT) is set */
-static
-int temp_sensor_init_talert_thresholds(struct omap_bandgap *bg_ptr, int id,
-				       int t_hot, int t_cold)
-{
-	struct temp_sensor_registers *tsr;
-	u32 reg_val, thresh_val;
-
-	tsr = bg_ptr->conf->sensors[id].registers;
-	thresh_val = omap_bandgap_readl(bg_ptr, tsr->bgap_threshold);
-
-	/* write the new t_cold value */
-	reg_val = thresh_val & ~tsr->threshold_tcold_mask;
-	reg_val |= (t_cold << __ffs(tsr->threshold_tcold_mask));
-	omap_bandgap_writel(bg_ptr, reg_val, tsr->bgap_threshold);
-
-	thresh_val = omap_bandgap_readl(bg_ptr, tsr->bgap_threshold);
-
-	/* write the new t_hot value */
-	reg_val = thresh_val & ~tsr->threshold_thot_mask;
-	reg_val |= (t_hot << __ffs(tsr->threshold_thot_mask));
-	omap_bandgap_writel(bg_ptr, reg_val, tsr->bgap_threshold);
-
-	reg_val = omap_bandgap_readl(bg_ptr, tsr->bgap_mask_ctrl);
-	reg_val |= tsr->mask_hot_mask;
-	reg_val |= tsr->mask_cold_mask;
-	omap_bandgap_writel(bg_ptr, reg_val, tsr->bgap_mask_ctrl);
-
-	return 0;
-}
-
-/* Talert Tcold threshold. Call it only if HAS(TALERT) is set */
-static
-int temp_sensor_configure_tcold(struct omap_bandgap *bg_ptr, int id,
-				int t_cold)
-{
-	struct temp_sensor_data *ts_data = bg_ptr->conf->sensors[id].ts_data;
-	struct temp_sensor_registers *tsr;
-	u32 thresh_val, reg_val;
-	int hot, err = 0;
-
-	tsr = bg_ptr->conf->sensors[id].registers;
-	/* obtain the T cold value */
-	thresh_val = omap_bandgap_readl(bg_ptr, tsr->bgap_threshold);
-	hot = (thresh_val & tsr->threshold_thot_mask) >>
-	    __ffs(tsr->threshold_thot_mask);
-
-	if (t_cold >= hot) {
-		/* change the t_hot to t_cold + 5000 millidegrees */
-		err |= add_hyst(t_cold, ts_data->hyst_val, bg_ptr, id, &hot);
-		/* write the new t_hot value */
-		reg_val = thresh_val & (~tsr->threshold_thot_mask);
-		reg_val |= hot << __ffs(tsr->threshold_thot_mask);
-		omap_bandgap_writel(bg_ptr, reg_val, tsr->bgap_threshold);
-		thresh_val = reg_val;
-	}
-
-	/* write the new t_cold value */
-	reg_val = thresh_val & ~tsr->threshold_tcold_mask;
-	reg_val |= (t_cold << __ffs(tsr->threshold_tcold_mask));
-	omap_bandgap_writel(bg_ptr, reg_val, tsr->bgap_threshold);
-	if (err) {
-		dev_err(bg_ptr->dev, "failed to reprogram tcold threshold\n");
-		return -EIO;
-	}
-
-	return temp_sensor_unmask_interrupts(bg_ptr, id, hot, t_cold);
-}
-
-/* This is Tshut Thot config. Call it only if HAS(TSHUT_CONFIG) is set */
-static int temp_sensor_configure_tshut_hot(struct omap_bandgap *bg_ptr,
-					   int id, int tshut_hot)
-{
-	struct temp_sensor_registers *tsr;
-	u32 reg_val;
-
-	tsr = bg_ptr->conf->sensors[id].registers;
-	reg_val = omap_bandgap_readl(bg_ptr, tsr->tshut_threshold);
-	reg_val &= ~tsr->tshut_hot_mask;
-	reg_val |= tshut_hot << __ffs(tsr->tshut_hot_mask);
-	omap_bandgap_writel(bg_ptr, reg_val, tsr->tshut_threshold);
-
-	return 0;
-}
-
-/* This is Tshut Tcold config. Call it only if HAS(TSHUT_CONFIG) is set */
-static int temp_sensor_configure_tshut_cold(struct omap_bandgap *bg_ptr,
-					    int id, int tshut_cold)
-{
-	struct temp_sensor_registers *tsr;
-	u32 reg_val;
-
-	tsr = bg_ptr->conf->sensors[id].registers;
-	reg_val = omap_bandgap_readl(bg_ptr, tsr->tshut_threshold);
-	reg_val &= ~tsr->tshut_cold_mask;
-	reg_val |= tshut_cold << __ffs(tsr->tshut_cold_mask);
-	omap_bandgap_writel(bg_ptr, reg_val, tsr->tshut_threshold);
-
-	return 0;
-}
-
-/* This is counter config. Call it only if HAS(COUNTER) is set */
-static int configure_temp_sensor_counter(struct omap_bandgap *bg_ptr, int id,
-					 u32 counter)
-{
-	struct temp_sensor_registers *tsr;
-	u32 val;
-
-	tsr = bg_ptr->conf->sensors[id].registers;
-	val = omap_bandgap_readl(bg_ptr, tsr->bgap_counter);
-	val &= ~tsr->counter_mask;
-	val |= counter << __ffs(tsr->counter_mask);
-	omap_bandgap_writel(bg_ptr, val, tsr->bgap_counter);
-
-	return 0;
-}
-
-#define bandgap_is_valid(b)						\
-			(!IS_ERR_OR_NULL(b))
-#define bandgap_is_valid_sensor_id(b, i)				\
-			((i) >= 0 && (i) < (b)->conf->sensor_count)
-static inline int omap_bandgap_validate(struct omap_bandgap *bg_ptr, int id)
-{
-	if (!bandgap_is_valid(bg_ptr)) {
-		pr_err("%s: invalid bandgap pointer\n", __func__);
-		return -EINVAL;
-	}
-
-	if (!bandgap_is_valid_sensor_id(bg_ptr, id)) {
-		dev_err(bg_ptr->dev, "%s: sensor id out of range (%d)\n",
-			__func__, id);
-		return -ERANGE;
-	}
-
-	return 0;
-}
-
-/* Exposed APIs */
-/**
- * omap_bandgap_read_thot() - reads sensor current thot
- * @bg_ptr - pointer to bandgap instance
- * @id - sensor id
- * @thot - resulting current thot value
- *
- * returns 0 on success or the proper error code
- */
-int omap_bandgap_read_thot(struct omap_bandgap *bg_ptr, int id,
-			      int *thot)
-{
-	struct temp_sensor_registers *tsr;
-	u32 temp;
-	int ret;
-
-	ret = omap_bandgap_validate(bg_ptr, id);
-	if (ret)
-		return ret;
-
-	if (!OMAP_BANDGAP_HAS(bg_ptr, TALERT))
-		return -ENOTSUPP;
-
-	tsr = bg_ptr->conf->sensors[id].registers;
-	temp = omap_bandgap_readl(bg_ptr, tsr->bgap_threshold);
-	temp = (temp & tsr->threshold_thot_mask) >>
-		__ffs(tsr->threshold_thot_mask);
-	ret |= adc_to_temp_conversion(bg_ptr, id, temp, &temp);
-	if (ret) {
-		dev_err(bg_ptr->dev, "failed to read thot\n");
-		return -EIO;
-	}
-
-	*thot = temp;
-
-	return 0;
-}
-
-/**
- * omap_bandgap_write_thot() - sets sensor current thot
- * @bg_ptr - pointer to bandgap instance
- * @id - sensor id
- * @val - desired thot value
- *
- * returns 0 on success or the proper error code
- */
-int omap_bandgap_write_thot(struct omap_bandgap *bg_ptr, int id, int val)
-{
-	struct temp_sensor_data *ts_data;
-	struct temp_sensor_registers *tsr;
-	u32 t_hot;
-	int ret;
-
-	ret = omap_bandgap_validate(bg_ptr, id);
-	if (ret)
-		return ret;
-
-	if (!OMAP_BANDGAP_HAS(bg_ptr, TALERT))
-		return -ENOTSUPP;
-
-	ts_data = bg_ptr->conf->sensors[id].ts_data;
-	tsr = bg_ptr->conf->sensors[id].registers;
-
-	if (val < ts_data->min_temp + ts_data->hyst_val)
-		return -EINVAL;
-	ret = temp_to_adc_conversion(val, bg_ptr, id, &t_hot);
-	if (ret < 0)
-		return ret;
-
-	mutex_lock(&bg_ptr->bg_mutex);
-	temp_sensor_configure_thot(bg_ptr, id, t_hot);
-	mutex_unlock(&bg_ptr->bg_mutex);
-
-	return 0;
-}
-
-/**
- * omap_bandgap_read_tcold() - reads sensor current tcold
- * @bg_ptr - pointer to bandgap instance
- * @id - sensor id
- * @tcold - resulting current tcold value
- *
- * returns 0 on success or the proper error code
- */
-int omap_bandgap_read_tcold(struct omap_bandgap *bg_ptr, int id,
-			       int *tcold)
-{
-	struct temp_sensor_registers *tsr;
-	u32 temp;
-	int ret;
-
-	ret = omap_bandgap_validate(bg_ptr, id);
-	if (ret)
-		return ret;
-
-	if (!OMAP_BANDGAP_HAS(bg_ptr, TALERT))
-		return -ENOTSUPP;
-
-	tsr = bg_ptr->conf->sensors[id].registers;
-	temp = omap_bandgap_readl(bg_ptr, tsr->bgap_threshold);
-	temp = (temp & tsr->threshold_tcold_mask)
-	    >> __ffs(tsr->threshold_tcold_mask);
-	ret |= adc_to_temp_conversion(bg_ptr, id, temp, &temp);
-	if (ret)
-		return -EIO;
-
-	*tcold = temp;
-
-	return 0;
-}
-
-/**
- * omap_bandgap_write_tcold() - sets the sensor tcold
- * @bg_ptr - pointer to bandgap instance
- * @id - sensor id
- * @val - desired tcold value
- *
- * returns 0 on success or the proper error code
- */
-int omap_bandgap_write_tcold(struct omap_bandgap *bg_ptr, int id, int val)
-{
-	struct temp_sensor_data *ts_data;
-	struct temp_sensor_registers *tsr;
-	u32 t_cold;
-	int ret;
-
-	ret = omap_bandgap_validate(bg_ptr, id);
-	if (ret)
-		return ret;
-
-	if (!OMAP_BANDGAP_HAS(bg_ptr, TALERT))
-		return -ENOTSUPP;
-
-	ts_data = bg_ptr->conf->sensors[id].ts_data;
-	tsr = bg_ptr->conf->sensors[id].registers;
-	if (val > ts_data->max_temp + ts_data->hyst_val)
-		return -EINVAL;
-
-	ret = temp_to_adc_conversion(val, bg_ptr, id, &t_cold);
-	if (ret < 0)
-		return ret;
-
-	mutex_lock(&bg_ptr->bg_mutex);
-	temp_sensor_configure_tcold(bg_ptr, id, t_cold);
-	mutex_unlock(&bg_ptr->bg_mutex);
-
-	return 0;
-}
-
-/**
- * omap_bandgap_read_update_interval() - read the sensor update interval
- * @bg_ptr - pointer to bandgap instance
- * @id - sensor id
- * @interval - resulting update interval in miliseconds
- *
- * returns 0 on success or the proper error code
- */
-int omap_bandgap_read_update_interval(struct omap_bandgap *bg_ptr, int id,
-					 int *interval)
-{
-	struct temp_sensor_registers *tsr;
-	u32 time;
-	int ret;
-
-	ret = omap_bandgap_validate(bg_ptr, id);
-	if (ret)
-		return ret;
-
-	if (!OMAP_BANDGAP_HAS(bg_ptr, COUNTER))
-		return -ENOTSUPP;
-
-	tsr = bg_ptr->conf->sensors[id].registers;
-	time = omap_bandgap_readl(bg_ptr, tsr->bgap_counter);
-	time = (time & tsr->counter_mask) >> __ffs(tsr->counter_mask);
-	time = time * 1000 / bg_ptr->clk_rate;
-
-	*interval = time;
-
-	return 0;
-}
-
-/**
- * omap_bandgap_write_update_interval() - set the update interval
- * @bg_ptr - pointer to bandgap instance
- * @id - sensor id
- * @interval - desired update interval in miliseconds
- *
- * returns 0 on success or the proper error code
- */
-int omap_bandgap_write_update_interval(struct omap_bandgap *bg_ptr,
-					  int id, u32 interval)
-{
-	int ret = omap_bandgap_validate(bg_ptr, id);
-	if (ret)
-		return ret;
-
-	if (!OMAP_BANDGAP_HAS(bg_ptr, COUNTER))
-		return -ENOTSUPP;
-
-	interval = interval * bg_ptr->clk_rate / 1000;
-	mutex_lock(&bg_ptr->bg_mutex);
-	configure_temp_sensor_counter(bg_ptr, id, interval);
-	mutex_unlock(&bg_ptr->bg_mutex);
-
-	return 0;
-}
-
-/**
- * omap_bandgap_read_temperature() - report current temperature
- * @bg_ptr - pointer to bandgap instance
- * @id - sensor id
- * @temperature - resulting temperature
- *
- * returns 0 on success or the proper error code
- */
-int omap_bandgap_read_temperature(struct omap_bandgap *bg_ptr, int id,
-				     int *temperature)
-{
-	struct temp_sensor_registers *tsr;
-	u32 temp;
-	int ret;
-
-	ret = omap_bandgap_validate(bg_ptr, id);
-	if (ret)
-		return ret;
-
-	tsr = bg_ptr->conf->sensors[id].registers;
-	temp = omap_bandgap_readl(bg_ptr, tsr->temp_sensor_ctrl);
-	temp &= tsr->bgap_dtemp_mask;
-
-	ret |= adc_to_temp_conversion(bg_ptr, id, temp, &temp);
-	if (ret)
-		return -EIO;
-
-	*temperature = temp;
-
-	return 0;
-}
-
-/**
- * omap_bandgap_set_sensor_data() - helper function to store thermal
- * framework related data.
- * @bg_ptr - pointer to bandgap instance
- * @id - sensor id
- * @data - thermal framework related data to be stored
- *
- * returns 0 on success or the proper error code
- */
-int omap_bandgap_set_sensor_data(struct omap_bandgap *bg_ptr, int id,
-				void *data)
-{
-	int ret = omap_bandgap_validate(bg_ptr, id);
-	if (ret)
-		return ret;
-
-	bg_ptr->conf->sensors[id].data = data;
-
-	return 0;
-}
-
-/**
- * omap_bandgap_get_sensor_data() - helper function to get thermal
- * framework related data.
- * @bg_ptr - pointer to bandgap instance
- * @id - sensor id
- *
- * returns data stored by set function with sensor id on success or NULL
- */
-void *omap_bandgap_get_sensor_data(struct omap_bandgap *bg_ptr, int id)
-{
-	int ret = omap_bandgap_validate(bg_ptr, id);
-	if (ret)
-		return ERR_PTR(ret);
-
-	return bg_ptr->conf->sensors[id].data;
-}
-
-static int
-omap_bandgap_force_single_read(struct omap_bandgap *bg_ptr, int id)
-{
-	struct temp_sensor_registers *tsr;
-	u32 temp = 0, counter = 1000;
-
-	tsr = bg_ptr->conf->sensors[id].registers;
-	/* Select single conversion mode */
-	if (OMAP_BANDGAP_HAS(bg_ptr, MODE_CONFIG)) {
-		temp = omap_bandgap_readl(bg_ptr, tsr->bgap_mode_ctrl);
-		temp &= ~(1 << __ffs(tsr->mode_ctrl_mask));
-		omap_bandgap_writel(bg_ptr, temp, tsr->bgap_mode_ctrl);
-	}
-
-	/* Start of Conversion = 1 */
-	temp = omap_bandgap_readl(bg_ptr, tsr->temp_sensor_ctrl);
-	temp |= 1 << __ffs(tsr->bgap_soc_mask);
-	omap_bandgap_writel(bg_ptr, temp, tsr->temp_sensor_ctrl);
-	/* Wait until DTEMP is updated */
-	temp = omap_bandgap_readl(bg_ptr, tsr->temp_sensor_ctrl);
-	temp &= (tsr->bgap_dtemp_mask);
-	while ((temp == 0) && --counter) {
-		temp = omap_bandgap_readl(bg_ptr, tsr->temp_sensor_ctrl);
-		temp &= (tsr->bgap_dtemp_mask);
-	}
-	/* Start of Conversion = 0 */
-	temp = omap_bandgap_readl(bg_ptr, tsr->temp_sensor_ctrl);
-	temp &= ~(1 << __ffs(tsr->bgap_soc_mask));
-	omap_bandgap_writel(bg_ptr, temp, tsr->temp_sensor_ctrl);
-
-	return 0;
-}
-
-/**
- * enable_continuous_mode() - One time enabling of continuous conversion mode
- * @bg_ptr - pointer to scm instance
- *
- * Call this function only if HAS(MODE_CONFIG) is set
- */
-static int enable_continuous_mode(struct omap_bandgap *bg_ptr)
-{
-	struct temp_sensor_registers *tsr;
-	int i;
-	u32 val;
-
-	for (i = 0; i < bg_ptr->conf->sensor_count; i++) {
-		/* Perform a single read just before enabling continuous */
-		omap_bandgap_force_single_read(bg_ptr, i);
-		tsr = bg_ptr->conf->sensors[i].registers;
-		val = omap_bandgap_readl(bg_ptr, tsr->bgap_mode_ctrl);
-		val |= 1 << __ffs(tsr->mode_ctrl_mask);
-		omap_bandgap_writel(bg_ptr, val, tsr->bgap_mode_ctrl);
-	}
-
-	return 0;
-}
-
-static int omap_bandgap_tshut_init(struct omap_bandgap *bg_ptr,
-				      struct platform_device *pdev)
-{
-	int gpio_nr = bg_ptr->tshut_gpio;
-	int status;
-
-	/* Request for gpio_86 line */
-	status = gpio_request(gpio_nr, "tshut");
-	if (status < 0) {
-		dev_err(bg_ptr->dev,
-			"Could not request for TSHUT GPIO:%i\n", 86);
-		return status;
-	}
-	status = gpio_direction_input(gpio_nr);
-	if (status) {
-		dev_err(bg_ptr->dev,
-			"Cannot set input TSHUT GPIO %d\n", gpio_nr);
-		return status;
-	}
-
-	status = request_irq(gpio_to_irq(gpio_nr),
-			     omap_bandgap_tshut_irq_handler,
-			     IRQF_TRIGGER_RISING, "tshut",
-			     NULL);
-	if (status) {
-		gpio_free(gpio_nr);
-		dev_err(bg_ptr->dev, "request irq failed for TSHUT");
-	}
-
-	return 0;
-}
-
-/* Initialization of Talert. Call it only if HAS(TALERT) is set */
-static int omap_bandgap_talert_init(struct omap_bandgap *bg_ptr,
-				       struct platform_device *pdev)
-{
-	int ret;
-
-	bg_ptr->irq = platform_get_irq(pdev, 0);
-	if (bg_ptr->irq < 0) {
-		dev_err(&pdev->dev, "get_irq failed\n");
-		return bg_ptr->irq;
-	}
-	ret = request_threaded_irq(bg_ptr->irq, NULL,
-				   talert_irq_handler,
-				   IRQF_TRIGGER_HIGH | IRQF_ONESHOT,
-				   "talert", bg_ptr);
-	if (ret) {
-		dev_err(&pdev->dev, "Request threaded irq failed.\n");
-		return ret;
-	}
-
-	return 0;
-}
-
-static const struct of_device_id of_omap_bandgap_match[];
-static struct omap_bandgap *omap_bandgap_build(struct platform_device *pdev)
-{
-	struct device_node *node = pdev->dev.of_node;
-	const struct of_device_id *of_id;
-	struct omap_bandgap *bg_ptr;
-	struct resource *res;
-	u32 prop;
-	int i;
-
-	/* just for the sake */
-	if (!node) {
-		dev_err(&pdev->dev, "no platform information available\n");
-		return ERR_PTR(-EINVAL);
-	}
-
-	bg_ptr = devm_kzalloc(&pdev->dev, sizeof(struct omap_bandgap),
-				    GFP_KERNEL);
-	if (!bg_ptr) {
-		dev_err(&pdev->dev, "Unable to allocate mem for driver ref\n");
-		return ERR_PTR(-ENOMEM);
-	}
-
-	of_id = of_match_device(of_omap_bandgap_match, &pdev->dev);
-	if (of_id)
-		bg_ptr->conf = of_id->data;
-
-	i = 0;
-	do {
-		void __iomem *chunk;
-
-		res = platform_get_resource(pdev, IORESOURCE_MEM, i);
-		if (!res)
-			break;
-		chunk = devm_ioremap_resource(&pdev->dev, res);
-		if (i == 0)
-			bg_ptr->base = chunk;
-		if (IS_ERR(chunk))
-			return ERR_CAST(chunk);
-		
-		i++;
-	} while (res);
-
-	if (OMAP_BANDGAP_HAS(bg_ptr, TSHUT)) {
-		if (of_property_read_u32(node, "ti,tshut-gpio", &prop) < 0) {
-			dev_err(&pdev->dev, "missing tshut gpio in device tree\n");
-			return ERR_PTR(-EINVAL);
-		}
-		bg_ptr->tshut_gpio = prop;
-		if (!gpio_is_valid(bg_ptr->tshut_gpio)) {
-			dev_err(&pdev->dev, "invalid gpio for tshut (%d)\n",
-				bg_ptr->tshut_gpio);
-			return ERR_PTR(-EINVAL);
-		}
-	}
-
-	return bg_ptr;
-}
-
-static
-int omap_bandgap_probe(struct platform_device *pdev)
-{
-	struct omap_bandgap *bg_ptr;
-	int clk_rate, ret = 0, i;
-
-	bg_ptr = omap_bandgap_build(pdev);
-	if (IS_ERR_OR_NULL(bg_ptr)) {
-		dev_err(&pdev->dev, "failed to fetch platform data\n");
-		return PTR_ERR(bg_ptr);
-	}
-	bg_ptr->dev = &pdev->dev;
-
-	if (OMAP_BANDGAP_HAS(bg_ptr, TSHUT)) {
-		ret = omap_bandgap_tshut_init(bg_ptr, pdev);
-		if (ret) {
-			dev_err(&pdev->dev,
-				"failed to initialize system tshut IRQ\n");
-			return ret;
-		}
-	}
-
-	bg_ptr->fclock = clk_get(NULL, bg_ptr->conf->fclock_name);
-	ret = IS_ERR_OR_NULL(bg_ptr->fclock);
-	if (ret) {
-		dev_err(&pdev->dev, "failed to request fclock reference\n");
-		goto free_irqs;
-	}
-
-	bg_ptr->div_clk = clk_get(NULL,  bg_ptr->conf->div_ck_name);
-	ret = IS_ERR_OR_NULL(bg_ptr->div_clk);
-	if (ret) {
-		dev_err(&pdev->dev,
-			"failed to request div_ts_ck clock ref\n");
-		goto free_irqs;
-	}
-
-	bg_ptr->conv_table = bg_ptr->conf->conv_table;
-	for (i = 0; i < bg_ptr->conf->sensor_count; i++) {
-		struct temp_sensor_registers *tsr;
-		u32 val;
-
-		tsr = bg_ptr->conf->sensors[i].registers;
-		/*
-		 * check if the efuse has a non-zero value if not
-		 * it is an untrimmed sample and the temperatures
-		 * may not be accurate
-		 */
-		val = omap_bandgap_readl(bg_ptr, tsr->bgap_efuse);
-		if (ret || !val)
-			dev_info(&pdev->dev,
-				 "Non-trimmed BGAP, Temp not accurate\n");
-	}
-
-	clk_rate = clk_round_rate(bg_ptr->div_clk,
-				  bg_ptr->conf->sensors[0].ts_data->max_freq);
-	if (clk_rate < bg_ptr->conf->sensors[0].ts_data->min_freq ||
-	    clk_rate == 0xffffffff) {
-		ret = -ENODEV;
-		dev_err(&pdev->dev, "wrong clock rate (%d)\n", clk_rate);
-		goto put_clks;
-	}
-
-	ret = clk_set_rate(bg_ptr->div_clk, clk_rate);
-	if (ret)
-		dev_err(&pdev->dev, "Cannot re-set clock rate. Continuing\n");
-
-	bg_ptr->clk_rate = clk_rate;
-	clk_enable(bg_ptr->fclock);
-
-	mutex_init(&bg_ptr->bg_mutex);
-	bg_ptr->dev = &pdev->dev;
-	platform_set_drvdata(pdev, bg_ptr);
-
-	omap_bandgap_power(bg_ptr, true);
-
-	/* Set default counter to 1 for now */
-	if (OMAP_BANDGAP_HAS(bg_ptr, COUNTER))
-		for (i = 0; i < bg_ptr->conf->sensor_count; i++)
-			configure_temp_sensor_counter(bg_ptr, i, 1);
-
-	for (i = 0; i < bg_ptr->conf->sensor_count; i++) {
-		struct temp_sensor_data *ts_data;
-
-		ts_data = bg_ptr->conf->sensors[i].ts_data;
-
-		if (OMAP_BANDGAP_HAS(bg_ptr, TALERT))
-			temp_sensor_init_talert_thresholds(bg_ptr, i,
-							   ts_data->t_hot,
-							   ts_data->t_cold);
-		if (OMAP_BANDGAP_HAS(bg_ptr, TSHUT_CONFIG)) {
-			temp_sensor_configure_tshut_hot(bg_ptr, i,
-							ts_data->tshut_hot);
-			temp_sensor_configure_tshut_cold(bg_ptr, i,
-							 ts_data->tshut_cold);
-		}
-	}
-
-	if (OMAP_BANDGAP_HAS(bg_ptr, MODE_CONFIG))
-		enable_continuous_mode(bg_ptr);
-
-	/* Set .250 seconds time as default counter */
-	if (OMAP_BANDGAP_HAS(bg_ptr, COUNTER))
-		for (i = 0; i < bg_ptr->conf->sensor_count; i++)
-			configure_temp_sensor_counter(bg_ptr, i,
-						      bg_ptr->clk_rate / 4);
-
-	/* Every thing is good? Then expose the sensors */
-	for (i = 0; i < bg_ptr->conf->sensor_count; i++) {
-		char *domain;
-
-		if (bg_ptr->conf->sensors[i].register_cooling)
-			bg_ptr->conf->sensors[i].register_cooling(bg_ptr, i);
-
-		domain = bg_ptr->conf->sensors[i].domain;
-		if (bg_ptr->conf->expose_sensor)
-			bg_ptr->conf->expose_sensor(bg_ptr, i, domain);
-	}
-
-	/*
-	 * Enable the Interrupts once everything is set. Otherwise irq handler
-	 * might be called as soon as it is enabled where as rest of framework
-	 * is still getting initialised.
-	 */
-	if (OMAP_BANDGAP_HAS(bg_ptr, TALERT)) {
-		ret = omap_bandgap_talert_init(bg_ptr, pdev);
-		if (ret) {
-			dev_err(&pdev->dev, "failed to initialize Talert IRQ\n");
-			i = bg_ptr->conf->sensor_count;
-			goto disable_clk;
-		}
-	}
-
-	return 0;
-
-disable_clk:
-	clk_disable(bg_ptr->fclock);
-put_clks:
-	clk_put(bg_ptr->fclock);
-	clk_put(bg_ptr->div_clk);
-free_irqs:
-	if (OMAP_BANDGAP_HAS(bg_ptr, TSHUT)) {
-		free_irq(gpio_to_irq(bg_ptr->tshut_gpio), NULL);
-		gpio_free(bg_ptr->tshut_gpio);
-	}
-
-	return ret;
-}
-
-static
-int omap_bandgap_remove(struct platform_device *pdev)
-{
-	struct omap_bandgap *bg_ptr = platform_get_drvdata(pdev);
-	int i;
-
-	/* First thing is to remove sensor interfaces */
-	for (i = 0; i < bg_ptr->conf->sensor_count; i++) {
-		if (bg_ptr->conf->sensors[i].register_cooling)
-			bg_ptr->conf->sensors[i].unregister_cooling(bg_ptr, i);
-
-		if (bg_ptr->conf->remove_sensor)
-			bg_ptr->conf->remove_sensor(bg_ptr, i);
-	}
-
-	omap_bandgap_power(bg_ptr, false);
-
-	clk_disable(bg_ptr->fclock);
-	clk_put(bg_ptr->fclock);
-	clk_put(bg_ptr->div_clk);
-
-	if (OMAP_BANDGAP_HAS(bg_ptr, TALERT))
-		free_irq(bg_ptr->irq, bg_ptr);
-
-	if (OMAP_BANDGAP_HAS(bg_ptr, TSHUT)) {
-		free_irq(gpio_to_irq(bg_ptr->tshut_gpio), NULL);
-		gpio_free(bg_ptr->tshut_gpio);
-	}
-
-	return 0;
-}
-
-#ifdef CONFIG_PM
-static int omap_bandgap_save_ctxt(struct omap_bandgap *bg_ptr)
-{
-	int i;
-
-	for (i = 0; i < bg_ptr->conf->sensor_count; i++) {
-		struct temp_sensor_registers *tsr;
-		struct temp_sensor_regval *rval;
-
-		rval = &bg_ptr->conf->sensors[i].regval;
-		tsr = bg_ptr->conf->sensors[i].registers;
-
-		if (OMAP_BANDGAP_HAS(bg_ptr, MODE_CONFIG))
-			rval->bg_mode_ctrl = omap_bandgap_readl(bg_ptr,
-							tsr->bgap_mode_ctrl);
-		if (OMAP_BANDGAP_HAS(bg_ptr, COUNTER))
-			rval->bg_counter = omap_bandgap_readl(bg_ptr,
-							tsr->bgap_counter);
-		if (OMAP_BANDGAP_HAS(bg_ptr, TALERT)) {
-			rval->bg_threshold = omap_bandgap_readl(bg_ptr,
-							tsr->bgap_threshold);
-			rval->bg_ctrl = omap_bandgap_readl(bg_ptr,
-						   tsr->bgap_mask_ctrl);
-		}
-
-		if (OMAP_BANDGAP_HAS(bg_ptr, TSHUT_CONFIG))
-			rval->tshut_threshold = omap_bandgap_readl(bg_ptr,
-						   tsr->tshut_threshold);
-	}
-
-	return 0;
-}
-
-static int omap_bandgap_restore_ctxt(struct omap_bandgap *bg_ptr)
-{
-	int i;
-
-	for (i = 0; i < bg_ptr->conf->sensor_count; i++) {
-		struct temp_sensor_registers *tsr;
-		struct temp_sensor_regval *rval;
-		u32 val = 0;
-
-		rval = &bg_ptr->conf->sensors[i].regval;
-		tsr = bg_ptr->conf->sensors[i].registers;
-
-		if (OMAP_BANDGAP_HAS(bg_ptr, COUNTER))
-			val = omap_bandgap_readl(bg_ptr, tsr->bgap_counter);
-
-		if (OMAP_BANDGAP_HAS(bg_ptr, TSHUT_CONFIG))
-			omap_bandgap_writel(bg_ptr,
-				rval->tshut_threshold,
-					   tsr->tshut_threshold);
-		/* Force immediate temperature measurement and update
-		 * of the DTEMP field
-		 */
-		omap_bandgap_force_single_read(bg_ptr, i);
-
-		if (OMAP_BANDGAP_HAS(bg_ptr, COUNTER))
-			omap_bandgap_writel(bg_ptr, rval->bg_counter,
-						   tsr->bgap_counter);
-		if (OMAP_BANDGAP_HAS(bg_ptr, MODE_CONFIG))
-			omap_bandgap_writel(bg_ptr, rval->bg_mode_ctrl,
-						   tsr->bgap_mode_ctrl);
-		if (OMAP_BANDGAP_HAS(bg_ptr, TALERT)) {
-			omap_bandgap_writel(bg_ptr,
-						   rval->bg_threshold,
-						   tsr->bgap_threshold);
-			omap_bandgap_writel(bg_ptr, rval->bg_ctrl,
-						   tsr->bgap_mask_ctrl);
-		}
-	}
-
-	return 0;
-}
-
-static int omap_bandgap_suspend(struct device *dev)
-{
-	struct omap_bandgap *bg_ptr = dev_get_drvdata(dev);
-	int err;
-
-	err = omap_bandgap_save_ctxt(bg_ptr);
-	omap_bandgap_power(bg_ptr, false);
-	clk_disable(bg_ptr->fclock);
-
-	return err;
-}
-
-static int omap_bandgap_resume(struct device *dev)
-{
-	struct omap_bandgap *bg_ptr = dev_get_drvdata(dev);
-
-	clk_enable(bg_ptr->fclock);
-	omap_bandgap_power(bg_ptr, true);
-
-	return omap_bandgap_restore_ctxt(bg_ptr);
-}
-static const struct dev_pm_ops omap_bandgap_dev_pm_ops = {
-	SET_SYSTEM_SLEEP_PM_OPS(omap_bandgap_suspend,
-				omap_bandgap_resume)
-};
-
-#define DEV_PM_OPS	(&omap_bandgap_dev_pm_ops)
-#else
-#define DEV_PM_OPS	NULL
-#endif
-
-static const struct of_device_id of_omap_bandgap_match[] = {
-#ifdef CONFIG_OMAP4_THERMAL
-	{
-		.compatible = "ti,omap4430-bandgap",
-		.data = (void *)&omap4430_data,
-	},
-	{
-		.compatible = "ti,omap4460-bandgap",
-		.data = (void *)&omap4460_data,
-	},
-	{
-		.compatible = "ti,omap4470-bandgap",
-		.data = (void *)&omap4470_data,
-	},
-#endif
-#ifdef CONFIG_OMAP5_THERMAL
-	{
-		.compatible = "ti,omap5430-bandgap",
-		.data = (void *)&omap5430_data,
-	},
-#endif
-	/* Sentinel */
-	{ },
-};
-MODULE_DEVICE_TABLE(of, of_omap_bandgap_match);
-
-static struct platform_driver omap_bandgap_sensor_driver = {
-	.probe = omap_bandgap_probe,
-	.remove = omap_bandgap_remove,
-	.driver = {
-			.name = "omap-bandgap",
-			.pm = DEV_PM_OPS,
-			.of_match_table	= of_omap_bandgap_match,
-	},
-};
-
-module_platform_driver(omap_bandgap_sensor_driver);
-
-MODULE_DESCRIPTION("OMAP4+ bandgap temperature sensor driver");
-MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("platform:omap-bandgap");
-MODULE_AUTHOR("Texas Instrument Inc.");
diff --git a/drivers/staging/omap-thermal/omap-bandgap.h b/drivers/staging/omap-thermal/omap-bandgap.h
deleted file mode 100644
index 2bb14bd7c6d..00000000000
--- a/drivers/staging/omap-thermal/omap-bandgap.h
+++ /dev/null
@@ -1,432 +0,0 @@
-/*
- * OMAP4 Bandgap temperature sensor driver
- *
- * Copyright (C) 2011 Texas Instruments Incorporated - http://www.ti.com/
- * Contact:
- *   Eduardo Valentin <eduardo.valentin@ti.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 __OMAP_BANDGAP_H
-#define __OMAP_BANDGAP_H
-
-#include <linux/mutex.h>
-#include <linux/types.h>
-#include <linux/err.h>
-
-/* TEMP_SENSOR OMAP4430 */
-#define OMAP4430_BGAP_TSHUT_SHIFT			11
-#define OMAP4430_BGAP_TSHUT_MASK			(1 << 11)
-
-/* TEMP_SENSOR OMAP4430 */
-#define OMAP4430_BGAP_TEMPSOFF_SHIFT			12
-#define OMAP4430_BGAP_TEMPSOFF_MASK			(1 << 12)
-#define OMAP4430_SINGLE_MODE_SHIFT			10
-#define OMAP4430_SINGLE_MODE_MASK			(1 << 10)
-#define OMAP4430_BGAP_TEMP_SENSOR_SOC_SHIFT		9
-#define OMAP4430_BGAP_TEMP_SENSOR_SOC_MASK		(1 << 9)
-#define OMAP4430_BGAP_TEMP_SENSOR_EOCZ_SHIFT		8
-#define OMAP4430_BGAP_TEMP_SENSOR_EOCZ_MASK		(1 << 8)
-#define OMAP4430_BGAP_TEMP_SENSOR_DTEMP_SHIFT		0
-#define OMAP4430_BGAP_TEMP_SENSOR_DTEMP_MASK		(0xff << 0)
-
-#define OMAP4430_ADC_START_VALUE			0
-#define OMAP4430_ADC_END_VALUE				127
-#define OMAP4430_MAX_FREQ				32768
-#define OMAP4430_MIN_FREQ				32768
-#define OMAP4430_MIN_TEMP				-40000
-#define OMAP4430_MAX_TEMP				125000
-#define OMAP4430_HYST_VAL				5000
-
-/* TEMP_SENSOR OMAP4460 */
-#define OMAP4460_BGAP_TEMPSOFF_SHIFT			13
-#define OMAP4460_BGAP_TEMPSOFF_MASK			(1 << 13)
-#define OMAP4460_BGAP_TEMP_SENSOR_SOC_SHIFT		11
-#define OMAP4460_BGAP_TEMP_SENSOR_SOC_MASK		(1 << 11)
-#define OMAP4460_BGAP_TEMP_SENSOR_EOCZ_SHIFT		10
-#define OMAP4460_BGAP_TEMP_SENSOR_EOCZ_MASK		(1 << 10)
-#define OMAP4460_BGAP_TEMP_SENSOR_DTEMP_SHIFT		0
-#define OMAP4460_BGAP_TEMP_SENSOR_DTEMP_MASK		(0x3ff << 0)
-
-/* BANDGAP_CTRL */
-#define OMAP4460_SINGLE_MODE_SHIFT			31
-#define OMAP4460_SINGLE_MODE_MASK			(1 << 31)
-#define OMAP4460_MASK_HOT_SHIFT				1
-#define OMAP4460_MASK_HOT_MASK				(1 << 1)
-#define OMAP4460_MASK_COLD_SHIFT			0
-#define OMAP4460_MASK_COLD_MASK				(1 << 0)
-
-/* BANDGAP_COUNTER */
-#define OMAP4460_COUNTER_SHIFT				0
-#define OMAP4460_COUNTER_MASK				(0xffffff << 0)
-
-/* BANDGAP_THRESHOLD */
-#define OMAP4460_T_HOT_SHIFT				16
-#define OMAP4460_T_HOT_MASK				(0x3ff << 16)
-#define OMAP4460_T_COLD_SHIFT				0
-#define OMAP4460_T_COLD_MASK				(0x3ff << 0)
-
-/* TSHUT_THRESHOLD */
-#define OMAP4460_TSHUT_HOT_SHIFT			16
-#define OMAP4460_TSHUT_HOT_MASK				(0x3ff << 16)
-#define OMAP4460_TSHUT_COLD_SHIFT			0
-#define OMAP4460_TSHUT_COLD_MASK			(0x3ff << 0)
-
-/* BANDGAP_STATUS */
-#define OMAP4460_CLEAN_STOP_SHIFT			3
-#define OMAP4460_CLEAN_STOP_MASK			(1 << 3)
-#define OMAP4460_BGAP_ALERT_SHIFT			2
-#define OMAP4460_BGAP_ALERT_MASK			(1 << 2)
-#define OMAP4460_HOT_FLAG_SHIFT				1
-#define OMAP4460_HOT_FLAG_MASK				(1 << 1)
-#define OMAP4460_COLD_FLAG_SHIFT			0
-#define OMAP4460_COLD_FLAG_MASK				(1 << 0)
-
-/* TEMP_SENSOR OMAP5430 */
-#define OMAP5430_BGAP_TEMP_SENSOR_SOC_SHIFT		12
-#define OMAP5430_BGAP_TEMP_SENSOR_SOC_MASK		(1 << 12)
-#define OMAP5430_BGAP_TEMPSOFF_SHIFT			11
-#define OMAP5430_BGAP_TEMPSOFF_MASK			(1 << 11)
-#define OMAP5430_BGAP_TEMP_SENSOR_EOCZ_SHIFT		10
-#define OMAP5430_BGAP_TEMP_SENSOR_EOCZ_MASK		(1 << 10)
-#define OMAP5430_BGAP_TEMP_SENSOR_DTEMP_SHIFT		0
-#define OMAP5430_BGAP_TEMP_SENSOR_DTEMP_MASK		(0x3ff << 0)
-
-/* BANDGAP_CTRL */
-#define OMAP5430_MASK_HOT_CORE_SHIFT			5
-#define OMAP5430_MASK_HOT_CORE_MASK			(1 << 5)
-#define OMAP5430_MASK_COLD_CORE_SHIFT			4
-#define OMAP5430_MASK_COLD_CORE_MASK			(1 << 4)
-#define OMAP5430_MASK_HOT_MM_SHIFT			3
-#define OMAP5430_MASK_HOT_MM_MASK			(1 << 3)
-#define OMAP5430_MASK_COLD_MM_SHIFT			2
-#define OMAP5430_MASK_COLD_MM_MASK			(1 << 2)
-#define OMAP5430_MASK_HOT_MPU_SHIFT			1
-#define OMAP5430_MASK_HOT_MPU_MASK			(1 << 1)
-#define OMAP5430_MASK_COLD_MPU_SHIFT			0
-#define OMAP5430_MASK_COLD_MPU_MASK			(1 << 0)
-
-/* BANDGAP_COUNTER */
-#define OMAP5430_REPEAT_MODE_SHIFT			31
-#define OMAP5430_REPEAT_MODE_MASK			(1 << 31)
-#define OMAP5430_COUNTER_SHIFT				0
-#define OMAP5430_COUNTER_MASK				(0xffffff << 0)
-
-/* BANDGAP_THRESHOLD */
-#define OMAP5430_T_HOT_SHIFT				16
-#define OMAP5430_T_HOT_MASK				(0x3ff << 16)
-#define OMAP5430_T_COLD_SHIFT				0
-#define OMAP5430_T_COLD_MASK				(0x3ff << 0)
-
-/* TSHUT_THRESHOLD */
-#define OMAP5430_TSHUT_HOT_SHIFT			16
-#define OMAP5430_TSHUT_HOT_MASK				(0x3ff << 16)
-#define OMAP5430_TSHUT_COLD_SHIFT			0
-#define OMAP5430_TSHUT_COLD_MASK			(0x3ff << 0)
-
-/* BANDGAP_STATUS */
-#define OMAP5430_BGAP_ALERT_SHIFT			31
-#define OMAP5430_BGAP_ALERT_MASK			(1 << 31)
-#define OMAP5430_HOT_CORE_FLAG_SHIFT			5
-#define OMAP5430_HOT_CORE_FLAG_MASK			(1 << 5)
-#define OMAP5430_COLD_CORE_FLAG_SHIFT			4
-#define OMAP5430_COLD_CORE_FLAG_MASK			(1 << 4)
-#define OMAP5430_HOT_MM_FLAG_SHIFT			3
-#define OMAP5430_HOT_MM_FLAG_MASK			(1 << 3)
-#define OMAP5430_COLD_MM_FLAG_SHIFT			2
-#define OMAP5430_COLD_MM_FLAG_MASK			(1 << 2)
-#define OMAP5430_HOT_MPU_FLAG_SHIFT			1
-#define OMAP5430_HOT_MPU_FLAG_MASK			(1 << 1)
-#define OMAP5430_COLD_MPU_FLAG_SHIFT			0
-#define OMAP5430_COLD_MPU_FLAG_MASK			(1 << 0)
-
-/* Offsets from the base of temperature sensor registers */
-
-/* 4430 - All goes relative to OPP_BGAP */
-#define OMAP4430_FUSE_OPP_BGAP				0x0
-#define OMAP4430_TEMP_SENSOR_CTRL_OFFSET		0xCC
-
-/* 4460 - All goes relative to OPP_BGAP */
-#define OMAP4460_FUSE_OPP_BGAP				0x0
-#define OMAP4460_TEMP_SENSOR_CTRL_OFFSET		0xCC
-#define OMAP4460_BGAP_CTRL_OFFSET			0x118
-#define OMAP4460_BGAP_COUNTER_OFFSET			0x11C
-#define OMAP4460_BGAP_THRESHOLD_OFFSET			0x120
-#define OMAP4460_BGAP_TSHUT_OFFSET			0x124
-#define OMAP4460_BGAP_STATUS_OFFSET			0x128
-
-/* 5430 - All goes relative to OPP_BGAP_GPU */
-#define OMAP5430_FUSE_OPP_BGAP_GPU			0x0
-#define OMAP5430_TEMP_SENSOR_GPU_OFFSET			0x150
-#define OMAP5430_BGAP_COUNTER_GPU_OFFSET		0x1C0
-#define OMAP5430_BGAP_THRESHOLD_GPU_OFFSET		0x1A8
-#define OMAP5430_BGAP_TSHUT_GPU_OFFSET			0x1B4
-
-#define OMAP5430_FUSE_OPP_BGAP_MPU			0x4
-#define OMAP5430_TEMP_SENSOR_MPU_OFFSET			0x14C
-#define OMAP5430_BGAP_CTRL_OFFSET			0x1A0
-#define OMAP5430_BGAP_COUNTER_MPU_OFFSET		0x1BC
-#define OMAP5430_BGAP_THRESHOLD_MPU_OFFSET		0x1A4
-#define OMAP5430_BGAP_TSHUT_MPU_OFFSET			0x1B0
-#define OMAP5430_BGAP_STATUS_OFFSET			0x1C8
-
-#define OMAP5430_FUSE_OPP_BGAP_CORE			0x8
-#define OMAP5430_TEMP_SENSOR_CORE_OFFSET		0x154
-#define OMAP5430_BGAP_COUNTER_CORE_OFFSET		0x1C4
-#define OMAP5430_BGAP_THRESHOLD_CORE_OFFSET		0x1AC
-#define OMAP5430_BGAP_TSHUT_CORE_OFFSET			0x1B8
-
-#define OMAP4460_TSHUT_HOT				900	/* 122 deg C */
-#define OMAP4460_TSHUT_COLD				895	/* 100 deg C */
-#define OMAP4460_T_HOT					800	/* 73 deg C */
-#define OMAP4460_T_COLD					795	/* 71 deg C */
-#define OMAP4460_MAX_FREQ				1500000
-#define OMAP4460_MIN_FREQ				1000000
-#define OMAP4460_MIN_TEMP				-40000
-#define OMAP4460_MAX_TEMP				123000
-#define OMAP4460_HYST_VAL				5000
-#define OMAP4460_ADC_START_VALUE			530
-#define OMAP4460_ADC_END_VALUE				932
-
-#define OMAP5430_MPU_TSHUT_HOT				915
-#define OMAP5430_MPU_TSHUT_COLD				900
-#define OMAP5430_MPU_T_HOT				800
-#define OMAP5430_MPU_T_COLD				795
-#define OMAP5430_MPU_MAX_FREQ				1500000
-#define OMAP5430_MPU_MIN_FREQ				1000000
-#define OMAP5430_MPU_MIN_TEMP				-40000
-#define OMAP5430_MPU_MAX_TEMP				125000
-#define OMAP5430_MPU_HYST_VAL				5000
-#define OMAP5430_ADC_START_VALUE			532
-#define OMAP5430_ADC_END_VALUE				934
-
-
-#define OMAP5430_GPU_TSHUT_HOT				915
-#define OMAP5430_GPU_TSHUT_COLD				900
-#define OMAP5430_GPU_T_HOT				800
-#define OMAP5430_GPU_T_COLD				795
-#define OMAP5430_GPU_MAX_FREQ				1500000
-#define OMAP5430_GPU_MIN_FREQ				1000000
-#define OMAP5430_GPU_MIN_TEMP				-40000
-#define OMAP5430_GPU_MAX_TEMP				125000
-#define OMAP5430_GPU_HYST_VAL				5000
-
-#define OMAP5430_CORE_TSHUT_HOT				915
-#define OMAP5430_CORE_TSHUT_COLD			900
-#define OMAP5430_CORE_T_HOT				800
-#define OMAP5430_CORE_T_COLD				795
-#define OMAP5430_CORE_MAX_FREQ				1500000
-#define OMAP5430_CORE_MIN_FREQ				1000000
-#define OMAP5430_CORE_MIN_TEMP				-40000
-#define OMAP5430_CORE_MAX_TEMP				125000
-#define OMAP5430_CORE_HYST_VAL				5000
-
-/**
- * The register offsets and bit fields might change across
- * OMAP versions hence populating them in this structure.
- */
-
-struct temp_sensor_registers {
-	u32	temp_sensor_ctrl;
-	u32	bgap_tempsoff_mask;
-	u32	bgap_soc_mask;
-	u32	bgap_eocz_mask;
-	u32	bgap_dtemp_mask;
-
-	u32	bgap_mask_ctrl;
-	u32	mask_hot_mask;
-	u32	mask_cold_mask;
-
-	u32	bgap_mode_ctrl;
-	u32	mode_ctrl_mask;
-
-	u32	bgap_counter;
-	u32	counter_mask;
-
-	u32	bgap_threshold;
-	u32	threshold_thot_mask;
-	u32	threshold_tcold_mask;
-
-	u32	tshut_threshold;
-	u32	tshut_hot_mask;
-	u32	tshut_cold_mask;
-
-	u32	bgap_status;
-	u32	status_clean_stop_mask;
-	u32	status_bgap_alert_mask;
-	u32	status_hot_mask;
-	u32	status_cold_mask;
-
-	u32	bgap_efuse;
-};
-
-/**
- * The thresholds and limits for temperature sensors.
- */
-struct temp_sensor_data {
-	u32	tshut_hot;
-	u32	tshut_cold;
-	u32	t_hot;
-	u32	t_cold;
-	u32	min_freq;
-	u32	max_freq;
-	int     max_temp;
-	int     min_temp;
-	int     hyst_val;
-	u32     adc_start_val;
-	u32     adc_end_val;
-	u32     update_int1;
-	u32     update_int2;
-};
-
-struct omap_bandgap_data;
-
-/**
- * struct omap_bandgap - bandgap device structure
- * @dev: device pointer
- * @conf: platform data with sensor data
- * @fclock: pointer to functional clock of temperature sensor
- * @div_clk: pointer to parent clock of temperature sensor fclk
- * @conv_table: Pointer to adc to temperature conversion table
- * @bg_mutex: Mutex for sysfs, irq and PM
- * @irq: MPU Irq number for thermal alert
- * @tshut_gpio: GPIO where Tshut signal is routed
- * @clk_rate: Holds current clock rate
- */
-struct omap_bandgap {
-	struct device			*dev;
-	void __iomem			*base;
-	struct omap_bandgap_data	*conf;
-	struct clk			*fclock;
-	struct clk			*div_clk;
-	const int			*conv_table;
-	struct mutex			bg_mutex; /* Mutex for irq and PM */
-	int				irq;
-	int				tshut_gpio;
-	u32				clk_rate;
-};
-
-/**
- * struct temp_sensor_regval - temperature sensor register values
- * @bg_mode_ctrl: temp sensor control register value
- * @bg_ctrl: bandgap ctrl register value
- * @bg_counter: bandgap counter value
- * @bg_threshold: bandgap threshold register value
- * @tshut_threshold: bandgap tshut register value
- */
-struct temp_sensor_regval {
-	u32			bg_mode_ctrl;
-	u32			bg_ctrl;
-	u32			bg_counter;
-	u32			bg_threshold;
-	u32			tshut_threshold;
-};
-
-/**
- * struct omap_temp_sensor - bandgap temperature sensor platform data
- * @ts_data: pointer to struct with thresholds, limits of temperature sensor
- * @registers: pointer to the list of register offsets and bitfields
- * @regval: temperature sensor register values
- * @domain: the name of the domain where the sensor is located
- * @cooling_data: description on how the zone should be cooled off.
- * @slope: sensor gradient slope info for hotspot extrapolation
- * @const: sensor gradient const info for hotspot extrapolation
- * @slope_pcb: sensor gradient slope info for hotspot extrapolation
- *             with no external influence
- * @const_pcb: sensor gradient const info for hotspot extrapolation
- *             with no external influence
- * @data: private data
- * @register_cooling: function to describe how this sensor is going to be cooled
- * @unregister_cooling: function to release cooling data
- */
-struct omap_temp_sensor {
-	struct temp_sensor_data		*ts_data;
-	struct temp_sensor_registers	*registers;
-	struct temp_sensor_regval	regval;
-	char				*domain;
-	/* for hotspot extrapolation */
-	const int			slope;
-	const int			constant;
-	const int			slope_pcb;
-	const int			constant_pcb;
-	void				*data;
-	int (*register_cooling)(struct omap_bandgap *bg_ptr, int id);
-	int (*unregister_cooling)(struct omap_bandgap *bg_ptr, int id);
-};
-
-/**
- * struct omap_bandgap_data - bandgap platform data structure
- * @features: a bitwise flag set to describe the device features
- * @conv_table: Pointer to adc to temperature conversion table
- * @fclock_name: clock name of the functional clock
- * @div_ck_nme: clock name of the clock divisor
- * @sensor_count: count of temperature sensor device in scm
- * @sensors: array of sensors present in this bandgap instance
- * @expose_sensor: callback to export sensor to thermal API
- */
-struct omap_bandgap_data {
-#define OMAP_BANDGAP_FEATURE_TSHUT		(1 << 0)
-#define OMAP_BANDGAP_FEATURE_TSHUT_CONFIG	(1 << 1)
-#define OMAP_BANDGAP_FEATURE_TALERT		(1 << 2)
-#define OMAP_BANDGAP_FEATURE_MODE_CONFIG	(1 << 3)
-#define OMAP_BANDGAP_FEATURE_COUNTER		(1 << 4)
-#define OMAP_BANDGAP_FEATURE_POWER_SWITCH	(1 << 5)
-#define OMAP_BANDGAP_HAS(b, f)			\
-			((b)->conf->features & OMAP_BANDGAP_FEATURE_ ## f)
-	unsigned int			features;
-	const int			*conv_table;
-	char				*fclock_name;
-	char				*div_ck_name;
-	int				sensor_count;
-	int (*report_temperature)(struct omap_bandgap *bg_ptr, int id);
-	int (*expose_sensor)(struct omap_bandgap *bg_ptr, int id, char *domain);
-	int (*remove_sensor)(struct omap_bandgap *bg_ptr, int id);
-
-	/* this needs to be at the end */
-	struct omap_temp_sensor		sensors[];
-};
-
-int omap_bandgap_read_thot(struct omap_bandgap *bg_ptr, int id, int *thot);
-int omap_bandgap_write_thot(struct omap_bandgap *bg_ptr, int id, int val);
-int omap_bandgap_read_tcold(struct omap_bandgap *bg_ptr, int id, int *tcold);
-int omap_bandgap_write_tcold(struct omap_bandgap *bg_ptr, int id, int val);
-int omap_bandgap_read_update_interval(struct omap_bandgap *bg_ptr, int id,
-				      int *interval);
-int omap_bandgap_write_update_interval(struct omap_bandgap *bg_ptr, int id,
-				       u32 interval);
-int omap_bandgap_read_temperature(struct omap_bandgap *bg_ptr, int id,
-				  int *temperature);
-int omap_bandgap_set_sensor_data(struct omap_bandgap *bg_ptr, int id,
-				 void *data);
-void *omap_bandgap_get_sensor_data(struct omap_bandgap *bg_ptr, int id);
-
-#ifdef CONFIG_OMAP4_THERMAL
-extern const struct omap_bandgap_data omap4430_data;
-extern const struct omap_bandgap_data omap4460_data;
-extern const struct omap_bandgap_data omap4470_data;
-#else
-#define omap4430_data					NULL
-#define omap4460_data					NULL
-#define omap4470_data					NULL
-#endif
-
-#ifdef CONFIG_OMAP5_THERMAL
-extern const struct omap_bandgap_data omap5430_data;
-#else
-#define omap5430_data					NULL
-#endif
-
-#endif
diff --git a/drivers/staging/omap-thermal/omap-thermal-common.c b/drivers/staging/omap-thermal/omap-thermal-common.c
deleted file mode 100644
index 79a55aaae5a..00000000000
--- a/drivers/staging/omap-thermal/omap-thermal-common.c
+++ /dev/null
@@ -1,338 +0,0 @@
-/*
- * OMAP thermal driver interface
- *
- * Copyright (C) 2012 Texas Instruments Incorporated - http://www.ti.com/
- * Contact:
- *   Eduardo Valentin <eduardo.valentin@ti.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/device.h>
-#include <linux/err.h>
-#include <linux/mutex.h>
-#include <linux/gfp.h>
-#include <linux/kernel.h>
-#include <linux/workqueue.h>
-#include <linux/thermal.h>
-#include <linux/cpufreq.h>
-#include <linux/cpumask.h>
-#include <linux/cpu_cooling.h>
-
-#include "omap-thermal.h"
-#include "omap-bandgap.h"
-
-/* common data structures */
-struct omap_thermal_data {
-	struct thermal_zone_device *omap_thermal;
-	struct thermal_cooling_device *cool_dev;
-	struct omap_bandgap *bg_ptr;
-	enum thermal_device_mode mode;
-	struct work_struct thermal_wq;
-	int sensor_id;
-};
-
-static void omap_thermal_work(struct work_struct *work)
-{
-	struct omap_thermal_data *data = container_of(work,
-					struct omap_thermal_data, thermal_wq);
-
-	thermal_zone_device_update(data->omap_thermal);
-
-	dev_dbg(&data->omap_thermal->device, "updated thermal zone %s\n",
-		data->omap_thermal->type);
-}
-
-/**
- * omap_thermal_hotspot_temperature - returns sensor extrapolated temperature
- * @t:	omap sensor temperature
- * @s:	omap sensor slope value
- * @c:	omap sensor const value
- */
-static inline int omap_thermal_hotspot_temperature(int t, int s, int c)
-{
-	int delta = t * s / 1000 + c;
-
-	if (delta < 0)
-		delta = 0;
-
-	return t + delta;
-}
-
-/* thermal zone ops */
-/* Get temperature callback function for thermal zone*/
-static inline int omap_thermal_get_temp(struct thermal_zone_device *thermal,
-					 unsigned long *temp)
-{
-	struct omap_thermal_data *data = thermal->devdata;
-	struct omap_bandgap *bg_ptr;
-	struct omap_temp_sensor *s;
-	int ret, tmp, pcb_temp, slope, constant;
-
-	if (!data)
-		return 0;
-
-	bg_ptr = data->bg_ptr;
-	s = &bg_ptr->conf->sensors[data->sensor_id];
-
-	ret = omap_bandgap_read_temperature(bg_ptr, data->sensor_id, &tmp);
-	if (ret)
-		return ret;
-
-	pcb_temp = 0;
-	/* TODO: Introduce pcb temperature lookup */
-	/* In case pcb zone is available, use the extrapolation rule with it */
-	if (pcb_temp) {
-		tmp -= pcb_temp;
-		slope = s->slope_pcb;
-		constant = s->constant_pcb;
-	} else {
-		slope = s->slope;
-		constant = s->constant;
-	}
-	*temp = omap_thermal_hotspot_temperature(tmp, slope, constant);
-
-	return ret;
-}
-
-/* Bind callback functions for thermal zone */
-static int omap_thermal_bind(struct thermal_zone_device *thermal,
-			      struct thermal_cooling_device *cdev)
-{
-	struct omap_thermal_data *data = thermal->devdata;
-	int id;
-
-	if (IS_ERR_OR_NULL(data))
-		return -ENODEV;
-
-	/* check if this is the cooling device we registered */
-	if (data->cool_dev != cdev)
-		return 0;
-
-	id = data->sensor_id;
-
-	/* TODO: bind with min and max states */
-	/* Simple thing, two trips, one passive another critical */
-	return thermal_zone_bind_cooling_device(thermal, 0, cdev,
-						THERMAL_NO_LIMIT,
-						THERMAL_NO_LIMIT);
-}
-
-/* Unbind callback functions for thermal zone */
-static int omap_thermal_unbind(struct thermal_zone_device *thermal,
-				struct thermal_cooling_device *cdev)
-{
-	struct omap_thermal_data *data = thermal->devdata;
-
-	if (IS_ERR_OR_NULL(data))
-		return -ENODEV;
-
-	/* check if this is the cooling device we registered */
-	if (data->cool_dev != cdev)
-		return 0;
-
-	/* Simple thing, two trips, one passive another critical */
-	return thermal_zone_unbind_cooling_device(thermal, 0, cdev);
-}
-
-/* Get mode callback functions for thermal zone */
-static int omap_thermal_get_mode(struct thermal_zone_device *thermal,
-				  enum thermal_device_mode *mode)
-{
-	struct omap_thermal_data *data = thermal->devdata;
-
-	if (data)
-		*mode = data->mode;
-
-	return 0;
-}
-
-/* Set mode callback functions for thermal zone */
-static int omap_thermal_set_mode(struct thermal_zone_device *thermal,
-				  enum thermal_device_mode mode)
-{
-	struct omap_thermal_data *data = thermal->devdata;
-
-	if (!data->omap_thermal) {
-		dev_notice(&thermal->device, "thermal zone not registered\n");
-		return 0;
-	}
-
-	mutex_lock(&data->omap_thermal->lock);
-
-	if (mode == THERMAL_DEVICE_ENABLED)
-		data->omap_thermal->polling_delay = FAST_TEMP_MONITORING_RATE;
-	else
-		data->omap_thermal->polling_delay = 0;
-
-	mutex_unlock(&data->omap_thermal->lock);
-
-	data->mode = mode;
-	thermal_zone_device_update(data->omap_thermal);
-	dev_dbg(&thermal->device, "thermal polling set for duration=%d msec\n",
-		data->omap_thermal->polling_delay);
-
-	return 0;
-}
-
-/* Get trip type callback functions for thermal zone */
-static int omap_thermal_get_trip_type(struct thermal_zone_device *thermal,
-				       int trip, enum thermal_trip_type *type)
-{
-	if (!omap_thermal_is_valid_trip(trip))
-		return -EINVAL;
-
-	if (trip + 1 == OMAP_TRIP_NUMBER)
-		*type = THERMAL_TRIP_CRITICAL;
-	else
-		*type = THERMAL_TRIP_PASSIVE;
-
-	return 0;
-}
-
-/* Get trip temperature callback functions for thermal zone */
-static int omap_thermal_get_trip_temp(struct thermal_zone_device *thermal,
-				       int trip, unsigned long *temp)
-{
-	if (!omap_thermal_is_valid_trip(trip))
-		return -EINVAL;
-
-	*temp = omap_thermal_get_trip_value(trip);
-
-	return 0;
-}
-
-/* Get critical temperature callback functions for thermal zone */
-static int omap_thermal_get_crit_temp(struct thermal_zone_device *thermal,
-				       unsigned long *temp)
-{
-	/* shutdown zone */
-	return omap_thermal_get_trip_temp(thermal, OMAP_TRIP_NUMBER - 1, temp);
-}
-
-static struct thermal_zone_device_ops omap_thermal_ops = {
-	.get_temp = omap_thermal_get_temp,
-	/* TODO: add .get_trend */
-	.bind = omap_thermal_bind,
-	.unbind = omap_thermal_unbind,
-	.get_mode = omap_thermal_get_mode,
-	.set_mode = omap_thermal_set_mode,
-	.get_trip_type = omap_thermal_get_trip_type,
-	.get_trip_temp = omap_thermal_get_trip_temp,
-	.get_crit_temp = omap_thermal_get_crit_temp,
-};
-
-static struct omap_thermal_data
-*omap_thermal_build_data(struct omap_bandgap *bg_ptr, int id)
-{
-	struct omap_thermal_data *data;
-
-	data = devm_kzalloc(bg_ptr->dev, sizeof(*data), GFP_KERNEL);
-	if (!data) {
-		dev_err(bg_ptr->dev, "kzalloc fail\n");
-		return NULL;
-	}
-	data->sensor_id = id;
-	data->bg_ptr = bg_ptr;
-	data->mode = THERMAL_DEVICE_ENABLED;
-	INIT_WORK(&data->thermal_wq, omap_thermal_work);
-
-	return data;
-}
-
-int omap_thermal_expose_sensor(struct omap_bandgap *bg_ptr, int id,
-			       char *domain)
-{
-	struct omap_thermal_data *data;
-
-	data = omap_bandgap_get_sensor_data(bg_ptr, id);
-
-	if (IS_ERR(data))
-		data = omap_thermal_build_data(bg_ptr, id);
-
-	if (!data)
-		return -EINVAL;
-
-	/* TODO: remove TC1 TC2 */
-	/* Create thermal zone */
-	data->omap_thermal = thermal_zone_device_register(domain,
-				OMAP_TRIP_NUMBER, 0, data, &omap_thermal_ops,
-				NULL, FAST_TEMP_MONITORING_RATE,
-				FAST_TEMP_MONITORING_RATE);
-	if (IS_ERR_OR_NULL(data->omap_thermal)) {
-		dev_err(bg_ptr->dev, "thermal zone device is NULL\n");
-		return PTR_ERR(data->omap_thermal);
-	}
-	data->omap_thermal->polling_delay = FAST_TEMP_MONITORING_RATE;
-	omap_bandgap_set_sensor_data(bg_ptr, id, data);
-
-	return 0;
-}
-
-int omap_thermal_remove_sensor(struct omap_bandgap *bg_ptr, int id)
-{
-	struct omap_thermal_data *data;
-
-	data = omap_bandgap_get_sensor_data(bg_ptr, id);
-
-	thermal_zone_device_unregister(data->omap_thermal);
-
-	return 0;
-}
-
-int omap_thermal_report_sensor_temperature(struct omap_bandgap *bg_ptr, int id)
-{
-	struct omap_thermal_data *data;
-
-	data = omap_bandgap_get_sensor_data(bg_ptr, id);
-
-	schedule_work(&data->thermal_wq);
-
-	return 0;
-}
-
-int omap_thermal_register_cpu_cooling(struct omap_bandgap *bg_ptr, int id)
-{
-	struct omap_thermal_data *data;
-
-	data = omap_bandgap_get_sensor_data(bg_ptr, id);
-	if (IS_ERR(data))
-		data = omap_thermal_build_data(bg_ptr, id);
-
-	if (!data)
-		return -EINVAL;
-
-	/* Register cooling device */
-	data->cool_dev = cpufreq_cooling_register(cpu_present_mask);
-	if (IS_ERR_OR_NULL(data->cool_dev)) {
-		dev_err(bg_ptr->dev,
-			"Failed to register cpufreq cooling device\n");
-		return PTR_ERR(data->cool_dev);
-	}
-	omap_bandgap_set_sensor_data(bg_ptr, id, data);
-
-	return 0;
-}
-
-int omap_thermal_unregister_cpu_cooling(struct omap_bandgap *bg_ptr, int id)
-{
-	struct omap_thermal_data *data;
-
-	data = omap_bandgap_get_sensor_data(bg_ptr, id);
-	cpufreq_cooling_unregister(data->cool_dev);
-
-	return 0;
-}
diff --git a/drivers/staging/omap-thermal/omap5-thermal.c b/drivers/staging/omap-thermal/omap5-thermal.c
deleted file mode 100644
index 2f3a498dacd..00000000000
--- a/drivers/staging/omap-thermal/omap5-thermal.c
+++ /dev/null
@@ -1,297 +0,0 @@
-/*
- * OMAP5 thermal driver.
- *
- * Copyright (C) 2011-2012 Texas Instruments Inc.
- * Contact:
- *	Eduardo Valentin <eduardo.valentin@ti.com>
- *
- * 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 "omap-bandgap.h"
-#include "omap-thermal.h"
-
-/*
- * omap5430 has one instance of thermal sensor for MPU
- * need to describe the individual bit fields
- */
-static struct temp_sensor_registers
-omap5430_mpu_temp_sensor_registers = {
-	.temp_sensor_ctrl = OMAP5430_TEMP_SENSOR_MPU_OFFSET,
-	.bgap_tempsoff_mask = OMAP5430_BGAP_TEMPSOFF_MASK,
-	.bgap_soc_mask = OMAP5430_BGAP_TEMP_SENSOR_SOC_MASK,
-	.bgap_eocz_mask = OMAP5430_BGAP_TEMP_SENSOR_EOCZ_MASK,
-	.bgap_dtemp_mask = OMAP5430_BGAP_TEMP_SENSOR_DTEMP_MASK,
-
-	.bgap_mask_ctrl = OMAP5430_BGAP_CTRL_OFFSET,
-	.mask_hot_mask = OMAP5430_MASK_HOT_MPU_MASK,
-	.mask_cold_mask = OMAP5430_MASK_COLD_MPU_MASK,
-
-	.bgap_mode_ctrl = OMAP5430_BGAP_COUNTER_MPU_OFFSET,
-	.mode_ctrl_mask = OMAP5430_REPEAT_MODE_MASK,
-
-	.bgap_counter = OMAP5430_BGAP_COUNTER_MPU_OFFSET,
-	.counter_mask = OMAP5430_COUNTER_MASK,
-
-	.bgap_threshold = OMAP5430_BGAP_THRESHOLD_MPU_OFFSET,
-	.threshold_thot_mask = OMAP5430_T_HOT_MASK,
-	.threshold_tcold_mask = OMAP5430_T_COLD_MASK,
-
-	.tshut_threshold = OMAP5430_BGAP_TSHUT_MPU_OFFSET,
-	.tshut_hot_mask = OMAP5430_TSHUT_HOT_MASK,
-	.tshut_cold_mask = OMAP5430_TSHUT_COLD_MASK,
-
-	.bgap_status = OMAP5430_BGAP_STATUS_OFFSET,
-	.status_clean_stop_mask = 0x0,
-	.status_bgap_alert_mask = OMAP5430_BGAP_ALERT_MASK,
-	.status_hot_mask = OMAP5430_HOT_MPU_FLAG_MASK,
-	.status_cold_mask = OMAP5430_COLD_MPU_FLAG_MASK,
-
-	.bgap_efuse = OMAP5430_FUSE_OPP_BGAP_MPU,
-};
-
-/*
- * omap5430 has one instance of thermal sensor for GPU
- * need to describe the individual bit fields
- */
-static struct temp_sensor_registers
-omap5430_gpu_temp_sensor_registers = {
-	.temp_sensor_ctrl = OMAP5430_TEMP_SENSOR_GPU_OFFSET,
-	.bgap_tempsoff_mask = OMAP5430_BGAP_TEMPSOFF_MASK,
-	.bgap_soc_mask = OMAP5430_BGAP_TEMP_SENSOR_SOC_MASK,
-	.bgap_eocz_mask = OMAP5430_BGAP_TEMP_SENSOR_EOCZ_MASK,
-	.bgap_dtemp_mask = OMAP5430_BGAP_TEMP_SENSOR_DTEMP_MASK,
-
-	.bgap_mask_ctrl = OMAP5430_BGAP_CTRL_OFFSET,
-	.mask_hot_mask = OMAP5430_MASK_HOT_MM_MASK,
-	.mask_cold_mask = OMAP5430_MASK_COLD_MM_MASK,
-
-	.bgap_mode_ctrl = OMAP5430_BGAP_COUNTER_GPU_OFFSET,
-	.mode_ctrl_mask = OMAP5430_REPEAT_MODE_MASK,
-
-	.bgap_counter = OMAP5430_BGAP_COUNTER_GPU_OFFSET,
-	.counter_mask = OMAP5430_COUNTER_MASK,
-
-	.bgap_threshold = OMAP5430_BGAP_THRESHOLD_GPU_OFFSET,
-	.threshold_thot_mask = OMAP5430_T_HOT_MASK,
-	.threshold_tcold_mask = OMAP5430_T_COLD_MASK,
-
-	.tshut_threshold = OMAP5430_BGAP_TSHUT_GPU_OFFSET,
-	.tshut_hot_mask = OMAP5430_TSHUT_HOT_MASK,
-	.tshut_cold_mask = OMAP5430_TSHUT_COLD_MASK,
-
-	.bgap_status = OMAP5430_BGAP_STATUS_OFFSET,
-	.status_clean_stop_mask = 0x0,
-	.status_bgap_alert_mask = OMAP5430_BGAP_ALERT_MASK,
-	.status_hot_mask = OMAP5430_HOT_MM_FLAG_MASK,
-	.status_cold_mask = OMAP5430_COLD_MM_FLAG_MASK,
-
-	.bgap_efuse = OMAP5430_FUSE_OPP_BGAP_GPU,
-};
-
-/*
- * omap5430 has one instance of thermal sensor for CORE
- * need to describe the individual bit fields
- */
-static struct temp_sensor_registers
-omap5430_core_temp_sensor_registers = {
-	.temp_sensor_ctrl = OMAP5430_TEMP_SENSOR_CORE_OFFSET,
-	.bgap_tempsoff_mask = OMAP5430_BGAP_TEMPSOFF_MASK,
-	.bgap_soc_mask = OMAP5430_BGAP_TEMP_SENSOR_SOC_MASK,
-	.bgap_eocz_mask = OMAP5430_BGAP_TEMP_SENSOR_EOCZ_MASK,
-	.bgap_dtemp_mask = OMAP5430_BGAP_TEMP_SENSOR_DTEMP_MASK,
-
-	.bgap_mask_ctrl = OMAP5430_BGAP_CTRL_OFFSET,
-	.mask_hot_mask = OMAP5430_MASK_HOT_CORE_MASK,
-	.mask_cold_mask = OMAP5430_MASK_COLD_CORE_MASK,
-
-	.bgap_mode_ctrl = OMAP5430_BGAP_COUNTER_CORE_OFFSET,
-	.mode_ctrl_mask = OMAP5430_REPEAT_MODE_MASK,
-
-	.bgap_counter = OMAP5430_BGAP_COUNTER_CORE_OFFSET,
-	.counter_mask = OMAP5430_COUNTER_MASK,
-
-	.bgap_threshold = OMAP5430_BGAP_THRESHOLD_CORE_OFFSET,
-	.threshold_thot_mask = OMAP5430_T_HOT_MASK,
-	.threshold_tcold_mask = OMAP5430_T_COLD_MASK,
-
-	.tshut_threshold = OMAP5430_BGAP_TSHUT_CORE_OFFSET,
-	.tshut_hot_mask = OMAP5430_TSHUT_HOT_MASK,
-	.tshut_cold_mask = OMAP5430_TSHUT_COLD_MASK,
-
-	.bgap_status = OMAP5430_BGAP_STATUS_OFFSET,
-	.status_clean_stop_mask = 0x0,
-	.status_bgap_alert_mask = OMAP5430_BGAP_ALERT_MASK,
-	.status_hot_mask = OMAP5430_HOT_CORE_FLAG_MASK,
-	.status_cold_mask = OMAP5430_COLD_CORE_FLAG_MASK,
-
-	.bgap_efuse = OMAP5430_FUSE_OPP_BGAP_CORE,
-};
-
-/* Thresholds and limits for OMAP5430 MPU temperature sensor */
-static struct temp_sensor_data omap5430_mpu_temp_sensor_data = {
-	.tshut_hot = OMAP5430_MPU_TSHUT_HOT,
-	.tshut_cold = OMAP5430_MPU_TSHUT_COLD,
-	.t_hot = OMAP5430_MPU_T_HOT,
-	.t_cold = OMAP5430_MPU_T_COLD,
-	.min_freq = OMAP5430_MPU_MIN_FREQ,
-	.max_freq = OMAP5430_MPU_MAX_FREQ,
-	.max_temp = OMAP5430_MPU_MAX_TEMP,
-	.min_temp = OMAP5430_MPU_MIN_TEMP,
-	.hyst_val = OMAP5430_MPU_HYST_VAL,
-	.adc_start_val = OMAP5430_ADC_START_VALUE,
-	.adc_end_val = OMAP5430_ADC_END_VALUE,
-	.update_int1 = 1000,
-	.update_int2 = 2000,
-};
-
-/* Thresholds and limits for OMAP5430 GPU temperature sensor */
-static struct temp_sensor_data omap5430_gpu_temp_sensor_data = {
-	.tshut_hot = OMAP5430_GPU_TSHUT_HOT,
-	.tshut_cold = OMAP5430_GPU_TSHUT_COLD,
-	.t_hot = OMAP5430_GPU_T_HOT,
-	.t_cold = OMAP5430_GPU_T_COLD,
-	.min_freq = OMAP5430_GPU_MIN_FREQ,
-	.max_freq = OMAP5430_GPU_MAX_FREQ,
-	.max_temp = OMAP5430_GPU_MAX_TEMP,
-	.min_temp = OMAP5430_GPU_MIN_TEMP,
-	.hyst_val = OMAP5430_GPU_HYST_VAL,
-	.adc_start_val = OMAP5430_ADC_START_VALUE,
-	.adc_end_val = OMAP5430_ADC_END_VALUE,
-	.update_int1 = 1000,
-	.update_int2 = 2000,
-};
-
-/* Thresholds and limits for OMAP5430 CORE temperature sensor */
-static struct temp_sensor_data omap5430_core_temp_sensor_data = {
-	.tshut_hot = OMAP5430_CORE_TSHUT_HOT,
-	.tshut_cold = OMAP5430_CORE_TSHUT_COLD,
-	.t_hot = OMAP5430_CORE_T_HOT,
-	.t_cold = OMAP5430_CORE_T_COLD,
-	.min_freq = OMAP5430_CORE_MIN_FREQ,
-	.max_freq = OMAP5430_CORE_MAX_FREQ,
-	.max_temp = OMAP5430_CORE_MAX_TEMP,
-	.min_temp = OMAP5430_CORE_MIN_TEMP,
-	.hyst_val = OMAP5430_CORE_HYST_VAL,
-	.adc_start_val = OMAP5430_ADC_START_VALUE,
-	.adc_end_val = OMAP5430_ADC_END_VALUE,
-	.update_int1 = 1000,
-	.update_int2 = 2000,
-};
-
-static const int
-omap5430_adc_to_temp[OMAP5430_ADC_END_VALUE - OMAP5430_ADC_START_VALUE + 1] = {
-	-40000, -40000, -40000, -40000, -39800, -39400, -39000, -38600,
-	-38200, -37800, -37300, -36800,
-	-36400, -36000, -35600, -35200, -34800, -34300, -33800, -33400, -33000,
-	-32600,
-	-32200, -31800, -31300, -30800, -30400, -30000, -29600, -29200, -28700,
-	-28200, -27800, -27400, -27000, -26600, -26200, -25700, -25200, -24800,
-	-24400, -24000, -23600, -23200, -22700, -22200, -21800, -21400, -21000,
-	-20600, -20200, -19700, -19200, -9300, -18400, -18000, -17600, -17200,
-	-16700, -16200, -15800, -15400, -15000, -14600, -14200, -13700, -13200,
-	-12800, -12400, -12000, -11600, -11200, -10700, -10200, -9800, -9400,
-	-9000,
-	-8600, -8200, -7700, -7200, -6800, -6400, -6000, -5600, -5200, -4800,
-	-4300,
-	-3800, -3400, -3000, -2600, -2200, -1800, -1300, -800, -400, 0, 400,
-	800,
-	1200, 1600, 2100, 2600, 3000, 3400, 3800, 4200, 4600, 5100, 5600, 6000,
-	6400, 6800, 7200, 7600, 8000, 8500, 9000, 9400, 9800, 10200, 10800,
-	11100,
-	11400, 11900, 12400, 12800, 13200, 13600, 14000, 14400, 14800, 15300,
-	15800,
-	16200, 16600, 17000, 17400, 17800, 18200, 18700, 19200, 19600, 20000,
-	20400,
-	20800, 21200, 21600, 22100, 22600, 23000, 23400, 23800, 24200, 24600,
-	25000,
-	25400, 25900, 26400, 26800, 27200, 27600, 28000, 28400, 28800, 29300,
-	29800,
-	30200, 30600, 31000, 31400, 31800, 32200, 32600, 33100, 33600, 34000,
-	34400,
-	34800, 35200, 35600, 36000, 36400, 36800, 37300, 37800, 38200, 38600,
-	39000,
-	39400, 39800, 40200, 40600, 41100, 41600, 42000, 42400, 42800, 43200,
-	43600,
-	44000, 44400, 44800, 45300, 45800, 46200, 46600, 47000, 47400, 47800,
-	48200,
-	48600, 49000, 49500, 50000, 50400, 50800, 51200, 51600, 52000, 52400,
-	52800,
-	53200, 53700, 54200, 54600, 55000, 55400, 55800, 56200, 56600, 57000,
-	57400,
-	57800, 58200, 58700, 59200, 59600, 60000, 60400, 60800, 61200, 61600,
-	62000,
-	62400, 62800, 63300, 63800, 64200, 64600, 65000, 65400, 65800, 66200,
-	66600,
-	67000, 67400, 67800, 68200, 68700, 69200, 69600, 70000, 70400, 70800,
-	71200,
-	71600, 72000, 72400, 72800, 73200, 73600, 74100, 74600, 75000, 75400,
-	75800,
-	76200, 76600, 77000, 77400, 77800, 78200, 78600, 79000, 79400, 79800,
-	80300,
-	80800, 81200, 81600, 82000, 82400, 82800, 83200, 83600, 84000, 84400,
-	84800,
-	85200, 85600, 86000, 86400, 86800, 87300, 87800, 88200, 88600, 89000,
-	89400,
-	89800, 90200, 90600, 91000, 91400, 91800, 92200, 92600, 93000, 93400,
-	93800,
-	94200, 94600, 95000, 95500, 96000, 96400, 96800, 97200, 97600, 98000,
-	98400,
-	98800, 99200, 99600, 100000, 100400, 100800, 101200, 101600, 102000,
-	102400,
-	102800, 103200, 103600, 104000, 104400, 104800, 105200, 105600, 106100,
-	106600, 107000, 107400, 107800, 108200, 108600, 109000, 109400, 109800,
-	110200, 110600, 111000, 111400, 111800, 112200, 112600, 113000, 113400,
-	113800, 114200, 114600, 115000, 115400, 115800, 116200, 116600, 117000,
-	117400, 117800, 118200, 118600, 119000, 119400, 119800, 120200, 120600,
-	121000, 121400, 121800, 122200, 122600, 123000, 123400, 123800, 124200,
-	124600, 124900, 125000, 125000, 125000, 125000,
-};
-
-const struct omap_bandgap_data omap5430_data = {
-	.features = OMAP_BANDGAP_FEATURE_TSHUT_CONFIG |
-			OMAP_BANDGAP_FEATURE_TALERT |
-			OMAP_BANDGAP_FEATURE_MODE_CONFIG |
-			OMAP_BANDGAP_FEATURE_COUNTER,
-	.fclock_name = "ts_clk_div_ck",
-	.div_ck_name = "ts_clk_div_ck",
-	.conv_table = omap5430_adc_to_temp,
-	.expose_sensor = omap_thermal_expose_sensor,
-	.remove_sensor = omap_thermal_remove_sensor,
-	.sensors = {
-		{
-		.registers = &omap5430_mpu_temp_sensor_registers,
-		.ts_data = &omap5430_mpu_temp_sensor_data,
-		.domain = "cpu",
-		.register_cooling = omap_thermal_register_cpu_cooling,
-		.unregister_cooling = omap_thermal_unregister_cpu_cooling,
-		.slope = OMAP_GRADIENT_SLOPE_5430_CPU,
-		.constant = OMAP_GRADIENT_CONST_5430_CPU,
-		.slope_pcb = OMAP_GRADIENT_SLOPE_W_PCB_5430_CPU,
-		.constant_pcb = OMAP_GRADIENT_CONST_W_PCB_5430_CPU,
-		},
-		{
-		.registers = &omap5430_gpu_temp_sensor_registers,
-		.ts_data = &omap5430_gpu_temp_sensor_data,
-		.domain = "gpu",
-		.slope = OMAP_GRADIENT_SLOPE_5430_GPU,
-		.constant = OMAP_GRADIENT_CONST_5430_GPU,
-		.slope_pcb = OMAP_GRADIENT_SLOPE_W_PCB_5430_GPU,
-		.constant_pcb = OMAP_GRADIENT_CONST_W_PCB_5430_GPU,
-		},
-		{
-		.registers = &omap5430_core_temp_sensor_registers,
-		.ts_data = &omap5430_core_temp_sensor_data,
-		.domain = "core",
-		},
-	},
-	.sensor_count = 3,
-};
diff --git a/drivers/staging/ozwpan/ozcdev.c b/drivers/staging/ozwpan/ozcdev.c
index ba15aeb7067..27d06666c81 100644
--- a/drivers/staging/ozwpan/ozcdev.c
+++ b/drivers/staging/ozwpan/ozcdev.c
@@ -253,6 +253,7 @@ static long oz_cdev_ioctl(struct file *filp, unsigned int cmd,
 	case OZ_IOCTL_GET_PD_LIST: {
 			struct oz_pd_list list;
 			oz_trace("OZ_IOCTL_GET_PD_LIST\n");
+			memset(&list, 0, sizeof(list));
 			list.count = oz_get_pd_list(list.addr, OZ_MAX_PDS);
 			if (copy_to_user((void __user *)arg, &list,
 				sizeof(list)))
diff --git a/drivers/staging/rtl8192u/r8192U_dm.h b/drivers/staging/rtl8192u/r8192U_dm.h
index ffb083c958a..ae550520311 100644
--- a/drivers/staging/rtl8192u/r8192U_dm.h
+++ b/drivers/staging/rtl8192u/r8192U_dm.h
@@ -48,16 +48,16 @@
 #define		VeryLowRSSI					15
 #define		CTSToSelfTHVal					30
 
-//defined by vivi, for tx power track
+/* defined by vivi, for tx power track */
 #define		E_FOR_TX_POWER_TRACK               300
-//Dynamic Tx Power Control Threshold
+/* Dynamic Tx Power Control Threshold */
 #define		TX_POWER_NEAR_FIELD_THRESH_HIGH		68
 #define		TX_POWER_NEAR_FIELD_THRESH_LOW		62
-//added by amy for atheros AP
+/* added by amy for atheros AP */
 #define         TX_POWER_ATHEROAP_THRESH_HIGH           78
 #define		TX_POWER_ATHEROAP_THRESH_LOW		72
 
-//defined by vivi, for showing on UI
+/* defined by vivi, for showing on UI */
 #define			Current_Tx_Rate_Reg         0x1b8
 #define			Initial_Tx_Rate_Reg		  0x1b9
 #define			Tx_Retry_Count_Reg         0x1ac
@@ -98,28 +98,25 @@ typedef struct _dynamic_initial_gain_threshold_ {
 	bool		initialgain_lowerbound_state;
 
 	long		rssi_val;
-}dig_t;
+} dig_t;
 
-typedef enum tag_dynamic_init_gain_state_definition
-{
+typedef enum tag_dynamic_init_gain_state_definition {
 	DM_STA_DIG_OFF = 0,
 	DM_STA_DIG_ON,
 	DM_STA_DIG_MAX
-}dm_dig_sta_e;
+} dm_dig_sta_e;
 
 
 /* 2007/10/08 MH Define RATR state. */
-typedef enum tag_dynamic_ratr_state_definition
-{
+typedef enum tag_dynamic_ratr_state_definition {
 	DM_RATR_STA_HIGH = 0,
 	DM_RATR_STA_MIDDLE = 1,
 	DM_RATR_STA_LOW = 2,
 	DM_RATR_STA_MAX
-}dm_ratr_sta_e;
+} dm_ratr_sta_e;
 
 /* 2007/10/11 MH Define DIG operation type. */
-typedef enum tag_dynamic_init_gain_operation_type_definition
-{
+typedef enum tag_dynamic_init_gain_operation_type_definition {
 	DIG_TYPE_THRESH_HIGH	= 0,
 	DIG_TYPE_THRESH_LOW	= 1,
 	DIG_TYPE_THRESH_HIGHPWR_HIGH	= 2,
@@ -134,43 +131,38 @@ typedef enum tag_dynamic_init_gain_operation_type_definition
 	DIG_TYPE_ENABLE			= 20,
 	DIG_TYPE_DISABLE		= 30,
 	DIG_OP_TYPE_MAX
-}dm_dig_op_e;
+} dm_dig_op_e;
 
-typedef enum tag_dig_algorithm_definition
-{
+typedef enum tag_dig_algorithm_definition {
 	DIG_ALGO_BY_FALSE_ALARM = 0,
 	DIG_ALGO_BY_RSSI	= 1,
 	DIG_ALGO_MAX
-}dm_dig_alg_e;
+} dm_dig_alg_e;
 
-typedef enum tag_dig_dbgmode_definition
-{
+typedef enum tag_dig_dbgmode_definition {
 	DIG_DBG_OFF = 0,
 	DIG_DBG_ON = 1,
 	DIG_DBG_MAX
-}dm_dig_dbg_e;
+} dm_dig_dbg_e;
 
-typedef enum tag_dig_connect_definition
-{
+typedef enum tag_dig_connect_definition {
 	DIG_DISCONNECT = 0,
 	DIG_CONNECT = 1,
 	DIG_CONNECT_MAX
-}dm_dig_connect_e;
+} dm_dig_connect_e;
 
-typedef enum tag_dig_packetdetection_threshold_definition
-{
+typedef enum tag_dig_packetdetection_threshold_definition {
 	DIG_PD_AT_LOW_POWER = 0,
 	DIG_PD_AT_NORMAL_POWER = 1,
 	DIG_PD_AT_HIGH_POWER = 2,
 	DIG_PD_MAX
-}dm_dig_pd_th_e;
+} dm_dig_pd_th_e;
 
-typedef enum tag_dig_cck_cs_ratio_state_definition
-{
+typedef enum tag_dig_cck_cs_ratio_state_definition {
 	DIG_CS_RATIO_LOWER = 0,
 	DIG_CS_RATIO_HIGHER = 1,
 	DIG_CS_MAX
-}dm_dig_cs_ratio_e;
+} dm_dig_cs_ratio_e;
 typedef struct _Dynamic_Rx_Path_Selection_ {
 	u8		Enable;
 	u8		DbgMode;
@@ -185,27 +177,25 @@ typedef struct _Dynamic_Rx_Path_Selection_ {
 	u8		rf_rssi[4];
 	u8		rf_enable_rssi_th[4];
 	long		cck_pwdb_sta[4];
-}DRxPathSel;
+} DRxPathSel;
 
-typedef enum tag_CCK_Rx_Path_Method_Definition
-{
+typedef enum tag_CCK_Rx_Path_Method_Definition {
 	CCK_Rx_Version_1 = 0,
-	CCK_Rx_Version_2= 1,
+	CCK_Rx_Version_2 = 1,
 	CCK_Rx_Version_MAX
-}DM_CCK_Rx_Path_Method;
+} DM_CCK_Rx_Path_Method;
 
-typedef enum tag_DM_DbgMode_Definition
-{
+typedef enum tag_DM_DbgMode_Definition {
 	DM_DBG_OFF = 0,
 	DM_DBG_ON = 1,
 	DM_DBG_MAX
-}DM_DBG_E;
+} DM_DBG_E;
 
 typedef struct tag_Tx_Config_Cmd_Format {
-	u32	Op;										/* Command packet type. */
-	u32	Length;									/* Command packet length. */
+	u32	Op;			/* Command packet type. */
+	u32	Length;			/* Command packet length. */
 	u32	Value;
-}DCMD_TXCMD_T, *PDCMD_TXCMD_T;
+} DCMD_TXCMD_T, *PDCMD_TXCMD_T;
 /*------------------------------Define structure----------------------------*/
 
 
@@ -232,13 +222,14 @@ extern  void    dm_txpower_trackingcallback(struct work_struct *work);
 extern  void    dm_restore_dynamic_mechanism_state(struct net_device *dev);
 extern  void    dm_backup_dynamic_mechanism_state(struct net_device *dev);
 extern  void    dm_change_dynamic_initgain_thresh(struct net_device *dev,
-								u32 dm_type, u32 dm_value);
-extern  void    dm_force_tx_fw_info(struct net_device *dev,u32 force_type, u32 force_value);
+						u32 dm_type, u32 dm_value);
+extern  void    dm_force_tx_fw_info(struct net_device *dev,
+					u32 force_type, u32 force_value);
 extern  void    dm_init_edca_turbo(struct net_device *dev);
 extern  void    dm_rf_operation_test_callback(unsigned long data);
 extern  void    dm_rf_pathcheck_workitemcallback(struct work_struct *work);
 extern  void dm_fsync_timer_callback(unsigned long data);
-extern	void	dm_cck_txpower_adjust(struct net_device *dev,bool  binch14);
+extern	void	dm_cck_txpower_adjust(struct net_device *dev, bool  binch14);
 extern  void    dm_shadow_init(struct net_device *dev);
 extern void dm_initialize_txpower_tracking(struct net_device *dev);
 /*--------------------------Exported Function prototype---------------------*/
diff --git a/drivers/staging/rtl8712/rtl8712_led.c b/drivers/staging/rtl8712/rtl8712_led.c
index c9eb4b74799..6cb1a0af517 100644
--- a/drivers/staging/rtl8712/rtl8712_led.c
+++ b/drivers/staging/rtl8712/rtl8712_led.c
@@ -267,12 +267,8 @@ static void SwLedBlink(struct LED_871x *pLed)
 				   LED_BLINK_SLOWLY_INTERVAL);
 			break;
 		case LED_BLINK_WPS:
-			if (pLed->BlinkingLedState == LED_ON)
-				_set_timer(&(pLed->BlinkTimer),
-					   LED_BLINK_LONG_INTERVAL);
-			else
-				_set_timer(&(pLed->BlinkTimer),
-					   LED_BLINK_LONG_INTERVAL);
+			_set_timer(&(pLed->BlinkTimer),
+					LED_BLINK_LONG_INTERVAL);
 			break;
 		default:
 			_set_timer(&(pLed->BlinkTimer),
diff --git a/drivers/staging/rtl8712/rtl871x_recv.h b/drivers/staging/rtl8712/rtl871x_recv.h
index e42e6f0a15e..92ca8997e5b 100644
--- a/drivers/staging/rtl8712/rtl871x_recv.h
+++ b/drivers/staging/rtl8712/rtl871x_recv.h
@@ -9,9 +9,6 @@
 #define RXFRAME_ALIGN	8
 #define RXFRAME_ALIGN_SZ	(1 << RXFRAME_ALIGN)
 
-#define MAX_RXFRAME_CNT	512
-#define MAX_RX_NUMBLKS		(32)
-#define RECVFRAME_HDR_ALIGN 128
 #define MAX_SUBFRAME_COUNT	64
 
 #define SNAP_SIZE sizeof(struct ieee80211_snap_hdr)
@@ -130,15 +127,10 @@ struct sta_recv_priv {
 
 /* get a free recv_frame from pfree_recv_queue */
 union recv_frame *r8712_alloc_recvframe(struct  __queue *pfree_recv_queue);
-union recv_frame *r8712_dequeue_recvframe(struct  __queue *queue);
-int r8712_enqueue_recvframe(union recv_frame *precvframe,
-			     struct  __queue *queue);
 int r8712_free_recvframe(union recv_frame *precvframe,
 			  struct  __queue *pfree_recv_queue);
 void r8712_free_recvframe_queue(struct  __queue *pframequeue,
 				 struct  __queue *pfree_recv_queue);
-void r8712_init_recvframe(union recv_frame *precvframe,
-			   struct recv_priv *precvpriv);
 int r8712_wlanhdr_to_ethhdr(union recv_frame *precvframe);
 int recv_func(struct _adapter *padapter, void *pcontext);
 
@@ -150,11 +142,6 @@ static inline u8 *get_rxmem(union recv_frame *precvframe)
 	return precvframe->u.hdr.rx_head;
 }
 
-static inline u8 *get_rx_status(union recv_frame *precvframe)
-{
-	return get_rxmem(precvframe);
-}
-
 static inline u8 *get_recvframe_data(union recv_frame *precvframe)
 {
 	/* always return rx_data */
@@ -163,28 +150,6 @@ static inline u8 *get_recvframe_data(union recv_frame *precvframe)
 	return precvframe->u.hdr.rx_data;
 }
 
-static inline u8 *recvframe_push(union recv_frame *precvframe, sint sz)
-{
-	/* append data before rx_data */
-
-	/* add data to the start of recv_frame
-	 *
-	 * This function extends the used data area of the recv_frame at the
-	 * buffer start. rx_data must be still larger than rx_head, after
-	 * pushing.
-	 */
-
-	if (precvframe == NULL)
-		return NULL;
-	precvframe->u.hdr.rx_data -= sz ;
-	if (precvframe->u.hdr.rx_data < precvframe->u.hdr.rx_head) {
-		precvframe->u.hdr.rx_data += sz ;
-		return NULL;
-	}
-	precvframe->u.hdr.len += sz;
-	return precvframe->u.hdr.rx_data;
-}
-
 static inline u8 *recvframe_pull(union recv_frame *precvframe, sint sz)
 {
 	/* used for extract sz bytes from rx_data, update rx_data and return
@@ -236,53 +201,6 @@ static inline u8 *recvframe_pull_tail(union recv_frame *precvframe, sint sz)
 	return precvframe->u.hdr.rx_tail;
 }
 
-static inline _buffer *get_rxbuf_desc(union recv_frame *precvframe)
-{
-	_buffer *buf_desc;
-	if (precvframe == NULL)
-		return NULL;
-	return buf_desc;
-}
-
-static inline union recv_frame *rxmem_to_recvframe(u8 *rxmem)
-{
-	/* due to the design of 2048 bytes alignment of recv_frame, we can
-	 * reference the union recv_frame from any given member of recv_frame.
-	 * rxmem indicates the any member/address in recv_frame */
-	return (union recv_frame *)(((addr_t)rxmem >> RXFRAME_ALIGN) <<
-				  RXFRAME_ALIGN);
-}
-
-static inline union recv_frame *pkt_to_recvframe(_pkt *pkt)
-{
-	u8 *buf_star;
-	union recv_frame *precv_frame;
-
-	precv_frame = rxmem_to_recvframe((unsigned char *)buf_star);
-	return precv_frame;
-}
-
-static inline u8 *pkt_to_recvmem(_pkt *pkt)
-{
-	/* return the rx_head */
-	union recv_frame *precv_frame = pkt_to_recvframe(pkt);
-
-	return	precv_frame->u.hdr.rx_head;
-}
-
-static inline u8 *pkt_to_recvdata(_pkt *pkt)
-{
-	/* return the rx_data */
-	union recv_frame *precv_frame = pkt_to_recvframe(pkt);
-
-	return	precv_frame->u.hdr.rx_data;
-}
-
-static inline sint get_recvframe_len(union recv_frame *precvframe)
-{
-	return precvframe->u.hdr.len;
-}
-
 struct sta_info;
 
 void	_r8712_init_sta_recv_priv(struct sta_recv_priv *psta_recvpriv);
@@ -292,36 +210,10 @@ union recv_frame *r8712_decryptor(struct _adapter *adapter,
 				  union recv_frame *precv_frame);
 union recv_frame *r8712_recvframe_chk_defrag(struct _adapter *adapter,
 					     union recv_frame *precv_frame);
-union recv_frame *r8712_recvframe_defrag(struct _adapter *adapter,
-					 struct  __queue *defrag_q);
-union recv_frame *r8712_recvframe_chk_defrag_new(struct _adapter *adapter,
-					union recv_frame *precv_frame);
-union recv_frame *r8712_recvframe_defrag_new(struct _adapter *adapter,
-					struct  __queue *defrag_q,
-					union recv_frame *precv_frame);
-int r8712_recv_decache(union recv_frame *precv_frame, u8 bretry,
-		       struct stainfo_rxcache *prxcache);
-int r8712_sta2sta_data_frame(struct _adapter *adapter,
-			     union recv_frame *precv_frame,
-			     struct sta_info **psta);
-int r8712_ap2sta_data_frame(struct _adapter *adapter,
-			    union recv_frame *precv_frame,
-			    struct sta_info **psta);
-int r8712_sta2ap_data_frame(struct _adapter *adapter,
-			    union recv_frame *precv_frame,
-			    struct sta_info **psta);
-int r8712_validate_recv_ctrl_frame(struct _adapter *adapter,
-				   union recv_frame *precv_frame);
-int r8712_validate_recv_mgnt_frame(struct _adapter *adapter,
-				   union recv_frame *precv_frame);
-int r8712_validate_recv_data_frame(struct _adapter *adapter,
-				   union recv_frame *precv_frame);
 int r8712_validate_recv_frame(struct _adapter *adapter,
 			      union recv_frame *precv_frame);
 union recv_frame *r8712_portctrl(struct _adapter *adapter,
 				 union recv_frame *precv_frame);
-void  r8712_mgt_dispatcher(struct _adapter *padapter, u8 *pframe, uint len);
-int r8712_amsdu_to_msdu(struct _adapter *padapter, union recv_frame *prframe);
 
 #endif
 
diff --git a/drivers/staging/rts5139/rts51x_scsi.c b/drivers/staging/rts5139/rts51x_scsi.c
index 052911c9310..b58f1dfe006 100644
--- a/drivers/staging/rts5139/rts51x_scsi.c
+++ b/drivers/staging/rts5139/rts51x_scsi.c
@@ -2072,7 +2072,7 @@ int command_abort(struct scsi_cmnd *srb)
 
 /* This invokes the transport reset mechanism to reset the state of the
  * device */
-int device_reset(struct scsi_cmnd *srb)
+static int device_reset(struct scsi_cmnd *srb)
 {
 	int result = 0;
 
diff --git a/drivers/staging/rts5139/rts51x_scsi.h b/drivers/staging/rts5139/rts51x_scsi.h
index cdfe550371c..3a5213611d2 100644
--- a/drivers/staging/rts5139/rts51x_scsi.h
+++ b/drivers/staging/rts5139/rts51x_scsi.h
@@ -151,7 +151,6 @@ int proc_info(struct Scsi_Host *host, char *buffer,
 	      char **start, off_t offset, int length, int inout);
 int queuecommand(struct Scsi_Host *, struct scsi_cmnd *);
 int command_abort(struct scsi_cmnd *srb);
-int device_reset(struct scsi_cmnd *srb);
 int bus_reset(struct scsi_cmnd *srb);
 
 #endif /* __RTS51X_SCSI_H */
diff --git a/drivers/staging/rts5139/trace.h b/drivers/staging/rts5139/trace.h
index c9dfb1ea411..ac58b452ecb 100644
--- a/drivers/staging/rts5139/trace.h
+++ b/drivers/staging/rts5139/trace.h
@@ -27,32 +27,16 @@
 #ifndef __RTS51X_TRACE_H
 #define __RTS51X_TRACE_H
 
+#include <linux/string.h>
+
 #include "debug.h"
 
 #define _MSG_TRACE
 
 #ifdef _MSG_TRACE
-static inline char *filename(char *path)
-{
-	char *ptr;
-
-	if (path == NULL)
-		return NULL;
-
-	ptr = path;
-
-	while (*ptr != '\0') {
-		if ((*ptr == '\\') || (*ptr == '/'))
-			path = ptr + 1;
-		ptr++;
-	}
-
-	return path;
-}
-
 #define TRACE_RET(chip, ret)						\
 do {									\
-	char *_file = filename((char *)__FILE__);			\
+	const char *_file = kbasename(__FILE__);			\
 	RTS51X_DEBUGP("[%s][%s]:[%d]\n", _file, __func__, __LINE__);	\
 	(chip)->trace_msg[(chip)->msg_idx].line = (u16)(__LINE__);	\
 	strncpy((chip)->trace_msg[(chip)->msg_idx].func,		\
@@ -71,7 +55,7 @@ do {									\
 
 #define TRACE_GOTO(chip, label)						\
 do {									\
-	char *_file = filename((char *)__FILE__);			\
+	const char *_file = kbasename(__FILE__);			\
 	RTS51X_DEBUGP("[%s][%s]:[%d]\n", _file, __func__, __LINE__);	\
 	(chip)->trace_msg[(chip)->msg_idx].line = (u16)(__LINE__);	\
 	strncpy((chip)->trace_msg[(chip)->msg_idx].func,		\
diff --git a/drivers/staging/sb105x/sb_mp_register.h b/drivers/staging/sb105x/sb_mp_register.h
index a2087f5b0d1..304e1bcd1e3 100644
--- a/drivers/staging/sb105x/sb_mp_register.h
+++ b/drivers/staging/sb105x/sb_mp_register.h
@@ -19,7 +19,7 @@
  * option register 
  */
 
-/* Device Infomation Register */
+/* Device Information Register */
 #define MP_OPTR_DIR0		0x04 	/* port0 ~ port8 */
 #define MP_OPTR_DIR1		0x05 	/* port8 ~ port15 */
 #define MP_OPTR_DIR2		0x06 	/* port16 ~ port23 */
@@ -47,7 +47,7 @@
 #define IIR_RS485		0x20		/* RS485 type */
 #define IIR_TYPE_MASK		0x30
 
-/* Interrrupt Mask Register */
+/* Interrupt Mask Register */
 #define MP_OPTR_IMR0		0x0C 	/* port0 ~ port8 */
 #define MP_OPTR_IMR1		0x0D 	/* port8 ~ port15 */
 #define MP_OPTR_IMR2		0x0E 	/* port16 ~ port23 */
diff --git a/drivers/staging/sb105x/sb_pci_mp.c b/drivers/staging/sb105x/sb_pci_mp.c
index f75ee1dd475..cd94f6c2772 100644
--- a/drivers/staging/sb105x/sb_pci_mp.c
+++ b/drivers/staging/sb105x/sb_pci_mp.c
@@ -2248,7 +2248,7 @@ static irqreturn_t multi_interrupt(int irq, void *dev_id)
 		mtpt = list_entry(lhead, struct mp_port, list);
 		
 		iir = serial_in(mtpt, UART_IIR);
-		printk("intrrupt! port %d, iir 0x%x\n", mtpt->port.line, iir); //wlee
+		printk("interrupt! port %d, iir 0x%x\n", mtpt->port.line, iir); //wlee
 		if (!(iir & UART_IIR_NO_INT)) 
 		{
 			printk("interrupt handle\n");
@@ -2830,7 +2830,7 @@ static void __init multi_init_ports(void)
 
 			mtpt->port.uartclk  = BASE_BAUD * 16;
 
-			/* get input clock infomation */
+			/* get input clock information */
 			osc = inb(sbdev->option_reg_addr + MP_OPTR_DIR0 + i/8) & 0x0F;
 			if (osc==0x0f)
 				osc = 0;
diff --git a/drivers/staging/sb105x/sb_pci_mp.h b/drivers/staging/sb105x/sb_pci_mp.h
index f33efde07b9..a15f470a172 100644
--- a/drivers/staging/sb105x/sb_pci_mp.h
+++ b/drivers/staging/sb105x/sb_pci_mp.h
@@ -174,7 +174,7 @@ static DEFINE_MUTEX(mp_mutex);
 #define DEBUG_INTR(fmt...)  do { } while (0)
 #endif
 
-#if defined(__i386__) && (defined(CONFIG_M386) || defined(CONFIG_M486))
+#if defined(__i386__) && defined(CONFIG_M486)
 #define SERIAL_INLINE
 #endif
 #ifdef SERIAL_INLINE
diff --git a/drivers/staging/sep/sep_crypto.c b/drivers/staging/sep/sep_crypto.c
index cd3bb39e425..490a31e0fd4 100644
--- a/drivers/staging/sep/sep_crypto.c
+++ b/drivers/staging/sep/sep_crypto.c
@@ -1206,7 +1206,7 @@ static int sep_crypto_block_data(struct ablkcipher_request *req)
 
 		if (copy_result != crypto_ablkcipher_blocksize(tfm)) {
 			dev_warn(&ta_ctx->sep_used->pdev->dev,
-				"des block copy faild\n");
+				"des block copy failed\n");
 			return -ENOMEM;
 		}
 
@@ -1637,7 +1637,7 @@ static u32 crypto_post_op(struct sep_device *sep)
 					crypto_ablkcipher_blocksize(tfm)) {
 
 					dev_warn(&ta_ctx->sep_used->pdev->dev,
-						"des block copy faild\n");
+						"des block copy failed\n");
 					sep_crypto_release(sctx, ta_ctx,
 						-ENOMEM);
 					return -ENOMEM;
diff --git a/drivers/staging/sep/sep_driver_config.h b/drivers/staging/sep/sep_driver_config.h
index 7d7c7ab610b..4b6e3074363 100644
--- a/drivers/staging/sep/sep_driver_config.h
+++ b/drivers/staging/sep/sep_driver_config.h
@@ -219,7 +219,7 @@ held by the process (struct file) */
 /* maximum number of entries in the caller id table */
 #define SEP_CALLER_ID_TABLE_NUM_ENTRIES                       20
 
-/* maximum number of symetric operation (that require DMA resource)
+/* maximum number of symmetric operation (that require DMA resource)
 	per one message */
 #define SEP_MAX_NUM_SYNC_DMA_OPS			16
 
diff --git a/drivers/staging/sep/sep_main.c b/drivers/staging/sep/sep_main.c
index 30e8d25113e..6a98a208bbf 100644
--- a/drivers/staging/sep/sep_main.c
+++ b/drivers/staging/sep/sep_main.c
@@ -1986,7 +1986,7 @@ static int sep_prepare_input_dma_table(struct sep_device *sep,
 					dma_ctx,
 					sep_lli_entries);
 		if (error)
-			return error;
+			goto end_function_error;
 		lli_table_alloc_addr = *dmatables_region;
 	}
 
@@ -2276,7 +2276,7 @@ static int sep_construct_dma_tables_from_lli(
 			table_data_size);
 
 		/* If info entry is null - this is the first table built */
-		if (info_in_entry_ptr == NULL) {
+		if (info_in_entry_ptr == NULL || info_out_entry_ptr == NULL) {
 			/* Set the output parameters to physical addresses */
 			*lli_table_in_ptr =
 			sep_shared_area_virt_to_bus(sep, dma_in_lli_table_ptr);
@@ -2880,6 +2880,8 @@ static int sep_free_dma_tables_and_dcb(struct sep_device *sep, bool isapplet,
 
 	dev_dbg(&sep->pdev->dev, "[PID%d] sep_free_dma_tables_and_dcb\n",
 					current->pid);
+	if (!dma_ctx || !*dma_ctx) /* nothing to be done here*/
+		return 0;
 
 	if (((*dma_ctx)->secure_dma == false) && (isapplet == true)) {
 		dev_dbg(&sep->pdev->dev, "[PID%d] handling applet\n",
@@ -2895,8 +2897,7 @@ static int sep_free_dma_tables_and_dcb(struct sep_device *sep, bool isapplet,
 		 * Go over each DCB and see if
 		 * tail pointer must be updated
 		 */
-		for (i = 0; dma_ctx && *dma_ctx &&
-			i < (*dma_ctx)->nr_dcb_creat; i++, dcb_table_ptr++) {
+		for (i = 0; i < (*dma_ctx)->nr_dcb_creat; i++, dcb_table_ptr++) {
 			if (dcb_table_ptr->out_vr_tail_pt) {
 				pt_hold = (unsigned long)dcb_table_ptr->
 					out_vr_tail_pt;
diff --git a/drivers/staging/sep/sep_trace_events.h b/drivers/staging/sep/sep_trace_events.h
index 2b053a93afe..74f4c9a2b5b 100644
--- a/drivers/staging/sep/sep_trace_events.h
+++ b/drivers/staging/sep/sep_trace_events.h
@@ -53,6 +53,11 @@
 #include <linux/tracepoint.h>
 
 /*
+ * Since use str*cpy in header file, better to include string.h, directly.
+ */
+#include <linux/string.h>
+
+/*
  * The TRACE_EVENT macro is broken up into 5 parts.
  *
  * name: name of the trace point. This is also how to enable the tracepoint.
@@ -97,7 +102,7 @@ TRACE_EVENT(sep_func_start,
 	),
 
 	TP_fast_assign(
-		strncpy(__entry->name, name, 20);
+		strlcpy(__entry->name, name, 20);
 		__entry->branch	= branch;
 	),
 
@@ -116,7 +121,7 @@ TRACE_EVENT(sep_func_end,
 	),
 
 	TP_fast_assign(
-		strncpy(__entry->name, name, 20);
+		strlcpy(__entry->name, name, 20);
 		__entry->branch	= branch;
 	),
 
@@ -135,7 +140,7 @@ TRACE_EVENT(sep_misc_event,
 	),
 
 	TP_fast_assign(
-		strncpy(__entry->name, name, 20);
+		strlcpy(__entry->name, name, 20);
 		__entry->branch	= branch;
 	),
 
diff --git a/drivers/staging/serqt_usb2/serqt_usb2.c b/drivers/staging/serqt_usb2/serqt_usb2.c
index b1bb1a6abe8..8a6e5ea476e 100644
--- a/drivers/staging/serqt_usb2/serqt_usb2.c
+++ b/drivers/staging/serqt_usb2/serqt_usb2.c
@@ -264,7 +264,6 @@ static void ProcessRxChar(struct usb_serial_port *port, unsigned char data)
 
 static void qt_write_bulk_callback(struct urb *urb)
 {
-	struct tty_struct *tty;
 	int status;
 	struct quatech_port *quatech_port;
 
@@ -278,11 +277,7 @@ static void qt_write_bulk_callback(struct urb *urb)
 
 	quatech_port = urb->context;
 
-	tty = tty_port_tty_get(&quatech_port->port->port);
-
-	if (tty)
-		tty_wakeup(tty);
-	tty_kref_put(tty);
+	tty_port_tty_wakeup(&quatech_port->port->port);
 }
 
 static void qt_interrupt_callback(struct urb *urb)
diff --git a/drivers/staging/silicom/bp_mod.c b/drivers/staging/silicom/bp_mod.c
index 58c5f5cf4ce..45a22272320 100644
--- a/drivers/staging/silicom/bp_mod.c
+++ b/drivers/staging/silicom/bp_mod.c
@@ -6983,7 +6983,7 @@ static void __exit bypass_cleanup_module(void)
 /*	spin_lock_irqsave(&bpvm_lock, flags);
 	rcu_read_lock(); */
 		bypass_proc_remove_dev_sd(&bpctl_dev_arr[i]);
-/*	spin_unlock_irqrestore(&bpvm_lock, flags);        
+/*	spin_unlock_irqrestore(&bpvm_lock, flags);
 	rcu_read_unlock(); */
 #endif
 		remove_bypass_wd_auto(&bpctl_dev_arr[i]);
@@ -6995,18 +6995,16 @@ static void __exit bypass_cleanup_module(void)
 	/* unmap all devices */
 	for (i = 0; i < device_num; i++) {
 #ifdef BP_SELF_TEST
-		if (bpctl_dev_arr[i].bp_tx_data)
-			kfree(bpctl_dev_arr[i].bp_tx_data);
+		kfree(bpctl_dev_arr[i].bp_tx_data);
 #endif
 		iounmap((void *)(bpctl_dev_arr[i].mem_map));
 	}
 
 	/* free all devices space */
-	if (bpctl_dev_arr)
-		kfree(bpctl_dev_arr);
+	kfree(bpctl_dev_arr);
 
 /*
-* Unregister the device                             
+* Unregister the device
 */
 	unregister_chrdev(major_num, DEVICE_NAME);
 }
diff --git a/drivers/staging/silicom/bypasslib/bypass.c b/drivers/staging/silicom/bypasslib/bypass.c
index 95a1f1815d9..9ed250848e8 100644
--- a/drivers/staging/silicom/bypasslib/bypass.c
+++ b/drivers/staging/silicom/bypasslib/bypass.c
@@ -134,7 +134,7 @@ static int is_dev_sd(int if_index)
 	return (ret >= 0 ? 1 : 0);
 }
 
-int is_bypass_dev(int if_index)
+static int is_bypass_dev(int if_index)
 {
 	struct pci_dev *pdev = NULL;
 	struct net_device *dev = NULL;
@@ -179,7 +179,7 @@ int is_bypass_dev(int if_index)
 	return (ret < 0 ? -1 : ret);
 }
 
-int is_bypass(int if_index)
+static int is_bypass(int if_index)
 {
 	int ret = 0;
 	SET_BPLIB_INT_FN(is_bypass, int, if_index, ret);
@@ -189,70 +189,70 @@ int is_bypass(int if_index)
 	return ret;
 }
 
-int get_bypass_slave(int if_index)
+static int get_bypass_slave(int if_index)
 {
 	DO_BPLIB_GET_ARG_FN(get_bypass_slave, GET_BYPASS_SLAVE, if_index);
 }
 
-int get_bypass_caps(int if_index)
+static int get_bypass_caps(int if_index)
 {
 	DO_BPLIB_GET_ARG_FN(get_bypass_caps, GET_BYPASS_CAPS, if_index);
 }
 
-int get_wd_set_caps(int if_index)
+static int get_wd_set_caps(int if_index)
 {
 	DO_BPLIB_GET_ARG_FN(get_wd_set_caps, GET_WD_SET_CAPS, if_index);
 }
 
-int set_bypass(int if_index, int bypass_mode)
+static int set_bypass(int if_index, int bypass_mode)
 {
 	DO_BPLIB_SET_ARG_FN(set_bypass, SET_BYPASS, if_index, bypass_mode);
 }
 
-int get_bypass(int if_index)
+static int get_bypass(int if_index)
 {
 	DO_BPLIB_GET_ARG_FN(get_bypass, GET_BYPASS, if_index);
 }
 
-int get_bypass_change(int if_index)
+static int get_bypass_change(int if_index)
 {
 	DO_BPLIB_GET_ARG_FN(get_bypass_change, GET_BYPASS_CHANGE, if_index);
 }
 
-int set_dis_bypass(int if_index, int dis_bypass)
+static int set_dis_bypass(int if_index, int dis_bypass)
 {
 	DO_BPLIB_SET_ARG_FN(set_dis_bypass, SET_DIS_BYPASS, if_index,
 			    dis_bypass);
 }
 
-int get_dis_bypass(int if_index)
+static int get_dis_bypass(int if_index)
 {
 	DO_BPLIB_GET_ARG_FN(get_dis_bypass, GET_DIS_BYPASS, if_index);
 }
 
-int set_bypass_pwoff(int if_index, int bypass_mode)
+static int set_bypass_pwoff(int if_index, int bypass_mode)
 {
 	DO_BPLIB_SET_ARG_FN(set_bypass_pwoff, SET_BYPASS_PWOFF, if_index,
 			    bypass_mode);
 }
 
-int get_bypass_pwoff(int if_index)
+static int get_bypass_pwoff(int if_index)
 {
 	DO_BPLIB_GET_ARG_FN(get_bypass_pwoff, GET_BYPASS_PWOFF, if_index);
 }
 
-int set_bypass_pwup(int if_index, int bypass_mode)
+static int set_bypass_pwup(int if_index, int bypass_mode)
 {
 	DO_BPLIB_SET_ARG_FN(set_bypass_pwup, SET_BYPASS_PWUP, if_index,
 			    bypass_mode);
 }
 
-int get_bypass_pwup(int if_index)
+static int get_bypass_pwup(int if_index)
 {
 	DO_BPLIB_GET_ARG_FN(get_bypass_pwup, GET_BYPASS_PWUP, if_index);
 }
 
-int set_bypass_wd(int if_index, int ms_timeout, int *ms_timeout_set)
+static int set_bypass_wd(int if_index, int ms_timeout, int *ms_timeout_set)
 {
 	int data = ms_timeout, ret = 0;
 	if (is_dev_sd(if_index))
@@ -268,7 +268,7 @@ int set_bypass_wd(int if_index, int ms_timeout, int *ms_timeout_set)
 	return ret;
 }
 
-int get_bypass_wd(int if_index, int *ms_timeout_set)
+static int get_bypass_wd(int if_index, int *ms_timeout_set)
 {
 	int *data = ms_timeout_set, ret = 0;
 	if (is_dev_sd(if_index))
@@ -279,7 +279,7 @@ int get_bypass_wd(int if_index, int *ms_timeout_set)
 	return ret;
 }
 
-int get_wd_expire_time(int if_index, int *ms_time_left)
+static int get_wd_expire_time(int if_index, int *ms_time_left)
 {
 	int *data = ms_time_left, ret = 0;
 	if (is_dev_sd(if_index))
@@ -293,144 +293,144 @@ int get_wd_expire_time(int if_index, int *ms_time_left)
 	return ret;
 }
 
-int reset_bypass_wd_timer(int if_index)
+static int reset_bypass_wd_timer(int if_index)
 {
 	DO_BPLIB_GET_ARG_FN(reset_bypass_wd_timer, RESET_BYPASS_WD_TIMER,
 			    if_index);
 }
 
-int set_std_nic(int if_index, int bypass_mode)
+static int set_std_nic(int if_index, int bypass_mode)
 {
 	DO_BPLIB_SET_ARG_FN(set_std_nic, SET_STD_NIC, if_index, bypass_mode);
 }
 
-int get_std_nic(int if_index)
+static int get_std_nic(int if_index)
 {
 	DO_BPLIB_GET_ARG_FN(get_std_nic, GET_STD_NIC, if_index);
 }
 
-int set_tx(int if_index, int tx_state)
+static int set_tx(int if_index, int tx_state)
 {
 	DO_BPLIB_SET_ARG_FN(set_tx, SET_TX, if_index, tx_state);
 }
 
-int get_tx(int if_index)
+static int get_tx(int if_index)
 {
 	DO_BPLIB_GET_ARG_FN(get_tx, GET_TX, if_index);
 }
 
-int set_tap(int if_index, int tap_mode)
+static int set_tap(int if_index, int tap_mode)
 {
 	DO_BPLIB_SET_ARG_FN(set_tap, SET_TAP, if_index, tap_mode);
 }
 
-int get_tap(int if_index)
+static int get_tap(int if_index)
 {
 	DO_BPLIB_GET_ARG_FN(get_tap, GET_TAP, if_index);
 }
 
-int get_tap_change(int if_index)
+static int get_tap_change(int if_index)
 {
 	DO_BPLIB_GET_ARG_FN(get_tap_change, GET_TAP_CHANGE, if_index);
 }
 
-int set_dis_tap(int if_index, int dis_tap)
+static int set_dis_tap(int if_index, int dis_tap)
 {
 	DO_BPLIB_SET_ARG_FN(set_dis_tap, SET_DIS_TAP, if_index, dis_tap);
 }
 
-int get_dis_tap(int if_index)
+static int get_dis_tap(int if_index)
 {
 	DO_BPLIB_GET_ARG_FN(get_dis_tap, GET_DIS_TAP, if_index);
 }
 
-int set_tap_pwup(int if_index, int tap_mode)
+static int set_tap_pwup(int if_index, int tap_mode)
 {
 	DO_BPLIB_SET_ARG_FN(set_tap_pwup, SET_TAP_PWUP, if_index, tap_mode);
 }
 
-int get_tap_pwup(int if_index)
+static int get_tap_pwup(int if_index)
 {
 	DO_BPLIB_GET_ARG_FN(get_tap_pwup, GET_TAP_PWUP, if_index);
 }
 
-int set_bp_disc(int if_index, int disc_mode)
+static int set_bp_disc(int if_index, int disc_mode)
 {
 	DO_BPLIB_SET_ARG_FN(set_bp_disc, SET_DISC, if_index, disc_mode);
 }
 
-int get_bp_disc(int if_index)
+static int get_bp_disc(int if_index)
 {
 	DO_BPLIB_GET_ARG_FN(get_bp_disc, GET_DISC, if_index);
 }
 
-int get_bp_disc_change(int if_index)
+static int get_bp_disc_change(int if_index)
 {
 	DO_BPLIB_GET_ARG_FN(get_bp_disc_change, GET_DISC_CHANGE, if_index);
 }
 
-int set_bp_dis_disc(int if_index, int dis_disc)
+static int set_bp_dis_disc(int if_index, int dis_disc)
 {
 	DO_BPLIB_SET_ARG_FN(set_bp_dis_disc, SET_DIS_DISC, if_index, dis_disc);
 }
 
-int get_bp_dis_disc(int if_index)
+static int get_bp_dis_disc(int if_index)
 {
 	DO_BPLIB_GET_ARG_FN(get_bp_dis_disc, GET_DIS_DISC, if_index);
 }
 
-int set_bp_disc_pwup(int if_index, int disc_mode)
+static int set_bp_disc_pwup(int if_index, int disc_mode)
 {
 	DO_BPLIB_SET_ARG_FN(set_bp_disc_pwup, SET_DISC_PWUP, if_index,
 			    disc_mode);
 }
 
-int get_bp_disc_pwup(int if_index)
+static int get_bp_disc_pwup(int if_index)
 {
 	DO_BPLIB_GET_ARG_FN(get_bp_disc_pwup, GET_DISC_PWUP, if_index);
 }
 
-int set_wd_exp_mode(int if_index, int mode)
+static int set_wd_exp_mode(int if_index, int mode)
 {
 	DO_BPLIB_SET_ARG_FN(set_wd_exp_mode, SET_WD_EXP_MODE, if_index, mode);
 }
 
-int get_wd_exp_mode(int if_index)
+static int get_wd_exp_mode(int if_index)
 {
 	DO_BPLIB_GET_ARG_FN(get_wd_exp_mode, GET_WD_EXP_MODE, if_index);
 }
 
-int set_wd_autoreset(int if_index, int time)
+static int set_wd_autoreset(int if_index, int time)
 {
 	DO_BPLIB_SET_ARG_FN(set_wd_autoreset, SET_WD_AUTORESET, if_index, time);
 }
 
-int get_wd_autoreset(int if_index)
+static int get_wd_autoreset(int if_index)
 {
 	DO_BPLIB_GET_ARG_FN(get_wd_autoreset, GET_WD_AUTORESET, if_index);
 }
 
-int set_tpl(int if_index, int tpl_mode)
+static int set_tpl(int if_index, int tpl_mode)
 {
 	DO_BPLIB_SET_ARG_FN(set_tpl, SET_TPL, if_index, tpl_mode);
 }
 
-int get_tpl(int if_index)
+static int get_tpl(int if_index)
 {
 	DO_BPLIB_GET_ARG_FN(get_tpl, GET_TPL, if_index);
 }
 
-int set_bp_hw_reset(int if_index, int mode)
+static int set_bp_hw_reset(int if_index, int mode)
 {
 	DO_BPLIB_SET_ARG_FN(set_tpl, SET_BP_HW_RESET, if_index, mode);
 }
 
-int get_bp_hw_reset(int if_index)
+static int get_bp_hw_reset(int if_index)
 {
 	DO_BPLIB_GET_ARG_FN(get_tpl, GET_BP_HW_RESET, if_index);
 }
 
-int get_bypass_info(int if_index, struct bp_info *bp_info)
+static int get_bypass_info(int if_index, struct bp_info *bp_info)
 {
 	int ret = 0;
 	if (is_dev_sd(if_index)) {
@@ -468,14 +468,14 @@ int get_bypass_info(int if_index, struct bp_info *bp_info)
 	return ret;
 }
 
-int init_lib_module()
+int init_lib_module(void)
 {
 
 	printk(VERSION);
 	return 0;
 }
 
-void cleanup_lib_module()
+void cleanup_lib_module(void)
 {
 }
 
diff --git a/drivers/staging/slicoss/slicoss.c b/drivers/staging/slicoss/slicoss.c
index 76fc2e554f3..e4b82770ed3 100644
--- a/drivers/staging/slicoss/slicoss.c
+++ b/drivers/staging/slicoss/slicoss.c
@@ -76,6 +76,7 @@
 #include <linux/bitops.h>
 #include <linux/io.h>
 #include <linux/netdevice.h>
+#include <linux/crc32.h>
 #include <linux/etherdevice.h>
 #include <linux/skbuff.h>
 #include <linux/delay.h>
@@ -189,76 +190,14 @@ static inline void slic_reg64_write(struct adapter *adapter, void __iomem *reg,
 				adapter->bit64reglock.flags);
 }
 
-/*
- * Functions to obtain the CRC corresponding to the destination mac address.
- * This is a standard ethernet CRC in that it is a 32-bit, reflected CRC using
- * the polynomial:
- *   x^32 + x^26 + x^23 + x^22 + x^16 + x^12 + x^11 + x^10 + x^8 + x^7 + x^5 +
- *   x^4 + x^2 + x^1.
- *
- * After the CRC for the 6 bytes is generated (but before the value is
- * complemented),
- * we must then transpose the value and return bits 30-23.
- *
- */
-static u32 slic_crc_table[256];	/* Table of CRCs for all possible byte values */
-static u32 slic_crc_init;	/* Is table initialized */
-
-/*
- *  Contruct the CRC32 table
- */
-static void slic_mcast_init_crc32(void)
-{
-	u32 c;			/*  CRC reg                      */
-	u32 e = 0;		/*  Poly X-or pattern            */
-	int i;			/*  counter                      */
-	int k;			/*  byte being shifted into crc  */
-
-	static int p[] = { 0, 1, 2, 4, 5, 7, 8, 10, 11, 12, 16, 22, 23, 26 };
-
-	for (i = 0; i < ARRAY_SIZE(p); i++)
-		e |= 1L << (31 - p[i]);
-
-	for (i = 1; i < 256; i++) {
-		c = i;
-		for (k = 8; k; k--)
-			c = c & 1 ? (c >> 1) ^ e : c >> 1;
-		slic_crc_table[i] = c;
-	}
-}
-
-/*
- *  Return the MAC hast as described above.
- */
-static unsigned char slic_mcast_get_mac_hash(char *macaddr)
-{
-	u32 crc;
-	char *p;
-	int i;
-	unsigned char machash = 0;
-
-	if (!slic_crc_init) {
-		slic_mcast_init_crc32();
-		slic_crc_init = 1;
-	}
-
-	crc = 0xFFFFFFFF;	/* Preload shift register, per crc-32 spec */
-	for (i = 0, p = macaddr; i < 6; ++p, ++i)
-		crc = (crc >> 8) ^ slic_crc_table[(crc ^ *p) & 0xFF];
-
-	/* Return bits 1-8, transposed */
-	for (i = 1; i < 9; i++)
-		machash |= (((crc >> i) & 1) << (8 - i));
-
-	return machash;
-}
-
 static void slic_mcast_set_bit(struct adapter *adapter, char *address)
 {
 	unsigned char crcpoly;
 
 	/* Get the CRC polynomial for the mac address */
-	crcpoly = slic_mcast_get_mac_hash(address);
+	/* we use bits 1-8 (lsb), bitwise reversed,
+	 * msb (= lsb bit 0 before bitrev) is automatically discarded */
+	crcpoly = (ether_crc(ETH_ALEN, address)>>23);
 
 	/* We only have space on the SLIC for 64 entries.  Lop
 	 * off the top two bits. (2^6 = 64)
@@ -901,10 +840,9 @@ static void slic_timer_load_check(ulong cardaddr)
 	u32 load = card->events;
 	u32 level = 0;
 
-	intagg = &adapter->slic_regs->slic_intagg;
-
 	if ((adapter) && (adapter->state == ADAPT_UP) &&
 	    (card->state == CARD_UP) && (slic_global.dynamic_intagg)) {
+		intagg = &adapter->slic_regs->slic_intagg;
 		if (adapter->devid == SLIC_1GB_DEVICE_ID) {
 			if (adapter->linkspeed == LINK_1000MB)
 				level = 100;
@@ -1061,7 +999,8 @@ static void slic_link_upr_complete(struct adapter *adapter, u32 isr)
 	if ((isr & ISR_UPCERR) || (isr & ISR_UPCBSY)) {
 		struct slic_shmem *pshmem;
 
-		pshmem = (struct slic_shmem *)adapter->phys_shmem;
+		pshmem = (struct slic_shmem *)(unsigned long)
+			 adapter->phys_shmem;
 #if BITS_PER_LONG == 64
 		slic_upr_queue_request(adapter,
 				       SLIC_UPR_RLSR,
@@ -1415,7 +1354,7 @@ static struct slic_rspbuf *slic_rspqueue_getnext(struct adapter *adapter)
 		slic_reg64_write(adapter, &adapter->slic_regs->slic_rbar64,
 			(rspq->paddr[rspq->pageindex] | SLIC_RSPQ_BUFSINPAGE),
 			&adapter->slic_regs->slic_addr_upper, 0, DONT_FLUSH);
-		rspq->pageindex = (++rspq->pageindex) % rspq->num_pages;
+		rspq->pageindex = (rspq->pageindex + 1) % rspq->num_pages;
 		rspq->offset = 0;
 		rspq->rspbuf = (struct slic_rspbuf *)
 						rspq->vaddr[rspq->pageindex];
@@ -1693,10 +1632,11 @@ retry_rcvqfill:
 #endif
 		skb = alloc_skb(SLIC_RCVQ_RCVBUFSIZE, GFP_ATOMIC);
 		if (skb) {
-			paddr = (void *)pci_map_single(adapter->pcidev,
-							  skb->data,
-							  SLIC_RCVQ_RCVBUFSIZE,
-							  PCI_DMA_FROMDEVICE);
+			paddr = (void *)(unsigned long)
+				pci_map_single(adapter->pcidev,
+					       skb->data,
+					       SLIC_RCVQ_RCVBUFSIZE,
+					       PCI_DMA_FROMDEVICE);
 			paddrl = SLIC_GET_ADDR_LOW(paddr);
 			paddrh = SLIC_GET_ADDR_HIGH(paddr);
 
@@ -1843,8 +1783,9 @@ static u32 slic_rcvqueue_reinsert(struct adapter *adapter, struct sk_buff *skb)
 	struct slic_rcvbuf *rcvbuf = (struct slic_rcvbuf *)skb->head;
 	struct device *dev;
 
-	paddr = (void *)pci_map_single(adapter->pcidev, skb->head,
-				  SLIC_RCVQ_RCVBUFSIZE, PCI_DMA_FROMDEVICE);
+	paddr = (void *)(unsigned long)
+		pci_map_single(adapter->pcidev, skb->head,
+			       SLIC_RCVQ_RCVBUFSIZE, PCI_DMA_FROMDEVICE);
 	rcvbuf->status = 0;
 	skb->next = NULL;
 
@@ -2341,7 +2282,7 @@ static void slic_link_event_handler(struct adapter *adapter)
 		return;
 	}
 
-	pshmem = (struct slic_shmem *)adapter->phys_shmem;
+	pshmem = (struct slic_shmem *)(unsigned long)adapter->phys_shmem;
 
 #if BITS_PER_LONG == 64
 	status = slic_upr_request(adapter,
@@ -2368,7 +2309,7 @@ static void slic_init_cleanup(struct adapter *adapter)
 				    sizeof(struct slic_shmem),
 				    adapter->pshmem, adapter->phys_shmem);
 		adapter->pshmem = NULL;
-		adapter->phys_shmem = (dma_addr_t) NULL;
+		adapter->phys_shmem = (dma_addr_t)(unsigned long)NULL;
 	}
 
 	if (adapter->pingtimerset) {
@@ -2787,7 +2728,6 @@ static netdev_tx_t slic_xmit_start(struct sk_buff *skb, struct net_device *dev)
 	struct adapter *adapter = netdev_priv(dev);
 	struct slic_hostcmd *hcmd = NULL;
 	u32 status = 0;
-	u32 skbtype = NORMAL_ETHFRAME;
 	void *offloadcmd = NULL;
 
 	card = adapter->card;
@@ -2801,19 +2741,16 @@ static netdev_tx_t slic_xmit_start(struct sk_buff *skb, struct net_device *dev)
 		goto xmit_fail;
 	}
 
-	if (skbtype == NORMAL_ETHFRAME) {
-		hcmd = slic_cmdq_getfree(adapter);
-		if (!hcmd) {
-			adapter->xmitq_full = 1;
-			status = XMIT_FAIL_HOSTCMD_FAIL;
-			goto xmit_fail;
-		}
-		hcmd->skb = skb;
-		hcmd->busy = 1;
-		hcmd->type = SLIC_CMD_DUMB;
-		if (skbtype == NORMAL_ETHFRAME)
-			slic_xmit_build_request(adapter, hcmd, skb);
+	hcmd = slic_cmdq_getfree(adapter);
+	if (!hcmd) {
+		adapter->xmitq_full = 1;
+		status = XMIT_FAIL_HOSTCMD_FAIL;
+		goto xmit_fail;
 	}
+	hcmd->skb = skb;
+	hcmd->busy = 1;
+	hcmd->type = SLIC_CMD_DUMB;
+	slic_xmit_build_request(adapter, hcmd, skb);
 	dev->stats.tx_packets++;
 	dev->stats.tx_bytes += skb->len;
 
@@ -2839,7 +2776,7 @@ static netdev_tx_t slic_xmit_start(struct sk_buff *skb, struct net_device *dev)
 xmit_done:
 	return NETDEV_TX_OK;
 xmit_fail:
-	slic_xmit_fail(adapter, skb, offloadcmd, skbtype, status);
+	slic_xmit_fail(adapter, skb, offloadcmd, NORMAL_ETHFRAME, status);
 	goto xmit_done;
 }
 
@@ -2946,7 +2883,8 @@ static int slic_if_init(struct adapter *adapter)
 	mdelay(1);
 
 	if (!adapter->isp_initialized) {
-		pshmem = (struct slic_shmem *)adapter->phys_shmem;
+		pshmem = (struct slic_shmem *)(unsigned long)
+			 adapter->phys_shmem;
 
 		spin_lock_irqsave(&adapter->bit64reglock.lock,
 					adapter->bit64reglock.flags);
@@ -3211,6 +3149,7 @@ static int slic_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 			return -EFAULT;
 
 		if (ecmd.cmd == ETHTOOL_GSET) {
+			memset(&edata, 0, sizeof(edata));
 			edata.supported = (SUPPORTED_10baseT_Half |
 					   SUPPORTED_10baseT_Full |
 					   SUPPORTED_100baseT_Half |
@@ -3348,7 +3287,8 @@ static int slic_card_init(struct sliccard *card, struct adapter *adapter)
 		}
 		slic_reg32_write(&slic_regs->slic_icr, ICR_INT_OFF, FLUSH);
 		mdelay(1);
-		pshmem = (struct slic_shmem *)adapter->phys_shmem;
+		pshmem = (struct slic_shmem *)(unsigned long)
+			 adapter->phys_shmem;
 
 		spin_lock_irqsave(&adapter->bit64reglock.lock,
 					adapter->bit64reglock.flags);
@@ -3648,11 +3588,12 @@ static u32 slic_card_locate(struct adapter *adapter)
 
 	while (physcard) {
 		for (i = 0; i < SLIC_MAX_PORTS; i++) {
-			if (!physcard->adapter[i])
-				continue;
-			else
+			if (physcard->adapter[i])
 				break;
 		}
+		if (i == SLIC_MAX_PORTS)
+			break;
+
 		if (physcard->adapter[i]->slotnumber == adapter->slotnumber)
 			break;
 		physcard = physcard->next;
diff --git a/drivers/staging/sm7xxfb/sm7xxfb.c b/drivers/staging/sm7xxfb/sm7xxfb.c
index 0764bbbfd49..8add64b1cb0 100644
--- a/drivers/staging/sm7xxfb/sm7xxfb.c
+++ b/drivers/staging/sm7xxfb/sm7xxfb.c
@@ -1006,15 +1006,7 @@ static int smtcfb_pci_resume(struct device *device)
 	return 0;
 }
 
-static const struct dev_pm_ops sm7xx_pm_ops = {
-	.suspend = smtcfb_pci_suspend,
-	.resume = smtcfb_pci_resume,
-	.freeze = smtcfb_pci_suspend,
-	.thaw = smtcfb_pci_resume,
-	.poweroff = smtcfb_pci_suspend,
-	.restore = smtcfb_pci_resume,
-};
-
+static SIMPLE_DEV_PM_OPS(sm7xx_pm_ops, smtcfb_pci_suspend, smtcfb_pci_resume);
 #define SM7XX_PM_OPS (&sm7xx_pm_ops)
 
 #else  /* !CONFIG_PM */
diff --git a/drivers/staging/speakup/kobjects.c b/drivers/staging/speakup/kobjects.c
index 35f647ce1f1..d6d9264e4ca 100644
--- a/drivers/staging/speakup/kobjects.c
+++ b/drivers/staging/speakup/kobjects.c
@@ -617,9 +617,9 @@ ssize_t spk_var_store(struct kobject *kobj, struct kobj_attribute *attr,
 			len = E_INC;
 		else
 			len = E_SET;
-		speakup_s2i(cp, &value);
+		value = simple_strtol(cp, NULL, 10);
 		ret = spk_set_num_var(value, param, len);
-		if (ret == E_RANGE) {
+		if (ret == -ERANGE) {
 			var_data = param->data;
 			pr_warn("value for %s out of range, expect %d to %d\n",
 				attr->attr.name,
@@ -637,7 +637,7 @@ ssize_t spk_var_store(struct kobject *kobj, struct kobj_attribute *attr,
 		cp = (char *) buf;
 		cp[len] = '\0';
 		ret = spk_set_string_var(buf, param, len);
-		if (ret == E_TOOLONG)
+		if (ret == -E2BIG)
 			pr_warn("value too long for %s\n",
 					attr->attr.name);
 		break;
@@ -670,7 +670,7 @@ ssize_t spk_var_store(struct kobject *kobj, struct kobj_attribute *attr,
 	}
 	spk_unlock(flags);
 
-	if (ret == SET_DEFAULT)
+	if (ret == -ERESTART)
 		pr_info("%s reset to default value\n", attr->attr.name);
 	return count;
 }
diff --git a/drivers/staging/speakup/main.c b/drivers/staging/speakup/main.c
index 9916e94aa36..6c7b55c2947 100644
--- a/drivers/staging/speakup/main.c
+++ b/drivers/staging/speakup/main.c
@@ -1892,7 +1892,7 @@ oops:
 		spk_special_handler = NULL;
 		return 1;
 	}
-	cp = speakup_s2i(goto_buf, &go_pos);
+	go_pos = simple_strtol(goto_buf, &cp, 10);
 	goto_pos = (u_long) go_pos;
 	if (*cp == 'x') {
 		if (*goto_buf < '0')
diff --git a/drivers/staging/speakup/selection.c b/drivers/staging/speakup/selection.c
index 775af26b991..f0fb00392d6 100644
--- a/drivers/staging/speakup/selection.c
+++ b/drivers/staging/speakup/selection.c
@@ -14,7 +14,7 @@
 unsigned short spk_xs, spk_ys, spk_xe, spk_ye; /* our region points */
 
 /* Variables for selection control. */
-/* must not be disallocated */
+/* must not be deallocated */
 struct vc_data *spk_sel_cons;
 /* cleared by clear_selection */
 static int sel_start = -1;
diff --git a/drivers/staging/speakup/speakup.h b/drivers/staging/speakup/speakup.h
index 22f0fbb85f4..c387a02fc1c 100644
--- a/drivers/staging/speakup/speakup.h
+++ b/drivers/staging/speakup/speakup.h
@@ -44,11 +44,6 @@
 #define IS_CHAR(x, type) (spk_chartab[((u_char)x)]&type)
 #define IS_TYPE(x, type) ((spk_chartab[((u_char)x)]&type) == type)
 
-#define SET_DEFAULT -4
-#define E_RANGE -3
-#define E_TOOLONG -2
-#define E_UNDEF -1
-
 extern int speakup_thread(void *data);
 extern void spk_reset_default_chars(void);
 extern void spk_reset_default_chartab(void);
@@ -58,7 +53,6 @@ void spk_reset_index_count(int sc);
 void spk_get_index_count(int *linecount, int *sentcount);
 extern int spk_set_key_info(const u_char *key_info, u_char *k_buffer);
 extern char *spk_strlwr(char *s);
-extern char *speakup_s2i(char *start, int *dest);
 extern char *spk_s2uchar(char *start, char *dest);
 extern char *spk_xlate(char *s);
 extern int speakup_kobj_init(void);
diff --git a/drivers/staging/speakup/varhandlers.c b/drivers/staging/speakup/varhandlers.c
index f8c1e457d38..0099cb12e56 100644
--- a/drivers/staging/speakup/varhandlers.c
+++ b/drivers/staging/speakup/varhandlers.c
@@ -184,19 +184,19 @@ int spk_set_num_var(int input, struct st_var_header *var, int how)
 	char buf[32];
 	char *cp;
 	struct var_t *var_data = var->data;
+
 	if (var_data == NULL)
-		return E_UNDEF;
+		return -ENODATA;
 
 	if (how == E_NEW_DEFAULT) {
 		if (input < var_data->u.n.low || input > var_data->u.n.high)
-			ret = E_RANGE;
-		else
-			var_data->u.n.default_val = input;
-		return ret;
+			return -ERANGE;
+		var_data->u.n.default_val = input;
+		return 0;
 	}
 	if (how == E_DEFAULT) {
 		val = var_data->u.n.default_val;
-		ret = SET_DEFAULT;
+		ret = -ERESTART;
 	} else {
 		if (how == E_SET)
 			val = input;
@@ -207,7 +207,7 @@ int spk_set_num_var(int input, struct st_var_header *var, int how)
 		else if (how == E_DEC)
 			val -= input;
 		if (val < var_data->u.n.low || val > var_data->u.n.high)
-			return E_RANGE;
+			return -ERANGE;
 	}
 	var_data->u.n.value = val;
 	if (var->var_type == VAR_TIME && p_val != NULL) {
@@ -246,25 +246,25 @@ int spk_set_num_var(int input, struct st_var_header *var, int how)
 
 int spk_set_string_var(const char *page, struct st_var_header *var, int len)
 {
-	int ret = 0;
 	struct var_t *var_data = var->data;
+
 	if (var_data == NULL)
-		return E_UNDEF;
+		return -ENODATA;
 	if (len > MAXVARLEN)
-		return -E_TOOLONG;
+		return -E2BIG;
 	if (!len) {
 		if (!var_data->u.s.default_val)
 			return 0;
-		ret = SET_DEFAULT;
 		if (!var->p_val)
 			var->p_val = var_data->u.s.default_val;
 		if (var->p_val != var_data->u.s.default_val)
 			strcpy((char *)var->p_val, var_data->u.s.default_val);
+		return -ERESTART;
 	} else if (var->p_val)
 		strcpy((char *)var->p_val, page);
 	else
-		return -E_TOOLONG;
-	return ret;
+		return -E2BIG;
+	return 0;
 }
 
 /* spk_set_mask_bits sets or clears the punc/delim/repeat bits,
@@ -319,38 +319,10 @@ char *spk_strlwr(char *s)
 	return s;
 }
 
-char *speakup_s2i(char *start, int *dest)
-{
-	int val;
-	char ch = *start;
-	if (ch == '-' || ch == '+')
-		start++;
-	if (*start < '0' || *start > '9')
-		return start;
-	val = (*start) - '0';
-	start++;
-	while (*start >= '0' && *start <= '9') {
-		val *= 10;
-		val += (*start) - '0';
-		start++;
-	}
-	if (ch == '-')
-		*dest = -val;
-	else
-		*dest = val;
-	return start;
-}
-
 char *spk_s2uchar(char *start, char *dest)
 {
 	int val = 0;
-	while (*start && *start <= SPACE)
-		start++;
-	while (*start >= '0' && *start <= '9') {
-		val *= 10;
-		val += (*start) - '0';
-		start++;
-	}
+	val = simple_strtoul(skip_spaces(start), &start, 10);
 	if (*start == ',')
 		start++;
 	*dest = (u_char)val;
@@ -372,9 +344,9 @@ char *spk_xlate(char *s)
 			p1++;
 		} else if (*p1 >= '0' && *p1 <= '7') {
 			num = (*p1++)&7;
-			while (num < 256 && *p1 >= '0' && *p1 <= '7') {
+			while (num < 32 && *p1 >= '0' && *p1 <= '7') {
 				num <<= 3;
-				num = (*p1++)&7;
+				num += (*p1++)&7;
 			}
 			*p++ = num;
 		} else if (*p1 == 'x' &&
diff --git a/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c b/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c
index 6a21f67af08..a126b25c82d 100644
--- a/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c
+++ b/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c
@@ -649,7 +649,7 @@ static int synpatics_rmi4_touchpad_detect(struct synaptics_rmi4_data *pdata,
  *
  * This function calls to configures the rmi4 touchpad device
  */
-int synaptics_rmi4_touchpad_config(struct synaptics_rmi4_data *pdata,
+static int synaptics_rmi4_touchpad_config(struct synaptics_rmi4_data *pdata,
 						struct synaptics_rmi4_fn *rfi)
 {
 	/*
diff --git a/drivers/staging/omap-thermal/Kconfig b/drivers/staging/ti-soc-thermal/Kconfig
index 30cbc3bc8df..e81375fb215 100644
--- a/drivers/staging/omap-thermal/Kconfig
+++ b/drivers/staging/ti-soc-thermal/Kconfig
@@ -1,7 +1,7 @@
-config OMAP_BANDGAP
-	tristate "Texas Instruments OMAP4+ temperature sensor driver"
+config TI_SOC_THERMAL
+	tristate "Texas Instruments SoCs temperature sensor driver"
 	depends on THERMAL
-	depends on ARCH_OMAP4 || SOC_OMAP5
+	depends on ARCH_HAS_BANDGAP
 	help
 	  If you say yes here you get support for the Texas Instruments
 	  OMAP4460+ on die bandgap temperature sensor support. The register
@@ -10,18 +10,20 @@ config OMAP_BANDGAP
 	  This includes alert interrupts generation and also the TSHUT
 	  support.
 
-config OMAP_THERMAL
-	bool "Texas Instruments OMAP4+ thermal framework support"
-	depends on OMAP_BANDGAP
+config TI_THERMAL
+	bool "Texas Instruments SoCs thermal framework support"
+	depends on TI_SOC_THERMAL
 	depends on CPU_THERMAL
 	help
 	  If you say yes here you want to get support for generic thermal
-	  framework for the Texas Instruments OMAP4460+ on die bandgap
-	  temperature sensor.
+	  framework for the Texas Instruments on die bandgap temperature sensor.
+
+	  This includes trip points definitions, extrapolation rules and
+	  CPU cooling device bindings.
 
 config OMAP4_THERMAL
 	bool "Texas Instruments OMAP4 thermal support"
-	depends on OMAP_BANDGAP
+	depends on TI_SOC_THERMAL
 	depends on ARCH_OMAP4
 	help
 	  If you say yes here you get thermal support for the Texas Instruments
@@ -35,7 +37,7 @@ config OMAP4_THERMAL
 
 config OMAP5_THERMAL
 	bool "Texas Instruments OMAP5 thermal support"
-	depends on OMAP_BANDGAP
+	depends on TI_SOC_THERMAL
 	depends on SOC_OMAP5
 	help
 	  If you say yes here you get thermal support for the Texas Instruments
diff --git a/drivers/staging/ti-soc-thermal/Makefile b/drivers/staging/ti-soc-thermal/Makefile
new file mode 100644
index 00000000000..0ca034fb419
--- /dev/null
+++ b/drivers/staging/ti-soc-thermal/Makefile
@@ -0,0 +1,5 @@
+obj-$(CONFIG_TI_SOC_THERMAL)		+= ti-soc-thermal.o
+ti-soc-thermal-y			:= ti-bandgap.o
+ti-soc-thermal-$(CONFIG_TI_THERMAL)	+= ti-thermal-common.o
+ti-soc-thermal-$(CONFIG_OMAP4_THERMAL)	+= omap4-thermal-data.o
+ti-soc-thermal-$(CONFIG_OMAP5_THERMAL)	+= omap5-thermal-data.o
diff --git a/drivers/staging/ti-soc-thermal/TODO b/drivers/staging/ti-soc-thermal/TODO
new file mode 100644
index 00000000000..7da787d1924
--- /dev/null
+++ b/drivers/staging/ti-soc-thermal/TODO
@@ -0,0 +1,12 @@
+List of TODOs (by Eduardo Valentin)
+
+on ti-bandgap.c:
+- Revisit PM support
+
+on ti-thermal-common.c/ti-thermal.h:
+- Revisit need for locking
+
+generally:
+- make sure this code works on OMAP4430, OMAP4460 and OMAP5430
+
+Copy patches to Eduardo Valentin <eduardo.valentin@ti.com>
diff --git a/drivers/staging/omap-thermal/omap4-thermal.c b/drivers/staging/ti-soc-thermal/omap4-thermal-data.c
index 04c02b6c007..d255d33da9e 100644
--- a/drivers/staging/omap-thermal/omap4-thermal.c
+++ b/drivers/staging/ti-soc-thermal/omap4-thermal-data.c
@@ -16,8 +16,9 @@
  *
  */
 
-#include "omap-thermal.h"
-#include "omap-bandgap.h"
+#include "ti-thermal.h"
+#include "ti-bandgap.h"
+#include "omap4xxx-bandgap.h"
 
 /*
  * OMAP4430 has one instance of thermal sensor for MPU
@@ -44,8 +45,6 @@ static struct temp_sensor_data omap4430_mpu_temp_sensor_data = {
 	.max_temp = OMAP4430_MAX_TEMP,
 	.min_temp = OMAP4430_MIN_TEMP,
 	.hyst_val = OMAP4430_HYST_VAL,
-	.adc_start_val = OMAP4430_ADC_START_VALUE,
-	.adc_end_val = OMAP4430_ADC_END_VALUE,
 };
 
 /*
@@ -67,25 +66,28 @@ omap4430_adc_to_temp[OMAP4430_ADC_END_VALUE - OMAP4430_ADC_START_VALUE + 1] = {
 };
 
 /* OMAP4430 data */
-const struct omap_bandgap_data omap4430_data = {
-	.features = OMAP_BANDGAP_FEATURE_MODE_CONFIG |
-			OMAP_BANDGAP_FEATURE_POWER_SWITCH,
+const struct ti_bandgap_data omap4430_data = {
+	.features = TI_BANDGAP_FEATURE_MODE_CONFIG |
+			TI_BANDGAP_FEATURE_CLK_CTRL |
+			TI_BANDGAP_FEATURE_POWER_SWITCH,
 	.fclock_name = "bandgap_fclk",
 	.div_ck_name = "bandgap_fclk",
 	.conv_table = omap4430_adc_to_temp,
-	.expose_sensor = omap_thermal_expose_sensor,
-	.remove_sensor = omap_thermal_remove_sensor,
+	.adc_start_val = OMAP4430_ADC_START_VALUE,
+	.adc_end_val = OMAP4430_ADC_END_VALUE,
+	.expose_sensor = ti_thermal_expose_sensor,
+	.remove_sensor = ti_thermal_remove_sensor,
 	.sensors = {
 		{
 		.registers = &omap4430_mpu_temp_sensor_registers,
 		.ts_data = &omap4430_mpu_temp_sensor_data,
 		.domain = "cpu",
-		.slope = 0,
-		.constant = 20000,
-		.slope_pcb = 0,
-		.constant_pcb = 20000,
-		.register_cooling = omap_thermal_register_cpu_cooling,
-		.unregister_cooling = omap_thermal_unregister_cpu_cooling,
+		.slope = OMAP_GRADIENT_SLOPE_4430,
+		.constant = OMAP_GRADIENT_CONST_4430,
+		.slope_pcb = OMAP_GRADIENT_SLOPE_W_PCB_4430,
+		.constant_pcb = OMAP_GRADIENT_CONST_W_PCB_4430,
+		.register_cooling = ti_thermal_register_cpu_cooling,
+		.unregister_cooling = ti_thermal_unregister_cpu_cooling,
 		},
 	},
 	.sensor_count = 1,
@@ -140,8 +142,6 @@ static struct temp_sensor_data omap4460_mpu_temp_sensor_data = {
 	.max_temp = OMAP4460_MAX_TEMP,
 	.min_temp = OMAP4460_MIN_TEMP,
 	.hyst_val = OMAP4460_HYST_VAL,
-	.adc_start_val = OMAP4460_ADC_START_VALUE,
-	.adc_end_val = OMAP4460_ADC_END_VALUE,
 	.update_int1 = 1000,
 	.update_int2 = 2000,
 };
@@ -201,18 +201,22 @@ omap4460_adc_to_temp[OMAP4460_ADC_END_VALUE - OMAP4460_ADC_START_VALUE + 1] = {
 };
 
 /* OMAP4460 data */
-const struct omap_bandgap_data omap4460_data = {
-	.features = OMAP_BANDGAP_FEATURE_TSHUT |
-			OMAP_BANDGAP_FEATURE_TSHUT_CONFIG |
-			OMAP_BANDGAP_FEATURE_TALERT |
-			OMAP_BANDGAP_FEATURE_MODE_CONFIG |
-			OMAP_BANDGAP_FEATURE_POWER_SWITCH |
-			OMAP_BANDGAP_FEATURE_COUNTER,
+const struct ti_bandgap_data omap4460_data = {
+	.features = TI_BANDGAP_FEATURE_TSHUT |
+			TI_BANDGAP_FEATURE_TSHUT_CONFIG |
+			TI_BANDGAP_FEATURE_TALERT |
+			TI_BANDGAP_FEATURE_MODE_CONFIG |
+			TI_BANDGAP_FEATURE_POWER_SWITCH |
+			TI_BANDGAP_FEATURE_CLK_CTRL |
+			TI_BANDGAP_FEATURE_COUNTER,
 	.fclock_name = "bandgap_ts_fclk",
 	.div_ck_name = "div_ts_ck",
 	.conv_table = omap4460_adc_to_temp,
-	.expose_sensor = omap_thermal_expose_sensor,
-	.remove_sensor = omap_thermal_remove_sensor,
+	.adc_start_val = OMAP4460_ADC_START_VALUE,
+	.adc_end_val = OMAP4460_ADC_END_VALUE,
+	.expose_sensor = ti_thermal_expose_sensor,
+	.remove_sensor = ti_thermal_remove_sensor,
+	.report_temperature = ti_thermal_report_sensor_temperature,
 	.sensors = {
 		{
 		.registers = &omap4460_mpu_temp_sensor_registers,
@@ -222,26 +226,30 @@ const struct omap_bandgap_data omap4460_data = {
 		.constant = OMAP_GRADIENT_CONST_4460,
 		.slope_pcb = OMAP_GRADIENT_SLOPE_W_PCB_4460,
 		.constant_pcb = OMAP_GRADIENT_CONST_W_PCB_4460,
-		.register_cooling = omap_thermal_register_cpu_cooling,
-		.unregister_cooling = omap_thermal_unregister_cpu_cooling,
+		.register_cooling = ti_thermal_register_cpu_cooling,
+		.unregister_cooling = ti_thermal_unregister_cpu_cooling,
 		},
 	},
 	.sensor_count = 1,
 };
 
 /* OMAP4470 data */
-const struct omap_bandgap_data omap4470_data = {
-	.features = OMAP_BANDGAP_FEATURE_TSHUT |
-			OMAP_BANDGAP_FEATURE_TSHUT_CONFIG |
-			OMAP_BANDGAP_FEATURE_TALERT |
-			OMAP_BANDGAP_FEATURE_MODE_CONFIG |
-			OMAP_BANDGAP_FEATURE_POWER_SWITCH |
-			OMAP_BANDGAP_FEATURE_COUNTER,
+const struct ti_bandgap_data omap4470_data = {
+	.features = TI_BANDGAP_FEATURE_TSHUT |
+			TI_BANDGAP_FEATURE_TSHUT_CONFIG |
+			TI_BANDGAP_FEATURE_TALERT |
+			TI_BANDGAP_FEATURE_MODE_CONFIG |
+			TI_BANDGAP_FEATURE_POWER_SWITCH |
+			TI_BANDGAP_FEATURE_CLK_CTRL |
+			TI_BANDGAP_FEATURE_COUNTER,
 	.fclock_name = "bandgap_ts_fclk",
 	.div_ck_name = "div_ts_ck",
 	.conv_table = omap4460_adc_to_temp,
-	.expose_sensor = omap_thermal_expose_sensor,
-	.remove_sensor = omap_thermal_remove_sensor,
+	.adc_start_val = OMAP4460_ADC_START_VALUE,
+	.adc_end_val = OMAP4460_ADC_END_VALUE,
+	.expose_sensor = ti_thermal_expose_sensor,
+	.remove_sensor = ti_thermal_remove_sensor,
+	.report_temperature = ti_thermal_report_sensor_temperature,
 	.sensors = {
 		{
 		.registers = &omap4460_mpu_temp_sensor_registers,
@@ -251,8 +259,8 @@ const struct omap_bandgap_data omap4470_data = {
 		.constant = OMAP_GRADIENT_CONST_4470,
 		.slope_pcb = OMAP_GRADIENT_SLOPE_W_PCB_4470,
 		.constant_pcb = OMAP_GRADIENT_CONST_W_PCB_4470,
-		.register_cooling = omap_thermal_register_cpu_cooling,
-		.unregister_cooling = omap_thermal_unregister_cpu_cooling,
+		.register_cooling = ti_thermal_register_cpu_cooling,
+		.unregister_cooling = ti_thermal_unregister_cpu_cooling,
 		},
 	},
 	.sensor_count = 1,
diff --git a/drivers/staging/ti-soc-thermal/omap4xxx-bandgap.h b/drivers/staging/ti-soc-thermal/omap4xxx-bandgap.h
new file mode 100644
index 00000000000..6f2de3a3356
--- /dev/null
+++ b/drivers/staging/ti-soc-thermal/omap4xxx-bandgap.h
@@ -0,0 +1,175 @@
+/*
+ * OMAP4xxx bandgap registers, bitfields and temperature definitions
+ *
+ * Copyright (C) 2013 Texas Instruments Incorporated - http://www.ti.com/
+ * Contact:
+ *   Eduardo Valentin <eduardo.valentin@ti.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 __OMAP4XXX_BANDGAP_H
+#define __OMAP4XXX_BANDGAP_H
+
+/**
+ * *** OMAP4430 ***
+ *
+ * Below, in sequence, are the Register definitions,
+ * the bitfields and the temperature definitions for OMAP4430.
+ */
+
+/**
+ * OMAP4430 register definitions
+ *
+ * Registers are defined as offsets. The offsets are
+ * relative to FUSE_OPP_BGAP on 4430.
+ */
+
+/* OMAP4430.FUSE_OPP_BGAP */
+#define OMAP4430_FUSE_OPP_BGAP				0x0
+
+/* OMAP4430.TEMP_SENSOR  */
+#define OMAP4430_TEMP_SENSOR_CTRL_OFFSET		0xCC
+
+/**
+ * Register and bit definitions for OMAP4430
+ *
+ * All the macros bellow define the required bits for
+ * controlling temperature on OMAP4430. Bit defines are
+ * grouped by register.
+ */
+
+/* OMAP4430.TEMP_SENSOR bits */
+#define OMAP4430_BGAP_TEMPSOFF_MASK			BIT(12)
+#define OMAP4430_BGAP_TSHUT_MASK			BIT(11)
+#define OMAP4430_SINGLE_MODE_MASK			BIT(10)
+#define OMAP4430_BGAP_TEMP_SENSOR_SOC_MASK		BIT(9)
+#define OMAP4430_BGAP_TEMP_SENSOR_EOCZ_MASK		BIT(8)
+#define OMAP4430_BGAP_TEMP_SENSOR_DTEMP_MASK		(0xff << 0)
+
+/**
+ * Temperature limits and thresholds for OMAP4430
+ *
+ * All the macros bellow are definitions for handling the
+ * ADC conversions and representation of temperature limits
+ * and thresholds for OMAP4430.
+ */
+
+/* ADC conversion table limits */
+#define OMAP4430_ADC_START_VALUE			0
+#define OMAP4430_ADC_END_VALUE				127
+/* bandgap clock limits (no control on 4430) */
+#define OMAP4430_MAX_FREQ				32768
+#define OMAP4430_MIN_FREQ				32768
+/* sensor limits */
+#define OMAP4430_MIN_TEMP				-40000
+#define OMAP4430_MAX_TEMP				125000
+#define OMAP4430_HYST_VAL				5000
+
+/**
+ * *** OMAP4460 *** Applicable for OMAP4470
+ *
+ * Below, in sequence, are the Register definitions,
+ * the bitfields and the temperature definitions for OMAP4460.
+ */
+
+/**
+ * OMAP4460 register definitions
+ *
+ * Registers are defined as offsets. The offsets are
+ * relative to FUSE_OPP_BGAP on 4460.
+ */
+
+/* OMAP4460.FUSE_OPP_BGAP */
+#define OMAP4460_FUSE_OPP_BGAP				0x0
+
+/* OMAP4460.TEMP_SENSOR */
+#define OMAP4460_TEMP_SENSOR_CTRL_OFFSET		0xCC
+
+/* OMAP4460.BANDGAP_CTRL */
+#define OMAP4460_BGAP_CTRL_OFFSET			0x118
+
+/* OMAP4460.BANDGAP_COUNTER */
+#define OMAP4460_BGAP_COUNTER_OFFSET			0x11C
+
+/* OMAP4460.BANDGAP_THRESHOLD */
+#define OMAP4460_BGAP_THRESHOLD_OFFSET			0x120
+
+/* OMAP4460.TSHUT_THRESHOLD */
+#define OMAP4460_BGAP_TSHUT_OFFSET			0x124
+
+/* OMAP4460.BANDGAP_STATUS */
+#define OMAP4460_BGAP_STATUS_OFFSET			0x128
+
+/**
+ * Register bitfields for OMAP4460
+ *
+ * All the macros bellow define the required bits for
+ * controlling temperature on OMAP4460. Bit defines are
+ * grouped by register.
+ */
+/* OMAP4460.TEMP_SENSOR bits */
+#define OMAP4460_BGAP_TEMPSOFF_MASK			BIT(13)
+#define OMAP4460_BGAP_TEMP_SENSOR_SOC_MASK		BIT(11)
+#define OMAP4460_BGAP_TEMP_SENSOR_EOCZ_MASK		BIT(10)
+#define OMAP4460_BGAP_TEMP_SENSOR_DTEMP_MASK		(0x3ff << 0)
+
+/* OMAP4460.BANDGAP_CTRL bits */
+#define OMAP4460_SINGLE_MODE_MASK			BIT(31)
+#define OMAP4460_MASK_HOT_MASK				BIT(1)
+#define OMAP4460_MASK_COLD_MASK				BIT(0)
+
+/* OMAP4460.BANDGAP_COUNTER bits */
+#define OMAP4460_COUNTER_MASK				(0xffffff << 0)
+
+/* OMAP4460.BANDGAP_THRESHOLD bits */
+#define OMAP4460_T_HOT_MASK				(0x3ff << 16)
+#define OMAP4460_T_COLD_MASK				(0x3ff << 0)
+
+/* OMAP4460.TSHUT_THRESHOLD bits */
+#define OMAP4460_TSHUT_HOT_MASK				(0x3ff << 16)
+#define OMAP4460_TSHUT_COLD_MASK			(0x3ff << 0)
+
+/* OMAP4460.BANDGAP_STATUS bits */
+#define OMAP4460_CLEAN_STOP_MASK			BIT(3)
+#define OMAP4460_BGAP_ALERT_MASK			BIT(2)
+#define OMAP4460_HOT_FLAG_MASK				BIT(1)
+#define OMAP4460_COLD_FLAG_MASK				BIT(0)
+
+/**
+ * Temperature limits and thresholds for OMAP4460
+ *
+ * All the macros bellow are definitions for handling the
+ * ADC conversions and representation of temperature limits
+ * and thresholds for OMAP4460.
+ */
+
+/* ADC conversion table limits */
+#define OMAP4460_ADC_START_VALUE			530
+#define OMAP4460_ADC_END_VALUE				932
+/* bandgap clock limits */
+#define OMAP4460_MAX_FREQ				1500000
+#define OMAP4460_MIN_FREQ				1000000
+/* sensor limits */
+#define OMAP4460_MIN_TEMP				-40000
+#define OMAP4460_MAX_TEMP				123000
+#define OMAP4460_HYST_VAL				5000
+/* interrupts thresholds */
+#define OMAP4460_TSHUT_HOT				900	/* 122 deg C */
+#define OMAP4460_TSHUT_COLD				895	/* 100 deg C */
+#define OMAP4460_T_HOT					800	/* 73 deg C */
+#define OMAP4460_T_COLD					795	/* 71 deg C */
+
+#endif /* __OMAP4XXX_BANDGAP_H */
diff --git a/drivers/staging/ti-soc-thermal/omap5-thermal-data.c b/drivers/staging/ti-soc-thermal/omap5-thermal-data.c
new file mode 100644
index 00000000000..eff0c80fd4a
--- /dev/null
+++ b/drivers/staging/ti-soc-thermal/omap5-thermal-data.c
@@ -0,0 +1,359 @@
+/*
+ * OMAP5 thermal driver.
+ *
+ * Copyright (C) 2011-2012 Texas Instruments Inc.
+ * Contact:
+ *	Eduardo Valentin <eduardo.valentin@ti.com>
+ *
+ * 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 "ti-thermal.h"
+#include "ti-bandgap.h"
+#include "omap5xxx-bandgap.h"
+
+/*
+ * OMAP5430 has three instances of thermal sensor for MPU, GPU & CORE,
+ * need to describe the individual registers and bit fields.
+ */
+
+/*
+ * OMAP5430 MPU thermal sensor register offset and bit-fields
+ */
+static struct temp_sensor_registers
+omap5430_mpu_temp_sensor_registers = {
+	.temp_sensor_ctrl = OMAP5430_TEMP_SENSOR_MPU_OFFSET,
+	.bgap_tempsoff_mask = OMAP5430_BGAP_TEMPSOFF_MASK,
+	.bgap_eocz_mask = OMAP5430_BGAP_TEMP_SENSOR_EOCZ_MASK,
+	.bgap_dtemp_mask = OMAP5430_BGAP_TEMP_SENSOR_DTEMP_MASK,
+
+	.bgap_mask_ctrl = OMAP5430_BGAP_CTRL_OFFSET,
+	.mask_hot_mask = OMAP5430_MASK_HOT_MPU_MASK,
+	.mask_cold_mask = OMAP5430_MASK_COLD_MPU_MASK,
+	.mask_sidlemode_mask = OMAP5430_MASK_SIDLEMODE_MASK,
+	.mask_counter_delay_mask = OMAP5430_MASK_COUNTER_DELAY_MASK,
+	.mask_freeze_mask = OMAP5430_MASK_FREEZE_MPU_MASK,
+	.mask_clear_mask = OMAP5430_MASK_CLEAR_MPU_MASK,
+	.mask_clear_accum_mask = OMAP5430_MASK_CLEAR_ACCUM_MPU_MASK,
+
+
+	.bgap_counter = OMAP5430_BGAP_CTRL_OFFSET,
+	.counter_mask = OMAP5430_COUNTER_MASK,
+
+	.bgap_threshold = OMAP5430_BGAP_THRESHOLD_MPU_OFFSET,
+	.threshold_thot_mask = OMAP5430_T_HOT_MASK,
+	.threshold_tcold_mask = OMAP5430_T_COLD_MASK,
+
+	.tshut_threshold = OMAP5430_BGAP_TSHUT_MPU_OFFSET,
+	.tshut_hot_mask = OMAP5430_TSHUT_HOT_MASK,
+	.tshut_cold_mask = OMAP5430_TSHUT_COLD_MASK,
+
+	.bgap_status = OMAP5430_BGAP_STATUS_OFFSET,
+	.status_clean_stop_mask = 0x0,
+	.status_bgap_alert_mask = OMAP5430_BGAP_ALERT_MASK,
+	.status_hot_mask = OMAP5430_HOT_MPU_FLAG_MASK,
+	.status_cold_mask = OMAP5430_COLD_MPU_FLAG_MASK,
+
+	.bgap_cumul_dtemp = OMAP5430_BGAP_CUMUL_DTEMP_MPU_OFFSET,
+	.ctrl_dtemp_0 = OMAP5430_BGAP_DTEMP_MPU_0_OFFSET,
+	.ctrl_dtemp_1 = OMAP5430_BGAP_DTEMP_MPU_1_OFFSET,
+	.ctrl_dtemp_2 = OMAP5430_BGAP_DTEMP_MPU_2_OFFSET,
+	.ctrl_dtemp_3 = OMAP5430_BGAP_DTEMP_MPU_3_OFFSET,
+	.ctrl_dtemp_4 = OMAP5430_BGAP_DTEMP_MPU_4_OFFSET,
+	.bgap_efuse = OMAP5430_FUSE_OPP_BGAP_MPU,
+};
+
+/*
+ * OMAP5430 GPU thermal sensor register offset and bit-fields
+ */
+static struct temp_sensor_registers
+omap5430_gpu_temp_sensor_registers = {
+	.temp_sensor_ctrl = OMAP5430_TEMP_SENSOR_GPU_OFFSET,
+	.bgap_tempsoff_mask = OMAP5430_BGAP_TEMPSOFF_MASK,
+	.bgap_eocz_mask = OMAP5430_BGAP_TEMP_SENSOR_EOCZ_MASK,
+	.bgap_dtemp_mask = OMAP5430_BGAP_TEMP_SENSOR_DTEMP_MASK,
+
+	.bgap_mask_ctrl = OMAP5430_BGAP_CTRL_OFFSET,
+	.mask_hot_mask = OMAP5430_MASK_HOT_GPU_MASK,
+	.mask_cold_mask = OMAP5430_MASK_COLD_GPU_MASK,
+	.mask_sidlemode_mask = OMAP5430_MASK_SIDLEMODE_MASK,
+	.mask_counter_delay_mask = OMAP5430_MASK_COUNTER_DELAY_MASK,
+	.mask_freeze_mask = OMAP5430_MASK_FREEZE_GPU_MASK,
+	.mask_clear_mask = OMAP5430_MASK_CLEAR_GPU_MASK,
+	.mask_clear_accum_mask = OMAP5430_MASK_CLEAR_ACCUM_GPU_MASK,
+
+	.bgap_counter = OMAP5430_BGAP_CTRL_OFFSET,
+	.counter_mask = OMAP5430_COUNTER_MASK,
+
+	.bgap_threshold = OMAP5430_BGAP_THRESHOLD_GPU_OFFSET,
+	.threshold_thot_mask = OMAP5430_T_HOT_MASK,
+	.threshold_tcold_mask = OMAP5430_T_COLD_MASK,
+
+	.tshut_threshold = OMAP5430_BGAP_TSHUT_GPU_OFFSET,
+	.tshut_hot_mask = OMAP5430_TSHUT_HOT_MASK,
+	.tshut_cold_mask = OMAP5430_TSHUT_COLD_MASK,
+
+	.bgap_status = OMAP5430_BGAP_STATUS_OFFSET,
+	.status_clean_stop_mask = 0x0,
+	.status_bgap_alert_mask = OMAP5430_BGAP_ALERT_MASK,
+	.status_hot_mask = OMAP5430_HOT_GPU_FLAG_MASK,
+	.status_cold_mask = OMAP5430_COLD_GPU_FLAG_MASK,
+
+	.bgap_cumul_dtemp = OMAP5430_BGAP_CUMUL_DTEMP_GPU_OFFSET,
+	.ctrl_dtemp_0 = OMAP5430_BGAP_DTEMP_GPU_0_OFFSET,
+	.ctrl_dtemp_1 = OMAP5430_BGAP_DTEMP_GPU_1_OFFSET,
+	.ctrl_dtemp_2 = OMAP5430_BGAP_DTEMP_GPU_2_OFFSET,
+	.ctrl_dtemp_3 = OMAP5430_BGAP_DTEMP_GPU_3_OFFSET,
+	.ctrl_dtemp_4 = OMAP5430_BGAP_DTEMP_GPU_4_OFFSET,
+
+	.bgap_efuse = OMAP5430_FUSE_OPP_BGAP_GPU,
+};
+
+/*
+ * OMAP5430 CORE thermal sensor register offset and bit-fields
+ */
+static struct temp_sensor_registers
+omap5430_core_temp_sensor_registers = {
+	.temp_sensor_ctrl = OMAP5430_TEMP_SENSOR_CORE_OFFSET,
+	.bgap_tempsoff_mask = OMAP5430_BGAP_TEMPSOFF_MASK,
+	.bgap_eocz_mask = OMAP5430_BGAP_TEMP_SENSOR_EOCZ_MASK,
+	.bgap_dtemp_mask = OMAP5430_BGAP_TEMP_SENSOR_DTEMP_MASK,
+
+	.bgap_mask_ctrl = OMAP5430_BGAP_CTRL_OFFSET,
+	.mask_hot_mask = OMAP5430_MASK_HOT_CORE_MASK,
+	.mask_cold_mask = OMAP5430_MASK_COLD_CORE_MASK,
+	.mask_sidlemode_mask = OMAP5430_MASK_SIDLEMODE_MASK,
+	.mask_counter_delay_mask = OMAP5430_MASK_COUNTER_DELAY_MASK,
+	.mask_freeze_mask = OMAP5430_MASK_FREEZE_CORE_MASK,
+	.mask_clear_mask = OMAP5430_MASK_CLEAR_CORE_MASK,
+	.mask_clear_accum_mask = OMAP5430_MASK_CLEAR_ACCUM_CORE_MASK,
+
+	.bgap_counter = OMAP5430_BGAP_CTRL_OFFSET,
+	.counter_mask = OMAP5430_COUNTER_MASK,
+
+	.bgap_threshold = OMAP5430_BGAP_THRESHOLD_CORE_OFFSET,
+	.threshold_thot_mask = OMAP5430_T_HOT_MASK,
+	.threshold_tcold_mask = OMAP5430_T_COLD_MASK,
+
+	.tshut_threshold = OMAP5430_BGAP_TSHUT_CORE_OFFSET,
+	.tshut_hot_mask = OMAP5430_TSHUT_HOT_MASK,
+	.tshut_cold_mask = OMAP5430_TSHUT_COLD_MASK,
+
+	.bgap_status = OMAP5430_BGAP_STATUS_OFFSET,
+	.status_clean_stop_mask = 0x0,
+	.status_bgap_alert_mask = OMAP5430_BGAP_ALERT_MASK,
+	.status_hot_mask = OMAP5430_HOT_CORE_FLAG_MASK,
+	.status_cold_mask = OMAP5430_COLD_CORE_FLAG_MASK,
+
+	.bgap_cumul_dtemp = OMAP5430_BGAP_CUMUL_DTEMP_CORE_OFFSET,
+	.ctrl_dtemp_0 = OMAP5430_BGAP_DTEMP_CORE_0_OFFSET,
+	.ctrl_dtemp_1 = OMAP5430_BGAP_DTEMP_CORE_1_OFFSET,
+	.ctrl_dtemp_2 = OMAP5430_BGAP_DTEMP_CORE_2_OFFSET,
+	.ctrl_dtemp_3 = OMAP5430_BGAP_DTEMP_CORE_3_OFFSET,
+	.ctrl_dtemp_4 = OMAP5430_BGAP_DTEMP_CORE_4_OFFSET,
+
+	.bgap_efuse = OMAP5430_FUSE_OPP_BGAP_CORE,
+};
+
+/* Thresholds and limits for OMAP5430 MPU temperature sensor */
+static struct temp_sensor_data omap5430_mpu_temp_sensor_data = {
+	.tshut_hot = OMAP5430_MPU_TSHUT_HOT,
+	.tshut_cold = OMAP5430_MPU_TSHUT_COLD,
+	.t_hot = OMAP5430_MPU_T_HOT,
+	.t_cold = OMAP5430_MPU_T_COLD,
+	.min_freq = OMAP5430_MPU_MIN_FREQ,
+	.max_freq = OMAP5430_MPU_MAX_FREQ,
+	.max_temp = OMAP5430_MPU_MAX_TEMP,
+	.min_temp = OMAP5430_MPU_MIN_TEMP,
+	.hyst_val = OMAP5430_MPU_HYST_VAL,
+	.update_int1 = 1000,
+	.update_int2 = 2000,
+};
+
+/* Thresholds and limits for OMAP5430 GPU temperature sensor */
+static struct temp_sensor_data omap5430_gpu_temp_sensor_data = {
+	.tshut_hot = OMAP5430_GPU_TSHUT_HOT,
+	.tshut_cold = OMAP5430_GPU_TSHUT_COLD,
+	.t_hot = OMAP5430_GPU_T_HOT,
+	.t_cold = OMAP5430_GPU_T_COLD,
+	.min_freq = OMAP5430_GPU_MIN_FREQ,
+	.max_freq = OMAP5430_GPU_MAX_FREQ,
+	.max_temp = OMAP5430_GPU_MAX_TEMP,
+	.min_temp = OMAP5430_GPU_MIN_TEMP,
+	.hyst_val = OMAP5430_GPU_HYST_VAL,
+	.update_int1 = 1000,
+	.update_int2 = 2000,
+};
+
+/* Thresholds and limits for OMAP5430 CORE temperature sensor */
+static struct temp_sensor_data omap5430_core_temp_sensor_data = {
+	.tshut_hot = OMAP5430_CORE_TSHUT_HOT,
+	.tshut_cold = OMAP5430_CORE_TSHUT_COLD,
+	.t_hot = OMAP5430_CORE_T_HOT,
+	.t_cold = OMAP5430_CORE_T_COLD,
+	.min_freq = OMAP5430_CORE_MIN_FREQ,
+	.max_freq = OMAP5430_CORE_MAX_FREQ,
+	.max_temp = OMAP5430_CORE_MAX_TEMP,
+	.min_temp = OMAP5430_CORE_MIN_TEMP,
+	.hyst_val = OMAP5430_CORE_HYST_VAL,
+	.update_int1 = 1000,
+	.update_int2 = 2000,
+};
+
+/*
+ * OMAP54xx ES2.0 : Temperature values in milli degree celsius
+ * ADC code values from 540 to 945
+ */
+static int
+omap5430_adc_to_temp[
+	OMAP5430_ADC_END_VALUE - OMAP5430_ADC_START_VALUE + 1] = {
+	/* Index 540 - 549 */
+	-40000, -40000, -40000, -40000, -39800, -39400, -39000, -38600, -38200,
+	-37800,
+	/* Index 550 - 559 */
+	-37400, -37000, -36600, -36200, -35800, -35300, -34700, -34200, -33800,
+	-33400,
+	/* Index 560 - 569 */
+	-33000, -32600, -32200, -31800, -31400, -31000, -30600, -30200, -29800,
+	-29400,
+	/* Index 570 - 579 */
+	-29000, -28600, -28200, -27700, -27100, -26600, -26200, -25800, -25400,
+	-25000,
+	/* Index 580 - 589 */
+	-24600, -24200, -23800, -23400, -23000, -22600, -22200, -21600, -21400,
+	-21000,
+	/* Index 590 - 599 */
+	-20500, -19900, -19400, -19000, -18600, -18200, -17800, -17400, -17000,
+	-16600,
+	/* Index 600 - 609 */
+	-16200, -15800, -15400, -15000, -14600, -14200, -13800,	-13400, -13000,
+	-12500,
+	/* Index 610 - 619 */
+	-11900, -11400, -11000, -10600, -10200, -9800, -9400, -9000, -8600,
+	-8200,
+	/* Index 620 - 629 */
+	-7800, -7400, -7000, -6600, -6200, -5800, -5400, -5000, -4500, -3900,
+	/* Index 630 - 639 */
+	-3400, -3000, -2600, -2200, -1800, -1400, -1000, -600, -200, 200,
+	/* Index 640 - 649 */
+	600, 1000, 1400, 1800, 2200, 2600, 3000, 3400, 3900, 4500,
+	/* Index 650 - 659 */
+	5000, 5400, 5800, 6200, 6600, 7000, 7400, 7800, 8200, 8600,
+	/* Index 660 - 669 */
+	9000, 9400, 9800, 10200, 10600, 11000, 11400, 11800, 12200, 12700,
+	/* Index 670 - 679 */
+	13300, 13800, 14200, 14600, 15000, 15400, 15800, 16200, 16600, 17000,
+	/* Index 680 - 689 */
+	17400, 17800, 18200, 18600, 19000, 19400, 19800, 20200, 20600, 21100,
+	/* Index 690 - 699 */
+	21400, 21900, 22500, 23000, 23400, 23800, 24200, 24600, 25000, 25400,
+	/* Index 700 - 709 */
+	25800, 26200, 26600, 27000, 27400, 27800, 28200, 28600, 29000, 29400,
+	/* Index 710 - 719 */
+	29800, 30200, 30600, 31000, 31400, 31900, 32500, 33000, 33400, 33800,
+	/* Index 720 - 729 */
+	34200, 34600, 35000, 35400, 35800, 36200, 36600, 37000, 37400, 37800,
+	/* Index 730 - 739 */
+	38200, 38600, 39000, 39400, 39800, 40200, 40600, 41000, 41400, 41800,
+	/* Index 740 - 749 */
+	42200, 42600, 43100, 43700, 44200, 44600, 45000, 45400, 45800, 46200,
+	/* Index 750 - 759 */
+	46600, 47000, 47400, 47800, 48200, 48600, 49000, 49400, 49800, 50200,
+	/* Index 760 - 769 */
+	50600, 51000, 51400, 51800, 52200, 52600, 53000, 53400, 53800, 54200,
+	/* Index 770 - 779 */
+	54600, 55000, 55400, 55900, 56500, 57000, 57400, 57800, 58200, 58600,
+	/* Index 780 - 789 */
+	59000, 59400, 59800, 60200, 60600, 61000, 61400, 61800, 62200, 62600,
+	/* Index 790 - 799 */
+	63000, 63400, 63800, 64200, 64600, 65000, 65400, 65800, 66200, 66600,
+	/* Index 800 - 809 */
+	67000, 67400, 67800, 68200, 68600, 69000, 69400, 69800, 70200, 70600,
+	/* Index 810 - 819 */
+	71000, 71500, 72100, 72600, 73000, 73400, 73800, 74200, 74600, 75000,
+	/* Index 820 - 829 */
+	75400, 75800, 76200, 76600, 77000, 77400, 77800, 78200, 78600, 79000,
+	/* Index 830 - 839 */
+	79400, 79800, 80200, 80600, 81000, 81400, 81800, 82200, 82600, 83000,
+	/* Index 840 - 849 */
+	83400, 83800, 84200, 84600, 85000, 85400, 85800, 86200, 86600, 87000,
+	/* Index 850 - 859 */
+	87400, 87800, 88200, 88600, 89000, 89400, 89800, 90200, 90600, 91000,
+	/* Index 860 - 869 */
+	91400, 91800, 92200, 92600, 93000, 93400, 93800, 94200, 94600, 95000,
+	/* Index 870 - 879 */
+	95400, 95800, 96200, 96600, 97000, 97500, 98100, 98600, 99000, 99400,
+	/* Index 880 - 889 */
+	99800, 100200, 100600, 101000, 101400, 101800, 102200, 102600, 103000,
+	103400,
+	/* Index 890 - 899 */
+	103800, 104200, 104600, 105000, 105400, 105800, 106200, 106600, 107000,
+	107400,
+	/* Index 900 - 909 */
+	107800, 108200, 108600, 109000, 109400, 109800, 110200, 110600, 111000,
+	111400,
+	/* Index 910 - 919 */
+	111800, 112200, 112600, 113000, 113400, 113800, 114200, 114600, 115000,
+	115400,
+	/* Index 920 - 929 */
+	115800, 116200, 116600, 117000, 117400, 117800, 118200, 118600, 119000,
+	119400,
+	/* Index 930 - 939 */
+	119800, 120200, 120600, 121000, 121400, 121800, 122400, 122600, 123000,
+	123400,
+	/* Index 940 - 945 */
+	123800, 1242000, 124600, 124900, 125000, 125000,
+};
+
+/* OMAP54xx ES2.0 data */
+const struct ti_bandgap_data omap5430_data = {
+	.features = TI_BANDGAP_FEATURE_TSHUT_CONFIG |
+			TI_BANDGAP_FEATURE_FREEZE_BIT |
+			TI_BANDGAP_FEATURE_TALERT |
+			TI_BANDGAP_FEATURE_COUNTER_DELAY |
+			TI_BANDGAP_FEATURE_HISTORY_BUFFER,
+	.fclock_name = "l3instr_ts_gclk_div",
+	.div_ck_name = "l3instr_ts_gclk_div",
+	.conv_table = omap5430_adc_to_temp,
+	.adc_start_val = OMAP5430_ADC_START_VALUE,
+	.adc_end_val = OMAP5430_ADC_END_VALUE,
+	.expose_sensor = ti_thermal_expose_sensor,
+	.remove_sensor = ti_thermal_remove_sensor,
+	.report_temperature = ti_thermal_report_sensor_temperature,
+	.sensors = {
+		{
+		.registers = &omap5430_mpu_temp_sensor_registers,
+		.ts_data = &omap5430_mpu_temp_sensor_data,
+		.domain = "cpu",
+		.register_cooling = ti_thermal_register_cpu_cooling,
+		.unregister_cooling = ti_thermal_unregister_cpu_cooling,
+		.slope = OMAP_GRADIENT_SLOPE_5430_CPU,
+		.constant = OMAP_GRADIENT_CONST_5430_CPU,
+		.slope_pcb = OMAP_GRADIENT_SLOPE_W_PCB_5430_CPU,
+		.constant_pcb = OMAP_GRADIENT_CONST_W_PCB_5430_CPU,
+		},
+		{
+		.registers = &omap5430_gpu_temp_sensor_registers,
+		.ts_data = &omap5430_gpu_temp_sensor_data,
+		.domain = "gpu",
+		.slope = OMAP_GRADIENT_SLOPE_5430_GPU,
+		.constant = OMAP_GRADIENT_CONST_5430_GPU,
+		.slope_pcb = OMAP_GRADIENT_SLOPE_W_PCB_5430_GPU,
+		.constant_pcb = OMAP_GRADIENT_CONST_W_PCB_5430_GPU,
+		},
+		{
+		.registers = &omap5430_core_temp_sensor_registers,
+		.ts_data = &omap5430_core_temp_sensor_data,
+		.domain = "core",
+		},
+	},
+	.sensor_count = 3,
+};
diff --git a/drivers/staging/ti-soc-thermal/omap5xxx-bandgap.h b/drivers/staging/ti-soc-thermal/omap5xxx-bandgap.h
new file mode 100644
index 00000000000..400b55dffad
--- /dev/null
+++ b/drivers/staging/ti-soc-thermal/omap5xxx-bandgap.h
@@ -0,0 +1,200 @@
+/*
+ * OMAP5xxx bandgap registers, bitfields and temperature definitions
+ *
+ * Copyright (C) 2013 Texas Instruments Incorporated - http://www.ti.com/
+ * Contact:
+ *   Eduardo Valentin <eduardo.valentin@ti.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 __OMAP5XXX_BANDGAP_H
+#define __OMAP5XXX_BANDGAP_H
+
+/**
+ * *** OMAP5430 ***
+ *
+ * Below, in sequence, are the Register definitions,
+ * the bitfields and the temperature definitions for OMAP5430.
+ */
+
+/**
+ * OMAP5430 register definitions
+ *
+ * Registers are defined as offsets. The offsets are
+ * relative to FUSE_OPP_BGAP_GPU on 5430.
+ *
+ * Register below are grouped by domain (not necessarily in offset order)
+ */
+
+/* OMAP5430.GPU register offsets */
+#define OMAP5430_FUSE_OPP_BGAP_GPU			0x0
+#define OMAP5430_TEMP_SENSOR_GPU_OFFSET			0x150
+#define OMAP5430_BGAP_THRESHOLD_GPU_OFFSET		0x1A8
+#define OMAP5430_BGAP_TSHUT_GPU_OFFSET			0x1B4
+#define OMAP5430_BGAP_CUMUL_DTEMP_GPU_OFFSET		0x1C0
+#define OMAP5430_BGAP_DTEMP_GPU_0_OFFSET		0x1F4
+#define OMAP5430_BGAP_DTEMP_GPU_1_OFFSET		0x1F8
+#define OMAP5430_BGAP_DTEMP_GPU_2_OFFSET		0x1FC
+#define OMAP5430_BGAP_DTEMP_GPU_3_OFFSET		0x200
+#define OMAP5430_BGAP_DTEMP_GPU_4_OFFSET		0x204
+
+/* OMAP5430.MPU register offsets */
+#define OMAP5430_FUSE_OPP_BGAP_MPU			0x4
+#define OMAP5430_TEMP_SENSOR_MPU_OFFSET			0x14C
+#define OMAP5430_BGAP_THRESHOLD_MPU_OFFSET		0x1A4
+#define OMAP5430_BGAP_TSHUT_MPU_OFFSET			0x1B0
+#define OMAP5430_BGAP_CUMUL_DTEMP_MPU_OFFSET		0x1BC
+#define OMAP5430_BGAP_DTEMP_MPU_0_OFFSET		0x1E0
+#define OMAP5430_BGAP_DTEMP_MPU_1_OFFSET		0x1E4
+#define OMAP5430_BGAP_DTEMP_MPU_2_OFFSET		0x1E8
+#define OMAP5430_BGAP_DTEMP_MPU_3_OFFSET		0x1EC
+#define OMAP5430_BGAP_DTEMP_MPU_4_OFFSET		0x1F0
+
+/* OMAP5430.MPU register offsets */
+#define OMAP5430_FUSE_OPP_BGAP_CORE			0x8
+#define OMAP5430_TEMP_SENSOR_CORE_OFFSET		0x154
+#define OMAP5430_BGAP_THRESHOLD_CORE_OFFSET		0x1AC
+#define OMAP5430_BGAP_TSHUT_CORE_OFFSET			0x1B8
+#define OMAP5430_BGAP_CUMUL_DTEMP_CORE_OFFSET		0x1C4
+#define OMAP5430_BGAP_DTEMP_CORE_0_OFFSET		0x208
+#define OMAP5430_BGAP_DTEMP_CORE_1_OFFSET		0x20C
+#define OMAP5430_BGAP_DTEMP_CORE_2_OFFSET		0x210
+#define OMAP5430_BGAP_DTEMP_CORE_3_OFFSET		0x214
+#define OMAP5430_BGAP_DTEMP_CORE_4_OFFSET		0x218
+
+/* OMAP5430.common register offsets */
+#define OMAP5430_BGAP_CTRL_OFFSET			0x1A0
+#define OMAP5430_BGAP_STATUS_OFFSET			0x1C8
+
+/**
+ * Register bitfields for OMAP5430
+ *
+ * All the macros bellow define the required bits for
+ * controlling temperature on OMAP5430. Bit defines are
+ * grouped by register.
+ */
+
+/* OMAP5430.TEMP_SENSOR */
+#define OMAP5430_BGAP_TEMP_SENSOR_SOC_MASK		BIT(12)
+#define OMAP5430_BGAP_TEMPSOFF_MASK			BIT(11)
+#define OMAP5430_BGAP_TEMP_SENSOR_EOCZ_MASK		BIT(10)
+#define OMAP5430_BGAP_TEMP_SENSOR_DTEMP_MASK		(0x3ff << 0)
+
+/* OMAP5430.BANDGAP_CTRL */
+#define OMAP5430_MASK_SIDLEMODE_MASK			(0x3 << 30)
+#define OMAP5430_MASK_COUNTER_DELAY_MASK		(0x7 << 27)
+#define OMAP5430_MASK_FREEZE_CORE_MASK			BIT(23)
+#define OMAP5430_MASK_FREEZE_GPU_MASK			BIT(22)
+#define OMAP5430_MASK_FREEZE_MPU_MASK			BIT(21)
+#define OMAP5430_MASK_CLEAR_CORE_MASK			BIT(20)
+#define OMAP5430_MASK_CLEAR_GPU_MASK			BIT(19)
+#define OMAP5430_MASK_CLEAR_MPU_MASK			BIT(18)
+#define OMAP5430_MASK_CLEAR_ACCUM_CORE_MASK		BIT(17)
+#define OMAP5430_MASK_CLEAR_ACCUM_GPU_MASK		BIT(16)
+#define OMAP5430_MASK_CLEAR_ACCUM_MPU_MASK		BIT(15)
+#define OMAP5430_MASK_HOT_CORE_MASK			BIT(5)
+#define OMAP5430_MASK_COLD_CORE_MASK			BIT(4)
+#define OMAP5430_MASK_HOT_GPU_MASK			BIT(3)
+#define OMAP5430_MASK_COLD_GPU_MASK			BIT(2)
+#define OMAP5430_MASK_HOT_MPU_MASK			BIT(1)
+#define OMAP5430_MASK_COLD_MPU_MASK			BIT(0)
+
+/* OMAP5430.BANDGAP_COUNTER */
+#define OMAP5430_COUNTER_MASK				(0xffffff << 0)
+
+/* OMAP5430.BANDGAP_THRESHOLD */
+#define OMAP5430_T_HOT_MASK				(0x3ff << 16)
+#define OMAP5430_T_COLD_MASK				(0x3ff << 0)
+
+/* OMAP5430.TSHUT_THRESHOLD */
+#define OMAP5430_TSHUT_HOT_MASK				(0x3ff << 16)
+#define OMAP5430_TSHUT_COLD_MASK			(0x3ff << 0)
+
+/* OMAP5430.BANDGAP_CUMUL_DTEMP_MPU */
+#define OMAP5430_CUMUL_DTEMP_MPU_MASK			(0xffffffff << 0)
+
+/* OMAP5430.BANDGAP_CUMUL_DTEMP_GPU */
+#define OMAP5430_CUMUL_DTEMP_GPU_MASK			(0xffffffff << 0)
+
+/* OMAP5430.BANDGAP_CUMUL_DTEMP_CORE */
+#define OMAP5430_CUMUL_DTEMP_CORE_MASK			(0xffffffff << 0)
+
+/* OMAP5430.BANDGAP_STATUS */
+#define OMAP5430_BGAP_ALERT_MASK			BIT(31)
+#define OMAP5430_HOT_CORE_FLAG_MASK			BIT(5)
+#define OMAP5430_COLD_CORE_FLAG_MASK			BIT(4)
+#define OMAP5430_HOT_GPU_FLAG_MASK			BIT(3)
+#define OMAP5430_COLD_GPU_FLAG_MASK			BIT(2)
+#define OMAP5430_HOT_MPU_FLAG_MASK			BIT(1)
+#define OMAP5430_COLD_MPU_FLAG_MASK			BIT(0)
+
+/**
+ * Temperature limits and thresholds for OMAP5430
+ *
+ * All the macros bellow are definitions for handling the
+ * ADC conversions and representation of temperature limits
+ * and thresholds for OMAP5430. Definitions are grouped
+ * by temperature domain.
+ */
+
+/* OMAP5430.common temperature definitions */
+/* ADC conversion table limits */
+#define OMAP5430_ADC_START_VALUE			540
+#define OMAP5430_ADC_END_VALUE				945
+
+/* OMAP5430.GPU temperature definitions */
+/* bandgap clock limits */
+#define OMAP5430_GPU_MAX_FREQ				1500000
+#define OMAP5430_GPU_MIN_FREQ				1000000
+/* sensor limits */
+#define OMAP5430_GPU_MIN_TEMP				-40000
+#define OMAP5430_GPU_MAX_TEMP				125000
+#define OMAP5430_GPU_HYST_VAL				5000
+/* interrupts thresholds */
+#define OMAP5430_GPU_TSHUT_HOT				915
+#define OMAP5430_GPU_TSHUT_COLD				900
+#define OMAP5430_GPU_T_HOT				800
+#define OMAP5430_GPU_T_COLD				795
+
+/* OMAP5430.MPU temperature definitions */
+/* bandgap clock limits */
+#define OMAP5430_MPU_MAX_FREQ				1500000
+#define OMAP5430_MPU_MIN_FREQ				1000000
+/* sensor limits */
+#define OMAP5430_MPU_MIN_TEMP				-40000
+#define OMAP5430_MPU_MAX_TEMP				125000
+#define OMAP5430_MPU_HYST_VAL				5000
+/* interrupts thresholds */
+#define OMAP5430_MPU_TSHUT_HOT				915
+#define OMAP5430_MPU_TSHUT_COLD				900
+#define OMAP5430_MPU_T_HOT				800
+#define OMAP5430_MPU_T_COLD				795
+
+/* OMAP5430.CORE temperature definitions */
+/* bandgap clock limits */
+#define OMAP5430_CORE_MAX_FREQ				1500000
+#define OMAP5430_CORE_MIN_FREQ				1000000
+/* sensor limits */
+#define OMAP5430_CORE_MIN_TEMP				-40000
+#define OMAP5430_CORE_MAX_TEMP				125000
+#define OMAP5430_CORE_HYST_VAL				5000
+/* interrupts thresholds */
+#define OMAP5430_CORE_TSHUT_HOT				915
+#define OMAP5430_CORE_TSHUT_COLD			900
+#define OMAP5430_CORE_T_HOT				800
+#define OMAP5430_CORE_T_COLD				795
+
+#endif /* __OMAP5XXX_BANDGAP_H */
diff --git a/drivers/staging/ti-soc-thermal/ti-bandgap.c b/drivers/staging/ti-soc-thermal/ti-bandgap.c
new file mode 100644
index 00000000000..f20c1cfe980
--- /dev/null
+++ b/drivers/staging/ti-soc-thermal/ti-bandgap.c
@@ -0,0 +1,1546 @@
+/*
+ * TI Bandgap temperature sensor driver
+ *
+ * Copyright (C) 2011-2012 Texas Instruments Incorporated - http://www.ti.com/
+ * Author: J Keerthy <j-keerthy@ti.com>
+ * Author: Moiz Sonasath <m-sonasath@ti.com>
+ * Couple of fixes, DT and MFD adaptation:
+ *   Eduardo Valentin <eduardo.valentin@ti.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/module.h>
+#include <linux/export.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/interrupt.h>
+#include <linux/clk.h>
+#include <linux/gpio.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+#include <linux/types.h>
+#include <linux/spinlock.h>
+#include <linux/reboot.h>
+#include <linux/of_device.h>
+#include <linux/of_platform.h>
+#include <linux/of_irq.h>
+#include <linux/io.h>
+
+#include "ti-bandgap.h"
+
+/***   Helper functions to access registers and their bitfields   ***/
+
+/**
+ * ti_bandgap_readl() - simple read helper function
+ * @bgp: pointer to ti_bandgap structure
+ * @reg: desired register (offset) to be read
+ *
+ * Helper function to read bandgap registers. It uses the io remapped area.
+ * Return: the register value.
+ */
+static u32 ti_bandgap_readl(struct ti_bandgap *bgp, u32 reg)
+{
+	return readl(bgp->base + reg);
+}
+
+/**
+ * ti_bandgap_writel() - simple write helper function
+ * @bgp: pointer to ti_bandgap structure
+ * @val: desired register value to be written
+ * @reg: desired register (offset) to be written
+ *
+ * Helper function to write bandgap registers. It uses the io remapped area.
+ */
+static void ti_bandgap_writel(struct ti_bandgap *bgp, u32 val, u32 reg)
+{
+	writel(val, bgp->base + reg);
+}
+
+/**
+ * DOC: macro to update bits.
+ *
+ * RMW_BITS() - used to read, modify and update bandgap bitfields.
+ *            The value passed will be shifted.
+ */
+#define RMW_BITS(bgp, id, reg, mask, val)			\
+do {								\
+	struct temp_sensor_registers *t;			\
+	u32 r;							\
+								\
+	t = bgp->conf->sensors[(id)].registers;		\
+	r = ti_bandgap_readl(bgp, t->reg);			\
+	r &= ~t->mask;						\
+	r |= (val) << __ffs(t->mask);				\
+	ti_bandgap_writel(bgp, r, t->reg);			\
+} while (0)
+
+/***   Basic helper functions   ***/
+
+/**
+ * ti_bandgap_power() - controls the power state of a bandgap device
+ * @bgp: pointer to ti_bandgap structure
+ * @on: desired power state (1 - on, 0 - off)
+ *
+ * Used to power on/off a bandgap device instance. Only used on those
+ * that features tempsoff bit.
+ *
+ * Return: 0 on success, -ENOTSUPP if tempsoff is not supported.
+ */
+static int ti_bandgap_power(struct ti_bandgap *bgp, bool on)
+{
+	int i, ret = 0;
+
+	if (!TI_BANDGAP_HAS(bgp, POWER_SWITCH)) {
+		ret = -ENOTSUPP;
+		goto exit;
+	}
+
+	for (i = 0; i < bgp->conf->sensor_count; i++)
+		/* active on 0 */
+		RMW_BITS(bgp, i, temp_sensor_ctrl, bgap_tempsoff_mask, !on);
+
+exit:
+	return ret;
+}
+
+/**
+ * ti_bandgap_read_temp() - helper function to read sensor temperature
+ * @bgp: pointer to ti_bandgap structure
+ * @id: bandgap sensor id
+ *
+ * Function to concentrate the steps to read sensor temperature register.
+ * This function is desired because, depending on bandgap device version,
+ * it might be needed to freeze the bandgap state machine, before fetching
+ * the register value.
+ *
+ * Return: temperature in ADC values.
+ */
+static u32 ti_bandgap_read_temp(struct ti_bandgap *bgp, int id)
+{
+	struct temp_sensor_registers *tsr;
+	u32 temp, reg;
+
+	tsr = bgp->conf->sensors[id].registers;
+	reg = tsr->temp_sensor_ctrl;
+
+	if (TI_BANDGAP_HAS(bgp, FREEZE_BIT)) {
+		RMW_BITS(bgp, id, bgap_mask_ctrl, mask_freeze_mask, 1);
+		/*
+		 * In case we cannot read from cur_dtemp / dtemp_0,
+		 * then we read from the last valid temp read
+		 */
+		reg = tsr->ctrl_dtemp_1;
+	}
+
+	/* read temperature */
+	temp = ti_bandgap_readl(bgp, reg);
+	temp &= tsr->bgap_dtemp_mask;
+
+	if (TI_BANDGAP_HAS(bgp, FREEZE_BIT))
+		RMW_BITS(bgp, id, bgap_mask_ctrl, mask_freeze_mask, 0);
+
+	return temp;
+}
+
+/***   IRQ handlers   ***/
+
+/**
+ * ti_bandgap_talert_irq_handler() - handles Temperature alert IRQs
+ * @irq: IRQ number
+ * @data: private data (struct ti_bandgap *)
+ *
+ * This is the Talert handler. Use it only if bandgap device features
+ * HAS(TALERT). This handler goes over all sensors and checks their
+ * conditions and acts accordingly. In case there are events pending,
+ * it will reset the event mask to wait for the opposite event (next event).
+ * Every time there is a new event, it will be reported to thermal layer.
+ *
+ * Return: IRQ_HANDLED
+ */
+static irqreturn_t ti_bandgap_talert_irq_handler(int irq, void *data)
+{
+	struct ti_bandgap *bgp = data;
+	struct temp_sensor_registers *tsr;
+	u32 t_hot = 0, t_cold = 0, ctrl;
+	int i;
+
+	spin_lock(&bgp->lock);
+	for (i = 0; i < bgp->conf->sensor_count; i++) {
+		tsr = bgp->conf->sensors[i].registers;
+		ctrl = ti_bandgap_readl(bgp, tsr->bgap_status);
+
+		/* Read the status of t_hot */
+		t_hot = ctrl & tsr->status_hot_mask;
+
+		/* Read the status of t_cold */
+		t_cold = ctrl & tsr->status_cold_mask;
+
+		if (!t_cold && !t_hot)
+			continue;
+
+		ctrl = ti_bandgap_readl(bgp, tsr->bgap_mask_ctrl);
+		/*
+		 * One TALERT interrupt: Two sources
+		 * If the interrupt is due to t_hot then mask t_hot and
+		 * and unmask t_cold else mask t_cold and unmask t_hot
+		 */
+		if (t_hot) {
+			ctrl &= ~tsr->mask_hot_mask;
+			ctrl |= tsr->mask_cold_mask;
+		} else if (t_cold) {
+			ctrl &= ~tsr->mask_cold_mask;
+			ctrl |= tsr->mask_hot_mask;
+		}
+
+		ti_bandgap_writel(bgp, ctrl, tsr->bgap_mask_ctrl);
+
+		dev_dbg(bgp->dev,
+			"%s: IRQ from %s sensor: hotevent %d coldevent %d\n",
+			__func__, bgp->conf->sensors[i].domain,
+			t_hot, t_cold);
+
+		/* report temperature to whom may concern */
+		if (bgp->conf->report_temperature)
+			bgp->conf->report_temperature(bgp, i);
+	}
+	spin_unlock(&bgp->lock);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ti_bandgap_tshut_irq_handler() - handles Temperature shutdown signal
+ * @irq: IRQ number
+ * @data: private data (unused)
+ *
+ * This is the Tshut handler. Use it only if bandgap device features
+ * HAS(TSHUT). If any sensor fires the Tshut signal, we simply shutdown
+ * the system.
+ *
+ * Return: IRQ_HANDLED
+ */
+static irqreturn_t ti_bandgap_tshut_irq_handler(int irq, void *data)
+{
+	pr_emerg("%s: TSHUT temperature reached. Needs shut down...\n",
+		 __func__);
+
+	orderly_poweroff(true);
+
+	return IRQ_HANDLED;
+}
+
+/***   Helper functions which manipulate conversion ADC <-> mi Celsius   ***/
+
+/**
+ * ti_bandgap_adc_to_mcelsius() - converts an ADC value to mCelsius scale
+ * @bgp: struct ti_bandgap pointer
+ * @adc_val: value in ADC representation
+ * @t: address where to write the resulting temperature in mCelsius
+ *
+ * Simple conversion from ADC representation to mCelsius. In case the ADC value
+ * is out of the ADC conv table range, it returns -ERANGE, 0 on success.
+ * The conversion table is indexed by the ADC values.
+ *
+ * Return: 0 if conversion was successful, else -ERANGE in case the @adc_val
+ * argument is out of the ADC conv table range.
+ */
+static
+int ti_bandgap_adc_to_mcelsius(struct ti_bandgap *bgp, int adc_val, int *t)
+{
+	const struct ti_bandgap_data *conf = bgp->conf;
+	int ret = 0;
+
+	/* look up for temperature in the table and return the temperature */
+	if (adc_val < conf->adc_start_val || adc_val > conf->adc_end_val) {
+		ret = -ERANGE;
+		goto exit;
+	}
+
+	*t = bgp->conf->conv_table[adc_val - conf->adc_start_val];
+
+exit:
+	return ret;
+}
+
+/**
+ * ti_bandgap_mcelsius_to_adc() - converts a mCelsius value to ADC scale
+ * @bgp: struct ti_bandgap pointer
+ * @temp: value in mCelsius
+ * @adc: address where to write the resulting temperature in ADC representation
+ *
+ * Simple conversion from mCelsius to ADC values. In case the temp value
+ * is out of the ADC conv table range, it returns -ERANGE, 0 on success.
+ * The conversion table is indexed by the ADC values.
+ *
+ * Return: 0 if conversion was successful, else -ERANGE in case the @temp
+ * argument is out of the ADC conv table range.
+ */
+static
+int ti_bandgap_mcelsius_to_adc(struct ti_bandgap *bgp, long temp, int *adc)
+{
+	const struct ti_bandgap_data *conf = bgp->conf;
+	const int *conv_table = bgp->conf->conv_table;
+	int high, low, mid, ret = 0;
+
+	low = 0;
+	high = conf->adc_end_val - conf->adc_start_val;
+	mid = (high + low) / 2;
+
+	if (temp < conv_table[low] || temp > conv_table[high]) {
+		ret = -ERANGE;
+		goto exit;
+	}
+
+	while (low < high) {
+		if (temp < conv_table[mid])
+			high = mid - 1;
+		else
+			low = mid + 1;
+		mid = (low + high) / 2;
+	}
+
+	*adc = conf->adc_start_val + low;
+
+exit:
+	return ret;
+}
+
+/**
+ * ti_bandgap_add_hyst() - add hysteresis (in mCelsius) to an ADC value
+ * @bgp: struct ti_bandgap pointer
+ * @adc_val: temperature value in ADC representation
+ * @hyst_val: hysteresis value in mCelsius
+ * @sum: address where to write the resulting temperature (in ADC scale)
+ *
+ * Adds an hysteresis value (in mCelsius) to a ADC temperature value.
+ *
+ * Return: 0 on success, -ERANGE otherwise.
+ */
+static
+int ti_bandgap_add_hyst(struct ti_bandgap *bgp, int adc_val, int hyst_val,
+			u32 *sum)
+{
+	int temp, ret;
+
+	/*
+	 * Need to add in the mcelsius domain, so we have a temperature
+	 * the conv_table range
+	 */
+	ret = ti_bandgap_adc_to_mcelsius(bgp, adc_val, &temp);
+	if (ret < 0)
+		goto exit;
+
+	temp += hyst_val;
+
+	ret = ti_bandgap_mcelsius_to_adc(bgp, temp, sum);
+
+exit:
+	return ret;
+}
+
+/***   Helper functions handling device Alert/Shutdown signals   ***/
+
+/**
+ * ti_bandgap_unmask_interrupts() - unmasks the events of thot & tcold
+ * @bgp: struct ti_bandgap pointer
+ * @id: bandgap sensor id
+ * @t_hot: hot temperature value to trigger alert signal
+ * @t_cold: cold temperature value to trigger alert signal
+ *
+ * Checks the requested t_hot and t_cold values and configures the IRQ event
+ * masks accordingly. Call this function only if bandgap features HAS(TALERT).
+ */
+static void ti_bandgap_unmask_interrupts(struct ti_bandgap *bgp, int id,
+					 u32 t_hot, u32 t_cold)
+{
+	struct temp_sensor_registers *tsr;
+	u32 temp, reg_val;
+
+	/* Read the current on die temperature */
+	temp = ti_bandgap_read_temp(bgp, id);
+
+	tsr = bgp->conf->sensors[id].registers;
+	reg_val = ti_bandgap_readl(bgp, tsr->bgap_mask_ctrl);
+
+	if (temp < t_hot)
+		reg_val |= tsr->mask_hot_mask;
+	else
+		reg_val &= ~tsr->mask_hot_mask;
+
+	if (t_cold < temp)
+		reg_val |= tsr->mask_cold_mask;
+	else
+		reg_val &= ~tsr->mask_cold_mask;
+	ti_bandgap_writel(bgp, reg_val, tsr->bgap_mask_ctrl);
+}
+
+/**
+ * ti_bandgap_update_alert_threshold() - sequence to update thresholds
+ * @bgp: struct ti_bandgap pointer
+ * @id: bandgap sensor id
+ * @val: value (ADC) of a new threshold
+ * @hot: desired threshold to be updated. true if threshold hot, false if
+ *       threshold cold
+ *
+ * It will program the required thresholds (hot and cold) for TALERT signal.
+ * This function can be used to update t_hot or t_cold, depending on @hot value.
+ * It checks the resulting t_hot and t_cold values, based on the new passed @val
+ * and configures the thresholds so that t_hot is always greater than t_cold.
+ * Call this function only if bandgap features HAS(TALERT).
+ *
+ * Return: 0 if no error, else corresponding error
+ */
+static int ti_bandgap_update_alert_threshold(struct ti_bandgap *bgp, int id,
+					     int val, bool hot)
+{
+	struct temp_sensor_data *ts_data = bgp->conf->sensors[id].ts_data;
+	struct temp_sensor_registers *tsr;
+	u32 thresh_val, reg_val, t_hot, t_cold;
+	int err = 0;
+
+	tsr = bgp->conf->sensors[id].registers;
+
+	/* obtain the current value */
+	thresh_val = ti_bandgap_readl(bgp, tsr->bgap_threshold);
+	t_cold = (thresh_val & tsr->threshold_tcold_mask) >>
+		__ffs(tsr->threshold_tcold_mask);
+	t_hot = (thresh_val & tsr->threshold_thot_mask) >>
+		__ffs(tsr->threshold_thot_mask);
+	if (hot)
+		t_hot = val;
+	else
+		t_cold = val;
+
+	if (t_cold > t_hot) {
+		if (hot)
+			err = ti_bandgap_add_hyst(bgp, t_hot,
+						  -ts_data->hyst_val,
+						  &t_cold);
+		else
+			err = ti_bandgap_add_hyst(bgp, t_cold,
+						  ts_data->hyst_val,
+						  &t_hot);
+	}
+
+	/* write the new threshold values */
+	reg_val = thresh_val &
+		  ~(tsr->threshold_thot_mask | tsr->threshold_tcold_mask);
+	reg_val |= (t_hot << __ffs(tsr->threshold_thot_mask)) |
+		   (t_cold << __ffs(tsr->threshold_tcold_mask));
+	ti_bandgap_writel(bgp, reg_val, tsr->bgap_threshold);
+
+	if (err) {
+		dev_err(bgp->dev, "failed to reprogram thot threshold\n");
+		err = -EIO;
+		goto exit;
+	}
+
+	ti_bandgap_unmask_interrupts(bgp, id, t_hot, t_cold);
+exit:
+	return err;
+}
+
+/**
+ * ti_bandgap_validate() - helper to check the sanity of a struct ti_bandgap
+ * @bgp: struct ti_bandgap pointer
+ * @id: bandgap sensor id
+ *
+ * Checks if the bandgap pointer is valid and if the sensor id is also
+ * applicable.
+ *
+ * Return: 0 if no errors, -EINVAL for invalid @bgp pointer or -ERANGE if
+ * @id cannot index @bgp sensors.
+ */
+static inline int ti_bandgap_validate(struct ti_bandgap *bgp, int id)
+{
+	int ret = 0;
+
+	if (IS_ERR_OR_NULL(bgp)) {
+		pr_err("%s: invalid bandgap pointer\n", __func__);
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	if ((id < 0) || (id >= bgp->conf->sensor_count)) {
+		dev_err(bgp->dev, "%s: sensor id out of range (%d)\n",
+			__func__, id);
+		ret = -ERANGE;
+	}
+
+exit:
+	return ret;
+}
+
+/**
+ * _ti_bandgap_write_threshold() - helper to update TALERT t_cold or t_hot
+ * @bgp: struct ti_bandgap pointer
+ * @id: bandgap sensor id
+ * @val: value (mCelsius) of a new threshold
+ * @hot: desired threshold to be updated. true if threshold hot, false if
+ *       threshold cold
+ *
+ * It will update the required thresholds (hot and cold) for TALERT signal.
+ * This function can be used to update t_hot or t_cold, depending on @hot value.
+ * Validates the mCelsius range and update the requested threshold.
+ * Call this function only if bandgap features HAS(TALERT).
+ *
+ * Return: 0 if no error, else corresponding error value.
+ */
+static int _ti_bandgap_write_threshold(struct ti_bandgap *bgp, int id, int val,
+				       bool hot)
+{
+	struct temp_sensor_data *ts_data;
+	struct temp_sensor_registers *tsr;
+	u32 adc_val;
+	int ret;
+
+	ret = ti_bandgap_validate(bgp, id);
+	if (ret)
+		goto exit;
+
+	if (!TI_BANDGAP_HAS(bgp, TALERT)) {
+		ret = -ENOTSUPP;
+		goto exit;
+	}
+
+	ts_data = bgp->conf->sensors[id].ts_data;
+	tsr = bgp->conf->sensors[id].registers;
+	if (hot) {
+		if (val < ts_data->min_temp + ts_data->hyst_val)
+			ret = -EINVAL;
+	} else {
+		if (val > ts_data->max_temp + ts_data->hyst_val)
+			ret = -EINVAL;
+	}
+
+	if (ret)
+		goto exit;
+
+	ret = ti_bandgap_mcelsius_to_adc(bgp, val, &adc_val);
+	if (ret < 0)
+		goto exit;
+
+	spin_lock(&bgp->lock);
+	ret = ti_bandgap_update_alert_threshold(bgp, id, adc_val, hot);
+	spin_unlock(&bgp->lock);
+
+exit:
+	return ret;
+}
+
+/**
+ * _ti_bandgap_read_threshold() - helper to read TALERT t_cold or t_hot
+ * @bgp: struct ti_bandgap pointer
+ * @id: bandgap sensor id
+ * @val: value (mCelsius) of a threshold
+ * @hot: desired threshold to be read. true if threshold hot, false if
+ *       threshold cold
+ *
+ * It will fetch the required thresholds (hot and cold) for TALERT signal.
+ * This function can be used to read t_hot or t_cold, depending on @hot value.
+ * Call this function only if bandgap features HAS(TALERT).
+ *
+ * Return: 0 if no error, -ENOTSUPP if it has no TALERT support, or the
+ * corresponding error value if some operation fails.
+ */
+static int _ti_bandgap_read_threshold(struct ti_bandgap *bgp, int id,
+				      int *val, bool hot)
+{
+	struct temp_sensor_registers *tsr;
+	u32 temp, mask;
+	int ret = 0;
+
+	ret = ti_bandgap_validate(bgp, id);
+	if (ret)
+		goto exit;
+
+	if (!TI_BANDGAP_HAS(bgp, TALERT)) {
+		ret = -ENOTSUPP;
+		goto exit;
+	}
+
+	tsr = bgp->conf->sensors[id].registers;
+	if (hot)
+		mask = tsr->threshold_thot_mask;
+	else
+		mask = tsr->threshold_tcold_mask;
+
+	temp = ti_bandgap_readl(bgp, tsr->bgap_threshold);
+	temp = (temp & mask) >> __ffs(mask);
+	ret |= ti_bandgap_adc_to_mcelsius(bgp, temp, &temp);
+	if (ret) {
+		dev_err(bgp->dev, "failed to read thot\n");
+		ret = -EIO;
+		goto exit;
+	}
+
+	*val = temp;
+
+exit:
+	return ret;
+}
+
+/***   Exposed APIs   ***/
+
+/**
+ * ti_bandgap_read_thot() - reads sensor current thot
+ * @bgp: pointer to bandgap instance
+ * @id: sensor id
+ * @thot: resulting current thot value
+ *
+ * Return: 0 on success or the proper error code
+ */
+int ti_bandgap_read_thot(struct ti_bandgap *bgp, int id, int *thot)
+{
+	return _ti_bandgap_read_threshold(bgp, id, thot, true);
+}
+
+/**
+ * ti_bandgap_write_thot() - sets sensor current thot
+ * @bgp: pointer to bandgap instance
+ * @id: sensor id
+ * @val: desired thot value
+ *
+ * Return: 0 on success or the proper error code
+ */
+int ti_bandgap_write_thot(struct ti_bandgap *bgp, int id, int val)
+{
+	return _ti_bandgap_write_threshold(bgp, id, val, true);
+}
+
+/**
+ * ti_bandgap_read_tcold() - reads sensor current tcold
+ * @bgp: pointer to bandgap instance
+ * @id: sensor id
+ * @tcold: resulting current tcold value
+ *
+ * Return: 0 on success or the proper error code
+ */
+int ti_bandgap_read_tcold(struct ti_bandgap *bgp, int id, int *tcold)
+{
+	return _ti_bandgap_read_threshold(bgp, id, tcold, false);
+}
+
+/**
+ * ti_bandgap_write_tcold() - sets the sensor tcold
+ * @bgp: pointer to bandgap instance
+ * @id: sensor id
+ * @val: desired tcold value
+ *
+ * Return: 0 on success or the proper error code
+ */
+int ti_bandgap_write_tcold(struct ti_bandgap *bgp, int id, int val)
+{
+	return _ti_bandgap_write_threshold(bgp, id, val, false);
+}
+
+/**
+ * ti_bandgap_read_counter() - read the sensor counter
+ * @bgp: pointer to bandgap instance
+ * @id: sensor id
+ * @interval: resulting update interval in miliseconds
+ */
+static void ti_bandgap_read_counter(struct ti_bandgap *bgp, int id,
+				    int *interval)
+{
+	struct temp_sensor_registers *tsr;
+	int time;
+
+	tsr = bgp->conf->sensors[id].registers;
+	time = ti_bandgap_readl(bgp, tsr->bgap_counter);
+	time = (time & tsr->counter_mask) >>
+					__ffs(tsr->counter_mask);
+	time = time * 1000 / bgp->clk_rate;
+	*interval = time;
+}
+
+/**
+ * ti_bandgap_read_counter_delay() - read the sensor counter delay
+ * @bgp: pointer to bandgap instance
+ * @id: sensor id
+ * @interval: resulting update interval in miliseconds
+ */
+static void ti_bandgap_read_counter_delay(struct ti_bandgap *bgp, int id,
+					  int *interval)
+{
+	struct temp_sensor_registers *tsr;
+	int reg_val;
+
+	tsr = bgp->conf->sensors[id].registers;
+
+	reg_val = ti_bandgap_readl(bgp, tsr->bgap_mask_ctrl);
+	reg_val = (reg_val & tsr->mask_counter_delay_mask) >>
+				__ffs(tsr->mask_counter_delay_mask);
+	switch (reg_val) {
+	case 0:
+		*interval = 0;
+		break;
+	case 1:
+		*interval = 1;
+		break;
+	case 2:
+		*interval = 10;
+		break;
+	case 3:
+		*interval = 100;
+		break;
+	case 4:
+		*interval = 250;
+		break;
+	case 5:
+		*interval = 500;
+		break;
+	default:
+		dev_warn(bgp->dev, "Wrong counter delay value read from register %X",
+			 reg_val);
+	}
+}
+
+/**
+ * ti_bandgap_read_update_interval() - read the sensor update interval
+ * @bgp: pointer to bandgap instance
+ * @id: sensor id
+ * @interval: resulting update interval in miliseconds
+ *
+ * Return: 0 on success or the proper error code
+ */
+int ti_bandgap_read_update_interval(struct ti_bandgap *bgp, int id,
+				    int *interval)
+{
+	int ret = 0;
+
+	ret = ti_bandgap_validate(bgp, id);
+	if (ret)
+		goto exit;
+
+	if (!TI_BANDGAP_HAS(bgp, COUNTER) &&
+	    !TI_BANDGAP_HAS(bgp, COUNTER_DELAY)) {
+		ret = -ENOTSUPP;
+		goto exit;
+	}
+
+	if (TI_BANDGAP_HAS(bgp, COUNTER)) {
+		ti_bandgap_read_counter(bgp, id, interval);
+		goto exit;
+	}
+
+	ti_bandgap_read_counter_delay(bgp, id, interval);
+exit:
+	return ret;
+}
+
+/**
+ * ti_bandgap_write_counter_delay() - set the counter_delay
+ * @bgp: pointer to bandgap instance
+ * @id: sensor id
+ * @interval: desired update interval in miliseconds
+ *
+ * Return: 0 on success or the proper error code
+ */
+static int ti_bandgap_write_counter_delay(struct ti_bandgap *bgp, int id,
+					  u32 interval)
+{
+	int rval;
+
+	switch (interval) {
+	case 0: /* Immediate conversion */
+		rval = 0x0;
+		break;
+	case 1: /* Conversion after ever 1ms */
+		rval = 0x1;
+		break;
+	case 10: /* Conversion after ever 10ms */
+		rval = 0x2;
+		break;
+	case 100: /* Conversion after ever 100ms */
+		rval = 0x3;
+		break;
+	case 250: /* Conversion after ever 250ms */
+		rval = 0x4;
+		break;
+	case 500: /* Conversion after ever 500ms */
+		rval = 0x5;
+		break;
+	default:
+		dev_warn(bgp->dev, "Delay %d ms is not supported\n", interval);
+		return -EINVAL;
+	}
+
+	spin_lock(&bgp->lock);
+	RMW_BITS(bgp, id, bgap_mask_ctrl, mask_counter_delay_mask, rval);
+	spin_unlock(&bgp->lock);
+
+	return 0;
+}
+
+/**
+ * ti_bandgap_write_counter() - set the bandgap sensor counter
+ * @bgp: pointer to bandgap instance
+ * @id: sensor id
+ * @interval: desired update interval in miliseconds
+ */
+static void ti_bandgap_write_counter(struct ti_bandgap *bgp, int id,
+				     u32 interval)
+{
+	interval = interval * bgp->clk_rate / 1000;
+	spin_lock(&bgp->lock);
+	RMW_BITS(bgp, id, bgap_counter, counter_mask, interval);
+	spin_unlock(&bgp->lock);
+}
+
+/**
+ * ti_bandgap_write_update_interval() - set the update interval
+ * @bgp: pointer to bandgap instance
+ * @id: sensor id
+ * @interval: desired update interval in miliseconds
+ *
+ * Return: 0 on success or the proper error code
+ */
+int ti_bandgap_write_update_interval(struct ti_bandgap *bgp,
+				     int id, u32 interval)
+{
+	int ret = ti_bandgap_validate(bgp, id);
+	if (ret)
+		goto exit;
+
+	if (!TI_BANDGAP_HAS(bgp, COUNTER) &&
+	    !TI_BANDGAP_HAS(bgp, COUNTER_DELAY)) {
+		ret = -ENOTSUPP;
+		goto exit;
+	}
+
+	if (TI_BANDGAP_HAS(bgp, COUNTER)) {
+		ti_bandgap_write_counter(bgp, id, interval);
+		goto exit;
+	}
+
+	ret = ti_bandgap_write_counter_delay(bgp, id, interval);
+exit:
+	return ret;
+}
+
+/**
+ * ti_bandgap_read_temperature() - report current temperature
+ * @bgp: pointer to bandgap instance
+ * @id: sensor id
+ * @temperature: resulting temperature
+ *
+ * Return: 0 on success or the proper error code
+ */
+int ti_bandgap_read_temperature(struct ti_bandgap *bgp, int id,
+				int *temperature)
+{
+	u32 temp;
+	int ret;
+
+	ret = ti_bandgap_validate(bgp, id);
+	if (ret)
+		return ret;
+
+	spin_lock(&bgp->lock);
+	temp = ti_bandgap_read_temp(bgp, id);
+	spin_unlock(&bgp->lock);
+
+	ret |= ti_bandgap_adc_to_mcelsius(bgp, temp, &temp);
+	if (ret)
+		return -EIO;
+
+	*temperature = temp;
+
+	return 0;
+}
+
+/**
+ * ti_bandgap_set_sensor_data() - helper function to store thermal
+ * framework related data.
+ * @bgp: pointer to bandgap instance
+ * @id: sensor id
+ * @data: thermal framework related data to be stored
+ *
+ * Return: 0 on success or the proper error code
+ */
+int ti_bandgap_set_sensor_data(struct ti_bandgap *bgp, int id, void *data)
+{
+	int ret = ti_bandgap_validate(bgp, id);
+	if (ret)
+		return ret;
+
+	bgp->regval[id].data = data;
+
+	return 0;
+}
+
+/**
+ * ti_bandgap_get_sensor_data() - helper function to get thermal
+ * framework related data.
+ * @bgp: pointer to bandgap instance
+ * @id: sensor id
+ *
+ * Return: data stored by set function with sensor id on success or NULL
+ */
+void *ti_bandgap_get_sensor_data(struct ti_bandgap *bgp, int id)
+{
+	int ret = ti_bandgap_validate(bgp, id);
+	if (ret)
+		return ERR_PTR(ret);
+
+	return bgp->regval[id].data;
+}
+
+/***   Helper functions used during device initialization   ***/
+
+/**
+ * ti_bandgap_force_single_read() - executes 1 single ADC conversion
+ * @bgp: pointer to struct ti_bandgap
+ * @id: sensor id which it is desired to read 1 temperature
+ *
+ * Used to initialize the conversion state machine and set it to a valid
+ * state. Called during device initialization and context restore events.
+ *
+ * Return: 0
+ */
+static int
+ti_bandgap_force_single_read(struct ti_bandgap *bgp, int id)
+{
+	u32 temp = 0, counter = 1000;
+
+	/* Select single conversion mode */
+	if (TI_BANDGAP_HAS(bgp, MODE_CONFIG))
+		RMW_BITS(bgp, id, bgap_mode_ctrl, mode_ctrl_mask, 0);
+
+	/* Start of Conversion = 1 */
+	RMW_BITS(bgp, id, temp_sensor_ctrl, bgap_soc_mask, 1);
+	/* Wait until DTEMP is updated */
+	temp = ti_bandgap_read_temp(bgp, id);
+
+	while ((temp == 0) && --counter)
+		temp = ti_bandgap_read_temp(bgp, id);
+	/* REVISIT: Check correct condition for end of conversion */
+
+	/* Start of Conversion = 0 */
+	RMW_BITS(bgp, id, temp_sensor_ctrl, bgap_soc_mask, 0);
+
+	return 0;
+}
+
+/**
+ * ti_bandgap_set_continous_mode() - One time enabling of continuous mode
+ * @bgp: pointer to struct ti_bandgap
+ *
+ * Call this function only if HAS(MODE_CONFIG) is set. As this driver may
+ * be used for junction temperature monitoring, it is desirable that the
+ * sensors are operational all the time, so that alerts are generated
+ * properly.
+ *
+ * Return: 0
+ */
+static int ti_bandgap_set_continuous_mode(struct ti_bandgap *bgp)
+{
+	int i;
+
+	for (i = 0; i < bgp->conf->sensor_count; i++) {
+		/* Perform a single read just before enabling continuous */
+		ti_bandgap_force_single_read(bgp, i);
+		RMW_BITS(bgp, i, bgap_mode_ctrl, mode_ctrl_mask, 1);
+	}
+
+	return 0;
+}
+
+/**
+ * ti_bandgap_get_trend() - To fetch the temperature trend of a sensor
+ * @bgp: pointer to struct ti_bandgap
+ * @id: id of the individual sensor
+ * @trend: Pointer to trend.
+ *
+ * This function needs to be called to fetch the temperature trend of a
+ * Particular sensor. The function computes the difference in temperature
+ * w.r.t time. For the bandgaps with built in history buffer the temperatures
+ * are read from the buffer and for those without the Buffer -ENOTSUPP is
+ * returned.
+ *
+ * Return: 0 if no error, else return corresponding error. If no
+ *		error then the trend value is passed on to trend parameter
+ */
+int ti_bandgap_get_trend(struct ti_bandgap *bgp, int id, int *trend)
+{
+	struct temp_sensor_registers *tsr;
+	u32 temp1, temp2, reg1, reg2;
+	int t1, t2, interval, ret = 0;
+
+	ret = ti_bandgap_validate(bgp, id);
+	if (ret)
+		goto exit;
+
+	if (!TI_BANDGAP_HAS(bgp, HISTORY_BUFFER) ||
+	    !TI_BANDGAP_HAS(bgp, FREEZE_BIT)) {
+		ret = -ENOTSUPP;
+		goto exit;
+	}
+
+	tsr = bgp->conf->sensors[id].registers;
+
+	/* Freeze and read the last 2 valid readings */
+	reg1 = tsr->ctrl_dtemp_1;
+	reg2 = tsr->ctrl_dtemp_2;
+
+	/* read temperature from history buffer */
+	temp1 = ti_bandgap_readl(bgp, reg1);
+	temp1 &= tsr->bgap_dtemp_mask;
+
+	temp2 = ti_bandgap_readl(bgp, reg2);
+	temp2 &= tsr->bgap_dtemp_mask;
+
+	/* Convert from adc values to mCelsius temperature */
+	ret = ti_bandgap_adc_to_mcelsius(bgp, temp1, &t1);
+	if (ret)
+		goto exit;
+
+	ret = ti_bandgap_adc_to_mcelsius(bgp, temp2, &t2);
+	if (ret)
+		goto exit;
+
+	/* Fetch the update interval */
+	ret = ti_bandgap_read_update_interval(bgp, id, &interval);
+	if (ret || !interval)
+		goto exit;
+
+	*trend = (t1 - t2) / interval;
+
+	dev_dbg(bgp->dev, "The temperatures are t1 = %d and t2 = %d and trend =%d\n",
+		t1, t2, *trend);
+
+exit:
+	return ret;
+}
+
+/**
+ * ti_bandgap_tshut_init() - setup and initialize tshut handling
+ * @bgp: pointer to struct ti_bandgap
+ * @pdev: pointer to device struct platform_device
+ *
+ * Call this function only in case the bandgap features HAS(TSHUT).
+ * In this case, the driver needs to handle the TSHUT signal as an IRQ.
+ * The IRQ is wired as a GPIO, and for this purpose, it is required
+ * to specify which GPIO line is used. TSHUT IRQ is fired anytime
+ * one of the bandgap sensors violates the TSHUT high/hot threshold.
+ * And in that case, the system must go off.
+ *
+ * Return: 0 if no error, else error status
+ */
+static int ti_bandgap_tshut_init(struct ti_bandgap *bgp,
+				 struct platform_device *pdev)
+{
+	int gpio_nr = bgp->tshut_gpio;
+	int status;
+
+	/* Request for gpio_86 line */
+	status = gpio_request(gpio_nr, "tshut");
+	if (status < 0) {
+		dev_err(bgp->dev, "Could not request for TSHUT GPIO:%i\n", 86);
+		return status;
+	}
+	status = gpio_direction_input(gpio_nr);
+	if (status) {
+		dev_err(bgp->dev, "Cannot set input TSHUT GPIO %d\n", gpio_nr);
+		return status;
+	}
+
+	status = request_irq(gpio_to_irq(gpio_nr), ti_bandgap_tshut_irq_handler,
+			     IRQF_TRIGGER_RISING, "tshut", NULL);
+	if (status) {
+		gpio_free(gpio_nr);
+		dev_err(bgp->dev, "request irq failed for TSHUT");
+	}
+
+	return 0;
+}
+
+/**
+ * ti_bandgap_alert_init() - setup and initialize talert handling
+ * @bgp: pointer to struct ti_bandgap
+ * @pdev: pointer to device struct platform_device
+ *
+ * Call this function only in case the bandgap features HAS(TALERT).
+ * In this case, the driver needs to handle the TALERT signals as an IRQs.
+ * TALERT is a normal IRQ and it is fired any time thresholds (hot or cold)
+ * are violated. In these situation, the driver must reprogram the thresholds,
+ * accordingly to specified policy.
+ *
+ * Return: 0 if no error, else return corresponding error.
+ */
+static int ti_bandgap_talert_init(struct ti_bandgap *bgp,
+				  struct platform_device *pdev)
+{
+	int ret;
+
+	bgp->irq = platform_get_irq(pdev, 0);
+	if (bgp->irq < 0) {
+		dev_err(&pdev->dev, "get_irq failed\n");
+		return bgp->irq;
+	}
+	ret = request_threaded_irq(bgp->irq, NULL,
+				   ti_bandgap_talert_irq_handler,
+				   IRQF_TRIGGER_HIGH | IRQF_ONESHOT,
+				   "talert", bgp);
+	if (ret) {
+		dev_err(&pdev->dev, "Request threaded irq failed.\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static const struct of_device_id of_ti_bandgap_match[];
+/**
+ * ti_bandgap_build() - parse DT and setup a struct ti_bandgap
+ * @pdev: pointer to device struct platform_device
+ *
+ * Used to read the device tree properties accordingly to the bandgap
+ * matching version. Based on bandgap version and its capabilities it
+ * will build a struct ti_bandgap out of the required DT entries.
+ *
+ * Return: valid bandgap structure if successful, else returns ERR_PTR
+ * return value must be verified with IS_ERR.
+ */
+static struct ti_bandgap *ti_bandgap_build(struct platform_device *pdev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	const struct of_device_id *of_id;
+	struct ti_bandgap *bgp;
+	struct resource *res;
+	u32 prop;
+	int i;
+
+	/* just for the sake */
+	if (!node) {
+		dev_err(&pdev->dev, "no platform information available\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	bgp = devm_kzalloc(&pdev->dev, sizeof(*bgp), GFP_KERNEL);
+	if (!bgp) {
+		dev_err(&pdev->dev, "Unable to allocate mem for driver ref\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	of_id = of_match_device(of_ti_bandgap_match, &pdev->dev);
+	if (of_id)
+		bgp->conf = of_id->data;
+
+	/* register shadow for context save and restore */
+	bgp->regval = devm_kzalloc(&pdev->dev, sizeof(*bgp->regval) *
+				   bgp->conf->sensor_count, GFP_KERNEL);
+	if (!bgp) {
+		dev_err(&pdev->dev, "Unable to allocate mem for driver ref\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	i = 0;
+	do {
+		void __iomem *chunk;
+
+		res = platform_get_resource(pdev, IORESOURCE_MEM, i);
+		if (!res)
+			break;
+		chunk = devm_ioremap_resource(&pdev->dev, res);
+		if (i == 0)
+			bgp->base = chunk;
+		if (IS_ERR(chunk))
+			return ERR_CAST(chunk);
+
+		i++;
+	} while (res);
+
+	if (TI_BANDGAP_HAS(bgp, TSHUT)) {
+		if (of_property_read_u32(node, "ti,tshut-gpio", &prop) < 0) {
+			dev_err(&pdev->dev, "missing tshut gpio in device tree\n");
+			return ERR_PTR(-EINVAL);
+		}
+		bgp->tshut_gpio = prop;
+		if (!gpio_is_valid(bgp->tshut_gpio)) {
+			dev_err(&pdev->dev, "invalid gpio for tshut (%d)\n",
+				bgp->tshut_gpio);
+			return ERR_PTR(-EINVAL);
+		}
+	}
+
+	return bgp;
+}
+
+/***   Device driver call backs   ***/
+
+static
+int ti_bandgap_probe(struct platform_device *pdev)
+{
+	struct ti_bandgap *bgp;
+	int clk_rate, ret = 0, i;
+
+	bgp = ti_bandgap_build(pdev);
+	if (IS_ERR_OR_NULL(bgp)) {
+		dev_err(&pdev->dev, "failed to fetch platform data\n");
+		return PTR_ERR(bgp);
+	}
+	bgp->dev = &pdev->dev;
+
+	if (TI_BANDGAP_HAS(bgp, TSHUT)) {
+		ret = ti_bandgap_tshut_init(bgp, pdev);
+		if (ret) {
+			dev_err(&pdev->dev,
+				"failed to initialize system tshut IRQ\n");
+			return ret;
+		}
+	}
+
+	bgp->fclock = clk_get(NULL, bgp->conf->fclock_name);
+	ret = IS_ERR_OR_NULL(bgp->fclock);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to request fclock reference\n");
+		goto free_irqs;
+	}
+
+	bgp->div_clk = clk_get(NULL,  bgp->conf->div_ck_name);
+	ret = IS_ERR_OR_NULL(bgp->div_clk);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"failed to request div_ts_ck clock ref\n");
+		goto free_irqs;
+	}
+
+	for (i = 0; i < bgp->conf->sensor_count; i++) {
+		struct temp_sensor_registers *tsr;
+		u32 val;
+
+		tsr = bgp->conf->sensors[i].registers;
+		/*
+		 * check if the efuse has a non-zero value if not
+		 * it is an untrimmed sample and the temperatures
+		 * may not be accurate
+		 */
+		val = ti_bandgap_readl(bgp, tsr->bgap_efuse);
+		if (ret || !val)
+			dev_info(&pdev->dev,
+				 "Non-trimmed BGAP, Temp not accurate\n");
+	}
+
+	clk_rate = clk_round_rate(bgp->div_clk,
+				  bgp->conf->sensors[0].ts_data->max_freq);
+	if (clk_rate < bgp->conf->sensors[0].ts_data->min_freq ||
+	    clk_rate == 0xffffffff) {
+		ret = -ENODEV;
+		dev_err(&pdev->dev, "wrong clock rate (%d)\n", clk_rate);
+		goto put_clks;
+	}
+
+	ret = clk_set_rate(bgp->div_clk, clk_rate);
+	if (ret)
+		dev_err(&pdev->dev, "Cannot re-set clock rate. Continuing\n");
+
+	bgp->clk_rate = clk_rate;
+	if (TI_BANDGAP_HAS(bgp, CLK_CTRL))
+		clk_prepare_enable(bgp->fclock);
+
+
+	spin_lock_init(&bgp->lock);
+	bgp->dev = &pdev->dev;
+	platform_set_drvdata(pdev, bgp);
+
+	ti_bandgap_power(bgp, true);
+
+	/* Set default counter to 1 for now */
+	if (TI_BANDGAP_HAS(bgp, COUNTER))
+		for (i = 0; i < bgp->conf->sensor_count; i++)
+			RMW_BITS(bgp, i, bgap_counter, counter_mask, 1);
+
+	/* Set default thresholds for alert and shutdown */
+	for (i = 0; i < bgp->conf->sensor_count; i++) {
+		struct temp_sensor_data *ts_data;
+
+		ts_data = bgp->conf->sensors[i].ts_data;
+
+		if (TI_BANDGAP_HAS(bgp, TALERT)) {
+			/* Set initial Talert thresholds */
+			RMW_BITS(bgp, i, bgap_threshold,
+				 threshold_tcold_mask, ts_data->t_cold);
+			RMW_BITS(bgp, i, bgap_threshold,
+				 threshold_thot_mask, ts_data->t_hot);
+			/* Enable the alert events */
+			RMW_BITS(bgp, i, bgap_mask_ctrl, mask_hot_mask, 1);
+			RMW_BITS(bgp, i, bgap_mask_ctrl, mask_cold_mask, 1);
+		}
+
+		if (TI_BANDGAP_HAS(bgp, TSHUT_CONFIG)) {
+			/* Set initial Tshut thresholds */
+			RMW_BITS(bgp, i, tshut_threshold,
+				 tshut_hot_mask, ts_data->tshut_hot);
+			RMW_BITS(bgp, i, tshut_threshold,
+				 tshut_cold_mask, ts_data->tshut_cold);
+		}
+	}
+
+	if (TI_BANDGAP_HAS(bgp, MODE_CONFIG))
+		ti_bandgap_set_continuous_mode(bgp);
+
+	/* Set .250 seconds time as default counter */
+	if (TI_BANDGAP_HAS(bgp, COUNTER))
+		for (i = 0; i < bgp->conf->sensor_count; i++)
+			RMW_BITS(bgp, i, bgap_counter, counter_mask,
+				 bgp->clk_rate / 4);
+
+	/* Every thing is good? Then expose the sensors */
+	for (i = 0; i < bgp->conf->sensor_count; i++) {
+		char *domain;
+
+		if (bgp->conf->sensors[i].register_cooling) {
+			ret = bgp->conf->sensors[i].register_cooling(bgp, i);
+			if (ret)
+				goto remove_sensors;
+		}
+
+		if (bgp->conf->expose_sensor) {
+			domain = bgp->conf->sensors[i].domain;
+			ret = bgp->conf->expose_sensor(bgp, i, domain);
+			if (ret)
+				goto remove_last_cooling;
+		}
+	}
+
+	/*
+	 * Enable the Interrupts once everything is set. Otherwise irq handler
+	 * might be called as soon as it is enabled where as rest of framework
+	 * is still getting initialised.
+	 */
+	if (TI_BANDGAP_HAS(bgp, TALERT)) {
+		ret = ti_bandgap_talert_init(bgp, pdev);
+		if (ret) {
+			dev_err(&pdev->dev, "failed to initialize Talert IRQ\n");
+			i = bgp->conf->sensor_count;
+			goto disable_clk;
+		}
+	}
+
+	return 0;
+
+remove_last_cooling:
+	if (bgp->conf->sensors[i].unregister_cooling)
+		bgp->conf->sensors[i].unregister_cooling(bgp, i);
+remove_sensors:
+	for (i--; i >= 0; i--) {
+		if (bgp->conf->sensors[i].unregister_cooling)
+			bgp->conf->sensors[i].unregister_cooling(bgp, i);
+		if (bgp->conf->remove_sensor)
+			bgp->conf->remove_sensor(bgp, i);
+	}
+	ti_bandgap_power(bgp, false);
+disable_clk:
+	if (TI_BANDGAP_HAS(bgp, CLK_CTRL))
+		clk_disable_unprepare(bgp->fclock);
+put_clks:
+	clk_put(bgp->fclock);
+	clk_put(bgp->div_clk);
+free_irqs:
+	if (TI_BANDGAP_HAS(bgp, TSHUT)) {
+		free_irq(gpio_to_irq(bgp->tshut_gpio), NULL);
+		gpio_free(bgp->tshut_gpio);
+	}
+
+	return ret;
+}
+
+static
+int ti_bandgap_remove(struct platform_device *pdev)
+{
+	struct ti_bandgap *bgp = platform_get_drvdata(pdev);
+	int i;
+
+	/* First thing is to remove sensor interfaces */
+	for (i = 0; i < bgp->conf->sensor_count; i++) {
+		if (bgp->conf->sensors[i].unregister_cooling)
+			bgp->conf->sensors[i].unregister_cooling(bgp, i);
+
+		if (bgp->conf->remove_sensor)
+			bgp->conf->remove_sensor(bgp, i);
+	}
+
+	ti_bandgap_power(bgp, false);
+
+	if (TI_BANDGAP_HAS(bgp, CLK_CTRL))
+		clk_disable_unprepare(bgp->fclock);
+	clk_put(bgp->fclock);
+	clk_put(bgp->div_clk);
+
+	if (TI_BANDGAP_HAS(bgp, TALERT))
+		free_irq(bgp->irq, bgp);
+
+	if (TI_BANDGAP_HAS(bgp, TSHUT)) {
+		free_irq(gpio_to_irq(bgp->tshut_gpio), NULL);
+		gpio_free(bgp->tshut_gpio);
+	}
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int ti_bandgap_save_ctxt(struct ti_bandgap *bgp)
+{
+	int i;
+
+	for (i = 0; i < bgp->conf->sensor_count; i++) {
+		struct temp_sensor_registers *tsr;
+		struct temp_sensor_regval *rval;
+
+		rval = &bgp->regval[i];
+		tsr = bgp->conf->sensors[i].registers;
+
+		if (TI_BANDGAP_HAS(bgp, MODE_CONFIG))
+			rval->bg_mode_ctrl = ti_bandgap_readl(bgp,
+							tsr->bgap_mode_ctrl);
+		if (TI_BANDGAP_HAS(bgp, COUNTER))
+			rval->bg_counter = ti_bandgap_readl(bgp,
+							tsr->bgap_counter);
+		if (TI_BANDGAP_HAS(bgp, TALERT)) {
+			rval->bg_threshold = ti_bandgap_readl(bgp,
+							tsr->bgap_threshold);
+			rval->bg_ctrl = ti_bandgap_readl(bgp,
+						   tsr->bgap_mask_ctrl);
+		}
+
+		if (TI_BANDGAP_HAS(bgp, TSHUT_CONFIG))
+			rval->tshut_threshold = ti_bandgap_readl(bgp,
+						   tsr->tshut_threshold);
+	}
+
+	return 0;
+}
+
+static int ti_bandgap_restore_ctxt(struct ti_bandgap *bgp)
+{
+	int i;
+
+	for (i = 0; i < bgp->conf->sensor_count; i++) {
+		struct temp_sensor_registers *tsr;
+		struct temp_sensor_regval *rval;
+		u32 val = 0;
+
+		rval = &bgp->regval[i];
+		tsr = bgp->conf->sensors[i].registers;
+
+		if (TI_BANDGAP_HAS(bgp, COUNTER))
+			val = ti_bandgap_readl(bgp, tsr->bgap_counter);
+
+		if (TI_BANDGAP_HAS(bgp, TSHUT_CONFIG))
+			ti_bandgap_writel(bgp, rval->tshut_threshold,
+					  tsr->tshut_threshold);
+		/* Force immediate temperature measurement and update
+		 * of the DTEMP field
+		 */
+		ti_bandgap_force_single_read(bgp, i);
+
+		if (TI_BANDGAP_HAS(bgp, COUNTER))
+			ti_bandgap_writel(bgp, rval->bg_counter,
+					  tsr->bgap_counter);
+		if (TI_BANDGAP_HAS(bgp, MODE_CONFIG))
+			ti_bandgap_writel(bgp, rval->bg_mode_ctrl,
+					  tsr->bgap_mode_ctrl);
+		if (TI_BANDGAP_HAS(bgp, TALERT)) {
+			ti_bandgap_writel(bgp, rval->bg_threshold,
+					  tsr->bgap_threshold);
+			ti_bandgap_writel(bgp, rval->bg_ctrl,
+					  tsr->bgap_mask_ctrl);
+		}
+	}
+
+	return 0;
+}
+
+static int ti_bandgap_suspend(struct device *dev)
+{
+	struct ti_bandgap *bgp = dev_get_drvdata(dev);
+	int err;
+
+	err = ti_bandgap_save_ctxt(bgp);
+	ti_bandgap_power(bgp, false);
+
+	if (TI_BANDGAP_HAS(bgp, CLK_CTRL))
+		clk_disable_unprepare(bgp->fclock);
+
+	return err;
+}
+
+static int ti_bandgap_resume(struct device *dev)
+{
+	struct ti_bandgap *bgp = dev_get_drvdata(dev);
+
+	if (TI_BANDGAP_HAS(bgp, CLK_CTRL))
+		clk_prepare_enable(bgp->fclock);
+
+	ti_bandgap_power(bgp, true);
+
+	return ti_bandgap_restore_ctxt(bgp);
+}
+static const struct dev_pm_ops ti_bandgap_dev_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(ti_bandgap_suspend,
+				ti_bandgap_resume)
+};
+
+#define DEV_PM_OPS	(&ti_bandgap_dev_pm_ops)
+#else
+#define DEV_PM_OPS	NULL
+#endif
+
+static const struct of_device_id of_ti_bandgap_match[] = {
+#ifdef CONFIG_OMAP4_THERMAL
+	{
+		.compatible = "ti,omap4430-bandgap",
+		.data = (void *)&omap4430_data,
+	},
+	{
+		.compatible = "ti,omap4460-bandgap",
+		.data = (void *)&omap4460_data,
+	},
+	{
+		.compatible = "ti,omap4470-bandgap",
+		.data = (void *)&omap4470_data,
+	},
+#endif
+#ifdef CONFIG_OMAP5_THERMAL
+	{
+		.compatible = "ti,omap5430-bandgap",
+		.data = (void *)&omap5430_data,
+	},
+#endif
+	/* Sentinel */
+	{ },
+};
+MODULE_DEVICE_TABLE(of, of_ti_bandgap_match);
+
+static struct platform_driver ti_bandgap_sensor_driver = {
+	.probe = ti_bandgap_probe,
+	.remove = ti_bandgap_remove,
+	.driver = {
+			.name = "ti-soc-thermal",
+			.pm = DEV_PM_OPS,
+			.of_match_table	= of_ti_bandgap_match,
+	},
+};
+
+module_platform_driver(ti_bandgap_sensor_driver);
+
+MODULE_DESCRIPTION("OMAP4+ bandgap temperature sensor driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:ti-soc-thermal");
+MODULE_AUTHOR("Texas Instrument Inc.");
diff --git a/drivers/staging/ti-soc-thermal/ti-bandgap.h b/drivers/staging/ti-soc-thermal/ti-bandgap.h
new file mode 100644
index 00000000000..5f4794abf58
--- /dev/null
+++ b/drivers/staging/ti-soc-thermal/ti-bandgap.h
@@ -0,0 +1,403 @@
+/*
+ * OMAP4 Bandgap temperature sensor driver
+ *
+ * Copyright (C) 2011 Texas Instruments Incorporated - http://www.ti.com/
+ * Contact:
+ *   Eduardo Valentin <eduardo.valentin@ti.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 __TI_BANDGAP_H
+#define __TI_BANDGAP_H
+
+#include <linux/spinlock.h>
+#include <linux/types.h>
+#include <linux/err.h>
+
+/**
+ * DOC: bandgap driver data structure
+ * ==================================
+ *
+ *   +----------+----------------+
+ *   | struct temp_sensor_regval |
+ *   +---------------------------+
+ *              * (Array of)
+ *              |
+ *              |
+ *   +-------------------+   +-----------------+
+ *   | struct ti_bandgap |-->| struct device * |
+ *   +----------+--------+   +-----------------+
+ *              |
+ *              |
+ *              V
+ *   +------------------------+
+ *   | struct ti_bandgap_data |
+ *   +------------------------+
+ *              |
+ *              |
+ *              * (Array of)
+ * +------------+------------------------------------------------------+
+ * | +----------+------------+   +-------------------------+           |
+ * | | struct ti_temp_sensor |-->| struct temp_sensor_data |           |
+ * | +-----------------------+   +------------+------------+           |
+ * |            |                                                      |
+ * |            +                                                      |
+ * |            V                                                      |
+ * | +----------+-------------------+                                  |
+ * | | struct temp_sensor_registers |                                  |
+ * | +------------------------------+                                  |
+ * |                                                                   |
+ * +-------------------------------------------------------------------+
+ *
+ * Above is a simple diagram describing how the data structure below
+ * are organized. For each bandgap device there should be a ti_bandgap_data
+ * containing the device instance configuration, as well as, an array of
+ * sensors, representing every sensor instance present in this bandgap.
+ */
+
+/**
+ * struct temp_sensor_registers - descriptor to access registers and bitfields
+ * @temp_sensor_ctrl: TEMP_SENSOR_CTRL register offset
+ * @bgap_tempsoff_mask: mask to temp_sensor_ctrl.tempsoff
+ * @bgap_soc_mask: mask to temp_sensor_ctrl.soc
+ * @bgap_eocz_mask: mask to temp_sensor_ctrl.eocz
+ * @bgap_dtemp_mask: mask to temp_sensor_ctrl.dtemp
+ * @bgap_mask_ctrl: BANDGAP_MASK_CTRL register offset
+ * @mask_hot_mask: mask to bandgap_mask_ctrl.mask_hot
+ * @mask_cold_mask: mask to bandgap_mask_ctrl.mask_cold
+ * @mask_sidlemode_mask: mask to bandgap_mask_ctrl.mask_sidlemode
+ * @mask_counter_delay_mask: mask to bandgap_mask_ctrl.mask_counter_delay
+ * @mask_freeze_mask: mask to bandgap_mask_ctrl.mask_free
+ * @mask_clear_mask: mask to bandgap_mask_ctrl.mask_clear
+ * @mask_clear_accum_mask: mask to bandgap_mask_ctrl.mask_clear_accum
+ * @bgap_mode_ctrl: BANDGAP_MODE_CTRL register offset
+ * @mode_ctrl_mask: mask to bandgap_mode_ctrl.mode_ctrl
+ * @bgap_counter: BANDGAP_COUNTER register offset
+ * @counter_mask: mask to bandgap_counter.counter
+ * @bgap_threshold: BANDGAP_THRESHOLD register offset (TALERT thresholds)
+ * @threshold_thot_mask: mask to bandgap_threhold.thot
+ * @threshold_tcold_mask: mask to bandgap_threhold.tcold
+ * @tshut_threshold: TSHUT_THRESHOLD register offset (TSHUT thresholds)
+ * @tshut_efuse_mask: mask to tshut_threshold.tshut_efuse
+ * @tshut_efuse_shift: shift to tshut_threshold.tshut_efuse
+ * @tshut_hot_mask: mask to tshut_threhold.thot
+ * @tshut_cold_mask: mask to tshut_threhold.thot
+ * @bgap_status: BANDGAP_STATUS register offset
+ * @status_clean_stop_mask: mask to bandgap_status.clean_stop
+ * @status_bgap_alert_mask: mask to bandgap_status.bandgap_alert
+ * @status_hot_mask: mask to bandgap_status.hot
+ * @status_cold_mask: mask to bandgap_status.cold
+ * @bgap_cumul_dtemp: BANDGAP_CUMUL_DTEMP register offset
+ * @ctrl_dtemp_0: CTRL_DTEMP0 register offset
+ * @ctrl_dtemp_1: CTRL_DTEMP1 register offset
+ * @ctrl_dtemp_2: CTRL_DTEMP2 register offset
+ * @ctrl_dtemp_3: CTRL_DTEMP3 register offset
+ * @ctrl_dtemp_4: CTRL_DTEMP4 register offset
+ * @bgap_efuse: BANDGAP_EFUSE register offset
+ *
+ * The register offsets and bitfields might change across
+ * OMAP and variants versions. Hence this struct serves as a
+ * descriptor map on how to access the registers and the bitfields.
+ *
+ * This descriptor contains registers of all versions of bandgap chips.
+ * Not all versions will use all registers, depending on the available
+ * features. Please read TRMs for descriptive explanation on each bitfield.
+ */
+
+struct temp_sensor_registers {
+	u32	temp_sensor_ctrl;
+	u32	bgap_tempsoff_mask;
+	u32	bgap_soc_mask;
+	u32	bgap_eocz_mask; /* not used: but needs revisit */
+	u32	bgap_dtemp_mask;
+
+	u32	bgap_mask_ctrl;
+	u32	mask_hot_mask;
+	u32	mask_cold_mask;
+	u32	mask_sidlemode_mask; /* not used: but may be needed for pm */
+	u32	mask_counter_delay_mask;
+	u32	mask_freeze_mask;
+	u32	mask_clear_mask; /* not used: but needed for trending */
+	u32	mask_clear_accum_mask; /* not used: but needed for trending */
+
+	u32	bgap_mode_ctrl;
+	u32	mode_ctrl_mask;
+
+	u32	bgap_counter;
+	u32	counter_mask;
+
+	u32	bgap_threshold;
+	u32	threshold_thot_mask;
+	u32	threshold_tcold_mask;
+
+	u32	tshut_threshold;
+	u32	tshut_efuse_mask; /* not used */
+	u32	tshut_efuse_shift; /* not used */
+	u32	tshut_hot_mask;
+	u32	tshut_cold_mask;
+
+	u32	bgap_status;
+	u32	status_clean_stop_mask; /* not used: but needed for trending */
+	u32	status_bgap_alert_mask; /* not used */
+	u32	status_hot_mask;
+	u32	status_cold_mask;
+
+	u32	bgap_cumul_dtemp; /* not used: but needed for trending */
+	u32	ctrl_dtemp_0; /* not used: but needed for trending */
+	u32	ctrl_dtemp_1; /* not used: but needed for trending */
+	u32	ctrl_dtemp_2; /* not used: but needed for trending */
+	u32	ctrl_dtemp_3; /* not used: but needed for trending */
+	u32	ctrl_dtemp_4; /* not used: but needed for trending */
+	u32	bgap_efuse;
+};
+
+/**
+ * struct temp_sensor_data - The thresholds and limits for temperature sensors.
+ * @tshut_hot: temperature to trigger a thermal reset (initial value)
+ * @tshut_cold: temp to get the plat out of reset due to thermal (init val)
+ * @t_hot: temperature to trigger a thermal alert (high initial value)
+ * @t_cold: temperature to trigger a thermal alert (low initial value)
+ * @min_freq: sensor minimum clock rate
+ * @max_freq: sensor maximum clock rate
+ * @max_temp: sensor maximum temperature
+ * @min_temp: sensor minimum temperature
+ * @hyst_val: temperature hysteresis considered while converting ADC values
+ * @update_int1: update interval
+ * @update_int2: update interval
+ *
+ * This data structure will hold the required thresholds and temperature limits
+ * for a specific temperature sensor, like shutdown temperature, alert
+ * temperature, clock / rate used, ADC conversion limits and update intervals
+ */
+struct temp_sensor_data {
+	u32	tshut_hot;
+	u32	tshut_cold;
+	u32	t_hot;
+	u32	t_cold;
+	u32	min_freq;
+	u32	max_freq;
+	int     max_temp;
+	int     min_temp;
+	int     hyst_val;
+	u32     update_int1; /* not used */
+	u32     update_int2; /* not used */
+};
+
+struct ti_bandgap_data;
+
+/**
+ * struct temp_sensor_regval - temperature sensor register values and priv data
+ * @bg_mode_ctrl: temp sensor control register value
+ * @bg_ctrl: bandgap ctrl register value
+ * @bg_counter: bandgap counter value
+ * @bg_threshold: bandgap threshold register value
+ * @tshut_threshold: bandgap tshut register value
+ * @data: private data
+ *
+ * Data structure to save and restore bandgap register set context. Only
+ * required registers are shadowed, when needed.
+ */
+struct temp_sensor_regval {
+	u32			bg_mode_ctrl;
+	u32			bg_ctrl;
+	u32			bg_counter;
+	u32			bg_threshold;
+	u32			tshut_threshold;
+	void			*data;
+};
+
+/**
+ * struct ti_bandgap - bandgap device structure
+ * @dev: struct device pointer
+ * @base: io memory base address
+ * @conf: struct with bandgap configuration set (# sensors, conv_table, etc)
+ * @regval: temperature sensor register values
+ * @fclock: pointer to functional clock of temperature sensor
+ * @div_clk: pointer to divider clock of temperature sensor fclk
+ * @lock: spinlock for ti_bandgap structure
+ * @irq: MPU IRQ number for thermal alert
+ * @tshut_gpio: GPIO where Tshut signal is routed
+ * @clk_rate: Holds current clock rate
+ *
+ * The bandgap device structure representing the bandgap device instance.
+ * It holds most of the dynamic stuff. Configurations and sensor specific
+ * entries are inside the @conf structure.
+ */
+struct ti_bandgap {
+	struct device			*dev;
+	void __iomem			*base;
+	const struct ti_bandgap_data	*conf;
+	struct temp_sensor_regval	*regval;
+	struct clk			*fclock;
+	struct clk			*div_clk;
+	spinlock_t			lock; /* shields this struct */
+	int				irq;
+	int				tshut_gpio;
+	u32				clk_rate;
+};
+
+/**
+ * struct ti_temp_sensor - bandgap temperature sensor configuration data
+ * @ts_data: pointer to struct with thresholds, limits of temperature sensor
+ * @registers: pointer to the list of register offsets and bitfields
+ * @domain: the name of the domain where the sensor is located
+ * @slope: sensor gradient slope info for hotspot extrapolation equation
+ * @constant: sensor gradient const info for hotspot extrapolation equation
+ * @slope_pcb: sensor gradient slope info for hotspot extrapolation equation
+ *             with no external influence
+ * @constant_pcb: sensor gradient const info for hotspot extrapolation equation
+ *             with no external influence
+ * @register_cooling: function to describe how this sensor is going to be cooled
+ * @unregister_cooling: function to release cooling data
+ *
+ * Data structure to describe a temperature sensor handled by a bandgap device.
+ * It should provide configuration details on this sensor, such as how to
+ * access the registers affecting this sensor, shadow register buffer, how to
+ * assess the gradient from hotspot, how to cooldown the domain when sensor
+ * reports too hot temperature.
+ */
+struct ti_temp_sensor {
+	struct temp_sensor_data		*ts_data;
+	struct temp_sensor_registers	*registers;
+	char				*domain;
+	/* for hotspot extrapolation */
+	const int			slope;
+	const int			constant;
+	const int			slope_pcb;
+	const int			constant_pcb;
+	int (*register_cooling)(struct ti_bandgap *bgp, int id);
+	int (*unregister_cooling)(struct ti_bandgap *bgp, int id);
+};
+
+/**
+ * DOC: ti bandgap feature types
+ *
+ * TI_BANDGAP_FEATURE_TSHUT - used when the thermal shutdown signal output
+ *      of a bandgap device instance is routed to the processor. This means
+ *      the system must react and perform the shutdown by itself (handle an
+ *      IRQ, for instance).
+ *
+ * TI_BANDGAP_FEATURE_TSHUT_CONFIG - used when the bandgap device has control
+ *      over the thermal shutdown configuration. This means that the thermal
+ *      shutdown thresholds are programmable, for instance.
+ *
+ * TI_BANDGAP_FEATURE_TALERT - used when the bandgap device instance outputs
+ *      a signal representing violation of programmable alert thresholds.
+ *
+ * TI_BANDGAP_FEATURE_MODE_CONFIG - used when it is possible to choose which
+ *      mode, continuous or one shot, the bandgap device instance will operate.
+ *
+ * TI_BANDGAP_FEATURE_COUNTER - used when the bandgap device instance allows
+ *      programming the update interval of its internal state machine.
+ *
+ * TI_BANDGAP_FEATURE_POWER_SWITCH - used when the bandgap device allows
+ *      itself to be switched on/off.
+ *
+ * TI_BANDGAP_FEATURE_CLK_CTRL - used when the clocks feeding the bandgap
+ *      device are gateable or not.
+ *
+ * TI_BANDGAP_FEATURE_FREEZE_BIT - used when the bandgap device features
+ *      a history buffer that its update can be freezed/unfreezed.
+ *
+ * TI_BANDGAP_FEATURE_COUNTER_DELAY - used when the bandgap device features
+ *	a delay programming based on distinct values.
+ *
+ * TI_BANDGAP_FEATURE_HISTORY_BUFFER - used when the bandgap device features
+ *	a history buffer of temperatures.
+ *
+ * TI_BANDGAP_HAS(b, f) - macro to check if a bandgap device is capable of a
+ *      specific feature (above) or not. Return non-zero, if yes.
+ */
+#define TI_BANDGAP_FEATURE_TSHUT		BIT(0)
+#define TI_BANDGAP_FEATURE_TSHUT_CONFIG		BIT(1)
+#define TI_BANDGAP_FEATURE_TALERT		BIT(2)
+#define TI_BANDGAP_FEATURE_MODE_CONFIG		BIT(3)
+#define TI_BANDGAP_FEATURE_COUNTER		BIT(4)
+#define TI_BANDGAP_FEATURE_POWER_SWITCH		BIT(5)
+#define TI_BANDGAP_FEATURE_CLK_CTRL		BIT(6)
+#define TI_BANDGAP_FEATURE_FREEZE_BIT		BIT(7)
+#define TI_BANDGAP_FEATURE_COUNTER_DELAY	BIT(8)
+#define TI_BANDGAP_FEATURE_HISTORY_BUFFER	BIT(9)
+#define TI_BANDGAP_HAS(b, f)			\
+			((b)->conf->features & TI_BANDGAP_FEATURE_ ## f)
+
+/**
+ * struct ti_bandgap_data - ti bandgap data configuration structure
+ * @features: a bitwise flag set to describe the device features
+ * @conv_table: Pointer to ADC to temperature conversion table
+ * @adc_start_val: ADC conversion table starting value
+ * @adc_end_val: ADC conversion table ending value
+ * @fclock_name: clock name of the functional clock
+ * @div_ck_name: clock name of the clock divisor
+ * @sensor_count: count of temperature sensor within this bandgap device
+ * @report_temperature: callback to report thermal alert to thermal API
+ * @expose_sensor: callback to export sensor to thermal API
+ * @remove_sensor: callback to destroy sensor from thermal API
+ * @sensors: array of sensors present in this bandgap instance
+ *
+ * This is a data structure which should hold most of the static configuration
+ * of a bandgap device instance. It should describe which features this instance
+ * is capable of, the clock names to feed this device, the amount of sensors and
+ * their configuration representation, and how to export and unexport them to
+ * a thermal API.
+ */
+struct ti_bandgap_data {
+	unsigned int			features;
+	const int			*conv_table;
+	u32				adc_start_val;
+	u32				adc_end_val;
+	char				*fclock_name;
+	char				*div_ck_name;
+	int				sensor_count;
+	int (*report_temperature)(struct ti_bandgap *bgp, int id);
+	int (*expose_sensor)(struct ti_bandgap *bgp, int id, char *domain);
+	int (*remove_sensor)(struct ti_bandgap *bgp, int id);
+
+	/* this needs to be at the end */
+	struct ti_temp_sensor		sensors[];
+};
+
+int ti_bandgap_read_thot(struct ti_bandgap *bgp, int id, int *thot);
+int ti_bandgap_write_thot(struct ti_bandgap *bgp, int id, int val);
+int ti_bandgap_read_tcold(struct ti_bandgap *bgp, int id, int *tcold);
+int ti_bandgap_write_tcold(struct ti_bandgap *bgp, int id, int val);
+int ti_bandgap_read_update_interval(struct ti_bandgap *bgp, int id,
+				    int *interval);
+int ti_bandgap_write_update_interval(struct ti_bandgap *bgp, int id,
+				     u32 interval);
+int ti_bandgap_read_temperature(struct ti_bandgap *bgp, int id,
+				  int *temperature);
+int ti_bandgap_set_sensor_data(struct ti_bandgap *bgp, int id, void *data);
+void *ti_bandgap_get_sensor_data(struct ti_bandgap *bgp, int id);
+int ti_bandgap_get_trend(struct ti_bandgap *bgp, int id, int *trend);
+
+#ifdef CONFIG_OMAP4_THERMAL
+extern const struct ti_bandgap_data omap4430_data;
+extern const struct ti_bandgap_data omap4460_data;
+extern const struct ti_bandgap_data omap4470_data;
+#else
+#define omap4430_data					NULL
+#define omap4460_data					NULL
+#define omap4470_data					NULL
+#endif
+
+#ifdef CONFIG_OMAP5_THERMAL
+extern const struct ti_bandgap_data omap5430_data;
+#else
+#define omap5430_data					NULL
+#endif
+
+#endif
diff --git a/drivers/staging/ti-soc-thermal/ti-thermal-common.c b/drivers/staging/ti-soc-thermal/ti-thermal-common.c
new file mode 100644
index 00000000000..e3c5e677eaa
--- /dev/null
+++ b/drivers/staging/ti-soc-thermal/ti-thermal-common.c
@@ -0,0 +1,367 @@
+/*
+ * OMAP thermal driver interface
+ *
+ * Copyright (C) 2012 Texas Instruments Incorporated - http://www.ti.com/
+ * Contact:
+ *   Eduardo Valentin <eduardo.valentin@ti.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/device.h>
+#include <linux/err.h>
+#include <linux/mutex.h>
+#include <linux/gfp.h>
+#include <linux/kernel.h>
+#include <linux/workqueue.h>
+#include <linux/thermal.h>
+#include <linux/cpufreq.h>
+#include <linux/cpumask.h>
+#include <linux/cpu_cooling.h>
+
+#include "ti-thermal.h"
+#include "ti-bandgap.h"
+
+/* common data structures */
+struct ti_thermal_data {
+	struct thermal_zone_device *ti_thermal;
+	struct thermal_cooling_device *cool_dev;
+	struct ti_bandgap *bgp;
+	enum thermal_device_mode mode;
+	struct work_struct thermal_wq;
+	int sensor_id;
+};
+
+static void ti_thermal_work(struct work_struct *work)
+{
+	struct ti_thermal_data *data = container_of(work,
+					struct ti_thermal_data, thermal_wq);
+
+	thermal_zone_device_update(data->ti_thermal);
+
+	dev_dbg(&data->ti_thermal->device, "updated thermal zone %s\n",
+		data->ti_thermal->type);
+}
+
+/**
+ * ti_thermal_hotspot_temperature - returns sensor extrapolated temperature
+ * @t:	omap sensor temperature
+ * @s:	omap sensor slope value
+ * @c:	omap sensor const value
+ */
+static inline int ti_thermal_hotspot_temperature(int t, int s, int c)
+{
+	int delta = t * s / 1000 + c;
+
+	if (delta < 0)
+		delta = 0;
+
+	return t + delta;
+}
+
+/* thermal zone ops */
+/* Get temperature callback function for thermal zone*/
+static inline int ti_thermal_get_temp(struct thermal_zone_device *thermal,
+				      unsigned long *temp)
+{
+	struct ti_thermal_data *data = thermal->devdata;
+	struct ti_bandgap *bgp;
+	const struct ti_temp_sensor *s;
+	int ret, tmp, pcb_temp, slope, constant;
+
+	if (!data)
+		return 0;
+
+	bgp = data->bgp;
+	s = &bgp->conf->sensors[data->sensor_id];
+
+	ret = ti_bandgap_read_temperature(bgp, data->sensor_id, &tmp);
+	if (ret)
+		return ret;
+
+	pcb_temp = 0;
+	/* TODO: Introduce pcb temperature lookup */
+	/* In case pcb zone is available, use the extrapolation rule with it */
+	if (pcb_temp) {
+		tmp -= pcb_temp;
+		slope = s->slope_pcb;
+		constant = s->constant_pcb;
+	} else {
+		slope = s->slope;
+		constant = s->constant;
+	}
+	*temp = ti_thermal_hotspot_temperature(tmp, slope, constant);
+
+	return ret;
+}
+
+/* Bind callback functions for thermal zone */
+static int ti_thermal_bind(struct thermal_zone_device *thermal,
+			   struct thermal_cooling_device *cdev)
+{
+	struct ti_thermal_data *data = thermal->devdata;
+	int id;
+
+	if (IS_ERR_OR_NULL(data))
+		return -ENODEV;
+
+	/* check if this is the cooling device we registered */
+	if (data->cool_dev != cdev)
+		return 0;
+
+	id = data->sensor_id;
+
+	/* Simple thing, two trips, one passive another critical */
+	return thermal_zone_bind_cooling_device(thermal, 0, cdev,
+	/* bind with min and max states defined by cpu_cooling */
+						THERMAL_NO_LIMIT,
+						THERMAL_NO_LIMIT);
+}
+
+/* Unbind callback functions for thermal zone */
+static int ti_thermal_unbind(struct thermal_zone_device *thermal,
+			     struct thermal_cooling_device *cdev)
+{
+	struct ti_thermal_data *data = thermal->devdata;
+
+	if (IS_ERR_OR_NULL(data))
+		return -ENODEV;
+
+	/* check if this is the cooling device we registered */
+	if (data->cool_dev != cdev)
+		return 0;
+
+	/* Simple thing, two trips, one passive another critical */
+	return thermal_zone_unbind_cooling_device(thermal, 0, cdev);
+}
+
+/* Get mode callback functions for thermal zone */
+static int ti_thermal_get_mode(struct thermal_zone_device *thermal,
+			       enum thermal_device_mode *mode)
+{
+	struct ti_thermal_data *data = thermal->devdata;
+
+	if (data)
+		*mode = data->mode;
+
+	return 0;
+}
+
+/* Set mode callback functions for thermal zone */
+static int ti_thermal_set_mode(struct thermal_zone_device *thermal,
+			       enum thermal_device_mode mode)
+{
+	struct ti_thermal_data *data = thermal->devdata;
+
+	if (!data->ti_thermal) {
+		dev_notice(&thermal->device, "thermal zone not registered\n");
+		return 0;
+	}
+
+	mutex_lock(&data->ti_thermal->lock);
+
+	if (mode == THERMAL_DEVICE_ENABLED)
+		data->ti_thermal->polling_delay = FAST_TEMP_MONITORING_RATE;
+	else
+		data->ti_thermal->polling_delay = 0;
+
+	mutex_unlock(&data->ti_thermal->lock);
+
+	data->mode = mode;
+	thermal_zone_device_update(data->ti_thermal);
+	dev_dbg(&thermal->device, "thermal polling set for duration=%d msec\n",
+		data->ti_thermal->polling_delay);
+
+	return 0;
+}
+
+/* Get trip type callback functions for thermal zone */
+static int ti_thermal_get_trip_type(struct thermal_zone_device *thermal,
+				    int trip, enum thermal_trip_type *type)
+{
+	if (!ti_thermal_is_valid_trip(trip))
+		return -EINVAL;
+
+	if (trip + 1 == OMAP_TRIP_NUMBER)
+		*type = THERMAL_TRIP_CRITICAL;
+	else
+		*type = THERMAL_TRIP_PASSIVE;
+
+	return 0;
+}
+
+/* Get trip temperature callback functions for thermal zone */
+static int ti_thermal_get_trip_temp(struct thermal_zone_device *thermal,
+				    int trip, unsigned long *temp)
+{
+	if (!ti_thermal_is_valid_trip(trip))
+		return -EINVAL;
+
+	*temp = ti_thermal_get_trip_value(trip);
+
+	return 0;
+}
+
+/* Get the temperature trend callback functions for thermal zone */
+static int ti_thermal_get_trend(struct thermal_zone_device *thermal,
+				int trip, enum thermal_trend *trend)
+{
+	struct ti_thermal_data *data = thermal->devdata;
+	struct ti_bandgap *bgp;
+	int id, tr, ret = 0;
+
+	bgp = data->bgp;
+	id = data->sensor_id;
+
+	ret = ti_bandgap_get_trend(bgp, id, &tr);
+	if (ret)
+		return ret;
+
+	if (tr > 0)
+		*trend = THERMAL_TREND_RAISING;
+	else if (tr < 0)
+		*trend = THERMAL_TREND_DROPPING;
+	else
+		*trend = THERMAL_TREND_STABLE;
+
+	return 0;
+}
+
+/* Get critical temperature callback functions for thermal zone */
+static int ti_thermal_get_crit_temp(struct thermal_zone_device *thermal,
+				    unsigned long *temp)
+{
+	/* shutdown zone */
+	return ti_thermal_get_trip_temp(thermal, OMAP_TRIP_NUMBER - 1, temp);
+}
+
+static struct thermal_zone_device_ops ti_thermal_ops = {
+	.get_temp = ti_thermal_get_temp,
+	.get_trend = ti_thermal_get_trend,
+	.bind = ti_thermal_bind,
+	.unbind = ti_thermal_unbind,
+	.get_mode = ti_thermal_get_mode,
+	.set_mode = ti_thermal_set_mode,
+	.get_trip_type = ti_thermal_get_trip_type,
+	.get_trip_temp = ti_thermal_get_trip_temp,
+	.get_crit_temp = ti_thermal_get_crit_temp,
+};
+
+static struct ti_thermal_data
+*ti_thermal_build_data(struct ti_bandgap *bgp, int id)
+{
+	struct ti_thermal_data *data;
+
+	data = devm_kzalloc(bgp->dev, sizeof(*data), GFP_KERNEL);
+	if (!data) {
+		dev_err(bgp->dev, "kzalloc fail\n");
+		return NULL;
+	}
+	data->sensor_id = id;
+	data->bgp = bgp;
+	data->mode = THERMAL_DEVICE_ENABLED;
+	INIT_WORK(&data->thermal_wq, ti_thermal_work);
+
+	return data;
+}
+
+int ti_thermal_expose_sensor(struct ti_bandgap *bgp, int id,
+			     char *domain)
+{
+	struct ti_thermal_data *data;
+
+	data = ti_bandgap_get_sensor_data(bgp, id);
+
+	if (IS_ERR_OR_NULL(data))
+		data = ti_thermal_build_data(bgp, id);
+
+	if (!data)
+		return -EINVAL;
+
+	/* Create thermal zone */
+	data->ti_thermal = thermal_zone_device_register(domain,
+				OMAP_TRIP_NUMBER, 0, data, &ti_thermal_ops,
+				NULL, FAST_TEMP_MONITORING_RATE,
+				FAST_TEMP_MONITORING_RATE);
+	if (IS_ERR_OR_NULL(data->ti_thermal)) {
+		dev_err(bgp->dev, "thermal zone device is NULL\n");
+		return PTR_ERR(data->ti_thermal);
+	}
+	data->ti_thermal->polling_delay = FAST_TEMP_MONITORING_RATE;
+	ti_bandgap_set_sensor_data(bgp, id, data);
+
+	return 0;
+}
+
+int ti_thermal_remove_sensor(struct ti_bandgap *bgp, int id)
+{
+	struct ti_thermal_data *data;
+
+	data = ti_bandgap_get_sensor_data(bgp, id);
+
+	thermal_zone_device_unregister(data->ti_thermal);
+
+	return 0;
+}
+
+int ti_thermal_report_sensor_temperature(struct ti_bandgap *bgp, int id)
+{
+	struct ti_thermal_data *data;
+
+	data = ti_bandgap_get_sensor_data(bgp, id);
+
+	schedule_work(&data->thermal_wq);
+
+	return 0;
+}
+
+int ti_thermal_register_cpu_cooling(struct ti_bandgap *bgp, int id)
+{
+	struct ti_thermal_data *data;
+
+	data = ti_bandgap_get_sensor_data(bgp, id);
+	if (IS_ERR_OR_NULL(data))
+		data = ti_thermal_build_data(bgp, id);
+
+	if (!data)
+		return -EINVAL;
+
+	if (!cpufreq_get_current_driver()) {
+		dev_dbg(bgp->dev, "no cpufreq driver yet\n");
+		return -EPROBE_DEFER;
+	}
+
+	/* Register cooling device */
+	data->cool_dev = cpufreq_cooling_register(cpu_present_mask);
+	if (IS_ERR_OR_NULL(data->cool_dev)) {
+		dev_err(bgp->dev,
+			"Failed to register cpufreq cooling device\n");
+		return PTR_ERR(data->cool_dev);
+	}
+	ti_bandgap_set_sensor_data(bgp, id, data);
+
+	return 0;
+}
+
+int ti_thermal_unregister_cpu_cooling(struct ti_bandgap *bgp, int id)
+{
+	struct ti_thermal_data *data;
+
+	data = ti_bandgap_get_sensor_data(bgp, id);
+	cpufreq_cooling_unregister(data->cool_dev);
+
+	return 0;
+}
diff --git a/drivers/staging/omap-thermal/omap-thermal.h b/drivers/staging/ti-soc-thermal/ti-thermal.h
index 0dd2184b966..5055777727c 100644
--- a/drivers/staging/omap-thermal/omap-thermal.h
+++ b/drivers/staging/ti-soc-thermal/ti-thermal.h
@@ -20,32 +20,36 @@
  * 02110-1301 USA
  *
  */
-#ifndef __OMAP_THERMAL_H
-#define __OMAP_THERMAL_H
+#ifndef __TI_THERMAL_H
+#define __TI_THERMAL_H
 
-#include "omap-bandgap.h"
+#include "ti-bandgap.h"
 
 /* sensors gradient and offsets */
+#define OMAP_GRADIENT_SLOPE_4430				0
+#define OMAP_GRADIENT_CONST_4430				20000
 #define OMAP_GRADIENT_SLOPE_4460				348
 #define OMAP_GRADIENT_CONST_4460				-9301
 #define OMAP_GRADIENT_SLOPE_4470				308
 #define OMAP_GRADIENT_CONST_4470				-7896
 
-#define OMAP_GRADIENT_SLOPE_5430_CPU				196
-#define OMAP_GRADIENT_CONST_5430_CPU				-6822
-#define OMAP_GRADIENT_SLOPE_5430_GPU				64
-#define OMAP_GRADIENT_CONST_5430_GPU				978
+#define OMAP_GRADIENT_SLOPE_5430_CPU				65
+#define OMAP_GRADIENT_CONST_5430_CPU				-1791
+#define OMAP_GRADIENT_SLOPE_5430_GPU				117
+#define OMAP_GRADIENT_CONST_5430_GPU				-2992
 
 /* PCB sensor calculation constants */
+#define OMAP_GRADIENT_SLOPE_W_PCB_4430				0
+#define OMAP_GRADIENT_CONST_W_PCB_4430				20000
 #define OMAP_GRADIENT_SLOPE_W_PCB_4460				1142
 #define OMAP_GRADIENT_CONST_W_PCB_4460				-393
 #define OMAP_GRADIENT_SLOPE_W_PCB_4470				1063
 #define OMAP_GRADIENT_CONST_W_PCB_4470				-477
 
-#define OMAP_GRADIENT_SLOPE_W_PCB_5430_CPU			469
-#define OMAP_GRADIENT_CONST_W_PCB_5430_CPU			-1272
-#define OMAP_GRADIENT_SLOPE_W_PCB_5430_GPU			378
-#define OMAP_GRADIENT_CONST_W_PCB_5430_GPU			154
+#define OMAP_GRADIENT_SLOPE_W_PCB_5430_CPU			100
+#define OMAP_GRADIENT_CONST_W_PCB_5430_CPU			484
+#define OMAP_GRADIENT_SLOPE_W_PCB_5430_GPU			464
+#define OMAP_GRADIENT_CONST_W_PCB_5430_GPU			-5102
 
 /* trip points of interest in milicelsius (at hotspot level) */
 #define OMAP_TRIP_COLD						100000
@@ -60,47 +64,52 @@
 
 /* helper macros */
 /**
- * omap_thermal_get_trip_value - returns trip temperature based on index
+ * ti_thermal_get_trip_value - returns trip temperature based on index
  * @i:	trip index
  */
-#define omap_thermal_get_trip_value(i)					\
+#define ti_thermal_get_trip_value(i)					\
 	(OMAP_TRIP_HOT + ((i) * OMAP_TRIP_STEP))
 
 /**
- * omap_thermal_is_valid_trip - check for trip index
+ * ti_thermal_is_valid_trip - check for trip index
  * @i:	trip index
  */
-#define omap_thermal_is_valid_trip(trip)				\
+#define ti_thermal_is_valid_trip(trip)				\
 	((trip) >= 0 && (trip) < OMAP_TRIP_NUMBER)
 
-#ifdef CONFIG_OMAP_THERMAL
-int omap_thermal_expose_sensor(struct omap_bandgap *bg_ptr, int id,
-			       char *domain);
-int omap_thermal_remove_sensor(struct omap_bandgap *bg_ptr, int id);
-int omap_thermal_register_cpu_cooling(struct omap_bandgap *bg_ptr, int id);
-int omap_thermal_unregister_cpu_cooling(struct omap_bandgap *bg_ptr, int id);
+#ifdef CONFIG_TI_THERMAL
+int ti_thermal_expose_sensor(struct ti_bandgap *bgp, int id, char *domain);
+int ti_thermal_remove_sensor(struct ti_bandgap *bgp, int id);
+int ti_thermal_report_sensor_temperature(struct ti_bandgap *bgp, int id);
+int ti_thermal_register_cpu_cooling(struct ti_bandgap *bgp, int id);
+int ti_thermal_unregister_cpu_cooling(struct ti_bandgap *bgp, int id);
 #else
 static inline
-int omap_thermal_expose_sensor(struct omap_bandgap *bg_ptr, int id,
-			       char *domain)
+int ti_thermal_expose_sensor(struct ti_bandgap *bgp, int id, char *domain)
 {
 	return 0;
 }
 
 static inline
-int omap_thermal_remove_sensor(struct omap_bandgap *bg_ptr, int id)
+int ti_thermal_remove_sensor(struct ti_bandgap *bgp, int id)
 {
 	return 0;
 }
 
 static inline
-int omap_thermal_register_cpu_cooling(struct omap_bandgap *bg_ptr, int id)
+int ti_thermal_report_sensor_temperature(struct ti_bandgap *bgp, int id)
 {
 	return 0;
 }
 
 static inline
-int omap_thermal_unregister_cpu_cooling(struct omap_bandgap *bg_ptr, int id)
+int ti_thermal_register_cpu_cooling(struct ti_bandgap *bgp, int id)
+{
+	return 0;
+}
+
+static inline
+int ti_thermal_unregister_cpu_cooling(struct ti_bandgap *bgp, int id)
 {
 	return 0;
 }
diff --git a/drivers/staging/omap-thermal/omap_bandgap.txt b/drivers/staging/ti-soc-thermal/ti_soc_thermal.txt
index 6008a1452fd..a4a33d1a074 100644
--- a/drivers/staging/omap-thermal/omap_bandgap.txt
+++ b/drivers/staging/ti-soc-thermal/ti_soc_thermal.txt
@@ -10,21 +10,51 @@ to the silicon temperature.
 
 Required properties:
 - compatible : Should be:
-  - "ti,omap4460-control-bandgap" : for OMAP4460 bandgap
-  - "ti,omap5430-control-bandgap" : for OMAP5430 bandgap
+  - "ti,omap4430-bandgap" : for OMAP4430 bandgap
+  - "ti,omap4460-bandgap" : for OMAP4460 bandgap
+  - "ti,omap4470-bandgap" : for OMAP4470 bandgap
+  - "ti,omap5430-bandgap" : for OMAP5430 bandgap
 - interrupts : this entry should indicate which interrupt line
 the talert signal is routed to;
 Specific:
 - ti,tshut-gpio : this entry should be used to inform which GPIO
 line the tshut signal is routed to;
+- regs : this entry must also be specified and it is specific
+to each bandgap version, because the mapping may change from
+soc to soc, apart of depending on available features.
 
 Example:
+OMAP4430:
+bandgap {
+	reg = <0x4a002260 0x4 0x4a00232C 0x4>;
+	compatible = "ti,omap4430-bandgap";
+};
 
+OMAP4460:
 bandgap {
 	reg = <0x4a002260 0x4
 		0x4a00232C 0x4
 		0x4a002378 0x18>;
-	compatible = "ti,omap4460-control-bandgap";
+	compatible = "ti,omap4460-bandgap";
 	interrupts = <0 126 4>; /* talert */
 	ti,tshut-gpio = <86>;
 };
+
+OMAP4470:
+bandgap {
+	reg = <0x4a002260 0x4
+		0x4a00232C 0x4
+		0x4a002378 0x18>;
+	compatible = "ti,omap4470-bandgap";
+	interrupts = <0 126 4>; /* talert */
+	ti,tshut-gpio = <86>;
+};
+
+OMAP5430:
+bandgap {
+	reg = <0x4a0021e0 0xc
+		0x4a00232c 0xc
+		0x4a002380 0x2c
+		0x4a0023C0 0x3c>;
+	compatible = "ti,omap5430-bandgap";
+};
diff --git a/drivers/staging/tidspbridge/rmgr/proc.c b/drivers/staging/tidspbridge/rmgr/proc.c
index 0df55bd5bde..cd5235a4f77 100644
--- a/drivers/staging/tidspbridge/rmgr/proc.c
+++ b/drivers/staging/tidspbridge/rmgr/proc.c
@@ -488,7 +488,7 @@ func_end:
  *      Call the bridge_dev_ctrl fxn with the Argument. This is a Synchronous
  *      Operation. arg can be null.
  */
-int proc_ctrl(void *hprocessor, u32 dw_cmd, struct dsp_cbdata * arg)
+int proc_ctrl(void *hprocessor, u32 dw_cmd, struct dsp_cbdata *arg)
 {
 	int status = 0;
 	struct proc_object *p_proc_object = hprocessor;
@@ -982,7 +982,7 @@ int proc_get_state(void *hprocessor,
  *      This call is destructive, meaning the processor is placed in the monitor
  *      state as a result of this function.
  */
-int proc_get_trace(void *hprocessor, u8 * pbuf, u32 max_size)
+int proc_get_trace(void *hprocessor, u8 *pbuf, u32 max_size)
 {
 	int status;
 	status = -ENOSYS;
@@ -1338,7 +1338,7 @@ func_end:
  */
 int proc_register_notify(void *hprocessor, u32 event_mask,
 				u32 notify_type, struct dsp_notification
-				* hnotification)
+				*hnotification)
 {
 	int status = 0;
 	struct proc_object *p_proc_object = (struct proc_object *)hprocessor;
@@ -1549,8 +1549,8 @@ int proc_stop(void *hprocessor)
 		status = node_enum_nodes(hnode_mgr, &hnode, node_tab_size,
 					 &num_nodes, &nodes_allocated);
 		if ((status == -EINVAL) || (nodes_allocated > 0)) {
-			pr_err("%s: Can't stop device, active nodes = %d \n",
-			       __func__, nodes_allocated);
+			pr_err("%s: Can't stop device, active nodes = %d\n",
+				__func__, nodes_allocated);
 			return -EBADR;
 		}
 	}
@@ -1819,7 +1819,7 @@ func_end:
  *  Purpose:
  *      Retrieves the processor ID.
  */
-int proc_get_processor_id(void *proc, u32 * proc_id)
+int proc_get_processor_id(void *proc, u32 *proc_id)
 {
 	int status = 0;
 	struct proc_object *p_proc_object = (struct proc_object *)proc;
diff --git a/drivers/staging/tidspbridge/rmgr/strm.c b/drivers/staging/tidspbridge/rmgr/strm.c
index 34cc934e0c3..b88b27bbe2e 100644
--- a/drivers/staging/tidspbridge/rmgr/strm.c
+++ b/drivers/staging/tidspbridge/rmgr/strm.c
@@ -223,7 +223,7 @@ void strm_delete(struct strm_mgr *strm_mgr_obj)
  *  Purpose:
  *      Frees the buffers allocated for a stream.
  */
-int strm_free_buffer(struct strm_res_object *strmres, u8 ** ap_buffer,
+int strm_free_buffer(struct strm_res_object *strmres, u8 **ap_buffer,
 			    u32 num_bufs, struct process_context *pr_ctxt)
 {
 	int status = 0;
@@ -523,7 +523,7 @@ func_cont:
  *  Purpose:
  *      Relcaims a buffer from a stream.
  */
-int strm_reclaim(struct strm_object *stream_obj, u8 ** buf_ptr,
+int strm_reclaim(struct strm_object *stream_obj, u8 **buf_ptr,
 			u32 *nbytes, u32 *buff_size, u32 *pdw_arg)
 {
 	struct bridge_drv_interface *intf_fxns;
@@ -599,7 +599,7 @@ func_end:
  */
 int strm_register_notify(struct strm_object *stream_obj, u32 event_mask,
 				u32 notify_type, struct dsp_notification
-				* hnotification)
+				*hnotification)
 {
 	struct bridge_drv_interface *intf_fxns;
 	int status = 0;
diff --git a/drivers/staging/usbip/stub_dev.c b/drivers/staging/usbip/stub_dev.c
index 67556acd151..83d629afdfe 100644
--- a/drivers/staging/usbip/stub_dev.c
+++ b/drivers/staging/usbip/stub_dev.c
@@ -86,6 +86,7 @@ static ssize_t store_sockfd(struct device *dev, struct device_attribute *attr,
 	struct stub_device *sdev = dev_get_drvdata(dev);
 	int sockfd = 0;
 	struct socket *socket;
+	ssize_t err = -EINVAL;
 
 	if (!sdev) {
 		dev_err(dev, "sdev is null\n");
@@ -101,21 +102,21 @@ static ssize_t store_sockfd(struct device *dev, struct device_attribute *attr,
 
 		if (sdev->ud.status != SDEV_ST_AVAILABLE) {
 			dev_err(dev, "not ready\n");
-			spin_unlock_irq(&sdev->ud.lock);
-			return -EINVAL;
+			goto err;
 		}
 
 		socket = sockfd_to_socket(sockfd);
-		if (!socket) {
-			spin_unlock_irq(&sdev->ud.lock);
-			return -EINVAL;
-		}
+		if (!socket)
+			goto err;
+
 		sdev->ud.tcp_socket = socket;
 
 		spin_unlock_irq(&sdev->ud.lock);
 
-		sdev->ud.tcp_rx = kthread_get_run(stub_rx_loop, &sdev->ud, "stub_rx");
-		sdev->ud.tcp_tx = kthread_get_run(stub_tx_loop, &sdev->ud, "stub_tx");
+		sdev->ud.tcp_rx = kthread_get_run(stub_rx_loop, &sdev->ud,
+						  "stub_rx");
+		sdev->ud.tcp_tx = kthread_get_run(stub_tx_loop, &sdev->ud,
+						  "stub_tx");
 
 		spin_lock_irq(&sdev->ud.lock);
 		sdev->ud.status = SDEV_ST_USED;
@@ -125,16 +126,19 @@ static ssize_t store_sockfd(struct device *dev, struct device_attribute *attr,
 		dev_info(dev, "stub down\n");
 
 		spin_lock_irq(&sdev->ud.lock);
-		if (sdev->ud.status != SDEV_ST_USED) {
-			spin_unlock_irq(&sdev->ud.lock);
-			return -EINVAL;
-		}
+		if (sdev->ud.status != SDEV_ST_USED)
+			goto err;
+
 		spin_unlock_irq(&sdev->ud.lock);
 
 		usbip_event_add(&sdev->ud, SDEV_EVENT_DOWN);
 	}
 
 	return count;
+
+err:
+	spin_unlock_irq(&sdev->ud.lock);
+	return err;
 }
 static DEVICE_ATTR(usbip_sockfd, S_IWUSR, NULL, store_sockfd);
 
@@ -323,15 +327,9 @@ static struct stub_device *stub_device_alloc(struct usb_device *udev,
 	return sdev;
 }
 
-static int stub_device_free(struct stub_device *sdev)
+static void stub_device_free(struct stub_device *sdev)
 {
-	if (!sdev)
-		return -EINVAL;
-
 	kfree(sdev);
-	pr_debug("kfree udev ok\n");
-
-	return 0;
 }
 
 /*
@@ -450,7 +448,7 @@ static void shutdown_busid(struct bus_id_priv *busid_priv)
 		busid_priv->shutdown_busid = 1;
 		usbip_event_add(&busid_priv->sdev->ud, SDEV_EVENT_REMOVED);
 
-		/* 2. wait for the stop of the event handler */
+		/* wait for the stop of the event handler */
 		usbip_stop_eh(&busid_priv->sdev->ud);
 	}
 }
diff --git a/drivers/staging/usbip/stub_main.c b/drivers/staging/usbip/stub_main.c
index 705a9e530a1..33027cce670 100644
--- a/drivers/staging/usbip/stub_main.c
+++ b/drivers/staging/usbip/stub_main.c
@@ -38,11 +38,11 @@ static spinlock_t busid_table_lock;
 
 static void init_busid_table(void)
 {
-	int i;
-
+	/*
+	 * This also sets the bus_table[i].status to
+	 * STUB_BUSID_OTHER, which is 0.
+	 */
 	memset(busid_table, 0, sizeof(busid_table));
-	for (i = 0; i < MAX_BUSID; i++)
-		busid_table[i].status = STUB_BUSID_OTHER;
 
 	spin_lock_init(&busid_table_lock);
 }
@@ -167,22 +167,22 @@ static ssize_t store_match_busid(struct device_driver *dev, const char *buf,
 	strncpy(busid, buf + 4, BUSID_SIZE);
 
 	if (!strncmp(buf, "add ", 4)) {
-		if (add_match_busid(busid) < 0) {
+		if (add_match_busid(busid) < 0)
 			return -ENOMEM;
-		} else {
-			pr_debug("add busid %s\n", busid);
-			return count;
-		}
-	} else if (!strncmp(buf, "del ", 4)) {
-		if (del_match_busid(busid) < 0) {
+
+		pr_debug("add busid %s\n", busid);
+		return count;
+	}
+
+	if (!strncmp(buf, "del ", 4)) {
+		if (del_match_busid(busid) < 0)
 			return -ENODEV;
-		} else {
-			pr_debug("del busid %s\n", busid);
-			return count;
-		}
-	} else {
-		return -EINVAL;
+
+		pr_debug("del busid %s\n", busid);
+		return count;
 	}
+
+	return -EINVAL;
 }
 static DRIVER_ATTR(match_busid, S_IRUSR | S_IWUSR, show_match_busid,
 		   store_match_busid);
diff --git a/drivers/staging/usbip/stub_rx.c b/drivers/staging/usbip/stub_rx.c
index 715e8a79fb4..db48a789d30 100644
--- a/drivers/staging/usbip/stub_rx.c
+++ b/drivers/staging/usbip/stub_rx.c
@@ -228,61 +228,61 @@ static void tweak_special_requests(struct urb *urb)
 static int stub_recv_cmd_unlink(struct stub_device *sdev,
 				struct usbip_header *pdu)
 {
+	int ret;
 	unsigned long flags;
-
 	struct stub_priv *priv;
 
 	spin_lock_irqsave(&sdev->priv_lock, flags);
 
 	list_for_each_entry(priv, &sdev->priv_init, list) {
-		if (priv->seqnum == pdu->u.cmd_unlink.seqnum) {
-			int ret;
-
-			dev_info(&priv->urb->dev->dev, "unlink urb %p\n",
-				 priv->urb);
-
-			/*
-			 * This matched urb is not completed yet (i.e., be in
-			 * flight in usb hcd hardware/driver). Now we are
-			 * cancelling it. The unlinking flag means that we are
-			 * now not going to return the normal result pdu of a
-			 * submission request, but going to return a result pdu
-			 * of the unlink request.
-			 */
-			priv->unlinking = 1;
-
-			/*
-			 * In the case that unlinking flag is on, prev->seqnum
-			 * is changed from the seqnum of the cancelling urb to
-			 * the seqnum of the unlink request. This will be used
-			 * to make the result pdu of the unlink request.
-			 */
-			priv->seqnum = pdu->base.seqnum;
-
-			spin_unlock_irqrestore(&sdev->priv_lock, flags);
-
-			/*
-			 * usb_unlink_urb() is now out of spinlocking to avoid
-			 * spinlock recursion since stub_complete() is
-			 * sometimes called in this context but not in the
-			 * interrupt context.  If stub_complete() is executed
-			 * before we call usb_unlink_urb(), usb_unlink_urb()
-			 * will return an error value. In this case, stub_tx
-			 * will return the result pdu of this unlink request
-			 * though submission is completed and actual unlinking
-			 * is not executed. OK?
-			 */
-			/* In the above case, urb->status is not -ECONNRESET,
-			 * so a driver in a client host will know the failure
-			 * of the unlink request ?
-			 */
-			ret = usb_unlink_urb(priv->urb);
-			if (ret != -EINPROGRESS)
-				dev_err(&priv->urb->dev->dev,
-					"failed to unlink a urb %p, ret %d\n",
-					priv->urb, ret);
-			return 0;
-		}
+		if (priv->seqnum != pdu->u.cmd_unlink.seqnum)
+			continue;
+
+		dev_info(&priv->urb->dev->dev, "unlink urb %p\n",
+			 priv->urb);
+
+		/*
+		 * This matched urb is not completed yet (i.e., be in
+		 * flight in usb hcd hardware/driver). Now we are
+		 * cancelling it. The unlinking flag means that we are
+		 * now not going to return the normal result pdu of a
+		 * submission request, but going to return a result pdu
+		 * of the unlink request.
+		 */
+		priv->unlinking = 1;
+
+		/*
+		 * In the case that unlinking flag is on, prev->seqnum
+		 * is changed from the seqnum of the cancelling urb to
+		 * the seqnum of the unlink request. This will be used
+		 * to make the result pdu of the unlink request.
+		 */
+		priv->seqnum = pdu->base.seqnum;
+
+		spin_unlock_irqrestore(&sdev->priv_lock, flags);
+
+		/*
+		 * usb_unlink_urb() is now out of spinlocking to avoid
+		 * spinlock recursion since stub_complete() is
+		 * sometimes called in this context but not in the
+		 * interrupt context.  If stub_complete() is executed
+		 * before we call usb_unlink_urb(), usb_unlink_urb()
+		 * will return an error value. In this case, stub_tx
+		 * will return the result pdu of this unlink request
+		 * though submission is completed and actual unlinking
+		 * is not executed. OK?
+		 */
+		/* In the above case, urb->status is not -ECONNRESET,
+		 * so a driver in a client host will know the failure
+		 * of the unlink request ?
+		 */
+		ret = usb_unlink_urb(priv->urb);
+		if (ret != -EINPROGRESS)
+			dev_err(&priv->urb->dev->dev,
+				"failed to unlink a urb %p, ret %d\n",
+				priv->urb, ret);
+
+		return 0;
 	}
 
 	usbip_dbg_stub_rx("seqnum %d is not pending\n",
@@ -552,7 +552,7 @@ static void stub_rx_pdu(struct usbip_device *ud)
 
 	memset(&pdu, 0, sizeof(pdu));
 
-	/* 1. receive a pdu header */
+	/* receive a pdu header */
 	ret = usbip_recv(ud->tcp_socket, &pdu, sizeof(pdu));
 	if (ret != sizeof(pdu)) {
 		dev_err(dev, "recv a header, %d\n", ret);
diff --git a/drivers/staging/usbip/usbip_common.c b/drivers/staging/usbip/usbip_common.c
index 539fa5785af..7b97df6f2a4 100644
--- a/drivers/staging/usbip/usbip_common.c
+++ b/drivers/staging/usbip/usbip_common.c
@@ -389,7 +389,7 @@ int usbip_recv(struct socket *sock, void *buf, int size)
 		pr_debug("receiving....\n");
 		usbip_dump_buffer(bp, osize);
 		pr_debug("received, osize %d ret %d size %d total %d\n",
-			osize, result, size, total);
+			 osize, result, size, total);
 	}
 
 	return total;
diff --git a/drivers/staging/usbip/userspace/README b/drivers/staging/usbip/userspace/README
index 233d1d7aef9..00a1658b272 100644
--- a/drivers/staging/usbip/userspace/README
+++ b/drivers/staging/usbip/userspace/README
@@ -54,7 +54,7 @@
     client:# usbip list --remote <host>
 	- List exported USB devices on the <host>.
 
-    client:# usbip attach --host <host> --busid 1-2
+    client:# usbip attach --remote <host> --busid 1-2
 	- Connect the remote USB device.
 
     client:# usbip port
@@ -163,7 +163,7 @@ exportable on the host.
 
 Attach a remote USB device:
 
-    deux:# usbip attach --host 10.0.0.3 --busid 1-1
+    deux:# usbip attach --remote 10.0.0.3 --busid 1-1
     port 0 attached
 
 Show the devices attached to this client:
diff --git a/drivers/staging/usbip/userspace/doc/usbip.8 b/drivers/staging/usbip/userspace/doc/usbip.8
index 6e0d7450379..ccdadc87c47 100644
--- a/drivers/staging/usbip/userspace/doc/usbip.8
+++ b/drivers/staging/usbip/userspace/doc/usbip.8
@@ -38,7 +38,7 @@ then exit.
 .PP
 
 .HP
-\fBattach\fR \-\-host=<\fIhost\fR> \-\-busid=<\fIbus_id\fR>
+\fBattach\fR \-\-remote=<\fIhost\fR> \-\-busid=<\fIbus_id\fR>
 .IP
 Attach a remote USB device.
 .PP
@@ -79,7 +79,7 @@ List local USB devices.
     client:# usbip list --remote=server
         - List exportable usb devices on the server.
 
-    client:# usbip attach --host=server --busid=1-2
+    client:# usbip attach --remote=server --busid=1-2
         - Connect the remote USB device.
 
     client:# usbip detach --port=0
diff --git a/drivers/staging/usbip/userspace/libsrc/names.c b/drivers/staging/usbip/userspace/libsrc/names.c
index b4de18b4bb9..3c8d28b771e 100644
--- a/drivers/staging/usbip/userspace/libsrc/names.c
+++ b/drivers/staging/usbip/userspace/libsrc/names.c
@@ -1,4 +1,3 @@
-/*****************************************************************************/
 /*
  *      names.c  --  USB name database manipulation routines
  *
@@ -19,15 +18,14 @@
  *      Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  *
  *
+ *
+ *
+ *
+ *	Copyright (C) 2005 Takahiro Hirofuchi
+ *		- names_deinit() is added.
+ *
  */
 
-/*
- * 	Copyright (C) 2005 Takahiro Hirofuchi
- * 		- names_deinit() is added.
- */
-
-/*****************************************************************************/
-
 #include <sys/types.h>
 #include <sys/stat.h>
 #include <fcntl.h>
@@ -39,11 +37,8 @@
 #include <stdio.h>
 #include <ctype.h>
 
-
 #include "names.h"
-
-
-/* ---------------------------------------------------------------------- */
+#include "usbip_common.h"
 
 struct vendor {
 	struct vendor *next;
@@ -75,19 +70,12 @@ struct protocol {
 	char name[1];
 };
 
-struct audioterminal {
-	struct audioterminal *next;
-	u_int16_t termt;
-	char name[1];
-};
-
 struct genericstrtable {
-        struct genericstrtable *next;
-        unsigned int num;
-        char name[1];
+	struct genericstrtable *next;
+	unsigned int num;
+	char name[1];
 };
 
-/* ---------------------------------------------------------------------- */
 
 #define HASH1  0x10
 #define HASH2  0x02
@@ -103,74 +91,12 @@ static unsigned int hashnum(unsigned int num)
 	return num & (HASHSZ-1);
 }
 
-/* ---------------------------------------------------------------------- */
 
 static struct vendor *vendors[HASHSZ] = { NULL, };
 static struct product *products[HASHSZ] = { NULL, };
 static struct class *classes[HASHSZ] = { NULL, };
 static struct subclass *subclasses[HASHSZ] = { NULL, };
 static struct protocol *protocols[HASHSZ] = { NULL, };
-static struct audioterminal *audioterminals[HASHSZ] = { NULL, };
-static struct genericstrtable *hiddescriptors[HASHSZ] = { NULL, };
-static struct genericstrtable *reports[HASHSZ] = { NULL, };
-static struct genericstrtable *huts[HASHSZ] = { NULL, };
-static struct genericstrtable *biass[HASHSZ] = { NULL, };
-static struct genericstrtable *physdess[HASHSZ] = { NULL, };
-static struct genericstrtable *hutus[HASHSZ] = { NULL, };
-static struct genericstrtable *langids[HASHSZ] = { NULL, };
-static struct genericstrtable *countrycodes[HASHSZ] = { NULL, };
-
-/* ---------------------------------------------------------------------- */
-
-static const char *names_genericstrtable(struct genericstrtable *t[HASHSZ], unsigned int index)
-{
-        struct genericstrtable *h;
-
-        for (h = t[hashnum(index)]; h; h = h->next)
-                if (h->num == index)
-                        return h->name;
-        return NULL;
-}
-
-const char *names_hid(u_int8_t hidd)
-{
-	return names_genericstrtable(hiddescriptors, hidd);
-}
-
-const char *names_reporttag(u_int8_t rt)
-{
-	return names_genericstrtable(reports, rt);
-}
-
-const char *names_huts(unsigned int data)
-{
-	return names_genericstrtable(huts, data);
-}
-
-const char *names_hutus(unsigned int data)
-{
-	return names_genericstrtable(hutus, data);
-}
-
-const char *names_langid(u_int16_t langid)
-{
-	return names_genericstrtable(langids, langid);
-}
-
-const char *names_physdes(u_int8_t ph)
-{
-	return names_genericstrtable(physdess, ph);
-}
-
-const char *names_bias(u_int8_t b)
-{
-	return names_genericstrtable(biass, b);
-}
-
-const char *names_countrycode(unsigned int countrycode)
-{
-	return names_genericstrtable(countrycodes, countrycode);
-}
 
 const char *names_vendor(u_int16_t vendorid)
 {
@@ -216,37 +142,27 @@ const char *names_subclass(u_int8_t classid, u_int8_t subclassid)
 	return NULL;
 }
 
-const char *names_protocol(u_int8_t classid, u_int8_t subclassid, u_int8_t protocolid)
+const char *names_protocol(u_int8_t classid, u_int8_t subclassid,
+			   u_int8_t protocolid)
 {
 	struct protocol *p;
 
-	p = protocols[hashnum((classid << 16) | (subclassid << 8) | protocolid)];
+	p = protocols[hashnum((classid << 16) | (subclassid << 8)
+			      | protocolid)];
 	for (; p; p = p->next)
-		if (p->classid == classid && p->subclassid == subclassid && p->protocolid == protocolid)
+		if (p->classid == classid && p->subclassid == subclassid &&
+		    p->protocolid == protocolid)
 			return p->name;
 	return NULL;
 }
 
-const char *names_audioterminal(u_int16_t termt)
-{
-	struct audioterminal *at;
-
-	at = audioterminals[hashnum(termt)];
-	for (; at; at = at->next)
-		if (at->termt == termt)
-			return at->name;
-	return NULL;
-}
-
-/* ---------------------------------------------------------------------- */
 /* add a cleanup function by takahiro */
-
 struct pool {
 	struct pool *next;
 	void *mem;
 };
 
-static struct pool *pool_head = NULL;
+static struct pool *pool_head;
 
 static void *my_malloc(size_t size)
 {
@@ -287,8 +203,6 @@ void names_free(void)
 	}
 }
 
-/* ---------------------------------------------------------------------- */
-
 static int new_vendor(const char *name, u_int16_t vendorid)
 {
 	struct vendor *v;
@@ -308,7 +222,8 @@ static int new_vendor(const char *name, u_int16_t vendorid)
 	return 0;
 }
 
-static int new_product(const char *name, u_int16_t vendorid, u_int16_t productid)
+static int new_product(const char *name, u_int16_t vendorid,
+		       u_int16_t productid)
 {
 	struct product *p;
 	unsigned int h = hashnum((vendorid << 16) | productid);
@@ -367,14 +282,17 @@ static int new_subclass(const char *name, u_int8_t classid, u_int8_t subclassid)
 	return 0;
 }
 
-static int new_protocol(const char *name, u_int8_t classid, u_int8_t subclassid, u_int8_t protocolid)
+static int new_protocol(const char *name, u_int8_t classid, u_int8_t subclassid,
+			u_int8_t protocolid)
 {
 	struct protocol *p;
-	unsigned int h = hashnum((classid << 16) | (subclassid << 8) | protocolid);
+	unsigned int h = hashnum((classid << 16) | (subclassid << 8)
+				 | protocolid);
 
 	p = protocols[h];
 	for (; p; p = p->next)
-		if (p->classid == classid && p->subclassid == subclassid && p->protocolid == protocolid)
+		if (p->classid == classid && p->subclassid == subclassid
+		    && p->protocolid == protocolid)
 			return -1;
 	p = my_malloc(sizeof(struct protocol) + strlen(name));
 	if (!p)
@@ -388,253 +306,84 @@ static int new_protocol(const char *name, u_int8_t classid, u_int8_t subclassid,
 	return 0;
 }
 
-static int new_audioterminal(const char *name, u_int16_t termt)
-{
-	struct audioterminal *at;
-	unsigned int h = hashnum(termt);
-
-	at = audioterminals[h];
-	for (; at; at = at->next)
-		if (at->termt == termt)
-			return -1;
-	at = my_malloc(sizeof(struct audioterminal) + strlen(name));
-	if (!at)
-		return -1;
-	strcpy(at->name, name);
-	at->termt = termt;
-	at->next = audioterminals[h];
-	audioterminals[h] = at;
-	return 0;
-}
-
-static int new_genericstrtable(struct genericstrtable *t[HASHSZ], const char *name, unsigned int index)
-{
-        struct genericstrtable *g;
-	unsigned int h = hashnum(index);
-
-        for (g = t[h]; g; g = g->next)
-                if (g->num == index)
-                        return -1;
-        g = my_malloc(sizeof(struct genericstrtable) + strlen(name));
-        if (!g)
-                return -1;
-        strcpy(g->name, name);
-        g->num = index;
-        g->next = t[h];
-        t[h] = g;
-        return 0;
-}
-
-static int new_hid(const char *name, u_int8_t hidd)
-{
-	return new_genericstrtable(hiddescriptors, name, hidd);
-}
-
-static int new_reporttag(const char *name, u_int8_t rt)
-{
-	return new_genericstrtable(reports, name, rt);
-}
-
-static int new_huts(const char *name, unsigned int data)
-{
-	return new_genericstrtable(huts, name, data);
-}
-
-static int new_hutus(const char *name, unsigned int data)
-{
-	return new_genericstrtable(hutus, name, data);
-}
-
-static int new_langid(const char *name, u_int16_t langid)
-{
-	return new_genericstrtable(langids, name, langid);
-}
-
-static int new_physdes(const char *name, u_int8_t ph)
-{
-	return new_genericstrtable(physdess, name, ph);
-}
-static int new_bias(const char *name, u_int8_t b)
-{
-	return new_genericstrtable(biass, name, b);
-}
-
-static int new_countrycode(const char *name, unsigned int countrycode)
-{
-	return new_genericstrtable(countrycodes, name, countrycode);
-}
-
-/* ---------------------------------------------------------------------- */
-
-#define DBG(x)
-
 static void parse(FILE *f)
 {
 	char buf[512], *cp;
 	unsigned int linectr = 0;
-	int lastvendor = -1, lastclass = -1, lastsubclass = -1, lasthut=-1, lastlang=-1;
+	int lastvendor = -1;
+	int lastclass = -1;
+	int lastsubclass = -1;
+	int lasthut = -1;
+	int lastlang = -1;
 	unsigned int u;
 
 	while (fgets(buf, sizeof(buf), f)) {
 		linectr++;
 		/* remove line ends */
-		if ((cp = strchr(buf, 13)))
+		cp = strchr(buf, '\r');
+		if (cp)
 			*cp = 0;
-		if ((cp = strchr(buf, 10)))
+		cp = strchr(buf, '\n');
+		if (cp)
 			*cp = 0;
 		if (buf[0] == '#' || !buf[0])
 			continue;
 		cp = buf;
-                if (buf[0] == 'P' && buf[1] == 'H' && buf[2] == 'Y' && buf[3] == 'S' && buf[4] == 'D' &&
-                    buf[5] == 'E' && buf[6] == 'S' && /*isspace(buf[7])*/ buf[7] == ' ') {
-                        cp = buf + 8;
-                        while (isspace(*cp))
-                                cp++;
-                        if (!isxdigit(*cp)) {
-                                fprintf(stderr, "Invalid Physdes type at line %u\n", linectr);
-                                continue;
-                        }
-                        u = strtoul(cp, &cp, 16);
-                        while (isspace(*cp))
-                                cp++;
-                        if (!*cp) {
-                                fprintf(stderr, "Invalid Physdes type at line %u\n", linectr);
-                                continue;
-                        }
-                        if (new_physdes(cp, u))
-                                fprintf(stderr, "Duplicate Physdes  type spec at line %u terminal type %04x %s\n", linectr, u, cp);
-                        DBG(printf("line %5u physdes type %02x %s\n", linectr, u, cp));
-                        continue;
-
-                }
-                if (buf[0] == 'P' && buf[1] == 'H' && buf[2] == 'Y' && /*isspace(buf[3])*/ buf[3] == ' ') {
-                        cp = buf + 4;
-                        while (isspace(*cp))
-                                cp++;
-                        if (!isxdigit(*cp)) {
-                                fprintf(stderr, "Invalid PHY type at line %u\n", linectr);
-                                continue;
-                        }
-                        u = strtoul(cp, &cp, 16);
-                        while (isspace(*cp))
-                                cp++;
-                        if (!*cp) {
-                                fprintf(stderr, "Invalid PHY type at line %u\n", linectr);
-                                continue;
-                        }
-                        if (new_physdes(cp, u))
-                                fprintf(stderr, "Duplicate PHY type spec at line %u terminal type %04x %s\n", linectr, u, cp);
-                        DBG(printf("line %5u PHY type %02x %s\n", linectr, u, cp));
-                        continue;
-
-                }
-                if (buf[0] == 'B' && buf[1] == 'I' && buf[2] == 'A' && buf[3] == 'S' && /*isspace(buf[4])*/ buf[4] == ' ') {
-                        cp = buf + 5;
-                        while (isspace(*cp))
-                                cp++;
-                        if (!isxdigit(*cp)) {
-                                fprintf(stderr, "Invalid BIAS type at line %u\n", linectr);
-                                continue;
-                        }
-                        u = strtoul(cp, &cp, 16);
-                        while (isspace(*cp))
-                                cp++;
-                        if (!*cp) {
-                                fprintf(stderr, "Invalid BIAS type at line %u\n", linectr);
-                                continue;
-                        }
-                        if (new_bias(cp, u))
-                                fprintf(stderr, "Duplicate BIAS  type spec at line %u terminal type %04x %s\n", linectr, u, cp);
-                        DBG(printf("line %5u BIAS type %02x %s\n", linectr, u, cp));
-                        continue;
-
-                }
-                if (buf[0] == 'L' && /*isspace(buf[1])*/ buf[1] == ' ') {
-                        cp =  buf+2;
-                        while (isspace(*cp))
-                                cp++;
-                        if (!isxdigit(*cp)) {
-                                fprintf(stderr, "Invalid LANGID spec at line %u\n", linectr);
-                                continue;
-                        }
-                        u = strtoul(cp, &cp, 16);
-                        while (isspace(*cp))
-                                cp++;
-                        if (!*cp) {
-                                fprintf(stderr, "Invalid LANGID spec at line %u\n", linectr);
-                                continue;
-                        }
-                        if (new_langid(cp, u))
-                                fprintf(stderr, "Duplicate LANGID spec at line %u language-id %04x %s\n", linectr, u, cp);
-                        DBG(printf("line %5u LANGID %02x %s\n", linectr, u, cp));
-                        lasthut = lastclass = lastvendor = lastsubclass = -1;
-                        lastlang = u;
-                        continue;
-                }
+		if (buf[0] == 'P' && buf[1] == 'H' && buf[2] == 'Y' &&
+		    buf[3] == 'S' && buf[4] == 'D' &&
+		    buf[5] == 'E' && buf[6] == 'S' && /*isspace(buf[7])*/
+		    buf[7] == ' ') {
+			continue;
+		}
+		if (buf[0] == 'P' && buf[1] == 'H' &&
+		    buf[2] == 'Y' && /*isspace(buf[3])*/ buf[3] == ' ') {
+			continue;
+		}
+		if (buf[0] == 'B' && buf[1] == 'I' && buf[2] == 'A' &&
+		    buf[3] == 'S' && /*isspace(buf[4])*/ buf[4] == ' ') {
+			continue;
+		}
+		if (buf[0] == 'L' && /*isspace(buf[1])*/ buf[1] == ' ') {
+			lasthut = lastclass = lastvendor = lastsubclass = -1;
+			/*
+			 * set 1 as pseudo-id to indicate that the parser is
+			 * in a `L' section.
+			 */
+			lastlang = 1;
+			continue;
+		}
 		if (buf[0] == 'C' && /*isspace(buf[1])*/ buf[1] == ' ') {
 			/* class spec */
 			cp = buf+2;
 			while (isspace(*cp))
 				cp++;
 			if (!isxdigit(*cp)) {
-				fprintf(stderr, "Invalid class spec at line %u\n", linectr);
+				err("Invalid class spec at line %u", linectr);
 				continue;
 			}
 			u = strtoul(cp, &cp, 16);
 			while (isspace(*cp))
 				cp++;
 			if (!*cp) {
-				fprintf(stderr, "Invalid class spec at line %u\n", linectr);
+				err("Invalid class spec at line %u", linectr);
 				continue;
 			}
 			if (new_class(cp, u))
-				fprintf(stderr, "Duplicate class spec at line %u class %04x %s\n", linectr, u, cp);
-			DBG(printf("line %5u class %02x %s\n", linectr, u, cp));
+				err("Duplicate class spec at line %u class %04x %s",
+				    linectr, u, cp);
+			dbg("line %5u class %02x %s", linectr, u, cp);
 			lasthut = lastlang = lastvendor = lastsubclass = -1;
 			lastclass = u;
 			continue;
 		}
 		if (buf[0] == 'A' && buf[1] == 'T' && isspace(buf[2])) {
 			/* audio terminal type spec */
-			cp = buf+3;
-			while (isspace(*cp))
-				cp++;
-			if (!isxdigit(*cp)) {
-				fprintf(stderr, "Invalid audio terminal type at line %u\n", linectr);
-				continue;
-			}
-			u = strtoul(cp, &cp, 16);
-			while (isspace(*cp))
-				cp++;
-			if (!*cp) {
-				fprintf(stderr, "Invalid audio terminal type at line %u\n", linectr);
-				continue;
-			}
-			if (new_audioterminal(cp, u))
-				fprintf(stderr, "Duplicate audio terminal type spec at line %u terminal type %04x %s\n", linectr, u, cp);
-			DBG(printf("line %5u audio terminal type %02x %s\n", linectr, u, cp));
 			continue;
 		}
-		if (buf[0] == 'H' && buf[1] == 'C' && buf[2] == 'C' && isspace(buf[3])) {
+		if (buf[0] == 'H' && buf[1] == 'C' && buf[2] == 'C'
+		    && isspace(buf[3])) {
 			/* HID Descriptor bCountryCode */
-                        cp =  buf+3;
-                        while (isspace(*cp))
-                                cp++;
-                        if (!isxdigit(*cp)) {
-                                fprintf(stderr, "Invalid HID country code at line %u\n", linectr);
-                                continue;
-                        }
-                        u = strtoul(cp, &cp, 10);
-                        while (isspace(*cp))
-                                cp++;
-                        if (!*cp) {
-                                fprintf(stderr, "Invalid HID country code at line %u\n", linectr);
-                                continue;
-                        }
-                        if (new_countrycode(cp, u))
-                                fprintf(stderr, "Duplicate HID country code at line %u country %02u %s\n", linectr, u, cp);
-                        DBG(printf("line %5u keyboard country code %02u %s\n", linectr, u, cp));
-                        continue;
+			continue;
 		}
 		if (isxdigit(*cp)) {
 			/* vendor */
@@ -642,12 +391,13 @@ static void parse(FILE *f)
 			while (isspace(*cp))
 				cp++;
 			if (!*cp) {
-				fprintf(stderr, "Invalid vendor spec at line %u\n", linectr);
+				err("Invalid vendor spec at line %u", linectr);
 				continue;
 			}
 			if (new_vendor(cp, u))
-				fprintf(stderr, "Duplicate vendor spec at line %u vendor %04x %s\n", linectr, u, cp);
-			DBG(printf("line %5u vendor %04x %s\n", linectr, u, cp));
+				err("Duplicate vendor spec at line %u vendor %04x %s",
+				    linectr, u, cp);
+			dbg("line %5u vendor %04x %s", linectr, u, cp);
 			lastvendor = u;
 			lasthut = lastlang = lastclass = lastsubclass = -1;
 			continue;
@@ -658,33 +408,37 @@ static void parse(FILE *f)
 			while (isspace(*cp))
 				cp++;
 			if (!*cp) {
-				fprintf(stderr, "Invalid product/subclass spec at line %u\n", linectr);
+				err("Invalid product/subclass spec at line %u",
+				    linectr);
 				continue;
 			}
 			if (lastvendor != -1) {
 				if (new_product(cp, lastvendor, u))
-					fprintf(stderr, "Duplicate product spec at line %u product %04x:%04x %s\n", linectr, lastvendor, u, cp);
-				DBG(printf("line %5u product %04x:%04x %s\n", linectr, lastvendor, u, cp));
+					err("Duplicate product spec at line %u product %04x:%04x %s",
+					    linectr, lastvendor, u, cp);
+				dbg("line %5u product %04x:%04x %s", linectr,
+				    lastvendor, u, cp);
 				continue;
 			}
 			if (lastclass != -1) {
 				if (new_subclass(cp, lastclass, u))
-					fprintf(stderr, "Duplicate subclass spec at line %u class %02x:%02x %s\n", linectr, lastclass, u, cp);
-				DBG(printf("line %5u subclass %02x:%02x %s\n", linectr, lastclass, u, cp));
+					err("Duplicate subclass spec at line %u class %02x:%02x %s",
+					    linectr, lastclass, u, cp);
+				dbg("line %5u subclass %02x:%02x %s", linectr,
+				    lastclass, u, cp);
 				lastsubclass = u;
 				continue;
 			}
 			if (lasthut != -1) {
-				if (new_hutus(cp, (lasthut << 16)+u))
-					fprintf(stderr, "Duplicate HUT Usage Spec at line %u\n", linectr);
+				/* do not store hut */
 				continue;
 			}
 			if (lastlang != -1) {
-                                if (new_langid(cp, lastlang+(u<<10)))
-                                        fprintf(stderr, "Duplicate LANGID Usage Spec at line %u\n", linectr);
-                                continue;
-                        }
-			fprintf(stderr, "Product/Subclass spec without prior Vendor/Class spec at line %u\n", linectr);
+				/* do not store langid */
+				continue;
+			}
+			err("Product/Subclass spec without prior Vendor/Class spec at line %u",
+			    linectr);
 			continue;
 		}
 		if (buf[0] == '\t' && buf[1] == '\t' && isxdigit(buf[2])) {
@@ -693,100 +447,57 @@ static void parse(FILE *f)
 			while (isspace(*cp))
 				cp++;
 			if (!*cp) {
-				fprintf(stderr, "Invalid protocol spec at line %u\n", linectr);
+				err("Invalid protocol spec at line %u",
+				    linectr);
 				continue;
 			}
 			if (lastclass != -1 && lastsubclass != -1) {
-				if (new_protocol(cp, lastclass, lastsubclass, u))
-					fprintf(stderr, "Duplicate protocol spec at line %u class %02x:%02x:%02x %s\n", linectr, lastclass, lastsubclass, u, cp);
-				DBG(printf("line %5u protocol %02x:%02x:%02x %s\n", linectr, lastclass, lastsubclass, u, cp));
+				if (new_protocol(cp, lastclass, lastsubclass,
+						 u))
+					err("Duplicate protocol spec at line %u class %02x:%02x:%02x %s",
+					    linectr, lastclass, lastsubclass,
+					    u, cp);
+				dbg("line %5u protocol %02x:%02x:%02x %s",
+				    linectr, lastclass, lastsubclass, u, cp);
 				continue;
 			}
-			fprintf(stderr, "Protocol spec without prior Class and Subclass spec at line %u\n", linectr);
+			err("Protocol spec without prior Class and Subclass spec at line %u",
+			    linectr);
 			continue;
 		}
-		if (buf[0] == 'H' && buf[1] == 'I' && buf[2] == 'D' && /*isspace(buf[3])*/ buf[3] == ' ') {
-			cp = buf + 4;
-                        while (isspace(*cp))
-                                cp++;
-                        if (!isxdigit(*cp)) {
-                                fprintf(stderr, "Invalid HID type at line %u\n", linectr);
-                                continue;
-                        }
-                        u = strtoul(cp, &cp, 16);
-                        while (isspace(*cp))
-                                cp++;
-                        if (!*cp) {
-                                fprintf(stderr, "Invalid HID type at line %u\n", linectr);
-                                continue;
-                        }
-                        if (new_hid(cp, u))
-                                fprintf(stderr, "Duplicate HID type spec at line %u terminal type %04x %s\n", linectr, u, cp);
-                        DBG(printf("line %5u HID type %02x %s\n", linectr, u, cp));
-                        continue;
-
+		if (buf[0] == 'H' && buf[1] == 'I' &&
+		    buf[2] == 'D' && /*isspace(buf[3])*/ buf[3] == ' ') {
+			continue;
 		}
-                if (buf[0] == 'H' && buf[1] == 'U' && buf[2] == 'T' && /*isspace(buf[3])*/ buf[3] == ' ') {
-                        cp = buf + 4;
-                        while (isspace(*cp))
-                                cp++;
-                        if (!isxdigit(*cp)) {
-                                fprintf(stderr, "Invalid HUT type at line %u\n", linectr);
-                                continue;
-                        }
-                        u = strtoul(cp, &cp, 16);
-                        while (isspace(*cp))
-                                cp++;
-                        if (!*cp) {
-                                fprintf(stderr, "Invalid HUT type at line %u\n", linectr);
-                                continue;
-                        }
-                        if (new_huts(cp, u))
-                                fprintf(stderr, "Duplicate HUT type spec at line %u terminal type %04x %s\n", linectr, u, cp);
+		if (buf[0] == 'H' && buf[1] == 'U' &&
+		    buf[2] == 'T' && /*isspace(buf[3])*/ buf[3] == ' ') {
 			lastlang = lastclass = lastvendor = lastsubclass = -1;
-			lasthut = u;
-                        DBG(printf("line %5u HUT type %02x %s\n", linectr, u, cp));
-                        continue;
-
-                }
-                if (buf[0] == 'R' && buf[1] == ' ') {
-                        cp = buf + 2;
-                        while (isspace(*cp))
-                                cp++;
-                        if (!isxdigit(*cp)) {
-                                fprintf(stderr, "Invalid Report type at line %u\n", linectr);
-                                continue;
-                        }
-                        u = strtoul(cp, &cp, 16);
-                        while (isspace(*cp))
-                                cp++;
-                        if (!*cp) {
-                                fprintf(stderr, "Invalid Report type at line %u\n", linectr);
-                                continue;
-                        }
-                        if (new_reporttag(cp, u))
-                                fprintf(stderr, "Duplicate Report type spec at line %u terminal type %04x %s\n", linectr, u, cp);
-                        DBG(printf("line %5u Report type %02x %s\n", linectr, u, cp));
-                        continue;
-
-                }
-                if (buf[0] == 'V' && buf[1] == 'T') {
-			/* add here */
+			/*
+			 * set 1 as pseudo-id to indicate that the parser is
+			 * in a `HUT' section.
+			 */
+			lasthut = 1;
 			continue;
 		}
-		fprintf(stderr, "Unknown line at line %u\n", linectr);
+		if (buf[0] == 'R' && buf[1] == ' ')
+			continue;
+
+		if (buf[0] == 'V' && buf[1] == 'T')
+			continue;
+
+		err("Unknown line at line %u", linectr);
 	}
 }
 
-/* ---------------------------------------------------------------------- */
 
 int names_init(char *n)
 {
 	FILE *f;
 
-	if (!(f = fopen(n, "r"))) {
+	f = fopen(n, "r");
+	if (!f)
 		return errno;
-	}
+
 	parse(f);
 	fclose(f);
 	return 0;
diff --git a/drivers/staging/usbip/userspace/libsrc/names.h b/drivers/staging/usbip/userspace/libsrc/names.h
index 3a269fecc02..680926512de 100644
--- a/drivers/staging/usbip/userspace/libsrc/names.h
+++ b/drivers/staging/usbip/userspace/libsrc/names.h
@@ -1,5 +1,3 @@
-/*****************************************************************************/
-
 /*
  *      names.h  --  USB name database manipulation routines
  *
@@ -20,38 +18,24 @@
  *      Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  *
  *
- */
-
-/*
+ *
  *	Copyright (C) 2005 Takahiro Hirofuchi
  *	       - names_free() is added.
  */
 
-/*****************************************************************************/
-
 #ifndef _NAMES_H
 #define _NAMES_H
 
 #include <sys/types.h>
 
-/* ---------------------------------------------------------------------- */
-
+/* used by usbip_common.c */
 extern const char *names_vendor(u_int16_t vendorid);
 extern const char *names_product(u_int16_t vendorid, u_int16_t productid);
 extern const char *names_class(u_int8_t classid);
 extern const char *names_subclass(u_int8_t classid, u_int8_t subclassid);
-extern const char *names_protocol(u_int8_t classid, u_int8_t subclassid, u_int8_t protocolid);
-extern const char *names_audioterminal(u_int16_t termt);
-extern const char *names_hid(u_int8_t hidd);
-extern const char *names_reporttag(u_int8_t rt);
-extern const char *names_huts(unsigned int data);
-extern const char *names_hutus(unsigned int data);
-extern const char *names_langid(u_int16_t langid);
-extern const char *names_physdes(u_int8_t ph);
-extern const char *names_bias(u_int8_t b);
-extern const char *names_countrycode(unsigned int countrycode);
+extern const char *names_protocol(u_int8_t classid, u_int8_t subclassid,
+				  u_int8_t protocolid);
 extern int  names_init(char *n);
 extern void names_free(void);
 
-/* ---------------------------------------------------------------------- */
 #endif /* _NAMES_H */
diff --git a/drivers/staging/usbip/userspace/libsrc/usbip_common.c b/drivers/staging/usbip/userspace/libsrc/usbip_common.c
index 154b4b1103e..17e08e022c0 100644
--- a/drivers/staging/usbip/userspace/libsrc/usbip_common.c
+++ b/drivers/staging/usbip/userspace/libsrc/usbip_common.c
@@ -8,9 +8,9 @@
 #undef  PROGNAME
 #define PROGNAME "libusbip"
 
-int usbip_use_syslog = 0;
-int usbip_use_stderr = 0;
-int usbip_use_debug  = 0;
+int usbip_use_syslog;
+int usbip_use_stderr;
+int usbip_use_debug;
 
 struct speed_string {
 	int num;
@@ -44,7 +44,7 @@ static struct portst_string portst_strings[] = {
 
 const char *usbip_status_string(int32_t status)
 {
-	for (int i=0; portst_strings[i].desc != NULL; i++)
+	for (int i = 0; portst_strings[i].desc != NULL; i++)
 		if (portst_strings[i].num == status)
 			return portst_strings[i].desc;
 
@@ -53,7 +53,7 @@ const char *usbip_status_string(int32_t status)
 
 const char *usbip_speed_string(int num)
 {
-	for (int i=0; speed_strings[i].speed != NULL; i++)
+	for (int i = 0; speed_strings[i].speed != NULL; i++)
 		if (speed_strings[i].num == num)
 			return speed_strings[i].desc;
 
@@ -109,7 +109,8 @@ void dump_usb_device(struct usbip_usb_device *udev)
 }
 
 
-int read_attr_value(struct sysfs_device *dev, const char *name, const char *format)
+int read_attr_value(struct sysfs_device *dev, const char *name,
+		    const char *format)
 {
 	char attrpath[SYSFS_PATH_MAX];
 	struct sysfs_attribute *attr;
@@ -172,7 +173,7 @@ int read_attr_speed(struct sysfs_device *dev)
 err:
 	sysfs_close_attribute(attr);
 
-	for (int i=0; speed_strings[i].speed != NULL; i++) {
+	for (int i = 0; speed_strings[i].speed != NULL; i++) {
 		if (!strcmp(speed, speed_strings[i].speed))
 			return speed_strings[i].num;
 	}
@@ -180,8 +181,11 @@ err:
 	return USB_SPEED_UNKNOWN;
 }
 
-#define READ_ATTR(object, type, dev, name, format)\
-	do { (object)->name = (type) read_attr_value(dev, to_string(name), format); } while (0)
+#define READ_ATTR(object, type, dev, name, format)			      \
+	do {								      \
+		(object)->name = (type) read_attr_value(dev, to_string(name), \
+							format);	      \
+	} while (0)
 
 
 int read_usb_device(struct sysfs_device *sdev, struct usbip_usb_device *udev)
@@ -245,7 +249,8 @@ void usbip_names_free()
 	names_free();
 }
 
-void usbip_names_get_product(char *buff, size_t size, uint16_t vendor, uint16_t product)
+void usbip_names_get_product(char *buff, size_t size, uint16_t vendor,
+			     uint16_t product)
 {
 	const char *prod, *vend;
 
@@ -261,7 +266,8 @@ void usbip_names_get_product(char *buff, size_t size, uint16_t vendor, uint16_t
 	snprintf(buff, size, "%s : %s (%04x:%04x)", vend, prod, vendor, product);
 }
 
-void usbip_names_get_class(char *buff, size_t size, uint8_t class, uint8_t subclass, uint8_t protocol)
+void usbip_names_get_class(char *buff, size_t size, uint8_t class,
+			   uint8_t subclass, uint8_t protocol)
 {
 	const char *c, *s, *p;
 
diff --git a/drivers/staging/usbip/userspace/libsrc/usbip_common.h b/drivers/staging/usbip/userspace/libsrc/usbip_common.h
index eedefbd12ea..938ad1c3694 100644
--- a/drivers/staging/usbip/userspace/libsrc/usbip_common.h
+++ b/drivers/staging/usbip/userspace/libsrc/usbip_common.h
@@ -84,7 +84,7 @@ enum usb_device_speed {
 };
 
 /* FIXME: how to sync with drivers/usbip_common.h ? */
-enum usbip_device_status{
+enum usbip_device_status {
 	/* sdev is available. */
 	SDEV_ST_AVAILABLE = 0x01,
 	/* sdev is now used. */
@@ -132,7 +132,8 @@ struct usbip_usb_device {
 void dump_usb_interface(struct usbip_usb_interface *);
 void dump_usb_device(struct usbip_usb_device *);
 int read_usb_device(struct sysfs_device *sdev, struct usbip_usb_device *udev);
-int read_attr_value(struct sysfs_device *dev, const char *name, const char *format);
+int read_attr_value(struct sysfs_device *dev, const char *name,
+		    const char *format);
 int read_usb_interface(struct usbip_usb_device *udev, int i,
 		       struct usbip_usb_interface *uinf);
 
@@ -141,7 +142,9 @@ const char *usbip_status_string(int32_t status);
 
 int usbip_names_init(char *);
 void usbip_names_free(void);
-void usbip_names_get_product(char *buff, size_t size, uint16_t vendor, uint16_t product);
-void usbip_names_get_class(char *buff, size_t size, uint8_t class, uint8_t subclass, uint8_t protocol);
+void usbip_names_get_product(char *buff, size_t size, uint16_t vendor,
+			     uint16_t product);
+void usbip_names_get_class(char *buff, size_t size, uint8_t class,
+			   uint8_t subclass, uint8_t protocol);
 
 #endif /* __USBIP_COMMON_H */
diff --git a/drivers/staging/usbip/userspace/libsrc/vhci_driver.c b/drivers/staging/usbip/userspace/libsrc/vhci_driver.c
index 0958ba53e94..25e62e9f0a3 100644
--- a/drivers/staging/usbip/userspace/libsrc/vhci_driver.c
+++ b/drivers/staging/usbip/userspace/libsrc/vhci_driver.c
@@ -10,7 +10,8 @@
 
 struct usbip_vhci_driver *vhci_driver;
 
-static struct usbip_imported_device *imported_device_init(struct usbip_imported_device *idev, char *busid)
+static struct usbip_imported_device *
+imported_device_init(struct usbip_imported_device *idev, char *busid)
 {
 	struct sysfs_device *sudev;
 
@@ -29,14 +30,16 @@ static struct usbip_imported_device *imported_device_init(struct usbip_imported_
 		if (!strncmp(cdev->dev_path, idev->udev.path,
 			     strlen(idev->udev.path))) {
 			struct usbip_class_device *new_cdev;
-
-			/* alloc and copy because dlist is linked from only one list */
+			/*
+			 * alloc and copy because dlist is linked
+			 * from only one list
+			 */
 			new_cdev = calloc(1, sizeof(*new_cdev));
 			if (!new_cdev)
 				goto err;
 
 			memcpy(new_cdev, cdev, sizeof(*new_cdev));
-			dlist_unshift(idev->cdev_list, (void*) new_cdev);
+			dlist_unshift(idev->cdev_list, (void *) new_cdev);
 		}
 	}
 
@@ -101,7 +104,8 @@ static int parse_status(char *value)
 				return -1;
 			}
 
-			if (idev->status != VDEV_ST_NULL && idev->status != VDEV_ST_NOTASSIGNED) {
+			if (idev->status != VDEV_ST_NULL
+			    && idev->status != VDEV_ST_NOTASSIGNED) {
 				idev = imported_device_init(idev, lbusid);
 				if (!idev) {
 					dbg("imported_device_init failed");
@@ -126,8 +130,10 @@ static int parse_status(char *value)
 
 static int check_usbip_device(struct sysfs_class_device *cdev)
 {
-	char class_path[SYSFS_PATH_MAX]; /* /sys/class/video4linux/video0/device */
-	char dev_path[SYSFS_PATH_MAX];	 /* /sys/devices/platform/vhci_hcd/usb6/6-1:1.1 */
+	/* /sys/class/video4linux/video0/device */
+	char class_path[SYSFS_PATH_MAX];
+	/* /sys/devices/platform/vhci_hcd/usb6/6-1:1.1 */
+	char dev_path[SYSFS_PATH_MAX];
 	int ret;
 	struct usbip_class_device *usbip_cdev;
 
@@ -289,25 +295,25 @@ static int get_nports(void)
 
 static int get_hc_busid(char *sysfs_mntpath, char *hc_busid)
 {
-        struct sysfs_driver *sdriver;
-        char sdriver_path[SYSFS_PATH_MAX];
+	struct sysfs_driver *sdriver;
+	char sdriver_path[SYSFS_PATH_MAX];
 
 	struct sysfs_device *hc_dev;
 	struct dlist *hc_devs;
 
 	int found = 0;
 
-        snprintf(sdriver_path, SYSFS_PATH_MAX, "%s/%s/%s/%s/%s", sysfs_mntpath,
-		 SYSFS_BUS_NAME, USBIP_VHCI_BUS_TYPE, SYSFS_DRIVERS_NAME,
-		 USBIP_VHCI_DRV_NAME);
+	snprintf(sdriver_path, SYSFS_PATH_MAX, "%s/%s/%s/%s/%s", sysfs_mntpath,
+	SYSFS_BUS_NAME, USBIP_VHCI_BUS_TYPE, SYSFS_DRIVERS_NAME,
+	USBIP_VHCI_DRV_NAME);
 
-        sdriver = sysfs_open_driver_path(sdriver_path);
-        if (!sdriver) {
+	sdriver = sysfs_open_driver_path(sdriver_path);
+	if (!sdriver) {
 		dbg("sysfs_open_driver_path failed: %s", sdriver_path);
-                dbg("make sure " USBIP_CORE_MOD_NAME ".ko and "
+		dbg("make sure " USBIP_CORE_MOD_NAME ".ko and "
 		    USBIP_VHCI_DRV_NAME ".ko are loaded!");
-                return -1;
-        }
+		return -1;
+	}
 
 	hc_devs = sysfs_get_driver_devices(sdriver);
 	if (!hc_devs) {
diff --git a/drivers/staging/usbip/userspace/src/usbip_attach.c b/drivers/staging/usbip/userspace/src/usbip_attach.c
index 2da4e44e163..0ec16e54fb0 100644
--- a/drivers/staging/usbip/userspace/src/usbip_attach.c
+++ b/drivers/staging/usbip/userspace/src/usbip_attach.c
@@ -36,7 +36,7 @@
 
 static const char usbip_attach_usage_string[] =
 	"usbip attach <args>\n"
-	"    -h, --host=<host>      The machine with exported USB devices\n"
+	"    -r, --remote=<host>      The machine with exported USB devices\n"
 	"    -b, --busid=<busid>    Busid of the device on <host>\n";
 
 void usbip_attach_usage(void)
@@ -201,9 +201,9 @@ static int attach_device(char *host, char *busid)
 int usbip_attach(int argc, char *argv[])
 {
 	static const struct option opts[] = {
-		{ "host", required_argument, NULL, 'h' },
-		{ "busid", required_argument, NULL, 'b' },
-		{ NULL, 0, NULL, 0 }
+		{ "remote", required_argument, NULL, 'r' },
+		{ "busid",  required_argument, NULL, 'b' },
+		{ NULL, 0,  NULL, 0 }
 	};
 	char *host = NULL;
 	char *busid = NULL;
@@ -211,13 +211,13 @@ int usbip_attach(int argc, char *argv[])
 	int ret = -1;
 
 	for (;;) {
-		opt = getopt_long(argc, argv, "h:b:", opts, NULL);
+		opt = getopt_long(argc, argv, "r:b:", opts, NULL);
 
 		if (opt == -1)
 			break;
 
 		switch (opt) {
-		case 'h':
+		case 'r':
 			host = optarg;
 			break;
 		case 'b':
diff --git a/drivers/staging/usbip/userspace/src/usbip_detach.c b/drivers/staging/usbip/userspace/src/usbip_detach.c
index dac5f065755..13308df613a 100644
--- a/drivers/staging/usbip/userspace/src/usbip_detach.c
+++ b/drivers/staging/usbip/userspace/src/usbip_detach.c
@@ -49,7 +49,7 @@ static int detach_port(char *port)
 	uint8_t portnum;
 	char path[PATH_MAX+1];
 
-	for (unsigned int i=0; i < strlen(port); i++)
+	for (unsigned int i = 0; i < strlen(port); i++)
 		if (!isdigit(port[i])) {
 			err("invalid port %s", port);
 			return -1;
diff --git a/drivers/staging/usbip/userspace/src/usbip_list.c b/drivers/staging/usbip/userspace/src/usbip_list.c
index ed30d910e03..ff56255f497 100644
--- a/drivers/staging/usbip/userspace/src/usbip_list.c
+++ b/drivers/staging/usbip/userspace/src/usbip_list.c
@@ -159,6 +159,12 @@ static void print_device(char *busid, char *vendor, char *product,
 		printf(" - busid %s (%.4s:%.4s)\n", busid, vendor, product);
 }
 
+static void print_product_name(char *product_name, bool parsable)
+{
+	if (!parsable)
+		printf("   %s\n", product_name);
+}
+
 static void print_interface(char *busid, char *driver, bool parsable)
 {
 	if (parsable)
@@ -189,6 +195,7 @@ static int list_devices(bool parsable)
 {
 	char bus_type[] = "usb";
 	char busid[SYSFS_BUS_ID_SIZE];
+	char product_name[128];
 	struct sysfs_bus *ubus;
 	struct sysfs_device *dev;
 	struct sysfs_device *intf;
@@ -231,8 +238,13 @@ static int list_devices(bool parsable)
 			goto err_out;
 		}
 
+		/* get product name */
+		usbip_names_get_product(product_name, sizeof(product_name),
+					strtol(idVendor->value, NULL, 16),
+					strtol(idProduct->value, NULL, 16));
 		print_device(dev->bus_id, idVendor->value, idProduct->value,
 			     parsable);
+		print_product_name(product_name, parsable);
 
 		for (i = 0; i < atoi(bNumIntfs->value); i++) {
 			snprintf(busid, sizeof(busid), "%s:%.1s.%d",
diff --git a/drivers/staging/usbip/userspace/src/usbip_network.c b/drivers/staging/usbip/userspace/src/usbip_network.c
index 1a84dd37e12..b12448ec69a 100644
--- a/drivers/staging/usbip/userspace/src/usbip_network.c
+++ b/drivers/staging/usbip/userspace/src/usbip_network.c
@@ -56,7 +56,7 @@ void usbip_net_pack_usb_device(int pack, struct usbip_usb_device *udev)
 {
 	usbip_net_pack_uint32_t(pack, &udev->busnum);
 	usbip_net_pack_uint32_t(pack, &udev->devnum);
-	usbip_net_pack_uint32_t(pack, &udev->speed );
+	usbip_net_pack_uint32_t(pack, &udev->speed);
 
 	usbip_net_pack_uint16_t(pack, &udev->idVendor);
 	usbip_net_pack_uint16_t(pack, &udev->idProduct);
@@ -248,10 +248,10 @@ int usbip_net_tcp_connect(char *hostname, char *service)
 		close(sockfd);
 	}
 
+	freeaddrinfo(res);
+
 	if (!rp)
 		return EAI_SYSTEM;
 
-	freeaddrinfo(res);
-
 	return sockfd;
 }
diff --git a/drivers/staging/usbip/userspace/src/usbip_network.h b/drivers/staging/usbip/userspace/src/usbip_network.h
index 2d1e070fb7b..1bbefc993fb 100644
--- a/drivers/staging/usbip/userspace/src/usbip_network.h
+++ b/drivers/staging/usbip/userspace/src/usbip_network.h
@@ -35,8 +35,8 @@ struct op_common {
 
 #define PACK_OP_COMMON(pack, op_common)  do {\
 	usbip_net_pack_uint16_t(pack, &(op_common)->version);\
-	usbip_net_pack_uint16_t(pack, &(op_common)->code   );\
-	usbip_net_pack_uint32_t(pack, &(op_common)->status );\
+	usbip_net_pack_uint16_t(pack, &(op_common)->code);\
+	usbip_net_pack_uint32_t(pack, &(op_common)->status);\
 } while (0)
 
 /* ---------------------------------------------------------------------- */
diff --git a/drivers/staging/usbip/userspace/src/usbipd.c b/drivers/staging/usbip/userspace/src/usbipd.c
index 34760cc1d10..3e913b861dc 100644
--- a/drivers/staging/usbip/userspace/src/usbipd.c
+++ b/drivers/staging/usbip/userspace/src/usbipd.c
@@ -60,7 +60,7 @@ static const char usbipd_help_string[] =
 	"	-d, --debug				\n"
 	"		Print debugging information.	\n"
 	"						\n"
-	"	-h, --help 				\n"
+	"	-h, --help				\n"
 	"		Print this help.		\n"
 	"						\n"
 	"	-v, --version				\n"
@@ -436,9 +436,6 @@ static int do_standalone_mode(int daemonize)
 	struct timespec timeout;
 	sigset_t sigmask;
 
-	if (usbip_names_init(USBIDS_FILE))
-		err("failed to open %s", USBIDS_FILE);
-
 	if (usbip_host_driver_open()) {
 		err("please load " USBIP_CORE_MOD_NAME ".ko and "
 		    USBIP_HOST_DRV_NAME ".ko!");
@@ -446,8 +443,9 @@ static int do_standalone_mode(int daemonize)
 	}
 
 	if (daemonize) {
-		if (daemon(0,0) < 0) {
+		if (daemon(0, 0) < 0) {
 			err("daemonizing failed: %s", strerror(errno));
+			usbip_host_driver_close();
 			return -1;
 		}
 		umask(0);
@@ -456,14 +454,18 @@ static int do_standalone_mode(int daemonize)
 	set_signal();
 
 	ai_head = do_getaddrinfo(NULL, PF_UNSPEC);
-	if (!ai_head)
+	if (!ai_head) {
+		usbip_host_driver_close();
 		return -1;
+	}
 
 	info("starting " PROGNAME " (%s)", usbip_version_string);
 
 	nsockfd = listen_all_addrinfo(ai_head, sockfdlist);
 	if (nsockfd <= 0) {
 		err("failed to open a listening socket");
+		freeaddrinfo(ai_head);
+		usbip_host_driver_close();
 		return -1;
 	}
 	fds = calloc(nsockfd, sizeof(struct pollfd));
@@ -502,7 +504,6 @@ static int do_standalone_mode(int daemonize)
 	free(fds);
 	freeaddrinfo(ai_head);
 	usbip_host_driver_close();
-	usbip_names_free();
 
 	return 0;
 }
diff --git a/drivers/staging/usbip/vhci.h b/drivers/staging/usbip/vhci.h
index 5dddc4d4b6a..a863a98a91c 100644
--- a/drivers/staging/usbip/vhci.h
+++ b/drivers/staging/usbip/vhci.h
@@ -95,7 +95,6 @@ struct vhci_hcd {
 
 extern struct vhci_hcd *the_controller;
 extern const struct attribute_group dev_attr_group;
-#define hardware (&the_controller->pdev.dev)
 
 /* vhci_hcd.c */
 void rh_port_connect(int rhport, enum usb_device_speed speed);
diff --git a/drivers/staging/usbip/vhci_hcd.c b/drivers/staging/usbip/vhci_hcd.c
index f1ca08478da..d7974cb2cc6 100644
--- a/drivers/staging/usbip/vhci_hcd.c
+++ b/drivers/staging/usbip/vhci_hcd.c
@@ -956,11 +956,10 @@ static int vhci_bus_resume(struct usb_hcd *hcd)
 	dev_dbg(&hcd->self.root_hub->dev, "%s\n", __func__);
 
 	spin_lock(&vhci->lock);
-	if (!HCD_HW_ACCESSIBLE(hcd)) {
+	if (!HCD_HW_ACCESSIBLE(hcd))
 		rc = -ESHUTDOWN;
-	} else {
+	else
 		hcd->state = HC_STATE_RUNNING;
-	}
 	spin_unlock(&vhci->lock);
 
 	return rc;
diff --git a/drivers/staging/usbip/vhci_rx.c b/drivers/staging/usbip/vhci_rx.c
index faf8e607c5c..d07fcb5ee93 100644
--- a/drivers/staging/usbip/vhci_rx.c
+++ b/drivers/staging/usbip/vhci_rx.c
@@ -31,33 +31,37 @@ struct urb *pickup_urb_and_free_priv(struct vhci_device *vdev, __u32 seqnum)
 	int status;
 
 	list_for_each_entry_safe(priv, tmp, &vdev->priv_rx, list) {
-		if (priv->seqnum == seqnum) {
-			urb = priv->urb;
-			status = urb->status;
-
-			usbip_dbg_vhci_rx("find urb %p vurb %p seqnum %u\n",
-					  urb, priv, seqnum);
-
-			/* TODO: fix logic here to improve indent situtation */
-			if (status != -EINPROGRESS) {
-				if (status == -ENOENT ||
-				    status == -ECONNRESET)
-					dev_info(&urb->dev->dev,
-						 "urb %p was unlinked "
-						 "%ssynchronuously.\n", urb,
-						 status == -ENOENT ? "" : "a");
-				else
-					dev_info(&urb->dev->dev,
-						 "urb %p may be in a error, "
-						 "status %d\n", urb, status);
-			}
-
-			list_del(&priv->list);
-			kfree(priv);
-			urb->hcpriv = NULL;
-
+		if (priv->seqnum != seqnum)
+			continue;
+
+		urb = priv->urb;
+		status = urb->status;
+
+		usbip_dbg_vhci_rx("find urb %p vurb %p seqnum %u\n",
+				urb, priv, seqnum);
+
+		switch (status) {
+		case -ENOENT:
+			/* fall through */
+		case -ECONNRESET:
+			dev_info(&urb->dev->dev,
+				 "urb %p was unlinked %ssynchronuously.\n", urb,
+				 status == -ENOENT ? "" : "a");
+			break;
+		case -EINPROGRESS:
+			/* no info output */
 			break;
+		default:
+			dev_info(&urb->dev->dev,
+				 "urb %p may be in a error, status %d\n", urb,
+				 status);
 		}
+
+		list_del(&priv->list);
+		kfree(priv);
+		urb->hcpriv = NULL;
+
+		break;
 	}
 
 	return urb;
@@ -202,7 +206,7 @@ static void vhci_rx_pdu(struct usbip_device *ud)
 
 	memset(&pdu, 0, sizeof(pdu));
 
-	/* 1. receive a pdu header */
+	/* receive a pdu header */
 	ret = usbip_recv(ud->tcp_socket, &pdu, sizeof(pdu));
 	if (ret < 0) {
 		if (ret == -ECONNRESET)
diff --git a/drivers/staging/vme/devices/vme_user.c b/drivers/staging/vme/devices/vme_user.c
index d074b1ecb41..da7f7598497 100644
--- a/drivers/staging/vme/devices/vme_user.c
+++ b/drivers/staging/vme/devices/vme_user.c
@@ -710,6 +710,10 @@ static int vme_user_probe(struct vme_dev *vdev)
 
 	/* Register the driver as a char device */
 	vme_user_cdev = cdev_alloc();
+	if (!vme_user_cdev) {
+		err = -ENOMEM;
+		goto err_char;
+	}
 	vme_user_cdev->ops = &vme_user_fops;
 	vme_user_cdev->owner = THIS_MODULE;
 	err = cdev_add(vme_user_cdev, MKDEV(VME_MAJOR, 0), VME_DEVS);
diff --git a/drivers/staging/vt6655/80211hdr.h b/drivers/staging/vt6655/80211hdr.h
index c4d2349260e..28078a114d4 100644
--- a/drivers/staging/vt6655/80211hdr.h
+++ b/drivers/staging/vt6655/80211hdr.h
@@ -86,7 +86,6 @@
 #define WLAN_DATA_MAXLEN            2312
 #define WLAN_A3FR_MAXLEN            (WLAN_HDR_ADDR3_LEN + WLAN_DATA_MAXLEN + WLAN_CRC_LEN)
 
-
 #define WLAN_BEACON_FR_MAXLEN       WLAN_A3FR_MAXLEN
 #define WLAN_ATIM_FR_MAXLEN         (WLAN_HDR_ADDR3_LEN + 0)
 #define WLAN_NULLDATA_FR_MAXLEN     (WLAN_HDR_ADDR3_LEN + 0)
@@ -100,7 +99,6 @@
 #define WLAN_AUTHEN_FR_MAXLEN       WLAN_A3FR_MAXLEN
 #define WLAN_DEAUTHEN_FR_MAXLEN     (WLAN_HDR_ADDR3_LEN + 2)
 
-
 #define WLAN_WEP_NKEYS              4
 #define WLAN_WEP40_KEYLEN           5
 #define WLAN_WEP104_KEYLEN          13
@@ -122,7 +120,6 @@
 #define WLAN_FTYPE_CTL  0x01
 #define WLAN_FTYPE_DATA 0x02
 
-
 /* Frame Subtypes */
 #define WLAN_FSTYPE_ASSOCREQ        0x00
 #define WLAN_FSTYPE_ASSOCRESP       0x01
@@ -155,7 +152,6 @@
 #define WLAN_FSTYPE_CFPOLL          0x06
 #define WLAN_FSTYPE_CFACK_CFPOLL    0x07
 
-
 #ifdef __BIG_ENDIAN
 
 /* GET & SET Frame Control bit */
@@ -175,7 +171,6 @@
 #define WLAN_GET_SEQ_FRGNUM(n) (((unsigned short)(n) >> 8) & (BIT0|BIT1|BIT2|BIT3))
 #define WLAN_GET_SEQ_SEQNUM(n) ((((unsigned short)(n) >> 8) & (~(BIT0|BIT1|BIT2|BIT3))) >> 4)
 
-
 /* Capability Field bit */
 #define WLAN_GET_CAP_INFO_ESS(n)           (((n) >> 8) & BIT0)
 #define WLAN_GET_CAP_INFO_IBSS(n)          ((((n) >> 8) & BIT1) >> 1)
@@ -190,7 +185,6 @@
 #define WLAN_GET_CAP_INFO_DSSSOFDM(n)      ((((n))      & BIT13) >> 13)
 #define WLAN_GET_CAP_INFO_GRPACK(n)        ((((n))      & BIT14) >> 14)
 
-
 #else
 
 /* GET & SET Frame Control bit */
@@ -206,12 +200,10 @@
 #define WLAN_GET_FC_ISWEP(n)    ((((unsigned short)(n)) & (BIT14)) >> 14)
 #define WLAN_GET_FC_ORDER(n)    ((((unsigned short)(n)) & (BIT15)) >> 15)
 
-
 /* Sequence Field bit */
 #define WLAN_GET_SEQ_FRGNUM(n) (((unsigned short)(n)) & (BIT0|BIT1|BIT2|BIT3))
 #define WLAN_GET_SEQ_SEQNUM(n) ((((unsigned short)(n)) & (~(BIT0|BIT1|BIT2|BIT3))) >> 4)
 
-
 /* Capability Field bit */
 #define WLAN_GET_CAP_INFO_ESS(n)           ((n) & BIT0)
 #define WLAN_GET_CAP_INFO_IBSS(n)          (((n) & BIT1) >> 1)
@@ -226,10 +218,8 @@
 #define WLAN_GET_CAP_INFO_DSSSOFDM(n)      (((n) & BIT13) >> 13)
 #define WLAN_GET_CAP_INFO_GRPACK(n)        (((n) & BIT14) >> 14)
 
-
 #endif /*#ifdef __BIG_ENDIAN */
 
-
 #define WLAN_SET_CAP_INFO_ESS(n)           (n)
 #define WLAN_SET_CAP_INFO_IBSS(n)          ((n) << 1)
 #define WLAN_SET_CAP_INFO_CFPOLLABLE(n)    ((n) << 2)
@@ -243,7 +233,6 @@
 #define WLAN_SET_CAP_INFO_DSSSOFDM(n)      ((n) << 13)
 #define WLAN_SET_CAP_INFO_GRPACK(n)        ((n) << 14)
 
-
 #define WLAN_SET_FC_PRVER(n)    ((unsigned short)(n))
 #define WLAN_SET_FC_FTYPE(n)    (((unsigned short)(n)) << 2)
 #define WLAN_SET_FC_FSTYPE(n)   (((unsigned short)(n)) << 4)
@@ -269,8 +258,6 @@
 #define WLAN_SET_ERP_USE_PROTECTION(n)     ((n) << 1)
 #define WLAN_SET_ERP_BARKER_MODE(n)        ((n) << 2)
 
-
-
 /* Support & Basic Rates field */
 #define WLAN_MGMT_IS_BASICRATE(b)    ((b) & BIT7)
 #define WLAN_MGMT_GET_RATE(b)        ((b) & ~BIT7)
@@ -288,64 +275,50 @@
 #define IEEE_ADDR_GROUP             0x01
 
 typedef struct {
-    unsigned char abyAddr[6];
+	unsigned char abyAddr[6];
 } IEEE_ADDR, *PIEEE_ADDR;
 
 /* 802.11 Header Format */
 
 typedef struct tagWLAN_80211HDR_A2 {
-
-    unsigned short wFrameCtl;
-    unsigned short wDurationID;
-    unsigned char abyAddr1[WLAN_ADDR_LEN];
-    unsigned char abyAddr2[WLAN_ADDR_LEN];
-
+	unsigned short wFrameCtl;
+	unsigned short wDurationID;
+	unsigned char abyAddr1[WLAN_ADDR_LEN];
+	unsigned char abyAddr2[WLAN_ADDR_LEN];
 } __attribute__ ((__packed__))
 WLAN_80211HDR_A2, *PWLAN_80211HDR_A2;
 
 typedef struct tagWLAN_80211HDR_A3 {
-
-    unsigned short wFrameCtl;
-    unsigned short wDurationID;
-    unsigned char abyAddr1[WLAN_ADDR_LEN];
-    unsigned char abyAddr2[WLAN_ADDR_LEN];
-    unsigned char abyAddr3[WLAN_ADDR_LEN];
-    unsigned short wSeqCtl;
-
-}__attribute__ ((__packed__))
+	unsigned short wFrameCtl;
+	unsigned short wDurationID;
+	unsigned char abyAddr1[WLAN_ADDR_LEN];
+	unsigned char abyAddr2[WLAN_ADDR_LEN];
+	unsigned char abyAddr3[WLAN_ADDR_LEN];
+	unsigned short wSeqCtl;
+} __attribute__ ((__packed__))
 WLAN_80211HDR_A3, *PWLAN_80211HDR_A3;
 
 typedef struct tagWLAN_80211HDR_A4 {
-
-    unsigned short wFrameCtl;
-    unsigned short wDurationID;
-    unsigned char abyAddr1[WLAN_ADDR_LEN];
-    unsigned char abyAddr2[WLAN_ADDR_LEN];
-    unsigned char abyAddr3[WLAN_ADDR_LEN];
-    unsigned short wSeqCtl;
-    unsigned char abyAddr4[WLAN_ADDR_LEN];
-
+	unsigned short wFrameCtl;
+	unsigned short wDurationID;
+	unsigned char abyAddr1[WLAN_ADDR_LEN];
+	unsigned char abyAddr2[WLAN_ADDR_LEN];
+	unsigned char abyAddr3[WLAN_ADDR_LEN];
+	unsigned short wSeqCtl;
+	unsigned char abyAddr4[WLAN_ADDR_LEN];
 } __attribute__ ((__packed__))
 WLAN_80211HDR_A4, *PWLAN_80211HDR_A4;
 
-
 typedef union tagUWLAN_80211HDR {
-
-    WLAN_80211HDR_A2        sA2;
-    WLAN_80211HDR_A3        sA3;
-    WLAN_80211HDR_A4        sA4;
-
+	WLAN_80211HDR_A2        sA2;
+	WLAN_80211HDR_A3        sA3;
+	WLAN_80211HDR_A4        sA4;
 } UWLAN_80211HDR, *PUWLAN_80211HDR;
 
-
 /*---------------------  Export Classes  ----------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
 
 /*---------------------  Export Functions  --------------------------*/
 
-
-
 #endif /* __80211HDR_H__ */
-
-
diff --git a/drivers/staging/vt6655/80211mgr.c b/drivers/staging/vt6655/80211mgr.c
index 1ed0f260b16..4cb26f3faf2 100644
--- a/drivers/staging/vt6655/80211mgr.c
+++ b/drivers/staging/vt6655/80211mgr.c
@@ -61,24 +61,18 @@
 
 /*---------------------  Static Definitions -------------------------*/
 
-
-
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
 
-static int          msglevel                =MSG_LEVEL_INFO;
+static int msglevel = MSG_LEVEL_INFO;
 //static int          msglevel                =MSG_LEVEL_DEBUG;
 /*---------------------  Static Functions  --------------------------*/
 
-
-
 /*---------------------  Export Variables  --------------------------*/
 
-
 /*---------------------  Export Functions  --------------------------*/
 
-
 /*+
  *
  * Routine Description:
@@ -87,26 +81,26 @@ static int          msglevel                =MSG_LEVEL_INFO;
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 void
 vMgrEncodeBeacon(
-    PWLAN_FR_BEACON  pFrame
-     )
+	PWLAN_FR_BEACON  pFrame
+)
 {
-    pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
+	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
 
-    // Fixed Fields
-    pFrame->pqwTimestamp = (PQWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                    + WLAN_BEACON_OFF_TS);
-    pFrame->pwBeaconInterval = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                       + WLAN_BEACON_OFF_BCN_INT);
-    pFrame->pwCapInfo = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                + WLAN_BEACON_OFF_CAPINFO);
+	// Fixed Fields
+	pFrame->pqwTimestamp = (PQWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					+ WLAN_BEACON_OFF_TS);
+	pFrame->pwBeaconInterval = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+						      + WLAN_BEACON_OFF_BCN_INT);
+	pFrame->pwCapInfo = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					       + WLAN_BEACON_OFF_CAPINFO);
 
-    pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_BEACON_OFF_SSID;
+	pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_BEACON_OFF_SSID;
 
-    return;
+	return;
 }
 
 /*+
@@ -118,118 +112,115 @@ vMgrEncodeBeacon(
  * Return Value:
  *    None.
  *
--*/
-
+ -*/
 
 void
 vMgrDecodeBeacon(
-    PWLAN_FR_BEACON  pFrame
-    )
+	PWLAN_FR_BEACON  pFrame
+)
 {
-    PWLAN_IE        pItem;
-
-    pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-    // Fixed Fields
-    pFrame->pqwTimestamp = (PQWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                    + WLAN_BEACON_OFF_TS);
-    pFrame->pwBeaconInterval = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                       + WLAN_BEACON_OFF_BCN_INT);
-    pFrame->pwCapInfo = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                + WLAN_BEACON_OFF_CAPINFO);
-
-    // Information elements
-    pItem = (PWLAN_IE)((unsigned char *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)))
-                       + WLAN_BEACON_OFF_SSID);
-    while( ((unsigned char *)pItem) < (pFrame->pBuf + pFrame->len) ){
-
-        switch (pItem->byElementID) {
-            case WLAN_EID_SSID:
-                if (pFrame->pSSID == NULL)
-                    pFrame->pSSID = (PWLAN_IE_SSID)pItem;
-                break;
-            case WLAN_EID_SUPP_RATES:
-                if (pFrame->pSuppRates == NULL)
-                    pFrame->pSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
-                break;
-            case WLAN_EID_FH_PARMS:
-                //pFrame->pFHParms = (PWLAN_IE_FH_PARMS)pItem;
-                break;
-            case WLAN_EID_DS_PARMS:
-                if (pFrame->pDSParms == NULL)
-                    pFrame->pDSParms = (PWLAN_IE_DS_PARMS)pItem;
-                break;
-            case WLAN_EID_CF_PARMS:
-                if (pFrame->pCFParms == NULL)
-                    pFrame->pCFParms = (PWLAN_IE_CF_PARMS)pItem;
-                break;
-            case WLAN_EID_IBSS_PARMS:
-                if (pFrame->pIBSSParms == NULL)
-                    pFrame->pIBSSParms = (PWLAN_IE_IBSS_PARMS)pItem;
-                break;
-            case WLAN_EID_TIM:
-                if (pFrame->pTIM == NULL)
-                    pFrame->pTIM = (PWLAN_IE_TIM)pItem;
-                break;
-
-            case WLAN_EID_RSN:
-                if (pFrame->pRSN == NULL) {
-                    pFrame->pRSN = (PWLAN_IE_RSN)pItem;
-                }
-                break;
-            case WLAN_EID_RSN_WPA:
-                if (pFrame->pRSNWPA == NULL) {
-                    if (WPAb_Is_RSN((PWLAN_IE_RSN_EXT)pItem) == true)
-                        pFrame->pRSNWPA = (PWLAN_IE_RSN_EXT)pItem;
-                }
-                break;
-
-            case WLAN_EID_ERP:
-                if (pFrame->pERP == NULL)
-                    pFrame->pERP = (PWLAN_IE_ERP)pItem;
-                break;
-            case WLAN_EID_EXTSUPP_RATES:
-                if (pFrame->pExtSuppRates == NULL)
-                    pFrame->pExtSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
-                break;
-
-            case WLAN_EID_COUNTRY:      //7
-                if (pFrame->pIE_Country == NULL)
-                    pFrame->pIE_Country = (PWLAN_IE_COUNTRY)pItem;
-                break;
-
-            case WLAN_EID_PWR_CONSTRAINT:   //32
-                if (pFrame->pIE_PowerConstraint == NULL)
-                    pFrame->pIE_PowerConstraint = (PWLAN_IE_PW_CONST)pItem;
-                break;
-
-            case WLAN_EID_CH_SWITCH:    //37
-                if (pFrame->pIE_CHSW == NULL)
-                    pFrame->pIE_CHSW = (PWLAN_IE_CH_SW)pItem;
-                break;
-
-            case WLAN_EID_QUIET:        //40
-                if (pFrame->pIE_Quiet == NULL)
-                    pFrame->pIE_Quiet = (PWLAN_IE_QUIET)pItem;
-                break;
-
-            case WLAN_EID_IBSS_DFS:
-                if (pFrame->pIE_IBSSDFS == NULL)
-                    pFrame->pIE_IBSSDFS = (PWLAN_IE_IBSS_DFS)pItem;
-                break;
-
-            default:
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Unrecognized EID=%dd in beacon decode.\n", pItem->byElementID);
-                break;
-
-        }
-        pItem = (PWLAN_IE)(((unsigned char *)pItem) + 2 + pItem->len);
-    }
-
-    return;
+	PWLAN_IE        pItem;
+
+	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
+
+	// Fixed Fields
+	pFrame->pqwTimestamp = (PQWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					+ WLAN_BEACON_OFF_TS);
+	pFrame->pwBeaconInterval = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+						      + WLAN_BEACON_OFF_BCN_INT);
+	pFrame->pwCapInfo = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					       + WLAN_BEACON_OFF_CAPINFO);
+
+	// Information elements
+	pItem = (PWLAN_IE)((unsigned char *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)))
+			   + WLAN_BEACON_OFF_SSID);
+	while (((unsigned char *)pItem) < (pFrame->pBuf + pFrame->len)) {
+		switch (pItem->byElementID) {
+		case WLAN_EID_SSID:
+			if (pFrame->pSSID == NULL)
+				pFrame->pSSID = (PWLAN_IE_SSID)pItem;
+			break;
+		case WLAN_EID_SUPP_RATES:
+			if (pFrame->pSuppRates == NULL)
+				pFrame->pSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
+			break;
+		case WLAN_EID_FH_PARMS:
+			//pFrame->pFHParms = (PWLAN_IE_FH_PARMS)pItem;
+			break;
+		case WLAN_EID_DS_PARMS:
+			if (pFrame->pDSParms == NULL)
+				pFrame->pDSParms = (PWLAN_IE_DS_PARMS)pItem;
+			break;
+		case WLAN_EID_CF_PARMS:
+			if (pFrame->pCFParms == NULL)
+				pFrame->pCFParms = (PWLAN_IE_CF_PARMS)pItem;
+			break;
+		case WLAN_EID_IBSS_PARMS:
+			if (pFrame->pIBSSParms == NULL)
+				pFrame->pIBSSParms = (PWLAN_IE_IBSS_PARMS)pItem;
+			break;
+		case WLAN_EID_TIM:
+			if (pFrame->pTIM == NULL)
+				pFrame->pTIM = (PWLAN_IE_TIM)pItem;
+			break;
+
+		case WLAN_EID_RSN:
+			if (pFrame->pRSN == NULL) {
+				pFrame->pRSN = (PWLAN_IE_RSN)pItem;
+			}
+			break;
+		case WLAN_EID_RSN_WPA:
+			if (pFrame->pRSNWPA == NULL) {
+				if (WPAb_Is_RSN((PWLAN_IE_RSN_EXT)pItem) == true)
+					pFrame->pRSNWPA = (PWLAN_IE_RSN_EXT)pItem;
+			}
+			break;
+
+		case WLAN_EID_ERP:
+			if (pFrame->pERP == NULL)
+				pFrame->pERP = (PWLAN_IE_ERP)pItem;
+			break;
+		case WLAN_EID_EXTSUPP_RATES:
+			if (pFrame->pExtSuppRates == NULL)
+				pFrame->pExtSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
+			break;
+
+		case WLAN_EID_COUNTRY:      //7
+			if (pFrame->pIE_Country == NULL)
+				pFrame->pIE_Country = (PWLAN_IE_COUNTRY)pItem;
+			break;
+
+		case WLAN_EID_PWR_CONSTRAINT:   //32
+			if (pFrame->pIE_PowerConstraint == NULL)
+				pFrame->pIE_PowerConstraint = (PWLAN_IE_PW_CONST)pItem;
+			break;
+
+		case WLAN_EID_CH_SWITCH:    //37
+			if (pFrame->pIE_CHSW == NULL)
+				pFrame->pIE_CHSW = (PWLAN_IE_CH_SW)pItem;
+			break;
+
+		case WLAN_EID_QUIET:        //40
+			if (pFrame->pIE_Quiet == NULL)
+				pFrame->pIE_Quiet = (PWLAN_IE_QUIET)pItem;
+			break;
+
+		case WLAN_EID_IBSS_DFS:
+			if (pFrame->pIE_IBSSDFS == NULL)
+				pFrame->pIE_IBSSDFS = (PWLAN_IE_IBSS_DFS)pItem;
+			break;
+
+		default:
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Unrecognized EID=%dd in beacon decode.\n", pItem->byElementID);
+			break;
+
+		}
+		pItem = (PWLAN_IE)(((unsigned char *)pItem) + 2 + pItem->len);
+	}
+
+	return;
 }
 
-
 /*+
  *
  * Routine Description:
@@ -239,21 +230,19 @@ vMgrDecodeBeacon(
  * Return Value:
  *    None.
  *
--*/
-
+ -*/
 
 void
 vMgrEncodeIBSSATIM(
-    PWLAN_FR_IBSSATIM   pFrame
-    )
+	PWLAN_FR_IBSSATIM   pFrame
+)
 {
-    pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-    pFrame->len = WLAN_HDR_ADDR3_LEN;
+	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
+	pFrame->len = WLAN_HDR_ADDR3_LEN;
 
-    return;
+	return;
 }
 
-
 /*+
  *
  * Routine Description:
@@ -263,19 +252,18 @@ vMgrEncodeIBSSATIM(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 void
 vMgrDecodeIBSSATIM(
-    PWLAN_FR_IBSSATIM   pFrame
-    )
+	PWLAN_FR_IBSSATIM   pFrame
+)
 {
-    pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
+	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
 
-    return;
+	return;
 }
 
-
 /*+
  *
  * Routine Description:
@@ -285,25 +273,23 @@ vMgrDecodeIBSSATIM(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 void
 vMgrEncodeDisassociation(
-    PWLAN_FR_DISASSOC  pFrame
-    )
+	PWLAN_FR_DISASSOC  pFrame
+)
 {
-    pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
+	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
 
+	// Fixed Fields
+	pFrame->pwReason = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					      + WLAN_DISASSOC_OFF_REASON);
+	pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_DISASSOC_OFF_REASON + sizeof(*(pFrame->pwReason));
 
-    // Fixed Fields
-    pFrame->pwReason = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                               + WLAN_DISASSOC_OFF_REASON);
-    pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_DISASSOC_OFF_REASON + sizeof(*(pFrame->pwReason));
-
-    return;
+	return;
 }
 
-
 /*+
  *
  * Routine Description:
@@ -313,20 +299,20 @@ vMgrEncodeDisassociation(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 void
 vMgrDecodeDisassociation(
-    PWLAN_FR_DISASSOC  pFrame
-    )
+	PWLAN_FR_DISASSOC  pFrame
+)
 {
-    pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
+	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
 
-    // Fixed Fields
-    pFrame->pwReason = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                               + WLAN_DISASSOC_OFF_REASON);
+	// Fixed Fields
+	pFrame->pwReason = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					      + WLAN_DISASSOC_OFF_REASON);
 
-    return;
+	return;
 }
 
 /*+
@@ -338,25 +324,23 @@ vMgrDecodeDisassociation(
  * Return Value:
  *    None.
  *
--*/
-
+ -*/
 
 void
 vMgrEncodeAssocRequest(
-    PWLAN_FR_ASSOCREQ  pFrame
-    )
+	PWLAN_FR_ASSOCREQ  pFrame
+)
 {
-    pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-    // Fixed Fields
-    pFrame->pwCapInfo = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                + WLAN_ASSOCREQ_OFF_CAP_INFO);
-    pFrame->pwListenInterval = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                       + WLAN_ASSOCREQ_OFF_LISTEN_INT);
-    pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_ASSOCREQ_OFF_LISTEN_INT + sizeof(*(pFrame->pwListenInterval));
-    return;
+	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
+	// Fixed Fields
+	pFrame->pwCapInfo = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					       + WLAN_ASSOCREQ_OFF_CAP_INFO);
+	pFrame->pwListenInterval = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+						      + WLAN_ASSOCREQ_OFF_LISTEN_INT);
+	pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_ASSOCREQ_OFF_LISTEN_INT + sizeof(*(pFrame->pwListenInterval));
+	return;
 }
 
-
 /*+
  *
  * Routine Description: (AP)
@@ -366,61 +350,61 @@ vMgrEncodeAssocRequest(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 void
 vMgrDecodeAssocRequest(
-    PWLAN_FR_ASSOCREQ  pFrame
-    )
+	PWLAN_FR_ASSOCREQ  pFrame
+)
 {
-    PWLAN_IE   pItem;
-
-    pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-    // Fixed Fields
-    pFrame->pwCapInfo = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                + WLAN_ASSOCREQ_OFF_CAP_INFO);
-    pFrame->pwListenInterval = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                + WLAN_ASSOCREQ_OFF_LISTEN_INT);
-
-    // Information elements
-    pItem = (PWLAN_IE)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                            + WLAN_ASSOCREQ_OFF_SSID);
-
-    while (((unsigned char *)pItem) < (pFrame->pBuf + pFrame->len)) {
-        switch (pItem->byElementID){
-            case WLAN_EID_SSID:
-                if (pFrame->pSSID == NULL)
-                    pFrame->pSSID = (PWLAN_IE_SSID)pItem;
-                break;
-            case WLAN_EID_SUPP_RATES:
-                if (pFrame->pSuppRates == NULL)
-                    pFrame->pSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
-                break;
-
-            case WLAN_EID_RSN:
-                if (pFrame->pRSN == NULL) {
-                    pFrame->pRSN = (PWLAN_IE_RSN)pItem;
-                }
-                break;
-            case WLAN_EID_RSN_WPA:
-                if (pFrame->pRSNWPA == NULL) {
-                    if (WPAb_Is_RSN((PWLAN_IE_RSN_EXT)pItem) == true)
-                        pFrame->pRSNWPA = (PWLAN_IE_RSN_EXT)pItem;
-                }
-                break;
-            case WLAN_EID_EXTSUPP_RATES:
-                if (pFrame->pExtSuppRates == NULL)
-                    pFrame->pExtSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
-                break;
-
-            default:
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Unrecognized EID=%dd in assocreq decode.\n",
-                        pItem->byElementID);
-                break;
-        }
-        pItem = (PWLAN_IE)(((unsigned char *)pItem) + 2 + pItem->len);
-    }
-    return;
+	PWLAN_IE   pItem;
+
+	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
+	// Fixed Fields
+	pFrame->pwCapInfo = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					       + WLAN_ASSOCREQ_OFF_CAP_INFO);
+	pFrame->pwListenInterval = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+						      + WLAN_ASSOCREQ_OFF_LISTEN_INT);
+
+	// Information elements
+	pItem = (PWLAN_IE)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+			   + WLAN_ASSOCREQ_OFF_SSID);
+
+	while (((unsigned char *)pItem) < (pFrame->pBuf + pFrame->len)) {
+		switch (pItem->byElementID) {
+		case WLAN_EID_SSID:
+			if (pFrame->pSSID == NULL)
+				pFrame->pSSID = (PWLAN_IE_SSID)pItem;
+			break;
+		case WLAN_EID_SUPP_RATES:
+			if (pFrame->pSuppRates == NULL)
+				pFrame->pSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
+			break;
+
+		case WLAN_EID_RSN:
+			if (pFrame->pRSN == NULL) {
+				pFrame->pRSN = (PWLAN_IE_RSN)pItem;
+			}
+			break;
+		case WLAN_EID_RSN_WPA:
+			if (pFrame->pRSNWPA == NULL) {
+				if (WPAb_Is_RSN((PWLAN_IE_RSN_EXT)pItem) == true)
+					pFrame->pRSNWPA = (PWLAN_IE_RSN_EXT)pItem;
+			}
+			break;
+		case WLAN_EID_EXTSUPP_RATES:
+			if (pFrame->pExtSuppRates == NULL)
+				pFrame->pExtSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
+			break;
+
+		default:
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Unrecognized EID=%dd in assocreq decode.\n",
+				pItem->byElementID);
+			break;
+		}
+		pItem = (PWLAN_IE)(((unsigned char *)pItem) + 2 + pItem->len);
+	}
+	return;
 }
 
 /*+
@@ -432,29 +416,28 @@ vMgrDecodeAssocRequest(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 void
 vMgrEncodeAssocResponse(
-    PWLAN_FR_ASSOCRESP  pFrame
-     )
+	PWLAN_FR_ASSOCRESP  pFrame
+)
 {
-    pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-    // Fixed Fields
-    pFrame->pwCapInfo = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                + WLAN_ASSOCRESP_OFF_CAP_INFO);
-    pFrame->pwStatus = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                               + WLAN_ASSOCRESP_OFF_STATUS);
-    pFrame->pwAid = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                            + WLAN_ASSOCRESP_OFF_AID);
-    pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_ASSOCRESP_OFF_AID
-                  + sizeof(*(pFrame->pwAid));
-
-    return;
+	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
+
+	// Fixed Fields
+	pFrame->pwCapInfo = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					       + WLAN_ASSOCRESP_OFF_CAP_INFO);
+	pFrame->pwStatus = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					      + WLAN_ASSOCRESP_OFF_STATUS);
+	pFrame->pwAid = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					   + WLAN_ASSOCRESP_OFF_AID);
+	pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_ASSOCRESP_OFF_AID
+		+ sizeof(*(pFrame->pwAid));
+
+	return;
 }
 
-
 /*+
  *
  * Routine Description:
@@ -464,44 +447,42 @@ vMgrEncodeAssocResponse(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 void
 vMgrDecodeAssocResponse(
-    PWLAN_FR_ASSOCRESP  pFrame
-     )
+	PWLAN_FR_ASSOCRESP  pFrame
+)
 {
-    PWLAN_IE   pItem;
-
-    pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-    // Fixed Fields
-    pFrame->pwCapInfo = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                + WLAN_ASSOCRESP_OFF_CAP_INFO);
-    pFrame->pwStatus = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                               + WLAN_ASSOCRESP_OFF_STATUS);
-    pFrame->pwAid = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                            + WLAN_ASSOCRESP_OFF_AID);
-
-    // Information elements
-    pFrame->pSuppRates  = (PWLAN_IE_SUPP_RATES)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                           + WLAN_ASSOCRESP_OFF_SUPP_RATES);
-
-    pItem = (PWLAN_IE)(pFrame->pSuppRates);
-    pItem = (PWLAN_IE)(((unsigned char *)pItem) + 2 + pItem->len);
-
-    if ((((unsigned char *)pItem) < (pFrame->pBuf + pFrame->len)) &&
-		    (pItem->byElementID == WLAN_EID_EXTSUPP_RATES)) {
-        pFrame->pExtSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pFrame->pExtSuppRates=[%p].\n", pItem);
-    }
-    else {
-        pFrame->pExtSuppRates = NULL;
-    }
-    return;
+	PWLAN_IE   pItem;
+
+	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
+
+	// Fixed Fields
+	pFrame->pwCapInfo = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					       + WLAN_ASSOCRESP_OFF_CAP_INFO);
+	pFrame->pwStatus = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					      + WLAN_ASSOCRESP_OFF_STATUS);
+	pFrame->pwAid = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					   + WLAN_ASSOCRESP_OFF_AID);
+
+	// Information elements
+	pFrame->pSuppRates  = (PWLAN_IE_SUPP_RATES)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+						    + WLAN_ASSOCRESP_OFF_SUPP_RATES);
+
+	pItem = (PWLAN_IE)(pFrame->pSuppRates);
+	pItem = (PWLAN_IE)(((unsigned char *)pItem) + 2 + pItem->len);
+
+	if ((((unsigned char *)pItem) < (pFrame->pBuf + pFrame->len)) &&
+	    (pItem->byElementID == WLAN_EID_EXTSUPP_RATES)) {
+		pFrame->pExtSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pFrame->pExtSuppRates=[%p].\n", pItem);
+	} else {
+		pFrame->pExtSuppRates = NULL;
+	}
+	return;
 }
 
-
 /*+
  *
  * Routine Description:
@@ -511,28 +492,27 @@ vMgrDecodeAssocResponse(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 void
 vMgrEncodeReassocRequest(
-    PWLAN_FR_REASSOCREQ  pFrame
-     )
+	PWLAN_FR_REASSOCREQ  pFrame
+)
 {
-    pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-    // Fixed Fields
-    pFrame->pwCapInfo = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                + WLAN_REASSOCREQ_OFF_CAP_INFO);
-    pFrame->pwListenInterval = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                      + WLAN_REASSOCREQ_OFF_LISTEN_INT);
-    pFrame->pAddrCurrAP = (PIEEE_ADDR)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                       + WLAN_REASSOCREQ_OFF_CURR_AP);
-    pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_REASSOCREQ_OFF_CURR_AP + sizeof(*(pFrame->pAddrCurrAP));
-
-    return;
+	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
+
+	// Fixed Fields
+	pFrame->pwCapInfo = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					       + WLAN_REASSOCREQ_OFF_CAP_INFO);
+	pFrame->pwListenInterval = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+						      + WLAN_REASSOCREQ_OFF_LISTEN_INT);
+	pFrame->pAddrCurrAP = (PIEEE_ADDR)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					   + WLAN_REASSOCREQ_OFF_CURR_AP);
+	pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_REASSOCREQ_OFF_CURR_AP + sizeof(*(pFrame->pAddrCurrAP));
+
+	return;
 }
 
-
 /*+
  *
  * Routine Description: (AP)
@@ -542,69 +522,65 @@ vMgrEncodeReassocRequest(
  * Return Value:
  *    None.
  *
--*/
-
+ -*/
 
 void
 vMgrDecodeReassocRequest(
-    PWLAN_FR_REASSOCREQ  pFrame
-     )
+	PWLAN_FR_REASSOCREQ  pFrame
+)
 {
-    PWLAN_IE   pItem;
-    pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-    // Fixed Fields
-    pFrame->pwCapInfo = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                + WLAN_REASSOCREQ_OFF_CAP_INFO);
-    pFrame->pwListenInterval = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                      + WLAN_REASSOCREQ_OFF_LISTEN_INT);
-    pFrame->pAddrCurrAP = (PIEEE_ADDR)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                       + WLAN_REASSOCREQ_OFF_CURR_AP);
-
-    // Information elements
-    pItem = (PWLAN_IE)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                       + WLAN_REASSOCREQ_OFF_SSID);
-
-    while(((unsigned char *)pItem) < (pFrame->pBuf + pFrame->len)) {
-
-        switch (pItem->byElementID){
-            case WLAN_EID_SSID:
-                if (pFrame->pSSID == NULL)
-                    pFrame->pSSID = (PWLAN_IE_SSID)pItem;
-                break;
-            case WLAN_EID_SUPP_RATES:
-                if (pFrame->pSuppRates == NULL)
-                    pFrame->pSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
-                break;
-
-            case WLAN_EID_RSN:
-                if (pFrame->pRSN == NULL) {
-                    pFrame->pRSN = (PWLAN_IE_RSN)pItem;
-                }
-                break;
-            case WLAN_EID_RSN_WPA:
-                if (pFrame->pRSNWPA == NULL) {
-                    if (WPAb_Is_RSN((PWLAN_IE_RSN_EXT)pItem) == true)
-                        pFrame->pRSNWPA = (PWLAN_IE_RSN_EXT)pItem;
-                }
-                break;
-
-            case WLAN_EID_EXTSUPP_RATES:
-                if (pFrame->pExtSuppRates == NULL)
-                    pFrame->pExtSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
-                break;
-            default:
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Unrecognized EID=%dd in reassocreq decode.\n",
-                            pItem->byElementID);
-                break;
-        }
-        pItem = (PWLAN_IE)(((unsigned char *)pItem) + 2 + pItem->len);
-    }
-    return;
+	PWLAN_IE   pItem;
+	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
+
+	// Fixed Fields
+	pFrame->pwCapInfo = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					       + WLAN_REASSOCREQ_OFF_CAP_INFO);
+	pFrame->pwListenInterval = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+						      + WLAN_REASSOCREQ_OFF_LISTEN_INT);
+	pFrame->pAddrCurrAP = (PIEEE_ADDR)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					   + WLAN_REASSOCREQ_OFF_CURR_AP);
+
+	// Information elements
+	pItem = (PWLAN_IE)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+			   + WLAN_REASSOCREQ_OFF_SSID);
+
+	while (((unsigned char *)pItem) < (pFrame->pBuf + pFrame->len)) {
+		switch (pItem->byElementID) {
+		case WLAN_EID_SSID:
+			if (pFrame->pSSID == NULL)
+				pFrame->pSSID = (PWLAN_IE_SSID)pItem;
+			break;
+		case WLAN_EID_SUPP_RATES:
+			if (pFrame->pSuppRates == NULL)
+				pFrame->pSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
+			break;
+
+		case WLAN_EID_RSN:
+			if (pFrame->pRSN == NULL) {
+				pFrame->pRSN = (PWLAN_IE_RSN)pItem;
+			}
+			break;
+		case WLAN_EID_RSN_WPA:
+			if (pFrame->pRSNWPA == NULL) {
+				if (WPAb_Is_RSN((PWLAN_IE_RSN_EXT)pItem) == true)
+					pFrame->pRSNWPA = (PWLAN_IE_RSN_EXT)pItem;
+			}
+			break;
+
+		case WLAN_EID_EXTSUPP_RATES:
+			if (pFrame->pExtSuppRates == NULL)
+				pFrame->pExtSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
+			break;
+		default:
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Unrecognized EID=%dd in reassocreq decode.\n",
+				pItem->byElementID);
+			break;
+		}
+		pItem = (PWLAN_IE)(((unsigned char *)pItem) + 2 + pItem->len);
+	}
+	return;
 }
 
-
-
 /*+
  *
  * Routine Description:
@@ -614,17 +590,16 @@ vMgrDecodeReassocRequest(
  * Return Value:
  *    None.
  *
--*/
-
+ -*/
 
 void
 vMgrEncodeProbeRequest(
-    PWLAN_FR_PROBEREQ  pFrame
-     )
+	PWLAN_FR_PROBEREQ  pFrame
+)
 {
-    pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-    pFrame->len = WLAN_HDR_ADDR3_LEN;
-    return;
+	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
+	pFrame->len = WLAN_HDR_ADDR3_LEN;
+	return;
 }
 
 /*+
@@ -636,49 +611,47 @@ vMgrEncodeProbeRequest(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 void
 vMgrDecodeProbeRequest(
-    PWLAN_FR_PROBEREQ  pFrame
-     )
+	PWLAN_FR_PROBEREQ  pFrame
+)
 {
-    PWLAN_IE   pItem;
-
-    pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-    // Information elements
-    pItem = (PWLAN_IE)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)));
-
-    while( ((unsigned char *)pItem) < (pFrame->pBuf + pFrame->len) ) {
-
-        switch (pItem->byElementID) {
-            case WLAN_EID_SSID:
-                if (pFrame->pSSID == NULL)
-                    pFrame->pSSID = (PWLAN_IE_SSID)pItem;
-                break;
-
-            case WLAN_EID_SUPP_RATES:
-                if (pFrame->pSuppRates == NULL)
-                    pFrame->pSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
-                break;
-
-            case WLAN_EID_EXTSUPP_RATES:
-                if (pFrame->pExtSuppRates == NULL)
-                    pFrame->pExtSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
-                break;
-
-            default:
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Bad EID=%dd in probereq\n", pItem->byElementID);
-                break;
-        }
-
-        pItem = (PWLAN_IE)(((unsigned char *)pItem) + 2 +  pItem->len);
-    }
-    return;
+	PWLAN_IE   pItem;
+
+	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
+
+	// Information elements
+	pItem = (PWLAN_IE)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)));
+
+	while (((unsigned char *)pItem) < (pFrame->pBuf + pFrame->len)) {
+		switch (pItem->byElementID) {
+		case WLAN_EID_SSID:
+			if (pFrame->pSSID == NULL)
+				pFrame->pSSID = (PWLAN_IE_SSID)pItem;
+			break;
+
+		case WLAN_EID_SUPP_RATES:
+			if (pFrame->pSuppRates == NULL)
+				pFrame->pSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
+			break;
+
+		case WLAN_EID_EXTSUPP_RATES:
+			if (pFrame->pExtSuppRates == NULL)
+				pFrame->pExtSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
+			break;
+
+		default:
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Bad EID=%dd in probereq\n", pItem->byElementID);
+			break;
+		}
+
+		pItem = (PWLAN_IE)(((unsigned char *)pItem) + 2 +  pItem->len);
+	}
+	return;
 }
 
-
 /*+
  *
  * Routine Description:
@@ -688,32 +661,29 @@ vMgrDecodeProbeRequest(
  * Return Value:
  *    None.
  *
--*/
-
+ -*/
 
 void
 vMgrEncodeProbeResponse(
-    PWLAN_FR_PROBERESP  pFrame
-    )
+	PWLAN_FR_PROBERESP  pFrame
+)
 {
-    pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
+	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
 
-    // Fixed Fields
-    pFrame->pqwTimestamp = (PQWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                    + WLAN_PROBERESP_OFF_TS);
-    pFrame->pwBeaconInterval = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                       + WLAN_PROBERESP_OFF_BCN_INT);
-    pFrame->pwCapInfo = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                + WLAN_PROBERESP_OFF_CAP_INFO);
+	// Fixed Fields
+	pFrame->pqwTimestamp = (PQWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					+ WLAN_PROBERESP_OFF_TS);
+	pFrame->pwBeaconInterval = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+						      + WLAN_PROBERESP_OFF_BCN_INT);
+	pFrame->pwCapInfo = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					       + WLAN_PROBERESP_OFF_CAP_INFO);
 
-    pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_PROBERESP_OFF_CAP_INFO +
-                  sizeof(*(pFrame->pwCapInfo));
+	pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_PROBERESP_OFF_CAP_INFO +
+		sizeof(*(pFrame->pwCapInfo));
 
-    return;
+	return;
 }
 
-
-
 /*+
  *
  * Routine Description:
@@ -723,111 +693,109 @@ vMgrEncodeProbeResponse(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 void
 vMgrDecodeProbeResponse(
-    PWLAN_FR_PROBERESP  pFrame
-    )
+	PWLAN_FR_PROBERESP  pFrame
+)
 {
-    PWLAN_IE    pItem;
-
-
-    pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-    // Fixed Fields
-    pFrame->pqwTimestamp = (PQWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                    + WLAN_PROBERESP_OFF_TS);
-    pFrame->pwBeaconInterval = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                       + WLAN_PROBERESP_OFF_BCN_INT);
-    pFrame->pwCapInfo = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                + WLAN_PROBERESP_OFF_CAP_INFO);
-
-    // Information elements
-    pItem = (PWLAN_IE)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                       + WLAN_PROBERESP_OFF_SSID);
-
-    while( ((unsigned char *)pItem) < (pFrame->pBuf + pFrame->len) ) {
-        switch (pItem->byElementID) {
-            case WLAN_EID_SSID:
-                if (pFrame->pSSID == NULL)
-                pFrame->pSSID = (PWLAN_IE_SSID)pItem;
-                break;
-            case WLAN_EID_SUPP_RATES:
-                if (pFrame->pSuppRates == NULL)
-                pFrame->pSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
-                break;
-            case WLAN_EID_FH_PARMS:
-                break;
-            case WLAN_EID_DS_PARMS:
-                if (pFrame->pDSParms == NULL)
-                    pFrame->pDSParms = (PWLAN_IE_DS_PARMS)pItem;
-                break;
-            case WLAN_EID_CF_PARMS:
-                if (pFrame->pCFParms == NULL)
-                    pFrame->pCFParms = (PWLAN_IE_CF_PARMS)pItem;
-                break;
-            case WLAN_EID_IBSS_PARMS:
-                if (pFrame->pIBSSParms == NULL)
-                    pFrame->pIBSSParms = (PWLAN_IE_IBSS_PARMS)pItem;
-                break;
-
-            case WLAN_EID_RSN:
-                if (pFrame->pRSN == NULL) {
-                    pFrame->pRSN = (PWLAN_IE_RSN)pItem;
-                }
-                break;
-            case WLAN_EID_RSN_WPA:
-                if (pFrame->pRSNWPA == NULL) {
-                    if (WPAb_Is_RSN((PWLAN_IE_RSN_EXT)pItem) == true)
-                        pFrame->pRSNWPA = (PWLAN_IE_RSN_EXT)pItem;
-                }
-                break;
-            case WLAN_EID_ERP:
-                if (pFrame->pERP == NULL)
-                    pFrame->pERP = (PWLAN_IE_ERP)pItem;
-                break;
-            case WLAN_EID_EXTSUPP_RATES:
-                if (pFrame->pExtSuppRates == NULL)
-                    pFrame->pExtSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
-                break;
-
-            case WLAN_EID_COUNTRY:      //7
-                if (pFrame->pIE_Country == NULL)
-                    pFrame->pIE_Country = (PWLAN_IE_COUNTRY)pItem;
-                break;
-
-            case WLAN_EID_PWR_CONSTRAINT:   //32
-                if (pFrame->pIE_PowerConstraint == NULL)
-                    pFrame->pIE_PowerConstraint = (PWLAN_IE_PW_CONST)pItem;
-                break;
-
-            case WLAN_EID_CH_SWITCH:    //37
-                if (pFrame->pIE_CHSW == NULL)
-                    pFrame->pIE_CHSW = (PWLAN_IE_CH_SW)pItem;
-                break;
-
-            case WLAN_EID_QUIET:        //40
-                if (pFrame->pIE_Quiet == NULL)
-                    pFrame->pIE_Quiet = (PWLAN_IE_QUIET)pItem;
-                break;
-
-            case WLAN_EID_IBSS_DFS:
-                if (pFrame->pIE_IBSSDFS == NULL)
-                    pFrame->pIE_IBSSDFS = (PWLAN_IE_IBSS_DFS)pItem;
-                break;
-
-            default:
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Bad EID=%dd in proberesp\n", pItem->byElementID);
-                break;
-        }
-
-        pItem = (PWLAN_IE)(((unsigned char *)pItem) + 2 +  pItem->len);
-    }
-    return;
+	PWLAN_IE    pItem;
+
+	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
+
+	// Fixed Fields
+	pFrame->pqwTimestamp = (PQWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					+ WLAN_PROBERESP_OFF_TS);
+	pFrame->pwBeaconInterval = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+						      + WLAN_PROBERESP_OFF_BCN_INT);
+	pFrame->pwCapInfo = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					       + WLAN_PROBERESP_OFF_CAP_INFO);
+
+	// Information elements
+	pItem = (PWLAN_IE)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+			   + WLAN_PROBERESP_OFF_SSID);
+
+	while (((unsigned char *)pItem) < (pFrame->pBuf + pFrame->len)) {
+		switch (pItem->byElementID) {
+		case WLAN_EID_SSID:
+			if (pFrame->pSSID == NULL)
+				pFrame->pSSID = (PWLAN_IE_SSID)pItem;
+			break;
+		case WLAN_EID_SUPP_RATES:
+			if (pFrame->pSuppRates == NULL)
+				pFrame->pSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
+			break;
+		case WLAN_EID_FH_PARMS:
+			break;
+		case WLAN_EID_DS_PARMS:
+			if (pFrame->pDSParms == NULL)
+				pFrame->pDSParms = (PWLAN_IE_DS_PARMS)pItem;
+			break;
+		case WLAN_EID_CF_PARMS:
+			if (pFrame->pCFParms == NULL)
+				pFrame->pCFParms = (PWLAN_IE_CF_PARMS)pItem;
+			break;
+		case WLAN_EID_IBSS_PARMS:
+			if (pFrame->pIBSSParms == NULL)
+				pFrame->pIBSSParms = (PWLAN_IE_IBSS_PARMS)pItem;
+			break;
+
+		case WLAN_EID_RSN:
+			if (pFrame->pRSN == NULL) {
+				pFrame->pRSN = (PWLAN_IE_RSN)pItem;
+			}
+			break;
+		case WLAN_EID_RSN_WPA:
+			if (pFrame->pRSNWPA == NULL) {
+				if (WPAb_Is_RSN((PWLAN_IE_RSN_EXT)pItem) == true)
+					pFrame->pRSNWPA = (PWLAN_IE_RSN_EXT)pItem;
+			}
+			break;
+		case WLAN_EID_ERP:
+			if (pFrame->pERP == NULL)
+				pFrame->pERP = (PWLAN_IE_ERP)pItem;
+			break;
+		case WLAN_EID_EXTSUPP_RATES:
+			if (pFrame->pExtSuppRates == NULL)
+				pFrame->pExtSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
+			break;
+
+		case WLAN_EID_COUNTRY:      //7
+			if (pFrame->pIE_Country == NULL)
+				pFrame->pIE_Country = (PWLAN_IE_COUNTRY)pItem;
+			break;
+
+		case WLAN_EID_PWR_CONSTRAINT:   //32
+			if (pFrame->pIE_PowerConstraint == NULL)
+				pFrame->pIE_PowerConstraint = (PWLAN_IE_PW_CONST)pItem;
+			break;
+
+		case WLAN_EID_CH_SWITCH:    //37
+			if (pFrame->pIE_CHSW == NULL)
+				pFrame->pIE_CHSW = (PWLAN_IE_CH_SW)pItem;
+			break;
+
+		case WLAN_EID_QUIET:        //40
+			if (pFrame->pIE_Quiet == NULL)
+				pFrame->pIE_Quiet = (PWLAN_IE_QUIET)pItem;
+			break;
+
+		case WLAN_EID_IBSS_DFS:
+			if (pFrame->pIE_IBSSDFS == NULL)
+				pFrame->pIE_IBSSDFS = (PWLAN_IE_IBSS_DFS)pItem;
+			break;
+
+		default:
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Bad EID=%dd in proberesp\n", pItem->byElementID);
+			break;
+		}
+
+		pItem = (PWLAN_IE)(((unsigned char *)pItem) + 2 +  pItem->len);
+	}
+	return;
 }
 
-
 /*+
  *
  * Routine Description:
@@ -837,28 +805,27 @@ vMgrDecodeProbeResponse(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 void
 vMgrEncodeAuthen(
-    PWLAN_FR_AUTHEN  pFrame
-    )
+	PWLAN_FR_AUTHEN  pFrame
+)
 {
-    pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-    // Fixed Fields
-    pFrame->pwAuthAlgorithm = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                      + WLAN_AUTHEN_OFF_AUTH_ALG);
-    pFrame->pwAuthSequence = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                     + WLAN_AUTHEN_OFF_AUTH_SEQ);
-    pFrame->pwStatus = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                               + WLAN_AUTHEN_OFF_STATUS);
-    pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_AUTHEN_OFF_STATUS + sizeof(*(pFrame->pwStatus));
-
-    return;
+	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
+
+	// Fixed Fields
+	pFrame->pwAuthAlgorithm = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+						     + WLAN_AUTHEN_OFF_AUTH_ALG);
+	pFrame->pwAuthSequence = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+						    + WLAN_AUTHEN_OFF_AUTH_SEQ);
+	pFrame->pwStatus = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					      + WLAN_AUTHEN_OFF_STATUS);
+	pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_AUTHEN_OFF_STATUS + sizeof(*(pFrame->pwStatus));
+
+	return;
 }
 
-
 /*+
  *
  * Routine Description:
@@ -868,37 +835,36 @@ vMgrEncodeAuthen(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 void
 vMgrDecodeAuthen(
-    PWLAN_FR_AUTHEN  pFrame
-    )
+	PWLAN_FR_AUTHEN  pFrame
+)
 {
-    PWLAN_IE    pItem;
+	PWLAN_IE    pItem;
 
-    pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
+	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
 
-    // Fixed Fields
-    pFrame->pwAuthAlgorithm = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                      + WLAN_AUTHEN_OFF_AUTH_ALG);
-    pFrame->pwAuthSequence = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                     + WLAN_AUTHEN_OFF_AUTH_SEQ);
-    pFrame->pwStatus = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                               + WLAN_AUTHEN_OFF_STATUS);
+	// Fixed Fields
+	pFrame->pwAuthAlgorithm = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+						     + WLAN_AUTHEN_OFF_AUTH_ALG);
+	pFrame->pwAuthSequence = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+						    + WLAN_AUTHEN_OFF_AUTH_SEQ);
+	pFrame->pwStatus = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					      + WLAN_AUTHEN_OFF_STATUS);
 
-    // Information elements
-    pItem = (PWLAN_IE)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                       + WLAN_AUTHEN_OFF_CHALLENGE);
+	// Information elements
+	pItem = (PWLAN_IE)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+			   + WLAN_AUTHEN_OFF_CHALLENGE);
 
-    if ((((unsigned char *)pItem) < (pFrame->pBuf + pFrame->len)) && (pItem->byElementID == WLAN_EID_CHALLENGE)) {
-        pFrame->pChallenge = (PWLAN_IE_CHALLENGE)pItem;
-    }
+	if ((((unsigned char *)pItem) < (pFrame->pBuf + pFrame->len)) && (pItem->byElementID == WLAN_EID_CHALLENGE)) {
+		pFrame->pChallenge = (PWLAN_IE_CHALLENGE)pItem;
+	}
 
-    return;
+	return;
 }
 
-
 /*+
  *
  * Routine Description:
@@ -908,24 +874,23 @@ vMgrDecodeAuthen(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 void
 vMgrEncodeDeauthen(
-    PWLAN_FR_DEAUTHEN  pFrame
-    )
+	PWLAN_FR_DEAUTHEN  pFrame
+)
 {
-    pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
+	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
 
-    // Fixed Fields
-    pFrame->pwReason = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                               + WLAN_DEAUTHEN_OFF_REASON);
-    pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_DEAUTHEN_OFF_REASON + sizeof(*(pFrame->pwReason));
+	// Fixed Fields
+	pFrame->pwReason = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					      + WLAN_DEAUTHEN_OFF_REASON);
+	pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_DEAUTHEN_OFF_REASON + sizeof(*(pFrame->pwReason));
 
-    return;
+	return;
 }
 
-
 /*+
  *
  * Routine Description:
@@ -935,23 +900,22 @@ vMgrEncodeDeauthen(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 void
 vMgrDecodeDeauthen(
-    PWLAN_FR_DEAUTHEN  pFrame
-    )
+	PWLAN_FR_DEAUTHEN  pFrame
+)
 {
-    pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
+	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
 
-    // Fixed Fields
-    pFrame->pwReason = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                               + WLAN_DEAUTHEN_OFF_REASON);
+	// Fixed Fields
+	pFrame->pwReason = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					      + WLAN_DEAUTHEN_OFF_REASON);
 
-    return;
+	return;
 }
 
-
 /*+
  *
  * Routine Description: (AP)
@@ -961,29 +925,28 @@ vMgrDecodeDeauthen(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 void
 vMgrEncodeReassocResponse(
-    PWLAN_FR_REASSOCRESP  pFrame
-     )
+	PWLAN_FR_REASSOCRESP  pFrame
+)
 {
-    pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
+	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
 
-    // Fixed Fields
-    pFrame->pwCapInfo = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                + WLAN_REASSOCRESP_OFF_CAP_INFO);
-    pFrame->pwStatus = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                               + WLAN_REASSOCRESP_OFF_STATUS);
-    pFrame->pwAid = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                            + WLAN_REASSOCRESP_OFF_AID);
+	// Fixed Fields
+	pFrame->pwCapInfo = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					       + WLAN_REASSOCRESP_OFF_CAP_INFO);
+	pFrame->pwStatus = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					      + WLAN_REASSOCRESP_OFF_STATUS);
+	pFrame->pwAid = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					   + WLAN_REASSOCRESP_OFF_AID);
 
-    pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_REASSOCRESP_OFF_AID + sizeof(*(pFrame->pwAid));
+	pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_REASSOCRESP_OFF_AID + sizeof(*(pFrame->pwAid));
 
-    return;
+	return;
 }
 
-
 /*+
  *
  * Routine Description:
@@ -993,36 +956,35 @@ vMgrEncodeReassocResponse(
  * Return Value:
  *    None.
  *
--*/
-
+ -*/
 
 void
 vMgrDecodeReassocResponse(
-    PWLAN_FR_REASSOCRESP  pFrame
-     )
+	PWLAN_FR_REASSOCRESP  pFrame
+)
 {
-    PWLAN_IE   pItem;
-
-    pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-    // Fixed Fields
-    pFrame->pwCapInfo = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                + WLAN_REASSOCRESP_OFF_CAP_INFO);
-    pFrame->pwStatus = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                               + WLAN_REASSOCRESP_OFF_STATUS);
-    pFrame->pwAid = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                            + WLAN_REASSOCRESP_OFF_AID);
-
-    //Information elements
-    pFrame->pSuppRates = (PWLAN_IE_SUPP_RATES)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                               + WLAN_REASSOCRESP_OFF_SUPP_RATES);
-
-    pItem = (PWLAN_IE)(pFrame->pSuppRates);
-    pItem = (PWLAN_IE)(((unsigned char *)pItem) + 2 + pItem->len);
-
-    if ((((unsigned char *)pItem) < (pFrame->pBuf + pFrame->len)) &&
-		    (pItem->byElementID == WLAN_EID_EXTSUPP_RATES)) {
-        pFrame->pExtSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
-    }
-    return;
+	PWLAN_IE   pItem;
+
+	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
+
+	// Fixed Fields
+	pFrame->pwCapInfo = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					       + WLAN_REASSOCRESP_OFF_CAP_INFO);
+	pFrame->pwStatus = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					      + WLAN_REASSOCRESP_OFF_STATUS);
+	pFrame->pwAid = (unsigned short *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+					   + WLAN_REASSOCRESP_OFF_AID);
+
+	//Information elements
+	pFrame->pSuppRates = (PWLAN_IE_SUPP_RATES)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+						   + WLAN_REASSOCRESP_OFF_SUPP_RATES);
+
+	pItem = (PWLAN_IE)(pFrame->pSuppRates);
+	pItem = (PWLAN_IE)(((unsigned char *)pItem) + 2 + pItem->len);
+
+	if ((((unsigned char *)pItem) < (pFrame->pBuf + pFrame->len)) &&
+	    (pItem->byElementID == WLAN_EID_EXTSUPP_RATES)) {
+		pFrame->pExtSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
+	}
+	return;
 }
diff --git a/drivers/staging/vt6655/80211mgr.h b/drivers/staging/vt6655/80211mgr.h
index 65780f28db4..16402cf5d25 100644
--- a/drivers/staging/vt6655/80211mgr.h
+++ b/drivers/staging/vt6655/80211mgr.h
@@ -65,7 +65,6 @@
 // reference WiFi WPA spec.
 #define WLAN_EID_RSN_WPA        221
 
-
 #define WLAN_EID_ERP_NONERP_PRESENT             0x01
 #define WLAN_EID_ERP_USE_PROTECTION             0x02
 #define WLAN_EID_ERP_BARKER_MODE                0x04
@@ -132,14 +131,10 @@
 #define WLAN_MGMT_STATUS_INVALID_RSN_IE_CAP             45
 #define WLAN_MGMT_STATUS_CIPHER_REJECT                  46
 
-
-
 // Auth Algorithm
 #define WLAN_AUTH_ALG_OPENSYSTEM                0
 #define WLAN_AUTH_ALG_SHAREDKEY                 1
 
-
-
 // Management Frame Field Offsets
 // Note: Not all fields are listed because of variable lengths.
 // Note: These offsets are from the start of the frame data
@@ -184,7 +179,6 @@
 
 #define WLAN_DEAUTHEN_OFF_REASON            0
 
-
 //
 // Cipher Suite Selectors defined in 802.11i
 //
@@ -217,611 +211,567 @@
 #define MEASURE_MODE_INCAPABLE  0x02
 #define MEASURE_MODE_REFUSED    0x04
 
-
-
 /*---------------------  Export Classes  ----------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
 
 /*---------------------  Export Types  ------------------------------*/
 
-
 // Information Element Types
 
 #pragma pack(1)
 typedef struct tagWLAN_IE {
-    unsigned char byElementID;
-    unsigned char len;
-}__attribute__ ((__packed__))
+	unsigned char byElementID;
+	unsigned char len;
+} __attribute__ ((__packed__))
 WLAN_IE, *PWLAN_IE;
 
-
 // Service Set Identity (SSID)
 #pragma pack(1)
 typedef struct tagWLAN_IE_SSID {
-    unsigned char byElementID;
-    unsigned char len;
-    unsigned char abySSID[1];
-}__attribute__ ((__packed__))
+	unsigned char byElementID;
+	unsigned char len;
+	unsigned char abySSID[1];
+} __attribute__ ((__packed__))
 WLAN_IE_SSID, *PWLAN_IE_SSID;
 
-
 // Supported Rates
 #pragma pack(1)
 typedef struct tagWLAN_IE_SUPP_RATES {
-    unsigned char byElementID;
-    unsigned char len;
-    unsigned char abyRates[1];
-}__attribute__ ((__packed__))
+	unsigned char byElementID;
+	unsigned char len;
+	unsigned char abyRates[1];
+} __attribute__ ((__packed__))
 WLAN_IE_SUPP_RATES,  *PWLAN_IE_SUPP_RATES;
 
-
-
 // FH Parameter Set
 #pragma pack(1)
 typedef struct _WLAN_IE_FH_PARMS {
-    unsigned char byElementID;
-    unsigned char len;
-    unsigned short wDwellTime;
-    unsigned char byHopSet;
-    unsigned char byHopPattern;
-    unsigned char byHopIndex;
+	unsigned char byElementID;
+	unsigned char len;
+	unsigned short wDwellTime;
+	unsigned char byHopSet;
+	unsigned char byHopPattern;
+	unsigned char byHopIndex;
 } WLAN_IE_FH_PARMS,  *PWLAN_IE_FH_PARMS;
 
 // DS Parameter Set
 #pragma pack(1)
 typedef struct tagWLAN_IE_DS_PARMS {
-    unsigned char byElementID;
-    unsigned char len;
-    unsigned char byCurrChannel;
-}__attribute__ ((__packed__))
+	unsigned char byElementID;
+	unsigned char len;
+	unsigned char byCurrChannel;
+} __attribute__ ((__packed__))
 WLAN_IE_DS_PARMS,  *PWLAN_IE_DS_PARMS;
 
-
 // CF Parameter Set
 #pragma pack(1)
 typedef struct tagWLAN_IE_CF_PARMS {
-    unsigned char byElementID;
-    unsigned char len;
-    unsigned char byCFPCount;
-    unsigned char byCFPPeriod;
-    unsigned short wCFPMaxDuration;
-    unsigned short wCFPDurRemaining;
-}__attribute__ ((__packed__))
+	unsigned char byElementID;
+	unsigned char len;
+	unsigned char byCFPCount;
+	unsigned char byCFPPeriod;
+	unsigned short wCFPMaxDuration;
+	unsigned short wCFPDurRemaining;
+} __attribute__ ((__packed__))
 WLAN_IE_CF_PARMS,  *PWLAN_IE_CF_PARMS;
 
-
 // TIM
 #pragma pack(1)
 typedef struct tagWLAN_IE_TIM {
-    unsigned char byElementID;
-    unsigned char len;
-    unsigned char byDTIMCount;
-    unsigned char byDTIMPeriod;
-    unsigned char byBitMapCtl;
-    unsigned char byVirtBitMap[1];
-}__attribute__ ((__packed__))
+	unsigned char byElementID;
+	unsigned char len;
+	unsigned char byDTIMCount;
+	unsigned char byDTIMPeriod;
+	unsigned char byBitMapCtl;
+	unsigned char byVirtBitMap[1];
+} __attribute__ ((__packed__))
 WLAN_IE_TIM,  *PWLAN_IE_TIM;
 
-
 // IBSS Parameter Set
 #pragma pack(1)
 typedef struct tagWLAN_IE_IBSS_PARMS {
-    unsigned char byElementID;
-    unsigned char len;
-    unsigned short wATIMWindow;
-}__attribute__ ((__packed__))
+	unsigned char byElementID;
+	unsigned char len;
+	unsigned short wATIMWindow;
+} __attribute__ ((__packed__))
 WLAN_IE_IBSS_PARMS, *PWLAN_IE_IBSS_PARMS;
 
-
 // Challenge Text
 #pragma pack(1)
 typedef struct tagWLAN_IE_CHALLENGE {
-    unsigned char byElementID;
-    unsigned char len;
-    unsigned char abyChallenge[1];
-}__attribute__ ((__packed__))
+	unsigned char byElementID;
+	unsigned char len;
+	unsigned char abyChallenge[1];
+} __attribute__ ((__packed__))
 WLAN_IE_CHALLENGE,  *PWLAN_IE_CHALLENGE;
 
-
 #pragma pack(1)
 typedef struct tagWLAN_IE_RSN_EXT {
-    unsigned char byElementID;
-    unsigned char len;
-    unsigned char abyOUI[4];
-    unsigned short wVersion;
-    unsigned char abyMulticast[4];
-    unsigned short wPKCount;
-    struct {
-        unsigned char abyOUI[4];
-    } PKSList[1]; // the rest is variable so need to
-    // overlay ieauth structure
+	unsigned char byElementID;
+	unsigned char len;
+	unsigned char abyOUI[4];
+	unsigned short wVersion;
+	unsigned char abyMulticast[4];
+	unsigned short wPKCount;
+	struct {
+		unsigned char abyOUI[4];
+	} PKSList[1]; // the rest is variable so need to
+	// overlay ieauth structure
 } WLAN_IE_RSN_EXT, *PWLAN_IE_RSN_EXT;
 
 #pragma pack(1)
 typedef struct tagWLAN_IE_RSN_AUTH {
-    unsigned short wAuthCount;
-    struct {
-        unsigned char abyOUI[4];
-    } AuthKSList[1];
+	unsigned short wAuthCount;
+	struct {
+		unsigned char abyOUI[4];
+	} AuthKSList[1];
 } WLAN_IE_RSN_AUTH, *PWLAN_IE_RSN_AUTH;
 
 // RSN Identity
 #pragma pack(1)
 typedef struct tagWLAN_IE_RSN {
-    unsigned char byElementID;
-    unsigned char len;
-    unsigned short wVersion;
-    unsigned char abyRSN[WLAN_MIN_ARRAY];
+	unsigned char byElementID;
+	unsigned char len;
+	unsigned short wVersion;
+	unsigned char abyRSN[WLAN_MIN_ARRAY];
 } WLAN_IE_RSN, *PWLAN_IE_RSN;
 
-
 // ERP
 #pragma pack(1)
 typedef struct tagWLAN_IE_ERP {
-    unsigned char byElementID;
-    unsigned char len;
-    unsigned char byContext;
-}__attribute__ ((__packed__))
+	unsigned char byElementID;
+	unsigned char len;
+	unsigned char byContext;
+} __attribute__ ((__packed__))
 WLAN_IE_ERP,  *PWLAN_IE_ERP;
 
-
 #pragma pack(1)
 typedef struct _MEASEURE_REQ {
-    unsigned char byChannel;
-    unsigned char abyStartTime[8];
-    unsigned char abyDuration[2];
+	unsigned char byChannel;
+	unsigned char abyStartTime[8];
+	unsigned char abyDuration[2];
 } MEASEURE_REQ, *PMEASEURE_REQ,
-  MEASEURE_REQ_BASIC, *PMEASEURE_REQ_BASIC,
-  MEASEURE_REQ_CCA, *PMEASEURE_REQ_CCA,
-  MEASEURE_REQ_RPI, *PMEASEURE_REQ_RPI;
+	MEASEURE_REQ_BASIC, *PMEASEURE_REQ_BASIC,
+	MEASEURE_REQ_CCA, *PMEASEURE_REQ_CCA,
+	MEASEURE_REQ_RPI, *PMEASEURE_REQ_RPI;
 
 typedef struct _MEASEURE_REP_BASIC {
-    unsigned char byChannel;
-    unsigned char abyStartTime[8];
-    unsigned char abyDuration[2];
-    unsigned char byMap;
+	unsigned char byChannel;
+	unsigned char abyStartTime[8];
+	unsigned char abyDuration[2];
+	unsigned char byMap;
 } MEASEURE_REP_BASIC, *PMEASEURE_REP_BASIC;
 
 typedef struct _MEASEURE_REP_CCA {
-    unsigned char byChannel;
-    unsigned char abyStartTime[8];
-    unsigned char abyDuration[2];
-    unsigned char byCCABusyFraction;
+	unsigned char byChannel;
+	unsigned char abyStartTime[8];
+	unsigned char abyDuration[2];
+	unsigned char byCCABusyFraction;
 } MEASEURE_REP_CCA, *PMEASEURE_REP_CCA;
 
 typedef struct _MEASEURE_REP_RPI {
-    unsigned char byChannel;
-    unsigned char abyStartTime[8];
-    unsigned char abyDuration[2];
-    unsigned char abyRPIdensity[8];
+	unsigned char byChannel;
+	unsigned char abyStartTime[8];
+	unsigned char abyDuration[2];
+	unsigned char abyRPIdensity[8];
 } MEASEURE_REP_RPI, *PMEASEURE_REP_RPI;
 
 typedef union _MEASEURE_REP {
-
-    MEASEURE_REP_BASIC  sBasic;
-    MEASEURE_REP_CCA    sCCA;
-    MEASEURE_REP_RPI    sRPI;
-
+	MEASEURE_REP_BASIC  sBasic;
+	MEASEURE_REP_CCA    sCCA;
+	MEASEURE_REP_RPI    sRPI;
 } MEASEURE_REP, *PMEASEURE_REP;
 
 typedef struct _WLAN_IE_MEASURE_REQ {
-    unsigned char byElementID;
-    unsigned char len;
-    unsigned char byToken;
-    unsigned char byMode;
-    unsigned char byType;
-    MEASEURE_REQ        sReq;
+	unsigned char byElementID;
+	unsigned char len;
+	unsigned char byToken;
+	unsigned char byMode;
+	unsigned char byType;
+	MEASEURE_REQ        sReq;
 } WLAN_IE_MEASURE_REQ, *PWLAN_IE_MEASURE_REQ;
 
 typedef struct _WLAN_IE_MEASURE_REP {
-    unsigned char byElementID;
-    unsigned char len;
-    unsigned char byToken;
-    unsigned char byMode;
-    unsigned char byType;
-    MEASEURE_REP        sRep;
+	unsigned char byElementID;
+	unsigned char len;
+	unsigned char byToken;
+	unsigned char byMode;
+	unsigned char byType;
+	MEASEURE_REP        sRep;
 } WLAN_IE_MEASURE_REP, *PWLAN_IE_MEASURE_REP;
 
 typedef struct _WLAN_IE_CH_SW {
-    unsigned char byElementID;
-    unsigned char len;
-    unsigned char byMode;
-    unsigned char byChannel;
-    unsigned char byCount;
+	unsigned char byElementID;
+	unsigned char len;
+	unsigned char byMode;
+	unsigned char byChannel;
+	unsigned char byCount;
 } WLAN_IE_CH_SW, *PWLAN_IE_CH_SW;
 
 typedef struct _WLAN_IE_QUIET {
-    unsigned char byElementID;
-    unsigned char len;
-    unsigned char byQuietCount;
-    unsigned char byQuietPeriod;
-    unsigned char abyQuietDuration[2];
-    unsigned char abyQuietOffset[2];
+	unsigned char byElementID;
+	unsigned char len;
+	unsigned char byQuietCount;
+	unsigned char byQuietPeriod;
+	unsigned char abyQuietDuration[2];
+	unsigned char abyQuietOffset[2];
 } WLAN_IE_QUIET, *PWLAN_IE_QUIET;
 
 typedef struct _WLAN_IE_COUNTRY {
-    unsigned char byElementID;
-    unsigned char len;
-    unsigned char abyCountryString[3];
-    unsigned char abyCountryInfo[3];
+	unsigned char byElementID;
+	unsigned char len;
+	unsigned char abyCountryString[3];
+	unsigned char abyCountryInfo[3];
 } WLAN_IE_COUNTRY, *PWLAN_IE_COUNTRY;
 
 typedef struct _WLAN_IE_PW_CONST {
-    unsigned char byElementID;
-    unsigned char len;
-    unsigned char byPower;
+	unsigned char byElementID;
+	unsigned char len;
+	unsigned char byPower;
 } WLAN_IE_PW_CONST, *PWLAN_IE_PW_CONST;
 
 typedef struct _WLAN_IE_PW_CAP {
-    unsigned char byElementID;
-    unsigned char len;
-    unsigned char byMinPower;
-    unsigned char byMaxPower;
+	unsigned char byElementID;
+	unsigned char len;
+	unsigned char byMinPower;
+	unsigned char byMaxPower;
 } WLAN_IE_PW_CAP, *PWLAN_IE_PW_CAP;
 
 typedef struct _WLAN_IE_SUPP_CH {
-    unsigned char byElementID;
-    unsigned char len;
-    unsigned char abyChannelTuple[2];
+	unsigned char byElementID;
+	unsigned char len;
+	unsigned char abyChannelTuple[2];
 } WLAN_IE_SUPP_CH, *PWLAN_IE_SUPP_CH;
 
 typedef struct _WLAN_IE_TPC_REQ {
-    unsigned char byElementID;
-    unsigned char len;
+	unsigned char byElementID;
+	unsigned char len;
 } WLAN_IE_TPC_REQ, *PWLAN_IE_TPC_REQ;
 
 typedef struct _WLAN_IE_TPC_REP {
-    unsigned char byElementID;
-    unsigned char len;
-    unsigned char byTxPower;
-    unsigned char byLinkMargin;
+	unsigned char byElementID;
+	unsigned char len;
+	unsigned char byTxPower;
+	unsigned char byLinkMargin;
 } WLAN_IE_TPC_REP, *PWLAN_IE_TPC_REP;
 
-
 typedef struct _WLAN_IE_IBSS_DFS {
-    unsigned char byElementID;
-    unsigned char len;
-    unsigned char abyDFSOwner[6];
-    unsigned char byDFSRecovery;
-    unsigned char abyChannelMap[2];
+	unsigned char byElementID;
+	unsigned char len;
+	unsigned char abyDFSOwner[6];
+	unsigned char byDFSRecovery;
+	unsigned char abyChannelMap[2];
 } WLAN_IE_IBSS_DFS, *PWLAN_IE_IBSS_DFS;
 
 #pragma pack()
 
-
-
 // Frame Types
 // prototype structure, all mgmt frame types will start with these members
 typedef struct tagWLAN_FR_MGMT {
-
-    unsigned int	uType;
-    unsigned int	len;
-    unsigned char *pBuf;
-    PUWLAN_80211HDR       pHdr;
-
+	unsigned int	uType;
+	unsigned int	len;
+	unsigned char *pBuf;
+	PUWLAN_80211HDR       pHdr;
 } WLAN_FR_MGMT,  *PWLAN_FR_MGMT;
 
 // Beacon frame
 typedef struct tagWLAN_FR_BEACON {
-
-    unsigned int	uType;
-    unsigned int	len;
-    unsigned char *pBuf;
-    PUWLAN_80211HDR         pHdr;
-    // fixed fields
-    PQWORD                  pqwTimestamp;
-    unsigned short *pwBeaconInterval;
-    unsigned short *pwCapInfo;
-    /*-- info elements ----------*/
-    PWLAN_IE_SSID           pSSID;
-    PWLAN_IE_SUPP_RATES     pSuppRates;
+	unsigned int	uType;
+	unsigned int	len;
+	unsigned char *pBuf;
+	PUWLAN_80211HDR         pHdr;
+	// fixed fields
+	PQWORD                  pqwTimestamp;
+	unsigned short *pwBeaconInterval;
+	unsigned short *pwCapInfo;
+	/*-- info elements ----------*/
+	PWLAN_IE_SSID           pSSID;
+	PWLAN_IE_SUPP_RATES     pSuppRates;
 //  PWLAN_IE_FH_PARMS       pFHParms;
-    PWLAN_IE_DS_PARMS       pDSParms;
-    PWLAN_IE_CF_PARMS       pCFParms;
-    PWLAN_IE_TIM            pTIM;
-    PWLAN_IE_IBSS_PARMS     pIBSSParms;
-    PWLAN_IE_RSN            pRSN;
-    PWLAN_IE_RSN_EXT        pRSNWPA;
-    PWLAN_IE_ERP            pERP;
-    PWLAN_IE_SUPP_RATES     pExtSuppRates;
-    PWLAN_IE_COUNTRY        pIE_Country;
-    PWLAN_IE_PW_CONST       pIE_PowerConstraint;
-    PWLAN_IE_CH_SW          pIE_CHSW;
-    PWLAN_IE_IBSS_DFS       pIE_IBSSDFS;
-    PWLAN_IE_QUIET          pIE_Quiet;
-
+	PWLAN_IE_DS_PARMS       pDSParms;
+	PWLAN_IE_CF_PARMS       pCFParms;
+	PWLAN_IE_TIM            pTIM;
+	PWLAN_IE_IBSS_PARMS     pIBSSParms;
+	PWLAN_IE_RSN            pRSN;
+	PWLAN_IE_RSN_EXT        pRSNWPA;
+	PWLAN_IE_ERP            pERP;
+	PWLAN_IE_SUPP_RATES     pExtSuppRates;
+	PWLAN_IE_COUNTRY        pIE_Country;
+	PWLAN_IE_PW_CONST       pIE_PowerConstraint;
+	PWLAN_IE_CH_SW          pIE_CHSW;
+	PWLAN_IE_IBSS_DFS       pIE_IBSSDFS;
+	PWLAN_IE_QUIET          pIE_Quiet;
 } WLAN_FR_BEACON, *PWLAN_FR_BEACON;
 
-
 // IBSS ATIM frame
 typedef struct tagWLAN_FR_IBSSATIM {
-
-    unsigned int	uType;
-    unsigned int	len;
-    unsigned char *pBuf;
-    PUWLAN_80211HDR         pHdr;
-
-    // fixed fields
-    // info elements
-    // this frame type has a null body
-
+	unsigned int	uType;
+	unsigned int	len;
+	unsigned char *pBuf;
+	PUWLAN_80211HDR         pHdr;
+
+	// fixed fields
+	// info elements
+	// this frame type has a null body
 } WLAN_FR_IBSSATIM, *PWLAN_FR_IBSSATIM;
 
 // Disassociation
 typedef struct tagWLAN_FR_DISASSOC {
-
-    unsigned int	uType;
-    unsigned int	len;
-    unsigned char *pBuf;
-    PUWLAN_80211HDR         pHdr;
-    /*-- fixed fields -----------*/
-    unsigned short *pwReason;
-    /*-- info elements ----------*/
-
+	unsigned int	uType;
+	unsigned int	len;
+	unsigned char *pBuf;
+	PUWLAN_80211HDR         pHdr;
+	/*-- fixed fields -----------*/
+	unsigned short *pwReason;
+	/*-- info elements ----------*/
 } WLAN_FR_DISASSOC, *PWLAN_FR_DISASSOC;
 
 // Association Request
 typedef struct tagWLAN_FR_ASSOCREQ {
-
-    unsigned int	uType;
-    unsigned int	len;
-    unsigned char *pBuf;
-    PUWLAN_80211HDR         pHdr;
-    /*-- fixed fields -----------*/
-    unsigned short *pwCapInfo;
-    unsigned short *pwListenInterval;
-    /*-- info elements ----------*/
-    PWLAN_IE_SSID           pSSID;
-    PWLAN_IE_SUPP_RATES     pSuppRates;
-    PWLAN_IE_RSN            pRSN;
-    PWLAN_IE_RSN_EXT        pRSNWPA;
-    PWLAN_IE_SUPP_RATES     pExtSuppRates;
-    PWLAN_IE_PW_CAP         pCurrPowerCap;
-    PWLAN_IE_SUPP_CH        pCurrSuppCh;
-
+	unsigned int	uType;
+	unsigned int	len;
+	unsigned char *pBuf;
+	PUWLAN_80211HDR         pHdr;
+	/*-- fixed fields -----------*/
+	unsigned short *pwCapInfo;
+	unsigned short *pwListenInterval;
+	/*-- info elements ----------*/
+	PWLAN_IE_SSID           pSSID;
+	PWLAN_IE_SUPP_RATES     pSuppRates;
+	PWLAN_IE_RSN            pRSN;
+	PWLAN_IE_RSN_EXT        pRSNWPA;
+	PWLAN_IE_SUPP_RATES     pExtSuppRates;
+	PWLAN_IE_PW_CAP         pCurrPowerCap;
+	PWLAN_IE_SUPP_CH        pCurrSuppCh;
 } WLAN_FR_ASSOCREQ, *PWLAN_FR_ASSOCREQ;
 
 // Association Response
 typedef struct tagWLAN_FR_ASSOCRESP {
-
-    unsigned int	uType;
-    unsigned int	len;
-    unsigned char *pBuf;
-    PUWLAN_80211HDR         pHdr;
-    /*-- fixed fields -----------*/
-    unsigned short *pwCapInfo;
-    unsigned short *pwStatus;
-    unsigned short *pwAid;
-    /*-- info elements ----------*/
-    PWLAN_IE_SUPP_RATES     pSuppRates;
-    PWLAN_IE_SUPP_RATES     pExtSuppRates;
-
+	unsigned int	uType;
+	unsigned int	len;
+	unsigned char *pBuf;
+	PUWLAN_80211HDR         pHdr;
+	/*-- fixed fields -----------*/
+	unsigned short *pwCapInfo;
+	unsigned short *pwStatus;
+	unsigned short *pwAid;
+	/*-- info elements ----------*/
+	PWLAN_IE_SUPP_RATES     pSuppRates;
+	PWLAN_IE_SUPP_RATES     pExtSuppRates;
 } WLAN_FR_ASSOCRESP, *PWLAN_FR_ASSOCRESP;
 
 // Reassociation Request
 typedef struct tagWLAN_FR_REASSOCREQ {
-
-    unsigned int	uType;
-    unsigned int	len;
-    unsigned char *pBuf;
-    PUWLAN_80211HDR         pHdr;
-
-    /*-- fixed fields -----------*/
-    unsigned short *pwCapInfo;
-    unsigned short *pwListenInterval;
-    PIEEE_ADDR              pAddrCurrAP;
-
-    /*-- info elements ----------*/
-    PWLAN_IE_SSID           pSSID;
-    PWLAN_IE_SUPP_RATES     pSuppRates;
-    PWLAN_IE_RSN            pRSN;
-    PWLAN_IE_RSN_EXT        pRSNWPA;
-    PWLAN_IE_SUPP_RATES     pExtSuppRates;
-
+	unsigned int	uType;
+	unsigned int	len;
+	unsigned char *pBuf;
+	PUWLAN_80211HDR         pHdr;
+
+	/*-- fixed fields -----------*/
+	unsigned short *pwCapInfo;
+	unsigned short *pwListenInterval;
+	PIEEE_ADDR              pAddrCurrAP;
+
+	/*-- info elements ----------*/
+	PWLAN_IE_SSID           pSSID;
+	PWLAN_IE_SUPP_RATES     pSuppRates;
+	PWLAN_IE_RSN            pRSN;
+	PWLAN_IE_RSN_EXT        pRSNWPA;
+	PWLAN_IE_SUPP_RATES     pExtSuppRates;
 } WLAN_FR_REASSOCREQ, *PWLAN_FR_REASSOCREQ;
 
 // Reassociation Response
 typedef struct tagWLAN_FR_REASSOCRESP {
-
-    unsigned int	uType;
-    unsigned int	len;
-    unsigned char *pBuf;
-    PUWLAN_80211HDR         pHdr;
-    /*-- fixed fields -----------*/
-    unsigned short *pwCapInfo;
-    unsigned short *pwStatus;
-    unsigned short *pwAid;
-    /*-- info elements ----------*/
-    PWLAN_IE_SUPP_RATES     pSuppRates;
-    PWLAN_IE_SUPP_RATES     pExtSuppRates;
-
+	unsigned int	uType;
+	unsigned int	len;
+	unsigned char *pBuf;
+	PUWLAN_80211HDR         pHdr;
+	/*-- fixed fields -----------*/
+	unsigned short *pwCapInfo;
+	unsigned short *pwStatus;
+	unsigned short *pwAid;
+	/*-- info elements ----------*/
+	PWLAN_IE_SUPP_RATES     pSuppRates;
+	PWLAN_IE_SUPP_RATES     pExtSuppRates;
 } WLAN_FR_REASSOCRESP, *PWLAN_FR_REASSOCRESP;
 
 // Probe Request
 typedef struct tagWLAN_FR_PROBEREQ {
-
-    unsigned int	uType;
-    unsigned int	len;
-    unsigned char *pBuf;
-    PUWLAN_80211HDR         pHdr;
-    /*-- fixed fields -----------*/
-    /*-- info elements ----------*/
-    PWLAN_IE_SSID           pSSID;
-    PWLAN_IE_SUPP_RATES     pSuppRates;
-    PWLAN_IE_SUPP_RATES     pExtSuppRates;
-
+	unsigned int	uType;
+	unsigned int	len;
+	unsigned char *pBuf;
+	PUWLAN_80211HDR         pHdr;
+	/*-- fixed fields -----------*/
+	/*-- info elements ----------*/
+	PWLAN_IE_SSID           pSSID;
+	PWLAN_IE_SUPP_RATES     pSuppRates;
+	PWLAN_IE_SUPP_RATES     pExtSuppRates;
 } WLAN_FR_PROBEREQ, *PWLAN_FR_PROBEREQ;
 
 // Probe Response
 typedef struct tagWLAN_FR_PROBERESP {
-
-    unsigned int	uType;
-    unsigned int	len;
-    unsigned char *pBuf;
-    PUWLAN_80211HDR         pHdr;
-    /*-- fixed fields -----------*/
-    PQWORD                  pqwTimestamp;
-    unsigned short *pwBeaconInterval;
-    unsigned short *pwCapInfo;
-    /*-- info elements ----------*/
-    PWLAN_IE_SSID           pSSID;
-    PWLAN_IE_SUPP_RATES     pSuppRates;
-    PWLAN_IE_DS_PARMS       pDSParms;
-    PWLAN_IE_CF_PARMS       pCFParms;
-    PWLAN_IE_IBSS_PARMS     pIBSSParms;
-    PWLAN_IE_RSN            pRSN;
-    PWLAN_IE_RSN_EXT        pRSNWPA;
-    PWLAN_IE_ERP            pERP;
-    PWLAN_IE_SUPP_RATES     pExtSuppRates;
-    PWLAN_IE_COUNTRY        pIE_Country;
-    PWLAN_IE_PW_CONST       pIE_PowerConstraint;
-    PWLAN_IE_CH_SW          pIE_CHSW;
-    PWLAN_IE_IBSS_DFS       pIE_IBSSDFS;
-    PWLAN_IE_QUIET          pIE_Quiet;
-
+	unsigned int	uType;
+	unsigned int	len;
+	unsigned char *pBuf;
+	PUWLAN_80211HDR         pHdr;
+	/*-- fixed fields -----------*/
+	PQWORD                  pqwTimestamp;
+	unsigned short *pwBeaconInterval;
+	unsigned short *pwCapInfo;
+	/*-- info elements ----------*/
+	PWLAN_IE_SSID           pSSID;
+	PWLAN_IE_SUPP_RATES     pSuppRates;
+	PWLAN_IE_DS_PARMS       pDSParms;
+	PWLAN_IE_CF_PARMS       pCFParms;
+	PWLAN_IE_IBSS_PARMS     pIBSSParms;
+	PWLAN_IE_RSN            pRSN;
+	PWLAN_IE_RSN_EXT        pRSNWPA;
+	PWLAN_IE_ERP            pERP;
+	PWLAN_IE_SUPP_RATES     pExtSuppRates;
+	PWLAN_IE_COUNTRY        pIE_Country;
+	PWLAN_IE_PW_CONST       pIE_PowerConstraint;
+	PWLAN_IE_CH_SW          pIE_CHSW;
+	PWLAN_IE_IBSS_DFS       pIE_IBSSDFS;
+	PWLAN_IE_QUIET          pIE_Quiet;
 } WLAN_FR_PROBERESP, *PWLAN_FR_PROBERESP;
 
 // Authentication
 typedef struct tagWLAN_FR_AUTHEN {
-
-    unsigned int	uType;
-    unsigned int	len;
-    unsigned char *pBuf;
-    PUWLAN_80211HDR         pHdr;
-    /*-- fixed fields -----------*/
-    unsigned short *pwAuthAlgorithm;
-    unsigned short *pwAuthSequence;
-    unsigned short *pwStatus;
-    /*-- info elements ----------*/
-    PWLAN_IE_CHALLENGE      pChallenge;
-
+	unsigned int	uType;
+	unsigned int	len;
+	unsigned char *pBuf;
+	PUWLAN_80211HDR         pHdr;
+	/*-- fixed fields -----------*/
+	unsigned short *pwAuthAlgorithm;
+	unsigned short *pwAuthSequence;
+	unsigned short *pwStatus;
+	/*-- info elements ----------*/
+	PWLAN_IE_CHALLENGE      pChallenge;
 } WLAN_FR_AUTHEN, *PWLAN_FR_AUTHEN;
 
 // Deauthenication
 typedef struct tagWLAN_FR_DEAUTHEN {
-
-    unsigned int	uType;
-    unsigned int	len;
-    unsigned char *pBuf;
-    PUWLAN_80211HDR         pHdr;
-    /*-- fixed fields -----------*/
-    unsigned short *pwReason;
-
-    /*-- info elements ----------*/
-
+	unsigned int	uType;
+	unsigned int	len;
+	unsigned char *pBuf;
+	PUWLAN_80211HDR         pHdr;
+	/*-- fixed fields -----------*/
+	unsigned short *pwReason;
+
+	/*-- info elements ----------*/
 } WLAN_FR_DEAUTHEN, *PWLAN_FR_DEAUTHEN;
 
 /*---------------------  Export Functions  --------------------------*/
 
 void
 vMgrEncodeBeacon(
-    PWLAN_FR_BEACON  pFrame
-     );
+	PWLAN_FR_BEACON  pFrame
+);
 
 void
 vMgrDecodeBeacon(
-    PWLAN_FR_BEACON  pFrame
-    );
+	PWLAN_FR_BEACON  pFrame
+);
 
 void
 vMgrEncodeIBSSATIM(
-    PWLAN_FR_IBSSATIM   pFrame
-    );
+	PWLAN_FR_IBSSATIM   pFrame
+);
 
 void
 vMgrDecodeIBSSATIM(
-    PWLAN_FR_IBSSATIM   pFrame
-    );
+	PWLAN_FR_IBSSATIM   pFrame
+);
 
 void
 vMgrEncodeDisassociation(
-    PWLAN_FR_DISASSOC  pFrame
-    );
+	PWLAN_FR_DISASSOC  pFrame
+);
 
 void
 vMgrDecodeDisassociation(
-    PWLAN_FR_DISASSOC  pFrame
-    );
+	PWLAN_FR_DISASSOC  pFrame
+);
 
 void
 vMgrEncodeAssocRequest(
-    PWLAN_FR_ASSOCREQ  pFrame
-    );
+	PWLAN_FR_ASSOCREQ  pFrame
+);
 
 void
 vMgrDecodeAssocRequest(
-    PWLAN_FR_ASSOCREQ  pFrame
-    );
+	PWLAN_FR_ASSOCREQ  pFrame
+);
 
 void
 vMgrEncodeAssocResponse(
-    PWLAN_FR_ASSOCRESP  pFrame
-    );
+	PWLAN_FR_ASSOCRESP  pFrame
+);
 
 void
 vMgrDecodeAssocResponse(
-    PWLAN_FR_ASSOCRESP  pFrame
-    );
+	PWLAN_FR_ASSOCRESP  pFrame
+);
 
 void
 vMgrEncodeReassocRequest(
-    PWLAN_FR_REASSOCREQ  pFrame
-    );
+	PWLAN_FR_REASSOCREQ  pFrame
+);
 
 void
 vMgrDecodeReassocRequest(
-    PWLAN_FR_REASSOCREQ  pFrame
-    );
+	PWLAN_FR_REASSOCREQ  pFrame
+);
 
 void
 vMgrEncodeProbeRequest(
-    PWLAN_FR_PROBEREQ  pFrame
-    );
+	PWLAN_FR_PROBEREQ  pFrame
+);
 
 void
 vMgrDecodeProbeRequest(
-    PWLAN_FR_PROBEREQ  pFrame
-    );
+	PWLAN_FR_PROBEREQ  pFrame
+);
 
 void
 vMgrEncodeProbeResponse(
-    PWLAN_FR_PROBERESP  pFrame
-    );
+	PWLAN_FR_PROBERESP  pFrame
+);
 
 void
 vMgrDecodeProbeResponse(
-    PWLAN_FR_PROBERESP  pFrame
-    );
+	PWLAN_FR_PROBERESP  pFrame
+);
 
 void
 vMgrEncodeAuthen(
-    PWLAN_FR_AUTHEN  pFrame
-    );
+	PWLAN_FR_AUTHEN  pFrame
+);
 
 void
 vMgrDecodeAuthen(
-    PWLAN_FR_AUTHEN  pFrame
-    );
+	PWLAN_FR_AUTHEN  pFrame
+);
 
 void
 vMgrEncodeDeauthen(
-    PWLAN_FR_DEAUTHEN  pFrame
-    );
+	PWLAN_FR_DEAUTHEN  pFrame
+);
 
 void
 vMgrDecodeDeauthen(
-    PWLAN_FR_DEAUTHEN  pFrame
-    );
+	PWLAN_FR_DEAUTHEN  pFrame
+);
 
 void
 vMgrEncodeReassocResponse(
-    PWLAN_FR_REASSOCRESP  pFrame
-    );
+	PWLAN_FR_REASSOCRESP  pFrame
+);
 
 void
 vMgrDecodeReassocResponse(
-    PWLAN_FR_REASSOCRESP  pFrame
-    );
+	PWLAN_FR_REASSOCRESP  pFrame
+);
 
 #endif// __80211MGR_H__
diff --git a/drivers/staging/vt6655/IEEE11h.c b/drivers/staging/vt6655/IEEE11h.c
index cf7364d6526..dfda3c82906 100644
--- a/drivers/staging/vt6655/IEEE11h.c
+++ b/drivers/staging/vt6655/IEEE11h.c
@@ -99,7 +99,7 @@ typedef struct _WLAN_FRAME_TPCREP {
 
 /*---------------------  Static Functions  --------------------------*/
 static bool s_bRxMSRReq(PSMgmtObject pMgmt, PWLAN_FRAME_MSRREQ pMSRReq,
-		unsigned int uLength)
+			unsigned int uLength)
 {
 	size_t    uNumOfEIDs = 0;
 	bool bResult = true;
@@ -107,20 +107,19 @@ static bool s_bRxMSRReq(PSMgmtObject pMgmt, PWLAN_FRAME_MSRREQ pMSRReq,
 	if (uLength <= WLAN_A3FR_MAXLEN)
 		memcpy(pMgmt->abyCurrentMSRReq, pMSRReq, uLength);
 	uNumOfEIDs = ((uLength - offsetof(WLAN_FRAME_MSRREQ,
-			sMSRReqEIDs))/
-			(sizeof(WLAN_IE_MEASURE_REQ)));
+					  sMSRReqEIDs))/
+		      (sizeof(WLAN_IE_MEASURE_REQ)));
 	pMgmt->pCurrMeasureEIDRep = &(((PWLAN_FRAME_MSRREP)
-			(pMgmt->abyCurrentMSRRep))->sMSRRepEIDs[0]);
+				       (pMgmt->abyCurrentMSRRep))->sMSRRepEIDs[0]);
 	pMgmt->uLengthOfRepEIDs = 0;
 	bResult = CARDbStartMeasure(pMgmt->pAdapter,
-				((PWLAN_FRAME_MSRREQ)
-				(pMgmt->abyCurrentMSRReq))->sMSRReqEIDs,
-				uNumOfEIDs
-				);
+				    ((PWLAN_FRAME_MSRREQ)
+				     (pMgmt->abyCurrentMSRReq))->sMSRReqEIDs,
+				    uNumOfEIDs
+);
 	return bResult;
 }
 
-
 static bool s_bRxTPCReq(PSMgmtObject pMgmt,
 			PWLAN_FRAME_TPCREQ pTPCReq,
 			unsigned char byRate,
@@ -132,29 +131,29 @@ static bool s_bRxTPCReq(PSMgmtObject pMgmt,
 	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
 	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_A3FR_MAXLEN);
 	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket +
-sizeof(STxMgmtPacket));
+						    sizeof(STxMgmtPacket));
 
 	pFrame = (PWLAN_FRAME_TPCREP)((unsigned char *)pTxPacket +
-sizeof(STxMgmtPacket));
+				      sizeof(STxMgmtPacket));
 
 	pFrame->Header.wFrameCtl = (WLAN_SET_FC_FTYPE(WLAN_FTYPE_MGMT) |
-				WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_ACTION)
-				);
+				    WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_ACTION)
+);
 
 	memcpy(pFrame->Header.abyAddr1,
-		pTPCReq->Header.abyAddr2,
-		WLAN_ADDR_LEN);
+	       pTPCReq->Header.abyAddr2,
+	       WLAN_ADDR_LEN);
 	memcpy(pFrame->Header.abyAddr2,
-		CARDpGetCurrentAddress(pMgmt->pAdapter),
-		WLAN_ADDR_LEN);
+	       CARDpGetCurrentAddress(pMgmt->pAdapter),
+	       WLAN_ADDR_LEN);
 	memcpy(pFrame->Header.abyAddr3,
-		pMgmt->abyCurrBSSID,
-		WLAN_BSSID_LEN);
+	       pMgmt->abyCurrBSSID,
+	       WLAN_BSSID_LEN);
 
 	pFrame->byCategory = 0;
 	pFrame->byAction = 3;
 	pFrame->byDialogToken = ((PWLAN_FRAME_MSRREQ)
-(pMgmt->abyCurrentMSRReq))->byDialogToken;
+				 (pMgmt->abyCurrentMSRReq))->byDialogToken;
 
 	pFrame->sTPCRepEIDs.byElementID = WLAN_EID_TPC_REP;
 	pFrame->sTPCRepEIDs.len = 2;
@@ -185,25 +184,22 @@ sizeof(STxMgmtPacket));
 	default:
 		pFrame->sTPCRepEIDs.byLinkMargin = 82 - byRSSI;
 		break;
-}
+	}
 
 	pTxPacket->cbMPDULen = sizeof(WLAN_FRAME_TPCREP);
 	pTxPacket->cbPayloadLen = sizeof(WLAN_FRAME_TPCREP) -
-WLAN_HDR_ADDR3_LEN;
+		WLAN_HDR_ADDR3_LEN;
 	if (csMgmt_xmit(pMgmt->pAdapter, pTxPacket) != CMD_STATUS_PENDING)
 		return false;
 	return true;
-/*    return (CARDbSendPacket(pMgmt->pAdapter, pFrame, PKT_TYPE_802_11_MNG,
-sizeof(WLAN_FRAME_TPCREP))); */
-
+/*    return CARDbSendPacket(pMgmt->pAdapter, pFrame, PKT_TYPE_802_11_MNG,
+      sizeof(WLAN_FRAME_TPCREP)); */
 }
 
-
 /*---------------------  Export Variables  --------------------------*/
 
 /*---------------------  Export Functions  --------------------------*/
 
-
 /*+
  *
  * Description:
@@ -218,7 +214,7 @@ sizeof(WLAN_FRAME_TPCREP))); */
  *
  * Return Value: None.
  *
--*/
+ -*/
 bool
 IEEE11hbMgrRxAction(void *pMgmtHandle, void *pRxPacket)
 {
@@ -233,86 +229,85 @@ IEEE11hbMgrRxAction(void *pMgmtHandle, void *pRxPacket)
 		return false;
 
 	pAction = (PWLAN_FRAME_ACTION)
-(((PSRxMgmtPacket)pRxPacket)->p80211Header);
+		(((PSRxMgmtPacket)pRxPacket)->p80211Header);
 
 	if (pAction->byCategory == 0) {
 		switch (pAction->byAction) {
 		case ACTION_MSRREQ:
 			return s_bRxMSRReq(pMgmt,
-					(PWLAN_FRAME_MSRREQ)
-					pAction,
-					uLength);
+					   (PWLAN_FRAME_MSRREQ)
+					   pAction,
+					   uLength);
 			break;
 		case ACTION_MSRREP:
 			break;
 		case ACTION_TPCREQ:
 			return s_bRxTPCReq(pMgmt,
-				(PWLAN_FRAME_TPCREQ) pAction,
-				((PSRxMgmtPacket)pRxPacket)->byRxRate,
-				(unsigned char)
-				((PSRxMgmtPacket)pRxPacket)->uRSSI);
+					   (PWLAN_FRAME_TPCREQ) pAction,
+					   ((PSRxMgmtPacket)pRxPacket)->byRxRate,
+					   (unsigned char)
+					   ((PSRxMgmtPacket)pRxPacket)->uRSSI);
 			break;
 		case ACTION_TPCREP:
 			break;
 		case ACTION_CHSW:
 			pChannelSwitch = (PWLAN_IE_CH_SW) (pAction->abyVars);
 			if ((pChannelSwitch->byElementID == WLAN_EID_CH_SWITCH)
-			&& (pChannelSwitch->len == 3)) {
+			    && (pChannelSwitch->len == 3)) {
 				/* valid element id */
 				CARDbChannelSwitch(pMgmt->pAdapter,
-				pChannelSwitch->byMode,
-				get_channel_mapping(pMgmt->pAdapter,
-				pChannelSwitch->byChannel,
-				pMgmt->eCurrentPHYMode),
-				pChannelSwitch->byCount);
+						   pChannelSwitch->byMode,
+						   get_channel_mapping(pMgmt->pAdapter,
+								       pChannelSwitch->byChannel,
+								       pMgmt->eCurrentPHYMode),
+						   pChannelSwitch->byCount);
 			}
 			break;
 		default:
 			DBG_PRT(MSG_LEVEL_DEBUG,
-				KERN_INFO"Unknown Action = %d\n",
+				KERN_INFO "Unknown Action = %d\n",
 				pAction->byAction);
 			break;
 		}
 	} else {
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Unknown Category = %d\n",
-pAction->byCategory);
-	pAction->byCategory |= 0x80;
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Unknown Category = %d\n",
+			pAction->byCategory);
+		pAction->byCategory |= 0x80;
 
-	/*return (CARDbSendPacket(pMgmt->pAdapter, pAction, PKT_TYPE_802_11_MNG,
-uLength));*/
-	return true;
+		/*return CARDbSendPacket(pMgmt->pAdapter, pAction, PKT_TYPE_802_11_MNG,
+		  uLength);*/
+		return true;
 	}
 	return true;
 }
 
-
 bool IEEE11hbMSRRepTx(void *pMgmtHandle)
 {
 	PSMgmtObject            pMgmt = (PSMgmtObject) pMgmtHandle;
 	PWLAN_FRAME_MSRREP      pMSRRep = (PWLAN_FRAME_MSRREP)
-(pMgmt->abyCurrentMSRRep + sizeof(STxMgmtPacket));
+		(pMgmt->abyCurrentMSRRep + sizeof(STxMgmtPacket));
 	size_t                    uLength = 0;
 	PSTxMgmtPacket          pTxPacket = NULL;
 
 	pTxPacket = (PSTxMgmtPacket)pMgmt->abyCurrentMSRRep;
 	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_A3FR_MAXLEN);
 	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket +
-sizeof(STxMgmtPacket));
+						    sizeof(STxMgmtPacket));
 
 	pMSRRep->Header.wFrameCtl = (WLAN_SET_FC_FTYPE(WLAN_FTYPE_MGMT) |
-				WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_ACTION)
-				);
+				     WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_ACTION)
+);
 
 	memcpy(pMSRRep->Header.abyAddr1, ((PWLAN_FRAME_MSRREQ)
-		(pMgmt->abyCurrentMSRReq))->Header.abyAddr2, WLAN_ADDR_LEN);
+					  (pMgmt->abyCurrentMSRReq))->Header.abyAddr2, WLAN_ADDR_LEN);
 	memcpy(pMSRRep->Header.abyAddr2,
-		CARDpGetCurrentAddress(pMgmt->pAdapter), WLAN_ADDR_LEN);
+	       CARDpGetCurrentAddress(pMgmt->pAdapter), WLAN_ADDR_LEN);
 	memcpy(pMSRRep->Header.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
 
 	pMSRRep->byCategory = 0;
 	pMSRRep->byAction = 1;
 	pMSRRep->byDialogToken = ((PWLAN_FRAME_MSRREQ)
-		(pMgmt->abyCurrentMSRReq))->byDialogToken;
+				  (pMgmt->abyCurrentMSRReq))->byDialogToken;
 
 	uLength = pMgmt->uLengthOfRepEIDs + offsetof(WLAN_FRAME_MSRREP,
 						     sMSRRepEIDs);
@@ -322,8 +317,6 @@ sizeof(STxMgmtPacket));
 	if (csMgmt_xmit(pMgmt->pAdapter, pTxPacket) != CMD_STATUS_PENDING)
 		return false;
 	return true;
-/*    return (CARDbSendPacket(pMgmt->pAdapter, pMSRRep, PKT_TYPE_802_11_MNG,
-uLength)); */
-
+/*    return CARDbSendPacket(pMgmt->pAdapter, pMSRRep, PKT_TYPE_802_11_MNG,
+      uLength); */
 }
-
diff --git a/drivers/staging/vt6655/IEEE11h.h b/drivers/staging/vt6655/IEEE11h.h
index 542340b96e3..8819fa1563b 100644
--- a/drivers/staging/vt6655/IEEE11h.h
+++ b/drivers/staging/vt6655/IEEE11h.h
@@ -45,8 +45,8 @@
 
 /*---------------------  Export Functions  --------------------------*/
 
-bool IEEE11hbMSRRepTx (
-    void *pMgmtHandle
-    );
+bool IEEE11hbMSRRepTx(
+	void *pMgmtHandle
+);
 
 #endif // __IEEE11h_H__
diff --git a/drivers/staging/vt6655/aes_ccmp.c b/drivers/staging/vt6655/aes_ccmp.c
index e30168f2da2..36081481c6d 100644
--- a/drivers/staging/vt6655/aes_ccmp.c
+++ b/drivers/staging/vt6655/aes_ccmp.c
@@ -48,60 +48,60 @@
 
 unsigned char sbox_table[256] =
 {
-0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5, 0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76,
-0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0, 0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0,
-0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc, 0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15,
-0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a, 0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75,
-0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0, 0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84,
-0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b, 0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf,
-0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85, 0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8,
-0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5, 0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2,
-0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17, 0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73,
-0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88, 0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb,
-0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c, 0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79,
-0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9, 0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08,
-0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6, 0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a,
-0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e, 0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e,
-0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94, 0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf,
-0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68, 0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16
+	0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5, 0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76,
+	0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0, 0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0,
+	0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc, 0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15,
+	0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a, 0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75,
+	0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0, 0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84,
+	0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b, 0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf,
+	0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85, 0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8,
+	0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5, 0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2,
+	0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17, 0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73,
+	0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88, 0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb,
+	0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c, 0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79,
+	0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9, 0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08,
+	0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6, 0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a,
+	0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e, 0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e,
+	0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94, 0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf,
+	0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68, 0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16
 };
 
 unsigned char dot2_table[256] = {
-0x00, 0x02, 0x04, 0x06, 0x08, 0x0a, 0x0c, 0x0e, 0x10, 0x12, 0x14, 0x16, 0x18, 0x1a, 0x1c, 0x1e,
-0x20, 0x22, 0x24, 0x26, 0x28, 0x2a, 0x2c, 0x2e, 0x30, 0x32, 0x34, 0x36, 0x38, 0x3a, 0x3c, 0x3e,
-0x40, 0x42, 0x44, 0x46, 0x48, 0x4a, 0x4c, 0x4e, 0x50, 0x52, 0x54, 0x56, 0x58, 0x5a, 0x5c, 0x5e,
-0x60, 0x62, 0x64, 0x66, 0x68, 0x6a, 0x6c, 0x6e, 0x70, 0x72, 0x74, 0x76, 0x78, 0x7a, 0x7c, 0x7e,
-0x80, 0x82, 0x84, 0x86, 0x88, 0x8a, 0x8c, 0x8e, 0x90, 0x92, 0x94, 0x96, 0x98, 0x9a, 0x9c, 0x9e,
-0xa0, 0xa2, 0xa4, 0xa6, 0xa8, 0xaa, 0xac, 0xae, 0xb0, 0xb2, 0xb4, 0xb6, 0xb8, 0xba, 0xbc, 0xbe,
-0xc0, 0xc2, 0xc4, 0xc6, 0xc8, 0xca, 0xcc, 0xce, 0xd0, 0xd2, 0xd4, 0xd6, 0xd8, 0xda, 0xdc, 0xde,
-0xe0, 0xe2, 0xe4, 0xe6, 0xe8, 0xea, 0xec, 0xee, 0xf0, 0xf2, 0xf4, 0xf6, 0xf8, 0xfa, 0xfc, 0xfe,
-0x1b, 0x19, 0x1f, 0x1d, 0x13, 0x11, 0x17, 0x15, 0x0b, 0x09, 0x0f, 0x0d, 0x03, 0x01, 0x07, 0x05,
-0x3b, 0x39, 0x3f, 0x3d, 0x33, 0x31, 0x37, 0x35, 0x2b, 0x29, 0x2f, 0x2d, 0x23, 0x21, 0x27, 0x25,
-0x5b, 0x59, 0x5f, 0x5d, 0x53, 0x51, 0x57, 0x55, 0x4b, 0x49, 0x4f, 0x4d, 0x43, 0x41, 0x47, 0x45,
-0x7b, 0x79, 0x7f, 0x7d, 0x73, 0x71, 0x77, 0x75, 0x6b, 0x69, 0x6f, 0x6d, 0x63, 0x61, 0x67, 0x65,
-0x9b, 0x99, 0x9f, 0x9d, 0x93, 0x91, 0x97, 0x95, 0x8b, 0x89, 0x8f, 0x8d, 0x83, 0x81, 0x87, 0x85,
-0xbb, 0xb9, 0xbf, 0xbd, 0xb3, 0xb1, 0xb7, 0xb5, 0xab, 0xa9, 0xaf, 0xad, 0xa3, 0xa1, 0xa7, 0xa5,
-0xdb, 0xd9, 0xdf, 0xdd, 0xd3, 0xd1, 0xd7, 0xd5, 0xcb, 0xc9, 0xcf, 0xcd, 0xc3, 0xc1, 0xc7, 0xc5,
-0xfb, 0xf9, 0xff, 0xfd, 0xf3, 0xf1, 0xf7, 0xf5, 0xeb, 0xe9, 0xef, 0xed, 0xe3, 0xe1, 0xe7, 0xe5
+	0x00, 0x02, 0x04, 0x06, 0x08, 0x0a, 0x0c, 0x0e, 0x10, 0x12, 0x14, 0x16, 0x18, 0x1a, 0x1c, 0x1e,
+	0x20, 0x22, 0x24, 0x26, 0x28, 0x2a, 0x2c, 0x2e, 0x30, 0x32, 0x34, 0x36, 0x38, 0x3a, 0x3c, 0x3e,
+	0x40, 0x42, 0x44, 0x46, 0x48, 0x4a, 0x4c, 0x4e, 0x50, 0x52, 0x54, 0x56, 0x58, 0x5a, 0x5c, 0x5e,
+	0x60, 0x62, 0x64, 0x66, 0x68, 0x6a, 0x6c, 0x6e, 0x70, 0x72, 0x74, 0x76, 0x78, 0x7a, 0x7c, 0x7e,
+	0x80, 0x82, 0x84, 0x86, 0x88, 0x8a, 0x8c, 0x8e, 0x90, 0x92, 0x94, 0x96, 0x98, 0x9a, 0x9c, 0x9e,
+	0xa0, 0xa2, 0xa4, 0xa6, 0xa8, 0xaa, 0xac, 0xae, 0xb0, 0xb2, 0xb4, 0xb6, 0xb8, 0xba, 0xbc, 0xbe,
+	0xc0, 0xc2, 0xc4, 0xc6, 0xc8, 0xca, 0xcc, 0xce, 0xd0, 0xd2, 0xd4, 0xd6, 0xd8, 0xda, 0xdc, 0xde,
+	0xe0, 0xe2, 0xe4, 0xe6, 0xe8, 0xea, 0xec, 0xee, 0xf0, 0xf2, 0xf4, 0xf6, 0xf8, 0xfa, 0xfc, 0xfe,
+	0x1b, 0x19, 0x1f, 0x1d, 0x13, 0x11, 0x17, 0x15, 0x0b, 0x09, 0x0f, 0x0d, 0x03, 0x01, 0x07, 0x05,
+	0x3b, 0x39, 0x3f, 0x3d, 0x33, 0x31, 0x37, 0x35, 0x2b, 0x29, 0x2f, 0x2d, 0x23, 0x21, 0x27, 0x25,
+	0x5b, 0x59, 0x5f, 0x5d, 0x53, 0x51, 0x57, 0x55, 0x4b, 0x49, 0x4f, 0x4d, 0x43, 0x41, 0x47, 0x45,
+	0x7b, 0x79, 0x7f, 0x7d, 0x73, 0x71, 0x77, 0x75, 0x6b, 0x69, 0x6f, 0x6d, 0x63, 0x61, 0x67, 0x65,
+	0x9b, 0x99, 0x9f, 0x9d, 0x93, 0x91, 0x97, 0x95, 0x8b, 0x89, 0x8f, 0x8d, 0x83, 0x81, 0x87, 0x85,
+	0xbb, 0xb9, 0xbf, 0xbd, 0xb3, 0xb1, 0xb7, 0xb5, 0xab, 0xa9, 0xaf, 0xad, 0xa3, 0xa1, 0xa7, 0xa5,
+	0xdb, 0xd9, 0xdf, 0xdd, 0xd3, 0xd1, 0xd7, 0xd5, 0xcb, 0xc9, 0xcf, 0xcd, 0xc3, 0xc1, 0xc7, 0xc5,
+	0xfb, 0xf9, 0xff, 0xfd, 0xf3, 0xf1, 0xf7, 0xf5, 0xeb, 0xe9, 0xef, 0xed, 0xe3, 0xe1, 0xe7, 0xe5
 };
 
 unsigned char dot3_table[256] = {
-0x00, 0x03, 0x06, 0x05, 0x0c, 0x0f, 0x0a, 0x09, 0x18, 0x1b, 0x1e, 0x1d, 0x14, 0x17, 0x12, 0x11,
-0x30, 0x33, 0x36, 0x35, 0x3c, 0x3f, 0x3a, 0x39, 0x28, 0x2b, 0x2e, 0x2d, 0x24, 0x27, 0x22, 0x21,
-0x60, 0x63, 0x66, 0x65, 0x6c, 0x6f, 0x6a, 0x69, 0x78, 0x7b, 0x7e, 0x7d, 0x74, 0x77, 0x72, 0x71,
-0x50, 0x53, 0x56, 0x55, 0x5c, 0x5f, 0x5a, 0x59, 0x48, 0x4b, 0x4e, 0x4d, 0x44, 0x47, 0x42, 0x41,
-0xc0, 0xc3, 0xc6, 0xc5, 0xcc, 0xcf, 0xca, 0xc9, 0xd8, 0xdb, 0xde, 0xdd, 0xd4, 0xd7, 0xd2, 0xd1,
-0xf0, 0xf3, 0xf6, 0xf5, 0xfc, 0xff, 0xfa, 0xf9, 0xe8, 0xeb, 0xee, 0xed, 0xe4, 0xe7, 0xe2, 0xe1,
-0xa0, 0xa3, 0xa6, 0xa5, 0xac, 0xaf, 0xaa, 0xa9, 0xb8, 0xbb, 0xbe, 0xbd, 0xb4, 0xb7, 0xb2, 0xb1,
-0x90, 0x93, 0x96, 0x95, 0x9c, 0x9f, 0x9a, 0x99, 0x88, 0x8b, 0x8e, 0x8d, 0x84, 0x87, 0x82, 0x81,
-0x9b, 0x98, 0x9d, 0x9e, 0x97, 0x94, 0x91, 0x92, 0x83, 0x80, 0x85, 0x86, 0x8f, 0x8c, 0x89, 0x8a,
-0xab, 0xa8, 0xad, 0xae, 0xa7, 0xa4, 0xa1, 0xa2, 0xb3, 0xb0, 0xb5, 0xb6, 0xbf, 0xbc, 0xb9, 0xba,
-0xfb, 0xf8, 0xfd, 0xfe, 0xf7, 0xf4, 0xf1, 0xf2, 0xe3, 0xe0, 0xe5, 0xe6, 0xef, 0xec, 0xe9, 0xea,
-0xcb, 0xc8, 0xcd, 0xce, 0xc7, 0xc4, 0xc1, 0xc2, 0xd3, 0xd0, 0xd5, 0xd6, 0xdf, 0xdc, 0xd9, 0xda,
-0x5b, 0x58, 0x5d, 0x5e, 0x57, 0x54, 0x51, 0x52, 0x43, 0x40, 0x45, 0x46, 0x4f, 0x4c, 0x49, 0x4a,
-0x6b, 0x68, 0x6d, 0x6e, 0x67, 0x64, 0x61, 0x62, 0x73, 0x70, 0x75, 0x76, 0x7f, 0x7c, 0x79, 0x7a,
-0x3b, 0x38, 0x3d, 0x3e, 0x37, 0x34, 0x31, 0x32, 0x23, 0x20, 0x25, 0x26, 0x2f, 0x2c, 0x29, 0x2a,
-0x0b, 0x08, 0x0d, 0x0e, 0x07, 0x04, 0x01, 0x02, 0x13, 0x10, 0x15, 0x16, 0x1f, 0x1c, 0x19, 0x1a
+	0x00, 0x03, 0x06, 0x05, 0x0c, 0x0f, 0x0a, 0x09, 0x18, 0x1b, 0x1e, 0x1d, 0x14, 0x17, 0x12, 0x11,
+	0x30, 0x33, 0x36, 0x35, 0x3c, 0x3f, 0x3a, 0x39, 0x28, 0x2b, 0x2e, 0x2d, 0x24, 0x27, 0x22, 0x21,
+	0x60, 0x63, 0x66, 0x65, 0x6c, 0x6f, 0x6a, 0x69, 0x78, 0x7b, 0x7e, 0x7d, 0x74, 0x77, 0x72, 0x71,
+	0x50, 0x53, 0x56, 0x55, 0x5c, 0x5f, 0x5a, 0x59, 0x48, 0x4b, 0x4e, 0x4d, 0x44, 0x47, 0x42, 0x41,
+	0xc0, 0xc3, 0xc6, 0xc5, 0xcc, 0xcf, 0xca, 0xc9, 0xd8, 0xdb, 0xde, 0xdd, 0xd4, 0xd7, 0xd2, 0xd1,
+	0xf0, 0xf3, 0xf6, 0xf5, 0xfc, 0xff, 0xfa, 0xf9, 0xe8, 0xeb, 0xee, 0xed, 0xe4, 0xe7, 0xe2, 0xe1,
+	0xa0, 0xa3, 0xa6, 0xa5, 0xac, 0xaf, 0xaa, 0xa9, 0xb8, 0xbb, 0xbe, 0xbd, 0xb4, 0xb7, 0xb2, 0xb1,
+	0x90, 0x93, 0x96, 0x95, 0x9c, 0x9f, 0x9a, 0x99, 0x88, 0x8b, 0x8e, 0x8d, 0x84, 0x87, 0x82, 0x81,
+	0x9b, 0x98, 0x9d, 0x9e, 0x97, 0x94, 0x91, 0x92, 0x83, 0x80, 0x85, 0x86, 0x8f, 0x8c, 0x89, 0x8a,
+	0xab, 0xa8, 0xad, 0xae, 0xa7, 0xa4, 0xa1, 0xa2, 0xb3, 0xb0, 0xb5, 0xb6, 0xbf, 0xbc, 0xb9, 0xba,
+	0xfb, 0xf8, 0xfd, 0xfe, 0xf7, 0xf4, 0xf1, 0xf2, 0xe3, 0xe0, 0xe5, 0xe6, 0xef, 0xec, 0xe9, 0xea,
+	0xcb, 0xc8, 0xcd, 0xce, 0xc7, 0xc4, 0xc1, 0xc2, 0xd3, 0xd0, 0xd5, 0xd6, 0xdf, 0xdc, 0xd9, 0xda,
+	0x5b, 0x58, 0x5d, 0x5e, 0x57, 0x54, 0x51, 0x52, 0x43, 0x40, 0x45, 0x46, 0x4f, 0x4c, 0x49, 0x4a,
+	0x6b, 0x68, 0x6d, 0x6e, 0x67, 0x64, 0x61, 0x62, 0x73, 0x70, 0x75, 0x76, 0x7f, 0x7c, 0x79, 0x7a,
+	0x3b, 0x38, 0x3d, 0x3e, 0x37, 0x34, 0x31, 0x32, 0x23, 0x20, 0x25, 0x26, 0x2f, 0x2c, 0x29, 0x2a,
+	0x0b, 0x08, 0x0d, 0x0e, 0x07, 0x04, 0x01, 0x02, 0x13, 0x10, 0x15, 0x16, 0x1f, 0x1c, 0x19, 0x1a
 };
 
 /*---------------------  Static Functions  --------------------------*/
@@ -112,121 +112,111 @@ unsigned char dot3_table[256] = {
 
 void xor_128(unsigned char *a, unsigned char *b, unsigned char *out)
 {
-unsigned long *dwPtrA = (unsigned long *) a;
-unsigned long *dwPtrB = (unsigned long *) b;
-unsigned long *dwPtrOut =(unsigned long *) out;
-
-    (*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
-    (*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
-    (*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
-    (*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
+	unsigned long *dwPtrA = (unsigned long *)a;
+	unsigned long *dwPtrB = (unsigned long *)b;
+	unsigned long *dwPtrOut = (unsigned long *)out;
+
+	(*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
+	(*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
+	(*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
+	(*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
 }
 
-
 void xor_32(unsigned char *a, unsigned char *b, unsigned char *out)
 {
-unsigned long *dwPtrA = (unsigned long *) a;
-unsigned long *dwPtrB = (unsigned long *) b;
-unsigned long *dwPtrOut =(unsigned long *) out;
+	unsigned long *dwPtrA = (unsigned long *)a;
+	unsigned long *dwPtrB = (unsigned long *)b;
+	unsigned long *dwPtrOut = (unsigned long *)out;
 
-    (*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
+	(*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
 }
 
 void AddRoundKey(unsigned char *key, int round)
 {
-unsigned char sbox_key[4];
-unsigned char rcon_table[10] = { 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80, 0x1b, 0x36};
+	unsigned char sbox_key[4];
+	unsigned char rcon_table[10] = { 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80, 0x1b, 0x36};
 
-    sbox_key[0] = sbox_table[key[13]];
-    sbox_key[1] = sbox_table[key[14]];
-    sbox_key[2] = sbox_table[key[15]];
-    sbox_key[3] = sbox_table[key[12]];
+	sbox_key[0] = sbox_table[key[13]];
+	sbox_key[1] = sbox_table[key[14]];
+	sbox_key[2] = sbox_table[key[15]];
+	sbox_key[3] = sbox_table[key[12]];
 
-    key[0] = key[0] ^ rcon_table[round];
-    xor_32(&key[0], sbox_key, &key[0]);
+	key[0] = key[0] ^ rcon_table[round];
+	xor_32(&key[0], sbox_key, &key[0]);
 
-    xor_32(&key[4], &key[0], &key[4]);
-    xor_32(&key[8], &key[4], &key[8]);
-    xor_32(&key[12], &key[8], &key[12]);
+	xor_32(&key[4], &key[0], &key[4]);
+	xor_32(&key[8], &key[4], &key[8]);
+	xor_32(&key[12], &key[8], &key[12]);
 }
 
 void SubBytes(unsigned char *in, unsigned char *out)
 {
-int i;
+	int i;
 
-    for (i=0; i< 16; i++)
-    {
-        out[i] = sbox_table[in[i]];
-    }
+	for (i = 0; i < 16; i++) {
+		out[i] = sbox_table[in[i]];
+	}
 }
 
 void ShiftRows(unsigned char *in, unsigned char *out)
 {
-    out[0]  = in[0];
-    out[1]  = in[5];
-    out[2]  = in[10];
-    out[3]  = in[15];
-    out[4]  = in[4];
-    out[5]  = in[9];
-    out[6]  = in[14];
-    out[7]  = in[3];
-    out[8]  = in[8];
-    out[9]  = in[13];
-    out[10] = in[2];
-    out[11] = in[7];
-    out[12] = in[12];
-    out[13] = in[1];
-    out[14] = in[6];
-    out[15] = in[11];
+	out[0]  = in[0];
+	out[1]  = in[5];
+	out[2]  = in[10];
+	out[3]  = in[15];
+	out[4]  = in[4];
+	out[5]  = in[9];
+	out[6]  = in[14];
+	out[7]  = in[3];
+	out[8]  = in[8];
+	out[9]  = in[13];
+	out[10] = in[2];
+	out[11] = in[7];
+	out[12] = in[12];
+	out[13] = in[1];
+	out[14] = in[6];
+	out[15] = in[11];
 }
 
 void MixColumns(unsigned char *in, unsigned char *out)
 {
-
-    out[0] = dot2_table[in[0]] ^ dot3_table[in[1]] ^ in[2] ^ in[3];
-    out[1] = in[0] ^ dot2_table[in[1]] ^ dot3_table[in[2]] ^ in[3];
-    out[2] = in[0] ^ in[1] ^ dot2_table[in[2]] ^ dot3_table[in[3]];
-    out[3] = dot3_table[in[0]] ^ in[1] ^ in[2] ^ dot2_table[in[3]];
+	out[0] = dot2_table[in[0]] ^ dot3_table[in[1]] ^ in[2] ^ in[3];
+	out[1] = in[0] ^ dot2_table[in[1]] ^ dot3_table[in[2]] ^ in[3];
+	out[2] = in[0] ^ in[1] ^ dot2_table[in[2]] ^ dot3_table[in[3]];
+	out[3] = dot3_table[in[0]] ^ in[1] ^ in[2] ^ dot2_table[in[3]];
 }
 
-
 void AESv128(unsigned char *key, unsigned char *data, unsigned char *ciphertext)
 {
-int  i;
-int  round;
-unsigned char TmpdataA[16];
-unsigned char TmpdataB[16];
-unsigned char abyRoundKey[16];
-
-    for(i=0; i<16; i++)
-        abyRoundKey[i] = key[i];
-
-    for (round = 0; round < 11; round++)
-    {
-        if (round == 0)
-        {
-            xor_128(abyRoundKey, data, ciphertext);
-            AddRoundKey(abyRoundKey, round);
-        }
-        else if (round == 10)
-        {
-            SubBytes(ciphertext, TmpdataA);
-            ShiftRows(TmpdataA, TmpdataB);
-            xor_128(TmpdataB, abyRoundKey, ciphertext);
-        }
-        else // round 1 ~ 9
-        {
-            SubBytes(ciphertext, TmpdataA);
-            ShiftRows(TmpdataA, TmpdataB);
-            MixColumns(&TmpdataB[0], &TmpdataA[0]);
-            MixColumns(&TmpdataB[4], &TmpdataA[4]);
-            MixColumns(&TmpdataB[8], &TmpdataA[8]);
-            MixColumns(&TmpdataB[12], &TmpdataA[12]);
-            xor_128(TmpdataA, abyRoundKey, ciphertext);
-            AddRoundKey(abyRoundKey, round);
-        }
-    }
-
+	int  i;
+	int  round;
+	unsigned char TmpdataA[16];
+	unsigned char TmpdataB[16];
+	unsigned char abyRoundKey[16];
+
+	for (i = 0; i < 16; i++)
+		abyRoundKey[i] = key[i];
+
+	for (round = 0; round < 11; round++) {
+		if (round == 0) {
+			xor_128(abyRoundKey, data, ciphertext);
+			AddRoundKey(abyRoundKey, round);
+		} else if (round == 10) {
+			SubBytes(ciphertext, TmpdataA);
+			ShiftRows(TmpdataA, TmpdataB);
+			xor_128(TmpdataB, abyRoundKey, ciphertext);
+		} else // round 1 ~ 9
+		{
+			SubBytes(ciphertext, TmpdataA);
+			ShiftRows(TmpdataA, TmpdataB);
+			MixColumns(&TmpdataB[0], &TmpdataA[0]);
+			MixColumns(&TmpdataB[4], &TmpdataA[4]);
+			MixColumns(&TmpdataB[8], &TmpdataA[8]);
+			MixColumns(&TmpdataB[12], &TmpdataA[12]);
+			xor_128(TmpdataA, abyRoundKey, ciphertext);
+			AddRoundKey(abyRoundKey, round);
+		}
+	}
 }
 
 /*
@@ -245,158 +235,155 @@ unsigned char abyRoundKey[16];
  */
 bool AESbGenCCMP(unsigned char *pbyRxKey, unsigned char *pbyFrame, unsigned short wFrameSize)
 {
-unsigned char abyNonce[13];
-unsigned char MIC_IV[16];
-unsigned char MIC_HDR1[16];
-unsigned char MIC_HDR2[16];
-unsigned char abyMIC[16];
-unsigned char abyCTRPLD[16];
-unsigned char abyTmp[16];
-unsigned char abyPlainText[16];
-unsigned char abyLastCipher[16];
-
-PS802_11Header  pMACHeader = (PS802_11Header) pbyFrame;
-unsigned char *pbyIV;
-unsigned char *pbyPayload;
-unsigned short wHLen = 22;
-unsigned short wPayloadSize = wFrameSize - 8 - 8 - 4 - WLAN_HDR_ADDR3_LEN;//8 is IV, 8 is MIC, 4 is CRC
-bool bA4 = false;
-unsigned char byTmp;
-unsigned short wCnt;
-int             ii,jj,kk;
-
-
-    pbyIV = pbyFrame + WLAN_HDR_ADDR3_LEN;
-    if ( WLAN_GET_FC_TODS(*(unsigned short *)pbyFrame) &&
-         WLAN_GET_FC_FROMDS(*(unsigned short *)pbyFrame) ) {
-         bA4 = true;
-         pbyIV += 6;             // 6 is 802.11 address4
-         wHLen += 6;
-         wPayloadSize -= 6;
-    }
-    pbyPayload = pbyIV + 8; //IV-length
-
-    abyNonce[0]  = 0x00; //now is 0, if Qos here will be priority
-    memcpy(&(abyNonce[1]), pMACHeader->abyAddr2, ETH_ALEN);
-    abyNonce[7]  = pbyIV[7];
-    abyNonce[8]  = pbyIV[6];
-    abyNonce[9]  = pbyIV[5];
-    abyNonce[10] = pbyIV[4];
-    abyNonce[11] = pbyIV[1];
-    abyNonce[12] = pbyIV[0];
-
-    //MIC_IV
-    MIC_IV[0] = 0x59;
-    memcpy(&(MIC_IV[1]), &(abyNonce[0]), 13);
-    MIC_IV[14] = (unsigned char)(wPayloadSize >> 8);
-    MIC_IV[15] = (unsigned char)(wPayloadSize & 0xff);
-
-    //MIC_HDR1
-    MIC_HDR1[0] = (unsigned char)(wHLen >> 8);
-    MIC_HDR1[1] = (unsigned char)(wHLen & 0xff);
-    byTmp = (unsigned char)(pMACHeader->wFrameCtl & 0xff);
-    MIC_HDR1[2] = byTmp & 0x8f;
-    byTmp = (unsigned char)(pMACHeader->wFrameCtl >> 8);
-    byTmp &= 0x87;
-    MIC_HDR1[3] = byTmp | 0x40;
-    memcpy(&(MIC_HDR1[4]), pMACHeader->abyAddr1, ETH_ALEN);
-    memcpy(&(MIC_HDR1[10]), pMACHeader->abyAddr2, ETH_ALEN);
-
-    //MIC_HDR2
-    memcpy(&(MIC_HDR2[0]), pMACHeader->abyAddr3, ETH_ALEN);
-    byTmp = (unsigned char)(pMACHeader->wSeqCtl & 0xff);
-    MIC_HDR2[6] = byTmp & 0x0f;
-    MIC_HDR2[7] = 0;
-    if ( bA4 ) {
-        memcpy(&(MIC_HDR2[8]), pMACHeader->abyAddr4, ETH_ALEN);
-    } else {
-        MIC_HDR2[8]  = 0x00;
-        MIC_HDR2[9]  = 0x00;
-        MIC_HDR2[10] = 0x00;
-        MIC_HDR2[11] = 0x00;
-        MIC_HDR2[12] = 0x00;
-        MIC_HDR2[13] = 0x00;
-    }
-    MIC_HDR2[14] = 0x00;
-    MIC_HDR2[15] = 0x00;
-
-    //CCMP
-    AESv128(pbyRxKey,MIC_IV,abyMIC);
-    for ( kk=0; kk<16; kk++ ) {
-        abyTmp[kk] = MIC_HDR1[kk] ^ abyMIC[kk];
-    }
-    AESv128(pbyRxKey,abyTmp,abyMIC);
-    for ( kk=0; kk<16; kk++ ) {
-        abyTmp[kk] = MIC_HDR2[kk] ^ abyMIC[kk];
-    }
-    AESv128(pbyRxKey,abyTmp,abyMIC);
-
-    wCnt = 1;
-    abyCTRPLD[0] = 0x01;
-    memcpy(&(abyCTRPLD[1]), &(abyNonce[0]), 13);
-
-    for(jj=wPayloadSize; jj>16; jj=jj-16) {
-
-        abyCTRPLD[14] = (unsigned char) (wCnt >> 8);
-        abyCTRPLD[15] = (unsigned char) (wCnt & 0xff);
-
-        AESv128(pbyRxKey,abyCTRPLD,abyTmp);
-
-        for ( kk=0; kk<16; kk++ ) {
-            abyPlainText[kk] = abyTmp[kk] ^ pbyPayload[kk];
-        }
-        for ( kk=0; kk<16; kk++ ) {
-            abyTmp[kk] = abyMIC[kk] ^ abyPlainText[kk];
-        }
-        AESv128(pbyRxKey,abyTmp,abyMIC);
-
-        memcpy(pbyPayload, abyPlainText, 16);
-        wCnt++;
-        pbyPayload += 16;
-    } //for wPayloadSize
-
-    //last payload
-    memcpy(&(abyLastCipher[0]), pbyPayload, jj);
-    for ( ii=jj; ii<16; ii++ ) {
-        abyLastCipher[ii] = 0x00;
-    }
-
-    abyCTRPLD[14] = (unsigned char) (wCnt >> 8);
-    abyCTRPLD[15] = (unsigned char) (wCnt & 0xff);
-
-    AESv128(pbyRxKey,abyCTRPLD,abyTmp);
-    for ( kk=0; kk<16; kk++ ) {
-        abyPlainText[kk] = abyTmp[kk] ^ abyLastCipher[kk];
-    }
-    memcpy(pbyPayload, abyPlainText, jj);
-    pbyPayload += jj;
-
-    //for MIC calculation
-    for ( ii=jj; ii<16; ii++ ) {
-        abyPlainText[ii] = 0x00;
-    }
-    for ( kk=0; kk<16; kk++ ) {
-        abyTmp[kk] = abyMIC[kk] ^ abyPlainText[kk];
-    }
-    AESv128(pbyRxKey,abyTmp,abyMIC);
-
-    //=>above is the calculate MIC
-    //--------------------------------------------
-
-    wCnt = 0;
-    abyCTRPLD[14] = (unsigned char) (wCnt >> 8);
-    abyCTRPLD[15] = (unsigned char) (wCnt & 0xff);
-    AESv128(pbyRxKey,abyCTRPLD,abyTmp);
-    for ( kk=0; kk<8; kk++ ) {
-        abyTmp[kk] = abyTmp[kk] ^ pbyPayload[kk];
-    }
-    //=>above is the dec-MIC from packet
-    //--------------------------------------------
-
-    if ( !memcmp(abyMIC,abyTmp,8) ) {
-        return true;
-    } else {
-        return false;
-    }
-
+	unsigned char abyNonce[13];
+	unsigned char MIC_IV[16];
+	unsigned char MIC_HDR1[16];
+	unsigned char MIC_HDR2[16];
+	unsigned char abyMIC[16];
+	unsigned char abyCTRPLD[16];
+	unsigned char abyTmp[16];
+	unsigned char abyPlainText[16];
+	unsigned char abyLastCipher[16];
+
+	PS802_11Header  pMACHeader = (PS802_11Header) pbyFrame;
+	unsigned char *pbyIV;
+	unsigned char *pbyPayload;
+	unsigned short wHLen = 22;
+	unsigned short wPayloadSize = wFrameSize - 8 - 8 - 4 - WLAN_HDR_ADDR3_LEN;//8 is IV, 8 is MIC, 4 is CRC
+	bool bA4 = false;
+	unsigned char byTmp;
+	unsigned short wCnt;
+	int ii, jj, kk;
+
+	pbyIV = pbyFrame + WLAN_HDR_ADDR3_LEN;
+	if (WLAN_GET_FC_TODS(*(unsigned short *)pbyFrame) &&
+	    WLAN_GET_FC_FROMDS(*(unsigned short *)pbyFrame)) {
+		bA4 = true;
+		pbyIV += 6;             // 6 is 802.11 address4
+		wHLen += 6;
+		wPayloadSize -= 6;
+	}
+	pbyPayload = pbyIV + 8; //IV-length
+
+	abyNonce[0]  = 0x00; //now is 0, if Qos here will be priority
+	memcpy(&(abyNonce[1]), pMACHeader->abyAddr2, ETH_ALEN);
+	abyNonce[7]  = pbyIV[7];
+	abyNonce[8]  = pbyIV[6];
+	abyNonce[9]  = pbyIV[5];
+	abyNonce[10] = pbyIV[4];
+	abyNonce[11] = pbyIV[1];
+	abyNonce[12] = pbyIV[0];
+
+	//MIC_IV
+	MIC_IV[0] = 0x59;
+	memcpy(&(MIC_IV[1]), &(abyNonce[0]), 13);
+	MIC_IV[14] = (unsigned char)(wPayloadSize >> 8);
+	MIC_IV[15] = (unsigned char)(wPayloadSize & 0xff);
+
+	//MIC_HDR1
+	MIC_HDR1[0] = (unsigned char)(wHLen >> 8);
+	MIC_HDR1[1] = (unsigned char)(wHLen & 0xff);
+	byTmp = (unsigned char)(pMACHeader->wFrameCtl & 0xff);
+	MIC_HDR1[2] = byTmp & 0x8f;
+	byTmp = (unsigned char)(pMACHeader->wFrameCtl >> 8);
+	byTmp &= 0x87;
+	MIC_HDR1[3] = byTmp | 0x40;
+	memcpy(&(MIC_HDR1[4]), pMACHeader->abyAddr1, ETH_ALEN);
+	memcpy(&(MIC_HDR1[10]), pMACHeader->abyAddr2, ETH_ALEN);
+
+	//MIC_HDR2
+	memcpy(&(MIC_HDR2[0]), pMACHeader->abyAddr3, ETH_ALEN);
+	byTmp = (unsigned char)(pMACHeader->wSeqCtl & 0xff);
+	MIC_HDR2[6] = byTmp & 0x0f;
+	MIC_HDR2[7] = 0;
+	if (bA4) {
+		memcpy(&(MIC_HDR2[8]), pMACHeader->abyAddr4, ETH_ALEN);
+	} else {
+		MIC_HDR2[8]  = 0x00;
+		MIC_HDR2[9]  = 0x00;
+		MIC_HDR2[10] = 0x00;
+		MIC_HDR2[11] = 0x00;
+		MIC_HDR2[12] = 0x00;
+		MIC_HDR2[13] = 0x00;
+	}
+	MIC_HDR2[14] = 0x00;
+	MIC_HDR2[15] = 0x00;
+
+	//CCMP
+	AESv128(pbyRxKey, MIC_IV, abyMIC);
+	for (kk = 0; kk < 16; kk++) {
+		abyTmp[kk] = MIC_HDR1[kk] ^ abyMIC[kk];
+	}
+	AESv128(pbyRxKey, abyTmp, abyMIC);
+	for (kk = 0; kk < 16; kk++) {
+		abyTmp[kk] = MIC_HDR2[kk] ^ abyMIC[kk];
+	}
+	AESv128(pbyRxKey, abyTmp, abyMIC);
+
+	wCnt = 1;
+	abyCTRPLD[0] = 0x01;
+	memcpy(&(abyCTRPLD[1]), &(abyNonce[0]), 13);
+
+	for (jj = wPayloadSize; jj > 16; jj = jj - 16) {
+		abyCTRPLD[14] = (unsigned char)(wCnt >> 8);
+		abyCTRPLD[15] = (unsigned char)(wCnt & 0xff);
+
+		AESv128(pbyRxKey, abyCTRPLD, abyTmp);
+
+		for (kk = 0; kk < 16; kk++) {
+			abyPlainText[kk] = abyTmp[kk] ^ pbyPayload[kk];
+		}
+		for (kk = 0; kk < 16; kk++) {
+			abyTmp[kk] = abyMIC[kk] ^ abyPlainText[kk];
+		}
+		AESv128(pbyRxKey, abyTmp, abyMIC);
+
+		memcpy(pbyPayload, abyPlainText, 16);
+		wCnt++;
+		pbyPayload += 16;
+	} //for wPayloadSize
+
+	//last payload
+	memcpy(&(abyLastCipher[0]), pbyPayload, jj);
+	for (ii = jj; ii < 16; ii++) {
+		abyLastCipher[ii] = 0x00;
+	}
+
+	abyCTRPLD[14] = (unsigned char)(wCnt >> 8);
+	abyCTRPLD[15] = (unsigned char)(wCnt & 0xff);
+
+	AESv128(pbyRxKey, abyCTRPLD, abyTmp);
+	for (kk = 0; kk < 16; kk++) {
+		abyPlainText[kk] = abyTmp[kk] ^ abyLastCipher[kk];
+	}
+	memcpy(pbyPayload, abyPlainText, jj);
+	pbyPayload += jj;
+
+	//for MIC calculation
+	for (ii = jj; ii < 16; ii++) {
+		abyPlainText[ii] = 0x00;
+	}
+	for (kk = 0; kk < 16; kk++) {
+		abyTmp[kk] = abyMIC[kk] ^ abyPlainText[kk];
+	}
+	AESv128(pbyRxKey, abyTmp, abyMIC);
+
+	//=>above is the calculate MIC
+	//--------------------------------------------
+
+	wCnt = 0;
+	abyCTRPLD[14] = (unsigned char)(wCnt >> 8);
+	abyCTRPLD[15] = (unsigned char)(wCnt & 0xff);
+	AESv128(pbyRxKey, abyCTRPLD, abyTmp);
+	for (kk = 0; kk < 8; kk++) {
+		abyTmp[kk] = abyTmp[kk] ^ pbyPayload[kk];
+	}
+	//=>above is the dec-MIC from packet
+	//--------------------------------------------
+
+	if (!memcmp(abyMIC, abyTmp, 8)) {
+		return true;
+	} else {
+		return false;
+	}
 }
diff --git a/drivers/staging/vt6655/baseband.c b/drivers/staging/vt6655/baseband.c
index 8d2c6a789ab..c26418d806f 100644
--- a/drivers/staging/vt6655/baseband.c
+++ b/drivers/staging/vt6655/baseband.c
@@ -58,7 +58,7 @@
 
 /*---------------------  Static Definitions -------------------------*/
 //static int          msglevel                =MSG_LEVEL_DEBUG;
-static int          msglevel                =MSG_LEVEL_INFO;
+static int msglevel = MSG_LEVEL_INFO;
 
 /*---------------------  Static Classes  ----------------------------*/
 
@@ -74,1646 +74,1641 @@ static int          msglevel                =MSG_LEVEL_INFO;
 
 /*---------------------  Static Variables  --------------------------*/
 
-
-
 #define CB_VT3253_INIT_FOR_RFMD 446
 unsigned char byVT3253InitTab_RFMD[CB_VT3253_INIT_FOR_RFMD][2] = {
-    {0x00, 0x30},
-    {0x01, 0x00},
-    {0x02, 0x00},
-    {0x03, 0x00},
-    {0x04, 0x00},
-    {0x05, 0x00},
-    {0x06, 0x00},
-    {0x07, 0x00},
-    {0x08, 0x70},
-    {0x09, 0x45},
-    {0x0a, 0x2a},
-    {0x0b, 0x76},
-    {0x0c, 0x00},
-    {0x0d, 0x01},
-    {0x0e, 0x80},
-    {0x0f, 0x00},
-    {0x10, 0x00},
-    {0x11, 0x00},
-    {0x12, 0x00},
-    {0x13, 0x00},
-    {0x14, 0x00},
-    {0x15, 0x00},
-    {0x16, 0x00},
-    {0x17, 0x00},
-    {0x18, 0x00},
-    {0x19, 0x00},
-    {0x1a, 0x00},
-    {0x1b, 0x9d},
-    {0x1c, 0x05},
-    {0x1d, 0x00},
-    {0x1e, 0x00},
-    {0x1f, 0x00},
-    {0x20, 0x00},
-    {0x21, 0x00},
-    {0x22, 0x00},
-    {0x23, 0x00},
-    {0x24, 0x00},
-    {0x25, 0x4a},
-    {0x26, 0x00},
-    {0x27, 0x00},
-    {0x28, 0x00},
-    {0x29, 0x00},
-    {0x2a, 0x00},
-    {0x2b, 0x00},
-    {0x2c, 0x00},
-    {0x2d, 0xa8},
-    {0x2e, 0x1a},
-    {0x2f, 0x0c},
-    {0x30, 0x26},
-    {0x31, 0x5b},
-    {0x32, 0x00},
-    {0x33, 0x00},
-    {0x34, 0x00},
-    {0x35, 0x00},
-    {0x36, 0xaa},
-    {0x37, 0xaa},
-    {0x38, 0xff},
-    {0x39, 0xff},
-    {0x3a, 0x00},
-    {0x3b, 0x00},
-    {0x3c, 0x00},
-    {0x3d, 0x0d},
-    {0x3e, 0x51},
-    {0x3f, 0x04},
-    {0x40, 0x00},
-    {0x41, 0x08},
-    {0x42, 0x00},
-    {0x43, 0x08},
-    {0x44, 0x06},
-    {0x45, 0x14},
-    {0x46, 0x05},
-    {0x47, 0x08},
-    {0x48, 0x00},
-    {0x49, 0x00},
-    {0x4a, 0x00},
-    {0x4b, 0x00},
-    {0x4c, 0x09},
-    {0x4d, 0x80},
-    {0x4e, 0x00},
-    {0x4f, 0xc5},
-    {0x50, 0x14},
-    {0x51, 0x19},
-    {0x52, 0x00},
-    {0x53, 0x00},
-    {0x54, 0x00},
-    {0x55, 0x00},
-    {0x56, 0x00},
-    {0x57, 0x00},
-    {0x58, 0x00},
-    {0x59, 0xb0},
-    {0x5a, 0x00},
-    {0x5b, 0x00},
-    {0x5c, 0x00},
-    {0x5d, 0x00},
-    {0x5e, 0x00},
-    {0x5f, 0x00},
-    {0x60, 0x44},
-    {0x61, 0x04},
-    {0x62, 0x00},
-    {0x63, 0x00},
-    {0x64, 0x00},
-    {0x65, 0x00},
-    {0x66, 0x04},
-    {0x67, 0xb7},
-    {0x68, 0x00},
-    {0x69, 0x00},
-    {0x6a, 0x00},
-    {0x6b, 0x00},
-    {0x6c, 0x00},
-    {0x6d, 0x03},
-    {0x6e, 0x01},
-    {0x6f, 0x00},
-    {0x70, 0x00},
-    {0x71, 0x00},
-    {0x72, 0x00},
-    {0x73, 0x00},
-    {0x74, 0x00},
-    {0x75, 0x00},
-    {0x76, 0x00},
-    {0x77, 0x00},
-    {0x78, 0x00},
-    {0x79, 0x00},
-    {0x7a, 0x00},
-    {0x7b, 0x00},
-    {0x7c, 0x00},
-    {0x7d, 0x00},
-    {0x7e, 0x00},
-    {0x7f, 0x00},
-    {0x80, 0x0b},
-    {0x81, 0x00},
-    {0x82, 0x3c},
-    {0x83, 0x00},
-    {0x84, 0x00},
-    {0x85, 0x00},
-    {0x86, 0x00},
-    {0x87, 0x00},
-    {0x88, 0x08},
-    {0x89, 0x00},
-    {0x8a, 0x08},
-    {0x8b, 0xa6},
-    {0x8c, 0x84},
-    {0x8d, 0x47},
-    {0x8e, 0xbb},
-    {0x8f, 0x02},
-    {0x90, 0x21},
-    {0x91, 0x0c},
-    {0x92, 0x04},
-    {0x93, 0x22},
-    {0x94, 0x00},
-    {0x95, 0x00},
-    {0x96, 0x00},
-    {0x97, 0xeb},
-    {0x98, 0x00},
-    {0x99, 0x00},
-    {0x9a, 0x00},
-    {0x9b, 0x00},
-    {0x9c, 0x00},
-    {0x9d, 0x00},
-    {0x9e, 0x00},
-    {0x9f, 0x00},
-    {0xa0, 0x00},
-    {0xa1, 0x00},
-    {0xa2, 0x00},
-    {0xa3, 0x00},
-    {0xa4, 0x00},
-    {0xa5, 0x00},
-    {0xa6, 0x10},
-    {0xa7, 0x04},
-    {0xa8, 0x10},
-    {0xa9, 0x00},
-    {0xaa, 0x8f},
-    {0xab, 0x00},
-    {0xac, 0x00},
-    {0xad, 0x00},
-    {0xae, 0x00},
-    {0xaf, 0x80},
-    {0xb0, 0x38},
-    {0xb1, 0x00},
-    {0xb2, 0x00},
-    {0xb3, 0x00},
-    {0xb4, 0xee},
-    {0xb5, 0xff},
-    {0xb6, 0x10},
-    {0xb7, 0x00},
-    {0xb8, 0x00},
-    {0xb9, 0x00},
-    {0xba, 0x00},
-    {0xbb, 0x03},
-    {0xbc, 0x00},
-    {0xbd, 0x00},
-    {0xbe, 0x00},
-    {0xbf, 0x00},
-    {0xc0, 0x10},
-    {0xc1, 0x10},
-    {0xc2, 0x18},
-    {0xc3, 0x20},
-    {0xc4, 0x10},
-    {0xc5, 0x00},
-    {0xc6, 0x22},
-    {0xc7, 0x14},
-    {0xc8, 0x0f},
-    {0xc9, 0x08},
-    {0xca, 0xa4},
-    {0xcb, 0xa7},
-    {0xcc, 0x3c},
-    {0xcd, 0x10},
-    {0xce, 0x20},
-    {0xcf, 0x00},
-    {0xd0, 0x00},
-    {0xd1, 0x10},
-    {0xd2, 0x00},
-    {0xd3, 0x00},
-    {0xd4, 0x10},
-    {0xd5, 0x33},
-    {0xd6, 0x70},
-    {0xd7, 0x01},
-    {0xd8, 0x00},
-    {0xd9, 0x00},
-    {0xda, 0x00},
-    {0xdb, 0x00},
-    {0xdc, 0x00},
-    {0xdd, 0x00},
-    {0xde, 0x00},
-    {0xdf, 0x00},
-    {0xe0, 0x00},
-    {0xe1, 0x00},
-    {0xe2, 0xcc},
-    {0xe3, 0x04},
-    {0xe4, 0x08},
-    {0xe5, 0x10},
-    {0xe6, 0x00},
-    {0xe7, 0x0e},
-    {0xe8, 0x88},
-    {0xe9, 0xd4},
-    {0xea, 0x05},
-    {0xeb, 0xf0},
-    {0xec, 0x79},
-    {0xed, 0x0f},
-    {0xee, 0x04},
-    {0xef, 0x04},
-    {0xf0, 0x00},
-    {0xf1, 0x00},
-    {0xf2, 0x00},
-    {0xf3, 0x00},
-    {0xf4, 0x00},
-    {0xf5, 0x00},
-    {0xf6, 0x00},
-    {0xf7, 0x00},
-    {0xf8, 0x00},
-    {0xf9, 0x00},
-    {0xF0, 0x00},
-    {0xF1, 0xF8},
-    {0xF0, 0x80},
-    {0xF0, 0x00},
-    {0xF1, 0xF4},
-    {0xF0, 0x81},
-    {0xF0, 0x01},
-    {0xF1, 0xF0},
-    {0xF0, 0x82},
-    {0xF0, 0x02},
-    {0xF1, 0xEC},
-    {0xF0, 0x83},
-    {0xF0, 0x03},
-    {0xF1, 0xE8},
-    {0xF0, 0x84},
-    {0xF0, 0x04},
-    {0xF1, 0xE4},
-    {0xF0, 0x85},
-    {0xF0, 0x05},
-    {0xF1, 0xE0},
-    {0xF0, 0x86},
-    {0xF0, 0x06},
-    {0xF1, 0xDC},
-    {0xF0, 0x87},
-    {0xF0, 0x07},
-    {0xF1, 0xD8},
-    {0xF0, 0x88},
-    {0xF0, 0x08},
-    {0xF1, 0xD4},
-    {0xF0, 0x89},
-    {0xF0, 0x09},
-    {0xF1, 0xD0},
-    {0xF0, 0x8A},
-    {0xF0, 0x0A},
-    {0xF1, 0xCC},
-    {0xF0, 0x8B},
-    {0xF0, 0x0B},
-    {0xF1, 0xC8},
-    {0xF0, 0x8C},
-    {0xF0, 0x0C},
-    {0xF1, 0xC4},
-    {0xF0, 0x8D},
-    {0xF0, 0x0D},
-    {0xF1, 0xC0},
-    {0xF0, 0x8E},
-    {0xF0, 0x0E},
-    {0xF1, 0xBC},
-    {0xF0, 0x8F},
-    {0xF0, 0x0F},
-    {0xF1, 0xB8},
-    {0xF0, 0x90},
-    {0xF0, 0x10},
-    {0xF1, 0xB4},
-    {0xF0, 0x91},
-    {0xF0, 0x11},
-    {0xF1, 0xB0},
-    {0xF0, 0x92},
-    {0xF0, 0x12},
-    {0xF1, 0xAC},
-    {0xF0, 0x93},
-    {0xF0, 0x13},
-    {0xF1, 0xA8},
-    {0xF0, 0x94},
-    {0xF0, 0x14},
-    {0xF1, 0xA4},
-    {0xF0, 0x95},
-    {0xF0, 0x15},
-    {0xF1, 0xA0},
-    {0xF0, 0x96},
-    {0xF0, 0x16},
-    {0xF1, 0x9C},
-    {0xF0, 0x97},
-    {0xF0, 0x17},
-    {0xF1, 0x98},
-    {0xF0, 0x98},
-    {0xF0, 0x18},
-    {0xF1, 0x94},
-    {0xF0, 0x99},
-    {0xF0, 0x19},
-    {0xF1, 0x90},
-    {0xF0, 0x9A},
-    {0xF0, 0x1A},
-    {0xF1, 0x8C},
-    {0xF0, 0x9B},
-    {0xF0, 0x1B},
-    {0xF1, 0x88},
-    {0xF0, 0x9C},
-    {0xF0, 0x1C},
-    {0xF1, 0x84},
-    {0xF0, 0x9D},
-    {0xF0, 0x1D},
-    {0xF1, 0x80},
-    {0xF0, 0x9E},
-    {0xF0, 0x1E},
-    {0xF1, 0x7C},
-    {0xF0, 0x9F},
-    {0xF0, 0x1F},
-    {0xF1, 0x78},
-    {0xF0, 0xA0},
-    {0xF0, 0x20},
-    {0xF1, 0x74},
-    {0xF0, 0xA1},
-    {0xF0, 0x21},
-    {0xF1, 0x70},
-    {0xF0, 0xA2},
-    {0xF0, 0x22},
-    {0xF1, 0x6C},
-    {0xF0, 0xA3},
-    {0xF0, 0x23},
-    {0xF1, 0x68},
-    {0xF0, 0xA4},
-    {0xF0, 0x24},
-    {0xF1, 0x64},
-    {0xF0, 0xA5},
-    {0xF0, 0x25},
-    {0xF1, 0x60},
-    {0xF0, 0xA6},
-    {0xF0, 0x26},
-    {0xF1, 0x5C},
-    {0xF0, 0xA7},
-    {0xF0, 0x27},
-    {0xF1, 0x58},
-    {0xF0, 0xA8},
-    {0xF0, 0x28},
-    {0xF1, 0x54},
-    {0xF0, 0xA9},
-    {0xF0, 0x29},
-    {0xF1, 0x50},
-    {0xF0, 0xAA},
-    {0xF0, 0x2A},
-    {0xF1, 0x4C},
-    {0xF0, 0xAB},
-    {0xF0, 0x2B},
-    {0xF1, 0x48},
-    {0xF0, 0xAC},
-    {0xF0, 0x2C},
-    {0xF1, 0x44},
-    {0xF0, 0xAD},
-    {0xF0, 0x2D},
-    {0xF1, 0x40},
-    {0xF0, 0xAE},
-    {0xF0, 0x2E},
-    {0xF1, 0x3C},
-    {0xF0, 0xAF},
-    {0xF0, 0x2F},
-    {0xF1, 0x38},
-    {0xF0, 0xB0},
-    {0xF0, 0x30},
-    {0xF1, 0x34},
-    {0xF0, 0xB1},
-    {0xF0, 0x31},
-    {0xF1, 0x30},
-    {0xF0, 0xB2},
-    {0xF0, 0x32},
-    {0xF1, 0x2C},
-    {0xF0, 0xB3},
-    {0xF0, 0x33},
-    {0xF1, 0x28},
-    {0xF0, 0xB4},
-    {0xF0, 0x34},
-    {0xF1, 0x24},
-    {0xF0, 0xB5},
-    {0xF0, 0x35},
-    {0xF1, 0x20},
-    {0xF0, 0xB6},
-    {0xF0, 0x36},
-    {0xF1, 0x1C},
-    {0xF0, 0xB7},
-    {0xF0, 0x37},
-    {0xF1, 0x18},
-    {0xF0, 0xB8},
-    {0xF0, 0x38},
-    {0xF1, 0x14},
-    {0xF0, 0xB9},
-    {0xF0, 0x39},
-    {0xF1, 0x10},
-    {0xF0, 0xBA},
-    {0xF0, 0x3A},
-    {0xF1, 0x0C},
-    {0xF0, 0xBB},
-    {0xF0, 0x3B},
-    {0xF1, 0x08},
-    {0xF0, 0x00},
-    {0xF0, 0x3C},
-    {0xF1, 0x04},
-    {0xF0, 0xBD},
-    {0xF0, 0x3D},
-    {0xF1, 0x00},
-    {0xF0, 0xBE},
-    {0xF0, 0x3E},
-    {0xF1, 0x00},
-    {0xF0, 0xBF},
-    {0xF0, 0x3F},
-    {0xF1, 0x00},
-    {0xF0, 0xC0},
-    {0xF0, 0x00},
+	{0x00, 0x30},
+	{0x01, 0x00},
+	{0x02, 0x00},
+	{0x03, 0x00},
+	{0x04, 0x00},
+	{0x05, 0x00},
+	{0x06, 0x00},
+	{0x07, 0x00},
+	{0x08, 0x70},
+	{0x09, 0x45},
+	{0x0a, 0x2a},
+	{0x0b, 0x76},
+	{0x0c, 0x00},
+	{0x0d, 0x01},
+	{0x0e, 0x80},
+	{0x0f, 0x00},
+	{0x10, 0x00},
+	{0x11, 0x00},
+	{0x12, 0x00},
+	{0x13, 0x00},
+	{0x14, 0x00},
+	{0x15, 0x00},
+	{0x16, 0x00},
+	{0x17, 0x00},
+	{0x18, 0x00},
+	{0x19, 0x00},
+	{0x1a, 0x00},
+	{0x1b, 0x9d},
+	{0x1c, 0x05},
+	{0x1d, 0x00},
+	{0x1e, 0x00},
+	{0x1f, 0x00},
+	{0x20, 0x00},
+	{0x21, 0x00},
+	{0x22, 0x00},
+	{0x23, 0x00},
+	{0x24, 0x00},
+	{0x25, 0x4a},
+	{0x26, 0x00},
+	{0x27, 0x00},
+	{0x28, 0x00},
+	{0x29, 0x00},
+	{0x2a, 0x00},
+	{0x2b, 0x00},
+	{0x2c, 0x00},
+	{0x2d, 0xa8},
+	{0x2e, 0x1a},
+	{0x2f, 0x0c},
+	{0x30, 0x26},
+	{0x31, 0x5b},
+	{0x32, 0x00},
+	{0x33, 0x00},
+	{0x34, 0x00},
+	{0x35, 0x00},
+	{0x36, 0xaa},
+	{0x37, 0xaa},
+	{0x38, 0xff},
+	{0x39, 0xff},
+	{0x3a, 0x00},
+	{0x3b, 0x00},
+	{0x3c, 0x00},
+	{0x3d, 0x0d},
+	{0x3e, 0x51},
+	{0x3f, 0x04},
+	{0x40, 0x00},
+	{0x41, 0x08},
+	{0x42, 0x00},
+	{0x43, 0x08},
+	{0x44, 0x06},
+	{0x45, 0x14},
+	{0x46, 0x05},
+	{0x47, 0x08},
+	{0x48, 0x00},
+	{0x49, 0x00},
+	{0x4a, 0x00},
+	{0x4b, 0x00},
+	{0x4c, 0x09},
+	{0x4d, 0x80},
+	{0x4e, 0x00},
+	{0x4f, 0xc5},
+	{0x50, 0x14},
+	{0x51, 0x19},
+	{0x52, 0x00},
+	{0x53, 0x00},
+	{0x54, 0x00},
+	{0x55, 0x00},
+	{0x56, 0x00},
+	{0x57, 0x00},
+	{0x58, 0x00},
+	{0x59, 0xb0},
+	{0x5a, 0x00},
+	{0x5b, 0x00},
+	{0x5c, 0x00},
+	{0x5d, 0x00},
+	{0x5e, 0x00},
+	{0x5f, 0x00},
+	{0x60, 0x44},
+	{0x61, 0x04},
+	{0x62, 0x00},
+	{0x63, 0x00},
+	{0x64, 0x00},
+	{0x65, 0x00},
+	{0x66, 0x04},
+	{0x67, 0xb7},
+	{0x68, 0x00},
+	{0x69, 0x00},
+	{0x6a, 0x00},
+	{0x6b, 0x00},
+	{0x6c, 0x00},
+	{0x6d, 0x03},
+	{0x6e, 0x01},
+	{0x6f, 0x00},
+	{0x70, 0x00},
+	{0x71, 0x00},
+	{0x72, 0x00},
+	{0x73, 0x00},
+	{0x74, 0x00},
+	{0x75, 0x00},
+	{0x76, 0x00},
+	{0x77, 0x00},
+	{0x78, 0x00},
+	{0x79, 0x00},
+	{0x7a, 0x00},
+	{0x7b, 0x00},
+	{0x7c, 0x00},
+	{0x7d, 0x00},
+	{0x7e, 0x00},
+	{0x7f, 0x00},
+	{0x80, 0x0b},
+	{0x81, 0x00},
+	{0x82, 0x3c},
+	{0x83, 0x00},
+	{0x84, 0x00},
+	{0x85, 0x00},
+	{0x86, 0x00},
+	{0x87, 0x00},
+	{0x88, 0x08},
+	{0x89, 0x00},
+	{0x8a, 0x08},
+	{0x8b, 0xa6},
+	{0x8c, 0x84},
+	{0x8d, 0x47},
+	{0x8e, 0xbb},
+	{0x8f, 0x02},
+	{0x90, 0x21},
+	{0x91, 0x0c},
+	{0x92, 0x04},
+	{0x93, 0x22},
+	{0x94, 0x00},
+	{0x95, 0x00},
+	{0x96, 0x00},
+	{0x97, 0xeb},
+	{0x98, 0x00},
+	{0x99, 0x00},
+	{0x9a, 0x00},
+	{0x9b, 0x00},
+	{0x9c, 0x00},
+	{0x9d, 0x00},
+	{0x9e, 0x00},
+	{0x9f, 0x00},
+	{0xa0, 0x00},
+	{0xa1, 0x00},
+	{0xa2, 0x00},
+	{0xa3, 0x00},
+	{0xa4, 0x00},
+	{0xa5, 0x00},
+	{0xa6, 0x10},
+	{0xa7, 0x04},
+	{0xa8, 0x10},
+	{0xa9, 0x00},
+	{0xaa, 0x8f},
+	{0xab, 0x00},
+	{0xac, 0x00},
+	{0xad, 0x00},
+	{0xae, 0x00},
+	{0xaf, 0x80},
+	{0xb0, 0x38},
+	{0xb1, 0x00},
+	{0xb2, 0x00},
+	{0xb3, 0x00},
+	{0xb4, 0xee},
+	{0xb5, 0xff},
+	{0xb6, 0x10},
+	{0xb7, 0x00},
+	{0xb8, 0x00},
+	{0xb9, 0x00},
+	{0xba, 0x00},
+	{0xbb, 0x03},
+	{0xbc, 0x00},
+	{0xbd, 0x00},
+	{0xbe, 0x00},
+	{0xbf, 0x00},
+	{0xc0, 0x10},
+	{0xc1, 0x10},
+	{0xc2, 0x18},
+	{0xc3, 0x20},
+	{0xc4, 0x10},
+	{0xc5, 0x00},
+	{0xc6, 0x22},
+	{0xc7, 0x14},
+	{0xc8, 0x0f},
+	{0xc9, 0x08},
+	{0xca, 0xa4},
+	{0xcb, 0xa7},
+	{0xcc, 0x3c},
+	{0xcd, 0x10},
+	{0xce, 0x20},
+	{0xcf, 0x00},
+	{0xd0, 0x00},
+	{0xd1, 0x10},
+	{0xd2, 0x00},
+	{0xd3, 0x00},
+	{0xd4, 0x10},
+	{0xd5, 0x33},
+	{0xd6, 0x70},
+	{0xd7, 0x01},
+	{0xd8, 0x00},
+	{0xd9, 0x00},
+	{0xda, 0x00},
+	{0xdb, 0x00},
+	{0xdc, 0x00},
+	{0xdd, 0x00},
+	{0xde, 0x00},
+	{0xdf, 0x00},
+	{0xe0, 0x00},
+	{0xe1, 0x00},
+	{0xe2, 0xcc},
+	{0xe3, 0x04},
+	{0xe4, 0x08},
+	{0xe5, 0x10},
+	{0xe6, 0x00},
+	{0xe7, 0x0e},
+	{0xe8, 0x88},
+	{0xe9, 0xd4},
+	{0xea, 0x05},
+	{0xeb, 0xf0},
+	{0xec, 0x79},
+	{0xed, 0x0f},
+	{0xee, 0x04},
+	{0xef, 0x04},
+	{0xf0, 0x00},
+	{0xf1, 0x00},
+	{0xf2, 0x00},
+	{0xf3, 0x00},
+	{0xf4, 0x00},
+	{0xf5, 0x00},
+	{0xf6, 0x00},
+	{0xf7, 0x00},
+	{0xf8, 0x00},
+	{0xf9, 0x00},
+	{0xF0, 0x00},
+	{0xF1, 0xF8},
+	{0xF0, 0x80},
+	{0xF0, 0x00},
+	{0xF1, 0xF4},
+	{0xF0, 0x81},
+	{0xF0, 0x01},
+	{0xF1, 0xF0},
+	{0xF0, 0x82},
+	{0xF0, 0x02},
+	{0xF1, 0xEC},
+	{0xF0, 0x83},
+	{0xF0, 0x03},
+	{0xF1, 0xE8},
+	{0xF0, 0x84},
+	{0xF0, 0x04},
+	{0xF1, 0xE4},
+	{0xF0, 0x85},
+	{0xF0, 0x05},
+	{0xF1, 0xE0},
+	{0xF0, 0x86},
+	{0xF0, 0x06},
+	{0xF1, 0xDC},
+	{0xF0, 0x87},
+	{0xF0, 0x07},
+	{0xF1, 0xD8},
+	{0xF0, 0x88},
+	{0xF0, 0x08},
+	{0xF1, 0xD4},
+	{0xF0, 0x89},
+	{0xF0, 0x09},
+	{0xF1, 0xD0},
+	{0xF0, 0x8A},
+	{0xF0, 0x0A},
+	{0xF1, 0xCC},
+	{0xF0, 0x8B},
+	{0xF0, 0x0B},
+	{0xF1, 0xC8},
+	{0xF0, 0x8C},
+	{0xF0, 0x0C},
+	{0xF1, 0xC4},
+	{0xF0, 0x8D},
+	{0xF0, 0x0D},
+	{0xF1, 0xC0},
+	{0xF0, 0x8E},
+	{0xF0, 0x0E},
+	{0xF1, 0xBC},
+	{0xF0, 0x8F},
+	{0xF0, 0x0F},
+	{0xF1, 0xB8},
+	{0xF0, 0x90},
+	{0xF0, 0x10},
+	{0xF1, 0xB4},
+	{0xF0, 0x91},
+	{0xF0, 0x11},
+	{0xF1, 0xB0},
+	{0xF0, 0x92},
+	{0xF0, 0x12},
+	{0xF1, 0xAC},
+	{0xF0, 0x93},
+	{0xF0, 0x13},
+	{0xF1, 0xA8},
+	{0xF0, 0x94},
+	{0xF0, 0x14},
+	{0xF1, 0xA4},
+	{0xF0, 0x95},
+	{0xF0, 0x15},
+	{0xF1, 0xA0},
+	{0xF0, 0x96},
+	{0xF0, 0x16},
+	{0xF1, 0x9C},
+	{0xF0, 0x97},
+	{0xF0, 0x17},
+	{0xF1, 0x98},
+	{0xF0, 0x98},
+	{0xF0, 0x18},
+	{0xF1, 0x94},
+	{0xF0, 0x99},
+	{0xF0, 0x19},
+	{0xF1, 0x90},
+	{0xF0, 0x9A},
+	{0xF0, 0x1A},
+	{0xF1, 0x8C},
+	{0xF0, 0x9B},
+	{0xF0, 0x1B},
+	{0xF1, 0x88},
+	{0xF0, 0x9C},
+	{0xF0, 0x1C},
+	{0xF1, 0x84},
+	{0xF0, 0x9D},
+	{0xF0, 0x1D},
+	{0xF1, 0x80},
+	{0xF0, 0x9E},
+	{0xF0, 0x1E},
+	{0xF1, 0x7C},
+	{0xF0, 0x9F},
+	{0xF0, 0x1F},
+	{0xF1, 0x78},
+	{0xF0, 0xA0},
+	{0xF0, 0x20},
+	{0xF1, 0x74},
+	{0xF0, 0xA1},
+	{0xF0, 0x21},
+	{0xF1, 0x70},
+	{0xF0, 0xA2},
+	{0xF0, 0x22},
+	{0xF1, 0x6C},
+	{0xF0, 0xA3},
+	{0xF0, 0x23},
+	{0xF1, 0x68},
+	{0xF0, 0xA4},
+	{0xF0, 0x24},
+	{0xF1, 0x64},
+	{0xF0, 0xA5},
+	{0xF0, 0x25},
+	{0xF1, 0x60},
+	{0xF0, 0xA6},
+	{0xF0, 0x26},
+	{0xF1, 0x5C},
+	{0xF0, 0xA7},
+	{0xF0, 0x27},
+	{0xF1, 0x58},
+	{0xF0, 0xA8},
+	{0xF0, 0x28},
+	{0xF1, 0x54},
+	{0xF0, 0xA9},
+	{0xF0, 0x29},
+	{0xF1, 0x50},
+	{0xF0, 0xAA},
+	{0xF0, 0x2A},
+	{0xF1, 0x4C},
+	{0xF0, 0xAB},
+	{0xF0, 0x2B},
+	{0xF1, 0x48},
+	{0xF0, 0xAC},
+	{0xF0, 0x2C},
+	{0xF1, 0x44},
+	{0xF0, 0xAD},
+	{0xF0, 0x2D},
+	{0xF1, 0x40},
+	{0xF0, 0xAE},
+	{0xF0, 0x2E},
+	{0xF1, 0x3C},
+	{0xF0, 0xAF},
+	{0xF0, 0x2F},
+	{0xF1, 0x38},
+	{0xF0, 0xB0},
+	{0xF0, 0x30},
+	{0xF1, 0x34},
+	{0xF0, 0xB1},
+	{0xF0, 0x31},
+	{0xF1, 0x30},
+	{0xF0, 0xB2},
+	{0xF0, 0x32},
+	{0xF1, 0x2C},
+	{0xF0, 0xB3},
+	{0xF0, 0x33},
+	{0xF1, 0x28},
+	{0xF0, 0xB4},
+	{0xF0, 0x34},
+	{0xF1, 0x24},
+	{0xF0, 0xB5},
+	{0xF0, 0x35},
+	{0xF1, 0x20},
+	{0xF0, 0xB6},
+	{0xF0, 0x36},
+	{0xF1, 0x1C},
+	{0xF0, 0xB7},
+	{0xF0, 0x37},
+	{0xF1, 0x18},
+	{0xF0, 0xB8},
+	{0xF0, 0x38},
+	{0xF1, 0x14},
+	{0xF0, 0xB9},
+	{0xF0, 0x39},
+	{0xF1, 0x10},
+	{0xF0, 0xBA},
+	{0xF0, 0x3A},
+	{0xF1, 0x0C},
+	{0xF0, 0xBB},
+	{0xF0, 0x3B},
+	{0xF1, 0x08},
+	{0xF0, 0x00},
+	{0xF0, 0x3C},
+	{0xF1, 0x04},
+	{0xF0, 0xBD},
+	{0xF0, 0x3D},
+	{0xF1, 0x00},
+	{0xF0, 0xBE},
+	{0xF0, 0x3E},
+	{0xF1, 0x00},
+	{0xF0, 0xBF},
+	{0xF0, 0x3F},
+	{0xF1, 0x00},
+	{0xF0, 0xC0},
+	{0xF0, 0x00},
 };
 
 #define CB_VT3253B0_INIT_FOR_RFMD 256
 unsigned char byVT3253B0_RFMD[CB_VT3253B0_INIT_FOR_RFMD][2] = {
-    {0x00, 0x31},
-    {0x01, 0x00},
-    {0x02, 0x00},
-    {0x03, 0x00},
-    {0x04, 0x00},
-    {0x05, 0x81},
-    {0x06, 0x00},
-    {0x07, 0x00},
-    {0x08, 0x38},
-    {0x09, 0x45},
-    {0x0a, 0x2a},
-    {0x0b, 0x76},
-    {0x0c, 0x00},
-    {0x0d, 0x00},
-    {0x0e, 0x80},
-    {0x0f, 0x00},
-    {0x10, 0x00},
-    {0x11, 0x00},
-    {0x12, 0x00},
-    {0x13, 0x00},
-    {0x14, 0x00},
-    {0x15, 0x00},
-    {0x16, 0x00},
-    {0x17, 0x00},
-    {0x18, 0x00},
-    {0x19, 0x00},
-    {0x1a, 0x00},
-    {0x1b, 0x8e},
-    {0x1c, 0x06},
-    {0x1d, 0x00},
-    {0x1e, 0x00},
-    {0x1f, 0x00},
-    {0x20, 0x00},
-    {0x21, 0x00},
-    {0x22, 0x00},
-    {0x23, 0x00},
-    {0x24, 0x00},
-    {0x25, 0x4a},
-    {0x26, 0x00},
-    {0x27, 0x00},
-    {0x28, 0x00},
-    {0x29, 0x00},
-    {0x2a, 0x00},
-    {0x2b, 0x00},
-    {0x2c, 0x00},
-    {0x2d, 0x34},
-    {0x2e, 0x18},
-    {0x2f, 0x0c},
-    {0x30, 0x26},
-    {0x31, 0x5b},
-    {0x32, 0x00},
-    {0x33, 0x00},
-    {0x34, 0x00},
-    {0x35, 0x00},
-    {0x36, 0xaa},
-    {0x37, 0xaa},
-    {0x38, 0xff},
-    {0x39, 0xff},
-    {0x3a, 0xf8},
-    {0x3b, 0x00},
-    {0x3c, 0x00},
-    {0x3d, 0x09},
-    {0x3e, 0x0d},
-    {0x3f, 0x04},
-    {0x40, 0x00},
-    {0x41, 0x08},
-    {0x42, 0x00},
-    {0x43, 0x08},
-    {0x44, 0x08},
-    {0x45, 0x14},
-    {0x46, 0x05},
-    {0x47, 0x08},
-    {0x48, 0x00},
-    {0x49, 0x00},
-    {0x4a, 0x00},
-    {0x4b, 0x00},
-    {0x4c, 0x09},
-    {0x4d, 0x80},
-    {0x4e, 0x00},
-    {0x4f, 0xc5},
-    {0x50, 0x14},
-    {0x51, 0x19},
-    {0x52, 0x00},
-    {0x53, 0x00},
-    {0x54, 0x00},
-    {0x55, 0x00},
-    {0x56, 0x00},
-    {0x57, 0x00},
-    {0x58, 0x00},
-    {0x59, 0xb0},
-    {0x5a, 0x00},
-    {0x5b, 0x00},
-    {0x5c, 0x00},
-    {0x5d, 0x00},
-    {0x5e, 0x00},
-    {0x5f, 0x00},
-    {0x60, 0x39},
-    {0x61, 0x83},
-    {0x62, 0x00},
-    {0x63, 0x00},
-    {0x64, 0x00},
-    {0x65, 0x00},
-    {0x66, 0xc0},
-    {0x67, 0x49},
-    {0x68, 0x00},
-    {0x69, 0x00},
-    {0x6a, 0x00},
-    {0x6b, 0x00},
-    {0x6c, 0x00},
-    {0x6d, 0x03},
-    {0x6e, 0x01},
-    {0x6f, 0x00},
-    {0x70, 0x00},
-    {0x71, 0x00},
-    {0x72, 0x00},
-    {0x73, 0x00},
-    {0x74, 0x00},
-    {0x75, 0x00},
-    {0x76, 0x00},
-    {0x77, 0x00},
-    {0x78, 0x00},
-    {0x79, 0x00},
-    {0x7a, 0x00},
-    {0x7b, 0x00},
-    {0x7c, 0x00},
-    {0x7d, 0x00},
-    {0x7e, 0x00},
-    {0x7f, 0x00},
-    {0x80, 0x89},
-    {0x81, 0x00},
-    {0x82, 0x0e},
-    {0x83, 0x00},
-    {0x84, 0x00},
-    {0x85, 0x00},
-    {0x86, 0x00},
-    {0x87, 0x00},
-    {0x88, 0x08},
-    {0x89, 0x00},
-    {0x8a, 0x0e},
-    {0x8b, 0xa7},
-    {0x8c, 0x88},
-    {0x8d, 0x47},
-    {0x8e, 0xaa},
-    {0x8f, 0x02},
-    {0x90, 0x23},
-    {0x91, 0x0c},
-    {0x92, 0x06},
-    {0x93, 0x08},
-    {0x94, 0x00},
-    {0x95, 0x00},
-    {0x96, 0x00},
-    {0x97, 0xeb},
-    {0x98, 0x00},
-    {0x99, 0x00},
-    {0x9a, 0x00},
-    {0x9b, 0x00},
-    {0x9c, 0x00},
-    {0x9d, 0x00},
-    {0x9e, 0x00},
-    {0x9f, 0x00},
-    {0xa0, 0x00},
-    {0xa1, 0x00},
-    {0xa2, 0x00},
-    {0xa3, 0xcd},
-    {0xa4, 0x07},
-    {0xa5, 0x33},
-    {0xa6, 0x18},
-    {0xa7, 0x00},
-    {0xa8, 0x18},
-    {0xa9, 0x00},
-    {0xaa, 0x28},
-    {0xab, 0x00},
-    {0xac, 0x00},
-    {0xad, 0x00},
-    {0xae, 0x00},
-    {0xaf, 0x18},
-    {0xb0, 0x38},
-    {0xb1, 0x30},
-    {0xb2, 0x00},
-    {0xb3, 0x00},
-    {0xb4, 0x00},
-    {0xb5, 0x00},
-    {0xb6, 0x84},
-    {0xb7, 0xfd},
-    {0xb8, 0x00},
-    {0xb9, 0x00},
-    {0xba, 0x00},
-    {0xbb, 0x03},
-    {0xbc, 0x00},
-    {0xbd, 0x00},
-    {0xbe, 0x00},
-    {0xbf, 0x00},
-    {0xc0, 0x10},
-    {0xc1, 0x20},
-    {0xc2, 0x18},
-    {0xc3, 0x20},
-    {0xc4, 0x10},
-    {0xc5, 0x2c},
-    {0xc6, 0x1e},
-    {0xc7, 0x10},
-    {0xc8, 0x12},
-    {0xc9, 0x01},
-    {0xca, 0x6f},
-    {0xcb, 0xa7},
-    {0xcc, 0x3c},
-    {0xcd, 0x10},
-    {0xce, 0x00},
-    {0xcf, 0x22},
-    {0xd0, 0x00},
-    {0xd1, 0x10},
-    {0xd2, 0x00},
-    {0xd3, 0x00},
-    {0xd4, 0x10},
-    {0xd5, 0x33},
-    {0xd6, 0x80},
-    {0xd7, 0x21},
-    {0xd8, 0x00},
-    {0xd9, 0x00},
-    {0xda, 0x00},
-    {0xdb, 0x00},
-    {0xdc, 0x00},
-    {0xdd, 0x00},
-    {0xde, 0x00},
-    {0xdf, 0x00},
-    {0xe0, 0x00},
-    {0xe1, 0xB3},
-    {0xe2, 0x00},
-    {0xe3, 0x00},
-    {0xe4, 0x00},
-    {0xe5, 0x10},
-    {0xe6, 0x00},
-    {0xe7, 0x18},
-    {0xe8, 0x08},
-    {0xe9, 0xd4},
-    {0xea, 0x00},
-    {0xeb, 0xff},
-    {0xec, 0x79},
-    {0xed, 0x10},
-    {0xee, 0x30},
-    {0xef, 0x02},
-    {0xf0, 0x00},
-    {0xf1, 0x09},
-    {0xf2, 0x00},
-    {0xf3, 0x00},
-    {0xf4, 0x00},
-    {0xf5, 0x00},
-    {0xf6, 0x00},
-    {0xf7, 0x00},
-    {0xf8, 0x00},
-    {0xf9, 0x00},
-    {0xfa, 0x00},
-    {0xfb, 0x00},
-    {0xfc, 0x00},
-    {0xfd, 0x00},
-    {0xfe, 0x00},
-    {0xff, 0x00},
+	{0x00, 0x31},
+	{0x01, 0x00},
+	{0x02, 0x00},
+	{0x03, 0x00},
+	{0x04, 0x00},
+	{0x05, 0x81},
+	{0x06, 0x00},
+	{0x07, 0x00},
+	{0x08, 0x38},
+	{0x09, 0x45},
+	{0x0a, 0x2a},
+	{0x0b, 0x76},
+	{0x0c, 0x00},
+	{0x0d, 0x00},
+	{0x0e, 0x80},
+	{0x0f, 0x00},
+	{0x10, 0x00},
+	{0x11, 0x00},
+	{0x12, 0x00},
+	{0x13, 0x00},
+	{0x14, 0x00},
+	{0x15, 0x00},
+	{0x16, 0x00},
+	{0x17, 0x00},
+	{0x18, 0x00},
+	{0x19, 0x00},
+	{0x1a, 0x00},
+	{0x1b, 0x8e},
+	{0x1c, 0x06},
+	{0x1d, 0x00},
+	{0x1e, 0x00},
+	{0x1f, 0x00},
+	{0x20, 0x00},
+	{0x21, 0x00},
+	{0x22, 0x00},
+	{0x23, 0x00},
+	{0x24, 0x00},
+	{0x25, 0x4a},
+	{0x26, 0x00},
+	{0x27, 0x00},
+	{0x28, 0x00},
+	{0x29, 0x00},
+	{0x2a, 0x00},
+	{0x2b, 0x00},
+	{0x2c, 0x00},
+	{0x2d, 0x34},
+	{0x2e, 0x18},
+	{0x2f, 0x0c},
+	{0x30, 0x26},
+	{0x31, 0x5b},
+	{0x32, 0x00},
+	{0x33, 0x00},
+	{0x34, 0x00},
+	{0x35, 0x00},
+	{0x36, 0xaa},
+	{0x37, 0xaa},
+	{0x38, 0xff},
+	{0x39, 0xff},
+	{0x3a, 0xf8},
+	{0x3b, 0x00},
+	{0x3c, 0x00},
+	{0x3d, 0x09},
+	{0x3e, 0x0d},
+	{0x3f, 0x04},
+	{0x40, 0x00},
+	{0x41, 0x08},
+	{0x42, 0x00},
+	{0x43, 0x08},
+	{0x44, 0x08},
+	{0x45, 0x14},
+	{0x46, 0x05},
+	{0x47, 0x08},
+	{0x48, 0x00},
+	{0x49, 0x00},
+	{0x4a, 0x00},
+	{0x4b, 0x00},
+	{0x4c, 0x09},
+	{0x4d, 0x80},
+	{0x4e, 0x00},
+	{0x4f, 0xc5},
+	{0x50, 0x14},
+	{0x51, 0x19},
+	{0x52, 0x00},
+	{0x53, 0x00},
+	{0x54, 0x00},
+	{0x55, 0x00},
+	{0x56, 0x00},
+	{0x57, 0x00},
+	{0x58, 0x00},
+	{0x59, 0xb0},
+	{0x5a, 0x00},
+	{0x5b, 0x00},
+	{0x5c, 0x00},
+	{0x5d, 0x00},
+	{0x5e, 0x00},
+	{0x5f, 0x00},
+	{0x60, 0x39},
+	{0x61, 0x83},
+	{0x62, 0x00},
+	{0x63, 0x00},
+	{0x64, 0x00},
+	{0x65, 0x00},
+	{0x66, 0xc0},
+	{0x67, 0x49},
+	{0x68, 0x00},
+	{0x69, 0x00},
+	{0x6a, 0x00},
+	{0x6b, 0x00},
+	{0x6c, 0x00},
+	{0x6d, 0x03},
+	{0x6e, 0x01},
+	{0x6f, 0x00},
+	{0x70, 0x00},
+	{0x71, 0x00},
+	{0x72, 0x00},
+	{0x73, 0x00},
+	{0x74, 0x00},
+	{0x75, 0x00},
+	{0x76, 0x00},
+	{0x77, 0x00},
+	{0x78, 0x00},
+	{0x79, 0x00},
+	{0x7a, 0x00},
+	{0x7b, 0x00},
+	{0x7c, 0x00},
+	{0x7d, 0x00},
+	{0x7e, 0x00},
+	{0x7f, 0x00},
+	{0x80, 0x89},
+	{0x81, 0x00},
+	{0x82, 0x0e},
+	{0x83, 0x00},
+	{0x84, 0x00},
+	{0x85, 0x00},
+	{0x86, 0x00},
+	{0x87, 0x00},
+	{0x88, 0x08},
+	{0x89, 0x00},
+	{0x8a, 0x0e},
+	{0x8b, 0xa7},
+	{0x8c, 0x88},
+	{0x8d, 0x47},
+	{0x8e, 0xaa},
+	{0x8f, 0x02},
+	{0x90, 0x23},
+	{0x91, 0x0c},
+	{0x92, 0x06},
+	{0x93, 0x08},
+	{0x94, 0x00},
+	{0x95, 0x00},
+	{0x96, 0x00},
+	{0x97, 0xeb},
+	{0x98, 0x00},
+	{0x99, 0x00},
+	{0x9a, 0x00},
+	{0x9b, 0x00},
+	{0x9c, 0x00},
+	{0x9d, 0x00},
+	{0x9e, 0x00},
+	{0x9f, 0x00},
+	{0xa0, 0x00},
+	{0xa1, 0x00},
+	{0xa2, 0x00},
+	{0xa3, 0xcd},
+	{0xa4, 0x07},
+	{0xa5, 0x33},
+	{0xa6, 0x18},
+	{0xa7, 0x00},
+	{0xa8, 0x18},
+	{0xa9, 0x00},
+	{0xaa, 0x28},
+	{0xab, 0x00},
+	{0xac, 0x00},
+	{0xad, 0x00},
+	{0xae, 0x00},
+	{0xaf, 0x18},
+	{0xb0, 0x38},
+	{0xb1, 0x30},
+	{0xb2, 0x00},
+	{0xb3, 0x00},
+	{0xb4, 0x00},
+	{0xb5, 0x00},
+	{0xb6, 0x84},
+	{0xb7, 0xfd},
+	{0xb8, 0x00},
+	{0xb9, 0x00},
+	{0xba, 0x00},
+	{0xbb, 0x03},
+	{0xbc, 0x00},
+	{0xbd, 0x00},
+	{0xbe, 0x00},
+	{0xbf, 0x00},
+	{0xc0, 0x10},
+	{0xc1, 0x20},
+	{0xc2, 0x18},
+	{0xc3, 0x20},
+	{0xc4, 0x10},
+	{0xc5, 0x2c},
+	{0xc6, 0x1e},
+	{0xc7, 0x10},
+	{0xc8, 0x12},
+	{0xc9, 0x01},
+	{0xca, 0x6f},
+	{0xcb, 0xa7},
+	{0xcc, 0x3c},
+	{0xcd, 0x10},
+	{0xce, 0x00},
+	{0xcf, 0x22},
+	{0xd0, 0x00},
+	{0xd1, 0x10},
+	{0xd2, 0x00},
+	{0xd3, 0x00},
+	{0xd4, 0x10},
+	{0xd5, 0x33},
+	{0xd6, 0x80},
+	{0xd7, 0x21},
+	{0xd8, 0x00},
+	{0xd9, 0x00},
+	{0xda, 0x00},
+	{0xdb, 0x00},
+	{0xdc, 0x00},
+	{0xdd, 0x00},
+	{0xde, 0x00},
+	{0xdf, 0x00},
+	{0xe0, 0x00},
+	{0xe1, 0xB3},
+	{0xe2, 0x00},
+	{0xe3, 0x00},
+	{0xe4, 0x00},
+	{0xe5, 0x10},
+	{0xe6, 0x00},
+	{0xe7, 0x18},
+	{0xe8, 0x08},
+	{0xe9, 0xd4},
+	{0xea, 0x00},
+	{0xeb, 0xff},
+	{0xec, 0x79},
+	{0xed, 0x10},
+	{0xee, 0x30},
+	{0xef, 0x02},
+	{0xf0, 0x00},
+	{0xf1, 0x09},
+	{0xf2, 0x00},
+	{0xf3, 0x00},
+	{0xf4, 0x00},
+	{0xf5, 0x00},
+	{0xf6, 0x00},
+	{0xf7, 0x00},
+	{0xf8, 0x00},
+	{0xf9, 0x00},
+	{0xfa, 0x00},
+	{0xfb, 0x00},
+	{0xfc, 0x00},
+	{0xfd, 0x00},
+	{0xfe, 0x00},
+	{0xff, 0x00},
 };
 
 #define CB_VT3253B0_AGC_FOR_RFMD2959 195
 // For RFMD2959
 unsigned char byVT3253B0_AGC4_RFMD2959[CB_VT3253B0_AGC_FOR_RFMD2959][2] = {
-    {0xF0, 0x00},
-    {0xF1, 0x3E},
-    {0xF0, 0x80},
-    {0xF0, 0x00},
-    {0xF1, 0x3E},
-    {0xF0, 0x81},
-    {0xF0, 0x01},
-    {0xF1, 0x3E},
-    {0xF0, 0x82},
-    {0xF0, 0x02},
-    {0xF1, 0x3E},
-    {0xF0, 0x83},
-    {0xF0, 0x03},
-    {0xF1, 0x3B},
-    {0xF0, 0x84},
-    {0xF0, 0x04},
-    {0xF1, 0x39},
-    {0xF0, 0x85},
-    {0xF0, 0x05},
-    {0xF1, 0x38},
-    {0xF0, 0x86},
-    {0xF0, 0x06},
-    {0xF1, 0x37},
-    {0xF0, 0x87},
-    {0xF0, 0x07},
-    {0xF1, 0x36},
-    {0xF0, 0x88},
-    {0xF0, 0x08},
-    {0xF1, 0x35},
-    {0xF0, 0x89},
-    {0xF0, 0x09},
-    {0xF1, 0x35},
-    {0xF0, 0x8A},
-    {0xF0, 0x0A},
-    {0xF1, 0x34},
-    {0xF0, 0x8B},
-    {0xF0, 0x0B},
-    {0xF1, 0x34},
-    {0xF0, 0x8C},
-    {0xF0, 0x0C},
-    {0xF1, 0x33},
-    {0xF0, 0x8D},
-    {0xF0, 0x0D},
-    {0xF1, 0x32},
-    {0xF0, 0x8E},
-    {0xF0, 0x0E},
-    {0xF1, 0x31},
-    {0xF0, 0x8F},
-    {0xF0, 0x0F},
-    {0xF1, 0x30},
-    {0xF0, 0x90},
-    {0xF0, 0x10},
-    {0xF1, 0x2F},
-    {0xF0, 0x91},
-    {0xF0, 0x11},
-    {0xF1, 0x2F},
-    {0xF0, 0x92},
-    {0xF0, 0x12},
-    {0xF1, 0x2E},
-    {0xF0, 0x93},
-    {0xF0, 0x13},
-    {0xF1, 0x2D},
-    {0xF0, 0x94},
-    {0xF0, 0x14},
-    {0xF1, 0x2C},
-    {0xF0, 0x95},
-    {0xF0, 0x15},
-    {0xF1, 0x2B},
-    {0xF0, 0x96},
-    {0xF0, 0x16},
-    {0xF1, 0x2B},
-    {0xF0, 0x97},
-    {0xF0, 0x17},
-    {0xF1, 0x2A},
-    {0xF0, 0x98},
-    {0xF0, 0x18},
-    {0xF1, 0x29},
-    {0xF0, 0x99},
-    {0xF0, 0x19},
-    {0xF1, 0x28},
-    {0xF0, 0x9A},
-    {0xF0, 0x1A},
-    {0xF1, 0x27},
-    {0xF0, 0x9B},
-    {0xF0, 0x1B},
-    {0xF1, 0x26},
-    {0xF0, 0x9C},
-    {0xF0, 0x1C},
-    {0xF1, 0x25},
-    {0xF0, 0x9D},
-    {0xF0, 0x1D},
-    {0xF1, 0x24},
-    {0xF0, 0x9E},
-    {0xF0, 0x1E},
-    {0xF1, 0x24},
-    {0xF0, 0x9F},
-    {0xF0, 0x1F},
-    {0xF1, 0x23},
-    {0xF0, 0xA0},
-    {0xF0, 0x20},
-    {0xF1, 0x22},
-    {0xF0, 0xA1},
-    {0xF0, 0x21},
-    {0xF1, 0x21},
-    {0xF0, 0xA2},
-    {0xF0, 0x22},
-    {0xF1, 0x20},
-    {0xF0, 0xA3},
-    {0xF0, 0x23},
-    {0xF1, 0x20},
-    {0xF0, 0xA4},
-    {0xF0, 0x24},
-    {0xF1, 0x1F},
-    {0xF0, 0xA5},
-    {0xF0, 0x25},
-    {0xF1, 0x1E},
-    {0xF0, 0xA6},
-    {0xF0, 0x26},
-    {0xF1, 0x1D},
-    {0xF0, 0xA7},
-    {0xF0, 0x27},
-    {0xF1, 0x1C},
-    {0xF0, 0xA8},
-    {0xF0, 0x28},
-    {0xF1, 0x1B},
-    {0xF0, 0xA9},
-    {0xF0, 0x29},
-    {0xF1, 0x1B},
-    {0xF0, 0xAA},
-    {0xF0, 0x2A},
-    {0xF1, 0x1A},
-    {0xF0, 0xAB},
-    {0xF0, 0x2B},
-    {0xF1, 0x1A},
-    {0xF0, 0xAC},
-    {0xF0, 0x2C},
-    {0xF1, 0x19},
-    {0xF0, 0xAD},
-    {0xF0, 0x2D},
-    {0xF1, 0x18},
-    {0xF0, 0xAE},
-    {0xF0, 0x2E},
-    {0xF1, 0x17},
-    {0xF0, 0xAF},
-    {0xF0, 0x2F},
-    {0xF1, 0x16},
-    {0xF0, 0xB0},
-    {0xF0, 0x30},
-    {0xF1, 0x15},
-    {0xF0, 0xB1},
-    {0xF0, 0x31},
-    {0xF1, 0x15},
-    {0xF0, 0xB2},
-    {0xF0, 0x32},
-    {0xF1, 0x15},
-    {0xF0, 0xB3},
-    {0xF0, 0x33},
-    {0xF1, 0x14},
-    {0xF0, 0xB4},
-    {0xF0, 0x34},
-    {0xF1, 0x13},
-    {0xF0, 0xB5},
-    {0xF0, 0x35},
-    {0xF1, 0x12},
-    {0xF0, 0xB6},
-    {0xF0, 0x36},
-    {0xF1, 0x11},
-    {0xF0, 0xB7},
-    {0xF0, 0x37},
-    {0xF1, 0x10},
-    {0xF0, 0xB8},
-    {0xF0, 0x38},
-    {0xF1, 0x0F},
-    {0xF0, 0xB9},
-    {0xF0, 0x39},
-    {0xF1, 0x0E},
-    {0xF0, 0xBA},
-    {0xF0, 0x3A},
-    {0xF1, 0x0D},
-    {0xF0, 0xBB},
-    {0xF0, 0x3B},
-    {0xF1, 0x0C},
-    {0xF0, 0xBC},
-    {0xF0, 0x3C},
-    {0xF1, 0x0B},
-    {0xF0, 0xBD},
-    {0xF0, 0x3D},
-    {0xF1, 0x0B},
-    {0xF0, 0xBE},
-    {0xF0, 0x3E},
-    {0xF1, 0x0A},
-    {0xF0, 0xBF},
-    {0xF0, 0x3F},
-    {0xF1, 0x09},
-    {0xF0, 0x00},
+	{0xF0, 0x00},
+	{0xF1, 0x3E},
+	{0xF0, 0x80},
+	{0xF0, 0x00},
+	{0xF1, 0x3E},
+	{0xF0, 0x81},
+	{0xF0, 0x01},
+	{0xF1, 0x3E},
+	{0xF0, 0x82},
+	{0xF0, 0x02},
+	{0xF1, 0x3E},
+	{0xF0, 0x83},
+	{0xF0, 0x03},
+	{0xF1, 0x3B},
+	{0xF0, 0x84},
+	{0xF0, 0x04},
+	{0xF1, 0x39},
+	{0xF0, 0x85},
+	{0xF0, 0x05},
+	{0xF1, 0x38},
+	{0xF0, 0x86},
+	{0xF0, 0x06},
+	{0xF1, 0x37},
+	{0xF0, 0x87},
+	{0xF0, 0x07},
+	{0xF1, 0x36},
+	{0xF0, 0x88},
+	{0xF0, 0x08},
+	{0xF1, 0x35},
+	{0xF0, 0x89},
+	{0xF0, 0x09},
+	{0xF1, 0x35},
+	{0xF0, 0x8A},
+	{0xF0, 0x0A},
+	{0xF1, 0x34},
+	{0xF0, 0x8B},
+	{0xF0, 0x0B},
+	{0xF1, 0x34},
+	{0xF0, 0x8C},
+	{0xF0, 0x0C},
+	{0xF1, 0x33},
+	{0xF0, 0x8D},
+	{0xF0, 0x0D},
+	{0xF1, 0x32},
+	{0xF0, 0x8E},
+	{0xF0, 0x0E},
+	{0xF1, 0x31},
+	{0xF0, 0x8F},
+	{0xF0, 0x0F},
+	{0xF1, 0x30},
+	{0xF0, 0x90},
+	{0xF0, 0x10},
+	{0xF1, 0x2F},
+	{0xF0, 0x91},
+	{0xF0, 0x11},
+	{0xF1, 0x2F},
+	{0xF0, 0x92},
+	{0xF0, 0x12},
+	{0xF1, 0x2E},
+	{0xF0, 0x93},
+	{0xF0, 0x13},
+	{0xF1, 0x2D},
+	{0xF0, 0x94},
+	{0xF0, 0x14},
+	{0xF1, 0x2C},
+	{0xF0, 0x95},
+	{0xF0, 0x15},
+	{0xF1, 0x2B},
+	{0xF0, 0x96},
+	{0xF0, 0x16},
+	{0xF1, 0x2B},
+	{0xF0, 0x97},
+	{0xF0, 0x17},
+	{0xF1, 0x2A},
+	{0xF0, 0x98},
+	{0xF0, 0x18},
+	{0xF1, 0x29},
+	{0xF0, 0x99},
+	{0xF0, 0x19},
+	{0xF1, 0x28},
+	{0xF0, 0x9A},
+	{0xF0, 0x1A},
+	{0xF1, 0x27},
+	{0xF0, 0x9B},
+	{0xF0, 0x1B},
+	{0xF1, 0x26},
+	{0xF0, 0x9C},
+	{0xF0, 0x1C},
+	{0xF1, 0x25},
+	{0xF0, 0x9D},
+	{0xF0, 0x1D},
+	{0xF1, 0x24},
+	{0xF0, 0x9E},
+	{0xF0, 0x1E},
+	{0xF1, 0x24},
+	{0xF0, 0x9F},
+	{0xF0, 0x1F},
+	{0xF1, 0x23},
+	{0xF0, 0xA0},
+	{0xF0, 0x20},
+	{0xF1, 0x22},
+	{0xF0, 0xA1},
+	{0xF0, 0x21},
+	{0xF1, 0x21},
+	{0xF0, 0xA2},
+	{0xF0, 0x22},
+	{0xF1, 0x20},
+	{0xF0, 0xA3},
+	{0xF0, 0x23},
+	{0xF1, 0x20},
+	{0xF0, 0xA4},
+	{0xF0, 0x24},
+	{0xF1, 0x1F},
+	{0xF0, 0xA5},
+	{0xF0, 0x25},
+	{0xF1, 0x1E},
+	{0xF0, 0xA6},
+	{0xF0, 0x26},
+	{0xF1, 0x1D},
+	{0xF0, 0xA7},
+	{0xF0, 0x27},
+	{0xF1, 0x1C},
+	{0xF0, 0xA8},
+	{0xF0, 0x28},
+	{0xF1, 0x1B},
+	{0xF0, 0xA9},
+	{0xF0, 0x29},
+	{0xF1, 0x1B},
+	{0xF0, 0xAA},
+	{0xF0, 0x2A},
+	{0xF1, 0x1A},
+	{0xF0, 0xAB},
+	{0xF0, 0x2B},
+	{0xF1, 0x1A},
+	{0xF0, 0xAC},
+	{0xF0, 0x2C},
+	{0xF1, 0x19},
+	{0xF0, 0xAD},
+	{0xF0, 0x2D},
+	{0xF1, 0x18},
+	{0xF0, 0xAE},
+	{0xF0, 0x2E},
+	{0xF1, 0x17},
+	{0xF0, 0xAF},
+	{0xF0, 0x2F},
+	{0xF1, 0x16},
+	{0xF0, 0xB0},
+	{0xF0, 0x30},
+	{0xF1, 0x15},
+	{0xF0, 0xB1},
+	{0xF0, 0x31},
+	{0xF1, 0x15},
+	{0xF0, 0xB2},
+	{0xF0, 0x32},
+	{0xF1, 0x15},
+	{0xF0, 0xB3},
+	{0xF0, 0x33},
+	{0xF1, 0x14},
+	{0xF0, 0xB4},
+	{0xF0, 0x34},
+	{0xF1, 0x13},
+	{0xF0, 0xB5},
+	{0xF0, 0x35},
+	{0xF1, 0x12},
+	{0xF0, 0xB6},
+	{0xF0, 0x36},
+	{0xF1, 0x11},
+	{0xF0, 0xB7},
+	{0xF0, 0x37},
+	{0xF1, 0x10},
+	{0xF0, 0xB8},
+	{0xF0, 0x38},
+	{0xF1, 0x0F},
+	{0xF0, 0xB9},
+	{0xF0, 0x39},
+	{0xF1, 0x0E},
+	{0xF0, 0xBA},
+	{0xF0, 0x3A},
+	{0xF1, 0x0D},
+	{0xF0, 0xBB},
+	{0xF0, 0x3B},
+	{0xF1, 0x0C},
+	{0xF0, 0xBC},
+	{0xF0, 0x3C},
+	{0xF1, 0x0B},
+	{0xF0, 0xBD},
+	{0xF0, 0x3D},
+	{0xF1, 0x0B},
+	{0xF0, 0xBE},
+	{0xF0, 0x3E},
+	{0xF1, 0x0A},
+	{0xF0, 0xBF},
+	{0xF0, 0x3F},
+	{0xF1, 0x09},
+	{0xF0, 0x00},
 };
 
 #define CB_VT3253B0_INIT_FOR_AIROHA2230 256
 // For AIROHA
 unsigned char byVT3253B0_AIROHA2230[CB_VT3253B0_INIT_FOR_AIROHA2230][2] = {
-    {0x00, 0x31},
-    {0x01, 0x00},
-    {0x02, 0x00},
-    {0x03, 0x00},
-    {0x04, 0x00},
-    {0x05, 0x80},
-    {0x06, 0x00},
-    {0x07, 0x00},
-    {0x08, 0x70},
-    {0x09, 0x41},
-    {0x0a, 0x2A},
-    {0x0b, 0x76},
-    {0x0c, 0x00},
-    {0x0d, 0x00},
-    {0x0e, 0x80},
-    {0x0f, 0x00},
-    {0x10, 0x00},
-    {0x11, 0x00},
-    {0x12, 0x00},
-    {0x13, 0x00},
-    {0x14, 0x00},
-    {0x15, 0x00},
-    {0x16, 0x00},
-    {0x17, 0x00},
-    {0x18, 0x00},
-    {0x19, 0x00},
-    {0x1a, 0x00},
-    {0x1b, 0x8f},
-    {0x1c, 0x09},
-    {0x1d, 0x00},
-    {0x1e, 0x00},
-    {0x1f, 0x00},
-    {0x20, 0x00},
-    {0x21, 0x00},
-    {0x22, 0x00},
-    {0x23, 0x00},
-    {0x24, 0x00},
-    {0x25, 0x4a},
-    {0x26, 0x00},
-    {0x27, 0x00},
-    {0x28, 0x00},
-    {0x29, 0x00},
-    {0x2a, 0x00},
-    {0x2b, 0x00},
-    {0x2c, 0x00},
-    {0x2d, 0x4a},
-    {0x2e, 0x00},
-    {0x2f, 0x0a},
-    {0x30, 0x26},
-    {0x31, 0x5b},
-    {0x32, 0x00},
-    {0x33, 0x00},
-    {0x34, 0x00},
-    {0x35, 0x00},
-    {0x36, 0xaa},
-    {0x37, 0xaa},
-    {0x38, 0xff},
-    {0x39, 0xff},
-    {0x3a, 0x79},
-    {0x3b, 0x00},
-    {0x3c, 0x00},
-    {0x3d, 0x0b},
-    {0x3e, 0x48},
-    {0x3f, 0x04},
-    {0x40, 0x00},
-    {0x41, 0x08},
-    {0x42, 0x00},
-    {0x43, 0x08},
-    {0x44, 0x08},
-    {0x45, 0x14},
-    {0x46, 0x05},
-    {0x47, 0x09},
-    {0x48, 0x00},
-    {0x49, 0x00},
-    {0x4a, 0x00},
-    {0x4b, 0x00},
-    {0x4c, 0x09},
-    {0x4d, 0x73},
-    {0x4e, 0x00},
-    {0x4f, 0xc5},
-    {0x50, 0x15},
-    {0x51, 0x19},
-    {0x52, 0x00},
-    {0x53, 0x00},
-    {0x54, 0x00},
-    {0x55, 0x00},
-    {0x56, 0x00},
-    {0x57, 0x00},
-    {0x58, 0x00},
-    {0x59, 0xb0},
-    {0x5a, 0x00},
-    {0x5b, 0x00},
-    {0x5c, 0x00},
-    {0x5d, 0x00},
-    {0x5e, 0x00},
-    {0x5f, 0x00},
-    {0x60, 0xe4},
-    {0x61, 0x80},
-    {0x62, 0x00},
-    {0x63, 0x00},
-    {0x64, 0x00},
-    {0x65, 0x00},
-    {0x66, 0x98},
-    {0x67, 0x0a},
-    {0x68, 0x00},
-    {0x69, 0x00},
-    {0x6a, 0x00},
-    {0x6b, 0x00},
-    //{0x6c, 0x80},
-    {0x6c, 0x00}, //RobertYu:20050125, request by JJSue
-    {0x6d, 0x03},
-    {0x6e, 0x01},
-    {0x6f, 0x00},
-    {0x70, 0x00},
-    {0x71, 0x00},
-    {0x72, 0x00},
-    {0x73, 0x00},
-    {0x74, 0x00},
-    {0x75, 0x00},
-    {0x76, 0x00},
-    {0x77, 0x00},
-    {0x78, 0x00},
-    {0x79, 0x00},
-    {0x7a, 0x00},
-    {0x7b, 0x00},
-    {0x7c, 0x00},
-    {0x7d, 0x00},
-    {0x7e, 0x00},
-    {0x7f, 0x00},
-    {0x80, 0x8c},
-    {0x81, 0x01},
-    {0x82, 0x09},
-    {0x83, 0x00},
-    {0x84, 0x00},
-    {0x85, 0x00},
-    {0x86, 0x00},
-    {0x87, 0x00},
-    {0x88, 0x08},
-    {0x89, 0x00},
-    {0x8a, 0x0f},
-    {0x8b, 0xb7},
-    {0x8c, 0x88},
-    {0x8d, 0x47},
-    {0x8e, 0xaa},
-    {0x8f, 0x02},
-    {0x90, 0x22},
-    {0x91, 0x00},
-    {0x92, 0x00},
-    {0x93, 0x00},
-    {0x94, 0x00},
-    {0x95, 0x00},
-    {0x96, 0x00},
-    {0x97, 0xeb},
-    {0x98, 0x00},
-    {0x99, 0x00},
-    {0x9a, 0x00},
-    {0x9b, 0x00},
-    {0x9c, 0x00},
-    {0x9d, 0x00},
-    {0x9e, 0x00},
-    {0x9f, 0x01},
-    {0xa0, 0x00},
-    {0xa1, 0x00},
-    {0xa2, 0x00},
-    {0xa3, 0x00},
-    {0xa4, 0x00},
-    {0xa5, 0x00},
-    {0xa6, 0x10},
-    {0xa7, 0x00},
-    {0xa8, 0x18},
-    {0xa9, 0x00},
-    {0xaa, 0x00},
-    {0xab, 0x00},
-    {0xac, 0x00},
-    {0xad, 0x00},
-    {0xae, 0x00},
-    {0xaf, 0x18},
-    {0xb0, 0x38},
-    {0xb1, 0x30},
-    {0xb2, 0x00},
-    {0xb3, 0x00},
-    {0xb4, 0xff},
-    {0xb5, 0x0f},
-    {0xb6, 0xe4},
-    {0xb7, 0xe2},
-    {0xb8, 0x00},
-    {0xb9, 0x00},
-    {0xba, 0x00},
-    {0xbb, 0x03},
-    {0xbc, 0x01},
-    {0xbd, 0x00},
-    {0xbe, 0x00},
-    {0xbf, 0x00},
-    {0xc0, 0x18},
-    {0xc1, 0x20},
-    {0xc2, 0x07},
-    {0xc3, 0x18},
-    {0xc4, 0xff},
-    {0xc5, 0x2c},
-    {0xc6, 0x0c},
-    {0xc7, 0x0a},
-    {0xc8, 0x0e},
-    {0xc9, 0x01},
-    {0xca, 0x68},
-    {0xcb, 0xa7},
-    {0xcc, 0x3c},
-    {0xcd, 0x10},
-    {0xce, 0x00},
-    {0xcf, 0x25},
-    {0xd0, 0x40},
-    {0xd1, 0x12},
-    {0xd2, 0x00},
-    {0xd3, 0x00},
-    {0xd4, 0x10},
-    {0xd5, 0x28},
-    {0xd6, 0x80},
-    {0xd7, 0x2A},
-    {0xd8, 0x00},
-    {0xd9, 0x00},
-    {0xda, 0x00},
-    {0xdb, 0x00},
-    {0xdc, 0x00},
-    {0xdd, 0x00},
-    {0xde, 0x00},
-    {0xdf, 0x00},
-    {0xe0, 0x00},
-    {0xe1, 0xB3},
-    {0xe2, 0x00},
-    {0xe3, 0x00},
-    {0xe4, 0x00},
-    {0xe5, 0x10},
-    {0xe6, 0x00},
-    {0xe7, 0x1C},
-    {0xe8, 0x00},
-    {0xe9, 0xf4},
-    {0xea, 0x00},
-    {0xeb, 0xff},
-    {0xec, 0x79},
-    {0xed, 0x20},
-    {0xee, 0x30},
-    {0xef, 0x01},
-    {0xf0, 0x00},
-    {0xf1, 0x3e},
-    {0xf2, 0x00},
-    {0xf3, 0x00},
-    {0xf4, 0x00},
-    {0xf5, 0x00},
-    {0xf6, 0x00},
-    {0xf7, 0x00},
-    {0xf8, 0x00},
-    {0xf9, 0x00},
-    {0xfa, 0x00},
-    {0xfb, 0x00},
-    {0xfc, 0x00},
-    {0xfd, 0x00},
-    {0xfe, 0x00},
-    {0xff, 0x00},
+	{0x00, 0x31},
+	{0x01, 0x00},
+	{0x02, 0x00},
+	{0x03, 0x00},
+	{0x04, 0x00},
+	{0x05, 0x80},
+	{0x06, 0x00},
+	{0x07, 0x00},
+	{0x08, 0x70},
+	{0x09, 0x41},
+	{0x0a, 0x2A},
+	{0x0b, 0x76},
+	{0x0c, 0x00},
+	{0x0d, 0x00},
+	{0x0e, 0x80},
+	{0x0f, 0x00},
+	{0x10, 0x00},
+	{0x11, 0x00},
+	{0x12, 0x00},
+	{0x13, 0x00},
+	{0x14, 0x00},
+	{0x15, 0x00},
+	{0x16, 0x00},
+	{0x17, 0x00},
+	{0x18, 0x00},
+	{0x19, 0x00},
+	{0x1a, 0x00},
+	{0x1b, 0x8f},
+	{0x1c, 0x09},
+	{0x1d, 0x00},
+	{0x1e, 0x00},
+	{0x1f, 0x00},
+	{0x20, 0x00},
+	{0x21, 0x00},
+	{0x22, 0x00},
+	{0x23, 0x00},
+	{0x24, 0x00},
+	{0x25, 0x4a},
+	{0x26, 0x00},
+	{0x27, 0x00},
+	{0x28, 0x00},
+	{0x29, 0x00},
+	{0x2a, 0x00},
+	{0x2b, 0x00},
+	{0x2c, 0x00},
+	{0x2d, 0x4a},
+	{0x2e, 0x00},
+	{0x2f, 0x0a},
+	{0x30, 0x26},
+	{0x31, 0x5b},
+	{0x32, 0x00},
+	{0x33, 0x00},
+	{0x34, 0x00},
+	{0x35, 0x00},
+	{0x36, 0xaa},
+	{0x37, 0xaa},
+	{0x38, 0xff},
+	{0x39, 0xff},
+	{0x3a, 0x79},
+	{0x3b, 0x00},
+	{0x3c, 0x00},
+	{0x3d, 0x0b},
+	{0x3e, 0x48},
+	{0x3f, 0x04},
+	{0x40, 0x00},
+	{0x41, 0x08},
+	{0x42, 0x00},
+	{0x43, 0x08},
+	{0x44, 0x08},
+	{0x45, 0x14},
+	{0x46, 0x05},
+	{0x47, 0x09},
+	{0x48, 0x00},
+	{0x49, 0x00},
+	{0x4a, 0x00},
+	{0x4b, 0x00},
+	{0x4c, 0x09},
+	{0x4d, 0x73},
+	{0x4e, 0x00},
+	{0x4f, 0xc5},
+	{0x50, 0x15},
+	{0x51, 0x19},
+	{0x52, 0x00},
+	{0x53, 0x00},
+	{0x54, 0x00},
+	{0x55, 0x00},
+	{0x56, 0x00},
+	{0x57, 0x00},
+	{0x58, 0x00},
+	{0x59, 0xb0},
+	{0x5a, 0x00},
+	{0x5b, 0x00},
+	{0x5c, 0x00},
+	{0x5d, 0x00},
+	{0x5e, 0x00},
+	{0x5f, 0x00},
+	{0x60, 0xe4},
+	{0x61, 0x80},
+	{0x62, 0x00},
+	{0x63, 0x00},
+	{0x64, 0x00},
+	{0x65, 0x00},
+	{0x66, 0x98},
+	{0x67, 0x0a},
+	{0x68, 0x00},
+	{0x69, 0x00},
+	{0x6a, 0x00},
+	{0x6b, 0x00},
+	//{0x6c, 0x80},
+	{0x6c, 0x00}, //RobertYu:20050125, request by JJSue
+	{0x6d, 0x03},
+	{0x6e, 0x01},
+	{0x6f, 0x00},
+	{0x70, 0x00},
+	{0x71, 0x00},
+	{0x72, 0x00},
+	{0x73, 0x00},
+	{0x74, 0x00},
+	{0x75, 0x00},
+	{0x76, 0x00},
+	{0x77, 0x00},
+	{0x78, 0x00},
+	{0x79, 0x00},
+	{0x7a, 0x00},
+	{0x7b, 0x00},
+	{0x7c, 0x00},
+	{0x7d, 0x00},
+	{0x7e, 0x00},
+	{0x7f, 0x00},
+	{0x80, 0x8c},
+	{0x81, 0x01},
+	{0x82, 0x09},
+	{0x83, 0x00},
+	{0x84, 0x00},
+	{0x85, 0x00},
+	{0x86, 0x00},
+	{0x87, 0x00},
+	{0x88, 0x08},
+	{0x89, 0x00},
+	{0x8a, 0x0f},
+	{0x8b, 0xb7},
+	{0x8c, 0x88},
+	{0x8d, 0x47},
+	{0x8e, 0xaa},
+	{0x8f, 0x02},
+	{0x90, 0x22},
+	{0x91, 0x00},
+	{0x92, 0x00},
+	{0x93, 0x00},
+	{0x94, 0x00},
+	{0x95, 0x00},
+	{0x96, 0x00},
+	{0x97, 0xeb},
+	{0x98, 0x00},
+	{0x99, 0x00},
+	{0x9a, 0x00},
+	{0x9b, 0x00},
+	{0x9c, 0x00},
+	{0x9d, 0x00},
+	{0x9e, 0x00},
+	{0x9f, 0x01},
+	{0xa0, 0x00},
+	{0xa1, 0x00},
+	{0xa2, 0x00},
+	{0xa3, 0x00},
+	{0xa4, 0x00},
+	{0xa5, 0x00},
+	{0xa6, 0x10},
+	{0xa7, 0x00},
+	{0xa8, 0x18},
+	{0xa9, 0x00},
+	{0xaa, 0x00},
+	{0xab, 0x00},
+	{0xac, 0x00},
+	{0xad, 0x00},
+	{0xae, 0x00},
+	{0xaf, 0x18},
+	{0xb0, 0x38},
+	{0xb1, 0x30},
+	{0xb2, 0x00},
+	{0xb3, 0x00},
+	{0xb4, 0xff},
+	{0xb5, 0x0f},
+	{0xb6, 0xe4},
+	{0xb7, 0xe2},
+	{0xb8, 0x00},
+	{0xb9, 0x00},
+	{0xba, 0x00},
+	{0xbb, 0x03},
+	{0xbc, 0x01},
+	{0xbd, 0x00},
+	{0xbe, 0x00},
+	{0xbf, 0x00},
+	{0xc0, 0x18},
+	{0xc1, 0x20},
+	{0xc2, 0x07},
+	{0xc3, 0x18},
+	{0xc4, 0xff},
+	{0xc5, 0x2c},
+	{0xc6, 0x0c},
+	{0xc7, 0x0a},
+	{0xc8, 0x0e},
+	{0xc9, 0x01},
+	{0xca, 0x68},
+	{0xcb, 0xa7},
+	{0xcc, 0x3c},
+	{0xcd, 0x10},
+	{0xce, 0x00},
+	{0xcf, 0x25},
+	{0xd0, 0x40},
+	{0xd1, 0x12},
+	{0xd2, 0x00},
+	{0xd3, 0x00},
+	{0xd4, 0x10},
+	{0xd5, 0x28},
+	{0xd6, 0x80},
+	{0xd7, 0x2A},
+	{0xd8, 0x00},
+	{0xd9, 0x00},
+	{0xda, 0x00},
+	{0xdb, 0x00},
+	{0xdc, 0x00},
+	{0xdd, 0x00},
+	{0xde, 0x00},
+	{0xdf, 0x00},
+	{0xe0, 0x00},
+	{0xe1, 0xB3},
+	{0xe2, 0x00},
+	{0xe3, 0x00},
+	{0xe4, 0x00},
+	{0xe5, 0x10},
+	{0xe6, 0x00},
+	{0xe7, 0x1C},
+	{0xe8, 0x00},
+	{0xe9, 0xf4},
+	{0xea, 0x00},
+	{0xeb, 0xff},
+	{0xec, 0x79},
+	{0xed, 0x20},
+	{0xee, 0x30},
+	{0xef, 0x01},
+	{0xf0, 0x00},
+	{0xf1, 0x3e},
+	{0xf2, 0x00},
+	{0xf3, 0x00},
+	{0xf4, 0x00},
+	{0xf5, 0x00},
+	{0xf6, 0x00},
+	{0xf7, 0x00},
+	{0xf8, 0x00},
+	{0xf9, 0x00},
+	{0xfa, 0x00},
+	{0xfb, 0x00},
+	{0xfc, 0x00},
+	{0xfd, 0x00},
+	{0xfe, 0x00},
+	{0xff, 0x00},
 };
 
-
-
 #define CB_VT3253B0_INIT_FOR_UW2451 256
 //For UW2451
 unsigned char byVT3253B0_UW2451[CB_VT3253B0_INIT_FOR_UW2451][2] = {
-    {0x00, 0x31},
-    {0x01, 0x00},
-    {0x02, 0x00},
-    {0x03, 0x00},
-    {0x04, 0x00},
-    {0x05, 0x81},
-    {0x06, 0x00},
-    {0x07, 0x00},
-    {0x08, 0x38},
-    {0x09, 0x45},
-    {0x0a, 0x28},
-    {0x0b, 0x76},
-    {0x0c, 0x00},
-    {0x0d, 0x00},
-    {0x0e, 0x80},
-    {0x0f, 0x00},
-    {0x10, 0x00},
-    {0x11, 0x00},
-    {0x12, 0x00},
-    {0x13, 0x00},
-    {0x14, 0x00},
-    {0x15, 0x00},
-    {0x16, 0x00},
-    {0x17, 0x00},
-    {0x18, 0x00},
-    {0x19, 0x00},
-    {0x1a, 0x00},
-    {0x1b, 0x8f},
-    {0x1c, 0x0f},
-    {0x1d, 0x00},
-    {0x1e, 0x00},
-    {0x1f, 0x00},
-    {0x20, 0x00},
-    {0x21, 0x00},
-    {0x22, 0x00},
-    {0x23, 0x00},
-    {0x24, 0x00},
-    {0x25, 0x4a},
-    {0x26, 0x00},
-    {0x27, 0x00},
-    {0x28, 0x00},
-    {0x29, 0x00},
-    {0x2a, 0x00},
-    {0x2b, 0x00},
-    {0x2c, 0x00},
-    {0x2d, 0x18},
-    {0x2e, 0x00},
-    {0x2f, 0x0a},
-    {0x30, 0x26},
-    {0x31, 0x5b},
-    {0x32, 0x00},
-    {0x33, 0x00},
-    {0x34, 0x00},
-    {0x35, 0x00},
-    {0x36, 0xaa},
-    {0x37, 0xaa},
-    {0x38, 0xff},
-    {0x39, 0xff},
-    {0x3a, 0x00},
-    {0x3b, 0x00},
-    {0x3c, 0x00},
-    {0x3d, 0x03},
-    {0x3e, 0x1d},
-    {0x3f, 0x04},
-    {0x40, 0x00},
-    {0x41, 0x08},
-    {0x42, 0x00},
-    {0x43, 0x08},
-    {0x44, 0x08},
-    {0x45, 0x14},
-    {0x46, 0x05},
-    {0x47, 0x09},
-    {0x48, 0x00},
-    {0x49, 0x00},
-    {0x4a, 0x00},
-    {0x4b, 0x00},
-    {0x4c, 0x09},
-    {0x4d, 0x90},
-    {0x4e, 0x00},
-    {0x4f, 0xc5},
-    {0x50, 0x15},
-    {0x51, 0x19},
-    {0x52, 0x00},
-    {0x53, 0x00},
-    {0x54, 0x00},
-    {0x55, 0x00},
-    {0x56, 0x00},
-    {0x57, 0x00},
-    {0x58, 0x00},
-    {0x59, 0xb0},
-    {0x5a, 0x00},
-    {0x5b, 0x00},
-    {0x5c, 0x00},
-    {0x5d, 0x00},
-    {0x5e, 0x00},
-    {0x5f, 0x00},
-    {0x60, 0xb3},
-    {0x61, 0x81},
-    {0x62, 0x00},
-    {0x63, 0x00},
-    {0x64, 0x00},
-    {0x65, 0x00},
-    {0x66, 0x57},
-    {0x67, 0x6c},
-    {0x68, 0x00},
-    {0x69, 0x00},
-    {0x6a, 0x00},
-    {0x6b, 0x00},
-    //{0x6c, 0x80},
-    {0x6c, 0x00}, //RobertYu:20050125, request by JJSue
-    {0x6d, 0x03},
-    {0x6e, 0x01},
-    {0x6f, 0x00},
-    {0x70, 0x00},
-    {0x71, 0x00},
-    {0x72, 0x00},
-    {0x73, 0x00},
-    {0x74, 0x00},
-    {0x75, 0x00},
-    {0x76, 0x00},
-    {0x77, 0x00},
-    {0x78, 0x00},
-    {0x79, 0x00},
-    {0x7a, 0x00},
-    {0x7b, 0x00},
-    {0x7c, 0x00},
-    {0x7d, 0x00},
-    {0x7e, 0x00},
-    {0x7f, 0x00},
-    {0x80, 0x8c},
-    {0x81, 0x00},
-    {0x82, 0x0e},
-    {0x83, 0x00},
-    {0x84, 0x00},
-    {0x85, 0x00},
-    {0x86, 0x00},
-    {0x87, 0x00},
-    {0x88, 0x08},
-    {0x89, 0x00},
-    {0x8a, 0x0e},
-    {0x8b, 0xa7},
-    {0x8c, 0x88},
-    {0x8d, 0x47},
-    {0x8e, 0xaa},
-    {0x8f, 0x02},
-    {0x90, 0x00},
-    {0x91, 0x00},
-    {0x92, 0x00},
-    {0x93, 0x00},
-    {0x94, 0x00},
-    {0x95, 0x00},
-    {0x96, 0x00},
-    {0x97, 0xe3},
-    {0x98, 0x00},
-    {0x99, 0x00},
-    {0x9a, 0x00},
-    {0x9b, 0x00},
-    {0x9c, 0x00},
-    {0x9d, 0x00},
-    {0x9e, 0x00},
-    {0x9f, 0x00},
-    {0xa0, 0x00},
-    {0xa1, 0x00},
-    {0xa2, 0x00},
-    {0xa3, 0x00},
-    {0xa4, 0x00},
-    {0xa5, 0x00},
-    {0xa6, 0x10},
-    {0xa7, 0x00},
-    {0xa8, 0x18},
-    {0xa9, 0x00},
-    {0xaa, 0x00},
-    {0xab, 0x00},
-    {0xac, 0x00},
-    {0xad, 0x00},
-    {0xae, 0x00},
-    {0xaf, 0x18},
-    {0xb0, 0x18},
-    {0xb1, 0x30},
-    {0xb2, 0x00},
-    {0xb3, 0x00},
-    {0xb4, 0x00},
-    {0xb5, 0x00},
-    {0xb6, 0x00},
-    {0xb7, 0x00},
-    {0xb8, 0x00},
-    {0xb9, 0x00},
-    {0xba, 0x00},
-    {0xbb, 0x03},
-    {0xbc, 0x01},
-    {0xbd, 0x00},
-    {0xbe, 0x00},
-    {0xbf, 0x00},
-    {0xc0, 0x10},
-    {0xc1, 0x20},
-    {0xc2, 0x00},
-    {0xc3, 0x20},
-    {0xc4, 0x00},
-    {0xc5, 0x2c},
-    {0xc6, 0x1c},
-    {0xc7, 0x10},
-    {0xc8, 0x10},
-    {0xc9, 0x01},
-    {0xca, 0x68},
-    {0xcb, 0xa7},
-    {0xcc, 0x3c},
-    {0xcd, 0x09},
-    {0xce, 0x00},
-    {0xcf, 0x20},
-    {0xd0, 0x40},
-    {0xd1, 0x10},
-    {0xd2, 0x00},
-    {0xd3, 0x00},
-    {0xd4, 0x20},
-    {0xd5, 0x28},
-    {0xd6, 0xa0},
-    {0xd7, 0x2a},
-    {0xd8, 0x00},
-    {0xd9, 0x00},
-    {0xda, 0x00},
-    {0xdb, 0x00},
-    {0xdc, 0x00},
-    {0xdd, 0x00},
-    {0xde, 0x00},
-    {0xdf, 0x00},
-    {0xe0, 0x00},
-    {0xe1, 0xd3},
-    {0xe2, 0xc0},
-    {0xe3, 0x00},
-    {0xe4, 0x00},
-    {0xe5, 0x10},
-    {0xe6, 0x00},
-    {0xe7, 0x12},
-    {0xe8, 0x12},
-    {0xe9, 0x34},
-    {0xea, 0x00},
-    {0xeb, 0xff},
-    {0xec, 0x79},
-    {0xed, 0x20},
-    {0xee, 0x30},
-    {0xef, 0x01},
-    {0xf0, 0x00},
-    {0xf1, 0x3e},
-    {0xf2, 0x00},
-    {0xf3, 0x00},
-    {0xf4, 0x00},
-    {0xf5, 0x00},
-    {0xf6, 0x00},
-    {0xf7, 0x00},
-    {0xf8, 0x00},
-    {0xf9, 0x00},
-    {0xfa, 0x00},
-    {0xfb, 0x00},
-    {0xfc, 0x00},
-    {0xfd, 0x00},
-    {0xfe, 0x00},
-    {0xff, 0x00},
+	{0x00, 0x31},
+	{0x01, 0x00},
+	{0x02, 0x00},
+	{0x03, 0x00},
+	{0x04, 0x00},
+	{0x05, 0x81},
+	{0x06, 0x00},
+	{0x07, 0x00},
+	{0x08, 0x38},
+	{0x09, 0x45},
+	{0x0a, 0x28},
+	{0x0b, 0x76},
+	{0x0c, 0x00},
+	{0x0d, 0x00},
+	{0x0e, 0x80},
+	{0x0f, 0x00},
+	{0x10, 0x00},
+	{0x11, 0x00},
+	{0x12, 0x00},
+	{0x13, 0x00},
+	{0x14, 0x00},
+	{0x15, 0x00},
+	{0x16, 0x00},
+	{0x17, 0x00},
+	{0x18, 0x00},
+	{0x19, 0x00},
+	{0x1a, 0x00},
+	{0x1b, 0x8f},
+	{0x1c, 0x0f},
+	{0x1d, 0x00},
+	{0x1e, 0x00},
+	{0x1f, 0x00},
+	{0x20, 0x00},
+	{0x21, 0x00},
+	{0x22, 0x00},
+	{0x23, 0x00},
+	{0x24, 0x00},
+	{0x25, 0x4a},
+	{0x26, 0x00},
+	{0x27, 0x00},
+	{0x28, 0x00},
+	{0x29, 0x00},
+	{0x2a, 0x00},
+	{0x2b, 0x00},
+	{0x2c, 0x00},
+	{0x2d, 0x18},
+	{0x2e, 0x00},
+	{0x2f, 0x0a},
+	{0x30, 0x26},
+	{0x31, 0x5b},
+	{0x32, 0x00},
+	{0x33, 0x00},
+	{0x34, 0x00},
+	{0x35, 0x00},
+	{0x36, 0xaa},
+	{0x37, 0xaa},
+	{0x38, 0xff},
+	{0x39, 0xff},
+	{0x3a, 0x00},
+	{0x3b, 0x00},
+	{0x3c, 0x00},
+	{0x3d, 0x03},
+	{0x3e, 0x1d},
+	{0x3f, 0x04},
+	{0x40, 0x00},
+	{0x41, 0x08},
+	{0x42, 0x00},
+	{0x43, 0x08},
+	{0x44, 0x08},
+	{0x45, 0x14},
+	{0x46, 0x05},
+	{0x47, 0x09},
+	{0x48, 0x00},
+	{0x49, 0x00},
+	{0x4a, 0x00},
+	{0x4b, 0x00},
+	{0x4c, 0x09},
+	{0x4d, 0x90},
+	{0x4e, 0x00},
+	{0x4f, 0xc5},
+	{0x50, 0x15},
+	{0x51, 0x19},
+	{0x52, 0x00},
+	{0x53, 0x00},
+	{0x54, 0x00},
+	{0x55, 0x00},
+	{0x56, 0x00},
+	{0x57, 0x00},
+	{0x58, 0x00},
+	{0x59, 0xb0},
+	{0x5a, 0x00},
+	{0x5b, 0x00},
+	{0x5c, 0x00},
+	{0x5d, 0x00},
+	{0x5e, 0x00},
+	{0x5f, 0x00},
+	{0x60, 0xb3},
+	{0x61, 0x81},
+	{0x62, 0x00},
+	{0x63, 0x00},
+	{0x64, 0x00},
+	{0x65, 0x00},
+	{0x66, 0x57},
+	{0x67, 0x6c},
+	{0x68, 0x00},
+	{0x69, 0x00},
+	{0x6a, 0x00},
+	{0x6b, 0x00},
+	//{0x6c, 0x80},
+	{0x6c, 0x00}, //RobertYu:20050125, request by JJSue
+	{0x6d, 0x03},
+	{0x6e, 0x01},
+	{0x6f, 0x00},
+	{0x70, 0x00},
+	{0x71, 0x00},
+	{0x72, 0x00},
+	{0x73, 0x00},
+	{0x74, 0x00},
+	{0x75, 0x00},
+	{0x76, 0x00},
+	{0x77, 0x00},
+	{0x78, 0x00},
+	{0x79, 0x00},
+	{0x7a, 0x00},
+	{0x7b, 0x00},
+	{0x7c, 0x00},
+	{0x7d, 0x00},
+	{0x7e, 0x00},
+	{0x7f, 0x00},
+	{0x80, 0x8c},
+	{0x81, 0x00},
+	{0x82, 0x0e},
+	{0x83, 0x00},
+	{0x84, 0x00},
+	{0x85, 0x00},
+	{0x86, 0x00},
+	{0x87, 0x00},
+	{0x88, 0x08},
+	{0x89, 0x00},
+	{0x8a, 0x0e},
+	{0x8b, 0xa7},
+	{0x8c, 0x88},
+	{0x8d, 0x47},
+	{0x8e, 0xaa},
+	{0x8f, 0x02},
+	{0x90, 0x00},
+	{0x91, 0x00},
+	{0x92, 0x00},
+	{0x93, 0x00},
+	{0x94, 0x00},
+	{0x95, 0x00},
+	{0x96, 0x00},
+	{0x97, 0xe3},
+	{0x98, 0x00},
+	{0x99, 0x00},
+	{0x9a, 0x00},
+	{0x9b, 0x00},
+	{0x9c, 0x00},
+	{0x9d, 0x00},
+	{0x9e, 0x00},
+	{0x9f, 0x00},
+	{0xa0, 0x00},
+	{0xa1, 0x00},
+	{0xa2, 0x00},
+	{0xa3, 0x00},
+	{0xa4, 0x00},
+	{0xa5, 0x00},
+	{0xa6, 0x10},
+	{0xa7, 0x00},
+	{0xa8, 0x18},
+	{0xa9, 0x00},
+	{0xaa, 0x00},
+	{0xab, 0x00},
+	{0xac, 0x00},
+	{0xad, 0x00},
+	{0xae, 0x00},
+	{0xaf, 0x18},
+	{0xb0, 0x18},
+	{0xb1, 0x30},
+	{0xb2, 0x00},
+	{0xb3, 0x00},
+	{0xb4, 0x00},
+	{0xb5, 0x00},
+	{0xb6, 0x00},
+	{0xb7, 0x00},
+	{0xb8, 0x00},
+	{0xb9, 0x00},
+	{0xba, 0x00},
+	{0xbb, 0x03},
+	{0xbc, 0x01},
+	{0xbd, 0x00},
+	{0xbe, 0x00},
+	{0xbf, 0x00},
+	{0xc0, 0x10},
+	{0xc1, 0x20},
+	{0xc2, 0x00},
+	{0xc3, 0x20},
+	{0xc4, 0x00},
+	{0xc5, 0x2c},
+	{0xc6, 0x1c},
+	{0xc7, 0x10},
+	{0xc8, 0x10},
+	{0xc9, 0x01},
+	{0xca, 0x68},
+	{0xcb, 0xa7},
+	{0xcc, 0x3c},
+	{0xcd, 0x09},
+	{0xce, 0x00},
+	{0xcf, 0x20},
+	{0xd0, 0x40},
+	{0xd1, 0x10},
+	{0xd2, 0x00},
+	{0xd3, 0x00},
+	{0xd4, 0x20},
+	{0xd5, 0x28},
+	{0xd6, 0xa0},
+	{0xd7, 0x2a},
+	{0xd8, 0x00},
+	{0xd9, 0x00},
+	{0xda, 0x00},
+	{0xdb, 0x00},
+	{0xdc, 0x00},
+	{0xdd, 0x00},
+	{0xde, 0x00},
+	{0xdf, 0x00},
+	{0xe0, 0x00},
+	{0xe1, 0xd3},
+	{0xe2, 0xc0},
+	{0xe3, 0x00},
+	{0xe4, 0x00},
+	{0xe5, 0x10},
+	{0xe6, 0x00},
+	{0xe7, 0x12},
+	{0xe8, 0x12},
+	{0xe9, 0x34},
+	{0xea, 0x00},
+	{0xeb, 0xff},
+	{0xec, 0x79},
+	{0xed, 0x20},
+	{0xee, 0x30},
+	{0xef, 0x01},
+	{0xf0, 0x00},
+	{0xf1, 0x3e},
+	{0xf2, 0x00},
+	{0xf3, 0x00},
+	{0xf4, 0x00},
+	{0xf5, 0x00},
+	{0xf6, 0x00},
+	{0xf7, 0x00},
+	{0xf8, 0x00},
+	{0xf9, 0x00},
+	{0xfa, 0x00},
+	{0xfb, 0x00},
+	{0xfc, 0x00},
+	{0xfd, 0x00},
+	{0xfe, 0x00},
+	{0xff, 0x00},
 };
 
 #define CB_VT3253B0_AGC 193
 // For AIROHA
 unsigned char byVT3253B0_AGC[CB_VT3253B0_AGC][2] = {
-    {0xF0, 0x00},
-    {0xF1, 0x00},
-    {0xF0, 0x80},
-    {0xF0, 0x01},
-    {0xF1, 0x00},
-    {0xF0, 0x81},
-    {0xF0, 0x02},
-    {0xF1, 0x02},
-    {0xF0, 0x82},
-    {0xF0, 0x03},
-    {0xF1, 0x04},
-    {0xF0, 0x83},
-    {0xF0, 0x03},
-    {0xF1, 0x04},
-    {0xF0, 0x84},
-    {0xF0, 0x04},
-    {0xF1, 0x06},
-    {0xF0, 0x85},
-    {0xF0, 0x05},
-    {0xF1, 0x06},
-    {0xF0, 0x86},
-    {0xF0, 0x06},
-    {0xF1, 0x06},
-    {0xF0, 0x87},
-    {0xF0, 0x07},
-    {0xF1, 0x08},
-    {0xF0, 0x88},
-    {0xF0, 0x08},
-    {0xF1, 0x08},
-    {0xF0, 0x89},
-    {0xF0, 0x09},
-    {0xF1, 0x0A},
-    {0xF0, 0x8A},
-    {0xF0, 0x0A},
-    {0xF1, 0x0A},
-    {0xF0, 0x8B},
-    {0xF0, 0x0B},
-    {0xF1, 0x0C},
-    {0xF0, 0x8C},
-    {0xF0, 0x0C},
-    {0xF1, 0x0C},
-    {0xF0, 0x8D},
-    {0xF0, 0x0D},
-    {0xF1, 0x0E},
-    {0xF0, 0x8E},
-    {0xF0, 0x0E},
-    {0xF1, 0x0E},
-    {0xF0, 0x8F},
-    {0xF0, 0x0F},
-    {0xF1, 0x10},
-    {0xF0, 0x90},
-    {0xF0, 0x10},
-    {0xF1, 0x10},
-    {0xF0, 0x91},
-    {0xF0, 0x11},
-    {0xF1, 0x12},
-    {0xF0, 0x92},
-    {0xF0, 0x12},
-    {0xF1, 0x12},
-    {0xF0, 0x93},
-    {0xF0, 0x13},
-    {0xF1, 0x14},
-    {0xF0, 0x94},
-    {0xF0, 0x14},
-    {0xF1, 0x14},
-    {0xF0, 0x95},
-    {0xF0, 0x15},
-    {0xF1, 0x16},
-    {0xF0, 0x96},
-    {0xF0, 0x16},
-    {0xF1, 0x16},
-    {0xF0, 0x97},
-    {0xF0, 0x17},
-    {0xF1, 0x18},
-    {0xF0, 0x98},
-    {0xF0, 0x18},
-    {0xF1, 0x18},
-    {0xF0, 0x99},
-    {0xF0, 0x19},
-    {0xF1, 0x1A},
-    {0xF0, 0x9A},
-    {0xF0, 0x1A},
-    {0xF1, 0x1A},
-    {0xF0, 0x9B},
-    {0xF0, 0x1B},
-    {0xF1, 0x1C},
-    {0xF0, 0x9C},
-    {0xF0, 0x1C},
-    {0xF1, 0x1C},
-    {0xF0, 0x9D},
-    {0xF0, 0x1D},
-    {0xF1, 0x1E},
-    {0xF0, 0x9E},
-    {0xF0, 0x1E},
-    {0xF1, 0x1E},
-    {0xF0, 0x9F},
-    {0xF0, 0x1F},
-    {0xF1, 0x20},
-    {0xF0, 0xA0},
-    {0xF0, 0x20},
-    {0xF1, 0x20},
-    {0xF0, 0xA1},
-    {0xF0, 0x21},
-    {0xF1, 0x22},
-    {0xF0, 0xA2},
-    {0xF0, 0x22},
-    {0xF1, 0x22},
-    {0xF0, 0xA3},
-    {0xF0, 0x23},
-    {0xF1, 0x24},
-    {0xF0, 0xA4},
-    {0xF0, 0x24},
-    {0xF1, 0x24},
-    {0xF0, 0xA5},
-    {0xF0, 0x25},
-    {0xF1, 0x26},
-    {0xF0, 0xA6},
-    {0xF0, 0x26},
-    {0xF1, 0x26},
-    {0xF0, 0xA7},
-    {0xF0, 0x27},
-    {0xF1, 0x28},
-    {0xF0, 0xA8},
-    {0xF0, 0x28},
-    {0xF1, 0x28},
-    {0xF0, 0xA9},
-    {0xF0, 0x29},
-    {0xF1, 0x2A},
-    {0xF0, 0xAA},
-    {0xF0, 0x2A},
-    {0xF1, 0x2A},
-    {0xF0, 0xAB},
-    {0xF0, 0x2B},
-    {0xF1, 0x2C},
-    {0xF0, 0xAC},
-    {0xF0, 0x2C},
-    {0xF1, 0x2C},
-    {0xF0, 0xAD},
-    {0xF0, 0x2D},
-    {0xF1, 0x2E},
-    {0xF0, 0xAE},
-    {0xF0, 0x2E},
-    {0xF1, 0x2E},
-    {0xF0, 0xAF},
-    {0xF0, 0x2F},
-    {0xF1, 0x30},
-    {0xF0, 0xB0},
-    {0xF0, 0x30},
-    {0xF1, 0x30},
-    {0xF0, 0xB1},
-    {0xF0, 0x31},
-    {0xF1, 0x32},
-    {0xF0, 0xB2},
-    {0xF0, 0x32},
-    {0xF1, 0x32},
-    {0xF0, 0xB3},
-    {0xF0, 0x33},
-    {0xF1, 0x34},
-    {0xF0, 0xB4},
-    {0xF0, 0x34},
-    {0xF1, 0x34},
-    {0xF0, 0xB5},
-    {0xF0, 0x35},
-    {0xF1, 0x36},
-    {0xF0, 0xB6},
-    {0xF0, 0x36},
-    {0xF1, 0x36},
-    {0xF0, 0xB7},
-    {0xF0, 0x37},
-    {0xF1, 0x38},
-    {0xF0, 0xB8},
-    {0xF0, 0x38},
-    {0xF1, 0x38},
-    {0xF0, 0xB9},
-    {0xF0, 0x39},
-    {0xF1, 0x3A},
-    {0xF0, 0xBA},
-    {0xF0, 0x3A},
-    {0xF1, 0x3A},
-    {0xF0, 0xBB},
-    {0xF0, 0x3B},
-    {0xF1, 0x3C},
-    {0xF0, 0xBC},
-    {0xF0, 0x3C},
-    {0xF1, 0x3C},
-    {0xF0, 0xBD},
-    {0xF0, 0x3D},
-    {0xF1, 0x3E},
-    {0xF0, 0xBE},
-    {0xF0, 0x3E},
-    {0xF1, 0x3E},
-    {0xF0, 0xBF},
-    {0xF0, 0x00},
+	{0xF0, 0x00},
+	{0xF1, 0x00},
+	{0xF0, 0x80},
+	{0xF0, 0x01},
+	{0xF1, 0x00},
+	{0xF0, 0x81},
+	{0xF0, 0x02},
+	{0xF1, 0x02},
+	{0xF0, 0x82},
+	{0xF0, 0x03},
+	{0xF1, 0x04},
+	{0xF0, 0x83},
+	{0xF0, 0x03},
+	{0xF1, 0x04},
+	{0xF0, 0x84},
+	{0xF0, 0x04},
+	{0xF1, 0x06},
+	{0xF0, 0x85},
+	{0xF0, 0x05},
+	{0xF1, 0x06},
+	{0xF0, 0x86},
+	{0xF0, 0x06},
+	{0xF1, 0x06},
+	{0xF0, 0x87},
+	{0xF0, 0x07},
+	{0xF1, 0x08},
+	{0xF0, 0x88},
+	{0xF0, 0x08},
+	{0xF1, 0x08},
+	{0xF0, 0x89},
+	{0xF0, 0x09},
+	{0xF1, 0x0A},
+	{0xF0, 0x8A},
+	{0xF0, 0x0A},
+	{0xF1, 0x0A},
+	{0xF0, 0x8B},
+	{0xF0, 0x0B},
+	{0xF1, 0x0C},
+	{0xF0, 0x8C},
+	{0xF0, 0x0C},
+	{0xF1, 0x0C},
+	{0xF0, 0x8D},
+	{0xF0, 0x0D},
+	{0xF1, 0x0E},
+	{0xF0, 0x8E},
+	{0xF0, 0x0E},
+	{0xF1, 0x0E},
+	{0xF0, 0x8F},
+	{0xF0, 0x0F},
+	{0xF1, 0x10},
+	{0xF0, 0x90},
+	{0xF0, 0x10},
+	{0xF1, 0x10},
+	{0xF0, 0x91},
+	{0xF0, 0x11},
+	{0xF1, 0x12},
+	{0xF0, 0x92},
+	{0xF0, 0x12},
+	{0xF1, 0x12},
+	{0xF0, 0x93},
+	{0xF0, 0x13},
+	{0xF1, 0x14},
+	{0xF0, 0x94},
+	{0xF0, 0x14},
+	{0xF1, 0x14},
+	{0xF0, 0x95},
+	{0xF0, 0x15},
+	{0xF1, 0x16},
+	{0xF0, 0x96},
+	{0xF0, 0x16},
+	{0xF1, 0x16},
+	{0xF0, 0x97},
+	{0xF0, 0x17},
+	{0xF1, 0x18},
+	{0xF0, 0x98},
+	{0xF0, 0x18},
+	{0xF1, 0x18},
+	{0xF0, 0x99},
+	{0xF0, 0x19},
+	{0xF1, 0x1A},
+	{0xF0, 0x9A},
+	{0xF0, 0x1A},
+	{0xF1, 0x1A},
+	{0xF0, 0x9B},
+	{0xF0, 0x1B},
+	{0xF1, 0x1C},
+	{0xF0, 0x9C},
+	{0xF0, 0x1C},
+	{0xF1, 0x1C},
+	{0xF0, 0x9D},
+	{0xF0, 0x1D},
+	{0xF1, 0x1E},
+	{0xF0, 0x9E},
+	{0xF0, 0x1E},
+	{0xF1, 0x1E},
+	{0xF0, 0x9F},
+	{0xF0, 0x1F},
+	{0xF1, 0x20},
+	{0xF0, 0xA0},
+	{0xF0, 0x20},
+	{0xF1, 0x20},
+	{0xF0, 0xA1},
+	{0xF0, 0x21},
+	{0xF1, 0x22},
+	{0xF0, 0xA2},
+	{0xF0, 0x22},
+	{0xF1, 0x22},
+	{0xF0, 0xA3},
+	{0xF0, 0x23},
+	{0xF1, 0x24},
+	{0xF0, 0xA4},
+	{0xF0, 0x24},
+	{0xF1, 0x24},
+	{0xF0, 0xA5},
+	{0xF0, 0x25},
+	{0xF1, 0x26},
+	{0xF0, 0xA6},
+	{0xF0, 0x26},
+	{0xF1, 0x26},
+	{0xF0, 0xA7},
+	{0xF0, 0x27},
+	{0xF1, 0x28},
+	{0xF0, 0xA8},
+	{0xF0, 0x28},
+	{0xF1, 0x28},
+	{0xF0, 0xA9},
+	{0xF0, 0x29},
+	{0xF1, 0x2A},
+	{0xF0, 0xAA},
+	{0xF0, 0x2A},
+	{0xF1, 0x2A},
+	{0xF0, 0xAB},
+	{0xF0, 0x2B},
+	{0xF1, 0x2C},
+	{0xF0, 0xAC},
+	{0xF0, 0x2C},
+	{0xF1, 0x2C},
+	{0xF0, 0xAD},
+	{0xF0, 0x2D},
+	{0xF1, 0x2E},
+	{0xF0, 0xAE},
+	{0xF0, 0x2E},
+	{0xF1, 0x2E},
+	{0xF0, 0xAF},
+	{0xF0, 0x2F},
+	{0xF1, 0x30},
+	{0xF0, 0xB0},
+	{0xF0, 0x30},
+	{0xF1, 0x30},
+	{0xF0, 0xB1},
+	{0xF0, 0x31},
+	{0xF1, 0x32},
+	{0xF0, 0xB2},
+	{0xF0, 0x32},
+	{0xF1, 0x32},
+	{0xF0, 0xB3},
+	{0xF0, 0x33},
+	{0xF1, 0x34},
+	{0xF0, 0xB4},
+	{0xF0, 0x34},
+	{0xF1, 0x34},
+	{0xF0, 0xB5},
+	{0xF0, 0x35},
+	{0xF1, 0x36},
+	{0xF0, 0xB6},
+	{0xF0, 0x36},
+	{0xF1, 0x36},
+	{0xF0, 0xB7},
+	{0xF0, 0x37},
+	{0xF1, 0x38},
+	{0xF0, 0xB8},
+	{0xF0, 0x38},
+	{0xF1, 0x38},
+	{0xF0, 0xB9},
+	{0xF0, 0x39},
+	{0xF1, 0x3A},
+	{0xF0, 0xBA},
+	{0xF0, 0x3A},
+	{0xF1, 0x3A},
+	{0xF0, 0xBB},
+	{0xF0, 0x3B},
+	{0xF1, 0x3C},
+	{0xF0, 0xBC},
+	{0xF0, 0x3C},
+	{0xF1, 0x3C},
+	{0xF0, 0xBD},
+	{0xF0, 0x3D},
+	{0xF1, 0x3E},
+	{0xF0, 0xBE},
+	{0xF0, 0x3E},
+	{0xF1, 0x3E},
+	{0xF0, 0xBF},
+	{0xF0, 0x00},
 };
 
 const unsigned short awcFrameTime[MAX_RATE] =
 {10, 20, 55, 110, 24, 36, 48, 72, 96, 144, 192, 216};
 
-
 /*---------------------  Static Functions  --------------------------*/
 
 static
@@ -1723,42 +1718,37 @@ s_ulGetRatio(PSDevice pDevice);
 static
 void
 s_vChangeAntenna(
-    PSDevice pDevice
-    );
+	PSDevice pDevice
+);
 
 static
 void
-s_vChangeAntenna (
-    PSDevice pDevice
-    )
+s_vChangeAntenna(
+	PSDevice pDevice
+)
 {
-
-#ifdef	PLICE_DEBUG
-	//printk("Enter s_vChangeAntenna:original RxMode is %d,TxMode is %d\n",pDevice->byRxAntennaMode,pDevice->byTxAntennaMode);
-#endif
-    if ( pDevice->dwRxAntennaSel == 0) {
-        pDevice->dwRxAntennaSel=1;
-        if (pDevice->bTxRxAntInv == true)
-            BBvSetRxAntennaMode(pDevice->PortOffset, ANT_A);
-        else
-            BBvSetRxAntennaMode(pDevice->PortOffset, ANT_B);
-    } else {
-        pDevice->dwRxAntennaSel=0;
-        if (pDevice->bTxRxAntInv == true)
-            BBvSetRxAntennaMode(pDevice->PortOffset, ANT_B);
-        else
-            BBvSetRxAntennaMode(pDevice->PortOffset, ANT_A);
-    }
-    if ( pDevice->dwTxAntennaSel == 0) {
-        pDevice->dwTxAntennaSel=1;
-        BBvSetTxAntennaMode(pDevice->PortOffset, ANT_B);
-    } else {
-        pDevice->dwTxAntennaSel=0;
-        BBvSetTxAntennaMode(pDevice->PortOffset, ANT_A);
-    }
+	if (pDevice->dwRxAntennaSel == 0) {
+		pDevice->dwRxAntennaSel = 1;
+		if (pDevice->bTxRxAntInv == true)
+			BBvSetRxAntennaMode(pDevice->PortOffset, ANT_A);
+		else
+			BBvSetRxAntennaMode(pDevice->PortOffset, ANT_B);
+	} else {
+		pDevice->dwRxAntennaSel = 0;
+		if (pDevice->bTxRxAntInv == true)
+			BBvSetRxAntennaMode(pDevice->PortOffset, ANT_B);
+		else
+			BBvSetRxAntennaMode(pDevice->PortOffset, ANT_A);
+	}
+	if (pDevice->dwTxAntennaSel == 0) {
+		pDevice->dwTxAntennaSel = 1;
+		BBvSetTxAntennaMode(pDevice->PortOffset, ANT_B);
+	} else {
+		pDevice->dwTxAntennaSel = 0;
+		BBvSetTxAntennaMode(pDevice->PortOffset, ANT_A);
+	}
 }
 
-
 /*---------------------  Export Variables  --------------------------*/
 /*
  * Description: Calculate data frame transmitting time
@@ -1775,54 +1765,52 @@ s_vChangeAntenna (
  *
  */
 unsigned int
-BBuGetFrameTime (
-    unsigned char byPreambleType,
-    unsigned char byPktType,
-    unsigned int cbFrameLength,
-    unsigned short wRate
-    )
+BBuGetFrameTime(
+	unsigned char byPreambleType,
+	unsigned char byPktType,
+	unsigned int cbFrameLength,
+	unsigned short wRate
+)
 {
-    unsigned int uFrameTime;
-    unsigned int uPreamble;
-    unsigned int uTmp;
-    unsigned int uRateIdx = (unsigned int) wRate;
-    unsigned int uRate = 0;
-
-
-    if (uRateIdx > RATE_54M) {
-	    ASSERT(0);
-        return 0;
-    }
-
-    uRate = (unsigned int) awcFrameTime[uRateIdx];
-
-    if (uRateIdx <= 3) {          //CCK mode
-
-        if (byPreambleType == 1) {//Short
-            uPreamble = 96;
-        } else {
-            uPreamble = 192;
-        }
-        uFrameTime = (cbFrameLength * 80) / uRate;  //?????
-        uTmp = (uFrameTime * uRate) / 80;
-        if (cbFrameLength != uTmp) {
-            uFrameTime ++;
-        }
-
-        return (uPreamble + uFrameTime);
-    }
-    else {
-        uFrameTime = (cbFrameLength * 8 + 22) / uRate;   //????????
-        uTmp = ((uFrameTime * uRate) - 22) / 8;
-        if(cbFrameLength != uTmp) {
-            uFrameTime ++;
-        }
-        uFrameTime = uFrameTime * 4;    //???????
-        if(byPktType != PK_TYPE_11A) {
-            uFrameTime += 6;     //??????
-        }
-        return (20 + uFrameTime); //??????
-    }
+	unsigned int uFrameTime;
+	unsigned int uPreamble;
+	unsigned int uTmp;
+	unsigned int uRateIdx = (unsigned int) wRate;
+	unsigned int uRate = 0;
+
+	if (uRateIdx > RATE_54M) {
+		ASSERT(0);
+		return 0;
+	}
+
+	uRate = (unsigned int)awcFrameTime[uRateIdx];
+
+	if (uRateIdx <= 3) {          //CCK mode
+
+		if (byPreambleType == 1) {//Short
+			uPreamble = 96;
+		} else {
+			uPreamble = 192;
+		}
+		uFrameTime = (cbFrameLength * 80) / uRate;  //?????
+		uTmp = (uFrameTime * uRate) / 80;
+		if (cbFrameLength != uTmp) {
+			uFrameTime++;
+		}
+
+		return uPreamble + uFrameTime;
+	} else {
+		uFrameTime = (cbFrameLength * 8 + 22) / uRate;   //????????
+		uTmp = ((uFrameTime * uRate) - 22) / 8;
+		if (cbFrameLength != uTmp) {
+			uFrameTime++;
+		}
+		uFrameTime = uFrameTime * 4;    //???????
+		if (byPktType != PK_TYPE_11A) {
+			uFrameTime += 6;     //??????
+		}
+		return 20 + uFrameTime; //??????
+	}
 }
 
 /*
@@ -1842,162 +1830,152 @@ BBuGetFrameTime (
  *
  */
 void
-BBvCalculateParameter (
-    PSDevice pDevice,
-    unsigned int cbFrameLength,
-    unsigned short wRate,
-    unsigned char byPacketType,
-    unsigned short *pwPhyLen,
-    unsigned char *pbyPhySrv,
-    unsigned char *pbyPhySgn
-    )
+BBvCalculateParameter(
+	PSDevice pDevice,
+	unsigned int cbFrameLength,
+	unsigned short wRate,
+	unsigned char byPacketType,
+	unsigned short *pwPhyLen,
+	unsigned char *pbyPhySrv,
+	unsigned char *pbyPhySgn
+)
 {
-    unsigned int cbBitCount;
-    unsigned int cbUsCount = 0;
-    unsigned int cbTmp;
-    bool bExtBit;
-    unsigned char byPreambleType = pDevice->byPreambleType;
-    bool bCCK = pDevice->bCCK;
-
-    cbBitCount = cbFrameLength * 8;
-    bExtBit = false;
-
-    switch (wRate) {
-    case RATE_1M :
-        cbUsCount = cbBitCount;
-        *pbyPhySgn = 0x00;
-        break;
-
-    case RATE_2M :
-        cbUsCount = cbBitCount / 2;
-        if (byPreambleType == 1)
-            *pbyPhySgn = 0x09;
-        else // long preamble
-            *pbyPhySgn = 0x01;
-        break;
-
-    case RATE_5M :
-        if (bCCK == false)
-            cbBitCount ++;
-        cbUsCount = (cbBitCount * 10) / 55;
-        cbTmp = (cbUsCount * 55) / 10;
-        if (cbTmp != cbBitCount)
-            cbUsCount ++;
-        if (byPreambleType == 1)
-            *pbyPhySgn = 0x0a;
-        else // long preamble
-            *pbyPhySgn = 0x02;
-        break;
-
-    case RATE_11M :
-
-        if (bCCK == false)
-            cbBitCount ++;
-        cbUsCount = cbBitCount / 11;
-        cbTmp = cbUsCount * 11;
-        if (cbTmp != cbBitCount) {
-            cbUsCount ++;
-            if ((cbBitCount - cbTmp) <= 3)
-                bExtBit = true;
-        }
-        if (byPreambleType == 1)
-            *pbyPhySgn = 0x0b;
-        else // long preamble
-            *pbyPhySgn = 0x03;
-        break;
-
-    case RATE_6M :
-        if(byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-            *pbyPhySgn = 0x9B; //1001 1011
-        }
-        else {//11g, 2.4GHZ
-            *pbyPhySgn = 0x8B; //1000 1011
-        }
-        break;
-
-    case RATE_9M :
-        if(byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-            *pbyPhySgn = 0x9F; //1001 1111
-        }
-        else {//11g, 2.4GHZ
-            *pbyPhySgn = 0x8F; //1000 1111
-        }
-        break;
-
-    case RATE_12M :
-        if(byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-            *pbyPhySgn = 0x9A; //1001 1010
-        }
-        else {//11g, 2.4GHZ
-            *pbyPhySgn = 0x8A; //1000 1010
-        }
-        break;
-
-    case RATE_18M :
-        if(byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-            *pbyPhySgn = 0x9E; //1001 1110
-        }
-        else {//11g, 2.4GHZ
-            *pbyPhySgn = 0x8E; //1000 1110
-        }
-        break;
-
-    case RATE_24M :
-        if(byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-            *pbyPhySgn = 0x99; //1001 1001
-        }
-        else {//11g, 2.4GHZ
-            *pbyPhySgn = 0x89; //1000 1001
-        }
-        break;
-
-    case RATE_36M :
-        if(byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-            *pbyPhySgn = 0x9D; //1001 1101
-        }
-        else {//11g, 2.4GHZ
-            *pbyPhySgn = 0x8D; //1000 1101
-        }
-        break;
-
-    case RATE_48M :
-        if(byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-            *pbyPhySgn = 0x98; //1001 1000
-        }
-        else {//11g, 2.4GHZ
-            *pbyPhySgn = 0x88; //1000 1000
-        }
-        break;
-
-    case RATE_54M :
-        if (byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-            *pbyPhySgn = 0x9C; //1001 1100
-        }
-        else {//11g, 2.4GHZ
-            *pbyPhySgn = 0x8C; //1000 1100
-        }
-        break;
-
-    default :
-        if (byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-            *pbyPhySgn = 0x9C; //1001 1100
-        }
-        else {//11g, 2.4GHZ
-            *pbyPhySgn = 0x8C; //1000 1100
-        }
-        break;
-    }
-
-    if (byPacketType == PK_TYPE_11B) {
-        *pbyPhySrv = 0x00;
-        if (bExtBit)
-            *pbyPhySrv = *pbyPhySrv | 0x80;
-        *pwPhyLen = (unsigned short)cbUsCount;
-    }
-    else {
-        *pbyPhySrv = 0x00;
-        *pwPhyLen = (unsigned short)cbFrameLength;
-    }
+	unsigned int cbBitCount;
+	unsigned int cbUsCount = 0;
+	unsigned int cbTmp;
+	bool bExtBit;
+	unsigned char byPreambleType = pDevice->byPreambleType;
+	bool bCCK = pDevice->bCCK;
+
+	cbBitCount = cbFrameLength * 8;
+	bExtBit = false;
+
+	switch (wRate) {
+	case RATE_1M:
+		cbUsCount = cbBitCount;
+		*pbyPhySgn = 0x00;
+		break;
+
+	case RATE_2M:
+		cbUsCount = cbBitCount / 2;
+		if (byPreambleType == 1)
+			*pbyPhySgn = 0x09;
+		else // long preamble
+			*pbyPhySgn = 0x01;
+		break;
+
+	case RATE_5M:
+		if (bCCK == false)
+			cbBitCount++;
+		cbUsCount = (cbBitCount * 10) / 55;
+		cbTmp = (cbUsCount * 55) / 10;
+		if (cbTmp != cbBitCount)
+			cbUsCount++;
+		if (byPreambleType == 1)
+			*pbyPhySgn = 0x0a;
+		else // long preamble
+			*pbyPhySgn = 0x02;
+		break;
+
+	case RATE_11M:
+
+		if (bCCK == false)
+			cbBitCount++;
+		cbUsCount = cbBitCount / 11;
+		cbTmp = cbUsCount * 11;
+		if (cbTmp != cbBitCount) {
+			cbUsCount++;
+			if ((cbBitCount - cbTmp) <= 3)
+				bExtBit = true;
+		}
+		if (byPreambleType == 1)
+			*pbyPhySgn = 0x0b;
+		else // long preamble
+			*pbyPhySgn = 0x03;
+		break;
+
+	case RATE_6M:
+		if (byPacketType == PK_TYPE_11A) {//11a, 5GHZ
+			*pbyPhySgn = 0x9B; //1001 1011
+		} else {//11g, 2.4GHZ
+			*pbyPhySgn = 0x8B; //1000 1011
+		}
+		break;
+
+	case RATE_9M:
+		if (byPacketType == PK_TYPE_11A) {//11a, 5GHZ
+			*pbyPhySgn = 0x9F; //1001 1111
+		} else {//11g, 2.4GHZ
+			*pbyPhySgn = 0x8F; //1000 1111
+		}
+		break;
+
+	case RATE_12M:
+		if (byPacketType == PK_TYPE_11A) {//11a, 5GHZ
+			*pbyPhySgn = 0x9A; //1001 1010
+		} else {//11g, 2.4GHZ
+			*pbyPhySgn = 0x8A; //1000 1010
+		}
+		break;
+
+	case RATE_18M:
+		if (byPacketType == PK_TYPE_11A) {//11a, 5GHZ
+			*pbyPhySgn = 0x9E; //1001 1110
+		} else {//11g, 2.4GHZ
+			*pbyPhySgn = 0x8E; //1000 1110
+		}
+		break;
+
+	case RATE_24M:
+		if (byPacketType == PK_TYPE_11A) {//11a, 5GHZ
+			*pbyPhySgn = 0x99; //1001 1001
+		} else {//11g, 2.4GHZ
+			*pbyPhySgn = 0x89; //1000 1001
+		}
+		break;
+
+	case RATE_36M:
+		if (byPacketType == PK_TYPE_11A) {//11a, 5GHZ
+			*pbyPhySgn = 0x9D; //1001 1101
+		} else {//11g, 2.4GHZ
+			*pbyPhySgn = 0x8D; //1000 1101
+		}
+		break;
+
+	case RATE_48M:
+		if (byPacketType == PK_TYPE_11A) {//11a, 5GHZ
+			*pbyPhySgn = 0x98; //1001 1000
+		} else {//11g, 2.4GHZ
+			*pbyPhySgn = 0x88; //1000 1000
+		}
+		break;
+
+	case RATE_54M:
+		if (byPacketType == PK_TYPE_11A) {//11a, 5GHZ
+			*pbyPhySgn = 0x9C; //1001 1100
+		} else {//11g, 2.4GHZ
+			*pbyPhySgn = 0x8C; //1000 1100
+		}
+		break;
+
+	default:
+		if (byPacketType == PK_TYPE_11A) {//11a, 5GHZ
+			*pbyPhySgn = 0x9C; //1001 1100
+		} else {//11g, 2.4GHZ
+			*pbyPhySgn = 0x8C; //1000 1100
+		}
+		break;
+	}
+
+	if (byPacketType == PK_TYPE_11B) {
+		*pbyPhySrv = 0x00;
+		if (bExtBit)
+			*pbyPhySrv = *pbyPhySrv | 0x80;
+		*pwPhyLen = (unsigned short)cbUsCount;
+	} else {
+		*pbyPhySrv = 0x00;
+		*pwPhyLen = (unsigned short)cbFrameLength;
+	}
 }
 
 /*
@@ -2013,35 +1991,34 @@ BBvCalculateParameter (
  * Return Value: true if succeeded; false if failed.
  *
  */
-bool BBbReadEmbedded (unsigned long dwIoBase, unsigned char byBBAddr, unsigned char *pbyData)
+bool BBbReadEmbedded(unsigned long dwIoBase, unsigned char byBBAddr, unsigned char *pbyData)
 {
-    unsigned short ww;
-    unsigned char byValue;
-
-    // BB reg offset
-    VNSvOutPortB(dwIoBase + MAC_REG_BBREGADR, byBBAddr);
-
-    // turn on REGR
-    MACvRegBitsOn(dwIoBase, MAC_REG_BBREGCTL, BBREGCTL_REGR);
-    // W_MAX_TIMEOUT is the timeout period
-    for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-        VNSvInPortB(dwIoBase + MAC_REG_BBREGCTL, &byValue);
-        if (byValue & BBREGCTL_DONE)
-            break;
-    }
-
-    // get BB data
-    VNSvInPortB(dwIoBase + MAC_REG_BBREGDATA, pbyData);
-
-    if (ww == W_MAX_TIMEOUT) {
-        DBG_PORT80(0x30);
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" DBG_PORT80(0x30)\n");
-        return false;
-    }
-    return true;
+	unsigned short ww;
+	unsigned char byValue;
+
+	// BB reg offset
+	VNSvOutPortB(dwIoBase + MAC_REG_BBREGADR, byBBAddr);
+
+	// turn on REGR
+	MACvRegBitsOn(dwIoBase, MAC_REG_BBREGCTL, BBREGCTL_REGR);
+	// W_MAX_TIMEOUT is the timeout period
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		VNSvInPortB(dwIoBase + MAC_REG_BBREGCTL, &byValue);
+		if (byValue & BBREGCTL_DONE)
+			break;
+	}
+
+	// get BB data
+	VNSvInPortB(dwIoBase + MAC_REG_BBREGDATA, pbyData);
+
+	if (ww == W_MAX_TIMEOUT) {
+		DBG_PORT80(0x30);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x30)\n");
+		return false;
+	}
+	return true;
 }
 
-
 /*
  * Description: Write a Byte to BASEBAND, by embedded programming
  *
@@ -2056,34 +2033,33 @@ bool BBbReadEmbedded (unsigned long dwIoBase, unsigned char byBBAddr, unsigned c
  * Return Value: true if succeeded; false if failed.
  *
  */
-bool BBbWriteEmbedded (unsigned long dwIoBase, unsigned char byBBAddr, unsigned char byData)
+bool BBbWriteEmbedded(unsigned long dwIoBase, unsigned char byBBAddr, unsigned char byData)
 {
-    unsigned short ww;
-    unsigned char byValue;
-
-    // BB reg offset
-    VNSvOutPortB(dwIoBase + MAC_REG_BBREGADR, byBBAddr);
-    // set BB data
-    VNSvOutPortB(dwIoBase + MAC_REG_BBREGDATA, byData);
-
-    // turn on BBREGCTL_REGW
-    MACvRegBitsOn(dwIoBase, MAC_REG_BBREGCTL, BBREGCTL_REGW);
-    // W_MAX_TIMEOUT is the timeout period
-    for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-        VNSvInPortB(dwIoBase + MAC_REG_BBREGCTL, &byValue);
-        if (byValue & BBREGCTL_DONE)
-            break;
-    }
-
-    if (ww == W_MAX_TIMEOUT) {
-        DBG_PORT80(0x31);
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" DBG_PORT80(0x31)\n");
-        return false;
-    }
-    return true;
+	unsigned short ww;
+	unsigned char byValue;
+
+	// BB reg offset
+	VNSvOutPortB(dwIoBase + MAC_REG_BBREGADR, byBBAddr);
+	// set BB data
+	VNSvOutPortB(dwIoBase + MAC_REG_BBREGDATA, byData);
+
+	// turn on BBREGCTL_REGW
+	MACvRegBitsOn(dwIoBase, MAC_REG_BBREGCTL, BBREGCTL_REGW);
+	// W_MAX_TIMEOUT is the timeout period
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		VNSvInPortB(dwIoBase + MAC_REG_BBREGCTL, &byValue);
+		if (byValue & BBREGCTL_DONE)
+			break;
+	}
+
+	if (ww == W_MAX_TIMEOUT) {
+		DBG_PORT80(0x31);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x31)\n");
+		return false;
+	}
+	return true;
 }
 
-
 /*
  * Description: Test if all bits are set for the Baseband register
  *
@@ -2098,15 +2074,14 @@ bool BBbWriteEmbedded (unsigned long dwIoBase, unsigned char byBBAddr, unsigned
  * Return Value: true if all TestBits are set; false otherwise.
  *
  */
-bool BBbIsRegBitsOn (unsigned long dwIoBase, unsigned char byBBAddr, unsigned char byTestBits)
+bool BBbIsRegBitsOn(unsigned long dwIoBase, unsigned char byBBAddr, unsigned char byTestBits)
 {
-    unsigned char byOrgData;
+	unsigned char byOrgData;
 
-    BBbReadEmbedded(dwIoBase, byBBAddr, &byOrgData);
-    return (byOrgData & byTestBits) == byTestBits;
+	BBbReadEmbedded(dwIoBase, byBBAddr, &byOrgData);
+	return (byOrgData & byTestBits) == byTestBits;
 }
 
-
 /*
  * Description: Test if all bits are clear for the Baseband register
  *
@@ -2121,12 +2096,12 @@ bool BBbIsRegBitsOn (unsigned long dwIoBase, unsigned char byBBAddr, unsigned ch
  * Return Value: true if all TestBits are clear; false otherwise.
  *
  */
-bool BBbIsRegBitsOff (unsigned long dwIoBase, unsigned char byBBAddr, unsigned char byTestBits)
+bool BBbIsRegBitsOff(unsigned long dwIoBase, unsigned char byBBAddr, unsigned char byTestBits)
 {
-    unsigned char byOrgData;
+	unsigned char byOrgData;
 
-    BBbReadEmbedded(dwIoBase, byBBAddr, &byOrgData);
-    return (byOrgData & byTestBits) == 0;
+	BBbReadEmbedded(dwIoBase, byBBAddr, &byOrgData);
+	return (byOrgData & byTestBits) == 0;
 }
 
 /*
@@ -2144,168 +2119,166 @@ bool BBbIsRegBitsOff (unsigned long dwIoBase, unsigned char byBBAddr, unsigned c
  *
  */
 
-bool BBbVT3253Init (PSDevice pDevice)
+bool BBbVT3253Init(PSDevice pDevice)
 {
-    bool bResult = true;
-    int        ii;
-    unsigned long dwIoBase = pDevice->PortOffset;
-    unsigned char byRFType = pDevice->byRFType;
-    unsigned char byLocalID = pDevice->byLocalID;
-
-    if (byRFType == RF_RFMD2959) {
-        if (byLocalID <= REV_ID_VT3253_A1) {
-            for (ii = 0; ii < CB_VT3253_INIT_FOR_RFMD; ii++) {
-                bResult &= BBbWriteEmbedded(dwIoBase,byVT3253InitTab_RFMD[ii][0],byVT3253InitTab_RFMD[ii][1]);
-            }
-        } else {
-            for (ii = 0; ii < CB_VT3253B0_INIT_FOR_RFMD; ii++) {
-                bResult &= BBbWriteEmbedded(dwIoBase,byVT3253B0_RFMD[ii][0],byVT3253B0_RFMD[ii][1]);
-            }
-            for (ii = 0; ii < CB_VT3253B0_AGC_FOR_RFMD2959; ii++) {
-        	    bResult &= BBbWriteEmbedded(dwIoBase,byVT3253B0_AGC4_RFMD2959[ii][0],byVT3253B0_AGC4_RFMD2959[ii][1]);
-            }
-            VNSvOutPortD(dwIoBase + MAC_REG_ITRTMSET, 0x23);
-            MACvRegBitsOn(dwIoBase, MAC_REG_PAPEDELAY, BIT0);
-        }
-        pDevice->abyBBVGA[0] = 0x18;
-        pDevice->abyBBVGA[1] = 0x0A;
-        pDevice->abyBBVGA[2] = 0x0;
-        pDevice->abyBBVGA[3] = 0x0;
-        pDevice->ldBmThreshold[0] = -70;
-        pDevice->ldBmThreshold[1] = -50;
-        pDevice->ldBmThreshold[2] = 0;
-        pDevice->ldBmThreshold[3] = 0;
-    } else if ((byRFType == RF_AIROHA) || (byRFType == RF_AL2230S) ) {
-        for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++) {
-    	    bResult &= BBbWriteEmbedded(dwIoBase,byVT3253B0_AIROHA2230[ii][0],byVT3253B0_AIROHA2230[ii][1]);
-    	}
-        for (ii = 0; ii < CB_VT3253B0_AGC; ii++) {
-    	    bResult &= BBbWriteEmbedded(dwIoBase,byVT3253B0_AGC[ii][0],byVT3253B0_AGC[ii][1]);
-    	}
-        pDevice->abyBBVGA[0] = 0x1C;
-        pDevice->abyBBVGA[1] = 0x10;
-        pDevice->abyBBVGA[2] = 0x0;
-        pDevice->abyBBVGA[3] = 0x0;
-        pDevice->ldBmThreshold[0] = -70;
-        pDevice->ldBmThreshold[1] = -48;
-        pDevice->ldBmThreshold[2] = 0;
-        pDevice->ldBmThreshold[3] = 0;
-    } else if (byRFType == RF_UW2451) {
-        for (ii = 0; ii < CB_VT3253B0_INIT_FOR_UW2451; ii++) {
-    	        bResult &= BBbWriteEmbedded(dwIoBase,byVT3253B0_UW2451[ii][0],byVT3253B0_UW2451[ii][1]);
-    	}
-        for (ii = 0; ii < CB_VT3253B0_AGC; ii++) {
-    	    bResult &= BBbWriteEmbedded(dwIoBase,byVT3253B0_AGC[ii][0],byVT3253B0_AGC[ii][1]);
-    	}
-        VNSvOutPortB(dwIoBase + MAC_REG_ITRTMSET, 0x23);
-        MACvRegBitsOn(dwIoBase, MAC_REG_PAPEDELAY, BIT0);
-
-        pDevice->abyBBVGA[0] = 0x14;
-        pDevice->abyBBVGA[1] = 0x0A;
-        pDevice->abyBBVGA[2] = 0x0;
-        pDevice->abyBBVGA[3] = 0x0;
-        pDevice->ldBmThreshold[0] = -60;
-        pDevice->ldBmThreshold[1] = -50;
-        pDevice->ldBmThreshold[2] = 0;
-        pDevice->ldBmThreshold[3] = 0;
-    } else if (byRFType == RF_UW2452) {
-        for (ii = 0; ii < CB_VT3253B0_INIT_FOR_UW2451; ii++) {
-            bResult &= BBbWriteEmbedded(dwIoBase,byVT3253B0_UW2451[ii][0],byVT3253B0_UW2451[ii][1]);
-    	}
-        // Init ANT B select,TX Config CR09 = 0x61->0x45, 0x45->0x41(VC1/VC2 define, make the ANT_A, ANT_B inverted)
-        //bResult &= BBbWriteEmbedded(dwIoBase,0x09,0x41);
-        // Init ANT B select,RX Config CR10 = 0x28->0x2A, 0x2A->0x28(VC1/VC2 define, make the ANT_A, ANT_B inverted)
-        //bResult &= BBbWriteEmbedded(dwIoBase,0x0a,0x28);
-        // Select VC1/VC2, CR215 = 0x02->0x06
-        bResult &= BBbWriteEmbedded(dwIoBase,0xd7,0x06);
-
-        //{{RobertYu:20050125, request by Jack
-        bResult &= BBbWriteEmbedded(dwIoBase,0x90,0x20);
-        bResult &= BBbWriteEmbedded(dwIoBase,0x97,0xeb);
-        //}}
-
-        //{{RobertYu:20050221, request by Jack
-        bResult &= BBbWriteEmbedded(dwIoBase,0xa6,0x00);
-        bResult &= BBbWriteEmbedded(dwIoBase,0xa8,0x30);
-        //}}
-        bResult &= BBbWriteEmbedded(dwIoBase,0xb0,0x58);
-
-        for (ii = 0; ii < CB_VT3253B0_AGC; ii++) {
-    	    bResult &= BBbWriteEmbedded(dwIoBase,byVT3253B0_AGC[ii][0],byVT3253B0_AGC[ii][1]);
-    	}
-        //VNSvOutPortB(dwIoBase + MAC_REG_ITRTMSET, 0x23); // RobertYu: 20050104, 20050131 disable PA_Delay
-        //MACvRegBitsOn(dwIoBase, MAC_REG_PAPEDELAY, BIT0); // RobertYu: 20050104, 20050131 disable PA_Delay
-
-        pDevice->abyBBVGA[0] = 0x14;
-        pDevice->abyBBVGA[1] = 0x0A;
-        pDevice->abyBBVGA[2] = 0x0;
-        pDevice->abyBBVGA[3] = 0x0;
-        pDevice->ldBmThreshold[0] = -60;
-        pDevice->ldBmThreshold[1] = -50;
-        pDevice->ldBmThreshold[2] = 0;
-        pDevice->ldBmThreshold[3] = 0;
-    //}} RobertYu
-
-    } else if (byRFType == RF_VT3226) {
-        for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++) {
-    	    bResult &= BBbWriteEmbedded(dwIoBase,byVT3253B0_AIROHA2230[ii][0],byVT3253B0_AIROHA2230[ii][1]);
-    	}
-        for (ii = 0; ii < CB_VT3253B0_AGC; ii++) {
-    	    bResult &= BBbWriteEmbedded(dwIoBase,byVT3253B0_AGC[ii][0],byVT3253B0_AGC[ii][1]);
-    	}
-        pDevice->abyBBVGA[0] = 0x1C;
-        pDevice->abyBBVGA[1] = 0x10;
-        pDevice->abyBBVGA[2] = 0x0;
-        pDevice->abyBBVGA[3] = 0x0;
-        pDevice->ldBmThreshold[0] = -70;
-        pDevice->ldBmThreshold[1] = -48;
-        pDevice->ldBmThreshold[2] = 0;
-        pDevice->ldBmThreshold[3] = 0;
-        // Fix VT3226 DFC system timing issue
-        MACvSetRFLE_LatchBase(dwIoBase);
-         //{{ RobertYu: 20050104
-    } else if (byRFType == RF_AIROHA7230) {
-        for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++) {
-    	    bResult &= BBbWriteEmbedded(dwIoBase,byVT3253B0_AIROHA2230[ii][0],byVT3253B0_AIROHA2230[ii][1]);
-    	}
-
-        //{{ RobertYu:20050223, request by JerryChung
-        // Init ANT B select,TX Config CR09 = 0x61->0x45, 0x45->0x41(VC1/VC2 define, make the ANT_A, ANT_B inverted)
-        //bResult &= BBbWriteEmbedded(dwIoBase,0x09,0x41);
-        // Init ANT B select,RX Config CR10 = 0x28->0x2A, 0x2A->0x28(VC1/VC2 define, make the ANT_A, ANT_B inverted)
-        //bResult &= BBbWriteEmbedded(dwIoBase,0x0a,0x28);
-        // Select VC1/VC2, CR215 = 0x02->0x06
-        bResult &= BBbWriteEmbedded(dwIoBase,0xd7,0x06);
-        //}}
-
-        for (ii = 0; ii < CB_VT3253B0_AGC; ii++) {
-    	    bResult &= BBbWriteEmbedded(dwIoBase,byVT3253B0_AGC[ii][0],byVT3253B0_AGC[ii][1]);
-    	}
-        pDevice->abyBBVGA[0] = 0x1C;
-        pDevice->abyBBVGA[1] = 0x10;
-        pDevice->abyBBVGA[2] = 0x0;
-        pDevice->abyBBVGA[3] = 0x0;
-        pDevice->ldBmThreshold[0] = -70;
-        pDevice->ldBmThreshold[1] = -48;
-        pDevice->ldBmThreshold[2] = 0;
-        pDevice->ldBmThreshold[3] = 0;
-    //}} RobertYu
-    } else {
-    	// No VGA Table now
-    	pDevice->bUpdateBBVGA = false;
-        pDevice->abyBBVGA[0] = 0x1C;
-    }
-
-    if (byLocalID > REV_ID_VT3253_A1) {
-        BBbWriteEmbedded(dwIoBase, 0x04, 0x7F);
-        BBbWriteEmbedded(dwIoBase, 0x0D, 0x01);
-    }
-
-    return bResult;
+	bool bResult = true;
+	int        ii;
+	unsigned long dwIoBase = pDevice->PortOffset;
+	unsigned char byRFType = pDevice->byRFType;
+	unsigned char byLocalID = pDevice->byLocalID;
+
+	if (byRFType == RF_RFMD2959) {
+		if (byLocalID <= REV_ID_VT3253_A1) {
+			for (ii = 0; ii < CB_VT3253_INIT_FOR_RFMD; ii++) {
+				bResult &= BBbWriteEmbedded(dwIoBase, byVT3253InitTab_RFMD[ii][0], byVT3253InitTab_RFMD[ii][1]);
+			}
+		} else {
+			for (ii = 0; ii < CB_VT3253B0_INIT_FOR_RFMD; ii++) {
+				bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_RFMD[ii][0], byVT3253B0_RFMD[ii][1]);
+			}
+			for (ii = 0; ii < CB_VT3253B0_AGC_FOR_RFMD2959; ii++) {
+				bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AGC4_RFMD2959[ii][0], byVT3253B0_AGC4_RFMD2959[ii][1]);
+			}
+			VNSvOutPortD(dwIoBase + MAC_REG_ITRTMSET, 0x23);
+			MACvRegBitsOn(dwIoBase, MAC_REG_PAPEDELAY, BIT0);
+		}
+		pDevice->abyBBVGA[0] = 0x18;
+		pDevice->abyBBVGA[1] = 0x0A;
+		pDevice->abyBBVGA[2] = 0x0;
+		pDevice->abyBBVGA[3] = 0x0;
+		pDevice->ldBmThreshold[0] = -70;
+		pDevice->ldBmThreshold[1] = -50;
+		pDevice->ldBmThreshold[2] = 0;
+		pDevice->ldBmThreshold[3] = 0;
+	} else if ((byRFType == RF_AIROHA) || (byRFType == RF_AL2230S)) {
+		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++) {
+			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AIROHA2230[ii][0], byVT3253B0_AIROHA2230[ii][1]);
+		}
+		for (ii = 0; ii < CB_VT3253B0_AGC; ii++) {
+			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
+		}
+		pDevice->abyBBVGA[0] = 0x1C;
+		pDevice->abyBBVGA[1] = 0x10;
+		pDevice->abyBBVGA[2] = 0x0;
+		pDevice->abyBBVGA[3] = 0x0;
+		pDevice->ldBmThreshold[0] = -70;
+		pDevice->ldBmThreshold[1] = -48;
+		pDevice->ldBmThreshold[2] = 0;
+		pDevice->ldBmThreshold[3] = 0;
+	} else if (byRFType == RF_UW2451) {
+		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_UW2451; ii++) {
+			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_UW2451[ii][0], byVT3253B0_UW2451[ii][1]);
+		}
+		for (ii = 0; ii < CB_VT3253B0_AGC; ii++) {
+			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
+		}
+		VNSvOutPortB(dwIoBase + MAC_REG_ITRTMSET, 0x23);
+		MACvRegBitsOn(dwIoBase, MAC_REG_PAPEDELAY, BIT0);
+
+		pDevice->abyBBVGA[0] = 0x14;
+		pDevice->abyBBVGA[1] = 0x0A;
+		pDevice->abyBBVGA[2] = 0x0;
+		pDevice->abyBBVGA[3] = 0x0;
+		pDevice->ldBmThreshold[0] = -60;
+		pDevice->ldBmThreshold[1] = -50;
+		pDevice->ldBmThreshold[2] = 0;
+		pDevice->ldBmThreshold[3] = 0;
+	} else if (byRFType == RF_UW2452) {
+		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_UW2451; ii++) {
+			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_UW2451[ii][0], byVT3253B0_UW2451[ii][1]);
+		}
+		// Init ANT B select,TX Config CR09 = 0x61->0x45, 0x45->0x41(VC1/VC2 define, make the ANT_A, ANT_B inverted)
+		//bResult &= BBbWriteEmbedded(dwIoBase,0x09,0x41);
+		// Init ANT B select,RX Config CR10 = 0x28->0x2A, 0x2A->0x28(VC1/VC2 define, make the ANT_A, ANT_B inverted)
+		//bResult &= BBbWriteEmbedded(dwIoBase,0x0a,0x28);
+		// Select VC1/VC2, CR215 = 0x02->0x06
+		bResult &= BBbWriteEmbedded(dwIoBase, 0xd7, 0x06);
+
+		//{{RobertYu:20050125, request by Jack
+		bResult &= BBbWriteEmbedded(dwIoBase, 0x90, 0x20);
+		bResult &= BBbWriteEmbedded(dwIoBase, 0x97, 0xeb);
+		//}}
+
+		//{{RobertYu:20050221, request by Jack
+		bResult &= BBbWriteEmbedded(dwIoBase, 0xa6, 0x00);
+		bResult &= BBbWriteEmbedded(dwIoBase, 0xa8, 0x30);
+		//}}
+		bResult &= BBbWriteEmbedded(dwIoBase, 0xb0, 0x58);
+
+		for (ii = 0; ii < CB_VT3253B0_AGC; ii++) {
+			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
+		}
+		//VNSvOutPortB(dwIoBase + MAC_REG_ITRTMSET, 0x23); // RobertYu: 20050104, 20050131 disable PA_Delay
+		//MACvRegBitsOn(dwIoBase, MAC_REG_PAPEDELAY, BIT0); // RobertYu: 20050104, 20050131 disable PA_Delay
+
+		pDevice->abyBBVGA[0] = 0x14;
+		pDevice->abyBBVGA[1] = 0x0A;
+		pDevice->abyBBVGA[2] = 0x0;
+		pDevice->abyBBVGA[3] = 0x0;
+		pDevice->ldBmThreshold[0] = -60;
+		pDevice->ldBmThreshold[1] = -50;
+		pDevice->ldBmThreshold[2] = 0;
+		pDevice->ldBmThreshold[3] = 0;
+		//}} RobertYu
+
+	} else if (byRFType == RF_VT3226) {
+		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++) {
+			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AIROHA2230[ii][0], byVT3253B0_AIROHA2230[ii][1]);
+		}
+		for (ii = 0; ii < CB_VT3253B0_AGC; ii++) {
+			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
+		}
+		pDevice->abyBBVGA[0] = 0x1C;
+		pDevice->abyBBVGA[1] = 0x10;
+		pDevice->abyBBVGA[2] = 0x0;
+		pDevice->abyBBVGA[3] = 0x0;
+		pDevice->ldBmThreshold[0] = -70;
+		pDevice->ldBmThreshold[1] = -48;
+		pDevice->ldBmThreshold[2] = 0;
+		pDevice->ldBmThreshold[3] = 0;
+		// Fix VT3226 DFC system timing issue
+		MACvSetRFLE_LatchBase(dwIoBase);
+		//{{ RobertYu: 20050104
+	} else if (byRFType == RF_AIROHA7230) {
+		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++) {
+			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AIROHA2230[ii][0], byVT3253B0_AIROHA2230[ii][1]);
+		}
+
+		//{{ RobertYu:20050223, request by JerryChung
+		// Init ANT B select,TX Config CR09 = 0x61->0x45, 0x45->0x41(VC1/VC2 define, make the ANT_A, ANT_B inverted)
+		//bResult &= BBbWriteEmbedded(dwIoBase,0x09,0x41);
+		// Init ANT B select,RX Config CR10 = 0x28->0x2A, 0x2A->0x28(VC1/VC2 define, make the ANT_A, ANT_B inverted)
+		//bResult &= BBbWriteEmbedded(dwIoBase,0x0a,0x28);
+		// Select VC1/VC2, CR215 = 0x02->0x06
+		bResult &= BBbWriteEmbedded(dwIoBase, 0xd7, 0x06);
+		//}}
+
+		for (ii = 0; ii < CB_VT3253B0_AGC; ii++) {
+			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
+		}
+		pDevice->abyBBVGA[0] = 0x1C;
+		pDevice->abyBBVGA[1] = 0x10;
+		pDevice->abyBBVGA[2] = 0x0;
+		pDevice->abyBBVGA[3] = 0x0;
+		pDevice->ldBmThreshold[0] = -70;
+		pDevice->ldBmThreshold[1] = -48;
+		pDevice->ldBmThreshold[2] = 0;
+		pDevice->ldBmThreshold[3] = 0;
+		//}} RobertYu
+	} else {
+		// No VGA Table now
+		pDevice->bUpdateBBVGA = false;
+		pDevice->abyBBVGA[0] = 0x1C;
+	}
+
+	if (byLocalID > REV_ID_VT3253_A1) {
+		BBbWriteEmbedded(dwIoBase, 0x04, 0x7F);
+		BBbWriteEmbedded(dwIoBase, 0x0D, 0x01);
+	}
+
+	return bResult;
 }
 
-
-
 /*
  * Description: Read All Baseband Registers
  *
@@ -2319,14 +2292,14 @@ bool BBbVT3253Init (PSDevice pDevice)
  * Return Value: none
  *
  */
-void BBvReadAllRegs (unsigned long dwIoBase, unsigned char *pbyBBRegs)
+void BBvReadAllRegs(unsigned long dwIoBase, unsigned char *pbyBBRegs)
 {
-    int  ii;
-    unsigned char byBase = 1;
-    for (ii = 0; ii < BB_MAX_CONTEXT_SIZE; ii++) {
-        BBbReadEmbedded(dwIoBase, (unsigned char)(ii*byBase), pbyBBRegs);
-        pbyBBRegs += byBase;
-    }
+	int  ii;
+	unsigned char byBase = 1;
+	for (ii = 0; ii < BB_MAX_CONTEXT_SIZE; ii++) {
+		BBbReadEmbedded(dwIoBase, (unsigned char)(ii*byBase), pbyBBRegs);
+		pbyBBRegs += byBase;
+	}
 }
 
 /*
@@ -2343,46 +2316,44 @@ void BBvReadAllRegs (unsigned long dwIoBase, unsigned char *pbyBBRegs)
  *
  */
 
-
-void BBvLoopbackOn (PSDevice pDevice)
+void BBvLoopbackOn(PSDevice pDevice)
 {
-    unsigned char byData;
-    unsigned long dwIoBase = pDevice->PortOffset;
-
-    //CR C9 = 0x00
-    BBbReadEmbedded(dwIoBase, 0xC9, &pDevice->byBBCRc9);//CR201
-    BBbWriteEmbedded(dwIoBase, 0xC9, 0);
-    BBbReadEmbedded(dwIoBase, 0x4D, &pDevice->byBBCR4d);//CR77
-    BBbWriteEmbedded(dwIoBase, 0x4D, 0x90);
-
-    //CR 88 = 0x02(CCK), 0x03(OFDM)
-    BBbReadEmbedded(dwIoBase, 0x88, &pDevice->byBBCR88);//CR136
-
-    if (pDevice->uConnectionRate <= RATE_11M) { //CCK
-        // Enable internal digital loopback: CR33 |= 0000 0001
-        BBbReadEmbedded(dwIoBase, 0x21, &byData);//CR33
-        BBbWriteEmbedded(dwIoBase, 0x21, (unsigned char)(byData | 0x01));//CR33
-        // CR154 = 0x00
-        BBbWriteEmbedded(dwIoBase, 0x9A, 0);   //CR154
-
-        BBbWriteEmbedded(dwIoBase, 0x88, 0x02);//CR239
-    }
-    else { //OFDM
-        // Enable internal digital loopback:CR154 |= 0000 0001
-        BBbReadEmbedded(dwIoBase, 0x9A, &byData);//CR154
-        BBbWriteEmbedded(dwIoBase, 0x9A, (unsigned char)(byData | 0x01));//CR154
-        // CR33 = 0x00
-        BBbWriteEmbedded(dwIoBase, 0x21, 0);   //CR33
-
-        BBbWriteEmbedded(dwIoBase, 0x88, 0x03);//CR239
-    }
-
-    //CR14 = 0x00
-    BBbWriteEmbedded(dwIoBase, 0x0E, 0);//CR14
-
-    // Disable TX_IQUN
-    BBbReadEmbedded(pDevice->PortOffset, 0x09, &pDevice->byBBCR09);
-    BBbWriteEmbedded(pDevice->PortOffset, 0x09, (unsigned char)(pDevice->byBBCR09 & 0xDE));
+	unsigned char byData;
+	unsigned long dwIoBase = pDevice->PortOffset;
+
+	//CR C9 = 0x00
+	BBbReadEmbedded(dwIoBase, 0xC9, &pDevice->byBBCRc9);//CR201
+	BBbWriteEmbedded(dwIoBase, 0xC9, 0);
+	BBbReadEmbedded(dwIoBase, 0x4D, &pDevice->byBBCR4d);//CR77
+	BBbWriteEmbedded(dwIoBase, 0x4D, 0x90);
+
+	//CR 88 = 0x02(CCK), 0x03(OFDM)
+	BBbReadEmbedded(dwIoBase, 0x88, &pDevice->byBBCR88);//CR136
+
+	if (pDevice->uConnectionRate <= RATE_11M) { //CCK
+		// Enable internal digital loopback: CR33 |= 0000 0001
+		BBbReadEmbedded(dwIoBase, 0x21, &byData);//CR33
+		BBbWriteEmbedded(dwIoBase, 0x21, (unsigned char)(byData | 0x01));//CR33
+		// CR154 = 0x00
+		BBbWriteEmbedded(dwIoBase, 0x9A, 0);   //CR154
+
+		BBbWriteEmbedded(dwIoBase, 0x88, 0x02);//CR239
+	} else { //OFDM
+		// Enable internal digital loopback:CR154 |= 0000 0001
+		BBbReadEmbedded(dwIoBase, 0x9A, &byData);//CR154
+		BBbWriteEmbedded(dwIoBase, 0x9A, (unsigned char)(byData | 0x01));//CR154
+		// CR33 = 0x00
+		BBbWriteEmbedded(dwIoBase, 0x21, 0);   //CR33
+
+		BBbWriteEmbedded(dwIoBase, 0x88, 0x03);//CR239
+	}
+
+	//CR14 = 0x00
+	BBbWriteEmbedded(dwIoBase, 0x0E, 0);//CR14
+
+	// Disable TX_IQUN
+	BBbReadEmbedded(pDevice->PortOffset, 0x09, &pDevice->byBBCR09);
+	BBbWriteEmbedded(pDevice->PortOffset, 0x09, (unsigned char)(pDevice->byBBCR09 & 0xDE));
 }
 
 /*
@@ -2398,32 +2369,28 @@ void BBvLoopbackOn (PSDevice pDevice)
  * Return Value: none
  *
  */
-void BBvLoopbackOff (PSDevice pDevice)
+void BBvLoopbackOff(PSDevice pDevice)
 {
-    unsigned char byData;
-    unsigned long dwIoBase = pDevice->PortOffset;
-
-    BBbWriteEmbedded(dwIoBase, 0xC9, pDevice->byBBCRc9);//CR201
-    BBbWriteEmbedded(dwIoBase, 0x88, pDevice->byBBCR88);//CR136
-    BBbWriteEmbedded(dwIoBase, 0x09, pDevice->byBBCR09);//CR136
-    BBbWriteEmbedded(dwIoBase, 0x4D, pDevice->byBBCR4d);//CR77
-
-    if (pDevice->uConnectionRate <= RATE_11M) { // CCK
-        // Set the CR33 Bit2 to disable internal Loopback.
-        BBbReadEmbedded(dwIoBase, 0x21, &byData);//CR33
-        BBbWriteEmbedded(dwIoBase, 0x21, (unsigned char)(byData & 0xFE));//CR33
-    }
-    else { // OFDM
-        BBbReadEmbedded(dwIoBase, 0x9A, &byData);//CR154
-        BBbWriteEmbedded(dwIoBase, 0x9A, (unsigned char)(byData & 0xFE));//CR154
-    }
-    BBbReadEmbedded(dwIoBase, 0x0E, &byData);//CR14
-    BBbWriteEmbedded(dwIoBase, 0x0E, (unsigned char)(byData | 0x80));//CR14
-
+	unsigned char byData;
+	unsigned long dwIoBase = pDevice->PortOffset;
+
+	BBbWriteEmbedded(dwIoBase, 0xC9, pDevice->byBBCRc9);//CR201
+	BBbWriteEmbedded(dwIoBase, 0x88, pDevice->byBBCR88);//CR136
+	BBbWriteEmbedded(dwIoBase, 0x09, pDevice->byBBCR09);//CR136
+	BBbWriteEmbedded(dwIoBase, 0x4D, pDevice->byBBCR4d);//CR77
+
+	if (pDevice->uConnectionRate <= RATE_11M) { // CCK
+		// Set the CR33 Bit2 to disable internal Loopback.
+		BBbReadEmbedded(dwIoBase, 0x21, &byData);//CR33
+		BBbWriteEmbedded(dwIoBase, 0x21, (unsigned char)(byData & 0xFE));//CR33
+	} else { // OFDM
+		BBbReadEmbedded(dwIoBase, 0x9A, &byData);//CR154
+		BBbWriteEmbedded(dwIoBase, 0x9A, (unsigned char)(byData & 0xFE));//CR154
+	}
+	BBbReadEmbedded(dwIoBase, 0x0E, &byData);//CR14
+	BBbWriteEmbedded(dwIoBase, 0x0E, (unsigned char)(byData | 0x80));//CR14
 }
 
-
-
 /*
  * Description: Set ShortSlotTime mode
  *
@@ -2437,49 +2404,47 @@ void BBvLoopbackOff (PSDevice pDevice)
  *
  */
 void
-BBvSetShortSlotTime (PSDevice pDevice)
+BBvSetShortSlotTime(PSDevice pDevice)
 {
-    unsigned char byBBRxConf=0;
-    unsigned char byBBVGA=0;
+	unsigned char byBBRxConf = 0;
+	unsigned char byBBVGA = 0;
 
-    BBbReadEmbedded(pDevice->PortOffset, 0x0A, &byBBRxConf);//CR10
+	BBbReadEmbedded(pDevice->PortOffset, 0x0A, &byBBRxConf);//CR10
 
-    if (pDevice->bShortSlotTime) {
-        byBBRxConf &= 0xDF;//1101 1111
-    } else {
-        byBBRxConf |= 0x20;//0010 0000
-    }
+	if (pDevice->bShortSlotTime) {
+		byBBRxConf &= 0xDF;//1101 1111
+	} else {
+		byBBRxConf |= 0x20;//0010 0000
+	}
 
-    // patch for 3253B0 Baseband with Cardbus module
-    BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byBBVGA);
-    if (byBBVGA == pDevice->abyBBVGA[0]) {
-        byBBRxConf |= 0x20;//0010 0000
-    }
-
-    BBbWriteEmbedded(pDevice->PortOffset, 0x0A, byBBRxConf);//CR10
+	// patch for 3253B0 Baseband with Cardbus module
+	BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byBBVGA);
+	if (byBBVGA == pDevice->abyBBVGA[0]) {
+		byBBRxConf |= 0x20;//0010 0000
+	}
 
+	BBbWriteEmbedded(pDevice->PortOffset, 0x0A, byBBRxConf);//CR10
 }
 
 void BBvSetVGAGainOffset(PSDevice pDevice, unsigned char byData)
 {
-    unsigned char byBBRxConf=0;
-
-    BBbWriteEmbedded(pDevice->PortOffset, 0xE7, byData);
-
-    BBbReadEmbedded(pDevice->PortOffset, 0x0A, &byBBRxConf);//CR10
-    // patch for 3253B0 Baseband with Cardbus module
-    if (byData == pDevice->abyBBVGA[0]) {
-        byBBRxConf |= 0x20;//0010 0000
-    } else if (pDevice->bShortSlotTime) {
-        byBBRxConf &= 0xDF;//1101 1111
-    } else {
-        byBBRxConf |= 0x20;//0010 0000
-    }
-    pDevice->byBBVGACurrent = byData;
-    BBbWriteEmbedded(pDevice->PortOffset, 0x0A, byBBRxConf);//CR10
+	unsigned char byBBRxConf = 0;
+
+	BBbWriteEmbedded(pDevice->PortOffset, 0xE7, byData);
+
+	BBbReadEmbedded(pDevice->PortOffset, 0x0A, &byBBRxConf);//CR10
+	// patch for 3253B0 Baseband with Cardbus module
+	if (byData == pDevice->abyBBVGA[0]) {
+		byBBRxConf |= 0x20;//0010 0000
+	} else if (pDevice->bShortSlotTime) {
+		byBBRxConf &= 0xDF;//1101 1111
+	} else {
+		byBBRxConf |= 0x20;//0010 0000
+	}
+	pDevice->byBBVGACurrent = byData;
+	BBbWriteEmbedded(pDevice->PortOffset, 0x0A, byBBRxConf);//CR10
 }
 
-
 /*
  * Description: Baseband SoftwareReset
  *
@@ -2493,12 +2458,12 @@ void BBvSetVGAGainOffset(PSDevice pDevice, unsigned char byData)
  *
  */
 void
-BBvSoftwareReset (unsigned long dwIoBase)
+BBvSoftwareReset(unsigned long dwIoBase)
 {
-    BBbWriteEmbedded(dwIoBase, 0x50, 0x40);
-    BBbWriteEmbedded(dwIoBase, 0x50, 0);
-    BBbWriteEmbedded(dwIoBase, 0x9C, 0x01);
-    BBbWriteEmbedded(dwIoBase, 0x9C, 0);
+	BBbWriteEmbedded(dwIoBase, 0x50, 0x40);
+	BBbWriteEmbedded(dwIoBase, 0x50, 0);
+	BBbWriteEmbedded(dwIoBase, 0x9C, 0x01);
+	BBbWriteEmbedded(dwIoBase, 0x9C, 0);
 }
 
 /*
@@ -2514,13 +2479,13 @@ BBvSoftwareReset (unsigned long dwIoBase)
  *
  */
 void
-BBvPowerSaveModeON (unsigned long dwIoBase)
+BBvPowerSaveModeON(unsigned long dwIoBase)
 {
-    unsigned char byOrgData;
+	unsigned char byOrgData;
 
-    BBbReadEmbedded(dwIoBase, 0x0D, &byOrgData);
-    byOrgData |= BIT0;
-    BBbWriteEmbedded(dwIoBase, 0x0D, byOrgData);
+	BBbReadEmbedded(dwIoBase, 0x0D, &byOrgData);
+	byOrgData |= BIT0;
+	BBbWriteEmbedded(dwIoBase, 0x0D, byOrgData);
 }
 
 /*
@@ -2536,13 +2501,13 @@ BBvPowerSaveModeON (unsigned long dwIoBase)
  *
  */
 void
-BBvPowerSaveModeOFF (unsigned long dwIoBase)
+BBvPowerSaveModeOFF(unsigned long dwIoBase)
 {
-    unsigned char byOrgData;
+	unsigned char byOrgData;
 
-    BBbReadEmbedded(dwIoBase, 0x0D, &byOrgData);
-    byOrgData &= ~(BIT0);
-    BBbWriteEmbedded(dwIoBase, 0x0D, byOrgData);
+	BBbReadEmbedded(dwIoBase, 0x0D, &byOrgData);
+	byOrgData &= ~(BIT0);
+	BBbWriteEmbedded(dwIoBase, 0x0D, byOrgData);
 }
 
 /*
@@ -2560,33 +2525,24 @@ BBvPowerSaveModeOFF (unsigned long dwIoBase)
  */
 
 void
-BBvSetTxAntennaMode (unsigned long dwIoBase, unsigned char byAntennaMode)
+BBvSetTxAntennaMode(unsigned long dwIoBase, unsigned char byAntennaMode)
 {
-    unsigned char byBBTxConf;
-
-#ifdef	PLICE_DEBUG
-	//printk("Enter BBvSetTxAntennaMode\n");
-#endif
-    BBbReadEmbedded(dwIoBase, 0x09, &byBBTxConf);//CR09
-    if (byAntennaMode == ANT_DIVERSITY) {
-        // bit 1 is diversity
-        byBBTxConf |= 0x02;
-    } else if (byAntennaMode == ANT_A) {
-        // bit 2 is ANTSEL
-        byBBTxConf &= 0xF9; // 1111 1001
-    } else if (byAntennaMode == ANT_B) {
-#ifdef	PLICE_DEBUG
-	//printk("BBvSetTxAntennaMode:ANT_B\n");
-#endif
-        byBBTxConf &= 0xFD; // 1111 1101
-        byBBTxConf |= 0x04;
-    }
-    BBbWriteEmbedded(dwIoBase, 0x09, byBBTxConf);//CR09
+	unsigned char byBBTxConf;
+
+	BBbReadEmbedded(dwIoBase, 0x09, &byBBTxConf);//CR09
+	if (byAntennaMode == ANT_DIVERSITY) {
+		// bit 1 is diversity
+		byBBTxConf |= 0x02;
+	} else if (byAntennaMode == ANT_A) {
+		// bit 2 is ANTSEL
+		byBBTxConf &= 0xF9; // 1111 1001
+	} else if (byAntennaMode == ANT_B) {
+		byBBTxConf &= 0xFD; // 1111 1101
+		byBBTxConf |= 0x04;
+	}
+	BBbWriteEmbedded(dwIoBase, 0x09, byBBTxConf);//CR09
 }
 
-
-
-
 /*
  * Description: Set Rx Antenna mode
  *
@@ -2602,24 +2558,23 @@ BBvSetTxAntennaMode (unsigned long dwIoBase, unsigned char byAntennaMode)
  */
 
 void
-BBvSetRxAntennaMode (unsigned long dwIoBase, unsigned char byAntennaMode)
+BBvSetRxAntennaMode(unsigned long dwIoBase, unsigned char byAntennaMode)
 {
-    unsigned char byBBRxConf;
-
-    BBbReadEmbedded(dwIoBase, 0x0A, &byBBRxConf);//CR10
-    if (byAntennaMode == ANT_DIVERSITY) {
-        byBBRxConf |= 0x01;
-
-    } else if (byAntennaMode == ANT_A) {
-        byBBRxConf &= 0xFC; // 1111 1100
-    } else if (byAntennaMode == ANT_B) {
-        byBBRxConf &= 0xFE; // 1111 1110
-        byBBRxConf |= 0x02;
-    }
-    BBbWriteEmbedded(dwIoBase, 0x0A, byBBRxConf);//CR10
+	unsigned char byBBRxConf;
+
+	BBbReadEmbedded(dwIoBase, 0x0A, &byBBRxConf);//CR10
+	if (byAntennaMode == ANT_DIVERSITY) {
+		byBBRxConf |= 0x01;
+
+	} else if (byAntennaMode == ANT_A) {
+		byBBRxConf &= 0xFC; // 1111 1100
+	} else if (byAntennaMode == ANT_B) {
+		byBBRxConf &= 0xFE; // 1111 1110
+		byBBRxConf |= 0x02;
+	}
+	BBbWriteEmbedded(dwIoBase, 0x0A, byBBRxConf);//CR10
 }
 
-
 /*
  * Description: BBvSetDeepSleep
  *
@@ -2633,142 +2588,138 @@ BBvSetRxAntennaMode (unsigned long dwIoBase, unsigned char byAntennaMode)
  *
  */
 void
-BBvSetDeepSleep (unsigned long dwIoBase, unsigned char byLocalID)
+BBvSetDeepSleep(unsigned long dwIoBase, unsigned char byLocalID)
 {
-    BBbWriteEmbedded(dwIoBase, 0x0C, 0x17);//CR12
-    BBbWriteEmbedded(dwIoBase, 0x0D, 0xB9);//CR13
+	BBbWriteEmbedded(dwIoBase, 0x0C, 0x17);//CR12
+	BBbWriteEmbedded(dwIoBase, 0x0D, 0xB9);//CR13
 }
 
 void
-BBvExitDeepSleep (unsigned long dwIoBase, unsigned char byLocalID)
+BBvExitDeepSleep(unsigned long dwIoBase, unsigned char byLocalID)
 {
-    BBbWriteEmbedded(dwIoBase, 0x0C, 0x00);//CR12
-    BBbWriteEmbedded(dwIoBase, 0x0D, 0x01);//CR13
+	BBbWriteEmbedded(dwIoBase, 0x0C, 0x00);//CR12
+	BBbWriteEmbedded(dwIoBase, 0x0D, 0x01);//CR13
 }
 
-
-
 static
 unsigned long
-s_ulGetRatio (PSDevice pDevice)
+s_ulGetRatio(PSDevice pDevice)
 {
-unsigned long ulRatio = 0;
-unsigned long ulMaxPacket;
-unsigned long ulPacketNum;
-
-    //This is a thousand-ratio
-    ulMaxPacket = pDevice->uNumSQ3[RATE_54M];
-    if ( pDevice->uNumSQ3[RATE_54M] != 0 ) {
-        ulPacketNum = pDevice->uNumSQ3[RATE_54M];
-        ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-        //ulRatio = (pDevice->uNumSQ3[RATE_54M] * 1000 / pDevice->uDiversityCnt);
-        ulRatio += TOP_RATE_54M;
-    }
-    if ( pDevice->uNumSQ3[RATE_48M] > ulMaxPacket ) {
-        ulPacketNum = pDevice->uNumSQ3[RATE_54M] + pDevice->uNumSQ3[RATE_48M];
-        ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-        //ulRatio = (pDevice->uNumSQ3[RATE_48M] * 1000 / pDevice->uDiversityCnt);
-        ulRatio += TOP_RATE_48M;
-        ulMaxPacket = pDevice->uNumSQ3[RATE_48M];
-    }
-    if ( pDevice->uNumSQ3[RATE_36M] > ulMaxPacket ) {
-        ulPacketNum = pDevice->uNumSQ3[RATE_54M] + pDevice->uNumSQ3[RATE_48M] +
-                      pDevice->uNumSQ3[RATE_36M];
-        ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-        //ulRatio = (pDevice->uNumSQ3[RATE_36M] * 1000 / pDevice->uDiversityCnt);
-        ulRatio += TOP_RATE_36M;
-        ulMaxPacket = pDevice->uNumSQ3[RATE_36M];
-    }
-    if ( pDevice->uNumSQ3[RATE_24M] > ulMaxPacket ) {
-        ulPacketNum = pDevice->uNumSQ3[RATE_54M] + pDevice->uNumSQ3[RATE_48M] +
-                      pDevice->uNumSQ3[RATE_36M] + pDevice->uNumSQ3[RATE_24M];
-        ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-        //ulRatio = (pDevice->uNumSQ3[RATE_24M] * 1000 / pDevice->uDiversityCnt);
-        ulRatio += TOP_RATE_24M;
-        ulMaxPacket = pDevice->uNumSQ3[RATE_24M];
-    }
-    if ( pDevice->uNumSQ3[RATE_18M] > ulMaxPacket ) {
-        ulPacketNum = pDevice->uNumSQ3[RATE_54M] + pDevice->uNumSQ3[RATE_48M] +
-                      pDevice->uNumSQ3[RATE_36M] + pDevice->uNumSQ3[RATE_24M] +
-                      pDevice->uNumSQ3[RATE_18M];
-        ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-        //ulRatio = (pDevice->uNumSQ3[RATE_18M] * 1000 / pDevice->uDiversityCnt);
-        ulRatio += TOP_RATE_18M;
-        ulMaxPacket = pDevice->uNumSQ3[RATE_18M];
-    }
-    if ( pDevice->uNumSQ3[RATE_12M] > ulMaxPacket ) {
-        ulPacketNum = pDevice->uNumSQ3[RATE_54M] + pDevice->uNumSQ3[RATE_48M] +
-                      pDevice->uNumSQ3[RATE_36M] + pDevice->uNumSQ3[RATE_24M] +
-                      pDevice->uNumSQ3[RATE_18M] + pDevice->uNumSQ3[RATE_12M];
-        ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-        //ulRatio = (pDevice->uNumSQ3[RATE_12M] * 1000 / pDevice->uDiversityCnt);
-        ulRatio += TOP_RATE_12M;
-        ulMaxPacket = pDevice->uNumSQ3[RATE_12M];
-    }
-    if ( pDevice->uNumSQ3[RATE_11M] > ulMaxPacket ) {
-        ulPacketNum = pDevice->uDiversityCnt - pDevice->uNumSQ3[RATE_1M] -
-                      pDevice->uNumSQ3[RATE_2M] - pDevice->uNumSQ3[RATE_5M] -
-                      pDevice->uNumSQ3[RATE_6M] - pDevice->uNumSQ3[RATE_9M];
-        ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-        //ulRatio = (pDevice->uNumSQ3[RATE_11M] * 1000 / pDevice->uDiversityCnt);
-        ulRatio += TOP_RATE_11M;
-        ulMaxPacket = pDevice->uNumSQ3[RATE_11M];
-    }
-    if ( pDevice->uNumSQ3[RATE_9M] > ulMaxPacket ) {
-        ulPacketNum = pDevice->uDiversityCnt - pDevice->uNumSQ3[RATE_1M] -
-                      pDevice->uNumSQ3[RATE_2M] - pDevice->uNumSQ3[RATE_5M] -
-                      pDevice->uNumSQ3[RATE_6M];
-        ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-        //ulRatio = (pDevice->uNumSQ3[RATE_9M] * 1000 / pDevice->uDiversityCnt);
-        ulRatio += TOP_RATE_9M;
-        ulMaxPacket = pDevice->uNumSQ3[RATE_9M];
-    }
-    if ( pDevice->uNumSQ3[RATE_6M] > ulMaxPacket ) {
-        ulPacketNum = pDevice->uDiversityCnt - pDevice->uNumSQ3[RATE_1M] -
-                      pDevice->uNumSQ3[RATE_2M] - pDevice->uNumSQ3[RATE_5M];
-        ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-        //ulRatio = (pDevice->uNumSQ3[RATE_6M] * 1000 / pDevice->uDiversityCnt);
-        ulRatio += TOP_RATE_6M;
-        ulMaxPacket = pDevice->uNumSQ3[RATE_6M];
-    }
-    if ( pDevice->uNumSQ3[RATE_5M] > ulMaxPacket ) {
-        ulPacketNum = pDevice->uDiversityCnt - pDevice->uNumSQ3[RATE_1M] -
-                      pDevice->uNumSQ3[RATE_2M];
-        ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-        //ulRatio = (pDevice->uNumSQ3[RATE_5M] * 1000 / pDevice->uDiversityCnt);
-        ulRatio += TOP_RATE_55M;
-        ulMaxPacket = pDevice->uNumSQ3[RATE_5M];
-    }
-    if ( pDevice->uNumSQ3[RATE_2M] > ulMaxPacket ) {
-        ulPacketNum = pDevice->uDiversityCnt - pDevice->uNumSQ3[RATE_1M];
-        ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-        //ulRatio = (pDevice->uNumSQ3[RATE_2M]  * 1000 / pDevice->uDiversityCnt);
-        ulRatio += TOP_RATE_2M;
-        ulMaxPacket = pDevice->uNumSQ3[RATE_2M];
-    }
-    if ( pDevice->uNumSQ3[RATE_1M] > ulMaxPacket ) {
-        ulPacketNum = pDevice->uDiversityCnt;
-        ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-        //ulRatio = (pDevice->uNumSQ3[RATE_1M]  * 1000 / pDevice->uDiversityCnt);
-        ulRatio += TOP_RATE_1M;
-    }
-
-    return ulRatio;
+	unsigned long ulRatio = 0;
+	unsigned long ulMaxPacket;
+	unsigned long ulPacketNum;
+
+	//This is a thousand-ratio
+	ulMaxPacket = pDevice->uNumSQ3[RATE_54M];
+	if (pDevice->uNumSQ3[RATE_54M] != 0) {
+		ulPacketNum = pDevice->uNumSQ3[RATE_54M];
+		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
+		//ulRatio = (pDevice->uNumSQ3[RATE_54M] * 1000 / pDevice->uDiversityCnt);
+		ulRatio += TOP_RATE_54M;
+	}
+	if (pDevice->uNumSQ3[RATE_48M] > ulMaxPacket) {
+		ulPacketNum = pDevice->uNumSQ3[RATE_54M] + pDevice->uNumSQ3[RATE_48M];
+		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
+		//ulRatio = (pDevice->uNumSQ3[RATE_48M] * 1000 / pDevice->uDiversityCnt);
+		ulRatio += TOP_RATE_48M;
+		ulMaxPacket = pDevice->uNumSQ3[RATE_48M];
+	}
+	if (pDevice->uNumSQ3[RATE_36M] > ulMaxPacket) {
+		ulPacketNum = pDevice->uNumSQ3[RATE_54M] + pDevice->uNumSQ3[RATE_48M] +
+			pDevice->uNumSQ3[RATE_36M];
+		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
+		//ulRatio = (pDevice->uNumSQ3[RATE_36M] * 1000 / pDevice->uDiversityCnt);
+		ulRatio += TOP_RATE_36M;
+		ulMaxPacket = pDevice->uNumSQ3[RATE_36M];
+	}
+	if (pDevice->uNumSQ3[RATE_24M] > ulMaxPacket) {
+		ulPacketNum = pDevice->uNumSQ3[RATE_54M] + pDevice->uNumSQ3[RATE_48M] +
+			pDevice->uNumSQ3[RATE_36M] + pDevice->uNumSQ3[RATE_24M];
+		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
+		//ulRatio = (pDevice->uNumSQ3[RATE_24M] * 1000 / pDevice->uDiversityCnt);
+		ulRatio += TOP_RATE_24M;
+		ulMaxPacket = pDevice->uNumSQ3[RATE_24M];
+	}
+	if (pDevice->uNumSQ3[RATE_18M] > ulMaxPacket) {
+		ulPacketNum = pDevice->uNumSQ3[RATE_54M] + pDevice->uNumSQ3[RATE_48M] +
+			pDevice->uNumSQ3[RATE_36M] + pDevice->uNumSQ3[RATE_24M] +
+			pDevice->uNumSQ3[RATE_18M];
+		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
+		//ulRatio = (pDevice->uNumSQ3[RATE_18M] * 1000 / pDevice->uDiversityCnt);
+		ulRatio += TOP_RATE_18M;
+		ulMaxPacket = pDevice->uNumSQ3[RATE_18M];
+	}
+	if (pDevice->uNumSQ3[RATE_12M] > ulMaxPacket) {
+		ulPacketNum = pDevice->uNumSQ3[RATE_54M] + pDevice->uNumSQ3[RATE_48M] +
+			pDevice->uNumSQ3[RATE_36M] + pDevice->uNumSQ3[RATE_24M] +
+			pDevice->uNumSQ3[RATE_18M] + pDevice->uNumSQ3[RATE_12M];
+		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
+		//ulRatio = (pDevice->uNumSQ3[RATE_12M] * 1000 / pDevice->uDiversityCnt);
+		ulRatio += TOP_RATE_12M;
+		ulMaxPacket = pDevice->uNumSQ3[RATE_12M];
+	}
+	if (pDevice->uNumSQ3[RATE_11M] > ulMaxPacket) {
+		ulPacketNum = pDevice->uDiversityCnt - pDevice->uNumSQ3[RATE_1M] -
+			pDevice->uNumSQ3[RATE_2M] - pDevice->uNumSQ3[RATE_5M] -
+			pDevice->uNumSQ3[RATE_6M] - pDevice->uNumSQ3[RATE_9M];
+		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
+		//ulRatio = (pDevice->uNumSQ3[RATE_11M] * 1000 / pDevice->uDiversityCnt);
+		ulRatio += TOP_RATE_11M;
+		ulMaxPacket = pDevice->uNumSQ3[RATE_11M];
+	}
+	if (pDevice->uNumSQ3[RATE_9M] > ulMaxPacket) {
+		ulPacketNum = pDevice->uDiversityCnt - pDevice->uNumSQ3[RATE_1M] -
+			pDevice->uNumSQ3[RATE_2M] - pDevice->uNumSQ3[RATE_5M] -
+			pDevice->uNumSQ3[RATE_6M];
+		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
+		//ulRatio = (pDevice->uNumSQ3[RATE_9M] * 1000 / pDevice->uDiversityCnt);
+		ulRatio += TOP_RATE_9M;
+		ulMaxPacket = pDevice->uNumSQ3[RATE_9M];
+	}
+	if (pDevice->uNumSQ3[RATE_6M] > ulMaxPacket) {
+		ulPacketNum = pDevice->uDiversityCnt - pDevice->uNumSQ3[RATE_1M] -
+			pDevice->uNumSQ3[RATE_2M] - pDevice->uNumSQ3[RATE_5M];
+		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
+		//ulRatio = (pDevice->uNumSQ3[RATE_6M] * 1000 / pDevice->uDiversityCnt);
+		ulRatio += TOP_RATE_6M;
+		ulMaxPacket = pDevice->uNumSQ3[RATE_6M];
+	}
+	if (pDevice->uNumSQ3[RATE_5M] > ulMaxPacket) {
+		ulPacketNum = pDevice->uDiversityCnt - pDevice->uNumSQ3[RATE_1M] -
+			pDevice->uNumSQ3[RATE_2M];
+		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
+		//ulRatio = (pDevice->uNumSQ3[RATE_5M] * 1000 / pDevice->uDiversityCnt);
+		ulRatio += TOP_RATE_55M;
+		ulMaxPacket = pDevice->uNumSQ3[RATE_5M];
+	}
+	if (pDevice->uNumSQ3[RATE_2M] > ulMaxPacket) {
+		ulPacketNum = pDevice->uDiversityCnt - pDevice->uNumSQ3[RATE_1M];
+		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
+		//ulRatio = (pDevice->uNumSQ3[RATE_2M]  * 1000 / pDevice->uDiversityCnt);
+		ulRatio += TOP_RATE_2M;
+		ulMaxPacket = pDevice->uNumSQ3[RATE_2M];
+	}
+	if (pDevice->uNumSQ3[RATE_1M] > ulMaxPacket) {
+		ulPacketNum = pDevice->uDiversityCnt;
+		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
+		//ulRatio = (pDevice->uNumSQ3[RATE_1M]  * 1000 / pDevice->uDiversityCnt);
+		ulRatio += TOP_RATE_1M;
+	}
+
+	return ulRatio;
 }
 
-
 void
-BBvClearAntDivSQ3Value (PSDevice pDevice)
+BBvClearAntDivSQ3Value(PSDevice pDevice)
 {
-    unsigned int ii;
+	unsigned int ii;
 
-    pDevice->uDiversityCnt = 0;
-    for (ii = 0; ii < MAX_RATE; ii++) {
-        pDevice->uNumSQ3[ii] = 0;
-    }
+	pDevice->uDiversityCnt = 0;
+	for (ii = 0; ii < MAX_RATE; ii++) {
+		pDevice->uNumSQ3[ii] = 0;
+	}
 }
 
-
 /*
  * Description: Antenna Diversity
  *
@@ -2785,79 +2736,70 @@ BBvClearAntDivSQ3Value (PSDevice pDevice)
  */
 
 void
-BBvAntennaDiversity (PSDevice pDevice, unsigned char byRxRate, unsigned char bySQ3)
+BBvAntennaDiversity(PSDevice pDevice, unsigned char byRxRate, unsigned char bySQ3)
 {
+	if ((byRxRate >= MAX_RATE) || (pDevice->wAntDiversityMaxRate >= MAX_RATE)) {
+		return;
+	}
+	pDevice->uDiversityCnt++;
+	// DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->uDiversityCnt = %d\n", (int)pDevice->uDiversityCnt);
 
-    if ((byRxRate >= MAX_RATE) || (pDevice->wAntDiversityMaxRate >= MAX_RATE)) {
-        return;
-    }
-    pDevice->uDiversityCnt++;
-   // DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->uDiversityCnt = %d\n", (int)pDevice->uDiversityCnt);
-
-    pDevice->uNumSQ3[byRxRate]++;
+	pDevice->uNumSQ3[byRxRate]++;
 
-    if (pDevice->byAntennaState == 0) {
+	if (pDevice->byAntennaState == 0) {
+		if (pDevice->uDiversityCnt > pDevice->ulDiversityNValue) {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ulDiversityNValue=[%d],54M-[%d]\n",
+				(int)pDevice->ulDiversityNValue, (int)pDevice->uNumSQ3[(int)pDevice->wAntDiversityMaxRate]);
 
-        if (pDevice->uDiversityCnt > pDevice->ulDiversityNValue) {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ulDiversityNValue=[%d],54M-[%d]\n",
-                          (int)pDevice->ulDiversityNValue, (int)pDevice->uNumSQ3[(int)pDevice->wAntDiversityMaxRate]);
+			if (pDevice->uNumSQ3[pDevice->wAntDiversityMaxRate] < pDevice->uDiversityCnt/2) {
+				pDevice->ulRatio_State0 = s_ulGetRatio(pDevice);
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "SQ3_State0, rate = [%08x]\n", (int)pDevice->ulRatio_State0);
 
-            if (pDevice->uNumSQ3[pDevice->wAntDiversityMaxRate] < pDevice->uDiversityCnt/2) {
+				if (pDevice->byTMax == 0)
+					return;
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "1.[%08x], uNumSQ3[%d]=%d, %d\n",
+					(int)pDevice->ulRatio_State0, (int)pDevice->wAntDiversityMaxRate,
+					(int)pDevice->uNumSQ3[(int)pDevice->wAntDiversityMaxRate], (int)pDevice->uDiversityCnt);
 
-                pDevice->ulRatio_State0 = s_ulGetRatio(pDevice);
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"SQ3_State0, rate = [%08x]\n", (int)pDevice->ulRatio_State0);
+				s_vChangeAntenna(pDevice);
+				pDevice->byAntennaState = 1;
+				del_timer(&pDevice->TimerSQ3Tmax3);
+				del_timer(&pDevice->TimerSQ3Tmax2);
+				pDevice->TimerSQ3Tmax1.expires =  RUN_AT(pDevice->byTMax * HZ);
+				add_timer(&pDevice->TimerSQ3Tmax1);
+
+			} else {
+				pDevice->TimerSQ3Tmax3.expires =  RUN_AT(pDevice->byTMax3 * HZ);
+				add_timer(&pDevice->TimerSQ3Tmax3);
+			}
+			BBvClearAntDivSQ3Value(pDevice);
+
+		}
+	} else { //byAntennaState == 1
+
+		if (pDevice->uDiversityCnt > pDevice->ulDiversityMValue) {
+			del_timer(&pDevice->TimerSQ3Tmax1);
+
+			pDevice->ulRatio_State1 = s_ulGetRatio(pDevice);
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "RX:SQ3_State1, rate0 = %08x,rate1 = %08x\n",
+				(int)pDevice->ulRatio_State0, (int)pDevice->ulRatio_State1);
+
+			if (pDevice->ulRatio_State1 < pDevice->ulRatio_State0) {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "2.[%08x][%08x], uNumSQ3[%d]=%d, %d\n",
+					(int)pDevice->ulRatio_State0, (int)pDevice->ulRatio_State1,
+					(int)pDevice->wAntDiversityMaxRate,
+					(int)pDevice->uNumSQ3[(int)pDevice->wAntDiversityMaxRate], (int)pDevice->uDiversityCnt);
 
-                if ( pDevice->byTMax == 0 )
-                    return;
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"1.[%08x], uNumSQ3[%d]=%d, %d\n",
-                              (int)pDevice->ulRatio_State0, (int)pDevice->wAntDiversityMaxRate,
-                              (int)pDevice->uNumSQ3[(int)pDevice->wAntDiversityMaxRate], (int)pDevice->uDiversityCnt);
-#ifdef	PLICE_DEBUG
-		//printk("BBvAntennaDiversity1:call s_vChangeAntenna\n");
-#endif
-		s_vChangeAntenna(pDevice);
-                pDevice->byAntennaState = 1;
-                del_timer(&pDevice->TimerSQ3Tmax3);
-                del_timer(&pDevice->TimerSQ3Tmax2);
-                pDevice->TimerSQ3Tmax1.expires =  RUN_AT(pDevice->byTMax * HZ);
-                add_timer(&pDevice->TimerSQ3Tmax1);
-
-            } else {
-
-                pDevice->TimerSQ3Tmax3.expires =  RUN_AT(pDevice->byTMax3 * HZ);
-                add_timer(&pDevice->TimerSQ3Tmax3);
-            }
-            BBvClearAntDivSQ3Value(pDevice);
-
-        }
-    } else { //byAntennaState == 1
-
-        if (pDevice->uDiversityCnt > pDevice->ulDiversityMValue) {
-
-            del_timer(&pDevice->TimerSQ3Tmax1);
-
-            pDevice->ulRatio_State1 = s_ulGetRatio(pDevice);
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"RX:SQ3_State1, rate0 = %08x,rate1 = %08x\n",
-                          (int)pDevice->ulRatio_State0,(int)pDevice->ulRatio_State1);
-
-            if (pDevice->ulRatio_State1 < pDevice->ulRatio_State0) {
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"2.[%08x][%08x], uNumSQ3[%d]=%d, %d\n",
-                              (int)pDevice->ulRatio_State0, (int)pDevice->ulRatio_State1,
-                              (int)pDevice->wAntDiversityMaxRate,
-                              (int)pDevice->uNumSQ3[(int)pDevice->wAntDiversityMaxRate], (int)pDevice->uDiversityCnt);
-#ifdef	PLICE_DEBUG
-		//printk("BBvAntennaDiversity2:call s_vChangeAntenna\n");
-#endif
 				s_vChangeAntenna(pDevice);
-                pDevice->TimerSQ3Tmax3.expires =  RUN_AT(pDevice->byTMax3 * HZ);
-                pDevice->TimerSQ3Tmax2.expires =  RUN_AT(pDevice->byTMax2 * HZ);
-                add_timer(&pDevice->TimerSQ3Tmax3);
-                add_timer(&pDevice->TimerSQ3Tmax2);
-            }
-            pDevice->byAntennaState = 0;
-            BBvClearAntDivSQ3Value(pDevice);
-        }
-    } //byAntennaState
+				pDevice->TimerSQ3Tmax3.expires =  RUN_AT(pDevice->byTMax3 * HZ);
+				pDevice->TimerSQ3Tmax2.expires =  RUN_AT(pDevice->byTMax2 * HZ);
+				add_timer(&pDevice->TimerSQ3Tmax3);
+				add_timer(&pDevice->TimerSQ3Tmax2);
+			}
+			pDevice->byAntennaState = 0;
+			BBvClearAntDivSQ3Value(pDevice);
+		}
+	} //byAntennaState
 }
 
 /*+
@@ -2872,38 +2814,33 @@ BBvAntennaDiversity (PSDevice pDevice, unsigned char byRxRate, unsigned char byS
  *
  * Return Value: none
  *
--*/
+ -*/
 
 void
-TimerSQ3CallBack (
-    void *hDeviceContext
-    )
+TimerSQ3CallBack(
+	void *hDeviceContext
+)
 {
-    PSDevice        pDevice = (PSDevice)hDeviceContext;
+	PSDevice        pDevice = (PSDevice)hDeviceContext;
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"TimerSQ3CallBack...");
-    spin_lock_irq(&pDevice->lock);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "TimerSQ3CallBack...");
+	spin_lock_irq(&pDevice->lock);
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"3.[%08x][%08x], %d\n",(int)pDevice->ulRatio_State0, (int)pDevice->ulRatio_State1, (int)pDevice->uDiversityCnt);
-#ifdef	PLICE_DEBUG
-		//printk("TimerSQ3CallBack1:call s_vChangeAntenna\n");
-#endif
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "3.[%08x][%08x], %d\n", (int)pDevice->ulRatio_State0, (int)pDevice->ulRatio_State1, (int)pDevice->uDiversityCnt);
 
-    s_vChangeAntenna(pDevice);
-    pDevice->byAntennaState = 0;
-    BBvClearAntDivSQ3Value(pDevice);
+	s_vChangeAntenna(pDevice);
+	pDevice->byAntennaState = 0;
+	BBvClearAntDivSQ3Value(pDevice);
 
-    pDevice->TimerSQ3Tmax3.expires =  RUN_AT(pDevice->byTMax3 * HZ);
-    pDevice->TimerSQ3Tmax2.expires =  RUN_AT(pDevice->byTMax2 * HZ);
-    add_timer(&pDevice->TimerSQ3Tmax3);
-    add_timer(&pDevice->TimerSQ3Tmax2);
+	pDevice->TimerSQ3Tmax3.expires =  RUN_AT(pDevice->byTMax3 * HZ);
+	pDevice->TimerSQ3Tmax2.expires =  RUN_AT(pDevice->byTMax2 * HZ);
+	add_timer(&pDevice->TimerSQ3Tmax3);
+	add_timer(&pDevice->TimerSQ3Tmax2);
 
-
-    spin_unlock_irq(&pDevice->lock);
-    return;
+	spin_unlock_irq(&pDevice->lock);
+	return;
 }
 
-
 /*+
  *
  * Description:
@@ -2920,54 +2857,46 @@ TimerSQ3CallBack (
  *
  * Return Value: none
  *
--*/
+ -*/
 
 void
-TimerState1CallBack (
-    void *hDeviceContext
-    )
+TimerState1CallBack(
+	void *hDeviceContext
+)
 {
-    PSDevice        pDevice = (PSDevice)hDeviceContext;
+	PSDevice        pDevice = (PSDevice)hDeviceContext;
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"TimerState1CallBack...");
-
-    spin_lock_irq(&pDevice->lock);
-    if (pDevice->uDiversityCnt < pDevice->ulDiversityMValue/100) {
-#ifdef	PLICE_DEBUG
-		//printk("TimerSQ3CallBack2:call s_vChangeAntenna\n");
-#endif
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "TimerState1CallBack...");
 
+	spin_lock_irq(&pDevice->lock);
+	if (pDevice->uDiversityCnt < pDevice->ulDiversityMValue/100) {
 		s_vChangeAntenna(pDevice);
-        pDevice->TimerSQ3Tmax3.expires =  RUN_AT(pDevice->byTMax3 * HZ);
-        pDevice->TimerSQ3Tmax2.expires =  RUN_AT(pDevice->byTMax2 * HZ);
-        add_timer(&pDevice->TimerSQ3Tmax3);
-        add_timer(&pDevice->TimerSQ3Tmax2);
-    } else {
-        pDevice->ulRatio_State1 = s_ulGetRatio(pDevice);
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"SQ3_State1, rate0 = %08x,rate1 = %08x\n",
-                      (int)pDevice->ulRatio_State0,(int)pDevice->ulRatio_State1);
-
-        if ( pDevice->ulRatio_State1 < pDevice->ulRatio_State0 ) {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"2.[%08x][%08x], uNumSQ3[%d]=%d, %d\n",
-                          (int)pDevice->ulRatio_State0, (int)pDevice->ulRatio_State1,
-                          (int)pDevice->wAntDiversityMaxRate,
-                          (int)pDevice->uNumSQ3[(int)pDevice->wAntDiversityMaxRate], (int)pDevice->uDiversityCnt);
-#ifdef	PLICE_DEBUG
-		//printk("TimerSQ3CallBack3:call s_vChangeAntenna\n");
-#endif
+		pDevice->TimerSQ3Tmax3.expires =  RUN_AT(pDevice->byTMax3 * HZ);
+		pDevice->TimerSQ3Tmax2.expires =  RUN_AT(pDevice->byTMax2 * HZ);
+		add_timer(&pDevice->TimerSQ3Tmax3);
+		add_timer(&pDevice->TimerSQ3Tmax2);
+	} else {
+		pDevice->ulRatio_State1 = s_ulGetRatio(pDevice);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "SQ3_State1, rate0 = %08x,rate1 = %08x\n",
+			(int)pDevice->ulRatio_State0, (int)pDevice->ulRatio_State1);
+
+		if (pDevice->ulRatio_State1 < pDevice->ulRatio_State0) {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "2.[%08x][%08x], uNumSQ3[%d]=%d, %d\n",
+				(int)pDevice->ulRatio_State0, (int)pDevice->ulRatio_State1,
+				(int)pDevice->wAntDiversityMaxRate,
+				(int)pDevice->uNumSQ3[(int)pDevice->wAntDiversityMaxRate], (int)pDevice->uDiversityCnt);
 
 			s_vChangeAntenna(pDevice);
 
-            pDevice->TimerSQ3Tmax3.expires =  RUN_AT(pDevice->byTMax3 * HZ);
-            pDevice->TimerSQ3Tmax2.expires =  RUN_AT(pDevice->byTMax2 * HZ);
-            add_timer(&pDevice->TimerSQ3Tmax3);
-            add_timer(&pDevice->TimerSQ3Tmax2);
-        }
-    }
-    pDevice->byAntennaState = 0;
-    BBvClearAntDivSQ3Value(pDevice);
-    spin_unlock_irq(&pDevice->lock);
-
-    return;
+			pDevice->TimerSQ3Tmax3.expires =  RUN_AT(pDevice->byTMax3 * HZ);
+			pDevice->TimerSQ3Tmax2.expires =  RUN_AT(pDevice->byTMax2 * HZ);
+			add_timer(&pDevice->TimerSQ3Tmax3);
+			add_timer(&pDevice->TimerSQ3Tmax2);
+		}
+	}
+	pDevice->byAntennaState = 0;
+	BBvClearAntDivSQ3Value(pDevice);
+	spin_unlock_irq(&pDevice->lock);
+
+	return;
 }
-
diff --git a/drivers/staging/vt6655/baseband.h b/drivers/staging/vt6655/baseband.h
index 9b5bc9c58d9..e31bb760067 100644
--- a/drivers/staging/vt6655/baseband.h
+++ b/drivers/staging/vt6655/baseband.h
@@ -41,7 +41,6 @@
 //
 #define BB_MAX_CONTEXT_SIZE 256
 
-
 //
 // Baseband RF pair definition in eeprom (Bits 6..0)
 //
@@ -49,7 +48,6 @@
 #define PREAMBLE_LONG   0
 #define PREAMBLE_SHORT  1
 
-
 #define F5G             0
 #define F2_4G           1
 
@@ -66,21 +64,15 @@
 #define TOP_RATE_2M         0x00200000
 #define TOP_RATE_1M         0x00100000
 
-
 /*---------------------  Export Types  ------------------------------*/
 
 /*---------------------  Export Macros ------------------------------*/
 
-#define BBvClearFOE(dwIoBase)                               \
-{                                                           \
-    BBbWriteEmbedded(dwIoBase, 0xB1, 0);                     \
-}
-
-#define BBvSetFOE(dwIoBase)                                 \
-{                                                           \
-    BBbWriteEmbedded(dwIoBase, 0xB1, 0x0C);                  \
-}
+#define BBvClearFOE(dwIoBase)				\
+	BBbWriteEmbedded(dwIoBase, 0xB1, 0)
 
+#define BBvSetFOE(dwIoBase)				\
+	BBbWriteEmbedded(dwIoBase, 0xB1, 0x0C)
 
 /*---------------------  Export Classes  ----------------------------*/
 
@@ -90,22 +82,22 @@
 
 unsigned int
 BBuGetFrameTime(
-    unsigned char byPreambleType,
-    unsigned char byPktType,
-    unsigned int cbFrameLength,
-    unsigned short wRate
-    );
+	unsigned char byPreambleType,
+	unsigned char byPktType,
+	unsigned int cbFrameLength,
+	unsigned short wRate
+);
 
 void
-BBvCalculateParameter (
-    PSDevice pDevice,
-    unsigned int cbFrameLength,
-    unsigned short wRate,
-    unsigned char byPacketType,
-    unsigned short *pwPhyLen,
-    unsigned char *pbyPhySrv,
-    unsigned char *pbyPhySgn
-    );
+BBvCalculateParameter(
+	PSDevice pDevice,
+	unsigned int cbFrameLength,
+	unsigned short wRate,
+	unsigned char byPacketType,
+	unsigned short *pwPhyLen,
+	unsigned char *pbyPhySrv,
+	unsigned char *pbyPhySgn
+);
 
 bool BBbReadEmbedded(unsigned long dwIoBase, unsigned char byBBAddr, unsigned char *pbyData);
 bool BBbWriteEmbedded(unsigned long dwIoBase, unsigned char byBBAddr, unsigned char byData);
@@ -131,17 +123,17 @@ void BBvExitDeepSleep(unsigned long dwIoBase, unsigned char byLocalID);
 // timer for antenna diversity
 
 void
-TimerSQ3CallBack (
-    void *hDeviceContext
-    );
+TimerSQ3CallBack(
+	void *hDeviceContext
+);
 
 void
 TimerState1CallBack(
-    void *hDeviceContext
-    );
+	void *hDeviceContext
+);
 
 void BBvAntennaDiversity(PSDevice pDevice, unsigned char byRxRate, unsigned char bySQ3);
 void
-BBvClearAntDivSQ3Value (PSDevice pDevice);
+BBvClearAntDivSQ3Value(PSDevice pDevice);
 
 #endif // __BASEBAND_H__
diff --git a/drivers/staging/vt6655/bssdb.c b/drivers/staging/vt6655/bssdb.c
index fe57fb880a8..f983915168b 100644
--- a/drivers/staging/vt6655/bssdb.c
+++ b/drivers/staging/vt6655/bssdb.c
@@ -60,59 +60,46 @@
 
 /*---------------------  Static Definitions -------------------------*/
 
-
-
-
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
-static int          msglevel                =MSG_LEVEL_INFO;
+static int msglevel = MSG_LEVEL_INFO;
 //static int          msglevel                =MSG_LEVEL_DEBUG;
 
-
-
 const unsigned short awHWRetry0[5][5] = {
-                                            {RATE_18M, RATE_18M, RATE_12M, RATE_12M, RATE_12M},
-                                            {RATE_24M, RATE_24M, RATE_18M, RATE_12M, RATE_12M},
-                                            {RATE_36M, RATE_36M, RATE_24M, RATE_18M, RATE_18M},
-                                            {RATE_48M, RATE_48M, RATE_36M, RATE_24M, RATE_24M},
-                                            {RATE_54M, RATE_54M, RATE_48M, RATE_36M, RATE_36M}
-                                           };
+	{RATE_18M, RATE_18M, RATE_12M, RATE_12M, RATE_12M},
+	{RATE_24M, RATE_24M, RATE_18M, RATE_12M, RATE_12M},
+	{RATE_36M, RATE_36M, RATE_24M, RATE_18M, RATE_18M},
+	{RATE_48M, RATE_48M, RATE_36M, RATE_24M, RATE_24M},
+	{RATE_54M, RATE_54M, RATE_48M, RATE_36M, RATE_36M}
+};
 const unsigned short awHWRetry1[5][5] = {
-                                            {RATE_18M, RATE_18M, RATE_12M, RATE_6M, RATE_6M},
-                                            {RATE_24M, RATE_24M, RATE_18M, RATE_6M, RATE_6M},
-                                            {RATE_36M, RATE_36M, RATE_24M, RATE_12M, RATE_12M},
-                                            {RATE_48M, RATE_48M, RATE_24M, RATE_12M, RATE_12M},
-                                            {RATE_54M, RATE_54M, RATE_36M, RATE_18M, RATE_18M}
-                                           };
-
-
+	{RATE_18M, RATE_18M, RATE_12M, RATE_6M, RATE_6M},
+	{RATE_24M, RATE_24M, RATE_18M, RATE_6M, RATE_6M},
+	{RATE_36M, RATE_36M, RATE_24M, RATE_12M, RATE_12M},
+	{RATE_48M, RATE_48M, RATE_24M, RATE_12M, RATE_12M},
+	{RATE_54M, RATE_54M, RATE_36M, RATE_18M, RATE_18M}
+};
 
 /*---------------------  Static Functions  --------------------------*/
 
 void s_vCheckSensitivity(
-    void *hDeviceContext
-    );
+	void *hDeviceContext
+);
 
 #ifdef Calcu_LinkQual
 void s_uCalculateLinkQual(
-    void *hDeviceContext
-    );
+	void *hDeviceContext
+);
 #endif
 
-
 void s_vCheckPreEDThreshold(
-    void *hDeviceContext
-    );
+	void *hDeviceContext
+);
 /*---------------------  Export Variables  --------------------------*/
 
-
 /*---------------------  Export Functions  --------------------------*/
 
-
-
-
-
 /*+
  *
  * Routine Description:
@@ -121,153 +108,150 @@ void s_vCheckPreEDThreshold(
  * Return Value:
  *    PTR to KnownBSS or NULL
  *
--*/
+ -*/
 
 PKnownBSS
 BSSpSearchBSSList(
-    void *hDeviceContext,
-    unsigned char *pbyDesireBSSID,
-    unsigned char *pbyDesireSSID,
-    CARD_PHY_TYPE  ePhyType
-    )
+	void *hDeviceContext,
+	unsigned char *pbyDesireBSSID,
+	unsigned char *pbyDesireSSID,
+	CARD_PHY_TYPE  ePhyType
+)
 {
-    PSDevice        pDevice = (PSDevice)hDeviceContext;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    unsigned char *pbyBSSID = NULL;
-    PWLAN_IE_SSID   pSSID = NULL;
-    PKnownBSS       pCurrBSS = NULL;
-    PKnownBSS       pSelect = NULL;
-    unsigned char ZeroBSSID[WLAN_BSSID_LEN]={0x00,0x00,0x00,0x00,0x00,0x00};
-    unsigned int ii = 0;
-
-    if (pbyDesireBSSID != NULL) {
+	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	unsigned char *pbyBSSID = NULL;
+	PWLAN_IE_SSID   pSSID = NULL;
+	PKnownBSS       pCurrBSS = NULL;
+	PKnownBSS       pSelect = NULL;
+	unsigned char ZeroBSSID[WLAN_BSSID_LEN] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+	unsigned int ii = 0;
+
+	if (pbyDesireBSSID != NULL) {
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
 			"BSSpSearchBSSList BSSID[%pM]\n", pbyDesireBSSID);
-        if ((!is_broadcast_ether_addr(pbyDesireBSSID)) &&
-	     (memcmp(pbyDesireBSSID, ZeroBSSID, 6)!= 0)){
-            pbyBSSID = pbyDesireBSSID;
-        }
-    }
-    if (pbyDesireSSID != NULL) {
-        if (((PWLAN_IE_SSID)pbyDesireSSID)->len != 0) {
-            pSSID = (PWLAN_IE_SSID) pbyDesireSSID;
-        }
-    }
-
-    if (pbyBSSID != NULL) {
-        // match BSSID first
-        for (ii = 0; ii <MAX_BSS_NUM; ii++) {
-            pCurrBSS = &(pMgmt->sBSSList[ii]);
-if(pDevice->bLinkPass==false) pCurrBSS->bSelected = false;
-            if ((pCurrBSS->bActive) &&
-                (pCurrBSS->bSelected == false)) {
-                if (!compare_ether_addr(pCurrBSS->abyBSSID, pbyBSSID)) {
-                    if (pSSID != NULL) {
-                        // compare ssid
-                        if ( !memcmp(pSSID->abySSID,
-                            ((PWLAN_IE_SSID)pCurrBSS->abySSID)->abySSID,
-                            pSSID->len)) {
-                            if ((pMgmt->eConfigMode == WMAC_CONFIG_AUTO) ||
-                                ((pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA) && WLAN_GET_CAP_INFO_IBSS(pCurrBSS->wCapInfo)) ||
-                                ((pMgmt->eConfigMode == WMAC_CONFIG_ESS_STA) && WLAN_GET_CAP_INFO_ESS(pCurrBSS->wCapInfo))
-                                ) {
-                                pCurrBSS->bSelected = true;
-                                return(pCurrBSS);
-                            }
-                        }
-                    } else {
-                        if ((pMgmt->eConfigMode == WMAC_CONFIG_AUTO) ||
-                            ((pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA) && WLAN_GET_CAP_INFO_IBSS(pCurrBSS->wCapInfo)) ||
-                            ((pMgmt->eConfigMode == WMAC_CONFIG_ESS_STA) && WLAN_GET_CAP_INFO_ESS(pCurrBSS->wCapInfo))
-                            ) {
-                            pCurrBSS->bSelected = true;
-                            return(pCurrBSS);
-                        }
-                    }
-                }
-            }
-        }
-    } else {
-        // ignore BSSID
-        for (ii = 0; ii <MAX_BSS_NUM; ii++) {
-            pCurrBSS = &(pMgmt->sBSSList[ii]);
-	//2007-0721-01<Add>by MikeLiu
-	  pCurrBSS->bSelected = false;
-          if (pCurrBSS->bActive) {
-
-                if (pSSID != NULL) {
-                    // matched SSID
-                    if (! !memcmp(pSSID->abySSID,
-                        ((PWLAN_IE_SSID)pCurrBSS->abySSID)->abySSID,
-                        pSSID->len) ||
-                        (pSSID->len != ((PWLAN_IE_SSID)pCurrBSS->abySSID)->len)) {
-                        // SSID not match skip this BSS
-                        continue;
-                      }
-                }
-                if (((pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA) && WLAN_GET_CAP_INFO_ESS(pCurrBSS->wCapInfo)) ||
-                    ((pMgmt->eConfigMode == WMAC_CONFIG_ESS_STA) && WLAN_GET_CAP_INFO_IBSS(pCurrBSS->wCapInfo))
-                    ){
-                    // Type not match skip this BSS
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"BSS type mismatch.... Config[%d] BSS[0x%04x]\n", pMgmt->eConfigMode, pCurrBSS->wCapInfo);
-                    continue;
-                }
-
-                if (ePhyType != PHY_TYPE_AUTO) {
-                    if (((ePhyType == PHY_TYPE_11A) && (PHY_TYPE_11A != pCurrBSS->eNetworkTypeInUse)) ||
-                        ((ePhyType != PHY_TYPE_11A) && (PHY_TYPE_11A == pCurrBSS->eNetworkTypeInUse))) {
-                        // PhyType not match skip this BSS
-                        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Physical type mismatch.... ePhyType[%d] BSS[%d]\n", ePhyType, pCurrBSS->eNetworkTypeInUse);
-                        continue;
-                    }
-                }
+		if ((!is_broadcast_ether_addr(pbyDesireBSSID)) &&
+		    (memcmp(pbyDesireBSSID, ZeroBSSID, 6) != 0)) {
+			pbyBSSID = pbyDesireBSSID;
+		}
+	}
+	if (pbyDesireSSID != NULL) {
+		if (((PWLAN_IE_SSID)pbyDesireSSID)->len != 0) {
+			pSSID = (PWLAN_IE_SSID) pbyDesireSSID;
+		}
+	}
+
+	if (pbyBSSID != NULL) {
+		// match BSSID first
+		for (ii = 0; ii < MAX_BSS_NUM; ii++) {
+			pCurrBSS = &(pMgmt->sBSSList[ii]);
+			if (pDevice->bLinkPass == false) pCurrBSS->bSelected = false;
+			if ((pCurrBSS->bActive) &&
+			    (pCurrBSS->bSelected == false)) {
+				if (!compare_ether_addr(pCurrBSS->abyBSSID, pbyBSSID)) {
+					if (pSSID != NULL) {
+						// compare ssid
+						if (!memcmp(pSSID->abySSID,
+							    ((PWLAN_IE_SSID)pCurrBSS->abySSID)->abySSID,
+							    pSSID->len)) {
+							if ((pMgmt->eConfigMode == WMAC_CONFIG_AUTO) ||
+							    ((pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA) && WLAN_GET_CAP_INFO_IBSS(pCurrBSS->wCapInfo)) ||
+							    ((pMgmt->eConfigMode == WMAC_CONFIG_ESS_STA) && WLAN_GET_CAP_INFO_ESS(pCurrBSS->wCapInfo))
+) {
+								pCurrBSS->bSelected = true;
+								return pCurrBSS;
+							}
+						}
+					} else {
+						if ((pMgmt->eConfigMode == WMAC_CONFIG_AUTO) ||
+						    ((pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA) && WLAN_GET_CAP_INFO_IBSS(pCurrBSS->wCapInfo)) ||
+						    ((pMgmt->eConfigMode == WMAC_CONFIG_ESS_STA) && WLAN_GET_CAP_INFO_ESS(pCurrBSS->wCapInfo))
+) {
+							pCurrBSS->bSelected = true;
+							return pCurrBSS;
+						}
+					}
+				}
+			}
+		}
+	} else {
+		// ignore BSSID
+		for (ii = 0; ii < MAX_BSS_NUM; ii++) {
+			pCurrBSS = &(pMgmt->sBSSList[ii]);
+			//2007-0721-01<Add>by MikeLiu
+			pCurrBSS->bSelected = false;
+			if (pCurrBSS->bActive) {
+				if (pSSID != NULL) {
+					// matched SSID
+					if (!!memcmp(pSSID->abySSID,
+						     ((PWLAN_IE_SSID)pCurrBSS->abySSID)->abySSID,
+						     pSSID->len) ||
+					    (pSSID->len != ((PWLAN_IE_SSID)pCurrBSS->abySSID)->len)) {
+						// SSID not match skip this BSS
+						continue;
+					}
+				}
+				if (((pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA) && WLAN_GET_CAP_INFO_ESS(pCurrBSS->wCapInfo)) ||
+				    ((pMgmt->eConfigMode == WMAC_CONFIG_ESS_STA) && WLAN_GET_CAP_INFO_IBSS(pCurrBSS->wCapInfo))
+) {
+					// Type not match skip this BSS
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BSS type mismatch.... Config[%d] BSS[0x%04x]\n", pMgmt->eConfigMode, pCurrBSS->wCapInfo);
+					continue;
+				}
+
+				if (ePhyType != PHY_TYPE_AUTO) {
+					if (((ePhyType == PHY_TYPE_11A) && (PHY_TYPE_11A != pCurrBSS->eNetworkTypeInUse)) ||
+					    ((ePhyType != PHY_TYPE_11A) && (PHY_TYPE_11A == pCurrBSS->eNetworkTypeInUse))) {
+						// PhyType not match skip this BSS
+						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Physical type mismatch.... ePhyType[%d] BSS[%d]\n", ePhyType, pCurrBSS->eNetworkTypeInUse);
+						continue;
+					}
+				}
 /*
-                if (pMgmt->eAuthenMode < WMAC_AUTH_WPA) {
-                    if (pCurrBSS->bWPAValid == true) {
-                        // WPA AP will reject connection of station without WPA enable.
-                        continue;
-                    }
-                } else if ((pMgmt->eAuthenMode == WMAC_AUTH_WPA) ||
-                           (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK)) {
-                    if (pCurrBSS->bWPAValid == false) {
-                        // station with WPA enable can't join NonWPA AP.
-                        continue;
-                    }
-                } else if ((pMgmt->eAuthenMode == WMAC_AUTH_WPA2) ||
-                           (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) {
-                    if (pCurrBSS->bWPA2Valid == false) {
-                        // station with WPA2 enable can't join NonWPA2 AP.
-                        continue;
-                    }
-                }
+  if (pMgmt->eAuthenMode < WMAC_AUTH_WPA) {
+  if (pCurrBSS->bWPAValid == true) {
+  // WPA AP will reject connection of station without WPA enable.
+  continue;
+  }
+  } else if ((pMgmt->eAuthenMode == WMAC_AUTH_WPA) ||
+  (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK)) {
+  if (pCurrBSS->bWPAValid == false) {
+  // station with WPA enable can't join NonWPA AP.
+  continue;
+  }
+  } else if ((pMgmt->eAuthenMode == WMAC_AUTH_WPA2) ||
+  (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) {
+  if (pCurrBSS->bWPA2Valid == false) {
+  // station with WPA2 enable can't join NonWPA2 AP.
+  continue;
+  }
+  }
 */
-                if (pSelect == NULL) {
-                    pSelect = pCurrBSS;
-                } else {
-                    // compare RSSI, select signal strong one
-                    if (pCurrBSS->uRSSI < pSelect->uRSSI) {
-                        pSelect = pCurrBSS;
-                    }
-                }
-            }
-        }
-        if (pSelect != NULL) {
-            pSelect->bSelected = true;
+				if (pSelect == NULL) {
+					pSelect = pCurrBSS;
+				} else {
+					// compare RSSI, select signal strong one
+					if (pCurrBSS->uRSSI < pSelect->uRSSI) {
+						pSelect = pCurrBSS;
+					}
+				}
+			}
+		}
+		if (pSelect != NULL) {
+			pSelect->bSelected = true;
 /*
-                        if (pDevice->bRoaming == false)  {
-	//       Einsn Add @20070907
-			memset(pbyDesireSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-			memcpy(pbyDesireSSID,pCurrBSS->abySSID,WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1) ;
-                                                }*/
-
-            return(pSelect);
-        }
-    }
-    return(NULL);
+  if (pDevice->bRoaming == false)  {
+  //       Einsn Add @20070907
+  memset(pbyDesireSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
+  memcpy(pbyDesireSSID,pCurrBSS->abySSID,WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
+  }*/
 
+			return pSelect;
+		}
+	}
+	return NULL;
 }
 
-
 /*+
  *
  * Routine Description:
@@ -276,42 +260,39 @@ if(pDevice->bLinkPass==false) pCurrBSS->bSelected = false;
  * Return Value:
  *    None.
  *
--*/
-
+ -*/
 
 void
 BSSvClearBSSList(
-    void *hDeviceContext,
-    bool bKeepCurrBSSID
-    )
+	void *hDeviceContext,
+	bool bKeepCurrBSSID
+)
 {
-    PSDevice     pDevice = (PSDevice)hDeviceContext;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    unsigned int ii;
-
-    for (ii = 0; ii < MAX_BSS_NUM; ii++) {
-        if (bKeepCurrBSSID) {
-            if (pMgmt->sBSSList[ii].bActive &&
-                !compare_ether_addr(pMgmt->sBSSList[ii].abyBSSID, pMgmt->abyCurrBSSID)) {
-               // bKeepCurrBSSID = false;
-                continue;
-            }
-        }
-
-        if ((pMgmt->sBSSList[ii].bActive) && (pMgmt->sBSSList[ii].uClearCount < BSS_CLEAR_COUNT)) {
-             pMgmt->sBSSList[ii].uClearCount ++;
-             continue;
-        }
-
-        pMgmt->sBSSList[ii].bActive = false;
-        memset(&pMgmt->sBSSList[ii], 0, sizeof(KnownBSS));
-    }
-    BSSvClearAnyBSSJoinRecord(pDevice);
-
-    return;
-}
+	PSDevice     pDevice = (PSDevice)hDeviceContext;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	unsigned int ii;
+
+	for (ii = 0; ii < MAX_BSS_NUM; ii++) {
+		if (bKeepCurrBSSID) {
+			if (pMgmt->sBSSList[ii].bActive &&
+			    !compare_ether_addr(pMgmt->sBSSList[ii].abyBSSID, pMgmt->abyCurrBSSID)) {
+				// bKeepCurrBSSID = false;
+				continue;
+			}
+		}
 
+		if ((pMgmt->sBSSList[ii].bActive) && (pMgmt->sBSSList[ii].uClearCount < BSS_CLEAR_COUNT)) {
+			pMgmt->sBSSList[ii].uClearCount++;
+			continue;
+		}
 
+		pMgmt->sBSSList[ii].bActive = false;
+		memset(&pMgmt->sBSSList[ii], 0, sizeof(KnownBSS));
+	}
+	BSSvClearAnyBSSJoinRecord(pDevice);
+
+	return;
+}
 
 /*+
  *
@@ -321,39 +302,35 @@ BSSvClearBSSList(
  * Return Value:
  *    true if found.
  *
--*/
+ -*/
 PKnownBSS
 BSSpAddrIsInBSSList(
-    void *hDeviceContext,
-    unsigned char *abyBSSID,
-    PWLAN_IE_SSID pSSID
-    )
+	void *hDeviceContext,
+	unsigned char *abyBSSID,
+	PWLAN_IE_SSID pSSID
+)
 {
-    PSDevice     pDevice = (PSDevice)hDeviceContext;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    PKnownBSS       pBSSList = NULL;
-    unsigned int ii;
-
-    for (ii = 0; ii < MAX_BSS_NUM; ii++) {
-        pBSSList = &(pMgmt->sBSSList[ii]);
-        if (pBSSList->bActive) {
-            if (!compare_ether_addr(pBSSList->abyBSSID, abyBSSID)) {
-//                if (pSSID == NULL)
-//                    return pBSSList;
-                if (pSSID->len == ((PWLAN_IE_SSID)pBSSList->abySSID)->len){
-                    if (memcmp(pSSID->abySSID,
-                            ((PWLAN_IE_SSID)pBSSList->abySSID)->abySSID,
-                            pSSID->len) == 0)
-                        return pBSSList;
-                }
-            }
-        }
-    }
-
-    return NULL;
-};
-
+	PSDevice     pDevice = (PSDevice)hDeviceContext;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	PKnownBSS       pBSSList = NULL;
+	unsigned int ii;
+
+	for (ii = 0; ii < MAX_BSS_NUM; ii++) {
+		pBSSList = &(pMgmt->sBSSList[ii]);
+		if (pBSSList->bActive) {
+			if (!compare_ether_addr(pBSSList->abyBSSID, abyBSSID)) {
+				if (pSSID->len == ((PWLAN_IE_SSID)pBSSList->abySSID)->len) {
+					if (memcmp(pSSID->abySSID,
+						   ((PWLAN_IE_SSID)pBSSList->abySSID)->abySSID,
+						   pSSID->len) == 0)
+						return pBSSList;
+				}
+			}
+		}
+	}
 
+	return NULL;
+};
 
 /*+
  *
@@ -363,212 +340,206 @@ BSSpAddrIsInBSSList(
  * Return Value:
  *    true if success.
  *
--*/
+ -*/
 
 bool
-BSSbInsertToBSSList (
-    void *hDeviceContext,
-    unsigned char *abyBSSIDAddr,
-    QWORD qwTimestamp,
-    unsigned short wBeaconInterval,
-    unsigned short wCapInfo,
-    unsigned char byCurrChannel,
-    PWLAN_IE_SSID pSSID,
-    PWLAN_IE_SUPP_RATES pSuppRates,
-    PWLAN_IE_SUPP_RATES pExtSuppRates,
-    PERPObject psERP,
-    PWLAN_IE_RSN pRSN,
-    PWLAN_IE_RSN_EXT pRSNWPA,
-    PWLAN_IE_COUNTRY pIE_Country,
-    PWLAN_IE_QUIET pIE_Quiet,
-    unsigned int uIELength,
-    unsigned char *pbyIEs,
-    void *pRxPacketContext
-    )
+BSSbInsertToBSSList(
+	void *hDeviceContext,
+	unsigned char *abyBSSIDAddr,
+	QWORD qwTimestamp,
+	unsigned short wBeaconInterval,
+	unsigned short wCapInfo,
+	unsigned char byCurrChannel,
+	PWLAN_IE_SSID pSSID,
+	PWLAN_IE_SUPP_RATES pSuppRates,
+	PWLAN_IE_SUPP_RATES pExtSuppRates,
+	PERPObject psERP,
+	PWLAN_IE_RSN pRSN,
+	PWLAN_IE_RSN_EXT pRSNWPA,
+	PWLAN_IE_COUNTRY pIE_Country,
+	PWLAN_IE_QUIET pIE_Quiet,
+	unsigned int uIELength,
+	unsigned char *pbyIEs,
+	void *pRxPacketContext
+)
 {
+	PSDevice     pDevice = (PSDevice)hDeviceContext;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	PSRxMgmtPacket  pRxPacket = (PSRxMgmtPacket)pRxPacketContext;
+	PKnownBSS       pBSSList = NULL;
+	unsigned int ii;
+	bool bParsingQuiet = false;
+	PWLAN_IE_QUIET  pQuiet = NULL;
+
+	pBSSList = (PKnownBSS)&(pMgmt->sBSSList[0]);
+
+	for (ii = 0; ii < MAX_BSS_NUM; ii++) {
+		pBSSList = (PKnownBSS)&(pMgmt->sBSSList[ii]);
+		if (!pBSSList->bActive)
+			break;
+	}
+
+	if (ii == MAX_BSS_NUM) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Get free KnowBSS node failed.\n");
+		return false;
+	}
+	// save the BSS info
+	pBSSList->bActive = true;
+	memcpy(pBSSList->abyBSSID, abyBSSIDAddr, WLAN_BSSID_LEN);
+	HIDWORD(pBSSList->qwBSSTimestamp) = cpu_to_le32(HIDWORD(qwTimestamp));
+	LODWORD(pBSSList->qwBSSTimestamp) = cpu_to_le32(LODWORD(qwTimestamp));
+	pBSSList->wBeaconInterval = cpu_to_le16(wBeaconInterval);
+	pBSSList->wCapInfo = cpu_to_le16(wCapInfo);
+	pBSSList->uClearCount = 0;
+
+	if (pSSID->len > WLAN_SSID_MAXLEN)
+		pSSID->len = WLAN_SSID_MAXLEN;
+	memcpy(pBSSList->abySSID, pSSID, pSSID->len + WLAN_IEHDR_LEN);
+
+	pBSSList->uChannel = byCurrChannel;
+
+	if (pSuppRates->len > WLAN_RATES_MAXLEN)
+		pSuppRates->len = WLAN_RATES_MAXLEN;
+	memcpy(pBSSList->abySuppRates, pSuppRates, pSuppRates->len + WLAN_IEHDR_LEN);
+
+	if (pExtSuppRates != NULL) {
+		if (pExtSuppRates->len > WLAN_RATES_MAXLEN)
+			pExtSuppRates->len = WLAN_RATES_MAXLEN;
+		memcpy(pBSSList->abyExtSuppRates, pExtSuppRates, pExtSuppRates->len + WLAN_IEHDR_LEN);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BSSbInsertToBSSList: pExtSuppRates->len = %d\n", pExtSuppRates->len);
+
+	} else {
+		memset(pBSSList->abyExtSuppRates, 0, WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1);
+	}
+	pBSSList->sERP.byERP = psERP->byERP;
+	pBSSList->sERP.bERPExist = psERP->bERPExist;
+
+	// Check if BSS is 802.11a/b/g
+	if (pBSSList->uChannel > CB_MAX_CHANNEL_24G) {
+		pBSSList->eNetworkTypeInUse = PHY_TYPE_11A;
+	} else {
+		if (pBSSList->sERP.bERPExist == true) {
+			pBSSList->eNetworkTypeInUse = PHY_TYPE_11G;
+		} else {
+			pBSSList->eNetworkTypeInUse = PHY_TYPE_11B;
+		}
+	}
+
+	pBSSList->byRxRate = pRxPacket->byRxRate;
+	pBSSList->qwLocalTSF = pRxPacket->qwLocalTSF;
+	pBSSList->uRSSI = pRxPacket->uRSSI;
+	pBSSList->bySQ = pRxPacket->bySQ;
+
+	if ((pMgmt->eCurrMode == WMAC_MODE_ESS_STA) &&
+	    (pMgmt->eCurrState == WMAC_STATE_ASSOC)) {
+		// assoc with BSS
+		if (pBSSList == pMgmt->pCurrBSS) {
+			bParsingQuiet = true;
+		}
+	}
 
-    PSDevice     pDevice = (PSDevice)hDeviceContext;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    PSRxMgmtPacket  pRxPacket = (PSRxMgmtPacket)pRxPacketContext;
-    PKnownBSS       pBSSList = NULL;
-    unsigned int ii;
-    bool bParsingQuiet = false;
-    PWLAN_IE_QUIET  pQuiet = NULL;
-
-
-
-    pBSSList = (PKnownBSS)&(pMgmt->sBSSList[0]);
-
-    for (ii = 0; ii < MAX_BSS_NUM; ii++) {
-        pBSSList = (PKnownBSS)&(pMgmt->sBSSList[ii]);
-        if (!pBSSList->bActive)
-                break;
-    }
-
-    if (ii == MAX_BSS_NUM){
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Get free KnowBSS node failed.\n");
-        return false;
-    }
-    // save the BSS info
-    pBSSList->bActive = true;
-    memcpy( pBSSList->abyBSSID, abyBSSIDAddr, WLAN_BSSID_LEN);
-    HIDWORD(pBSSList->qwBSSTimestamp) = cpu_to_le32(HIDWORD(qwTimestamp));
-    LODWORD(pBSSList->qwBSSTimestamp) = cpu_to_le32(LODWORD(qwTimestamp));
-    pBSSList->wBeaconInterval = cpu_to_le16(wBeaconInterval);
-    pBSSList->wCapInfo = cpu_to_le16(wCapInfo);
-    pBSSList->uClearCount = 0;
-
-    if (pSSID->len > WLAN_SSID_MAXLEN)
-        pSSID->len = WLAN_SSID_MAXLEN;
-    memcpy( pBSSList->abySSID, pSSID, pSSID->len + WLAN_IEHDR_LEN);
-
-    pBSSList->uChannel = byCurrChannel;
-
-    if (pSuppRates->len > WLAN_RATES_MAXLEN)
-        pSuppRates->len = WLAN_RATES_MAXLEN;
-    memcpy( pBSSList->abySuppRates, pSuppRates, pSuppRates->len + WLAN_IEHDR_LEN);
-
-    if (pExtSuppRates != NULL) {
-        if (pExtSuppRates->len > WLAN_RATES_MAXLEN)
-            pExtSuppRates->len = WLAN_RATES_MAXLEN;
-        memcpy(pBSSList->abyExtSuppRates, pExtSuppRates, pExtSuppRates->len + WLAN_IEHDR_LEN);
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"BSSbInsertToBSSList: pExtSuppRates->len = %d\n", pExtSuppRates->len);
-
-    } else {
-        memset(pBSSList->abyExtSuppRates, 0, WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1);
-    }
-    pBSSList->sERP.byERP = psERP->byERP;
-    pBSSList->sERP.bERPExist = psERP->bERPExist;
-
-    // Check if BSS is 802.11a/b/g
-    if (pBSSList->uChannel > CB_MAX_CHANNEL_24G) {
-        pBSSList->eNetworkTypeInUse = PHY_TYPE_11A;
-    } else {
-        if (pBSSList->sERP.bERPExist == true) {
-            pBSSList->eNetworkTypeInUse = PHY_TYPE_11G;
-        } else {
-            pBSSList->eNetworkTypeInUse = PHY_TYPE_11B;
-        }
-    }
-
-    pBSSList->byRxRate = pRxPacket->byRxRate;
-    pBSSList->qwLocalTSF = pRxPacket->qwLocalTSF;
-    pBSSList->uRSSI = pRxPacket->uRSSI;
-    pBSSList->bySQ = pRxPacket->bySQ;
-
-   if ((pMgmt->eCurrMode == WMAC_MODE_ESS_STA) &&
-        (pMgmt->eCurrState == WMAC_STATE_ASSOC)) {
-        // assoc with BSS
-        if (pBSSList == pMgmt->pCurrBSS) {
-            bParsingQuiet = true;
-        }
-    }
-
-    WPA_ClearRSN(pBSSList);
-
-    if (pRSNWPA != NULL) {
-        unsigned int uLen = pRSNWPA->len + 2;
-
-        if (uLen <= (uIELength - (unsigned int)((unsigned char *)pRSNWPA - pbyIEs))) {
-            pBSSList->wWPALen = uLen;
-            memcpy(pBSSList->byWPAIE, pRSNWPA, uLen);
-            WPA_ParseRSN(pBSSList, pRSNWPA);
-        }
-    }
-
-    WPA2_ClearRSN(pBSSList);
-
-    if (pRSN != NULL) {
-        unsigned int uLen = pRSN->len + 2;
-        if (uLen <= (uIELength - (unsigned int)((unsigned char *)pRSN - pbyIEs))) {
-            pBSSList->wRSNLen = uLen;
-            memcpy(pBSSList->byRSNIE, pRSN, uLen);
-            WPA2vParseRSN(pBSSList, pRSN);
-        }
-    }
-
-    if ((pMgmt->eAuthenMode == WMAC_AUTH_WPA2) || (pBSSList->bWPA2Valid == true)) {
-
-        PSKeyItem  pTransmitKey = NULL;
-        bool bIs802_1x = false;
-
-        for (ii = 0; ii < pBSSList->wAKMSSAuthCount; ii ++) {
-            if (pBSSList->abyAKMSSAuthType[ii] == WLAN_11i_AKMSS_802_1X) {
-                bIs802_1x = true;
-                break;
-            }
-        }
-        if ((bIs802_1x == true) && (pSSID->len == ((PWLAN_IE_SSID)pMgmt->abyDesireSSID)->len) &&
-            ( !memcmp(pSSID->abySSID, ((PWLAN_IE_SSID)pMgmt->abyDesireSSID)->abySSID, pSSID->len))) {
-
-            bAdd_PMKID_Candidate((void *)pDevice, pBSSList->abyBSSID, &pBSSList->sRSNCapObj);
-
-            if ((pDevice->bLinkPass == true) && (pMgmt->eCurrState == WMAC_STATE_ASSOC)) {
-                if ((KeybGetTransmitKey(&(pDevice->sKey), pDevice->abyBSSID, PAIRWISE_KEY, &pTransmitKey) == true) ||
-                    (KeybGetTransmitKey(&(pDevice->sKey), pDevice->abyBSSID, GROUP_KEY, &pTransmitKey) == true)) {
-                    pDevice->gsPMKIDCandidate.StatusType = Ndis802_11StatusType_PMKID_CandidateList;
-                    pDevice->gsPMKIDCandidate.Version = 1;
-
-                }
-
-            }
-        }
-    }
-
-    if (pDevice->bUpdateBBVGA) {
-        // Moniter if RSSI is too strong.
-        pBSSList->byRSSIStatCnt = 0;
-        RFvRSSITodBm(pDevice, (unsigned char)(pRxPacket->uRSSI), &pBSSList->ldBmMAX);
-        pBSSList->ldBmAverage[0] = pBSSList->ldBmMAX;
-        for (ii = 1; ii < RSSI_STAT_COUNT; ii++)
-            pBSSList->ldBmAverage[ii] = 0;
-    }
-
-    if ((pIE_Country != NULL) &&
-        (pMgmt->b11hEnable == true)) {
-        set_country_info(pMgmt->pAdapter, pBSSList->eNetworkTypeInUse,
-                            pIE_Country);
-    }
-
-    if ((bParsingQuiet == true) && (pIE_Quiet != NULL)) {
-        if ((((PWLAN_IE_QUIET)pIE_Quiet)->len == 8) &&
-            (((PWLAN_IE_QUIET)pIE_Quiet)->byQuietCount != 0)) {
-            // valid EID
-            if (pQuiet == NULL) {
-                pQuiet = (PWLAN_IE_QUIET)pIE_Quiet;
-                CARDbSetQuiet(  pMgmt->pAdapter,
-                                true,
-                                pQuiet->byQuietCount,
-                                pQuiet->byQuietPeriod,
-                                *((unsigned short *)pQuiet->abyQuietDuration),
-                                *((unsigned short *)pQuiet->abyQuietOffset)
-                                );
-            } else {
-                pQuiet = (PWLAN_IE_QUIET)pIE_Quiet;
-                CARDbSetQuiet(  pMgmt->pAdapter,
-                                false,
-                                pQuiet->byQuietCount,
-                                pQuiet->byQuietPeriod,
-                                *((unsigned short *)pQuiet->abyQuietDuration),
-                                *((unsigned short *)pQuiet->abyQuietOffset)
-                                );
-            }
-        }
-    }
-
-    if ((bParsingQuiet == true) &&
-        (pQuiet != NULL)) {
-        CARDbStartQuiet(pMgmt->pAdapter);
-    }
-
-    pBSSList->uIELength = uIELength;
-    if (pBSSList->uIELength > WLAN_BEACON_FR_MAXLEN)
-        pBSSList->uIELength = WLAN_BEACON_FR_MAXLEN;
-    memcpy(pBSSList->abyIEs, pbyIEs, pBSSList->uIELength);
-
-    return true;
-}
+	WPA_ClearRSN(pBSSList);
 
+	if (pRSNWPA != NULL) {
+		unsigned int uLen = pRSNWPA->len + 2;
+
+		if (uLen <= (uIELength - (unsigned int)((unsigned char *)pRSNWPA - pbyIEs))) {
+			pBSSList->wWPALen = uLen;
+			memcpy(pBSSList->byWPAIE, pRSNWPA, uLen);
+			WPA_ParseRSN(pBSSList, pRSNWPA);
+		}
+	}
+
+	WPA2_ClearRSN(pBSSList);
+
+	if (pRSN != NULL) {
+		unsigned int uLen = pRSN->len + 2;
+		if (uLen <= (uIELength - (unsigned int)((unsigned char *)pRSN - pbyIEs))) {
+			pBSSList->wRSNLen = uLen;
+			memcpy(pBSSList->byRSNIE, pRSN, uLen);
+			WPA2vParseRSN(pBSSList, pRSN);
+		}
+	}
+
+	if ((pMgmt->eAuthenMode == WMAC_AUTH_WPA2) || (pBSSList->bWPA2Valid == true)) {
+		PSKeyItem  pTransmitKey = NULL;
+		bool bIs802_1x = false;
+
+		for (ii = 0; ii < pBSSList->wAKMSSAuthCount; ii++) {
+			if (pBSSList->abyAKMSSAuthType[ii] == WLAN_11i_AKMSS_802_1X) {
+				bIs802_1x = true;
+				break;
+			}
+		}
+		if ((bIs802_1x == true) && (pSSID->len == ((PWLAN_IE_SSID)pMgmt->abyDesireSSID)->len) &&
+		    (!memcmp(pSSID->abySSID, ((PWLAN_IE_SSID)pMgmt->abyDesireSSID)->abySSID, pSSID->len))) {
+			bAdd_PMKID_Candidate((void *)pDevice, pBSSList->abyBSSID, &pBSSList->sRSNCapObj);
+
+			if ((pDevice->bLinkPass == true) && (pMgmt->eCurrState == WMAC_STATE_ASSOC)) {
+				if ((KeybGetTransmitKey(&(pDevice->sKey), pDevice->abyBSSID, PAIRWISE_KEY, &pTransmitKey) == true) ||
+				    (KeybGetTransmitKey(&(pDevice->sKey), pDevice->abyBSSID, GROUP_KEY, &pTransmitKey) == true)) {
+					pDevice->gsPMKIDCandidate.StatusType = Ndis802_11StatusType_PMKID_CandidateList;
+					pDevice->gsPMKIDCandidate.Version = 1;
+
+				}
+
+			}
+		}
+	}
+
+	if (pDevice->bUpdateBBVGA) {
+		// Moniter if RSSI is too strong.
+		pBSSList->byRSSIStatCnt = 0;
+		RFvRSSITodBm(pDevice, (unsigned char)(pRxPacket->uRSSI), &pBSSList->ldBmMAX);
+		pBSSList->ldBmAverage[0] = pBSSList->ldBmMAX;
+		for (ii = 1; ii < RSSI_STAT_COUNT; ii++)
+			pBSSList->ldBmAverage[ii] = 0;
+	}
+
+	if ((pIE_Country != NULL) &&
+	    (pMgmt->b11hEnable == true)) {
+		set_country_info(pMgmt->pAdapter, pBSSList->eNetworkTypeInUse,
+				 pIE_Country);
+	}
+
+	if ((bParsingQuiet == true) && (pIE_Quiet != NULL)) {
+		if ((((PWLAN_IE_QUIET)pIE_Quiet)->len == 8) &&
+		    (((PWLAN_IE_QUIET)pIE_Quiet)->byQuietCount != 0)) {
+			// valid EID
+			if (pQuiet == NULL) {
+				pQuiet = (PWLAN_IE_QUIET)pIE_Quiet;
+				CARDbSetQuiet(pMgmt->pAdapter,
+					      true,
+					      pQuiet->byQuietCount,
+					      pQuiet->byQuietPeriod,
+					      *((unsigned short *)pQuiet->abyQuietDuration),
+					      *((unsigned short *)pQuiet->abyQuietOffset)
+);
+			} else {
+				pQuiet = (PWLAN_IE_QUIET)pIE_Quiet;
+				CARDbSetQuiet(pMgmt->pAdapter,
+					      false,
+					      pQuiet->byQuietCount,
+					      pQuiet->byQuietPeriod,
+					      *((unsigned short *)pQuiet->abyQuietDuration),
+					      *((unsigned short *)pQuiet->abyQuietOffset)
+					);
+			}
+		}
+	}
+
+	if ((bParsingQuiet == true) &&
+	    (pQuiet != NULL)) {
+		CARDbStartQuiet(pMgmt->pAdapter);
+	}
+
+	pBSSList->uIELength = uIELength;
+	if (pBSSList->uIELength > WLAN_BEACON_FR_MAXLEN)
+		pBSSList->uIELength = WLAN_BEACON_FR_MAXLEN;
+	memcpy(pBSSList->abyIEs, pbyIEs, pBSSList->uIELength);
+
+	return true;
+}
 
 /*+
  *
@@ -578,176 +549,169 @@ BSSbInsertToBSSList (
  * Return Value:
  *    true if success.
  *
--*/
+ -*/
 // TODO: input structure modify
 
 bool
-BSSbUpdateToBSSList (
-    void *hDeviceContext,
-    QWORD qwTimestamp,
-    unsigned short wBeaconInterval,
-    unsigned short wCapInfo,
-    unsigned char byCurrChannel,
-    bool bChannelHit,
-    PWLAN_IE_SSID pSSID,
-    PWLAN_IE_SUPP_RATES pSuppRates,
-    PWLAN_IE_SUPP_RATES pExtSuppRates,
-    PERPObject psERP,
-    PWLAN_IE_RSN pRSN,
-    PWLAN_IE_RSN_EXT pRSNWPA,
-    PWLAN_IE_COUNTRY pIE_Country,
-    PWLAN_IE_QUIET pIE_Quiet,
-    PKnownBSS pBSSList,
-    unsigned int uIELength,
-    unsigned char *pbyIEs,
-    void *pRxPacketContext
-    )
+BSSbUpdateToBSSList(
+	void *hDeviceContext,
+	QWORD qwTimestamp,
+	unsigned short wBeaconInterval,
+	unsigned short wCapInfo,
+	unsigned char byCurrChannel,
+	bool bChannelHit,
+	PWLAN_IE_SSID pSSID,
+	PWLAN_IE_SUPP_RATES pSuppRates,
+	PWLAN_IE_SUPP_RATES pExtSuppRates,
+	PERPObject psERP,
+	PWLAN_IE_RSN pRSN,
+	PWLAN_IE_RSN_EXT pRSNWPA,
+	PWLAN_IE_COUNTRY pIE_Country,
+	PWLAN_IE_QUIET pIE_Quiet,
+	PKnownBSS pBSSList,
+	unsigned int uIELength,
+	unsigned char *pbyIEs,
+	void *pRxPacketContext
+)
 {
-    int             ii;
-    PSDevice        pDevice = (PSDevice)hDeviceContext;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    PSRxMgmtPacket  pRxPacket = (PSRxMgmtPacket)pRxPacketContext;
-    long            ldBm;
-    bool bParsingQuiet = false;
-    PWLAN_IE_QUIET  pQuiet = NULL;
-
-
-
-    if (pBSSList == NULL)
-        return false;
-
-
-    HIDWORD(pBSSList->qwBSSTimestamp) = cpu_to_le32(HIDWORD(qwTimestamp));
-    LODWORD(pBSSList->qwBSSTimestamp) = cpu_to_le32(LODWORD(qwTimestamp));
-    pBSSList->wBeaconInterval = cpu_to_le16(wBeaconInterval);
-    pBSSList->wCapInfo = cpu_to_le16(wCapInfo);
-    pBSSList->uClearCount = 0;
-    pBSSList->uChannel = byCurrChannel;
-//    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"BSSbUpdateToBSSList: pBSSList->uChannel: %d\n", pBSSList->uChannel);
-
-    if (pSSID->len > WLAN_SSID_MAXLEN)
-        pSSID->len = WLAN_SSID_MAXLEN;
-
-    if ((pSSID->len != 0) && (pSSID->abySSID[0] != 0))
-        memcpy(pBSSList->abySSID, pSSID, pSSID->len + WLAN_IEHDR_LEN);
-    memcpy(pBSSList->abySuppRates, pSuppRates,pSuppRates->len + WLAN_IEHDR_LEN);
-
-    if (pExtSuppRates != NULL) {
-        memcpy(pBSSList->abyExtSuppRates, pExtSuppRates,pExtSuppRates->len + WLAN_IEHDR_LEN);
-    } else {
-        memset(pBSSList->abyExtSuppRates, 0, WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1);
-    }
-    pBSSList->sERP.byERP = psERP->byERP;
-    pBSSList->sERP.bERPExist = psERP->bERPExist;
-
-    // Check if BSS is 802.11a/b/g
-    if (pBSSList->uChannel > CB_MAX_CHANNEL_24G) {
-        pBSSList->eNetworkTypeInUse = PHY_TYPE_11A;
-    } else {
-        if (pBSSList->sERP.bERPExist == true) {
-            pBSSList->eNetworkTypeInUse = PHY_TYPE_11G;
-        } else {
-            pBSSList->eNetworkTypeInUse = PHY_TYPE_11B;
-        }
-    }
-
-    pBSSList->byRxRate = pRxPacket->byRxRate;
-    pBSSList->qwLocalTSF = pRxPacket->qwLocalTSF;
-    if(bChannelHit)
-        pBSSList->uRSSI = pRxPacket->uRSSI;
-    pBSSList->bySQ = pRxPacket->bySQ;
-
-   if ((pMgmt->eCurrMode == WMAC_MODE_ESS_STA) &&
-        (pMgmt->eCurrState == WMAC_STATE_ASSOC)) {
-        // assoc with BSS
-        if (pBSSList == pMgmt->pCurrBSS) {
-            bParsingQuiet = true;
-        }
-    }
-
-   WPA_ClearRSN(pBSSList);         //mike update
-
-    if (pRSNWPA != NULL) {
-        unsigned int uLen = pRSNWPA->len + 2;
-        if (uLen <= (uIELength - (unsigned int)((unsigned char *)pRSNWPA - pbyIEs))) {
-            pBSSList->wWPALen = uLen;
-            memcpy(pBSSList->byWPAIE, pRSNWPA, uLen);
-            WPA_ParseRSN(pBSSList, pRSNWPA);
-        }
-    }
-
-   WPA2_ClearRSN(pBSSList);  //mike update
-
-    if (pRSN != NULL) {
-        unsigned int uLen = pRSN->len + 2;
-        if (uLen <= (uIELength - (unsigned int)((unsigned char *)pRSN - pbyIEs))) {
-            pBSSList->wRSNLen = uLen;
-            memcpy(pBSSList->byRSNIE, pRSN, uLen);
-            WPA2vParseRSN(pBSSList, pRSN);
-        }
-    }
-
-    if (pRxPacket->uRSSI != 0) {
-        RFvRSSITodBm(pDevice, (unsigned char)(pRxPacket->uRSSI), &ldBm);
-        // Moniter if RSSI is too strong.
-        pBSSList->byRSSIStatCnt++;
-        pBSSList->byRSSIStatCnt %= RSSI_STAT_COUNT;
-        pBSSList->ldBmAverage[pBSSList->byRSSIStatCnt] = ldBm;
-        for(ii=0;ii<RSSI_STAT_COUNT;ii++) {
-            if (pBSSList->ldBmAverage[ii] != 0) {
-                pBSSList->ldBmMAX = max(pBSSList->ldBmAverage[ii], ldBm);
-            }
-        }
-    }
-
-    if ((pIE_Country != NULL) &&
-        (pMgmt->b11hEnable == true)) {
-        set_country_info(pMgmt->pAdapter, pBSSList->eNetworkTypeInUse,
-                            pIE_Country);
-    }
-
-    if ((bParsingQuiet == true) && (pIE_Quiet != NULL)) {
-        if ((((PWLAN_IE_QUIET)pIE_Quiet)->len == 8) &&
-            (((PWLAN_IE_QUIET)pIE_Quiet)->byQuietCount != 0)) {
-            // valid EID
-            if (pQuiet == NULL) {
-                pQuiet = (PWLAN_IE_QUIET)pIE_Quiet;
-                CARDbSetQuiet(  pMgmt->pAdapter,
-                                true,
-                                pQuiet->byQuietCount,
-                                pQuiet->byQuietPeriod,
-                                *((unsigned short *)pQuiet->abyQuietDuration),
-                                *((unsigned short *)pQuiet->abyQuietOffset)
-                                );
-            } else {
-                pQuiet = (PWLAN_IE_QUIET)pIE_Quiet;
-                CARDbSetQuiet(  pMgmt->pAdapter,
-                                false,
-                                pQuiet->byQuietCount,
-                                pQuiet->byQuietPeriod,
-                                *((unsigned short *)pQuiet->abyQuietDuration),
-                                *((unsigned short *)pQuiet->abyQuietOffset)
-                                );
-            }
-        }
-    }
-
-    if ((bParsingQuiet == true) &&
-        (pQuiet != NULL)) {
-        CARDbStartQuiet(pMgmt->pAdapter);
-    }
-
-    pBSSList->uIELength = uIELength;
-    if (pBSSList->uIELength > WLAN_BEACON_FR_MAXLEN)
-        pBSSList->uIELength = WLAN_BEACON_FR_MAXLEN;
-    memcpy(pBSSList->abyIEs, pbyIEs, pBSSList->uIELength);
-
-    return true;
-}
+	int             ii;
+	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	PSRxMgmtPacket  pRxPacket = (PSRxMgmtPacket)pRxPacketContext;
+	long            ldBm;
+	bool bParsingQuiet = false;
+	PWLAN_IE_QUIET  pQuiet = NULL;
+
+	if (pBSSList == NULL)
+		return false;
+
+	HIDWORD(pBSSList->qwBSSTimestamp) = cpu_to_le32(HIDWORD(qwTimestamp));
+	LODWORD(pBSSList->qwBSSTimestamp) = cpu_to_le32(LODWORD(qwTimestamp));
+	pBSSList->wBeaconInterval = cpu_to_le16(wBeaconInterval);
+	pBSSList->wCapInfo = cpu_to_le16(wCapInfo);
+	pBSSList->uClearCount = 0;
+	pBSSList->uChannel = byCurrChannel;
+//    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BSSbUpdateToBSSList: pBSSList->uChannel: %d\n", pBSSList->uChannel);
+
+	if (pSSID->len > WLAN_SSID_MAXLEN)
+		pSSID->len = WLAN_SSID_MAXLEN;
+
+	if ((pSSID->len != 0) && (pSSID->abySSID[0] != 0))
+		memcpy(pBSSList->abySSID, pSSID, pSSID->len + WLAN_IEHDR_LEN);
+	memcpy(pBSSList->abySuppRates, pSuppRates, pSuppRates->len + WLAN_IEHDR_LEN);
+
+	if (pExtSuppRates != NULL) {
+		memcpy(pBSSList->abyExtSuppRates, pExtSuppRates, pExtSuppRates->len + WLAN_IEHDR_LEN);
+	} else {
+		memset(pBSSList->abyExtSuppRates, 0, WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1);
+	}
+	pBSSList->sERP.byERP = psERP->byERP;
+	pBSSList->sERP.bERPExist = psERP->bERPExist;
+
+	// Check if BSS is 802.11a/b/g
+	if (pBSSList->uChannel > CB_MAX_CHANNEL_24G) {
+		pBSSList->eNetworkTypeInUse = PHY_TYPE_11A;
+	} else {
+		if (pBSSList->sERP.bERPExist == true) {
+			pBSSList->eNetworkTypeInUse = PHY_TYPE_11G;
+		} else {
+			pBSSList->eNetworkTypeInUse = PHY_TYPE_11B;
+		}
+	}
+
+	pBSSList->byRxRate = pRxPacket->byRxRate;
+	pBSSList->qwLocalTSF = pRxPacket->qwLocalTSF;
+	if (bChannelHit)
+		pBSSList->uRSSI = pRxPacket->uRSSI;
+	pBSSList->bySQ = pRxPacket->bySQ;
+
+	if ((pMgmt->eCurrMode == WMAC_MODE_ESS_STA) &&
+	    (pMgmt->eCurrState == WMAC_STATE_ASSOC)) {
+		// assoc with BSS
+		if (pBSSList == pMgmt->pCurrBSS) {
+			bParsingQuiet = true;
+		}
+	}
 
+	WPA_ClearRSN(pBSSList);         //mike update
 
+	if (pRSNWPA != NULL) {
+		unsigned int uLen = pRSNWPA->len + 2;
+		if (uLen <= (uIELength - (unsigned int)((unsigned char *)pRSNWPA - pbyIEs))) {
+			pBSSList->wWPALen = uLen;
+			memcpy(pBSSList->byWPAIE, pRSNWPA, uLen);
+			WPA_ParseRSN(pBSSList, pRSNWPA);
+		}
+	}
+
+	WPA2_ClearRSN(pBSSList);  //mike update
+
+	if (pRSN != NULL) {
+		unsigned int uLen = pRSN->len + 2;
+		if (uLen <= (uIELength - (unsigned int)((unsigned char *)pRSN - pbyIEs))) {
+			pBSSList->wRSNLen = uLen;
+			memcpy(pBSSList->byRSNIE, pRSN, uLen);
+			WPA2vParseRSN(pBSSList, pRSN);
+		}
+	}
+
+	if (pRxPacket->uRSSI != 0) {
+		RFvRSSITodBm(pDevice, (unsigned char)(pRxPacket->uRSSI), &ldBm);
+		// Moniter if RSSI is too strong.
+		pBSSList->byRSSIStatCnt++;
+		pBSSList->byRSSIStatCnt %= RSSI_STAT_COUNT;
+		pBSSList->ldBmAverage[pBSSList->byRSSIStatCnt] = ldBm;
+		for (ii = 0; ii < RSSI_STAT_COUNT; ii++) {
+			if (pBSSList->ldBmAverage[ii] != 0) {
+				pBSSList->ldBmMAX = max(pBSSList->ldBmAverage[ii], ldBm);
+			}
+		}
+	}
+
+	if ((pIE_Country != NULL) &&
+	    (pMgmt->b11hEnable == true)) {
+		set_country_info(pMgmt->pAdapter, pBSSList->eNetworkTypeInUse,
+				 pIE_Country);
+	}
+
+	if ((bParsingQuiet == true) && (pIE_Quiet != NULL)) {
+		if ((((PWLAN_IE_QUIET)pIE_Quiet)->len == 8) &&
+		    (((PWLAN_IE_QUIET)pIE_Quiet)->byQuietCount != 0)) {
+			// valid EID
+			if (pQuiet == NULL) {
+				pQuiet = (PWLAN_IE_QUIET)pIE_Quiet;
+				CARDbSetQuiet(pMgmt->pAdapter,
+					      true,
+					      pQuiet->byQuietCount,
+					      pQuiet->byQuietPeriod,
+					      *((unsigned short *)pQuiet->abyQuietDuration),
+					      *((unsigned short *)pQuiet->abyQuietOffset)
+);
+			} else {
+				pQuiet = (PWLAN_IE_QUIET)pIE_Quiet;
+				CARDbSetQuiet(pMgmt->pAdapter,
+					      false,
+					      pQuiet->byQuietCount,
+					      pQuiet->byQuietPeriod,
+					      *((unsigned short *)pQuiet->abyQuietDuration),
+					      *((unsigned short *)pQuiet->abyQuietOffset)
+					);
+			}
+		}
+	}
 
+	if ((bParsingQuiet == true) &&
+	    (pQuiet != NULL)) {
+		CARDbStartQuiet(pMgmt->pAdapter);
+	}
 
+	pBSSList->uIELength = uIELength;
+	if (pBSSList->uIELength > WLAN_BEACON_FR_MAXLEN)
+		pBSSList->uIELength = WLAN_BEACON_FR_MAXLEN;
+	memcpy(pBSSList->abyIEs, pbyIEs, pBSSList->uIELength);
+
+	return true;
+}
 
 /*+
  *
@@ -757,29 +721,27 @@ BSSbUpdateToBSSList (
  * Return Value:
  *    None
  *
--*/
+ -*/
 
 bool
 BSSDBbIsSTAInNodeDB(void *pMgmtObject, unsigned char *abyDstAddr,
-		unsigned int *puNodeIndex)
+		    unsigned int *puNodeIndex)
 {
-    PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
-    unsigned int ii;
-
-    // Index = 0 reserved for AP Node
-    for (ii = 1; ii < (MAX_NODE_NUM + 1); ii++) {
-        if (pMgmt->sNodeDBTable[ii].bActive) {
-            if (!compare_ether_addr(abyDstAddr, pMgmt->sNodeDBTable[ii].abyMACAddr)) {
-                *puNodeIndex = ii;
-                return true;
-            }
-        }
-    }
-
-   return false;
-};
-
+	PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
+	unsigned int ii;
+
+	// Index = 0 reserved for AP Node
+	for (ii = 1; ii < (MAX_NODE_NUM + 1); ii++) {
+		if (pMgmt->sNodeDBTable[ii].bActive) {
+			if (!compare_ether_addr(abyDstAddr, pMgmt->sNodeDBTable[ii].abyMACAddr)) {
+				*puNodeIndex = ii;
+				return true;
+			}
+		}
+	}
 
+	return false;
+};
 
 /*+
  *
@@ -790,59 +752,54 @@ BSSDBbIsSTAInNodeDB(void *pMgmtObject, unsigned char *abyDstAddr,
  * Return Value:
  *    None
  *
--*/
+ -*/
 void
 BSSvCreateOneNode(void *hDeviceContext, unsigned int *puNodeIndex)
 {
-
-    PSDevice     pDevice = (PSDevice)hDeviceContext;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    unsigned int ii;
-    unsigned int BigestCount = 0;
-    unsigned int SelectIndex;
-    struct sk_buff  *skb;
-    // Index = 0 reserved for AP Node (In STA mode)
-    // Index = 0 reserved for Broadcast/MultiCast (In AP mode)
-    SelectIndex = 1;
-    for (ii = 1; ii < (MAX_NODE_NUM + 1); ii++) {
-        if (pMgmt->sNodeDBTable[ii].bActive) {
-            if (pMgmt->sNodeDBTable[ii].uInActiveCount > BigestCount) {
-                BigestCount = pMgmt->sNodeDBTable[ii].uInActiveCount;
-                SelectIndex = ii;
-            }
-        }
-        else {
-            break;
-        }
-    }
-
-    // if not found replace uInActiveCount is largest one.
-    if ( ii == (MAX_NODE_NUM + 1)) {
-        *puNodeIndex = SelectIndex;
-        DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Replace inactive node = %d\n", SelectIndex);
-        // clear ps buffer
-        if (pMgmt->sNodeDBTable[*puNodeIndex].sTxPSQueue.next != NULL) {
-      	    while ((skb = skb_dequeue(&pMgmt->sNodeDBTable[*puNodeIndex].sTxPSQueue)) != NULL)
-            dev_kfree_skb(skb);
-        }
-    }
-    else {
-        *puNodeIndex = ii;
-    }
-
-    memset(&pMgmt->sNodeDBTable[*puNodeIndex], 0, sizeof(KnownNodeDB));
-    pMgmt->sNodeDBTable[*puNodeIndex].bActive = true;
-    pMgmt->sNodeDBTable[*puNodeIndex].uRatePollTimeout = FALLBACK_POLL_SECOND;
-    // for AP mode PS queue
-    skb_queue_head_init(&pMgmt->sNodeDBTable[*puNodeIndex].sTxPSQueue);
-    pMgmt->sNodeDBTable[*puNodeIndex].byAuthSequence = 0;
-    pMgmt->sNodeDBTable[*puNodeIndex].wEnQueueCnt = 0;
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Create node index = %d\n", ii);
-    return;
+	PSDevice     pDevice = (PSDevice)hDeviceContext;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	unsigned int ii;
+	unsigned int BigestCount = 0;
+	unsigned int SelectIndex;
+	struct sk_buff  *skb;
+	// Index = 0 reserved for AP Node (In STA mode)
+	// Index = 0 reserved for Broadcast/MultiCast (In AP mode)
+	SelectIndex = 1;
+	for (ii = 1; ii < (MAX_NODE_NUM + 1); ii++) {
+		if (pMgmt->sNodeDBTable[ii].bActive) {
+			if (pMgmt->sNodeDBTable[ii].uInActiveCount > BigestCount) {
+				BigestCount = pMgmt->sNodeDBTable[ii].uInActiveCount;
+				SelectIndex = ii;
+			}
+		} else {
+			break;
+		}
+	}
+
+	// if not found replace uInActiveCount is largest one.
+	if (ii == (MAX_NODE_NUM + 1)) {
+		*puNodeIndex = SelectIndex;
+		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Replace inactive node = %d\n", SelectIndex);
+		// clear ps buffer
+		if (pMgmt->sNodeDBTable[*puNodeIndex].sTxPSQueue.next != NULL) {
+			while ((skb = skb_dequeue(&pMgmt->sNodeDBTable[*puNodeIndex].sTxPSQueue)) != NULL)
+				dev_kfree_skb(skb);
+		}
+	} else {
+		*puNodeIndex = ii;
+	}
+
+	memset(&pMgmt->sNodeDBTable[*puNodeIndex], 0, sizeof(KnownNodeDB));
+	pMgmt->sNodeDBTable[*puNodeIndex].bActive = true;
+	pMgmt->sNodeDBTable[*puNodeIndex].uRatePollTimeout = FALLBACK_POLL_SECOND;
+	// for AP mode PS queue
+	skb_queue_head_init(&pMgmt->sNodeDBTable[*puNodeIndex].sTxPSQueue);
+	pMgmt->sNodeDBTable[*puNodeIndex].byAuthSequence = 0;
+	pMgmt->sNodeDBTable[*puNodeIndex].wEnQueueCnt = 0;
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Create node index = %d\n", ii);
+	return;
 };
 
-
-
 /*+
  *
  * Routine Description:
@@ -852,28 +809,26 @@ BSSvCreateOneNode(void *hDeviceContext, unsigned int *puNodeIndex)
  * Return Value:
  *    None
  *
--*/
+ -*/
 void
 BSSvRemoveOneNode(
-    void *hDeviceContext,
-    unsigned int uNodeIndex
-    )
+	void *hDeviceContext,
+	unsigned int uNodeIndex
+)
 {
-
-    PSDevice        pDevice = (PSDevice)hDeviceContext;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    unsigned char byMask[8] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80};
-    struct sk_buff  *skb;
-
-
-    while ((skb = skb_dequeue(&pMgmt->sNodeDBTable[uNodeIndex].sTxPSQueue)) != NULL)
-            dev_kfree_skb(skb);
-    // clear context
-    memset(&pMgmt->sNodeDBTable[uNodeIndex], 0, sizeof(KnownNodeDB));
-    // clear tx bit map
-    pMgmt->abyPSTxMap[pMgmt->sNodeDBTable[uNodeIndex].wAID >> 3] &=  ~byMask[pMgmt->sNodeDBTable[uNodeIndex].wAID & 7];
-
-    return;
+	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	unsigned char byMask[8] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80};
+	struct sk_buff  *skb;
+
+	while ((skb = skb_dequeue(&pMgmt->sNodeDBTable[uNodeIndex].sTxPSQueue)) != NULL)
+		dev_kfree_skb(skb);
+	// clear context
+	memset(&pMgmt->sNodeDBTable[uNodeIndex], 0, sizeof(KnownNodeDB));
+	// clear tx bit map
+	pMgmt->abyPSTxMap[pMgmt->sNodeDBTable[uNodeIndex].wAID >> 3] &=  ~byMask[pMgmt->sNodeDBTable[uNodeIndex].wAID & 7];
+
+	return;
 };
 /*+
  *
@@ -884,59 +839,54 @@ BSSvRemoveOneNode(
  * Return Value:
  *    None
  *
--*/
+ -*/
 
 void
 BSSvUpdateAPNode(
-    void *hDeviceContext,
-    unsigned short *pwCapInfo,
-    PWLAN_IE_SUPP_RATES pSuppRates,
-    PWLAN_IE_SUPP_RATES pExtSuppRates
-    )
+	void *hDeviceContext,
+	unsigned short *pwCapInfo,
+	PWLAN_IE_SUPP_RATES pSuppRates,
+	PWLAN_IE_SUPP_RATES pExtSuppRates
+)
 {
-    PSDevice     pDevice = (PSDevice)hDeviceContext;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    unsigned int uRateLen = WLAN_RATES_MAXLEN;
-
-    memset(&pMgmt->sNodeDBTable[0], 0, sizeof(KnownNodeDB));
-
-    pMgmt->sNodeDBTable[0].bActive = true;
-    if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
-        uRateLen = WLAN_RATES_MAXLEN_11B;
-    }
-    pMgmt->abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)pSuppRates,
-                                            (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-                                            uRateLen);
-    pMgmt->abyCurrExtSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)pExtSuppRates,
-                                            (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates,
-                                            uRateLen);
-    RATEvParseMaxRate((void *)pDevice,
-                       (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-                       (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates,
-                       true,
-                       &(pMgmt->sNodeDBTable[0].wMaxBasicRate),
-                       &(pMgmt->sNodeDBTable[0].wMaxSuppRate),
-                       &(pMgmt->sNodeDBTable[0].wSuppRate),
-                       &(pMgmt->sNodeDBTable[0].byTopCCKBasicRate),
-                       &(pMgmt->sNodeDBTable[0].byTopOFDMBasicRate)
-                      );
-    memcpy(pMgmt->sNodeDBTable[0].abyMACAddr, pMgmt->abyCurrBSSID, WLAN_ADDR_LEN);
-    pMgmt->sNodeDBTable[0].wTxDataRate = pMgmt->sNodeDBTable[0].wMaxSuppRate;
-    pMgmt->sNodeDBTable[0].bShortPreamble = WLAN_GET_CAP_INFO_SHORTPREAMBLE(*pwCapInfo);
-    pMgmt->sNodeDBTable[0].uRatePollTimeout = FALLBACK_POLL_SECOND;
+	PSDevice     pDevice = (PSDevice)hDeviceContext;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	unsigned int uRateLen = WLAN_RATES_MAXLEN;
+
+	memset(&pMgmt->sNodeDBTable[0], 0, sizeof(KnownNodeDB));
+
+	pMgmt->sNodeDBTable[0].bActive = true;
+	if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
+		uRateLen = WLAN_RATES_MAXLEN_11B;
+	}
+	pMgmt->abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)pSuppRates,
+						(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
+						uRateLen);
+	pMgmt->abyCurrExtSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)pExtSuppRates,
+						   (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates,
+						   uRateLen);
+	RATEvParseMaxRate((void *)pDevice,
+			  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
+			  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates,
+			  true,
+			  &(pMgmt->sNodeDBTable[0].wMaxBasicRate),
+			  &(pMgmt->sNodeDBTable[0].wMaxSuppRate),
+			  &(pMgmt->sNodeDBTable[0].wSuppRate),
+			  &(pMgmt->sNodeDBTable[0].byTopCCKBasicRate),
+			  &(pMgmt->sNodeDBTable[0].byTopOFDMBasicRate)
+);
+	memcpy(pMgmt->sNodeDBTable[0].abyMACAddr, pMgmt->abyCurrBSSID, WLAN_ADDR_LEN);
+	pMgmt->sNodeDBTable[0].wTxDataRate = pMgmt->sNodeDBTable[0].wMaxSuppRate;
+	pMgmt->sNodeDBTable[0].bShortPreamble = WLAN_GET_CAP_INFO_SHORTPREAMBLE(*pwCapInfo);
+	pMgmt->sNodeDBTable[0].uRatePollTimeout = FALLBACK_POLL_SECOND;
 #ifdef	PLICE_DEBUG
-	printk("BSSvUpdateAPNode:MaxSuppRate is %d\n",pMgmt->sNodeDBTable[0].wMaxSuppRate);
+	printk("BSSvUpdateAPNode:MaxSuppRate is %d\n", pMgmt->sNodeDBTable[0].wMaxSuppRate);
 #endif
-    // Auto rate fallback function initiation.
-    // RATEbInit(pDevice);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pMgmt->sNodeDBTable[0].wTxDataRate = %d \n", pMgmt->sNodeDBTable[0].wTxDataRate);
-
+	// Auto rate fallback function initiation.
+	// RATEbInit(pDevice);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pMgmt->sNodeDBTable[0].wTxDataRate = %d \n", pMgmt->sNodeDBTable[0].wTxDataRate);
 };
 
-
-
-
-
 /*+
  *
  * Routine Description:
@@ -946,45 +896,39 @@ BSSvUpdateAPNode(
  * Return Value:
  *    None
  *
--*/
-
+ -*/
 
 void
 BSSvAddMulticastNode(
-    void *hDeviceContext
-    )
+	void *hDeviceContext
+)
 {
-    PSDevice     pDevice = (PSDevice)hDeviceContext;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-
-    if (!pDevice->bEnableHostWEP)
-        memset(&pMgmt->sNodeDBTable[0], 0, sizeof(KnownNodeDB));
-    memset(pMgmt->sNodeDBTable[0].abyMACAddr, 0xff, WLAN_ADDR_LEN);
-    pMgmt->sNodeDBTable[0].bActive = true;
-    pMgmt->sNodeDBTable[0].bPSEnable = false;
-    skb_queue_head_init(&pMgmt->sNodeDBTable[0].sTxPSQueue);
-    RATEvParseMaxRate((void *)pDevice,
-                      (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-                      (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates,
-                      true,
-                      &(pMgmt->sNodeDBTable[0].wMaxBasicRate),
-                      &(pMgmt->sNodeDBTable[0].wMaxSuppRate),
-                       &(pMgmt->sNodeDBTable[0].wSuppRate),
-                      &(pMgmt->sNodeDBTable[0].byTopCCKBasicRate),
-                      &(pMgmt->sNodeDBTable[0].byTopOFDMBasicRate)
-                     );
-    pMgmt->sNodeDBTable[0].wTxDataRate = pMgmt->sNodeDBTable[0].wMaxBasicRate;
+	PSDevice     pDevice = (PSDevice)hDeviceContext;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+
+	if (!pDevice->bEnableHostWEP)
+		memset(&pMgmt->sNodeDBTable[0], 0, sizeof(KnownNodeDB));
+	memset(pMgmt->sNodeDBTable[0].abyMACAddr, 0xff, WLAN_ADDR_LEN);
+	pMgmt->sNodeDBTable[0].bActive = true;
+	pMgmt->sNodeDBTable[0].bPSEnable = false;
+	skb_queue_head_init(&pMgmt->sNodeDBTable[0].sTxPSQueue);
+	RATEvParseMaxRate((void *)pDevice,
+			  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
+			  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates,
+			  true,
+			  &(pMgmt->sNodeDBTable[0].wMaxBasicRate),
+			  &(pMgmt->sNodeDBTable[0].wMaxSuppRate),
+			  &(pMgmt->sNodeDBTable[0].wSuppRate),
+			  &(pMgmt->sNodeDBTable[0].byTopCCKBasicRate),
+			  &(pMgmt->sNodeDBTable[0].byTopOFDMBasicRate)
+);
+	pMgmt->sNodeDBTable[0].wTxDataRate = pMgmt->sNodeDBTable[0].wMaxBasicRate;
 #ifdef	PLICE_DEBUG
-	printk("BSSvAddMultiCastNode:pMgmt->sNodeDBTable[0].wTxDataRate is %d\n",pMgmt->sNodeDBTable[0].wTxDataRate);
+	printk("BSSvAddMultiCastNode:pMgmt->sNodeDBTable[0].wTxDataRate is %d\n", pMgmt->sNodeDBTable[0].wTxDataRate);
 #endif
-    pMgmt->sNodeDBTable[0].uRatePollTimeout = FALLBACK_POLL_SECOND;
-
+	pMgmt->sNodeDBTable[0].uRatePollTimeout = FALLBACK_POLL_SECOND;
 };
 
-
-
-
-
 /*+
  *
  * Routine Description:
@@ -996,373 +940,348 @@ BSSvAddMulticastNode(
  * Return Value:
  *    none.
  *
--*/
- //2008-4-14 <add> by chester for led issue
- #ifdef FOR_LED_ON_NOTEBOOK
-bool cc=false;
+ -*/
+//2008-4-14 <add> by chester for led issue
+#ifdef FOR_LED_ON_NOTEBOOK
+bool cc = false;
 unsigned int status;
 #endif
 void
 BSSvSecondCallBack(
-    void *hDeviceContext
-    )
+	void *hDeviceContext
+)
 {
-    PSDevice        pDevice = (PSDevice)hDeviceContext;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    unsigned int ii;
-    PWLAN_IE_SSID   pItemSSID, pCurrSSID;
-    unsigned int uSleepySTACnt = 0;
-    unsigned int uNonShortSlotSTACnt = 0;
-    unsigned int uLongPreambleSTACnt = 0;
-    viawget_wpa_header* wpahdr;  //DavidWang
-
-    spin_lock_irq(&pDevice->lock);
-
-    pDevice->uAssocCount = 0;
-
-    pDevice->byERPFlag &=
-        ~(WLAN_SET_ERP_BARKER_MODE(1) | WLAN_SET_ERP_NONERP_PRESENT(1));
- //2008-4-14 <add> by chester for led issue
+	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	unsigned int ii;
+	PWLAN_IE_SSID   pItemSSID, pCurrSSID;
+	unsigned int uSleepySTACnt = 0;
+	unsigned int uNonShortSlotSTACnt = 0;
+	unsigned int uLongPreambleSTACnt = 0;
+	viawget_wpa_header *wpahdr;  //DavidWang
+
+	spin_lock_irq(&pDevice->lock);
+
+	pDevice->uAssocCount = 0;
+
+	pDevice->byERPFlag &=
+		~(WLAN_SET_ERP_BARKER_MODE(1) | WLAN_SET_ERP_NONERP_PRESENT(1));
+	//2008-4-14 <add> by chester for led issue
 #ifdef FOR_LED_ON_NOTEBOOK
-MACvGPIOIn(pDevice->PortOffset, &pDevice->byGPIO);
-if ((( !(pDevice->byGPIO & GPIO0_DATA)&&(pDevice->bHWRadioOff == false))||((pDevice->byGPIO & GPIO0_DATA)&&(pDevice->bHWRadioOff == true)))&&(cc==false)){
-cc=true;
-}
-else if(cc==true){
-
-if(pDevice->bHWRadioOff == true){
-            if ( !(pDevice->byGPIO & GPIO0_DATA))
-//||( !(pDevice->byGPIO & GPIO0_DATA) && (pDevice->byRadioCtl & EEP_RADIOCTL_INV)))
-{if(status==1) goto start;
-status=1;
-CARDbRadioPowerOff(pDevice);
-                pMgmt->sNodeDBTable[0].bActive = false;
-                pMgmt->eCurrMode = WMAC_MODE_STANDBY;
-                pMgmt->eCurrState = WMAC_STATE_IDLE;
-                //netif_stop_queue(pDevice->dev);
-                pDevice->bLinkPass = false;
-
-}
-  if (pDevice->byGPIO &GPIO0_DATA)
-//||( !(pDevice->byGPIO & GPIO0_DATA) && (pDevice->byRadioCtl & EEP_RADIOCTL_INV)))
-{if(status==2) goto start;
-status=2;
-CARDbRadioPowerOn(pDevice);
-} }
-else{
-            if (pDevice->byGPIO & GPIO0_DATA)
-//||( !(pDevice->byGPIO & GPIO0_DATA) && (pDevice->byRadioCtl & EEP_RADIOCTL_INV)))
-{if(status==3) goto start;
-status=3;
-CARDbRadioPowerOff(pDevice);
-                pMgmt->sNodeDBTable[0].bActive = false;
-                pMgmt->eCurrMode = WMAC_MODE_STANDBY;
-                pMgmt->eCurrState = WMAC_STATE_IDLE;
-                //netif_stop_queue(pDevice->dev);
-                pDevice->bLinkPass = false;
-
-}
-  if ( !(pDevice->byGPIO & GPIO0_DATA))
-//||( !(pDevice->byGPIO & GPIO0_DATA) && (pDevice->byRadioCtl & EEP_RADIOCTL_INV)))
-{if(status==4) goto start;
-status=4;
-CARDbRadioPowerOn(pDevice);
-} }
-}
+	MACvGPIOIn(pDevice->PortOffset, &pDevice->byGPIO);
+	if (((!(pDevice->byGPIO & GPIO0_DATA) && (pDevice->bHWRadioOff == false)) || ((pDevice->byGPIO & GPIO0_DATA) && (pDevice->bHWRadioOff == true))) && (cc == false)) {
+		cc = true;
+	} else if (cc == true) {
+		if (pDevice->bHWRadioOff == true) {
+			if (!(pDevice->byGPIO & GPIO0_DATA))
+//||(!(pDevice->byGPIO & GPIO0_DATA) && (pDevice->byRadioCtl & EEP_RADIOCTL_INV)))
+			{
+				if (status == 1) goto start;
+				status = 1;
+				CARDbRadioPowerOff(pDevice);
+				pMgmt->sNodeDBTable[0].bActive = false;
+				pMgmt->eCurrMode = WMAC_MODE_STANDBY;
+				pMgmt->eCurrState = WMAC_STATE_IDLE;
+				//netif_stop_queue(pDevice->dev);
+				pDevice->bLinkPass = false;
+
+			}
+			if (pDevice->byGPIO & GPIO0_DATA)
+//||(!(pDevice->byGPIO & GPIO0_DATA) && (pDevice->byRadioCtl & EEP_RADIOCTL_INV)))
+			{
+				if (status == 2) goto start;
+				status = 2;
+				CARDbRadioPowerOn(pDevice);
+			}
+		} else {
+			if (pDevice->byGPIO & GPIO0_DATA)
+//||(!(pDevice->byGPIO & GPIO0_DATA) && (pDevice->byRadioCtl & EEP_RADIOCTL_INV)))
+			{
+				if (status == 3) goto start;
+				status = 3;
+				CARDbRadioPowerOff(pDevice);
+				pMgmt->sNodeDBTable[0].bActive = false;
+				pMgmt->eCurrMode = WMAC_MODE_STANDBY;
+				pMgmt->eCurrState = WMAC_STATE_IDLE;
+				//netif_stop_queue(pDevice->dev);
+				pDevice->bLinkPass = false;
+
+			}
+			if (!(pDevice->byGPIO & GPIO0_DATA))
+//||(!(pDevice->byGPIO & GPIO0_DATA) && (pDevice->byRadioCtl & EEP_RADIOCTL_INV)))
+			{
+				if (status == 4) goto start;
+				status = 4;
+				CARDbRadioPowerOn(pDevice);
+			}
+		}
+	}
 start:
 #endif
 
-
-    if (pDevice->wUseProtectCntDown > 0) {
-        pDevice->wUseProtectCntDown --;
-    }
-    else {
-        // disable protect mode
-        pDevice->byERPFlag &= ~(WLAN_SET_ERP_USE_PROTECTION(1));
-    }
-
-{
-       pDevice->byReAssocCount++;
-   if((pDevice->byReAssocCount > 10) && (pDevice->bLinkPass != true)) {  //10 sec timeout
-                     printk("Re-association timeout!!!\n");
-		   pDevice->byReAssocCount = 0;
-                     #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-                    // if(pDevice->bWPASuppWextEnabled == true)
-                        {
-                  	union iwreq_data  wrqu;
-                  	memset(&wrqu, 0, sizeof (wrqu));
-                          wrqu.ap_addr.sa_family = ARPHRD_ETHER;
-                  	PRINT_K("wireless_send_event--->SIOCGIWAP(disassociated)\n");
-                  	wireless_send_event(pDevice->dev, SIOCGIWAP, &wrqu, NULL);
-                       }
-                    #endif
-     }
-   else if(pDevice->bLinkPass == true)
-   	pDevice->byReAssocCount = 0;
-}
+	if (pDevice->wUseProtectCntDown > 0) {
+		pDevice->wUseProtectCntDown--;
+	} else {
+		// disable protect mode
+		pDevice->byERPFlag &= ~(WLAN_SET_ERP_USE_PROTECTION(1));
+	}
+
+	{
+		pDevice->byReAssocCount++;
+		if ((pDevice->byReAssocCount > 10) && (pDevice->bLinkPass != true)) {  //10 sec timeout
+			printk("Re-association timeout!!!\n");
+			pDevice->byReAssocCount = 0;
+#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
+			{
+				union iwreq_data  wrqu;
+				memset(&wrqu, 0, sizeof(wrqu));
+				wrqu.ap_addr.sa_family = ARPHRD_ETHER;
+				PRINT_K("wireless_send_event--->SIOCGIWAP(disassociated)\n");
+				wireless_send_event(pDevice->dev, SIOCGIWAP, &wrqu, NULL);
+			}
+#endif
+		} else if (pDevice->bLinkPass == true)
+			pDevice->byReAssocCount = 0;
+	}
 
 #ifdef Calcu_LinkQual
-   s_uCalculateLinkQual((void *)pDevice);
+	s_uCalculateLinkQual((void *)pDevice);
 #endif
 
-    for (ii = 0; ii < (MAX_NODE_NUM + 1); ii++) {
-
-        if (pMgmt->sNodeDBTable[ii].bActive) {
-            // Increase in-activity counter
-            pMgmt->sNodeDBTable[ii].uInActiveCount++;
-
-            if (ii > 0) {
-                if (pMgmt->sNodeDBTable[ii].uInActiveCount > MAX_INACTIVE_COUNT) {
-                    BSSvRemoveOneNode(pDevice, ii);
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-                        "Inactive timeout [%d] sec, STA index = [%d] remove\n", MAX_INACTIVE_COUNT, ii);
-                    continue;
-                }
-
-                if (pMgmt->sNodeDBTable[ii].eNodeState >= NODE_ASSOC) {
-
-                    pDevice->uAssocCount++;
-
-                    // check if Non ERP exist
-                    if (pMgmt->sNodeDBTable[ii].uInActiveCount < ERP_RECOVER_COUNT) {
-                        if (!pMgmt->sNodeDBTable[ii].bShortPreamble) {
-                            pDevice->byERPFlag |= WLAN_SET_ERP_BARKER_MODE(1);
-                            uLongPreambleSTACnt ++;
-                        }
-                        if (!pMgmt->sNodeDBTable[ii].bERPExist) {
-                            pDevice->byERPFlag |= WLAN_SET_ERP_NONERP_PRESENT(1);
-                            pDevice->byERPFlag |= WLAN_SET_ERP_USE_PROTECTION(1);
-                        }
-                        if (!pMgmt->sNodeDBTable[ii].bShortSlotTime)
-                            uNonShortSlotSTACnt++;
-                    }
-                }
+	for (ii = 0; ii < (MAX_NODE_NUM + 1); ii++) {
+		if (pMgmt->sNodeDBTable[ii].bActive) {
+			// Increase in-activity counter
+			pMgmt->sNodeDBTable[ii].uInActiveCount++;
+
+			if (ii > 0) {
+				if (pMgmt->sNodeDBTable[ii].uInActiveCount > MAX_INACTIVE_COUNT) {
+					BSSvRemoveOneNode(pDevice, ii);
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
+						"Inactive timeout [%d] sec, STA index = [%d] remove\n", MAX_INACTIVE_COUNT, ii);
+					continue;
+				}
 
-                // check if any STA in PS mode
-                if (pMgmt->sNodeDBTable[ii].bPSEnable)
-                    uSleepySTACnt++;
+				if (pMgmt->sNodeDBTable[ii].eNodeState >= NODE_ASSOC) {
+					pDevice->uAssocCount++;
+
+					// check if Non ERP exist
+					if (pMgmt->sNodeDBTable[ii].uInActiveCount < ERP_RECOVER_COUNT) {
+						if (!pMgmt->sNodeDBTable[ii].bShortPreamble) {
+							pDevice->byERPFlag |= WLAN_SET_ERP_BARKER_MODE(1);
+							uLongPreambleSTACnt++;
+						}
+						if (!pMgmt->sNodeDBTable[ii].bERPExist) {
+							pDevice->byERPFlag |= WLAN_SET_ERP_NONERP_PRESENT(1);
+							pDevice->byERPFlag |= WLAN_SET_ERP_USE_PROTECTION(1);
+						}
+						if (!pMgmt->sNodeDBTable[ii].bShortSlotTime)
+							uNonShortSlotSTACnt++;
+					}
+				}
 
+				// check if any STA in PS mode
+				if (pMgmt->sNodeDBTable[ii].bPSEnable)
+					uSleepySTACnt++;
 
-            }
+			}
 
-            // Rate fallback check
-            if (!pDevice->bFixRate) {
+			// Rate fallback check
+			if (!pDevice->bFixRate) {
 /*
-                if ((pMgmt->eCurrMode == WMAC_MODE_ESS_STA) && (ii == 0))
-                    RATEvTxRateFallBack(pDevice, &(pMgmt->sNodeDBTable[ii]));
+  if ((pMgmt->eCurrMode == WMAC_MODE_ESS_STA) && (ii == 0))
+  RATEvTxRateFallBack(pDevice, &(pMgmt->sNodeDBTable[ii]));
 */
-                if (ii > 0) {
-                    // ii = 0 for multicast node (AP & Adhoc)
-                    RATEvTxRateFallBack((void *)pDevice, &(pMgmt->sNodeDBTable[ii]));
-                }
-                else {
-                    // ii = 0 reserved for unicast AP node (Infra STA)
-                    if (pMgmt->eCurrMode == WMAC_MODE_ESS_STA)
+				if (ii > 0) {
+					// ii = 0 for multicast node (AP & Adhoc)
+					RATEvTxRateFallBack((void *)pDevice, &(pMgmt->sNodeDBTable[ii]));
+				} else {
+					// ii = 0 reserved for unicast AP node (Infra STA)
+					if (pMgmt->eCurrMode == WMAC_MODE_ESS_STA)
 #ifdef	PLICE_DEBUG
-		printk("SecondCallback:Before:TxDataRate is %d\n",pMgmt->sNodeDBTable[0].wTxDataRate);
+						printk("SecondCallback:Before:TxDataRate is %d\n", pMgmt->sNodeDBTable[0].wTxDataRate);
 #endif
-                        RATEvTxRateFallBack((void *)pDevice, &(pMgmt->sNodeDBTable[ii]));
+					RATEvTxRateFallBack((void *)pDevice, &(pMgmt->sNodeDBTable[ii]));
 #ifdef	PLICE_DEBUG
-		printk("SecondCallback:After:TxDataRate is %d\n",pMgmt->sNodeDBTable[0].wTxDataRate);
+					printk("SecondCallback:After:TxDataRate is %d\n", pMgmt->sNodeDBTable[0].wTxDataRate);
 #endif
 
+				}
+
+			}
+
+			// check if pending PS queue
+			if (pMgmt->sNodeDBTable[ii].wEnQueueCnt != 0) {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Index= %d, Queue = %d pending \n",
+					ii, pMgmt->sNodeDBTable[ii].wEnQueueCnt);
+				if ((ii > 0) && (pMgmt->sNodeDBTable[ii].wEnQueueCnt > 15)) {
+					BSSvRemoveOneNode(pDevice, ii);
+					DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Pending many queues PS STA Index = %d remove \n", ii);
+					continue;
+				}
+			}
 		}
 
-            }
-
-            // check if pending PS queue
-            if (pMgmt->sNodeDBTable[ii].wEnQueueCnt != 0) {
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Index= %d, Queue = %d pending \n",
-                           ii, pMgmt->sNodeDBTable[ii].wEnQueueCnt);
-                if ((ii >0) && (pMgmt->sNodeDBTable[ii].wEnQueueCnt > 15)) {
-                    BSSvRemoveOneNode(pDevice, ii);
-                    DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Pending many queues PS STA Index = %d remove \n", ii);
-                    continue;
-                }
-            }
-        }
-
-    }
-
-
-    if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) && (pDevice->eCurrentPHYType == PHY_TYPE_11G)) {
-
-        // on/off protect mode
-        if (WLAN_GET_ERP_USE_PROTECTION(pDevice->byERPFlag)) {
-            if (!pDevice->bProtectMode) {
-                MACvEnableProtectMD(pDevice->PortOffset);
-                pDevice->bProtectMode = true;
-            }
-        }
-        else {
-            if (pDevice->bProtectMode) {
-                MACvDisableProtectMD(pDevice->PortOffset);
-                pDevice->bProtectMode = false;
-            }
-        }
-        // on/off short slot time
-
-        if (uNonShortSlotSTACnt > 0) {
-            if (pDevice->bShortSlotTime) {
-                pDevice->bShortSlotTime = false;
-                BBvSetShortSlotTime(pDevice);
-                vUpdateIFS((void *)pDevice);
-            }
-        }
-        else {
-            if (!pDevice->bShortSlotTime) {
-                pDevice->bShortSlotTime = true;
-                BBvSetShortSlotTime(pDevice);
-                vUpdateIFS((void *)pDevice);
-            }
-        }
-
-        // on/off barker long preamble mode
-
-        if (uLongPreambleSTACnt > 0) {
-            if (!pDevice->bBarkerPreambleMd) {
-                MACvEnableBarkerPreambleMd(pDevice->PortOffset);
-                pDevice->bBarkerPreambleMd = true;
-            }
-        }
-        else {
-            if (pDevice->bBarkerPreambleMd) {
-                MACvDisableBarkerPreambleMd(pDevice->PortOffset);
-                pDevice->bBarkerPreambleMd = false;
-            }
-        }
-
-    }
-
-
-    // Check if any STA in PS mode, enable DTIM multicast deliver
-    if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-        if (uSleepySTACnt > 0)
-            pMgmt->sNodeDBTable[0].bPSEnable = true;
-        else
-            pMgmt->sNodeDBTable[0].bPSEnable = false;
-    }
-
-    pItemSSID = (PWLAN_IE_SSID)pMgmt->abyDesireSSID;
-    pCurrSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
-
-    if ((pMgmt->eCurrMode == WMAC_MODE_STANDBY) ||
-        (pMgmt->eCurrMode == WMAC_MODE_ESS_STA)) {
-
-        if (pMgmt->sNodeDBTable[0].bActive) { // Assoc with BSS
-           // DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Callback inactive Count = [%d]\n", pMgmt->sNodeDBTable[0].uInActiveCount);
-            //if (pDevice->bUpdateBBVGA) {
-            //  s_vCheckSensitivity((void *) pDevice);
-            //}
-
-            if (pDevice->bUpdateBBVGA) {
-               // s_vCheckSensitivity((void *) pDevice);
-               s_vCheckPreEDThreshold((void *)pDevice);
-            }
-
-    	    if ((pMgmt->sNodeDBTable[0].uInActiveCount >= (LOST_BEACON_COUNT/2)) &&
-    	        (pDevice->byBBVGACurrent != pDevice->abyBBVGA[0]) ) {
-    	        pDevice->byBBVGANew = pDevice->abyBBVGA[0];
-                bScheduleCommand((void *) pDevice, WLAN_CMD_CHANGE_BBSENSITIVITY, NULL);
-    	    }
-
-        	if (pMgmt->sNodeDBTable[0].uInActiveCount >= LOST_BEACON_COUNT) {
-                pMgmt->sNodeDBTable[0].bActive = false;
-                pMgmt->eCurrMode = WMAC_MODE_STANDBY;
-                pMgmt->eCurrState = WMAC_STATE_IDLE;
-                netif_stop_queue(pDevice->dev);
-                pDevice->bLinkPass = false;
-                pDevice->bRoaming = true;
-                DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Lost AP beacon [%d] sec, disconnected !\n", pMgmt->sNodeDBTable[0].uInActiveCount);
-        if ((pDevice->bWPADEVUp) && (pDevice->skb != NULL)) {
-             wpahdr = (viawget_wpa_header *)pDevice->skb->data;
-             wpahdr->type = VIAWGET_DISASSOC_MSG;
-             wpahdr->resp_ie_len = 0;
-             wpahdr->req_ie_len = 0;
-             skb_put(pDevice->skb, sizeof(viawget_wpa_header));
-             pDevice->skb->dev = pDevice->wpadev;
-	     skb_reset_mac_header(pDevice->skb);
-             pDevice->skb->pkt_type = PACKET_HOST;
-             pDevice->skb->protocol = htons(ETH_P_802_2);
-             memset(pDevice->skb->cb, 0, sizeof(pDevice->skb->cb));
-             netif_rx(pDevice->skb);
-             pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
-         }
-   #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-  // if(pDevice->bWPASuppWextEnabled == true)
-      {
-	union iwreq_data  wrqu;
-	memset(&wrqu, 0, sizeof (wrqu));
-        wrqu.ap_addr.sa_family = ARPHRD_ETHER;
-	PRINT_K("wireless_send_event--->SIOCGIWAP(disassociated)\n");
-	wireless_send_event(pDevice->dev, SIOCGIWAP, &wrqu, NULL);
-     }
-  #endif
-	    }
-        }
-        else if (pItemSSID->len != 0) {
-            if (pDevice->uAutoReConnectTime < 10) {
-                pDevice->uAutoReConnectTime++;
-	       #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-                //network manager support need not do Roaming scan???
-                if(pDevice->bWPASuppWextEnabled ==true)
-		 pDevice->uAutoReConnectTime = 0;
-	     #endif
-            }
-            else {
-	   //mike use old encryption status for wpa reauthen
-	      if(pDevice->bWPADEVUp)
-	          pDevice->eEncryptionStatus = pDevice->eOldEncryptionStatus;
-
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Roaming ...\n");
-                BSSvClearBSSList((void *)pDevice, pDevice->bLinkPass);
- 	      pMgmt->eScanType = WMAC_SCAN_ACTIVE;
-                bScheduleCommand((void *) pDevice, WLAN_CMD_BSSID_SCAN, pMgmt->abyDesireSSID);
-                bScheduleCommand((void *) pDevice, WLAN_CMD_SSID, pMgmt->abyDesireSSID);
-                pDevice->uAutoReConnectTime = 0;
-            }
-        }
-    }
-
-    if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
-        // if adhoc started which essid is NULL string, rescanning.
-        if ((pMgmt->eCurrState == WMAC_STATE_STARTED) && (pCurrSSID->len == 0)) {
-            if (pDevice->uAutoReConnectTime < 10) {
-                pDevice->uAutoReConnectTime++;
-            }
-            else {
-                DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Adhoc re-scanning ...\n");
-	      pMgmt->eScanType = WMAC_SCAN_ACTIVE;
-                bScheduleCommand((void *) pDevice, WLAN_CMD_BSSID_SCAN, NULL);
-                bScheduleCommand((void *) pDevice, WLAN_CMD_SSID, NULL);
-                pDevice->uAutoReConnectTime = 0;
-            };
-        }
-        if (pMgmt->eCurrState == WMAC_STATE_JOINTED) {
-
-            if (pDevice->bUpdateBBVGA) {
-               //s_vCheckSensitivity((void *) pDevice);
-               s_vCheckPreEDThreshold((void *)pDevice);
-            }
-        	if (pMgmt->sNodeDBTable[0].uInActiveCount >=ADHOC_LOST_BEACON_COUNT) {
-        	    DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Lost other STA beacon [%d] sec, started !\n", pMgmt->sNodeDBTable[0].uInActiveCount);
-                pMgmt->sNodeDBTable[0].uInActiveCount = 0;
-                pMgmt->eCurrState = WMAC_STATE_STARTED;
-                netif_stop_queue(pDevice->dev);
-                pDevice->bLinkPass = false;
-            }
-        }
-    }
-
-    spin_unlock_irq(&pDevice->lock);
-
-    pMgmt->sTimerSecondCallback.expires = RUN_AT(HZ);
-    add_timer(&pMgmt->sTimerSecondCallback);
-    return;
-}
+	}
+
+	if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) && (pDevice->eCurrentPHYType == PHY_TYPE_11G)) {
+		// on/off protect mode
+		if (WLAN_GET_ERP_USE_PROTECTION(pDevice->byERPFlag)) {
+			if (!pDevice->bProtectMode) {
+				MACvEnableProtectMD(pDevice->PortOffset);
+				pDevice->bProtectMode = true;
+			}
+		} else {
+			if (pDevice->bProtectMode) {
+				MACvDisableProtectMD(pDevice->PortOffset);
+				pDevice->bProtectMode = false;
+			}
+		}
+		// on/off short slot time
+
+		if (uNonShortSlotSTACnt > 0) {
+			if (pDevice->bShortSlotTime) {
+				pDevice->bShortSlotTime = false;
+				BBvSetShortSlotTime(pDevice);
+				vUpdateIFS((void *)pDevice);
+			}
+		} else {
+			if (!pDevice->bShortSlotTime) {
+				pDevice->bShortSlotTime = true;
+				BBvSetShortSlotTime(pDevice);
+				vUpdateIFS((void *)pDevice);
+			}
+		}
+
+		// on/off barker long preamble mode
+
+		if (uLongPreambleSTACnt > 0) {
+			if (!pDevice->bBarkerPreambleMd) {
+				MACvEnableBarkerPreambleMd(pDevice->PortOffset);
+				pDevice->bBarkerPreambleMd = true;
+			}
+		} else {
+			if (pDevice->bBarkerPreambleMd) {
+				MACvDisableBarkerPreambleMd(pDevice->PortOffset);
+				pDevice->bBarkerPreambleMd = false;
+			}
+		}
 
+	}
+
+	// Check if any STA in PS mode, enable DTIM multicast deliver
+	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
+		if (uSleepySTACnt > 0)
+			pMgmt->sNodeDBTable[0].bPSEnable = true;
+		else
+			pMgmt->sNodeDBTable[0].bPSEnable = false;
+	}
+
+	pItemSSID = (PWLAN_IE_SSID)pMgmt->abyDesireSSID;
+	pCurrSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
+
+	if ((pMgmt->eCurrMode == WMAC_MODE_STANDBY) ||
+	    (pMgmt->eCurrMode == WMAC_MODE_ESS_STA)) {
+		if (pMgmt->sNodeDBTable[0].bActive) { // Assoc with BSS
+			if (pDevice->bUpdateBBVGA) {
+				// s_vCheckSensitivity((void *) pDevice);
+				s_vCheckPreEDThreshold((void *)pDevice);
+			}
+
+			if ((pMgmt->sNodeDBTable[0].uInActiveCount >= (LOST_BEACON_COUNT/2)) &&
+			    (pDevice->byBBVGACurrent != pDevice->abyBBVGA[0])) {
+				pDevice->byBBVGANew = pDevice->abyBBVGA[0];
+				bScheduleCommand((void *)pDevice, WLAN_CMD_CHANGE_BBSENSITIVITY, NULL);
+			}
+
+			if (pMgmt->sNodeDBTable[0].uInActiveCount >= LOST_BEACON_COUNT) {
+				pMgmt->sNodeDBTable[0].bActive = false;
+				pMgmt->eCurrMode = WMAC_MODE_STANDBY;
+				pMgmt->eCurrState = WMAC_STATE_IDLE;
+				netif_stop_queue(pDevice->dev);
+				pDevice->bLinkPass = false;
+				pDevice->bRoaming = true;
+				DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Lost AP beacon [%d] sec, disconnected !\n", pMgmt->sNodeDBTable[0].uInActiveCount);
+				if ((pDevice->bWPADEVUp) && (pDevice->skb != NULL)) {
+					wpahdr = (viawget_wpa_header *)pDevice->skb->data;
+					wpahdr->type = VIAWGET_DISASSOC_MSG;
+					wpahdr->resp_ie_len = 0;
+					wpahdr->req_ie_len = 0;
+					skb_put(pDevice->skb, sizeof(viawget_wpa_header));
+					pDevice->skb->dev = pDevice->wpadev;
+					skb_reset_mac_header(pDevice->skb);
+					pDevice->skb->pkt_type = PACKET_HOST;
+					pDevice->skb->protocol = htons(ETH_P_802_2);
+					memset(pDevice->skb->cb, 0, sizeof(pDevice->skb->cb));
+					netif_rx(pDevice->skb);
+					pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
+				}
+#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
+				{
+					union iwreq_data  wrqu;
+					memset(&wrqu, 0, sizeof(wrqu));
+					wrqu.ap_addr.sa_family = ARPHRD_ETHER;
+					PRINT_K("wireless_send_event--->SIOCGIWAP(disassociated)\n");
+					wireless_send_event(pDevice->dev, SIOCGIWAP, &wrqu, NULL);
+				}
+#endif
+			}
+		} else if (pItemSSID->len != 0) {
+			if (pDevice->uAutoReConnectTime < 10) {
+				pDevice->uAutoReConnectTime++;
+#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
+				//network manager support need not do Roaming scan???
+				if (pDevice->bWPASuppWextEnabled == true)
+					pDevice->uAutoReConnectTime = 0;
+#endif
+			} else {
+				//mike use old encryption status for wpa reauthen
+				if (pDevice->bWPADEVUp)
+					pDevice->eEncryptionStatus = pDevice->eOldEncryptionStatus;
+
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Roaming ...\n");
+				BSSvClearBSSList((void *)pDevice, pDevice->bLinkPass);
+				pMgmt->eScanType = WMAC_SCAN_ACTIVE;
+				bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, pMgmt->abyDesireSSID);
+				bScheduleCommand((void *)pDevice, WLAN_CMD_SSID, pMgmt->abyDesireSSID);
+				pDevice->uAutoReConnectTime = 0;
+			}
+		}
+	}
+
+	if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
+		// if adhoc started which essid is NULL string, rescanning.
+		if ((pMgmt->eCurrState == WMAC_STATE_STARTED) && (pCurrSSID->len == 0)) {
+			if (pDevice->uAutoReConnectTime < 10) {
+				pDevice->uAutoReConnectTime++;
+			} else {
+				DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Adhoc re-scanning ...\n");
+				pMgmt->eScanType = WMAC_SCAN_ACTIVE;
+				bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, NULL);
+				bScheduleCommand((void *)pDevice, WLAN_CMD_SSID, NULL);
+				pDevice->uAutoReConnectTime = 0;
+			};
+		}
+		if (pMgmt->eCurrState == WMAC_STATE_JOINTED) {
+			if (pDevice->bUpdateBBVGA) {
+				//s_vCheckSensitivity((void *) pDevice);
+				s_vCheckPreEDThreshold((void *)pDevice);
+			}
+			if (pMgmt->sNodeDBTable[0].uInActiveCount >= ADHOC_LOST_BEACON_COUNT) {
+				DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Lost other STA beacon [%d] sec, started !\n", pMgmt->sNodeDBTable[0].uInActiveCount);
+				pMgmt->sNodeDBTable[0].uInActiveCount = 0;
+				pMgmt->eCurrState = WMAC_STATE_STARTED;
+				netif_stop_queue(pDevice->dev);
+				pDevice->bLinkPass = false;
+			}
+		}
+	}
 
+	spin_unlock_irq(&pDevice->lock);
 
+	pMgmt->sTimerSecondCallback.expires = RUN_AT(HZ);
+	add_timer(&pMgmt->sTimerSecondCallback);
+	return;
+}
 
 /*+
  *
@@ -1375,184 +1294,162 @@ start:
  * Return Value:
  *    none.
  *
--*/
-
-
+ -*/
 
 void
 BSSvUpdateNodeTxCounter(
-    void *hDeviceContext,
-    unsigned char byTsr0,
-    unsigned char byTsr1,
-    unsigned char *pbyBuffer,
-    unsigned int uFIFOHeaderSize
-    )
+	void *hDeviceContext,
+	unsigned char byTsr0,
+	unsigned char byTsr1,
+	unsigned char *pbyBuffer,
+	unsigned int uFIFOHeaderSize
+)
 {
-    PSDevice        pDevice = (PSDevice)hDeviceContext;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    unsigned int uNodeIndex = 0;
-    unsigned char byTxRetry = (byTsr0 & TSR0_NCR);
-    PSTxBufHead     pTxBufHead;
-    PS802_11Header  pMACHeader;
-    unsigned short wRate;
-    unsigned short wFallBackRate = RATE_1M;
-    unsigned char byFallBack;
-    unsigned int ii;
+	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	unsigned int uNodeIndex = 0;
+	unsigned char byTxRetry = (byTsr0 & TSR0_NCR);
+	PSTxBufHead     pTxBufHead;
+	PS802_11Header  pMACHeader;
+	unsigned short wRate;
+	unsigned short wFallBackRate = RATE_1M;
+	unsigned char byFallBack;
+	unsigned int ii;
 //	unsigned int txRetryTemp;
 //PLICE_DEBUG->
 	//txRetryTemp = byTxRetry;
-	//if (txRetryTemp== 8)
-	//txRetryTemp -=3;
 //PLICE_DEBUG <-
-    pTxBufHead = (PSTxBufHead) pbyBuffer;
-    if (pTxBufHead->wFIFOCtl & FIFOCTL_AUTO_FB_0) {
-        byFallBack = AUTO_FB_0;
-    } else if (pTxBufHead->wFIFOCtl & FIFOCTL_AUTO_FB_1) {
-        byFallBack = AUTO_FB_1;
-    } else {
-        byFallBack = AUTO_FB_NONE;
-    }
-    wRate = pTxBufHead->wReserved; //?wRate
-    //printk("BSSvUpdateNodeTxCounter:byTxRetry is %d\n",byTxRetry);
-
-//printk("BSSvUpdateNodeTx:wRate is %d,byFallback is %d\n",wRate,byFallBack);
-//#ifdef	PLICE_DEBUG
-	//printk("BSSvUpdateNodeTx: wRate is %d\n",wRate);
-////#endif
-    // Only Unicast using support rates
-    if (pTxBufHead->wFIFOCtl & FIFOCTL_NEEDACK) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"wRate %04X, byTsr0 %02X, byTsr1 %02X\n", wRate, byTsr0, byTsr1);
-        if (pMgmt->eCurrMode == WMAC_MODE_ESS_STA) {
-            pMgmt->sNodeDBTable[0].uTxAttempts += 1;
-            if ((byTsr1 & TSR1_TERR) == 0) {
-                // transmit success, TxAttempts at least plus one
-                pMgmt->sNodeDBTable[0].uTxOk[MAX_RATE]++;
-                if ( (byFallBack == AUTO_FB_NONE) ||
-                     (wRate < RATE_18M) ) {
-                    wFallBackRate = wRate;
-                } else if (byFallBack == AUTO_FB_0) {
+	pTxBufHead = (PSTxBufHead) pbyBuffer;
+	if (pTxBufHead->wFIFOCtl & FIFOCTL_AUTO_FB_0) {
+		byFallBack = AUTO_FB_0;
+	} else if (pTxBufHead->wFIFOCtl & FIFOCTL_AUTO_FB_1) {
+		byFallBack = AUTO_FB_1;
+	} else {
+		byFallBack = AUTO_FB_NONE;
+	}
+	wRate = pTxBufHead->wReserved; //?wRate
+
+	// Only Unicast using support rates
+	if (pTxBufHead->wFIFOCtl & FIFOCTL_NEEDACK) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wRate %04X, byTsr0 %02X, byTsr1 %02X\n", wRate, byTsr0, byTsr1);
+		if (pMgmt->eCurrMode == WMAC_MODE_ESS_STA) {
+			pMgmt->sNodeDBTable[0].uTxAttempts += 1;
+			if ((byTsr1 & TSR1_TERR) == 0) {
+				// transmit success, TxAttempts at least plus one
+				pMgmt->sNodeDBTable[0].uTxOk[MAX_RATE]++;
+				if ((byFallBack == AUTO_FB_NONE) ||
+				    (wRate < RATE_18M)) {
+					wFallBackRate = wRate;
+				} else if (byFallBack == AUTO_FB_0) {
 //PLICE_DEBUG
-				  if (byTxRetry < 5)
-				//if (txRetryTemp < 5)
-					wFallBackRate = awHWRetry0[wRate-RATE_18M][byTxRetry];
-			//wFallBackRate = awHWRetry0[wRate-RATE_12M][byTxRetry];
-			//wFallBackRate = awHWRetry0[wRate-RATE_18M][txRetryTemp] +1;
-		else
-                        wFallBackRate = awHWRetry0[wRate-RATE_18M][4];
-			//wFallBackRate = awHWRetry0[wRate-RATE_12M][4];
-		} else if (byFallBack == AUTO_FB_1) {
-                    if (byTxRetry < 5)
-                        wFallBackRate = awHWRetry1[wRate-RATE_18M][byTxRetry];
-                    else
-                        wFallBackRate = awHWRetry1[wRate-RATE_18M][4];
-                }
-                pMgmt->sNodeDBTable[0].uTxOk[wFallBackRate]++;
-            } else {
-                pMgmt->sNodeDBTable[0].uTxFailures ++;
-            }
-            pMgmt->sNodeDBTable[0].uTxRetry += byTxRetry;
-            if (byTxRetry != 0) {
-                pMgmt->sNodeDBTable[0].uTxFail[MAX_RATE]+=byTxRetry;
-                if ( (byFallBack == AUTO_FB_NONE) ||
-                     (wRate < RATE_18M) ) {
-                    pMgmt->sNodeDBTable[0].uTxFail[wRate]+=byTxRetry;
-                } else if (byFallBack == AUTO_FB_0) {
+					if (byTxRetry < 5)
+						wFallBackRate = awHWRetry0[wRate-RATE_18M][byTxRetry];
+					//wFallBackRate = awHWRetry0[wRate-RATE_12M][byTxRetry];
+					//wFallBackRate = awHWRetry0[wRate-RATE_18M][txRetryTemp] +1;
+					else
+						wFallBackRate = awHWRetry0[wRate-RATE_18M][4];
+					//wFallBackRate = awHWRetry0[wRate-RATE_12M][4];
+				} else if (byFallBack == AUTO_FB_1) {
+					if (byTxRetry < 5)
+						wFallBackRate = awHWRetry1[wRate-RATE_18M][byTxRetry];
+					else
+						wFallBackRate = awHWRetry1[wRate-RATE_18M][4];
+				}
+				pMgmt->sNodeDBTable[0].uTxOk[wFallBackRate]++;
+			} else {
+				pMgmt->sNodeDBTable[0].uTxFailures++;
+			}
+			pMgmt->sNodeDBTable[0].uTxRetry += byTxRetry;
+			if (byTxRetry != 0) {
+				pMgmt->sNodeDBTable[0].uTxFail[MAX_RATE] += byTxRetry;
+				if ((byFallBack == AUTO_FB_NONE) ||
+				    (wRate < RATE_18M)) {
+					pMgmt->sNodeDBTable[0].uTxFail[wRate] += byTxRetry;
+				} else if (byFallBack == AUTO_FB_0) {
 //PLICE_DEBUG
-				   for(ii=0;ii<byTxRetry;ii++)
-		//for (ii=0;ii<txRetryTemp;ii++)
-		{
-                        if (ii < 5)
-                        	{
-
+					for (ii = 0; ii < byTxRetry; ii++)
+						//for (ii=0;ii<txRetryTemp;ii++)
+					{
+						if (ii < 5) {
 //PLICE_DEBUG
-						wFallBackRate = awHWRetry0[wRate-RATE_18M][ii];
-					//printk(" II is %d:BSSvUpdateNodeTx:wFallBackRate is %d\n",ii,wFallBackRate);
-				//wFallBackRate = awHWRetry0[wRate-RATE_12M][ii];
-                        	}
-			else
-				{
-			wFallBackRate = awHWRetry0[wRate-RATE_18M][4];
-			//printk("ii is %d BSSvUpdateNodeTx:wFallBackRate is %d\n",ii,wFallBackRate);
-				//wFallBackRate = awHWRetry0[wRate-RATE_12M][4];
-				}
+							wFallBackRate = awHWRetry0[wRate-RATE_18M][ii];
+							//wFallBackRate = awHWRetry0[wRate-RATE_12M][ii];
+						} else {
+							wFallBackRate = awHWRetry0[wRate-RATE_18M][4];
+							//wFallBackRate = awHWRetry0[wRate-RATE_12M][4];
+						}
 						pMgmt->sNodeDBTable[0].uTxFail[wFallBackRate]++;
-                    }
-                } else if (byFallBack == AUTO_FB_1) {
-                    for(ii=0;ii<byTxRetry;ii++) {
-                        if (ii < 5)
-                            wFallBackRate = awHWRetry1[wRate-RATE_18M][ii];
-                        else
-                            wFallBackRate = awHWRetry1[wRate-RATE_18M][4];
-                        pMgmt->sNodeDBTable[0].uTxFail[wFallBackRate]++;
-                    }
-                }
-            }
-        }
-
-        if ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) ||
-            (pMgmt->eCurrMode == WMAC_MODE_ESS_AP)) {
-
-            pMACHeader = (PS802_11Header)(pbyBuffer + uFIFOHeaderSize);
-
-            if (BSSDBbIsSTAInNodeDB((void *)pMgmt, &(pMACHeader->abyAddr1[0]), &uNodeIndex)){
-                pMgmt->sNodeDBTable[uNodeIndex].uTxAttempts += 1;
-                if ((byTsr1 & TSR1_TERR) == 0) {
-                    // transmit success, TxAttempts at least plus one
-                    pMgmt->sNodeDBTable[uNodeIndex].uTxOk[MAX_RATE]++;
-                    if ( (byFallBack == AUTO_FB_NONE) ||
-                         (wRate < RATE_18M) ) {
-                        wFallBackRate = wRate;
-                    } else if (byFallBack == AUTO_FB_0) {
-                        if (byTxRetry < 5)
-                            wFallBackRate = awHWRetry0[wRate-RATE_18M][byTxRetry];
-                        else
-                            wFallBackRate = awHWRetry0[wRate-RATE_18M][4];
-                    } else if (byFallBack == AUTO_FB_1) {
-                        if (byTxRetry < 5)
-                            wFallBackRate = awHWRetry1[wRate-RATE_18M][byTxRetry];
-                        else
-                            wFallBackRate = awHWRetry1[wRate-RATE_18M][4];
-                    }
-                    pMgmt->sNodeDBTable[uNodeIndex].uTxOk[wFallBackRate]++;
-                } else {
-                    pMgmt->sNodeDBTable[uNodeIndex].uTxFailures ++;
-                }
-                pMgmt->sNodeDBTable[uNodeIndex].uTxRetry += byTxRetry;
-                if (byTxRetry != 0) {
-                    pMgmt->sNodeDBTable[uNodeIndex].uTxFail[MAX_RATE]+=byTxRetry;
-                    if ( (byFallBack == AUTO_FB_NONE) ||
-                         (wRate < RATE_18M) ) {
-                        pMgmt->sNodeDBTable[uNodeIndex].uTxFail[wRate]+=byTxRetry;
-                    } else if (byFallBack == AUTO_FB_0) {
-                        for(ii=0;ii<byTxRetry;ii++) {
-                            if (ii < 5)
-                                wFallBackRate = awHWRetry0[wRate-RATE_18M][ii];
-                            else
-                                wFallBackRate = awHWRetry0[wRate-RATE_18M][4];
-                            pMgmt->sNodeDBTable[uNodeIndex].uTxFail[wFallBackRate]++;
-                        }
-                    } else if (byFallBack == AUTO_FB_1) {
-                        for(ii=0;ii<byTxRetry;ii++) {
-                            if (ii < 5)
-                                wFallBackRate = awHWRetry1[wRate-RATE_18M][ii];
-                            else
-                                wFallBackRate = awHWRetry1[wRate-RATE_18M][4];
-                            pMgmt->sNodeDBTable[uNodeIndex].uTxFail[wFallBackRate]++;
-                        }
-                    }
-                }
-            }
-        }
-    }
-
-    return;
+					}
+				} else if (byFallBack == AUTO_FB_1) {
+					for (ii = 0; ii < byTxRetry; ii++) {
+						if (ii < 5)
+							wFallBackRate = awHWRetry1[wRate-RATE_18M][ii];
+						else
+							wFallBackRate = awHWRetry1[wRate-RATE_18M][4];
+						pMgmt->sNodeDBTable[0].uTxFail[wFallBackRate]++;
+					}
+				}
+			}
+		}
 
+		if ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) ||
+		    (pMgmt->eCurrMode == WMAC_MODE_ESS_AP)) {
+			pMACHeader = (PS802_11Header)(pbyBuffer + uFIFOHeaderSize);
+
+			if (BSSDBbIsSTAInNodeDB((void *)pMgmt,  &(pMACHeader->abyAddr1[0]), &uNodeIndex)) {
+				pMgmt->sNodeDBTable[uNodeIndex].uTxAttempts += 1;
+				if ((byTsr1 & TSR1_TERR) == 0) {
+					// transmit success, TxAttempts at least plus one
+					pMgmt->sNodeDBTable[uNodeIndex].uTxOk[MAX_RATE]++;
+					if ((byFallBack == AUTO_FB_NONE) ||
+					    (wRate < RATE_18M)) {
+						wFallBackRate = wRate;
+					} else if (byFallBack == AUTO_FB_0) {
+						if (byTxRetry < 5)
+							wFallBackRate = awHWRetry0[wRate-RATE_18M][byTxRetry];
+						else
+							wFallBackRate = awHWRetry0[wRate-RATE_18M][4];
+					} else if (byFallBack == AUTO_FB_1) {
+						if (byTxRetry < 5)
+							wFallBackRate = awHWRetry1[wRate-RATE_18M][byTxRetry];
+						else
+							wFallBackRate = awHWRetry1[wRate-RATE_18M][4];
+					}
+					pMgmt->sNodeDBTable[uNodeIndex].uTxOk[wFallBackRate]++;
+				} else {
+					pMgmt->sNodeDBTable[uNodeIndex].uTxFailures++;
+				}
+				pMgmt->sNodeDBTable[uNodeIndex].uTxRetry += byTxRetry;
+				if (byTxRetry != 0) {
+					pMgmt->sNodeDBTable[uNodeIndex].uTxFail[MAX_RATE] += byTxRetry;
+					if ((byFallBack == AUTO_FB_NONE) ||
+					    (wRate < RATE_18M)) {
+						pMgmt->sNodeDBTable[uNodeIndex].uTxFail[wRate] += byTxRetry;
+					} else if (byFallBack == AUTO_FB_0) {
+						for (ii = 0; ii < byTxRetry; ii++) {
+							if (ii < 5)
+								wFallBackRate = awHWRetry0[wRate - RATE_18M][ii];
+							else
+								wFallBackRate = awHWRetry0[wRate - RATE_18M][4];
+							pMgmt->sNodeDBTable[uNodeIndex].uTxFail[wFallBackRate]++;
+						}
+					} else if (byFallBack == AUTO_FB_1) {
+						for (ii = 0; ii < byTxRetry; ii++) {
+							if (ii < 5)
+								wFallBackRate = awHWRetry1[wRate-RATE_18M][ii];
+							else
+								wFallBackRate = awHWRetry1[wRate-RATE_18M][4];
+							pMgmt->sNodeDBTable[uNodeIndex].uTxFail[wFallBackRate]++;
+						}
+					}
+				}
+			}
+		}
+	}
 
+	return;
 }
 
-
-
-
 /*+
  *
  * Routine Description:
@@ -1569,167 +1466,157 @@ BSSvUpdateNodeTxCounter(
  * Return Value:
  *    None.
  *
--*/
-
+ -*/
 
 void
 BSSvClearNodeDBTable(
-    void *hDeviceContext,
-    unsigned int uStartIndex
-    )
+	void *hDeviceContext,
+	unsigned int uStartIndex
+)
 
 {
-    PSDevice     pDevice = (PSDevice)hDeviceContext;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    struct sk_buff  *skb;
-    unsigned int ii;
-
-    for (ii = uStartIndex; ii < (MAX_NODE_NUM + 1); ii++) {
-        if (pMgmt->sNodeDBTable[ii].bActive) {
-            // check if sTxPSQueue has been initial
-            if (pMgmt->sNodeDBTable[ii].sTxPSQueue.next != NULL) {
-                while ((skb = skb_dequeue(&pMgmt->sNodeDBTable[ii].sTxPSQueue)) != NULL){
-                        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "PS skb != NULL %d\n", ii);
-                        dev_kfree_skb(skb);
-                }
-            }
-            memset(&pMgmt->sNodeDBTable[ii], 0, sizeof(KnownNodeDB));
-        }
-    }
-
-    return;
-};
+	PSDevice     pDevice = (PSDevice)hDeviceContext;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	struct sk_buff  *skb;
+	unsigned int ii;
+
+	for (ii = uStartIndex; ii < (MAX_NODE_NUM + 1); ii++) {
+		if (pMgmt->sNodeDBTable[ii].bActive) {
+			// check if sTxPSQueue has been initial
+			if (pMgmt->sNodeDBTable[ii].sTxPSQueue.next != NULL) {
+				while ((skb = skb_dequeue(&pMgmt->sNodeDBTable[ii].sTxPSQueue)) != NULL) {
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "PS skb != NULL %d\n", ii);
+					dev_kfree_skb(skb);
+				}
+			}
+			memset(&pMgmt->sNodeDBTable[ii], 0, sizeof(KnownNodeDB));
+		}
+	}
 
+	return;
+};
 
 void s_vCheckSensitivity(
-    void *hDeviceContext
-    )
+	void *hDeviceContext
+)
 {
-    PSDevice        pDevice = (PSDevice)hDeviceContext;
-    PKnownBSS       pBSSList = NULL;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    int             ii;
-
-    if ((pDevice->byLocalID <= REV_ID_VT3253_A1) && (pDevice->byRFType == RF_RFMD2959) &&
-        (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA)) {
-        return;
-    }
-
-    if ((pMgmt->eCurrState == WMAC_STATE_ASSOC) ||
-        ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) && (pMgmt->eCurrState == WMAC_STATE_JOINTED))) {
-        pBSSList = BSSpAddrIsInBSSList(pDevice, pMgmt->abyCurrBSSID, (PWLAN_IE_SSID)pMgmt->abyCurrSSID);
-        if (pBSSList != NULL) {
-            // Updata BB Reg if RSSI is too strong.
-            long    LocalldBmAverage = 0;
-            long    uNumofdBm = 0;
-            for (ii = 0; ii < RSSI_STAT_COUNT; ii++) {
-                if (pBSSList->ldBmAverage[ii] != 0) {
-                    uNumofdBm ++;
-                    LocalldBmAverage += pBSSList->ldBmAverage[ii];
-                }
-            }
-            if (uNumofdBm > 0) {
-                LocalldBmAverage = LocalldBmAverage/uNumofdBm;
-                for (ii=0;ii<BB_VGA_LEVEL;ii++) {
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"LocalldBmAverage:%ld, %ld %02x\n", LocalldBmAverage, pDevice->ldBmThreshold[ii], pDevice->abyBBVGA[ii]);
-                    if (LocalldBmAverage < pDevice->ldBmThreshold[ii]) {
-                	    pDevice->byBBVGANew = pDevice->abyBBVGA[ii];
-                        break;
-                    }
-                }
-                if (pDevice->byBBVGANew != pDevice->byBBVGACurrent) {
-                    pDevice->uBBVGADiffCount++;
-                    if (pDevice->uBBVGADiffCount >= BB_VGA_CHANGE_THRESHOLD)
-                        bScheduleCommand((void *) pDevice, WLAN_CMD_CHANGE_BBSENSITIVITY, NULL);
-                } else {
-                    pDevice->uBBVGADiffCount = 0;
-                }
-            }
-        }
-    }
+	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	PKnownBSS       pBSSList = NULL;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	int             ii;
+
+	if ((pDevice->byLocalID <= REV_ID_VT3253_A1) && (pDevice->byRFType == RF_RFMD2959) &&
+	    (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA)) {
+		return;
+	}
+
+	if ((pMgmt->eCurrState == WMAC_STATE_ASSOC) ||
+	    ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) && (pMgmt->eCurrState == WMAC_STATE_JOINTED))) {
+		pBSSList = BSSpAddrIsInBSSList(pDevice, pMgmt->abyCurrBSSID, (PWLAN_IE_SSID)pMgmt->abyCurrSSID);
+		if (pBSSList != NULL) {
+			// Updata BB Reg if RSSI is too strong.
+			long    LocalldBmAverage = 0;
+			long    uNumofdBm = 0;
+			for (ii = 0; ii < RSSI_STAT_COUNT; ii++) {
+				if (pBSSList->ldBmAverage[ii] != 0) {
+					uNumofdBm++;
+					LocalldBmAverage += pBSSList->ldBmAverage[ii];
+				}
+			}
+			if (uNumofdBm > 0) {
+				LocalldBmAverage = LocalldBmAverage/uNumofdBm;
+				for (ii = 0; ii < BB_VGA_LEVEL; ii++) {
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "LocalldBmAverage:%ld, %ld %02x\n", LocalldBmAverage, pDevice->ldBmThreshold[ii], pDevice->abyBBVGA[ii]);
+					if (LocalldBmAverage < pDevice->ldBmThreshold[ii]) {
+						pDevice->byBBVGANew = pDevice->abyBBVGA[ii];
+						break;
+					}
+				}
+				if (pDevice->byBBVGANew != pDevice->byBBVGACurrent) {
+					pDevice->uBBVGADiffCount++;
+					if (pDevice->uBBVGADiffCount >= BB_VGA_CHANGE_THRESHOLD)
+						bScheduleCommand((void *)pDevice, WLAN_CMD_CHANGE_BBSENSITIVITY, NULL);
+				} else {
+					pDevice->uBBVGADiffCount = 0;
+				}
+			}
+		}
+	}
 }
 
-
 void
-BSSvClearAnyBSSJoinRecord (
-    void *hDeviceContext
-    )
+BSSvClearAnyBSSJoinRecord(
+	void *hDeviceContext
+)
 {
-    PSDevice        pDevice = (PSDevice)hDeviceContext;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    unsigned int ii;
-
-    for (ii = 0; ii < MAX_BSS_NUM; ii++) {
-        pMgmt->sBSSList[ii].bSelected = false;
-    }
-    return;
+	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	unsigned int ii;
+
+	for (ii = 0; ii < MAX_BSS_NUM; ii++) {
+		pMgmt->sBSSList[ii].bSelected = false;
+	}
+	return;
 }
 
 #ifdef Calcu_LinkQual
 void s_uCalculateLinkQual(
-    void *hDeviceContext
-    )
+	void *hDeviceContext
+)
 {
-   PSDevice        pDevice = (PSDevice)hDeviceContext;
-   unsigned long TxOkRatio, TxCnt;
-   unsigned long RxOkRatio,RxCnt;
-   unsigned long RssiRatio;
-   long ldBm;
-
-TxCnt = pDevice->scStatistic.TxNoRetryOkCount +
-	      pDevice->scStatistic.TxRetryOkCount +
-	      pDevice->scStatistic.TxFailCount;
-RxCnt = pDevice->scStatistic.RxFcsErrCnt +
-	      pDevice->scStatistic.RxOkCnt;
-TxOkRatio = (TxCnt < 6) ? 4000:((pDevice->scStatistic.TxNoRetryOkCount * 4000) / TxCnt);
-RxOkRatio = (RxCnt < 6) ? 2000:((pDevice->scStatistic.RxOkCnt * 2000) / RxCnt);
+	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	unsigned long TxOkRatio, TxCnt;
+	unsigned long RxOkRatio, RxCnt;
+	unsigned long RssiRatio;
+	long ldBm;
+
+	TxCnt = pDevice->scStatistic.TxNoRetryOkCount +
+		pDevice->scStatistic.TxRetryOkCount +
+		pDevice->scStatistic.TxFailCount;
+	RxCnt = pDevice->scStatistic.RxFcsErrCnt +
+		pDevice->scStatistic.RxOkCnt;
+	TxOkRatio = (TxCnt < 6) ? 4000 : ((pDevice->scStatistic.TxNoRetryOkCount * 4000) / TxCnt);
+	RxOkRatio = (RxCnt < 6) ? 2000 : ((pDevice->scStatistic.RxOkCnt * 2000) / RxCnt);
 //decide link quality
-if(pDevice->bLinkPass !=true)
-{
- //  printk("s_uCalculateLinkQual-->Link disconnect and Poor quality**\n");
-   pDevice->scStatistic.LinkQuality = 0;
-   pDevice->scStatistic.SignalStren = 0;
-}
-else
-{
-   RFvRSSITodBm(pDevice, (unsigned char)(pDevice->uCurrRSSI), &ldBm);
-   if(-ldBm < 50)  {
-   	RssiRatio = 4000;
-     }
-   else if(-ldBm > 90) {
-   	RssiRatio = 0;
-     }
-   else {
-   	RssiRatio = (40-(-ldBm-50))*4000/40;
-     }
-   pDevice->scStatistic.SignalStren = RssiRatio/40;
-   pDevice->scStatistic.LinkQuality = (RssiRatio+TxOkRatio+RxOkRatio)/100;
-}
-   pDevice->scStatistic.RxFcsErrCnt = 0;
-   pDevice->scStatistic.RxOkCnt = 0;
-   pDevice->scStatistic.TxFailCount = 0;
-   pDevice->scStatistic.TxNoRetryOkCount = 0;
-   pDevice->scStatistic.TxRetryOkCount = 0;
-   return;
+	if (pDevice->bLinkPass != true) {
+		pDevice->scStatistic.LinkQuality = 0;
+		pDevice->scStatistic.SignalStren = 0;
+	} else {
+		RFvRSSITodBm(pDevice, (unsigned char)(pDevice->uCurrRSSI), &ldBm);
+		if (-ldBm < 50)  {
+			RssiRatio = 4000;
+		} else if (-ldBm > 90) {
+			RssiRatio = 0;
+		} else {
+			RssiRatio = (40-(-ldBm-50))*4000/40;
+		}
+		pDevice->scStatistic.SignalStren = RssiRatio/40;
+		pDevice->scStatistic.LinkQuality = (RssiRatio+TxOkRatio+RxOkRatio)/100;
+	}
+	pDevice->scStatistic.RxFcsErrCnt = 0;
+	pDevice->scStatistic.RxOkCnt = 0;
+	pDevice->scStatistic.TxFailCount = 0;
+	pDevice->scStatistic.TxNoRetryOkCount = 0;
+	pDevice->scStatistic.TxRetryOkCount = 0;
+	return;
 }
 #endif
 
 void s_vCheckPreEDThreshold(
-    void *hDeviceContext
-    )
+	void *hDeviceContext
+)
 {
-    PSDevice        pDevice = (PSDevice)hDeviceContext;
-    PKnownBSS       pBSSList = NULL;
-    PSMgmtObject    pMgmt = &(pDevice->sMgmtObj);
-
-    if ((pMgmt->eCurrState == WMAC_STATE_ASSOC) ||
-        ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) && (pMgmt->eCurrState == WMAC_STATE_JOINTED))) {
-        pBSSList = BSSpAddrIsInBSSList(pDevice, pMgmt->abyCurrBSSID, (PWLAN_IE_SSID)pMgmt->abyCurrSSID);
-        if (pBSSList != NULL) {
-            pDevice->byBBPreEDRSSI = (unsigned char) (~(pBSSList->ldBmAverRange) + 1);
-            //BBvUpdatePreEDThreshold(pDevice, false);
-        }
-    }
-    return;
+	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	PKnownBSS       pBSSList = NULL;
+	PSMgmtObject    pMgmt = &(pDevice->sMgmtObj);
+
+	if ((pMgmt->eCurrState == WMAC_STATE_ASSOC) ||
+	    ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) && (pMgmt->eCurrState == WMAC_STATE_JOINTED))) {
+		pBSSList = BSSpAddrIsInBSSList(pDevice, pMgmt->abyCurrBSSID, (PWLAN_IE_SSID)pMgmt->abyCurrSSID);
+		if (pBSSList != NULL) {
+			pDevice->byBBPreEDRSSI = (unsigned char) (~(pBSSList->ldBmAverRange) + 1);
+			//BBvUpdatePreEDThreshold(pDevice, false);
+		}
+	}
+	return;
 }
-
diff --git a/drivers/staging/vt6655/bssdb.h b/drivers/staging/vt6655/bssdb.h
index 0af42118612..5c77677251e 100644
--- a/drivers/staging/vt6655/bssdb.h
+++ b/drivers/staging/vt6655/bssdb.h
@@ -39,7 +39,7 @@
 
 #define MAX_NODE_NUM             64
 #define MAX_BSS_NUM              42
-#define LOST_BEACON_COUNT      	 10   // 10 sec, XP defined
+#define LOST_BEACON_COUNT        10   // 10 sec, XP defined
 #define MAX_PS_TX_BUF            32   // sta max power saving tx buf
 #define ADHOC_LOST_BEACON_COUNT  30   // 30 sec, beacon lost for adhoc only
 #define MAX_INACTIVE_COUNT       300  // 300 sec, inactive STA node refresh
@@ -67,12 +67,10 @@
 
 #define MAX_WPA_IE_LEN      64
 
-
 /*---------------------  Export Classes  ----------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
 
-
 /*---------------------  Export Types  ------------------------------*/
 
 //
@@ -81,285 +79,272 @@
 
 typedef enum _NDIS_802_11_NETWORK_TYPE
 {
-    Ndis802_11FH,
-    Ndis802_11DS,
-    Ndis802_11OFDM5,
-    Ndis802_11OFDM24,
-    Ndis802_11NetworkTypeMax    // not a real type, defined as an upper bound
+	Ndis802_11FH,
+	Ndis802_11DS,
+	Ndis802_11OFDM5,
+	Ndis802_11OFDM24,
+	Ndis802_11NetworkTypeMax    // not a real type, defined as an upper bound
 } NDIS_802_11_NETWORK_TYPE, *PNDIS_802_11_NETWORK_TYPE;
 
-
 typedef struct tagSERPObject {
-    bool bERPExist;
-    unsigned char byERP;
-}ERPObject, *PERPObject;
-
+	bool bERPExist;
+	unsigned char byERP;
+} ERPObject, *PERPObject;
 
 typedef struct tagSRSNCapObject {
-    bool bRSNCapExist;
-    unsigned short wRSNCap;
-}SRSNCapObject, *PSRSNCapObject;
+	bool bRSNCapExist;
+	unsigned short wRSNCap;
+} SRSNCapObject, *PSRSNCapObject;
 
 // BSS info(AP)
 #pragma pack(1)
 typedef struct tagKnownBSS {
-    // BSS info
-    bool bActive;
-    unsigned char abyBSSID[WLAN_BSSID_LEN];
-    unsigned int	uChannel;
-    unsigned char abySuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
-    unsigned char abyExtSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
-    unsigned int	uRSSI;
-    unsigned char bySQ;
-    unsigned short wBeaconInterval;
-    unsigned short wCapInfo;
-    unsigned char abySSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
-    unsigned char byRxRate;
+	// BSS info
+	bool bActive;
+	unsigned char abyBSSID[WLAN_BSSID_LEN];
+	unsigned int	uChannel;
+	unsigned char abySuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
+	unsigned char abyExtSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
+	unsigned int	uRSSI;
+	unsigned char bySQ;
+	unsigned short wBeaconInterval;
+	unsigned short wCapInfo;
+	unsigned char abySSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
+	unsigned char byRxRate;
 
 //    unsigned short wATIMWindow;
-    unsigned char byRSSIStatCnt;
-    long            ldBmMAX;
-    long            ldBmAverage[RSSI_STAT_COUNT];
-    long            ldBmAverRange;
-    //For any BSSID selection improvment
-    bool bSelected;
-
-    //++ WPA informations
-    bool bWPAValid;
-    unsigned char byGKType;
-    unsigned char abyPKType[4];
-    unsigned short wPKCount;
-    unsigned char abyAuthType[4];
-    unsigned short wAuthCount;
-    unsigned char byDefaultK_as_PK;
-    unsigned char byReplayIdx;
-    //--
-
-    //++ WPA2 informations
-    bool bWPA2Valid;
-    unsigned char byCSSGK;
-    unsigned short wCSSPKCount;
-    unsigned char abyCSSPK[4];
-    unsigned short wAKMSSAuthCount;
-    unsigned char abyAKMSSAuthType[4];
-
-    //++  wpactl
-    unsigned char byWPAIE[MAX_WPA_IE_LEN];
-    unsigned char byRSNIE[MAX_WPA_IE_LEN];
-    unsigned short wWPALen;
-    unsigned short wRSNLen;
-
-    // Clear count
-    unsigned int	uClearCount;
+	unsigned char byRSSIStatCnt;
+	long            ldBmMAX;
+	long            ldBmAverage[RSSI_STAT_COUNT];
+	long            ldBmAverRange;
+	//For any BSSID selection improvment
+	bool bSelected;
+
+	//++ WPA informations
+	bool bWPAValid;
+	unsigned char byGKType;
+	unsigned char abyPKType[4];
+	unsigned short wPKCount;
+	unsigned char abyAuthType[4];
+	unsigned short wAuthCount;
+	unsigned char byDefaultK_as_PK;
+	unsigned char byReplayIdx;
+	//--
+
+	//++ WPA2 informations
+	bool bWPA2Valid;
+	unsigned char byCSSGK;
+	unsigned short wCSSPKCount;
+	unsigned char abyCSSPK[4];
+	unsigned short wAKMSSAuthCount;
+	unsigned char abyAKMSSAuthType[4];
+
+	//++  wpactl
+	unsigned char byWPAIE[MAX_WPA_IE_LEN];
+	unsigned char byRSNIE[MAX_WPA_IE_LEN];
+	unsigned short wWPALen;
+	unsigned short wRSNLen;
+
+	// Clear count
+	unsigned int	uClearCount;
 //    unsigned char abyIEs[WLAN_BEACON_FR_MAXLEN];
-    unsigned int	uIELength;
-    QWORD           qwBSSTimestamp;
-    QWORD           qwLocalTSF;     // local TSF timer
+	unsigned int	uIELength;
+	QWORD           qwBSSTimestamp;
+	QWORD           qwLocalTSF;     // local TSF timer
 
 //    NDIS_802_11_NETWORK_TYPE    NetworkTypeInUse;
-    CARD_PHY_TYPE   eNetworkTypeInUse;
+	CARD_PHY_TYPE   eNetworkTypeInUse;
 
-    ERPObject       sERP;
-    SRSNCapObject   sRSNCapObj;
-    unsigned char abyIEs[1024];   // don't move this field !!
-
-}__attribute__ ((__packed__))
+	ERPObject       sERP;
+	SRSNCapObject   sRSNCapObj;
+	unsigned char abyIEs[1024];   // don't move this field !!
+} __attribute__ ((__packed__))
 KnownBSS , *PKnownBSS;
 
 //2006-1116-01,<Add> by NomadZhao
 #pragma pack()
 
 typedef enum tagNODE_STATE {
-    NODE_FREE,
-    NODE_AGED,
-    NODE_KNOWN,
-    NODE_AUTH,
-    NODE_ASSOC
+	NODE_FREE,
+	NODE_AGED,
+	NODE_KNOWN,
+	NODE_AUTH,
+	NODE_ASSOC
 } NODE_STATE, *PNODE_STATE;
 
-
 // STA node info
 typedef struct tagKnownNodeDB {
-    // STA info
-    bool bActive;
-    unsigned char abyMACAddr[WLAN_ADDR_LEN];
-    unsigned char abyCurrSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN];
-    unsigned char abyCurrExtSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN];
-    unsigned short wTxDataRate;
-    bool bShortPreamble;
-    bool bERPExist;
-    bool bShortSlotTime;
-    unsigned int	uInActiveCount;
-    unsigned short wMaxBasicRate;     //Get from byTopOFDMBasicRate or byTopCCKBasicRate which depends on packetTyp.
-    unsigned short wMaxSuppRate;      //Records the highest supported rate getting from SuppRates IE and ExtSuppRates IE in Beacon.
-    unsigned short wSuppRate;
-    unsigned char byTopOFDMBasicRate;//Records the highest basic rate in OFDM mode
-    unsigned char byTopCCKBasicRate; //Records the highest basic rate in CCK mode
-
-    // For AP mode
-    struct sk_buff_head sTxPSQueue;
-    unsigned short wCapInfo;
-    unsigned short wListenInterval;
-    unsigned short wAID;
-    NODE_STATE      eNodeState;
-    bool bPSEnable;
-    bool bRxPSPoll;
-    unsigned char byAuthSequence;
-    unsigned long ulLastRxJiffer;
-    unsigned char bySuppRate;
-    unsigned long dwFlags;
-    unsigned short wEnQueueCnt;
-
-    bool bOnFly;
-    unsigned long long       KeyRSC;
-    unsigned char byKeyIndex;
-    unsigned long dwKeyIndex;
-    unsigned char byCipherSuite;
-    unsigned long dwTSC47_16;
-    unsigned short wTSC15_0;
-    unsigned int	uWepKeyLength;
-    unsigned char abyWepKey[WLAN_WEPMAX_KEYLEN];
-    //
-    // Auto rate fallback vars
-    bool bIsInFallback;
-    unsigned int	uAverageRSSI;
-    unsigned int	uRateRecoveryTimeout;
-    unsigned int	uRatePollTimeout;
-    unsigned int	uTxFailures;
-    unsigned int	uTxAttempts;
-
-    unsigned int	uTxRetry;
-    unsigned int	uFailureRatio;
-    unsigned int	uRetryRatio;
-    unsigned int	uTxOk[MAX_RATE+1];
-    unsigned int	uTxFail[MAX_RATE+1];
-    unsigned int	uTimeCount;
-
+	// STA info
+	bool bActive;
+	unsigned char abyMACAddr[WLAN_ADDR_LEN];
+	unsigned char abyCurrSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN];
+	unsigned char abyCurrExtSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN];
+	unsigned short wTxDataRate;
+	bool bShortPreamble;
+	bool bERPExist;
+	bool bShortSlotTime;
+	unsigned int	uInActiveCount;
+	unsigned short wMaxBasicRate;     //Get from byTopOFDMBasicRate or byTopCCKBasicRate which depends on packetTyp.
+	unsigned short wMaxSuppRate;      //Records the highest supported rate getting from SuppRates IE and ExtSuppRates IE in Beacon.
+	unsigned short wSuppRate;
+	unsigned char byTopOFDMBasicRate;//Records the highest basic rate in OFDM mode
+	unsigned char byTopCCKBasicRate; //Records the highest basic rate in CCK mode
+
+	// For AP mode
+	struct sk_buff_head sTxPSQueue;
+	unsigned short wCapInfo;
+	unsigned short wListenInterval;
+	unsigned short wAID;
+	NODE_STATE      eNodeState;
+	bool bPSEnable;
+	bool bRxPSPoll;
+	unsigned char byAuthSequence;
+	unsigned long ulLastRxJiffer;
+	unsigned char bySuppRate;
+	unsigned long dwFlags;
+	unsigned short wEnQueueCnt;
+
+	bool bOnFly;
+	unsigned long long       KeyRSC;
+	unsigned char byKeyIndex;
+	unsigned long dwKeyIndex;
+	unsigned char byCipherSuite;
+	unsigned long dwTSC47_16;
+	unsigned short wTSC15_0;
+	unsigned int	uWepKeyLength;
+	unsigned char abyWepKey[WLAN_WEPMAX_KEYLEN];
+	//
+	// Auto rate fallback vars
+	bool bIsInFallback;
+	unsigned int	uAverageRSSI;
+	unsigned int	uRateRecoveryTimeout;
+	unsigned int	uRatePollTimeout;
+	unsigned int	uTxFailures;
+	unsigned int	uTxAttempts;
+
+	unsigned int	uTxRetry;
+	unsigned int	uFailureRatio;
+	unsigned int	uRetryRatio;
+	unsigned int	uTxOk[MAX_RATE+1];
+	unsigned int	uTxFail[MAX_RATE+1];
+	unsigned int	uTimeCount;
 } KnownNodeDB, *PKnownNodeDB;
 
-
 /*---------------------  Export Functions  --------------------------*/
 
-
-
 PKnownBSS
 BSSpSearchBSSList(
-    void *hDeviceContext,
-    unsigned char *pbyDesireBSSID,
-    unsigned char *pbyDesireSSID,
-    CARD_PHY_TYPE ePhyType
-    );
+	void *hDeviceContext,
+	unsigned char *pbyDesireBSSID,
+	unsigned char *pbyDesireSSID,
+	CARD_PHY_TYPE ePhyType
+);
 
 PKnownBSS
 BSSpAddrIsInBSSList(
-    void *hDeviceContext,
-    unsigned char *abyBSSID,
-    PWLAN_IE_SSID pSSID
-    );
+	void *hDeviceContext,
+	unsigned char *abyBSSID,
+	PWLAN_IE_SSID pSSID
+);
 
 void
 BSSvClearBSSList(
-    void *hDeviceContext,
-    bool bKeepCurrBSSID
-    );
+	void *hDeviceContext,
+	bool bKeepCurrBSSID
+);
 
 bool
 BSSbInsertToBSSList(
-    void *hDeviceContext,
-    unsigned char *abyBSSIDAddr,
-    QWORD qwTimestamp,
-    unsigned short wBeaconInterval,
-    unsigned short wCapInfo,
-    unsigned char byCurrChannel,
-    PWLAN_IE_SSID pSSID,
-    PWLAN_IE_SUPP_RATES pSuppRates,
-    PWLAN_IE_SUPP_RATES pExtSuppRates,
-    PERPObject psERP,
-    PWLAN_IE_RSN pRSN,
-    PWLAN_IE_RSN_EXT pRSNWPA,
-    PWLAN_IE_COUNTRY pIE_Country,
-    PWLAN_IE_QUIET pIE_Quiet,
-    unsigned int uIELength,
-    unsigned char *pbyIEs,
-    void *pRxPacketContext
-    );
-
+	void *hDeviceContext,
+	unsigned char *abyBSSIDAddr,
+	QWORD qwTimestamp,
+	unsigned short wBeaconInterval,
+	unsigned short wCapInfo,
+	unsigned char byCurrChannel,
+	PWLAN_IE_SSID pSSID,
+	PWLAN_IE_SUPP_RATES pSuppRates,
+	PWLAN_IE_SUPP_RATES pExtSuppRates,
+	PERPObject psERP,
+	PWLAN_IE_RSN pRSN,
+	PWLAN_IE_RSN_EXT pRSNWPA,
+	PWLAN_IE_COUNTRY pIE_Country,
+	PWLAN_IE_QUIET pIE_Quiet,
+	unsigned int uIELength,
+	unsigned char *pbyIEs,
+	void *pRxPacketContext
+);
 
 bool
 BSSbUpdateToBSSList(
-    void *hDeviceContext,
-    QWORD qwTimestamp,
-    unsigned short wBeaconInterval,
-    unsigned short wCapInfo,
-    unsigned char byCurrChannel,
-    bool bChannelHit,
-    PWLAN_IE_SSID pSSID,
-    PWLAN_IE_SUPP_RATES pSuppRates,
-    PWLAN_IE_SUPP_RATES pExtSuppRates,
-    PERPObject psERP,
-    PWLAN_IE_RSN pRSN,
-    PWLAN_IE_RSN_EXT pRSNWPA,
-    PWLAN_IE_COUNTRY pIE_Country,
-    PWLAN_IE_QUIET pIE_Quiet,
-    PKnownBSS pBSSList,
-    unsigned int uIELength,
-    unsigned char *pbyIEs,
-    void *pRxPacketContext
-    );
-
+	void *hDeviceContext,
+	QWORD qwTimestamp,
+	unsigned short wBeaconInterval,
+	unsigned short wCapInfo,
+	unsigned char byCurrChannel,
+	bool bChannelHit,
+	PWLAN_IE_SSID pSSID,
+	PWLAN_IE_SUPP_RATES pSuppRates,
+	PWLAN_IE_SUPP_RATES pExtSuppRates,
+	PERPObject psERP,
+	PWLAN_IE_RSN pRSN,
+	PWLAN_IE_RSN_EXT pRSNWPA,
+	PWLAN_IE_COUNTRY pIE_Country,
+	PWLAN_IE_QUIET pIE_Quiet,
+	PKnownBSS pBSSList,
+	unsigned int uIELength,
+	unsigned char *pbyIEs,
+	void *pRxPacketContext
+);
 
 bool
 BSSDBbIsSTAInNodeDB(void *hDeviceContext, unsigned char *abyDstAddr,
-		unsigned int *puNodeIndex);
+		    unsigned int *puNodeIndex);
 
 void
 BSSvCreateOneNode(void *hDeviceContext, unsigned int *puNodeIndex);
 
 void
 BSSvUpdateAPNode(
-    void *hDeviceContext,
-    unsigned short *pwCapInfo,
-    PWLAN_IE_SUPP_RATES pItemRates,
-    PWLAN_IE_SUPP_RATES pExtSuppRates
-    );
-
+	void *hDeviceContext,
+	unsigned short *pwCapInfo,
+	PWLAN_IE_SUPP_RATES pItemRates,
+	PWLAN_IE_SUPP_RATES pExtSuppRates
+);
 
 void
 BSSvSecondCallBack(
-    void *hDeviceContext
-    );
-
+	void *hDeviceContext
+);
 
 void
 BSSvUpdateNodeTxCounter(
-    void *hDeviceContext,
-    unsigned char byTsr0,
-    unsigned char byTsr1,
-    unsigned char *pbyBuffer,
-    unsigned int uFIFOHeaderSize
-    );
+	void *hDeviceContext,
+	unsigned char byTsr0,
+	unsigned char byTsr1,
+	unsigned char *pbyBuffer,
+	unsigned int uFIFOHeaderSize
+);
 
 void
 BSSvRemoveOneNode(
-    void *hDeviceContext,
-    unsigned int uNodeIndex
-    );
+	void *hDeviceContext,
+	unsigned int uNodeIndex
+);
 
 void
 BSSvAddMulticastNode(
-    void *hDeviceContext
-    );
-
+	void *hDeviceContext
+);
 
 void
 BSSvClearNodeDBTable(
-    void *hDeviceContext,
-    unsigned int uStartIndex
-    );
+	void *hDeviceContext,
+	unsigned int uStartIndex
+);
 
 void
 BSSvClearAnyBSSJoinRecord(
-    void *hDeviceContext
-    );
+	void *hDeviceContext
+);
 
 #endif //__BSSDB_H__
diff --git a/drivers/staging/vt6655/card.c b/drivers/staging/vt6655/card.c
index 319ca482f00..fbf18e23e78 100644
--- a/drivers/staging/vt6655/card.c
+++ b/drivers/staging/vt6655/card.c
@@ -61,14 +61,13 @@
 /*---------------------  Static Definitions -------------------------*/
 
 //static int          msglevel                =MSG_LEVEL_DEBUG;
-static int          msglevel                =MSG_LEVEL_INFO;
+static int msglevel = MSG_LEVEL_INFO;
 
 #define C_SIFS_A        16      // micro sec.
 #define C_SIFS_BG       10
 
 #define C_EIFS          80      // micro sec.
 
-
 #define C_SLOT_SHORT    9       // micro sec.
 #define C_SLOT_LONG     20
 
@@ -79,34 +78,30 @@ static int          msglevel                =MSG_LEVEL_INFO;
 
 #define WAIT_BEACON_TX_DOWN_TMO         3    // Times
 
-                                                              //1M,   2M,   5M,  11M,  18M,  24M,  36M,  54M
+//1M,   2M,   5M,  11M,  18M,  24M,  36M,  54M
 static unsigned char abyDefaultSuppRatesG[] = {WLAN_EID_SUPP_RATES, 8, 0x02, 0x04, 0x0B, 0x16, 0x24, 0x30, 0x48, 0x6C};
-                                                                    //6M,   9M,  12M,  48M
+//6M,   9M,  12M,  48M
 static unsigned char abyDefaultExtSuppRatesG[] = {WLAN_EID_EXTSUPP_RATES, 4, 0x0C, 0x12, 0x18, 0x60};
-                                                              //6M,   9M,  12M,  18M,  24M,  36M,  48M,  54M
+//6M,   9M,  12M,  18M,  24M,  36M,  48M,  54M
 static unsigned char abyDefaultSuppRatesA[] = {WLAN_EID_SUPP_RATES, 8, 0x0C, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C};
-                                                              //1M,   2M,   5M,  11M,
+//1M,   2M,   5M,  11M,
 static unsigned char abyDefaultSuppRatesB[] = {WLAN_EID_SUPP_RATES, 4, 0x02, 0x04, 0x0B, 0x16};
 
-
 /*---------------------  Static Variables  --------------------------*/
 
-
 const unsigned short cwRXBCNTSFOff[MAX_RATE] =
 {17, 17, 17, 17, 34, 23, 17, 11, 8, 5, 4, 3};
 
-
 /*---------------------  Static Functions  --------------------------*/
 
 static
 void
 s_vCalculateOFDMRParameter(
-    unsigned char byRate,
-    CARD_PHY_TYPE ePHYType,
-    unsigned char *pbyTxRate,
-    unsigned char *pbyRsvTime
-    );
-
+	unsigned char byRate,
+	CARD_PHY_TYPE ePHYType,
+	unsigned char *pbyTxRate,
+	unsigned char *pbyRsvTime
+);
 
 /*---------------------  Export Functions  --------------------------*/
 
@@ -126,107 +121,97 @@ s_vCalculateOFDMRParameter(
  */
 static
 void
-s_vCalculateOFDMRParameter (
-    unsigned char byRate,
-    CARD_PHY_TYPE ePHYType,
-    unsigned char *pbyTxRate,
-    unsigned char *pbyRsvTime
-    )
+s_vCalculateOFDMRParameter(
+	unsigned char byRate,
+	CARD_PHY_TYPE ePHYType,
+	unsigned char *pbyTxRate,
+	unsigned char *pbyRsvTime
+)
 {
-    switch (byRate) {
-    case RATE_6M :
-        if (ePHYType == PHY_TYPE_11A) {//5GHZ
-            *pbyTxRate = 0x9B;
-            *pbyRsvTime = 44;
-        }
-        else {
-            *pbyTxRate = 0x8B;
-            *pbyRsvTime = 50;
-        }
-        break;
-
-    case RATE_9M :
-        if (ePHYType == PHY_TYPE_11A) {//5GHZ
-            *pbyTxRate = 0x9F;
-            *pbyRsvTime = 36;
-        }
-        else {
-            *pbyTxRate = 0x8F;
-            *pbyRsvTime = 42;
-        }
-        break;
-
-   case RATE_12M :
-        if (ePHYType == PHY_TYPE_11A) {//5GHZ
-            *pbyTxRate = 0x9A;
-            *pbyRsvTime = 32;
-        }
-        else {
-            *pbyTxRate = 0x8A;
-            *pbyRsvTime = 38;
-        }
-        break;
-
-   case RATE_18M :
-        if (ePHYType == PHY_TYPE_11A) {//5GHZ
-            *pbyTxRate = 0x9E;
-            *pbyRsvTime = 28;
-        }
-        else {
-            *pbyTxRate = 0x8E;
-            *pbyRsvTime = 34;
-        }
-        break;
-
-    case RATE_36M :
-        if (ePHYType == PHY_TYPE_11A) {//5GHZ
-            *pbyTxRate = 0x9D;
-            *pbyRsvTime = 24;
-        }
-        else {
-            *pbyTxRate = 0x8D;
-            *pbyRsvTime = 30;
-        }
-        break;
-
-    case RATE_48M :
-        if (ePHYType == PHY_TYPE_11A) {//5GHZ
-            *pbyTxRate = 0x98;
-            *pbyRsvTime = 24;
-        }
-        else {
-            *pbyTxRate = 0x88;
-            *pbyRsvTime = 30;
-        }
-        break;
-
-    case RATE_54M :
-        if (ePHYType == PHY_TYPE_11A) {//5GHZ
-            *pbyTxRate = 0x9C;
-            *pbyRsvTime = 24;
-        }
-        else {
-            *pbyTxRate = 0x8C;
-            *pbyRsvTime = 30;
-        }
-        break;
-
-    case RATE_24M :
-    default :
-        if (ePHYType == PHY_TYPE_11A) {//5GHZ
-            *pbyTxRate = 0x99;
-            *pbyRsvTime = 28;
-        }
-        else {
-            *pbyTxRate = 0x89;
-            *pbyRsvTime = 34;
-        }
-        break;
-    }
+	switch (byRate) {
+	case RATE_6M:
+		if (ePHYType == PHY_TYPE_11A) {//5GHZ
+			*pbyTxRate = 0x9B;
+			*pbyRsvTime = 44;
+		} else {
+			*pbyTxRate = 0x8B;
+			*pbyRsvTime = 50;
+		}
+		break;
+
+	case RATE_9M:
+		if (ePHYType == PHY_TYPE_11A) {//5GHZ
+			*pbyTxRate = 0x9F;
+			*pbyRsvTime = 36;
+		} else {
+			*pbyTxRate = 0x8F;
+			*pbyRsvTime = 42;
+		}
+		break;
+
+	case RATE_12M:
+		if (ePHYType == PHY_TYPE_11A) {//5GHZ
+			*pbyTxRate = 0x9A;
+			*pbyRsvTime = 32;
+		} else {
+			*pbyTxRate = 0x8A;
+			*pbyRsvTime = 38;
+		}
+		break;
+
+	case RATE_18M:
+		if (ePHYType == PHY_TYPE_11A) {//5GHZ
+			*pbyTxRate = 0x9E;
+			*pbyRsvTime = 28;
+		} else {
+			*pbyTxRate = 0x8E;
+			*pbyRsvTime = 34;
+		}
+		break;
+
+	case RATE_36M:
+		if (ePHYType == PHY_TYPE_11A) {//5GHZ
+			*pbyTxRate = 0x9D;
+			*pbyRsvTime = 24;
+		} else {
+			*pbyTxRate = 0x8D;
+			*pbyRsvTime = 30;
+		}
+		break;
+
+	case RATE_48M:
+		if (ePHYType == PHY_TYPE_11A) {//5GHZ
+			*pbyTxRate = 0x98;
+			*pbyRsvTime = 24;
+		} else {
+			*pbyTxRate = 0x88;
+			*pbyRsvTime = 30;
+		}
+		break;
+
+	case RATE_54M:
+		if (ePHYType == PHY_TYPE_11A) {//5GHZ
+			*pbyTxRate = 0x9C;
+			*pbyRsvTime = 24;
+		} else {
+			*pbyTxRate = 0x8C;
+			*pbyRsvTime = 30;
+		}
+		break;
+
+	case RATE_24M:
+	default:
+		if (ePHYType == PHY_TYPE_11A) {//5GHZ
+			*pbyTxRate = 0x99;
+			*pbyRsvTime = 28;
+		} else {
+			*pbyTxRate = 0x89;
+			*pbyRsvTime = 34;
+		}
+		break;
+	}
 }
 
-
-
 /*
  * Description: Set RSPINF
  *
@@ -241,114 +226,114 @@ s_vCalculateOFDMRParameter (
  */
 static
 void
-s_vSetRSPINF (PSDevice pDevice, CARD_PHY_TYPE ePHYType, void *pvSupportRateIEs, void *pvExtSupportRateIEs)
+s_vSetRSPINF(PSDevice pDevice, CARD_PHY_TYPE ePHYType, void *pvSupportRateIEs, void *pvExtSupportRateIEs)
 {
-    unsigned char byServ = 0, bySignal = 0; // For CCK
-    unsigned short wLen = 0;
-    unsigned char byTxRate = 0, byRsvTime = 0;    // For OFDM
-
-    //Set to Page1
-    MACvSelectPage1(pDevice->PortOffset);
-
-    //RSPINF_b_1
-    BBvCalculateParameter(pDevice,
-                         14,
-                         VNTWIFIbyGetACKTxRate(RATE_1M, pvSupportRateIEs, pvExtSupportRateIEs),
-                         PK_TYPE_11B,
-                         &wLen,
-                         &byServ,
-                         &bySignal
-    );
-
-    VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_1, MAKEDWORD(wLen,MAKEWORD(bySignal,byServ)));
-    ///RSPINF_b_2
-    BBvCalculateParameter(pDevice,
-                         14,
-                         VNTWIFIbyGetACKTxRate(RATE_2M, pvSupportRateIEs, pvExtSupportRateIEs),
-                         PK_TYPE_11B,
-                         &wLen,
-                         &byServ,
-                         &bySignal
-    );
-
-    VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_2, MAKEDWORD(wLen,MAKEWORD(bySignal,byServ)));
-    //RSPINF_b_5
-    BBvCalculateParameter(pDevice,
-                         14,
-                         VNTWIFIbyGetACKTxRate(RATE_5M, pvSupportRateIEs, pvExtSupportRateIEs),
-                         PK_TYPE_11B,
-                         &wLen,
-                         &byServ,
-                         &bySignal
-    );
-
-    VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_5, MAKEDWORD(wLen,MAKEWORD(bySignal,byServ)));
-    //RSPINF_b_11
-    BBvCalculateParameter(pDevice,
-                         14,
-                         VNTWIFIbyGetACKTxRate(RATE_11M, pvSupportRateIEs, pvExtSupportRateIEs),
-                         PK_TYPE_11B,
-                         &wLen,
-                         &byServ,
-                         &bySignal
-    );
-
-    VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_11, MAKEDWORD(wLen,MAKEWORD(bySignal,byServ)));
-    //RSPINF_a_6
-    s_vCalculateOFDMRParameter(RATE_6M,
-                              ePHYType,
-                              &byTxRate,
-                              &byRsvTime);
-    VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_6, MAKEWORD(byTxRate,byRsvTime));
-    //RSPINF_a_9
-    s_vCalculateOFDMRParameter(RATE_9M,
-                              ePHYType,
-                              &byTxRate,
-                              &byRsvTime);
-    VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_9, MAKEWORD(byTxRate,byRsvTime));
-    //RSPINF_a_12
-    s_vCalculateOFDMRParameter(RATE_12M,
-                              ePHYType,
-                              &byTxRate,
-                              &byRsvTime);
-    VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_12, MAKEWORD(byTxRate,byRsvTime));
-    //RSPINF_a_18
-    s_vCalculateOFDMRParameter(RATE_18M,
-                              ePHYType,
-                              &byTxRate,
-                              &byRsvTime);
-    VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_18, MAKEWORD(byTxRate,byRsvTime));
-    //RSPINF_a_24
-    s_vCalculateOFDMRParameter(RATE_24M,
-                              ePHYType,
-                              &byTxRate,
-                              &byRsvTime);
-    VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_24, MAKEWORD(byTxRate,byRsvTime));
-    //RSPINF_a_36
-    s_vCalculateOFDMRParameter(
-                              VNTWIFIbyGetACKTxRate(RATE_36M, pvSupportRateIEs, pvExtSupportRateIEs),
-                              ePHYType,
-                              &byTxRate,
-                              &byRsvTime);
-    VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_36, MAKEWORD(byTxRate,byRsvTime));
-    //RSPINF_a_48
-    s_vCalculateOFDMRParameter(
-                              VNTWIFIbyGetACKTxRate(RATE_48M, pvSupportRateIEs, pvExtSupportRateIEs),
-                              ePHYType,
-                              &byTxRate,
-                              &byRsvTime);
-    VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_48, MAKEWORD(byTxRate,byRsvTime));
-    //RSPINF_a_54
-    s_vCalculateOFDMRParameter(
-                              VNTWIFIbyGetACKTxRate(RATE_54M, pvSupportRateIEs, pvExtSupportRateIEs),
-                              ePHYType,
-                              &byTxRate,
-                              &byRsvTime);
-    VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_54, MAKEWORD(byTxRate,byRsvTime));
-    //RSPINF_a_72
-    VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_72, MAKEWORD(byTxRate,byRsvTime));
-    //Set to Page0
-    MACvSelectPage0(pDevice->PortOffset);
+	unsigned char byServ = 0, bySignal = 0; // For CCK
+	unsigned short wLen = 0;
+	unsigned char byTxRate = 0, byRsvTime = 0;    // For OFDM
+
+	//Set to Page1
+	MACvSelectPage1(pDevice->PortOffset);
+
+	//RSPINF_b_1
+	BBvCalculateParameter(pDevice,
+			      14,
+			      VNTWIFIbyGetACKTxRate(RATE_1M, pvSupportRateIEs, pvExtSupportRateIEs),
+			      PK_TYPE_11B,
+			      &wLen,
+			      &byServ,
+			      &bySignal
+);
+
+	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_1, MAKEDWORD(wLen, MAKEWORD(bySignal, byServ)));
+	///RSPINF_b_2
+	BBvCalculateParameter(pDevice,
+			      14,
+			      VNTWIFIbyGetACKTxRate(RATE_2M, pvSupportRateIEs, pvExtSupportRateIEs),
+			      PK_TYPE_11B,
+			      &wLen,
+			      &byServ,
+			      &bySignal
+);
+
+	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_2, MAKEDWORD(wLen, MAKEWORD(bySignal, byServ)));
+	//RSPINF_b_5
+	BBvCalculateParameter(pDevice,
+			      14,
+			      VNTWIFIbyGetACKTxRate(RATE_5M, pvSupportRateIEs, pvExtSupportRateIEs),
+			      PK_TYPE_11B,
+			      &wLen,
+			      &byServ,
+			      &bySignal
+);
+
+	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_5, MAKEDWORD(wLen, MAKEWORD(bySignal, byServ)));
+	//RSPINF_b_11
+	BBvCalculateParameter(pDevice,
+			      14,
+			      VNTWIFIbyGetACKTxRate(RATE_11M, pvSupportRateIEs, pvExtSupportRateIEs),
+			      PK_TYPE_11B,
+			      &wLen,
+			      &byServ,
+			      &bySignal
+);
+
+	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_11, MAKEDWORD(wLen, MAKEWORD(bySignal, byServ)));
+	//RSPINF_a_6
+	s_vCalculateOFDMRParameter(RATE_6M,
+				   ePHYType,
+				   &byTxRate,
+				   &byRsvTime);
+	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_6, MAKEWORD(byTxRate, byRsvTime));
+	//RSPINF_a_9
+	s_vCalculateOFDMRParameter(RATE_9M,
+				   ePHYType,
+				   &byTxRate,
+				   &byRsvTime);
+	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_9, MAKEWORD(byTxRate, byRsvTime));
+	//RSPINF_a_12
+	s_vCalculateOFDMRParameter(RATE_12M,
+				   ePHYType,
+				   &byTxRate,
+				   &byRsvTime);
+	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_12, MAKEWORD(byTxRate, byRsvTime));
+	//RSPINF_a_18
+	s_vCalculateOFDMRParameter(RATE_18M,
+				   ePHYType,
+				   &byTxRate,
+				   &byRsvTime);
+	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_18, MAKEWORD(byTxRate, byRsvTime));
+	//RSPINF_a_24
+	s_vCalculateOFDMRParameter(RATE_24M,
+				   ePHYType,
+				   &byTxRate,
+				   &byRsvTime);
+	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_24, MAKEWORD(byTxRate, byRsvTime));
+	//RSPINF_a_36
+	s_vCalculateOFDMRParameter(
+		VNTWIFIbyGetACKTxRate(RATE_36M, pvSupportRateIEs, pvExtSupportRateIEs),
+		ePHYType,
+		&byTxRate,
+		&byRsvTime);
+	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_36, MAKEWORD(byTxRate, byRsvTime));
+	//RSPINF_a_48
+	s_vCalculateOFDMRParameter(
+		VNTWIFIbyGetACKTxRate(RATE_48M, pvSupportRateIEs, pvExtSupportRateIEs),
+		ePHYType,
+		&byTxRate,
+		&byRsvTime);
+	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_48, MAKEWORD(byTxRate, byRsvTime));
+	//RSPINF_a_54
+	s_vCalculateOFDMRParameter(
+		VNTWIFIbyGetACKTxRate(RATE_54M, pvSupportRateIEs, pvExtSupportRateIEs),
+		ePHYType,
+		&byTxRate,
+		&byRsvTime);
+	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_54, MAKEWORD(byTxRate, byRsvTime));
+	//RSPINF_a_72
+	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_72, MAKEWORD(byTxRate, byRsvTime));
+	//Set to Page0
+	MACvSelectPage0(pDevice->PortOffset);
 }
 
 /*---------------------  Export Functions  --------------------------*/
@@ -369,22 +354,20 @@ s_vSetRSPINF (PSDevice pDevice, CARD_PHY_TYPE ePHYType, void *pvSupportRateIEs,
  *
  */
 /*
-bool CARDbSendPacket (void *pDeviceHandler, void *pPacket, CARD_PKT_TYPE ePktType, unsigned int uLength)
-{
-    PSDevice    pDevice = (PSDevice) pDeviceHandler;
-    if (ePktType == PKT_TYPE_802_11_MNG) {
-        return TXbTD0Send(pDevice, pPacket, uLength);
-    } else if (ePktType == PKT_TYPE_802_11_BCN) {
-        return TXbBeaconSend(pDevice, pPacket, uLength);
-    } if (ePktType == PKT_TYPE_802_11_DATA) {
-        return TXbTD1Send(pDevice, pPacket, uLength);
-    }
-
-    return (true);
-}
+  bool CARDbSendPacket (void *pDeviceHandler, void *pPacket, CARD_PKT_TYPE ePktType, unsigned int uLength) {
+  PSDevice    pDevice = (PSDevice) pDeviceHandler;
+  if (ePktType == PKT_TYPE_802_11_MNG) {
+  return TXbTD0Send(pDevice, pPacket, uLength);
+  } else if (ePktType == PKT_TYPE_802_11_BCN) {
+  return TXbBeaconSend(pDevice, pPacket, uLength);
+  } if (ePktType == PKT_TYPE_802_11_DATA) {
+  return TXbTD1Send(pDevice, pPacket, uLength);
+  }
+
+  return true;
+  }
 */
 
-
 /*
  * Description: Get Card short preamble option value
  *
@@ -397,13 +380,13 @@ bool CARDbSendPacket (void *pDeviceHandler, void *pPacket, CARD_PKT_TYPE ePktTyp
  * Return Value: true if short preamble; otherwise false
  *
  */
-bool CARDbIsShortPreamble (void *pDeviceHandler)
+bool CARDbIsShortPreamble(void *pDeviceHandler)
 {
-    PSDevice    pDevice = (PSDevice) pDeviceHandler;
-    if (pDevice->byPreambleType == 0) {
-        return(false);
-    }
-    return(true);
+	PSDevice    pDevice = (PSDevice) pDeviceHandler;
+	if (pDevice->byPreambleType == 0) {
+		return false;
+	}
+	return true;
 }
 
 /*
@@ -418,13 +401,12 @@ bool CARDbIsShortPreamble (void *pDeviceHandler)
  * Return Value: true if short slot time; otherwise false
  *
  */
-bool CARDbIsShorSlotTime (void *pDeviceHandler)
+bool CARDbIsShorSlotTime(void *pDeviceHandler)
 {
-    PSDevice    pDevice = (PSDevice) pDeviceHandler;
-    return(pDevice->bShortSlotTime);
+	PSDevice    pDevice = (PSDevice) pDeviceHandler;
+	return pDevice->bShortSlotTime;
 }
 
-
 /*
  * Description: Update IFS
  *
@@ -437,175 +419,174 @@ bool CARDbIsShorSlotTime (void *pDeviceHandler)
  * Return Value: None.
  *
  */
-bool CARDbSetPhyParameter (void *pDeviceHandler, CARD_PHY_TYPE ePHYType, unsigned short wCapInfo, unsigned char byERPField, void *pvSupportRateIEs, void *pvExtSupportRateIEs)
+bool CARDbSetPhyParameter(void *pDeviceHandler, CARD_PHY_TYPE ePHYType, unsigned short wCapInfo, unsigned char byERPField, void *pvSupportRateIEs, void *pvExtSupportRateIEs)
 {
-    PSDevice    pDevice = (PSDevice) pDeviceHandler;
-    unsigned char byCWMaxMin = 0;
-    unsigned char bySlot = 0;
-    unsigned char bySIFS = 0;
-    unsigned char byDIFS = 0;
-    unsigned char byData;
+	PSDevice    pDevice = (PSDevice) pDeviceHandler;
+	unsigned char byCWMaxMin = 0;
+	unsigned char bySlot = 0;
+	unsigned char bySIFS = 0;
+	unsigned char byDIFS = 0;
+	unsigned char byData;
 //    PWLAN_IE_SUPP_RATES pRates = NULL;
-    PWLAN_IE_SUPP_RATES pSupportRates = (PWLAN_IE_SUPP_RATES) pvSupportRateIEs;
-    PWLAN_IE_SUPP_RATES pExtSupportRates = (PWLAN_IE_SUPP_RATES) pvExtSupportRateIEs;
-
-
-    //Set SIFS, DIFS, EIFS, SlotTime, CwMin
-    if (ePHYType == PHY_TYPE_11A) {
-        if (pSupportRates == NULL) {
-            pSupportRates = (PWLAN_IE_SUPP_RATES) abyDefaultSuppRatesA;
-        }
-        if (pDevice->byRFType == RF_AIROHA7230) {
-            // AL7230 use single PAPE and connect to PAPE_2.4G
-            MACvSetBBType(pDevice->PortOffset, BB_TYPE_11G);
-            pDevice->abyBBVGA[0] = 0x20;
-            pDevice->abyBBVGA[2] = 0x10;
-            pDevice->abyBBVGA[3] = 0x10;
-            BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
-            if (byData == 0x1C) {
-                BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
-            }
-        } else if (pDevice->byRFType == RF_UW2452) {
-            MACvSetBBType(pDevice->PortOffset, BB_TYPE_11A);
-            pDevice->abyBBVGA[0] = 0x18;
-            BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
-            if (byData == 0x14) {
-                BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
-                BBbWriteEmbedded(pDevice->PortOffset, 0xE1, 0x57);
-            }
-        } else {
-            MACvSetBBType(pDevice->PortOffset, BB_TYPE_11A);
-        }
-        BBbWriteEmbedded(pDevice->PortOffset, 0x88, 0x03);
-        bySlot = C_SLOT_SHORT;
-        bySIFS = C_SIFS_A;
-        byDIFS = C_SIFS_A + 2*C_SLOT_SHORT;
-        byCWMaxMin = 0xA4;
-    } else if (ePHYType == PHY_TYPE_11B) {
-        if (pSupportRates == NULL) {
-            pSupportRates = (PWLAN_IE_SUPP_RATES) abyDefaultSuppRatesB;
-        }
-        MACvSetBBType(pDevice->PortOffset, BB_TYPE_11B);
-        if (pDevice->byRFType == RF_AIROHA7230) {
-            pDevice->abyBBVGA[0] = 0x1C;
-            pDevice->abyBBVGA[2] = 0x00;
-            pDevice->abyBBVGA[3] = 0x00;
-            BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
-            if (byData == 0x20) {
-                BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
-            }
-        } else if (pDevice->byRFType == RF_UW2452) {
-            pDevice->abyBBVGA[0] = 0x14;
-            BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
-            if (byData == 0x18) {
-                BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
-                BBbWriteEmbedded(pDevice->PortOffset, 0xE1, 0xD3);
-            }
-        }
-        BBbWriteEmbedded(pDevice->PortOffset, 0x88, 0x02);
-        bySlot = C_SLOT_LONG;
-        bySIFS = C_SIFS_BG;
-        byDIFS = C_SIFS_BG + 2*C_SLOT_LONG;
-        byCWMaxMin = 0xA5;
-    } else {// PK_TYPE_11GA & PK_TYPE_11GB
-        if (pSupportRates == NULL) {
-            pSupportRates = (PWLAN_IE_SUPP_RATES) abyDefaultSuppRatesG;
-            pExtSupportRates = (PWLAN_IE_SUPP_RATES) abyDefaultExtSuppRatesG;
-        }
-        MACvSetBBType(pDevice->PortOffset, BB_TYPE_11G);
-        if (pDevice->byRFType == RF_AIROHA7230) {
-            pDevice->abyBBVGA[0] = 0x1C;
-            pDevice->abyBBVGA[2] = 0x00;
-            pDevice->abyBBVGA[3] = 0x00;
-            BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
-            if (byData == 0x20) {
-                BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
-            }
-        } else if (pDevice->byRFType == RF_UW2452) {
-            pDevice->abyBBVGA[0] = 0x14;
-            BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
-            if (byData == 0x18) {
-                BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
-                BBbWriteEmbedded(pDevice->PortOffset, 0xE1, 0xD3);
-            }
-        }
-        BBbWriteEmbedded(pDevice->PortOffset, 0x88, 0x08);
-        bySIFS = C_SIFS_BG;
-        if(VNTWIFIbIsShortSlotTime(wCapInfo)) {
-            bySlot = C_SLOT_SHORT;
-            byDIFS = C_SIFS_BG + 2*C_SLOT_SHORT;
-        } else {
-            bySlot = C_SLOT_LONG;
-            byDIFS = C_SIFS_BG + 2*C_SLOT_LONG;
-	    }
-        if (VNTWIFIbyGetMaxSupportRate(pSupportRates, pExtSupportRates) > RATE_11M) {
-            byCWMaxMin = 0xA4;
-        } else {
-            byCWMaxMin = 0xA5;
-        }
-        if (pDevice->bProtectMode != VNTWIFIbIsProtectMode(byERPField)) {
-            pDevice->bProtectMode = VNTWIFIbIsProtectMode(byERPField);
-            if (pDevice->bProtectMode) {
-                MACvEnableProtectMD(pDevice->PortOffset);
-            } else {
-                MACvDisableProtectMD(pDevice->PortOffset);
-            }
-        }
-        if (pDevice->bBarkerPreambleMd != VNTWIFIbIsBarkerMode(byERPField)) {
-            pDevice->bBarkerPreambleMd = VNTWIFIbIsBarkerMode(byERPField);
-            if (pDevice->bBarkerPreambleMd) {
-                MACvEnableBarkerPreambleMd(pDevice->PortOffset);
-            } else {
-                MACvDisableBarkerPreambleMd(pDevice->PortOffset);
-            }
-        }
-    }
-
-    if (pDevice->byRFType == RF_RFMD2959) {
-        // bcs TX_PE will reserve 3 us
-        // hardware's processing time here is 2 us.
-        bySIFS -= 3;
-        byDIFS -= 3;
-    //{{ RobertYu: 20041202
-    //// TX_PE will reserve 3 us for MAX2829 A mode only, it is for better TX throughput
-    //// MAC will need 2 us to process, so the SIFS, DIFS can be shorter by 2 us.
-    }
-
-    if (pDevice->bySIFS != bySIFS) {
-        pDevice->bySIFS = bySIFS;
-        VNSvOutPortB(pDevice->PortOffset + MAC_REG_SIFS, pDevice->bySIFS);
-    }
-    if (pDevice->byDIFS != byDIFS) {
-        pDevice->byDIFS = byDIFS;
-        VNSvOutPortB(pDevice->PortOffset + MAC_REG_DIFS, pDevice->byDIFS);
-    }
-    if (pDevice->byEIFS != C_EIFS) {
-        pDevice->byEIFS = C_EIFS;
-        VNSvOutPortB(pDevice->PortOffset + MAC_REG_EIFS, pDevice->byEIFS);
-    }
-    if (pDevice->bySlot != bySlot) {
-        pDevice->bySlot = bySlot;
-        VNSvOutPortB(pDevice->PortOffset + MAC_REG_SLOT, pDevice->bySlot);
-        if (pDevice->bySlot == C_SLOT_SHORT) {
-            pDevice->bShortSlotTime = true;
-        } else {
-            pDevice->bShortSlotTime = false;
-        }
-        BBvSetShortSlotTime(pDevice);
-    }
-    if (pDevice->byCWMaxMin != byCWMaxMin) {
-        pDevice->byCWMaxMin = byCWMaxMin;
-        VNSvOutPortB(pDevice->PortOffset + MAC_REG_CWMAXMIN0, pDevice->byCWMaxMin);
-    }
-    if (VNTWIFIbIsShortPreamble(wCapInfo)) {
-        pDevice->byPreambleType = pDevice->byShortPreamble;
-    } else {
-        pDevice->byPreambleType = 0;
-    }
-    s_vSetRSPINF(pDevice, ePHYType, pSupportRates, pExtSupportRates);
-    pDevice->eCurrentPHYType = ePHYType;
-    // set for NDIS OID_802_11SUPPORTED_RATES
-    return (true);
+	PWLAN_IE_SUPP_RATES pSupportRates = (PWLAN_IE_SUPP_RATES) pvSupportRateIEs;
+	PWLAN_IE_SUPP_RATES pExtSupportRates = (PWLAN_IE_SUPP_RATES) pvExtSupportRateIEs;
+
+	//Set SIFS, DIFS, EIFS, SlotTime, CwMin
+	if (ePHYType == PHY_TYPE_11A) {
+		if (pSupportRates == NULL) {
+			pSupportRates = (PWLAN_IE_SUPP_RATES) abyDefaultSuppRatesA;
+		}
+		if (pDevice->byRFType == RF_AIROHA7230) {
+			// AL7230 use single PAPE and connect to PAPE_2.4G
+			MACvSetBBType(pDevice->PortOffset, BB_TYPE_11G);
+			pDevice->abyBBVGA[0] = 0x20;
+			pDevice->abyBBVGA[2] = 0x10;
+			pDevice->abyBBVGA[3] = 0x10;
+			BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
+			if (byData == 0x1C) {
+				BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
+			}
+		} else if (pDevice->byRFType == RF_UW2452) {
+			MACvSetBBType(pDevice->PortOffset, BB_TYPE_11A);
+			pDevice->abyBBVGA[0] = 0x18;
+			BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
+			if (byData == 0x14) {
+				BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
+				BBbWriteEmbedded(pDevice->PortOffset, 0xE1, 0x57);
+			}
+		} else {
+			MACvSetBBType(pDevice->PortOffset, BB_TYPE_11A);
+		}
+		BBbWriteEmbedded(pDevice->PortOffset, 0x88, 0x03);
+		bySlot = C_SLOT_SHORT;
+		bySIFS = C_SIFS_A;
+		byDIFS = C_SIFS_A + 2*C_SLOT_SHORT;
+		byCWMaxMin = 0xA4;
+	} else if (ePHYType == PHY_TYPE_11B) {
+		if (pSupportRates == NULL) {
+			pSupportRates = (PWLAN_IE_SUPP_RATES) abyDefaultSuppRatesB;
+		}
+		MACvSetBBType(pDevice->PortOffset, BB_TYPE_11B);
+		if (pDevice->byRFType == RF_AIROHA7230) {
+			pDevice->abyBBVGA[0] = 0x1C;
+			pDevice->abyBBVGA[2] = 0x00;
+			pDevice->abyBBVGA[3] = 0x00;
+			BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
+			if (byData == 0x20) {
+				BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
+			}
+		} else if (pDevice->byRFType == RF_UW2452) {
+			pDevice->abyBBVGA[0] = 0x14;
+			BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
+			if (byData == 0x18) {
+				BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
+				BBbWriteEmbedded(pDevice->PortOffset, 0xE1, 0xD3);
+			}
+		}
+		BBbWriteEmbedded(pDevice->PortOffset, 0x88, 0x02);
+		bySlot = C_SLOT_LONG;
+		bySIFS = C_SIFS_BG;
+		byDIFS = C_SIFS_BG + 2*C_SLOT_LONG;
+		byCWMaxMin = 0xA5;
+	} else {// PK_TYPE_11GA & PK_TYPE_11GB
+		if (pSupportRates == NULL) {
+			pSupportRates = (PWLAN_IE_SUPP_RATES) abyDefaultSuppRatesG;
+			pExtSupportRates = (PWLAN_IE_SUPP_RATES) abyDefaultExtSuppRatesG;
+		}
+		MACvSetBBType(pDevice->PortOffset, BB_TYPE_11G);
+		if (pDevice->byRFType == RF_AIROHA7230) {
+			pDevice->abyBBVGA[0] = 0x1C;
+			pDevice->abyBBVGA[2] = 0x00;
+			pDevice->abyBBVGA[3] = 0x00;
+			BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
+			if (byData == 0x20) {
+				BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
+			}
+		} else if (pDevice->byRFType == RF_UW2452) {
+			pDevice->abyBBVGA[0] = 0x14;
+			BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
+			if (byData == 0x18) {
+				BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
+				BBbWriteEmbedded(pDevice->PortOffset, 0xE1, 0xD3);
+			}
+		}
+		BBbWriteEmbedded(pDevice->PortOffset, 0x88, 0x08);
+		bySIFS = C_SIFS_BG;
+		if (VNTWIFIbIsShortSlotTime(wCapInfo)) {
+			bySlot = C_SLOT_SHORT;
+			byDIFS = C_SIFS_BG + 2*C_SLOT_SHORT;
+		} else {
+			bySlot = C_SLOT_LONG;
+			byDIFS = C_SIFS_BG + 2*C_SLOT_LONG;
+		}
+		if (VNTWIFIbyGetMaxSupportRate(pSupportRates, pExtSupportRates) > RATE_11M) {
+			byCWMaxMin = 0xA4;
+		} else {
+			byCWMaxMin = 0xA5;
+		}
+		if (pDevice->bProtectMode != VNTWIFIbIsProtectMode(byERPField)) {
+			pDevice->bProtectMode = VNTWIFIbIsProtectMode(byERPField);
+			if (pDevice->bProtectMode) {
+				MACvEnableProtectMD(pDevice->PortOffset);
+			} else {
+				MACvDisableProtectMD(pDevice->PortOffset);
+			}
+		}
+		if (pDevice->bBarkerPreambleMd != VNTWIFIbIsBarkerMode(byERPField)) {
+			pDevice->bBarkerPreambleMd = VNTWIFIbIsBarkerMode(byERPField);
+			if (pDevice->bBarkerPreambleMd) {
+				MACvEnableBarkerPreambleMd(pDevice->PortOffset);
+			} else {
+				MACvDisableBarkerPreambleMd(pDevice->PortOffset);
+			}
+		}
+	}
+
+	if (pDevice->byRFType == RF_RFMD2959) {
+		// bcs TX_PE will reserve 3 us
+		// hardware's processing time here is 2 us.
+		bySIFS -= 3;
+		byDIFS -= 3;
+		//{{ RobertYu: 20041202
+		//// TX_PE will reserve 3 us for MAX2829 A mode only, it is for better TX throughput
+		//// MAC will need 2 us to process, so the SIFS, DIFS can be shorter by 2 us.
+	}
+
+	if (pDevice->bySIFS != bySIFS) {
+		pDevice->bySIFS = bySIFS;
+		VNSvOutPortB(pDevice->PortOffset + MAC_REG_SIFS, pDevice->bySIFS);
+	}
+	if (pDevice->byDIFS != byDIFS) {
+		pDevice->byDIFS = byDIFS;
+		VNSvOutPortB(pDevice->PortOffset + MAC_REG_DIFS, pDevice->byDIFS);
+	}
+	if (pDevice->byEIFS != C_EIFS) {
+		pDevice->byEIFS = C_EIFS;
+		VNSvOutPortB(pDevice->PortOffset + MAC_REG_EIFS, pDevice->byEIFS);
+	}
+	if (pDevice->bySlot != bySlot) {
+		pDevice->bySlot = bySlot;
+		VNSvOutPortB(pDevice->PortOffset + MAC_REG_SLOT, pDevice->bySlot);
+		if (pDevice->bySlot == C_SLOT_SHORT) {
+			pDevice->bShortSlotTime = true;
+		} else {
+			pDevice->bShortSlotTime = false;
+		}
+		BBvSetShortSlotTime(pDevice);
+	}
+	if (pDevice->byCWMaxMin != byCWMaxMin) {
+		pDevice->byCWMaxMin = byCWMaxMin;
+		VNSvOutPortB(pDevice->PortOffset + MAC_REG_CWMAXMIN0, pDevice->byCWMaxMin);
+	}
+	if (VNTWIFIbIsShortPreamble(wCapInfo)) {
+		pDevice->byPreambleType = pDevice->byShortPreamble;
+	} else {
+		pDevice->byPreambleType = 0;
+	}
+	s_vSetRSPINF(pDevice, ePHYType, pSupportRates, pExtSupportRates);
+	pDevice->eCurrentPHYType = ePHYType;
+	// set for NDIS OID_802_11SUPPORTED_RATES
+	return true;
 }
 
 /*
@@ -624,27 +605,26 @@ bool CARDbSetPhyParameter (void *pDeviceHandler, CARD_PHY_TYPE ePHYType, unsigne
  * Return Value: none
  *
  */
-bool CARDbUpdateTSF (void *pDeviceHandler, unsigned char byRxRate, QWORD qwBSSTimestamp, QWORD qwLocalTSF)
+bool CARDbUpdateTSF(void *pDeviceHandler, unsigned char byRxRate, QWORD qwBSSTimestamp, QWORD qwLocalTSF)
 {
-    PSDevice    pDevice = (PSDevice) pDeviceHandler;
-    QWORD       qwTSFOffset;
-
-    HIDWORD(qwTSFOffset) = 0;
-    LODWORD(qwTSFOffset) = 0;
-
-    if ((HIDWORD(qwBSSTimestamp) != HIDWORD(qwLocalTSF)) ||
-        (LODWORD(qwBSSTimestamp) != LODWORD(qwLocalTSF))) {
-        qwTSFOffset = CARDqGetTSFOffset(byRxRate, qwBSSTimestamp, qwLocalTSF);
-        // adjust TSF
-        // HW's TSF add TSF Offset reg
-        VNSvOutPortD(pDevice->PortOffset + MAC_REG_TSFOFST, LODWORD(qwTSFOffset));
-        VNSvOutPortD(pDevice->PortOffset + MAC_REG_TSFOFST + 4, HIDWORD(qwTSFOffset));
-        MACvRegBitsOn(pDevice->PortOffset, MAC_REG_TFTCTL, TFTCTL_TSFSYNCEN);
-    }
-    return(true);
+	PSDevice    pDevice = (PSDevice) pDeviceHandler;
+	QWORD       qwTSFOffset;
+
+	HIDWORD(qwTSFOffset) = 0;
+	LODWORD(qwTSFOffset) = 0;
+
+	if ((HIDWORD(qwBSSTimestamp) != HIDWORD(qwLocalTSF)) ||
+	    (LODWORD(qwBSSTimestamp) != LODWORD(qwLocalTSF))) {
+		qwTSFOffset = CARDqGetTSFOffset(byRxRate, qwBSSTimestamp, qwLocalTSF);
+		// adjust TSF
+		// HW's TSF add TSF Offset reg
+		VNSvOutPortD(pDevice->PortOffset + MAC_REG_TSFOFST, LODWORD(qwTSFOffset));
+		VNSvOutPortD(pDevice->PortOffset + MAC_REG_TSFOFST + 4, HIDWORD(qwTSFOffset));
+		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_TFTCTL, TFTCTL_TSFSYNCEN);
+	}
+	return true;
 }
 
-
 /*
  * Description: Set NIC TSF counter for first Beacon time
  *              Get NEXTTBTT from adjusted TSF and Beacon Interval
@@ -659,47 +639,45 @@ bool CARDbUpdateTSF (void *pDeviceHandler, unsigned char byRxRate, QWORD qwBSSTi
  * Return Value: true if succeed; otherwise false
  *
  */
-bool CARDbSetBeaconPeriod (void *pDeviceHandler, unsigned short wBeaconInterval)
+bool CARDbSetBeaconPeriod(void *pDeviceHandler, unsigned short wBeaconInterval)
 {
-    PSDevice    pDevice = (PSDevice) pDeviceHandler;
-    unsigned int uBeaconInterval = 0;
-    unsigned int uLowNextTBTT = 0;
-    unsigned int uHighRemain = 0;
-    unsigned int uLowRemain = 0;
-    QWORD       qwNextTBTT;
-
-    HIDWORD(qwNextTBTT) = 0;
-    LODWORD(qwNextTBTT) = 0;
-    CARDbGetCurrentTSF(pDevice->PortOffset, &qwNextTBTT); //Get Local TSF counter
-    uBeaconInterval = wBeaconInterval * 1024;
-    // Next TBTT = ((local_current_TSF / beacon_interval) + 1 ) * beacon_interval
-    uLowNextTBTT = (LODWORD(qwNextTBTT) >> 10) << 10;
-    uLowRemain = (uLowNextTBTT) % uBeaconInterval;
-    // high dword (mod) bcn
-    uHighRemain = (((0xffffffff % uBeaconInterval) + 1) * HIDWORD(qwNextTBTT))
-                  % uBeaconInterval;
-    uLowRemain = (uHighRemain + uLowRemain) % uBeaconInterval;
-    uLowRemain = uBeaconInterval - uLowRemain;
-
-    // check if carry when add one beacon interval
-    if ((~uLowNextTBTT) < uLowRemain) {
-        HIDWORD(qwNextTBTT) ++ ;
-    }
-    LODWORD(qwNextTBTT) = uLowNextTBTT + uLowRemain;
-
-    // set HW beacon interval
-    VNSvOutPortW(pDevice->PortOffset + MAC_REG_BI, wBeaconInterval);
-    pDevice->wBeaconInterval = wBeaconInterval;
-    // Set NextTBTT
-    VNSvOutPortD(pDevice->PortOffset + MAC_REG_NEXTTBTT, LODWORD(qwNextTBTT));
-    VNSvOutPortD(pDevice->PortOffset + MAC_REG_NEXTTBTT + 4, HIDWORD(qwNextTBTT));
-    MACvRegBitsOn(pDevice->PortOffset, MAC_REG_TFTCTL, TFTCTL_TBTTSYNCEN);
-
-    return(true);
+	PSDevice    pDevice = (PSDevice) pDeviceHandler;
+	unsigned int uBeaconInterval = 0;
+	unsigned int uLowNextTBTT = 0;
+	unsigned int uHighRemain = 0;
+	unsigned int uLowRemain = 0;
+	QWORD       qwNextTBTT;
+
+	HIDWORD(qwNextTBTT) = 0;
+	LODWORD(qwNextTBTT) = 0;
+	CARDbGetCurrentTSF(pDevice->PortOffset, &qwNextTBTT); //Get Local TSF counter
+	uBeaconInterval = wBeaconInterval * 1024;
+	// Next TBTT = ((local_current_TSF / beacon_interval) + 1) * beacon_interval
+	uLowNextTBTT = (LODWORD(qwNextTBTT) >> 10) << 10;
+	uLowRemain = (uLowNextTBTT) % uBeaconInterval;
+	// high dword (mod) bcn
+	uHighRemain = (((0xffffffff % uBeaconInterval) + 1) * HIDWORD(qwNextTBTT))
+		% uBeaconInterval;
+	uLowRemain = (uHighRemain + uLowRemain) % uBeaconInterval;
+	uLowRemain = uBeaconInterval - uLowRemain;
+
+	// check if carry when add one beacon interval
+	if ((~uLowNextTBTT) < uLowRemain) {
+		HIDWORD(qwNextTBTT)++;
+	}
+	LODWORD(qwNextTBTT) = uLowNextTBTT + uLowRemain;
+
+	// set HW beacon interval
+	VNSvOutPortW(pDevice->PortOffset + MAC_REG_BI, wBeaconInterval);
+	pDevice->wBeaconInterval = wBeaconInterval;
+	// Set NextTBTT
+	VNSvOutPortD(pDevice->PortOffset + MAC_REG_NEXTTBTT, LODWORD(qwNextTBTT));
+	VNSvOutPortD(pDevice->PortOffset + MAC_REG_NEXTTBTT + 4, HIDWORD(qwNextTBTT));
+	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_TFTCTL, TFTCTL_TBTTSYNCEN);
+
+	return true;
 }
 
-
-
 /*
  * Description: Card Stop Hardware Tx
  *
@@ -713,51 +691,49 @@ bool CARDbSetBeaconPeriod (void *pDeviceHandler, unsigned short wBeaconInterval)
  * Return Value: true if all data packet complete; otherwise false.
  *
  */
-bool CARDbStopTxPacket (void *pDeviceHandler, CARD_PKT_TYPE ePktType)
+bool CARDbStopTxPacket(void *pDeviceHandler, CARD_PKT_TYPE ePktType)
 {
-    PSDevice    pDevice = (PSDevice) pDeviceHandler;
-
-
-    if (ePktType == PKT_TYPE_802_11_ALL) {
-        pDevice->bStopBeacon = true;
-        pDevice->bStopTx0Pkt = true;
-        pDevice->bStopDataPkt = true;
-    } else if (ePktType == PKT_TYPE_802_11_BCN) {
-        pDevice->bStopBeacon = true;
-    } else if (ePktType == PKT_TYPE_802_11_MNG) {
-        pDevice->bStopTx0Pkt = true;
-    } else if (ePktType == PKT_TYPE_802_11_DATA) {
-        pDevice->bStopDataPkt = true;
-    }
-
-    if (pDevice->bStopBeacon == true) {
-        if (pDevice->bIsBeaconBufReadySet == true) {
-            if (pDevice->cbBeaconBufReadySetCnt < WAIT_BEACON_TX_DOWN_TMO) {
-                pDevice->cbBeaconBufReadySetCnt ++;
-                return(false);
-            }
-        }
-        pDevice->bIsBeaconBufReadySet = false;
-        pDevice->cbBeaconBufReadySetCnt = 0;
-        MACvRegBitsOff(pDevice->PortOffset, MAC_REG_TCR, TCR_AUTOBCNTX);
-    }
-    // wait all TD0 complete
-    if (pDevice->bStopTx0Pkt == true) {
-         if (pDevice->iTDUsed[TYPE_TXDMA0] != 0){
-            return(false);
-        }
-    }
-    // wait all Data TD complete
-    if (pDevice->bStopDataPkt == true) {
-        if (pDevice->iTDUsed[TYPE_AC0DMA] != 0){
-            return(false);
-        }
-    }
-
-    return(true);
+	PSDevice    pDevice = (PSDevice) pDeviceHandler;
+
+	if (ePktType == PKT_TYPE_802_11_ALL) {
+		pDevice->bStopBeacon = true;
+		pDevice->bStopTx0Pkt = true;
+		pDevice->bStopDataPkt = true;
+	} else if (ePktType == PKT_TYPE_802_11_BCN) {
+		pDevice->bStopBeacon = true;
+	} else if (ePktType == PKT_TYPE_802_11_MNG) {
+		pDevice->bStopTx0Pkt = true;
+	} else if (ePktType == PKT_TYPE_802_11_DATA) {
+		pDevice->bStopDataPkt = true;
+	}
+
+	if (pDevice->bStopBeacon == true) {
+		if (pDevice->bIsBeaconBufReadySet == true) {
+			if (pDevice->cbBeaconBufReadySetCnt < WAIT_BEACON_TX_DOWN_TMO) {
+				pDevice->cbBeaconBufReadySetCnt++;
+				return false;
+			}
+		}
+		pDevice->bIsBeaconBufReadySet = false;
+		pDevice->cbBeaconBufReadySetCnt = 0;
+		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_TCR, TCR_AUTOBCNTX);
+	}
+	// wait all TD0 complete
+	if (pDevice->bStopTx0Pkt == true) {
+		if (pDevice->iTDUsed[TYPE_TXDMA0] != 0) {
+			return false;
+		}
+	}
+	// wait all Data TD complete
+	if (pDevice->bStopDataPkt == true) {
+		if (pDevice->iTDUsed[TYPE_AC0DMA] != 0) {
+			return false;
+		}
+	}
+
+	return true;
 }
 
-
 /*
  * Description: Card Start Hardware Tx
  *
@@ -771,34 +747,31 @@ bool CARDbStopTxPacket (void *pDeviceHandler, CARD_PKT_TYPE ePktType)
  * Return Value: true if success; false if failed.
  *
  */
-bool CARDbStartTxPacket (void *pDeviceHandler, CARD_PKT_TYPE ePktType)
+bool CARDbStartTxPacket(void *pDeviceHandler, CARD_PKT_TYPE ePktType)
 {
-    PSDevice    pDevice = (PSDevice) pDeviceHandler;
-
-
-    if (ePktType == PKT_TYPE_802_11_ALL) {
-        pDevice->bStopBeacon = false;
-        pDevice->bStopTx0Pkt = false;
-        pDevice->bStopDataPkt = false;
-    } else if (ePktType == PKT_TYPE_802_11_BCN) {
-        pDevice->bStopBeacon = false;
-    } else if (ePktType == PKT_TYPE_802_11_MNG) {
-        pDevice->bStopTx0Pkt = false;
-    } else if (ePktType == PKT_TYPE_802_11_DATA) {
-        pDevice->bStopDataPkt = false;
-    }
-
-    if ((pDevice->bStopBeacon == false) &&
-        (pDevice->bBeaconBufReady == true) &&
-        (pDevice->eOPMode == OP_MODE_ADHOC)) {
-        MACvRegBitsOn(pDevice->PortOffset, MAC_REG_TCR, TCR_AUTOBCNTX);
-    }
-
-    return(true);
+	PSDevice    pDevice = (PSDevice) pDeviceHandler;
+
+	if (ePktType == PKT_TYPE_802_11_ALL) {
+		pDevice->bStopBeacon = false;
+		pDevice->bStopTx0Pkt = false;
+		pDevice->bStopDataPkt = false;
+	} else if (ePktType == PKT_TYPE_802_11_BCN) {
+		pDevice->bStopBeacon = false;
+	} else if (ePktType == PKT_TYPE_802_11_MNG) {
+		pDevice->bStopTx0Pkt = false;
+	} else if (ePktType == PKT_TYPE_802_11_DATA) {
+		pDevice->bStopDataPkt = false;
+	}
+
+	if ((pDevice->bStopBeacon == false) &&
+	    (pDevice->bBeaconBufReady == true) &&
+	    (pDevice->eOPMode == OP_MODE_ADHOC)) {
+		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_TCR, TCR_AUTOBCNTX);
+	}
+
+	return true;
 }
 
-
-
 /*
  * Description: Card Set BSSID value
  *
@@ -815,39 +788,38 @@ bool CARDbStartTxPacket (void *pDeviceHandler, CARD_PKT_TYPE ePktType)
  */
 bool CARDbSetBSSID(void *pDeviceHandler, unsigned char *pbyBSSID, CARD_OP_MODE eOPMode)
 {
-    PSDevice    pDevice = (PSDevice) pDeviceHandler;
-
-    MACvWriteBSSIDAddress(pDevice->PortOffset, pbyBSSID);
-    memcpy(pDevice->abyBSSID, pbyBSSID, WLAN_BSSID_LEN);
-    if (eOPMode == OP_MODE_ADHOC) {
-        MACvRegBitsOn(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_ADHOC);
-    } else {
-        MACvRegBitsOff(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_ADHOC);
-    }
-    if (eOPMode == OP_MODE_AP) {
-        MACvRegBitsOn(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_AP);
-    } else {
-        MACvRegBitsOff(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_AP);
-    }
-    if (eOPMode == OP_MODE_UNKNOWN) {
-        MACvRegBitsOff(pDevice->PortOffset, MAC_REG_RCR, RCR_BSSID);
-        pDevice->bBSSIDFilter = false;
-        pDevice->byRxMode &= ~RCR_BSSID;
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wcmd: rx_mode = %x\n", pDevice->byRxMode );
-    } else {
-        if (is_zero_ether_addr(pDevice->abyBSSID) == false) {
-            MACvRegBitsOn(pDevice->PortOffset, MAC_REG_RCR, RCR_BSSID);
-            pDevice->bBSSIDFilter = true;
-            pDevice->byRxMode |= RCR_BSSID;
-	    }
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wmgr: rx_mode = %x\n", pDevice->byRxMode );
-    }
-    // Adopt BSS state in Adapter Device Object
-    pDevice->eOPMode = eOPMode;
-    return(true);
+	PSDevice    pDevice = (PSDevice) pDeviceHandler;
+
+	MACvWriteBSSIDAddress(pDevice->PortOffset, pbyBSSID);
+	memcpy(pDevice->abyBSSID, pbyBSSID, WLAN_BSSID_LEN);
+	if (eOPMode == OP_MODE_ADHOC) {
+		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_ADHOC);
+	} else {
+		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_ADHOC);
+	}
+	if (eOPMode == OP_MODE_AP) {
+		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_AP);
+	} else {
+		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_AP);
+	}
+	if (eOPMode == OP_MODE_UNKNOWN) {
+		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_RCR, RCR_BSSID);
+		pDevice->bBSSIDFilter = false;
+		pDevice->byRxMode &= ~RCR_BSSID;
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wcmd: rx_mode = %x\n", pDevice->byRxMode);
+	} else {
+		if (is_zero_ether_addr(pDevice->abyBSSID) == false) {
+			MACvRegBitsOn(pDevice->PortOffset, MAC_REG_RCR, RCR_BSSID);
+			pDevice->bBSSIDFilter = true;
+			pDevice->byRxMode |= RCR_BSSID;
+		}
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wmgr: rx_mode = %x\n", pDevice->byRxMode);
+	}
+	// Adopt BSS state in Adapter Device Object
+	pDevice->eOPMode = eOPMode;
+	return true;
 }
 
-
 /*
  * Description: Card indicate status
  *
@@ -862,9 +834,6 @@ bool CARDbSetBSSID(void *pDeviceHandler, unsigned char *pbyBSSID, CARD_OP_MODE e
  *
  */
 
-
-
-
 /*
  * Description: Save Assoc info. contain in assoc. response frame
  *
@@ -883,14 +852,14 @@ bool CARDbSetBSSID(void *pDeviceHandler, unsigned char *pbyBSSID, CARD_OP_MODE e
  *
  */
 bool CARDbSetTxDataRate(
-    void *pDeviceHandler,
-    unsigned short wDataRate
-    )
+	void *pDeviceHandler,
+	unsigned short wDataRate
+)
 {
-    PSDevice    pDevice = (PSDevice) pDeviceHandler;
+	PSDevice    pDevice = (PSDevice) pDeviceHandler;
 
-    pDevice->wCurrentRate = wDataRate;
-    return(true);
+	pDevice->wCurrentRate = wDataRate;
+	return true;
 }
 
 /*+
@@ -906,32 +875,32 @@ bool CARDbSetTxDataRate(
  *
  * Return Value: true if power down success; otherwise false
  *
--*/
+ -*/
 bool
 CARDbPowerDown(
-    void *pDeviceHandler
-    )
+	void *pDeviceHandler
+)
 {
-    PSDevice        pDevice = (PSDevice)pDeviceHandler;
-    unsigned int uIdx;
+	PSDevice        pDevice = (PSDevice)pDeviceHandler;
+	unsigned int uIdx;
 
-    // check if already in Doze mode
-    if (MACbIsRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PS))
-        return true;
+	// check if already in Doze mode
+	if (MACbIsRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PS))
+		return true;
 
-    // Froce PSEN on
-    MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PSEN);
+	// Froce PSEN on
+	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PSEN);
 
-    // check if all TD are empty,
+	// check if all TD are empty,
 
-    for (uIdx = 0; uIdx < TYPE_MAXTD; uIdx ++) {
-        if (pDevice->iTDUsed[uIdx] != 0)
-            return false;
-    }
+	for (uIdx = 0; uIdx < TYPE_MAXTD; uIdx++) {
+		if (pDevice->iTDUsed[uIdx] != 0)
+			return false;
+	}
 
-    MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_GO2DOZE);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Go to Doze ZZZZZZZZZZZZZZZ\n");
-    return true;
+	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_GO2DOZE);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Go to Doze ZZZZZZZZZZZZZZZ\n");
+	return true;
 }
 
 /*
@@ -946,43 +915,40 @@ CARDbPowerDown(
  * Return Value: true if success; otherwise false
  *
  */
-bool CARDbRadioPowerOff (void *pDeviceHandler)
+bool CARDbRadioPowerOff(void *pDeviceHandler)
 {
-    PSDevice    pDevice = (PSDevice) pDeviceHandler;
-    bool bResult = true;
-
-    if (pDevice->bRadioOff == true)
-        return true;
+	PSDevice    pDevice = (PSDevice)pDeviceHandler;
+	bool bResult = true;
 
+	if (pDevice->bRadioOff == true)
+		return true;
 
-    switch (pDevice->byRFType) {
+	switch (pDevice->byRFType) {
+	case RF_RFMD2959:
+		MACvWordRegBitsOff(pDevice->PortOffset, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_TXPEINV);
+		MACvWordRegBitsOn(pDevice->PortOffset, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE1);
+		break;
 
-        case RF_RFMD2959:
-            MACvWordRegBitsOff(pDevice->PortOffset, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_TXPEINV);
-            MACvWordRegBitsOn(pDevice->PortOffset, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE1);
-            break;
+	case RF_AIROHA:
+	case RF_AL2230S:
+	case RF_AIROHA7230: //RobertYu:20050104
+		MACvWordRegBitsOff(pDevice->PortOffset, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE2);
+		MACvWordRegBitsOff(pDevice->PortOffset, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE3);
+		break;
 
-        case RF_AIROHA:
-        case RF_AL2230S:
-        case RF_AIROHA7230: //RobertYu:20050104
-            MACvWordRegBitsOff(pDevice->PortOffset, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE2);
-            MACvWordRegBitsOff(pDevice->PortOffset, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE3);
-            break;
+	}
 
-    }
+	MACvRegBitsOff(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_RXON);
 
-    MACvRegBitsOff(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_RXON);
+	BBvSetDeepSleep(pDevice->PortOffset, pDevice->byLocalID);
 
-    BBvSetDeepSleep(pDevice->PortOffset, pDevice->byLocalID);
-
-    pDevice->bRadioOff = true;
-     //2007-0409-03,<Add> by chester
-printk("chester power off\n");
-MACvRegBitsOn(pDevice->PortOffset, MAC_REG_GPIOCTL0, LED_ACTSET);  //LED issue
-    return bResult;
+	pDevice->bRadioOff = true;
+	//2007-0409-03,<Add> by chester
+	printk("chester power off\n");
+	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_GPIOCTL0, LED_ACTSET);  //LED issue
+	return bResult;
 }
 
-
 /*
  * Description: Turn on Radio power
  *
@@ -995,59 +961,54 @@ MACvRegBitsOn(pDevice->PortOffset, MAC_REG_GPIOCTL0, LED_ACTSET);  //LED issue
  * Return Value: true if success; otherwise false
  *
  */
-bool CARDbRadioPowerOn (void *pDeviceHandler)
+bool CARDbRadioPowerOn(void *pDeviceHandler)
 {
-    PSDevice    pDevice = (PSDevice) pDeviceHandler;
-    bool bResult = true;
-printk("chester power on\n");
-    if (pDevice->bRadioControlOff == true){
-if (pDevice->bHWRadioOff == true) printk("chester bHWRadioOff\n");
-if (pDevice->bRadioControlOff == true) printk("chester bRadioControlOff\n");
-        return false;}
-
-    if (pDevice->bRadioOff == false)
-       {
-printk("chester pbRadioOff\n");
-return true;}
-
-    BBvExitDeepSleep(pDevice->PortOffset, pDevice->byLocalID);
-
-    MACvRegBitsOn(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_RXON);
-
-    switch (pDevice->byRFType) {
-
-        case RF_RFMD2959:
-            MACvWordRegBitsOn(pDevice->PortOffset, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_TXPEINV);
-            MACvWordRegBitsOff(pDevice->PortOffset, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE1);
-            break;
-
-        case RF_AIROHA:
-        case RF_AL2230S:
-        case RF_AIROHA7230: //RobertYu:20050104
-            MACvWordRegBitsOn(pDevice->PortOffset, MAC_REG_SOFTPWRCTL, (SOFTPWRCTL_SWPE2 |
-                                                                        SOFTPWRCTL_SWPE3));
-            break;
-
-    }
-
-    pDevice->bRadioOff = false;
+	PSDevice    pDevice = (PSDevice) pDeviceHandler;
+	bool bResult = true;
+	printk("chester power on\n");
+	if (pDevice->bRadioControlOff == true) {
+		if (pDevice->bHWRadioOff == true) printk("chester bHWRadioOff\n");
+		if (pDevice->bRadioControlOff == true) printk("chester bRadioControlOff\n");
+		return false; }
+
+	if (pDevice->bRadioOff == false) {
+		printk("chester pbRadioOff\n");
+		return true; }
+
+	BBvExitDeepSleep(pDevice->PortOffset, pDevice->byLocalID);
+
+	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_RXON);
+
+	switch (pDevice->byRFType) {
+	case RF_RFMD2959:
+		MACvWordRegBitsOn(pDevice->PortOffset, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_TXPEINV);
+		MACvWordRegBitsOff(pDevice->PortOffset, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE1);
+		break;
+
+	case RF_AIROHA:
+	case RF_AL2230S:
+	case RF_AIROHA7230: //RobertYu:20050104
+		MACvWordRegBitsOn(pDevice->PortOffset, MAC_REG_SOFTPWRCTL, (SOFTPWRCTL_SWPE2 |
+									    SOFTPWRCTL_SWPE3));
+		break;
+
+	}
+
+	pDevice->bRadioOff = false;
 //  2007-0409-03,<Add> by chester
-printk("chester power on\n");
-MACvRegBitsOff(pDevice->PortOffset, MAC_REG_GPIOCTL0, LED_ACTSET); //LED issue
-    return bResult;
+	printk("chester power on\n");
+	MACvRegBitsOff(pDevice->PortOffset, MAC_REG_GPIOCTL0, LED_ACTSET); //LED issue
+	return bResult;
 }
 
-
-
-bool CARDbRemoveKey (void *pDeviceHandler, unsigned char *pbyBSSID)
+bool CARDbRemoveKey(void *pDeviceHandler, unsigned char *pbyBSSID)
 {
-    PSDevice    pDevice = (PSDevice) pDeviceHandler;
+	PSDevice    pDevice = (PSDevice) pDeviceHandler;
 
-    KeybRemoveAllKey(&(pDevice->sKey), pbyBSSID, pDevice->PortOffset);
-    return (true);
+	KeybRemoveAllKey(&(pDevice->sKey), pbyBSSID, pDevice->PortOffset);
+	return true;
 }
 
-
 /*
  *
  * Description:
@@ -1063,66 +1024,65 @@ bool CARDbRemoveKey (void *pDeviceHandler, unsigned char *pbyBSSID)
  *
  * Return Value: none.
  *
--*/
+ -*/
 bool
-CARDbAdd_PMKID_Candidate (
-    void *pDeviceHandler,
-    unsigned char *pbyBSSID,
-    bool bRSNCapExist,
-    unsigned short wRSNCap
-    )
+CARDbAdd_PMKID_Candidate(
+	void *pDeviceHandler,
+	unsigned char *pbyBSSID,
+	bool bRSNCapExist,
+	unsigned short wRSNCap
+)
 {
-    PSDevice            pDevice = (PSDevice) pDeviceHandler;
-    PPMKID_CANDIDATE    pCandidateList;
-    unsigned int ii = 0;
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"bAdd_PMKID_Candidate START: (%d)\n", (int)pDevice->gsPMKIDCandidate.NumCandidates);
-
-    if (pDevice->gsPMKIDCandidate.NumCandidates >= MAX_PMKIDLIST) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"vFlush_PMKID_Candidate: 3\n");
-        memset(&pDevice->gsPMKIDCandidate, 0, sizeof(SPMKIDCandidateEvent));
-    }
-
-    for (ii = 0; ii < 6; ii++) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"%02X ", *(pbyBSSID + ii));
-    }
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"\n");
-
-
-    // Update Old Candidate
-    for (ii = 0; ii < pDevice->gsPMKIDCandidate.NumCandidates; ii++) {
-        pCandidateList = &pDevice->gsPMKIDCandidate.CandidateList[ii];
-        if ( !memcmp(pCandidateList->BSSID, pbyBSSID, ETH_ALEN)) {
-            if ((bRSNCapExist == true) && (wRSNCap & BIT0)) {
-                pCandidateList->Flags |= NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED;
-            } else {
-                pCandidateList->Flags &= ~(NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED);
-            }
-            return true;
-        }
-    }
-
-    // New Candidate
-    pCandidateList = &pDevice->gsPMKIDCandidate.CandidateList[pDevice->gsPMKIDCandidate.NumCandidates];
-    if ((bRSNCapExist == true) && (wRSNCap & BIT0)) {
-        pCandidateList->Flags |= NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED;
-    } else {
-        pCandidateList->Flags &= ~(NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED);
-    }
-    memcpy(pCandidateList->BSSID, pbyBSSID, ETH_ALEN);
-    pDevice->gsPMKIDCandidate.NumCandidates++;
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"NumCandidates:%d\n", (int)pDevice->gsPMKIDCandidate.NumCandidates);
-    return true;
+	PSDevice            pDevice = (PSDevice) pDeviceHandler;
+	PPMKID_CANDIDATE    pCandidateList;
+	unsigned int ii = 0;
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "bAdd_PMKID_Candidate START: (%d)\n", (int)pDevice->gsPMKIDCandidate.NumCandidates);
+
+	if (pDevice->gsPMKIDCandidate.NumCandidates >= MAX_PMKIDLIST) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "vFlush_PMKID_Candidate: 3\n");
+		memset(&pDevice->gsPMKIDCandidate, 0, sizeof(SPMKIDCandidateEvent));
+	}
+
+	for (ii = 0; ii < 6; ii++) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02X ", *(pbyBSSID + ii));
+	}
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
+
+	// Update Old Candidate
+	for (ii = 0; ii < pDevice->gsPMKIDCandidate.NumCandidates; ii++) {
+		pCandidateList = &pDevice->gsPMKIDCandidate.CandidateList[ii];
+		if (!memcmp(pCandidateList->BSSID, pbyBSSID, ETH_ALEN)) {
+			if ((bRSNCapExist == true) && (wRSNCap & BIT0)) {
+				pCandidateList->Flags |= NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED;
+			} else {
+				pCandidateList->Flags &= ~(NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED);
+			}
+			return true;
+		}
+	}
+
+	// New Candidate
+	pCandidateList = &pDevice->gsPMKIDCandidate.CandidateList[pDevice->gsPMKIDCandidate.NumCandidates];
+	if ((bRSNCapExist == true) && (wRSNCap & BIT0)) {
+		pCandidateList->Flags |= NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED;
+	} else {
+		pCandidateList->Flags &= ~(NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED);
+	}
+	memcpy(pCandidateList->BSSID, pbyBSSID, ETH_ALEN);
+	pDevice->gsPMKIDCandidate.NumCandidates++;
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "NumCandidates:%d\n", (int)pDevice->gsPMKIDCandidate.NumCandidates);
+	return true;
 }
 
 void *
-CARDpGetCurrentAddress (
-    void *pDeviceHandler
-    )
+CARDpGetCurrentAddress(
+	void *pDeviceHandler
+)
 {
-    PSDevice            pDevice = (PSDevice) pDeviceHandler;
+	PSDevice            pDevice = (PSDevice) pDeviceHandler;
 
-    return (pDevice->abyCurrentNetAddr);
+	return pDevice->abyCurrentNetAddr;
 }
 
 /*
@@ -1138,120 +1098,119 @@ CARDpGetCurrentAddress (
  *
  * Return Value: none.
  *
--*/
+ -*/
 bool
-CARDbStartMeasure (
-    void *pDeviceHandler,
-    void *pvMeasureEIDs,
-    unsigned int uNumOfMeasureEIDs
-    )
+CARDbStartMeasure(
+	void *pDeviceHandler,
+	void *pvMeasureEIDs,
+	unsigned int uNumOfMeasureEIDs
+)
 {
-    PSDevice                pDevice = (PSDevice) pDeviceHandler;
-    PWLAN_IE_MEASURE_REQ    pEID = (PWLAN_IE_MEASURE_REQ) pvMeasureEIDs;
-    QWORD                   qwCurrTSF;
-    QWORD                   qwStartTSF;
-    bool bExpired = true;
-    unsigned short wDuration = 0;
-
-    if ((pEID == NULL) ||
-        (uNumOfMeasureEIDs == 0)) {
-        return (true);
-    }
-    CARDbGetCurrentTSF(pDevice->PortOffset, &qwCurrTSF);
-    if (pDevice->bMeasureInProgress == true) {
-        pDevice->bMeasureInProgress = false;
-        VNSvOutPortB(pDevice->PortOffset + MAC_REG_RCR, pDevice->byOrgRCR);
-        MACvSelectPage1(pDevice->PortOffset);
-        VNSvOutPortD(pDevice->PortOffset + MAC_REG_MAR0, pDevice->dwOrgMAR0);
-        VNSvOutPortD(pDevice->PortOffset + MAC_REG_MAR4, pDevice->dwOrgMAR4);
-        // clear measure control
-        MACvRegBitsOff(pDevice->PortOffset, MAC_REG_MSRCTL, MSRCTL_EN);
-        MACvSelectPage0(pDevice->PortOffset);
-        set_channel(pDevice, pDevice->byOrgChannel);
-        MACvSelectPage1(pDevice->PortOffset);
-        MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL+1, MSRCTL1_TXPAUSE);
-        MACvSelectPage0(pDevice->PortOffset);
-    }
-    pDevice->uNumOfMeasureEIDs = uNumOfMeasureEIDs;
-
-    do {
-        pDevice->pCurrMeasureEID = pEID;
-        pEID++;
-        pDevice->uNumOfMeasureEIDs--;
-
-        if (pDevice->byLocalID > REV_ID_VT3253_B1) {
-            HIDWORD(qwStartTSF) = HIDWORD(*((PQWORD) (pDevice->pCurrMeasureEID->sReq.abyStartTime)));
-            LODWORD(qwStartTSF) = LODWORD(*((PQWORD) (pDevice->pCurrMeasureEID->sReq.abyStartTime)));
-            wDuration = *((unsigned short *) (pDevice->pCurrMeasureEID->sReq.abyDuration));
-            wDuration += 1; // 1 TU for channel switching
-
-            if ((LODWORD(qwStartTSF) == 0) && (HIDWORD(qwStartTSF) == 0)) {
-                // start immediately by setting start TSF == current TSF + 2 TU
-                LODWORD(qwStartTSF) = LODWORD(qwCurrTSF) + 2048;
-                HIDWORD(qwStartTSF) = HIDWORD(qwCurrTSF);
-                if (LODWORD(qwCurrTSF) > LODWORD(qwStartTSF)) {
-                    HIDWORD(qwStartTSF)++;
-                }
-                bExpired = false;
-                break;
-            } else {
-                // start at setting start TSF - 1TU(for channel switching)
-                if (LODWORD(qwStartTSF) < 1024) {
-                    HIDWORD(qwStartTSF)--;
-                }
-                LODWORD(qwStartTSF) -= 1024;
-            }
-
-            if ((HIDWORD(qwCurrTSF) < HIDWORD(qwStartTSF)) ||
-                ((HIDWORD(qwCurrTSF) == HIDWORD(qwStartTSF)) &&
-                (LODWORD(qwCurrTSF) < LODWORD(qwStartTSF)))
-                ) {
-                bExpired = false;
-                break;
-            }
-            VNTWIFIbMeasureReport(  pDevice->pMgmt,
-                                    false,
-                                    pDevice->pCurrMeasureEID,
-                                    MEASURE_MODE_LATE,
-                                    pDevice->byBasicMap,
-                                    pDevice->byCCAFraction,
-                                    pDevice->abyRPIs
-                                    );
-        } else {
-            // hardware do not support measure
-            VNTWIFIbMeasureReport(  pDevice->pMgmt,
-                                    false,
-                                    pDevice->pCurrMeasureEID,
-                                    MEASURE_MODE_INCAPABLE,
-                                    pDevice->byBasicMap,
-                                    pDevice->byCCAFraction,
-                                    pDevice->abyRPIs
-                                    );
-        }
-    } while (pDevice->uNumOfMeasureEIDs != 0);
-
-    if (bExpired == false) {
-        MACvSelectPage1(pDevice->PortOffset);
-        VNSvOutPortD(pDevice->PortOffset + MAC_REG_MSRSTART, LODWORD(qwStartTSF));
-        VNSvOutPortD(pDevice->PortOffset + MAC_REG_MSRSTART + 4, HIDWORD(qwStartTSF));
-        VNSvOutPortW(pDevice->PortOffset + MAC_REG_MSRDURATION, wDuration);
-        MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL, MSRCTL_EN);
-        MACvSelectPage0(pDevice->PortOffset);
-    } else {
-        // all measure start time expired we should complete action
-        VNTWIFIbMeasureReport(  pDevice->pMgmt,
-                                true,
-                                NULL,
-                                0,
-                                pDevice->byBasicMap,
-                                pDevice->byCCAFraction,
-                                pDevice->abyRPIs
-                                );
-    }
-    return (true);
+	PSDevice                pDevice = (PSDevice) pDeviceHandler;
+	PWLAN_IE_MEASURE_REQ    pEID = (PWLAN_IE_MEASURE_REQ) pvMeasureEIDs;
+	QWORD                   qwCurrTSF;
+	QWORD                   qwStartTSF;
+	bool bExpired = true;
+	unsigned short wDuration = 0;
+
+	if ((pEID == NULL) ||
+	    (uNumOfMeasureEIDs == 0)) {
+		return true;
+	}
+	CARDbGetCurrentTSF(pDevice->PortOffset, &qwCurrTSF);
+	if (pDevice->bMeasureInProgress == true) {
+		pDevice->bMeasureInProgress = false;
+		VNSvOutPortB(pDevice->PortOffset + MAC_REG_RCR, pDevice->byOrgRCR);
+		MACvSelectPage1(pDevice->PortOffset);
+		VNSvOutPortD(pDevice->PortOffset + MAC_REG_MAR0, pDevice->dwOrgMAR0);
+		VNSvOutPortD(pDevice->PortOffset + MAC_REG_MAR4, pDevice->dwOrgMAR4);
+		// clear measure control
+		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_MSRCTL, MSRCTL_EN);
+		MACvSelectPage0(pDevice->PortOffset);
+		set_channel(pDevice, pDevice->byOrgChannel);
+		MACvSelectPage1(pDevice->PortOffset);
+		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL+1, MSRCTL1_TXPAUSE);
+		MACvSelectPage0(pDevice->PortOffset);
+	}
+	pDevice->uNumOfMeasureEIDs = uNumOfMeasureEIDs;
+
+	do {
+		pDevice->pCurrMeasureEID = pEID;
+		pEID++;
+		pDevice->uNumOfMeasureEIDs--;
+
+		if (pDevice->byLocalID > REV_ID_VT3253_B1) {
+			HIDWORD(qwStartTSF) = HIDWORD(*((PQWORD)(pDevice->pCurrMeasureEID->sReq.abyStartTime)));
+			LODWORD(qwStartTSF) = LODWORD(*((PQWORD)(pDevice->pCurrMeasureEID->sReq.abyStartTime)));
+			wDuration = *((unsigned short *)(pDevice->pCurrMeasureEID->sReq.abyDuration));
+			wDuration += 1; // 1 TU for channel switching
+
+			if ((LODWORD(qwStartTSF) == 0) && (HIDWORD(qwStartTSF) == 0)) {
+				// start immediately by setting start TSF == current TSF + 2 TU
+				LODWORD(qwStartTSF) = LODWORD(qwCurrTSF) + 2048;
+				HIDWORD(qwStartTSF) = HIDWORD(qwCurrTSF);
+				if (LODWORD(qwCurrTSF) > LODWORD(qwStartTSF)) {
+					HIDWORD(qwStartTSF)++;
+				}
+				bExpired = false;
+				break;
+			} else {
+				// start at setting start TSF - 1TU(for channel switching)
+				if (LODWORD(qwStartTSF) < 1024) {
+					HIDWORD(qwStartTSF)--;
+				}
+				LODWORD(qwStartTSF) -= 1024;
+			}
+
+			if ((HIDWORD(qwCurrTSF) < HIDWORD(qwStartTSF)) ||
+			    ((HIDWORD(qwCurrTSF) == HIDWORD(qwStartTSF)) &&
+			     (LODWORD(qwCurrTSF) < LODWORD(qwStartTSF)))
+) {
+				bExpired = false;
+				break;
+			}
+			VNTWIFIbMeasureReport(pDevice->pMgmt,
+					      false,
+					      pDevice->pCurrMeasureEID,
+					      MEASURE_MODE_LATE,
+					      pDevice->byBasicMap,
+					      pDevice->byCCAFraction,
+					      pDevice->abyRPIs
+				);
+		} else {
+			// hardware do not support measure
+			VNTWIFIbMeasureReport(pDevice->pMgmt,
+					      false,
+					      pDevice->pCurrMeasureEID,
+					      MEASURE_MODE_INCAPABLE,
+					      pDevice->byBasicMap,
+					      pDevice->byCCAFraction,
+					      pDevice->abyRPIs
+				);
+		}
+	} while (pDevice->uNumOfMeasureEIDs != 0);
+
+	if (bExpired == false) {
+		MACvSelectPage1(pDevice->PortOffset);
+		VNSvOutPortD(pDevice->PortOffset + MAC_REG_MSRSTART, LODWORD(qwStartTSF));
+		VNSvOutPortD(pDevice->PortOffset + MAC_REG_MSRSTART + 4, HIDWORD(qwStartTSF));
+		VNSvOutPortW(pDevice->PortOffset + MAC_REG_MSRDURATION, wDuration);
+		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL, MSRCTL_EN);
+		MACvSelectPage0(pDevice->PortOffset);
+	} else {
+		// all measure start time expired we should complete action
+		VNTWIFIbMeasureReport(pDevice->pMgmt,
+				      true,
+				      NULL,
+				      0,
+				      pDevice->byBasicMap,
+				      pDevice->byCCAFraction,
+				      pDevice->abyRPIs
+			);
+	}
+	return true;
 }
 
-
 /*
  *
  * Description:
@@ -1265,36 +1224,35 @@ CARDbStartMeasure (
  *
  * Return Value: none.
  *
--*/
+ -*/
 bool
-CARDbChannelSwitch (
-    void *pDeviceHandler,
-    unsigned char byMode,
-    unsigned char byNewChannel,
-    unsigned char byCount
-    )
+CARDbChannelSwitch(
+	void *pDeviceHandler,
+	unsigned char byMode,
+	unsigned char byNewChannel,
+	unsigned char byCount
+)
 {
-    PSDevice    pDevice = (PSDevice) pDeviceHandler;
-    bool bResult = true;
-
-    if (byCount == 0) {
-        bResult = set_channel(pDevice, byNewChannel);
-        VNTWIFIbChannelSwitch(pDevice->pMgmt, byNewChannel);
-        MACvSelectPage1(pDevice->PortOffset);
-        MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL+1, MSRCTL1_TXPAUSE);
-        MACvSelectPage0(pDevice->PortOffset);
-        return(bResult);
-    }
-    pDevice->byChannelSwitchCount = byCount;
-    pDevice->byNewChannel = byNewChannel;
-    pDevice->bChannelSwitch = true;
-    if (byMode == 1) {
-        bResult=CARDbStopTxPacket(pDevice, PKT_TYPE_802_11_ALL);
-    }
-    return (bResult);
+	PSDevice    pDevice = (PSDevice) pDeviceHandler;
+	bool bResult = true;
+
+	if (byCount == 0) {
+		bResult = set_channel(pDevice, byNewChannel);
+		VNTWIFIbChannelSwitch(pDevice->pMgmt, byNewChannel);
+		MACvSelectPage1(pDevice->PortOffset);
+		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL+1, MSRCTL1_TXPAUSE);
+		MACvSelectPage0(pDevice->PortOffset);
+		return bResult;
+	}
+	pDevice->byChannelSwitchCount = byCount;
+	pDevice->byNewChannel = byNewChannel;
+	pDevice->bChannelSwitch = true;
+	if (byMode == 1) {
+		bResult = CARDbStopTxPacket(pDevice, PKT_TYPE_802_11_ALL);
+	}
+	return bResult;
 }
 
-
 /*
  *
  * Description:
@@ -1308,53 +1266,52 @@ CARDbChannelSwitch (
  *
  * Return Value: none.
  *
--*/
+ -*/
 bool
-CARDbSetQuiet (
-    void *pDeviceHandler,
-    bool bResetQuiet,
-    unsigned char byQuietCount,
-    unsigned char byQuietPeriod,
-    unsigned short wQuietDuration,
-    unsigned short wQuietOffset
-    )
+CARDbSetQuiet(
+	void *pDeviceHandler,
+	bool bResetQuiet,
+	unsigned char byQuietCount,
+	unsigned char byQuietPeriod,
+	unsigned short wQuietDuration,
+	unsigned short wQuietOffset
+)
 {
-    PSDevice    pDevice = (PSDevice) pDeviceHandler;
-    unsigned int ii = 0;
-
-    if (bResetQuiet == true) {
-        MACvRegBitsOff(pDevice->PortOffset, MAC_REG_MSRCTL, (MSRCTL_QUIETTXCHK | MSRCTL_QUIETEN));
-        for(ii=0;ii<MAX_QUIET_COUNT;ii++) {
-            pDevice->sQuiet[ii].bEnable = false;
-        }
-        pDevice->uQuietEnqueue = 0;
-        pDevice->bEnableFirstQuiet = false;
-        pDevice->bQuietEnable = false;
-        pDevice->byQuietStartCount = byQuietCount;
-    }
-    if (pDevice->sQuiet[pDevice->uQuietEnqueue].bEnable == false) {
-        pDevice->sQuiet[pDevice->uQuietEnqueue].bEnable = true;
-        pDevice->sQuiet[pDevice->uQuietEnqueue].byPeriod = byQuietPeriod;
-        pDevice->sQuiet[pDevice->uQuietEnqueue].wDuration = wQuietDuration;
-        pDevice->sQuiet[pDevice->uQuietEnqueue].dwStartTime = (unsigned long) byQuietCount;
-        pDevice->sQuiet[pDevice->uQuietEnqueue].dwStartTime *= pDevice->wBeaconInterval;
-        pDevice->sQuiet[pDevice->uQuietEnqueue].dwStartTime += wQuietOffset;
-        pDevice->uQuietEnqueue++;
-        pDevice->uQuietEnqueue %= MAX_QUIET_COUNT;
-        if (pDevice->byQuietStartCount < byQuietCount) {
-            pDevice->byQuietStartCount = byQuietCount;
-        }
-    } else {
-        // we can not handle Quiet EID more
-    }
-    return (true);
+	PSDevice    pDevice = (PSDevice) pDeviceHandler;
+	unsigned int ii = 0;
+
+	if (bResetQuiet == true) {
+		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_MSRCTL, (MSRCTL_QUIETTXCHK | MSRCTL_QUIETEN));
+		for (ii = 0; ii < MAX_QUIET_COUNT; ii++) {
+			pDevice->sQuiet[ii].bEnable = false;
+		}
+		pDevice->uQuietEnqueue = 0;
+		pDevice->bEnableFirstQuiet = false;
+		pDevice->bQuietEnable = false;
+		pDevice->byQuietStartCount = byQuietCount;
+	}
+	if (pDevice->sQuiet[pDevice->uQuietEnqueue].bEnable == false) {
+		pDevice->sQuiet[pDevice->uQuietEnqueue].bEnable = true;
+		pDevice->sQuiet[pDevice->uQuietEnqueue].byPeriod = byQuietPeriod;
+		pDevice->sQuiet[pDevice->uQuietEnqueue].wDuration = wQuietDuration;
+		pDevice->sQuiet[pDevice->uQuietEnqueue].dwStartTime = (unsigned long) byQuietCount;
+		pDevice->sQuiet[pDevice->uQuietEnqueue].dwStartTime *= pDevice->wBeaconInterval;
+		pDevice->sQuiet[pDevice->uQuietEnqueue].dwStartTime += wQuietOffset;
+		pDevice->uQuietEnqueue++;
+		pDevice->uQuietEnqueue %= MAX_QUIET_COUNT;
+		if (pDevice->byQuietStartCount < byQuietCount) {
+			pDevice->byQuietStartCount = byQuietCount;
+		}
+	} else {
+		// we can not handle Quiet EID more
+	}
+	return true;
 }
 
-
 /*
  *
  * Description:
- *    Do Quiet, It will be called by either ISR(after start) 
+ *    Do Quiet, It will be called by either ISR(after start)
  *    or VNTWIFI(before start) so we do not need a SPINLOCK
  *
  * Parameters:
@@ -1365,91 +1322,91 @@ CARDbSetQuiet (
  *
  * Return Value: none.
  *
--*/
+ -*/
 bool
-CARDbStartQuiet (
-    void *pDeviceHandler
-    )
+CARDbStartQuiet(
+	void *pDeviceHandler
+)
 {
-    PSDevice    pDevice = (PSDevice) pDeviceHandler;
-    unsigned int ii = 0;
-    unsigned long dwStartTime = 0xFFFFFFFF;
-    unsigned int uCurrentQuietIndex = 0;
-    unsigned long dwNextTime = 0;
-    unsigned long dwGap = 0;
-    unsigned long dwDuration = 0;
-
-    for(ii=0;ii<MAX_QUIET_COUNT;ii++) {
-        if ((pDevice->sQuiet[ii].bEnable == true) &&
-            (dwStartTime > pDevice->sQuiet[ii].dwStartTime)) {
-            dwStartTime = pDevice->sQuiet[ii].dwStartTime;
-            uCurrentQuietIndex = ii;
-        }
-    }
-    if (dwStartTime == 0xFFFFFFFF) {
-        // no more quiet
-        pDevice->bQuietEnable = false;
-        MACvRegBitsOff(pDevice->PortOffset, MAC_REG_MSRCTL, (MSRCTL_QUIETTXCHK | MSRCTL_QUIETEN));
-    } else {
-        if (pDevice->bQuietEnable == false) {
-            // first quiet
-            pDevice->byQuietStartCount--;
-            dwNextTime = pDevice->sQuiet[uCurrentQuietIndex].dwStartTime;
-            dwNextTime %= pDevice->wBeaconInterval;
-            MACvSelectPage1(pDevice->PortOffset);
-            VNSvOutPortW(pDevice->PortOffset + MAC_REG_QUIETINIT, (unsigned short) dwNextTime);
-            VNSvOutPortW(pDevice->PortOffset + MAC_REG_QUIETDUR, (unsigned short) pDevice->sQuiet[uCurrentQuietIndex].wDuration);
-            if (pDevice->byQuietStartCount == 0) {
-                pDevice->bEnableFirstQuiet = false;
-                MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL, (MSRCTL_QUIETTXCHK | MSRCTL_QUIETEN));
-            } else {
-                pDevice->bEnableFirstQuiet = true;
-            }
-            MACvSelectPage0(pDevice->PortOffset);
-        } else {
-            if (pDevice->dwCurrentQuietEndTime > pDevice->sQuiet[uCurrentQuietIndex].dwStartTime) {
-                // overlap with previous Quiet
-                dwGap =  pDevice->dwCurrentQuietEndTime - pDevice->sQuiet[uCurrentQuietIndex].dwStartTime;
-                if (dwGap >= pDevice->sQuiet[uCurrentQuietIndex].wDuration) {
-                    // return false to indicate next quiet expired, should call this function again
-                    return (false);
-                }
-                dwDuration = pDevice->sQuiet[uCurrentQuietIndex].wDuration - dwGap;
-                dwGap = 0;
-            } else {
-                dwGap = pDevice->sQuiet[uCurrentQuietIndex].dwStartTime - pDevice->dwCurrentQuietEndTime;
-                dwDuration = pDevice->sQuiet[uCurrentQuietIndex].wDuration;
-            }
-            // set GAP and Next duration
-            MACvSelectPage1(pDevice->PortOffset);
-            VNSvOutPortW(pDevice->PortOffset + MAC_REG_QUIETGAP, (unsigned short) dwGap);
-            VNSvOutPortW(pDevice->PortOffset + MAC_REG_QUIETDUR, (unsigned short) dwDuration);
-            MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL, MSRCTL_QUIETRPT);
-            MACvSelectPage0(pDevice->PortOffset);
-        }
-        pDevice->bQuietEnable = true;
-        pDevice->dwCurrentQuietEndTime = pDevice->sQuiet[uCurrentQuietIndex].dwStartTime;
-        pDevice->dwCurrentQuietEndTime += pDevice->sQuiet[uCurrentQuietIndex].wDuration;
-        if (pDevice->sQuiet[uCurrentQuietIndex].byPeriod == 0) {
-            // not period disable current quiet element
-            pDevice->sQuiet[uCurrentQuietIndex].bEnable = false;
-        } else {
-            // set next period start time
-            dwNextTime = (unsigned long) pDevice->sQuiet[uCurrentQuietIndex].byPeriod;
-            dwNextTime *= pDevice->wBeaconInterval;
-            pDevice->sQuiet[uCurrentQuietIndex].dwStartTime = dwNextTime;
-        }
-        if (pDevice->dwCurrentQuietEndTime > 0x80010000) {
-            // decreament all time to avoid wrap around
-            for(ii=0;ii<MAX_QUIET_COUNT;ii++) {
-                if (pDevice->sQuiet[ii].bEnable == true) {
-                    pDevice->sQuiet[ii].dwStartTime -= 0x80000000;
-                }
-            }
-            pDevice->dwCurrentQuietEndTime -= 0x80000000;
-        }
-    }
-    return (true);
+	PSDevice    pDevice = (PSDevice) pDeviceHandler;
+	unsigned int ii = 0;
+	unsigned long dwStartTime = 0xFFFFFFFF;
+	unsigned int uCurrentQuietIndex = 0;
+	unsigned long dwNextTime = 0;
+	unsigned long dwGap = 0;
+	unsigned long dwDuration = 0;
+
+	for (ii = 0; ii < MAX_QUIET_COUNT; ii++) {
+		if ((pDevice->sQuiet[ii].bEnable == true) &&
+		    (dwStartTime > pDevice->sQuiet[ii].dwStartTime)) {
+			dwStartTime = pDevice->sQuiet[ii].dwStartTime;
+			uCurrentQuietIndex = ii;
+		}
+	}
+	if (dwStartTime == 0xFFFFFFFF) {
+		// no more quiet
+		pDevice->bQuietEnable = false;
+		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_MSRCTL, (MSRCTL_QUIETTXCHK | MSRCTL_QUIETEN));
+	} else {
+		if (pDevice->bQuietEnable == false) {
+			// first quiet
+			pDevice->byQuietStartCount--;
+			dwNextTime = pDevice->sQuiet[uCurrentQuietIndex].dwStartTime;
+			dwNextTime %= pDevice->wBeaconInterval;
+			MACvSelectPage1(pDevice->PortOffset);
+			VNSvOutPortW(pDevice->PortOffset + MAC_REG_QUIETINIT, (unsigned short) dwNextTime);
+			VNSvOutPortW(pDevice->PortOffset + MAC_REG_QUIETDUR, (unsigned short) pDevice->sQuiet[uCurrentQuietIndex].wDuration);
+			if (pDevice->byQuietStartCount == 0) {
+				pDevice->bEnableFirstQuiet = false;
+				MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL, (MSRCTL_QUIETTXCHK | MSRCTL_QUIETEN));
+			} else {
+				pDevice->bEnableFirstQuiet = true;
+			}
+			MACvSelectPage0(pDevice->PortOffset);
+		} else {
+			if (pDevice->dwCurrentQuietEndTime > pDevice->sQuiet[uCurrentQuietIndex].dwStartTime) {
+				// overlap with previous Quiet
+				dwGap =  pDevice->dwCurrentQuietEndTime - pDevice->sQuiet[uCurrentQuietIndex].dwStartTime;
+				if (dwGap >= pDevice->sQuiet[uCurrentQuietIndex].wDuration) {
+					// return false to indicate next quiet expired, should call this function again
+					return false;
+				}
+				dwDuration = pDevice->sQuiet[uCurrentQuietIndex].wDuration - dwGap;
+				dwGap = 0;
+			} else {
+				dwGap = pDevice->sQuiet[uCurrentQuietIndex].dwStartTime - pDevice->dwCurrentQuietEndTime;
+				dwDuration = pDevice->sQuiet[uCurrentQuietIndex].wDuration;
+			}
+			// set GAP and Next duration
+			MACvSelectPage1(pDevice->PortOffset);
+			VNSvOutPortW(pDevice->PortOffset + MAC_REG_QUIETGAP, (unsigned short) dwGap);
+			VNSvOutPortW(pDevice->PortOffset + MAC_REG_QUIETDUR, (unsigned short) dwDuration);
+			MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL, MSRCTL_QUIETRPT);
+			MACvSelectPage0(pDevice->PortOffset);
+		}
+		pDevice->bQuietEnable = true;
+		pDevice->dwCurrentQuietEndTime = pDevice->sQuiet[uCurrentQuietIndex].dwStartTime;
+		pDevice->dwCurrentQuietEndTime += pDevice->sQuiet[uCurrentQuietIndex].wDuration;
+		if (pDevice->sQuiet[uCurrentQuietIndex].byPeriod == 0) {
+			// not period disable current quiet element
+			pDevice->sQuiet[uCurrentQuietIndex].bEnable = false;
+		} else {
+			// set next period start time
+			dwNextTime = (unsigned long) pDevice->sQuiet[uCurrentQuietIndex].byPeriod;
+			dwNextTime *= pDevice->wBeaconInterval;
+			pDevice->sQuiet[uCurrentQuietIndex].dwStartTime = dwNextTime;
+		}
+		if (pDevice->dwCurrentQuietEndTime > 0x80010000) {
+			// decreament all time to avoid wrap around
+			for (ii = 0; ii < MAX_QUIET_COUNT; ii++) {
+				if (pDevice->sQuiet[ii].bEnable == true) {
+					pDevice->sQuiet[ii].dwStartTime -= 0x80000000;
+				}
+			}
+			pDevice->dwCurrentQuietEndTime -= 0x80000000;
+		}
+	}
+	return true;
 }
 
 /*
@@ -1465,28 +1422,27 @@ CARDbStartQuiet (
  *
  * Return Value: none.
  *
--*/
+ -*/
 void
-CARDvSetPowerConstraint (
-    void *pDeviceHandler,
-    unsigned char byChannel,
-    char byPower
-    )
+CARDvSetPowerConstraint(
+	void *pDeviceHandler,
+	unsigned char byChannel,
+	char byPower
+)
 {
-    PSDevice    pDevice = (PSDevice) pDeviceHandler;
-
-    if (byChannel > CB_MAX_CHANNEL_24G) {
-        if (pDevice->bCountryInfo5G == true) {
-            pDevice->abyLocalPwr[byChannel] = pDevice->abyRegPwr[byChannel] - byPower;
-        }
-    } else {
-        if (pDevice->bCountryInfo24G == true) {
-            pDevice->abyLocalPwr[byChannel] = pDevice->abyRegPwr[byChannel] - byPower;
-        }
-    }
+	PSDevice    pDevice = (PSDevice) pDeviceHandler;
+
+	if (byChannel > CB_MAX_CHANNEL_24G) {
+		if (pDevice->bCountryInfo5G == true) {
+			pDevice->abyLocalPwr[byChannel] = pDevice->abyRegPwr[byChannel] - byPower;
+		}
+	} else {
+		if (pDevice->bCountryInfo24G == true) {
+			pDevice->abyLocalPwr[byChannel] = pDevice->abyRegPwr[byChannel] - byPower;
+		}
+	}
 }
 
-
 /*
  *
  * Description:
@@ -1500,26 +1456,26 @@ CARDvSetPowerConstraint (
  *
  * Return Value: none.
  *
--*/
+ -*/
 void
-CARDvGetPowerCapability (
-    void *pDeviceHandler,
-    unsigned char *pbyMinPower,
-    unsigned char *pbyMaxPower
-    )
+CARDvGetPowerCapability(
+	void *pDeviceHandler,
+	unsigned char *pbyMinPower,
+	unsigned char *pbyMaxPower
+)
 {
-    PSDevice    pDevice = (PSDevice) pDeviceHandler;
-    unsigned char byDec = 0;
-
-    *pbyMaxPower = pDevice->abyOFDMDefaultPwr[pDevice->byCurrentCh];
-    byDec = pDevice->abyOFDMPwrTbl[pDevice->byCurrentCh];
-    if (pDevice->byRFType == RF_UW2452) {
-        byDec *= 3;
-        byDec >>= 1;
-    } else {
-        byDec <<= 1;
-    }
-    *pbyMinPower = pDevice->abyOFDMDefaultPwr[pDevice->byCurrentCh] - byDec;
+	PSDevice    pDevice = (PSDevice) pDeviceHandler;
+	unsigned char byDec = 0;
+
+	*pbyMaxPower = pDevice->abyOFDMDefaultPwr[pDevice->byCurrentCh];
+	byDec = pDevice->abyOFDMPwrTbl[pDevice->byCurrentCh];
+	if (pDevice->byRFType == RF_UW2452) {
+		byDec *= 3;
+		byDec >>= 1;
+	} else {
+		byDec <<= 1;
+	}
+	*pbyMinPower = pDevice->abyOFDMDefaultPwr[pDevice->byCurrentCh] - byDec;
 }
 
 /*
@@ -1537,58 +1493,55 @@ CARDvGetPowerCapability (
  *
  */
 char
-CARDbyGetTransmitPower (
-    void *pDeviceHandler
-    )
+CARDbyGetTransmitPower(
+	void *pDeviceHandler
+)
 {
-    PSDevice    pDevice = (PSDevice) pDeviceHandler;
+	PSDevice    pDevice = (PSDevice) pDeviceHandler;
 
-    return (pDevice->byCurPwrdBm);
+	return pDevice->byCurPwrdBm;
 }
 
 //xxx
 void
-CARDvSafeResetTx (
-    void *pDeviceHandler
-    )
+CARDvSafeResetTx(
+	void *pDeviceHandler
+)
 {
-    PSDevice    pDevice = (PSDevice) pDeviceHandler;
-    unsigned int uu;
-    PSTxDesc    pCurrTD;
-
-    // initialize TD index
-    pDevice->apTailTD[0] = pDevice->apCurrTD[0] = &(pDevice->apTD0Rings[0]);
-    pDevice->apTailTD[1] = pDevice->apCurrTD[1] = &(pDevice->apTD1Rings[0]);
-
-    for (uu = 0; uu < TYPE_MAXTD; uu ++)
-        pDevice->iTDUsed[uu] = 0;
-
-    for (uu = 0; uu < pDevice->sOpts.nTxDescs[0]; uu++) {
-        pCurrTD = &(pDevice->apTD0Rings[uu]);
-        pCurrTD->m_td0TD0.f1Owner = OWNED_BY_HOST;
-        // init all Tx Packet pointer to NULL
-    }
-    for (uu = 0; uu < pDevice->sOpts.nTxDescs[1]; uu++) {
-        pCurrTD = &(pDevice->apTD1Rings[uu]);
-        pCurrTD->m_td0TD0.f1Owner = OWNED_BY_HOST;
-        // init all Tx Packet pointer to NULL
-    }
-
-    // set MAC TD pointer
-    MACvSetCurrTXDescAddr(TYPE_TXDMA0, pDevice->PortOffset,
-                        (pDevice->td0_pool_dma));
-
-    MACvSetCurrTXDescAddr(TYPE_AC0DMA, pDevice->PortOffset,
-                        (pDevice->td1_pool_dma));
-
-    // set MAC Beacon TX pointer
-    MACvSetCurrBCNTxDescAddr(pDevice->PortOffset,
-                        (pDevice->tx_beacon_dma));
-
+	PSDevice    pDevice = (PSDevice) pDeviceHandler;
+	unsigned int uu;
+	PSTxDesc    pCurrTD;
+
+	// initialize TD index
+	pDevice->apTailTD[0] = pDevice->apCurrTD[0] = &(pDevice->apTD0Rings[0]);
+	pDevice->apTailTD[1] = pDevice->apCurrTD[1] = &(pDevice->apTD1Rings[0]);
+
+	for (uu = 0; uu < TYPE_MAXTD; uu++)
+		pDevice->iTDUsed[uu] = 0;
+
+	for (uu = 0; uu < pDevice->sOpts.nTxDescs[0]; uu++) {
+		pCurrTD = &(pDevice->apTD0Rings[uu]);
+		pCurrTD->m_td0TD0.f1Owner = OWNED_BY_HOST;
+		// init all Tx Packet pointer to NULL
+	}
+	for (uu = 0; uu < pDevice->sOpts.nTxDescs[1]; uu++) {
+		pCurrTD = &(pDevice->apTD1Rings[uu]);
+		pCurrTD->m_td0TD0.f1Owner = OWNED_BY_HOST;
+		// init all Tx Packet pointer to NULL
+	}
+
+	// set MAC TD pointer
+	MACvSetCurrTXDescAddr(TYPE_TXDMA0, pDevice->PortOffset,
+			      (pDevice->td0_pool_dma));
+
+	MACvSetCurrTXDescAddr(TYPE_AC0DMA, pDevice->PortOffset,
+			      (pDevice->td1_pool_dma));
+
+	// set MAC Beacon TX pointer
+	MACvSetCurrBCNTxDescAddr(pDevice->PortOffset,
+				 (pDevice->tx_beacon_dma));
 }
 
-
-
 /*+
  *
  * Description:
@@ -1602,55 +1555,50 @@ CARDvSafeResetTx (
  *
  * Return Value: none
  *
--*/
+ -*/
 void
-CARDvSafeResetRx (
-    void *pDeviceHandler
-    )
+CARDvSafeResetRx(
+	void *pDeviceHandler
+)
 {
-    PSDevice    pDevice = (PSDevice) pDeviceHandler;
-    unsigned int uu;
-    PSRxDesc    pDesc;
-
-
-
-    // initialize RD index
-    pDevice->pCurrRD[0]=&(pDevice->aRD0Ring[0]);
-    pDevice->pCurrRD[1]=&(pDevice->aRD1Ring[0]);
-
-    // init state, all RD is chip's
-    for (uu = 0; uu < pDevice->sOpts.nRxDescs0; uu++) {
-        pDesc =&(pDevice->aRD0Ring[uu]);
-        pDesc->m_rd0RD0.wResCount = (unsigned short)(pDevice->rx_buf_sz);
-        pDesc->m_rd0RD0.f1Owner=OWNED_BY_NIC;
-        pDesc->m_rd1RD1.wReqCount = (unsigned short)(pDevice->rx_buf_sz);
-    }
-
-    // init state, all RD is chip's
-    for (uu = 0; uu < pDevice->sOpts.nRxDescs1; uu++) {
-        pDesc =&(pDevice->aRD1Ring[uu]);
-        pDesc->m_rd0RD0.wResCount = (unsigned short)(pDevice->rx_buf_sz);
-        pDesc->m_rd0RD0.f1Owner=OWNED_BY_NIC;
-        pDesc->m_rd1RD1.wReqCount = (unsigned short)(pDevice->rx_buf_sz);
-    }
-
-    pDevice->cbDFCB = CB_MAX_RX_FRAG;
-    pDevice->cbFreeDFCB = pDevice->cbDFCB;
-
-    // set perPkt mode
-    MACvRx0PerPktMode(pDevice->PortOffset);
-    MACvRx1PerPktMode(pDevice->PortOffset);
-    // set MAC RD pointer
-    MACvSetCurrRx0DescAddr(pDevice->PortOffset,
-                            pDevice->rd0_pool_dma);
-
-    MACvSetCurrRx1DescAddr(pDevice->PortOffset,
-                            pDevice->rd1_pool_dma);
+	PSDevice    pDevice = (PSDevice) pDeviceHandler;
+	unsigned int uu;
+	PSRxDesc    pDesc;
+
+	// initialize RD index
+	pDevice->pCurrRD[0] = &(pDevice->aRD0Ring[0]);
+	pDevice->pCurrRD[1] = &(pDevice->aRD1Ring[0]);
+
+	// init state, all RD is chip's
+	for (uu = 0; uu < pDevice->sOpts.nRxDescs0; uu++) {
+		pDesc = &(pDevice->aRD0Ring[uu]);
+		pDesc->m_rd0RD0.wResCount = (unsigned short)(pDevice->rx_buf_sz);
+		pDesc->m_rd0RD0.f1Owner = OWNED_BY_NIC;
+		pDesc->m_rd1RD1.wReqCount = (unsigned short)(pDevice->rx_buf_sz);
+	}
+
+	// init state, all RD is chip's
+	for (uu = 0; uu < pDevice->sOpts.nRxDescs1; uu++) {
+		pDesc = &(pDevice->aRD1Ring[uu]);
+		pDesc->m_rd0RD0.wResCount = (unsigned short)(pDevice->rx_buf_sz);
+		pDesc->m_rd0RD0.f1Owner = OWNED_BY_NIC;
+		pDesc->m_rd1RD1.wReqCount = (unsigned short)(pDevice->rx_buf_sz);
+	}
+
+	pDevice->cbDFCB = CB_MAX_RX_FRAG;
+	pDevice->cbFreeDFCB = pDevice->cbDFCB;
+
+	// set perPkt mode
+	MACvRx0PerPktMode(pDevice->PortOffset);
+	MACvRx1PerPktMode(pDevice->PortOffset);
+	// set MAC RD pointer
+	MACvSetCurrRx0DescAddr(pDevice->PortOffset,
+			       pDevice->rd0_pool_dma);
+
+	MACvSetCurrRx1DescAddr(pDevice->PortOffset,
+			       pDevice->rd1_pool_dma);
 }
 
-
-
-
 /*
  * Description: Get response Control frame rate in CCK mode
  *
@@ -1666,16 +1614,16 @@ CARDvSafeResetRx (
  */
 unsigned short CARDwGetCCKControlRate(void *pDeviceHandler, unsigned short wRateIdx)
 {
-    PSDevice    pDevice = (PSDevice) pDeviceHandler;
-    unsigned int ui = (unsigned int) wRateIdx;
-
-    while (ui > RATE_1M) {
-        if (pDevice->wBasicRate & ((unsigned short)1 << ui)) {
-            return (unsigned short)ui;
-        }
-        ui --;
-    }
-    return (unsigned short)RATE_1M;
+	PSDevice    pDevice = (PSDevice) pDeviceHandler;
+	unsigned int ui = (unsigned int) wRateIdx;
+
+	while (ui > RATE_1M) {
+		if (pDevice->wBasicRate & ((unsigned short)1 << ui)) {
+			return (unsigned short)ui;
+		}
+		ui--;
+	}
+	return (unsigned short)RATE_1M;
 }
 
 /*
@@ -1691,31 +1639,30 @@ unsigned short CARDwGetCCKControlRate(void *pDeviceHandler, unsigned short wRate
  * Return Value: response Control frame rate
  *
  */
-unsigned short CARDwGetOFDMControlRate (void *pDeviceHandler, unsigned short wRateIdx)
+unsigned short CARDwGetOFDMControlRate(void *pDeviceHandler, unsigned short wRateIdx)
 {
-    PSDevice pDevice = (PSDevice) pDeviceHandler;
-    unsigned int ui = (unsigned int) wRateIdx;
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"BASIC RATE: %X\n", pDevice->wBasicRate);
-
-    if (!CARDbIsOFDMinBasicRate((void *)pDevice)) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"CARDwGetOFDMControlRate:(NO OFDM) %d\n", wRateIdx);
-        if (wRateIdx > RATE_24M)
-            wRateIdx = RATE_24M;
-        return wRateIdx;
-    }
-    while (ui > RATE_11M) {
-        if (pDevice->wBasicRate & ((unsigned short)1 << ui)) {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"CARDwGetOFDMControlRate : %d\n", ui);
-            return (unsigned short)ui;
-        }
-        ui --;
-    }
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"CARDwGetOFDMControlRate: 6M\n");
-    return (unsigned short)RATE_24M;
+	PSDevice pDevice = (PSDevice) pDeviceHandler;
+	unsigned int ui = (unsigned int) wRateIdx;
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BASIC RATE: %X\n", pDevice->wBasicRate);
+
+	if (!CARDbIsOFDMinBasicRate((void *)pDevice)) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "CARDwGetOFDMControlRate:(NO OFDM) %d\n", wRateIdx);
+		if (wRateIdx > RATE_24M)
+			wRateIdx = RATE_24M;
+		return wRateIdx;
+	}
+	while (ui > RATE_11M) {
+		if (pDevice->wBasicRate & ((unsigned short)1 << ui)) {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "CARDwGetOFDMControlRate : %d\n", ui);
+			return (unsigned short)ui;
+		}
+		ui--;
+	}
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "CARDwGetOFDMControlRate: 6M\n");
+	return (unsigned short)RATE_24M;
 }
 
-
 /*
  * Description: Set RSPINF
  *
@@ -1728,117 +1675,117 @@ unsigned short CARDwGetOFDMControlRate (void *pDeviceHandler, unsigned short wRa
  * Return Value: None.
  *
  */
-void CARDvSetRSPINF (void *pDeviceHandler, CARD_PHY_TYPE ePHYType)
+void CARDvSetRSPINF(void *pDeviceHandler, CARD_PHY_TYPE ePHYType)
 {
-    PSDevice pDevice = (PSDevice) pDeviceHandler;
-    unsigned char byServ = 0x00, bySignal = 0x00; //For CCK
-    unsigned short wLen = 0x0000;
-    unsigned char byTxRate, byRsvTime;             //For OFDM
-
-    //Set to Page1
-    MACvSelectPage1(pDevice->PortOffset);
-
-    //RSPINF_b_1
-    BBvCalculateParameter(pDevice,
-                         14,
-                         CARDwGetCCKControlRate((void *)pDevice, RATE_1M),
-                         PK_TYPE_11B,
-                         &wLen,
-                         &byServ,
-                         &bySignal
-    );
-
-    VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_1, MAKEDWORD(wLen,MAKEWORD(bySignal,byServ)));
-    ///RSPINF_b_2
-    BBvCalculateParameter(pDevice,
-                         14,
-                         CARDwGetCCKControlRate((void *)pDevice, RATE_2M),
-                         PK_TYPE_11B,
-                         &wLen,
-                         &byServ,
-                         &bySignal
-    );
-
-    VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_2, MAKEDWORD(wLen,MAKEWORD(bySignal,byServ)));
-    //RSPINF_b_5
-    BBvCalculateParameter(pDevice,
-                         14,
-                         CARDwGetCCKControlRate((void *)pDevice, RATE_5M),
-                         PK_TYPE_11B,
-                         &wLen,
-                         &byServ,
-                         &bySignal
-    );
-
-    VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_5, MAKEDWORD(wLen,MAKEWORD(bySignal,byServ)));
-    //RSPINF_b_11
-    BBvCalculateParameter(pDevice,
-                         14,
-                         CARDwGetCCKControlRate((void *)pDevice, RATE_11M),
-                         PK_TYPE_11B,
-                         &wLen,
-                         &byServ,
-                         &bySignal
-    );
-
-    VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_11, MAKEDWORD(wLen,MAKEWORD(bySignal,byServ)));
-    //RSPINF_a_6
-    s_vCalculateOFDMRParameter(RATE_6M,
-                              ePHYType,
-                              &byTxRate,
-                              &byRsvTime);
-    VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_6, MAKEWORD(byTxRate,byRsvTime));
-    //RSPINF_a_9
-    s_vCalculateOFDMRParameter(RATE_9M,
-                              ePHYType,
-                              &byTxRate,
-                              &byRsvTime);
-    VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_9, MAKEWORD(byTxRate,byRsvTime));
-    //RSPINF_a_12
-    s_vCalculateOFDMRParameter(RATE_12M,
-                              ePHYType,
-                              &byTxRate,
-                              &byRsvTime);
-    VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_12, MAKEWORD(byTxRate,byRsvTime));
-    //RSPINF_a_18
-    s_vCalculateOFDMRParameter(RATE_18M,
-                              ePHYType,
-                              &byTxRate,
-                              &byRsvTime);
-   VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_18, MAKEWORD(byTxRate,byRsvTime));
-    //RSPINF_a_24
-    s_vCalculateOFDMRParameter(RATE_24M,
-                              ePHYType,
-                              &byTxRate,
-                              &byRsvTime);
-    VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_24, MAKEWORD(byTxRate,byRsvTime));
-    //RSPINF_a_36
-    s_vCalculateOFDMRParameter(CARDwGetOFDMControlRate((void *)pDevice, RATE_36M),
-                              ePHYType,
-                              &byTxRate,
-                              &byRsvTime);
-    VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_36, MAKEWORD(byTxRate,byRsvTime));
-    //RSPINF_a_48
-    s_vCalculateOFDMRParameter(CARDwGetOFDMControlRate((void *)pDevice, RATE_48M),
-                              ePHYType,
-                              &byTxRate,
-                              &byRsvTime);
-    VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_48, MAKEWORD(byTxRate,byRsvTime));
-    //RSPINF_a_54
-    s_vCalculateOFDMRParameter(CARDwGetOFDMControlRate((void *)pDevice, RATE_54M),
-                              ePHYType,
-                              &byTxRate,
-                              &byRsvTime);
-    VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_54, MAKEWORD(byTxRate,byRsvTime));
-
-    //RSPINF_a_72
-    s_vCalculateOFDMRParameter(CARDwGetOFDMControlRate((void *)pDevice, RATE_54M),
-                              ePHYType,
-                              &byTxRate,
-                              &byRsvTime);
-    VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_72, MAKEWORD(byTxRate,byRsvTime));
-    //Set to Page0
-    MACvSelectPage0(pDevice->PortOffset);
+	PSDevice pDevice = (PSDevice) pDeviceHandler;
+	unsigned char byServ = 0x00, bySignal = 0x00; //For CCK
+	unsigned short wLen = 0x0000;
+	unsigned char byTxRate, byRsvTime;             //For OFDM
+
+	//Set to Page1
+	MACvSelectPage1(pDevice->PortOffset);
+
+	//RSPINF_b_1
+	BBvCalculateParameter(pDevice,
+			      14,
+			      CARDwGetCCKControlRate((void *)pDevice, RATE_1M),
+			      PK_TYPE_11B,
+			      &wLen,
+			      &byServ,
+			      &bySignal
+);
+
+	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_1, MAKEDWORD(wLen, MAKEWORD(bySignal, byServ)));
+	///RSPINF_b_2
+	BBvCalculateParameter(pDevice,
+			      14,
+			      CARDwGetCCKControlRate((void *)pDevice, RATE_2M),
+			      PK_TYPE_11B,
+			      &wLen,
+			      &byServ,
+			      &bySignal
+);
+
+	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_2, MAKEDWORD(wLen, MAKEWORD(bySignal, byServ)));
+	//RSPINF_b_5
+	BBvCalculateParameter(pDevice,
+			      14,
+			      CARDwGetCCKControlRate((void *)pDevice, RATE_5M),
+			      PK_TYPE_11B,
+			      &wLen,
+			      &byServ,
+			      &bySignal
+);
+
+	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_5, MAKEDWORD(wLen, MAKEWORD(bySignal, byServ)));
+	//RSPINF_b_11
+	BBvCalculateParameter(pDevice,
+			      14,
+			      CARDwGetCCKControlRate((void *)pDevice, RATE_11M),
+			      PK_TYPE_11B,
+			      &wLen,
+			      &byServ,
+			      &bySignal
+);
+
+	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_11, MAKEDWORD(wLen, MAKEWORD(bySignal, byServ)));
+	//RSPINF_a_6
+	s_vCalculateOFDMRParameter(RATE_6M,
+				   ePHYType,
+				   &byTxRate,
+				   &byRsvTime);
+	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_6, MAKEWORD(byTxRate, byRsvTime));
+	//RSPINF_a_9
+	s_vCalculateOFDMRParameter(RATE_9M,
+				   ePHYType,
+				   &byTxRate,
+				   &byRsvTime);
+	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_9, MAKEWORD(byTxRate, byRsvTime));
+	//RSPINF_a_12
+	s_vCalculateOFDMRParameter(RATE_12M,
+				   ePHYType,
+				   &byTxRate,
+				   &byRsvTime);
+	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_12, MAKEWORD(byTxRate, byRsvTime));
+	//RSPINF_a_18
+	s_vCalculateOFDMRParameter(RATE_18M,
+				   ePHYType,
+				   &byTxRate,
+				   &byRsvTime);
+	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_18, MAKEWORD(byTxRate, byRsvTime));
+	//RSPINF_a_24
+	s_vCalculateOFDMRParameter(RATE_24M,
+				   ePHYType,
+				   &byTxRate,
+				   &byRsvTime);
+	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_24, MAKEWORD(byTxRate, byRsvTime));
+	//RSPINF_a_36
+	s_vCalculateOFDMRParameter(CARDwGetOFDMControlRate((void *)pDevice, RATE_36M),
+				   ePHYType,
+				   &byTxRate,
+				   &byRsvTime);
+	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_36, MAKEWORD(byTxRate, byRsvTime));
+	//RSPINF_a_48
+	s_vCalculateOFDMRParameter(CARDwGetOFDMControlRate((void *)pDevice, RATE_48M),
+				   ePHYType,
+				   &byTxRate,
+				   &byRsvTime);
+	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_48, MAKEWORD(byTxRate, byRsvTime));
+	//RSPINF_a_54
+	s_vCalculateOFDMRParameter(CARDwGetOFDMControlRate((void *)pDevice, RATE_54M),
+				   ePHYType,
+				   &byTxRate,
+				   &byRsvTime);
+	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_54, MAKEWORD(byTxRate, byRsvTime));
+
+	//RSPINF_a_72
+	s_vCalculateOFDMRParameter(CARDwGetOFDMControlRate((void *)pDevice, RATE_54M),
+				   ePHYType,
+				   &byTxRate,
+				   &byRsvTime);
+	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_72, MAKEWORD(byTxRate, byRsvTime));
+	//Set to Page0
+	MACvSelectPage0(pDevice->PortOffset);
 }
 
 /*
@@ -1853,87 +1800,83 @@ void CARDvSetRSPINF (void *pDeviceHandler, CARD_PHY_TYPE ePHYType)
  * Return Value: None.
  *
  */
-void vUpdateIFS (void *pDeviceHandler)
+void vUpdateIFS(void *pDeviceHandler)
 {
-    //Set SIFS, DIFS, EIFS, SlotTime, CwMin
-    PSDevice pDevice = (PSDevice) pDeviceHandler;
-
-    unsigned char byMaxMin = 0;
-    if (pDevice->byPacketType==PK_TYPE_11A) {//0000 0000 0000 0000,11a
-        pDevice->uSlot = C_SLOT_SHORT;
-        pDevice->uSIFS = C_SIFS_A;
-        pDevice->uDIFS = C_SIFS_A + 2*C_SLOT_SHORT;
-        pDevice->uCwMin = C_CWMIN_A;
-        byMaxMin = 4;
-    }
-    else if (pDevice->byPacketType==PK_TYPE_11B) {//0000 0001 0000 0000,11b
-        pDevice->uSlot = C_SLOT_LONG;
-        pDevice->uSIFS = C_SIFS_BG;
-        pDevice->uDIFS = C_SIFS_BG + 2*C_SLOT_LONG;
-	    pDevice->uCwMin = C_CWMIN_B;
-        byMaxMin = 5;
-    }
-    else { // PK_TYPE_11GA & PK_TYPE_11GB
-        pDevice->uSIFS = C_SIFS_BG;
-        if (pDevice->bShortSlotTime) {
-            pDevice->uSlot = C_SLOT_SHORT;
-        } else {
-	        pDevice->uSlot = C_SLOT_LONG;
-	    }
-	    pDevice->uDIFS = C_SIFS_BG + 2*pDevice->uSlot;
-        if (pDevice->wBasicRate & 0x0150) { //0000 0001 0101 0000,24M,12M,6M
-            pDevice->uCwMin = C_CWMIN_A;
-            byMaxMin = 4;
-        }
-        else {
-            pDevice->uCwMin = C_CWMIN_B;
-            byMaxMin = 5;
-        }
-    }
-
-    pDevice->uCwMax = C_CWMAX;
-    pDevice->uEIFS = C_EIFS;
-    if (pDevice->byRFType == RF_RFMD2959) {
-        // bcs TX_PE will reserve 3 us
-        VNSvOutPortB(pDevice->PortOffset + MAC_REG_SIFS, (unsigned char)(pDevice->uSIFS - 3));
-        VNSvOutPortB(pDevice->PortOffset + MAC_REG_DIFS, (unsigned char)(pDevice->uDIFS - 3));
-    } else {
-        VNSvOutPortB(pDevice->PortOffset + MAC_REG_SIFS, (unsigned char)pDevice->uSIFS);
-        VNSvOutPortB(pDevice->PortOffset + MAC_REG_DIFS, (unsigned char)pDevice->uDIFS);
-    }
-    VNSvOutPortB(pDevice->PortOffset + MAC_REG_EIFS, (unsigned char)pDevice->uEIFS);
-    VNSvOutPortB(pDevice->PortOffset + MAC_REG_SLOT, (unsigned char)pDevice->uSlot);
-    byMaxMin |= 0xA0;//1010 1111,C_CWMAX = 1023
-    VNSvOutPortB(pDevice->PortOffset + MAC_REG_CWMAXMIN0, (unsigned char)byMaxMin);
+	//Set SIFS, DIFS, EIFS, SlotTime, CwMin
+	PSDevice pDevice = (PSDevice) pDeviceHandler;
+
+	unsigned char byMaxMin = 0;
+	if (pDevice->byPacketType == PK_TYPE_11A) {//0000 0000 0000 0000,11a
+		pDevice->uSlot = C_SLOT_SHORT;
+		pDevice->uSIFS = C_SIFS_A;
+		pDevice->uDIFS = C_SIFS_A + 2*C_SLOT_SHORT;
+		pDevice->uCwMin = C_CWMIN_A;
+		byMaxMin = 4;
+	} else if (pDevice->byPacketType == PK_TYPE_11B) {//0000 0001 0000 0000,11b
+		pDevice->uSlot = C_SLOT_LONG;
+		pDevice->uSIFS = C_SIFS_BG;
+		pDevice->uDIFS = C_SIFS_BG + 2*C_SLOT_LONG;
+		pDevice->uCwMin = C_CWMIN_B;
+		byMaxMin = 5;
+	} else { // PK_TYPE_11GA & PK_TYPE_11GB
+		pDevice->uSIFS = C_SIFS_BG;
+		if (pDevice->bShortSlotTime) {
+			pDevice->uSlot = C_SLOT_SHORT;
+		} else {
+			pDevice->uSlot = C_SLOT_LONG;
+		}
+		pDevice->uDIFS = C_SIFS_BG + 2*pDevice->uSlot;
+		if (pDevice->wBasicRate & 0x0150) { //0000 0001 0101 0000,24M,12M,6M
+			pDevice->uCwMin = C_CWMIN_A;
+			byMaxMin = 4;
+		} else {
+			pDevice->uCwMin = C_CWMIN_B;
+			byMaxMin = 5;
+		}
+	}
+
+	pDevice->uCwMax = C_CWMAX;
+	pDevice->uEIFS = C_EIFS;
+	if (pDevice->byRFType == RF_RFMD2959) {
+		// bcs TX_PE will reserve 3 us
+		VNSvOutPortB(pDevice->PortOffset + MAC_REG_SIFS, (unsigned char)(pDevice->uSIFS - 3));
+		VNSvOutPortB(pDevice->PortOffset + MAC_REG_DIFS, (unsigned char)(pDevice->uDIFS - 3));
+	} else {
+		VNSvOutPortB(pDevice->PortOffset + MAC_REG_SIFS, (unsigned char)pDevice->uSIFS);
+		VNSvOutPortB(pDevice->PortOffset + MAC_REG_DIFS, (unsigned char)pDevice->uDIFS);
+	}
+	VNSvOutPortB(pDevice->PortOffset + MAC_REG_EIFS, (unsigned char)pDevice->uEIFS);
+	VNSvOutPortB(pDevice->PortOffset + MAC_REG_SLOT, (unsigned char)pDevice->uSlot);
+	byMaxMin |= 0xA0;//1010 1111,C_CWMAX = 1023
+	VNSvOutPortB(pDevice->PortOffset + MAC_REG_CWMAXMIN0, (unsigned char)byMaxMin);
 }
 
-void CARDvUpdateBasicTopRate (void *pDeviceHandler)
+void CARDvUpdateBasicTopRate(void *pDeviceHandler)
 {
-    PSDevice pDevice = (PSDevice) pDeviceHandler;
-    unsigned char byTopOFDM = RATE_24M, byTopCCK = RATE_1M;
-    unsigned char ii;
-
-     //Determines the highest basic rate.
-     for (ii = RATE_54M; ii >= RATE_6M; ii --) {
-         if ( (pDevice->wBasicRate) & ((unsigned short)(1<<ii)) ) {
-             byTopOFDM = ii;
-             break;
-         }
-     }
-     pDevice->byTopOFDMBasicRate = byTopOFDM;
-
-     for (ii = RATE_11M;; ii --) {
-         if ( (pDevice->wBasicRate) & ((unsigned short)(1<<ii)) ) {
-             byTopCCK = ii;
-             break;
-         }
-         if (ii == RATE_1M)
-            break;
-     }
-     pDevice->byTopCCKBasicRate = byTopCCK;
+	PSDevice pDevice = (PSDevice) pDeviceHandler;
+	unsigned char byTopOFDM = RATE_24M, byTopCCK = RATE_1M;
+	unsigned char ii;
+
+	//Determines the highest basic rate.
+	for (ii = RATE_54M; ii >= RATE_6M; ii--) {
+		if ((pDevice->wBasicRate) & ((unsigned short)(1<<ii))) {
+			byTopOFDM = ii;
+			break;
+		}
+	}
+	pDevice->byTopOFDMBasicRate = byTopOFDM;
+
+	for (ii = RATE_11M;; ii--) {
+		if ((pDevice->wBasicRate) & ((unsigned short)(1<<ii))) {
+			byTopCCK = ii;
+			break;
+		}
+		if (ii == RATE_1M)
+			break;
+	}
+	pDevice->byTopCCKBasicRate = byTopCCK;
 }
 
-
 /*
  * Description: Set NIC Tx Basic Rate
  *
@@ -1947,44 +1890,42 @@ void CARDvUpdateBasicTopRate (void *pDeviceHandler)
  * Return Value: true if succeeded; false if failed.
  *
  */
-bool CARDbAddBasicRate (void *pDeviceHandler, unsigned short wRateIdx)
+bool CARDbAddBasicRate(void *pDeviceHandler, unsigned short wRateIdx)
 {
-    PSDevice pDevice = (PSDevice) pDeviceHandler;
-    unsigned short wRate = (unsigned short)(1<<wRateIdx);
+	PSDevice pDevice = (PSDevice) pDeviceHandler;
+	unsigned short wRate = (unsigned short)(1<<wRateIdx);
 
-    pDevice->wBasicRate |= wRate;
+	pDevice->wBasicRate |= wRate;
 
-    //Determines the highest basic rate.
-    CARDvUpdateBasicTopRate((void *)pDevice);
+	//Determines the highest basic rate.
+	CARDvUpdateBasicTopRate((void *)pDevice);
 
-    return(true);
+	return true;
 }
 
-bool CARDbIsOFDMinBasicRate (void *pDeviceHandler)
+bool CARDbIsOFDMinBasicRate(void *pDeviceHandler)
 {
-    PSDevice pDevice = (PSDevice) pDeviceHandler;
-    int ii;
-
-    for (ii = RATE_54M; ii >= RATE_6M; ii --) {
-        if ((pDevice->wBasicRate) & ((unsigned short)(1<<ii)))
-            return true;
-    }
-    return false;
+	PSDevice pDevice = (PSDevice)pDeviceHandler;
+	int ii;
+
+	for (ii = RATE_54M; ii >= RATE_6M; ii--) {
+		if ((pDevice->wBasicRate) & ((unsigned short)(1 << ii)))
+			return true;
+	}
+	return false;
 }
 
-unsigned char CARDbyGetPktType (void *pDeviceHandler)
+unsigned char CARDbyGetPktType(void *pDeviceHandler)
 {
-    PSDevice pDevice = (PSDevice) pDeviceHandler;
-
-    if (pDevice->byBBType == BB_TYPE_11A || pDevice->byBBType == BB_TYPE_11B) {
-        return (unsigned char)pDevice->byBBType;
-    }
-    else if (CARDbIsOFDMinBasicRate((void *)pDevice)) {
-        return PK_TYPE_11GA;
-    }
-    else {
-    	return PK_TYPE_11GB;
-    }
+	PSDevice pDevice = (PSDevice) pDeviceHandler;
+
+	if (pDevice->byBBType == BB_TYPE_11A || pDevice->byBBType == BB_TYPE_11B) {
+		return (unsigned char)pDevice->byBBType;
+	} else if (CARDbIsOFDMinBasicRate((void *)pDevice)) {
+		return PK_TYPE_11GA;
+	} else {
+		return PK_TYPE_11GB;
+	}
 }
 
 /*
@@ -2000,23 +1941,22 @@ unsigned char CARDbyGetPktType (void *pDeviceHandler)
  * Return Value: none
  *
  */
-void CARDvSetLoopbackMode (unsigned long dwIoBase, unsigned short wLoopbackMode)
+void CARDvSetLoopbackMode(unsigned long dwIoBase, unsigned short wLoopbackMode)
 {
-    switch(wLoopbackMode) {
-    case CARD_LB_NONE:
-    case CARD_LB_MAC:
-    case CARD_LB_PHY:
-        break;
-    default:
-        ASSERT(false);
-        break;
-    }
-    // set MAC loopback
-    MACvSetLoopbackMode(dwIoBase, LOBYTE(wLoopbackMode));
-    // set Baseband loopback
+	switch (wLoopbackMode) {
+	case CARD_LB_NONE:
+	case CARD_LB_MAC:
+	case CARD_LB_PHY:
+		break;
+	default:
+		ASSERT(false);
+		break;
+	}
+	// set MAC loopback
+	MACvSetLoopbackMode(dwIoBase, LOBYTE(wLoopbackMode));
+	// set Baseband loopback
 }
 
-
 /*
  * Description: Software Reset NIC
  *
@@ -2029,18 +1969,17 @@ void CARDvSetLoopbackMode (unsigned long dwIoBase, unsigned short wLoopbackMode)
  * Return Value: none
  *
  */
-bool CARDbSoftwareReset (void *pDeviceHandler)
+bool CARDbSoftwareReset(void *pDeviceHandler)
 {
-    PSDevice pDevice = (PSDevice) pDeviceHandler;
+	PSDevice pDevice = (PSDevice) pDeviceHandler;
 
-    // reset MAC
-    if (!MACbSafeSoftwareReset(pDevice->PortOffset))
-        return false;
+	// reset MAC
+	if (!MACbSafeSoftwareReset(pDevice->PortOffset))
+		return false;
 
-    return true;
+	return true;
 }
 
-
 /*
  * Description: Calculate TSF offset of two TSF input
  *              Get TSF Offset from RxBCN's TSF and local TSF
@@ -2056,30 +1995,28 @@ bool CARDbSoftwareReset (void *pDeviceHandler)
  * Return Value: TSF Offset value
  *
  */
-QWORD CARDqGetTSFOffset (unsigned char byRxRate, QWORD qwTSF1, QWORD qwTSF2)
+QWORD CARDqGetTSFOffset(unsigned char byRxRate, QWORD qwTSF1, QWORD qwTSF2)
 {
-    QWORD   qwTSFOffset;
-    unsigned short wRxBcnTSFOffst= 0;
-
-    HIDWORD(qwTSFOffset) = 0;
-    LODWORD(qwTSFOffset) = 0;
-    wRxBcnTSFOffst = cwRXBCNTSFOff[byRxRate%MAX_RATE];
-    (qwTSF2).u.dwLowDword += (unsigned long)(wRxBcnTSFOffst);
-    if ((qwTSF2).u.dwLowDword < (unsigned long)(wRxBcnTSFOffst)) {
-        (qwTSF2).u.dwHighDword++;
-    }
-    LODWORD(qwTSFOffset) = LODWORD(qwTSF1) - LODWORD(qwTSF2);
-    if (LODWORD(qwTSF1) < LODWORD(qwTSF2)) {
-        // if borrow needed
-        HIDWORD(qwTSFOffset) = HIDWORD(qwTSF1) - HIDWORD(qwTSF2) - 1 ;
-    }
-    else {
-        HIDWORD(qwTSFOffset) = HIDWORD(qwTSF1) - HIDWORD(qwTSF2);
-    };
-    return (qwTSFOffset);
+	QWORD   qwTSFOffset;
+	unsigned short wRxBcnTSFOffst = 0;
+
+	HIDWORD(qwTSFOffset) = 0;
+	LODWORD(qwTSFOffset) = 0;
+	wRxBcnTSFOffst = cwRXBCNTSFOff[byRxRate%MAX_RATE];
+	(qwTSF2).u.dwLowDword += (unsigned long)(wRxBcnTSFOffst);
+	if ((qwTSF2).u.dwLowDword < (unsigned long)(wRxBcnTSFOffst)) {
+		(qwTSF2).u.dwHighDword++;
+	}
+	LODWORD(qwTSFOffset) = LODWORD(qwTSF1) - LODWORD(qwTSF2);
+	if (LODWORD(qwTSF1) < LODWORD(qwTSF2)) {
+		// if borrow needed
+		HIDWORD(qwTSFOffset) = HIDWORD(qwTSF1) - HIDWORD(qwTSF2) - 1;
+	} else {
+		HIDWORD(qwTSFOffset) = HIDWORD(qwTSF1) - HIDWORD(qwTSF2);
+	};
+	return qwTSFOffset;
 }
 
-
 /*
  * Description: Read NIC TSF counter
  *              Get local TSF counter
@@ -2093,26 +2030,25 @@ QWORD CARDqGetTSFOffset (unsigned char byRxRate, QWORD qwTSF1, QWORD qwTSF2)
  * Return Value: true if success; otherwise false
  *
  */
-bool CARDbGetCurrentTSF (unsigned long dwIoBase, PQWORD pqwCurrTSF)
+bool CARDbGetCurrentTSF(unsigned long dwIoBase, PQWORD pqwCurrTSF)
 {
-    unsigned short ww;
-    unsigned char byData;
-
-    MACvRegBitsOn(dwIoBase, MAC_REG_TFTCTL, TFTCTL_TSFCNTRRD);
-    for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-        VNSvInPortB(dwIoBase + MAC_REG_TFTCTL, &byData);
-        if ( !(byData & TFTCTL_TSFCNTRRD))
-            break;
-    }
-    if (ww == W_MAX_TIMEOUT)
-        return(false);
-    VNSvInPortD(dwIoBase + MAC_REG_TSFCNTR, &LODWORD(*pqwCurrTSF));
-    VNSvInPortD(dwIoBase + MAC_REG_TSFCNTR + 4, &HIDWORD(*pqwCurrTSF));
-
-    return(true);
+	unsigned short ww;
+	unsigned char byData;
+
+	MACvRegBitsOn(dwIoBase, MAC_REG_TFTCTL, TFTCTL_TSFCNTRRD);
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		VNSvInPortB(dwIoBase + MAC_REG_TFTCTL, &byData);
+		if (!(byData & TFTCTL_TSFCNTRRD))
+			break;
+	}
+	if (ww == W_MAX_TIMEOUT)
+		return false;
+	VNSvInPortD(dwIoBase + MAC_REG_TSFCNTR, &LODWORD(*pqwCurrTSF));
+	VNSvInPortD(dwIoBase + MAC_REG_TSFCNTR + 4, &HIDWORD(*pqwCurrTSF));
+
+	return true;
 }
 
-
 /*
  * Description: Read NIC TSF counter
  *              Get NEXTTBTT from adjusted TSF and Beacon Interval
@@ -2127,36 +2063,34 @@ bool CARDbGetCurrentTSF (unsigned long dwIoBase, PQWORD pqwCurrTSF)
  * Return Value: TSF value of next Beacon
  *
  */
-QWORD CARDqGetNextTBTT (QWORD qwTSF, unsigned short wBeaconInterval)
+QWORD CARDqGetNextTBTT(QWORD qwTSF, unsigned short wBeaconInterval)
 {
-
-    unsigned int uLowNextTBTT;
-    unsigned int uHighRemain, uLowRemain;
-    unsigned int uBeaconInterval;
-
-    uBeaconInterval = wBeaconInterval * 1024;
-    // Next TBTT = ((local_current_TSF / beacon_interval) + 1 ) * beacon_interval
-    uLowNextTBTT = (LODWORD(qwTSF) >> 10) << 10;
-    // low dword (mod) bcn
-    uLowRemain = (uLowNextTBTT) % uBeaconInterval;
+	unsigned int uLowNextTBTT;
+	unsigned int uHighRemain, uLowRemain;
+	unsigned int uBeaconInterval;
+
+	uBeaconInterval = wBeaconInterval * 1024;
+	// Next TBTT = ((local_current_TSF / beacon_interval) + 1) * beacon_interval
+	uLowNextTBTT = (LODWORD(qwTSF) >> 10) << 10;
+	// low dword (mod) bcn
+	uLowRemain = (uLowNextTBTT) % uBeaconInterval;
 //    uHighRemain = ((0x80000000 % uBeaconInterval)* 2 * HIDWORD(qwTSF))
 //                  % uBeaconInterval;
-    // high dword (mod) bcn
-    uHighRemain = (((0xffffffff % uBeaconInterval) + 1) * HIDWORD(qwTSF))
-                  % uBeaconInterval;
-    uLowRemain = (uHighRemain + uLowRemain) % uBeaconInterval;
-    uLowRemain = uBeaconInterval - uLowRemain;
+	// high dword (mod) bcn
+	uHighRemain = (((0xffffffff % uBeaconInterval) + 1) * HIDWORD(qwTSF))
+		% uBeaconInterval;
+	uLowRemain = (uHighRemain + uLowRemain) % uBeaconInterval;
+	uLowRemain = uBeaconInterval - uLowRemain;
 
-    // check if carry when add one beacon interval
-    if ((~uLowNextTBTT) < uLowRemain)
-        HIDWORD(qwTSF) ++ ;
+	// check if carry when add one beacon interval
+	if ((~uLowNextTBTT) < uLowRemain)
+		HIDWORD(qwTSF)++;
 
-    LODWORD(qwTSF) = uLowNextTBTT + uLowRemain;
+	LODWORD(qwTSF) = uLowNextTBTT + uLowRemain;
 
-    return (qwTSF);
+	return qwTSF;
 }
 
-
 /*
  * Description: Set NIC TSF counter for first Beacon time
  *              Get NEXTTBTT from adjusted TSF and Beacon Interval
@@ -2171,24 +2105,22 @@ QWORD CARDqGetNextTBTT (QWORD qwTSF, unsigned short wBeaconInterval)
  * Return Value: none
  *
  */
-void CARDvSetFirstNextTBTT (unsigned long dwIoBase, unsigned short wBeaconInterval)
+void CARDvSetFirstNextTBTT(unsigned long dwIoBase, unsigned short wBeaconInterval)
 {
-
-    QWORD   qwNextTBTT;
-
-    HIDWORD(qwNextTBTT) = 0;
-    LODWORD(qwNextTBTT) = 0;
-    CARDbGetCurrentTSF(dwIoBase, &qwNextTBTT); //Get Local TSF counter
-    qwNextTBTT = CARDqGetNextTBTT(qwNextTBTT, wBeaconInterval);
-    // Set NextTBTT
-    VNSvOutPortD(dwIoBase + MAC_REG_NEXTTBTT, LODWORD(qwNextTBTT));
-    VNSvOutPortD(dwIoBase + MAC_REG_NEXTTBTT + 4, HIDWORD(qwNextTBTT));
-    MACvRegBitsOn(dwIoBase, MAC_REG_TFTCTL, TFTCTL_TBTTSYNCEN);
-    //DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Card:First Next TBTT[%8xh:%8xh] \n", HIDWORD(qwNextTBTT), LODWORD(qwNextTBTT));
-    return;
+	QWORD   qwNextTBTT;
+
+	HIDWORD(qwNextTBTT) = 0;
+	LODWORD(qwNextTBTT) = 0;
+	CARDbGetCurrentTSF(dwIoBase, &qwNextTBTT); //Get Local TSF counter
+	qwNextTBTT = CARDqGetNextTBTT(qwNextTBTT, wBeaconInterval);
+	// Set NextTBTT
+	VNSvOutPortD(dwIoBase + MAC_REG_NEXTTBTT, LODWORD(qwNextTBTT));
+	VNSvOutPortD(dwIoBase + MAC_REG_NEXTTBTT + 4, HIDWORD(qwNextTBTT));
+	MACvRegBitsOn(dwIoBase, MAC_REG_TFTCTL, TFTCTL_TBTTSYNCEN);
+	//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Card:First Next TBTT[%8xh:%8xh] \n", HIDWORD(qwNextTBTT), LODWORD(qwNextTBTT));
+	return;
 }
 
-
 /*
  * Description: Sync NIC TSF counter for Beacon time
  *              Get NEXTTBTT and write to HW
@@ -2204,23 +2136,15 @@ void CARDvSetFirstNextTBTT (unsigned long dwIoBase, unsigned short wBeaconInterv
  * Return Value: none
  *
  */
-void CARDvUpdateNextTBTT (unsigned long dwIoBase, QWORD qwTSF, unsigned short wBeaconInterval)
+void CARDvUpdateNextTBTT(unsigned long dwIoBase, QWORD qwTSF, unsigned short wBeaconInterval)
 {
-
-    qwTSF = CARDqGetNextTBTT(qwTSF, wBeaconInterval);
-    // Set NextTBTT
-    VNSvOutPortD(dwIoBase + MAC_REG_NEXTTBTT, LODWORD(qwTSF));
-    VNSvOutPortD(dwIoBase + MAC_REG_NEXTTBTT + 4, HIDWORD(qwTSF));
-    MACvRegBitsOn(dwIoBase, MAC_REG_TFTCTL, TFTCTL_TBTTSYNCEN);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Card:Update Next TBTT[%8xh:%8xh] \n",
-		    (unsigned int) HIDWORD(qwTSF), (unsigned int) LODWORD(qwTSF));
-
-    return;
+	qwTSF = CARDqGetNextTBTT(qwTSF, wBeaconInterval);
+	// Set NextTBTT
+	VNSvOutPortD(dwIoBase + MAC_REG_NEXTTBTT, LODWORD(qwTSF));
+	VNSvOutPortD(dwIoBase + MAC_REG_NEXTTBTT + 4, HIDWORD(qwTSF));
+	MACvRegBitsOn(dwIoBase, MAC_REG_TFTCTL, TFTCTL_TBTTSYNCEN);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Card:Update Next TBTT[%8xh:%8xh] \n",
+		(unsigned int) HIDWORD(qwTSF), (unsigned int) LODWORD(qwTSF));
+
+	return;
 }
-
-
-
-
-
-
-
diff --git a/drivers/staging/vt6655/card.h b/drivers/staging/vt6655/card.h
index e0836e1d511..ac6e2b4d0fd 100644
--- a/drivers/staging/vt6655/card.h
+++ b/drivers/staging/vt6655/card.h
@@ -41,7 +41,6 @@
 #define CARD_LB_MAC             MAKEWORD(MAC_LB_INTERNAL, 0)   // PHY must ISO, avoid MAC loopback packet go out
 #define CARD_LB_PHY             MAKEWORD(MAC_LB_EXT, 0)
 
-
 #define DEFAULT_MSDU_LIFETIME           512  // ms
 #define DEFAULT_MSDU_LIFETIME_RES_64us  8000 // 64us
 
@@ -53,34 +52,32 @@
 #define CB_MAX_CHANNEL          (CB_MAX_CHANNEL_24G+CB_MAX_CHANNEL_5G)
 
 typedef enum _CARD_PHY_TYPE {
-    PHY_TYPE_AUTO,
-    PHY_TYPE_11B,
-    PHY_TYPE_11G,
-    PHY_TYPE_11A
+	PHY_TYPE_AUTO,
+	PHY_TYPE_11B,
+	PHY_TYPE_11G,
+	PHY_TYPE_11A
 } CARD_PHY_TYPE, *PCARD_PHY_TYPE;
 
 typedef enum _CARD_PKT_TYPE {
-    PKT_TYPE_802_11_BCN,
-    PKT_TYPE_802_11_MNG,
-    PKT_TYPE_802_11_DATA,
-    PKT_TYPE_802_11_ALL
+	PKT_TYPE_802_11_BCN,
+	PKT_TYPE_802_11_MNG,
+	PKT_TYPE_802_11_DATA,
+	PKT_TYPE_802_11_ALL
 } CARD_PKT_TYPE, *PCARD_PKT_TYPE;
 
 typedef enum _CARD_STATUS_TYPE {
-    CARD_STATUS_MEDIA_CONNECT,
-    CARD_STATUS_MEDIA_DISCONNECT,
-    CARD_STATUS_PMKID
+	CARD_STATUS_MEDIA_CONNECT,
+	CARD_STATUS_MEDIA_DISCONNECT,
+	CARD_STATUS_PMKID
 } CARD_STATUS_TYPE, *PCARD_STATUS_TYPE;
 
 typedef enum _CARD_OP_MODE {
-    OP_MODE_INFRASTRUCTURE,
-    OP_MODE_ADHOC,
-    OP_MODE_AP,
-    OP_MODE_UNKNOWN
+	OP_MODE_INFRASTRUCTURE,
+	OP_MODE_ADHOC,
+	OP_MODE_AP,
+	OP_MODE_UNKNOWN
 } CARD_OP_MODE, *PCARD_OP_MODE;
 
-
-
 /*---------------------  Export Classes  ----------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
@@ -119,80 +116,76 @@ bool CARDbSetBSSID(void *pDeviceHandler, unsigned char *pbyBSSID, CARD_OP_MODE e
 
 bool
 CARDbPowerDown(
-    void *pDeviceHandler
-    );
+	void *pDeviceHandler
+);
 
 bool CARDbSetTxDataRate(
-    void *pDeviceHandler,
-    unsigned short wDataRate
-    );
-
+	void *pDeviceHandler,
+	unsigned short wDataRate
+);
 
-bool CARDbRemoveKey (void *pDeviceHandler, unsigned char *pbyBSSID);
+bool CARDbRemoveKey(void *pDeviceHandler, unsigned char *pbyBSSID);
 
 bool
-CARDbAdd_PMKID_Candidate (
-    void *pDeviceHandler,
-    unsigned char *pbyBSSID,
-    bool bRSNCapExist,
-    unsigned short wRSNCap
-    );
+CARDbAdd_PMKID_Candidate(
+	void *pDeviceHandler,
+	unsigned char *pbyBSSID,
+	bool bRSNCapExist,
+	unsigned short wRSNCap
+);
 
 void *
-CARDpGetCurrentAddress (
-    void *pDeviceHandler
-    );
+CARDpGetCurrentAddress(
+	void *pDeviceHandler
+);
 
 bool
-CARDbStartMeasure (
-    void *pDeviceHandler,
-    void *pvMeasureEIDs,
-    unsigned int uNumOfMeasureEIDs
-    );
+CARDbStartMeasure(
+	void *pDeviceHandler,
+	void *pvMeasureEIDs,
+	unsigned int uNumOfMeasureEIDs
+);
 
 bool
-CARDbChannelSwitch (
-    void *pDeviceHandler,
-    unsigned char byMode,
-    unsigned char byNewChannel,
-    unsigned char byCount
-    );
+CARDbChannelSwitch(
+	void *pDeviceHandler,
+	unsigned char byMode,
+	unsigned char byNewChannel,
+	unsigned char byCount
+);
 
 bool
-CARDbSetQuiet (
-    void *pDeviceHandler,
-    bool bResetQuiet,
-    unsigned char byQuietCount,
-    unsigned char byQuietPeriod,
-    unsigned short wQuietDuration,
-    unsigned short wQuietOffset
-    );
+CARDbSetQuiet(
+	void *pDeviceHandler,
+	bool bResetQuiet,
+	unsigned char byQuietCount,
+	unsigned char byQuietPeriod,
+	unsigned short wQuietDuration,
+	unsigned short wQuietOffset
+);
 
 bool
-CARDbStartQuiet (
-    void *pDeviceHandler
-    );
+CARDbStartQuiet(
+	void *pDeviceHandler
+);
 
 void
-CARDvSetPowerConstraint (
-    void *pDeviceHandler,
-    unsigned char byChannel,
-    char byPower
-    );
+CARDvSetPowerConstraint(
+	void *pDeviceHandler,
+	unsigned char byChannel,
+	char byPower
+);
 
 void
-CARDvGetPowerCapability (
-    void *pDeviceHandler,
-    unsigned char *pbyMinPower,
-    unsigned char *pbyMaxPower
-    );
+CARDvGetPowerCapability(
+	void *pDeviceHandler,
+	unsigned char *pbyMinPower,
+	unsigned char *pbyMaxPower
+);
 
 char
-CARDbyGetTransmitPower (
-    void *pDeviceHandler
-    );
+CARDbyGetTransmitPower(
+	void *pDeviceHandler
+);
 
 #endif // __CARD_H__
-
-
-
diff --git a/drivers/staging/vt6655/channel.c b/drivers/staging/vt6655/channel.c
index aa76e39a46f..ba9481fa654 100644
--- a/drivers/staging/vt6655/channel.c
+++ b/drivers/staging/vt6655/channel.c
@@ -37,63 +37,63 @@ static int msglevel = MSG_LEVEL_INFO;
 
 static SChannelTblElement sChannelTbl[CARD_MAX_CHANNEL_TBL + 1] =
 {
-  {0,   0,    false,    0},
-  {1,   2412, true,     0},
-  {2,   2417, true,     0},
-  {3,   2422, true,     0},
-  {4,   2427, true,     0},
-  {5,   2432, true,     0},
-  {6,   2437, true,     0},
-  {7,   2442, true,     0},
-  {8,   2447, true,     0},
-  {9,   2452, true,     0},
-  {10,  2457, true,     0},
-  {11,  2462, true,     0},
-  {12,  2467, true,     0},
-  {13,  2472, true,     0},
-  {14,  2484, true,     0},
-  {183, 4915, true,     0},
-  {184, 4920, true,     0},
-  {185, 4925, true,     0},
-  {187, 4935, true,     0},
-  {188, 4940, true,     0},
-  {189, 4945, true,     0},
-  {192, 4960, true,     0},
-  {196, 4980, true,     0},
-  {7,   5035, true,     0},
-  {8,   5040, true,     0},
-  {9,   5045, true,     0},
-  {11,  5055, true,     0},
-  {12,  5060, true,     0},
-  {16,  5080, true,     0},
-  {34,  5170, true,     0},
-  {36,  5180, true,     0},
-  {38,  5190, true,     0},
-  {40,  5200, true,     0},
-  {42,  5210, true,     0},
-  {44,  5220, true,     0},
-  {46,  5230, true,     0},
-  {48,  5240, true,     0},
-  {52,  5260, true,     0},
-  {56,  5280, true,     0},
-  {60,  5300, true,     0},
-  {64,  5320, true,     0},
-  {100, 5500, true,     0},
-  {104, 5520, true,     0},
-  {108, 5540, true,     0},
-  {112, 5560, true,     0},
-  {116, 5580, true,     0},
-  {120, 5600, true,     0},
-  {124, 5620, true,     0},
-  {128, 5640, true,     0},
-  {132, 5660, true,     0},
-  {136, 5680, true,     0},
-  {140, 5700, true,     0},
-  {149, 5745, true,     0},
-  {153, 5765, true,     0},
-  {157, 5785, true,     0},
-  {161, 5805, true,     0},
-  {165, 5825, true,     0}
+	{0,   0,    false,    0},
+	{1,   2412, true,     0},
+	{2,   2417, true,     0},
+	{3,   2422, true,     0},
+	{4,   2427, true,     0},
+	{5,   2432, true,     0},
+	{6,   2437, true,     0},
+	{7,   2442, true,     0},
+	{8,   2447, true,     0},
+	{9,   2452, true,     0},
+	{10,  2457, true,     0},
+	{11,  2462, true,     0},
+	{12,  2467, true,     0},
+	{13,  2472, true,     0},
+	{14,  2484, true,     0},
+	{183, 4915, true,     0},
+	{184, 4920, true,     0},
+	{185, 4925, true,     0},
+	{187, 4935, true,     0},
+	{188, 4940, true,     0},
+	{189, 4945, true,     0},
+	{192, 4960, true,     0},
+	{196, 4980, true,     0},
+	{7,   5035, true,     0},
+	{8,   5040, true,     0},
+	{9,   5045, true,     0},
+	{11,  5055, true,     0},
+	{12,  5060, true,     0},
+	{16,  5080, true,     0},
+	{34,  5170, true,     0},
+	{36,  5180, true,     0},
+	{38,  5190, true,     0},
+	{40,  5200, true,     0},
+	{42,  5210, true,     0},
+	{44,  5220, true,     0},
+	{46,  5230, true,     0},
+	{48,  5240, true,     0},
+	{52,  5260, true,     0},
+	{56,  5280, true,     0},
+	{60,  5300, true,     0},
+	{64,  5320, true,     0},
+	{100, 5500, true,     0},
+	{104, 5520, true,     0},
+	{108, 5540, true,     0},
+	{112, 5560, true,     0},
+	{116, 5580, true,     0},
+	{120, 5600, true,     0},
+	{124, 5620, true,     0},
+	{128, 5640, true,     0},
+	{132, 5660, true,     0},
+	{136, 5680, true,     0},
+	{140, 5700, true,     0},
+	{149, 5745, true,     0},
+	{153, 5765, true,     0},
+	{157, 5785, true,     0},
+	{161, 5805, true,     0},
+	{165, 5825, true,     0}
 };
 
 /************************************************************************
@@ -112,244 +112,244 @@ static struct
  ************************************************************************/
 /* Country          Available channels, ended with 0                    */
 /*                                              1   2   3   4   5   6   7   8   9  10  11  12  13  14  15  16  17  18  19  20  21  22  23  24  25  26  27  28  29  30  31  32  33  34  35  36  37  38  39  40  41  42  43  44  45  46  47  48  49  50  51  52  53  54  55  56  */
-{CCODE_FCC,                     {'U','S'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-                                         ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-{CCODE_TELEC,                   {'J','P'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  1,  0,  0,  1,  0,  1,  1,  0,  1,  0,  0,  1,  1,  1,  0,  1,  0,  1,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0, 23,  0,  0, 23,  0, 23, 23,  0, 23,  0,  0, 23, 23, 23,  0, 23,  0, 23,  0, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_ETSI,                    {'E','U'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-{CCODE_RESV3,                   {' ',' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_RESV4,                   {' ',' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_RESV5,                   {' ',' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_RESV6,                   {' ',' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_RESV7,                   {' ',' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_RESV8,                   {' ',' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_RESV9,                   {' ',' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_RESVa,                   {' ',' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_RESVb,                   {' ',' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_RESVc,                   {' ',' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_RESVd,                   {' ',' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_RESVe,                   {' ',' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_ALLBAND,                 {' ',' '},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_ALBANIA,                 {'A','L'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_ALGERIA,                 {'D','Z'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_ARGENTINA,               {'A','R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17, 17, 17,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30,  0}  },
-{CCODE_ARMENIA,                 {'A','M'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18, 18, 18, 18, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_AUSTRALIA,               {'A','U'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23,  0, 23,  0, 23,  0, 23, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-{CCODE_AUSTRIA,                 {'A','T'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 15,  0, 15,  0, 15,  0, 15,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_AZERBAIJAN,              {'A','Z'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18, 18, 18, 18, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_BAHRAIN,                 {'B','H'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_BELARUS,                 {'B','Y'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_BELGIUM,                 {'B','E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_BELIZE,                  {'B','Z'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-                                         ,  {  30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-{CCODE_BOLIVIA,                 {'B','O'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-                                         ,  {  30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-{CCODE_BRAZIL,                  {'B','R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_BRUNEI_DARUSSALAM,       {'B','N'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-{CCODE_BULGARIA,                {'B','G'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  0,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23,  0, 23,  0, 23,  0, 23, 23, 23,  0,  0, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30,  0,  0,  0,  0,  0}  },
-{CCODE_CANADA,                  {'C','A'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-                                         ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-{CCODE_CHILE,                   {'C','L'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17, 17, 17, 17, 17}  },
-{CCODE_CHINA,                   {'C','N'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-{CCODE_COLOMBIA,                {'C','O'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-                                         ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-{CCODE_COSTA_RICA,              {'C','R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_CROATIA,                 {'H','R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_CYPRUS,                  {'C','Y'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-{CCODE_CZECH,                   {'C','Z'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_DENMARK,                 {'D','K'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-{CCODE_DOMINICAN_REPUBLIC,      {'D','O'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-                                         ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-{CCODE_ECUADOR,                 {'E','C'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_EGYPT,                   {'E','G'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_EL_SALVADOR,             {'S','V'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_ESTONIA,                 {'E','E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-{CCODE_FINLAND,                 {'F','I'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-{CCODE_FRANCE,                  {'F','R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_GERMANY,                 {'D','E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-{CCODE_GREECE,                  {'G','R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_GEORGIA,                 {'G','E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18, 18, 18, 18, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_GUATEMALA,               {'G','T'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-                                         ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-{CCODE_HONDURAS,                {'H','N'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_HONG_KONG,               {'H','K'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23,  0, 23,  0, 23,  0, 23, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-{CCODE_HUNGARY,                 {'H','U'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_ICELAND,                 {'I','S'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-{CCODE_INDIA,                   {'I','N'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_INDONESIA,               {'I','D'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_IRAN,                    {'I','R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-{CCODE_IRELAND,                 {'I','E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-{CCODE_ITALY,                   {'I','T'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-{CCODE_ISRAEL,                  {'I','L'},  {   0,  0,  0,  0,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_JAPAN,                   {'J','P'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23,  0, 23,  0, 23,  0, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_JORDAN,                  {'J','O'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_KAZAKHSTAN,              {'K','Z'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_KUWAIT,                  {'K','W'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_LATVIA,                  {'L','V'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_LEBANON,                 {'L','B'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_LEICHTENSTEIN,           {'L','I'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_LITHUANIA,               {'L','T'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-{CCODE_LUXEMBURG,               {'L','U'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-{CCODE_MACAU,                   {'M','O'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23,  0, 23,  0, 23,  0, 23, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-{CCODE_MACEDONIA,               {'M','K'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_MALTA,                   {'M','T'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
-                                         ,  {  23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 16,  0, 16,  0, 16,  0, 16, 16, 16, 16, 16,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 16, 16, 16, 16,  0}  },
-{CCODE_MALAYSIA,                {'M','Y'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_MEXICO,                  {'M','X'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-                                         ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-{CCODE_MONACO,                  {'M','C'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18, 18, 18, 18, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_MOROCCO,                 {'M','A'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_NETHERLANDS,             {'N','L'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-{CCODE_NEW_ZEALAND,             {'N','Z'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-                                         ,  {  30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23,  0, 23,  0, 23,  0, 23, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-{CCODE_NORTH_KOREA,             {'K','P'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23, 23, 23, 23,  0}  },
-{CCODE_NORWAY,                  {'N','O'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-{CCODE_OMAN,                    {'O','M'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_PAKISTAN,                {'P','K'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_PANAMA,                  {'P','A'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-                                         ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-{CCODE_PERU,                    {'P','E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_PHILIPPINES,             {'P','H'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-{CCODE_POLAND,                  {'P','L'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-{CCODE_PORTUGAL,                {'P','T'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-{CCODE_PUERTO_RICO,             {'P','R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-                                         ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-{CCODE_QATAR,                   {'Q','A'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_ROMANIA,                 {'R','O'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_RUSSIA,                  {'R','U'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_SAUDI_ARABIA,            {'S','A'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_SINGAPORE,               {'S','G'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20, 20, 20, 20, 20}  },
-{CCODE_SLOVAKIA,                {'S','K'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
-                                         ,  {  23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 16,  0, 16,  0, 16,  0, 16, 16, 16, 16, 16,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 16, 16, 16, 16,  0}  },
-{CCODE_SLOVENIA,                {'S','I'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-{CCODE_SOUTH_AFRICA,            {'Z','A'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-{CCODE_SOUTH_KOREA,             {'K','R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23, 23, 23, 23,  0}  },
-{CCODE_SPAIN,                   {'E','S'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
-                                         ,  {  23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 16,  0, 16,  0, 16,  0, 16, 16, 16, 16, 16,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 16, 16, 16, 16,  0}  },
-{CCODE_SWEDEN,                  {'S','E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-{CCODE_SWITZERLAND,             {'C','H'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_SYRIA,                   {'S','Y'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_TAIWAN,                  {'T','W'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17, 17, 17,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30,  0}  },
-{CCODE_THAILAND,                {'T','H'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23, 23, 23, 23,  0}  },
-{CCODE_TRINIDAD_TOBAGO,         {'T','T'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18, 18, 18, 18, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_TUNISIA,                 {'T','N'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_TURKEY,                  {'T','R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_UK,                      {'G','B'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-{CCODE_UKRAINE,                 {'U','A'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_UNITED_ARAB_EMIRATES,    {'A','E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_UNITED_STATES,           {'U','S'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-                                         ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-{CCODE_URUGUAY,                 {'U','Y'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23, 23, 23, 23,  0}  },
-{CCODE_UZBEKISTAN,              {'U','Z'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_VENEZUELA,               {'V','E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
-                                         ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23, 23, 23, 23,  0}  },
-{CCODE_VIETNAM,                 {'V','N'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_YEMEN,                   {'Y','E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_ZIMBABWE,                {'Z','W'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_JAPAN_W52_W53,           {'J','J'},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-{CCODE_MAX,                     {'U','N'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1}
-                                         ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  }
+	{CCODE_FCC,                     {'U' , 'S'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
+	 ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
+	{CCODE_TELEC,                   {'J' , 'P'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  1,  0,  0,  1,  0,  1,  1,  0,  1,  0,  0,  1,  1,  1,  0,  1,  0,  1,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0, 23,  0,  0, 23,  0, 23, 23,  0, 23,  0,  0, 23, 23, 23,  0, 23,  0, 23,  0, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_ETSI,                    {'E' , 'U'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
+	{CCODE_RESV3,                   {' ' , ' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_RESV4,                   {' ' , ' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_RESV5,                   {' ' , ' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_RESV6,                   {' ' , ' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_RESV7,                   {' ' , ' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_RESV8,                   {' ' , ' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_RESV9,                   {' ' , ' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_RESVa,                   {' ' , ' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_RESVb,                   {' ' , ' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_RESVc,                   {' ' , ' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_RESVd,                   {' ' , ' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_RESVe,                   {' ' , ' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_ALLBAND,                 {' ' , ' '},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_ALBANIA,                 {'A' , 'L'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_ALGERIA,                 {'D' , 'Z'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_ARGENTINA,               {'A' , 'R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17, 17, 17,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30,  0}  },
+	{CCODE_ARMENIA,                 {'A' , 'M'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18, 18, 18, 18, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_AUSTRALIA,               {'A' , 'U'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23,  0, 23,  0, 23,  0, 23, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
+	{CCODE_AUSTRIA,                 {'A' , 'T'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 15,  0, 15,  0, 15,  0, 15,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_AZERBAIJAN,              {'A' , 'Z'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18, 18, 18, 18, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_BAHRAIN,                 {'B' , 'H'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_BELARUS,                 {'B' , 'Y'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_BELGIUM,                 {'B' , 'E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_BELIZE,                  {'B' , 'Z'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
+	 ,  {  30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
+	{CCODE_BOLIVIA,                 {'B' , 'O'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
+	 ,  {  30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
+	{CCODE_BRAZIL,                  {'B' , 'R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_BRUNEI_DARUSSALAM,       {'B' , 'N'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
+	{CCODE_BULGARIA,                {'B' , 'G'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  0,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23,  0, 23,  0, 23,  0, 23, 23, 23,  0,  0, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30,  0,  0,  0,  0,  0}  },
+	{CCODE_CANADA,                  {'C' , 'A'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
+	 ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
+	{CCODE_CHILE,                   {'C' , 'L'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17, 17, 17, 17, 17}  },
+	{CCODE_CHINA,                   {'C' , 'N'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
+	{CCODE_COLOMBIA,                {'C' , 'O'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
+	 ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
+	{CCODE_COSTA_RICA,              {'C' , 'R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_CROATIA,                 {'H' , 'R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_CYPRUS,                  {'C' , 'Y'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
+	{CCODE_CZECH,                   {'C' , 'Z'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_DENMARK,                 {'D' , 'K'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
+	{CCODE_DOMINICAN_REPUBLIC,      {'D' , 'O'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
+	 ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
+	{CCODE_ECUADOR,                 {'E' , 'C'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_EGYPT,                   {'E' , 'G'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_EL_SALVADOR,             {'S' , 'V'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_ESTONIA,                 {'E' , 'E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
+	{CCODE_FINLAND,                 {'F' , 'I'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
+	{CCODE_FRANCE,                  {'F' , 'R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_GERMANY,                 {'D' , 'E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
+	{CCODE_GREECE,                  {'G' , 'R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_GEORGIA,                 {'G' , 'E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18, 18, 18, 18, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_GUATEMALA,               {'G' , 'T'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
+	 ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
+	{CCODE_HONDURAS,                {'H' , 'N'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_HONG_KONG,               {'H' , 'K'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23,  0, 23,  0, 23,  0, 23, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
+	{CCODE_HUNGARY,                 {'H' , 'U'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_ICELAND,                 {'I' , 'S'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
+	{CCODE_INDIA,                   {'I' , 'N'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_INDONESIA,               {'I' , 'D'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_IRAN,                    {'I' , 'R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
+	{CCODE_IRELAND,                 {'I' , 'E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
+	{CCODE_ITALY,                   {'I' , 'T'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
+	{CCODE_ISRAEL,                  {'I' , 'L'},  {   0,  0,  0,  0,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_JAPAN,                   {'J' , 'P'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23,  0, 23,  0, 23,  0, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_JORDAN,                  {'J' , 'O'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_KAZAKHSTAN,              {'K' , 'Z'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_KUWAIT,                  {'K' , 'W'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_LATVIA,                  {'L' , 'V'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_LEBANON,                 {'L' , 'B'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_LEICHTENSTEIN,           {'L' , 'I'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_LITHUANIA,               {'L' , 'T'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
+	{CCODE_LUXEMBURG,               {'L' , 'U'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
+	{CCODE_MACAU,                   {'M' , 'O'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23,  0, 23,  0, 23,  0, 23, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
+	{CCODE_MACEDONIA,               {'M' , 'K'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_MALTA,                   {'M' , 'T'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
+	 ,  {  23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 16,  0, 16,  0, 16,  0, 16, 16, 16, 16, 16,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 16, 16, 16, 16,  0}  },
+	{CCODE_MALAYSIA,                {'M' , 'Y'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_MEXICO,                  {'M' , 'X'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
+	 ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
+	{CCODE_MONACO,                  {'M' , 'C'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18, 18, 18, 18, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_MOROCCO,                 {'M' , 'A'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_NETHERLANDS,             {'N' , 'L'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
+	{CCODE_NEW_ZEALAND,             {'N' , 'Z'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
+	 ,  {  30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23,  0, 23,  0, 23,  0, 23, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
+	{CCODE_NORTH_KOREA,             {'K' , 'P'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23, 23, 23, 23,  0}  },
+	{CCODE_NORWAY,                  {'N' , 'O'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
+	{CCODE_OMAN,                    {'O' , 'M'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_PAKISTAN,                {'P' , 'K'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_PANAMA,                  {'P' , 'A'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
+	 ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
+	{CCODE_PERU,                    {'P' , 'E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_PHILIPPINES,             {'P' , 'H'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
+	{CCODE_POLAND,                  {'P' , 'L'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
+	{CCODE_PORTUGAL,                {'P' , 'T'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
+	{CCODE_PUERTO_RICO,             {'P' , 'R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
+	 ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
+	{CCODE_QATAR,                   {'Q' , 'A'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_ROMANIA,                 {'R' , 'O'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_RUSSIA,                  {'R' , 'U'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_SAUDI_ARABIA,            {'S' , 'A'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_SINGAPORE,               {'S' , 'G'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20, 20, 20, 20, 20}  },
+	{CCODE_SLOVAKIA,                {'S' , 'K'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
+	 ,  {  23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 16,  0, 16,  0, 16,  0, 16, 16, 16, 16, 16,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 16, 16, 16, 16,  0}  },
+	{CCODE_SLOVENIA,                {'S' , 'I'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
+	{CCODE_SOUTH_AFRICA,            {'Z' , 'A'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
+	{CCODE_SOUTH_KOREA,             {'K' , 'R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23, 23, 23, 23,  0}  },
+	{CCODE_SPAIN,                   {'E' , 'S'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
+	 ,  {  23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 16,  0, 16,  0, 16,  0, 16, 16, 16, 16, 16,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 16, 16, 16, 16,  0}  },
+	{CCODE_SWEDEN,                  {'S' , 'E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
+	{CCODE_SWITZERLAND,             {'C' , 'H'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_SYRIA,                   {'S' , 'Y'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_TAIWAN,                  {'T' , 'W'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17, 17, 17,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30,  0}  },
+	{CCODE_THAILAND,                {'T' , 'H'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23, 23, 23, 23,  0}  },
+	{CCODE_TRINIDAD_TOBAGO,         {'T' , 'T'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18, 18, 18, 18, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_TUNISIA,                 {'T' , 'N'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_TURKEY,                  {'T' , 'R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_UK,                      {'G' , 'B'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
+	{CCODE_UKRAINE,                 {'U' , 'A'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_UNITED_ARAB_EMIRATES,    {'A' , 'E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_UNITED_STATES,           {'U' , 'S'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
+	 ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
+	{CCODE_URUGUAY,                 {'U' , 'Y'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23, 23, 23, 23,  0}  },
+	{CCODE_UZBEKISTAN,              {'U' , 'Z'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_VENEZUELA,               {'V' , 'E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
+	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23, 23, 23, 23,  0}  },
+	{CCODE_VIETNAM,                 {'V' , 'N'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_YEMEN,                   {'Y' , 'E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_ZIMBABWE,                {'Z' , 'W'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_JAPAN_W52_W53,           {'J' , 'J'},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
+	{CCODE_MAX,                     {'U' , 'N'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1}
+	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  }
 /*                                              1   2   3   4   5   6   7   8   9  10  11  12  13  14  15  16  17  18  19  20  21  22  23  24  25  26  27  28  29  30  31  32  33  34  35  36  37  38  39  40  41  42  43  44  45  46  47  48  49  50  51  52  53  54  55  56  */
 };
 
@@ -382,8 +382,7 @@ bool is_channel_valid(unsigned int ChannelIndex)
 	 * If Channel Index is invalid, return invalid
 	 */
 	if ((ChannelIndex > CB_MAX_CHANNEL) ||
-		(ChannelIndex == 0))
-	{
+	    (ChannelIndex == 0)) {
 		bValid = false;
 		goto exit;
 	}
@@ -391,8 +390,7 @@ bool is_channel_valid(unsigned int ChannelIndex)
 	bValid = sChannelTbl[ChannelIndex].bValid;
 
 exit:
-	return (bValid);
-
+	return bValid;
 }
 
 /**
@@ -410,11 +408,11 @@ exit:
 bool channel_get_list(unsigned int uCountryCodeIdx, unsigned char *pbyChannelTable)
 {
 	if (uCountryCodeIdx >= CCODE_MAX)
-		return (false);
+		return false;
 
 	memcpy(pbyChannelTable, ChannelRuleTab[uCountryCodeIdx].bChannelIdxList, CB_MAX_CHANNEL);
 
-	return (true);
+	return true;
 }
 
 void init_channel_table(void *pDeviceHandler)
@@ -423,75 +421,74 @@ void init_channel_table(void *pDeviceHandler)
 	bool bMultiBand = false;
 	unsigned int ii;
 
-	for(ii = 1 ; ii<=CARD_MAX_CHANNEL_TBL ; ii++) {
+	for (ii = 1; ii <= CARD_MAX_CHANNEL_TBL; ii++) {
 		sChannelTbl[ii].bValid = false;
 	}
 
 	switch (pDevice->byRFType) {
-		case RF_RFMD2959 :
-		case RF_AIROHA :
-		case RF_AL2230S:
-		case RF_UW2451 :
-		case RF_VT3226 :
-			//printk("chester-false\n");
-			bMultiBand = false;
-			break;
-		case RF_AIROHA7230 :
-		case RF_UW2452 :
-		case RF_NOTHING :
-		default :
-			bMultiBand = true;
-			break;
+	case RF_RFMD2959:
+	case RF_AIROHA:
+	case RF_AL2230S:
+	case RF_UW2451:
+	case RF_VT3226:
+		bMultiBand = false;
+		break;
+	case RF_AIROHA7230:
+	case RF_UW2452:
+	case RF_NOTHING:
+	default:
+		bMultiBand = true;
+		break;
 	}
 
 	if ((pDevice->dwDiagRefCount != 0) || (pDevice->b11hEnable == true)) {
 		if (bMultiBand == true) {
-			for(ii = 0 ; ii<CARD_MAX_CHANNEL_TBL ; ii++) {
-				sChannelTbl[ii+1].bValid = true;
-				pDevice->abyRegPwr[ii+1] = pDevice->abyOFDMDefaultPwr[ii+1];
-				pDevice->abyLocalPwr[ii+1] = pDevice->abyOFDMDefaultPwr[ii+1];
+			for (ii = 0; ii < CARD_MAX_CHANNEL_TBL; ii++) {
+				sChannelTbl[ii + 1].bValid = true;
+				pDevice->abyRegPwr[ii + 1] = pDevice->abyOFDMDefaultPwr[ii + 1];
+				pDevice->abyLocalPwr[ii + 1] = pDevice->abyOFDMDefaultPwr[ii + 1];
 			}
-			for(ii = 0 ; ii<CHANNEL_MAX_24G ; ii++) {
-				pDevice->abyRegPwr[ii+1] = pDevice->abyCCKDefaultPwr[ii+1];
-				pDevice->abyLocalPwr[ii+1] = pDevice->abyCCKDefaultPwr[ii+1];
+			for (ii = 0; ii < CHANNEL_MAX_24G; ii++) {
+				pDevice->abyRegPwr[ii + 1] = pDevice->abyCCKDefaultPwr[ii + 1];
+				pDevice->abyLocalPwr[ii + 1] = pDevice->abyCCKDefaultPwr[ii + 1];
 			}
 		} else {
-			for(ii = 0 ; ii<CHANNEL_MAX_24G ; ii++) {
+			for (ii = 0; ii < CHANNEL_MAX_24G; ii++) {
 				//2008-8-4 <add> by chester
 				if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] != 0) {
-					sChannelTbl[ii+1].bValid = true;
-					pDevice->abyRegPwr[ii+1] = pDevice->abyCCKDefaultPwr[ii+1];
-					pDevice->abyLocalPwr[ii+1] = pDevice->abyCCKDefaultPwr[ii+1];
+					sChannelTbl[ii + 1].bValid = true;
+					pDevice->abyRegPwr[ii + 1] = pDevice->abyCCKDefaultPwr[ii + 1];
+					pDevice->abyLocalPwr[ii + 1] = pDevice->abyCCKDefaultPwr[ii + 1];
 				}
 			}
 		}
 	} else if (pDevice->byZoneType <= CCODE_MAX) {
 		if (bMultiBand == true) {
-			for(ii = 0 ; ii<CARD_MAX_CHANNEL_TBL ; ii++) {
+			for (ii = 0; ii < CARD_MAX_CHANNEL_TBL; ii++) {
 				if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] != 0) {
-					sChannelTbl[ii+1].bValid = true;
-					pDevice->abyRegPwr[ii+1] = ChannelRuleTab[pDevice->byZoneType].byPower[ii];
-					pDevice->abyLocalPwr[ii+1] = ChannelRuleTab[pDevice->byZoneType].byPower[ii];
+					sChannelTbl[ii + 1].bValid = true;
+					pDevice->abyRegPwr[ii + 1] = ChannelRuleTab[pDevice->byZoneType].byPower[ii];
+					pDevice->abyLocalPwr[ii + 1] = ChannelRuleTab[pDevice->byZoneType].byPower[ii];
 				}
 			}
 		} else {
-			for(ii = 0 ; ii<CHANNEL_MAX_24G ; ii++) {
+			for (ii = 0; ii < CHANNEL_MAX_24G; ii++) {
 				if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] != 0) {
-					sChannelTbl[ii+1].bValid = true;
-					pDevice->abyRegPwr[ii+1] = ChannelRuleTab[pDevice->byZoneType].byPower[ii];
-					pDevice->abyLocalPwr[ii+1] = ChannelRuleTab[pDevice->byZoneType].byPower[ii];
+					sChannelTbl[ii + 1].bValid = true;
+					pDevice->abyRegPwr[ii + 1] = ChannelRuleTab[pDevice->byZoneType].byPower[ii];
+					pDevice->abyLocalPwr[ii + 1] = ChannelRuleTab[pDevice->byZoneType].byPower[ii];
 				}
 			}
 		}
 	}
 
-	DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO"Zone=[%d][%c][%c]!!\n",pDevice->byZoneType,ChannelRuleTab[pDevice->byZoneType].chCountryCode[0],ChannelRuleTab[pDevice->byZoneType].chCountryCode[1]);
+	DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Zone=[%d][%c][%c]!!\n", pDevice->byZoneType, ChannelRuleTab[pDevice->byZoneType].chCountryCode[0], ChannelRuleTab[pDevice->byZoneType].chCountryCode[1]);
 
-	for(ii = 0 ; ii<CARD_MAX_CHANNEL_TBL ; ii++) {
-		if (pDevice->abyRegPwr[ii+1] == 0)
-			pDevice->abyRegPwr[ii+1] = pDevice->abyOFDMDefaultPwr[ii+1];
-		if (pDevice->abyLocalPwr[ii+1] == 0)
-			pDevice->abyLocalPwr[ii+1] = pDevice->abyOFDMDefaultPwr[ii+1];
+	for (ii = 0; ii < CARD_MAX_CHANNEL_TBL; ii++) {
+		if (pDevice->abyRegPwr[ii + 1] == 0)
+			pDevice->abyRegPwr[ii + 1] = pDevice->abyOFDMDefaultPwr[ii + 1];
+		if (pDevice->abyLocalPwr[ii + 1] == 0)
+			pDevice->abyLocalPwr[ii + 1] = pDevice->abyOFDMDefaultPwr[ii + 1];
 	}
 }
 
@@ -500,11 +497,11 @@ unsigned char get_channel_mapping(void *pDeviceHandler, unsigned char byChannelN
 	unsigned int ii;
 
 	if ((ePhyType == PHY_TYPE_11B) || (ePhyType == PHY_TYPE_11G))
-		return (byChannelNumber);
+		return byChannelNumber;
 
-	for(ii = (CB_MAX_CHANNEL_24G + 1); ii <= CB_MAX_CHANNEL; ) {
+	for (ii = (CB_MAX_CHANNEL_24G + 1); ii <= CB_MAX_CHANNEL;) {
 		if (sChannelTbl[ii].byChannelNumber == byChannelNumber)
-			return ((unsigned char) ii);
+			return (unsigned char) ii;
 		ii++;
 	}
 	return 0;
@@ -513,7 +510,7 @@ unsigned char get_channel_mapping(void *pDeviceHandler, unsigned char byChannelN
 unsigned char get_channel_number(void *pDeviceHandler, unsigned char byChannelIndex)
 {
 	//PSDevice    pDevice = (PSDevice) pDeviceHandler;
-	return(sChannelTbl[byChannelIndex].byChannelNumber);
+	return sChannelTbl[byChannelIndex].byChannelNumber;
 }
 
 /**
@@ -525,25 +522,24 @@ unsigned char get_channel_number(void *pDeviceHandler, unsigned char byChannelIn
  * Return Value: true if succeeded; false if failed.
  *
  */
-bool set_channel (void *pDeviceHandler, unsigned int uConnectionChannel)
+bool set_channel(void *pDeviceHandler, unsigned int uConnectionChannel)
 {
 	PSDevice pDevice = (PSDevice) pDeviceHandler;
 	bool bResult = true;
 
-
 	if (pDevice->byCurrentCh == uConnectionChannel) {
 		return bResult;
 	}
 
 	if (sChannelTbl[uConnectionChannel].bValid == false) {
-		return (false);
+		return false;
 	}
 
 	if ((uConnectionChannel > CB_MAX_CHANNEL_24G) &&
-			(pDevice->eCurrentPHYType != PHY_TYPE_11A)) {
+	    (pDevice->eCurrentPHYType != PHY_TYPE_11A)) {
 		CARDbSetPhyParameter(pDevice, PHY_TYPE_11A, 0, 0, NULL, NULL);
 	} else if ((uConnectionChannel <= CB_MAX_CHANNEL_24G) &&
-			(pDevice->eCurrentPHYType == PHY_TYPE_11A)) {
+		   (pDevice->eCurrentPHYType == PHY_TYPE_11A)) {
 		CARDbSetPhyParameter(pDevice, PHY_TYPE_11G, 0, 0, NULL, NULL);
 	}
 	// clear NAV
@@ -552,13 +548,11 @@ bool set_channel (void *pDeviceHandler, unsigned int uConnectionChannel)
 	//{{ RobertYu: 20041202
 	//// TX_PE will reserve 3 us for MAX2829 A mode only, it is for better TX throughput
 
-	if ( pDevice->byRFType == RF_AIROHA7230 )
-	{
+	if (pDevice->byRFType == RF_AIROHA7230) {
 		RFbAL7230SelectChannelPostProcess(pDevice->PortOffset, pDevice->byCurrentCh, (unsigned char)uConnectionChannel);
 	}
 	//}} RobertYu
 
-
 	pDevice->byCurrentCh = (unsigned char)uConnectionChannel;
 	bResult &= RFbSelectChannel(pDevice->PortOffset, pDevice->byRFType, (unsigned char)uConnectionChannel);
 
@@ -566,8 +560,7 @@ bool set_channel (void *pDeviceHandler, unsigned int uConnectionChannel)
 	if (pDevice->bEnablePSMode == true)
 		RFvWriteWakeProgSyn(pDevice->PortOffset, pDevice->byRFType, uConnectionChannel);
 
-
-	//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"CARDbSetMediaChannel: %d\n", (unsigned char)uConnectionChannel);
+	//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "CARDbSetMediaChannel: %d\n", (unsigned char)uConnectionChannel);
 	BBvSoftwareReset(pDevice->PortOffset);
 
 	if (pDevice->byLocalID > REV_ID_VT3253_B1) {
@@ -581,18 +574,12 @@ bool set_channel (void *pDeviceHandler, unsigned int uConnectionChannel)
 	}
 
 	if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
-#ifdef	PLICE_DEBUG
-		//printk("Func:ChbSetChannel:call RFbSetPower:11B\n");
-#endif
 		RFbSetPower(pDevice, RATE_1M, pDevice->byCurrentCh);
 	} else {
-#ifdef	PLICE_DEBUG
-		//printk("Func:ChbSetChannel:call RFbSetPower\n");
-#endif
 		RFbSetPower(pDevice, RATE_6M, pDevice->byCurrentCh);
 	}
 
-	return(bResult);
+	return bResult;
 }
 
 /**
@@ -612,19 +599,18 @@ void set_country_info(void *pDeviceHandler, CARD_PHY_TYPE ePHYType, void *pIE)
 	unsigned char byCh = 0;
 	PWLAN_IE_COUNTRY pIE_Country = (PWLAN_IE_COUNTRY) pIE;
 
-
 	uNumOfCountryInfo = (pIE_Country->len - 3);
 	uNumOfCountryInfo /= 3;
 
 	if (ePHYType == PHY_TYPE_11A) {
 		pDevice->bCountryInfo5G = true;
-		for(ii = CB_MAX_CHANNEL_24G + 1 ; ii <= CARD_MAX_CHANNEL_TBL ; ii++) {
+		for (ii = CB_MAX_CHANNEL_24G + 1; ii <= CARD_MAX_CHANNEL_TBL; ii++) {
 			sChannelTbl[ii].bValid = false;
 		}
 		step = 4;
 	} else {
 		pDevice->bCountryInfo24G = true;
-		for(ii = 1 ; ii <= CB_MAX_CHANNEL_24G ; ii++) {
+		for (ii = 1; ii <= CB_MAX_CHANNEL_24G; ii++) {
 			sChannelTbl[ii].bValid = false;
 		}
 		step = 1;
@@ -633,8 +619,8 @@ void set_country_info(void *pDeviceHandler, CARD_PHY_TYPE ePHYType, void *pIE)
 	pDevice->abyCountryCode[1] = pIE_Country->abyCountryString[1];
 	pDevice->abyCountryCode[2] = pIE_Country->abyCountryString[2];
 
-	for(ii = 0 ; ii < uNumOfCountryInfo ; ii++) {
-		for(uu = 0 ; uu < pIE_Country->abyCountryInfo[ii*3+1] ; uu++) {
+	for (ii = 0; ii < uNumOfCountryInfo; ii++) {
+		for (uu = 0; uu < pIE_Country->abyCountryInfo[ii*3+1]; uu++) {
 			byCh = get_channel_mapping(pDevice, (unsigned char)(pIE_Country->abyCountryInfo[ii*3]+step*uu), ePHYType);
 			sChannelTbl[byCh].bValid = true;
 			pDevice->abyRegPwr[byCh] = pIE_Country->abyCountryInfo[ii*3+2];
@@ -661,7 +647,6 @@ unsigned char set_support_channels(void *pDeviceHandler, unsigned char *pbyIEs)
 	unsigned char *pbyChTupple;
 	unsigned char byLen = 0;
 
-
 	pIE->byElementID = WLAN_EID_SUPP_CH;
 	pIE->len = 0;
 	pbyChTupple = pIE->abyChannelTuple;
@@ -669,7 +654,7 @@ unsigned char set_support_channels(void *pDeviceHandler, unsigned char *pbyIEs)
 	// lower band
 	byCount = 0;
 	if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[28] == true) {
-		for (ii = 28 ; ii < 36 ; ii+= 2) {
+		for (ii = 28; ii < 36; ii += 2) {
 			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true) {
 				byCount++;
 			}
@@ -678,7 +663,7 @@ unsigned char set_support_channels(void *pDeviceHandler, unsigned char *pbyIEs)
 		*pbyChTupple++ = byCount;
 		byLen += 2;
 	} else if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[29] == true) {
-		for (ii = 29 ; ii < 36 ; ii+= 2) {
+		for (ii = 29; ii < 36; ii += 2) {
 			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true) {
 				byCount++;
 			}
@@ -690,7 +675,7 @@ unsigned char set_support_channels(void *pDeviceHandler, unsigned char *pbyIEs)
 	// middle band
 	byCount = 0;
 	if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[36] == true) {
-		for (ii = 36 ; ii < 40 ; ii++) {
+		for (ii = 36; ii < 40; ii++) {
 			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true) {
 				byCount++;
 			}
@@ -702,7 +687,7 @@ unsigned char set_support_channels(void *pDeviceHandler, unsigned char *pbyIEs)
 	// higher band
 	byCount = 0;
 	if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[40] == true) {
-		for (ii = 40 ; ii < 51 ; ii++) {
+		for (ii = 40; ii < 51; ii++) {
 			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true) {
 				byCount++;
 			}
@@ -711,7 +696,7 @@ unsigned char set_support_channels(void *pDeviceHandler, unsigned char *pbyIEs)
 		*pbyChTupple++ = byCount;
 		byLen += 2;
 	} else if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[51] == true) {
-		for (ii = 51 ; ii < 56 ; ii++) {
+		for (ii = 51; ii < 56; ii++) {
 			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true) {
 				byCount++;
 			}
@@ -721,7 +706,7 @@ unsigned char set_support_channels(void *pDeviceHandler, unsigned char *pbyIEs)
 		byLen += 2;
 	}
 	pIE->len += (byLen - 2);
-	return (byLen);
+	return byLen;
 }
 
 void set_country_IE(void *pDeviceHandler, void *pIE)
@@ -735,9 +720,9 @@ void set_country_IE(void *pDeviceHandler, void *pIE)
 	pIECountry->abyCountryString[0] = ChannelRuleTab[pDevice->byZoneType].chCountryCode[0];
 	pIECountry->abyCountryString[1] = ChannelRuleTab[pDevice->byZoneType].chCountryCode[1];
 	pIECountry->abyCountryString[2] = ' ';
-	for (ii = CB_MAX_CHANNEL_24G; ii < CB_MAX_CHANNEL; ii++ ) {
+	for (ii = CB_MAX_CHANNEL_24G; ii < CB_MAX_CHANNEL; ii++) {
 		if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] != 0) {
-			pIECountry->abyCountryInfo[pIECountry->len++] = sChannelTbl[ii+1].byChannelNumber;
+			pIECountry->abyCountryInfo[pIECountry->len++] = sChannelTbl[ii + 1].byChannelNumber;
 			pIECountry->abyCountryInfo[pIECountry->len++] = 1;
 			pIECountry->abyCountryInfo[pIECountry->len++] = ChannelRuleTab[pDevice->byZoneType].byPower[ii];
 		}
@@ -746,9 +731,8 @@ void set_country_IE(void *pDeviceHandler, void *pIE)
 }
 
 bool get_channel_map_info(void *pDeviceHandler, unsigned int uChannelIndex,
-		unsigned char *pbyChannelNumber, unsigned char *pbyMap)
+			  unsigned char *pbyChannelNumber, unsigned char *pbyMap)
 {
-
 	if (uChannelIndex > CB_MAX_CHANNEL)
 		return false;
 
@@ -758,9 +742,8 @@ bool get_channel_map_info(void *pDeviceHandler, unsigned int uChannelIndex,
 }
 
 void set_channel_map_info(void *pDeviceHandler, unsigned int uChannelIndex,
-		unsigned char byMap)
+			  unsigned char byMap)
 {
-
 	if (uChannelIndex > CB_MAX_CHANNEL)
 		return;
 
@@ -779,40 +762,40 @@ unsigned char auto_channel_select(void *pDeviceHandler, CARD_PHY_TYPE ePHYType)
 {
 	unsigned int ii = 0;
 	unsigned char byOptionChannel = 0;
-	int aiWeight[CB_MAX_CHANNEL_24G+1] = {-1000,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
+	int aiWeight[CB_MAX_CHANNEL_24G + 1] = {-1000, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
 
 	if (ePHYType == PHY_TYPE_11A) {
-		for(ii = CB_MAX_CHANNEL_24G + 1 ; ii <= CB_MAX_CHANNEL ; ii++) {
+		for (ii = CB_MAX_CHANNEL_24G + 1; ii <= CB_MAX_CHANNEL; ii++) {
 			if (sChannelTbl[ii].bValid == true) {
 				if (byOptionChannel == 0) {
 					byOptionChannel = (unsigned char) ii;
 				}
 				if (sChannelTbl[ii].byMAP == 0) {
-					return ((unsigned char) ii);
-				} else if ( !(sChannelTbl[ii].byMAP & 0x08)) {
+					return (unsigned char) ii;
+				} else if (!(sChannelTbl[ii].byMAP & 0x08)) {
 					byOptionChannel = (unsigned char) ii;
 				}
 			}
 		}
 	} else {
 		byOptionChannel = 0;
-		for(ii = 1 ; ii <= CB_MAX_CHANNEL_24G ; ii++) {
+		for (ii = 1; ii <= CB_MAX_CHANNEL_24G; ii++) {
 			if (sChannelTbl[ii].bValid == true) {
 				if (sChannelTbl[ii].byMAP == 0) {
 					aiWeight[ii] += 100;
 				} else if (sChannelTbl[ii].byMAP & 0x01) {
 					if (ii > 3) {
-						aiWeight[ii-3] -= 10;
+						aiWeight[ii - 3] -= 10;
 					}
 					if (ii > 2) {
-						aiWeight[ii-2] -= 20;
+						aiWeight[ii - 2] -= 20;
 					}
 					if (ii > 1) {
-						aiWeight[ii-1] -= 40;
+						aiWeight[ii - 1] -= 40;
 					}
 					aiWeight[ii] -= 80;
 					if (ii < CB_MAX_CHANNEL_24G) {
-						aiWeight[ii+1] -= 40;
+						aiWeight[ii + 1] -= 40;
 					}
 					if (ii < (CB_MAX_CHANNEL_24G - 1)) {
 						aiWeight[ii+2] -= 20;
@@ -823,12 +806,12 @@ unsigned char auto_channel_select(void *pDeviceHandler, CARD_PHY_TYPE ePHYType)
 				}
 			}
 		}
-		for(ii = 1 ; ii <= CB_MAX_CHANNEL_24G ; ii++) {
+		for (ii = 1; ii <= CB_MAX_CHANNEL_24G; ii++) {
 			if ((sChannelTbl[ii].bValid == true) &&
-					(aiWeight[ii] > aiWeight[byOptionChannel])) {
+			    (aiWeight[ii] > aiWeight[byOptionChannel])) {
 				byOptionChannel = (unsigned char) ii;
 			}
 		}
 	}
-	return (byOptionChannel);
+	return byOptionChannel;
 }
diff --git a/drivers/staging/vt6655/channel.h b/drivers/staging/vt6655/channel.h
index 7038f0d3bde..c9931d7f665 100644
--- a/drivers/staging/vt6655/channel.h
+++ b/drivers/staging/vt6655/channel.h
@@ -29,12 +29,11 @@
 /*---------------------  Export Classes  ----------------------------*/
 
 typedef struct tagSChannelTblElement {
-    unsigned char byChannelNumber;
-    unsigned int uFrequency;
-    bool bValid;
-    unsigned char byMAP;
-}SChannelTblElement, *PSChannelTblElement;
-
+	unsigned char byChannelNumber;
+	unsigned int uFrequency;
+	bool bValid;
+	unsigned char byMAP;
+} SChannelTblElement, *PSChannelTblElement;
 
 /*---------------------  Export Functions  --------------------------*/
 
@@ -48,11 +47,10 @@ void set_country_info(void *pDeviceHandler, CARD_PHY_TYPE ePHYType, void *pIE);
 unsigned char set_support_channels(void *pDeviceHandler, unsigned char *pbyIEs);
 void set_country_IE(void *pDeviceHandler, void *pIE);
 bool get_channel_map_info(void *pDeviceHandler, unsigned int uChannelIndex,
-		unsigned char *pbyChannelNumber, unsigned char *pbyMap);
+			  unsigned char *pbyChannelNumber, unsigned char *pbyMap);
 void set_channel_map_info(void *pDeviceHandler, unsigned int uChannelIndex,
-		unsigned char byMap);
+			  unsigned char byMap);
 void clear_channel_map_info(void *pDeviceHandler);
 unsigned char auto_channel_select(void *pDeviceHandler, CARD_PHY_TYPE ePHYType);
 
-
 #endif /* _CHANNEL_H_ */
diff --git a/drivers/staging/vt6655/country.h b/drivers/staging/vt6655/country.h
index 05fda410420..415e7672aa3 100644
--- a/drivers/staging/vt6655/country.h
+++ b/drivers/staging/vt6655/country.h
@@ -38,125 +38,125 @@
  * Please check with VNWL.inf/VNWL64.inf/VNWL*.inf
  ************************************************************************/
 typedef enum _COUNTRY_CODE {
-    CCODE_FCC = 0,
-    CCODE_TELEC,
-    CCODE_ETSI,
-    CCODE_RESV3,
-    CCODE_RESV4,
-    CCODE_RESV5,
-    CCODE_RESV6,
-    CCODE_RESV7,
-    CCODE_RESV8,
-    CCODE_RESV9,
-    CCODE_RESVa,
-    CCODE_RESVb,
-    CCODE_RESVc,
-    CCODE_RESVd,
-    CCODE_RESVe,
-    CCODE_ALLBAND,
-    CCODE_ALBANIA,
-    CCODE_ALGERIA,
-    CCODE_ARGENTINA,
-    CCODE_ARMENIA,
-    CCODE_AUSTRALIA,
-    CCODE_AUSTRIA,
-    CCODE_AZERBAIJAN,
-    CCODE_BAHRAIN,
-    CCODE_BELARUS,
-    CCODE_BELGIUM,
-    CCODE_BELIZE,
-    CCODE_BOLIVIA,
-    CCODE_BRAZIL,
-    CCODE_BRUNEI_DARUSSALAM,
-    CCODE_BULGARIA,
-    CCODE_CANADA,
-    CCODE_CHILE,
-    CCODE_CHINA,
-    CCODE_COLOMBIA,
-    CCODE_COSTA_RICA,
-    CCODE_CROATIA,
-    CCODE_CYPRUS,
-    CCODE_CZECH,
-    CCODE_DENMARK,
-    CCODE_DOMINICAN_REPUBLIC,
-    CCODE_ECUADOR,
-    CCODE_EGYPT,
-    CCODE_EL_SALVADOR,
-    CCODE_ESTONIA,
-    CCODE_FINLAND,
-    CCODE_FRANCE,
-    CCODE_GERMANY,
-    CCODE_GREECE,
-    CCODE_GEORGIA,
-    CCODE_GUATEMALA,
-    CCODE_HONDURAS,
-    CCODE_HONG_KONG,
-    CCODE_HUNGARY,
-    CCODE_ICELAND,
-    CCODE_INDIA,
-    CCODE_INDONESIA,
-    CCODE_IRAN,
-    CCODE_IRELAND,
-    CCODE_ITALY,
-    CCODE_ISRAEL,
-    CCODE_JAPAN,
-    CCODE_JORDAN,
-    CCODE_KAZAKHSTAN,
-    CCODE_KUWAIT,
-    CCODE_LATVIA,
-    CCODE_LEBANON,
-    CCODE_LEICHTENSTEIN,
-    CCODE_LITHUANIA,
-    CCODE_LUXEMBURG,
-    CCODE_MACAU,
-    CCODE_MACEDONIA,
-    CCODE_MALTA,
-    CCODE_MALAYSIA,
-    CCODE_MEXICO,
-    CCODE_MONACO,
-    CCODE_MOROCCO,
-    CCODE_NETHERLANDS,
-    CCODE_NEW_ZEALAND,
-    CCODE_NORTH_KOREA,
-    CCODE_NORWAY,
-    CCODE_OMAN,
-    CCODE_PAKISTAN,
-    CCODE_PANAMA,
-    CCODE_PERU,
-    CCODE_PHILIPPINES,
-    CCODE_POLAND,
-    CCODE_PORTUGAL,
-    CCODE_PUERTO_RICO,
-    CCODE_QATAR,
-    CCODE_ROMANIA,
-    CCODE_RUSSIA,
-    CCODE_SAUDI_ARABIA,
-    CCODE_SINGAPORE,
-    CCODE_SLOVAKIA,
-    CCODE_SLOVENIA,
-    CCODE_SOUTH_AFRICA,
-    CCODE_SOUTH_KOREA,
-    CCODE_SPAIN,
-    CCODE_SWEDEN,
-    CCODE_SWITZERLAND,
-    CCODE_SYRIA,
-    CCODE_TAIWAN,
-    CCODE_THAILAND,
-    CCODE_TRINIDAD_TOBAGO,
-    CCODE_TUNISIA,
-    CCODE_TURKEY,
-    CCODE_UK,
-    CCODE_UKRAINE,
-    CCODE_UNITED_ARAB_EMIRATES,
-    CCODE_UNITED_STATES,
-    CCODE_URUGUAY,
-    CCODE_UZBEKISTAN,
-    CCODE_VENEZUELA,
-    CCODE_VIETNAM,
-    CCODE_YEMEN,
-    CCODE_ZIMBABWE,
-    CCODE_JAPAN_W52_W53,
-    CCODE_MAX
+	CCODE_FCC = 0,
+	CCODE_TELEC,
+	CCODE_ETSI,
+	CCODE_RESV3,
+	CCODE_RESV4,
+	CCODE_RESV5,
+	CCODE_RESV6,
+	CCODE_RESV7,
+	CCODE_RESV8,
+	CCODE_RESV9,
+	CCODE_RESVa,
+	CCODE_RESVb,
+	CCODE_RESVc,
+	CCODE_RESVd,
+	CCODE_RESVe,
+	CCODE_ALLBAND,
+	CCODE_ALBANIA,
+	CCODE_ALGERIA,
+	CCODE_ARGENTINA,
+	CCODE_ARMENIA,
+	CCODE_AUSTRALIA,
+	CCODE_AUSTRIA,
+	CCODE_AZERBAIJAN,
+	CCODE_BAHRAIN,
+	CCODE_BELARUS,
+	CCODE_BELGIUM,
+	CCODE_BELIZE,
+	CCODE_BOLIVIA,
+	CCODE_BRAZIL,
+	CCODE_BRUNEI_DARUSSALAM,
+	CCODE_BULGARIA,
+	CCODE_CANADA,
+	CCODE_CHILE,
+	CCODE_CHINA,
+	CCODE_COLOMBIA,
+	CCODE_COSTA_RICA,
+	CCODE_CROATIA,
+	CCODE_CYPRUS,
+	CCODE_CZECH,
+	CCODE_DENMARK,
+	CCODE_DOMINICAN_REPUBLIC,
+	CCODE_ECUADOR,
+	CCODE_EGYPT,
+	CCODE_EL_SALVADOR,
+	CCODE_ESTONIA,
+	CCODE_FINLAND,
+	CCODE_FRANCE,
+	CCODE_GERMANY,
+	CCODE_GREECE,
+	CCODE_GEORGIA,
+	CCODE_GUATEMALA,
+	CCODE_HONDURAS,
+	CCODE_HONG_KONG,
+	CCODE_HUNGARY,
+	CCODE_ICELAND,
+	CCODE_INDIA,
+	CCODE_INDONESIA,
+	CCODE_IRAN,
+	CCODE_IRELAND,
+	CCODE_ITALY,
+	CCODE_ISRAEL,
+	CCODE_JAPAN,
+	CCODE_JORDAN,
+	CCODE_KAZAKHSTAN,
+	CCODE_KUWAIT,
+	CCODE_LATVIA,
+	CCODE_LEBANON,
+	CCODE_LEICHTENSTEIN,
+	CCODE_LITHUANIA,
+	CCODE_LUXEMBURG,
+	CCODE_MACAU,
+	CCODE_MACEDONIA,
+	CCODE_MALTA,
+	CCODE_MALAYSIA,
+	CCODE_MEXICO,
+	CCODE_MONACO,
+	CCODE_MOROCCO,
+	CCODE_NETHERLANDS,
+	CCODE_NEW_ZEALAND,
+	CCODE_NORTH_KOREA,
+	CCODE_NORWAY,
+	CCODE_OMAN,
+	CCODE_PAKISTAN,
+	CCODE_PANAMA,
+	CCODE_PERU,
+	CCODE_PHILIPPINES,
+	CCODE_POLAND,
+	CCODE_PORTUGAL,
+	CCODE_PUERTO_RICO,
+	CCODE_QATAR,
+	CCODE_ROMANIA,
+	CCODE_RUSSIA,
+	CCODE_SAUDI_ARABIA,
+	CCODE_SINGAPORE,
+	CCODE_SLOVAKIA,
+	CCODE_SLOVENIA,
+	CCODE_SOUTH_AFRICA,
+	CCODE_SOUTH_KOREA,
+	CCODE_SPAIN,
+	CCODE_SWEDEN,
+	CCODE_SWITZERLAND,
+	CCODE_SYRIA,
+	CCODE_TAIWAN,
+	CCODE_THAILAND,
+	CCODE_TRINIDAD_TOBAGO,
+	CCODE_TUNISIA,
+	CCODE_TURKEY,
+	CCODE_UK,
+	CCODE_UKRAINE,
+	CCODE_UNITED_ARAB_EMIRATES,
+	CCODE_UNITED_STATES,
+	CCODE_URUGUAY,
+	CCODE_UZBEKISTAN,
+	CCODE_VENEZUELA,
+	CCODE_VIETNAM,
+	CCODE_YEMEN,
+	CCODE_ZIMBABWE,
+	CCODE_JAPAN_W52_W53,
+	CCODE_MAX
 } COUNTRY_CODE;
 
 #endif  /* __COUNTRY_H__ */
diff --git a/drivers/staging/vt6655/datarate.c b/drivers/staging/vt6655/datarate.c
index b86ec1b6d18..e7b6bc7de4a 100644
--- a/drivers/staging/vt6655/datarate.c
+++ b/drivers/staging/vt6655/datarate.c
@@ -45,16 +45,12 @@
 
 /*---------------------  Static Definitions -------------------------*/
 
-
-
-
 /*---------------------  Static Classes  ----------------------------*/
 
-
- extern unsigned short TxRate_iwconfig; //2008-5-8 <add> by chester
+extern unsigned short TxRate_iwconfig; //2008-5-8 <add> by chester
 /*---------------------  Static Variables  --------------------------*/
 //static int          msglevel                =MSG_LEVEL_DEBUG;
-static int          msglevel                =MSG_LEVEL_INFO;
+static int msglevel = MSG_LEVEL_INFO;
 const unsigned char acbyIERate[MAX_RATE] =
 {0x02, 0x04, 0x0B, 0x16, 0x0C, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C};
 
@@ -64,32 +60,28 @@ const unsigned char acbyIERate[MAX_RATE] =
 
 /*---------------------  Static Functions  --------------------------*/
 
-void s_vResetCounter (
-    PKnownNodeDB psNodeDBTable
-    );
-
-
+void s_vResetCounter(
+	PKnownNodeDB psNodeDBTable
+);
 
 void
-s_vResetCounter (
-    PKnownNodeDB psNodeDBTable
-    )
+s_vResetCounter(
+	PKnownNodeDB psNodeDBTable
+)
 {
-    unsigned char ii;
+	unsigned char ii;
 
-    // clear statistic counter for auto_rate
-    for(ii=0;ii<=MAX_RATE;ii++) {
-        psNodeDBTable->uTxOk[ii] = 0;
-        psNodeDBTable->uTxFail[ii] = 0;
-    }
+	// clear statistic counter for auto_rate
+	for (ii = 0; ii <= MAX_RATE; ii++) {
+		psNodeDBTable->uTxOk[ii] = 0;
+		psNodeDBTable->uTxFail[ii] = 0;
+	}
 }
 
 /*---------------------  Export Variables  --------------------------*/
 
-
 /*---------------------  Export Functions  --------------------------*/
 
-
 /*+
  *
  * Description:
@@ -103,26 +95,24 @@ s_vResetCounter (
  *
  * Return Value: RateIdx
  *
--*/
+ -*/
 unsigned char
-DATARATEbyGetRateIdx (
-    unsigned char byRate
-    )
+DATARATEbyGetRateIdx(
+	unsigned char byRate
+)
 {
-    unsigned char ii;
+	unsigned char ii;
 
-    //Erase basicRate flag.
-    byRate = byRate & 0x7F;//0111 1111
+	//Erase basicRate flag.
+	byRate = byRate & 0x7F;//0111 1111
 
-    for (ii = 0; ii < MAX_RATE; ii ++) {
-        if (acbyIERate[ii] == byRate)
-            return ii;
-    }
-    return 0;
+	for (ii = 0; ii < MAX_RATE; ii++) {
+		if (acbyIERate[ii] == byRate)
+			return ii;
+	}
+	return 0;
 }
 
-
-
 /*+
  *
  * Routine Description:
@@ -137,13 +127,10 @@ DATARATEbyGetRateIdx (
  *
  * Return Value: none
  *
--*/
+ -*/
 #define AUTORATE_TXCNT_THRESHOLD        20
 #define AUTORATE_INC_THRESHOLD          30
 
-
-
-
 /*+
  *
  * Description:
@@ -157,22 +144,22 @@ DATARATEbyGetRateIdx (
  *
  * Return Value: RateIdx
  *
--*/
+ -*/
 unsigned short
 wGetRateIdx(
-    unsigned char byRate
-    )
+	unsigned char byRate
+)
 {
-    unsigned short ii;
+	unsigned short ii;
 
-    //Erase basicRate flag.
-    byRate = byRate & 0x7F;//0111 1111
+	//Erase basicRate flag.
+	byRate = byRate & 0x7F;//0111 1111
 
-    for (ii = 0; ii < MAX_RATE; ii ++) {
-        if (acbyIERate[ii] == byRate)
-            return ii;
-    }
-    return 0;
+	for (ii = 0; ii < MAX_RATE; ii++) {
+		if (acbyIERate[ii] == byRate)
+			return ii;
+	}
+	return 0;
 }
 
 /*+
@@ -193,102 +180,99 @@ wGetRateIdx(
  *
  * Return Value: none
  *
--*/
+ -*/
 void
-RATEvParseMaxRate (
-    void *pDeviceHandler,
-    PWLAN_IE_SUPP_RATES pItemRates,
-    PWLAN_IE_SUPP_RATES pItemExtRates,
-    bool bUpdateBasicRate,
-    unsigned short *pwMaxBasicRate,
-    unsigned short *pwMaxSuppRate,
-    unsigned short *pwSuppRate,
-    unsigned char *pbyTopCCKRate,
-    unsigned char *pbyTopOFDMRate
-    )
+RATEvParseMaxRate(
+	void *pDeviceHandler,
+	PWLAN_IE_SUPP_RATES pItemRates,
+	PWLAN_IE_SUPP_RATES pItemExtRates,
+	bool bUpdateBasicRate,
+	unsigned short *pwMaxBasicRate,
+	unsigned short *pwMaxSuppRate,
+	unsigned short *pwSuppRate,
+	unsigned char *pbyTopCCKRate,
+	unsigned char *pbyTopOFDMRate
+)
 {
-PSDevice  pDevice = (PSDevice) pDeviceHandler;
-unsigned int ii;
-unsigned char byHighSuppRate = 0;
-unsigned char byRate = 0;
-unsigned short wOldBasicRate = pDevice->wBasicRate;
-unsigned int uRateLen;
-
-
-    if (pItemRates == NULL)
-        return;
-
-    *pwSuppRate = 0;
-    uRateLen = pItemRates->len;
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ParseMaxRate Len: %d\n", uRateLen);
-    if (pDevice->eCurrentPHYType != PHY_TYPE_11B) {
-        if (uRateLen > WLAN_RATES_MAXLEN)
-            uRateLen = WLAN_RATES_MAXLEN;
-    } else {
-        if (uRateLen > WLAN_RATES_MAXLEN_11B)
-            uRateLen = WLAN_RATES_MAXLEN_11B;
-    }
-
-    for (ii = 0; ii < uRateLen; ii++) {
-    	byRate = (unsigned char)(pItemRates->abyRates[ii]);
-        if (WLAN_MGMT_IS_BASICRATE(byRate) &&
-            (bUpdateBasicRate == true))  {
-            // Add to basic rate set, update pDevice->byTopCCKBasicRate and pDevice->byTopOFDMBasicRate
-            CARDbAddBasicRate((void *)pDevice, wGetRateIdx(byRate));
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ParseMaxRate AddBasicRate: %d\n", wGetRateIdx(byRate));
-        }
-        byRate = (unsigned char)(pItemRates->abyRates[ii]&0x7F);
-        if (byHighSuppRate == 0)
-            byHighSuppRate = byRate;
-        if (byRate > byHighSuppRate)
-            byHighSuppRate = byRate;
-        *pwSuppRate |= (1<<wGetRateIdx(byRate));
-    }
-    if ((pItemExtRates != NULL) && (pItemExtRates->byElementID == WLAN_EID_EXTSUPP_RATES) &&
-        (pDevice->eCurrentPHYType != PHY_TYPE_11B)) {
-
-        unsigned int uExtRateLen = pItemExtRates->len;
-
-        if (uExtRateLen > WLAN_RATES_MAXLEN)
-            uExtRateLen = WLAN_RATES_MAXLEN;
-
-        for (ii = 0; ii < uExtRateLen ; ii++) {
-            byRate = (unsigned char)(pItemExtRates->abyRates[ii]);
-            // select highest basic rate
-            if (WLAN_MGMT_IS_BASICRATE(pItemExtRates->abyRates[ii])) {
-            	// Add to basic rate set, update pDevice->byTopCCKBasicRate and pDevice->byTopOFDMBasicRate
-                CARDbAddBasicRate((void *)pDevice, wGetRateIdx(byRate));
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ParseMaxRate AddBasicRate: %d\n", wGetRateIdx(byRate));
-            }
-            byRate = (unsigned char)(pItemExtRates->abyRates[ii]&0x7F);
-            if (byHighSuppRate == 0)
-                byHighSuppRate = byRate;
-            if (byRate > byHighSuppRate)
-                byHighSuppRate = byRate;
-            *pwSuppRate |= (1<<wGetRateIdx(byRate));
-            //DBG_PRN_GRP09(("ParseMaxRate : HighSuppRate: %d, %X\n", wGetRateIdx(byRate), byRate));
-        }
-    } //if(pItemExtRates != NULL)
-
-    if ((pDevice->byPacketType == PK_TYPE_11GB) && CARDbIsOFDMinBasicRate((void *)pDevice)) {
-        pDevice->byPacketType = PK_TYPE_11GA;
-    }
-
-    *pbyTopCCKRate = pDevice->byTopCCKBasicRate;
-    *pbyTopOFDMRate = pDevice->byTopOFDMBasicRate;
-    *pwMaxSuppRate = wGetRateIdx(byHighSuppRate);
-    if ((pDevice->byPacketType==PK_TYPE_11B) || (pDevice->byPacketType==PK_TYPE_11GB))
-       *pwMaxBasicRate = pDevice->byTopCCKBasicRate;
-    else
-       *pwMaxBasicRate = pDevice->byTopOFDMBasicRate;
-    if (wOldBasicRate != pDevice->wBasicRate)
-        CARDvSetRSPINF((void *)pDevice, pDevice->eCurrentPHYType);
-
-     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Exit ParseMaxRate\n");
+	PSDevice  pDevice = (PSDevice) pDeviceHandler;
+	unsigned int ii;
+	unsigned char byHighSuppRate = 0;
+	unsigned char byRate = 0;
+	unsigned short wOldBasicRate = pDevice->wBasicRate;
+	unsigned int uRateLen;
+
+	if (pItemRates == NULL)
+		return;
+
+	*pwSuppRate = 0;
+	uRateLen = pItemRates->len;
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ParseMaxRate Len: %d\n", uRateLen);
+	if (pDevice->eCurrentPHYType != PHY_TYPE_11B) {
+		if (uRateLen > WLAN_RATES_MAXLEN)
+			uRateLen = WLAN_RATES_MAXLEN;
+	} else {
+		if (uRateLen > WLAN_RATES_MAXLEN_11B)
+			uRateLen = WLAN_RATES_MAXLEN_11B;
+	}
+
+	for (ii = 0; ii < uRateLen; ii++) {
+		byRate = (unsigned char)(pItemRates->abyRates[ii]);
+		if (WLAN_MGMT_IS_BASICRATE(byRate) &&
+		    (bUpdateBasicRate == true))  {
+			// Add to basic rate set, update pDevice->byTopCCKBasicRate and pDevice->byTopOFDMBasicRate
+			CARDbAddBasicRate((void *)pDevice, wGetRateIdx(byRate));
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ParseMaxRate AddBasicRate: %d\n", wGetRateIdx(byRate));
+		}
+		byRate = (unsigned char)(pItemRates->abyRates[ii]&0x7F);
+		if (byHighSuppRate == 0)
+			byHighSuppRate = byRate;
+		if (byRate > byHighSuppRate)
+			byHighSuppRate = byRate;
+		*pwSuppRate |= (1<<wGetRateIdx(byRate));
+	}
+	if ((pItemExtRates != NULL) && (pItemExtRates->byElementID == WLAN_EID_EXTSUPP_RATES) &&
+	    (pDevice->eCurrentPHYType != PHY_TYPE_11B)) {
+		unsigned int uExtRateLen = pItemExtRates->len;
+
+		if (uExtRateLen > WLAN_RATES_MAXLEN)
+			uExtRateLen = WLAN_RATES_MAXLEN;
+
+		for (ii = 0; ii < uExtRateLen; ii++) {
+			byRate = (unsigned char)(pItemExtRates->abyRates[ii]);
+			// select highest basic rate
+			if (WLAN_MGMT_IS_BASICRATE(pItemExtRates->abyRates[ii])) {
+				// Add to basic rate set, update pDevice->byTopCCKBasicRate and pDevice->byTopOFDMBasicRate
+				CARDbAddBasicRate((void *)pDevice, wGetRateIdx(byRate));
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ParseMaxRate AddBasicRate: %d\n", wGetRateIdx(byRate));
+			}
+			byRate = (unsigned char)(pItemExtRates->abyRates[ii]&0x7F);
+			if (byHighSuppRate == 0)
+				byHighSuppRate = byRate;
+			if (byRate > byHighSuppRate)
+				byHighSuppRate = byRate;
+			*pwSuppRate |= (1<<wGetRateIdx(byRate));
+			//DBG_PRN_GRP09(("ParseMaxRate : HighSuppRate: %d, %X\n", wGetRateIdx(byRate), byRate));
+		}
+	} //if (pItemExtRates != NULL)
+
+	if ((pDevice->byPacketType == PK_TYPE_11GB) && CARDbIsOFDMinBasicRate((void *)pDevice)) {
+		pDevice->byPacketType = PK_TYPE_11GA;
+	}
+
+	*pbyTopCCKRate = pDevice->byTopCCKBasicRate;
+	*pbyTopOFDMRate = pDevice->byTopOFDMBasicRate;
+	*pwMaxSuppRate = wGetRateIdx(byHighSuppRate);
+	if ((pDevice->byPacketType == PK_TYPE_11B) || (pDevice->byPacketType == PK_TYPE_11GB))
+		*pwMaxBasicRate = pDevice->byTopCCKBasicRate;
+	else
+		*pwMaxBasicRate = pDevice->byTopOFDMBasicRate;
+	if (wOldBasicRate != pDevice->wBasicRate)
+		CARDvSetRSPINF((void *)pDevice, pDevice->eCurrentPHYType);
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Exit ParseMaxRate\n");
 }
 
-
 /*+
  *
  * Routine Description:
@@ -303,97 +287,95 @@ unsigned int uRateLen;
  *
  * Return Value: none
  *
--*/
+ -*/
 #define AUTORATE_TXCNT_THRESHOLD        20
 #define AUTORATE_INC_THRESHOLD          30
 
 void
-RATEvTxRateFallBack (
-    void *pDeviceHandler,
-    PKnownNodeDB psNodeDBTable
-    )
+RATEvTxRateFallBack(
+	void *pDeviceHandler,
+	PKnownNodeDB psNodeDBTable
+)
 {
-PSDevice        pDevice = (PSDevice) pDeviceHandler;
-unsigned short wIdxDownRate = 0;
-unsigned int ii;
+	PSDevice        pDevice = (PSDevice) pDeviceHandler;
+	unsigned short wIdxDownRate = 0;
+	unsigned int ii;
 //unsigned long dwRateTable[MAX_RATE]  = {1,   2,   5,   11,  6,    9,    12,   18,  24,  36,  48,  54};
-bool bAutoRate[MAX_RATE]    = {true,true,true,true,false,false,true,true,true,true,true,true};
+	bool bAutoRate[MAX_RATE]    = {true, true, true, true, false, false, true, true, true, true, true, true};
 	unsigned long dwThroughputTbl[MAX_RATE] = {10, 20, 55, 110, 60, 90, 120, 180, 240, 360, 480, 540};
 	unsigned long dwThroughput = 0;
 	unsigned short wIdxUpRate = 0;
 	unsigned long dwTxDiff = 0;
 
-    if (pDevice->pMgmt->eScanState != WMAC_NO_SCANNING) {
-        // Don't do Fallback when scanning Channel
-        return;
-    }
-
-    psNodeDBTable->uTimeCount ++;
-
-    if (psNodeDBTable->uTxFail[MAX_RATE] > psNodeDBTable->uTxOk[MAX_RATE])
-        dwTxDiff = psNodeDBTable->uTxFail[MAX_RATE] - psNodeDBTable->uTxOk[MAX_RATE];
-
-    if ((psNodeDBTable->uTxOk[MAX_RATE] < AUTORATE_TXOK_CNT) &&
-        (dwTxDiff < AUTORATE_TXFAIL_CNT) &&
-        (psNodeDBTable->uTimeCount < AUTORATE_TIMEOUT)) {
-        return;
-    }
-
-    if (psNodeDBTable->uTimeCount >= AUTORATE_TIMEOUT) {
-        psNodeDBTable->uTimeCount = 0;
-    }
-
-
-    for(ii=0;ii<MAX_RATE;ii++) {
-        if (psNodeDBTable->wSuppRate & (0x0001<<ii)) {
-            if (bAutoRate[ii] == true) {
-                wIdxUpRate = (unsigned short) ii;
-            }
-        } else {
-            bAutoRate[ii] = false;
-        }
-    }
-
-    for(ii=0;ii<=psNodeDBTable->wTxDataRate;ii++) {
-        if ( (psNodeDBTable->uTxOk[ii] != 0) ||
-             (psNodeDBTable->uTxFail[ii] != 0) ) {
-            dwThroughputTbl[ii] *= psNodeDBTable->uTxOk[ii];
-            if (ii < RATE_11M) {
-                psNodeDBTable->uTxFail[ii] *= 4;
-            }
-            dwThroughputTbl[ii] /= (psNodeDBTable->uTxOk[ii] + psNodeDBTable->uTxFail[ii]);
-        }
-//        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Rate %d,Ok: %d, Fail:%d, Throughput:%d\n",
+	if (pDevice->pMgmt->eScanState != WMAC_NO_SCANNING) {
+		// Don't do Fallback when scanning Channel
+		return;
+	}
+
+	psNodeDBTable->uTimeCount++;
+
+	if (psNodeDBTable->uTxFail[MAX_RATE] > psNodeDBTable->uTxOk[MAX_RATE])
+		dwTxDiff = psNodeDBTable->uTxFail[MAX_RATE] - psNodeDBTable->uTxOk[MAX_RATE];
+
+	if ((psNodeDBTable->uTxOk[MAX_RATE] < AUTORATE_TXOK_CNT) &&
+	    (dwTxDiff < AUTORATE_TXFAIL_CNT) &&
+	    (psNodeDBTable->uTimeCount < AUTORATE_TIMEOUT)) {
+		return;
+	}
+
+	if (psNodeDBTable->uTimeCount >= AUTORATE_TIMEOUT) {
+		psNodeDBTable->uTimeCount = 0;
+	}
+
+	for (ii = 0; ii < MAX_RATE; ii++) {
+		if (psNodeDBTable->wSuppRate & (0x0001<<ii)) {
+			if (bAutoRate[ii] == true) {
+				wIdxUpRate = (unsigned short) ii;
+			}
+		} else {
+			bAutoRate[ii] = false;
+		}
+	}
+
+	for (ii = 0; ii <= psNodeDBTable->wTxDataRate; ii++) {
+		if ((psNodeDBTable->uTxOk[ii] != 0) ||
+		    (psNodeDBTable->uTxFail[ii] != 0)) {
+			dwThroughputTbl[ii] *= psNodeDBTable->uTxOk[ii];
+			if (ii < RATE_11M) {
+				psNodeDBTable->uTxFail[ii] *= 4;
+			}
+			dwThroughputTbl[ii] /= (psNodeDBTable->uTxOk[ii] + psNodeDBTable->uTxFail[ii]);
+		}
+//        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Rate %d,Ok: %d, Fail:%d, Throughput:%d\n",
 //                       ii, psNodeDBTable->uTxOk[ii], psNodeDBTable->uTxFail[ii], dwThroughputTbl[ii]);
-    }
-    dwThroughput = dwThroughputTbl[psNodeDBTable->wTxDataRate];
-
-    wIdxDownRate = psNodeDBTable->wTxDataRate;
-    for(ii = psNodeDBTable->wTxDataRate; ii > 0;) {
-        ii--;
-        if ( (dwThroughputTbl[ii] > dwThroughput) &&
-             (bAutoRate[ii]==true) ) {
-            dwThroughput = dwThroughputTbl[ii];
-            wIdxDownRate = (unsigned short) ii;
-        }
-    }
-    psNodeDBTable->wTxDataRate = wIdxDownRate;
-    if (psNodeDBTable->uTxOk[MAX_RATE]) {
-        if (psNodeDBTable->uTxOk[MAX_RATE] >
-           (psNodeDBTable->uTxFail[MAX_RATE] * 4) ) {
-            psNodeDBTable->wTxDataRate = wIdxUpRate;
-        }
-    }else { // adhoc, if uTxOk =0 & uTxFail = 0
-        if (psNodeDBTable->uTxFail[MAX_RATE] == 0)
-            psNodeDBTable->wTxDataRate = wIdxUpRate;
-    }
+	}
+	dwThroughput = dwThroughputTbl[psNodeDBTable->wTxDataRate];
+
+	wIdxDownRate = psNodeDBTable->wTxDataRate;
+	for (ii = psNodeDBTable->wTxDataRate; ii > 0;) {
+		ii--;
+		if ((dwThroughputTbl[ii] > dwThroughput) &&
+		    (bAutoRate[ii] == true)) {
+			dwThroughput = dwThroughputTbl[ii];
+			wIdxDownRate = (unsigned short) ii;
+		}
+	}
+	psNodeDBTable->wTxDataRate = wIdxDownRate;
+	if (psNodeDBTable->uTxOk[MAX_RATE]) {
+		if (psNodeDBTable->uTxOk[MAX_RATE] >
+		    (psNodeDBTable->uTxFail[MAX_RATE] * 4)) {
+			psNodeDBTable->wTxDataRate = wIdxUpRate;
+		}
+	} else { // adhoc, if uTxOk =0 & uTxFail = 0
+		if (psNodeDBTable->uTxFail[MAX_RATE] == 0)
+			psNodeDBTable->wTxDataRate = wIdxUpRate;
+	}
 //2008-5-8 <add> by chester
-TxRate_iwconfig=psNodeDBTable->wTxDataRate;
-    s_vResetCounter(psNodeDBTable);
-//    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Rate: %d, U:%d, D:%d\n", psNodeDBTable->wTxDataRate, wIdxUpRate, wIdxDownRate);
-
-    return;
+	TxRate_iwconfig = psNodeDBTable->wTxDataRate;
+	s_vResetCounter(psNodeDBTable);
+//    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Rate: %d, U:%d, D:%d\n", psNodeDBTable->wTxDataRate, wIdxUpRate, wIdxDownRate);
 
+	return;
 }
 
 /*+
@@ -408,30 +390,29 @@ TxRate_iwconfig=psNodeDBTable->wTxDataRate;
  *
  * Return Value: None
  *
--*/
+ -*/
 unsigned char
-RATEuSetIE (
-    PWLAN_IE_SUPP_RATES pSrcRates,
-    PWLAN_IE_SUPP_RATES pDstRates,
-    unsigned int uRateLen
-    )
+RATEuSetIE(
+	PWLAN_IE_SUPP_RATES pSrcRates,
+	PWLAN_IE_SUPP_RATES pDstRates,
+	unsigned int uRateLen
+)
 {
-    unsigned int ii, uu, uRateCnt = 0;
-
-    if ((pSrcRates == NULL) || (pDstRates == NULL))
-        return 0;
-
-    if (pSrcRates->len == 0)
-        return 0;
-
-    for (ii = 0; ii < uRateLen; ii++) {
-        for (uu = 0; uu < pSrcRates->len; uu++) {
-            if ((pSrcRates->abyRates[uu] & 0x7F) == acbyIERate[ii]) {
-                pDstRates->abyRates[uRateCnt ++] = pSrcRates->abyRates[uu];
-                break;
-            }
-        }
-    }
-    return (unsigned char)uRateCnt;
+	unsigned int ii, uu, uRateCnt = 0;
+
+	if ((pSrcRates == NULL) || (pDstRates == NULL))
+		return 0;
+
+	if (pSrcRates->len == 0)
+		return 0;
+
+	for (ii = 0; ii < uRateLen; ii++) {
+		for (uu = 0; uu < pSrcRates->len; uu++) {
+			if ((pSrcRates->abyRates[uu] & 0x7F) == acbyIERate[ii]) {
+				pDstRates->abyRates[uRateCnt++] = pSrcRates->abyRates[uu];
+				break;
+			}
+		}
+	}
+	return (unsigned char)uRateCnt;
 }
-
diff --git a/drivers/staging/vt6655/datarate.h b/drivers/staging/vt6655/datarate.h
index 4f8ea0b0532..e4fad05ad85 100644
--- a/drivers/staging/vt6655/datarate.h
+++ b/drivers/staging/vt6655/datarate.h
@@ -41,55 +41,48 @@
 #define RETRY_TIMES_THRD_H         2    // times
 #define RETRY_TIMES_THRD_L         1    // times
 
-
 /*---------------------  Export Classes  ----------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
 
-
 /*---------------------  Export Types  ------------------------------*/
 
-
 /*---------------------  Export Functions  --------------------------*/
 
-
-
 void
 RATEvParseMaxRate(
-    void *pDeviceHandler,
-    PWLAN_IE_SUPP_RATES pItemRates,
-    PWLAN_IE_SUPP_RATES pItemExtRates,
-    bool bUpdateBasicRate,
-    unsigned short *pwMaxBasicRate,
-    unsigned short *pwMaxSuppRate,
-    unsigned short *pwSuppRate,
-    unsigned char *pbyTopCCKRate,
-    unsigned char *pbyTopOFDMRate
-    );
+	void *pDeviceHandler,
+	PWLAN_IE_SUPP_RATES pItemRates,
+	PWLAN_IE_SUPP_RATES pItemExtRates,
+	bool bUpdateBasicRate,
+	unsigned short *pwMaxBasicRate,
+	unsigned short *pwMaxSuppRate,
+	unsigned short *pwSuppRate,
+	unsigned char *pbyTopCCKRate,
+	unsigned char *pbyTopOFDMRate
+);
 
 void
 RATEvTxRateFallBack(
-    void *pDeviceHandler,
-    PKnownNodeDB psNodeDBTable
-    );
+	void *pDeviceHandler,
+	PKnownNodeDB psNodeDBTable
+);
 
 unsigned char
 RATEuSetIE(
-    PWLAN_IE_SUPP_RATES pSrcRates,
-    PWLAN_IE_SUPP_RATES pDstRates,
-    unsigned int uRateLen
-    );
+	PWLAN_IE_SUPP_RATES pSrcRates,
+	PWLAN_IE_SUPP_RATES pDstRates,
+	unsigned int uRateLen
+);
 
 unsigned short
 wGetRateIdx(
-    unsigned char byRate
-    );
-
+	unsigned char byRate
+);
 
 unsigned char
 DATARATEbyGetRateIdx(
-    unsigned char byRate
-    );
-
+	unsigned char byRate
+);
 
 #endif //__DATARATE_H__
diff --git a/drivers/staging/vt6655/desc.h b/drivers/staging/vt6655/desc.h
index 084a1a5566a..32d808ec502 100644
--- a/drivers/staging/vt6655/desc.h
+++ b/drivers/staging/vt6655/desc.h
@@ -89,7 +89,7 @@
 
 // max transmit or receive buffer size
 #define CB_MAX_BUF_SIZE     2900U       // max buffer size
-                                        // NOTE: must be multiple of 4
+					// NOTE: must be multiple of 4
 #define CB_MAX_TX_BUF_SIZE          CB_MAX_BUF_SIZE // max Tx buffer size
 #define CB_MAX_RX_BUF_SIZE_NORMAL   CB_MAX_BUF_SIZE // max Rx buffer size when not use Multi-RD
 
@@ -101,16 +101,15 @@
 #define CB_MIN_TX_DESC      16          // min # of tx descriptor
 
 #define CB_MAX_RECEIVED_PACKETS     16  // max # of received packets at one time
-                                        // limit our receive routine to indicating
-                                        // this many at a time for 2 reasons:
-                                        // 1. driver flow control to protocol layer
-                                        // 2. limit the time used in ISR routine
+					// limit our receive routine to indicating
+					// this many at a time for 2 reasons:
+					// 1. driver flow control to protocol layer
+					// 2. limit the time used in ISR routine
 
 #define CB_EXTRA_RD_NUM     32          // default # of Extra RD
 #define CB_RD_NUM           32          // default # of RD
 #define CB_TD_NUM           32          // default # of TD
 
-
 // max number of physical segments
 // in a single NDIS packet. Above this threshold, the packet
 // is copied into a single physically contiguous buffer
@@ -121,8 +120,6 @@
 
 #define CB_PROTOCOL_RESERVED_SECTION    16
 
-
-
 // if retrys excess 15 times , tx will abort, and
 // if tx fifo underflow, tx will fail
 // we should try to resend it
@@ -199,8 +196,6 @@
 #define TYPE_RXDMA1     1
 #define TYPE_MAXRD      2
 
-
-
 // TD_INFO flags control bit
 #define TD_FLAGS_NETIF_SKB               0x01       // check if need release skb
 #define TD_FLAGS_PRIV_SKB                0x02       // check if called from private skb(hostap)
@@ -213,28 +208,28 @@
 // may link to older skb that leads error.
 
 typedef struct tagDEVICE_RD_INFO {
-    struct sk_buff* skb;
-    dma_addr_t  skb_dma;
-    dma_addr_t  curr_desc;
+	struct sk_buff *skb;
+	dma_addr_t  skb_dma;
+	dma_addr_t  curr_desc;
 } DEVICE_RD_INFO,   *PDEVICE_RD_INFO;
 
 /*
-static inline PDEVICE_RD_INFO alloc_rd_info(void) {
-    PDEVICE_RD_INFO  ptr;
-    ptr = kmalloc(sizeof(DEVICE_RD_INFO), GFP_ATOMIC);
-    if (ptr == NULL)
-        return NULL;
-    else {
-        memset(ptr,0,sizeof(DEVICE_RD_INFO));
-        return ptr;
-    }
-}
+  static inline PDEVICE_RD_INFO alloc_rd_info(void) {
+  PDEVICE_RD_INFO  ptr;
+  ptr = kmalloc(sizeof(DEVICE_RD_INFO), GFP_ATOMIC);
+  if (ptr == NULL)
+  return NULL;
+  else {
+  memset(ptr,0,sizeof(DEVICE_RD_INFO));
+  return ptr;
+  }
+  }
 */
 
 /*
-typedef struct tagRDES0 {
-    unsigned short wResCount;
-    unsigned short wf1Owner ;
+  typedef struct tagRDES0 {
+  unsigned short wResCount;
+  unsigned short wf1Owner;
 //    unsigned short f15Reserved : 15;
 //    unsigned short f1Owner : 1;
 } __attribute__ ((__packed__))
@@ -244,11 +239,11 @@ SRDES0;
 #ifdef __BIG_ENDIAN
 
 typedef struct tagRDES0 {
-   volatile unsigned short wResCount;
+	volatile unsigned short wResCount;
 	union {
 		volatile u16    f15Reserved;
 		struct {
-            volatile u8 f8Reserved1;
+			volatile u8 f8Reserved1;
 			volatile u8 f1Owner:1;
 			volatile u8 f7Reserved:7;
 		} __attribute__ ((__packed__));
@@ -259,18 +254,17 @@ SRDES0, *PSRDES0;
 #else
 
 typedef struct tagRDES0 {
-    unsigned short wResCount;
-    unsigned short f15Reserved : 15;
-    unsigned short f1Owner : 1;
+	unsigned short wResCount;
+	unsigned short f15Reserved:15;
+	unsigned short f1Owner:1;
 } __attribute__ ((__packed__))
 SRDES0;
 
-
 #endif
 
 typedef struct tagRDES1 {
-    unsigned short wReqCount;
-    unsigned short wReserved;
+	unsigned short wReqCount;
+	unsigned short wReserved;
 } __attribute__ ((__packed__))
 SRDES1;
 
@@ -278,13 +272,13 @@ SRDES1;
 // Rx descriptor
 //
 typedef struct tagSRxDesc {
-    volatile SRDES0 m_rd0RD0;
-    volatile SRDES1 m_rd1RD1;
-    volatile u32    buff_addr;
-    volatile u32    next_desc;
-    struct tagSRxDesc   *next;//4 bytes
-    volatile PDEVICE_RD_INFO    pRDInfo;//4 bytes
-    volatile u32    Reserved[2];//8 bytes
+	volatile SRDES0 m_rd0RD0;
+	volatile SRDES1 m_rd1RD1;
+	volatile u32    buff_addr;
+	volatile u32    next_desc;
+	struct tagSRxDesc   *next;//4 bytes
+	volatile PDEVICE_RD_INFO    pRDInfo;//4 bytes
+	volatile u32    Reserved[2];//8 bytes
 } __attribute__ ((__packed__))
 SRxDesc, *PSRxDesc;
 typedef const SRxDesc *PCSRxDesc;
@@ -292,10 +286,10 @@ typedef const SRxDesc *PCSRxDesc;
 #ifdef __BIG_ENDIAN
 
 /*
-typedef struct tagTDES0 {
-    volatile    unsigned char byTSR0;
-    volatile    unsigned char byTSR1;
-    volatile    unsigned short wOwner_Txtime;
+  typedef struct tagTDES0 {
+  volatile    unsigned char byTSR0;
+  volatile    unsigned char byTSR1;
+  volatile    unsigned short wOwner_Txtime;
 //    volatile    unsigned short f15Txtime : 15;
 //    volatile    unsigned short f1Owner:1;
 } __attribute__ ((__packed__))
@@ -303,12 +297,12 @@ STDES0;
 */
 
 typedef struct tagTDES0 {
-    volatile    unsigned char byTSR0;
-    volatile    unsigned char byTSR1;
+	volatile    unsigned char byTSR0;
+	volatile    unsigned char byTSR1;
 	union {
 		volatile u16    f15Txtime;
 		struct {
-            volatile u8 f8Reserved1;
+			volatile u8 f8Reserved1;
 			volatile u8 f1Owner:1;
 			volatile u8 f7Reserved:7;
 		} __attribute__ ((__packed__));
@@ -319,113 +313,109 @@ STDES0, PSTDES0;
 #else
 
 typedef struct tagTDES0 {
-    volatile    unsigned char byTSR0;
-    volatile    unsigned char byTSR1;
-    volatile    unsigned short f15Txtime : 15;
-    volatile    unsigned short f1Owner:1;
+	volatile    unsigned char byTSR0;
+	volatile    unsigned char byTSR1;
+	volatile    unsigned short f15Txtime:15;
+	volatile    unsigned short f1Owner:1;
 } __attribute__ ((__packed__))
 STDES0;
 
 #endif
 
-
 typedef struct tagTDES1 {
-    volatile    unsigned short wReqCount;
-    volatile    unsigned char byTCR;
-    volatile    unsigned char byReserved;
+	volatile    unsigned short wReqCount;
+	volatile    unsigned char byTCR;
+	volatile    unsigned char byReserved;
 } __attribute__ ((__packed__))
 STDES1;
 
-
-typedef struct tagDEVICE_TD_INFO{
-    struct sk_buff*     skb;
-    unsigned char *buf;
-    dma_addr_t          skb_dma;
-    dma_addr_t          buf_dma;
-    dma_addr_t          curr_desc;
-    unsigned long dwReqCount;
-    unsigned long dwHeaderLength;
-    unsigned char byFlags;
+typedef struct tagDEVICE_TD_INFO {
+	struct sk_buff *skb;
+	unsigned char *buf;
+	dma_addr_t          skb_dma;
+	dma_addr_t          buf_dma;
+	dma_addr_t          curr_desc;
+	unsigned long dwReqCount;
+	unsigned long dwHeaderLength;
+	unsigned char byFlags;
 } DEVICE_TD_INFO,    *PDEVICE_TD_INFO;
 
 /*
-static inline PDEVICE_TD_INFO alloc_td_info(void) {
-    PDEVICE_TD_INFO  ptr;
-    ptr = kmalloc(sizeof(DEVICE_TD_INFO),GFP_ATOMIC);
-    if (ptr == NULL)
-        return NULL;
-    else {
-        memset(ptr,0,sizeof(DEVICE_TD_INFO));
-        return ptr;
-    }
-}
+  static inline PDEVICE_TD_INFO alloc_td_info(void) {
+  PDEVICE_TD_INFO  ptr;
+  ptr = kmalloc(sizeof(DEVICE_TD_INFO),GFP_ATOMIC);
+  if (ptr == NULL)
+  return NULL;
+  else {
+  memset(ptr,0,sizeof(DEVICE_TD_INFO));
+  return ptr;
+  }
+  }
 */
 
 //
 // transmit descriptor
 //
 typedef struct tagSTxDesc {
-    volatile    STDES0  m_td0TD0;
-    volatile    STDES1  m_td1TD1;
-    volatile    u32    buff_addr;
-    volatile    u32    next_desc;
-    struct tagSTxDesc*  next; //4 bytes
-    volatile    PDEVICE_TD_INFO pTDInfo;//4 bytes
-    volatile    u32    Reserved[2];//8 bytes
+	volatile    STDES0  m_td0TD0;
+	volatile    STDES1  m_td1TD1;
+	volatile    u32    buff_addr;
+	volatile    u32    next_desc;
+	struct tagSTxDesc *next; //4 bytes
+	volatile    PDEVICE_TD_INFO pTDInfo;//4 bytes
+	volatile    u32    Reserved[2];//8 bytes
 } __attribute__ ((__packed__))
 STxDesc, *PSTxDesc;
 typedef const STxDesc *PCSTxDesc;
 
-
 typedef struct tagSTxSyncDesc {
-    volatile    STDES0  m_td0TD0;
-    volatile    STDES1  m_td1TD1;
-    volatile    u32 buff_addr; // pointer to logical buffer
-    volatile    u32 next_desc; // pointer to next logical descriptor
-    volatile    unsigned short m_wFIFOCtl;
-    volatile    unsigned short m_wTimeStamp;
-    struct tagSTxSyncDesc*  next; //4 bytes
-    volatile    PDEVICE_TD_INFO pTDInfo;//4 bytes
-    volatile    u32 m_dwReserved2;
+	volatile    STDES0  m_td0TD0;
+	volatile    STDES1  m_td1TD1;
+	volatile    u32 buff_addr; // pointer to logical buffer
+	volatile    u32 next_desc; // pointer to next logical descriptor
+	volatile    unsigned short m_wFIFOCtl;
+	volatile    unsigned short m_wTimeStamp;
+	struct tagSTxSyncDesc *next; //4 bytes
+	volatile    PDEVICE_TD_INFO pTDInfo;//4 bytes
+	volatile    u32 m_dwReserved2;
 } __attribute__ ((__packed__))
 STxSyncDesc, *PSTxSyncDesc;
 typedef const STxSyncDesc *PCSTxSyncDesc;
 
-
 //
 // RsvTime buffer header
 //
 typedef struct tagSRrvTime_gRTS {
-    unsigned short wRTSTxRrvTime_ba;
-    unsigned short wRTSTxRrvTime_aa;
-    unsigned short wRTSTxRrvTime_bb;
-    unsigned short wReserved;
-    unsigned short wTxRrvTime_b;
-    unsigned short wTxRrvTime_a;
-}__attribute__ ((__packed__))
+	unsigned short wRTSTxRrvTime_ba;
+	unsigned short wRTSTxRrvTime_aa;
+	unsigned short wRTSTxRrvTime_bb;
+	unsigned short wReserved;
+	unsigned short wTxRrvTime_b;
+	unsigned short wTxRrvTime_a;
+} __attribute__ ((__packed__))
 SRrvTime_gRTS, *PSRrvTime_gRTS;
 typedef const SRrvTime_gRTS *PCSRrvTime_gRTS;
 
 typedef struct tagSRrvTime_gCTS {
-    unsigned short wCTSTxRrvTime_ba;
-    unsigned short wReserved;
-    unsigned short wTxRrvTime_b;
-    unsigned short wTxRrvTime_a;
-}__attribute__ ((__packed__))
+	unsigned short wCTSTxRrvTime_ba;
+	unsigned short wReserved;
+	unsigned short wTxRrvTime_b;
+	unsigned short wTxRrvTime_a;
+} __attribute__ ((__packed__))
 SRrvTime_gCTS, *PSRrvTime_gCTS;
 typedef const SRrvTime_gCTS *PCSRrvTime_gCTS;
 
 typedef struct tagSRrvTime_ab {
-    unsigned short wRTSTxRrvTime;
-    unsigned short wTxRrvTime;
-}__attribute__ ((__packed__))
+	unsigned short wRTSTxRrvTime;
+	unsigned short wTxRrvTime;
+} __attribute__ ((__packed__))
 SRrvTime_ab, *PSRrvTime_ab;
 typedef const SRrvTime_ab *PCSRrvTime_ab;
 
 typedef struct tagSRrvTime_atim {
-    unsigned short wCTSTxRrvTime_ba;
-    unsigned short wTxRrvTime_a;
-}__attribute__ ((__packed__))
+	unsigned short wCTSTxRrvTime_ba;
+	unsigned short wTxRrvTime_a;
+} __attribute__ ((__packed__))
 SRrvTime_atim, *PSRrvTime_atim;
 typedef const SRrvTime_atim *PCSRrvTime_atim;
 
@@ -433,132 +423,127 @@ typedef const SRrvTime_atim *PCSRrvTime_atim;
 // RTS buffer header
 //
 typedef struct tagSRTSData {
-    unsigned short wFrameControl;
-    unsigned short wDurationID;
-    unsigned char abyRA[ETH_ALEN];
-    unsigned char abyTA[ETH_ALEN];
-}__attribute__ ((__packed__))
+	unsigned short wFrameControl;
+	unsigned short wDurationID;
+	unsigned char abyRA[ETH_ALEN];
+	unsigned char abyTA[ETH_ALEN];
+} __attribute__ ((__packed__))
 SRTSData, *PSRTSData;
 typedef const SRTSData *PCSRTSData;
 
 typedef struct tagSRTS_g {
-    unsigned char bySignalField_b;
-    unsigned char byServiceField_b;
-    unsigned short wTransmitLength_b;
-    unsigned char bySignalField_a;
-    unsigned char byServiceField_a;
-    unsigned short wTransmitLength_a;
-    unsigned short wDuration_ba;
-    unsigned short wDuration_aa;
-    unsigned short wDuration_bb;
-    unsigned short wReserved;
-    SRTSData    Data;
-}__attribute__ ((__packed__))
+	unsigned char bySignalField_b;
+	unsigned char byServiceField_b;
+	unsigned short wTransmitLength_b;
+	unsigned char bySignalField_a;
+	unsigned char byServiceField_a;
+	unsigned short wTransmitLength_a;
+	unsigned short wDuration_ba;
+	unsigned short wDuration_aa;
+	unsigned short wDuration_bb;
+	unsigned short wReserved;
+	SRTSData    Data;
+} __attribute__ ((__packed__))
 SRTS_g, *PSRTS_g;
 typedef const SRTS_g *PCSRTS_g;
 
-
 typedef struct tagSRTS_g_FB {
-    unsigned char bySignalField_b;
-    unsigned char byServiceField_b;
-    unsigned short wTransmitLength_b;
-    unsigned char bySignalField_a;
-    unsigned char byServiceField_a;
-    unsigned short wTransmitLength_a;
-    unsigned short wDuration_ba;
-    unsigned short wDuration_aa;
-    unsigned short wDuration_bb;
-    unsigned short wReserved;
-    unsigned short wRTSDuration_ba_f0;
-    unsigned short wRTSDuration_aa_f0;
-    unsigned short wRTSDuration_ba_f1;
-    unsigned short wRTSDuration_aa_f1;
-    SRTSData    Data;
-}__attribute__ ((__packed__))
+	unsigned char bySignalField_b;
+	unsigned char byServiceField_b;
+	unsigned short wTransmitLength_b;
+	unsigned char bySignalField_a;
+	unsigned char byServiceField_a;
+	unsigned short wTransmitLength_a;
+	unsigned short wDuration_ba;
+	unsigned short wDuration_aa;
+	unsigned short wDuration_bb;
+	unsigned short wReserved;
+	unsigned short wRTSDuration_ba_f0;
+	unsigned short wRTSDuration_aa_f0;
+	unsigned short wRTSDuration_ba_f1;
+	unsigned short wRTSDuration_aa_f1;
+	SRTSData    Data;
+} __attribute__ ((__packed__))
 SRTS_g_FB, *PSRTS_g_FB;
 typedef const SRTS_g_FB *PCSRTS_g_FB;
 
-
 typedef struct tagSRTS_ab {
-    unsigned char bySignalField;
-    unsigned char byServiceField;
-    unsigned short wTransmitLength;
-    unsigned short wDuration;
-    unsigned short wReserved;
-    SRTSData    Data;
-}__attribute__ ((__packed__))
+	unsigned char bySignalField;
+	unsigned char byServiceField;
+	unsigned short wTransmitLength;
+	unsigned short wDuration;
+	unsigned short wReserved;
+	SRTSData    Data;
+} __attribute__ ((__packed__))
 SRTS_ab, *PSRTS_ab;
 typedef const SRTS_ab *PCSRTS_ab;
 
-
 typedef struct tagSRTS_a_FB {
-    unsigned char bySignalField;
-    unsigned char byServiceField;
-    unsigned short wTransmitLength;
-    unsigned short wDuration;
-    unsigned short wReserved;
-    unsigned short wRTSDuration_f0;
-    unsigned short wRTSDuration_f1;
-    SRTSData    Data;
-}__attribute__ ((__packed__))
+	unsigned char bySignalField;
+	unsigned char byServiceField;
+	unsigned short wTransmitLength;
+	unsigned short wDuration;
+	unsigned short wReserved;
+	unsigned short wRTSDuration_f0;
+	unsigned short wRTSDuration_f1;
+	SRTSData    Data;
+} __attribute__ ((__packed__))
 SRTS_a_FB, *PSRTS_a_FB;
 typedef const SRTS_a_FB *PCSRTS_a_FB;
 
-
 //
 // CTS buffer header
 //
 typedef struct tagSCTSData {
-    unsigned short wFrameControl;
-    unsigned short wDurationID;
-    unsigned char abyRA[ETH_ALEN];
-    unsigned short wReserved;
-}__attribute__ ((__packed__))
+	unsigned short wFrameControl;
+	unsigned short wDurationID;
+	unsigned char abyRA[ETH_ALEN];
+	unsigned short wReserved;
+} __attribute__ ((__packed__))
 SCTSData, *PSCTSData;
 
 typedef struct tagSCTS {
-    unsigned char bySignalField_b;
-    unsigned char byServiceField_b;
-    unsigned short wTransmitLength_b;
-    unsigned short wDuration_ba;
-    unsigned short wReserved;
-    SCTSData    Data;
-}__attribute__ ((__packed__))
+	unsigned char bySignalField_b;
+	unsigned char byServiceField_b;
+	unsigned short wTransmitLength_b;
+	unsigned short wDuration_ba;
+	unsigned short wReserved;
+	SCTSData    Data;
+} __attribute__ ((__packed__))
 SCTS, *PSCTS;
 typedef const SCTS *PCSCTS;
 
 typedef struct tagSCTS_FB {
-    unsigned char bySignalField_b;
-    unsigned char byServiceField_b;
-    unsigned short wTransmitLength_b;
-    unsigned short wDuration_ba;
-    unsigned short wReserved;
-    unsigned short wCTSDuration_ba_f0;
-    unsigned short wCTSDuration_ba_f1;
-    SCTSData    Data;
-}__attribute__ ((__packed__))
+	unsigned char bySignalField_b;
+	unsigned char byServiceField_b;
+	unsigned short wTransmitLength_b;
+	unsigned short wDuration_ba;
+	unsigned short wReserved;
+	unsigned short wCTSDuration_ba_f0;
+	unsigned short wCTSDuration_ba_f1;
+	SCTSData    Data;
+} __attribute__ ((__packed__))
 SCTS_FB, *PSCTS_FB;
 typedef const SCTS_FB *PCSCTS_FB;
 
-
 //
 // Tx FIFO header
 //
 typedef struct tagSTxBufHead {
-    u32 adwTxKey[4];
-    unsigned short wFIFOCtl;
-    unsigned short wTimeStamp;
-    unsigned short wFragCtl;
-    unsigned char byTxPower;
-    unsigned char wReserved;
-}__attribute__ ((__packed__))
+	u32 adwTxKey[4];
+	unsigned short wFIFOCtl;
+	unsigned short wTimeStamp;
+	unsigned short wFragCtl;
+	unsigned char byTxPower;
+	unsigned char wReserved;
+} __attribute__ ((__packed__))
 STxBufHead, *PSTxBufHead;
 typedef const STxBufHead *PCSTxBufHead;
 
 typedef struct tagSTxShortBufHead {
-    unsigned short wFIFOCtl;
-    unsigned short wTimeStamp;
-}__attribute__ ((__packed__))
+	unsigned short wFIFOCtl;
+	unsigned short wTimeStamp;
+} __attribute__ ((__packed__))
 STxShortBufHead, *PSTxShortBufHead;
 typedef const STxShortBufHead *PCSTxShortBufHead;
 
@@ -566,58 +551,56 @@ typedef const STxShortBufHead *PCSTxShortBufHead;
 // Tx data header
 //
 typedef struct tagSTxDataHead_g {
-    unsigned char bySignalField_b;
-    unsigned char byServiceField_b;
-    unsigned short wTransmitLength_b;
-    unsigned char bySignalField_a;
-    unsigned char byServiceField_a;
-    unsigned short wTransmitLength_a;
-    unsigned short wDuration_b;
-    unsigned short wDuration_a;
-    unsigned short wTimeStampOff_b;
-    unsigned short wTimeStampOff_a;
-}__attribute__ ((__packed__))
+	unsigned char bySignalField_b;
+	unsigned char byServiceField_b;
+	unsigned short wTransmitLength_b;
+	unsigned char bySignalField_a;
+	unsigned char byServiceField_a;
+	unsigned short wTransmitLength_a;
+	unsigned short wDuration_b;
+	unsigned short wDuration_a;
+	unsigned short wTimeStampOff_b;
+	unsigned short wTimeStampOff_a;
+} __attribute__ ((__packed__))
 STxDataHead_g, *PSTxDataHead_g;
 typedef const STxDataHead_g *PCSTxDataHead_g;
 
 typedef struct tagSTxDataHead_g_FB {
-    unsigned char bySignalField_b;
-    unsigned char byServiceField_b;
-    unsigned short wTransmitLength_b;
-    unsigned char bySignalField_a;
-    unsigned char byServiceField_a;
-    unsigned short wTransmitLength_a;
-    unsigned short wDuration_b;
-    unsigned short wDuration_a;
-    unsigned short wDuration_a_f0;
-    unsigned short wDuration_a_f1;
-    unsigned short wTimeStampOff_b;
-    unsigned short wTimeStampOff_a;
-}__attribute__ ((__packed__))
+	unsigned char bySignalField_b;
+	unsigned char byServiceField_b;
+	unsigned short wTransmitLength_b;
+	unsigned char bySignalField_a;
+	unsigned char byServiceField_a;
+	unsigned short wTransmitLength_a;
+	unsigned short wDuration_b;
+	unsigned short wDuration_a;
+	unsigned short wDuration_a_f0;
+	unsigned short wDuration_a_f1;
+	unsigned short wTimeStampOff_b;
+	unsigned short wTimeStampOff_a;
+} __attribute__ ((__packed__))
 STxDataHead_g_FB, *PSTxDataHead_g_FB;
 typedef const STxDataHead_g_FB *PCSTxDataHead_g_FB;
 
-
 typedef struct tagSTxDataHead_ab {
-    unsigned char bySignalField;
-    unsigned char byServiceField;
-    unsigned short wTransmitLength;
-    unsigned short wDuration;
-    unsigned short wTimeStampOff;
-}__attribute__ ((__packed__))
+	unsigned char bySignalField;
+	unsigned char byServiceField;
+	unsigned short wTransmitLength;
+	unsigned short wDuration;
+	unsigned short wTimeStampOff;
+} __attribute__ ((__packed__))
 STxDataHead_ab, *PSTxDataHead_ab;
 typedef const STxDataHead_ab *PCSTxDataHead_ab;
 
-
 typedef struct tagSTxDataHead_a_FB {
-    unsigned char bySignalField;
-    unsigned char byServiceField;
-    unsigned short wTransmitLength;
-    unsigned short wDuration;
-    unsigned short wTimeStampOff;
-    unsigned short wDuration_f0;
-    unsigned short wDuration_f1;
-}__attribute__ ((__packed__))
+	unsigned char bySignalField;
+	unsigned char byServiceField;
+	unsigned short wTransmitLength;
+	unsigned short wDuration;
+	unsigned short wTimeStampOff;
+	unsigned short wDuration_f0;
+	unsigned short wDuration_f1;
+} __attribute__ ((__packed__))
 STxDataHead_a_FB, *PSTxDataHead_a_FB;
 typedef const STxDataHead_a_FB *PCSTxDataHead_a_FB;
 
@@ -625,38 +608,37 @@ typedef const STxDataHead_a_FB *PCSTxDataHead_a_FB;
 // MICHDR data header
 //
 typedef struct tagSMICHDRHead {
-    u32 adwHDR0[4];
-    u32 adwHDR1[4];
-    u32 adwHDR2[4];
-}__attribute__ ((__packed__))
+	u32 adwHDR0[4];
+	u32 adwHDR1[4];
+	u32 adwHDR2[4];
+} __attribute__ ((__packed__))
 SMICHDRHead, *PSMICHDRHead;
 typedef const SMICHDRHead *PCSMICHDRHead;
 
 typedef struct tagSBEACONCtl {
-    u32 BufReady : 1;
-    u32 TSF      : 15;
-    u32 BufLen   : 11;
-    u32 Reserved : 5;
-}__attribute__ ((__packed__))
+	u32 BufReady:1;
+	u32 TSF:15;
+	u32 BufLen:11;
+	u32 Reserved:5;
+} __attribute__ ((__packed__))
 SBEACONCtl;
 
-
 typedef struct tagSSecretKey {
-    u32 dwLowDword;
-    unsigned char byHighByte;
-}__attribute__ ((__packed__))
+	u32 dwLowDword;
+	unsigned char byHighByte;
+} __attribute__ ((__packed__))
 SSecretKey;
 
 typedef struct tagSKeyEntry {
-    unsigned char abyAddrHi[2];
-    unsigned short wKCTL;
-    unsigned char abyAddrLo[4];
-    u32 dwKey0[4];
-    u32 dwKey1[4];
-    u32 dwKey2[4];
-    u32 dwKey3[4];
-    u32 dwKey4[4];
-}__attribute__ ((__packed__))
+	unsigned char abyAddrHi[2];
+	unsigned short wKCTL;
+	unsigned char abyAddrLo[4];
+	u32 dwKey0[4];
+	u32 dwKey1[4];
+	u32 dwKey2[4];
+	u32 dwKey3[4];
+	u32 dwKey4[4];
+} __attribute__ ((__packed__))
 SKeyEntry;
 /*---------------------  Export Macros ------------------------------*/
 
@@ -666,8 +648,4 @@ SKeyEntry;
 
 /*---------------------  Export Functions  --------------------------*/
 
-
-
-
 #endif // __DESC_H__
-
diff --git a/drivers/staging/vt6655/device.h b/drivers/staging/vt6655/device.h
index e27244ce383..ca1b8578cf7 100644
--- a/drivers/staging/vt6655/device.h
+++ b/drivers/staging/vt6655/device.h
@@ -87,7 +87,6 @@
 #include "key.h"
 #include "mac.h"
 
-
 /*---------------------  Export Definitions -------------------------*/
 
 #define MAC_MAX_CONTEXT_REG     (256+128)
@@ -111,8 +110,6 @@
 #define KEYSEL_TKIP                     2
 #define KEYSEL_CCMP                     3
 
-
-
 #define AUTO_FB_NONE            0
 #define AUTO_FB_0               1
 #define AUTO_FB_1               2
@@ -133,8 +130,6 @@
 #define BB_VGA_LEVEL            4
 #define BB_VGA_CHANGE_THRESHOLD 16
 
-
-
 #ifndef RUN_AT
 #define RUN_AT(x)                       (jiffies+(x))
 #endif
@@ -142,59 +137,52 @@
 // DMA related
 #define RESERV_AC0DMA                   4
 
-
 // BUILD OBJ mode
 
-
-#define	AVAIL_TD(p,q)	((p)->sOpts.nTxDescs[(q)]-((p)->iTDUsed[(q)]))
+#define	AVAIL_TD(p, q)	((p)->sOpts.nTxDescs[(q)] - ((p)->iTDUsed[(q)]))
 
 //PLICE_DEBUG ->
 #define	NUM				64
 //PLICE_DEUBG <-
 
-
-
 #define PRIVATE_Message                 0
 
 /*---------------------  Export Types  ------------------------------*/
 
-
-#define DBG_PRT(l, p, args...) {if (l<=msglevel) printk( p ,##args);}
-#define PRINT_K(p, args...) {if (PRIVATE_Message) printk( p ,##args);}
+#define DBG_PRT(l, p, args...) { if (l <= msglevel) printk(p, ##args); }
+#define PRINT_K(p, args...) { if (PRIVATE_Message) printk(p, ##args); }
 
 //0:11A 1:11B 2:11G
 typedef enum _VIA_BB_TYPE
 {
-    BB_TYPE_11A=0,
-    BB_TYPE_11B,
-    BB_TYPE_11G
+	BB_TYPE_11A = 0,
+	BB_TYPE_11B,
+	BB_TYPE_11G
 } VIA_BB_TYPE, *PVIA_BB_TYPE;
 
 //0:11a,1:11b,2:11gb(only CCK in BasicRate),3:11ga(OFDM in Basic Rate)
 typedef enum _VIA_PKT_TYPE
 {
-    PK_TYPE_11A=0,
-    PK_TYPE_11B,
-    PK_TYPE_11GB,
-    PK_TYPE_11GA
+	PK_TYPE_11A = 0,
+	PK_TYPE_11B,
+	PK_TYPE_11GB,
+	PK_TYPE_11GA
 } VIA_PKT_TYPE, *PVIA_PKT_TYPE;
 
-
 typedef enum __device_msg_level {
-    MSG_LEVEL_ERR=0,            //Errors that will cause abnormal operation.
-    MSG_LEVEL_NOTICE=1,         //Some errors need users to be notified.
-    MSG_LEVEL_INFO=2,           //Normal message.
-    MSG_LEVEL_VERBOSE=3,        //Will report all trival errors.
-    MSG_LEVEL_DEBUG=4           //Only for debug purpose.
+	MSG_LEVEL_ERR = 0,            //Errors that will cause abnormal operation.
+	MSG_LEVEL_NOTICE = 1,         //Some errors need users to be notified.
+	MSG_LEVEL_INFO = 2,           //Normal message.
+	MSG_LEVEL_VERBOSE = 3,        //Will report all trival errors.
+	MSG_LEVEL_DEBUG = 4           //Only for debug purpose.
 } DEVICE_MSG_LEVEL, *PDEVICE_MSG_LEVEL;
 
 typedef enum __device_init_type {
-    DEVICE_INIT_COLD=0,         // cold init
-    DEVICE_INIT_RESET,          // reset init or Dx to D0 power remain init
-    DEVICE_INIT_DXPL            // Dx to D0 power lost init
+	DEVICE_INIT_COLD = 0,         // cold init
+	DEVICE_INIT_RESET,          // reset init or Dx to D0 power remain init
+	DEVICE_INIT_DXPL            // Dx to D0 power lost init
 } DEVICE_INIT_TYPE, *PDEVICE_INIT_TYPE;
 
-
 //++ NDIS related
 
 #define MAX_BSSIDINFO_4_PMKID   16
@@ -205,57 +193,54 @@ typedef enum __device_init_type {
 // PMKID Structures
 typedef unsigned char NDIS_802_11_PMKID_VALUE[16];
 
-
 typedef enum _NDIS_802_11_WEP_STATUS
 {
-    Ndis802_11WEPEnabled,
-    Ndis802_11Encryption1Enabled = Ndis802_11WEPEnabled,
-    Ndis802_11WEPDisabled,
-    Ndis802_11EncryptionDisabled = Ndis802_11WEPDisabled,
-    Ndis802_11WEPKeyAbsent,
-    Ndis802_11Encryption1KeyAbsent = Ndis802_11WEPKeyAbsent,
-    Ndis802_11WEPNotSupported,
-    Ndis802_11EncryptionNotSupported = Ndis802_11WEPNotSupported,
-    Ndis802_11Encryption2Enabled,
-    Ndis802_11Encryption2KeyAbsent,
-    Ndis802_11Encryption3Enabled,
-    Ndis802_11Encryption3KeyAbsent
+	Ndis802_11WEPEnabled,
+	Ndis802_11Encryption1Enabled = Ndis802_11WEPEnabled,
+	Ndis802_11WEPDisabled,
+	Ndis802_11EncryptionDisabled = Ndis802_11WEPDisabled,
+	Ndis802_11WEPKeyAbsent,
+	Ndis802_11Encryption1KeyAbsent = Ndis802_11WEPKeyAbsent,
+	Ndis802_11WEPNotSupported,
+	Ndis802_11EncryptionNotSupported = Ndis802_11WEPNotSupported,
+	Ndis802_11Encryption2Enabled,
+	Ndis802_11Encryption2KeyAbsent,
+	Ndis802_11Encryption3Enabled,
+	Ndis802_11Encryption3KeyAbsent
 } NDIS_802_11_WEP_STATUS, *PNDIS_802_11_WEP_STATUS,
-  NDIS_802_11_ENCRYPTION_STATUS, *PNDIS_802_11_ENCRYPTION_STATUS;
-
+	NDIS_802_11_ENCRYPTION_STATUS, *PNDIS_802_11_ENCRYPTION_STATUS;
 
 typedef enum _NDIS_802_11_STATUS_TYPE
 {
-    Ndis802_11StatusType_Authentication,
-    Ndis802_11StatusType_MediaStreamMode,
-    Ndis802_11StatusType_PMKID_CandidateList,
-    Ndis802_11StatusTypeMax    // not a real type, defined as an upper bound
+	Ndis802_11StatusType_Authentication,
+	Ndis802_11StatusType_MediaStreamMode,
+	Ndis802_11StatusType_PMKID_CandidateList,
+	Ndis802_11StatusTypeMax    // not a real type, defined as an upper bound
 } NDIS_802_11_STATUS_TYPE, *PNDIS_802_11_STATUS_TYPE;
 
 //Added new types for PMKID Candidate lists.
 typedef struct _PMKID_CANDIDATE {
-    NDIS_802_11_MAC_ADDRESS BSSID;
-    unsigned long Flags;
+	NDIS_802_11_MAC_ADDRESS BSSID;
+	unsigned long Flags;
 } PMKID_CANDIDATE, *PPMKID_CANDIDATE;
 
-
 typedef struct _BSSID_INFO
 {
-    NDIS_802_11_MAC_ADDRESS BSSID;
-    NDIS_802_11_PMKID_VALUE PMKID;
+	NDIS_802_11_MAC_ADDRESS BSSID;
+	NDIS_802_11_PMKID_VALUE PMKID;
 } BSSID_INFO, *PBSSID_INFO;
 
 typedef struct tagSPMKID {
-    unsigned long Length;
-    unsigned long BSSIDInfoCount;
-    BSSID_INFO BSSIDInfo[MAX_BSSIDINFO_4_PMKID];
+	unsigned long Length;
+	unsigned long BSSIDInfoCount;
+	BSSID_INFO BSSIDInfo[MAX_BSSIDINFO_4_PMKID];
 } SPMKID, *PSPMKID;
 
 typedef struct tagSPMKIDCandidateEvent {
-    NDIS_802_11_STATUS_TYPE     StatusType;
-    unsigned long Version;       // Version of the structure
-    unsigned long NumCandidates; // No. of pmkid candidates
-    PMKID_CANDIDATE CandidateList[MAX_PMKIDLIST];
+	NDIS_802_11_STATUS_TYPE     StatusType;
+	unsigned long Version;       // Version of the structure
+	unsigned long NumCandidates; // No. of pmkid candidates
+	PMKID_CANDIDATE CandidateList[MAX_PMKIDLIST];
 } SPMKIDCandidateEvent, *PSPMKIDCandidateEvent;
 
 //--
@@ -264,58 +249,54 @@ typedef struct tagSPMKIDCandidateEvent {
 #define MAX_QUIET_COUNT     8
 
 typedef struct tagSQuietControl {
-    bool bEnable;
-    unsigned long dwStartTime;
-    unsigned char byPeriod;
-    unsigned short wDuration;
+	bool bEnable;
+	unsigned long dwStartTime;
+	unsigned char byPeriod;
+	unsigned short wDuration;
 } SQuietControl, *PSQuietControl;
 
 //--
-typedef struct __chip_info_tbl{
-    CHIP_TYPE   chip_id;
-    char*       name;
-    int         io_size;
-    int         nTxQueue;
-    u32         flags;
+typedef struct __chip_info_tbl {
+	CHIP_TYPE   chip_id;
+	char *name;
+	int         io_size;
+	int         nTxQueue;
+	u32         flags;
 } CHIP_INFO, *PCHIP_INFO;
 
-
 typedef enum {
-    OWNED_BY_HOST=0,
-    OWNED_BY_NIC=1
+	OWNED_BY_HOST = 0,
+	OWNED_BY_NIC = 1
 } DEVICE_OWNER_TYPE, *PDEVICE_OWNER_TYPE;
 
-
 // The receive duplicate detection cache entry
-typedef struct tagSCacheEntry{
-    unsigned short wFmSequence;
-    unsigned char abyAddr2[ETH_ALEN];
+typedef struct tagSCacheEntry {
+	unsigned short wFmSequence;
+	unsigned char abyAddr2[ETH_ALEN];
 } SCacheEntry, *PSCacheEntry;
 
-typedef struct tagSCache{
+typedef struct tagSCache {
 /* The receive cache is updated circularly.  The next entry to be written is
  * indexed by the "InPtr".
-*/
-    unsigned int uInPtr;         // Place to use next
-    SCacheEntry     asCacheEntry[DUPLICATE_RX_CACHE_LENGTH];
+ */
+	unsigned int uInPtr;         // Place to use next
+	SCacheEntry     asCacheEntry[DUPLICATE_RX_CACHE_LENGTH];
 } SCache, *PSCache;
 
 #define CB_MAX_RX_FRAG                 64
 // DeFragment Control Block, used for collecting fragments prior to reassembly
 typedef struct tagSDeFragControlBlock
 {
-    unsigned short wSequence;
-    unsigned short wFragNum;
-    unsigned char abyAddr2[ETH_ALEN];
-    unsigned int uLifetime;
-    struct sk_buff* skb;
-    unsigned char *pbyRxBuffer;
-    unsigned int cbFrameLength;
-    bool bInUse;
+	unsigned short wSequence;
+	unsigned short wFragNum;
+	unsigned char abyAddr2[ETH_ALEN];
+	unsigned int uLifetime;
+	struct sk_buff *skb;
+	unsigned char *pbyRxBuffer;
+	unsigned int cbFrameLength;
+	bool bInUse;
 } SDeFragControlBlock, *PSDeFragControlBlock;
 
-
-
 //flags for options
 #define     DEVICE_FLAGS_IP_ALIGN        0x00000001UL
 #define     DEVICE_FLAGS_PREAMBLE_TYPE   0x00000002UL
@@ -343,511 +324,474 @@ typedef struct tagSDeFragControlBlock
 //for device_set_media_duplex
 #define     DEVICE_LINK_CHANGE           0x00000001UL
 
-
 //PLICE_DEBUG->
 
-
 typedef	struct _RxManagementQueue
 {
 	int	packet_num;
-	int	head,tail;
+	int	head, tail;
 	PSRxMgmtPacket	Q[NUM];
-} RxManagementQueue,*PSRxManagementQueue;
-
-
+} RxManagementQueue, *PSRxManagementQueue;
 
 //PLICE_DEBUG<-
 
-
 typedef struct __device_opt {
-    int         nRxDescs0;    //Number of RX descriptors0
-    int         nRxDescs1;    //Number of RX descriptors1
-    int         nTxDescs[2];  //Number of TX descriptors 0, 1
-    int         int_works;    //interrupt limits
-    int         rts_thresh;   //rts threshold
-    int         frag_thresh;
-    int         data_rate;
-    int         channel_num;
-    int         short_retry;
-    int         long_retry;
-    int         bbp_type;
-    u32         flags;
+	int         nRxDescs0;    //Number of RX descriptors0
+	int         nRxDescs1;    //Number of RX descriptors1
+	int         nTxDescs[2];  //Number of TX descriptors 0, 1
+	int         int_works;    //interrupt limits
+	int         rts_thresh;   //rts threshold
+	int         frag_thresh;
+	int         data_rate;
+	int         channel_num;
+	int         short_retry;
+	int         long_retry;
+	int         bbp_type;
+	u32         flags;
 } OPTIONS, *POPTIONS;
 
-
 typedef struct __device_info {
-    struct __device_info*        next;
-    struct __device_info*        prev;
+	struct __device_info *next;
+	struct __device_info *prev;
 
-    struct pci_dev*             pcid;
+	struct pci_dev *pcid;
 
 #ifdef CONFIG_PM
-    u32                         pci_state[16];
+	u32                         pci_state[16];
 #endif
 
 // netdev
-    struct net_device*          dev;
-    struct net_device*          next_module;
-    struct net_device_stats     stats;
+	struct net_device *dev;
+	struct net_device *next_module;
+	struct net_device_stats     stats;
 
 //dma addr, rx/tx pool
-    dma_addr_t                  pool_dma;
-    dma_addr_t                  rd0_pool_dma;
-    dma_addr_t                  rd1_pool_dma;
+	dma_addr_t                  pool_dma;
+	dma_addr_t                  rd0_pool_dma;
+	dma_addr_t                  rd1_pool_dma;
 
-    dma_addr_t                  td0_pool_dma;
-    dma_addr_t                  td1_pool_dma;
+	dma_addr_t                  td0_pool_dma;
+	dma_addr_t                  td1_pool_dma;
 
-    dma_addr_t                  tx_bufs_dma0;
-    dma_addr_t                  tx_bufs_dma1;
-    dma_addr_t                  tx_beacon_dma;
+	dma_addr_t                  tx_bufs_dma0;
+	dma_addr_t                  tx_bufs_dma1;
+	dma_addr_t                  tx_beacon_dma;
 
-    unsigned char *tx0_bufs;
-    unsigned char *tx1_bufs;
-    unsigned char *tx_beacon_bufs;
+	unsigned char *tx0_bufs;
+	unsigned char *tx1_bufs;
+	unsigned char *tx_beacon_bufs;
 
-    CHIP_TYPE                   chip_id;
+	CHIP_TYPE                   chip_id;
 
-    unsigned long               PortOffset;
-    unsigned long dwIsr;
-    u32                         memaddr;
-    u32                         ioaddr;
-    u32                         io_size;
+	unsigned long               PortOffset;
+	unsigned long dwIsr;
+	u32                         memaddr;
+	u32                         ioaddr;
+	u32                         io_size;
 
-    unsigned char byRevId;
-    unsigned short SubSystemID;
-    unsigned short SubVendorID;
+	unsigned char byRevId;
+	unsigned short SubSystemID;
+	unsigned short SubVendorID;
 
-    int                         nTxQueues;
-    volatile int                iTDUsed[TYPE_MAXTD];
+	int                         nTxQueues;
+	volatile int                iTDUsed[TYPE_MAXTD];
 
-    volatile PSTxDesc           apCurrTD[TYPE_MAXTD];
-    volatile PSTxDesc           apTailTD[TYPE_MAXTD];
+	volatile PSTxDesc           apCurrTD[TYPE_MAXTD];
+	volatile PSTxDesc           apTailTD[TYPE_MAXTD];
 
-    volatile PSTxDesc           apTD0Rings;
-    volatile PSTxDesc           apTD1Rings;
+	volatile PSTxDesc           apTD0Rings;
+	volatile PSTxDesc           apTD1Rings;
 
-    volatile PSRxDesc           aRD0Ring;
-    volatile PSRxDesc           aRD1Ring;
-    volatile PSRxDesc           pCurrRD[TYPE_MAXRD];
-    SCache                      sDupRxCache;
+	volatile PSRxDesc           aRD0Ring;
+	volatile PSRxDesc           aRD1Ring;
+	volatile PSRxDesc           pCurrRD[TYPE_MAXRD];
+	SCache                      sDupRxCache;
 
-    SDeFragControlBlock         sRxDFCB[CB_MAX_RX_FRAG];
-    unsigned int	cbDFCB;
-    unsigned int	cbFreeDFCB;
-    unsigned int	uCurrentDFCBIdx;
+	SDeFragControlBlock         sRxDFCB[CB_MAX_RX_FRAG];
+	unsigned int	cbDFCB;
+	unsigned int	cbFreeDFCB;
+	unsigned int	uCurrentDFCBIdx;
 
-    OPTIONS                     sOpts;
+	OPTIONS                     sOpts;
 
-    u32                         flags;
+	u32                         flags;
 
-    u32                         rx_buf_sz;
-    int                         multicast_limit;
-    unsigned char byRxMode;
+	u32                         rx_buf_sz;
+	int                         multicast_limit;
+	unsigned char byRxMode;
 
-    spinlock_t                  lock;
+	spinlock_t                  lock;
 //PLICE_DEBUG->
-	struct	tasklet_struct 		RxMngWorkItem;
+	struct	tasklet_struct	RxMngWorkItem;
 	RxManagementQueue	rxManeQueue;
 //PLICE_DEBUG<-
 //PLICE_DEBUG ->
-	pid_t				MLMEThr_pid;
-	struct 	completion	notify;
-	struct 	semaphore	mlme_semaphore;
+	pid_t			MLMEThr_pid;
+	struct completion	notify;
+	struct semaphore	mlme_semaphore;
 //PLICE_DEBUG <-
 
-
-    u32                         rx_bytes;
-
-    // Version control
-    unsigned char byLocalID;
-    unsigned char byRFType;
-
-    unsigned char byMaxPwrLevel;
-    unsigned char byZoneType;
-    bool bZoneRegExist;
-   unsigned char byOriginalZonetype;
-    unsigned char abyMacContext[MAC_MAX_CONTEXT_REG];
-    bool bLinkPass;          // link status: OK or fail
-    unsigned char abyCurrentNetAddr[ETH_ALEN];
-
-    // Adapter statistics
-    SStatCounter                scStatistic;
-    // 802.11 counter
-    SDot11Counters              s802_11Counter;
-
-
-    // 802.11 management
-    PSMgmtObject                pMgmt;
-    SMgmtObject                 sMgmtObj;
-
-    // 802.11 MAC specific
-    unsigned int	uCurrRSSI;
-    unsigned char byCurrSQ;
-
-    unsigned long dwTxAntennaSel;
-    unsigned long dwRxAntennaSel;
-    unsigned char byAntennaCount;
-    unsigned char byRxAntennaMode;
-    unsigned char byTxAntennaMode;
-    bool bTxRxAntInv;
-
-    unsigned char *pbyTmpBuff;
-    unsigned int	uSIFS;    //Current SIFS
-    unsigned int	uDIFS;    //Current DIFS
-    unsigned int	uEIFS;    //Current EIFS
-    unsigned int	uSlot;    //Current SlotTime
-    unsigned int	uCwMin;   //Current CwMin
-    unsigned int	uCwMax;   //CwMax is fixed on 1023.
-    // PHY parameter
-    unsigned char bySIFS;
-    unsigned char byDIFS;
-    unsigned char byEIFS;
-    unsigned char bySlot;
-    unsigned char byCWMaxMin;
-    CARD_PHY_TYPE               eCurrentPHYType;
-
-
-    VIA_BB_TYPE                 byBBType; //0: 11A, 1:11B, 2:11G
-    VIA_PKT_TYPE                byPacketType; //0:11a,1:11b,2:11gb(only CCK in BasicRate),3:11ga(OFDM in Basic Rate)
-    unsigned short wBasicRate;
-    unsigned char byACKRate;
-    unsigned char byTopOFDMBasicRate;
-    unsigned char byTopCCKBasicRate;
-
-    unsigned char byMinChannel;
-    unsigned char byMaxChannel;
-    unsigned int	uConnectionRate;
-
-    unsigned char byPreambleType;
-    unsigned char byShortPreamble;
-
-    unsigned short wCurrentRate;
-    unsigned short wRTSThreshold;
-    unsigned short wFragmentationThreshold;
-    unsigned char byShortRetryLimit;
-    unsigned char byLongRetryLimit;
-    CARD_OP_MODE                eOPMode;
-    unsigned char byOpMode;
-    bool bBSSIDFilter;
-    unsigned short wMaxTransmitMSDULifetime;
-    unsigned char abyBSSID[ETH_ALEN];
-    unsigned char abyDesireBSSID[ETH_ALEN];
-    unsigned short wCTSDuration;       // update while speed change
-    unsigned short wACKDuration;       // update while speed change
-    unsigned short wRTSTransmitLen;    // update while speed change
-    unsigned char byRTSServiceField;  // update while speed change
-    unsigned char byRTSSignalField;   // update while speed change
-
-    unsigned long dwMaxReceiveLifetime;       // dot11MaxReceiveLifetime
-
-    bool bCCK;
-    bool bEncryptionEnable;
-    bool bLongHeader;
-    bool bShortSlotTime;
-    bool bProtectMode;
-    bool bNonERPPresent;
-    bool bBarkerPreambleMd;
-
-    unsigned char byERPFlag;
-    unsigned short wUseProtectCntDown;
-
-    bool bRadioControlOff;
-    bool bRadioOff;
-    bool bEnablePSMode;
-    unsigned short wListenInterval;
-    bool bPWBitOn;
-    WMAC_POWER_MODE         ePSMode;
-
-
-    // GPIO Radio Control
-    unsigned char byRadioCtl;
-    unsigned char byGPIO;
-    bool bHWRadioOff;
-    bool bPrvActive4RadioOFF;
-    bool bGPIOBlockRead;
-
-    // Beacon related
-    unsigned short wSeqCounter;
-    unsigned short wBCNBufLen;
-    bool bBeaconBufReady;
-    bool bBeaconSent;
-    bool bIsBeaconBufReadySet;
-    unsigned int	cbBeaconBufReadySetCnt;
-    bool bFixRate;
-    unsigned char byCurrentCh;
-    unsigned int	uScanTime;
-
-    CMD_STATE               eCommandState;
-
-    CMD_CODE                eCommand;
-    bool bBeaconTx;
-
-    bool bStopBeacon;
-    bool bStopDataPkt;
-    bool bStopTx0Pkt;
-    unsigned int	uAutoReConnectTime;
-
-    // 802.11 counter
-
-    CMD_ITEM                eCmdQueue[CMD_Q_SIZE];
-    unsigned int	uCmdDequeueIdx;
-    unsigned int	uCmdEnqueueIdx;
-    unsigned int	cbFreeCmdQueue;
-    bool bCmdRunning;
-    bool bCmdClear;
-
-
-
-    bool bRoaming;
-    //WOW
-    unsigned char abyIPAddr[4];
-
-    unsigned long ulTxPower;
-    NDIS_802_11_WEP_STATUS  eEncryptionStatus;
-    bool bTransmitKey;
+	u32                         rx_bytes;
+
+	// Version control
+	unsigned char byLocalID;
+	unsigned char byRFType;
+
+	unsigned char byMaxPwrLevel;
+	unsigned char byZoneType;
+	bool bZoneRegExist;
+	unsigned char byOriginalZonetype;
+	unsigned char abyMacContext[MAC_MAX_CONTEXT_REG];
+	bool bLinkPass;          // link status: OK or fail
+	unsigned char abyCurrentNetAddr[ETH_ALEN];
+
+	// Adapter statistics
+	SStatCounter                scStatistic;
+	// 802.11 counter
+	SDot11Counters              s802_11Counter;
+
+	// 802.11 management
+	PSMgmtObject                pMgmt;
+	SMgmtObject                 sMgmtObj;
+
+	// 802.11 MAC specific
+	unsigned int	uCurrRSSI;
+	unsigned char byCurrSQ;
+
+	unsigned long dwTxAntennaSel;
+	unsigned long dwRxAntennaSel;
+	unsigned char byAntennaCount;
+	unsigned char byRxAntennaMode;
+	unsigned char byTxAntennaMode;
+	bool bTxRxAntInv;
+
+	unsigned char *pbyTmpBuff;
+	unsigned int	uSIFS;    //Current SIFS
+	unsigned int	uDIFS;    //Current DIFS
+	unsigned int	uEIFS;    //Current EIFS
+	unsigned int	uSlot;    //Current SlotTime
+	unsigned int	uCwMin;   //Current CwMin
+	unsigned int	uCwMax;   //CwMax is fixed on 1023.
+	// PHY parameter
+	unsigned char bySIFS;
+	unsigned char byDIFS;
+	unsigned char byEIFS;
+	unsigned char bySlot;
+	unsigned char byCWMaxMin;
+	CARD_PHY_TYPE               eCurrentPHYType;
+
+	VIA_BB_TYPE                 byBBType; //0: 11A, 1:11B, 2:11G
+	VIA_PKT_TYPE                byPacketType; //0:11a,1:11b,2:11gb(only CCK in BasicRate),3:11ga(OFDM in Basic Rate)
+	unsigned short wBasicRate;
+	unsigned char byACKRate;
+	unsigned char byTopOFDMBasicRate;
+	unsigned char byTopCCKBasicRate;
+
+	unsigned char byMinChannel;
+	unsigned char byMaxChannel;
+	unsigned int	uConnectionRate;
+
+	unsigned char byPreambleType;
+	unsigned char byShortPreamble;
+
+	unsigned short wCurrentRate;
+	unsigned short wRTSThreshold;
+	unsigned short wFragmentationThreshold;
+	unsigned char byShortRetryLimit;
+	unsigned char byLongRetryLimit;
+	CARD_OP_MODE                eOPMode;
+	unsigned char byOpMode;
+	bool bBSSIDFilter;
+	unsigned short wMaxTransmitMSDULifetime;
+	unsigned char abyBSSID[ETH_ALEN];
+	unsigned char abyDesireBSSID[ETH_ALEN];
+	unsigned short wCTSDuration;       // update while speed change
+	unsigned short wACKDuration;       // update while speed change
+	unsigned short wRTSTransmitLen;    // update while speed change
+	unsigned char byRTSServiceField;  // update while speed change
+	unsigned char byRTSSignalField;   // update while speed change
+
+	unsigned long dwMaxReceiveLifetime;       // dot11MaxReceiveLifetime
+
+	bool bCCK;
+	bool bEncryptionEnable;
+	bool bLongHeader;
+	bool bShortSlotTime;
+	bool bProtectMode;
+	bool bNonERPPresent;
+	bool bBarkerPreambleMd;
+
+	unsigned char byERPFlag;
+	unsigned short wUseProtectCntDown;
+
+	bool bRadioControlOff;
+	bool bRadioOff;
+	bool bEnablePSMode;
+	unsigned short wListenInterval;
+	bool bPWBitOn;
+	WMAC_POWER_MODE         ePSMode;
+
+	// GPIO Radio Control
+	unsigned char byRadioCtl;
+	unsigned char byGPIO;
+	bool bHWRadioOff;
+	bool bPrvActive4RadioOFF;
+	bool bGPIOBlockRead;
+
+	// Beacon related
+	unsigned short wSeqCounter;
+	unsigned short wBCNBufLen;
+	bool bBeaconBufReady;
+	bool bBeaconSent;
+	bool bIsBeaconBufReadySet;
+	unsigned int	cbBeaconBufReadySetCnt;
+	bool bFixRate;
+	unsigned char byCurrentCh;
+	unsigned int	uScanTime;
+
+	CMD_STATE               eCommandState;
+
+	CMD_CODE                eCommand;
+	bool bBeaconTx;
+
+	bool bStopBeacon;
+	bool bStopDataPkt;
+	bool bStopTx0Pkt;
+	unsigned int	uAutoReConnectTime;
+
+	// 802.11 counter
+
+	CMD_ITEM                eCmdQueue[CMD_Q_SIZE];
+	unsigned int	uCmdDequeueIdx;
+	unsigned int	uCmdEnqueueIdx;
+	unsigned int	cbFreeCmdQueue;
+	bool bCmdRunning;
+	bool bCmdClear;
+
+	bool bRoaming;
+	//WOW
+	unsigned char abyIPAddr[4];
+
+	unsigned long ulTxPower;
+	NDIS_802_11_WEP_STATUS  eEncryptionStatus;
+	bool bTransmitKey;
 //2007-0925-01<Add>by MikeLiu
 //mike add :save old Encryption
-    NDIS_802_11_WEP_STATUS  eOldEncryptionStatus;
-
-    SKeyManagement          sKey;
-    unsigned long dwIVCounter;
-
-    QWORD                   qwPacketNumber; //For CCMP and TKIP as TSC(6 bytes)
-    unsigned int	uCurrentWEPMode;
-
-    RC4Ext                  SBox;
-    unsigned char abyPRNG[WLAN_WEPMAX_KEYLEN+3];
-    unsigned char byKeyIndex;
-    unsigned int	uKeyLength;
-    unsigned char abyKey[WLAN_WEP232_KEYLEN];
-
-    bool bAES;
-    unsigned char byCntMeasure;
-
-    // for AP mode
-    unsigned int	uAssocCount;
-    bool bMoreData;
-
-    // QoS
-    bool bGrpAckPolicy;
-
-    // for OID_802_11_ASSOCIATION_INFORMATION
-    bool bAssocInfoSet;
-
-
-    unsigned char byAutoFBCtrl;
-
-    bool bTxMICFail;
-    bool bRxMICFail;
-
-
-    unsigned int	uRATEIdx;
-
-
-    // For Update BaseBand VGA Gain Offset
-    bool bUpdateBBVGA;
-    unsigned int	uBBVGADiffCount;
-    unsigned char byBBVGANew;
-    unsigned char byBBVGACurrent;
-    unsigned char abyBBVGA[BB_VGA_LEVEL];
-    long                    ldBmThreshold[BB_VGA_LEVEL];
-
-    unsigned char byBBPreEDRSSI;
-    unsigned char byBBPreEDIndex;
-
-
-    bool bRadioCmd;
-    unsigned long dwDiagRefCount;
-
-    // For FOE Tuning
-    unsigned char byFOETuning;
-
-    // For Auto Power Tunning
-
-    unsigned char byAutoPwrTunning;
-    short                   sPSetPointCCK;
-    short                   sPSetPointOFDMG;
-    short                   sPSetPointOFDMA;
-    long                    lPFormulaOffset;
-    short                   sPThreshold;
-    char                    cAdjustStep;
-    char                    cMinTxAGC;
-
-    // For RF Power table
-    unsigned char byCCKPwr;
-    unsigned char byOFDMPwrG;
-    unsigned char byCurPwr;
-    char	 byCurPwrdBm;
-    unsigned char abyCCKPwrTbl[CB_MAX_CHANNEL_24G+1];
-    unsigned char abyOFDMPwrTbl[CB_MAX_CHANNEL+1];
-    char	abyCCKDefaultPwr[CB_MAX_CHANNEL_24G+1];
-    char	abyOFDMDefaultPwr[CB_MAX_CHANNEL+1];
-    char	abyRegPwr[CB_MAX_CHANNEL+1];
-    char	abyLocalPwr[CB_MAX_CHANNEL+1];
-
-
-    // BaseBand Loopback Use
-    unsigned char byBBCR4d;
-    unsigned char byBBCRc9;
-    unsigned char byBBCR88;
-    unsigned char byBBCR09;
-
-    // command timer
-    struct timer_list       sTimerCommand;
+	NDIS_802_11_WEP_STATUS  eOldEncryptionStatus;
+
+	SKeyManagement          sKey;
+	unsigned long dwIVCounter;
+
+	QWORD                   qwPacketNumber; //For CCMP and TKIP as TSC(6 bytes)
+	unsigned int	uCurrentWEPMode;
+
+	RC4Ext                  SBox;
+	unsigned char abyPRNG[WLAN_WEPMAX_KEYLEN+3];
+	unsigned char byKeyIndex;
+	unsigned int	uKeyLength;
+	unsigned char abyKey[WLAN_WEP232_KEYLEN];
+
+	bool bAES;
+	unsigned char byCntMeasure;
+
+	// for AP mode
+	unsigned int	uAssocCount;
+	bool bMoreData;
+
+	// QoS
+	bool bGrpAckPolicy;
+
+	// for OID_802_11_ASSOCIATION_INFORMATION
+	bool bAssocInfoSet;
+
+	unsigned char byAutoFBCtrl;
+
+	bool bTxMICFail;
+	bool bRxMICFail;
+
+	unsigned int	uRATEIdx;
+
+	// For Update BaseBand VGA Gain Offset
+	bool bUpdateBBVGA;
+	unsigned int	uBBVGADiffCount;
+	unsigned char byBBVGANew;
+	unsigned char byBBVGACurrent;
+	unsigned char abyBBVGA[BB_VGA_LEVEL];
+	long                    ldBmThreshold[BB_VGA_LEVEL];
+
+	unsigned char byBBPreEDRSSI;
+	unsigned char byBBPreEDIndex;
+
+	bool bRadioCmd;
+	unsigned long dwDiagRefCount;
+
+	// For FOE Tuning
+	unsigned char byFOETuning;
+
+	// For Auto Power Tunning
+
+	unsigned char byAutoPwrTunning;
+	short                   sPSetPointCCK;
+	short                   sPSetPointOFDMG;
+	short                   sPSetPointOFDMA;
+	long                    lPFormulaOffset;
+	short                   sPThreshold;
+	char                    cAdjustStep;
+	char                    cMinTxAGC;
+
+	// For RF Power table
+	unsigned char byCCKPwr;
+	unsigned char byOFDMPwrG;
+	unsigned char byCurPwr;
+	char	 byCurPwrdBm;
+	unsigned char abyCCKPwrTbl[CB_MAX_CHANNEL_24G+1];
+	unsigned char abyOFDMPwrTbl[CB_MAX_CHANNEL+1];
+	char	abyCCKDefaultPwr[CB_MAX_CHANNEL_24G+1];
+	char	abyOFDMDefaultPwr[CB_MAX_CHANNEL+1];
+	char	abyRegPwr[CB_MAX_CHANNEL+1];
+	char	abyLocalPwr[CB_MAX_CHANNEL+1];
+
+	// BaseBand Loopback Use
+	unsigned char byBBCR4d;
+	unsigned char byBBCRc9;
+	unsigned char byBBCR88;
+	unsigned char byBBCR09;
+
+	// command timer
+	struct timer_list       sTimerCommand;
 #ifdef TxInSleep
-     struct timer_list       sTimerTxData;
-     unsigned long nTxDataTimeCout;
-     bool fTxDataInSleep;
-     bool IsTxDataTrigger;
+	struct timer_list       sTimerTxData;
+	unsigned long nTxDataTimeCout;
+	bool fTxDataInSleep;
+	bool IsTxDataTrigger;
 #endif
 
 #ifdef WPA_SM_Transtatus
-    bool fWPA_Authened;           //is WPA/WPA-PSK or WPA2/WPA2-PSK authen??
+	bool fWPA_Authened;           //is WPA/WPA-PSK or WPA2/WPA2-PSK authen??
 #endif
-    unsigned char byReAssocCount;   //mike add:re-association retry times!
-    unsigned char byLinkWaitCount;
+	unsigned char byReAssocCount;   //mike add:re-association retry times!
+	unsigned char byLinkWaitCount;
 
+	unsigned char abyNodeName[17];
 
-    unsigned char abyNodeName[17];
-
-    bool bDiversityRegCtlON;
-    bool bDiversityEnable;
-    unsigned long ulDiversityNValue;
-    unsigned long ulDiversityMValue;
-    unsigned char byTMax;
-    unsigned char byTMax2;
-    unsigned char byTMax3;
-    unsigned long ulSQ3TH;
+	bool bDiversityRegCtlON;
+	bool bDiversityEnable;
+	unsigned long ulDiversityNValue;
+	unsigned long ulDiversityMValue;
+	unsigned char byTMax;
+	unsigned char byTMax2;
+	unsigned char byTMax3;
+	unsigned long ulSQ3TH;
 
 // ANT diversity
-    unsigned long uDiversityCnt;
-    unsigned char byAntennaState;
-    unsigned long ulRatio_State0;
-    unsigned long ulRatio_State1;
-
-    //SQ3 functions for antenna diversity
-    struct timer_list           TimerSQ3Tmax1;
-    struct timer_list           TimerSQ3Tmax2;
-    struct timer_list           TimerSQ3Tmax3;
-
-
-    unsigned long uNumSQ3[MAX_RATE];
-    unsigned short wAntDiversityMaxRate;
-
-
-    SEthernetHeader         sTxEthHeader;
-    SEthernetHeader         sRxEthHeader;
-    unsigned char abyBroadcastAddr[ETH_ALEN];
-    unsigned char abySNAP_RFC1042[ETH_ALEN];
-    unsigned char abySNAP_Bridgetunnel[ETH_ALEN];
-     unsigned char abyEEPROM[EEP_MAX_CONTEXT_SIZE];  //unsigned long alignment
-    // Pre-Authentication & PMK cache
-    SPMKID                  gsPMKID;
-    SPMKIDCandidateEvent    gsPMKIDCandidate;
-
-
-    // for 802.11h
-    bool b11hEnable;
-    unsigned char abyCountryCode[3];
-    // for 802.11h DFS
-    unsigned int	uNumOfMeasureEIDs;
-    PWLAN_IE_MEASURE_REQ    pCurrMeasureEID;
-    bool bMeasureInProgress;
-    unsigned char byOrgChannel;
-    unsigned char byOrgRCR;
-    unsigned long dwOrgMAR0;
-    unsigned long dwOrgMAR4;
-    unsigned char byBasicMap;
-    unsigned char byCCAFraction;
-    unsigned char abyRPIs[8];
-    unsigned long dwRPIs[8];
-    bool bChannelSwitch;
-    unsigned char byNewChannel;
-    unsigned char byChannelSwitchCount;
-    bool bQuietEnable;
-    bool bEnableFirstQuiet;
-    unsigned char byQuietStartCount;
-    unsigned int	uQuietEnqueue;
-    unsigned long dwCurrentQuietEndTime;
-    SQuietControl           sQuiet[MAX_QUIET_COUNT];
-    // for 802.11h TPC
-    bool bCountryInfo5G;
-    bool bCountryInfo24G;
-
-    unsigned short wBeaconInterval;
-
-    //WPA supplicant deamon
+	unsigned long uDiversityCnt;
+	unsigned char byAntennaState;
+	unsigned long ulRatio_State0;
+	unsigned long ulRatio_State1;
+
+	//SQ3 functions for antenna diversity
+	struct timer_list           TimerSQ3Tmax1;
+	struct timer_list           TimerSQ3Tmax2;
+	struct timer_list           TimerSQ3Tmax3;
+
+	unsigned long uNumSQ3[MAX_RATE];
+	unsigned short wAntDiversityMaxRate;
+
+	SEthernetHeader         sTxEthHeader;
+	SEthernetHeader         sRxEthHeader;
+	unsigned char abyBroadcastAddr[ETH_ALEN];
+	unsigned char abySNAP_RFC1042[ETH_ALEN];
+	unsigned char abySNAP_Bridgetunnel[ETH_ALEN];
+	unsigned char abyEEPROM[EEP_MAX_CONTEXT_SIZE];  //unsigned long alignment
+	// Pre-Authentication & PMK cache
+	SPMKID                  gsPMKID;
+	SPMKIDCandidateEvent    gsPMKIDCandidate;
+
+	// for 802.11h
+	bool b11hEnable;
+	unsigned char abyCountryCode[3];
+	// for 802.11h DFS
+	unsigned int	uNumOfMeasureEIDs;
+	PWLAN_IE_MEASURE_REQ    pCurrMeasureEID;
+	bool bMeasureInProgress;
+	unsigned char byOrgChannel;
+	unsigned char byOrgRCR;
+	unsigned long dwOrgMAR0;
+	unsigned long dwOrgMAR4;
+	unsigned char byBasicMap;
+	unsigned char byCCAFraction;
+	unsigned char abyRPIs[8];
+	unsigned long dwRPIs[8];
+	bool bChannelSwitch;
+	unsigned char byNewChannel;
+	unsigned char byChannelSwitchCount;
+	bool bQuietEnable;
+	bool bEnableFirstQuiet;
+	unsigned char byQuietStartCount;
+	unsigned int	uQuietEnqueue;
+	unsigned long dwCurrentQuietEndTime;
+	SQuietControl           sQuiet[MAX_QUIET_COUNT];
+	// for 802.11h TPC
+	bool bCountryInfo5G;
+	bool bCountryInfo24G;
+
+	unsigned short wBeaconInterval;
+
+	//WPA supplicant deamon
 	struct net_device       *wpadev;
 	bool bWPADEVUp;
-    struct sk_buff          *skb;
+	struct sk_buff          *skb;
 #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
 /*
-        bool bwextstep0;
-        bool bwextstep1;
-        bool bwextstep2;
-        bool bwextstep3;
-        */
-        unsigned int	bwextcount;
-        bool bWPASuppWextEnabled;
+  bool bwextstep0;
+  bool bwextstep1;
+  bool bwextstep2;
+  bool bwextstep3;
+*/
+	unsigned int	bwextcount;
+	bool bWPASuppWextEnabled;
 #endif
 
-    //--
+	//--
 #ifdef HOSTAP
-    // user space daemon: hostapd, is used for HOSTAP
+	// user space daemon: hostapd, is used for HOSTAP
 	bool bEnableHostapd;
 	bool bEnable8021x;
 	bool bEnableHostWEP;
 	struct net_device       *apdev;
 	int (*tx_80211)(struct sk_buff *skb, struct net_device *dev);
 #endif
-    unsigned int	uChannel;
-    bool bMACSuspend;
+	unsigned int	uChannel;
+	bool bMACSuspend;
 
 	struct iw_statistics	wstats;		// wireless stats
-    bool bCommit;
-
+	bool bCommit;
 } DEVICE_INFO, *PSDevice;
 
-
 //PLICE_DEBUG->
 
-
- inline  static	void   EnQueue (PSDevice pDevice,PSRxMgmtPacket  pRxMgmtPacket)
+inline  static	void   EnQueue(PSDevice pDevice, PSRxMgmtPacket  pRxMgmtPacket)
 {
-	//printk("Enter EnQueue:tail is %d\n",pDevice->rxManeQueue.tail);
-	if ((pDevice->rxManeQueue.tail+1) % NUM == pDevice->rxManeQueue.head)
-	{
-		//printk("Queue is Full,tail is %d\n",pDevice->rxManeQueue.tail);
-		return ;
-	}
-	else
-	{
-		pDevice->rxManeQueue.tail = (pDevice->rxManeQueue.tail+1)% NUM;
+	if ((pDevice->rxManeQueue.tail+1) % NUM == pDevice->rxManeQueue.head) {
+		return;
+	} else {
+		pDevice->rxManeQueue.tail = (pDevice->rxManeQueue.tail + 1) % NUM;
 		pDevice->rxManeQueue.Q[pDevice->rxManeQueue.tail] = pRxMgmtPacket;
 		pDevice->rxManeQueue.packet_num++;
-		//printk("packet num is %d\n",pDevice->rxManeQueue.packet_num);
 	}
 }
 
-
-
-
-	inline  static  PSRxMgmtPacket DeQueue (PSDevice pDevice)
+inline  static  PSRxMgmtPacket DeQueue(PSDevice pDevice)
 {
 	PSRxMgmtPacket  pRxMgmtPacket;
-	if (pDevice->rxManeQueue.tail == pDevice->rxManeQueue.head)
-	{
+	if (pDevice->rxManeQueue.tail == pDevice->rxManeQueue.head) {
 		printk("Queue is Empty\n");
 		return NULL;
-	}
-	else
-	{
+	} else {
 		int	x;
 		//x=pDevice->rxManeQueue.head = (pDevice->rxManeQueue.head+1)%NUM;
 		pDevice->rxManeQueue.head = (pDevice->rxManeQueue.head+1)%NUM;
 		x = pDevice->rxManeQueue.head;
-		//printk("Enter DeQueue:head is %d\n",x);
 		pRxMgmtPacket = pDevice->rxManeQueue.Q[x];
 		pDevice->rxManeQueue.packet_num--;
 		return pRxMgmtPacket;
@@ -856,31 +800,22 @@ typedef struct __device_info {
 
 void	InitRxManagementQueue(PSDevice   pDevice);
 
-
-
 //PLICE_DEBUG<-
 
-
-
-
-
-
 inline static bool device_get_ip(PSDevice pInfo) {
-    struct in_device* in_dev=(struct in_device*) pInfo->dev->ip_ptr;
-    struct in_ifaddr* ifa;
-
-    if (in_dev!=NULL) {
-        ifa=(struct in_ifaddr*) in_dev->ifa_list;
-        if (ifa!=NULL) {
-            memcpy(pInfo->abyIPAddr,&ifa->ifa_address,4);
-            return true;
-        }
-    }
-    return false;
+	struct in_device *in_dev = (struct in_device *)pInfo->dev->ip_ptr;
+	struct in_ifaddr *ifa;
+
+	if (in_dev != NULL) {
+		ifa = (struct in_ifaddr *)in_dev->ifa_list;
+		if (ifa != NULL) {
+			memcpy(pInfo->abyIPAddr, &ifa->ifa_address, 4);
+			return true;
+		}
+	}
+	return false;
 }
 
-
-
 static inline PDEVICE_RD_INFO alloc_rd_info(void)
 {
 	return kzalloc(sizeof(DEVICE_RD_INFO), GFP_ATOMIC);
@@ -897,5 +832,3 @@ bool device_dma0_xmit(PSDevice pDevice, struct sk_buff *skb, unsigned int uNodeI
 bool device_alloc_frag_buf(PSDevice pDevice, PSDeFragControlBlock pDeF);
 int Config_FileOperation(PSDevice pDevice, bool fwrite, unsigned char *Parameter);
 #endif
-
-
diff --git a/drivers/staging/vt6655/device_cfg.h b/drivers/staging/vt6655/device_cfg.h
index 408edc27075..1137adede9e 100644
--- a/drivers/staging/vt6655/device_cfg.h
+++ b/drivers/staging/vt6655/device_cfg.h
@@ -34,9 +34,9 @@
 
 typedef
 struct _version {
-    unsigned char   major;
-    unsigned char   minor;
-    unsigned char   build;
+	unsigned char   major;
+	unsigned char   minor;
+	unsigned char   build;
 } version_t, *pversion_t;
 
 #define VID_TABLE_SIZE      64
@@ -70,31 +70,26 @@ struct _version {
 //Max: 2378=2312Payload + 30HD +4CRC + 2Padding + 4Len + 8TSF + 4RSR
 #define PKT_BUF_SZ          2390
 
-
 #define MAX_UINTS           8
 #define OPTION_DEFAULT      { [0 ... MAX_UINTS-1] = -1}
 
-
-
-typedef enum  _chip_type{
-    VT3253=1
+typedef enum  _chip_type {
+	VT3253 = 1
 } CHIP_TYPE, *PCHIP_TYPE;
 
-
-
 #ifdef VIAWET_DEBUG
-#define ASSERT(x) { \
-    if (!(x)) { \
-        printk(KERN_ERR "assertion %s failed: file %s line %d\n", #x,\
-        __FUNCTION__, __LINE__);\
-        *(int*) 0=0;\
-    }\
-}
+#define ASSERT(x)							\
+do {									\
+	if (!(x)) {							\
+		printk(KERN_ERR "assertion %s failed: file %s line %d\n", \
+		       #x, __func__, __LINE__);				\
+		*(int *)0 = 0;						\
+	}								\
+} while (0)
 #define DBG_PORT80(value)                   outb(value, 0x80)
 #else
 #define ASSERT(x)
 #define DBG_PORT80(value)
 #endif
 
-
 #endif
diff --git a/drivers/staging/vt6655/device_main.c b/drivers/staging/vt6655/device_main.c
index 453c83d7fe8..be4f6c2ca3f 100644
--- a/drivers/staging/vt6655/device_main.c
+++ b/drivers/staging/vt6655/device_main.c
@@ -98,52 +98,50 @@ MODULE_AUTHOR("VIA Networking Technologies, Inc., <lyndonchen@vntek.com.tw>");
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("VIA Networking Solomon-A/B/G Wireless LAN Adapter Driver");
 
-	static int mlme_kill;
-	//static  struct task_struct * mlme_task;
+static int mlme_kill;
+//static  struct task_struct * mlme_task;
 
-#define DEVICE_PARAM(N,D)
+#define DEVICE_PARAM(N, D)
 /*
-        static const int N[MAX_UINTS]=OPTION_DEFAULT;\
-        MODULE_PARM(N, "1-" __MODULE_STRING(MAX_UINTS) "i");\
-        MODULE_PARM_DESC(N, D);
+  static const int N[MAX_UINTS]=OPTION_DEFAULT;\
+  MODULE_PARM(N, "1-" __MODULE_STRING(MAX_UINTS) "i");\
+  MODULE_PARM_DESC(N, D);
 */
 
 #define RX_DESC_MIN0     16
 #define RX_DESC_MAX0     128
 #define RX_DESC_DEF0     32
-DEVICE_PARAM(RxDescriptors0,"Number of receive descriptors0");
+DEVICE_PARAM(RxDescriptors0, "Number of receive descriptors0");
 
 #define RX_DESC_MIN1     16
 #define RX_DESC_MAX1     128
 #define RX_DESC_DEF1     32
-DEVICE_PARAM(RxDescriptors1,"Number of receive descriptors1");
+DEVICE_PARAM(RxDescriptors1, "Number of receive descriptors1");
 
 #define TX_DESC_MIN0     16
 #define TX_DESC_MAX0     128
 #define TX_DESC_DEF0     32
-DEVICE_PARAM(TxDescriptors0,"Number of transmit descriptors0");
+DEVICE_PARAM(TxDescriptors0, "Number of transmit descriptors0");
 
 #define TX_DESC_MIN1     16
 #define TX_DESC_MAX1     128
 #define TX_DESC_DEF1     64
-DEVICE_PARAM(TxDescriptors1,"Number of transmit descriptors1");
-
+DEVICE_PARAM(TxDescriptors1, "Number of transmit descriptors1");
 
 #define IP_ALIG_DEF     0
 /* IP_byte_align[] is used for IP header unsigned long byte aligned
    0: indicate the IP header won't be unsigned long byte aligned.(Default) .
    1: indicate the IP header will be unsigned long byte aligned.
-      In some environment, the IP header should be unsigned long byte aligned,
-      or the packet will be droped when we receive it. (eg: IPVS)
+   In some environment, the IP header should be unsigned long byte aligned,
+   or the packet will be droped when we receive it. (eg: IPVS)
 */
-DEVICE_PARAM(IP_byte_align,"Enable IP header dword aligned");
-
+DEVICE_PARAM(IP_byte_align, "Enable IP header dword aligned");
 
 #define INT_WORKS_DEF   20
 #define INT_WORKS_MIN   10
 #define INT_WORKS_MAX   64
 
-DEVICE_PARAM(int_works,"Number of packets per interrupt services");
+DEVICE_PARAM(int_works, "Number of packets per interrupt services");
 
 #define CHANNEL_MIN     1
 #define CHANNEL_MAX     14
@@ -151,7 +149,6 @@ DEVICE_PARAM(int_works,"Number of packets per interrupt services");
 
 DEVICE_PARAM(Channel, "Channel number");
 
-
 /* PreambleType[] is the preamble length used for transmit.
    0: indicate allows long preamble type
    1: indicate allows short preamble type
@@ -161,21 +158,18 @@ DEVICE_PARAM(Channel, "Channel number");
 
 DEVICE_PARAM(PreambleType, "Preamble Type");
 
-
 #define RTS_THRESH_MIN     512
 #define RTS_THRESH_MAX     2347
 #define RTS_THRESH_DEF     2347
 
 DEVICE_PARAM(RTSThreshold, "RTS threshold");
 
-
 #define FRAG_THRESH_MIN     256
 #define FRAG_THRESH_MAX     2346
 #define FRAG_THRESH_DEF     2346
 
 DEVICE_PARAM(FragThreshold, "Fragmentation threshold");
 
-
 #define DATA_RATE_MIN     0
 #define DATA_RATE_MAX     13
 #define DATA_RATE_DEF     13
@@ -190,10 +184,10 @@ DEVICE_PARAM(FragThreshold, "Fragmentation threshold");
    7: indicate 18 Mbps  0x24
    8: indicate 24 Mbps  0x30
    9: indicate 36 Mbps  0x48
-  10: indicate 48 Mbps  0x60
-  11: indicate 54 Mbps  0x6c
-  12: indicate 72 Mbps  0x90
-  13: indicate auto rate
+   10: indicate 48 Mbps  0x60
+   11: indicate 54 Mbps  0x6c
+   12: indicate 72 Mbps  0x90
+   13: indicate auto rate
 */
 
 DEVICE_PARAM(ConnectionRate, "Connection data rate");
@@ -208,7 +202,6 @@ DEVICE_PARAM(OPMode, "Infrastruct, adhoc, AP mode ");
    2: indicate AP mode used
 */
 
-
 /* PSMode[]
    0: indicate disable power saving mode
    1: indicate enable power saving mode
@@ -218,22 +211,18 @@ DEVICE_PARAM(OPMode, "Infrastruct, adhoc, AP mode ");
 
 DEVICE_PARAM(PSMode, "Power saving mode");
 
-
 #define SHORT_RETRY_MIN     0
 #define SHORT_RETRY_MAX     31
 #define SHORT_RETRY_DEF     8
 
-
 DEVICE_PARAM(ShortRetryLimit, "Short frame retry limits");
 
 #define LONG_RETRY_MIN     0
 #define LONG_RETRY_MAX     15
 #define LONG_RETRY_DEF     4
 
-
 DEVICE_PARAM(LongRetryLimit, "long frame retry limits");
 
-
 /* BasebandType[] baseband type selected
    0: indicate 802.11a type
    1: indicate 802.11b type
@@ -245,8 +234,6 @@ DEVICE_PARAM(LongRetryLimit, "long frame retry limits");
 
 DEVICE_PARAM(BasebandType, "baseband type");
 
-
-
 /* 80211hEnable[]
    0: indicate disable 802.11h
    1: indicate enable 802.11h
@@ -265,20 +252,18 @@ DEVICE_PARAM(b80211hEnable, "802.11h mode");
 
 DEVICE_PARAM(bDiversityANTEnable, "ANT diversity mode");
 
-
 //
 // Static vars definitions
 //
 
-
-static int          device_nics             =0;
-static PSDevice     pDevice_Infos           =NULL;
+static int          device_nics             = 0;
+static PSDevice     pDevice_Infos           = NULL;
 static struct net_device *root_device_dev = NULL;
 
-static CHIP_INFO chip_info_table[]= {
-    { VT3253,       "VIA Networking Solomon-A/B/G Wireless LAN Adapter ",
-        256, 1,     DEVICE_FLAGS_IP_ALIGN|DEVICE_FLAGS_TX_ALIGN },
-    {0,NULL}
+static CHIP_INFO chip_info_table[] = {
+	{ VT3253,       "VIA Networking Solomon-A/B/G Wireless LAN Adapter ",
+	  256, 1,     DEVICE_FLAGS_IP_ALIGN|DEVICE_FLAGS_TX_ALIGN },
+	{0, NULL}
 };
 
 DEFINE_PCI_DEVICE_TABLE(vt6655_pci_id_table) = {
@@ -288,17 +273,16 @@ DEFINE_PCI_DEVICE_TABLE(vt6655_pci_id_table) = {
 
 /*---------------------  Static Functions  --------------------------*/
 
-
 static int  vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent);
-static void vt6655_init_info(struct pci_dev* pcid, PSDevice* ppDevice, PCHIP_INFO);
+static void vt6655_init_info(struct pci_dev *pcid, PSDevice *ppDevice, PCHIP_INFO);
 static void device_free_info(PSDevice pDevice);
-static bool device_get_pci_info(PSDevice, struct pci_dev* pcid);
+static bool device_get_pci_info(PSDevice, struct pci_dev *pcid);
 static void device_print_info(PSDevice pDevice);
 static struct net_device_stats *device_get_stats(struct net_device *dev);
 static void device_init_diversity_timer(PSDevice pDevice);
 static int  device_open(struct net_device *dev);
 static int  device_xmit(struct sk_buff *skb, struct net_device *dev);
-static  irqreturn_t  device_intr(int irq,  void*dev_instance);
+static  irqreturn_t  device_intr(int irq,  void *dev_instance);
 static void device_set_multi(struct net_device *dev);
 static int  device_close(struct net_device *dev);
 static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd);
@@ -314,7 +298,6 @@ struct notifier_block device_notifier = {
 };
 #endif
 
-
 static void device_init_rd0_ring(PSDevice pDevice);
 static void device_init_rd1_ring(PSDevice pDevice);
 static void device_init_defrag_cb(PSDevice pDevice);
@@ -338,16 +321,13 @@ static void device_free_rd1_ring(PSDevice pDevice);
 static void device_free_rings(PSDevice pDevice);
 static void device_free_frag_buf(PSDevice pDevice);
 static int Config_FileGetParameter(unsigned char *string,
-		unsigned char *dest, unsigned char *source);
-
+				   unsigned char *dest, unsigned char *source);
 
 /*---------------------  Export Variables  --------------------------*/
 
 /*---------------------  Export Functions  --------------------------*/
 
-
-
-static char* get_chip_name(int chip_id)
+static char *get_chip_name(int chip_id)
 {
 	int i;
 	for (i = 0; chip_info_table[i].name != NULL; i++)
@@ -363,42 +343,41 @@ static void vt6655_remove(struct pci_dev *pcid)
 	if (pDevice == NULL)
 		return;
 	device_free_info(pDevice);
-
 }
 
 /*
-static void
-device_set_int_opt(int *opt, int val, int min, int max, int def,char* name,char* devname) {
-    if (val==-1)
-        *opt=def;
-    else if (val<min || val>max) {
-        DBG_PRT(MSG_LEVEL_INFO, KERN_NOTICE "%s: the value of parameter %s is invalid, the valid range is (%d-%d)\n" ,
-            devname,name, min,max);
-        *opt=def;
-    } else {
-        DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "%s: set value of parameter %s to %d\n",
-            devname, name, val);
-        *opt=val;
-    }
-}
+  static void
+  device_set_int_opt(int *opt, int val, int min, int max, int def,char* name,char* devname) {
+  if (val==-1)
+  *opt=def;
+  else if (val<min || val>max) {
+  DBG_PRT(MSG_LEVEL_INFO, KERN_NOTICE "%s: the value of parameter %s is invalid, the valid range is (%d-%d)\n" ,
+  devname,name, min,max);
+  *opt=def;
+  } else {
+  DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "%s: set value of parameter %s to %d\n",
+  devname, name, val);
+  *opt=val;
+  }
+  }
 
-static void
-device_set_bool_opt(unsigned int *opt, int val,bool def,u32 flag, char* name,char* devname) {
-    (*opt)&=(~flag);
-    if (val==-1)
-        *opt|=(def ? flag : 0);
-    else if (val<0 || val>1) {
-        DBG_PRT(MSG_LEVEL_INFO, KERN_NOTICE
-            "%s: the value of parameter %s is invalid, the valid range is (0-1)\n",devname,name);
-        *opt|=(def ? flag : 0);
-    } else {
-        DBG_PRT(MSG_LEVEL_INFO, KERN_NOTICE "%s: set parameter %s to %s\n",
-            devname,name , val ? "true" : "false");
-        *opt|=(val ? flag : 0);
-    }
-}
+  static void
+  device_set_bool_opt(unsigned int *opt, int val,bool def,u32 flag, char* name,char* devname) {
+  (*opt)&=(~flag);
+  if (val==-1)
+  *opt|=(def ? flag : 0);
+  else if (val<0 || val>1) {
+  DBG_PRT(MSG_LEVEL_INFO, KERN_NOTICE
+  "%s: the value of parameter %s is invalid, the valid range is (0-1)\n",devname,name);
+  *opt|=(def ? flag : 0);
+  } else {
+  DBG_PRT(MSG_LEVEL_INFO, KERN_NOTICE "%s: set parameter %s to %s\n",
+  devname,name , val ? "true" : "false");
+  *opt|=(val ? flag : 0);
+  }
+  }
 */
-static void device_get_options(PSDevice pDevice, int index, char* devname)
+static void device_get_options(PSDevice pDevice, int index, char *devname)
 {
 	POPTIONS pOpts = &(pDevice->sOpts);
 
@@ -425,472 +404,436 @@ static void device_get_options(PSDevice pDevice, int index, char* devname)
 
 static void
 device_set_options(PSDevice pDevice) {
-
-    unsigned char abyBroadcastAddr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
-    unsigned char abySNAP_RFC1042[ETH_ALEN] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00};
-    unsigned char abySNAP_Bridgetunnel[ETH_ALEN] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0xF8};
-
-
-    memcpy(pDevice->abyBroadcastAddr, abyBroadcastAddr, ETH_ALEN);
-    memcpy(pDevice->abySNAP_RFC1042, abySNAP_RFC1042, ETH_ALEN);
-    memcpy(pDevice->abySNAP_Bridgetunnel, abySNAP_Bridgetunnel, ETH_ALEN);
-
-    pDevice->uChannel = pDevice->sOpts.channel_num;
-    pDevice->wRTSThreshold = pDevice->sOpts.rts_thresh;
-    pDevice->wFragmentationThreshold = pDevice->sOpts.frag_thresh;
-    pDevice->byShortRetryLimit = pDevice->sOpts.short_retry;
-    pDevice->byLongRetryLimit = pDevice->sOpts.long_retry;
-    pDevice->wMaxTransmitMSDULifetime = DEFAULT_MSDU_LIFETIME;
-    pDevice->byShortPreamble = (pDevice->sOpts.flags & DEVICE_FLAGS_PREAMBLE_TYPE) ? 1 : 0;
-    pDevice->byOpMode = (pDevice->sOpts.flags & DEVICE_FLAGS_OP_MODE) ? 1 : 0;
-    pDevice->ePSMode = (pDevice->sOpts.flags & DEVICE_FLAGS_PS_MODE) ? 1 : 0;
-    pDevice->b11hEnable = (pDevice->sOpts.flags & DEVICE_FLAGS_80211h_MODE) ? 1 : 0;
-    pDevice->bDiversityRegCtlON = (pDevice->sOpts.flags & DEVICE_FLAGS_DiversityANT) ? 1 : 0;
-    pDevice->uConnectionRate = pDevice->sOpts.data_rate;
-    if (pDevice->uConnectionRate < RATE_AUTO) pDevice->bFixRate = true;
-    pDevice->byBBType = pDevice->sOpts.bbp_type;
-    pDevice->byPacketType = pDevice->byBBType;
+	unsigned char abyBroadcastAddr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	unsigned char abySNAP_RFC1042[ETH_ALEN] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00};
+	unsigned char abySNAP_Bridgetunnel[ETH_ALEN] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0xF8};
+
+	memcpy(pDevice->abyBroadcastAddr, abyBroadcastAddr, ETH_ALEN);
+	memcpy(pDevice->abySNAP_RFC1042, abySNAP_RFC1042, ETH_ALEN);
+	memcpy(pDevice->abySNAP_Bridgetunnel, abySNAP_Bridgetunnel, ETH_ALEN);
+
+	pDevice->uChannel = pDevice->sOpts.channel_num;
+	pDevice->wRTSThreshold = pDevice->sOpts.rts_thresh;
+	pDevice->wFragmentationThreshold = pDevice->sOpts.frag_thresh;
+	pDevice->byShortRetryLimit = pDevice->sOpts.short_retry;
+	pDevice->byLongRetryLimit = pDevice->sOpts.long_retry;
+	pDevice->wMaxTransmitMSDULifetime = DEFAULT_MSDU_LIFETIME;
+	pDevice->byShortPreamble = (pDevice->sOpts.flags & DEVICE_FLAGS_PREAMBLE_TYPE) ? 1 : 0;
+	pDevice->byOpMode = (pDevice->sOpts.flags & DEVICE_FLAGS_OP_MODE) ? 1 : 0;
+	pDevice->ePSMode = (pDevice->sOpts.flags & DEVICE_FLAGS_PS_MODE) ? 1 : 0;
+	pDevice->b11hEnable = (pDevice->sOpts.flags & DEVICE_FLAGS_80211h_MODE) ? 1 : 0;
+	pDevice->bDiversityRegCtlON = (pDevice->sOpts.flags & DEVICE_FLAGS_DiversityANT) ? 1 : 0;
+	pDevice->uConnectionRate = pDevice->sOpts.data_rate;
+	if (pDevice->uConnectionRate < RATE_AUTO) pDevice->bFixRate = true;
+	pDevice->byBBType = pDevice->sOpts.bbp_type;
+	pDevice->byPacketType = pDevice->byBBType;
 
 //PLICE_DEBUG->
 	pDevice->byAutoFBCtrl = AUTO_FB_0;
 	//pDevice->byAutoFBCtrl = AUTO_FB_1;
 //PLICE_DEBUG<-
-pDevice->bUpdateBBVGA = true;
-    pDevice->byFOETuning = 0;
-    pDevice->wCTSDuration = 0;
-    pDevice->byPreambleType = 0;
-
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" uChannel= %d\n",(int)pDevice->uChannel);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" byOpMode= %d\n",(int)pDevice->byOpMode);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" ePSMode= %d\n",(int)pDevice->ePSMode);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" wRTSThreshold= %d\n",(int)pDevice->wRTSThreshold);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" byShortRetryLimit= %d\n",(int)pDevice->byShortRetryLimit);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" byLongRetryLimit= %d\n",(int)pDevice->byLongRetryLimit);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" byPreambleType= %d\n",(int)pDevice->byPreambleType);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" byShortPreamble= %d\n",(int)pDevice->byShortPreamble);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" uConnectionRate= %d\n",(int)pDevice->uConnectionRate);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" byBBType= %d\n",(int)pDevice->byBBType);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" pDevice->b11hEnable= %d\n",(int)pDevice->b11hEnable);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" pDevice->bDiversityRegCtlON= %d\n",(int)pDevice->bDiversityRegCtlON);
+	pDevice->bUpdateBBVGA = true;
+	pDevice->byFOETuning = 0;
+	pDevice->wCTSDuration = 0;
+	pDevice->byPreambleType = 0;
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " uChannel= %d\n", (int)pDevice->uChannel);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " byOpMode= %d\n", (int)pDevice->byOpMode);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " ePSMode= %d\n", (int)pDevice->ePSMode);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " wRTSThreshold= %d\n", (int)pDevice->wRTSThreshold);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " byShortRetryLimit= %d\n", (int)pDevice->byShortRetryLimit);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " byLongRetryLimit= %d\n", (int)pDevice->byLongRetryLimit);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " byPreambleType= %d\n", (int)pDevice->byPreambleType);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " byShortPreamble= %d\n", (int)pDevice->byShortPreamble);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " uConnectionRate= %d\n", (int)pDevice->uConnectionRate);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " byBBType= %d\n", (int)pDevice->byBBType);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " pDevice->b11hEnable= %d\n", (int)pDevice->b11hEnable);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " pDevice->bDiversityRegCtlON= %d\n", (int)pDevice->bDiversityRegCtlON);
 }
 
-static void s_vCompleteCurrentMeasure (PSDevice pDevice, unsigned char byResult)
+static void s_vCompleteCurrentMeasure(PSDevice pDevice, unsigned char byResult)
 {
-    unsigned int ii;
-    unsigned long dwDuration = 0;
-    unsigned char byRPI0 = 0;
-
-    for(ii=1;ii<8;ii++) {
-        pDevice->dwRPIs[ii] *= 255;
-        dwDuration |= *((unsigned short *) (pDevice->pCurrMeasureEID->sReq.abyDuration));
-        dwDuration <<= 10;
-        pDevice->dwRPIs[ii] /= dwDuration;
-        pDevice->abyRPIs[ii] = (unsigned char) pDevice->dwRPIs[ii];
-        byRPI0 += pDevice->abyRPIs[ii];
-    }
-    pDevice->abyRPIs[0] = (0xFF - byRPI0);
-
-     if (pDevice->uNumOfMeasureEIDs == 0) {
-        VNTWIFIbMeasureReport(  pDevice->pMgmt,
-                                true,
-                                pDevice->pCurrMeasureEID,
-                                byResult,
-                                pDevice->byBasicMap,
-                                pDevice->byCCAFraction,
-                                pDevice->abyRPIs
-                                );
-    } else {
-        VNTWIFIbMeasureReport(  pDevice->pMgmt,
-                                false,
-                                pDevice->pCurrMeasureEID,
-                                byResult,
-                                pDevice->byBasicMap,
-                                pDevice->byCCAFraction,
-                                pDevice->abyRPIs
-                                );
-        CARDbStartMeasure (pDevice, pDevice->pCurrMeasureEID++, pDevice->uNumOfMeasureEIDs);
-    }
-
+	unsigned int ii;
+	unsigned long dwDuration = 0;
+	unsigned char byRPI0 = 0;
+
+	for (ii = 1; ii < 8; ii++) {
+		pDevice->dwRPIs[ii] *= 255;
+		dwDuration |= *((unsigned short *)(pDevice->pCurrMeasureEID->sReq.abyDuration));
+		dwDuration <<= 10;
+		pDevice->dwRPIs[ii] /= dwDuration;
+		pDevice->abyRPIs[ii] = (unsigned char)pDevice->dwRPIs[ii];
+		byRPI0 += pDevice->abyRPIs[ii];
+	}
+	pDevice->abyRPIs[0] = (0xFF - byRPI0);
+
+	if (pDevice->uNumOfMeasureEIDs == 0) {
+		VNTWIFIbMeasureReport(pDevice->pMgmt,
+				      true,
+				      pDevice->pCurrMeasureEID,
+				      byResult,
+				      pDevice->byBasicMap,
+				      pDevice->byCCAFraction,
+				      pDevice->abyRPIs
+			);
+	} else {
+		VNTWIFIbMeasureReport(pDevice->pMgmt,
+				      false,
+				      pDevice->pCurrMeasureEID,
+				      byResult,
+				      pDevice->byBasicMap,
+				      pDevice->byCCAFraction,
+				      pDevice->abyRPIs
+			);
+		CARDbStartMeasure(pDevice, pDevice->pCurrMeasureEID++, pDevice->uNumOfMeasureEIDs);
+	}
 }
 
-
-
 //
 // Initialisation of MAC & BBP registers
 //
 
 static void device_init_registers(PSDevice pDevice, DEVICE_INIT_TYPE InitType)
 {
-    unsigned int ii;
-    unsigned char byValue;
-    unsigned char byValue1;
-    unsigned char byCCKPwrdBm = 0;
-    unsigned char byOFDMPwrdBm = 0;
-    int zonetype=0;
-     PSMgmtObject    pMgmt = &(pDevice->sMgmtObj);
-    MACbShutdown(pDevice->PortOffset);
-    BBvSoftwareReset(pDevice->PortOffset);
-
-    if ((InitType == DEVICE_INIT_COLD) ||
-        (InitType == DEVICE_INIT_DXPL)) {
-        // Do MACbSoftwareReset in MACvInitialize
-        MACbSoftwareReset(pDevice->PortOffset);
-        // force CCK
-        pDevice->bCCK = true;
-        pDevice->bAES = false;
-        pDevice->bProtectMode = false;      //Only used in 11g type, sync with ERP IE
-        pDevice->bNonERPPresent = false;
-        pDevice->bBarkerPreambleMd = false;
-        pDevice->wCurrentRate = RATE_1M;
-        pDevice->byTopOFDMBasicRate = RATE_24M;
-        pDevice->byTopCCKBasicRate = RATE_1M;
-
-        pDevice->byRevId = 0;                   //Target to IF pin while programming to RF chip.
-
-        // init MAC
-        MACvInitialize(pDevice->PortOffset);
-
-        // Get Local ID
-        VNSvInPortB(pDevice->PortOffset + MAC_REG_LOCALID, &(pDevice->byLocalID));
-
-           spin_lock_irq(&pDevice->lock);
-	 SROMvReadAllContents(pDevice->PortOffset,pDevice->abyEEPROM);
-
-           spin_unlock_irq(&pDevice->lock);
-
-        // Get Channel range
-
-        pDevice->byMinChannel = 1;
-        pDevice->byMaxChannel = CB_MAX_CHANNEL;
-
-        // Get Antena
-        byValue = SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_ANTENNA);
-        if (byValue & EEP_ANTINV)
-            pDevice->bTxRxAntInv = true;
-        else
-            pDevice->bTxRxAntInv = false;
-#ifdef	PLICE_DEBUG
-	//printk("init_register:TxRxAntInv is %d,byValue is %d\n",pDevice->bTxRxAntInv,byValue);
-#endif
-
-        byValue &= (EEP_ANTENNA_AUX | EEP_ANTENNA_MAIN);
-        if (byValue == 0) // if not set default is All
-            byValue = (EEP_ANTENNA_AUX | EEP_ANTENNA_MAIN);
-#ifdef	PLICE_DEBUG
-	//printk("init_register:byValue is %d\n",byValue);
-#endif
-        pDevice->ulDiversityNValue = 100*260;//100*SROMbyReadEmbedded(pDevice->PortOffset, 0x51);
-        pDevice->ulDiversityMValue = 100*16;//SROMbyReadEmbedded(pDevice->PortOffset, 0x52);
-        pDevice->byTMax = 1;//SROMbyReadEmbedded(pDevice->PortOffset, 0x53);
-        pDevice->byTMax2 = 4;//SROMbyReadEmbedded(pDevice->PortOffset, 0x54);
-        pDevice->ulSQ3TH = 0;//(unsigned long) SROMbyReadEmbedded(pDevice->PortOffset, 0x55);
-        pDevice->byTMax3 = 64;//SROMbyReadEmbedded(pDevice->PortOffset, 0x56);
-
-        if (byValue == (EEP_ANTENNA_AUX | EEP_ANTENNA_MAIN)) {
-            pDevice->byAntennaCount = 2;
-            pDevice->byTxAntennaMode = ANT_B;
-            pDevice->dwTxAntennaSel = 1;
-            pDevice->dwRxAntennaSel = 1;
-            if (pDevice->bTxRxAntInv == true)
-                pDevice->byRxAntennaMode = ANT_A;
-            else
-                pDevice->byRxAntennaMode = ANT_B;
-                // chester for antenna
-byValue1 = SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_ANTENNA);
-          //  if (pDevice->bDiversityRegCtlON)
-          if((byValue1&0x08)==0)
-                pDevice->bDiversityEnable = false;//SROMbyReadEmbedded(pDevice->PortOffset, 0x50);
-            else
-                pDevice->bDiversityEnable = true;
-#ifdef	PLICE_DEBUG
-		//printk("aux |main antenna: RxAntennaMode is %d\n",pDevice->byRxAntennaMode);
-#endif
-	} else  {
-            pDevice->bDiversityEnable = false;
-            pDevice->byAntennaCount = 1;
-            pDevice->dwTxAntennaSel = 0;
-            pDevice->dwRxAntennaSel = 0;
-            if (byValue & EEP_ANTENNA_AUX) {
-                pDevice->byTxAntennaMode = ANT_A;
-                if (pDevice->bTxRxAntInv == true)
-                    pDevice->byRxAntennaMode = ANT_B;
-                else
-                    pDevice->byRxAntennaMode = ANT_A;
-            } else {
-                pDevice->byTxAntennaMode = ANT_B;
-                if (pDevice->bTxRxAntInv == true)
-                    pDevice->byRxAntennaMode = ANT_A;
-                else
-                    pDevice->byRxAntennaMode = ANT_B;
-            }
-        }
-#ifdef	PLICE_DEBUG
-	//printk("init registers: TxAntennaMode is %d\n",pDevice->byTxAntennaMode);
-#endif
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "bDiversityEnable=[%d],NValue=[%d],MValue=[%d],TMax=[%d],TMax2=[%d]\n",
-            pDevice->bDiversityEnable,(int)pDevice->ulDiversityNValue,(int)pDevice->ulDiversityMValue,pDevice->byTMax,pDevice->byTMax2);
-
-//#ifdef ZoneType_DefaultSetting
-//2008-8-4 <add> by chester
-//zonetype initial
- pDevice->byOriginalZonetype = pDevice->abyEEPROM[EEP_OFS_ZONETYPE];
- zonetype = Config_FileOperation(pDevice,false,NULL);
- if (zonetype >= 0) {         //read zonetype file ok!
-  if ((zonetype == 0)&&
-        (pDevice->abyEEPROM[EEP_OFS_ZONETYPE] !=0x00)){          //for USA
-    pDevice->abyEEPROM[EEP_OFS_ZONETYPE] = 0;
-    pDevice->abyEEPROM[EEP_OFS_MAXCHANNEL] = 0x0B;
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Init Zone Type :USA\n");
-  }
- else if((zonetype == 1)&&
- 	     (pDevice->abyEEPROM[EEP_OFS_ZONETYPE]!=0x01)){   //for Japan
-    pDevice->abyEEPROM[EEP_OFS_ZONETYPE] = 0x01;
-    pDevice->abyEEPROM[EEP_OFS_MAXCHANNEL] = 0x0D;
-  }
- else if((zonetype == 2)&&
- 	     (pDevice->abyEEPROM[EEP_OFS_ZONETYPE]!=0x02)){   //for Europe
-    pDevice->abyEEPROM[EEP_OFS_ZONETYPE] = 0x02;
-    pDevice->abyEEPROM[EEP_OFS_MAXCHANNEL] = 0x0D;
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Init Zone Type :Europe\n");
-  }
+	unsigned int ii;
+	unsigned char byValue;
+	unsigned char byValue1;
+	unsigned char byCCKPwrdBm = 0;
+	unsigned char byOFDMPwrdBm = 0;
+	int zonetype = 0;
+	PSMgmtObject    pMgmt = &(pDevice->sMgmtObj);
+	MACbShutdown(pDevice->PortOffset);
+	BBvSoftwareReset(pDevice->PortOffset);
+
+	if ((InitType == DEVICE_INIT_COLD) ||
+	    (InitType == DEVICE_INIT_DXPL)) {
+		// Do MACbSoftwareReset in MACvInitialize
+		MACbSoftwareReset(pDevice->PortOffset);
+		// force CCK
+		pDevice->bCCK = true;
+		pDevice->bAES = false;
+		pDevice->bProtectMode = false;      //Only used in 11g type, sync with ERP IE
+		pDevice->bNonERPPresent = false;
+		pDevice->bBarkerPreambleMd = false;
+		pDevice->wCurrentRate = RATE_1M;
+		pDevice->byTopOFDMBasicRate = RATE_24M;
+		pDevice->byTopCCKBasicRate = RATE_1M;
+
+		pDevice->byRevId = 0;                   //Target to IF pin while programming to RF chip.
+
+		// init MAC
+		MACvInitialize(pDevice->PortOffset);
+
+		// Get Local ID
+		VNSvInPortB(pDevice->PortOffset + MAC_REG_LOCALID, &(pDevice->byLocalID));
 
-else
-{
-   if(zonetype!=pDevice->abyEEPROM[EEP_OFS_ZONETYPE])
-      printk("zonetype in file[%02x] mismatch with in EEPROM[%02x]\n",zonetype,pDevice->abyEEPROM[EEP_OFS_ZONETYPE]);
-   else
-      printk("Read Zonetype file success,use default zonetype setting[%02x]\n",zonetype);
- }
- 	}
-  else
-    printk("Read Zonetype file fail,use default zonetype setting[%02x]\n",SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_ZONETYPE));
-
-        // Get RFType
-        pDevice->byRFType = SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_RFTYPE);
-
-        if ((pDevice->byRFType & RF_EMU) != 0) {
-            // force change RevID for VT3253 emu
-            pDevice->byRevId = 0x80;
-        }
-
-        pDevice->byRFType &= RF_MASK;
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->byRFType = %x\n", pDevice->byRFType);
-
-        if (pDevice->bZoneRegExist == false) {
-            pDevice->byZoneType = pDevice->abyEEPROM[EEP_OFS_ZONETYPE];
-        }
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->byZoneType = %x\n", pDevice->byZoneType);
-
-        //Init RF module
-        RFbInit(pDevice);
-
-        //Get Desire Power Value
-        pDevice->byCurPwr = 0xFF;
-        pDevice->byCCKPwr = SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_PWR_CCK);
-        pDevice->byOFDMPwrG = SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_PWR_OFDMG);
-        //byCCKPwrdBm = SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_CCK_PWR_dBm);
-
-	//byOFDMPwrdBm = SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_OFDM_PWR_dBm);
-//printk("CCKPwrdBm is 0x%x,byOFDMPwrdBm is 0x%x\n",byCCKPwrdBm,byOFDMPwrdBm);
-		// Load power Table
+		spin_lock_irq(&pDevice->lock);
+		SROMvReadAllContents(pDevice->PortOffset, pDevice->abyEEPROM);
 
+		spin_unlock_irq(&pDevice->lock);
 
-        for (ii=0;ii<CB_MAX_CHANNEL_24G;ii++) {
-            pDevice->abyCCKPwrTbl[ii+1] = SROMbyReadEmbedded(pDevice->PortOffset, (unsigned char)(ii + EEP_OFS_CCK_PWR_TBL));
-            if (pDevice->abyCCKPwrTbl[ii+1] == 0) {
-                pDevice->abyCCKPwrTbl[ii+1] = pDevice->byCCKPwr;
-            }
-            pDevice->abyOFDMPwrTbl[ii+1] = SROMbyReadEmbedded(pDevice->PortOffset, (unsigned char)(ii + EEP_OFS_OFDM_PWR_TBL));
-            if (pDevice->abyOFDMPwrTbl[ii+1] == 0) {
-                pDevice->abyOFDMPwrTbl[ii+1] = pDevice->byOFDMPwrG;
-            }
-            pDevice->abyCCKDefaultPwr[ii+1] = byCCKPwrdBm;
-            pDevice->abyOFDMDefaultPwr[ii+1] = byOFDMPwrdBm;
-        }
-		//2008-8-4 <add> by chester
-	  //recover 12,13 ,14channel for EUROPE by 11 channel
-          if(((pDevice->abyEEPROM[EEP_OFS_ZONETYPE] == ZoneType_Japan) ||
-	        (pDevice->abyEEPROM[EEP_OFS_ZONETYPE] == ZoneType_Europe))&&
-	     (pDevice->byOriginalZonetype == ZoneType_USA)) {
-	    for(ii=11;ii<14;ii++) {
-                pDevice->abyCCKPwrTbl[ii] = pDevice->abyCCKPwrTbl[10];
-	       pDevice->abyOFDMPwrTbl[ii] = pDevice->abyOFDMPwrTbl[10];
+		// Get Channel range
 
-	    }
-	  }
+		pDevice->byMinChannel = 1;
+		pDevice->byMaxChannel = CB_MAX_CHANNEL;
 
+		// Get Antena
+		byValue = SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_ANTENNA);
+		if (byValue & EEP_ANTINV)
+			pDevice->bTxRxAntInv = true;
+		else
+			pDevice->bTxRxAntInv = false;
+
+		byValue &= (EEP_ANTENNA_AUX | EEP_ANTENNA_MAIN);
+		if (byValue == 0) // if not set default is All
+			byValue = (EEP_ANTENNA_AUX | EEP_ANTENNA_MAIN);
+
+		pDevice->ulDiversityNValue = 100*260;//100*SROMbyReadEmbedded(pDevice->PortOffset, 0x51);
+		pDevice->ulDiversityMValue = 100*16;//SROMbyReadEmbedded(pDevice->PortOffset, 0x52);
+		pDevice->byTMax = 1;//SROMbyReadEmbedded(pDevice->PortOffset, 0x53);
+		pDevice->byTMax2 = 4;//SROMbyReadEmbedded(pDevice->PortOffset, 0x54);
+		pDevice->ulSQ3TH = 0;//(unsigned long) SROMbyReadEmbedded(pDevice->PortOffset, 0x55);
+		pDevice->byTMax3 = 64;//SROMbyReadEmbedded(pDevice->PortOffset, 0x56);
+
+		if (byValue == (EEP_ANTENNA_AUX | EEP_ANTENNA_MAIN)) {
+			pDevice->byAntennaCount = 2;
+			pDevice->byTxAntennaMode = ANT_B;
+			pDevice->dwTxAntennaSel = 1;
+			pDevice->dwRxAntennaSel = 1;
+			if (pDevice->bTxRxAntInv == true)
+				pDevice->byRxAntennaMode = ANT_A;
+			else
+				pDevice->byRxAntennaMode = ANT_B;
+			// chester for antenna
+			byValue1 = SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_ANTENNA);
+			if ((byValue1 & 0x08) == 0)
+				pDevice->bDiversityEnable = false;//SROMbyReadEmbedded(pDevice->PortOffset, 0x50);
+			else
+				pDevice->bDiversityEnable = true;
+		} else  {
+			pDevice->bDiversityEnable = false;
+			pDevice->byAntennaCount = 1;
+			pDevice->dwTxAntennaSel = 0;
+			pDevice->dwRxAntennaSel = 0;
+			if (byValue & EEP_ANTENNA_AUX) {
+				pDevice->byTxAntennaMode = ANT_A;
+				if (pDevice->bTxRxAntInv == true)
+					pDevice->byRxAntennaMode = ANT_B;
+				else
+					pDevice->byRxAntennaMode = ANT_A;
+			} else {
+				pDevice->byTxAntennaMode = ANT_B;
+				if (pDevice->bTxRxAntInv == true)
+					pDevice->byRxAntennaMode = ANT_A;
+				else
+					pDevice->byRxAntennaMode = ANT_B;
+			}
+		}
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "bDiversityEnable=[%d],NValue=[%d],MValue=[%d],TMax=[%d],TMax2=[%d]\n",
+			pDevice->bDiversityEnable, (int)pDevice->ulDiversityNValue, (int)pDevice->ulDiversityMValue, pDevice->byTMax, pDevice->byTMax2);
 
-        // Load OFDM A Power Table
-        for (ii=0;ii<CB_MAX_CHANNEL_5G;ii++) { //RobertYu:20041224, bug using CB_MAX_CHANNEL
-            pDevice->abyOFDMPwrTbl[ii+CB_MAX_CHANNEL_24G+1] = SROMbyReadEmbedded(pDevice->PortOffset, (unsigned char)(ii + EEP_OFS_OFDMA_PWR_TBL));
-            pDevice->abyOFDMDefaultPwr[ii+CB_MAX_CHANNEL_24G+1] = SROMbyReadEmbedded(pDevice->PortOffset, (unsigned char)(ii + EEP_OFS_OFDMA_PWR_dBm));
-        }
-        init_channel_table((void *)pDevice);
+//#ifdef ZoneType_DefaultSetting
+//2008-8-4 <add> by chester
+//zonetype initial
+		pDevice->byOriginalZonetype = pDevice->abyEEPROM[EEP_OFS_ZONETYPE];
+		zonetype = Config_FileOperation(pDevice, false, NULL);
+		if (zonetype >= 0) {         //read zonetype file ok!
+			if ((zonetype == 0) &&
+			    (pDevice->abyEEPROM[EEP_OFS_ZONETYPE] != 0x00)) {          //for USA
+				pDevice->abyEEPROM[EEP_OFS_ZONETYPE] = 0;
+				pDevice->abyEEPROM[EEP_OFS_MAXCHANNEL] = 0x0B;
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Init Zone Type :USA\n");
+			} else if ((zonetype == 1) &&
+				 (pDevice->abyEEPROM[EEP_OFS_ZONETYPE] != 0x01)) {   //for Japan
+				pDevice->abyEEPROM[EEP_OFS_ZONETYPE] = 0x01;
+				pDevice->abyEEPROM[EEP_OFS_MAXCHANNEL] = 0x0D;
+			} else if ((zonetype == 2) &&
+				 (pDevice->abyEEPROM[EEP_OFS_ZONETYPE] != 0x02)) {   //for Europe
+				pDevice->abyEEPROM[EEP_OFS_ZONETYPE] = 0x02;
+				pDevice->abyEEPROM[EEP_OFS_MAXCHANNEL] = 0x0D;
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Init Zone Type :Europe\n");
+			}
 
+			else {
+				if (zonetype != pDevice->abyEEPROM[EEP_OFS_ZONETYPE])
+					printk("zonetype in file[%02x] mismatch with in EEPROM[%02x]\n", zonetype, pDevice->abyEEPROM[EEP_OFS_ZONETYPE]);
+				else
+					printk("Read Zonetype file success,use default zonetype setting[%02x]\n", zonetype);
+			}
+		} else
+			printk("Read Zonetype file fail,use default zonetype setting[%02x]\n", SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_ZONETYPE));
 
-        if (pDevice->byLocalID > REV_ID_VT3253_B1) {
-            MACvSelectPage1(pDevice->PortOffset);
-            VNSvOutPortB(pDevice->PortOffset + MAC_REG_MSRCTL + 1, (MSRCTL1_TXPWR | MSRCTL1_CSAPAREN));
-            MACvSelectPage0(pDevice->PortOffset);
-        }
+		// Get RFType
+		pDevice->byRFType = SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_RFTYPE);
 
+		if ((pDevice->byRFType & RF_EMU) != 0) {
+			// force change RevID for VT3253 emu
+			pDevice->byRevId = 0x80;
+		}
 
-         // use relative tx timeout and 802.11i D4
-        MACvWordRegBitsOn(pDevice->PortOffset, MAC_REG_CFG, (CFG_TKIPOPT | CFG_NOTXTIMEOUT));
+		pDevice->byRFType &= RF_MASK;
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->byRFType = %x\n", pDevice->byRFType);
 
-        // set performance parameter by registry
-        MACvSetShortRetryLimit(pDevice->PortOffset, pDevice->byShortRetryLimit);
-        MACvSetLongRetryLimit(pDevice->PortOffset, pDevice->byLongRetryLimit);
+		if (pDevice->bZoneRegExist == false) {
+			pDevice->byZoneType = pDevice->abyEEPROM[EEP_OFS_ZONETYPE];
+		}
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->byZoneType = %x\n", pDevice->byZoneType);
 
-        // reset TSF counter
-        VNSvOutPortB(pDevice->PortOffset + MAC_REG_TFTCTL, TFTCTL_TSFCNTRST);
-        // enable TSF counter
-        VNSvOutPortB(pDevice->PortOffset + MAC_REG_TFTCTL, TFTCTL_TSFCNTREN);
+		//Init RF module
+		RFbInit(pDevice);
 
-        // initialize BBP registers
-        BBbVT3253Init(pDevice);
+		//Get Desire Power Value
+		pDevice->byCurPwr = 0xFF;
+		pDevice->byCCKPwr = SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_PWR_CCK);
+		pDevice->byOFDMPwrG = SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_PWR_OFDMG);
+		//byCCKPwrdBm = SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_CCK_PWR_dBm);
 
-        if (pDevice->bUpdateBBVGA) {
-            pDevice->byBBVGACurrent = pDevice->abyBBVGA[0];
-            pDevice->byBBVGANew = pDevice->byBBVGACurrent;
-            BBvSetVGAGainOffset(pDevice, pDevice->abyBBVGA[0]);
-        }
-#ifdef	PLICE_DEBUG
-	//printk("init registers:RxAntennaMode is %x,TxAntennaMode is %x\n",pDevice->byRxAntennaMode,pDevice->byTxAntennaMode);
-#endif
-        BBvSetRxAntennaMode(pDevice->PortOffset, pDevice->byRxAntennaMode);
-        BBvSetTxAntennaMode(pDevice->PortOffset, pDevice->byTxAntennaMode);
+		//byOFDMPwrdBm = SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_OFDM_PWR_dBm);
 
-        pDevice->byCurrentCh = 0;
+		// Load power Table
 
-        //pDevice->NetworkType = Ndis802_11Automode;
-        // Set BB and packet type at the same time.
-        // Set Short Slot Time, xIFS, and RSPINF.
-        if (pDevice->uConnectionRate == RATE_AUTO) {
-            pDevice->wCurrentRate = RATE_54M;
-        } else {
-            pDevice->wCurrentRate = (unsigned short)pDevice->uConnectionRate;
-        }
+		for (ii = 0; ii < CB_MAX_CHANNEL_24G; ii++) {
+			pDevice->abyCCKPwrTbl[ii + 1] = SROMbyReadEmbedded(pDevice->PortOffset, (unsigned char)(ii + EEP_OFS_CCK_PWR_TBL));
+			if (pDevice->abyCCKPwrTbl[ii + 1] == 0) {
+				pDevice->abyCCKPwrTbl[ii+1] = pDevice->byCCKPwr;
+			}
+			pDevice->abyOFDMPwrTbl[ii + 1] = SROMbyReadEmbedded(pDevice->PortOffset, (unsigned char)(ii + EEP_OFS_OFDM_PWR_TBL));
+			if (pDevice->abyOFDMPwrTbl[ii + 1] == 0) {
+				pDevice->abyOFDMPwrTbl[ii + 1] = pDevice->byOFDMPwrG;
+			}
+			pDevice->abyCCKDefaultPwr[ii + 1] = byCCKPwrdBm;
+			pDevice->abyOFDMDefaultPwr[ii + 1] = byOFDMPwrdBm;
+		}
+		//2008-8-4 <add> by chester
+		//recover 12,13 ,14channel for EUROPE by 11 channel
+		if (((pDevice->abyEEPROM[EEP_OFS_ZONETYPE] == ZoneType_Japan) ||
+		     (pDevice->abyEEPROM[EEP_OFS_ZONETYPE] == ZoneType_Europe)) &&
+		    (pDevice->byOriginalZonetype == ZoneType_USA)) {
+			for (ii = 11; ii < 14; ii++) {
+				pDevice->abyCCKPwrTbl[ii] = pDevice->abyCCKPwrTbl[10];
+				pDevice->abyOFDMPwrTbl[ii] = pDevice->abyOFDMPwrTbl[10];
 
-        // default G Mode
-        VNTWIFIbConfigPhyMode(pDevice->pMgmt, PHY_TYPE_11G);
-        VNTWIFIbConfigPhyMode(pDevice->pMgmt, PHY_TYPE_AUTO);
+			}
+		}
 
-        pDevice->bRadioOff = false;
+		// Load OFDM A Power Table
+		for (ii = 0; ii < CB_MAX_CHANNEL_5G; ii++) { //RobertYu:20041224, bug using CB_MAX_CHANNEL
+			pDevice->abyOFDMPwrTbl[ii + CB_MAX_CHANNEL_24G + 1] = SROMbyReadEmbedded(pDevice->PortOffset, (unsigned char)(ii + EEP_OFS_OFDMA_PWR_TBL));
+			pDevice->abyOFDMDefaultPwr[ii + CB_MAX_CHANNEL_24G + 1] = SROMbyReadEmbedded(pDevice->PortOffset, (unsigned char)(ii + EEP_OFS_OFDMA_PWR_dBm));
+		}
+		init_channel_table((void *)pDevice);
 
-        pDevice->byRadioCtl = SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_RADIOCTL);
-        pDevice->bHWRadioOff = false;
+		if (pDevice->byLocalID > REV_ID_VT3253_B1) {
+			MACvSelectPage1(pDevice->PortOffset);
+			VNSvOutPortB(pDevice->PortOffset + MAC_REG_MSRCTL + 1, (MSRCTL1_TXPWR | MSRCTL1_CSAPAREN));
+			MACvSelectPage0(pDevice->PortOffset);
+		}
 
-        if (pDevice->byRadioCtl & EEP_RADIOCTL_ENABLE) {
-            // Get GPIO
-            MACvGPIOIn(pDevice->PortOffset, &pDevice->byGPIO);
-//2008-4-14 <add> by chester for led issue
- #ifdef FOR_LED_ON_NOTEBOOK
-if (pDevice->byGPIO & GPIO0_DATA){pDevice->bHWRadioOff = true;}
-if ( !(pDevice->byGPIO & GPIO0_DATA)){pDevice->bHWRadioOff = false;}
-
-            }
-        if ( (pDevice->bRadioControlOff == true)) {
-            CARDbRadioPowerOff(pDevice);
-        }
-else  CARDbRadioPowerOn(pDevice);
-#else
-            if (((pDevice->byGPIO & GPIO0_DATA) && !(pDevice->byRadioCtl & EEP_RADIOCTL_INV)) ||
-                ( !(pDevice->byGPIO & GPIO0_DATA) && (pDevice->byRadioCtl & EEP_RADIOCTL_INV))) {
-                pDevice->bHWRadioOff = true;
-            }
-        }
-        if ((pDevice->bHWRadioOff == true) || (pDevice->bRadioControlOff == true)) {
-            CARDbRadioPowerOff(pDevice);
-        }
+		// use relative tx timeout and 802.11i D4
+		MACvWordRegBitsOn(pDevice->PortOffset, MAC_REG_CFG, (CFG_TKIPOPT | CFG_NOTXTIMEOUT));
 
-#endif
-    }
-            pMgmt->eScanType = WMAC_SCAN_PASSIVE;
-    // get Permanent network address
-    SROMvReadEtherAddress(pDevice->PortOffset, pDevice->abyCurrentNetAddr);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Network address = %pM\n",
-		pDevice->abyCurrentNetAddr);
+		// set performance parameter by registry
+		MACvSetShortRetryLimit(pDevice->PortOffset, pDevice->byShortRetryLimit);
+		MACvSetLongRetryLimit(pDevice->PortOffset, pDevice->byLongRetryLimit);
 
-    // reset Tx pointer
-    CARDvSafeResetRx(pDevice);
-    // reset Rx pointer
-    CARDvSafeResetTx(pDevice);
+		// reset TSF counter
+		VNSvOutPortB(pDevice->PortOffset + MAC_REG_TFTCTL, TFTCTL_TSFCNTRST);
+		// enable TSF counter
+		VNSvOutPortB(pDevice->PortOffset + MAC_REG_TFTCTL, TFTCTL_TSFCNTREN);
 
-    if (pDevice->byLocalID <= REV_ID_VT3253_A1) {
-        MACvRegBitsOn(pDevice->PortOffset, MAC_REG_RCR, RCR_WPAERR);
-    }
+		// initialize BBP registers
+		BBbVT3253Init(pDevice);
 
-    pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
+		if (pDevice->bUpdateBBVGA) {
+			pDevice->byBBVGACurrent = pDevice->abyBBVGA[0];
+			pDevice->byBBVGANew = pDevice->byBBVGACurrent;
+			BBvSetVGAGainOffset(pDevice, pDevice->abyBBVGA[0]);
+		}
+		BBvSetRxAntennaMode(pDevice->PortOffset, pDevice->byRxAntennaMode);
+		BBvSetTxAntennaMode(pDevice->PortOffset, pDevice->byTxAntennaMode);
 
-    // Turn On Rx DMA
-    MACvReceive0(pDevice->PortOffset);
-    MACvReceive1(pDevice->PortOffset);
+		pDevice->byCurrentCh = 0;
 
-    // start the adapter
-    MACvStart(pDevice->PortOffset);
+		//pDevice->NetworkType = Ndis802_11Automode;
+		// Set BB and packet type at the same time.
+		// Set Short Slot Time, xIFS, and RSPINF.
+		if (pDevice->uConnectionRate == RATE_AUTO) {
+			pDevice->wCurrentRate = RATE_54M;
+		} else {
+			pDevice->wCurrentRate = (unsigned short)pDevice->uConnectionRate;
+		}
 
-    netif_stop_queue(pDevice->dev);
+		// default G Mode
+		VNTWIFIbConfigPhyMode(pDevice->pMgmt, PHY_TYPE_11G);
+		VNTWIFIbConfigPhyMode(pDevice->pMgmt, PHY_TYPE_AUTO);
 
+		pDevice->bRadioOff = false;
 
-}
+		pDevice->byRadioCtl = SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_RADIOCTL);
+		pDevice->bHWRadioOff = false;
 
+		if (pDevice->byRadioCtl & EEP_RADIOCTL_ENABLE) {
+			// Get GPIO
+			MACvGPIOIn(pDevice->PortOffset, &pDevice->byGPIO);
+//2008-4-14 <add> by chester for led issue
+#ifdef FOR_LED_ON_NOTEBOOK
+			if (pDevice->byGPIO & GPIO0_DATA) { pDevice->bHWRadioOff = true; }
+			if (!(pDevice->byGPIO & GPIO0_DATA)) { pDevice->bHWRadioOff = false; }
 
+		}
+		if ((pDevice->bRadioControlOff == true)) {
+			CARDbRadioPowerOff(pDevice);
+		} else  CARDbRadioPowerOn(pDevice);
+#else
+		if (((pDevice->byGPIO & GPIO0_DATA) && !(pDevice->byRadioCtl & EEP_RADIOCTL_INV)) ||
+		    (!(pDevice->byGPIO & GPIO0_DATA) && (pDevice->byRadioCtl & EEP_RADIOCTL_INV))) {
+			pDevice->bHWRadioOff = true;
+		}
+	}
+	if ((pDevice->bHWRadioOff == true) || (pDevice->bRadioControlOff == true)) {
+		CARDbRadioPowerOff(pDevice);
+	}
 
-static void device_init_diversity_timer(PSDevice pDevice) {
+#endif
+}
+pMgmt->eScanType = WMAC_SCAN_PASSIVE;
+// get Permanent network address
+SROMvReadEtherAddress(pDevice->PortOffset, pDevice->abyCurrentNetAddr);
+DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Network address = %pM\n",
+	pDevice->abyCurrentNetAddr);
+
+// reset Tx pointer
+CARDvSafeResetRx(pDevice);
+// reset Rx pointer
+CARDvSafeResetTx(pDevice);
+
+if (pDevice->byLocalID <= REV_ID_VT3253_A1) {
+	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_RCR, RCR_WPAERR);
+}
 
-    init_timer(&pDevice->TimerSQ3Tmax1);
-    pDevice->TimerSQ3Tmax1.data = (unsigned long) pDevice;
-    pDevice->TimerSQ3Tmax1.function = (TimerFunction)TimerSQ3CallBack;
-    pDevice->TimerSQ3Tmax1.expires = RUN_AT(HZ);
+pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
 
-    init_timer(&pDevice->TimerSQ3Tmax2);
-    pDevice->TimerSQ3Tmax2.data = (unsigned long) pDevice;
-    pDevice->TimerSQ3Tmax2.function = (TimerFunction)TimerSQ3CallBack;
-    pDevice->TimerSQ3Tmax2.expires = RUN_AT(HZ);
+// Turn On Rx DMA
+MACvReceive0(pDevice->PortOffset);
+MACvReceive1(pDevice->PortOffset);
 
-    init_timer(&pDevice->TimerSQ3Tmax3);
-    pDevice->TimerSQ3Tmax3.data = (unsigned long) pDevice;
-    pDevice->TimerSQ3Tmax3.function = (TimerFunction)TimerState1CallBack;
-    pDevice->TimerSQ3Tmax3.expires = RUN_AT(HZ);
+// start the adapter
+MACvStart(pDevice->PortOffset);
 
-    return;
+netif_stop_queue(pDevice->dev);
 }
 
+static void device_init_diversity_timer(PSDevice pDevice) {
+	init_timer(&pDevice->TimerSQ3Tmax1);
+	pDevice->TimerSQ3Tmax1.data = (unsigned long) pDevice;
+	pDevice->TimerSQ3Tmax1.function = (TimerFunction)TimerSQ3CallBack;
+	pDevice->TimerSQ3Tmax1.expires = RUN_AT(HZ);
+
+	init_timer(&pDevice->TimerSQ3Tmax2);
+	pDevice->TimerSQ3Tmax2.data = (unsigned long) pDevice;
+	pDevice->TimerSQ3Tmax2.function = (TimerFunction)TimerSQ3CallBack;
+	pDevice->TimerSQ3Tmax2.expires = RUN_AT(HZ);
+
+	init_timer(&pDevice->TimerSQ3Tmax3);
+	pDevice->TimerSQ3Tmax3.data = (unsigned long) pDevice;
+	pDevice->TimerSQ3Tmax3.function = (TimerFunction)TimerState1CallBack;
+	pDevice->TimerSQ3Tmax3.expires = RUN_AT(HZ);
+
+	return;
+}
 
 static bool device_release_WPADEV(PSDevice pDevice)
 {
-  viawget_wpa_header *wpahdr;
-  int ii=0;
- // wait_queue_head_t	Set_wait;
-  //send device close to wpa_supplicnat layer
-    if (pDevice->bWPADEVUp==true) {
-                 wpahdr = (viawget_wpa_header *)pDevice->skb->data;
-                 wpahdr->type = VIAWGET_DEVICECLOSE_MSG;
-                 wpahdr->resp_ie_len = 0;
-                 wpahdr->req_ie_len = 0;
-                 skb_put(pDevice->skb, sizeof(viawget_wpa_header));
-                 pDevice->skb->dev = pDevice->wpadev;
-		 skb_reset_mac_header(pDevice->skb);
-                 pDevice->skb->pkt_type = PACKET_HOST;
-                 pDevice->skb->protocol = htons(ETH_P_802_2);
-                 memset(pDevice->skb->cb, 0, sizeof(pDevice->skb->cb));
-                 netif_rx(pDevice->skb);
-                 pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
-
- //wait release WPADEV
-              //    init_waitqueue_head(&Set_wait);
-              //    wait_event_timeout(Set_wait, ((pDevice->wpadev==NULL)&&(pDevice->skb == NULL)),5*HZ);    //1s wait
-              while((pDevice->bWPADEVUp==true)) {
-	        set_current_state(TASK_UNINTERRUPTIBLE);
-                 schedule_timeout (HZ/20);          //wait 50ms
-                 ii++;
-	        if(ii>20)
-		  break;
-              }
-           }
-    return true;
+	viawget_wpa_header *wpahdr;
+	int ii = 0;
+	// wait_queue_head_t	Set_wait;
+	//send device close to wpa_supplicnat layer
+	if (pDevice->bWPADEVUp == true) {
+		wpahdr = (viawget_wpa_header *)pDevice->skb->data;
+		wpahdr->type = VIAWGET_DEVICECLOSE_MSG;
+		wpahdr->resp_ie_len = 0;
+		wpahdr->req_ie_len = 0;
+		skb_put(pDevice->skb, sizeof(viawget_wpa_header));
+		pDevice->skb->dev = pDevice->wpadev;
+		skb_reset_mac_header(pDevice->skb);
+		pDevice->skb->pkt_type = PACKET_HOST;
+		pDevice->skb->protocol = htons(ETH_P_802_2);
+		memset(pDevice->skb->cb, 0, sizeof(pDevice->skb->cb));
+		netif_rx(pDevice->skb);
+		pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
+
+		//wait release WPADEV
+		//    init_waitqueue_head(&Set_wait);
+		//    wait_event_timeout(Set_wait, ((pDevice->wpadev==NULL)&&(pDevice->skb == NULL)),5*HZ);    //1s wait
+		while ((pDevice->bWPADEVUp == true)) {
+			set_current_state(TASK_UNINTERRUPTIBLE);
+			schedule_timeout(HZ / 20);          //wait 50ms
+			ii++;
+			if (ii > 20)
+				break;
+		}
+	}
+	return true;
 }
 
 static const struct net_device_ops device_netdev_ops = {
@@ -905,93 +848,88 @@ static const struct net_device_ops device_netdev_ops = {
 static int
 vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
 {
-    static bool bFirst = true;
-    struct net_device*  dev = NULL;
-    PCHIP_INFO  pChip_info = (PCHIP_INFO)ent->driver_data;
-    PSDevice    pDevice;
-    int         rc;
-    if (device_nics ++>= MAX_UINTS) {
-        printk(KERN_NOTICE DEVICE_NAME ": already found %d NICs\n", device_nics);
-        return -ENODEV;
-    }
-
-
-    dev = alloc_etherdev(sizeof(DEVICE_INFO));
-
-    pDevice = (PSDevice) netdev_priv(dev);
-
-    if (dev == NULL) {
-        printk(KERN_ERR DEVICE_NAME ": allocate net device failed \n");
-        return -ENOMEM;
-    }
-
-    // Chain it all together
-   // SET_MODULE_OWNER(dev);
-    SET_NETDEV_DEV(dev, &pcid->dev);
-
-    if (bFirst) {
-        printk(KERN_NOTICE "%s Ver. %s\n",DEVICE_FULL_DRV_NAM, DEVICE_VERSION);
-        printk(KERN_NOTICE "Copyright (c) 2003 VIA Networking Technologies, Inc.\n");
-        bFirst=false;
-    }
-
-    vt6655_init_info(pcid, &pDevice, pChip_info);
-    pDevice->dev = dev;
-    pDevice->next_module = root_device_dev;
-    root_device_dev = dev;
-
-    if (pci_enable_device(pcid)) {
-        device_free_info(pDevice);
-        return -ENODEV;
-    }
-    dev->irq = pcid->irq;
+	static bool bFirst = true;
+	struct net_device *dev = NULL;
+	PCHIP_INFO  pChip_info = (PCHIP_INFO)ent->driver_data;
+	PSDevice    pDevice;
+	int         rc;
+	if (device_nics++ >= MAX_UINTS) {
+		printk(KERN_NOTICE DEVICE_NAME ": already found %d NICs\n", device_nics);
+		return -ENODEV;
+	}
+
+	dev = alloc_etherdev(sizeof(DEVICE_INFO));
+
+	pDevice = (PSDevice) netdev_priv(dev);
+
+	if (dev == NULL) {
+		printk(KERN_ERR DEVICE_NAME ": allocate net device failed \n");
+		return -ENOMEM;
+	}
+
+	// Chain it all together
+	// SET_MODULE_OWNER(dev);
+	SET_NETDEV_DEV(dev, &pcid->dev);
+
+	if (bFirst) {
+		printk(KERN_NOTICE "%s Ver. %s\n", DEVICE_FULL_DRV_NAM, DEVICE_VERSION);
+		printk(KERN_NOTICE "Copyright (c) 2003 VIA Networking Technologies, Inc.\n");
+		bFirst = false;
+	}
+
+	vt6655_init_info(pcid, &pDevice, pChip_info);
+	pDevice->dev = dev;
+	pDevice->next_module = root_device_dev;
+	root_device_dev = dev;
+
+	if (pci_enable_device(pcid)) {
+		device_free_info(pDevice);
+		return -ENODEV;
+	}
+	dev->irq = pcid->irq;
 
 #ifdef	DEBUG
-	printk("Before get pci_info memaddr is %x\n",pDevice->memaddr);
+	printk("Before get pci_info memaddr is %x\n", pDevice->memaddr);
 #endif
-    if (device_get_pci_info(pDevice,pcid) == false) {
-        printk(KERN_ERR DEVICE_NAME ": Failed to find PCI device.\n");
-        device_free_info(pDevice);
-        return -ENODEV;
-    }
+	if (device_get_pci_info(pDevice, pcid) == false) {
+		printk(KERN_ERR DEVICE_NAME ": Failed to find PCI device.\n");
+		device_free_info(pDevice);
+		return -ENODEV;
+	}
 
 #if 1
 
 #ifdef	DEBUG
 
 	//pci_read_config_byte(pcid, PCI_BASE_ADDRESS_0, &pDevice->byRevId);
-	printk("after get pci_info memaddr is %x, io addr is %x,io_size is %d\n",pDevice->memaddr,pDevice->ioaddr,pDevice->io_size);
+	printk("after get pci_info memaddr is %x, io addr is %x,io_size is %d\n", pDevice->memaddr, pDevice->ioaddr, pDevice->io_size);
 	{
 		int i;
-		u32			bar,len;
+		u32 bar, len;
 		u32 address[] = {
-		PCI_BASE_ADDRESS_0,
-		PCI_BASE_ADDRESS_1,
-		PCI_BASE_ADDRESS_2,
-		PCI_BASE_ADDRESS_3,
-		PCI_BASE_ADDRESS_4,
-		PCI_BASE_ADDRESS_5,
-		0};
-		for (i=0;address[i];i++)
-		{
+			PCI_BASE_ADDRESS_0,
+			PCI_BASE_ADDRESS_1,
+			PCI_BASE_ADDRESS_2,
+			PCI_BASE_ADDRESS_3,
+			PCI_BASE_ADDRESS_4,
+			PCI_BASE_ADDRESS_5,
+			0};
+		for (i = 0; address[i]; i++) {
 			//pci_write_config_dword(pcid,address[i], 0xFFFFFFFF);
 			pci_read_config_dword(pcid, address[i], &bar);
-			printk("bar %d is %x\n",i,bar);
-			if (!bar)
-			{
-				printk("bar %d not implemented\n",i);
+			printk("bar %d is %x\n", i, bar);
+			if (!bar) {
+				printk("bar %d not implemented\n", i);
 				continue;
 			}
 			if (bar & PCI_BASE_ADDRESS_SPACE_IO) {
-			/* This is IO */
+				/* This is IO */
 
-			len = bar & (PCI_BASE_ADDRESS_IO_MASK & 0xFFFF);
-			len = len & ~(len - 1);
+				len = bar & (PCI_BASE_ADDRESS_IO_MASK & 0xFFFF);
+				len = len & ~(len - 1);
 
-			printk("IO space:  len in IO %x, BAR %d\n", len, i);
-			}
-			else
-			{
+				printk("IO space:  len in IO %x, BAR %d\n", len, i);
+			} else {
 				len = bar & 0xFFFFFFF0;
 				len = ~len + 1;
 
@@ -1001,813 +939,745 @@ vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
 	}
 #endif
 
-
 #endif
 
 #ifdef	DEBUG
-	//return  0  ;
+	//return  0;
 #endif
-    pDevice->PortOffset = (unsigned long)ioremap(pDevice->memaddr & PCI_BASE_ADDRESS_MEM_MASK, pDevice->io_size);
+	pDevice->PortOffset = (unsigned long)ioremap(pDevice->memaddr & PCI_BASE_ADDRESS_MEM_MASK, pDevice->io_size);
 	//pDevice->PortOffset = (unsigned long)ioremap(pDevice->ioaddr & PCI_BASE_ADDRESS_IO_MASK, pDevice->io_size);
 
-	if(pDevice->PortOffset == 0) {
-       printk(KERN_ERR DEVICE_NAME ": Failed to IO remapping ..\n");
-       device_free_info(pDevice);
-        return -ENODEV;
-    }
-
-
-
+	if (pDevice->PortOffset == 0) {
+		printk(KERN_ERR DEVICE_NAME ": Failed to IO remapping ..\n");
+		device_free_info(pDevice);
+		return -ENODEV;
+	}
 
-    rc = pci_request_regions(pcid, DEVICE_NAME);
-    if (rc) {
-        printk(KERN_ERR DEVICE_NAME ": Failed to find PCI device\n");
-        device_free_info(pDevice);
-        return -ENODEV;
-    }
+	rc = pci_request_regions(pcid, DEVICE_NAME);
+	if (rc) {
+		printk(KERN_ERR DEVICE_NAME ": Failed to find PCI device\n");
+		device_free_info(pDevice);
+		return -ENODEV;
+	}
 
-    dev->base_addr = pDevice->ioaddr;
+	dev->base_addr = pDevice->ioaddr;
 #ifdef	PLICE_DEBUG
-	unsigned char 	value;
+	unsigned char value;
 
 	VNSvInPortB(pDevice->PortOffset+0x4F, &value);
-	printk("Before write: value is %x\n",value);
+	printk("Before write: value is %x\n", value);
 	//VNSvInPortB(pDevice->PortOffset+0x3F, 0x00);
-	VNSvOutPortB(pDevice->PortOffset,value);
+	VNSvOutPortB(pDevice->PortOffset, value);
 	VNSvInPortB(pDevice->PortOffset+0x4F, &value);
-	printk("After write: value is %x\n",value);
+	printk("After write: value is %x\n", value);
 #endif
 
-
-
 #ifdef IO_MAP
-    pDevice->PortOffset = pDevice->ioaddr;
+	pDevice->PortOffset = pDevice->ioaddr;
 #endif
-    // do reset
-    if (!MACbSoftwareReset(pDevice->PortOffset)) {
-        printk(KERN_ERR DEVICE_NAME ": Failed to access MAC hardware..\n");
-        device_free_info(pDevice);
-        return -ENODEV;
-    }
-    // initial to reload eeprom
-    MACvInitialize(pDevice->PortOffset);
-    MACvReadEtherAddress(pDevice->PortOffset, dev->dev_addr);
-
-    device_get_options(pDevice, device_nics-1, dev->name);
-    device_set_options(pDevice);
-    //Mask out the options cannot be set to the chip
-    pDevice->sOpts.flags &= pChip_info->flags;
-
-    //Enable the chip specified capabilities
-    pDevice->flags = pDevice->sOpts.flags | (pChip_info->flags & 0xFF000000UL);
-    pDevice->tx_80211 = device_dma0_tx_80211;
-    pDevice->sMgmtObj.pAdapter = (void *)pDevice;
-    pDevice->pMgmt = &(pDevice->sMgmtObj);
-
-    dev->irq                = pcid->irq;
-    dev->netdev_ops         = &device_netdev_ops;
+	// do reset
+	if (!MACbSoftwareReset(pDevice->PortOffset)) {
+		printk(KERN_ERR DEVICE_NAME ": Failed to access MAC hardware..\n");
+		device_free_info(pDevice);
+		return -ENODEV;
+	}
+	// initial to reload eeprom
+	MACvInitialize(pDevice->PortOffset);
+	MACvReadEtherAddress(pDevice->PortOffset, dev->dev_addr);
 
-	dev->wireless_handlers = (struct iw_handler_def *)&iwctl_handler_def;
+	device_get_options(pDevice, device_nics-1, dev->name);
+	device_set_options(pDevice);
+	//Mask out the options cannot be set to the chip
+	pDevice->sOpts.flags &= pChip_info->flags;
+
+	//Enable the chip specified capabilities
+	pDevice->flags = pDevice->sOpts.flags | (pChip_info->flags & 0xFF000000UL);
+	pDevice->tx_80211 = device_dma0_tx_80211;
+	pDevice->sMgmtObj.pAdapter = (void *)pDevice;
+	pDevice->pMgmt = &(pDevice->sMgmtObj);
 
-    rc = register_netdev(dev);
-    if (rc)
-    {
-        printk(KERN_ERR DEVICE_NAME " Failed to register netdev\n");
-        device_free_info(pDevice);
-        return -ENODEV;
-    }
-    device_print_info(pDevice);
-    pci_set_drvdata(pcid, pDevice);
-    return 0;
+	dev->irq                = pcid->irq;
+	dev->netdev_ops         = &device_netdev_ops;
 
+	dev->wireless_handlers = (struct iw_handler_def *)&iwctl_handler_def;
+
+	rc = register_netdev(dev);
+	if (rc) {
+		printk(KERN_ERR DEVICE_NAME " Failed to register netdev\n");
+		device_free_info(pDevice);
+		return -ENODEV;
+	}
+	device_print_info(pDevice);
+	pci_set_drvdata(pcid, pDevice);
+	return 0;
 }
 
 static void device_print_info(PSDevice pDevice)
 {
-    struct net_device* dev=pDevice->dev;
+	struct net_device *dev = pDevice->dev;
 
-    DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "%s: %s\n",dev->name, get_chip_name(pDevice->chip_id));
-    DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "%s: MAC=%pM", dev->name, dev->dev_addr);
+	DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "%s: %s\n", dev->name, get_chip_name(pDevice->chip_id));
+	DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "%s: MAC=%pM", dev->name, dev->dev_addr);
 #ifdef IO_MAP
-    DBG_PRT(MSG_LEVEL_INFO, KERN_INFO" IO=0x%lx  ",(unsigned long) pDevice->ioaddr);
-    DBG_PRT(MSG_LEVEL_INFO, KERN_INFO" IRQ=%d \n", pDevice->dev->irq);
+	DBG_PRT(MSG_LEVEL_INFO, KERN_INFO " IO=0x%lx  ", (unsigned long)pDevice->ioaddr);
+	DBG_PRT(MSG_LEVEL_INFO, KERN_INFO " IRQ=%d \n", pDevice->dev->irq);
 #else
-    DBG_PRT(MSG_LEVEL_INFO, KERN_INFO" IO=0x%lx Mem=0x%lx ",
-		    (unsigned long) pDevice->ioaddr,(unsigned long) pDevice->PortOffset);
-    DBG_PRT(MSG_LEVEL_INFO, KERN_INFO" IRQ=%d \n", pDevice->dev->irq);
+	DBG_PRT(MSG_LEVEL_INFO, KERN_INFO " IO=0x%lx Mem=0x%lx ",
+		(unsigned long)pDevice->ioaddr, (unsigned long)pDevice->PortOffset);
+	DBG_PRT(MSG_LEVEL_INFO, KERN_INFO " IRQ=%d \n", pDevice->dev->irq);
 #endif
-
 }
 
-static void vt6655_init_info(struct pci_dev* pcid, PSDevice* ppDevice,
-    PCHIP_INFO pChip_info) {
-
-    PSDevice p;
+static void vt6655_init_info(struct pci_dev *pcid, PSDevice *ppDevice,
+			     PCHIP_INFO pChip_info) {
+	PSDevice p;
 
-    memset(*ppDevice,0,sizeof(DEVICE_INFO));
+	memset(*ppDevice, 0, sizeof(DEVICE_INFO));
 
-    if (pDevice_Infos == NULL) {
-        pDevice_Infos =*ppDevice;
-    }
-    else {
-        for (p=pDevice_Infos;p->next!=NULL;p=p->next)
-            do {} while (0);
-        p->next = *ppDevice;
-        (*ppDevice)->prev = p;
-    }
+	if (pDevice_Infos == NULL) {
+		pDevice_Infos = *ppDevice;
+	} else {
+		for (p = pDevice_Infos; p->next != NULL; p = p->next)
+			do {} while (0);
+		p->next = *ppDevice;
+		(*ppDevice)->prev = p;
+	}
 
-    (*ppDevice)->pcid = pcid;
-    (*ppDevice)->chip_id = pChip_info->chip_id;
-    (*ppDevice)->io_size = pChip_info->io_size;
-    (*ppDevice)->nTxQueues = pChip_info->nTxQueue;
-    (*ppDevice)->multicast_limit =32;
+	(*ppDevice)->pcid = pcid;
+	(*ppDevice)->chip_id = pChip_info->chip_id;
+	(*ppDevice)->io_size = pChip_info->io_size;
+	(*ppDevice)->nTxQueues = pChip_info->nTxQueue;
+	(*ppDevice)->multicast_limit = 32;
 
-    spin_lock_init(&((*ppDevice)->lock));
+	spin_lock_init(&((*ppDevice)->lock));
 }
 
-static bool device_get_pci_info(PSDevice pDevice, struct pci_dev* pcid) {
-
-    u16 pci_cmd;
-    u8  b;
-    unsigned int cis_addr;
+static bool device_get_pci_info(PSDevice pDevice, struct pci_dev *pcid) {
+	u16 pci_cmd;
+	u8  b;
+	unsigned int cis_addr;
 #ifdef	PLICE_DEBUG
 	unsigned char pci_config[256];
-	unsigned char 	value =0x00;
-	int		ii,j;
-	u16	max_lat=0x0000;
-	memset(pci_config,0x00,256);
+	unsigned char value = 0x00;
+	int		ii, j;
+	u16	max_lat = 0x0000;
+	memset(pci_config, 0x00, 256);
 #endif
 
-    pci_read_config_byte(pcid, PCI_REVISION_ID, &pDevice->byRevId);
-    pci_read_config_word(pcid, PCI_SUBSYSTEM_ID,&pDevice->SubSystemID);
-    pci_read_config_word(pcid, PCI_SUBSYSTEM_VENDOR_ID, &pDevice->SubVendorID);
-    pci_read_config_word(pcid, PCI_COMMAND, (u16 *) & (pci_cmd));
+	pci_read_config_byte(pcid, PCI_REVISION_ID, &pDevice->byRevId);
+	pci_read_config_word(pcid, PCI_SUBSYSTEM_ID, &pDevice->SubSystemID);
+	pci_read_config_word(pcid, PCI_SUBSYSTEM_VENDOR_ID, &pDevice->SubVendorID);
+	pci_read_config_word(pcid, PCI_COMMAND, (u16 *)&(pci_cmd));
 
-    pci_set_master(pcid);
+	pci_set_master(pcid);
 
-    pDevice->memaddr = pci_resource_start(pcid,0);
-    pDevice->ioaddr = pci_resource_start(pcid,1);
+	pDevice->memaddr = pci_resource_start(pcid, 0);
+	pDevice->ioaddr = pci_resource_start(pcid, 1);
 
 #ifdef	DEBUG
 //	pDevice->ioaddr = pci_resource_start(pcid, 0);
 //	pDevice->memaddr = pci_resource_start(pcid,1);
 #endif
 
-    cis_addr = pci_resource_start(pcid,2);
+	cis_addr = pci_resource_start(pcid, 2);
 
-    pDevice->pcid = pcid;
+	pDevice->pcid = pcid;
 
-    pci_read_config_byte(pcid, PCI_COMMAND, &b);
-    pci_write_config_byte(pcid, PCI_COMMAND, (b|PCI_COMMAND_MASTER));
+	pci_read_config_byte(pcid, PCI_COMMAND, &b);
+	pci_write_config_byte(pcid, PCI_COMMAND, (b|PCI_COMMAND_MASTER));
 
 #ifdef	PLICE_DEBUG
-   	//pci_read_config_word(pcid,PCI_MAX_LAT,&max_lat);
-	//printk("max lat is %x,SubSystemID is %x\n",max_lat,pDevice->SubSystemID);
+	//pci_read_config_word(pcid,PCI_MAX_LAT,&max_lat);
 	//for (ii=0;ii<0xFF;ii++)
 	//pci_read_config_word(pcid,PCI_MAX_LAT,&max_lat);
 	//max_lat  = 0x20;
 	//pci_write_config_word(pcid,PCI_MAX_LAT,max_lat);
 	//pci_read_config_word(pcid,PCI_MAX_LAT,&max_lat);
-	//printk("max lat is %x\n",max_lat);
 
-	for (ii=0;ii<0xFF;ii++)
-	{
-		pci_read_config_byte(pcid,ii,&value);
+	for (ii = 0; ii < 0xFF; ii++) {
+		pci_read_config_byte(pcid, ii, &value);
 		pci_config[ii] = value;
 	}
-	for (ii=0,j=1;ii<0x100;ii++,j++)
-	{
-		if (j %16 == 0)
-		{
-			printk("%x:",pci_config[ii]);
+	for (ii = 0, j = 1; ii < 0x100; ii++, j++) {
+		if (j % 16 == 0) {
+			printk("%x:", pci_config[ii]);
 			printk("\n");
-		}
-		else
-		{
-			printk("%x:",pci_config[ii]);
+		} else {
+			printk("%x:", pci_config[ii]);
 		}
 	}
 #endif
-    return true;
+	return true;
 }
 
 static void device_free_info(PSDevice pDevice) {
-    PSDevice         ptr;
-    struct net_device*  dev=pDevice->dev;
+	PSDevice         ptr;
+	struct net_device *dev = pDevice->dev;
 
-    ASSERT(pDevice);
+	ASSERT(pDevice);
 //2008-0714-01<Add>by chester
-device_release_WPADEV(pDevice);
+	device_release_WPADEV(pDevice);
 
 //2008-07-21-01<Add>by MikeLiu
 //unregister wpadev
-   if(wpa_set_wpadev(pDevice, 0)!=0)
-     printk("unregister wpadev fail?\n");
-
-    if (pDevice_Infos==NULL)
-        return;
-
-    for (ptr=pDevice_Infos;ptr && (ptr!=pDevice);ptr=ptr->next)
-            do {} while (0);
-
-    if (ptr==pDevice) {
-        if (ptr==pDevice_Infos)
-            pDevice_Infos=ptr->next;
-        else
-            ptr->prev->next=ptr->next;
-    }
-    else {
-        DBG_PRT(MSG_LEVEL_ERR, KERN_ERR "info struct not found\n");
-        return;
-    }
+	if (wpa_set_wpadev(pDevice, 0) != 0)
+		printk("unregister wpadev fail?\n");
+
+	if (pDevice_Infos == NULL)
+		return;
+
+	for (ptr = pDevice_Infos; ptr && (ptr != pDevice); ptr = ptr->next)
+		do {} while (0);
+
+	if (ptr == pDevice) {
+		if (ptr == pDevice_Infos)
+			pDevice_Infos = ptr->next;
+		else
+			ptr->prev->next = ptr->next;
+	} else {
+		DBG_PRT(MSG_LEVEL_ERR, KERN_ERR "info struct not found\n");
+		return;
+	}
 #ifdef HOSTAP
-    if (dev)
-        vt6655_hostap_set_hostapd(pDevice, 0, 0);
+	if (dev)
+		vt6655_hostap_set_hostapd(pDevice, 0, 0);
 #endif
-    if (dev)
-        unregister_netdev(dev);
+	if (dev)
+		unregister_netdev(dev);
 
-    if (pDevice->PortOffset)
-        iounmap((void *)pDevice->PortOffset);
+	if (pDevice->PortOffset)
+		iounmap((void *)pDevice->PortOffset);
 
-    if (pDevice->pcid)
-        pci_release_regions(pDevice->pcid);
-    if (dev)
-        free_netdev(dev);
+	if (pDevice->pcid)
+		pci_release_regions(pDevice->pcid);
+	if (dev)
+		free_netdev(dev);
 
-    if (pDevice->pcid) {
-        pci_set_drvdata(pDevice->pcid,NULL);
-    }
+	if (pDevice->pcid) {
+		pci_set_drvdata(pDevice->pcid, NULL);
+	}
 }
 
 static bool device_init_rings(PSDevice pDevice) {
-    void*   vir_pool;
-
-
-    /*allocate all RD/TD rings a single pool*/
-    vir_pool = pci_alloc_consistent(pDevice->pcid,
-                    pDevice->sOpts.nRxDescs0 * sizeof(SRxDesc) +
-                    pDevice->sOpts.nRxDescs1 * sizeof(SRxDesc) +
-                    pDevice->sOpts.nTxDescs[0] * sizeof(STxDesc) +
-                    pDevice->sOpts.nTxDescs[1] * sizeof(STxDesc),
-                    &pDevice->pool_dma);
-
-    if (vir_pool == NULL) {
-        DBG_PRT(MSG_LEVEL_ERR,KERN_ERR "%s : allocate desc dma memory failed\n", pDevice->dev->name);
-        return false;
-    }
-
-    memset(vir_pool, 0,
-            pDevice->sOpts.nRxDescs0 * sizeof(SRxDesc) +
-            pDevice->sOpts.nRxDescs1 * sizeof(SRxDesc) +
-            pDevice->sOpts.nTxDescs[0] * sizeof(STxDesc) +
-            pDevice->sOpts.nTxDescs[1] * sizeof(STxDesc)
-          );
-
-    pDevice->aRD0Ring = vir_pool;
-    pDevice->aRD1Ring = vir_pool +
-                        pDevice->sOpts.nRxDescs0 * sizeof(SRxDesc);
-
-
-    pDevice->rd0_pool_dma = pDevice->pool_dma;
-    pDevice->rd1_pool_dma = pDevice->rd0_pool_dma +
-                            pDevice->sOpts.nRxDescs0 * sizeof(SRxDesc);
-
-    pDevice->tx0_bufs = pci_alloc_consistent(pDevice->pcid,
-                    pDevice->sOpts.nTxDescs[0] * PKT_BUF_SZ +
-                    pDevice->sOpts.nTxDescs[1] * PKT_BUF_SZ +
-                    CB_BEACON_BUF_SIZE +
-                    CB_MAX_BUF_SIZE,
-                    &pDevice->tx_bufs_dma0);
-
-    if (pDevice->tx0_bufs == NULL) {
-        DBG_PRT(MSG_LEVEL_ERR,KERN_ERR "%s: allocate buf dma memory failed\n", pDevice->dev->name);
-        pci_free_consistent(pDevice->pcid,
-            pDevice->sOpts.nRxDescs0 * sizeof(SRxDesc) +
-            pDevice->sOpts.nRxDescs1 * sizeof(SRxDesc) +
-            pDevice->sOpts.nTxDescs[0] * sizeof(STxDesc) +
-            pDevice->sOpts.nTxDescs[1] * sizeof(STxDesc),
-            vir_pool, pDevice->pool_dma
-            );
-        return false;
-    }
-
-    memset(pDevice->tx0_bufs, 0,
-           pDevice->sOpts.nTxDescs[0] * PKT_BUF_SZ +
-           pDevice->sOpts.nTxDescs[1] * PKT_BUF_SZ +
-           CB_BEACON_BUF_SIZE +
-           CB_MAX_BUF_SIZE
-          );
-
-    pDevice->td0_pool_dma = pDevice->rd1_pool_dma +
-            pDevice->sOpts.nRxDescs1 * sizeof(SRxDesc);
-
-    pDevice->td1_pool_dma = pDevice->td0_pool_dma +
-            pDevice->sOpts.nTxDescs[0] * sizeof(STxDesc);
-
+	void *vir_pool;
+
+	/*allocate all RD/TD rings a single pool*/
+	vir_pool = pci_alloc_consistent(pDevice->pcid,
+					pDevice->sOpts.nRxDescs0 * sizeof(SRxDesc) +
+					pDevice->sOpts.nRxDescs1 * sizeof(SRxDesc) +
+					pDevice->sOpts.nTxDescs[0] * sizeof(STxDesc) +
+					pDevice->sOpts.nTxDescs[1] * sizeof(STxDesc),
+					&pDevice->pool_dma);
+
+	if (vir_pool == NULL) {
+		DBG_PRT(MSG_LEVEL_ERR, KERN_ERR "%s : allocate desc dma memory failed\n", pDevice->dev->name);
+		return false;
+	}
 
-    // vir_pool: pvoid type
-    pDevice->apTD0Rings = vir_pool
-                          + pDevice->sOpts.nRxDescs0 * sizeof(SRxDesc)
-                          + pDevice->sOpts.nRxDescs1 * sizeof(SRxDesc);
+	memset(vir_pool, 0,
+	       pDevice->sOpts.nRxDescs0 * sizeof(SRxDesc) +
+	       pDevice->sOpts.nRxDescs1 * sizeof(SRxDesc) +
+	       pDevice->sOpts.nTxDescs[0] * sizeof(STxDesc) +
+	       pDevice->sOpts.nTxDescs[1] * sizeof(STxDesc)
+		);
+
+	pDevice->aRD0Ring = vir_pool;
+	pDevice->aRD1Ring = vir_pool +
+		pDevice->sOpts.nRxDescs0 * sizeof(SRxDesc);
+
+	pDevice->rd0_pool_dma = pDevice->pool_dma;
+	pDevice->rd1_pool_dma = pDevice->rd0_pool_dma +
+		pDevice->sOpts.nRxDescs0 * sizeof(SRxDesc);
+
+	pDevice->tx0_bufs = pci_alloc_consistent(pDevice->pcid,
+						 pDevice->sOpts.nTxDescs[0] * PKT_BUF_SZ +
+						 pDevice->sOpts.nTxDescs[1] * PKT_BUF_SZ +
+						 CB_BEACON_BUF_SIZE +
+						 CB_MAX_BUF_SIZE,
+						 &pDevice->tx_bufs_dma0);
+
+	if (pDevice->tx0_bufs == NULL) {
+		DBG_PRT(MSG_LEVEL_ERR, KERN_ERR "%s: allocate buf dma memory failed\n", pDevice->dev->name);
+		pci_free_consistent(pDevice->pcid,
+				    pDevice->sOpts.nRxDescs0 * sizeof(SRxDesc) +
+				    pDevice->sOpts.nRxDescs1 * sizeof(SRxDesc) +
+				    pDevice->sOpts.nTxDescs[0] * sizeof(STxDesc) +
+				    pDevice->sOpts.nTxDescs[1] * sizeof(STxDesc),
+				    vir_pool, pDevice->pool_dma
+			);
+		return false;
+	}
 
-    pDevice->apTD1Rings = vir_pool
-            + pDevice->sOpts.nRxDescs0 * sizeof(SRxDesc)
-            + pDevice->sOpts.nRxDescs1 * sizeof(SRxDesc)
-            + pDevice->sOpts.nTxDescs[0] * sizeof(STxDesc);
+	memset(pDevice->tx0_bufs, 0,
+	       pDevice->sOpts.nTxDescs[0] * PKT_BUF_SZ +
+	       pDevice->sOpts.nTxDescs[1] * PKT_BUF_SZ +
+	       CB_BEACON_BUF_SIZE +
+	       CB_MAX_BUF_SIZE
+		);
 
+	pDevice->td0_pool_dma = pDevice->rd1_pool_dma +
+		pDevice->sOpts.nRxDescs1 * sizeof(SRxDesc);
 
-    pDevice->tx1_bufs = pDevice->tx0_bufs +
-            pDevice->sOpts.nTxDescs[0] * PKT_BUF_SZ;
+	pDevice->td1_pool_dma = pDevice->td0_pool_dma +
+		pDevice->sOpts.nTxDescs[0] * sizeof(STxDesc);
 
+	// vir_pool: pvoid type
+	pDevice->apTD0Rings = vir_pool
+		+ pDevice->sOpts.nRxDescs0 * sizeof(SRxDesc)
+		+ pDevice->sOpts.nRxDescs1 * sizeof(SRxDesc);
 
-    pDevice->tx_beacon_bufs = pDevice->tx1_bufs +
-            pDevice->sOpts.nTxDescs[1] * PKT_BUF_SZ;
+	pDevice->apTD1Rings = vir_pool
+		+ pDevice->sOpts.nRxDescs0 * sizeof(SRxDesc)
+		+ pDevice->sOpts.nRxDescs1 * sizeof(SRxDesc)
+		+ pDevice->sOpts.nTxDescs[0] * sizeof(STxDesc);
 
-    pDevice->pbyTmpBuff = pDevice->tx_beacon_bufs +
-            CB_BEACON_BUF_SIZE;
+	pDevice->tx1_bufs = pDevice->tx0_bufs +
+		pDevice->sOpts.nTxDescs[0] * PKT_BUF_SZ;
 
-    pDevice->tx_bufs_dma1 = pDevice->tx_bufs_dma0 +
-            pDevice->sOpts.nTxDescs[0] * PKT_BUF_SZ;
+	pDevice->tx_beacon_bufs = pDevice->tx1_bufs +
+		pDevice->sOpts.nTxDescs[1] * PKT_BUF_SZ;
 
+	pDevice->pbyTmpBuff = pDevice->tx_beacon_bufs +
+		CB_BEACON_BUF_SIZE;
 
-    pDevice->tx_beacon_dma = pDevice->tx_bufs_dma1 +
-            pDevice->sOpts.nTxDescs[1] * PKT_BUF_SZ;
+	pDevice->tx_bufs_dma1 = pDevice->tx_bufs_dma0 +
+		pDevice->sOpts.nTxDescs[0] * PKT_BUF_SZ;
 
+	pDevice->tx_beacon_dma = pDevice->tx_bufs_dma1 +
+		pDevice->sOpts.nTxDescs[1] * PKT_BUF_SZ;
 
-    return true;
+	return true;
 }
 
 static void device_free_rings(PSDevice pDevice) {
-
-    pci_free_consistent(pDevice->pcid,
-            pDevice->sOpts.nRxDescs0 * sizeof(SRxDesc) +
-            pDevice->sOpts.nRxDescs1 * sizeof(SRxDesc) +
-            pDevice->sOpts.nTxDescs[0] * sizeof(STxDesc) +
-            pDevice->sOpts.nTxDescs[1] * sizeof(STxDesc)
-            ,
-            pDevice->aRD0Ring, pDevice->pool_dma
-        );
-
-    if (pDevice->tx0_bufs)
-        pci_free_consistent(pDevice->pcid,
-           pDevice->sOpts.nTxDescs[0] * PKT_BUF_SZ +
-           pDevice->sOpts.nTxDescs[1] * PKT_BUF_SZ +
-           CB_BEACON_BUF_SIZE +
-           CB_MAX_BUF_SIZE,
-           pDevice->tx0_bufs, pDevice->tx_bufs_dma0
-        );
+	pci_free_consistent(pDevice->pcid,
+			    pDevice->sOpts.nRxDescs0 * sizeof(SRxDesc) +
+			    pDevice->sOpts.nRxDescs1 * sizeof(SRxDesc) +
+			    pDevice->sOpts.nTxDescs[0] * sizeof(STxDesc) +
+			    pDevice->sOpts.nTxDescs[1] * sizeof(STxDesc)
+			    ,
+			    pDevice->aRD0Ring, pDevice->pool_dma
+		);
+
+	if (pDevice->tx0_bufs)
+		pci_free_consistent(pDevice->pcid,
+				    pDevice->sOpts.nTxDescs[0] * PKT_BUF_SZ +
+				    pDevice->sOpts.nTxDescs[1] * PKT_BUF_SZ +
+				    CB_BEACON_BUF_SIZE +
+				    CB_MAX_BUF_SIZE,
+				    pDevice->tx0_bufs, pDevice->tx_bufs_dma0
+			);
 }
 
 static void device_init_rd0_ring(PSDevice pDevice) {
-    int i;
-    dma_addr_t      curr = pDevice->rd0_pool_dma;
-    PSRxDesc        pDesc;
-
-    /* Init the RD0 ring entries */
-    for (i = 0; i < pDevice->sOpts.nRxDescs0; i ++, curr += sizeof(SRxDesc)) {
-        pDesc = &(pDevice->aRD0Ring[i]);
-        pDesc->pRDInfo = alloc_rd_info();
-        ASSERT(pDesc->pRDInfo);
-        if (!device_alloc_rx_buf(pDevice, pDesc)) {
-            DBG_PRT(MSG_LEVEL_ERR,KERN_ERR "%s: can not alloc rx bufs\n",
-            pDevice->dev->name);
-        }
-        pDesc->next = &(pDevice->aRD0Ring[(i+1) % pDevice->sOpts.nRxDescs0]);
-        pDesc->pRDInfo->curr_desc = cpu_to_le32(curr);
-        pDesc->next_desc = cpu_to_le32(curr + sizeof(SRxDesc));
-    }
-
-    if (i > 0)
-        pDevice->aRD0Ring[i-1].next_desc = cpu_to_le32(pDevice->rd0_pool_dma);
-    pDevice->pCurrRD[0] = &(pDevice->aRD0Ring[0]);
-}
+	int i;
+	dma_addr_t      curr = pDevice->rd0_pool_dma;
+	PSRxDesc        pDesc;
+
+	/* Init the RD0 ring entries */
+	for (i = 0; i < pDevice->sOpts.nRxDescs0; i ++, curr += sizeof(SRxDesc)) {
+		pDesc = &(pDevice->aRD0Ring[i]);
+		pDesc->pRDInfo = alloc_rd_info();
+		ASSERT(pDesc->pRDInfo);
+		if (!device_alloc_rx_buf(pDevice, pDesc)) {
+			DBG_PRT(MSG_LEVEL_ERR, KERN_ERR "%s: can not alloc rx bufs\n",
+				pDevice->dev->name);
+		}
+		pDesc->next = &(pDevice->aRD0Ring[(i+1) % pDevice->sOpts.nRxDescs0]);
+		pDesc->pRDInfo->curr_desc = cpu_to_le32(curr);
+		pDesc->next_desc = cpu_to_le32(curr + sizeof(SRxDesc));
+	}
 
+	if (i > 0)
+		pDevice->aRD0Ring[i-1].next_desc = cpu_to_le32(pDevice->rd0_pool_dma);
+	pDevice->pCurrRD[0] = &(pDevice->aRD0Ring[0]);
+}
 
 static void device_init_rd1_ring(PSDevice pDevice) {
-    int i;
-    dma_addr_t      curr = pDevice->rd1_pool_dma;
-    PSRxDesc        pDesc;
-
-    /* Init the RD1 ring entries */
-    for (i = 0; i < pDevice->sOpts.nRxDescs1; i ++, curr += sizeof(SRxDesc)) {
-        pDesc = &(pDevice->aRD1Ring[i]);
-        pDesc->pRDInfo = alloc_rd_info();
-        ASSERT(pDesc->pRDInfo);
-        if (!device_alloc_rx_buf(pDevice, pDesc)) {
-            DBG_PRT(MSG_LEVEL_ERR,KERN_ERR "%s: can not alloc rx bufs\n",
-            pDevice->dev->name);
-        }
-        pDesc->next = &(pDevice->aRD1Ring[(i+1) % pDevice->sOpts.nRxDescs1]);
-        pDesc->pRDInfo->curr_desc = cpu_to_le32(curr);
-        pDesc->next_desc = cpu_to_le32(curr + sizeof(SRxDesc));
-    }
-
-    if (i > 0)
-        pDevice->aRD1Ring[i-1].next_desc = cpu_to_le32(pDevice->rd1_pool_dma);
-    pDevice->pCurrRD[1] = &(pDevice->aRD1Ring[0]);
-}
+	int i;
+	dma_addr_t      curr = pDevice->rd1_pool_dma;
+	PSRxDesc        pDesc;
+
+	/* Init the RD1 ring entries */
+	for (i = 0; i < pDevice->sOpts.nRxDescs1; i ++, curr += sizeof(SRxDesc)) {
+		pDesc = &(pDevice->aRD1Ring[i]);
+		pDesc->pRDInfo = alloc_rd_info();
+		ASSERT(pDesc->pRDInfo);
+		if (!device_alloc_rx_buf(pDevice, pDesc)) {
+			DBG_PRT(MSG_LEVEL_ERR, KERN_ERR "%s: can not alloc rx bufs\n",
+				pDevice->dev->name);
+		}
+		pDesc->next = &(pDevice->aRD1Ring[(i+1) % pDevice->sOpts.nRxDescs1]);
+		pDesc->pRDInfo->curr_desc = cpu_to_le32(curr);
+		pDesc->next_desc = cpu_to_le32(curr + sizeof(SRxDesc));
+	}
 
+	if (i > 0)
+		pDevice->aRD1Ring[i-1].next_desc = cpu_to_le32(pDevice->rd1_pool_dma);
+	pDevice->pCurrRD[1] = &(pDevice->aRD1Ring[0]);
+}
 
 static void device_init_defrag_cb(PSDevice pDevice) {
-    int i;
-    PSDeFragControlBlock pDeF;
-
-    /* Init the fragment ctl entries */
-    for (i = 0; i < CB_MAX_RX_FRAG; i++) {
-        pDeF = &(pDevice->sRxDFCB[i]);
-        if (!device_alloc_frag_buf(pDevice, pDeF)) {
-            DBG_PRT(MSG_LEVEL_ERR,KERN_ERR "%s: can not alloc frag bufs\n",
-                pDevice->dev->name);
-        }
-    }
-    pDevice->cbDFCB = CB_MAX_RX_FRAG;
-    pDevice->cbFreeDFCB = pDevice->cbDFCB;
+	int i;
+	PSDeFragControlBlock pDeF;
+
+	/* Init the fragment ctl entries */
+	for (i = 0; i < CB_MAX_RX_FRAG; i++) {
+		pDeF = &(pDevice->sRxDFCB[i]);
+		if (!device_alloc_frag_buf(pDevice, pDeF)) {
+			DBG_PRT(MSG_LEVEL_ERR, KERN_ERR "%s: can not alloc frag bufs\n",
+				pDevice->dev->name);
+		}
+	}
+	pDevice->cbDFCB = CB_MAX_RX_FRAG;
+	pDevice->cbFreeDFCB = pDevice->cbDFCB;
 }
 
-
-
-
 static void device_free_rd0_ring(PSDevice pDevice) {
-    int i;
-
-    for (i = 0; i < pDevice->sOpts.nRxDescs0; i++) {
-        PSRxDesc        pDesc =&(pDevice->aRD0Ring[i]);
-        PDEVICE_RD_INFO  pRDInfo =pDesc->pRDInfo;
+	int i;
 
-        pci_unmap_single(pDevice->pcid,pRDInfo->skb_dma,
-           pDevice->rx_buf_sz, PCI_DMA_FROMDEVICE);
+	for (i = 0; i < pDevice->sOpts.nRxDescs0; i++) {
+		PSRxDesc        pDesc = &(pDevice->aRD0Ring[i]);
+		PDEVICE_RD_INFO  pRDInfo = pDesc->pRDInfo;
 
-        dev_kfree_skb(pRDInfo->skb);
+		pci_unmap_single(pDevice->pcid, pRDInfo->skb_dma,
+				 pDevice->rx_buf_sz, PCI_DMA_FROMDEVICE);
 
-        kfree((void *)pDesc->pRDInfo);
-    }
+		dev_kfree_skb(pRDInfo->skb);
 
+		kfree((void *)pDesc->pRDInfo);
+	}
 }
 
 static void device_free_rd1_ring(PSDevice pDevice) {
-    int i;
-
-
-    for (i = 0; i < pDevice->sOpts.nRxDescs1; i++) {
-        PSRxDesc        pDesc=&(pDevice->aRD1Ring[i]);
-        PDEVICE_RD_INFO  pRDInfo=pDesc->pRDInfo;
+	int i;
 
-        pci_unmap_single(pDevice->pcid,pRDInfo->skb_dma,
-           pDevice->rx_buf_sz, PCI_DMA_FROMDEVICE);
+	for (i = 0; i < pDevice->sOpts.nRxDescs1; i++) {
+		PSRxDesc        pDesc = &(pDevice->aRD1Ring[i]);
+		PDEVICE_RD_INFO  pRDInfo = pDesc->pRDInfo;
 
-        dev_kfree_skb(pRDInfo->skb);
+		pci_unmap_single(pDevice->pcid, pRDInfo->skb_dma,
+				 pDevice->rx_buf_sz, PCI_DMA_FROMDEVICE);
 
-        kfree((void *)pDesc->pRDInfo);
-    }
+		dev_kfree_skb(pRDInfo->skb);
 
+		kfree((void *)pDesc->pRDInfo);
+	}
 }
 
 static void device_free_frag_buf(PSDevice pDevice) {
-    PSDeFragControlBlock pDeF;
-    int i;
-
-    for (i = 0; i < CB_MAX_RX_FRAG; i++) {
-
-        pDeF = &(pDevice->sRxDFCB[i]);
+	PSDeFragControlBlock pDeF;
+	int i;
 
-        if (pDeF->skb)
-            dev_kfree_skb(pDeF->skb);
+	for (i = 0; i < CB_MAX_RX_FRAG; i++) {
+		pDeF = &(pDevice->sRxDFCB[i]);
 
-    }
+		if (pDeF->skb)
+			dev_kfree_skb(pDeF->skb);
 
+	}
 }
 
 static void device_init_td0_ring(PSDevice pDevice) {
-    int i;
-    dma_addr_t  curr;
-    PSTxDesc        pDesc;
-
-    curr = pDevice->td0_pool_dma;
-    for (i = 0; i < pDevice->sOpts.nTxDescs[0]; i++, curr += sizeof(STxDesc)) {
-        pDesc = &(pDevice->apTD0Rings[i]);
-        pDesc->pTDInfo = alloc_td_info();
-        ASSERT(pDesc->pTDInfo);
-        if (pDevice->flags & DEVICE_FLAGS_TX_ALIGN) {
-            pDesc->pTDInfo->buf = pDevice->tx0_bufs + (i)*PKT_BUF_SZ;
-            pDesc->pTDInfo->buf_dma = pDevice->tx_bufs_dma0 + (i)*PKT_BUF_SZ;
-        }
-        pDesc->next =&(pDevice->apTD0Rings[(i+1) % pDevice->sOpts.nTxDescs[0]]);
-        pDesc->pTDInfo->curr_desc = cpu_to_le32(curr);
-        pDesc->next_desc = cpu_to_le32(curr+sizeof(STxDesc));
-    }
-
-    if (i > 0)
-        pDevice->apTD0Rings[i-1].next_desc = cpu_to_le32(pDevice->td0_pool_dma);
-    pDevice->apTailTD[0] = pDevice->apCurrTD[0] =&(pDevice->apTD0Rings[0]);
+	int i;
+	dma_addr_t  curr;
+	PSTxDesc        pDesc;
+
+	curr = pDevice->td0_pool_dma;
+	for (i = 0; i < pDevice->sOpts.nTxDescs[0]; i++, curr += sizeof(STxDesc)) {
+		pDesc = &(pDevice->apTD0Rings[i]);
+		pDesc->pTDInfo = alloc_td_info();
+		ASSERT(pDesc->pTDInfo);
+		if (pDevice->flags & DEVICE_FLAGS_TX_ALIGN) {
+			pDesc->pTDInfo->buf = pDevice->tx0_bufs + (i)*PKT_BUF_SZ;
+			pDesc->pTDInfo->buf_dma = pDevice->tx_bufs_dma0 + (i)*PKT_BUF_SZ;
+		}
+		pDesc->next = &(pDevice->apTD0Rings[(i+1) % pDevice->sOpts.nTxDescs[0]]);
+		pDesc->pTDInfo->curr_desc = cpu_to_le32(curr);
+		pDesc->next_desc = cpu_to_le32(curr+sizeof(STxDesc));
+	}
 
+	if (i > 0)
+		pDevice->apTD0Rings[i-1].next_desc = cpu_to_le32(pDevice->td0_pool_dma);
+	pDevice->apTailTD[0] = pDevice->apCurrTD[0] = &(pDevice->apTD0Rings[0]);
 }
 
 static void device_init_td1_ring(PSDevice pDevice) {
-    int i;
-    dma_addr_t  curr;
-    PSTxDesc    pDesc;
-
-    /* Init the TD ring entries */
-    curr=pDevice->td1_pool_dma;
-    for (i = 0; i < pDevice->sOpts.nTxDescs[1]; i++, curr+=sizeof(STxDesc)) {
-        pDesc=&(pDevice->apTD1Rings[i]);
-        pDesc->pTDInfo = alloc_td_info();
-        ASSERT(pDesc->pTDInfo);
-        if (pDevice->flags & DEVICE_FLAGS_TX_ALIGN) {
-            pDesc->pTDInfo->buf=pDevice->tx1_bufs+(i)*PKT_BUF_SZ;
-            pDesc->pTDInfo->buf_dma=pDevice->tx_bufs_dma1+(i)*PKT_BUF_SZ;
-        }
-        pDesc->next=&(pDevice->apTD1Rings[(i+1) % pDevice->sOpts.nTxDescs[1]]);
-        pDesc->pTDInfo->curr_desc = cpu_to_le32(curr);
-        pDesc->next_desc = cpu_to_le32(curr+sizeof(STxDesc));
-    }
-
-    if (i > 0)
-        pDevice->apTD1Rings[i-1].next_desc = cpu_to_le32(pDevice->td1_pool_dma);
-    pDevice->apTailTD[1] = pDevice->apCurrTD[1] = &(pDevice->apTD1Rings[0]);
-}
-
+	int i;
+	dma_addr_t  curr;
+	PSTxDesc    pDesc;
+
+	/* Init the TD ring entries */
+	curr = pDevice->td1_pool_dma;
+	for (i = 0; i < pDevice->sOpts.nTxDescs[1]; i++, curr += sizeof(STxDesc)) {
+		pDesc = &(pDevice->apTD1Rings[i]);
+		pDesc->pTDInfo = alloc_td_info();
+		ASSERT(pDesc->pTDInfo);
+		if (pDevice->flags & DEVICE_FLAGS_TX_ALIGN) {
+			pDesc->pTDInfo->buf = pDevice->tx1_bufs + (i) * PKT_BUF_SZ;
+			pDesc->pTDInfo->buf_dma = pDevice->tx_bufs_dma1 + (i) * PKT_BUF_SZ;
+		}
+		pDesc->next = &(pDevice->apTD1Rings[(i + 1) % pDevice->sOpts.nTxDescs[1]]);
+		pDesc->pTDInfo->curr_desc = cpu_to_le32(curr);
+		pDesc->next_desc = cpu_to_le32(curr+sizeof(STxDesc));
+	}
 
+	if (i > 0)
+		pDevice->apTD1Rings[i-1].next_desc = cpu_to_le32(pDevice->td1_pool_dma);
+	pDevice->apTailTD[1] = pDevice->apCurrTD[1] = &(pDevice->apTD1Rings[0]);
+}
 
 static void device_free_td0_ring(PSDevice pDevice) {
-    int i;
-    for (i = 0; i < pDevice->sOpts.nTxDescs[0]; i++) {
-        PSTxDesc        pDesc=&(pDevice->apTD0Rings[i]);
-        PDEVICE_TD_INFO  pTDInfo=pDesc->pTDInfo;
+	int i;
+	for (i = 0; i < pDevice->sOpts.nTxDescs[0]; i++) {
+		PSTxDesc        pDesc = &(pDevice->apTD0Rings[i]);
+		PDEVICE_TD_INFO  pTDInfo = pDesc->pTDInfo;
 
-        if (pTDInfo->skb_dma && (pTDInfo->skb_dma != pTDInfo->buf_dma))
-            pci_unmap_single(pDevice->pcid,pTDInfo->skb_dma,
-               pTDInfo->skb->len, PCI_DMA_TODEVICE);
+		if (pTDInfo->skb_dma && (pTDInfo->skb_dma != pTDInfo->buf_dma))
+			pci_unmap_single(pDevice->pcid, pTDInfo->skb_dma,
+					 pTDInfo->skb->len, PCI_DMA_TODEVICE);
 
-        if (pTDInfo->skb)
-            dev_kfree_skb(pTDInfo->skb);
+		if (pTDInfo->skb)
+			dev_kfree_skb(pTDInfo->skb);
 
-        kfree((void *)pDesc->pTDInfo);
-    }
+		kfree((void *)pDesc->pTDInfo);
+	}
 }
 
 static void device_free_td1_ring(PSDevice pDevice) {
-    int i;
-
-    for (i = 0; i < pDevice->sOpts.nTxDescs[1]; i++) {
-        PSTxDesc        pDesc=&(pDevice->apTD1Rings[i]);
-        PDEVICE_TD_INFO  pTDInfo=pDesc->pTDInfo;
+	int i;
 
-        if (pTDInfo->skb_dma && (pTDInfo->skb_dma != pTDInfo->buf_dma))
-            pci_unmap_single(pDevice->pcid, pTDInfo->skb_dma,
-               pTDInfo->skb->len, PCI_DMA_TODEVICE);
+	for (i = 0; i < pDevice->sOpts.nTxDescs[1]; i++) {
+		PSTxDesc        pDesc = &(pDevice->apTD1Rings[i]);
+		PDEVICE_TD_INFO  pTDInfo = pDesc->pTDInfo;
 
-        if (pTDInfo->skb)
-            dev_kfree_skb(pTDInfo->skb);
+		if (pTDInfo->skb_dma && (pTDInfo->skb_dma != pTDInfo->buf_dma))
+			pci_unmap_single(pDevice->pcid, pTDInfo->skb_dma,
+					 pTDInfo->skb->len, PCI_DMA_TODEVICE);
 
-        kfree((void *)pDesc->pTDInfo);
-    }
+		if (pTDInfo->skb)
+			dev_kfree_skb(pTDInfo->skb);
 
+		kfree((void *)pDesc->pTDInfo);
+	}
 }
 
-
-
 /*-----------------------------------------------------------------*/
 
 static int device_rx_srv(PSDevice pDevice, unsigned int uIdx) {
-    PSRxDesc    pRD;
-    int works = 0;
-
+	PSRxDesc    pRD;
+	int works = 0;
 
-    for (pRD = pDevice->pCurrRD[uIdx];
-         pRD->m_rd0RD0.f1Owner == OWNED_BY_HOST;
-         pRD = pRD->next) {
+	for (pRD = pDevice->pCurrRD[uIdx];
+	     pRD->m_rd0RD0.f1Owner == OWNED_BY_HOST;
+	     pRD = pRD->next) {
 //        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->pCurrRD = %x, works = %d\n", pRD, works);
-        if (works++>15)
-            break;
-        if (device_receive_frame(pDevice, pRD)) {
-            if (!device_alloc_rx_buf(pDevice,pRD)) {
-                    DBG_PRT(MSG_LEVEL_ERR, KERN_ERR
-                    "%s: can not allocate rx buf\n", pDevice->dev->name);
-                    break;
-            }
-        }
-        pRD->m_rd0RD0.f1Owner = OWNED_BY_NIC;
-        pDevice->dev->last_rx = jiffies;
-    }
-
-    pDevice->pCurrRD[uIdx]=pRD;
-
-    return works;
-}
+		if (works++ > 15)
+			break;
+		if (device_receive_frame(pDevice, pRD)) {
+			if (!device_alloc_rx_buf(pDevice, pRD)) {
+				DBG_PRT(MSG_LEVEL_ERR, KERN_ERR
+					"%s: can not allocate rx buf\n", pDevice->dev->name);
+				break;
+			}
+		}
+		pRD->m_rd0RD0.f1Owner = OWNED_BY_NIC;
+		pDevice->dev->last_rx = jiffies;
+	}
 
+	pDevice->pCurrRD[uIdx] = pRD;
 
-static bool device_alloc_rx_buf(PSDevice pDevice, PSRxDesc pRD) {
+	return works;
+}
 
-    PDEVICE_RD_INFO pRDInfo=pRD->pRDInfo;
+static bool device_alloc_rx_buf(PSDevice pDevice, PSRxDesc pRD) {
+	PDEVICE_RD_INFO pRDInfo = pRD->pRDInfo;
+
+	pRDInfo->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
+	if (pRDInfo->skb == NULL)
+		return false;
+	ASSERT(pRDInfo->skb);
+	pRDInfo->skb->dev = pDevice->dev;
+	pRDInfo->skb_dma = pci_map_single(pDevice->pcid, skb_tail_pointer(pRDInfo->skb),
+					  pDevice->rx_buf_sz, PCI_DMA_FROMDEVICE);
+	*((unsigned int *)&(pRD->m_rd0RD0)) = 0; /* FIX cast */
+
+	pRD->m_rd0RD0.wResCount = cpu_to_le16(pDevice->rx_buf_sz);
+	pRD->m_rd0RD0.f1Owner = OWNED_BY_NIC;
+	pRD->m_rd1RD1.wReqCount = cpu_to_le16(pDevice->rx_buf_sz);
+	pRD->buff_addr = cpu_to_le32(pRDInfo->skb_dma);
+
+	return true;
+}
 
+bool device_alloc_frag_buf(PSDevice pDevice, PSDeFragControlBlock pDeF) {
+	pDeF->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
+	if (pDeF->skb == NULL)
+		return false;
+	ASSERT(pDeF->skb);
+	pDeF->skb->dev = pDevice->dev;
 
-    pRDInfo->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
-#ifdef	PLICE_DEBUG
-	//printk("device_alloc_rx_buf:skb is %x\n",pRDInfo->skb);
-#endif
-    if (pRDInfo->skb==NULL)
-        return false;
-    ASSERT(pRDInfo->skb);
-    pRDInfo->skb->dev = pDevice->dev;
-    pRDInfo->skb_dma = pci_map_single(pDevice->pcid, skb_tail_pointer(pRDInfo->skb),
-				      pDevice->rx_buf_sz, PCI_DMA_FROMDEVICE);
-    *((unsigned int *) &(pRD->m_rd0RD0)) = 0; /* FIX cast */
-
-    pRD->m_rd0RD0.wResCount = cpu_to_le16(pDevice->rx_buf_sz);
-    pRD->m_rd0RD0.f1Owner = OWNED_BY_NIC;
-    pRD->m_rd1RD1.wReqCount = cpu_to_le16(pDevice->rx_buf_sz);
-    pRD->buff_addr = cpu_to_le32(pRDInfo->skb_dma);
-
-    return true;
+	return true;
 }
 
+static int device_tx_srv(PSDevice pDevice, unsigned int uIdx) {
+	PSTxDesc                 pTD;
+	bool bFull = false;
+	int                      works = 0;
+	unsigned char byTsr0;
+	unsigned char byTsr1;
+	unsigned int	uFrameSize, uFIFOHeaderSize;
+	PSTxBufHead              pTxBufHead;
+	struct net_device_stats *pStats = &pDevice->stats;
+	struct sk_buff *skb;
+	unsigned int	uNodeIndex;
+	PSMgmtObject             pMgmt = pDevice->pMgmt;
+
+	for (pTD = pDevice->apTailTD[uIdx]; pDevice->iTDUsed[uIdx] > 0; pTD = pTD->next) {
+		if (pTD->m_td0TD0.f1Owner == OWNED_BY_NIC)
+			break;
+		if (works++ > 15)
+			break;
 
+		byTsr0 = pTD->m_td0TD0.byTSR0;
+		byTsr1 = pTD->m_td0TD0.byTSR1;
+
+		//Only the status of first TD in the chain is correct
+		if (pTD->m_td1TD1.byTCR & TCR_STP) {
+			if ((pTD->pTDInfo->byFlags & TD_FLAGS_NETIF_SKB) != 0) {
+				uFIFOHeaderSize = pTD->pTDInfo->dwHeaderLength;
+				uFrameSize = pTD->pTDInfo->dwReqCount - uFIFOHeaderSize;
+				pTxBufHead = (PSTxBufHead) (pTD->pTDInfo->buf);
+				// Update the statistics based on the Transmit status
+				// now, we DONT check TSR0_CDH
+
+				STAvUpdateTDStatCounter(&pDevice->scStatistic,
+							byTsr0, byTsr1,
+							(unsigned char *)(pTD->pTDInfo->buf + uFIFOHeaderSize),
+							uFrameSize, uIdx);
+
+				BSSvUpdateNodeTxCounter(pDevice,
+							byTsr0, byTsr1,
+							(unsigned char *)(pTD->pTDInfo->buf),
+							uFIFOHeaderSize
+					);
+
+				if (!(byTsr1 & TSR1_TERR)) {
+					if (byTsr0 != 0) {
+						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Tx[%d] OK but has error. tsr1[%02X] tsr0[%02X].\n",
+							(int)uIdx, byTsr1, byTsr0);
+					}
+					if ((pTxBufHead->wFragCtl & FRAGCTL_ENDFRAG) != FRAGCTL_NONFRAG) {
+						pDevice->s802_11Counter.TransmittedFragmentCount++;
+					}
+					pStats->tx_packets++;
+					pStats->tx_bytes += pTD->pTDInfo->skb->len;
+				} else {
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Tx[%d] dropped & tsr1[%02X] tsr0[%02X].\n",
+						(int)uIdx, byTsr1, byTsr0);
+					pStats->tx_errors++;
+					pStats->tx_dropped++;
+				}
+			}
 
-bool device_alloc_frag_buf(PSDevice pDevice, PSDeFragControlBlock pDeF) {
+			if ((pTD->pTDInfo->byFlags & TD_FLAGS_PRIV_SKB) != 0) {
+				if (pDevice->bEnableHostapd) {
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "tx call back netif.. \n");
+					skb = pTD->pTDInfo->skb;
+					skb->dev = pDevice->apdev;
+					skb_reset_mac_header(skb);
+					skb->pkt_type = PACKET_OTHERHOST;
+					//skb->protocol = htons(ETH_P_802_2);
+					memset(skb->cb, 0, sizeof(skb->cb));
+					netif_rx(skb);
+				}
+			}
 
-    pDeF->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
-    if (pDeF->skb == NULL)
-        return false;
-    ASSERT(pDeF->skb);
-    pDeF->skb->dev = pDevice->dev;
+			if (byTsr1 & TSR1_TERR) {
+				if ((pTD->pTDInfo->byFlags & TD_FLAGS_PRIV_SKB) != 0) {
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Tx[%d] fail has error. tsr1[%02X] tsr0[%02X].\n",
+						(int)uIdx, byTsr1, byTsr0);
+				}
 
-    return true;
-}
+//                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Tx[%d] fail has error. tsr1[%02X] tsr0[%02X].\n",
+//                          (int)uIdx, byTsr1, byTsr0);
 
+				if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) &&
+				    (pTD->pTDInfo->byFlags & TD_FLAGS_NETIF_SKB)) {
+					unsigned short wAID;
+					unsigned char byMask[8] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80};
+
+					skb = pTD->pTDInfo->skb;
+					if (BSSDBbIsSTAInNodeDB(pMgmt, (unsigned char *)(skb->data), &uNodeIndex)) {
+						if (pMgmt->sNodeDBTable[uNodeIndex].bPSEnable) {
+							skb_queue_tail(&pMgmt->sNodeDBTable[uNodeIndex].sTxPSQueue, skb);
+							pMgmt->sNodeDBTable[uNodeIndex].wEnQueueCnt++;
+							// set tx map
+							wAID = pMgmt->sNodeDBTable[uNodeIndex].wAID;
+							pMgmt->abyPSTxMap[wAID >> 3] |=  byMask[wAID & 7];
+							pTD->pTDInfo->byFlags &= ~(TD_FLAGS_NETIF_SKB);
+							DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "tx_srv:tx fail re-queue sta index= %d, QueCnt= %d\n"
+								, (int)uNodeIndex, pMgmt->sNodeDBTable[uNodeIndex].wEnQueueCnt);
+							pStats->tx_errors--;
+							pStats->tx_dropped--;
+						}
+					}
+				}
+			}
+			device_free_tx_buf(pDevice, pTD);
+			pDevice->iTDUsed[uIdx]--;
+		}
+	}
 
+	if (uIdx == TYPE_AC0DMA) {
+		// RESERV_AC0DMA reserved for relay
 
-static int device_tx_srv(PSDevice pDevice, unsigned int uIdx) {
-    PSTxDesc                 pTD;
-    bool bFull=false;
-    int                      works = 0;
-    unsigned char byTsr0;
-    unsigned char byTsr1;
-    unsigned int	uFrameSize, uFIFOHeaderSize;
-    PSTxBufHead              pTxBufHead;
-    struct net_device_stats* pStats = &pDevice->stats;
-    struct sk_buff*          skb;
-    unsigned int	uNodeIndex;
-    PSMgmtObject             pMgmt = pDevice->pMgmt;
-
-
-    for (pTD = pDevice->apTailTD[uIdx]; pDevice->iTDUsed[uIdx] >0; pTD = pTD->next) {
-
-        if (pTD->m_td0TD0.f1Owner == OWNED_BY_NIC)
-            break;
-        if (works++>15)
-            break;
-
-        byTsr0 = pTD->m_td0TD0.byTSR0;
-        byTsr1 = pTD->m_td0TD0.byTSR1;
-
-        //Only the status of first TD in the chain is correct
-        if (pTD->m_td1TD1.byTCR & TCR_STP) {
-
-            if ((pTD->pTDInfo->byFlags & TD_FLAGS_NETIF_SKB) != 0) {
-                uFIFOHeaderSize = pTD->pTDInfo->dwHeaderLength;
-                uFrameSize = pTD->pTDInfo->dwReqCount - uFIFOHeaderSize;
-                pTxBufHead = (PSTxBufHead) (pTD->pTDInfo->buf);
-                // Update the statistics based on the Transmit status
-                // now, we DONT check TSR0_CDH
-
-                STAvUpdateTDStatCounter(&pDevice->scStatistic,
-                        byTsr0, byTsr1,
-                        (unsigned char *)(pTD->pTDInfo->buf + uFIFOHeaderSize),
-                        uFrameSize, uIdx);
-
-
-                BSSvUpdateNodeTxCounter(pDevice,
-                         byTsr0, byTsr1,
-                         (unsigned char *)(pTD->pTDInfo->buf),
-                         uFIFOHeaderSize
-                         );
-
-                if ( !(byTsr1 & TSR1_TERR)) {
-                    if (byTsr0 != 0) {
-                        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" Tx[%d] OK but has error. tsr1[%02X] tsr0[%02X].\n",
-                           (int)uIdx, byTsr1, byTsr0);
-                    }
-                    if ((pTxBufHead->wFragCtl & FRAGCTL_ENDFRAG) != FRAGCTL_NONFRAG) {
-                        pDevice->s802_11Counter.TransmittedFragmentCount ++;
-                    }
-                    pStats->tx_packets++;
-                    pStats->tx_bytes += pTD->pTDInfo->skb->len;
-                }
-                else {
-                     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" Tx[%d] dropped & tsr1[%02X] tsr0[%02X].\n",
-                           (int)uIdx, byTsr1, byTsr0);
-                    pStats->tx_errors++;
-                    pStats->tx_dropped++;
-                }
-            }
-
-            if ((pTD->pTDInfo->byFlags & TD_FLAGS_PRIV_SKB) != 0) {
-                if (pDevice->bEnableHostapd) {
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "tx call back netif.. \n");
-                    skb = pTD->pTDInfo->skb;
-	                skb->dev = pDevice->apdev;
-			skb_reset_mac_header(skb);
-	                skb->pkt_type = PACKET_OTHERHOST;
-    	            //skb->protocol = htons(ETH_P_802_2);
-	                memset(skb->cb, 0, sizeof(skb->cb));
-	                netif_rx(skb);
-	            }
-            }
-
-            if (byTsr1 & TSR1_TERR) {
-            if ((pTD->pTDInfo->byFlags & TD_FLAGS_PRIV_SKB) != 0) {
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" Tx[%d] fail has error. tsr1[%02X] tsr0[%02X].\n",
-                          (int)uIdx, byTsr1, byTsr0);
-            }
-
-//                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" Tx[%d] fail has error. tsr1[%02X] tsr0[%02X].\n",
-//                          (int)uIdx, byTsr1, byTsr0);
+		if (AVAIL_TD(pDevice, uIdx) < RESERV_AC0DMA) {
+			bFull = true;
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " AC0DMA is Full = %d\n", pDevice->iTDUsed[uIdx]);
+		}
+		if (netif_queue_stopped(pDevice->dev) && (bFull == false)) {
+			netif_wake_queue(pDevice->dev);
+		}
+	}
 
-                if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) &&
-                    (pTD->pTDInfo->byFlags & TD_FLAGS_NETIF_SKB)) {
-                    unsigned short wAID;
-                    unsigned char byMask[8] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80};
-
-                    skb = pTD->pTDInfo->skb;
-                    if (BSSDBbIsSTAInNodeDB(pMgmt, (unsigned char *)(skb->data), &uNodeIndex)) {
-                        if (pMgmt->sNodeDBTable[uNodeIndex].bPSEnable) {
-                            skb_queue_tail(&pMgmt->sNodeDBTable[uNodeIndex].sTxPSQueue, skb);
-                            pMgmt->sNodeDBTable[uNodeIndex].wEnQueueCnt++;
-                            // set tx map
-                            wAID = pMgmt->sNodeDBTable[uNodeIndex].wAID;
-                            pMgmt->abyPSTxMap[wAID >> 3] |=  byMask[wAID & 7];
-                            pTD->pTDInfo->byFlags &= ~(TD_FLAGS_NETIF_SKB);
-                            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "tx_srv:tx fail re-queue sta index= %d, QueCnt= %d\n"
-                                    ,(int)uNodeIndex, pMgmt->sNodeDBTable[uNodeIndex].wEnQueueCnt);
-                            pStats->tx_errors--;
-                            pStats->tx_dropped--;
-                        }
-                    }
-                }
-            }
-            device_free_tx_buf(pDevice,pTD);
-            pDevice->iTDUsed[uIdx]--;
-        }
-    }
-
-
-    if (uIdx == TYPE_AC0DMA) {
-        // RESERV_AC0DMA reserved for relay
-
-        if (AVAIL_TD(pDevice, uIdx) < RESERV_AC0DMA) {
-            bFull = true;
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " AC0DMA is Full = %d\n", pDevice->iTDUsed[uIdx]);
-        }
-        if (netif_queue_stopped(pDevice->dev) && (bFull==false)){
-            netif_wake_queue(pDevice->dev);
-        }
-    }
-
-
-    pDevice->apTailTD[uIdx] = pTD;
-
-    return works;
-}
+	pDevice->apTailTD[uIdx] = pTD;
 
+	return works;
+}
 
 static void device_error(PSDevice pDevice, unsigned short status) {
-
-    if (status & ISR_FETALERR) {
-        DBG_PRT(MSG_LEVEL_ERR, KERN_ERR
-            "%s: Hardware fatal error.\n",
-            pDevice->dev->name);
-        netif_stop_queue(pDevice->dev);
-        del_timer(&pDevice->sTimerCommand);
-        del_timer(&(pDevice->pMgmt->sTimerSecondCallback));
-        pDevice->bCmdRunning = false;
-        MACbShutdown(pDevice->PortOffset);
-        return;
-    }
-
+	if (status & ISR_FETALERR) {
+		DBG_PRT(MSG_LEVEL_ERR, KERN_ERR
+			"%s: Hardware fatal error.\n",
+			pDevice->dev->name);
+		netif_stop_queue(pDevice->dev);
+		del_timer(&pDevice->sTimerCommand);
+		del_timer(&(pDevice->pMgmt->sTimerSecondCallback));
+		pDevice->bCmdRunning = false;
+		MACbShutdown(pDevice->PortOffset);
+		return;
+	}
 }
 
 static void device_free_tx_buf(PSDevice pDevice, PSTxDesc pDesc) {
-    PDEVICE_TD_INFO  pTDInfo=pDesc->pTDInfo;
-    struct sk_buff* skb=pTDInfo->skb;
+	PDEVICE_TD_INFO  pTDInfo = pDesc->pTDInfo;
+	struct sk_buff *skb = pTDInfo->skb;
 
-    // pre-allocated buf_dma can't be unmapped.
-    if (pTDInfo->skb_dma && (pTDInfo->skb_dma != pTDInfo->buf_dma)) {
-        pci_unmap_single(pDevice->pcid,pTDInfo->skb_dma,skb->len,
-              PCI_DMA_TODEVICE);
-    }
+	// pre-allocated buf_dma can't be unmapped.
+	if (pTDInfo->skb_dma && (pTDInfo->skb_dma != pTDInfo->buf_dma)) {
+		pci_unmap_single(pDevice->pcid, pTDInfo->skb_dma, skb->len,
+				 PCI_DMA_TODEVICE);
+	}
 
-    if ((pTDInfo->byFlags & TD_FLAGS_NETIF_SKB) != 0)
-        dev_kfree_skb_irq(skb);
+	if ((pTDInfo->byFlags & TD_FLAGS_NETIF_SKB) != 0)
+		dev_kfree_skb_irq(skb);
 
-    pTDInfo->skb_dma = 0;
-    pTDInfo->skb = 0;
-    pTDInfo->byFlags = 0;
+	pTDInfo->skb_dma = 0;
+	pTDInfo->skb = 0;
+	pTDInfo->byFlags = 0;
 }
 
-
-
 //PLICE_DEBUG ->
 void	InitRxManagementQueue(PSDevice  pDevice)
 {
@@ -1816,107 +1686,88 @@ void	InitRxManagementQueue(PSDevice  pDevice)
 }
 //PLICE_DEBUG<-
 
-
-
-
-
 //PLICE_DEBUG ->
 int MlmeThread(
-     void * Context)
+	void *Context)
 {
 	PSDevice	pDevice =  (PSDevice) Context;
 	PSRxMgmtPacket			pRxMgmtPacket;
-	// int i ;
+	// int i;
 	//complete(&pDevice->notify);
-//printk("Enter MngWorkItem,Queue packet num is %d\n",pDevice->rxManeQueue.packet_num);
 
-	//printk("Enter MlmeThread,packet _num is %d\n",pDevice->rxManeQueue.packet_num);
 	//i = 0;
 #if 1
-	while (1)
-	{
-
-	//printk("DDDD\n");
-	//down(&pDevice->mlme_semaphore);
-        // pRxMgmtPacket =  DeQueue(pDevice);
+	while (1) {
+		//down(&pDevice->mlme_semaphore);
+		// pRxMgmtPacket =  DeQueue(pDevice);
 #if 1
 		spin_lock_irq(&pDevice->lock);
-		 while(pDevice->rxManeQueue.packet_num != 0)
-	 	{
-			 pRxMgmtPacket =  DeQueue(pDevice);
-        			//pDevice;
-        			//DequeueManageObject(pDevice->FirstRecvMngList, pDevice->LastRecvMngList);
+		while (pDevice->rxManeQueue.packet_num != 0) {
+			pRxMgmtPacket = DeQueue(pDevice);
+			//pDevice;
+			//DequeueManageObject(pDevice->FirstRecvMngList, pDevice->LastRecvMngList);
 			vMgrRxManagePacket(pDevice, pDevice->pMgmt, pRxMgmtPacket);
-			//printk("packet_num is %d\n",pDevice->rxManeQueue.packet_num);
-
-		 }
+		}
 		spin_unlock_irq(&pDevice->lock);
 		if (mlme_kill == 0)
-		break;
+			break;
 		//udelay(200);
 #endif
-	//printk("Before schedule thread jiffies is %x\n",jiffies);
-	schedule();
-	//printk("after schedule thread jiffies is %x\n",jiffies);
-	if (mlme_kill == 0)
-		break;
-	//printk("i is %d\n",i);
+		schedule();
+		if (mlme_kill == 0)
+			break;
 	}
 
 #endif
 	return 0;
-
 }
 
-
-
 static int  device_open(struct net_device *dev) {
-    PSDevice    pDevice=(PSDevice) netdev_priv(dev);
-    int i;
+	PSDevice pDevice = (PSDevice)netdev_priv(dev);
+	int i;
 #ifdef WPA_SM_Transtatus
-    extern SWPAResult wpa_Result;
+	extern SWPAResult wpa_Result;
 #endif
 
-    pDevice->rx_buf_sz = PKT_BUF_SZ;
-    if (!device_init_rings(pDevice)) {
-        return -ENOMEM;
-    }
+	pDevice->rx_buf_sz = PKT_BUF_SZ;
+	if (!device_init_rings(pDevice)) {
+		return -ENOMEM;
+	}
 //2008-5-13 <add> by chester
-    i=request_irq(pDevice->pcid->irq, &device_intr, IRQF_SHARED, dev->name, dev);
-    if (i)
-        return i;
-	//printk("DEBUG1\n");
+	i = request_irq(pDevice->pcid->irq, &device_intr, IRQF_SHARED, dev->name, dev);
+	if (i)
+		return i;
+
 #ifdef WPA_SM_Transtatus
-     memset(wpa_Result.ifname,0,sizeof(wpa_Result.ifname));
-     wpa_Result.proto = 0;
-     wpa_Result.key_mgmt = 0;
-     wpa_Result.eap_type = 0;
-     wpa_Result.authenticated = false;
-     pDevice->fWPA_Authened = false;
+	memset(wpa_Result.ifname, 0, sizeof(wpa_Result.ifname));
+	wpa_Result.proto = 0;
+	wpa_Result.key_mgmt = 0;
+	wpa_Result.eap_type = 0;
+	wpa_Result.authenticated = false;
+	pDevice->fWPA_Authened = false;
 #endif
-DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "call device init rd0 ring\n");
-device_init_rd0_ring(pDevice);
-    device_init_rd1_ring(pDevice);
-    device_init_defrag_cb(pDevice);
-    device_init_td0_ring(pDevice);
-    device_init_td1_ring(pDevice);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "call device init rd0 ring\n");
+	device_init_rd0_ring(pDevice);
+	device_init_rd1_ring(pDevice);
+	device_init_defrag_cb(pDevice);
+	device_init_td0_ring(pDevice);
+	device_init_td1_ring(pDevice);
 //    VNTWIFIvSet11h(pDevice->pMgmt, pDevice->b11hEnable);
 
-
-    if (pDevice->bDiversityRegCtlON) {
-        device_init_diversity_timer(pDevice);
-    }
-    vMgrObjectInit(pDevice);
-    vMgrTimerInit(pDevice);
+	if (pDevice->bDiversityRegCtlON) {
+		device_init_diversity_timer(pDevice);
+	}
+	vMgrObjectInit(pDevice);
+	vMgrTimerInit(pDevice);
 
 //PLICE_DEBUG->
 #ifdef	TASK_LET
-	tasklet_init (&pDevice->RxMngWorkItem,(void *)MngWorkItem,(unsigned long )pDevice);
+	tasklet_init(&pDevice->RxMngWorkItem, (void *)MngWorkItem, (unsigned long)pDevice);
 #endif
 #ifdef	THREAD
 	InitRxManagementQueue(pDevice);
 	mlme_kill = 0;
-	mlme_task = kthread_run(MlmeThread,(void *) pDevice, "MLME");
+	mlme_task = kthread_run(MlmeThread, (void *)pDevice, "MLME");
 	if (IS_ERR(mlme_task)) {
 		printk("thread create fail\n");
 		return -1;
@@ -1925,1184 +1776,1111 @@ device_init_rd0_ring(pDevice);
 	mlme_kill = 1;
 #endif
 
-
-
-	//printk("thread id is %d\n",pDevice->MLMEThr_pid);
-	//printk("Create thread time is %x\n",jiffies);
 	//wait_for_completion(&pDevice->notify);
 
-
-
-
-  // if (( SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_RADIOCTL)&0x06)==0x04)
-    //    return -ENOMEM;
-DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "call device_init_registers\n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "call device_init_registers\n");
 	device_init_registers(pDevice, DEVICE_INIT_COLD);
-    MACvReadEtherAddress(pDevice->PortOffset, pDevice->abyCurrentNetAddr);
-    memcpy(pDevice->pMgmt->abyMACAddr, pDevice->abyCurrentNetAddr, ETH_ALEN);
-    device_set_multi(pDevice->dev);
+	MACvReadEtherAddress(pDevice->PortOffset, pDevice->abyCurrentNetAddr);
+	memcpy(pDevice->pMgmt->abyMACAddr, pDevice->abyCurrentNetAddr, ETH_ALEN);
+	device_set_multi(pDevice->dev);
 
-    // Init for Key Management
-    KeyvInitTable(&pDevice->sKey, pDevice->PortOffset);
-    add_timer(&(pDevice->pMgmt->sTimerSecondCallback));
+	// Init for Key Management
+	KeyvInitTable(&pDevice->sKey, pDevice->PortOffset);
+	add_timer(&(pDevice->pMgmt->sTimerSecondCallback));
 
-	#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
+#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
 	/*
-     pDevice->bwextstep0 = false;
-     pDevice->bwextstep1 = false;
-     pDevice->bwextstep2 = false;
-     pDevice->bwextstep3 = false;
-     */
-       pDevice->bwextcount=0;
-     pDevice->bWPASuppWextEnabled = false;
+	  pDevice->bwextstep0 = false;
+	  pDevice->bwextstep1 = false;
+	  pDevice->bwextstep2 = false;
+	  pDevice->bwextstep3 = false;
+	*/
+	pDevice->bwextcount = 0;
+	pDevice->bWPASuppWextEnabled = false;
 #endif
-    pDevice->byReAssocCount = 0;
-   pDevice->bWPADEVUp = false;
-    // Patch: if WEP key already set by iwconfig but device not yet open
-    if ((pDevice->bEncryptionEnable == true) && (pDevice->bTransmitKey == true)) {
-        KeybSetDefaultKey(&(pDevice->sKey),
-                            (unsigned long)(pDevice->byKeyIndex | (1 << 31)),
-                            pDevice->uKeyLength,
-                            NULL,
-                            pDevice->abyKey,
-                            KEY_CTL_WEP,
-                            pDevice->PortOffset,
-                            pDevice->byLocalID
-                          );
-         pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
-    }
-
-//printk("DEBUG2\n");
-
-
-DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "call MACvIntEnable\n");
+	pDevice->byReAssocCount = 0;
+	pDevice->bWPADEVUp = false;
+	// Patch: if WEP key already set by iwconfig but device not yet open
+	if ((pDevice->bEncryptionEnable == true) && (pDevice->bTransmitKey == true)) {
+		KeybSetDefaultKey(&(pDevice->sKey),
+				  (unsigned long)(pDevice->byKeyIndex | (1 << 31)),
+				  pDevice->uKeyLength,
+				  NULL,
+				  pDevice->abyKey,
+				  KEY_CTL_WEP,
+				  pDevice->PortOffset,
+				  pDevice->byLocalID
+			);
+		pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
+	}
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "call MACvIntEnable\n");
 	MACvIntEnable(pDevice->PortOffset, IMR_MASK_VALUE);
 
-    if (pDevice->pMgmt->eConfigMode == WMAC_CONFIG_AP) {
-        bScheduleCommand((void *)pDevice, WLAN_CMD_RUN_AP, NULL);
+	if (pDevice->pMgmt->eConfigMode == WMAC_CONFIG_AP) {
+		bScheduleCommand((void *)pDevice, WLAN_CMD_RUN_AP, NULL);
+	} else {
+		bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, NULL);
+		bScheduleCommand((void *)pDevice, WLAN_CMD_SSID, NULL);
 	}
-	else {
-        bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, NULL);
-        bScheduleCommand((void *)pDevice, WLAN_CMD_SSID, NULL);
-    }
-    pDevice->flags |=DEVICE_FLAGS_OPENED;
+	pDevice->flags |= DEVICE_FLAGS_OPENED;
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "device_open success.. \n");
-    return 0;
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "device_open success.. \n");
+	return 0;
 }
 
-
 static int  device_close(struct net_device *dev) {
-    PSDevice  pDevice=(PSDevice) netdev_priv(dev);
-    PSMgmtObject     pMgmt = pDevice->pMgmt;
- //PLICE_DEBUG->
+	PSDevice  pDevice = (PSDevice)netdev_priv(dev);
+	PSMgmtObject     pMgmt = pDevice->pMgmt;
+	//PLICE_DEBUG->
 #ifdef	THREAD
 	mlme_kill = 0;
 #endif
 //PLICE_DEBUG<-
 //2007-1121-02<Add>by EinsnLiu
-    if (pDevice->bLinkPass) {
-	bScheduleCommand((void *)pDevice, WLAN_CMD_DISASSOCIATE, NULL);
-        mdelay(30);
-    }
+	if (pDevice->bLinkPass) {
+		bScheduleCommand((void *)pDevice, WLAN_CMD_DISASSOCIATE, NULL);
+		mdelay(30);
+	}
 #ifdef TxInSleep
-    del_timer(&pDevice->sTimerTxData);
+	del_timer(&pDevice->sTimerTxData);
 #endif
-    del_timer(&pDevice->sTimerCommand);
-    del_timer(&pMgmt->sTimerSecondCallback);
-    if (pDevice->bDiversityRegCtlON) {
-        del_timer(&pDevice->TimerSQ3Tmax1);
-        del_timer(&pDevice->TimerSQ3Tmax2);
-        del_timer(&pDevice->TimerSQ3Tmax3);
-    }
+	del_timer(&pDevice->sTimerCommand);
+	del_timer(&pMgmt->sTimerSecondCallback);
+	if (pDevice->bDiversityRegCtlON) {
+		del_timer(&pDevice->TimerSQ3Tmax1);
+		del_timer(&pDevice->TimerSQ3Tmax2);
+		del_timer(&pDevice->TimerSQ3Tmax3);
+	}
 
 #ifdef	TASK_LET
 	tasklet_kill(&pDevice->RxMngWorkItem);
 #endif
-     netif_stop_queue(dev);
-    pDevice->bCmdRunning = false;
-    MACbShutdown(pDevice->PortOffset);
-    MACbSoftwareReset(pDevice->PortOffset);
-    CARDbRadioPowerOff(pDevice);
-
-    pDevice->bLinkPass = false;
-    memset(pMgmt->abyCurrBSSID, 0, 6);
-    pMgmt->eCurrState = WMAC_STATE_IDLE;
-    device_free_td0_ring(pDevice);
-    device_free_td1_ring(pDevice);
-    device_free_rd0_ring(pDevice);
-    device_free_rd1_ring(pDevice);
-    device_free_frag_buf(pDevice);
-    device_free_rings(pDevice);
-    BSSvClearNodeDBTable(pDevice, 0);
-    free_irq(dev->irq, dev);
-    pDevice->flags &=(~DEVICE_FLAGS_OPENED);
+	netif_stop_queue(dev);
+	pDevice->bCmdRunning = false;
+	MACbShutdown(pDevice->PortOffset);
+	MACbSoftwareReset(pDevice->PortOffset);
+	CARDbRadioPowerOff(pDevice);
+
+	pDevice->bLinkPass = false;
+	memset(pMgmt->abyCurrBSSID, 0, 6);
+	pMgmt->eCurrState = WMAC_STATE_IDLE;
+	device_free_td0_ring(pDevice);
+	device_free_td1_ring(pDevice);
+	device_free_rd0_ring(pDevice);
+	device_free_rd1_ring(pDevice);
+	device_free_frag_buf(pDevice);
+	device_free_rings(pDevice);
+	BSSvClearNodeDBTable(pDevice, 0);
+	free_irq(dev->irq, dev);
+	pDevice->flags &= (~DEVICE_FLAGS_OPENED);
 	//2008-0714-01<Add>by chester
-device_release_WPADEV(pDevice);
+	device_release_WPADEV(pDevice);
 //PLICE_DEBUG->
 	//tasklet_kill(&pDevice->RxMngWorkItem);
 //PLICE_DEBUG<-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "device_close.. \n");
-    return 0;
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "device_close.. \n");
+	return 0;
 }
 
+static int device_dma0_tx_80211(struct sk_buff *skb, struct net_device *dev) {
+	PSDevice pDevice = netdev_priv(dev);
+	unsigned char *pbMPDU;
+	unsigned int cbMPDULen = 0;
 
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "device_dma0_tx_80211\n");
+	spin_lock_irq(&pDevice->lock);
 
-static int device_dma0_tx_80211(struct sk_buff *skb, struct net_device *dev) {
-    PSDevice        pDevice=netdev_priv(dev);
-    unsigned char *pbMPDU;
-    unsigned int cbMPDULen = 0;
+	if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 0) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "device_dma0_tx_80211, td0 <=0\n");
+		dev_kfree_skb_irq(skb);
+		spin_unlock_irq(&pDevice->lock);
+		return 0;
+	}
 
+	if (pDevice->bStopTx0Pkt == true) {
+		dev_kfree_skb_irq(skb);
+		spin_unlock_irq(&pDevice->lock);
+		return 0;
+	}
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "device_dma0_tx_80211\n");
-    spin_lock_irq(&pDevice->lock);
+	cbMPDULen = skb->len;
+	pbMPDU = skb->data;
 
-    if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 0) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "device_dma0_tx_80211, td0 <=0\n");
-        dev_kfree_skb_irq(skb);
-        spin_unlock_irq(&pDevice->lock);
-        return 0;
-    }
+	vDMA0_tx_80211(pDevice, skb, pbMPDU, cbMPDULen);
 
-    if (pDevice->bStopTx0Pkt == true) {
-        dev_kfree_skb_irq(skb);
-        spin_unlock_irq(&pDevice->lock);
-        return 0;
-    }
+	spin_unlock_irq(&pDevice->lock);
 
-    cbMPDULen = skb->len;
-    pbMPDU = skb->data;
+	return 0;
+}
 
-    vDMA0_tx_80211(pDevice, skb, pbMPDU, cbMPDULen);
+bool device_dma0_xmit(PSDevice pDevice, struct sk_buff *skb, unsigned int uNodeIndex) {
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	PSTxDesc        pHeadTD, pLastTD;
+	unsigned int cbFrameBodySize;
+	unsigned int uMACfragNum;
+	unsigned char byPktType;
+	bool bNeedEncryption = false;
+	PSKeyItem       pTransmitKey = NULL;
+	unsigned int cbHeaderSize;
+	unsigned int ii;
+	SKeyItem        STempKey;
+//    unsigned char byKeyIndex = 0;
 
-    spin_unlock_irq(&pDevice->lock);
+	if (pDevice->bStopTx0Pkt == true) {
+		dev_kfree_skb_irq(skb);
+		return false;
+	}
 
-    return 0;
+	if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 0) {
+		dev_kfree_skb_irq(skb);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "device_dma0_xmit, td0 <=0\n");
+		return false;
+	}
 
-}
+	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
+		if (pDevice->uAssocCount == 0) {
+			dev_kfree_skb_irq(skb);
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "device_dma0_xmit, assocCount = 0\n");
+			return false;
+		}
+	}
 
+	pHeadTD = pDevice->apCurrTD[TYPE_TXDMA0];
 
+	pHeadTD->m_td1TD1.byTCR = (TCR_EDP|TCR_STP);
 
-bool device_dma0_xmit(PSDevice pDevice, struct sk_buff *skb, unsigned int uNodeIndex) {
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    PSTxDesc        pHeadTD, pLastTD;
-    unsigned int cbFrameBodySize;
-    unsigned int uMACfragNum;
-    unsigned char byPktType;
-    bool bNeedEncryption = false;
-    PSKeyItem       pTransmitKey = NULL;
-    unsigned int cbHeaderSize;
-    unsigned int ii;
-    SKeyItem        STempKey;
-//    unsigned char byKeyIndex = 0;
+	memcpy(pDevice->sTxEthHeader.abyDstAddr, (unsigned char *)(skb->data), ETH_HLEN);
+	cbFrameBodySize = skb->len - ETH_HLEN;
+
+	// 802.1H
+	if (ntohs(pDevice->sTxEthHeader.wType) > ETH_DATA_LEN) {
+		cbFrameBodySize += 8;
+	}
+	uMACfragNum = cbGetFragCount(pDevice, pTransmitKey, cbFrameBodySize, &pDevice->sTxEthHeader);
+
+	if (uMACfragNum > AVAIL_TD(pDevice, TYPE_TXDMA0)) {
+		dev_kfree_skb_irq(skb);
+		return false;
+	}
+	byPktType = (unsigned char)pDevice->byPacketType;
+
+	if (pDevice->bFixRate) {
+		if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
+			if (pDevice->uConnectionRate >= RATE_11M) {
+				pDevice->wCurrentRate = RATE_11M;
+			} else {
+				pDevice->wCurrentRate = (unsigned short)pDevice->uConnectionRate;
+			}
+		} else {
+			if (pDevice->uConnectionRate >= RATE_54M)
+				pDevice->wCurrentRate = RATE_54M;
+			else
+				pDevice->wCurrentRate = (unsigned short)pDevice->uConnectionRate;
+		}
+	} else {
+		pDevice->wCurrentRate = pDevice->pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate;
+	}
+
+	//preamble type
+	if (pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble) {
+		pDevice->byPreambleType = pDevice->byShortPreamble;
+	} else {
+		pDevice->byPreambleType = PREAMBLE_LONG;
+	}
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dma0: pDevice->wCurrentRate = %d \n", pDevice->wCurrentRate);
+
+	if (pDevice->wCurrentRate <= RATE_11M) {
+		byPktType = PK_TYPE_11B;
+	} else if (pDevice->eCurrentPHYType == PHY_TYPE_11A) {
+		byPktType = PK_TYPE_11A;
+	} else {
+		if (pDevice->bProtectMode == true) {
+			byPktType = PK_TYPE_11GB;
+		} else {
+			byPktType = PK_TYPE_11GA;
+		}
+	}
+
+	if (pDevice->bEncryptionEnable == true)
+		bNeedEncryption = true;
+
+	if (pDevice->bEnableHostWEP) {
+		pTransmitKey = &STempKey;
+		pTransmitKey->byCipherSuite = pMgmt->sNodeDBTable[uNodeIndex].byCipherSuite;
+		pTransmitKey->dwKeyIndex = pMgmt->sNodeDBTable[uNodeIndex].dwKeyIndex;
+		pTransmitKey->uKeyLength = pMgmt->sNodeDBTable[uNodeIndex].uWepKeyLength;
+		pTransmitKey->dwTSC47_16 = pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16;
+		pTransmitKey->wTSC15_0 = pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0;
+		memcpy(pTransmitKey->abyKey,
+		       &pMgmt->sNodeDBTable[uNodeIndex].abyWepKey[0],
+		       pTransmitKey->uKeyLength
+			);
+	}
+	vGenerateFIFOHeader(pDevice, byPktType, pDevice->pbyTmpBuff, bNeedEncryption,
+			    cbFrameBodySize, TYPE_TXDMA0, pHeadTD,
+			    &pDevice->sTxEthHeader, (unsigned char *)skb->data, pTransmitKey, uNodeIndex,
+			    &uMACfragNum,
+			    &cbHeaderSize
+		);
+
+	if (MACbIsRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PS)) {
+		// Disable PS
+		MACbPSWakeup(pDevice->PortOffset);
+	}
 
+	pDevice->bPWBitOn = false;
+
+	pLastTD = pHeadTD;
+	for (ii = 0; ii < uMACfragNum; ii++) {
+		// Poll Transmit the adapter
+		wmb();
+		pHeadTD->m_td0TD0.f1Owner = OWNED_BY_NIC;
+		wmb();
+		if (ii == (uMACfragNum - 1))
+			pLastTD = pHeadTD;
+		pHeadTD = pHeadTD->next;
+	}
+
+	// Save the information needed by the tx interrupt handler
+	// to complete the Send request
+	pLastTD->pTDInfo->skb = skb;
+	pLastTD->pTDInfo->byFlags = 0;
+	pLastTD->pTDInfo->byFlags |= TD_FLAGS_NETIF_SKB;
+
+	pDevice->apCurrTD[TYPE_TXDMA0] = pHeadTD;
 
-    if (pDevice->bStopTx0Pkt == true) {
-        dev_kfree_skb_irq(skb);
-        return false;
-    }
-
-    if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 0) {
-        dev_kfree_skb_irq(skb);
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "device_dma0_xmit, td0 <=0\n");
-        return false;
-    }
-
-    if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-        if (pDevice->uAssocCount == 0) {
-            dev_kfree_skb_irq(skb);
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "device_dma0_xmit, assocCount = 0\n");
-            return false;
-        }
-    }
-
-    pHeadTD = pDevice->apCurrTD[TYPE_TXDMA0];
-
-    pHeadTD->m_td1TD1.byTCR = (TCR_EDP|TCR_STP);
-
-    memcpy(pDevice->sTxEthHeader.abyDstAddr, (unsigned char *)(skb->data), ETH_HLEN);
-    cbFrameBodySize = skb->len - ETH_HLEN;
-
-    // 802.1H
-    if (ntohs(pDevice->sTxEthHeader.wType) > ETH_DATA_LEN) {
-        cbFrameBodySize += 8;
-    }
-    uMACfragNum = cbGetFragCount(pDevice, pTransmitKey, cbFrameBodySize, &pDevice->sTxEthHeader);
-
-    if ( uMACfragNum > AVAIL_TD(pDevice, TYPE_TXDMA0)) {
-        dev_kfree_skb_irq(skb);
-        return false;
-    }
-    byPktType = (unsigned char)pDevice->byPacketType;
-
-
-    if (pDevice->bFixRate) {
-        if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
-            if (pDevice->uConnectionRate >= RATE_11M) {
-                pDevice->wCurrentRate = RATE_11M;
-            } else {
-                pDevice->wCurrentRate = (unsigned short)pDevice->uConnectionRate;
-            }
-        } else {
-            if (pDevice->uConnectionRate >= RATE_54M)
-                pDevice->wCurrentRate = RATE_54M;
-            else
-                pDevice->wCurrentRate = (unsigned short)pDevice->uConnectionRate;
-        }
-    }
-    else {
-        pDevice->wCurrentRate = pDevice->pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate;
-    }
-
-    //preamble type
-    if (pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble) {
-        pDevice->byPreambleType = pDevice->byShortPreamble;
-    }
-    else {
-        pDevice->byPreambleType = PREAMBLE_LONG;
-    }
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dma0: pDevice->wCurrentRate = %d \n", pDevice->wCurrentRate);
-
-
-    if (pDevice->wCurrentRate <= RATE_11M) {
-        byPktType = PK_TYPE_11B;
-    } else if (pDevice->eCurrentPHYType == PHY_TYPE_11A) {
-        byPktType = PK_TYPE_11A;
-    } else {
-        if (pDevice->bProtectMode == true) {
-            byPktType = PK_TYPE_11GB;
-        } else {
-            byPktType = PK_TYPE_11GA;
-        }
-    }
-
-    if (pDevice->bEncryptionEnable == true)
-        bNeedEncryption = true;
-
-    if (pDevice->bEnableHostWEP) {
-        pTransmitKey = &STempKey;
-        pTransmitKey->byCipherSuite = pMgmt->sNodeDBTable[uNodeIndex].byCipherSuite;
-        pTransmitKey->dwKeyIndex = pMgmt->sNodeDBTable[uNodeIndex].dwKeyIndex;
-        pTransmitKey->uKeyLength = pMgmt->sNodeDBTable[uNodeIndex].uWepKeyLength;
-        pTransmitKey->dwTSC47_16 = pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16;
-        pTransmitKey->wTSC15_0 = pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0;
-        memcpy(pTransmitKey->abyKey,
-            &pMgmt->sNodeDBTable[uNodeIndex].abyWepKey[0],
-            pTransmitKey->uKeyLength
-            );
-    }
-    vGenerateFIFOHeader(pDevice, byPktType, pDevice->pbyTmpBuff, bNeedEncryption,
-                        cbFrameBodySize, TYPE_TXDMA0, pHeadTD,
-                        &pDevice->sTxEthHeader, (unsigned char *)skb->data, pTransmitKey, uNodeIndex,
-                        &uMACfragNum,
-                        &cbHeaderSize
-                        );
-
-    if (MACbIsRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PS)) {
-        // Disable PS
-        MACbPSWakeup(pDevice->PortOffset);
-    }
-
-    pDevice->bPWBitOn = false;
-
-    pLastTD = pHeadTD;
-    for (ii = 0; ii < uMACfragNum; ii++) {
-        // Poll Transmit the adapter
-        wmb();
-        pHeadTD->m_td0TD0.f1Owner=OWNED_BY_NIC;
-        wmb();
-        if (ii == (uMACfragNum - 1))
-            pLastTD = pHeadTD;
-        pHeadTD = pHeadTD->next;
-    }
-
-    // Save the information needed by the tx interrupt handler
-    // to complete the Send request
-    pLastTD->pTDInfo->skb = skb;
-    pLastTD->pTDInfo->byFlags = 0;
-    pLastTD->pTDInfo->byFlags |= TD_FLAGS_NETIF_SKB;
-
-    pDevice->apCurrTD[TYPE_TXDMA0] = pHeadTD;
-
-    MACvTransmit0(pDevice->PortOffset);
-
-
-    return true;
+	MACvTransmit0(pDevice->PortOffset);
+
+	return true;
 }
 
 //TYPE_AC0DMA data tx
 static int  device_xmit(struct sk_buff *skb, struct net_device *dev) {
-    PSDevice pDevice=netdev_priv(dev);
-
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    PSTxDesc        pHeadTD, pLastTD;
-    unsigned int uNodeIndex = 0;
-    unsigned char byMask[8] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80};
-    unsigned short wAID;
-    unsigned int uMACfragNum = 1;
-    unsigned int cbFrameBodySize;
-    unsigned char byPktType;
-    unsigned int cbHeaderSize;
-    bool bNeedEncryption = false;
-    PSKeyItem       pTransmitKey = NULL;
-    SKeyItem        STempKey;
-    unsigned int ii;
-    bool bTKIP_UseGTK = false;
-    bool bNeedDeAuth = false;
-    unsigned char *pbyBSSID;
-    bool bNodeExist = false;
-
-
-
-    spin_lock_irq(&pDevice->lock);
-    if (pDevice->bLinkPass == false) {
-        dev_kfree_skb_irq(skb);
-        spin_unlock_irq(&pDevice->lock);
-        return 0;
-    }
-
-    if (pDevice->bStopDataPkt) {
-        dev_kfree_skb_irq(skb);
-        spin_unlock_irq(&pDevice->lock);
-        return 0;
-    }
-
-
-    if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-        if (pDevice->uAssocCount == 0) {
-            dev_kfree_skb_irq(skb);
-            spin_unlock_irq(&pDevice->lock);
-            return 0;
-        }
-        if (is_multicast_ether_addr((unsigned char *)(skb->data))) {
-            uNodeIndex = 0;
-            bNodeExist = true;
-            if (pMgmt->sNodeDBTable[0].bPSEnable) {
-                skb_queue_tail(&(pMgmt->sNodeDBTable[0].sTxPSQueue), skb);
-                pMgmt->sNodeDBTable[0].wEnQueueCnt++;
-                // set tx map
-                pMgmt->abyPSTxMap[0] |= byMask[0];
-                spin_unlock_irq(&pDevice->lock);
-                return 0;
-            }
-}else {
-            if (BSSDBbIsSTAInNodeDB(pMgmt, (unsigned char *)(skb->data), &uNodeIndex)) {
-                if (pMgmt->sNodeDBTable[uNodeIndex].bPSEnable) {
-                    skb_queue_tail(&pMgmt->sNodeDBTable[uNodeIndex].sTxPSQueue, skb);
-                    pMgmt->sNodeDBTable[uNodeIndex].wEnQueueCnt++;
-                    // set tx map
-                    wAID = pMgmt->sNodeDBTable[uNodeIndex].wAID;
-                    pMgmt->abyPSTxMap[wAID >> 3] |=  byMask[wAID & 7];
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set:pMgmt->abyPSTxMap[%d]= %d\n",
-                             (wAID >> 3), pMgmt->abyPSTxMap[wAID >> 3]);
-                    spin_unlock_irq(&pDevice->lock);
-                    return 0;
-                }
-
-                if (pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble) {
-                    pDevice->byPreambleType = pDevice->byShortPreamble;
-
-                }else {
-                    pDevice->byPreambleType = PREAMBLE_LONG;
-                }
-                bNodeExist = true;
-
-            }
-        }
-
-        if (bNodeExist == false) {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG"Unknown STA not found in node DB \n");
-            dev_kfree_skb_irq(skb);
-            spin_unlock_irq(&pDevice->lock);
-            return 0;
-        }
-    }
-
-    pHeadTD = pDevice->apCurrTD[TYPE_AC0DMA];
-
-    pHeadTD->m_td1TD1.byTCR = (TCR_EDP|TCR_STP);
-
-
-    memcpy(pDevice->sTxEthHeader.abyDstAddr, (unsigned char *)(skb->data), ETH_HLEN);
-    cbFrameBodySize = skb->len - ETH_HLEN;
-    // 802.1H
-    if (ntohs(pDevice->sTxEthHeader.wType) > ETH_DATA_LEN) {
-        cbFrameBodySize += 8;
-    }
-
-
-    if (pDevice->bEncryptionEnable == true) {
-        bNeedEncryption = true;
-        // get Transmit key
-        do {
-            if ((pDevice->pMgmt->eCurrMode == WMAC_MODE_ESS_STA) &&
-                (pDevice->pMgmt->eCurrState == WMAC_STATE_ASSOC)) {
-                pbyBSSID = pDevice->abyBSSID;
-                // get pairwise key
-                if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, PAIRWISE_KEY, &pTransmitKey) == false) {
-                    // get group key
-                    if(KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, GROUP_KEY, &pTransmitKey) == true) {
-                        bTKIP_UseGTK = true;
-                        DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG"Get GTK.\n");
-                        break;
-                    }
-                } else {
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG"Get PTK.\n");
-                    break;
-                }
-            }else if (pDevice->pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
-
-                pbyBSSID = pDevice->sTxEthHeader.abyDstAddr;  //TO_DS = 0 and FROM_DS = 0 --> 802.11 MAC Address1
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG"IBSS Serach Key: \n");
-                for (ii = 0; ii< 6; ii++)
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG"%x \n", *(pbyBSSID+ii));
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG"\n");
-
-                // get pairwise key
-                if(KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, PAIRWISE_KEY, &pTransmitKey) == true)
-                    break;
-            }
-            // get group key
-            pbyBSSID = pDevice->abyBroadcastAddr;
-            if(KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, GROUP_KEY, &pTransmitKey) == false) {
-                pTransmitKey = NULL;
-                if (pDevice->pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG"IBSS and KEY is NULL. [%d]\n", pDevice->pMgmt->eCurrMode);
-                }
-                else
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG"NOT IBSS and KEY is NULL. [%d]\n", pDevice->pMgmt->eCurrMode);
-            } else {
-                bTKIP_UseGTK = true;
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG"Get GTK.\n");
-            }
-        } while(false);
-    }
-
-    if (pDevice->bEnableHostWEP) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG"acdma0: STA index %d\n", uNodeIndex);
-        if (pDevice->bEncryptionEnable == true) {
-            pTransmitKey = &STempKey;
-            pTransmitKey->byCipherSuite = pMgmt->sNodeDBTable[uNodeIndex].byCipherSuite;
-            pTransmitKey->dwKeyIndex = pMgmt->sNodeDBTable[uNodeIndex].dwKeyIndex;
-            pTransmitKey->uKeyLength = pMgmt->sNodeDBTable[uNodeIndex].uWepKeyLength;
-            pTransmitKey->dwTSC47_16 = pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16;
-            pTransmitKey->wTSC15_0 = pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0;
-            memcpy(pTransmitKey->abyKey,
-                &pMgmt->sNodeDBTable[uNodeIndex].abyWepKey[0],
-                pTransmitKey->uKeyLength
-                );
-         }
-    }
-
-    uMACfragNum = cbGetFragCount(pDevice, pTransmitKey, cbFrameBodySize, &pDevice->sTxEthHeader);
-
-    if (uMACfragNum > AVAIL_TD(pDevice, TYPE_AC0DMA)) {
-        DBG_PRT(MSG_LEVEL_ERR, KERN_DEBUG "uMACfragNum > AVAIL_TD(TYPE_AC0DMA) = %d\n", uMACfragNum);
-        dev_kfree_skb_irq(skb);
-        spin_unlock_irq(&pDevice->lock);
-        return 0;
-    }
-
-    if (pTransmitKey != NULL) {
-        if ((pTransmitKey->byCipherSuite == KEY_CTL_WEP) &&
-            (pTransmitKey->uKeyLength == WLAN_WEP232_KEYLEN)) {
-            uMACfragNum = 1; //WEP256 doesn't support fragment
-        }
-    }
-
-    byPktType = (unsigned char)pDevice->byPacketType;
-
-    if (pDevice->bFixRate) {
+	PSDevice pDevice = netdev_priv(dev);
+
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	PSTxDesc        pHeadTD, pLastTD;
+	unsigned int uNodeIndex = 0;
+	unsigned char byMask[8] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80};
+	unsigned short wAID;
+	unsigned int uMACfragNum = 1;
+	unsigned int cbFrameBodySize;
+	unsigned char byPktType;
+	unsigned int cbHeaderSize;
+	bool bNeedEncryption = false;
+	PSKeyItem       pTransmitKey = NULL;
+	SKeyItem        STempKey;
+	unsigned int ii;
+	bool bTKIP_UseGTK = false;
+	bool bNeedDeAuth = false;
+	unsigned char *pbyBSSID;
+	bool bNodeExist = false;
+
+	spin_lock_irq(&pDevice->lock);
+	if (pDevice->bLinkPass == false) {
+		dev_kfree_skb_irq(skb);
+		spin_unlock_irq(&pDevice->lock);
+		return 0;
+	}
+
+	if (pDevice->bStopDataPkt) {
+		dev_kfree_skb_irq(skb);
+		spin_unlock_irq(&pDevice->lock);
+		return 0;
+	}
+
+	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
+		if (pDevice->uAssocCount == 0) {
+			dev_kfree_skb_irq(skb);
+			spin_unlock_irq(&pDevice->lock);
+			return 0;
+		}
+		if (is_multicast_ether_addr((unsigned char *)(skb->data))) {
+			uNodeIndex = 0;
+			bNodeExist = true;
+			if (pMgmt->sNodeDBTable[0].bPSEnable) {
+				skb_queue_tail(&(pMgmt->sNodeDBTable[0].sTxPSQueue), skb);
+				pMgmt->sNodeDBTable[0].wEnQueueCnt++;
+				// set tx map
+				pMgmt->abyPSTxMap[0] |= byMask[0];
+				spin_unlock_irq(&pDevice->lock);
+				return 0;
+			}
+		} else {
+			if (BSSDBbIsSTAInNodeDB(pMgmt, (unsigned char *)(skb->data), &uNodeIndex)) {
+				if (pMgmt->sNodeDBTable[uNodeIndex].bPSEnable) {
+					skb_queue_tail(&pMgmt->sNodeDBTable[uNodeIndex].sTxPSQueue, skb);
+					pMgmt->sNodeDBTable[uNodeIndex].wEnQueueCnt++;
+					// set tx map
+					wAID = pMgmt->sNodeDBTable[uNodeIndex].wAID;
+					pMgmt->abyPSTxMap[wAID >> 3] |=  byMask[wAID & 7];
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set:pMgmt->abyPSTxMap[%d]= %d\n",
+						(wAID >> 3), pMgmt->abyPSTxMap[wAID >> 3]);
+					spin_unlock_irq(&pDevice->lock);
+					return 0;
+				}
+
+				if (pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble) {
+					pDevice->byPreambleType = pDevice->byShortPreamble;
+
+				} else {
+					pDevice->byPreambleType = PREAMBLE_LONG;
+				}
+				bNodeExist = true;
+
+			}
+		}
+
+		if (bNodeExist == false) {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG "Unknown STA not found in node DB \n");
+			dev_kfree_skb_irq(skb);
+			spin_unlock_irq(&pDevice->lock);
+			return 0;
+		}
+	}
+
+	pHeadTD = pDevice->apCurrTD[TYPE_AC0DMA];
+
+	pHeadTD->m_td1TD1.byTCR = (TCR_EDP|TCR_STP);
+
+	memcpy(pDevice->sTxEthHeader.abyDstAddr, (unsigned char *)(skb->data), ETH_HLEN);
+	cbFrameBodySize = skb->len - ETH_HLEN;
+	// 802.1H
+	if (ntohs(pDevice->sTxEthHeader.wType) > ETH_DATA_LEN) {
+		cbFrameBodySize += 8;
+	}
+
+	if (pDevice->bEncryptionEnable == true) {
+		bNeedEncryption = true;
+		// get Transmit key
+		do {
+			if ((pDevice->pMgmt->eCurrMode == WMAC_MODE_ESS_STA) &&
+			    (pDevice->pMgmt->eCurrState == WMAC_STATE_ASSOC)) {
+				pbyBSSID = pDevice->abyBSSID;
+				// get pairwise key
+				if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, PAIRWISE_KEY, &pTransmitKey) == false) {
+					// get group key
+					if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, GROUP_KEY, &pTransmitKey) == true) {
+						bTKIP_UseGTK = true;
+						DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG "Get GTK.\n");
+						break;
+					}
+				} else {
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG "Get PTK.\n");
+					break;
+				}
+			} else if (pDevice->pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
+				pbyBSSID = pDevice->sTxEthHeader.abyDstAddr;  //TO_DS = 0 and FROM_DS = 0 --> 802.11 MAC Address1
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG "IBSS Serach Key: \n");
+				for (ii = 0; ii < 6; ii++)
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG "%x \n", *(pbyBSSID+ii));
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG "\n");
+
+				// get pairwise key
+				if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, PAIRWISE_KEY, &pTransmitKey) == true)
+					break;
+			}
+			// get group key
+			pbyBSSID = pDevice->abyBroadcastAddr;
+			if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, GROUP_KEY, &pTransmitKey) == false) {
+				pTransmitKey = NULL;
+				if (pDevice->pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG "IBSS and KEY is NULL. [%d]\n", pDevice->pMgmt->eCurrMode);
+				} else
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG "NOT IBSS and KEY is NULL. [%d]\n", pDevice->pMgmt->eCurrMode);
+			} else {
+				bTKIP_UseGTK = true;
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG "Get GTK.\n");
+			}
+		} while (false);
+	}
+
+	if (pDevice->bEnableHostWEP) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG "acdma0: STA index %d\n", uNodeIndex);
+		if (pDevice->bEncryptionEnable == true) {
+			pTransmitKey = &STempKey;
+			pTransmitKey->byCipherSuite = pMgmt->sNodeDBTable[uNodeIndex].byCipherSuite;
+			pTransmitKey->dwKeyIndex = pMgmt->sNodeDBTable[uNodeIndex].dwKeyIndex;
+			pTransmitKey->uKeyLength = pMgmt->sNodeDBTable[uNodeIndex].uWepKeyLength;
+			pTransmitKey->dwTSC47_16 = pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16;
+			pTransmitKey->wTSC15_0 = pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0;
+			memcpy(pTransmitKey->abyKey,
+			       &pMgmt->sNodeDBTable[uNodeIndex].abyWepKey[0],
+			       pTransmitKey->uKeyLength
+				);
+		}
+	}
+
+	uMACfragNum = cbGetFragCount(pDevice, pTransmitKey, cbFrameBodySize, &pDevice->sTxEthHeader);
+
+	if (uMACfragNum > AVAIL_TD(pDevice, TYPE_AC0DMA)) {
+		DBG_PRT(MSG_LEVEL_ERR, KERN_DEBUG "uMACfragNum > AVAIL_TD(TYPE_AC0DMA) = %d\n", uMACfragNum);
+		dev_kfree_skb_irq(skb);
+		spin_unlock_irq(&pDevice->lock);
+		return 0;
+	}
+
+	if (pTransmitKey != NULL) {
+		if ((pTransmitKey->byCipherSuite == KEY_CTL_WEP) &&
+		    (pTransmitKey->uKeyLength == WLAN_WEP232_KEYLEN)) {
+			uMACfragNum = 1; //WEP256 doesn't support fragment
+		}
+	}
+
+	byPktType = (unsigned char)pDevice->byPacketType;
+
+	if (pDevice->bFixRate) {
 #ifdef	PLICE_DEBUG
-	printk("Fix Rate: PhyType is %d,ConnectionRate is %d\n",pDevice->eCurrentPHYType,pDevice->uConnectionRate);
+		printk("Fix Rate: PhyType is %d,ConnectionRate is %d\n", pDevice->eCurrentPHYType, pDevice->uConnectionRate);
 #endif
 
-        if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
-            if (pDevice->uConnectionRate >= RATE_11M) {
-                pDevice->wCurrentRate = RATE_11M;
-            } else {
-                pDevice->wCurrentRate = (unsigned short)pDevice->uConnectionRate;
-            }
-        } else {
-            if ((pDevice->eCurrentPHYType == PHY_TYPE_11A) &&
-                (pDevice->uConnectionRate <= RATE_6M)) {
-                pDevice->wCurrentRate = RATE_6M;
-            } else {
-                if (pDevice->uConnectionRate >= RATE_54M)
-                    pDevice->wCurrentRate = RATE_54M;
-                else
-                    pDevice->wCurrentRate = (unsigned short)pDevice->uConnectionRate;
-
-            }
-        }
-        pDevice->byACKRate = (unsigned char) pDevice->wCurrentRate;
-        pDevice->byTopCCKBasicRate = RATE_1M;
-        pDevice->byTopOFDMBasicRate = RATE_6M;
-    }
-    else {
-        //auto rate
-    if (pDevice->sTxEthHeader.wType == TYPE_PKT_802_1x) {
-            if (pDevice->eCurrentPHYType != PHY_TYPE_11A) {
-                pDevice->wCurrentRate = RATE_1M;
-                pDevice->byACKRate = RATE_1M;
-                pDevice->byTopCCKBasicRate = RATE_1M;
-                pDevice->byTopOFDMBasicRate = RATE_6M;
-            } else {
-                pDevice->wCurrentRate = RATE_6M;
-                pDevice->byACKRate = RATE_6M;
-                pDevice->byTopCCKBasicRate = RATE_1M;
-                pDevice->byTopOFDMBasicRate = RATE_6M;
-            }
-        }
-        else {
-		VNTWIFIvGetTxRate(  pDevice->pMgmt,
-                                pDevice->sTxEthHeader.abyDstAddr,
-                                &(pDevice->wCurrentRate),
-                                &(pDevice->byACKRate),
-                                &(pDevice->byTopCCKBasicRate),
-                                &(pDevice->byTopOFDMBasicRate));
+		if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
+			if (pDevice->uConnectionRate >= RATE_11M) {
+				pDevice->wCurrentRate = RATE_11M;
+			} else {
+				pDevice->wCurrentRate = (unsigned short)pDevice->uConnectionRate;
+			}
+		} else {
+			if ((pDevice->eCurrentPHYType == PHY_TYPE_11A) &&
+			    (pDevice->uConnectionRate <= RATE_6M)) {
+				pDevice->wCurrentRate = RATE_6M;
+			} else {
+				if (pDevice->uConnectionRate >= RATE_54M)
+					pDevice->wCurrentRate = RATE_54M;
+				else
+					pDevice->wCurrentRate = (unsigned short)pDevice->uConnectionRate;
 
+			}
+		}
+		pDevice->byACKRate = (unsigned char) pDevice->wCurrentRate;
+		pDevice->byTopCCKBasicRate = RATE_1M;
+		pDevice->byTopOFDMBasicRate = RATE_6M;
+	} else {
+		//auto rate
+		if (pDevice->sTxEthHeader.wType == TYPE_PKT_802_1x) {
+			if (pDevice->eCurrentPHYType != PHY_TYPE_11A) {
+				pDevice->wCurrentRate = RATE_1M;
+				pDevice->byACKRate = RATE_1M;
+				pDevice->byTopCCKBasicRate = RATE_1M;
+				pDevice->byTopOFDMBasicRate = RATE_6M;
+			} else {
+				pDevice->wCurrentRate = RATE_6M;
+				pDevice->byACKRate = RATE_6M;
+				pDevice->byTopCCKBasicRate = RATE_1M;
+				pDevice->byTopOFDMBasicRate = RATE_6M;
+			}
+		} else {
+			VNTWIFIvGetTxRate(pDevice->pMgmt,
+					  pDevice->sTxEthHeader.abyDstAddr,
+					  &(pDevice->wCurrentRate),
+					  &(pDevice->byACKRate),
+					  &(pDevice->byTopCCKBasicRate),
+					  &(pDevice->byTopOFDMBasicRate));
 
 		}
-    }
+	}
 
 //    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "acdma0: pDevice->wCurrentRate = %d \n", pDevice->wCurrentRate);
 
-    if (pDevice->wCurrentRate <= RATE_11M) {
-        byPktType = PK_TYPE_11B;
-    } else if (pDevice->eCurrentPHYType == PHY_TYPE_11A) {
-        byPktType = PK_TYPE_11A;
-    } else {
-        if (pDevice->bProtectMode == true) {
-            byPktType = PK_TYPE_11GB;
-        } else {
-            byPktType = PK_TYPE_11GA;
-        }
-    }
+	if (pDevice->wCurrentRate <= RATE_11M) {
+		byPktType = PK_TYPE_11B;
+	} else if (pDevice->eCurrentPHYType == PHY_TYPE_11A) {
+		byPktType = PK_TYPE_11A;
+	} else {
+		if (pDevice->bProtectMode == true) {
+			byPktType = PK_TYPE_11GB;
+		} else {
+			byPktType = PK_TYPE_11GA;
+		}
+	}
 
 //#ifdef	PLICE_DEBUG
 //	printk("FIX RATE:CurrentRate is %d");
 //#endif
 
-    if (bNeedEncryption == true) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ntohs Pkt Type=%04x\n", ntohs(pDevice->sTxEthHeader.wType));
-        if ((pDevice->sTxEthHeader.wType) == TYPE_PKT_802_1x) {
-            bNeedEncryption = false;
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Pkt Type=%04x\n", (pDevice->sTxEthHeader.wType));
-            if ((pDevice->pMgmt->eCurrMode == WMAC_MODE_ESS_STA) && (pDevice->pMgmt->eCurrState == WMAC_STATE_ASSOC)) {
-                if (pTransmitKey == NULL) {
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Don't Find TX KEY\n");
-                }
-                else {
-                    if (bTKIP_UseGTK == true) {
-                        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"error: KEY is GTK!!~~\n");
-                    }
-                    else {
-                        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Find PTK [%lX]\n", pTransmitKey->dwKeyIndex);
-                        bNeedEncryption = true;
-                    }
-                }
-            }
-
-            if (pDevice->byCntMeasure == 2) {
-                bNeedDeAuth = true;
-                pDevice->s802_11Counter.TKIPCounterMeasuresInvoked++;
-            }
-
-            if (pDevice->bEnableHostWEP) {
-                if ((uNodeIndex != 0) &&
-                    (pMgmt->sNodeDBTable[uNodeIndex].dwKeyIndex & PAIRWISE_KEY)) {
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Find PTK [%lX]\n", pTransmitKey->dwKeyIndex);
-                    bNeedEncryption = true;
-                 }
-             }
-        }
-        else {
-            if (pTransmitKey == NULL) {
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"return no tx key\n");
-                dev_kfree_skb_irq(skb);
-                spin_unlock_irq(&pDevice->lock);
-                return 0;
-            }
-        }
-    }
+	if (bNeedEncryption == true) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ntohs Pkt Type=%04x\n", ntohs(pDevice->sTxEthHeader.wType));
+		if ((pDevice->sTxEthHeader.wType) == TYPE_PKT_802_1x) {
+			bNeedEncryption = false;
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Pkt Type=%04x\n", (pDevice->sTxEthHeader.wType));
+			if ((pDevice->pMgmt->eCurrMode == WMAC_MODE_ESS_STA) && (pDevice->pMgmt->eCurrState == WMAC_STATE_ASSOC)) {
+				if (pTransmitKey == NULL) {
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Don't Find TX KEY\n");
+				} else {
+					if (bTKIP_UseGTK == true) {
+						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "error: KEY is GTK!!~~\n");
+					} else {
+						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Find PTK [%lX]\n", pTransmitKey->dwKeyIndex);
+						bNeedEncryption = true;
+					}
+				}
+			}
 
+			if (pDevice->byCntMeasure == 2) {
+				bNeedDeAuth = true;
+				pDevice->s802_11Counter.TKIPCounterMeasuresInvoked++;
+			}
 
-#ifdef	PLICE_DEBUG
-	//if (skb->len == 98)
-	//{
-	//	printk("ping:len is %d\n");
-	//}
-#endif
-    vGenerateFIFOHeader(pDevice, byPktType, pDevice->pbyTmpBuff, bNeedEncryption,
-                        cbFrameBodySize, TYPE_AC0DMA, pHeadTD,
-                        &pDevice->sTxEthHeader, (unsigned char *)skb->data, pTransmitKey, uNodeIndex,
-                        &uMACfragNum,
-                        &cbHeaderSize
-                        );
-
-    if (MACbIsRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PS)) {
-        // Disable PS
-        MACbPSWakeup(pDevice->PortOffset);
-    }
-    pDevice->bPWBitOn = false;
-
-    pLastTD = pHeadTD;
-    for (ii = 0; ii < uMACfragNum; ii++) {
-        // Poll Transmit the adapter
-        wmb();
-        pHeadTD->m_td0TD0.f1Owner=OWNED_BY_NIC;
-        wmb();
-        if (ii == uMACfragNum - 1)
-            pLastTD = pHeadTD;
-        pHeadTD = pHeadTD->next;
-    }
-
-    // Save the information needed by the tx interrupt handler
-    // to complete the Send request
-    pLastTD->pTDInfo->skb = skb;
-    pLastTD->pTDInfo->byFlags = 0;
-    pLastTD->pTDInfo->byFlags |= TD_FLAGS_NETIF_SKB;
+			if (pDevice->bEnableHostWEP) {
+				if ((uNodeIndex != 0) &&
+				    (pMgmt->sNodeDBTable[uNodeIndex].dwKeyIndex & PAIRWISE_KEY)) {
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Find PTK [%lX]\n", pTransmitKey->dwKeyIndex);
+					bNeedEncryption = true;
+				}
+			}
+		} else {
+			if (pTransmitKey == NULL) {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "return no tx key\n");
+				dev_kfree_skb_irq(skb);
+				spin_unlock_irq(&pDevice->lock);
+				return 0;
+			}
+		}
+	}
+
+	vGenerateFIFOHeader(pDevice, byPktType, pDevice->pbyTmpBuff, bNeedEncryption,
+			    cbFrameBodySize, TYPE_AC0DMA, pHeadTD,
+			    &pDevice->sTxEthHeader, (unsigned char *)skb->data, pTransmitKey, uNodeIndex,
+			    &uMACfragNum,
+			    &cbHeaderSize
+		);
+
+	if (MACbIsRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PS)) {
+		// Disable PS
+		MACbPSWakeup(pDevice->PortOffset);
+	}
+	pDevice->bPWBitOn = false;
+
+	pLastTD = pHeadTD;
+	for (ii = 0; ii < uMACfragNum; ii++) {
+		// Poll Transmit the adapter
+		wmb();
+		pHeadTD->m_td0TD0.f1Owner = OWNED_BY_NIC;
+		wmb();
+		if (ii == uMACfragNum - 1)
+			pLastTD = pHeadTD;
+		pHeadTD = pHeadTD->next;
+	}
+
+	// Save the information needed by the tx interrupt handler
+	// to complete the Send request
+	pLastTD->pTDInfo->skb = skb;
+	pLastTD->pTDInfo->byFlags = 0;
+	pLastTD->pTDInfo->byFlags |= TD_FLAGS_NETIF_SKB;
 #ifdef TxInSleep
-  pDevice->nTxDataTimeCout=0; //2008-8-21 chester <add> for send null packet
-  #endif
-    if (AVAIL_TD(pDevice, TYPE_AC0DMA) <= 1) {
-        netif_stop_queue(dev);
-    }
+	pDevice->nTxDataTimeCout = 0; //2008-8-21 chester <add> for send null packet
+#endif
+	if (AVAIL_TD(pDevice, TYPE_AC0DMA) <= 1) {
+		netif_stop_queue(dev);
+	}
 
-    pDevice->apCurrTD[TYPE_AC0DMA] = pHeadTD;
+	pDevice->apCurrTD[TYPE_AC0DMA] = pHeadTD;
 //#ifdef	PLICE_DEBUG
-	if (pDevice->bFixRate)
-	{
-		printk("FixRate:Rate is %d,TxPower is %d\n",pDevice->wCurrentRate,pDevice->byCurPwr);
-	}
-	else
-	{
-		//printk("Auto Rate:Rate is %d,TxPower is %d\n",pDevice->wCurrentRate,pDevice->byCurPwr);
+	if (pDevice->bFixRate) {
+		printk("FixRate:Rate is %d,TxPower is %d\n", pDevice->wCurrentRate, pDevice->byCurPwr);
+	} else {
 	}
 //#endif
 
-{
-    unsigned char Protocol_Version;    //802.1x Authentication
-    unsigned char Packet_Type;           //802.1x Authentication
-    unsigned char Descriptor_type;
-    unsigned short Key_info;
-bool bTxeapol_key = false;
-    Protocol_Version = skb->data[ETH_HLEN];
-    Packet_Type = skb->data[ETH_HLEN+1];
-    Descriptor_type = skb->data[ETH_HLEN+1+1+2];
-    Key_info = (skb->data[ETH_HLEN+1+1+2+1] << 8)|(skb->data[ETH_HLEN+1+1+2+2]);
-   if (pDevice->sTxEthHeader.wType == TYPE_PKT_802_1x) {
-           if(((Protocol_Version==1) ||(Protocol_Version==2)) &&
-	        (Packet_Type==3)) {  //802.1x OR eapol-key challenge frame transfer
-                        bTxeapol_key = true;
-		if((Descriptor_type==254)||(Descriptor_type==2)) {       //WPA or RSN
-                       if(!(Key_info & BIT3) &&   //group-key challenge
-			   (Key_info & BIT8) && (Key_info & BIT9)) {    //send 2/2 key
-			  pDevice->fWPA_Authened = true;
-			  if(Descriptor_type==254)
-			      printk("WPA ");
-			  else
-			      printk("WPA2 ");
-			  printk("Authentication completed!!\n");
-                        }
-		 }
-             }
-   }
-}
+	{
+		unsigned char Protocol_Version;    //802.1x Authentication
+		unsigned char Packet_Type;           //802.1x Authentication
+		unsigned char Descriptor_type;
+		unsigned short Key_info;
+		bool bTxeapol_key = false;
+		Protocol_Version = skb->data[ETH_HLEN];
+		Packet_Type = skb->data[ETH_HLEN+1];
+		Descriptor_type = skb->data[ETH_HLEN+1+1+2];
+		Key_info = (skb->data[ETH_HLEN+1+1+2+1] << 8)|(skb->data[ETH_HLEN+1+1+2+2]);
+		if (pDevice->sTxEthHeader.wType == TYPE_PKT_802_1x) {
+			if (((Protocol_Version == 1) || (Protocol_Version == 2)) &&
+			    (Packet_Type == 3)) {  //802.1x OR eapol-key challenge frame transfer
+				bTxeapol_key = true;
+				if ((Descriptor_type == 254) || (Descriptor_type == 2)) {       //WPA or RSN
+					if (!(Key_info & BIT3) &&   //group-key challenge
+					    (Key_info & BIT8) && (Key_info & BIT9)) {    //send 2/2 key
+						pDevice->fWPA_Authened = true;
+						if (Descriptor_type == 254)
+							printk("WPA ");
+						else
+							printk("WPA2 ");
+						printk("Authentication completed!!\n");
+					}
+				}
+			}
+		}
+	}
 
-    MACvTransmitAC0(pDevice->PortOffset);
+	MACvTransmitAC0(pDevice->PortOffset);
 //    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "acdma0:pDevice->apCurrTD= %p\n", pHeadTD);
 
-    dev->trans_start = jiffies;
-
-    spin_unlock_irq(&pDevice->lock);
-    return 0;
+	dev->trans_start = jiffies;
 
+	spin_unlock_irq(&pDevice->lock);
+	return 0;
 }
 
 static  irqreturn_t  device_intr(int irq,  void *dev_instance) {
-    struct net_device* dev=dev_instance;
-    PSDevice     pDevice=(PSDevice) netdev_priv(dev);
-
-    int             max_count=0;
-    unsigned long dwMIBCounter=0;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    unsigned char byOrgPageSel=0;
-    int             handled = 0;
-    unsigned char byData = 0;
-    int             ii= 0;
+	struct net_device *dev = dev_instance;
+	PSDevice     pDevice = (PSDevice)netdev_priv(dev);
+
+	int             max_count = 0;
+	unsigned long dwMIBCounter = 0;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	unsigned char byOrgPageSel = 0;
+	int             handled = 0;
+	unsigned char byData = 0;
+	int             ii = 0;
 //    unsigned char byRSSI;
 
+	MACvReadISR(pDevice->PortOffset, &pDevice->dwIsr);
 
-    MACvReadISR(pDevice->PortOffset, &pDevice->dwIsr);
-
-    if (pDevice->dwIsr == 0)
-        return IRQ_RETVAL(handled);
-
-    if (pDevice->dwIsr == 0xffffffff) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dwIsr = 0xffff\n");
-        return IRQ_RETVAL(handled);
-    }
-    /*
-      // 2008-05-21 <mark> by Richardtai, we can't read RSSI here, because no packet bound with RSSI
-
-    	if ((pDevice->dwIsr & ISR_RXDMA0) &&
-        (pDevice->byLocalID != REV_ID_VT3253_B0) &&
-        (pDevice->bBSSIDFilter == true)) {
-        // update RSSI
-        //BBbReadEmbedded(pDevice->PortOffset, 0x3E, &byRSSI);
-        //pDevice->uCurrRSSI = byRSSI;
-    }
-    */
-
-    handled = 1;
-    MACvIntDisable(pDevice->PortOffset);
-    spin_lock_irq(&pDevice->lock);
-
-    //Make sure current page is 0
-    VNSvInPortB(pDevice->PortOffset + MAC_REG_PAGE1SEL, &byOrgPageSel);
-    if (byOrgPageSel == 1) {
-        MACvSelectPage0(pDevice->PortOffset);
-    }
-    else
-        byOrgPageSel = 0;
-
-    MACvReadMIBCounter(pDevice->PortOffset, &dwMIBCounter);
-    // TBD....
-    // Must do this after doing rx/tx, cause ISR bit is slow
-    // than RD/TD write back
-    // update ISR counter
-    STAvUpdate802_11Counter(&pDevice->s802_11Counter, &pDevice->scStatistic , dwMIBCounter);
-    while (pDevice->dwIsr != 0) {
-
-        STAvUpdateIsrStatCounter(&pDevice->scStatistic, pDevice->dwIsr);
-        MACvWriteISR(pDevice->PortOffset, pDevice->dwIsr);
-
-        if (pDevice->dwIsr & ISR_FETALERR){
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " ISR_FETALERR \n");
-            VNSvOutPortB(pDevice->PortOffset + MAC_REG_SOFTPWRCTL, 0);
-            VNSvOutPortW(pDevice->PortOffset + MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPECTI);
-            device_error(pDevice, pDevice->dwIsr);
-        }
-
-        if (pDevice->byLocalID > REV_ID_VT3253_B1) {
-
-            if (pDevice->dwIsr & ISR_MEASURESTART) {
-                // 802.11h measure start
-                pDevice->byOrgChannel = pDevice->byCurrentCh;
-                VNSvInPortB(pDevice->PortOffset + MAC_REG_RCR, &(pDevice->byOrgRCR));
-                VNSvOutPortB(pDevice->PortOffset + MAC_REG_RCR, (RCR_RXALLTYPE | RCR_UNICAST | RCR_BROADCAST | RCR_MULTICAST | RCR_WPAERR));
-                MACvSelectPage1(pDevice->PortOffset);
-                VNSvInPortD(pDevice->PortOffset + MAC_REG_MAR0, &(pDevice->dwOrgMAR0));
-                VNSvInPortD(pDevice->PortOffset + MAC_REG_MAR4, &(pDevice->dwOrgMAR4));
-                MACvSelectPage0(pDevice->PortOffset);
-               //xxxx
-               // WCMDbFlushCommandQueue(pDevice->pMgmt, true);
-                if (set_channel(pDevice, pDevice->pCurrMeasureEID->sReq.byChannel) == true) {
-                    pDevice->bMeasureInProgress = true;
-                    MACvSelectPage1(pDevice->PortOffset);
-                    MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL, MSRCTL_READY);
-                    MACvSelectPage0(pDevice->PortOffset);
-                    pDevice->byBasicMap = 0;
-                    pDevice->byCCAFraction = 0;
-                    for(ii=0;ii<8;ii++) {
-                        pDevice->dwRPIs[ii] = 0;
-                    }
-                } else {
-                    // can not measure because set channel fail
-                   // WCMDbResetCommandQueue(pDevice->pMgmt);
-                    // clear measure control
-                    MACvRegBitsOff(pDevice->PortOffset, MAC_REG_MSRCTL, MSRCTL_EN);
-                    s_vCompleteCurrentMeasure(pDevice, MEASURE_MODE_INCAPABLE);
-                    MACvSelectPage1(pDevice->PortOffset);
-                    MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL+1, MSRCTL1_TXPAUSE);
-                    MACvSelectPage0(pDevice->PortOffset);
-                }
-            }
-            if (pDevice->dwIsr & ISR_MEASUREEND) {
-                // 802.11h measure end
-                pDevice->bMeasureInProgress = false;
-                VNSvOutPortB(pDevice->PortOffset + MAC_REG_RCR, pDevice->byOrgRCR);
-                MACvSelectPage1(pDevice->PortOffset);
-                VNSvOutPortD(pDevice->PortOffset + MAC_REG_MAR0, pDevice->dwOrgMAR0);
-                VNSvOutPortD(pDevice->PortOffset + MAC_REG_MAR4, pDevice->dwOrgMAR4);
-                VNSvInPortB(pDevice->PortOffset + MAC_REG_MSRBBSTS, &byData);
-                pDevice->byBasicMap |= (byData >> 4);
-                VNSvInPortB(pDevice->PortOffset + MAC_REG_CCAFRACTION, &pDevice->byCCAFraction);
-                VNSvInPortB(pDevice->PortOffset + MAC_REG_MSRCTL, &byData);
-                // clear measure control
-                MACvRegBitsOff(pDevice->PortOffset, MAC_REG_MSRCTL, MSRCTL_EN);
-                MACvSelectPage0(pDevice->PortOffset);
-                set_channel(pDevice, pDevice->byOrgChannel);
-                // WCMDbResetCommandQueue(pDevice->pMgmt);
-                MACvSelectPage1(pDevice->PortOffset);
-                MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL+1, MSRCTL1_TXPAUSE);
-                MACvSelectPage0(pDevice->PortOffset);
-                if (byData & MSRCTL_FINISH) {
-                    // measure success
-                    s_vCompleteCurrentMeasure(pDevice, 0);
-                } else {
-                    // can not measure because not ready before end of measure time
-                    s_vCompleteCurrentMeasure(pDevice, MEASURE_MODE_LATE);
-                }
-            }
-            if (pDevice->dwIsr & ISR_QUIETSTART) {
-                do {
-                    ;
-                } while (CARDbStartQuiet(pDevice) == false);
-            }
-        }
-
-        if (pDevice->dwIsr & ISR_TBTT) {
-            if (pDevice->bEnableFirstQuiet == true) {
-                pDevice->byQuietStartCount--;
-                if (pDevice->byQuietStartCount == 0) {
-                    pDevice->bEnableFirstQuiet = false;
-                    MACvSelectPage1(pDevice->PortOffset);
-                    MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL, (MSRCTL_QUIETTXCHK | MSRCTL_QUIETEN));
-                    MACvSelectPage0(pDevice->PortOffset);
-                }
-            }
-            if ((pDevice->bChannelSwitch == true) &&
-                (pDevice->eOPMode == OP_MODE_INFRASTRUCTURE)) {
-                pDevice->byChannelSwitchCount--;
-                if (pDevice->byChannelSwitchCount == 0) {
-                    pDevice->bChannelSwitch = false;
-                    set_channel(pDevice, pDevice->byNewChannel);
-                    VNTWIFIbChannelSwitch(pDevice->pMgmt, pDevice->byNewChannel);
-                    MACvSelectPage1(pDevice->PortOffset);
-                    MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL+1, MSRCTL1_TXPAUSE);
-                    MACvSelectPage0(pDevice->PortOffset);
-                    CARDbStartTxPacket(pDevice, PKT_TYPE_802_11_ALL);
-
-                }
-            }
-            if (pDevice->eOPMode == OP_MODE_ADHOC) {
-                //pDevice->bBeaconSent = false;
-            } else {
-                if ((pDevice->bUpdateBBVGA) && (pDevice->bLinkPass == true) && (pDevice->uCurrRSSI != 0)) {
-                    long            ldBm;
-
-                    RFvRSSITodBm(pDevice, (unsigned char) pDevice->uCurrRSSI, &ldBm);
-                    for (ii=0;ii<BB_VGA_LEVEL;ii++) {
-                        if (ldBm < pDevice->ldBmThreshold[ii]) {
-                            pDevice->byBBVGANew = pDevice->abyBBVGA[ii];
-                            break;
-                        }
-                    }
-                    if (pDevice->byBBVGANew != pDevice->byBBVGACurrent) {
-                        pDevice->uBBVGADiffCount++;
-                        if (pDevice->uBBVGADiffCount == 1) {
-                            // first VGA diff gain
-                            BBvSetVGAGainOffset(pDevice, pDevice->byBBVGANew);
-                            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"First RSSI[%d] NewGain[%d] OldGain[%d] Count[%d]\n",
-                                            (int)ldBm, pDevice->byBBVGANew, pDevice->byBBVGACurrent, (int)pDevice->uBBVGADiffCount);
-                        }
-                        if (pDevice->uBBVGADiffCount >= BB_VGA_CHANGE_THRESHOLD) {
-                            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"RSSI[%d] NewGain[%d] OldGain[%d] Count[%d]\n",
-                                            (int)ldBm, pDevice->byBBVGANew, pDevice->byBBVGACurrent, (int)pDevice->uBBVGADiffCount);
-                            BBvSetVGAGainOffset(pDevice, pDevice->byBBVGANew);
-                        }
-                    } else {
-                        pDevice->uBBVGADiffCount = 1;
-                    }
-                }
-            }
-
-            pDevice->bBeaconSent = false;
-            if (pDevice->bEnablePSMode) {
-                PSbIsNextTBTTWakeUp((void *)pDevice);
-            }
-
-            if ((pDevice->eOPMode == OP_MODE_AP) ||
-                (pDevice->eOPMode == OP_MODE_ADHOC)) {
-
-                MACvOneShotTimer1MicroSec(pDevice->PortOffset,
-                        (pMgmt->wIBSSBeaconPeriod - MAKE_BEACON_RESERVED) << 10);
-            }
-
-            if (pDevice->eOPMode == OP_MODE_ADHOC && pDevice->pMgmt->wCurrATIMWindow > 0) {
-                // todo adhoc PS mode
-            }
-
-        }
-
-        if (pDevice->dwIsr & ISR_BNTX) {
-
-            if (pDevice->eOPMode == OP_MODE_ADHOC) {
-                pDevice->bIsBeaconBufReadySet = false;
-                pDevice->cbBeaconBufReadySetCnt = 0;
-            }
-
-            if (pDevice->eOPMode == OP_MODE_AP) {
-                if(pMgmt->byDTIMCount > 0) {
-                   pMgmt->byDTIMCount --;
-                   pMgmt->sNodeDBTable[0].bRxPSPoll = false;
-                }
-                else {
-                    if(pMgmt->byDTIMCount == 0) {
-                        // check if mutltcast tx bufferring
-                        pMgmt->byDTIMCount = pMgmt->byDTIMPeriod - 1;
-                        pMgmt->sNodeDBTable[0].bRxPSPoll = true;
-                        bScheduleCommand((void *)pDevice, WLAN_CMD_RX_PSPOLL, NULL);
-                    }
-                }
-            }
-            pDevice->bBeaconSent = true;
-
-            if (pDevice->bChannelSwitch == true) {
-                pDevice->byChannelSwitchCount--;
-                if (pDevice->byChannelSwitchCount == 0) {
-                    pDevice->bChannelSwitch = false;
-                    set_channel(pDevice, pDevice->byNewChannel);
-                    VNTWIFIbChannelSwitch(pDevice->pMgmt, pDevice->byNewChannel);
-                    MACvSelectPage1(pDevice->PortOffset);
-                    MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL+1, MSRCTL1_TXPAUSE);
-                    MACvSelectPage0(pDevice->PortOffset);
-                    //VNTWIFIbSendBeacon(pDevice->pMgmt);
-                    CARDbStartTxPacket(pDevice, PKT_TYPE_802_11_ALL);
-                }
-            }
-
-        }
-
-        if (pDevice->dwIsr & ISR_RXDMA0) {
-            max_count += device_rx_srv(pDevice, TYPE_RXDMA0);
-        }
-        if (pDevice->dwIsr & ISR_RXDMA1) {
-            max_count += device_rx_srv(pDevice, TYPE_RXDMA1);
-        }
-        if (pDevice->dwIsr & ISR_TXDMA0){
-            max_count += device_tx_srv(pDevice, TYPE_TXDMA0);
-        }
-        if (pDevice->dwIsr & ISR_AC0DMA){
-            max_count += device_tx_srv(pDevice, TYPE_AC0DMA);
-        }
-        if (pDevice->dwIsr & ISR_SOFTTIMER) {
-
-        }
-        if (pDevice->dwIsr & ISR_SOFTTIMER1) {
-            if (pDevice->eOPMode == OP_MODE_AP) {
-               if (pDevice->bShortSlotTime)
-                   pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTSLOTTIME(1);
-               else
-                   pMgmt->wCurrCapInfo &= ~(WLAN_SET_CAP_INFO_SHORTSLOTTIME(1));
-            }
-            bMgrPrepareBeaconToSend(pDevice, pMgmt);
-            pDevice->byCntMeasure = 0;
-        }
-
-        MACvReadISR(pDevice->PortOffset, &pDevice->dwIsr);
-
-        MACvReceive0(pDevice->PortOffset);
-        MACvReceive1(pDevice->PortOffset);
-
-        if (max_count>pDevice->sOpts.int_works)
-            break;
-    }
-
-    if (byOrgPageSel == 1) {
-        MACvSelectPage1(pDevice->PortOffset);
-    }
-
-    spin_unlock_irq(&pDevice->lock);
-    MACvIntEnable(pDevice->PortOffset, IMR_MASK_VALUE);
-
-    return IRQ_RETVAL(handled);
-}
+	if (pDevice->dwIsr == 0)
+		return IRQ_RETVAL(handled);
+
+	if (pDevice->dwIsr == 0xffffffff) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dwIsr = 0xffff\n");
+		return IRQ_RETVAL(handled);
+	}
+	/*
+	// 2008-05-21 <mark> by Richardtai, we can't read RSSI here, because no packet bound with RSSI
+
+	if ((pDevice->dwIsr & ISR_RXDMA0) &&
+	(pDevice->byLocalID != REV_ID_VT3253_B0) &&
+	(pDevice->bBSSIDFilter == true)) {
+	// update RSSI
+	//BBbReadEmbedded(pDevice->PortOffset, 0x3E, &byRSSI);
+	//pDevice->uCurrRSSI = byRSSI;
+	}
+	*/
+
+	handled = 1;
+	MACvIntDisable(pDevice->PortOffset);
+	spin_lock_irq(&pDevice->lock);
+
+	//Make sure current page is 0
+	VNSvInPortB(pDevice->PortOffset + MAC_REG_PAGE1SEL, &byOrgPageSel);
+	if (byOrgPageSel == 1) {
+		MACvSelectPage0(pDevice->PortOffset);
+	} else
+		byOrgPageSel = 0;
+
+	MACvReadMIBCounter(pDevice->PortOffset, &dwMIBCounter);
+	// TBD....
+	// Must do this after doing rx/tx, cause ISR bit is slow
+	// than RD/TD write back
+	// update ISR counter
+	STAvUpdate802_11Counter(&pDevice->s802_11Counter, &pDevice->scStatistic , dwMIBCounter);
+	while (pDevice->dwIsr != 0) {
+		STAvUpdateIsrStatCounter(&pDevice->scStatistic, pDevice->dwIsr);
+		MACvWriteISR(pDevice->PortOffset, pDevice->dwIsr);
+
+		if (pDevice->dwIsr & ISR_FETALERR) {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " ISR_FETALERR \n");
+			VNSvOutPortB(pDevice->PortOffset + MAC_REG_SOFTPWRCTL, 0);
+			VNSvOutPortW(pDevice->PortOffset + MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPECTI);
+			device_error(pDevice, pDevice->dwIsr);
+		}
+
+		if (pDevice->byLocalID > REV_ID_VT3253_B1) {
+			if (pDevice->dwIsr & ISR_MEASURESTART) {
+				// 802.11h measure start
+				pDevice->byOrgChannel = pDevice->byCurrentCh;
+				VNSvInPortB(pDevice->PortOffset + MAC_REG_RCR, &(pDevice->byOrgRCR));
+				VNSvOutPortB(pDevice->PortOffset + MAC_REG_RCR, (RCR_RXALLTYPE | RCR_UNICAST | RCR_BROADCAST | RCR_MULTICAST | RCR_WPAERR));
+				MACvSelectPage1(pDevice->PortOffset);
+				VNSvInPortD(pDevice->PortOffset + MAC_REG_MAR0, &(pDevice->dwOrgMAR0));
+				VNSvInPortD(pDevice->PortOffset + MAC_REG_MAR4, &(pDevice->dwOrgMAR4));
+				MACvSelectPage0(pDevice->PortOffset);
+				//xxxx
+				// WCMDbFlushCommandQueue(pDevice->pMgmt, true);
+				if (set_channel(pDevice, pDevice->pCurrMeasureEID->sReq.byChannel) == true) {
+					pDevice->bMeasureInProgress = true;
+					MACvSelectPage1(pDevice->PortOffset);
+					MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL, MSRCTL_READY);
+					MACvSelectPage0(pDevice->PortOffset);
+					pDevice->byBasicMap = 0;
+					pDevice->byCCAFraction = 0;
+					for (ii = 0; ii < 8; ii++) {
+						pDevice->dwRPIs[ii] = 0;
+					}
+				} else {
+					// can not measure because set channel fail
+					// WCMDbResetCommandQueue(pDevice->pMgmt);
+					// clear measure control
+					MACvRegBitsOff(pDevice->PortOffset, MAC_REG_MSRCTL, MSRCTL_EN);
+					s_vCompleteCurrentMeasure(pDevice, MEASURE_MODE_INCAPABLE);
+					MACvSelectPage1(pDevice->PortOffset);
+					MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL+1, MSRCTL1_TXPAUSE);
+					MACvSelectPage0(pDevice->PortOffset);
+				}
+			}
+			if (pDevice->dwIsr & ISR_MEASUREEND) {
+				// 802.11h measure end
+				pDevice->bMeasureInProgress = false;
+				VNSvOutPortB(pDevice->PortOffset + MAC_REG_RCR, pDevice->byOrgRCR);
+				MACvSelectPage1(pDevice->PortOffset);
+				VNSvOutPortD(pDevice->PortOffset + MAC_REG_MAR0, pDevice->dwOrgMAR0);
+				VNSvOutPortD(pDevice->PortOffset + MAC_REG_MAR4, pDevice->dwOrgMAR4);
+				VNSvInPortB(pDevice->PortOffset + MAC_REG_MSRBBSTS, &byData);
+				pDevice->byBasicMap |= (byData >> 4);
+				VNSvInPortB(pDevice->PortOffset + MAC_REG_CCAFRACTION, &pDevice->byCCAFraction);
+				VNSvInPortB(pDevice->PortOffset + MAC_REG_MSRCTL, &byData);
+				// clear measure control
+				MACvRegBitsOff(pDevice->PortOffset, MAC_REG_MSRCTL, MSRCTL_EN);
+				MACvSelectPage0(pDevice->PortOffset);
+				set_channel(pDevice, pDevice->byOrgChannel);
+				// WCMDbResetCommandQueue(pDevice->pMgmt);
+				MACvSelectPage1(pDevice->PortOffset);
+				MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL+1, MSRCTL1_TXPAUSE);
+				MACvSelectPage0(pDevice->PortOffset);
+				if (byData & MSRCTL_FINISH) {
+					// measure success
+					s_vCompleteCurrentMeasure(pDevice, 0);
+				} else {
+					// can not measure because not ready before end of measure time
+					s_vCompleteCurrentMeasure(pDevice, MEASURE_MODE_LATE);
+				}
+			}
+			if (pDevice->dwIsr & ISR_QUIETSTART) {
+				do {
+					;
+				} while (CARDbStartQuiet(pDevice) == false);
+			}
+		}
 
+		if (pDevice->dwIsr & ISR_TBTT) {
+			if (pDevice->bEnableFirstQuiet == true) {
+				pDevice->byQuietStartCount--;
+				if (pDevice->byQuietStartCount == 0) {
+					pDevice->bEnableFirstQuiet = false;
+					MACvSelectPage1(pDevice->PortOffset);
+					MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL, (MSRCTL_QUIETTXCHK | MSRCTL_QUIETEN));
+					MACvSelectPage0(pDevice->PortOffset);
+				}
+			}
+			if ((pDevice->bChannelSwitch == true) &&
+			    (pDevice->eOPMode == OP_MODE_INFRASTRUCTURE)) {
+				pDevice->byChannelSwitchCount--;
+				if (pDevice->byChannelSwitchCount == 0) {
+					pDevice->bChannelSwitch = false;
+					set_channel(pDevice, pDevice->byNewChannel);
+					VNTWIFIbChannelSwitch(pDevice->pMgmt, pDevice->byNewChannel);
+					MACvSelectPage1(pDevice->PortOffset);
+					MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL+1, MSRCTL1_TXPAUSE);
+					MACvSelectPage0(pDevice->PortOffset);
+					CARDbStartTxPacket(pDevice, PKT_TYPE_802_11_ALL);
+
+				}
+			}
+			if (pDevice->eOPMode == OP_MODE_ADHOC) {
+				//pDevice->bBeaconSent = false;
+			} else {
+				if ((pDevice->bUpdateBBVGA) && (pDevice->bLinkPass == true) && (pDevice->uCurrRSSI != 0)) {
+					long            ldBm;
+
+					RFvRSSITodBm(pDevice, (unsigned char) pDevice->uCurrRSSI, &ldBm);
+					for (ii = 0; ii < BB_VGA_LEVEL; ii++) {
+						if (ldBm < pDevice->ldBmThreshold[ii]) {
+							pDevice->byBBVGANew = pDevice->abyBBVGA[ii];
+							break;
+						}
+					}
+					if (pDevice->byBBVGANew != pDevice->byBBVGACurrent) {
+						pDevice->uBBVGADiffCount++;
+						if (pDevice->uBBVGADiffCount == 1) {
+							// first VGA diff gain
+							BBvSetVGAGainOffset(pDevice, pDevice->byBBVGANew);
+							DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "First RSSI[%d] NewGain[%d] OldGain[%d] Count[%d]\n",
+								(int)ldBm, pDevice->byBBVGANew, pDevice->byBBVGACurrent, (int)pDevice->uBBVGADiffCount);
+						}
+						if (pDevice->uBBVGADiffCount >= BB_VGA_CHANGE_THRESHOLD) {
+							DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "RSSI[%d] NewGain[%d] OldGain[%d] Count[%d]\n",
+								(int)ldBm, pDevice->byBBVGANew, pDevice->byBBVGACurrent, (int)pDevice->uBBVGADiffCount);
+							BBvSetVGAGainOffset(pDevice, pDevice->byBBVGANew);
+						}
+					} else {
+						pDevice->uBBVGADiffCount = 1;
+					}
+				}
+			}
+
+			pDevice->bBeaconSent = false;
+			if (pDevice->bEnablePSMode) {
+				PSbIsNextTBTTWakeUp((void *)pDevice);
+			}
+
+			if ((pDevice->eOPMode == OP_MODE_AP) ||
+			    (pDevice->eOPMode == OP_MODE_ADHOC)) {
+				MACvOneShotTimer1MicroSec(pDevice->PortOffset,
+							  (pMgmt->wIBSSBeaconPeriod - MAKE_BEACON_RESERVED) << 10);
+			}
+
+			if (pDevice->eOPMode == OP_MODE_ADHOC && pDevice->pMgmt->wCurrATIMWindow > 0) {
+				// todo adhoc PS mode
+			}
+
+		}
+
+		if (pDevice->dwIsr & ISR_BNTX) {
+			if (pDevice->eOPMode == OP_MODE_ADHOC) {
+				pDevice->bIsBeaconBufReadySet = false;
+				pDevice->cbBeaconBufReadySetCnt = 0;
+			}
+
+			if (pDevice->eOPMode == OP_MODE_AP) {
+				if (pMgmt->byDTIMCount > 0) {
+					pMgmt->byDTIMCount--;
+					pMgmt->sNodeDBTable[0].bRxPSPoll = false;
+				} else {
+					if (pMgmt->byDTIMCount == 0) {
+						// check if mutltcast tx bufferring
+						pMgmt->byDTIMCount = pMgmt->byDTIMPeriod - 1;
+						pMgmt->sNodeDBTable[0].bRxPSPoll = true;
+						bScheduleCommand((void *)pDevice, WLAN_CMD_RX_PSPOLL, NULL);
+					}
+				}
+			}
+			pDevice->bBeaconSent = true;
+
+			if (pDevice->bChannelSwitch == true) {
+				pDevice->byChannelSwitchCount--;
+				if (pDevice->byChannelSwitchCount == 0) {
+					pDevice->bChannelSwitch = false;
+					set_channel(pDevice, pDevice->byNewChannel);
+					VNTWIFIbChannelSwitch(pDevice->pMgmt, pDevice->byNewChannel);
+					MACvSelectPage1(pDevice->PortOffset);
+					MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL+1, MSRCTL1_TXPAUSE);
+					MACvSelectPage0(pDevice->PortOffset);
+					//VNTWIFIbSendBeacon(pDevice->pMgmt);
+					CARDbStartTxPacket(pDevice, PKT_TYPE_802_11_ALL);
+				}
+			}
+
+		}
+
+		if (pDevice->dwIsr & ISR_RXDMA0) {
+			max_count += device_rx_srv(pDevice, TYPE_RXDMA0);
+		}
+		if (pDevice->dwIsr & ISR_RXDMA1) {
+			max_count += device_rx_srv(pDevice, TYPE_RXDMA1);
+		}
+		if (pDevice->dwIsr & ISR_TXDMA0) {
+			max_count += device_tx_srv(pDevice, TYPE_TXDMA0);
+		}
+		if (pDevice->dwIsr & ISR_AC0DMA) {
+			max_count += device_tx_srv(pDevice, TYPE_AC0DMA);
+		}
+		if (pDevice->dwIsr & ISR_SOFTTIMER) {
+		}
+		if (pDevice->dwIsr & ISR_SOFTTIMER1) {
+			if (pDevice->eOPMode == OP_MODE_AP) {
+				if (pDevice->bShortSlotTime)
+					pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTSLOTTIME(1);
+				else
+					pMgmt->wCurrCapInfo &= ~(WLAN_SET_CAP_INFO_SHORTSLOTTIME(1));
+			}
+			bMgrPrepareBeaconToSend(pDevice, pMgmt);
+			pDevice->byCntMeasure = 0;
+		}
+
+		MACvReadISR(pDevice->PortOffset, &pDevice->dwIsr);
+
+		MACvReceive0(pDevice->PortOffset);
+		MACvReceive1(pDevice->PortOffset);
+
+		if (max_count > pDevice->sOpts.int_works)
+			break;
+	}
+
+	if (byOrgPageSel == 1) {
+		MACvSelectPage1(pDevice->PortOffset);
+	}
+
+	spin_unlock_irq(&pDevice->lock);
+	MACvIntEnable(pDevice->PortOffset, IMR_MASK_VALUE);
+
+	return IRQ_RETVAL(handled);
+}
 
 static unsigned const ethernet_polynomial = 0x04c11db7U;
 static inline u32 ether_crc(int length, unsigned char *data)
 {
-    int crc = -1;
-
-    while(--length >= 0) {
-        unsigned char current_octet = *data++;
-        int bit;
-        for (bit = 0; bit < 8; bit++, current_octet >>= 1) {
-            crc = (crc << 1) ^
-                ((crc < 0) ^ (current_octet & 1) ? ethernet_polynomial : 0);
-        }
-    }
-    return crc;
+	int crc = -1;
+
+	while (--length >= 0) {
+		unsigned char current_octet = *data++;
+		int bit;
+		for (bit = 0; bit < 8; bit++, current_octet >>= 1) {
+			crc = (crc << 1) ^
+				((crc < 0) ^ (current_octet & 1) ? ethernet_polynomial : 0);
+		}
+	}
+	return crc;
 }
 
 //2008-8-4 <add> by chester
 static int Config_FileGetParameter(unsigned char *string,
-		unsigned char *dest, unsigned char *source)
+				   unsigned char *dest, unsigned char *source)
 {
-  unsigned char buf1[100];
-  int source_len = strlen(source);
+	unsigned char buf1[100];
+	int source_len = strlen(source);
 
-    memset(buf1,0,100);
-    strcat(buf1, string);
-    strcat(buf1, "=");
-    source+=strlen(buf1);
+	memset(buf1, 0, 100);
+	strcat(buf1, string);
+	strcat(buf1, "=");
+	source += strlen(buf1);
 
-   memcpy(dest,source,source_len-strlen(buf1));
- return true;
+	memcpy(dest, source, source_len - strlen(buf1));
+	return true;
 }
 
-int Config_FileOperation(PSDevice pDevice,bool fwrite,unsigned char *Parameter) {
-    unsigned char *config_path = CONFIG_PATH;
-    unsigned char *buffer = NULL;
-    unsigned char tmpbuffer[20];
-    struct file   *filp=NULL;
-    mm_segment_t old_fs = get_fs();
-    //int oldfsuid=0,oldfsgid=0;
-    int result=0;
-
-    set_fs (KERNEL_DS);
-
-    /* Can't do this anymore, so we rely on correct filesystem permissions:
-    //Make sure a caller can read or write power as root
-    oldfsuid=current->cred->fsuid;
-    oldfsgid=current->cred->fsgid;
-    current->cred->fsuid = 0;
-    current->cred->fsgid = 0;
-    */
-
-    //open file
-      filp = filp_open(config_path, O_RDWR, 0);
-        if (IS_ERR(filp)) {
-	     printk("Config_FileOperation:open file fail?\n");
-	     result=-1;
-             goto error2;
-	  }
-
-     if(!(filp->f_op) || !(filp->f_op->read) ||!(filp->f_op->write)) {
-           printk("file %s cann't readable or writable?\n",config_path);
-	  result = -1;
-	  goto error1;
-     	}
-
-buffer = kmalloc(1024, GFP_KERNEL);
-if(buffer==NULL) {
-  printk("allocate mem for file fail?\n");
-  result = -1;
-  goto error1;
-}
+int Config_FileOperation(PSDevice pDevice, bool fwrite, unsigned char *Parameter) {
+	unsigned char *config_path = CONFIG_PATH;
+	unsigned char *buffer = NULL;
+	unsigned char tmpbuffer[20];
+	struct file   *filp = NULL;
+	mm_segment_t old_fs = get_fs();
+	//int oldfsuid=0,oldfsgid=0;
+	int result = 0;
+
+	set_fs(KERNEL_DS);
+
+	/* Can't do this anymore, so we rely on correct filesystem permissions:
+	//Make sure a caller can read or write power as root
+	oldfsuid=current->cred->fsuid;
+	oldfsgid=current->cred->fsgid;
+	current->cred->fsuid = 0;
+	current->cred->fsgid = 0;
+	*/
+
+	//open file
+	filp = filp_open(config_path, O_RDWR, 0);
+	if (IS_ERR(filp)) {
+		printk("Config_FileOperation:open file fail?\n");
+		result = -1;
+		goto error2;
+	}
 
-if(filp->f_op->read(filp, buffer, 1024, &filp->f_pos)<0) {
- printk("read file error?\n");
- result = -1;
- goto error1;
-}
+	if (!(filp->f_op) || !(filp->f_op->read) || !(filp->f_op->write)) {
+		printk("file %s cann't readable or writable?\n", config_path);
+		result = -1;
+		goto error1;
+	}
 
-if(Config_FileGetParameter("ZONETYPE",tmpbuffer,buffer)!=true) {
-  printk("get parameter error?\n");
-  result = -1;
-  goto error1;
-}
+	buffer = kmalloc(1024, GFP_KERNEL);
+	if (buffer == NULL) {
+		printk("allocate mem for file fail?\n");
+		result = -1;
+		goto error1;
+	}
 
-if(memcmp(tmpbuffer,"USA",3)==0) {
-  result=ZoneType_USA;
-}
-else if(memcmp(tmpbuffer,"JAPAN",5)==0) {
-  result=ZoneType_Japan;
-}
-else if(memcmp(tmpbuffer,"EUROPE",5)==0) {
- result=ZoneType_Europe;
-}
-else {
-  result = -1;
-  printk("Unknown Zonetype[%s]?\n",tmpbuffer);
-}
+	if (filp->f_op->read(filp, buffer, 1024, &filp->f_pos) < 0) {
+		printk("read file error?\n");
+		result = -1;
+		goto error1;
+	}
+
+	if (Config_FileGetParameter("ZONETYPE", tmpbuffer, buffer) != true) {
+		printk("get parameter error?\n");
+		result = -1;
+		goto error1;
+	}
+
+	if (memcmp(tmpbuffer, "USA", 3) == 0) {
+		result = ZoneType_USA;
+	} else if (memcmp(tmpbuffer, "JAPAN", 5) == 0) {
+		result = ZoneType_Japan;
+	} else if (memcmp(tmpbuffer, "EUROPE", 5) == 0) {
+		result = ZoneType_Europe;
+	} else {
+		result = -1;
+		printk("Unknown Zonetype[%s]?\n", tmpbuffer);
+	}
 
 error1:
-  kfree(buffer);
+	kfree(buffer);
 
-  if(filp_close(filp,NULL))
-       printk("Config_FileOperation:close file fail\n");
+	if (filp_close(filp, NULL))
+		printk("Config_FileOperation:close file fail\n");
 
 error2:
-  set_fs (old_fs);
+	set_fs(old_fs);
 
-  /*
-  current->cred->fsuid=oldfsuid;
-  current->cred->fsgid=oldfsgid;
-  */
+	/*
+	  current->cred->fsuid=oldfsuid;
+	  current->cred->fsgid=oldfsgid;
+	*/
 
-  return result;
+	return result;
 }
 
+static void device_set_multi(struct net_device *dev) {
+	PSDevice         pDevice = (PSDevice)netdev_priv(dev);
+
+	PSMgmtObject     pMgmt = pDevice->pMgmt;
+	u32              mc_filter[2];
+	struct netdev_hw_addr *ha;
+
+	VNSvInPortB(pDevice->PortOffset + MAC_REG_RCR, &(pDevice->byRxMode));
+
+	if (dev->flags & IFF_PROMISC) {         /* Set promiscuous. */
+		DBG_PRT(MSG_LEVEL_ERR, KERN_NOTICE "%s: Promiscuous mode enabled.\n", dev->name);
+		/* Unconditionally log net taps. */
+		pDevice->byRxMode |= (RCR_MULTICAST|RCR_BROADCAST|RCR_UNICAST);
+	} else if ((netdev_mc_count(dev) > pDevice->multicast_limit)
+		 ||  (dev->flags & IFF_ALLMULTI)) {
+		MACvSelectPage1(pDevice->PortOffset);
+		VNSvOutPortD(pDevice->PortOffset + MAC_REG_MAR0, 0xffffffff);
+		VNSvOutPortD(pDevice->PortOffset + MAC_REG_MAR0 + 4, 0xffffffff);
+		MACvSelectPage0(pDevice->PortOffset);
+		pDevice->byRxMode |= (RCR_MULTICAST|RCR_BROADCAST);
+	} else {
+		memset(mc_filter, 0, sizeof(mc_filter));
+		netdev_for_each_mc_addr(ha, dev) {
+			int bit_nr = ether_crc(ETH_ALEN, ha->addr) >> 26;
+			mc_filter[bit_nr >> 5] |= cpu_to_le32(1 << (bit_nr & 31));
+		}
+		MACvSelectPage1(pDevice->PortOffset);
+		VNSvOutPortD(pDevice->PortOffset + MAC_REG_MAR0, mc_filter[0]);
+		VNSvOutPortD(pDevice->PortOffset + MAC_REG_MAR0 + 4, mc_filter[1]);
+		MACvSelectPage0(pDevice->PortOffset);
+		pDevice->byRxMode &= ~(RCR_UNICAST);
+		pDevice->byRxMode |= (RCR_MULTICAST|RCR_BROADCAST);
+	}
 
+	if (pMgmt->eConfigMode == WMAC_CONFIG_AP) {
+		// If AP mode, don't enable RCR_UNICAST. Since hw only compare addr1 with local mac.
+		pDevice->byRxMode |= (RCR_MULTICAST|RCR_BROADCAST);
+		pDevice->byRxMode &= ~(RCR_UNICAST);
+	}
 
-static void device_set_multi(struct net_device *dev) {
-    PSDevice         pDevice = (PSDevice) netdev_priv(dev);
-
-    PSMgmtObject     pMgmt = pDevice->pMgmt;
-    u32              mc_filter[2];
-    struct netdev_hw_addr *ha;
-
-
-    VNSvInPortB(pDevice->PortOffset + MAC_REG_RCR, &(pDevice->byRxMode));
-
-    if (dev->flags & IFF_PROMISC) {         /* Set promiscuous. */
-        DBG_PRT(MSG_LEVEL_ERR,KERN_NOTICE "%s: Promiscuous mode enabled.\n", dev->name);
-        /* Unconditionally log net taps. */
-        pDevice->byRxMode |= (RCR_MULTICAST|RCR_BROADCAST|RCR_UNICAST);
-    }
-    else if ((netdev_mc_count(dev) > pDevice->multicast_limit)
-        ||  (dev->flags & IFF_ALLMULTI)) {
-        MACvSelectPage1(pDevice->PortOffset);
-        VNSvOutPortD(pDevice->PortOffset + MAC_REG_MAR0, 0xffffffff);
-        VNSvOutPortD(pDevice->PortOffset + MAC_REG_MAR0 + 4, 0xffffffff);
-        MACvSelectPage0(pDevice->PortOffset);
-        pDevice->byRxMode |= (RCR_MULTICAST|RCR_BROADCAST);
-    }
-    else {
-        memset(mc_filter, 0, sizeof(mc_filter));
-	netdev_for_each_mc_addr(ha, dev) {
-            int bit_nr = ether_crc(ETH_ALEN, ha->addr) >> 26;
-            mc_filter[bit_nr >> 5] |= cpu_to_le32(1 << (bit_nr & 31));
-        }
-        MACvSelectPage1(pDevice->PortOffset);
-        VNSvOutPortD(pDevice->PortOffset + MAC_REG_MAR0, mc_filter[0]);
-        VNSvOutPortD(pDevice->PortOffset + MAC_REG_MAR0 + 4, mc_filter[1]);
-        MACvSelectPage0(pDevice->PortOffset);
-        pDevice->byRxMode &= ~(RCR_UNICAST);
-        pDevice->byRxMode |= (RCR_MULTICAST|RCR_BROADCAST);
-    }
-
-    if (pMgmt->eConfigMode == WMAC_CONFIG_AP) {
-        // If AP mode, don't enable RCR_UNICAST. Since hw only compare addr1 with local mac.
-        pDevice->byRxMode |= (RCR_MULTICAST|RCR_BROADCAST);
-        pDevice->byRxMode &= ~(RCR_UNICAST);
-    }
-
-    VNSvOutPortB(pDevice->PortOffset + MAC_REG_RCR, pDevice->byRxMode);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->byRxMode = %x\n", pDevice->byRxMode );
+	VNSvOutPortB(pDevice->PortOffset + MAC_REG_RCR, pDevice->byRxMode);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->byRxMode = %x\n", pDevice->byRxMode);
 }
 
-
 static struct net_device_stats *device_get_stats(struct net_device *dev) {
-    PSDevice pDevice=(PSDevice) netdev_priv(dev);
+	PSDevice pDevice = (PSDevice)netdev_priv(dev);
 
-    return &pDevice->stats;
+	return &pDevice->stats;
 }
 
-
-
 static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd) {
 	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
 
-	struct iwreq *wrq = (struct iwreq *) rq;
-	int                 rc =0;
-    PSMgmtObject        pMgmt = pDevice->pMgmt;
-    PSCmdRequest        pReq;
+	struct iwreq *wrq = (struct iwreq *)rq;
+	int rc = 0;
+	PSMgmtObject pMgmt = pDevice->pMgmt;
+	PSCmdRequest pReq;
 
+	if (pMgmt == NULL) {
+		rc = -EFAULT;
+		return rc;
+	}
 
-    if (pMgmt == NULL) {
-        rc = -EFAULT;
-        return rc;
-    }
-
-    switch(cmd) {
-
+	switch (cmd) {
 	case SIOCGIWNAME:
 		rc = iwctl_giwname(dev, NULL, (char *)&(wrq->u.name), NULL);
 		break;
@@ -3113,7 +2891,7 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd) {
 
 		// Set frequency/channel
 	case SIOCSIWFREQ:
-	    rc = iwctl_siwfreq(dev, NULL, &(wrq->u.freq), NULL);
+		rc = iwctl_siwfreq(dev, NULL, &(wrq->u.freq), NULL);
 		break;
 
 		// Get frequency/channel
@@ -3124,60 +2902,57 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd) {
 		// Set desired network name (ESSID)
 	case SIOCSIWESSID:
 
-		{
-			char essid[IW_ESSID_MAX_SIZE+1];
-			if (wrq->u.essid.length > IW_ESSID_MAX_SIZE) {
-				rc = -E2BIG;
-				break;
-			}
-			if (copy_from_user(essid, wrq->u.essid.pointer,
-					   wrq->u.essid.length)) {
-				rc = -EFAULT;
-				break;
-			}
-			rc = iwctl_siwessid(dev, NULL,
-					    &(wrq->u.essid), essid);
+	{
+		char essid[IW_ESSID_MAX_SIZE+1];
+		if (wrq->u.essid.length > IW_ESSID_MAX_SIZE) {
+			rc = -E2BIG;
+			break;
 		}
-		break;
-
+		if (copy_from_user(essid, wrq->u.essid.pointer,
+				   wrq->u.essid.length)) {
+			rc = -EFAULT;
+			break;
+		}
+		rc = iwctl_siwessid(dev, NULL,
+				    &(wrq->u.essid), essid);
+	}
+	break;
 
-		// Get current network name (ESSID)
+	// Get current network name (ESSID)
 	case SIOCGIWESSID:
 
-		{
-			char essid[IW_ESSID_MAX_SIZE+1];
-			if (wrq->u.essid.pointer)
-				rc = iwctl_giwessid(dev, NULL,
-						    &(wrq->u.essid), essid);
-				if (copy_to_user(wrq->u.essid.pointer,
-						         essid,
-						         wrq->u.essid.length) )
-					rc = -EFAULT;
-		}
-		break;
+	{
+		char essid[IW_ESSID_MAX_SIZE+1];
+		if (wrq->u.essid.pointer)
+			rc = iwctl_giwessid(dev, NULL,
+					    &(wrq->u.essid), essid);
+		if (copy_to_user(wrq->u.essid.pointer,
+				 essid,
+				 wrq->u.essid.length))
+			rc = -EFAULT;
+	}
+	break;
 
 	case SIOCSIWAP:
 
 		rc = iwctl_siwap(dev, NULL, &(wrq->u.ap_addr), NULL);
 		break;
 
-
 		// Get current Access Point (BSSID)
 	case SIOCGIWAP:
 		rc = iwctl_giwap(dev, NULL, &(wrq->u.ap_addr), NULL);
 		break;
 
-
 		// Set desired station name
 	case SIOCSIWNICKN:
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWNICKN \n");
-        rc = -EOPNOTSUPP;
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWNICKN \n");
+		rc = -EOPNOTSUPP;
 		break;
 
 		// Get current station name
 	case SIOCGIWNICKN:
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWNICKN \n");
-        rc = -EOPNOTSUPP;
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWNICKN \n");
+		rc = -EOPNOTSUPP;
 		break;
 
 		// Set the desired bit-rate
@@ -3185,19 +2960,19 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd) {
 		rc = iwctl_siwrate(dev, NULL, &(wrq->u.bitrate), NULL);
 		break;
 
-	// Get the current bit-rate
+		// Get the current bit-rate
 	case SIOCGIWRATE:
 
 		rc = iwctl_giwrate(dev, NULL, &(wrq->u.bitrate), NULL);
 		break;
 
-	// Set the desired RTS threshold
+		// Set the desired RTS threshold
 	case SIOCSIWRTS:
 
 		rc = iwctl_siwrts(dev, NULL, &(wrq->u.rts), NULL);
 		break;
 
-	// Get the current RTS threshold
+		// Get the current RTS threshold
 	case SIOCGIWRTS:
 
 		rc = iwctl_giwrts(dev, NULL, &(wrq->u.rts), NULL);
@@ -3207,9 +2982,9 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd) {
 	case SIOCSIWFRAG:
 
 		rc = iwctl_siwfrag(dev, NULL, &(wrq->u.frag), NULL);
-	    break;
+		break;
 
-	// Get the current fragmentation threshold
+		// Get the current fragmentation threshold
 	case SIOCGIWFRAG:
 
 		rc = iwctl_giwfrag(dev, NULL, &(wrq->u.frag), NULL);
@@ -3217,7 +2992,7 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd) {
 
 		// Set mode of operation
 	case SIOCSIWMODE:
-    	rc = iwctl_siwmode(dev, NULL, &(wrq->u.mode), NULL);
+		rc = iwctl_siwmode(dev, NULL, &(wrq->u.mode), NULL);
 		break;
 
 		// Get mode of operation
@@ -3226,33 +3001,30 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd) {
 		break;
 
 		// Set WEP keys and mode
-	case SIOCSIWENCODE:
-		{
-            char abyKey[WLAN_WEP232_KEYLEN];
-
-			if (wrq->u.encoding.pointer) {
-
+	case SIOCSIWENCODE: {
+		char abyKey[WLAN_WEP232_KEYLEN];
 
-				if (wrq->u.encoding.length > WLAN_WEP232_KEYLEN) {
-					rc = -E2BIG;
-					break;
-				}
-				memset(abyKey, 0, WLAN_WEP232_KEYLEN);
-				if (copy_from_user(abyKey,
-				                  wrq->u.encoding.pointer,
-				                  wrq->u.encoding.length)) {
-					rc = -EFAULT;
-					break;
-				}
-			} else if (wrq->u.encoding.length != 0) {
-				rc = -EINVAL;
+		if (wrq->u.encoding.pointer) {
+			if (wrq->u.encoding.length > WLAN_WEP232_KEYLEN) {
+				rc = -E2BIG;
 				break;
 			}
-			rc = iwctl_siwencode(dev, NULL, &(wrq->u.encoding), abyKey);
+			memset(abyKey, 0, WLAN_WEP232_KEYLEN);
+			if (copy_from_user(abyKey,
+					   wrq->u.encoding.pointer,
+					   wrq->u.encoding.length)) {
+				rc = -EFAULT;
+				break;
+			}
+		} else if (wrq->u.encoding.length != 0) {
+			rc = -EINVAL;
+			break;
 		}
-		break;
+		rc = iwctl_siwencode(dev, NULL, &(wrq->u.encoding), abyKey);
+	}
+	break;
 
-		// Get the WEP keys and mode
+	// Get the WEP keys and mode
 	case SIOCGIWENCODE:
 
 		if (!capable(CAP_NET_ADMIN)) {
@@ -3260,14 +3032,14 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd) {
 			break;
 		}
 		{
-		    char abyKey[WLAN_WEP232_KEYLEN];
+			char abyKey[WLAN_WEP232_KEYLEN];
 
-		    rc = iwctl_giwencode(dev, NULL, &(wrq->u.encoding), abyKey);
-		    if (rc != 0) break;
+			rc = iwctl_giwencode(dev, NULL, &(wrq->u.encoding), abyKey);
+			if (rc != 0) break;
 			if (wrq->u.encoding.pointer) {
 				if (copy_to_user(wrq->u.encoding.pointer,
-						        abyKey,
-						        wrq->u.encoding.length))
+						 abyKey,
+						 wrq->u.encoding.length))
 					rc = -EFAULT;
 			}
 		}
@@ -3275,13 +3047,13 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd) {
 
 		// Get the current Tx-Power
 	case SIOCGIWTXPOW:
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWTXPOW \n");
-        rc = -EOPNOTSUPP;
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWTXPOW \n");
+		rc = -EOPNOTSUPP;
 		break;
 
 	case SIOCSIWTXPOW:
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWTXPOW \n");
-        rc = -EOPNOTSUPP;
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWTXPOW \n");
+		rc = -EOPNOTSUPP;
 		break;
 
 	case SIOCSIWRETRY:
@@ -3297,91 +3069,86 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd) {
 		// Get range of parameters
 	case SIOCGIWRANGE:
 
-		{
-			struct iw_range range;
+	{
+		struct iw_range range;
 
-			rc = iwctl_giwrange(dev, NULL, &(wrq->u.data), (char *) &range);
-			if (copy_to_user(wrq->u.data.pointer, &range, sizeof(struct iw_range)))
-				rc = -EFAULT;
-		}
+		rc = iwctl_giwrange(dev, NULL, &(wrq->u.data), (char *)&range);
+		if (copy_to_user(wrq->u.data.pointer, &range, sizeof(struct iw_range)))
+			rc = -EFAULT;
+	}
 
-		break;
+	break;
 
 	case SIOCGIWPOWER:
 
 		rc = iwctl_giwpower(dev, NULL, &(wrq->u.power), NULL);
 		break;
 
-
 	case SIOCSIWPOWER:
 
 		rc = iwctl_siwpower(dev, NULL, &(wrq->u.power), NULL);
 		break;
 
-
 	case SIOCGIWSENS:
 
-	    rc = iwctl_giwsens(dev, NULL, &(wrq->u.sens), NULL);
+		rc = iwctl_giwsens(dev, NULL, &(wrq->u.sens), NULL);
 		break;
 
 	case SIOCSIWSENS:
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWSENS \n");
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWSENS \n");
 		rc = -EOPNOTSUPP;
 		break;
 
-	case SIOCGIWAPLIST:
-	    {
-            char buffer[IW_MAX_AP * (sizeof(struct sockaddr) + sizeof(struct iw_quality))];
-
-		    if (wrq->u.data.pointer) {
-		        rc = iwctl_giwaplist(dev, NULL, &(wrq->u.data), buffer);
-		        if (rc == 0) {
-                    if (copy_to_user(wrq->u.data.pointer,
-					                buffer,
-					               (wrq->u.data.length * (sizeof(struct sockaddr) +  sizeof(struct iw_quality)))
-				        ))
-				    rc = -EFAULT;
-		        }
-            }
-        }
-		break;
+	case SIOCGIWAPLIST: {
+		char buffer[IW_MAX_AP * (sizeof(struct sockaddr) + sizeof(struct iw_quality))];
 
+		if (wrq->u.data.pointer) {
+			rc = iwctl_giwaplist(dev, NULL, &(wrq->u.data), buffer);
+			if (rc == 0) {
+				if (copy_to_user(wrq->u.data.pointer,
+						 buffer,
+						 (wrq->u.data.length * (sizeof(struct sockaddr) +  sizeof(struct iw_quality)))
+					    ))
+					rc = -EFAULT;
+			}
+		}
+	}
+	break;
 
 #ifdef WIRELESS_SPY
-		// Set the spy list
+	// Set the spy list
 	case SIOCSIWSPY:
 
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWSPY \n");
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWSPY \n");
 		rc = -EOPNOTSUPP;
 		break;
 
 		// Get the spy list
 	case SIOCGIWSPY:
 
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWSPY \n");
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWSPY \n");
 		rc = -EOPNOTSUPP;
 		break;
 
 #endif // WIRELESS_SPY
 
 	case SIOCGIWPRIV:
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWPRIV \n");
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWPRIV \n");
 		rc = -EOPNOTSUPP;
 /*
-		if(wrq->u.data.pointer) {
-			wrq->u.data.length = sizeof(iwctl_private_args) / sizeof( iwctl_private_args[0]);
+  if (wrq->u.data.pointer) {
+  wrq->u.data.length = sizeof(iwctl_private_args) / sizeof(iwctl_private_args[0]);
 
-			if(copy_to_user(wrq->u.data.pointer,
-					(u_char *) iwctl_private_args,
-					sizeof(iwctl_private_args)))
-				rc = -EFAULT;
-		}
+  if (copy_to_user(wrq->u.data.pointer,
+  (u_char *) iwctl_private_args,
+  sizeof(iwctl_private_args)))
+  rc = -EFAULT;
+  }
 */
 		break;
 
-
 //2008-0409-07, <Add> by Einsn Liu
-#ifdef  WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
+#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
 	case SIOCSIWAUTH:
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWAUTH \n");
 		rc = iwctl_siwauth(dev, NULL, &(wrq->u.param), NULL);
@@ -3402,27 +3169,26 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd) {
 		rc = iwctl_giwgenie(dev, NULL, &(wrq->u.data), wrq->u.data.pointer);
 		break;
 
-	case SIOCSIWENCODEEXT:
-		{
-			char extra[sizeof(struct iw_encode_ext)+MAX_KEY_LEN+1];
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWENCODEEXT \n");
-			if(wrq->u.encoding.pointer){
-				memset(extra, 0, sizeof(struct iw_encode_ext)+MAX_KEY_LEN+1);
-				if(wrq->u.encoding.length > (sizeof(struct iw_encode_ext)+ MAX_KEY_LEN)){
-					rc = -E2BIG;
-					break;
-				}
-				if(copy_from_user(extra, wrq->u.encoding.pointer,wrq->u.encoding.length)){
-					rc = -EFAULT;
-					break;
-				}
-			}else if(wrq->u.encoding.length != 0){
-				rc = -EINVAL;
+	case SIOCSIWENCODEEXT: {
+		char extra[sizeof(struct iw_encode_ext)+MAX_KEY_LEN+1];
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWENCODEEXT \n");
+		if (wrq->u.encoding.pointer) {
+			memset(extra, 0, sizeof(struct iw_encode_ext)+MAX_KEY_LEN + 1);
+			if (wrq->u.encoding.length > (sizeof(struct iw_encode_ext) + MAX_KEY_LEN)) {
+				rc = -E2BIG;
+				break;
+			}
+			if (copy_from_user(extra, wrq->u.encoding.pointer, wrq->u.encoding.length)) {
+				rc = -EFAULT;
 				break;
 			}
-			rc = iwctl_siwencodeext(dev, NULL, &(wrq->u.encoding), extra);
+		} else if (wrq->u.encoding.length != 0) {
+			rc = -EINVAL;
+			break;
 		}
-		break;
+		rc = iwctl_siwencodeext(dev, NULL, &(wrq->u.encoding), extra);
+	}
+	break;
 
 	case SIOCGIWENCODEEXT:
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWENCODEEXT \n");
@@ -3437,91 +3203,87 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd) {
 #endif // #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
 //End Add -- //2008-0409-07, <Add> by Einsn Liu
 
-    case IOCTL_CMD_TEST:
+	case IOCTL_CMD_TEST:
 
 		if (!(pDevice->flags & DEVICE_FLAGS_OPENED)) {
-		    rc = -EFAULT;
-		    break;
-		} else {
-		    rc = 0;
-		}
-        pReq = (PSCmdRequest)rq;
-        pReq->wResult = MAGIC_CODE;
-        break;
-
-    case IOCTL_CMD_SET:
-
-               #ifdef SndEvt_ToAPI
-                  if((((PSCmdRequest)rq)->wCmdCode !=WLAN_CMD_SET_EVT) &&
-		       !(pDevice->flags & DEVICE_FLAGS_OPENED))
-	      #else
-		if (!(pDevice->flags & DEVICE_FLAGS_OPENED) &&
-		       (((PSCmdRequest)rq)->wCmdCode !=WLAN_CMD_SET_WPA))
-	      #endif
-		{
-		    rc = -EFAULT;
-		    break;
+			rc = -EFAULT;
+			break;
 		} else {
-		    rc = 0;
+			rc = 0;
 		}
+		pReq = (PSCmdRequest)rq;
+		pReq->wResult = MAGIC_CODE;
+		break;
 
-	    if (test_and_set_bit( 0, (void*)&(pMgmt->uCmdBusy))) {
-		    return -EBUSY;
-	    }
-        rc = private_ioctl(pDevice, rq);
-        clear_bit( 0, (void*)&(pMgmt->uCmdBusy));
-        break;
+	case IOCTL_CMD_SET:
 
-    case IOCTL_CMD_HOSTAPD:
+#ifdef SndEvt_ToAPI
+		if ((((PSCmdRequest)rq)->wCmdCode != WLAN_CMD_SET_EVT) &&
+		    !(pDevice->flags & DEVICE_FLAGS_OPENED))
+#else
+			if (!(pDevice->flags & DEVICE_FLAGS_OPENED) &&
+			    (((PSCmdRequest)rq)->wCmdCode != WLAN_CMD_SET_WPA))
+#endif
+			{
+				rc = -EFAULT;
+				break;
+			} else {
+				rc = 0;
+			}
+
+		if (test_and_set_bit(0, (void *)&(pMgmt->uCmdBusy))) {
+			return -EBUSY;
+		}
+		rc = private_ioctl(pDevice, rq);
+		clear_bit(0, (void *)&(pMgmt->uCmdBusy));
+		break;
 
+	case IOCTL_CMD_HOSTAPD:
 
-	rc = vt6655_hostap_ioctl(pDevice, &wrq->u.data);
-        break;
+		rc = vt6655_hostap_ioctl(pDevice, &wrq->u.data);
+		break;
 
-    case IOCTL_CMD_WPA:
+	case IOCTL_CMD_WPA:
 
-	rc = wpa_ioctl(pDevice, &wrq->u.data);
-        break;
+		rc = wpa_ioctl(pDevice, &wrq->u.data);
+		break;
 
 	case SIOCETHTOOL:
-        return ethtool_ioctl(dev, (void *) rq->ifr_data);
-	// All other calls are currently unsupported
+		return ethtool_ioctl(dev, (void *)rq->ifr_data);
+		// All other calls are currently unsupported
 
 	default:
 		rc = -EOPNOTSUPP;
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Ioctl command not support..%x\n", cmd);
-
-
-    }
-
-    if (pDevice->bCommit) {
-       if (pMgmt->eConfigMode == WMAC_CONFIG_AP) {
-           netif_stop_queue(pDevice->dev);
-           spin_lock_irq(&pDevice->lock);
-           bScheduleCommand((void *)pDevice, WLAN_CMD_RUN_AP, NULL);
-           spin_unlock_irq(&pDevice->lock);
-       }
-       else {
-           DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Commit the settings\n");
-           spin_lock_irq(&pDevice->lock);
-           pDevice->bLinkPass = false;
-           memset(pMgmt->abyCurrBSSID, 0, 6);
-           pMgmt->eCurrState = WMAC_STATE_IDLE;
-           netif_stop_queue(pDevice->dev);
-	#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-	      pMgmt->eScanType = WMAC_SCAN_ACTIVE;
-	 if(pDevice->bWPASuppWextEnabled !=true)
-	 #endif
-           bScheduleCommand((void *) pDevice, WLAN_CMD_BSSID_SCAN, pMgmt->abyDesireSSID);
-           bScheduleCommand((void *) pDevice, WLAN_CMD_SSID, NULL);
-           spin_unlock_irq(&pDevice->lock);
-      }
-      pDevice->bCommit = false;
-    }
-
-    return rc;
-}
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Ioctl command not support..%x\n", cmd);
 
+	}
+
+	if (pDevice->bCommit) {
+		if (pMgmt->eConfigMode == WMAC_CONFIG_AP) {
+			netif_stop_queue(pDevice->dev);
+			spin_lock_irq(&pDevice->lock);
+			bScheduleCommand((void *)pDevice, WLAN_CMD_RUN_AP, NULL);
+			spin_unlock_irq(&pDevice->lock);
+		} else {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Commit the settings\n");
+			spin_lock_irq(&pDevice->lock);
+			pDevice->bLinkPass = false;
+			memset(pMgmt->abyCurrBSSID, 0, 6);
+			pMgmt->eCurrState = WMAC_STATE_IDLE;
+			netif_stop_queue(pDevice->dev);
+#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
+			pMgmt->eScanType = WMAC_SCAN_ACTIVE;
+			if (pDevice->bWPASuppWextEnabled != true)
+#endif
+				bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, pMgmt->abyDesireSSID);
+			bScheduleCommand((void *)pDevice, WLAN_CMD_SSID, NULL);
+			spin_unlock_irq(&pDevice->lock);
+		}
+		pDevice->bCommit = false;
+	}
+
+	return rc;
+}
 
 static int ethtool_ioctl(struct net_device *dev, void *useraddr)
 {
@@ -3530,7 +3292,7 @@ static int ethtool_ioctl(struct net_device *dev, void *useraddr)
 	if (copy_from_user(&ethcmd, useraddr, sizeof(ethcmd)))
 		return -EFAULT;
 
-        switch (ethcmd) {
+	switch (ethcmd) {
 	case ETHTOOL_GDRVINFO: {
 		struct ethtool_drvinfo info = {ETHTOOL_GDRVINFO};
 		strncpy(info.driver, DEVICE_NAME, sizeof(info.driver)-1);
@@ -3540,7 +3302,7 @@ static int ethtool_ioctl(struct net_device *dev, void *useraddr)
 		return 0;
 	}
 
-        }
+	}
 
 	return -EOPNOTSUPP;
 }
@@ -3562,122 +3324,111 @@ static struct pci_driver device_driver = {
 
 static int __init vt6655_init_module(void)
 {
-    int ret;
-
+	int ret;
 
 //    ret=pci_module_init(&device_driver);
 	//ret = pcie_port_service_register(&device_driver);
 	ret = pci_register_driver(&device_driver);
 #ifdef CONFIG_PM
-    if(ret >= 0)
-        register_reboot_notifier(&device_notifier);
+	if (ret >= 0)
+		register_reboot_notifier(&device_notifier);
 #endif
 
-    return ret;
+	return ret;
 }
 
 static void __exit vt6655_cleanup_module(void)
 {
-
-
 #ifdef CONFIG_PM
-    unregister_reboot_notifier(&device_notifier);
+	unregister_reboot_notifier(&device_notifier);
 #endif
-    pci_unregister_driver(&device_driver);
-
+	pci_unregister_driver(&device_driver);
 }
 
 module_init(vt6655_init_module);
 module_exit(vt6655_cleanup_module);
 
-
 #ifdef CONFIG_PM
 static int
 device_notify_reboot(struct notifier_block *nb, unsigned long event, void *p)
 {
-    struct pci_dev *pdev = NULL;
-    switch(event) {
-    case SYS_DOWN:
-    case SYS_HALT:
-    case SYS_POWER_OFF:
-	for_each_pci_dev(pdev) {
-            if(pci_dev_driver(pdev) == &device_driver) {
-                if (pci_get_drvdata(pdev))
-                    viawget_suspend(pdev, PMSG_HIBERNATE);
-            }
-        }
-    }
-    return NOTIFY_DONE;
+	struct pci_dev *pdev = NULL;
+	switch (event) {
+	case SYS_DOWN:
+	case SYS_HALT:
+	case SYS_POWER_OFF:
+		for_each_pci_dev(pdev) {
+			if (pci_dev_driver(pdev) == &device_driver) {
+				if (pci_get_drvdata(pdev))
+					viawget_suspend(pdev, PMSG_HIBERNATE);
+			}
+		}
+	}
+	return NOTIFY_DONE;
 }
 
 static int
 viawget_suspend(struct pci_dev *pcid, pm_message_t state)
 {
-    int power_status;   // to silence the compiler
-
-    PSDevice pDevice=pci_get_drvdata(pcid);
-    PSMgmtObject  pMgmt = pDevice->pMgmt;
-
-    netif_stop_queue(pDevice->dev);
-    spin_lock_irq(&pDevice->lock);
-    pci_save_state(pcid);
-    del_timer(&pDevice->sTimerCommand);
-    del_timer(&pMgmt->sTimerSecondCallback);
-    pDevice->cbFreeCmdQueue = CMD_Q_SIZE;
-    pDevice->uCmdDequeueIdx = 0;
-    pDevice->uCmdEnqueueIdx = 0;
-    pDevice->bCmdRunning = false;
-    MACbShutdown(pDevice->PortOffset);
-    MACvSaveContext(pDevice->PortOffset, pDevice->abyMacContext);
-    pDevice->bLinkPass = false;
-    memset(pMgmt->abyCurrBSSID, 0, 6);
-    pMgmt->eCurrState = WMAC_STATE_IDLE;
-    pci_disable_device(pcid);
-    power_status = pci_set_power_state(pcid, pci_choose_state(pcid, state));
-    spin_unlock_irq(&pDevice->lock);
-    return 0;
+	int power_status;   // to silence the compiler
+
+	PSDevice pDevice = pci_get_drvdata(pcid);
+	PSMgmtObject  pMgmt = pDevice->pMgmt;
+
+	netif_stop_queue(pDevice->dev);
+	spin_lock_irq(&pDevice->lock);
+	pci_save_state(pcid);
+	del_timer(&pDevice->sTimerCommand);
+	del_timer(&pMgmt->sTimerSecondCallback);
+	pDevice->cbFreeCmdQueue = CMD_Q_SIZE;
+	pDevice->uCmdDequeueIdx = 0;
+	pDevice->uCmdEnqueueIdx = 0;
+	pDevice->bCmdRunning = false;
+	MACbShutdown(pDevice->PortOffset);
+	MACvSaveContext(pDevice->PortOffset, pDevice->abyMacContext);
+	pDevice->bLinkPass = false;
+	memset(pMgmt->abyCurrBSSID, 0, 6);
+	pMgmt->eCurrState = WMAC_STATE_IDLE;
+	pci_disable_device(pcid);
+	power_status = pci_set_power_state(pcid, pci_choose_state(pcid, state));
+	spin_unlock_irq(&pDevice->lock);
+	return 0;
 }
 
 static int
 viawget_resume(struct pci_dev *pcid)
 {
-    PSDevice  pDevice=pci_get_drvdata(pcid);
-    PSMgmtObject  pMgmt = pDevice->pMgmt;
-    int power_status;   // to silence the compiler
-
-
-    power_status = pci_set_power_state(pcid, 0);
-    power_status = pci_enable_wake(pcid, 0, 0);
-    pci_restore_state(pcid);
-    if (netif_running(pDevice->dev)) {
-        spin_lock_irq(&pDevice->lock);
-        MACvRestoreContext(pDevice->PortOffset, pDevice->abyMacContext);
-        device_init_registers(pDevice, DEVICE_INIT_DXPL);
-        if (pMgmt->sNodeDBTable[0].bActive == true) { // Assoc with BSS
-            pMgmt->sNodeDBTable[0].bActive = false;
-            pDevice->bLinkPass = false;
-            if(pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
-                // In Adhoc, BSS state set back to started.
-                pMgmt->eCurrState = WMAC_STATE_STARTED;
-           }
-            else {
-                pMgmt->eCurrMode = WMAC_MODE_STANDBY;
-                pMgmt->eCurrState = WMAC_STATE_IDLE;
-            }
-        }
-        init_timer(&pMgmt->sTimerSecondCallback);
-        init_timer(&pDevice->sTimerCommand);
-        MACvIntEnable(pDevice->PortOffset, IMR_MASK_VALUE);
-        BSSvClearBSSList((void *)pDevice, pDevice->bLinkPass);
-        bScheduleCommand((void *) pDevice, WLAN_CMD_BSSID_SCAN, NULL);
-        bScheduleCommand((void *) pDevice, WLAN_CMD_SSID, NULL);
-        spin_unlock_irq(&pDevice->lock);
-    }
-    return 0;
+	PSDevice  pDevice = pci_get_drvdata(pcid);
+	PSMgmtObject  pMgmt = pDevice->pMgmt;
+	int power_status;   // to silence the compiler
+
+	power_status = pci_set_power_state(pcid, 0);
+	power_status = pci_enable_wake(pcid, 0, 0);
+	pci_restore_state(pcid);
+	if (netif_running(pDevice->dev)) {
+		spin_lock_irq(&pDevice->lock);
+		MACvRestoreContext(pDevice->PortOffset, pDevice->abyMacContext);
+		device_init_registers(pDevice, DEVICE_INIT_DXPL);
+		if (pMgmt->sNodeDBTable[0].bActive == true) { // Assoc with BSS
+			pMgmt->sNodeDBTable[0].bActive = false;
+			pDevice->bLinkPass = false;
+			if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
+				// In Adhoc, BSS state set back to started.
+				pMgmt->eCurrState = WMAC_STATE_STARTED;
+			} else {
+				pMgmt->eCurrMode = WMAC_MODE_STANDBY;
+				pMgmt->eCurrState = WMAC_STATE_IDLE;
+			}
+		}
+		init_timer(&pMgmt->sTimerSecondCallback);
+		init_timer(&pDevice->sTimerCommand);
+		MACvIntEnable(pDevice->PortOffset, IMR_MASK_VALUE);
+		BSSvClearBSSList((void *)pDevice, pDevice->bLinkPass);
+		bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, NULL);
+		bScheduleCommand((void *)pDevice, WLAN_CMD_SSID, NULL);
+		spin_unlock_irq(&pDevice->lock);
+	}
+	return 0;
 }
 
 #endif
-
-
-
-
diff --git a/drivers/staging/vt6655/dpc.c b/drivers/staging/vt6655/dpc.c
index 373e9e4fc87..a9533f3f252 100644
--- a/drivers/staging/vt6655/dpc.c
+++ b/drivers/staging/vt6655/dpc.c
@@ -55,20 +55,17 @@
 #include "iowpa.h"
 #include "aes_ccmp.h"
 
-
-
 /*---------------------  Static Definitions -------------------------*/
 
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
 //static int          msglevel                =MSG_LEVEL_DEBUG;
-static int          msglevel                =MSG_LEVEL_INFO;
+static int msglevel = MSG_LEVEL_INFO;
 
 const unsigned char acbyRxRate[MAX_RATE] =
 {2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108};
 
-
 /*---------------------  Static Functions  --------------------------*/
 
 /*---------------------  Static Definitions -------------------------*/
@@ -77,60 +74,56 @@ const unsigned char acbyRxRate[MAX_RATE] =
 
 static unsigned char s_byGetRateIdx(unsigned char byRate);
 
-
 static void
 s_vGetDASA(unsigned char *pbyRxBufferAddr, unsigned int *pcbHeaderSize,
-		PSEthernetHeader psEthHeader);
+	   PSEthernetHeader psEthHeader);
 
 static void
 s_vProcessRxMACHeader(PSDevice pDevice, unsigned char *pbyRxBufferAddr,
-		unsigned int cbPacketSize, bool bIsWEP, bool bExtIV,
-		unsigned int *pcbHeadSize);
+		      unsigned int cbPacketSize, bool bIsWEP, bool bExtIV,
+		      unsigned int *pcbHeadSize);
 
 static bool s_bAPModeRxCtl(
-    PSDevice pDevice,
-    unsigned char *pbyFrame,
-    int      iSANodeIndex
-    );
-
-
-
-static bool s_bAPModeRxData (
-    PSDevice pDevice,
-    struct sk_buff* skb,
-    unsigned int FrameSize,
-    unsigned int cbHeaderOffset,
-    int      iSANodeIndex,
-    int      iDANodeIndex
-    );
-
+	PSDevice pDevice,
+	unsigned char *pbyFrame,
+	int      iSANodeIndex
+);
+
+static bool s_bAPModeRxData(
+	PSDevice pDevice,
+	struct sk_buff *skb,
+	unsigned int FrameSize,
+	unsigned int cbHeaderOffset,
+	int      iSANodeIndex,
+	int      iDANodeIndex
+);
 
 static bool s_bHandleRxEncryption(
-    PSDevice     pDevice,
-    unsigned char *pbyFrame,
-    unsigned int FrameSize,
-    unsigned char *pbyRsr,
-    unsigned char *pbyNewRsr,
-    PSKeyItem   *pKeyOut,
-    bool *pbExtIV,
-    unsigned short *pwRxTSC15_0,
-    unsigned long *pdwRxTSC47_16
-    );
+	PSDevice     pDevice,
+	unsigned char *pbyFrame,
+	unsigned int FrameSize,
+	unsigned char *pbyRsr,
+	unsigned char *pbyNewRsr,
+	PSKeyItem   *pKeyOut,
+	bool *pbExtIV,
+	unsigned short *pwRxTSC15_0,
+	unsigned long *pdwRxTSC47_16
+);
 
 static bool s_bHostWepRxEncryption(
 
-    PSDevice     pDevice,
-    unsigned char *pbyFrame,
-    unsigned int FrameSize,
-    unsigned char *pbyRsr,
-    bool bOnFly,
-    PSKeyItem    pKey,
-    unsigned char *pbyNewRsr,
-    bool *pbExtIV,
-    unsigned short *pwRxTSC15_0,
-    unsigned long *pdwRxTSC47_16
+	PSDevice     pDevice,
+	unsigned char *pbyFrame,
+	unsigned int FrameSize,
+	unsigned char *pbyRsr,
+	bool bOnFly,
+	PSKeyItem    pKey,
+	unsigned char *pbyNewRsr,
+	bool *pbExtIV,
+	unsigned short *pwRxTSC15_0,
+	unsigned long *pdwRxTSC47_16
 
-    );
+);
 
 /*---------------------  Export Variables  --------------------------*/
 
@@ -150,694 +143,647 @@ static bool s_bHostWepRxEncryption(
  *
  * Return Value: None
  *
--*/
+ -*/
 static void
 s_vProcessRxMACHeader(PSDevice pDevice, unsigned char *pbyRxBufferAddr,
-		unsigned int cbPacketSize, bool bIsWEP, bool bExtIV,
-		unsigned int *pcbHeadSize)
+		      unsigned int cbPacketSize, bool bIsWEP, bool bExtIV,
+		      unsigned int *pcbHeadSize)
 {
-    unsigned char *pbyRxBuffer;
-    unsigned int cbHeaderSize = 0;
-    unsigned short *pwType;
-    PS802_11Header  pMACHeader;
-    int             ii;
-
-
-    pMACHeader = (PS802_11Header) (pbyRxBufferAddr + cbHeaderSize);
-
-    s_vGetDASA((unsigned char *)pMACHeader, &cbHeaderSize, &pDevice->sRxEthHeader);
-
-    if (bIsWEP) {
-        if (bExtIV) {
-            // strip IV&ExtIV , add 8 byte
-            cbHeaderSize += (WLAN_HDR_ADDR3_LEN + 8);
-        } else {
-            // strip IV , add 4 byte
-            cbHeaderSize += (WLAN_HDR_ADDR3_LEN + 4);
-        }
-    }
-    else {
-        cbHeaderSize += WLAN_HDR_ADDR3_LEN;
-    };
-
-    pbyRxBuffer = (unsigned char *) (pbyRxBufferAddr + cbHeaderSize);
-    if (!compare_ether_addr(pbyRxBuffer, &pDevice->abySNAP_Bridgetunnel[0])) {
-        cbHeaderSize += 6;
-    }
-    else if (!compare_ether_addr(pbyRxBuffer, &pDevice->abySNAP_RFC1042[0])) {
-        cbHeaderSize += 6;
-        pwType = (unsigned short *) (pbyRxBufferAddr + cbHeaderSize);
-        if ((*pwType!= TYPE_PKT_IPX) && (*pwType != cpu_to_le16(0xF380))) {
-        }
-        else {
-            cbHeaderSize -= 8;
-            pwType = (unsigned short *) (pbyRxBufferAddr + cbHeaderSize);
-            if (bIsWEP) {
-                if (bExtIV) {
-                    *pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN - 8);    // 8 is IV&ExtIV
-                } else {
-                    *pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN - 4);    // 4 is IV
-                }
-            }
-            else {
-                *pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN);
-            }
-        }
-    }
-    else {
-        cbHeaderSize -= 2;
-        pwType = (unsigned short *) (pbyRxBufferAddr + cbHeaderSize);
-        if (bIsWEP) {
-            if (bExtIV) {
-                *pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN - 8);    // 8 is IV&ExtIV
-            } else {
-                *pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN - 4);    // 4 is IV
-            }
-        }
-        else {
-            *pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN);
-        }
-    }
-
-    cbHeaderSize -= (ETH_ALEN * 2);
-    pbyRxBuffer = (unsigned char *) (pbyRxBufferAddr + cbHeaderSize);
-    for(ii=0;ii<ETH_ALEN;ii++)
-        *pbyRxBuffer++ = pDevice->sRxEthHeader.abyDstAddr[ii];
-    for(ii=0;ii<ETH_ALEN;ii++)
-        *pbyRxBuffer++ = pDevice->sRxEthHeader.abySrcAddr[ii];
-
-    *pcbHeadSize = cbHeaderSize;
-}
-
+	unsigned char *pbyRxBuffer;
+	unsigned int cbHeaderSize = 0;
+	unsigned short *pwType;
+	PS802_11Header  pMACHeader;
+	int             ii;
+
+	pMACHeader = (PS802_11Header) (pbyRxBufferAddr + cbHeaderSize);
+
+	s_vGetDASA((unsigned char *)pMACHeader, &cbHeaderSize, &pDevice->sRxEthHeader);
+
+	if (bIsWEP) {
+		if (bExtIV) {
+			// strip IV&ExtIV , add 8 byte
+			cbHeaderSize += (WLAN_HDR_ADDR3_LEN + 8);
+		} else {
+			// strip IV , add 4 byte
+			cbHeaderSize += (WLAN_HDR_ADDR3_LEN + 4);
+		}
+	} else {
+		cbHeaderSize += WLAN_HDR_ADDR3_LEN;
+	};
+
+	pbyRxBuffer = (unsigned char *)(pbyRxBufferAddr + cbHeaderSize);
+	if (!compare_ether_addr(pbyRxBuffer, &pDevice->abySNAP_Bridgetunnel[0])) {
+		cbHeaderSize += 6;
+	} else if (!compare_ether_addr(pbyRxBuffer, &pDevice->abySNAP_RFC1042[0])) {
+		cbHeaderSize += 6;
+		pwType = (unsigned short *)(pbyRxBufferAddr + cbHeaderSize);
+		if ((*pwType != TYPE_PKT_IPX) && (*pwType != cpu_to_le16(0xF380))) {
+		} else {
+			cbHeaderSize -= 8;
+			pwType = (unsigned short *)(pbyRxBufferAddr + cbHeaderSize);
+			if (bIsWEP) {
+				if (bExtIV) {
+					*pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN - 8);    // 8 is IV&ExtIV
+				} else {
+					*pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN - 4);    // 4 is IV
+				}
+			} else {
+				*pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN);
+			}
+		}
+	} else {
+		cbHeaderSize -= 2;
+		pwType = (unsigned short *)(pbyRxBufferAddr + cbHeaderSize);
+		if (bIsWEP) {
+			if (bExtIV) {
+				*pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN - 8);    // 8 is IV&ExtIV
+			} else {
+				*pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN - 4);    // 4 is IV
+			}
+		} else {
+			*pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN);
+		}
+	}
 
+	cbHeaderSize -= (ETH_ALEN * 2);
+	pbyRxBuffer = (unsigned char *)(pbyRxBufferAddr + cbHeaderSize);
+	for (ii = 0; ii < ETH_ALEN; ii++)
+		*pbyRxBuffer++ = pDevice->sRxEthHeader.abyDstAddr[ii];
+	for (ii = 0; ii < ETH_ALEN; ii++)
+		*pbyRxBuffer++ = pDevice->sRxEthHeader.abySrcAddr[ii];
 
+	*pcbHeadSize = cbHeaderSize;
+}
 
-static unsigned char s_byGetRateIdx (unsigned char byRate)
+static unsigned char s_byGetRateIdx(unsigned char byRate)
 {
-    unsigned char byRateIdx;
+	unsigned char byRateIdx;
 
-    for (byRateIdx = 0; byRateIdx <MAX_RATE ; byRateIdx++) {
-        if (acbyRxRate[byRateIdx%MAX_RATE] == byRate)
-            return byRateIdx;
-    }
-    return 0;
+	for (byRateIdx = 0; byRateIdx < MAX_RATE; byRateIdx++) {
+		if (acbyRxRate[byRateIdx % MAX_RATE] == byRate)
+			return byRateIdx;
+	}
+	return 0;
 }
 
-
 static void
 s_vGetDASA(unsigned char *pbyRxBufferAddr, unsigned int *pcbHeaderSize,
-	PSEthernetHeader psEthHeader)
+	   PSEthernetHeader psEthHeader)
 {
-    unsigned int cbHeaderSize = 0;
-    PS802_11Header  pMACHeader;
-    int             ii;
-
-    pMACHeader = (PS802_11Header) (pbyRxBufferAddr + cbHeaderSize);
-
-    if ((pMACHeader->wFrameCtl & FC_TODS) == 0) {
-        if (pMACHeader->wFrameCtl & FC_FROMDS) {
-            for(ii=0;ii<ETH_ALEN;ii++) {
-                psEthHeader->abyDstAddr[ii] = pMACHeader->abyAddr1[ii];
-                psEthHeader->abySrcAddr[ii] = pMACHeader->abyAddr3[ii];
-            }
-        }
-        else {
-            // IBSS mode
-            for(ii=0;ii<ETH_ALEN;ii++) {
-                psEthHeader->abyDstAddr[ii] = pMACHeader->abyAddr1[ii];
-                psEthHeader->abySrcAddr[ii] = pMACHeader->abyAddr2[ii];
-            }
-        }
-    }
-    else {
-        // Is AP mode..
-        if (pMACHeader->wFrameCtl & FC_FROMDS) {
-            for(ii=0;ii<ETH_ALEN;ii++) {
-                psEthHeader->abyDstAddr[ii] = pMACHeader->abyAddr3[ii];
-                psEthHeader->abySrcAddr[ii] = pMACHeader->abyAddr4[ii];
-                cbHeaderSize += 6;
-            }
-        }
-        else {
-            for(ii=0;ii<ETH_ALEN;ii++) {
-                psEthHeader->abyDstAddr[ii] = pMACHeader->abyAddr3[ii];
-                psEthHeader->abySrcAddr[ii] = pMACHeader->abyAddr2[ii];
-            }
-        }
-    };
-    *pcbHeaderSize = cbHeaderSize;
+	unsigned int cbHeaderSize = 0;
+	PS802_11Header  pMACHeader;
+	int             ii;
+
+	pMACHeader = (PS802_11Header) (pbyRxBufferAddr + cbHeaderSize);
+
+	if ((pMACHeader->wFrameCtl & FC_TODS) == 0) {
+		if (pMACHeader->wFrameCtl & FC_FROMDS) {
+			for (ii = 0; ii < ETH_ALEN; ii++) {
+				psEthHeader->abyDstAddr[ii] = pMACHeader->abyAddr1[ii];
+				psEthHeader->abySrcAddr[ii] = pMACHeader->abyAddr3[ii];
+			}
+		} else {
+			// IBSS mode
+			for (ii = 0; ii < ETH_ALEN; ii++) {
+				psEthHeader->abyDstAddr[ii] = pMACHeader->abyAddr1[ii];
+				psEthHeader->abySrcAddr[ii] = pMACHeader->abyAddr2[ii];
+			}
+		}
+	} else {
+		// Is AP mode..
+		if (pMACHeader->wFrameCtl & FC_FROMDS) {
+			for (ii = 0; ii < ETH_ALEN; ii++) {
+				psEthHeader->abyDstAddr[ii] = pMACHeader->abyAddr3[ii];
+				psEthHeader->abySrcAddr[ii] = pMACHeader->abyAddr4[ii];
+				cbHeaderSize += 6;
+			}
+		} else {
+			for (ii = 0; ii < ETH_ALEN; ii++) {
+				psEthHeader->abyDstAddr[ii] = pMACHeader->abyAddr3[ii];
+				psEthHeader->abySrcAddr[ii] = pMACHeader->abyAddr2[ii];
+			}
+		}
+	};
+	*pcbHeaderSize = cbHeaderSize;
 }
 
-
-
-
 //PLICE_DEBUG ->
 
 void	MngWorkItem(void *Context)
 {
 	PSRxMgmtPacket			pRxMgmtPacket;
 	PSDevice	pDevice =  (PSDevice) Context;
-	//printk("Enter MngWorkItem,Queue packet num is %d\n",pDevice->rxManeQueue.packet_num);
+
 	spin_lock_irq(&pDevice->lock);
-	 while(pDevice->rxManeQueue.packet_num != 0)
-	 {
-		 pRxMgmtPacket =  DeQueue(pDevice);
-        		vMgrRxManagePacket(pDevice, pDevice->pMgmt, pRxMgmtPacket);
+	while (pDevice->rxManeQueue.packet_num != 0) {
+		pRxMgmtPacket =  DeQueue(pDevice);
+		vMgrRxManagePacket(pDevice, pDevice->pMgmt, pRxMgmtPacket);
 	}
 	spin_unlock_irq(&pDevice->lock);
 }
 
-
 //PLICE_DEBUG<-
 
-
-
 bool
-device_receive_frame (
-    PSDevice pDevice,
-    PSRxDesc pCurrRD
-    )
+device_receive_frame(
+	PSDevice pDevice,
+	PSRxDesc pCurrRD
+)
 {
-
-    PDEVICE_RD_INFO  pRDInfo = pCurrRD->pRDInfo;
-#ifdef	PLICE_DEBUG
-	//printk("device_receive_frame:pCurrRD is %x,pRDInfo is %x\n",pCurrRD,pCurrRD->pRDInfo);
-#endif
-    struct net_device_stats* pStats=&pDevice->stats;
-    struct sk_buff* skb;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    PSRxMgmtPacket  pRxPacket = &(pDevice->pMgmt->sRxPacket);
-    PS802_11Header  p802_11Header;
-    unsigned char *pbyRsr;
-    unsigned char *pbyNewRsr;
-    unsigned char *pbyRSSI;
-    PQWORD          pqwTSFTime;
-    unsigned short *pwFrameSize;
-    unsigned char *pbyFrame;
-    bool bDeFragRx = false;
-    bool bIsWEP = false;
-    unsigned int cbHeaderOffset;
-    unsigned int FrameSize;
-    unsigned short wEtherType = 0;
-    int             iSANodeIndex = -1;
-    int             iDANodeIndex = -1;
-    unsigned int ii;
-    unsigned int cbIVOffset;
-    bool bExtIV = false;
-    unsigned char *pbyRxSts;
-    unsigned char *pbyRxRate;
-    unsigned char *pbySQ;
-    unsigned int cbHeaderSize;
-    PSKeyItem       pKey = NULL;
-    unsigned short wRxTSC15_0 = 0;
-    unsigned long dwRxTSC47_16 = 0;
-    SKeyItem        STempKey;
-    // 802.11h RPI
-    unsigned long dwDuration = 0;
-    long            ldBm = 0;
-    long            ldBmThreshold = 0;
-    PS802_11Header pMACHeader;
- bool bRxeapol_key = false;
-
-//    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---------- device_receive_frame---\n");
-
-    skb = pRDInfo->skb;
-
+	PDEVICE_RD_INFO  pRDInfo = pCurrRD->pRDInfo;
+	struct net_device_stats *pStats = &pDevice->stats;
+	struct sk_buff *skb;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	PSRxMgmtPacket  pRxPacket = &(pDevice->pMgmt->sRxPacket);
+	PS802_11Header  p802_11Header;
+	unsigned char *pbyRsr;
+	unsigned char *pbyNewRsr;
+	unsigned char *pbyRSSI;
+	PQWORD          pqwTSFTime;
+	unsigned short *pwFrameSize;
+	unsigned char *pbyFrame;
+	bool bDeFragRx = false;
+	bool bIsWEP = false;
+	unsigned int cbHeaderOffset;
+	unsigned int FrameSize;
+	unsigned short wEtherType = 0;
+	int             iSANodeIndex = -1;
+	int             iDANodeIndex = -1;
+	unsigned int ii;
+	unsigned int cbIVOffset;
+	bool bExtIV = false;
+	unsigned char *pbyRxSts;
+	unsigned char *pbyRxRate;
+	unsigned char *pbySQ;
+	unsigned int cbHeaderSize;
+	PSKeyItem       pKey = NULL;
+	unsigned short wRxTSC15_0 = 0;
+	unsigned long dwRxTSC47_16 = 0;
+	SKeyItem        STempKey;
+	// 802.11h RPI
+	unsigned long dwDuration = 0;
+	long            ldBm = 0;
+	long            ldBmThreshold = 0;
+	PS802_11Header pMACHeader;
+	bool bRxeapol_key = false;
+
+//    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "---------- device_receive_frame---\n");
+
+	skb = pRDInfo->skb;
 
 //PLICE_DEBUG->
 #if 1
 	pci_unmap_single(pDevice->pcid, pRDInfo->skb_dma,
-                     pDevice->rx_buf_sz, PCI_DMA_FROMDEVICE);
+			 pDevice->rx_buf_sz, PCI_DMA_FROMDEVICE);
 #endif
 //PLICE_DEBUG<-
-    pwFrameSize = (unsigned short *)(skb->data + 2);
-    FrameSize = cpu_to_le16(pCurrRD->m_rd1RD1.wReqCount) - cpu_to_le16(pCurrRD->m_rd0RD0.wResCount);
-
-    // Max: 2312Payload + 30HD +4CRC + 2Padding + 4Len + 8TSF + 4RSR
-    // Min (ACK): 10HD +4CRC + 2Padding + 4Len + 8TSF + 4RSR
-    if ((FrameSize > 2364)||(FrameSize <= 32)) {
-        // Frame Size error drop this packet.
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---------- WRONG Length 1 \n");
-        return false;
-    }
-
-    pbyRxSts = (unsigned char *) (skb->data);
-    pbyRxRate = (unsigned char *) (skb->data + 1);
-    pbyRsr = (unsigned char *) (skb->data + FrameSize - 1);
-    pbyRSSI = (unsigned char *) (skb->data + FrameSize - 2);
-    pbyNewRsr = (unsigned char *) (skb->data + FrameSize - 3);
-    pbySQ = (unsigned char *) (skb->data + FrameSize - 4);
-    pqwTSFTime = (PQWORD) (skb->data + FrameSize - 12);
-    pbyFrame = (unsigned char *)(skb->data + 4);
-
-    // get packet size
-    FrameSize = cpu_to_le16(*pwFrameSize);
-
-    if ((FrameSize > 2346)|(FrameSize < 14)) { // Max: 2312Payload + 30HD +4CRC
-                                               // Min: 14 bytes ACK
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---------- WRONG Length 2 \n");
-        return false;
-    }
+	pwFrameSize = (unsigned short *)(skb->data + 2);
+	FrameSize = cpu_to_le16(pCurrRD->m_rd1RD1.wReqCount) - cpu_to_le16(pCurrRD->m_rd0RD0.wResCount);
+
+	// Max: 2312Payload + 30HD +4CRC + 2Padding + 4Len + 8TSF + 4RSR
+	// Min (ACK): 10HD +4CRC + 2Padding + 4Len + 8TSF + 4RSR
+	if ((FrameSize > 2364) || (FrameSize <= 32)) {
+		// Frame Size error drop this packet.
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "---------- WRONG Length 1 \n");
+		return false;
+	}
+
+	pbyRxSts = (unsigned char *)(skb->data);
+	pbyRxRate = (unsigned char *)(skb->data + 1);
+	pbyRsr = (unsigned char *)(skb->data + FrameSize - 1);
+	pbyRSSI = (unsigned char *)(skb->data + FrameSize - 2);
+	pbyNewRsr = (unsigned char *)(skb->data + FrameSize - 3);
+	pbySQ = (unsigned char *)(skb->data + FrameSize - 4);
+	pqwTSFTime = (PQWORD)(skb->data + FrameSize - 12);
+	pbyFrame = (unsigned char *)(skb->data + 4);
+
+	// get packet size
+	FrameSize = cpu_to_le16(*pwFrameSize);
+
+	if ((FrameSize > 2346)|(FrameSize < 14)) { // Max: 2312Payload + 30HD +4CRC
+		// Min: 14 bytes ACK
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "---------- WRONG Length 2 \n");
+		return false;
+	}
 //PLICE_DEBUG->
 #if 1
 	// update receive statistic counter
-    STAvUpdateRDStatCounter(&pDevice->scStatistic,
-                            *pbyRsr,
-                            *pbyNewRsr,
-                            *pbyRxRate,
-                            pbyFrame,
-                            FrameSize);
+	STAvUpdateRDStatCounter(&pDevice->scStatistic,
+				*pbyRsr,
+				*pbyNewRsr,
+				*pbyRxRate,
+				pbyFrame,
+				FrameSize);
 
 #endif
 
-  pMACHeader=(PS802_11Header)((unsigned char *) (skb->data)+8);
+	pMACHeader = (PS802_11Header)((unsigned char *)(skb->data) + 8);
 //PLICE_DEBUG<-
 	if (pDevice->bMeasureInProgress == true) {
-        if ((*pbyRsr & RSR_CRCOK) != 0) {
-            pDevice->byBasicMap |= 0x01;
-        }
-        dwDuration = (FrameSize << 4);
-        dwDuration /= acbyRxRate[*pbyRxRate%MAX_RATE];
-        if (*pbyRxRate <= RATE_11M) {
-            if (*pbyRxSts & 0x01) {
-                // long preamble
-                dwDuration += 192;
-            } else {
-                // short preamble
-                dwDuration += 96;
-            }
-        } else {
-            dwDuration += 16;
-        }
-        RFvRSSITodBm(pDevice, *pbyRSSI, &ldBm);
-        ldBmThreshold = -57;
-        for (ii = 7; ii > 0;) {
-            if (ldBm > ldBmThreshold) {
-                break;
-            }
-            ldBmThreshold -= 5;
-            ii--;
-        }
-        pDevice->dwRPIs[ii] += dwDuration;
-        return false;
-    }
-
-    if (!is_multicast_ether_addr(pbyFrame)) {
-        if (WCTLbIsDuplicate(&(pDevice->sDupRxCache), (PS802_11Header) (skb->data + 4))) {
-            pDevice->s802_11Counter.FrameDuplicateCount++;
-            return false;
-        }
-    }
-
-
-    // Use for TKIP MIC
-    s_vGetDASA(skb->data+4, &cbHeaderSize, &pDevice->sRxEthHeader);
-
-    // filter packet send from myself
-    if (!compare_ether_addr((unsigned char *)&(pDevice->sRxEthHeader.abySrcAddr[0]), pDevice->abyCurrentNetAddr))
-        return false;
-
-    if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) || (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA)) {
-        if (IS_CTL_PSPOLL(pbyFrame) || !IS_TYPE_CONTROL(pbyFrame)) {
-            p802_11Header = (PS802_11Header) (pbyFrame);
-            // get SA NodeIndex
-            if (BSSDBbIsSTAInNodeDB(pMgmt, (unsigned char *)(p802_11Header->abyAddr2), &iSANodeIndex)) {
-                pMgmt->sNodeDBTable[iSANodeIndex].ulLastRxJiffer = jiffies;
-                pMgmt->sNodeDBTable[iSANodeIndex].uInActiveCount = 0;
-            }
-        }
-    }
-
-    if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-        if (s_bAPModeRxCtl(pDevice, pbyFrame, iSANodeIndex) == true) {
-            return false;
-        }
-    }
-
-
-    if (IS_FC_WEP(pbyFrame)) {
-        bool bRxDecryOK = false;
-
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"rx WEP pkt\n");
-        bIsWEP = true;
-        if ((pDevice->bEnableHostWEP) && (iSANodeIndex >= 0)) {
-            pKey = &STempKey;
-            pKey->byCipherSuite = pMgmt->sNodeDBTable[iSANodeIndex].byCipherSuite;
-            pKey->dwKeyIndex = pMgmt->sNodeDBTable[iSANodeIndex].dwKeyIndex;
-            pKey->uKeyLength = pMgmt->sNodeDBTable[iSANodeIndex].uWepKeyLength;
-            pKey->dwTSC47_16 = pMgmt->sNodeDBTable[iSANodeIndex].dwTSC47_16;
-            pKey->wTSC15_0 = pMgmt->sNodeDBTable[iSANodeIndex].wTSC15_0;
-            memcpy(pKey->abyKey,
-                &pMgmt->sNodeDBTable[iSANodeIndex].abyWepKey[0],
-                pKey->uKeyLength
-                );
-
-            bRxDecryOK = s_bHostWepRxEncryption(pDevice,
-                                                pbyFrame,
-                                                FrameSize,
-                                                pbyRsr,
-                                                pMgmt->sNodeDBTable[iSANodeIndex].bOnFly,
-                                                pKey,
-                                                pbyNewRsr,
-                                                &bExtIV,
-                                                &wRxTSC15_0,
-                                                &dwRxTSC47_16);
-        } else {
-            bRxDecryOK = s_bHandleRxEncryption(pDevice,
-                                                pbyFrame,
-                                                FrameSize,
-                                                pbyRsr,
-                                                pbyNewRsr,
-                                                &pKey,
-                                                &bExtIV,
-                                                &wRxTSC15_0,
-                                                &dwRxTSC47_16);
-        }
-
-        if (bRxDecryOK) {
-            if ((*pbyNewRsr & NEWRSR_DECRYPTOK) == 0) {
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ICV Fail\n");
-                if ( (pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPA) ||
-                    (pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) ||
-                    (pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) ||
-                    (pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPA2) ||
-                    (pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) {
-
-                    if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_TKIP)) {
-                        pDevice->s802_11Counter.TKIPICVErrors++;
-                    } else if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_CCMP)) {
-                        pDevice->s802_11Counter.CCMPDecryptErrors++;
-                    } else if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_WEP)) {
+		if ((*pbyRsr & RSR_CRCOK) != 0) {
+			pDevice->byBasicMap |= 0x01;
+		}
+		dwDuration = (FrameSize << 4);
+		dwDuration /= acbyRxRate[*pbyRxRate%MAX_RATE];
+		if (*pbyRxRate <= RATE_11M) {
+			if (*pbyRxSts & 0x01) {
+				// long preamble
+				dwDuration += 192;
+			} else {
+				// short preamble
+				dwDuration += 96;
+			}
+		} else {
+			dwDuration += 16;
+		}
+		RFvRSSITodBm(pDevice, *pbyRSSI, &ldBm);
+		ldBmThreshold = -57;
+		for (ii = 7; ii > 0;) {
+			if (ldBm > ldBmThreshold) {
+				break;
+			}
+			ldBmThreshold -= 5;
+			ii--;
+		}
+		pDevice->dwRPIs[ii] += dwDuration;
+		return false;
+	}
+
+	if (!is_multicast_ether_addr(pbyFrame)) {
+		if (WCTLbIsDuplicate(&(pDevice->sDupRxCache), (PS802_11Header)(skb->data + 4))) {
+			pDevice->s802_11Counter.FrameDuplicateCount++;
+			return false;
+		}
+	}
+
+	// Use for TKIP MIC
+	s_vGetDASA(skb->data+4, &cbHeaderSize, &pDevice->sRxEthHeader);
+
+	// filter packet send from myself
+	if (!compare_ether_addr((unsigned char *)&(pDevice->sRxEthHeader.abySrcAddr[0]), pDevice->abyCurrentNetAddr))
+		return false;
+
+	if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) || (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA)) {
+		if (IS_CTL_PSPOLL(pbyFrame) || !IS_TYPE_CONTROL(pbyFrame)) {
+			p802_11Header = (PS802_11Header)(pbyFrame);
+			// get SA NodeIndex
+			if (BSSDBbIsSTAInNodeDB(pMgmt, (unsigned char *)(p802_11Header->abyAddr2), &iSANodeIndex)) {
+				pMgmt->sNodeDBTable[iSANodeIndex].ulLastRxJiffer = jiffies;
+				pMgmt->sNodeDBTable[iSANodeIndex].uInActiveCount = 0;
+			}
+		}
+	}
+
+	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
+		if (s_bAPModeRxCtl(pDevice, pbyFrame, iSANodeIndex) == true) {
+			return false;
+		}
+	}
+
+	if (IS_FC_WEP(pbyFrame)) {
+		bool bRxDecryOK = false;
+
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx WEP pkt\n");
+		bIsWEP = true;
+		if ((pDevice->bEnableHostWEP) && (iSANodeIndex >= 0)) {
+			pKey = &STempKey;
+			pKey->byCipherSuite = pMgmt->sNodeDBTable[iSANodeIndex].byCipherSuite;
+			pKey->dwKeyIndex = pMgmt->sNodeDBTable[iSANodeIndex].dwKeyIndex;
+			pKey->uKeyLength = pMgmt->sNodeDBTable[iSANodeIndex].uWepKeyLength;
+			pKey->dwTSC47_16 = pMgmt->sNodeDBTable[iSANodeIndex].dwTSC47_16;
+			pKey->wTSC15_0 = pMgmt->sNodeDBTable[iSANodeIndex].wTSC15_0;
+			memcpy(pKey->abyKey,
+			       &pMgmt->sNodeDBTable[iSANodeIndex].abyWepKey[0],
+			       pKey->uKeyLength
+);
+
+			bRxDecryOK = s_bHostWepRxEncryption(pDevice,
+							    pbyFrame,
+							    FrameSize,
+							    pbyRsr,
+							    pMgmt->sNodeDBTable[iSANodeIndex].bOnFly,
+							    pKey,
+							    pbyNewRsr,
+							    &bExtIV,
+							    &wRxTSC15_0,
+							    &dwRxTSC47_16);
+		} else {
+			bRxDecryOK = s_bHandleRxEncryption(pDevice,
+							   pbyFrame,
+							   FrameSize,
+							   pbyRsr,
+							   pbyNewRsr,
+							   &pKey,
+							   &bExtIV,
+							   &wRxTSC15_0,
+							   &dwRxTSC47_16);
+		}
+
+		if (bRxDecryOK) {
+			if ((*pbyNewRsr & NEWRSR_DECRYPTOK) == 0) {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ICV Fail\n");
+				if ((pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPA) ||
+				    (pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) ||
+				    (pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) ||
+				    (pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPA2) ||
+				    (pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) {
+					if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_TKIP)) {
+						pDevice->s802_11Counter.TKIPICVErrors++;
+					} else if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_CCMP)) {
+						pDevice->s802_11Counter.CCMPDecryptErrors++;
+					} else if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_WEP)) {
 //                      pDevice->s802_11Counter.WEPICVErrorCount.QuadPart++;
-                    }
-                }
-                return false;
-            }
-        } else {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"WEP Func Fail\n");
-            return false;
-        }
-        if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_CCMP))
-            FrameSize -= 8;         // Message Integrity Code
-        else
-            FrameSize -= 4;         // 4 is ICV
-    }
-
-
-    //
-    // RX OK
-    //
-    //remove the CRC length
-    FrameSize -= ETH_FCS_LEN;
-
-    if (( !(*pbyRsr & (RSR_ADDRBROAD | RSR_ADDRMULTI))) && // unicast address
-        (IS_FRAGMENT_PKT((skb->data+4)))
-        ) {
-        // defragment
-        bDeFragRx = WCTLbHandleFragment(pDevice, (PS802_11Header) (skb->data+4), FrameSize, bIsWEP, bExtIV);
-        pDevice->s802_11Counter.ReceivedFragmentCount++;
-        if (bDeFragRx) {
-            // defrag complete
-            skb = pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].skb;
-            FrameSize = pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].cbFrameLength;
-
-        }
-        else {
-            return false;
-        }
-    }
+					}
+				}
+				return false;
+			}
+		} else {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "WEP Func Fail\n");
+			return false;
+		}
+		if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_CCMP))
+			FrameSize -= 8;         // Message Integrity Code
+		else
+			FrameSize -= 4;         // 4 is ICV
+	}
 
+	//
+	// RX OK
+	//
+	//remove the CRC length
+	FrameSize -= ETH_FCS_LEN;
+
+	if ((!(*pbyRsr & (RSR_ADDRBROAD | RSR_ADDRMULTI))) && // unicast address
+	    (IS_FRAGMENT_PKT((skb->data+4)))
+) {
+		// defragment
+		bDeFragRx = WCTLbHandleFragment(pDevice, (PS802_11Header)(skb->data+4), FrameSize, bIsWEP, bExtIV);
+		pDevice->s802_11Counter.ReceivedFragmentCount++;
+		if (bDeFragRx) {
+			// defrag complete
+			skb = pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].skb;
+			FrameSize = pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].cbFrameLength;
+
+		} else {
+			return false;
+		}
+	}
 
 // Management & Control frame Handle
-    if ((IS_TYPE_DATA((skb->data+4))) == false) {
-        // Handle Control & Manage Frame
-
-        if (IS_TYPE_MGMT((skb->data+4))) {
-            unsigned char *pbyData1;
-            unsigned char *pbyData2;
-
-            pRxPacket->p80211Header = (PUWLAN_80211HDR)(skb->data+4);
-            pRxPacket->cbMPDULen = FrameSize;
-            pRxPacket->uRSSI = *pbyRSSI;
-            pRxPacket->bySQ = *pbySQ;
-            HIDWORD(pRxPacket->qwLocalTSF) = cpu_to_le32(HIDWORD(*pqwTSFTime));
-            LODWORD(pRxPacket->qwLocalTSF) = cpu_to_le32(LODWORD(*pqwTSFTime));
-            if (bIsWEP) {
-                // strip IV
-                pbyData1 = WLAN_HDR_A3_DATA_PTR(skb->data+4);
-                pbyData2 = WLAN_HDR_A3_DATA_PTR(skb->data+4) + 4;
-                for (ii = 0; ii < (FrameSize - 4); ii++) {
-                    *pbyData1 = *pbyData2;
-                     pbyData1++;
-                     pbyData2++;
-                }
-            }
-            pRxPacket->byRxRate = s_byGetRateIdx(*pbyRxRate);
-            pRxPacket->byRxChannel = (*pbyRxSts) >> 2;
+	if ((IS_TYPE_DATA((skb->data+4))) == false) {
+		// Handle Control & Manage Frame
+
+		if (IS_TYPE_MGMT((skb->data+4))) {
+			unsigned char *pbyData1;
+			unsigned char *pbyData2;
+
+			pRxPacket->p80211Header = (PUWLAN_80211HDR)(skb->data+4);
+			pRxPacket->cbMPDULen = FrameSize;
+			pRxPacket->uRSSI = *pbyRSSI;
+			pRxPacket->bySQ = *pbySQ;
+			HIDWORD(pRxPacket->qwLocalTSF) = cpu_to_le32(HIDWORD(*pqwTSFTime));
+			LODWORD(pRxPacket->qwLocalTSF) = cpu_to_le32(LODWORD(*pqwTSFTime));
+			if (bIsWEP) {
+				// strip IV
+				pbyData1 = WLAN_HDR_A3_DATA_PTR(skb->data+4);
+				pbyData2 = WLAN_HDR_A3_DATA_PTR(skb->data+4) + 4;
+				for (ii = 0; ii < (FrameSize - 4); ii++) {
+					*pbyData1 = *pbyData2;
+					pbyData1++;
+					pbyData2++;
+				}
+			}
+			pRxPacket->byRxRate = s_byGetRateIdx(*pbyRxRate);
+			pRxPacket->byRxChannel = (*pbyRxSts) >> 2;
 //PLICE_DEBUG->
 //EnQueue(pDevice,pRxPacket);
 
 #ifdef	THREAD
-		EnQueue(pDevice,pRxPacket);
+			EnQueue(pDevice, pRxPacket);
 
-		//printk("enque time is %x\n",jiffies);
-		//up(&pDevice->mlme_semaphore);
+			//up(&pDevice->mlme_semaphore);
 			//Enque (pDevice->FirstRecvMngList,pDevice->LastRecvMngList,pMgmt);
 #else
 
 #ifdef	TASK_LET
-		EnQueue(pDevice,pRxPacket);
-		tasklet_schedule(&pDevice->RxMngWorkItem);
+			EnQueue(pDevice, pRxPacket);
+			tasklet_schedule(&pDevice->RxMngWorkItem);
 #else
-//printk("RxMan\n");
-	vMgrRxManagePacket((void *)pDevice, pDevice->pMgmt, pRxPacket);
-           //tasklet_schedule(&pDevice->RxMngWorkItem);
+			vMgrRxManagePacket((void *)pDevice, pDevice->pMgmt, pRxPacket);
+			//tasklet_schedule(&pDevice->RxMngWorkItem);
 #endif
 
 #endif
 //PLICE_DEBUG<-
 			//vMgrRxManagePacket((void *)pDevice, pDevice->pMgmt, pRxPacket);
-            // hostap Deamon handle 802.11 management
-            if (pDevice->bEnableHostapd) {
-	            skb->dev = pDevice->apdev;
-	            skb->data += 4;
-	            skb->tail += 4;
-                     skb_put(skb, FrameSize);
-		skb_reset_mac_header(skb);
-	            skb->pkt_type = PACKET_OTHERHOST;
-    	        skb->protocol = htons(ETH_P_802_2);
-	            memset(skb->cb, 0, sizeof(skb->cb));
-	            netif_rx(skb);
-                return true;
-	        }
-        }
-        else {
-            // Control Frame
-        };
-        return false;
-    }
-    else {
-        if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-            //In AP mode, hw only check addr1(BSSID or RA) if equal to local MAC.
-            if ( !(*pbyRsr & RSR_BSSIDOK)) {
-                if (bDeFragRx) {
-                    if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
-                        DBG_PRT(MSG_LEVEL_ERR,KERN_ERR "%s: can not alloc more frag bufs\n",
-                        pDevice->dev->name);
-                    }
-                }
-                return false;
-            }
-        }
-        else {
-            // discard DATA packet while not associate || BSSID error
-            if ((pDevice->bLinkPass == false) ||
-                !(*pbyRsr & RSR_BSSIDOK)) {
-                if (bDeFragRx) {
-                    if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
-                        DBG_PRT(MSG_LEVEL_ERR,KERN_ERR "%s: can not alloc more frag bufs\n",
-                        pDevice->dev->name);
-                    }
-                }
-                return false;
-            }
-   //mike add:station mode check eapol-key challenge--->
-   	  {
-   	    unsigned char Protocol_Version;    //802.1x Authentication
-	    unsigned char Packet_Type;           //802.1x Authentication
-              if (bIsWEP)
-                  cbIVOffset = 8;
-              else
-                  cbIVOffset = 0;
-              wEtherType = (skb->data[cbIVOffset + 8 + 24 + 6] << 8) |
-                          skb->data[cbIVOffset + 8 + 24 + 6 + 1];
-	      Protocol_Version = skb->data[cbIVOffset + 8 + 24 + 6 + 1 +1];
-	      Packet_Type = skb->data[cbIVOffset + 8 + 24 + 6 + 1 +1+1];
-	     if (wEtherType == ETH_P_PAE) {         //Protocol Type in LLC-Header
-                  if(((Protocol_Version==1) ||(Protocol_Version==2)) &&
-		     (Packet_Type==3)) {  //802.1x OR eapol-key challenge frame receive
-                        bRxeapol_key = true;
-                  }
-	      }
-   	  }
-    //mike add:station mode check eapol-key challenge<---
-        }
-    }
-
+			// hostap Deamon handle 802.11 management
+			if (pDevice->bEnableHostapd) {
+				skb->dev = pDevice->apdev;
+				skb->data += 4;
+				skb->tail += 4;
+				skb_put(skb, FrameSize);
+				skb_reset_mac_header(skb);
+				skb->pkt_type = PACKET_OTHERHOST;
+				skb->protocol = htons(ETH_P_802_2);
+				memset(skb->cb, 0, sizeof(skb->cb));
+				netif_rx(skb);
+				return true;
+			}
+		} else {
+			// Control Frame
+		};
+		return false;
+	} else {
+		if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
+			//In AP mode, hw only check addr1(BSSID or RA) if equal to local MAC.
+			if (!(*pbyRsr & RSR_BSSIDOK)) {
+				if (bDeFragRx) {
+					if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
+						DBG_PRT(MSG_LEVEL_ERR, KERN_ERR "%s: can not alloc more frag bufs\n",
+							pDevice->dev->name);
+					}
+				}
+				return false;
+			}
+		} else {
+			// discard DATA packet while not associate || BSSID error
+			if ((pDevice->bLinkPass == false) ||
+			    !(*pbyRsr & RSR_BSSIDOK)) {
+				if (bDeFragRx) {
+					if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
+						DBG_PRT(MSG_LEVEL_ERR, KERN_ERR "%s: can not alloc more frag bufs\n",
+							pDevice->dev->name);
+					}
+				}
+				return false;
+			}
+			//mike add:station mode check eapol-key challenge--->
+			{
+				unsigned char Protocol_Version;    //802.1x Authentication
+				unsigned char Packet_Type;           //802.1x Authentication
+				if (bIsWEP)
+					cbIVOffset = 8;
+				else
+					cbIVOffset = 0;
+				wEtherType = (skb->data[cbIVOffset + 8 + 24 + 6] << 8) |
+					skb->data[cbIVOffset + 8 + 24 + 6 + 1];
+				Protocol_Version = skb->data[cbIVOffset + 8 + 24 + 6 + 1 + 1];
+				Packet_Type = skb->data[cbIVOffset + 8 + 24 + 6 + 1 + 1 + 1];
+				if (wEtherType == ETH_P_PAE) {         //Protocol Type in LLC-Header
+					if (((Protocol_Version == 1) || (Protocol_Version == 2)) &&
+					    (Packet_Type == 3)) {  //802.1x OR eapol-key challenge frame receive
+						bRxeapol_key = true;
+					}
+				}
+			}
+			//mike add:station mode check eapol-key challenge<---
+		}
+	}
 
 // Data frame Handle
 
+	if (pDevice->bEnablePSMode) {
+		if (IS_FC_MOREDATA((skb->data+4))) {
+			if (*pbyRsr & RSR_ADDROK) {
+				//PSbSendPSPOLL((PSDevice)pDevice);
+			}
+		} else {
+			if (pDevice->pMgmt->bInTIMWake == true) {
+				pDevice->pMgmt->bInTIMWake = false;
+			}
+		}
+	}
 
-    if (pDevice->bEnablePSMode) {
-        if (IS_FC_MOREDATA((skb->data+4))) {
-            if (*pbyRsr & RSR_ADDROK) {
-                //PSbSendPSPOLL((PSDevice)pDevice);
-            }
-        }
-        else {
-            if (pDevice->pMgmt->bInTIMWake == true) {
-                pDevice->pMgmt->bInTIMWake = false;
-            }
-        }
-    }
-
-    // Now it only supports 802.11g Infrastructure Mode, and support rate must up to 54 Mbps
-    if (pDevice->bDiversityEnable && (FrameSize>50) &&
-        (pDevice->eOPMode == OP_MODE_INFRASTRUCTURE) &&
-        (pDevice->bLinkPass == true)) {
-	//printk("device_receive_frame: RxRate is %d\n",*pbyRxRate);
+	// Now it only supports 802.11g Infrastructure Mode, and support rate must up to 54 Mbps
+	if (pDevice->bDiversityEnable && (FrameSize > 50) &&
+	    (pDevice->eOPMode == OP_MODE_INFRASTRUCTURE) &&
+	    (pDevice->bLinkPass == true)) {
 		BBvAntennaDiversity(pDevice, s_byGetRateIdx(*pbyRxRate), 0);
-    }
-
-
-    if (pDevice->byLocalID != REV_ID_VT3253_B1) {
-        pDevice->uCurrRSSI = *pbyRSSI;
-    }
-    pDevice->byCurrSQ = *pbySQ;
-
-    if ((*pbyRSSI != 0) &&
-        (pMgmt->pCurrBSS!=NULL)) {
-        RFvRSSITodBm(pDevice, *pbyRSSI, &ldBm);
-        // Monitor if RSSI is too strong.
-        pMgmt->pCurrBSS->byRSSIStatCnt++;
-        pMgmt->pCurrBSS->byRSSIStatCnt %= RSSI_STAT_COUNT;
-        pMgmt->pCurrBSS->ldBmAverage[pMgmt->pCurrBSS->byRSSIStatCnt] = ldBm;
-        for(ii=0;ii<RSSI_STAT_COUNT;ii++) {
-            if (pMgmt->pCurrBSS->ldBmAverage[ii] != 0) {
-            pMgmt->pCurrBSS->ldBmMAX = max(pMgmt->pCurrBSS->ldBmAverage[ii], ldBm);
-            }
-        }
-    }
-
-    // -----------------------------------------------
-
-    if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) && (pDevice->bEnable8021x == true)){
-        unsigned char abyMacHdr[24];
-
-        // Only 802.1x packet incoming allowed
-        if (bIsWEP)
-            cbIVOffset = 8;
-        else
-            cbIVOffset = 0;
-        wEtherType = (skb->data[cbIVOffset + 4 + 24 + 6] << 8) |
-                    skb->data[cbIVOffset + 4 + 24 + 6 + 1];
-
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"wEtherType = %04x \n", wEtherType);
-        if (wEtherType == ETH_P_PAE) {
-            skb->dev = pDevice->apdev;
-
-            if (bIsWEP == true) {
-                // strip IV header(8)
-                memcpy(&abyMacHdr[0], (skb->data + 4), 24);
-                memcpy((skb->data + 4 + cbIVOffset), &abyMacHdr[0], 24);
-            }
-            skb->data +=  (cbIVOffset + 4);
-            skb->tail +=  (cbIVOffset + 4);
-            skb_put(skb, FrameSize);
-	    skb_reset_mac_header(skb);
-
-	skb->pkt_type = PACKET_OTHERHOST;
-            skb->protocol = htons(ETH_P_802_2);
-            memset(skb->cb, 0, sizeof(skb->cb));
-            netif_rx(skb);
-            return true;
+	}
 
-}
-        // check if 802.1x authorized
-        if (!(pMgmt->sNodeDBTable[iSANodeIndex].dwFlags & WLAN_STA_AUTHORIZED))
-            return false;
-    }
-
-
-    if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_TKIP)) {
-        if (bIsWEP) {
-            FrameSize -= 8;  //MIC
-        }
-    }
-
-    //--------------------------------------------------------------------------------
-    // Soft MIC
-    if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_TKIP)) {
-        if (bIsWEP) {
-            unsigned long *pdwMIC_L;
-            unsigned long *pdwMIC_R;
-            unsigned long dwMIC_Priority;
-            unsigned long dwMICKey0 = 0, dwMICKey1 = 0;
-            unsigned long dwLocalMIC_L = 0;
-            unsigned long dwLocalMIC_R = 0;
-            viawget_wpa_header *wpahdr;
-
-
-            if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-                dwMICKey0 = cpu_to_le32(*(unsigned long *)(&pKey->abyKey[24]));
-                dwMICKey1 = cpu_to_le32(*(unsigned long *)(&pKey->abyKey[28]));
-            }
-            else {
-                if (pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) {
-                    dwMICKey0 = cpu_to_le32(*(unsigned long *)(&pKey->abyKey[16]));
-                    dwMICKey1 = cpu_to_le32(*(unsigned long *)(&pKey->abyKey[20]));
-                } else if ((pKey->dwKeyIndex & BIT28) == 0) {
-                    dwMICKey0 = cpu_to_le32(*(unsigned long *)(&pKey->abyKey[16]));
-                    dwMICKey1 = cpu_to_le32(*(unsigned long *)(&pKey->abyKey[20]));
-                } else {
-                    dwMICKey0 = cpu_to_le32(*(unsigned long *)(&pKey->abyKey[24]));
-                    dwMICKey1 = cpu_to_le32(*(unsigned long *)(&pKey->abyKey[28]));
-                }
-            }
-
-            MIC_vInit(dwMICKey0, dwMICKey1);
-            MIC_vAppend((unsigned char *)&(pDevice->sRxEthHeader.abyDstAddr[0]), 12);
-            dwMIC_Priority = 0;
-            MIC_vAppend((unsigned char *)&dwMIC_Priority, 4);
-            // 4 is Rcv buffer header, 24 is MAC Header, and 8 is IV and Ext IV.
-            MIC_vAppend((unsigned char *)(skb->data + 4 + WLAN_HDR_ADDR3_LEN + 8),
-                        FrameSize - WLAN_HDR_ADDR3_LEN - 8);
-            MIC_vGetMIC(&dwLocalMIC_L, &dwLocalMIC_R);
-            MIC_vUnInit();
-
-            pdwMIC_L = (unsigned long *)(skb->data + 4 + FrameSize);
-            pdwMIC_R = (unsigned long *)(skb->data + 4 + FrameSize + 4);
-            //DBG_PRN_GRP12(("RxL: %lx, RxR: %lx\n", *pdwMIC_L, *pdwMIC_R));
-            //DBG_PRN_GRP12(("LocalL: %lx, LocalR: %lx\n", dwLocalMIC_L, dwLocalMIC_R));
-            //DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"dwMICKey0= %lx,dwMICKey1= %lx \n", dwMICKey0, dwMICKey1);
-
-
-            if ((cpu_to_le32(*pdwMIC_L) != dwLocalMIC_L) || (cpu_to_le32(*pdwMIC_R) != dwLocalMIC_R) ||
-                (pDevice->bRxMICFail == true)) {
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"MIC comparison is fail!\n");
-                pDevice->bRxMICFail = false;
-                //pDevice->s802_11Counter.TKIPLocalMICFailures.QuadPart++;
-                pDevice->s802_11Counter.TKIPLocalMICFailures++;
-                if (bDeFragRx) {
-                    if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
-                        DBG_PRT(MSG_LEVEL_ERR,KERN_ERR "%s: can not alloc more frag bufs\n",
-                            pDevice->dev->name);
-                    }
-                }
-               //2008-0409-07, <Add> by Einsn Liu
-       #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
+	if (pDevice->byLocalID != REV_ID_VT3253_B1) {
+		pDevice->uCurrRSSI = *pbyRSSI;
+	}
+	pDevice->byCurrSQ = *pbySQ;
+
+	if ((*pbyRSSI != 0) &&
+	    (pMgmt->pCurrBSS != NULL)) {
+		RFvRSSITodBm(pDevice, *pbyRSSI, &ldBm);
+		// Monitor if RSSI is too strong.
+		pMgmt->pCurrBSS->byRSSIStatCnt++;
+		pMgmt->pCurrBSS->byRSSIStatCnt %= RSSI_STAT_COUNT;
+		pMgmt->pCurrBSS->ldBmAverage[pMgmt->pCurrBSS->byRSSIStatCnt] = ldBm;
+		for (ii = 0; ii < RSSI_STAT_COUNT; ii++) {
+			if (pMgmt->pCurrBSS->ldBmAverage[ii] != 0) {
+				pMgmt->pCurrBSS->ldBmMAX = max(pMgmt->pCurrBSS->ldBmAverage[ii], ldBm);
+			}
+		}
+	}
+
+	// -----------------------------------------------
+
+	if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) && (pDevice->bEnable8021x == true)) {
+		unsigned char abyMacHdr[24];
+
+		// Only 802.1x packet incoming allowed
+		if (bIsWEP)
+			cbIVOffset = 8;
+		else
+			cbIVOffset = 0;
+		wEtherType = (skb->data[cbIVOffset + 4 + 24 + 6] << 8) |
+			skb->data[cbIVOffset + 4 + 24 + 6 + 1];
+
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wEtherType = %04x \n", wEtherType);
+		if (wEtherType == ETH_P_PAE) {
+			skb->dev = pDevice->apdev;
+
+			if (bIsWEP == true) {
+				// strip IV header(8)
+				memcpy(&abyMacHdr[0], (skb->data + 4), 24);
+				memcpy((skb->data + 4 + cbIVOffset), &abyMacHdr[0], 24);
+			}
+			skb->data +=  (cbIVOffset + 4);
+			skb->tail +=  (cbIVOffset + 4);
+			skb_put(skb, FrameSize);
+			skb_reset_mac_header(skb);
+
+			skb->pkt_type = PACKET_OTHERHOST;
+			skb->protocol = htons(ETH_P_802_2);
+			memset(skb->cb, 0, sizeof(skb->cb));
+			netif_rx(skb);
+			return true;
+
+		}
+		// check if 802.1x authorized
+		if (!(pMgmt->sNodeDBTable[iSANodeIndex].dwFlags & WLAN_STA_AUTHORIZED))
+			return false;
+	}
+
+	if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_TKIP)) {
+		if (bIsWEP) {
+			FrameSize -= 8;  //MIC
+		}
+	}
+
+	//--------------------------------------------------------------------------------
+	// Soft MIC
+	if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_TKIP)) {
+		if (bIsWEP) {
+			unsigned long *pdwMIC_L;
+			unsigned long *pdwMIC_R;
+			unsigned long dwMIC_Priority;
+			unsigned long dwMICKey0 = 0, dwMICKey1 = 0;
+			unsigned long dwLocalMIC_L = 0;
+			unsigned long dwLocalMIC_R = 0;
+			viawget_wpa_header *wpahdr;
+
+			if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
+				dwMICKey0 = cpu_to_le32(*(unsigned long *)(&pKey->abyKey[24]));
+				dwMICKey1 = cpu_to_le32(*(unsigned long *)(&pKey->abyKey[28]));
+			} else {
+				if (pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) {
+					dwMICKey0 = cpu_to_le32(*(unsigned long *)(&pKey->abyKey[16]));
+					dwMICKey1 = cpu_to_le32(*(unsigned long *)(&pKey->abyKey[20]));
+				} else if ((pKey->dwKeyIndex & BIT28) == 0) {
+					dwMICKey0 = cpu_to_le32(*(unsigned long *)(&pKey->abyKey[16]));
+					dwMICKey1 = cpu_to_le32(*(unsigned long *)(&pKey->abyKey[20]));
+				} else {
+					dwMICKey0 = cpu_to_le32(*(unsigned long *)(&pKey->abyKey[24]));
+					dwMICKey1 = cpu_to_le32(*(unsigned long *)(&pKey->abyKey[28]));
+				}
+			}
+
+			MIC_vInit(dwMICKey0, dwMICKey1);
+			MIC_vAppend((unsigned char *)&(pDevice->sRxEthHeader.abyDstAddr[0]), 12);
+			dwMIC_Priority = 0;
+			MIC_vAppend((unsigned char *)&dwMIC_Priority, 4);
+			// 4 is Rcv buffer header, 24 is MAC Header, and 8 is IV and Ext IV.
+			MIC_vAppend((unsigned char *)(skb->data + 4 + WLAN_HDR_ADDR3_LEN + 8),
+				    FrameSize - WLAN_HDR_ADDR3_LEN - 8);
+			MIC_vGetMIC(&dwLocalMIC_L, &dwLocalMIC_R);
+			MIC_vUnInit();
+
+			pdwMIC_L = (unsigned long *)(skb->data + 4 + FrameSize);
+			pdwMIC_R = (unsigned long *)(skb->data + 4 + FrameSize + 4);
+			//DBG_PRN_GRP12(("RxL: %lx, RxR: %lx\n", *pdwMIC_L, *pdwMIC_R));
+			//DBG_PRN_GRP12(("LocalL: %lx, LocalR: %lx\n", dwLocalMIC_L, dwLocalMIC_R));
+			//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dwMICKey0= %lx,dwMICKey1= %lx \n", dwMICKey0, dwMICKey1);
+
+			if ((cpu_to_le32(*pdwMIC_L) != dwLocalMIC_L) || (cpu_to_le32(*pdwMIC_R) != dwLocalMIC_R) ||
+			    (pDevice->bRxMICFail == true)) {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "MIC comparison is fail!\n");
+				pDevice->bRxMICFail = false;
+				//pDevice->s802_11Counter.TKIPLocalMICFailures.QuadPart++;
+				pDevice->s802_11Counter.TKIPLocalMICFailures++;
+				if (bDeFragRx) {
+					if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
+						DBG_PRT(MSG_LEVEL_ERR, KERN_ERR "%s: can not alloc more frag bufs\n",
+							pDevice->dev->name);
+					}
+				}
+				//2008-0409-07, <Add> by Einsn Liu
+#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
 				//send event to wpa_supplicant
-				//if(pDevice->bWPADevEnable == true)
 				{
 					union iwreq_data wrqu;
 					struct iw_michaelmicfailure ev;
@@ -845,8 +791,8 @@ device_receive_frame (
 					memset(&ev, 0, sizeof(ev));
 					ev.flags = keyidx & IW_MICFAILURE_KEY_ID;
 					if ((pMgmt->eCurrMode == WMAC_MODE_ESS_STA) &&
-							(pMgmt->eCurrState == WMAC_STATE_ASSOC) &&
-								(*pbyRsr & (RSR_ADDRBROAD | RSR_ADDRMULTI)) == 0) {
+					    (pMgmt->eCurrState == WMAC_STATE_ASSOC) &&
+					    (*pbyRsr & (RSR_ADDRBROAD | RSR_ADDRMULTI)) == 0) {
 						ev.flags |= IW_MICFAILURE_PAIRWISE;
 					} else {
 						ev.flags |= IW_MICFAILURE_GROUP;
@@ -859,633 +805,596 @@ device_receive_frame (
 					wireless_send_event(pDevice->dev, IWEVMICHAELMICFAILURE, &wrqu, (char *)&ev);
 
 				}
-         #endif
-
-
-                if ((pDevice->bWPADEVUp) && (pDevice->skb != NULL)) {
-                     wpahdr = (viawget_wpa_header *)pDevice->skb->data;
-                     if ((pDevice->pMgmt->eCurrMode == WMAC_MODE_ESS_STA) &&
-                         (pDevice->pMgmt->eCurrState == WMAC_STATE_ASSOC) &&
-                         (*pbyRsr & (RSR_ADDRBROAD | RSR_ADDRMULTI)) == 0) {
-                         //s802_11_Status.Flags = NDIS_802_11_AUTH_REQUEST_PAIRWISE_ERROR;
-                         wpahdr->type = VIAWGET_PTK_MIC_MSG;
-                     } else {
-                         //s802_11_Status.Flags = NDIS_802_11_AUTH_REQUEST_GROUP_ERROR;
-                         wpahdr->type = VIAWGET_GTK_MIC_MSG;
-                     }
-                     wpahdr->resp_ie_len = 0;
-                     wpahdr->req_ie_len = 0;
-                     skb_put(pDevice->skb, sizeof(viawget_wpa_header));
-                     pDevice->skb->dev = pDevice->wpadev;
-		     skb_reset_mac_header(pDevice->skb);
-                     pDevice->skb->pkt_type = PACKET_HOST;
-                     pDevice->skb->protocol = htons(ETH_P_802_2);
-                     memset(pDevice->skb->cb, 0, sizeof(pDevice->skb->cb));
-                     netif_rx(pDevice->skb);
-                     pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
-                 }
-
-                return false;
-
-            }
-        }
-    } //---end of SOFT MIC-----------------------------------------------------------------------
-
-    // ++++++++++ Reply Counter Check +++++++++++++
-
-    if ((pKey != NULL) && ((pKey->byCipherSuite == KEY_CTL_TKIP) ||
-                           (pKey->byCipherSuite == KEY_CTL_CCMP))) {
-        if (bIsWEP) {
-            unsigned short wLocalTSC15_0 = 0;
-            unsigned long dwLocalTSC47_16 = 0;
-            unsigned long long       RSC = 0;
-            // endian issues
-            RSC = *((unsigned long long *) &(pKey->KeyRSC));
-            wLocalTSC15_0 = (unsigned short) RSC;
-            dwLocalTSC47_16 = (unsigned long) (RSC>>16);
-
-            RSC = dwRxTSC47_16;
-            RSC <<= 16;
-            RSC += wRxTSC15_0;
-            memcpy(&(pKey->KeyRSC), &RSC,  sizeof(QWORD));
-
-            if ( (pDevice->sMgmtObj.eCurrMode == WMAC_MODE_ESS_STA) &&
-                 (pDevice->sMgmtObj.eCurrState == WMAC_STATE_ASSOC)) {
-                // check RSC
-                if ( (wRxTSC15_0 < wLocalTSC15_0) &&
-                     (dwRxTSC47_16 <= dwLocalTSC47_16) &&
-                     !((dwRxTSC47_16 == 0) && (dwLocalTSC47_16 == 0xFFFFFFFF))) {
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"TSC is illegal~~!\n ");
-                    if (pKey->byCipherSuite == KEY_CTL_TKIP)
-                        //pDevice->s802_11Counter.TKIPReplays.QuadPart++;
-                        pDevice->s802_11Counter.TKIPReplays++;
-                    else
-                        //pDevice->s802_11Counter.CCMPReplays.QuadPart++;
-                        pDevice->s802_11Counter.CCMPReplays++;
-
-                    if (bDeFragRx) {
-                        if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
-                            DBG_PRT(MSG_LEVEL_ERR,KERN_ERR "%s: can not alloc more frag bufs\n",
-                                pDevice->dev->name);
-                        }
-                    }
-                    return false;
-                }
-            }
-        }
-    } // ----- End of Reply Counter Check --------------------------
-
-
-
-    if ((pKey != NULL) && (bIsWEP)) {
+#endif
+
+				if ((pDevice->bWPADEVUp) && (pDevice->skb != NULL)) {
+					wpahdr = (viawget_wpa_header *)pDevice->skb->data;
+					if ((pDevice->pMgmt->eCurrMode == WMAC_MODE_ESS_STA) &&
+					    (pDevice->pMgmt->eCurrState == WMAC_STATE_ASSOC) &&
+					    (*pbyRsr & (RSR_ADDRBROAD | RSR_ADDRMULTI)) == 0) {
+						//s802_11_Status.Flags = NDIS_802_11_AUTH_REQUEST_PAIRWISE_ERROR;
+						wpahdr->type = VIAWGET_PTK_MIC_MSG;
+					} else {
+						//s802_11_Status.Flags = NDIS_802_11_AUTH_REQUEST_GROUP_ERROR;
+						wpahdr->type = VIAWGET_GTK_MIC_MSG;
+					}
+					wpahdr->resp_ie_len = 0;
+					wpahdr->req_ie_len = 0;
+					skb_put(pDevice->skb, sizeof(viawget_wpa_header));
+					pDevice->skb->dev = pDevice->wpadev;
+					skb_reset_mac_header(pDevice->skb);
+					pDevice->skb->pkt_type = PACKET_HOST;
+					pDevice->skb->protocol = htons(ETH_P_802_2);
+					memset(pDevice->skb->cb, 0, sizeof(pDevice->skb->cb));
+					netif_rx(pDevice->skb);
+					pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
+				}
+
+				return false;
+
+			}
+		}
+	} //---end of SOFT MIC-----------------------------------------------------------------------
+
+	// ++++++++++ Reply Counter Check +++++++++++++
+
+	if ((pKey != NULL) && ((pKey->byCipherSuite == KEY_CTL_TKIP) ||
+			       (pKey->byCipherSuite == KEY_CTL_CCMP))) {
+		if (bIsWEP) {
+			unsigned short wLocalTSC15_0 = 0;
+			unsigned long dwLocalTSC47_16 = 0;
+			unsigned long long       RSC = 0;
+			// endian issues
+			RSC = *((unsigned long long *)&(pKey->KeyRSC));
+			wLocalTSC15_0 = (unsigned short)RSC;
+			dwLocalTSC47_16 = (unsigned long)(RSC>>16);
+
+			RSC = dwRxTSC47_16;
+			RSC <<= 16;
+			RSC += wRxTSC15_0;
+			memcpy(&(pKey->KeyRSC), &RSC,  sizeof(QWORD));
+
+			if ((pDevice->sMgmtObj.eCurrMode == WMAC_MODE_ESS_STA) &&
+			    (pDevice->sMgmtObj.eCurrState == WMAC_STATE_ASSOC)) {
+				// check RSC
+				if ((wRxTSC15_0 < wLocalTSC15_0) &&
+				    (dwRxTSC47_16 <= dwLocalTSC47_16) &&
+				    !((dwRxTSC47_16 == 0) && (dwLocalTSC47_16 == 0xFFFFFFFF))) {
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "TSC is illegal~~!\n ");
+					if (pKey->byCipherSuite == KEY_CTL_TKIP)
+						//pDevice->s802_11Counter.TKIPReplays.QuadPart++;
+						pDevice->s802_11Counter.TKIPReplays++;
+					else
+						//pDevice->s802_11Counter.CCMPReplays.QuadPart++;
+						pDevice->s802_11Counter.CCMPReplays++;
+
+					if (bDeFragRx) {
+						if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
+							DBG_PRT(MSG_LEVEL_ERR, KERN_ERR "%s: can not alloc more frag bufs\n",
+								pDevice->dev->name);
+						}
+					}
+					return false;
+				}
+			}
+		}
+	} // ----- End of Reply Counter Check --------------------------
+
+	if ((pKey != NULL) && (bIsWEP)) {
 //      pDevice->s802_11Counter.DecryptSuccessCount.QuadPart++;
-    }
-
-
-    s_vProcessRxMACHeader(pDevice, (unsigned char *)(skb->data+4), FrameSize, bIsWEP, bExtIV, &cbHeaderOffset);
-    FrameSize -= cbHeaderOffset;
-    cbHeaderOffset += 4;        // 4 is Rcv buffer header
-
-    // Null data, framesize = 14
-    if (FrameSize < 15)
-        return false;
-
-    if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-        if (s_bAPModeRxData(pDevice,
-                            skb,
-                            FrameSize,
-                            cbHeaderOffset,
-                            iSANodeIndex,
-                            iDANodeIndex
-                            ) == false) {
-
-            if (bDeFragRx) {
-                if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
-                    DBG_PRT(MSG_LEVEL_ERR,KERN_ERR "%s: can not alloc more frag bufs\n",
-                    pDevice->dev->name);
-                }
-            }
-            return false;
-        }
-
-//        if(pDevice->bRxMICFail == false) {
-//           for (ii =0; ii < 100; ii++)
-//                printk(" %02x", *(skb->data + ii));
-//           printk("\n");
-//	    }
-
-    }
+	}
+
+	s_vProcessRxMACHeader(pDevice, (unsigned char *)(skb->data+4), FrameSize, bIsWEP, bExtIV, &cbHeaderOffset);
+	FrameSize -= cbHeaderOffset;
+	cbHeaderOffset += 4;        // 4 is Rcv buffer header
+
+	// Null data, framesize = 14
+	if (FrameSize < 15)
+		return false;
+
+	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
+		if (s_bAPModeRxData(pDevice,
+				    skb,
+				    FrameSize,
+				    cbHeaderOffset,
+				    iSANodeIndex,
+				    iDANodeIndex
+) == false) {
+			if (bDeFragRx) {
+				if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
+					DBG_PRT(MSG_LEVEL_ERR, KERN_ERR "%s: can not alloc more frag bufs\n",
+						pDevice->dev->name);
+				}
+			}
+			return false;
+		}
+	}
 
 	skb->data += cbHeaderOffset;
 	skb->tail += cbHeaderOffset;
-    skb_put(skb, FrameSize);
-    skb->protocol=eth_type_trans(skb, skb->dev);
-
+	skb_put(skb, FrameSize);
+	skb->protocol = eth_type_trans(skb, skb->dev);
 
 	//drop frame not met IEEE 802.3
 /*
-	if (pDevice->flags & DEVICE_FLAGS_VAL_PKT_LEN) {
-		if ((skb->protocol==htons(ETH_P_802_3)) &&
-			(skb->len!=htons(skb->mac.ethernet->h_proto))) {
-			pStats->rx_length_errors++;
-			pStats->rx_dropped++;
-            if (bDeFragRx) {
-                if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
-                    DBG_PRT(MSG_LEVEL_ERR,KERN_ERR "%s: can not alloc more frag bufs\n",
-                    pDevice->dev->name);
-                }
-            }
-			return false;
-		}
-	}
+  if (pDevice->flags & DEVICE_FLAGS_VAL_PKT_LEN) {
+  if ((skb->protocol==htons(ETH_P_802_3)) &&
+  (skb->len!=htons(skb->mac.ethernet->h_proto))) {
+  pStats->rx_length_errors++;
+  pStats->rx_dropped++;
+  if (bDeFragRx) {
+  if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
+  DBG_PRT(MSG_LEVEL_ERR,KERN_ERR "%s: can not alloc more frag bufs\n",
+  pDevice->dev->name);
+  }
+  }
+  return false;
+  }
+  }
 */
 
-    skb->ip_summed=CHECKSUM_NONE;
-    pStats->rx_bytes +=skb->len;
-    pStats->rx_packets++;
-    netif_rx(skb);
+	skb->ip_summed = CHECKSUM_NONE;
+	pStats->rx_bytes += skb->len;
+	pStats->rx_packets++;
+	netif_rx(skb);
 
-    if (bDeFragRx) {
-        if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
-            DBG_PRT(MSG_LEVEL_ERR,KERN_ERR "%s: can not alloc more frag bufs\n",
-                pDevice->dev->name);
-        }
-        return false;
-    }
+	if (bDeFragRx) {
+		if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
+			DBG_PRT(MSG_LEVEL_ERR, KERN_ERR "%s: can not alloc more frag bufs\n",
+				pDevice->dev->name);
+		}
+		return false;
+	}
 
-    return true;
+	return true;
 }
 
-
-static bool s_bAPModeRxCtl (
-    PSDevice pDevice,
-    unsigned char *pbyFrame,
-    int      iSANodeIndex
-    )
+static bool s_bAPModeRxCtl(
+	PSDevice pDevice,
+	unsigned char *pbyFrame,
+	int      iSANodeIndex
+)
 {
-    PS802_11Header      p802_11Header;
-    CMD_STATUS          Status;
-    PSMgmtObject        pMgmt = pDevice->pMgmt;
-
-
-    if (IS_CTL_PSPOLL(pbyFrame) || !IS_TYPE_CONTROL(pbyFrame)) {
-
-        p802_11Header = (PS802_11Header) (pbyFrame);
-        if (!IS_TYPE_MGMT(pbyFrame)) {
-
-            // Data & PS-Poll packet
-            // check frame class
-            if (iSANodeIndex > 0) {
-                // frame class 3 fliter & checking
-                if (pMgmt->sNodeDBTable[iSANodeIndex].eNodeState < NODE_AUTH) {
-                    // send deauth notification
-                    // reason = (6) class 2 received from nonauth sta
-                    vMgrDeAuthenBeginSta(pDevice,
-                                         pMgmt,
-                                         (unsigned char *)(p802_11Header->abyAddr2),
-                                         (WLAN_MGMT_REASON_CLASS2_NONAUTH),
-                                         &Status
-                                         );
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: send vMgrDeAuthenBeginSta 1\n");
-                    return true;
-                }
-                if (pMgmt->sNodeDBTable[iSANodeIndex].eNodeState < NODE_ASSOC) {
-                    // send deassoc notification
-                    // reason = (7) class 3 received from nonassoc sta
-                    vMgrDisassocBeginSta(pDevice,
-                                         pMgmt,
-                                         (unsigned char *)(p802_11Header->abyAddr2),
-                                         (WLAN_MGMT_REASON_CLASS3_NONASSOC),
-                                         &Status
-                                         );
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: send vMgrDisassocBeginSta 2\n");
-                    return true;
-                }
-
-                if (pMgmt->sNodeDBTable[iSANodeIndex].bPSEnable) {
-                    // delcare received ps-poll event
-                    if (IS_CTL_PSPOLL(pbyFrame)) {
-                        pMgmt->sNodeDBTable[iSANodeIndex].bRxPSPoll = true;
-                        bScheduleCommand((void *)pDevice, WLAN_CMD_RX_PSPOLL, NULL);
-                        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: WLAN_CMD_RX_PSPOLL 1\n");
-                    }
-                    else {
-                        // check Data PS state
-                        // if PW bit off, send out all PS bufferring packets.
-                        if (!IS_FC_POWERMGT(pbyFrame)) {
-                            pMgmt->sNodeDBTable[iSANodeIndex].bPSEnable = false;
-                            pMgmt->sNodeDBTable[iSANodeIndex].bRxPSPoll = true;
-                            bScheduleCommand((void *)pDevice, WLAN_CMD_RX_PSPOLL, NULL);
-                            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: WLAN_CMD_RX_PSPOLL 2\n");
-                        }
-                    }
-                }
-                else {
-                   if (IS_FC_POWERMGT(pbyFrame)) {
-                       pMgmt->sNodeDBTable[iSANodeIndex].bPSEnable = true;
-                       // Once if STA in PS state, enable multicast bufferring
-                       pMgmt->sNodeDBTable[0].bPSEnable = true;
-                   }
-                   else {
-                      // clear all pending PS frame.
-                      if (pMgmt->sNodeDBTable[iSANodeIndex].wEnQueueCnt > 0) {
-                          pMgmt->sNodeDBTable[iSANodeIndex].bPSEnable = false;
-                          pMgmt->sNodeDBTable[iSANodeIndex].bRxPSPoll = true;
-                          bScheduleCommand((void *)pDevice, WLAN_CMD_RX_PSPOLL, NULL);
-                         DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: WLAN_CMD_RX_PSPOLL 3\n");
-
-                      }
-                   }
-                }
-            }
-            else {
-                  vMgrDeAuthenBeginSta(pDevice,
-                                       pMgmt,
-                                       (unsigned char *)(p802_11Header->abyAddr2),
-                                       (WLAN_MGMT_REASON_CLASS2_NONAUTH),
-                                       &Status
-                                       );
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: send vMgrDeAuthenBeginSta 3\n");
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BSSID:%pM\n",
-				p802_11Header->abyAddr3);
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ADDR2:%pM\n",
-				p802_11Header->abyAddr2);
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ADDR1:%pM\n",
-				p802_11Header->abyAddr1);
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: wFrameCtl= %x\n", p802_11Header->wFrameCtl );
-                    VNSvInPortB(pDevice->PortOffset + MAC_REG_RCR, &(pDevice->byRxMode));
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc:pDevice->byRxMode = %x\n", pDevice->byRxMode );
-                    return true;
-            }
-        }
-    }
-    return false;
+	PS802_11Header      p802_11Header;
+	CMD_STATUS          Status;
+	PSMgmtObject        pMgmt = pDevice->pMgmt;
+
+	if (IS_CTL_PSPOLL(pbyFrame) || !IS_TYPE_CONTROL(pbyFrame)) {
+		p802_11Header = (PS802_11Header)(pbyFrame);
+		if (!IS_TYPE_MGMT(pbyFrame)) {
+			// Data & PS-Poll packet
+			// check frame class
+			if (iSANodeIndex > 0) {
+				// frame class 3 fliter & checking
+				if (pMgmt->sNodeDBTable[iSANodeIndex].eNodeState < NODE_AUTH) {
+					// send deauth notification
+					// reason = (6) class 2 received from nonauth sta
+					vMgrDeAuthenBeginSta(pDevice,
+							     pMgmt,
+							     (unsigned char *)(p802_11Header->abyAddr2),
+							     (WLAN_MGMT_REASON_CLASS2_NONAUTH),
+							     &Status
+);
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: send vMgrDeAuthenBeginSta 1\n");
+					return true;
+				}
+				if (pMgmt->sNodeDBTable[iSANodeIndex].eNodeState < NODE_ASSOC) {
+					// send deassoc notification
+					// reason = (7) class 3 received from nonassoc sta
+					vMgrDisassocBeginSta(pDevice,
+							     pMgmt,
+							     (unsigned char *)(p802_11Header->abyAddr2),
+							     (WLAN_MGMT_REASON_CLASS3_NONASSOC),
+							     &Status
+);
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: send vMgrDisassocBeginSta 2\n");
+					return true;
+				}
 
+				if (pMgmt->sNodeDBTable[iSANodeIndex].bPSEnable) {
+					// delcare received ps-poll event
+					if (IS_CTL_PSPOLL(pbyFrame)) {
+						pMgmt->sNodeDBTable[iSANodeIndex].bRxPSPoll = true;
+						bScheduleCommand((void *)pDevice, WLAN_CMD_RX_PSPOLL, NULL);
+						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: WLAN_CMD_RX_PSPOLL 1\n");
+					} else {
+						// check Data PS state
+						// if PW bit off, send out all PS bufferring packets.
+						if (!IS_FC_POWERMGT(pbyFrame)) {
+							pMgmt->sNodeDBTable[iSANodeIndex].bPSEnable = false;
+							pMgmt->sNodeDBTable[iSANodeIndex].bRxPSPoll = true;
+							bScheduleCommand((void *)pDevice, WLAN_CMD_RX_PSPOLL, NULL);
+							DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: WLAN_CMD_RX_PSPOLL 2\n");
+						}
+					}
+				} else {
+					if (IS_FC_POWERMGT(pbyFrame)) {
+						pMgmt->sNodeDBTable[iSANodeIndex].bPSEnable = true;
+						// Once if STA in PS state, enable multicast bufferring
+						pMgmt->sNodeDBTable[0].bPSEnable = true;
+					} else {
+						// clear all pending PS frame.
+						if (pMgmt->sNodeDBTable[iSANodeIndex].wEnQueueCnt > 0) {
+							pMgmt->sNodeDBTable[iSANodeIndex].bPSEnable = false;
+							pMgmt->sNodeDBTable[iSANodeIndex].bRxPSPoll = true;
+							bScheduleCommand((void *)pDevice, WLAN_CMD_RX_PSPOLL, NULL);
+							DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: WLAN_CMD_RX_PSPOLL 3\n");
+
+						}
+					}
+				}
+			} else {
+				vMgrDeAuthenBeginSta(pDevice,
+						     pMgmt,
+						     (unsigned char *)(p802_11Header->abyAddr2),
+						     (WLAN_MGMT_REASON_CLASS2_NONAUTH),
+						     &Status
+);
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: send vMgrDeAuthenBeginSta 3\n");
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BSSID:%pM\n",
+					p802_11Header->abyAddr3);
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ADDR2:%pM\n",
+					p802_11Header->abyAddr2);
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ADDR1:%pM\n",
+					p802_11Header->abyAddr1);
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: wFrameCtl= %x\n", p802_11Header->wFrameCtl);
+				VNSvInPortB(pDevice->PortOffset + MAC_REG_RCR, &(pDevice->byRxMode));
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc:pDevice->byRxMode = %x\n", pDevice->byRxMode);
+				return true;
+			}
+		}
+	}
+	return false;
 }
 
-static bool s_bHandleRxEncryption (
-    PSDevice     pDevice,
-    unsigned char *pbyFrame,
-    unsigned int FrameSize,
-    unsigned char *pbyRsr,
-    unsigned char *pbyNewRsr,
-    PSKeyItem   *pKeyOut,
-    bool *pbExtIV,
-    unsigned short *pwRxTSC15_0,
-    unsigned long *pdwRxTSC47_16
-    )
+static bool s_bHandleRxEncryption(
+	PSDevice     pDevice,
+	unsigned char *pbyFrame,
+	unsigned int FrameSize,
+	unsigned char *pbyRsr,
+	unsigned char *pbyNewRsr,
+	PSKeyItem   *pKeyOut,
+	bool *pbExtIV,
+	unsigned short *pwRxTSC15_0,
+	unsigned long *pdwRxTSC47_16
+)
 {
-    unsigned int PayloadLen = FrameSize;
-    unsigned char *pbyIV;
-    unsigned char byKeyIdx;
-    PSKeyItem       pKey = NULL;
-    unsigned char byDecMode = KEY_CTL_WEP;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-
-
-    *pwRxTSC15_0 = 0;
-    *pdwRxTSC47_16 = 0;
-
-    pbyIV = pbyFrame + WLAN_HDR_ADDR3_LEN;
-    if ( WLAN_GET_FC_TODS(*(unsigned short *)pbyFrame) &&
-         WLAN_GET_FC_FROMDS(*(unsigned short *)pbyFrame) ) {
-         pbyIV += 6;             // 6 is 802.11 address4
-         PayloadLen -= 6;
-    }
-    byKeyIdx = (*(pbyIV+3) & 0xc0);
-    byKeyIdx >>= 6;
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"\nKeyIdx: %d\n", byKeyIdx);
-
-    if ((pMgmt->eAuthenMode == WMAC_AUTH_WPA) ||
-        (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) ||
-        (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) ||
-        (pMgmt->eAuthenMode == WMAC_AUTH_WPA2) ||
-        (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) {
-        if (((*pbyRsr & (RSR_ADDRBROAD | RSR_ADDRMULTI)) == 0) &&
-            (pDevice->pMgmt->byCSSPK != KEY_CTL_NONE)) {
-            // unicast pkt use pairwise key
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"unicast pkt\n");
-            if (KeybGetKey(&(pDevice->sKey), pDevice->abyBSSID, 0xFFFFFFFF, &pKey) == true) {
-                if (pDevice->pMgmt->byCSSPK == KEY_CTL_TKIP)
-                    byDecMode = KEY_CTL_TKIP;
-                else if (pDevice->pMgmt->byCSSPK == KEY_CTL_CCMP)
-                    byDecMode = KEY_CTL_CCMP;
-            }
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"unicast pkt: %d, %p\n", byDecMode, pKey);
-        } else {
-            // use group key
-            KeybGetKey(&(pDevice->sKey), pDevice->abyBSSID, byKeyIdx, &pKey);
-            if (pDevice->pMgmt->byCSSGK == KEY_CTL_TKIP)
-                byDecMode = KEY_CTL_TKIP;
-            else if (pDevice->pMgmt->byCSSGK == KEY_CTL_CCMP)
-                byDecMode = KEY_CTL_CCMP;
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"group pkt: %d, %d, %p\n", byKeyIdx, byDecMode, pKey);
-        }
-    }
-    // our WEP only support Default Key
-    if (pKey == NULL) {
-        // use default group key
-        KeybGetKey(&(pDevice->sKey), pDevice->abyBroadcastAddr, byKeyIdx, &pKey);
-        if (pDevice->pMgmt->byCSSGK == KEY_CTL_TKIP)
-            byDecMode = KEY_CTL_TKIP;
-        else if (pDevice->pMgmt->byCSSGK == KEY_CTL_CCMP)
-            byDecMode = KEY_CTL_CCMP;
-    }
-    *pKeyOut = pKey;
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"AES:%d %d %d\n", pDevice->pMgmt->byCSSPK, pDevice->pMgmt->byCSSGK, byDecMode);
-
-    if (pKey == NULL) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pKey == NULL\n");
-        if (byDecMode == KEY_CTL_WEP) {
+	unsigned int PayloadLen = FrameSize;
+	unsigned char *pbyIV;
+	unsigned char byKeyIdx;
+	PSKeyItem       pKey = NULL;
+	unsigned char byDecMode = KEY_CTL_WEP;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+
+	*pwRxTSC15_0 = 0;
+	*pdwRxTSC47_16 = 0;
+
+	pbyIV = pbyFrame + WLAN_HDR_ADDR3_LEN;
+	if (WLAN_GET_FC_TODS(*(unsigned short *)pbyFrame) &&
+	    WLAN_GET_FC_FROMDS(*(unsigned short *)pbyFrame)) {
+		pbyIV += 6;             // 6 is 802.11 address4
+		PayloadLen -= 6;
+	}
+	byKeyIdx = (*(pbyIV+3) & 0xc0);
+	byKeyIdx >>= 6;
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\nKeyIdx: %d\n", byKeyIdx);
+
+	if ((pMgmt->eAuthenMode == WMAC_AUTH_WPA) ||
+	    (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) ||
+	    (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) ||
+	    (pMgmt->eAuthenMode == WMAC_AUTH_WPA2) ||
+	    (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) {
+		if (((*pbyRsr & (RSR_ADDRBROAD | RSR_ADDRMULTI)) == 0) &&
+		    (pDevice->pMgmt->byCSSPK != KEY_CTL_NONE)) {
+			// unicast pkt use pairwise key
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "unicast pkt\n");
+			if (KeybGetKey(&(pDevice->sKey), pDevice->abyBSSID, 0xFFFFFFFF, &pKey) == true) {
+				if (pDevice->pMgmt->byCSSPK == KEY_CTL_TKIP)
+					byDecMode = KEY_CTL_TKIP;
+				else if (pDevice->pMgmt->byCSSPK == KEY_CTL_CCMP)
+					byDecMode = KEY_CTL_CCMP;
+			}
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "unicast pkt: %d, %p\n", byDecMode, pKey);
+		} else {
+			// use group key
+			KeybGetKey(&(pDevice->sKey), pDevice->abyBSSID, byKeyIdx, &pKey);
+			if (pDevice->pMgmt->byCSSGK == KEY_CTL_TKIP)
+				byDecMode = KEY_CTL_TKIP;
+			else if (pDevice->pMgmt->byCSSGK == KEY_CTL_CCMP)
+				byDecMode = KEY_CTL_CCMP;
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "group pkt: %d, %d, %p\n", byKeyIdx, byDecMode, pKey);
+		}
+	}
+	// our WEP only support Default Key
+	if (pKey == NULL) {
+		// use default group key
+		KeybGetKey(&(pDevice->sKey), pDevice->abyBroadcastAddr, byKeyIdx, &pKey);
+		if (pDevice->pMgmt->byCSSGK == KEY_CTL_TKIP)
+			byDecMode = KEY_CTL_TKIP;
+		else if (pDevice->pMgmt->byCSSGK == KEY_CTL_CCMP)
+			byDecMode = KEY_CTL_CCMP;
+	}
+	*pKeyOut = pKey;
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "AES:%d %d %d\n", pDevice->pMgmt->byCSSPK, pDevice->pMgmt->byCSSGK, byDecMode);
+
+	if (pKey == NULL) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey == NULL\n");
+		if (byDecMode == KEY_CTL_WEP) {
 //            pDevice->s802_11Counter.WEPUndecryptableCount.QuadPart++;
-        } else if (pDevice->bLinkPass == true) {
+		} else if (pDevice->bLinkPass == true) {
 //            pDevice->s802_11Counter.DecryptFailureCount.QuadPart++;
-        }
-        return false;
-    }
-    if (byDecMode != pKey->byCipherSuite) {
-        if (byDecMode == KEY_CTL_WEP) {
+		}
+		return false;
+	}
+	if (byDecMode != pKey->byCipherSuite) {
+		if (byDecMode == KEY_CTL_WEP) {
 //            pDevice->s802_11Counter.WEPUndecryptableCount.QuadPart++;
-        } else if (pDevice->bLinkPass == true) {
+		} else if (pDevice->bLinkPass == true) {
 //            pDevice->s802_11Counter.DecryptFailureCount.QuadPart++;
-        }
-        *pKeyOut = NULL;
-        return false;
-    }
-    if (byDecMode == KEY_CTL_WEP) {
-        // handle WEP
-        if ((pDevice->byLocalID <= REV_ID_VT3253_A1) ||
-            (((PSKeyTable)(pKey->pvKeyTable))->bSoftWEP == true)) {
-            // Software WEP
-            // 1. 3253A
-            // 2. WEP 256
-
-            PayloadLen -= (WLAN_HDR_ADDR3_LEN + 4 + 4); // 24 is 802.11 header,4 is IV, 4 is crc
-            memcpy(pDevice->abyPRNG, pbyIV, 3);
-            memcpy(pDevice->abyPRNG + 3, pKey->abyKey, pKey->uKeyLength);
-            rc4_init(&pDevice->SBox, pDevice->abyPRNG, pKey->uKeyLength + 3);
-            rc4_encrypt(&pDevice->SBox, pbyIV+4, pbyIV+4, PayloadLen);
-
-            if (ETHbIsBufferCrc32Ok(pbyIV+4, PayloadLen)) {
-                *pbyNewRsr |= NEWRSR_DECRYPTOK;
-            }
-        }
-    } else if ((byDecMode == KEY_CTL_TKIP) ||
-               (byDecMode == KEY_CTL_CCMP)) {
-        // TKIP/AES
-
-        PayloadLen -= (WLAN_HDR_ADDR3_LEN + 8 + 4); // 24 is 802.11 header, 8 is IV&ExtIV, 4 is crc
-        *pdwRxTSC47_16 = cpu_to_le32(*(unsigned long *)(pbyIV + 4));
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ExtIV: %lx\n",*pdwRxTSC47_16);
-        if (byDecMode == KEY_CTL_TKIP) {
-            *pwRxTSC15_0 = cpu_to_le16(MAKEWORD(*(pbyIV+2), *pbyIV));
-        } else {
-            *pwRxTSC15_0 = cpu_to_le16(*(unsigned short *)pbyIV);
-        }
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"TSC0_15: %x\n", *pwRxTSC15_0);
-
-        if ((byDecMode == KEY_CTL_TKIP) &&
-            (pDevice->byLocalID <= REV_ID_VT3253_A1)) {
-            // Software TKIP
-            // 1. 3253 A
-            PS802_11Header  pMACHeader = (PS802_11Header) (pbyFrame);
-            TKIPvMixKey(pKey->abyKey, pMACHeader->abyAddr2, *pwRxTSC15_0, *pdwRxTSC47_16, pDevice->abyPRNG);
-            rc4_init(&pDevice->SBox, pDevice->abyPRNG, TKIP_KEY_LEN);
-            rc4_encrypt(&pDevice->SBox, pbyIV+8, pbyIV+8, PayloadLen);
-            if (ETHbIsBufferCrc32Ok(pbyIV+8, PayloadLen)) {
-                *pbyNewRsr |= NEWRSR_DECRYPTOK;
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ICV OK!\n");
-            } else {
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ICV FAIL!!!\n");
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"PayloadLen = %d\n", PayloadLen);
-            }
-        }
-    }// end of TKIP/AES
-
-    if ((*(pbyIV+3) & 0x20) != 0)
-        *pbExtIV = true;
-    return true;
-}
+		}
+		*pKeyOut = NULL;
+		return false;
+	}
+	if (byDecMode == KEY_CTL_WEP) {
+		// handle WEP
+		if ((pDevice->byLocalID <= REV_ID_VT3253_A1) ||
+		    (((PSKeyTable)(pKey->pvKeyTable))->bSoftWEP == true)) {
+			// Software WEP
+			// 1. 3253A
+			// 2. WEP 256
+
+			PayloadLen -= (WLAN_HDR_ADDR3_LEN + 4 + 4); // 24 is 802.11 header,4 is IV, 4 is crc
+			memcpy(pDevice->abyPRNG, pbyIV, 3);
+			memcpy(pDevice->abyPRNG + 3, pKey->abyKey, pKey->uKeyLength);
+			rc4_init(&pDevice->SBox, pDevice->abyPRNG, pKey->uKeyLength + 3);
+			rc4_encrypt(&pDevice->SBox, pbyIV+4, pbyIV+4, PayloadLen);
+
+			if (ETHbIsBufferCrc32Ok(pbyIV+4, PayloadLen)) {
+				*pbyNewRsr |= NEWRSR_DECRYPTOK;
+			}
+		}
+	} else if ((byDecMode == KEY_CTL_TKIP) ||
+		   (byDecMode == KEY_CTL_CCMP)) {
+		// TKIP/AES
+
+		PayloadLen -= (WLAN_HDR_ADDR3_LEN + 8 + 4); // 24 is 802.11 header, 8 is IV&ExtIV, 4 is crc
+		*pdwRxTSC47_16 = cpu_to_le32(*(unsigned long *)(pbyIV + 4));
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ExtIV: %lx\n", *pdwRxTSC47_16);
+		if (byDecMode == KEY_CTL_TKIP) {
+			*pwRxTSC15_0 = cpu_to_le16(MAKEWORD(*(pbyIV + 2), *pbyIV));
+		} else {
+			*pwRxTSC15_0 = cpu_to_le16(*(unsigned short *)pbyIV);
+		}
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "TSC0_15: %x\n", *pwRxTSC15_0);
+
+		if ((byDecMode == KEY_CTL_TKIP) &&
+		    (pDevice->byLocalID <= REV_ID_VT3253_A1)) {
+			// Software TKIP
+			// 1. 3253 A
+			PS802_11Header  pMACHeader = (PS802_11Header)(pbyFrame);
+			TKIPvMixKey(pKey->abyKey, pMACHeader->abyAddr2, *pwRxTSC15_0, *pdwRxTSC47_16, pDevice->abyPRNG);
+			rc4_init(&pDevice->SBox, pDevice->abyPRNG, TKIP_KEY_LEN);
+			rc4_encrypt(&pDevice->SBox, pbyIV+8, pbyIV+8, PayloadLen);
+			if (ETHbIsBufferCrc32Ok(pbyIV+8, PayloadLen)) {
+				*pbyNewRsr |= NEWRSR_DECRYPTOK;
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ICV OK!\n");
+			} else {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ICV FAIL!!!\n");
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "PayloadLen = %d\n", PayloadLen);
+			}
+		}
+	}// end of TKIP/AES
 
+	if ((*(pbyIV+3) & 0x20) != 0)
+		*pbExtIV = true;
+	return true;
+}
 
-static bool s_bHostWepRxEncryption (
-    PSDevice     pDevice,
-    unsigned char *pbyFrame,
-    unsigned int FrameSize,
-    unsigned char *pbyRsr,
-    bool bOnFly,
-    PSKeyItem    pKey,
-    unsigned char *pbyNewRsr,
-    bool *pbExtIV,
-    unsigned short *pwRxTSC15_0,
-    unsigned long *pdwRxTSC47_16
-    )
+static bool s_bHostWepRxEncryption(
+	PSDevice     pDevice,
+	unsigned char *pbyFrame,
+	unsigned int FrameSize,
+	unsigned char *pbyRsr,
+	bool bOnFly,
+	PSKeyItem    pKey,
+	unsigned char *pbyNewRsr,
+	bool *pbExtIV,
+	unsigned short *pwRxTSC15_0,
+	unsigned long *pdwRxTSC47_16
+)
 {
-    unsigned int PayloadLen = FrameSize;
-    unsigned char *pbyIV;
-    unsigned char byKeyIdx;
-    unsigned char byDecMode = KEY_CTL_WEP;
-    PS802_11Header  pMACHeader;
-
+	unsigned int PayloadLen = FrameSize;
+	unsigned char *pbyIV;
+	unsigned char byKeyIdx;
+	unsigned char byDecMode = KEY_CTL_WEP;
+	PS802_11Header  pMACHeader;
+
+	*pwRxTSC15_0 = 0;
+	*pdwRxTSC47_16 = 0;
+
+	pbyIV = pbyFrame + WLAN_HDR_ADDR3_LEN;
+	if (WLAN_GET_FC_TODS(*(unsigned short *)pbyFrame) &&
+	    WLAN_GET_FC_FROMDS(*(unsigned short *)pbyFrame)) {
+		pbyIV += 6;             // 6 is 802.11 address4
+		PayloadLen -= 6;
+	}
+	byKeyIdx = (*(pbyIV+3) & 0xc0);
+	byKeyIdx >>= 6;
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\nKeyIdx: %d\n", byKeyIdx);
 
+	if (pDevice->pMgmt->byCSSGK == KEY_CTL_TKIP)
+		byDecMode = KEY_CTL_TKIP;
+	else if (pDevice->pMgmt->byCSSGK == KEY_CTL_CCMP)
+		byDecMode = KEY_CTL_CCMP;
 
-    *pwRxTSC15_0 = 0;
-    *pdwRxTSC47_16 = 0;
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "AES:%d %d %d\n", pDevice->pMgmt->byCSSPK, pDevice->pMgmt->byCSSGK, byDecMode);
 
-    pbyIV = pbyFrame + WLAN_HDR_ADDR3_LEN;
-    if ( WLAN_GET_FC_TODS(*(unsigned short *)pbyFrame) &&
-         WLAN_GET_FC_FROMDS(*(unsigned short *)pbyFrame) ) {
-         pbyIV += 6;             // 6 is 802.11 address4
-         PayloadLen -= 6;
-    }
-    byKeyIdx = (*(pbyIV+3) & 0xc0);
-    byKeyIdx >>= 6;
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"\nKeyIdx: %d\n", byKeyIdx);
+	if (byDecMode != pKey->byCipherSuite) {
+		if (byDecMode == KEY_CTL_WEP) {
+//            pDevice->s802_11Counter.WEPUndecryptableCount.QuadPart++;
+		} else if (pDevice->bLinkPass == true) {
+//            pDevice->s802_11Counter.DecryptFailureCount.QuadPart++;
+		}
+		return false;
+	}
 
+	if (byDecMode == KEY_CTL_WEP) {
+		// handle WEP
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "byDecMode == KEY_CTL_WEP \n");
+		if ((pDevice->byLocalID <= REV_ID_VT3253_A1) ||
+		    (((PSKeyTable)(pKey->pvKeyTable))->bSoftWEP == true) ||
+		    (bOnFly == false)) {
+			// Software WEP
+			// 1. 3253A
+			// 2. WEP 256
+			// 3. NotOnFly
+
+			PayloadLen -= (WLAN_HDR_ADDR3_LEN + 4 + 4); // 24 is 802.11 header,4 is IV, 4 is crc
+			memcpy(pDevice->abyPRNG, pbyIV, 3);
+			memcpy(pDevice->abyPRNG + 3, pKey->abyKey, pKey->uKeyLength);
+			rc4_init(&pDevice->SBox, pDevice->abyPRNG, pKey->uKeyLength + 3);
+			rc4_encrypt(&pDevice->SBox, pbyIV+4, pbyIV+4, PayloadLen);
+
+			if (ETHbIsBufferCrc32Ok(pbyIV+4, PayloadLen)) {
+				*pbyNewRsr |= NEWRSR_DECRYPTOK;
+			}
+		}
+	} else if ((byDecMode == KEY_CTL_TKIP) ||
+		   (byDecMode == KEY_CTL_CCMP)) {
+		// TKIP/AES
+
+		PayloadLen -= (WLAN_HDR_ADDR3_LEN + 8 + 4); // 24 is 802.11 header, 8 is IV&ExtIV, 4 is crc
+		*pdwRxTSC47_16 = cpu_to_le32(*(unsigned long *)(pbyIV + 4));
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ExtIV: %lx\n", *pdwRxTSC47_16);
+
+		if (byDecMode == KEY_CTL_TKIP) {
+			*pwRxTSC15_0 = cpu_to_le16(MAKEWORD(*(pbyIV+2), *pbyIV));
+		} else {
+			*pwRxTSC15_0 = cpu_to_le16(*(unsigned short *)pbyIV);
+		}
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "TSC0_15: %x\n", *pwRxTSC15_0);
+
+		if (byDecMode == KEY_CTL_TKIP) {
+			if ((pDevice->byLocalID <= REV_ID_VT3253_A1) || (bOnFly == false)) {
+				// Software TKIP
+				// 1. 3253 A
+				// 2. NotOnFly
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "soft KEY_CTL_TKIP \n");
+				pMACHeader = (PS802_11Header)(pbyFrame);
+				TKIPvMixKey(pKey->abyKey, pMACHeader->abyAddr2, *pwRxTSC15_0, *pdwRxTSC47_16, pDevice->abyPRNG);
+				rc4_init(&pDevice->SBox, pDevice->abyPRNG, TKIP_KEY_LEN);
+				rc4_encrypt(&pDevice->SBox, pbyIV+8, pbyIV+8, PayloadLen);
+				if (ETHbIsBufferCrc32Ok(pbyIV+8, PayloadLen)) {
+					*pbyNewRsr |= NEWRSR_DECRYPTOK;
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ICV OK!\n");
+				} else {
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ICV FAIL!!!\n");
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "PayloadLen = %d\n", PayloadLen);
+				}
+			}
+		}
 
-    if (pDevice->pMgmt->byCSSGK == KEY_CTL_TKIP)
-        byDecMode = KEY_CTL_TKIP;
-    else if (pDevice->pMgmt->byCSSGK == KEY_CTL_CCMP)
-        byDecMode = KEY_CTL_CCMP;
+		if (byDecMode == KEY_CTL_CCMP) {
+			if (bOnFly == false) {
+				// Software CCMP
+				// NotOnFly
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "soft KEY_CTL_CCMP\n");
+				if (AESbGenCCMP(pKey->abyKey, pbyFrame, FrameSize)) {
+					*pbyNewRsr |= NEWRSR_DECRYPTOK;
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "CCMP MIC compare OK!\n");
+				} else {
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "CCMP MIC fail!\n");
+				}
+			}
+		}
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"AES:%d %d %d\n", pDevice->pMgmt->byCSSPK, pDevice->pMgmt->byCSSGK, byDecMode);
+	}// end of TKIP/AES
 
-    if (byDecMode != pKey->byCipherSuite) {
-        if (byDecMode == KEY_CTL_WEP) {
-//            pDevice->s802_11Counter.WEPUndecryptableCount.QuadPart++;
-        } else if (pDevice->bLinkPass == true) {
-//            pDevice->s802_11Counter.DecryptFailureCount.QuadPart++;
-        }
-        return false;
-    }
-
-    if (byDecMode == KEY_CTL_WEP) {
-        // handle WEP
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"byDecMode == KEY_CTL_WEP \n");
-        if ((pDevice->byLocalID <= REV_ID_VT3253_A1) ||
-            (((PSKeyTable)(pKey->pvKeyTable))->bSoftWEP == true) ||
-            (bOnFly == false)) {
-            // Software WEP
-            // 1. 3253A
-            // 2. WEP 256
-            // 3. NotOnFly
-
-            PayloadLen -= (WLAN_HDR_ADDR3_LEN + 4 + 4); // 24 is 802.11 header,4 is IV, 4 is crc
-            memcpy(pDevice->abyPRNG, pbyIV, 3);
-            memcpy(pDevice->abyPRNG + 3, pKey->abyKey, pKey->uKeyLength);
-            rc4_init(&pDevice->SBox, pDevice->abyPRNG, pKey->uKeyLength + 3);
-            rc4_encrypt(&pDevice->SBox, pbyIV+4, pbyIV+4, PayloadLen);
-
-            if (ETHbIsBufferCrc32Ok(pbyIV+4, PayloadLen)) {
-                *pbyNewRsr |= NEWRSR_DECRYPTOK;
-            }
-        }
-    } else if ((byDecMode == KEY_CTL_TKIP) ||
-               (byDecMode == KEY_CTL_CCMP)) {
-        // TKIP/AES
-
-        PayloadLen -= (WLAN_HDR_ADDR3_LEN + 8 + 4); // 24 is 802.11 header, 8 is IV&ExtIV, 4 is crc
-        *pdwRxTSC47_16 = cpu_to_le32(*(unsigned long *)(pbyIV + 4));
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ExtIV: %lx\n",*pdwRxTSC47_16);
-
-        if (byDecMode == KEY_CTL_TKIP) {
-            *pwRxTSC15_0 = cpu_to_le16(MAKEWORD(*(pbyIV+2), *pbyIV));
-        } else {
-            *pwRxTSC15_0 = cpu_to_le16(*(unsigned short *)pbyIV);
-        }
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"TSC0_15: %x\n", *pwRxTSC15_0);
-
-        if (byDecMode == KEY_CTL_TKIP) {
-
-            if ((pDevice->byLocalID <= REV_ID_VT3253_A1) || (bOnFly == false)) {
-                // Software TKIP
-                // 1. 3253 A
-                // 2. NotOnFly
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"soft KEY_CTL_TKIP \n");
-                pMACHeader = (PS802_11Header) (pbyFrame);
-                TKIPvMixKey(pKey->abyKey, pMACHeader->abyAddr2, *pwRxTSC15_0, *pdwRxTSC47_16, pDevice->abyPRNG);
-                rc4_init(&pDevice->SBox, pDevice->abyPRNG, TKIP_KEY_LEN);
-                rc4_encrypt(&pDevice->SBox, pbyIV+8, pbyIV+8, PayloadLen);
-                if (ETHbIsBufferCrc32Ok(pbyIV+8, PayloadLen)) {
-                    *pbyNewRsr |= NEWRSR_DECRYPTOK;
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ICV OK!\n");
-                } else {
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ICV FAIL!!!\n");
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"PayloadLen = %d\n", PayloadLen);
-                }
-            }
-        }
-
-        if (byDecMode == KEY_CTL_CCMP) {
-            if (bOnFly == false) {
-                // Software CCMP
-                // NotOnFly
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"soft KEY_CTL_CCMP\n");
-                if (AESbGenCCMP(pKey->abyKey, pbyFrame, FrameSize)) {
-                    *pbyNewRsr |= NEWRSR_DECRYPTOK;
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"CCMP MIC compare OK!\n");
-                } else {
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"CCMP MIC fail!\n");
-                }
-            }
-        }
-
-    }// end of TKIP/AES
-
-    if ((*(pbyIV+3) & 0x20) != 0)
-        *pbExtIV = true;
-    return true;
+	if ((*(pbyIV+3) & 0x20) != 0)
+		*pbExtIV = true;
+	return true;
 }
 
+static bool s_bAPModeRxData(
+	PSDevice pDevice,
+	struct sk_buff *skb,
+	unsigned int FrameSize,
+	unsigned int cbHeaderOffset,
+	int      iSANodeIndex,
+	int      iDANodeIndex
+)
+{
+	PSMgmtObject        pMgmt = pDevice->pMgmt;
+	bool bRelayAndForward = false;
+	bool bRelayOnly = false;
+	unsigned char byMask[8] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80};
+	unsigned short wAID;
+
+	struct sk_buff *skbcpy = NULL;
+
+	if (FrameSize > CB_MAX_BUF_SIZE)
+		return false;
+	// check DA
+	if (is_multicast_ether_addr((unsigned char *)(skb->data+cbHeaderOffset))) {
+		if (pMgmt->sNodeDBTable[0].bPSEnable) {
+			skbcpy = dev_alloc_skb((int)pDevice->rx_buf_sz);
+
+			// if any node in PS mode, buffer packet until DTIM.
+			if (skbcpy == NULL) {
+				DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "relay multicast no skb available \n");
+			} else {
+				skbcpy->dev = pDevice->dev;
+				skbcpy->len = FrameSize;
+				memcpy(skbcpy->data, skb->data+cbHeaderOffset, FrameSize);
+				skb_queue_tail(&(pMgmt->sNodeDBTable[0].sTxPSQueue), skbcpy);
+
+				pMgmt->sNodeDBTable[0].wEnQueueCnt++;
+				// set tx map
+				pMgmt->abyPSTxMap[0] |= byMask[0];
+			}
+		} else {
+			bRelayAndForward = true;
+		}
+	} else {
+		// check if relay
+		if (BSSDBbIsSTAInNodeDB(pMgmt, (unsigned char *)(skb->data+cbHeaderOffset), &iDANodeIndex)) {
+			if (pMgmt->sNodeDBTable[iDANodeIndex].eNodeState >= NODE_ASSOC) {
+				if (pMgmt->sNodeDBTable[iDANodeIndex].bPSEnable) {
+					// queue this skb until next PS tx, and then release.
+
+					skb->data += cbHeaderOffset;
+					skb->tail += cbHeaderOffset;
+					skb_put(skb, FrameSize);
+					skb_queue_tail(&pMgmt->sNodeDBTable[iDANodeIndex].sTxPSQueue, skb);
+					pMgmt->sNodeDBTable[iDANodeIndex].wEnQueueCnt++;
+					wAID = pMgmt->sNodeDBTable[iDANodeIndex].wAID;
+					pMgmt->abyPSTxMap[wAID >> 3] |=  byMask[wAID & 7];
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "relay: index= %d, pMgmt->abyPSTxMap[%d]= %d\n",
+						iDANodeIndex, (wAID >> 3), pMgmt->abyPSTxMap[wAID >> 3]);
+					return true;
+				} else {
+					bRelayOnly = true;
+				}
+			}
+		}
+	}
+
+	if (bRelayOnly || bRelayAndForward) {
+		// relay this packet right now
+		if (bRelayAndForward)
+			iDANodeIndex = 0;
+
+		if ((pDevice->uAssocCount > 1) && (iDANodeIndex >= 0)) {
+			ROUTEbRelay(pDevice, (unsigned char *)(skb->data + cbHeaderOffset), FrameSize, (unsigned int)iDANodeIndex);
+		}
 
+		if (bRelayOnly)
+			return false;
+	}
+	// none associate, don't forward
+	if (pDevice->uAssocCount == 0)
+		return false;
 
-static bool s_bAPModeRxData (
-    PSDevice pDevice,
-    struct sk_buff* skb,
-    unsigned int FrameSize,
-    unsigned int cbHeaderOffset,
-    int      iSANodeIndex,
-    int      iDANodeIndex
-    )
-{
-    PSMgmtObject        pMgmt = pDevice->pMgmt;
-    bool bRelayAndForward = false;
-    bool bRelayOnly = false;
-    unsigned char byMask[8] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80};
-    unsigned short wAID;
-
-
-    struct sk_buff* skbcpy = NULL;
-
-    if (FrameSize > CB_MAX_BUF_SIZE)
-        return false;
-    // check DA
-    if(is_multicast_ether_addr((unsigned char *)(skb->data+cbHeaderOffset))) {
-       if (pMgmt->sNodeDBTable[0].bPSEnable) {
-
-           skbcpy = dev_alloc_skb((int)pDevice->rx_buf_sz);
-
-        // if any node in PS mode, buffer packet until DTIM.
-           if (skbcpy == NULL) {
-               DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "relay multicast no skb available \n");
-           }
-           else {
-               skbcpy->dev = pDevice->dev;
-               skbcpy->len = FrameSize;
-               memcpy(skbcpy->data, skb->data+cbHeaderOffset, FrameSize);
-               skb_queue_tail(&(pMgmt->sNodeDBTable[0].sTxPSQueue), skbcpy);
-
-               pMgmt->sNodeDBTable[0].wEnQueueCnt++;
-               // set tx map
-               pMgmt->abyPSTxMap[0] |= byMask[0];
-           }
-       }
-       else {
-           bRelayAndForward = true;
-       }
-    }
-    else {
-        // check if relay
-        if (BSSDBbIsSTAInNodeDB(pMgmt, (unsigned char *)(skb->data+cbHeaderOffset), &iDANodeIndex)) {
-            if (pMgmt->sNodeDBTable[iDANodeIndex].eNodeState >= NODE_ASSOC) {
-                if (pMgmt->sNodeDBTable[iDANodeIndex].bPSEnable) {
-                    // queue this skb until next PS tx, and then release.
-
-	                skb->data += cbHeaderOffset;
-	                skb->tail += cbHeaderOffset;
-                    skb_put(skb, FrameSize);
-                    skb_queue_tail(&pMgmt->sNodeDBTable[iDANodeIndex].sTxPSQueue, skb);
-                    pMgmt->sNodeDBTable[iDANodeIndex].wEnQueueCnt++;
-                    wAID = pMgmt->sNodeDBTable[iDANodeIndex].wAID;
-                    pMgmt->abyPSTxMap[wAID >> 3] |=  byMask[wAID & 7];
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "relay: index= %d, pMgmt->abyPSTxMap[%d]= %d\n",
-                               iDANodeIndex, (wAID >> 3), pMgmt->abyPSTxMap[wAID >> 3]);
-                    return true;
-                }
-                else {
-                    bRelayOnly = true;
-                }
-            }
-        }
-    }
-
-    if (bRelayOnly || bRelayAndForward) {
-        // relay this packet right now
-        if (bRelayAndForward)
-            iDANodeIndex = 0;
-
-        if ((pDevice->uAssocCount > 1) && (iDANodeIndex >= 0)) {
-            ROUTEbRelay(pDevice, (unsigned char *)(skb->data + cbHeaderOffset), FrameSize, (unsigned int)iDANodeIndex);
-        }
-
-        if (bRelayOnly)
-            return false;
-    }
-    // none associate, don't forward
-    if (pDevice->uAssocCount == 0)
-        return false;
-
-    return true;
+	return true;
 }
-
diff --git a/drivers/staging/vt6655/dpc.h b/drivers/staging/vt6655/dpc.h
index c1b6e76a421..0ce31557d5d 100644
--- a/drivers/staging/vt6655/dpc.h
+++ b/drivers/staging/vt6655/dpc.h
@@ -42,14 +42,11 @@
 /*---------------------  Export Functions  --------------------------*/
 
 bool
-device_receive_frame (
-    PSDevice pDevice,
-    PSRxDesc pCurrRD
-    );
+device_receive_frame(
+	PSDevice pDevice,
+	PSRxDesc pCurrRD
+);
 
 void	MngWorkItem(void *Context);
 
 #endif // __RXTX_H__
-
-
-
diff --git a/drivers/staging/vt6655/hostap.c b/drivers/staging/vt6655/hostap.c
index 5f13890cf12..8417c2f2c6c 100644
--- a/drivers/staging/vt6655/hostap.c
+++ b/drivers/staging/vt6655/hostap.c
@@ -49,16 +49,12 @@
 
 /*---------------------  Static Variables  --------------------------*/
 //static int          msglevel                =MSG_LEVEL_DEBUG;
-static int          msglevel                =MSG_LEVEL_INFO;
+static int msglevel = MSG_LEVEL_INFO;
 
 /*---------------------  Static Functions  --------------------------*/
 
-
-
-
 /*---------------------  Export Variables  --------------------------*/
 
-
 /*
  * Description:
  *      register net_device (AP) for hostap deamon
@@ -75,21 +71,21 @@ static int          msglevel                =MSG_LEVEL_INFO;
 
 static int hostap_enable_hostapd(PSDevice pDevice, int rtnl_locked)
 {
-    PSDevice apdev_priv;
+	PSDevice apdev_priv;
 	struct net_device *dev = pDevice->dev;
 	int ret;
 	const struct net_device_ops apdev_netdev_ops = {
 		.ndo_start_xmit         = pDevice->tx_80211,
 	};
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: Enabling hostapd mode\n", dev->name);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: Enabling hostapd mode\n", dev->name);
 
 	pDevice->apdev = kzalloc(sizeof(struct net_device), GFP_KERNEL);
 	if (pDevice->apdev == NULL)
 		return -ENOMEM;
 
-    apdev_priv = netdev_priv(pDevice->apdev);
-    *apdev_priv = *pDevice;
+	apdev_priv = netdev_priv(pDevice->apdev);
+	*apdev_priv = *pDevice;
 	memcpy(pDevice->apdev->dev_addr, dev->dev_addr, ETH_ALEN);
 
 	pDevice->apdev->netdev_ops = &apdev_netdev_ops;
@@ -107,14 +103,14 @@ static int hostap_enable_hostapd(PSDevice pDevice, int rtnl_locked)
 		ret = register_netdev(pDevice->apdev);
 	if (ret) {
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: register_netdevice(AP) failed!\n",
-		       dev->name);
+			dev->name);
 		return -1;
 	}
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: Registered netdevice %s for AP management\n",
-	       dev->name, pDevice->apdev->name);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: Registered netdevice %s for AP management\n",
+		dev->name, pDevice->apdev->name);
 
-    KeyvInitTable(&pDevice->sKey, pDevice->PortOffset);
+	KeyvInitTable(&pDevice->sKey, pDevice->PortOffset);
 
 	return 0;
 }
@@ -135,33 +131,31 @@ static int hostap_enable_hostapd(PSDevice pDevice, int rtnl_locked)
 
 static int hostap_disable_hostapd(PSDevice pDevice, int rtnl_locked)
 {
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: disabling hostapd mode\n", pDevice->dev->name);
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: disabling hostapd mode\n", pDevice->dev->name);
-
-    if (pDevice->apdev && pDevice->apdev->name && pDevice->apdev->name[0]) {
+	if (pDevice->apdev && pDevice->apdev->name && pDevice->apdev->name[0]) {
 		if (rtnl_locked)
 			unregister_netdevice(pDevice->apdev);
 		else
 			unregister_netdev(pDevice->apdev);
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: Netdevice %s unregistered\n",
-		       pDevice->dev->name, pDevice->apdev->name);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: Netdevice %s unregistered\n",
+			pDevice->dev->name, pDevice->apdev->name);
 	}
 	kfree(pDevice->apdev);
 	pDevice->apdev = NULL;
-    pDevice->bEnable8021x = false;
-    pDevice->bEnableHostWEP = false;
-    pDevice->bEncryptionEnable = false;
+	pDevice->bEnable8021x = false;
+	pDevice->bEnableHostWEP = false;
+	pDevice->bEncryptionEnable = false;
 
 //4.2007-0118-03,<Add> by EinsnLiu
 //execute some clear work
-pDevice->pMgmt->byCSSPK=KEY_CTL_NONE;
-pDevice->pMgmt->byCSSGK=KEY_CTL_NONE;
-KeyvInitTable(&pDevice->sKey,pDevice->PortOffset);
+	pDevice->pMgmt->byCSSPK = KEY_CTL_NONE;
+	pDevice->pMgmt->byCSSGK = KEY_CTL_NONE;
+	KeyvInitTable(&pDevice->sKey, pDevice->PortOffset);
 
 	return 0;
 }
 
-
 /*
  * Description:
  *      Set enable/disable hostapd mode
@@ -192,7 +186,6 @@ int vt6655_hostap_set_hostapd(PSDevice pDevice, int val, int rtnl_locked)
 		return hostap_disable_hostapd(pDevice, rtnl_locked);
 }
 
-
 /*
  * Description:
  *      remove station function supported for hostap deamon
@@ -207,17 +200,15 @@ int vt6655_hostap_set_hostapd(PSDevice pDevice, int val, int rtnl_locked)
  *
  */
 static int hostap_remove_sta(PSDevice pDevice,
-				     struct viawget_hostapd_param *param)
+			     struct viawget_hostapd_param *param)
 {
 	unsigned int uNodeIndex;
 
-
-    if (BSSDBbIsSTAInNodeDB(pDevice->pMgmt, param->sta_addr, &uNodeIndex)) {
-        BSSvRemoveOneNode(pDevice, uNodeIndex);
-    }
-    else {
-        return -ENOENT;
-    }
+	if (BSSDBbIsSTAInNodeDB(pDevice->pMgmt, param->sta_addr, &uNodeIndex)) {
+		BSSvRemoveOneNode(pDevice, uNodeIndex);
+	} else {
+		return -ENOENT;
+	}
 	return 0;
 }
 
@@ -235,47 +226,46 @@ static int hostap_remove_sta(PSDevice pDevice,
  *
  */
 static int hostap_add_sta(PSDevice pDevice,
-				  struct viawget_hostapd_param *param)
+			  struct viawget_hostapd_param *param)
 {
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	unsigned int uNodeIndex;
 
-
-    if (!BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &uNodeIndex)) {
-        BSSvCreateOneNode((PSDevice)pDevice, &uNodeIndex);
-    }
-    memcpy(pMgmt->sNodeDBTable[uNodeIndex].abyMACAddr, param->sta_addr, WLAN_ADDR_LEN);
-    pMgmt->sNodeDBTable[uNodeIndex].eNodeState = NODE_ASSOC;
-    pMgmt->sNodeDBTable[uNodeIndex].wCapInfo = param->u.add_sta.capability;
+	if (!BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &uNodeIndex)) {
+		BSSvCreateOneNode((PSDevice)pDevice, &uNodeIndex);
+	}
+	memcpy(pMgmt->sNodeDBTable[uNodeIndex].abyMACAddr, param->sta_addr, WLAN_ADDR_LEN);
+	pMgmt->sNodeDBTable[uNodeIndex].eNodeState = NODE_ASSOC;
+	pMgmt->sNodeDBTable[uNodeIndex].wCapInfo = param->u.add_sta.capability;
 // TODO listenInterval
 //    pMgmt->sNodeDBTable[uNodeIndex].wListenInterval = 1;
-    pMgmt->sNodeDBTable[uNodeIndex].bPSEnable = false;
-    pMgmt->sNodeDBTable[uNodeIndex].bySuppRate = param->u.add_sta.tx_supp_rates;
-
-    // set max tx rate
-    pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate =
-           pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate;
-    // set max basic rate
-    pMgmt->sNodeDBTable[uNodeIndex].wMaxBasicRate = RATE_2M;
-    // Todo: check sta preamble, if ap can't support, set status code
-    pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble =
-            WLAN_GET_CAP_INFO_SHORTPREAMBLE(pMgmt->sNodeDBTable[uNodeIndex].wCapInfo);
-
-    pMgmt->sNodeDBTable[uNodeIndex].wAID = (unsigned short)param->u.add_sta.aid;
-
-    pMgmt->sNodeDBTable[uNodeIndex].ulLastRxJiffer = jiffies;
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Add STA AID= %d \n", pMgmt->sNodeDBTable[uNodeIndex].wAID);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "MAC=%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X \n",
-               param->sta_addr[0],
-               param->sta_addr[1],
-               param->sta_addr[2],
-               param->sta_addr[3],
-               param->sta_addr[4],
-               param->sta_addr[5]
-              ) ;
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Max Support rate = %d \n",
-               pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate);
+	pMgmt->sNodeDBTable[uNodeIndex].bPSEnable = false;
+	pMgmt->sNodeDBTable[uNodeIndex].bySuppRate = param->u.add_sta.tx_supp_rates;
+
+	// set max tx rate
+	pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate =
+		pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate;
+	// set max basic rate
+	pMgmt->sNodeDBTable[uNodeIndex].wMaxBasicRate = RATE_2M;
+	// Todo: check sta preamble, if ap can't support, set status code
+	pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble =
+		WLAN_GET_CAP_INFO_SHORTPREAMBLE(pMgmt->sNodeDBTable[uNodeIndex].wCapInfo);
+
+	pMgmt->sNodeDBTable[uNodeIndex].wAID = (unsigned short)param->u.add_sta.aid;
+
+	pMgmt->sNodeDBTable[uNodeIndex].ulLastRxJiffer = jiffies;
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Add STA AID= %d \n", pMgmt->sNodeDBTable[uNodeIndex].wAID);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "MAC=%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X \n",
+		param->sta_addr[0],
+		param->sta_addr[1],
+		param->sta_addr[2],
+		param->sta_addr[3],
+		param->sta_addr[4],
+		param->sta_addr[5]
+		);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Max Support rate = %d \n",
+		pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate);
 
 	return 0;
 }
@@ -295,19 +285,18 @@ static int hostap_add_sta(PSDevice pDevice,
  */
 
 static int hostap_get_info_sta(PSDevice pDevice,
-				       struct viawget_hostapd_param *param)
+			       struct viawget_hostapd_param *param)
 {
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	unsigned int uNodeIndex;
 
-    if (BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &uNodeIndex)) {
-	    param->u.get_info_sta.inactive_sec =
-	        (jiffies - pMgmt->sNodeDBTable[uNodeIndex].ulLastRxJiffer) / HZ;
+	if (BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &uNodeIndex)) {
+		param->u.get_info_sta.inactive_sec =
+			(jiffies - pMgmt->sNodeDBTable[uNodeIndex].ulLastRxJiffer) / HZ;
 
-	    //param->u.get_info_sta.txexc = pMgmt->sNodeDBTable[uNodeIndex].uTxAttempts;
-	}
-	else {
-	    return -ENOENT;
+		//param->u.get_info_sta.txexc = pMgmt->sNodeDBTable[uNodeIndex].uTxAttempts;
+	} else {
+		return -ENOENT;
 	}
 
 	return 0;
@@ -328,21 +317,20 @@ static int hostap_get_info_sta(PSDevice pDevice,
  *
  */
 /*
-static int hostap_reset_txexc_sta(PSDevice pDevice,
-					  struct viawget_hostapd_param *param)
-{
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-	unsigned int uNodeIndex;
-
-    if (BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &uNodeIndex)) {
-        pMgmt->sNodeDBTable[uNodeIndex].uTxAttempts = 0;
-	}
-	else {
-	    return -ENOENT;
-	}
-
-	return 0;
-}
+  static int hostap_reset_txexc_sta(PSDevice pDevice,
+  struct viawget_hostapd_param *param)
+  {
+  PSMgmtObject    pMgmt = pDevice->pMgmt;
+  unsigned int uNodeIndex;
+
+  if (BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &uNodeIndex)) {
+  pMgmt->sNodeDBTable[uNodeIndex].uTxAttempts = 0;
+  } else {
+  return -ENOENT;
+  }
+
+  return 0;
+  }
 */
 
 /*
@@ -359,26 +347,23 @@ static int hostap_reset_txexc_sta(PSDevice pDevice,
  *
  */
 static int hostap_set_flags_sta(PSDevice pDevice,
-					struct viawget_hostapd_param *param)
+				struct viawget_hostapd_param *param)
 {
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	unsigned int uNodeIndex;
 
-    if (BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &uNodeIndex)) {
+	if (BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &uNodeIndex)) {
 		pMgmt->sNodeDBTable[uNodeIndex].dwFlags |= param->u.set_flags_sta.flags_or;
 		pMgmt->sNodeDBTable[uNodeIndex].dwFlags &= param->u.set_flags_sta.flags_and;
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " dwFlags = %x \n",
-		            (unsigned int)pMgmt->sNodeDBTable[uNodeIndex].dwFlags);
-	}
-	else {
-	    return -ENOENT;
+			(unsigned int)pMgmt->sNodeDBTable[uNodeIndex].dwFlags);
+	} else {
+		return -ENOENT;
 	}
 
 	return 0;
 }
 
-
-
 /*
  * Description:
  *      set generic element (wpa ie)
@@ -393,34 +378,32 @@ static int hostap_set_flags_sta(PSDevice pDevice,
  *
  */
 static int hostap_set_generic_element(PSDevice pDevice,
-					struct viawget_hostapd_param *param)
+				      struct viawget_hostapd_param *param)
 {
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-
-
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
 
-    memcpy( pMgmt->abyWPAIE,
-            param->u.generic_elem.data,
-            param->u.generic_elem.len
-           );
+	memcpy(pMgmt->abyWPAIE,
+	       param->u.generic_elem.data,
+	       param->u.generic_elem.len
+		);
 
-    pMgmt->wWPAIELen = 	param->u.generic_elem.len;
+	pMgmt->wWPAIELen = param->u.generic_elem.len;
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pMgmt->wWPAIELen = %d\n",  pMgmt->wWPAIELen);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pMgmt->wWPAIELen = %d\n", pMgmt->wWPAIELen);
 
-    // disable wpa
-    if (pMgmt->wWPAIELen == 0) {
-        pMgmt->eAuthenMode = WMAC_AUTH_OPEN;
+	// disable wpa
+	if (pMgmt->wWPAIELen == 0) {
+		pMgmt->eAuthenMode = WMAC_AUTH_OPEN;
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " No WPAIE, Disable WPA \n");
-    } else  {
-        // enable wpa
-        if ((pMgmt->abyWPAIE[0] == WLAN_EID_RSN_WPA) ||
-             (pMgmt->abyWPAIE[0] == WLAN_EID_RSN)) {
-              pMgmt->eAuthenMode = WMAC_AUTH_WPANONE;
-               DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set WPAIE enable WPA\n");
-        } else
-            return -EINVAL;
-    }
+	} else  {
+		// enable wpa
+		if ((pMgmt->abyWPAIE[0] == WLAN_EID_RSN_WPA) ||
+		    (pMgmt->abyWPAIE[0] == WLAN_EID_RSN)) {
+			pMgmt->eAuthenMode = WMAC_AUTH_WPANONE;
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set WPAIE enable WPA\n");
+		} else
+			return -EINVAL;
+	}
 
 	return 0;
 }
@@ -440,11 +423,11 @@ static int hostap_set_generic_element(PSDevice pDevice,
 
 static void hostap_flush_sta(PSDevice pDevice)
 {
-    // reserved node index =0 for multicast node.
-    BSSvClearNodeDBTable(pDevice, 1);
-    pDevice->uAssocCount = 0;
+	// reserved node index =0 for multicast node.
+	BSSvClearNodeDBTable(pDevice, 1);
+	pDevice->uAssocCount = 0;
 
-    return;
+	return;
 }
 
 /*
@@ -461,34 +444,32 @@ static void hostap_flush_sta(PSDevice pDevice)
  *
  */
 static int hostap_set_encryption(PSDevice pDevice,
-				       struct viawget_hostapd_param *param,
-				       int param_len)
+				 struct viawget_hostapd_param *param,
+				 int param_len)
 {
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    unsigned long dwKeyIndex = 0;
-    unsigned char abyKey[MAX_KEY_LEN];
-    unsigned char abySeq[MAX_KEY_LEN];
-    NDIS_802_11_KEY_RSC   KeyRSC;
-    unsigned char byKeyDecMode = KEY_CTL_WEP;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	unsigned long dwKeyIndex = 0;
+	unsigned char abyKey[MAX_KEY_LEN];
+	unsigned char abySeq[MAX_KEY_LEN];
+	NDIS_802_11_KEY_RSC   KeyRSC;
+	unsigned char byKeyDecMode = KEY_CTL_WEP;
 	int     ret = 0;
 	int     iNodeIndex = -1;
 	int     ii;
 	bool bKeyTableFull = false;
 	unsigned short wKeyCtl = 0;
 
-
 	param->u.crypt.err = 0;
 /*
-	if (param_len !=
-	    (int) ((char *) param->u.crypt.key - (char *) param) +
-	    param->u.crypt.key_len)
-		return -EINVAL;
+  if (param_len !=
+  (int) ((char *) param->u.crypt.key - (char *) param) +
+  param->u.crypt.key_len)
+  return -EINVAL;
 */
 
 	if (param->u.crypt.alg > WPA_ALG_CCMP)
 		return -EINVAL;
 
-
 	if ((param->u.crypt.idx > 3) || (param->u.crypt.key_len > MAX_KEY_LEN)) {
 		param->u.crypt.err = HOSTAP_CRYPT_ERR_KEY_SET_FAILED;
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " HOSTAP_CRYPT_ERR_KEY_SET_FAILED\n");
@@ -498,105 +479,102 @@ static int hostap_set_encryption(PSDevice pDevice,
 	if (is_broadcast_ether_addr(param->sta_addr)) {
 		if (param->u.crypt.idx >= MAX_GROUP_KEY)
 			return -EINVAL;
-        iNodeIndex = 0;
+		iNodeIndex = 0;
 
 	} else {
-	    if (BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &iNodeIndex) == false) {
-	        param->u.crypt.err = HOSTAP_CRYPT_ERR_UNKNOWN_ADDR;
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " HOSTAP_CRYPT_ERR_UNKNOWN_ADDR\n");
-	        return -EINVAL;
-	    }
+		if (BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &iNodeIndex) == false) {
+			param->u.crypt.err = HOSTAP_CRYPT_ERR_UNKNOWN_ADDR;
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " HOSTAP_CRYPT_ERR_UNKNOWN_ADDR\n");
+			return -EINVAL;
+		}
 	}
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " hostap_set_encryption: sta_index %d \n", iNodeIndex);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " hostap_set_encryption: alg %d \n", param->u.crypt.alg);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " hostap_set_encryption: sta_index %d \n", iNodeIndex);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " hostap_set_encryption: alg %d \n", param->u.crypt.alg);
 
 	if (param->u.crypt.alg == WPA_ALG_NONE) {
-
-        if (pMgmt->sNodeDBTable[iNodeIndex].bOnFly == true) {
-            if (KeybRemoveKey(&(pDevice->sKey),
-                                param->sta_addr,
-                                pMgmt->sNodeDBTable[iNodeIndex].dwKeyIndex,
-                                pDevice->PortOffset) == false) {
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "KeybRemoveKey fail \n");
-            }
-            pMgmt->sNodeDBTable[iNodeIndex].bOnFly = false;
-        }
-        pMgmt->sNodeDBTable[iNodeIndex].byKeyIndex = 0;
-        pMgmt->sNodeDBTable[iNodeIndex].dwKeyIndex = 0;
-        pMgmt->sNodeDBTable[iNodeIndex].uWepKeyLength = 0;
-        pMgmt->sNodeDBTable[iNodeIndex].KeyRSC = 0;
-        pMgmt->sNodeDBTable[iNodeIndex].dwTSC47_16 = 0;
-        pMgmt->sNodeDBTable[iNodeIndex].wTSC15_0 = 0;
-        pMgmt->sNodeDBTable[iNodeIndex].byCipherSuite = 0;
-        memset(&pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[0],
-                0,
-                MAX_KEY_LEN
-               );
-
-        return ret;
+		if (pMgmt->sNodeDBTable[iNodeIndex].bOnFly == true) {
+			if (KeybRemoveKey(&(pDevice->sKey),
+					  param->sta_addr,
+					  pMgmt->sNodeDBTable[iNodeIndex].dwKeyIndex,
+					  pDevice->PortOffset) == false) {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "KeybRemoveKey fail \n");
+			}
+			pMgmt->sNodeDBTable[iNodeIndex].bOnFly = false;
+		}
+		pMgmt->sNodeDBTable[iNodeIndex].byKeyIndex = 0;
+		pMgmt->sNodeDBTable[iNodeIndex].dwKeyIndex = 0;
+		pMgmt->sNodeDBTable[iNodeIndex].uWepKeyLength = 0;
+		pMgmt->sNodeDBTable[iNodeIndex].KeyRSC = 0;
+		pMgmt->sNodeDBTable[iNodeIndex].dwTSC47_16 = 0;
+		pMgmt->sNodeDBTable[iNodeIndex].wTSC15_0 = 0;
+		pMgmt->sNodeDBTable[iNodeIndex].byCipherSuite = 0;
+		memset(&pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[0],
+		       0,
+		       MAX_KEY_LEN
+);
+
+		return ret;
 	}
 
-    memcpy(abyKey, param->u.crypt.key, param->u.crypt.key_len);
-    // copy to node key tbl
-    pMgmt->sNodeDBTable[iNodeIndex].byKeyIndex = param->u.crypt.idx;
-    pMgmt->sNodeDBTable[iNodeIndex].uWepKeyLength = param->u.crypt.key_len;
-    memcpy(&pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[0],
-            param->u.crypt.key,
-            param->u.crypt.key_len
-           );
-
-    dwKeyIndex = (unsigned long)(param->u.crypt.idx);
-    if (param->u.crypt.flags & HOSTAP_CRYPT_FLAG_SET_TX_KEY) {
-        pDevice->byKeyIndex = (unsigned char)dwKeyIndex;
-        pDevice->bTransmitKey = true;
-        dwKeyIndex |= (1 << 31);
-    }
+	memcpy(abyKey, param->u.crypt.key, param->u.crypt.key_len);
+	// copy to node key tbl
+	pMgmt->sNodeDBTable[iNodeIndex].byKeyIndex = param->u.crypt.idx;
+	pMgmt->sNodeDBTable[iNodeIndex].uWepKeyLength = param->u.crypt.key_len;
+	memcpy(&pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[0],
+	       param->u.crypt.key,
+	       param->u.crypt.key_len
+);
+
+	dwKeyIndex = (unsigned long)(param->u.crypt.idx);
+	if (param->u.crypt.flags & HOSTAP_CRYPT_FLAG_SET_TX_KEY) {
+		pDevice->byKeyIndex = (unsigned char)dwKeyIndex;
+		pDevice->bTransmitKey = true;
+		dwKeyIndex |= (1 << 31);
+	}
 
 	if (param->u.crypt.alg == WPA_ALG_WEP) {
-
-        if ((pDevice->bEnable8021x == false) || (iNodeIndex == 0)) {
-            KeybSetDefaultKey(&(pDevice->sKey),
-                                dwKeyIndex & ~(BIT30 | USE_KEYRSC),
-                                param->u.crypt.key_len,
-                                NULL,
-                                abyKey,
-                                KEY_CTL_WEP,
-                                pDevice->PortOffset,
-                                pDevice->byLocalID);
-
-        } else {
-            // 8021x enable, individual key
-            dwKeyIndex |= (1 << 30); // set pairwise key
-            if (KeybSetKey(&(pDevice->sKey),
-                           &param->sta_addr[0],
-                           dwKeyIndex & ~(USE_KEYRSC),
-                           param->u.crypt.key_len,
-                           (PQWORD) &(KeyRSC),
-                           (unsigned char *)abyKey,
-                            KEY_CTL_WEP,
-                            pDevice->PortOffset,
-                            pDevice->byLocalID) == true) {
-
-                pMgmt->sNodeDBTable[iNodeIndex].bOnFly = true;
-
-            } else {
-                // Key Table Full
-                pMgmt->sNodeDBTable[iNodeIndex].bOnFly = false;
-                bKeyTableFull = true;
-            }
-        }
-        pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
-        pDevice->bEncryptionEnable = true;
-        pMgmt->byCSSPK = KEY_CTL_WEP;
-        pMgmt->byCSSGK = KEY_CTL_WEP;
-        pMgmt->sNodeDBTable[iNodeIndex].byCipherSuite = KEY_CTL_WEP;
-        pMgmt->sNodeDBTable[iNodeIndex].dwKeyIndex = dwKeyIndex;
-        return ret;
+		if ((pDevice->bEnable8021x == false) || (iNodeIndex == 0)) {
+			KeybSetDefaultKey(&(pDevice->sKey),
+					  dwKeyIndex & ~(BIT30 | USE_KEYRSC),
+					  param->u.crypt.key_len,
+					  NULL,
+					  abyKey,
+					  KEY_CTL_WEP,
+					  pDevice->PortOffset,
+					  pDevice->byLocalID);
+
+		} else {
+			// 8021x enable, individual key
+			dwKeyIndex |= (1 << 30); // set pairwise key
+			if (KeybSetKey(&(pDevice->sKey),
+				       &param->sta_addr[0],
+				       dwKeyIndex & ~(USE_KEYRSC),
+				       param->u.crypt.key_len,
+				       (PQWORD) &(KeyRSC),
+				       (unsigned char *)abyKey,
+				       KEY_CTL_WEP,
+				       pDevice->PortOffset,
+				       pDevice->byLocalID) == true) {
+				pMgmt->sNodeDBTable[iNodeIndex].bOnFly = true;
+
+			} else {
+				// Key Table Full
+				pMgmt->sNodeDBTable[iNodeIndex].bOnFly = false;
+				bKeyTableFull = true;
+			}
+		}
+		pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
+		pDevice->bEncryptionEnable = true;
+		pMgmt->byCSSPK = KEY_CTL_WEP;
+		pMgmt->byCSSGK = KEY_CTL_WEP;
+		pMgmt->sNodeDBTable[iNodeIndex].byCipherSuite = KEY_CTL_WEP;
+		pMgmt->sNodeDBTable[iNodeIndex].dwKeyIndex = dwKeyIndex;
+		return ret;
 	}
 
 	if (param->u.crypt.seq) {
-	    memcpy(&abySeq, param->u.crypt.seq, 8);
-		for (ii = 0 ; ii < 8 ; ii++)
+		memcpy(&abySeq, param->u.crypt.seq, 8);
+		for (ii = 0; ii < 8; ii++)
 			KeyRSC |= (unsigned long)abySeq[ii] << (ii * 8);
 
 		dwKeyIndex |= 1 << 29;
@@ -604,91 +582,87 @@ static int hostap_set_encryption(PSDevice pDevice,
 	}
 
 	if (param->u.crypt.alg == WPA_ALG_TKIP) {
-	    if (param->u.crypt.key_len != MAX_KEY_LEN)
-	        return -EINVAL;
-	    pDevice->eEncryptionStatus = Ndis802_11Encryption2Enabled;
-        byKeyDecMode = KEY_CTL_TKIP;
-        pMgmt->byCSSPK = KEY_CTL_TKIP;
-        pMgmt->byCSSGK = KEY_CTL_TKIP;
+		if (param->u.crypt.key_len != MAX_KEY_LEN)
+			return -EINVAL;
+		pDevice->eEncryptionStatus = Ndis802_11Encryption2Enabled;
+		byKeyDecMode = KEY_CTL_TKIP;
+		pMgmt->byCSSPK = KEY_CTL_TKIP;
+		pMgmt->byCSSGK = KEY_CTL_TKIP;
 	}
 
 	if (param->u.crypt.alg == WPA_ALG_CCMP) {
-	    if ((param->u.crypt.key_len != AES_KEY_LEN) ||
-	        (pDevice->byLocalID <= REV_ID_VT3253_A1))
-	        return -EINVAL;
-        pDevice->eEncryptionStatus = Ndis802_11Encryption3Enabled;
-        byKeyDecMode = KEY_CTL_CCMP;
-        pMgmt->byCSSPK = KEY_CTL_CCMP;
-        pMgmt->byCSSGK = KEY_CTL_CCMP;
-    }
-
-
-    if (iNodeIndex == 0) {
-       KeybSetDefaultKey(&(pDevice->sKey),
-                           dwKeyIndex,
-                           param->u.crypt.key_len,
-                           (PQWORD) &(KeyRSC),
-                           abyKey,
-                           byKeyDecMode,
-                           pDevice->PortOffset,
-                           pDevice->byLocalID);
-       pMgmt->sNodeDBTable[iNodeIndex].bOnFly = true;
-
-    } else {
-        dwKeyIndex |= (1 << 30); // set pairwise key
-        if (KeybSetKey(&(pDevice->sKey),
-                       &param->sta_addr[0],
-                       dwKeyIndex,
-                       param->u.crypt.key_len,
-                       (PQWORD) &(KeyRSC),
-                       (unsigned char *)abyKey,
-                        byKeyDecMode,
-                        pDevice->PortOffset,
-                        pDevice->byLocalID) == true) {
-
-            pMgmt->sNodeDBTable[iNodeIndex].bOnFly = true;
-
-        } else {
-            // Key Table Full
-            pMgmt->sNodeDBTable[iNodeIndex].bOnFly = false;
-            bKeyTableFull = true;
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Key Table Full\n");
-        }
-
-    }
-
-    if (bKeyTableFull == true) {
-        wKeyCtl &= 0x7F00;              // clear all key control filed
-        wKeyCtl |= (byKeyDecMode << 4);
-        wKeyCtl |= (byKeyDecMode);
-        wKeyCtl |= 0x0044;              // use group key for all address
-        wKeyCtl |= 0x4000;              // disable KeyTable[MAX_KEY_TABLE-1] on-fly to genernate rx int
-        MACvSetDefaultKeyCtl(pDevice->PortOffset, wKeyCtl, MAX_KEY_TABLE-1, pDevice->byLocalID);
-    }
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Set key sta_index= %d \n", iNodeIndex);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " tx_index=%d len=%d \n", param->u.crypt.idx,
-               param->u.crypt.key_len );
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " key=%x-%x-%x-%x-%x-xxxxx \n",
-               pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[0],
-               pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[1],
-               pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[2],
-               pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[3],
-               pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[4]
-              );
+		if ((param->u.crypt.key_len != AES_KEY_LEN) ||
+		    (pDevice->byLocalID <= REV_ID_VT3253_A1))
+			return -EINVAL;
+		pDevice->eEncryptionStatus = Ndis802_11Encryption3Enabled;
+		byKeyDecMode = KEY_CTL_CCMP;
+		pMgmt->byCSSPK = KEY_CTL_CCMP;
+		pMgmt->byCSSGK = KEY_CTL_CCMP;
+	}
+
+	if (iNodeIndex == 0) {
+		KeybSetDefaultKey(&(pDevice->sKey),
+				  dwKeyIndex,
+				  param->u.crypt.key_len,
+				  (PQWORD) &(KeyRSC),
+				  abyKey,
+				  byKeyDecMode,
+				  pDevice->PortOffset,
+				  pDevice->byLocalID);
+		pMgmt->sNodeDBTable[iNodeIndex].bOnFly = true;
+
+	} else {
+		dwKeyIndex |= (1 << 30); // set pairwise key
+		if (KeybSetKey(&(pDevice->sKey),
+			       &param->sta_addr[0],
+			       dwKeyIndex,
+			       param->u.crypt.key_len,
+			       (PQWORD) &(KeyRSC),
+			       (unsigned char *)abyKey,
+			       byKeyDecMode,
+			       pDevice->PortOffset,
+			       pDevice->byLocalID) == true) {
+			pMgmt->sNodeDBTable[iNodeIndex].bOnFly = true;
+
+		} else {
+			// Key Table Full
+			pMgmt->sNodeDBTable[iNodeIndex].bOnFly = false;
+			bKeyTableFull = true;
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Key Table Full\n");
+		}
+
+	}
+
+	if (bKeyTableFull == true) {
+		wKeyCtl &= 0x7F00;              // clear all key control filed
+		wKeyCtl |= (byKeyDecMode << 4);
+		wKeyCtl |= (byKeyDecMode);
+		wKeyCtl |= 0x0044;              // use group key for all address
+		wKeyCtl |= 0x4000;              // disable KeyTable[MAX_KEY_TABLE-1] on-fly to genernate rx int
+		MACvSetDefaultKeyCtl(pDevice->PortOffset, wKeyCtl, MAX_KEY_TABLE-1, pDevice->byLocalID);
+	}
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Set key sta_index= %d \n", iNodeIndex);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " tx_index=%d len=%d \n", param->u.crypt.idx,
+		param->u.crypt.key_len);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " key=%x-%x-%x-%x-%x-xxxxx \n",
+		pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[0],
+		pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[1],
+		pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[2],
+		pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[3],
+		pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[4]
+);
 
 	// set wep key
-    pDevice->bEncryptionEnable = true;
-    pMgmt->sNodeDBTable[iNodeIndex].byCipherSuite = byKeyDecMode;
-    pMgmt->sNodeDBTable[iNodeIndex].dwKeyIndex = dwKeyIndex;
-    pMgmt->sNodeDBTable[iNodeIndex].dwTSC47_16 = 0;
-    pMgmt->sNodeDBTable[iNodeIndex].wTSC15_0 = 0;
+	pDevice->bEncryptionEnable = true;
+	pMgmt->sNodeDBTable[iNodeIndex].byCipherSuite = byKeyDecMode;
+	pMgmt->sNodeDBTable[iNodeIndex].dwKeyIndex = dwKeyIndex;
+	pMgmt->sNodeDBTable[iNodeIndex].dwTSC47_16 = 0;
+	pMgmt->sNodeDBTable[iNodeIndex].wTSC15_0 = 0;
 
 	return ret;
 }
 
-
-
 /*
  * Description:
  *      get each stations encryption key
@@ -703,36 +677,34 @@ static int hostap_set_encryption(PSDevice pDevice,
  *
  */
 static int hostap_get_encryption(PSDevice pDevice,
-				       struct viawget_hostapd_param *param,
-				       int param_len)
+				 struct viawget_hostapd_param *param,
+				 int param_len)
 {
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	int     ret = 0;
 	int     ii;
-	int     iNodeIndex =0;
-
+	int     iNodeIndex = 0;
 
 	param->u.crypt.err = 0;
 
 	if (is_broadcast_ether_addr(param->sta_addr)) {
-        iNodeIndex = 0;
+		iNodeIndex = 0;
 	} else {
-	    if (BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &iNodeIndex) == false) {
-	        param->u.crypt.err = HOSTAP_CRYPT_ERR_UNKNOWN_ADDR;
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "hostap_get_encryption: HOSTAP_CRYPT_ERR_UNKNOWN_ADDR\n");
-	        return -EINVAL;
-	    }
+		if (BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &iNodeIndex) == false) {
+			param->u.crypt.err = HOSTAP_CRYPT_ERR_UNKNOWN_ADDR;
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "hostap_get_encryption: HOSTAP_CRYPT_ERR_UNKNOWN_ADDR\n");
+			return -EINVAL;
+		}
 	}
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "hostap_get_encryption: %d\n", iNodeIndex);
-    memset(param->u.crypt.seq, 0, 8);
-    for (ii = 0 ; ii < 8 ; ii++) {
-        param->u.crypt.seq[ii] = (unsigned char)pMgmt->sNodeDBTable[iNodeIndex].KeyRSC >> (ii * 8);
-    }
+	memset(param->u.crypt.seq, 0, 8);
+	for (ii = 0; ii < 8; ii++) {
+		param->u.crypt.seq[ii] = (unsigned char)pMgmt->sNodeDBTable[iNodeIndex].KeyRSC >> (ii * 8);
+	}
 
 	return ret;
 }
 
-
 /*
  * Description:
  *      vt6655_hostap_ioctl main function supported for hostap deamon.
@@ -768,80 +740,79 @@ int vt6655_hostap_ioctl(PSDevice pDevice, struct iw_point *p)
 
 	switch (param->cmd) {
 	case VIAWGET_HOSTAPD_SET_ENCRYPTION:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_SET_ENCRYPTION \n");
-        spin_lock_irq(&pDevice->lock);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_SET_ENCRYPTION \n");
+		spin_lock_irq(&pDevice->lock);
 		ret = hostap_set_encryption(pDevice, param, p->length);
-        spin_unlock_irq(&pDevice->lock);
+		spin_unlock_irq(&pDevice->lock);
 		break;
 	case VIAWGET_HOSTAPD_GET_ENCRYPTION:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_GET_ENCRYPTION \n");
-        spin_lock_irq(&pDevice->lock);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_GET_ENCRYPTION \n");
+		spin_lock_irq(&pDevice->lock);
 		ret = hostap_get_encryption(pDevice, param, p->length);
-        spin_unlock_irq(&pDevice->lock);
+		spin_unlock_irq(&pDevice->lock);
 		break;
 	case VIAWGET_HOSTAPD_SET_ASSOC_AP_ADDR:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_SET_ASSOC_AP_ADDR \n");
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_SET_ASSOC_AP_ADDR \n");
 		return -EOPNOTSUPP;
 		break;
 	case VIAWGET_HOSTAPD_FLUSH:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_FLUSH \n");
-        spin_lock_irq(&pDevice->lock);
-    	hostap_flush_sta(pDevice);
-        spin_unlock_irq(&pDevice->lock);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_FLUSH \n");
+		spin_lock_irq(&pDevice->lock);
+		hostap_flush_sta(pDevice);
+		spin_unlock_irq(&pDevice->lock);
 		break;
 	case VIAWGET_HOSTAPD_ADD_STA:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_ADD_STA \n");
-         spin_lock_irq(&pDevice->lock);
-		 ret = hostap_add_sta(pDevice, param);
-         spin_unlock_irq(&pDevice->lock);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_ADD_STA \n");
+		spin_lock_irq(&pDevice->lock);
+		ret = hostap_add_sta(pDevice, param);
+		spin_unlock_irq(&pDevice->lock);
 		break;
 	case VIAWGET_HOSTAPD_REMOVE_STA:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_REMOVE_STA \n");
-         spin_lock_irq(&pDevice->lock);
-		 ret = hostap_remove_sta(pDevice, param);
-         spin_unlock_irq(&pDevice->lock);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_REMOVE_STA \n");
+		spin_lock_irq(&pDevice->lock);
+		ret = hostap_remove_sta(pDevice, param);
+		spin_unlock_irq(&pDevice->lock);
 		break;
 	case VIAWGET_HOSTAPD_GET_INFO_STA:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_GET_INFO_STA \n");
-		 ret = hostap_get_info_sta(pDevice, param);
-		 ap_ioctl = 1;
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_GET_INFO_STA \n");
+		ret = hostap_get_info_sta(pDevice, param);
+		ap_ioctl = 1;
 		break;
 /*
 	case VIAWGET_HOSTAPD_RESET_TXEXC_STA:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_RESET_TXEXC_STA \n");
-		 ret = hostap_reset_txexc_sta(pDevice, param);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_RESET_TXEXC_STA \n");
+		ret = hostap_reset_txexc_sta(pDevice, param);
 		break;
 */
 	case VIAWGET_HOSTAPD_SET_FLAGS_STA:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_SET_FLAGS_STA \n");
-		 ret = hostap_set_flags_sta(pDevice, param);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_SET_FLAGS_STA \n");
+		ret = hostap_set_flags_sta(pDevice, param);
 		break;
 
 	case VIAWGET_HOSTAPD_MLME:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_MLME \n");
-	    return -EOPNOTSUPP;
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_MLME \n");
+		return -EOPNOTSUPP;
 
 	case VIAWGET_HOSTAPD_SET_GENERIC_ELEMENT:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_SET_GENERIC_ELEMENT \n");
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_SET_GENERIC_ELEMENT \n");
 		ret = hostap_set_generic_element(pDevice, param);
 		break;
 
 	case VIAWGET_HOSTAPD_SCAN_REQ:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_SCAN_REQ \n");
-	    return -EOPNOTSUPP;
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_SCAN_REQ \n");
+		return -EOPNOTSUPP;
 
 	case VIAWGET_HOSTAPD_STA_CLEAR_STATS:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_STA_CLEAR_STATS \n");
-	    return -EOPNOTSUPP;
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_STA_CLEAR_STATS \n");
+		return -EOPNOTSUPP;
 
 	default:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "vt6655_hostap_ioctl: unknown cmd=%d\n",
-		       (int)param->cmd);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "vt6655_hostap_ioctl: unknown cmd=%d\n",
+			(int)param->cmd);
 		return -EOPNOTSUPP;
 		break;
 	}
 
-
 	if ((ret == 0) && ap_ioctl) {
 		if (copy_to_user(p->pointer, param, p->length)) {
 			ret = -EFAULT;
@@ -849,9 +820,8 @@ int vt6655_hostap_ioctl(PSDevice pDevice, struct iw_point *p)
 		}
 	}
 
- out:
+out:
 	kfree(param);
 
 	return ret;
 }
-
diff --git a/drivers/staging/vt6655/hostap.h b/drivers/staging/vt6655/hostap.h
index 55db55524d9..f1a4f2eff05 100644
--- a/drivers/staging/vt6655/hostap.h
+++ b/drivers/staging/vt6655/hostap.h
@@ -46,7 +46,6 @@
 #define WLAN_RATE_48M   BIT10
 #define WLAN_RATE_54M   BIT11
 
-
 /*---------------------  Export Classes  ----------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
@@ -65,6 +64,3 @@ int vt6655_hostap_set_hostapd(PSDevice pDevice, int val, int rtnl_locked);
 int vt6655_hostap_ioctl(PSDevice pDevice, struct iw_point *p);
 
 #endif // __HOSTAP_H__
-
-
-
diff --git a/drivers/staging/vt6655/iocmd.h b/drivers/staging/vt6655/iocmd.h
index 166351bb71a..e499f1b6f53 100644
--- a/drivers/staging/vt6655/iocmd.h
+++ b/drivers/staging/vt6655/iocmd.h
@@ -37,7 +37,6 @@
 //typedef uint16_t u16;
 //typedef uint8_t u8;
 
-
 // ioctl Command code
 #define MAGIC_CODE	                 0x3142
 #define IOCTL_CMD_TEST	            (SIOCDEVPRIVATE + 0)
@@ -45,37 +44,34 @@
 #define IOCTL_CMD_HOSTAPD           (SIOCDEVPRIVATE + 2)
 #define IOCTL_CMD_WPA               (SIOCDEVPRIVATE + 3)
 
-
 typedef enum tagWMAC_CMD {
-
-    WLAN_CMD_BSS_SCAN,
-    WLAN_CMD_BSS_JOIN,
-    WLAN_CMD_DISASSOC,
-    WLAN_CMD_SET_WEP,
-    WLAN_CMD_GET_LINK,
-    WLAN_CMD_GET_LISTLEN,
-    WLAN_CMD_GET_LIST,
-    WLAN_CMD_GET_MIB,
-    WLAN_CMD_GET_STAT,
-    WLAN_CMD_STOP_MAC,
-    WLAN_CMD_START_MAC,
-    WLAN_CMD_AP_START,
-    WLAN_CMD_SET_HOSTAPD,
-    WLAN_CMD_SET_HOSTAPD_STA,
-    WLAN_CMD_SET_802_1X,
-    WLAN_CMD_SET_HOST_WEP,
-    WLAN_CMD_SET_WPA,
-    WLAN_CMD_GET_NODE_CNT,
-    WLAN_CMD_ZONETYPE_SET,
-    WLAN_CMD_GET_NODE_LIST
-
+	WLAN_CMD_BSS_SCAN,
+	WLAN_CMD_BSS_JOIN,
+	WLAN_CMD_DISASSOC,
+	WLAN_CMD_SET_WEP,
+	WLAN_CMD_GET_LINK,
+	WLAN_CMD_GET_LISTLEN,
+	WLAN_CMD_GET_LIST,
+	WLAN_CMD_GET_MIB,
+	WLAN_CMD_GET_STAT,
+	WLAN_CMD_STOP_MAC,
+	WLAN_CMD_START_MAC,
+	WLAN_CMD_AP_START,
+	WLAN_CMD_SET_HOSTAPD,
+	WLAN_CMD_SET_HOSTAPD_STA,
+	WLAN_CMD_SET_802_1X,
+	WLAN_CMD_SET_HOST_WEP,
+	WLAN_CMD_SET_WPA,
+	WLAN_CMD_GET_NODE_CNT,
+	WLAN_CMD_ZONETYPE_SET,
+	WLAN_CMD_GET_NODE_LIST
 } WMAC_CMD, *PWMAC_CMD;
 
 typedef enum tagWZONETYPE {
-  ZoneType_USA=0,
-  ZoneType_Japan=1,
-  ZoneType_Europe=2
-}WZONETYPE;
+	ZoneType_USA = 0,
+	ZoneType_Japan = 1,
+	ZoneType_Europe = 2
+} WZONETYPE;
 
 #define ADHOC	0
 #define INFRA	1
@@ -85,8 +81,7 @@ typedef enum tagWZONETYPE {
 #define ADHOC_STARTED	   1
 #define ADHOC_JOINTED	   2
 
-
-#define PHY80211a 	    0
+#define PHY80211a       0
 #define PHY80211b       1
 #define PHY80211g       2
 
@@ -115,25 +110,20 @@ typedef struct tagSCmdRequest {
 //
 
 typedef struct tagSCmdScan {
-
 	u8 ssid[SSID_MAXLEN + 2];
-
 } SCmdScan, *PSCmdScan;
 
-
 //
 // BSS Join
 //
 
 typedef struct tagSCmdBSSJoin {
-
-    u16	    wBSSType;
-    u16     wBBPType;
-    u8	    ssid[SSID_MAXLEN + 2];
-    u32	    uChannel;
-    bool bPSEnable;
-    bool bShareKeyAuth;
-
+	u16	    wBSSType;
+	u16     wBBPType;
+	u8	    ssid[SSID_MAXLEN + 2];
+	u32	    uChannel;
+	bool bPSEnable;
+	bool bShareKeyAuth;
 } SCmdBSSJoin, *PSCmdBSSJoin;
 
 //
@@ -141,83 +131,67 @@ typedef struct tagSCmdBSSJoin {
 //
 
 typedef struct tagSCmdZoneTypeSet {
-
- bool bWrite;
- WZONETYPE  ZoneType;
-
+	bool bWrite;
+	WZONETYPE  ZoneType;
 } SCmdZoneTypeSet, *PSCmdZoneTypeSet;
 
 #ifdef WPA_SM_Transtatus
 typedef struct tagSWPAResult {
-         char	ifname[100];
-         u8 proto;
-         u8 key_mgmt;
-         u8 eap_type;
-         bool authenticated;
+	char	ifname[100];
+	u8 proto;
+	u8 key_mgmt;
+	u8 eap_type;
+	bool authenticated;
 } SWPAResult, *PSWPAResult;
 #endif
 
 typedef struct tagSCmdStartAP {
-
-    u16	    wBSSType;
-    u16     wBBPType;
-    u8	    ssid[SSID_MAXLEN + 2];
-    u32	    uChannel;
-    u32     uBeaconInt;
-    bool bShareKeyAuth;
-    u8      byBasicRate;
-
+	u16	    wBSSType;
+	u16     wBBPType;
+	u8	    ssid[SSID_MAXLEN + 2];
+	u32	    uChannel;
+	u32     uBeaconInt;
+	bool bShareKeyAuth;
+	u8      byBasicRate;
 } SCmdStartAP, *PSCmdStartAP;
 
-
 typedef struct tagSCmdSetWEP {
-
-    bool bEnableWep;
-    u8      byKeyIndex;
-    u8      abyWepKey[WEP_NKEYS][WEP_KEYMAXLEN];
-    bool bWepKeyAvailable[WEP_NKEYS];
-    u32     auWepKeyLength[WEP_NKEYS];
-
+	bool bEnableWep;
+	u8      byKeyIndex;
+	u8      abyWepKey[WEP_NKEYS][WEP_KEYMAXLEN];
+	bool bWepKeyAvailable[WEP_NKEYS];
+	u32     auWepKeyLength[WEP_NKEYS];
 } SCmdSetWEP, *PSCmdSetWEP;
 
-
-
 typedef struct tagSBSSIDItem {
-
 	u32	    uChannel;
-    u8      abyBSSID[BSSID_LEN];
-    u8      abySSID[SSID_MAXLEN + 1];
-    //2006-1116-01,<Modify> by NomadZhao
-    //u16	    wBeaconInterval;
-    //u16	    wCapInfo;
-    //u8      byNetType;
-    u8      byNetType;
-    u16	    wBeaconInterval;
-    u16	    wCapInfo;        // for address of byNetType at align 4
-
-    bool bWEPOn;
-    u32     uRSSI;
-
+	u8      abyBSSID[BSSID_LEN];
+	u8      abySSID[SSID_MAXLEN + 1];
+	//2006-1116-01,<Modify> by NomadZhao
+	//u16	    wBeaconInterval;
+	//u16	    wCapInfo;
+	//u8      byNetType;
+	u8      byNetType;
+	u16	    wBeaconInterval;
+	u16	    wCapInfo;        // for address of byNetType at align 4
+
+	bool bWEPOn;
+	u32     uRSSI;
 } SBSSIDItem;
 
-
 typedef struct tagSBSSIDList {
-
 	u32		    uItem;
 	SBSSIDItem	sBSSIDList[0];
 } SBSSIDList, *PSBSSIDList;
 
-
 typedef struct tagSCmdLinkStatus {
-
-    bool bLink;
+	bool bLink;
 	u16   wBSSType;
 	u8      byState;
-    u8      abyBSSID[BSSID_LEN];
-    u8      abySSID[SSID_MAXLEN + 2];
-    u32     uChannel;
-    u32     uLinkRate;
-
+	u8      abyBSSID[BSSID_LEN];
+	u8      abySSID[SSID_MAXLEN + 2];
+	u32     uChannel;
+	u32     uLinkRate;
 } SCmdLinkStatus, *PSCmdLinkStatus;
 
 //
@@ -238,15 +212,13 @@ typedef struct tagSDot11MIBCount {
 	u32 FCSErrorCount;
 } SDot11MIBCount, *PSDot11MIBCount;
 
-
-
 //
 // statistic counter
 //
 typedef struct tagSStatMIBCount {
-    //
-    // ISR status count
-    //
+	//
+	// ISR status count
+	//
 	u32   dwIsrTx0OK;
 	u32   dwIsrTx1OK;
 	u32   dwIsrBeaconTxOK;
@@ -256,12 +228,12 @@ typedef struct tagSStatMIBCount {
 	u32   dwIsrUnrecoverableError;
 	u32   dwIsrSoftInterrupt;
 	u32   dwIsrRxNoBuf;
-    /////////////////////////////////////
+	/////////////////////////////////////
 
 	u32   dwIsrUnknown;               // unknown interrupt count
 
-    // RSR status count
-    //
+	// RSR status count
+	//
 	u32   dwRsrFrmAlgnErr;
 	u32   dwRsrErr;
 	u32   dwRsrCRCErr;
@@ -282,10 +254,10 @@ typedef struct tagSStatMIBCount {
 	u32   dwRsrBroadcast;
 	u32   dwRsrMulticast;
 	u32   dwRsrDirected;
-    // 64-bit OID
+	// 64-bit OID
 	u32   ullRsrOK;
 
-    // for some optional OIDs (64 bits) and DMI support
+	// for some optional OIDs (64 bits) and DMI support
 	u32   ullRxBroadcastBytes;
 	u32   ullRxMulticastBytes;
 	u32   ullRxDirectedBytes;
@@ -301,13 +273,13 @@ typedef struct tagSStatMIBCount {
 	u32   dwRsrRxFrmLen512_1023;
 	u32   dwRsrRxFrmLen1024_1518;
 
-    // TSR0,1 status count
-    //
+	// TSR0,1 status count
+	//
 	u32   dwTsrTotalRetry[2];        // total collision retry count
 	u32   dwTsrOnceRetry[2];         // this packet only occur one collision
 	u32   dwTsrMoreThanOnceRetry[2]; // this packet occur more than one collision
 	u32   dwTsrRetry[2];             // this packet has ever occur collision,
-                                       // that is (dwTsrOnceCollision0 + dwTsrMoreThanOnceCollision0)
+	// that is (dwTsrOnceCollision0 + dwTsrMoreThanOnceCollision0)
 	u32   dwTsrACKData[2];
 	u32   dwTsrErr[2];
 	u32   dwAllTsrOK[2];
@@ -320,23 +292,23 @@ typedef struct tagSStatMIBCount {
 	u32   dwTsrMulticast[2];
 	u32   dwTsrDirected[2];
 
-    // RD/TD count
+	// RD/TD count
 	u32   dwCntRxFrmLength;
 	u32   dwCntTxBufLength;
 
 	u8    abyCntRxPattern[16];
 	u8    abyCntTxPattern[16];
 
-    // Software check....
+	// Software check....
 	u32   dwCntRxDataErr;             // rx buffer data software compare CRC err count
 	u32   dwCntDecryptErr;            // rx buffer data software compare CRC err count
 	u32   dwCntRxICVErr;              // rx buffer data software compare CRC err count
 	u32    idxRxErrorDesc;             // index for rx data error RD
 
-    // 64-bit OID
+	// 64-bit OID
 	u32   ullTsrOK[2];
 
-    // for some optional OIDs (64 bits) and DMI support
+	// for some optional OIDs (64 bits) and DMI support
 	u32   ullTxBroadcastFrames[2];
 	u32   ullTxMulticastFrames[2];
 	u32   ullTxDirectedFrames[2];
@@ -345,49 +317,38 @@ typedef struct tagSStatMIBCount {
 	u32   ullTxDirectedBytes[2];
 } SStatMIBCount, *PSStatMIBCount;
 
-
 typedef struct tagSNodeItem {
-    // STA info
-    u16            wAID;
-    u8             abyMACAddr[6];
-    u16            wTxDataRate;
-    u16            wInActiveCount;
-    u16            wEnQueueCnt;
-    u16            wFlags;
-    bool bPWBitOn;
-    u8             byKeyIndex;
-    u16            wWepKeyLength;
-    u8            abyWepKey[WEP_KEYMAXLEN];
-    // Auto rate fallback vars
-    bool bIsInFallback;
-    u32            uTxFailures;
-    u32            uTxAttempts;
-    u16            wFailureRatio;
-
+	// STA info
+	u16            wAID;
+	u8             abyMACAddr[6];
+	u16            wTxDataRate;
+	u16            wInActiveCount;
+	u16            wEnQueueCnt;
+	u16            wFlags;
+	bool bPWBitOn;
+	u8             byKeyIndex;
+	u16            wWepKeyLength;
+	u8            abyWepKey[WEP_KEYMAXLEN];
+	// Auto rate fallback vars
+	bool bIsInFallback;
+	u32            uTxFailures;
+	u32            uTxAttempts;
+	u16            wFailureRatio;
 } SNodeItem;
 
-
 typedef struct tagSNodeList {
-
 	u32		    uItem;
 	SNodeItem	sNodeList[0];
-
 } SNodeList, *PSNodeList;
 
-
-
 typedef struct tagSCmdValue {
-
 	u32 dwValue;
-
 } SCmdValue,  *PSCmdValue;
 
-
 //
 // hostapd & viawget ioctl related
 //
 
-
 // VIAGWET_IOCTL_HOSTAPD ioctl() cmd:
 enum {
 	VIAWGET_HOSTAPD_FLUSH = 1,
@@ -404,14 +365,11 @@ enum {
 	VIAWGET_HOSTAPD_STA_CLEAR_STATS = 12,
 };
 
-
-#define VIAWGET_HOSTAPD_GENERIC_ELEMENT_HDR_LEN \
-((int) (&((struct viawget_hostapd_param *) 0)->u.generic_elem.data))
+#define VIAWGET_HOSTAPD_GENERIC_ELEMENT_HDR_LEN				\
+	((int)(&((struct viawget_hostapd_param *)0)->u.generic_elem.data))
 
 // Maximum length for algorithm names (-1 for nul termination) used in ioctl()
 
-
-
 struct viawget_hostapd_param {
 	u32 cmd;
 	u8 sta_addr[6];
@@ -464,12 +422,8 @@ struct viawget_hostapd_param {
 
 /*---------------------  Export Variables  --------------------------*/
 
-
 /*---------------------  Export Types  ------------------------------*/
 
-
 /*---------------------  Export Functions  --------------------------*/
 
-
-
 #endif //__IOCMD_H__
diff --git a/drivers/staging/vt6655/ioctl.c b/drivers/staging/vt6655/ioctl.c
index afed6e33dfc..2ae8116869e 100644
--- a/drivers/staging/vt6655/ioctl.c
+++ b/drivers/staging/vt6655/ioctl.c
@@ -41,7 +41,7 @@
 static int msglevel = MSG_LEVEL_INFO;
 
 #ifdef WPA_SM_Transtatus
-	SWPAResult wpa_Result;
+SWPAResult wpa_Result;
 #endif
 
 int private_ioctl(PSDevice pDevice, struct ifreq *rq)
@@ -104,9 +104,9 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 			BSSvClearBSSList((void *)pDevice, pDevice->bLinkPass);
 
 		if (pItemSSID->len != 0)
-			bScheduleCommand((void *) pDevice, WLAN_CMD_BSSID_SCAN, abyScanSSID);
+			bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, abyScanSSID);
 		else
-			bScheduleCommand((void *) pDevice, WLAN_CMD_BSSID_SCAN, NULL);
+			bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, NULL);
 		spin_unlock_irq(&pDevice->lock);
 		break;
 
@@ -202,8 +202,8 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 		netif_stop_queue(pDevice->dev);
 		spin_lock_irq(&pDevice->lock);
 		pMgmt->eCurrState = WMAC_STATE_IDLE;
-		bScheduleCommand((void *) pDevice, WLAN_CMD_BSSID_SCAN, pMgmt->abyDesireSSID);
-		bScheduleCommand((void *) pDevice, WLAN_CMD_SSID, NULL);
+		bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, pMgmt->abyDesireSSID);
+		bScheduleCommand((void *)pDevice, WLAN_CMD_SSID, NULL);
 		spin_unlock_irq(&pDevice->lock);
 		break;
 
@@ -267,7 +267,7 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 			memcpy(sLinkStatus.abySSID, pItemSSID->abySSID, pItemSSID->len);
 			memcpy(sLinkStatus.abyBSSID, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
 			sLinkStatus.uLinkRate = pMgmt->sNodeDBTable[0].wTxDataRate;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" Link Success!\n");
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Link Success!\n");
 		} else {
 			sLinkStatus.bLink = false;
 			sLinkStatus.uLinkRate = 0;
@@ -311,7 +311,7 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 		}
 		pList->uItem = sList.uItem;
 		pBSS = &(pMgmt->sBSSList[0]);
-		for (ii = 0, jj = 0; jj < MAX_BSS_NUM ; jj++) {
+		for (ii = 0, jj = 0; jj < MAX_BSS_NUM; jj++) {
 			pBSS = &(pMgmt->sBSSList[jj]);
 			if (pBSS->bActive) {
 				pList->sBSSIDList[ii].uChannel = pBSS->uChannel;
@@ -540,7 +540,7 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 		}
 
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Support Rate= %*ph\n",
-				4, pMgmt->abyIBSSSuppRates + 2);
+			4, pMgmt->abyIBSSSuppRates + 2);
 
 		netif_stop_queue(pDevice->dev);
 		spin_lock_irq(&pDevice->lock);
diff --git a/drivers/staging/vt6655/ioctl.h b/drivers/staging/vt6655/ioctl.h
index ba85015c11b..ae240fd71a1 100644
--- a/drivers/staging/vt6655/ioctl.h
+++ b/drivers/staging/vt6655/ioctl.h
@@ -33,7 +33,6 @@
 
 /*---------------------  Export Definitions -------------------------*/
 
-
 /*---------------------  Export Classes  ----------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
@@ -43,15 +42,12 @@
 int private_ioctl(PSDevice pDevice, struct ifreq *rq);
 
 /*
-void vConfigWEPKey (
-    PSDevice pDevice,
-    unsigned long dwKeyIndex,
-    unsigned char *pbyKey,
-    unsigned long uKeyLength
-    );
+  void vConfigWEPKey(
+  PSDevice pDevice,
+  unsigned long dwKeyIndex,
+  unsigned char *pbyKey,
+  unsigned long uKeyLength
+);
 */
 
 #endif // __IOCTL_H__
-
-
-
diff --git a/drivers/staging/vt6655/iowpa.h b/drivers/staging/vt6655/iowpa.h
index 33ae054478d..bfea01f5e00 100644
--- a/drivers/staging/vt6655/iowpa.h
+++ b/drivers/staging/vt6655/iowpa.h
@@ -31,17 +31,15 @@
 
 /*---------------------  Export Definitions -------------------------*/
 
-
 #define WPA_IE_LEN 64
 
-
 //WPA related
 /*
-typedef enum { WPA_ALG_NONE, WPA_ALG_WEP, WPA_ALG_TKIP, WPA_ALG_CCMP } wpa_alg;
-typedef enum { CIPHER_NONE, CIPHER_WEP40, CIPHER_TKIP, CIPHER_CCMP,
-	       CIPHER_WEP104 } wpa_cipher;
-typedef enum { KEY_MGMT_802_1X, KEY_MGMT_PSK, KEY_MGMT_NONE,
-	       KEY_MGMT_802_1X_NO_WPA, KEY_MGMT_WPA_NONE } wpa_key_mgmt;
+  typedef enum { WPA_ALG_NONE, WPA_ALG_WEP, WPA_ALG_TKIP, WPA_ALG_CCMP } wpa_alg;
+  typedef enum { CIPHER_NONE, CIPHER_WEP40, CIPHER_TKIP, CIPHER_CCMP,
+  CIPHER_WEP104 } wpa_cipher;
+  typedef enum { KEY_MGMT_802_1X, KEY_MGMT_PSK, KEY_MGMT_NONE,
+  KEY_MGMT_802_1X_NO_WPA, KEY_MGMT_WPA_NONE } wpa_key_mgmt;
 */
 
 enum {
@@ -54,10 +52,9 @@ enum {
 	VIAWGET_SET_DROP_UNENCRYPT = 7,
 	VIAWGET_SET_DEAUTHENTICATE = 8,
 	VIAWGET_SET_ASSOCIATE = 9,
-	VIAWGET_SET_DISASSOCIATE= 10
+	VIAWGET_SET_DISASSOCIATE = 10
 };
 
-
 enum {
 	VIAWGET_ASSOC_MSG = 1,
 	VIAWGET_DISASSOC_MSG = 2,
@@ -67,8 +64,6 @@ enum {
 	VIAWGET_DEVICECLOSE_MSG = 6
 };
 
-
-
 #pragma pack(1)
 typedef struct viawget_wpa_header {
 	u8 type;
@@ -76,8 +71,6 @@ typedef struct viawget_wpa_header {
 	u16 resp_ie_len;
 } viawget_wpa_header;
 
-
-
 struct viawget_wpa_param {
 	u32 cmd;
 	u8 addr[6];
@@ -88,27 +81,27 @@ struct viawget_wpa_param {
 		} generic_elem;
 
 		struct {
-        	u8 bssid[6];
+			u8 bssid[6];
 			u8 ssid[32];
 			u8 ssid_len;
-        	u8 *wpa_ie;
-        	u16 wpa_ie_len;
-        	int pairwise_suite;
-        	int group_suite;
-        	int key_mgmt_suite;
-        	int auth_alg;
-        	int mode;
+			u8 *wpa_ie;
+			u16 wpa_ie_len;
+			int pairwise_suite;
+			int group_suite;
+			int key_mgmt_suite;
+			int auth_alg;
+			int mode;
 
 		} wpa_associate;
 
 		struct {
-	        int alg_name;
-	        u16 key_index;
-	        u16 set_tx;
-	        u8 *seq;
-	        u16 seq_len;
-	        u8 *key;
-	        u16 key_len;
+			int alg_name;
+			u16 key_index;
+			u16 set_tx;
+			u8 *seq;
+			u16 seq_len;
+			u8 *key;
+			u16 key_len;
 		} wpa_key;
 
 		struct {
@@ -122,7 +115,6 @@ struct viawget_wpa_param {
 		} scan_results;
 
 	} u;
-
 };
 
 #pragma pack(1)
@@ -148,12 +140,8 @@ struct viawget_scan_result {
 
 /*---------------------  Export Variables  --------------------------*/
 
-
 /*---------------------  Export Types  ------------------------------*/
 
-
 /*---------------------  Export Functions  --------------------------*/
 
-
-
 #endif //__IOWPA_H__
diff --git a/drivers/staging/vt6655/iwctl.c b/drivers/staging/vt6655/iwctl.c
index 5cdda8dab85..9de698ef25f 100644
--- a/drivers/staging/vt6655/iwctl.c
+++ b/drivers/staging/vt6655/iwctl.c
@@ -57,20 +57,17 @@ extern unsigned short TxRate_iwconfig;//2008-5-8 <add> by chester
 #endif
 
 static const long frequency_list[] = {
-    2412, 2417, 2422, 2427, 2432, 2437, 2442, 2447, 2452, 2457, 2462, 2467, 2472, 2484,
-    4915, 4920, 4925, 4935, 4940, 4945, 4960, 4980,
-    5035, 5040, 5045, 5055, 5060, 5080, 5170, 5180, 5190, 5200, 5210, 5220, 5230, 5240,
-    5260, 5280, 5300, 5320, 5500, 5520, 5540, 5560, 5580, 5600, 5620, 5640, 5660, 5680,
-    5700, 5745, 5765, 5785, 5805, 5825
-	};
-
+	2412, 2417, 2422, 2427, 2432, 2437, 2442, 2447, 2452, 2457, 2462, 2467, 2472, 2484,
+	4915, 4920, 4925, 4935, 4940, 4945, 4960, 4980,
+	5035, 5040, 5045, 5055, 5060, 5080, 5170, 5180, 5190, 5200, 5210, 5220, 5230, 5240,
+	5260, 5280, 5300, 5320, 5500, 5520, 5540, 5560, 5580, 5600, 5620, 5640, 5660, 5680,
+	5700, 5745, 5765, 5785, 5805, 5825
+};
 
 /*---------------------  Static Classes  ----------------------------*/
 
-
 //static int          msglevel                =MSG_LEVEL_DEBUG;
-static int          msglevel                =MSG_LEVEL_INFO;
-
+static int msglevel = MSG_LEVEL_INFO;
 
 /*---------------------  Static Variables  --------------------------*/
 /*---------------------  Static Functions  --------------------------*/
@@ -83,13 +80,13 @@ struct iw_statistics *iwctl_get_wireless_stats(struct net_device *dev)
 	long ldBm;
 
 	pDevice->wstats.status = pDevice->eOPMode;
-	#ifdef Calcu_LinkQual
-	   if(pDevice->scStatistic.LinkQuality > 100)
-   	       pDevice->scStatistic.LinkQuality = 100;
-               pDevice->wstats.qual.qual =(unsigned char) pDevice->scStatistic.LinkQuality;
-	#else
+#ifdef Calcu_LinkQual
+	if (pDevice->scStatistic.LinkQuality > 100)
+		pDevice->scStatistic.LinkQuality = 100;
+	pDevice->wstats.qual.qual = (unsigned char)pDevice->scStatistic.LinkQuality;
+#else
 	pDevice->wstats.qual.qual = pDevice->byCurrSQ;
-	#endif
+#endif
 	RFvRSSITodBm(pDevice, (unsigned char)(pDevice->uCurrRSSI), &ldBm);
 	pDevice->wstats.qual.level = ldBm;
 	//pDevice->wstats.qual.level = 0x100 - pDevice->uCurrRSSI;
@@ -105,29 +102,25 @@ struct iw_statistics *iwctl_get_wireless_stats(struct net_device *dev)
 	return &pDevice->wstats;
 }
 
-
-
 /*------------------------------------------------------------------*/
 
-
 static int iwctl_commit(struct net_device *dev,
-			      struct iw_request_info *info,
-			      void *wrq,
-			      char *extra)
+			struct iw_request_info *info,
+			void *wrq,
+			char *extra)
 {
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWCOMMIT \n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWCOMMIT \n");
 
 	return 0;
-
 }
 /*
  * Wireless Handler : get protocol name
  */
 
 int iwctl_giwname(struct net_device *dev,
-			 struct iw_request_info *info,
-			 char *wrq,
-			 char *extra)
+		  struct iw_request_info *info,
+		  char *wrq,
+		  char *extra)
 {
 	strcpy(wrq, "802.11-a/b/g");
 	return 0;
@@ -138,83 +131,77 @@ int iwctl_giwname(struct net_device *dev,
  */
 
 int iwctl_siwscan(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_point *wrq,
-             char *extra)
+		  struct iw_request_info *info,
+		  struct iw_point *wrq,
+		  char *extra)
 {
 	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-	 PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
+	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
 	struct iw_scan_req  *req = (struct iw_scan_req *)extra;
 	unsigned char abyScanSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
-	PWLAN_IE_SSID       pItemSSID=NULL;
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWSCAN \n");
+	PWLAN_IE_SSID pItemSSID = NULL;
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWSCAN \n");
 
-
-if(pDevice->byReAssocCount > 0) {   //reject scan when re-associating!
+	if (pDevice->byReAssocCount > 0) {   //reject scan when re-associating!
 //send scan event to wpa_Supplicant
-  union iwreq_data wrqu;
- PRINT_K("wireless_send_event--->SIOCGIWSCAN(scan done)\n");
- memset(&wrqu, 0, sizeof(wrqu));
- wireless_send_event(pDevice->dev, SIOCGIWSCAN, &wrqu, NULL);
-  return 0;
-}
+		union iwreq_data wrqu;
+		PRINT_K("wireless_send_event--->SIOCGIWSCAN(scan done)\n");
+		memset(&wrqu, 0, sizeof(wrqu));
+		wireless_send_event(pDevice->dev, SIOCGIWSCAN, &wrqu, NULL);
+		return 0;
+	}
 
 	spin_lock_irq(&pDevice->lock);
-	 BSSvClearBSSList((void *)pDevice, pDevice->bLinkPass);
+	BSSvClearBSSList((void *)pDevice, pDevice->bLinkPass);
 
 //mike add: active scan OR passive scan OR desire_ssid scan
- if(wrq->length == sizeof(struct iw_scan_req)) {
-   if (wrq->flags & IW_SCAN_THIS_ESSID)  {                               //desire_ssid scan
-       memset(abyScanSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-       pItemSSID = (PWLAN_IE_SSID)abyScanSSID;
-       pItemSSID->byElementID = WLAN_EID_SSID;
-       memcpy(pItemSSID->abySSID, req->essid, (int)req->essid_len);
-         if (pItemSSID->abySSID[req->essid_len - 1] == '\0') {
-           if(req->essid_len>0)
-		pItemSSID->len = req->essid_len - 1;
-         }
-	else
-	  pItemSSID->len = req->essid_len;
-	  pMgmt->eScanType = WMAC_SCAN_PASSIVE;
-         PRINT_K("SIOCSIWSCAN:[desired_ssid=%s,len=%d]\n",((PWLAN_IE_SSID)abyScanSSID)->abySSID,
-		 	                                                                                ((PWLAN_IE_SSID)abyScanSSID)->len);
-	bScheduleCommand((void *) pDevice, WLAN_CMD_BSSID_SCAN, abyScanSSID);
-	spin_unlock_irq(&pDevice->lock);
+	if (wrq->length == sizeof(struct iw_scan_req)) {
+		if (wrq->flags & IW_SCAN_THIS_ESSID)  {                               //desire_ssid scan
+			memset(abyScanSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
+			pItemSSID = (PWLAN_IE_SSID)abyScanSSID;
+			pItemSSID->byElementID = WLAN_EID_SSID;
+			memcpy(pItemSSID->abySSID, req->essid, (int)req->essid_len);
+			if (pItemSSID->abySSID[req->essid_len - 1] == '\0') {
+				if (req->essid_len > 0)
+					pItemSSID->len = req->essid_len - 1;
+			} else
+				pItemSSID->len = req->essid_len;
+			pMgmt->eScanType = WMAC_SCAN_PASSIVE;
+			PRINT_K("SIOCSIWSCAN:[desired_ssid=%s,len=%d]\n", ((PWLAN_IE_SSID)abyScanSSID)->abySSID,
+				((PWLAN_IE_SSID)abyScanSSID)->len);
+			bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, abyScanSSID);
+			spin_unlock_irq(&pDevice->lock);
+
+			return 0;
+		} else if (req->scan_type == IW_SCAN_TYPE_PASSIVE) {          //passive scan
+			pMgmt->eScanType = WMAC_SCAN_PASSIVE;
+		}
+	} else {           //active scan
+		pMgmt->eScanType = WMAC_SCAN_ACTIVE;
+	}
 
-	return 0;
-   }
-   else if(req->scan_type == IW_SCAN_TYPE_PASSIVE) {          //passive scan
-       pMgmt->eScanType = WMAC_SCAN_PASSIVE;
-   }
- }
- else {           //active scan
-     pMgmt->eScanType = WMAC_SCAN_ACTIVE;
- }
-
-	 pMgmt->eScanType = WMAC_SCAN_PASSIVE;
-          //printk("SIOCSIWSCAN:WLAN_CMD_BSSID_SCAN\n");
-	bScheduleCommand((void *) pDevice, WLAN_CMD_BSSID_SCAN, NULL);
+	pMgmt->eScanType = WMAC_SCAN_PASSIVE;
+	bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, NULL);
 	spin_unlock_irq(&pDevice->lock);
 
 	return 0;
 }
 
-
 /*
  * Wireless Handler : get scan results
  */
 
 int iwctl_giwscan(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_point *wrq,
-             char *extra)
+		  struct iw_request_info *info,
+		  struct iw_point *wrq,
+		  char *extra)
 {
-    int ii, jj, kk;
+	int ii, jj, kk;
 	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-    PKnownBSS           pBSS;
-    PWLAN_IE_SSID       pItemSSID;
-    PWLAN_IE_SUPP_RATES pSuppRates, pExtSuppRates;
+	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
+	PKnownBSS           pBSS;
+	PWLAN_IE_SSID       pItemSSID;
+	PWLAN_IE_SUPP_RATES pSuppRates, pExtSuppRates;
 	char *current_ev = extra;
 	char *end_buf = extra + IW_SCAN_MAX_DATA;
 	char *current_val = NULL;
@@ -222,181 +209,177 @@ int iwctl_giwscan(struct net_device *dev,
 	long ldBm;
 	char buf[MAX_WPA_IE_LEN * 2 + 30];
 
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWSCAN \n");
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWSCAN \n");
-
-    if (pMgmt->eScanState ==  WMAC_IS_SCANNING) {
-        // In scanning..
+	if (pMgmt->eScanState ==  WMAC_IS_SCANNING) {
+		// In scanning..
 		return -EAGAIN;
 	}
 	pBSS = &(pMgmt->sBSSList[0]);
-    for (ii = 0, jj = 0; jj < MAX_BSS_NUM ; jj++) {
+	for (ii = 0, jj = 0; jj < MAX_BSS_NUM; jj++) {
 		if (current_ev >= end_buf)
 			break;
-        pBSS = &(pMgmt->sBSSList[jj]);
-        if (pBSS->bActive) {
-		//ADD mac address
-		    memset(&iwe, 0, sizeof(iwe));
-		    iwe.cmd = SIOCGIWAP;
-		    iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
+		pBSS = &(pMgmt->sBSSList[jj]);
+		if (pBSS->bActive) {
+			//ADD mac address
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = SIOCGIWAP;
+			iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
 			memcpy(iwe.u.ap_addr.sa_data, pBSS->abyBSSID, WLAN_BSSID_LEN);
-                            current_ev = iwe_stream_add_event(info,current_ev,end_buf, &iwe, IW_EV_ADDR_LEN);
-                 //ADD ssid
-	             memset(&iwe, 0, sizeof(iwe));
-                      iwe.cmd = SIOCGIWESSID;
-                      pItemSSID = (PWLAN_IE_SSID)pBSS->abySSID;
-                       iwe.u.data.length = pItemSSID->len;
-                       iwe.u.data.flags = 1;
-                      current_ev = iwe_stream_add_point(info,current_ev,end_buf, &iwe, pItemSSID->abySSID);
-		//ADD mode
-		    memset(&iwe, 0, sizeof(iwe));
-		    iwe.cmd = SIOCGIWMODE;
-            if (WLAN_GET_CAP_INFO_ESS(pBSS->wCapInfo)) {
-		        iwe.u.mode = IW_MODE_INFRA;
-            }
-            else {
-                iwe.u.mode = IW_MODE_ADHOC;
-		    }
-	        iwe.len = IW_EV_UINT_LEN;
-                      current_ev = iwe_stream_add_event(info,current_ev, end_buf, &iwe,  IW_EV_UINT_LEN);
-           //ADD frequency
-            pSuppRates = (PWLAN_IE_SUPP_RATES)pBSS->abySuppRates;
-            pExtSuppRates = (PWLAN_IE_SUPP_RATES)pBSS->abyExtSuppRates;
-            memset(&iwe, 0, sizeof(iwe));
-           	iwe.cmd = SIOCGIWFREQ;
-           	iwe.u.freq.m = pBSS->uChannel;
-           	iwe.u.freq.e = 0;
-           	iwe.u.freq.i = 0;
-                   current_ev = iwe_stream_add_event(info,current_ev,end_buf, &iwe, IW_EV_FREQ_LEN);
-            //2008-0409-04, <Add> by Einsn Liu
+			current_ev = iwe_stream_add_event(info, current_ev, end_buf, &iwe, IW_EV_ADDR_LEN);
+			//ADD ssid
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = SIOCGIWESSID;
+			pItemSSID = (PWLAN_IE_SSID)pBSS->abySSID;
+			iwe.u.data.length = pItemSSID->len;
+			iwe.u.data.flags = 1;
+			current_ev = iwe_stream_add_point(info, current_ev, end_buf, &iwe, pItemSSID->abySSID);
+			//ADD mode
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = SIOCGIWMODE;
+			if (WLAN_GET_CAP_INFO_ESS(pBSS->wCapInfo)) {
+				iwe.u.mode = IW_MODE_INFRA;
+			} else {
+				iwe.u.mode = IW_MODE_ADHOC;
+			}
+			iwe.len = IW_EV_UINT_LEN;
+			current_ev = iwe_stream_add_event(info, current_ev, end_buf, &iwe, IW_EV_UINT_LEN);
+			//ADD frequency
+			pSuppRates = (PWLAN_IE_SUPP_RATES)pBSS->abySuppRates;
+			pExtSuppRates = (PWLAN_IE_SUPP_RATES)pBSS->abyExtSuppRates;
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = SIOCGIWFREQ;
+			iwe.u.freq.m = pBSS->uChannel;
+			iwe.u.freq.e = 0;
+			iwe.u.freq.i = 0;
+			current_ev = iwe_stream_add_event(info, current_ev, end_buf, &iwe, IW_EV_FREQ_LEN);
+			//2008-0409-04, <Add> by Einsn Liu
 			{
-			int f = (int)pBSS->uChannel - 1;
-			if(f < 0)f = 0;
-			iwe.u.freq.m = frequency_list[f] * 100000;
-			iwe.u.freq.e = 1;
+				int f = (int)pBSS->uChannel - 1;
+				if (f < 0)f = 0;
+				iwe.u.freq.m = frequency_list[f] * 100000;
+				iwe.u.freq.e = 1;
 			}
-                   current_ev = iwe_stream_add_event(info,current_ev,end_buf, &iwe, IW_EV_FREQ_LEN);
-       		//ADD quality
-            memset(&iwe, 0, sizeof(iwe));
-	        iwe.cmd = IWEVQUAL;
-	        RFvRSSITodBm(pDevice, (unsigned char)(pBSS->uRSSI), &ldBm);
-		    iwe.u.qual.level = ldBm;
-	        iwe.u.qual.noise = 0;
+			current_ev = iwe_stream_add_event(info, current_ev, end_buf, &iwe, IW_EV_FREQ_LEN);
+			//ADD quality
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = IWEVQUAL;
+			RFvRSSITodBm(pDevice, (unsigned char)(pBSS->uRSSI), &ldBm);
+			iwe.u.qual.level = ldBm;
+			iwe.u.qual.noise = 0;
 //2008-0409-01, <Add> by Einsn Liu
-			if(-ldBm<50){
+			if (-ldBm < 50) {
 				iwe.u.qual.qual = 100;
-			}else  if(-ldBm > 90) {
-				 iwe.u.qual.qual = 0;
-			}else {
-				iwe.u.qual.qual=(40-(-ldBm-50))*100/40;
+			} else if (-ldBm > 90) {
+				iwe.u.qual.qual = 0;
+			} else {
+				iwe.u.qual.qual = (40 - (-ldBm - 50)) * 100 / 40;
+			}
+			iwe.u.qual.updated = 7;
+
+			//  iwe.u.qual.qual = 0;
+			current_ev = iwe_stream_add_event(info, current_ev, end_buf, &iwe, IW_EV_QUAL_LEN);
+
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = SIOCGIWENCODE;
+			iwe.u.data.length = 0;
+			if (WLAN_GET_CAP_INFO_PRIVACY(pBSS->wCapInfo)) {
+				iwe.u.data.flags = IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
+			} else {
+				iwe.u.data.flags = IW_ENCODE_DISABLED;
+			}
+			current_ev = iwe_stream_add_point(info, current_ev, end_buf, &iwe, pItemSSID->abySSID);
+
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = SIOCGIWRATE;
+			iwe.u.bitrate.fixed = iwe.u.bitrate.disabled = 0;
+			current_val = current_ev + IW_EV_LCP_LEN;
+
+			for (kk = 0; kk < 12; kk++) {
+				if (pSuppRates->abyRates[kk] == 0)
+					break;
+				// Bit rate given in 500 kb/s units (+ 0x80)
+				iwe.u.bitrate.value = ((pSuppRates->abyRates[kk] & 0x7f) * 500000);
+				current_val = iwe_stream_add_value(info, current_ev, current_val, end_buf, &iwe, IW_EV_PARAM_LEN);
 			}
-			iwe.u.qual.updated=7;
-
-	      //  iwe.u.qual.qual = 0;
-            current_ev = iwe_stream_add_event(info,current_ev, end_buf, &iwe, IW_EV_QUAL_LEN);
-
-            memset(&iwe, 0, sizeof(iwe));
-            iwe.cmd = SIOCGIWENCODE;
-            iwe.u.data.length = 0;
-            if (WLAN_GET_CAP_INFO_PRIVACY(pBSS->wCapInfo)) {
-                iwe.u.data.flags =IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
-            }else {
-                iwe.u.data.flags = IW_ENCODE_DISABLED;
-            }
-            current_ev = iwe_stream_add_point(info,current_ev,end_buf, &iwe, pItemSSID->abySSID);
-
-            memset(&iwe, 0, sizeof(iwe));
-            iwe.cmd = SIOCGIWRATE;
-           	iwe.u.bitrate.fixed = iwe.u.bitrate.disabled = 0;
-      		current_val = current_ev + IW_EV_LCP_LEN;
-
-       		for (kk = 0 ; kk < 12 ; kk++) {
-		        if (pSuppRates->abyRates[kk] == 0)
-			        break;
-		        // Bit rate given in 500 kb/s units (+ 0x80)
-		        iwe.u.bitrate.value = ((pSuppRates->abyRates[kk] & 0x7f) * 500000);
-                           current_val = iwe_stream_add_value(info,current_ev, current_val, end_buf, &iwe, IW_EV_PARAM_LEN);
+			for (kk = 0; kk < 8; kk++) {
+				if (pExtSuppRates->abyRates[kk] == 0)
+					break;
+				// Bit rate given in 500 kb/s units (+ 0x80)
+				iwe.u.bitrate.value = ((pExtSuppRates->abyRates[kk] & 0x7f) * 500000);
+				current_val = iwe_stream_add_value(info, current_ev, current_val, end_buf, &iwe, IW_EV_PARAM_LEN);
+			}
+
+			if ((current_val - current_ev) > IW_EV_LCP_LEN)
+				current_ev = current_val;
+
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = IWEVCUSTOM;
+			sprintf(buf, "bcn_int=%d", pBSS->wBeaconInterval);
+			iwe.u.data.length = strlen(buf);
+			current_ev = iwe_stream_add_point(info, current_ev, end_buf, &iwe, buf);
+
+			if ((pBSS->wWPALen > 0) && (pBSS->wWPALen <= MAX_WPA_IE_LEN)) {
+				memset(&iwe, 0, sizeof(iwe));
+				iwe.cmd = IWEVGENIE;
+				iwe.u.data.length = pBSS->wWPALen;
+				current_ev = iwe_stream_add_point(info, current_ev, end_buf, &iwe, pBSS->byWPAIE);
+			}
+
+			if ((pBSS->wRSNLen > 0) && (pBSS->wRSNLen <= MAX_WPA_IE_LEN)) {
+				memset(&iwe, 0, sizeof(iwe));
+				iwe.cmd = IWEVGENIE;
+				iwe.u.data.length = pBSS->wRSNLen;
+				current_ev = iwe_stream_add_point(info, current_ev, end_buf, &iwe, pBSS->byRSNIE);
+			}
+
 		}
-       		for (kk = 0 ; kk < 8 ; kk++) {
-		        if (pExtSuppRates->abyRates[kk] == 0)
-			        break;
-		        // Bit rate given in 500 kb/s units (+ 0x80)
-		        iwe.u.bitrate.value = ((pExtSuppRates->abyRates[kk] & 0x7f) * 500000);
-                          current_val = iwe_stream_add_value(info,current_ev, current_val, end_buf, &iwe, IW_EV_PARAM_LEN);
-	        }
-
-	        if((current_val - current_ev) > IW_EV_LCP_LEN)
-		        current_ev = current_val;
-
-            memset(&iwe, 0, sizeof(iwe));
-            iwe.cmd = IWEVCUSTOM;
-            sprintf(buf, "bcn_int=%d", pBSS->wBeaconInterval);
-            iwe.u.data.length = strlen(buf);
-             current_ev = iwe_stream_add_point(info,current_ev, end_buf, &iwe, buf);
-
-            if ((pBSS->wWPALen > 0) && (pBSS->wWPALen <= MAX_WPA_IE_LEN)) {
-                memset(&iwe, 0, sizeof(iwe));
-                iwe.cmd = IWEVGENIE;
-                iwe.u.data.length = pBSS->wWPALen;
-                current_ev = iwe_stream_add_point(info,current_ev, end_buf, &iwe, pBSS->byWPAIE);
-            }
-
-            if ((pBSS->wRSNLen > 0) && (pBSS->wRSNLen <= MAX_WPA_IE_LEN)) {
-                memset(&iwe, 0, sizeof(iwe));
-                iwe.cmd = IWEVGENIE;
-                iwe.u.data.length = pBSS->wRSNLen;
-                current_ev = iwe_stream_add_point(info,current_ev, end_buf, &iwe, pBSS->byRSNIE);
-            }
-
-        }
-    }// for
+	}// for
 
 	wrq->length = current_ev - extra;
 	return 0;
-
 }
 
-
 /*
  * Wireless Handler : set frequency or channel
  */
 
 int iwctl_siwfreq(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_freq *wrq,
-             char *extra)
+		  struct iw_request_info *info,
+		  struct iw_freq *wrq,
+		  char *extra)
 {
 	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
 	int rc = 0;
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWFREQ \n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWFREQ \n");
 
 	// If setting by frequency, convert to a channel
-	if((wrq->e == 1) &&
-	   (wrq->m >= (int) 2.412e8) &&
-	   (wrq->m <= (int) 2.487e8)) {
+	if ((wrq->e == 1) &&
+	    (wrq->m >= (int) 2.412e8) &&
+	    (wrq->m <= (int) 2.487e8)) {
 		int f = wrq->m / 100000;
 		int c = 0;
-		while((c < 14) && (f != frequency_list[c]))
+		while ((c < 14) && (f != frequency_list[c]))
 			c++;
 		wrq->e = 0;
 		wrq->m = c + 1;
 	}
 	// Setting by channel number
-	if((wrq->m > 14) || (wrq->e > 0))
+	if ((wrq->m > 14) || (wrq->e > 0))
 		rc = -EOPNOTSUPP;
 	else {
 		int channel = wrq->m;
-		if((channel < 1) || (channel > 14)) {
+		if ((channel < 1) || (channel > 14)) {
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: New channel value of %d is invalid!\n", dev->name, wrq->m);
 			rc = -EINVAL;
 		} else {
-			  // Yes ! We can set it !!!
-              DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Set to channel = %d\n", channel);
-			  pDevice->uChannel = channel;
- 			 //2007-0207-04,<Add> by EinsnLiu
-			 //Make change effect at once
-			  pDevice->bCommit = true;
+			// Yes ! We can set it !!!
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Set to channel = %d\n", channel);
+			pDevice->uChannel = channel;
+			//2007-0207-04,<Add> by EinsnLiu
+			//Make change effect at once
+			pDevice->bCommit = true;
 		}
 	}
 
@@ -408,14 +391,14 @@ int iwctl_siwfreq(struct net_device *dev,
  */
 
 int iwctl_giwfreq(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_freq *wrq,
-             char *extra)
+		  struct iw_request_info *info,
+		  struct iw_freq *wrq,
+		  char *extra)
 {
 	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
+	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWFREQ \n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWFREQ \n");
 
 #ifdef WEXT_USECHANNELS
 	wrq->m = (int)pMgmt->uCurrChannel;
@@ -423,8 +406,8 @@ int iwctl_giwfreq(struct net_device *dev,
 #else
 	{
 		int f = (int)pMgmt->uCurrChannel - 1;
-		if(f < 0)
-		   f = 0;
+		if (f < 0)
+			f = 0;
 		wrq->m = frequency_list[f] * 100000;
 		wrq->e = 1;
 	}
@@ -438,59 +421,58 @@ int iwctl_giwfreq(struct net_device *dev,
  */
 
 int iwctl_siwmode(struct net_device *dev,
-             struct iw_request_info *info,
-             __u32 *wmode,
-             char *extra)
+		  struct iw_request_info *info,
+		  __u32 *wmode,
+		  char *extra)
 {
 	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-    int rc = 0;
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWMODE \n");
+	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
+	int rc = 0;
 
-    if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP && pDevice->bEnableHostapd) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Can't set operation mode, hostapd is running \n");
-        return rc;
-    }
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWMODE \n");
 
-	switch(*wmode) {
+	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP && pDevice->bEnableHostapd) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Can't set operation mode, hostapd is running \n");
+		return rc;
+	}
 
+	switch (*wmode) {
 	case IW_MODE_ADHOC:
-	    if (pMgmt->eConfigMode != WMAC_CONFIG_IBSS_STA) {
-            pMgmt->eConfigMode = WMAC_CONFIG_IBSS_STA;
-            if (pDevice->flags & DEVICE_FLAGS_OPENED) {
-		        pDevice->bCommit = true;
-   		    }
+		if (pMgmt->eConfigMode != WMAC_CONFIG_IBSS_STA) {
+			pMgmt->eConfigMode = WMAC_CONFIG_IBSS_STA;
+			if (pDevice->flags & DEVICE_FLAGS_OPENED) {
+				pDevice->bCommit = true;
+			}
 		}
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set mode to ad-hoc \n");
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set mode to ad-hoc \n");
 		break;
 	case IW_MODE_AUTO:
 	case IW_MODE_INFRA:
-	    if (pMgmt->eConfigMode != WMAC_CONFIG_ESS_STA) {
-            pMgmt->eConfigMode = WMAC_CONFIG_ESS_STA;
-            if (pDevice->flags & DEVICE_FLAGS_OPENED) {
-		        pDevice->bCommit = true;
-   		    }
+		if (pMgmt->eConfigMode != WMAC_CONFIG_ESS_STA) {
+			pMgmt->eConfigMode = WMAC_CONFIG_ESS_STA;
+			if (pDevice->flags & DEVICE_FLAGS_OPENED) {
+				pDevice->bCommit = true;
+			}
 		}
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set mode to infrastructure \n");
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set mode to infrastructure \n");
 		break;
 	case IW_MODE_MASTER:
 
-        pMgmt->eConfigMode = WMAC_CONFIG_ESS_STA;
+		pMgmt->eConfigMode = WMAC_CONFIG_ESS_STA;
 		rc = -EOPNOTSUPP;
 		break;
 
-	    if (pMgmt->eConfigMode != WMAC_CONFIG_AP) {
-            pMgmt->eConfigMode = WMAC_CONFIG_AP;
-            if (pDevice->flags & DEVICE_FLAGS_OPENED) {
-		        pDevice->bCommit = true;
-   		    }
+		if (pMgmt->eConfigMode != WMAC_CONFIG_AP) {
+			pMgmt->eConfigMode = WMAC_CONFIG_AP;
+			if (pDevice->flags & DEVICE_FLAGS_OPENED) {
+				pDevice->bCommit = true;
+			}
 		}
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set mode to Access Point \n");
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set mode to Access Point \n");
 		break;
 
 	case IW_MODE_REPEAT:
-        pMgmt->eConfigMode = WMAC_CONFIG_ESS_STA;
+		pMgmt->eConfigMode = WMAC_CONFIG_ESS_STA;
 		rc = -EOPNOTSUPP;
 		break;
 	default:
@@ -505,22 +487,21 @@ int iwctl_siwmode(struct net_device *dev,
  */
 
 int iwctl_giwmode(struct net_device *dev,
-             struct iw_request_info *info,
-             __u32 *wmode,
-             char *extra)
+		  struct iw_request_info *info,
+		  __u32 *wmode,
+		  char *extra)
 {
 	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-
+	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWMODE \n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWMODE \n");
 	// If not managed, assume it's ad-hoc
 	switch (pMgmt->eConfigMode) {
 	case WMAC_CONFIG_ESS_STA:
 		*wmode = IW_MODE_INFRA;
 		break;
 	case WMAC_CONFIG_IBSS_STA:
-        *wmode = IW_MODE_ADHOC;
+		*wmode = IW_MODE_ADHOC;
 		break;
 	case WMAC_CONFIG_AUTO:
 		*wmode = IW_MODE_INFRA;
@@ -535,22 +516,20 @@ int iwctl_giwmode(struct net_device *dev,
 	return 0;
 }
 
-
 /*
  * Wireless Handler : get capability range
  */
 
 int iwctl_giwrange(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra)
+		   struct iw_request_info *info,
+		   struct iw_point *wrq,
+		   char *extra)
 {
-	struct iw_range *range = (struct iw_range *) extra;
-	int		i,k;
-    unsigned char abySupportedRates[13]= {0x02, 0x04, 0x0B, 0x16, 0x0c, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C, 0x90};
-
+	struct iw_range *range = (struct iw_range *)extra;
+	int i, k;
+	unsigned char abySupportedRates[13] = {0x02, 0x04, 0x0B, 0x16, 0x0c, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C, 0x90};
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWRANGE \n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWRANGE \n");
 	if (wrq->pointer) {
 		wrq->length = sizeof(struct iw_range);
 		memset(range, 0, sizeof(struct iw_range));
@@ -560,25 +539,25 @@ int iwctl_giwrange(struct net_device *dev,
 		// Should be based on cap_rid.country to give only
 		//  what the current card support
 		k = 0;
-		for(i = 0; i < 14; i++) {
+		for (i = 0; i < 14; i++) {
 			range->freq[k].i = i + 1; // List index
 			range->freq[k].m = frequency_list[i] * 100000;
 			range->freq[k++].e = 1;	// Values in table in MHz -> * 10^5 * 10
 		}
 		range->num_frequency = k;
 		// Hum... Should put the right values there
-	     #ifdef Calcu_LinkQual
-                 range->max_qual.qual = 100;
-	     #else
+#ifdef Calcu_LinkQual
+		range->max_qual.qual = 100;
+#else
 		range->max_qual.qual = 255;
-	     #endif
+#endif
 		range->max_qual.level = 0;
 		range->max_qual.noise = 0;
 		range->sensitivity = 255;
 
-		for(i = 0 ; i < 13 ; i++) {
+		for (i = 0; i < 13; i++) {
 			range->bitrate[i] = abySupportedRates[i] * 500000;
-			if(range->bitrate[i] == 0)
+			if (range->bitrate[i] == 0)
 				break;
 		}
 		range->num_bitrates = i;
@@ -586,7 +565,7 @@ int iwctl_giwrange(struct net_device *dev,
 		// Set an indication of the max TCP throughput
 		// in bit/s that we can expect using this interface.
 		//  May be use for QoS stuff... Jean II
-		if(i > 2)
+		if (i > 2)
 			range->throughput = 5 * 1000 * 1000;
 		else
 			range->throughput = 1.5 * 1000 * 1000;
@@ -596,20 +575,19 @@ int iwctl_giwrange(struct net_device *dev,
 		range->min_frag = 256;
 		range->max_frag = 2312;
 
+		// the encoding capabilities
+		range->num_encoding_sizes = 3;
+		// 64(40) bits WEP
+		range->encoding_size[0] = 5;
+		// 128(104) bits WEP
+		range->encoding_size[1] = 13;
+		// 256 bits for WPA-PSK
+		range->encoding_size[2] = 32;
+		// 4 keys are allowed
+		range->max_encoding_tokens = 4;
 
-	    // the encoding capabilities
-	    range->num_encoding_sizes = 3;
-	    // 64(40) bits WEP
-	    range->encoding_size[0] = 5;
-	    // 128(104) bits WEP
-	    range->encoding_size[1] = 13;
-	    // 256 bits for WPA-PSK
-	    range->encoding_size[2] = 32;
-	    // 4 keys are allowed
-	    range->max_encoding_tokens = 4;
-
-	    range->enc_capa = IW_ENC_CAPA_WPA | IW_ENC_CAPA_WPA2 |
-		    IW_ENC_CAPA_CIPHER_TKIP | IW_ENC_CAPA_CIPHER_CCMP;
+		range->enc_capa = IW_ENC_CAPA_WPA | IW_ENC_CAPA_WPA2 |
+			IW_ENC_CAPA_CIPHER_TKIP | IW_ENC_CAPA_CIPHER_CCMP;
 
 		range->min_pmp = 0;
 		range->max_pmp = 1000000;// 1 secs
@@ -621,7 +599,7 @@ int iwctl_giwrange(struct net_device *dev,
 
 		// Transmit Power - values are in mW
 
-        range->txpower[0] = 100;
+		range->txpower[0] = 100;
 		range->num_txpower = 1;
 		range->txpower_capa = IW_TXPOW_MWATT;
 		range->we_version_source = SUPPORTED_WIRELESS_EXT;
@@ -641,65 +619,63 @@ int iwctl_giwrange(struct net_device *dev,
 		range->avg_qual.noise = 0;
 	}
 
-
 	return 0;
 }
 
-
 /*
  * Wireless Handler : set ap mac address
  */
 
 int iwctl_siwap(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct sockaddr *wrq,
-             char *extra)
+		struct iw_request_info *info,
+		struct sockaddr *wrq,
+		char *extra)
 {
 	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-    int rc = 0;
-    unsigned char ZeroBSSID[WLAN_BSSID_LEN]={0x00,0x00,0x00,0x00,0x00,0x00};
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWAP \n");
-if (pMgmt->eScanState ==  WMAC_IS_SCANNING) {
-        // In scanning..
-     printk("SIOCSIWAP(??)-->In scanning...\n");
-   //  return -EAGAIN;
-  }
+	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
+	int rc = 0;
+	unsigned char ZeroBSSID[WLAN_BSSID_LEN] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWAP \n");
+	if (pMgmt->eScanState ==  WMAC_IS_SCANNING) {
+		// In scanning..
+		printk("SIOCSIWAP(??)-->In scanning...\n");
+		//  return -EAGAIN;
+	}
 	if (wrq->sa_family != ARPHRD_ETHER)
 		rc = -EINVAL;
 	else {
 		memcpy(pMgmt->abyDesireBSSID, wrq->sa_data, 6);
-		                //2008-0409-05, <Add> by Einsn Liu
-		if((pDevice->bLinkPass == true) &&
-                     (memcmp(pMgmt->abyDesireBSSID, pMgmt->abyCurrBSSID, 6)== 0)){
+		//2008-0409-05, <Add> by Einsn Liu
+		if ((pDevice->bLinkPass == true) &&
+		    (memcmp(pMgmt->abyDesireBSSID, pMgmt->abyCurrBSSID, 6) == 0)) {
 			return rc;
+		}
+		//mike :add
+		if ((is_broadcast_ether_addr(pMgmt->abyDesireBSSID)) ||
+		    (memcmp(pMgmt->abyDesireBSSID, ZeroBSSID, 6) == 0)) {
+			PRINT_K("SIOCSIWAP:invalid desired BSSID return!\n");
+			return rc;
+		}
+		//mike add: if desired AP is hidden ssid(there are two same BSSID in list),
+		//                  then ignore,because you don't known which one to be connect with??
+		{
+			unsigned int ii, uSameBssidNum = 0;
+			for (ii = 0; ii < MAX_BSS_NUM; ii++) {
+				if (pMgmt->sBSSList[ii].bActive &&
+				    !compare_ether_addr(pMgmt->sBSSList[ii].abyBSSID, pMgmt->abyDesireBSSID)) {
+					uSameBssidNum++;
+				}
+			}
+			if (uSameBssidNum >= 2) {  //hit: desired AP is in hidden ssid mode!!!
+				PRINT_K("SIOCSIWAP:ignore for desired AP in hidden mode\n");
+				return rc;
 			}
-	//mike :add
-	 if ((is_broadcast_ether_addr(pMgmt->abyDesireBSSID)) ||
-	     (memcmp(pMgmt->abyDesireBSSID, ZeroBSSID, 6) == 0)){
-	      PRINT_K("SIOCSIWAP:invalid desired BSSID return!\n");
-               return rc;
-         }
-       //mike add: if desired AP is hidden ssid(there are two same BSSID in list),
-       //                  then ignore,because you don't known which one to be connect with??
-       	{
-           unsigned int ii , uSameBssidNum=0;
-                  for (ii = 0; ii < MAX_BSS_NUM; ii++) {
-                     if (pMgmt->sBSSList[ii].bActive &&
-                        !compare_ether_addr(pMgmt->sBSSList[ii].abyBSSID, pMgmt->abyDesireBSSID)) {
-                        uSameBssidNum++;
-                     }
-                  }
-	     if(uSameBssidNum >= 2) {  //hit: desired AP is in hidden ssid mode!!!
-                 PRINT_K("SIOCSIWAP:ignore for desired AP in hidden mode\n");
-	        return rc;
-	     }
-       	}
-
-        if (pDevice->flags & DEVICE_FLAGS_OPENED) {
-		    pDevice->bCommit = true;
-   		}
+		}
+
+		if (pDevice->flags & DEVICE_FLAGS_OPENED) {
+			pDevice->bCommit = true;
+		}
 	}
 	return rc;
 }
@@ -709,49 +685,45 @@ if (pMgmt->eScanState ==  WMAC_IS_SCANNING) {
  */
 
 int iwctl_giwap(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct sockaddr *wrq,
-             char *extra)
+		struct iw_request_info *info,
+		struct sockaddr *wrq,
+		char *extra)
 {
 	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
+	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
 
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWAP \n");
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWAP \n");
+	memcpy(wrq->sa_data, pMgmt->abyCurrBSSID, 6);
+	//2008-0410,<Modify> by Einsn Liu
+	if ((pDevice->bLinkPass == false) && (pMgmt->eCurrMode != WMAC_MODE_ESS_AP))
+		memset(wrq->sa_data, 0, 6);
 
-    memcpy(wrq->sa_data, pMgmt->abyCurrBSSID, 6);
-   //2008-0410,<Modify> by Einsn Liu
-    if ((pDevice->bLinkPass == false) && (pMgmt->eCurrMode != WMAC_MODE_ESS_AP))
-        memset(wrq->sa_data, 0, 6);
-
-    if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-        memcpy(wrq->sa_data, pMgmt->abyCurrBSSID, 6);
-    }
+	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
+		memcpy(wrq->sa_data, pMgmt->abyCurrBSSID, 6);
+	}
 
 	wrq->sa_family = ARPHRD_ETHER;
 
 	return 0;
-
 }
 
-
 /*
  * Wireless Handler : get ap list
  */
 
 int iwctl_giwaplist(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra)
+		    struct iw_request_info *info,
+		    struct iw_point *wrq,
+		    char *extra)
 {
-	int ii,jj, rc = 0;
+	int ii, jj, rc = 0;
 	struct sockaddr sock[IW_MAX_AP];
 	struct iw_quality qual[IW_MAX_AP];
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-
+	PSDevice pDevice = (PSDevice)netdev_priv(dev);
+	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWAPLIST \n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWAPLIST \n");
 	// Only super-user can see AP list
 
 	if (!capable(CAP_NET_ADMIN)) {
@@ -760,15 +732,14 @@ int iwctl_giwaplist(struct net_device *dev,
 	}
 
 	if (wrq->pointer) {
-
 		PKnownBSS pBSS = &(pMgmt->sBSSList[0]);
 
-		for (ii = 0, jj= 0; ii < MAX_BSS_NUM; ii++) {
-		    pBSS = &(pMgmt->sBSSList[ii]);
-            if (!pBSS->bActive)
-                continue;
-            if ( jj >= IW_MAX_AP)
-                break;
+		for (ii = 0, jj = 0; ii < MAX_BSS_NUM; ii++) {
+			pBSS = &(pMgmt->sBSSList[ii]);
+			if (!pBSS->bActive)
+				continue;
+			if (jj >= IW_MAX_AP)
+				break;
 			memcpy(sock[jj].sa_data, pBSS->abyBSSID, 6);
 			sock[jj].sa_family = ARPHRD_ETHER;
 			qual[jj].level = pBSS->uRSSI;
@@ -786,155 +757,144 @@ int iwctl_giwaplist(struct net_device *dev,
 	return rc;
 }
 
-
 /*
  * Wireless Handler : set essid
  */
 
 int iwctl_siwessid(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra)
+		   struct iw_request_info *info,
+		   struct iw_point *wrq,
+		   char *extra)
 {
 	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-    PWLAN_IE_SSID       pItemSSID;
-  //2008-0409-05, <Add> by Einsn Liu
-    unsigned char len;
-
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWESSID \n");
- pDevice->fWPA_Authened = false;
-if (pMgmt->eScanState ==  WMAC_IS_SCANNING) {
-        // In scanning..
-     printk("SIOCSIWESSID(??)-->In scanning...\n");
-   //  return -EAGAIN;
-  }
+	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
+	PWLAN_IE_SSID       pItemSSID;
+	//2008-0409-05, <Add> by Einsn Liu
+	unsigned char len;
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWESSID \n");
+	pDevice->fWPA_Authened = false;
+	if (pMgmt->eScanState ==  WMAC_IS_SCANNING) {
+		// In scanning..
+		printk("SIOCSIWESSID(??)-->In scanning...\n");
+		//  return -EAGAIN;
+	}
 	// Check if we asked for `any'
-	if(wrq->flags == 0) {
+	if (wrq->flags == 0) {
 		// Just send an empty SSID list
 		memset(pMgmt->abyDesireSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-                  memset(pMgmt->abyDesireBSSID, 0xFF,6);
-	    PRINT_K("set essid to 'any' \n");
-           #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-             //Unknown desired AP,so here need not associate??
-            //if(pDevice->bWPASuppWextEnabled == true)  {
-                  return 0;
-            // }
-            #endif
+		memset(pMgmt->abyDesireBSSID, 0xFF, 6);
+		PRINT_K("set essid to 'any' \n");
+#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
+		return 0;
+#endif
 	} else {
 		// Set the SSID
 		memset(pMgmt->abyDesireSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-        pItemSSID = (PWLAN_IE_SSID)pMgmt->abyDesireSSID;
-        pItemSSID->byElementID = WLAN_EID_SSID;
+		pItemSSID = (PWLAN_IE_SSID)pMgmt->abyDesireSSID;
+		pItemSSID->byElementID = WLAN_EID_SSID;
 
 		memcpy(pItemSSID->abySSID, extra, wrq->length);
-	 if (pItemSSID->abySSID[wrq->length - 1] == '\0') {
-           if(wrq->length>0)
-		pItemSSID->len = wrq->length - 1;
-         }
-	else
-	  pItemSSID->len = wrq->length;
-	printk("set essid to %s \n",pItemSSID->abySSID);
+		if (pItemSSID->abySSID[wrq->length - 1] == '\0') {
+			if (wrq->length > 0)
+				pItemSSID->len = wrq->length - 1;
+		} else
+			pItemSSID->len = wrq->length;
+		printk("set essid to %s \n", pItemSSID->abySSID);
 		//2008-0409-05, <Add> by Einsn Liu
-       len=(pItemSSID->len > ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len)?pItemSSID->len:((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len;
-   if((pDevice->bLinkPass == true) &&
-  	(memcmp(pItemSSID->abySSID,((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->abySSID,len)==0))
-         return 0;
-
-     //mike:need clear desiredBSSID
-     if(pItemSSID->len==0) {
-        memset(pMgmt->abyDesireBSSID, 0xFF,6);
-        return 0;
-     }
+		len = (pItemSSID->len > ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len) ? pItemSSID->len : ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len;
+		if ((pDevice->bLinkPass == true) &&
+		    (memcmp(pItemSSID->abySSID, ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->abySSID, len) == 0))
+			return 0;
+
+		//mike:need clear desiredBSSID
+		if (pItemSSID->len == 0) {
+			memset(pMgmt->abyDesireBSSID, 0xFF, 6);
+			return 0;
+		}
 
 #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
- //Wext wil order another command of siwap to link with desired AP,
- //so here need not associate??
-  if(pDevice->bWPASuppWextEnabled == true)  {
-        /*******search if  in hidden ssid mode ****/
-        {
-           PKnownBSS       pCurr = NULL;
-           unsigned char abyTmpDesireSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
-	  unsigned int ii , uSameBssidNum=0;
-
-	  memcpy(abyTmpDesireSSID,pMgmt->abyDesireSSID,sizeof(abyTmpDesireSSID));
-            pCurr = BSSpSearchBSSList(pDevice,
-                                      NULL,
-                                      abyTmpDesireSSID,
-                                      pMgmt->eConfigPHYMode
-                                      );
-
-            if (pCurr == NULL){
-               PRINT_K("SIOCSIWESSID:hidden ssid site survey before associate.......\n");
-	      vResetCommandTimer((void *) pDevice);
-	      pMgmt->eScanType = WMAC_SCAN_ACTIVE;
-               bScheduleCommand((void *) pDevice, WLAN_CMD_BSSID_SCAN, pMgmt->abyDesireSSID);
-	      bScheduleCommand((void *) pDevice, WLAN_CMD_SSID, pMgmt->abyDesireSSID);
-          }
-	 else {  //mike:to find out if that desired SSID is a hidden-ssid AP ,
-                     //         by means of judging if there are two same BSSID exist in list ?
-                  for (ii = 0; ii < MAX_BSS_NUM; ii++) {
-                     if (pMgmt->sBSSList[ii].bActive &&
-                        !compare_ether_addr(pMgmt->sBSSList[ii].abyBSSID, pCurr->abyBSSID)) {
-                        uSameBssidNum++;
-                     }
-                  }
-	     if(uSameBssidNum >= 2) {  //hit: desired AP is in hidden ssid mode!!!
-                 printk("SIOCSIWESSID:hidden ssid directly associate.......\n");
-	        vResetCommandTimer((void *) pDevice);
-	        pMgmt->eScanType = WMAC_SCAN_PASSIVE;          //this scan type,you'll submit scan result!
-	        bScheduleCommand((void *) pDevice, WLAN_CMD_BSSID_SCAN, pMgmt->abyDesireSSID);
-	        bScheduleCommand((void *) pDevice, WLAN_CMD_SSID, pMgmt->abyDesireSSID);
-	     }
-	 }
-        }
-     return 0;
-  }
-	     #endif
+		//Wext wil order another command of siwap to link with desired AP,
+		//so here need not associate??
+		if (pDevice->bWPASuppWextEnabled == true)  {
+			/*******search if  in hidden ssid mode ****/
+			{
+				PKnownBSS       pCurr = NULL;
+				unsigned char abyTmpDesireSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
+				unsigned int ii, uSameBssidNum = 0;
+
+				memcpy(abyTmpDesireSSID, pMgmt->abyDesireSSID, sizeof(abyTmpDesireSSID));
+				pCurr = BSSpSearchBSSList(pDevice,
+							  NULL,
+							  abyTmpDesireSSID,
+							  pMgmt->eConfigPHYMode
+);
+
+				if (pCurr == NULL) {
+					PRINT_K("SIOCSIWESSID:hidden ssid site survey before associate.......\n");
+					vResetCommandTimer((void *)pDevice);
+					pMgmt->eScanType = WMAC_SCAN_ACTIVE;
+					bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, pMgmt->abyDesireSSID);
+					bScheduleCommand((void *)pDevice, WLAN_CMD_SSID, pMgmt->abyDesireSSID);
+				} else {  //mike:to find out if that desired SSID is a hidden-ssid AP ,
+					//         by means of judging if there are two same BSSID exist in list ?
+					for (ii = 0; ii < MAX_BSS_NUM; ii++) {
+						if (pMgmt->sBSSList[ii].bActive &&
+						    !compare_ether_addr(pMgmt->sBSSList[ii].abyBSSID, pCurr->abyBSSID)) {
+							uSameBssidNum++;
+						}
+					}
+					if (uSameBssidNum >= 2) {  //hit: desired AP is in hidden ssid mode!!!
+						printk("SIOCSIWESSID:hidden ssid directly associate.......\n");
+						vResetCommandTimer((void *)pDevice);
+						pMgmt->eScanType = WMAC_SCAN_PASSIVE;          //this scan type,you'll submit scan result!
+						bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, pMgmt->abyDesireSSID);
+						bScheduleCommand((void *)pDevice, WLAN_CMD_SSID, pMgmt->abyDesireSSID);
+					}
+				}
+			}
+			return 0;
+		}
+#endif
 
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set essid = %s \n", pItemSSID->abySSID);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set essid = %s \n", pItemSSID->abySSID);
 	}
 
-    if (pDevice->flags & DEVICE_FLAGS_OPENED) {
-	    pDevice->bCommit = true;
+	if (pDevice->flags & DEVICE_FLAGS_OPENED) {
+		pDevice->bCommit = true;
 	}
 
-
 	return 0;
 }
 
-
 /*
  * Wireless Handler : get essid
  */
 
 int iwctl_giwessid(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra)
+		   struct iw_request_info *info,
+		   struct iw_point *wrq,
+		   char *extra)
 {
-
 	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
+	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
 	PWLAN_IE_SSID       pItemSSID;
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWESSID \n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWESSID \n");
 
 	// Note : if wrq->u.data.flags != 0, we should
 	// get the relevant SSID from the SSID list...
 
 	// Get the current SSID
-    pItemSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
+	pItemSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
 	//pItemSSID = (PWLAN_IE_SSID)pMgmt->abyDesireSSID;
 	memcpy(extra, pItemSSID->abySSID , pItemSSID->len);
 	extra[pItemSSID->len] = '\0';
 	wrq->length = pItemSSID->len + 1;
-	        //2008-0409-03, <Add> by Einsn Liu
-        wrq->length = pItemSSID->len;
+	//2008-0409-03, <Add> by Einsn Liu
+	wrq->length = pItemSSID->len;
 	wrq->flags = 1; // active
 
-
 	return 0;
 }
 
@@ -943,28 +903,27 @@ int iwctl_giwessid(struct net_device *dev,
  */
 
 int iwctl_siwrate(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra)
+		  struct iw_request_info *info,
+		  struct iw_param *wrq,
+		  char *extra)
 {
 	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    int rc = 0;
+	int rc = 0;
 	u8	brate = 0;
 	int	i;
-	unsigned char abySupportedRates[13]= {0x02, 0x04, 0x0B, 0x16, 0x0c, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C, 0x90};
-
+	unsigned char abySupportedRates[13] = {0x02, 0x04, 0x0B, 0x16, 0x0c, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C, 0x90};
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWRATE \n");
-    if (!(pDevice->flags & DEVICE_FLAGS_OPENED)) {
-        rc = -EINVAL;
-        return rc;
-    }
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWRATE \n");
+	if (!(pDevice->flags & DEVICE_FLAGS_OPENED)) {
+		rc = -EINVAL;
+		return rc;
+	}
 
 	// First : get a valid bit rate value
 
 	// Which type of value
-	if((wrq->value < 13) &&
-	   (wrq->value >= 0)) {
+	if ((wrq->value < 13) &&
+	    (wrq->value >= 0)) {
 		// Setting by rate index
 		// Find value in the magic rate table
 		brate = wrq->value;
@@ -973,51 +932,49 @@ int iwctl_siwrate(struct net_device *dev,
 		u8	normvalue = (u8) (wrq->value/500000);
 
 		// Check if rate is valid
-		for(i = 0 ; i < 13 ; i++) {
-			if(normvalue == abySupportedRates[i]) {
+		for (i = 0; i < 13; i++) {
+			if (normvalue == abySupportedRates[i]) {
 				brate = i;
 				break;
 			}
 		}
 	}
 	// -1 designed the max rate (mostly auto mode)
-	if(wrq->value == -1) {
+	if (wrq->value == -1) {
 		// Get the highest available rate
-		for(i = 0 ; i < 13 ; i++) {
-			if(abySupportedRates[i] == 0)
+		for (i = 0; i < 13; i++) {
+			if (abySupportedRates[i] == 0)
 				break;
 		}
-		if(i != 0)
+		if (i != 0)
 			brate = i - 1;
 
 	}
 	// Check that it is valid
 	// brate is index of abySupportedRates[]
-	if(brate > 13 ) {
+	if (brate > 13) {
 		rc = -EINVAL;
 		return rc;
 	}
 
 	// Now, check if we want a fixed or auto value
-	if(wrq->fixed != 0) {
+	if (wrq->fixed != 0) {
 		// Fixed mode
 		// One rate, fixed
-	printk("Rate Fix\n");
+		printk("Rate Fix\n");
 		pDevice->bFixRate = true;
-        if ((pDevice->byBBType == BB_TYPE_11B)&& (brate > 3)) {
-	    pDevice->uConnectionRate = 3;
-        }
-        else {
-            pDevice->uConnectionRate = brate;
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Fixed to Rate %d \n", pDevice->uConnectionRate);
-        }
+		if ((pDevice->byBBType == BB_TYPE_11B) && (brate > 3)) {
+			pDevice->uConnectionRate = 3;
+		} else {
+			pDevice->uConnectionRate = brate;
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Fixed to Rate %d \n", pDevice->uConnectionRate);
+		}
 
+	} else {
+		pDevice->bFixRate = false;
+		pDevice->uConnectionRate = 13;
+		printk("auto rate:connection_rate is 13\n");
 	}
-	else {
-        pDevice->bFixRate = false;
-        pDevice->uConnectionRate = 13;
-	printk("auto rate:connection_rate is 13\n");
-     }
 
 	return rc;
 }
@@ -1027,91 +984,84 @@ int iwctl_siwrate(struct net_device *dev,
  */
 
 int iwctl_giwrate(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_param *wrq,
-             char *extra)
+		  struct iw_request_info *info,
+		  struct iw_param *wrq,
+		  char *extra)
 {
 	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
 //2007-0118-05,<Mark> by EinsnLiu
 //Mark the unnecessary sentences.
 //    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWRATE \n");
-    {
-        unsigned char abySupportedRates[13]= {0x02, 0x04, 0x0B, 0x16, 0x0c, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C, 0x90};
-	    int brate = 0;
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWRATE \n");
+	{
+		unsigned char abySupportedRates[13] = {0x02, 0x04, 0x0B, 0x16, 0x0c, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C, 0x90};
+		int brate = 0;
 //2008-5-8 <modify> by chester
-if(pDevice->bLinkPass){
-if(pDevice->bFixRate == true){
-		if (pDevice->uConnectionRate < 13) {
-	        brate = abySupportedRates[pDevice->uConnectionRate];
-	    }else {
-            if (pDevice->byBBType == BB_TYPE_11B)
-	            brate = 0x16;
-            if (pDevice->byBBType == BB_TYPE_11G)
-	            brate = 0x6C;
-            if (pDevice->byBBType == BB_TYPE_11A)
-	            brate = 0x6C;
-	    }
-}
-else
-{
-
- brate = abySupportedRates[TxRate_iwconfig];
-}
-}
-else brate =0;
+		if (pDevice->bLinkPass) {
+			if (pDevice->bFixRate == true) {
+				if (pDevice->uConnectionRate < 13) {
+					brate = abySupportedRates[pDevice->uConnectionRate];
+				} else {
+					if (pDevice->byBBType == BB_TYPE_11B)
+						brate = 0x16;
+					if (pDevice->byBBType == BB_TYPE_11G)
+						brate = 0x6C;
+					if (pDevice->byBBType == BB_TYPE_11A)
+						brate = 0x6C;
+				}
+			} else {
+				brate = abySupportedRates[TxRate_iwconfig];
+			}
+		} else brate = 0;
 //2007-0118-05,<Mark> by EinsnLiu
 //Mark the unnecessary sentences.
 /*
-	    if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-            if (pDevice->byBBType == BB_TYPE_11B)
-	            brate = 0x16;
-            if (pDevice->byBBType == BB_TYPE_11G)
-	            brate = 0x6C;
-            if (pDevice->byBBType == BB_TYPE_11A)
-	            brate = 0x6C;
-	    }
+  if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
+  if (pDevice->byBBType == BB_TYPE_11B)
+  brate = 0x16;
+  if (pDevice->byBBType == BB_TYPE_11G)
+  brate = 0x6C;
+  if (pDevice->byBBType == BB_TYPE_11A)
+  brate = 0x6C;
+  }
 */
 
-//    		if (pDevice->uConnectionRate == 13)
+//		if (pDevice->uConnectionRate == 13)
 //                brate = abySupportedRates[pDevice->wCurrentRate];
-	    wrq->value = brate * 500000;
-	    // If more than one rate, set auto
-	    if (pDevice->bFixRate == true)
-	        wrq->fixed = true;
-    }
-
+		wrq->value = brate * 500000;
+		// If more than one rate, set auto
+		if (pDevice->bFixRate == true)
+			wrq->fixed = true;
+	}
 
 	return 0;
 }
 
-
-
 /*
  * Wireless Handler : set rts threshold
  */
 
 int iwctl_siwrts(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra)
+		 struct iw_request_info *info,
+		 struct iw_param *wrq,
+		 char *extra)
 {
 	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
 	int rc = 0;
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWRTS \n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWRTS \n");
 
 	{
-	    int rthr = wrq->value;
-	    if(wrq->disabled)
+		int rthr = wrq->value;
+		if (wrq->disabled)
 			rthr = 2312;
-	    if((rthr < 0) || (rthr > 2312)) {
+		if ((rthr < 0) || (rthr > 2312)) {
 			rc = -EINVAL;
-    	}else {
-		    pDevice->wRTSThreshold = rthr;
-	    }
-    }
+		} else {
+			pDevice->wRTSThreshold = rthr;
+		}
+	}
 
 	return 0;
 }
@@ -1121,13 +1071,13 @@ int iwctl_siwrts(struct net_device *dev,
  */
 
 int iwctl_giwrts(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra)
+		 struct iw_request_info *info,
+		 struct iw_param *wrq,
+		 char *extra)
 {
 	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWRTS \n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWRTS \n");
 	wrq->value = pDevice->wRTSThreshold;
 	wrq->disabled = (wrq->value >= 2312);
 	wrq->fixed = 1;
@@ -1140,26 +1090,24 @@ int iwctl_giwrts(struct net_device *dev,
  */
 
 int iwctl_siwfrag(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra)
+		  struct iw_request_info *info,
+		  struct iw_param *wrq,
+		  char *extra)
 {
-    PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    int rc = 0;
-    int fthr = wrq->value;
-
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWFRAG \n");
+	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	int rc = 0;
+	int fthr = wrq->value;
 
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWFRAG \n");
 
-    if (wrq->disabled)
+	if (wrq->disabled)
 		fthr = 2312;
-    if((fthr < 256) || (fthr > 2312)) {
+	if ((fthr < 256) || (fthr > 2312)) {
 		rc = -EINVAL;
-    }else {
-		 fthr &= ~0x1;	// Get an even value
-	     pDevice->wFragmentationThreshold = (u16)fthr;
-    }
+	} else {
+		fthr &= ~0x1;	// Get an even value
+		pDevice->wFragmentationThreshold = (u16)fthr;
+	}
 
 	return rc;
 }
@@ -1169,13 +1117,13 @@ int iwctl_siwfrag(struct net_device *dev,
  */
 
 int iwctl_giwfrag(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra)
+		  struct iw_request_info *info,
+		  struct iw_param *wrq,
+		  char *extra)
 {
-    PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWFRAG \n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWFRAG \n");
 	wrq->value = pDevice->wFragmentationThreshold;
 	wrq->disabled = (wrq->value >= 2312);
 	wrq->fixed = 1;
@@ -1183,21 +1131,18 @@ int iwctl_giwfrag(struct net_device *dev,
 	return 0;
 }
 
-
-
 /*
  * Wireless Handler : set retry threshold
  */
 int iwctl_siwretry(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra)
+		   struct iw_request_info *info,
+		   struct iw_param *wrq,
+		   char *extra)
 {
-    PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    int rc = 0;
-
+	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	int rc = 0;
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWRETRY \n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWRETRY \n");
 
 	if (wrq->disabled) {
 		rc = -EINVAL;
@@ -1205,7 +1150,7 @@ int iwctl_siwretry(struct net_device *dev,
 	}
 
 	if (wrq->flags & IW_RETRY_LIMIT) {
-		if(wrq->flags & IW_RETRY_MAX)
+		if (wrq->flags & IW_RETRY_MAX)
 			pDevice->byLongRetryLimit = wrq->value;
 		else if (wrq->flags & IW_RETRY_MIN)
 			pDevice->byShortRetryLimit = wrq->value;
@@ -1219,7 +1164,6 @@ int iwctl_siwretry(struct net_device *dev,
 		pDevice->wMaxTransmitMSDULifetime = wrq->value;
 	}
 
-
 	return rc;
 }
 
@@ -1227,45 +1171,43 @@ int iwctl_siwretry(struct net_device *dev,
  * Wireless Handler : get retry threshold
  */
 int iwctl_giwretry(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra)
+		   struct iw_request_info *info,
+		   struct iw_param *wrq,
+		   char *extra)
 {
-    PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWRETRY \n");
+	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWRETRY \n");
 	wrq->disabled = 0;      // Can't be disabled
 
 	// Note : by default, display the min retry number
-	if((wrq->flags & IW_RETRY_TYPE) == IW_RETRY_LIFETIME) {
+	if ((wrq->flags & IW_RETRY_TYPE) == IW_RETRY_LIFETIME) {
 		wrq->flags = IW_RETRY_LIFETIME;
 		wrq->value = (int)pDevice->wMaxTransmitMSDULifetime; //ms
-	} else if((wrq->flags & IW_RETRY_MAX)) {
+	} else if ((wrq->flags & IW_RETRY_MAX)) {
 		wrq->flags = IW_RETRY_LIMIT | IW_RETRY_MAX;
 		wrq->value = (int)pDevice->byLongRetryLimit;
 	} else {
 		wrq->flags = IW_RETRY_LIMIT;
 		wrq->value = (int)pDevice->byShortRetryLimit;
-		if((int)pDevice->byShortRetryLimit != (int)pDevice->byLongRetryLimit)
+		if ((int)pDevice->byShortRetryLimit != (int)pDevice->byLongRetryLimit)
 			wrq->flags |= IW_RETRY_MIN;
 	}
 
-
 	return 0;
 }
 
-
 /*
  * Wireless Handler : set encode mode
  */
 int iwctl_siwencode(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra)
+		    struct iw_request_info *info,
+		    struct iw_point *wrq,
+		    char *extra)
 {
-    PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
+	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
 	unsigned long dwKeyIndex = (unsigned long)(wrq->flags & IW_ENCODE_INDEX);
-	int ii,uu, rc = 0;
+	int ii, uu, rc = 0;
 	int index = (wrq->flags & IW_ENCODE_INDEX);
 
 //2007-0207-07,<Modify> by EinsnLiu
@@ -1281,192 +1223,182 @@ int iwctl_siwencode(struct net_device *dev,
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWENCODE \n");
 
-if((wrq->flags & IW_ENCODE_DISABLED)==0){
-	//Not disable encryption
+	if ((wrq->flags & IW_ENCODE_DISABLED) == 0) {
+		//Not disable encryption
 
-	if (dwKeyIndex > WLAN_WEP_NKEYS) {
-		rc = -EINVAL;
-        return rc;
-    	}
+		if (dwKeyIndex > WLAN_WEP_NKEYS) {
+			rc = -EINVAL;
+			return rc;
+		}
 
-	if(dwKeyIndex<1&&((wrq->flags&IW_ENCODE_NOKEY)==0)){//set default key
-		if(pDevice->byKeyIndex<WLAN_WEP_NKEYS){
-			dwKeyIndex=pDevice->byKeyIndex;
+		if (dwKeyIndex < 1 && ((wrq->flags & IW_ENCODE_NOKEY) == 0)) {//set default key
+			if (pDevice->byKeyIndex < WLAN_WEP_NKEYS) {
+				dwKeyIndex = pDevice->byKeyIndex;
+			} else dwKeyIndex = 0;
+		} else dwKeyIndex--;
+
+		// Check the size of the key
+		if (wrq->length > WLAN_WEP232_KEYLEN) {
+			rc = -EINVAL;
+			return rc;
+		}
+
+		if (wrq->length > 0) {//have key
+
+			if (wrq->length ==  WLAN_WEP232_KEYLEN) {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set 232 bit wep key\n");
+			} else if (wrq->length ==  WLAN_WEP104_KEYLEN) {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set 104 bit wep key\n");
+			} else if (wrq->length == WLAN_WEP40_KEYLEN) {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set 40 bit wep key, index= %d\n", (int)dwKeyIndex);
+			} else {//no support length
+				rc = -EINVAL;
+				return rc;
 			}
-		else dwKeyIndex=0;
-		}else dwKeyIndex--;
+			memset(pDevice->abyKey, 0, WLAN_WEP232_KEYLEN);
+			memcpy(pDevice->abyKey, extra, wrq->length);
 
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "abyKey: ");
+			for (ii = 0; ii < wrq->length; ii++) {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", pDevice->abyKey[ii]);
+			}
 
-	// Check the size of the key
-	if (wrq->length > WLAN_WEP232_KEYLEN) {
-		rc = -EINVAL;
-        return rc;
-	}
+			if (pDevice->flags & DEVICE_FLAGS_OPENED) {
+				spin_lock_irq(&pDevice->lock);
+				KeybSetDefaultKey(&(pDevice->sKey),
+						  (unsigned long)(dwKeyIndex | (1 << 31)),
+						  wrq->length,
+						  NULL,
+						  pDevice->abyKey,
+						  KEY_CTL_WEP,
+						  pDevice->PortOffset,
+						  pDevice->byLocalID
+);
+				spin_unlock_irq(&pDevice->lock);
+			}
+			pDevice->byKeyIndex = (unsigned char)dwKeyIndex;
+			pDevice->uKeyLength = wrq->length;
+			pDevice->bTransmitKey = true;
+			pDevice->bEncryptionEnable = true;
+			pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
 
-	if(wrq->length>0){//have key
-
-        if (wrq->length ==  WLAN_WEP232_KEYLEN) {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set 232 bit wep key\n");
-        }
-        else if (wrq->length ==  WLAN_WEP104_KEYLEN) {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set 104 bit wep key\n");
-        }
-        else if (wrq->length == WLAN_WEP40_KEYLEN) {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set 40 bit wep key, index= %d\n", (int)dwKeyIndex);
-        }else {//no support length
-		rc = -EINVAL;
-        return rc;
+		} else if (index > 0) {
+			//when the length is 0 the request only changes the default transmit key index
+			//check the new key if it has a non zero length
+			if (pDevice->bEncryptionEnable == false) {
+				rc = -EINVAL;
+				return rc;
+			}
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Just set Default key Index:\n");
+			pkeytab = &(pDevice->sKey.KeyTable[MAX_KEY_TABLE - 1]);
+			if (pkeytab->GroupKey[(unsigned char)dwKeyIndex].uKeyLength == 0) {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Default key len is 0\n");
+				rc = -EINVAL;
+				return rc;
+			}
+			pDevice->byKeyIndex = (unsigned char)dwKeyIndex;
+			pkeytab->dwGTKeyIndex = dwKeyIndex | (1 << 31);
+			pkeytab->GroupKey[(unsigned char)dwKeyIndex].dwKeyIndex = dwKeyIndex | (1 << 31);
 		}
-        memset(pDevice->abyKey, 0, WLAN_WEP232_KEYLEN);
-        memcpy(pDevice->abyKey, extra, wrq->length);
-
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"abyKey: ");
-        for (ii = 0; ii < wrq->length; ii++) {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", pDevice->abyKey[ii]);
-        }
-
-        if (pDevice->flags & DEVICE_FLAGS_OPENED) {
-            spin_lock_irq(&pDevice->lock);
-            KeybSetDefaultKey(&(pDevice->sKey),
-                            (unsigned long)(dwKeyIndex | (1 << 31)),
-                           	wrq->length,
-                            NULL,
-                            pDevice->abyKey,
-                            KEY_CTL_WEP,
-                            pDevice->PortOffset,
-                            pDevice->byLocalID
-                          );
-            spin_unlock_irq(&pDevice->lock);
-        }
-        pDevice->byKeyIndex = (unsigned char)dwKeyIndex;
-        pDevice->uKeyLength = wrq->length;
-        pDevice->bTransmitKey = true;
-        pDevice->bEncryptionEnable = true;
-        pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
-
-		}else if(index>0){
-	//when the length is 0 the request only changes the default transmit key index
-	//check the new key if it has a non zero length
-	if(pDevice->bEncryptionEnable==false)
-	{
-		rc = -EINVAL;
-        	return rc;
-	}
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Just set Default key Index:\n");
-	pkeytab=&(pDevice->sKey.KeyTable[MAX_KEY_TABLE-1]);
-	if(pkeytab->GroupKey[(unsigned char)dwKeyIndex].uKeyLength==0){
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Default key len is 0\n");
-		rc = -EINVAL;
-      	  	return rc;
+
+	} else {//disable the key
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Disable WEP function\n");
+		if (pDevice->bEncryptionEnable == false)
+			return 0;
+		pMgmt->bShareKeyAlgorithm = false;
+		pDevice->bEncryptionEnable = false;
+		pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
+		if (pDevice->flags & DEVICE_FLAGS_OPENED) {
+			spin_lock_irq(&pDevice->lock);
+			for (uu = 0; uu < MAX_KEY_TABLE; uu++)
+				MACvDisableKeyEntry(pDevice->PortOffset, uu);
+			spin_unlock_irq(&pDevice->lock);
 		}
-	 pDevice->byKeyIndex =(unsigned char)dwKeyIndex;
-	 pkeytab->dwGTKeyIndex =dwKeyIndex | (1 << 31);
-	 pkeytab->GroupKey[(unsigned char)dwKeyIndex].dwKeyIndex=dwKeyIndex | (1 << 31);
 	}
-
-}else {//disable the key
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Disable WEP function\n");
-	if(pDevice->bEncryptionEnable==false)
-		return 0;
-	pMgmt->bShareKeyAlgorithm = false;
-        pDevice->bEncryptionEnable = false;
-        pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
-        if (pDevice->flags & DEVICE_FLAGS_OPENED) {
-            spin_lock_irq(&pDevice->lock);
-            for(uu=0;uu<MAX_KEY_TABLE;uu++)
-                MACvDisableKeyEntry(pDevice->PortOffset, uu);
-            spin_unlock_irq(&pDevice->lock);
-        }
-}
 //End Modify,Einsn
 
 /*
-     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWENCODE \n");
+  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWENCODE \n");
 
-	// Check the size of the key
-	if (wrq->length > WLAN_WEP232_KEYLEN) {
-		rc = -EINVAL;
-        return rc;
-	}
+  // Check the size of the key
+  if (wrq->length > WLAN_WEP232_KEYLEN) {
+  rc = -EINVAL;
+  return rc;
+  }
 
-	if (dwKeyIndex > WLAN_WEP_NKEYS) {
-		rc = -EINVAL;
-        return rc;
-    }
-
-    if (dwKeyIndex > 0)
-		dwKeyIndex--;
-
-	// Send the key to the card
-	if (wrq->length > 0) {
-
-        if (wrq->length ==  WLAN_WEP232_KEYLEN) {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set 232 bit wep key\n");
-        }
-        else if (wrq->length ==  WLAN_WEP104_KEYLEN) {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set 104 bit wep key\n");
-        }
-        else if (wrq->length == WLAN_WEP40_KEYLEN) {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set 40 bit wep key, index= %d\n", (int)dwKeyIndex);
-        }
-        memset(pDevice->abyKey, 0, WLAN_WEP232_KEYLEN);
-        memcpy(pDevice->abyKey, extra, wrq->length);
-
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"abyKey: ");
-        for (ii = 0; ii < wrq->length; ii++) {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", pDevice->abyKey[ii]);
-        }
-
-        if (pDevice->flags & DEVICE_FLAGS_OPENED) {
-            spin_lock_irq(&pDevice->lock);
-            KeybSetDefaultKey(&(pDevice->sKey),
-                            (unsigned long)(pDevice->byKeyIndex | (1 << 31)),
-                            pDevice->uKeyLength,
-                            NULL,
-                            pDevice->abyKey,
-                            KEY_CTL_WEP,
-                            pDevice->PortOffset,
-                            pDevice->byLocalID
-                          );
-            spin_unlock_irq(&pDevice->lock);
-        }
-        pDevice->byKeyIndex = (unsigned char)dwKeyIndex;
-        pDevice->uKeyLength = wrq->length;
-        pDevice->bTransmitKey = true;
-        pDevice->bEncryptionEnable = true;
-        pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
-
-		// Do we want to just set the transmit key index ?
-		if ( index < 4 ) {
-		    pDevice->byKeyIndex = index;
-		}
-		else if(!(wrq->flags & IW_ENCODE_MODE)) {
-				rc = -EINVAL;
-				return rc;
-	    }
-	}
-	// Read the flags
-	if(wrq->flags & IW_ENCODE_DISABLED){
+  if (dwKeyIndex > WLAN_WEP_NKEYS) {
+  rc = -EINVAL;
+  return rc;
+  }
 
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Disable WEP function\n");
-		pMgmt->bShareKeyAlgorithm = false;
-        pDevice->bEncryptionEnable = false;
-        pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
-        if (pDevice->flags & DEVICE_FLAGS_OPENED) {
-            spin_lock_irq(&pDevice->lock);
-            for(uu=0;uu<MAX_KEY_TABLE;uu++)
-                MACvDisableKeyEntry(pDevice->PortOffset, uu);
-            spin_unlock_irq(&pDevice->lock);
-        }
-	}
+  if (dwKeyIndex > 0)
+  dwKeyIndex--;
+
+  // Send the key to the card
+  if (wrq->length > 0) {
+  if (wrq->length ==  WLAN_WEP232_KEYLEN) {
+  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set 232 bit wep key\n");
+  } else if (wrq->length ==  WLAN_WEP104_KEYLEN) {
+  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set 104 bit wep key\n");
+  } else if (wrq->length == WLAN_WEP40_KEYLEN) {
+  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set 40 bit wep key, index= %d\n", (int)dwKeyIndex);
+  }
+  memset(pDevice->abyKey, 0, WLAN_WEP232_KEYLEN);
+  memcpy(pDevice->abyKey, extra, wrq->length);
+
+  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "abyKey: ");
+  for (ii = 0; ii < wrq->length; ii++) {
+  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", pDevice->abyKey[ii]);
+  }
+
+  if (pDevice->flags & DEVICE_FLAGS_OPENED) {
+  spin_lock_irq(&pDevice->lock);
+  KeybSetDefaultKey(&(pDevice->sKey),
+  (unsigned long)(pDevice->byKeyIndex | (1 << 31)),
+  pDevice->uKeyLength,
+  NULL,
+  pDevice->abyKey,
+  KEY_CTL_WEP,
+  pDevice->PortOffset,
+  pDevice->byLocalID
+);
+  spin_unlock_irq(&pDevice->lock);
+  }
+  pDevice->byKeyIndex = (unsigned char)dwKeyIndex;
+  pDevice->uKeyLength = wrq->length;
+  pDevice->bTransmitKey = true;
+  pDevice->bEncryptionEnable = true;
+  pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
+
+  // Do we want to just set the transmit key index ?
+  if (index < 4) {
+  pDevice->byKeyIndex = index;
+  } else if (!(wrq->flags & IW_ENCODE_MODE)) {
+  rc = -EINVAL;
+  return rc;
+  }
+  }
+  // Read the flags
+  if (wrq->flags & IW_ENCODE_DISABLED) {
+  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Disable WEP function\n");
+  pMgmt->bShareKeyAlgorithm = false;
+  pDevice->bEncryptionEnable = false;
+  pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
+  if (pDevice->flags & DEVICE_FLAGS_OPENED) {
+  spin_lock_irq(&pDevice->lock);
+  for (uu=0; uu<MAX_KEY_TABLE; uu++)
+  MACvDisableKeyEntry(pDevice->PortOffset, uu);
+  spin_unlock_irq(&pDevice->lock);
+  }
+  }
 */
 
-	if(wrq->flags & IW_ENCODE_RESTRICTED) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Enable WEP & ShareKey System\n");
+	if (wrq->flags & IW_ENCODE_RESTRICTED) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Enable WEP & ShareKey System\n");
 		pMgmt->bShareKeyAlgorithm = true;
 	}
-	if(wrq->flags & IW_ENCODE_OPEN) {
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Enable WEP & Open System\n");
+	if (wrq->flags & IW_ENCODE_OPEN) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Enable WEP & Open System\n");
 		pMgmt->bShareKeyAlgorithm = false;
 	}
 	return rc;
@@ -1475,80 +1407,78 @@ if((wrq->flags & IW_ENCODE_DISABLED)==0){
 /*
  * Wireless Handler : get encode mode
  */
- /*
-int iwctl_giwencode(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra)
-{
-    PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-    int rc = 0;
-    char abyKey[WLAN_WEP232_KEYLEN];
-	unsigned int index = (unsigned int)(wrq->flags & IW_ENCODE_INDEX);
-	PSKeyItem   pKey = NULL;
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWENCODE\n");
+/*
+  int iwctl_giwencode(struct net_device *dev,
+  struct iw_request_info *info,
+  struct iw_point *wrq,
+  char *extra) {
+  PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+  PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
+  int rc = 0;
+  char abyKey[WLAN_WEP232_KEYLEN];
+  unsigned int index = (unsigned int)(wrq->flags & IW_ENCODE_INDEX);
+  PSKeyItem   pKey = NULL;
+
+  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWENCODE\n");
 //2007-0207-06,<Add> by EinsnLiu
 //the key index in iwconfig is 1-4 when our driver is 0-3
 //so it can't be used directly.
 //if the index is 0,we should used the index set by driver.
-	if (index > WLAN_WEP_NKEYS) {
-		rc = -EINVAL;
-        return rc;
-    	}
-	if(index<1){//set default key
-		if(pDevice->byKeyIndex<WLAN_WEP_NKEYS){
-			index=pDevice->byKeyIndex;
-			}
-		else index=0;
-		}else index--;
+if (index > WLAN_WEP_NKEYS) {
+rc = -EINVAL;
+return rc;
+}
+if (index<1) {//set default key
+if (pDevice->byKeyIndex<WLAN_WEP_NKEYS) {
+index=pDevice->byKeyIndex;
+}
+else index=0;
+} else index--;
 //End Add,Einsn
 
-	memset(abyKey, 0, sizeof(abyKey));
-	// Check encryption mode
-	wrq->flags = IW_ENCODE_NOKEY;
-	// Is WEP enabled ???
-	if (pDevice->bEncryptionEnable)
-		wrq->flags |=  IW_ENCODE_ENABLED;
-    else
-		wrq->flags |=  IW_ENCODE_DISABLED;
+memset(abyKey, 0, sizeof(abyKey));
+// Check encryption mode
+wrq->flags = IW_ENCODE_NOKEY;
+// Is WEP enabled ???
+if (pDevice->bEncryptionEnable)
+wrq->flags |=  IW_ENCODE_ENABLED;
+else
+wrq->flags |=  IW_ENCODE_DISABLED;
 
-    if (pMgmt->bShareKeyAlgorithm)
-		wrq->flags |=  IW_ENCODE_RESTRICTED;
-	else
-		wrq->flags |=  IW_ENCODE_OPEN;
+if (pMgmt->bShareKeyAlgorithm)
+wrq->flags |=  IW_ENCODE_RESTRICTED;
+else
+wrq->flags |=  IW_ENCODE_OPEN;
 
-	if (KeybGetKey(&(pDevice->sKey), pDevice->abyBroadcastAddr, (unsigned char)index , &pKey)){
-        wrq->length = pKey->uKeyLength;
-        memcpy(abyKey, pKey->abyKey,  pKey->uKeyLength);
+if (KeybGetKey(&(pDevice->sKey), pDevice->abyBroadcastAddr, (unsigned char)index , &pKey)) {
+wrq->length = pKey->uKeyLength;
+memcpy(abyKey, pKey->abyKey,  pKey->uKeyLength);
 //2007-0207-06,<Modify> by EinsnLiu
 //only get key success need to  copy data
 //index should +1.
 //there is not necessary to return -EINVAL when get key failed
 //if return -EINVAL,the encryption item can't be display by the command "iwconfig".
-	wrq->flags |= index+1;
-	memcpy(extra,  abyKey, WLAN_WEP232_KEYLEN);
-    }
-
-    //else {
-    //    rc = -EINVAL;
-   //     return rc;
-  //  }
+wrq->flags |= index+1;
+memcpy(extra,  abyKey, WLAN_WEP232_KEYLEN);
+}
 
+//else {
+//    rc = -EINVAL;
+//     return rc;
+//  }
 
 //End Modify,Einsn
 
-	return 0;
+return 0;
 }
 */
 
 //2008-0409-06, <Add> by Einsn Liu
 
 int iwctl_giwencode(struct net_device *dev,
-			struct iw_request_info *info,
-			struct iw_point *wrq,
-			char *extra)
+		    struct iw_request_info *info,
+		    struct iw_point *wrq,
+		    char *extra)
 {
 	PSDevice			pDevice = (PSDevice)netdev_priv(dev);
 	PSMgmtObject		pMgmt = &(pDevice->sMgmtObj);
@@ -1562,13 +1492,13 @@ int iwctl_giwencode(struct net_device *dev,
 	if (index > WLAN_WEP_NKEYS) {
 		return	-EINVAL;
 	}
-	if(index<1){//get default key
-		if(pDevice->byKeyIndex<WLAN_WEP_NKEYS){
-			index=pDevice->byKeyIndex;
-         	} else
-                      index=0;
-	}else
-             index--;
+	if (index < 1) {//get default key
+		if (pDevice->byKeyIndex < WLAN_WEP_NKEYS) {
+			index = pDevice->byKeyIndex;
+		} else
+			index = 0;
+	} else
+		index--;
 
 	memset(abyKey, 0, WLAN_WEP232_KEYLEN);
 	// Check encryption mode
@@ -1583,18 +1513,18 @@ int iwctl_giwencode(struct net_device *dev,
 		wrq->flags |=  IW_ENCODE_RESTRICTED;
 	else
 		wrq->flags |=  IW_ENCODE_OPEN;
-		wrq->length=0;
-
-	if((index==0)&&(pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled||
-		pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled)){//get wpa pairwise  key
-			if (KeybGetKey(&(pDevice->sKey),pMgmt->abyCurrBSSID, 0xffffffff, &pKey)){
-			   wrq->length = pKey->uKeyLength;
-				  memcpy(abyKey, pKey->abyKey,	pKey->uKeyLength);
-				  memcpy(extra,  abyKey, WLAN_WEP232_KEYLEN);
-			   }
-	}else if (KeybGetKey(&(pDevice->sKey), pDevice->abyBroadcastAddr, (unsigned char)index , &pKey)){
+	wrq->length = 0;
+
+	if ((index == 0) && (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled ||
+			     pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled)) {//get wpa pairwise  key
+		if (KeybGetKey(&(pDevice->sKey), pMgmt->abyCurrBSSID, 0xffffffff, &pKey)) {
 			wrq->length = pKey->uKeyLength;
-			memcpy(abyKey, pKey->abyKey,  pKey->uKeyLength);
+			memcpy(abyKey, pKey->abyKey,	pKey->uKeyLength);
+			memcpy(extra,  abyKey, WLAN_WEP232_KEYLEN);
+		}
+	} else if (KeybGetKey(&(pDevice->sKey), pDevice->abyBroadcastAddr, (unsigned char)index , &pKey)) {
+		wrq->length = pKey->uKeyLength;
+		memcpy(abyKey, pKey->abyKey,  pKey->uKeyLength);
 		memcpy(extra,  abyKey, WLAN_WEP232_KEYLEN);
 	}
 
@@ -1603,24 +1533,23 @@ int iwctl_giwencode(struct net_device *dev,
 	return 0;
 }
 
-
 /*
  * Wireless Handler : set power mode
  */
 int iwctl_siwpower(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra)
+		   struct iw_request_info *info,
+		   struct iw_param *wrq,
+		   char *extra)
 {
-    PSDevice            pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-    int rc = 0;
+	PSDevice            pDevice = (PSDevice)netdev_priv(dev);
+	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
+	int rc = 0;
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWPOWER \n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWPOWER \n");
 
-    if (!(pDevice->flags & DEVICE_FLAGS_OPENED)) {
-		 rc = -EINVAL;
-		 return rc;
+	if (!(pDevice->flags & DEVICE_FLAGS_OPENED)) {
+		rc = -EINVAL;
+		return rc;
 	}
 
 	if (wrq->disabled) {
@@ -1629,23 +1558,23 @@ int iwctl_siwpower(struct net_device *dev,
 		return rc;
 	}
 	if ((wrq->flags & IW_POWER_TYPE) == IW_POWER_TIMEOUT) {
-         pDevice->ePSMode = WMAC_POWER_FAST;
-         PSvEnablePowerSaving((void *)pDevice, pMgmt->wListenInterval);
+		pDevice->ePSMode = WMAC_POWER_FAST;
+		PSvEnablePowerSaving((void *)pDevice, pMgmt->wListenInterval);
 
 	} else if ((wrq->flags & IW_POWER_TYPE) == IW_POWER_PERIOD) {
-	     pDevice->ePSMode = WMAC_POWER_FAST;
-         PSvEnablePowerSaving((void *)pDevice, pMgmt->wListenInterval);
+		pDevice->ePSMode = WMAC_POWER_FAST;
+		PSvEnablePowerSaving((void *)pDevice, pMgmt->wListenInterval);
 	}
 	switch (wrq->flags & IW_POWER_MODE) {
 	case IW_POWER_UNICAST_R:
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWPOWER: IW_POWER_UNICAST_R \n");
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWPOWER: IW_POWER_UNICAST_R \n");
 		rc = -EINVAL;
 		break;
 	case IW_POWER_ALL_R:
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWPOWER: IW_POWER_ALL_R \n");
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWPOWER: IW_POWER_ALL_R \n");
 		rc = -EINVAL;
 	case IW_POWER_ON:
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWPOWER: IW_POWER_ON \n");
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWPOWER: IW_POWER_ON \n");
 		break;
 	default:
 		rc = -EINVAL;
@@ -1658,21 +1587,19 @@ int iwctl_siwpower(struct net_device *dev,
  * Wireless Handler : get power mode
  */
 int iwctl_giwpower(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra)
+		   struct iw_request_info *info,
+		   struct iw_param *wrq,
+		   char *extra)
 {
-    PSDevice            pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-    int mode = pDevice->ePSMode;
-
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWPOWER \n");
+	PSDevice            pDevice = (PSDevice)netdev_priv(dev);
+	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
+	int mode = pDevice->ePSMode;
 
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWPOWER \n");
 
 	wrq->disabled = (mode == WMAC_POWER_CAM);
 	if (wrq->disabled)
-	    return 0;
+		return 0;
 
 	if ((wrq->flags & IW_POWER_TYPE) == IW_POWER_TIMEOUT) {
 		wrq->value = (int)((pMgmt->wListenInterval * pMgmt->wCurrBeaconPeriod) << 10);
@@ -1686,30 +1613,27 @@ int iwctl_giwpower(struct net_device *dev,
 	return 0;
 }
 
-
 /*
  * Wireless Handler : get Sensitivity
  */
 int iwctl_giwsens(struct net_device *dev,
-			 struct iw_request_info *info,
-			 struct iw_param *wrq,
-			 char *extra)
+		  struct iw_request_info *info,
+		  struct iw_param *wrq,
+		  char *extra)
 {
-    PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    long ldBm;
+	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	long ldBm;
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWSENS \n");
-    if (pDevice->bLinkPass == true) {
-        RFvRSSITodBm(pDevice, (unsigned char)(pDevice->uCurrRSSI), &ldBm);
-	    wrq->value = ldBm;
-	}
-	else {
-	    wrq->value = 0;
-    };
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWSENS \n");
+	if (pDevice->bLinkPass == true) {
+		RFvRSSITodBm(pDevice, (unsigned char)(pDevice->uCurrRSSI), &ldBm);
+		wrq->value = ldBm;
+	} else {
+		wrq->value = 0;
+	};
 	wrq->disabled = (wrq->value == 0);
 	wrq->fixed = 1;
 
-
 	return 0;
 }
 
@@ -1717,66 +1641,64 @@ int iwctl_giwsens(struct net_device *dev,
 #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
 
 int iwctl_siwauth(struct net_device *dev,
-			  struct iw_request_info *info,
-			  struct iw_param *wrq,
-			  char *extra)
+		  struct iw_request_info *info,
+		  struct iw_param *wrq,
+		  char *extra)
 {
 	PSDevice			pDevice = (PSDevice)netdev_priv(dev);
 	PSMgmtObject	pMgmt = &(pDevice->sMgmtObj);
-	int ret=0;
-	static int wpa_version=0;  //must be static to save the last value,einsn liu
-	static int pairwise=0;
+	int ret = 0;
+	static int wpa_version = 0;  //must be static to save the last value,einsn liu
+	static int pairwise = 0;
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWAUTH \n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWAUTH \n");
 	switch (wrq->flags & IW_AUTH_INDEX) {
 	case IW_AUTH_WPA_VERSION:
 		wpa_version = wrq->value;
-		if(wrq->value == IW_AUTH_WPA_VERSION_DISABLED) {
-		       PRINT_K("iwctl_siwauth:set WPADEV to disable at 1??????\n");
+		if (wrq->value == IW_AUTH_WPA_VERSION_DISABLED) {
+			PRINT_K("iwctl_siwauth:set WPADEV to disable at 1??????\n");
 			//pDevice->bWPADevEnable = false;
-		}
-		else if(wrq->value == IW_AUTH_WPA_VERSION_WPA) {
-                          PRINT_K("iwctl_siwauth:set WPADEV to WPA1******\n");
-		}
-		else {
-                          PRINT_K("iwctl_siwauth:set WPADEV to WPA2******\n");
+		} else if (wrq->value == IW_AUTH_WPA_VERSION_WPA) {
+			PRINT_K("iwctl_siwauth:set WPADEV to WPA1******\n");
+		} else {
+			PRINT_K("iwctl_siwauth:set WPADEV to WPA2******\n");
 		}
 		//pDevice->bWPASuppWextEnabled =true;
 		break;
 	case IW_AUTH_CIPHER_PAIRWISE:
 		pairwise = wrq->value;
-		if(pairwise == IW_AUTH_CIPHER_CCMP){
+		if (pairwise == IW_AUTH_CIPHER_CCMP) {
 			pDevice->eEncryptionStatus = Ndis802_11Encryption3Enabled;
-		}else if(pairwise == IW_AUTH_CIPHER_TKIP){
+		} else if (pairwise == IW_AUTH_CIPHER_TKIP) {
 			pDevice->eEncryptionStatus = Ndis802_11Encryption2Enabled;
-		}else if(pairwise == IW_AUTH_CIPHER_WEP40||pairwise == IW_AUTH_CIPHER_WEP104){
+		} else if (pairwise == IW_AUTH_CIPHER_WEP40 || pairwise == IW_AUTH_CIPHER_WEP104) {
 			pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
-		}else if(pairwise == IW_AUTH_CIPHER_NONE){
+		} else if (pairwise == IW_AUTH_CIPHER_NONE) {
 			//do nothing,einsn liu
-		}else pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
+		} else pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
 
 		break;
 	case IW_AUTH_CIPHER_GROUP:
-		if(wpa_version == IW_AUTH_WPA_VERSION_DISABLED)
+		if (wpa_version == IW_AUTH_WPA_VERSION_DISABLED)
 			break;
-		if(pairwise == IW_AUTH_CIPHER_NONE){
-			if(wrq->value == IW_AUTH_CIPHER_CCMP){
+		if (pairwise == IW_AUTH_CIPHER_NONE) {
+			if (wrq->value == IW_AUTH_CIPHER_CCMP) {
 				pDevice->eEncryptionStatus = Ndis802_11Encryption3Enabled;
-			}else {
+			} else {
 				pDevice->eEncryptionStatus = Ndis802_11Encryption2Enabled;
 			}
 		}
 		break;
 	case IW_AUTH_KEY_MGMT:
 
-		if(wpa_version == IW_AUTH_WPA_VERSION_WPA2){
-			if(wrq->value == IW_AUTH_KEY_MGMT_PSK)
+		if (wpa_version == IW_AUTH_WPA_VERSION_WPA2) {
+			if (wrq->value == IW_AUTH_KEY_MGMT_PSK)
 				pMgmt->eAuthenMode = WMAC_AUTH_WPA2PSK;
 			else pMgmt->eAuthenMode = WMAC_AUTH_WPA2;
-		}else if(wpa_version == IW_AUTH_WPA_VERSION_WPA){
-			if(wrq->value == 0){
+		} else if (wpa_version == IW_AUTH_WPA_VERSION_WPA) {
+			if (wrq->value == 0) {
 				pMgmt->eAuthenMode = WMAC_AUTH_WPANONE;
-			}else if(wrq->value == IW_AUTH_KEY_MGMT_PSK)
+			} else if (wrq->value == IW_AUTH_KEY_MGMT_PSK)
 				pMgmt->eAuthenMode = WMAC_AUTH_WPAPSK;
 			else pMgmt->eAuthenMode = WMAC_AUTH_WPA;
 		}
@@ -1787,10 +1709,10 @@ int iwctl_siwauth(struct net_device *dev,
 	case IW_AUTH_DROP_UNENCRYPTED:
 		break;
 	case IW_AUTH_80211_AUTH_ALG:
-		if(wrq->value==IW_AUTH_ALG_OPEN_SYSTEM){
-			pMgmt->bShareKeyAlgorithm=false;
-		}else if(wrq->value==IW_AUTH_ALG_SHARED_KEY){
-			pMgmt->bShareKeyAlgorithm=true;
+		if (wrq->value == IW_AUTH_ALG_OPEN_SYSTEM) {
+			pMgmt->bShareKeyAlgorithm = false;
+		} else if (wrq->value == IW_AUTH_ALG_SHARED_KEY) {
+			pMgmt->bShareKeyAlgorithm = true;
 		}
 		break;
 	case IW_AUTH_WPA_ENABLED:
@@ -1803,7 +1725,7 @@ int iwctl_siwauth(struct net_device *dev,
 		break;
 	case IW_AUTH_PRIVACY_INVOKED:
 		pDevice->bEncryptionEnable = !!wrq->value;
-		if(pDevice->bEncryptionEnable == false){
+		if (pDevice->bEncryptionEnable == false) {
 			wpa_version = 0;
 			pairwise = 0;
 			pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
@@ -1818,220 +1740,214 @@ int iwctl_siwauth(struct net_device *dev,
 		break;
 	}
 /*
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wpa_version = %d\n",wpa_version);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pairwise = %d\n",pairwise);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->eEncryptionStatus = %d\n",pDevice->eEncryptionStatus);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pMgmt->eAuthenMode  = %d\n",pMgmt->eAuthenMode);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pMgmt->bShareKeyAlgorithm = %s\n",pMgmt->bShareKeyAlgorithm?"true":"false");
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->bEncryptionEnable = %s\n",pDevice->bEncryptionEnable?"true":"false");
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->bWPADevEnable = %s\n",pDevice->bWPADevEnable?"true":"false");
+  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wpa_version = %d\n",wpa_version);
+  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pairwise = %d\n",pairwise);
+  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->eEncryptionStatus = %d\n",pDevice->eEncryptionStatus);
+  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pMgmt->eAuthenMode  = %d\n",pMgmt->eAuthenMode);
+  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pMgmt->bShareKeyAlgorithm = %s\n",pMgmt->bShareKeyAlgorithm?"true":"false");
+  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->bEncryptionEnable = %s\n",pDevice->bEncryptionEnable?"true":"false");
+  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->bWPADevEnable = %s\n",pDevice->bWPADevEnable?"true":"false");
 */
-   return ret;
+	return ret;
 }
 
-
 int iwctl_giwauth(struct net_device *dev,
-			  struct iw_request_info *info,
-			  struct iw_param *wrq,
-			  char *extra)
+		  struct iw_request_info *info,
+		  struct iw_param *wrq,
+		  char *extra)
 {
 	return -EOPNOTSUPP;
 }
 
-
-
 int iwctl_siwgenie(struct net_device *dev,
-			  struct iw_request_info *info,
-			  struct iw_point *wrq,
-			  char *extra)
+		   struct iw_request_info *info,
+		   struct iw_point *wrq,
+		   char *extra)
 {
 	PSDevice			pDevice = (PSDevice)netdev_priv(dev);
 	PSMgmtObject	pMgmt = &(pDevice->sMgmtObj);
-	int ret=0;
+	int ret = 0;
 
-	if(wrq->length){
+	if (wrq->length) {
 		if ((wrq->length < 2) || (extra[1]+2 != wrq->length)) {
 			ret = -EINVAL;
 			goto out;
 		}
-		if(wrq->length > MAX_WPA_IE_LEN){
+		if (wrq->length > MAX_WPA_IE_LEN) {
 			ret = -ENOMEM;
 			goto out;
 		}
 		memset(pMgmt->abyWPAIE, 0, MAX_WPA_IE_LEN);
-		if(copy_from_user(pMgmt->abyWPAIE, extra, wrq->length)){
+		if (copy_from_user(pMgmt->abyWPAIE, extra, wrq->length)) {
 			ret = -EFAULT;
 			goto out;
 		}
 		pMgmt->wWPAIELen = wrq->length;
-	}else {
+	} else {
 		memset(pMgmt->abyWPAIE, 0, MAX_WPA_IE_LEN);
 		pMgmt->wWPAIELen = 0;
 	}
 
-	out://not completely ...not necessary in wpa_supplicant 0.5.8
+out://not completely ...not necessary in wpa_supplicant 0.5.8
 	return ret;
 }
 
 int iwctl_giwgenie(struct net_device *dev,
-			  struct iw_request_info *info,
-			  struct iw_point *wrq,
-			  char *extra)
+		   struct iw_request_info *info,
+		   struct iw_point *wrq,
+		   char *extra)
 {
 	PSDevice			pDevice = (PSDevice)netdev_priv(dev);
 	PSMgmtObject	pMgmt = &(pDevice->sMgmtObj);
-	int ret=0;
+	int ret = 0;
 	int space = wrq->length;
 
 	wrq->length = 0;
-	if(pMgmt->wWPAIELen > 0){
+	if (pMgmt->wWPAIELen > 0) {
 		wrq->length = pMgmt->wWPAIELen;
-		if(pMgmt->wWPAIELen <= space){
-			if(copy_to_user(extra, pMgmt->abyWPAIE, pMgmt->wWPAIELen)){
+		if (pMgmt->wWPAIELen <= space) {
+			if (copy_to_user(extra, pMgmt->abyWPAIE, pMgmt->wWPAIELen)) {
 				ret = -EFAULT;
 			}
-		}else
+		} else
 			ret = -E2BIG;
 	}
 
 	return ret;
 }
 
-
 int iwctl_siwencodeext(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra)
+		       struct iw_request_info *info,
+		       struct iw_point *wrq,
+		       char *extra)
 {
-    PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-	struct iw_encode_ext *ext = (struct iw_encode_ext*)extra;
-    struct viawget_wpa_param *param=NULL;
+	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
+	struct viawget_wpa_param *param = NULL;
 //original member
-    wpa_alg alg_name;
-    u8  addr[6];
-    int key_idx, set_tx=0;
-    u8  seq[IW_ENCODE_SEQ_MAX_SIZE];
-    u8 key[64];
-    size_t seq_len=0,key_len=0;
+	wpa_alg alg_name;
+	u8  addr[6];
+	int key_idx, set_tx = 0;
+	u8  seq[IW_ENCODE_SEQ_MAX_SIZE];
+	u8 key[64];
+	size_t seq_len = 0, key_len = 0;
 //
-   // int ii;
-    u8 *buf;
-    size_t blen;
-    u8 key_array[64];
-    int ret=0;
+	// int ii;
+	u8 *buf;
+	size_t blen;
+	u8 key_array[64];
+	int ret = 0;
 
-PRINT_K("SIOCSIWENCODEEXT...... \n");
+	PRINT_K("SIOCSIWENCODEEXT...... \n");
 
-blen = sizeof(*param);
-buf = kmalloc((int)blen, (int)GFP_KERNEL);
-if (buf == NULL)
-    return -ENOMEM;
-memset(buf, 0, blen);
-param = (struct viawget_wpa_param *) buf;
+	blen = sizeof(*param);
+	buf = kmalloc((int)blen, (int)GFP_KERNEL);
+	if (buf == NULL)
+		return -ENOMEM;
+	memset(buf, 0, blen);
+	param = (struct viawget_wpa_param *)buf;
 
 //recover alg_name
-switch (ext->alg) {
-    case IW_ENCODE_ALG_NONE:
-                  alg_name = WPA_ALG_NONE;
+	switch (ext->alg) {
+	case IW_ENCODE_ALG_NONE:
+		alg_name = WPA_ALG_NONE;
 		break;
-    case IW_ENCODE_ALG_WEP:
-                  alg_name = WPA_ALG_WEP;
+	case IW_ENCODE_ALG_WEP:
+		alg_name = WPA_ALG_WEP;
 		break;
-    case IW_ENCODE_ALG_TKIP:
-                  alg_name = WPA_ALG_TKIP;
+	case IW_ENCODE_ALG_TKIP:
+		alg_name = WPA_ALG_TKIP;
 		break;
-    case IW_ENCODE_ALG_CCMP:
-                  alg_name = WPA_ALG_CCMP;
+	case IW_ENCODE_ALG_CCMP:
+		alg_name = WPA_ALG_CCMP;
 		break;
-    default:
-		PRINT_K("Unknown alg = %d\n",ext->alg);
-		ret= -ENOMEM;
+	default:
+		PRINT_K("Unknown alg = %d\n", ext->alg);
+		ret = -ENOMEM;
 		goto error;
-		}
+	}
 //recover addr
- memcpy(addr, ext->addr.sa_data, ETH_ALEN);
+	memcpy(addr, ext->addr.sa_data, ETH_ALEN);
 //recover key_idx
-  key_idx = (wrq->flags&IW_ENCODE_INDEX) - 1;
+	key_idx = (wrq->flags&IW_ENCODE_INDEX) - 1;
 //recover set_tx
-if(ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
-   set_tx = 1;
+	if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+		set_tx = 1;
 //recover seq,seq_len
-	if(ext->ext_flags & IW_ENCODE_EXT_RX_SEQ_VALID) {
-   seq_len=IW_ENCODE_SEQ_MAX_SIZE;
-   memcpy(seq, ext->rx_seq, seq_len);
-		}
+	if (ext->ext_flags & IW_ENCODE_EXT_RX_SEQ_VALID) {
+		seq_len = IW_ENCODE_SEQ_MAX_SIZE;
+		memcpy(seq, ext->rx_seq, seq_len);
+	}
 //recover key,key_len
-if(ext->key_len) {
-  key_len=ext->key_len;
-  memcpy(key, &ext->key[0], key_len);
+	if (ext->key_len) {
+		key_len = ext->key_len;
+		memcpy(key, &ext->key[0], key_len);
 	}
 
-memset(key_array, 0, 64);
-if ( key_len > 0) {
-     memcpy(key_array, key, key_len);
-    if (key_len == 32) {
-          // notice ! the oder
-	  memcpy(&key_array[16], &key[24], 8);
-	  memcpy(&key_array[24], &key[16], 8);
-	}
+	memset(key_array, 0, 64);
+	if (key_len > 0) {
+		memcpy(key_array, key, key_len);
+		if (key_len == 32) {
+			// notice ! the oder
+			memcpy(&key_array[16], &key[24], 8);
+			memcpy(&key_array[24], &key[16], 8);
+		}
 	}
 
 /**************Translate iw_encode_ext to viawget_wpa_param****************/
-memcpy(param->addr, addr, ETH_ALEN);
-param->u.wpa_key.alg_name = (int)alg_name;
-param->u.wpa_key.set_tx = set_tx;
-param->u.wpa_key.key_index = key_idx;
-param->u.wpa_key.key_len = key_len;
-param->u.wpa_key.key = (u8 *)key_array;
-param->u.wpa_key.seq = (u8 *)seq;
-param->u.wpa_key.seq_len = seq_len;
+	memcpy(param->addr, addr, ETH_ALEN);
+	param->u.wpa_key.alg_name = (int)alg_name;
+	param->u.wpa_key.set_tx = set_tx;
+	param->u.wpa_key.key_index = key_idx;
+	param->u.wpa_key.key_len = key_len;
+	param->u.wpa_key.key = (u8 *)key_array;
+	param->u.wpa_key.seq = (u8 *)seq;
+	param->u.wpa_key.seq_len = seq_len;
 
 //****set if current action is Network Manager count??
 //****this method is so foolish,but there is no other way???
-if(param->u.wpa_key.alg_name == WPA_ALG_NONE) {
-   if(param->u.wpa_key.key_index ==0) {
-     pDevice->bwextcount++;
-    }
-   if((pDevice->bwextcount == 1)&&(param->u.wpa_key.key_index ==1)) {
- pDevice->bwextcount++;
-    }
-   if((pDevice->bwextcount ==2)&&(param->u.wpa_key.key_index ==2)) {
- pDevice->bwextcount++;
+	if (param->u.wpa_key.alg_name == WPA_ALG_NONE) {
+		if (param->u.wpa_key.key_index == 0) {
+			pDevice->bwextcount++;
+		}
+		if ((pDevice->bwextcount == 1) && (param->u.wpa_key.key_index == 1)) {
+			pDevice->bwextcount++;
+		}
+		if ((pDevice->bwextcount == 2) && (param->u.wpa_key.key_index == 2)) {
+			pDevice->bwextcount++;
+		}
+		if ((pDevice->bwextcount == 3) && (param->u.wpa_key.key_index == 3)) {
+			pDevice->bwextcount++;
+		}
+	}
+	if (pDevice->bwextcount == 4) {
+		printk("SIOCSIWENCODEEXT:Enable WPA WEXT SUPPORT!!!!!\n");
+		pDevice->bwextcount = 0;
+		pDevice->bWPASuppWextEnabled = true;
 	}
-   if((pDevice->bwextcount ==3)&&(param->u.wpa_key.key_index ==3)) {
- pDevice->bwextcount++;
-        }
-		 }
-if( pDevice->bwextcount == 4) {
-    printk("SIOCSIWENCODEEXT:Enable WPA WEXT SUPPORT!!!!!\n");
- pDevice->bwextcount=0;
-   pDevice->bWPASuppWextEnabled = true;
-		 }
 //******
 
-		spin_lock_irq(&pDevice->lock);
- ret = wpa_set_keys(pDevice, param, true);
-		spin_unlock_irq(&pDevice->lock);
+	spin_lock_irq(&pDevice->lock);
+	ret = wpa_set_keys(pDevice, param, true);
+	spin_unlock_irq(&pDevice->lock);
 
 error:
-kfree(param);
+	kfree(param);
 	return ret;
 }
 
-
-
 int iwctl_giwencodeext(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra)
+		       struct iw_request_info *info,
+		       struct iw_point *wrq,
+		       char *extra)
 {
-		return -EOPNOTSUPP;
+	return -EOPNOTSUPP;
 }
 
 int iwctl_siwmlme(struct net_device *dev,
-				struct iw_request_info * info,
-				struct iw_point *wrq,
-				char *extra)
+		  struct iw_request_info *info,
+		  struct iw_point *wrq,
+		  char *extra)
 {
 	PSDevice			pDevice = (PSDevice)netdev_priv(dev);
 	PSMgmtObject	pMgmt = &(pDevice->sMgmtObj);
@@ -2039,21 +1955,21 @@ int iwctl_siwmlme(struct net_device *dev,
 	//u16 reason = cpu_to_le16(mlme->reason_code);
 	int ret = 0;
 
-	if(memcmp(pMgmt->abyCurrBSSID, mlme->addr.sa_data, ETH_ALEN)){
+	if (memcmp(pMgmt->abyCurrBSSID, mlme->addr.sa_data, ETH_ALEN)) {
 		ret = -EINVAL;
 		return ret;
 	}
-	switch(mlme->cmd){
+	switch (mlme->cmd) {
 	case IW_MLME_DEAUTH:
 		//this command seems to be not complete,please test it --einsnliu
 		//bScheduleCommand((void *) pDevice, WLAN_CMD_DEAUTH, (unsigned char *)&reason);
 		break;
 	case IW_MLME_DISASSOC:
-		if(pDevice->bLinkPass == true){
-					  printk("iwctl_siwmlme--->send DISASSOCIATE\n");
-		  //clear related flags
-		   memset(pMgmt->abyDesireBSSID, 0xFF,6);
-		KeyvInitTable(&pDevice->sKey, pDevice->PortOffset);
+		if (pDevice->bLinkPass == true) {
+			printk("iwctl_siwmlme--->send DISASSOCIATE\n");
+			//clear related flags
+			memset(pMgmt->abyDesireBSSID, 0xFF, 6);
+			KeyvInitTable(&pDevice->sKey, pDevice->PortOffset);
 			bScheduleCommand((void *)pDevice, WLAN_CMD_DISASSOCIATE, NULL);
 		}
 		break;
@@ -2062,109 +1978,106 @@ int iwctl_siwmlme(struct net_device *dev,
 	}
 
 	return ret;
-
 }
 
 #endif
 
-
 /*------------------------------------------------------------------*/
 /*
  * Structures to export the Wireless Handlers
  */
 
-
 /*
-static const iw_handler		iwctl_handler[] =
-{
-	(iw_handler) iwctl_commit,      // SIOCSIWCOMMIT
-	(iw_handler) iwctl_giwname,     // SIOCGIWNAME
-	(iw_handler) NULL,				// SIOCSIWNWID
-	(iw_handler) NULL,				// SIOCGIWNWID
-	(iw_handler) iwctl_siwfreq,		// SIOCSIWFREQ
-	(iw_handler) iwctl_giwfreq,		// SIOCGIWFREQ
-	(iw_handler) iwctl_siwmode,		// SIOCSIWMODE
-	(iw_handler) iwctl_giwmode,		// SIOCGIWMODE
-	(iw_handler) NULL,		        // SIOCSIWSENS
-	(iw_handler) iwctl_giwsens,		        // SIOCGIWSENS
-	(iw_handler) NULL, 		        // SIOCSIWRANGE
-	(iw_handler) iwctl_giwrange,		// SIOCGIWRANGE
-	(iw_handler) NULL,         		    // SIOCSIWPRIV
-	(iw_handler) NULL,             		// SIOCGIWPRIV
-	(iw_handler) NULL,             		// SIOCSIWSTATS
-	(iw_handler) NULL,                  // SIOCGIWSTATS
-    (iw_handler) NULL,                  // SIOCSIWSPY
-	(iw_handler) NULL,		            // SIOCGIWSPY
-	(iw_handler) NULL,				    // -- hole --
-	(iw_handler) NULL,				    // -- hole --
-	(iw_handler) iwctl_siwap,		    // SIOCSIWAP
-	(iw_handler) iwctl_giwap,		    // SIOCGIWAP
-	(iw_handler) NULL,				    // -- hole -- 0x16
-	(iw_handler) iwctl_giwaplist,       // SIOCGIWAPLIST
-	(iw_handler) iwctl_siwscan,         // SIOCSIWSCAN
-	(iw_handler) iwctl_giwscan,         // SIOCGIWSCAN
-	(iw_handler) iwctl_siwessid,		// SIOCSIWESSID
-	(iw_handler) iwctl_giwessid,		// SIOCGIWESSID
-	(iw_handler) NULL,		// SIOCSIWNICKN
-	(iw_handler) NULL,		// SIOCGIWNICKN
-	(iw_handler) NULL,				    // -- hole --
-	(iw_handler) NULL,				    // -- hole --
-	(iw_handler) iwctl_siwrate,		// SIOCSIWRATE 0x20
-	(iw_handler) iwctl_giwrate,		// SIOCGIWRATE
-	(iw_handler) iwctl_siwrts,		// SIOCSIWRTS
-	(iw_handler) iwctl_giwrts,		// SIOCGIWRTS
-	(iw_handler) iwctl_siwfrag,		// SIOCSIWFRAG
-	(iw_handler) iwctl_giwfrag,		// SIOCGIWFRAG
-	(iw_handler) NULL,		// SIOCSIWTXPOW
-	(iw_handler) NULL,		// SIOCGIWTXPOW
-	(iw_handler) iwctl_siwretry,		// SIOCSIWRETRY
-	(iw_handler) iwctl_giwretry,		// SIOCGIWRETRY
-	(iw_handler) iwctl_siwencode,		// SIOCSIWENCODE
-	(iw_handler) iwctl_giwencode,		// SIOCGIWENCODE
-	(iw_handler) iwctl_siwpower,		// SIOCSIWPOWER
-	(iw_handler) iwctl_giwpower,		// SIOCGIWPOWER
-	(iw_handler) NULL,			// -- hole --
-	(iw_handler) NULL,			// -- hole --
-	(iw_handler) iwctl_siwgenie,    // SIOCSIWGENIE
-	(iw_handler) iwctl_giwgenie,    // SIOCGIWGENIE
-	(iw_handler) iwctl_siwauth,		// SIOCSIWAUTH
-	(iw_handler) iwctl_giwauth,		// SIOCGIWAUTH
-	(iw_handler) iwctl_siwencodeext,		// SIOCSIWENCODEEXT
-	(iw_handler) iwctl_giwencodeext,		// SIOCGIWENCODEEXT
-	(iw_handler) NULL,				// SIOCSIWPMKSA
-	(iw_handler) NULL,				// -- hole --
-
-};
+  static const iw_handler		iwctl_handler[] =
+  {
+  (iw_handler) iwctl_commit,      // SIOCSIWCOMMIT
+  (iw_handler) iwctl_giwname,     // SIOCGIWNAME
+  (iw_handler) NULL,				// SIOCSIWNWID
+  (iw_handler) NULL,				// SIOCGIWNWID
+  (iw_handler) iwctl_siwfreq,		// SIOCSIWFREQ
+  (iw_handler) iwctl_giwfreq,		// SIOCGIWFREQ
+  (iw_handler) iwctl_siwmode,		// SIOCSIWMODE
+  (iw_handler) iwctl_giwmode,		// SIOCGIWMODE
+  (iw_handler) NULL,		        // SIOCSIWSENS
+  (iw_handler) iwctl_giwsens,		        // SIOCGIWSENS
+  (iw_handler) NULL,		        // SIOCSIWRANGE
+  (iw_handler) iwctl_giwrange,		// SIOCGIWRANGE
+  (iw_handler) NULL,			// SIOCSIWPRIV
+  (iw_handler) NULL,			// SIOCGIWPRIV
+  (iw_handler) NULL,			// SIOCSIWSTATS
+  (iw_handler) NULL,                  // SIOCGIWSTATS
+  (iw_handler) NULL,                  // SIOCSIWSPY
+  (iw_handler) NULL,		            // SIOCGIWSPY
+  (iw_handler) NULL,				    // -- hole --
+  (iw_handler) NULL,				    // -- hole --
+  (iw_handler) iwctl_siwap,		    // SIOCSIWAP
+  (iw_handler) iwctl_giwap,		    // SIOCGIWAP
+  (iw_handler) NULL,				    // -- hole -- 0x16
+  (iw_handler) iwctl_giwaplist,       // SIOCGIWAPLIST
+  (iw_handler) iwctl_siwscan,         // SIOCSIWSCAN
+  (iw_handler) iwctl_giwscan,         // SIOCGIWSCAN
+  (iw_handler) iwctl_siwessid,		// SIOCSIWESSID
+  (iw_handler) iwctl_giwessid,		// SIOCGIWESSID
+  (iw_handler) NULL,		// SIOCSIWNICKN
+  (iw_handler) NULL,		// SIOCGIWNICKN
+  (iw_handler) NULL,				    // -- hole --
+  (iw_handler) NULL,				    // -- hole --
+  (iw_handler) iwctl_siwrate,		// SIOCSIWRATE 0x20
+  (iw_handler) iwctl_giwrate,		// SIOCGIWRATE
+  (iw_handler) iwctl_siwrts,		// SIOCSIWRTS
+  (iw_handler) iwctl_giwrts,		// SIOCGIWRTS
+  (iw_handler) iwctl_siwfrag,		// SIOCSIWFRAG
+  (iw_handler) iwctl_giwfrag,		// SIOCGIWFRAG
+  (iw_handler) NULL,		// SIOCSIWTXPOW
+  (iw_handler) NULL,		// SIOCGIWTXPOW
+  (iw_handler) iwctl_siwretry,		// SIOCSIWRETRY
+  (iw_handler) iwctl_giwretry,		// SIOCGIWRETRY
+  (iw_handler) iwctl_siwencode,		// SIOCSIWENCODE
+  (iw_handler) iwctl_giwencode,		// SIOCGIWENCODE
+  (iw_handler) iwctl_siwpower,		// SIOCSIWPOWER
+  (iw_handler) iwctl_giwpower,		// SIOCGIWPOWER
+  (iw_handler) NULL,			// -- hole --
+  (iw_handler) NULL,			// -- hole --
+  (iw_handler) iwctl_siwgenie,    // SIOCSIWGENIE
+  (iw_handler) iwctl_giwgenie,    // SIOCGIWGENIE
+  (iw_handler) iwctl_siwauth,		// SIOCSIWAUTH
+  (iw_handler) iwctl_giwauth,		// SIOCGIWAUTH
+  (iw_handler) iwctl_siwencodeext,		// SIOCSIWENCODEEXT
+  (iw_handler) iwctl_giwencodeext,		// SIOCGIWENCODEEXT
+  (iw_handler) NULL,				// SIOCSIWPMKSA
+  (iw_handler) NULL,				// -- hole --
+
+  };
 */
 
 static const iw_handler		iwctl_handler[] =
 {
 	(iw_handler) iwctl_commit,      // SIOCSIWCOMMIT
-	(iw_handler) NULL,      // SIOCGIWNAME
-	(iw_handler) NULL,				// SIOCSIWNWID
-	(iw_handler) NULL,				// SIOCGIWNWID
+	(iw_handler) NULL,		// SIOCGIWNAME
+	(iw_handler) NULL,		// SIOCSIWNWID
+	(iw_handler) NULL,		// SIOCGIWNWID
 	(iw_handler) NULL,		// SIOCSIWFREQ
 	(iw_handler) NULL,		// SIOCGIWFREQ
 	(iw_handler) NULL,		// SIOCSIWMODE
 	(iw_handler) NULL,		// SIOCGIWMODE
-	(iw_handler) NULL,		        // SIOCSIWSENS
-	(iw_handler) NULL,		        // SIOCGIWSENS
-	(iw_handler) NULL, 		        // SIOCSIWRANGE
-	(iw_handler) iwctl_giwrange,		// SIOCGIWRANGE
-	(iw_handler) NULL,         		    // SIOCSIWPRIV
-	(iw_handler) NULL,             		// SIOCGIWPRIV
-	(iw_handler) NULL,             		// SIOCSIWSTATS
-	(iw_handler) NULL,                  // SIOCGIWSTATS
-    (iw_handler) NULL,                  // SIOCSIWSPY
-	(iw_handler) NULL,		            // SIOCGIWSPY
-	(iw_handler) NULL,				    // -- hole --
-	(iw_handler) NULL,				    // -- hole --
-	(iw_handler) NULL,		    // SIOCSIWAP
-	(iw_handler) NULL,		    // SIOCGIWAP
-	(iw_handler) NULL,				    // -- hole -- 0x16
-	(iw_handler) NULL,       // SIOCGIWAPLIST
-	(iw_handler) iwctl_siwscan,         // SIOCSIWSCAN
-	(iw_handler) iwctl_giwscan,         // SIOCGIWSCAN
+	(iw_handler) NULL,		// SIOCSIWSENS
+	(iw_handler) NULL,		// SIOCGIWSENS
+	(iw_handler) NULL,		// SIOCSIWRANGE
+	(iw_handler) iwctl_giwrange,	// SIOCGIWRANGE
+	(iw_handler) NULL,		// SIOCSIWPRIV
+	(iw_handler) NULL,		// SIOCGIWPRIV
+	(iw_handler) NULL,		// SIOCSIWSTATS
+	(iw_handler) NULL,		// SIOCGIWSTATS
+	(iw_handler) NULL,		// SIOCSIWSPY
+	(iw_handler) NULL,		// SIOCGIWSPY
+	(iw_handler) NULL,		// -- hole --
+	(iw_handler) NULL,		// -- hole --
+	(iw_handler) NULL,		// SIOCSIWAP
+	(iw_handler) NULL,		// SIOCGIWAP
+	(iw_handler) NULL,		// -- hole -- 0x16
+	(iw_handler) NULL,		// SIOCGIWAPLIST
+	(iw_handler) iwctl_siwscan,	// SIOCSIWSCAN
+	(iw_handler) iwctl_giwscan,	// SIOCGIWSCAN
 	(iw_handler) NULL,		// SIOCSIWESSID
 	(iw_handler) NULL,		// SIOCGIWESSID
 	(iw_handler) NULL,		// SIOCSIWNICKN
@@ -2187,33 +2100,29 @@ static const iw_handler		iwctl_handler[] =
 	(iw_handler) NULL,		// SIOCGIWPOWER
 
 //2008-0409-07, <Add> by Einsn Liu
-	(iw_handler) NULL,			// -- hole --
-	(iw_handler) NULL,			// -- hole --
-	(iw_handler) NULL,    // SIOCSIWGENIE
-	(iw_handler) NULL,    // SIOCGIWGENIE
+	(iw_handler) NULL,		// -- hole --
+	(iw_handler) NULL,		// -- hole --
+	(iw_handler) NULL,		// SIOCSIWGENIE
+	(iw_handler) NULL,		// SIOCGIWGENIE
 	(iw_handler) NULL,		// SIOCSIWAUTH
 	(iw_handler) NULL,		// SIOCGIWAUTH
 	(iw_handler) NULL,		// SIOCSIWENCODEEXT
 	(iw_handler) NULL,		// SIOCGIWENCODEEXT
-	(iw_handler) NULL,				// SIOCSIWPMKSA
-	(iw_handler) NULL,				// -- hole --
+	(iw_handler) NULL,		// SIOCSIWPMKSA
+	(iw_handler) NULL,		// -- hole --
 };
 
-
 static const iw_handler		iwctl_private_handler[] =
 {
 	NULL,				// SIOCIWFIRSTPRIV
 };
 
-
 struct iw_priv_args iwctl_private_args[] = {
-{ IOCTL_CMD_SET,
-  IW_PRIV_TYPE_CHAR | 1024, 0,
-  "set"},
+	{ IOCTL_CMD_SET,
+	  IW_PRIV_TYPE_CHAR | 1024, 0,
+	  "set"},
 };
 
-
-
 const struct iw_handler_def	iwctl_handler_def =
 {
 	.get_wireless_stats = &iwctl_get_wireless_stats,
@@ -2222,7 +2131,7 @@ const struct iw_handler_def	iwctl_handler_def =
 //	.num_private_args = sizeof(iwctl_private_args)/sizeof(struct iw_priv_args),
 	.num_private	= 0,
 	.num_private_args = 0,
-	.standard	= (iw_handler *) iwctl_handler,
+	.standard	= (iw_handler *)iwctl_handler,
 //	.private	= (iw_handler *) iwctl_private_handler,
 //	.private_args	= (struct iw_priv_args *)iwctl_private_args,
 	.private	= NULL,
diff --git a/drivers/staging/vt6655/iwctl.h b/drivers/staging/vt6655/iwctl.h
index d224f913a62..871bd7c4e71 100644
--- a/drivers/staging/vt6655/iwctl.h
+++ b/drivers/staging/vt6655/iwctl.h
@@ -33,192 +33,184 @@
 
 /*---------------------  Export Definitions -------------------------*/
 
-
 /*---------------------  Export Classes  ----------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
 
 /*---------------------  Export Functions  --------------------------*/
 
-struct iw_statistics *iwctl_get_wireless_stats (struct net_device *dev);
-
+struct iw_statistics *iwctl_get_wireless_stats(struct net_device *dev);
 
 int iwctl_siwap(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct sockaddr *wrq,
-             char *extra);
+		struct iw_request_info *info,
+		struct sockaddr *wrq,
+		char *extra);
 
 int iwctl_giwrange(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra);
-
+		   struct iw_request_info *info,
+		   struct iw_point *wrq,
+		   char *extra);
 
 int iwctl_giwmode(struct net_device *dev,
-             struct iw_request_info *info,
-             __u32 *wmode,
-             char *extra);
+		  struct iw_request_info *info,
+		  __u32 *wmode,
+		  char *extra);
 
 int iwctl_siwmode(struct net_device *dev,
-             struct iw_request_info *info,
-             __u32 *wmode,
-             char *extra);
+		  struct iw_request_info *info,
+		  __u32 *wmode,
+		  char *extra);
 
 int iwctl_giwfreq(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_freq *wrq,
-             char *extra);
+		  struct iw_request_info *info,
+		  struct iw_freq *wrq,
+		  char *extra);
 
 int iwctl_siwfreq(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_freq *wrq,
-             char *extra);
+		  struct iw_request_info *info,
+		  struct iw_freq *wrq,
+		  char *extra);
 
 int iwctl_giwname(struct net_device *dev,
-			 struct iw_request_info *info,
-			 char *wrq,
-			 char *extra);
+		  struct iw_request_info *info,
+		  char *wrq,
+		  char *extra);
 
 int iwctl_giwsens(struct net_device *dev,
-			 struct iw_request_info *info,
-			 struct iw_param *wrq,
-			 char *extra);
+		  struct iw_request_info *info,
+		  struct iw_param *wrq,
+		  char *extra);
 
 int iwctl_giwap(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct sockaddr *wrq,
-             char *extra);
+		struct iw_request_info *info,
+		struct sockaddr *wrq,
+		char *extra);
 
 int iwctl_giwaplist(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra);
+		    struct iw_request_info *info,
+		    struct iw_point *wrq,
+		    char *extra);
 
 int iwctl_siwessid(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra);
+		   struct iw_request_info *info,
+		   struct iw_point *wrq,
+		   char *extra);
 
 int iwctl_giwessid(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra);
+		   struct iw_request_info *info,
+		   struct iw_point *wrq,
+		   char *extra);
 
 int iwctl_siwrate(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra);
+		  struct iw_request_info *info,
+		  struct iw_param *wrq,
+		  char *extra);
 
 int iwctl_giwrate(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_param *wrq,
-             char *extra);
+		  struct iw_request_info *info,
+		  struct iw_param *wrq,
+		  char *extra);
 
 int iwctl_siwrts(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra);
-
+		 struct iw_request_info *info,
+		 struct iw_param *wrq,
+		 char *extra);
 
 int iwctl_giwrts(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra);
+		 struct iw_request_info *info,
+		 struct iw_param *wrq,
+		 char *extra);
 
 int iwctl_siwfrag(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra);
+		  struct iw_request_info *info,
+		  struct iw_param *wrq,
+		  char *extra);
 
 int iwctl_giwfrag(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra);
+		  struct iw_request_info *info,
+		  struct iw_param *wrq,
+		  char *extra);
 
 int iwctl_siwretry(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra);
+		   struct iw_request_info *info,
+		   struct iw_param *wrq,
+		   char *extra);
 
 int iwctl_giwretry(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra);
+		   struct iw_request_info *info,
+		   struct iw_param *wrq,
+		   char *extra);
 
 int iwctl_siwencode(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra);
+		    struct iw_request_info *info,
+		    struct iw_point *wrq,
+		    char *extra);
 
 int iwctl_giwencode(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra);
+		    struct iw_request_info *info,
+		    struct iw_point *wrq,
+		    char *extra);
 
 int iwctl_siwpower(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra);
+		   struct iw_request_info *info,
+		   struct iw_param *wrq,
+		   char *extra);
 
 int iwctl_giwpower(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra);
+		   struct iw_request_info *info,
+		   struct iw_param *wrq,
+		   char *extra);
 
 int iwctl_giwscan(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_point *wrq,
-             char *extra);
+		  struct iw_request_info *info,
+		  struct iw_point *wrq,
+		  char *extra);
 
 int iwctl_siwscan(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra);
+		  struct iw_request_info *info,
+		  struct iw_param *wrq,
+		  char *extra);
 
 //2008-0409-07, <Add> by Einsn Liu
 #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
 int iwctl_siwauth(struct net_device *dev,
-			  struct iw_request_info *info,
-			  struct iw_param *wrq,
-			  char *extra);
+		  struct iw_request_info *info,
+		  struct iw_param *wrq,
+		  char *extra);
 
 int iwctl_giwauth(struct net_device *dev,
-			  struct iw_request_info *info,
-			  struct iw_param *wrq,
-			  char *extra);
+		  struct iw_request_info *info,
+		  struct iw_param *wrq,
+		  char *extra);
 
 int iwctl_siwgenie(struct net_device *dev,
-			  struct iw_request_info *info,
-			  struct iw_point *wrq,
-			  char *extra);
+		   struct iw_request_info *info,
+		   struct iw_point *wrq,
+		   char *extra);
 
 int iwctl_giwgenie(struct net_device *dev,
-			  struct iw_request_info *info,
-			  struct iw_point *wrq,
-			  char *extra);
+		   struct iw_request_info *info,
+		   struct iw_point *wrq,
+		   char *extra);
 
 int iwctl_siwencodeext(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra);
+		       struct iw_request_info *info,
+		       struct iw_point *wrq,
+		       char *extra);
 
 int iwctl_giwencodeext(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra);
+		       struct iw_request_info *info,
+		       struct iw_point *wrq,
+		       char *extra);
 
 int iwctl_siwmlme(struct net_device *dev,
-			struct iw_request_info * info,
-			struct iw_point *wrq,
-			char *extra);
+		  struct iw_request_info *info,
+		  struct iw_point *wrq,
+		  char *extra);
 #endif // #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
 //End Add -- //2008-0409-07, <Add> by Einsn Liu
 
-
 extern const struct iw_handler_def	iwctl_handler_def;
 extern const struct iw_priv_args	iwctl_private_args;
 
 #endif // __IWCTL_H__
-
-
-
diff --git a/drivers/staging/vt6655/key.c b/drivers/staging/vt6655/key.c
index 194fedc715f..92b84b5ea11 100644
--- a/drivers/staging/vt6655/key.c
+++ b/drivers/staging/vt6655/key.c
@@ -45,7 +45,7 @@
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
-static int          msglevel                =MSG_LEVEL_INFO;
+static int msglevel = MSG_LEVEL_INFO;
 //static int          msglevel                =MSG_LEVEL_DEBUG;
 /*---------------------  Static Functions  --------------------------*/
 
@@ -59,31 +59,28 @@ static int          msglevel                =MSG_LEVEL_INFO;
 
 /*---------------------  Static Functions  --------------------------*/
 static void
-s_vCheckKeyTableValid (PSKeyManagement pTable, unsigned long dwIoBase)
+s_vCheckKeyTableValid(PSKeyManagement pTable, unsigned long dwIoBase)
 {
-    int i;
-
-    for (i=0;i<MAX_KEY_TABLE;i++) {
-        if ((pTable->KeyTable[i].bInUse == true) &&
-            (pTable->KeyTable[i].PairwiseKey.bKeyValid == false) &&
-            (pTable->KeyTable[i].GroupKey[0].bKeyValid == false) &&
-            (pTable->KeyTable[i].GroupKey[1].bKeyValid == false) &&
-            (pTable->KeyTable[i].GroupKey[2].bKeyValid == false) &&
-            (pTable->KeyTable[i].GroupKey[3].bKeyValid == false)
-            ) {
-
-            pTable->KeyTable[i].bInUse = false;
-            pTable->KeyTable[i].wKeyCtl = 0;
-            pTable->KeyTable[i].bSoftWEP = false;
-            MACvDisableKeyEntry(dwIoBase, i);
-        }
-    }
+	int i;
+
+	for (i = 0; i < MAX_KEY_TABLE; i++) {
+		if ((pTable->KeyTable[i].bInUse == true) &&
+		    (pTable->KeyTable[i].PairwiseKey.bKeyValid == false) &&
+		    (pTable->KeyTable[i].GroupKey[0].bKeyValid == false) &&
+		    (pTable->KeyTable[i].GroupKey[1].bKeyValid == false) &&
+		    (pTable->KeyTable[i].GroupKey[2].bKeyValid == false) &&
+		    (pTable->KeyTable[i].GroupKey[3].bKeyValid == false)
+) {
+			pTable->KeyTable[i].bInUse = false;
+			pTable->KeyTable[i].wKeyCtl = 0;
+			pTable->KeyTable[i].bSoftWEP = false;
+			MACvDisableKeyEntry(dwIoBase, i);
+		}
+	}
 }
 
-
 /*---------------------  Export Functions  --------------------------*/
 
-
 /*
  * Description: Init Key management table
  *
@@ -96,27 +93,26 @@ s_vCheckKeyTableValid (PSKeyManagement pTable, unsigned long dwIoBase)
  * Return Value: none
  *
  */
-void KeyvInitTable (PSKeyManagement pTable, unsigned long dwIoBase)
+void KeyvInitTable(PSKeyManagement pTable, unsigned long dwIoBase)
 {
-    int i;
-    int jj;
-
-    for (i=0;i<MAX_KEY_TABLE;i++) {
-        pTable->KeyTable[i].bInUse = false;
-        pTable->KeyTable[i].PairwiseKey.bKeyValid = false;
-        pTable->KeyTable[i].PairwiseKey.pvKeyTable = (void *)&pTable->KeyTable[i];
-        for (jj=0; jj < MAX_GROUP_KEY; jj++) {
-            pTable->KeyTable[i].GroupKey[jj].bKeyValid = false;
-            pTable->KeyTable[i].GroupKey[jj].pvKeyTable = (void *)&pTable->KeyTable[i];
-        }
-        pTable->KeyTable[i].wKeyCtl = 0;
-        pTable->KeyTable[i].dwGTKeyIndex = 0;
-        pTable->KeyTable[i].bSoftWEP = false;
-        MACvDisableKeyEntry(dwIoBase, i);
-    }
+	int i;
+	int jj;
+
+	for (i = 0; i < MAX_KEY_TABLE; i++) {
+		pTable->KeyTable[i].bInUse = false;
+		pTable->KeyTable[i].PairwiseKey.bKeyValid = false;
+		pTable->KeyTable[i].PairwiseKey.pvKeyTable = (void *)&pTable->KeyTable[i];
+		for (jj = 0; jj < MAX_GROUP_KEY; jj++) {
+			pTable->KeyTable[i].GroupKey[jj].bKeyValid = false;
+			pTable->KeyTable[i].GroupKey[jj].pvKeyTable = (void *)&pTable->KeyTable[i];
+		}
+		pTable->KeyTable[i].wKeyCtl = 0;
+		pTable->KeyTable[i].dwGTKeyIndex = 0;
+		pTable->KeyTable[i].bSoftWEP = false;
+		MACvDisableKeyEntry(dwIoBase, i);
+	}
 }
 
-
 /*
  * Description: Get Key from table
  *
@@ -131,47 +127,43 @@ void KeyvInitTable (PSKeyManagement pTable, unsigned long dwIoBase)
  * Return Value: true if found otherwise false
  *
  */
-bool KeybGetKey (
-    PSKeyManagement pTable,
-    unsigned char *pbyBSSID,
-    unsigned long dwKeyIndex,
-    PSKeyItem       *pKey
-    )
+bool KeybGetKey(
+	PSKeyManagement pTable,
+	unsigned char *pbyBSSID,
+	unsigned long dwKeyIndex,
+	PSKeyItem       *pKey
+)
 {
-    int i;
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"KeybGetKey() \n");
-
-    *pKey = NULL;
-    for (i=0;i<MAX_KEY_TABLE;i++) {
-        if ((pTable->KeyTable[i].bInUse == true) &&
-            !compare_ether_addr(pTable->KeyTable[i].abyBSSID, pbyBSSID)) {
-            if (dwKeyIndex == 0xFFFFFFFF) {
-                if (pTable->KeyTable[i].PairwiseKey.bKeyValid == true) {
-                    *pKey = &(pTable->KeyTable[i].PairwiseKey);
-                    return (true);
-                }
-                else {
-                    return (false);
-                }
-            } else if (dwKeyIndex < MAX_GROUP_KEY) {
-                if (pTable->KeyTable[i].GroupKey[dwKeyIndex].bKeyValid == true) {
-                    *pKey = &(pTable->KeyTable[i].GroupKey[dwKeyIndex]);
-                    return (true);
-                }
-                else {
-                    return (false);
-                }
-            }
-            else {
-                return (false);
-            }
-        }
-    }
-    return (false);
+	int i;
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "KeybGetKey() \n");
+
+	*pKey = NULL;
+	for (i = 0; i < MAX_KEY_TABLE; i++) {
+		if ((pTable->KeyTable[i].bInUse == true) &&
+		    !compare_ether_addr(pTable->KeyTable[i].abyBSSID, pbyBSSID)) {
+			if (dwKeyIndex == 0xFFFFFFFF) {
+				if (pTable->KeyTable[i].PairwiseKey.bKeyValid == true) {
+					*pKey = &(pTable->KeyTable[i].PairwiseKey);
+					return true;
+				} else {
+					return false;
+				}
+			} else if (dwKeyIndex < MAX_GROUP_KEY) {
+				if (pTable->KeyTable[i].GroupKey[dwKeyIndex].bKeyValid == true) {
+					*pKey = &(pTable->KeyTable[i].GroupKey[dwKeyIndex]);
+					return true;
+				} else {
+					return false;
+				}
+			} else {
+				return false;
+			}
+		}
+	}
+	return false;
 }
 
-
 /*
  * Description: Set Key to table
  *
@@ -189,165 +181,162 @@ bool KeybGetKey (
  * Return Value: true if success otherwise false
  *
  */
-bool KeybSetKey (
-    PSKeyManagement pTable,
-    unsigned char *pbyBSSID,
-    unsigned long dwKeyIndex,
-    unsigned long uKeyLength,
-    PQWORD          pKeyRSC,
-    unsigned char *pbyKey,
-    unsigned char byKeyDecMode,
-    unsigned long dwIoBase,
-    unsigned char byLocalID
-    )
+bool KeybSetKey(
+	PSKeyManagement pTable,
+	unsigned char *pbyBSSID,
+	unsigned long dwKeyIndex,
+	unsigned long uKeyLength,
+	PQWORD          pKeyRSC,
+	unsigned char *pbyKey,
+	unsigned char byKeyDecMode,
+	unsigned long dwIoBase,
+	unsigned char byLocalID
+)
 {
-    int         i,j;
-    unsigned int ii;
-    PSKeyItem   pKey;
-    unsigned int uKeyIdx;
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Enter KeybSetKey: %lX\n", dwKeyIndex);
-
-    j = (MAX_KEY_TABLE-1);
-    for (i=0;i<(MAX_KEY_TABLE-1);i++) {
-        if ((pTable->KeyTable[i].bInUse == false) &&
-            (j == (MAX_KEY_TABLE-1))) {
-            // found empty table
-            j = i;
-        }
-        if ((pTable->KeyTable[i].bInUse == true) &&
-            !compare_ether_addr(pTable->KeyTable[i].abyBSSID, pbyBSSID)) {
-            // found table already exist
-            if ((dwKeyIndex & PAIRWISE_KEY) != 0) {
-                // Pairwise key
-                pKey = &(pTable->KeyTable[i].PairwiseKey);
-                pTable->KeyTable[i].wKeyCtl &= 0xFFF0;          // clear pairwise key control filed
-                pTable->KeyTable[i].wKeyCtl |= byKeyDecMode;
-                uKeyIdx = 4;                                    // use HW key entry 4 for pairwise key
-            } else {
-                // Group key
-                if ((dwKeyIndex & 0x000000FF) >= MAX_GROUP_KEY)
-                    return (false);
-                pKey = &(pTable->KeyTable[i].GroupKey[dwKeyIndex & 0x000000FF]);
-                if ((dwKeyIndex & TRANSMIT_KEY) != 0)  {
-                    // Group transmit key
-                    pTable->KeyTable[i].dwGTKeyIndex = dwKeyIndex;
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Group transmit key(R)[%lX]: %d\n", pTable->KeyTable[i].dwGTKeyIndex, i);
-                }
-                pTable->KeyTable[i].wKeyCtl &= 0xFF0F;          // clear group key control filed
-                pTable->KeyTable[i].wKeyCtl |= (byKeyDecMode << 4);
-                pTable->KeyTable[i].wKeyCtl |= 0x0040;          // use group key for group address
-                uKeyIdx = (dwKeyIndex & 0x000000FF);
-            }
-            pTable->KeyTable[i].wKeyCtl |= 0x8000;              // enable on-fly
-
-            pKey->bKeyValid = true;
-            pKey->uKeyLength = uKeyLength;
-            pKey->dwKeyIndex = dwKeyIndex;
-            pKey->byCipherSuite = byKeyDecMode;
-            memcpy(pKey->abyKey, pbyKey, uKeyLength);
-            if (byKeyDecMode == KEY_CTL_WEP) {
-                if (uKeyLength == WLAN_WEP40_KEYLEN)
-                    pKey->abyKey[15] &= 0x7F;
-                if (uKeyLength == WLAN_WEP104_KEYLEN)
-                    pKey->abyKey[15] |= 0x80;
-            }
-            MACvSetKeyEntry(dwIoBase, pTable->KeyTable[i].wKeyCtl, i, uKeyIdx, pbyBSSID, (unsigned long *)pKey->abyKey, byLocalID);
-
-            if ((dwKeyIndex & USE_KEYRSC) == 0) {
-                // RSC set by NIC
-		    memset(&(pKey->KeyRSC), 0, sizeof(QWORD));
-            }
-            else {
-                memcpy(&(pKey->KeyRSC), pKeyRSC,  sizeof(QWORD));
-            }
-            pKey->dwTSC47_16 = 0;
-            pKey->wTSC15_0 = 0;
-
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"KeybSetKey(R): \n");
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pKey->bKeyValid: %d\n ", pKey->bKeyValid);
-            //DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pKey->uKeyLength: %d\n ", pKey->uKeyLength);
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pKey->abyKey: ");
-            for (ii = 0; ii < pKey->uKeyLength; ii++) {
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", pKey->abyKey[ii]);
-            }
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"\n");
-
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pKey->dwTSC47_16: %lx\n ", pKey->dwTSC47_16);
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pKey->wTSC15_0: %x\n ", pKey->wTSC15_0);
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pKey->dwKeyIndex: %lx\n ", pKey->dwKeyIndex);
-
-            return (true);
-        }
-    }
-    if (j < (MAX_KEY_TABLE-1)) {
-        memcpy(pTable->KeyTable[j].abyBSSID,pbyBSSID,ETH_ALEN);
-        pTable->KeyTable[j].bInUse = true;
-        if ((dwKeyIndex & PAIRWISE_KEY) != 0)  {
-            // Pairwise key
-            pKey = &(pTable->KeyTable[j].PairwiseKey);
-            pTable->KeyTable[j].wKeyCtl &= 0xFFF0;          // clear pairwise key control filed
-            pTable->KeyTable[j].wKeyCtl |= byKeyDecMode;
-            uKeyIdx = 4;                                    // use HW key entry 4 for pairwise key
-        } else {
-            // Group key
-            if ((dwKeyIndex & 0x000000FF) >= MAX_GROUP_KEY)
-                return (false);
-            pKey = &(pTable->KeyTable[j].GroupKey[dwKeyIndex & 0x000000FF]);
-            if ((dwKeyIndex & TRANSMIT_KEY) != 0)  {
-                // Group transmit key
-                pTable->KeyTable[j].dwGTKeyIndex = dwKeyIndex;
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Group transmit key(N)[%lX]: %d\n", pTable->KeyTable[j].dwGTKeyIndex, j);
-            }
-            pTable->KeyTable[j].wKeyCtl &= 0xFF0F;          // clear group key control filed
-            pTable->KeyTable[j].wKeyCtl |= (byKeyDecMode << 4);
-            pTable->KeyTable[j].wKeyCtl |= 0x0040;          // use group key for group address
-            uKeyIdx = (dwKeyIndex & 0x000000FF);
-        }
-        pTable->KeyTable[j].wKeyCtl |= 0x8000;              // enable on-fly
-
-        pKey->bKeyValid = true;
-        pKey->uKeyLength = uKeyLength;
-        pKey->dwKeyIndex = dwKeyIndex;
-        pKey->byCipherSuite = byKeyDecMode;
-        memcpy(pKey->abyKey, pbyKey, uKeyLength);
-        if (byKeyDecMode == KEY_CTL_WEP) {
-            if (uKeyLength == WLAN_WEP40_KEYLEN)
-                pKey->abyKey[15] &= 0x7F;
-            if (uKeyLength == WLAN_WEP104_KEYLEN)
-                pKey->abyKey[15] |= 0x80;
-        }
-        MACvSetKeyEntry(dwIoBase, pTable->KeyTable[j].wKeyCtl, j, uKeyIdx, pbyBSSID, (unsigned long *)pKey->abyKey, byLocalID);
-
-        if ((dwKeyIndex & USE_KEYRSC) == 0) {
-            // RSC set by NIC
-		memset(&(pKey->KeyRSC), 0, sizeof(QWORD));
-        }
-        else {
-            memcpy(&(pKey->KeyRSC), pKeyRSC,  sizeof(QWORD));
-        }
-        pKey->dwTSC47_16 = 0;
-        pKey->wTSC15_0 = 0;
-
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"KeybSetKey(N): \n");
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pKey->bKeyValid: %d\n ", pKey->bKeyValid);
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pKey->uKeyLength: %d\n ", (int)pKey->uKeyLength);
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pKey->abyKey: ");
-        for (ii = 0; ii < pKey->uKeyLength; ii++) {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", pKey->abyKey[ii]);
-        }
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"\n");
-
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pKey->dwTSC47_16: %lx\n ", pKey->dwTSC47_16);
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pKey->wTSC15_0: %x\n ", pKey->wTSC15_0);
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pKey->dwKeyIndex: %lx\n ", pKey->dwKeyIndex);
-
-        return (true);
-    }
-    return (false);
+	int i, j;
+	unsigned int ii;
+	PSKeyItem   pKey;
+	unsigned int uKeyIdx;
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Enter KeybSetKey: %lX\n", dwKeyIndex);
+
+	j = (MAX_KEY_TABLE-1);
+	for (i = 0; i < (MAX_KEY_TABLE - 1); i++) {
+		if ((pTable->KeyTable[i].bInUse == false) &&
+		    (j == (MAX_KEY_TABLE-1))) {
+			// found empty table
+			j = i;
+		}
+		if ((pTable->KeyTable[i].bInUse == true) &&
+		    !compare_ether_addr(pTable->KeyTable[i].abyBSSID, pbyBSSID)) {
+			// found table already exist
+			if ((dwKeyIndex & PAIRWISE_KEY) != 0) {
+				// Pairwise key
+				pKey = &(pTable->KeyTable[i].PairwiseKey);
+				pTable->KeyTable[i].wKeyCtl &= 0xFFF0;          // clear pairwise key control filed
+				pTable->KeyTable[i].wKeyCtl |= byKeyDecMode;
+				uKeyIdx = 4;                                    // use HW key entry 4 for pairwise key
+			} else {
+				// Group key
+				if ((dwKeyIndex & 0x000000FF) >= MAX_GROUP_KEY)
+					return false;
+				pKey = &(pTable->KeyTable[i].GroupKey[dwKeyIndex & 0x000000FF]);
+				if ((dwKeyIndex & TRANSMIT_KEY) != 0)  {
+					// Group transmit key
+					pTable->KeyTable[i].dwGTKeyIndex = dwKeyIndex;
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Group transmit key(R)[%lX]: %d\n", pTable->KeyTable[i].dwGTKeyIndex, i);
+				}
+				pTable->KeyTable[i].wKeyCtl &= 0xFF0F;          // clear group key control filed
+				pTable->KeyTable[i].wKeyCtl |= (byKeyDecMode << 4);
+				pTable->KeyTable[i].wKeyCtl |= 0x0040;          // use group key for group address
+				uKeyIdx = (dwKeyIndex & 0x000000FF);
+			}
+			pTable->KeyTable[i].wKeyCtl |= 0x8000;              // enable on-fly
+
+			pKey->bKeyValid = true;
+			pKey->uKeyLength = uKeyLength;
+			pKey->dwKeyIndex = dwKeyIndex;
+			pKey->byCipherSuite = byKeyDecMode;
+			memcpy(pKey->abyKey, pbyKey, uKeyLength);
+			if (byKeyDecMode == KEY_CTL_WEP) {
+				if (uKeyLength == WLAN_WEP40_KEYLEN)
+					pKey->abyKey[15] &= 0x7F;
+				if (uKeyLength == WLAN_WEP104_KEYLEN)
+					pKey->abyKey[15] |= 0x80;
+			}
+			MACvSetKeyEntry(dwIoBase, pTable->KeyTable[i].wKeyCtl, i, uKeyIdx, pbyBSSID, (unsigned long *)pKey->abyKey, byLocalID);
+
+			if ((dwKeyIndex & USE_KEYRSC) == 0) {
+				// RSC set by NIC
+				memset(&(pKey->KeyRSC), 0, sizeof(QWORD));
+			} else {
+				memcpy(&(pKey->KeyRSC), pKeyRSC,  sizeof(QWORD));
+			}
+			pKey->dwTSC47_16 = 0;
+			pKey->wTSC15_0 = 0;
+
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "KeybSetKey(R): \n");
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->bKeyValid: %d\n ", pKey->bKeyValid);
+			//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->uKeyLength: %d\n ", pKey->uKeyLength);
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->abyKey: ");
+			for (ii = 0; ii < pKey->uKeyLength; ii++) {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", pKey->abyKey[ii]);
+			}
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
+
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->dwTSC47_16: %lx\n ", pKey->dwTSC47_16);
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->wTSC15_0: %x\n ", pKey->wTSC15_0);
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->dwKeyIndex: %lx\n ", pKey->dwKeyIndex);
+
+			return true;
+		}
+	}
+	if (j < (MAX_KEY_TABLE-1)) {
+		memcpy(pTable->KeyTable[j].abyBSSID, pbyBSSID, ETH_ALEN);
+		pTable->KeyTable[j].bInUse = true;
+		if ((dwKeyIndex & PAIRWISE_KEY) != 0)  {
+			// Pairwise key
+			pKey = &(pTable->KeyTable[j].PairwiseKey);
+			pTable->KeyTable[j].wKeyCtl &= 0xFFF0;          // clear pairwise key control filed
+			pTable->KeyTable[j].wKeyCtl |= byKeyDecMode;
+			uKeyIdx = 4;                                    // use HW key entry 4 for pairwise key
+		} else {
+			// Group key
+			if ((dwKeyIndex & 0x000000FF) >= MAX_GROUP_KEY)
+				return false;
+			pKey = &(pTable->KeyTable[j].GroupKey[dwKeyIndex & 0x000000FF]);
+			if ((dwKeyIndex & TRANSMIT_KEY) != 0)  {
+				// Group transmit key
+				pTable->KeyTable[j].dwGTKeyIndex = dwKeyIndex;
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Group transmit key(N)[%lX]: %d\n", pTable->KeyTable[j].dwGTKeyIndex, j);
+			}
+			pTable->KeyTable[j].wKeyCtl &= 0xFF0F;          // clear group key control filed
+			pTable->KeyTable[j].wKeyCtl |= (byKeyDecMode << 4);
+			pTable->KeyTable[j].wKeyCtl |= 0x0040;          // use group key for group address
+			uKeyIdx = (dwKeyIndex & 0x000000FF);
+		}
+		pTable->KeyTable[j].wKeyCtl |= 0x8000;              // enable on-fly
+
+		pKey->bKeyValid = true;
+		pKey->uKeyLength = uKeyLength;
+		pKey->dwKeyIndex = dwKeyIndex;
+		pKey->byCipherSuite = byKeyDecMode;
+		memcpy(pKey->abyKey, pbyKey, uKeyLength);
+		if (byKeyDecMode == KEY_CTL_WEP) {
+			if (uKeyLength == WLAN_WEP40_KEYLEN)
+				pKey->abyKey[15] &= 0x7F;
+			if (uKeyLength == WLAN_WEP104_KEYLEN)
+				pKey->abyKey[15] |= 0x80;
+		}
+		MACvSetKeyEntry(dwIoBase, pTable->KeyTable[j].wKeyCtl, j, uKeyIdx, pbyBSSID, (unsigned long *)pKey->abyKey, byLocalID);
+
+		if ((dwKeyIndex & USE_KEYRSC) == 0) {
+			// RSC set by NIC
+			memset(&(pKey->KeyRSC), 0, sizeof(QWORD));
+		} else {
+			memcpy(&(pKey->KeyRSC), pKeyRSC,  sizeof(QWORD));
+		}
+		pKey->dwTSC47_16 = 0;
+		pKey->wTSC15_0 = 0;
+
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "KeybSetKey(N): \n");
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->bKeyValid: %d\n ", pKey->bKeyValid);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->uKeyLength: %d\n ", (int)pKey->uKeyLength);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->abyKey: ");
+		for (ii = 0; ii < pKey->uKeyLength; ii++) {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", pKey->abyKey[ii]);
+		}
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
+
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->dwTSC47_16: %lx\n ", pKey->dwTSC47_16);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->wTSC15_0: %x\n ", pKey->wTSC15_0);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->dwKeyIndex: %lx\n ", pKey->dwKeyIndex);
+
+		return true;
+	}
+	return false;
 }
 
-
 /*
  * Description: Remove Key from table
  *
@@ -362,66 +351,61 @@ bool KeybSetKey (
  * Return Value: true if success otherwise false
  *
  */
-bool KeybRemoveKey (
-    PSKeyManagement pTable,
-    unsigned char *pbyBSSID,
-    unsigned long dwKeyIndex,
-    unsigned long dwIoBase
-    )
+bool KeybRemoveKey(
+	PSKeyManagement pTable,
+	unsigned char *pbyBSSID,
+	unsigned long dwKeyIndex,
+	unsigned long dwIoBase
+)
 {
-    int  i;
-
-    if (is_broadcast_ether_addr(pbyBSSID)) {
-        // delete all keys
-        if ((dwKeyIndex & PAIRWISE_KEY) != 0) {
-            for (i=0;i<MAX_KEY_TABLE;i++) {
-                pTable->KeyTable[i].PairwiseKey.bKeyValid = false;
-            }
-            s_vCheckKeyTableValid(pTable, dwIoBase);
-            return true;
-        }
-        else if ((dwKeyIndex & 0x000000FF) < MAX_GROUP_KEY) {
-            for (i=0;i<MAX_KEY_TABLE;i++) {
-                pTable->KeyTable[i].GroupKey[dwKeyIndex & 0x000000FF].bKeyValid = false;
-                if ((dwKeyIndex & 0x7FFFFFFF) == (pTable->KeyTable[i].dwGTKeyIndex & 0x7FFFFFFF)) {
-                    // remove Group transmit key
-                    pTable->KeyTable[i].dwGTKeyIndex = 0;
-                }
-            }
-            s_vCheckKeyTableValid(pTable, dwIoBase);
-            return true;
-        }
-        else {
-            return false;
-        }
-    }
-
-    for (i=0;i<MAX_KEY_TABLE;i++) {
-        if ((pTable->KeyTable[i].bInUse == true) &&
-            !compare_ether_addr(pTable->KeyTable[i].abyBSSID, pbyBSSID)) {
-            if ((dwKeyIndex & PAIRWISE_KEY) != 0) {
-                pTable->KeyTable[i].PairwiseKey.bKeyValid = false;
-                s_vCheckKeyTableValid(pTable, dwIoBase);
-                return (true);
-            }
-            else if ((dwKeyIndex & 0x000000FF) < MAX_GROUP_KEY) {
-                pTable->KeyTable[i].GroupKey[dwKeyIndex & 0x000000FF].bKeyValid = false;
-                if ((dwKeyIndex & 0x7FFFFFFF) == (pTable->KeyTable[i].dwGTKeyIndex & 0x7FFFFFFF)) {
-                    // remove Group transmit key
-                    pTable->KeyTable[i].dwGTKeyIndex = 0;
-                }
-                s_vCheckKeyTableValid(pTable, dwIoBase);
-                return (true);
-            }
-            else {
-                return (false);
-            }
-        }
-    }
-    return (false);
+	int  i;
+
+	if (is_broadcast_ether_addr(pbyBSSID)) {
+		// delete all keys
+		if ((dwKeyIndex & PAIRWISE_KEY) != 0) {
+			for (i = 0; i < MAX_KEY_TABLE; i++) {
+				pTable->KeyTable[i].PairwiseKey.bKeyValid = false;
+			}
+			s_vCheckKeyTableValid(pTable, dwIoBase);
+			return true;
+		} else if ((dwKeyIndex & 0x000000FF) < MAX_GROUP_KEY) {
+			for (i = 0; i < MAX_KEY_TABLE; i++) {
+				pTable->KeyTable[i].GroupKey[dwKeyIndex & 0x000000FF].bKeyValid = false;
+				if ((dwKeyIndex & 0x7FFFFFFF) == (pTable->KeyTable[i].dwGTKeyIndex & 0x7FFFFFFF)) {
+					// remove Group transmit key
+					pTable->KeyTable[i].dwGTKeyIndex = 0;
+				}
+			}
+			s_vCheckKeyTableValid(pTable, dwIoBase);
+			return true;
+		} else {
+			return false;
+		}
+	}
+
+	for (i = 0; i < MAX_KEY_TABLE; i++) {
+		if ((pTable->KeyTable[i].bInUse == true) &&
+		    !compare_ether_addr(pTable->KeyTable[i].abyBSSID, pbyBSSID)) {
+			if ((dwKeyIndex & PAIRWISE_KEY) != 0) {
+				pTable->KeyTable[i].PairwiseKey.bKeyValid = false;
+				s_vCheckKeyTableValid(pTable, dwIoBase);
+				return true;
+			} else if ((dwKeyIndex & 0x000000FF) < MAX_GROUP_KEY) {
+				pTable->KeyTable[i].GroupKey[dwKeyIndex & 0x000000FF].bKeyValid = false;
+				if ((dwKeyIndex & 0x7FFFFFFF) == (pTable->KeyTable[i].dwGTKeyIndex & 0x7FFFFFFF)) {
+					// remove Group transmit key
+					pTable->KeyTable[i].dwGTKeyIndex = 0;
+				}
+				s_vCheckKeyTableValid(pTable, dwIoBase);
+				return true;
+			} else {
+				return false;
+			}
+		}
+	}
+	return false;
 }
 
-
 /*
  * Description: Remove Key from table
  *
@@ -435,27 +419,27 @@ bool KeybRemoveKey (
  * Return Value: true if success otherwise false
  *
  */
-bool KeybRemoveAllKey (
-    PSKeyManagement pTable,
-    unsigned char *pbyBSSID,
-    unsigned long dwIoBase
-    )
+bool KeybRemoveAllKey(
+	PSKeyManagement pTable,
+	unsigned char *pbyBSSID,
+	unsigned long dwIoBase
+)
 {
-    int  i,u;
-
-    for (i=0;i<MAX_KEY_TABLE;i++) {
-        if ((pTable->KeyTable[i].bInUse == true) &&
-            !compare_ether_addr(pTable->KeyTable[i].abyBSSID, pbyBSSID)) {
-            pTable->KeyTable[i].PairwiseKey.bKeyValid = false;
-            for(u=0;u<MAX_GROUP_KEY;u++) {
-                pTable->KeyTable[i].GroupKey[u].bKeyValid = false;
-            }
-            pTable->KeyTable[i].dwGTKeyIndex = 0;
-            s_vCheckKeyTableValid(pTable, dwIoBase);
-            return (true);
-        }
-    }
-    return (false);
+	int i, u;
+
+	for (i = 0; i < MAX_KEY_TABLE; i++) {
+		if ((pTable->KeyTable[i].bInUse == true) &&
+		    !compare_ether_addr(pTable->KeyTable[i].abyBSSID, pbyBSSID)) {
+			pTable->KeyTable[i].PairwiseKey.bKeyValid = false;
+			for (u = 0; u < MAX_GROUP_KEY; u++) {
+				pTable->KeyTable[i].GroupKey[u].bKeyValid = false;
+			}
+			pTable->KeyTable[i].dwGTKeyIndex = 0;
+			s_vCheckKeyTableValid(pTable, dwIoBase);
+			return true;
+		}
+	}
+	return false;
 }
 
 /*
@@ -470,38 +454,37 @@ bool KeybRemoveAllKey (
  * Return Value: true if success otherwise false
  *
  */
-void KeyvRemoveWEPKey (
-    PSKeyManagement pTable,
-    unsigned long dwKeyIndex,
-    unsigned long dwIoBase
-    )
+void KeyvRemoveWEPKey(
+	PSKeyManagement pTable,
+	unsigned long dwKeyIndex,
+	unsigned long dwIoBase
+)
 {
-
-   if ((dwKeyIndex & 0x000000FF) < MAX_GROUP_KEY) {
-        if (pTable->KeyTable[MAX_KEY_TABLE-1].bInUse == true) {
-            if (pTable->KeyTable[MAX_KEY_TABLE-1].GroupKey[dwKeyIndex & 0x000000FF].byCipherSuite == KEY_CTL_WEP) {
-                pTable->KeyTable[MAX_KEY_TABLE-1].GroupKey[dwKeyIndex & 0x000000FF].bKeyValid = false;
-                if ((dwKeyIndex & 0x7FFFFFFF) == (pTable->KeyTable[MAX_KEY_TABLE-1].dwGTKeyIndex & 0x7FFFFFFF)) {
-                    // remove Group transmit key
-                    pTable->KeyTable[MAX_KEY_TABLE-1].dwGTKeyIndex = 0;
-                }
-            }
-        }
-        s_vCheckKeyTableValid(pTable, dwIoBase);
-    }
-    return;
+	if ((dwKeyIndex & 0x000000FF) < MAX_GROUP_KEY) {
+		if (pTable->KeyTable[MAX_KEY_TABLE-1].bInUse == true) {
+			if (pTable->KeyTable[MAX_KEY_TABLE-1].GroupKey[dwKeyIndex & 0x000000FF].byCipherSuite == KEY_CTL_WEP) {
+				pTable->KeyTable[MAX_KEY_TABLE-1].GroupKey[dwKeyIndex & 0x000000FF].bKeyValid = false;
+				if ((dwKeyIndex & 0x7FFFFFFF) == (pTable->KeyTable[MAX_KEY_TABLE-1].dwGTKeyIndex & 0x7FFFFFFF)) {
+					// remove Group transmit key
+					pTable->KeyTable[MAX_KEY_TABLE-1].dwGTKeyIndex = 0;
+				}
+			}
+		}
+		s_vCheckKeyTableValid(pTable, dwIoBase);
+	}
+	return;
 }
 
-void KeyvRemoveAllWEPKey (
-    PSKeyManagement pTable,
-    unsigned long dwIoBase
-    )
+void KeyvRemoveAllWEPKey(
+	PSKeyManagement pTable,
+	unsigned long dwIoBase
+)
 {
-    int i;
+	int i;
 
-    for(i=0;i<MAX_GROUP_KEY;i++) {
-        KeyvRemoveWEPKey(pTable, i, dwIoBase);
-    }
+	for (i = 0; i < MAX_GROUP_KEY; i++) {
+		KeyvRemoveWEPKey(pTable, i, dwIoBase);
+	}
 }
 
 /*
@@ -517,74 +500,68 @@ void KeyvRemoveAllWEPKey (
  * Return Value: true if found otherwise false
  *
  */
-bool KeybGetTransmitKey (
-    PSKeyManagement pTable,
-    unsigned char *pbyBSSID,
-    unsigned long dwKeyType,
-    PSKeyItem       *pKey
-    )
+bool KeybGetTransmitKey(
+	PSKeyManagement pTable,
+	unsigned char *pbyBSSID,
+	unsigned long dwKeyType,
+	PSKeyItem       *pKey
+)
 {
-    int i, ii;
-
-    *pKey = NULL;
-    for (i=0;i<MAX_KEY_TABLE;i++) {
-        if ((pTable->KeyTable[i].bInUse == true) &&
-            !compare_ether_addr(pTable->KeyTable[i].abyBSSID, pbyBSSID)) {
-
-            if (dwKeyType == PAIRWISE_KEY) {
-
-                if (pTable->KeyTable[i].PairwiseKey.bKeyValid == true) {
-                    *pKey = &(pTable->KeyTable[i].PairwiseKey);
-
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"KeybGetTransmitKey:");
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"PAIRWISE_KEY: KeyTable.abyBSSID: ");
-                    for (ii = 0; ii < 6; ii++) {
-                        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"%x ", pTable->KeyTable[i].abyBSSID[ii]);
-                    }
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"\n");
-
-
-                    return (true);
-                }
-                else {
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"PairwiseKey.bKeyValid == false\n");
-                    return (false);
-                }
-            } // End of Type == PAIRWISE
-            else {
-                if (pTable->KeyTable[i].dwGTKeyIndex == 0) {
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ERROR: dwGTKeyIndex == 0 !!!\n");
-                    return false;
-                }
-                if (pTable->KeyTable[i].GroupKey[(pTable->KeyTable[i].dwGTKeyIndex&0x000000FF)].bKeyValid == true) {
-                    *pKey = &(pTable->KeyTable[i].GroupKey[(pTable->KeyTable[i].dwGTKeyIndex&0x000000FF)]);
-
-                        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"KeybGetTransmitKey:");
-                        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"GROUP_KEY: KeyTable.abyBSSID\n");
-                        for (ii = 0; ii < 6; ii++) {
-                            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"%x ", pTable->KeyTable[i].abyBSSID[ii]);
-                        }
-                        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"\n");
-                        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"dwGTKeyIndex: %lX\n", pTable->KeyTable[i].dwGTKeyIndex);
-
-                    return (true);
-                }
-                else {
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"GroupKey.bKeyValid == false\n");
-                    return (false);
-                }
-            } // End of Type = GROUP
-        } // BSSID match
-    }
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ERROR: NO Match BSSID !!! ");
-    for (ii = 0; ii < 6; ii++) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"%02x ", *(pbyBSSID+ii));
-    }
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"\n");
-    return (false);
+	int i, ii;
+
+	*pKey = NULL;
+	for (i = 0; i < MAX_KEY_TABLE; i++) {
+		if ((pTable->KeyTable[i].bInUse == true) &&
+		    !compare_ether_addr(pTable->KeyTable[i].abyBSSID, pbyBSSID)) {
+			if (dwKeyType == PAIRWISE_KEY) {
+				if (pTable->KeyTable[i].PairwiseKey.bKeyValid == true) {
+					*pKey = &(pTable->KeyTable[i].PairwiseKey);
+
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "KeybGetTransmitKey:");
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "PAIRWISE_KEY: KeyTable.abyBSSID: ");
+					for (ii = 0; ii < 6; ii++) {
+						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%x ", pTable->KeyTable[i].abyBSSID[ii]);
+					}
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
+
+					return true;
+				} else {
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "PairwiseKey.bKeyValid == false\n");
+					return false;
+				}
+			} // End of Type == PAIRWISE
+			else {
+				if (pTable->KeyTable[i].dwGTKeyIndex == 0) {
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ERROR: dwGTKeyIndex == 0 !!!\n");
+					return false;
+				}
+				if (pTable->KeyTable[i].GroupKey[(pTable->KeyTable[i].dwGTKeyIndex&0x000000FF)].bKeyValid == true) {
+					*pKey = &(pTable->KeyTable[i].GroupKey[(pTable->KeyTable[i].dwGTKeyIndex&0x000000FF)]);
+
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "KeybGetTransmitKey:");
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "GROUP_KEY: KeyTable.abyBSSID\n");
+					for (ii = 0; ii < 6; ii++) {
+						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%x ", pTable->KeyTable[i].abyBSSID[ii]);
+					}
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dwGTKeyIndex: %lX\n", pTable->KeyTable[i].dwGTKeyIndex);
+
+					return true;
+				} else {
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "GroupKey.bKeyValid == false\n");
+					return false;
+				}
+			} // End of Type = GROUP
+		} // BSSID match
+	}
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ERROR: NO Match BSSID !!! ");
+	for (ii = 0; ii < 6; ii++) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", *(pbyBSSID+ii));
+	}
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
+	return false;
 }
 
-
 /*
  * Description: Check Pairewise Key
  *
@@ -597,22 +574,22 @@ bool KeybGetTransmitKey (
  * Return Value: true if found otherwise false
  *
  */
-bool KeybCheckPairewiseKey (
-    PSKeyManagement pTable,
-    PSKeyItem       *pKey
-    )
+bool KeybCheckPairewiseKey(
+	PSKeyManagement pTable,
+	PSKeyItem       *pKey
+)
 {
-    int i;
-
-    *pKey = NULL;
-    for (i=0;i<MAX_KEY_TABLE;i++) {
-        if ((pTable->KeyTable[i].bInUse == true) &&
-            (pTable->KeyTable[i].PairwiseKey.bKeyValid == true)) {
-            *pKey = &(pTable->KeyTable[i].PairwiseKey);
-            return (true);
-        }
-    }
-    return (false);
+	int i;
+
+	*pKey = NULL;
+	for (i = 0; i < MAX_KEY_TABLE; i++) {
+		if ((pTable->KeyTable[i].bInUse == true) &&
+		    (pTable->KeyTable[i].PairwiseKey.bKeyValid == true)) {
+			*pKey = &(pTable->KeyTable[i].PairwiseKey);
+			return true;
+		}
+	}
+	return false;
 }
 
 /*
@@ -631,100 +608,97 @@ bool KeybCheckPairewiseKey (
  * Return Value: true if success otherwise false
  *
  */
-bool KeybSetDefaultKey (
-    PSKeyManagement pTable,
-    unsigned long dwKeyIndex,
-    unsigned long uKeyLength,
-    PQWORD          pKeyRSC,
-    unsigned char *pbyKey,
-    unsigned char byKeyDecMode,
-    unsigned long dwIoBase,
-    unsigned char byLocalID
-    )
+bool KeybSetDefaultKey(
+	PSKeyManagement pTable,
+	unsigned long dwKeyIndex,
+	unsigned long uKeyLength,
+	PQWORD          pKeyRSC,
+	unsigned char *pbyKey,
+	unsigned char byKeyDecMode,
+	unsigned long dwIoBase,
+	unsigned char byLocalID
+)
 {
-    unsigned int ii;
-    PSKeyItem   pKey;
-    unsigned int uKeyIdx;
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Enter KeybSetDefaultKey: %1x, %d \n", (int)dwKeyIndex, (int)uKeyLength);
-
-
-    if ((dwKeyIndex & PAIRWISE_KEY) != 0) {                  // Pairwise key
-        return (false);
-    } else if ((dwKeyIndex & 0x000000FF) >= MAX_GROUP_KEY) {
-        return (false);
-    }
-
-    if (uKeyLength > MAX_KEY_LEN)
-	    return false;
-
-    pTable->KeyTable[MAX_KEY_TABLE-1].bInUse = true;
-    for(ii=0;ii<ETH_ALEN;ii++)
-        pTable->KeyTable[MAX_KEY_TABLE-1].abyBSSID[ii] = 0xFF;
-
-    // Group key
-    pKey = &(pTable->KeyTable[MAX_KEY_TABLE-1].GroupKey[dwKeyIndex & 0x000000FF]);
-    if ((dwKeyIndex & TRANSMIT_KEY) != 0)  {
-        // Group transmit key
-        pTable->KeyTable[MAX_KEY_TABLE-1].dwGTKeyIndex = dwKeyIndex;
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Group transmit key(R)[%lX]: %d\n", pTable->KeyTable[MAX_KEY_TABLE-1].dwGTKeyIndex, MAX_KEY_TABLE-1);
-
-    }
-    pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl &= 0x7F00;          // clear all key control filed
-    pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl |= (byKeyDecMode << 4);
-    pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl |= (byKeyDecMode);
-    pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl |= 0x0044;          // use group key for all address
-    uKeyIdx = (dwKeyIndex & 0x000000FF);
-
-    if ((uKeyLength == WLAN_WEP232_KEYLEN) &&
-        (byKeyDecMode == KEY_CTL_WEP)) {
-        pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl |= 0x4000;              // disable on-fly disable address match
-        pTable->KeyTable[MAX_KEY_TABLE-1].bSoftWEP = true;
-    } else {
-        if (pTable->KeyTable[MAX_KEY_TABLE-1].bSoftWEP == false)
-            pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl |= 0xC000;          // enable on-fly disable address match
-    }
-
-    pKey->bKeyValid = true;
-    pKey->uKeyLength = uKeyLength;
-    pKey->dwKeyIndex = dwKeyIndex;
-    pKey->byCipherSuite = byKeyDecMode;
-    memcpy(pKey->abyKey, pbyKey, uKeyLength);
-    if (byKeyDecMode == KEY_CTL_WEP) {
-        if (uKeyLength == WLAN_WEP40_KEYLEN)
-            pKey->abyKey[15] &= 0x7F;
-        if (uKeyLength == WLAN_WEP104_KEYLEN)
-            pKey->abyKey[15] |= 0x80;
-    }
-    MACvSetKeyEntry(dwIoBase, pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl, MAX_KEY_TABLE-1, uKeyIdx, pTable->KeyTable[MAX_KEY_TABLE-1].abyBSSID, (unsigned long *)pKey->abyKey, byLocalID);
-
-    if ((dwKeyIndex & USE_KEYRSC) == 0) {
-        // RSC set by NIC
-	    memset(&(pKey->KeyRSC), 0, sizeof(QWORD));
-    } else {
-        memcpy(&(pKey->KeyRSC), pKeyRSC,  sizeof(QWORD));
-    }
-    pKey->dwTSC47_16 = 0;
-    pKey->wTSC15_0 = 0;
-
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"KeybSetKey(R): \n");
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pKey->bKeyValid: %d\n", pKey->bKeyValid);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pKey->uKeyLength: %d\n", (int)pKey->uKeyLength);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pKey->abyKey: \n");
-    for (ii = 0; ii < pKey->uKeyLength; ii++) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"%x", pKey->abyKey[ii]);
-    }
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"\n");
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pKey->dwTSC47_16: %lx\n", pKey->dwTSC47_16);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pKey->wTSC15_0: %x\n", pKey->wTSC15_0);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pKey->dwKeyIndex: %lx\n", pKey->dwKeyIndex);
-
-    return (true);
+	unsigned int ii;
+	PSKeyItem   pKey;
+	unsigned int uKeyIdx;
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Enter KeybSetDefaultKey: %1x, %d \n", (int)dwKeyIndex, (int)uKeyLength);
+
+	if ((dwKeyIndex & PAIRWISE_KEY) != 0) {                  // Pairwise key
+		return false;
+	} else if ((dwKeyIndex & 0x000000FF) >= MAX_GROUP_KEY) {
+		return false;
+	}
+
+	if (uKeyLength > MAX_KEY_LEN)
+		return false;
+
+	pTable->KeyTable[MAX_KEY_TABLE - 1].bInUse = true;
+	for (ii = 0; ii < ETH_ALEN; ii++)
+		pTable->KeyTable[MAX_KEY_TABLE - 1].abyBSSID[ii] = 0xFF;
+
+	// Group key
+	pKey = &(pTable->KeyTable[MAX_KEY_TABLE - 1].GroupKey[dwKeyIndex & 0x000000FF]);
+	if ((dwKeyIndex & TRANSMIT_KEY) != 0)  {
+		// Group transmit key
+		pTable->KeyTable[MAX_KEY_TABLE-1].dwGTKeyIndex = dwKeyIndex;
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Group transmit key(R)[%lX]: %d\n", pTable->KeyTable[MAX_KEY_TABLE-1].dwGTKeyIndex, MAX_KEY_TABLE-1);
+
+	}
+	pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl &= 0x7F00;          // clear all key control filed
+	pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl |= (byKeyDecMode << 4);
+	pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl |= (byKeyDecMode);
+	pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl |= 0x0044;          // use group key for all address
+	uKeyIdx = (dwKeyIndex & 0x000000FF);
+
+	if ((uKeyLength == WLAN_WEP232_KEYLEN) &&
+	    (byKeyDecMode == KEY_CTL_WEP)) {
+		pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl |= 0x4000;              // disable on-fly disable address match
+		pTable->KeyTable[MAX_KEY_TABLE-1].bSoftWEP = true;
+	} else {
+		if (pTable->KeyTable[MAX_KEY_TABLE-1].bSoftWEP == false)
+			pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl |= 0xC000;          // enable on-fly disable address match
+	}
+
+	pKey->bKeyValid = true;
+	pKey->uKeyLength = uKeyLength;
+	pKey->dwKeyIndex = dwKeyIndex;
+	pKey->byCipherSuite = byKeyDecMode;
+	memcpy(pKey->abyKey, pbyKey, uKeyLength);
+	if (byKeyDecMode == KEY_CTL_WEP) {
+		if (uKeyLength == WLAN_WEP40_KEYLEN)
+			pKey->abyKey[15] &= 0x7F;
+		if (uKeyLength == WLAN_WEP104_KEYLEN)
+			pKey->abyKey[15] |= 0x80;
+	}
+	MACvSetKeyEntry(dwIoBase, pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl, MAX_KEY_TABLE-1, uKeyIdx, pTable->KeyTable[MAX_KEY_TABLE-1].abyBSSID, (unsigned long *)pKey->abyKey, byLocalID);
+
+	if ((dwKeyIndex & USE_KEYRSC) == 0) {
+		// RSC set by NIC
+		memset(&(pKey->KeyRSC), 0, sizeof(QWORD));
+	} else {
+		memcpy(&(pKey->KeyRSC), pKeyRSC,  sizeof(QWORD));
+	}
+	pKey->dwTSC47_16 = 0;
+	pKey->wTSC15_0 = 0;
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "KeybSetKey(R): \n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->bKeyValid: %d\n", pKey->bKeyValid);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->uKeyLength: %d\n", (int)pKey->uKeyLength);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->abyKey: \n");
+	for (ii = 0; ii < pKey->uKeyLength; ii++) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%x", pKey->abyKey[ii]);
+	}
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->dwTSC47_16: %lx\n", pKey->dwTSC47_16);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->wTSC15_0: %x\n", pKey->wTSC15_0);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->dwKeyIndex: %lx\n", pKey->dwKeyIndex);
+
+	return true;
 }
 
-
 /*
  * Description: Set Key to table
  *
@@ -741,86 +715,84 @@ bool KeybSetDefaultKey (
  * Return Value: true if success otherwise false
  *
  */
-bool KeybSetAllGroupKey (
-    PSKeyManagement pTable,
-    unsigned long dwKeyIndex,
-    unsigned long uKeyLength,
-    PQWORD          pKeyRSC,
-    unsigned char *pbyKey,
-    unsigned char byKeyDecMode,
-    unsigned long dwIoBase,
-    unsigned char byLocalID
-    )
+bool KeybSetAllGroupKey(
+	PSKeyManagement pTable,
+	unsigned long dwKeyIndex,
+	unsigned long uKeyLength,
+	PQWORD          pKeyRSC,
+	unsigned char *pbyKey,
+	unsigned char byKeyDecMode,
+	unsigned long dwIoBase,
+	unsigned char byLocalID
+)
 {
-    int         i;
-    unsigned int ii;
-    PSKeyItem   pKey;
-    unsigned int uKeyIdx;
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Enter KeybSetAllGroupKey: %lX\n", dwKeyIndex);
-
-
-    if ((dwKeyIndex & PAIRWISE_KEY) != 0) {                  // Pairwise key
-        return (false);
-    } else if ((dwKeyIndex & 0x000000FF) >= MAX_GROUP_KEY) {
-        return (false);
-    }
-
-    for (i=0; i < MAX_KEY_TABLE-1; i++) {
-        if (pTable->KeyTable[i].bInUse == true) {
-            // found table already exist
-            // Group key
-            pKey = &(pTable->KeyTable[i].GroupKey[dwKeyIndex & 0x000000FF]);
-            if ((dwKeyIndex & TRANSMIT_KEY) != 0)  {
-                // Group transmit key
-                pTable->KeyTable[i].dwGTKeyIndex = dwKeyIndex;
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Group transmit key(R)[%lX]: %d\n", pTable->KeyTable[i].dwGTKeyIndex, i);
-
-            }
-            pTable->KeyTable[i].wKeyCtl &= 0xFF0F;          // clear group key control filed
-            pTable->KeyTable[i].wKeyCtl |= (byKeyDecMode << 4);
-            pTable->KeyTable[i].wKeyCtl |= 0x0040;          // use group key for group address
-            uKeyIdx = (dwKeyIndex & 0x000000FF);
-
-            pTable->KeyTable[i].wKeyCtl |= 0x8000;              // enable on-fly
-
-            pKey->bKeyValid = true;
-            pKey->uKeyLength = uKeyLength;
-            pKey->dwKeyIndex = dwKeyIndex;
-            pKey->byCipherSuite = byKeyDecMode;
-            memcpy(pKey->abyKey, pbyKey, uKeyLength);
-            if (byKeyDecMode == KEY_CTL_WEP) {
-                if (uKeyLength == WLAN_WEP40_KEYLEN)
-                    pKey->abyKey[15] &= 0x7F;
-                if (uKeyLength == WLAN_WEP104_KEYLEN)
-                    pKey->abyKey[15] |= 0x80;
-            }
-            MACvSetKeyEntry(dwIoBase, pTable->KeyTable[i].wKeyCtl, i, uKeyIdx, pTable->KeyTable[i].abyBSSID, (unsigned long *)pKey->abyKey, byLocalID);
-
-            if ((dwKeyIndex & USE_KEYRSC) == 0) {
-                // RSC set by NIC
-		    memset(&(pKey->KeyRSC), 0, sizeof(QWORD));
-            }
-            else {
-                memcpy(&(pKey->KeyRSC), pKeyRSC,  sizeof(QWORD));
-            }
-            pKey->dwTSC47_16 = 0;
-            pKey->wTSC15_0 = 0;
-
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"KeybSetKey(R): \n");
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pKey->bKeyValid: %d\n ", pKey->bKeyValid);
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pKey->uKeyLength: %d\n ", (int)pKey->uKeyLength);
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pKey->abyKey: ");
-            for (ii = 0; ii < pKey->uKeyLength; ii++) {
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"%02x ", pKey->abyKey[ii]);
-            }
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"\n");
-
-            //DBG_PRN_GRP12(("pKey->dwTSC47_16: %lX\n ", pKey->dwTSC47_16));
-            //DBG_PRN_GRP12(("pKey->wTSC15_0: %X\n ", pKey->wTSC15_0));
-            //DBG_PRN_GRP12(("pKey->dwKeyIndex: %lX\n ", pKey->dwKeyIndex));
-
-        } // (pTable->KeyTable[i].bInUse == true)
-    }
-    return (true);
+	int         i;
+	unsigned int ii;
+	PSKeyItem   pKey;
+	unsigned int uKeyIdx;
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Enter KeybSetAllGroupKey: %lX\n", dwKeyIndex);
+
+	if ((dwKeyIndex & PAIRWISE_KEY) != 0) {                  // Pairwise key
+		return false;
+	} else if ((dwKeyIndex & 0x000000FF) >= MAX_GROUP_KEY) {
+		return false;
+	}
+
+	for (i = 0; i < MAX_KEY_TABLE - 1; i++) {
+		if (pTable->KeyTable[i].bInUse == true) {
+			// found table already exist
+			// Group key
+			pKey = &(pTable->KeyTable[i].GroupKey[dwKeyIndex & 0x000000FF]);
+			if ((dwKeyIndex & TRANSMIT_KEY) != 0)  {
+				// Group transmit key
+				pTable->KeyTable[i].dwGTKeyIndex = dwKeyIndex;
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Group transmit key(R)[%lX]: %d\n", pTable->KeyTable[i].dwGTKeyIndex, i);
+
+			}
+			pTable->KeyTable[i].wKeyCtl &= 0xFF0F;          // clear group key control filed
+			pTable->KeyTable[i].wKeyCtl |= (byKeyDecMode << 4);
+			pTable->KeyTable[i].wKeyCtl |= 0x0040;          // use group key for group address
+			uKeyIdx = (dwKeyIndex & 0x000000FF);
+
+			pTable->KeyTable[i].wKeyCtl |= 0x8000;              // enable on-fly
+
+			pKey->bKeyValid = true;
+			pKey->uKeyLength = uKeyLength;
+			pKey->dwKeyIndex = dwKeyIndex;
+			pKey->byCipherSuite = byKeyDecMode;
+			memcpy(pKey->abyKey, pbyKey, uKeyLength);
+			if (byKeyDecMode == KEY_CTL_WEP) {
+				if (uKeyLength == WLAN_WEP40_KEYLEN)
+					pKey->abyKey[15] &= 0x7F;
+				if (uKeyLength == WLAN_WEP104_KEYLEN)
+					pKey->abyKey[15] |= 0x80;
+			}
+			MACvSetKeyEntry(dwIoBase, pTable->KeyTable[i].wKeyCtl, i, uKeyIdx, pTable->KeyTable[i].abyBSSID, (unsigned long *)pKey->abyKey, byLocalID);
+
+			if ((dwKeyIndex & USE_KEYRSC) == 0) {
+				// RSC set by NIC
+				memset(&(pKey->KeyRSC), 0, sizeof(QWORD));
+			} else {
+				memcpy(&(pKey->KeyRSC), pKeyRSC,  sizeof(QWORD));
+			}
+			pKey->dwTSC47_16 = 0;
+			pKey->wTSC15_0 = 0;
+
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "KeybSetKey(R): \n");
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->bKeyValid: %d\n ", pKey->bKeyValid);
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->uKeyLength: %d\n ", (int)pKey->uKeyLength);
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->abyKey: ");
+			for (ii = 0; ii < pKey->uKeyLength; ii++) {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", pKey->abyKey[ii]);
+			}
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
+
+			//DBG_PRN_GRP12(("pKey->dwTSC47_16: %lX\n ", pKey->dwTSC47_16));
+			//DBG_PRN_GRP12(("pKey->wTSC15_0: %X\n ", pKey->wTSC15_0));
+			//DBG_PRN_GRP12(("pKey->dwKeyIndex: %lX\n ", pKey->dwKeyIndex));
+
+		} // (pTable->KeyTable[i].bInUse == true)
+	}
+	return true;
 }
diff --git a/drivers/staging/vt6655/key.h b/drivers/staging/vt6655/key.h
index 6b2dad331a5..356e6de8f87 100644
--- a/drivers/staging/vt6655/key.h
+++ b/drivers/staging/vt6655/key.h
@@ -40,7 +40,6 @@
 #define MAX_KEY_LEN         32
 #define AES_KEY_LEN         16
 
-
 #define AUTHENTICATOR_KEY   0x10000000
 #define USE_KEYRSC          0x20000000
 #define PAIRWISE_KEY        0x40000000
@@ -54,42 +53,41 @@
 #define KEY_CTL_CCMP        0x03
 #define KEY_CTL_INVALID     0xFF
 
-
 typedef struct tagSKeyItem
 {
-    bool bKeyValid;
-    unsigned long uKeyLength;
-    unsigned char abyKey[MAX_KEY_LEN];
-    QWORD       KeyRSC;
-    unsigned long dwTSC47_16;
-    unsigned short wTSC15_0;
-    unsigned char byCipherSuite;
-    unsigned char byReserved0;
-    unsigned long dwKeyIndex;
-    void *pvKeyTable;
+	bool bKeyValid;
+	unsigned long uKeyLength;
+	unsigned char abyKey[MAX_KEY_LEN];
+	QWORD       KeyRSC;
+	unsigned long dwTSC47_16;
+	unsigned short wTSC15_0;
+	unsigned char byCipherSuite;
+	unsigned char byReserved0;
+	unsigned long dwKeyIndex;
+	void *pvKeyTable;
 } SKeyItem, *PSKeyItem; //64
 
 typedef struct tagSKeyTable
 {
-    unsigned char abyBSSID[ETH_ALEN];  //6
-    unsigned char byReserved0[2];              //8
-    SKeyItem    PairwiseKey;
-    SKeyItem    GroupKey[MAX_GROUP_KEY]; //64*5 = 320, 320+8=328
-    unsigned long dwGTKeyIndex;            // GroupTransmitKey Index
-    bool bInUse;
-    //2006-1116-01,<Modify> by NomadZhao
-    //unsigned short wKeyCtl;
-    //bool bSoftWEP;
-    bool bSoftWEP;
-    unsigned short wKeyCtl;      // for address of wKeyCtl at align 4
-
-    unsigned char byReserved1[6];
+	unsigned char abyBSSID[ETH_ALEN];  //6
+	unsigned char byReserved0[2];              //8
+	SKeyItem    PairwiseKey;
+	SKeyItem    GroupKey[MAX_GROUP_KEY]; //64*5 = 320, 320+8=328
+	unsigned long dwGTKeyIndex;            // GroupTransmitKey Index
+	bool bInUse;
+	//2006-1116-01,<Modify> by NomadZhao
+	//unsigned short wKeyCtl;
+	//bool bSoftWEP;
+	bool bSoftWEP;
+	unsigned short wKeyCtl;      // for address of wKeyCtl at align 4
+
+	unsigned char byReserved1[6];
 } SKeyTable, *PSKeyTable; //348
 
 typedef struct tagSKeyManagement
 {
-    SKeyTable   KeyTable[MAX_KEY_TABLE];
-} SKeyManagement, * PSKeyManagement;
+	SKeyTable   KeyTable[MAX_KEY_TABLE];
+} SKeyManagement, *PSKeyManagement;
 
 /*---------------------  Export Types  ------------------------------*/
 
@@ -104,81 +102,80 @@ typedef struct tagSKeyManagement
 void KeyvInitTable(PSKeyManagement pTable, unsigned long dwIoBase);
 
 bool KeybGetKey(
-    PSKeyManagement pTable,
-    unsigned char *pbyBSSID,
-    unsigned long dwKeyIndex,
-    PSKeyItem       *pKey
-    );
+	PSKeyManagement pTable,
+	unsigned char *pbyBSSID,
+	unsigned long dwKeyIndex,
+	PSKeyItem       *pKey
+);
 
 bool KeybSetKey(
-    PSKeyManagement pTable,
-    unsigned char *pbyBSSID,
-    unsigned long dwKeyIndex,
-    unsigned long uKeyLength,
-    PQWORD          pKeyRSC,
-    unsigned char *pbyKey,
-    unsigned char byKeyDecMode,
-    unsigned long dwIoBase,
-    unsigned char byLocalID
-    );
+	PSKeyManagement pTable,
+	unsigned char *pbyBSSID,
+	unsigned long dwKeyIndex,
+	unsigned long uKeyLength,
+	PQWORD          pKeyRSC,
+	unsigned char *pbyKey,
+	unsigned char byKeyDecMode,
+	unsigned long dwIoBase,
+	unsigned char byLocalID
+);
 
 bool KeybSetDefaultKey(
-    PSKeyManagement pTable,
-    unsigned long dwKeyIndex,
-    unsigned long uKeyLength,
-    PQWORD          pKeyRSC,
-    unsigned char *pbyKey,
-    unsigned char byKeyDecMode,
-    unsigned long dwIoBase,
-    unsigned char byLocalID
-    );
+	PSKeyManagement pTable,
+	unsigned long dwKeyIndex,
+	unsigned long uKeyLength,
+	PQWORD          pKeyRSC,
+	unsigned char *pbyKey,
+	unsigned char byKeyDecMode,
+	unsigned long dwIoBase,
+	unsigned char byLocalID
+);
 
 bool KeybRemoveKey(
-    PSKeyManagement pTable,
-    unsigned char *pbyBSSID,
-    unsigned long dwKeyIndex,
-    unsigned long dwIoBase
-    );
+	PSKeyManagement pTable,
+	unsigned char *pbyBSSID,
+	unsigned long dwKeyIndex,
+	unsigned long dwIoBase
+);
 
 bool KeybGetTransmitKey(
-    PSKeyManagement pTable,
-    unsigned char *pbyBSSID,
-    unsigned long dwKeyType,
-    PSKeyItem       *pKey
-    );
+	PSKeyManagement pTable,
+	unsigned char *pbyBSSID,
+	unsigned long dwKeyType,
+	PSKeyItem       *pKey
+);
 
 bool KeybCheckPairewiseKey(
-    PSKeyManagement pTable,
-    PSKeyItem       *pKey
-    );
+	PSKeyManagement pTable,
+	PSKeyItem       *pKey
+);
 
 bool KeybRemoveAllKey(
-    PSKeyManagement pTable,
-    unsigned char *pbyBSSID,
-    unsigned long dwIoBase
-    );
+	PSKeyManagement pTable,
+	unsigned char *pbyBSSID,
+	unsigned long dwIoBase
+);
 
 void KeyvRemoveWEPKey(
-    PSKeyManagement pTable,
-    unsigned long dwKeyIndex,
-    unsigned long dwIoBase
-    );
+	PSKeyManagement pTable,
+	unsigned long dwKeyIndex,
+	unsigned long dwIoBase
+);
 
 void KeyvRemoveAllWEPKey(
-    PSKeyManagement pTable,
-    unsigned long dwIoBase
-    );
-
-bool KeybSetAllGroupKey (
-    PSKeyManagement pTable,
-    unsigned long dwKeyIndex,
-    unsigned long uKeyLength,
-    PQWORD          pKeyRSC,
-    unsigned char *pbyKey,
-    unsigned char byKeyDecMode,
-    unsigned long dwIoBase,
-    unsigned char byLocalID
-    );
+	PSKeyManagement pTable,
+	unsigned long dwIoBase
+);
+
+bool KeybSetAllGroupKey(
+	PSKeyManagement pTable,
+	unsigned long dwKeyIndex,
+	unsigned long uKeyLength,
+	PQWORD          pKeyRSC,
+	unsigned char *pbyKey,
+	unsigned char byKeyDecMode,
+	unsigned long dwIoBase,
+	unsigned char byLocalID
+);
 
 #endif // __KEY_H__
-
diff --git a/drivers/staging/vt6655/mac.c b/drivers/staging/vt6655/mac.c
index 30c26157941..001d15c0fa4 100644
--- a/drivers/staging/vt6655/mac.c
+++ b/drivers/staging/vt6655/mac.c
@@ -75,7 +75,7 @@
 unsigned short TxRate_iwconfig;//2008-5-8 <add> by chester
 /*---------------------  Static Definitions -------------------------*/
 //static int          msglevel                =MSG_LEVEL_DEBUG;
-static int          msglevel                =MSG_LEVEL_INFO;
+static int msglevel = MSG_LEVEL_INFO;
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
@@ -86,10 +86,6 @@ static int          msglevel                =MSG_LEVEL_INFO;
 
 /*---------------------  Export Functions  --------------------------*/
 
-
-
-
-
 /*
  * Description:
  *      Read All MAC Registers to buffer
@@ -103,26 +99,25 @@ static int          msglevel                =MSG_LEVEL_INFO;
  * Return Value: none
  *
  */
-void MACvReadAllRegs (unsigned long dwIoBase, unsigned char *pbyMacRegs)
+void MACvReadAllRegs(unsigned long dwIoBase, unsigned char *pbyMacRegs)
 {
-    int ii;
-
-    // read page0 register
-    for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE0; ii++) {
-        VNSvInPortB(dwIoBase + ii, pbyMacRegs);
-        pbyMacRegs++;
-    }
+	int ii;
 
-    MACvSelectPage1(dwIoBase);
+	// read page0 register
+	for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE0; ii++) {
+		VNSvInPortB(dwIoBase + ii, pbyMacRegs);
+		pbyMacRegs++;
+	}
 
-    // read page1 register
-    for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE1; ii++) {
-        VNSvInPortB(dwIoBase + ii, pbyMacRegs);
-        pbyMacRegs++;
-    }
+	MACvSelectPage1(dwIoBase);
 
-    MACvSelectPage0(dwIoBase);
+	// read page1 register
+	for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE1; ii++) {
+		VNSvInPortB(dwIoBase + ii, pbyMacRegs);
+		pbyMacRegs++;
+	}
 
+	MACvSelectPage0(dwIoBase);
 }
 
 /*
@@ -140,12 +135,12 @@ void MACvReadAllRegs (unsigned long dwIoBase, unsigned char *pbyMacRegs)
  * Return Value: true if all test bits On; otherwise false
  *
  */
-bool MACbIsRegBitsOn (unsigned long dwIoBase, unsigned char byRegOfs, unsigned char byTestBits)
+bool MACbIsRegBitsOn(unsigned long dwIoBase, unsigned char byRegOfs, unsigned char byTestBits)
 {
-    unsigned char byData;
+	unsigned char byData;
 
-    VNSvInPortB(dwIoBase + byRegOfs, &byData);
-    return (byData & byTestBits) == byTestBits;
+	VNSvInPortB(dwIoBase + byRegOfs, &byData);
+	return (byData & byTestBits) == byTestBits;
 }
 
 /*
@@ -163,12 +158,12 @@ bool MACbIsRegBitsOn (unsigned long dwIoBase, unsigned char byRegOfs, unsigned c
  * Return Value: true if all test bits Off; otherwise false
  *
  */
-bool MACbIsRegBitsOff (unsigned long dwIoBase, unsigned char byRegOfs, unsigned char byTestBits)
+bool MACbIsRegBitsOff(unsigned long dwIoBase, unsigned char byRegOfs, unsigned char byTestBits)
 {
-    unsigned char byData;
+	unsigned char byData;
 
-    VNSvInPortB(dwIoBase + byRegOfs, &byData);
-    return !(byData & byTestBits);
+	VNSvInPortB(dwIoBase + byRegOfs, &byData);
+	return !(byData & byTestBits);
 }
 
 /*
@@ -184,15 +179,15 @@ bool MACbIsRegBitsOff (unsigned long dwIoBase, unsigned char byRegOfs, unsigned
  * Return Value: true if interrupt is disable; otherwise false
  *
  */
-bool MACbIsIntDisable (unsigned long dwIoBase)
+bool MACbIsIntDisable(unsigned long dwIoBase)
 {
-    unsigned long dwData;
+	unsigned long dwData;
 
-    VNSvInPortD(dwIoBase + MAC_REG_IMR, &dwData);
-    if (dwData != 0)
-        return false;
+	VNSvInPortD(dwIoBase + MAC_REG_IMR, &dwData);
+	if (dwData != 0)
+		return false;
 
-    return true;
+	return true;
 }
 
 /*
@@ -209,14 +204,14 @@ bool MACbIsIntDisable (unsigned long dwIoBase)
  * Return Value: Mask Value read
  *
  */
-unsigned char MACbyReadMultiAddr (unsigned long dwIoBase, unsigned int uByteIdx)
+unsigned char MACbyReadMultiAddr(unsigned long dwIoBase, unsigned int uByteIdx)
 {
-    unsigned char byData;
+	unsigned char byData;
 
-    MACvSelectPage1(dwIoBase);
-    VNSvInPortB(dwIoBase + MAC_REG_MAR0 + uByteIdx, &byData);
-    MACvSelectPage0(dwIoBase);
-    return byData;
+	MACvSelectPage1(dwIoBase);
+	VNSvInPortB(dwIoBase + MAC_REG_MAR0 + uByteIdx, &byData);
+	MACvSelectPage0(dwIoBase);
+	return byData;
 }
 
 /*
@@ -234,11 +229,11 @@ unsigned char MACbyReadMultiAddr (unsigned long dwIoBase, unsigned int uByteIdx)
  * Return Value: none
  *
  */
-void MACvWriteMultiAddr (unsigned long dwIoBase, unsigned int uByteIdx, unsigned char byData)
+void MACvWriteMultiAddr(unsigned long dwIoBase, unsigned int uByteIdx, unsigned char byData)
 {
-    MACvSelectPage1(dwIoBase);
-    VNSvOutPortB(dwIoBase + MAC_REG_MAR0 + uByteIdx, byData);
-    MACvSelectPage0(dwIoBase);
+	MACvSelectPage1(dwIoBase);
+	VNSvOutPortB(dwIoBase + MAC_REG_MAR0 + uByteIdx, byData);
+	MACvSelectPage0(dwIoBase);
 }
 
 /*
@@ -255,21 +250,21 @@ void MACvWriteMultiAddr (unsigned long dwIoBase, unsigned int uByteIdx, unsigned
  * Return Value: none
  *
  */
-void MACvSetMultiAddrByHash (unsigned long dwIoBase, unsigned char byHashIdx)
+void MACvSetMultiAddrByHash(unsigned long dwIoBase, unsigned char byHashIdx)
 {
-    unsigned int uByteIdx;
-    unsigned char byBitMask;
-    unsigned char byOrgValue;
-
-    // calculate byte position
-    uByteIdx = byHashIdx / 8;
-    ASSERT(uByteIdx < 8);
-    // calculate bit position
-    byBitMask = 1;
-    byBitMask <<= (byHashIdx % 8);
-    // turn on the bit
-    byOrgValue = MACbyReadMultiAddr(dwIoBase, uByteIdx);
-    MACvWriteMultiAddr(dwIoBase, uByteIdx, (unsigned char)(byOrgValue | byBitMask));
+	unsigned int uByteIdx;
+	unsigned char byBitMask;
+	unsigned char byOrgValue;
+
+	// calculate byte position
+	uByteIdx = byHashIdx / 8;
+	ASSERT(uByteIdx < 8);
+	// calculate bit position
+	byBitMask = 1;
+	byBitMask <<= (byHashIdx % 8);
+	// turn on the bit
+	byOrgValue = MACbyReadMultiAddr(dwIoBase, uByteIdx);
+	MACvWriteMultiAddr(dwIoBase, uByteIdx, (unsigned char)(byOrgValue | byBitMask));
 }
 
 /*
@@ -286,21 +281,21 @@ void MACvSetMultiAddrByHash (unsigned long dwIoBase, unsigned char byHashIdx)
  * Return Value: none
  *
  */
-void MACvResetMultiAddrByHash (unsigned long dwIoBase, unsigned char byHashIdx)
+void MACvResetMultiAddrByHash(unsigned long dwIoBase, unsigned char byHashIdx)
 {
-    unsigned int uByteIdx;
-    unsigned char byBitMask;
-    unsigned char byOrgValue;
-
-    // calculate byte position
-    uByteIdx = byHashIdx / 8;
-    ASSERT(uByteIdx < 8);
-    // calculate bit position
-    byBitMask = 1;
-    byBitMask <<= (byHashIdx % 8);
-    // turn off the bit
-    byOrgValue = MACbyReadMultiAddr(dwIoBase, uByteIdx);
-    MACvWriteMultiAddr(dwIoBase, uByteIdx, (unsigned char)(byOrgValue & (~byBitMask)));
+	unsigned int uByteIdx;
+	unsigned char byBitMask;
+	unsigned char byOrgValue;
+
+	// calculate byte position
+	uByteIdx = byHashIdx / 8;
+	ASSERT(uByteIdx < 8);
+	// calculate bit position
+	byBitMask = 1;
+	byBitMask <<= (byHashIdx % 8);
+	// turn off the bit
+	byOrgValue = MACbyReadMultiAddr(dwIoBase, uByteIdx);
+	MACvWriteMultiAddr(dwIoBase, uByteIdx, (unsigned char)(byOrgValue & (~byBitMask)));
 }
 
 /*
@@ -317,16 +312,16 @@ void MACvResetMultiAddrByHash (unsigned long dwIoBase, unsigned char byHashIdx)
  * Return Value: none
  *
  */
-void MACvSetRxThreshold (unsigned long dwIoBase, unsigned char byThreshold)
+void MACvSetRxThreshold(unsigned long dwIoBase, unsigned char byThreshold)
 {
-    unsigned char byOrgValue;
+	unsigned char byOrgValue;
 
-    ASSERT(byThreshold < 4);
+	ASSERT(byThreshold < 4);
 
-    // set FCR0
-    VNSvInPortB(dwIoBase + MAC_REG_FCR0, &byOrgValue);
-    byOrgValue = (byOrgValue & 0xCF) | (byThreshold << 4);
-    VNSvOutPortB(dwIoBase + MAC_REG_FCR0, byOrgValue);
+	// set FCR0
+	VNSvInPortB(dwIoBase + MAC_REG_FCR0, &byOrgValue);
+	byOrgValue = (byOrgValue & 0xCF) | (byThreshold << 4);
+	VNSvOutPortB(dwIoBase + MAC_REG_FCR0, byOrgValue);
 }
 
 /*
@@ -342,11 +337,11 @@ void MACvSetRxThreshold (unsigned long dwIoBase, unsigned char byThreshold)
  * Return Value: none
  *
  */
-void MACvGetRxThreshold (unsigned long dwIoBase, unsigned char *pbyThreshold)
+void MACvGetRxThreshold(unsigned long dwIoBase, unsigned char *pbyThreshold)
 {
-    // get FCR0
-    VNSvInPortB(dwIoBase + MAC_REG_FCR0, pbyThreshold);
-    *pbyThreshold = (*pbyThreshold >> 4) & 0x03;
+	// get FCR0
+	VNSvInPortB(dwIoBase + MAC_REG_FCR0, pbyThreshold);
+	*pbyThreshold = (*pbyThreshold >> 4) & 0x03;
 }
 
 /*
@@ -363,16 +358,16 @@ void MACvGetRxThreshold (unsigned long dwIoBase, unsigned char *pbyThreshold)
  * Return Value: none
  *
  */
-void MACvSetTxThreshold (unsigned long dwIoBase, unsigned char byThreshold)
+void MACvSetTxThreshold(unsigned long dwIoBase, unsigned char byThreshold)
 {
-    unsigned char byOrgValue;
+	unsigned char byOrgValue;
 
-    ASSERT(byThreshold < 4);
+	ASSERT(byThreshold < 4);
 
-    // set FCR0
-    VNSvInPortB(dwIoBase + MAC_REG_FCR0, &byOrgValue);
-    byOrgValue = (byOrgValue & 0xF3) | (byThreshold << 2);
-    VNSvOutPortB(dwIoBase + MAC_REG_FCR0, byOrgValue);
+	// set FCR0
+	VNSvInPortB(dwIoBase + MAC_REG_FCR0, &byOrgValue);
+	byOrgValue = (byOrgValue & 0xF3) | (byThreshold << 2);
+	VNSvOutPortB(dwIoBase + MAC_REG_FCR0, byOrgValue);
 }
 
 /*
@@ -388,11 +383,11 @@ void MACvSetTxThreshold (unsigned long dwIoBase, unsigned char byThreshold)
  * Return Value: none
  *
  */
-void MACvGetTxThreshold (unsigned long dwIoBase, unsigned char *pbyThreshold)
+void MACvGetTxThreshold(unsigned long dwIoBase, unsigned char *pbyThreshold)
 {
-    // get FCR0
-    VNSvInPortB(dwIoBase + MAC_REG_FCR0, pbyThreshold);
-    *pbyThreshold = (*pbyThreshold >> 2) & 0x03;
+	// get FCR0
+	VNSvInPortB(dwIoBase + MAC_REG_FCR0, pbyThreshold);
+	*pbyThreshold = (*pbyThreshold >> 2) & 0x03;
 }
 
 /*
@@ -409,16 +404,16 @@ void MACvGetTxThreshold (unsigned long dwIoBase, unsigned char *pbyThreshold)
  * Return Value: none
  *
  */
-void MACvSetDmaLength (unsigned long dwIoBase, unsigned char byDmaLength)
+void MACvSetDmaLength(unsigned long dwIoBase, unsigned char byDmaLength)
 {
-    unsigned char byOrgValue;
+	unsigned char byOrgValue;
 
-    ASSERT(byDmaLength < 4);
+	ASSERT(byDmaLength < 4);
 
-    // set FCR0
-    VNSvInPortB(dwIoBase + MAC_REG_FCR0, &byOrgValue);
-    byOrgValue = (byOrgValue & 0xFC) | byDmaLength;
-    VNSvOutPortB(dwIoBase + MAC_REG_FCR0, byOrgValue);
+	// set FCR0
+	VNSvInPortB(dwIoBase + MAC_REG_FCR0, &byOrgValue);
+	byOrgValue = (byOrgValue & 0xFC) | byDmaLength;
+	VNSvOutPortB(dwIoBase + MAC_REG_FCR0, byOrgValue);
 }
 
 /*
@@ -434,11 +429,11 @@ void MACvSetDmaLength (unsigned long dwIoBase, unsigned char byDmaLength)
  * Return Value: none
  *
  */
-void MACvGetDmaLength (unsigned long dwIoBase, unsigned char *pbyDmaLength)
+void MACvGetDmaLength(unsigned long dwIoBase, unsigned char *pbyDmaLength)
 {
-    // get FCR0
-    VNSvInPortB(dwIoBase + MAC_REG_FCR0, pbyDmaLength);
-    *pbyDmaLength &= 0x03;
+	// get FCR0
+	VNSvInPortB(dwIoBase + MAC_REG_FCR0, pbyDmaLength);
+	*pbyDmaLength &= 0x03;
 }
 
 /*
@@ -455,10 +450,10 @@ void MACvGetDmaLength (unsigned long dwIoBase, unsigned char *pbyDmaLength)
  * Return Value: none
  *
  */
-void MACvSetShortRetryLimit (unsigned long dwIoBase, unsigned char byRetryLimit)
+void MACvSetShortRetryLimit(unsigned long dwIoBase, unsigned char byRetryLimit)
 {
-    // set SRT
-    VNSvOutPortB(dwIoBase + MAC_REG_SRT, byRetryLimit);
+	// set SRT
+	VNSvOutPortB(dwIoBase + MAC_REG_SRT, byRetryLimit);
 }
 
 /*
@@ -474,10 +469,10 @@ void MACvSetShortRetryLimit (unsigned long dwIoBase, unsigned char byRetryLimit)
  * Return Value: none
  *
  */
-void MACvGetShortRetryLimit (unsigned long dwIoBase, unsigned char *pbyRetryLimit)
+void MACvGetShortRetryLimit(unsigned long dwIoBase, unsigned char *pbyRetryLimit)
 {
-    // get SRT
-    VNSvInPortB(dwIoBase + MAC_REG_SRT, pbyRetryLimit);
+	// get SRT
+	VNSvInPortB(dwIoBase + MAC_REG_SRT, pbyRetryLimit);
 }
 
 /*
@@ -494,10 +489,10 @@ void MACvGetShortRetryLimit (unsigned long dwIoBase, unsigned char *pbyRetryLimi
  * Return Value: none
  *
  */
-void MACvSetLongRetryLimit (unsigned long dwIoBase, unsigned char byRetryLimit)
+void MACvSetLongRetryLimit(unsigned long dwIoBase, unsigned char byRetryLimit)
 {
-    // set LRT
-    VNSvOutPortB(dwIoBase + MAC_REG_LRT, byRetryLimit);
+	// set LRT
+	VNSvOutPortB(dwIoBase + MAC_REG_LRT, byRetryLimit);
 }
 
 /*
@@ -513,10 +508,10 @@ void MACvSetLongRetryLimit (unsigned long dwIoBase, unsigned char byRetryLimit)
  * Return Value: none
  *
  */
-void MACvGetLongRetryLimit (unsigned long dwIoBase, unsigned char *pbyRetryLimit)
+void MACvGetLongRetryLimit(unsigned long dwIoBase, unsigned char *pbyRetryLimit)
 {
-    // get LRT
-    VNSvInPortB(dwIoBase + MAC_REG_LRT, pbyRetryLimit);
+	// get LRT
+	VNSvInPortB(dwIoBase + MAC_REG_LRT, pbyRetryLimit);
 }
 
 /*
@@ -533,17 +528,17 @@ void MACvGetLongRetryLimit (unsigned long dwIoBase, unsigned char *pbyRetryLimit
  * Return Value: none
  *
  */
-void MACvSetLoopbackMode (unsigned long dwIoBase, unsigned char byLoopbackMode)
+void MACvSetLoopbackMode(unsigned long dwIoBase, unsigned char byLoopbackMode)
 {
-    unsigned char byOrgValue;
-
-    ASSERT(byLoopbackMode < 3);
-    byLoopbackMode <<= 6;
-    // set TCR
-    VNSvInPortB(dwIoBase + MAC_REG_TEST, &byOrgValue);
-    byOrgValue = byOrgValue & 0x3F;
-    byOrgValue = byOrgValue | byLoopbackMode;
-    VNSvOutPortB(dwIoBase + MAC_REG_TEST, byOrgValue);
+	unsigned char byOrgValue;
+
+	ASSERT(byLoopbackMode < 3);
+	byLoopbackMode <<= 6;
+	// set TCR
+	VNSvInPortB(dwIoBase + MAC_REG_TEST, &byOrgValue);
+	byOrgValue = byOrgValue & 0x3F;
+	byOrgValue = byOrgValue | byLoopbackMode;
+	VNSvOutPortB(dwIoBase + MAC_REG_TEST, byOrgValue);
 }
 
 /*
@@ -559,14 +554,14 @@ void MACvSetLoopbackMode (unsigned long dwIoBase, unsigned char byLoopbackMode)
  * Return Value: true if in Loopback mode; otherwise false
  *
  */
-bool MACbIsInLoopbackMode (unsigned long dwIoBase)
+bool MACbIsInLoopbackMode(unsigned long dwIoBase)
 {
-    unsigned char byOrgValue;
+	unsigned char byOrgValue;
 
-    VNSvInPortB(dwIoBase + MAC_REG_TEST, &byOrgValue);
-    if (byOrgValue & (TEST_LBINT | TEST_LBEXT))
-        return true;
-    return false;
+	VNSvInPortB(dwIoBase + MAC_REG_TEST, &byOrgValue);
+	if (byOrgValue & (TEST_LBINT | TEST_LBEXT))
+		return true;
+	return false;
 }
 
 /*
@@ -583,51 +578,50 @@ bool MACbIsInLoopbackMode (unsigned long dwIoBase)
  * Return Value: none
  *
  */
-void MACvSetPacketFilter (unsigned long dwIoBase, unsigned short wFilterType)
+void MACvSetPacketFilter(unsigned long dwIoBase, unsigned short wFilterType)
 {
-    unsigned char byOldRCR;
-    unsigned char byNewRCR = 0;
-
-    // if only in DIRECTED mode, multicast-address will set to zero,
-    // but if other mode exist (e.g. PROMISCUOUS), multicast-address
-    // will be open
-    if (wFilterType & PKT_TYPE_DIRECTED) {
-        // set multicast address to accept none
-        MACvSelectPage1(dwIoBase);
-        VNSvOutPortD(dwIoBase + MAC_REG_MAR0, 0L);
-        VNSvOutPortD(dwIoBase + MAC_REG_MAR0 + sizeof(unsigned long), 0L);
-        MACvSelectPage0(dwIoBase);
-    }
-
-    if (wFilterType & (PKT_TYPE_PROMISCUOUS | PKT_TYPE_ALL_MULTICAST)) {
-        // set multicast address to accept all
-        MACvSelectPage1(dwIoBase);
-        VNSvOutPortD(dwIoBase + MAC_REG_MAR0, 0xFFFFFFFFL);
-        VNSvOutPortD(dwIoBase + MAC_REG_MAR0 + sizeof(unsigned long), 0xFFFFFFFFL);
-        MACvSelectPage0(dwIoBase);
-    }
-
-    if (wFilterType & PKT_TYPE_PROMISCUOUS) {
-
-        byNewRCR |= (RCR_RXALLTYPE | RCR_UNICAST | RCR_MULTICAST | RCR_BROADCAST);
-
-        byNewRCR &= ~RCR_BSSID;
-    }
-
-    if (wFilterType & (PKT_TYPE_ALL_MULTICAST | PKT_TYPE_MULTICAST))
-        byNewRCR |= RCR_MULTICAST;
-
-    if (wFilterType & PKT_TYPE_BROADCAST)
-        byNewRCR |= RCR_BROADCAST;
-
-    if (wFilterType & PKT_TYPE_ERROR_CRC)
-        byNewRCR |= RCR_ERRCRC;
-
-    VNSvInPortB(dwIoBase + MAC_REG_RCR,  &byOldRCR);
-    if (byNewRCR != byOldRCR) {
-        // Modify the Receive Command Register
-        VNSvOutPortB(dwIoBase + MAC_REG_RCR, byNewRCR);
-    }
+	unsigned char byOldRCR;
+	unsigned char byNewRCR = 0;
+
+	// if only in DIRECTED mode, multicast-address will set to zero,
+	// but if other mode exist (e.g. PROMISCUOUS), multicast-address
+	// will be open
+	if (wFilterType & PKT_TYPE_DIRECTED) {
+		// set multicast address to accept none
+		MACvSelectPage1(dwIoBase);
+		VNSvOutPortD(dwIoBase + MAC_REG_MAR0, 0L);
+		VNSvOutPortD(dwIoBase + MAC_REG_MAR0 + sizeof(unsigned long), 0L);
+		MACvSelectPage0(dwIoBase);
+	}
+
+	if (wFilterType & (PKT_TYPE_PROMISCUOUS | PKT_TYPE_ALL_MULTICAST)) {
+		// set multicast address to accept all
+		MACvSelectPage1(dwIoBase);
+		VNSvOutPortD(dwIoBase + MAC_REG_MAR0, 0xFFFFFFFFL);
+		VNSvOutPortD(dwIoBase + MAC_REG_MAR0 + sizeof(unsigned long), 0xFFFFFFFFL);
+		MACvSelectPage0(dwIoBase);
+	}
+
+	if (wFilterType & PKT_TYPE_PROMISCUOUS) {
+		byNewRCR |= (RCR_RXALLTYPE | RCR_UNICAST | RCR_MULTICAST | RCR_BROADCAST);
+
+		byNewRCR &= ~RCR_BSSID;
+	}
+
+	if (wFilterType & (PKT_TYPE_ALL_MULTICAST | PKT_TYPE_MULTICAST))
+		byNewRCR |= RCR_MULTICAST;
+
+	if (wFilterType & PKT_TYPE_BROADCAST)
+		byNewRCR |= RCR_BROADCAST;
+
+	if (wFilterType & PKT_TYPE_ERROR_CRC)
+		byNewRCR |= RCR_ERRCRC;
+
+	VNSvInPortB(dwIoBase + MAC_REG_RCR,  &byOldRCR);
+	if (byNewRCR != byOldRCR) {
+		// Modify the Receive Command Register
+		VNSvOutPortB(dwIoBase + MAC_REG_RCR, byNewRCR);
+	}
 }
 
 /*
@@ -643,23 +637,23 @@ void MACvSetPacketFilter (unsigned long dwIoBase, unsigned short wFilterType)
  * Return Value: none
  *
  */
-void MACvSaveContext (unsigned long dwIoBase, unsigned char *pbyCxtBuf)
+void MACvSaveContext(unsigned long dwIoBase, unsigned char *pbyCxtBuf)
 {
-    int         ii;
+	int         ii;
 
-    // read page0 register
-    for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE0; ii++) {
-        VNSvInPortB((dwIoBase + ii), (pbyCxtBuf + ii));
-    }
+	// read page0 register
+	for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE0; ii++) {
+		VNSvInPortB((dwIoBase + ii), (pbyCxtBuf + ii));
+	}
 
-    MACvSelectPage1(dwIoBase);
+	MACvSelectPage1(dwIoBase);
 
-    // read page1 register
-    for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE1; ii++) {
-        VNSvInPortB((dwIoBase + ii), (pbyCxtBuf + MAC_MAX_CONTEXT_SIZE_PAGE0 + ii));
-    }
+	// read page1 register
+	for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE1; ii++) {
+		VNSvInPortB((dwIoBase + ii), (pbyCxtBuf + MAC_MAX_CONTEXT_SIZE_PAGE0 + ii));
+	}
 
-    MACvSelectPage0(dwIoBase);
+	MACvSelectPage0(dwIoBase);
 }
 
 /*
@@ -676,42 +670,40 @@ void MACvSaveContext (unsigned long dwIoBase, unsigned char *pbyCxtBuf)
  * Return Value: none
  *
  */
-void MACvRestoreContext (unsigned long dwIoBase, unsigned char *pbyCxtBuf)
+void MACvRestoreContext(unsigned long dwIoBase, unsigned char *pbyCxtBuf)
 {
-    int         ii;
-
-    MACvSelectPage1(dwIoBase);
-    // restore page1
-    for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE1; ii++) {
-        VNSvOutPortB((dwIoBase + ii), *(pbyCxtBuf + MAC_MAX_CONTEXT_SIZE_PAGE0 + ii));
-    }
-    MACvSelectPage0(dwIoBase);
-
-    // restore RCR,TCR,IMR...
-    for (ii = MAC_REG_RCR; ii < MAC_REG_ISR; ii++) {
-        VNSvOutPortB(dwIoBase + ii, *(pbyCxtBuf + ii));
-    }
-    // restore MAC Config.
-    for (ii = MAC_REG_LRT; ii < MAC_REG_PAGE1SEL; ii++) {
-        VNSvOutPortB(dwIoBase + ii, *(pbyCxtBuf + ii));
-    }
-    VNSvOutPortB(dwIoBase + MAC_REG_CFG, *(pbyCxtBuf + MAC_REG_CFG));
-
-    // restore PS Config.
-    for (ii = MAC_REG_PSCFG; ii < MAC_REG_BBREGCTL; ii++) {
-        VNSvOutPortB(dwIoBase + ii, *(pbyCxtBuf + ii));
-    }
-
-    // restore CURR_RX_DESC_ADDR, CURR_TX_DESC_ADDR
-    VNSvOutPortD(dwIoBase + MAC_REG_TXDMAPTR0, *(unsigned long *)(pbyCxtBuf + MAC_REG_TXDMAPTR0));
-    VNSvOutPortD(dwIoBase + MAC_REG_AC0DMAPTR, *(unsigned long *)(pbyCxtBuf + MAC_REG_AC0DMAPTR));
-    VNSvOutPortD(dwIoBase + MAC_REG_BCNDMAPTR, *(unsigned long *)(pbyCxtBuf + MAC_REG_BCNDMAPTR));
-
-
-    VNSvOutPortD(dwIoBase + MAC_REG_RXDMAPTR0, *(unsigned long *)(pbyCxtBuf + MAC_REG_RXDMAPTR0));
-
-    VNSvOutPortD(dwIoBase + MAC_REG_RXDMAPTR1, *(unsigned long *)(pbyCxtBuf + MAC_REG_RXDMAPTR1));
-
+	int         ii;
+
+	MACvSelectPage1(dwIoBase);
+	// restore page1
+	for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE1; ii++) {
+		VNSvOutPortB((dwIoBase + ii), *(pbyCxtBuf + MAC_MAX_CONTEXT_SIZE_PAGE0 + ii));
+	}
+	MACvSelectPage0(dwIoBase);
+
+	// restore RCR,TCR,IMR...
+	for (ii = MAC_REG_RCR; ii < MAC_REG_ISR; ii++) {
+		VNSvOutPortB(dwIoBase + ii, *(pbyCxtBuf + ii));
+	}
+	// restore MAC Config.
+	for (ii = MAC_REG_LRT; ii < MAC_REG_PAGE1SEL; ii++) {
+		VNSvOutPortB(dwIoBase + ii, *(pbyCxtBuf + ii));
+	}
+	VNSvOutPortB(dwIoBase + MAC_REG_CFG, *(pbyCxtBuf + MAC_REG_CFG));
+
+	// restore PS Config.
+	for (ii = MAC_REG_PSCFG; ii < MAC_REG_BBREGCTL; ii++) {
+		VNSvOutPortB(dwIoBase + ii, *(pbyCxtBuf + ii));
+	}
+
+	// restore CURR_RX_DESC_ADDR, CURR_TX_DESC_ADDR
+	VNSvOutPortD(dwIoBase + MAC_REG_TXDMAPTR0, *(unsigned long *)(pbyCxtBuf + MAC_REG_TXDMAPTR0));
+	VNSvOutPortD(dwIoBase + MAC_REG_AC0DMAPTR, *(unsigned long *)(pbyCxtBuf + MAC_REG_AC0DMAPTR));
+	VNSvOutPortD(dwIoBase + MAC_REG_BCNDMAPTR, *(unsigned long *)(pbyCxtBuf + MAC_REG_BCNDMAPTR));
+
+	VNSvOutPortD(dwIoBase + MAC_REG_RXDMAPTR0, *(unsigned long *)(pbyCxtBuf + MAC_REG_RXDMAPTR0));
+
+	VNSvOutPortD(dwIoBase + MAC_REG_RXDMAPTR1, *(unsigned long *)(pbyCxtBuf + MAC_REG_RXDMAPTR1));
 }
 
 /*
@@ -728,36 +720,35 @@ void MACvRestoreContext (unsigned long dwIoBase, unsigned char *pbyCxtBuf)
  * Return Value: true if all values are the same; otherwise false
  *
  */
-bool MACbCompareContext (unsigned long dwIoBase, unsigned char *pbyCxtBuf)
+bool MACbCompareContext(unsigned long dwIoBase, unsigned char *pbyCxtBuf)
 {
-    unsigned long dwData;
-
-    // compare MAC context to determine if this is a power lost init,
-    // return true for power remaining init, return false for power lost init
+	unsigned long dwData;
 
-    // compare CURR_RX_DESC_ADDR, CURR_TX_DESC_ADDR
-    VNSvInPortD(dwIoBase + MAC_REG_TXDMAPTR0, &dwData);
-    if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_TXDMAPTR0)) {
-        return false;
-    }
+	// compare MAC context to determine if this is a power lost init,
+	// return true for power remaining init, return false for power lost init
 
-    VNSvInPortD(dwIoBase + MAC_REG_AC0DMAPTR, &dwData);
-    if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_AC0DMAPTR)) {
-        return false;
-    }
+	// compare CURR_RX_DESC_ADDR, CURR_TX_DESC_ADDR
+	VNSvInPortD(dwIoBase + MAC_REG_TXDMAPTR0, &dwData);
+	if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_TXDMAPTR0)) {
+		return false;
+	}
 
-    VNSvInPortD(dwIoBase + MAC_REG_RXDMAPTR0, &dwData);
-    if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_RXDMAPTR0)) {
-        return false;
-    }
+	VNSvInPortD(dwIoBase + MAC_REG_AC0DMAPTR, &dwData);
+	if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_AC0DMAPTR)) {
+		return false;
+	}
 
-    VNSvInPortD(dwIoBase + MAC_REG_RXDMAPTR1, &dwData);
-    if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_RXDMAPTR1)) {
-        return false;
-    }
+	VNSvInPortD(dwIoBase + MAC_REG_RXDMAPTR0, &dwData);
+	if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_RXDMAPTR0)) {
+		return false;
+	}
 
+	VNSvInPortD(dwIoBase + MAC_REG_RXDMAPTR1, &dwData);
+	if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_RXDMAPTR1)) {
+		return false;
+	}
 
-    return true;
+	return true;
 }
 
 /*
@@ -773,24 +764,23 @@ bool MACbCompareContext (unsigned long dwIoBase, unsigned char *pbyCxtBuf)
  * Return Value: true if Reset Success; otherwise false
  *
  */
-bool MACbSoftwareReset (unsigned long dwIoBase)
+bool MACbSoftwareReset(unsigned long dwIoBase)
 {
-    unsigned char byData;
-    unsigned short ww;
-
-    // turn on HOSTCR_SOFTRST, just write 0x01 to reset
-    //MACvRegBitsOn(dwIoBase, MAC_REG_HOSTCR, HOSTCR_SOFTRST);
-    VNSvOutPortB(dwIoBase+ MAC_REG_HOSTCR, 0x01);
-
-    for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-        VNSvInPortB(dwIoBase + MAC_REG_HOSTCR, &byData);
-        if ( !(byData & HOSTCR_SOFTRST))
-            break;
-    }
-    if (ww == W_MAX_TIMEOUT)
-        return false;
-    return true;
-
+	unsigned char byData;
+	unsigned short ww;
+
+	// turn on HOSTCR_SOFTRST, just write 0x01 to reset
+	//MACvRegBitsOn(dwIoBase, MAC_REG_HOSTCR, HOSTCR_SOFTRST);
+	VNSvOutPortB(dwIoBase + MAC_REG_HOSTCR, 0x01);
+
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		VNSvInPortB(dwIoBase + MAC_REG_HOSTCR, &byData);
+		if (!(byData & HOSTCR_SOFTRST))
+			break;
+	}
+	if (ww == W_MAX_TIMEOUT)
+		return false;
+	return true;
 }
 
 /*
@@ -806,24 +796,24 @@ bool MACbSoftwareReset (unsigned long dwIoBase)
  * Return Value: true if success; otherwise false
  *
  */
-bool MACbSafeSoftwareReset (unsigned long dwIoBase)
+bool MACbSafeSoftwareReset(unsigned long dwIoBase)
 {
-    unsigned char abyTmpRegData[MAC_MAX_CONTEXT_SIZE_PAGE0+MAC_MAX_CONTEXT_SIZE_PAGE1];
-    bool bRetVal;
-
-    // PATCH....
-    // save some important register's value, then do
-    // reset, then restore register's value
-
-    // save MAC context
-    MACvSaveContext(dwIoBase, abyTmpRegData);
-    // do reset
-    bRetVal = MACbSoftwareReset(dwIoBase);
-    //BBvSoftwareReset(pDevice->PortOffset);
-    // restore MAC context, except CR0
-    MACvRestoreContext(dwIoBase, abyTmpRegData);
-
-    return bRetVal;
+	unsigned char abyTmpRegData[MAC_MAX_CONTEXT_SIZE_PAGE0+MAC_MAX_CONTEXT_SIZE_PAGE1];
+	bool bRetVal;
+
+	// PATCH....
+	// save some important register's value, then do
+	// reset, then restore register's value
+
+	// save MAC context
+	MACvSaveContext(dwIoBase, abyTmpRegData);
+	// do reset
+	bRetVal = MACbSoftwareReset(dwIoBase);
+	//BBvSoftwareReset(pDevice->PortOffset);
+	// restore MAC context, except CR0
+	MACvRestoreContext(dwIoBase, abyTmpRegData);
+
+	return bRetVal;
 }
 
 /*
@@ -839,52 +829,52 @@ bool MACbSafeSoftwareReset (unsigned long dwIoBase)
  * Return Value: true if success; otherwise false
  *
  */
-bool MACbSafeRxOff (unsigned long dwIoBase)
+bool MACbSafeRxOff(unsigned long dwIoBase)
 {
-    unsigned short ww;
-    unsigned long dwData;
-    unsigned char byData;
-
-    // turn off wow temp for turn off Rx safely
-
-    // Clear RX DMA0,1
-    VNSvOutPortD(dwIoBase + MAC_REG_RXDMACTL0, DMACTL_CLRRUN);
-    VNSvOutPortD(dwIoBase + MAC_REG_RXDMACTL1, DMACTL_CLRRUN);
-    for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-        VNSvInPortD(dwIoBase + MAC_REG_RXDMACTL0, &dwData);
-        if (!(dwData & DMACTL_RUN))
-            break;
-    }
-    if (ww == W_MAX_TIMEOUT) {
-        DBG_PORT80(0x10);
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" DBG_PORT80(0x10)\n");
-        return(false);
-    }
-    for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-        VNSvInPortD(dwIoBase + MAC_REG_RXDMACTL1, &dwData);
-        if ( !(dwData & DMACTL_RUN))
-            break;
-    }
-    if (ww == W_MAX_TIMEOUT) {
-        DBG_PORT80(0x11);
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" DBG_PORT80(0x11)\n");
-        return(false);
-    }
-
-    // try to safe shutdown RX
-    MACvRegBitsOff(dwIoBase, MAC_REG_HOSTCR, HOSTCR_RXON);
-    // W_MAX_TIMEOUT is the timeout period
-    for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-        VNSvInPortB(dwIoBase + MAC_REG_HOSTCR, &byData);
-        if ( !(byData & HOSTCR_RXONST))
-            break;
-    }
-    if (ww == W_MAX_TIMEOUT) {
-        DBG_PORT80(0x12);
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" DBG_PORT80(0x12)\n");
-        return(false);
-    }
-    return true;
+	unsigned short ww;
+	unsigned long dwData;
+	unsigned char byData;
+
+	// turn off wow temp for turn off Rx safely
+
+	// Clear RX DMA0,1
+	VNSvOutPortD(dwIoBase + MAC_REG_RXDMACTL0, DMACTL_CLRRUN);
+	VNSvOutPortD(dwIoBase + MAC_REG_RXDMACTL1, DMACTL_CLRRUN);
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		VNSvInPortD(dwIoBase + MAC_REG_RXDMACTL0, &dwData);
+		if (!(dwData & DMACTL_RUN))
+			break;
+	}
+	if (ww == W_MAX_TIMEOUT) {
+		DBG_PORT80(0x10);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x10)\n");
+		return false;
+	}
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		VNSvInPortD(dwIoBase + MAC_REG_RXDMACTL1, &dwData);
+		if (!(dwData & DMACTL_RUN))
+			break;
+	}
+	if (ww == W_MAX_TIMEOUT) {
+		DBG_PORT80(0x11);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x11)\n");
+		return false;
+	}
+
+	// try to safe shutdown RX
+	MACvRegBitsOff(dwIoBase, MAC_REG_HOSTCR, HOSTCR_RXON);
+	// W_MAX_TIMEOUT is the timeout period
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		VNSvInPortB(dwIoBase + MAC_REG_HOSTCR, &byData);
+		if (!(byData & HOSTCR_RXONST))
+			break;
+	}
+	if (ww == W_MAX_TIMEOUT) {
+		DBG_PORT80(0x12);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x12)\n");
+		return false;
+	}
+	return true;
 }
 
 /*
@@ -900,55 +890,54 @@ bool MACbSafeRxOff (unsigned long dwIoBase)
  * Return Value: true if success; otherwise false
  *
  */
-bool MACbSafeTxOff (unsigned long dwIoBase)
+bool MACbSafeTxOff(unsigned long dwIoBase)
 {
-    unsigned short ww;
-    unsigned long dwData;
-    unsigned char byData;
-
-    // Clear TX DMA
-    //Tx0
-    VNSvOutPortD(dwIoBase + MAC_REG_TXDMACTL0, DMACTL_CLRRUN);
-    //AC0
-    VNSvOutPortD(dwIoBase + MAC_REG_AC0DMACTL, DMACTL_CLRRUN);
-
-
-    for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-        VNSvInPortD(dwIoBase + MAC_REG_TXDMACTL0, &dwData);
-        if ( !(dwData & DMACTL_RUN))
-            break;
-    }
-    if (ww == W_MAX_TIMEOUT) {
-        DBG_PORT80(0x20);
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" DBG_PORT80(0x20)\n");
-        return(false);
-    }
-    for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-        VNSvInPortD(dwIoBase + MAC_REG_AC0DMACTL, &dwData);
-        if ( !(dwData & DMACTL_RUN))
-            break;
-    }
-    if (ww == W_MAX_TIMEOUT) {
-        DBG_PORT80(0x21);
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" DBG_PORT80(0x21)\n");
-        return(false);
-    }
-
-    // try to safe shutdown TX
-    MACvRegBitsOff(dwIoBase, MAC_REG_HOSTCR, HOSTCR_TXON);
-
-    // W_MAX_TIMEOUT is the timeout period
-    for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-        VNSvInPortB(dwIoBase + MAC_REG_HOSTCR, &byData);
-        if ( !(byData & HOSTCR_TXONST))
-            break;
-    }
-    if (ww == W_MAX_TIMEOUT) {
-        DBG_PORT80(0x24);
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" DBG_PORT80(0x24)\n");
-        return(false);
-    }
-    return true;
+	unsigned short ww;
+	unsigned long dwData;
+	unsigned char byData;
+
+	// Clear TX DMA
+	//Tx0
+	VNSvOutPortD(dwIoBase + MAC_REG_TXDMACTL0, DMACTL_CLRRUN);
+	//AC0
+	VNSvOutPortD(dwIoBase + MAC_REG_AC0DMACTL, DMACTL_CLRRUN);
+
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		VNSvInPortD(dwIoBase + MAC_REG_TXDMACTL0, &dwData);
+		if (!(dwData & DMACTL_RUN))
+			break;
+	}
+	if (ww == W_MAX_TIMEOUT) {
+		DBG_PORT80(0x20);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x20)\n");
+		return false;
+	}
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		VNSvInPortD(dwIoBase + MAC_REG_AC0DMACTL, &dwData);
+		if (!(dwData & DMACTL_RUN))
+			break;
+	}
+	if (ww == W_MAX_TIMEOUT) {
+		DBG_PORT80(0x21);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x21)\n");
+		return false;
+	}
+
+	// try to safe shutdown TX
+	MACvRegBitsOff(dwIoBase, MAC_REG_HOSTCR, HOSTCR_TXON);
+
+	// W_MAX_TIMEOUT is the timeout period
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		VNSvInPortB(dwIoBase + MAC_REG_HOSTCR, &byData);
+		if (!(byData & HOSTCR_TXONST))
+			break;
+	}
+	if (ww == W_MAX_TIMEOUT) {
+		DBG_PORT80(0x24);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x24)\n");
+		return false;
+	}
+	return true;
 }
 
 /*
@@ -964,26 +953,26 @@ bool MACbSafeTxOff (unsigned long dwIoBase)
  * Return Value: true if success; otherwise false
  *
  */
-bool MACbSafeStop (unsigned long dwIoBase)
+bool MACbSafeStop(unsigned long dwIoBase)
 {
-    MACvRegBitsOff(dwIoBase, MAC_REG_TCR, TCR_AUTOBCNTX);
-
-    if (MACbSafeRxOff(dwIoBase) == false) {
-        DBG_PORT80(0xA1);
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" MACbSafeRxOff == false)\n");
-        MACbSafeSoftwareReset(dwIoBase);
-        return false;
-    }
-    if (MACbSafeTxOff(dwIoBase) == false) {
-        DBG_PORT80(0xA2);
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" MACbSafeTxOff == false)\n");
-        MACbSafeSoftwareReset(dwIoBase);
-        return false;
-    }
-
-    MACvRegBitsOff(dwIoBase, MAC_REG_HOSTCR, HOSTCR_MACEN);
-
-    return true;
+	MACvRegBitsOff(dwIoBase, MAC_REG_TCR, TCR_AUTOBCNTX);
+
+	if (MACbSafeRxOff(dwIoBase) == false) {
+		DBG_PORT80(0xA1);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " MACbSafeRxOff == false)\n");
+		MACbSafeSoftwareReset(dwIoBase);
+		return false;
+	}
+	if (MACbSafeTxOff(dwIoBase) == false) {
+		DBG_PORT80(0xA2);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " MACbSafeTxOff == false)\n");
+		MACbSafeSoftwareReset(dwIoBase);
+		return false;
+	}
+
+	MACvRegBitsOff(dwIoBase, MAC_REG_HOSTCR, HOSTCR_MACEN);
+
+	return true;
 }
 
 /*
@@ -999,18 +988,18 @@ bool MACbSafeStop (unsigned long dwIoBase)
  * Return Value: true if success; otherwise false
  *
  */
-bool MACbShutdown (unsigned long dwIoBase)
+bool MACbShutdown(unsigned long dwIoBase)
 {
-    // disable MAC IMR
-    MACvIntDisable(dwIoBase);
-    MACvSetLoopbackMode(dwIoBase, MAC_LB_INTERNAL);
-    // stop the adapter
-    if (!MACbSafeStop(dwIoBase)) {
-        MACvSetLoopbackMode(dwIoBase, MAC_LB_NONE);
-        return false;
-    }
-    MACvSetLoopbackMode(dwIoBase, MAC_LB_NONE);
-    return true;
+	// disable MAC IMR
+	MACvIntDisable(dwIoBase);
+	MACvSetLoopbackMode(dwIoBase, MAC_LB_INTERNAL);
+	// stop the adapter
+	if (!MACbSafeStop(dwIoBase)) {
+		MACvSetLoopbackMode(dwIoBase, MAC_LB_NONE);
+		return false;
+	}
+	MACvSetLoopbackMode(dwIoBase, MAC_LB_NONE);
+	return true;
 }
 
 /*
@@ -1026,43 +1015,31 @@ bool MACbShutdown (unsigned long dwIoBase)
  * Return Value: none
  *
  */
-void MACvInitialize (unsigned long dwIoBase)
+void MACvInitialize(unsigned long dwIoBase)
 {
-    // clear sticky bits
-    MACvClearStckDS(dwIoBase);
-    // disable force PME-enable
-    VNSvOutPortB(dwIoBase + MAC_REG_PMC1, PME_OVR);
-    // only 3253 A
-    /*
-    MACvPwrEvntDisable(dwIoBase);
-    // clear power status
-    VNSvOutPortW(dwIoBase + MAC_REG_WAKEUPSR0, 0x0F0F);
-    */
-
-    // do reset
-    MACbSoftwareReset(dwIoBase);
-
-    // issue AUTOLD in EECSR to reload eeprom
-    //MACvRegBitsOn(dwIoBase, MAC_REG_I2MCSR, I2MCSR_AUTOLD);
-    // wait until EEPROM loading complete
-    //while (true) {
-    //    u8 u8Data;
-    //    VNSvInPortB(dwIoBase + MAC_REG_I2MCSR, &u8Data);
-    //    if ( !(u8Data & I2MCSR_AUTOLD))
-    //        break;
-    //}
-
-    // reset TSF counter
-    VNSvOutPortB(dwIoBase + MAC_REG_TFTCTL, TFTCTL_TSFCNTRST);
-    // enable TSF counter
-    VNSvOutPortB(dwIoBase + MAC_REG_TFTCTL, TFTCTL_TSFCNTREN);
-
-
-    // set packet filter
-    // receive directed and broadcast address
-
-    MACvSetPacketFilter(dwIoBase, PKT_TYPE_DIRECTED | PKT_TYPE_BROADCAST);
-
+	// clear sticky bits
+	MACvClearStckDS(dwIoBase);
+	// disable force PME-enable
+	VNSvOutPortB(dwIoBase + MAC_REG_PMC1, PME_OVR);
+	// only 3253 A
+	/*
+	  MACvPwrEvntDisable(dwIoBase);
+	  // clear power status
+	  VNSvOutPortW(dwIoBase + MAC_REG_WAKEUPSR0, 0x0F0F);
+	*/
+
+	// do reset
+	MACbSoftwareReset(dwIoBase);
+
+	// reset TSF counter
+	VNSvOutPortB(dwIoBase + MAC_REG_TFTCTL, TFTCTL_TSFCNTRST);
+	// enable TSF counter
+	VNSvOutPortB(dwIoBase + MAC_REG_TFTCTL, TFTCTL_TSFCNTREN);
+
+	// set packet filter
+	// receive directed and broadcast address
+
+	MACvSetPacketFilter(dwIoBase, PKT_TYPE_DIRECTED | PKT_TYPE_BROADCAST);
 }
 
 /*
@@ -1079,28 +1056,28 @@ void MACvInitialize (unsigned long dwIoBase)
  * Return Value: none
  *
  */
-void MACvSetCurrRx0DescAddr (unsigned long dwIoBase, unsigned long dwCurrDescAddr)
+void MACvSetCurrRx0DescAddr(unsigned long dwIoBase, unsigned long dwCurrDescAddr)
 {
-unsigned short ww;
-unsigned char byData;
-unsigned char byOrgDMACtl;
-
-    VNSvInPortB(dwIoBase + MAC_REG_RXDMACTL0, &byOrgDMACtl);
-    if (byOrgDMACtl & DMACTL_RUN) {
-        VNSvOutPortB(dwIoBase + MAC_REG_RXDMACTL0+2, DMACTL_RUN);
-    }
-    for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-        VNSvInPortB(dwIoBase + MAC_REG_RXDMACTL0, &byData);
-        if ( !(byData & DMACTL_RUN))
-            break;
-    }
-    if (ww == W_MAX_TIMEOUT) {
-        DBG_PORT80(0x13);
-    }
-    VNSvOutPortD(dwIoBase + MAC_REG_RXDMAPTR0, dwCurrDescAddr);
-    if (byOrgDMACtl & DMACTL_RUN) {
-        VNSvOutPortB(dwIoBase + MAC_REG_RXDMACTL0, DMACTL_RUN);
-    }
+	unsigned short ww;
+	unsigned char byData;
+	unsigned char byOrgDMACtl;
+
+	VNSvInPortB(dwIoBase + MAC_REG_RXDMACTL0, &byOrgDMACtl);
+	if (byOrgDMACtl & DMACTL_RUN) {
+		VNSvOutPortB(dwIoBase + MAC_REG_RXDMACTL0+2, DMACTL_RUN);
+	}
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		VNSvInPortB(dwIoBase + MAC_REG_RXDMACTL0, &byData);
+		if (!(byData & DMACTL_RUN))
+			break;
+	}
+	if (ww == W_MAX_TIMEOUT) {
+		DBG_PORT80(0x13);
+	}
+	VNSvOutPortD(dwIoBase + MAC_REG_RXDMAPTR0, dwCurrDescAddr);
+	if (byOrgDMACtl & DMACTL_RUN) {
+		VNSvOutPortB(dwIoBase + MAC_REG_RXDMACTL0, DMACTL_RUN);
+	}
 }
 
 /*
@@ -1117,28 +1094,28 @@ unsigned char byOrgDMACtl;
  * Return Value: none
  *
  */
-void MACvSetCurrRx1DescAddr (unsigned long dwIoBase, unsigned long dwCurrDescAddr)
+void MACvSetCurrRx1DescAddr(unsigned long dwIoBase, unsigned long dwCurrDescAddr)
 {
-unsigned short ww;
-unsigned char byData;
-unsigned char byOrgDMACtl;
-
-    VNSvInPortB(dwIoBase + MAC_REG_RXDMACTL1, &byOrgDMACtl);
-    if (byOrgDMACtl & DMACTL_RUN) {
-        VNSvOutPortB(dwIoBase + MAC_REG_RXDMACTL1+2, DMACTL_RUN);
-    }
-    for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-        VNSvInPortB(dwIoBase + MAC_REG_RXDMACTL1, &byData);
-        if ( !(byData & DMACTL_RUN))
-            break;
-    }
-    if (ww == W_MAX_TIMEOUT) {
-        DBG_PORT80(0x14);
-    }
-    VNSvOutPortD(dwIoBase + MAC_REG_RXDMAPTR1, dwCurrDescAddr);
-    if (byOrgDMACtl & DMACTL_RUN) {
-        VNSvOutPortB(dwIoBase + MAC_REG_RXDMACTL1, DMACTL_RUN);
-    }
+	unsigned short ww;
+	unsigned char byData;
+	unsigned char byOrgDMACtl;
+
+	VNSvInPortB(dwIoBase + MAC_REG_RXDMACTL1, &byOrgDMACtl);
+	if (byOrgDMACtl & DMACTL_RUN) {
+		VNSvOutPortB(dwIoBase + MAC_REG_RXDMACTL1+2, DMACTL_RUN);
+	}
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		VNSvInPortB(dwIoBase + MAC_REG_RXDMACTL1, &byData);
+		if (!(byData & DMACTL_RUN))
+			break;
+	}
+	if (ww == W_MAX_TIMEOUT) {
+		DBG_PORT80(0x14);
+	}
+	VNSvOutPortD(dwIoBase + MAC_REG_RXDMAPTR1, dwCurrDescAddr);
+	if (byOrgDMACtl & DMACTL_RUN) {
+		VNSvOutPortB(dwIoBase + MAC_REG_RXDMACTL1, DMACTL_RUN);
+	}
 }
 
 /*
@@ -1155,28 +1132,28 @@ unsigned char byOrgDMACtl;
  * Return Value: none
  *
  */
-void MACvSetCurrTx0DescAddrEx (unsigned long dwIoBase, unsigned long dwCurrDescAddr)
+void MACvSetCurrTx0DescAddrEx(unsigned long dwIoBase, unsigned long dwCurrDescAddr)
 {
-unsigned short ww;
-unsigned char byData;
-unsigned char byOrgDMACtl;
-
-    VNSvInPortB(dwIoBase + MAC_REG_TXDMACTL0, &byOrgDMACtl);
-    if (byOrgDMACtl & DMACTL_RUN) {
-        VNSvOutPortB(dwIoBase + MAC_REG_TXDMACTL0+2, DMACTL_RUN);
-    }
-    for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-        VNSvInPortB(dwIoBase + MAC_REG_TXDMACTL0, &byData);
-        if ( !(byData & DMACTL_RUN))
-            break;
-    }
-    if (ww == W_MAX_TIMEOUT) {
-        DBG_PORT80(0x25);
-    }
-    VNSvOutPortD(dwIoBase + MAC_REG_TXDMAPTR0, dwCurrDescAddr);
-    if (byOrgDMACtl & DMACTL_RUN) {
-        VNSvOutPortB(dwIoBase + MAC_REG_TXDMACTL0, DMACTL_RUN);
-    }
+	unsigned short ww;
+	unsigned char byData;
+	unsigned char byOrgDMACtl;
+
+	VNSvInPortB(dwIoBase + MAC_REG_TXDMACTL0, &byOrgDMACtl);
+	if (byOrgDMACtl & DMACTL_RUN) {
+		VNSvOutPortB(dwIoBase + MAC_REG_TXDMACTL0+2, DMACTL_RUN);
+	}
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		VNSvInPortB(dwIoBase + MAC_REG_TXDMACTL0, &byData);
+		if (!(byData & DMACTL_RUN))
+			break;
+	}
+	if (ww == W_MAX_TIMEOUT) {
+		DBG_PORT80(0x25);
+	}
+	VNSvOutPortD(dwIoBase + MAC_REG_TXDMAPTR0, dwCurrDescAddr);
+	if (byOrgDMACtl & DMACTL_RUN) {
+		VNSvOutPortB(dwIoBase + MAC_REG_TXDMACTL0, DMACTL_RUN);
+	}
 }
 
 /*
@@ -1193,41 +1170,39 @@ unsigned char byOrgDMACtl;
  * Return Value: none
  *
  */
- //TxDMA1 = AC0DMA
-void MACvSetCurrAC0DescAddrEx (unsigned long dwIoBase, unsigned long dwCurrDescAddr)
+//TxDMA1 = AC0DMA
+void MACvSetCurrAC0DescAddrEx(unsigned long dwIoBase, unsigned long dwCurrDescAddr)
 {
-unsigned short ww;
-unsigned char byData;
-unsigned char byOrgDMACtl;
-
-    VNSvInPortB(dwIoBase + MAC_REG_AC0DMACTL, &byOrgDMACtl);
-    if (byOrgDMACtl & DMACTL_RUN) {
-        VNSvOutPortB(dwIoBase + MAC_REG_AC0DMACTL+2, DMACTL_RUN);
-    }
-    for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-        VNSvInPortB(dwIoBase + MAC_REG_AC0DMACTL, &byData);
-        if (!(byData & DMACTL_RUN))
-            break;
-    }
-    if (ww == W_MAX_TIMEOUT) {
-        DBG_PORT80(0x26);
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" DBG_PORT80(0x26)\n");
-    }
-    VNSvOutPortD(dwIoBase + MAC_REG_AC0DMAPTR, dwCurrDescAddr);
-    if (byOrgDMACtl & DMACTL_RUN) {
-        VNSvOutPortB(dwIoBase + MAC_REG_AC0DMACTL, DMACTL_RUN);
-    }
+	unsigned short ww;
+	unsigned char byData;
+	unsigned char byOrgDMACtl;
+
+	VNSvInPortB(dwIoBase + MAC_REG_AC0DMACTL, &byOrgDMACtl);
+	if (byOrgDMACtl & DMACTL_RUN) {
+		VNSvOutPortB(dwIoBase + MAC_REG_AC0DMACTL+2, DMACTL_RUN);
+	}
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		VNSvInPortB(dwIoBase + MAC_REG_AC0DMACTL, &byData);
+		if (!(byData & DMACTL_RUN))
+			break;
+	}
+	if (ww == W_MAX_TIMEOUT) {
+		DBG_PORT80(0x26);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x26)\n");
+	}
+	VNSvOutPortD(dwIoBase + MAC_REG_AC0DMAPTR, dwCurrDescAddr);
+	if (byOrgDMACtl & DMACTL_RUN) {
+		VNSvOutPortB(dwIoBase + MAC_REG_AC0DMACTL, DMACTL_RUN);
+	}
 }
 
-
-
-void MACvSetCurrTXDescAddr (int iTxType, unsigned long dwIoBase, unsigned long dwCurrDescAddr)
+void MACvSetCurrTXDescAddr(int iTxType, unsigned long dwIoBase, unsigned long dwCurrDescAddr)
 {
-    if(iTxType == TYPE_AC0DMA){
-        MACvSetCurrAC0DescAddrEx(dwIoBase, dwCurrDescAddr);
-    }else if(iTxType == TYPE_TXDMA0){
-        MACvSetCurrTx0DescAddrEx(dwIoBase, dwCurrDescAddr);
-    }
+	if (iTxType == TYPE_AC0DMA) {
+		MACvSetCurrAC0DescAddrEx(dwIoBase, dwCurrDescAddr);
+	} else if (iTxType == TYPE_TXDMA0) {
+		MACvSetCurrTx0DescAddrEx(dwIoBase, dwCurrDescAddr);
+	}
 }
 
 /*
@@ -1244,26 +1219,25 @@ void MACvSetCurrTXDescAddr (int iTxType, unsigned long dwIoBase, unsigned long d
  * Return Value: none
  *
  */
-void MACvTimer0MicroSDelay (unsigned long dwIoBase, unsigned int uDelay)
+void MACvTimer0MicroSDelay(unsigned long dwIoBase, unsigned int uDelay)
 {
-unsigned char byValue;
-unsigned int uu,ii;
-
-    VNSvOutPortB(dwIoBase + MAC_REG_TMCTL0, 0);
-    VNSvOutPortD(dwIoBase + MAC_REG_TMDATA0, uDelay);
-    VNSvOutPortB(dwIoBase + MAC_REG_TMCTL0, (TMCTL_TMD | TMCTL_TE));
-    for(ii=0;ii<66;ii++) {  // assume max PCI clock is 66Mhz
-        for (uu = 0; uu < uDelay; uu++) {
-            VNSvInPortB(dwIoBase + MAC_REG_TMCTL0, &byValue);
-            if ((byValue == 0) ||
-                (byValue & TMCTL_TSUSP)) {
-                VNSvOutPortB(dwIoBase + MAC_REG_TMCTL0, 0);
-                return;
-            }
-        }
-    }
-    VNSvOutPortB(dwIoBase + MAC_REG_TMCTL0, 0);
-
+	unsigned char byValue;
+	unsigned int uu, ii;
+
+	VNSvOutPortB(dwIoBase + MAC_REG_TMCTL0, 0);
+	VNSvOutPortD(dwIoBase + MAC_REG_TMDATA0, uDelay);
+	VNSvOutPortB(dwIoBase + MAC_REG_TMCTL0, (TMCTL_TMD | TMCTL_TE));
+	for (ii = 0; ii < 66; ii++) {  // assume max PCI clock is 66Mhz
+		for (uu = 0; uu < uDelay; uu++) {
+			VNSvInPortB(dwIoBase + MAC_REG_TMCTL0, &byValue);
+			if ((byValue == 0) ||
+			    (byValue & TMCTL_TSUSP)) {
+				VNSvOutPortB(dwIoBase + MAC_REG_TMCTL0, 0);
+				return;
+			}
+		}
+	}
+	VNSvOutPortB(dwIoBase + MAC_REG_TMCTL0, 0);
 }
 
 /*
@@ -1280,11 +1254,11 @@ unsigned int uu,ii;
  * Return Value: none
  *
  */
-void MACvOneShotTimer0MicroSec (unsigned long dwIoBase, unsigned int uDelayTime)
+void MACvOneShotTimer0MicroSec(unsigned long dwIoBase, unsigned int uDelayTime)
 {
-    VNSvOutPortB(dwIoBase + MAC_REG_TMCTL0, 0);
-    VNSvOutPortD(dwIoBase + MAC_REG_TMDATA0, uDelayTime);
-    VNSvOutPortB(dwIoBase + MAC_REG_TMCTL0, (TMCTL_TMD | TMCTL_TE));
+	VNSvOutPortB(dwIoBase + MAC_REG_TMCTL0, 0);
+	VNSvOutPortD(dwIoBase + MAC_REG_TMDATA0, uDelayTime);
+	VNSvOutPortB(dwIoBase + MAC_REG_TMCTL0, (TMCTL_TMD | TMCTL_TE));
 }
 
 /*
@@ -1301,143 +1275,141 @@ void MACvOneShotTimer0MicroSec (unsigned long dwIoBase, unsigned int uDelayTime)
  * Return Value: none
  *
  */
-void MACvOneShotTimer1MicroSec (unsigned long dwIoBase, unsigned int uDelayTime)
+void MACvOneShotTimer1MicroSec(unsigned long dwIoBase, unsigned int uDelayTime)
 {
-    VNSvOutPortB(dwIoBase + MAC_REG_TMCTL1, 0);
-    VNSvOutPortD(dwIoBase + MAC_REG_TMDATA1, uDelayTime);
-    VNSvOutPortB(dwIoBase + MAC_REG_TMCTL1, (TMCTL_TMD | TMCTL_TE));
+	VNSvOutPortB(dwIoBase + MAC_REG_TMCTL1, 0);
+	VNSvOutPortD(dwIoBase + MAC_REG_TMDATA1, uDelayTime);
+	VNSvOutPortB(dwIoBase + MAC_REG_TMCTL1, (TMCTL_TMD | TMCTL_TE));
 }
 
-
-void MACvSetMISCFifo (unsigned long dwIoBase, unsigned short wOffset, unsigned long dwData)
+void MACvSetMISCFifo(unsigned long dwIoBase, unsigned short wOffset, unsigned long dwData)
 {
-    if (wOffset > 273)
-        return;
-    VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
-    VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
-    VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
+	if (wOffset > 273)
+		return;
+	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
+	VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
+	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
 }
 
-
-bool MACbTxDMAOff (unsigned long dwIoBase, unsigned int idx)
+bool MACbTxDMAOff(unsigned long dwIoBase, unsigned int idx)
 {
-unsigned char byData;
-unsigned int ww = 0;
-
-    if (idx == TYPE_TXDMA0) {
-        VNSvOutPortB(dwIoBase + MAC_REG_TXDMACTL0+2, DMACTL_RUN);
-        for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-            VNSvInPortB(dwIoBase + MAC_REG_TXDMACTL0, &byData);
-            if ( !(byData & DMACTL_RUN))
-                break;
-        }
-    } else if (idx == TYPE_AC0DMA) {
-        VNSvOutPortB(dwIoBase + MAC_REG_AC0DMACTL+2, DMACTL_RUN);
-        for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-            VNSvInPortB(dwIoBase + MAC_REG_AC0DMACTL, &byData);
-            if ( !(byData & DMACTL_RUN))
-                break;
-        }
-    }
-    if (ww == W_MAX_TIMEOUT) {
-        DBG_PORT80(0x29);
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" DBG_PORT80(0x29)\n");
-        return false;
-    }
-    return true;
+	unsigned char byData;
+	unsigned int ww = 0;
+
+	if (idx == TYPE_TXDMA0) {
+		VNSvOutPortB(dwIoBase + MAC_REG_TXDMACTL0+2, DMACTL_RUN);
+		for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+			VNSvInPortB(dwIoBase + MAC_REG_TXDMACTL0, &byData);
+			if (!(byData & DMACTL_RUN))
+				break;
+		}
+	} else if (idx == TYPE_AC0DMA) {
+		VNSvOutPortB(dwIoBase + MAC_REG_AC0DMACTL+2, DMACTL_RUN);
+		for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+			VNSvInPortB(dwIoBase + MAC_REG_AC0DMACTL, &byData);
+			if (!(byData & DMACTL_RUN))
+				break;
+		}
+	}
+	if (ww == W_MAX_TIMEOUT) {
+		DBG_PORT80(0x29);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x29)\n");
+		return false;
+	}
+	return true;
 }
 
-void MACvClearBusSusInd (unsigned long dwIoBase)
+void MACvClearBusSusInd(unsigned long dwIoBase)
 {
-    unsigned long dwOrgValue;
-    unsigned int ww;
-    // check if BcnSusInd enabled
-    VNSvInPortD(dwIoBase + MAC_REG_ENCFG , &dwOrgValue);
-    if( !(dwOrgValue & EnCFG_BcnSusInd))
-        return;
-    //Set BcnSusClr
-    dwOrgValue = dwOrgValue | EnCFG_BcnSusClr;
-    VNSvOutPortD(dwIoBase + MAC_REG_ENCFG, dwOrgValue);
-    for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-        VNSvInPortD(dwIoBase + MAC_REG_ENCFG , &dwOrgValue);
-        if( !(dwOrgValue & EnCFG_BcnSusInd))
-            break;
-    }
-    if (ww == W_MAX_TIMEOUT) {
-        DBG_PORT80(0x33);
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" DBG_PORT80(0x33)\n");
-    }
+	unsigned long dwOrgValue;
+	unsigned int ww;
+	// check if BcnSusInd enabled
+	VNSvInPortD(dwIoBase + MAC_REG_ENCFG , &dwOrgValue);
+	if (!(dwOrgValue & EnCFG_BcnSusInd))
+		return;
+	//Set BcnSusClr
+	dwOrgValue = dwOrgValue | EnCFG_BcnSusClr;
+	VNSvOutPortD(dwIoBase + MAC_REG_ENCFG, dwOrgValue);
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		VNSvInPortD(dwIoBase + MAC_REG_ENCFG , &dwOrgValue);
+		if (!(dwOrgValue & EnCFG_BcnSusInd))
+			break;
+	}
+	if (ww == W_MAX_TIMEOUT) {
+		DBG_PORT80(0x33);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x33)\n");
+	}
 }
 
-void MACvEnableBusSusEn (unsigned long dwIoBase)
+void MACvEnableBusSusEn(unsigned long dwIoBase)
 {
-    unsigned char byOrgValue;
-    unsigned long dwOrgValue;
-    unsigned int ww;
-    // check if BcnSusInd enabled
-    VNSvInPortB(dwIoBase + MAC_REG_CFG , &byOrgValue);
-
-    //Set BcnSusEn
-    byOrgValue = byOrgValue | CFG_BCNSUSEN;
-    VNSvOutPortB(dwIoBase + MAC_REG_ENCFG, byOrgValue);
-    for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-        VNSvInPortD(dwIoBase + MAC_REG_ENCFG , &dwOrgValue);
-        if(dwOrgValue & EnCFG_BcnSusInd)
-            break;
-    }
-    if (ww == W_MAX_TIMEOUT) {
-        DBG_PORT80(0x34);
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" DBG_PORT80(0x34)\n");
-    }
+	unsigned char byOrgValue;
+	unsigned long dwOrgValue;
+	unsigned int ww;
+	// check if BcnSusInd enabled
+	VNSvInPortB(dwIoBase + MAC_REG_CFG , &byOrgValue);
+
+	//Set BcnSusEn
+	byOrgValue = byOrgValue | CFG_BCNSUSEN;
+	VNSvOutPortB(dwIoBase + MAC_REG_ENCFG, byOrgValue);
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		VNSvInPortD(dwIoBase + MAC_REG_ENCFG , &dwOrgValue);
+		if (dwOrgValue & EnCFG_BcnSusInd)
+			break;
+	}
+	if (ww == W_MAX_TIMEOUT) {
+		DBG_PORT80(0x34);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x34)\n");
+	}
 }
 
-bool MACbFlushSYNCFifo (unsigned long dwIoBase)
+bool MACbFlushSYNCFifo(unsigned long dwIoBase)
 {
-    unsigned char byOrgValue;
-    unsigned int ww;
-    // Read MACCR
-    VNSvInPortB(dwIoBase + MAC_REG_MACCR , &byOrgValue);
-
-    // Set SYNCFLUSH
-    byOrgValue = byOrgValue | MACCR_SYNCFLUSH;
-    VNSvOutPortB(dwIoBase + MAC_REG_MACCR, byOrgValue);
-
-    // Check if SyncFlushOK
-    for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-        VNSvInPortB(dwIoBase + MAC_REG_MACCR , &byOrgValue);
-        if(byOrgValue & MACCR_SYNCFLUSHOK)
-            break;
-    }
-    if (ww == W_MAX_TIMEOUT) {
-        DBG_PORT80(0x35);
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" DBG_PORT80(0x33)\n");
-    }
-    return true;
+	unsigned char byOrgValue;
+	unsigned int ww;
+	// Read MACCR
+	VNSvInPortB(dwIoBase + MAC_REG_MACCR , &byOrgValue);
+
+	// Set SYNCFLUSH
+	byOrgValue = byOrgValue | MACCR_SYNCFLUSH;
+	VNSvOutPortB(dwIoBase + MAC_REG_MACCR, byOrgValue);
+
+	// Check if SyncFlushOK
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		VNSvInPortB(dwIoBase + MAC_REG_MACCR , &byOrgValue);
+		if (byOrgValue & MACCR_SYNCFLUSHOK)
+			break;
+	}
+	if (ww == W_MAX_TIMEOUT) {
+		DBG_PORT80(0x35);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x33)\n");
+	}
+	return true;
 }
 
-bool MACbPSWakeup (unsigned long dwIoBase)
+bool MACbPSWakeup(unsigned long dwIoBase)
 {
-    unsigned char byOrgValue;
-    unsigned int ww;
-    // Read PSCTL
-    if (MACbIsRegBitsOff(dwIoBase, MAC_REG_PSCTL, PSCTL_PS)) {
-        return true;
-    }
-    // Disable PS
-    MACvRegBitsOff(dwIoBase, MAC_REG_PSCTL, PSCTL_PSEN);
-
-    // Check if SyncFlushOK
-    for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-        VNSvInPortB(dwIoBase + MAC_REG_PSCTL , &byOrgValue);
-        if(byOrgValue & PSCTL_WAKEDONE)
-            break;
-    }
-    if (ww == W_MAX_TIMEOUT) {
-        DBG_PORT80(0x36);
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" DBG_PORT80(0x33)\n");
-        return false;
-    }
-    return true;
+	unsigned char byOrgValue;
+	unsigned int ww;
+	// Read PSCTL
+	if (MACbIsRegBitsOff(dwIoBase, MAC_REG_PSCTL, PSCTL_PS)) {
+		return true;
+	}
+	// Disable PS
+	MACvRegBitsOff(dwIoBase, MAC_REG_PSCTL, PSCTL_PSEN);
+
+	// Check if SyncFlushOK
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		VNSvInPortB(dwIoBase + MAC_REG_PSCTL , &byOrgValue);
+		if (byOrgValue & PSCTL_WAKEDONE)
+			break;
+	}
+	if (ww == W_MAX_TIMEOUT) {
+		DBG_PORT80(0x36);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x33)\n");
+		return false;
+	}
+	return true;
 }
 
 /*
@@ -1455,59 +1427,56 @@ bool MACbPSWakeup (unsigned long dwIoBase)
  *
  */
 
-void MACvSetKeyEntry (unsigned long dwIoBase, unsigned short wKeyCtl, unsigned int uEntryIdx,
-		unsigned int uKeyIdx, unsigned char *pbyAddr, unsigned long *pdwKey, unsigned char byLocalID)
+void MACvSetKeyEntry(unsigned long dwIoBase, unsigned short wKeyCtl, unsigned int uEntryIdx,
+		     unsigned int uKeyIdx, unsigned char *pbyAddr, unsigned long *pdwKey, unsigned char byLocalID)
 {
-unsigned short wOffset;
-unsigned long dwData;
-int     ii;
-
-    if (byLocalID <= 1)
-        return;
-
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"MACvSetKeyEntry\n");
-    wOffset = MISCFIFO_KEYETRY0;
-    wOffset += (uEntryIdx * MISCFIFO_KEYENTRYSIZE);
-
-    dwData = 0;
-    dwData |= wKeyCtl;
-    dwData <<= 16;
-    dwData |= MAKEWORD(*(pbyAddr+4), *(pbyAddr+5));
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"1. wOffset: %d, Data: %lX, KeyCtl:%X\n", wOffset, dwData, wKeyCtl);
-
-    VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
-    VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
-    VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
-    wOffset++;
-
-    dwData = 0;
-    dwData |= *(pbyAddr+3);
-    dwData <<= 8;
-    dwData |= *(pbyAddr+2);
-    dwData <<= 8;
-    dwData |= *(pbyAddr+1);
-    dwData <<= 8;
-    dwData |= *(pbyAddr+0);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"2. wOffset: %d, Data: %lX\n", wOffset, dwData);
-
-    VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
-    VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
-    VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
-    wOffset++;
-
-    wOffset += (uKeyIdx * 4);
-    for (ii=0;ii<4;ii++) {
-        // always push 128 bits
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"3.(%d) wOffset: %d, Data: %lX\n", ii, wOffset+ii, *pdwKey);
-        VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset+ii);
-        VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, *pdwKey++);
-        VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
-    }
+	unsigned short wOffset;
+	unsigned long dwData;
+	int     ii;
+
+	if (byLocalID <= 1)
+		return;
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "MACvSetKeyEntry\n");
+	wOffset = MISCFIFO_KEYETRY0;
+	wOffset += (uEntryIdx * MISCFIFO_KEYENTRYSIZE);
+
+	dwData = 0;
+	dwData |= wKeyCtl;
+	dwData <<= 16;
+	dwData |= MAKEWORD(*(pbyAddr+4), *(pbyAddr+5));
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "1. wOffset: %d, Data: %lX, KeyCtl:%X\n", wOffset, dwData, wKeyCtl);
+
+	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
+	VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
+	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
+	wOffset++;
+
+	dwData = 0;
+	dwData |= *(pbyAddr+3);
+	dwData <<= 8;
+	dwData |= *(pbyAddr+2);
+	dwData <<= 8;
+	dwData |= *(pbyAddr+1);
+	dwData <<= 8;
+	dwData |= *(pbyAddr+0);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "2. wOffset: %d, Data: %lX\n", wOffset, dwData);
+
+	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
+	VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
+	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
+	wOffset++;
+
+	wOffset += (uKeyIdx * 4);
+	for (ii = 0; ii < 4; ii++) {
+		// always push 128 bits
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "3.(%d) wOffset: %d, Data: %lX\n", ii, wOffset+ii, *pdwKey);
+		VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset+ii);
+		VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, *pdwKey++);
+		VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
+	}
 }
 
-
-
 /*
  * Description:
  *      Disable the Key Entry by MISCFIFO
@@ -1522,19 +1491,18 @@ int     ii;
  * Return Value: none
  *
  */
-void MACvDisableKeyEntry (unsigned long dwIoBase, unsigned int uEntryIdx)
+void MACvDisableKeyEntry(unsigned long dwIoBase, unsigned int uEntryIdx)
 {
-unsigned short wOffset;
+	unsigned short wOffset;
 
-    wOffset = MISCFIFO_KEYETRY0;
-    wOffset += (uEntryIdx * MISCFIFO_KEYENTRYSIZE);
+	wOffset = MISCFIFO_KEYETRY0;
+	wOffset += (uEntryIdx * MISCFIFO_KEYENTRYSIZE);
 
-    VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
-    VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, 0);
-    VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
+	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
+	VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, 0);
+	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
 }
 
-
 /*
  * Description:
  *      Set the default Key (KeyEntry[10]) by MISCFIFO
@@ -1550,42 +1518,40 @@ unsigned short wOffset;
  *
  */
 
-void MACvSetDefaultKeyEntry (unsigned long dwIoBase, unsigned int uKeyLen,
-		unsigned int uKeyIdx, unsigned long *pdwKey, unsigned char byLocalID)
+void MACvSetDefaultKeyEntry(unsigned long dwIoBase, unsigned int uKeyLen,
+			    unsigned int uKeyIdx, unsigned long *pdwKey, unsigned char byLocalID)
 {
-unsigned short wOffset;
-unsigned long dwData;
-int     ii;
-
-    if (byLocalID <= 1)
-        return;
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"MACvSetDefaultKeyEntry\n");
-    wOffset = MISCFIFO_KEYETRY0;
-    wOffset += (10 * MISCFIFO_KEYENTRYSIZE);
-
-    wOffset++;
-    wOffset++;
-    wOffset += (uKeyIdx * 4);
-    // always push 128 bits
-    for (ii=0; ii<3; ii++) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"(%d) wOffset: %d, Data: %lX\n", ii, wOffset+ii, *pdwKey);
-        VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset+ii);
-        VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, *pdwKey++);
-        VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
-    }
-    dwData = *pdwKey;
-    if (uKeyLen == WLAN_WEP104_KEYLEN) {
-        dwData |= 0x80000000;
-    }
-    VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset+3);
-    VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
-    VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"End. wOffset: %d, Data: %lX\n", wOffset+3, dwData);
-
+	unsigned short wOffset;
+	unsigned long dwData;
+	int     ii;
+
+	if (byLocalID <= 1)
+		return;
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "MACvSetDefaultKeyEntry\n");
+	wOffset = MISCFIFO_KEYETRY0;
+	wOffset += (10 * MISCFIFO_KEYENTRYSIZE);
+
+	wOffset++;
+	wOffset++;
+	wOffset += (uKeyIdx * 4);
+	// always push 128 bits
+	for (ii = 0; ii < 3; ii++) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "(%d) wOffset: %d, Data: %lX\n", ii, wOffset+ii, *pdwKey);
+		VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset+ii);
+		VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, *pdwKey++);
+		VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
+	}
+	dwData = *pdwKey;
+	if (uKeyLen == WLAN_WEP104_KEYLEN) {
+		dwData |= 0x80000000;
+	}
+	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset+3);
+	VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
+	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "End. wOffset: %d, Data: %lX\n", wOffset+3, dwData);
 }
 
-
 /*
  * Description:
  *      Enable default Key (KeyEntry[10]) by MISCFIFO
@@ -1601,25 +1567,24 @@ int     ii;
  *
  */
 /*
-void MACvEnableDefaultKey (unsigned long dwIoBase, unsigned char byLocalID)
-{
-unsigned short wOffset;
-unsigned long dwData;
+  void MACvEnableDefaultKey(unsigned long dwIoBase, unsigned char byLocalID)
+  {
+  unsigned short wOffset;
+  unsigned long dwData;
 
+  if (byLocalID <= 1)
+  return;
 
-    if (byLocalID <= 1)
-        return;
+  wOffset = MISCFIFO_KEYETRY0;
+  wOffset += (10 * MISCFIFO_KEYENTRYSIZE);
 
-    wOffset = MISCFIFO_KEYETRY0;
-    wOffset += (10 * MISCFIFO_KEYENTRYSIZE);
+  dwData = 0xC0440000;
+  VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
+  VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
+  VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
+  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "MACvEnableDefaultKey: wOffset: %d, Data: %lX\n", wOffset, dwData);
 
-    dwData = 0xC0440000;
-    VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
-    VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
-    VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"MACvEnableDefaultKey: wOffset: %d, Data: %lX\n", wOffset, dwData);
-
-}
+  }
 */
 
 /*
@@ -1636,20 +1601,19 @@ unsigned long dwData;
  * Return Value: none
  *
  */
-void MACvDisableDefaultKey (unsigned long dwIoBase)
+void MACvDisableDefaultKey(unsigned long dwIoBase)
 {
-unsigned short wOffset;
-unsigned long dwData;
-
+	unsigned short wOffset;
+	unsigned long dwData;
 
-    wOffset = MISCFIFO_KEYETRY0;
-    wOffset += (10 * MISCFIFO_KEYENTRYSIZE);
+	wOffset = MISCFIFO_KEYETRY0;
+	wOffset += (10 * MISCFIFO_KEYENTRYSIZE);
 
-    dwData = 0x0;
-    VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
-    VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
-    VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"MACvDisableDefaultKey: wOffset: %d, Data: %lX\n", wOffset, dwData);
+	dwData = 0x0;
+	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
+	VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
+	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "MACvDisableDefaultKey: wOffset: %d, Data: %lX\n", wOffset, dwData);
 }
 
 /*
@@ -1666,48 +1630,44 @@ unsigned long dwData;
  * Return Value: none
  *
  */
-void MACvSetDefaultTKIPKeyEntry (unsigned long dwIoBase, unsigned int uKeyLen,
-		unsigned int uKeyIdx, unsigned long *pdwKey, unsigned char byLocalID)
+void MACvSetDefaultTKIPKeyEntry(unsigned long dwIoBase, unsigned int uKeyLen,
+				unsigned int uKeyIdx, unsigned long *pdwKey, unsigned char byLocalID)
 {
-unsigned short wOffset;
-unsigned long dwData;
-int     ii;
-
-    if (byLocalID <= 1)
-        return;
-
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"MACvSetDefaultTKIPKeyEntry\n");
-    wOffset = MISCFIFO_KEYETRY0;
-    // Kyle test : change offset from 10 -> 0
-    wOffset += (10 * MISCFIFO_KEYENTRYSIZE);
-
-    dwData = 0xC0660000;
-    VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
-    VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
-    VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
-    wOffset++;
-
-    dwData = 0;
-    VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
-    VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
-    VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
-    wOffset++;
-
-    wOffset += (uKeyIdx * 4);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"1. wOffset: %d, Data: %lX, idx:%d\n", wOffset, *pdwKey, uKeyIdx);
-    // always push 128 bits
-    for (ii=0; ii<4; ii++) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"2.(%d) wOffset: %d, Data: %lX\n", ii, wOffset+ii, *pdwKey);
-        VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset+ii);
-        VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, *pdwKey++);
-        VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
-    }
-
+	unsigned short wOffset;
+	unsigned long dwData;
+	int     ii;
+
+	if (byLocalID <= 1)
+		return;
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "MACvSetDefaultTKIPKeyEntry\n");
+	wOffset = MISCFIFO_KEYETRY0;
+	// Kyle test : change offset from 10 -> 0
+	wOffset += (10 * MISCFIFO_KEYENTRYSIZE);
+
+	dwData = 0xC0660000;
+	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
+	VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
+	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
+	wOffset++;
+
+	dwData = 0;
+	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
+	VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
+	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
+	wOffset++;
+
+	wOffset += (uKeyIdx * 4);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "1. wOffset: %d, Data: %lX, idx:%d\n", wOffset, *pdwKey, uKeyIdx);
+	// always push 128 bits
+	for (ii = 0; ii < 4; ii++) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "2.(%d) wOffset: %d, Data: %lX\n", ii, wOffset+ii, *pdwKey);
+		VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset+ii);
+		VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, *pdwKey++);
+		VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
+	}
 }
 
-
-
 /*
  * Description:
  *      Set the Key Control by MISCFIFO
@@ -1723,28 +1683,25 @@ int     ii;
  *
  */
 
-void MACvSetDefaultKeyCtl (unsigned long dwIoBase, unsigned short wKeyCtl, unsigned int uEntryIdx, unsigned char byLocalID)
+void MACvSetDefaultKeyCtl(unsigned long dwIoBase, unsigned short wKeyCtl, unsigned int uEntryIdx, unsigned char byLocalID)
 {
-unsigned short wOffset;
-unsigned long dwData;
-
-    if (byLocalID <= 1)
-        return;
+	unsigned short wOffset;
+	unsigned long dwData;
 
+	if (byLocalID <= 1)
+		return;
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"MACvSetKeyEntry\n");
-    wOffset = MISCFIFO_KEYETRY0;
-    wOffset += (uEntryIdx * MISCFIFO_KEYENTRYSIZE);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "MACvSetKeyEntry\n");
+	wOffset = MISCFIFO_KEYETRY0;
+	wOffset += (uEntryIdx * MISCFIFO_KEYENTRYSIZE);
 
-    dwData = 0;
-    dwData |= wKeyCtl;
-    dwData <<= 16;
-    dwData |= 0xffff;
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"1. wOffset: %d, Data: %lX, KeyCtl:%X\n", wOffset, dwData, wKeyCtl);
-
-    VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
-    VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
-    VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
+	dwData = 0;
+	dwData |= wKeyCtl;
+	dwData <<= 16;
+	dwData |= 0xffff;
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "1. wOffset: %d, Data: %lX, KeyCtl:%X\n", wOffset, dwData, wKeyCtl);
 
+	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
+	VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
+	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
 }
-
diff --git a/drivers/staging/vt6655/mac.h b/drivers/staging/vt6655/mac.h
index adfb366f490..3f177f7c581 100644
--- a/drivers/staging/vt6655/mac.h
+++ b/drivers/staging/vt6655/mac.h
@@ -197,7 +197,6 @@
 #define MAC_REG_PWRCCK      0x73
 #define MAC_REG_PWROFDM     0x7C
 
-
 //
 // Bits in the BCFG0 register
 //
@@ -398,7 +397,6 @@
 #define IMR_AC0DMA          0x00000002      //
 #define IMR_TXDMA0          0x00000001      //
 
-
 //
 // Bits in the ISR register
 //
@@ -422,7 +420,6 @@
 #define ISR_AC0DMA          0x00000002      //
 #define ISR_TXDMA0          0x00000001      //
 
-
 //
 // Bits in the PSCFG register
 //
@@ -513,7 +510,6 @@
 //
 #define MISCFFCTL_WRITE     0x0001      //
 
-
 //
 // Bits in WAKEUPEN0
 //
@@ -557,7 +553,6 @@
 #define GPIO1_MD            0x10        //
 #define GPIO1_DATA          0x20        //
 
-
 //
 // Bits in the MSRCTL register
 //
@@ -576,7 +571,6 @@
 #define MSRCTL1_CSAPAREN    0x04
 #define MSRCTL1_TXPAUSE     0x01
 
-
 // Loopback mode
 #define MAC_LB_EXT          0x02        //
 #define MAC_LB_INTERNAL     0x01        //
@@ -595,7 +589,6 @@
 
 #define Default_BI              0x200
 
-
 // MiscFIFO Offset
 #define MISCFIFO_KEYETRY0       32
 #define MISCFIFO_KEYENTRYSIZE   22
@@ -604,20 +597,20 @@
 #define MISCFIFO_SYNDATASIZE    21
 
 // enabled mask value of irq
-#define IMR_MASK_VALUE     (IMR_SOFTTIMER1 | \
-                            IMR_RXDMA1 | \
-                            IMR_RXNOBUF | \
-                            IMR_MIBNEARFULL | \
-                            IMR_SOFTINT | \
-                            IMR_FETALERR | \
-                            IMR_WATCHDOG | \
-                            IMR_SOFTTIMER | \
-                            IMR_GPIO | \
-                            IMR_TBTT | \
-                            IMR_RXDMA0 | \
-                            IMR_BNTX | \
-                            IMR_AC0DMA | \
-                            IMR_TXDMA0)
+#define IMR_MASK_VALUE     (IMR_SOFTTIMER1 |	\
+			    IMR_RXDMA1 |	\
+			    IMR_RXNOBUF |	\
+			    IMR_MIBNEARFULL |	\
+			    IMR_SOFTINT |	\
+			    IMR_FETALERR |	\
+			    IMR_WATCHDOG |	\
+			    IMR_SOFTTIMER |	\
+			    IMR_GPIO |		\
+			    IMR_TBTT |		\
+			    IMR_RXDMA0 |	\
+			    IMR_BNTX |		\
+			    IMR_AC0DMA |	\
+			    IMR_TXDMA0)
 
 // max time out delay time
 #define W_MAX_TIMEOUT       0xFFF0U     //
@@ -637,412 +630,341 @@
 
 /*---------------------  Export Macros ------------------------------*/
 
-#define MACvRegBitsOn(dwIoBase, byRegOfs, byBits)           \
-{                                                           \
-    unsigned char byData;                                   \
-    VNSvInPortB(dwIoBase + byRegOfs, &byData);              \
-    VNSvOutPortB(dwIoBase + byRegOfs, byData | (byBits));   \
-}
-
-#define MACvWordRegBitsOn(dwIoBase, byRegOfs, wBits)        \
-{                                                           \
-    unsigned short wData;                                   \
-    VNSvInPortW(dwIoBase + byRegOfs, &wData);               \
-    VNSvOutPortW(dwIoBase + byRegOfs, wData | (wBits));     \
-}
-
-#define MACvDWordRegBitsOn(dwIoBase, byRegOfs, dwBits)      \
-{                                                           \
-    unsigned long dwData;                                   \
-    VNSvInPortD(dwIoBase + byRegOfs, &dwData);              \
-    VNSvOutPortD(dwIoBase + byRegOfs, dwData | (dwBits));   \
-}
-
-#define MACvRegBitsOnEx(dwIoBase, byRegOfs, byMask, byBits) \
-{                                                           \
-    unsigned char byData;                                   \
-    VNSvInPortB(dwIoBase + byRegOfs, &byData);              \
-    byData &= byMask;                                       \
-    VNSvOutPortB(dwIoBase + byRegOfs, byData | (byBits));   \
-}
-
-#define MACvRegBitsOff(dwIoBase, byRegOfs, byBits)          \
-{                                                           \
-    unsigned char byData;                                   \
-    VNSvInPortB(dwIoBase + byRegOfs, &byData);              \
-    VNSvOutPortB(dwIoBase + byRegOfs, byData & ~(byBits));  \
-}
-
-#define MACvWordRegBitsOff(dwIoBase, byRegOfs, wBits)       \
-{                                                           \
-    unsigned short wData;                                   \
-    VNSvInPortW(dwIoBase + byRegOfs, &wData);               \
-    VNSvOutPortW(dwIoBase + byRegOfs, wData & ~(wBits));    \
-}
-
-#define MACvDWordRegBitsOff(dwIoBase, byRegOfs, dwBits)     \
-{                                                           \
-    unsigned long dwData;                                   \
-    VNSvInPortD(dwIoBase + byRegOfs, &dwData);              \
-    VNSvOutPortD(dwIoBase + byRegOfs, dwData & ~(dwBits));  \
-}
-
-#define MACvGetCurrRx0DescAddr(dwIoBase, pdwCurrDescAddr)    \
-{                                                           \
-    VNSvInPortD(dwIoBase + MAC_REG_RXDMAPTR0,               \
-                (unsigned long *)pdwCurrDescAddr);          \
-}
-
-#define MACvGetCurrRx1DescAddr(dwIoBase, pdwCurrDescAddr)   \
-{                                                           \
-    VNSvInPortD(dwIoBase + MAC_REG_RXDMAPTR1,               \
-                (unsigned long *)pdwCurrDescAddr);          \
-}
-
-#define MACvGetCurrTx0DescAddr(dwIoBase, pdwCurrDescAddr)   \
-{                                                           \
-    VNSvInPortD(dwIoBase + MAC_REG_TXDMAPTR0,               \
-                (unsigned long *)pdwCurrDescAddr);          \
-}
-
-#define MACvGetCurrAC0DescAddr(dwIoBase, pdwCurrDescAddr)   \
-{                                                           \
-    VNSvInPortD(dwIoBase + MAC_REG_AC0DMAPTR,               \
-                (unsigned long *)pdwCurrDescAddr);          \
-}
-
-#define MACvGetCurrSyncDescAddr(dwIoBase, pdwCurrDescAddr)  \
-{                                                           \
-    VNSvInPortD(dwIoBase + MAC_REG_SYNCDMAPTR,              \
-                (unsigned long *)pdwCurrDescAddr);          \
-}
-
-#define MACvGetCurrATIMDescAddr(dwIoBase, pdwCurrDescAddr)  \
-{                                                           \
-    VNSvInPortD(dwIoBase + MAC_REG_ATIMDMAPTR,              \
-                (unsigned long *)pdwCurrDescAddr);          \
-}                                                           \
+#define MACvRegBitsOn(dwIoBase, byRegOfs, byBits)			\
+do {									\
+	unsigned char byData;						\
+	VNSvInPortB(dwIoBase + byRegOfs, &byData);			\
+	VNSvOutPortB(dwIoBase + byRegOfs, byData | (byBits));		\
+} while (0)
+
+#define MACvWordRegBitsOn(dwIoBase, byRegOfs, wBits)			\
+do {									\
+	unsigned short wData;						\
+	VNSvInPortW(dwIoBase + byRegOfs, &wData);			\
+	VNSvOutPortW(dwIoBase + byRegOfs, wData | (wBits));		\
+} while (0)
+
+#define MACvDWordRegBitsOn(dwIoBase, byRegOfs, dwBits)			\
+do {									\
+	unsigned long dwData;						\
+	VNSvInPortD(dwIoBase + byRegOfs, &dwData);			\
+	VNSvOutPortD(dwIoBase + byRegOfs, dwData | (dwBits));		\
+} while (0)
+
+#define MACvRegBitsOnEx(dwIoBase, byRegOfs, byMask, byBits)		\
+do {									\
+	unsigned char byData;						\
+	VNSvInPortB(dwIoBase + byRegOfs, &byData);			\
+	byData &= byMask;						\
+	VNSvOutPortB(dwIoBase + byRegOfs, byData | (byBits));		\
+} while (0)
+
+#define MACvRegBitsOff(dwIoBase, byRegOfs, byBits)			\
+do {									\
+	unsigned char byData;						\
+	VNSvInPortB(dwIoBase + byRegOfs, &byData);			\
+	VNSvOutPortB(dwIoBase + byRegOfs, byData & ~(byBits));		\
+} while (0)
+
+#define MACvWordRegBitsOff(dwIoBase, byRegOfs, wBits)			\
+do {									\
+	unsigned short wData;						\
+	VNSvInPortW(dwIoBase + byRegOfs, &wData);			\
+	VNSvOutPortW(dwIoBase + byRegOfs, wData & ~(wBits));		\
+} while (0)
+
+#define MACvDWordRegBitsOff(dwIoBase, byRegOfs, dwBits)			\
+do {									\
+	unsigned long dwData;						\
+	VNSvInPortD(dwIoBase + byRegOfs, &dwData);			\
+	VNSvOutPortD(dwIoBase + byRegOfs, dwData & ~(dwBits));		\
+} while (0)
+
+#define MACvGetCurrRx0DescAddr(dwIoBase, pdwCurrDescAddr)	\
+	VNSvInPortD(dwIoBase + MAC_REG_RXDMAPTR0,		\
+		    (unsigned long *)pdwCurrDescAddr)
+
+#define MACvGetCurrRx1DescAddr(dwIoBase, pdwCurrDescAddr)	\
+	VNSvInPortD(dwIoBase + MAC_REG_RXDMAPTR1,		\
+		    (unsigned long *)pdwCurrDescAddr)
+
+#define MACvGetCurrTx0DescAddr(dwIoBase, pdwCurrDescAddr)	\
+	VNSvInPortD(dwIoBase + MAC_REG_TXDMAPTR0,		\
+		    (unsigned long *)pdwCurrDescAddr)
+
+#define MACvGetCurrAC0DescAddr(dwIoBase, pdwCurrDescAddr)	\
+	VNSvInPortD(dwIoBase + MAC_REG_AC0DMAPTR,		\
+		    (unsigned long *)pdwCurrDescAddr)
+
+#define MACvGetCurrSyncDescAddr(dwIoBase, pdwCurrDescAddr)	\
+	VNSvInPortD(dwIoBase + MAC_REG_SYNCDMAPTR,		\
+		    (unsigned long *)pdwCurrDescAddr)
+
+#define MACvGetCurrATIMDescAddr(dwIoBase, pdwCurrDescAddr)	\
+	VNSvInPortD(dwIoBase + MAC_REG_ATIMDMAPTR,		\
+		    (unsigned long *)pdwCurrDescAddr)
 
 // set the chip with current BCN tx descriptor address
-#define MACvSetCurrBCNTxDescAddr(dwIoBase, dwCurrDescAddr)  \
-{                                                           \
-    VNSvOutPortD(dwIoBase + MAC_REG_BCNDMAPTR,              \
-                 dwCurrDescAddr);                           \
-}
+#define MACvSetCurrBCNTxDescAddr(dwIoBase, dwCurrDescAddr)	\
+	VNSvOutPortD(dwIoBase + MAC_REG_BCNDMAPTR,		\
+		     dwCurrDescAddr)
 
 // set the chip with current BCN length
-#define MACvSetCurrBCNLength(dwIoBase, wCurrBCNLength)     \
-{                                                          \
-    VNSvOutPortW(dwIoBase + MAC_REG_BCNDMACTL+2,           \
-                 wCurrBCNLength);                          \
-}
-
-#define MACvReadBSSIDAddress(dwIoBase, pbyEtherAddr)        \
-{                                                           \
-    VNSvOutPortB(dwIoBase + MAC_REG_PAGE1SEL, 1);           \
-    VNSvInPortB(dwIoBase + MAC_REG_BSSID0,                  \
-                (unsigned char *)pbyEtherAddr);             \
-    VNSvInPortB(dwIoBase + MAC_REG_BSSID0 + 1,              \
-                pbyEtherAddr + 1);                          \
-    VNSvInPortB(dwIoBase + MAC_REG_BSSID0 + 2,              \
-                pbyEtherAddr + 2);                          \
-    VNSvInPortB(dwIoBase + MAC_REG_BSSID0 + 3,              \
-                pbyEtherAddr + 3);                          \
-    VNSvInPortB(dwIoBase + MAC_REG_BSSID0 + 4,              \
-                pbyEtherAddr + 4);                          \
-    VNSvInPortB(dwIoBase + MAC_REG_BSSID0 + 5,              \
-                pbyEtherAddr + 5);                          \
-    VNSvOutPortB(dwIoBase + MAC_REG_PAGE1SEL, 0);           \
-}
-
-#define MACvWriteBSSIDAddress(dwIoBase, pbyEtherAddr)       \
-{                                                           \
-    VNSvOutPortB(dwIoBase + MAC_REG_PAGE1SEL, 1);           \
-    VNSvOutPortB(dwIoBase + MAC_REG_BSSID0,                 \
-                *(pbyEtherAddr));                           \
-    VNSvOutPortB(dwIoBase + MAC_REG_BSSID0 + 1,             \
-                *(pbyEtherAddr + 1));                       \
-    VNSvOutPortB(dwIoBase + MAC_REG_BSSID0 + 2,             \
-                *(pbyEtherAddr + 2));                       \
-    VNSvOutPortB(dwIoBase + MAC_REG_BSSID0 + 3,             \
-                *(pbyEtherAddr + 3));                       \
-    VNSvOutPortB(dwIoBase + MAC_REG_BSSID0 + 4,             \
-                *(pbyEtherAddr + 4));                       \
-    VNSvOutPortB(dwIoBase + MAC_REG_BSSID0 + 5,             \
-                *(pbyEtherAddr + 5));                       \
-    VNSvOutPortB(dwIoBase + MAC_REG_PAGE1SEL, 0);           \
-}
-
-#define MACvReadEtherAddress(dwIoBase, pbyEtherAddr)        \
-{                                                           \
-    VNSvOutPortB(dwIoBase + MAC_REG_PAGE1SEL, 1);           \
-    VNSvInPortB(dwIoBase + MAC_REG_PAR0,                    \
-                (unsigned char *)pbyEtherAddr);             \
-    VNSvInPortB(dwIoBase + MAC_REG_PAR0 + 1,                \
-                pbyEtherAddr + 1);                          \
-    VNSvInPortB(dwIoBase + MAC_REG_PAR0 + 2,                \
-                pbyEtherAddr + 2);                          \
-    VNSvInPortB(dwIoBase + MAC_REG_PAR0 + 3,                \
-                pbyEtherAddr + 3);                          \
-    VNSvInPortB(dwIoBase + MAC_REG_PAR0 + 4,                \
-                pbyEtherAddr + 4);                          \
-    VNSvInPortB(dwIoBase + MAC_REG_PAR0 + 5,                \
-                pbyEtherAddr + 5);                          \
-    VNSvOutPortB(dwIoBase + MAC_REG_PAGE1SEL, 0);           \
-}
-
-
-#define MACvWriteEtherAddress(dwIoBase, pbyEtherAddr)       \
-{                                                           \
-    VNSvOutPortB(dwIoBase + MAC_REG_PAGE1SEL, 1);           \
-    VNSvOutPortB(dwIoBase + MAC_REG_PAR0,                   \
-                *pbyEtherAddr);                             \
-    VNSvOutPortB(dwIoBase + MAC_REG_PAR0 + 1,               \
-                *(pbyEtherAddr + 1));                       \
-    VNSvOutPortB(dwIoBase + MAC_REG_PAR0 + 2,               \
-                *(pbyEtherAddr + 2));                       \
-    VNSvOutPortB(dwIoBase + MAC_REG_PAR0 + 3,               \
-                *(pbyEtherAddr + 3));                       \
-    VNSvOutPortB(dwIoBase + MAC_REG_PAR0 + 4,               \
-                *(pbyEtherAddr + 4));                       \
-    VNSvOutPortB(dwIoBase + MAC_REG_PAR0 + 5,               \
-                *(pbyEtherAddr + 5));                       \
-    VNSvOutPortB(dwIoBase + MAC_REG_PAGE1SEL, 0);           \
-}
-
-
-#define MACvClearISR(dwIoBase)                              \
-{                                                           \
-    VNSvOutPortD(dwIoBase + MAC_REG_ISR, IMR_MASK_VALUE);   \
-}
-
-#define MACvStart(dwIoBase)                                      \
-{                                                                \
-    VNSvOutPortB(dwIoBase + MAC_REG_HOSTCR,                      \
-                    (HOSTCR_MACEN | HOSTCR_RXON | HOSTCR_TXON)); \
-}
-
-#define MACvRx0PerPktMode(dwIoBase)                         \
-{                                                           \
-    VNSvOutPortD(dwIoBase + MAC_REG_RXDMACTL0, RX_PERPKT);  \
-}
-
-#define MACvRx0BufferFillMode(dwIoBase)                         \
-{                                                               \
-    VNSvOutPortD(dwIoBase + MAC_REG_RXDMACTL0, RX_PERPKTCLR);   \
-}
-
-#define MACvRx1PerPktMode(dwIoBase)                         \
-{                                                           \
-    VNSvOutPortD(dwIoBase + MAC_REG_RXDMACTL1, RX_PERPKT);  \
-}
-
-#define MACvRx1BufferFillMode(dwIoBase)                         \
-{                                                               \
-    VNSvOutPortD(dwIoBase + MAC_REG_RXDMACTL1, RX_PERPKTCLR);   \
-}
-
-#define MACvRxOn(dwIoBase)                                      \
-{                                                               \
-    MACvRegBitsOn(dwIoBase, MAC_REG_HOSTCR, HOSTCR_RXON);       \
-}
-
-#define MACvReceive0(dwIoBase)                                  \
-{                                                               \
-    unsigned long dwData;                                       \
-    VNSvInPortD(dwIoBase + MAC_REG_RXDMACTL0, &dwData);         \
-    if (dwData & DMACTL_RUN) {                                  \
-        VNSvOutPortD(dwIoBase + MAC_REG_RXDMACTL0, DMACTL_WAKE);\
-    }                                                           \
-    else {                                                      \
-        VNSvOutPortD(dwIoBase + MAC_REG_RXDMACTL0, DMACTL_RUN); \
-    }                                                           \
-}
-
-#define MACvReceive1(dwIoBase)                                  \
-{                                                               \
-    unsigned long dwData;                                       \
-    VNSvInPortD(dwIoBase + MAC_REG_RXDMACTL1, &dwData);         \
-    if (dwData & DMACTL_RUN) {                                  \
-        VNSvOutPortD(dwIoBase + MAC_REG_RXDMACTL1, DMACTL_WAKE);\
-    }                                                           \
-    else {                                                      \
-        VNSvOutPortD(dwIoBase + MAC_REG_RXDMACTL1, DMACTL_RUN); \
-    }                                                           \
-}
-
-#define MACvTxOn(dwIoBase)                                      \
-{                                                               \
-    MACvRegBitsOn(dwIoBase, MAC_REG_HOSTCR, HOSTCR_TXON);       \
-}
-
-#define MACvTransmit0(dwIoBase)                                 \
-{                                                               \
-    unsigned long dwData;                                       \
-    VNSvInPortD(dwIoBase + MAC_REG_TXDMACTL0, &dwData);         \
-    if (dwData & DMACTL_RUN) {                                  \
-        VNSvOutPortD(dwIoBase + MAC_REG_TXDMACTL0, DMACTL_WAKE);\
-    }                                                           \
-    else {                                                      \
-        VNSvOutPortD(dwIoBase + MAC_REG_TXDMACTL0, DMACTL_RUN); \
-    }                                                           \
-}
-
-#define MACvTransmitAC0(dwIoBase)                               \
-{                                                               \
-    unsigned long dwData;                                       \
-    VNSvInPortD(dwIoBase + MAC_REG_AC0DMACTL, &dwData);         \
-    if (dwData & DMACTL_RUN) {                                  \
-        VNSvOutPortD(dwIoBase + MAC_REG_AC0DMACTL, DMACTL_WAKE);\
-    }                                                           \
-    else {                                                      \
-        VNSvOutPortD(dwIoBase + MAC_REG_AC0DMACTL, DMACTL_RUN); \
-    }                                                           \
-}
-
-#define MACvTransmitSYNC(dwIoBase)                               \
-{                                                                \
-    unsigned long dwData;                                        \
-    VNSvInPortD(dwIoBase + MAC_REG_SYNCDMACTL, &dwData);         \
-    if (dwData & DMACTL_RUN) {                                   \
-        VNSvOutPortD(dwIoBase + MAC_REG_SYNCDMACTL, DMACTL_WAKE);\
-    }                                                            \
-    else {                                                       \
-        VNSvOutPortD(dwIoBase + MAC_REG_SYNCDMACTL, DMACTL_RUN); \
-    }                                                            \
-}
-
-#define MACvTransmitATIM(dwIoBase)                               \
-{                                                                \
-    unsigned long dwData;                                        \
-    VNSvInPortD(dwIoBase + MAC_REG_ATIMDMACTL, &dwData);         \
-    if (dwData & DMACTL_RUN) {                                   \
-        VNSvOutPortD(dwIoBase + MAC_REG_ATIMDMACTL, DMACTL_WAKE);\
-    }                                                            \
-    else {                                                       \
-        VNSvOutPortD(dwIoBase + MAC_REG_ATIMDMACTL, DMACTL_RUN); \
-    }                                                            \
-}
-
-#define MACvTransmitBCN(dwIoBase)                               \
-{                                                               \
-    VNSvOutPortB(dwIoBase + MAC_REG_BCNDMACTL, BEACON_READY);   \
-}
-
-#define MACvClearStckDS(dwIoBase)                           \
-{                                                           \
-    unsigned char byOrgValue;                               \
-    VNSvInPortB(dwIoBase + MAC_REG_STICKHW, &byOrgValue);   \
-    byOrgValue = byOrgValue & 0xFC;                         \
-    VNSvOutPortB(dwIoBase + MAC_REG_STICKHW, byOrgValue);   \
-}
-
-#define MACvReadISR(dwIoBase, pdwValue)             \
-{                                                   \
-    VNSvInPortD(dwIoBase + MAC_REG_ISR, pdwValue);  \
-}
-
-#define MACvWriteISR(dwIoBase, dwValue)             \
-{                                                   \
-    VNSvOutPortD(dwIoBase + MAC_REG_ISR, dwValue);  \
-}
-
-#define MACvIntEnable(dwIoBase, dwMask)             \
-{                                                   \
-    VNSvOutPortD(dwIoBase + MAC_REG_IMR, dwMask);   \
-}
-
-#define MACvIntDisable(dwIoBase)                    \
-{                                                   \
-    VNSvOutPortD(dwIoBase + MAC_REG_IMR, 0);        \
-}
-
-#define MACvSelectPage0(dwIoBase)                   \
-{                                                   \
-    VNSvOutPortB(dwIoBase + MAC_REG_PAGE1SEL, 0);   \
-}
-#define MACvSelectPage1(dwIoBase)                   \
-{                                                   \
-    VNSvOutPortB(dwIoBase + MAC_REG_PAGE1SEL, 1);   \
-}
-
-#define MACvReadMIBCounter(dwIoBase, pdwCounter)            \
-{                                                           \
-    VNSvInPortD(dwIoBase + MAC_REG_MIBCNTR , pdwCounter);   \
-}
-
-#define MACvPwrEvntDisable(dwIoBase)                    \
-{                                                       \
-    VNSvOutPortW(dwIoBase + MAC_REG_WAKEUPEN0, 0x0000); \
-}
-
-#define MACvEnableProtectMD(dwIoBase)                    \
-{                                                        \
-    unsigned long dwOrgValue;                            \
-    VNSvInPortD(dwIoBase + MAC_REG_ENCFG , &dwOrgValue); \
-    dwOrgValue = dwOrgValue | EnCFG_ProtectMd;           \
-    VNSvOutPortD(dwIoBase + MAC_REG_ENCFG, dwOrgValue);  \
-}
-
-#define MACvDisableProtectMD(dwIoBase)                   \
-{                                                        \
-    unsigned long dwOrgValue;                            \
-    VNSvInPortD(dwIoBase + MAC_REG_ENCFG , &dwOrgValue); \
-    dwOrgValue = dwOrgValue & ~EnCFG_ProtectMd;          \
-    VNSvOutPortD(dwIoBase + MAC_REG_ENCFG, dwOrgValue);  \
-}
-
-#define MACvEnableBarkerPreambleMd(dwIoBase)             \
-{                                                        \
-    unsigned long dwOrgValue;                            \
-    VNSvInPortD(dwIoBase + MAC_REG_ENCFG , &dwOrgValue); \
-    dwOrgValue = dwOrgValue | EnCFG_BarkerPream;         \
-    VNSvOutPortD(dwIoBase + MAC_REG_ENCFG, dwOrgValue);  \
-}
-
-#define MACvDisableBarkerPreambleMd(dwIoBase)            \
-{                                                        \
-    unsigned long dwOrgValue;                            \
-    VNSvInPortD(dwIoBase + MAC_REG_ENCFG , &dwOrgValue); \
-    dwOrgValue = dwOrgValue & ~EnCFG_BarkerPream;        \
-    VNSvOutPortD(dwIoBase + MAC_REG_ENCFG, dwOrgValue);  \
-}
-
-#define MACvSetBBType(dwIoBase, byTyp)                   \
-{                                                        \
-    unsigned long dwOrgValue;                            \
-    VNSvInPortD(dwIoBase + MAC_REG_ENCFG , &dwOrgValue); \
-    dwOrgValue = dwOrgValue & ~EnCFG_BBType_MASK;        \
-    dwOrgValue = dwOrgValue | (unsigned long) byTyp;     \
-    VNSvOutPortD(dwIoBase + MAC_REG_ENCFG, dwOrgValue);  \
-}
-
-#define MACvReadATIMW(dwIoBase, pwCounter)                 \
-{                                                          \
-    VNSvInPortW(dwIoBase + MAC_REG_AIDATIM , pwCounter);   \
-}
-
-#define MACvWriteATIMW(dwIoBase, wCounter)                 \
-{                                                          \
-    VNSvOutPortW(dwIoBase + MAC_REG_AIDATIM , wCounter);   \
-}
-
-#define MACvWriteCRC16_128(dwIoBase, byRegOfs, wCRC)       \
-{                                                          \
-    VNSvOutPortB(dwIoBase + MAC_REG_PAGE1SEL, 1);          \
-    VNSvOutPortW(dwIoBase + byRegOfs, wCRC);               \
-    VNSvOutPortB(dwIoBase + MAC_REG_PAGE1SEL, 0);          \
-}
-
-#define MACvGPIOIn(dwIoBase, pbyValue)                      \
-{                                                           \
-    VNSvInPortB(dwIoBase + MAC_REG_GPIOCTL1, pbyValue);     \
-}
+#define MACvSetCurrBCNLength(dwIoBase, wCurrBCNLength)		\
+	VNSvOutPortW(dwIoBase + MAC_REG_BCNDMACTL+2,		\
+		     wCurrBCNLength)
+
+#define MACvReadBSSIDAddress(dwIoBase, pbyEtherAddr)		\
+do {								\
+	VNSvOutPortB(dwIoBase + MAC_REG_PAGE1SEL, 1);		\
+	VNSvInPortB(dwIoBase + MAC_REG_BSSID0,			\
+		    (unsigned char *)pbyEtherAddr);		\
+	VNSvInPortB(dwIoBase + MAC_REG_BSSID0 + 1,		\
+		    pbyEtherAddr + 1);				\
+	VNSvInPortB(dwIoBase + MAC_REG_BSSID0 + 2,		\
+		    pbyEtherAddr + 2);				\
+	VNSvInPortB(dwIoBase + MAC_REG_BSSID0 + 3,		\
+		    pbyEtherAddr + 3);				\
+	VNSvInPortB(dwIoBase + MAC_REG_BSSID0 + 4,		\
+		    pbyEtherAddr + 4);				\
+	VNSvInPortB(dwIoBase + MAC_REG_BSSID0 + 5,		\
+		    pbyEtherAddr + 5);				\
+	VNSvOutPortB(dwIoBase + MAC_REG_PAGE1SEL, 0);		\
+} while (0)
+
+#define MACvWriteBSSIDAddress(dwIoBase, pbyEtherAddr)		\
+do {								\
+	VNSvOutPortB(dwIoBase + MAC_REG_PAGE1SEL, 1);		\
+	VNSvOutPortB(dwIoBase + MAC_REG_BSSID0,			\
+		     *(pbyEtherAddr));				\
+	VNSvOutPortB(dwIoBase + MAC_REG_BSSID0 + 1,		\
+		     *(pbyEtherAddr + 1));			\
+	VNSvOutPortB(dwIoBase + MAC_REG_BSSID0 + 2,		\
+		     *(pbyEtherAddr + 2));			\
+	VNSvOutPortB(dwIoBase + MAC_REG_BSSID0 + 3,		\
+		     *(pbyEtherAddr + 3));			\
+	VNSvOutPortB(dwIoBase + MAC_REG_BSSID0 + 4,		\
+		     *(pbyEtherAddr + 4));			\
+	VNSvOutPortB(dwIoBase + MAC_REG_BSSID0 + 5,		\
+		     *(pbyEtherAddr + 5));			\
+	VNSvOutPortB(dwIoBase + MAC_REG_PAGE1SEL, 0);		\
+} while (0)
+
+#define MACvReadEtherAddress(dwIoBase, pbyEtherAddr)		\
+do {								\
+	VNSvOutPortB(dwIoBase + MAC_REG_PAGE1SEL, 1);		\
+	VNSvInPortB(dwIoBase + MAC_REG_PAR0,			\
+		    (unsigned char *)pbyEtherAddr);		\
+	VNSvInPortB(dwIoBase + MAC_REG_PAR0 + 1,		\
+		    pbyEtherAddr + 1);				\
+	VNSvInPortB(dwIoBase + MAC_REG_PAR0 + 2,		\
+		    pbyEtherAddr + 2);				\
+	VNSvInPortB(dwIoBase + MAC_REG_PAR0 + 3,		\
+		    pbyEtherAddr + 3);				\
+	VNSvInPortB(dwIoBase + MAC_REG_PAR0 + 4,		\
+		    pbyEtherAddr + 4);				\
+	VNSvInPortB(dwIoBase + MAC_REG_PAR0 + 5,		\
+		    pbyEtherAddr + 5);				\
+	VNSvOutPortB(dwIoBase + MAC_REG_PAGE1SEL, 0);		\
+} while (0)
+
+#define MACvWriteEtherAddress(dwIoBase, pbyEtherAddr)		\
+do {								\
+	VNSvOutPortB(dwIoBase + MAC_REG_PAGE1SEL, 1);		\
+	VNSvOutPortB(dwIoBase + MAC_REG_PAR0,			\
+		     *pbyEtherAddr);				\
+	VNSvOutPortB(dwIoBase + MAC_REG_PAR0 + 1,		\
+		     *(pbyEtherAddr + 1));			\
+	VNSvOutPortB(dwIoBase + MAC_REG_PAR0 + 2,		\
+		     *(pbyEtherAddr + 2));			\
+	VNSvOutPortB(dwIoBase + MAC_REG_PAR0 + 3,		\
+		     *(pbyEtherAddr + 3));			\
+	VNSvOutPortB(dwIoBase + MAC_REG_PAR0 + 4,		\
+		     *(pbyEtherAddr + 4));			\
+	VNSvOutPortB(dwIoBase + MAC_REG_PAR0 + 5,		\
+		     *(pbyEtherAddr + 5));			\
+	VNSvOutPortB(dwIoBase + MAC_REG_PAGE1SEL, 0);		\
+} while (0)
+
+#define MACvClearISR(dwIoBase)						\
+	VNSvOutPortD(dwIoBase + MAC_REG_ISR, IMR_MASK_VALUE)
+
+#define MACvStart(dwIoBase)						\
+	VNSvOutPortB(dwIoBase + MAC_REG_HOSTCR,				\
+		     (HOSTCR_MACEN | HOSTCR_RXON | HOSTCR_TXON))
+
+#define MACvRx0PerPktMode(dwIoBase)					\
+	VNSvOutPortD(dwIoBase + MAC_REG_RXDMACTL0, RX_PERPKT)
+
+#define MACvRx0BufferFillMode(dwIoBase)					\
+	VNSvOutPortD(dwIoBase + MAC_REG_RXDMACTL0, RX_PERPKTCLR)
+
+#define MACvRx1PerPktMode(dwIoBase)					\
+	VNSvOutPortD(dwIoBase + MAC_REG_RXDMACTL1, RX_PERPKT)
+
+#define MACvRx1BufferFillMode(dwIoBase)					\
+	VNSvOutPortD(dwIoBase + MAC_REG_RXDMACTL1, RX_PERPKTCLR)
+
+#define MACvRxOn(dwIoBase)						\
+	MACvRegBitsOn(dwIoBase, MAC_REG_HOSTCR, HOSTCR_RXON)
+
+#define MACvReceive0(dwIoBase)						\
+do {									\
+	unsigned long dwData;						\
+	VNSvInPortD(dwIoBase + MAC_REG_RXDMACTL0, &dwData);		\
+	if (dwData & DMACTL_RUN)					\
+		VNSvOutPortD(dwIoBase + MAC_REG_RXDMACTL0, DMACTL_WAKE); \
+	else								\
+		VNSvOutPortD(dwIoBase + MAC_REG_RXDMACTL0, DMACTL_RUN); \
+} while (0)
+
+#define MACvReceive1(dwIoBase)						\
+do {									\
+	unsigned long dwData;						\
+	VNSvInPortD(dwIoBase + MAC_REG_RXDMACTL1, &dwData);		\
+	if (dwData & DMACTL_RUN)					\
+		VNSvOutPortD(dwIoBase + MAC_REG_RXDMACTL1, DMACTL_WAKE); \
+	else								\
+		VNSvOutPortD(dwIoBase + MAC_REG_RXDMACTL1, DMACTL_RUN); \
+} while (0)
+
+#define MACvTxOn(dwIoBase)						\
+	MACvRegBitsOn(dwIoBase, MAC_REG_HOSTCR, HOSTCR_TXON)
+
+#define MACvTransmit0(dwIoBase)						\
+do {									\
+	unsigned long dwData;						\
+	VNSvInPortD(dwIoBase + MAC_REG_TXDMACTL0, &dwData);		\
+	if (dwData & DMACTL_RUN)					\
+		VNSvOutPortD(dwIoBase + MAC_REG_TXDMACTL0, DMACTL_WAKE); \
+	else								\
+		VNSvOutPortD(dwIoBase + MAC_REG_TXDMACTL0, DMACTL_RUN); \
+} while (0)
+
+#define MACvTransmitAC0(dwIoBase)					\
+do {									\
+	unsigned long dwData;						\
+	VNSvInPortD(dwIoBase + MAC_REG_AC0DMACTL, &dwData);		\
+	if (dwData & DMACTL_RUN)					\
+		VNSvOutPortD(dwIoBase + MAC_REG_AC0DMACTL, DMACTL_WAKE); \
+	else								\
+		VNSvOutPortD(dwIoBase + MAC_REG_AC0DMACTL, DMACTL_RUN); \
+} while (0)
+
+#define MACvTransmitSYNC(dwIoBase)					\
+do {									\
+	unsigned long dwData;						\
+	VNSvInPortD(dwIoBase + MAC_REG_SYNCDMACTL, &dwData);		\
+	if (dwData & DMACTL_RUN)					\
+		VNSvOutPortD(dwIoBase + MAC_REG_SYNCDMACTL, DMACTL_WAKE); \
+	else								\
+		VNSvOutPortD(dwIoBase + MAC_REG_SYNCDMACTL, DMACTL_RUN); \
+} while (0)
+
+#define MACvTransmitATIM(dwIoBase)					\
+do {									\
+	unsigned long dwData;						\
+	VNSvInPortD(dwIoBase + MAC_REG_ATIMDMACTL, &dwData);		\
+	if (dwData & DMACTL_RUN)					\
+		VNSvOutPortD(dwIoBase + MAC_REG_ATIMDMACTL, DMACTL_WAKE); \
+	else								\
+		VNSvOutPortD(dwIoBase + MAC_REG_ATIMDMACTL, DMACTL_RUN); \
+} while (0)
+
+#define MACvTransmitBCN(dwIoBase)					\
+	VNSvOutPortB(dwIoBase + MAC_REG_BCNDMACTL, BEACON_READY)
+
+#define MACvClearStckDS(dwIoBase)					\
+do {									\
+	unsigned char byOrgValue;					\
+	VNSvInPortB(dwIoBase + MAC_REG_STICKHW, &byOrgValue);		\
+	byOrgValue = byOrgValue & 0xFC;					\
+	VNSvOutPortB(dwIoBase + MAC_REG_STICKHW, byOrgValue);		\
+} while (0)
+
+#define MACvReadISR(dwIoBase, pdwValue)				\
+	VNSvInPortD(dwIoBase + MAC_REG_ISR, pdwValue)
+
+#define MACvWriteISR(dwIoBase, dwValue)				\
+	VNSvOutPortD(dwIoBase + MAC_REG_ISR, dwValue)
+
+#define MACvIntEnable(dwIoBase, dwMask)				\
+	VNSvOutPortD(dwIoBase + MAC_REG_IMR, dwMask)
+
+#define MACvIntDisable(dwIoBase)				\
+	VNSvOutPortD(dwIoBase + MAC_REG_IMR, 0)
+
+#define MACvSelectPage0(dwIoBase)				\
+		VNSvOutPortB(dwIoBase + MAC_REG_PAGE1SEL, 0)
+
+#define MACvSelectPage1(dwIoBase)				\
+	VNSvOutPortB(dwIoBase + MAC_REG_PAGE1SEL, 1)
+
+#define MACvReadMIBCounter(dwIoBase, pdwCounter)			\
+	VNSvInPortD(dwIoBase + MAC_REG_MIBCNTR , pdwCounter)
+
+#define MACvPwrEvntDisable(dwIoBase)					\
+	VNSvOutPortW(dwIoBase + MAC_REG_WAKEUPEN0, 0x0000)
+
+#define MACvEnableProtectMD(dwIoBase)					\
+do {									\
+	unsigned long dwOrgValue;					\
+	VNSvInPortD(dwIoBase + MAC_REG_ENCFG , &dwOrgValue);		\
+	dwOrgValue = dwOrgValue | EnCFG_ProtectMd;			\
+	VNSvOutPortD(dwIoBase + MAC_REG_ENCFG, dwOrgValue);		\
+} while (0)
+
+#define MACvDisableProtectMD(dwIoBase)					\
+do {									\
+	unsigned long dwOrgValue;					\
+	VNSvInPortD(dwIoBase + MAC_REG_ENCFG , &dwOrgValue);		\
+	dwOrgValue = dwOrgValue & ~EnCFG_ProtectMd;			\
+	VNSvOutPortD(dwIoBase + MAC_REG_ENCFG, dwOrgValue);		\
+} while (0)
+
+#define MACvEnableBarkerPreambleMd(dwIoBase)				\
+do {									\
+	unsigned long dwOrgValue;					\
+	VNSvInPortD(dwIoBase + MAC_REG_ENCFG , &dwOrgValue);		\
+	dwOrgValue = dwOrgValue | EnCFG_BarkerPream;			\
+	VNSvOutPortD(dwIoBase + MAC_REG_ENCFG, dwOrgValue);		\
+} while (0)
+
+#define MACvDisableBarkerPreambleMd(dwIoBase)				\
+do {									\
+	unsigned long dwOrgValue;					\
+	VNSvInPortD(dwIoBase + MAC_REG_ENCFG , &dwOrgValue);		\
+	dwOrgValue = dwOrgValue & ~EnCFG_BarkerPream;			\
+	VNSvOutPortD(dwIoBase + MAC_REG_ENCFG, dwOrgValue);		\
+} while (0)
+
+#define MACvSetBBType(dwIoBase, byTyp)					\
+do {									\
+	unsigned long dwOrgValue;					\
+	VNSvInPortD(dwIoBase + MAC_REG_ENCFG , &dwOrgValue);		\
+	dwOrgValue = dwOrgValue & ~EnCFG_BBType_MASK;			\
+	dwOrgValue = dwOrgValue | (unsigned long)byTyp;			\
+	VNSvOutPortD(dwIoBase + MAC_REG_ENCFG, dwOrgValue);		\
+} while (0)
+
+#define MACvReadATIMW(dwIoBase, pwCounter)				\
+	VNSvInPortW(dwIoBase + MAC_REG_AIDATIM, pwCounter)
+
+#define MACvWriteATIMW(dwIoBase, wCounter)				\
+	VNSvOutPortW(dwIoBase + MAC_REG_AIDATIM, wCounter)
+
+#define MACvWriteCRC16_128(dwIoBase, byRegOfs, wCRC)		\
+do {								\
+	VNSvOutPortB(dwIoBase + MAC_REG_PAGE1SEL, 1);		\
+	VNSvOutPortW(dwIoBase + byRegOfs, wCRC);		\
+	VNSvOutPortB(dwIoBase + MAC_REG_PAGE1SEL, 0);		\
+} while (0)
+
+#define MACvGPIOIn(dwIoBase, pbyValue)					\
+	VNSvInPortB(dwIoBase + MAC_REG_GPIOCTL1, pbyValue)
 
 #define MACvSetRFLE_LatchBase(dwIoBase)                                 \
-{                                                                        \
-    MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_RFLEOPT); \
-}
+	MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_RFLEOPT)
 
 /*---------------------  Export Classes  ----------------------------*/
 
@@ -1107,7 +1029,7 @@ void MACvOneShotTimer1MicroSec(unsigned long dwIoBase, unsigned int uDelayTime);
 
 void MACvSetMISCFifo(unsigned long dwIoBase, unsigned short wOffset, unsigned long dwData);
 
-bool MACbTxDMAOff (unsigned long dwIoBase, unsigned int idx);
+bool MACbTxDMAOff(unsigned long dwIoBase, unsigned int idx);
 
 void MACvClearBusSusInd(unsigned long dwIoBase);
 void MACvEnableBusSusEn(unsigned long dwIoBase);
@@ -1116,15 +1038,14 @@ bool MACbFlushSYNCFifo(unsigned long dwIoBase);
 bool MACbPSWakeup(unsigned long dwIoBase);
 
 void MACvSetKeyEntry(unsigned long dwIoBase, unsigned short wKeyCtl, unsigned int uEntryIdx,
-		unsigned int uKeyIdx, unsigned char *pbyAddr, unsigned long *pdwKey, unsigned char byLocalID);
+		     unsigned int uKeyIdx, unsigned char *pbyAddr, unsigned long *pdwKey, unsigned char byLocalID);
 void MACvDisableKeyEntry(unsigned long dwIoBase, unsigned int uEntryIdx);
 void MACvSetDefaultKeyEntry(unsigned long dwIoBase, unsigned int uKeyLen,
-		unsigned int uKeyIdx, unsigned long *pdwKey, unsigned char byLocalID);
+			    unsigned int uKeyIdx, unsigned long *pdwKey, unsigned char byLocalID);
 //void MACvEnableDefaultKey(unsigned long dwIoBase, unsigned char byLocalID);
 void MACvDisableDefaultKey(unsigned long dwIoBase);
 void MACvSetDefaultTKIPKeyEntry(unsigned long dwIoBase, unsigned int uKeyLen,
-		unsigned int uKeyIdx, unsigned long *pdwKey, unsigned char byLocalID);
+				unsigned int uKeyIdx, unsigned long *pdwKey, unsigned char byLocalID);
 void MACvSetDefaultKeyCtl(unsigned long dwIoBase, unsigned short wKeyCtl, unsigned int uEntryIdx, unsigned char byLocalID);
 
 #endif // __MAC_H__
-
diff --git a/drivers/staging/vt6655/mib.c b/drivers/staging/vt6655/mib.c
index 63ae4adddf2..6a59652052b 100644
--- a/drivers/staging/vt6655/mib.c
+++ b/drivers/staging/vt6655/mib.c
@@ -45,7 +45,7 @@
 #include "baseband.h"
 
 /*---------------------  Static Definitions -------------------------*/
-static int          msglevel                =MSG_LEVEL_INFO;
+static int msglevel = MSG_LEVEL_INFO;
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
@@ -56,8 +56,6 @@ static int          msglevel                =MSG_LEVEL_INFO;
 
 /*---------------------  Export Functions  --------------------------*/
 
-
-
 /*
  * Description: Clear All Statistic Counter
  *
@@ -70,13 +68,12 @@ static int          msglevel                =MSG_LEVEL_INFO;
  * Return Value: none
  *
  */
-void STAvClearAllCounter (PSStatCounter pStatistic)
+void STAvClearAllCounter(PSStatCounter pStatistic)
 {
-    // set memory to zero
+	// set memory to zero
 	memset(pStatistic, 0, sizeof(SStatCounter));
 }
 
-
 /*
  * Description: Update Isr Statistic Counter
  *
@@ -90,76 +87,59 @@ void STAvClearAllCounter (PSStatCounter pStatistic)
  * Return Value: none
  *
  */
-void STAvUpdateIsrStatCounter (PSStatCounter pStatistic, unsigned long dwIsr)
+void STAvUpdateIsrStatCounter(PSStatCounter pStatistic, unsigned long dwIsr)
 {
-    /**********************/
-    /* ABNORMAL interrupt */
-    /**********************/
-    // not any IMR bit invoke irq
+	/**********************/
+	/* ABNORMAL interrupt */
+	/**********************/
+	// not any IMR bit invoke irq
 
-    if (dwIsr == 0) {
-        pStatistic->ISRStat.dwIsrUnknown++;
-        return;
-    }
+	if (dwIsr == 0) {
+		pStatistic->ISRStat.dwIsrUnknown++;
+		return;
+	}
 
 //Added by Kyle
-    if (dwIsr & ISR_TXDMA0)               // ISR, bit0
-        pStatistic->ISRStat.dwIsrTx0OK++;             // TXDMA0 successful
-
-    if (dwIsr & ISR_AC0DMA)               // ISR, bit1
-        pStatistic->ISRStat.dwIsrAC0TxOK++;           // AC0DMA successful
-
-    if (dwIsr & ISR_BNTX)                 // ISR, bit2
-        pStatistic->ISRStat.dwIsrBeaconTxOK++;        // BeaconTx successful
-
-    if (dwIsr & ISR_RXDMA0)               // ISR, bit3
-        pStatistic->ISRStat.dwIsrRx0OK++;             // Rx0 successful
-
-    if (dwIsr & ISR_TBTT)                 // ISR, bit4
-        pStatistic->ISRStat.dwIsrTBTTInt++;           // TBTT successful
-
-    if (dwIsr & ISR_SOFTTIMER)            // ISR, bit6
-        pStatistic->ISRStat.dwIsrSTIMERInt++;
-
-    if (dwIsr & ISR_WATCHDOG)             // ISR, bit7
-        pStatistic->ISRStat.dwIsrWatchDog++;
+	if (dwIsr & ISR_TXDMA0)               // ISR, bit0
+		pStatistic->ISRStat.dwIsrTx0OK++;             // TXDMA0 successful
 
-    if (dwIsr & ISR_FETALERR)             // ISR, bit8
-        pStatistic->ISRStat.dwIsrUnrecoverableError++;
+	if (dwIsr & ISR_AC0DMA)               // ISR, bit1
+		pStatistic->ISRStat.dwIsrAC0TxOK++;           // AC0DMA successful
 
-    if (dwIsr & ISR_SOFTINT)              // ISR, bit9
-        pStatistic->ISRStat.dwIsrSoftInterrupt++;     // software interrupt
+	if (dwIsr & ISR_BNTX)                 // ISR, bit2
+		pStatistic->ISRStat.dwIsrBeaconTxOK++;        // BeaconTx successful
 
-    if (dwIsr & ISR_MIBNEARFULL)          // ISR, bit10
-        pStatistic->ISRStat.dwIsrMIBNearfull++;
+	if (dwIsr & ISR_RXDMA0)               // ISR, bit3
+		pStatistic->ISRStat.dwIsrRx0OK++;             // Rx0 successful
 
-    if (dwIsr & ISR_RXNOBUF)              // ISR, bit11
-        pStatistic->ISRStat.dwIsrRxNoBuf++;           // Rx No Buff
+	if (dwIsr & ISR_TBTT)                 // ISR, bit4
+		pStatistic->ISRStat.dwIsrTBTTInt++;           // TBTT successful
 
-    if (dwIsr & ISR_RXDMA1)               // ISR, bit12
-        pStatistic->ISRStat.dwIsrRx1OK++;             // Rx1 successful
+	if (dwIsr & ISR_SOFTTIMER)            // ISR, bit6
+		pStatistic->ISRStat.dwIsrSTIMERInt++;
 
-//    if (dwIsr & ISR_ATIMTX)               // ISR, bit13
-//        pStatistic->ISRStat.dwIsrATIMTxOK++;          // ATIMTX successful
+	if (dwIsr & ISR_WATCHDOG)             // ISR, bit7
+		pStatistic->ISRStat.dwIsrWatchDog++;
 
-//    if (dwIsr & ISR_SYNCTX)               // ISR, bit14
-//        pStatistic->ISRStat.dwIsrSYNCTxOK++;          // SYNCTX successful
+	if (dwIsr & ISR_FETALERR)             // ISR, bit8
+		pStatistic->ISRStat.dwIsrUnrecoverableError++;
 
-//    if (dwIsr & ISR_CFPEND)               // ISR, bit18
-//        pStatistic->ISRStat.dwIsrCFPEnd++;
+	if (dwIsr & ISR_SOFTINT)              // ISR, bit9
+		pStatistic->ISRStat.dwIsrSoftInterrupt++;     // software interrupt
 
-//    if (dwIsr & ISR_ATIMEND)              // ISR, bit19
-//        pStatistic->ISRStat.dwIsrATIMEnd++;
+	if (dwIsr & ISR_MIBNEARFULL)          // ISR, bit10
+		pStatistic->ISRStat.dwIsrMIBNearfull++;
 
-//    if (dwIsr & ISR_SYNCFLUSHOK)          // ISR, bit20
-//        pStatistic->ISRStat.dwIsrSYNCFlushOK++;
+	if (dwIsr & ISR_RXNOBUF)              // ISR, bit11
+		pStatistic->ISRStat.dwIsrRxNoBuf++;           // Rx No Buff
 
-    if (dwIsr & ISR_SOFTTIMER1)           // ISR, bit21
-        pStatistic->ISRStat.dwIsrSTIMER1Int++;
+	if (dwIsr & ISR_RXDMA1)               // ISR, bit12
+		pStatistic->ISRStat.dwIsrRx1OK++;             // Rx1 successful
 
+	if (dwIsr & ISR_SOFTTIMER1)           // ISR, bit21
+		pStatistic->ISRStat.dwIsrSTIMER1Int++;
 }
 
-
 /*
  * Description: Update Rx Statistic Counter
  *
@@ -176,199 +156,175 @@ void STAvUpdateIsrStatCounter (PSStatCounter pStatistic, unsigned long dwIsr)
  * Return Value: none
  *
  */
-void STAvUpdateRDStatCounter (PSStatCounter pStatistic,
-                              unsigned char byRSR, unsigned char byNewRSR, unsigned char byRxRate,
-                              unsigned char *pbyBuffer, unsigned int cbFrameLength)
+void STAvUpdateRDStatCounter(PSStatCounter pStatistic,
+			     unsigned char byRSR, unsigned char byNewRSR, unsigned char byRxRate,
+			     unsigned char *pbyBuffer, unsigned int cbFrameLength)
 {
-    //need change
-    PS802_11Header pHeader = (PS802_11Header)pbyBuffer;
-
-    if (byRSR & RSR_ADDROK)
-        pStatistic->dwRsrADDROk++;
-    if (byRSR & RSR_CRCOK) {
-        pStatistic->dwRsrCRCOk++;
-
-        pStatistic->ullRsrOK++;
-
-        if (cbFrameLength >= ETH_ALEN) {
-            // update counters in case of successful transmit
-            if (byRSR & RSR_ADDRBROAD) {
-                pStatistic->ullRxBroadcastFrames++;
-                pStatistic->ullRxBroadcastBytes += (unsigned long long) cbFrameLength;
-            }
-            else if (byRSR & RSR_ADDRMULTI) {
-                pStatistic->ullRxMulticastFrames++;
-                pStatistic->ullRxMulticastBytes += (unsigned long long) cbFrameLength;
-            }
-            else {
-                pStatistic->ullRxDirectedFrames++;
-                pStatistic->ullRxDirectedBytes += (unsigned long long) cbFrameLength;
-            }
-        }
-    }
-
-    if(byRxRate==22) {
-        pStatistic->CustomStat.ullRsr11M++;
-        if(byRSR & RSR_CRCOK) {
-            pStatistic->CustomStat.ullRsr11MCRCOk++;
-        }
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"11M: ALL[%d], OK[%d]:[%02x]\n", (int)pStatistic->CustomStat.ullRsr11M, (int)pStatistic->CustomStat.ullRsr11MCRCOk, byRSR);
-    }
-    else if(byRxRate==11) {
-        pStatistic->CustomStat.ullRsr5M++;
-        if(byRSR & RSR_CRCOK) {
-            pStatistic->CustomStat.ullRsr5MCRCOk++;
-        }
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" 5M: ALL[%d], OK[%d]:[%02x]\n", (int)pStatistic->CustomStat.ullRsr5M, (int)pStatistic->CustomStat.ullRsr5MCRCOk, byRSR);
-    }
-    else if(byRxRate==4) {
-        pStatistic->CustomStat.ullRsr2M++;
-        if(byRSR & RSR_CRCOK) {
-            pStatistic->CustomStat.ullRsr2MCRCOk++;
-        }
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" 2M: ALL[%d], OK[%d]:[%02x]\n", (int)pStatistic->CustomStat.ullRsr2M, (int)pStatistic->CustomStat.ullRsr2MCRCOk, byRSR);
-    }
-    else if(byRxRate==2){
-        pStatistic->CustomStat.ullRsr1M++;
-        if(byRSR & RSR_CRCOK) {
-            pStatistic->CustomStat.ullRsr1MCRCOk++;
-        }
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" 1M: ALL[%d], OK[%d]:[%02x]\n", (int)pStatistic->CustomStat.ullRsr1M, (int)pStatistic->CustomStat.ullRsr1MCRCOk, byRSR);
-    }
-    else if(byRxRate==12){
-        pStatistic->CustomStat.ullRsr6M++;
-        if(byRSR & RSR_CRCOK) {
-            pStatistic->CustomStat.ullRsr6MCRCOk++;
-        }
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" 6M: ALL[%d], OK[%d]\n", (int)pStatistic->CustomStat.ullRsr6M, (int)pStatistic->CustomStat.ullRsr6MCRCOk);
-    }
-    else if(byRxRate==18){
-        pStatistic->CustomStat.ullRsr9M++;
-        if(byRSR & RSR_CRCOK) {
-            pStatistic->CustomStat.ullRsr9MCRCOk++;
-        }
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" 9M: ALL[%d], OK[%d]\n", (int)pStatistic->CustomStat.ullRsr9M, (int)pStatistic->CustomStat.ullRsr9MCRCOk);
-    }
-    else if(byRxRate==24){
-        pStatistic->CustomStat.ullRsr12M++;
-        if(byRSR & RSR_CRCOK) {
-            pStatistic->CustomStat.ullRsr12MCRCOk++;
-        }
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"12M: ALL[%d], OK[%d]\n", (int)pStatistic->CustomStat.ullRsr12M, (int)pStatistic->CustomStat.ullRsr12MCRCOk);
-    }
-    else if(byRxRate==36){
-        pStatistic->CustomStat.ullRsr18M++;
-        if(byRSR & RSR_CRCOK) {
-            pStatistic->CustomStat.ullRsr18MCRCOk++;
-        }
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"18M: ALL[%d], OK[%d]\n", (int)pStatistic->CustomStat.ullRsr18M, (int)pStatistic->CustomStat.ullRsr18MCRCOk);
-    }
-    else if(byRxRate==48){
-        pStatistic->CustomStat.ullRsr24M++;
-        if(byRSR & RSR_CRCOK) {
-            pStatistic->CustomStat.ullRsr24MCRCOk++;
-        }
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"24M: ALL[%d], OK[%d]\n", (int)pStatistic->CustomStat.ullRsr24M, (int)pStatistic->CustomStat.ullRsr24MCRCOk);
-    }
-    else if(byRxRate==72){
-        pStatistic->CustomStat.ullRsr36M++;
-        if(byRSR & RSR_CRCOK) {
-            pStatistic->CustomStat.ullRsr36MCRCOk++;
-        }
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"36M: ALL[%d], OK[%d]\n", (int)pStatistic->CustomStat.ullRsr36M, (int)pStatistic->CustomStat.ullRsr36MCRCOk);
-    }
-    else if(byRxRate==96){
-        pStatistic->CustomStat.ullRsr48M++;
-        if(byRSR & RSR_CRCOK) {
-            pStatistic->CustomStat.ullRsr48MCRCOk++;
-        }
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"48M: ALL[%d], OK[%d]\n", (int)pStatistic->CustomStat.ullRsr48M, (int)pStatistic->CustomStat.ullRsr48MCRCOk);
-    }
-    else if(byRxRate==108){
-        pStatistic->CustomStat.ullRsr54M++;
-        if(byRSR & RSR_CRCOK) {
-            pStatistic->CustomStat.ullRsr54MCRCOk++;
-        }
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"54M: ALL[%d], OK[%d]\n", (int)pStatistic->CustomStat.ullRsr54M, (int)pStatistic->CustomStat.ullRsr54MCRCOk);
-    }
-    else {
-    	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Unknown: Total[%d], CRCOK[%d]\n", (int)pStatistic->dwRsrRxPacket+1, (int)pStatistic->dwRsrCRCOk);
-    }
-
-    if (byRSR & RSR_BSSIDOK)
-        pStatistic->dwRsrBSSIDOk++;
-
-    if (byRSR & RSR_BCNSSIDOK)
-        pStatistic->dwRsrBCNSSIDOk++;
-    if (byRSR & RSR_IVLDLEN)  //invalid len (> 2312 byte)
-        pStatistic->dwRsrLENErr++;
-    if (byRSR & RSR_IVLDTYP)  //invalid packet type
-        pStatistic->dwRsrTYPErr++;
-    if (byRSR & (RSR_IVLDTYP | RSR_IVLDLEN))
-        pStatistic->dwRsrErr++;
-
-    if (byNewRSR & NEWRSR_DECRYPTOK)
-        pStatistic->dwNewRsrDECRYPTOK++;
-    if (byNewRSR & NEWRSR_CFPIND)
-        pStatistic->dwNewRsrCFP++;
-    if (byNewRSR & NEWRSR_HWUTSF)
-        pStatistic->dwNewRsrUTSF++;
-    if (byNewRSR & NEWRSR_BCNHITAID)
-        pStatistic->dwNewRsrHITAID++;
-    if (byNewRSR & NEWRSR_BCNHITAID0)
-        pStatistic->dwNewRsrHITAID0++;
-
-    // increase rx packet count
-    pStatistic->dwRsrRxPacket++;
-    pStatistic->dwRsrRxOctet += cbFrameLength;
-
-
-    if (IS_TYPE_DATA(pbyBuffer)) {
-        pStatistic->dwRsrRxData++;
-    } else if (IS_TYPE_MGMT(pbyBuffer)){
-        pStatistic->dwRsrRxManage++;
-    } else if (IS_TYPE_CONTROL(pbyBuffer)){
-        pStatistic->dwRsrRxControl++;
-    }
-
-    if (byRSR & RSR_ADDRBROAD)
-        pStatistic->dwRsrBroadcast++;
-    else if (byRSR & RSR_ADDRMULTI)
-        pStatistic->dwRsrMulticast++;
-    else
-        pStatistic->dwRsrDirected++;
-
-    if (WLAN_GET_FC_MOREFRAG(pHeader->wFrameCtl))
-        pStatistic->dwRsrRxFragment++;
-
-    if (cbFrameLength < ETH_ZLEN + 4) {
-        pStatistic->dwRsrRunt++;
-    }
-    else if (cbFrameLength == ETH_ZLEN + 4) {
-        pStatistic->dwRsrRxFrmLen64++;
-    }
-    else if ((65 <= cbFrameLength) && (cbFrameLength <= 127)) {
-        pStatistic->dwRsrRxFrmLen65_127++;
-    }
-    else if ((128 <= cbFrameLength) && (cbFrameLength <= 255)) {
-        pStatistic->dwRsrRxFrmLen128_255++;
-    }
-    else if ((256 <= cbFrameLength) && (cbFrameLength <= 511)) {
-        pStatistic->dwRsrRxFrmLen256_511++;
-    }
-    else if ((512 <= cbFrameLength) && (cbFrameLength <= 1023)) {
-        pStatistic->dwRsrRxFrmLen512_1023++;
-    }
-    else if ((1024 <= cbFrameLength) && (cbFrameLength <= ETH_FRAME_LEN + 4)) {
-        pStatistic->dwRsrRxFrmLen1024_1518++;
-    } else if (cbFrameLength > ETH_FRAME_LEN + 4) {
-        pStatistic->dwRsrLong++;
-    }
-
+	//need change
+	PS802_11Header pHeader = (PS802_11Header)pbyBuffer;
+
+	if (byRSR & RSR_ADDROK)
+		pStatistic->dwRsrADDROk++;
+	if (byRSR & RSR_CRCOK) {
+		pStatistic->dwRsrCRCOk++;
+
+		pStatistic->ullRsrOK++;
+
+		if (cbFrameLength >= ETH_ALEN) {
+			// update counters in case of successful transmit
+			if (byRSR & RSR_ADDRBROAD) {
+				pStatistic->ullRxBroadcastFrames++;
+				pStatistic->ullRxBroadcastBytes += (unsigned long long) cbFrameLength;
+			} else if (byRSR & RSR_ADDRMULTI) {
+				pStatistic->ullRxMulticastFrames++;
+				pStatistic->ullRxMulticastBytes += (unsigned long long) cbFrameLength;
+			} else {
+				pStatistic->ullRxDirectedFrames++;
+				pStatistic->ullRxDirectedBytes += (unsigned long long) cbFrameLength;
+			}
+		}
+	}
+
+	if (byRxRate == 22) {
+		pStatistic->CustomStat.ullRsr11M++;
+		if (byRSR & RSR_CRCOK) {
+			pStatistic->CustomStat.ullRsr11MCRCOk++;
+		}
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "11M: ALL[%d], OK[%d]:[%02x]\n", (int)pStatistic->CustomStat.ullRsr11M, (int)pStatistic->CustomStat.ullRsr11MCRCOk, byRSR);
+	} else if (byRxRate == 11) {
+		pStatistic->CustomStat.ullRsr5M++;
+		if (byRSR & RSR_CRCOK) {
+			pStatistic->CustomStat.ullRsr5MCRCOk++;
+		}
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " 5M: ALL[%d], OK[%d]:[%02x]\n", (int)pStatistic->CustomStat.ullRsr5M, (int)pStatistic->CustomStat.ullRsr5MCRCOk, byRSR);
+	} else if (byRxRate == 4) {
+		pStatistic->CustomStat.ullRsr2M++;
+		if (byRSR & RSR_CRCOK) {
+			pStatistic->CustomStat.ullRsr2MCRCOk++;
+		}
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " 2M: ALL[%d], OK[%d]:[%02x]\n", (int)pStatistic->CustomStat.ullRsr2M, (int)pStatistic->CustomStat.ullRsr2MCRCOk, byRSR);
+	} else if (byRxRate == 2) {
+		pStatistic->CustomStat.ullRsr1M++;
+		if (byRSR & RSR_CRCOK) {
+			pStatistic->CustomStat.ullRsr1MCRCOk++;
+		}
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " 1M: ALL[%d], OK[%d]:[%02x]\n", (int)pStatistic->CustomStat.ullRsr1M, (int)pStatistic->CustomStat.ullRsr1MCRCOk, byRSR);
+	} else if (byRxRate == 12) {
+		pStatistic->CustomStat.ullRsr6M++;
+		if (byRSR & RSR_CRCOK) {
+			pStatistic->CustomStat.ullRsr6MCRCOk++;
+		}
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " 6M: ALL[%d], OK[%d]\n", (int)pStatistic->CustomStat.ullRsr6M, (int)pStatistic->CustomStat.ullRsr6MCRCOk);
+	} else if (byRxRate == 18) {
+		pStatistic->CustomStat.ullRsr9M++;
+		if (byRSR & RSR_CRCOK) {
+			pStatistic->CustomStat.ullRsr9MCRCOk++;
+		}
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " 9M: ALL[%d], OK[%d]\n", (int)pStatistic->CustomStat.ullRsr9M, (int)pStatistic->CustomStat.ullRsr9MCRCOk);
+	} else if (byRxRate == 24) {
+		pStatistic->CustomStat.ullRsr12M++;
+		if (byRSR & RSR_CRCOK) {
+			pStatistic->CustomStat.ullRsr12MCRCOk++;
+		}
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "12M: ALL[%d], OK[%d]\n", (int)pStatistic->CustomStat.ullRsr12M, (int)pStatistic->CustomStat.ullRsr12MCRCOk);
+	} else if (byRxRate == 36) {
+		pStatistic->CustomStat.ullRsr18M++;
+		if (byRSR & RSR_CRCOK) {
+			pStatistic->CustomStat.ullRsr18MCRCOk++;
+		}
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "18M: ALL[%d], OK[%d]\n", (int)pStatistic->CustomStat.ullRsr18M, (int)pStatistic->CustomStat.ullRsr18MCRCOk);
+	} else if (byRxRate == 48) {
+		pStatistic->CustomStat.ullRsr24M++;
+		if (byRSR & RSR_CRCOK) {
+			pStatistic->CustomStat.ullRsr24MCRCOk++;
+		}
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "24M: ALL[%d], OK[%d]\n", (int)pStatistic->CustomStat.ullRsr24M, (int)pStatistic->CustomStat.ullRsr24MCRCOk);
+	} else if (byRxRate == 72) {
+		pStatistic->CustomStat.ullRsr36M++;
+		if (byRSR & RSR_CRCOK) {
+			pStatistic->CustomStat.ullRsr36MCRCOk++;
+		}
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "36M: ALL[%d], OK[%d]\n", (int)pStatistic->CustomStat.ullRsr36M, (int)pStatistic->CustomStat.ullRsr36MCRCOk);
+	} else if (byRxRate == 96) {
+		pStatistic->CustomStat.ullRsr48M++;
+		if (byRSR & RSR_CRCOK) {
+			pStatistic->CustomStat.ullRsr48MCRCOk++;
+		}
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "48M: ALL[%d], OK[%d]\n", (int)pStatistic->CustomStat.ullRsr48M, (int)pStatistic->CustomStat.ullRsr48MCRCOk);
+	} else if (byRxRate == 108) {
+		pStatistic->CustomStat.ullRsr54M++;
+		if (byRSR & RSR_CRCOK) {
+			pStatistic->CustomStat.ullRsr54MCRCOk++;
+		}
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "54M: ALL[%d], OK[%d]\n", (int)pStatistic->CustomStat.ullRsr54M, (int)pStatistic->CustomStat.ullRsr54MCRCOk);
+	} else {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Unknown: Total[%d], CRCOK[%d]\n", (int)pStatistic->dwRsrRxPacket+1, (int)pStatistic->dwRsrCRCOk);
+	}
+
+	if (byRSR & RSR_BSSIDOK)
+		pStatistic->dwRsrBSSIDOk++;
+
+	if (byRSR & RSR_BCNSSIDOK)
+		pStatistic->dwRsrBCNSSIDOk++;
+	if (byRSR & RSR_IVLDLEN)  //invalid len (> 2312 byte)
+		pStatistic->dwRsrLENErr++;
+	if (byRSR & RSR_IVLDTYP)  //invalid packet type
+		pStatistic->dwRsrTYPErr++;
+	if (byRSR & (RSR_IVLDTYP | RSR_IVLDLEN))
+		pStatistic->dwRsrErr++;
+
+	if (byNewRSR & NEWRSR_DECRYPTOK)
+		pStatistic->dwNewRsrDECRYPTOK++;
+	if (byNewRSR & NEWRSR_CFPIND)
+		pStatistic->dwNewRsrCFP++;
+	if (byNewRSR & NEWRSR_HWUTSF)
+		pStatistic->dwNewRsrUTSF++;
+	if (byNewRSR & NEWRSR_BCNHITAID)
+		pStatistic->dwNewRsrHITAID++;
+	if (byNewRSR & NEWRSR_BCNHITAID0)
+		pStatistic->dwNewRsrHITAID0++;
+
+	// increase rx packet count
+	pStatistic->dwRsrRxPacket++;
+	pStatistic->dwRsrRxOctet += cbFrameLength;
+
+	if (IS_TYPE_DATA(pbyBuffer)) {
+		pStatistic->dwRsrRxData++;
+	} else if (IS_TYPE_MGMT(pbyBuffer)) {
+		pStatistic->dwRsrRxManage++;
+	} else if (IS_TYPE_CONTROL(pbyBuffer)) {
+		pStatistic->dwRsrRxControl++;
+	}
+
+	if (byRSR & RSR_ADDRBROAD)
+		pStatistic->dwRsrBroadcast++;
+	else if (byRSR & RSR_ADDRMULTI)
+		pStatistic->dwRsrMulticast++;
+	else
+		pStatistic->dwRsrDirected++;
+
+	if (WLAN_GET_FC_MOREFRAG(pHeader->wFrameCtl))
+		pStatistic->dwRsrRxFragment++;
+
+	if (cbFrameLength < ETH_ZLEN + 4) {
+		pStatistic->dwRsrRunt++;
+	} else if (cbFrameLength == ETH_ZLEN + 4) {
+		pStatistic->dwRsrRxFrmLen64++;
+	} else if ((65 <= cbFrameLength) && (cbFrameLength <= 127)) {
+		pStatistic->dwRsrRxFrmLen65_127++;
+	} else if ((128 <= cbFrameLength) && (cbFrameLength <= 255)) {
+		pStatistic->dwRsrRxFrmLen128_255++;
+	} else if ((256 <= cbFrameLength) && (cbFrameLength <= 511)) {
+		pStatistic->dwRsrRxFrmLen256_511++;
+	} else if ((512 <= cbFrameLength) && (cbFrameLength <= 1023)) {
+		pStatistic->dwRsrRxFrmLen512_1023++;
+	} else if ((1024 <= cbFrameLength) && (cbFrameLength <= ETH_FRAME_LEN + 4)) {
+		pStatistic->dwRsrRxFrmLen1024_1518++;
+	} else if (cbFrameLength > ETH_FRAME_LEN + 4) {
+		pStatistic->dwRsrLong++;
+	}
 }
 
-
-
 /*
  * Description: Update Rx Statistic Counter and copy Rx buffer
  *
@@ -387,31 +343,30 @@ void STAvUpdateRDStatCounter (PSStatCounter pStatistic,
  */
 
 void
-STAvUpdateRDStatCounterEx (
-    PSStatCounter   pStatistic,
-    unsigned char byRSR,
-    unsigned char byNewRSR,
-    unsigned char byRxRate,
-    unsigned char *pbyBuffer,
-    unsigned int cbFrameLength
-    )
+STAvUpdateRDStatCounterEx(
+	PSStatCounter   pStatistic,
+	unsigned char byRSR,
+	unsigned char byNewRSR,
+	unsigned char byRxRate,
+	unsigned char *pbyBuffer,
+	unsigned int cbFrameLength
+)
 {
-    STAvUpdateRDStatCounter(
-                    pStatistic,
-                    byRSR,
-                    byNewRSR,
-                    byRxRate,
-                    pbyBuffer,
-                    cbFrameLength
-                    );
-
-    // rx length
-    pStatistic->dwCntRxFrmLength = cbFrameLength;
-    // rx pattern, we just see 10 bytes for sample
-    memcpy(pStatistic->abyCntRxPattern, (unsigned char *)pbyBuffer, 10);
+	STAvUpdateRDStatCounter(
+		pStatistic,
+		byRSR,
+		byNewRSR,
+		byRxRate,
+		pbyBuffer,
+		cbFrameLength
+);
+
+	// rx length
+	pStatistic->dwCntRxFrmLength = cbFrameLength;
+	// rx pattern, we just see 10 bytes for sample
+	memcpy(pStatistic->abyCntRxPattern, (unsigned char *)pbyBuffer, 10);
 }
 
-
 /*
  * Description: Update Tx Statistic Counter
  *
@@ -430,81 +385,73 @@ STAvUpdateRDStatCounterEx (
  *
  */
 void
-STAvUpdateTDStatCounter (
-    PSStatCounter   pStatistic,
-    unsigned char byTSR0,
-    unsigned char byTSR1,
-    unsigned char *pbyBuffer,
-    unsigned int cbFrameLength,
-    unsigned int uIdx
-    )
+STAvUpdateTDStatCounter(
+	PSStatCounter   pStatistic,
+	unsigned char byTSR0,
+	unsigned char byTSR1,
+	unsigned char *pbyBuffer,
+	unsigned int cbFrameLength,
+	unsigned int uIdx
+)
 {
-    PWLAN_80211HDR_A4   pHeader;
-    unsigned char *pbyDestAddr;
-    unsigned char byTSR0_NCR = byTSR0 & TSR0_NCR;
-
-
-
-    pHeader = (PWLAN_80211HDR_A4) pbyBuffer;
-    if (WLAN_GET_FC_TODS(pHeader->wFrameCtl) == 0) {
-        pbyDestAddr = &(pHeader->abyAddr1[0]);
-    }
-    else {
-        pbyDestAddr = &(pHeader->abyAddr3[0]);
-    }
-    // increase tx packet count
-    pStatistic->dwTsrTxPacket[uIdx]++;
-    pStatistic->dwTsrTxOctet[uIdx] += cbFrameLength;
-
-    if (byTSR0_NCR != 0) {
-        pStatistic->dwTsrRetry[uIdx]++;
-        pStatistic->dwTsrTotalRetry[uIdx] += byTSR0_NCR;
-
-        if (byTSR0_NCR == 1)
-            pStatistic->dwTsrOnceRetry[uIdx]++;
-        else
-            pStatistic->dwTsrMoreThanOnceRetry[uIdx]++;
-    }
-
-    if ((byTSR1&(TSR1_TERR|TSR1_RETRYTMO|TSR1_TMO|ACK_DATA)) == 0) {
-        pStatistic->ullTsrOK[uIdx]++;
-        pStatistic->CustomStat.ullTsrAllOK =
-            (pStatistic->ullTsrOK[TYPE_AC0DMA] + pStatistic->ullTsrOK[TYPE_TXDMA0]);
-        // update counters in case that successful transmit
-        if (is_broadcast_ether_addr(pbyDestAddr)) {
-            pStatistic->ullTxBroadcastFrames[uIdx]++;
-            pStatistic->ullTxBroadcastBytes[uIdx] += (unsigned long long) cbFrameLength;
-        }
-        else if (is_multicast_ether_addr(pbyDestAddr)) {
-            pStatistic->ullTxMulticastFrames[uIdx]++;
-            pStatistic->ullTxMulticastBytes[uIdx] += (unsigned long long) cbFrameLength;
-        }
-        else {
-            pStatistic->ullTxDirectedFrames[uIdx]++;
-            pStatistic->ullTxDirectedBytes[uIdx] += (unsigned long long) cbFrameLength;
-        }
-    }
-    else {
-        if (byTSR1 & TSR1_TERR)
-            pStatistic->dwTsrErr[uIdx]++;
-        if (byTSR1 & TSR1_RETRYTMO)
-            pStatistic->dwTsrRetryTimeout[uIdx]++;
-        if (byTSR1 & TSR1_TMO)
-            pStatistic->dwTsrTransmitTimeout[uIdx]++;
-        if (byTSR1 & ACK_DATA)
-            pStatistic->dwTsrACKData[uIdx]++;
-    }
-
-    if (is_broadcast_ether_addr(pbyDestAddr))
-        pStatistic->dwTsrBroadcast[uIdx]++;
-    else if (is_multicast_ether_addr(pbyDestAddr))
-        pStatistic->dwTsrMulticast[uIdx]++;
-    else
-        pStatistic->dwTsrDirected[uIdx]++;
-
+	PWLAN_80211HDR_A4   pHeader;
+	unsigned char *pbyDestAddr;
+	unsigned char byTSR0_NCR = byTSR0 & TSR0_NCR;
+
+	pHeader = (PWLAN_80211HDR_A4) pbyBuffer;
+	if (WLAN_GET_FC_TODS(pHeader->wFrameCtl) == 0) {
+		pbyDestAddr = &(pHeader->abyAddr1[0]);
+	} else {
+		pbyDestAddr = &(pHeader->abyAddr3[0]);
+	}
+	// increase tx packet count
+	pStatistic->dwTsrTxPacket[uIdx]++;
+	pStatistic->dwTsrTxOctet[uIdx] += cbFrameLength;
+
+	if (byTSR0_NCR != 0) {
+		pStatistic->dwTsrRetry[uIdx]++;
+		pStatistic->dwTsrTotalRetry[uIdx] += byTSR0_NCR;
+
+		if (byTSR0_NCR == 1)
+			pStatistic->dwTsrOnceRetry[uIdx]++;
+		else
+			pStatistic->dwTsrMoreThanOnceRetry[uIdx]++;
+	}
+
+	if ((byTSR1&(TSR1_TERR|TSR1_RETRYTMO|TSR1_TMO|ACK_DATA)) == 0) {
+		pStatistic->ullTsrOK[uIdx]++;
+		pStatistic->CustomStat.ullTsrAllOK =
+			(pStatistic->ullTsrOK[TYPE_AC0DMA] + pStatistic->ullTsrOK[TYPE_TXDMA0]);
+		// update counters in case that successful transmit
+		if (is_broadcast_ether_addr(pbyDestAddr)) {
+			pStatistic->ullTxBroadcastFrames[uIdx]++;
+			pStatistic->ullTxBroadcastBytes[uIdx] += (unsigned long long) cbFrameLength;
+		} else if (is_multicast_ether_addr(pbyDestAddr)) {
+			pStatistic->ullTxMulticastFrames[uIdx]++;
+			pStatistic->ullTxMulticastBytes[uIdx] += (unsigned long long) cbFrameLength;
+		} else {
+			pStatistic->ullTxDirectedFrames[uIdx]++;
+			pStatistic->ullTxDirectedBytes[uIdx] += (unsigned long long) cbFrameLength;
+		}
+	} else {
+		if (byTSR1 & TSR1_TERR)
+			pStatistic->dwTsrErr[uIdx]++;
+		if (byTSR1 & TSR1_RETRYTMO)
+			pStatistic->dwTsrRetryTimeout[uIdx]++;
+		if (byTSR1 & TSR1_TMO)
+			pStatistic->dwTsrTransmitTimeout[uIdx]++;
+		if (byTSR1 & ACK_DATA)
+			pStatistic->dwTsrACKData[uIdx]++;
+	}
+
+	if (is_broadcast_ether_addr(pbyDestAddr))
+		pStatistic->dwTsrBroadcast[uIdx]++;
+	else if (is_multicast_ether_addr(pbyDestAddr))
+		pStatistic->dwTsrMulticast[uIdx]++;
+	else
+		pStatistic->dwTsrDirected[uIdx]++;
 }
 
-
 /*
  * Description: Update Tx Statistic Counter and copy Tx buffer
  *
@@ -520,23 +467,22 @@ STAvUpdateTDStatCounter (
  *
  */
 void
-STAvUpdateTDStatCounterEx (
-    PSStatCounter   pStatistic,
-    unsigned char *pbyBuffer,
-    unsigned long cbFrameLength
-    )
+STAvUpdateTDStatCounterEx(
+	PSStatCounter   pStatistic,
+	unsigned char *pbyBuffer,
+	unsigned long cbFrameLength
+)
 {
-    unsigned int uPktLength;
+	unsigned int uPktLength;
 
-    uPktLength = (unsigned int)cbFrameLength;
+	uPktLength = (unsigned int)cbFrameLength;
 
-    // tx length
-    pStatistic->dwCntTxBufLength = uPktLength;
-    // tx pattern, we just see 16 bytes for sample
-    memcpy(pStatistic->abyCntTxPattern, pbyBuffer, 16);
+	// tx length
+	pStatistic->dwCntTxBufLength = uPktLength;
+	// tx pattern, we just see 16 bytes for sample
+	memcpy(pStatistic->abyCntTxPattern, pbyBuffer, 16);
 }
 
-
 /*
  * Description: Update 802.11 mib counter
  *
@@ -553,28 +499,28 @@ STAvUpdateTDStatCounterEx (
  */
 void
 STAvUpdate802_11Counter(
-    PSDot11Counters         p802_11Counter,
-    PSStatCounter           pStatistic,
-    unsigned long dwCounter
-    )
+	PSDot11Counters         p802_11Counter,
+	PSStatCounter           pStatistic,
+	unsigned long dwCounter
+)
 {
-    //p802_11Counter->TransmittedFragmentCount
-    p802_11Counter->MulticastTransmittedFrameCount = (unsigned long long) (pStatistic->dwTsrBroadcast[TYPE_AC0DMA] +
-                                                                  pStatistic->dwTsrBroadcast[TYPE_TXDMA0] +
-                                                                  pStatistic->dwTsrMulticast[TYPE_AC0DMA] +
-                                                                  pStatistic->dwTsrMulticast[TYPE_TXDMA0]);
-    p802_11Counter->FailedCount = (unsigned long long) (pStatistic->dwTsrErr[TYPE_AC0DMA] + pStatistic->dwTsrErr[TYPE_TXDMA0]);
-    p802_11Counter->RetryCount = (unsigned long long) (pStatistic->dwTsrRetry[TYPE_AC0DMA] + pStatistic->dwTsrRetry[TYPE_TXDMA0]);
-    p802_11Counter->MultipleRetryCount = (unsigned long long) (pStatistic->dwTsrMoreThanOnceRetry[TYPE_AC0DMA] +
-                                                          pStatistic->dwTsrMoreThanOnceRetry[TYPE_TXDMA0]);
-    //p802_11Counter->FrameDuplicateCount
-    p802_11Counter->RTSSuccessCount += (unsigned long long)  (dwCounter & 0x000000ff);
-    p802_11Counter->RTSFailureCount += (unsigned long long) ((dwCounter & 0x0000ff00) >> 8);
-    p802_11Counter->ACKFailureCount += (unsigned long long) ((dwCounter & 0x00ff0000) >> 16);
-    p802_11Counter->FCSErrorCount +=   (unsigned long long) ((dwCounter & 0xff000000) >> 24);
-    //p802_11Counter->ReceivedFragmentCount
-    p802_11Counter->MulticastReceivedFrameCount = (unsigned long long) (pStatistic->dwRsrBroadcast +
-                                                               pStatistic->dwRsrMulticast);
+	//p802_11Counter->TransmittedFragmentCount
+	p802_11Counter->MulticastTransmittedFrameCount = (unsigned long long) (pStatistic->dwTsrBroadcast[TYPE_AC0DMA] +
+									       pStatistic->dwTsrBroadcast[TYPE_TXDMA0] +
+									       pStatistic->dwTsrMulticast[TYPE_AC0DMA] +
+									       pStatistic->dwTsrMulticast[TYPE_TXDMA0]);
+	p802_11Counter->FailedCount = (unsigned long long) (pStatistic->dwTsrErr[TYPE_AC0DMA] + pStatistic->dwTsrErr[TYPE_TXDMA0]);
+	p802_11Counter->RetryCount = (unsigned long long) (pStatistic->dwTsrRetry[TYPE_AC0DMA] + pStatistic->dwTsrRetry[TYPE_TXDMA0]);
+	p802_11Counter->MultipleRetryCount = (unsigned long long) (pStatistic->dwTsrMoreThanOnceRetry[TYPE_AC0DMA] +
+								   pStatistic->dwTsrMoreThanOnceRetry[TYPE_TXDMA0]);
+	//p802_11Counter->FrameDuplicateCount
+	p802_11Counter->RTSSuccessCount += (unsigned long long)  (dwCounter & 0x000000ff);
+	p802_11Counter->RTSFailureCount += (unsigned long long) ((dwCounter & 0x0000ff00) >> 8);
+	p802_11Counter->ACKFailureCount += (unsigned long long) ((dwCounter & 0x00ff0000) >> 16);
+	p802_11Counter->FCSErrorCount +=   (unsigned long long) ((dwCounter & 0xff000000) >> 24);
+	//p802_11Counter->ReceivedFragmentCount
+	p802_11Counter->MulticastReceivedFrameCount = (unsigned long long) (pStatistic->dwRsrBroadcast +
+									    pStatistic->dwRsrMulticast);
 }
 
 /*
@@ -592,6 +538,6 @@ STAvUpdate802_11Counter(
 void
 STAvClear802_11Counter(PSDot11Counters p802_11Counter)
 {
-    // set memory to zero
+	// set memory to zero
 	memset(p802_11Counter, 0, sizeof(SDot11Counters));
 }
diff --git a/drivers/staging/vt6655/mib.h b/drivers/staging/vt6655/mib.h
index 5cd697a2bc7..6b99c119b78 100644
--- a/drivers/staging/vt6655/mib.h
+++ b/drivers/staging/vt6655/mib.h
@@ -39,62 +39,61 @@
 //
 
 typedef struct tagSDot11Counters {
-    unsigned long Length;             // Length of structure
-    unsigned long long   TransmittedFragmentCount;
-    unsigned long long   MulticastTransmittedFrameCount;
-    unsigned long long   FailedCount;
-    unsigned long long   RetryCount;
-    unsigned long long   MultipleRetryCount;
-    unsigned long long   RTSSuccessCount;
-    unsigned long long   RTSFailureCount;
-    unsigned long long   ACKFailureCount;
-    unsigned long long   FrameDuplicateCount;
-    unsigned long long   ReceivedFragmentCount;
-    unsigned long long   MulticastReceivedFrameCount;
-    unsigned long long   FCSErrorCount;
-    unsigned long long   TKIPLocalMICFailures;
-    unsigned long long   TKIPRemoteMICFailures;
-    unsigned long long   TKIPICVErrors;
-    unsigned long long   TKIPCounterMeasuresInvoked;
-    unsigned long long   TKIPReplays;
-    unsigned long long   CCMPFormatErrors;
-    unsigned long long   CCMPReplays;
-    unsigned long long   CCMPDecryptErrors;
-    unsigned long long   FourWayHandshakeFailures;
+	unsigned long Length;             // Length of structure
+	unsigned long long   TransmittedFragmentCount;
+	unsigned long long   MulticastTransmittedFrameCount;
+	unsigned long long   FailedCount;
+	unsigned long long   RetryCount;
+	unsigned long long   MultipleRetryCount;
+	unsigned long long   RTSSuccessCount;
+	unsigned long long   RTSFailureCount;
+	unsigned long long   ACKFailureCount;
+	unsigned long long   FrameDuplicateCount;
+	unsigned long long   ReceivedFragmentCount;
+	unsigned long long   MulticastReceivedFrameCount;
+	unsigned long long   FCSErrorCount;
+	unsigned long long   TKIPLocalMICFailures;
+	unsigned long long   TKIPRemoteMICFailures;
+	unsigned long long   TKIPICVErrors;
+	unsigned long long   TKIPCounterMeasuresInvoked;
+	unsigned long long   TKIPReplays;
+	unsigned long long   CCMPFormatErrors;
+	unsigned long long   CCMPReplays;
+	unsigned long long   CCMPDecryptErrors;
+	unsigned long long   FourWayHandshakeFailures;
 //    unsigned long long   WEPUndecryptableCount;
 //    unsigned long long   WEPICVErrorCount;
 //    unsigned long long   DecryptSuccessCount;
 //    unsigned long long   DecryptFailureCount;
 } SDot11Counters, *PSDot11Counters;
 
-
 //
 // MIB2 counter
 //
 typedef struct tagSMib2Counter {
-    long    ifIndex;
-    char    ifDescr[256];               // max size 255 plus zero ending
-                                        // e.g. "interface 1"
-    long    ifType;
-    long    ifMtu;
-    unsigned long ifSpeed;
-    unsigned char ifPhysAddress[ETH_ALEN];
-    long    ifAdminStatus;
-    long    ifOperStatus;
-    unsigned long ifLastChange;
-    unsigned long ifInOctets;
-    unsigned long ifInUcastPkts;
-    unsigned long ifInNUcastPkts;
-    unsigned long ifInDiscards;
-    unsigned long ifInErrors;
-    unsigned long ifInUnknownProtos;
-    unsigned long ifOutOctets;
-    unsigned long ifOutUcastPkts;
-    unsigned long ifOutNUcastPkts;
-    unsigned long ifOutDiscards;
-    unsigned long ifOutErrors;
-    unsigned long ifOutQLen;
-    unsigned long ifSpecific;
+	long    ifIndex;
+	char    ifDescr[256];               // max size 255 plus zero ending
+	// e.g. "interface 1"
+	long    ifType;
+	long    ifMtu;
+	unsigned long ifSpeed;
+	unsigned char ifPhysAddress[ETH_ALEN];
+	long    ifAdminStatus;
+	long    ifOperStatus;
+	unsigned long ifLastChange;
+	unsigned long ifInOctets;
+	unsigned long ifInUcastPkts;
+	unsigned long ifInNUcastPkts;
+	unsigned long ifInDiscards;
+	unsigned long ifInErrors;
+	unsigned long ifInUnknownProtos;
+	unsigned long ifOutOctets;
+	unsigned long ifOutUcastPkts;
+	unsigned long ifOutNUcastPkts;
+	unsigned long ifOutDiscards;
+	unsigned long ifOutErrors;
+	unsigned long ifOutQLen;
+	unsigned long ifSpecific;
 } SMib2Counter, *PSMib2Counter;
 
 // Value in the ifType entry
@@ -105,104 +104,100 @@ typedef struct tagSMib2Counter {
 #define DOWN                2           //
 #define TESTING             3           //
 
-
 //
 // RMON counter
 //
 typedef struct tagSRmonCounter {
-    long    etherStatsIndex;
-    unsigned long etherStatsDataSource;
-    unsigned long etherStatsDropEvents;
-    unsigned long etherStatsOctets;
-    unsigned long etherStatsPkts;
-    unsigned long etherStatsBroadcastPkts;
-    unsigned long etherStatsMulticastPkts;
-    unsigned long etherStatsCRCAlignErrors;
-    unsigned long etherStatsUndersizePkts;
-    unsigned long etherStatsOversizePkts;
-    unsigned long etherStatsFragments;
-    unsigned long etherStatsJabbers;
-    unsigned long etherStatsCollisions;
-    unsigned long etherStatsPkt64Octets;
-    unsigned long etherStatsPkt65to127Octets;
-    unsigned long etherStatsPkt128to255Octets;
-    unsigned long etherStatsPkt256to511Octets;
-    unsigned long etherStatsPkt512to1023Octets;
-    unsigned long etherStatsPkt1024to1518Octets;
-    unsigned long etherStatsOwners;
-    unsigned long etherStatsStatus;
+	long    etherStatsIndex;
+	unsigned long etherStatsDataSource;
+	unsigned long etherStatsDropEvents;
+	unsigned long etherStatsOctets;
+	unsigned long etherStatsPkts;
+	unsigned long etherStatsBroadcastPkts;
+	unsigned long etherStatsMulticastPkts;
+	unsigned long etherStatsCRCAlignErrors;
+	unsigned long etherStatsUndersizePkts;
+	unsigned long etherStatsOversizePkts;
+	unsigned long etherStatsFragments;
+	unsigned long etherStatsJabbers;
+	unsigned long etherStatsCollisions;
+	unsigned long etherStatsPkt64Octets;
+	unsigned long etherStatsPkt65to127Octets;
+	unsigned long etherStatsPkt128to255Octets;
+	unsigned long etherStatsPkt256to511Octets;
+	unsigned long etherStatsPkt512to1023Octets;
+	unsigned long etherStatsPkt1024to1518Octets;
+	unsigned long etherStatsOwners;
+	unsigned long etherStatsStatus;
 } SRmonCounter, *PSRmonCounter;
 
 //
 // Custom counter
 //
 typedef struct tagSCustomCounters {
-    unsigned long Length;
-
-    unsigned long long   ullTsrAllOK;
-
-    unsigned long long   ullRsr11M;
-    unsigned long long   ullRsr5M;
-    unsigned long long   ullRsr2M;
-    unsigned long long   ullRsr1M;
-
-    unsigned long long   ullRsr11MCRCOk;
-    unsigned long long   ullRsr5MCRCOk;
-    unsigned long long   ullRsr2MCRCOk;
-    unsigned long long   ullRsr1MCRCOk;
-
-    unsigned long long   ullRsr54M;
-    unsigned long long   ullRsr48M;
-    unsigned long long   ullRsr36M;
-    unsigned long long   ullRsr24M;
-    unsigned long long   ullRsr18M;
-    unsigned long long   ullRsr12M;
-    unsigned long long   ullRsr9M;
-    unsigned long long   ullRsr6M;
-
-    unsigned long long   ullRsr54MCRCOk;
-    unsigned long long   ullRsr48MCRCOk;
-    unsigned long long   ullRsr36MCRCOk;
-    unsigned long long   ullRsr24MCRCOk;
-    unsigned long long   ullRsr18MCRCOk;
-    unsigned long long   ullRsr12MCRCOk;
-    unsigned long long   ullRsr9MCRCOk;
-    unsigned long long   ullRsr6MCRCOk;
-
+	unsigned long Length;
+
+	unsigned long long   ullTsrAllOK;
+
+	unsigned long long   ullRsr11M;
+	unsigned long long   ullRsr5M;
+	unsigned long long   ullRsr2M;
+	unsigned long long   ullRsr1M;
+
+	unsigned long long   ullRsr11MCRCOk;
+	unsigned long long   ullRsr5MCRCOk;
+	unsigned long long   ullRsr2MCRCOk;
+	unsigned long long   ullRsr1MCRCOk;
+
+	unsigned long long   ullRsr54M;
+	unsigned long long   ullRsr48M;
+	unsigned long long   ullRsr36M;
+	unsigned long long   ullRsr24M;
+	unsigned long long   ullRsr18M;
+	unsigned long long   ullRsr12M;
+	unsigned long long   ullRsr9M;
+	unsigned long long   ullRsr6M;
+
+	unsigned long long   ullRsr54MCRCOk;
+	unsigned long long   ullRsr48MCRCOk;
+	unsigned long long   ullRsr36MCRCOk;
+	unsigned long long   ullRsr24MCRCOk;
+	unsigned long long   ullRsr18MCRCOk;
+	unsigned long long   ullRsr12MCRCOk;
+	unsigned long long   ullRsr9MCRCOk;
+	unsigned long long   ullRsr6MCRCOk;
 } SCustomCounters, *PSCustomCounters;
 
-
 //
 // Custom counter
 //
 typedef struct tagSISRCounters {
-    unsigned long Length;
-
-    unsigned long dwIsrTx0OK;
-    unsigned long dwIsrAC0TxOK;
-    unsigned long dwIsrBeaconTxOK;
-    unsigned long dwIsrRx0OK;
-    unsigned long dwIsrTBTTInt;
-    unsigned long dwIsrSTIMERInt;
-    unsigned long dwIsrWatchDog;
-    unsigned long dwIsrUnrecoverableError;
-    unsigned long dwIsrSoftInterrupt;
-    unsigned long dwIsrMIBNearfull;
-    unsigned long dwIsrRxNoBuf;
-
-    unsigned long dwIsrUnknown;               // unknown interrupt count
-
-    unsigned long dwIsrRx1OK;
-    unsigned long dwIsrATIMTxOK;
-    unsigned long dwIsrSYNCTxOK;
-    unsigned long dwIsrCFPEnd;
-    unsigned long dwIsrATIMEnd;
-    unsigned long dwIsrSYNCFlushOK;
-    unsigned long dwIsrSTIMER1Int;
-    /////////////////////////////////////
+	unsigned long Length;
+
+	unsigned long dwIsrTx0OK;
+	unsigned long dwIsrAC0TxOK;
+	unsigned long dwIsrBeaconTxOK;
+	unsigned long dwIsrRx0OK;
+	unsigned long dwIsrTBTTInt;
+	unsigned long dwIsrSTIMERInt;
+	unsigned long dwIsrWatchDog;
+	unsigned long dwIsrUnrecoverableError;
+	unsigned long dwIsrSoftInterrupt;
+	unsigned long dwIsrMIBNearfull;
+	unsigned long dwIsrRxNoBuf;
+
+	unsigned long dwIsrUnknown;               // unknown interrupt count
+
+	unsigned long dwIsrRx1OK;
+	unsigned long dwIsrATIMTxOK;
+	unsigned long dwIsrSYNCTxOK;
+	unsigned long dwIsrCFPEnd;
+	unsigned long dwIsrATIMEnd;
+	unsigned long dwIsrSYNCFlushOK;
+	unsigned long dwIsrSTIMER1Int;
+	/////////////////////////////////////
 } SISRCounters, *PSISRCounters;
 
-
 // Value in the etherStatsStatus entry
 #define VALID               1           //
 #define CREATE_REQUEST      2           //
@@ -213,125 +208,122 @@ typedef struct tagSISRCounters {
 // statistic counter
 //
 typedef struct tagSStatCounter {
-    //
-    // ISR status count
-    //
-
-
-    // RSR status count
-    //
-    unsigned long dwRsrFrmAlgnErr;
-    unsigned long dwRsrErr;
-    unsigned long dwRsrCRCErr;
-    unsigned long dwRsrCRCOk;
-    unsigned long dwRsrBSSIDOk;
-    unsigned long dwRsrADDROk;
-    unsigned long dwRsrBCNSSIDOk;
-    unsigned long dwRsrLENErr;
-    unsigned long dwRsrTYPErr;
-
-    unsigned long dwNewRsrDECRYPTOK;
-    unsigned long dwNewRsrCFP;
-    unsigned long dwNewRsrUTSF;
-    unsigned long dwNewRsrHITAID;
-    unsigned long dwNewRsrHITAID0;
-
-    unsigned long dwRsrLong;
-    unsigned long dwRsrRunt;
-
-    unsigned long dwRsrRxControl;
-    unsigned long dwRsrRxData;
-    unsigned long dwRsrRxManage;
-
-    unsigned long dwRsrRxPacket;
-    unsigned long dwRsrRxOctet;
-    unsigned long dwRsrBroadcast;
-    unsigned long dwRsrMulticast;
-    unsigned long dwRsrDirected;
-    // 64-bit OID
-    unsigned long long   ullRsrOK;
-
-    // for some optional OIDs (64 bits) and DMI support
-    unsigned long long   ullRxBroadcastBytes;
-    unsigned long long   ullRxMulticastBytes;
-    unsigned long long   ullRxDirectedBytes;
-    unsigned long long   ullRxBroadcastFrames;
-    unsigned long long   ullRxMulticastFrames;
-    unsigned long long   ullRxDirectedFrames;
-
-    unsigned long dwRsrRxFragment;
-    unsigned long dwRsrRxFrmLen64;
-    unsigned long dwRsrRxFrmLen65_127;
-    unsigned long dwRsrRxFrmLen128_255;
-    unsigned long dwRsrRxFrmLen256_511;
-    unsigned long dwRsrRxFrmLen512_1023;
-    unsigned long dwRsrRxFrmLen1024_1518;
-
-    // TSR status count
-    //
-    unsigned long dwTsrTotalRetry[TYPE_MAXTD];        // total collision retry count
-    unsigned long dwTsrOnceRetry[TYPE_MAXTD];         // this packet only occur one collision
-    unsigned long dwTsrMoreThanOnceRetry[TYPE_MAXTD]; // this packet occur more than one collision
-    unsigned long dwTsrRetry[TYPE_MAXTD];             // this packet has ever occur collision,
-                                         // that is (dwTsrOnceCollision0 + dwTsrMoreThanOnceCollision0)
-    unsigned long dwTsrACKData[TYPE_MAXTD];
-    unsigned long dwTsrErr[TYPE_MAXTD];
-    unsigned long dwAllTsrOK[TYPE_MAXTD];
-    unsigned long dwTsrRetryTimeout[TYPE_MAXTD];
-    unsigned long dwTsrTransmitTimeout[TYPE_MAXTD];
-
-    unsigned long dwTsrTxPacket[TYPE_MAXTD];
-    unsigned long dwTsrTxOctet[TYPE_MAXTD];
-    unsigned long dwTsrBroadcast[TYPE_MAXTD];
-    unsigned long dwTsrMulticast[TYPE_MAXTD];
-    unsigned long dwTsrDirected[TYPE_MAXTD];
-
-    // RD/TD count
-    unsigned long dwCntRxFrmLength;
-    unsigned long dwCntTxBufLength;
-
-    unsigned char abyCntRxPattern[16];
-    unsigned char abyCntTxPattern[16];
-
-
-
-    // Software check....
-    unsigned long dwCntRxDataErr;             // rx buffer data software compare CRC err count
-    unsigned long dwCntDecryptErr;            // rx buffer data software compare CRC err count
-    unsigned long dwCntRxICVErr;              // rx buffer data software compare CRC err count
-    unsigned int idxRxErrorDesc[TYPE_MAXRD]; // index for rx data error RD
-
-    // 64-bit OID
-    unsigned long long   ullTsrOK[TYPE_MAXTD];
-
-    // for some optional OIDs (64 bits) and DMI support
-    unsigned long long   ullTxBroadcastFrames[TYPE_MAXTD];
-    unsigned long long   ullTxMulticastFrames[TYPE_MAXTD];
-    unsigned long long   ullTxDirectedFrames[TYPE_MAXTD];
-    unsigned long long   ullTxBroadcastBytes[TYPE_MAXTD];
-    unsigned long long   ullTxMulticastBytes[TYPE_MAXTD];
-    unsigned long long   ullTxDirectedBytes[TYPE_MAXTD];
+	//
+	// ISR status count
+	//
+
+	// RSR status count
+	//
+	unsigned long dwRsrFrmAlgnErr;
+	unsigned long dwRsrErr;
+	unsigned long dwRsrCRCErr;
+	unsigned long dwRsrCRCOk;
+	unsigned long dwRsrBSSIDOk;
+	unsigned long dwRsrADDROk;
+	unsigned long dwRsrBCNSSIDOk;
+	unsigned long dwRsrLENErr;
+	unsigned long dwRsrTYPErr;
+
+	unsigned long dwNewRsrDECRYPTOK;
+	unsigned long dwNewRsrCFP;
+	unsigned long dwNewRsrUTSF;
+	unsigned long dwNewRsrHITAID;
+	unsigned long dwNewRsrHITAID0;
+
+	unsigned long dwRsrLong;
+	unsigned long dwRsrRunt;
+
+	unsigned long dwRsrRxControl;
+	unsigned long dwRsrRxData;
+	unsigned long dwRsrRxManage;
+
+	unsigned long dwRsrRxPacket;
+	unsigned long dwRsrRxOctet;
+	unsigned long dwRsrBroadcast;
+	unsigned long dwRsrMulticast;
+	unsigned long dwRsrDirected;
+	// 64-bit OID
+	unsigned long long   ullRsrOK;
+
+	// for some optional OIDs (64 bits) and DMI support
+	unsigned long long   ullRxBroadcastBytes;
+	unsigned long long   ullRxMulticastBytes;
+	unsigned long long   ullRxDirectedBytes;
+	unsigned long long   ullRxBroadcastFrames;
+	unsigned long long   ullRxMulticastFrames;
+	unsigned long long   ullRxDirectedFrames;
+
+	unsigned long dwRsrRxFragment;
+	unsigned long dwRsrRxFrmLen64;
+	unsigned long dwRsrRxFrmLen65_127;
+	unsigned long dwRsrRxFrmLen128_255;
+	unsigned long dwRsrRxFrmLen256_511;
+	unsigned long dwRsrRxFrmLen512_1023;
+	unsigned long dwRsrRxFrmLen1024_1518;
+
+	// TSR status count
+	//
+	unsigned long dwTsrTotalRetry[TYPE_MAXTD];        // total collision retry count
+	unsigned long dwTsrOnceRetry[TYPE_MAXTD];         // this packet only occur one collision
+	unsigned long dwTsrMoreThanOnceRetry[TYPE_MAXTD]; // this packet occur more than one collision
+	unsigned long dwTsrRetry[TYPE_MAXTD];             // this packet has ever occur collision,
+	// that is (dwTsrOnceCollision0 + dwTsrMoreThanOnceCollision0)
+	unsigned long dwTsrACKData[TYPE_MAXTD];
+	unsigned long dwTsrErr[TYPE_MAXTD];
+	unsigned long dwAllTsrOK[TYPE_MAXTD];
+	unsigned long dwTsrRetryTimeout[TYPE_MAXTD];
+	unsigned long dwTsrTransmitTimeout[TYPE_MAXTD];
+
+	unsigned long dwTsrTxPacket[TYPE_MAXTD];
+	unsigned long dwTsrTxOctet[TYPE_MAXTD];
+	unsigned long dwTsrBroadcast[TYPE_MAXTD];
+	unsigned long dwTsrMulticast[TYPE_MAXTD];
+	unsigned long dwTsrDirected[TYPE_MAXTD];
+
+	// RD/TD count
+	unsigned long dwCntRxFrmLength;
+	unsigned long dwCntTxBufLength;
+
+	unsigned char abyCntRxPattern[16];
+	unsigned char abyCntTxPattern[16];
+
+	// Software check....
+	unsigned long dwCntRxDataErr;             // rx buffer data software compare CRC err count
+	unsigned long dwCntDecryptErr;            // rx buffer data software compare CRC err count
+	unsigned long dwCntRxICVErr;              // rx buffer data software compare CRC err count
+	unsigned int idxRxErrorDesc[TYPE_MAXRD]; // index for rx data error RD
+
+	// 64-bit OID
+	unsigned long long   ullTsrOK[TYPE_MAXTD];
+
+	// for some optional OIDs (64 bits) and DMI support
+	unsigned long long   ullTxBroadcastFrames[TYPE_MAXTD];
+	unsigned long long   ullTxMulticastFrames[TYPE_MAXTD];
+	unsigned long long   ullTxDirectedFrames[TYPE_MAXTD];
+	unsigned long long   ullTxBroadcastBytes[TYPE_MAXTD];
+	unsigned long long   ullTxMulticastBytes[TYPE_MAXTD];
+	unsigned long long   ullTxDirectedBytes[TYPE_MAXTD];
 
 //    unsigned long dwTxRetryCount[8];
-    //
-    // ISR status count
-    //
-    SISRCounters ISRStat;
-
-    SCustomCounters CustomStat;
-
-   #ifdef Calcu_LinkQual
-       //Tx count:
-    unsigned long TxNoRetryOkCount;         //success tx no retry !
-    unsigned long TxRetryOkCount;              //success tx but retry !
-    unsigned long TxFailCount;                      //fail tx ?
-      //Rx count:
-    unsigned long RxOkCnt;                          //success rx !
-    unsigned long RxFcsErrCnt;                    //fail rx ?
-      //statistic
-    unsigned long SignalStren;
-    unsigned long LinkQuality;
-   #endif
+	//
+	// ISR status count
+	//
+	SISRCounters ISRStat;
+
+	SCustomCounters CustomStat;
+
+#ifdef Calcu_LinkQual
+	//Tx count:
+	unsigned long TxNoRetryOkCount;         //success tx no retry !
+	unsigned long TxRetryOkCount;              //success tx but retry !
+	unsigned long TxFailCount;                      //fail tx ?
+	//Rx count:
+	unsigned long RxOkCnt;                          //success rx !
+	unsigned long RxFcsErrCnt;                    //fail rx ?
+	//statistic
+	unsigned long SignalStren;
+	unsigned long LinkQuality;
+#endif
 } SStatCounter, *PSStatCounter;
 
 /*---------------------  Export Classes  ----------------------------*/
@@ -345,31 +337,28 @@ void STAvClearAllCounter(PSStatCounter pStatistic);
 void STAvUpdateIsrStatCounter(PSStatCounter pStatistic, unsigned long dwIsr);
 
 void STAvUpdateRDStatCounter(PSStatCounter pStatistic,
-                              unsigned char byRSR, unsigned char byNewRSR, unsigned char byRxRate,
-                              unsigned char *pbyBuffer, unsigned int cbFrameLength);
+			     unsigned char byRSR, unsigned char byNewRSR, unsigned char byRxRate,
+			     unsigned char *pbyBuffer, unsigned int cbFrameLength);
 
 void STAvUpdateRDStatCounterEx(PSStatCounter pStatistic,
-                              unsigned char byRSR, unsigned char byNewRsr, unsigned char byRxRate,
-                              unsigned char *pbyBuffer, unsigned int cbFrameLength);
+			       unsigned char byRSR, unsigned char byNewRsr, unsigned char byRxRate,
+			       unsigned char *pbyBuffer, unsigned int cbFrameLength);
 
 void STAvUpdateTDStatCounter(PSStatCounter pStatistic, unsigned char byTSR0, unsigned char byTSR1,
-		unsigned char *pbyBuffer, unsigned int cbFrameLength, unsigned int uIdx);
+			     unsigned char *pbyBuffer, unsigned int cbFrameLength, unsigned int uIdx);
 
 void STAvUpdateTDStatCounterEx(
-    PSStatCounter   pStatistic,
-    unsigned char *pbyBuffer,
-    unsigned long cbFrameLength
-    );
+	PSStatCounter   pStatistic,
+	unsigned char *pbyBuffer,
+	unsigned long cbFrameLength
+);
 
 void STAvUpdate802_11Counter(
-    PSDot11Counters p802_11Counter,
-    PSStatCounter   pStatistic,
-    unsigned long dwCounter
-    );
+	PSDot11Counters p802_11Counter,
+	PSStatCounter   pStatistic,
+	unsigned long dwCounter
+);
 
 void STAvClear802_11Counter(PSDot11Counters p802_11Counter);
 
 #endif // __MIB_H__
-
-
-
diff --git a/drivers/staging/vt6655/michael.c b/drivers/staging/vt6655/michael.c
index 67618f069d0..7ea5f7f82f4 100644
--- a/drivers/staging/vt6655/michael.c
+++ b/drivers/staging/vt6655/michael.c
@@ -48,11 +48,11 @@
 
 /*---------------------  Static Functions  --------------------------*/
 /*
-static unsigned long s_dwGetUINT32(unsigned char *p);         // Get unsigned long from 4 bytes LSByte first
-static void s_vPutUINT32(unsigned char *p, unsigned long val); // Put unsigned long into 4 bytes LSByte first
+  static unsigned long s_dwGetUINT32(unsigned char *p);         // Get unsigned long from 4 bytes LSByte first
+  static void s_vPutUINT32(unsigned char *p, unsigned long val); // Put unsigned long into 4 bytes LSByte first
 */
 static void s_vClear(void);                       // Clear the internal message,
-                                              // resets the object to the state just after construction.
+// resets the object to the state just after construction.
 static void s_vSetKey(unsigned long dwK0, unsigned long dwK1);
 static void s_vAppendByte(unsigned char b);            // Add a single byte to the internal message
 
@@ -66,116 +66,110 @@ static unsigned int nBytesInM;      // # bytes in M
 /*---------------------  Export Functions  --------------------------*/
 
 /*
-static unsigned long s_dwGetUINT32 (unsigned char *p)
+  static unsigned long s_dwGetUINT32 (unsigned char *p)
 // Convert from unsigned char [] to unsigned long in a portable way
 {
-    unsigned long res = 0;
-    unsigned int i;
-    for(i=0; i<4; i++ )
-    {
-        res |= (*p++) << (8*i);
-    }
-    return res;
+unsigned long res = 0;
+unsigned int i;
+for (i=0; i<4; i++)
+{
+	res |= (*p++) << (8 * i);
+}
+return res;
 }
 
 static void s_vPutUINT32 (unsigned char *p, unsigned long val)
 // Convert from unsigned long to unsigned char [] in a portable way
 {
-    unsigned int i;
-    for(i=0; i<4; i++ )
-    {
-        *p++ = (unsigned char) (val & 0xff);
-        val >>= 8;
-    }
+	unsigned int i;
+	for (i=0; i<4; i++) {
+		*p++ = (unsigned char) (val & 0xff);
+		val >>= 8;
+	}
 }
 */
 
-static void s_vClear (void)
+static void s_vClear(void)
 {
-    // Reset the state to the empty message.
-    L = K0;
-    R = K1;
-    nBytesInM = 0;
-    M = 0;
+	// Reset the state to the empty message.
+	L = K0;
+	R = K1;
+	nBytesInM = 0;
+	M = 0;
 }
 
-static void s_vSetKey (unsigned long dwK0, unsigned long dwK1)
+static void s_vSetKey(unsigned long dwK0, unsigned long dwK1)
 {
-    // Set the key
-    K0 = dwK0;
-    K1 = dwK1;
-    // and reset the message
-    s_vClear();
+	// Set the key
+	K0 = dwK0;
+	K1 = dwK1;
+	// and reset the message
+	s_vClear();
 }
 
-static void s_vAppendByte (unsigned char b)
+static void s_vAppendByte(unsigned char b)
 {
-    // Append the byte to our word-sized buffer
-    M |= b << (8*nBytesInM);
-    nBytesInM++;
-    // Process the word if it is full.
-    if( nBytesInM >= 4 )
-    {
-        L ^= M;
-        R ^= ROL32( L, 17 );
-        L += R;
-        R ^= ((L & 0xff00ff00) >> 8) | ((L & 0x00ff00ff) << 8);
-        L += R;
-        R ^= ROL32( L, 3 );
-        L += R;
-        R ^= ROR32( L, 2 );
-        L += R;
-        // Clear the buffer
-        M = 0;
-        nBytesInM = 0;
-    }
+	// Append the byte to our word-sized buffer
+	M |= b << (8*nBytesInM);
+	nBytesInM++;
+	// Process the word if it is full.
+	if (nBytesInM >= 4) {
+		L ^= M;
+		R ^= ROL32(L, 17);
+		L += R;
+		R ^= ((L & 0xff00ff00) >> 8) | ((L & 0x00ff00ff) << 8);
+		L += R;
+		R ^= ROL32(L, 3);
+		L += R;
+		R ^= ROR32(L, 2);
+		L += R;
+		// Clear the buffer
+		M = 0;
+		nBytesInM = 0;
+	}
 }
 
-void MIC_vInit (unsigned long dwK0, unsigned long dwK1)
+void MIC_vInit(unsigned long dwK0, unsigned long dwK1)
 {
-    // Set the key
-    s_vSetKey(dwK0, dwK1);
+	// Set the key
+	s_vSetKey(dwK0, dwK1);
 }
 
-
-void MIC_vUnInit (void)
+void MIC_vUnInit(void)
 {
-    // Wipe the key material
-    K0 = 0;
-    K1 = 0;
+	// Wipe the key material
+	K0 = 0;
+	K1 = 0;
 
-    // And the other fields as well.
-    //Note that this sets (L,R) to (K0,K1) which is just fine.
-    s_vClear();
+	// And the other fields as well.
+	//Note that this sets (L,R) to (K0,K1) which is just fine.
+	s_vClear();
 }
 
-void MIC_vAppend (unsigned char *src, unsigned int nBytes)
+void MIC_vAppend(unsigned char *src, unsigned int nBytes)
 {
-    // This is simple
-    while (nBytes > 0)
-    {
-        s_vAppendByte(*src++);
-        nBytes--;
-    }
+	// This is simple
+	while (nBytes > 0) {
+		s_vAppendByte(*src++);
+		nBytes--;
+	}
 }
 
-void MIC_vGetMIC (unsigned long *pdwL, unsigned long *pdwR)
+void MIC_vGetMIC(unsigned long *pdwL, unsigned long *pdwR)
 {
-    // Append the minimum padding
-    s_vAppendByte(0x5a);
-    s_vAppendByte(0);
-    s_vAppendByte(0);
-    s_vAppendByte(0);
-    s_vAppendByte(0);
-    // and then zeroes until the length is a multiple of 4
-    while( nBytesInM != 0 )
-    {
-        s_vAppendByte(0);
-    }
-    // The s_vAppendByte function has already computed the result.
-    *pdwL = L;
-    *pdwR = R;
-    // Reset to the empty message.
-    s_vClear();
+	// Append the minimum padding
+	s_vAppendByte(0x5a);
+	s_vAppendByte(0);
+	s_vAppendByte(0);
+	s_vAppendByte(0);
+	s_vAppendByte(0);
+	// and then zeroes until the length is a multiple of 4
+	while (nBytesInM != 0) {
+		s_vAppendByte(0);
+	}
+	// The s_vAppendByte function has already computed the result.
+	*pdwL = L;
+	*pdwR = R;
+	// Reset to the empty message.
+	s_vClear();
 }
-
diff --git a/drivers/staging/vt6655/michael.h b/drivers/staging/vt6655/michael.h
index 3131b161d6a..0828d18ad52 100644
--- a/drivers/staging/vt6655/michael.h
+++ b/drivers/staging/vt6655/michael.h
@@ -49,10 +49,8 @@ void MIC_vGetMIC(unsigned long *pdwL, unsigned long *pdwR);
 /*---------------------  Export Macros ------------------------------*/
 
 // Rotation functions on 32 bit values
-#define ROL32( A, n ) \
- ( ((A) << (n)) | ( ((A)>>(32-(n)))  & ( (1UL << (n)) - 1 ) ) )
-#define ROR32( A, n ) ROL32( (A), 32-(n) )
+#define ROL32(A, n)							\
+	(((A) << (n)) | (((A)>>(32-(n)))  & ((1UL << (n)) - 1)))
+#define ROR32(A, n) ROL32((A), 32-(n))
 
 #endif //__MICHAEL_H__
-
-
diff --git a/drivers/staging/vt6655/power.c b/drivers/staging/vt6655/power.c
index 661d534304c..2340d2f0399 100644
--- a/drivers/staging/vt6655/power.c
+++ b/drivers/staging/vt6655/power.c
@@ -48,19 +48,14 @@
 
 /*---------------------  Static Definitions -------------------------*/
 
-
-
-
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
-static int          msglevel                =MSG_LEVEL_INFO;
+static int msglevel = MSG_LEVEL_INFO;
 /*---------------------  Static Functions  --------------------------*/
 
-
 /*---------------------  Export Variables  --------------------------*/
 
-
 /*---------------------  Export Functions  --------------------------*/
 
 /*+
@@ -71,69 +66,62 @@ static int          msglevel                =MSG_LEVEL_INFO;
  * Return Value:
  *    None.
  *
--*/
-
+ -*/
 
 void
 PSvEnablePowerSaving(
-    void *hDeviceContext,
-    unsigned short wListenInterval
-    )
+	void *hDeviceContext,
+	unsigned short wListenInterval
+)
 {
-    PSDevice        pDevice = (PSDevice)hDeviceContext;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    unsigned short wAID = pMgmt->wCurrAID | BIT14 | BIT15;
-
-    // set period of power up before TBTT
-    VNSvOutPortW(pDevice->PortOffset + MAC_REG_PWBT, C_PWBT);
-    if (pDevice->eOPMode != OP_MODE_ADHOC) {
-        // set AID
-        VNSvOutPortW(pDevice->PortOffset + MAC_REG_AIDATIM, wAID);
-    } else {
-    	// set ATIM Window
-        MACvWriteATIMW(pDevice->PortOffset, pMgmt->wCurrATIMWindow);
-    }
-    // Set AutoSleep
-    MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCFG, PSCFG_AUTOSLEEP);
-    // Set HWUTSF
-    MACvRegBitsOn(pDevice->PortOffset, MAC_REG_TFTCTL, TFTCTL_HWUTSF);
-
-    if (wListenInterval >= 2) {
-        // clear always listen beacon
-        MACvRegBitsOff(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_ALBCN);
-        //pDevice->wCFG &= ~CFG_ALB;
-        // first time set listen next beacon
-        MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_LNBCN);
-        pMgmt->wCountToWakeUp = wListenInterval;
-    }
-    else {
-        // always listen beacon
-        MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_ALBCN);
-        //pDevice->wCFG |= CFG_ALB;
-        pMgmt->wCountToWakeUp = 0;
-    }
-
-    // enable power saving hw function
-    MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PSEN);
-    pDevice->bEnablePSMode = true;
-
-    if (pDevice->eOPMode == OP_MODE_ADHOC) {
+	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	unsigned short wAID = pMgmt->wCurrAID | BIT14 | BIT15;
+
+	// set period of power up before TBTT
+	VNSvOutPortW(pDevice->PortOffset + MAC_REG_PWBT, C_PWBT);
+	if (pDevice->eOPMode != OP_MODE_ADHOC) {
+		// set AID
+		VNSvOutPortW(pDevice->PortOffset + MAC_REG_AIDATIM, wAID);
+	} else {
+		// set ATIM Window
+		MACvWriteATIMW(pDevice->PortOffset, pMgmt->wCurrATIMWindow);
+	}
+	// Set AutoSleep
+	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCFG, PSCFG_AUTOSLEEP);
+	// Set HWUTSF
+	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_TFTCTL, TFTCTL_HWUTSF);
+
+	if (wListenInterval >= 2) {
+		// clear always listen beacon
+		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_ALBCN);
+		//pDevice->wCFG &= ~CFG_ALB;
+		// first time set listen next beacon
+		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_LNBCN);
+		pMgmt->wCountToWakeUp = wListenInterval;
+	} else {
+		// always listen beacon
+		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_ALBCN);
+		//pDevice->wCFG |= CFG_ALB;
+		pMgmt->wCountToWakeUp = 0;
+	}
+
+	// enable power saving hw function
+	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PSEN);
+	pDevice->bEnablePSMode = true;
+
+	if (pDevice->eOPMode == OP_MODE_ADHOC) {
 //        bMgrPrepareBeaconToSend((void *)pDevice, pMgmt);
-    }
-    // We don't send null pkt in ad hoc mode since beacon will handle this.
-    else if (pDevice->eOPMode == OP_MODE_INFRASTRUCTURE) {
-        PSbSendNullPacket(pDevice);
-    }
-    pDevice->bPWBitOn = true;
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "PS:Power Saving Mode Enable... \n");
-    return;
+	}
+	// We don't send null pkt in ad hoc mode since beacon will handle this.
+	else if (pDevice->eOPMode == OP_MODE_INFRASTRUCTURE) {
+		PSbSendNullPacket(pDevice);
+	}
+	pDevice->bPWBitOn = true;
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "PS:Power Saving Mode Enable... \n");
+	return;
 }
 
-
-
-
-
-
 /*+
  *
  * Routine Description:
@@ -142,35 +130,34 @@ PSvEnablePowerSaving(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 void
 PSvDisablePowerSaving(
-    void *hDeviceContext
-    )
+	void *hDeviceContext
+)
 {
-    PSDevice        pDevice = (PSDevice)hDeviceContext;
+	PSDevice        pDevice = (PSDevice)hDeviceContext;
 //    PSMgmtObject    pMgmt = pDevice->pMgmt;
 
-    // disable power saving hw function
-    MACbPSWakeup(pDevice->PortOffset);
-    //clear AutoSleep
-    MACvRegBitsOff(pDevice->PortOffset, MAC_REG_PSCFG, PSCFG_AUTOSLEEP);
-    //clear HWUTSF
-    MACvRegBitsOff(pDevice->PortOffset, MAC_REG_TFTCTL, TFTCTL_HWUTSF);
-    // set always listen beacon
-    MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_ALBCN);
-
-    pDevice->bEnablePSMode = false;
-
-    if (pDevice->eOPMode == OP_MODE_INFRASTRUCTURE) {
-        PSbSendNullPacket(pDevice);
-    }
-    pDevice->bPWBitOn = false;
-    return;
+	// disable power saving hw function
+	MACbPSWakeup(pDevice->PortOffset);
+	//clear AutoSleep
+	MACvRegBitsOff(pDevice->PortOffset, MAC_REG_PSCFG, PSCFG_AUTOSLEEP);
+	//clear HWUTSF
+	MACvRegBitsOff(pDevice->PortOffset, MAC_REG_TFTCTL, TFTCTL_HWUTSF);
+	// set always listen beacon
+	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_ALBCN);
+
+	pDevice->bEnablePSMode = false;
+
+	if (pDevice->eOPMode == OP_MODE_INFRASTRUCTURE) {
+		PSbSendNullPacket(pDevice);
+	}
+	pDevice->bPWBitOn = false;
+	return;
 }
 
-
 /*+
  *
  * Routine Description:
@@ -179,65 +166,62 @@ PSvDisablePowerSaving(
  * Return Value:
  *    true, if power down success
  *    false, if fail
--*/
-
+ -*/
 
 bool
 PSbConsiderPowerDown(
-    void *hDeviceContext,
-    bool bCheckRxDMA,
-    bool bCheckCountToWakeUp
-    )
+	void *hDeviceContext,
+	bool bCheckRxDMA,
+	bool bCheckCountToWakeUp
+)
 {
-    PSDevice        pDevice = (PSDevice)hDeviceContext;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    unsigned int uIdx;
-
-    // check if already in Doze mode
-    if (MACbIsRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PS))
-        return true;
-
-    if (pMgmt->eCurrMode != WMAC_MODE_IBSS_STA) {
-        // check if in TIM wake period
-        if (pMgmt->bInTIMWake)
-            return false;
-    }
-
-    // check scan state
-    if (pDevice->bCmdRunning)
-        return false;
-
-    // Force PSEN on
-    MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PSEN);
-
-    // check if all TD are empty,
-    for (uIdx = 0; uIdx < TYPE_MAXTD; uIdx ++) {
-        if (pDevice->iTDUsed[uIdx] != 0)
-            return false;
-    }
-
-    // check if rx isr is clear
-    if (bCheckRxDMA &&
-        ((pDevice->dwIsr& ISR_RXDMA0) != 0) &&
-        ((pDevice->dwIsr & ISR_RXDMA1) != 0)){
-        return false;
-    }
-
-    if (pMgmt->eCurrMode != WMAC_MODE_IBSS_STA) {
-        if (bCheckCountToWakeUp &&
-           (pMgmt->wCountToWakeUp == 0 || pMgmt->wCountToWakeUp == 1)) {
-             return false;
-        }
-    }
-
-    // no Tx, no Rx isr, now go to Doze
-    MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_GO2DOZE);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Go to Doze ZZZZZZZZZZZZZZZ\n");
-    return true;
+	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	unsigned int uIdx;
+
+	// check if already in Doze mode
+	if (MACbIsRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PS))
+		return true;
+
+	if (pMgmt->eCurrMode != WMAC_MODE_IBSS_STA) {
+		// check if in TIM wake period
+		if (pMgmt->bInTIMWake)
+			return false;
+	}
+
+	// check scan state
+	if (pDevice->bCmdRunning)
+		return false;
+
+	// Force PSEN on
+	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PSEN);
+
+	// check if all TD are empty,
+	for (uIdx = 0; uIdx < TYPE_MAXTD; uIdx++) {
+		if (pDevice->iTDUsed[uIdx] != 0)
+			return false;
+	}
+
+	// check if rx isr is clear
+	if (bCheckRxDMA &&
+	    ((pDevice->dwIsr & ISR_RXDMA0) != 0) &&
+	    ((pDevice->dwIsr & ISR_RXDMA1) != 0)) {
+		return false;
+	}
+
+	if (pMgmt->eCurrMode != WMAC_MODE_IBSS_STA) {
+		if (bCheckCountToWakeUp &&
+		    (pMgmt->wCountToWakeUp == 0 || pMgmt->wCountToWakeUp == 1)) {
+			return false;
+		}
+	}
+
+	// no Tx, no Rx isr, now go to Doze
+	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_GO2DOZE);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Go to Doze ZZZZZZZZZZZZZZZ\n");
+	return true;
 }
 
-
-
 /*+
  *
  * Routine Description:
@@ -246,47 +230,41 @@ PSbConsiderPowerDown(
  * Return Value:
  *    None.
  *
--*/
-
-
+ -*/
 
 void
 PSvSendPSPOLL(
-    void *hDeviceContext
-    )
+	void *hDeviceContext
+)
 {
-    PSDevice            pDevice = (PSDevice)hDeviceContext;
-    PSMgmtObject        pMgmt = pDevice->pMgmt;
-    PSTxMgmtPacket      pTxPacket = NULL;
-
-
-    memset(pMgmt->pbyPSPacketPool, 0, sizeof(STxMgmtPacket) + WLAN_HDR_ADDR2_LEN);
-    pTxPacket = (PSTxMgmtPacket)pMgmt->pbyPSPacketPool;
-    pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-    pTxPacket->p80211Header->sA2.wFrameCtl = cpu_to_le16(
-         (
-         WLAN_SET_FC_FTYPE(WLAN_TYPE_CTL) |
-         WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_PSPOLL) |
-         WLAN_SET_FC_PWRMGT(0)
-         ));
-    pTxPacket->p80211Header->sA2.wDurationID = pMgmt->wCurrAID | BIT14 | BIT15;
-    memcpy(pTxPacket->p80211Header->sA2.abyAddr1, pMgmt->abyCurrBSSID, WLAN_ADDR_LEN);
-    memcpy(pTxPacket->p80211Header->sA2.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-    pTxPacket->cbMPDULen = WLAN_HDR_ADDR2_LEN;
-    pTxPacket->cbPayloadLen = 0;
-    // send the frame
-    if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Send PS-Poll packet failed..\n");
-    }
-    else {
+	PSDevice            pDevice = (PSDevice)hDeviceContext;
+	PSMgmtObject        pMgmt = pDevice->pMgmt;
+	PSTxMgmtPacket      pTxPacket = NULL;
+
+	memset(pMgmt->pbyPSPacketPool, 0, sizeof(STxMgmtPacket) + WLAN_HDR_ADDR2_LEN);
+	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyPSPacketPool;
+	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
+	pTxPacket->p80211Header->sA2.wFrameCtl = cpu_to_le16(
+		(
+			WLAN_SET_FC_FTYPE(WLAN_TYPE_CTL) |
+			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_PSPOLL) |
+			WLAN_SET_FC_PWRMGT(0)
+));
+	pTxPacket->p80211Header->sA2.wDurationID = pMgmt->wCurrAID | BIT14 | BIT15;
+	memcpy(pTxPacket->p80211Header->sA2.abyAddr1, pMgmt->abyCurrBSSID, WLAN_ADDR_LEN);
+	memcpy(pTxPacket->p80211Header->sA2.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
+	pTxPacket->cbMPDULen = WLAN_HDR_ADDR2_LEN;
+	pTxPacket->cbPayloadLen = 0;
+	// send the frame
+	if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Send PS-Poll packet failed..\n");
+	} else {
 //        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Send PS-Poll packet success..\n");
-    };
+	};
 
-    return;
+	return;
 }
 
-
-
 /*+
  *
  * Routine Description:
@@ -295,81 +273,75 @@ PSvSendPSPOLL(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 bool
 PSbSendNullPacket(
-    void *hDeviceContext
-    )
+	void *hDeviceContext
+)
 {
-    PSDevice            pDevice = (PSDevice)hDeviceContext;
-    PSTxMgmtPacket      pTxPacket = NULL;
-    PSMgmtObject        pMgmt = pDevice->pMgmt;
-    unsigned int uIdx;
-
-
-    if (pDevice->bLinkPass == false) {
-        return false;
-    }
-    #ifdef TxInSleep
-     if ((pDevice->bEnablePSMode == false) &&
-	  (pDevice->fTxDataInSleep == false)){
-        return false;
-    }
+	PSDevice            pDevice = (PSDevice)hDeviceContext;
+	PSTxMgmtPacket      pTxPacket = NULL;
+	PSMgmtObject        pMgmt = pDevice->pMgmt;
+	unsigned int uIdx;
+
+	if (pDevice->bLinkPass == false) {
+		return false;
+	}
+#ifdef TxInSleep
+	if ((pDevice->bEnablePSMode == false) &&
+	    (pDevice->fTxDataInSleep == false)) {
+		return false;
+	}
 #else
-    if (pDevice->bEnablePSMode == false) {
-        return false;
-    }
+	if (pDevice->bEnablePSMode == false) {
+		return false;
+	}
 #endif
-    if (pDevice->bEnablePSMode) {
-        for (uIdx = 0; uIdx < TYPE_MAXTD; uIdx ++) {
-            if (pDevice->iTDUsed[uIdx] != 0)
-                return false;
-        }
-    }
-
-    memset(pMgmt->pbyPSPacketPool, 0, sizeof(STxMgmtPacket) + WLAN_NULLDATA_FR_MAXLEN);
-    pTxPacket = (PSTxMgmtPacket)pMgmt->pbyPSPacketPool;
-    pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-
-    if (pDevice->bEnablePSMode) {
-
-        pTxPacket->p80211Header->sA3.wFrameCtl = cpu_to_le16(
-             (
-            WLAN_SET_FC_FTYPE(WLAN_TYPE_DATA) |
-            WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_NULL) |
-            WLAN_SET_FC_PWRMGT(1)
-            ));
-    }
-    else {
-        pTxPacket->p80211Header->sA3.wFrameCtl = cpu_to_le16(
-             (
-            WLAN_SET_FC_FTYPE(WLAN_TYPE_DATA) |
-            WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_NULL) |
-            WLAN_SET_FC_PWRMGT(0)
-            ));
-    }
-
-    if(pMgmt->eCurrMode != WMAC_MODE_IBSS_STA) {
-        pTxPacket->p80211Header->sA3.wFrameCtl |= cpu_to_le16((unsigned short)WLAN_SET_FC_TODS(1));
-    }
-
-    memcpy(pTxPacket->p80211Header->sA3.abyAddr1, pMgmt->abyCurrBSSID, WLAN_ADDR_LEN);
-    memcpy(pTxPacket->p80211Header->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-    memcpy(pTxPacket->p80211Header->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-    pTxPacket->cbMPDULen = WLAN_HDR_ADDR3_LEN;
-    pTxPacket->cbPayloadLen = 0;
-    // send the frame
-    if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Send Null Packet failed !\n");
-        return false;
-    }
-    else {
-
+	if (pDevice->bEnablePSMode) {
+		for (uIdx = 0; uIdx < TYPE_MAXTD; uIdx++) {
+			if (pDevice->iTDUsed[uIdx] != 0)
+				return false;
+		}
+	}
+
+	memset(pMgmt->pbyPSPacketPool, 0, sizeof(STxMgmtPacket) + WLAN_NULLDATA_FR_MAXLEN);
+	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyPSPacketPool;
+	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
+
+	if (pDevice->bEnablePSMode) {
+		pTxPacket->p80211Header->sA3.wFrameCtl = cpu_to_le16(
+			(
+				WLAN_SET_FC_FTYPE(WLAN_TYPE_DATA) |
+				WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_NULL) |
+				WLAN_SET_FC_PWRMGT(1)
+));
+	} else {
+		pTxPacket->p80211Header->sA3.wFrameCtl = cpu_to_le16(
+			(
+				WLAN_SET_FC_FTYPE(WLAN_TYPE_DATA) |
+				WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_NULL) |
+				WLAN_SET_FC_PWRMGT(0)
+));
+	}
+
+	if (pMgmt->eCurrMode != WMAC_MODE_IBSS_STA) {
+		pTxPacket->p80211Header->sA3.wFrameCtl |= cpu_to_le16((unsigned short)WLAN_SET_FC_TODS(1));
+	}
+
+	memcpy(pTxPacket->p80211Header->sA3.abyAddr1, pMgmt->abyCurrBSSID, WLAN_ADDR_LEN);
+	memcpy(pTxPacket->p80211Header->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
+	memcpy(pTxPacket->p80211Header->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
+	pTxPacket->cbMPDULen = WLAN_HDR_ADDR3_LEN;
+	pTxPacket->cbPayloadLen = 0;
+	// send the frame
+	if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Send Null Packet failed !\n");
+		return false;
+	} else {
 //            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Send Null Packet success....\n");
-    }
-
+	}
 
-    return true ;
+	return true;
 }
 
 /*+
@@ -380,33 +352,31 @@ PSbSendNullPacket(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 bool
 PSbIsNextTBTTWakeUp(
-    void *hDeviceContext
-    )
+	void *hDeviceContext
+)
 {
+	PSDevice         pDevice = (PSDevice)hDeviceContext;
+	PSMgmtObject        pMgmt = pDevice->pMgmt;
+	bool bWakeUp = false;
 
-    PSDevice         pDevice = (PSDevice)hDeviceContext;
-    PSMgmtObject        pMgmt = pDevice->pMgmt;
-    bool bWakeUp = false;
+	if (pMgmt->wListenInterval >= 2) {
+		if (pMgmt->wCountToWakeUp == 0) {
+			pMgmt->wCountToWakeUp = pMgmt->wListenInterval;
+		}
 
-    if (pMgmt->wListenInterval >= 2) {
-        if (pMgmt->wCountToWakeUp == 0) {
-            pMgmt->wCountToWakeUp = pMgmt->wListenInterval;
-        }
+		pMgmt->wCountToWakeUp--;
 
-        pMgmt->wCountToWakeUp --;
+		if (pMgmt->wCountToWakeUp == 1) {
+			// Turn on wake up to listen next beacon
+			MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_LNBCN);
+			bWakeUp = true;
+		}
 
-        if (pMgmt->wCountToWakeUp == 1) {
-            // Turn on wake up to listen next beacon
-            MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_LNBCN);
-            bWakeUp = true;
-        }
+	}
 
-    }
-
-    return bWakeUp;
+	return bWakeUp;
 }
-
diff --git a/drivers/staging/vt6655/power.h b/drivers/staging/vt6655/power.h
index 01013b59228..337dd659f12 100644
--- a/drivers/staging/vt6655/power.h
+++ b/drivers/staging/vt6655/power.h
@@ -29,7 +29,6 @@
 #ifndef __POWER_H__
 #define __POWER_H__
 
-
 /*---------------------  Export Definitions -------------------------*/
 #define     C_PWBT                   1000      // micro sec. power up before TBTT
 #define     PS_FAST_INTERVAL         1         // Fast power saving listen interval
@@ -39,10 +38,8 @@
 
 /*---------------------  Export Variables  --------------------------*/
 
-
 /*---------------------  Export Types  ------------------------------*/
 
-
 /*---------------------  Export Functions  --------------------------*/
 
 // PSDevice pDevice
@@ -50,35 +47,35 @@
 
 bool
 PSbConsiderPowerDown(
-    void *hDeviceContext,
-    bool bCheckRxDMA,
-    bool bCheckCountToWakeUp
-    );
+	void *hDeviceContext,
+	bool bCheckRxDMA,
+	bool bCheckCountToWakeUp
+);
 
 void
 PSvDisablePowerSaving(
-    void *hDeviceContext
-    );
+	void *hDeviceContext
+);
 
 void
 PSvEnablePowerSaving(
-    void *hDeviceContext,
-    unsigned short wListenInterval
-    );
+	void *hDeviceContext,
+	unsigned short wListenInterval
+);
 
 void
 PSvSendPSPOLL(
-    void *hDeviceContext
-    );
+	void *hDeviceContext
+);
 
 bool
 PSbSendNullPacket(
-    void *hDeviceContext
-    );
+	void *hDeviceContext
+);
 
 bool
 PSbIsNextTBTTWakeUp(
-    void *hDeviceContext
-    );
+	void *hDeviceContext
+);
 
 #endif //__POWER_H__
diff --git a/drivers/staging/vt6655/rc4.c b/drivers/staging/vt6655/rc4.c
index 9856c08b3d7..343b815c0a9 100644
--- a/drivers/staging/vt6655/rc4.c
+++ b/drivers/staging/vt6655/rc4.c
@@ -34,54 +34,54 @@
 
 void rc4_init(PRC4Ext pRC4, unsigned char *pbyKey, unsigned int cbKey_len)
 {
-    unsigned int ust1, ust2;
-    unsigned int keyindex;
-    unsigned int stateindex;
-    unsigned char *pbyst;
-    unsigned int idx;
+	unsigned int ust1, ust2;
+	unsigned int keyindex;
+	unsigned int stateindex;
+	unsigned char *pbyst;
+	unsigned int idx;
 
-    pbyst = pRC4->abystate;
-    pRC4->ux = 0;
-    pRC4->uy = 0;
-    for (idx = 0; idx < 256; idx++)
-        pbyst[idx] = (unsigned char)idx;
-    keyindex = 0;
-    stateindex = 0;
-    for (idx = 0; idx < 256; idx++) {
-        ust1 = pbyst[idx];
-        stateindex = (stateindex + pbyKey[keyindex] + ust1) & 0xff;
-        ust2 = pbyst[stateindex];
-        pbyst[stateindex] = (unsigned char)ust1;
-        pbyst[idx] = (unsigned char)ust2;
-        if (++keyindex >= cbKey_len)
-            keyindex = 0;
-    }
+	pbyst = pRC4->abystate;
+	pRC4->ux = 0;
+	pRC4->uy = 0;
+	for (idx = 0; idx < 256; idx++)
+		pbyst[idx] = (unsigned char)idx;
+	keyindex = 0;
+	stateindex = 0;
+	for (idx = 0; idx < 256; idx++) {
+		ust1 = pbyst[idx];
+		stateindex = (stateindex + pbyKey[keyindex] + ust1) & 0xff;
+		ust2 = pbyst[stateindex];
+		pbyst[stateindex] = (unsigned char)ust1;
+		pbyst[idx] = (unsigned char)ust2;
+		if (++keyindex >= cbKey_len)
+			keyindex = 0;
+	}
 }
 
 unsigned int rc4_byte(PRC4Ext pRC4)
 {
-    unsigned int ux;
-    unsigned int uy;
-    unsigned int ustx, usty;
-    unsigned char *pbyst;
+	unsigned int ux;
+	unsigned int uy;
+	unsigned int ustx, usty;
+	unsigned char *pbyst;
 
-    pbyst = pRC4->abystate;
-    ux = (pRC4->ux + 1) & 0xff;
-    ustx = pbyst[ux];
-    uy = (ustx + pRC4->uy) & 0xff;
-    usty = pbyst[uy];
-    pRC4->ux = ux;
-    pRC4->uy = uy;
-    pbyst[uy] = (unsigned char)ustx;
-    pbyst[ux] = (unsigned char)usty;
+	pbyst = pRC4->abystate;
+	ux = (pRC4->ux + 1) & 0xff;
+	ustx = pbyst[ux];
+	uy = (ustx + pRC4->uy) & 0xff;
+	usty = pbyst[uy];
+	pRC4->ux = ux;
+	pRC4->uy = uy;
+	pbyst[uy] = (unsigned char)ustx;
+	pbyst[ux] = (unsigned char)usty;
 
-    return pbyst[(ustx + usty) & 0xff];
+	return pbyst[(ustx + usty) & 0xff];
 }
 
 void rc4_encrypt(PRC4Ext pRC4, unsigned char *pbyDest,
-                     unsigned char *pbySrc, unsigned int cbData_len)
+		 unsigned char *pbySrc, unsigned int cbData_len)
 {
-    unsigned int ii;
-    for (ii = 0; ii < cbData_len; ii++)
-        pbyDest[ii] = (unsigned char)(pbySrc[ii] ^ rc4_byte(pRC4));
+	unsigned int ii;
+	for (ii = 0; ii < cbData_len; ii++)
+		pbyDest[ii] = (unsigned char)(pbySrc[ii] ^ rc4_byte(pRC4));
 }
diff --git a/drivers/staging/vt6655/rc4.h b/drivers/staging/vt6655/rc4.h
index ad04e351365..74b2eed9bce 100644
--- a/drivers/staging/vt6655/rc4.h
+++ b/drivers/staging/vt6655/rc4.h
@@ -35,9 +35,9 @@
 /*---------------------  Export Definitions -------------------------*/
 /*---------------------  Export Types  ------------------------------*/
 typedef struct {
-    unsigned int ux;
-    unsigned int uy;
-    unsigned char abystate[256];
+	unsigned int ux;
+	unsigned int uy;
+	unsigned char abystate[256];
 } RC4Ext, *PRC4Ext;
 
 void rc4_init(PRC4Ext pRC4, unsigned char *pbyKey, unsigned int cbKey_len);
diff --git a/drivers/staging/vt6655/rf.c b/drivers/staging/vt6655/rf.c
index aaa231aaf4e..6948984a25a 100644
--- a/drivers/staging/vt6655/rf.c
+++ b/drivers/staging/vt6655/rf.c
@@ -46,7 +46,6 @@
 #define SWITCH_CHANNEL_DELAY_AL2230 200 //us
 #define AL2230_PWR_IDX_LEN    64
 
-
 #define BY_AL7230_REG_LEN     23 //24bit
 #define CB_AL7230_INIT_SEQ    16
 #define SWITCH_CHANNEL_DELAY_AL7230 200 //us
@@ -56,376 +55,367 @@
 
 /*---------------------  Static Variables  --------------------------*/
 
-
-
 const unsigned long dwAL2230InitTable[CB_AL2230_INIT_SEQ] = {
-    0x03F79000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, //
-    0x03333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, //
-    0x01A00200+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, //
-    0x00FFF300+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, //
-    0x0005A400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, //
-    0x0F4DC500+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, //
-    0x0805B600+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, //
-    0x0146C700+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, //
-    0x00068800+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, //
-    0x0403B900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, //
-    0x00DBBA00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, //
-    0x00099B00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, //
-    0x0BDFFC00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x00000D00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x00580F00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW
-    };
+	0x03F79000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, //
+	0x03333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, //
+	0x01A00200+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, //
+	0x00FFF300+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, //
+	0x0005A400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, //
+	0x0F4DC500+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, //
+	0x0805B600+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, //
+	0x0146C700+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, //
+	0x00068800+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, //
+	0x0403B900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, //
+	0x00DBBA00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, //
+	0x00099B00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, //
+	0x0BDFFC00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x00000D00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x00580F00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW
+};
 
 const unsigned long dwAL2230ChannelTable0[CB_MAX_CHANNEL] = {
-    0x03F79000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 1, Tf = 2412MHz
-    0x03F79000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 2, Tf = 2417MHz
-    0x03E79000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 3, Tf = 2422MHz
-    0x03E79000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 4, Tf = 2427MHz
-    0x03F7A000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 5, Tf = 2432MHz
-    0x03F7A000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 6, Tf = 2437MHz
-    0x03E7A000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 7, Tf = 2442MHz
-    0x03E7A000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 8, Tf = 2447MHz
-    0x03F7B000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 9, Tf = 2452MHz
-    0x03F7B000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 10, Tf = 2457MHz
-    0x03E7B000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 11, Tf = 2462MHz
-    0x03E7B000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 12, Tf = 2467MHz
-    0x03F7C000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 13, Tf = 2472MHz
-    0x03E7C000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW  // channel = 14, Tf = 2412M
-    };
+	0x03F79000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 1, Tf = 2412MHz
+	0x03F79000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 2, Tf = 2417MHz
+	0x03E79000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 3, Tf = 2422MHz
+	0x03E79000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 4, Tf = 2427MHz
+	0x03F7A000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 5, Tf = 2432MHz
+	0x03F7A000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 6, Tf = 2437MHz
+	0x03E7A000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 7, Tf = 2442MHz
+	0x03E7A000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 8, Tf = 2447MHz
+	0x03F7B000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 9, Tf = 2452MHz
+	0x03F7B000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 10, Tf = 2457MHz
+	0x03E7B000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 11, Tf = 2462MHz
+	0x03E7B000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 12, Tf = 2467MHz
+	0x03F7C000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 13, Tf = 2472MHz
+	0x03E7C000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW  // channel = 14, Tf = 2412M
+};
 
 const unsigned long dwAL2230ChannelTable1[CB_MAX_CHANNEL] = {
-    0x03333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 1, Tf = 2412MHz
-    0x0B333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 2, Tf = 2417MHz
-    0x03333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 3, Tf = 2422MHz
-    0x0B333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 4, Tf = 2427MHz
-    0x03333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 5, Tf = 2432MHz
-    0x0B333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 6, Tf = 2437MHz
-    0x03333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 7, Tf = 2442MHz
-    0x0B333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 8, Tf = 2447MHz
-    0x03333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 9, Tf = 2452MHz
-    0x0B333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 10, Tf = 2457MHz
-    0x03333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 11, Tf = 2462MHz
-    0x0B333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 12, Tf = 2467MHz
-    0x03333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 13, Tf = 2472MHz
-    0x06666100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW  // channel = 14, Tf = 2412M
-    };
+	0x03333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 1, Tf = 2412MHz
+	0x0B333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 2, Tf = 2417MHz
+	0x03333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 3, Tf = 2422MHz
+	0x0B333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 4, Tf = 2427MHz
+	0x03333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 5, Tf = 2432MHz
+	0x0B333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 6, Tf = 2437MHz
+	0x03333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 7, Tf = 2442MHz
+	0x0B333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 8, Tf = 2447MHz
+	0x03333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 9, Tf = 2452MHz
+	0x0B333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 10, Tf = 2457MHz
+	0x03333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 11, Tf = 2462MHz
+	0x0B333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 12, Tf = 2467MHz
+	0x03333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 13, Tf = 2472MHz
+	0x06666100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW  // channel = 14, Tf = 2412M
+};
 
 unsigned long dwAL2230PowerTable[AL2230_PWR_IDX_LEN] = {
-    0x04040900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04041900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04042900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04043900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04044900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04045900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04046900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04047900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04048900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04049900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x0404A900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x0404B900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x0404C900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x0404D900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x0404E900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x0404F900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04050900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04051900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04052900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04053900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04054900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04055900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04056900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04057900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04058900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04059900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x0405A900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x0405B900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x0405C900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x0405D900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x0405E900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x0405F900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04060900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04061900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04062900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04063900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04064900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04065900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04066900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04067900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04068900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04069900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x0406A900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x0406B900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x0406C900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x0406D900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x0406E900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x0406F900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04070900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04071900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04072900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04073900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04074900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04075900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04076900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04077900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04078900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x04079900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x0407A900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x0407B900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x0407C900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x0407D900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x0407E900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x0407F900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW
-    };
+	0x04040900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04041900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04042900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04043900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04044900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04045900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04046900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04047900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04048900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04049900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x0404A900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x0404B900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x0404C900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x0404D900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x0404E900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x0404F900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04050900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04051900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04052900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04053900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04054900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04055900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04056900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04057900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04058900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04059900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x0405A900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x0405B900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x0405C900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x0405D900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x0405E900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x0405F900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04060900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04061900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04062900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04063900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04064900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04065900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04066900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04067900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04068900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04069900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x0406A900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x0406B900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x0406C900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x0406D900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x0406E900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x0406F900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04070900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04071900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04072900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04073900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04074900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04075900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04076900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04077900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04078900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x04079900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x0407A900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x0407B900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x0407C900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x0407D900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x0407E900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x0407F900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW
+};
 
 //{{ RobertYu:20050104
 // 40MHz reference frequency
 // Need to Pull PLLON(PE3) low when writing channel registers through 3-wire.
 const unsigned long dwAL7230InitTable[CB_AL7230_INIT_SEQ] = {
-    0x00379000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Channel1 // Need modify for 11a
-    0x13333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Channel1 // Need modify for 11a
-    0x841FF200+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11a: 451FE2
-    0x3FDFA300+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11a: 5FDFA3
-    0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // 11b/g    // Need modify for 11a
-    //0x802B4500+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11a: 8D1B45
-    // RoberYu:20050113, Rev0.47 Regsiter Setting Guide
-    0x802B5500+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11a: 8D1B55
-    0x56AF3600+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW,
-    0xCE020700+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11a: 860207
-    0x6EBC0800+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x221BB900+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW,
-    0xE0000A00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11a: E0600A
-    0x08031B00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // init 0x080B1B00 => 0x080F1B00 for 3 wire control TxGain(D10)
-    //0x00093C00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11a: 00143C
-    // RoberYu:20050113, Rev0.47 Regsiter Setting Guide
-    0x000A3C00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11a: 00143C
-    0xFFFFFD00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x00000E00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x1ABA8F00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW  // Need modify for 11a: 12BACF
-    };
+	0x00379000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Channel1 // Need modify for 11a
+	0x13333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Channel1 // Need modify for 11a
+	0x841FF200+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11a: 451FE2
+	0x3FDFA300+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11a: 5FDFA3
+	0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // 11b/g    // Need modify for 11a
+	//0x802B4500+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11a: 8D1B45
+	// RoberYu:20050113, Rev0.47 Regsiter Setting Guide
+	0x802B5500+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11a: 8D1B55
+	0x56AF3600+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW,
+	0xCE020700+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11a: 860207
+	0x6EBC0800+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x221BB900+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW,
+	0xE0000A00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11a: E0600A
+	0x08031B00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // init 0x080B1B00 => 0x080F1B00 for 3 wire control TxGain(D10)
+	//0x00093C00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11a: 00143C
+	// RoberYu:20050113, Rev0.47 Regsiter Setting Guide
+	0x000A3C00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11a: 00143C
+	0xFFFFFD00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x00000E00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x1ABA8F00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW  // Need modify for 11a: 12BACF
+};
 
 const unsigned long dwAL7230InitTableAMode[CB_AL7230_INIT_SEQ] = {
-    0x0FF52000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Channel184 // Need modify for 11b/g
-    0x00000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Channel184 // Need modify for 11b/g
-    0x451FE200+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11b/g
-    0x5FDFA300+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11b/g
-    0x67F78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // 11a    // Need modify for 11b/g
-    0x853F5500+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11b/g, RoberYu:20050113
-    0x56AF3600+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW,
-    0xCE020700+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11b/g
-    0x6EBC0800+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x221BB900+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW,
-    0xE0600A00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11b/g
-    0x08031B00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // init 0x080B1B00 => 0x080F1B00 for 3 wire control TxGain(D10)
-    0x00147C00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11b/g
-    0xFFFFFD00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x00000E00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW,
-    0x12BACF00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW  // Need modify for 11b/g
-    };
-
+	0x0FF52000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Channel184 // Need modify for 11b/g
+	0x00000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Channel184 // Need modify for 11b/g
+	0x451FE200+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11b/g
+	0x5FDFA300+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11b/g
+	0x67F78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // 11a    // Need modify for 11b/g
+	0x853F5500+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11b/g, RoberYu:20050113
+	0x56AF3600+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW,
+	0xCE020700+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11b/g
+	0x6EBC0800+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x221BB900+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW,
+	0xE0600A00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11b/g
+	0x08031B00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // init 0x080B1B00 => 0x080F1B00 for 3 wire control TxGain(D10)
+	0x00147C00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11b/g
+	0xFFFFFD00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x00000E00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW,
+	0x12BACF00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW  // Need modify for 11b/g
+};
 
 const unsigned long dwAL7230ChannelTable0[CB_MAX_CHANNEL] = {
-    0x00379000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  1, Tf = 2412MHz
-    0x00379000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  2, Tf = 2417MHz
-    0x00379000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  3, Tf = 2422MHz
-    0x00379000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  4, Tf = 2427MHz
-    0x0037A000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  5, Tf = 2432MHz
-    0x0037A000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  6, Tf = 2437MHz
-    0x0037A000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  7, Tf = 2442MHz
-    0x0037A000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  8, Tf = 2447MHz //RobertYu: 20050218, update for APNode 0.49
-    0x0037B000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  9, Tf = 2452MHz //RobertYu: 20050218, update for APNode 0.49
-    0x0037B000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 10, Tf = 2457MHz //RobertYu: 20050218, update for APNode 0.49
-    0x0037B000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 11, Tf = 2462MHz //RobertYu: 20050218, update for APNode 0.49
-    0x0037B000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 12, Tf = 2467MHz //RobertYu: 20050218, update for APNode 0.49
-    0x0037C000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 13, Tf = 2472MHz //RobertYu: 20050218, update for APNode 0.49
-    0x0037C000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 14, Tf = 2484MHz
-
-    // 4.9G => Ch 183, 184, 185, 187, 188, 189, 192, 196  (Value:15 ~ 22)
-    0x0FF52000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 183, Tf = 4915MHz (15)
-    0x0FF52000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 184, Tf = 4920MHz (16)
-    0x0FF52000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 185, Tf = 4925MHz (17)
-    0x0FF52000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 187, Tf = 4935MHz (18)
-    0x0FF52000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 188, Tf = 4940MHz (19)
-    0x0FF52000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 189, Tf = 4945MHz (20)
-    0x0FF53000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 192, Tf = 4960MHz (21)
-    0x0FF53000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 196, Tf = 4980MHz (22)
-
-    // 5G => Ch 7, 8, 9, 11, 12, 16, 34, 36, 38, 40, 42, 44, 46, 48, 52, 56, 60, 64,
-    // 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165  (Value 23 ~ 56)
-
-    0x0FF54000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =   7, Tf = 5035MHz (23)
-    0x0FF54000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =   8, Tf = 5040MHz (24)
-    0x0FF54000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =   9, Tf = 5045MHz (25)
-    0x0FF54000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  11, Tf = 5055MHz (26)
-    0x0FF54000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  12, Tf = 5060MHz (27)
-    0x0FF55000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  16, Tf = 5080MHz (28)
-    0x0FF56000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  34, Tf = 5170MHz (29)
-    0x0FF56000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  36, Tf = 5180MHz (30)
-    0x0FF57000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  38, Tf = 5190MHz (31) //RobertYu: 20050218, update for APNode 0.49
-    0x0FF57000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  40, Tf = 5200MHz (32)
-    0x0FF57000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  42, Tf = 5210MHz (33)
-    0x0FF57000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  44, Tf = 5220MHz (34)
-    0x0FF57000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  46, Tf = 5230MHz (35)
-    0x0FF57000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  48, Tf = 5240MHz (36)
-    0x0FF58000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  52, Tf = 5260MHz (37)
-    0x0FF58000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  56, Tf = 5280MHz (38)
-    0x0FF58000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  60, Tf = 5300MHz (39)
-    0x0FF59000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  64, Tf = 5320MHz (40)
-
-    0x0FF5C000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 100, Tf = 5500MHz (41)
-    0x0FF5C000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 104, Tf = 5520MHz (42)
-    0x0FF5C000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 108, Tf = 5540MHz (43)
-    0x0FF5D000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 112, Tf = 5560MHz (44)
-    0x0FF5D000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 116, Tf = 5580MHz (45)
-    0x0FF5D000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 120, Tf = 5600MHz (46)
-    0x0FF5E000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 124, Tf = 5620MHz (47)
-    0x0FF5E000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 128, Tf = 5640MHz (48)
-    0x0FF5E000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 132, Tf = 5660MHz (49)
-    0x0FF5F000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 136, Tf = 5680MHz (50)
-    0x0FF5F000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 140, Tf = 5700MHz (51)
-    0x0FF60000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 149, Tf = 5745MHz (52)
-    0x0FF60000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 153, Tf = 5765MHz (53)
-    0x0FF60000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 157, Tf = 5785MHz (54)
-    0x0FF61000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 161, Tf = 5805MHz (55)
-    0x0FF61000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW  // channel = 165, Tf = 5825MHz (56)
-    };
+	0x00379000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  1, Tf = 2412MHz
+	0x00379000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  2, Tf = 2417MHz
+	0x00379000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  3, Tf = 2422MHz
+	0x00379000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  4, Tf = 2427MHz
+	0x0037A000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  5, Tf = 2432MHz
+	0x0037A000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  6, Tf = 2437MHz
+	0x0037A000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  7, Tf = 2442MHz
+	0x0037A000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  8, Tf = 2447MHz //RobertYu: 20050218, update for APNode 0.49
+	0x0037B000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  9, Tf = 2452MHz //RobertYu: 20050218, update for APNode 0.49
+	0x0037B000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 10, Tf = 2457MHz //RobertYu: 20050218, update for APNode 0.49
+	0x0037B000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 11, Tf = 2462MHz //RobertYu: 20050218, update for APNode 0.49
+	0x0037B000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 12, Tf = 2467MHz //RobertYu: 20050218, update for APNode 0.49
+	0x0037C000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 13, Tf = 2472MHz //RobertYu: 20050218, update for APNode 0.49
+	0x0037C000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 14, Tf = 2484MHz
+
+	// 4.9G => Ch 183, 184, 185, 187, 188, 189, 192, 196  (Value:15 ~ 22)
+	0x0FF52000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 183, Tf = 4915MHz (15)
+	0x0FF52000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 184, Tf = 4920MHz (16)
+	0x0FF52000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 185, Tf = 4925MHz (17)
+	0x0FF52000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 187, Tf = 4935MHz (18)
+	0x0FF52000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 188, Tf = 4940MHz (19)
+	0x0FF52000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 189, Tf = 4945MHz (20)
+	0x0FF53000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 192, Tf = 4960MHz (21)
+	0x0FF53000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 196, Tf = 4980MHz (22)
+
+	// 5G => Ch 7, 8, 9, 11, 12, 16, 34, 36, 38, 40, 42, 44, 46, 48, 52, 56, 60, 64,
+	// 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165  (Value 23 ~ 56)
+
+	0x0FF54000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =   7, Tf = 5035MHz (23)
+	0x0FF54000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =   8, Tf = 5040MHz (24)
+	0x0FF54000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =   9, Tf = 5045MHz (25)
+	0x0FF54000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  11, Tf = 5055MHz (26)
+	0x0FF54000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  12, Tf = 5060MHz (27)
+	0x0FF55000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  16, Tf = 5080MHz (28)
+	0x0FF56000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  34, Tf = 5170MHz (29)
+	0x0FF56000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  36, Tf = 5180MHz (30)
+	0x0FF57000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  38, Tf = 5190MHz (31) //RobertYu: 20050218, update for APNode 0.49
+	0x0FF57000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  40, Tf = 5200MHz (32)
+	0x0FF57000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  42, Tf = 5210MHz (33)
+	0x0FF57000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  44, Tf = 5220MHz (34)
+	0x0FF57000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  46, Tf = 5230MHz (35)
+	0x0FF57000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  48, Tf = 5240MHz (36)
+	0x0FF58000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  52, Tf = 5260MHz (37)
+	0x0FF58000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  56, Tf = 5280MHz (38)
+	0x0FF58000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  60, Tf = 5300MHz (39)
+	0x0FF59000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  64, Tf = 5320MHz (40)
+
+	0x0FF5C000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 100, Tf = 5500MHz (41)
+	0x0FF5C000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 104, Tf = 5520MHz (42)
+	0x0FF5C000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 108, Tf = 5540MHz (43)
+	0x0FF5D000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 112, Tf = 5560MHz (44)
+	0x0FF5D000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 116, Tf = 5580MHz (45)
+	0x0FF5D000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 120, Tf = 5600MHz (46)
+	0x0FF5E000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 124, Tf = 5620MHz (47)
+	0x0FF5E000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 128, Tf = 5640MHz (48)
+	0x0FF5E000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 132, Tf = 5660MHz (49)
+	0x0FF5F000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 136, Tf = 5680MHz (50)
+	0x0FF5F000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 140, Tf = 5700MHz (51)
+	0x0FF60000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 149, Tf = 5745MHz (52)
+	0x0FF60000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 153, Tf = 5765MHz (53)
+	0x0FF60000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 157, Tf = 5785MHz (54)
+	0x0FF61000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 161, Tf = 5805MHz (55)
+	0x0FF61000+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW  // channel = 165, Tf = 5825MHz (56)
+};
 
 const unsigned long dwAL7230ChannelTable1[CB_MAX_CHANNEL] = {
-    0x13333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  1, Tf = 2412MHz
-    0x1B333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  2, Tf = 2417MHz
-    0x03333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  3, Tf = 2422MHz
-    0x0B333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  4, Tf = 2427MHz
-    0x13333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  5, Tf = 2432MHz
-    0x1B333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  6, Tf = 2437MHz
-    0x03333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  7, Tf = 2442MHz
-    0x0B333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  8, Tf = 2447MHz
-    0x13333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  9, Tf = 2452MHz
-    0x1B333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 10, Tf = 2457MHz
-    0x03333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 11, Tf = 2462MHz
-    0x0B333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 12, Tf = 2467MHz
-    0x13333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 13, Tf = 2472MHz
-    0x06666100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 14, Tf = 2484MHz
-
-    // 4.9G => Ch 183, 184, 185, 187, 188, 189, 192, 196  (Value:15 ~ 22)
-    0x1D555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 183, Tf = 4915MHz (15)
-    0x00000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 184, Tf = 4920MHz (16)
-    0x02AAA100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 185, Tf = 4925MHz (17)
-    0x08000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 187, Tf = 4935MHz (18)
-    0x0AAAA100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 188, Tf = 4940MHz (19)
-    0x0D555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 189, Tf = 4945MHz (20)
-    0x15555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 192, Tf = 4960MHz (21)
-    0x00000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 196, Tf = 4980MHz (22)
-
-    // 5G => Ch 7, 8, 9, 11, 12, 16, 34, 36, 38, 40, 42, 44, 46, 48, 52, 56, 60, 64,
-    // 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165  (Value 23 ~ 56)
-    0x1D555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =   7, Tf = 5035MHz (23)
-    0x00000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =   8, Tf = 5040MHz (24)
-    0x02AAA100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =   9, Tf = 5045MHz (25)
-    0x08000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  11, Tf = 5055MHz (26)
-    0x0AAAA100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  12, Tf = 5060MHz (27)
-    0x15555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  16, Tf = 5080MHz (28)
-    0x05555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  34, Tf = 5170MHz (29)
-    0x0AAAA100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  36, Tf = 5180MHz (30)
-    0x10000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  38, Tf = 5190MHz (31)
-    0x15555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  40, Tf = 5200MHz (32)
-    0x1AAAA100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  42, Tf = 5210MHz (33)
-    0x00000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  44, Tf = 5220MHz (34)
-    0x05555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  46, Tf = 5230MHz (35)
-    0x0AAAA100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  48, Tf = 5240MHz (36)
-    0x15555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  52, Tf = 5260MHz (37)
-    0x00000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  56, Tf = 5280MHz (38)
-    0x0AAAA100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  60, Tf = 5300MHz (39)
-    0x15555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  64, Tf = 5320MHz (40)
-    0x15555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 100, Tf = 5500MHz (41)
-    0x00000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 104, Tf = 5520MHz (42)
-    0x0AAAA100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 108, Tf = 5540MHz (43)
-    0x15555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 112, Tf = 5560MHz (44)
-    0x00000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 116, Tf = 5580MHz (45)
-    0x0AAAA100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 120, Tf = 5600MHz (46)
-    0x15555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 124, Tf = 5620MHz (47)
-    0x00000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 128, Tf = 5640MHz (48)
-    0x0AAAA100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 132, Tf = 5660MHz (49)
-    0x15555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 136, Tf = 5680MHz (50)
-    0x00000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 140, Tf = 5700MHz (51)
-    0x18000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 149, Tf = 5745MHz (52)
-    0x02AAA100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 153, Tf = 5765MHz (53)
-    0x0D555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 157, Tf = 5785MHz (54)
-    0x18000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 161, Tf = 5805MHz (55)
-    0x02AAA100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW  // channel = 165, Tf = 5825MHz (56)
-    };
+	0x13333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  1, Tf = 2412MHz
+	0x1B333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  2, Tf = 2417MHz
+	0x03333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  3, Tf = 2422MHz
+	0x0B333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  4, Tf = 2427MHz
+	0x13333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  5, Tf = 2432MHz
+	0x1B333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  6, Tf = 2437MHz
+	0x03333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  7, Tf = 2442MHz
+	0x0B333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  8, Tf = 2447MHz
+	0x13333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  9, Tf = 2452MHz
+	0x1B333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 10, Tf = 2457MHz
+	0x03333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 11, Tf = 2462MHz
+	0x0B333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 12, Tf = 2467MHz
+	0x13333100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 13, Tf = 2472MHz
+	0x06666100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 14, Tf = 2484MHz
+
+	// 4.9G => Ch 183, 184, 185, 187, 188, 189, 192, 196  (Value:15 ~ 22)
+	0x1D555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 183, Tf = 4915MHz (15)
+	0x00000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 184, Tf = 4920MHz (16)
+	0x02AAA100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 185, Tf = 4925MHz (17)
+	0x08000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 187, Tf = 4935MHz (18)
+	0x0AAAA100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 188, Tf = 4940MHz (19)
+	0x0D555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 189, Tf = 4945MHz (20)
+	0x15555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 192, Tf = 4960MHz (21)
+	0x00000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 196, Tf = 4980MHz (22)
+
+	// 5G => Ch 7, 8, 9, 11, 12, 16, 34, 36, 38, 40, 42, 44, 46, 48, 52, 56, 60, 64,
+	// 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165  (Value 23 ~ 56)
+	0x1D555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =   7, Tf = 5035MHz (23)
+	0x00000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =   8, Tf = 5040MHz (24)
+	0x02AAA100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =   9, Tf = 5045MHz (25)
+	0x08000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  11, Tf = 5055MHz (26)
+	0x0AAAA100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  12, Tf = 5060MHz (27)
+	0x15555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  16, Tf = 5080MHz (28)
+	0x05555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  34, Tf = 5170MHz (29)
+	0x0AAAA100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  36, Tf = 5180MHz (30)
+	0x10000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  38, Tf = 5190MHz (31)
+	0x15555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  40, Tf = 5200MHz (32)
+	0x1AAAA100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  42, Tf = 5210MHz (33)
+	0x00000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  44, Tf = 5220MHz (34)
+	0x05555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  46, Tf = 5230MHz (35)
+	0x0AAAA100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  48, Tf = 5240MHz (36)
+	0x15555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  52, Tf = 5260MHz (37)
+	0x00000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  56, Tf = 5280MHz (38)
+	0x0AAAA100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  60, Tf = 5300MHz (39)
+	0x15555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  64, Tf = 5320MHz (40)
+	0x15555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 100, Tf = 5500MHz (41)
+	0x00000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 104, Tf = 5520MHz (42)
+	0x0AAAA100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 108, Tf = 5540MHz (43)
+	0x15555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 112, Tf = 5560MHz (44)
+	0x00000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 116, Tf = 5580MHz (45)
+	0x0AAAA100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 120, Tf = 5600MHz (46)
+	0x15555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 124, Tf = 5620MHz (47)
+	0x00000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 128, Tf = 5640MHz (48)
+	0x0AAAA100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 132, Tf = 5660MHz (49)
+	0x15555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 136, Tf = 5680MHz (50)
+	0x00000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 140, Tf = 5700MHz (51)
+	0x18000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 149, Tf = 5745MHz (52)
+	0x02AAA100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 153, Tf = 5765MHz (53)
+	0x0D555100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 157, Tf = 5785MHz (54)
+	0x18000100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 161, Tf = 5805MHz (55)
+	0x02AAA100+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW  // channel = 165, Tf = 5825MHz (56)
+};
 
 const unsigned long dwAL7230ChannelTable2[CB_MAX_CHANNEL] = {
-    0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  1, Tf = 2412MHz
-    0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  2, Tf = 2417MHz
-    0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  3, Tf = 2422MHz
-    0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  4, Tf = 2427MHz
-    0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  5, Tf = 2432MHz
-    0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  6, Tf = 2437MHz
-    0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  7, Tf = 2442MHz
-    0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  8, Tf = 2447MHz
-    0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  9, Tf = 2452MHz
-    0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 10, Tf = 2457MHz
-    0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 11, Tf = 2462MHz
-    0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 12, Tf = 2467MHz
-    0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 13, Tf = 2472MHz
-    0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 14, Tf = 2484MHz
-
-    // 4.9G => Ch 183, 184, 185, 187, 188, 189, 192, 196  (Value:15 ~ 22)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 183, Tf = 4915MHz (15)
-    0x67D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 184, Tf = 4920MHz (16)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 185, Tf = 4925MHz (17)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 187, Tf = 4935MHz (18)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 188, Tf = 4940MHz (19)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 189, Tf = 4945MHz (20)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 192, Tf = 4960MHz (21)
-    0x67D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 196, Tf = 4980MHz (22)
-
-    // 5G => Ch 7, 8, 9, 11, 12, 16, 34, 36, 38, 40, 42, 44, 46, 48, 52, 56, 60, 64,
-    // 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165  (Value 23 ~ 56)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =   7, Tf = 5035MHz (23)
-    0x67D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =   8, Tf = 5040MHz (24)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =   9, Tf = 5045MHz (25)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  11, Tf = 5055MHz (26)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  12, Tf = 5060MHz (27)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  16, Tf = 5080MHz (28)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  34, Tf = 5170MHz (29)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  36, Tf = 5180MHz (30)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  38, Tf = 5190MHz (31)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  40, Tf = 5200MHz (32)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  42, Tf = 5210MHz (33)
-    0x67D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  44, Tf = 5220MHz (34)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  46, Tf = 5230MHz (35)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  48, Tf = 5240MHz (36)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  52, Tf = 5260MHz (37)
-    0x67D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  56, Tf = 5280MHz (38)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  60, Tf = 5300MHz (39)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  64, Tf = 5320MHz (40)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 100, Tf = 5500MHz (41)
-    0x67D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 104, Tf = 5520MHz (42)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 108, Tf = 5540MHz (43)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 112, Tf = 5560MHz (44)
-    0x67D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 116, Tf = 5580MHz (45)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 120, Tf = 5600MHz (46)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 124, Tf = 5620MHz (47)
-    0x67D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 128, Tf = 5640MHz (48)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 132, Tf = 5660MHz (49)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 136, Tf = 5680MHz (50)
-    0x67D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 140, Tf = 5700MHz (51)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 149, Tf = 5745MHz (52)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 153, Tf = 5765MHz (53)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 157, Tf = 5785MHz (54)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 161, Tf = 5805MHz (55)
-    0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW  // channel = 165, Tf = 5825MHz (56)
-    };
+	0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  1, Tf = 2412MHz
+	0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  2, Tf = 2417MHz
+	0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  3, Tf = 2422MHz
+	0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  4, Tf = 2427MHz
+	0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  5, Tf = 2432MHz
+	0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  6, Tf = 2437MHz
+	0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  7, Tf = 2442MHz
+	0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  8, Tf = 2447MHz
+	0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  9, Tf = 2452MHz
+	0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 10, Tf = 2457MHz
+	0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 11, Tf = 2462MHz
+	0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 12, Tf = 2467MHz
+	0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 13, Tf = 2472MHz
+	0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 14, Tf = 2484MHz
+
+	// 4.9G => Ch 183, 184, 185, 187, 188, 189, 192, 196  (Value:15 ~ 22)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 183, Tf = 4915MHz (15)
+	0x67D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 184, Tf = 4920MHz (16)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 185, Tf = 4925MHz (17)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 187, Tf = 4935MHz (18)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 188, Tf = 4940MHz (19)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 189, Tf = 4945MHz (20)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 192, Tf = 4960MHz (21)
+	0x67D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 196, Tf = 4980MHz (22)
+
+	// 5G => Ch 7, 8, 9, 11, 12, 16, 34, 36, 38, 40, 42, 44, 46, 48, 52, 56, 60, 64,
+	// 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165  (Value 23 ~ 56)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =   7, Tf = 5035MHz (23)
+	0x67D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =   8, Tf = 5040MHz (24)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =   9, Tf = 5045MHz (25)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  11, Tf = 5055MHz (26)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  12, Tf = 5060MHz (27)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  16, Tf = 5080MHz (28)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  34, Tf = 5170MHz (29)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  36, Tf = 5180MHz (30)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  38, Tf = 5190MHz (31)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  40, Tf = 5200MHz (32)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  42, Tf = 5210MHz (33)
+	0x67D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  44, Tf = 5220MHz (34)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  46, Tf = 5230MHz (35)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  48, Tf = 5240MHz (36)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  52, Tf = 5260MHz (37)
+	0x67D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  56, Tf = 5280MHz (38)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  60, Tf = 5300MHz (39)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel =  64, Tf = 5320MHz (40)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 100, Tf = 5500MHz (41)
+	0x67D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 104, Tf = 5520MHz (42)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 108, Tf = 5540MHz (43)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 112, Tf = 5560MHz (44)
+	0x67D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 116, Tf = 5580MHz (45)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 120, Tf = 5600MHz (46)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 124, Tf = 5620MHz (47)
+	0x67D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 128, Tf = 5640MHz (48)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 132, Tf = 5660MHz (49)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 136, Tf = 5680MHz (50)
+	0x67D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 140, Tf = 5700MHz (51)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 149, Tf = 5745MHz (52)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 153, Tf = 5765MHz (53)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 157, Tf = 5785MHz (54)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 161, Tf = 5805MHz (55)
+	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW  // channel = 165, Tf = 5825MHz (56)
+};
 //}} RobertYu
 
-
-
-
 /*---------------------  Static Functions  --------------------------*/
 
-
-
-
 /*
  * Description: AIROHA IFRF chip init function
  *
@@ -438,72 +428,72 @@ const unsigned long dwAL7230ChannelTable2[CB_MAX_CHANNEL] = {
  * Return Value: true if succeeded; false if failed.
  *
  */
-bool s_bAL7230Init (unsigned long dwIoBase)
+bool s_bAL7230Init(unsigned long dwIoBase)
 {
-    int     ii;
-    bool bResult;
+	int     ii;
+	bool bResult;
 
-    bResult = true;
+	bResult = true;
 
-    //3-wire control for normal mode
-    VNSvOutPortB(dwIoBase + MAC_REG_SOFTPWRCTL, 0);
+	//3-wire control for normal mode
+	VNSvOutPortB(dwIoBase + MAC_REG_SOFTPWRCTL, 0);
 
-    MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, (SOFTPWRCTL_SWPECTI  |
-                                                     SOFTPWRCTL_TXPEINV));
-    BBvPowerSaveModeOFF(dwIoBase); //RobertYu:20050106, have DC value for Calibration
+	MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, (SOFTPWRCTL_SWPECTI  |
+							 SOFTPWRCTL_TXPEINV));
+	BBvPowerSaveModeOFF(dwIoBase); //RobertYu:20050106, have DC value for Calibration
 
-    for (ii = 0; ii < CB_AL7230_INIT_SEQ; ii++)
-        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[ii]);
+	for (ii = 0; ii < CB_AL7230_INIT_SEQ; ii++)
+		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[ii]);
 
-    // PLL On
-    MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE3);
+	// PLL On
+	MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE3);
 
-    //Calibration
-    MACvTimer0MicroSDelay(dwIoBase, 150);//150us
-    bResult &= IFRFbWriteEmbedded(dwIoBase, (0x9ABA8F00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW)); //TXDCOC:active, RCK:disable
-    MACvTimer0MicroSDelay(dwIoBase, 30);//30us
-    bResult &= IFRFbWriteEmbedded(dwIoBase, (0x3ABA8F00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW)); //TXDCOC:disable, RCK:active
-    MACvTimer0MicroSDelay(dwIoBase, 30);//30us
-    bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[CB_AL7230_INIT_SEQ-1]); //TXDCOC:disable, RCK:disable
+	//Calibration
+	MACvTimer0MicroSDelay(dwIoBase, 150);//150us
+	bResult &= IFRFbWriteEmbedded(dwIoBase, (0x9ABA8F00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW)); //TXDCOC:active, RCK:disable
+	MACvTimer0MicroSDelay(dwIoBase, 30);//30us
+	bResult &= IFRFbWriteEmbedded(dwIoBase, (0x3ABA8F00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW)); //TXDCOC:disable, RCK:active
+	MACvTimer0MicroSDelay(dwIoBase, 30);//30us
+	bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[CB_AL7230_INIT_SEQ-1]); //TXDCOC:disable, RCK:disable
 
-    MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, (SOFTPWRCTL_SWPE3    |
-                                                     SOFTPWRCTL_SWPE2    |
-                                                     SOFTPWRCTL_SWPECTI  |
-                                                     SOFTPWRCTL_TXPEINV));
+	MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, (SOFTPWRCTL_SWPE3    |
+							 SOFTPWRCTL_SWPE2    |
+							 SOFTPWRCTL_SWPECTI  |
+							 SOFTPWRCTL_TXPEINV));
 
-    BBvPowerSaveModeON(dwIoBase); // RobertYu:20050106
+	BBvPowerSaveModeON(dwIoBase); // RobertYu:20050106
 
-    // PE1: TX_ON, PE2: RX_ON, PE3: PLLON
-    //3-wire control for power saving mode
-    VNSvOutPortB(dwIoBase + MAC_REG_PSPWRSIG, (PSSIG_WPE3 | PSSIG_WPE2)); //1100 0000
+	// PE1: TX_ON, PE2: RX_ON, PE3: PLLON
+	//3-wire control for power saving mode
+	VNSvOutPortB(dwIoBase + MAC_REG_PSPWRSIG, (PSSIG_WPE3 | PSSIG_WPE2)); //1100 0000
 
-    return bResult;
+	return bResult;
 }
 
 // Need to Pull PLLON low when writing channel registers through 3-wire interface
-bool s_bAL7230SelectChannel (unsigned long dwIoBase, unsigned char byChannel)
+bool s_bAL7230SelectChannel(unsigned long dwIoBase, unsigned char byChannel)
 {
-    bool bResult;
+	bool bResult;
 
-    bResult = true;
+	bResult = true;
 
-    // PLLON Off
-    MACvWordRegBitsOff(dwIoBase, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE3);
+	// PLLON Off
+	MACvWordRegBitsOff(dwIoBase, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE3);
 
-    bResult &= IFRFbWriteEmbedded (dwIoBase, dwAL7230ChannelTable0[byChannel-1]); //Reg0
-    bResult &= IFRFbWriteEmbedded (dwIoBase, dwAL7230ChannelTable1[byChannel-1]); //Reg1
-    bResult &= IFRFbWriteEmbedded (dwIoBase, dwAL7230ChannelTable2[byChannel-1]); //Reg4
+	bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230ChannelTable0[byChannel - 1]); //Reg0
+	bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230ChannelTable1[byChannel - 1]); //Reg1
+	bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230ChannelTable2[byChannel - 1]); //Reg4
 
-    // PLLOn On
-    MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE3);
+	// PLLOn On
+	MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE3);
 
-    // Set Channel[7] = 0 to tell H/W channel is changing now.
-    VNSvOutPortB(dwIoBase + MAC_REG_CHANNEL, (byChannel & 0x7F));
-    MACvTimer0MicroSDelay(dwIoBase, SWITCH_CHANNEL_DELAY_AL7230);
-    // Set Channel[7] = 1 to tell H/W channel change is done.
-    VNSvOutPortB(dwIoBase + MAC_REG_CHANNEL, (byChannel | 0x80));
+	// Set Channel[7] = 0 to tell H/W channel is changing now.
+	VNSvOutPortB(dwIoBase + MAC_REG_CHANNEL, (byChannel & 0x7F));
+	MACvTimer0MicroSDelay(dwIoBase, SWITCH_CHANNEL_DELAY_AL7230);
+	// Set Channel[7] = 1 to tell H/W channel change is done.
+	VNSvOutPortB(dwIoBase + MAC_REG_CHANNEL, (byChannel | 0x80));
 
-    return bResult;
+	return bResult;
 }
 
 /*
@@ -520,7 +510,6 @@ bool s_bAL7230SelectChannel (unsigned long dwIoBase, unsigned char byChannel)
  *
  */
 
-
 //{{ RobertYu: 20041210
 /*
  * Description: UW2452 IFRF chip init function
@@ -535,8 +524,6 @@ bool s_bAL7230SelectChannel (unsigned long dwIoBase, unsigned char byChannel)
  *
  */
 
-
-
 //}} RobertYu
 ////////////////////////////////////////////////////////////////////////////////
 
@@ -567,8 +554,6 @@ bool s_bAL7230SelectChannel (unsigned long dwIoBase, unsigned char byChannel)
  *
  */
 
-
-
 /*---------------------  Export Variables  --------------------------*/
 
 /*---------------------  Export Functions  --------------------------*/
@@ -586,29 +571,27 @@ bool s_bAL7230SelectChannel (unsigned long dwIoBase, unsigned char byChannel)
  * Return Value: true if succeeded; false if failed.
  *
  */
-bool IFRFbWriteEmbedded (unsigned long dwIoBase, unsigned long dwData)
+bool IFRFbWriteEmbedded(unsigned long dwIoBase, unsigned long dwData)
 {
-    unsigned short ww;
-    unsigned long dwValue;
+	unsigned short ww;
+	unsigned long dwValue;
 
-    VNSvOutPortD(dwIoBase + MAC_REG_IFREGCTL, dwData);
+	VNSvOutPortD(dwIoBase + MAC_REG_IFREGCTL, dwData);
 
-    // W_MAX_TIMEOUT is the timeout period
-    for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-        VNSvInPortD(dwIoBase + MAC_REG_IFREGCTL, &dwValue);
-        if (dwValue & IFREGCTL_DONE)
-            break;
-    }
+	// W_MAX_TIMEOUT is the timeout period
+	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
+		VNSvInPortD(dwIoBase + MAC_REG_IFREGCTL, &dwValue);
+		if (dwValue & IFREGCTL_DONE)
+			break;
+	}
 
-    if (ww == W_MAX_TIMEOUT) {
+	if (ww == W_MAX_TIMEOUT) {
 //        DBG_PORT80_ALWAYS(0x32);
-        return false;
-    }
-    return true;
+		return false;
+	}
+	return true;
 }
 
-
-
 /*
  * Description: RFMD RF2959 IFRF chip init function
  *
@@ -648,72 +631,69 @@ bool IFRFbWriteEmbedded (unsigned long dwIoBase, unsigned long dwData)
  * Return Value: true if succeeded; false if failed.
  *
  */
-bool RFbAL2230Init (unsigned long dwIoBase)
+bool RFbAL2230Init(unsigned long dwIoBase)
 {
-    int     ii;
-    bool bResult;
+	int     ii;
+	bool bResult;
 
-    bResult = true;
+	bResult = true;
 
-    //3-wire control for normal mode
-    VNSvOutPortB(dwIoBase + MAC_REG_SOFTPWRCTL, 0);
+	//3-wire control for normal mode
+	VNSvOutPortB(dwIoBase + MAC_REG_SOFTPWRCTL, 0);
 
-    MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, (SOFTPWRCTL_SWPECTI  |
-                                                     SOFTPWRCTL_TXPEINV));
+	MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, (SOFTPWRCTL_SWPECTI  |
+							 SOFTPWRCTL_TXPEINV));
 //2008-8-21 chester <add>
-    // PLL  Off
-
-    MACvWordRegBitsOff(dwIoBase, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE3);
-
+	// PLL  Off
 
+	MACvWordRegBitsOff(dwIoBase, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE3);
 
-    //patch abnormal AL2230 frequency output
+	//patch abnormal AL2230 frequency output
 //2008-8-21 chester <add>
-    IFRFbWriteEmbedded(dwIoBase, (0x07168700+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW));
+	IFRFbWriteEmbedded(dwIoBase, (0x07168700+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW));
 
-
-    for (ii = 0; ii < CB_AL2230_INIT_SEQ; ii++)
-        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL2230InitTable[ii]);
+	for (ii = 0; ii < CB_AL2230_INIT_SEQ; ii++)
+		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL2230InitTable[ii]);
 //2008-8-21 chester <add>
-MACvTimer0MicroSDelay(dwIoBase, 30); //delay 30 us
+	MACvTimer0MicroSDelay(dwIoBase, 30); //delay 30 us
 
-    // PLL On
-    MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE3);
+	// PLL On
+	MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE3);
 
-    MACvTimer0MicroSDelay(dwIoBase, 150);//150us
-    bResult &= IFRFbWriteEmbedded(dwIoBase, (0x00d80f00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW));
-    MACvTimer0MicroSDelay(dwIoBase, 30);//30us
-    bResult &= IFRFbWriteEmbedded(dwIoBase, (0x00780f00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW));
-    MACvTimer0MicroSDelay(dwIoBase, 30);//30us
-    bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL2230InitTable[CB_AL2230_INIT_SEQ-1]);
+	MACvTimer0MicroSDelay(dwIoBase, 150);//150us
+	bResult &= IFRFbWriteEmbedded(dwIoBase, (0x00d80f00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW));
+	MACvTimer0MicroSDelay(dwIoBase, 30);//30us
+	bResult &= IFRFbWriteEmbedded(dwIoBase, (0x00780f00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW));
+	MACvTimer0MicroSDelay(dwIoBase, 30);//30us
+	bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL2230InitTable[CB_AL2230_INIT_SEQ-1]);
 
-    MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, (SOFTPWRCTL_SWPE3    |
-                                                     SOFTPWRCTL_SWPE2    |
-                                                     SOFTPWRCTL_SWPECTI  |
-                                                     SOFTPWRCTL_TXPEINV));
+	MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, (SOFTPWRCTL_SWPE3    |
+							 SOFTPWRCTL_SWPE2    |
+							 SOFTPWRCTL_SWPECTI  |
+							 SOFTPWRCTL_TXPEINV));
 
-    //3-wire control for power saving mode
-    VNSvOutPortB(dwIoBase + MAC_REG_PSPWRSIG, (PSSIG_WPE3 | PSSIG_WPE2)); //1100 0000
+	//3-wire control for power saving mode
+	VNSvOutPortB(dwIoBase + MAC_REG_PSPWRSIG, (PSSIG_WPE3 | PSSIG_WPE2)); //1100 0000
 
-    return bResult;
+	return bResult;
 }
 
-bool RFbAL2230SelectChannel (unsigned long dwIoBase, unsigned char byChannel)
+bool RFbAL2230SelectChannel(unsigned long dwIoBase, unsigned char byChannel)
 {
-    bool bResult;
+	bool bResult;
 
-    bResult = true;
+	bResult = true;
 
-    bResult &= IFRFbWriteEmbedded (dwIoBase, dwAL2230ChannelTable0[byChannel-1]);
-    bResult &= IFRFbWriteEmbedded (dwIoBase, dwAL2230ChannelTable1[byChannel-1]);
+	bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL2230ChannelTable0[byChannel - 1]);
+	bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL2230ChannelTable1[byChannel - 1]);
 
-    // Set Channel[7] = 0 to tell H/W channel is changing now.
-    VNSvOutPortB(dwIoBase + MAC_REG_CHANNEL, (byChannel & 0x7F));
-    MACvTimer0MicroSDelay(dwIoBase, SWITCH_CHANNEL_DELAY_AL2230);
-    // Set Channel[7] = 1 to tell H/W channel change is done.
-    VNSvOutPortB(dwIoBase + MAC_REG_CHANNEL, (byChannel | 0x80));
+	// Set Channel[7] = 0 to tell H/W channel is changing now.
+	VNSvOutPortB(dwIoBase + MAC_REG_CHANNEL, (byChannel & 0x7F));
+	MACvTimer0MicroSDelay(dwIoBase, SWITCH_CHANNEL_DELAY_AL2230);
+	// Set Channel[7] = 1 to tell H/W channel change is done.
+	VNSvOutPortB(dwIoBase + MAC_REG_CHANNEL, (byChannel | 0x80));
 
-    return bResult;
+	return bResult;
 }
 
 /*
@@ -729,7 +709,6 @@ bool RFbAL2230SelectChannel (unsigned long dwIoBase, unsigned char byChannel)
  *
  */
 
-
 /*
  * Description: Select channel with UW2451 chip
  *
@@ -771,29 +750,29 @@ bool RFbAL2230SelectChannel (unsigned long dwIoBase, unsigned char byChannel)
  * Return Value: true if succeeded; false if failed.
  *
  */
-bool RFbInit (
-    PSDevice  pDevice
-    )
+bool RFbInit(
+	PSDevice  pDevice
+)
 {
-bool bResult = true;
-    switch (pDevice->byRFType) {
-        case RF_AIROHA :
-        case RF_AL2230S:
-            pDevice->byMaxPwrLevel = AL2230_PWR_IDX_LEN;
-            bResult = RFbAL2230Init(pDevice->PortOffset);
-            break;
-        case RF_AIROHA7230 :
-            pDevice->byMaxPwrLevel = AL7230_PWR_IDX_LEN;
-            bResult = s_bAL7230Init(pDevice->PortOffset);
-            break;
-        case RF_NOTHING :
-            bResult = true;
-            break;
-        default :
-            bResult = false;
-            break;
-    }
-    return bResult;
+	bool bResult = true;
+	switch (pDevice->byRFType) {
+	case RF_AIROHA:
+	case RF_AL2230S:
+		pDevice->byMaxPwrLevel = AL2230_PWR_IDX_LEN;
+		bResult = RFbAL2230Init(pDevice->PortOffset);
+		break;
+	case RF_AIROHA7230:
+		pDevice->byMaxPwrLevel = AL7230_PWR_IDX_LEN;
+		bResult = s_bAL7230Init(pDevice->PortOffset);
+		break;
+	case RF_NOTHING:
+		bResult = true;
+		break;
+	default:
+		bResult = false;
+		break;
+	}
+	return bResult;
 }
 
 /*
@@ -809,21 +788,21 @@ bool bResult = true;
  * Return Value: true if succeeded; false if failed.
  *
  */
-bool RFbShutDown (
-    PSDevice  pDevice
-    )
+bool RFbShutDown(
+	PSDevice  pDevice
+)
 {
-bool bResult = true;
-
-    switch (pDevice->byRFType) {
-        case RF_AIROHA7230 :
-            bResult = IFRFbWriteEmbedded (pDevice->PortOffset, 0x1ABAEF00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW);
-            break;
-        default :
-            bResult = true;
-            break;
-    }
-    return bResult;
+	bool bResult = true;
+
+	switch (pDevice->byRFType) {
+	case RF_AIROHA7230:
+		bResult = IFRFbWriteEmbedded(pDevice->PortOffset, 0x1ABAEF00 + (BY_AL7230_REG_LEN << 3) + IFREGCTL_REGW);
+		break;
+	default:
+		bResult = true;
+		break;
+	}
+	return bResult;
 }
 
 /*
@@ -839,28 +818,27 @@ bool bResult = true;
  * Return Value: true if succeeded; false if failed.
  *
  */
-bool RFbSelectChannel (unsigned long dwIoBase, unsigned char byRFType, unsigned char byChannel)
+bool RFbSelectChannel(unsigned long dwIoBase, unsigned char byRFType, unsigned char byChannel)
 {
-bool bResult = true;
-    switch (byRFType) {
-
-        case RF_AIROHA :
-        case RF_AL2230S:
-            bResult = RFbAL2230SelectChannel(dwIoBase, byChannel);
-            break;
-        //{{ RobertYu: 20050104
-        case RF_AIROHA7230 :
-            bResult = s_bAL7230SelectChannel(dwIoBase, byChannel);
-            break;
-        //}} RobertYu
-        case RF_NOTHING :
-            bResult = true;
-            break;
-        default:
-            bResult = false;
-            break;
-    }
-    return bResult;
+	bool bResult = true;
+	switch (byRFType) {
+	case RF_AIROHA:
+	case RF_AL2230S:
+		bResult = RFbAL2230SelectChannel(dwIoBase, byChannel);
+		break;
+		//{{ RobertYu: 20050104
+	case RF_AIROHA7230:
+		bResult = s_bAL7230SelectChannel(dwIoBase, byChannel);
+		break;
+		//}} RobertYu
+	case RF_NOTHING:
+		bResult = true;
+		break;
+	default:
+		bResult = false;
+		break;
+	}
+	return bResult;
 }
 
 /*
@@ -875,76 +853,73 @@ bool bResult = true;
  * Return Value: None.
  *
  */
-bool RFvWriteWakeProgSyn (unsigned long dwIoBase, unsigned char byRFType, unsigned int uChannel)
+bool RFvWriteWakeProgSyn(unsigned long dwIoBase, unsigned char byRFType, unsigned int uChannel)
 {
-    int   ii;
-    unsigned char byInitCount = 0;
-    unsigned char bySleepCount = 0;
-
-    VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, 0);
-    switch (byRFType) {
-        case RF_AIROHA:
-        case RF_AL2230S:
-
-            if (uChannel > CB_MAX_CHANNEL_24G)
-                return false;
-
-            byInitCount = CB_AL2230_INIT_SEQ + 2; // Init Reg + Channel Reg (2)
-            bySleepCount = 0;
-            if (byInitCount > (MISCFIFO_SYNDATASIZE - bySleepCount)) {
-                return false;
-            }
-
-            for (ii = 0; ii < CB_AL2230_INIT_SEQ; ii++ ) {
-                MACvSetMISCFifo(dwIoBase, (unsigned short)(MISCFIFO_SYNDATA_IDX + ii), dwAL2230InitTable[ii]);
-            }
-            MACvSetMISCFifo(dwIoBase, (unsigned short)(MISCFIFO_SYNDATA_IDX + ii), dwAL2230ChannelTable0[uChannel-1]);
-            ii ++;
-            MACvSetMISCFifo(dwIoBase, (unsigned short)(MISCFIFO_SYNDATA_IDX + ii), dwAL2230ChannelTable1[uChannel-1]);
-            break;
-
-        //{{ RobertYu: 20050104
-        // Need to check, PLLON need to be low for channel setting
-        case RF_AIROHA7230:
-            byInitCount = CB_AL7230_INIT_SEQ + 3; // Init Reg + Channel Reg (3)
-            bySleepCount = 0;
-            if (byInitCount > (MISCFIFO_SYNDATASIZE - bySleepCount)) {
-                return false;
-            }
-
-            if (uChannel <= CB_MAX_CHANNEL_24G)
-            {
-                for (ii = 0; ii < CB_AL7230_INIT_SEQ; ii++ ) {
-                    MACvSetMISCFifo(dwIoBase, (unsigned short)(MISCFIFO_SYNDATA_IDX + ii), dwAL7230InitTable[ii]);
-                }
-            }
-            else
-            {
-                for (ii = 0; ii < CB_AL7230_INIT_SEQ; ii++ ) {
-                    MACvSetMISCFifo(dwIoBase, (unsigned short)(MISCFIFO_SYNDATA_IDX + ii), dwAL7230InitTableAMode[ii]);
-                }
-            }
-
-            MACvSetMISCFifo(dwIoBase, (unsigned short)(MISCFIFO_SYNDATA_IDX + ii), dwAL7230ChannelTable0[uChannel-1]);
-            ii ++;
-            MACvSetMISCFifo(dwIoBase, (unsigned short)(MISCFIFO_SYNDATA_IDX + ii), dwAL7230ChannelTable1[uChannel-1]);
-            ii ++;
-            MACvSetMISCFifo(dwIoBase, (unsigned short)(MISCFIFO_SYNDATA_IDX + ii), dwAL7230ChannelTable2[uChannel-1]);
-            break;
-        //}} RobertYu
-
-        case RF_NOTHING :
-            return true;
-            break;
-
-        default:
-            return false;
-            break;
-    }
-
-    MACvSetMISCFifo(dwIoBase, MISCFIFO_SYNINFO_IDX, (unsigned long )MAKEWORD(bySleepCount, byInitCount));
-
-    return true;
+	int   ii;
+	unsigned char byInitCount = 0;
+	unsigned char bySleepCount = 0;
+
+	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, 0);
+	switch (byRFType) {
+	case RF_AIROHA:
+	case RF_AL2230S:
+
+		if (uChannel > CB_MAX_CHANNEL_24G)
+			return false;
+
+		byInitCount = CB_AL2230_INIT_SEQ + 2; // Init Reg + Channel Reg (2)
+		bySleepCount = 0;
+		if (byInitCount > (MISCFIFO_SYNDATASIZE - bySleepCount)) {
+			return false;
+		}
+
+		for (ii = 0; ii < CB_AL2230_INIT_SEQ; ii++) {
+			MACvSetMISCFifo(dwIoBase, (unsigned short)(MISCFIFO_SYNDATA_IDX + ii), dwAL2230InitTable[ii]);
+		}
+		MACvSetMISCFifo(dwIoBase, (unsigned short)(MISCFIFO_SYNDATA_IDX + ii), dwAL2230ChannelTable0[uChannel-1]);
+		ii++;
+		MACvSetMISCFifo(dwIoBase, (unsigned short)(MISCFIFO_SYNDATA_IDX + ii), dwAL2230ChannelTable1[uChannel-1]);
+		break;
+
+		//{{ RobertYu: 20050104
+		// Need to check, PLLON need to be low for channel setting
+	case RF_AIROHA7230:
+		byInitCount = CB_AL7230_INIT_SEQ + 3; // Init Reg + Channel Reg (3)
+		bySleepCount = 0;
+		if (byInitCount > (MISCFIFO_SYNDATASIZE - bySleepCount)) {
+			return false;
+		}
+
+		if (uChannel <= CB_MAX_CHANNEL_24G) {
+			for (ii = 0; ii < CB_AL7230_INIT_SEQ; ii++) {
+				MACvSetMISCFifo(dwIoBase, (unsigned short)(MISCFIFO_SYNDATA_IDX + ii), dwAL7230InitTable[ii]);
+			}
+		} else {
+			for (ii = 0; ii < CB_AL7230_INIT_SEQ; ii++) {
+				MACvSetMISCFifo(dwIoBase, (unsigned short)(MISCFIFO_SYNDATA_IDX + ii), dwAL7230InitTableAMode[ii]);
+			}
+		}
+
+		MACvSetMISCFifo(dwIoBase, (unsigned short)(MISCFIFO_SYNDATA_IDX + ii), dwAL7230ChannelTable0[uChannel-1]);
+		ii++;
+		MACvSetMISCFifo(dwIoBase, (unsigned short)(MISCFIFO_SYNDATA_IDX + ii), dwAL7230ChannelTable1[uChannel-1]);
+		ii++;
+		MACvSetMISCFifo(dwIoBase, (unsigned short)(MISCFIFO_SYNDATA_IDX + ii), dwAL7230ChannelTable2[uChannel-1]);
+		break;
+		//}} RobertYu
+
+	case RF_NOTHING:
+		return true;
+		break;
+
+	default:
+		return false;
+		break;
+	}
+
+	MACvSetMISCFifo(dwIoBase, MISCFIFO_SYNINFO_IDX, (unsigned long)MAKEWORD(bySleepCount, byInitCount));
+
+	return true;
 }
 
 /*
@@ -960,87 +935,81 @@ bool RFvWriteWakeProgSyn (unsigned long dwIoBase, unsigned char byRFType, unsign
  * Return Value: true if succeeded; false if failed.
  *
  */
-bool RFbSetPower (
-    PSDevice  pDevice,
-    unsigned int uRATE,
-    unsigned int uCH
-    )
+bool RFbSetPower(
+	PSDevice  pDevice,
+	unsigned int uRATE,
+	unsigned int uCH
+)
 {
-bool bResult = true;
-unsigned char byPwr = 0;
-unsigned char byDec = 0;
-unsigned char byPwrdBm = 0;
-
-    if (pDevice->dwDiagRefCount != 0) {
-        return true;
-    }
-    if ((uCH < 1) || (uCH > CB_MAX_CHANNEL)) {
-        return false;
-    }
-
-    switch (uRATE) {
-    case RATE_1M:
-    case RATE_2M:
-    case RATE_5M:
-    case RATE_11M:
-        byPwr = pDevice->abyCCKPwrTbl[uCH];
-        byPwrdBm = pDevice->abyCCKDefaultPwr[uCH];
+	bool bResult = true;
+	unsigned char byPwr = 0;
+	unsigned char byDec = 0;
+	unsigned char byPwrdBm = 0;
+
+	if (pDevice->dwDiagRefCount != 0) {
+		return true;
+	}
+	if ((uCH < 1) || (uCH > CB_MAX_CHANNEL)) {
+		return false;
+	}
+
+	switch (uRATE) {
+	case RATE_1M:
+	case RATE_2M:
+	case RATE_5M:
+	case RATE_11M:
+		byPwr = pDevice->abyCCKPwrTbl[uCH];
+		byPwrdBm = pDevice->abyCCKDefaultPwr[uCH];
 //PLICE_DEBUG->
-	//byPwr+=5;
+		//byPwr+=5;
 //PLICE_DEBUG <-
-
-//printk("Rate <11:byPwr is %d\n",byPwr);
 		break;
-    case RATE_6M:
-    case RATE_9M:
-    case RATE_18M:
-        byPwr = pDevice->abyOFDMPwrTbl[uCH];
-        if (pDevice->byRFType == RF_UW2452) {
-            byDec = byPwr + 14;
-        } else {
-            byDec = byPwr + 10;
-        }
-        if (byDec >= pDevice->byMaxPwrLevel) {
-            byDec = pDevice->byMaxPwrLevel-1;
-        }
-        if (pDevice->byRFType == RF_UW2452) {
-            byPwrdBm = byDec - byPwr;
-            byPwrdBm /= 3;
-        } else {
-            byPwrdBm = byDec - byPwr;
-            byPwrdBm >>= 1;
-        }
-        byPwrdBm += pDevice->abyOFDMDefaultPwr[uCH];
-        byPwr = byDec;
+	case RATE_6M:
+	case RATE_9M:
+	case RATE_18M:
+		byPwr = pDevice->abyOFDMPwrTbl[uCH];
+		if (pDevice->byRFType == RF_UW2452) {
+			byDec = byPwr + 14;
+		} else {
+			byDec = byPwr + 10;
+		}
+		if (byDec >= pDevice->byMaxPwrLevel) {
+			byDec = pDevice->byMaxPwrLevel-1;
+		}
+		if (pDevice->byRFType == RF_UW2452) {
+			byPwrdBm = byDec - byPwr;
+			byPwrdBm /= 3;
+		} else {
+			byPwrdBm = byDec - byPwr;
+			byPwrdBm >>= 1;
+		}
+		byPwrdBm += pDevice->abyOFDMDefaultPwr[uCH];
+		byPwr = byDec;
 //PLICE_DEBUG->
-	//byPwr+=5;
+		//byPwr+=5;
 //PLICE_DEBUG<-
-
-//printk("Rate <24:byPwr is %d\n",byPwr);
 		break;
-    case RATE_24M:
-    case RATE_36M:
-    case RATE_48M:
-    case RATE_54M:
-        byPwr = pDevice->abyOFDMPwrTbl[uCH];
-        byPwrdBm = pDevice->abyOFDMDefaultPwr[uCH];
+	case RATE_24M:
+	case RATE_36M:
+	case RATE_48M:
+	case RATE_54M:
+		byPwr = pDevice->abyOFDMPwrTbl[uCH];
+		byPwrdBm = pDevice->abyOFDMDefaultPwr[uCH];
 //PLICE_DEBUG->
-	//byPwr+=5;
+		//byPwr+=5;
 //PLICE_DEBUG<-
-//printk("Rate < 54:byPwr is %d\n",byPwr);
 		break;
-    }
-
-//    if (pDevice->byLocalID <= REV_ID_VT3253_B1) {
-    if (pDevice->byCurPwr == byPwr) {
-        return true;
-    }
-    bResult = RFbRawSetPower(pDevice, byPwr, uRATE);
-//    }
-    if (bResult == true) {
-       pDevice->byCurPwr = byPwr;
-    }
-    return bResult;
+	}
+
+	if (pDevice->byCurPwr == byPwr) {
+		return true;
+	}
+
+	bResult = RFbRawSetPower(pDevice, byPwr, uRATE);
+	if (bResult == true) {
+		pDevice->byCurPwr = byPwr;
+	}
+	return bResult;
 }
 
 /*
@@ -1057,55 +1026,52 @@ unsigned char byPwrdBm = 0;
  *
  */
 
-bool RFbRawSetPower (
-    PSDevice  pDevice,
-    unsigned char byPwr,
-    unsigned int uRATE
-    )
+bool RFbRawSetPower(
+	PSDevice  pDevice,
+	unsigned char byPwr,
+	unsigned int uRATE
+)
 {
-bool bResult = true;
-unsigned long dwMax7230Pwr = 0;
-
-    if (byPwr >=  pDevice->byMaxPwrLevel) {
-        return (false);
-    }
-    switch (pDevice->byRFType) {
-
-        case RF_AIROHA :
-            bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, dwAL2230PowerTable[byPwr]);
-            if (uRATE <= RATE_11M) {
-                bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x0001B400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
-            } else {
-                bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x0005A400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
-            }
-            break;
-
-
-        case RF_AL2230S :
-            bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, dwAL2230PowerTable[byPwr]);
-            if (uRATE <= RATE_11M) {
-                bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x040C1400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
-                bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x00299B00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
-            }else {
-                bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x0005A400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
-                bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x00099B00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
-            }
-
-            break;
-
-        case RF_AIROHA7230:
-            //  0x080F1B00 for 3 wire control TxGain(D10) and 0x31 as TX Gain value
-            dwMax7230Pwr = 0x080C0B00 | ( (byPwr) << 12 ) |
-                           (BY_AL7230_REG_LEN << 3 )  | IFREGCTL_REGW;
-
-            bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, dwMax7230Pwr);
-            break;
-
-
-        default :
-            break;
-    }
-    return bResult;
+	bool bResult = true;
+	unsigned long dwMax7230Pwr = 0;
+
+	if (byPwr >=  pDevice->byMaxPwrLevel) {
+		return false;
+	}
+	switch (pDevice->byRFType) {
+	case RF_AIROHA:
+		bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, dwAL2230PowerTable[byPwr]);
+		if (uRATE <= RATE_11M) {
+			bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x0001B400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
+		} else {
+			bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x0005A400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
+		}
+		break;
+
+	case RF_AL2230S:
+		bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, dwAL2230PowerTable[byPwr]);
+		if (uRATE <= RATE_11M) {
+			bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x040C1400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
+			bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x00299B00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
+		} else {
+			bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x0005A400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
+			bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x00099B00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
+		}
+
+		break;
+
+	case RF_AIROHA7230:
+		//  0x080F1B00 for 3 wire control TxGain(D10) and 0x31 as TX Gain value
+		dwMax7230Pwr = 0x080C0B00 | ((byPwr) << 12) |
+			(BY_AL7230_REG_LEN << 3)  | IFREGCTL_REGW;
+
+		bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, dwMax7230Pwr);
+		break;
+
+	default:
+		break;
+	}
+	return bResult;
 }
 
 /*+
@@ -1122,74 +1088,68 @@ unsigned long dwMax7230Pwr = 0;
  *
  * Return Value: none
  *
--*/
+ -*/
 void
-RFvRSSITodBm (
-    PSDevice pDevice,
-    unsigned char byCurrRSSI,
-    long *    pldBm
-    )
+RFvRSSITodBm(
+	PSDevice pDevice,
+	unsigned char byCurrRSSI,
+	long *pldBm
+	)
 {
-    unsigned char byIdx = (((byCurrRSSI & 0xC0) >> 6) & 0x03);
-    long b = (byCurrRSSI & 0x3F);
-    long a = 0;
-    unsigned char abyAIROHARF[4] = {0, 18, 0, 40};
-
-    switch (pDevice->byRFType) {
-        case RF_AIROHA:
-        case RF_AL2230S:
-        case RF_AIROHA7230: //RobertYu: 20040104
-            a = abyAIROHARF[byIdx];
-            break;
-        default:
-            break;
-    }
-
-    *pldBm = -1 * (a + b * 2);
+	unsigned char byIdx = (((byCurrRSSI & 0xC0) >> 6) & 0x03);
+	long b = (byCurrRSSI & 0x3F);
+	long a = 0;
+	unsigned char abyAIROHARF[4] = {0, 18, 0, 40};
+
+	switch (pDevice->byRFType) {
+	case RF_AIROHA:
+	case RF_AL2230S:
+	case RF_AIROHA7230: //RobertYu: 20040104
+		a = abyAIROHARF[byIdx];
+		break;
+	default:
+		break;
+	}
+
+	*pldBm = -1 * (a + b * 2);
 }
 
 ////////////////////////////////////////////////////////////////////////////////
 //{{ RobertYu: 20050104
 
-
 // Post processing for the 11b/g and 11a.
 // for save time on changing Reg2,3,5,7,10,12,15
-bool RFbAL7230SelectChannelPostProcess (unsigned long dwIoBase, unsigned char byOldChannel, unsigned char byNewChannel)
+bool RFbAL7230SelectChannelPostProcess(unsigned long dwIoBase, unsigned char byOldChannel, unsigned char byNewChannel)
 {
-    bool bResult;
-
-    bResult = true;
-
-    // if change between 11 b/g and 11a need to update the following register
-    // Channel Index 1~14
-
-    if( (byOldChannel <= CB_MAX_CHANNEL_24G) && (byNewChannel > CB_MAX_CHANNEL_24G) )
-    {
-        // Change from 2.4G to 5G
-        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[2]); //Reg2
-        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[3]); //Reg3
-        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[5]); //Reg5
-        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[7]); //Reg7
-        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[10]);//Reg10
-        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[12]);//Reg12
-        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[15]);//Reg15
-    }
-    else if( (byOldChannel > CB_MAX_CHANNEL_24G) && (byNewChannel <= CB_MAX_CHANNEL_24G) )
-    {
-        // change from 5G to 2.4G
-        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[2]); //Reg2
-        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[3]); //Reg3
-        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[5]); //Reg5
-        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[7]); //Reg7
-        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[10]);//Reg10
-        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[12]);//Reg12
-        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[15]);//Reg15
-    }
-
-    return bResult;
+	bool bResult;
+
+	bResult = true;
+
+	// if change between 11 b/g and 11a need to update the following register
+	// Channel Index 1~14
+
+	if ((byOldChannel <= CB_MAX_CHANNEL_24G) && (byNewChannel > CB_MAX_CHANNEL_24G)) {
+		// Change from 2.4G to 5G
+		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[2]); //Reg2
+		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[3]); //Reg3
+		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[5]); //Reg5
+		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[7]); //Reg7
+		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[10]);//Reg10
+		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[12]);//Reg12
+		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[15]);//Reg15
+	} else if ((byOldChannel > CB_MAX_CHANNEL_24G) && (byNewChannel <= CB_MAX_CHANNEL_24G)) {
+		// change from 5G to 2.4G
+		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[2]); //Reg2
+		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[3]); //Reg3
+		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[5]); //Reg5
+		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[7]); //Reg7
+		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[10]);//Reg10
+		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[12]);//Reg12
+		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[15]);//Reg15
+	}
+
+	return bResult;
 }
 
-
 //}} RobertYu
 ////////////////////////////////////////////////////////////////////////////////
-
diff --git a/drivers/staging/vt6655/rf.h b/drivers/staging/vt6655/rf.h
index 1da0fdeb2e1..ef3c6de09fe 100644
--- a/drivers/staging/vt6655/rf.h
+++ b/drivers/staging/vt6655/rf.h
@@ -68,7 +68,6 @@
 #define CB_MAXIM2829_CHANNEL_5G_HIGH    41 //Index41: channel = 100, Tf = 5500MHz, set the (A3:A0=0101) D6=1
 #define CB_UW2452_CHANNEL_5G_HIGH       41 //[20041210] Index41: channel = 100, Tf = 5500MHz, change VCO2->VCO3
 
-
 /*---------------------  Export Classes  ----------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
@@ -77,29 +76,26 @@
 
 bool IFRFbWriteEmbedded(unsigned long dwIoBase, unsigned long dwData);
 bool RFbSelectChannel(unsigned long dwIoBase, unsigned char byRFType, unsigned char byChannel);
-bool RFbInit (
-    PSDevice  pDevice
-    );
+bool RFbInit(
+	PSDevice  pDevice
+);
 bool RFvWriteWakeProgSyn(unsigned long dwIoBase, unsigned char byRFType, unsigned int uChannel);
 bool RFbSetPower(PSDevice pDevice, unsigned int uRATE, unsigned int uCH);
 bool RFbRawSetPower(
-    PSDevice  pDevice,
-    unsigned char byPwr,
-    unsigned int uRATE
-    );
+	PSDevice  pDevice,
+	unsigned char byPwr,
+	unsigned int uRATE
+);
 
 void
 RFvRSSITodBm(
-    PSDevice pDevice,
-    unsigned char byCurrRSSI,
-    long    *pldBm
-    );
+	PSDevice pDevice,
+	unsigned char byCurrRSSI,
+	long    *pldBm
+);
 
 //{{ RobertYu: 20050104
 bool RFbAL7230SelectChannelPostProcess(unsigned long dwIoBase, unsigned char byOldChannel, unsigned char byNewChannel);
 //}} RobertYu
 
 #endif // __RF_H__
-
-
-
diff --git a/drivers/staging/vt6655/rxtx.c b/drivers/staging/vt6655/rxtx.c
index d66854f5b30..3a2661e2b27 100644
--- a/drivers/staging/vt6655/rxtx.c
+++ b/drivers/staging/vt6655/rxtx.c
@@ -69,11 +69,10 @@
 
 /*---------------------  Static Variables  --------------------------*/
 //static int          msglevel                =MSG_LEVEL_DEBUG;
-static int          msglevel                =MSG_LEVEL_INFO;
+static int msglevel = MSG_LEVEL_INFO;
 
 #define	PLICE_DEBUG
 
-
 /*---------------------  Static Functions  --------------------------*/
 
 /*---------------------  Static Definitions -------------------------*/
@@ -81,19 +80,18 @@ static int          msglevel                =MSG_LEVEL_INFO;
                                         //    packet size >= 256 -> direct send
 
 const unsigned short wTimeStampOff[2][MAX_RATE] = {
-        {384, 288, 226, 209, 54, 43, 37, 31, 28, 25, 24, 23}, // Long Preamble
-        {384, 192, 130, 113, 54, 43, 37, 31, 28, 25, 24, 23}, // Short Preamble
-    };
+	{384, 288, 226, 209, 54, 43, 37, 31, 28, 25, 24, 23}, // Long Preamble
+	{384, 192, 130, 113, 54, 43, 37, 31, 28, 25, 24, 23}, // Short Preamble
+};
 
 const unsigned short wFB_Opt0[2][5] = {
-        {RATE_12M, RATE_18M, RATE_24M, RATE_36M, RATE_48M}, // fallback_rate0
-        {RATE_12M, RATE_12M, RATE_18M, RATE_24M, RATE_36M}, // fallback_rate1
-    };
+	{RATE_12M, RATE_18M, RATE_24M, RATE_36M, RATE_48M}, // fallback_rate0
+	{RATE_12M, RATE_12M, RATE_18M, RATE_24M, RATE_36M}, // fallback_rate1
+};
 const unsigned short wFB_Opt1[2][5] = {
-        {RATE_12M, RATE_18M, RATE_24M, RATE_24M, RATE_36M}, // fallback_rate0
-        {RATE_6M , RATE_6M,  RATE_12M, RATE_12M, RATE_18M}, // fallback_rate1
-    };
-
+	{RATE_12M, RATE_18M, RATE_24M, RATE_24M, RATE_36M}, // fallback_rate0
+	{RATE_6M , RATE_6M,  RATE_12M, RATE_12M, RATE_18M}, // fallback_rate1
+};
 
 #define RTSDUR_BB       0
 #define RTSDUR_BA       1
@@ -112,1009 +110,946 @@ const unsigned short wFB_Opt1[2][5] = {
 
 /*---------------------  Static Functions  --------------------------*/
 
-
-
 static
 void
 s_vFillTxKey(
-    PSDevice   pDevice,
-    unsigned char *pbyBuf,
-    unsigned char *pbyIVHead,
-    PSKeyItem  pTransmitKey,
-    unsigned char *pbyHdrBuf,
-    unsigned short wPayloadLen,
-    unsigned char *pMICHDR
-    );
-
-
+	PSDevice   pDevice,
+	unsigned char *pbyBuf,
+	unsigned char *pbyIVHead,
+	PSKeyItem  pTransmitKey,
+	unsigned char *pbyHdrBuf,
+	unsigned short wPayloadLen,
+	unsigned char *pMICHDR
+);
 
 static
 void
 s_vFillRTSHead(
-    PSDevice         pDevice,
-    unsigned char byPktType,
-    void *           pvRTS,
-    unsigned int	cbFrameLength,
-    bool bNeedAck,
-    bool bDisCRC,
-    PSEthernetHeader psEthHeader,
-    unsigned short wCurrentRate,
-    unsigned char byFBOption
-    );
+	PSDevice         pDevice,
+	unsigned char byPktType,
+	void *pvRTS,
+	unsigned int	cbFrameLength,
+	bool bNeedAck,
+	bool bDisCRC,
+	PSEthernetHeader psEthHeader,
+	unsigned short wCurrentRate,
+	unsigned char byFBOption
+);
 
 static
 void
 s_vGenerateTxParameter(
-    PSDevice         pDevice,
-    unsigned char byPktType,
-    void *           pTxBufHead,
-    void *           pvRrvTime,
-    void *           pvRTS,
-    void *           pvCTS,
-    unsigned int	cbFrameSize,
-    bool bNeedACK,
-    unsigned int	uDMAIdx,
-    PSEthernetHeader psEthHeader,
-    unsigned short wCurrentRate
-    );
-
-
+	PSDevice         pDevice,
+	unsigned char byPktType,
+	void *pTxBufHead,
+	void *pvRrvTime,
+	void *pvRTS,
+	void *pvCTS,
+	unsigned int	cbFrameSize,
+	bool bNeedACK,
+	unsigned int	uDMAIdx,
+	PSEthernetHeader psEthHeader,
+	unsigned short wCurrentRate
+);
 
 static void s_vFillFragParameter(
-    PSDevice pDevice,
-    unsigned char *pbyBuffer,
-    unsigned int	uTxType,
-    void *   pvtdCurr,
-    unsigned short wFragType,
-    unsigned int	cbReqCount
-    );
-
+	PSDevice pDevice,
+	unsigned char *pbyBuffer,
+	unsigned int	uTxType,
+	void *pvtdCurr,
+	unsigned short wFragType,
+	unsigned int	cbReqCount
+);
 
 static unsigned int
 s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyTxBufferAddr,
-	unsigned int cbFrameBodySize, unsigned int uDMAIdx, PSTxDesc pHeadTD,
-	PSEthernetHeader psEthHeader, unsigned char *pPacket, bool bNeedEncrypt,
-	PSKeyItem pTransmitKey, unsigned int uNodeIndex, unsigned int *puMACfragNum);
-
+		  unsigned int cbFrameBodySize, unsigned int uDMAIdx, PSTxDesc pHeadTD,
+		  PSEthernetHeader psEthHeader, unsigned char *pPacket, bool bNeedEncrypt,
+		  PSKeyItem pTransmitKey, unsigned int uNodeIndex, unsigned int *puMACfragNum);
 
 static
 unsigned int
-s_uFillDataHead (
-    PSDevice pDevice,
-    unsigned char byPktType,
-    void *   pTxDataHead,
-    unsigned int cbFrameLength,
-    unsigned int uDMAIdx,
-    bool bNeedAck,
-    unsigned int uFragIdx,
-    unsigned int cbLastFragmentSize,
-    unsigned int uMACfragNum,
-    unsigned char byFBOption,
-    unsigned short wCurrentRate
-    );
-
+s_uFillDataHead(
+	PSDevice pDevice,
+	unsigned char byPktType,
+	void *pTxDataHead,
+	unsigned int cbFrameLength,
+	unsigned int uDMAIdx,
+	bool bNeedAck,
+	unsigned int uFragIdx,
+	unsigned int cbLastFragmentSize,
+	unsigned int uMACfragNum,
+	unsigned char byFBOption,
+	unsigned short wCurrentRate
+);
 
 /*---------------------  Export Variables  --------------------------*/
 
-
-
 static
 void
-s_vFillTxKey (
-    PSDevice   pDevice,
-    unsigned char *pbyBuf,
-    unsigned char *pbyIVHead,
-    PSKeyItem  pTransmitKey,
-    unsigned char *pbyHdrBuf,
-    unsigned short wPayloadLen,
-    unsigned char *pMICHDR
-    )
+s_vFillTxKey(
+	PSDevice   pDevice,
+	unsigned char *pbyBuf,
+	unsigned char *pbyIVHead,
+	PSKeyItem  pTransmitKey,
+	unsigned char *pbyHdrBuf,
+	unsigned short wPayloadLen,
+	unsigned char *pMICHDR
+)
 {
-    unsigned long *pdwIV = (unsigned long *) pbyIVHead;
-    unsigned long *pdwExtIV = (unsigned long *) ((unsigned char *)pbyIVHead+4);
-    unsigned short wValue;
-    PS802_11Header  pMACHeader = (PS802_11Header)pbyHdrBuf;
-    unsigned long dwRevIVCounter;
-    unsigned char byKeyIndex = 0;
-
-
-
-    //Fill TXKEY
-    if (pTransmitKey == NULL)
-        return;
-
-    dwRevIVCounter = cpu_to_le32(pDevice->dwIVCounter);
-    *pdwIV = pDevice->dwIVCounter;
-    byKeyIndex = pTransmitKey->dwKeyIndex & 0xf;
-
-    if (pTransmitKey->byCipherSuite == KEY_CTL_WEP) {
-        if (pTransmitKey->uKeyLength == WLAN_WEP232_KEYLEN ){
-            memcpy(pDevice->abyPRNG, (unsigned char *)&(dwRevIVCounter), 3);
-            memcpy(pDevice->abyPRNG+3, pTransmitKey->abyKey, pTransmitKey->uKeyLength);
-        } else {
-            memcpy(pbyBuf, (unsigned char *)&(dwRevIVCounter), 3);
-            memcpy(pbyBuf+3, pTransmitKey->abyKey, pTransmitKey->uKeyLength);
-            if(pTransmitKey->uKeyLength == WLAN_WEP40_KEYLEN) {
-                memcpy(pbyBuf+8, (unsigned char *)&(dwRevIVCounter), 3);
-                memcpy(pbyBuf+11, pTransmitKey->abyKey, pTransmitKey->uKeyLength);
-            }
-            memcpy(pDevice->abyPRNG, pbyBuf, 16);
-        }
-        // Append IV after Mac Header
-        *pdwIV &= WEP_IV_MASK;//00000000 11111111 11111111 11111111
-        *pdwIV |= (unsigned long)byKeyIndex << 30;
-        *pdwIV = cpu_to_le32(*pdwIV);
-        pDevice->dwIVCounter++;
-        if (pDevice->dwIVCounter > WEP_IV_MASK) {
-            pDevice->dwIVCounter = 0;
-        }
-    } else if (pTransmitKey->byCipherSuite == KEY_CTL_TKIP) {
-        pTransmitKey->wTSC15_0++;
-        if (pTransmitKey->wTSC15_0 == 0) {
-            pTransmitKey->dwTSC47_16++;
-        }
-        TKIPvMixKey(pTransmitKey->abyKey, pDevice->abyCurrentNetAddr,
-                    pTransmitKey->wTSC15_0, pTransmitKey->dwTSC47_16, pDevice->abyPRNG);
-        memcpy(pbyBuf, pDevice->abyPRNG, 16);
-        // Make IV
-        memcpy(pdwIV, pDevice->abyPRNG, 3);
-
-        *(pbyIVHead+3) = (unsigned char)(((byKeyIndex << 6) & 0xc0) | 0x20); // 0x20 is ExtIV
-        // Append IV&ExtIV after Mac Header
-        *pdwExtIV = cpu_to_le32(pTransmitKey->dwTSC47_16);
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"vFillTxKey()---- pdwExtIV: %lx\n", *pdwExtIV);
-
-    } else if (pTransmitKey->byCipherSuite == KEY_CTL_CCMP) {
-        pTransmitKey->wTSC15_0++;
-        if (pTransmitKey->wTSC15_0 == 0) {
-            pTransmitKey->dwTSC47_16++;
-        }
-        memcpy(pbyBuf, pTransmitKey->abyKey, 16);
-
-        // Make IV
-        *pdwIV = 0;
-        *(pbyIVHead+3) = (unsigned char)(((byKeyIndex << 6) & 0xc0) | 0x20); // 0x20 is ExtIV
-        *pdwIV |= cpu_to_le16((unsigned short)(pTransmitKey->wTSC15_0));
-        //Append IV&ExtIV after Mac Header
-        *pdwExtIV = cpu_to_le32(pTransmitKey->dwTSC47_16);
-
-        //Fill MICHDR0
-        *pMICHDR = 0x59;
-        *((unsigned char *)(pMICHDR+1)) = 0; // TxPriority
-        memcpy(pMICHDR+2, &(pMACHeader->abyAddr2[0]), 6);
-        *((unsigned char *)(pMICHDR+8)) = HIBYTE(HIWORD(pTransmitKey->dwTSC47_16));
-        *((unsigned char *)(pMICHDR+9)) = LOBYTE(HIWORD(pTransmitKey->dwTSC47_16));
-        *((unsigned char *)(pMICHDR+10)) = HIBYTE(LOWORD(pTransmitKey->dwTSC47_16));
-        *((unsigned char *)(pMICHDR+11)) = LOBYTE(LOWORD(pTransmitKey->dwTSC47_16));
-        *((unsigned char *)(pMICHDR+12)) = HIBYTE(pTransmitKey->wTSC15_0);
-        *((unsigned char *)(pMICHDR+13)) = LOBYTE(pTransmitKey->wTSC15_0);
-        *((unsigned char *)(pMICHDR+14)) = HIBYTE(wPayloadLen);
-        *((unsigned char *)(pMICHDR+15)) = LOBYTE(wPayloadLen);
-
-        //Fill MICHDR1
-        *((unsigned char *)(pMICHDR+16)) = 0; // HLEN[15:8]
-        if (pDevice->bLongHeader) {
-            *((unsigned char *)(pMICHDR+17)) = 28; // HLEN[7:0]
-        } else {
-            *((unsigned char *)(pMICHDR+17)) = 22; // HLEN[7:0]
-        }
-        wValue = cpu_to_le16(pMACHeader->wFrameCtl & 0xC78F);
-        memcpy(pMICHDR+18, (unsigned char *)&wValue, 2); // MSKFRACTL
-        memcpy(pMICHDR+20, &(pMACHeader->abyAddr1[0]), 6);
-        memcpy(pMICHDR+26, &(pMACHeader->abyAddr2[0]), 6);
-
-        //Fill MICHDR2
-        memcpy(pMICHDR+32, &(pMACHeader->abyAddr3[0]), 6);
-        wValue = pMACHeader->wSeqCtl;
-        wValue &= 0x000F;
-        wValue = cpu_to_le16(wValue);
-        memcpy(pMICHDR+38, (unsigned char *)&wValue, 2); // MSKSEQCTL
-        if (pDevice->bLongHeader) {
-            memcpy(pMICHDR+40, &(pMACHeader->abyAddr4[0]), 6);
-        }
-    }
+	unsigned long *pdwIV = (unsigned long *)pbyIVHead;
+	unsigned long *pdwExtIV = (unsigned long *)((unsigned char *)pbyIVHead+4);
+	unsigned short wValue;
+	PS802_11Header  pMACHeader = (PS802_11Header)pbyHdrBuf;
+	unsigned long dwRevIVCounter;
+	unsigned char byKeyIndex = 0;
+
+	//Fill TXKEY
+	if (pTransmitKey == NULL)
+		return;
+
+	dwRevIVCounter = cpu_to_le32(pDevice->dwIVCounter);
+	*pdwIV = pDevice->dwIVCounter;
+	byKeyIndex = pTransmitKey->dwKeyIndex & 0xf;
+
+	if (pTransmitKey->byCipherSuite == KEY_CTL_WEP) {
+		if (pTransmitKey->uKeyLength == WLAN_WEP232_KEYLEN) {
+			memcpy(pDevice->abyPRNG, (unsigned char *)&(dwRevIVCounter), 3);
+			memcpy(pDevice->abyPRNG+3, pTransmitKey->abyKey, pTransmitKey->uKeyLength);
+		} else {
+			memcpy(pbyBuf, (unsigned char *)&(dwRevIVCounter), 3);
+			memcpy(pbyBuf+3, pTransmitKey->abyKey, pTransmitKey->uKeyLength);
+			if (pTransmitKey->uKeyLength == WLAN_WEP40_KEYLEN) {
+				memcpy(pbyBuf+8, (unsigned char *)&(dwRevIVCounter), 3);
+				memcpy(pbyBuf+11, pTransmitKey->abyKey, pTransmitKey->uKeyLength);
+			}
+			memcpy(pDevice->abyPRNG, pbyBuf, 16);
+		}
+		// Append IV after Mac Header
+		*pdwIV &= WEP_IV_MASK;//00000000 11111111 11111111 11111111
+		*pdwIV |= (unsigned long)byKeyIndex << 30;
+		*pdwIV = cpu_to_le32(*pdwIV);
+		pDevice->dwIVCounter++;
+		if (pDevice->dwIVCounter > WEP_IV_MASK) {
+			pDevice->dwIVCounter = 0;
+		}
+	} else if (pTransmitKey->byCipherSuite == KEY_CTL_TKIP) {
+		pTransmitKey->wTSC15_0++;
+		if (pTransmitKey->wTSC15_0 == 0) {
+			pTransmitKey->dwTSC47_16++;
+		}
+		TKIPvMixKey(pTransmitKey->abyKey, pDevice->abyCurrentNetAddr,
+			    pTransmitKey->wTSC15_0, pTransmitKey->dwTSC47_16, pDevice->abyPRNG);
+		memcpy(pbyBuf, pDevice->abyPRNG, 16);
+		// Make IV
+		memcpy(pdwIV, pDevice->abyPRNG, 3);
+
+		*(pbyIVHead+3) = (unsigned char)(((byKeyIndex << 6) & 0xc0) | 0x20); // 0x20 is ExtIV
+		// Append IV&ExtIV after Mac Header
+		*pdwExtIV = cpu_to_le32(pTransmitKey->dwTSC47_16);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "vFillTxKey()---- pdwExtIV: %lx\n", *pdwExtIV);
+
+	} else if (pTransmitKey->byCipherSuite == KEY_CTL_CCMP) {
+		pTransmitKey->wTSC15_0++;
+		if (pTransmitKey->wTSC15_0 == 0) {
+			pTransmitKey->dwTSC47_16++;
+		}
+		memcpy(pbyBuf, pTransmitKey->abyKey, 16);
+
+		// Make IV
+		*pdwIV = 0;
+		*(pbyIVHead+3) = (unsigned char)(((byKeyIndex << 6) & 0xc0) | 0x20); // 0x20 is ExtIV
+		*pdwIV |= cpu_to_le16((unsigned short)(pTransmitKey->wTSC15_0));
+		//Append IV&ExtIV after Mac Header
+		*pdwExtIV = cpu_to_le32(pTransmitKey->dwTSC47_16);
+
+		//Fill MICHDR0
+		*pMICHDR = 0x59;
+		*((unsigned char *)(pMICHDR+1)) = 0; // TxPriority
+		memcpy(pMICHDR+2, &(pMACHeader->abyAddr2[0]), 6);
+		*((unsigned char *)(pMICHDR+8)) = HIBYTE(HIWORD(pTransmitKey->dwTSC47_16));
+		*((unsigned char *)(pMICHDR+9)) = LOBYTE(HIWORD(pTransmitKey->dwTSC47_16));
+		*((unsigned char *)(pMICHDR+10)) = HIBYTE(LOWORD(pTransmitKey->dwTSC47_16));
+		*((unsigned char *)(pMICHDR+11)) = LOBYTE(LOWORD(pTransmitKey->dwTSC47_16));
+		*((unsigned char *)(pMICHDR+12)) = HIBYTE(pTransmitKey->wTSC15_0);
+		*((unsigned char *)(pMICHDR+13)) = LOBYTE(pTransmitKey->wTSC15_0);
+		*((unsigned char *)(pMICHDR+14)) = HIBYTE(wPayloadLen);
+		*((unsigned char *)(pMICHDR+15)) = LOBYTE(wPayloadLen);
+
+		//Fill MICHDR1
+		*((unsigned char *)(pMICHDR+16)) = 0; // HLEN[15:8]
+		if (pDevice->bLongHeader) {
+			*((unsigned char *)(pMICHDR+17)) = 28; // HLEN[7:0]
+		} else {
+			*((unsigned char *)(pMICHDR+17)) = 22; // HLEN[7:0]
+		}
+		wValue = cpu_to_le16(pMACHeader->wFrameCtl & 0xC78F);
+		memcpy(pMICHDR+18, (unsigned char *)&wValue, 2); // MSKFRACTL
+		memcpy(pMICHDR+20, &(pMACHeader->abyAddr1[0]), 6);
+		memcpy(pMICHDR+26, &(pMACHeader->abyAddr2[0]), 6);
+
+		//Fill MICHDR2
+		memcpy(pMICHDR+32, &(pMACHeader->abyAddr3[0]), 6);
+		wValue = pMACHeader->wSeqCtl;
+		wValue &= 0x000F;
+		wValue = cpu_to_le16(wValue);
+		memcpy(pMICHDR+38, (unsigned char *)&wValue, 2); // MSKSEQCTL
+		if (pDevice->bLongHeader) {
+			memcpy(pMICHDR+40, &(pMACHeader->abyAddr4[0]), 6);
+		}
+	}
 }
 
-
 static
 void
-s_vSWencryption (
-    PSDevice            pDevice,
-    PSKeyItem           pTransmitKey,
-    unsigned char *pbyPayloadHead,
-    unsigned short wPayloadSize
-    )
+s_vSWencryption(
+	PSDevice            pDevice,
+	PSKeyItem           pTransmitKey,
+	unsigned char *pbyPayloadHead,
+	unsigned short wPayloadSize
+)
 {
-    unsigned int cbICVlen = 4;
-    unsigned long dwICV = 0xFFFFFFFFL;
-    unsigned long *pdwICV;
-
-    if (pTransmitKey == NULL)
-        return;
-
-    if (pTransmitKey->byCipherSuite == KEY_CTL_WEP) {
-        //=======================================================================
-        // Append ICV after payload
-        dwICV = CRCdwGetCrc32Ex(pbyPayloadHead, wPayloadSize, dwICV);//ICV(Payload)
-        pdwICV = (unsigned long *)(pbyPayloadHead + wPayloadSize);
-        // finally, we must invert dwCRC to get the correct answer
-        *pdwICV = cpu_to_le32(~dwICV);
-        // RC4 encryption
-        rc4_init(&pDevice->SBox, pDevice->abyPRNG, pTransmitKey->uKeyLength + 3);
-        rc4_encrypt(&pDevice->SBox, pbyPayloadHead, pbyPayloadHead, wPayloadSize+cbICVlen);
-        //=======================================================================
-    } else if (pTransmitKey->byCipherSuite == KEY_CTL_TKIP) {
-        //=======================================================================
-        //Append ICV after payload
-        dwICV = CRCdwGetCrc32Ex(pbyPayloadHead, wPayloadSize, dwICV);//ICV(Payload)
-        pdwICV = (unsigned long *)(pbyPayloadHead + wPayloadSize);
-        // finally, we must invert dwCRC to get the correct answer
-        *pdwICV = cpu_to_le32(~dwICV);
-        // RC4 encryption
-        rc4_init(&pDevice->SBox, pDevice->abyPRNG, TKIP_KEY_LEN);
-        rc4_encrypt(&pDevice->SBox, pbyPayloadHead, pbyPayloadHead, wPayloadSize+cbICVlen);
-        //=======================================================================
-    }
+	unsigned int cbICVlen = 4;
+	unsigned long dwICV = 0xFFFFFFFFL;
+	unsigned long *pdwICV;
+
+	if (pTransmitKey == NULL)
+		return;
+
+	if (pTransmitKey->byCipherSuite == KEY_CTL_WEP) {
+		//=======================================================================
+		// Append ICV after payload
+		dwICV = CRCdwGetCrc32Ex(pbyPayloadHead, wPayloadSize, dwICV);//ICV(Payload)
+		pdwICV = (unsigned long *)(pbyPayloadHead + wPayloadSize);
+		// finally, we must invert dwCRC to get the correct answer
+		*pdwICV = cpu_to_le32(~dwICV);
+		// RC4 encryption
+		rc4_init(&pDevice->SBox, pDevice->abyPRNG, pTransmitKey->uKeyLength + 3);
+		rc4_encrypt(&pDevice->SBox, pbyPayloadHead, pbyPayloadHead, wPayloadSize+cbICVlen);
+		//=======================================================================
+	} else if (pTransmitKey->byCipherSuite == KEY_CTL_TKIP) {
+		//=======================================================================
+		//Append ICV after payload
+		dwICV = CRCdwGetCrc32Ex(pbyPayloadHead, wPayloadSize, dwICV);//ICV(Payload)
+		pdwICV = (unsigned long *)(pbyPayloadHead + wPayloadSize);
+		// finally, we must invert dwCRC to get the correct answer
+		*pdwICV = cpu_to_le32(~dwICV);
+		// RC4 encryption
+		rc4_init(&pDevice->SBox, pDevice->abyPRNG, TKIP_KEY_LEN);
+		rc4_encrypt(&pDevice->SBox, pbyPayloadHead, pbyPayloadHead, wPayloadSize+cbICVlen);
+		//=======================================================================
+	}
 }
 
-
-
-
 /*byPktType : PK_TYPE_11A     0
-             PK_TYPE_11B     1
-             PK_TYPE_11GB    2
-             PK_TYPE_11GA    3
+  PK_TYPE_11B     1
+  PK_TYPE_11GB    2
+  PK_TYPE_11GA    3
 */
 static
 unsigned int
-s_uGetTxRsvTime (
-    PSDevice pDevice,
-    unsigned char byPktType,
-    unsigned int cbFrameLength,
-    unsigned short wRate,
-    bool bNeedAck
-    )
+s_uGetTxRsvTime(
+	PSDevice pDevice,
+	unsigned char byPktType,
+	unsigned int cbFrameLength,
+	unsigned short wRate,
+	bool bNeedAck
+)
 {
-    unsigned int uDataTime, uAckTime;
-
-    uDataTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, cbFrameLength, wRate);
-#ifdef	PLICE_DEBUG
-	//printk("s_uGetTxRsvTime is %d\n",uDataTime);
-#endif
-    if (byPktType == PK_TYPE_11B) {//llb,CCK mode
-        uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, (unsigned short)pDevice->byTopCCKBasicRate);
-    } else {//11g 2.4G OFDM mode & 11a 5G OFDM mode
-        uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, (unsigned short)pDevice->byTopOFDMBasicRate);
-    }
-
-    if (bNeedAck) {
-        return (uDataTime + pDevice->uSIFS + uAckTime);
-    }
-    else {
-        return uDataTime;
-    }
+	unsigned int uDataTime, uAckTime;
+
+	uDataTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, cbFrameLength, wRate);
+	if (byPktType == PK_TYPE_11B) {//llb,CCK mode
+		uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, (unsigned short)pDevice->byTopCCKBasicRate);
+	} else {//11g 2.4G OFDM mode & 11a 5G OFDM mode
+		uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, (unsigned short)pDevice->byTopOFDMBasicRate);
+	}
+
+	if (bNeedAck) {
+		return uDataTime + pDevice->uSIFS + uAckTime;
+	} else {
+		return uDataTime;
+	}
 }
 
 //byFreqType: 0=>5GHZ 1=>2.4GHZ
 static
 unsigned int
-s_uGetRTSCTSRsvTime (
-    PSDevice pDevice,
-    unsigned char byRTSRsvType,
-    unsigned char byPktType,
-    unsigned int cbFrameLength,
-    unsigned short wCurrentRate
-    )
+s_uGetRTSCTSRsvTime(
+	PSDevice pDevice,
+	unsigned char byRTSRsvType,
+	unsigned char byPktType,
+	unsigned int cbFrameLength,
+	unsigned short wCurrentRate
+)
 {
-    unsigned int uRrvTime  , uRTSTime, uCTSTime, uAckTime, uDataTime;
-
-    uRrvTime = uRTSTime = uCTSTime = uAckTime = uDataTime = 0;
-
-
-    uDataTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, cbFrameLength, wCurrentRate);
-    if (byRTSRsvType == 0) { //RTSTxRrvTime_bb
-        uRTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 20, pDevice->byTopCCKBasicRate);
-        uCTSTime = uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
-    }
-    else if (byRTSRsvType == 1){ //RTSTxRrvTime_ba, only in 2.4GHZ
-        uRTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 20, pDevice->byTopCCKBasicRate);
-        uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
-        uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
-    }
-    else if (byRTSRsvType == 2) { //RTSTxRrvTime_aa
-        uRTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 20, pDevice->byTopOFDMBasicRate);
-        uCTSTime = uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
-    }
-    else if (byRTSRsvType == 3) { //CTSTxRrvTime_ba, only in 2.4GHZ
-        uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
-        uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
-        uRrvTime = uCTSTime + uAckTime + uDataTime + 2*pDevice->uSIFS;
-        return uRrvTime;
-    }
-
-    //RTSRrvTime
-    uRrvTime = uRTSTime + uCTSTime + uAckTime + uDataTime + 3*pDevice->uSIFS;
-    return uRrvTime;
+	unsigned int uRrvTime  , uRTSTime, uCTSTime, uAckTime, uDataTime;
+
+	uRrvTime = uRTSTime = uCTSTime = uAckTime = uDataTime = 0;
+
+	uDataTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, cbFrameLength, wCurrentRate);
+	if (byRTSRsvType == 0) { //RTSTxRrvTime_bb
+		uRTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 20, pDevice->byTopCCKBasicRate);
+		uCTSTime = uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
+	} else if (byRTSRsvType == 1) { //RTSTxRrvTime_ba, only in 2.4GHZ
+		uRTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 20, pDevice->byTopCCKBasicRate);
+		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
+		uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
+	} else if (byRTSRsvType == 2) { //RTSTxRrvTime_aa
+		uRTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 20, pDevice->byTopOFDMBasicRate);
+		uCTSTime = uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
+	} else if (byRTSRsvType == 3) { //CTSTxRrvTime_ba, only in 2.4GHZ
+		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
+		uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
+		uRrvTime = uCTSTime + uAckTime + uDataTime + 2*pDevice->uSIFS;
+		return uRrvTime;
+	}
+
+	//RTSRrvTime
+	uRrvTime = uRTSTime + uCTSTime + uAckTime + uDataTime + 3*pDevice->uSIFS;
+	return uRrvTime;
 }
 
 //byFreqType 0: 5GHz, 1:2.4Ghz
 static
 unsigned int
-s_uGetDataDuration (
-    PSDevice pDevice,
-    unsigned char byDurType,
-    unsigned int cbFrameLength,
-    unsigned char byPktType,
-    unsigned short wRate,
-    bool bNeedAck,
-    unsigned int uFragIdx,
-    unsigned int cbLastFragmentSize,
-    unsigned int uMACfragNum,
-    unsigned char byFBOption
-    )
+s_uGetDataDuration(
+	PSDevice pDevice,
+	unsigned char byDurType,
+	unsigned int cbFrameLength,
+	unsigned char byPktType,
+	unsigned short wRate,
+	bool bNeedAck,
+	unsigned int uFragIdx,
+	unsigned int cbLastFragmentSize,
+	unsigned int uMACfragNum,
+	unsigned char byFBOption
+)
 {
-    bool bLastFrag = 0;
-    unsigned int uAckTime =0, uNextPktTime = 0;
-
-
-
-    if (uFragIdx == (uMACfragNum-1)) {
-        bLastFrag = 1;
-    }
-
-
-    switch (byDurType) {
-
-    case DATADUR_B:    //DATADUR_B
-        if (((uMACfragNum == 1)) || (bLastFrag == 1)) {//Non Frag or Last Frag
-            if (bNeedAck) {
-            	uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
-                return (pDevice->uSIFS + uAckTime);
-            } else {
-                return 0;
-            }
-        }
-        else {//First Frag or Mid Frag
-            if (uFragIdx == (uMACfragNum-2)) {
-            	uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wRate, bNeedAck);
-            } else {
-                uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
-            }
-            if (bNeedAck) {
-            	uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
-                return (pDevice->uSIFS + uAckTime + uNextPktTime);
-            } else {
-                return (pDevice->uSIFS + uNextPktTime);
-            }
-        }
-        break;
-
-    case DATADUR_A:    //DATADUR_A
-        if (((uMACfragNum==1)) || (bLastFrag==1)) {//Non Frag or Last Frag
-            if(bNeedAck){
-            	uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
-                return (pDevice->uSIFS + uAckTime);
-            } else {
-                return 0;
-            }
-        }
-        else {//First Frag or Mid Frag
-            if(uFragIdx == (uMACfragNum-2)){
-            	uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wRate, bNeedAck);
-            } else {
-                uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
-            }
-            if(bNeedAck){
-            	uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
-                return (pDevice->uSIFS + uAckTime + uNextPktTime);
-            } else {
-                return (pDevice->uSIFS + uNextPktTime);
-            }
-        }
-        break;
-
-    case DATADUR_A_F0:    //DATADUR_A_F0
-	    if (((uMACfragNum==1)) || (bLastFrag==1)) {//Non Frag or Last Frag
-            if(bNeedAck){
-            	uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
-                return (pDevice->uSIFS + uAckTime);
-            } else {
-                return 0;
-            }
-        }
-	    else { //First Frag or Mid Frag
-	        if (byFBOption == AUTO_FB_0) {
-                if (wRate < RATE_18M)
-                    wRate = RATE_18M;
-                else if (wRate > RATE_54M)
-                    wRate = RATE_54M;
-
-	            if(uFragIdx == (uMACfragNum-2)){
-            	    uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wFB_Opt0[FB_RATE0][wRate-RATE_18M], bNeedAck);
-                } else {
-                    uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE0][wRate-RATE_18M], bNeedAck);
-                }
-	        } else { // (byFBOption == AUTO_FB_1)
-                if (wRate < RATE_18M)
-                    wRate = RATE_18M;
-                else if (wRate > RATE_54M)
-                    wRate = RATE_54M;
-
-	            if(uFragIdx == (uMACfragNum-2)){
-            	    uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wFB_Opt1[FB_RATE0][wRate-RATE_18M], bNeedAck);
-                } else {
-                    uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE0][wRate-RATE_18M], bNeedAck);
-                }
-	        }
-
-	        if(bNeedAck){
-            	uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
-                return (pDevice->uSIFS + uAckTime + uNextPktTime);
-            } else {
-                return (pDevice->uSIFS + uNextPktTime);
-            }
-	    }
-        break;
-
-    case DATADUR_A_F1:    //DATADUR_A_F1
-        if (((uMACfragNum==1)) || (bLastFrag==1)) {//Non Frag or Last Frag
-            if(bNeedAck){
-            	uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
-                return (pDevice->uSIFS + uAckTime);
-            } else {
-                return 0;
-            }
-        }
-	    else { //First Frag or Mid Frag
-	        if (byFBOption == AUTO_FB_0) {
-                if (wRate < RATE_18M)
-                    wRate = RATE_18M;
-                else if (wRate > RATE_54M)
-                    wRate = RATE_54M;
-
-	            if(uFragIdx == (uMACfragNum-2)){
-            	    uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wFB_Opt0[FB_RATE1][wRate-RATE_18M], bNeedAck);
-                } else {
-                    uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE1][wRate-RATE_18M], bNeedAck);
-                }
-
-	        } else { // (byFBOption == AUTO_FB_1)
-                if (wRate < RATE_18M)
-                    wRate = RATE_18M;
-                else if (wRate > RATE_54M)
-                    wRate = RATE_54M;
-
-	            if(uFragIdx == (uMACfragNum-2)){
-            	    uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wFB_Opt1[FB_RATE1][wRate-RATE_18M], bNeedAck);
-                } else {
-                    uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE1][wRate-RATE_18M], bNeedAck);
-                }
-	        }
-	        if(bNeedAck){
-            	uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
-                return (pDevice->uSIFS + uAckTime + uNextPktTime);
-            } else {
-                return (pDevice->uSIFS + uNextPktTime);
-            }
-	    }
-        break;
-
-    default:
-        break;
-    }
+	bool bLastFrag = 0;
+	unsigned int uAckTime = 0, uNextPktTime = 0;
+
+	if (uFragIdx == (uMACfragNum-1)) {
+		bLastFrag = 1;
+	}
+
+	switch (byDurType) {
+	case DATADUR_B:    //DATADUR_B
+		if (((uMACfragNum == 1)) || (bLastFrag == 1)) {//Non Frag or Last Frag
+			if (bNeedAck) {
+				uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
+				return pDevice->uSIFS + uAckTime;
+			} else {
+				return 0;
+			}
+		} else {//First Frag or Mid Frag
+			if (uFragIdx == (uMACfragNum-2)) {
+				uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wRate, bNeedAck);
+			} else {
+				uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
+			}
+			if (bNeedAck) {
+				uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
+				return pDevice->uSIFS + uAckTime + uNextPktTime;
+			} else {
+				return pDevice->uSIFS + uNextPktTime;
+			}
+		}
+		break;
+
+	case DATADUR_A:    //DATADUR_A
+		if (((uMACfragNum == 1)) || (bLastFrag == 1)) {//Non Frag or Last Frag
+			if (bNeedAck) {
+				uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
+				return pDevice->uSIFS + uAckTime;
+			} else {
+				return 0;
+			}
+		} else {//First Frag or Mid Frag
+			if (uFragIdx == (uMACfragNum-2)) {
+				uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wRate, bNeedAck);
+			} else {
+				uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
+			}
+			if (bNeedAck) {
+				uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
+				return pDevice->uSIFS + uAckTime + uNextPktTime;
+			} else {
+				return pDevice->uSIFS + uNextPktTime;
+			}
+		}
+		break;
+
+	case DATADUR_A_F0:    //DATADUR_A_F0
+		if (((uMACfragNum == 1)) || (bLastFrag == 1)) {//Non Frag or Last Frag
+			if (bNeedAck) {
+				uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
+				return pDevice->uSIFS + uAckTime;
+			} else {
+				return 0;
+			}
+		} else { //First Frag or Mid Frag
+			if (byFBOption == AUTO_FB_0) {
+				if (wRate < RATE_18M)
+					wRate = RATE_18M;
+				else if (wRate > RATE_54M)
+					wRate = RATE_54M;
+
+				if (uFragIdx == (uMACfragNum-2)) {
+					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wFB_Opt0[FB_RATE0][wRate-RATE_18M], bNeedAck);
+				} else {
+					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE0][wRate-RATE_18M], bNeedAck);
+				}
+			} else { // (byFBOption == AUTO_FB_1)
+				if (wRate < RATE_18M)
+					wRate = RATE_18M;
+				else if (wRate > RATE_54M)
+					wRate = RATE_54M;
+
+				if (uFragIdx == (uMACfragNum-2)) {
+					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wFB_Opt1[FB_RATE0][wRate-RATE_18M], bNeedAck);
+				} else {
+					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE0][wRate-RATE_18M], bNeedAck);
+				}
+			}
+
+			if (bNeedAck) {
+				uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
+				return pDevice->uSIFS + uAckTime + uNextPktTime;
+			} else {
+				return pDevice->uSIFS + uNextPktTime;
+			}
+		}
+		break;
+
+	case DATADUR_A_F1:    //DATADUR_A_F1
+		if (((uMACfragNum == 1)) || (bLastFrag == 1)) {//Non Frag or Last Frag
+			if (bNeedAck) {
+				uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
+				return pDevice->uSIFS + uAckTime;
+			} else {
+				return 0;
+			}
+		} else { //First Frag or Mid Frag
+			if (byFBOption == AUTO_FB_0) {
+				if (wRate < RATE_18M)
+					wRate = RATE_18M;
+				else if (wRate > RATE_54M)
+					wRate = RATE_54M;
+
+				if (uFragIdx == (uMACfragNum-2)) {
+					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wFB_Opt0[FB_RATE1][wRate-RATE_18M], bNeedAck);
+				} else {
+					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE1][wRate-RATE_18M], bNeedAck);
+				}
+
+			} else { // (byFBOption == AUTO_FB_1)
+				if (wRate < RATE_18M)
+					wRate = RATE_18M;
+				else if (wRate > RATE_54M)
+					wRate = RATE_54M;
+
+				if (uFragIdx == (uMACfragNum-2)) {
+					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wFB_Opt1[FB_RATE1][wRate-RATE_18M], bNeedAck);
+				} else {
+					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE1][wRate-RATE_18M], bNeedAck);
+				}
+			}
+			if (bNeedAck) {
+				uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
+				return pDevice->uSIFS + uAckTime + uNextPktTime;
+			} else {
+				return pDevice->uSIFS + uNextPktTime;
+			}
+		}
+		break;
+
+	default:
+		break;
+	}
 
 	ASSERT(false);
 	return 0;
 }
 
-
 //byFreqType: 0=>5GHZ 1=>2.4GHZ
 static
 unsigned int
-s_uGetRTSCTSDuration (
-    PSDevice pDevice,
-    unsigned char byDurType,
-    unsigned int cbFrameLength,
-    unsigned char byPktType,
-    unsigned short wRate,
-    bool bNeedAck,
-    unsigned char byFBOption
-    )
+s_uGetRTSCTSDuration(
+	PSDevice pDevice,
+	unsigned char byDurType,
+	unsigned int cbFrameLength,
+	unsigned char byPktType,
+	unsigned short wRate,
+	bool bNeedAck,
+	unsigned char byFBOption
+)
 {
-    unsigned int uCTSTime = 0, uDurTime = 0;
-
-
-    switch (byDurType) {
-
-    case RTSDUR_BB:    //RTSDuration_bb
-        uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
-        uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
-        break;
-
-    case RTSDUR_BA:    //RTSDuration_ba
-        uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
-        uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
-        break;
-
-    case RTSDUR_AA:    //RTSDuration_aa
-        uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
-        uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
-        break;
-
-    case CTSDUR_BA:    //CTSDuration_ba
-        uDurTime = pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
-        break;
-
-    case RTSDUR_BA_F0: //RTSDuration_ba_f0
-        uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
-        if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <=RATE_54M)) {
-            uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE0][wRate-RATE_18M], bNeedAck);
-        } else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <=RATE_54M)) {
-            uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE0][wRate-RATE_18M], bNeedAck);
-        }
-        break;
-
-    case RTSDUR_AA_F0: //RTSDuration_aa_f0
-        uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
-        if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <=RATE_54M)) {
-            uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE0][wRate-RATE_18M], bNeedAck);
-        } else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <=RATE_54M)) {
-            uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE0][wRate-RATE_18M], bNeedAck);
-        }
-        break;
-
-    case RTSDUR_BA_F1: //RTSDuration_ba_f1
-        uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
-        if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <=RATE_54M)) {
-            uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE1][wRate-RATE_18M], bNeedAck);
-        } else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <=RATE_54M)) {
-            uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE1][wRate-RATE_18M], bNeedAck);
-        }
-        break;
-
-    case RTSDUR_AA_F1: //RTSDuration_aa_f1
-        uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
-        if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <=RATE_54M)) {
-            uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE1][wRate-RATE_18M], bNeedAck);
-        } else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <=RATE_54M)) {
-            uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE1][wRate-RATE_18M], bNeedAck);
-        }
-        break;
-
-    case CTSDUR_BA_F0: //CTSDuration_ba_f0
-        if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <=RATE_54M)) {
-            uDurTime = pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE0][wRate-RATE_18M], bNeedAck);
-        } else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <=RATE_54M)) {
-            uDurTime = pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE0][wRate-RATE_18M], bNeedAck);
-        }
-        break;
-
-    case CTSDUR_BA_F1: //CTSDuration_ba_f1
-        if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <=RATE_54M)) {
-            uDurTime = pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE1][wRate-RATE_18M], bNeedAck);
-        } else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <=RATE_54M)) {
-            uDurTime = pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE1][wRate-RATE_18M], bNeedAck);
-        }
-        break;
-
-    default:
-        break;
-    }
-
-    return uDurTime;
-
+	unsigned int uCTSTime = 0, uDurTime = 0;
+
+	switch (byDurType) {
+	case RTSDUR_BB:    //RTSDuration_bb
+		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
+		uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
+		break;
+
+	case RTSDUR_BA:    //RTSDuration_ba
+		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
+		uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
+		break;
+
+	case RTSDUR_AA:    //RTSDuration_aa
+		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
+		uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
+		break;
+
+	case CTSDUR_BA:    //CTSDuration_ba
+		uDurTime = pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
+		break;
+
+	case RTSDUR_BA_F0: //RTSDuration_ba_f0
+		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
+		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M)) {
+			uDurTime = uCTSTime + 2 * pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE0][wRate-RATE_18M], bNeedAck);
+		} else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M)) {
+			uDurTime = uCTSTime + 2 * pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE0][wRate-RATE_18M], bNeedAck);
+		}
+		break;
+
+	case RTSDUR_AA_F0: //RTSDuration_aa_f0
+		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
+		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M)) {
+			uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE0][wRate-RATE_18M], bNeedAck);
+		} else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M)) {
+			uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE0][wRate-RATE_18M], bNeedAck);
+		}
+		break;
+
+	case RTSDUR_BA_F1: //RTSDuration_ba_f1
+		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
+		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M)) {
+			uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE1][wRate-RATE_18M], bNeedAck);
+		} else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M)) {
+			uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE1][wRate-RATE_18M], bNeedAck);
+		}
+		break;
+
+	case RTSDUR_AA_F1: //RTSDuration_aa_f1
+		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
+		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M)) {
+			uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE1][wRate-RATE_18M], bNeedAck);
+		} else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M)) {
+			uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE1][wRate-RATE_18M], bNeedAck);
+		}
+		break;
+
+	case CTSDUR_BA_F0: //CTSDuration_ba_f0
+		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M)) {
+			uDurTime = pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE0][wRate-RATE_18M], bNeedAck);
+		} else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M)) {
+			uDurTime = pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE0][wRate-RATE_18M], bNeedAck);
+		}
+		break;
+
+	case CTSDUR_BA_F1: //CTSDuration_ba_f1
+		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M)) {
+			uDurTime = pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE1][wRate-RATE_18M], bNeedAck);
+		} else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M)) {
+			uDurTime = pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE1][wRate-RATE_18M], bNeedAck);
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	return uDurTime;
 }
 
-
-
 static
 unsigned int
-s_uFillDataHead (
-    PSDevice pDevice,
-    unsigned char byPktType,
-    void *   pTxDataHead,
-    unsigned int cbFrameLength,
-    unsigned int uDMAIdx,
-    bool bNeedAck,
-    unsigned int uFragIdx,
-    unsigned int cbLastFragmentSize,
-    unsigned int uMACfragNum,
-    unsigned char byFBOption,
-    unsigned short wCurrentRate
-    )
+s_uFillDataHead(
+	PSDevice pDevice,
+	unsigned char byPktType,
+	void *pTxDataHead,
+	unsigned int cbFrameLength,
+	unsigned int uDMAIdx,
+	bool bNeedAck,
+	unsigned int uFragIdx,
+	unsigned int cbLastFragmentSize,
+	unsigned int uMACfragNum,
+	unsigned char byFBOption,
+	unsigned short wCurrentRate
+)
 {
-    unsigned short wLen = 0x0000;
-
-    if (pTxDataHead == NULL) {
-        return 0;
-    }
-
-    if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
-        if (byFBOption == AUTO_FB_NONE) {
-            PSTxDataHead_g pBuf = (PSTxDataHead_g)pTxDataHead;
-            //Get SignalField,ServiceField,Length
-            BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
-                (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_a), (unsigned char *)&(pBuf->bySignalField_a)
-            );
-            pBuf->wTransmitLength_a = cpu_to_le16(wLen);
-            BBvCalculateParameter(pDevice, cbFrameLength, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
-                (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_b), (unsigned char *)&(pBuf->bySignalField_b)
-            );
-            pBuf->wTransmitLength_b = cpu_to_le16(wLen);
-            //Get Duration and TimeStamp
-            pBuf->wDuration_a = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength,
-                                                         byPktType, wCurrentRate, bNeedAck, uFragIdx,
-                                                         cbLastFragmentSize, uMACfragNum,
-                                                         byFBOption)); //1: 2.4GHz
-            pBuf->wDuration_b = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameLength,
-                                                         PK_TYPE_11B, pDevice->byTopCCKBasicRate,
-                                                         bNeedAck, uFragIdx, cbLastFragmentSize,
-                                                         uMACfragNum, byFBOption)); //1: 2.4
-
-            pBuf->wTimeStampOff_a = cpu_to_le16(wTimeStampOff[pDevice->byPreambleType%2][wCurrentRate%MAX_RATE]);
-            pBuf->wTimeStampOff_b = cpu_to_le16(wTimeStampOff[pDevice->byPreambleType%2][pDevice->byTopCCKBasicRate%MAX_RATE]);
-
-            return (pBuf->wDuration_a);
-         } else {
-            // Auto Fallback
-            PSTxDataHead_g_FB pBuf = (PSTxDataHead_g_FB)pTxDataHead;
-            //Get SignalField,ServiceField,Length
-            BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
-                (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_a), (unsigned char *)&(pBuf->bySignalField_a)
-            );
-            pBuf->wTransmitLength_a = cpu_to_le16(wLen);
-            BBvCalculateParameter(pDevice, cbFrameLength, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
-                (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_b), (unsigned char *)&(pBuf->bySignalField_b)
-            );
-            pBuf->wTransmitLength_b = cpu_to_le16(wLen);
-            //Get Duration and TimeStamp
-            pBuf->wDuration_a = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength, byPktType,
-                                         wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption)); //1: 2.4GHz
-            pBuf->wDuration_b = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameLength, PK_TYPE_11B,
-                                         pDevice->byTopCCKBasicRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption)); //1: 2.4GHz
-            pBuf->wDuration_a_f0 = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A_F0, cbFrameLength, byPktType,
-                                         wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption)); //1: 2.4GHz
-            pBuf->wDuration_a_f1 = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A_F1, cbFrameLength, byPktType,
-                                         wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption)); //1: 2.4GHz
-
-            pBuf->wTimeStampOff_a = cpu_to_le16(wTimeStampOff[pDevice->byPreambleType%2][wCurrentRate%MAX_RATE]);
-            pBuf->wTimeStampOff_b = cpu_to_le16(wTimeStampOff[pDevice->byPreambleType%2][pDevice->byTopCCKBasicRate%MAX_RATE]);
-
-            return (pBuf->wDuration_a);
-        } //if (byFBOption == AUTO_FB_NONE)
-    }
-    else if (byPktType == PK_TYPE_11A) {
-        if ((byFBOption != AUTO_FB_NONE)) {
-            // Auto Fallback
-            PSTxDataHead_a_FB pBuf = (PSTxDataHead_a_FB)pTxDataHead;
-            //Get SignalField,ServiceField,Length
-            BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
-                (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField), (unsigned char *)&(pBuf->bySignalField)
-            );
-            pBuf->wTransmitLength = cpu_to_le16(wLen);
-            //Get Duration and TimeStampOff
-
-            pBuf->wDuration = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength, byPktType,
-                                        wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption)); //0: 5GHz
-            pBuf->wDuration_f0 = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A_F0, cbFrameLength, byPktType,
-                                        wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption)); //0: 5GHz
-            pBuf->wDuration_f1 = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A_F1, cbFrameLength, byPktType,
-                                        wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption)); //0: 5GHz
-            pBuf->wTimeStampOff = cpu_to_le16(wTimeStampOff[pDevice->byPreambleType%2][wCurrentRate%MAX_RATE]);
-            return (pBuf->wDuration);
-        } else {
-            PSTxDataHead_ab pBuf = (PSTxDataHead_ab)pTxDataHead;
-            //Get SignalField,ServiceField,Length
-            BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
-                (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField), (unsigned char *)&(pBuf->bySignalField)
-            );
-            pBuf->wTransmitLength = cpu_to_le16(wLen);
-            //Get Duration and TimeStampOff
-
-            pBuf->wDuration = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength, byPktType,
-                                                       wCurrentRate, bNeedAck, uFragIdx,
-                                                       cbLastFragmentSize, uMACfragNum,
-                                                       byFBOption));
-
-            pBuf->wTimeStampOff = cpu_to_le16(wTimeStampOff[pDevice->byPreambleType%2][wCurrentRate%MAX_RATE]);
-            return (pBuf->wDuration);
-        }
-    }
-    else {
-            PSTxDataHead_ab pBuf = (PSTxDataHead_ab)pTxDataHead;
-            //Get SignalField,ServiceField,Length
-            BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
-                (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField), (unsigned char *)&(pBuf->bySignalField)
-            );
-            pBuf->wTransmitLength = cpu_to_le16(wLen);
-            //Get Duration and TimeStampOff
-            pBuf->wDuration = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameLength, byPktType,
-                                                       wCurrentRate, bNeedAck, uFragIdx,
-                                                       cbLastFragmentSize, uMACfragNum,
-                                                       byFBOption));
-            pBuf->wTimeStampOff = cpu_to_le16(wTimeStampOff[pDevice->byPreambleType%2][wCurrentRate%MAX_RATE]);
-            return (pBuf->wDuration);
-    }
-    return 0;
+	unsigned short wLen = 0x0000;
+
+	if (pTxDataHead == NULL) {
+		return 0;
+	}
+
+	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
+		if (byFBOption == AUTO_FB_NONE) {
+			PSTxDataHead_g pBuf = (PSTxDataHead_g)pTxDataHead;
+			//Get SignalField,ServiceField,Length
+			BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
+					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_a), (unsigned char *)&(pBuf->bySignalField_a)
+);
+			pBuf->wTransmitLength_a = cpu_to_le16(wLen);
+			BBvCalculateParameter(pDevice, cbFrameLength, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
+					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_b), (unsigned char *)&(pBuf->bySignalField_b)
+);
+			pBuf->wTransmitLength_b = cpu_to_le16(wLen);
+			//Get Duration and TimeStamp
+			pBuf->wDuration_a = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength,
+											   byPktType, wCurrentRate, bNeedAck, uFragIdx,
+											   cbLastFragmentSize, uMACfragNum,
+											   byFBOption)); //1: 2.4GHz
+			pBuf->wDuration_b = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameLength,
+											   PK_TYPE_11B, pDevice->byTopCCKBasicRate,
+											   bNeedAck, uFragIdx, cbLastFragmentSize,
+											   uMACfragNum, byFBOption)); //1: 2.4
+
+			pBuf->wTimeStampOff_a = cpu_to_le16(wTimeStampOff[pDevice->byPreambleType%2][wCurrentRate%MAX_RATE]);
+			pBuf->wTimeStampOff_b = cpu_to_le16(wTimeStampOff[pDevice->byPreambleType%2][pDevice->byTopCCKBasicRate%MAX_RATE]);
+
+			return pBuf->wDuration_a;
+		} else {
+			// Auto Fallback
+			PSTxDataHead_g_FB pBuf = (PSTxDataHead_g_FB)pTxDataHead;
+			//Get SignalField,ServiceField,Length
+			BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
+					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_a), (unsigned char *)&(pBuf->bySignalField_a)
+);
+			pBuf->wTransmitLength_a = cpu_to_le16(wLen);
+			BBvCalculateParameter(pDevice, cbFrameLength, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
+					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_b), (unsigned char *)&(pBuf->bySignalField_b)
+);
+			pBuf->wTransmitLength_b = cpu_to_le16(wLen);
+			//Get Duration and TimeStamp
+			pBuf->wDuration_a = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength, byPktType,
+											   wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption)); //1: 2.4GHz
+			pBuf->wDuration_b = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameLength, PK_TYPE_11B,
+											   pDevice->byTopCCKBasicRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption)); //1: 2.4GHz
+			pBuf->wDuration_a_f0 = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A_F0, cbFrameLength, byPktType,
+											      wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption)); //1: 2.4GHz
+			pBuf->wDuration_a_f1 = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A_F1, cbFrameLength, byPktType,
+											      wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption)); //1: 2.4GHz
+
+			pBuf->wTimeStampOff_a = cpu_to_le16(wTimeStampOff[pDevice->byPreambleType%2][wCurrentRate%MAX_RATE]);
+			pBuf->wTimeStampOff_b = cpu_to_le16(wTimeStampOff[pDevice->byPreambleType%2][pDevice->byTopCCKBasicRate%MAX_RATE]);
+
+			return pBuf->wDuration_a;
+		} //if (byFBOption == AUTO_FB_NONE)
+	} else if (byPktType == PK_TYPE_11A) {
+		if ((byFBOption != AUTO_FB_NONE)) {
+			// Auto Fallback
+			PSTxDataHead_a_FB pBuf = (PSTxDataHead_a_FB)pTxDataHead;
+			//Get SignalField,ServiceField,Length
+			BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
+					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField), (unsigned char *)&(pBuf->bySignalField)
+);
+			pBuf->wTransmitLength = cpu_to_le16(wLen);
+			//Get Duration and TimeStampOff
+
+			pBuf->wDuration = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength, byPktType,
+											 wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption)); //0: 5GHz
+			pBuf->wDuration_f0 = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A_F0, cbFrameLength, byPktType,
+											    wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption)); //0: 5GHz
+			pBuf->wDuration_f1 = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A_F1, cbFrameLength, byPktType,
+											    wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption)); //0: 5GHz
+			pBuf->wTimeStampOff = cpu_to_le16(wTimeStampOff[pDevice->byPreambleType%2][wCurrentRate%MAX_RATE]);
+			return pBuf->wDuration;
+		} else {
+			PSTxDataHead_ab pBuf = (PSTxDataHead_ab)pTxDataHead;
+			//Get SignalField,ServiceField,Length
+			BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
+					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField), (unsigned char *)&(pBuf->bySignalField)
+);
+			pBuf->wTransmitLength = cpu_to_le16(wLen);
+			//Get Duration and TimeStampOff
+
+			pBuf->wDuration = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength, byPktType,
+											 wCurrentRate, bNeedAck, uFragIdx,
+											 cbLastFragmentSize, uMACfragNum,
+											 byFBOption));
+
+			pBuf->wTimeStampOff = cpu_to_le16(wTimeStampOff[pDevice->byPreambleType%2][wCurrentRate%MAX_RATE]);
+			return pBuf->wDuration;
+		}
+	} else {
+		PSTxDataHead_ab pBuf = (PSTxDataHead_ab)pTxDataHead;
+		//Get SignalField,ServiceField,Length
+		BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
+				      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField), (unsigned char *)&(pBuf->bySignalField)
+);
+		pBuf->wTransmitLength = cpu_to_le16(wLen);
+		//Get Duration and TimeStampOff
+		pBuf->wDuration = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameLength, byPktType,
+										 wCurrentRate, bNeedAck, uFragIdx,
+										 cbLastFragmentSize, uMACfragNum,
+										 byFBOption));
+		pBuf->wTimeStampOff = cpu_to_le16(wTimeStampOff[pDevice->byPreambleType%2][wCurrentRate%MAX_RATE]);
+		return pBuf->wDuration;
+	}
+	return 0;
 }
 
-
 static
 void
-s_vFillRTSHead (
-    PSDevice         pDevice,
-    unsigned char byPktType,
-    void *           pvRTS,
-    unsigned int cbFrameLength,
-    bool bNeedAck,
-    bool bDisCRC,
-    PSEthernetHeader psEthHeader,
-    unsigned short wCurrentRate,
-    unsigned char byFBOption
-    )
+s_vFillRTSHead(
+	PSDevice         pDevice,
+	unsigned char byPktType,
+	void *pvRTS,
+	unsigned int cbFrameLength,
+	bool bNeedAck,
+	bool bDisCRC,
+	PSEthernetHeader psEthHeader,
+	unsigned short wCurrentRate,
+	unsigned char byFBOption
+)
 {
-    unsigned int uRTSFrameLen = 20;
-    unsigned short wLen = 0x0000;
-
-    if (pvRTS == NULL)
-    	return;
-
-    if (bDisCRC) {
-        // When CRCDIS bit is on, H/W forgot to generate FCS for RTS frame,
-        // in this case we need to decrease its length by 4.
-        uRTSFrameLen -= 4;
-    }
-
-    // Note: So far RTSHead dosen't appear in ATIM & Beacom DMA, so we don't need to take them into account.
-    //       Otherwise, we need to modify codes for them.
-    if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
-        if (byFBOption == AUTO_FB_NONE) {
-            PSRTS_g pBuf = (PSRTS_g)pvRTS;
-            //Get SignalField,ServiceField,Length
-            BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
-                (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_b), (unsigned char *)&(pBuf->bySignalField_b)
-            );
-            pBuf->wTransmitLength_b = cpu_to_le16(wLen);
-            BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
-                (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_a), (unsigned char *)&(pBuf->bySignalField_a)
-            );
-            pBuf->wTransmitLength_a = cpu_to_le16(wLen);
-            //Get Duration
-            pBuf->wDuration_bb = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_BB, cbFrameLength, PK_TYPE_11B, pDevice->byTopCCKBasicRate, bNeedAck, byFBOption));    //0:RTSDuration_bb, 1:2.4G, 1:CCKData
-            pBuf->wDuration_aa = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //2:RTSDuration_aa, 1:2.4G, 2,3: 2.4G OFDMData
-            pBuf->wDuration_ba = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_BA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //1:RTSDuration_ba, 1:2.4G, 2,3:2.4G OFDM Data
-
-            pBuf->Data.wDurationID = pBuf->wDuration_aa;
-            //Get RTS Frame body
-            pBuf->Data.wFrameControl = TYPE_CTL_RTS;//0x00B4
-            if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
-                (pDevice->eOPMode == OP_MODE_AP)) {
-                memcpy(&(pBuf->Data.abyRA[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
-            }
-            else {
-                memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-            }
-            if (pDevice->eOPMode == OP_MODE_AP) {
-                memcpy(&(pBuf->Data.abyTA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-            }
-            else {
-                memcpy(&(pBuf->Data.abyTA[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
-            }
-        }
-        else {
-           PSRTS_g_FB pBuf = (PSRTS_g_FB)pvRTS;
-            //Get SignalField,ServiceField,Length
-            BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
-                (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_b), (unsigned char *)&(pBuf->bySignalField_b)
-            );
-            pBuf->wTransmitLength_b = cpu_to_le16(wLen);
-            BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
-                (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_a), (unsigned char *)&(pBuf->bySignalField_a)
-            );
-            pBuf->wTransmitLength_a = cpu_to_le16(wLen);
-
-            //Get Duration
-            pBuf->wDuration_bb = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_BB, cbFrameLength, PK_TYPE_11B, pDevice->byTopCCKBasicRate, bNeedAck, byFBOption));    //0:RTSDuration_bb, 1:2.4G, 1:CCKData
-            pBuf->wDuration_aa = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //2:RTSDuration_aa, 1:2.4G, 2,3:2.4G OFDMData
-            pBuf->wDuration_ba = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_BA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //1:RTSDuration_ba, 1:2.4G, 2,3:2.4G OFDMData
-            pBuf->wRTSDuration_ba_f0 = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_BA_F0, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption));    //4:wRTSDuration_ba_f0, 1:2.4G, 1:CCKData
-            pBuf->wRTSDuration_aa_f0 = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F0, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption));    //5:wRTSDuration_aa_f0, 1:2.4G, 1:CCKData
-            pBuf->wRTSDuration_ba_f1 = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_BA_F1, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption));    //6:wRTSDuration_ba_f1, 1:2.4G, 1:CCKData
-            pBuf->wRTSDuration_aa_f1 = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F1, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption));    //7:wRTSDuration_aa_f1, 1:2.4G, 1:CCKData
-            pBuf->Data.wDurationID = pBuf->wDuration_aa;
-            //Get RTS Frame body
-            pBuf->Data.wFrameControl = TYPE_CTL_RTS;//0x00B4
-
-            if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
-                (pDevice->eOPMode == OP_MODE_AP)) {
-                memcpy(&(pBuf->Data.abyRA[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
-            }
-            else {
-                memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-            }
-
-            if (pDevice->eOPMode == OP_MODE_AP) {
-                memcpy(&(pBuf->Data.abyTA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-            }
-            else {
-                memcpy(&(pBuf->Data.abyTA[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
-            }
-
-        } // if (byFBOption == AUTO_FB_NONE)
-    }
-    else if (byPktType == PK_TYPE_11A) {
-        if (byFBOption == AUTO_FB_NONE) {
-            PSRTS_ab pBuf = (PSRTS_ab)pvRTS;
-            //Get SignalField,ServiceField,Length
-            BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
-                (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField), (unsigned char *)&(pBuf->bySignalField)
-            );
-            pBuf->wTransmitLength = cpu_to_le16(wLen);
-            //Get Duration
-            pBuf->wDuration = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //0:RTSDuration_aa, 0:5G, 0: 5G OFDMData
-    	    pBuf->Data.wDurationID = pBuf->wDuration;
-            //Get RTS Frame body
-            pBuf->Data.wFrameControl = TYPE_CTL_RTS;//0x00B4
-
-            if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
-                (pDevice->eOPMode == OP_MODE_AP)) {
-                memcpy(&(pBuf->Data.abyRA[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
-            }
-            else {
-                memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-            }
-
-            if (pDevice->eOPMode == OP_MODE_AP) {
-                memcpy(&(pBuf->Data.abyTA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-            }
-            else {
-                memcpy(&(pBuf->Data.abyTA[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
-            }
-
-        }
-        else {
-            PSRTS_a_FB pBuf = (PSRTS_a_FB)pvRTS;
-            //Get SignalField,ServiceField,Length
-            BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
-                (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField), (unsigned char *)&(pBuf->bySignalField)
-            );
-            pBuf->wTransmitLength = cpu_to_le16(wLen);
-            //Get Duration
-            pBuf->wDuration = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //0:RTSDuration_aa, 0:5G, 0: 5G OFDMData
-    	    pBuf->wRTSDuration_f0 = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F0, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //5:RTSDuration_aa_f0, 0:5G, 0: 5G OFDMData
-    	    pBuf->wRTSDuration_f1 = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F1, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //7:RTSDuration_aa_f1, 0:5G, 0:
-    	    pBuf->Data.wDurationID = pBuf->wDuration;
-    	    //Get RTS Frame body
-            pBuf->Data.wFrameControl = TYPE_CTL_RTS;//0x00B4
-
-            if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
-                (pDevice->eOPMode == OP_MODE_AP)) {
-                memcpy(&(pBuf->Data.abyRA[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
-            }
-            else {
-                memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-            }
-            if (pDevice->eOPMode == OP_MODE_AP) {
-                memcpy(&(pBuf->Data.abyTA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-            }
-            else {
-                memcpy(&(pBuf->Data.abyTA[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
-            }
-        }
-    }
-    else if (byPktType == PK_TYPE_11B) {
-        PSRTS_ab pBuf = (PSRTS_ab)pvRTS;
-        //Get SignalField,ServiceField,Length
-        BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
-            (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField), (unsigned char *)&(pBuf->bySignalField)
-        );
-        pBuf->wTransmitLength = cpu_to_le16(wLen);
-        //Get Duration
-        pBuf->wDuration = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_BB, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //0:RTSDuration_bb, 1:2.4G, 1:CCKData
-        pBuf->Data.wDurationID = pBuf->wDuration;
-        //Get RTS Frame body
-        pBuf->Data.wFrameControl = TYPE_CTL_RTS;//0x00B4
-
-
-        if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
-            (pDevice->eOPMode == OP_MODE_AP)) {
-            memcpy(&(pBuf->Data.abyRA[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
-        }
-        else {
-            memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-        }
-
-        if (pDevice->eOPMode == OP_MODE_AP) {
-            memcpy(&(pBuf->Data.abyTA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-        }
-        else {
-            memcpy(&(pBuf->Data.abyTA[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
-        }
-    }
+	unsigned int uRTSFrameLen = 20;
+	unsigned short wLen = 0x0000;
+
+	if (pvRTS == NULL)
+		return;
+
+	if (bDisCRC) {
+		// When CRCDIS bit is on, H/W forgot to generate FCS for RTS frame,
+		// in this case we need to decrease its length by 4.
+		uRTSFrameLen -= 4;
+	}
+
+	// Note: So far RTSHead dosen't appear in ATIM & Beacom DMA, so we don't need to take them into account.
+	//       Otherwise, we need to modify codes for them.
+	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
+		if (byFBOption == AUTO_FB_NONE) {
+			PSRTS_g pBuf = (PSRTS_g)pvRTS;
+			//Get SignalField,ServiceField,Length
+			BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
+					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_b), (unsigned char *)&(pBuf->bySignalField_b)
+);
+			pBuf->wTransmitLength_b = cpu_to_le16(wLen);
+			BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
+					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_a), (unsigned char *)&(pBuf->bySignalField_a)
+);
+			pBuf->wTransmitLength_a = cpu_to_le16(wLen);
+			//Get Duration
+			pBuf->wDuration_bb = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_BB, cbFrameLength, PK_TYPE_11B, pDevice->byTopCCKBasicRate, bNeedAck, byFBOption));    //0:RTSDuration_bb, 1:2.4G, 1:CCKData
+			pBuf->wDuration_aa = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //2:RTSDuration_aa, 1:2.4G, 2,3: 2.4G OFDMData
+			pBuf->wDuration_ba = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_BA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //1:RTSDuration_ba, 1:2.4G, 2,3:2.4G OFDM Data
+
+			pBuf->Data.wDurationID = pBuf->wDuration_aa;
+			//Get RTS Frame body
+			pBuf->Data.wFrameControl = TYPE_CTL_RTS;//0x00B4
+			if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
+			    (pDevice->eOPMode == OP_MODE_AP)) {
+				memcpy(&(pBuf->Data.abyRA[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
+			} else {
+				memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
+			}
+			if (pDevice->eOPMode == OP_MODE_AP) {
+				memcpy(&(pBuf->Data.abyTA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
+			} else {
+				memcpy(&(pBuf->Data.abyTA[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
+			}
+		} else {
+			PSRTS_g_FB pBuf = (PSRTS_g_FB)pvRTS;
+			//Get SignalField,ServiceField,Length
+			BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
+					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_b), (unsigned char *)&(pBuf->bySignalField_b)
+);
+			pBuf->wTransmitLength_b = cpu_to_le16(wLen);
+			BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
+					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_a), (unsigned char *)&(pBuf->bySignalField_a)
+);
+			pBuf->wTransmitLength_a = cpu_to_le16(wLen);
+
+			//Get Duration
+			pBuf->wDuration_bb = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_BB, cbFrameLength, PK_TYPE_11B, pDevice->byTopCCKBasicRate, bNeedAck, byFBOption));    //0:RTSDuration_bb, 1:2.4G, 1:CCKData
+			pBuf->wDuration_aa = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //2:RTSDuration_aa, 1:2.4G, 2,3:2.4G OFDMData
+			pBuf->wDuration_ba = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_BA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //1:RTSDuration_ba, 1:2.4G, 2,3:2.4G OFDMData
+			pBuf->wRTSDuration_ba_f0 = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_BA_F0, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption));    //4:wRTSDuration_ba_f0, 1:2.4G, 1:CCKData
+			pBuf->wRTSDuration_aa_f0 = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F0, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption));    //5:wRTSDuration_aa_f0, 1:2.4G, 1:CCKData
+			pBuf->wRTSDuration_ba_f1 = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_BA_F1, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption));    //6:wRTSDuration_ba_f1, 1:2.4G, 1:CCKData
+			pBuf->wRTSDuration_aa_f1 = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F1, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption));    //7:wRTSDuration_aa_f1, 1:2.4G, 1:CCKData
+			pBuf->Data.wDurationID = pBuf->wDuration_aa;
+			//Get RTS Frame body
+			pBuf->Data.wFrameControl = TYPE_CTL_RTS;//0x00B4
+
+			if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
+			    (pDevice->eOPMode == OP_MODE_AP)) {
+				memcpy(&(pBuf->Data.abyRA[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
+			} else {
+				memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
+			}
+
+			if (pDevice->eOPMode == OP_MODE_AP) {
+				memcpy(&(pBuf->Data.abyTA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
+			} else {
+				memcpy(&(pBuf->Data.abyTA[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
+			}
+
+		} // if (byFBOption == AUTO_FB_NONE)
+	} else if (byPktType == PK_TYPE_11A) {
+		if (byFBOption == AUTO_FB_NONE) {
+			PSRTS_ab pBuf = (PSRTS_ab)pvRTS;
+			//Get SignalField,ServiceField,Length
+			BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
+					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField), (unsigned char *)&(pBuf->bySignalField)
+);
+			pBuf->wTransmitLength = cpu_to_le16(wLen);
+			//Get Duration
+			pBuf->wDuration = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //0:RTSDuration_aa, 0:5G, 0: 5G OFDMData
+			pBuf->Data.wDurationID = pBuf->wDuration;
+			//Get RTS Frame body
+			pBuf->Data.wFrameControl = TYPE_CTL_RTS;//0x00B4
+
+			if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
+			    (pDevice->eOPMode == OP_MODE_AP)) {
+				memcpy(&(pBuf->Data.abyRA[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
+			} else {
+				memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
+			}
+
+			if (pDevice->eOPMode == OP_MODE_AP) {
+				memcpy(&(pBuf->Data.abyTA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
+			} else {
+				memcpy(&(pBuf->Data.abyTA[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
+			}
+
+		} else {
+			PSRTS_a_FB pBuf = (PSRTS_a_FB)pvRTS;
+			//Get SignalField,ServiceField,Length
+			BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
+					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField), (unsigned char *)&(pBuf->bySignalField)
+);
+			pBuf->wTransmitLength = cpu_to_le16(wLen);
+			//Get Duration
+			pBuf->wDuration = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //0:RTSDuration_aa, 0:5G, 0: 5G OFDMData
+			pBuf->wRTSDuration_f0 = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F0, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //5:RTSDuration_aa_f0, 0:5G, 0: 5G OFDMData
+			pBuf->wRTSDuration_f1 = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F1, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //7:RTSDuration_aa_f1, 0:5G, 0:
+			pBuf->Data.wDurationID = pBuf->wDuration;
+			//Get RTS Frame body
+			pBuf->Data.wFrameControl = TYPE_CTL_RTS;//0x00B4
+
+			if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
+			    (pDevice->eOPMode == OP_MODE_AP)) {
+				memcpy(&(pBuf->Data.abyRA[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
+			} else {
+				memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
+			}
+			if (pDevice->eOPMode == OP_MODE_AP) {
+				memcpy(&(pBuf->Data.abyTA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
+			} else {
+				memcpy(&(pBuf->Data.abyTA[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
+			}
+		}
+	} else if (byPktType == PK_TYPE_11B) {
+		PSRTS_ab pBuf = (PSRTS_ab)pvRTS;
+		//Get SignalField,ServiceField,Length
+		BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
+				      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField), (unsigned char *)&(pBuf->bySignalField)
+);
+		pBuf->wTransmitLength = cpu_to_le16(wLen);
+		//Get Duration
+		pBuf->wDuration = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_BB, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //0:RTSDuration_bb, 1:2.4G, 1:CCKData
+		pBuf->Data.wDurationID = pBuf->wDuration;
+		//Get RTS Frame body
+		pBuf->Data.wFrameControl = TYPE_CTL_RTS;//0x00B4
+
+		if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
+		    (pDevice->eOPMode == OP_MODE_AP)) {
+			memcpy(&(pBuf->Data.abyRA[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
+		} else {
+			memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
+		}
+
+		if (pDevice->eOPMode == OP_MODE_AP) {
+			memcpy(&(pBuf->Data.abyTA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
+		} else {
+			memcpy(&(pBuf->Data.abyTA[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
+		}
+	}
 }
 
 static
 void
-s_vFillCTSHead (
-    PSDevice pDevice,
-    unsigned int uDMAIdx,
-    unsigned char byPktType,
-    void *   pvCTS,
-    unsigned int cbFrameLength,
-    bool bNeedAck,
-    bool bDisCRC,
-    unsigned short wCurrentRate,
-    unsigned char byFBOption
-    )
+s_vFillCTSHead(
+	PSDevice pDevice,
+	unsigned int uDMAIdx,
+	unsigned char byPktType,
+	void *pvCTS,
+	unsigned int cbFrameLength,
+	bool bNeedAck,
+	bool bDisCRC,
+	unsigned short wCurrentRate,
+	unsigned char byFBOption
+)
 {
-    unsigned int uCTSFrameLen = 14;
-    unsigned short wLen = 0x0000;
-
-    if (pvCTS == NULL) {
-        return;
-    }
-
-    if (bDisCRC) {
-        // When CRCDIS bit is on, H/W forgot to generate FCS for CTS frame,
-        // in this case we need to decrease its length by 4.
-        uCTSFrameLen -= 4;
-    }
-
-    if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
-        if (byFBOption != AUTO_FB_NONE && uDMAIdx != TYPE_ATIMDMA && uDMAIdx != TYPE_BEACONDMA) {
-            // Auto Fall back
-            PSCTS_FB pBuf = (PSCTS_FB)pvCTS;
-            //Get SignalField,ServiceField,Length
-            BBvCalculateParameter(pDevice, uCTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
-                (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_b), (unsigned char *)&(pBuf->bySignalField_b)
-            );
-
-
-            pBuf->wTransmitLength_b = cpu_to_le16(wLen);
-
-            pBuf->wDuration_ba = (unsigned short)s_uGetRTSCTSDuration(pDevice, CTSDUR_BA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption); //3:CTSDuration_ba, 1:2.4G, 2,3:2.4G OFDM Data
-            pBuf->wDuration_ba += pDevice->wCTSDuration;
-            pBuf->wDuration_ba = cpu_to_le16(pBuf->wDuration_ba);
-            //Get CTSDuration_ba_f0
-            pBuf->wCTSDuration_ba_f0 = (unsigned short)s_uGetRTSCTSDuration(pDevice, CTSDUR_BA_F0, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption); //8:CTSDuration_ba_f0, 1:2.4G, 2,3:2.4G OFDM Data
-            pBuf->wCTSDuration_ba_f0 += pDevice->wCTSDuration;
-            pBuf->wCTSDuration_ba_f0 = cpu_to_le16(pBuf->wCTSDuration_ba_f0);
-            //Get CTSDuration_ba_f1
-            pBuf->wCTSDuration_ba_f1 = (unsigned short)s_uGetRTSCTSDuration(pDevice, CTSDUR_BA_F1, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption); //9:CTSDuration_ba_f1, 1:2.4G, 2,3:2.4G OFDM Data
-            pBuf->wCTSDuration_ba_f1 += pDevice->wCTSDuration;
-            pBuf->wCTSDuration_ba_f1 = cpu_to_le16(pBuf->wCTSDuration_ba_f1);
-            //Get CTS Frame body
-            pBuf->Data.wDurationID = pBuf->wDuration_ba;
-            pBuf->Data.wFrameControl = TYPE_CTL_CTS;//0x00C4
-            pBuf->Data.wReserved = 0x0000;
-            memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyCurrentNetAddr[0]), ETH_ALEN);
-
-        } else { //if (byFBOption != AUTO_FB_NONE && uDMAIdx != TYPE_ATIMDMA && uDMAIdx != TYPE_BEACONDMA)
-            PSCTS pBuf = (PSCTS)pvCTS;
-            //Get SignalField,ServiceField,Length
-            BBvCalculateParameter(pDevice, uCTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
-                (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_b), (unsigned char *)&(pBuf->bySignalField_b)
-            );
-            pBuf->wTransmitLength_b = cpu_to_le16(wLen);
-            //Get CTSDuration_ba
-            pBuf->wDuration_ba = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, CTSDUR_BA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //3:CTSDuration_ba, 1:2.4G, 2,3:2.4G OFDM Data
-            pBuf->wDuration_ba += pDevice->wCTSDuration;
-            pBuf->wDuration_ba = cpu_to_le16(pBuf->wDuration_ba);
-
-            //Get CTS Frame body
-            pBuf->Data.wDurationID = pBuf->wDuration_ba;
-            pBuf->Data.wFrameControl = TYPE_CTL_CTS;//0x00C4
-            pBuf->Data.wReserved = 0x0000;
-            memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyCurrentNetAddr[0]), ETH_ALEN);
-        }
-    }
+	unsigned int uCTSFrameLen = 14;
+	unsigned short wLen = 0x0000;
+
+	if (pvCTS == NULL) {
+		return;
+	}
+
+	if (bDisCRC) {
+		// When CRCDIS bit is on, H/W forgot to generate FCS for CTS frame,
+		// in this case we need to decrease its length by 4.
+		uCTSFrameLen -= 4;
+	}
+
+	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
+		if (byFBOption != AUTO_FB_NONE && uDMAIdx != TYPE_ATIMDMA && uDMAIdx != TYPE_BEACONDMA) {
+			// Auto Fall back
+			PSCTS_FB pBuf = (PSCTS_FB)pvCTS;
+			//Get SignalField,ServiceField,Length
+			BBvCalculateParameter(pDevice, uCTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
+					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_b), (unsigned char *)&(pBuf->bySignalField_b)
+);
+
+			pBuf->wTransmitLength_b = cpu_to_le16(wLen);
+
+			pBuf->wDuration_ba = (unsigned short)s_uGetRTSCTSDuration(pDevice, CTSDUR_BA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption); //3:CTSDuration_ba, 1:2.4G, 2,3:2.4G OFDM Data
+			pBuf->wDuration_ba += pDevice->wCTSDuration;
+			pBuf->wDuration_ba = cpu_to_le16(pBuf->wDuration_ba);
+			//Get CTSDuration_ba_f0
+			pBuf->wCTSDuration_ba_f0 = (unsigned short)s_uGetRTSCTSDuration(pDevice, CTSDUR_BA_F0, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption); //8:CTSDuration_ba_f0, 1:2.4G, 2,3:2.4G OFDM Data
+			pBuf->wCTSDuration_ba_f0 += pDevice->wCTSDuration;
+			pBuf->wCTSDuration_ba_f0 = cpu_to_le16(pBuf->wCTSDuration_ba_f0);
+			//Get CTSDuration_ba_f1
+			pBuf->wCTSDuration_ba_f1 = (unsigned short)s_uGetRTSCTSDuration(pDevice, CTSDUR_BA_F1, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption); //9:CTSDuration_ba_f1, 1:2.4G, 2,3:2.4G OFDM Data
+			pBuf->wCTSDuration_ba_f1 += pDevice->wCTSDuration;
+			pBuf->wCTSDuration_ba_f1 = cpu_to_le16(pBuf->wCTSDuration_ba_f1);
+			//Get CTS Frame body
+			pBuf->Data.wDurationID = pBuf->wDuration_ba;
+			pBuf->Data.wFrameControl = TYPE_CTL_CTS;//0x00C4
+			pBuf->Data.wReserved = 0x0000;
+			memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyCurrentNetAddr[0]), ETH_ALEN);
+
+		} else { //if (byFBOption != AUTO_FB_NONE && uDMAIdx != TYPE_ATIMDMA && uDMAIdx != TYPE_BEACONDMA)
+			PSCTS pBuf = (PSCTS)pvCTS;
+			//Get SignalField,ServiceField,Length
+			BBvCalculateParameter(pDevice, uCTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
+					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_b), (unsigned char *)&(pBuf->bySignalField_b)
+);
+			pBuf->wTransmitLength_b = cpu_to_le16(wLen);
+			//Get CTSDuration_ba
+			pBuf->wDuration_ba = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, CTSDUR_BA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //3:CTSDuration_ba, 1:2.4G, 2,3:2.4G OFDM Data
+			pBuf->wDuration_ba += pDevice->wCTSDuration;
+			pBuf->wDuration_ba = cpu_to_le16(pBuf->wDuration_ba);
+
+			//Get CTS Frame body
+			pBuf->Data.wDurationID = pBuf->wDuration_ba;
+			pBuf->Data.wFrameControl = TYPE_CTL_CTS;//0x00C4
+			pBuf->Data.wReserved = 0x0000;
+			memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyCurrentNetAddr[0]), ETH_ALEN);
+		}
+	}
 }
 
-
-
-
-
-
 /*+
  *
  * Description:
@@ -1136,1079 +1071,977 @@ s_vFillCTSHead (
  *
  * Return Value: none
  *
--*/
+ -*/
 // unsigned int cbFrameSize,//Hdr+Payload+FCS
 static
 void
-s_vGenerateTxParameter (
-    PSDevice         pDevice,
-    unsigned char byPktType,
-    void *           pTxBufHead,
-    void *           pvRrvTime,
-    void *           pvRTS,
-    void *           pvCTS,
-    unsigned int cbFrameSize,
-    bool bNeedACK,
-    unsigned int uDMAIdx,
-    PSEthernetHeader psEthHeader,
-    unsigned short wCurrentRate
-    )
+s_vGenerateTxParameter(
+	PSDevice         pDevice,
+	unsigned char byPktType,
+	void *pTxBufHead,
+	void *pvRrvTime,
+	void *pvRTS,
+	void *pvCTS,
+	unsigned int cbFrameSize,
+	bool bNeedACK,
+	unsigned int uDMAIdx,
+	PSEthernetHeader psEthHeader,
+	unsigned short wCurrentRate
+)
 {
-    unsigned int cbMACHdLen = WLAN_HDR_ADDR3_LEN; //24
-    unsigned short wFifoCtl;
-    bool bDisCRC = false;
-    unsigned char byFBOption = AUTO_FB_NONE;
+	unsigned int cbMACHdLen = WLAN_HDR_ADDR3_LEN; //24
+	unsigned short wFifoCtl;
+	bool bDisCRC = false;
+	unsigned char byFBOption = AUTO_FB_NONE;
 //    unsigned short wCurrentRate = pDevice->wCurrentRate;
 
-    //DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"s_vGenerateTxParameter...\n");
-    PSTxBufHead pFifoHead = (PSTxBufHead)pTxBufHead;
-    pFifoHead->wReserved = wCurrentRate;
-    wFifoCtl = pFifoHead->wFIFOCtl;
-
-    if (wFifoCtl & FIFOCTL_CRCDIS) {
-        bDisCRC = true;
-    }
-
-    if (wFifoCtl & FIFOCTL_AUTO_FB_0) {
-        byFBOption = AUTO_FB_0;
-    }
-    else if (wFifoCtl & FIFOCTL_AUTO_FB_1) {
-        byFBOption = AUTO_FB_1;
-    }
-
-    if (pDevice->bLongHeader)
-        cbMACHdLen = WLAN_HDR_ADDR3_LEN + 6;
-
-    if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
-
-        if (pvRTS != NULL) { //RTS_need
-            //Fill RsvTime
-            if (pvRrvTime) {
-                PSRrvTime_gRTS pBuf = (PSRrvTime_gRTS)pvRrvTime;
-                pBuf->wRTSTxRrvTime_aa = cpu_to_le16((unsigned short)s_uGetRTSCTSRsvTime(pDevice, 2, byPktType, cbFrameSize, wCurrentRate));//2:RTSTxRrvTime_aa, 1:2.4GHz
-                pBuf->wRTSTxRrvTime_ba = cpu_to_le16((unsigned short)s_uGetRTSCTSRsvTime(pDevice, 1, byPktType, cbFrameSize, wCurrentRate));//1:RTSTxRrvTime_ba, 1:2.4GHz
-                pBuf->wRTSTxRrvTime_bb = cpu_to_le16((unsigned short)s_uGetRTSCTSRsvTime(pDevice, 0, byPktType, cbFrameSize, wCurrentRate));//0:RTSTxRrvTime_bb, 1:2.4GHz
-                pBuf->wTxRrvTime_a = cpu_to_le16((unsigned short) s_uGetTxRsvTime(pDevice, byPktType, cbFrameSize, wCurrentRate, bNeedACK));//2.4G OFDM
-                pBuf->wTxRrvTime_b = cpu_to_le16((unsigned short) s_uGetTxRsvTime(pDevice, PK_TYPE_11B, cbFrameSize, pDevice->byTopCCKBasicRate, bNeedACK));//1:CCK
-            }
-            //Fill RTS
-            s_vFillRTSHead(pDevice, byPktType, pvRTS, cbFrameSize, bNeedACK, bDisCRC, psEthHeader, wCurrentRate, byFBOption);
-        }
-        else {//RTS_needless, PCF mode
-
-            //Fill RsvTime
-            if (pvRrvTime) {
-                PSRrvTime_gCTS pBuf = (PSRrvTime_gCTS)pvRrvTime;
-                pBuf->wTxRrvTime_a = cpu_to_le16((unsigned short)s_uGetTxRsvTime(pDevice, byPktType, cbFrameSize, wCurrentRate, bNeedACK));//2.4G OFDM
-                pBuf->wTxRrvTime_b = cpu_to_le16((unsigned short)s_uGetTxRsvTime(pDevice, PK_TYPE_11B, cbFrameSize, pDevice->byTopCCKBasicRate, bNeedACK));//1:CCK
-                pBuf->wCTSTxRrvTime_ba = cpu_to_le16((unsigned short)s_uGetRTSCTSRsvTime(pDevice, 3, byPktType, cbFrameSize, wCurrentRate));//3:CTSTxRrvTime_Ba, 1:2.4GHz
-            }
-
-
-            //Fill CTS
-            s_vFillCTSHead(pDevice, uDMAIdx, byPktType, pvCTS, cbFrameSize, bNeedACK, bDisCRC, wCurrentRate, byFBOption);
-        }
-    }
-    else if (byPktType == PK_TYPE_11A) {
-
-        if (pvRTS != NULL) {//RTS_need, non PCF mode
-            //Fill RsvTime
-            if (pvRrvTime) {
-                PSRrvTime_ab pBuf = (PSRrvTime_ab)pvRrvTime;
-                pBuf->wRTSTxRrvTime = cpu_to_le16((unsigned short)s_uGetRTSCTSRsvTime(pDevice, 2, byPktType, cbFrameSize, wCurrentRate));//2:RTSTxRrvTime_aa, 0:5GHz
-                pBuf->wTxRrvTime = cpu_to_le16((unsigned short)s_uGetTxRsvTime(pDevice, byPktType, cbFrameSize, wCurrentRate, bNeedACK));//0:OFDM
-            }
-            //Fill RTS
-            s_vFillRTSHead(pDevice, byPktType, pvRTS, cbFrameSize, bNeedACK, bDisCRC, psEthHeader, wCurrentRate, byFBOption);
-        }
-        else if (pvRTS == NULL) {//RTS_needless, non PCF mode
-            //Fill RsvTime
-            if (pvRrvTime) {
-                PSRrvTime_ab pBuf = (PSRrvTime_ab)pvRrvTime;
-                pBuf->wTxRrvTime = cpu_to_le16((unsigned short)s_uGetTxRsvTime(pDevice, PK_TYPE_11A, cbFrameSize, wCurrentRate, bNeedACK)); //0:OFDM
-            }
-        }
-    }
-    else if (byPktType == PK_TYPE_11B) {
-
-        if ((pvRTS != NULL)) {//RTS_need, non PCF mode
-            //Fill RsvTime
-            if (pvRrvTime) {
-                PSRrvTime_ab pBuf = (PSRrvTime_ab)pvRrvTime;
-                pBuf->wRTSTxRrvTime = cpu_to_le16((unsigned short)s_uGetRTSCTSRsvTime(pDevice, 0, byPktType, cbFrameSize, wCurrentRate));//0:RTSTxRrvTime_bb, 1:2.4GHz
-                pBuf->wTxRrvTime = cpu_to_le16((unsigned short)s_uGetTxRsvTime(pDevice, PK_TYPE_11B, cbFrameSize, wCurrentRate, bNeedACK));//1:CCK
-            }
-            //Fill RTS
-            s_vFillRTSHead(pDevice, byPktType, pvRTS, cbFrameSize, bNeedACK, bDisCRC, psEthHeader, wCurrentRate, byFBOption);
-        }
-        else { //RTS_needless, non PCF mode
-            //Fill RsvTime
-            if (pvRrvTime) {
-                PSRrvTime_ab pBuf = (PSRrvTime_ab)pvRrvTime;
-                pBuf->wTxRrvTime = cpu_to_le16((unsigned short)s_uGetTxRsvTime(pDevice, PK_TYPE_11B, cbFrameSize, wCurrentRate, bNeedACK)); //1:CCK
-            }
-        }
-    }
-    //DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"s_vGenerateTxParameter END.\n");
+	//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "s_vGenerateTxParameter...\n");
+	PSTxBufHead pFifoHead = (PSTxBufHead)pTxBufHead;
+	pFifoHead->wReserved = wCurrentRate;
+	wFifoCtl = pFifoHead->wFIFOCtl;
+
+	if (wFifoCtl & FIFOCTL_CRCDIS) {
+		bDisCRC = true;
+	}
+
+	if (wFifoCtl & FIFOCTL_AUTO_FB_0) {
+		byFBOption = AUTO_FB_0;
+	} else if (wFifoCtl & FIFOCTL_AUTO_FB_1) {
+		byFBOption = AUTO_FB_1;
+	}
+
+	if (pDevice->bLongHeader)
+		cbMACHdLen = WLAN_HDR_ADDR3_LEN + 6;
+
+	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
+		if (pvRTS != NULL) { //RTS_need
+			//Fill RsvTime
+			if (pvRrvTime) {
+				PSRrvTime_gRTS pBuf = (PSRrvTime_gRTS)pvRrvTime;
+				pBuf->wRTSTxRrvTime_aa = cpu_to_le16((unsigned short)s_uGetRTSCTSRsvTime(pDevice, 2, byPktType, cbFrameSize, wCurrentRate));//2:RTSTxRrvTime_aa, 1:2.4GHz
+				pBuf->wRTSTxRrvTime_ba = cpu_to_le16((unsigned short)s_uGetRTSCTSRsvTime(pDevice, 1, byPktType, cbFrameSize, wCurrentRate));//1:RTSTxRrvTime_ba, 1:2.4GHz
+				pBuf->wRTSTxRrvTime_bb = cpu_to_le16((unsigned short)s_uGetRTSCTSRsvTime(pDevice, 0, byPktType, cbFrameSize, wCurrentRate));//0:RTSTxRrvTime_bb, 1:2.4GHz
+				pBuf->wTxRrvTime_a = cpu_to_le16((unsigned short) s_uGetTxRsvTime(pDevice, byPktType, cbFrameSize, wCurrentRate, bNeedACK));//2.4G OFDM
+				pBuf->wTxRrvTime_b = cpu_to_le16((unsigned short) s_uGetTxRsvTime(pDevice, PK_TYPE_11B, cbFrameSize, pDevice->byTopCCKBasicRate, bNeedACK));//1:CCK
+			}
+			//Fill RTS
+			s_vFillRTSHead(pDevice, byPktType, pvRTS, cbFrameSize, bNeedACK, bDisCRC, psEthHeader, wCurrentRate, byFBOption);
+		} else {//RTS_needless, PCF mode
+
+			//Fill RsvTime
+			if (pvRrvTime) {
+				PSRrvTime_gCTS pBuf = (PSRrvTime_gCTS)pvRrvTime;
+				pBuf->wTxRrvTime_a = cpu_to_le16((unsigned short)s_uGetTxRsvTime(pDevice, byPktType, cbFrameSize, wCurrentRate, bNeedACK));//2.4G OFDM
+				pBuf->wTxRrvTime_b = cpu_to_le16((unsigned short)s_uGetTxRsvTime(pDevice, PK_TYPE_11B, cbFrameSize, pDevice->byTopCCKBasicRate, bNeedACK));//1:CCK
+				pBuf->wCTSTxRrvTime_ba = cpu_to_le16((unsigned short)s_uGetRTSCTSRsvTime(pDevice, 3, byPktType, cbFrameSize, wCurrentRate));//3:CTSTxRrvTime_Ba, 1:2.4GHz
+			}
+
+			//Fill CTS
+			s_vFillCTSHead(pDevice, uDMAIdx, byPktType, pvCTS, cbFrameSize, bNeedACK, bDisCRC, wCurrentRate, byFBOption);
+		}
+	} else if (byPktType == PK_TYPE_11A) {
+		if (pvRTS != NULL) {//RTS_need, non PCF mode
+			//Fill RsvTime
+			if (pvRrvTime) {
+				PSRrvTime_ab pBuf = (PSRrvTime_ab)pvRrvTime;
+				pBuf->wRTSTxRrvTime = cpu_to_le16((unsigned short)s_uGetRTSCTSRsvTime(pDevice, 2, byPktType, cbFrameSize, wCurrentRate));//2:RTSTxRrvTime_aa, 0:5GHz
+				pBuf->wTxRrvTime = cpu_to_le16((unsigned short)s_uGetTxRsvTime(pDevice, byPktType, cbFrameSize, wCurrentRate, bNeedACK));//0:OFDM
+			}
+			//Fill RTS
+			s_vFillRTSHead(pDevice, byPktType, pvRTS, cbFrameSize, bNeedACK, bDisCRC, psEthHeader, wCurrentRate, byFBOption);
+		} else if (pvRTS == NULL) {//RTS_needless, non PCF mode
+			//Fill RsvTime
+			if (pvRrvTime) {
+				PSRrvTime_ab pBuf = (PSRrvTime_ab)pvRrvTime;
+				pBuf->wTxRrvTime = cpu_to_le16((unsigned short)s_uGetTxRsvTime(pDevice, PK_TYPE_11A, cbFrameSize, wCurrentRate, bNeedACK)); //0:OFDM
+			}
+		}
+	} else if (byPktType == PK_TYPE_11B) {
+		if ((pvRTS != NULL)) {//RTS_need, non PCF mode
+			//Fill RsvTime
+			if (pvRrvTime) {
+				PSRrvTime_ab pBuf = (PSRrvTime_ab)pvRrvTime;
+				pBuf->wRTSTxRrvTime = cpu_to_le16((unsigned short)s_uGetRTSCTSRsvTime(pDevice, 0, byPktType, cbFrameSize, wCurrentRate));//0:RTSTxRrvTime_bb, 1:2.4GHz
+				pBuf->wTxRrvTime = cpu_to_le16((unsigned short)s_uGetTxRsvTime(pDevice, PK_TYPE_11B, cbFrameSize, wCurrentRate, bNeedACK));//1:CCK
+			}
+			//Fill RTS
+			s_vFillRTSHead(pDevice, byPktType, pvRTS, cbFrameSize, bNeedACK, bDisCRC, psEthHeader, wCurrentRate, byFBOption);
+		} else { //RTS_needless, non PCF mode
+			//Fill RsvTime
+			if (pvRrvTime) {
+				PSRrvTime_ab pBuf = (PSRrvTime_ab)pvRrvTime;
+				pBuf->wTxRrvTime = cpu_to_le16((unsigned short)s_uGetTxRsvTime(pDevice, PK_TYPE_11B, cbFrameSize, wCurrentRate, bNeedACK)); //1:CCK
+			}
+		}
+	}
+	//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "s_vGenerateTxParameter END.\n");
 }
 /*
-    unsigned char *pbyBuffer,//point to pTxBufHead
-    unsigned short wFragType,//00:Non-Frag, 01:Start, 02:Mid, 03:Last
-    unsigned int cbFragmentSize,//Hdr+payoad+FCS
+  unsigned char *pbyBuffer,//point to pTxBufHead
+  unsigned short wFragType,//00:Non-Frag, 01:Start, 02:Mid, 03:Last
+  unsigned int cbFragmentSize,//Hdr+payoad+FCS
 */
 static
 void
 s_vFillFragParameter(
-    PSDevice pDevice,
-    unsigned char *pbyBuffer,
-    unsigned int uTxType,
-    void *   pvtdCurr,
-    unsigned short wFragType,
-    unsigned int cbReqCount
-    )
+	PSDevice pDevice,
+	unsigned char *pbyBuffer,
+	unsigned int uTxType,
+	void *pvtdCurr,
+	unsigned short wFragType,
+	unsigned int cbReqCount
+)
 {
-    PSTxBufHead pTxBufHead = (PSTxBufHead) pbyBuffer;
-    //DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"s_vFillFragParameter...\n");
-
-    if (uTxType == TYPE_SYNCDMA) {
-        //PSTxSyncDesc ptdCurr = (PSTxSyncDesc)s_pvGetTxDescHead(pDevice, uTxType, uCurIdx);
-        PSTxSyncDesc ptdCurr = (PSTxSyncDesc)pvtdCurr;
-
-         //Set FIFOCtl & TimeStamp in TxSyncDesc
-        ptdCurr->m_wFIFOCtl = pTxBufHead->wFIFOCtl;
-        ptdCurr->m_wTimeStamp = pTxBufHead->wTimeStamp;
-        //Set TSR1 & ReqCount in TxDescHead
-        ptdCurr->m_td1TD1.wReqCount = cpu_to_le16((unsigned short)(cbReqCount));
-        if (wFragType == FRAGCTL_ENDFRAG) { //Last Fragmentation
-            ptdCurr->m_td1TD1.byTCR |= (TCR_STP | TCR_EDP | EDMSDU);
-        }
-        else {
-            ptdCurr->m_td1TD1.byTCR |= (TCR_STP | TCR_EDP);
-        }
-    }
-    else {
-        //PSTxDesc ptdCurr = (PSTxDesc)s_pvGetTxDescHead(pDevice, uTxType, uCurIdx);
-        PSTxDesc ptdCurr = (PSTxDesc)pvtdCurr;
-        //Set TSR1 & ReqCount in TxDescHead
-        ptdCurr->m_td1TD1.wReqCount = cpu_to_le16((unsigned short)(cbReqCount));
-        if (wFragType == FRAGCTL_ENDFRAG) { //Last Fragmentation
-            ptdCurr->m_td1TD1.byTCR |= (TCR_STP | TCR_EDP | EDMSDU);
-        }
-        else {
-            ptdCurr->m_td1TD1.byTCR |= (TCR_STP | TCR_EDP);
-        }
-    }
-
-    pTxBufHead->wFragCtl |= (unsigned short)wFragType;//0x0001; //0000 0000 0000 0001
-
-    //DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"s_vFillFragParameter END\n");
+	PSTxBufHead pTxBufHead = (PSTxBufHead) pbyBuffer;
+	//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "s_vFillFragParameter...\n");
+
+	if (uTxType == TYPE_SYNCDMA) {
+		//PSTxSyncDesc ptdCurr = (PSTxSyncDesc)s_pvGetTxDescHead(pDevice, uTxType, uCurIdx);
+		PSTxSyncDesc ptdCurr = (PSTxSyncDesc)pvtdCurr;
+
+		//Set FIFOCtl & TimeStamp in TxSyncDesc
+		ptdCurr->m_wFIFOCtl = pTxBufHead->wFIFOCtl;
+		ptdCurr->m_wTimeStamp = pTxBufHead->wTimeStamp;
+		//Set TSR1 & ReqCount in TxDescHead
+		ptdCurr->m_td1TD1.wReqCount = cpu_to_le16((unsigned short)(cbReqCount));
+		if (wFragType == FRAGCTL_ENDFRAG) { //Last Fragmentation
+			ptdCurr->m_td1TD1.byTCR |= (TCR_STP | TCR_EDP | EDMSDU);
+		} else {
+			ptdCurr->m_td1TD1.byTCR |= (TCR_STP | TCR_EDP);
+		}
+	} else {
+		//PSTxDesc ptdCurr = (PSTxDesc)s_pvGetTxDescHead(pDevice, uTxType, uCurIdx);
+		PSTxDesc ptdCurr = (PSTxDesc)pvtdCurr;
+		//Set TSR1 & ReqCount in TxDescHead
+		ptdCurr->m_td1TD1.wReqCount = cpu_to_le16((unsigned short)(cbReqCount));
+		if (wFragType == FRAGCTL_ENDFRAG) { //Last Fragmentation
+			ptdCurr->m_td1TD1.byTCR |= (TCR_STP | TCR_EDP | EDMSDU);
+		} else {
+			ptdCurr->m_td1TD1.byTCR |= (TCR_STP | TCR_EDP);
+		}
+	}
+
+	pTxBufHead->wFragCtl |= (unsigned short)wFragType;//0x0001; //0000 0000 0000 0001
+
+	//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "s_vFillFragParameter END\n");
 }
 
 static unsigned int
 s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyTxBufferAddr,
-	unsigned int cbFrameBodySize, unsigned int uDMAIdx, PSTxDesc pHeadTD,
-	PSEthernetHeader psEthHeader, unsigned char *pPacket, bool bNeedEncrypt,
-	PSKeyItem pTransmitKey, unsigned int uNodeIndex, unsigned int *puMACfragNum)
+		  unsigned int cbFrameBodySize, unsigned int uDMAIdx, PSTxDesc pHeadTD,
+		  PSEthernetHeader psEthHeader, unsigned char *pPacket, bool bNeedEncrypt,
+		  PSKeyItem pTransmitKey, unsigned int uNodeIndex, unsigned int *puMACfragNum)
 {
-    unsigned int cbMACHdLen;
-    unsigned int cbFrameSize;
-    unsigned int cbFragmentSize; //Hdr+(IV)+payoad+(MIC)+(ICV)+FCS
-    unsigned int cbFragPayloadSize;
-    unsigned int cbLastFragmentSize; //Hdr+(IV)+payoad+(MIC)+(ICV)+FCS
-    unsigned int cbLastFragPayloadSize;
-    unsigned int uFragIdx;
-    unsigned char *pbyPayloadHead;
-    unsigned char *pbyIVHead;
-    unsigned char *pbyMacHdr;
-    unsigned short wFragType; //00:Non-Frag, 01:Start, 10:Mid, 11:Last
-    unsigned int uDuration;
-    unsigned char *pbyBuffer;
+	unsigned int cbMACHdLen;
+	unsigned int cbFrameSize;
+	unsigned int cbFragmentSize; //Hdr+(IV)+payoad+(MIC)+(ICV)+FCS
+	unsigned int cbFragPayloadSize;
+	unsigned int cbLastFragmentSize; //Hdr+(IV)+payoad+(MIC)+(ICV)+FCS
+	unsigned int cbLastFragPayloadSize;
+	unsigned int uFragIdx;
+	unsigned char *pbyPayloadHead;
+	unsigned char *pbyIVHead;
+	unsigned char *pbyMacHdr;
+	unsigned short wFragType; //00:Non-Frag, 01:Start, 10:Mid, 11:Last
+	unsigned int uDuration;
+	unsigned char *pbyBuffer;
 //    unsigned int uKeyEntryIdx = NUM_KEY_ENTRY+1;
 //    unsigned char byKeySel = 0xFF;
-    unsigned int cbIVlen = 0;
-    unsigned int cbICVlen = 0;
-    unsigned int cbMIClen = 0;
-    unsigned int cbFCSlen = 4;
-    unsigned int cb802_1_H_len = 0;
-    unsigned int uLength = 0;
-    unsigned int uTmpLen = 0;
+	unsigned int cbIVlen = 0;
+	unsigned int cbICVlen = 0;
+	unsigned int cbMIClen = 0;
+	unsigned int cbFCSlen = 4;
+	unsigned int cb802_1_H_len = 0;
+	unsigned int uLength = 0;
+	unsigned int uTmpLen = 0;
 //    unsigned char abyTmp[8];
 //    unsigned long dwCRC;
-    unsigned int cbMICHDR = 0;
-    unsigned long dwMICKey0, dwMICKey1;
-    unsigned long dwMIC_Priority;
-    unsigned long *pdwMIC_L;
-    unsigned long *pdwMIC_R;
-    unsigned long dwSafeMIC_L, dwSafeMIC_R; //Fix "Last Frag Size" < "MIC length".
-    bool bMIC2Frag = false;
-    unsigned int uMICFragLen = 0;
-    unsigned int uMACfragNum = 1;
-    unsigned int uPadding = 0;
-    unsigned int cbReqCount = 0;
-
-    bool bNeedACK;
-    bool bRTS;
-    bool bIsAdhoc;
-    unsigned char *pbyType;
-    PSTxDesc       ptdCurr;
-    PSTxBufHead    psTxBufHd = (PSTxBufHead) pbyTxBufferAddr;
+	unsigned int cbMICHDR = 0;
+	unsigned long dwMICKey0, dwMICKey1;
+	unsigned long dwMIC_Priority;
+	unsigned long *pdwMIC_L;
+	unsigned long *pdwMIC_R;
+	unsigned long dwSafeMIC_L, dwSafeMIC_R; //Fix "Last Frag Size" < "MIC length".
+	bool bMIC2Frag = false;
+	unsigned int uMICFragLen = 0;
+	unsigned int uMACfragNum = 1;
+	unsigned int uPadding = 0;
+	unsigned int cbReqCount = 0;
+
+	bool bNeedACK;
+	bool bRTS;
+	bool bIsAdhoc;
+	unsigned char *pbyType;
+	PSTxDesc       ptdCurr;
+	PSTxBufHead    psTxBufHd = (PSTxBufHead) pbyTxBufferAddr;
 //    unsigned int tmpDescIdx;
-    unsigned int cbHeaderLength = 0;
-    void *         pvRrvTime;
-    PSMICHDRHead   pMICHDR;
-    void *         pvRTS;
-    void *         pvCTS;
-    void *         pvTxDataHd;
-    unsigned short wTxBufSize;   // FFinfo size
-    unsigned int uTotalCopyLength = 0;
-    unsigned char byFBOption = AUTO_FB_NONE;
-    bool bIsWEP256 = false;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-
-
-    pvRrvTime = pMICHDR = pvRTS = pvCTS = pvTxDataHd = NULL;
-
-    //DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"s_cbFillTxBufHead...\n");
-    if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
-        (pDevice->eOPMode == OP_MODE_AP)) {
-
-	if (is_multicast_ether_addr(&(psEthHeader->abyDstAddr[0])))
-		bNeedACK = false;
-        else
-            bNeedACK = true;
-        bIsAdhoc = true;
-    }
-    else {
-        // MSDUs in Infra mode always need ACK
-        bNeedACK = true;
-        bIsAdhoc = false;
-    }
-
-    if (pDevice->bLongHeader)
-        cbMACHdLen = WLAN_HDR_ADDR3_LEN + 6;
-    else
-        cbMACHdLen = WLAN_HDR_ADDR3_LEN;
-
-
-    if ((bNeedEncrypt == true) && (pTransmitKey != NULL)) {
-        if (pTransmitKey->byCipherSuite == KEY_CTL_WEP) {
-            cbIVlen = 4;
-            cbICVlen = 4;
-            if (pTransmitKey->uKeyLength == WLAN_WEP232_KEYLEN) {
-                bIsWEP256 = true;
-            }
-        }
-        if (pTransmitKey->byCipherSuite == KEY_CTL_TKIP) {
-            cbIVlen = 8;//IV+ExtIV
-            cbMIClen = 8;
-            cbICVlen = 4;
-        }
-        if (pTransmitKey->byCipherSuite == KEY_CTL_CCMP) {
-            cbIVlen = 8;//RSN Header
-            cbICVlen = 8;//MIC
-            cbMICHDR = sizeof(SMICHDRHead);
-        }
-        if (pDevice->byLocalID > REV_ID_VT3253_A1) {
-            //MAC Header should be padding 0 to DW alignment.
-            uPadding = 4 - (cbMACHdLen%4);
-            uPadding %= 4;
-        }
-    }
-
-
-    cbFrameSize = cbMACHdLen + cbIVlen + (cbFrameBodySize + cbMIClen) + cbICVlen + cbFCSlen;
-
-    if ((bNeedACK == false) ||
-        (cbFrameSize < pDevice->wRTSThreshold) ||
-        ((cbFrameSize >= pDevice->wFragmentationThreshold) && (pDevice->wFragmentationThreshold <= pDevice->wRTSThreshold))
-        ) {
-        bRTS = false;
-    }
-    else {
-        bRTS = true;
-        psTxBufHd->wFIFOCtl |= (FIFOCTL_RTS | FIFOCTL_LRETRY);
-    }
-    //
-    // Use for AUTO FALL BACK
-    //
-    if (psTxBufHd->wFIFOCtl & FIFOCTL_AUTO_FB_0) {
-        byFBOption = AUTO_FB_0;
-    }
-    else if (psTxBufHd->wFIFOCtl & FIFOCTL_AUTO_FB_1) {
-        byFBOption = AUTO_FB_1;
-    }
-
-    //////////////////////////////////////////////////////
-    //Set RrvTime/RTS/CTS Buffer
-    wTxBufSize = sizeof(STxBufHead);
-    if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {//802.11g packet
-
-        if (byFBOption == AUTO_FB_NONE) {
-            if (bRTS == true) {//RTS_need
-                pvRrvTime = (PSRrvTime_gRTS) (pbyTxBufferAddr + wTxBufSize);
-                pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gRTS));
-                pvRTS = (PSRTS_g) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gRTS) + cbMICHDR);
-                pvCTS = NULL;
-                pvTxDataHd = (PSTxDataHead_g) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gRTS) + cbMICHDR + sizeof(SRTS_g));
-                cbHeaderLength = wTxBufSize + sizeof(SRrvTime_gRTS) + cbMICHDR + sizeof(SRTS_g) + sizeof(STxDataHead_g);
-            }
-            else { //RTS_needless
-                pvRrvTime = (PSRrvTime_gCTS) (pbyTxBufferAddr + wTxBufSize);
-                pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS));
-                pvRTS = NULL;
-                pvCTS = (PSCTS) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR);
-                pvTxDataHd = (PSTxDataHead_g) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR + sizeof(SCTS));
-                cbHeaderLength = wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR + sizeof(SCTS) + sizeof(STxDataHead_g);
-            }
-        } else {
-            // Auto Fall Back
-            if (bRTS == true) {//RTS_need
-                pvRrvTime = (PSRrvTime_gRTS) (pbyTxBufferAddr + wTxBufSize);
-                pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gRTS));
-                pvRTS = (PSRTS_g_FB) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gRTS) + cbMICHDR);
-                pvCTS = NULL;
-                pvTxDataHd = (PSTxDataHead_g_FB) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gRTS) + cbMICHDR + sizeof(SRTS_g_FB));
-                cbHeaderLength = wTxBufSize + sizeof(SRrvTime_gRTS) + cbMICHDR + sizeof(SRTS_g_FB) + sizeof(STxDataHead_g_FB);
-            }
-            else { //RTS_needless
-                pvRrvTime = (PSRrvTime_gCTS) (pbyTxBufferAddr + wTxBufSize);
-                pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS));
-                pvRTS = NULL;
-                pvCTS = (PSCTS_FB) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR);
-                pvTxDataHd = (PSTxDataHead_g_FB) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR + sizeof(SCTS_FB));
-                cbHeaderLength = wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR + sizeof(SCTS_FB) + sizeof(STxDataHead_g_FB);
-            }
-        } // Auto Fall Back
-    }
-    else {//802.11a/b packet
-
-        if (byFBOption == AUTO_FB_NONE) {
-            if (bRTS == true) {
-                pvRrvTime = (PSRrvTime_ab) (pbyTxBufferAddr + wTxBufSize);
-                pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab));
-                pvRTS = (PSRTS_ab) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab) + cbMICHDR);
-                pvCTS = NULL;
-                pvTxDataHd = (PSTxDataHead_ab) (pbyTxBufferAddr + wTxBufSize + sizeof(PSRrvTime_ab) + cbMICHDR + sizeof(SRTS_ab));
-                cbHeaderLength = wTxBufSize + sizeof(PSRrvTime_ab) + cbMICHDR + sizeof(SRTS_ab) + sizeof(STxDataHead_ab);
-            }
-            else { //RTS_needless, need MICHDR
-                pvRrvTime = (PSRrvTime_ab) (pbyTxBufferAddr + wTxBufSize);
-                pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab));
-                pvRTS = NULL;
-                pvCTS = NULL;
-                pvTxDataHd = (PSTxDataHead_ab) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab) + cbMICHDR);
-                cbHeaderLength = wTxBufSize + sizeof(SRrvTime_ab) + cbMICHDR + sizeof(STxDataHead_ab);
-            }
-        } else {
-            // Auto Fall Back
-            if (bRTS == true) {//RTS_need
-                pvRrvTime = (PSRrvTime_ab) (pbyTxBufferAddr + wTxBufSize);
-                pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab));
-                pvRTS = (PSRTS_a_FB) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab) + cbMICHDR);
-                pvCTS = NULL;
-                pvTxDataHd = (PSTxDataHead_a_FB) (pbyTxBufferAddr + wTxBufSize + sizeof(PSRrvTime_ab) + cbMICHDR + sizeof(SRTS_a_FB));
-                cbHeaderLength = wTxBufSize + sizeof(PSRrvTime_ab) + cbMICHDR + sizeof(SRTS_a_FB) + sizeof(STxDataHead_a_FB);
-            }
-            else { //RTS_needless
-                pvRrvTime = (PSRrvTime_ab) (pbyTxBufferAddr + wTxBufSize);
-                pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab));
-                pvRTS = NULL;
-                pvCTS = NULL;
-                pvTxDataHd = (PSTxDataHead_a_FB) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab) + cbMICHDR);
-                cbHeaderLength = wTxBufSize + sizeof(SRrvTime_ab) + cbMICHDR + sizeof(STxDataHead_a_FB);
-            }
-        } // Auto Fall Back
-    }
-    memset((void *)(pbyTxBufferAddr + wTxBufSize), 0, (cbHeaderLength - wTxBufSize));
+	unsigned int cbHeaderLength = 0;
+	void *pvRrvTime;
+	PSMICHDRHead   pMICHDR;
+	void *pvRTS;
+	void *pvCTS;
+	void *pvTxDataHd;
+	unsigned short wTxBufSize;   // FFinfo size
+	unsigned int uTotalCopyLength = 0;
+	unsigned char byFBOption = AUTO_FB_NONE;
+	bool bIsWEP256 = false;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+
+	pvRrvTime = pMICHDR = pvRTS = pvCTS = pvTxDataHd = NULL;
+
+	//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "s_cbFillTxBufHead...\n");
+	if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
+	    (pDevice->eOPMode == OP_MODE_AP)) {
+		if (is_multicast_ether_addr(&(psEthHeader->abyDstAddr[0])))
+			bNeedACK = false;
+		else
+			bNeedACK = true;
+		bIsAdhoc = true;
+	} else {
+		// MSDUs in Infra mode always need ACK
+		bNeedACK = true;
+		bIsAdhoc = false;
+	}
+
+	if (pDevice->bLongHeader)
+		cbMACHdLen = WLAN_HDR_ADDR3_LEN + 6;
+	else
+		cbMACHdLen = WLAN_HDR_ADDR3_LEN;
+
+	if ((bNeedEncrypt == true) && (pTransmitKey != NULL)) {
+		if (pTransmitKey->byCipherSuite == KEY_CTL_WEP) {
+			cbIVlen = 4;
+			cbICVlen = 4;
+			if (pTransmitKey->uKeyLength == WLAN_WEP232_KEYLEN) {
+				bIsWEP256 = true;
+			}
+		}
+		if (pTransmitKey->byCipherSuite == KEY_CTL_TKIP) {
+			cbIVlen = 8;//IV+ExtIV
+			cbMIClen = 8;
+			cbICVlen = 4;
+		}
+		if (pTransmitKey->byCipherSuite == KEY_CTL_CCMP) {
+			cbIVlen = 8;//RSN Header
+			cbICVlen = 8;//MIC
+			cbMICHDR = sizeof(SMICHDRHead);
+		}
+		if (pDevice->byLocalID > REV_ID_VT3253_A1) {
+			//MAC Header should be padding 0 to DW alignment.
+			uPadding = 4 - (cbMACHdLen%4);
+			uPadding %= 4;
+		}
+	}
+
+	cbFrameSize = cbMACHdLen + cbIVlen + (cbFrameBodySize + cbMIClen) + cbICVlen + cbFCSlen;
+
+	if ((bNeedACK == false) ||
+	    (cbFrameSize < pDevice->wRTSThreshold) ||
+	    ((cbFrameSize >= pDevice->wFragmentationThreshold) && (pDevice->wFragmentationThreshold <= pDevice->wRTSThreshold))
+) {
+		bRTS = false;
+	} else {
+		bRTS = true;
+		psTxBufHd->wFIFOCtl |= (FIFOCTL_RTS | FIFOCTL_LRETRY);
+	}
+	//
+	// Use for AUTO FALL BACK
+	//
+	if (psTxBufHd->wFIFOCtl & FIFOCTL_AUTO_FB_0) {
+		byFBOption = AUTO_FB_0;
+	} else if (psTxBufHd->wFIFOCtl & FIFOCTL_AUTO_FB_1) {
+		byFBOption = AUTO_FB_1;
+	}
+
+	//////////////////////////////////////////////////////
+	//Set RrvTime/RTS/CTS Buffer
+	wTxBufSize = sizeof(STxBufHead);
+	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {//802.11g packet
+
+		if (byFBOption == AUTO_FB_NONE) {
+			if (bRTS == true) {//RTS_need
+				pvRrvTime = (PSRrvTime_gRTS) (pbyTxBufferAddr + wTxBufSize);
+				pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gRTS));
+				pvRTS = (PSRTS_g) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gRTS) + cbMICHDR);
+				pvCTS = NULL;
+				pvTxDataHd = (PSTxDataHead_g) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gRTS) + cbMICHDR + sizeof(SRTS_g));
+				cbHeaderLength = wTxBufSize + sizeof(SRrvTime_gRTS) + cbMICHDR + sizeof(SRTS_g) + sizeof(STxDataHead_g);
+			} else { //RTS_needless
+				pvRrvTime = (PSRrvTime_gCTS) (pbyTxBufferAddr + wTxBufSize);
+				pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS));
+				pvRTS = NULL;
+				pvCTS = (PSCTS) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR);
+				pvTxDataHd = (PSTxDataHead_g) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR + sizeof(SCTS));
+				cbHeaderLength = wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR + sizeof(SCTS) + sizeof(STxDataHead_g);
+			}
+		} else {
+			// Auto Fall Back
+			if (bRTS == true) {//RTS_need
+				pvRrvTime = (PSRrvTime_gRTS) (pbyTxBufferAddr + wTxBufSize);
+				pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gRTS));
+				pvRTS = (PSRTS_g_FB) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gRTS) + cbMICHDR);
+				pvCTS = NULL;
+				pvTxDataHd = (PSTxDataHead_g_FB) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gRTS) + cbMICHDR + sizeof(SRTS_g_FB));
+				cbHeaderLength = wTxBufSize + sizeof(SRrvTime_gRTS) + cbMICHDR + sizeof(SRTS_g_FB) + sizeof(STxDataHead_g_FB);
+			} else { //RTS_needless
+				pvRrvTime = (PSRrvTime_gCTS) (pbyTxBufferAddr + wTxBufSize);
+				pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS));
+				pvRTS = NULL;
+				pvCTS = (PSCTS_FB) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR);
+				pvTxDataHd = (PSTxDataHead_g_FB) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR + sizeof(SCTS_FB));
+				cbHeaderLength = wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR + sizeof(SCTS_FB) + sizeof(STxDataHead_g_FB);
+			}
+		} // Auto Fall Back
+	} else {//802.11a/b packet
+
+		if (byFBOption == AUTO_FB_NONE) {
+			if (bRTS == true) {
+				pvRrvTime = (PSRrvTime_ab) (pbyTxBufferAddr + wTxBufSize);
+				pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab));
+				pvRTS = (PSRTS_ab) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab) + cbMICHDR);
+				pvCTS = NULL;
+				pvTxDataHd = (PSTxDataHead_ab) (pbyTxBufferAddr + wTxBufSize + sizeof(PSRrvTime_ab) + cbMICHDR + sizeof(SRTS_ab));
+				cbHeaderLength = wTxBufSize + sizeof(PSRrvTime_ab) + cbMICHDR + sizeof(SRTS_ab) + sizeof(STxDataHead_ab);
+			} else { //RTS_needless, need MICHDR
+				pvRrvTime = (PSRrvTime_ab) (pbyTxBufferAddr + wTxBufSize);
+				pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab));
+				pvRTS = NULL;
+				pvCTS = NULL;
+				pvTxDataHd = (PSTxDataHead_ab) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab) + cbMICHDR);
+				cbHeaderLength = wTxBufSize + sizeof(SRrvTime_ab) + cbMICHDR + sizeof(STxDataHead_ab);
+			}
+		} else {
+			// Auto Fall Back
+			if (bRTS == true) {//RTS_need
+				pvRrvTime = (PSRrvTime_ab) (pbyTxBufferAddr + wTxBufSize);
+				pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab));
+				pvRTS = (PSRTS_a_FB) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab) + cbMICHDR);
+				pvCTS = NULL;
+				pvTxDataHd = (PSTxDataHead_a_FB) (pbyTxBufferAddr + wTxBufSize + sizeof(PSRrvTime_ab) + cbMICHDR + sizeof(SRTS_a_FB));
+				cbHeaderLength = wTxBufSize + sizeof(PSRrvTime_ab) + cbMICHDR + sizeof(SRTS_a_FB) + sizeof(STxDataHead_a_FB);
+			} else { //RTS_needless
+				pvRrvTime = (PSRrvTime_ab) (pbyTxBufferAddr + wTxBufSize);
+				pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab));
+				pvRTS = NULL;
+				pvCTS = NULL;
+				pvTxDataHd = (PSTxDataHead_a_FB) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab) + cbMICHDR);
+				cbHeaderLength = wTxBufSize + sizeof(SRrvTime_ab) + cbMICHDR + sizeof(STxDataHead_a_FB);
+			}
+		} // Auto Fall Back
+	}
+	memset((void *)(pbyTxBufferAddr + wTxBufSize), 0, (cbHeaderLength - wTxBufSize));
 
 //////////////////////////////////////////////////////////////////
-    if ((bNeedEncrypt == true) && (pTransmitKey != NULL) && (pTransmitKey->byCipherSuite == KEY_CTL_TKIP)) {
-        if (pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) {
-            dwMICKey0 = *(unsigned long *)(&pTransmitKey->abyKey[16]);
-            dwMICKey1 = *(unsigned long *)(&pTransmitKey->abyKey[20]);
-        }
-        else if ((pTransmitKey->dwKeyIndex & AUTHENTICATOR_KEY) != 0) {
-            dwMICKey0 = *(unsigned long *)(&pTransmitKey->abyKey[16]);
-            dwMICKey1 = *(unsigned long *)(&pTransmitKey->abyKey[20]);
-        }
-        else {
-            dwMICKey0 = *(unsigned long *)(&pTransmitKey->abyKey[24]);
-            dwMICKey1 = *(unsigned long *)(&pTransmitKey->abyKey[28]);
-        }
-        // DO Software Michael
-        MIC_vInit(dwMICKey0, dwMICKey1);
-        MIC_vAppend((unsigned char *)&(psEthHeader->abyDstAddr[0]), 12);
-        dwMIC_Priority = 0;
-        MIC_vAppend((unsigned char *)&dwMIC_Priority, 4);
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"MIC KEY: %lX, %lX\n", dwMICKey0, dwMICKey1);
-    }
+	if ((bNeedEncrypt == true) && (pTransmitKey != NULL) && (pTransmitKey->byCipherSuite == KEY_CTL_TKIP)) {
+		if (pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) {
+			dwMICKey0 = *(unsigned long *)(&pTransmitKey->abyKey[16]);
+			dwMICKey1 = *(unsigned long *)(&pTransmitKey->abyKey[20]);
+		} else if ((pTransmitKey->dwKeyIndex & AUTHENTICATOR_KEY) != 0) {
+			dwMICKey0 = *(unsigned long *)(&pTransmitKey->abyKey[16]);
+			dwMICKey1 = *(unsigned long *)(&pTransmitKey->abyKey[20]);
+		} else {
+			dwMICKey0 = *(unsigned long *)(&pTransmitKey->abyKey[24]);
+			dwMICKey1 = *(unsigned long *)(&pTransmitKey->abyKey[28]);
+		}
+		// DO Software Michael
+		MIC_vInit(dwMICKey0, dwMICKey1);
+		MIC_vAppend((unsigned char *)&(psEthHeader->abyDstAddr[0]), 12);
+		dwMIC_Priority = 0;
+		MIC_vAppend((unsigned char *)&dwMIC_Priority, 4);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "MIC KEY: %lX, %lX\n", dwMICKey0, dwMICKey1);
+	}
 
 ///////////////////////////////////////////////////////////////////
 
-    pbyMacHdr = (unsigned char *)(pbyTxBufferAddr + cbHeaderLength);
-    pbyPayloadHead = (unsigned char *)(pbyMacHdr + cbMACHdLen + uPadding + cbIVlen);
-    pbyIVHead = (unsigned char *)(pbyMacHdr + cbMACHdLen + uPadding);
-
-    if ((cbFrameSize > pDevice->wFragmentationThreshold) && (bNeedACK == true) && (bIsWEP256 == false)) {
-        // Fragmentation
-        // FragThreshold = Fragment size(Hdr+(IV)+fragment payload+(MIC)+(ICV)+FCS)
-        cbFragmentSize = pDevice->wFragmentationThreshold;
-        cbFragPayloadSize = cbFragmentSize - cbMACHdLen - cbIVlen - cbICVlen - cbFCSlen;
-        //FragNum = (FrameSize-(Hdr+FCS))/(Fragment Size -(Hrd+FCS)))
-        uMACfragNum = (unsigned short) ((cbFrameBodySize + cbMIClen) / cbFragPayloadSize);
-        cbLastFragPayloadSize = (cbFrameBodySize + cbMIClen) % cbFragPayloadSize;
-        if (cbLastFragPayloadSize == 0) {
-            cbLastFragPayloadSize = cbFragPayloadSize;
-        } else {
-            uMACfragNum++;
-        }
-        //[Hdr+(IV)+last fragment payload+(MIC)+(ICV)+FCS]
-        cbLastFragmentSize = cbMACHdLen + cbLastFragPayloadSize + cbIVlen + cbICVlen + cbFCSlen;
-
-        for (uFragIdx = 0; uFragIdx < uMACfragNum; uFragIdx ++) {
-            if (uFragIdx == 0) {
-                //=========================
-                //    Start Fragmentation
-                //=========================
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Start Fragmentation...\n");
-                wFragType = FRAGCTL_STAFRAG;
-
-
-                //Fill FIFO,RrvTime,RTS,and CTS
-                s_vGenerateTxParameter(pDevice, byPktType, (void *)psTxBufHd, pvRrvTime, pvRTS, pvCTS,
-                                       cbFragmentSize, bNeedACK, uDMAIdx, psEthHeader, pDevice->wCurrentRate);
-                //Fill DataHead
-                uDuration = s_uFillDataHead(pDevice, byPktType, pvTxDataHd, cbFragmentSize, uDMAIdx, bNeedACK,
-                                            uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption, pDevice->wCurrentRate);
-                // Generate TX MAC Header
-                vGenerateMACHeader(pDevice, pbyMacHdr, (unsigned short)uDuration, psEthHeader, bNeedEncrypt,
-                                   wFragType, uDMAIdx, uFragIdx);
-
-                if (bNeedEncrypt == true) {
-                    //Fill TXKEY
-                    s_vFillTxKey(pDevice, (unsigned char *)(psTxBufHd->adwTxKey), pbyIVHead, pTransmitKey,
-                                 pbyMacHdr, (unsigned short)cbFragPayloadSize, (unsigned char *)pMICHDR);
-                    //Fill IV(ExtIV,RSNHDR)
-                    if (pDevice->bEnableHostWEP) {
-                        pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16 = pTransmitKey->dwTSC47_16;
-                        pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0 = pTransmitKey->wTSC15_0;
-                    }
-                }
-
-
-                // 802.1H
-                if (ntohs(psEthHeader->wType) > ETH_DATA_LEN) {
-                    if ((psEthHeader->wType == TYPE_PKT_IPX) ||
-                        (psEthHeader->wType == cpu_to_le16(0xF380))) {
-                        memcpy((unsigned char *) (pbyPayloadHead), &pDevice->abySNAP_Bridgetunnel[0], 6);
-                    }
-                    else {
-                        memcpy((unsigned char *) (pbyPayloadHead), &pDevice->abySNAP_RFC1042[0], 6);
-                    }
-                    pbyType = (unsigned char *) (pbyPayloadHead + 6);
-                    memcpy(pbyType, &(psEthHeader->wType), sizeof(unsigned short));
-                    cb802_1_H_len = 8;
-                }
-
-                cbReqCount = cbHeaderLength + cbMACHdLen + uPadding + cbIVlen + cbFragPayloadSize;
-                //---------------------------
-                // S/W or H/W Encryption
-                //---------------------------
-                //Fill MICHDR
-                //if (pDevice->bAES) {
-                //    s_vFillMICHDR(pDevice, (unsigned char *)pMICHDR, pbyMacHdr, (unsigned short)cbFragPayloadSize);
-                //}
-                //cbReqCount += s_uDoEncryption(pDevice, psEthHeader, (void *)psTxBufHd, byKeySel,
-                //                                pbyPayloadHead, (unsigned short)cbFragPayloadSize, uDMAIdx);
-
-
-
-                //pbyBuffer = (unsigned char *)pDevice->aamTxBuf[uDMAIdx][uDescIdx].pbyVAddr;
-                pbyBuffer = (unsigned char *)pHeadTD->pTDInfo->buf;
-
-                uLength = cbHeaderLength + cbMACHdLen + uPadding + cbIVlen + cb802_1_H_len;
-                //copy TxBufferHeader + MacHeader to desc
-                memcpy(pbyBuffer, (void *)psTxBufHd, uLength);
-
-                // Copy the Packet into a tx Buffer
-                memcpy((pbyBuffer + uLength), (pPacket + 14), (cbFragPayloadSize - cb802_1_H_len));
-
-
-                uTotalCopyLength += cbFragPayloadSize - cb802_1_H_len;
-
-                if ((bNeedEncrypt == true) && (pTransmitKey != NULL) && (pTransmitKey->byCipherSuite == KEY_CTL_TKIP)) {
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Start MIC: %d\n", cbFragPayloadSize);
-                    MIC_vAppend((pbyBuffer + uLength - cb802_1_H_len), cbFragPayloadSize);
-
-                }
-
-                //---------------------------
-                // S/W Encryption
-                //---------------------------
-                if ((pDevice->byLocalID <= REV_ID_VT3253_A1)) {
-                    if (bNeedEncrypt) {
-                        s_vSWencryption(pDevice, pTransmitKey, (pbyBuffer + uLength - cb802_1_H_len), (unsigned short)cbFragPayloadSize);
-                        cbReqCount += cbICVlen;
-                    }
-                }
-
-                ptdCurr = (PSTxDesc)pHeadTD;
-                //--------------------
-                //1.Set TSR1 & ReqCount in TxDescHead
-                //2.Set FragCtl in TxBufferHead
-                //3.Set Frame Control
-                //4.Set Sequence Control
-                //5.Get S/W generate FCS
-                //--------------------
-                s_vFillFragParameter(pDevice, pbyBuffer, uDMAIdx, (void *)ptdCurr, wFragType, cbReqCount);
-
-                ptdCurr->pTDInfo->dwReqCount = cbReqCount - uPadding;
-                ptdCurr->pTDInfo->dwHeaderLength = cbHeaderLength;
-                ptdCurr->pTDInfo->skb_dma = ptdCurr->pTDInfo->buf_dma;
-                ptdCurr->buff_addr = cpu_to_le32(ptdCurr->pTDInfo->skb_dma);
-                pDevice->iTDUsed[uDMAIdx]++;
-                pHeadTD = ptdCurr->next;
-            }
-            else if (uFragIdx == (uMACfragNum-1)) {
-                //=========================
-                //    Last Fragmentation
-                //=========================
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Last Fragmentation...\n");
-                //tmpDescIdx = (uDescIdx + uFragIdx) % pDevice->cbTD[uDMAIdx];
-
-                wFragType = FRAGCTL_ENDFRAG;
-
-                //Fill FIFO,RrvTime,RTS,and CTS
-                s_vGenerateTxParameter(pDevice, byPktType, (void *)psTxBufHd, pvRrvTime, pvRTS, pvCTS,
-                                       cbLastFragmentSize, bNeedACK, uDMAIdx, psEthHeader, pDevice->wCurrentRate);
-                //Fill DataHead
-                uDuration = s_uFillDataHead(pDevice, byPktType, pvTxDataHd, cbLastFragmentSize, uDMAIdx, bNeedACK,
-                                            uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption, pDevice->wCurrentRate);
-
-                // Generate TX MAC Header
-                vGenerateMACHeader(pDevice, pbyMacHdr, (unsigned short)uDuration, psEthHeader, bNeedEncrypt,
-                                   wFragType, uDMAIdx, uFragIdx);
-
-                if (bNeedEncrypt == true) {
-                    //Fill TXKEY
-                    s_vFillTxKey(pDevice, (unsigned char *)(psTxBufHd->adwTxKey), pbyIVHead, pTransmitKey,
-                                 pbyMacHdr, (unsigned short)cbLastFragPayloadSize, (unsigned char *)pMICHDR);
-
-                    if (pDevice->bEnableHostWEP) {
-                        pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16 = pTransmitKey->dwTSC47_16;
-                        pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0 = pTransmitKey->wTSC15_0;
-                    }
-
-                }
-
-
-                cbReqCount = cbHeaderLength + cbMACHdLen + uPadding + cbIVlen + cbLastFragPayloadSize;
-                //---------------------------
-                // S/W or H/W Encryption
-                //---------------------------
-
-
-
-                pbyBuffer = (unsigned char *)pHeadTD->pTDInfo->buf;
-                //pbyBuffer = (unsigned char *)pDevice->aamTxBuf[uDMAIdx][tmpDescIdx].pbyVAddr;
-
-                uLength = cbHeaderLength + cbMACHdLen + uPadding + cbIVlen;
-
-                //copy TxBufferHeader + MacHeader to desc
-                memcpy(pbyBuffer, (void *)psTxBufHd, uLength);
-
-                // Copy the Packet into a tx Buffer
-                if (bMIC2Frag == false) {
-
-                    memcpy((pbyBuffer + uLength),
-                             (pPacket + 14 + uTotalCopyLength),
-                             (cbLastFragPayloadSize - cbMIClen)
-                             );
-                    //TODO check uTmpLen !
-                    uTmpLen = cbLastFragPayloadSize - cbMIClen;
-
-                }
-                if ((bNeedEncrypt == true) && (pTransmitKey != NULL) && (pTransmitKey->byCipherSuite == KEY_CTL_TKIP)) {
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"LAST: uMICFragLen:%d, cbLastFragPayloadSize:%d, uTmpLen:%d\n",
-                                   uMICFragLen, cbLastFragPayloadSize, uTmpLen);
-
-                    if (bMIC2Frag == false) {
-                        if (uTmpLen != 0)
-                            MIC_vAppend((pbyBuffer + uLength), uTmpLen);
-                        pdwMIC_L = (unsigned long *)(pbyBuffer + uLength + uTmpLen);
-                        pdwMIC_R = (unsigned long *)(pbyBuffer + uLength + uTmpLen + 4);
-                        MIC_vGetMIC(pdwMIC_L, pdwMIC_R);
-                        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Last MIC:%lX, %lX\n", *pdwMIC_L, *pdwMIC_R);
-                    } else {
-                        if (uMICFragLen >= 4) {
-                            memcpy((pbyBuffer + uLength), ((unsigned char *)&dwSafeMIC_R + (uMICFragLen - 4)),
-                                     (cbMIClen - uMICFragLen));
-                            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"LAST: uMICFragLen >= 4: %X, %d\n",
-                                           *(unsigned char *)((unsigned char *)&dwSafeMIC_R + (uMICFragLen - 4)),
-                                           (cbMIClen - uMICFragLen));
-
-                        } else {
-                            memcpy((pbyBuffer + uLength), ((unsigned char *)&dwSafeMIC_L + uMICFragLen),
-                                     (4 - uMICFragLen));
-                            memcpy((pbyBuffer + uLength + (4 - uMICFragLen)), &dwSafeMIC_R, 4);
-                            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"LAST: uMICFragLen < 4: %X, %d\n",
-                                           *(unsigned char *)((unsigned char *)&dwSafeMIC_R + uMICFragLen - 4),
-                                           (cbMIClen - uMICFragLen));
-                        }
-                        /*
-                        for (ii = 0; ii < cbLastFragPayloadSize + 8 + 24; ii++) {
-                            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"%02x ", *((unsigned char *)((pbyBuffer + uLength) + ii - 8 - 24)));
-                        }
-                        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"\n\n");
-                        */
-                    }
-                    MIC_vUnInit();
-                } else {
-                    ASSERT(uTmpLen == (cbLastFragPayloadSize - cbMIClen));
-                }
-
-
-                //---------------------------
-                // S/W Encryption
-                //---------------------------
-                if ((pDevice->byLocalID <= REV_ID_VT3253_A1)) {
-                    if (bNeedEncrypt) {
-                        s_vSWencryption(pDevice, pTransmitKey, (pbyBuffer + uLength), (unsigned short)cbLastFragPayloadSize);
-                        cbReqCount += cbICVlen;
-                    }
-                }
-
-                ptdCurr = (PSTxDesc)pHeadTD;
-
-                //--------------------
-                //1.Set TSR1 & ReqCount in TxDescHead
-                //2.Set FragCtl in TxBufferHead
-                //3.Set Frame Control
-                //4.Set Sequence Control
-                //5.Get S/W generate FCS
-                //--------------------
-
-
-                s_vFillFragParameter(pDevice, pbyBuffer, uDMAIdx, (void *)ptdCurr, wFragType, cbReqCount);
-
-                ptdCurr->pTDInfo->dwReqCount = cbReqCount - uPadding;
-                ptdCurr->pTDInfo->dwHeaderLength = cbHeaderLength;
-                ptdCurr->pTDInfo->skb_dma = ptdCurr->pTDInfo->buf_dma;
-                ptdCurr->buff_addr = cpu_to_le32(ptdCurr->pTDInfo->skb_dma);
-                pDevice->iTDUsed[uDMAIdx]++;
-                pHeadTD = ptdCurr->next;
-
-            }
-            else {
-                //=========================
-                //    Middle Fragmentation
-                //=========================
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Middle Fragmentation...\n");
-                //tmpDescIdx = (uDescIdx + uFragIdx) % pDevice->cbTD[uDMAIdx];
-
-                wFragType = FRAGCTL_MIDFRAG;
-
-                //Fill FIFO,RrvTime,RTS,and CTS
-                s_vGenerateTxParameter(pDevice, byPktType, (void *)psTxBufHd, pvRrvTime, pvRTS, pvCTS,
-                                       cbFragmentSize, bNeedACK, uDMAIdx, psEthHeader, pDevice->wCurrentRate);
-                //Fill DataHead
-                uDuration = s_uFillDataHead(pDevice, byPktType, pvTxDataHd, cbFragmentSize, uDMAIdx, bNeedACK,
-                                            uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption, pDevice->wCurrentRate);
-
-                // Generate TX MAC Header
-                vGenerateMACHeader(pDevice, pbyMacHdr, (unsigned short)uDuration, psEthHeader, bNeedEncrypt,
-                                   wFragType, uDMAIdx, uFragIdx);
-
-
-                if (bNeedEncrypt == true) {
-                    //Fill TXKEY
-                    s_vFillTxKey(pDevice, (unsigned char *)(psTxBufHd->adwTxKey), pbyIVHead, pTransmitKey,
-                                 pbyMacHdr, (unsigned short)cbFragPayloadSize, (unsigned char *)pMICHDR);
-
-                    if (pDevice->bEnableHostWEP) {
-                        pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16 = pTransmitKey->dwTSC47_16;
-                        pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0 = pTransmitKey->wTSC15_0;
-                    }
-                }
-
-                cbReqCount = cbHeaderLength + cbMACHdLen + uPadding + cbIVlen + cbFragPayloadSize;
-                //---------------------------
-                // S/W or H/W Encryption
-                //---------------------------
-                //Fill MICHDR
-                //if (pDevice->bAES) {
-                //    s_vFillMICHDR(pDevice, (unsigned char *)pMICHDR, pbyMacHdr, (unsigned short)cbFragPayloadSize);
-                //}
-                //cbReqCount += s_uDoEncryption(pDevice, psEthHeader, (void *)psTxBufHd, byKeySel,
-                //                              pbyPayloadHead, (unsigned short)cbFragPayloadSize, uDMAIdx);
-
-
-                pbyBuffer = (unsigned char *)pHeadTD->pTDInfo->buf;
-                //pbyBuffer = (unsigned char *)pDevice->aamTxBuf[uDMAIdx][tmpDescIdx].pbyVAddr;
-
-
-                uLength = cbHeaderLength + cbMACHdLen + uPadding + cbIVlen;
-
-                //copy TxBufferHeader + MacHeader to desc
-                memcpy(pbyBuffer, (void *)psTxBufHd, uLength);
-
-                // Copy the Packet into a tx Buffer
-                memcpy((pbyBuffer + uLength),
-                         (pPacket + 14 + uTotalCopyLength),
-                         cbFragPayloadSize
-                        );
-                uTmpLen = cbFragPayloadSize;
-
-                uTotalCopyLength += uTmpLen;
-
-                if ((bNeedEncrypt == true) && (pTransmitKey != NULL) && (pTransmitKey->byCipherSuite == KEY_CTL_TKIP)) {
-
-                    MIC_vAppend((pbyBuffer + uLength), uTmpLen);
-
-                    if (uTmpLen < cbFragPayloadSize) {
-                        bMIC2Frag = true;
-                        uMICFragLen = cbFragPayloadSize - uTmpLen;
-                        ASSERT(uMICFragLen < cbMIClen);
-
-                        pdwMIC_L = (unsigned long *)(pbyBuffer + uLength + uTmpLen);
-                        pdwMIC_R = (unsigned long *)(pbyBuffer + uLength + uTmpLen + 4);
-                        MIC_vGetMIC(pdwMIC_L, pdwMIC_R);
-                        dwSafeMIC_L = *pdwMIC_L;
-                        dwSafeMIC_R = *pdwMIC_R;
-
-                        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"MIDDLE: uMICFragLen:%d, cbFragPayloadSize:%d, uTmpLen:%d\n",
-                                       uMICFragLen, cbFragPayloadSize, uTmpLen);
-                        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Fill MIC in Middle frag [%d]\n", uMICFragLen);
-                        /*
-                        for (ii = 0; ii < uMICFragLen; ii++) {
-                            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"%02x ", *((unsigned char *)((pbyBuffer + uLength + uTmpLen) + ii)));
-                        }
-                        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"\n");
-                        */
-                        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Get MIC:%lX, %lX\n", *pdwMIC_L, *pdwMIC_R);
-                    }
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Middle frag len: %d\n", uTmpLen);
-                    /*
-                    for (ii = 0; ii < uTmpLen; ii++) {
-                        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"%02x ", *((unsigned char *)((pbyBuffer + uLength) + ii)));
-                    }
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"\n\n");
-                    */
-
-                } else {
-                    ASSERT(uTmpLen == (cbFragPayloadSize));
-                }
-
-                if ((pDevice->byLocalID <= REV_ID_VT3253_A1)) {
-                    if (bNeedEncrypt) {
-                        s_vSWencryption(pDevice, pTransmitKey, (pbyBuffer + uLength), (unsigned short)cbFragPayloadSize);
-                        cbReqCount += cbICVlen;
-                    }
-                }
-
-                ptdCurr = (PSTxDesc)pHeadTD;
-
-                //--------------------
-                //1.Set TSR1 & ReqCount in TxDescHead
-                //2.Set FragCtl in TxBufferHead
-                //3.Set Frame Control
-                //4.Set Sequence Control
-                //5.Get S/W generate FCS
-                //--------------------
-
-                s_vFillFragParameter(pDevice, pbyBuffer, uDMAIdx, (void *)ptdCurr, wFragType, cbReqCount);
-
-                ptdCurr->pTDInfo->dwReqCount = cbReqCount - uPadding;
-                ptdCurr->pTDInfo->dwHeaderLength = cbHeaderLength;
-                ptdCurr->pTDInfo->skb_dma = ptdCurr->pTDInfo->buf_dma;
-                ptdCurr->buff_addr = cpu_to_le32(ptdCurr->pTDInfo->skb_dma);
-                pDevice->iTDUsed[uDMAIdx]++;
-                pHeadTD = ptdCurr->next;
-            }
-        }  // for (uMACfragNum)
-    }
-    else {
-        //=========================
-        //    No Fragmentation
-        //=========================
-        //DBG_PRTGRP03(("No Fragmentation...\n"));
-        //DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"No Fragmentation...\n");
-        wFragType = FRAGCTL_NONFRAG;
-
-        //Set FragCtl in TxBufferHead
-        psTxBufHd->wFragCtl |= (unsigned short)wFragType;
-
-        //Fill FIFO,RrvTime,RTS,and CTS
-        s_vGenerateTxParameter(pDevice, byPktType, (void *)psTxBufHd, pvRrvTime, pvRTS, pvCTS,
-                               cbFrameSize, bNeedACK, uDMAIdx, psEthHeader, pDevice->wCurrentRate);
-        //Fill DataHead
-        uDuration = s_uFillDataHead(pDevice, byPktType, pvTxDataHd, cbFrameSize, uDMAIdx, bNeedACK,
-                                    0, 0, uMACfragNum, byFBOption, pDevice->wCurrentRate);
-
-        // Generate TX MAC Header
-        vGenerateMACHeader(pDevice, pbyMacHdr, (unsigned short)uDuration, psEthHeader, bNeedEncrypt,
-                           wFragType, uDMAIdx, 0);
-
-        if (bNeedEncrypt == true) {
-            //Fill TXKEY
-            s_vFillTxKey(pDevice, (unsigned char *)(psTxBufHd->adwTxKey), pbyIVHead, pTransmitKey,
-                         pbyMacHdr, (unsigned short)cbFrameBodySize, (unsigned char *)pMICHDR);
-
-            if (pDevice->bEnableHostWEP) {
-                pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16 = pTransmitKey->dwTSC47_16;
-                pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0 = pTransmitKey->wTSC15_0;
-            }
-        }
-
-        // 802.1H
-        if (ntohs(psEthHeader->wType) > ETH_DATA_LEN) {
-            if ((psEthHeader->wType == TYPE_PKT_IPX) ||
-                (psEthHeader->wType == cpu_to_le16(0xF380))) {
-                memcpy((unsigned char *) (pbyPayloadHead), &pDevice->abySNAP_Bridgetunnel[0], 6);
-            }
-            else {
-                memcpy((unsigned char *) (pbyPayloadHead), &pDevice->abySNAP_RFC1042[0], 6);
-            }
-            pbyType = (unsigned char *) (pbyPayloadHead + 6);
-            memcpy(pbyType, &(psEthHeader->wType), sizeof(unsigned short));
-            cb802_1_H_len = 8;
-        }
-
-        cbReqCount = cbHeaderLength + cbMACHdLen + uPadding + cbIVlen + (cbFrameBodySize + cbMIClen);
-        //---------------------------
-        // S/W or H/W Encryption
-        //---------------------------
-        //Fill MICHDR
-        //if (pDevice->bAES) {
-        //    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Fill MICHDR...\n");
-        //    s_vFillMICHDR(pDevice, (unsigned char *)pMICHDR, pbyMacHdr, (unsigned short)cbFrameBodySize);
-        //}
-
-        pbyBuffer = (unsigned char *)pHeadTD->pTDInfo->buf;
-        //pbyBuffer = (unsigned char *)pDevice->aamTxBuf[uDMAIdx][uDescIdx].pbyVAddr;
-
-        uLength = cbHeaderLength + cbMACHdLen + uPadding + cbIVlen + cb802_1_H_len;
-
-        //copy TxBufferHeader + MacHeader to desc
-        memcpy(pbyBuffer, (void *)psTxBufHd, uLength);
-
-        // Copy the Packet into a tx Buffer
-        memcpy((pbyBuffer + uLength),
-                 (pPacket + 14),
-                 cbFrameBodySize - cb802_1_H_len
-                 );
-
-        if ((bNeedEncrypt == true) && (pTransmitKey != NULL) && (pTransmitKey->byCipherSuite == KEY_CTL_TKIP)){
-
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Length:%d, %d\n", cbFrameBodySize - cb802_1_H_len, uLength);
-            /*
-            for (ii = 0; ii < (cbFrameBodySize - cb802_1_H_len); ii++) {
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"%02x ", *((unsigned char *)((pbyBuffer + uLength) + ii)));
-            }
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"\n");
-            */
-
-            MIC_vAppend((pbyBuffer + uLength - cb802_1_H_len), cbFrameBodySize);
-
-            pdwMIC_L = (unsigned long *)(pbyBuffer + uLength - cb802_1_H_len + cbFrameBodySize);
-            pdwMIC_R = (unsigned long *)(pbyBuffer + uLength - cb802_1_H_len + cbFrameBodySize + 4);
-
-            MIC_vGetMIC(pdwMIC_L, pdwMIC_R);
-            MIC_vUnInit();
-
-
-            if (pDevice->bTxMICFail == true) {
-                *pdwMIC_L = 0;
-                *pdwMIC_R = 0;
-                pDevice->bTxMICFail = false;
-            }
-
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"uLength: %d, %d\n", uLength, cbFrameBodySize);
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"cbReqCount:%d, %d, %d, %d\n", cbReqCount, cbHeaderLength, uPadding, cbIVlen);
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"MIC:%lx, %lx\n", *pdwMIC_L, *pdwMIC_R);
+	pbyMacHdr = (unsigned char *)(pbyTxBufferAddr + cbHeaderLength);
+	pbyPayloadHead = (unsigned char *)(pbyMacHdr + cbMACHdLen + uPadding + cbIVlen);
+	pbyIVHead = (unsigned char *)(pbyMacHdr + cbMACHdLen + uPadding);
+
+	if ((cbFrameSize > pDevice->wFragmentationThreshold) && (bNeedACK == true) && (bIsWEP256 == false)) {
+		// Fragmentation
+		// FragThreshold = Fragment size(Hdr+(IV)+fragment payload+(MIC)+(ICV)+FCS)
+		cbFragmentSize = pDevice->wFragmentationThreshold;
+		cbFragPayloadSize = cbFragmentSize - cbMACHdLen - cbIVlen - cbICVlen - cbFCSlen;
+		//FragNum = (FrameSize-(Hdr+FCS))/(Fragment Size -(Hrd+FCS)))
+		uMACfragNum = (unsigned short) ((cbFrameBodySize + cbMIClen) / cbFragPayloadSize);
+		cbLastFragPayloadSize = (cbFrameBodySize + cbMIClen) % cbFragPayloadSize;
+		if (cbLastFragPayloadSize == 0) {
+			cbLastFragPayloadSize = cbFragPayloadSize;
+		} else {
+			uMACfragNum++;
+		}
+		//[Hdr+(IV)+last fragment payload+(MIC)+(ICV)+FCS]
+		cbLastFragmentSize = cbMACHdLen + cbLastFragPayloadSize + cbIVlen + cbICVlen + cbFCSlen;
+
+		for (uFragIdx = 0; uFragIdx < uMACfragNum; uFragIdx++) {
+			if (uFragIdx == 0) {
+				//=========================
+				//    Start Fragmentation
+				//=========================
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Start Fragmentation...\n");
+				wFragType = FRAGCTL_STAFRAG;
+
+				//Fill FIFO,RrvTime,RTS,and CTS
+				s_vGenerateTxParameter(pDevice, byPktType, (void *)psTxBufHd, pvRrvTime, pvRTS, pvCTS,
+						       cbFragmentSize, bNeedACK, uDMAIdx, psEthHeader, pDevice->wCurrentRate);
+				//Fill DataHead
+				uDuration = s_uFillDataHead(pDevice, byPktType, pvTxDataHd, cbFragmentSize, uDMAIdx, bNeedACK,
+							    uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption, pDevice->wCurrentRate);
+				// Generate TX MAC Header
+				vGenerateMACHeader(pDevice, pbyMacHdr, (unsigned short)uDuration, psEthHeader, bNeedEncrypt,
+						   wFragType, uDMAIdx, uFragIdx);
+
+				if (bNeedEncrypt == true) {
+					//Fill TXKEY
+					s_vFillTxKey(pDevice, (unsigned char *)(psTxBufHd->adwTxKey), pbyIVHead, pTransmitKey,
+						     pbyMacHdr, (unsigned short)cbFragPayloadSize, (unsigned char *)pMICHDR);
+					//Fill IV(ExtIV,RSNHDR)
+					if (pDevice->bEnableHostWEP) {
+						pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16 = pTransmitKey->dwTSC47_16;
+						pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0 = pTransmitKey->wTSC15_0;
+					}
+				}
+
+				// 802.1H
+				if (ntohs(psEthHeader->wType) > ETH_DATA_LEN) {
+					if ((psEthHeader->wType == TYPE_PKT_IPX) ||
+					    (psEthHeader->wType == cpu_to_le16(0xF380))) {
+						memcpy((unsigned char *)(pbyPayloadHead), &pDevice->abySNAP_Bridgetunnel[0], 6);
+					} else {
+						memcpy((unsigned char *)(pbyPayloadHead), &pDevice->abySNAP_RFC1042[0], 6);
+					}
+					pbyType = (unsigned char *)(pbyPayloadHead + 6);
+					memcpy(pbyType, &(psEthHeader->wType), sizeof(unsigned short));
+					cb802_1_H_len = 8;
+				}
+
+				cbReqCount = cbHeaderLength + cbMACHdLen + uPadding + cbIVlen + cbFragPayloadSize;
+				//---------------------------
+				// S/W or H/W Encryption
+				//---------------------------
+				pbyBuffer = (unsigned char *)pHeadTD->pTDInfo->buf;
+
+				uLength = cbHeaderLength + cbMACHdLen + uPadding + cbIVlen + cb802_1_H_len;
+				//copy TxBufferHeader + MacHeader to desc
+				memcpy(pbyBuffer, (void *)psTxBufHd, uLength);
+
+				// Copy the Packet into a tx Buffer
+				memcpy((pbyBuffer + uLength), (pPacket + 14), (cbFragPayloadSize - cb802_1_H_len));
+
+				uTotalCopyLength += cbFragPayloadSize - cb802_1_H_len;
+
+				if ((bNeedEncrypt == true) && (pTransmitKey != NULL) && (pTransmitKey->byCipherSuite == KEY_CTL_TKIP)) {
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Start MIC: %d\n", cbFragPayloadSize);
+					MIC_vAppend((pbyBuffer + uLength - cb802_1_H_len), cbFragPayloadSize);
+
+				}
+
+				//---------------------------
+				// S/W Encryption
+				//---------------------------
+				if ((pDevice->byLocalID <= REV_ID_VT3253_A1)) {
+					if (bNeedEncrypt) {
+						s_vSWencryption(pDevice, pTransmitKey, (pbyBuffer + uLength - cb802_1_H_len), (unsigned short)cbFragPayloadSize);
+						cbReqCount += cbICVlen;
+					}
+				}
+
+				ptdCurr = (PSTxDesc)pHeadTD;
+				//--------------------
+				//1.Set TSR1 & ReqCount in TxDescHead
+				//2.Set FragCtl in TxBufferHead
+				//3.Set Frame Control
+				//4.Set Sequence Control
+				//5.Get S/W generate FCS
+				//--------------------
+				s_vFillFragParameter(pDevice, pbyBuffer, uDMAIdx, (void *)ptdCurr, wFragType, cbReqCount);
+
+				ptdCurr->pTDInfo->dwReqCount = cbReqCount - uPadding;
+				ptdCurr->pTDInfo->dwHeaderLength = cbHeaderLength;
+				ptdCurr->pTDInfo->skb_dma = ptdCurr->pTDInfo->buf_dma;
+				ptdCurr->buff_addr = cpu_to_le32(ptdCurr->pTDInfo->skb_dma);
+				pDevice->iTDUsed[uDMAIdx]++;
+				pHeadTD = ptdCurr->next;
+			} else if (uFragIdx == (uMACfragNum-1)) {
+				//=========================
+				//    Last Fragmentation
+				//=========================
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Last Fragmentation...\n");
+				//tmpDescIdx = (uDescIdx + uFragIdx) % pDevice->cbTD[uDMAIdx];
+
+				wFragType = FRAGCTL_ENDFRAG;
+
+				//Fill FIFO,RrvTime,RTS,and CTS
+				s_vGenerateTxParameter(pDevice, byPktType, (void *)psTxBufHd, pvRrvTime, pvRTS, pvCTS,
+						       cbLastFragmentSize, bNeedACK, uDMAIdx, psEthHeader, pDevice->wCurrentRate);
+				//Fill DataHead
+				uDuration = s_uFillDataHead(pDevice, byPktType, pvTxDataHd, cbLastFragmentSize, uDMAIdx, bNeedACK,
+							    uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption, pDevice->wCurrentRate);
+
+				// Generate TX MAC Header
+				vGenerateMACHeader(pDevice, pbyMacHdr, (unsigned short)uDuration, psEthHeader, bNeedEncrypt,
+						   wFragType, uDMAIdx, uFragIdx);
+
+				if (bNeedEncrypt == true) {
+					//Fill TXKEY
+					s_vFillTxKey(pDevice, (unsigned char *)(psTxBufHd->adwTxKey), pbyIVHead, pTransmitKey,
+						     pbyMacHdr, (unsigned short)cbLastFragPayloadSize, (unsigned char *)pMICHDR);
+
+					if (pDevice->bEnableHostWEP) {
+						pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16 = pTransmitKey->dwTSC47_16;
+						pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0 = pTransmitKey->wTSC15_0;
+					}
+
+				}
+
+				cbReqCount = cbHeaderLength + cbMACHdLen + uPadding + cbIVlen + cbLastFragPayloadSize;
+				//---------------------------
+				// S/W or H/W Encryption
+				//---------------------------
+
+				pbyBuffer = (unsigned char *)pHeadTD->pTDInfo->buf;
+				//pbyBuffer = (unsigned char *)pDevice->aamTxBuf[uDMAIdx][tmpDescIdx].pbyVAddr;
+
+				uLength = cbHeaderLength + cbMACHdLen + uPadding + cbIVlen;
+
+				//copy TxBufferHeader + MacHeader to desc
+				memcpy(pbyBuffer, (void *)psTxBufHd, uLength);
+
+				// Copy the Packet into a tx Buffer
+				if (bMIC2Frag == false) {
+					memcpy((pbyBuffer + uLength),
+					       (pPacket + 14 + uTotalCopyLength),
+					       (cbLastFragPayloadSize - cbMIClen)
+);
+					//TODO check uTmpLen !
+					uTmpLen = cbLastFragPayloadSize - cbMIClen;
+
+				}
+				if ((bNeedEncrypt == true) && (pTransmitKey != NULL) && (pTransmitKey->byCipherSuite == KEY_CTL_TKIP)) {
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "LAST: uMICFragLen:%d, cbLastFragPayloadSize:%d, uTmpLen:%d\n",
+						uMICFragLen, cbLastFragPayloadSize, uTmpLen);
+
+					if (bMIC2Frag == false) {
+						if (uTmpLen != 0)
+							MIC_vAppend((pbyBuffer + uLength), uTmpLen);
+						pdwMIC_L = (unsigned long *)(pbyBuffer + uLength + uTmpLen);
+						pdwMIC_R = (unsigned long *)(pbyBuffer + uLength + uTmpLen + 4);
+						MIC_vGetMIC(pdwMIC_L, pdwMIC_R);
+						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Last MIC:%lX, %lX\n", *pdwMIC_L, *pdwMIC_R);
+					} else {
+						if (uMICFragLen >= 4) {
+							memcpy((pbyBuffer + uLength), ((unsigned char *)&dwSafeMIC_R + (uMICFragLen - 4)),
+							       (cbMIClen - uMICFragLen));
+							DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "LAST: uMICFragLen >= 4: %X, %d\n",
+								*(unsigned char *)((unsigned char *)&dwSafeMIC_R + (uMICFragLen - 4)),
+								(cbMIClen - uMICFragLen));
+
+						} else {
+							memcpy((pbyBuffer + uLength), ((unsigned char *)&dwSafeMIC_L + uMICFragLen),
+							       (4 - uMICFragLen));
+							memcpy((pbyBuffer + uLength + (4 - uMICFragLen)), &dwSafeMIC_R, 4);
+							DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "LAST: uMICFragLen < 4: %X, %d\n",
+								*(unsigned char *)((unsigned char *)&dwSafeMIC_R + uMICFragLen - 4),
+								(cbMIClen - uMICFragLen));
+						}
+						/*
+						  for (ii = 0; ii < cbLastFragPayloadSize + 8 + 24; ii++) {
+						  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", *((unsigned char *)((pbyBuffer + uLength) + ii - 8 - 24)));
+						  }
+						  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n\n");
+						*/
+					}
+					MIC_vUnInit();
+				} else {
+					ASSERT(uTmpLen == (cbLastFragPayloadSize - cbMIClen));
+				}
+
+				//---------------------------
+				// S/W Encryption
+				//---------------------------
+				if ((pDevice->byLocalID <= REV_ID_VT3253_A1)) {
+					if (bNeedEncrypt) {
+						s_vSWencryption(pDevice, pTransmitKey, (pbyBuffer + uLength), (unsigned short)cbLastFragPayloadSize);
+						cbReqCount += cbICVlen;
+					}
+				}
+
+				ptdCurr = (PSTxDesc)pHeadTD;
+
+				//--------------------
+				//1.Set TSR1 & ReqCount in TxDescHead
+				//2.Set FragCtl in TxBufferHead
+				//3.Set Frame Control
+				//4.Set Sequence Control
+				//5.Get S/W generate FCS
+				//--------------------
+
+				s_vFillFragParameter(pDevice, pbyBuffer, uDMAIdx, (void *)ptdCurr, wFragType, cbReqCount);
+
+				ptdCurr->pTDInfo->dwReqCount = cbReqCount - uPadding;
+				ptdCurr->pTDInfo->dwHeaderLength = cbHeaderLength;
+				ptdCurr->pTDInfo->skb_dma = ptdCurr->pTDInfo->buf_dma;
+				ptdCurr->buff_addr = cpu_to_le32(ptdCurr->pTDInfo->skb_dma);
+				pDevice->iTDUsed[uDMAIdx]++;
+				pHeadTD = ptdCurr->next;
+
+			} else {
+				//=========================
+				//    Middle Fragmentation
+				//=========================
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Middle Fragmentation...\n");
+				//tmpDescIdx = (uDescIdx + uFragIdx) % pDevice->cbTD[uDMAIdx];
+
+				wFragType = FRAGCTL_MIDFRAG;
+
+				//Fill FIFO,RrvTime,RTS,and CTS
+				s_vGenerateTxParameter(pDevice, byPktType, (void *)psTxBufHd, pvRrvTime, pvRTS, pvCTS,
+						       cbFragmentSize, bNeedACK, uDMAIdx, psEthHeader, pDevice->wCurrentRate);
+				//Fill DataHead
+				uDuration = s_uFillDataHead(pDevice, byPktType, pvTxDataHd, cbFragmentSize, uDMAIdx, bNeedACK,
+							    uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption, pDevice->wCurrentRate);
+
+				// Generate TX MAC Header
+				vGenerateMACHeader(pDevice, pbyMacHdr, (unsigned short)uDuration, psEthHeader, bNeedEncrypt,
+						   wFragType, uDMAIdx, uFragIdx);
+
+				if (bNeedEncrypt == true) {
+					//Fill TXKEY
+					s_vFillTxKey(pDevice, (unsigned char *)(psTxBufHd->adwTxKey), pbyIVHead, pTransmitKey,
+						     pbyMacHdr, (unsigned short)cbFragPayloadSize, (unsigned char *)pMICHDR);
+
+					if (pDevice->bEnableHostWEP) {
+						pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16 = pTransmitKey->dwTSC47_16;
+						pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0 = pTransmitKey->wTSC15_0;
+					}
+				}
+
+				cbReqCount = cbHeaderLength + cbMACHdLen + uPadding + cbIVlen + cbFragPayloadSize;
+				//---------------------------
+				// S/W or H/W Encryption
+				//---------------------------
+
+				pbyBuffer = (unsigned char *)pHeadTD->pTDInfo->buf;
+				uLength = cbHeaderLength + cbMACHdLen + uPadding + cbIVlen;
+
+				//copy TxBufferHeader + MacHeader to desc
+				memcpy(pbyBuffer, (void *)psTxBufHd, uLength);
+
+				// Copy the Packet into a tx Buffer
+				memcpy((pbyBuffer + uLength),
+				       (pPacket + 14 + uTotalCopyLength),
+				       cbFragPayloadSize
+);
+				uTmpLen = cbFragPayloadSize;
+
+				uTotalCopyLength += uTmpLen;
+
+				if ((bNeedEncrypt == true) && (pTransmitKey != NULL) && (pTransmitKey->byCipherSuite == KEY_CTL_TKIP)) {
+					MIC_vAppend((pbyBuffer + uLength), uTmpLen);
+
+					if (uTmpLen < cbFragPayloadSize) {
+						bMIC2Frag = true;
+						uMICFragLen = cbFragPayloadSize - uTmpLen;
+						ASSERT(uMICFragLen < cbMIClen);
+
+						pdwMIC_L = (unsigned long *)(pbyBuffer + uLength + uTmpLen);
+						pdwMIC_R = (unsigned long *)(pbyBuffer + uLength + uTmpLen + 4);
+						MIC_vGetMIC(pdwMIC_L, pdwMIC_R);
+						dwSafeMIC_L = *pdwMIC_L;
+						dwSafeMIC_R = *pdwMIC_R;
+
+						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "MIDDLE: uMICFragLen:%d, cbFragPayloadSize:%d, uTmpLen:%d\n",
+							uMICFragLen, cbFragPayloadSize, uTmpLen);
+						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Fill MIC in Middle frag [%d]\n", uMICFragLen);
+						/*
+						  for (ii = 0; ii < uMICFragLen; ii++) {
+						  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", *((unsigned char *)((pbyBuffer + uLength + uTmpLen) + ii)));
+						  }
+						  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
+						*/
+						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Get MIC:%lX, %lX\n", *pdwMIC_L, *pdwMIC_R);
+					}
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Middle frag len: %d\n", uTmpLen);
+					/*
+					  for (ii = 0; ii < uTmpLen; ii++) {
+					  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", *((unsigned char *)((pbyBuffer + uLength) + ii)));
+					  }
+					  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n\n");
+					*/
+
+				} else {
+					ASSERT(uTmpLen == (cbFragPayloadSize));
+				}
+
+				if ((pDevice->byLocalID <= REV_ID_VT3253_A1)) {
+					if (bNeedEncrypt) {
+						s_vSWencryption(pDevice, pTransmitKey, (pbyBuffer + uLength), (unsigned short)cbFragPayloadSize);
+						cbReqCount += cbICVlen;
+					}
+				}
+
+				ptdCurr = (PSTxDesc)pHeadTD;
+
+				//--------------------
+				//1.Set TSR1 & ReqCount in TxDescHead
+				//2.Set FragCtl in TxBufferHead
+				//3.Set Frame Control
+				//4.Set Sequence Control
+				//5.Get S/W generate FCS
+				//--------------------
+
+				s_vFillFragParameter(pDevice, pbyBuffer, uDMAIdx, (void *)ptdCurr, wFragType, cbReqCount);
+
+				ptdCurr->pTDInfo->dwReqCount = cbReqCount - uPadding;
+				ptdCurr->pTDInfo->dwHeaderLength = cbHeaderLength;
+				ptdCurr->pTDInfo->skb_dma = ptdCurr->pTDInfo->buf_dma;
+				ptdCurr->buff_addr = cpu_to_le32(ptdCurr->pTDInfo->skb_dma);
+				pDevice->iTDUsed[uDMAIdx]++;
+				pHeadTD = ptdCurr->next;
+			}
+		}  // for (uMACfragNum)
+	} else {
+		//=========================
+		//    No Fragmentation
+		//=========================
+		//DBG_PRTGRP03(("No Fragmentation...\n"));
+		//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "No Fragmentation...\n");
+		wFragType = FRAGCTL_NONFRAG;
+
+		//Set FragCtl in TxBufferHead
+		psTxBufHd->wFragCtl |= (unsigned short)wFragType;
+
+		//Fill FIFO,RrvTime,RTS,and CTS
+		s_vGenerateTxParameter(pDevice, byPktType, (void *)psTxBufHd, pvRrvTime, pvRTS, pvCTS,
+				       cbFrameSize, bNeedACK, uDMAIdx, psEthHeader, pDevice->wCurrentRate);
+		//Fill DataHead
+		uDuration = s_uFillDataHead(pDevice, byPktType, pvTxDataHd, cbFrameSize, uDMAIdx, bNeedACK,
+					    0, 0, uMACfragNum, byFBOption, pDevice->wCurrentRate);
+
+		// Generate TX MAC Header
+		vGenerateMACHeader(pDevice, pbyMacHdr, (unsigned short)uDuration, psEthHeader, bNeedEncrypt,
+				   wFragType, uDMAIdx, 0);
+
+		if (bNeedEncrypt == true) {
+			//Fill TXKEY
+			s_vFillTxKey(pDevice, (unsigned char *)(psTxBufHd->adwTxKey), pbyIVHead, pTransmitKey,
+				     pbyMacHdr, (unsigned short)cbFrameBodySize, (unsigned char *)pMICHDR);
+
+			if (pDevice->bEnableHostWEP) {
+				pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16 = pTransmitKey->dwTSC47_16;
+				pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0 = pTransmitKey->wTSC15_0;
+			}
+		}
+
+		// 802.1H
+		if (ntohs(psEthHeader->wType) > ETH_DATA_LEN) {
+			if ((psEthHeader->wType == TYPE_PKT_IPX) ||
+			    (psEthHeader->wType == cpu_to_le16(0xF380))) {
+				memcpy((unsigned char *)(pbyPayloadHead), &pDevice->abySNAP_Bridgetunnel[0], 6);
+			} else {
+				memcpy((unsigned char *)(pbyPayloadHead), &pDevice->abySNAP_RFC1042[0], 6);
+			}
+			pbyType = (unsigned char *)(pbyPayloadHead + 6);
+			memcpy(pbyType, &(psEthHeader->wType), sizeof(unsigned short));
+			cb802_1_H_len = 8;
+		}
+
+		cbReqCount = cbHeaderLength + cbMACHdLen + uPadding + cbIVlen + (cbFrameBodySize + cbMIClen);
+		//---------------------------
+		// S/W or H/W Encryption
+		//---------------------------
+		pbyBuffer = (unsigned char *)pHeadTD->pTDInfo->buf;
+		uLength = cbHeaderLength + cbMACHdLen + uPadding + cbIVlen + cb802_1_H_len;
+
+		//copy TxBufferHeader + MacHeader to desc
+		memcpy(pbyBuffer, (void *)psTxBufHd, uLength);
+
+		// Copy the Packet into a tx Buffer
+		memcpy((pbyBuffer + uLength),
+		       (pPacket + 14),
+		       cbFrameBodySize - cb802_1_H_len
+);
+
+		if ((bNeedEncrypt == true) && (pTransmitKey != NULL) && (pTransmitKey->byCipherSuite == KEY_CTL_TKIP)) {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Length:%d, %d\n", cbFrameBodySize - cb802_1_H_len, uLength);
+			/*
+			  for (ii = 0; ii < (cbFrameBodySize - cb802_1_H_len); ii++) {
+			  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", *((unsigned char *)((pbyBuffer + uLength) + ii)));
+			  }
+			  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
+			*/
+
+			MIC_vAppend((pbyBuffer + uLength - cb802_1_H_len), cbFrameBodySize);
+
+			pdwMIC_L = (unsigned long *)(pbyBuffer + uLength - cb802_1_H_len + cbFrameBodySize);
+			pdwMIC_R = (unsigned long *)(pbyBuffer + uLength - cb802_1_H_len + cbFrameBodySize + 4);
+
+			MIC_vGetMIC(pdwMIC_L, pdwMIC_R);
+			MIC_vUnInit();
+
+			if (pDevice->bTxMICFail == true) {
+				*pdwMIC_L = 0;
+				*pdwMIC_R = 0;
+				pDevice->bTxMICFail = false;
+			}
+
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "uLength: %d, %d\n", uLength, cbFrameBodySize);
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "cbReqCount:%d, %d, %d, %d\n", cbReqCount, cbHeaderLength, uPadding, cbIVlen);
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "MIC:%lx, %lx\n", *pdwMIC_L, *pdwMIC_R);
 /*
-            for (ii = 0; ii < 8; ii++) {
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"%02x ", *(((unsigned char *)(pdwMIC_L) + ii)));
-            }
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"\n");
+  for (ii = 0; ii < 8; ii++) {
+  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", *(((unsigned char *)(pdwMIC_L) + ii)));
+  }
+  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
 */
 
-        }
-
-
-        if ((pDevice->byLocalID <= REV_ID_VT3253_A1)){
-            if (bNeedEncrypt) {
-                s_vSWencryption(pDevice, pTransmitKey, (pbyBuffer + uLength - cb802_1_H_len),
-                                (unsigned short)(cbFrameBodySize + cbMIClen));
-                cbReqCount += cbICVlen;
-            }
-        }
+		}
 
+		if ((pDevice->byLocalID <= REV_ID_VT3253_A1)) {
+			if (bNeedEncrypt) {
+				s_vSWencryption(pDevice, pTransmitKey, (pbyBuffer + uLength - cb802_1_H_len),
+						(unsigned short)(cbFrameBodySize + cbMIClen));
+				cbReqCount += cbICVlen;
+			}
+		}
 
-        ptdCurr = (PSTxDesc)pHeadTD;
+		ptdCurr = (PSTxDesc)pHeadTD;
 
-        ptdCurr->pTDInfo->dwReqCount = cbReqCount - uPadding;
-        ptdCurr->pTDInfo->dwHeaderLength = cbHeaderLength;
-        ptdCurr->pTDInfo->skb_dma = ptdCurr->pTDInfo->buf_dma;
-        ptdCurr->buff_addr = cpu_to_le32(ptdCurr->pTDInfo->skb_dma);
-  	    //Set TSR1 & ReqCount in TxDescHead
-        ptdCurr->m_td1TD1.byTCR |= (TCR_STP | TCR_EDP | EDMSDU);
-        ptdCurr->m_td1TD1.wReqCount = cpu_to_le16((unsigned short)(cbReqCount));
+		ptdCurr->pTDInfo->dwReqCount = cbReqCount - uPadding;
+		ptdCurr->pTDInfo->dwHeaderLength = cbHeaderLength;
+		ptdCurr->pTDInfo->skb_dma = ptdCurr->pTDInfo->buf_dma;
+		ptdCurr->buff_addr = cpu_to_le32(ptdCurr->pTDInfo->skb_dma);
+		//Set TSR1 & ReqCount in TxDescHead
+		ptdCurr->m_td1TD1.byTCR |= (TCR_STP | TCR_EDP | EDMSDU);
+		ptdCurr->m_td1TD1.wReqCount = cpu_to_le16((unsigned short)(cbReqCount));
 
-        pDevice->iTDUsed[uDMAIdx]++;
+		pDevice->iTDUsed[uDMAIdx]++;
 
+//   DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " ptdCurr->m_dwReserved0[%d] ptdCurr->m_dwReserved1[%d].\n", ptdCurr->pTDInfo->dwReqCount, ptdCurr->pTDInfo->dwHeaderLength);
+//   DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " cbHeaderLength[%d]\n", cbHeaderLength);
 
-//   DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" ptdCurr->m_dwReserved0[%d] ptdCurr->m_dwReserved1[%d].\n", ptdCurr->pTDInfo->dwReqCount, ptdCurr->pTDInfo->dwHeaderLength);
-//   DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" cbHeaderLength[%d]\n", cbHeaderLength);
-
-    }
-    *puMACfragNum = uMACfragNum;
-    //DBG_PRTGRP03(("s_cbFillTxBufHead END\n"));
-    return cbHeaderLength;
+	}
+	*puMACfragNum = uMACfragNum;
+	//DBG_PRTGRP03(("s_cbFillTxBufHead END\n"));
+	return cbHeaderLength;
 }
 
-
 void
 vGenerateFIFOHeader(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyTxBufferAddr,
-	bool bNeedEncrypt, unsigned int cbPayloadSize, unsigned int uDMAIdx,
-	PSTxDesc pHeadTD, PSEthernetHeader psEthHeader, unsigned char *pPacket,
-	PSKeyItem pTransmitKey, unsigned int uNodeIndex, unsigned int *puMACfragNum,
-	unsigned int *pcbHeaderSize)
+		    bool bNeedEncrypt, unsigned int cbPayloadSize, unsigned int uDMAIdx,
+		    PSTxDesc pHeadTD, PSEthernetHeader psEthHeader, unsigned char *pPacket,
+		    PSKeyItem pTransmitKey, unsigned int uNodeIndex, unsigned int *puMACfragNum,
+		    unsigned int *pcbHeaderSize)
 {
-    unsigned int wTxBufSize;       // FFinfo size
-    bool bNeedACK;
-    bool bIsAdhoc;
-    unsigned short cbMacHdLen;
-    PSTxBufHead     pTxBufHead = (PSTxBufHead) pbyTxBufferAddr;
-
-    wTxBufSize = sizeof(STxBufHead);
-
-    memset(pTxBufHead, 0, wTxBufSize);
-    //Set FIFOCTL_NEEDACK
-
-    if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
-        (pDevice->eOPMode == OP_MODE_AP)) {
-        if (is_multicast_ether_addr(&(psEthHeader->abyDstAddr[0]))) {
-            bNeedACK = false;
-            pTxBufHead->wFIFOCtl = pTxBufHead->wFIFOCtl & (~FIFOCTL_NEEDACK);
-        }
-        else {
-            bNeedACK = true;
-            pTxBufHead->wFIFOCtl |= FIFOCTL_NEEDACK;
-        }
-        bIsAdhoc = true;
-    }
-    else {
-        // MSDUs in Infra mode always need ACK
-        bNeedACK = true;
-        pTxBufHead->wFIFOCtl |= FIFOCTL_NEEDACK;
-        bIsAdhoc = false;
-    }
-
-
-    pTxBufHead->wFIFOCtl |= FIFOCTL_TMOEN;
-    pTxBufHead->wTimeStamp = cpu_to_le16(DEFAULT_MSDU_LIFETIME_RES_64us);
-
-    //Set FIFOCTL_LHEAD
-    if (pDevice->bLongHeader)
-        pTxBufHead->wFIFOCtl |= FIFOCTL_LHEAD;
-
-    //Set FIFOCTL_GENINT
-
-    pTxBufHead->wFIFOCtl |= FIFOCTL_GENINT;
-
-
-    //Set FIFOCTL_ISDMA0
-    if (TYPE_TXDMA0 == uDMAIdx) {
-        pTxBufHead->wFIFOCtl |= FIFOCTL_ISDMA0;
-    }
-
-    //Set FRAGCTL_MACHDCNT
-    if (pDevice->bLongHeader) {
-        cbMacHdLen = WLAN_HDR_ADDR3_LEN + 6;
-    } else {
-        cbMacHdLen = WLAN_HDR_ADDR3_LEN;
-    }
-    pTxBufHead->wFragCtl |= cpu_to_le16((unsigned short)(cbMacHdLen << 10));
-
-    //Set packet type
-    if (byPktType == PK_TYPE_11A) {//0000 0000 0000 0000
-        ;
-    }
-    else if (byPktType == PK_TYPE_11B) {//0000 0001 0000 0000
-        pTxBufHead->wFIFOCtl |= FIFOCTL_11B;
-    }
-    else if (byPktType == PK_TYPE_11GB) {//0000 0010 0000 0000
-        pTxBufHead->wFIFOCtl |= FIFOCTL_11GB;
-    }
-    else if (byPktType == PK_TYPE_11GA) {//0000 0011 0000 0000
-        pTxBufHead->wFIFOCtl |= FIFOCTL_11GA;
-    }
-    //Set FIFOCTL_GrpAckPolicy
-    if (pDevice->bGrpAckPolicy == true) {//0000 0100 0000 0000
-        pTxBufHead->wFIFOCtl |=	FIFOCTL_GRPACK;
-    }
-
-    //Set Auto Fallback Ctl
-    if (pDevice->wCurrentRate >= RATE_18M) {
-        if (pDevice->byAutoFBCtrl == AUTO_FB_0) {
-            pTxBufHead->wFIFOCtl |= FIFOCTL_AUTO_FB_0;
-        } else if (pDevice->byAutoFBCtrl == AUTO_FB_1) {
-            pTxBufHead->wFIFOCtl |= FIFOCTL_AUTO_FB_1;
-        }
-    }
-
-    //Set FRAGCTL_WEPTYP
-    pDevice->bAES = false;
-
-    //Set FRAGCTL_WEPTYP
-    if (pDevice->byLocalID > REV_ID_VT3253_A1) {
-        if ((bNeedEncrypt) && (pTransmitKey != NULL))  { //WEP enabled
-            if (pTransmitKey->byCipherSuite == KEY_CTL_TKIP) {
-                pTxBufHead->wFragCtl |= FRAGCTL_TKIP;
-            }
-            else if (pTransmitKey->byCipherSuite == KEY_CTL_WEP) { //WEP40 or WEP104
-                if (pTransmitKey->uKeyLength != WLAN_WEP232_KEYLEN)
-                    pTxBufHead->wFragCtl |= FRAGCTL_LEGACY;
-            }
-            else if (pTransmitKey->byCipherSuite == KEY_CTL_CCMP) { //CCMP
-                pTxBufHead->wFragCtl |= FRAGCTL_AES;
-            }
-        }
-    }
+	unsigned int wTxBufSize;       // FFinfo size
+	bool bNeedACK;
+	bool bIsAdhoc;
+	unsigned short cbMacHdLen;
+	PSTxBufHead     pTxBufHead = (PSTxBufHead) pbyTxBufferAddr;
+
+	wTxBufSize = sizeof(STxBufHead);
+
+	memset(pTxBufHead, 0, wTxBufSize);
+	//Set FIFOCTL_NEEDACK
+
+	if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
+	    (pDevice->eOPMode == OP_MODE_AP)) {
+		if (is_multicast_ether_addr(&(psEthHeader->abyDstAddr[0]))) {
+			bNeedACK = false;
+			pTxBufHead->wFIFOCtl = pTxBufHead->wFIFOCtl & (~FIFOCTL_NEEDACK);
+		} else {
+			bNeedACK = true;
+			pTxBufHead->wFIFOCtl |= FIFOCTL_NEEDACK;
+		}
+		bIsAdhoc = true;
+	} else {
+		// MSDUs in Infra mode always need ACK
+		bNeedACK = true;
+		pTxBufHead->wFIFOCtl |= FIFOCTL_NEEDACK;
+		bIsAdhoc = false;
+	}
+
+	pTxBufHead->wFIFOCtl |= FIFOCTL_TMOEN;
+	pTxBufHead->wTimeStamp = cpu_to_le16(DEFAULT_MSDU_LIFETIME_RES_64us);
+
+	//Set FIFOCTL_LHEAD
+	if (pDevice->bLongHeader)
+		pTxBufHead->wFIFOCtl |= FIFOCTL_LHEAD;
+
+	//Set FIFOCTL_GENINT
+
+	pTxBufHead->wFIFOCtl |= FIFOCTL_GENINT;
+
+	//Set FIFOCTL_ISDMA0
+	if (TYPE_TXDMA0 == uDMAIdx) {
+		pTxBufHead->wFIFOCtl |= FIFOCTL_ISDMA0;
+	}
+
+	//Set FRAGCTL_MACHDCNT
+	if (pDevice->bLongHeader) {
+		cbMacHdLen = WLAN_HDR_ADDR3_LEN + 6;
+	} else {
+		cbMacHdLen = WLAN_HDR_ADDR3_LEN;
+	}
+	pTxBufHead->wFragCtl |= cpu_to_le16((unsigned short)(cbMacHdLen << 10));
+
+	//Set packet type
+	if (byPktType == PK_TYPE_11A) {//0000 0000 0000 0000
+		;
+	} else if (byPktType == PK_TYPE_11B) {//0000 0001 0000 0000
+		pTxBufHead->wFIFOCtl |= FIFOCTL_11B;
+	} else if (byPktType == PK_TYPE_11GB) {//0000 0010 0000 0000
+		pTxBufHead->wFIFOCtl |= FIFOCTL_11GB;
+	} else if (byPktType == PK_TYPE_11GA) {//0000 0011 0000 0000
+		pTxBufHead->wFIFOCtl |= FIFOCTL_11GA;
+	}
+	//Set FIFOCTL_GrpAckPolicy
+	if (pDevice->bGrpAckPolicy == true) {//0000 0100 0000 0000
+		pTxBufHead->wFIFOCtl |=	FIFOCTL_GRPACK;
+	}
+
+	//Set Auto Fallback Ctl
+	if (pDevice->wCurrentRate >= RATE_18M) {
+		if (pDevice->byAutoFBCtrl == AUTO_FB_0) {
+			pTxBufHead->wFIFOCtl |= FIFOCTL_AUTO_FB_0;
+		} else if (pDevice->byAutoFBCtrl == AUTO_FB_1) {
+			pTxBufHead->wFIFOCtl |= FIFOCTL_AUTO_FB_1;
+		}
+	}
+
+	//Set FRAGCTL_WEPTYP
+	pDevice->bAES = false;
+
+	//Set FRAGCTL_WEPTYP
+	if (pDevice->byLocalID > REV_ID_VT3253_A1) {
+		if ((bNeedEncrypt) && (pTransmitKey != NULL))  { //WEP enabled
+			if (pTransmitKey->byCipherSuite == KEY_CTL_TKIP) {
+				pTxBufHead->wFragCtl |= FRAGCTL_TKIP;
+			} else if (pTransmitKey->byCipherSuite == KEY_CTL_WEP) { //WEP40 or WEP104
+				if (pTransmitKey->uKeyLength != WLAN_WEP232_KEYLEN)
+					pTxBufHead->wFragCtl |= FRAGCTL_LEGACY;
+			} else if (pTransmitKey->byCipherSuite == KEY_CTL_CCMP) { //CCMP
+				pTxBufHead->wFragCtl |= FRAGCTL_AES;
+			}
+		}
+	}
 
 #ifdef	PLICE_DEBUG
-	//printk("Func:vGenerateFIFOHeader:TxDataRate is %d,TxPower is %d\n",pDevice->wCurrentRate,pDevice->byCurPwr);
-
-	//if (pDevice->wCurrentRate <= 3)
-	//{
-	//	RFbRawSetPower(pDevice,36,pDevice->wCurrentRate);
-	//}
-	//else
-
 	RFbSetPower(pDevice, pDevice->wCurrentRate, pDevice->byCurrentCh);
 #endif
-		//if (pDevice->wCurrentRate == 3)
-		//pDevice->byCurPwr = 46;
-		pTxBufHead->byTxPower = pDevice->byCurPwr;
-
-
-
+	pTxBufHead->byTxPower = pDevice->byCurPwr;
 
 /*
-    if(pDevice->bEnableHostWEP)
-        pTxBufHead->wFragCtl &=  ~(FRAGCTL_TKIP | FRAGCTL_LEGACY |FRAGCTL_AES);
+  if (pDevice->bEnableHostWEP)
+  pTxBufHead->wFragCtl &=  ~(FRAGCTL_TKIP | FRAGCTL_LEGACY |FRAGCTL_AES);
 */
-    *pcbHeaderSize = s_cbFillTxBufHead(pDevice, byPktType, pbyTxBufferAddr, cbPayloadSize,
-                                   uDMAIdx, pHeadTD, psEthHeader, pPacket, bNeedEncrypt,
-                                   pTransmitKey, uNodeIndex, puMACfragNum);
+	*pcbHeaderSize = s_cbFillTxBufHead(pDevice, byPktType, pbyTxBufferAddr, cbPayloadSize,
+					   uDMAIdx, pHeadTD, psEthHeader, pPacket, bNeedEncrypt,
+					   pTransmitKey, uNodeIndex, puMACfragNum);
 
-    return;
+	return;
 }
 
-
-
-
 /*+
  *
  * Description:
@@ -2226,964 +2059,882 @@ vGenerateFIFOHeader(PSDevice pDevice, unsigned char byPktType, unsigned char *pb
  *
  * Return Value: none
  *
--*/
+ -*/
 
 void
-vGenerateMACHeader (
-    PSDevice         pDevice,
-    unsigned char *pbyBufferAddr,
-    unsigned short wDuration,
-    PSEthernetHeader psEthHeader,
-    bool bNeedEncrypt,
-    unsigned short wFragType,
-    unsigned int uDMAIdx,
-    unsigned int uFragIdx
-    )
+vGenerateMACHeader(
+	PSDevice         pDevice,
+	unsigned char *pbyBufferAddr,
+	unsigned short wDuration,
+	PSEthernetHeader psEthHeader,
+	bool bNeedEncrypt,
+	unsigned short wFragType,
+	unsigned int uDMAIdx,
+	unsigned int uFragIdx
+)
 {
-    PS802_11Header  pMACHeader = (PS802_11Header)pbyBufferAddr;
-
-    memset(pMACHeader, 0, (sizeof(S802_11Header)));  //- sizeof(pMACHeader->dwIV)));
-
-    if (uDMAIdx == TYPE_ATIMDMA) {
-    	pMACHeader->wFrameCtl = TYPE_802_11_ATIM;
-    } else {
-        pMACHeader->wFrameCtl = TYPE_802_11_DATA;
-    }
-
-    if (pDevice->eOPMode == OP_MODE_AP) {
-        memcpy(&(pMACHeader->abyAddr1[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
-        memcpy(&(pMACHeader->abyAddr2[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-        memcpy(&(pMACHeader->abyAddr3[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
-        pMACHeader->wFrameCtl |= FC_FROMDS;
-    }
-    else {
-        if (pDevice->eOPMode == OP_MODE_ADHOC) {
-            memcpy(&(pMACHeader->abyAddr1[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
-            memcpy(&(pMACHeader->abyAddr2[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
-            memcpy(&(pMACHeader->abyAddr3[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-        }
-        else {
-            memcpy(&(pMACHeader->abyAddr3[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
-            memcpy(&(pMACHeader->abyAddr2[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
-            memcpy(&(pMACHeader->abyAddr1[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-            pMACHeader->wFrameCtl |= FC_TODS;
-        }
-    }
-
-    if (bNeedEncrypt)
-        pMACHeader->wFrameCtl |= cpu_to_le16((unsigned short)WLAN_SET_FC_ISWEP(1));
-
-    pMACHeader->wDurationID = cpu_to_le16(wDuration);
-
-    if (pDevice->bLongHeader) {
-        PWLAN_80211HDR_A4 pMACA4Header  = (PWLAN_80211HDR_A4) pbyBufferAddr;
-        pMACHeader->wFrameCtl |= (FC_TODS | FC_FROMDS);
-        memcpy(pMACA4Header->abyAddr4, pDevice->abyBSSID, WLAN_ADDR_LEN);
-    }
-    pMACHeader->wSeqCtl = cpu_to_le16(pDevice->wSeqCounter << 4);
-
-    //Set FragNumber in Sequence Control
-    pMACHeader->wSeqCtl |= cpu_to_le16((unsigned short)uFragIdx);
-
-    if ((wFragType == FRAGCTL_ENDFRAG) || (wFragType == FRAGCTL_NONFRAG)) {
-        pDevice->wSeqCounter++;
-        if (pDevice->wSeqCounter > 0x0fff)
-            pDevice->wSeqCounter = 0;
-    }
-
-    if ((wFragType == FRAGCTL_STAFRAG) || (wFragType == FRAGCTL_MIDFRAG)) { //StartFrag or MidFrag
-        pMACHeader->wFrameCtl |= FC_MOREFRAG;
-    }
+	PS802_11Header  pMACHeader = (PS802_11Header)pbyBufferAddr;
+
+	memset(pMACHeader, 0, (sizeof(S802_11Header)));  //- sizeof(pMACHeader->dwIV)));
+
+	if (uDMAIdx == TYPE_ATIMDMA) {
+		pMACHeader->wFrameCtl = TYPE_802_11_ATIM;
+	} else {
+		pMACHeader->wFrameCtl = TYPE_802_11_DATA;
+	}
+
+	if (pDevice->eOPMode == OP_MODE_AP) {
+		memcpy(&(pMACHeader->abyAddr1[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
+		memcpy(&(pMACHeader->abyAddr2[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
+		memcpy(&(pMACHeader->abyAddr3[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
+		pMACHeader->wFrameCtl |= FC_FROMDS;
+	} else {
+		if (pDevice->eOPMode == OP_MODE_ADHOC) {
+			memcpy(&(pMACHeader->abyAddr1[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
+			memcpy(&(pMACHeader->abyAddr2[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
+			memcpy(&(pMACHeader->abyAddr3[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
+		} else {
+			memcpy(&(pMACHeader->abyAddr3[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
+			memcpy(&(pMACHeader->abyAddr2[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
+			memcpy(&(pMACHeader->abyAddr1[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
+			pMACHeader->wFrameCtl |= FC_TODS;
+		}
+	}
+
+	if (bNeedEncrypt)
+		pMACHeader->wFrameCtl |= cpu_to_le16((unsigned short)WLAN_SET_FC_ISWEP(1));
+
+	pMACHeader->wDurationID = cpu_to_le16(wDuration);
+
+	if (pDevice->bLongHeader) {
+		PWLAN_80211HDR_A4 pMACA4Header  = (PWLAN_80211HDR_A4) pbyBufferAddr;
+		pMACHeader->wFrameCtl |= (FC_TODS | FC_FROMDS);
+		memcpy(pMACA4Header->abyAddr4, pDevice->abyBSSID, WLAN_ADDR_LEN);
+	}
+	pMACHeader->wSeqCtl = cpu_to_le16(pDevice->wSeqCounter << 4);
+
+	//Set FragNumber in Sequence Control
+	pMACHeader->wSeqCtl |= cpu_to_le16((unsigned short)uFragIdx);
+
+	if ((wFragType == FRAGCTL_ENDFRAG) || (wFragType == FRAGCTL_NONFRAG)) {
+		pDevice->wSeqCounter++;
+		if (pDevice->wSeqCounter > 0x0fff)
+			pDevice->wSeqCounter = 0;
+	}
+
+	if ((wFragType == FRAGCTL_STAFRAG) || (wFragType == FRAGCTL_MIDFRAG)) { //StartFrag or MidFrag
+		pMACHeader->wFrameCtl |= FC_MOREFRAG;
+	}
 }
 
-
-
-
-
-
 CMD_STATUS csMgmt_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket) {
-
-    PSTxDesc        pFrstTD;
-    unsigned char byPktType;
-    unsigned char *pbyTxBufferAddr;
-    void *          pvRTS;
-    PSCTS           pCTS;
-    void *          pvTxDataHd;
-    unsigned int uDuration;
-    unsigned int cbReqCount;
-    PS802_11Header  pMACHeader;
-    unsigned int cbHeaderSize;
-    unsigned int cbFrameBodySize;
-    bool bNeedACK;
-    bool bIsPSPOLL = false;
-    PSTxBufHead     pTxBufHead;
-    unsigned int cbFrameSize;
-    unsigned int cbIVlen = 0;
-    unsigned int cbICVlen = 0;
-    unsigned int cbMIClen = 0;
-    unsigned int cbFCSlen = 4;
-    unsigned int uPadding = 0;
-    unsigned short wTxBufSize;
-    unsigned int cbMacHdLen;
-    SEthernetHeader sEthHeader;
-    void *          pvRrvTime;
-    void *          pMICHDR;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    unsigned short wCurrentRate = RATE_1M;
-
-
-    if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 0) {
-        return CMD_STATUS_RESOURCES;
-    }
-
-    pFrstTD = pDevice->apCurrTD[TYPE_TXDMA0];
-    pbyTxBufferAddr = (unsigned char *)pFrstTD->pTDInfo->buf;
-    cbFrameBodySize = pPacket->cbPayloadLen;
-    pTxBufHead = (PSTxBufHead) pbyTxBufferAddr;
-    wTxBufSize = sizeof(STxBufHead);
-    memset(pTxBufHead, 0, wTxBufSize);
-
-    if (pDevice->eCurrentPHYType == PHY_TYPE_11A) {
-        wCurrentRate = RATE_6M;
-        byPktType = PK_TYPE_11A;
-    } else {
-        wCurrentRate = RATE_1M;
-        byPktType = PK_TYPE_11B;
-    }
-
-    // SetPower will cause error power TX state for OFDM Date packet in TX buffer.
-    // 2004.11.11 Kyle -- Using OFDM power to tx MngPkt will decrease the connection capability.
-    //                    And cmd timer will wait data pkt TX finish before scanning so it's OK
-    //                    to set power here.
-    if (pDevice->pMgmt->eScanState != WMAC_NO_SCANNING) {
-
+	PSTxDesc        pFrstTD;
+	unsigned char byPktType;
+	unsigned char *pbyTxBufferAddr;
+	void *pvRTS;
+	PSCTS           pCTS;
+	void *pvTxDataHd;
+	unsigned int uDuration;
+	unsigned int cbReqCount;
+	PS802_11Header  pMACHeader;
+	unsigned int cbHeaderSize;
+	unsigned int cbFrameBodySize;
+	bool bNeedACK;
+	bool bIsPSPOLL = false;
+	PSTxBufHead     pTxBufHead;
+	unsigned int cbFrameSize;
+	unsigned int cbIVlen = 0;
+	unsigned int cbICVlen = 0;
+	unsigned int cbMIClen = 0;
+	unsigned int cbFCSlen = 4;
+	unsigned int uPadding = 0;
+	unsigned short wTxBufSize;
+	unsigned int cbMacHdLen;
+	SEthernetHeader sEthHeader;
+	void *pvRrvTime;
+	void *pMICHDR;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	unsigned short wCurrentRate = RATE_1M;
+
+	if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 0) {
+		return CMD_STATUS_RESOURCES;
+	}
+
+	pFrstTD = pDevice->apCurrTD[TYPE_TXDMA0];
+	pbyTxBufferAddr = (unsigned char *)pFrstTD->pTDInfo->buf;
+	cbFrameBodySize = pPacket->cbPayloadLen;
+	pTxBufHead = (PSTxBufHead) pbyTxBufferAddr;
+	wTxBufSize = sizeof(STxBufHead);
+	memset(pTxBufHead, 0, wTxBufSize);
+
+	if (pDevice->eCurrentPHYType == PHY_TYPE_11A) {
+		wCurrentRate = RATE_6M;
+		byPktType = PK_TYPE_11A;
+	} else {
+		wCurrentRate = RATE_1M;
+		byPktType = PK_TYPE_11B;
+	}
+
+	// SetPower will cause error power TX state for OFDM Date packet in TX buffer.
+	// 2004.11.11 Kyle -- Using OFDM power to tx MngPkt will decrease the connection capability.
+	//                    And cmd timer will wait data pkt TX finish before scanning so it's OK
+	//                    to set power here.
+	if (pDevice->pMgmt->eScanState != WMAC_NO_SCANNING) {
 		RFbSetPower(pDevice, wCurrentRate, pDevice->byCurrentCh);
-    } else {
-        RFbSetPower(pDevice, wCurrentRate, pMgmt->uCurrChannel);
-    }
-    pTxBufHead->byTxPower = pDevice->byCurPwr;
-    //+++++++++++++++++++++ Patch VT3253 A1 performance +++++++++++++++++++++++++++
-    if (pDevice->byFOETuning) {
-        if ((pPacket->p80211Header->sA3.wFrameCtl & TYPE_DATE_NULL) == TYPE_DATE_NULL) {
-            wCurrentRate = RATE_24M;
-            byPktType = PK_TYPE_11GA;
-        }
-    }
-
-    //Set packet type
-    if (byPktType == PK_TYPE_11A) {//0000 0000 0000 0000
-        pTxBufHead->wFIFOCtl = 0;
-    }
-    else if (byPktType == PK_TYPE_11B) {//0000 0001 0000 0000
-        pTxBufHead->wFIFOCtl |= FIFOCTL_11B;
-    }
-    else if (byPktType == PK_TYPE_11GB) {//0000 0010 0000 0000
-        pTxBufHead->wFIFOCtl |= FIFOCTL_11GB;
-    }
-    else if (byPktType == PK_TYPE_11GA) {//0000 0011 0000 0000
-        pTxBufHead->wFIFOCtl |= FIFOCTL_11GA;
-    }
-
-    pTxBufHead->wFIFOCtl |= FIFOCTL_TMOEN;
-    pTxBufHead->wTimeStamp = cpu_to_le16(DEFAULT_MGN_LIFETIME_RES_64us);
-
-
-    if (is_multicast_ether_addr(&(pPacket->p80211Header->sA3.abyAddr1[0])))
-        bNeedACK = false;
-    else {
-        bNeedACK = true;
-        pTxBufHead->wFIFOCtl |= FIFOCTL_NEEDACK;
-    };
-
-    if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) ||
-        (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) ) {
-
-        pTxBufHead->wFIFOCtl |= FIFOCTL_LRETRY;
-        //Set Preamble type always long
-        //pDevice->byPreambleType = PREAMBLE_LONG;
-        // probe-response don't retry
-        //if ((pPacket->p80211Header->sA4.wFrameCtl & TYPE_SUBTYPE_MASK) == TYPE_MGMT_PROBE_RSP) {
-        //     bNeedACK = false;
-        //     pTxBufHead->wFIFOCtl  &= (~FIFOCTL_NEEDACK);
-        //}
-    }
-
-    pTxBufHead->wFIFOCtl |= (FIFOCTL_GENINT | FIFOCTL_ISDMA0);
-
-    if ((pPacket->p80211Header->sA4.wFrameCtl & TYPE_SUBTYPE_MASK) == TYPE_CTL_PSPOLL) {
-        bIsPSPOLL = true;
-        cbMacHdLen = WLAN_HDR_ADDR2_LEN;
-    } else {
-        cbMacHdLen = WLAN_HDR_ADDR3_LEN;
-    }
-
-    //Set FRAGCTL_MACHDCNT
-    pTxBufHead->wFragCtl |= cpu_to_le16((unsigned short)(cbMacHdLen << 10));
-
-    // Notes:
-    // Although spec says MMPDU can be fragmented; In most cases,
-    // no one will send a MMPDU under fragmentation. With RTS may occur.
-    pDevice->bAES = false;  //Set FRAGCTL_WEPTYP
-
-    if (WLAN_GET_FC_ISWEP(pPacket->p80211Header->sA4.wFrameCtl) != 0) {
-        if (pDevice->eEncryptionStatus == Ndis802_11Encryption1Enabled) {
-            cbIVlen = 4;
-            cbICVlen = 4;
-    	    pTxBufHead->wFragCtl |= FRAGCTL_LEGACY;
-        }
-        else if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled) {
-            cbIVlen = 8;//IV+ExtIV
-            cbMIClen = 8;
-            cbICVlen = 4;
-    	    pTxBufHead->wFragCtl |= FRAGCTL_TKIP;
-    	    //We need to get seed here for filling TxKey entry.
-            //TKIPvMixKey(pTransmitKey->abyKey, pDevice->abyCurrentNetAddr,
-            //            pTransmitKey->wTSC15_0, pTransmitKey->dwTSC47_16, pDevice->abyPRNG);
-        }
-        else if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled) {
-            cbIVlen = 8;//RSN Header
-            cbICVlen = 8;//MIC
-            pTxBufHead->wFragCtl |= FRAGCTL_AES;
-            pDevice->bAES = true;
-        }
-        //MAC Header should be padding 0 to DW alignment.
-        uPadding = 4 - (cbMacHdLen%4);
-        uPadding %= 4;
-    }
-
-    cbFrameSize = cbMacHdLen + cbFrameBodySize + cbIVlen + cbMIClen + cbICVlen + cbFCSlen;
-
-    //Set FIFOCTL_GrpAckPolicy
-    if (pDevice->bGrpAckPolicy == true) {//0000 0100 0000 0000
-        pTxBufHead->wFIFOCtl |=	FIFOCTL_GRPACK;
-    }
-    //the rest of pTxBufHead->wFragCtl:FragTyp will be set later in s_vFillFragParameter()
-
-    //Set RrvTime/RTS/CTS Buffer
-    if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {//802.11g packet
-
-        pvRrvTime = (PSRrvTime_gCTS) (pbyTxBufferAddr + wTxBufSize);
-        pMICHDR = NULL;
-        pvRTS = NULL;
-        pCTS = (PSCTS) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS));
-        pvTxDataHd = (PSTxDataHead_g) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS) + sizeof(SCTS));
-        cbHeaderSize = wTxBufSize + sizeof(SRrvTime_gCTS) + sizeof(SCTS) + sizeof(STxDataHead_g);
-    }
-    else { // 802.11a/b packet
-        pvRrvTime = (PSRrvTime_ab) (pbyTxBufferAddr + wTxBufSize);
-        pMICHDR = NULL;
-        pvRTS = NULL;
-        pCTS = NULL;
-        pvTxDataHd = (PSTxDataHead_ab) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab));
-        cbHeaderSize = wTxBufSize + sizeof(SRrvTime_ab) + sizeof(STxDataHead_ab);
-    }
-
-    memset((void *)(pbyTxBufferAddr + wTxBufSize), 0, (cbHeaderSize - wTxBufSize));
-
-    memcpy(&(sEthHeader.abyDstAddr[0]), &(pPacket->p80211Header->sA3.abyAddr1[0]), ETH_ALEN);
-    memcpy(&(sEthHeader.abySrcAddr[0]), &(pPacket->p80211Header->sA3.abyAddr2[0]), ETH_ALEN);
-    //=========================
-    //    No Fragmentation
-    //=========================
-    pTxBufHead->wFragCtl |= (unsigned short)FRAGCTL_NONFRAG;
-
-
-    //Fill FIFO,RrvTime,RTS,and CTS
-    s_vGenerateTxParameter(pDevice, byPktType, pbyTxBufferAddr, pvRrvTime, pvRTS, pCTS,
-                           cbFrameSize, bNeedACK, TYPE_TXDMA0, &sEthHeader, wCurrentRate);
-
-    //Fill DataHead
-    uDuration = s_uFillDataHead(pDevice, byPktType, pvTxDataHd, cbFrameSize, TYPE_TXDMA0, bNeedACK,
-                                0, 0, 1, AUTO_FB_NONE, wCurrentRate);
-
-    pMACHeader = (PS802_11Header) (pbyTxBufferAddr + cbHeaderSize);
-
-    cbReqCount = cbHeaderSize + cbMacHdLen + uPadding + cbIVlen + cbFrameBodySize;
-
-    if (WLAN_GET_FC_ISWEP(pPacket->p80211Header->sA4.wFrameCtl) != 0) {
-        unsigned char *pbyIVHead;
-        unsigned char *pbyPayloadHead;
-        unsigned char *pbyBSSID;
-        PSKeyItem       pTransmitKey = NULL;
-
-        pbyIVHead = (unsigned char *)(pbyTxBufferAddr + cbHeaderSize + cbMacHdLen + uPadding);
-        pbyPayloadHead = (unsigned char *)(pbyTxBufferAddr + cbHeaderSize + cbMacHdLen + uPadding + cbIVlen);
-
-        //Fill TXKEY
-        //Kyle: Need fix: TKIP and AES did't encrypt Mnt Packet.
-        //s_vFillTxKey(pDevice, (unsigned char *)pTxBufHead->adwTxKey, NULL);
-
-        //Fill IV(ExtIV,RSNHDR)
-        //s_vFillPrePayload(pDevice, pbyIVHead, NULL);
-        //---------------------------
-        // S/W or H/W Encryption
-        //---------------------------
-        //Fill MICHDR
-        //if (pDevice->bAES) {
-        //    s_vFillMICHDR(pDevice, (unsigned char *)pMICHDR, (unsigned char *)pMACHeader, (unsigned short)cbFrameBodySize);
-        //}
-        do {
-            if ((pDevice->eOPMode == OP_MODE_INFRASTRUCTURE) &&
-                (pDevice->bLinkPass == true)) {
-                pbyBSSID = pDevice->abyBSSID;
-                // get pairwise key
-                if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, PAIRWISE_KEY, &pTransmitKey) == false) {
-                    // get group key
-                    if(KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, GROUP_KEY, &pTransmitKey) == true) {
-                        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Get GTK.\n");
-                        break;
-                    }
-                } else {
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Get PTK.\n");
-                    break;
-                }
-            }
-            // get group key
-            pbyBSSID = pDevice->abyBroadcastAddr;
-            if(KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, GROUP_KEY, &pTransmitKey) == false) {
-                pTransmitKey = NULL;
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"KEY is NULL. OP Mode[%d]\n", pDevice->eOPMode);
-            } else {
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Get GTK.\n");
-            }
-        } while(false);
-        //Fill TXKEY
-        s_vFillTxKey(pDevice, (unsigned char *)(pTxBufHead->adwTxKey), pbyIVHead, pTransmitKey,
-                     (unsigned char *)pMACHeader, (unsigned short)cbFrameBodySize, NULL);
-
-        memcpy(pMACHeader, pPacket->p80211Header, cbMacHdLen);
-        memcpy(pbyPayloadHead, ((unsigned char *)(pPacket->p80211Header) + cbMacHdLen),
-                 cbFrameBodySize);
-    }
-    else {
-        // Copy the Packet into a tx Buffer
-        memcpy(pMACHeader, pPacket->p80211Header, pPacket->cbMPDULen);
-    }
-
-    pMACHeader->wSeqCtl = cpu_to_le16(pDevice->wSeqCounter << 4);
-    pDevice->wSeqCounter++ ;
-    if (pDevice->wSeqCounter > 0x0fff)
-        pDevice->wSeqCounter = 0;
-
-    if (bIsPSPOLL) {
-        // The MAC will automatically replace the Duration-field of MAC header by Duration-field
-        // of  FIFO control header.
-        // This will cause AID-field of PS-POLL packet to be incorrect (Because PS-POLL's AID field is
-        // in the same place of other packet's Duration-field).
-        // And it will cause Cisco-AP to issue Disassociation-packet
-        if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
-            ((PSTxDataHead_g)pvTxDataHd)->wDuration_a = cpu_to_le16(pPacket->p80211Header->sA2.wDurationID);
-            ((PSTxDataHead_g)pvTxDataHd)->wDuration_b = cpu_to_le16(pPacket->p80211Header->sA2.wDurationID);
-        } else {
-            ((PSTxDataHead_ab)pvTxDataHd)->wDuration = cpu_to_le16(pPacket->p80211Header->sA2.wDurationID);
-        }
-    }
-
-
-    // first TD is the only TD
-    //Set TSR1 & ReqCount in TxDescHead
-    pFrstTD->m_td1TD1.byTCR = (TCR_STP | TCR_EDP | EDMSDU);
-    pFrstTD->pTDInfo->skb_dma = pFrstTD->pTDInfo->buf_dma;
-    pFrstTD->m_td1TD1.wReqCount = cpu_to_le16((unsigned short)(cbReqCount));
-    pFrstTD->buff_addr = cpu_to_le32(pFrstTD->pTDInfo->skb_dma);
-    pFrstTD->pTDInfo->byFlags = 0;
-
-    if (MACbIsRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PS)) {
-        // Disable PS
-        MACbPSWakeup(pDevice->PortOffset);
-    }
-    pDevice->bPWBitOn = false;
-
-    wmb();
-    pFrstTD->m_td0TD0.f1Owner = OWNED_BY_NIC;
-    wmb();
-
-    pDevice->iTDUsed[TYPE_TXDMA0]++;
-
-    if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 1) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " available td0 <= 1\n");
-    }
-
-    pDevice->apCurrTD[TYPE_TXDMA0] = pFrstTD->next;
-#ifdef	PLICE_DEBUG
-		//printk("SCAN:CurrentRate is  %d,TxPower is %d\n",wCurrentRate,pTxBufHead->byTxPower);
-#endif
+	} else {
+		RFbSetPower(pDevice, wCurrentRate, pMgmt->uCurrChannel);
+	}
+	pTxBufHead->byTxPower = pDevice->byCurPwr;
+	//+++++++++++++++++++++ Patch VT3253 A1 performance +++++++++++++++++++++++++++
+	if (pDevice->byFOETuning) {
+		if ((pPacket->p80211Header->sA3.wFrameCtl & TYPE_DATE_NULL) == TYPE_DATE_NULL) {
+			wCurrentRate = RATE_24M;
+			byPktType = PK_TYPE_11GA;
+		}
+	}
+
+	//Set packet type
+	if (byPktType == PK_TYPE_11A) {//0000 0000 0000 0000
+		pTxBufHead->wFIFOCtl = 0;
+	} else if (byPktType == PK_TYPE_11B) {//0000 0001 0000 0000
+		pTxBufHead->wFIFOCtl |= FIFOCTL_11B;
+	} else if (byPktType == PK_TYPE_11GB) {//0000 0010 0000 0000
+		pTxBufHead->wFIFOCtl |= FIFOCTL_11GB;
+	} else if (byPktType == PK_TYPE_11GA) {//0000 0011 0000 0000
+		pTxBufHead->wFIFOCtl |= FIFOCTL_11GA;
+	}
+
+	pTxBufHead->wFIFOCtl |= FIFOCTL_TMOEN;
+	pTxBufHead->wTimeStamp = cpu_to_le16(DEFAULT_MGN_LIFETIME_RES_64us);
+
+	if (is_multicast_ether_addr(&(pPacket->p80211Header->sA3.abyAddr1[0])))
+		bNeedACK = false;
+	else {
+		bNeedACK = true;
+		pTxBufHead->wFIFOCtl |= FIFOCTL_NEEDACK;
+	};
+
+	if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) ||
+	    (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA)) {
+		pTxBufHead->wFIFOCtl |= FIFOCTL_LRETRY;
+	}
+
+	pTxBufHead->wFIFOCtl |= (FIFOCTL_GENINT | FIFOCTL_ISDMA0);
+
+	if ((pPacket->p80211Header->sA4.wFrameCtl & TYPE_SUBTYPE_MASK) == TYPE_CTL_PSPOLL) {
+		bIsPSPOLL = true;
+		cbMacHdLen = WLAN_HDR_ADDR2_LEN;
+	} else {
+		cbMacHdLen = WLAN_HDR_ADDR3_LEN;
+	}
+
+	//Set FRAGCTL_MACHDCNT
+	pTxBufHead->wFragCtl |= cpu_to_le16((unsigned short)(cbMacHdLen << 10));
+
+	// Notes:
+	// Although spec says MMPDU can be fragmented; In most cases,
+	// no one will send a MMPDU under fragmentation. With RTS may occur.
+	pDevice->bAES = false;  //Set FRAGCTL_WEPTYP
+
+	if (WLAN_GET_FC_ISWEP(pPacket->p80211Header->sA4.wFrameCtl) != 0) {
+		if (pDevice->eEncryptionStatus == Ndis802_11Encryption1Enabled) {
+			cbIVlen = 4;
+			cbICVlen = 4;
+			pTxBufHead->wFragCtl |= FRAGCTL_LEGACY;
+		} else if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled) {
+			cbIVlen = 8;//IV+ExtIV
+			cbMIClen = 8;
+			cbICVlen = 4;
+			pTxBufHead->wFragCtl |= FRAGCTL_TKIP;
+			//We need to get seed here for filling TxKey entry.
+			//TKIPvMixKey(pTransmitKey->abyKey, pDevice->abyCurrentNetAddr,
+			//            pTransmitKey->wTSC15_0, pTransmitKey->dwTSC47_16, pDevice->abyPRNG);
+		} else if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled) {
+			cbIVlen = 8;//RSN Header
+			cbICVlen = 8;//MIC
+			pTxBufHead->wFragCtl |= FRAGCTL_AES;
+			pDevice->bAES = true;
+		}
+		//MAC Header should be padding 0 to DW alignment.
+		uPadding = 4 - (cbMacHdLen%4);
+		uPadding %= 4;
+	}
+
+	cbFrameSize = cbMacHdLen + cbFrameBodySize + cbIVlen + cbMIClen + cbICVlen + cbFCSlen;
+
+	//Set FIFOCTL_GrpAckPolicy
+	if (pDevice->bGrpAckPolicy == true) {//0000 0100 0000 0000
+		pTxBufHead->wFIFOCtl |=	FIFOCTL_GRPACK;
+	}
+	//the rest of pTxBufHead->wFragCtl:FragTyp will be set later in s_vFillFragParameter()
+
+	//Set RrvTime/RTS/CTS Buffer
+	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {//802.11g packet
+
+		pvRrvTime = (PSRrvTime_gCTS) (pbyTxBufferAddr + wTxBufSize);
+		pMICHDR = NULL;
+		pvRTS = NULL;
+		pCTS = (PSCTS) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS));
+		pvTxDataHd = (PSTxDataHead_g) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS) + sizeof(SCTS));
+		cbHeaderSize = wTxBufSize + sizeof(SRrvTime_gCTS) + sizeof(SCTS) + sizeof(STxDataHead_g);
+	} else { // 802.11a/b packet
+		pvRrvTime = (PSRrvTime_ab) (pbyTxBufferAddr + wTxBufSize);
+		pMICHDR = NULL;
+		pvRTS = NULL;
+		pCTS = NULL;
+		pvTxDataHd = (PSTxDataHead_ab) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab));
+		cbHeaderSize = wTxBufSize + sizeof(SRrvTime_ab) + sizeof(STxDataHead_ab);
+	}
+
+	memset((void *)(pbyTxBufferAddr + wTxBufSize), 0, (cbHeaderSize - wTxBufSize));
+
+	memcpy(&(sEthHeader.abyDstAddr[0]), &(pPacket->p80211Header->sA3.abyAddr1[0]), ETH_ALEN);
+	memcpy(&(sEthHeader.abySrcAddr[0]), &(pPacket->p80211Header->sA3.abyAddr2[0]), ETH_ALEN);
+	//=========================
+	//    No Fragmentation
+	//=========================
+	pTxBufHead->wFragCtl |= (unsigned short)FRAGCTL_NONFRAG;
+
+	//Fill FIFO,RrvTime,RTS,and CTS
+	s_vGenerateTxParameter(pDevice, byPktType, pbyTxBufferAddr, pvRrvTime, pvRTS, pCTS,
+			       cbFrameSize, bNeedACK, TYPE_TXDMA0, &sEthHeader, wCurrentRate);
+
+	//Fill DataHead
+	uDuration = s_uFillDataHead(pDevice, byPktType, pvTxDataHd, cbFrameSize, TYPE_TXDMA0, bNeedACK,
+				    0, 0, 1, AUTO_FB_NONE, wCurrentRate);
+
+	pMACHeader = (PS802_11Header) (pbyTxBufferAddr + cbHeaderSize);
+
+	cbReqCount = cbHeaderSize + cbMacHdLen + uPadding + cbIVlen + cbFrameBodySize;
+
+	if (WLAN_GET_FC_ISWEP(pPacket->p80211Header->sA4.wFrameCtl) != 0) {
+		unsigned char *pbyIVHead;
+		unsigned char *pbyPayloadHead;
+		unsigned char *pbyBSSID;
+		PSKeyItem       pTransmitKey = NULL;
+
+		pbyIVHead = (unsigned char *)(pbyTxBufferAddr + cbHeaderSize + cbMacHdLen + uPadding);
+		pbyPayloadHead = (unsigned char *)(pbyTxBufferAddr + cbHeaderSize + cbMacHdLen + uPadding + cbIVlen);
+
+		//Fill TXKEY
+		//Kyle: Need fix: TKIP and AES did't encrypt Mnt Packet.
+		//s_vFillTxKey(pDevice, (unsigned char *)pTxBufHead->adwTxKey, NULL);
+
+		//Fill IV(ExtIV,RSNHDR)
+		//s_vFillPrePayload(pDevice, pbyIVHead, NULL);
+		//---------------------------
+		// S/W or H/W Encryption
+		//---------------------------
+		do {
+			if ((pDevice->eOPMode == OP_MODE_INFRASTRUCTURE) &&
+			    (pDevice->bLinkPass == true)) {
+				pbyBSSID = pDevice->abyBSSID;
+				// get pairwise key
+				if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, PAIRWISE_KEY, &pTransmitKey) == false) {
+					// get group key
+					if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, GROUP_KEY, &pTransmitKey) == true) {
+						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Get GTK.\n");
+						break;
+					}
+				} else {
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Get PTK.\n");
+					break;
+				}
+			}
+			// get group key
+			pbyBSSID = pDevice->abyBroadcastAddr;
+			if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, GROUP_KEY, &pTransmitKey) == false) {
+				pTransmitKey = NULL;
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "KEY is NULL. OP Mode[%d]\n", pDevice->eOPMode);
+			} else {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Get GTK.\n");
+			}
+		} while (false);
+		//Fill TXKEY
+		s_vFillTxKey(pDevice, (unsigned char *)(pTxBufHead->adwTxKey), pbyIVHead, pTransmitKey,
+			     (unsigned char *)pMACHeader, (unsigned short)cbFrameBodySize, NULL);
+
+		memcpy(pMACHeader, pPacket->p80211Header, cbMacHdLen);
+		memcpy(pbyPayloadHead, ((unsigned char *)(pPacket->p80211Header) + cbMacHdLen),
+		       cbFrameBodySize);
+	} else {
+		// Copy the Packet into a tx Buffer
+		memcpy(pMACHeader, pPacket->p80211Header, pPacket->cbMPDULen);
+	}
+
+	pMACHeader->wSeqCtl = cpu_to_le16(pDevice->wSeqCounter << 4);
+	pDevice->wSeqCounter++;
+	if (pDevice->wSeqCounter > 0x0fff)
+		pDevice->wSeqCounter = 0;
+
+	if (bIsPSPOLL) {
+		// The MAC will automatically replace the Duration-field of MAC header by Duration-field
+		// of  FIFO control header.
+		// This will cause AID-field of PS-POLL packet to be incorrect (Because PS-POLL's AID field is
+		// in the same place of other packet's Duration-field).
+		// And it will cause Cisco-AP to issue Disassociation-packet
+		if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
+			((PSTxDataHead_g)pvTxDataHd)->wDuration_a = cpu_to_le16(pPacket->p80211Header->sA2.wDurationID);
+			((PSTxDataHead_g)pvTxDataHd)->wDuration_b = cpu_to_le16(pPacket->p80211Header->sA2.wDurationID);
+		} else {
+			((PSTxDataHead_ab)pvTxDataHd)->wDuration = cpu_to_le16(pPacket->p80211Header->sA2.wDurationID);
+		}
+	}
+
+	// first TD is the only TD
+	//Set TSR1 & ReqCount in TxDescHead
+	pFrstTD->m_td1TD1.byTCR = (TCR_STP | TCR_EDP | EDMSDU);
+	pFrstTD->pTDInfo->skb_dma = pFrstTD->pTDInfo->buf_dma;
+	pFrstTD->m_td1TD1.wReqCount = cpu_to_le16((unsigned short)(cbReqCount));
+	pFrstTD->buff_addr = cpu_to_le32(pFrstTD->pTDInfo->skb_dma);
+	pFrstTD->pTDInfo->byFlags = 0;
+
+	if (MACbIsRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PS)) {
+		// Disable PS
+		MACbPSWakeup(pDevice->PortOffset);
+	}
+	pDevice->bPWBitOn = false;
+
+	wmb();
+	pFrstTD->m_td0TD0.f1Owner = OWNED_BY_NIC;
+	wmb();
+
+	pDevice->iTDUsed[TYPE_TXDMA0]++;
+
+	if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 1) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " available td0 <= 1\n");
+	}
+
+	pDevice->apCurrTD[TYPE_TXDMA0] = pFrstTD->next;
 
 #ifdef TxInSleep
-  pDevice->nTxDataTimeCout=0; //2008-8-21 chester <add> for send null packet
-  #endif
-
-    // Poll Transmit the adapter
-    MACvTransmit0(pDevice->PortOffset);
+	pDevice->nTxDataTimeCout = 0; //2008-8-21 chester <add> for send null packet
+#endif
 
-    return CMD_STATUS_PENDING;
+	// Poll Transmit the adapter
+	MACvTransmit0(pDevice->PortOffset);
 
+	return CMD_STATUS_PENDING;
 }
 
-
 CMD_STATUS csBeacon_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket) {
-
-    unsigned char byPktType;
-    unsigned char *pbyBuffer = (unsigned char *)pDevice->tx_beacon_bufs;
-    unsigned int cbFrameSize = pPacket->cbMPDULen + WLAN_FCS_LEN;
-    unsigned int cbHeaderSize = 0;
-    unsigned short wTxBufSize = sizeof(STxShortBufHead);
-    PSTxShortBufHead pTxBufHead = (PSTxShortBufHead) pbyBuffer;
-    PSTxDataHead_ab  pTxDataHead = (PSTxDataHead_ab) (pbyBuffer + wTxBufSize);
-    PS802_11Header   pMACHeader;
-    unsigned short wCurrentRate;
-    unsigned short wLen = 0x0000;
-
-
-    memset(pTxBufHead, 0, wTxBufSize);
-
-    if (pDevice->eCurrentPHYType == PHY_TYPE_11A) {
-        wCurrentRate = RATE_6M;
-        byPktType = PK_TYPE_11A;
-    } else {
-        wCurrentRate = RATE_2M;
-        byPktType = PK_TYPE_11B;
-    }
-
-    //Set Preamble type always long
-    pDevice->byPreambleType = PREAMBLE_LONG;
-
-    //Set FIFOCTL_GENINT
-
-    pTxBufHead->wFIFOCtl |= FIFOCTL_GENINT;
-
-
-    //Set packet type & Get Duration
-    if (byPktType == PK_TYPE_11A) {//0000 0000 0000 0000
-        pTxDataHead->wDuration = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameSize, byPktType,
-                                                          wCurrentRate, false, 0, 0, 1, AUTO_FB_NONE));
-    }
-    else if (byPktType == PK_TYPE_11B) {//0000 0001 0000 0000
-        pTxBufHead->wFIFOCtl |= FIFOCTL_11B;
-        pTxDataHead->wDuration = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameSize, byPktType,
-                                                          wCurrentRate, false, 0, 0, 1, AUTO_FB_NONE));
-    }
-
-    BBvCalculateParameter(pDevice, cbFrameSize, wCurrentRate, byPktType,
-        (unsigned short *)&(wLen), (unsigned char *)&(pTxDataHead->byServiceField), (unsigned char *)&(pTxDataHead->bySignalField)
-    );
-    pTxDataHead->wTransmitLength = cpu_to_le16(wLen);
-    //Get TimeStampOff
-    pTxDataHead->wTimeStampOff = cpu_to_le16(wTimeStampOff[pDevice->byPreambleType%2][wCurrentRate%MAX_RATE]);
-    cbHeaderSize = wTxBufSize + sizeof(STxDataHead_ab);
-
-   //Generate Beacon Header
-    pMACHeader = (PS802_11Header)(pbyBuffer + cbHeaderSize);
-    memcpy(pMACHeader, pPacket->p80211Header, pPacket->cbMPDULen);
-
-    pMACHeader->wDurationID = 0;
-    pMACHeader->wSeqCtl = cpu_to_le16(pDevice->wSeqCounter << 4);
-    pDevice->wSeqCounter++ ;
-    if (pDevice->wSeqCounter > 0x0fff)
-        pDevice->wSeqCounter = 0;
-
-    // Set Beacon buffer length
-    pDevice->wBCNBufLen = pPacket->cbMPDULen + cbHeaderSize;
-
-    MACvSetCurrBCNTxDescAddr(pDevice->PortOffset, (pDevice->tx_beacon_dma));
-
-    MACvSetCurrBCNLength(pDevice->PortOffset, pDevice->wBCNBufLen);
-    // Set auto Transmit on
-    MACvRegBitsOn(pDevice->PortOffset, MAC_REG_TCR, TCR_AUTOBCNTX);
-    // Poll Transmit the adapter
-    MACvTransmitBCN(pDevice->PortOffset);
-
-    return CMD_STATUS_PENDING;
+	unsigned char byPktType;
+	unsigned char *pbyBuffer = (unsigned char *)pDevice->tx_beacon_bufs;
+	unsigned int cbFrameSize = pPacket->cbMPDULen + WLAN_FCS_LEN;
+	unsigned int cbHeaderSize = 0;
+	unsigned short wTxBufSize = sizeof(STxShortBufHead);
+	PSTxShortBufHead pTxBufHead = (PSTxShortBufHead) pbyBuffer;
+	PSTxDataHead_ab  pTxDataHead = (PSTxDataHead_ab) (pbyBuffer + wTxBufSize);
+	PS802_11Header   pMACHeader;
+	unsigned short wCurrentRate;
+	unsigned short wLen = 0x0000;
+
+	memset(pTxBufHead, 0, wTxBufSize);
+
+	if (pDevice->eCurrentPHYType == PHY_TYPE_11A) {
+		wCurrentRate = RATE_6M;
+		byPktType = PK_TYPE_11A;
+	} else {
+		wCurrentRate = RATE_2M;
+		byPktType = PK_TYPE_11B;
+	}
+
+	//Set Preamble type always long
+	pDevice->byPreambleType = PREAMBLE_LONG;
+
+	//Set FIFOCTL_GENINT
+
+	pTxBufHead->wFIFOCtl |= FIFOCTL_GENINT;
+
+	//Set packet type & Get Duration
+	if (byPktType == PK_TYPE_11A) {//0000 0000 0000 0000
+		pTxDataHead->wDuration = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameSize, byPktType,
+											wCurrentRate, false, 0, 0, 1, AUTO_FB_NONE));
+	} else if (byPktType == PK_TYPE_11B) {//0000 0001 0000 0000
+		pTxBufHead->wFIFOCtl |= FIFOCTL_11B;
+		pTxDataHead->wDuration = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameSize, byPktType,
+											wCurrentRate, false, 0, 0, 1, AUTO_FB_NONE));
+	}
+
+	BBvCalculateParameter(pDevice, cbFrameSize, wCurrentRate, byPktType,
+			      (unsigned short *)&(wLen), (unsigned char *)&(pTxDataHead->byServiceField), (unsigned char *)&(pTxDataHead->bySignalField)
+);
+	pTxDataHead->wTransmitLength = cpu_to_le16(wLen);
+	//Get TimeStampOff
+	pTxDataHead->wTimeStampOff = cpu_to_le16(wTimeStampOff[pDevice->byPreambleType%2][wCurrentRate%MAX_RATE]);
+	cbHeaderSize = wTxBufSize + sizeof(STxDataHead_ab);
+
+	//Generate Beacon Header
+	pMACHeader = (PS802_11Header)(pbyBuffer + cbHeaderSize);
+	memcpy(pMACHeader, pPacket->p80211Header, pPacket->cbMPDULen);
+
+	pMACHeader->wDurationID = 0;
+	pMACHeader->wSeqCtl = cpu_to_le16(pDevice->wSeqCounter << 4);
+	pDevice->wSeqCounter++;
+	if (pDevice->wSeqCounter > 0x0fff)
+		pDevice->wSeqCounter = 0;
+
+	// Set Beacon buffer length
+	pDevice->wBCNBufLen = pPacket->cbMPDULen + cbHeaderSize;
+
+	MACvSetCurrBCNTxDescAddr(pDevice->PortOffset, (pDevice->tx_beacon_dma));
+
+	MACvSetCurrBCNLength(pDevice->PortOffset, pDevice->wBCNBufLen);
+	// Set auto Transmit on
+	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_TCR, TCR_AUTOBCNTX);
+	// Poll Transmit the adapter
+	MACvTransmitBCN(pDevice->PortOffset);
+
+	return CMD_STATUS_PENDING;
 }
 
-
-
 unsigned int
-cbGetFragCount (
-    PSDevice         pDevice,
-    PSKeyItem        pTransmitKey,
-    unsigned int cbFrameBodySize,
-    PSEthernetHeader psEthHeader
-    )
+cbGetFragCount(
+	PSDevice         pDevice,
+	PSKeyItem        pTransmitKey,
+	unsigned int cbFrameBodySize,
+	PSEthernetHeader psEthHeader
+)
 {
-    unsigned int cbMACHdLen;
-    unsigned int cbFrameSize;
-    unsigned int cbFragmentSize; //Hdr+(IV)+payoad+(MIC)+(ICV)+FCS
-    unsigned int cbFragPayloadSize;
-    unsigned int cbLastFragPayloadSize;
-    unsigned int cbIVlen = 0;
-    unsigned int cbICVlen = 0;
-    unsigned int cbMIClen = 0;
-    unsigned int cbFCSlen = 4;
-    unsigned int uMACfragNum = 1;
-    bool bNeedACK;
-
-
-
-    if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
-        (pDevice->eOPMode == OP_MODE_AP)) {
-        if (is_multicast_ether_addr(&(psEthHeader->abyDstAddr[0])))
-            bNeedACK = false;
-        else
-            bNeedACK = true;
-    }
-    else {
-        // MSDUs in Infra mode always need ACK
-        bNeedACK = true;
-    }
-
-    if (pDevice->bLongHeader)
-        cbMACHdLen = WLAN_HDR_ADDR3_LEN + 6;
-    else
-        cbMACHdLen = WLAN_HDR_ADDR3_LEN;
-
-
-    if (pDevice->bEncryptionEnable == true) {
-
-        if (pTransmitKey == NULL) {
-            if ((pDevice->eEncryptionStatus == Ndis802_11Encryption1Enabled) ||
-                (pDevice->pMgmt->eAuthenMode < WMAC_AUTH_WPA)) {
-                cbIVlen = 4;
-                cbICVlen = 4;
-            } else if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled) {
-                cbIVlen = 8;//IV+ExtIV
-                cbMIClen = 8;
-                cbICVlen = 4;
-            } else if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled) {
-                cbIVlen = 8;//RSN Header
-                cbICVlen = 8;//MIC
-            }
-        } else if (pTransmitKey->byCipherSuite == KEY_CTL_WEP) {
-            cbIVlen = 4;
-            cbICVlen = 4;
-        } else if (pTransmitKey->byCipherSuite == KEY_CTL_TKIP) {
-            cbIVlen = 8;//IV+ExtIV
-            cbMIClen = 8;
-            cbICVlen = 4;
-        } else if (pTransmitKey->byCipherSuite == KEY_CTL_CCMP) {
-            cbIVlen = 8;//RSN Header
-            cbICVlen = 8;//MIC
-        }
-    }
-
-    cbFrameSize = cbMACHdLen + cbIVlen + (cbFrameBodySize + cbMIClen) + cbICVlen + cbFCSlen;
-
-    if ((cbFrameSize > pDevice->wFragmentationThreshold) && (bNeedACK == true)) {
-        // Fragmentation
-        cbFragmentSize = pDevice->wFragmentationThreshold;
-        cbFragPayloadSize = cbFragmentSize - cbMACHdLen - cbIVlen - cbICVlen - cbFCSlen;
-        uMACfragNum = (unsigned short) ((cbFrameBodySize + cbMIClen) / cbFragPayloadSize);
-        cbLastFragPayloadSize = (cbFrameBodySize + cbMIClen) % cbFragPayloadSize;
-        if (cbLastFragPayloadSize == 0) {
-            cbLastFragPayloadSize = cbFragPayloadSize;
-        } else {
-            uMACfragNum++;
-        }
-    }
-    return uMACfragNum;
+	unsigned int cbMACHdLen;
+	unsigned int cbFrameSize;
+	unsigned int cbFragmentSize; //Hdr+(IV)+payoad+(MIC)+(ICV)+FCS
+	unsigned int cbFragPayloadSize;
+	unsigned int cbLastFragPayloadSize;
+	unsigned int cbIVlen = 0;
+	unsigned int cbICVlen = 0;
+	unsigned int cbMIClen = 0;
+	unsigned int cbFCSlen = 4;
+	unsigned int uMACfragNum = 1;
+	bool bNeedACK;
+
+	if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
+	    (pDevice->eOPMode == OP_MODE_AP)) {
+		if (is_multicast_ether_addr(&(psEthHeader->abyDstAddr[0])))
+			bNeedACK = false;
+		else
+			bNeedACK = true;
+	} else {
+		// MSDUs in Infra mode always need ACK
+		bNeedACK = true;
+	}
+
+	if (pDevice->bLongHeader)
+		cbMACHdLen = WLAN_HDR_ADDR3_LEN + 6;
+	else
+		cbMACHdLen = WLAN_HDR_ADDR3_LEN;
+
+	if (pDevice->bEncryptionEnable == true) {
+		if (pTransmitKey == NULL) {
+			if ((pDevice->eEncryptionStatus == Ndis802_11Encryption1Enabled) ||
+			    (pDevice->pMgmt->eAuthenMode < WMAC_AUTH_WPA)) {
+				cbIVlen = 4;
+				cbICVlen = 4;
+			} else if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled) {
+				cbIVlen = 8;//IV+ExtIV
+				cbMIClen = 8;
+				cbICVlen = 4;
+			} else if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled) {
+				cbIVlen = 8;//RSN Header
+				cbICVlen = 8;//MIC
+			}
+		} else if (pTransmitKey->byCipherSuite == KEY_CTL_WEP) {
+			cbIVlen = 4;
+			cbICVlen = 4;
+		} else if (pTransmitKey->byCipherSuite == KEY_CTL_TKIP) {
+			cbIVlen = 8;//IV+ExtIV
+			cbMIClen = 8;
+			cbICVlen = 4;
+		} else if (pTransmitKey->byCipherSuite == KEY_CTL_CCMP) {
+			cbIVlen = 8;//RSN Header
+			cbICVlen = 8;//MIC
+		}
+	}
+
+	cbFrameSize = cbMACHdLen + cbIVlen + (cbFrameBodySize + cbMIClen) + cbICVlen + cbFCSlen;
+
+	if ((cbFrameSize > pDevice->wFragmentationThreshold) && (bNeedACK == true)) {
+		// Fragmentation
+		cbFragmentSize = pDevice->wFragmentationThreshold;
+		cbFragPayloadSize = cbFragmentSize - cbMACHdLen - cbIVlen - cbICVlen - cbFCSlen;
+		uMACfragNum = (unsigned short) ((cbFrameBodySize + cbMIClen) / cbFragPayloadSize);
+		cbLastFragPayloadSize = (cbFrameBodySize + cbMIClen) % cbFragPayloadSize;
+		if (cbLastFragPayloadSize == 0) {
+			cbLastFragPayloadSize = cbFragPayloadSize;
+		} else {
+			uMACfragNum++;
+		}
+	}
+	return uMACfragNum;
 }
 
-
 void
 vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, unsigned int cbMPDULen) {
-
-    PSTxDesc        pFrstTD;
-    unsigned char byPktType;
-    unsigned char *pbyTxBufferAddr;
-    void *          pvRTS;
-    void *          pvCTS;
-    void *          pvTxDataHd;
-    unsigned int uDuration;
-    unsigned int cbReqCount;
-    PS802_11Header  pMACHeader;
-    unsigned int cbHeaderSize;
-    unsigned int cbFrameBodySize;
-    bool bNeedACK;
-    bool bIsPSPOLL = false;
-    PSTxBufHead     pTxBufHead;
-    unsigned int cbFrameSize;
-    unsigned int cbIVlen = 0;
-    unsigned int cbICVlen = 0;
-    unsigned int cbMIClen = 0;
-    unsigned int cbFCSlen = 4;
-    unsigned int uPadding = 0;
-    unsigned int cbMICHDR = 0;
-    unsigned int uLength = 0;
-    unsigned long dwMICKey0, dwMICKey1;
-    unsigned long dwMIC_Priority;
-    unsigned long *pdwMIC_L;
-    unsigned long *pdwMIC_R;
-    unsigned short wTxBufSize;
-    unsigned int cbMacHdLen;
-    SEthernetHeader sEthHeader;
-    void *          pvRrvTime;
-    void *          pMICHDR;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    unsigned short wCurrentRate = RATE_1M;
-    PUWLAN_80211HDR  p80211Header;
-    unsigned int uNodeIndex = 0;
-    bool bNodeExist = false;
-    SKeyItem        STempKey;
-    PSKeyItem       pTransmitKey = NULL;
-    unsigned char *pbyIVHead;
-    unsigned char *pbyPayloadHead;
-    unsigned char *pbyMacHdr;
-
-    unsigned int cbExtSuppRate = 0;
+	PSTxDesc        pFrstTD;
+	unsigned char byPktType;
+	unsigned char *pbyTxBufferAddr;
+	void *pvRTS;
+	void *pvCTS;
+	void *pvTxDataHd;
+	unsigned int uDuration;
+	unsigned int cbReqCount;
+	PS802_11Header  pMACHeader;
+	unsigned int cbHeaderSize;
+	unsigned int cbFrameBodySize;
+	bool bNeedACK;
+	bool bIsPSPOLL = false;
+	PSTxBufHead     pTxBufHead;
+	unsigned int cbFrameSize;
+	unsigned int cbIVlen = 0;
+	unsigned int cbICVlen = 0;
+	unsigned int cbMIClen = 0;
+	unsigned int cbFCSlen = 4;
+	unsigned int uPadding = 0;
+	unsigned int cbMICHDR = 0;
+	unsigned int uLength = 0;
+	unsigned long dwMICKey0, dwMICKey1;
+	unsigned long dwMIC_Priority;
+	unsigned long *pdwMIC_L;
+	unsigned long *pdwMIC_R;
+	unsigned short wTxBufSize;
+	unsigned int cbMacHdLen;
+	SEthernetHeader sEthHeader;
+	void *pvRrvTime;
+	void *pMICHDR;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	unsigned short wCurrentRate = RATE_1M;
+	PUWLAN_80211HDR  p80211Header;
+	unsigned int uNodeIndex = 0;
+	bool bNodeExist = false;
+	SKeyItem        STempKey;
+	PSKeyItem       pTransmitKey = NULL;
+	unsigned char *pbyIVHead;
+	unsigned char *pbyPayloadHead;
+	unsigned char *pbyMacHdr;
+
+	unsigned int cbExtSuppRate = 0;
 //    PWLAN_IE        pItem;
 
-
-    pvRrvTime = pMICHDR = pvRTS = pvCTS = pvTxDataHd = NULL;
-
-    if(cbMPDULen <= WLAN_HDR_ADDR3_LEN) {
-       cbFrameBodySize = 0;
-    }
-    else {
-       cbFrameBodySize = cbMPDULen - WLAN_HDR_ADDR3_LEN;
-    }
-    p80211Header = (PUWLAN_80211HDR)pbMPDU;
-
-
-    pFrstTD = pDevice->apCurrTD[TYPE_TXDMA0];
-    pbyTxBufferAddr = (unsigned char *)pFrstTD->pTDInfo->buf;
-    pTxBufHead = (PSTxBufHead) pbyTxBufferAddr;
-    wTxBufSize = sizeof(STxBufHead);
-    memset(pTxBufHead, 0, wTxBufSize);
-
-    if (pDevice->eCurrentPHYType == PHY_TYPE_11A) {
-        wCurrentRate = RATE_6M;
-        byPktType = PK_TYPE_11A;
-    } else {
-        wCurrentRate = RATE_1M;
-        byPktType = PK_TYPE_11B;
-    }
-
-    // SetPower will cause error power TX state for OFDM Date packet in TX buffer.
-    // 2004.11.11 Kyle -- Using OFDM power to tx MngPkt will decrease the connection capability.
-    //                    And cmd timer will wait data pkt TX to finish before scanning so it's OK
-    //                    to set power here.
-    if (pDevice->pMgmt->eScanState != WMAC_NO_SCANNING) {
-        RFbSetPower(pDevice, wCurrentRate, pDevice->byCurrentCh);
-    } else {
-        RFbSetPower(pDevice, wCurrentRate, pMgmt->uCurrChannel);
-    }
-    pTxBufHead->byTxPower = pDevice->byCurPwr;
-
-    //+++++++++++++++++++++ Patch VT3253 A1 performance +++++++++++++++++++++++++++
-    if (pDevice->byFOETuning) {
-        if ((p80211Header->sA3.wFrameCtl & TYPE_DATE_NULL) == TYPE_DATE_NULL) {
-            wCurrentRate = RATE_24M;
-            byPktType = PK_TYPE_11GA;
-        }
-    }
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"vDMA0_tx_80211: p80211Header->sA3.wFrameCtl = %x \n", p80211Header->sA3.wFrameCtl);
-
-    //Set packet type
-    if (byPktType == PK_TYPE_11A) {//0000 0000 0000 0000
-        pTxBufHead->wFIFOCtl = 0;
-    }
-    else if (byPktType == PK_TYPE_11B) {//0000 0001 0000 0000
-        pTxBufHead->wFIFOCtl |= FIFOCTL_11B;
-    }
-    else if (byPktType == PK_TYPE_11GB) {//0000 0010 0000 0000
-        pTxBufHead->wFIFOCtl |= FIFOCTL_11GB;
-    }
-    else if (byPktType == PK_TYPE_11GA) {//0000 0011 0000 0000
-        pTxBufHead->wFIFOCtl |= FIFOCTL_11GA;
-    }
-
-    pTxBufHead->wFIFOCtl |= FIFOCTL_TMOEN;
-    pTxBufHead->wTimeStamp = cpu_to_le16(DEFAULT_MGN_LIFETIME_RES_64us);
-
-
-    if (is_multicast_ether_addr(&(p80211Header->sA3.abyAddr1[0]))) {
-        bNeedACK = false;
-        if (pDevice->bEnableHostWEP) {
-            uNodeIndex = 0;
-            bNodeExist = true;
-        }
-    }
-    else {
-        if (pDevice->bEnableHostWEP) {
-            if (BSSDBbIsSTAInNodeDB(pDevice->pMgmt, (unsigned char *)(p80211Header->sA3.abyAddr1), &uNodeIndex))
-                bNodeExist = true;
-        }
-        bNeedACK = true;
-        pTxBufHead->wFIFOCtl |= FIFOCTL_NEEDACK;
-    };
-
-    if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) ||
-        (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) ) {
-
-        pTxBufHead->wFIFOCtl |= FIFOCTL_LRETRY;
-        //Set Preamble type always long
-        //pDevice->byPreambleType = PREAMBLE_LONG;
-
-        // probe-response don't retry
-        //if ((p80211Header->sA4.wFrameCtl & TYPE_SUBTYPE_MASK) == TYPE_MGMT_PROBE_RSP) {
-        //     bNeedACK = false;
-        //     pTxBufHead->wFIFOCtl  &= (~FIFOCTL_NEEDACK);
-        //}
-    }
-
-    pTxBufHead->wFIFOCtl |= (FIFOCTL_GENINT | FIFOCTL_ISDMA0);
-
-    if ((p80211Header->sA4.wFrameCtl & TYPE_SUBTYPE_MASK) == TYPE_CTL_PSPOLL) {
-        bIsPSPOLL = true;
-        cbMacHdLen = WLAN_HDR_ADDR2_LEN;
-    } else {
-        cbMacHdLen = WLAN_HDR_ADDR3_LEN;
-    }
-
-    // hostapd deamon ext support rate patch
-    if (WLAN_GET_FC_FSTYPE(p80211Header->sA4.wFrameCtl) == WLAN_FSTYPE_ASSOCRESP) {
-
-        if (((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates)->len != 0) {
-            cbExtSuppRate += ((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates)->len + WLAN_IEHDR_LEN;
-         }
-
-        if (((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates)->len != 0) {
-            cbExtSuppRate += ((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates)->len + WLAN_IEHDR_LEN;
-         }
-
-         if (cbExtSuppRate >0) {
-            cbFrameBodySize = WLAN_ASSOCRESP_OFF_SUPP_RATES;
-         }
-    }
-
-
-    //Set FRAGCTL_MACHDCNT
-    pTxBufHead->wFragCtl |= cpu_to_le16((unsigned short)cbMacHdLen << 10);
-
-    // Notes:
-    // Although spec says MMPDU can be fragmented; In most cases,
-    // no one will send a MMPDU under fragmentation. With RTS may occur.
-    pDevice->bAES = false;  //Set FRAGCTL_WEPTYP
-
-
-    if (WLAN_GET_FC_ISWEP(p80211Header->sA4.wFrameCtl) != 0) {
-        if (pDevice->eEncryptionStatus == Ndis802_11Encryption1Enabled) {
-            cbIVlen = 4;
-            cbICVlen = 4;
-    	    pTxBufHead->wFragCtl |= FRAGCTL_LEGACY;
-        }
-        else if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled) {
-            cbIVlen = 8;//IV+ExtIV
-            cbMIClen = 8;
-            cbICVlen = 4;
-    	    pTxBufHead->wFragCtl |= FRAGCTL_TKIP;
-    	    //We need to get seed here for filling TxKey entry.
-            //TKIPvMixKey(pTransmitKey->abyKey, pDevice->abyCurrentNetAddr,
-            //            pTransmitKey->wTSC15_0, pTransmitKey->dwTSC47_16, pDevice->abyPRNG);
-        }
-        else if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled) {
-            cbIVlen = 8;//RSN Header
-            cbICVlen = 8;//MIC
-            cbMICHDR = sizeof(SMICHDRHead);
-            pTxBufHead->wFragCtl |= FRAGCTL_AES;
-            pDevice->bAES = true;
-        }
-        //MAC Header should be padding 0 to DW alignment.
-        uPadding = 4 - (cbMacHdLen%4);
-        uPadding %= 4;
-    }
-
-    cbFrameSize = cbMacHdLen + cbFrameBodySize + cbIVlen + cbMIClen + cbICVlen + cbFCSlen + cbExtSuppRate;
-
-    //Set FIFOCTL_GrpAckPolicy
-    if (pDevice->bGrpAckPolicy == true) {//0000 0100 0000 0000
-        pTxBufHead->wFIFOCtl |=	FIFOCTL_GRPACK;
-    }
-    //the rest of pTxBufHead->wFragCtl:FragTyp will be set later in s_vFillFragParameter()
-
-
-    if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {//802.11g packet
-
-        pvRrvTime = (PSRrvTime_gCTS) (pbyTxBufferAddr + wTxBufSize);
-        pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS));
-        pvRTS = NULL;
-        pvCTS = (PSCTS) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR);
-        pvTxDataHd = (PSTxDataHead_g) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR + sizeof(SCTS));
-        cbHeaderSize = wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR + sizeof(SCTS) + sizeof(STxDataHead_g);
-
-    }
-    else {//802.11a/b packet
-
-        pvRrvTime = (PSRrvTime_ab) (pbyTxBufferAddr + wTxBufSize);
-        pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab));
-        pvRTS = NULL;
-        pvCTS = NULL;
-        pvTxDataHd = (PSTxDataHead_ab) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab) + cbMICHDR);
-        cbHeaderSize = wTxBufSize + sizeof(SRrvTime_ab) + cbMICHDR + sizeof(STxDataHead_ab);
-
-    }
-
-    memset((void *)(pbyTxBufferAddr + wTxBufSize), 0, (cbHeaderSize - wTxBufSize));
-    memcpy(&(sEthHeader.abyDstAddr[0]), &(p80211Header->sA3.abyAddr1[0]), ETH_ALEN);
-    memcpy(&(sEthHeader.abySrcAddr[0]), &(p80211Header->sA3.abyAddr2[0]), ETH_ALEN);
-    //=========================
-    //    No Fragmentation
-    //=========================
-    pTxBufHead->wFragCtl |= (unsigned short)FRAGCTL_NONFRAG;
-
-
-    //Fill FIFO,RrvTime,RTS,and CTS
-    s_vGenerateTxParameter(pDevice, byPktType, pbyTxBufferAddr, pvRrvTime, pvRTS, pvCTS,
-                           cbFrameSize, bNeedACK, TYPE_TXDMA0, &sEthHeader, wCurrentRate);
-
-    //Fill DataHead
-    uDuration = s_uFillDataHead(pDevice, byPktType, pvTxDataHd, cbFrameSize, TYPE_TXDMA0, bNeedACK,
-                                0, 0, 1, AUTO_FB_NONE, wCurrentRate);
-
-    pMACHeader = (PS802_11Header) (pbyTxBufferAddr + cbHeaderSize);
-
-    cbReqCount = cbHeaderSize + cbMacHdLen + uPadding + cbIVlen + (cbFrameBodySize + cbMIClen) + cbExtSuppRate;
-
-    pbyMacHdr = (unsigned char *)(pbyTxBufferAddr + cbHeaderSize);
-    pbyPayloadHead = (unsigned char *)(pbyMacHdr + cbMacHdLen + uPadding + cbIVlen);
-    pbyIVHead = (unsigned char *)(pbyMacHdr + cbMacHdLen + uPadding);
-
-    // Copy the Packet into a tx Buffer
-    memcpy(pbyMacHdr, pbMPDU, cbMacHdLen);
-
-    // version set to 0, patch for hostapd deamon
-    pMACHeader->wFrameCtl &= cpu_to_le16(0xfffc);
-    memcpy(pbyPayloadHead, (pbMPDU + cbMacHdLen), cbFrameBodySize);
-
-    // replace support rate, patch for hostapd deamon( only support 11M)
-    if (WLAN_GET_FC_FSTYPE(p80211Header->sA4.wFrameCtl) == WLAN_FSTYPE_ASSOCRESP) {
-        if (cbExtSuppRate != 0) {
-            if (((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates)->len != 0)
-                memcpy((pbyPayloadHead + cbFrameBodySize),
-                        pMgmt->abyCurrSuppRates,
-                        ((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates)->len + WLAN_IEHDR_LEN
-                       );
-             if (((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates)->len != 0)
-                memcpy((pbyPayloadHead + cbFrameBodySize) + ((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates)->len + WLAN_IEHDR_LEN,
-                        pMgmt->abyCurrExtSuppRates,
-                        ((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates)->len + WLAN_IEHDR_LEN
-                       );
-         }
-    }
-
-    // Set wep
-    if (WLAN_GET_FC_ISWEP(p80211Header->sA4.wFrameCtl) != 0) {
-
-        if (pDevice->bEnableHostWEP) {
-            pTransmitKey = &STempKey;
-            pTransmitKey->byCipherSuite = pMgmt->sNodeDBTable[uNodeIndex].byCipherSuite;
-            pTransmitKey->dwKeyIndex = pMgmt->sNodeDBTable[uNodeIndex].dwKeyIndex;
-            pTransmitKey->uKeyLength = pMgmt->sNodeDBTable[uNodeIndex].uWepKeyLength;
-            pTransmitKey->dwTSC47_16 = pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16;
-            pTransmitKey->wTSC15_0 = pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0;
-            memcpy(pTransmitKey->abyKey,
-                &pMgmt->sNodeDBTable[uNodeIndex].abyWepKey[0],
-                pTransmitKey->uKeyLength
-                );
-        }
-
-        if ((pTransmitKey != NULL) && (pTransmitKey->byCipherSuite == KEY_CTL_TKIP)) {
-
-            dwMICKey0 = *(unsigned long *)(&pTransmitKey->abyKey[16]);
-            dwMICKey1 = *(unsigned long *)(&pTransmitKey->abyKey[20]);
-
-            // DO Software Michael
-            MIC_vInit(dwMICKey0, dwMICKey1);
-            MIC_vAppend((unsigned char *)&(sEthHeader.abyDstAddr[0]), 12);
-            dwMIC_Priority = 0;
-            MIC_vAppend((unsigned char *)&dwMIC_Priority, 4);
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"DMA0_tx_8021:MIC KEY: %lX, %lX\n", dwMICKey0, dwMICKey1);
-
-            uLength = cbHeaderSize + cbMacHdLen + uPadding + cbIVlen;
-
-            MIC_vAppend((pbyTxBufferAddr + uLength), cbFrameBodySize);
-
-            pdwMIC_L = (unsigned long *)(pbyTxBufferAddr + uLength + cbFrameBodySize);
-            pdwMIC_R = (unsigned long *)(pbyTxBufferAddr + uLength + cbFrameBodySize + 4);
-
-            MIC_vGetMIC(pdwMIC_L, pdwMIC_R);
-            MIC_vUnInit();
-
-            if (pDevice->bTxMICFail == true) {
-                *pdwMIC_L = 0;
-                *pdwMIC_R = 0;
-                pDevice->bTxMICFail = false;
-            }
-
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"uLength: %d, %d\n", uLength, cbFrameBodySize);
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"cbReqCount:%d, %d, %d, %d\n", cbReqCount, cbHeaderSize, uPadding, cbIVlen);
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"MIC:%lx, %lx\n", *pdwMIC_L, *pdwMIC_R);
-
-        }
-
-
-        s_vFillTxKey(pDevice, (unsigned char *)(pTxBufHead->adwTxKey), pbyIVHead, pTransmitKey,
-                     pbyMacHdr, (unsigned short)cbFrameBodySize, (unsigned char *)pMICHDR);
-
-        if (pDevice->bEnableHostWEP) {
-            pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16 = pTransmitKey->dwTSC47_16;
-            pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0 = pTransmitKey->wTSC15_0;
-        }
-
-        if ((pDevice->byLocalID <= REV_ID_VT3253_A1)) {
-            s_vSWencryption(pDevice, pTransmitKey, pbyPayloadHead, (unsigned short)(cbFrameBodySize + cbMIClen));
-        }
-    }
-
-    pMACHeader->wSeqCtl = cpu_to_le16(pDevice->wSeqCounter << 4);
-    pDevice->wSeqCounter++ ;
-    if (pDevice->wSeqCounter > 0x0fff)
-        pDevice->wSeqCounter = 0;
-
-
-    if (bIsPSPOLL) {
-        // The MAC will automatically replace the Duration-field of MAC header by Duration-field
-        // of  FIFO control header.
-        // This will cause AID-field of PS-POLL packet be incorrect (Because PS-POLL's AID field is
-        // in the same place of other packet's Duration-field).
-        // And it will cause Cisco-AP to issue Disassociation-packet
-        if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
-            ((PSTxDataHead_g)pvTxDataHd)->wDuration_a = cpu_to_le16(p80211Header->sA2.wDurationID);
-            ((PSTxDataHead_g)pvTxDataHd)->wDuration_b = cpu_to_le16(p80211Header->sA2.wDurationID);
-        } else {
-            ((PSTxDataHead_ab)pvTxDataHd)->wDuration = cpu_to_le16(p80211Header->sA2.wDurationID);
-        }
-    }
-
-
-    // first TD is the only TD
-    //Set TSR1 & ReqCount in TxDescHead
-    pFrstTD->pTDInfo->skb = skb;
-    pFrstTD->m_td1TD1.byTCR = (TCR_STP | TCR_EDP | EDMSDU);
-    pFrstTD->pTDInfo->skb_dma = pFrstTD->pTDInfo->buf_dma;
-    pFrstTD->m_td1TD1.wReqCount = cpu_to_le16(cbReqCount);
-    pFrstTD->buff_addr = cpu_to_le32(pFrstTD->pTDInfo->skb_dma);
-    pFrstTD->pTDInfo->byFlags = 0;
-    pFrstTD->pTDInfo->byFlags |= TD_FLAGS_PRIV_SKB;
-
-    if (MACbIsRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PS)) {
-        // Disable PS
-        MACbPSWakeup(pDevice->PortOffset);
-    }
-    pDevice->bPWBitOn = false;
-
-    wmb();
-    pFrstTD->m_td0TD0.f1Owner = OWNED_BY_NIC;
-    wmb();
-
-    pDevice->iTDUsed[TYPE_TXDMA0]++;
-
-    if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 1) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " available td0 <= 1\n");
-    }
-
-    pDevice->apCurrTD[TYPE_TXDMA0] = pFrstTD->next;
-
-    // Poll Transmit the adapter
-    MACvTransmit0(pDevice->PortOffset);
-
-    return;
+	pvRrvTime = pMICHDR = pvRTS = pvCTS = pvTxDataHd = NULL;
+
+	if (cbMPDULen <= WLAN_HDR_ADDR3_LEN) {
+		cbFrameBodySize = 0;
+	} else {
+		cbFrameBodySize = cbMPDULen - WLAN_HDR_ADDR3_LEN;
+	}
+	p80211Header = (PUWLAN_80211HDR)pbMPDU;
+
+	pFrstTD = pDevice->apCurrTD[TYPE_TXDMA0];
+	pbyTxBufferAddr = (unsigned char *)pFrstTD->pTDInfo->buf;
+	pTxBufHead = (PSTxBufHead) pbyTxBufferAddr;
+	wTxBufSize = sizeof(STxBufHead);
+	memset(pTxBufHead, 0, wTxBufSize);
+
+	if (pDevice->eCurrentPHYType == PHY_TYPE_11A) {
+		wCurrentRate = RATE_6M;
+		byPktType = PK_TYPE_11A;
+	} else {
+		wCurrentRate = RATE_1M;
+		byPktType = PK_TYPE_11B;
+	}
+
+	// SetPower will cause error power TX state for OFDM Date packet in TX buffer.
+	// 2004.11.11 Kyle -- Using OFDM power to tx MngPkt will decrease the connection capability.
+	//                    And cmd timer will wait data pkt TX to finish before scanning so it's OK
+	//                    to set power here.
+	if (pDevice->pMgmt->eScanState != WMAC_NO_SCANNING) {
+		RFbSetPower(pDevice, wCurrentRate, pDevice->byCurrentCh);
+	} else {
+		RFbSetPower(pDevice, wCurrentRate, pMgmt->uCurrChannel);
+	}
+	pTxBufHead->byTxPower = pDevice->byCurPwr;
+
+	//+++++++++++++++++++++ Patch VT3253 A1 performance +++++++++++++++++++++++++++
+	if (pDevice->byFOETuning) {
+		if ((p80211Header->sA3.wFrameCtl & TYPE_DATE_NULL) == TYPE_DATE_NULL) {
+			wCurrentRate = RATE_24M;
+			byPktType = PK_TYPE_11GA;
+		}
+	}
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "vDMA0_tx_80211: p80211Header->sA3.wFrameCtl = %x \n", p80211Header->sA3.wFrameCtl);
+
+	//Set packet type
+	if (byPktType == PK_TYPE_11A) {//0000 0000 0000 0000
+		pTxBufHead->wFIFOCtl = 0;
+	} else if (byPktType == PK_TYPE_11B) {//0000 0001 0000 0000
+		pTxBufHead->wFIFOCtl |= FIFOCTL_11B;
+	} else if (byPktType == PK_TYPE_11GB) {//0000 0010 0000 0000
+		pTxBufHead->wFIFOCtl |= FIFOCTL_11GB;
+	} else if (byPktType == PK_TYPE_11GA) {//0000 0011 0000 0000
+		pTxBufHead->wFIFOCtl |= FIFOCTL_11GA;
+	}
+
+	pTxBufHead->wFIFOCtl |= FIFOCTL_TMOEN;
+	pTxBufHead->wTimeStamp = cpu_to_le16(DEFAULT_MGN_LIFETIME_RES_64us);
+
+	if (is_multicast_ether_addr(&(p80211Header->sA3.abyAddr1[0]))) {
+		bNeedACK = false;
+		if (pDevice->bEnableHostWEP) {
+			uNodeIndex = 0;
+			bNodeExist = true;
+		}
+	} else {
+		if (pDevice->bEnableHostWEP) {
+			if (BSSDBbIsSTAInNodeDB(pDevice->pMgmt, (unsigned char *)(p80211Header->sA3.abyAddr1), &uNodeIndex))
+				bNodeExist = true;
+		}
+		bNeedACK = true;
+		pTxBufHead->wFIFOCtl |= FIFOCTL_NEEDACK;
+	};
+
+	if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) ||
+	    (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA)) {
+		pTxBufHead->wFIFOCtl |= FIFOCTL_LRETRY;
+	}
+
+	pTxBufHead->wFIFOCtl |= (FIFOCTL_GENINT | FIFOCTL_ISDMA0);
+
+	if ((p80211Header->sA4.wFrameCtl & TYPE_SUBTYPE_MASK) == TYPE_CTL_PSPOLL) {
+		bIsPSPOLL = true;
+		cbMacHdLen = WLAN_HDR_ADDR2_LEN;
+	} else {
+		cbMacHdLen = WLAN_HDR_ADDR3_LEN;
+	}
+
+	// hostapd deamon ext support rate patch
+	if (WLAN_GET_FC_FSTYPE(p80211Header->sA4.wFrameCtl) == WLAN_FSTYPE_ASSOCRESP) {
+		if (((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates)->len != 0) {
+			cbExtSuppRate += ((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates)->len + WLAN_IEHDR_LEN;
+		}
+
+		if (((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates)->len != 0) {
+			cbExtSuppRate += ((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates)->len + WLAN_IEHDR_LEN;
+		}
+
+		if (cbExtSuppRate > 0) {
+			cbFrameBodySize = WLAN_ASSOCRESP_OFF_SUPP_RATES;
+		}
+	}
+
+	//Set FRAGCTL_MACHDCNT
+	pTxBufHead->wFragCtl |= cpu_to_le16((unsigned short)cbMacHdLen << 10);
+
+	// Notes:
+	// Although spec says MMPDU can be fragmented; In most cases,
+	// no one will send a MMPDU under fragmentation. With RTS may occur.
+	pDevice->bAES = false;  //Set FRAGCTL_WEPTYP
+
+	if (WLAN_GET_FC_ISWEP(p80211Header->sA4.wFrameCtl) != 0) {
+		if (pDevice->eEncryptionStatus == Ndis802_11Encryption1Enabled) {
+			cbIVlen = 4;
+			cbICVlen = 4;
+			pTxBufHead->wFragCtl |= FRAGCTL_LEGACY;
+		} else if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled) {
+			cbIVlen = 8;//IV+ExtIV
+			cbMIClen = 8;
+			cbICVlen = 4;
+			pTxBufHead->wFragCtl |= FRAGCTL_TKIP;
+			//We need to get seed here for filling TxKey entry.
+			//TKIPvMixKey(pTransmitKey->abyKey, pDevice->abyCurrentNetAddr,
+			//            pTransmitKey->wTSC15_0, pTransmitKey->dwTSC47_16, pDevice->abyPRNG);
+		} else if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled) {
+			cbIVlen = 8;//RSN Header
+			cbICVlen = 8;//MIC
+			cbMICHDR = sizeof(SMICHDRHead);
+			pTxBufHead->wFragCtl |= FRAGCTL_AES;
+			pDevice->bAES = true;
+		}
+		//MAC Header should be padding 0 to DW alignment.
+		uPadding = 4 - (cbMacHdLen%4);
+		uPadding %= 4;
+	}
+
+	cbFrameSize = cbMacHdLen + cbFrameBodySize + cbIVlen + cbMIClen + cbICVlen + cbFCSlen + cbExtSuppRate;
+
+	//Set FIFOCTL_GrpAckPolicy
+	if (pDevice->bGrpAckPolicy == true) {//0000 0100 0000 0000
+		pTxBufHead->wFIFOCtl |=	FIFOCTL_GRPACK;
+	}
+	//the rest of pTxBufHead->wFragCtl:FragTyp will be set later in s_vFillFragParameter()
+
+	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {//802.11g packet
+
+		pvRrvTime = (PSRrvTime_gCTS) (pbyTxBufferAddr + wTxBufSize);
+		pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS));
+		pvRTS = NULL;
+		pvCTS = (PSCTS) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR);
+		pvTxDataHd = (PSTxDataHead_g) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR + sizeof(SCTS));
+		cbHeaderSize = wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR + sizeof(SCTS) + sizeof(STxDataHead_g);
+
+	} else {//802.11a/b packet
+
+		pvRrvTime = (PSRrvTime_ab) (pbyTxBufferAddr + wTxBufSize);
+		pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab));
+		pvRTS = NULL;
+		pvCTS = NULL;
+		pvTxDataHd = (PSTxDataHead_ab) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab) + cbMICHDR);
+		cbHeaderSize = wTxBufSize + sizeof(SRrvTime_ab) + cbMICHDR + sizeof(STxDataHead_ab);
+
+	}
+
+	memset((void *)(pbyTxBufferAddr + wTxBufSize), 0, (cbHeaderSize - wTxBufSize));
+	memcpy(&(sEthHeader.abyDstAddr[0]), &(p80211Header->sA3.abyAddr1[0]), ETH_ALEN);
+	memcpy(&(sEthHeader.abySrcAddr[0]), &(p80211Header->sA3.abyAddr2[0]), ETH_ALEN);
+	//=========================
+	//    No Fragmentation
+	//=========================
+	pTxBufHead->wFragCtl |= (unsigned short)FRAGCTL_NONFRAG;
+
+	//Fill FIFO,RrvTime,RTS,and CTS
+	s_vGenerateTxParameter(pDevice, byPktType, pbyTxBufferAddr, pvRrvTime, pvRTS, pvCTS,
+			       cbFrameSize, bNeedACK, TYPE_TXDMA0, &sEthHeader, wCurrentRate);
+
+	//Fill DataHead
+	uDuration = s_uFillDataHead(pDevice, byPktType, pvTxDataHd, cbFrameSize, TYPE_TXDMA0, bNeedACK,
+				    0, 0, 1, AUTO_FB_NONE, wCurrentRate);
+
+	pMACHeader = (PS802_11Header) (pbyTxBufferAddr + cbHeaderSize);
+
+	cbReqCount = cbHeaderSize + cbMacHdLen + uPadding + cbIVlen + (cbFrameBodySize + cbMIClen) + cbExtSuppRate;
+
+	pbyMacHdr = (unsigned char *)(pbyTxBufferAddr + cbHeaderSize);
+	pbyPayloadHead = (unsigned char *)(pbyMacHdr + cbMacHdLen + uPadding + cbIVlen);
+	pbyIVHead = (unsigned char *)(pbyMacHdr + cbMacHdLen + uPadding);
+
+	// Copy the Packet into a tx Buffer
+	memcpy(pbyMacHdr, pbMPDU, cbMacHdLen);
+
+	// version set to 0, patch for hostapd deamon
+	pMACHeader->wFrameCtl &= cpu_to_le16(0xfffc);
+	memcpy(pbyPayloadHead, (pbMPDU + cbMacHdLen), cbFrameBodySize);
+
+	// replace support rate, patch for hostapd deamon(only support 11M)
+	if (WLAN_GET_FC_FSTYPE(p80211Header->sA4.wFrameCtl) == WLAN_FSTYPE_ASSOCRESP) {
+		if (cbExtSuppRate != 0) {
+			if (((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates)->len != 0)
+				memcpy((pbyPayloadHead + cbFrameBodySize),
+				       pMgmt->abyCurrSuppRates,
+				       ((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates)->len + WLAN_IEHDR_LEN
+);
+			if (((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates)->len != 0)
+				memcpy((pbyPayloadHead + cbFrameBodySize) + ((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates)->len + WLAN_IEHDR_LEN,
+				       pMgmt->abyCurrExtSuppRates,
+				       ((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates)->len + WLAN_IEHDR_LEN
+);
+		}
+	}
+
+	// Set wep
+	if (WLAN_GET_FC_ISWEP(p80211Header->sA4.wFrameCtl) != 0) {
+		if (pDevice->bEnableHostWEP) {
+			pTransmitKey = &STempKey;
+			pTransmitKey->byCipherSuite = pMgmt->sNodeDBTable[uNodeIndex].byCipherSuite;
+			pTransmitKey->dwKeyIndex = pMgmt->sNodeDBTable[uNodeIndex].dwKeyIndex;
+			pTransmitKey->uKeyLength = pMgmt->sNodeDBTable[uNodeIndex].uWepKeyLength;
+			pTransmitKey->dwTSC47_16 = pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16;
+			pTransmitKey->wTSC15_0 = pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0;
+			memcpy(pTransmitKey->abyKey,
+			       &pMgmt->sNodeDBTable[uNodeIndex].abyWepKey[0],
+			       pTransmitKey->uKeyLength
+);
+		}
+
+		if ((pTransmitKey != NULL) && (pTransmitKey->byCipherSuite == KEY_CTL_TKIP)) {
+			dwMICKey0 = *(unsigned long *)(&pTransmitKey->abyKey[16]);
+			dwMICKey1 = *(unsigned long *)(&pTransmitKey->abyKey[20]);
+
+			// DO Software Michael
+			MIC_vInit(dwMICKey0, dwMICKey1);
+			MIC_vAppend((unsigned char *)&(sEthHeader.abyDstAddr[0]), 12);
+			dwMIC_Priority = 0;
+			MIC_vAppend((unsigned char *)&dwMIC_Priority, 4);
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "DMA0_tx_8021:MIC KEY: %lX, %lX\n", dwMICKey0, dwMICKey1);
+
+			uLength = cbHeaderSize + cbMacHdLen + uPadding + cbIVlen;
+
+			MIC_vAppend((pbyTxBufferAddr + uLength), cbFrameBodySize);
+
+			pdwMIC_L = (unsigned long *)(pbyTxBufferAddr + uLength + cbFrameBodySize);
+			pdwMIC_R = (unsigned long *)(pbyTxBufferAddr + uLength + cbFrameBodySize + 4);
+
+			MIC_vGetMIC(pdwMIC_L, pdwMIC_R);
+			MIC_vUnInit();
+
+			if (pDevice->bTxMICFail == true) {
+				*pdwMIC_L = 0;
+				*pdwMIC_R = 0;
+				pDevice->bTxMICFail = false;
+			}
+
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "uLength: %d, %d\n", uLength, cbFrameBodySize);
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "cbReqCount:%d, %d, %d, %d\n", cbReqCount, cbHeaderSize, uPadding, cbIVlen);
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "MIC:%lx, %lx\n", *pdwMIC_L, *pdwMIC_R);
+
+		}
+
+		s_vFillTxKey(pDevice, (unsigned char *)(pTxBufHead->adwTxKey), pbyIVHead, pTransmitKey,
+			     pbyMacHdr, (unsigned short)cbFrameBodySize, (unsigned char *)pMICHDR);
+
+		if (pDevice->bEnableHostWEP) {
+			pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16 = pTransmitKey->dwTSC47_16;
+			pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0 = pTransmitKey->wTSC15_0;
+		}
+
+		if ((pDevice->byLocalID <= REV_ID_VT3253_A1)) {
+			s_vSWencryption(pDevice, pTransmitKey, pbyPayloadHead, (unsigned short)(cbFrameBodySize + cbMIClen));
+		}
+	}
+
+	pMACHeader->wSeqCtl = cpu_to_le16(pDevice->wSeqCounter << 4);
+	pDevice->wSeqCounter++;
+	if (pDevice->wSeqCounter > 0x0fff)
+		pDevice->wSeqCounter = 0;
+
+	if (bIsPSPOLL) {
+		// The MAC will automatically replace the Duration-field of MAC header by Duration-field
+		// of  FIFO control header.
+		// This will cause AID-field of PS-POLL packet be incorrect (Because PS-POLL's AID field is
+		// in the same place of other packet's Duration-field).
+		// And it will cause Cisco-AP to issue Disassociation-packet
+		if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
+			((PSTxDataHead_g)pvTxDataHd)->wDuration_a = cpu_to_le16(p80211Header->sA2.wDurationID);
+			((PSTxDataHead_g)pvTxDataHd)->wDuration_b = cpu_to_le16(p80211Header->sA2.wDurationID);
+		} else {
+			((PSTxDataHead_ab)pvTxDataHd)->wDuration = cpu_to_le16(p80211Header->sA2.wDurationID);
+		}
+	}
+
+	// first TD is the only TD
+	//Set TSR1 & ReqCount in TxDescHead
+	pFrstTD->pTDInfo->skb = skb;
+	pFrstTD->m_td1TD1.byTCR = (TCR_STP | TCR_EDP | EDMSDU);
+	pFrstTD->pTDInfo->skb_dma = pFrstTD->pTDInfo->buf_dma;
+	pFrstTD->m_td1TD1.wReqCount = cpu_to_le16(cbReqCount);
+	pFrstTD->buff_addr = cpu_to_le32(pFrstTD->pTDInfo->skb_dma);
+	pFrstTD->pTDInfo->byFlags = 0;
+	pFrstTD->pTDInfo->byFlags |= TD_FLAGS_PRIV_SKB;
+
+	if (MACbIsRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PS)) {
+		// Disable PS
+		MACbPSWakeup(pDevice->PortOffset);
+	}
+	pDevice->bPWBitOn = false;
+
+	wmb();
+	pFrstTD->m_td0TD0.f1Owner = OWNED_BY_NIC;
+	wmb();
+
+	pDevice->iTDUsed[TYPE_TXDMA0]++;
+
+	if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 1) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " available td0 <= 1\n");
+	}
+
+	pDevice->apCurrTD[TYPE_TXDMA0] = pFrstTD->next;
+
+	// Poll Transmit the adapter
+	MACvTransmit0(pDevice->PortOffset);
+
+	return;
 }
-
-
diff --git a/drivers/staging/vt6655/rxtx.h b/drivers/staging/vt6655/rxtx.h
index fa827b828a3..4f2cf34dc39 100644
--- a/drivers/staging/vt6655/rxtx.h
+++ b/drivers/staging/vt6655/rxtx.h
@@ -40,45 +40,41 @@
 /*---------------------  Export Functions  --------------------------*/
 
 /*
-void
-vGenerateMACHeader(PSDevice pDevice, unsigned long dwTxBufferAddr, unsigned char *pbySkbData,
-	unsigned int cbPacketSize, bool bDMA0Used, unsigned int *pcbHeadSize,
-	unsigned int *pcbAppendPayload);
-
-void
-vProcessRxMACHeader(PSDevice pDevice, unsigned long dwRxBufferAddr, unsigned int cbPacketSize,
-	bool bIsWEP, unsigned int *pcbHeadSize);
+  void
+  vGenerateMACHeader(PSDevice pDevice, unsigned long dwTxBufferAddr, unsigned char *pbySkbData,
+  unsigned int cbPacketSize, bool bDMA0Used, unsigned int *pcbHeadSize,
+  unsigned int *pcbAppendPayload);
+
+  void
+  vProcessRxMACHeader(PSDevice pDevice, unsigned long dwRxBufferAddr, unsigned int cbPacketSize,
+  bool bIsWEP, unsigned int *pcbHeadSize);
 */
 
-
 void
-vGenerateMACHeader (
-    PSDevice         pDevice,
-    unsigned char *pbyBufferAddr,
-    unsigned short wDuration,
-    PSEthernetHeader psEthHeader,
-    bool bNeedEncrypt,
-    unsigned short wFragType,
-    unsigned int uDMAIdx,
-    unsigned int uFragIdx
-    );
-
+vGenerateMACHeader(
+	PSDevice         pDevice,
+	unsigned char *pbyBufferAddr,
+	unsigned short wDuration,
+	PSEthernetHeader psEthHeader,
+	bool bNeedEncrypt,
+	unsigned short wFragType,
+	unsigned int uDMAIdx,
+	unsigned int uFragIdx
+);
 
 unsigned int
 cbGetFragCount(
-    PSDevice         pDevice,
-    PSKeyItem        pTransmitKey,
-    unsigned int	cbFrameBodySize,
-    PSEthernetHeader psEthHeader
-    );
-
+	PSDevice         pDevice,
+	PSKeyItem        pTransmitKey,
+	unsigned int	cbFrameBodySize,
+	PSEthernetHeader psEthHeader
+);
 
 void
 vGenerateFIFOHeader(PSDevice pDevice, unsigned char byPktTyp, unsigned char *pbyTxBufferAddr,
-	bool bNeedEncrypt, unsigned int	cbPayloadSize, unsigned int uDMAIdx, PSTxDesc pHeadTD,
-	PSEthernetHeader psEthHeader, unsigned char *pPacket, PSKeyItem pTransmitKey,
-	unsigned int uNodeIndex, unsigned int *puMACfragNum, unsigned int *pcbHeaderSize);
-
+		    bool bNeedEncrypt, unsigned int	cbPayloadSize, unsigned int uDMAIdx, PSTxDesc pHeadTD,
+		    PSEthernetHeader psEthHeader, unsigned char *pPacket, PSKeyItem pTransmitKey,
+		    unsigned int uNodeIndex, unsigned int *puMACfragNum, unsigned int *pcbHeaderSize);
 
 void vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, unsigned int cbMPDULen);
 CMD_STATUS csMgmt_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket);
diff --git a/drivers/staging/vt6655/srom.c b/drivers/staging/vt6655/srom.c
index 6a0a232d1a0..eaddc33c9d2 100644
--- a/drivers/staging/vt6655/srom.c
+++ b/drivers/staging/vt6655/srom.c
@@ -60,9 +60,6 @@
 
 /*---------------------  Export Functions  --------------------------*/
 
-
-
-
 /*
  * Description: Read a byte from EEPROM, by MAC I2C
  *
@@ -78,39 +75,38 @@
  */
 unsigned char SROMbyReadEmbedded(unsigned long dwIoBase, unsigned char byContntOffset)
 {
-    unsigned short wDelay, wNoACK;
-    unsigned char byWait;
-    unsigned char byData;
-    unsigned char byOrg;
-
-    byData = 0xFF;
-    VNSvInPortB(dwIoBase + MAC_REG_I2MCFG, &byOrg);
-    /* turn off hardware retry for getting NACK */
-    VNSvOutPortB(dwIoBase + MAC_REG_I2MCFG, (byOrg & (~I2MCFG_NORETRY)));
-    for (wNoACK = 0; wNoACK < W_MAX_I2CRETRY; wNoACK++) {
-        VNSvOutPortB(dwIoBase + MAC_REG_I2MTGID, EEP_I2C_DEV_ID);
-        VNSvOutPortB(dwIoBase + MAC_REG_I2MTGAD, byContntOffset);
-
-        /* issue read command */
-        VNSvOutPortB(dwIoBase + MAC_REG_I2MCSR, I2MCSR_EEMR);
-        /* wait DONE be set */
-        for (wDelay = 0; wDelay < W_MAX_TIMEOUT; wDelay++) {
-            VNSvInPortB(dwIoBase + MAC_REG_I2MCSR, &byWait);
-            if (byWait & (I2MCSR_DONE | I2MCSR_NACK))
-                break;
-            PCAvDelayByIO(CB_DELAY_LOOP_WAIT);
-        }
-        if ((wDelay < W_MAX_TIMEOUT) &&
-             ( !(byWait & I2MCSR_NACK))) {
-            break;
-        }
-    }
-    VNSvInPortB(dwIoBase + MAC_REG_I2MDIPT, &byData);
-    VNSvOutPortB(dwIoBase + MAC_REG_I2MCFG, byOrg);
-    return byData;
+	unsigned short wDelay, wNoACK;
+	unsigned char byWait;
+	unsigned char byData;
+	unsigned char byOrg;
+
+	byData = 0xFF;
+	VNSvInPortB(dwIoBase + MAC_REG_I2MCFG, &byOrg);
+	/* turn off hardware retry for getting NACK */
+	VNSvOutPortB(dwIoBase + MAC_REG_I2MCFG, (byOrg & (~I2MCFG_NORETRY)));
+	for (wNoACK = 0; wNoACK < W_MAX_I2CRETRY; wNoACK++) {
+		VNSvOutPortB(dwIoBase + MAC_REG_I2MTGID, EEP_I2C_DEV_ID);
+		VNSvOutPortB(dwIoBase + MAC_REG_I2MTGAD, byContntOffset);
+
+		/* issue read command */
+		VNSvOutPortB(dwIoBase + MAC_REG_I2MCSR, I2MCSR_EEMR);
+		/* wait DONE be set */
+		for (wDelay = 0; wDelay < W_MAX_TIMEOUT; wDelay++) {
+			VNSvInPortB(dwIoBase + MAC_REG_I2MCSR, &byWait);
+			if (byWait & (I2MCSR_DONE | I2MCSR_NACK))
+				break;
+			PCAvDelayByIO(CB_DELAY_LOOP_WAIT);
+		}
+		if ((wDelay < W_MAX_TIMEOUT) &&
+		    (!(byWait & I2MCSR_NACK))) {
+			break;
+		}
+	}
+	VNSvInPortB(dwIoBase + MAC_REG_I2MDIPT, &byData);
+	VNSvOutPortB(dwIoBase + MAC_REG_I2MCFG, byOrg);
+	return byData;
 }
 
-
 /*
  * Description: Write a byte to EEPROM, by MAC I2C
  *
@@ -127,43 +123,42 @@ unsigned char SROMbyReadEmbedded(unsigned long dwIoBase, unsigned char byContntO
  */
 bool SROMbWriteEmbedded(unsigned long dwIoBase, unsigned char byContntOffset, unsigned char byData)
 {
-    unsigned short wDelay, wNoACK;
-    unsigned char byWait;
-
-    unsigned char byOrg;
-
-    VNSvInPortB(dwIoBase + MAC_REG_I2MCFG, &byOrg);
-    /* turn off hardware retry for getting NACK */
-    VNSvOutPortB(dwIoBase + MAC_REG_I2MCFG, (byOrg & (~I2MCFG_NORETRY)));
-    for (wNoACK = 0; wNoACK < W_MAX_I2CRETRY; wNoACK++) {
-        VNSvOutPortB(dwIoBase + MAC_REG_I2MTGID, EEP_I2C_DEV_ID);
-        VNSvOutPortB(dwIoBase + MAC_REG_I2MTGAD, byContntOffset);
-        VNSvOutPortB(dwIoBase + MAC_REG_I2MDOPT, byData);
-
-        /* issue write command */
-        VNSvOutPortB(dwIoBase + MAC_REG_I2MCSR, I2MCSR_EEMW);
-        /* wait DONE be set */
-        for (wDelay = 0; wDelay < W_MAX_TIMEOUT; wDelay++) {
-            VNSvInPortB(dwIoBase + MAC_REG_I2MCSR, &byWait);
-            if (byWait & (I2MCSR_DONE | I2MCSR_NACK))
-                break;
-            PCAvDelayByIO(CB_DELAY_LOOP_WAIT);
-        }
-
-        if ((wDelay < W_MAX_TIMEOUT) &&
-             ( !(byWait & I2MCSR_NACK))) {
-            break;
-        }
-    }
-    if (wNoACK == W_MAX_I2CRETRY) {
-        VNSvOutPortB(dwIoBase + MAC_REG_I2MCFG, byOrg);
-        return false;
-    }
-    VNSvOutPortB(dwIoBase + MAC_REG_I2MCFG, byOrg);
-    return true;
+	unsigned short wDelay, wNoACK;
+	unsigned char byWait;
+
+	unsigned char byOrg;
+
+	VNSvInPortB(dwIoBase + MAC_REG_I2MCFG, &byOrg);
+	/* turn off hardware retry for getting NACK */
+	VNSvOutPortB(dwIoBase + MAC_REG_I2MCFG, (byOrg & (~I2MCFG_NORETRY)));
+	for (wNoACK = 0; wNoACK < W_MAX_I2CRETRY; wNoACK++) {
+		VNSvOutPortB(dwIoBase + MAC_REG_I2MTGID, EEP_I2C_DEV_ID);
+		VNSvOutPortB(dwIoBase + MAC_REG_I2MTGAD, byContntOffset);
+		VNSvOutPortB(dwIoBase + MAC_REG_I2MDOPT, byData);
+
+		/* issue write command */
+		VNSvOutPortB(dwIoBase + MAC_REG_I2MCSR, I2MCSR_EEMW);
+		/* wait DONE be set */
+		for (wDelay = 0; wDelay < W_MAX_TIMEOUT; wDelay++) {
+			VNSvInPortB(dwIoBase + MAC_REG_I2MCSR, &byWait);
+			if (byWait & (I2MCSR_DONE | I2MCSR_NACK))
+				break;
+			PCAvDelayByIO(CB_DELAY_LOOP_WAIT);
+		}
+
+		if ((wDelay < W_MAX_TIMEOUT) &&
+		    (!(byWait & I2MCSR_NACK))) {
+			break;
+		}
+	}
+	if (wNoACK == W_MAX_I2CRETRY) {
+		VNSvOutPortB(dwIoBase + MAC_REG_I2MCFG, byOrg);
+		return false;
+	}
+	VNSvOutPortB(dwIoBase + MAC_REG_I2MCFG, byOrg);
+	return true;
 }
 
-
 /*
  * Description: Turn bits on in eeprom
  *
@@ -180,13 +175,12 @@ bool SROMbWriteEmbedded(unsigned long dwIoBase, unsigned char byContntOffset, un
  */
 void SROMvRegBitsOn(unsigned long dwIoBase, unsigned char byContntOffset, unsigned char byBits)
 {
-    unsigned char byOrgData;
+	unsigned char byOrgData;
 
-    byOrgData = SROMbyReadEmbedded(dwIoBase, byContntOffset);
-    SROMbWriteEmbedded(dwIoBase, byContntOffset,(unsigned char)(byOrgData | byBits));
+	byOrgData = SROMbyReadEmbedded(dwIoBase, byContntOffset);
+	SROMbWriteEmbedded(dwIoBase, byContntOffset, (unsigned char)(byOrgData | byBits));
 }
 
-
 /*
  * Description: Turn bits off in eeprom
  *
@@ -201,13 +195,12 @@ void SROMvRegBitsOn(unsigned long dwIoBase, unsigned char byContntOffset, unsign
  */
 void SROMvRegBitsOff(unsigned long dwIoBase, unsigned char byContntOffset, unsigned char byBits)
 {
-    unsigned char byOrgData;
+	unsigned char byOrgData;
 
-    byOrgData = SROMbyReadEmbedded(dwIoBase, byContntOffset);
-    SROMbWriteEmbedded(dwIoBase, byContntOffset,(unsigned char)(byOrgData & (~byBits)));
+	byOrgData = SROMbyReadEmbedded(dwIoBase, byContntOffset);
+	SROMbWriteEmbedded(dwIoBase, byContntOffset, (unsigned char)(byOrgData & (~byBits)));
 }
 
-
 /*
  * Description: Test if bits on in eeprom
  *
@@ -224,13 +217,12 @@ void SROMvRegBitsOff(unsigned long dwIoBase, unsigned char byContntOffset, unsig
  */
 bool SROMbIsRegBitsOn(unsigned long dwIoBase, unsigned char byContntOffset, unsigned char byTestBits)
 {
-    unsigned char byOrgData;
+	unsigned char byOrgData;
 
-    byOrgData = SROMbyReadEmbedded(dwIoBase, byContntOffset);
-    return (byOrgData & byTestBits) == byTestBits;
+	byOrgData = SROMbyReadEmbedded(dwIoBase, byContntOffset);
+	return (byOrgData & byTestBits) == byTestBits;
 }
 
-
 /*
  * Description: Test if bits off in eeprom
  *
@@ -247,13 +239,12 @@ bool SROMbIsRegBitsOn(unsigned long dwIoBase, unsigned char byContntOffset, unsi
  */
 bool SROMbIsRegBitsOff(unsigned long dwIoBase, unsigned char byContntOffset, unsigned char byTestBits)
 {
-    unsigned char byOrgData;
+	unsigned char byOrgData;
 
-    byOrgData = SROMbyReadEmbedded(dwIoBase, byContntOffset);
-    return !(byOrgData & byTestBits);
+	byOrgData = SROMbyReadEmbedded(dwIoBase, byContntOffset);
+	return !(byOrgData & byTestBits);
 }
 
-
 /*
  * Description: Read all contents of eeprom to buffer
  *
@@ -268,16 +259,15 @@ bool SROMbIsRegBitsOff(unsigned long dwIoBase, unsigned char byContntOffset, uns
  */
 void SROMvReadAllContents(unsigned long dwIoBase, unsigned char *pbyEepromRegs)
 {
-    int     ii;
+	int     ii;
 
-    /* ii = Rom Address */
-    for (ii = 0; ii < EEP_MAX_CONTEXT_SIZE; ii++) {
-        *pbyEepromRegs = SROMbyReadEmbedded(dwIoBase,(unsigned char) ii);
-        pbyEepromRegs++;
-    }
+	/* ii = Rom Address */
+	for (ii = 0; ii < EEP_MAX_CONTEXT_SIZE; ii++) {
+		*pbyEepromRegs = SROMbyReadEmbedded(dwIoBase, (unsigned char)ii);
+		pbyEepromRegs++;
+	}
 }
 
-
 /*
  * Description: Write all contents of buffer to eeprom
  *
@@ -293,16 +283,15 @@ void SROMvReadAllContents(unsigned long dwIoBase, unsigned char *pbyEepromRegs)
  */
 void SROMvWriteAllContents(unsigned long dwIoBase, unsigned char *pbyEepromRegs)
 {
-    int     ii;
+	int     ii;
 
-    /* ii = Rom Address */
-    for (ii = 0; ii < EEP_MAX_CONTEXT_SIZE; ii++) {
-        SROMbWriteEmbedded(dwIoBase,(unsigned char) ii, *pbyEepromRegs);
-        pbyEepromRegs++;
-    }
+	/* ii = Rom Address */
+	for (ii = 0; ii < EEP_MAX_CONTEXT_SIZE; ii++) {
+		SROMbWriteEmbedded(dwIoBase, (unsigned char)ii, *pbyEepromRegs);
+		pbyEepromRegs++;
+	}
 }
 
-
 /*
  * Description: Read Ethernet Address from eeprom to buffer
  *
@@ -317,16 +306,15 @@ void SROMvWriteAllContents(unsigned long dwIoBase, unsigned char *pbyEepromRegs)
  */
 void SROMvReadEtherAddress(unsigned long dwIoBase, unsigned char *pbyEtherAddress)
 {
-    unsigned char ii;
+	unsigned char ii;
 
-    /* ii = Rom Address */
-    for (ii = 0; ii < ETH_ALEN; ii++) {
-        *pbyEtherAddress = SROMbyReadEmbedded(dwIoBase, ii);
-        pbyEtherAddress++;
-    }
+	/* ii = Rom Address */
+	for (ii = 0; ii < ETH_ALEN; ii++) {
+		*pbyEtherAddress = SROMbyReadEmbedded(dwIoBase, ii);
+		pbyEtherAddress++;
+	}
 }
 
-
 /*
  * Description: Write Ethernet Address from buffer to eeprom
  *
@@ -342,16 +330,15 @@ void SROMvReadEtherAddress(unsigned long dwIoBase, unsigned char *pbyEtherAddres
  */
 void SROMvWriteEtherAddress(unsigned long dwIoBase, unsigned char *pbyEtherAddress)
 {
-    unsigned char ii;
+	unsigned char ii;
 
-    /* ii = Rom Address */
-    for (ii = 0; ii < ETH_ALEN; ii++) {
-        SROMbWriteEmbedded(dwIoBase, ii, *pbyEtherAddress);
-        pbyEtherAddress++;
-    }
+	/* ii = Rom Address */
+	for (ii = 0; ii < ETH_ALEN; ii++) {
+		SROMbWriteEmbedded(dwIoBase, ii, *pbyEtherAddress);
+		pbyEtherAddress++;
+	}
 }
 
-
 /*
  * Description: Read Sub_VID and Sub_SysId from eeprom to buffer
  *
@@ -366,15 +353,15 @@ void SROMvWriteEtherAddress(unsigned long dwIoBase, unsigned char *pbyEtherAddre
  */
 void SROMvReadSubSysVenId(unsigned long dwIoBase, unsigned long *pdwSubSysVenId)
 {
-    unsigned char *pbyData;
-
-    pbyData = (unsigned char *)pdwSubSysVenId;
-    /* sub vendor */
-    *pbyData = SROMbyReadEmbedded(dwIoBase, 6);
-    *(pbyData+1) = SROMbyReadEmbedded(dwIoBase, 7);
-    /* sub system */
-    *(pbyData+2) = SROMbyReadEmbedded(dwIoBase, 8);
-    *(pbyData+3) = SROMbyReadEmbedded(dwIoBase, 9);
+	unsigned char *pbyData;
+
+	pbyData = (unsigned char *)pdwSubSysVenId;
+	/* sub vendor */
+	*pbyData = SROMbyReadEmbedded(dwIoBase, 6);
+	*(pbyData+1) = SROMbyReadEmbedded(dwIoBase, 7);
+	/* sub system */
+	*(pbyData+2) = SROMbyReadEmbedded(dwIoBase, 8);
+	*(pbyData+3) = SROMbyReadEmbedded(dwIoBase, 9);
 }
 
 /*
@@ -391,30 +378,28 @@ void SROMvReadSubSysVenId(unsigned long dwIoBase, unsigned long *pdwSubSysVenId)
  */
 bool SROMbAutoLoad(unsigned long dwIoBase)
 {
-    unsigned char byWait;
-    int     ii;
+	unsigned char byWait;
+	int     ii;
 
-    unsigned char byOrg;
+	unsigned char byOrg;
 
-    VNSvInPortB(dwIoBase + MAC_REG_I2MCFG, &byOrg);
-    /* turn on hardware retry */
-    VNSvOutPortB(dwIoBase + MAC_REG_I2MCFG, (byOrg | I2MCFG_NORETRY));
+	VNSvInPortB(dwIoBase + MAC_REG_I2MCFG, &byOrg);
+	/* turn on hardware retry */
+	VNSvOutPortB(dwIoBase + MAC_REG_I2MCFG, (byOrg | I2MCFG_NORETRY));
 
-    MACvRegBitsOn(dwIoBase, MAC_REG_I2MCSR, I2MCSR_AUTOLD);
+	MACvRegBitsOn(dwIoBase, MAC_REG_I2MCSR, I2MCSR_AUTOLD);
 
-    /* ii = Rom Address */
-    for (ii = 0; ii < EEP_MAX_CONTEXT_SIZE; ii++) {
-        MACvTimer0MicroSDelay(dwIoBase, CB_EEPROM_READBYTE_WAIT);
-        VNSvInPortB(dwIoBase + MAC_REG_I2MCSR, &byWait);
-        if ( !(byWait & I2MCSR_AUTOLD))
-            break;
-    }
+	/* ii = Rom Address */
+	for (ii = 0; ii < EEP_MAX_CONTEXT_SIZE; ii++) {
+		MACvTimer0MicroSDelay(dwIoBase, CB_EEPROM_READBYTE_WAIT);
+		VNSvInPortB(dwIoBase + MAC_REG_I2MCSR, &byWait);
+		if (!(byWait & I2MCSR_AUTOLD))
+			break;
+	}
 
-    VNSvOutPortB(dwIoBase + MAC_REG_I2MCFG, byOrg);
+	VNSvOutPortB(dwIoBase + MAC_REG_I2MCFG, byOrg);
 
-    if (ii == EEP_MAX_CONTEXT_SIZE)
-        return false;
-    return true;
+	if (ii == EEP_MAX_CONTEXT_SIZE)
+		return false;
+	return true;
 }
-
-
diff --git a/drivers/staging/vt6655/srom.h b/drivers/staging/vt6655/srom.h
index 4c261dac01b..1df58c516dd 100644
--- a/drivers/staging/vt6655/srom.h
+++ b/drivers/staging/vt6655/srom.h
@@ -69,7 +69,6 @@
 //}}
 #define EEP_OFS_OFDMA_PWR_dBm       0xD2
 
-
 //----------need to remove --------------------
 #define EEP_OFS_BBTAB_LEN   0x70        // BB Table Length
 #define EEP_OFS_BBTAB_ADR   0x71        // BB Table Offset
@@ -77,7 +76,6 @@
 
 #define EEP_I2C_DEV_ID      0x50        // EEPROM device address on the I2C bus
 
-
 //
 // Bits in EEP_OFS_ANTENNA
 //
@@ -97,34 +95,34 @@
 //      2048 bits = 256 bytes = 128 words
 //
 typedef struct tagSSromReg {
-    unsigned char abyPAR[6];                  // 0x00 (unsigned short)
-
-    unsigned short wSUB_VID;                   // 0x03 (unsigned short)
-    unsigned short wSUB_SID;
-
-    unsigned char byBCFG0;                    // 0x05 (unsigned short)
-    unsigned char byBCFG1;
-
-    unsigned char byFCR0;                     // 0x06 (unsigned short)
-    unsigned char byFCR1;
-    unsigned char byPMC0;                     // 0x07 (unsigned short)
-    unsigned char byPMC1;
-    unsigned char byMAXLAT;                   // 0x08 (unsigned short)
-    unsigned char byMINGNT;
-    unsigned char byCFG0;                     // 0x09 (unsigned short)
-    unsigned char byCFG1;
-    unsigned short wCISPTR;                    // 0x0A (unsigned short)
-    unsigned short wRsv0;                      // 0x0B (unsigned short)
-    unsigned short wRsv1;                      // 0x0C (unsigned short)
-    unsigned char byBBPAIR;                   // 0x0D (unsigned short)
-    unsigned char byRFTYPE;
-    unsigned char byMinChannel;               // 0x0E (unsigned short)
-    unsigned char byMaxChannel;
-    unsigned char bySignature;                // 0x0F (unsigned short)
-    unsigned char byCheckSum;
-
-    unsigned char abyReserved0[96];           // 0x10 (unsigned short)
-    unsigned char abyCIS[128];                // 0x80 (unsigned short)
+	unsigned char abyPAR[6];                  // 0x00 (unsigned short)
+
+	unsigned short wSUB_VID;                   // 0x03 (unsigned short)
+	unsigned short wSUB_SID;
+
+	unsigned char byBCFG0;                    // 0x05 (unsigned short)
+	unsigned char byBCFG1;
+
+	unsigned char byFCR0;                     // 0x06 (unsigned short)
+	unsigned char byFCR1;
+	unsigned char byPMC0;                     // 0x07 (unsigned short)
+	unsigned char byPMC1;
+	unsigned char byMAXLAT;                   // 0x08 (unsigned short)
+	unsigned char byMINGNT;
+	unsigned char byCFG0;                     // 0x09 (unsigned short)
+	unsigned char byCFG1;
+	unsigned short wCISPTR;                    // 0x0A (unsigned short)
+	unsigned short wRsv0;                      // 0x0B (unsigned short)
+	unsigned short wRsv1;                      // 0x0C (unsigned short)
+	unsigned char byBBPAIR;                   // 0x0D (unsigned short)
+	unsigned char byRFTYPE;
+	unsigned char byMinChannel;               // 0x0E (unsigned short)
+	unsigned char byMaxChannel;
+	unsigned char bySignature;                // 0x0F (unsigned short)
+	unsigned char byCheckSum;
+
+	unsigned char abyReserved0[96];           // 0x10 (unsigned short)
+	unsigned char abyCIS[128];                // 0x80 (unsigned short)
 } SSromReg, *PSSromReg;
 
 /*---------------------  Export Macros ------------------------------*/
@@ -152,6 +150,6 @@ void SROMvWriteEtherAddress(unsigned long dwIoBase, unsigned char *pbyEtherAddre
 
 void SROMvReadSubSysVenId(unsigned long dwIoBase, unsigned long *pdwSubSysVenId);
 
-bool SROMbAutoLoad (unsigned long dwIoBase);
+bool SROMbAutoLoad(unsigned long dwIoBase);
 
 #endif // __EEPROM_H__
diff --git a/drivers/staging/vt6655/tcrc.c b/drivers/staging/vt6655/tcrc.c
index 1313c4cd086..ed6868a9c16 100644
--- a/drivers/staging/vt6655/tcrc.c
+++ b/drivers/staging/vt6655/tcrc.c
@@ -43,79 +43,76 @@
 
 // 32-bit CRC table
 static const unsigned long s_adwCrc32Table[256] = {
-    0x00000000L, 0x77073096L, 0xEE0E612CL, 0x990951BAL,
-    0x076DC419L, 0x706AF48FL, 0xE963A535L, 0x9E6495A3L,
-    0x0EDB8832L, 0x79DCB8A4L, 0xE0D5E91EL, 0x97D2D988L,
-    0x09B64C2BL, 0x7EB17CBDL, 0xE7B82D07L, 0x90BF1D91L,
-    0x1DB71064L, 0x6AB020F2L, 0xF3B97148L, 0x84BE41DEL,
-    0x1ADAD47DL, 0x6DDDE4EBL, 0xF4D4B551L, 0x83D385C7L,
-    0x136C9856L, 0x646BA8C0L, 0xFD62F97AL, 0x8A65C9ECL,
-    0x14015C4FL, 0x63066CD9L, 0xFA0F3D63L, 0x8D080DF5L,
-    0x3B6E20C8L, 0x4C69105EL, 0xD56041E4L, 0xA2677172L,
-    0x3C03E4D1L, 0x4B04D447L, 0xD20D85FDL, 0xA50AB56BL,
-    0x35B5A8FAL, 0x42B2986CL, 0xDBBBC9D6L, 0xACBCF940L,
-    0x32D86CE3L, 0x45DF5C75L, 0xDCD60DCFL, 0xABD13D59L,
-    0x26D930ACL, 0x51DE003AL, 0xC8D75180L, 0xBFD06116L,
-    0x21B4F4B5L, 0x56B3C423L, 0xCFBA9599L, 0xB8BDA50FL,
-    0x2802B89EL, 0x5F058808L, 0xC60CD9B2L, 0xB10BE924L,
-    0x2F6F7C87L, 0x58684C11L, 0xC1611DABL, 0xB6662D3DL,
-    0x76DC4190L, 0x01DB7106L, 0x98D220BCL, 0xEFD5102AL,
-    0x71B18589L, 0x06B6B51FL, 0x9FBFE4A5L, 0xE8B8D433L,
-    0x7807C9A2L, 0x0F00F934L, 0x9609A88EL, 0xE10E9818L,
-    0x7F6A0DBBL, 0x086D3D2DL, 0x91646C97L, 0xE6635C01L,
-    0x6B6B51F4L, 0x1C6C6162L, 0x856530D8L, 0xF262004EL,
-    0x6C0695EDL, 0x1B01A57BL, 0x8208F4C1L, 0xF50FC457L,
-    0x65B0D9C6L, 0x12B7E950L, 0x8BBEB8EAL, 0xFCB9887CL,
-    0x62DD1DDFL, 0x15DA2D49L, 0x8CD37CF3L, 0xFBD44C65L,
-    0x4DB26158L, 0x3AB551CEL, 0xA3BC0074L, 0xD4BB30E2L,
-    0x4ADFA541L, 0x3DD895D7L, 0xA4D1C46DL, 0xD3D6F4FBL,
-    0x4369E96AL, 0x346ED9FCL, 0xAD678846L, 0xDA60B8D0L,
-    0x44042D73L, 0x33031DE5L, 0xAA0A4C5FL, 0xDD0D7CC9L,
-    0x5005713CL, 0x270241AAL, 0xBE0B1010L, 0xC90C2086L,
-    0x5768B525L, 0x206F85B3L, 0xB966D409L, 0xCE61E49FL,
-    0x5EDEF90EL, 0x29D9C998L, 0xB0D09822L, 0xC7D7A8B4L,
-    0x59B33D17L, 0x2EB40D81L, 0xB7BD5C3BL, 0xC0BA6CADL,
-    0xEDB88320L, 0x9ABFB3B6L, 0x03B6E20CL, 0x74B1D29AL,
-    0xEAD54739L, 0x9DD277AFL, 0x04DB2615L, 0x73DC1683L,
-    0xE3630B12L, 0x94643B84L, 0x0D6D6A3EL, 0x7A6A5AA8L,
-    0xE40ECF0BL, 0x9309FF9DL, 0x0A00AE27L, 0x7D079EB1L,
-    0xF00F9344L, 0x8708A3D2L, 0x1E01F268L, 0x6906C2FEL,
-    0xF762575DL, 0x806567CBL, 0x196C3671L, 0x6E6B06E7L,
-    0xFED41B76L, 0x89D32BE0L, 0x10DA7A5AL, 0x67DD4ACCL,
-    0xF9B9DF6FL, 0x8EBEEFF9L, 0x17B7BE43L, 0x60B08ED5L,
-    0xD6D6A3E8L, 0xA1D1937EL, 0x38D8C2C4L, 0x4FDFF252L,
-    0xD1BB67F1L, 0xA6BC5767L, 0x3FB506DDL, 0x48B2364BL,
-    0xD80D2BDAL, 0xAF0A1B4CL, 0x36034AF6L, 0x41047A60L,
-    0xDF60EFC3L, 0xA867DF55L, 0x316E8EEFL, 0x4669BE79L,
-    0xCB61B38CL, 0xBC66831AL, 0x256FD2A0L, 0x5268E236L,
-    0xCC0C7795L, 0xBB0B4703L, 0x220216B9L, 0x5505262FL,
-    0xC5BA3BBEL, 0xB2BD0B28L, 0x2BB45A92L, 0x5CB36A04L,
-    0xC2D7FFA7L, 0xB5D0CF31L, 0x2CD99E8BL, 0x5BDEAE1DL,
-    0x9B64C2B0L, 0xEC63F226L, 0x756AA39CL, 0x026D930AL,
-    0x9C0906A9L, 0xEB0E363FL, 0x72076785L, 0x05005713L,
-    0x95BF4A82L, 0xE2B87A14L, 0x7BB12BAEL, 0x0CB61B38L,
-    0x92D28E9BL, 0xE5D5BE0DL, 0x7CDCEFB7L, 0x0BDBDF21L,
-    0x86D3D2D4L, 0xF1D4E242L, 0x68DDB3F8L, 0x1FDA836EL,
-    0x81BE16CDL, 0xF6B9265BL, 0x6FB077E1L, 0x18B74777L,
-    0x88085AE6L, 0xFF0F6A70L, 0x66063BCAL, 0x11010B5CL,
-    0x8F659EFFL, 0xF862AE69L, 0x616BFFD3L, 0x166CCF45L,
-    0xA00AE278L, 0xD70DD2EEL, 0x4E048354L, 0x3903B3C2L,
-    0xA7672661L, 0xD06016F7L, 0x4969474DL, 0x3E6E77DBL,
-    0xAED16A4AL, 0xD9D65ADCL, 0x40DF0B66L, 0x37D83BF0L,
-    0xA9BCAE53L, 0xDEBB9EC5L, 0x47B2CF7FL, 0x30B5FFE9L,
-    0xBDBDF21CL, 0xCABAC28AL, 0x53B39330L, 0x24B4A3A6L,
-    0xBAD03605L, 0xCDD70693L, 0x54DE5729L, 0x23D967BFL,
-    0xB3667A2EL, 0xC4614AB8L, 0x5D681B02L, 0x2A6F2B94L,
-    0xB40BBE37L, 0xC30C8EA1L, 0x5A05DF1BL, 0x2D02EF8DL
+	0x00000000L, 0x77073096L, 0xEE0E612CL, 0x990951BAL,
+	0x076DC419L, 0x706AF48FL, 0xE963A535L, 0x9E6495A3L,
+	0x0EDB8832L, 0x79DCB8A4L, 0xE0D5E91EL, 0x97D2D988L,
+	0x09B64C2BL, 0x7EB17CBDL, 0xE7B82D07L, 0x90BF1D91L,
+	0x1DB71064L, 0x6AB020F2L, 0xF3B97148L, 0x84BE41DEL,
+	0x1ADAD47DL, 0x6DDDE4EBL, 0xF4D4B551L, 0x83D385C7L,
+	0x136C9856L, 0x646BA8C0L, 0xFD62F97AL, 0x8A65C9ECL,
+	0x14015C4FL, 0x63066CD9L, 0xFA0F3D63L, 0x8D080DF5L,
+	0x3B6E20C8L, 0x4C69105EL, 0xD56041E4L, 0xA2677172L,
+	0x3C03E4D1L, 0x4B04D447L, 0xD20D85FDL, 0xA50AB56BL,
+	0x35B5A8FAL, 0x42B2986CL, 0xDBBBC9D6L, 0xACBCF940L,
+	0x32D86CE3L, 0x45DF5C75L, 0xDCD60DCFL, 0xABD13D59L,
+	0x26D930ACL, 0x51DE003AL, 0xC8D75180L, 0xBFD06116L,
+	0x21B4F4B5L, 0x56B3C423L, 0xCFBA9599L, 0xB8BDA50FL,
+	0x2802B89EL, 0x5F058808L, 0xC60CD9B2L, 0xB10BE924L,
+	0x2F6F7C87L, 0x58684C11L, 0xC1611DABL, 0xB6662D3DL,
+	0x76DC4190L, 0x01DB7106L, 0x98D220BCL, 0xEFD5102AL,
+	0x71B18589L, 0x06B6B51FL, 0x9FBFE4A5L, 0xE8B8D433L,
+	0x7807C9A2L, 0x0F00F934L, 0x9609A88EL, 0xE10E9818L,
+	0x7F6A0DBBL, 0x086D3D2DL, 0x91646C97L, 0xE6635C01L,
+	0x6B6B51F4L, 0x1C6C6162L, 0x856530D8L, 0xF262004EL,
+	0x6C0695EDL, 0x1B01A57BL, 0x8208F4C1L, 0xF50FC457L,
+	0x65B0D9C6L, 0x12B7E950L, 0x8BBEB8EAL, 0xFCB9887CL,
+	0x62DD1DDFL, 0x15DA2D49L, 0x8CD37CF3L, 0xFBD44C65L,
+	0x4DB26158L, 0x3AB551CEL, 0xA3BC0074L, 0xD4BB30E2L,
+	0x4ADFA541L, 0x3DD895D7L, 0xA4D1C46DL, 0xD3D6F4FBL,
+	0x4369E96AL, 0x346ED9FCL, 0xAD678846L, 0xDA60B8D0L,
+	0x44042D73L, 0x33031DE5L, 0xAA0A4C5FL, 0xDD0D7CC9L,
+	0x5005713CL, 0x270241AAL, 0xBE0B1010L, 0xC90C2086L,
+	0x5768B525L, 0x206F85B3L, 0xB966D409L, 0xCE61E49FL,
+	0x5EDEF90EL, 0x29D9C998L, 0xB0D09822L, 0xC7D7A8B4L,
+	0x59B33D17L, 0x2EB40D81L, 0xB7BD5C3BL, 0xC0BA6CADL,
+	0xEDB88320L, 0x9ABFB3B6L, 0x03B6E20CL, 0x74B1D29AL,
+	0xEAD54739L, 0x9DD277AFL, 0x04DB2615L, 0x73DC1683L,
+	0xE3630B12L, 0x94643B84L, 0x0D6D6A3EL, 0x7A6A5AA8L,
+	0xE40ECF0BL, 0x9309FF9DL, 0x0A00AE27L, 0x7D079EB1L,
+	0xF00F9344L, 0x8708A3D2L, 0x1E01F268L, 0x6906C2FEL,
+	0xF762575DL, 0x806567CBL, 0x196C3671L, 0x6E6B06E7L,
+	0xFED41B76L, 0x89D32BE0L, 0x10DA7A5AL, 0x67DD4ACCL,
+	0xF9B9DF6FL, 0x8EBEEFF9L, 0x17B7BE43L, 0x60B08ED5L,
+	0xD6D6A3E8L, 0xA1D1937EL, 0x38D8C2C4L, 0x4FDFF252L,
+	0xD1BB67F1L, 0xA6BC5767L, 0x3FB506DDL, 0x48B2364BL,
+	0xD80D2BDAL, 0xAF0A1B4CL, 0x36034AF6L, 0x41047A60L,
+	0xDF60EFC3L, 0xA867DF55L, 0x316E8EEFL, 0x4669BE79L,
+	0xCB61B38CL, 0xBC66831AL, 0x256FD2A0L, 0x5268E236L,
+	0xCC0C7795L, 0xBB0B4703L, 0x220216B9L, 0x5505262FL,
+	0xC5BA3BBEL, 0xB2BD0B28L, 0x2BB45A92L, 0x5CB36A04L,
+	0xC2D7FFA7L, 0xB5D0CF31L, 0x2CD99E8BL, 0x5BDEAE1DL,
+	0x9B64C2B0L, 0xEC63F226L, 0x756AA39CL, 0x026D930AL,
+	0x9C0906A9L, 0xEB0E363FL, 0x72076785L, 0x05005713L,
+	0x95BF4A82L, 0xE2B87A14L, 0x7BB12BAEL, 0x0CB61B38L,
+	0x92D28E9BL, 0xE5D5BE0DL, 0x7CDCEFB7L, 0x0BDBDF21L,
+	0x86D3D2D4L, 0xF1D4E242L, 0x68DDB3F8L, 0x1FDA836EL,
+	0x81BE16CDL, 0xF6B9265BL, 0x6FB077E1L, 0x18B74777L,
+	0x88085AE6L, 0xFF0F6A70L, 0x66063BCAL, 0x11010B5CL,
+	0x8F659EFFL, 0xF862AE69L, 0x616BFFD3L, 0x166CCF45L,
+	0xA00AE278L, 0xD70DD2EEL, 0x4E048354L, 0x3903B3C2L,
+	0xA7672661L, 0xD06016F7L, 0x4969474DL, 0x3E6E77DBL,
+	0xAED16A4AL, 0xD9D65ADCL, 0x40DF0B66L, 0x37D83BF0L,
+	0xA9BCAE53L, 0xDEBB9EC5L, 0x47B2CF7FL, 0x30B5FFE9L,
+	0xBDBDF21CL, 0xCABAC28AL, 0x53B39330L, 0x24B4A3A6L,
+	0xBAD03605L, 0xCDD70693L, 0x54DE5729L, 0x23D967BFL,
+	0xB3667A2EL, 0xC4614AB8L, 0x5D681B02L, 0x2A6F2B94L,
+	0xB40BBE37L, 0xC30C8EA1L, 0x5A05DF1BL, 0x2D02EF8DL
 };
 
 /*---------------------  Static Functions  --------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
 
-
-
-
 /*+
  *
  * Description:
@@ -131,21 +128,20 @@ static const unsigned long s_adwCrc32Table[256] = {
  *
  * Return Value: CRC-32
  *
--*/
-unsigned long CRCdwCrc32 (unsigned char *pbyData, unsigned int cbByte, unsigned long dwCrcSeed)
+ -*/
+unsigned long CRCdwCrc32(unsigned char *pbyData, unsigned int cbByte, unsigned long dwCrcSeed)
 {
-    unsigned long dwCrc;
+	unsigned long dwCrc;
 
-    dwCrc = dwCrcSeed;
-    while (cbByte--) {
-        dwCrc = s_adwCrc32Table[(unsigned char)((dwCrc ^ (*pbyData)) & 0xFF)] ^ (dwCrc >> 8);
-        pbyData++;
-    }
+	dwCrc = dwCrcSeed;
+	while (cbByte--) {
+		dwCrc = s_adwCrc32Table[(unsigned char)((dwCrc ^ (*pbyData)) & 0xFF)] ^ (dwCrc >> 8);
+		pbyData++;
+	}
 
-    return dwCrc;
+	return dwCrc;
 }
 
-
 /*+
  *
  * Description:
@@ -163,13 +159,12 @@ unsigned long CRCdwCrc32 (unsigned char *pbyData, unsigned int cbByte, unsigned
  *
  * Return Value: CRC-32
  *
--*/
-unsigned long CRCdwGetCrc32 (unsigned char *pbyData, unsigned int cbByte)
+ -*/
+unsigned long CRCdwGetCrc32(unsigned char *pbyData, unsigned int cbByte)
 {
-    return ~CRCdwCrc32(pbyData, cbByte, 0xFFFFFFFFL);
+	return ~CRCdwCrc32(pbyData, cbByte, 0xFFFFFFFFL);
 }
 
-
 /*+
  *
  * Description:
@@ -189,10 +184,8 @@ unsigned long CRCdwGetCrc32 (unsigned char *pbyData, unsigned int cbByte)
  *
  * Return Value: CRC-32
  *
--*/
+ -*/
 unsigned long CRCdwGetCrc32Ex(unsigned char *pbyData, unsigned int cbByte, unsigned long dwPreCRC)
 {
-    return CRCdwCrc32(pbyData, cbByte, dwPreCRC);
+	return CRCdwCrc32(pbyData, cbByte, dwPreCRC);
 }
-
-
diff --git a/drivers/staging/vt6655/tcrc.h b/drivers/staging/vt6655/tcrc.h
index a2044212d1c..82b5ddafae5 100644
--- a/drivers/staging/vt6655/tcrc.h
+++ b/drivers/staging/vt6655/tcrc.h
@@ -48,6 +48,3 @@ unsigned long CRCdwGetCrc32(unsigned char *pbyData, unsigned int cbByte);
 unsigned long CRCdwGetCrc32Ex(unsigned char *pbyData, unsigned int cbByte, unsigned long dwPreCRC);
 
 #endif // __TCRC_H__
-
-
-
diff --git a/drivers/staging/vt6655/tether.c b/drivers/staging/vt6655/tether.c
index 28554bf7554..a5dc3c09cfd 100644
--- a/drivers/staging/vt6655/tether.c
+++ b/drivers/staging/vt6655/tether.c
@@ -47,8 +47,6 @@
 
 /*---------------------  Export Variables  --------------------------*/
 
-
-
 /*
  * Description: Calculate multicast hash value by CRC32
  *
@@ -61,28 +59,27 @@
  * Return Value: Hash value
  *
  */
-unsigned char ETHbyGetHashIndexByCrc32 (unsigned char *pbyMultiAddr)
+unsigned char ETHbyGetHashIndexByCrc32(unsigned char *pbyMultiAddr)
 {
-    int     ii;
-    unsigned char byTmpHash;
-    unsigned char byHash = 0;
+	int     ii;
+	unsigned char byTmpHash;
+	unsigned char byHash = 0;
 
-    // get the least 6-bits from CRC generator
-    byTmpHash = (unsigned char)(CRCdwCrc32(pbyMultiAddr, ETH_ALEN,
-            0xFFFFFFFFL) & 0x3F);
-    // reverse most bit to least bit
-    for (ii = 0; ii < (sizeof(byTmpHash) * 8); ii++) {
-        byHash <<= 1;
-        if (byTmpHash & 0x01)
-            byHash |= 1;
-        byTmpHash >>= 1;
-    }
+	// get the least 6-bits from CRC generator
+	byTmpHash = (unsigned char)(CRCdwCrc32(pbyMultiAddr, ETH_ALEN,
+					       0xFFFFFFFFL) & 0x3F);
+	// reverse most bit to least bit
+	for (ii = 0; ii < (sizeof(byTmpHash) * 8); ii++) {
+		byHash <<= 1;
+		if (byTmpHash & 0x01)
+			byHash |= 1;
+		byTmpHash >>= 1;
+	}
 
-    // adjust 6-bits to the right most
-    return (byHash >> 2);
+	// adjust 6-bits to the right most
+	return byHash >> 2;
 }
 
-
 /*
  * Description: Check CRC value of the buffer if Ok or not
  *
@@ -96,14 +93,13 @@ unsigned char ETHbyGetHashIndexByCrc32 (unsigned char *pbyMultiAddr)
  * Return Value: true if ok; false if error.
  *
  */
-bool ETHbIsBufferCrc32Ok (unsigned char *pbyBuffer, unsigned int cbFrameLength)
+bool ETHbIsBufferCrc32Ok(unsigned char *pbyBuffer, unsigned int cbFrameLength)
 {
-    unsigned long dwCRC;
+	unsigned long dwCRC;
 
-    dwCRC = CRCdwGetCrc32(pbyBuffer, cbFrameLength - 4);
-    if (cpu_to_le32(*((unsigned long *)(pbyBuffer + cbFrameLength - 4))) != dwCRC) {
-        return false;
-    }
-    return true;
+	dwCRC = CRCdwGetCrc32(pbyBuffer, cbFrameLength - 4);
+	if (cpu_to_le32(*((unsigned long *)(pbyBuffer + cbFrameLength - 4))) != dwCRC) {
+		return false;
+	}
+	return true;
 }
-
diff --git a/drivers/staging/vt6655/tether.h b/drivers/staging/vt6655/tether.h
index 6a68f97d9a3..94cc8830d8c 100644
--- a/drivers/staging/vt6655/tether.h
+++ b/drivers/staging/vt6655/tether.h
@@ -37,13 +37,12 @@
 // constants
 //
 #define U_ETHER_ADDR_STR_LEN (ETH_ALEN * 2 + 1)
-                                        // Ethernet address string length
+// Ethernet address string length
 
 #define MAX_LOOKAHEAD_SIZE  ETH_FRAME_LEN
 
 #define U_MULTI_ADDR_LEN    8           // multicast address length
 
-
 #ifdef __BIG_ENDIAN
 
 #define TYPE_PKT_IP         0x0800      //
@@ -89,7 +88,6 @@
 #define TYPE_CTL_CTS        0xc400
 #define TYPE_CTL_ACK        0xd400
 
-
 #else //if LITTLE_ENDIAN
 //
 // wType field in the SEthernetHeader
@@ -140,8 +138,6 @@
 #define TYPE_CTL_CTS        0x00c4
 #define TYPE_CTL_ACK        0x00d4
 
-
-
 #endif //#ifdef __BIG_ENDIAN
 
 #define WEP_IV_MASK         0x00FFFFFF
@@ -151,35 +147,34 @@
 // Ethernet packet
 //
 typedef struct tagSEthernetHeader {
-    unsigned char abyDstAddr[ETH_ALEN];
-    unsigned char abySrcAddr[ETH_ALEN];
-    unsigned short wType;
-}__attribute__ ((__packed__))
+	unsigned char abyDstAddr[ETH_ALEN];
+	unsigned char abySrcAddr[ETH_ALEN];
+	unsigned short wType;
+} __attribute__ ((__packed__))
 SEthernetHeader, *PSEthernetHeader;
 
-
 //
 // 802_3 packet
 //
 typedef struct tagS802_3Header {
-    unsigned char abyDstAddr[ETH_ALEN];
-    unsigned char abySrcAddr[ETH_ALEN];
-    unsigned short wLen;
-}__attribute__ ((__packed__))
+	unsigned char abyDstAddr[ETH_ALEN];
+	unsigned char abySrcAddr[ETH_ALEN];
+	unsigned short wLen;
+} __attribute__ ((__packed__))
 S802_3Header, *PS802_3Header;
 
 //
 // 802_11 packet
 //
 typedef struct tagS802_11Header {
-    unsigned short wFrameCtl;
-    unsigned short wDurationID;
-    unsigned char abyAddr1[ETH_ALEN];
-    unsigned char abyAddr2[ETH_ALEN];
-    unsigned char abyAddr3[ETH_ALEN];
-    unsigned short wSeqCtl;
-    unsigned char abyAddr4[ETH_ALEN];
-}__attribute__ ((__packed__))
+	unsigned short wFrameCtl;
+	unsigned short wDurationID;
+	unsigned char abyAddr1[ETH_ALEN];
+	unsigned char abyAddr2[ETH_ALEN];
+	unsigned char abyAddr3[ETH_ALEN];
+	unsigned short wSeqCtl;
+	unsigned char abyAddr4[ETH_ALEN];
+} __attribute__ ((__packed__))
 S802_11Header, *PS802_11Header;
 
 /*---------------------  Export Macros ------------------------------*/
@@ -195,6 +190,3 @@ unsigned char ETHbyGetHashIndexByCrc32(unsigned char *pbyMultiAddr);
 bool ETHbIsBufferCrc32Ok(unsigned char *pbyBuffer, unsigned int cbFrameLength);
 
 #endif // __TETHER_H__
-
-
-
diff --git a/drivers/staging/vt6655/tkip.c b/drivers/staging/vt6655/tkip.c
index f141ba1cbb9..b3e087e1903 100644
--- a/drivers/staging/vt6655/tkip.c
+++ b/drivers/staging/vt6655/tkip.c
@@ -56,76 +56,75 @@
 /* bytes swapped. To allow an endian tolerant implementation, the byte */
 /* halves have been expressed independently here.                      */
 const unsigned char TKIP_Sbox_Lower[256] = {
-    0xA5,0x84,0x99,0x8D,0x0D,0xBD,0xB1,0x54,
-    0x50,0x03,0xA9,0x7D,0x19,0x62,0xE6,0x9A,
-    0x45,0x9D,0x40,0x87,0x15,0xEB,0xC9,0x0B,
-    0xEC,0x67,0xFD,0xEA,0xBF,0xF7,0x96,0x5B,
-    0xC2,0x1C,0xAE,0x6A,0x5A,0x41,0x02,0x4F,
-    0x5C,0xF4,0x34,0x08,0x93,0x73,0x53,0x3F,
-    0x0C,0x52,0x65,0x5E,0x28,0xA1,0x0F,0xB5,
-    0x09,0x36,0x9B,0x3D,0x26,0x69,0xCD,0x9F,
-    0x1B,0x9E,0x74,0x2E,0x2D,0xB2,0xEE,0xFB,
-    0xF6,0x4D,0x61,0xCE,0x7B,0x3E,0x71,0x97,
-    0xF5,0x68,0x00,0x2C,0x60,0x1F,0xC8,0xED,
-    0xBE,0x46,0xD9,0x4B,0xDE,0xD4,0xE8,0x4A,
-    0x6B,0x2A,0xE5,0x16,0xC5,0xD7,0x55,0x94,
-    0xCF,0x10,0x06,0x81,0xF0,0x44,0xBA,0xE3,
-    0xF3,0xFE,0xC0,0x8A,0xAD,0xBC,0x48,0x04,
-    0xDF,0xC1,0x75,0x63,0x30,0x1A,0x0E,0x6D,
-    0x4C,0x14,0x35,0x2F,0xE1,0xA2,0xCC,0x39,
-    0x57,0xF2,0x82,0x47,0xAC,0xE7,0x2B,0x95,
-    0xA0,0x98,0xD1,0x7F,0x66,0x7E,0xAB,0x83,
-    0xCA,0x29,0xD3,0x3C,0x79,0xE2,0x1D,0x76,
-    0x3B,0x56,0x4E,0x1E,0xDB,0x0A,0x6C,0xE4,
-    0x5D,0x6E,0xEF,0xA6,0xA8,0xA4,0x37,0x8B,
-    0x32,0x43,0x59,0xB7,0x8C,0x64,0xD2,0xE0,
-    0xB4,0xFA,0x07,0x25,0xAF,0x8E,0xE9,0x18,
-    0xD5,0x88,0x6F,0x72,0x24,0xF1,0xC7,0x51,
-    0x23,0x7C,0x9C,0x21,0xDD,0xDC,0x86,0x85,
-    0x90,0x42,0xC4,0xAA,0xD8,0x05,0x01,0x12,
-    0xA3,0x5F,0xF9,0xD0,0x91,0x58,0x27,0xB9,
-    0x38,0x13,0xB3,0x33,0xBB,0x70,0x89,0xA7,
-    0xB6,0x22,0x92,0x20,0x49,0xFF,0x78,0x7A,
-    0x8F,0xF8,0x80,0x17,0xDA,0x31,0xC6,0xB8,
-    0xC3,0xB0,0x77,0x11,0xCB,0xFC,0xD6,0x3A
+	0xA5, 0x84, 0x99, 0x8D, 0x0D, 0xBD, 0xB1, 0x54,
+	0x50, 0x03, 0xA9, 0x7D, 0x19, 0x62, 0xE6, 0x9A,
+	0x45, 0x9D, 0x40, 0x87, 0x15, 0xEB, 0xC9, 0x0B,
+	0xEC, 0x67, 0xFD, 0xEA, 0xBF, 0xF7, 0x96, 0x5B,
+	0xC2, 0x1C, 0xAE, 0x6A, 0x5A, 0x41, 0x02, 0x4F,
+	0x5C, 0xF4, 0x34, 0x08, 0x93, 0x73, 0x53, 0x3F,
+	0x0C, 0x52, 0x65, 0x5E, 0x28, 0xA1, 0x0F, 0xB5,
+	0x09, 0x36, 0x9B, 0x3D, 0x26, 0x69, 0xCD, 0x9F,
+	0x1B, 0x9E, 0x74, 0x2E, 0x2D, 0xB2, 0xEE, 0xFB,
+	0xF6, 0x4D, 0x61, 0xCE, 0x7B, 0x3E, 0x71, 0x97,
+	0xF5, 0x68, 0x00, 0x2C, 0x60, 0x1F, 0xC8, 0xED,
+	0xBE, 0x46, 0xD9, 0x4B, 0xDE, 0xD4, 0xE8, 0x4A,
+	0x6B, 0x2A, 0xE5, 0x16, 0xC5, 0xD7, 0x55, 0x94,
+	0xCF, 0x10, 0x06, 0x81, 0xF0, 0x44, 0xBA, 0xE3,
+	0xF3, 0xFE, 0xC0, 0x8A, 0xAD, 0xBC, 0x48, 0x04,
+	0xDF, 0xC1, 0x75, 0x63, 0x30, 0x1A, 0x0E, 0x6D,
+	0x4C, 0x14, 0x35, 0x2F, 0xE1, 0xA2, 0xCC, 0x39,
+	0x57, 0xF2, 0x82, 0x47, 0xAC, 0xE7, 0x2B, 0x95,
+	0xA0, 0x98, 0xD1, 0x7F, 0x66, 0x7E, 0xAB, 0x83,
+	0xCA, 0x29, 0xD3, 0x3C, 0x79, 0xE2, 0x1D, 0x76,
+	0x3B, 0x56, 0x4E, 0x1E, 0xDB, 0x0A, 0x6C, 0xE4,
+	0x5D, 0x6E, 0xEF, 0xA6, 0xA8, 0xA4, 0x37, 0x8B,
+	0x32, 0x43, 0x59, 0xB7, 0x8C, 0x64, 0xD2, 0xE0,
+	0xB4, 0xFA, 0x07, 0x25, 0xAF, 0x8E, 0xE9, 0x18,
+	0xD5, 0x88, 0x6F, 0x72, 0x24, 0xF1, 0xC7, 0x51,
+	0x23, 0x7C, 0x9C, 0x21, 0xDD, 0xDC, 0x86, 0x85,
+	0x90, 0x42, 0xC4, 0xAA, 0xD8, 0x05, 0x01, 0x12,
+	0xA3, 0x5F, 0xF9, 0xD0, 0x91, 0x58, 0x27, 0xB9,
+	0x38, 0x13, 0xB3, 0x33, 0xBB, 0x70, 0x89, 0xA7,
+	0xB6, 0x22, 0x92, 0x20, 0x49, 0xFF, 0x78, 0x7A,
+	0x8F, 0xF8, 0x80, 0x17, 0xDA, 0x31, 0xC6, 0xB8,
+	0xC3, 0xB0, 0x77, 0x11, 0xCB, 0xFC, 0xD6, 0x3A
 };
 
 const unsigned char TKIP_Sbox_Upper[256] = {
-    0xC6,0xF8,0xEE,0xF6,0xFF,0xD6,0xDE,0x91,
-    0x60,0x02,0xCE,0x56,0xE7,0xB5,0x4D,0xEC,
-    0x8F,0x1F,0x89,0xFA,0xEF,0xB2,0x8E,0xFB,
-    0x41,0xB3,0x5F,0x45,0x23,0x53,0xE4,0x9B,
-    0x75,0xE1,0x3D,0x4C,0x6C,0x7E,0xF5,0x83,
-    0x68,0x51,0xD1,0xF9,0xE2,0xAB,0x62,0x2A,
-    0x08,0x95,0x46,0x9D,0x30,0x37,0x0A,0x2F,
-    0x0E,0x24,0x1B,0xDF,0xCD,0x4E,0x7F,0xEA,
-    0x12,0x1D,0x58,0x34,0x36,0xDC,0xB4,0x5B,
-    0xA4,0x76,0xB7,0x7D,0x52,0xDD,0x5E,0x13,
-    0xA6,0xB9,0x00,0xC1,0x40,0xE3,0x79,0xB6,
-    0xD4,0x8D,0x67,0x72,0x94,0x98,0xB0,0x85,
-    0xBB,0xC5,0x4F,0xED,0x86,0x9A,0x66,0x11,
-    0x8A,0xE9,0x04,0xFE,0xA0,0x78,0x25,0x4B,
-    0xA2,0x5D,0x80,0x05,0x3F,0x21,0x70,0xF1,
-    0x63,0x77,0xAF,0x42,0x20,0xE5,0xFD,0xBF,
-    0x81,0x18,0x26,0xC3,0xBE,0x35,0x88,0x2E,
-    0x93,0x55,0xFC,0x7A,0xC8,0xBA,0x32,0xE6,
-    0xC0,0x19,0x9E,0xA3,0x44,0x54,0x3B,0x0B,
-    0x8C,0xC7,0x6B,0x28,0xA7,0xBC,0x16,0xAD,
-    0xDB,0x64,0x74,0x14,0x92,0x0C,0x48,0xB8,
-    0x9F,0xBD,0x43,0xC4,0x39,0x31,0xD3,0xF2,
-    0xD5,0x8B,0x6E,0xDA,0x01,0xB1,0x9C,0x49,
-    0xD8,0xAC,0xF3,0xCF,0xCA,0xF4,0x47,0x10,
-    0x6F,0xF0,0x4A,0x5C,0x38,0x57,0x73,0x97,
-    0xCB,0xA1,0xE8,0x3E,0x96,0x61,0x0D,0x0F,
-    0xE0,0x7C,0x71,0xCC,0x90,0x06,0xF7,0x1C,
-    0xC2,0x6A,0xAE,0x69,0x17,0x99,0x3A,0x27,
-    0xD9,0xEB,0x2B,0x22,0xD2,0xA9,0x07,0x33,
-    0x2D,0x3C,0x15,0xC9,0x87,0xAA,0x50,0xA5,
-    0x03,0x59,0x09,0x1A,0x65,0xD7,0x84,0xD0,
-    0x82,0x29,0x5A,0x1E,0x7B,0xA8,0x6D,0x2C
+	0xC6, 0xF8, 0xEE, 0xF6, 0xFF, 0xD6, 0xDE, 0x91,
+	0x60, 0x02, 0xCE, 0x56, 0xE7, 0xB5, 0x4D, 0xEC,
+	0x8F, 0x1F, 0x89, 0xFA, 0xEF, 0xB2, 0x8E, 0xFB,
+	0x41, 0xB3, 0x5F, 0x45, 0x23, 0x53, 0xE4, 0x9B,
+	0x75, 0xE1, 0x3D, 0x4C, 0x6C, 0x7E, 0xF5, 0x83,
+	0x68, 0x51, 0xD1, 0xF9, 0xE2, 0xAB, 0x62, 0x2A,
+	0x08, 0x95, 0x46, 0x9D, 0x30, 0x37, 0x0A, 0x2F,
+	0x0E, 0x24, 0x1B, 0xDF, 0xCD, 0x4E, 0x7F, 0xEA,
+	0x12, 0x1D, 0x58, 0x34, 0x36, 0xDC, 0xB4, 0x5B,
+	0xA4, 0x76, 0xB7, 0x7D, 0x52, 0xDD, 0x5E, 0x13,
+	0xA6, 0xB9, 0x00, 0xC1, 0x40, 0xE3, 0x79, 0xB6,
+	0xD4, 0x8D, 0x67, 0x72, 0x94, 0x98, 0xB0, 0x85,
+	0xBB, 0xC5, 0x4F, 0xED, 0x86, 0x9A, 0x66, 0x11,
+	0x8A, 0xE9, 0x04, 0xFE, 0xA0, 0x78, 0x25, 0x4B,
+	0xA2, 0x5D, 0x80, 0x05, 0x3F, 0x21, 0x70, 0xF1,
+	0x63, 0x77, 0xAF, 0x42, 0x20, 0xE5, 0xFD, 0xBF,
+	0x81, 0x18, 0x26, 0xC3, 0xBE, 0x35, 0x88, 0x2E,
+	0x93, 0x55, 0xFC, 0x7A, 0xC8, 0xBA, 0x32, 0xE6,
+	0xC0, 0x19, 0x9E, 0xA3, 0x44, 0x54, 0x3B, 0x0B,
+	0x8C, 0xC7, 0x6B, 0x28, 0xA7, 0xBC, 0x16, 0xAD,
+	0xDB, 0x64, 0x74, 0x14, 0x92, 0x0C, 0x48, 0xB8,
+	0x9F, 0xBD, 0x43, 0xC4, 0x39, 0x31, 0xD3, 0xF2,
+	0xD5, 0x8B, 0x6E, 0xDA, 0x01, 0xB1, 0x9C, 0x49,
+	0xD8, 0xAC, 0xF3, 0xCF, 0xCA, 0xF4, 0x47, 0x10,
+	0x6F, 0xF0, 0x4A, 0x5C, 0x38, 0x57, 0x73, 0x97,
+	0xCB, 0xA1, 0xE8, 0x3E, 0x96, 0x61, 0x0D, 0x0F,
+	0xE0, 0x7C, 0x71, 0xCC, 0x90, 0x06, 0xF7, 0x1C,
+	0xC2, 0x6A, 0xAE, 0x69, 0x17, 0x99, 0x3A, 0x27,
+	0xD9, 0xEB, 0x2B, 0x22, 0xD2, 0xA9, 0x07, 0x33,
+	0x2D, 0x3C, 0x15, 0xC9, 0x87, 0xAA, 0x50, 0xA5,
+	0x03, 0x59, 0x09, 0x1A, 0x65, 0xD7, 0x84, 0xD0,
+	0x82, 0x29, 0x5A, 0x1E, 0x7B, 0xA8, 0x6D, 0x2C
 };
 
-
 //STKIPKeyManagement  sTKIPKeyTable[MAX_TKIP_KEY];
 
 /*---------------------  Static Functions  --------------------------*/
@@ -141,34 +140,32 @@ unsigned int rotr1(unsigned int a);
 /************************************************************/
 unsigned int tkip_sbox(unsigned int index)
 {
-    unsigned int index_low;
-    unsigned int index_high;
-    unsigned int left, right;
+	unsigned int index_low;
+	unsigned int index_high;
+	unsigned int left, right;
 
-    index_low = (index % 256);
-    index_high = ((index >> 8) % 256);
+	index_low = (index % 256);
+	index_high = ((index >> 8) % 256);
 
-    left = TKIP_Sbox_Lower[index_low] + (TKIP_Sbox_Upper[index_low] * 256);
-    right = TKIP_Sbox_Upper[index_high] + (TKIP_Sbox_Lower[index_high] * 256);
+	left = TKIP_Sbox_Lower[index_low] + (TKIP_Sbox_Upper[index_low] * 256);
+	right = TKIP_Sbox_Upper[index_high] + (TKIP_Sbox_Lower[index_high] * 256);
 
-    return (left ^ right);
+	return left ^ right;
 };
 
-
 unsigned int rotr1(unsigned int a)
 {
-    unsigned int b;
-
-    if ((a & 0x01) == 0x01) {
-        b = (a >> 1) | 0x8000;
-    } else {
-        b = (a >> 1) & 0x7fff;
-    }
-    b = b % 65536;
-    return b;
+	unsigned int b;
+
+	if ((a & 0x01) == 0x01) {
+		b = (a >> 1) | 0x8000;
+	} else {
+		b = (a >> 1) & 0x7fff;
+	}
+	b = b % 65536;
+	return b;
 }
 
-
 /*
  * Description: Calculate RC4Key fom TK, TA, and TSC
  *
@@ -184,89 +181,89 @@ unsigned int rotr1(unsigned int a)
  *
  */
 void TKIPvMixKey(
-    unsigned char *pbyTKey,
-    unsigned char *pbyTA,
-    unsigned short wTSC15_0,
-    unsigned long dwTSC47_16,
-    unsigned char *pbyRC4Key
-    )
+	unsigned char *pbyTKey,
+	unsigned char *pbyTA,
+	unsigned short wTSC15_0,
+	unsigned long dwTSC47_16,
+	unsigned char *pbyRC4Key
+)
 {
-    unsigned int p1k[5];
+	unsigned int p1k[5];
 //    unsigned int ttak0, ttak1, ttak2, ttak3, ttak4;
-    unsigned int tsc0, tsc1, tsc2;
-    unsigned int ppk0, ppk1, ppk2, ppk3, ppk4, ppk5;
-    unsigned long int pnl,pnh;
-
-    int i, j;
-
-    pnl = wTSC15_0;
-    pnh = dwTSC47_16;
-
-    tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
-    tsc1 = (unsigned int)(pnh % 65536);
-    tsc2 = (unsigned int)(pnl % 65536); /* lsb */
-
-    /* Phase 1, step 1 */
-    p1k[0] = tsc1;
-    p1k[1] = tsc0;
-    p1k[2] = (unsigned int)(pbyTA[0] + (pbyTA[1]*256));
-    p1k[3] = (unsigned int)(pbyTA[2] + (pbyTA[3]*256));
-    p1k[4] = (unsigned int)(pbyTA[4] + (pbyTA[5]*256));
-
-    /* Phase 1, step 2 */
-    for (i=0; i<8; i++) {
-        j = 2*(i & 1);
-        p1k[0] = (p1k[0] + tkip_sbox( (p1k[4] ^ ((256*pbyTKey[1+j]) + pbyTKey[j])) % 65536 )) % 65536;
-        p1k[1] = (p1k[1] + tkip_sbox( (p1k[0] ^ ((256*pbyTKey[5+j]) + pbyTKey[4+j])) % 65536 )) % 65536;
-        p1k[2] = (p1k[2] + tkip_sbox( (p1k[1] ^ ((256*pbyTKey[9+j]) + pbyTKey[8+j])) % 65536 )) % 65536;
-        p1k[3] = (p1k[3] + tkip_sbox( (p1k[2] ^ ((256*pbyTKey[13+j]) + pbyTKey[12+j])) % 65536 )) % 65536;
-        p1k[4] = (p1k[4] + tkip_sbox( (p1k[3] ^ (((256*pbyTKey[1+j]) + pbyTKey[j]))) % 65536 )) % 65536;
-        p1k[4] = (p1k[4] + i) % 65536;
-    }
-    /* Phase 2, Step 1 */
-    ppk0 = p1k[0];
-    ppk1 = p1k[1];
-    ppk2 = p1k[2];
-    ppk3 = p1k[3];
-    ppk4 = p1k[4];
-    ppk5 = (p1k[4] + tsc2) % 65536;
-
-    /* Phase2, Step 2 */
-    ppk0 = ppk0 + tkip_sbox( (ppk5 ^ ((256*pbyTKey[1]) + pbyTKey[0])) % 65536);
-    ppk1 = ppk1 + tkip_sbox( (ppk0 ^ ((256*pbyTKey[3]) + pbyTKey[2])) % 65536);
-    ppk2 = ppk2 + tkip_sbox( (ppk1 ^ ((256*pbyTKey[5]) + pbyTKey[4])) % 65536);
-    ppk3 = ppk3 + tkip_sbox( (ppk2 ^ ((256*pbyTKey[7]) + pbyTKey[6])) % 65536);
-    ppk4 = ppk4 + tkip_sbox( (ppk3 ^ ((256*pbyTKey[9]) + pbyTKey[8])) % 65536);
-    ppk5 = ppk5 + tkip_sbox( (ppk4 ^ ((256*pbyTKey[11]) + pbyTKey[10])) % 65536);
-
-    ppk0 = ppk0 + rotr1(ppk5 ^ ((256*pbyTKey[13]) + pbyTKey[12]));
-    ppk1 = ppk1 + rotr1(ppk0 ^ ((256*pbyTKey[15]) + pbyTKey[14]));
-    ppk2 = ppk2 + rotr1(ppk1);
-    ppk3 = ppk3 + rotr1(ppk2);
-    ppk4 = ppk4 + rotr1(ppk3);
-    ppk5 = ppk5 + rotr1(ppk4);
-
-    /* Phase 2, Step 3 */
-    pbyRC4Key[0] = (tsc2 >> 8) % 256;
-    pbyRC4Key[1] = (((tsc2 >> 8) % 256) | 0x20) & 0x7f;
-    pbyRC4Key[2] = tsc2 % 256;
-    pbyRC4Key[3] = ((ppk5 ^ ((256*pbyTKey[1]) + pbyTKey[0])) >> 1) % 256;
-
-    pbyRC4Key[4] = ppk0 % 256;
-    pbyRC4Key[5] = (ppk0 >> 8) % 256;
-
-    pbyRC4Key[6] = ppk1 % 256;
-    pbyRC4Key[7] = (ppk1 >> 8) % 256;
-
-    pbyRC4Key[8] = ppk2 % 256;
-    pbyRC4Key[9] = (ppk2 >> 8) % 256;
-
-    pbyRC4Key[10] = ppk3 % 256;
-    pbyRC4Key[11] = (ppk3 >> 8) % 256;
-
-    pbyRC4Key[12] = ppk4 % 256;
-    pbyRC4Key[13] = (ppk4 >> 8) % 256;
-
-    pbyRC4Key[14] = ppk5 % 256;
-    pbyRC4Key[15] = (ppk5 >> 8) % 256;
+	unsigned int tsc0, tsc1, tsc2;
+	unsigned int ppk0, ppk1, ppk2, ppk3, ppk4, ppk5;
+	unsigned long int pnl, pnh;
+
+	int i, j;
+
+	pnl = wTSC15_0;
+	pnh = dwTSC47_16;
+
+	tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
+	tsc1 = (unsigned int)(pnh % 65536);
+	tsc2 = (unsigned int)(pnl % 65536); /* lsb */
+
+	/* Phase 1, step 1 */
+	p1k[0] = tsc1;
+	p1k[1] = tsc0;
+	p1k[2] = (unsigned int)(pbyTA[0] + (pbyTA[1]*256));
+	p1k[3] = (unsigned int)(pbyTA[2] + (pbyTA[3]*256));
+	p1k[4] = (unsigned int)(pbyTA[4] + (pbyTA[5]*256));
+
+	/* Phase 1, step 2 */
+	for (i = 0; i < 8; i++) {
+		j = 2 * (i & 1);
+		p1k[0] = (p1k[0] + tkip_sbox((p1k[4] ^ ((256*pbyTKey[1+j]) + pbyTKey[j])) % 65536)) % 65536;
+		p1k[1] = (p1k[1] + tkip_sbox((p1k[0] ^ ((256*pbyTKey[5+j]) + pbyTKey[4+j])) % 65536)) % 65536;
+		p1k[2] = (p1k[2] + tkip_sbox((p1k[1] ^ ((256*pbyTKey[9+j]) + pbyTKey[8+j])) % 65536)) % 65536;
+		p1k[3] = (p1k[3] + tkip_sbox((p1k[2] ^ ((256*pbyTKey[13+j]) + pbyTKey[12+j])) % 65536)) % 65536;
+		p1k[4] = (p1k[4] + tkip_sbox((p1k[3] ^ (((256*pbyTKey[1+j]) + pbyTKey[j]))) % 65536)) % 65536;
+		p1k[4] = (p1k[4] + i) % 65536;
+	}
+	/* Phase 2, Step 1 */
+	ppk0 = p1k[0];
+	ppk1 = p1k[1];
+	ppk2 = p1k[2];
+	ppk3 = p1k[3];
+	ppk4 = p1k[4];
+	ppk5 = (p1k[4] + tsc2) % 65536;
+
+	/* Phase2, Step 2 */
+	ppk0 = ppk0 + tkip_sbox((ppk5 ^ ((256*pbyTKey[1]) + pbyTKey[0])) % 65536);
+	ppk1 = ppk1 + tkip_sbox((ppk0 ^ ((256*pbyTKey[3]) + pbyTKey[2])) % 65536);
+	ppk2 = ppk2 + tkip_sbox((ppk1 ^ ((256*pbyTKey[5]) + pbyTKey[4])) % 65536);
+	ppk3 = ppk3 + tkip_sbox((ppk2 ^ ((256*pbyTKey[7]) + pbyTKey[6])) % 65536);
+	ppk4 = ppk4 + tkip_sbox((ppk3 ^ ((256*pbyTKey[9]) + pbyTKey[8])) % 65536);
+	ppk5 = ppk5 + tkip_sbox((ppk4 ^ ((256*pbyTKey[11]) + pbyTKey[10])) % 65536);
+
+	ppk0 = ppk0 + rotr1(ppk5 ^ ((256*pbyTKey[13]) + pbyTKey[12]));
+	ppk1 = ppk1 + rotr1(ppk0 ^ ((256*pbyTKey[15]) + pbyTKey[14]));
+	ppk2 = ppk2 + rotr1(ppk1);
+	ppk3 = ppk3 + rotr1(ppk2);
+	ppk4 = ppk4 + rotr1(ppk3);
+	ppk5 = ppk5 + rotr1(ppk4);
+
+	/* Phase 2, Step 3 */
+	pbyRC4Key[0] = (tsc2 >> 8) % 256;
+	pbyRC4Key[1] = (((tsc2 >> 8) % 256) | 0x20) & 0x7f;
+	pbyRC4Key[2] = tsc2 % 256;
+	pbyRC4Key[3] = ((ppk5 ^ ((256*pbyTKey[1]) + pbyTKey[0])) >> 1) % 256;
+
+	pbyRC4Key[4] = ppk0 % 256;
+	pbyRC4Key[5] = (ppk0 >> 8) % 256;
+
+	pbyRC4Key[6] = ppk1 % 256;
+	pbyRC4Key[7] = (ppk1 >> 8) % 256;
+
+	pbyRC4Key[8] = ppk2 % 256;
+	pbyRC4Key[9] = (ppk2 >> 8) % 256;
+
+	pbyRC4Key[10] = ppk3 % 256;
+	pbyRC4Key[11] = (ppk3 >> 8) % 256;
+
+	pbyRC4Key[12] = ppk4 % 256;
+	pbyRC4Key[13] = (ppk4 >> 8) % 256;
+
+	pbyRC4Key[14] = ppk5 % 256;
+	pbyRC4Key[15] = (ppk5 >> 8) % 256;
 }
diff --git a/drivers/staging/vt6655/tkip.h b/drivers/staging/vt6655/tkip.h
index eb5951d726e..3b6357ac6de 100644
--- a/drivers/staging/vt6655/tkip.h
+++ b/drivers/staging/vt6655/tkip.h
@@ -47,14 +47,11 @@
 /*---------------------  Export Functions  --------------------------*/
 
 void TKIPvMixKey(
-    unsigned char *pbyTKey,
-    unsigned char *pbyTA,
-    unsigned short wTSC15_0,
-    unsigned long dwTSC47_16,
-    unsigned char *pbyRC4Key
-    );
+	unsigned char *pbyTKey,
+	unsigned char *pbyTA,
+	unsigned short wTSC15_0,
+	unsigned long dwTSC47_16,
+	unsigned char *pbyRC4Key
+);
 
 #endif // __TKIP_H__
-
-
-
diff --git a/drivers/staging/vt6655/tmacro.h b/drivers/staging/vt6655/tmacro.h
index e8b177d4128..59c6e72f993 100644
--- a/drivers/staging/vt6655/tmacro.h
+++ b/drivers/staging/vt6655/tmacro.h
@@ -58,5 +58,3 @@
 #endif
 
 #endif // __TMACRO_H__
-
-
diff --git a/drivers/staging/vt6655/ttype.h b/drivers/staging/vt6655/ttype.h
index be223bd25d2..b1d8ae73277 100644
--- a/drivers/staging/vt6655/ttype.h
+++ b/drivers/staging/vt6655/ttype.h
@@ -26,11 +26,9 @@
  *
  */
 
-
 #ifndef __TTYPE_H__
 #define __TTYPE_H__
 
-
 /******* Common definitions and typedefs ***********************************/
 
 #ifndef TxInSleep
@@ -56,16 +54,16 @@
 // an 8-byte-aligned 8 byte long structure
 // which is NOT really a floating point number.
 typedef union tagUQuadWord {
-    struct {
-        unsigned int dwLowDword;
-        unsigned int dwHighDword;
-    } u;
-    double      DoNotUseThisField;
+	struct {
+		unsigned int dwLowDword;
+		unsigned int dwHighDword;
+	} u;
+	double      DoNotUseThisField;
 } UQuadWord;
 typedef UQuadWord       QWORD;          // 64-bit
 
 /****** Common pointer types ***********************************************/
 
-typedef QWORD *          PQWORD;
+typedef QWORD *PQWORD;
 
 #endif // __TTYPE_H__
diff --git a/drivers/staging/vt6655/upc.h b/drivers/staging/vt6655/upc.h
index 9596fdef0e3..155e6643907 100644
--- a/drivers/staging/vt6655/upc.h
+++ b/drivers/staging/vt6655/upc.h
@@ -34,39 +34,35 @@
 
 /*---------------------  Export Definitions -------------------------*/
 
-
 //
 //  For IO mapped
 //
 
 #ifdef IO_MAP
 
-#define VNSvInPortB(dwIOAddress, pbyData) {                     \
-	*(pbyData) = inb(dwIOAddress);                              \
-}
-
-
-#define VNSvInPortW(dwIOAddress, pwData) {                      \
-	    *(pwData) = inw(dwIOAddress);                           \
-}
-
-#define VNSvInPortD(dwIOAddress, pdwData) {                     \
-	    *(pdwData) = inl(dwIOAddress);                          \
-}
+#define VNSvInPortB(dwIOAddress, pbyData)	\
+do {						\
+	*(pbyData) = inb(dwIOAddress);		\
+} while (0)
 
+#define VNSvInPortW(dwIOAddress, pwData)	\
+do {						\
+	*(pwData) = inw(dwIOAddress);		\
+} while (0)
 
-#define VNSvOutPortB(dwIOAddress, byData) {                     \
-        outb(byData, dwIOAddress);                              \
-}
+#define VNSvInPortD(dwIOAddress, pdwData)	\
+do {						\
+	*(pdwData) = inl(dwIOAddress);		\
+} while (0)
 
+#define VNSvOutPortB(dwIOAddress, byData)	\
+	outb(byData, dwIOAddress)
 
-#define VNSvOutPortW(dwIOAddress, wData) {                      \
-        outw(wData, dwIOAddress);                               \
-}
+#define VNSvOutPortW(dwIOAddress, wData)	\
+	outw(wData, dwIOAddress)
 
-#define VNSvOutPortD(dwIOAddress, dwData) {                     \
-        outl(dwData, dwIOAddress);                              \
-}
+#define VNSvOutPortD(dwIOAddress, dwData)	\
+	outl(dwData, dwIOAddress)
 
 #else
 
@@ -74,84 +70,83 @@
 //  For memory mapped IO
 //
 
-
-#define VNSvInPortB(dwIOAddress, pbyData) {                     \
-	volatile unsigned char * pbyAddr = ((unsigned char *)(dwIOAddress));            \
-	*(pbyData) = readb(pbyAddr);                           \
-}
-
-
-#define VNSvInPortW(dwIOAddress, pwData) {                      \
-	volatile unsigned short *pwAddr = ((unsigned short *)(dwIOAddress));             \
-	*(pwData) = readw(pwAddr);                             \
-}
-
-#define VNSvInPortD(dwIOAddress, pdwData) {                     \
-	volatile unsigned long *pdwAddr = ((unsigned long *)(dwIOAddress));          \
-	*(pdwData) = readl(pdwAddr);                           \
-}
-
-
-#define VNSvOutPortB(dwIOAddress, byData) {                     \
-    volatile unsigned char * pbyAddr = ((unsigned char *)(dwIOAddress));            \
-    writeb((unsigned char)byData, pbyAddr);							\
-}
-
-
-#define VNSvOutPortW(dwIOAddress, wData) {                      \
-    volatile unsigned short *pwAddr = ((unsigned short *)(dwIOAddress));             \
-    writew((unsigned short)wData, pwAddr);							\
-}
-
-#define VNSvOutPortD(dwIOAddress, dwData) {                     \
-    volatile unsigned long *pdwAddr = ((unsigned long *)(dwIOAddress));          \
-    writel((unsigned long)dwData, pdwAddr);					    \
-}
+#define VNSvInPortB(dwIOAddress, pbyData)				\
+do {									\
+	volatile unsigned char *pbyAddr = (unsigned char *)(dwIOAddress); \
+	*(pbyData) = readb(pbyAddr);					\
+} while (0)
+
+#define VNSvInPortW(dwIOAddress, pwData)				\
+do {									\
+	volatile unsigned short *pwAddr = (unsigned short *)(dwIOAddress); \
+	*(pwData) = readw(pwAddr);					\
+} while (0)
+
+#define VNSvInPortD(dwIOAddress, pdwData)				\
+do {									\
+	volatile unsigned long *pdwAddr = (unsigned long *)(dwIOAddress); \
+	*(pdwData) = readl(pdwAddr);					\
+} while (0)
+
+#define VNSvOutPortB(dwIOAddress, byData)				\
+do {									\
+	volatile unsigned char *pbyAddr = (unsigned char *)(dwIOAddress); \
+	writeb((unsigned char)byData, pbyAddr);				\
+} while (0)
+
+#define VNSvOutPortW(dwIOAddress, wData)				\
+do {									\
+	volatile unsigned short *pwAddr = ((unsigned short *)(dwIOAddress)); \
+	writew((unsigned short)wData, pwAddr);				\
+} while (0)
+
+#define VNSvOutPortD(dwIOAddress, dwData)				\
+do {									\
+	volatile unsigned long *pdwAddr = (unsigned long *)(dwIOAddress); \
+	writel((unsigned long)dwData, pdwAddr);				\
+} while (0)
 
 #endif
 
-
 //
 // ALWAYS IO-Mapped IO when in 16-bit/32-bit environment
 //
-#define PCBvInPortB(dwIOAddress, pbyData) {     \
-	    *(pbyData) = inb(dwIOAddress);          \
-}
-
-#define PCBvInPortW(dwIOAddress, pwData) {      \
-	    *(pwData) = inw(dwIOAddress);           \
-}
-
-#define PCBvInPortD(dwIOAddress, pdwData) {     \
-	    *(pdwData) = inl(dwIOAddress);          \
-}
-
-#define PCBvOutPortB(dwIOAddress, byData) {     \
-        outb(byData, dwIOAddress);              \
-}
-
-#define PCBvOutPortW(dwIOAddress, wData) {      \
-        outw(wData, dwIOAddress);               \
-}
-
-#define PCBvOutPortD(dwIOAddress, dwData) {     \
-        outl(dwData, dwIOAddress);              \
-}
-
-
-#define PCAvDelayByIO(uDelayUnit) {             \
-    unsigned char byData;                       \
-    unsigned long ii;                           \
-                                                \
-    if (uDelayUnit <= 50) {                     \
-        udelay(uDelayUnit);                     \
-    }                                           \
-    else {                                      \
-        for (ii = 0; ii < (uDelayUnit); ii++)   \
-		     byData = inb(0x61);				\
-    }                                           \
-}
-
+#define PCBvInPortB(dwIOAddress, pbyData)	\
+do {						\
+	*(pbyData) = inb(dwIOAddress);		\
+} while (0)
+
+#define PCBvInPortW(dwIOAddress, pwData)	\
+do {						\
+	*(pwData) = inw(dwIOAddress);		\
+} while (0)
+
+#define PCBvInPortD(dwIOAddress, pdwData)	\
+do {						\
+	*(pdwData) = inl(dwIOAddress);		\
+} while (0)
+
+#define PCBvOutPortB(dwIOAddress, byData)	\
+	outb(byData, dwIOAddress)
+
+#define PCBvOutPortW(dwIOAddress, wData)	\
+	outw(wData, dwIOAddress)
+
+#define PCBvOutPortD(dwIOAddress, dwData)	\
+	outl(dwData, dwIOAddress)
+
+#define PCAvDelayByIO(uDelayUnit)				\
+do {								\
+	unsigned char byData;					\
+	unsigned long ii;					\
+								\
+	if (uDelayUnit <= 50) {					\
+		udelay(uDelayUnit);				\
+	} else {						\
+		for (ii = 0; ii < (uDelayUnit); ii++)		\
+			byData = inb(0x61);			\
+	}							\
+} while (0)
 
 /*---------------------  Export Classes  ----------------------------*/
 
@@ -159,8 +154,4 @@
 
 /*---------------------  Export Functions  --------------------------*/
 
-
-
-
 #endif // __UPC_H__
-
diff --git a/drivers/staging/vt6655/vntwifi.c b/drivers/staging/vt6655/vntwifi.c
index 62c44b87d31..d8f4f8e7d05 100644
--- a/drivers/staging/vt6655/vntwifi.c
+++ b/drivers/staging/vt6655/vntwifi.c
@@ -66,19 +66,18 @@
  *
  * Return Value: none
  *
--*/
+ -*/
 void
-VNTWIFIvSetOPMode (
-    void *pMgmtHandle,
-    WMAC_CONFIG_MODE eOPMode
-    )
+VNTWIFIvSetOPMode(
+	void *pMgmtHandle,
+	WMAC_CONFIG_MODE eOPMode
+)
 {
-    PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
+	PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
 
-    pMgmt->eConfigMode = eOPMode;
+	pMgmt->eConfigMode = eOPMode;
 }
 
-
 /*+
  *
  * Description:
@@ -95,20 +94,20 @@ VNTWIFIvSetOPMode (
  *
  * Return Value: none
  *
--*/
+ -*/
 void
-VNTWIFIvSetIBSSParameter (
-    void *pMgmtHandle,
-    unsigned short wBeaconPeriod,
-    unsigned short wATIMWindow,
-    unsigned int uChannel
-    )
+VNTWIFIvSetIBSSParameter(
+	void *pMgmtHandle,
+	unsigned short wBeaconPeriod,
+	unsigned short wATIMWindow,
+	unsigned int uChannel
+)
 {
-    PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
+	PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
 
-    pMgmt->wIBSSBeaconPeriod = wBeaconPeriod;
-    pMgmt->wIBSSATIMWindow = wATIMWindow;
-    pMgmt->uIBSSChannel = uChannel;
+	pMgmt->wIBSSBeaconPeriod = wBeaconPeriod;
+	pMgmt->wIBSSATIMWindow = wATIMWindow;
+	pMgmt->uIBSSChannel = uChannel;
 }
 
 /*+
@@ -124,14 +123,14 @@ VNTWIFIvSetIBSSParameter (
  *
  * Return Value: current SSID pointer.
  *
--*/
+ -*/
 PWLAN_IE_SSID
-VNTWIFIpGetCurrentSSID (
-    void *pMgmtHandle
-    )
+VNTWIFIpGetCurrentSSID(
+	void *pMgmtHandle
+)
 {
-    PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
-    return((PWLAN_IE_SSID) pMgmt->abyCurrSSID);
+	PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
+	return (PWLAN_IE_SSID) pMgmt->abyCurrSSID;
 }
 
 /*+
@@ -147,17 +146,17 @@ VNTWIFIpGetCurrentSSID (
  *
  * Return Value: current Channel.
  *
--*/
+ -*/
 unsigned int
-VNTWIFIpGetCurrentChannel (
-    void *pMgmtHandle
-    )
+VNTWIFIpGetCurrentChannel(
+	void *pMgmtHandle
+)
 {
-    PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
-    if (pMgmtHandle != NULL) {
-        return (pMgmt->uCurrChannel);
-    }
-    return 0;
+	PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
+	if (pMgmtHandle != NULL) {
+		return pMgmt->uCurrChannel;
+	}
+	return 0;
 }
 
 /*+
@@ -173,18 +172,16 @@ VNTWIFIpGetCurrentChannel (
  *
  * Return Value: current Assoc ID
  *
--*/
+ -*/
 unsigned short
-VNTWIFIwGetAssocID (
-    void *pMgmtHandle
-    )
+VNTWIFIwGetAssocID(
+	void *pMgmtHandle
+)
 {
-    PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
-    return(pMgmt->wCurrAID);
+	PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
+	return pMgmt->wCurrAID;
 }
 
-
-
 /*+
  *
  * Description:
@@ -199,35 +196,35 @@ VNTWIFIwGetAssocID (
  *
  * Return Value: max support rate
  *
--*/
+ -*/
 unsigned char
-VNTWIFIbyGetMaxSupportRate (
-    PWLAN_IE_SUPP_RATES pSupportRateIEs,
-    PWLAN_IE_SUPP_RATES pExtSupportRateIEs
-    )
+VNTWIFIbyGetMaxSupportRate(
+	PWLAN_IE_SUPP_RATES pSupportRateIEs,
+	PWLAN_IE_SUPP_RATES pExtSupportRateIEs
+)
 {
-    unsigned char byMaxSupportRate = RATE_1M;
-    unsigned char bySupportRate = RATE_1M;
-    unsigned int ii = 0;
-
-    if (pSupportRateIEs) {
-        for (ii = 0; ii < pSupportRateIEs->len; ii++) {
-            bySupportRate = DATARATEbyGetRateIdx(pSupportRateIEs->abyRates[ii]);
-            if (bySupportRate > byMaxSupportRate) {
-                byMaxSupportRate = bySupportRate;
-            }
-        }
-    }
-    if (pExtSupportRateIEs) {
-        for (ii = 0; ii < pExtSupportRateIEs->len; ii++) {
-            bySupportRate = DATARATEbyGetRateIdx(pExtSupportRateIEs->abyRates[ii]);
-            if (bySupportRate > byMaxSupportRate) {
-                byMaxSupportRate = bySupportRate;
-            }
-        }
-    }
-
-    return byMaxSupportRate;
+	unsigned char byMaxSupportRate = RATE_1M;
+	unsigned char bySupportRate = RATE_1M;
+	unsigned int ii = 0;
+
+	if (pSupportRateIEs) {
+		for (ii = 0; ii < pSupportRateIEs->len; ii++) {
+			bySupportRate = DATARATEbyGetRateIdx(pSupportRateIEs->abyRates[ii]);
+			if (bySupportRate > byMaxSupportRate) {
+				byMaxSupportRate = bySupportRate;
+			}
+		}
+	}
+	if (pExtSupportRateIEs) {
+		for (ii = 0; ii < pExtSupportRateIEs->len; ii++) {
+			bySupportRate = DATARATEbyGetRateIdx(pExtSupportRateIEs->abyRates[ii]);
+			if (bySupportRate > byMaxSupportRate) {
+				byMaxSupportRate = bySupportRate;
+			}
+		}
+	}
+
+	return byMaxSupportRate;
 }
 
 /*+
@@ -245,48 +242,48 @@ VNTWIFIbyGetMaxSupportRate (
  *
  * Return Value: max support rate
  *
--*/
+ -*/
 unsigned char
-VNTWIFIbyGetACKTxRate (
-    unsigned char byRxDataRate,
-    PWLAN_IE_SUPP_RATES pSupportRateIEs,
-    PWLAN_IE_SUPP_RATES pExtSupportRateIEs
-    )
+VNTWIFIbyGetACKTxRate(
+	unsigned char byRxDataRate,
+	PWLAN_IE_SUPP_RATES pSupportRateIEs,
+	PWLAN_IE_SUPP_RATES pExtSupportRateIEs
+)
 {
-    unsigned char byMaxAckRate;
-    unsigned char byBasicRate;
-    unsigned int ii;
-
-    if (byRxDataRate <= RATE_11M) {
-        byMaxAckRate = RATE_1M;
-    } else  {
-        // 24M is mandatory for 802.11a and 802.11g
-        byMaxAckRate = RATE_24M;
-    }
-    if (pSupportRateIEs) {
-        for (ii = 0; ii < pSupportRateIEs->len; ii++) {
-            if (pSupportRateIEs->abyRates[ii] & 0x80) {
-                byBasicRate = DATARATEbyGetRateIdx(pSupportRateIEs->abyRates[ii]);
-                if ((byBasicRate <= byRxDataRate) &&
-                    (byBasicRate > byMaxAckRate))  {
-                    byMaxAckRate = byBasicRate;
-                }
-            }
-        }
-    }
-    if (pExtSupportRateIEs) {
-        for (ii = 0; ii < pExtSupportRateIEs->len; ii++) {
-            if (pExtSupportRateIEs->abyRates[ii] & 0x80) {
-                byBasicRate = DATARATEbyGetRateIdx(pExtSupportRateIEs->abyRates[ii]);
-                if ((byBasicRate <= byRxDataRate) &&
-                    (byBasicRate > byMaxAckRate))  {
-                    byMaxAckRate = byBasicRate;
-                }
-            }
-        }
-    }
-
-    return byMaxAckRate;
+	unsigned char byMaxAckRate;
+	unsigned char byBasicRate;
+	unsigned int ii;
+
+	if (byRxDataRate <= RATE_11M) {
+		byMaxAckRate = RATE_1M;
+	} else  {
+		// 24M is mandatory for 802.11a and 802.11g
+		byMaxAckRate = RATE_24M;
+	}
+	if (pSupportRateIEs) {
+		for (ii = 0; ii < pSupportRateIEs->len; ii++) {
+			if (pSupportRateIEs->abyRates[ii] & 0x80) {
+				byBasicRate = DATARATEbyGetRateIdx(pSupportRateIEs->abyRates[ii]);
+				if ((byBasicRate <= byRxDataRate) &&
+				    (byBasicRate > byMaxAckRate))  {
+					byMaxAckRate = byBasicRate;
+				}
+			}
+		}
+	}
+	if (pExtSupportRateIEs) {
+		for (ii = 0; ii < pExtSupportRateIEs->len; ii++) {
+			if (pExtSupportRateIEs->abyRates[ii] & 0x80) {
+				byBasicRate = DATARATEbyGetRateIdx(pExtSupportRateIEs->abyRates[ii]);
+				if ((byBasicRate <= byRxDataRate) &&
+				    (byBasicRate > byMaxAckRate))  {
+					byMaxAckRate = byBasicRate;
+				}
+			}
+		}
+	}
+
+	return byMaxAckRate;
 }
 
 /*+
@@ -303,22 +300,22 @@ VNTWIFIbyGetACKTxRate (
  *
  * Return Value: none
  *
--*/
+ -*/
 void
-VNTWIFIvSetAuthenticationMode (
-    void *pMgmtHandle,
-    WMAC_AUTHENTICATION_MODE eAuthMode
-    )
+VNTWIFIvSetAuthenticationMode(
+	void *pMgmtHandle,
+	WMAC_AUTHENTICATION_MODE eAuthMode
+)
 {
-    PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
-
-    pMgmt->eAuthenMode = eAuthMode;
-    if ((eAuthMode == WMAC_AUTH_SHAREKEY) ||
-        (eAuthMode == WMAC_AUTH_AUTO)) {
-        pMgmt->bShareKeyAlgorithm = true;
-    } else {
-        pMgmt->bShareKeyAlgorithm = false;
-    }
+	PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
+
+	pMgmt->eAuthenMode = eAuthMode;
+	if ((eAuthMode == WMAC_AUTH_SHAREKEY) ||
+	    (eAuthMode == WMAC_AUTH_AUTO)) {
+		pMgmt->bShareKeyAlgorithm = true;
+	} else {
+		pMgmt->bShareKeyAlgorithm = false;
+	}
 }
 
 /*+
@@ -335,59 +332,56 @@ VNTWIFIvSetAuthenticationMode (
  *
  * Return Value: none
  *
--*/
+ -*/
 void
-VNTWIFIvSetEncryptionMode (
-    void *pMgmtHandle,
-    WMAC_ENCRYPTION_MODE eEncryptionMode
-    )
+VNTWIFIvSetEncryptionMode(
+	void *pMgmtHandle,
+	WMAC_ENCRYPTION_MODE eEncryptionMode
+)
 {
-    PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
-
-    pMgmt->eEncryptionMode = eEncryptionMode;
-    if ((eEncryptionMode == WMAC_ENCRYPTION_WEPEnabled) ||
-        (eEncryptionMode == WMAC_ENCRYPTION_TKIPEnabled) ||
-        (eEncryptionMode == WMAC_ENCRYPTION_AESEnabled) ) {
-        pMgmt->bPrivacyInvoked = true;
-    } else {
-        pMgmt->bPrivacyInvoked = false;
-    }
+	PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
+
+	pMgmt->eEncryptionMode = eEncryptionMode;
+	if ((eEncryptionMode == WMAC_ENCRYPTION_WEPEnabled) ||
+	    (eEncryptionMode == WMAC_ENCRYPTION_TKIPEnabled) ||
+	    (eEncryptionMode == WMAC_ENCRYPTION_AESEnabled)) {
+		pMgmt->bPrivacyInvoked = true;
+	} else {
+		pMgmt->bPrivacyInvoked = false;
+	}
 }
 
-
-
 bool
-VNTWIFIbConfigPhyMode (
-    void *pMgmtHandle,
-    CARD_PHY_TYPE ePhyType
-    )
+VNTWIFIbConfigPhyMode(
+	void *pMgmtHandle,
+	CARD_PHY_TYPE ePhyType
+)
 {
-    PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
-
-    if ((ePhyType != PHY_TYPE_AUTO) &&
-        (ePhyType != pMgmt->eCurrentPHYMode)) {
-        if (CARDbSetPhyParameter(pMgmt->pAdapter, ePhyType, 0, 0, NULL, NULL)==true) {
-            pMgmt->eCurrentPHYMode = ePhyType;
-        } else {
-            return(false);
-        }
-    }
-    pMgmt->eConfigPHYMode = ePhyType;
-    return(true);
+	PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
+
+	if ((ePhyType != PHY_TYPE_AUTO) &&
+	    (ePhyType != pMgmt->eCurrentPHYMode)) {
+		if (CARDbSetPhyParameter(pMgmt->pAdapter, ePhyType, 0, 0, NULL, NULL) == true) {
+			pMgmt->eCurrentPHYMode = ePhyType;
+		} else {
+			return false;
+		}
+	}
+	pMgmt->eConfigPHYMode = ePhyType;
+	return true;
 }
 
-
 void
-VNTWIFIbGetConfigPhyMode (
-    void *pMgmtHandle,
-    void *pePhyType
-    )
+VNTWIFIbGetConfigPhyMode(
+	void *pMgmtHandle,
+	void *pePhyType
+)
 {
-    PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
+	PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
 
-    if ((pMgmt != NULL) && (pePhyType != NULL)) {
-        *(PCARD_PHY_TYPE)pePhyType = pMgmt->eConfigPHYMode;
-    }
+	if ((pMgmt != NULL) && (pePhyType != NULL)) {
+		*(PCARD_PHY_TYPE)pePhyType = pMgmt->eConfigPHYMode;
+	}
 }
 
 /*+
@@ -403,8 +397,7 @@ VNTWIFIbGetConfigPhyMode (
  *
  * Return Value: None.
  *
--*/
-
+ -*/
 
 /*+
  *
@@ -420,62 +413,55 @@ VNTWIFIbGetConfigPhyMode (
  *
  * Return Value: None.
  *
--*/
+ -*/
 
 void
 VNTWIFIvQueryBSSList(void *pMgmtHandle, unsigned int *puBSSCount, void **pvFirstBSS)
 {
-    unsigned int ii = 0;
-    PSMgmtObject    pMgmt = (PSMgmtObject)pMgmtHandle;
-    PKnownBSS       pBSS = NULL;
-    unsigned int uCount = 0;
-
-    *pvFirstBSS = NULL;
-
-    for (ii = 0; ii < MAX_BSS_NUM; ii++) {
-        pBSS = &(pMgmt->sBSSList[ii]);
-        if (!pBSS->bActive) {
-            continue;
-        }
-        if (*pvFirstBSS == NULL) {
-            *pvFirstBSS = &(pMgmt->sBSSList[ii]);
-        }
-        uCount++;
-    }
-    *puBSSCount = uCount;
+	unsigned int ii = 0;
+	PSMgmtObject    pMgmt = (PSMgmtObject)pMgmtHandle;
+	PKnownBSS       pBSS = NULL;
+	unsigned int uCount = 0;
+
+	*pvFirstBSS = NULL;
+
+	for (ii = 0; ii < MAX_BSS_NUM; ii++) {
+		pBSS = &(pMgmt->sBSSList[ii]);
+		if (!pBSS->bActive) {
+			continue;
+		}
+		if (*pvFirstBSS == NULL) {
+			*pvFirstBSS = &(pMgmt->sBSSList[ii]);
+		}
+		uCount++;
+	}
+	*puBSSCount = uCount;
 }
 
-
-
-
 void
-VNTWIFIvGetNextBSS (
-    void *pMgmtHandle,
-    void *pvCurrentBSS,
-    void **pvNextBSS
-    )
+VNTWIFIvGetNextBSS(
+	void *pMgmtHandle,
+	void *pvCurrentBSS,
+	void **pvNextBSS
+)
 {
-    PKnownBSS       pBSS = (PKnownBSS) pvCurrentBSS;
-    PSMgmtObject    pMgmt = (PSMgmtObject)pMgmtHandle;
-
-    *pvNextBSS = NULL;
-
-    while (*pvNextBSS == NULL) {
-        pBSS++;
-        if (pBSS > &(pMgmt->sBSSList[MAX_BSS_NUM])) {
-            return;
-        }
-        if (pBSS->bActive == true) {
-            *pvNextBSS = pBSS;
-            return;
-        }
-    }
+	PKnownBSS       pBSS = (PKnownBSS) pvCurrentBSS;
+	PSMgmtObject    pMgmt = (PSMgmtObject)pMgmtHandle;
+
+	*pvNextBSS = NULL;
+
+	while (*pvNextBSS == NULL) {
+		pBSS++;
+		if (pBSS > &(pMgmt->sBSSList[MAX_BSS_NUM])) {
+			return;
+		}
+		if (pBSS->bActive == true) {
+			*pvNextBSS = pBSS;
+			return;
+		}
+	}
 }
 
-
-
-
-
 /*+
  *
  * Description:
@@ -487,319 +473,304 @@ VNTWIFIvGetNextBSS (
  *
  * Return Value: none
  *
--*/
+ -*/
 void
 VNTWIFIvUpdateNodeTxCounter(
-    void *pMgmtHandle,
-    unsigned char *pbyDestAddress,
-    bool bTxOk,
-    unsigned short wRate,
-    unsigned char *pbyTxFailCount
-    )
+	void *pMgmtHandle,
+	unsigned char *pbyDestAddress,
+	bool bTxOk,
+	unsigned short wRate,
+	unsigned char *pbyTxFailCount
+)
 {
-    PSMgmtObject    pMgmt = (PSMgmtObject)pMgmtHandle;
-    unsigned int uNodeIndex = 0;
-    unsigned int ii;
-
-    if ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) ||
-        (pMgmt->eCurrMode == WMAC_MODE_ESS_AP)) {
-        if (BSSDBbIsSTAInNodeDB(pMgmt, pbyDestAddress, &uNodeIndex) == false) {
-            return;
-        }
-    }
-    pMgmt->sNodeDBTable[uNodeIndex].uTxAttempts++;
-    if (bTxOk == true) {
-        // transmit success, TxAttempts at least plus one
-        pMgmt->sNodeDBTable[uNodeIndex].uTxOk[MAX_RATE]++;
-        pMgmt->sNodeDBTable[uNodeIndex].uTxOk[wRate]++;
-    } else {
-        pMgmt->sNodeDBTable[uNodeIndex].uTxFailures++;
-    }
-    pMgmt->sNodeDBTable[uNodeIndex].uTxRetry += pbyTxFailCount[MAX_RATE];
-    for(ii=0;ii<MAX_RATE;ii++) {
-        pMgmt->sNodeDBTable[uNodeIndex].uTxFail[ii] += pbyTxFailCount[ii];
-    }
-    return;
+	PSMgmtObject    pMgmt = (PSMgmtObject)pMgmtHandle;
+	unsigned int uNodeIndex = 0;
+	unsigned int ii;
+
+	if ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) ||
+	    (pMgmt->eCurrMode == WMAC_MODE_ESS_AP)) {
+		if (BSSDBbIsSTAInNodeDB(pMgmt, pbyDestAddress, &uNodeIndex) == false) {
+			return;
+		}
+	}
+	pMgmt->sNodeDBTable[uNodeIndex].uTxAttempts++;
+	if (bTxOk == true) {
+		// transmit success, TxAttempts at least plus one
+		pMgmt->sNodeDBTable[uNodeIndex].uTxOk[MAX_RATE]++;
+		pMgmt->sNodeDBTable[uNodeIndex].uTxOk[wRate]++;
+	} else {
+		pMgmt->sNodeDBTable[uNodeIndex].uTxFailures++;
+	}
+	pMgmt->sNodeDBTable[uNodeIndex].uTxRetry += pbyTxFailCount[MAX_RATE];
+	for (ii = 0; ii < MAX_RATE; ii++) {
+		pMgmt->sNodeDBTable[uNodeIndex].uTxFail[ii] += pbyTxFailCount[ii];
+	}
+	return;
 }
 
-
 void
 VNTWIFIvGetTxRate(
-    void *pMgmtHandle,
-    unsigned char *pbyDestAddress,
-    unsigned short *pwTxDataRate,
-    unsigned char *pbyACKRate,
-    unsigned char *pbyCCKBasicRate,
-    unsigned char *pbyOFDMBasicRate
-    )
+	void *pMgmtHandle,
+	unsigned char *pbyDestAddress,
+	unsigned short *pwTxDataRate,
+	unsigned char *pbyACKRate,
+	unsigned char *pbyCCKBasicRate,
+	unsigned char *pbyOFDMBasicRate
+)
 {
-    PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
-    unsigned int uNodeIndex = 0;
-    unsigned short wTxDataRate = RATE_1M;
-    unsigned char byACKRate = RATE_1M;
-    unsigned char byCCKBasicRate = RATE_1M;
-    unsigned char byOFDMBasicRate = RATE_24M;
-    PWLAN_IE_SUPP_RATES pSupportRateIEs = NULL;
-    PWLAN_IE_SUPP_RATES pExtSupportRateIEs = NULL;
-
-
-    if ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) ||
-        (pMgmt->eCurrMode == WMAC_MODE_ESS_AP)) {
-        // Adhoc Tx rate decided from node DB
-        if(BSSDBbIsSTAInNodeDB(pMgmt, pbyDestAddress, &uNodeIndex)) {
-            wTxDataRate = (pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate);
-            pSupportRateIEs = (PWLAN_IE_SUPP_RATES) (pMgmt->sNodeDBTable[uNodeIndex].abyCurrSuppRates);
-            pExtSupportRateIEs = (PWLAN_IE_SUPP_RATES) (pMgmt->sNodeDBTable[uNodeIndex].abyCurrExtSuppRates);
-        } else {
-            if (pMgmt->eCurrentPHYMode != PHY_TYPE_11A) {
-                wTxDataRate = RATE_2M;
-            } else {
-                wTxDataRate = RATE_24M;
-            }
-            pSupportRateIEs = (PWLAN_IE_SUPP_RATES) pMgmt->abyCurrSuppRates;
-            pExtSupportRateIEs = (PWLAN_IE_SUPP_RATES) pMgmt->abyCurrExtSuppRates;
-        }
-    } else { // Infrastructure: rate decided from AP Node, index = 0
+	PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
+	unsigned int uNodeIndex = 0;
+	unsigned short wTxDataRate = RATE_1M;
+	unsigned char byACKRate = RATE_1M;
+	unsigned char byCCKBasicRate = RATE_1M;
+	unsigned char byOFDMBasicRate = RATE_24M;
+	PWLAN_IE_SUPP_RATES pSupportRateIEs = NULL;
+	PWLAN_IE_SUPP_RATES pExtSupportRateIEs = NULL;
+
+	if ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) ||
+	    (pMgmt->eCurrMode == WMAC_MODE_ESS_AP)) {
+		// Adhoc Tx rate decided from node DB
+		if (BSSDBbIsSTAInNodeDB(pMgmt, pbyDestAddress, &uNodeIndex)) {
+			wTxDataRate = (pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate);
+			pSupportRateIEs = (PWLAN_IE_SUPP_RATES) (pMgmt->sNodeDBTable[uNodeIndex].abyCurrSuppRates);
+			pExtSupportRateIEs = (PWLAN_IE_SUPP_RATES) (pMgmt->sNodeDBTable[uNodeIndex].abyCurrExtSuppRates);
+		} else {
+			if (pMgmt->eCurrentPHYMode != PHY_TYPE_11A) {
+				wTxDataRate = RATE_2M;
+			} else {
+				wTxDataRate = RATE_24M;
+			}
+			pSupportRateIEs = (PWLAN_IE_SUPP_RATES) pMgmt->abyCurrSuppRates;
+			pExtSupportRateIEs = (PWLAN_IE_SUPP_RATES) pMgmt->abyCurrExtSuppRates;
+		}
+	} else { // Infrastructure: rate decided from AP Node, index = 0
 
 		wTxDataRate = (pMgmt->sNodeDBTable[0].wTxDataRate);
 #ifdef	PLICE_DEBUG
 		printk(KERN_DEBUG "GetTxRate:AP MAC is %pM,TxRate is %d\n",
-				pMgmt->sNodeDBTable[0].abyMACAddr, wTxDataRate);
+		       pMgmt->sNodeDBTable[0].abyMACAddr, wTxDataRate);
 #endif
 
-
-        pSupportRateIEs = (PWLAN_IE_SUPP_RATES) pMgmt->abyCurrSuppRates;
-        pExtSupportRateIEs = (PWLAN_IE_SUPP_RATES) pMgmt->abyCurrExtSuppRates;
-    }
-    byACKRate = VNTWIFIbyGetACKTxRate(  (unsigned char) wTxDataRate,
-                                        pSupportRateIEs,
-                                        pExtSupportRateIEs
-                                        );
-    if (byACKRate > (unsigned char) wTxDataRate) {
-        byACKRate = (unsigned char) wTxDataRate;
-    }
-    byCCKBasicRate = VNTWIFIbyGetACKTxRate( RATE_11M,
-                                            pSupportRateIEs,
-                                            pExtSupportRateIEs
-                                            );
-    byOFDMBasicRate = VNTWIFIbyGetACKTxRate(RATE_54M,
-                                            pSupportRateIEs,
-                                            pExtSupportRateIEs
-                                            );
-    *pwTxDataRate = wTxDataRate;
-    *pbyACKRate = byACKRate;
-    *pbyCCKBasicRate = byCCKBasicRate;
-    *pbyOFDMBasicRate = byOFDMBasicRate;
-    return;
+		pSupportRateIEs = (PWLAN_IE_SUPP_RATES) pMgmt->abyCurrSuppRates;
+		pExtSupportRateIEs = (PWLAN_IE_SUPP_RATES) pMgmt->abyCurrExtSuppRates;
+	}
+	byACKRate = VNTWIFIbyGetACKTxRate((unsigned char) wTxDataRate,
+					    pSupportRateIEs,
+					    pExtSupportRateIEs
+);
+	if (byACKRate > (unsigned char) wTxDataRate) {
+		byACKRate = (unsigned char) wTxDataRate;
+	}
+	byCCKBasicRate = VNTWIFIbyGetACKTxRate(RATE_11M,
+						pSupportRateIEs,
+						pExtSupportRateIEs
+);
+	byOFDMBasicRate = VNTWIFIbyGetACKTxRate(RATE_54M,
+						pSupportRateIEs,
+						pExtSupportRateIEs
+);
+	*pwTxDataRate = wTxDataRate;
+	*pbyACKRate = byACKRate;
+	*pbyCCKBasicRate = byCCKBasicRate;
+	*pbyOFDMBasicRate = byOFDMBasicRate;
+	return;
 }
 
 unsigned char
 VNTWIFIbyGetKeyCypher(
-    void *pMgmtHandle,
-    bool bGroupKey
-    )
+	void *pMgmtHandle,
+	bool bGroupKey
+)
 {
-    PSMgmtObject    pMgmt = (PSMgmtObject)pMgmtHandle;
+	PSMgmtObject    pMgmt = (PSMgmtObject)pMgmtHandle;
 
-    if (bGroupKey == true) {
-        return (pMgmt->byCSSGK);
-    } else {
-        return (pMgmt->byCSSPK);
-    }
+	if (bGroupKey == true) {
+		return pMgmt->byCSSGK;
+	} else {
+		return pMgmt->byCSSPK;
+	}
 }
 
-
 /*
-bool
-VNTWIFIbInit(
-    void *pAdapterHandler,
-    void **pMgmtHandler
-    )
-{
-
-    PSMgmtObject        pMgmt = NULL;
-    unsigned int ii;
-
-
-    pMgmt = (PSMgmtObject)kmalloc(sizeof(SMgmtObject), (int)GFP_ATOMIC);
-    if (pMgmt == NULL) {
-        *pMgmtHandler = NULL;
-        return false;
-    }
-
-    memset(pMgmt, 0, sizeof(SMgmtObject));
-    pMgmt->pAdapter = (void *) pAdapterHandler;
-
-    // should initial MAC address abyMACAddr
-    for(ii=0;ii<WLAN_BSSID_LEN;ii++) {
-        pMgmt->abyDesireBSSID[ii] = 0xFF;
-    }
-    pMgmt->pbyPSPacketPool = &pMgmt->byPSPacketPool[0];
-    pMgmt->pbyMgmtPacketPool = &pMgmt->byMgmtPacketPool[0];
-    pMgmt->byCSSPK = KEY_CTL_NONE;
-    pMgmt->byCSSGK = KEY_CTL_NONE;
-    pMgmt->wIBSSBeaconPeriod = DEFAULT_IBSS_BI;
-
-    pMgmt->cbFreeCmdQueue = CMD_Q_SIZE;
-    pMgmt->uCmdDequeueIdx = 0;
-    pMgmt->uCmdEnqueueIdx = 0;
-    pMgmt->eCommandState = WLAN_CMD_STATE_IDLE;
-    pMgmt->bCmdStop = false;
-    pMgmt->bCmdRunning = false;
-
-    *pMgmtHandler = pMgmt;
-    return true;
-}
+  bool
+  VNTWIFIbInit(
+  void *pAdapterHandler,
+  void **pMgmtHandler
+) {
+  PSMgmtObject        pMgmt = NULL;
+  unsigned int ii;
+
+  pMgmt = (PSMgmtObject)kmalloc(sizeof(SMgmtObject), (int)GFP_ATOMIC);
+  if (pMgmt == NULL) {
+  *pMgmtHandler = NULL;
+  return false;
+  }
+
+  memset(pMgmt, 0, sizeof(SMgmtObject));
+  pMgmt->pAdapter = (void *) pAdapterHandler;
+
+  // should initial MAC address abyMACAddr
+  for (ii=0; ii<WLAN_BSSID_LEN; ii++) {
+  pMgmt->abyDesireBSSID[ii] = 0xFF;
+  }
+  pMgmt->pbyPSPacketPool = &pMgmt->byPSPacketPool[0];
+  pMgmt->pbyMgmtPacketPool = &pMgmt->byMgmtPacketPool[0];
+  pMgmt->byCSSPK = KEY_CTL_NONE;
+  pMgmt->byCSSGK = KEY_CTL_NONE;
+  pMgmt->wIBSSBeaconPeriod = DEFAULT_IBSS_BI;
+
+  pMgmt->cbFreeCmdQueue = CMD_Q_SIZE;
+  pMgmt->uCmdDequeueIdx = 0;
+  pMgmt->uCmdEnqueueIdx = 0;
+  pMgmt->eCommandState = WLAN_CMD_STATE_IDLE;
+  pMgmt->bCmdStop = false;
+  pMgmt->bCmdRunning = false;
+
+  *pMgmtHandler = pMgmt;
+  return true;
+  }
 */
 
-
-
 bool
-VNTWIFIbSetPMKIDCache (
-    void *pMgmtObject,
-    unsigned long ulCount,
-    void *pPMKIDInfo
-    )
+VNTWIFIbSetPMKIDCache(
+	void *pMgmtObject,
+	unsigned long ulCount,
+	void *pPMKIDInfo
+)
 {
-    PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
-
-    if (ulCount > MAX_PMKID_CACHE) {
-        return (false);
-    }
-    pMgmt->gsPMKIDCache.BSSIDInfoCount = ulCount;
-    memcpy(pMgmt->gsPMKIDCache.BSSIDInfo, pPMKIDInfo, (ulCount*sizeof(PMKIDInfo)));
-    return (true);
+	PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
+
+	if (ulCount > MAX_PMKID_CACHE) {
+		return false;
+	}
+	pMgmt->gsPMKIDCache.BSSIDInfoCount = ulCount;
+	memcpy(pMgmt->gsPMKIDCache.BSSIDInfo, pPMKIDInfo, (ulCount*sizeof(PMKIDInfo)));
+	return true;
 }
 
-
-
 unsigned short
 VNTWIFIwGetMaxSupportRate(
-    void *pMgmtObject
-    )
+	void *pMgmtObject
+)
 {
-    unsigned short wRate = RATE_54M;
-    PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
-
-    for(wRate = RATE_54M; wRate > RATE_1M; wRate--) {
-        if (pMgmt->sNodeDBTable[0].wSuppRate & (1<<wRate)) {
-            return (wRate);
-        }
-    }
-    if (pMgmt->eCurrentPHYMode == PHY_TYPE_11A) {
-        return (RATE_6M);
-    } else {
-        return (RATE_1M);
-    }
+	unsigned short wRate = RATE_54M;
+	PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
+
+	for (wRate = RATE_54M; wRate > RATE_1M; wRate--) {
+		if (pMgmt->sNodeDBTable[0].wSuppRate & (1<<wRate)) {
+			return wRate;
+		}
+	}
+	if (pMgmt->eCurrentPHYMode == PHY_TYPE_11A) {
+		return RATE_6M;
+	} else {
+		return RATE_1M;
+	}
 }
 
-
 void
-VNTWIFIvSet11h (
-    void *pMgmtObject,
-    bool b11hEnable
-    )
+VNTWIFIvSet11h(
+	void *pMgmtObject,
+	bool b11hEnable
+)
 {
-    PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
+	PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
 
-    pMgmt->b11hEnable = b11hEnable;
+	pMgmt->b11hEnable = b11hEnable;
 }
 
 bool
 VNTWIFIbMeasureReport(
-    void *pMgmtObject,
-    bool bEndOfReport,
-    void *pvMeasureEID,
-    unsigned char byReportMode,
-    unsigned char byBasicMap,
-    unsigned char byCCAFraction,
-    unsigned char *pbyRPIs
-    )
+	void *pMgmtObject,
+	bool bEndOfReport,
+	void *pvMeasureEID,
+	unsigned char byReportMode,
+	unsigned char byBasicMap,
+	unsigned char byCCAFraction,
+	unsigned char *pbyRPIs
+)
 {
-    PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
-    unsigned char *pbyCurrentEID = (unsigned char *) (pMgmt->pCurrMeasureEIDRep);
-
-    //spin_lock_irq(&pDevice->lock);
-    if ((pvMeasureEID != NULL) &&
-        (pMgmt->uLengthOfRepEIDs < (WLAN_A3FR_MAXLEN - sizeof(MEASEURE_REP) - sizeof(WLAN_80211HDR_A3) - 3))
-        ) {
-        pMgmt->pCurrMeasureEIDRep->byElementID = WLAN_EID_MEASURE_REP;
-        pMgmt->pCurrMeasureEIDRep->len = 3;
-        pMgmt->pCurrMeasureEIDRep->byToken = ((PWLAN_IE_MEASURE_REQ) pvMeasureEID)->byToken;
-        pMgmt->pCurrMeasureEIDRep->byMode = byReportMode;
-        pMgmt->pCurrMeasureEIDRep->byType = ((PWLAN_IE_MEASURE_REQ) pvMeasureEID)->byType;
-        switch (pMgmt->pCurrMeasureEIDRep->byType) {
-            case MEASURE_TYPE_BASIC :
-                pMgmt->pCurrMeasureEIDRep->len += sizeof(MEASEURE_REP_BASIC);
-                memcpy(   &(pMgmt->pCurrMeasureEIDRep->sRep.sBasic),
-                            &(((PWLAN_IE_MEASURE_REQ) pvMeasureEID)->sReq),
-                            sizeof(MEASEURE_REQ));
-                pMgmt->pCurrMeasureEIDRep->sRep.sBasic.byMap = byBasicMap;
-                break;
-            case MEASURE_TYPE_CCA :
-                pMgmt->pCurrMeasureEIDRep->len += sizeof(MEASEURE_REP_CCA);
-                memcpy(   &(pMgmt->pCurrMeasureEIDRep->sRep.sCCA),
-                            &(((PWLAN_IE_MEASURE_REQ) pvMeasureEID)->sReq),
-                            sizeof(MEASEURE_REQ));
-                pMgmt->pCurrMeasureEIDRep->sRep.sCCA.byCCABusyFraction = byCCAFraction;
-                break;
-            case MEASURE_TYPE_RPI :
-                pMgmt->pCurrMeasureEIDRep->len += sizeof(MEASEURE_REP_RPI);
-                memcpy(   &(pMgmt->pCurrMeasureEIDRep->sRep.sRPI),
-                            &(((PWLAN_IE_MEASURE_REQ) pvMeasureEID)->sReq),
-                            sizeof(MEASEURE_REQ));
-                memcpy(pMgmt->pCurrMeasureEIDRep->sRep.sRPI.abyRPIdensity, pbyRPIs, 8);
-                break;
-            default :
-                break;
-        }
-        pbyCurrentEID += (2 + pMgmt->pCurrMeasureEIDRep->len);
-        pMgmt->uLengthOfRepEIDs += (2 + pMgmt->pCurrMeasureEIDRep->len);
-        pMgmt->pCurrMeasureEIDRep = (PWLAN_IE_MEASURE_REP) pbyCurrentEID;
-    }
-    if (bEndOfReport == true) {
-        IEEE11hbMSRRepTx(pMgmt);
-    }
-    //spin_unlock_irq(&pDevice->lock);
-    return (true);
+	PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
+	unsigned char *pbyCurrentEID = (unsigned char *)(pMgmt->pCurrMeasureEIDRep);
+
+	//spin_lock_irq(&pDevice->lock);
+	if ((pvMeasureEID != NULL) &&
+	    (pMgmt->uLengthOfRepEIDs < (WLAN_A3FR_MAXLEN - sizeof(MEASEURE_REP) - sizeof(WLAN_80211HDR_A3) - 3))
+) {
+		pMgmt->pCurrMeasureEIDRep->byElementID = WLAN_EID_MEASURE_REP;
+		pMgmt->pCurrMeasureEIDRep->len = 3;
+		pMgmt->pCurrMeasureEIDRep->byToken = ((PWLAN_IE_MEASURE_REQ)pvMeasureEID)->byToken;
+		pMgmt->pCurrMeasureEIDRep->byMode = byReportMode;
+		pMgmt->pCurrMeasureEIDRep->byType = ((PWLAN_IE_MEASURE_REQ) pvMeasureEID)->byType;
+		switch (pMgmt->pCurrMeasureEIDRep->byType) {
+		case MEASURE_TYPE_BASIC:
+			pMgmt->pCurrMeasureEIDRep->len += sizeof(MEASEURE_REP_BASIC);
+			memcpy(&(pMgmt->pCurrMeasureEIDRep->sRep.sBasic),
+			       &(((PWLAN_IE_MEASURE_REQ) pvMeasureEID)->sReq),
+			       sizeof(MEASEURE_REQ));
+			pMgmt->pCurrMeasureEIDRep->sRep.sBasic.byMap = byBasicMap;
+			break;
+		case MEASURE_TYPE_CCA:
+			pMgmt->pCurrMeasureEIDRep->len += sizeof(MEASEURE_REP_CCA);
+			memcpy(&(pMgmt->pCurrMeasureEIDRep->sRep.sCCA),
+			       &(((PWLAN_IE_MEASURE_REQ) pvMeasureEID)->sReq),
+			       sizeof(MEASEURE_REQ));
+			pMgmt->pCurrMeasureEIDRep->sRep.sCCA.byCCABusyFraction = byCCAFraction;
+			break;
+		case MEASURE_TYPE_RPI:
+			pMgmt->pCurrMeasureEIDRep->len += sizeof(MEASEURE_REP_RPI);
+			memcpy(&(pMgmt->pCurrMeasureEIDRep->sRep.sRPI),
+			       &(((PWLAN_IE_MEASURE_REQ) pvMeasureEID)->sReq),
+			       sizeof(MEASEURE_REQ));
+			memcpy(pMgmt->pCurrMeasureEIDRep->sRep.sRPI.abyRPIdensity, pbyRPIs, 8);
+			break;
+		default:
+			break;
+		}
+		pbyCurrentEID += (2 + pMgmt->pCurrMeasureEIDRep->len);
+		pMgmt->uLengthOfRepEIDs += (2 + pMgmt->pCurrMeasureEIDRep->len);
+		pMgmt->pCurrMeasureEIDRep = (PWLAN_IE_MEASURE_REP) pbyCurrentEID;
+	}
+	if (bEndOfReport == true) {
+		IEEE11hbMSRRepTx(pMgmt);
+	}
+	//spin_unlock_irq(&pDevice->lock);
+	return true;
 }
 
-
 bool
 VNTWIFIbChannelSwitch(
-    void *pMgmtObject,
-    unsigned char byNewChannel
-    )
+	void *pMgmtObject,
+	unsigned char byNewChannel
+)
 {
-    PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
+	PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
 
-    //spin_lock_irq(&pDevice->lock);
-    pMgmt->uCurrChannel = byNewChannel;
-    pMgmt->bSwitchChannel = false;
-    //spin_unlock_irq(&pDevice->lock);
-    return true;
+	//spin_lock_irq(&pDevice->lock);
+	pMgmt->uCurrChannel = byNewChannel;
+	pMgmt->bSwitchChannel = false;
+	//spin_unlock_irq(&pDevice->lock);
+	return true;
 }
 
 /*
-bool
-VNTWIFIbRadarPresent(
-    void *pMgmtObject,
-    unsigned char byChannel
-    )
-{
-    PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
-    if ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) &&
-        (byChannel == (unsigned char) pMgmt->uCurrChannel) &&
-        (pMgmt->bSwitchChannel != true) &&
-        (pMgmt->b11hEnable == true)) {
-        if (!compare_ether_addr(pMgmt->abyIBSSDFSOwner, CARDpGetCurrentAddress(pMgmt->pAdapter))) {
-            pMgmt->byNewChannel = CARDbyAutoChannelSelect(pMgmt->pAdapter,(unsigned char) pMgmt->uCurrChannel);
-            pMgmt->bSwitchChannel = true;
-        }
-        BEACONbSendBeacon(pMgmt);
-        CARDbChannelSwitch(pMgmt->pAdapter, 0, pMgmt->byNewChannel, 10);
-    }
-    return true;
-}
+  bool
+  VNTWIFIbRadarPresent(
+  void *pMgmtObject,
+  unsigned char byChannel
+) {
+  PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
+  if ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) &&
+  (byChannel == (unsigned char) pMgmt->uCurrChannel) &&
+  (pMgmt->bSwitchChannel != true) &&
+  (pMgmt->b11hEnable == true)) {
+  if (!compare_ether_addr(pMgmt->abyIBSSDFSOwner, CARDpGetCurrentAddress(pMgmt->pAdapter))) {
+  pMgmt->byNewChannel = CARDbyAutoChannelSelect(pMgmt->pAdapter,(unsigned char) pMgmt->uCurrChannel);
+  pMgmt->bSwitchChannel = true;
+  }
+  BEACONbSendBeacon(pMgmt);
+  CARDbChannelSwitch(pMgmt->pAdapter, 0, pMgmt->byNewChannel, 10);
+  }
+  return true;
+  }
 */
-
diff --git a/drivers/staging/vt6655/vntwifi.h b/drivers/staging/vt6655/vntwifi.h
index f4327abaa77..4b01ebd7299 100644
--- a/drivers/staging/vt6655/vntwifi.h
+++ b/drivers/staging/vt6655/vntwifi.h
@@ -64,244 +64,224 @@
 
 // Pre-configured Authenticaiton Mode (from XP)
 typedef enum tagWMAC_AUTHENTICATION_MODE {
-
-    WMAC_AUTH_OPEN,
-    WMAC_AUTH_SHAREKEY,
-    WMAC_AUTH_AUTO,
-    WMAC_AUTH_WPA,
-    WMAC_AUTH_WPAPSK,
-    WMAC_AUTH_WPANONE,
-    WMAC_AUTH_WPA2,
-    WMAC_AUTH_WPA2PSK,
-    WMAC_AUTH_MAX       // Not a real mode, defined as upper bound
-
+	WMAC_AUTH_OPEN,
+	WMAC_AUTH_SHAREKEY,
+	WMAC_AUTH_AUTO,
+	WMAC_AUTH_WPA,
+	WMAC_AUTH_WPAPSK,
+	WMAC_AUTH_WPANONE,
+	WMAC_AUTH_WPA2,
+	WMAC_AUTH_WPA2PSK,
+	WMAC_AUTH_MAX       // Not a real mode, defined as upper bound
 } WMAC_AUTHENTICATION_MODE, *PWMAC_AUTHENTICATION_MODE;
 
 typedef enum tagWMAC_ENCRYPTION_MODE {
-
-    WMAC_ENCRYPTION_WEPEnabled,
-    WMAC_ENCRYPTION_WEPDisabled,
-    WMAC_ENCRYPTION_WEPKeyAbsent,
-    WMAC_ENCRYPTION_WEPNotSupported,
-    WMAC_ENCRYPTION_TKIPEnabled,
-    WMAC_ENCRYPTION_TKIPKeyAbsent,
-    WMAC_ENCRYPTION_AESEnabled,
-    WMAC_ENCRYPTION_AESKeyAbsent
-
+	WMAC_ENCRYPTION_WEPEnabled,
+	WMAC_ENCRYPTION_WEPDisabled,
+	WMAC_ENCRYPTION_WEPKeyAbsent,
+	WMAC_ENCRYPTION_WEPNotSupported,
+	WMAC_ENCRYPTION_TKIPEnabled,
+	WMAC_ENCRYPTION_TKIPKeyAbsent,
+	WMAC_ENCRYPTION_AESEnabled,
+	WMAC_ENCRYPTION_AESKeyAbsent
 } WMAC_ENCRYPTION_MODE, *PWMAC_ENCRYPTION_MODE;
 
 // Pre-configured Mode (from XP)
 
 typedef enum tagWMAC_CONFIG_MODE {
-
-    WMAC_CONFIG_ESS_STA = 0,
-    WMAC_CONFIG_IBSS_STA,
-    WMAC_CONFIG_AUTO,
-    WMAC_CONFIG_AP
-
+	WMAC_CONFIG_ESS_STA = 0,
+	WMAC_CONFIG_IBSS_STA,
+	WMAC_CONFIG_AUTO,
+	WMAC_CONFIG_AP
 } WMAC_CONFIG_MODE, *PWMAC_CONFIG_MODE;
 
-
-
 typedef enum tagWMAC_POWER_MODE {
-
-    WMAC_POWER_CAM,
-    WMAC_POWER_FAST,
-    WMAC_POWER_MAX
-
+	WMAC_POWER_CAM,
+	WMAC_POWER_FAST,
+	WMAC_POWER_MAX
 } WMAC_POWER_MODE, *PWMAC_POWER_MODE;
 
 #define VNTWIFIbIsShortSlotTime(wCapInfo)               \
-        WLAN_GET_CAP_INFO_SHORTSLOTTIME(wCapInfo)       \
+	WLAN_GET_CAP_INFO_SHORTSLOTTIME(wCapInfo)       \
 
 #define VNTWIFIbIsProtectMode(byERP)                    \
-        ((byERP & WLAN_EID_ERP_USE_PROTECTION) != 0)    \
+	((byERP & WLAN_EID_ERP_USE_PROTECTION) != 0)    \
 
 #define VNTWIFIbIsBarkerMode(byERP)                     \
-        ((byERP & WLAN_EID_ERP_BARKER_MODE) != 0)       \
+	((byERP & WLAN_EID_ERP_BARKER_MODE) != 0)       \
 
 #define VNTWIFIbIsShortPreamble(wCapInfo)               \
-        WLAN_GET_CAP_INFO_SHORTPREAMBLE(wCapInfo)       \
-
-#define VNTWIFIbIsEncryption(wCapInfo)                  \
-        WLAN_GET_CAP_INFO_PRIVACY(wCapInfo)             \
+	WLAN_GET_CAP_INFO_SHORTPREAMBLE(wCapInfo)       \
 
-#define VNTWIFIbIsESS(wCapInfo)                         \
-        WLAN_GET_CAP_INFO_ESS(wCapInfo)                 \
+#define VNTWIFIbIsEncryption(wCapInfo)		\
+	WLAN_GET_CAP_INFO_PRIVACY(wCapInfo)	\
 
+#define VNTWIFIbIsESS(wCapInfo)			\
+	WLAN_GET_CAP_INFO_ESS(wCapInfo)		\
 
 /*---------------------  Export Classes  ----------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
 
-
 /*---------------------  Export Types  ------------------------------*/
 
-
 /*---------------------  Export Functions  --------------------------*/
 
 void
-VNTWIFIvSetIBSSParameter (
-    void *pMgmtHandle,
-    unsigned short wBeaconPeriod,
-    unsigned short wATIMWindow,
-    unsigned int uChannel
-    );
+VNTWIFIvSetIBSSParameter(
+	void *pMgmtHandle,
+	unsigned short wBeaconPeriod,
+	unsigned short wATIMWindow,
+	unsigned int uChannel
+);
 
 void
-VNTWIFIvSetOPMode (
-    void *pMgmtHandle,
-    WMAC_CONFIG_MODE eOPMode
-    );
+VNTWIFIvSetOPMode(
+	void *pMgmtHandle,
+	WMAC_CONFIG_MODE eOPMode
+);
 
 PWLAN_IE_SSID
 VNTWIFIpGetCurrentSSID(
-    void *pMgmtHandle
-    );
+	void *pMgmtHandle
+);
 
 unsigned int
 VNTWIFIpGetCurrentChannel(
-    void *pMgmtHandle
-    );
+	void *pMgmtHandle
+);
 
 unsigned short
-VNTWIFIwGetAssocID (
-    void *pMgmtHandle
-    );
+VNTWIFIwGetAssocID(
+	void *pMgmtHandle
+);
 
 unsigned char
-VNTWIFIbyGetMaxSupportRate (
-    PWLAN_IE_SUPP_RATES pSupportRateIEs,
-    PWLAN_IE_SUPP_RATES pExtSupportRateIEs
-    );
+VNTWIFIbyGetMaxSupportRate(
+	PWLAN_IE_SUPP_RATES pSupportRateIEs,
+	PWLAN_IE_SUPP_RATES pExtSupportRateIEs
+);
 
 unsigned char
-VNTWIFIbyGetACKTxRate (
-    unsigned char byRxDataRate,
-    PWLAN_IE_SUPP_RATES pSupportRateIEs,
-    PWLAN_IE_SUPP_RATES pExtSupportRateIEs
-    );
+VNTWIFIbyGetACKTxRate(
+	unsigned char byRxDataRate,
+	PWLAN_IE_SUPP_RATES pSupportRateIEs,
+	PWLAN_IE_SUPP_RATES pExtSupportRateIEs
+);
 
 void
-VNTWIFIvSetAuthenticationMode (
-    void *pMgmtHandle,
-    WMAC_AUTHENTICATION_MODE eAuthMode
-    );
+VNTWIFIvSetAuthenticationMode(
+	void *pMgmtHandle,
+	WMAC_AUTHENTICATION_MODE eAuthMode
+);
 
 void
-VNTWIFIvSetEncryptionMode (
-    void *pMgmtHandle,
-    WMAC_ENCRYPTION_MODE eEncryptionMode
-    );
-
+VNTWIFIvSetEncryptionMode(
+	void *pMgmtHandle,
+	WMAC_ENCRYPTION_MODE eEncryptionMode
+);
 
 bool
 VNTWIFIbConfigPhyMode(
-    void *pMgmtHandle,
-    CARD_PHY_TYPE ePhyType
-    );
+	void *pMgmtHandle,
+	CARD_PHY_TYPE ePhyType
+);
 
 void
 VNTWIFIbGetConfigPhyMode(
-    void *pMgmtHandle,
-    void *pePhyType
-    );
+	void *pMgmtHandle,
+	void *pePhyType
+);
 
 void
 VNTWIFIvQueryBSSList(void *pMgmtHandle, unsigned int *puBSSCount,
-		void **pvFirstBSS);
+		     void **pvFirstBSS);
 
 void
-VNTWIFIvGetNextBSS (
-    void *pMgmtHandle,
-    void *pvCurrentBSS,
-    void **pvNextBSS
-    );
-
-
+VNTWIFIvGetNextBSS(
+	void *pMgmtHandle,
+	void *pvCurrentBSS,
+	void **pvNextBSS
+);
 
 void
 VNTWIFIvUpdateNodeTxCounter(
-    void *pMgmtHandle,
-    unsigned char *pbyDestAddress,
-    bool bTxOk,
-    unsigned short wRate,
-    unsigned char *pbyTxFailCount
-    );
-
+	void *pMgmtHandle,
+	unsigned char *pbyDestAddress,
+	bool bTxOk,
+	unsigned short wRate,
+	unsigned char *pbyTxFailCount
+);
 
 void
 VNTWIFIvGetTxRate(
-    void *pMgmtHandle,
-    unsigned char *pbyDestAddress,
-    unsigned short *pwTxDataRate,
-    unsigned char *pbyACKRate,
-    unsigned char *pbyCCKBasicRate,
-    unsigned char *pbyOFDMBasicRate
-    );
+	void *pMgmtHandle,
+	unsigned char *pbyDestAddress,
+	unsigned short *pwTxDataRate,
+	unsigned char *pbyACKRate,
+	unsigned char *pbyCCKBasicRate,
+	unsigned char *pbyOFDMBasicRate
+);
 /*
-bool
-VNTWIFIbInit(
-    void *pAdapterHandler,
-    void **pMgmtHandler
-    );
+  bool
+  VNTWIFIbInit(
+  void *pAdapterHandler,
+  void **pMgmtHandler
+);
 */
 
 unsigned char
 VNTWIFIbyGetKeyCypher(
-    void *pMgmtHandle,
-    bool bGroupKey
-    );
-
-
-
+	void *pMgmtHandle,
+	bool bGroupKey
+);
 
 bool
-VNTWIFIbSetPMKIDCache (
-    void *pMgmtObject,
-    unsigned long ulCount,
-    void *pPMKIDInfo
-    );
+VNTWIFIbSetPMKIDCache(
+	void *pMgmtObject,
+	unsigned long ulCount,
+	void *pPMKIDInfo
+);
 
 bool
-VNTWIFIbCommandRunning (
-    void *pMgmtObject
-    );
+VNTWIFIbCommandRunning(
+	void *pMgmtObject
+);
 
 unsigned short
 VNTWIFIwGetMaxSupportRate(
-    void *pMgmtObject
-    );
+	void *pMgmtObject
+);
 
 // for 802.11h
 void
-VNTWIFIvSet11h (
-    void *pMgmtObject,
-    bool b11hEnable
-    );
+VNTWIFIvSet11h(
+	void *pMgmtObject,
+	bool b11hEnable
+);
 
 bool
 VNTWIFIbMeasureReport(
-    void *pMgmtObject,
-    bool bEndOfReport,
-    void *pvMeasureEID,
-    unsigned char byReportMode,
-    unsigned char byBasicMap,
-    unsigned char byCCAFraction,
-    unsigned char *pbyRPIs
-    );
+	void *pMgmtObject,
+	bool bEndOfReport,
+	void *pvMeasureEID,
+	unsigned char byReportMode,
+	unsigned char byBasicMap,
+	unsigned char byCCAFraction,
+	unsigned char *pbyRPIs
+);
 
 bool
 VNTWIFIbChannelSwitch(
-    void *pMgmtObject,
-    unsigned char byNewChannel
-    );
+	void *pMgmtObject,
+	unsigned char byNewChannel
+);
 /*
-bool
-VNTWIFIbRadarPresent(
-    void *pMgmtObject,
-    unsigned char byChannel
-    );
+  bool
+  VNTWIFIbRadarPresent(
+  void *pMgmtObject,
+  unsigned char byChannel
+);
 */
 
 #endif //__VNTWIFI_H__
diff --git a/drivers/staging/vt6655/wcmd.c b/drivers/staging/vt6655/wcmd.c
index 101c7359f41..d551653537b 100644
--- a/drivers/staging/vt6655/wcmd.c
+++ b/drivers/staging/vt6655/wcmd.c
@@ -56,48 +56,40 @@
 
 /*---------------------  Static Definitions -------------------------*/
 
-
-
-
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
-static int          msglevel                =MSG_LEVEL_INFO;
+static int msglevel = MSG_LEVEL_INFO;
 //static int          msglevel                =MSG_LEVEL_DEBUG;
 /*---------------------  Static Functions  --------------------------*/
 
 static
 void
 s_vProbeChannel(
-    PSDevice pDevice
-    );
-
+	PSDevice pDevice
+);
 
 static
 PSTxMgmtPacket
 s_MgrMakeProbeRequest(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    unsigned char *pScanBSSID,
-    PWLAN_IE_SSID pSSID,
-    PWLAN_IE_SUPP_RATES pCurrRates,
-    PWLAN_IE_SUPP_RATES pCurrExtSuppRates
-    );
-
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	unsigned char *pScanBSSID,
+	PWLAN_IE_SSID pSSID,
+	PWLAN_IE_SUPP_RATES pCurrRates,
+	PWLAN_IE_SUPP_RATES pCurrExtSuppRates
+);
 
 static
 bool
-s_bCommandComplete (
-    PSDevice pDevice
-    );
+s_bCommandComplete(
+	PSDevice pDevice
+);
 
 /*---------------------  Export Variables  --------------------------*/
 
-
 /*---------------------  Export Functions  --------------------------*/
 
-
-
 /*
  * Description:
  *      Stop AdHoc beacon during scan process
@@ -115,44 +107,37 @@ static
 void
 vAdHocBeaconStop(PSDevice  pDevice)
 {
-
-    PSMgmtObject    pMgmt = &(pDevice->sMgmtObj);
-    bool bStop;
-
-    /*
-     * temporarily stop Beacon packet for AdHoc Server
-     * if all of the following conditions are met:
-     *  (1) STA is in AdHoc mode
-     *  (2) VT3253 is programmed as automatic Beacon Transmitting
-     *  (3) One of the following conditions is met
-     *      (3.1) AdHoc channel is in B/G band and the
-     *      current scan channel is in A band
-     *      or
-     *      (3.2) AdHoc channel is in A mode
-     */
-    bStop = false;
-    if ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) &&
-    (pMgmt->eCurrState >= WMAC_STATE_STARTED))
-    {
-        if ((pMgmt->uIBSSChannel <=  CB_MAX_CHANNEL_24G) &&
-             (pMgmt->uScanChannel > CB_MAX_CHANNEL_24G))
-        {
-            bStop = true;
-        }
-        if (pMgmt->uIBSSChannel >  CB_MAX_CHANNEL_24G)
-        {
-            bStop = true;
-        }
-    }
-
-    if (bStop)
-    {
-        MACvRegBitsOff(pDevice->PortOffset, MAC_REG_TCR, TCR_AUTOBCNTX);
-    }
-
+	PSMgmtObject    pMgmt = &(pDevice->sMgmtObj);
+	bool bStop;
+
+	/*
+	 * temporarily stop Beacon packet for AdHoc Server
+	 * if all of the following conditions are met:
+	 *  (1) STA is in AdHoc mode
+	 *  (2) VT3253 is programmed as automatic Beacon Transmitting
+	 *  (3) One of the following conditions is met
+	 *      (3.1) AdHoc channel is in B/G band and the
+	 *      current scan channel is in A band
+	 *      or
+	 *      (3.2) AdHoc channel is in A mode
+	 */
+	bStop = false;
+	if ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) &&
+	    (pMgmt->eCurrState >= WMAC_STATE_STARTED)) {
+		if ((pMgmt->uIBSSChannel <=  CB_MAX_CHANNEL_24G) &&
+		    (pMgmt->uScanChannel > CB_MAX_CHANNEL_24G)) {
+			bStop = true;
+		}
+		if (pMgmt->uIBSSChannel >  CB_MAX_CHANNEL_24G) {
+			bStop = true;
+		}
+	}
+
+	if (bStop) {
+		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_TCR, TCR_AUTOBCNTX);
+	}
 } /* vAdHocBeaconStop */
 
-
 /*
  * Description:
  *      Restart AdHoc beacon after scan process complete
@@ -170,27 +155,20 @@ static
 void
 vAdHocBeaconRestart(PSDevice pDevice)
 {
-    PSMgmtObject    pMgmt = &(pDevice->sMgmtObj);
-
-    /*
-     * Restart Beacon packet for AdHoc Server
-     * if all of the following coditions are met:
-     *  (1) STA is in AdHoc mode
-     *  (2) VT3253 is programmed as automatic Beacon Transmitting
-     */
-    if ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) &&
-    (pMgmt->eCurrState >= WMAC_STATE_STARTED))
-    {
-         MACvRegBitsOn(pDevice->PortOffset, MAC_REG_TCR, TCR_AUTOBCNTX);
-    }
-
+	PSMgmtObject    pMgmt = &(pDevice->sMgmtObj);
+
+	/*
+	 * Restart Beacon packet for AdHoc Server
+	 * if all of the following coditions are met:
+	 *  (1) STA is in AdHoc mode
+	 *  (2) VT3253 is programmed as automatic Beacon Transmitting
+	 */
+	if ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) &&
+	    (pMgmt->eCurrState >= WMAC_STATE_STARTED)) {
+		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_TCR, TCR_AUTOBCNTX);
+	}
 }
 
-
-
-
-
-
 /*+
  *
  * Routine Description:
@@ -200,60 +178,54 @@ vAdHocBeaconRestart(PSDevice pDevice)
  * Return Value:
  *    none.
  *
--*/
+ -*/
 
 static
 void
 s_vProbeChannel(
-    PSDevice pDevice
-    )
+	PSDevice pDevice
+)
 {
-                                                     //1M,   2M,   5M,   11M,  18M,  24M,  36M,  54M
-    unsigned char abyCurrSuppRatesG[] = {WLAN_EID_SUPP_RATES, 8, 0x02, 0x04, 0x0B, 0x16, 0x24, 0x30, 0x48, 0x6C};
-    unsigned char abyCurrExtSuppRatesG[] = {WLAN_EID_EXTSUPP_RATES, 4, 0x0C, 0x12, 0x18, 0x60};
-                                                           //6M,   9M,   12M,  48M
-    unsigned char abyCurrSuppRatesA[] = {WLAN_EID_SUPP_RATES, 8, 0x0C, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C};
-    unsigned char abyCurrSuppRatesB[] = {WLAN_EID_SUPP_RATES, 4, 0x02, 0x04, 0x0B, 0x16};
-    unsigned char *pbyRate;
-    PSTxMgmtPacket  pTxPacket;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    unsigned int ii;
-
-
-    if (pDevice->eCurrentPHYType == PHY_TYPE_11A) {
-        pbyRate = &abyCurrSuppRatesA[0];
-    } else if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
-        pbyRate = &abyCurrSuppRatesB[0];
-    } else {
-        pbyRate = &abyCurrSuppRatesG[0];
-    }
-    // build an assocreq frame and send it
-    pTxPacket = s_MgrMakeProbeRequest
-                (
-                  pDevice,
-                  pMgmt,
-                  pMgmt->abyScanBSSID,
-                  (PWLAN_IE_SSID)pMgmt->abyScanSSID,
-                  (PWLAN_IE_SUPP_RATES)pbyRate,
-                  (PWLAN_IE_SUPP_RATES)abyCurrExtSuppRatesG
-                );
-
-    if (pTxPacket != NULL ){
-        for (ii = 0; ii < 2 ; ii++) {
-            if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING) {
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Probe request sending fail.. \n");
-            }
-            else {
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Probe request is sending.. \n");
-            }
-        }
-    }
-
+	//1M,   2M,   5M,   11M,  18M,  24M,  36M,  54M
+	unsigned char abyCurrSuppRatesG[] = {WLAN_EID_SUPP_RATES, 8, 0x02, 0x04, 0x0B, 0x16, 0x24, 0x30, 0x48, 0x6C};
+	unsigned char abyCurrExtSuppRatesG[] = {WLAN_EID_EXTSUPP_RATES, 4, 0x0C, 0x12, 0x18, 0x60};
+	//6M,   9M,   12M,  48M
+	unsigned char abyCurrSuppRatesA[] = {WLAN_EID_SUPP_RATES, 8, 0x0C, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C};
+	unsigned char abyCurrSuppRatesB[] = {WLAN_EID_SUPP_RATES, 4, 0x02, 0x04, 0x0B, 0x16};
+	unsigned char *pbyRate;
+	PSTxMgmtPacket  pTxPacket;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	unsigned int ii;
+
+	if (pDevice->eCurrentPHYType == PHY_TYPE_11A) {
+		pbyRate = &abyCurrSuppRatesA[0];
+	} else if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
+		pbyRate = &abyCurrSuppRatesB[0];
+	} else {
+		pbyRate = &abyCurrSuppRatesG[0];
+	}
+	// build an assocreq frame and send it
+	pTxPacket = s_MgrMakeProbeRequest
+		(
+			pDevice,
+			pMgmt,
+			pMgmt->abyScanBSSID,
+			(PWLAN_IE_SSID)pMgmt->abyScanSSID,
+			(PWLAN_IE_SUPP_RATES)pbyRate,
+			(PWLAN_IE_SUPP_RATES)abyCurrExtSuppRatesG
+			);
+
+	if (pTxPacket != NULL) {
+		for (ii = 0; ii < 2; ii++) {
+			if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING) {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Probe request sending fail.. \n");
+			} else {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Probe request is sending.. \n");
+			}
+		}
+	}
 }
 
-
-
-
 /*+
  *
  * Routine Description:
@@ -263,765 +235,724 @@ s_vProbeChannel(
  * Return Value:
  *    A ptr to Tx frame or NULL on allocation failue
  *
--*/
-
+ -*/
 
 PSTxMgmtPacket
 s_MgrMakeProbeRequest(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    unsigned char *pScanBSSID,
-    PWLAN_IE_SSID pSSID,
-    PWLAN_IE_SUPP_RATES pCurrRates,
-    PWLAN_IE_SUPP_RATES pCurrExtSuppRates
-
-    )
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	unsigned char *pScanBSSID,
+	PWLAN_IE_SSID pSSID,
+	PWLAN_IE_SUPP_RATES pCurrRates,
+	PWLAN_IE_SUPP_RATES pCurrExtSuppRates
+
+)
 {
-    PSTxMgmtPacket      pTxPacket = NULL;
-    WLAN_FR_PROBEREQ    sFrame;
-
-
-    pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
-    memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_PROBEREQ_FR_MAXLEN);
-    pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-    sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
-    sFrame.len = WLAN_PROBEREQ_FR_MAXLEN;
-    vMgrEncodeProbeRequest(&sFrame);
-    sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
-        (
-        WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
-        WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_PROBEREQ)
-        ));
-    memcpy( sFrame.pHdr->sA3.abyAddr1, pScanBSSID, WLAN_ADDR_LEN);
-    memcpy( sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-    memcpy( sFrame.pHdr->sA3.abyAddr3, pScanBSSID, WLAN_BSSID_LEN);
-    // Copy the SSID, pSSID->len=0 indicate broadcast SSID
-    sFrame.pSSID = (PWLAN_IE_SSID)(sFrame.pBuf + sFrame.len);
-    sFrame.len += pSSID->len + WLAN_IEHDR_LEN;
-    memcpy(sFrame.pSSID, pSSID, pSSID->len + WLAN_IEHDR_LEN);
-    sFrame.pSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-    sFrame.len += pCurrRates->len + WLAN_IEHDR_LEN;
-    memcpy(sFrame.pSuppRates, pCurrRates, pCurrRates->len + WLAN_IEHDR_LEN);
-    // Copy the extension rate set
-    if (pDevice->eCurrentPHYType == PHY_TYPE_11G) {
-        sFrame.pExtSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-        sFrame.len += pCurrExtSuppRates->len + WLAN_IEHDR_LEN;
-        memcpy(sFrame.pExtSuppRates, pCurrExtSuppRates, pCurrExtSuppRates->len + WLAN_IEHDR_LEN);
-    }
-    pTxPacket->cbMPDULen = sFrame.len;
-    pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
-
-    return pTxPacket;
+	PSTxMgmtPacket      pTxPacket = NULL;
+	WLAN_FR_PROBEREQ    sFrame;
+
+	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
+	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_PROBEREQ_FR_MAXLEN);
+	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
+	sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
+	sFrame.len = WLAN_PROBEREQ_FR_MAXLEN;
+	vMgrEncodeProbeRequest(&sFrame);
+	sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
+		(
+			WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
+			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_PROBEREQ)
+));
+	memcpy(sFrame.pHdr->sA3.abyAddr1, pScanBSSID, WLAN_ADDR_LEN);
+	memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
+	memcpy(sFrame.pHdr->sA3.abyAddr3, pScanBSSID, WLAN_BSSID_LEN);
+	// Copy the SSID, pSSID->len=0 indicate broadcast SSID
+	sFrame.pSSID = (PWLAN_IE_SSID)(sFrame.pBuf + sFrame.len);
+	sFrame.len += pSSID->len + WLAN_IEHDR_LEN;
+	memcpy(sFrame.pSSID, pSSID, pSSID->len + WLAN_IEHDR_LEN);
+	sFrame.pSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
+	sFrame.len += pCurrRates->len + WLAN_IEHDR_LEN;
+	memcpy(sFrame.pSuppRates, pCurrRates, pCurrRates->len + WLAN_IEHDR_LEN);
+	// Copy the extension rate set
+	if (pDevice->eCurrentPHYType == PHY_TYPE_11G) {
+		sFrame.pExtSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
+		sFrame.len += pCurrExtSuppRates->len + WLAN_IEHDR_LEN;
+		memcpy(sFrame.pExtSuppRates, pCurrExtSuppRates, pCurrExtSuppRates->len + WLAN_IEHDR_LEN);
+	}
+	pTxPacket->cbMPDULen = sFrame.len;
+	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
+
+	return pTxPacket;
 }
 
-
-
-
-
 void
 vCommandTimerWait(
-    void *hDeviceContext,
-    unsigned int MSecond
-    )
+	void *hDeviceContext,
+	unsigned int MSecond
+)
 {
-    PSDevice        pDevice = (PSDevice)hDeviceContext;
-
-    init_timer(&pDevice->sTimerCommand);
-    pDevice->sTimerCommand.data = (unsigned long) pDevice;
-    pDevice->sTimerCommand.function = (TimerFunction)vCommandTimer;
-    // RUN_AT :1 msec ~= (HZ/1024)
-    pDevice->sTimerCommand.expires = (unsigned int)RUN_AT((MSecond * HZ) >> 10);
-    add_timer(&pDevice->sTimerCommand);
-    return;
+	PSDevice        pDevice = (PSDevice)hDeviceContext;
+
+	init_timer(&pDevice->sTimerCommand);
+	pDevice->sTimerCommand.data = (unsigned long) pDevice;
+	pDevice->sTimerCommand.function = (TimerFunction)vCommandTimer;
+	// RUN_AT :1 msec ~= (HZ/1024)
+	pDevice->sTimerCommand.expires = (unsigned int)RUN_AT((MSecond * HZ) >> 10);
+	add_timer(&pDevice->sTimerCommand);
+	return;
 }
 
-
-
-
 void
-vCommandTimer (
-    void *hDeviceContext
-    )
+vCommandTimer(
+	void *hDeviceContext
+)
 {
-    PSDevice        pDevice = (PSDevice)hDeviceContext;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    PWLAN_IE_SSID   pItemSSID;
-    PWLAN_IE_SSID   pItemSSIDCurr;
-    CMD_STATUS      Status;
-    unsigned int ii;
-    unsigned char byMask[8] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80};
-    struct sk_buff  *skb;
-
-
-    if (pDevice->dwDiagRefCount != 0)
-        return;
-    if (pDevice->bCmdRunning != true)
-        return;
-
-    spin_lock_irq(&pDevice->lock);
-
-    switch ( pDevice->eCommandState ) {
-
-        case WLAN_CMD_SCAN_START:
-
-	pDevice->byReAssocCount = 0;
-            if (pDevice->bRadioOff == true) {
-                s_bCommandComplete(pDevice);
-                spin_unlock_irq(&pDevice->lock);
-                return;
-            }
-
-            if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-                s_bCommandComplete(pDevice);
-                CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_AP);
-                spin_unlock_irq(&pDevice->lock);
-                return;
-            }
-
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"eCommandState= WLAN_CMD_SCAN_START\n");
-            pItemSSID = (PWLAN_IE_SSID)pMgmt->abyScanSSID;
-            // wait all Data TD complete
-            if (pDevice->iTDUsed[TYPE_AC0DMA] != 0){
-                spin_unlock_irq(&pDevice->lock);
-                vCommandTimerWait((void *)pDevice, 10);
-                return;
-            }
-
-            if (pMgmt->uScanChannel == 0 ) {
-                pMgmt->uScanChannel = pDevice->byMinChannel;
-                // Set Baseband to be more sensitive.
-
-            }
-            if (pMgmt->uScanChannel > pDevice->byMaxChannel) {
-                pMgmt->eScanState = WMAC_NO_SCANNING;
-
-                // Set Baseband's sensitivity back.
-                // Set channel back
-                set_channel(pMgmt->pAdapter, pMgmt->uCurrChannel);
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Scanning, set back to channel: [%d]\n", pMgmt->uCurrChannel);
-                if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
-                    CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_ADHOC);
-                } else {
-                    CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_INFRASTRUCTURE);
-                }
-                vAdHocBeaconRestart(pDevice);
-                s_bCommandComplete(pDevice);
-
-            } else {
+	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	PWLAN_IE_SSID   pItemSSID;
+	PWLAN_IE_SSID   pItemSSIDCurr;
+	CMD_STATUS      Status;
+	unsigned int ii;
+	unsigned char byMask[8] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80};
+	struct sk_buff  *skb;
+
+	if (pDevice->dwDiagRefCount != 0)
+		return;
+	if (pDevice->bCmdRunning != true)
+		return;
+
+	spin_lock_irq(&pDevice->lock);
+
+	switch (pDevice->eCommandState) {
+	case WLAN_CMD_SCAN_START:
+
+		pDevice->byReAssocCount = 0;
+		if (pDevice->bRadioOff == true) {
+			s_bCommandComplete(pDevice);
+			spin_unlock_irq(&pDevice->lock);
+			return;
+		}
+
+		if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
+			s_bCommandComplete(pDevice);
+			CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_AP);
+			spin_unlock_irq(&pDevice->lock);
+			return;
+		}
+
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "eCommandState= WLAN_CMD_SCAN_START\n");
+		pItemSSID = (PWLAN_IE_SSID)pMgmt->abyScanSSID;
+		// wait all Data TD complete
+		if (pDevice->iTDUsed[TYPE_AC0DMA] != 0) {
+			spin_unlock_irq(&pDevice->lock);
+			vCommandTimerWait((void *)pDevice, 10);
+			return;
+		}
+
+		if (pMgmt->uScanChannel == 0) {
+			pMgmt->uScanChannel = pDevice->byMinChannel;
+			// Set Baseband to be more sensitive.
+
+		}
+		if (pMgmt->uScanChannel > pDevice->byMaxChannel) {
+			pMgmt->eScanState = WMAC_NO_SCANNING;
+
+			// Set Baseband's sensitivity back.
+			// Set channel back
+			set_channel(pMgmt->pAdapter, pMgmt->uCurrChannel);
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Scanning, set back to channel: [%d]\n", pMgmt->uCurrChannel);
+			if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
+				CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_ADHOC);
+			} else {
+				CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_INFRASTRUCTURE);
+			}
+			vAdHocBeaconRestart(pDevice);
+			s_bCommandComplete(pDevice);
+
+		} else {
 //2008-8-4 <add> by chester
-		if (!is_channel_valid(pMgmt->uScanChannel)) {
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Invalid channel pMgmt->uScanChannel = %d \n",pMgmt->uScanChannel);
-                    s_bCommandComplete(pDevice);
-                    spin_unlock_irq(&pDevice->lock);
-                    return;
-                }
-//printk("chester-pMgmt->uScanChannel=%d,pDevice->byMaxChannel=%d\n",pMgmt->uScanChannel,pDevice->byMaxChannel);
-                if (pMgmt->uScanChannel == pDevice->byMinChannel) {
-                    //pMgmt->eScanType = WMAC_SCAN_ACTIVE;
-                    pMgmt->abyScanBSSID[0] = 0xFF;
-                    pMgmt->abyScanBSSID[1] = 0xFF;
-                    pMgmt->abyScanBSSID[2] = 0xFF;
-                    pMgmt->abyScanBSSID[3] = 0xFF;
-                    pMgmt->abyScanBSSID[4] = 0xFF;
-                    pMgmt->abyScanBSSID[5] = 0xFF;
-                    pItemSSID->byElementID = WLAN_EID_SSID;
-                    // clear bssid list
-                    // BSSvClearBSSList((void *)pDevice, pDevice->bLinkPass);
-                    pMgmt->eScanState = WMAC_IS_SCANNING;
-
-                }
-
-                vAdHocBeaconStop(pDevice);
-
-                if (set_channel(pMgmt->pAdapter, pMgmt->uScanChannel) == true) {
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"SCAN Channel: %d\n", pMgmt->uScanChannel);
-                } else {
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"SET SCAN Channel Fail: %d\n", pMgmt->uScanChannel);
-                }
-                CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_UNKNOWN);
-//	printk("chester-mxch=%d\n",pDevice->byMaxChannel);
-      //          printk("chester-ch=%d\n",pMgmt->uScanChannel);
-	pMgmt->uScanChannel++;
+			if (!is_channel_valid(pMgmt->uScanChannel)) {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Invalid channel pMgmt->uScanChannel = %d \n", pMgmt->uScanChannel);
+				s_bCommandComplete(pDevice);
+				spin_unlock_irq(&pDevice->lock);
+				return;
+			}
+			if (pMgmt->uScanChannel == pDevice->byMinChannel) {
+				//pMgmt->eScanType = WMAC_SCAN_ACTIVE;
+				pMgmt->abyScanBSSID[0] = 0xFF;
+				pMgmt->abyScanBSSID[1] = 0xFF;
+				pMgmt->abyScanBSSID[2] = 0xFF;
+				pMgmt->abyScanBSSID[3] = 0xFF;
+				pMgmt->abyScanBSSID[4] = 0xFF;
+				pMgmt->abyScanBSSID[5] = 0xFF;
+				pItemSSID->byElementID = WLAN_EID_SSID;
+				// clear bssid list
+				// BSSvClearBSSList((void *)pDevice, pDevice->bLinkPass);
+				pMgmt->eScanState = WMAC_IS_SCANNING;
+
+			}
+
+			vAdHocBeaconStop(pDevice);
+
+			if (set_channel(pMgmt->pAdapter, pMgmt->uScanChannel) == true) {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "SCAN Channel: %d\n", pMgmt->uScanChannel);
+			} else {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "SET SCAN Channel Fail: %d\n", pMgmt->uScanChannel);
+			}
+			CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_UNKNOWN);
+			pMgmt->uScanChannel++;
 //2008-8-4 <modify> by chester
-		if (!is_channel_valid(pMgmt->uScanChannel) &&
-                        pMgmt->uScanChannel <= pDevice->byMaxChannel ){
-                    pMgmt->uScanChannel=pDevice->byMaxChannel+1;
-		 pMgmt->eCommandState = WLAN_CMD_SCAN_END;
-
-                }
-
-
-                if ((pMgmt->b11hEnable == false) ||
-                    (pMgmt->uScanChannel < CB_MAX_CHANNEL_24G)) {
-                    s_vProbeChannel(pDevice);
-                    spin_unlock_irq(&pDevice->lock);
-                    vCommandTimerWait((void *)pDevice, WCMD_ACTIVE_SCAN_TIME);
-                    return;
-                } else {
-                    spin_unlock_irq(&pDevice->lock);
-                    vCommandTimerWait((void *)pDevice, WCMD_PASSIVE_SCAN_TIME);
-                    return;
-                }
-
-            }
-
-            break;
-
-        case WLAN_CMD_SCAN_END:
-
-            // Set Baseband's sensitivity back.
-            // Set channel back
-            set_channel(pMgmt->pAdapter, pMgmt->uCurrChannel);
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Scanning, set back to channel: [%d]\n", pMgmt->uCurrChannel);
-            if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
-                CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_ADHOC);
-            } else {
-                CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_INFRASTRUCTURE);
-            }
-
-            pMgmt->eScanState = WMAC_NO_SCANNING;
-            vAdHocBeaconRestart(pDevice);
+			if (!is_channel_valid(pMgmt->uScanChannel) &&
+			    pMgmt->uScanChannel <= pDevice->byMaxChannel) {
+				pMgmt->uScanChannel = pDevice->byMaxChannel + 1;
+				pMgmt->eCommandState = WLAN_CMD_SCAN_END;
+
+			}
+
+			if ((pMgmt->b11hEnable == false) ||
+			    (pMgmt->uScanChannel < CB_MAX_CHANNEL_24G)) {
+				s_vProbeChannel(pDevice);
+				spin_unlock_irq(&pDevice->lock);
+				vCommandTimerWait((void *)pDevice, WCMD_ACTIVE_SCAN_TIME);
+				return;
+			} else {
+				spin_unlock_irq(&pDevice->lock);
+				vCommandTimerWait((void *)pDevice, WCMD_PASSIVE_SCAN_TIME);
+				return;
+			}
+
+		}
+
+		break;
+
+	case WLAN_CMD_SCAN_END:
+
+		// Set Baseband's sensitivity back.
+		// Set channel back
+		set_channel(pMgmt->pAdapter, pMgmt->uCurrChannel);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Scanning, set back to channel: [%d]\n", pMgmt->uCurrChannel);
+		if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
+			CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_ADHOC);
+		} else {
+			CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_INFRASTRUCTURE);
+		}
+
+		pMgmt->eScanState = WMAC_NO_SCANNING;
+		vAdHocBeaconRestart(pDevice);
 //2008-0409-07, <Add> by Einsn Liu
 #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-	if(pMgmt->eScanType == WMAC_SCAN_PASSIVE)
-			{//send scan event to wpa_Supplicant
-				union iwreq_data wrqu;
-				memset(&wrqu, 0, sizeof(wrqu));
-				wireless_send_event(pDevice->dev, SIOCGIWSCAN, &wrqu, NULL);
-			}
+		if (pMgmt->eScanType == WMAC_SCAN_PASSIVE)
+		{//send scan event to wpa_Supplicant
+			union iwreq_data wrqu;
+			memset(&wrqu, 0, sizeof(wrqu));
+			wireless_send_event(pDevice->dev, SIOCGIWSCAN, &wrqu, NULL);
+		}
 #endif
-            s_bCommandComplete(pDevice);
-            break;
-
-        case WLAN_CMD_DISASSOCIATE_START :
-	pDevice->byReAssocCount = 0;
-            if ((pMgmt->eCurrMode == WMAC_MODE_ESS_STA) &&
-                (pMgmt->eCurrState != WMAC_STATE_ASSOC)) {
-                s_bCommandComplete(pDevice);
-                spin_unlock_irq(&pDevice->lock);
-                return;
-            } else {
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Send Disassociation Packet..\n");
-                // reason = 8 : disassoc because sta has left
-                vMgrDisassocBeginSta((void *)pDevice, pMgmt, pMgmt->abyCurrBSSID, (8), &Status);
-                pDevice->bLinkPass = false;
-                // unlock command busy
-                pItemSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
-                pItemSSID->len = 0;
-                memset(pItemSSID->abySSID, 0, WLAN_SSID_MAXLEN);
-                pMgmt->eCurrState = WMAC_STATE_IDLE;
-                pMgmt->sNodeDBTable[0].bActive = false;
+		s_bCommandComplete(pDevice);
+		break;
+
+	case WLAN_CMD_DISASSOCIATE_START:
+		pDevice->byReAssocCount = 0;
+		if ((pMgmt->eCurrMode == WMAC_MODE_ESS_STA) &&
+		    (pMgmt->eCurrState != WMAC_STATE_ASSOC)) {
+			s_bCommandComplete(pDevice);
+			spin_unlock_irq(&pDevice->lock);
+			return;
+		} else {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Send Disassociation Packet..\n");
+			// reason = 8 : disassoc because sta has left
+			vMgrDisassocBeginSta((void *)pDevice, pMgmt, pMgmt->abyCurrBSSID, (8), &Status);
+			pDevice->bLinkPass = false;
+			// unlock command busy
+			pItemSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
+			pItemSSID->len = 0;
+			memset(pItemSSID->abySSID, 0, WLAN_SSID_MAXLEN);
+			pMgmt->eCurrState = WMAC_STATE_IDLE;
+			pMgmt->sNodeDBTable[0].bActive = false;
 //                pDevice->bBeaconBufReady = false;
-            }
-            netif_stop_queue(pDevice->dev);
-            pDevice->eCommandState = WLAN_DISASSOCIATE_WAIT;
-            // wait all Control TD complete
-            if (pDevice->iTDUsed[TYPE_TXDMA0] != 0){
-                vCommandTimerWait((void *)pDevice, 10);
-                spin_unlock_irq(&pDevice->lock);
-                return;
-            }
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" CARDbRadioPowerOff\n");
-	//2008-09-02  <mark>	by chester
-           // CARDbRadioPowerOff(pDevice);
-            s_bCommandComplete(pDevice);
-            break;
-
-        case WLAN_DISASSOCIATE_WAIT :
-            // wait all Control TD complete
-            if (pDevice->iTDUsed[TYPE_TXDMA0] != 0){
-                vCommandTimerWait((void *)pDevice, 10);
-                spin_unlock_irq(&pDevice->lock);
-                return;
-            }
+		}
+		netif_stop_queue(pDevice->dev);
+		pDevice->eCommandState = WLAN_DISASSOCIATE_WAIT;
+		// wait all Control TD complete
+		if (pDevice->iTDUsed[TYPE_TXDMA0] != 0) {
+			vCommandTimerWait((void *)pDevice, 10);
+			spin_unlock_irq(&pDevice->lock);
+			return;
+		}
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " CARDbRadioPowerOff\n");
+		//2008-09-02  <mark>	by chester
+		// CARDbRadioPowerOff(pDevice);
+		s_bCommandComplete(pDevice);
+		break;
+
+	case WLAN_DISASSOCIATE_WAIT:
+		// wait all Control TD complete
+		if (pDevice->iTDUsed[TYPE_TXDMA0] != 0) {
+			vCommandTimerWait((void *)pDevice, 10);
+			spin_unlock_irq(&pDevice->lock);
+			return;
+		}
 //2008-09-02  <mark> by chester
-           // CARDbRadioPowerOff(pDevice);
-            s_bCommandComplete(pDevice);
-            break;
-
-        case WLAN_CMD_SSID_START:
-        	pDevice->byReAssocCount = 0;
-            if (pDevice->bRadioOff == true) {
-                s_bCommandComplete(pDevice);
-                spin_unlock_irq(&pDevice->lock);
-                return;
-            }
-//printk("chester-currmode=%d\n",pMgmt->eCurrMode);
-printk("chester-abyDesireSSID=%s\n",((PWLAN_IE_SSID)pMgmt->abyDesireSSID)->abySSID);
-                     //memcpy(pMgmt->abyAdHocSSID,pMgmt->abyDesireSSID,
-                              //((PWLAN_IE_SSID)pMgmt->abyDesireSSID)->len + WLAN_IEHDR_LEN);
-            pItemSSID = (PWLAN_IE_SSID)pMgmt->abyDesireSSID;
-            pItemSSIDCurr = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" cmd: desire ssid = %s\n", pItemSSID->abySSID);
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" cmd: curr ssid = %s\n", pItemSSIDCurr->abySSID);
-
-            if (pMgmt->eCurrState == WMAC_STATE_ASSOC) {
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" Cmd pMgmt->eCurrState == WMAC_STATE_ASSOC\n");
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" pItemSSID->len =%d\n",pItemSSID->len);
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" pItemSSIDCurr->len = %d\n",pItemSSIDCurr->len);
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" desire ssid = %s\n", pItemSSID->abySSID);
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" curr ssid = %s\n", pItemSSIDCurr->abySSID);
-            }
-
-            if ((pMgmt->eCurrState == WMAC_STATE_ASSOC) ||
-                ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA)&& (pMgmt->eCurrState == WMAC_STATE_JOINTED))) {
-
-                if (pItemSSID->len == pItemSSIDCurr->len) {
-                    if (memcmp(pItemSSID->abySSID, pItemSSIDCurr->abySSID, pItemSSID->len) == 0) {
-                        s_bCommandComplete(pDevice);
-                        spin_unlock_irq(&pDevice->lock);
-                        return;
-                    }
-                }
-
-                netif_stop_queue(pDevice->dev);
-                pDevice->bLinkPass = false;
-            }
-            // set initial state
-            pMgmt->eCurrState = WMAC_STATE_IDLE;
-            pMgmt->eCurrMode = WMAC_MODE_STANDBY;
-            PSvDisablePowerSaving((void *)pDevice);
-            BSSvClearNodeDBTable(pDevice, 0);
-
-            vMgrJoinBSSBegin((void *)pDevice, &Status);
-            // if Infra mode
-            if ((pMgmt->eCurrMode == WMAC_MODE_ESS_STA) && (pMgmt->eCurrState == WMAC_STATE_JOINTED)) {
-
-		// Call mgr to begin the deauthentication
-                // reason = (3) because sta has left ESS
-                if (pMgmt->eCurrState>= WMAC_STATE_AUTH) {
-                    vMgrDeAuthenBeginSta((void *)pDevice, pMgmt, pMgmt->abyCurrBSSID, (3), &Status);
-                }
-                // Call mgr to begin the authentication
-                vMgrAuthenBeginSta((void *)pDevice, pMgmt, &Status);
-                if (Status == CMD_STATUS_SUCCESS) {
+		// CARDbRadioPowerOff(pDevice);
+		s_bCommandComplete(pDevice);
+		break;
+
+	case WLAN_CMD_SSID_START:
+		pDevice->byReAssocCount = 0;
+		if (pDevice->bRadioOff == true) {
+			s_bCommandComplete(pDevice);
+			spin_unlock_irq(&pDevice->lock);
+			return;
+		}
+		printk("chester-abyDesireSSID=%s\n", ((PWLAN_IE_SSID)pMgmt->abyDesireSSID)->abySSID);
+		//memcpy(pMgmt->abyAdHocSSID,pMgmt->abyDesireSSID,
+		//((PWLAN_IE_SSID)pMgmt->abyDesireSSID)->len + WLAN_IEHDR_LEN);
+		pItemSSID = (PWLAN_IE_SSID)pMgmt->abyDesireSSID;
+		pItemSSIDCurr = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " cmd: desire ssid = %s\n", pItemSSID->abySSID);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " cmd: curr ssid = %s\n", pItemSSIDCurr->abySSID);
+
+		if (pMgmt->eCurrState == WMAC_STATE_ASSOC) {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Cmd pMgmt->eCurrState == WMAC_STATE_ASSOC\n");
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " pItemSSID->len =%d\n", pItemSSID->len);
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " pItemSSIDCurr->len = %d\n", pItemSSIDCurr->len);
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " desire ssid = %s\n", pItemSSID->abySSID);
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " curr ssid = %s\n", pItemSSIDCurr->abySSID);
+		}
+
+		if ((pMgmt->eCurrState == WMAC_STATE_ASSOC) ||
+		    ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) && (pMgmt->eCurrState == WMAC_STATE_JOINTED))) {
+			if (pItemSSID->len == pItemSSIDCurr->len) {
+				if (memcmp(pItemSSID->abySSID, pItemSSIDCurr->abySSID, pItemSSID->len) == 0) {
+					s_bCommandComplete(pDevice);
+					spin_unlock_irq(&pDevice->lock);
+					return;
+				}
+			}
+
+			netif_stop_queue(pDevice->dev);
+			pDevice->bLinkPass = false;
+		}
+		// set initial state
+		pMgmt->eCurrState = WMAC_STATE_IDLE;
+		pMgmt->eCurrMode = WMAC_MODE_STANDBY;
+		PSvDisablePowerSaving((void *)pDevice);
+		BSSvClearNodeDBTable(pDevice, 0);
+
+		vMgrJoinBSSBegin((void *)pDevice, &Status);
+		// if Infra mode
+		if ((pMgmt->eCurrMode == WMAC_MODE_ESS_STA) && (pMgmt->eCurrState == WMAC_STATE_JOINTED)) {
+			// Call mgr to begin the deauthentication
+			// reason = (3) because sta has left ESS
+			if (pMgmt->eCurrState >= WMAC_STATE_AUTH) {
+				vMgrDeAuthenBeginSta((void *)pDevice, pMgmt, pMgmt->abyCurrBSSID, (3), &Status);
+			}
+			// Call mgr to begin the authentication
+			vMgrAuthenBeginSta((void *)pDevice, pMgmt, &Status);
+			if (Status == CMD_STATUS_SUCCESS) {
+				pDevice->byLinkWaitCount = 0;
+				pDevice->eCommandState = WLAN_AUTHENTICATE_WAIT;
+				vCommandTimerWait((void *)pDevice, AUTHENTICATE_TIMEOUT);
+				spin_unlock_irq(&pDevice->lock);
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Set eCommandState = WLAN_AUTHENTICATE_WAIT\n");
+				return;
+			}
+		}
+		// if Adhoc mode
+		else if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
+			if (pMgmt->eCurrState == WMAC_STATE_JOINTED) {
+				if (netif_queue_stopped(pDevice->dev)) {
+					netif_wake_queue(pDevice->dev);
+				}
+				pDevice->bLinkPass = true;
+
+				pMgmt->sNodeDBTable[0].bActive = true;
+				pMgmt->sNodeDBTable[0].uInActiveCount = 0;
+				bClearBSSID_SCAN(pDevice);
+			} else {
+				// start own IBSS
+				vMgrCreateOwnIBSS((void *)pDevice, &Status);
+				if (Status != CMD_STATUS_SUCCESS) {
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " WLAN_CMD_IBSS_CREATE fail ! \n");
+				}
+				BSSvAddMulticastNode(pDevice);
+			}
+		}
+		// if SSID not found
+		else if (pMgmt->eCurrMode == WMAC_MODE_STANDBY) {
+			if (pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA ||
+			    pMgmt->eConfigMode == WMAC_CONFIG_AUTO) {
+				// start own IBSS
+				vMgrCreateOwnIBSS((void *)pDevice, &Status);
+				if (Status != CMD_STATUS_SUCCESS) {
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " WLAN_CMD_IBSS_CREATE fail ! \n");
+				}
+				BSSvAddMulticastNode(pDevice);
+				if (netif_queue_stopped(pDevice->dev)) {
+					netif_wake_queue(pDevice->dev);
+				}
+				pDevice->bLinkPass = true;
+			} else {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Disconnect SSID none\n");
+#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
+				{
+					union iwreq_data  wrqu;
+					memset(&wrqu, 0, sizeof(wrqu));
+					wrqu.ap_addr.sa_family = ARPHRD_ETHER;
+					printk("wireless_send_event--->SIOCGIWAP(disassociated:vMgrJoinBSSBegin Fail !!)\n");
+					wireless_send_event(pDevice->dev, SIOCGIWAP, &wrqu, NULL);
+				}
+#endif
+
+			}
+		}
+		s_bCommandComplete(pDevice);
+		break;
+
+	case WLAN_AUTHENTICATE_WAIT:
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "eCommandState == WLAN_AUTHENTICATE_WAIT\n");
+		if (pMgmt->eCurrState == WMAC_STATE_AUTH) {
+			// Call mgr to begin the association
+			pDevice->byLinkWaitCount = 0;
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "eCurrState == WMAC_STATE_AUTH\n");
+			vMgrAssocBeginSta((void *)pDevice, pMgmt, &Status);
+			if (Status == CMD_STATUS_SUCCESS) {
+				pDevice->byLinkWaitCount = 0;
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "eCommandState = WLAN_ASSOCIATE_WAIT\n");
+				pDevice->eCommandState = WLAN_ASSOCIATE_WAIT;
+				vCommandTimerWait((void *)pDevice, ASSOCIATE_TIMEOUT);
+				spin_unlock_irq(&pDevice->lock);
+				return;
+			}
+		}
+
+		else if (pMgmt->eCurrState < WMAC_STATE_AUTHPENDING) {
+			printk("WLAN_AUTHENTICATE_WAIT:Authen Fail???\n");
+		} else if (pDevice->byLinkWaitCount <= 4) {    //mike add:wait another 2 sec if authenticated_frame delay!
+			pDevice->byLinkWaitCount++;
+			printk("WLAN_AUTHENTICATE_WAIT:wait %d times!!\n", pDevice->byLinkWaitCount);
+			spin_unlock_irq(&pDevice->lock);
+			vCommandTimerWait((void *)pDevice, AUTHENTICATE_TIMEOUT/2);
+			return;
+		}
 		pDevice->byLinkWaitCount = 0;
-                    pDevice->eCommandState = WLAN_AUTHENTICATE_WAIT;
-                    vCommandTimerWait((void *)pDevice, AUTHENTICATE_TIMEOUT);
-                    spin_unlock_irq(&pDevice->lock);
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" Set eCommandState = WLAN_AUTHENTICATE_WAIT\n");
-                    return;
-                }
-            }
-            // if Adhoc mode
-            else if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
-                if (pMgmt->eCurrState == WMAC_STATE_JOINTED) {
-                    if (netif_queue_stopped(pDevice->dev)){
-                        netif_wake_queue(pDevice->dev);
-                    }
-                    pDevice->bLinkPass = true;
-
-                    pMgmt->sNodeDBTable[0].bActive = true;
-                    pMgmt->sNodeDBTable[0].uInActiveCount = 0;
-                    bClearBSSID_SCAN(pDevice);
-                }
-                else {
-                    // start own IBSS
-                    vMgrCreateOwnIBSS((void *)pDevice, &Status);
-                    if (Status != CMD_STATUS_SUCCESS){
-                        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " WLAN_CMD_IBSS_CREATE fail ! \n");
-                    }
-                    BSSvAddMulticastNode(pDevice);
-                }
-            }
-            // if SSID not found
-            else if (pMgmt->eCurrMode == WMAC_MODE_STANDBY) {
-                if (pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA ||
-                    pMgmt->eConfigMode == WMAC_CONFIG_AUTO) {
-                    // start own IBSS
-                    vMgrCreateOwnIBSS((void *)pDevice, &Status);
-                    if (Status != CMD_STATUS_SUCCESS){
-                        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" WLAN_CMD_IBSS_CREATE fail ! \n");
-                    }
-                    BSSvAddMulticastNode(pDevice);
-                    if (netif_queue_stopped(pDevice->dev)){
-                        netif_wake_queue(pDevice->dev);
-                    }
-                    pDevice->bLinkPass = true;
-                }
-                else {
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Disconnect SSID none\n");
-		  #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-                    // if(pDevice->bWPASuppWextEnabled == true)
-                        {
-                  	union iwreq_data  wrqu;
-                  	memset(&wrqu, 0, sizeof (wrqu));
-                          wrqu.ap_addr.sa_family = ARPHRD_ETHER;
-                  	printk("wireless_send_event--->SIOCGIWAP(disassociated:vMgrJoinBSSBegin Fail !!)\n");
-                  	wireless_send_event(pDevice->dev, SIOCGIWAP, &wrqu, NULL);
-                       }
-                    #endif
-
-                }
-            }
-            s_bCommandComplete(pDevice);
-            break;
-
-        case WLAN_AUTHENTICATE_WAIT :
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"eCommandState == WLAN_AUTHENTICATE_WAIT\n");
-            if (pMgmt->eCurrState == WMAC_STATE_AUTH) {
-                // Call mgr to begin the association
-                	pDevice->byLinkWaitCount = 0;
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"eCurrState == WMAC_STATE_AUTH\n");
-                vMgrAssocBeginSta((void *)pDevice, pMgmt, &Status);
-                if (Status == CMD_STATUS_SUCCESS) {
+		s_bCommandComplete(pDevice);
+		break;
+
+	case WLAN_ASSOCIATE_WAIT:
+		if (pMgmt->eCurrState == WMAC_STATE_ASSOC) {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "eCurrState == WMAC_STATE_ASSOC\n");
+			if (pDevice->ePSMode != WMAC_POWER_CAM) {
+				PSvEnablePowerSaving((void *)pDevice, pMgmt->wListenInterval);
+			}
+			if (pMgmt->eAuthenMode >= WMAC_AUTH_WPA) {
+				KeybRemoveAllKey(&(pDevice->sKey), pDevice->abyBSSID, pDevice->PortOffset);
+			}
+			pDevice->bLinkPass = true;
+			pDevice->byLinkWaitCount = 0;
+			pDevice->byReAssocCount = 0;
+			bClearBSSID_SCAN(pDevice);
+			if (pDevice->byFOETuning) {
+				BBvSetFOE(pDevice->PortOffset);
+				PSbSendNullPacket(pDevice);
+			}
+			if (netif_queue_stopped(pDevice->dev)) {
+				netif_wake_queue(pDevice->dev);
+			}
+#ifdef TxInSleep
+			if (pDevice->IsTxDataTrigger != false)   {    //TxDataTimer is not triggered at the first time
+				del_timer(&pDevice->sTimerTxData);
+				init_timer(&pDevice->sTimerTxData);
+				pDevice->sTimerTxData.data = (unsigned long) pDevice;
+				pDevice->sTimerTxData.function = (TimerFunction)BSSvSecondTxData;
+				pDevice->sTimerTxData.expires = RUN_AT(10*HZ);      //10s callback
+				pDevice->fTxDataInSleep = false;
+				pDevice->nTxDataTimeCout = 0;
+			} else {
+			}
+			pDevice->IsTxDataTrigger = true;
+			add_timer(&pDevice->sTimerTxData);
+#endif
+		} else if (pMgmt->eCurrState < WMAC_STATE_ASSOCPENDING) {
+			printk("WLAN_ASSOCIATE_WAIT:Association Fail???\n");
+		} else if (pDevice->byLinkWaitCount <= 4) {    //mike add:wait another 2 sec if associated_frame delay!
+			pDevice->byLinkWaitCount++;
+			printk("WLAN_ASSOCIATE_WAIT:wait %d times!!\n", pDevice->byLinkWaitCount);
+			spin_unlock_irq(&pDevice->lock);
+			vCommandTimerWait((void *)pDevice, ASSOCIATE_TIMEOUT/2);
+			return;
+		}
 		pDevice->byLinkWaitCount = 0;
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"eCommandState = WLAN_ASSOCIATE_WAIT\n");
-                    pDevice->eCommandState = WLAN_ASSOCIATE_WAIT;
-                    vCommandTimerWait((void *)pDevice, ASSOCIATE_TIMEOUT);
-                    spin_unlock_irq(&pDevice->lock);
-                    return;
-                }
-            }
-
-	else if(pMgmt->eCurrState < WMAC_STATE_AUTHPENDING) {
-               printk("WLAN_AUTHENTICATE_WAIT:Authen Fail???\n");
-	   }
-	   else  if(pDevice->byLinkWaitCount <= 4){    //mike add:wait another 2 sec if authenticated_frame delay!
-                pDevice->byLinkWaitCount ++;
-	       printk("WLAN_AUTHENTICATE_WAIT:wait %d times!!\n",pDevice->byLinkWaitCount);
-	       spin_unlock_irq(&pDevice->lock);
-	       vCommandTimerWait((void *)pDevice, AUTHENTICATE_TIMEOUT/2);
-	       return;
-	   }
-	          pDevice->byLinkWaitCount = 0;
-            s_bCommandComplete(pDevice);
-            break;
-
-        case WLAN_ASSOCIATE_WAIT :
-            if (pMgmt->eCurrState == WMAC_STATE_ASSOC) {
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"eCurrState == WMAC_STATE_ASSOC\n");
-                if (pDevice->ePSMode != WMAC_POWER_CAM) {
-                    PSvEnablePowerSaving((void *)pDevice, pMgmt->wListenInterval);
-                }
-                if (pMgmt->eAuthenMode >= WMAC_AUTH_WPA) {
-                    KeybRemoveAllKey(&(pDevice->sKey), pDevice->abyBSSID, pDevice->PortOffset);
-                }
-                pDevice->bLinkPass = true;
-                pDevice->byLinkWaitCount = 0;
-                pDevice->byReAssocCount = 0;
-                bClearBSSID_SCAN(pDevice);
-                if (pDevice->byFOETuning) {
-                    BBvSetFOE(pDevice->PortOffset);
-                    PSbSendNullPacket(pDevice);
-                }
-                if (netif_queue_stopped(pDevice->dev)){
-                    netif_wake_queue(pDevice->dev);
-                }
-	     #ifdef TxInSleep
-		 if(pDevice->IsTxDataTrigger != false)   {    //TxDataTimer is not triggered at the first time
-                     // printk("Re-initial TxDataTimer****\n");
-		    del_timer(&pDevice->sTimerTxData);
-                      init_timer(&pDevice->sTimerTxData);
-                      pDevice->sTimerTxData.data = (unsigned long) pDevice;
-                      pDevice->sTimerTxData.function = (TimerFunction)BSSvSecondTxData;
-                      pDevice->sTimerTxData.expires = RUN_AT(10*HZ);      //10s callback
-                      pDevice->fTxDataInSleep = false;
-                      pDevice->nTxDataTimeCout = 0;
-		 }
-		 else {
-		   // printk("mike:-->First time trigger TimerTxData InSleep\n");
-		 }
-		pDevice->IsTxDataTrigger = true;
-                add_timer(&pDevice->sTimerTxData);
-             #endif
-            }
-		   else if(pMgmt->eCurrState < WMAC_STATE_ASSOCPENDING) {
-               printk("WLAN_ASSOCIATE_WAIT:Association Fail???\n");
-	   }
-	   else  if(pDevice->byLinkWaitCount <= 4){    //mike add:wait another 2 sec if associated_frame delay!
-                pDevice->byLinkWaitCount ++;
-	       printk("WLAN_ASSOCIATE_WAIT:wait %d times!!\n",pDevice->byLinkWaitCount);
-	       spin_unlock_irq(&pDevice->lock);
-	       vCommandTimerWait((void *)pDevice, ASSOCIATE_TIMEOUT/2);
-	       return;
-	   }
-	          pDevice->byLinkWaitCount = 0;
-
-            s_bCommandComplete(pDevice);
-            break;
-
-        case WLAN_CMD_AP_MODE_START :
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"eCommandState == WLAN_CMD_AP_MODE_START\n");
-
-            if (pMgmt->eConfigMode == WMAC_CONFIG_AP) {
-                del_timer(&pMgmt->sTimerSecondCallback);
-                pMgmt->eCurrState = WMAC_STATE_IDLE;
-                pMgmt->eCurrMode = WMAC_MODE_STANDBY;
-                pDevice->bLinkPass = false;
-                if (pDevice->bEnableHostWEP == true)
-                    BSSvClearNodeDBTable(pDevice, 1);
-                else
-                    BSSvClearNodeDBTable(pDevice, 0);
-                pDevice->uAssocCount = 0;
-                pMgmt->eCurrState = WMAC_STATE_IDLE;
-                pDevice->bFixRate = false;
-
-                vMgrCreateOwnIBSS((void *)pDevice, &Status);
-                if (Status != CMD_STATUS_SUCCESS){
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " vMgrCreateOwnIBSS fail ! \n");
-                }
-                // alway turn off unicast bit
-                MACvRegBitsOff(pDevice->PortOffset, MAC_REG_RCR, RCR_UNICAST);
-                pDevice->byRxMode &= ~RCR_UNICAST;
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wcmd: rx_mode = %x\n", pDevice->byRxMode );
-                BSSvAddMulticastNode(pDevice);
-                if (netif_queue_stopped(pDevice->dev)){
-                    netif_wake_queue(pDevice->dev);
-                }
-                pDevice->bLinkPass = true;
-                add_timer(&pMgmt->sTimerSecondCallback);
-            }
-            s_bCommandComplete(pDevice);
-            break;
-
-        case WLAN_CMD_TX_PSPACKET_START :
-            // DTIM Multicast tx
-            if (pMgmt->sNodeDBTable[0].bRxPSPoll) {
-                while ((skb = skb_dequeue(&pMgmt->sNodeDBTable[0].sTxPSQueue)) != NULL) {
-                    if (skb_queue_empty(&pMgmt->sNodeDBTable[0].sTxPSQueue)) {
-                        pMgmt->abyPSTxMap[0] &= ~byMask[0];
-                        pDevice->bMoreData = false;
-                    }
-                    else {
-                        pDevice->bMoreData = true;
-                    }
-                    if (!device_dma0_xmit(pDevice, skb, 0)) {
-                        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Multicast ps tx fail \n");
-                    }
-                    pMgmt->sNodeDBTable[0].wEnQueueCnt--;
-                }
-            }
-
-            // PS nodes tx
-            for (ii = 1; ii < (MAX_NODE_NUM + 1); ii++) {
-                if (pMgmt->sNodeDBTable[ii].bActive &&
-                    pMgmt->sNodeDBTable[ii].bRxPSPoll) {
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Index=%d Enqueu Cnt= %d\n",
-                               ii, pMgmt->sNodeDBTable[ii].wEnQueueCnt);
-                    while ((skb = skb_dequeue(&pMgmt->sNodeDBTable[ii].sTxPSQueue)) != NULL) {
-                        if (skb_queue_empty(&pMgmt->sNodeDBTable[ii].sTxPSQueue)) {
-                            // clear tx map
-                            pMgmt->abyPSTxMap[pMgmt->sNodeDBTable[ii].wAID >> 3] &=
-                                    ~byMask[pMgmt->sNodeDBTable[ii].wAID & 7];
-                            pDevice->bMoreData = false;
-                        }
-                        else {
-                            pDevice->bMoreData = true;
-                        }
-                        if (!device_dma0_xmit(pDevice, skb, ii)) {
-                            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "sta ps tx fail \n");
-                        }
-                        pMgmt->sNodeDBTable[ii].wEnQueueCnt--;
-                        // check if sta ps enabled, and wait next pspoll.
-                        // if sta ps disable, then send all pending buffers.
-                        if (pMgmt->sNodeDBTable[ii].bPSEnable)
-                            break;
-                    }
-                    if (skb_queue_empty(&pMgmt->sNodeDBTable[ii].sTxPSQueue)) {
-                        // clear tx map
-                        pMgmt->abyPSTxMap[pMgmt->sNodeDBTable[ii].wAID >> 3] &=
-                                    ~byMask[pMgmt->sNodeDBTable[ii].wAID & 7];
-                        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Index=%d PS queue clear \n", ii);
-                    }
-                    pMgmt->sNodeDBTable[ii].bRxPSPoll = false;
-                }
-            }
-
-            s_bCommandComplete(pDevice);
-            break;
-
-
-        case WLAN_CMD_RADIO_START :
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"eCommandState == WLAN_CMD_RADIO_START\n");
-            if (pDevice->bRadioCmd == true)
-                CARDbRadioPowerOn(pDevice);
-            else
-                CARDbRadioPowerOff(pDevice);
-
-            s_bCommandComplete(pDevice);
-            break;
-
-
-        case WLAN_CMD_CHECK_BBSENSITIVITY_CHANGE :
-            //DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"eCommandState == WLAN_CMD_CHECK_BBSENSITIVITY_START\n");
-            // wait all TD complete
-            if (pDevice->iTDUsed[TYPE_AC0DMA] != 0){
-                vCommandTimerWait((void *)pDevice, 10);
-                spin_unlock_irq(&pDevice->lock);
-                return;
-            }
-            if (pDevice->iTDUsed[TYPE_TXDMA0] != 0){
-                vCommandTimerWait((void *)pDevice, 10);
-                spin_unlock_irq(&pDevice->lock);
-                return;
-            }
-            pDevice->byBBVGACurrent = pDevice->byBBVGANew;
-            BBvSetVGAGainOffset(pDevice, pDevice->byBBVGACurrent);
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"SetVGAGainOffset %02X\n", pDevice->byBBVGACurrent);
-            s_bCommandComplete(pDevice);
-            break;
-
-        default :
-            s_bCommandComplete(pDevice);
-            break;
-
-    } //switch
-    spin_unlock_irq(&pDevice->lock);
-    return;
 
+		s_bCommandComplete(pDevice);
+		break;
+
+	case WLAN_CMD_AP_MODE_START:
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "eCommandState == WLAN_CMD_AP_MODE_START\n");
+
+		if (pMgmt->eConfigMode == WMAC_CONFIG_AP) {
+			del_timer(&pMgmt->sTimerSecondCallback);
+			pMgmt->eCurrState = WMAC_STATE_IDLE;
+			pMgmt->eCurrMode = WMAC_MODE_STANDBY;
+			pDevice->bLinkPass = false;
+			if (pDevice->bEnableHostWEP == true)
+				BSSvClearNodeDBTable(pDevice, 1);
+			else
+				BSSvClearNodeDBTable(pDevice, 0);
+			pDevice->uAssocCount = 0;
+			pMgmt->eCurrState = WMAC_STATE_IDLE;
+			pDevice->bFixRate = false;
+
+			vMgrCreateOwnIBSS((void *)pDevice, &Status);
+			if (Status != CMD_STATUS_SUCCESS) {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " vMgrCreateOwnIBSS fail ! \n");
+			}
+			// alway turn off unicast bit
+			MACvRegBitsOff(pDevice->PortOffset, MAC_REG_RCR, RCR_UNICAST);
+			pDevice->byRxMode &= ~RCR_UNICAST;
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wcmd: rx_mode = %x\n", pDevice->byRxMode);
+			BSSvAddMulticastNode(pDevice);
+			if (netif_queue_stopped(pDevice->dev)) {
+				netif_wake_queue(pDevice->dev);
+			}
+			pDevice->bLinkPass = true;
+			add_timer(&pMgmt->sTimerSecondCallback);
+		}
+		s_bCommandComplete(pDevice);
+		break;
+
+	case WLAN_CMD_TX_PSPACKET_START:
+		// DTIM Multicast tx
+		if (pMgmt->sNodeDBTable[0].bRxPSPoll) {
+			while ((skb = skb_dequeue(&pMgmt->sNodeDBTable[0].sTxPSQueue)) != NULL) {
+				if (skb_queue_empty(&pMgmt->sNodeDBTable[0].sTxPSQueue)) {
+					pMgmt->abyPSTxMap[0] &= ~byMask[0];
+					pDevice->bMoreData = false;
+				} else {
+					pDevice->bMoreData = true;
+				}
+				if (!device_dma0_xmit(pDevice, skb, 0)) {
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Multicast ps tx fail \n");
+				}
+				pMgmt->sNodeDBTable[0].wEnQueueCnt--;
+			}
+		}
+
+		// PS nodes tx
+		for (ii = 1; ii < (MAX_NODE_NUM + 1); ii++) {
+			if (pMgmt->sNodeDBTable[ii].bActive &&
+			    pMgmt->sNodeDBTable[ii].bRxPSPoll) {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Index=%d Enqueu Cnt= %d\n",
+					ii, pMgmt->sNodeDBTable[ii].wEnQueueCnt);
+				while ((skb = skb_dequeue(&pMgmt->sNodeDBTable[ii].sTxPSQueue)) != NULL) {
+					if (skb_queue_empty(&pMgmt->sNodeDBTable[ii].sTxPSQueue)) {
+						// clear tx map
+						pMgmt->abyPSTxMap[pMgmt->sNodeDBTable[ii].wAID >> 3] &=
+							~byMask[pMgmt->sNodeDBTable[ii].wAID & 7];
+						pDevice->bMoreData = false;
+					} else {
+						pDevice->bMoreData = true;
+					}
+					if (!device_dma0_xmit(pDevice, skb, ii)) {
+						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "sta ps tx fail \n");
+					}
+					pMgmt->sNodeDBTable[ii].wEnQueueCnt--;
+					// check if sta ps enabled, and wait next pspoll.
+					// if sta ps disable, then send all pending buffers.
+					if (pMgmt->sNodeDBTable[ii].bPSEnable)
+						break;
+				}
+				if (skb_queue_empty(&pMgmt->sNodeDBTable[ii].sTxPSQueue)) {
+					// clear tx map
+					pMgmt->abyPSTxMap[pMgmt->sNodeDBTable[ii].wAID >> 3] &=
+						~byMask[pMgmt->sNodeDBTable[ii].wAID & 7];
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Index=%d PS queue clear \n", ii);
+				}
+				pMgmt->sNodeDBTable[ii].bRxPSPoll = false;
+			}
+		}
+
+		s_bCommandComplete(pDevice);
+		break;
+
+	case WLAN_CMD_RADIO_START:
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "eCommandState == WLAN_CMD_RADIO_START\n");
+		if (pDevice->bRadioCmd == true)
+			CARDbRadioPowerOn(pDevice);
+		else
+			CARDbRadioPowerOff(pDevice);
+
+		s_bCommandComplete(pDevice);
+		break;
+
+	case WLAN_CMD_CHECK_BBSENSITIVITY_CHANGE:
+		//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "eCommandState == WLAN_CMD_CHECK_BBSENSITIVITY_START\n");
+		// wait all TD complete
+		if (pDevice->iTDUsed[TYPE_AC0DMA] != 0) {
+			vCommandTimerWait((void *)pDevice, 10);
+			spin_unlock_irq(&pDevice->lock);
+			return;
+		}
+		if (pDevice->iTDUsed[TYPE_TXDMA0] != 0) {
+			vCommandTimerWait((void *)pDevice, 10);
+			spin_unlock_irq(&pDevice->lock);
+			return;
+		}
+		pDevice->byBBVGACurrent = pDevice->byBBVGANew;
+		BBvSetVGAGainOffset(pDevice, pDevice->byBBVGACurrent);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "SetVGAGainOffset %02X\n", pDevice->byBBVGACurrent);
+		s_bCommandComplete(pDevice);
+		break;
+
+	default:
+		s_bCommandComplete(pDevice);
+		break;
+
+	} //switch
+	spin_unlock_irq(&pDevice->lock);
+	return;
 }
 
-
 static
 bool
-s_bCommandComplete (
-    PSDevice pDevice
-    )
+s_bCommandComplete(
+	PSDevice pDevice
+)
 {
-    PWLAN_IE_SSID pSSID;
-    bool bRadioCmd = false;
-    //unsigned short wDeAuthenReason = 0;
-    bool bForceSCAN = true;
-    PSMgmtObject  pMgmt = pDevice->pMgmt;
-
-
-    pDevice->eCommandState = WLAN_CMD_IDLE;
-    if (pDevice->cbFreeCmdQueue == CMD_Q_SIZE) {
-        //Command Queue Empty
-        pDevice->bCmdRunning = false;
-        return true;
-    }
-    else {
-        pDevice->eCommand = pDevice->eCmdQueue[pDevice->uCmdDequeueIdx].eCmd;
-        pSSID = (PWLAN_IE_SSID)pDevice->eCmdQueue[pDevice->uCmdDequeueIdx].abyCmdDesireSSID;
-        bRadioCmd = pDevice->eCmdQueue[pDevice->uCmdDequeueIdx].bRadioCmd;
-        bForceSCAN = pDevice->eCmdQueue[pDevice->uCmdDequeueIdx].bForceSCAN;
-        ADD_ONE_WITH_WRAP_AROUND(pDevice->uCmdDequeueIdx, CMD_Q_SIZE);
-        pDevice->cbFreeCmdQueue++;
-        pDevice->bCmdRunning = true;
-        switch ( pDevice->eCommand ) {
-            case WLAN_CMD_BSSID_SCAN:
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"eCommandState= WLAN_CMD_BSSID_SCAN\n");
-                pDevice->eCommandState = WLAN_CMD_SCAN_START;
-                pMgmt->uScanChannel = 0;
-                if (pSSID->len != 0) {
-                    memcpy(pMgmt->abyScanSSID, pSSID, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-                } else {
-                    memset(pMgmt->abyScanSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-                }
+	PWLAN_IE_SSID pSSID;
+	bool bRadioCmd = false;
+	//unsigned short wDeAuthenReason = 0;
+	bool bForceSCAN = true;
+	PSMgmtObject  pMgmt = pDevice->pMgmt;
+
+	pDevice->eCommandState = WLAN_CMD_IDLE;
+	if (pDevice->cbFreeCmdQueue == CMD_Q_SIZE) {
+		//Command Queue Empty
+		pDevice->bCmdRunning = false;
+		return true;
+	} else {
+		pDevice->eCommand = pDevice->eCmdQueue[pDevice->uCmdDequeueIdx].eCmd;
+		pSSID = (PWLAN_IE_SSID)pDevice->eCmdQueue[pDevice->uCmdDequeueIdx].abyCmdDesireSSID;
+		bRadioCmd = pDevice->eCmdQueue[pDevice->uCmdDequeueIdx].bRadioCmd;
+		bForceSCAN = pDevice->eCmdQueue[pDevice->uCmdDequeueIdx].bForceSCAN;
+		ADD_ONE_WITH_WRAP_AROUND(pDevice->uCmdDequeueIdx, CMD_Q_SIZE);
+		pDevice->cbFreeCmdQueue++;
+		pDevice->bCmdRunning = true;
+		switch (pDevice->eCommand) {
+		case WLAN_CMD_BSSID_SCAN:
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "eCommandState= WLAN_CMD_BSSID_SCAN\n");
+			pDevice->eCommandState = WLAN_CMD_SCAN_START;
+			pMgmt->uScanChannel = 0;
+			if (pSSID->len != 0) {
+				memcpy(pMgmt->abyScanSSID, pSSID, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
+			} else {
+				memset(pMgmt->abyScanSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
+			}
 /*
-                if ((bForceSCAN == false) && (pDevice->bLinkPass == true)) {
-                    if ((pSSID->len == ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len) &&
-                        ( !memcmp(pSSID->abySSID, ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->abySSID, pSSID->len))) {
-                        pDevice->eCommandState = WLAN_CMD_IDLE;
-                    }
-                }
+  if ((bForceSCAN == false) && (pDevice->bLinkPass == true)) {
+  if ((pSSID->len == ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len) &&
+  (!memcmp(pSSID->abySSID, ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->abySSID, pSSID->len))) {
+  pDevice->eCommandState = WLAN_CMD_IDLE;
+  }
+  }
 */
-                break;
-            case WLAN_CMD_SSID:
-                pDevice->eCommandState = WLAN_CMD_SSID_START;
-                if (pSSID->len > WLAN_SSID_MAXLEN)
-                    pSSID->len = WLAN_SSID_MAXLEN;
-                if (pSSID->len != 0)
-                    memcpy(pDevice->pMgmt->abyDesireSSID, pSSID, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"eCommandState= WLAN_CMD_SSID_START\n");
-                break;
-            case WLAN_CMD_DISASSOCIATE:
-                pDevice->eCommandState = WLAN_CMD_DISASSOCIATE_START;
-                break;
-            case WLAN_CMD_RX_PSPOLL:
-                pDevice->eCommandState = WLAN_CMD_TX_PSPACKET_START;
-                break;
-            case WLAN_CMD_RUN_AP:
-                pDevice->eCommandState = WLAN_CMD_AP_MODE_START;
-                break;
-            case WLAN_CMD_RADIO:
-                pDevice->eCommandState = WLAN_CMD_RADIO_START;
-                pDevice->bRadioCmd = bRadioCmd;
-                break;
-            case WLAN_CMD_CHANGE_BBSENSITIVITY:
-                pDevice->eCommandState = WLAN_CMD_CHECK_BBSENSITIVITY_CHANGE;
-                break;
-
-            default:
-                break;
-
-        }
-
-        vCommandTimerWait((void *)pDevice, 0);
-    }
-
-    return true;
+			break;
+		case WLAN_CMD_SSID:
+			pDevice->eCommandState = WLAN_CMD_SSID_START;
+			if (pSSID->len > WLAN_SSID_MAXLEN)
+				pSSID->len = WLAN_SSID_MAXLEN;
+			if (pSSID->len != 0)
+				memcpy(pDevice->pMgmt->abyDesireSSID, pSSID, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "eCommandState= WLAN_CMD_SSID_START\n");
+			break;
+		case WLAN_CMD_DISASSOCIATE:
+			pDevice->eCommandState = WLAN_CMD_DISASSOCIATE_START;
+			break;
+		case WLAN_CMD_RX_PSPOLL:
+			pDevice->eCommandState = WLAN_CMD_TX_PSPACKET_START;
+			break;
+		case WLAN_CMD_RUN_AP:
+			pDevice->eCommandState = WLAN_CMD_AP_MODE_START;
+			break;
+		case WLAN_CMD_RADIO:
+			pDevice->eCommandState = WLAN_CMD_RADIO_START;
+			pDevice->bRadioCmd = bRadioCmd;
+			break;
+		case WLAN_CMD_CHANGE_BBSENSITIVITY:
+			pDevice->eCommandState = WLAN_CMD_CHECK_BBSENSITIVITY_CHANGE;
+			break;
+
+		default:
+			break;
+
+		}
+
+		vCommandTimerWait((void *)pDevice, 0);
+	}
+
+	return true;
 }
 
-
-
-bool bScheduleCommand (
-    void *hDeviceContext,
-    CMD_CODE    eCommand,
-    unsigned char *pbyItem0
-    )
+bool bScheduleCommand(
+	void *hDeviceContext,
+	CMD_CODE    eCommand,
+	unsigned char *pbyItem0
+)
 {
-    PSDevice        pDevice = (PSDevice)hDeviceContext;
-
-
-    if (pDevice->cbFreeCmdQueue == 0) {
-        return (false);
-    }
-    pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].eCmd = eCommand;
-    pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].bForceSCAN = true;
-    memset(pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].abyCmdDesireSSID, 0 , WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-
-    if (pbyItem0 != NULL) {
-        switch (eCommand) {
-
-            case WLAN_CMD_BSSID_SCAN:
-                memcpy(pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].abyCmdDesireSSID,
-                         pbyItem0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-                pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].bForceSCAN = false;
-                break;
-
-            case WLAN_CMD_SSID:
-                memcpy(pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].abyCmdDesireSSID,
-                         pbyItem0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-                break;
-
-            case WLAN_CMD_DISASSOCIATE:
-                pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].bNeedRadioOFF = *((int *)pbyItem0);
-                break;
+	PSDevice        pDevice = (PSDevice)hDeviceContext;
+
+	if (pDevice->cbFreeCmdQueue == 0) {
+		return false;
+	}
+	pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].eCmd = eCommand;
+	pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].bForceSCAN = true;
+	memset(pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].abyCmdDesireSSID, 0 , WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
+
+	if (pbyItem0 != NULL) {
+		switch (eCommand) {
+		case WLAN_CMD_BSSID_SCAN:
+			memcpy(pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].abyCmdDesireSSID,
+			       pbyItem0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
+			pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].bForceSCAN = false;
+			break;
+
+		case WLAN_CMD_SSID:
+			memcpy(pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].abyCmdDesireSSID,
+			       pbyItem0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
+			break;
+
+		case WLAN_CMD_DISASSOCIATE:
+			pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].bNeedRadioOFF = *((int *)pbyItem0);
+			break;
 /*
-            case WLAN_CMD_DEAUTH:
-                pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].wDeAuthenReason = *((unsigned short *)pbyItem0);
-                break;
+  case WLAN_CMD_DEAUTH:
+  pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].wDeAuthenReason = *((unsigned short *)pbyItem0);
+  break;
 */
 
-            case WLAN_CMD_RX_PSPOLL:
-                break;
+		case WLAN_CMD_RX_PSPOLL:
+			break;
 
-            case WLAN_CMD_RADIO:
-                pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].bRadioCmd = *((int *)pbyItem0);
-                break;
+		case WLAN_CMD_RADIO:
+			pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].bRadioCmd = *((int *)pbyItem0);
+			break;
 
-            case WLAN_CMD_CHANGE_BBSENSITIVITY:
-                pDevice->eCommandState = WLAN_CMD_CHECK_BBSENSITIVITY_CHANGE;
-                break;
+		case WLAN_CMD_CHANGE_BBSENSITIVITY:
+			pDevice->eCommandState = WLAN_CMD_CHECK_BBSENSITIVITY_CHANGE;
+			break;
 
-            default:
-                break;
-        }
-    }
+		default:
+			break;
+		}
+	}
 
-    ADD_ONE_WITH_WRAP_AROUND(pDevice->uCmdEnqueueIdx, CMD_Q_SIZE);
-    pDevice->cbFreeCmdQueue--;
-
-    if (pDevice->bCmdRunning == false) {
-        s_bCommandComplete(pDevice);
-    }
-    else {
-    }
-    return (true);
+	ADD_ONE_WITH_WRAP_AROUND(pDevice->uCmdEnqueueIdx, CMD_Q_SIZE);
+	pDevice->cbFreeCmdQueue--;
 
+	if (pDevice->bCmdRunning == false) {
+		s_bCommandComplete(pDevice);
+	} else {
+	}
+	return true;
 }
 
 /*
@@ -1038,87 +969,81 @@ bool bScheduleCommand (
  * Return Value: true if success; otherwise false
  *
  */
-bool bClearBSSID_SCAN (
-    void *hDeviceContext
-    )
+bool bClearBSSID_SCAN(
+	void *hDeviceContext
+)
 {
-    PSDevice        pDevice = (PSDevice)hDeviceContext;
-    unsigned int uCmdDequeueIdx = pDevice->uCmdDequeueIdx;
-    unsigned int ii;
-
-    if ((pDevice->cbFreeCmdQueue < CMD_Q_SIZE) && (uCmdDequeueIdx != pDevice->uCmdEnqueueIdx)) {
-        for (ii = 0; ii < (CMD_Q_SIZE - pDevice->cbFreeCmdQueue); ii ++) {
-            if (pDevice->eCmdQueue[uCmdDequeueIdx].eCmd == WLAN_CMD_BSSID_SCAN)
-                pDevice->eCmdQueue[uCmdDequeueIdx].eCmd = WLAN_CMD_IDLE;
-            ADD_ONE_WITH_WRAP_AROUND(uCmdDequeueIdx, CMD_Q_SIZE);
-            if (uCmdDequeueIdx == pDevice->uCmdEnqueueIdx)
-                break;
-        }
-    }
-    return true;
+	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	unsigned int uCmdDequeueIdx = pDevice->uCmdDequeueIdx;
+	unsigned int ii;
+
+	if ((pDevice->cbFreeCmdQueue < CMD_Q_SIZE) && (uCmdDequeueIdx != pDevice->uCmdEnqueueIdx)) {
+		for (ii = 0; ii < (CMD_Q_SIZE - pDevice->cbFreeCmdQueue); ii++) {
+			if (pDevice->eCmdQueue[uCmdDequeueIdx].eCmd == WLAN_CMD_BSSID_SCAN)
+				pDevice->eCmdQueue[uCmdDequeueIdx].eCmd = WLAN_CMD_IDLE;
+			ADD_ONE_WITH_WRAP_AROUND(uCmdDequeueIdx, CMD_Q_SIZE);
+			if (uCmdDequeueIdx == pDevice->uCmdEnqueueIdx)
+				break;
+		}
+	}
+	return true;
 }
 
 //mike add:reset command timer
 void
 vResetCommandTimer(
-    void *hDeviceContext
-    )
+	void *hDeviceContext
+)
 {
-  PSDevice        pDevice = (PSDevice)hDeviceContext;
-
-  //delete timer
-      del_timer(&pDevice->sTimerCommand);
-  //init timer
-      init_timer(&pDevice->sTimerCommand);
-    pDevice->sTimerCommand.data = (unsigned long) pDevice;
-    pDevice->sTimerCommand.function = (TimerFunction)vCommandTimer;
-    pDevice->sTimerCommand.expires = RUN_AT(HZ);
-    pDevice->cbFreeCmdQueue = CMD_Q_SIZE;
-    pDevice->uCmdDequeueIdx = 0;
-    pDevice->uCmdEnqueueIdx = 0;
-    pDevice->eCommandState = WLAN_CMD_IDLE;
-    pDevice->bCmdRunning = false;
-    pDevice->bCmdClear = false;
+	PSDevice        pDevice = (PSDevice)hDeviceContext;
+
+	//delete timer
+	del_timer(&pDevice->sTimerCommand);
+	//init timer
+	init_timer(&pDevice->sTimerCommand);
+	pDevice->sTimerCommand.data = (unsigned long) pDevice;
+	pDevice->sTimerCommand.function = (TimerFunction)vCommandTimer;
+	pDevice->sTimerCommand.expires = RUN_AT(HZ);
+	pDevice->cbFreeCmdQueue = CMD_Q_SIZE;
+	pDevice->uCmdDequeueIdx = 0;
+	pDevice->uCmdEnqueueIdx = 0;
+	pDevice->eCommandState = WLAN_CMD_IDLE;
+	pDevice->bCmdRunning = false;
+	pDevice->bCmdClear = false;
 }
 
-
 #ifdef TxInSleep
 void
 BSSvSecondTxData(
-    void *hDeviceContext
-    )
+	void *hDeviceContext
+)
 {
-  PSDevice        pDevice = (PSDevice)hDeviceContext;
-  PSMgmtObject  pMgmt = &(pDevice->sMgmtObj);
-  pDevice->nTxDataTimeCout++;
-
-  if(pDevice->nTxDataTimeCout<4)     //don't tx data if timer less than 40s
-    {
-     // printk("mike:%s-->no data Tx not exceed the desired Time as %d\n",__FUNCTION__,
-	//  	(int)pDevice->nTxDataTimeCout);
-     pDevice->sTimerTxData.expires = RUN_AT(10*HZ);      //10s callback
-     add_timer(&pDevice->sTimerTxData);
-      return;
-    }
-
-  spin_lock_irq(&pDevice->lock);
-  #if 1
-  if(((pDevice->bLinkPass ==true)&&(pMgmt->eAuthenMode < WMAC_AUTH_WPA)) ||  //open && sharekey linking
-      (pDevice->fWPA_Authened == true)) {   //wpa linking
- #else
-  if(pDevice->bLinkPass ==true) {
- #endif
-
-        //   printk("mike:%s-->InSleep Tx Data Procedure\n",__FUNCTION__);
-	  pDevice->fTxDataInSleep = true;
-	  PSbSendNullPacket(pDevice);      //send null packet
-	  pDevice->fTxDataInSleep = false;
-  	}
-  spin_unlock_irq(&pDevice->lock);
-
-  pDevice->sTimerTxData.expires = RUN_AT(10*HZ);      //10s callback
-  add_timer(&pDevice->sTimerTxData);
-  return;
-}
+	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	PSMgmtObject  pMgmt = &(pDevice->sMgmtObj);
+	pDevice->nTxDataTimeCout++;
+
+	if (pDevice->nTxDataTimeCout < 4)     //don't tx data if timer less than 40s
+	{
+		pDevice->sTimerTxData.expires = RUN_AT(10*HZ);      //10s callback
+		add_timer(&pDevice->sTimerTxData);
+		return;
+	}
+
+	spin_lock_irq(&pDevice->lock);
+#if 1
+	if (((pDevice->bLinkPass == true) && (pMgmt->eAuthenMode < WMAC_AUTH_WPA)) ||  //open && sharekey linking
+	   (pDevice->fWPA_Authened == true)) {   //wpa linking
+#else
+		if (pDevice->bLinkPass == true) {
+#endif
+			pDevice->fTxDataInSleep = true;
+			PSbSendNullPacket(pDevice);      //send null packet
+			pDevice->fTxDataInSleep = false;
+		}
+		spin_unlock_irq(&pDevice->lock);
+
+		pDevice->sTimerTxData.expires = RUN_AT(10*HZ);      //10s callback
+		add_timer(&pDevice->sTimerTxData);
+		return;
+	}
 #endif
-
diff --git a/drivers/staging/vt6655/wcmd.h b/drivers/staging/vt6655/wcmd.h
index 69d4fc55b84..2844476d5dc 100644
--- a/drivers/staging/vt6655/wcmd.h
+++ b/drivers/staging/vt6655/wcmd.h
@@ -35,111 +35,101 @@
 
 /*---------------------  Export Definitions -------------------------*/
 
-
-
 #define AUTHENTICATE_TIMEOUT   1000 //ms
 #define ASSOCIATE_TIMEOUT      1000 //ms
 
-
 // Command code
 typedef enum tagCMD_CODE {
-    WLAN_CMD_BSSID_SCAN,
-    WLAN_CMD_SSID,
-    WLAN_CMD_DISASSOCIATE,
-    WLAN_CMD_DEAUTH,
-    WLAN_CMD_RX_PSPOLL,
-    WLAN_CMD_RADIO,
-    WLAN_CMD_CHANGE_BBSENSITIVITY,
-    WLAN_CMD_SETPOWER,
-    WLAN_CMD_TBTT_WAKEUP,
-    WLAN_CMD_BECON_SEND,
-    WLAN_CMD_CHANGE_ANTENNA,
-    WLAN_CMD_REMOVE_ALLKEY,
-    WLAN_CMD_MAC_DISPOWERSAVING,
-    WLAN_CMD_11H_CHSW,
-    WLAN_CMD_RUN_AP
+	WLAN_CMD_BSSID_SCAN,
+	WLAN_CMD_SSID,
+	WLAN_CMD_DISASSOCIATE,
+	WLAN_CMD_DEAUTH,
+	WLAN_CMD_RX_PSPOLL,
+	WLAN_CMD_RADIO,
+	WLAN_CMD_CHANGE_BBSENSITIVITY,
+	WLAN_CMD_SETPOWER,
+	WLAN_CMD_TBTT_WAKEUP,
+	WLAN_CMD_BECON_SEND,
+	WLAN_CMD_CHANGE_ANTENNA,
+	WLAN_CMD_REMOVE_ALLKEY,
+	WLAN_CMD_MAC_DISPOWERSAVING,
+	WLAN_CMD_11H_CHSW,
+	WLAN_CMD_RUN_AP
 } CMD_CODE, *PCMD_CODE;
 
 #define CMD_Q_SIZE              32
 
 typedef enum tagCMD_STATUS {
-
-    CMD_STATUS_SUCCESS = 0,
-    CMD_STATUS_FAILURE,
-    CMD_STATUS_RESOURCES,
-    CMD_STATUS_TIMEOUT,
-    CMD_STATUS_PENDING
-
+	CMD_STATUS_SUCCESS = 0,
+	CMD_STATUS_FAILURE,
+	CMD_STATUS_RESOURCES,
+	CMD_STATUS_TIMEOUT,
+	CMD_STATUS_PENDING
 } CMD_STATUS, *PCMD_STATUS;
 
-
 typedef struct tagCMD_ITEM {
-    CMD_CODE eCmd;
-    unsigned char abyCmdDesireSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
-    bool bNeedRadioOFF;
-    unsigned short wDeAuthenReason;
-    bool bRadioCmd;
-    bool bForceSCAN;
+	CMD_CODE eCmd;
+	unsigned char abyCmdDesireSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
+	bool bNeedRadioOFF;
+	unsigned short wDeAuthenReason;
+	bool bRadioCmd;
+	bool bForceSCAN;
 } CMD_ITEM, *PCMD_ITEM;
 
 // Command state
 typedef enum tagCMD_STATE {
-    WLAN_CMD_SCAN_START,
-    WLAN_CMD_SCAN_END,
-    WLAN_CMD_DISASSOCIATE_START,
-    WLAN_CMD_SSID_START,
-    WLAN_AUTHENTICATE_WAIT,
-    WLAN_ASSOCIATE_WAIT,
-    WLAN_DISASSOCIATE_WAIT,
-    WLAN_CMD_TX_PSPACKET_START,
-    WLAN_CMD_AP_MODE_START,
-    WLAN_CMD_RADIO_START,
-    WLAN_CMD_CHECK_BBSENSITIVITY_CHANGE,
-    WLAN_CMD_IDLE
+	WLAN_CMD_SCAN_START,
+	WLAN_CMD_SCAN_END,
+	WLAN_CMD_DISASSOCIATE_START,
+	WLAN_CMD_SSID_START,
+	WLAN_AUTHENTICATE_WAIT,
+	WLAN_ASSOCIATE_WAIT,
+	WLAN_DISASSOCIATE_WAIT,
+	WLAN_CMD_TX_PSPACKET_START,
+	WLAN_CMD_AP_MODE_START,
+	WLAN_CMD_RADIO_START,
+	WLAN_CMD_CHECK_BBSENSITIVITY_CHANGE,
+	WLAN_CMD_IDLE
 } CMD_STATE, *PCMD_STATE;
 
-
-
 /*---------------------  Export Classes  ----------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
 
-
 /*---------------------  Export Types  ------------------------------*/
 
-
 /*---------------------  Export Functions  --------------------------*/
 void
 vResetCommandTimer(
-    void *hDeviceContext
-    );
+	void *hDeviceContext
+);
 
 void
-vCommandTimer (
-    void *hDeviceContext
-    );
+vCommandTimer(
+	void *hDeviceContext
+);
 
 bool bClearBSSID_SCAN(
-    void *hDeviceContext
-    );
+	void *hDeviceContext
+);
 
 bool
 bScheduleCommand(
-    void *hDeviceContext,
-    CMD_CODE    eCommand,
-    unsigned char *pbyItem0
-    );
+	void *hDeviceContext,
+	CMD_CODE    eCommand,
+	unsigned char *pbyItem0
+);
 
 void
 vCommandTimerWait(
-    void *hDeviceContext,
-    unsigned int MSecond
-    );
+	void *hDeviceContext,
+	unsigned int MSecond
+);
 #ifdef TxInSleep
 void
 BSSvSecondTxData(
-    void *hDeviceContext
-    );
+	void *hDeviceContext
+);
 #endif
 
 #endif //__WCMD_H__
diff --git a/drivers/staging/vt6655/wctl.c b/drivers/staging/vt6655/wctl.c
index c096583a772..9eb81b4eee8 100644
--- a/drivers/staging/vt6655/wctl.c
+++ b/drivers/staging/vt6655/wctl.c
@@ -48,8 +48,6 @@
 
 /*---------------------  Export Variables  --------------------------*/
 
-
-
 /*
  * Description:
  *      Scan Rx cache.  Return true if packet is duplicate, else
@@ -66,32 +64,31 @@
  *
  */
 
-bool WCTLbIsDuplicate (PSCache pCache, PS802_11Header pMACHeader)
+bool WCTLbIsDuplicate(PSCache pCache, PS802_11Header pMACHeader)
 {
-    unsigned int uIndex;
-    unsigned int ii;
-    PSCacheEntry    pCacheEntry;
-
-    if (IS_FC_RETRY(pMACHeader)) {
-
-        uIndex = pCache->uInPtr;
-        for (ii = 0; ii < DUPLICATE_RX_CACHE_LENGTH; ii++) {
-            pCacheEntry = &(pCache->asCacheEntry[uIndex]);
-            if ((pCacheEntry->wFmSequence == pMACHeader->wSeqCtl) &&
-                (!compare_ether_addr(&(pCacheEntry->abyAddr2[0]), &(pMACHeader->abyAddr2[0])))
-                ) {
-                /* Duplicate match */
-                return true;
-            }
-            ADD_ONE_WITH_WRAP_AROUND(uIndex, DUPLICATE_RX_CACHE_LENGTH);
-        }
-    }
-    /* Not fount in cache - insert */
-    pCacheEntry = &pCache->asCacheEntry[pCache->uInPtr];
-    pCacheEntry->wFmSequence = pMACHeader->wSeqCtl;
-    memcpy(&(pCacheEntry->abyAddr2[0]), &(pMACHeader->abyAddr2[0]), ETH_ALEN);
-    ADD_ONE_WITH_WRAP_AROUND(pCache->uInPtr, DUPLICATE_RX_CACHE_LENGTH);
-    return false;
+	unsigned int uIndex;
+	unsigned int ii;
+	PSCacheEntry    pCacheEntry;
+
+	if (IS_FC_RETRY(pMACHeader)) {
+		uIndex = pCache->uInPtr;
+		for (ii = 0; ii < DUPLICATE_RX_CACHE_LENGTH; ii++) {
+			pCacheEntry = &(pCache->asCacheEntry[uIndex]);
+			if ((pCacheEntry->wFmSequence == pMACHeader->wSeqCtl) &&
+			    (!compare_ether_addr(&(pCacheEntry->abyAddr2[0]), &(pMACHeader->abyAddr2[0])))
+) {
+				/* Duplicate match */
+				return true;
+			}
+			ADD_ONE_WITH_WRAP_AROUND(uIndex, DUPLICATE_RX_CACHE_LENGTH);
+		}
+	}
+	/* Not fount in cache - insert */
+	pCacheEntry = &pCache->asCacheEntry[pCache->uInPtr];
+	pCacheEntry->wFmSequence = pMACHeader->wSeqCtl;
+	memcpy(&(pCacheEntry->abyAddr2[0]), &(pMACHeader->abyAddr2[0]), ETH_ALEN);
+	ADD_ONE_WITH_WRAP_AROUND(pCache->uInPtr, DUPLICATE_RX_CACHE_LENGTH);
+	return false;
 }
 
 /*
@@ -108,22 +105,21 @@ bool WCTLbIsDuplicate (PSCache pCache, PS802_11Header pMACHeader)
  * Return Value: index number in Defragment Database
  *
  */
-unsigned int WCTLuSearchDFCB (PSDevice pDevice, PS802_11Header pMACHeader)
+unsigned int WCTLuSearchDFCB(PSDevice pDevice, PS802_11Header pMACHeader)
 {
-unsigned int ii;
-
-    for(ii=0;ii<pDevice->cbDFCB;ii++) {
-        if ((pDevice->sRxDFCB[ii].bInUse == true) &&
-            (!compare_ether_addr(&(pDevice->sRxDFCB[ii].abyAddr2[0]), &(pMACHeader->abyAddr2[0])))
-            ) {
-            //
-            return(ii);
-        }
-    }
-    return(pDevice->cbDFCB);
+	unsigned int ii;
+
+	for (ii = 0; ii < pDevice->cbDFCB; ii++) {
+		if ((pDevice->sRxDFCB[ii].bInUse == true) &&
+		    (!compare_ether_addr(&(pDevice->sRxDFCB[ii].abyAddr2[0]), &(pMACHeader->abyAddr2[0])))
+) {
+			//
+			return ii;
+		}
+	}
+	return pDevice->cbDFCB;
 }
 
-
 /*
  * Description:
  *      Insert received fragment packet in Defragment Database
@@ -138,27 +134,26 @@ unsigned int ii;
  * Return Value: index number in Defragment Database
  *
  */
-unsigned int WCTLuInsertDFCB (PSDevice pDevice, PS802_11Header pMACHeader)
+unsigned int WCTLuInsertDFCB(PSDevice pDevice, PS802_11Header pMACHeader)
 {
-unsigned int ii;
-
-    if (pDevice->cbFreeDFCB == 0)
-        return(pDevice->cbDFCB);
-    for(ii=0;ii<pDevice->cbDFCB;ii++) {
-        if (pDevice->sRxDFCB[ii].bInUse == false) {
-            pDevice->cbFreeDFCB--;
-            pDevice->sRxDFCB[ii].uLifetime = pDevice->dwMaxReceiveLifetime;
-            pDevice->sRxDFCB[ii].bInUse = true;
-            pDevice->sRxDFCB[ii].wSequence = (pMACHeader->wSeqCtl >> 4);
-            pDevice->sRxDFCB[ii].wFragNum = (pMACHeader->wSeqCtl & 0x000F);
-            memcpy(&(pDevice->sRxDFCB[ii].abyAddr2[0]), &(pMACHeader->abyAddr2[0]), ETH_ALEN);
-            return(ii);
-        }
-    }
-    return(pDevice->cbDFCB);
+	unsigned int ii;
+
+	if (pDevice->cbFreeDFCB == 0)
+		return pDevice->cbDFCB;
+	for (ii = 0; ii < pDevice->cbDFCB; ii++) {
+		if (pDevice->sRxDFCB[ii].bInUse == false) {
+			pDevice->cbFreeDFCB--;
+			pDevice->sRxDFCB[ii].uLifetime = pDevice->dwMaxReceiveLifetime;
+			pDevice->sRxDFCB[ii].bInUse = true;
+			pDevice->sRxDFCB[ii].wSequence = (pMACHeader->wSeqCtl >> 4);
+			pDevice->sRxDFCB[ii].wFragNum = (pMACHeader->wSeqCtl & 0x000F);
+			memcpy(&(pDevice->sRxDFCB[ii].abyAddr2[0]), &(pMACHeader->abyAddr2[0]), ETH_ALEN);
+			return ii;
+		}
+	}
+	return pDevice->cbDFCB;
 }
 
-
 /*
  * Description:
  *      Handle received fragment packet
@@ -175,76 +170,67 @@ unsigned int ii;
  * Return Value: true if it is valid fragment packet and we have resource to defragment; otherwise false
  *
  */
-bool WCTLbHandleFragment (PSDevice pDevice, PS802_11Header pMACHeader, unsigned int cbFrameLength, bool bWEP, bool bExtIV)
+bool WCTLbHandleFragment(PSDevice pDevice, PS802_11Header pMACHeader, unsigned int cbFrameLength, bool bWEP, bool bExtIV)
 {
-unsigned int uHeaderSize;
-
-
-    if (bWEP == true) {
-        uHeaderSize = 28;
-        if (bExtIV)
-        // ExtIV
-            uHeaderSize +=4;
-    }
-    else {
-        uHeaderSize = 24;
-    }
-
-    if (IS_FIRST_FRAGMENT_PKT(pMACHeader)) {
-        pDevice->uCurrentDFCBIdx = WCTLuSearchDFCB(pDevice, pMACHeader);
-        if (pDevice->uCurrentDFCBIdx < pDevice->cbDFCB) {
-            // duplicate, we must flush previous DCB
-            pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].uLifetime = pDevice->dwMaxReceiveLifetime;
-            pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wSequence = (pMACHeader->wSeqCtl >> 4);
-            pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wFragNum = (pMACHeader->wSeqCtl & 0x000F);
-        }
-        else {
-            pDevice->uCurrentDFCBIdx = WCTLuInsertDFCB(pDevice, pMACHeader);
-            if (pDevice->uCurrentDFCBIdx == pDevice->cbDFCB) {
-                return(false);
-            }
-        }
-        // reserve 4 byte to match MAC RX Buffer
-        pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer = (unsigned char *) (pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].skb->data + 4);
-        memcpy(pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer, pMACHeader, cbFrameLength);
-        pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].cbFrameLength = cbFrameLength;
-        pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer += cbFrameLength;
-        pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wFragNum++;
-        //DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "First pDevice->uCurrentDFCBIdx= %d\n", pDevice->uCurrentDFCBIdx);
-        return(false);
-    }
-    else {
-        pDevice->uCurrentDFCBIdx = WCTLuSearchDFCB(pDevice, pMACHeader);
-        if (pDevice->uCurrentDFCBIdx != pDevice->cbDFCB) {
-            if ((pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wSequence == (pMACHeader->wSeqCtl >> 4)) &&
-                (pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wFragNum == (pMACHeader->wSeqCtl & 0x000F)) &&
-                ((pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].cbFrameLength + cbFrameLength - uHeaderSize) < 2346)) {
-
-                memcpy(pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer, ((unsigned char *) (pMACHeader) + uHeaderSize), (cbFrameLength - uHeaderSize));
-                pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].cbFrameLength += (cbFrameLength - uHeaderSize);
-                pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer += (cbFrameLength - uHeaderSize);
-                pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wFragNum++;
-                //DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Second pDevice->uCurrentDFCBIdx= %d\n", pDevice->uCurrentDFCBIdx);
-            }
-            else {
-                // seq error or frag # error flush DFCB
-                pDevice->cbFreeDFCB++;
-                pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].bInUse = false;
-                return(false);
-            }
-        }
-        else {
-            return(false);
-        }
-        if (IS_LAST_FRAGMENT_PKT(pMACHeader)) {
-            //enq defragcontrolblock
-            pDevice->cbFreeDFCB++;
-            pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].bInUse = false;
-            //DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Last pDevice->uCurrentDFCBIdx= %d\n", pDevice->uCurrentDFCBIdx);
-            return(true);
-        }
-        return(false);
-    }
+	unsigned int uHeaderSize;
+
+	if (bWEP == true) {
+		uHeaderSize = 28;
+		if (bExtIV)
+			// ExtIV
+			uHeaderSize += 4;
+	} else {
+		uHeaderSize = 24;
+	}
+
+	if (IS_FIRST_FRAGMENT_PKT(pMACHeader)) {
+		pDevice->uCurrentDFCBIdx = WCTLuSearchDFCB(pDevice, pMACHeader);
+		if (pDevice->uCurrentDFCBIdx < pDevice->cbDFCB) {
+			// duplicate, we must flush previous DCB
+			pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].uLifetime = pDevice->dwMaxReceiveLifetime;
+			pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wSequence = (pMACHeader->wSeqCtl >> 4);
+			pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wFragNum = (pMACHeader->wSeqCtl & 0x000F);
+		} else {
+			pDevice->uCurrentDFCBIdx = WCTLuInsertDFCB(pDevice, pMACHeader);
+			if (pDevice->uCurrentDFCBIdx == pDevice->cbDFCB) {
+				return false;
+			}
+		}
+		// reserve 4 byte to match MAC RX Buffer
+		pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer = (unsigned char *)(pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].skb->data + 4);
+		memcpy(pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer, pMACHeader, cbFrameLength);
+		pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].cbFrameLength = cbFrameLength;
+		pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer += cbFrameLength;
+		pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wFragNum++;
+		//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "First pDevice->uCurrentDFCBIdx= %d\n", pDevice->uCurrentDFCBIdx);
+		return false;
+	} else {
+		pDevice->uCurrentDFCBIdx = WCTLuSearchDFCB(pDevice, pMACHeader);
+		if (pDevice->uCurrentDFCBIdx != pDevice->cbDFCB) {
+			if ((pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wSequence == (pMACHeader->wSeqCtl >> 4)) &&
+			    (pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wFragNum == (pMACHeader->wSeqCtl & 0x000F)) &&
+			    ((pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].cbFrameLength + cbFrameLength - uHeaderSize) < 2346)) {
+				memcpy(pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer, ((unsigned char *)(pMACHeader) + uHeaderSize), (cbFrameLength - uHeaderSize));
+				pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].cbFrameLength += (cbFrameLength - uHeaderSize);
+				pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer += (cbFrameLength - uHeaderSize);
+				pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wFragNum++;
+				//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Second pDevice->uCurrentDFCBIdx= %d\n", pDevice->uCurrentDFCBIdx);
+			} else {
+				// seq error or frag # error flush DFCB
+				pDevice->cbFreeDFCB++;
+				pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].bInUse = false;
+				return false;
+			}
+		} else {
+			return false;
+		}
+		if (IS_LAST_FRAGMENT_PKT(pMACHeader)) {
+			//enq defragcontrolblock
+			pDevice->cbFreeDFCB++;
+			pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].bInUse = false;
+			//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Last pDevice->uCurrentDFCBIdx= %d\n", pDevice->uCurrentDFCBIdx);
+			return true;
+		}
+		return false;
+	}
 }
-
-
diff --git a/drivers/staging/vt6655/wctl.h b/drivers/staging/vt6655/wctl.h
index a92bb6d2b3f..1ffb2734cfa 100644
--- a/drivers/staging/vt6655/wctl.h
+++ b/drivers/staging/vt6655/wctl.h
@@ -35,61 +35,60 @@
 
 /*---------------------  Export Definitions -------------------------*/
 
-#define IS_TYPE_DATA(pMACHeader)                                                        \
-    ((((PS802_11Header) pMACHeader)->wFrameCtl & TYPE_802_11_MASK) == TYPE_802_11_DATA)
+#define IS_TYPE_DATA(pMACHeader)					\
+	((((PS802_11Header) pMACHeader)->wFrameCtl & TYPE_802_11_MASK) == TYPE_802_11_DATA)
 
-#define IS_TYPE_MGMT(pMACHeader)                                                        \
-    ((((PS802_11Header) pMACHeader)->wFrameCtl & TYPE_802_11_MASK) == TYPE_802_11_MGMT)
+#define IS_TYPE_MGMT(pMACHeader)					\
+	((((PS802_11Header) pMACHeader)->wFrameCtl & TYPE_802_11_MASK) == TYPE_802_11_MGMT)
 
-#define IS_TYPE_CONTROL(pMACHeader)                                                     \
-    ((((PS802_11Header) pMACHeader)->wFrameCtl & TYPE_802_11_MASK) == TYPE_802_11_CTL)
+#define IS_TYPE_CONTROL(pMACHeader)					\
+	((((PS802_11Header) pMACHeader)->wFrameCtl & TYPE_802_11_MASK) == TYPE_802_11_CTL)
 
-#define IS_FC_MOREDATA(pMACHeader)                                                      \
-    ((((PS802_11Header) pMACHeader)->wFrameCtl & FC_MOREDATA) == FC_MOREDATA)
+#define IS_FC_MOREDATA(pMACHeader)					\
+	((((PS802_11Header) pMACHeader)->wFrameCtl & FC_MOREDATA) == FC_MOREDATA)
 
-#define IS_FC_POWERMGT(pMACHeader)                                                      \
-    ((((PS802_11Header) pMACHeader)->wFrameCtl & FC_POWERMGT) == FC_POWERMGT)
+#define IS_FC_POWERMGT(pMACHeader)					\
+	((((PS802_11Header) pMACHeader)->wFrameCtl & FC_POWERMGT) == FC_POWERMGT)
 
-#define IS_FC_RETRY(pMACHeader)                                                         \
-    ((((PS802_11Header) pMACHeader)->wFrameCtl & FC_RETRY) == FC_RETRY)
+#define IS_FC_RETRY(pMACHeader)						\
+	((((PS802_11Header) pMACHeader)->wFrameCtl & FC_RETRY) == FC_RETRY)
 
-#define IS_FC_WEP(pMACHeader)                                                           \
-    ((((PS802_11Header) pMACHeader)->wFrameCtl & FC_WEP) == FC_WEP)
+#define IS_FC_WEP(pMACHeader)						\
+	((((PS802_11Header) pMACHeader)->wFrameCtl & FC_WEP) == FC_WEP)
 
 #ifdef __BIG_ENDIAN
 
-#define IS_FRAGMENT_PKT(pMACHeader)                                                     \
-    (((((PS802_11Header) pMACHeader)->wFrameCtl & FC_MOREFRAG) != 0) |                  \
-     ((((PS802_11Header) pMACHeader)->wSeqCtl & 0x0F00) != 0))
+#define IS_FRAGMENT_PKT(pMACHeader)					\
+	(((((PS802_11Header) pMACHeader)->wFrameCtl & FC_MOREFRAG) != 0) | \
+	 ((((PS802_11Header) pMACHeader)->wSeqCtl & 0x0F00) != 0))
 
-#define IS_FIRST_FRAGMENT_PKT(pMACHeader)                                               \
-    ((((PS802_11Header) pMACHeader)->wSeqCtl & 0x0F00) == 0)
+#define IS_FIRST_FRAGMENT_PKT(pMACHeader)				\
+	((((PS802_11Header) pMACHeader)->wSeqCtl & 0x0F00) == 0)
 
 #else
 
-#define IS_FRAGMENT_PKT(pMACHeader)                                                     \
-    (((((PS802_11Header) pMACHeader)->wFrameCtl & FC_MOREFRAG) != 0) |                  \
-     ((((PS802_11Header) pMACHeader)->wSeqCtl & 0x000F) != 0))
+#define IS_FRAGMENT_PKT(pMACHeader)					\
+	(((((PS802_11Header) pMACHeader)->wFrameCtl & FC_MOREFRAG) != 0) | \
+	 ((((PS802_11Header) pMACHeader)->wSeqCtl & 0x000F) != 0))
 
-#define IS_FIRST_FRAGMENT_PKT(pMACHeader)                                               \
-    ((((PS802_11Header) pMACHeader)->wSeqCtl & 0x000F) == 0)
+#define IS_FIRST_FRAGMENT_PKT(pMACHeader)				\
+	((((PS802_11Header) pMACHeader)->wSeqCtl & 0x000F) == 0)
 
 #endif//#ifdef __BIG_ENDIAN
 
-#define IS_LAST_FRAGMENT_PKT(pMACHeader)                                                \
-    ((((PS802_11Header) pMACHeader)->wFrameCtl & FC_MOREFRAG) == 0)
+#define IS_LAST_FRAGMENT_PKT(pMACHeader)				\
+	((((PS802_11Header) pMACHeader)->wFrameCtl & FC_MOREFRAG) == 0)
 
-#define IS_CTL_PSPOLL(pMACHeader)                                                       \
-    ((((PS802_11Header) pMACHeader)->wFrameCtl & TYPE_SUBTYPE_MASK) == TYPE_CTL_PSPOLL)
-
-
-#define ADD_ONE_WITH_WRAP_AROUND(uVar, uModulo) {   \
-    if ((uVar) >= ((uModulo) - 1))                  \
-        (uVar) = 0;                                 \
-    else                                            \
-        (uVar)++;                                   \
-}
+#define IS_CTL_PSPOLL(pMACHeader)					\
+	((((PS802_11Header) pMACHeader)->wFrameCtl & TYPE_SUBTYPE_MASK) == TYPE_CTL_PSPOLL)
 
+#define ADD_ONE_WITH_WRAP_AROUND(uVar, uModulo)		\
+do {							\
+	if ((uVar) >= ((uModulo) - 1))			\
+		(uVar) = 0;				\
+	else						\
+		(uVar)++;				\
+} while (0)
 
 /*---------------------  Export Classes  ----------------------------*/
 
@@ -99,11 +98,8 @@
 
 bool WCTLbIsDuplicate(PSCache pCache, PS802_11Header pMACHeader);
 bool WCTLbHandleFragment(PSDevice pDevice, PS802_11Header pMACHeader,
-		unsigned int cbFrameLength, bool bWEP, bool bExtIV);
+			 unsigned int cbFrameLength, bool bWEP, bool bExtIV);
 unsigned int WCTLuSearchDFCB(PSDevice pDevice, PS802_11Header pMACHeader);
 unsigned int WCTLuInsertDFCB(PSDevice pDevice, PS802_11Header pMACHeader);
 
 #endif // __WCTL_H__
-
-
-
diff --git a/drivers/staging/vt6655/wmgr.c b/drivers/staging/vt6655/wmgr.c
index b08a611a184..9938813f997 100644
--- a/drivers/staging/vt6655/wmgr.c
+++ b/drivers/staging/vt6655/wmgr.c
@@ -84,260 +84,251 @@
 
 /*---------------------  Static Definitions -------------------------*/
 
-
-
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
-static int          msglevel                =MSG_LEVEL_INFO;
+static int msglevel = MSG_LEVEL_INFO;
 //static int          msglevel                =MSG_LEVEL_DEBUG;
 
 /*---------------------  Static Functions  --------------------------*/
 //2008-8-4 <add> by chester
 static bool ChannelExceedZoneType(
-    PSDevice pDevice,
-    unsigned char byCurrChannel
-    );
+	PSDevice pDevice,
+	unsigned char byCurrChannel
+);
 
 // Association/diassociation functions
 static
 PSTxMgmtPacket
 s_MgrMakeAssocRequest(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    unsigned char *pDAddr,
-    unsigned short wCurrCapInfo,
-    unsigned short wListenInterval,
-    PWLAN_IE_SSID pCurrSSID,
-    PWLAN_IE_SUPP_RATES pCurrRates,
-    PWLAN_IE_SUPP_RATES pCurrExtSuppRates
-    );
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	unsigned char *pDAddr,
+	unsigned short wCurrCapInfo,
+	unsigned short wListenInterval,
+	PWLAN_IE_SSID pCurrSSID,
+	PWLAN_IE_SUPP_RATES pCurrRates,
+	PWLAN_IE_SUPP_RATES pCurrExtSuppRates
+);
 
 static
 void
 s_vMgrRxAssocRequest(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    PSRxMgmtPacket pRxPacket,
-    unsigned int uNodeIndex
-    );
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	PSRxMgmtPacket pRxPacket,
+	unsigned int uNodeIndex
+);
 
 static
 PSTxMgmtPacket
 s_MgrMakeReAssocRequest(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    unsigned char *pDAddr,
-    unsigned short wCurrCapInfo,
-    unsigned short wListenInterval,
-    PWLAN_IE_SSID pCurrSSID,
-    PWLAN_IE_SUPP_RATES pCurrRates,
-    PWLAN_IE_SUPP_RATES pCurrExtSuppRates
-    );
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	unsigned char *pDAddr,
+	unsigned short wCurrCapInfo,
+	unsigned short wListenInterval,
+	PWLAN_IE_SSID pCurrSSID,
+	PWLAN_IE_SUPP_RATES pCurrRates,
+	PWLAN_IE_SUPP_RATES pCurrExtSuppRates
+);
 
 static
 void
 s_vMgrRxAssocResponse(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    PSRxMgmtPacket pRxPacket,
-    bool bReAssocType
-    );
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	PSRxMgmtPacket pRxPacket,
+	bool bReAssocType
+);
 
 static
 void
 s_vMgrRxDisassociation(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    PSRxMgmtPacket pRxPacket
-    );
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	PSRxMgmtPacket pRxPacket
+);
 
 // Authentication/deauthen functions
 static
 void
 s_vMgrRxAuthenSequence_1(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    PWLAN_FR_AUTHEN pFrame
-    );
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	PWLAN_FR_AUTHEN pFrame
+);
 
 static
 void
 s_vMgrRxAuthenSequence_2(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    PWLAN_FR_AUTHEN pFrame
-    );
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	PWLAN_FR_AUTHEN pFrame
+);
 
 static
 void
 s_vMgrRxAuthenSequence_3(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    PWLAN_FR_AUTHEN pFrame
-    );
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	PWLAN_FR_AUTHEN pFrame
+);
 
 static
 void
 s_vMgrRxAuthenSequence_4(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    PWLAN_FR_AUTHEN pFrame
-    );
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	PWLAN_FR_AUTHEN pFrame
+);
 
 static
 void
 s_vMgrRxAuthentication(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    PSRxMgmtPacket pRxPacket
-    );
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	PSRxMgmtPacket pRxPacket
+);
 
 static
 void
 s_vMgrRxDeauthentication(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    PSRxMgmtPacket pRxPacket
-    );
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	PSRxMgmtPacket pRxPacket
+);
 
 // Scan functions
 // probe request/response functions
 static
 void
 s_vMgrRxProbeRequest(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    PSRxMgmtPacket pRxPacket
-    );
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	PSRxMgmtPacket pRxPacket
+);
 
 static
 void
 s_vMgrRxProbeResponse(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    PSRxMgmtPacket pRxPacket
-    );
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	PSRxMgmtPacket pRxPacket
+);
 
 // beacon functions
 static
 void
 s_vMgrRxBeacon(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    PSRxMgmtPacket pRxPacket,
-    bool bInScan
-    );
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	PSRxMgmtPacket pRxPacket,
+	bool bInScan
+);
 
 static
 void
 s_vMgrFormatTIM(
-    PSMgmtObject pMgmt,
-    PWLAN_IE_TIM pTIM
-    );
+	PSMgmtObject pMgmt,
+	PWLAN_IE_TIM pTIM
+);
 
 static
 PSTxMgmtPacket
 s_MgrMakeBeacon(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    unsigned short wCurrCapInfo,
-    unsigned short wCurrBeaconPeriod,
-    unsigned int uCurrChannel,
-    unsigned short wCurrATIMWinodw,
-    PWLAN_IE_SSID pCurrSSID,
-    unsigned char *pCurrBSSID,
-    PWLAN_IE_SUPP_RATES pCurrSuppRates,
-    PWLAN_IE_SUPP_RATES pCurrExtSuppRates
-    );
-
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	unsigned short wCurrCapInfo,
+	unsigned short wCurrBeaconPeriod,
+	unsigned int uCurrChannel,
+	unsigned short wCurrATIMWinodw,
+	PWLAN_IE_SSID pCurrSSID,
+	unsigned char *pCurrBSSID,
+	PWLAN_IE_SUPP_RATES pCurrSuppRates,
+	PWLAN_IE_SUPP_RATES pCurrExtSuppRates
+);
 
 // Association response
 static
 PSTxMgmtPacket
 s_MgrMakeAssocResponse(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    unsigned short wCurrCapInfo,
-    unsigned short wAssocStatus,
-    unsigned short wAssocAID,
-    unsigned char *pDstAddr,
-    PWLAN_IE_SUPP_RATES pCurrSuppRates,
-    PWLAN_IE_SUPP_RATES pCurrExtSuppRates
-    );
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	unsigned short wCurrCapInfo,
+	unsigned short wAssocStatus,
+	unsigned short wAssocAID,
+	unsigned char *pDstAddr,
+	PWLAN_IE_SUPP_RATES pCurrSuppRates,
+	PWLAN_IE_SUPP_RATES pCurrExtSuppRates
+);
 
 // ReAssociation response
 static
 PSTxMgmtPacket
 s_MgrMakeReAssocResponse(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    unsigned short wCurrCapInfo,
-    unsigned short wAssocStatus,
-    unsigned short wAssocAID,
-    unsigned char *pDstAddr,
-    PWLAN_IE_SUPP_RATES pCurrSuppRates,
-    PWLAN_IE_SUPP_RATES pCurrExtSuppRates
-    );
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	unsigned short wCurrCapInfo,
+	unsigned short wAssocStatus,
+	unsigned short wAssocAID,
+	unsigned char *pDstAddr,
+	PWLAN_IE_SUPP_RATES pCurrSuppRates,
+	PWLAN_IE_SUPP_RATES pCurrExtSuppRates
+);
 
 // Probe response
 static
 PSTxMgmtPacket
 s_MgrMakeProbeResponse(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    unsigned short wCurrCapInfo,
-    unsigned short wCurrBeaconPeriod,
-    unsigned int uCurrChannel,
-    unsigned short wCurrATIMWinodw,
-    unsigned char *pDstAddr,
-    PWLAN_IE_SSID pCurrSSID,
-    unsigned char *pCurrBSSID,
-    PWLAN_IE_SUPP_RATES pCurrSuppRates,
-    PWLAN_IE_SUPP_RATES pCurrExtSuppRates,
-    unsigned char byPHYType
-    );
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	unsigned short wCurrCapInfo,
+	unsigned short wCurrBeaconPeriod,
+	unsigned int uCurrChannel,
+	unsigned short wCurrATIMWinodw,
+	unsigned char *pDstAddr,
+	PWLAN_IE_SSID pCurrSSID,
+	unsigned char *pCurrBSSID,
+	PWLAN_IE_SUPP_RATES pCurrSuppRates,
+	PWLAN_IE_SUPP_RATES pCurrExtSuppRates,
+	unsigned char byPHYType
+);
 
 // received status
 static
 void
 s_vMgrLogStatus(
-    PSMgmtObject pMgmt,
-    unsigned short wStatus
-    );
-
+	PSMgmtObject pMgmt,
+	unsigned short wStatus
+);
 
 static
 void
-s_vMgrSynchBSS (
-    PSDevice      pDevice,
-    unsigned int uBSSMode,
-    PKnownBSS     pCurr,
-    PCMD_STATUS  pStatus
-    );
-
+s_vMgrSynchBSS(
+	PSDevice      pDevice,
+	unsigned int uBSSMode,
+	PKnownBSS     pCurr,
+	PCMD_STATUS  pStatus
+);
 
 static bool
-s_bCipherMatch (
-    PKnownBSS                        pBSSNode,
-    NDIS_802_11_ENCRYPTION_STATUS    EncStatus,
-    unsigned char *pbyCCSPK,
-    unsigned char *pbyCCSGK
-    );
-
- static void  Encyption_Rebuild(
-    PSDevice pDevice,
-    PKnownBSS pCurr
- );
-
-
+s_bCipherMatch(
+	PKnownBSS                        pBSSNode,
+	NDIS_802_11_ENCRYPTION_STATUS    EncStatus,
+	unsigned char *pbyCCSPK,
+	unsigned char *pbyCCSGK
+);
+
+static void  Encyption_Rebuild(
+	PSDevice pDevice,
+	PKnownBSS pCurr
+);
 
 /*---------------------  Export Variables  --------------------------*/
 
-
 /*---------------------  Export Functions  --------------------------*/
 
-
 /*+
  *
  * Routine Description:
@@ -346,32 +337,31 @@ s_bCipherMatch (
  * Return Value:
  *    Ndis_staus.
  *
--*/
+ -*/
 
 void
 vMgrObjectInit(
-    void *hDeviceContext
-    )
+	void *hDeviceContext
+)
 {
-    PSDevice     pDevice = (PSDevice)hDeviceContext;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    int ii;
-
-
-    pMgmt->pbyPSPacketPool = &pMgmt->byPSPacketPool[0];
-    pMgmt->pbyMgmtPacketPool = &pMgmt->byMgmtPacketPool[0];
-    pMgmt->uCurrChannel = pDevice->uChannel;
-    for(ii=0;ii<WLAN_BSSID_LEN;ii++) {
-        pMgmt->abyDesireBSSID[ii] = 0xFF;
-    }
-    pMgmt->sAssocInfo.AssocInfo.Length = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
-    //memset(pMgmt->abyDesireSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN +1);
-    pMgmt->byCSSPK = KEY_CTL_NONE;
-    pMgmt->byCSSGK = KEY_CTL_NONE;
-    pMgmt->wIBSSBeaconPeriod = DEFAULT_IBSS_BI;
-    BSSvClearBSSList((void *)pDevice, false);
-
-    return;
+	PSDevice     pDevice = (PSDevice)hDeviceContext;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	int ii;
+
+	pMgmt->pbyPSPacketPool = &pMgmt->byPSPacketPool[0];
+	pMgmt->pbyMgmtPacketPool = &pMgmt->byMgmtPacketPool[0];
+	pMgmt->uCurrChannel = pDevice->uChannel;
+	for (ii = 0; ii < WLAN_BSSID_LEN; ii++) {
+		pMgmt->abyDesireBSSID[ii] = 0xFF;
+	}
+	pMgmt->sAssocInfo.AssocInfo.Length = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
+	//memset(pMgmt->abyDesireSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN +1);
+	pMgmt->byCSSPK = KEY_CTL_NONE;
+	pMgmt->byCSSGK = KEY_CTL_NONE;
+	pMgmt->wIBSSBeaconPeriod = DEFAULT_IBSS_BI;
+	BSSvClearBSSList((void *)pDevice, false);
+
+	return;
 }
 
 /*+
@@ -382,45 +372,42 @@ vMgrObjectInit(
  * Return Value:
  *    Ndis_staus.
  *
--*/
+ -*/
 
 void
 vMgrTimerInit(
-    void *hDeviceContext
-    )
+	void *hDeviceContext
+)
 {
-    PSDevice     pDevice = (PSDevice)hDeviceContext;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-
-
-    init_timer(&pMgmt->sTimerSecondCallback);
-    pMgmt->sTimerSecondCallback.data = (unsigned long) pDevice;
-    pMgmt->sTimerSecondCallback.function = (TimerFunction)BSSvSecondCallBack;
-    pMgmt->sTimerSecondCallback.expires = RUN_AT(HZ);
-
-    init_timer(&pDevice->sTimerCommand);
-    pDevice->sTimerCommand.data = (unsigned long) pDevice;
-    pDevice->sTimerCommand.function = (TimerFunction)vCommandTimer;
-    pDevice->sTimerCommand.expires = RUN_AT(HZ);
-
-   #ifdef TxInSleep
-    init_timer(&pDevice->sTimerTxData);
-    pDevice->sTimerTxData.data = (unsigned long) pDevice;
-    pDevice->sTimerTxData.function = (TimerFunction)BSSvSecondTxData;
-    pDevice->sTimerTxData.expires = RUN_AT(10*HZ);      //10s callback
-    pDevice->fTxDataInSleep = false;
-    pDevice->IsTxDataTrigger = false;
-    pDevice->nTxDataTimeCout = 0;
-   #endif
-
-    pDevice->cbFreeCmdQueue = CMD_Q_SIZE;
-    pDevice->uCmdDequeueIdx = 0;
-    pDevice->uCmdEnqueueIdx = 0;
-
-    return;
-}
+	PSDevice     pDevice = (PSDevice)hDeviceContext;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+
+	init_timer(&pMgmt->sTimerSecondCallback);
+	pMgmt->sTimerSecondCallback.data = (unsigned long) pDevice;
+	pMgmt->sTimerSecondCallback.function = (TimerFunction)BSSvSecondCallBack;
+	pMgmt->sTimerSecondCallback.expires = RUN_AT(HZ);
+
+	init_timer(&pDevice->sTimerCommand);
+	pDevice->sTimerCommand.data = (unsigned long) pDevice;
+	pDevice->sTimerCommand.function = (TimerFunction)vCommandTimer;
+	pDevice->sTimerCommand.expires = RUN_AT(HZ);
+
+#ifdef TxInSleep
+	init_timer(&pDevice->sTimerTxData);
+	pDevice->sTimerTxData.data = (unsigned long) pDevice;
+	pDevice->sTimerTxData.function = (TimerFunction)BSSvSecondTxData;
+	pDevice->sTimerTxData.expires = RUN_AT(10*HZ);      //10s callback
+	pDevice->fTxDataInSleep = false;
+	pDevice->IsTxDataTrigger = false;
+	pDevice->nTxDataTimeCout = 0;
+#endif
 
+	pDevice->cbFreeCmdQueue = CMD_Q_SIZE;
+	pDevice->uCmdDequeueIdx = 0;
+	pDevice->uCmdEnqueueIdx = 0;
 
+	return;
+}
 
 /*+
  *
@@ -430,25 +417,24 @@ vMgrTimerInit(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 void
 vMgrObjectReset(
-    void *hDeviceContext
-    )
+	void *hDeviceContext
+)
 {
-    PSDevice         pDevice = (PSDevice)hDeviceContext;
-    PSMgmtObject        pMgmt = pDevice->pMgmt;
+	PSDevice         pDevice = (PSDevice)hDeviceContext;
+	PSMgmtObject        pMgmt = pDevice->pMgmt;
 
-    pMgmt->eCurrMode = WMAC_MODE_STANDBY;
-    pMgmt->eCurrState = WMAC_STATE_IDLE;
-    pDevice->bEnablePSMode = false;
-    // TODO: timer
+	pMgmt->eCurrMode = WMAC_MODE_STANDBY;
+	pMgmt->eCurrState = WMAC_STATE_IDLE;
+	pDevice->bEnablePSMode = false;
+	// TODO: timer
 
-    return;
+	return;
 }
 
-
 /*+
  *
  * Routine Description:
@@ -458,74 +444,66 @@ vMgrObjectReset(
  * Return Value:
  *    None.
  *
--*/
-
+ -*/
 
 void
 vMgrAssocBeginSta(
-    void *hDeviceContext,
-    PSMgmtObject pMgmt,
-    PCMD_STATUS pStatus
-    )
+	void *hDeviceContext,
+	PSMgmtObject pMgmt,
+	PCMD_STATUS pStatus
+)
 {
-    PSDevice             pDevice = (PSDevice)hDeviceContext;
-    PSTxMgmtPacket          pTxPacket;
-
-
-    pMgmt->wCurrCapInfo = 0;
-    pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_ESS(1);
-    if (pDevice->bEncryptionEnable) {
-        pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_PRIVACY(1);
-    }
-    // always allow receive short preamble
-    //if (pDevice->byPreambleType == 1) {
-    //    pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
-    //}
-    pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
-    if (pMgmt->wListenInterval == 0)
-        pMgmt->wListenInterval = 1;    // at least one.
-
-    // ERP Phy (802.11g) should support short preamble.
-    if (pMgmt->eCurrentPHYMode == PHY_TYPE_11G) {
-        pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
-        if (CARDbIsShorSlotTime(pMgmt->pAdapter) == true) {
-            pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTSLOTTIME(1);
-        }
-    } else if (pMgmt->eCurrentPHYMode == PHY_TYPE_11B) {
-        if (CARDbIsShortPreamble(pMgmt->pAdapter) == true) {
-            pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
-        }
-    }
-    if (pMgmt->b11hEnable == true)
-        pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SPECTRUMMNG(1);
-
-    /* build an assocreq frame and send it */
-    pTxPacket = s_MgrMakeAssocRequest
-                (
-                  pDevice,
-                  pMgmt,
-                  pMgmt->abyCurrBSSID,
-                  pMgmt->wCurrCapInfo,
-                  pMgmt->wListenInterval,
-                  (PWLAN_IE_SSID)pMgmt->abyCurrSSID,
-                  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-                  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates
-                );
-
-    if (pTxPacket != NULL ){
-        /* send the frame */
-        *pStatus = csMgmt_xmit(pDevice, pTxPacket);
-        if (*pStatus == CMD_STATUS_PENDING) {
-            pMgmt->eCurrState = WMAC_STATE_ASSOCPENDING;
-            *pStatus = CMD_STATUS_SUCCESS;
-        }
-    }
-    else
-        *pStatus = CMD_STATUS_RESOURCES;
-
-    return ;
-}
+	PSDevice             pDevice = (PSDevice)hDeviceContext;
+	PSTxMgmtPacket          pTxPacket;
+
+	pMgmt->wCurrCapInfo = 0;
+	pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_ESS(1);
+	if (pDevice->bEncryptionEnable) {
+		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_PRIVACY(1);
+	}
+	pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
+	if (pMgmt->wListenInterval == 0)
+		pMgmt->wListenInterval = 1;    // at least one.
+
+	// ERP Phy (802.11g) should support short preamble.
+	if (pMgmt->eCurrentPHYMode == PHY_TYPE_11G) {
+		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
+		if (CARDbIsShorSlotTime(pMgmt->pAdapter) == true) {
+			pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTSLOTTIME(1);
+		}
+	} else if (pMgmt->eCurrentPHYMode == PHY_TYPE_11B) {
+		if (CARDbIsShortPreamble(pMgmt->pAdapter) == true) {
+			pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
+		}
+	}
+	if (pMgmt->b11hEnable == true)
+		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SPECTRUMMNG(1);
+
+	/* build an assocreq frame and send it */
+	pTxPacket = s_MgrMakeAssocRequest
+		(
+			pDevice,
+			pMgmt,
+			pMgmt->abyCurrBSSID,
+			pMgmt->wCurrCapInfo,
+			pMgmt->wListenInterval,
+			(PWLAN_IE_SSID)pMgmt->abyCurrSSID,
+			(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
+			(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates
+);
+
+	if (pTxPacket != NULL) {
+		/* send the frame */
+		*pStatus = csMgmt_xmit(pDevice, pTxPacket);
+		if (*pStatus == CMD_STATUS_PENDING) {
+			pMgmt->eCurrState = WMAC_STATE_ASSOCPENDING;
+			*pStatus = CMD_STATUS_SUCCESS;
+		}
+	} else
+		*pStatus = CMD_STATUS_RESOURCES;
 
+	return;
+}
 
 /*+
  *
@@ -535,75 +513,66 @@ vMgrAssocBeginSta(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 void
 vMgrReAssocBeginSta(
-    void *hDeviceContext,
-    PSMgmtObject pMgmt,
-    PCMD_STATUS pStatus
-    )
+	void *hDeviceContext,
+	PSMgmtObject pMgmt,
+	PCMD_STATUS pStatus
+)
 {
-    PSDevice             pDevice = (PSDevice)hDeviceContext;
-    PSTxMgmtPacket          pTxPacket;
-
-
-
-    pMgmt->wCurrCapInfo = 0;
-    pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_ESS(1);
-    if (pDevice->bEncryptionEnable) {
-        pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_PRIVACY(1);
-    }
-
-    //if (pDevice->byPreambleType == 1) {
-    //    pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
-    //}
-    pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
-
-    if (pMgmt->wListenInterval == 0)
-        pMgmt->wListenInterval = 1;    // at least one.
-
-
-    // ERP Phy (802.11g) should support short preamble.
-    if (pMgmt->eCurrentPHYMode == PHY_TYPE_11G) {
-        pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
-        if (CARDbIsShorSlotTime(pMgmt->pAdapter) == true) {
-            pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTSLOTTIME(1);
-        }
-    } else if (pMgmt->eCurrentPHYMode == PHY_TYPE_11B) {
-        if (CARDbIsShortPreamble(pMgmt->pAdapter) == true) {
-            pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
-        }
-    }
-    if (pMgmt->b11hEnable == true)
-        pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SPECTRUMMNG(1);
-
-
-    pTxPacket = s_MgrMakeReAssocRequest
-                (
-                  pDevice,
-                  pMgmt,
-                  pMgmt->abyCurrBSSID,
-                  pMgmt->wCurrCapInfo,
-                  pMgmt->wListenInterval,
-                  (PWLAN_IE_SSID)pMgmt->abyCurrSSID,
-                  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-                  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates
-                );
-
-    if (pTxPacket != NULL ){
-        /* send the frame */
-        *pStatus = csMgmt_xmit(pDevice, pTxPacket);
-        if (*pStatus != CMD_STATUS_PENDING) {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Reassociation tx failed.\n");
-        }
-        else {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Reassociation tx sending.\n");
-        }
-    }
-
-
-    return ;
+	PSDevice             pDevice = (PSDevice)hDeviceContext;
+	PSTxMgmtPacket          pTxPacket;
+
+	pMgmt->wCurrCapInfo = 0;
+	pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_ESS(1);
+	if (pDevice->bEncryptionEnable) {
+		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_PRIVACY(1);
+	}
+
+	pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
+
+	if (pMgmt->wListenInterval == 0)
+		pMgmt->wListenInterval = 1;    // at least one.
+
+	// ERP Phy (802.11g) should support short preamble.
+	if (pMgmt->eCurrentPHYMode == PHY_TYPE_11G) {
+		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
+		if (CARDbIsShorSlotTime(pMgmt->pAdapter) == true) {
+			pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTSLOTTIME(1);
+		}
+	} else if (pMgmt->eCurrentPHYMode == PHY_TYPE_11B) {
+		if (CARDbIsShortPreamble(pMgmt->pAdapter) == true) {
+			pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
+		}
+	}
+	if (pMgmt->b11hEnable == true)
+		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SPECTRUMMNG(1);
+
+	pTxPacket = s_MgrMakeReAssocRequest
+		(
+			pDevice,
+			pMgmt,
+			pMgmt->abyCurrBSSID,
+			pMgmt->wCurrCapInfo,
+			pMgmt->wListenInterval,
+			(PWLAN_IE_SSID)pMgmt->abyCurrSSID,
+			(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
+			(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates
+);
+
+	if (pTxPacket != NULL) {
+		/* send the frame */
+		*pStatus = csMgmt_xmit(pDevice, pTxPacket);
+		if (*pStatus != CMD_STATUS_PENDING) {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Reassociation tx failed.\n");
+		} else {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Reassociation tx sending.\n");
+		}
+	}
+
+	return;
 }
 
 /*+
@@ -614,59 +583,57 @@ vMgrReAssocBeginSta(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 void
 vMgrDisassocBeginSta(
-    void *hDeviceContext,
-    PSMgmtObject pMgmt,
-    unsigned char *abyDestAddress,
-    unsigned short wReason,
-    PCMD_STATUS pStatus
-    )
+	void *hDeviceContext,
+	PSMgmtObject pMgmt,
+	unsigned char *abyDestAddress,
+	unsigned short wReason,
+	PCMD_STATUS pStatus
+)
 {
-    PSDevice            pDevice = (PSDevice)hDeviceContext;
-    PSTxMgmtPacket      pTxPacket = NULL;
-    WLAN_FR_DISASSOC    sFrame;
-
-    pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
-    memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_DISASSOC_FR_MAXLEN);
-    pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-
-    // Setup the sFrame structure
-    sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
-    sFrame.len = WLAN_DISASSOC_FR_MAXLEN;
-
-    // format fixed field frame structure
-    vMgrEncodeDisassociation(&sFrame);
-
-    // Setup the header
-    sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
-        (
-        WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
-        WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_DISASSOC)
-        ));
-
-    memcpy( sFrame.pHdr->sA3.abyAddr1, abyDestAddress, WLAN_ADDR_LEN);
-    memcpy( sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-    memcpy( sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-
-    // Set reason code
-    *(sFrame.pwReason) = cpu_to_le16(wReason);
-    pTxPacket->cbMPDULen = sFrame.len;
-    pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
-
-    // send the frame
-    *pStatus = csMgmt_xmit(pDevice, pTxPacket);
-    if (*pStatus == CMD_STATUS_PENDING) {
-        pMgmt->eCurrState = WMAC_STATE_IDLE;
-        *pStatus = CMD_STATUS_SUCCESS;
-    }
-
-    return;
-}
-
+	PSDevice            pDevice = (PSDevice)hDeviceContext;
+	PSTxMgmtPacket      pTxPacket = NULL;
+	WLAN_FR_DISASSOC    sFrame;
+
+	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
+	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_DISASSOC_FR_MAXLEN);
+	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
+
+	// Setup the sFrame structure
+	sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
+	sFrame.len = WLAN_DISASSOC_FR_MAXLEN;
+
+	// format fixed field frame structure
+	vMgrEncodeDisassociation(&sFrame);
+
+	// Setup the header
+	sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
+		(
+			WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
+			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_DISASSOC)
+));
+
+	memcpy(sFrame.pHdr->sA3.abyAddr1, abyDestAddress, WLAN_ADDR_LEN);
+	memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
+	memcpy(sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
+
+	// Set reason code
+	*(sFrame.pwReason) = cpu_to_le16(wReason);
+	pTxPacket->cbMPDULen = sFrame.len;
+	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
+
+	// send the frame
+	*pStatus = csMgmt_xmit(pDevice, pTxPacket);
+	if (*pStatus == CMD_STATUS_PENDING) {
+		pMgmt->eCurrState = WMAC_STATE_IDLE;
+		*pStatus = CMD_STATUS_SUCCESS;
+	}
 
+	return;
+}
 
 /*+
  *
@@ -676,153 +643,147 @@ vMgrDisassocBeginSta(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 static
 void
 s_vMgrRxAssocRequest(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    PSRxMgmtPacket pRxPacket,
-    unsigned int uNodeIndex
-    )
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	PSRxMgmtPacket pRxPacket,
+	unsigned int uNodeIndex
+)
 {
-    WLAN_FR_ASSOCREQ    sFrame;
-    CMD_STATUS          Status;
-    PSTxMgmtPacket      pTxPacket;
-    unsigned short wAssocStatus = 0;
-    unsigned short wAssocAID = 0;
-    unsigned int uRateLen = WLAN_RATES_MAXLEN;
-    unsigned char abyCurrSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
-    unsigned char abyCurrExtSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
-
-
-    if (pMgmt->eCurrMode != WMAC_MODE_ESS_AP)
-        return;
-    //  node index not found
-    if (!uNodeIndex)
-        return;
-
-    //check if node is authenticated
-    //decode the frame
-    memset(&sFrame, 0, sizeof(WLAN_FR_ASSOCREQ));
-    memset(abyCurrSuppRates, 0, WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1);
-    memset(abyCurrExtSuppRates, 0, WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1);
-    sFrame.len = pRxPacket->cbMPDULen;
-    sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
-
-    vMgrDecodeAssocRequest(&sFrame);
-
-    if (pMgmt->sNodeDBTable[uNodeIndex].eNodeState >= NODE_AUTH) {
-        pMgmt->sNodeDBTable[uNodeIndex].eNodeState = NODE_ASSOC;
-        pMgmt->sNodeDBTable[uNodeIndex].wCapInfo = cpu_to_le16(*sFrame.pwCapInfo);
-        pMgmt->sNodeDBTable[uNodeIndex].wListenInterval = cpu_to_le16(*sFrame.pwListenInterval);
-        pMgmt->sNodeDBTable[uNodeIndex].bPSEnable =
-                WLAN_GET_FC_PWRMGT(sFrame.pHdr->sA3.wFrameCtl) ? true : false;
-        // Todo: check sta basic rate, if ap can't support, set status code
-        if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
-            uRateLen = WLAN_RATES_MAXLEN_11B;
-        }
-        abyCurrSuppRates[0] = WLAN_EID_SUPP_RATES;
-        abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pSuppRates,
-                                         (PWLAN_IE_SUPP_RATES)abyCurrSuppRates,
-                                         uRateLen);
-        abyCurrExtSuppRates[0] = WLAN_EID_EXTSUPP_RATES;
-        if (pDevice->eCurrentPHYType == PHY_TYPE_11G) {
-            abyCurrExtSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pExtSuppRates,
-                                                (PWLAN_IE_SUPP_RATES)abyCurrExtSuppRates,
-                                                uRateLen);
-        } else {
-            abyCurrExtSuppRates[1] = 0;
-        }
-
-
-        RATEvParseMaxRate((void *)pDevice,
-                           (PWLAN_IE_SUPP_RATES)abyCurrSuppRates,
-                           (PWLAN_IE_SUPP_RATES)abyCurrExtSuppRates,
-                           false, // do not change our basic rate
-                           &(pMgmt->sNodeDBTable[uNodeIndex].wMaxBasicRate),
-                           &(pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate),
-                           &(pMgmt->sNodeDBTable[uNodeIndex].wSuppRate),
-                           &(pMgmt->sNodeDBTable[uNodeIndex].byTopCCKBasicRate),
-                           &(pMgmt->sNodeDBTable[uNodeIndex].byTopOFDMBasicRate)
-                          );
-
-        // set max tx rate
-        pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate =
-                pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate;
+	WLAN_FR_ASSOCREQ    sFrame;
+	CMD_STATUS          Status;
+	PSTxMgmtPacket      pTxPacket;
+	unsigned short wAssocStatus = 0;
+	unsigned short wAssocAID = 0;
+	unsigned int uRateLen = WLAN_RATES_MAXLEN;
+	unsigned char abyCurrSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
+	unsigned char abyCurrExtSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
+
+	if (pMgmt->eCurrMode != WMAC_MODE_ESS_AP)
+		return;
+	//  node index not found
+	if (!uNodeIndex)
+		return;
+
+	//check if node is authenticated
+	//decode the frame
+	memset(&sFrame, 0, sizeof(WLAN_FR_ASSOCREQ));
+	memset(abyCurrSuppRates, 0, WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1);
+	memset(abyCurrExtSuppRates, 0, WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1);
+	sFrame.len = pRxPacket->cbMPDULen;
+	sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
+
+	vMgrDecodeAssocRequest(&sFrame);
+
+	if (pMgmt->sNodeDBTable[uNodeIndex].eNodeState >= NODE_AUTH) {
+		pMgmt->sNodeDBTable[uNodeIndex].eNodeState = NODE_ASSOC;
+		pMgmt->sNodeDBTable[uNodeIndex].wCapInfo = cpu_to_le16(*sFrame.pwCapInfo);
+		pMgmt->sNodeDBTable[uNodeIndex].wListenInterval = cpu_to_le16(*sFrame.pwListenInterval);
+		pMgmt->sNodeDBTable[uNodeIndex].bPSEnable =
+			WLAN_GET_FC_PWRMGT(sFrame.pHdr->sA3.wFrameCtl) ? true : false;
+		// Todo: check sta basic rate, if ap can't support, set status code
+		if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
+			uRateLen = WLAN_RATES_MAXLEN_11B;
+		}
+		abyCurrSuppRates[0] = WLAN_EID_SUPP_RATES;
+		abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pSuppRates,
+						 (PWLAN_IE_SUPP_RATES)abyCurrSuppRates,
+						 uRateLen);
+		abyCurrExtSuppRates[0] = WLAN_EID_EXTSUPP_RATES;
+		if (pDevice->eCurrentPHYType == PHY_TYPE_11G) {
+			abyCurrExtSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pExtSuppRates,
+							    (PWLAN_IE_SUPP_RATES)abyCurrExtSuppRates,
+							    uRateLen);
+		} else {
+			abyCurrExtSuppRates[1] = 0;
+		}
+
+		RATEvParseMaxRate((void *)pDevice,
+				  (PWLAN_IE_SUPP_RATES)abyCurrSuppRates,
+				  (PWLAN_IE_SUPP_RATES)abyCurrExtSuppRates,
+				  false, // do not change our basic rate
+				  &(pMgmt->sNodeDBTable[uNodeIndex].wMaxBasicRate),
+				  &(pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate),
+				  &(pMgmt->sNodeDBTable[uNodeIndex].wSuppRate),
+				  &(pMgmt->sNodeDBTable[uNodeIndex].byTopCCKBasicRate),
+				  &(pMgmt->sNodeDBTable[uNodeIndex].byTopOFDMBasicRate)
+);
+
+		// set max tx rate
+		pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate =
+			pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate;
 #ifdef	PLICE_DEBUG
-	printk("RxAssocRequest:wTxDataRate is %d\n",pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate);
+		printk("RxAssocRequest:wTxDataRate is %d\n", pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate);
 #endif
 		// Todo: check sta preamble, if ap can't support, set status code
-        pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble =
-                WLAN_GET_CAP_INFO_SHORTPREAMBLE(*sFrame.pwCapInfo);
-        pMgmt->sNodeDBTable[uNodeIndex].bShortSlotTime =
-                WLAN_GET_CAP_INFO_SHORTSLOTTIME(*sFrame.pwCapInfo);
-        pMgmt->sNodeDBTable[uNodeIndex].wAID = (unsigned short)uNodeIndex;
-        wAssocStatus = WLAN_MGMT_STATUS_SUCCESS;
-        wAssocAID = (unsigned short)uNodeIndex;
-        // check if ERP support
-        if(pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate > RATE_11M)
-           pMgmt->sNodeDBTable[uNodeIndex].bERPExist = true;
-
-        if (pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate <= RATE_11M) {
-            // B only STA join
-            pDevice->bProtectMode = true;
-            pDevice->bNonERPPresent = true;
-        }
-        if (pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble == false) {
-            pDevice->bBarkerPreambleMd = true;
-        }
-
-        DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Associate AID= %d \n", wAssocAID);
-        DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "MAC=%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X \n",
-                   sFrame.pHdr->sA3.abyAddr2[0],
-                   sFrame.pHdr->sA3.abyAddr2[1],
-                   sFrame.pHdr->sA3.abyAddr2[2],
-                   sFrame.pHdr->sA3.abyAddr2[3],
-                   sFrame.pHdr->sA3.abyAddr2[4],
-                   sFrame.pHdr->sA3.abyAddr2[5]
-                  ) ;
-        DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Max Support rate = %d \n",
-                   pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate);
-    }//else { TODO: received STA under state1 handle }
-    else {
-        return;
-    }
-
-
-    // assoc response reply..
-    pTxPacket = s_MgrMakeAssocResponse
-                (
-                  pDevice,
-                  pMgmt,
-                  pMgmt->wCurrCapInfo,
-                  wAssocStatus,
-                  wAssocAID,
-                  sFrame.pHdr->sA3.abyAddr2,
-                  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-                  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates
-                );
-    if (pTxPacket != NULL ){
-
-        if (pDevice->bEnableHostapd) {
-            return;
-        }
-        /* send the frame */
-        Status = csMgmt_xmit(pDevice, pTxPacket);
-        if (Status != CMD_STATUS_PENDING) {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Assoc response tx failed\n");
-        }
-        else {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Assoc response tx sending..\n");
-        }
-
-    }
-
-    return;
-}
+		pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble =
+			WLAN_GET_CAP_INFO_SHORTPREAMBLE(*sFrame.pwCapInfo);
+		pMgmt->sNodeDBTable[uNodeIndex].bShortSlotTime =
+			WLAN_GET_CAP_INFO_SHORTSLOTTIME(*sFrame.pwCapInfo);
+		pMgmt->sNodeDBTable[uNodeIndex].wAID = (unsigned short)uNodeIndex;
+		wAssocStatus = WLAN_MGMT_STATUS_SUCCESS;
+		wAssocAID = (unsigned short)uNodeIndex;
+		// check if ERP support
+		if (pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate > RATE_11M)
+			pMgmt->sNodeDBTable[uNodeIndex].bERPExist = true;
+
+		if (pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate <= RATE_11M) {
+			// B only STA join
+			pDevice->bProtectMode = true;
+			pDevice->bNonERPPresent = true;
+		}
+		if (pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble == false) {
+			pDevice->bBarkerPreambleMd = true;
+		}
+
+		DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Associate AID= %d \n", wAssocAID);
+		DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "MAC=%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X \n",
+			sFrame.pHdr->sA3.abyAddr2[0],
+			sFrame.pHdr->sA3.abyAddr2[1],
+			sFrame.pHdr->sA3.abyAddr2[2],
+			sFrame.pHdr->sA3.abyAddr2[3],
+			sFrame.pHdr->sA3.abyAddr2[4],
+			sFrame.pHdr->sA3.abyAddr2[5]
+			);
+		DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Max Support rate = %d \n",
+			pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate);
+	}//else { TODO: received STA under state1 handle }
+	else {
+		return;
+	}
+
+	// assoc response reply..
+	pTxPacket = s_MgrMakeAssocResponse
+		(
+			pDevice,
+			pMgmt,
+			pMgmt->wCurrCapInfo,
+			wAssocStatus,
+			wAssocAID,
+			sFrame.pHdr->sA3.abyAddr2,
+			(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
+			(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates
+);
+	if (pTxPacket != NULL) {
+		if (pDevice->bEnableHostapd) {
+			return;
+		}
+		/* send the frame */
+		Status = csMgmt_xmit(pDevice, pTxPacket);
+		if (Status != CMD_STATUS_PENDING) {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Assoc response tx failed\n");
+		} else {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Assoc response tx sending..\n");
+		}
 
+	}
+
+	return;
+}
 
 /*+
  *
@@ -838,147 +799,143 @@ s_vMgrRxAssocRequest(
  *
  * Return Value: None.
  *
--*/
+ -*/
 
 static
 void
 s_vMgrRxReAssocRequest(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    PSRxMgmtPacket pRxPacket,
-    unsigned int uNodeIndex
-    )
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	PSRxMgmtPacket pRxPacket,
+	unsigned int uNodeIndex
+)
 {
-    WLAN_FR_REASSOCREQ    sFrame;
-    CMD_STATUS          Status;
-    PSTxMgmtPacket      pTxPacket;
-    unsigned short wAssocStatus = 0;
-    unsigned short wAssocAID = 0;
-    unsigned int	uRateLen = WLAN_RATES_MAXLEN;
-    unsigned char abyCurrSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
-    unsigned char abyCurrExtSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
-
-    if (pMgmt->eCurrMode != WMAC_MODE_ESS_AP)
-        return;
-    //  node index not found
-    if (!uNodeIndex)
-        return;
-    //check if node is authenticated
-    //decode the frame
-    memset(&sFrame, 0, sizeof(WLAN_FR_REASSOCREQ));
-    sFrame.len = pRxPacket->cbMPDULen;
-    sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
-    vMgrDecodeReassocRequest(&sFrame);
-
-    if (pMgmt->sNodeDBTable[uNodeIndex].eNodeState >= NODE_AUTH) {
-        pMgmt->sNodeDBTable[uNodeIndex].eNodeState = NODE_ASSOC;
-        pMgmt->sNodeDBTable[uNodeIndex].wCapInfo = cpu_to_le16(*sFrame.pwCapInfo);
-        pMgmt->sNodeDBTable[uNodeIndex].wListenInterval = cpu_to_le16(*sFrame.pwListenInterval);
-        pMgmt->sNodeDBTable[uNodeIndex].bPSEnable =
-                WLAN_GET_FC_PWRMGT(sFrame.pHdr->sA3.wFrameCtl) ? true : false;
-        // Todo: check sta basic rate, if ap can't support, set status code
-
-        if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
-            uRateLen = WLAN_RATES_MAXLEN_11B;
-        }
-
-        abyCurrSuppRates[0] = WLAN_EID_SUPP_RATES;
-        abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pSuppRates,
-                                         (PWLAN_IE_SUPP_RATES)abyCurrSuppRates,
-                                         uRateLen);
-        abyCurrExtSuppRates[0] = WLAN_EID_EXTSUPP_RATES;
-        if (pDevice->eCurrentPHYType == PHY_TYPE_11G) {
-            abyCurrExtSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pExtSuppRates,
-                                                (PWLAN_IE_SUPP_RATES)abyCurrExtSuppRates,
-                                                uRateLen);
-        } else {
-            abyCurrExtSuppRates[1] = 0;
-        }
-
-
-        RATEvParseMaxRate((void *)pDevice,
-                          (PWLAN_IE_SUPP_RATES)abyCurrSuppRates,
-                          (PWLAN_IE_SUPP_RATES)abyCurrExtSuppRates,
-                           false, // do not change our basic rate
-                           &(pMgmt->sNodeDBTable[uNodeIndex].wMaxBasicRate),
-                           &(pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate),
-                           &(pMgmt->sNodeDBTable[uNodeIndex].wSuppRate),
-                           &(pMgmt->sNodeDBTable[uNodeIndex].byTopCCKBasicRate),
-                           &(pMgmt->sNodeDBTable[uNodeIndex].byTopOFDMBasicRate)
-                          );
-
-        // set max tx rate
-        pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate =
-                pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate;
+	WLAN_FR_REASSOCREQ    sFrame;
+	CMD_STATUS          Status;
+	PSTxMgmtPacket      pTxPacket;
+	unsigned short wAssocStatus = 0;
+	unsigned short wAssocAID = 0;
+	unsigned int	uRateLen = WLAN_RATES_MAXLEN;
+	unsigned char abyCurrSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
+	unsigned char abyCurrExtSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
+
+	if (pMgmt->eCurrMode != WMAC_MODE_ESS_AP)
+		return;
+	//  node index not found
+	if (!uNodeIndex)
+		return;
+	//check if node is authenticated
+	//decode the frame
+	memset(&sFrame, 0, sizeof(WLAN_FR_REASSOCREQ));
+	sFrame.len = pRxPacket->cbMPDULen;
+	sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
+	vMgrDecodeReassocRequest(&sFrame);
+
+	if (pMgmt->sNodeDBTable[uNodeIndex].eNodeState >= NODE_AUTH) {
+		pMgmt->sNodeDBTable[uNodeIndex].eNodeState = NODE_ASSOC;
+		pMgmt->sNodeDBTable[uNodeIndex].wCapInfo = cpu_to_le16(*sFrame.pwCapInfo);
+		pMgmt->sNodeDBTable[uNodeIndex].wListenInterval = cpu_to_le16(*sFrame.pwListenInterval);
+		pMgmt->sNodeDBTable[uNodeIndex].bPSEnable =
+			WLAN_GET_FC_PWRMGT(sFrame.pHdr->sA3.wFrameCtl) ? true : false;
+		// Todo: check sta basic rate, if ap can't support, set status code
+
+		if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
+			uRateLen = WLAN_RATES_MAXLEN_11B;
+		}
+
+		abyCurrSuppRates[0] = WLAN_EID_SUPP_RATES;
+		abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pSuppRates,
+						 (PWLAN_IE_SUPP_RATES)abyCurrSuppRates,
+						 uRateLen);
+		abyCurrExtSuppRates[0] = WLAN_EID_EXTSUPP_RATES;
+		if (pDevice->eCurrentPHYType == PHY_TYPE_11G) {
+			abyCurrExtSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pExtSuppRates,
+							    (PWLAN_IE_SUPP_RATES)abyCurrExtSuppRates,
+							    uRateLen);
+		} else {
+			abyCurrExtSuppRates[1] = 0;
+		}
+
+		RATEvParseMaxRate((void *)pDevice,
+				  (PWLAN_IE_SUPP_RATES)abyCurrSuppRates,
+				  (PWLAN_IE_SUPP_RATES)abyCurrExtSuppRates,
+				  false, // do not change our basic rate
+				  &(pMgmt->sNodeDBTable[uNodeIndex].wMaxBasicRate),
+				  &(pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate),
+				  &(pMgmt->sNodeDBTable[uNodeIndex].wSuppRate),
+				  &(pMgmt->sNodeDBTable[uNodeIndex].byTopCCKBasicRate),
+				  &(pMgmt->sNodeDBTable[uNodeIndex].byTopOFDMBasicRate)
+);
+
+		// set max tx rate
+		pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate =
+			pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate;
 #ifdef	PLICE_DEBUG
-	printk("RxReAssocRequest:TxDataRate is %d\n",pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate);
+		printk("RxReAssocRequest:TxDataRate is %d\n", pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate);
 #endif
 		// Todo: check sta preamble, if ap can't support, set status code
-        pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble =
-                WLAN_GET_CAP_INFO_SHORTPREAMBLE(*sFrame.pwCapInfo);
-        pMgmt->sNodeDBTable[uNodeIndex].bShortSlotTime =
-                WLAN_GET_CAP_INFO_SHORTSLOTTIME(*sFrame.pwCapInfo);
-        pMgmt->sNodeDBTable[uNodeIndex].wAID = (unsigned short)uNodeIndex;
-        wAssocStatus = WLAN_MGMT_STATUS_SUCCESS;
-        wAssocAID = (unsigned short)uNodeIndex;
-
-        // if suppurt ERP
-        if(pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate > RATE_11M)
-           pMgmt->sNodeDBTable[uNodeIndex].bERPExist = true;
-
-        if (pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate <= RATE_11M) {
-            // B only STA join
-            pDevice->bProtectMode = true;
-            pDevice->bNonERPPresent = true;
-        }
-        if (pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble == false) {
-            pDevice->bBarkerPreambleMd = true;
-        }
-
-        DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Rx ReAssociate AID= %d \n", wAssocAID);
-        DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "MAC=%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X \n",
-                   sFrame.pHdr->sA3.abyAddr2[0],
-                   sFrame.pHdr->sA3.abyAddr2[1],
-                   sFrame.pHdr->sA3.abyAddr2[2],
-                   sFrame.pHdr->sA3.abyAddr2[3],
-                   sFrame.pHdr->sA3.abyAddr2[4],
-                   sFrame.pHdr->sA3.abyAddr2[5]
-                  ) ;
-        DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Max Support rate = %d \n",
-                   pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate);
-
-    }
-
-
-    // assoc response reply..
-    pTxPacket = s_MgrMakeReAssocResponse
-                (
-                  pDevice,
-                  pMgmt,
-                  pMgmt->wCurrCapInfo,
-                  wAssocStatus,
-                  wAssocAID,
-                  sFrame.pHdr->sA3.abyAddr2,
-                  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-                  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates
-                );
-
-    if (pTxPacket != NULL ){
-        /* send the frame */
-        if (pDevice->bEnableHostapd) {
-            return;
-        }
-        Status = csMgmt_xmit(pDevice, pTxPacket);
-        if (Status != CMD_STATUS_PENDING) {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:ReAssoc response tx failed\n");
-        }
-        else {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:ReAssoc response tx sending..\n");
-        }
-    }
-    return;
-}
+		pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble =
+			WLAN_GET_CAP_INFO_SHORTPREAMBLE(*sFrame.pwCapInfo);
+		pMgmt->sNodeDBTable[uNodeIndex].bShortSlotTime =
+			WLAN_GET_CAP_INFO_SHORTSLOTTIME(*sFrame.pwCapInfo);
+		pMgmt->sNodeDBTable[uNodeIndex].wAID = (unsigned short)uNodeIndex;
+		wAssocStatus = WLAN_MGMT_STATUS_SUCCESS;
+		wAssocAID = (unsigned short)uNodeIndex;
+
+		// if suppurt ERP
+		if (pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate > RATE_11M)
+			pMgmt->sNodeDBTable[uNodeIndex].bERPExist = true;
+
+		if (pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate <= RATE_11M) {
+			// B only STA join
+			pDevice->bProtectMode = true;
+			pDevice->bNonERPPresent = true;
+		}
+		if (pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble == false) {
+			pDevice->bBarkerPreambleMd = true;
+		}
+
+		DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Rx ReAssociate AID= %d \n", wAssocAID);
+		DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "MAC=%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X \n",
+			sFrame.pHdr->sA3.abyAddr2[0],
+			sFrame.pHdr->sA3.abyAddr2[1],
+			sFrame.pHdr->sA3.abyAddr2[2],
+			sFrame.pHdr->sA3.abyAddr2[3],
+			sFrame.pHdr->sA3.abyAddr2[4],
+			sFrame.pHdr->sA3.abyAddr2[5]
+			);
+		DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Max Support rate = %d \n",
+			pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate);
 
+	}
+
+	// assoc response reply..
+	pTxPacket = s_MgrMakeReAssocResponse
+		(
+			pDevice,
+			pMgmt,
+			pMgmt->wCurrCapInfo,
+			wAssocStatus,
+			wAssocAID,
+			sFrame.pHdr->sA3.abyAddr2,
+			(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
+			(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates
+			);
+
+	if (pTxPacket != NULL) {
+		/* send the frame */
+		if (pDevice->bEnableHostapd) {
+			return;
+		}
+		Status = csMgmt_xmit(pDevice, pTxPacket);
+		if (Status != CMD_STATUS_PENDING) {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:ReAssoc response tx failed\n");
+		} else {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:ReAssoc response tx sending..\n");
+		}
+	}
+	return;
+}
 
 /*+
  *
@@ -988,158 +945,147 @@ s_vMgrRxReAssocRequest(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 static
 void
 s_vMgrRxAssocResponse(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    PSRxMgmtPacket pRxPacket,
-    bool bReAssocType
-    )
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	PSRxMgmtPacket pRxPacket,
+	bool bReAssocType
+)
 {
-    WLAN_FR_ASSOCRESP   sFrame;
-    PWLAN_IE_SSID   pItemSSID;
-    unsigned char *pbyIEs;
-    viawget_wpa_header *wpahdr;
-
-
-
-    if (pMgmt->eCurrState == WMAC_STATE_ASSOCPENDING ||
-         pMgmt->eCurrState == WMAC_STATE_ASSOC) {
-
-        sFrame.len = pRxPacket->cbMPDULen;
-        sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
-        // decode the frame
-        vMgrDecodeAssocResponse(&sFrame);
-        if ((sFrame.pwCapInfo == 0) ||
-            (sFrame.pwStatus == 0) ||
-            (sFrame.pwAid == 0) ||
-            (sFrame.pSuppRates == 0)){
-            DBG_PORT80(0xCC);
-            return;
-        }
-
-        pMgmt->sAssocInfo.AssocInfo.ResponseFixedIEs.Capabilities = *(sFrame.pwCapInfo);
-        pMgmt->sAssocInfo.AssocInfo.ResponseFixedIEs.StatusCode = *(sFrame.pwStatus);
-        pMgmt->sAssocInfo.AssocInfo.ResponseFixedIEs.AssociationId = *(sFrame.pwAid);
-        pMgmt->sAssocInfo.AssocInfo.AvailableResponseFixedIEs |= 0x07;
-
-        pMgmt->sAssocInfo.AssocInfo.ResponseIELength = sFrame.len - 24 - 6;
-        pMgmt->sAssocInfo.AssocInfo.OffsetResponseIEs = pMgmt->sAssocInfo.AssocInfo.OffsetRequestIEs + pMgmt->sAssocInfo.AssocInfo.RequestIELength;
-        pbyIEs = pMgmt->sAssocInfo.abyIEs;
-        pbyIEs += pMgmt->sAssocInfo.AssocInfo.RequestIELength;
-        memcpy(pbyIEs, (sFrame.pBuf + 24 +6), pMgmt->sAssocInfo.AssocInfo.ResponseIELength);
-
-        // save values and set current BSS state
-        if (cpu_to_le16((*(sFrame.pwStatus))) == WLAN_MGMT_STATUS_SUCCESS ){
-            // set AID
-            pMgmt->wCurrAID = cpu_to_le16((*(sFrame.pwAid)));
-            if ( (pMgmt->wCurrAID >> 14) != (BIT0 | BIT1) )
-            {
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "AID from AP, has two msb clear.\n");
-            }
-            DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Association Successful, AID=%d.\n", pMgmt->wCurrAID & ~(BIT14|BIT15));
-            pMgmt->eCurrState = WMAC_STATE_ASSOC;
-            BSSvUpdateAPNode((void *)pDevice, sFrame.pwCapInfo, sFrame.pSuppRates, sFrame.pExtSuppRates);
-            pItemSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
-            DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Link with AP(SSID): %s\n", pItemSSID->abySSID);
-            pDevice->bLinkPass = true;
-            pDevice->uBBVGADiffCount = 0;
-            if ((pDevice->bWPADEVUp) && (pDevice->skb != NULL)) {
-	  if(skb_tailroom(pDevice->skb) <(sizeof(viawget_wpa_header)+pMgmt->sAssocInfo.AssocInfo.ResponseIELength+
-		   	                                                 pMgmt->sAssocInfo.AssocInfo.RequestIELength)) {    //data room not enough
-                     dev_kfree_skb(pDevice->skb);
-		   pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
-	       	}
-                wpahdr = (viawget_wpa_header *)pDevice->skb->data;
-                wpahdr->type = VIAWGET_ASSOC_MSG;
-                wpahdr->resp_ie_len = pMgmt->sAssocInfo.AssocInfo.ResponseIELength;
-                wpahdr->req_ie_len = pMgmt->sAssocInfo.AssocInfo.RequestIELength;
-                memcpy(pDevice->skb->data + sizeof(viawget_wpa_header), pMgmt->sAssocInfo.abyIEs, wpahdr->req_ie_len);
-                memcpy(pDevice->skb->data + sizeof(viawget_wpa_header) + wpahdr->req_ie_len,
-                       pbyIEs,
-                       wpahdr->resp_ie_len
-                       );
-                skb_put(pDevice->skb, sizeof(viawget_wpa_header) + wpahdr->resp_ie_len + wpahdr->req_ie_len);
-                pDevice->skb->dev = pDevice->wpadev;
-		skb_reset_mac_header(pDevice->skb);
-                pDevice->skb->pkt_type = PACKET_HOST;
-                pDevice->skb->protocol = htons(ETH_P_802_2);
-                memset(pDevice->skb->cb, 0, sizeof(pDevice->skb->cb));
-                netif_rx(pDevice->skb);
-                pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
-            }
-
-//2008-0409-07, <Add> by Einsn Liu
-#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-	//if(pDevice->bWPADevEnable == true)
-		{
-		unsigned char buf[512];
-		size_t len;
-		union iwreq_data  wrqu;
-		int we_event;
-
-		memset(buf, 0, 512);
-
-		len = pMgmt->sAssocInfo.AssocInfo.RequestIELength;
-		if(len)	{
-			memcpy(buf, pMgmt->sAssocInfo.abyIEs, len);
-			memset(&wrqu, 0, sizeof (wrqu));
-			wrqu.data.length = len;
-			we_event = IWEVASSOCREQIE;
-			wireless_send_event(pDevice->dev, we_event, &wrqu, buf);
-		}
-
-		memset(buf, 0, 512);
-		len = pMgmt->sAssocInfo.AssocInfo.ResponseIELength;
-
-		if(len)	{
-			memcpy(buf, pbyIEs, len);
-			memset(&wrqu, 0, sizeof (wrqu));
-			wrqu.data.length = len;
-			we_event = IWEVASSOCRESPIE;
-			wireless_send_event(pDevice->dev, we_event, &wrqu, buf);
+	WLAN_FR_ASSOCRESP   sFrame;
+	PWLAN_IE_SSID   pItemSSID;
+	unsigned char *pbyIEs;
+	viawget_wpa_header *wpahdr;
+
+	if (pMgmt->eCurrState == WMAC_STATE_ASSOCPENDING ||
+	    pMgmt->eCurrState == WMAC_STATE_ASSOC) {
+		sFrame.len = pRxPacket->cbMPDULen;
+		sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
+		// decode the frame
+		vMgrDecodeAssocResponse(&sFrame);
+		if ((sFrame.pwCapInfo == 0) ||
+		    (sFrame.pwStatus == 0) ||
+		    (sFrame.pwAid == 0) ||
+		    (sFrame.pSuppRates == 0)) {
+			DBG_PORT80(0xCC);
+			return;
 		}
 
+		pMgmt->sAssocInfo.AssocInfo.ResponseFixedIEs.Capabilities = *(sFrame.pwCapInfo);
+		pMgmt->sAssocInfo.AssocInfo.ResponseFixedIEs.StatusCode = *(sFrame.pwStatus);
+		pMgmt->sAssocInfo.AssocInfo.ResponseFixedIEs.AssociationId = *(sFrame.pwAid);
+		pMgmt->sAssocInfo.AssocInfo.AvailableResponseFixedIEs |= 0x07;
+
+		pMgmt->sAssocInfo.AssocInfo.ResponseIELength = sFrame.len - 24 - 6;
+		pMgmt->sAssocInfo.AssocInfo.OffsetResponseIEs = pMgmt->sAssocInfo.AssocInfo.OffsetRequestIEs + pMgmt->sAssocInfo.AssocInfo.RequestIELength;
+		pbyIEs = pMgmt->sAssocInfo.abyIEs;
+		pbyIEs += pMgmt->sAssocInfo.AssocInfo.RequestIELength;
+		memcpy(pbyIEs, (sFrame.pBuf + 24 + 6), pMgmt->sAssocInfo.AssocInfo.ResponseIELength);
+
+		// save values and set current BSS state
+		if (cpu_to_le16((*(sFrame.pwStatus))) == WLAN_MGMT_STATUS_SUCCESS) {
+			// set AID
+			pMgmt->wCurrAID = cpu_to_le16((*(sFrame.pwAid)));
+			if ((pMgmt->wCurrAID >> 14) != (BIT0 | BIT1)) {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "AID from AP, has two msb clear.\n");
+			}
+			DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Association Successful, AID=%d.\n", pMgmt->wCurrAID & ~(BIT14 | BIT15));
+			pMgmt->eCurrState = WMAC_STATE_ASSOC;
+			BSSvUpdateAPNode((void *)pDevice, sFrame.pwCapInfo, sFrame.pSuppRates, sFrame.pExtSuppRates);
+			pItemSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
+			DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Link with AP(SSID): %s\n", pItemSSID->abySSID);
+			pDevice->bLinkPass = true;
+			pDevice->uBBVGADiffCount = 0;
+			if ((pDevice->bWPADEVUp) && (pDevice->skb != NULL)) {
+				if (skb_tailroom(pDevice->skb) < (sizeof(viawget_wpa_header) + pMgmt->sAssocInfo.AssocInfo.ResponseIELength +
+								  pMgmt->sAssocInfo.AssocInfo.RequestIELength)) {    //data room not enough
+					dev_kfree_skb(pDevice->skb);
+					pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
+				}
+				wpahdr = (viawget_wpa_header *)pDevice->skb->data;
+				wpahdr->type = VIAWGET_ASSOC_MSG;
+				wpahdr->resp_ie_len = pMgmt->sAssocInfo.AssocInfo.ResponseIELength;
+				wpahdr->req_ie_len = pMgmt->sAssocInfo.AssocInfo.RequestIELength;
+				memcpy(pDevice->skb->data + sizeof(viawget_wpa_header), pMgmt->sAssocInfo.abyIEs, wpahdr->req_ie_len);
+				memcpy(pDevice->skb->data + sizeof(viawget_wpa_header) + wpahdr->req_ie_len,
+				       pbyIEs,
+				       wpahdr->resp_ie_len
+);
+				skb_put(pDevice->skb, sizeof(viawget_wpa_header) + wpahdr->resp_ie_len + wpahdr->req_ie_len);
+				pDevice->skb->dev = pDevice->wpadev;
+				skb_reset_mac_header(pDevice->skb);
+				pDevice->skb->pkt_type = PACKET_HOST;
+				pDevice->skb->protocol = htons(ETH_P_802_2);
+				memset(pDevice->skb->cb, 0, sizeof(pDevice->skb->cb));
+				netif_rx(pDevice->skb);
+				pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
+			}
 
-  memset(&wrqu, 0, sizeof (wrqu));
-	memcpy(wrqu.ap_addr.sa_data, &pMgmt->abyCurrBSSID[0], ETH_ALEN);
-        wrqu.ap_addr.sa_family = ARPHRD_ETHER;
-	wireless_send_event(pDevice->dev, SIOCGIWAP, &wrqu, NULL);
-	}
+//2008-0409-07, <Add> by Einsn Liu
+#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
+			{
+				unsigned char buf[512];
+				size_t len;
+				union iwreq_data  wrqu;
+				int we_event;
+
+				memset(buf, 0, 512);
+
+				len = pMgmt->sAssocInfo.AssocInfo.RequestIELength;
+				if (len)	{
+					memcpy(buf, pMgmt->sAssocInfo.abyIEs, len);
+					memset(&wrqu, 0, sizeof(wrqu));
+					wrqu.data.length = len;
+					we_event = IWEVASSOCREQIE;
+					wireless_send_event(pDevice->dev, we_event, &wrqu, buf);
+				}
+
+				memset(buf, 0, 512);
+				len = pMgmt->sAssocInfo.AssocInfo.ResponseIELength;
+
+				if (len)	{
+					memcpy(buf, pbyIEs, len);
+					memset(&wrqu, 0, sizeof(wrqu));
+					wrqu.data.length = len;
+					we_event = IWEVASSOCRESPIE;
+					wireless_send_event(pDevice->dev, we_event, &wrqu, buf);
+				}
+
+				memset(&wrqu, 0, sizeof(wrqu));
+				memcpy(wrqu.ap_addr.sa_data, &pMgmt->abyCurrBSSID[0], ETH_ALEN);
+				wrqu.ap_addr.sa_family = ARPHRD_ETHER;
+				wireless_send_event(pDevice->dev, SIOCGIWAP, &wrqu, NULL);
+			}
 #endif //#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
 //End Add -- //2008-0409-07, <Add> by Einsn Liu
+		} else {
+			if (bReAssocType) {
+				pMgmt->eCurrState = WMAC_STATE_IDLE;
+			} else {
+				// jump back to the auth state and indicate the error
+				pMgmt->eCurrState = WMAC_STATE_AUTH;
+			}
+			s_vMgrLogStatus(pMgmt, cpu_to_le16((*(sFrame.pwStatus))));
 		}
-        else {
-            if (bReAssocType) {
-                pMgmt->eCurrState = WMAC_STATE_IDLE;
-            }
-            else {
-                // jump back to the auth state and indicate the error
-                pMgmt->eCurrState = WMAC_STATE_AUTH;
-            }
-            s_vMgrLogStatus(pMgmt,cpu_to_le16((*(sFrame.pwStatus))));
-        }
-
-    }
+
+	}
 
 #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
 //need clear flags related to Networkmanager
 
-              pDevice->bwextcount = 0;
-              pDevice->bWPASuppWextEnabled = false;
+	pDevice->bwextcount = 0;
+	pDevice->bWPASuppWextEnabled = false;
 #endif
 
-
-if(pMgmt->eCurrState == WMAC_STATE_ASSOC)
-      timer_expire(pDevice->sTimerCommand, 0);
-    return;
+	if (pMgmt->eCurrState == WMAC_STATE_ASSOC)
+		timer_expire(pDevice->sTimerCommand, 0);
+	return;
 }
 
-
-
 /*+
  *
  * Routine Description:
@@ -1149,54 +1095,52 @@ if(pMgmt->eCurrState == WMAC_STATE_ASSOC)
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 void
 vMgrAuthenBeginSta(
-    void *hDeviceContext,
-    PSMgmtObject  pMgmt,
-    PCMD_STATUS pStatus
-    )
+	void *hDeviceContext,
+	PSMgmtObject  pMgmt,
+	PCMD_STATUS pStatus
+)
 {
-    PSDevice     pDevice = (PSDevice)hDeviceContext;
-    WLAN_FR_AUTHEN  sFrame;
-    PSTxMgmtPacket  pTxPacket = NULL;
-
-    pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
-    memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_AUTHEN_FR_MAXLEN);
-    pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-    sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
-    sFrame.len = WLAN_AUTHEN_FR_MAXLEN;
-    vMgrEncodeAuthen(&sFrame);
-    /* insert values */
-    sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
-        (
-        WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
-        WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_AUTHEN)
-        ));
-    memcpy( sFrame.pHdr->sA3.abyAddr1, pMgmt->abyCurrBSSID, WLAN_ADDR_LEN);
-    memcpy( sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-    memcpy( sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-    if (pMgmt->bShareKeyAlgorithm)
-        *(sFrame.pwAuthAlgorithm) = cpu_to_le16(WLAN_AUTH_ALG_SHAREDKEY);
-    else
-        *(sFrame.pwAuthAlgorithm) = cpu_to_le16(WLAN_AUTH_ALG_OPENSYSTEM);
-
-    *(sFrame.pwAuthSequence) = cpu_to_le16(1);
-    /* Adjust the length fields */
-    pTxPacket->cbMPDULen = sFrame.len;
-    pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
-
-    *pStatus = csMgmt_xmit(pDevice, pTxPacket);
-    if (*pStatus == CMD_STATUS_PENDING){
-        pMgmt->eCurrState = WMAC_STATE_AUTHPENDING;
-        *pStatus = CMD_STATUS_SUCCESS;
-    }
-
-    return ;
-}
-
+	PSDevice     pDevice = (PSDevice)hDeviceContext;
+	WLAN_FR_AUTHEN  sFrame;
+	PSTxMgmtPacket  pTxPacket = NULL;
+
+	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
+	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_AUTHEN_FR_MAXLEN);
+	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
+	sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
+	sFrame.len = WLAN_AUTHEN_FR_MAXLEN;
+	vMgrEncodeAuthen(&sFrame);
+	/* insert values */
+	sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
+		(
+			WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
+			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_AUTHEN)
+));
+	memcpy(sFrame.pHdr->sA3.abyAddr1, pMgmt->abyCurrBSSID, WLAN_ADDR_LEN);
+	memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
+	memcpy(sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
+	if (pMgmt->bShareKeyAlgorithm)
+		*(sFrame.pwAuthAlgorithm) = cpu_to_le16(WLAN_AUTH_ALG_SHAREDKEY);
+	else
+		*(sFrame.pwAuthAlgorithm) = cpu_to_le16(WLAN_AUTH_ALG_OPENSYSTEM);
+
+	*(sFrame.pwAuthSequence) = cpu_to_le16(1);
+	/* Adjust the length fields */
+	pTxPacket->cbMPDULen = sFrame.len;
+	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
+
+	*pStatus = csMgmt_xmit(pDevice, pTxPacket);
+	if (*pStatus == CMD_STATUS_PENDING) {
+		pMgmt->eCurrState = WMAC_STATE_AUTHPENDING;
+		*pStatus = CMD_STATUS_SUCCESS;
+	}
 
+	return;
+}
 
 /*+
  *
@@ -1207,53 +1151,50 @@ vMgrAuthenBeginSta(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 void
 vMgrDeAuthenBeginSta(
-    void *hDeviceContext,
-    PSMgmtObject  pMgmt,
-    unsigned char *abyDestAddress,
-    unsigned short wReason,
-    PCMD_STATUS pStatus
-    )
+	void *hDeviceContext,
+	PSMgmtObject  pMgmt,
+	unsigned char *abyDestAddress,
+	unsigned short wReason,
+	PCMD_STATUS pStatus
+)
 {
-    PSDevice            pDevice = (PSDevice)hDeviceContext;
-    WLAN_FR_DEAUTHEN    sFrame;
-    PSTxMgmtPacket      pTxPacket = NULL;
-
-
-    pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
-    memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_DEAUTHEN_FR_MAXLEN);
-    pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-    sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
-    sFrame.len = WLAN_DEAUTHEN_FR_MAXLEN;
-    vMgrEncodeDeauthen(&sFrame);
-    /* insert values */
-    sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
-        (
-        WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
-        WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_DEAUTHEN)
-        ));
-
-    memcpy( sFrame.pHdr->sA3.abyAddr1, abyDestAddress, WLAN_ADDR_LEN);
-    memcpy( sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-    memcpy( sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-
-    *(sFrame.pwReason) = cpu_to_le16(wReason);       // deauthen. bcs left BSS
-    /* Adjust the length fields */
-    pTxPacket->cbMPDULen = sFrame.len;
-    pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
-
-    *pStatus = csMgmt_xmit(pDevice, pTxPacket);
-    if (*pStatus == CMD_STATUS_PENDING){
-        *pStatus = CMD_STATUS_SUCCESS;
-    }
-
-
-    return ;
-}
+	PSDevice            pDevice = (PSDevice)hDeviceContext;
+	WLAN_FR_DEAUTHEN    sFrame;
+	PSTxMgmtPacket      pTxPacket = NULL;
+
+	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
+	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_DEAUTHEN_FR_MAXLEN);
+	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
+	sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
+	sFrame.len = WLAN_DEAUTHEN_FR_MAXLEN;
+	vMgrEncodeDeauthen(&sFrame);
+	/* insert values */
+	sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
+		(
+			WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
+			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_DEAUTHEN)
+));
+
+	memcpy(sFrame.pHdr->sA3.abyAddr1, abyDestAddress, WLAN_ADDR_LEN);
+	memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
+	memcpy(sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
+
+	*(sFrame.pwReason) = cpu_to_le16(wReason);       // deauthen. bcs left BSS
+	/* Adjust the length fields */
+	pTxPacket->cbMPDULen = sFrame.len;
+	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
+
+	*pStatus = csMgmt_xmit(pDevice, pTxPacket);
+	if (*pStatus == CMD_STATUS_PENDING) {
+		*pStatus = CMD_STATUS_SUCCESS;
+	}
 
+	return;
+}
 
 /*+
  *
@@ -1263,52 +1204,50 @@ vMgrDeAuthenBeginSta(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 static
 void
 s_vMgrRxAuthentication(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    PSRxMgmtPacket pRxPacket
-    )
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	PSRxMgmtPacket pRxPacket
+)
 {
-    WLAN_FR_AUTHEN  sFrame;
-
-    // we better be an AP or a STA in AUTHPENDING otherwise ignore
-    if (!(pMgmt->eCurrMode == WMAC_MODE_ESS_AP ||
-          pMgmt->eCurrState == WMAC_STATE_AUTHPENDING)) {
-        return;
-    }
-
-    // decode the frame
-    sFrame.len = pRxPacket->cbMPDULen;
-    sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
-    vMgrDecodeAuthen(&sFrame);
-    switch (cpu_to_le16((*(sFrame.pwAuthSequence )))){
-        case 1:
-            //AP function
-            s_vMgrRxAuthenSequence_1(pDevice,pMgmt, &sFrame);
-            break;
-        case 2:
-            s_vMgrRxAuthenSequence_2(pDevice, pMgmt, &sFrame);
-            break;
-        case 3:
-            //AP function
-            s_vMgrRxAuthenSequence_3(pDevice, pMgmt, &sFrame);
-            break;
-        case 4:
-            s_vMgrRxAuthenSequence_4(pDevice, pMgmt, &sFrame);
-            break;
-        default:
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Auth Sequence error, seq = %d\n",
-                        cpu_to_le16((*(sFrame.pwAuthSequence))));
-            break;
-    }
-    return;
-}
+	WLAN_FR_AUTHEN  sFrame;
 
+	// we better be an AP or a STA in AUTHPENDING otherwise ignore
+	if (!(pMgmt->eCurrMode == WMAC_MODE_ESS_AP ||
+	      pMgmt->eCurrState == WMAC_STATE_AUTHPENDING)) {
+		return;
+	}
 
+	// decode the frame
+	sFrame.len = pRxPacket->cbMPDULen;
+	sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
+	vMgrDecodeAuthen(&sFrame);
+	switch (cpu_to_le16((*(sFrame.pwAuthSequence)))) {
+	case 1:
+		//AP function
+		s_vMgrRxAuthenSequence_1(pDevice, pMgmt, &sFrame);
+		break;
+	case 2:
+		s_vMgrRxAuthenSequence_2(pDevice, pMgmt, &sFrame);
+		break;
+	case 3:
+		//AP function
+		s_vMgrRxAuthenSequence_3(pDevice, pMgmt, &sFrame);
+		break;
+	case 4:
+		s_vMgrRxAuthenSequence_4(pDevice, pMgmt, &sFrame);
+		break;
+	default:
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Auth Sequence error, seq = %d\n",
+			cpu_to_le16((*(sFrame.pwAuthSequence))));
+		break;
+	}
+	return;
+}
 
 /*+
  *
@@ -1320,102 +1259,96 @@ s_vMgrRxAuthentication(
  * Return Value:
  *    None.
  *
--*/
-
+ -*/
 
 static
 void
 s_vMgrRxAuthenSequence_1(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    PWLAN_FR_AUTHEN pFrame
-     )
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	PWLAN_FR_AUTHEN pFrame
+)
 {
-    PSTxMgmtPacket      pTxPacket = NULL;
-    unsigned int	uNodeIndex;
-    WLAN_FR_AUTHEN      sFrame;
-    PSKeyItem           pTransmitKey;
-
-    // Insert a Node entry
-    if (!BSSDBbIsSTAInNodeDB(pMgmt, pFrame->pHdr->sA3.abyAddr2, &uNodeIndex)) {
-        BSSvCreateOneNode((PSDevice)pDevice, &uNodeIndex);
-        memcpy(pMgmt->sNodeDBTable[uNodeIndex].abyMACAddr, pFrame->pHdr->sA3.abyAddr2,
-               WLAN_ADDR_LEN);
-    }
-
-    if (pMgmt->bShareKeyAlgorithm) {
-        pMgmt->sNodeDBTable[uNodeIndex].eNodeState = NODE_KNOWN;
-        pMgmt->sNodeDBTable[uNodeIndex].byAuthSequence = 1;
-    }
-    else {
-        pMgmt->sNodeDBTable[uNodeIndex].eNodeState = NODE_AUTH;
-    }
-
-    // send auth reply
-    pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
-    memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_AUTHEN_FR_MAXLEN);
-    pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-    sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
-    sFrame.len = WLAN_AUTHEN_FR_MAXLEN;
-    // format buffer structure
-    vMgrEncodeAuthen(&sFrame);
-    // insert values
-    sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
-         (
-         WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
-         WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_AUTHEN)|
-         WLAN_SET_FC_ISWEP(0)
-         ));
-    memcpy( sFrame.pHdr->sA3.abyAddr1, pFrame->pHdr->sA3.abyAddr2, WLAN_ADDR_LEN);
-    memcpy( sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-    memcpy( sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-    *(sFrame.pwAuthAlgorithm) = *(pFrame->pwAuthAlgorithm);
-    *(sFrame.pwAuthSequence) = cpu_to_le16(2);
-
-    if (cpu_to_le16(*(pFrame->pwAuthAlgorithm)) == WLAN_AUTH_ALG_SHAREDKEY) {
-        if (pMgmt->bShareKeyAlgorithm)
-            *(sFrame.pwStatus) = cpu_to_le16(WLAN_MGMT_STATUS_SUCCESS);
-        else
-            *(sFrame.pwStatus) = cpu_to_le16(WLAN_MGMT_STATUS_UNSUPPORTED_AUTHALG);
-    }
-    else {
-        if (pMgmt->bShareKeyAlgorithm)
-            *(sFrame.pwStatus) = cpu_to_le16(WLAN_MGMT_STATUS_UNSUPPORTED_AUTHALG);
-        else
-            *(sFrame.pwStatus) = cpu_to_le16(WLAN_MGMT_STATUS_SUCCESS);
-    }
-
-    if (pMgmt->bShareKeyAlgorithm &&
-        (cpu_to_le16(*(sFrame.pwStatus)) == WLAN_MGMT_STATUS_SUCCESS)) {
-
-        sFrame.pChallenge = (PWLAN_IE_CHALLENGE)(sFrame.pBuf + sFrame.len);
-        sFrame.len += WLAN_CHALLENGE_IE_LEN;
-        sFrame.pChallenge->byElementID = WLAN_EID_CHALLENGE;
-        sFrame.pChallenge->len = WLAN_CHALLENGE_LEN;
-        memset(pMgmt->abyChallenge, 0, WLAN_CHALLENGE_LEN);
-        // get group key
-        if(KeybGetTransmitKey(&(pDevice->sKey), pDevice->abyBroadcastAddr, GROUP_KEY, &pTransmitKey) == true) {
-            rc4_init(&pDevice->SBox, pDevice->abyPRNG, pTransmitKey->uKeyLength+3);
-            rc4_encrypt(&pDevice->SBox, pMgmt->abyChallenge, pMgmt->abyChallenge, WLAN_CHALLENGE_LEN);
-        }
-        memcpy(sFrame.pChallenge->abyChallenge, pMgmt->abyChallenge , WLAN_CHALLENGE_LEN);
-    }
-
-    /* Adjust the length fields */
-    pTxPacket->cbMPDULen = sFrame.len;
-    pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
-    // send the frame
-    if (pDevice->bEnableHostapd) {
-        return;
-    }
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Authreq_reply sequence_1 tx.. \n");
-    if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Authreq_reply sequence_1 tx failed.\n");
-    }
-    return;
-}
+	PSTxMgmtPacket      pTxPacket = NULL;
+	unsigned int	uNodeIndex;
+	WLAN_FR_AUTHEN      sFrame;
+	PSKeyItem           pTransmitKey;
+
+	// Insert a Node entry
+	if (!BSSDBbIsSTAInNodeDB(pMgmt, pFrame->pHdr->sA3.abyAddr2, &uNodeIndex)) {
+		BSSvCreateOneNode((PSDevice)pDevice, &uNodeIndex);
+		memcpy(pMgmt->sNodeDBTable[uNodeIndex].abyMACAddr, pFrame->pHdr->sA3.abyAddr2,
+		       WLAN_ADDR_LEN);
+	}
+
+	if (pMgmt->bShareKeyAlgorithm) {
+		pMgmt->sNodeDBTable[uNodeIndex].eNodeState = NODE_KNOWN;
+		pMgmt->sNodeDBTable[uNodeIndex].byAuthSequence = 1;
+	} else {
+		pMgmt->sNodeDBTable[uNodeIndex].eNodeState = NODE_AUTH;
+	}
 
+	// send auth reply
+	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
+	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_AUTHEN_FR_MAXLEN);
+	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
+	sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
+	sFrame.len = WLAN_AUTHEN_FR_MAXLEN;
+	// format buffer structure
+	vMgrEncodeAuthen(&sFrame);
+	// insert values
+	sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
+		(
+			WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
+			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_AUTHEN)|
+			WLAN_SET_FC_ISWEP(0)
+));
+	memcpy(sFrame.pHdr->sA3.abyAddr1, pFrame->pHdr->sA3.abyAddr2, WLAN_ADDR_LEN);
+	memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
+	memcpy(sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
+	*(sFrame.pwAuthAlgorithm) = *(pFrame->pwAuthAlgorithm);
+	*(sFrame.pwAuthSequence) = cpu_to_le16(2);
+
+	if (cpu_to_le16(*(pFrame->pwAuthAlgorithm)) == WLAN_AUTH_ALG_SHAREDKEY) {
+		if (pMgmt->bShareKeyAlgorithm)
+			*(sFrame.pwStatus) = cpu_to_le16(WLAN_MGMT_STATUS_SUCCESS);
+		else
+			*(sFrame.pwStatus) = cpu_to_le16(WLAN_MGMT_STATUS_UNSUPPORTED_AUTHALG);
+	} else {
+		if (pMgmt->bShareKeyAlgorithm)
+			*(sFrame.pwStatus) = cpu_to_le16(WLAN_MGMT_STATUS_UNSUPPORTED_AUTHALG);
+		else
+			*(sFrame.pwStatus) = cpu_to_le16(WLAN_MGMT_STATUS_SUCCESS);
+	}
+
+	if (pMgmt->bShareKeyAlgorithm &&
+	    (cpu_to_le16(*(sFrame.pwStatus)) == WLAN_MGMT_STATUS_SUCCESS)) {
+		sFrame.pChallenge = (PWLAN_IE_CHALLENGE)(sFrame.pBuf + sFrame.len);
+		sFrame.len += WLAN_CHALLENGE_IE_LEN;
+		sFrame.pChallenge->byElementID = WLAN_EID_CHALLENGE;
+		sFrame.pChallenge->len = WLAN_CHALLENGE_LEN;
+		memset(pMgmt->abyChallenge, 0, WLAN_CHALLENGE_LEN);
+		// get group key
+		if (KeybGetTransmitKey(&(pDevice->sKey), pDevice->abyBroadcastAddr, GROUP_KEY, &pTransmitKey) == true) {
+			rc4_init(&pDevice->SBox, pDevice->abyPRNG, pTransmitKey->uKeyLength+3);
+			rc4_encrypt(&pDevice->SBox, pMgmt->abyChallenge, pMgmt->abyChallenge, WLAN_CHALLENGE_LEN);
+		}
+		memcpy(sFrame.pChallenge->abyChallenge, pMgmt->abyChallenge , WLAN_CHALLENGE_LEN);
+	}
 
+	/* Adjust the length fields */
+	pTxPacket->cbMPDULen = sFrame.len;
+	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
+	// send the frame
+	if (pDevice->bEnableHostapd) {
+		return;
+	}
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Authreq_reply sequence_1 tx.. \n");
+	if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Authreq_reply sequence_1 tx failed.\n");
+	}
+	return;
+}
 
 /*+
  *
@@ -1427,97 +1360,91 @@ s_vMgrRxAuthenSequence_1(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 static
 void
 s_vMgrRxAuthenSequence_2(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    PWLAN_FR_AUTHEN pFrame
-    )
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	PWLAN_FR_AUTHEN pFrame
+)
 {
-    WLAN_FR_AUTHEN      sFrame;
-    PSTxMgmtPacket      pTxPacket = NULL;
-
-
-    switch (cpu_to_le16((*(pFrame->pwAuthAlgorithm))))
-    {
-        case WLAN_AUTH_ALG_OPENSYSTEM:
-            if ( cpu_to_le16((*(pFrame->pwStatus))) == WLAN_MGMT_STATUS_SUCCESS ){
-                DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "802.11 Authen (OPEN) Successful.\n");
-                pMgmt->eCurrState = WMAC_STATE_AUTH;
-	 timer_expire(pDevice->sTimerCommand, 0);
-            }
-            else {
-                DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "802.11 Authen (OPEN) Failed.\n");
-                s_vMgrLogStatus(pMgmt, cpu_to_le16((*(pFrame->pwStatus))));
-                pMgmt->eCurrState = WMAC_STATE_IDLE;
-            }
-            if (pDevice->eCommandState == WLAN_AUTHENTICATE_WAIT ) {
+	WLAN_FR_AUTHEN      sFrame;
+	PSTxMgmtPacket      pTxPacket = NULL;
+
+	switch (cpu_to_le16((*(pFrame->pwAuthAlgorithm)))) {
+	case WLAN_AUTH_ALG_OPENSYSTEM:
+		if (cpu_to_le16((*(pFrame->pwStatus))) == WLAN_MGMT_STATUS_SUCCESS) {
+			DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "802.11 Authen (OPEN) Successful.\n");
+			pMgmt->eCurrState = WMAC_STATE_AUTH;
+			timer_expire(pDevice->sTimerCommand, 0);
+		} else {
+			DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "802.11 Authen (OPEN) Failed.\n");
+			s_vMgrLogStatus(pMgmt, cpu_to_le16((*(pFrame->pwStatus))));
+			pMgmt->eCurrState = WMAC_STATE_IDLE;
+		}
+		if (pDevice->eCommandState == WLAN_AUTHENTICATE_WAIT) {
 //                spin_unlock_irq(&pDevice->lock);
 //                vCommandTimerWait((void *)pDevice, 0);
 //                spin_lock_irq(&pDevice->lock);
-            }
-
-            break;
-
-        case WLAN_AUTH_ALG_SHAREDKEY:
-
-            if (cpu_to_le16((*(pFrame->pwStatus))) == WLAN_MGMT_STATUS_SUCCESS) {
-                pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
-                memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_AUTHEN_FR_MAXLEN);
-                pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-                sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
-                sFrame.len = WLAN_AUTHEN_FR_MAXLEN;
-                // format buffer structure
-                vMgrEncodeAuthen(&sFrame);
-                // insert values
-                sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
-                     (
-                     WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
-                     WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_AUTHEN)|
-                     WLAN_SET_FC_ISWEP(1)
-                     ));
-                memcpy( sFrame.pHdr->sA3.abyAddr1, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-                memcpy( sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-                memcpy( sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-                *(sFrame.pwAuthAlgorithm) = *(pFrame->pwAuthAlgorithm);
-                *(sFrame.pwAuthSequence) = cpu_to_le16(3);
-                *(sFrame.pwStatus) = cpu_to_le16(WLAN_MGMT_STATUS_SUCCESS);
-                sFrame.pChallenge = (PWLAN_IE_CHALLENGE)(sFrame.pBuf + sFrame.len);
-                sFrame.len += WLAN_CHALLENGE_IE_LEN;
-                sFrame.pChallenge->byElementID = WLAN_EID_CHALLENGE;
-                sFrame.pChallenge->len = WLAN_CHALLENGE_LEN;
-                memcpy( sFrame.pChallenge->abyChallenge, pFrame->pChallenge->abyChallenge, WLAN_CHALLENGE_LEN);
-                // Adjust the length fields
-                pTxPacket->cbMPDULen = sFrame.len;
-                pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
-                // send the frame
-                if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING) {
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Auth_reply sequence_2 tx failed.\n");
-                }
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Auth_reply sequence_2 tx ...\n");
-            }
-            else {
-            	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:rx Auth_reply sequence_2 status error ...\n");
-                if ( pDevice->eCommandState == WLAN_AUTHENTICATE_WAIT ) {
+		}
+
+		break;
+
+	case WLAN_AUTH_ALG_SHAREDKEY:
+
+		if (cpu_to_le16((*(pFrame->pwStatus))) == WLAN_MGMT_STATUS_SUCCESS) {
+			pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
+			memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_AUTHEN_FR_MAXLEN);
+			pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
+			sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
+			sFrame.len = WLAN_AUTHEN_FR_MAXLEN;
+			// format buffer structure
+			vMgrEncodeAuthen(&sFrame);
+			// insert values
+			sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
+				(
+					WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
+					WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_AUTHEN)|
+					WLAN_SET_FC_ISWEP(1)
+));
+			memcpy(sFrame.pHdr->sA3.abyAddr1, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
+			memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
+			memcpy(sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
+			*(sFrame.pwAuthAlgorithm) = *(pFrame->pwAuthAlgorithm);
+			*(sFrame.pwAuthSequence) = cpu_to_le16(3);
+			*(sFrame.pwStatus) = cpu_to_le16(WLAN_MGMT_STATUS_SUCCESS);
+			sFrame.pChallenge = (PWLAN_IE_CHALLENGE)(sFrame.pBuf + sFrame.len);
+			sFrame.len += WLAN_CHALLENGE_IE_LEN;
+			sFrame.pChallenge->byElementID = WLAN_EID_CHALLENGE;
+			sFrame.pChallenge->len = WLAN_CHALLENGE_LEN;
+			memcpy(sFrame.pChallenge->abyChallenge, pFrame->pChallenge->abyChallenge, WLAN_CHALLENGE_LEN);
+			// Adjust the length fields
+			pTxPacket->cbMPDULen = sFrame.len;
+			pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
+			// send the frame
+			if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING) {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Auth_reply sequence_2 tx failed.\n");
+			}
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Auth_reply sequence_2 tx ...\n");
+		} else {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:rx Auth_reply sequence_2 status error ...\n");
+			if (pDevice->eCommandState == WLAN_AUTHENTICATE_WAIT) {
 //                    spin_unlock_irq(&pDevice->lock);
 //                    vCommandTimerWait((void *)pDevice, 0);
 //                    spin_lock_irq(&pDevice->lock);
-                }
-                s_vMgrLogStatus(pMgmt, cpu_to_le16((*(pFrame->pwStatus))));
-            }
-            break;
-        default:
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt: rx auth.seq = 2 unknown AuthAlgorithm=%d\n", cpu_to_le16((*(pFrame->pwAuthAlgorithm))));
-            break;
-    }
-    return;
+			}
+			s_vMgrLogStatus(pMgmt, cpu_to_le16((*(pFrame->pwStatus))));
+		}
+		break;
+	default:
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt: rx auth.seq = 2 unknown AuthAlgorithm=%d\n", cpu_to_le16((*(pFrame->pwAuthAlgorithm))));
+		break;
+	}
+	return;
 }
 
-
-
 /*+
  *
  * Routine Description:
@@ -1529,86 +1456,82 @@ s_vMgrRxAuthenSequence_2(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 static
 void
 s_vMgrRxAuthenSequence_3(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    PWLAN_FR_AUTHEN pFrame
-    )
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	PWLAN_FR_AUTHEN pFrame
+)
 {
-    PSTxMgmtPacket      pTxPacket = NULL;
-    unsigned int uStatusCode = 0 ;
-    unsigned int uNodeIndex = 0;
-    WLAN_FR_AUTHEN      sFrame;
-
-    if (!WLAN_GET_FC_ISWEP(pFrame->pHdr->sA3.wFrameCtl)) {
-        uStatusCode = WLAN_MGMT_STATUS_CHALLENGE_FAIL;
-        goto reply;
-    }
-    if (BSSDBbIsSTAInNodeDB(pMgmt, pFrame->pHdr->sA3.abyAddr2, &uNodeIndex)) {
-         if (pMgmt->sNodeDBTable[uNodeIndex].byAuthSequence != 1) {
-            uStatusCode = WLAN_MGMT_STATUS_RX_AUTH_NOSEQ;
-            goto reply;
-         }
-         if (memcmp(pMgmt->abyChallenge, pFrame->pChallenge->abyChallenge, WLAN_CHALLENGE_LEN) != 0) {
-            uStatusCode = WLAN_MGMT_STATUS_CHALLENGE_FAIL;
-            goto reply;
-         }
-    }
-    else {
-        uStatusCode = WLAN_MGMT_STATUS_UNSPEC_FAILURE;
-        goto reply;
-    }
-
-    if (uNodeIndex) {
-        pMgmt->sNodeDBTable[uNodeIndex].eNodeState = NODE_AUTH;
-        pMgmt->sNodeDBTable[uNodeIndex].byAuthSequence = 0;
-    }
-    uStatusCode = WLAN_MGMT_STATUS_SUCCESS;
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Challenge text check ok..\n");
+	PSTxMgmtPacket      pTxPacket = NULL;
+	unsigned int uStatusCode = 0;
+	unsigned int uNodeIndex = 0;
+	WLAN_FR_AUTHEN      sFrame;
+
+	if (!WLAN_GET_FC_ISWEP(pFrame->pHdr->sA3.wFrameCtl)) {
+		uStatusCode = WLAN_MGMT_STATUS_CHALLENGE_FAIL;
+		goto reply;
+	}
+	if (BSSDBbIsSTAInNodeDB(pMgmt, pFrame->pHdr->sA3.abyAddr2, &uNodeIndex)) {
+		if (pMgmt->sNodeDBTable[uNodeIndex].byAuthSequence != 1) {
+			uStatusCode = WLAN_MGMT_STATUS_RX_AUTH_NOSEQ;
+			goto reply;
+		}
+		if (memcmp(pMgmt->abyChallenge, pFrame->pChallenge->abyChallenge, WLAN_CHALLENGE_LEN) != 0) {
+			uStatusCode = WLAN_MGMT_STATUS_CHALLENGE_FAIL;
+			goto reply;
+		}
+	} else {
+		uStatusCode = WLAN_MGMT_STATUS_UNSPEC_FAILURE;
+		goto reply;
+	}
 
-reply:
-    // send auth reply
-    pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
-    memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_AUTHEN_FR_MAXLEN);
-    pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-    sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
-    sFrame.len = WLAN_AUTHEN_FR_MAXLEN;
-    // format buffer structure
-    vMgrEncodeAuthen(&sFrame);
-    /* insert values */
-    sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
-         (
-         WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
-         WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_AUTHEN)|
-         WLAN_SET_FC_ISWEP(0)
-         ));
-    memcpy( sFrame.pHdr->sA3.abyAddr1, pFrame->pHdr->sA3.abyAddr2, WLAN_ADDR_LEN);
-    memcpy( sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-    memcpy( sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-    *(sFrame.pwAuthAlgorithm) = *(pFrame->pwAuthAlgorithm);
-    *(sFrame.pwAuthSequence) = cpu_to_le16(4);
-    *(sFrame.pwStatus) = cpu_to_le16(uStatusCode);
-
-    /* Adjust the length fields */
-    pTxPacket->cbMPDULen = sFrame.len;
-    pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
-    // send the frame
-    if (pDevice->bEnableHostapd) {
-        return;
-    }
-    if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Authreq_reply sequence_4 tx failed.\n");
-    }
-    return;
+	if (uNodeIndex) {
+		pMgmt->sNodeDBTable[uNodeIndex].eNodeState = NODE_AUTH;
+		pMgmt->sNodeDBTable[uNodeIndex].byAuthSequence = 0;
+	}
+	uStatusCode = WLAN_MGMT_STATUS_SUCCESS;
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Challenge text check ok..\n");
 
+reply:
+	// send auth reply
+	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
+	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_AUTHEN_FR_MAXLEN);
+	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
+	sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
+	sFrame.len = WLAN_AUTHEN_FR_MAXLEN;
+	// format buffer structure
+	vMgrEncodeAuthen(&sFrame);
+	/* insert values */
+	sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
+		(
+			WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
+			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_AUTHEN)|
+			WLAN_SET_FC_ISWEP(0)
+));
+	memcpy(sFrame.pHdr->sA3.abyAddr1, pFrame->pHdr->sA3.abyAddr2, WLAN_ADDR_LEN);
+	memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
+	memcpy(sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
+	*(sFrame.pwAuthAlgorithm) = *(pFrame->pwAuthAlgorithm);
+	*(sFrame.pwAuthSequence) = cpu_to_le16(4);
+	*(sFrame.pwStatus) = cpu_to_le16(uStatusCode);
+
+	/* Adjust the length fields */
+	pTxPacket->cbMPDULen = sFrame.len;
+	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
+	// send the frame
+	if (pDevice->bEnableHostapd) {
+		return;
+	}
+	if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Authreq_reply sequence_4 tx failed.\n");
+	}
+	return;
 }
 
-
-
 /*+
  *
  * Routine Description:
@@ -1618,33 +1541,30 @@ reply:
  * Return Value:
  *    None.
  *
--*/
+ -*/
 static
 void
 s_vMgrRxAuthenSequence_4(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    PWLAN_FR_AUTHEN pFrame
-    )
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	PWLAN_FR_AUTHEN pFrame
+)
 {
+	if (cpu_to_le16((*(pFrame->pwStatus))) == WLAN_MGMT_STATUS_SUCCESS) {
+		DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "802.11 Authen (SHAREDKEY) Successful.\n");
+		pMgmt->eCurrState = WMAC_STATE_AUTH;
+		timer_expire(pDevice->sTimerCommand, 0);
+	} else{
+		DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "802.11 Authen (SHAREDKEY) Failed.\n");
+		s_vMgrLogStatus(pMgmt, cpu_to_le16((*(pFrame->pwStatus))));
+		pMgmt->eCurrState = WMAC_STATE_IDLE;
+	}
 
-    if ( cpu_to_le16((*(pFrame->pwStatus))) == WLAN_MGMT_STATUS_SUCCESS ){
-        DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "802.11 Authen (SHAREDKEY) Successful.\n");
-        pMgmt->eCurrState = WMAC_STATE_AUTH;
-	  timer_expire(pDevice->sTimerCommand, 0);
-    }
-    else{
-        DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "802.11 Authen (SHAREDKEY) Failed.\n");
-        s_vMgrLogStatus(pMgmt, cpu_to_le16((*(pFrame->pwStatus))) );
-        pMgmt->eCurrState = WMAC_STATE_IDLE;
-    }
-
-    if ( pDevice->eCommandState == WLAN_AUTHENTICATE_WAIT ) {
+	if (pDevice->eCommandState == WLAN_AUTHENTICATE_WAIT) {
 //        spin_unlock_irq(&pDevice->lock);
 //        vCommandTimerWait((void *)pDevice, 0);
 //        spin_lock_irq(&pDevice->lock);
-    }
-
+	}
 }
 
 /*+
@@ -1656,75 +1576,68 @@ s_vMgrRxAuthenSequence_4(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 static
 void
 s_vMgrRxDisassociation(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    PSRxMgmtPacket pRxPacket
-    )
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	PSRxMgmtPacket pRxPacket
+)
 {
-    WLAN_FR_DISASSOC    sFrame;
-    unsigned int uNodeIndex = 0;
+	WLAN_FR_DISASSOC    sFrame;
+	unsigned int uNodeIndex = 0;
 //    CMD_STATUS          CmdStatus;
-    viawget_wpa_header *wpahdr;
-
-    if ( pMgmt->eCurrMode == WMAC_MODE_ESS_AP ){
-        // if is acting an AP..
-        // a STA is leaving this BSS..
-        sFrame.len = pRxPacket->cbMPDULen;
-        sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
-        if (BSSDBbIsSTAInNodeDB(pMgmt, pRxPacket->p80211Header->sA3.abyAddr2, &uNodeIndex)) {
-            BSSvRemoveOneNode(pDevice, uNodeIndex);
-        }
-        else {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Rx disassoc, sta not found\n");
-        }
-    }
-    else if (pMgmt->eCurrMode == WMAC_MODE_ESS_STA ){
-        sFrame.len = pRxPacket->cbMPDULen;
-        sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
-        vMgrDecodeDisassociation(&sFrame);
-        DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "AP disassociated me, reason=%d.\n", cpu_to_le16(*(sFrame.pwReason)));
-        //TODO: do something let upper layer know or
-        //try to send associate packet again because of inactivity timeout
-      //  if (pMgmt->eCurrState == WMAC_STATE_ASSOC) {
-       //     vMgrReAssocBeginSta((PSDevice)pDevice, pMgmt, &CmdStatus);
-      //  }
-        if ((pDevice->bWPADEVUp) && (pDevice->skb != NULL)) {
-             wpahdr = (viawget_wpa_header *)pDevice->skb->data;
-             wpahdr->type = VIAWGET_DISASSOC_MSG;
-             wpahdr->resp_ie_len = 0;
-             wpahdr->req_ie_len = 0;
-             skb_put(pDevice->skb, sizeof(viawget_wpa_header));
-             pDevice->skb->dev = pDevice->wpadev;
-	     skb_reset_mac_header(pDevice->skb);
-
-             pDevice->skb->pkt_type = PACKET_HOST;
-             pDevice->skb->protocol = htons(ETH_P_802_2);
-             memset(pDevice->skb->cb, 0, sizeof(pDevice->skb->cb));
-             netif_rx(pDevice->skb);
-             pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
-         }
-
- #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-  // if(pDevice->bWPASuppWextEnabled == true)
-      {
-	union iwreq_data  wrqu;
-	memset(&wrqu, 0, sizeof (wrqu));
-        wrqu.ap_addr.sa_family = ARPHRD_ETHER;
-	printk("wireless_send_event--->SIOCGIWAP(disassociated)\n");
-	wireless_send_event(pDevice->dev, SIOCGIWAP, &wrqu, NULL);
-     }
-  #endif
-    }
-    /* else, ignore it */
-
-    return;
-}
+	viawget_wpa_header *wpahdr;
+
+	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
+		// if is acting an AP..
+		// a STA is leaving this BSS..
+		sFrame.len = pRxPacket->cbMPDULen;
+		sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
+		if (BSSDBbIsSTAInNodeDB(pMgmt, pRxPacket->p80211Header->sA3.abyAddr2, &uNodeIndex)) {
+			BSSvRemoveOneNode(pDevice, uNodeIndex);
+		} else {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Rx disassoc, sta not found\n");
+		}
+	} else if (pMgmt->eCurrMode == WMAC_MODE_ESS_STA) {
+		sFrame.len = pRxPacket->cbMPDULen;
+		sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
+		vMgrDecodeDisassociation(&sFrame);
+		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "AP disassociated me, reason=%d.\n", cpu_to_le16(*(sFrame.pwReason)));
+		//TODO: do something let upper layer know or
+		//try to send associate packet again because of inactivity timeout
+		if ((pDevice->bWPADEVUp) && (pDevice->skb != NULL)) {
+			wpahdr = (viawget_wpa_header *)pDevice->skb->data;
+			wpahdr->type = VIAWGET_DISASSOC_MSG;
+			wpahdr->resp_ie_len = 0;
+			wpahdr->req_ie_len = 0;
+			skb_put(pDevice->skb, sizeof(viawget_wpa_header));
+			pDevice->skb->dev = pDevice->wpadev;
+			skb_reset_mac_header(pDevice->skb);
+
+			pDevice->skb->pkt_type = PACKET_HOST;
+			pDevice->skb->protocol = htons(ETH_P_802_2);
+			memset(pDevice->skb->cb, 0, sizeof(pDevice->skb->cb));
+			netif_rx(pDevice->skb);
+			pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
+		}
 
+#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
+		{
+			union iwreq_data  wrqu;
+			memset(&wrqu, 0, sizeof(wrqu));
+			wrqu.ap_addr.sa_family = ARPHRD_ETHER;
+			printk("wireless_send_event--->SIOCGIWAP(disassociated)\n");
+			wireless_send_event(pDevice->dev, SIOCGIWAP, &wrqu, NULL);
+		}
+#endif
+	}
+	/* else, ignore it */
+
+	return;
+}
 
 /*+
  *
@@ -1735,84 +1648,79 @@ s_vMgrRxDisassociation(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 static
 void
 s_vMgrRxDeauthentication(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    PSRxMgmtPacket pRxPacket
-    )
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	PSRxMgmtPacket pRxPacket
+)
 {
-    WLAN_FR_DEAUTHEN    sFrame;
-    unsigned int uNodeIndex = 0;
-    viawget_wpa_header *wpahdr;
-
-
-    if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP ){
-        //Todo:
-        // if is acting an AP..
-        // a STA is leaving this BSS..
-        sFrame.len = pRxPacket->cbMPDULen;
-        sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
-        if (BSSDBbIsSTAInNodeDB(pMgmt, pRxPacket->p80211Header->sA3.abyAddr2, &uNodeIndex)) {
-            BSSvRemoveOneNode(pDevice, uNodeIndex);
-        }
-        else {
-            DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Rx deauth, sta not found\n");
-        }
-    }
-    else {
-        if (pMgmt->eCurrMode == WMAC_MODE_ESS_STA ) {
-            sFrame.len = pRxPacket->cbMPDULen;
-            sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
-            vMgrDecodeDeauthen(&sFrame);
-            DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO  "AP deauthed me, reason=%d.\n", cpu_to_le16((*(sFrame.pwReason))));
-            // TODO: update BSS list for specific BSSID if pre-authentication case
-            if (!compare_ether_addr(sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID)) {
-                if (pMgmt->eCurrState >= WMAC_STATE_AUTHPENDING) {
-                    pMgmt->sNodeDBTable[0].bActive = false;
-                    pMgmt->eCurrMode = WMAC_MODE_STANDBY;
-                    pMgmt->eCurrState = WMAC_STATE_IDLE;
-                    netif_stop_queue(pDevice->dev);
-                    pDevice->bLinkPass = false;
-                }
-            }
-
-            if ((pDevice->bWPADEVUp) && (pDevice->skb != NULL)) {
-                 wpahdr = (viawget_wpa_header *)pDevice->skb->data;
-                 wpahdr->type = VIAWGET_DISASSOC_MSG;
-                 wpahdr->resp_ie_len = 0;
-                 wpahdr->req_ie_len = 0;
-                 skb_put(pDevice->skb, sizeof(viawget_wpa_header));
-                 pDevice->skb->dev = pDevice->wpadev;
-		 skb_reset_mac_header(pDevice->skb);
-                 pDevice->skb->pkt_type = PACKET_HOST;
-                 pDevice->skb->protocol = htons(ETH_P_802_2);
-                 memset(pDevice->skb->cb, 0, sizeof(pDevice->skb->cb));
-                 netif_rx(pDevice->skb);
-                 pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
-           }
-
-   #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-  // if(pDevice->bWPASuppWextEnabled == true)
-      {
-	union iwreq_data  wrqu;
-	memset(&wrqu, 0, sizeof (wrqu));
-        wrqu.ap_addr.sa_family = ARPHRD_ETHER;
-	PRINT_K("wireless_send_event--->SIOCGIWAP(disauthen)\n");
-	wireless_send_event(pDevice->dev, SIOCGIWAP, &wrqu, NULL);
-     }
-  #endif
-
-        }
-        /* else, ignore it.  TODO: IBSS authentication service
-            would be implemented here */
-    };
-    return;
-}
+	WLAN_FR_DEAUTHEN    sFrame;
+	unsigned int uNodeIndex = 0;
+	viawget_wpa_header *wpahdr;
+
+	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
+		//Todo:
+		// if is acting an AP..
+		// a STA is leaving this BSS..
+		sFrame.len = pRxPacket->cbMPDULen;
+		sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
+		if (BSSDBbIsSTAInNodeDB(pMgmt, pRxPacket->p80211Header->sA3.abyAddr2, &uNodeIndex)) {
+			BSSvRemoveOneNode(pDevice, uNodeIndex);
+		} else {
+			DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Rx deauth, sta not found\n");
+		}
+	} else {
+		if (pMgmt->eCurrMode == WMAC_MODE_ESS_STA) {
+			sFrame.len = pRxPacket->cbMPDULen;
+			sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
+			vMgrDecodeDeauthen(&sFrame);
+			DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO  "AP deauthed me, reason=%d.\n", cpu_to_le16((*(sFrame.pwReason))));
+			// TODO: update BSS list for specific BSSID if pre-authentication case
+			if (!compare_ether_addr(sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID)) {
+				if (pMgmt->eCurrState >= WMAC_STATE_AUTHPENDING) {
+					pMgmt->sNodeDBTable[0].bActive = false;
+					pMgmt->eCurrMode = WMAC_MODE_STANDBY;
+					pMgmt->eCurrState = WMAC_STATE_IDLE;
+					netif_stop_queue(pDevice->dev);
+					pDevice->bLinkPass = false;
+				}
+			}
+
+			if ((pDevice->bWPADEVUp) && (pDevice->skb != NULL)) {
+				wpahdr = (viawget_wpa_header *)pDevice->skb->data;
+				wpahdr->type = VIAWGET_DISASSOC_MSG;
+				wpahdr->resp_ie_len = 0;
+				wpahdr->req_ie_len = 0;
+				skb_put(pDevice->skb, sizeof(viawget_wpa_header));
+				pDevice->skb->dev = pDevice->wpadev;
+				skb_reset_mac_header(pDevice->skb);
+				pDevice->skb->pkt_type = PACKET_HOST;
+				pDevice->skb->protocol = htons(ETH_P_802_2);
+				memset(pDevice->skb->cb, 0, sizeof(pDevice->skb->cb));
+				netif_rx(pDevice->skb);
+				pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
+			}
 
+#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
+			{
+				union iwreq_data  wrqu;
+				memset(&wrqu, 0, sizeof(wrqu));
+				wrqu.ap_addr.sa_family = ARPHRD_ETHER;
+				PRINT_K("wireless_send_event--->SIOCGIWAP(disauthen)\n");
+				wireless_send_event(pDevice->dev, SIOCGIWAP, &wrqu, NULL);
+			}
+#endif
+
+		}
+		/* else, ignore it.  TODO: IBSS authentication service
+		   would be implemented here */
+	};
+	return;
+}
 
 //2008-8-4 <add> by chester
 /*+
@@ -1825,33 +1733,32 @@ s_vMgrRxDeauthentication(
  * Return Value:
  *               True:exceed;
  *                False:normal case
--*/
+ -*/
 static bool
 ChannelExceedZoneType(
-    PSDevice pDevice,
-    unsigned char byCurrChannel
-    )
+	PSDevice pDevice,
+	unsigned char byCurrChannel
+)
 {
-  bool exceed=false;
+	bool exceed = false;
 
-  switch(pDevice->byZoneType) {
-  	case 0x00:                  //USA:1~11
-                     if((byCurrChannel<1) ||(byCurrChannel>11))
-	                exceed = true;
-	         break;
+	switch (pDevice->byZoneType) {
+	case 0x00:                  //USA:1~11
+		if ((byCurrChannel < 1) || (byCurrChannel > 11))
+			exceed = true;
+		break;
 	case 0x01:                  //Japan:1~13
 	case 0x02:                  //Europe:1~13
-                     if((byCurrChannel<1) ||(byCurrChannel>13))
-	                exceed = true;
-	         break;
+		if ((byCurrChannel < 1) || (byCurrChannel > 13))
+			exceed = true;
+		break;
 	default:                    //reserve for other zonetype
 		break;
-  }
+	}
 
-  return exceed;
+	return exceed;
 }
 
-
 /*+
  *
  * Routine Description:
@@ -1861,517 +1768,487 @@ ChannelExceedZoneType(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 static
 void
 s_vMgrRxBeacon(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    PSRxMgmtPacket pRxPacket,
-    bool bInScan
-    )
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	PSRxMgmtPacket pRxPacket,
+	bool bInScan
+)
 {
+	PKnownBSS           pBSSList;
+	WLAN_FR_BEACON      sFrame;
+	QWORD               qwTSFOffset;
+	bool bIsBSSIDEqual = false;
+	bool bIsSSIDEqual = false;
+	bool bTSFLargeDiff = false;
+	bool bTSFOffsetPostive = false;
+	bool bUpdateTSF = false;
+	bool bIsAPBeacon = false;
+	bool bIsChannelEqual = false;
+	unsigned int uLocateByteIndex;
+	unsigned char byTIMBitOn = 0;
+	unsigned short wAIDNumber = 0;
+	unsigned int uNodeIndex;
+	QWORD               qwTimestamp, qwLocalTSF;
+	QWORD               qwCurrTSF;
+	unsigned short wStartIndex = 0;
+	unsigned short wAIDIndex = 0;
+	unsigned char byCurrChannel = pRxPacket->byRxChannel;
+	ERPObject           sERP;
+	unsigned int uRateLen = WLAN_RATES_MAXLEN;
+	bool bChannelHit = false;
+	bool bUpdatePhyParameter = false;
+	unsigned char byIEChannel = 0;
+
+	memset(&sFrame, 0, sizeof(WLAN_FR_BEACON));
+	sFrame.len = pRxPacket->cbMPDULen;
+	sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
+
+	// decode the beacon frame
+	vMgrDecodeBeacon(&sFrame);
+
+	if ((sFrame.pwBeaconInterval == 0) ||
+	    (sFrame.pwCapInfo == 0) ||
+	    (sFrame.pSSID == 0) ||
+	    (sFrame.pSuppRates == 0)) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Rx beacon frame error\n");
+		return;
+	}
 
-    PKnownBSS           pBSSList;
-    WLAN_FR_BEACON      sFrame;
-    QWORD               qwTSFOffset;
-    bool bIsBSSIDEqual = false;
-    bool bIsSSIDEqual = false;
-    bool bTSFLargeDiff = false;
-    bool bTSFOffsetPostive = false;
-    bool bUpdateTSF = false;
-    bool bIsAPBeacon = false;
-    bool bIsChannelEqual = false;
-    unsigned int uLocateByteIndex;
-    unsigned char byTIMBitOn = 0;
-    unsigned short wAIDNumber = 0;
-    unsigned int uNodeIndex;
-    QWORD               qwTimestamp, qwLocalTSF;
-    QWORD               qwCurrTSF;
-    unsigned short wStartIndex = 0;
-    unsigned short wAIDIndex = 0;
-    unsigned char byCurrChannel = pRxPacket->byRxChannel;
-    ERPObject           sERP;
-    unsigned int uRateLen = WLAN_RATES_MAXLEN;
-    bool bChannelHit = false;
-    bool bUpdatePhyParameter = false;
-    unsigned char byIEChannel = 0;
-
-
-    memset(&sFrame, 0, sizeof(WLAN_FR_BEACON));
-    sFrame.len = pRxPacket->cbMPDULen;
-    sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
-
-    // decode the beacon frame
-    vMgrDecodeBeacon(&sFrame);
-
-    if ((sFrame.pwBeaconInterval == 0) ||
-        (sFrame.pwCapInfo == 0) ||
-        (sFrame.pSSID == 0) ||
-        (sFrame.pSuppRates == 0) ) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Rx beacon frame error\n");
-        return;
-    }
-
-
-    if (sFrame.pDSParms != NULL) {
-        if (byCurrChannel > CB_MAX_CHANNEL_24G) {
-            // channel remapping to
-            byIEChannel = get_channel_mapping(pDevice, sFrame.pDSParms->byCurrChannel, PHY_TYPE_11A);
-        } else {
-            byIEChannel = sFrame.pDSParms->byCurrChannel;
-        }
-        if (byCurrChannel != byIEChannel) {
-            // adjust channel info. bcs we rcv adjacent channel packets
-            bChannelHit = false;
-            byCurrChannel = byIEChannel;
-        }
-    } else {
-        // no DS channel info
-        bChannelHit = true;
-    }
+	if (sFrame.pDSParms != NULL) {
+		if (byCurrChannel > CB_MAX_CHANNEL_24G) {
+			// channel remapping to
+			byIEChannel = get_channel_mapping(pDevice, sFrame.pDSParms->byCurrChannel, PHY_TYPE_11A);
+		} else {
+			byIEChannel = sFrame.pDSParms->byCurrChannel;
+		}
+		if (byCurrChannel != byIEChannel) {
+			// adjust channel info. bcs we rcv adjacent channel packets
+			bChannelHit = false;
+			byCurrChannel = byIEChannel;
+		}
+	} else {
+		// no DS channel info
+		bChannelHit = true;
+	}
 //2008-0730-01<Add>by MikeLiu
-if(ChannelExceedZoneType(pDevice,byCurrChannel)==true)
-      return;
-
-    if (sFrame.pERP != NULL) {
-        sERP.byERP = sFrame.pERP->byContext;
-        sERP.bERPExist = true;
-
-    } else {
-        sERP.bERPExist = false;
-        sERP.byERP = 0;
-    }
-
-    pBSSList = BSSpAddrIsInBSSList((void *)pDevice, sFrame.pHdr->sA3.abyAddr3, sFrame.pSSID);
-    if (pBSSList == NULL) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Beacon/insert: RxChannel = : %d\n", byCurrChannel);
-        BSSbInsertToBSSList((void *)pDevice,
-                            sFrame.pHdr->sA3.abyAddr3,
-                            *sFrame.pqwTimestamp,
-                            *sFrame.pwBeaconInterval,
-                            *sFrame.pwCapInfo,
-                            byCurrChannel,
-                            sFrame.pSSID,
-                            sFrame.pSuppRates,
-                            sFrame.pExtSuppRates,
-                            &sERP,
-                            sFrame.pRSN,
-                            sFrame.pRSNWPA,
-                            sFrame.pIE_Country,
-                            sFrame.pIE_Quiet,
-                            sFrame.len - WLAN_HDR_ADDR3_LEN,
-                            sFrame.pHdr->sA4.abyAddr4,   // payload of beacon
-                            (void *)pRxPacket
-                           );
-    }
-    else {
-//        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"update bcn: RxChannel = : %d\n", byCurrChannel);
-        BSSbUpdateToBSSList((void *)pDevice,
-                            *sFrame.pqwTimestamp,
-                            *sFrame.pwBeaconInterval,
-                            *sFrame.pwCapInfo,
-                            byCurrChannel,
-                            bChannelHit,
-                            sFrame.pSSID,
-                            sFrame.pSuppRates,
-                            sFrame.pExtSuppRates,
-                            &sERP,
-                            sFrame.pRSN,
-                            sFrame.pRSNWPA,
-                            sFrame.pIE_Country,
-                            sFrame.pIE_Quiet,
-                            pBSSList,
-                            sFrame.len - WLAN_HDR_ADDR3_LEN,
-                            sFrame.pHdr->sA4.abyAddr4,   // payload of probresponse
-                            (void *)pRxPacket
-                           );
-
-    }
-
-    if (bInScan) {
-        return;
-    }
-
-    if(byCurrChannel == (unsigned char)pMgmt->uCurrChannel)
-       bIsChannelEqual = true;
-
-    if (bIsChannelEqual && (pMgmt->eCurrMode == WMAC_MODE_ESS_AP)) {
-
-        // if rx beacon without ERP field
-        if (sERP.bERPExist) {
-            if (WLAN_GET_ERP_USE_PROTECTION(sERP.byERP)){
-                pDevice->byERPFlag |= WLAN_SET_ERP_USE_PROTECTION(1);
-                pDevice->wUseProtectCntDown = USE_PROTECT_PERIOD;
-            }
-        }
-        else {
-            pDevice->byERPFlag |= WLAN_SET_ERP_USE_PROTECTION(1);
-            pDevice->wUseProtectCntDown = USE_PROTECT_PERIOD;
-        }
-
-        if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
-            if(!WLAN_GET_CAP_INFO_SHORTPREAMBLE(*sFrame.pwCapInfo))
-                pDevice->byERPFlag |= WLAN_SET_ERP_BARKER_MODE(1);
-            if(!sERP.bERPExist)
-                pDevice->byERPFlag |= WLAN_SET_ERP_NONERP_PRESENT(1);
-        }
-
-        // set to MAC&BBP
-        if (WLAN_GET_ERP_USE_PROTECTION(pDevice->byERPFlag)){
-            if (!pDevice->bProtectMode) {
-                 MACvEnableProtectMD(pDevice->PortOffset);
-                 pDevice->bProtectMode = true;
-            }
-        }
-    }
-
-
-    if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP)
-        return;
-
-    // check if BSSID the same
-    if (memcmp(sFrame.pHdr->sA3.abyAddr3,
-               pMgmt->abyCurrBSSID,
-               WLAN_BSSID_LEN) == 0) {
-
-        bIsBSSIDEqual = true;
+	if (ChannelExceedZoneType(pDevice, byCurrChannel) == true)
+		return;
+
+	if (sFrame.pERP != NULL) {
+		sERP.byERP = sFrame.pERP->byContext;
+		sERP.bERPExist = true;
+
+	} else {
+		sERP.bERPExist = false;
+		sERP.byERP = 0;
+	}
+
+	pBSSList = BSSpAddrIsInBSSList((void *)pDevice, sFrame.pHdr->sA3.abyAddr3, sFrame.pSSID);
+	if (pBSSList == NULL) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Beacon/insert: RxChannel = : %d\n", byCurrChannel);
+		BSSbInsertToBSSList((void *)pDevice,
+				    sFrame.pHdr->sA3.abyAddr3,
+				    *sFrame.pqwTimestamp,
+				    *sFrame.pwBeaconInterval,
+				    *sFrame.pwCapInfo,
+				    byCurrChannel,
+				    sFrame.pSSID,
+				    sFrame.pSuppRates,
+				    sFrame.pExtSuppRates,
+				    &sERP,
+				    sFrame.pRSN,
+				    sFrame.pRSNWPA,
+				    sFrame.pIE_Country,
+				    sFrame.pIE_Quiet,
+				    sFrame.len - WLAN_HDR_ADDR3_LEN,
+				    sFrame.pHdr->sA4.abyAddr4,   // payload of beacon
+				    (void *)pRxPacket
+);
+	} else {
+//        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "update bcn: RxChannel = : %d\n", byCurrChannel);
+		BSSbUpdateToBSSList((void *)pDevice,
+				    *sFrame.pqwTimestamp,
+				    *sFrame.pwBeaconInterval,
+				    *sFrame.pwCapInfo,
+				    byCurrChannel,
+				    bChannelHit,
+				    sFrame.pSSID,
+				    sFrame.pSuppRates,
+				    sFrame.pExtSuppRates,
+				    &sERP,
+				    sFrame.pRSN,
+				    sFrame.pRSNWPA,
+				    sFrame.pIE_Country,
+				    sFrame.pIE_Quiet,
+				    pBSSList,
+				    sFrame.len - WLAN_HDR_ADDR3_LEN,
+				    sFrame.pHdr->sA4.abyAddr4,   // payload of probresponse
+				    (void *)pRxPacket
+);
+
+	}
+
+	if (bInScan) {
+		return;
+	}
+
+	if (byCurrChannel == (unsigned char)pMgmt->uCurrChannel)
+		bIsChannelEqual = true;
+
+	if (bIsChannelEqual && (pMgmt->eCurrMode == WMAC_MODE_ESS_AP)) {
+		// if rx beacon without ERP field
+		if (sERP.bERPExist) {
+			if (WLAN_GET_ERP_USE_PROTECTION(sERP.byERP)) {
+				pDevice->byERPFlag |= WLAN_SET_ERP_USE_PROTECTION(1);
+				pDevice->wUseProtectCntDown = USE_PROTECT_PERIOD;
+			}
+		} else {
+			pDevice->byERPFlag |= WLAN_SET_ERP_USE_PROTECTION(1);
+			pDevice->wUseProtectCntDown = USE_PROTECT_PERIOD;
+		}
+
+		if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
+			if (!WLAN_GET_CAP_INFO_SHORTPREAMBLE(*sFrame.pwCapInfo))
+				pDevice->byERPFlag |= WLAN_SET_ERP_BARKER_MODE(1);
+			if (!sERP.bERPExist)
+				pDevice->byERPFlag |= WLAN_SET_ERP_NONERP_PRESENT(1);
+		}
+
+		// set to MAC&BBP
+		if (WLAN_GET_ERP_USE_PROTECTION(pDevice->byERPFlag)) {
+			if (!pDevice->bProtectMode) {
+				MACvEnableProtectMD(pDevice->PortOffset);
+				pDevice->bProtectMode = true;
+			}
+		}
+	}
+
+	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP)
+		return;
+
+	// check if BSSID the same
+	if (memcmp(sFrame.pHdr->sA3.abyAddr3,
+		   pMgmt->abyCurrBSSID,
+		   WLAN_BSSID_LEN) == 0) {
+		bIsBSSIDEqual = true;
 
 // 2008-05-21 <add> by Richardtai
-        pDevice->uCurrRSSI = pRxPacket->uRSSI;
-        pDevice->byCurrSQ = pRxPacket->bySQ;
-
-        if (pMgmt->sNodeDBTable[0].uInActiveCount != 0) {
-            pMgmt->sNodeDBTable[0].uInActiveCount = 0;
-            //DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"BCN:Wake Count= [%d]\n", pMgmt->wCountToWakeUp);
-        }
-    }
-    // check if SSID the same
-    if (sFrame.pSSID->len == ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len) {
-        if (memcmp(sFrame.pSSID->abySSID,
-                   ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->abySSID,
-                   sFrame.pSSID->len
-                   ) == 0) {
-            bIsSSIDEqual = true;
-        }
-    }
-
-    if ((WLAN_GET_CAP_INFO_ESS(*sFrame.pwCapInfo)== true) &&
-        (bIsBSSIDEqual == true) &&
-        (bIsSSIDEqual == true) &&
-        (pMgmt->eCurrMode == WMAC_MODE_ESS_STA) &&
-        (pMgmt->eCurrState == WMAC_STATE_ASSOC)) {
-        // add state check to prevent reconnect fail since we'll receive Beacon
-
-        bIsAPBeacon = true;
-
-        if (pBSSList != NULL) {
-
-            // Compare PHY parameter setting
-            if (pMgmt->wCurrCapInfo != pBSSList->wCapInfo) {
-                bUpdatePhyParameter = true;
-                pMgmt->wCurrCapInfo = pBSSList->wCapInfo;
-            }
-            if (sFrame.pERP != NULL) {
-                if ((sFrame.pERP->byElementID == WLAN_EID_ERP) &&
-                    (pMgmt->byERPContext != sFrame.pERP->byContext)) {
-                    bUpdatePhyParameter = true;
-                    pMgmt->byERPContext = sFrame.pERP->byContext;
-                }
-            }
-            //
-            // Basic Rate Set may change dynamically
-            //
-            if (pBSSList->eNetworkTypeInUse == PHY_TYPE_11B) {
-                uRateLen = WLAN_RATES_MAXLEN_11B;
-            }
-            pMgmt->abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)pBSSList->abySuppRates,
-                                                    (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-                                                    uRateLen);
-            pMgmt->abyCurrExtSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)pBSSList->abyExtSuppRates,
-                                                    (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates,
-                                                    uRateLen);
-            RATEvParseMaxRate( (void *)pDevice,
-                               (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-                               (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates,
-                               true,
-                               &(pMgmt->sNodeDBTable[0].wMaxBasicRate),
-                               &(pMgmt->sNodeDBTable[0].wMaxSuppRate),
-                               &(pMgmt->sNodeDBTable[0].wSuppRate),
-                               &(pMgmt->sNodeDBTable[0].byTopCCKBasicRate),
-                               &(pMgmt->sNodeDBTable[0].byTopOFDMBasicRate)
-                              );
-#ifdef	PLICE_DEBUG
-		//printk("RxBeacon:MaxSuppRate is %d\n",pMgmt->sNodeDBTable[0].wMaxSuppRate);
-#endif
+		pDevice->uCurrRSSI = pRxPacket->uRSSI;
+		pDevice->byCurrSQ = pRxPacket->bySQ;
+
+		if (pMgmt->sNodeDBTable[0].uInActiveCount != 0) {
+			pMgmt->sNodeDBTable[0].uInActiveCount = 0;
+			//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BCN:Wake Count= [%d]\n", pMgmt->wCountToWakeUp);
+		}
+	}
+	// check if SSID the same
+	if (sFrame.pSSID->len == ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len) {
+		if (memcmp(sFrame.pSSID->abySSID,
+			   ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->abySSID,
+			   sFrame.pSSID->len
+) == 0) {
+			bIsSSIDEqual = true;
+		}
+	}
+
+	if ((WLAN_GET_CAP_INFO_ESS(*sFrame.pwCapInfo) == true) &&
+	    (bIsBSSIDEqual == true) &&
+	    (bIsSSIDEqual == true) &&
+	    (pMgmt->eCurrMode == WMAC_MODE_ESS_STA) &&
+	    (pMgmt->eCurrState == WMAC_STATE_ASSOC)) {
+		// add state check to prevent reconnect fail since we'll receive Beacon
+
+		bIsAPBeacon = true;
+
+		if (pBSSList != NULL) {
+			// Compare PHY parameter setting
+			if (pMgmt->wCurrCapInfo != pBSSList->wCapInfo) {
+				bUpdatePhyParameter = true;
+				pMgmt->wCurrCapInfo = pBSSList->wCapInfo;
+			}
+			if (sFrame.pERP != NULL) {
+				if ((sFrame.pERP->byElementID == WLAN_EID_ERP) &&
+				    (pMgmt->byERPContext != sFrame.pERP->byContext)) {
+					bUpdatePhyParameter = true;
+					pMgmt->byERPContext = sFrame.pERP->byContext;
+				}
+			}
+			//
+			// Basic Rate Set may change dynamically
+			//
+			if (pBSSList->eNetworkTypeInUse == PHY_TYPE_11B) {
+				uRateLen = WLAN_RATES_MAXLEN_11B;
+			}
+			pMgmt->abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)pBSSList->abySuppRates,
+								(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
+								uRateLen);
+			pMgmt->abyCurrExtSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)pBSSList->abyExtSuppRates,
+								   (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates,
+								   uRateLen);
+			RATEvParseMaxRate((void *)pDevice,
+					  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
+					  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates,
+					  true,
+					  &(pMgmt->sNodeDBTable[0].wMaxBasicRate),
+					  &(pMgmt->sNodeDBTable[0].wMaxSuppRate),
+					  &(pMgmt->sNodeDBTable[0].wSuppRate),
+					  &(pMgmt->sNodeDBTable[0].byTopCCKBasicRate),
+					  &(pMgmt->sNodeDBTable[0].byTopOFDMBasicRate)
+				);
 			if (bUpdatePhyParameter == true) {
-                CARDbSetPhyParameter( pMgmt->pAdapter,
-                                      pMgmt->eCurrentPHYMode,
-                                      pMgmt->wCurrCapInfo,
-                                      pMgmt->byERPContext,
-                                      pMgmt->abyCurrSuppRates,
-                                      pMgmt->abyCurrExtSuppRates
-                                      );
-            }
-            if (sFrame.pIE_PowerConstraint != NULL) {
-                CARDvSetPowerConstraint(pMgmt->pAdapter,
-                                        (unsigned char) pBSSList->uChannel,
-                                        sFrame.pIE_PowerConstraint->byPower
-                                        );
-            }
-            if (sFrame.pIE_CHSW != NULL) {
-                CARDbChannelSwitch( pMgmt->pAdapter,
-                                    sFrame.pIE_CHSW->byMode,
-                                    get_channel_mapping(pMgmt->pAdapter, sFrame.pIE_CHSW->byMode, pMgmt->eCurrentPHYMode),
-                                    sFrame.pIE_CHSW->byCount
-                                    );
-
-            } else if (bIsChannelEqual == false) {
-                set_channel(pMgmt->pAdapter, pBSSList->uChannel);
-            }
-        }
-    }
-
-//    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Beacon 2 \n");
-    // check if CF field exists
-    if (WLAN_GET_CAP_INFO_ESS(*sFrame.pwCapInfo)) {
-        if (sFrame.pCFParms->wCFPDurRemaining > 0) {
-            // TODO: deal with CFP period to set NAV
-        }
-    }
-
-    HIDWORD(qwTimestamp) = cpu_to_le32(HIDWORD(*sFrame.pqwTimestamp));
-    LODWORD(qwTimestamp) = cpu_to_le32(LODWORD(*sFrame.pqwTimestamp));
-    HIDWORD(qwLocalTSF) = HIDWORD(pRxPacket->qwLocalTSF);
-    LODWORD(qwLocalTSF) = LODWORD(pRxPacket->qwLocalTSF);
-
-    // check if beacon TSF larger or small than our local TSF
-    if (HIDWORD(qwTimestamp) == HIDWORD(qwLocalTSF)) {
-        if (LODWORD(qwTimestamp) >= LODWORD(qwLocalTSF)) {
-            bTSFOffsetPostive = true;
-        }
-        else {
-            bTSFOffsetPostive = false;
-        }
-    }
-    else if (HIDWORD(qwTimestamp) > HIDWORD(qwLocalTSF)) {
-        bTSFOffsetPostive = true;
-    }
-    else if (HIDWORD(qwTimestamp) < HIDWORD(qwLocalTSF)) {
-        bTSFOffsetPostive = false;
-    }
-
-    if (bTSFOffsetPostive) {
-        qwTSFOffset = CARDqGetTSFOffset(pRxPacket->byRxRate, (qwTimestamp), (qwLocalTSF));
-    }
-    else {
-        qwTSFOffset = CARDqGetTSFOffset(pRxPacket->byRxRate, (qwLocalTSF), (qwTimestamp));
-    }
-
-    if (HIDWORD(qwTSFOffset) != 0 ||
-        (LODWORD(qwTSFOffset) > TRIVIAL_SYNC_DIFFERENCE )) {
-         bTSFLargeDiff = true;
-    }
-
-
-    // if infra mode
-    if (bIsAPBeacon == true) {
-
-        // Infra mode: Local TSF always follow AP's TSF if Difference huge.
-        if (bTSFLargeDiff)
-            bUpdateTSF = true;
-
-        if ((pDevice->bEnablePSMode == true) &&(sFrame.pTIM != 0)) {
-
-            // deal with DTIM, analysis TIM
-            pMgmt->bMulticastTIM = WLAN_MGMT_IS_MULTICAST_TIM(sFrame.pTIM->byBitMapCtl) ? true : false ;
-            pMgmt->byDTIMCount = sFrame.pTIM->byDTIMCount;
-            pMgmt->byDTIMPeriod = sFrame.pTIM->byDTIMPeriod;
-            wAIDNumber = pMgmt->wCurrAID & ~(BIT14|BIT15);
-
-            // check if AID in TIM field bit on
-            // wStartIndex = N1
-            wStartIndex = WLAN_MGMT_GET_TIM_OFFSET(sFrame.pTIM->byBitMapCtl) << 1;
-            // AIDIndex = N2
-            wAIDIndex = (wAIDNumber >> 3);
-            if ((wAIDNumber > 0) && (wAIDIndex >= wStartIndex)) {
-                uLocateByteIndex = wAIDIndex - wStartIndex;
-                // len = byDTIMCount + byDTIMPeriod + byDTIMPeriod + byVirtBitMap[0~250]
-                if (sFrame.pTIM->len >= (uLocateByteIndex + 4)) {
-                    byTIMBitOn  = (0x01) << ((wAIDNumber) % 8);
-                    pMgmt->bInTIM = sFrame.pTIM->byVirtBitMap[uLocateByteIndex] & byTIMBitOn ? true : false;
-                }
-                else {
-                    pMgmt->bInTIM = false;
-                };
-            }
-            else {
-                pMgmt->bInTIM = false;
-            };
-
-            if (pMgmt->bInTIM ||
-                (pMgmt->bMulticastTIM && (pMgmt->byDTIMCount == 0))) {
-                pMgmt->bInTIMWake = true;
-                // send out ps-poll packet
+				CARDbSetPhyParameter(pMgmt->pAdapter,
+						     pMgmt->eCurrentPHYMode,
+						     pMgmt->wCurrCapInfo,
+						     pMgmt->byERPContext,
+						     pMgmt->abyCurrSuppRates,
+						     pMgmt->abyCurrExtSuppRates
+					);
+			}
+			if (sFrame.pIE_PowerConstraint != NULL) {
+				CARDvSetPowerConstraint(pMgmt->pAdapter,
+							(unsigned char) pBSSList->uChannel,
+							sFrame.pIE_PowerConstraint->byPower
+);
+			}
+			if (sFrame.pIE_CHSW != NULL) {
+				CARDbChannelSwitch(pMgmt->pAdapter,
+						   sFrame.pIE_CHSW->byMode,
+						   get_channel_mapping(pMgmt->pAdapter, sFrame.pIE_CHSW->byMode, pMgmt->eCurrentPHYMode),
+						   sFrame.pIE_CHSW->byCount
+					);
+
+			} else if (bIsChannelEqual == false) {
+				set_channel(pMgmt->pAdapter, pBSSList->uChannel);
+			}
+		}
+	}
+
+//    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Beacon 2 \n");
+	// check if CF field exists
+	if (WLAN_GET_CAP_INFO_ESS(*sFrame.pwCapInfo)) {
+		if (sFrame.pCFParms->wCFPDurRemaining > 0) {
+			// TODO: deal with CFP period to set NAV
+		}
+	}
+
+	HIDWORD(qwTimestamp) = cpu_to_le32(HIDWORD(*sFrame.pqwTimestamp));
+	LODWORD(qwTimestamp) = cpu_to_le32(LODWORD(*sFrame.pqwTimestamp));
+	HIDWORD(qwLocalTSF) = HIDWORD(pRxPacket->qwLocalTSF);
+	LODWORD(qwLocalTSF) = LODWORD(pRxPacket->qwLocalTSF);
+
+	// check if beacon TSF larger or small than our local TSF
+	if (HIDWORD(qwTimestamp) == HIDWORD(qwLocalTSF)) {
+		if (LODWORD(qwTimestamp) >= LODWORD(qwLocalTSF)) {
+			bTSFOffsetPostive = true;
+		} else {
+			bTSFOffsetPostive = false;
+		}
+	} else if (HIDWORD(qwTimestamp) > HIDWORD(qwLocalTSF)) {
+		bTSFOffsetPostive = true;
+	} else if (HIDWORD(qwTimestamp) < HIDWORD(qwLocalTSF)) {
+		bTSFOffsetPostive = false;
+	}
+
+	if (bTSFOffsetPostive) {
+		qwTSFOffset = CARDqGetTSFOffset(pRxPacket->byRxRate, (qwTimestamp), (qwLocalTSF));
+	} else {
+		qwTSFOffset = CARDqGetTSFOffset(pRxPacket->byRxRate, (qwLocalTSF), (qwTimestamp));
+	}
+
+	if (HIDWORD(qwTSFOffset) != 0 ||
+	    (LODWORD(qwTSFOffset) > TRIVIAL_SYNC_DIFFERENCE)) {
+		bTSFLargeDiff = true;
+	}
+
+	// if infra mode
+	if (bIsAPBeacon == true) {
+		// Infra mode: Local TSF always follow AP's TSF if Difference huge.
+		if (bTSFLargeDiff)
+			bUpdateTSF = true;
+
+		if ((pDevice->bEnablePSMode == true) && (sFrame.pTIM != 0)) {
+			// deal with DTIM, analysis TIM
+			pMgmt->bMulticastTIM = WLAN_MGMT_IS_MULTICAST_TIM(sFrame.pTIM->byBitMapCtl) ? true : false;
+			pMgmt->byDTIMCount = sFrame.pTIM->byDTIMCount;
+			pMgmt->byDTIMPeriod = sFrame.pTIM->byDTIMPeriod;
+			wAIDNumber = pMgmt->wCurrAID & ~(BIT14|BIT15);
+
+			// check if AID in TIM field bit on
+			// wStartIndex = N1
+			wStartIndex = WLAN_MGMT_GET_TIM_OFFSET(sFrame.pTIM->byBitMapCtl) << 1;
+			// AIDIndex = N2
+			wAIDIndex = (wAIDNumber >> 3);
+			if ((wAIDNumber > 0) && (wAIDIndex >= wStartIndex)) {
+				uLocateByteIndex = wAIDIndex - wStartIndex;
+				// len = byDTIMCount + byDTIMPeriod + byDTIMPeriod + byVirtBitMap[0~250]
+				if (sFrame.pTIM->len >= (uLocateByteIndex + 4)) {
+					byTIMBitOn  = (0x01) << ((wAIDNumber) % 8);
+					pMgmt->bInTIM = sFrame.pTIM->byVirtBitMap[uLocateByteIndex] & byTIMBitOn ? true : false;
+				} else {
+					pMgmt->bInTIM = false;
+				};
+			} else {
+				pMgmt->bInTIM = false;
+			};
+
+			if (pMgmt->bInTIM ||
+			    (pMgmt->bMulticastTIM && (pMgmt->byDTIMCount == 0))) {
+				pMgmt->bInTIMWake = true;
+				// send out ps-poll packet
 //                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BCN:In TIM\n");
-                if (pMgmt->bInTIM) {
-                    PSvSendPSPOLL((PSDevice)pDevice);
+				if (pMgmt->bInTIM) {
+					PSvSendPSPOLL((PSDevice)pDevice);
 //                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BCN:PS-POLL sent..\n");
-                }
-
-            }
-            else {
-                pMgmt->bInTIMWake = false;
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BCN: Not In TIM..\n");
-                if (pDevice->bPWBitOn == false) {
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BCN: Send Null Packet\n");
-                    if (PSbSendNullPacket(pDevice))
-                        pDevice->bPWBitOn = true;
-                }
-                if(PSbConsiderPowerDown(pDevice, false, false)) {
-                   DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BCN: Power down now...\n");
-                }
-            }
-
-        }
-
-    }
-    // if adhoc mode
-    if ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) && !bIsAPBeacon && bIsChannelEqual) {
-        if (bIsBSSIDEqual) {
-            // Use sNodeDBTable[0].uInActiveCount as IBSS beacons received count.
-		    if (pMgmt->sNodeDBTable[0].uInActiveCount != 0)
-		 	    pMgmt->sNodeDBTable[0].uInActiveCount = 0;
-
-            // adhoc mode:TSF updated only when beacon larger than local TSF
-            if (bTSFLargeDiff && bTSFOffsetPostive &&
-                (pMgmt->eCurrState == WMAC_STATE_JOINTED))
-                bUpdateTSF = true;
-
-            // During dpc, already in spinlocked.
-            if (BSSDBbIsSTAInNodeDB(pMgmt, sFrame.pHdr->sA3.abyAddr2, &uNodeIndex)) {
-
-                // Update the STA, (Technically the Beacons of all the IBSS nodes
-		        // should be identical, but that's not happening in practice.
-                pMgmt->abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pSuppRates,
-                                                        (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-                                                        WLAN_RATES_MAXLEN_11B);
-                RATEvParseMaxRate( (void *)pDevice,
-                                   (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-                                   NULL,
-                                   true,
-                                   &(pMgmt->sNodeDBTable[uNodeIndex].wMaxBasicRate),
-                                   &(pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate),
-                                   &(pMgmt->sNodeDBTable[uNodeIndex].wSuppRate),
-                                   &(pMgmt->sNodeDBTable[uNodeIndex].byTopCCKBasicRate),
-                                   &(pMgmt->sNodeDBTable[uNodeIndex].byTopOFDMBasicRate)
-                                  );
-                pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble = WLAN_GET_CAP_INFO_SHORTPREAMBLE(*sFrame.pwCapInfo);
-                pMgmt->sNodeDBTable[uNodeIndex].bShortSlotTime = WLAN_GET_CAP_INFO_SHORTSLOTTIME(*sFrame.pwCapInfo);
-                pMgmt->sNodeDBTable[uNodeIndex].uInActiveCount = 0;
-            }
-            else {
-                // Todo, initial Node content
-                BSSvCreateOneNode((PSDevice)pDevice, &uNodeIndex);
-
-                pMgmt->abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pSuppRates,
-                                                        (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-                                                        WLAN_RATES_MAXLEN_11B);
-                RATEvParseMaxRate( (void *)pDevice,
-                                   (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-                                   NULL,
-                                   true,
-                                   &(pMgmt->sNodeDBTable[uNodeIndex].wMaxBasicRate),
-                                   &(pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate),
-                                   &(pMgmt->sNodeDBTable[uNodeIndex].wSuppRate),
-                                   &(pMgmt->sNodeDBTable[uNodeIndex].byTopCCKBasicRate),
-                                   &(pMgmt->sNodeDBTable[uNodeIndex].byTopOFDMBasicRate)
-                                 );
-
-                memcpy(pMgmt->sNodeDBTable[uNodeIndex].abyMACAddr, sFrame.pHdr->sA3.abyAddr2, WLAN_ADDR_LEN);
-                pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble = WLAN_GET_CAP_INFO_SHORTPREAMBLE(*sFrame.pwCapInfo);
-                pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate = pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate;
-#ifdef	PLICE_DEBUG
-		//if (uNodeIndex == 0)
-		{
-			printk("s_vMgrRxBeacon:TxDataRate is %d,Index is %d\n",pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate,uNodeIndex);
+				}
+
+			} else {
+				pMgmt->bInTIMWake = false;
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BCN: Not In TIM..\n");
+				if (pDevice->bPWBitOn == false) {
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BCN: Send Null Packet\n");
+					if (PSbSendNullPacket(pDevice))
+						pDevice->bPWBitOn = true;
+				}
+				if (PSbConsiderPowerDown(pDevice, false, false)) {
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BCN: Power down now...\n");
+				}
+			}
+
 		}
+
+	}
+	// if adhoc mode
+	if ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) && !bIsAPBeacon && bIsChannelEqual) {
+		if (bIsBSSIDEqual) {
+			// Use sNodeDBTable[0].uInActiveCount as IBSS beacons received count.
+			if (pMgmt->sNodeDBTable[0].uInActiveCount != 0)
+				pMgmt->sNodeDBTable[0].uInActiveCount = 0;
+
+			// adhoc mode:TSF updated only when beacon larger than local TSF
+			if (bTSFLargeDiff && bTSFOffsetPostive &&
+			    (pMgmt->eCurrState == WMAC_STATE_JOINTED))
+				bUpdateTSF = true;
+
+			// During dpc, already in spinlocked.
+			if (BSSDBbIsSTAInNodeDB(pMgmt, sFrame.pHdr->sA3.abyAddr2, &uNodeIndex)) {
+				// Update the STA, (Technically the Beacons of all the IBSS nodes
+				// should be identical, but that's not happening in practice.
+				pMgmt->abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pSuppRates,
+									(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
+									WLAN_RATES_MAXLEN_11B);
+				RATEvParseMaxRate((void *)pDevice,
+						  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
+						  NULL,
+						  true,
+						  &(pMgmt->sNodeDBTable[uNodeIndex].wMaxBasicRate),
+						  &(pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate),
+						  &(pMgmt->sNodeDBTable[uNodeIndex].wSuppRate),
+						  &(pMgmt->sNodeDBTable[uNodeIndex].byTopCCKBasicRate),
+						  &(pMgmt->sNodeDBTable[uNodeIndex].byTopOFDMBasicRate)
+					);
+				pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble = WLAN_GET_CAP_INFO_SHORTPREAMBLE(*sFrame.pwCapInfo);
+				pMgmt->sNodeDBTable[uNodeIndex].bShortSlotTime = WLAN_GET_CAP_INFO_SHORTSLOTTIME(*sFrame.pwCapInfo);
+				pMgmt->sNodeDBTable[uNodeIndex].uInActiveCount = 0;
+			} else {
+				// Todo, initial Node content
+				BSSvCreateOneNode((PSDevice)pDevice, &uNodeIndex);
+
+				pMgmt->abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pSuppRates,
+									(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
+									WLAN_RATES_MAXLEN_11B);
+				RATEvParseMaxRate((void *)pDevice,
+						  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
+						  NULL,
+						  true,
+						  &(pMgmt->sNodeDBTable[uNodeIndex].wMaxBasicRate),
+						  &(pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate),
+						  &(pMgmt->sNodeDBTable[uNodeIndex].wSuppRate),
+						  &(pMgmt->sNodeDBTable[uNodeIndex].byTopCCKBasicRate),
+						  &(pMgmt->sNodeDBTable[uNodeIndex].byTopOFDMBasicRate)
+					);
+
+				memcpy(pMgmt->sNodeDBTable[uNodeIndex].abyMACAddr, sFrame.pHdr->sA3.abyAddr2, WLAN_ADDR_LEN);
+				pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble = WLAN_GET_CAP_INFO_SHORTPREAMBLE(*sFrame.pwCapInfo);
+				pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate = pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate;
+#ifdef	PLICE_DEBUG
+				{
+					printk("s_vMgrRxBeacon:TxDataRate is %d,Index is %d\n", pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate, uNodeIndex);
+				}
 #endif
 /*
-                pMgmt->sNodeDBTable[uNodeIndex].bShortSlotTime = WLAN_GET_CAP_INFO_SHORTSLOTTIME(*sFrame.pwCapInfo);
-                if(pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate > RATE_11M)
-                       pMgmt->sNodeDBTable[uNodeIndex].bERPExist = true;
+  pMgmt->sNodeDBTable[uNodeIndex].bShortSlotTime = WLAN_GET_CAP_INFO_SHORTSLOTTIME(*sFrame.pwCapInfo);
+  if (pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate > RATE_11M)
+  pMgmt->sNodeDBTable[uNodeIndex].bERPExist = true;
 */
-            }
-
-            // if other stations joined, indicate connection to upper layer..
-            if (pMgmt->eCurrState == WMAC_STATE_STARTED) {
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Current IBSS State: [Started]........to: [Jointed] \n");
-                pMgmt->eCurrState = WMAC_STATE_JOINTED;
-                pDevice->bLinkPass = true;
-                if (netif_queue_stopped(pDevice->dev)){
-                    netif_wake_queue(pDevice->dev);
-                }
-                pMgmt->sNodeDBTable[0].bActive = true;
-                pMgmt->sNodeDBTable[0].uInActiveCount = 0;
-
-            }
-        }
-        else if (bIsSSIDEqual) {
-
-            // See other adhoc sta with the same SSID but BSSID is different.
-            // adpot this vars only when TSF larger then us.
-            if (bTSFLargeDiff && bTSFOffsetPostive) {
-                 // we don't support ATIM under adhoc mode
-               // if ( sFrame.pIBSSParms->wATIMWindow == 0) {
-                     // adpot this vars
-                     // TODO: check sFrame cap if privacy on, and support rate syn
-                     memcpy(pMgmt->abyCurrBSSID, sFrame.pHdr->sA3.abyAddr3, WLAN_BSSID_LEN);
-                     memcpy(pDevice->abyBSSID, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-                     pMgmt->wCurrATIMWindow = cpu_to_le16(sFrame.pIBSSParms->wATIMWindow);
-                     pMgmt->wCurrBeaconPeriod = cpu_to_le16(*sFrame.pwBeaconInterval);
-                     pMgmt->abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pSuppRates,
-                                                      (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-                                                      WLAN_RATES_MAXLEN_11B);
-                     // set HW beacon interval and re-synchronizing....
-                     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Rejoining to Other Adhoc group with same SSID........\n");
-                     VNSvOutPortW(pDevice->PortOffset + MAC_REG_BI, pMgmt->wCurrBeaconPeriod);
-                     CARDbUpdateTSF(pDevice, pRxPacket->byRxRate, qwTimestamp, qwLocalTSF);
-                     CARDvUpdateNextTBTT(pDevice->PortOffset, qwTimestamp, pMgmt->wCurrBeaconPeriod);
-                     // Turn off bssid filter to avoid filter others adhoc station which bssid is different.
-                     MACvWriteBSSIDAddress(pDevice->PortOffset, pMgmt->abyCurrBSSID);
-
-                     CARDbSetPhyParameter (  pMgmt->pAdapter,
-                                            pMgmt->eCurrentPHYMode,
-                                            pMgmt->wCurrCapInfo,
-                                            pMgmt->byERPContext,
-                                            pMgmt->abyCurrSuppRates,
-                                            pMgmt->abyCurrExtSuppRates);
-
-
-                     // MACvRegBitsOff(pDevice->PortOffset, MAC_REG_RCR, RCR_BSSID);
-                     // set highest basic rate
-                     // s_vSetHighestBasicRate(pDevice, (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates);
-                     // Prepare beacon frame
-                     bMgrPrepareBeaconToSend((void *)pDevice, pMgmt);
-              //  }
-            }
-        }
-    }
-    // endian issue ???
-    // Update TSF
-    if (bUpdateTSF) {
-        CARDbGetCurrentTSF(pDevice->PortOffset, &qwCurrTSF);
-        CARDbUpdateTSF(pDevice, pRxPacket->byRxRate, qwTimestamp, pRxPacket->qwLocalTSF);
-        CARDbGetCurrentTSF(pDevice->PortOffset, &qwCurrTSF);
-        CARDvUpdateNextTBTT(pDevice->PortOffset, qwTimestamp, pMgmt->wCurrBeaconPeriod);
-    }
-
-    return;
-}
-
+			}
+
+			// if other stations joined, indicate connection to upper layer..
+			if (pMgmt->eCurrState == WMAC_STATE_STARTED) {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Current IBSS State: [Started]........to: [Jointed] \n");
+				pMgmt->eCurrState = WMAC_STATE_JOINTED;
+				pDevice->bLinkPass = true;
+				if (netif_queue_stopped(pDevice->dev)) {
+					netif_wake_queue(pDevice->dev);
+				}
+				pMgmt->sNodeDBTable[0].bActive = true;
+				pMgmt->sNodeDBTable[0].uInActiveCount = 0;
+
+			}
+		} else if (bIsSSIDEqual) {
+			// See other adhoc sta with the same SSID but BSSID is different.
+			// adpot this vars only when TSF larger then us.
+			if (bTSFLargeDiff && bTSFOffsetPostive) {
+				// we don't support ATIM under adhoc mode
+				// if (sFrame.pIBSSParms->wATIMWindow == 0) {
+				// adpot this vars
+				// TODO: check sFrame cap if privacy on, and support rate syn
+				memcpy(pMgmt->abyCurrBSSID, sFrame.pHdr->sA3.abyAddr3, WLAN_BSSID_LEN);
+				memcpy(pDevice->abyBSSID, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
+				pMgmt->wCurrATIMWindow = cpu_to_le16(sFrame.pIBSSParms->wATIMWindow);
+				pMgmt->wCurrBeaconPeriod = cpu_to_le16(*sFrame.pwBeaconInterval);
+				pMgmt->abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pSuppRates,
+									(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
+									WLAN_RATES_MAXLEN_11B);
+				// set HW beacon interval and re-synchronizing....
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Rejoining to Other Adhoc group with same SSID........\n");
+				VNSvOutPortW(pDevice->PortOffset + MAC_REG_BI, pMgmt->wCurrBeaconPeriod);
+				CARDbUpdateTSF(pDevice, pRxPacket->byRxRate, qwTimestamp, qwLocalTSF);
+				CARDvUpdateNextTBTT(pDevice->PortOffset, qwTimestamp, pMgmt->wCurrBeaconPeriod);
+				// Turn off bssid filter to avoid filter others adhoc station which bssid is different.
+				MACvWriteBSSIDAddress(pDevice->PortOffset, pMgmt->abyCurrBSSID);
+
+				CARDbSetPhyParameter(pMgmt->pAdapter,
+						     pMgmt->eCurrentPHYMode,
+						     pMgmt->wCurrCapInfo,
+						     pMgmt->byERPContext,
+						     pMgmt->abyCurrSuppRates,
+						     pMgmt->abyCurrExtSuppRates);
+
+				// MACvRegBitsOff(pDevice->PortOffset, MAC_REG_RCR, RCR_BSSID);
+				// set highest basic rate
+				// s_vSetHighestBasicRate(pDevice, (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates);
+				// Prepare beacon frame
+				bMgrPrepareBeaconToSend((void *)pDevice, pMgmt);
+				//  }
+			}
+		}
+	}
+	// endian issue ???
+	// Update TSF
+if (bUpdateTSF) {
+		CARDbGetCurrentTSF(pDevice->PortOffset, &qwCurrTSF);
+		CARDbUpdateTSF(pDevice, pRxPacket->byRxRate, qwTimestamp, pRxPacket->qwLocalTSF);
+		CARDbGetCurrentTSF(pDevice->PortOffset, &qwCurrTSF);
+		CARDvUpdateNextTBTT(pDevice->PortOffset, qwTimestamp, pMgmt->wCurrBeaconPeriod);
+	}
 
+	return;
+}
 
 /*+
  *
@@ -2384,248 +2261,239 @@ if(ChannelExceedZoneType(pDevice,byCurrChannel)==true)
  * Return Value:
  *    CMD_STATUS
  *
--*/
+ -*/
 void
 vMgrCreateOwnIBSS(
-    void *hDeviceContext,
-    PCMD_STATUS pStatus
-    )
+	void *hDeviceContext,
+	PCMD_STATUS pStatus
+)
 {
-    PSDevice            pDevice = (PSDevice)hDeviceContext;
-    PSMgmtObject        pMgmt = pDevice->pMgmt;
-    unsigned short wMaxBasicRate;
-    unsigned short wMaxSuppRate;
-    unsigned char byTopCCKBasicRate;
-    unsigned char byTopOFDMBasicRate;
-    QWORD               qwCurrTSF;
-    unsigned int ii;
-    unsigned char abyRATE[] = {0x82, 0x84, 0x8B, 0x96, 0x24, 0x30, 0x48, 0x6C, 0x0C, 0x12, 0x18, 0x60};
-    unsigned char abyCCK_RATE[] = {0x82, 0x84, 0x8B, 0x96};
-    unsigned char abyOFDM_RATE[] = {0x0C, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C};
-    unsigned short wSuppRate;
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Create Basic Service Set .......\n");
-
-    if (pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA) {
-        if ((pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) &&
-            (pDevice->eEncryptionStatus != Ndis802_11Encryption2Enabled) &&
-            (pDevice->eEncryptionStatus != Ndis802_11Encryption3Enabled)) {
-            // encryption mode error
-            *pStatus = CMD_STATUS_FAILURE;
-            return;
-        }
-    }
-
-    pMgmt->abyCurrSuppRates[0] = WLAN_EID_SUPP_RATES;
-    pMgmt->abyCurrExtSuppRates[0] = WLAN_EID_EXTSUPP_RATES;
-
-    if (pMgmt->eConfigMode == WMAC_CONFIG_AP) {
-        pMgmt->eCurrentPHYMode = pMgmt->byAPBBType;
-    } else {
-        if (pDevice->byBBType == BB_TYPE_11G)
-            pMgmt->eCurrentPHYMode = PHY_TYPE_11G;
-        if (pDevice->byBBType == BB_TYPE_11B)
-            pMgmt->eCurrentPHYMode = PHY_TYPE_11B;
-        if (pDevice->byBBType == BB_TYPE_11A)
-            pMgmt->eCurrentPHYMode = PHY_TYPE_11A;
-    }
-
-    if (pMgmt->eCurrentPHYMode != PHY_TYPE_11A) {
-        pMgmt->abyCurrSuppRates[1] = WLAN_RATES_MAXLEN_11B;
-        pMgmt->abyCurrExtSuppRates[1] = 0;
-        for (ii = 0; ii < 4; ii++)
-            pMgmt->abyCurrSuppRates[2+ii] = abyRATE[ii];
-    } else {
-        pMgmt->abyCurrSuppRates[1] = 8;
-        pMgmt->abyCurrExtSuppRates[1] = 0;
-        for (ii = 0; ii < 8; ii++)
-            pMgmt->abyCurrSuppRates[2+ii] = abyRATE[ii];
-    }
-
-
-    if (pMgmt->eCurrentPHYMode == PHY_TYPE_11G) {
-        pMgmt->abyCurrSuppRates[1] = 8;
-        pMgmt->abyCurrExtSuppRates[1] = 4;
-        for (ii = 0; ii < 4; ii++)
-            pMgmt->abyCurrSuppRates[2+ii] =  abyCCK_RATE[ii];
-        for (ii = 4; ii < 8; ii++)
-            pMgmt->abyCurrSuppRates[2+ii] =  abyOFDM_RATE[ii-4];
-        for (ii = 0; ii < 4; ii++)
-            pMgmt->abyCurrExtSuppRates[2+ii] =  abyOFDM_RATE[ii+4];
-    }
-
-
-    // Disable Protect Mode
-    pDevice->bProtectMode = 0;
-    MACvDisableProtectMD(pDevice->PortOffset);
-
-    pDevice->bBarkerPreambleMd = 0;
-    MACvDisableBarkerPreambleMd(pDevice->PortOffset);
-
-    // Kyle Test 2003.11.04
-
-    // set HW beacon interval
-    if (pMgmt->wIBSSBeaconPeriod == 0)
-        pMgmt->wIBSSBeaconPeriod = DEFAULT_IBSS_BI;
-
-
-    CARDbGetCurrentTSF(pDevice->PortOffset, &qwCurrTSF);
-    // clear TSF counter
-    VNSvOutPortB(pDevice->PortOffset + MAC_REG_TFTCTL, TFTCTL_TSFCNTRST);
-    // enable TSF counter
-    VNSvOutPortB(pDevice->PortOffset + MAC_REG_TFTCTL, TFTCTL_TSFCNTREN);
-
-    // set Next TBTT
-    CARDvSetFirstNextTBTT(pDevice->PortOffset, pMgmt->wIBSSBeaconPeriod);
-
-    pMgmt->uIBSSChannel = pDevice->uChannel;
-
-    if (pMgmt->uIBSSChannel == 0)
-        pMgmt->uIBSSChannel = DEFAULT_IBSS_CHANNEL;
-
-
-    // set basic rate
-
-    RATEvParseMaxRate((void *)pDevice, (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-                      (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates, true,
-                      &wMaxBasicRate, &wMaxSuppRate, &wSuppRate,
-                      &byTopCCKBasicRate, &byTopOFDMBasicRate);
-
-
-    if (pMgmt->eConfigMode == WMAC_CONFIG_AP) {
-        pMgmt->eCurrMode = WMAC_MODE_ESS_AP;
-    }
-
-    if (pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA) {
-        memcpy(pMgmt->abyIBSSDFSOwner, pDevice->abyCurrentNetAddr, 6);
-        pMgmt->byIBSSDFSRecovery = 10;
-        pMgmt->eCurrMode = WMAC_MODE_IBSS_STA;
-    }
-
-    // Adopt pre-configured IBSS vars to current vars
-    pMgmt->eCurrState = WMAC_STATE_STARTED;
-    pMgmt->wCurrBeaconPeriod = pMgmt->wIBSSBeaconPeriod;
-    pMgmt->uCurrChannel = pMgmt->uIBSSChannel;
-    pMgmt->wCurrATIMWindow = pMgmt->wIBSSATIMWindow;
-    MACvWriteATIMW(pDevice->PortOffset, pMgmt->wCurrATIMWindow);
-    pDevice->uCurrRSSI = 0;
-    pDevice->byCurrSQ = 0;
-    //memcpy(pMgmt->abyDesireSSID,pMgmt->abyAdHocSSID,
-                     // ((PWLAN_IE_SSID)pMgmt->abyAdHocSSID)->len + WLAN_IEHDR_LEN);
-    memset(pMgmt->abyCurrSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-    memcpy(pMgmt->abyCurrSSID,
-           pMgmt->abyDesireSSID,
-           ((PWLAN_IE_SSID)pMgmt->abyDesireSSID)->len + WLAN_IEHDR_LEN
-          );
-
-    if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-        // AP mode BSSID = MAC addr
-        memcpy(pMgmt->abyCurrBSSID, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-	DBG_PRT(MSG_LEVEL_INFO, KERN_INFO"AP beacon created BSSID:%pM\n",
-		pMgmt->abyCurrBSSID);
-    }
-
-    if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
-
-        // BSSID selected must be randomized as spec 11.1.3
-        pMgmt->abyCurrBSSID[5] = (unsigned char) (LODWORD(qwCurrTSF)& 0x000000ff);
-        pMgmt->abyCurrBSSID[4] = (unsigned char)((LODWORD(qwCurrTSF)& 0x0000ff00) >> 8);
-        pMgmt->abyCurrBSSID[3] = (unsigned char)((LODWORD(qwCurrTSF)& 0x00ff0000) >> 16);
-        pMgmt->abyCurrBSSID[2] = (unsigned char)((LODWORD(qwCurrTSF)& 0x00000ff0) >> 4);
-        pMgmt->abyCurrBSSID[1] = (unsigned char)((LODWORD(qwCurrTSF)& 0x000ff000) >> 12);
-        pMgmt->abyCurrBSSID[0] = (unsigned char)((LODWORD(qwCurrTSF)& 0x0ff00000) >> 20);
-        pMgmt->abyCurrBSSID[5] ^= pMgmt->abyMACAddr[0];
-        pMgmt->abyCurrBSSID[4] ^= pMgmt->abyMACAddr[1];
-        pMgmt->abyCurrBSSID[3] ^= pMgmt->abyMACAddr[2];
-        pMgmt->abyCurrBSSID[2] ^= pMgmt->abyMACAddr[3];
-        pMgmt->abyCurrBSSID[1] ^= pMgmt->abyMACAddr[4];
-        pMgmt->abyCurrBSSID[0] ^= pMgmt->abyMACAddr[5];
-        pMgmt->abyCurrBSSID[0] &= ~IEEE_ADDR_GROUP;
-        pMgmt->abyCurrBSSID[0] |= IEEE_ADDR_UNIVERSAL;
-
-
-	DBG_PRT(MSG_LEVEL_INFO, KERN_INFO"Adhoc beacon created bssid:%pM\n",
-		pMgmt->abyCurrBSSID);
-    }
-
-    // Set Capability Info
-    pMgmt->wCurrCapInfo = 0;
-
-    if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-        pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_ESS(1);
-        pMgmt->byDTIMPeriod = DEFAULT_DTIM_PERIOD;
-        pMgmt->byDTIMCount = pMgmt->byDTIMPeriod - 1;
-    }
-
-    if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
-        pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_IBSS(1);
-    }
-
-    if (pDevice->bEncryptionEnable) {
-        pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_PRIVACY(1);
-        if (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) {
-            if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled) {
-                pMgmt->byCSSPK = KEY_CTL_CCMP;
-                pMgmt->byCSSGK = KEY_CTL_CCMP;
-            } else if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled) {
-                pMgmt->byCSSPK = KEY_CTL_TKIP;
-                pMgmt->byCSSGK = KEY_CTL_TKIP;
-            } else {
-                pMgmt->byCSSPK = KEY_CTL_NONE;
-                pMgmt->byCSSGK = KEY_CTL_WEP;
-            }
-        } else {
-            pMgmt->byCSSPK = KEY_CTL_WEP;
-            pMgmt->byCSSGK = KEY_CTL_WEP;
-        }
-    }
-
-    pMgmt->byERPContext = 0;
+	PSDevice            pDevice = (PSDevice)hDeviceContext;
+	PSMgmtObject        pMgmt = pDevice->pMgmt;
+	unsigned short wMaxBasicRate;
+	unsigned short wMaxSuppRate;
+	unsigned char byTopCCKBasicRate;
+	unsigned char byTopOFDMBasicRate;
+	QWORD               qwCurrTSF;
+	unsigned int ii;
+	unsigned char abyRATE[] = {0x82, 0x84, 0x8B, 0x96, 0x24, 0x30, 0x48, 0x6C, 0x0C, 0x12, 0x18, 0x60};
+	unsigned char abyCCK_RATE[] = {0x82, 0x84, 0x8B, 0x96};
+	unsigned char abyOFDM_RATE[] = {0x0C, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C};
+	unsigned short wSuppRate;
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Create Basic Service Set .......\n");
+
+	if (pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA) {
+		if ((pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) &&
+		    (pDevice->eEncryptionStatus != Ndis802_11Encryption2Enabled) &&
+		    (pDevice->eEncryptionStatus != Ndis802_11Encryption3Enabled)) {
+			// encryption mode error
+			*pStatus = CMD_STATUS_FAILURE;
+			return;
+		}
+	}
+
+	pMgmt->abyCurrSuppRates[0] = WLAN_EID_SUPP_RATES;
+	pMgmt->abyCurrExtSuppRates[0] = WLAN_EID_EXTSUPP_RATES;
+
+	if (pMgmt->eConfigMode == WMAC_CONFIG_AP) {
+		pMgmt->eCurrentPHYMode = pMgmt->byAPBBType;
+	} else {
+		if (pDevice->byBBType == BB_TYPE_11G)
+			pMgmt->eCurrentPHYMode = PHY_TYPE_11G;
+		if (pDevice->byBBType == BB_TYPE_11B)
+			pMgmt->eCurrentPHYMode = PHY_TYPE_11B;
+		if (pDevice->byBBType == BB_TYPE_11A)
+			pMgmt->eCurrentPHYMode = PHY_TYPE_11A;
+	}
+
+	if (pMgmt->eCurrentPHYMode != PHY_TYPE_11A) {
+		pMgmt->abyCurrSuppRates[1] = WLAN_RATES_MAXLEN_11B;
+		pMgmt->abyCurrExtSuppRates[1] = 0;
+		for (ii = 0; ii < 4; ii++)
+			pMgmt->abyCurrSuppRates[2+ii] = abyRATE[ii];
+	} else {
+		pMgmt->abyCurrSuppRates[1] = 8;
+		pMgmt->abyCurrExtSuppRates[1] = 0;
+		for (ii = 0; ii < 8; ii++)
+			pMgmt->abyCurrSuppRates[2+ii] = abyRATE[ii];
+	}
+
+	if (pMgmt->eCurrentPHYMode == PHY_TYPE_11G) {
+		pMgmt->abyCurrSuppRates[1] = 8;
+		pMgmt->abyCurrExtSuppRates[1] = 4;
+		for (ii = 0; ii < 4; ii++)
+			pMgmt->abyCurrSuppRates[2+ii] =  abyCCK_RATE[ii];
+		for (ii = 4; ii < 8; ii++)
+			pMgmt->abyCurrSuppRates[2+ii] =  abyOFDM_RATE[ii-4];
+		for (ii = 0; ii < 4; ii++)
+			pMgmt->abyCurrExtSuppRates[2+ii] =  abyOFDM_RATE[ii+4];
+	}
+
+	// Disable Protect Mode
+	pDevice->bProtectMode = 0;
+	MACvDisableProtectMD(pDevice->PortOffset);
+
+	pDevice->bBarkerPreambleMd = 0;
+	MACvDisableBarkerPreambleMd(pDevice->PortOffset);
+
+	// Kyle Test 2003.11.04
+
+	// set HW beacon interval
+	if (pMgmt->wIBSSBeaconPeriod == 0)
+		pMgmt->wIBSSBeaconPeriod = DEFAULT_IBSS_BI;
+
+	CARDbGetCurrentTSF(pDevice->PortOffset, &qwCurrTSF);
+	// clear TSF counter
+	VNSvOutPortB(pDevice->PortOffset + MAC_REG_TFTCTL, TFTCTL_TSFCNTRST);
+	// enable TSF counter
+	VNSvOutPortB(pDevice->PortOffset + MAC_REG_TFTCTL, TFTCTL_TSFCNTREN);
+
+	// set Next TBTT
+	CARDvSetFirstNextTBTT(pDevice->PortOffset, pMgmt->wIBSSBeaconPeriod);
+
+	pMgmt->uIBSSChannel = pDevice->uChannel;
+
+	if (pMgmt->uIBSSChannel == 0)
+		pMgmt->uIBSSChannel = DEFAULT_IBSS_CHANNEL;
+
+	// set basic rate
+
+	RATEvParseMaxRate((void *)pDevice, (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
+			  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates, true,
+			  &wMaxBasicRate, &wMaxSuppRate, &wSuppRate,
+			  &byTopCCKBasicRate, &byTopOFDMBasicRate);
+
+	if (pMgmt->eConfigMode == WMAC_CONFIG_AP) {
+		pMgmt->eCurrMode = WMAC_MODE_ESS_AP;
+	}
+
+	if (pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA) {
+		memcpy(pMgmt->abyIBSSDFSOwner, pDevice->abyCurrentNetAddr, 6);
+		pMgmt->byIBSSDFSRecovery = 10;
+		pMgmt->eCurrMode = WMAC_MODE_IBSS_STA;
+	}
+
+	// Adopt pre-configured IBSS vars to current vars
+	pMgmt->eCurrState = WMAC_STATE_STARTED;
+	pMgmt->wCurrBeaconPeriod = pMgmt->wIBSSBeaconPeriod;
+	pMgmt->uCurrChannel = pMgmt->uIBSSChannel;
+	pMgmt->wCurrATIMWindow = pMgmt->wIBSSATIMWindow;
+	MACvWriteATIMW(pDevice->PortOffset, pMgmt->wCurrATIMWindow);
+	pDevice->uCurrRSSI = 0;
+	pDevice->byCurrSQ = 0;
+	//memcpy(pMgmt->abyDesireSSID,pMgmt->abyAdHocSSID,
+	// ((PWLAN_IE_SSID)pMgmt->abyAdHocSSID)->len + WLAN_IEHDR_LEN);
+	memset(pMgmt->abyCurrSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
+	memcpy(pMgmt->abyCurrSSID,
+	       pMgmt->abyDesireSSID,
+	       ((PWLAN_IE_SSID)pMgmt->abyDesireSSID)->len + WLAN_IEHDR_LEN
+);
+
+	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
+		// AP mode BSSID = MAC addr
+		memcpy(pMgmt->abyCurrBSSID, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
+		DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "AP beacon created BSSID:%pM\n",
+			pMgmt->abyCurrBSSID);
+	}
+
+	if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
+		// BSSID selected must be randomized as spec 11.1.3
+		pMgmt->abyCurrBSSID[5] = (unsigned char) (LODWORD(qwCurrTSF) & 0x000000ff);
+		pMgmt->abyCurrBSSID[4] = (unsigned char)((LODWORD(qwCurrTSF) & 0x0000ff00) >> 8);
+		pMgmt->abyCurrBSSID[3] = (unsigned char)((LODWORD(qwCurrTSF) & 0x00ff0000) >> 16);
+		pMgmt->abyCurrBSSID[2] = (unsigned char)((LODWORD(qwCurrTSF) & 0x00000ff0) >> 4);
+		pMgmt->abyCurrBSSID[1] = (unsigned char)((LODWORD(qwCurrTSF) & 0x000ff000) >> 12);
+		pMgmt->abyCurrBSSID[0] = (unsigned char)((LODWORD(qwCurrTSF) & 0x0ff00000) >> 20);
+		pMgmt->abyCurrBSSID[5] ^= pMgmt->abyMACAddr[0];
+		pMgmt->abyCurrBSSID[4] ^= pMgmt->abyMACAddr[1];
+		pMgmt->abyCurrBSSID[3] ^= pMgmt->abyMACAddr[2];
+		pMgmt->abyCurrBSSID[2] ^= pMgmt->abyMACAddr[3];
+		pMgmt->abyCurrBSSID[1] ^= pMgmt->abyMACAddr[4];
+		pMgmt->abyCurrBSSID[0] ^= pMgmt->abyMACAddr[5];
+		pMgmt->abyCurrBSSID[0] &= ~IEEE_ADDR_GROUP;
+		pMgmt->abyCurrBSSID[0] |= IEEE_ADDR_UNIVERSAL;
+
+		DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Adhoc beacon created bssid:%pM\n",
+			pMgmt->abyCurrBSSID);
+	}
+
+	// Set Capability Info
+	pMgmt->wCurrCapInfo = 0;
+
+	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
+		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_ESS(1);
+		pMgmt->byDTIMPeriod = DEFAULT_DTIM_PERIOD;
+		pMgmt->byDTIMCount = pMgmt->byDTIMPeriod - 1;
+	}
+
+	if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
+		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_IBSS(1);
+	}
+
+	if (pDevice->bEncryptionEnable) {
+		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_PRIVACY(1);
+		if (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) {
+			if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled) {
+				pMgmt->byCSSPK = KEY_CTL_CCMP;
+				pMgmt->byCSSGK = KEY_CTL_CCMP;
+			} else if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled) {
+				pMgmt->byCSSPK = KEY_CTL_TKIP;
+				pMgmt->byCSSGK = KEY_CTL_TKIP;
+			} else {
+				pMgmt->byCSSPK = KEY_CTL_NONE;
+				pMgmt->byCSSGK = KEY_CTL_WEP;
+			}
+		} else {
+			pMgmt->byCSSPK = KEY_CTL_WEP;
+			pMgmt->byCSSGK = KEY_CTL_WEP;
+		}
+	}
+
+	pMgmt->byERPContext = 0;
 
 //    memcpy(pDevice->abyBSSID, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
 
-    if (pMgmt->eConfigMode == WMAC_CONFIG_AP) {
-        CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_AP);
-    } else {
-        CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_ADHOC);
-    }
-
-    CARDbSetPhyParameter(   pMgmt->pAdapter,
-                            pMgmt->eCurrentPHYMode,
-                            pMgmt->wCurrCapInfo,
-                            pMgmt->byERPContext,
-                            pMgmt->abyCurrSuppRates,
-                            pMgmt->abyCurrExtSuppRates
-                            );
-
-    CARDbSetBeaconPeriod(pMgmt->pAdapter, pMgmt->wIBSSBeaconPeriod);
-    // set channel and clear NAV
-    set_channel(pMgmt->pAdapter, pMgmt->uIBSSChannel);
-    pMgmt->uCurrChannel = pMgmt->uIBSSChannel;
-
-    if (CARDbIsShortPreamble(pMgmt->pAdapter)) {
-        pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
-    } else {
-        pMgmt->wCurrCapInfo &= (~WLAN_SET_CAP_INFO_SHORTPREAMBLE(1));
-    }
-
-    if ((pMgmt->b11hEnable == true) &&
-        (pMgmt->eCurrentPHYMode == PHY_TYPE_11A)) {
-        pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SPECTRUMMNG(1);
-    } else {
-        pMgmt->wCurrCapInfo &= (~WLAN_SET_CAP_INFO_SPECTRUMMNG(1));
-    }
-
-    pMgmt->eCurrState = WMAC_STATE_STARTED;
-    // Prepare beacon to send
-    if (bMgrPrepareBeaconToSend((void *)pDevice, pMgmt)) {
-        *pStatus = CMD_STATUS_SUCCESS;
-    }
-
-    return ;
-}
+	if (pMgmt->eConfigMode == WMAC_CONFIG_AP) {
+		CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_AP);
+	} else {
+		CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_ADHOC);
+	}
+
+	CARDbSetPhyParameter(pMgmt->pAdapter,
+			     pMgmt->eCurrentPHYMode,
+			     pMgmt->wCurrCapInfo,
+			     pMgmt->byERPContext,
+			     pMgmt->abyCurrSuppRates,
+			     pMgmt->abyCurrExtSuppRates
+		);
+
+	CARDbSetBeaconPeriod(pMgmt->pAdapter, pMgmt->wIBSSBeaconPeriod);
+	// set channel and clear NAV
+	set_channel(pMgmt->pAdapter, pMgmt->uIBSSChannel);
+	pMgmt->uCurrChannel = pMgmt->uIBSSChannel;
+
+	if (CARDbIsShortPreamble(pMgmt->pAdapter)) {
+		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
+	} else {
+		pMgmt->wCurrCapInfo &= (~WLAN_SET_CAP_INFO_SHORTPREAMBLE(1));
+	}
 
+	if ((pMgmt->b11hEnable == true) &&
+	    (pMgmt->eCurrentPHYMode == PHY_TYPE_11A)) {
+		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SPECTRUMMNG(1);
+	} else {
+		pMgmt->wCurrCapInfo &= (~WLAN_SET_CAP_INFO_SPECTRUMMNG(1));
+	}
+
+	pMgmt->eCurrState = WMAC_STATE_STARTED;
+	// Prepare beacon to send
+	if (bMgrPrepareBeaconToSend((void *)pDevice, pMgmt)) {
+		*pStatus = CMD_STATUS_SUCCESS;
+	}
 
+	return;
+}
 
 /*+
  *
@@ -2638,260 +2506,244 @@ vMgrCreateOwnIBSS(
  * Return Value:
  *    None.
  *
--*/
+ -*/
 
 void
 vMgrJoinBSSBegin(
-    void *hDeviceContext,
-    PCMD_STATUS pStatus
-    )
+	void *hDeviceContext,
+	PCMD_STATUS pStatus
+)
 {
+	PSDevice     pDevice = (PSDevice)hDeviceContext;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	PKnownBSS       pCurr = NULL;
+	unsigned int ii, uu;
+	PWLAN_IE_SUPP_RATES pItemRates = NULL;
+	PWLAN_IE_SUPP_RATES pItemExtRates = NULL;
+	PWLAN_IE_SSID   pItemSSID;
+	unsigned int uRateLen = WLAN_RATES_MAXLEN;
+	unsigned short wMaxBasicRate = RATE_1M;
+	unsigned short wMaxSuppRate = RATE_1M;
+	unsigned short wSuppRate;
+	unsigned char byTopCCKBasicRate = RATE_1M;
+	unsigned char byTopOFDMBasicRate = RATE_1M;
+
+	for (ii = 0; ii < MAX_BSS_NUM; ii++) {
+		if (pMgmt->sBSSList[ii].bActive == true)
+			break;
+	}
+
+	if (ii == MAX_BSS_NUM) {
+		*pStatus = CMD_STATUS_RESOURCES;
+		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "BSS finding:BSS list is empty.\n");
+		return;
+	}
+
+	// memset(pMgmt->abyDesireBSSID, 0,  WLAN_BSSID_LEN);
+	// Search known BSS list for prefer BSSID or SSID
 
-    PSDevice     pDevice = (PSDevice)hDeviceContext;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    PKnownBSS       pCurr = NULL;
-    unsigned int ii, uu;
-    PWLAN_IE_SUPP_RATES pItemRates = NULL;
-    PWLAN_IE_SUPP_RATES pItemExtRates = NULL;
-    PWLAN_IE_SSID   pItemSSID;
-    unsigned int uRateLen = WLAN_RATES_MAXLEN;
-    unsigned short wMaxBasicRate = RATE_1M;
-    unsigned short wMaxSuppRate = RATE_1M;
-    unsigned short wSuppRate;
-    unsigned char byTopCCKBasicRate = RATE_1M;
-    unsigned char byTopOFDMBasicRate = RATE_1M;
-
-
-    for (ii = 0; ii < MAX_BSS_NUM; ii++) {
-        if (pMgmt->sBSSList[ii].bActive == true)
-            break;
-    }
-
-    if (ii == MAX_BSS_NUM) {
-       *pStatus = CMD_STATUS_RESOURCES;
-        DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "BSS finding:BSS list is empty.\n");
-       return;
-    }
-
-    // memset(pMgmt->abyDesireBSSID, 0,  WLAN_BSSID_LEN);
-    // Search known BSS list for prefer BSSID or SSID
-
-    pCurr = BSSpSearchBSSList(pDevice,
-                              pMgmt->abyDesireBSSID,
-                              pMgmt->abyDesireSSID,
-                              pMgmt->eConfigPHYMode
-                              );
-
-    if (pCurr == NULL){
-       *pStatus = CMD_STATUS_RESOURCES;
-       pItemSSID = (PWLAN_IE_SSID)pMgmt->abyDesireSSID;
-       DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Scanning [%s] not found, disconnected !\n", pItemSSID->abySSID);
-       return;
-    }
-
-    DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "AP(BSS) finding:Found a AP(BSS)..\n");
-    if (WLAN_GET_CAP_INFO_ESS(cpu_to_le16(pCurr->wCapInfo))){
-
-        if ((pMgmt->eAuthenMode == WMAC_AUTH_WPA)||(pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK)) {
-
-    // patch for CISCO migration mode
+	pCurr = BSSpSearchBSSList(pDevice,
+				  pMgmt->abyDesireBSSID,
+				  pMgmt->abyDesireSSID,
+				  pMgmt->eConfigPHYMode
+);
+
+	if (pCurr == NULL) {
+		*pStatus = CMD_STATUS_RESOURCES;
+		pItemSSID = (PWLAN_IE_SSID)pMgmt->abyDesireSSID;
+		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Scanning [%s] not found, disconnected !\n", pItemSSID->abySSID);
+		return;
+	}
+
+	DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "AP(BSS) finding:Found a AP(BSS)..\n");
+	if (WLAN_GET_CAP_INFO_ESS(cpu_to_le16(pCurr->wCapInfo))) {
+		if ((pMgmt->eAuthenMode == WMAC_AUTH_WPA) || (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK)) {
+			// patch for CISCO migration mode
 /*
-            if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled) {
-                if (WPA_SearchRSN(0, WPA_TKIP, pCurr) == false) {
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"No match RSN info. ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
-                    // encryption mode error
-                    pMgmt->eCurrState = WMAC_STATE_IDLE;
-                    return;
-                }
-            } else if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled) {
-                if (WPA_SearchRSN(0, WPA_AESCCMP, pCurr) == false) {
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"No match RSN info. ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
-                    // encryption mode error
-                    pMgmt->eCurrState = WMAC_STATE_IDLE;
-                    return;
-                }
-            }
+  if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled) {
+  if (WPA_SearchRSN(0, WPA_TKIP, pCurr) == false) {
+  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "No match RSN info. ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
+  // encryption mode error
+  pMgmt->eCurrState = WMAC_STATE_IDLE;
+  return;
+  }
+  } else if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled) {
+  if (WPA_SearchRSN(0, WPA_AESCCMP, pCurr) == false) {
+  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "No match RSN info. ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
+  // encryption mode error
+  pMgmt->eCurrState = WMAC_STATE_IDLE;
+  return;
+  }
+  }
 */
-        }
+		}
 
 #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-	//if(pDevice->bWPASuppWextEnabled == true)
-            Encyption_Rebuild(pDevice, pCurr);
+		Encyption_Rebuild(pDevice, pCurr);
 #endif
-        // Infrastructure BSS
-        s_vMgrSynchBSS(pDevice,
-                       WMAC_MODE_ESS_STA,
-                       pCurr,
-                       pStatus
-                       );
-
-        if (*pStatus == CMD_STATUS_SUCCESS){
-
-            // Adopt this BSS state vars in Mgmt Object
-            pMgmt->uCurrChannel = pCurr->uChannel;
-
-            memset(pMgmt->abyCurrSuppRates, 0 , WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1);
-            memset(pMgmt->abyCurrExtSuppRates, 0 , WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1);
-
-            if (pCurr->eNetworkTypeInUse == PHY_TYPE_11B) {
-                uRateLen = WLAN_RATES_MAXLEN_11B;
-            }
-
-            pItemRates = (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates;
-            pItemExtRates = (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates;
-
-            // Parse Support Rate IE
-            pItemRates->byElementID = WLAN_EID_SUPP_RATES;
-            pItemRates->len = RATEuSetIE((PWLAN_IE_SUPP_RATES)pCurr->abySuppRates,
-                                         pItemRates,
-                                         uRateLen);
-
-            // Parse Extension Support Rate IE
-            pItemExtRates->byElementID = WLAN_EID_EXTSUPP_RATES;
-            pItemExtRates->len = RATEuSetIE((PWLAN_IE_SUPP_RATES)pCurr->abyExtSuppRates,
-                                            pItemExtRates,
-                                            uRateLen);
-            // Stuffing Rate IE
-            if ((pItemExtRates->len > 0) && (pItemRates->len < 8)) {
-                for (ii = 0; ii < (unsigned int)(8 - pItemRates->len); ) {
-                    pItemRates->abyRates[pItemRates->len + ii] = pItemExtRates->abyRates[ii];
-                    ii ++;
-                    if (pItemExtRates->len <= ii)
-                        break;
-                }
-                pItemRates->len += (unsigned char)ii;
-                if (pItemExtRates->len - ii > 0) {
-                    pItemExtRates->len -= (unsigned char)ii;
-                    for (uu = 0; uu < pItemExtRates->len; uu ++) {
-                        pItemExtRates->abyRates[uu] = pItemExtRates->abyRates[uu + ii];
-                    }
-                } else {
-                    pItemExtRates->len = 0;
-                }
-            }
-
-            RATEvParseMaxRate((void *)pDevice, pItemRates, pItemExtRates, true,
-                              &wMaxBasicRate, &wMaxSuppRate, &wSuppRate,
-                              &byTopCCKBasicRate, &byTopOFDMBasicRate);
-
-            // TODO: deal with if wCapInfo the privacy is on, but station WEP is off
-            // TODO: deal with if wCapInfo the PS-Pollable is on.
-            pMgmt->wCurrBeaconPeriod = pCurr->wBeaconInterval;
-            memset(pMgmt->abyCurrSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-            memcpy(pMgmt->abyCurrBSSID, pCurr->abyBSSID, WLAN_BSSID_LEN);
-            memcpy(pMgmt->abyCurrSSID, pCurr->abySSID, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-
-            pMgmt->eCurrMode = WMAC_MODE_ESS_STA;
-
-            pMgmt->eCurrState = WMAC_STATE_JOINTED;
-            // Adopt BSS state in Adapter Device Object
-            //pDevice->byOpMode = OP_MODE_INFRASTRUCTURE;
+		// Infrastructure BSS
+		s_vMgrSynchBSS(pDevice,
+			       WMAC_MODE_ESS_STA,
+			       pCurr,
+			       pStatus
+);
+
+		if (*pStatus == CMD_STATUS_SUCCESS) {
+			// Adopt this BSS state vars in Mgmt Object
+			pMgmt->uCurrChannel = pCurr->uChannel;
+
+			memset(pMgmt->abyCurrSuppRates, 0 , WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1);
+			memset(pMgmt->abyCurrExtSuppRates, 0 , WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1);
+
+			if (pCurr->eNetworkTypeInUse == PHY_TYPE_11B) {
+				uRateLen = WLAN_RATES_MAXLEN_11B;
+			}
+
+			pItemRates = (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates;
+			pItemExtRates = (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates;
+
+			// Parse Support Rate IE
+			pItemRates->byElementID = WLAN_EID_SUPP_RATES;
+			pItemRates->len = RATEuSetIE((PWLAN_IE_SUPP_RATES)pCurr->abySuppRates,
+						     pItemRates,
+						     uRateLen);
+
+			// Parse Extension Support Rate IE
+			pItemExtRates->byElementID = WLAN_EID_EXTSUPP_RATES;
+			pItemExtRates->len = RATEuSetIE((PWLAN_IE_SUPP_RATES)pCurr->abyExtSuppRates,
+							pItemExtRates,
+							uRateLen);
+			// Stuffing Rate IE
+			if ((pItemExtRates->len > 0) && (pItemRates->len < 8)) {
+				for (ii = 0; ii < (unsigned int)(8 - pItemRates->len);) {
+					pItemRates->abyRates[pItemRates->len + ii] = pItemExtRates->abyRates[ii];
+					ii++;
+					if (pItemExtRates->len <= ii)
+						break;
+				}
+				pItemRates->len += (unsigned char)ii;
+				if (pItemExtRates->len - ii > 0) {
+					pItemExtRates->len -= (unsigned char)ii;
+					for (uu = 0; uu < pItemExtRates->len; uu++) {
+						pItemExtRates->abyRates[uu] = pItemExtRates->abyRates[uu + ii];
+					}
+				} else {
+					pItemExtRates->len = 0;
+				}
+			}
+
+			RATEvParseMaxRate((void *)pDevice, pItemRates, pItemExtRates, true,
+					  &wMaxBasicRate, &wMaxSuppRate, &wSuppRate,
+					  &byTopCCKBasicRate, &byTopOFDMBasicRate);
+
+			// TODO: deal with if wCapInfo the privacy is on, but station WEP is off
+			// TODO: deal with if wCapInfo the PS-Pollable is on.
+			pMgmt->wCurrBeaconPeriod = pCurr->wBeaconInterval;
+			memset(pMgmt->abyCurrSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
+			memcpy(pMgmt->abyCurrBSSID, pCurr->abyBSSID, WLAN_BSSID_LEN);
+			memcpy(pMgmt->abyCurrSSID, pCurr->abySSID, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
+
+			pMgmt->eCurrMode = WMAC_MODE_ESS_STA;
+
+			pMgmt->eCurrState = WMAC_STATE_JOINTED;
+			// Adopt BSS state in Adapter Device Object
+			//pDevice->byOpMode = OP_MODE_INFRASTRUCTURE;
 //            memcpy(pDevice->abyBSSID, pCurr->abyBSSID, WLAN_BSSID_LEN);
 
-            // Add current BSS to Candidate list
-            // This should only works for WPA2 BSS, and WPA2 BSS check must be done before.
-            if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2) {
-                bool bResult = bAdd_PMKID_Candidate((void *)pDevice, pMgmt->abyCurrBSSID, &pCurr->sRSNCapObj);
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"bAdd_PMKID_Candidate: 1(%d)\n", bResult);
-                if (bResult == false) {
-                    vFlush_PMKID_Candidate((void *)pDevice);
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"vFlush_PMKID_Candidate: 4\n");
-                    bAdd_PMKID_Candidate((void *)pDevice, pMgmt->abyCurrBSSID, &pCurr->sRSNCapObj);
-                }
-            }
-
-            // Preamble type auto-switch: if AP can receive short-preamble cap,
-            // we can turn on too.
-
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Join ESS\n");
-
-
-
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"End of Join AP -- A/B/G Action\n");
-        }
-        else {
-            pMgmt->eCurrState = WMAC_STATE_IDLE;
-        };
-
-
-     }
-     else {
-        // ad-hoc mode BSS
-        if (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) {
-
-            if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled) {
-                if (WPA_SearchRSN(0, WPA_TKIP, pCurr) == false) {
-                    // encryption mode error
-                    pMgmt->eCurrState = WMAC_STATE_IDLE;
-                    return;
-                }
-            } else if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled) {
-                if (WPA_SearchRSN(0, WPA_AESCCMP, pCurr) == false) {
-                    // encryption mode error
-                    pMgmt->eCurrState = WMAC_STATE_IDLE;
-                    return;
-                }
-            } else {
-                // encryption mode error
-                pMgmt->eCurrState = WMAC_STATE_IDLE;
-                return;
-            }
-        }
-
-        s_vMgrSynchBSS(pDevice,
-                       WMAC_MODE_IBSS_STA,
-                       pCurr,
-                       pStatus
-                       );
-
-        if (*pStatus == CMD_STATUS_SUCCESS){
-            // Adopt this BSS state vars in Mgmt Object
-            // TODO: check if CapInfo privacy on, but we don't..
-            pMgmt->uCurrChannel = pCurr->uChannel;
-
-
-            // Parse Support Rate IE
-            pMgmt->abyCurrSuppRates[0] = WLAN_EID_SUPP_RATES;
-            pMgmt->abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)pCurr->abySuppRates,
-                                                    (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-                                                    WLAN_RATES_MAXLEN_11B);
-            // set basic rate
-            RATEvParseMaxRate((void *)pDevice, (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-                              NULL, true, &wMaxBasicRate, &wMaxSuppRate, &wSuppRate,
-                              &byTopCCKBasicRate, &byTopOFDMBasicRate);
-
-            pMgmt->wCurrCapInfo = pCurr->wCapInfo;
-            pMgmt->wCurrBeaconPeriod = pCurr->wBeaconInterval;
-            memset(pMgmt->abyCurrSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN);
-            memcpy(pMgmt->abyCurrBSSID, pCurr->abyBSSID, WLAN_BSSID_LEN);
-            memcpy(pMgmt->abyCurrSSID, pCurr->abySSID, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN);
+			// Add current BSS to Candidate list
+			// This should only works for WPA2 BSS, and WPA2 BSS check must be done before.
+			if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2) {
+				bool bResult = bAdd_PMKID_Candidate((void *)pDevice, pMgmt->abyCurrBSSID, &pCurr->sRSNCapObj);
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "bAdd_PMKID_Candidate: 1(%d)\n", bResult);
+				if (bResult == false) {
+					vFlush_PMKID_Candidate((void *)pDevice);
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "vFlush_PMKID_Candidate: 4\n");
+					bAdd_PMKID_Candidate((void *)pDevice, pMgmt->abyCurrBSSID, &pCurr->sRSNCapObj);
+				}
+			}
+
+			// Preamble type auto-switch: if AP can receive short-preamble cap,
+			// we can turn on too.
+
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Join ESS\n");
+
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "End of Join AP -- A/B/G Action\n");
+		} else {
+			pMgmt->eCurrState = WMAC_STATE_IDLE;
+		};
+
+	} else {
+		// ad-hoc mode BSS
+		if (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) {
+			if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled) {
+				if (WPA_SearchRSN(0, WPA_TKIP, pCurr) == false) {
+					// encryption mode error
+					pMgmt->eCurrState = WMAC_STATE_IDLE;
+					return;
+				}
+			} else if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled) {
+				if (WPA_SearchRSN(0, WPA_AESCCMP, pCurr) == false) {
+					// encryption mode error
+					pMgmt->eCurrState = WMAC_STATE_IDLE;
+					return;
+				}
+			} else {
+				// encryption mode error
+				pMgmt->eCurrState = WMAC_STATE_IDLE;
+				return;
+			}
+		}
+
+		s_vMgrSynchBSS(pDevice,
+			       WMAC_MODE_IBSS_STA,
+			       pCurr,
+			       pStatus
+);
+
+		if (*pStatus == CMD_STATUS_SUCCESS) {
+			// Adopt this BSS state vars in Mgmt Object
+			// TODO: check if CapInfo privacy on, but we don't..
+			pMgmt->uCurrChannel = pCurr->uChannel;
+
+			// Parse Support Rate IE
+			pMgmt->abyCurrSuppRates[0] = WLAN_EID_SUPP_RATES;
+			pMgmt->abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)pCurr->abySuppRates,
+								(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
+								WLAN_RATES_MAXLEN_11B);
+			// set basic rate
+			RATEvParseMaxRate((void *)pDevice, (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
+					  NULL, true, &wMaxBasicRate, &wMaxSuppRate, &wSuppRate,
+					  &byTopCCKBasicRate, &byTopOFDMBasicRate);
+
+			pMgmt->wCurrCapInfo = pCurr->wCapInfo;
+			pMgmt->wCurrBeaconPeriod = pCurr->wBeaconInterval;
+			memset(pMgmt->abyCurrSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN);
+			memcpy(pMgmt->abyCurrBSSID, pCurr->abyBSSID, WLAN_BSSID_LEN);
+			memcpy(pMgmt->abyCurrSSID, pCurr->abySSID, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN);
 //          pMgmt->wCurrATIMWindow = pCurr->wATIMWindow;
-            MACvWriteATIMW(pDevice->PortOffset, pMgmt->wCurrATIMWindow);
-            pMgmt->eCurrMode = WMAC_MODE_IBSS_STA;
+			MACvWriteATIMW(pDevice->PortOffset, pMgmt->wCurrATIMWindow);
+			pMgmt->eCurrMode = WMAC_MODE_IBSS_STA;
 
-            pMgmt->eCurrState = WMAC_STATE_STARTED;
-            // Adopt BSS state in Adapter Device Object
-            //pDevice->byOpMode = OP_MODE_ADHOC;
+			pMgmt->eCurrState = WMAC_STATE_STARTED;
+			// Adopt BSS state in Adapter Device Object
+			//pDevice->byOpMode = OP_MODE_ADHOC;
 //            pDevice->bLinkPass = true;
 //            memcpy(pDevice->abyBSSID, pCurr->abyBSSID, WLAN_BSSID_LEN);
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Join IBSS ok:%pM\n",
-			pMgmt->abyCurrBSSID);
-            // Preamble type auto-switch: if AP can receive short-preamble cap,
-            // and if registry setting is short preamble we can turn on too.
-
-            // Prepare beacon
-            bMgrPrepareBeaconToSend((void *)pDevice, pMgmt);
-        }
-        else {
-            pMgmt->eCurrState = WMAC_STATE_IDLE;
-        };
-     };
-    return;
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Join IBSS ok:%pM\n",
+				pMgmt->abyCurrBSSID);
+			// Preamble type auto-switch: if AP can receive short-preamble cap,
+			// and if registry setting is short preamble we can turn on too.
+
+			// Prepare beacon
+			bMgrPrepareBeaconToSend((void *)pDevice, pMgmt);
+		} else {
+			pMgmt->eCurrState = WMAC_STATE_IDLE;
+		};
+	};
+	return;
 }
 
-
-
 /*+
  *
  * Routine Description:
@@ -2901,215 +2753,199 @@ vMgrJoinBSSBegin(
  * Return Value:
  *    PCM_STATUS
  *
--*/
+ -*/
 static
 void
-s_vMgrSynchBSS (
-    PSDevice      pDevice,
-    unsigned int uBSSMode,
-    PKnownBSS     pCurr,
-    PCMD_STATUS  pStatus
-    )
+s_vMgrSynchBSS(
+	PSDevice      pDevice,
+	unsigned int uBSSMode,
+	PKnownBSS     pCurr,
+	PCMD_STATUS  pStatus
+)
 {
-    CARD_PHY_TYPE   ePhyType = PHY_TYPE_11B;
-    PSMgmtObject  pMgmt = pDevice->pMgmt;
+	CARD_PHY_TYPE   ePhyType = PHY_TYPE_11B;
+	PSMgmtObject  pMgmt = pDevice->pMgmt;
 //    int     ii;
-                                                     //1M,   2M,   5M,   11M,  18M,  24M,  36M,  54M
-    unsigned char abyCurrSuppRatesG[] = {WLAN_EID_SUPP_RATES, 8, 0x02, 0x04, 0x0B, 0x16, 0x24, 0x30, 0x48, 0x6C};
-    unsigned char abyCurrExtSuppRatesG[] = {WLAN_EID_EXTSUPP_RATES, 4, 0x0C, 0x12, 0x18, 0x60};
-                                                           //6M,   9M,   12M,  48M
-    unsigned char abyCurrSuppRatesA[] = {WLAN_EID_SUPP_RATES, 8, 0x0C, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C};
-    unsigned char abyCurrSuppRatesB[] = {WLAN_EID_SUPP_RATES, 4, 0x02, 0x04, 0x0B, 0x16};
-
-
-    *pStatus = CMD_STATUS_FAILURE;
-
-    if (s_bCipherMatch(pCurr,
-                       pDevice->eEncryptionStatus,
-                       &(pMgmt->byCSSPK),
-                       &(pMgmt->byCSSGK)) == false) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "s_bCipherMatch Fail .......\n");
-        return;
-    }
-
-    pMgmt->pCurrBSS = pCurr;
-
-    // if previous mode is IBSS.
-    if(pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
-        MACvRegBitsOff(pDevice->PortOffset, MAC_REG_BCNDMACTL, BEACON_READY);
-        MACvRegBitsOff(pDevice->PortOffset, MAC_REG_TCR, TCR_AUTOBCNTX);
-    }
-
-    // Init the BSS informations
-    pDevice->bCCK = true;
-    pDevice->bProtectMode = false;
-    MACvDisableProtectMD(pDevice->PortOffset);
-    pDevice->bBarkerPreambleMd = false;
-    MACvDisableBarkerPreambleMd(pDevice->PortOffset);
-    pDevice->bNonERPPresent = false;
-    pDevice->byPreambleType = 0;
-    pDevice->wBasicRate = 0;
-    // Set Basic Rate
-    CARDbAddBasicRate((void *)pDevice, RATE_1M);
-    // calculate TSF offset
-    // TSF Offset = Received Timestamp TSF - Marked Local's TSF
-    CARDbUpdateTSF(pDevice, pCurr->byRxRate, pCurr->qwBSSTimestamp, pCurr->qwLocalTSF);
-
-    CARDbSetBeaconPeriod(pDevice, pCurr->wBeaconInterval);
-
-    // set Next TBTT
-    // Next TBTT = ((local_current_TSF / beacon_interval) + 1 ) * beacon_interval
-    CARDvSetFirstNextTBTT(pDevice->PortOffset, pCurr->wBeaconInterval);
-
-    // set BSSID
-    MACvWriteBSSIDAddress(pDevice->PortOffset, pCurr->abyBSSID);
-
-    MACvReadBSSIDAddress(pDevice->PortOffset, pMgmt->abyCurrBSSID);
+	//1M,   2M,   5M,   11M,  18M,  24M,  36M,  54M
+	unsigned char abyCurrSuppRatesG[] = {WLAN_EID_SUPP_RATES, 8, 0x02, 0x04, 0x0B, 0x16, 0x24, 0x30, 0x48, 0x6C};
+	unsigned char abyCurrExtSuppRatesG[] = {WLAN_EID_EXTSUPP_RATES, 4, 0x0C, 0x12, 0x18, 0x60};
+	//6M,   9M,   12M,  48M
+	unsigned char abyCurrSuppRatesA[] = {WLAN_EID_SUPP_RATES, 8, 0x0C, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C};
+	unsigned char abyCurrSuppRatesB[] = {WLAN_EID_SUPP_RATES, 4, 0x02, 0x04, 0x0B, 0x16};
+
+	*pStatus = CMD_STATUS_FAILURE;
+
+	if (s_bCipherMatch(pCurr,
+			   pDevice->eEncryptionStatus,
+			   &(pMgmt->byCSSPK),
+			   &(pMgmt->byCSSGK)) == false) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "s_bCipherMatch Fail .......\n");
+		return;
+	}
+
+	pMgmt->pCurrBSS = pCurr;
+
+	// if previous mode is IBSS.
+	if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
+		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_BCNDMACTL, BEACON_READY);
+		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_TCR, TCR_AUTOBCNTX);
+	}
+
+	// Init the BSS informations
+	pDevice->bCCK = true;
+	pDevice->bProtectMode = false;
+	MACvDisableProtectMD(pDevice->PortOffset);
+	pDevice->bBarkerPreambleMd = false;
+	MACvDisableBarkerPreambleMd(pDevice->PortOffset);
+	pDevice->bNonERPPresent = false;
+	pDevice->byPreambleType = 0;
+	pDevice->wBasicRate = 0;
+	// Set Basic Rate
+	CARDbAddBasicRate((void *)pDevice, RATE_1M);
+	// calculate TSF offset
+	// TSF Offset = Received Timestamp TSF - Marked Local's TSF
+	CARDbUpdateTSF(pDevice, pCurr->byRxRate, pCurr->qwBSSTimestamp, pCurr->qwLocalTSF);
+
+	CARDbSetBeaconPeriod(pDevice, pCurr->wBeaconInterval);
+
+	// set Next TBTT
+	// Next TBTT = ((local_current_TSF / beacon_interval) + 1) * beacon_interval
+	CARDvSetFirstNextTBTT(pDevice->PortOffset, pCurr->wBeaconInterval);
+
+	// set BSSID
+	MACvWriteBSSIDAddress(pDevice->PortOffset, pCurr->abyBSSID);
+
+	MACvReadBSSIDAddress(pDevice->PortOffset, pMgmt->abyCurrBSSID);
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Sync:set CurrBSSID address = "
 		"%pM\n", pMgmt->abyCurrBSSID);
 
-    if (pCurr->eNetworkTypeInUse == PHY_TYPE_11A) {
-        if ((pMgmt->eConfigPHYMode == PHY_TYPE_11A) ||
-            (pMgmt->eConfigPHYMode == PHY_TYPE_AUTO)) {
-            ePhyType = PHY_TYPE_11A;
-        } else {
-            return;
-        }
-    } else if (pCurr->eNetworkTypeInUse == PHY_TYPE_11B) {
-        if ((pMgmt->eConfigPHYMode == PHY_TYPE_11B) ||
-            (pMgmt->eConfigPHYMode == PHY_TYPE_11G) ||
-            (pMgmt->eConfigPHYMode == PHY_TYPE_AUTO)) {
-            ePhyType = PHY_TYPE_11B;
-        } else {
-            return;
-        }
-    } else {
-        if ((pMgmt->eConfigPHYMode == PHY_TYPE_11G) ||
-            (pMgmt->eConfigPHYMode == PHY_TYPE_AUTO)) {
-            ePhyType = PHY_TYPE_11G;
-        } else if (pMgmt->eConfigPHYMode == PHY_TYPE_11B) {
-            ePhyType = PHY_TYPE_11B;
-        } else {
-            return;
-        }
-    }
-
-    if (ePhyType == PHY_TYPE_11A) {
-        memcpy(pMgmt->abyCurrSuppRates, &abyCurrSuppRatesA[0], sizeof(abyCurrSuppRatesA));
-        pMgmt->abyCurrExtSuppRates[1] = 0;
-    } else if (ePhyType == PHY_TYPE_11B) {
-        memcpy(pMgmt->abyCurrSuppRates, &abyCurrSuppRatesB[0], sizeof(abyCurrSuppRatesB));
-        pMgmt->abyCurrExtSuppRates[1] = 0;
-    } else {
-        memcpy(pMgmt->abyCurrSuppRates, &abyCurrSuppRatesG[0], sizeof(abyCurrSuppRatesG));
-        memcpy(pMgmt->abyCurrExtSuppRates, &abyCurrExtSuppRatesG[0], sizeof(abyCurrExtSuppRatesG));
-    }
-
-
-    if (WLAN_GET_CAP_INFO_ESS(pCurr->wCapInfo)) {
-        CARDbSetBSSID(pMgmt->pAdapter, pCurr->abyBSSID, OP_MODE_INFRASTRUCTURE);
-        // Add current BSS to Candidate list
-        // This should only works for WPA2 BSS, and WPA2 BSS check must be done before.
-        if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2) {
-            CARDbAdd_PMKID_Candidate(pMgmt->pAdapter, pMgmt->abyCurrBSSID, pCurr->sRSNCapObj.bRSNCapExist, pCurr->sRSNCapObj.wRSNCap);
-        }
-    } else {
-        CARDbSetBSSID(pMgmt->pAdapter, pCurr->abyBSSID, OP_MODE_ADHOC);
-    }
-
-    if (CARDbSetPhyParameter(   pMgmt->pAdapter,
-                                ePhyType,
-                                pCurr->wCapInfo,
-                                pCurr->sERP.byERP,
-                                pMgmt->abyCurrSuppRates,
-                                pMgmt->abyCurrExtSuppRates
-                            ) != true) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "<----s_bSynchBSS Set Phy Mode Fail [%d]\n", ePhyType);
-        return;
-    }
-    // set channel and clear NAV
-    if (set_channel(pMgmt->pAdapter, pCurr->uChannel) == false) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "<----s_bSynchBSS Set Channel [%d]\n", pCurr->uChannel);
-        return;
-    }
+	if (pCurr->eNetworkTypeInUse == PHY_TYPE_11A) {
+		if ((pMgmt->eConfigPHYMode == PHY_TYPE_11A) ||
+		    (pMgmt->eConfigPHYMode == PHY_TYPE_AUTO)) {
+			ePhyType = PHY_TYPE_11A;
+		} else {
+			return;
+		}
+	} else if (pCurr->eNetworkTypeInUse == PHY_TYPE_11B) {
+		if ((pMgmt->eConfigPHYMode == PHY_TYPE_11B) ||
+		    (pMgmt->eConfigPHYMode == PHY_TYPE_11G) ||
+		    (pMgmt->eConfigPHYMode == PHY_TYPE_AUTO)) {
+			ePhyType = PHY_TYPE_11B;
+		} else {
+			return;
+		}
+	} else {
+		if ((pMgmt->eConfigPHYMode == PHY_TYPE_11G) ||
+		    (pMgmt->eConfigPHYMode == PHY_TYPE_AUTO)) {
+			ePhyType = PHY_TYPE_11G;
+		} else if (pMgmt->eConfigPHYMode == PHY_TYPE_11B) {
+			ePhyType = PHY_TYPE_11B;
+		} else {
+			return;
+		}
+	}
 
-/*
-    for (ii=0;ii<BB_VGA_LEVEL;ii++) {
-        if (pCurr->ldBmMAX< pDevice->ldBmThreshold[ii]) {
-            pDevice->byBBVGANew = pDevice->abyBBVGA[ii];
-            break;
-        }
-    }
-
-    if (pDevice->byBBVGANew != pDevice->byBBVGACurrent) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"RSSI[%d] NewGain[%d] OldGain[%d] \n",
-                        (int)pCurr->ldBmMAX, pDevice->byBBVGANew, pDevice->byBBVGACurrent);
-        printk("RSSI[%d] NewGain[%d] OldGain[%d] \n",
-                        (int)pCurr->ldBmMAX, pDevice->byBBVGANew, pDevice->byBBVGACurrent);
-        BBvSetVGAGainOffset(pDevice, pDevice->byBBVGANew);
-    }
-    printk("ldBmMAX[%d] NewGain[%d] OldGain[%d] \n",
-           (int)pCurr->ldBmMAX, pDevice->byBBVGANew, pDevice->byBBVGACurrent);
-*/
-    pMgmt->uCurrChannel = pCurr->uChannel;
-    pMgmt->eCurrentPHYMode = ePhyType;
-    pMgmt->byERPContext = pCurr->sERP.byERP;
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Sync:Set to channel = [%d]\n", (int)pCurr->uChannel);
+	if (ePhyType == PHY_TYPE_11A) {
+		memcpy(pMgmt->abyCurrSuppRates, &abyCurrSuppRatesA[0], sizeof(abyCurrSuppRatesA));
+		pMgmt->abyCurrExtSuppRates[1] = 0;
+	} else if (ePhyType == PHY_TYPE_11B) {
+		memcpy(pMgmt->abyCurrSuppRates, &abyCurrSuppRatesB[0], sizeof(abyCurrSuppRatesB));
+		pMgmt->abyCurrExtSuppRates[1] = 0;
+	} else {
+		memcpy(pMgmt->abyCurrSuppRates, &abyCurrSuppRatesG[0], sizeof(abyCurrSuppRatesG));
+		memcpy(pMgmt->abyCurrExtSuppRates, &abyCurrExtSuppRatesG[0], sizeof(abyCurrExtSuppRatesG));
+	}
+
+	if (WLAN_GET_CAP_INFO_ESS(pCurr->wCapInfo)) {
+		CARDbSetBSSID(pMgmt->pAdapter, pCurr->abyBSSID, OP_MODE_INFRASTRUCTURE);
+		// Add current BSS to Candidate list
+		// This should only works for WPA2 BSS, and WPA2 BSS check must be done before.
+		if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2) {
+			CARDbAdd_PMKID_Candidate(pMgmt->pAdapter, pMgmt->abyCurrBSSID, pCurr->sRSNCapObj.bRSNCapExist, pCurr->sRSNCapObj.wRSNCap);
+		}
+	} else {
+		CARDbSetBSSID(pMgmt->pAdapter, pCurr->abyBSSID, OP_MODE_ADHOC);
+	}
 
+	if (CARDbSetPhyParameter(pMgmt->pAdapter,
+				 ePhyType,
+				 pCurr->wCapInfo,
+				 pCurr->sERP.byERP,
+				 pMgmt->abyCurrSuppRates,
+				 pMgmt->abyCurrExtSuppRates
+		    ) != true) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "<----s_bSynchBSS Set Phy Mode Fail [%d]\n", ePhyType);
+		return;
+	}
+	// set channel and clear NAV
+	if (set_channel(pMgmt->pAdapter, pCurr->uChannel) == false) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "<----s_bSynchBSS Set Channel [%d]\n", pCurr->uChannel);
+		return;
+	}
 
-    *pStatus = CMD_STATUS_SUCCESS;
+/*
+  for (ii=0; ii<BB_VGA_LEVEL; ii++) {
+  if (pCurr->ldBmMAX< pDevice->ldBmThreshold[ii]) {
+  pDevice->byBBVGANew = pDevice->abyBBVGA[ii];
+  break;
+  }
+  }
 
+  if (pDevice->byBBVGANew != pDevice->byBBVGACurrent) {
+  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "RSSI[%d] NewGain[%d] OldGain[%d] \n",
+  (int)pCurr->ldBmMAX, pDevice->byBBVGANew, pDevice->byBBVGACurrent);
+  printk("RSSI[%d] NewGain[%d] OldGain[%d] \n",
+  (int)pCurr->ldBmMAX, pDevice->byBBVGANew, pDevice->byBBVGACurrent);
+  BBvSetVGAGainOffset(pDevice, pDevice->byBBVGANew);
+  }
+  printk("ldBmMAX[%d] NewGain[%d] OldGain[%d] \n",
+  (int)pCurr->ldBmMAX, pDevice->byBBVGANew, pDevice->byBBVGACurrent);
+*/
+	pMgmt->uCurrChannel = pCurr->uChannel;
+	pMgmt->eCurrentPHYMode = ePhyType;
+	pMgmt->byERPContext = pCurr->sERP.byERP;
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Sync:Set to channel = [%d]\n", (int)pCurr->uChannel);
 
-    return;
+	*pStatus = CMD_STATUS_SUCCESS;
+
+	return;
 };
 
 //mike add: fix NetworkManager 0.7.0 hidden ssid mode in WPA encryption
 //                   ,need reset eAuthenMode and eEncryptionStatus
- static void  Encyption_Rebuild(
-    PSDevice pDevice,
-    PKnownBSS pCurr
- )
- {
-  PSMgmtObject  pMgmt = &(pDevice->sMgmtObj);
- // unsigned int ii , uSameBssidNum=0;
-
-        //  for (ii = 0; ii < MAX_BSS_NUM; ii++) {
-          //   if (pMgmt->sBSSList[ii].bActive &&
-            //      !compare_ether_addr(pMgmt->sBSSList[ii].abyBSSID, pCurr->abyBSSID)) {
-             //       uSameBssidNum++;
-               //   }
-           // }
-  //   if( uSameBssidNum>=2) {	 //we only check AP in hidden sssid  mode
-        if ((pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) ||           //networkmanager 0.7.0 does not give the pairwise-key selection,
-             (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) {         // so we need re-select it according to real pairwise-key info.
-               if(pCurr->bWPAValid == true)  {   //WPA-PSK
-                          pMgmt->eAuthenMode = WMAC_AUTH_WPAPSK;
-		    if(pCurr->abyPKType[0] == WPA_TKIP) {
-     		        pDevice->eEncryptionStatus = Ndis802_11Encryption2Enabled;    //TKIP
-     		        PRINT_K("Encyption_Rebuild--->ssid reset config to [WPAPSK-TKIP]\n");
-		      }
-     		   else if(pCurr->abyPKType[0] == WPA_AESCCMP) {
-		        pDevice->eEncryptionStatus = Ndis802_11Encryption3Enabled;    //AES
-                          PRINT_K("Encyption_Rebuild--->ssid reset config to [WPAPSK-AES]\n");
-     		     }
-               	}
-               else if(pCurr->bWPA2Valid == true) {  //WPA2-PSK
-                         pMgmt->eAuthenMode = WMAC_AUTH_WPA2PSK;
-		       if(pCurr->abyCSSPK[0] == WLAN_11i_CSS_TKIP) {
-      		           pDevice->eEncryptionStatus = Ndis802_11Encryption2Enabled;     //TKIP
-                             PRINT_K("Encyption_Rebuild--->ssid reset config to [WPA2PSK-TKIP]\n");
-		       	}
-      		       else if(pCurr->abyCSSPK[0] == WLAN_11i_CSS_CCMP) {
-		           pDevice->eEncryptionStatus = Ndis802_11Encryption3Enabled;    //AES
-                            PRINT_K("Encyption_Rebuild--->ssid reset config to [WPA2PSK-AES]\n");
-      		       	}
-               	}
-              }
-        //  }
-      return;
- }
-
+static void  Encyption_Rebuild(
+	PSDevice pDevice,
+	PKnownBSS pCurr
+)
+{
+	PSMgmtObject  pMgmt = &(pDevice->sMgmtObj);
+
+	if ((pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) ||           //networkmanager 0.7.0 does not give the pairwise-key selection,
+	    (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) {         // so we need re-select it according to real pairwise-key info.
+		if (pCurr->bWPAValid == true)  {   //WPA-PSK
+			pMgmt->eAuthenMode = WMAC_AUTH_WPAPSK;
+			if (pCurr->abyPKType[0] == WPA_TKIP) {
+				pDevice->eEncryptionStatus = Ndis802_11Encryption2Enabled;    //TKIP
+				PRINT_K("Encyption_Rebuild--->ssid reset config to [WPAPSK-TKIP]\n");
+			} else if (pCurr->abyPKType[0] == WPA_AESCCMP) {
+				pDevice->eEncryptionStatus = Ndis802_11Encryption3Enabled;    //AES
+				PRINT_K("Encyption_Rebuild--->ssid reset config to [WPAPSK-AES]\n");
+			}
+		} else if (pCurr->bWPA2Valid == true) {  //WPA2-PSK
+			pMgmt->eAuthenMode = WMAC_AUTH_WPA2PSK;
+			if (pCurr->abyCSSPK[0] == WLAN_11i_CSS_TKIP) {
+				pDevice->eEncryptionStatus = Ndis802_11Encryption2Enabled;     //TKIP
+				PRINT_K("Encyption_Rebuild--->ssid reset config to [WPA2PSK-TKIP]\n");
+			} else if (pCurr->abyCSSPK[0] == WLAN_11i_CSS_CCMP) {
+				pDevice->eEncryptionStatus = Ndis802_11Encryption3Enabled;    //AES
+				PRINT_K("Encyption_Rebuild--->ssid reset config to [WPA2PSK-AES]\n");
+			}
+		}
+	}
+	//  }
+	return;
+}
 
 /*+
  *
@@ -3120,298 +2956,290 @@ s_vMgrSynchBSS (
  * Return Value:
  *    void
  *
--*/
+ -*/
 
 static
 void
 s_vMgrFormatTIM(
-    PSMgmtObject pMgmt,
-    PWLAN_IE_TIM pTIM
-    )
+	PSMgmtObject pMgmt,
+	PWLAN_IE_TIM pTIM
+)
 {
-    unsigned char byMask[8] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80};
-    unsigned char byMap;
-    unsigned int ii, jj;
-    bool bStartFound = false;
-    bool bMulticast = false;
-    unsigned short wStartIndex = 0;
-    unsigned short wEndIndex = 0;
-
-
-    // Find size of partial virtual bitmap
-    for (ii = 0; ii < (MAX_NODE_NUM + 1); ii++) {
-        byMap = pMgmt->abyPSTxMap[ii];
-        if (!ii) {
-            // Mask out the broadcast bit which is indicated separately.
-            bMulticast = (byMap & byMask[0]) != 0;
-            if(bMulticast) {
-               pMgmt->sNodeDBTable[0].bRxPSPoll = true;
-            }
-            byMap = 0;
-        }
-        if (byMap) {
-            if (!bStartFound) {
-                bStartFound = true;
-                wStartIndex = ii;
-            }
-            wEndIndex = ii;
-        }
-    }
-
-
-    // Round start index down to nearest even number
-    wStartIndex &=  ~BIT0;
-
-    // Round end index up to nearest even number
-    wEndIndex = ((wEndIndex + 1) & ~BIT0);
-
-    // Size of element payload
-
-    pTIM->len =  3 + (wEndIndex - wStartIndex) + 1;
-
-    // Fill in the Fixed parts of the TIM
-    pTIM->byDTIMCount = pMgmt->byDTIMCount;
-    pTIM->byDTIMPeriod = pMgmt->byDTIMPeriod;
-    pTIM->byBitMapCtl = (bMulticast ? TIM_MULTICAST_MASK : 0) |
-        (((wStartIndex >> 1) << 1) & TIM_BITMAPOFFSET_MASK);
-
-    // Append variable part of TIM
-
-    for (ii = wStartIndex, jj =0 ; ii <= wEndIndex; ii++, jj++) {
-         pTIM->byVirtBitMap[jj] = pMgmt->abyPSTxMap[ii];
-    }
-
-    // Aid = 0 don't used.
-    pTIM->byVirtBitMap[0]  &= ~BIT0;
-}
+	unsigned char byMask[8] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80};
+	unsigned char byMap;
+	unsigned int ii, jj;
+	bool bStartFound = false;
+	bool bMulticast = false;
+	unsigned short wStartIndex = 0;
+	unsigned short wEndIndex = 0;
+
+	// Find size of partial virtual bitmap
+	for (ii = 0; ii < (MAX_NODE_NUM + 1); ii++) {
+		byMap = pMgmt->abyPSTxMap[ii];
+		if (!ii) {
+			// Mask out the broadcast bit which is indicated separately.
+			bMulticast = (byMap & byMask[0]) != 0;
+			if (bMulticast) {
+				pMgmt->sNodeDBTable[0].bRxPSPoll = true;
+			}
+			byMap = 0;
+		}
+		if (byMap) {
+			if (!bStartFound) {
+				bStartFound = true;
+				wStartIndex = ii;
+			}
+			wEndIndex = ii;
+		}
+	}
+
+	// Round start index down to nearest even number
+	wStartIndex &=  ~BIT0;
+
+	// Round end index up to nearest even number
+	wEndIndex = ((wEndIndex + 1) & ~BIT0);
+
+	// Size of element payload
+
+	pTIM->len =  3 + (wEndIndex - wStartIndex) + 1;
+
+	// Fill in the Fixed parts of the TIM
+	pTIM->byDTIMCount = pMgmt->byDTIMCount;
+	pTIM->byDTIMPeriod = pMgmt->byDTIMPeriod;
+	pTIM->byBitMapCtl = (bMulticast ? TIM_MULTICAST_MASK : 0) |
+		(((wStartIndex >> 1) << 1) & TIM_BITMAPOFFSET_MASK);
 
+	// Append variable part of TIM
+
+	for (ii = wStartIndex, jj = 0; ii <= wEndIndex; ii++, jj++) {
+		pTIM->byVirtBitMap[jj] = pMgmt->abyPSTxMap[ii];
+	}
+
+	// Aid = 0 don't used.
+	pTIM->byVirtBitMap[0]  &= ~BIT0;
+}
 
 /*+
  *
  * Routine Description:
- *  Constructs an Beacon frame( Ad-hoc mode)
+ *  Constructs an Beacon frame(Ad-hoc mode)
  *
  *
  * Return Value:
  *    PTR to frame; or NULL on allocation failure
  *
--*/
+ -*/
 
 static
 PSTxMgmtPacket
 s_MgrMakeBeacon(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    unsigned short wCurrCapInfo,
-    unsigned short wCurrBeaconPeriod,
-    unsigned int uCurrChannel,
-    unsigned short wCurrATIMWinodw,
-    PWLAN_IE_SSID pCurrSSID,
-    unsigned char *pCurrBSSID,
-    PWLAN_IE_SUPP_RATES pCurrSuppRates,
-    PWLAN_IE_SUPP_RATES pCurrExtSuppRates
-    )
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	unsigned short wCurrCapInfo,
+	unsigned short wCurrBeaconPeriod,
+	unsigned int uCurrChannel,
+	unsigned short wCurrATIMWinodw,
+	PWLAN_IE_SSID pCurrSSID,
+	unsigned char *pCurrBSSID,
+	PWLAN_IE_SUPP_RATES pCurrSuppRates,
+	PWLAN_IE_SUPP_RATES pCurrExtSuppRates
+)
 {
-    PSTxMgmtPacket      pTxPacket = NULL;
-    WLAN_FR_BEACON      sFrame;
-    unsigned char abyBroadcastAddr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
-    unsigned char *pbyBuffer;
-    unsigned int uLength = 0;
-    PWLAN_IE_IBSS_DFS   pIBSSDFS = NULL;
-    unsigned int ii;
-
-    // prepare beacon frame
-    pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
-    memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_BEACON_FR_MAXLEN);
-    pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-    // Setup the sFrame structure.
-    sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
-    sFrame.len = WLAN_BEACON_FR_MAXLEN;
-    vMgrEncodeBeacon(&sFrame);
-    // Setup the header
-    sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
-        (
-        WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
-        WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_BEACON)
-        ));
-
-    if (pDevice->bEnablePSMode) {
-        sFrame.pHdr->sA3.wFrameCtl |= cpu_to_le16((unsigned short)WLAN_SET_FC_PWRMGT(1));
-    }
-
-    memcpy( sFrame.pHdr->sA3.abyAddr1, abyBroadcastAddr, WLAN_ADDR_LEN);
-    memcpy( sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-    memcpy( sFrame.pHdr->sA3.abyAddr3, pCurrBSSID, WLAN_BSSID_LEN);
-    *sFrame.pwBeaconInterval = cpu_to_le16(wCurrBeaconPeriod);
-    *sFrame.pwCapInfo = cpu_to_le16(wCurrCapInfo);
-    // Copy SSID
-    sFrame.pSSID = (PWLAN_IE_SSID)(sFrame.pBuf + sFrame.len);
-    sFrame.len += ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len + WLAN_IEHDR_LEN;
-    memcpy(sFrame.pSSID,
-             pCurrSSID,
-             ((PWLAN_IE_SSID)pCurrSSID)->len + WLAN_IEHDR_LEN
-            );
-    // Copy the rate set
-    sFrame.pSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-    sFrame.len += ((PWLAN_IE_SUPP_RATES)pCurrSuppRates)->len + WLAN_IEHDR_LEN;
-    memcpy(sFrame.pSuppRates,
-           pCurrSuppRates,
-           ((PWLAN_IE_SUPP_RATES)pCurrSuppRates)->len + WLAN_IEHDR_LEN
-          );
-    // DS parameter
-    if (pDevice->eCurrentPHYType != PHY_TYPE_11A) {
-        sFrame.pDSParms = (PWLAN_IE_DS_PARMS)(sFrame.pBuf + sFrame.len);
-        sFrame.len += (1) + WLAN_IEHDR_LEN;
-        sFrame.pDSParms->byElementID = WLAN_EID_DS_PARMS;
-        sFrame.pDSParms->len = 1;
-        sFrame.pDSParms->byCurrChannel = (unsigned char)uCurrChannel;
-    }
-    // TIM field
-    if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-        sFrame.pTIM = (PWLAN_IE_TIM)(sFrame.pBuf + sFrame.len);
-        sFrame.pTIM->byElementID = WLAN_EID_TIM;
-        s_vMgrFormatTIM(pMgmt, sFrame.pTIM);
-        sFrame.len += (WLAN_IEHDR_LEN + sFrame.pTIM->len);
-    }
-
-    if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
-
-        // IBSS parameter
-        sFrame.pIBSSParms = (PWLAN_IE_IBSS_PARMS)(sFrame.pBuf + sFrame.len);
-        sFrame.len += (2) + WLAN_IEHDR_LEN;
-        sFrame.pIBSSParms->byElementID = WLAN_EID_IBSS_PARMS;
-        sFrame.pIBSSParms->len = 2;
-        sFrame.pIBSSParms->wATIMWindow = wCurrATIMWinodw;
-        if (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) {
-            /* RSN parameter */
-            sFrame.pRSNWPA = (PWLAN_IE_RSN_EXT)(sFrame.pBuf + sFrame.len);
-            sFrame.pRSNWPA->byElementID = WLAN_EID_RSN_WPA;
-            sFrame.pRSNWPA->len = 12;
-            sFrame.pRSNWPA->abyOUI[0] = 0x00;
-            sFrame.pRSNWPA->abyOUI[1] = 0x50;
-            sFrame.pRSNWPA->abyOUI[2] = 0xf2;
-            sFrame.pRSNWPA->abyOUI[3] = 0x01;
-            sFrame.pRSNWPA->wVersion = 1;
-            sFrame.pRSNWPA->abyMulticast[0] = 0x00;
-            sFrame.pRSNWPA->abyMulticast[1] = 0x50;
-            sFrame.pRSNWPA->abyMulticast[2] = 0xf2;
-            if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled)
-                sFrame.pRSNWPA->abyMulticast[3] = 0x04;//AES
-            else if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled)
-                sFrame.pRSNWPA->abyMulticast[3] = 0x02;//TKIP
-            else if (pDevice->eEncryptionStatus == Ndis802_11Encryption1Enabled)
-                sFrame.pRSNWPA->abyMulticast[3] = 0x01;//WEP40
-            else
-                sFrame.pRSNWPA->abyMulticast[3] = 0x00;//NONE
-
-            // Pairwise Key Cipher Suite
-            sFrame.pRSNWPA->wPKCount = 0;
-            // Auth Key Management Suite
-            *((unsigned short *)(sFrame.pBuf + sFrame.len + sFrame.pRSNWPA->len))=0;
-            sFrame.pRSNWPA->len +=2;
-
-            // RSN Capabilities
-            *((unsigned short *)(sFrame.pBuf + sFrame.len + sFrame.pRSNWPA->len))=0;
-            sFrame.pRSNWPA->len +=2;
-            sFrame.len += sFrame.pRSNWPA->len + WLAN_IEHDR_LEN;
-        }
-    }
-
-    if ((pMgmt->b11hEnable == true) &&
-        (pMgmt->eCurrentPHYMode == PHY_TYPE_11A)) {
-        // Country IE
-        pbyBuffer = (unsigned char *)(sFrame.pBuf + sFrame.len);
-        set_country_IE(pMgmt->pAdapter, pbyBuffer);
-        set_country_info(pMgmt->pAdapter, PHY_TYPE_11A, pbyBuffer);
-        uLength += ((PWLAN_IE_COUNTRY) pbyBuffer)->len + WLAN_IEHDR_LEN;
-        pbyBuffer += (((PWLAN_IE_COUNTRY) pbyBuffer)->len + WLAN_IEHDR_LEN);
-        // Power Constrain IE
-        ((PWLAN_IE_PW_CONST) pbyBuffer)->byElementID = WLAN_EID_PWR_CONSTRAINT;
-        ((PWLAN_IE_PW_CONST) pbyBuffer)->len = 1;
-        ((PWLAN_IE_PW_CONST) pbyBuffer)->byPower = 0;
-        pbyBuffer += (1) + WLAN_IEHDR_LEN;
-        uLength += (1) + WLAN_IEHDR_LEN;
-        if (pMgmt->bSwitchChannel == true) {
-            // Channel Switch IE
-            ((PWLAN_IE_CH_SW) pbyBuffer)->byElementID = WLAN_EID_CH_SWITCH;
-            ((PWLAN_IE_CH_SW) pbyBuffer)->len = 3;
-            ((PWLAN_IE_CH_SW) pbyBuffer)->byMode = 1;
-            ((PWLAN_IE_CH_SW) pbyBuffer)->byChannel = get_channel_number(pMgmt->pAdapter, pMgmt->byNewChannel);
-            ((PWLAN_IE_CH_SW) pbyBuffer)->byCount = 0;
-            pbyBuffer += (3) + WLAN_IEHDR_LEN;
-            uLength += (3) + WLAN_IEHDR_LEN;
-        }
-        // TPC report
-        ((PWLAN_IE_TPC_REP) pbyBuffer)->byElementID = WLAN_EID_TPC_REP;
-        ((PWLAN_IE_TPC_REP) pbyBuffer)->len = 2;
-        ((PWLAN_IE_TPC_REP) pbyBuffer)->byTxPower = CARDbyGetTransmitPower(pMgmt->pAdapter);
-        ((PWLAN_IE_TPC_REP) pbyBuffer)->byLinkMargin = 0;
-        pbyBuffer += (2) + WLAN_IEHDR_LEN;
-        uLength += (2) + WLAN_IEHDR_LEN;
-        // IBSS DFS
-        if (pMgmt->eCurrMode != WMAC_MODE_ESS_AP) {
-            pIBSSDFS = (PWLAN_IE_IBSS_DFS) pbyBuffer;
-            pIBSSDFS->byElementID = WLAN_EID_IBSS_DFS;
-            pIBSSDFS->len = 7;
-            memcpy(   pIBSSDFS->abyDFSOwner,
-                        pMgmt->abyIBSSDFSOwner,
-                        6);
-            pIBSSDFS->byDFSRecovery = pMgmt->byIBSSDFSRecovery;
-            pbyBuffer += (7) + WLAN_IEHDR_LEN;
-            uLength += (7) + WLAN_IEHDR_LEN;
-            for(ii=CB_MAX_CHANNEL_24G+1; ii<=CB_MAX_CHANNEL; ii++ ) {
-                if (get_channel_map_info(pMgmt->pAdapter, ii, pbyBuffer, pbyBuffer+1) == true) {
-                    pbyBuffer += 2;
-                    uLength += 2;
-                    pIBSSDFS->len += 2;
-                }
-            }
-        }
-        sFrame.len += uLength;
-    }
-
-    if (pMgmt->eCurrentPHYMode == PHY_TYPE_11G) {
-        sFrame.pERP = (PWLAN_IE_ERP)(sFrame.pBuf + sFrame.len);
-        sFrame.len += 1 + WLAN_IEHDR_LEN;
-        sFrame.pERP->byElementID = WLAN_EID_ERP;
-        sFrame.pERP->len = 1;
-        sFrame.pERP->byContext = 0;
-        if (pDevice->bProtectMode == true)
-            sFrame.pERP->byContext |= WLAN_EID_ERP_USE_PROTECTION;
-        if (pDevice->bNonERPPresent == true)
-            sFrame.pERP->byContext |= WLAN_EID_ERP_NONERP_PRESENT;
-        if (pDevice->bBarkerPreambleMd == true)
-            sFrame.pERP->byContext |= WLAN_EID_ERP_BARKER_MODE;
-    }
-    if (((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len != 0) {
-        sFrame.pExtSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-        sFrame.len += ((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len + WLAN_IEHDR_LEN;
-        memcpy(sFrame.pExtSuppRates,
-             pCurrExtSuppRates,
-             ((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len + WLAN_IEHDR_LEN
-             );
-    }
-    // hostapd wpa/wpa2 IE
-    if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) && (pDevice->bEnableHostapd == true)) {
-         if (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) {
-             if (pMgmt->wWPAIELen != 0) {
-                 sFrame.pRSN = (PWLAN_IE_RSN)(sFrame.pBuf + sFrame.len);
-                 memcpy(sFrame.pRSN, pMgmt->abyWPAIE, pMgmt->wWPAIELen);
-                 sFrame.len += pMgmt->wWPAIELen;
-             }
-         }
-    }
-
-    /* Adjust the length fields */
-    pTxPacket->cbMPDULen = sFrame.len;
-    pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
-
-    return pTxPacket;
-}
+	PSTxMgmtPacket      pTxPacket = NULL;
+	WLAN_FR_BEACON      sFrame;
+	unsigned char abyBroadcastAddr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	unsigned char *pbyBuffer;
+	unsigned int uLength = 0;
+	PWLAN_IE_IBSS_DFS   pIBSSDFS = NULL;
+	unsigned int ii;
+
+	// prepare beacon frame
+	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
+	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_BEACON_FR_MAXLEN);
+	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
+	// Setup the sFrame structure.
+	sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
+	sFrame.len = WLAN_BEACON_FR_MAXLEN;
+	vMgrEncodeBeacon(&sFrame);
+	// Setup the header
+	sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
+		(
+			WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
+			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_BEACON)
+));
+
+	if (pDevice->bEnablePSMode) {
+		sFrame.pHdr->sA3.wFrameCtl |= cpu_to_le16((unsigned short)WLAN_SET_FC_PWRMGT(1));
+	}
 
+	memcpy(sFrame.pHdr->sA3.abyAddr1, abyBroadcastAddr, WLAN_ADDR_LEN);
+	memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
+	memcpy(sFrame.pHdr->sA3.abyAddr3, pCurrBSSID, WLAN_BSSID_LEN);
+	*sFrame.pwBeaconInterval = cpu_to_le16(wCurrBeaconPeriod);
+	*sFrame.pwCapInfo = cpu_to_le16(wCurrCapInfo);
+	// Copy SSID
+	sFrame.pSSID = (PWLAN_IE_SSID)(sFrame.pBuf + sFrame.len);
+	sFrame.len += ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len + WLAN_IEHDR_LEN;
+	memcpy(sFrame.pSSID,
+	       pCurrSSID,
+	       ((PWLAN_IE_SSID)pCurrSSID)->len + WLAN_IEHDR_LEN
+);
+	// Copy the rate set
+	sFrame.pSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
+	sFrame.len += ((PWLAN_IE_SUPP_RATES)pCurrSuppRates)->len + WLAN_IEHDR_LEN;
+	memcpy(sFrame.pSuppRates,
+	       pCurrSuppRates,
+	       ((PWLAN_IE_SUPP_RATES)pCurrSuppRates)->len + WLAN_IEHDR_LEN
+);
+	// DS parameter
+	if (pDevice->eCurrentPHYType != PHY_TYPE_11A) {
+		sFrame.pDSParms = (PWLAN_IE_DS_PARMS)(sFrame.pBuf + sFrame.len);
+		sFrame.len += (1) + WLAN_IEHDR_LEN;
+		sFrame.pDSParms->byElementID = WLAN_EID_DS_PARMS;
+		sFrame.pDSParms->len = 1;
+		sFrame.pDSParms->byCurrChannel = (unsigned char)uCurrChannel;
+	}
+	// TIM field
+	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
+		sFrame.pTIM = (PWLAN_IE_TIM)(sFrame.pBuf + sFrame.len);
+		sFrame.pTIM->byElementID = WLAN_EID_TIM;
+		s_vMgrFormatTIM(pMgmt, sFrame.pTIM);
+		sFrame.len += (WLAN_IEHDR_LEN + sFrame.pTIM->len);
+	}
 
+	if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
+		// IBSS parameter
+		sFrame.pIBSSParms = (PWLAN_IE_IBSS_PARMS)(sFrame.pBuf + sFrame.len);
+		sFrame.len += (2) + WLAN_IEHDR_LEN;
+		sFrame.pIBSSParms->byElementID = WLAN_EID_IBSS_PARMS;
+		sFrame.pIBSSParms->len = 2;
+		sFrame.pIBSSParms->wATIMWindow = wCurrATIMWinodw;
+		if (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) {
+			/* RSN parameter */
+			sFrame.pRSNWPA = (PWLAN_IE_RSN_EXT)(sFrame.pBuf + sFrame.len);
+			sFrame.pRSNWPA->byElementID = WLAN_EID_RSN_WPA;
+			sFrame.pRSNWPA->len = 12;
+			sFrame.pRSNWPA->abyOUI[0] = 0x00;
+			sFrame.pRSNWPA->abyOUI[1] = 0x50;
+			sFrame.pRSNWPA->abyOUI[2] = 0xf2;
+			sFrame.pRSNWPA->abyOUI[3] = 0x01;
+			sFrame.pRSNWPA->wVersion = 1;
+			sFrame.pRSNWPA->abyMulticast[0] = 0x00;
+			sFrame.pRSNWPA->abyMulticast[1] = 0x50;
+			sFrame.pRSNWPA->abyMulticast[2] = 0xf2;
+			if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled)
+				sFrame.pRSNWPA->abyMulticast[3] = 0x04;//AES
+			else if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled)
+				sFrame.pRSNWPA->abyMulticast[3] = 0x02;//TKIP
+			else if (pDevice->eEncryptionStatus == Ndis802_11Encryption1Enabled)
+				sFrame.pRSNWPA->abyMulticast[3] = 0x01;//WEP40
+			else
+				sFrame.pRSNWPA->abyMulticast[3] = 0x00;//NONE
+
+			// Pairwise Key Cipher Suite
+			sFrame.pRSNWPA->wPKCount = 0;
+			// Auth Key Management Suite
+			*((unsigned short *)(sFrame.pBuf + sFrame.len + sFrame.pRSNWPA->len)) = 0;
+			sFrame.pRSNWPA->len += 2;
+
+			// RSN Capabilities
+			*((unsigned short *)(sFrame.pBuf + sFrame.len + sFrame.pRSNWPA->len)) = 0;
+			sFrame.pRSNWPA->len += 2;
+			sFrame.len += sFrame.pRSNWPA->len + WLAN_IEHDR_LEN;
+		}
+	}
 
+	if ((pMgmt->b11hEnable == true) &&
+	    (pMgmt->eCurrentPHYMode == PHY_TYPE_11A)) {
+		// Country IE
+		pbyBuffer = (unsigned char *)(sFrame.pBuf + sFrame.len);
+		set_country_IE(pMgmt->pAdapter, pbyBuffer);
+		set_country_info(pMgmt->pAdapter, PHY_TYPE_11A, pbyBuffer);
+		uLength += ((PWLAN_IE_COUNTRY) pbyBuffer)->len + WLAN_IEHDR_LEN;
+		pbyBuffer += (((PWLAN_IE_COUNTRY) pbyBuffer)->len + WLAN_IEHDR_LEN);
+		// Power Constrain IE
+		((PWLAN_IE_PW_CONST) pbyBuffer)->byElementID = WLAN_EID_PWR_CONSTRAINT;
+		((PWLAN_IE_PW_CONST) pbyBuffer)->len = 1;
+		((PWLAN_IE_PW_CONST) pbyBuffer)->byPower = 0;
+		pbyBuffer += (1) + WLAN_IEHDR_LEN;
+		uLength += (1) + WLAN_IEHDR_LEN;
+		if (pMgmt->bSwitchChannel == true) {
+			// Channel Switch IE
+			((PWLAN_IE_CH_SW) pbyBuffer)->byElementID = WLAN_EID_CH_SWITCH;
+			((PWLAN_IE_CH_SW) pbyBuffer)->len = 3;
+			((PWLAN_IE_CH_SW) pbyBuffer)->byMode = 1;
+			((PWLAN_IE_CH_SW) pbyBuffer)->byChannel = get_channel_number(pMgmt->pAdapter, pMgmt->byNewChannel);
+			((PWLAN_IE_CH_SW) pbyBuffer)->byCount = 0;
+			pbyBuffer += (3) + WLAN_IEHDR_LEN;
+			uLength += (3) + WLAN_IEHDR_LEN;
+		}
+		// TPC report
+		((PWLAN_IE_TPC_REP) pbyBuffer)->byElementID = WLAN_EID_TPC_REP;
+		((PWLAN_IE_TPC_REP) pbyBuffer)->len = 2;
+		((PWLAN_IE_TPC_REP) pbyBuffer)->byTxPower = CARDbyGetTransmitPower(pMgmt->pAdapter);
+		((PWLAN_IE_TPC_REP) pbyBuffer)->byLinkMargin = 0;
+		pbyBuffer += (2) + WLAN_IEHDR_LEN;
+		uLength += (2) + WLAN_IEHDR_LEN;
+		// IBSS DFS
+		if (pMgmt->eCurrMode != WMAC_MODE_ESS_AP) {
+			pIBSSDFS = (PWLAN_IE_IBSS_DFS) pbyBuffer;
+			pIBSSDFS->byElementID = WLAN_EID_IBSS_DFS;
+			pIBSSDFS->len = 7;
+			memcpy(pIBSSDFS->abyDFSOwner,
+			       pMgmt->abyIBSSDFSOwner,
+			       6);
+			pIBSSDFS->byDFSRecovery = pMgmt->byIBSSDFSRecovery;
+			pbyBuffer += (7) + WLAN_IEHDR_LEN;
+			uLength += (7) + WLAN_IEHDR_LEN;
+			for (ii = CB_MAX_CHANNEL_24G+1; ii <= CB_MAX_CHANNEL; ii++) {
+				if (get_channel_map_info(pMgmt->pAdapter, ii, pbyBuffer, pbyBuffer+1) == true) {
+					pbyBuffer += 2;
+					uLength += 2;
+					pIBSSDFS->len += 2;
+				}
+			}
+		}
+		sFrame.len += uLength;
+	}
 
+	if (pMgmt->eCurrentPHYMode == PHY_TYPE_11G) {
+		sFrame.pERP = (PWLAN_IE_ERP)(sFrame.pBuf + sFrame.len);
+		sFrame.len += 1 + WLAN_IEHDR_LEN;
+		sFrame.pERP->byElementID = WLAN_EID_ERP;
+		sFrame.pERP->len = 1;
+		sFrame.pERP->byContext = 0;
+		if (pDevice->bProtectMode == true)
+			sFrame.pERP->byContext |= WLAN_EID_ERP_USE_PROTECTION;
+		if (pDevice->bNonERPPresent == true)
+			sFrame.pERP->byContext |= WLAN_EID_ERP_NONERP_PRESENT;
+		if (pDevice->bBarkerPreambleMd == true)
+			sFrame.pERP->byContext |= WLAN_EID_ERP_BARKER_MODE;
+	}
+	if (((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len != 0) {
+		sFrame.pExtSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
+		sFrame.len += ((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len + WLAN_IEHDR_LEN;
+		memcpy(sFrame.pExtSuppRates,
+		       pCurrExtSuppRates,
+		       ((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len + WLAN_IEHDR_LEN
+);
+	}
+	// hostapd wpa/wpa2 IE
+	if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) && (pDevice->bEnableHostapd == true)) {
+		if (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) {
+			if (pMgmt->wWPAIELen != 0) {
+				sFrame.pRSN = (PWLAN_IE_RSN)(sFrame.pBuf + sFrame.len);
+				memcpy(sFrame.pRSN, pMgmt->abyWPAIE, pMgmt->wWPAIELen);
+				sFrame.len += pMgmt->wWPAIELen;
+			}
+		}
+	}
+
+	/* Adjust the length fields */
+	pTxPacket->cbMPDULen = sFrame.len;
+	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
+
+	return pTxPacket;
+}
 
 /*+
  *
@@ -3422,187 +3250,180 @@ s_MgrMakeBeacon(
  * Return Value:
  *    PTR to frame; or NULL on allocation failure
  *
--*/
-
-
-
+ -*/
 
 PSTxMgmtPacket
 s_MgrMakeProbeResponse(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    unsigned short wCurrCapInfo,
-    unsigned short wCurrBeaconPeriod,
-    unsigned int uCurrChannel,
-    unsigned short wCurrATIMWinodw,
-    unsigned char *pDstAddr,
-    PWLAN_IE_SSID pCurrSSID,
-    unsigned char *pCurrBSSID,
-    PWLAN_IE_SUPP_RATES pCurrSuppRates,
-    PWLAN_IE_SUPP_RATES pCurrExtSuppRates,
-    unsigned char byPHYType
-    )
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	unsigned short wCurrCapInfo,
+	unsigned short wCurrBeaconPeriod,
+	unsigned int uCurrChannel,
+	unsigned short wCurrATIMWinodw,
+	unsigned char *pDstAddr,
+	PWLAN_IE_SSID pCurrSSID,
+	unsigned char *pCurrBSSID,
+	PWLAN_IE_SUPP_RATES pCurrSuppRates,
+	PWLAN_IE_SUPP_RATES pCurrExtSuppRates,
+	unsigned char byPHYType
+)
 {
-    PSTxMgmtPacket      pTxPacket = NULL;
-    WLAN_FR_PROBERESP   sFrame;
-    unsigned char *pbyBuffer;
-    unsigned int uLength = 0;
-    PWLAN_IE_IBSS_DFS   pIBSSDFS = NULL;
-    unsigned int ii;
-
-
-    pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
-    memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_PROBERESP_FR_MAXLEN);
-    pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-    // Setup the sFrame structure.
-    sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
-    sFrame.len = WLAN_PROBERESP_FR_MAXLEN;
-    vMgrEncodeProbeResponse(&sFrame);
-    // Setup the header
-    sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
-        (
-        WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
-        WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_PROBERESP)
-        ));
-    memcpy( sFrame.pHdr->sA3.abyAddr1, pDstAddr, WLAN_ADDR_LEN);
-    memcpy( sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-    memcpy( sFrame.pHdr->sA3.abyAddr3, pCurrBSSID, WLAN_BSSID_LEN);
-    *sFrame.pwBeaconInterval = cpu_to_le16(wCurrBeaconPeriod);
-    *sFrame.pwCapInfo = cpu_to_le16(wCurrCapInfo);
-
-    if (byPHYType == BB_TYPE_11B) {
-        *sFrame.pwCapInfo &= cpu_to_le16((unsigned short)~(WLAN_SET_CAP_INFO_SHORTSLOTTIME(1)));
-    }
-
-    // Copy SSID
-    sFrame.pSSID = (PWLAN_IE_SSID)(sFrame.pBuf + sFrame.len);
-    sFrame.len += ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len + WLAN_IEHDR_LEN;
-    memcpy(sFrame.pSSID,
-           pCurrSSID,
-           ((PWLAN_IE_SSID)pCurrSSID)->len + WLAN_IEHDR_LEN
-           );
-    // Copy the rate set
-    sFrame.pSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-
-    sFrame.len += ((PWLAN_IE_SUPP_RATES)pCurrSuppRates)->len + WLAN_IEHDR_LEN;
-    memcpy(sFrame.pSuppRates,
-           pCurrSuppRates,
-           ((PWLAN_IE_SUPP_RATES)pCurrSuppRates)->len + WLAN_IEHDR_LEN
-          );
-
-    // DS parameter
-    if (pDevice->eCurrentPHYType != PHY_TYPE_11A) {
-        sFrame.pDSParms = (PWLAN_IE_DS_PARMS)(sFrame.pBuf + sFrame.len);
-        sFrame.len += (1) + WLAN_IEHDR_LEN;
-        sFrame.pDSParms->byElementID = WLAN_EID_DS_PARMS;
-        sFrame.pDSParms->len = 1;
-        sFrame.pDSParms->byCurrChannel = (unsigned char)uCurrChannel;
-    }
-
-    if (pMgmt->eCurrMode != WMAC_MODE_ESS_AP) {
-        // IBSS parameter
-        sFrame.pIBSSParms = (PWLAN_IE_IBSS_PARMS)(sFrame.pBuf + sFrame.len);
-        sFrame.len += (2) + WLAN_IEHDR_LEN;
-        sFrame.pIBSSParms->byElementID = WLAN_EID_IBSS_PARMS;
-        sFrame.pIBSSParms->len = 2;
-        sFrame.pIBSSParms->wATIMWindow = 0;
-    }
-    if (pDevice->eCurrentPHYType == PHY_TYPE_11G) {
-        sFrame.pERP = (PWLAN_IE_ERP)(sFrame.pBuf + sFrame.len);
-        sFrame.len += 1 + WLAN_IEHDR_LEN;
-        sFrame.pERP->byElementID = WLAN_EID_ERP;
-        sFrame.pERP->len = 1;
-        sFrame.pERP->byContext = 0;
-        if (pDevice->bProtectMode == true)
-            sFrame.pERP->byContext |= WLAN_EID_ERP_USE_PROTECTION;
-        if (pDevice->bNonERPPresent == true)
-            sFrame.pERP->byContext |= WLAN_EID_ERP_NONERP_PRESENT;
-        if (pDevice->bBarkerPreambleMd == true)
-            sFrame.pERP->byContext |= WLAN_EID_ERP_BARKER_MODE;
-    }
-
-    if ((pMgmt->b11hEnable == true) &&
-        (pMgmt->eCurrentPHYMode == PHY_TYPE_11A)) {
-        // Country IE
-        pbyBuffer = (unsigned char *)(sFrame.pBuf + sFrame.len);
-        set_country_IE(pMgmt->pAdapter, pbyBuffer);
-        set_country_info(pMgmt->pAdapter, PHY_TYPE_11A, pbyBuffer);
-        uLength += ((PWLAN_IE_COUNTRY) pbyBuffer)->len + WLAN_IEHDR_LEN;
-        pbyBuffer += (((PWLAN_IE_COUNTRY) pbyBuffer)->len + WLAN_IEHDR_LEN);
-        // Power Constrain IE
-        ((PWLAN_IE_PW_CONST) pbyBuffer)->byElementID = WLAN_EID_PWR_CONSTRAINT;
-        ((PWLAN_IE_PW_CONST) pbyBuffer)->len = 1;
-        ((PWLAN_IE_PW_CONST) pbyBuffer)->byPower = 0;
-        pbyBuffer += (1) + WLAN_IEHDR_LEN;
-        uLength += (1) + WLAN_IEHDR_LEN;
-        if (pMgmt->bSwitchChannel == true) {
-            // Channel Switch IE
-            ((PWLAN_IE_CH_SW) pbyBuffer)->byElementID = WLAN_EID_CH_SWITCH;
-            ((PWLAN_IE_CH_SW) pbyBuffer)->len = 3;
-            ((PWLAN_IE_CH_SW) pbyBuffer)->byMode = 1;
-            ((PWLAN_IE_CH_SW) pbyBuffer)->byChannel = get_channel_number(pMgmt->pAdapter, pMgmt->byNewChannel);
-            ((PWLAN_IE_CH_SW) pbyBuffer)->byCount = 0;
-            pbyBuffer += (3) + WLAN_IEHDR_LEN;
-            uLength += (3) + WLAN_IEHDR_LEN;
-        }
-        // TPC report
-        ((PWLAN_IE_TPC_REP) pbyBuffer)->byElementID = WLAN_EID_TPC_REP;
-        ((PWLAN_IE_TPC_REP) pbyBuffer)->len = 2;
-        ((PWLAN_IE_TPC_REP) pbyBuffer)->byTxPower = CARDbyGetTransmitPower(pMgmt->pAdapter);
-        ((PWLAN_IE_TPC_REP) pbyBuffer)->byLinkMargin = 0;
-        pbyBuffer += (2) + WLAN_IEHDR_LEN;
-        uLength += (2) + WLAN_IEHDR_LEN;
-        // IBSS DFS
-        if (pMgmt->eCurrMode != WMAC_MODE_ESS_AP) {
-            pIBSSDFS = (PWLAN_IE_IBSS_DFS) pbyBuffer;
-            pIBSSDFS->byElementID = WLAN_EID_IBSS_DFS;
-            pIBSSDFS->len = 7;
-            memcpy(   pIBSSDFS->abyDFSOwner,
-                        pMgmt->abyIBSSDFSOwner,
-                        6);
-            pIBSSDFS->byDFSRecovery = pMgmt->byIBSSDFSRecovery;
-            pbyBuffer += (7) + WLAN_IEHDR_LEN;
-            uLength += (7) + WLAN_IEHDR_LEN;
-            for(ii=CB_MAX_CHANNEL_24G+1; ii<=CB_MAX_CHANNEL; ii++ ) {
-                if (get_channel_map_info(pMgmt->pAdapter, ii, pbyBuffer, pbyBuffer+1) == true) {
-                    pbyBuffer += 2;
-                    uLength += 2;
-                    pIBSSDFS->len += 2;
-                }
-            }
-        }
-        sFrame.len += uLength;
-    }
-
-
-    if (((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len != 0) {
-        sFrame.pExtSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-        sFrame.len += ((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len + WLAN_IEHDR_LEN;
-        memcpy(sFrame.pExtSuppRates,
-             pCurrExtSuppRates,
-             ((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len + WLAN_IEHDR_LEN
-             );
-    }
-
-    // hostapd wpa/wpa2 IE
-    if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) && (pDevice->bEnableHostapd == true)) {
-         if (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) {
-             if (pMgmt->wWPAIELen != 0) {
-                 sFrame.pRSN = (PWLAN_IE_RSN)(sFrame.pBuf + sFrame.len);
-                 memcpy(sFrame.pRSN, pMgmt->abyWPAIE, pMgmt->wWPAIELen);
-                 sFrame.len += pMgmt->wWPAIELen;
-             }
-         }
-    }
-
-    // Adjust the length fields
-    pTxPacket->cbMPDULen = sFrame.len;
-    pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
-
-    return pTxPacket;
-}
+	PSTxMgmtPacket      pTxPacket = NULL;
+	WLAN_FR_PROBERESP   sFrame;
+	unsigned char *pbyBuffer;
+	unsigned int uLength = 0;
+	PWLAN_IE_IBSS_DFS   pIBSSDFS = NULL;
+	unsigned int ii;
+
+	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
+	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_PROBERESP_FR_MAXLEN);
+	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
+	// Setup the sFrame structure.
+	sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
+	sFrame.len = WLAN_PROBERESP_FR_MAXLEN;
+	vMgrEncodeProbeResponse(&sFrame);
+	// Setup the header
+	sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
+		(
+			WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
+			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_PROBERESP)
+));
+	memcpy(sFrame.pHdr->sA3.abyAddr1, pDstAddr, WLAN_ADDR_LEN);
+	memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
+	memcpy(sFrame.pHdr->sA3.abyAddr3, pCurrBSSID, WLAN_BSSID_LEN);
+	*sFrame.pwBeaconInterval = cpu_to_le16(wCurrBeaconPeriod);
+	*sFrame.pwCapInfo = cpu_to_le16(wCurrCapInfo);
+
+	if (byPHYType == BB_TYPE_11B) {
+		*sFrame.pwCapInfo &= cpu_to_le16((unsigned short)~(WLAN_SET_CAP_INFO_SHORTSLOTTIME(1)));
+	}
+
+	// Copy SSID
+	sFrame.pSSID = (PWLAN_IE_SSID)(sFrame.pBuf + sFrame.len);
+	sFrame.len += ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len + WLAN_IEHDR_LEN;
+	memcpy(sFrame.pSSID,
+	       pCurrSSID,
+	       ((PWLAN_IE_SSID)pCurrSSID)->len + WLAN_IEHDR_LEN
+);
+	// Copy the rate set
+	sFrame.pSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
+
+	sFrame.len += ((PWLAN_IE_SUPP_RATES)pCurrSuppRates)->len + WLAN_IEHDR_LEN;
+	memcpy(sFrame.pSuppRates,
+	       pCurrSuppRates,
+	       ((PWLAN_IE_SUPP_RATES)pCurrSuppRates)->len + WLAN_IEHDR_LEN
+);
+
+	// DS parameter
+	if (pDevice->eCurrentPHYType != PHY_TYPE_11A) {
+		sFrame.pDSParms = (PWLAN_IE_DS_PARMS)(sFrame.pBuf + sFrame.len);
+		sFrame.len += (1) + WLAN_IEHDR_LEN;
+		sFrame.pDSParms->byElementID = WLAN_EID_DS_PARMS;
+		sFrame.pDSParms->len = 1;
+		sFrame.pDSParms->byCurrChannel = (unsigned char)uCurrChannel;
+	}
+
+	if (pMgmt->eCurrMode != WMAC_MODE_ESS_AP) {
+		// IBSS parameter
+		sFrame.pIBSSParms = (PWLAN_IE_IBSS_PARMS)(sFrame.pBuf + sFrame.len);
+		sFrame.len += (2) + WLAN_IEHDR_LEN;
+		sFrame.pIBSSParms->byElementID = WLAN_EID_IBSS_PARMS;
+		sFrame.pIBSSParms->len = 2;
+		sFrame.pIBSSParms->wATIMWindow = 0;
+	}
+	if (pDevice->eCurrentPHYType == PHY_TYPE_11G) {
+		sFrame.pERP = (PWLAN_IE_ERP)(sFrame.pBuf + sFrame.len);
+		sFrame.len += 1 + WLAN_IEHDR_LEN;
+		sFrame.pERP->byElementID = WLAN_EID_ERP;
+		sFrame.pERP->len = 1;
+		sFrame.pERP->byContext = 0;
+		if (pDevice->bProtectMode == true)
+			sFrame.pERP->byContext |= WLAN_EID_ERP_USE_PROTECTION;
+		if (pDevice->bNonERPPresent == true)
+			sFrame.pERP->byContext |= WLAN_EID_ERP_NONERP_PRESENT;
+		if (pDevice->bBarkerPreambleMd == true)
+			sFrame.pERP->byContext |= WLAN_EID_ERP_BARKER_MODE;
+	}
+
+	if ((pMgmt->b11hEnable == true) &&
+	    (pMgmt->eCurrentPHYMode == PHY_TYPE_11A)) {
+		// Country IE
+		pbyBuffer = (unsigned char *)(sFrame.pBuf + sFrame.len);
+		set_country_IE(pMgmt->pAdapter, pbyBuffer);
+		set_country_info(pMgmt->pAdapter, PHY_TYPE_11A, pbyBuffer);
+		uLength += ((PWLAN_IE_COUNTRY) pbyBuffer)->len + WLAN_IEHDR_LEN;
+		pbyBuffer += (((PWLAN_IE_COUNTRY) pbyBuffer)->len + WLAN_IEHDR_LEN);
+		// Power Constrain IE
+		((PWLAN_IE_PW_CONST) pbyBuffer)->byElementID = WLAN_EID_PWR_CONSTRAINT;
+		((PWLAN_IE_PW_CONST) pbyBuffer)->len = 1;
+		((PWLAN_IE_PW_CONST) pbyBuffer)->byPower = 0;
+		pbyBuffer += (1) + WLAN_IEHDR_LEN;
+		uLength += (1) + WLAN_IEHDR_LEN;
+		if (pMgmt->bSwitchChannel == true) {
+			// Channel Switch IE
+			((PWLAN_IE_CH_SW) pbyBuffer)->byElementID = WLAN_EID_CH_SWITCH;
+			((PWLAN_IE_CH_SW) pbyBuffer)->len = 3;
+			((PWLAN_IE_CH_SW) pbyBuffer)->byMode = 1;
+			((PWLAN_IE_CH_SW) pbyBuffer)->byChannel = get_channel_number(pMgmt->pAdapter, pMgmt->byNewChannel);
+			((PWLAN_IE_CH_SW) pbyBuffer)->byCount = 0;
+			pbyBuffer += (3) + WLAN_IEHDR_LEN;
+			uLength += (3) + WLAN_IEHDR_LEN;
+		}
+		// TPC report
+		((PWLAN_IE_TPC_REP) pbyBuffer)->byElementID = WLAN_EID_TPC_REP;
+		((PWLAN_IE_TPC_REP) pbyBuffer)->len = 2;
+		((PWLAN_IE_TPC_REP) pbyBuffer)->byTxPower = CARDbyGetTransmitPower(pMgmt->pAdapter);
+		((PWLAN_IE_TPC_REP) pbyBuffer)->byLinkMargin = 0;
+		pbyBuffer += (2) + WLAN_IEHDR_LEN;
+		uLength += (2) + WLAN_IEHDR_LEN;
+		// IBSS DFS
+		if (pMgmt->eCurrMode != WMAC_MODE_ESS_AP) {
+			pIBSSDFS = (PWLAN_IE_IBSS_DFS) pbyBuffer;
+			pIBSSDFS->byElementID = WLAN_EID_IBSS_DFS;
+			pIBSSDFS->len = 7;
+			memcpy(pIBSSDFS->abyDFSOwner,
+			       pMgmt->abyIBSSDFSOwner,
+			       6);
+			pIBSSDFS->byDFSRecovery = pMgmt->byIBSSDFSRecovery;
+			pbyBuffer += (7) + WLAN_IEHDR_LEN;
+			uLength += (7) + WLAN_IEHDR_LEN;
+			for (ii = CB_MAX_CHANNEL_24G + 1; ii <= CB_MAX_CHANNEL; ii++) {
+				if (get_channel_map_info(pMgmt->pAdapter, ii, pbyBuffer, pbyBuffer+1) == true) {
+					pbyBuffer += 2;
+					uLength += 2;
+					pIBSSDFS->len += 2;
+				}
+			}
+		}
+		sFrame.len += uLength;
+	}
+
+	if (((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len != 0) {
+		sFrame.pExtSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
+		sFrame.len += ((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len + WLAN_IEHDR_LEN;
+		memcpy(sFrame.pExtSuppRates,
+		       pCurrExtSuppRates,
+		       ((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len + WLAN_IEHDR_LEN
+);
+	}
+
+	// hostapd wpa/wpa2 IE
+	if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) && (pDevice->bEnableHostapd == true)) {
+		if (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) {
+			if (pMgmt->wWPAIELen != 0) {
+				sFrame.pRSN = (PWLAN_IE_RSN)(sFrame.pBuf + sFrame.len);
+				memcpy(sFrame.pRSN, pMgmt->abyWPAIE, pMgmt->wWPAIELen);
+				sFrame.len += pMgmt->wWPAIELen;
+			}
+		}
+	}
 
+	// Adjust the length fields
+	pTxPacket->cbMPDULen = sFrame.len;
+	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
 
+	return pTxPacket;
+}
 
 /*+
  *
@@ -3613,271 +3434,259 @@ s_MgrMakeProbeResponse(
  * Return Value:
  *    A ptr to frame or NULL on allocation failure
  *
--*/
-
+ -*/
 
 PSTxMgmtPacket
 s_MgrMakeAssocRequest(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    unsigned char *pDAddr,
-    unsigned short wCurrCapInfo,
-    unsigned short wListenInterval,
-    PWLAN_IE_SSID pCurrSSID,
-    PWLAN_IE_SUPP_RATES pCurrRates,
-    PWLAN_IE_SUPP_RATES pCurrExtSuppRates
-    )
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	unsigned char *pDAddr,
+	unsigned short wCurrCapInfo,
+	unsigned short wListenInterval,
+	PWLAN_IE_SSID pCurrSSID,
+	PWLAN_IE_SUPP_RATES pCurrRates,
+	PWLAN_IE_SUPP_RATES pCurrExtSuppRates
+)
 {
-    PSTxMgmtPacket      pTxPacket = NULL;
-    WLAN_FR_ASSOCREQ    sFrame;
-    unsigned char *pbyIEs;
-    unsigned char *pbyRSN;
-
-
-    pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
-    memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_ASSOCREQ_FR_MAXLEN);
-    pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-    // Setup the sFrame structure.
-    sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
-    sFrame.len = WLAN_ASSOCREQ_FR_MAXLEN;
-    // format fixed field frame structure
-    vMgrEncodeAssocRequest(&sFrame);
-    // Setup the header
-    sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
-        (
-        WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
-        WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_ASSOCREQ)
-        ));
-    memcpy( sFrame.pHdr->sA3.abyAddr1, pDAddr, WLAN_ADDR_LEN);
-    memcpy( sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-    memcpy( sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-
-    // Set the capability and listen interval
-    *(sFrame.pwCapInfo) = cpu_to_le16(wCurrCapInfo);
-    *(sFrame.pwListenInterval) = cpu_to_le16(wListenInterval);
-
-    // sFrame.len point to end of fixed field
-    sFrame.pSSID = (PWLAN_IE_SSID)(sFrame.pBuf + sFrame.len);
-    sFrame.len += pCurrSSID->len + WLAN_IEHDR_LEN;
-    memcpy(sFrame.pSSID, pCurrSSID, pCurrSSID->len + WLAN_IEHDR_LEN);
-
-    pMgmt->sAssocInfo.AssocInfo.RequestIELength = pCurrSSID->len + WLAN_IEHDR_LEN;
-    pMgmt->sAssocInfo.AssocInfo.OffsetRequestIEs = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
-    pbyIEs = pMgmt->sAssocInfo.abyIEs;
-    memcpy(pbyIEs, pCurrSSID, pCurrSSID->len + WLAN_IEHDR_LEN);
-    pbyIEs += pCurrSSID->len + WLAN_IEHDR_LEN;
-
-    // Copy the rate set
-    sFrame.pSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-    if ((pDevice->eCurrentPHYType == PHY_TYPE_11B) && (pCurrRates->len > 4))
-        sFrame.len += 4 + WLAN_IEHDR_LEN;
-    else
-        sFrame.len += pCurrRates->len + WLAN_IEHDR_LEN;
-    memcpy(sFrame.pSuppRates, pCurrRates, pCurrRates->len + WLAN_IEHDR_LEN);
-
-    // Copy the extension rate set
-    if ((pDevice->eCurrentPHYType == PHY_TYPE_11G) && (pCurrExtSuppRates->len > 0)) {
-        sFrame.pExtSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-        sFrame.len += pCurrExtSuppRates->len + WLAN_IEHDR_LEN;
-        memcpy(sFrame.pExtSuppRates, pCurrExtSuppRates, pCurrExtSuppRates->len + WLAN_IEHDR_LEN);
-    }
-
-    pMgmt->sAssocInfo.AssocInfo.RequestIELength += pCurrRates->len + WLAN_IEHDR_LEN;
-    memcpy(pbyIEs, pCurrRates, pCurrRates->len + WLAN_IEHDR_LEN);
-    pbyIEs += pCurrRates->len + WLAN_IEHDR_LEN;
-
-    // for 802.11h
-    if (pMgmt->b11hEnable == true) {
-        if (sFrame.pCurrPowerCap == NULL) {
-            sFrame.pCurrPowerCap = (PWLAN_IE_PW_CAP)(sFrame.pBuf + sFrame.len);
-            sFrame.len += (2 + WLAN_IEHDR_LEN);
-            sFrame.pCurrPowerCap->byElementID = WLAN_EID_PWR_CAPABILITY;
-            sFrame.pCurrPowerCap->len = 2;
-            CARDvGetPowerCapability(pMgmt->pAdapter,
-                                    &(sFrame.pCurrPowerCap->byMinPower),
-                                    &(sFrame.pCurrPowerCap->byMaxPower)
-                                    );
-        }
-        if (sFrame.pCurrSuppCh == NULL) {
-            sFrame.pCurrSuppCh = (PWLAN_IE_SUPP_CH)(sFrame.pBuf + sFrame.len);
-            sFrame.len += set_support_channels(pMgmt->pAdapter,(unsigned char *)sFrame.pCurrSuppCh);
-        }
-    }
-
-    if (((pMgmt->eAuthenMode == WMAC_AUTH_WPA) ||
-         (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) ||
-         (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE)) &&
-        (pMgmt->pCurrBSS != NULL)) {
-        /* WPA IE */
-        sFrame.pRSNWPA = (PWLAN_IE_RSN_EXT)(sFrame.pBuf + sFrame.len);
-        sFrame.pRSNWPA->byElementID = WLAN_EID_RSN_WPA;
-        sFrame.pRSNWPA->len = 16;
-        sFrame.pRSNWPA->abyOUI[0] = 0x00;
-        sFrame.pRSNWPA->abyOUI[1] = 0x50;
-        sFrame.pRSNWPA->abyOUI[2] = 0xf2;
-        sFrame.pRSNWPA->abyOUI[3] = 0x01;
-        sFrame.pRSNWPA->wVersion = 1;
-        //Group Key Cipher Suite
-        sFrame.pRSNWPA->abyMulticast[0] = 0x00;
-        sFrame.pRSNWPA->abyMulticast[1] = 0x50;
-        sFrame.pRSNWPA->abyMulticast[2] = 0xf2;
-        if (pMgmt->byCSSGK == KEY_CTL_WEP) {
-            sFrame.pRSNWPA->abyMulticast[3] = pMgmt->pCurrBSS->byGKType;
-        } else if (pMgmt->byCSSGK == KEY_CTL_TKIP) {
-            sFrame.pRSNWPA->abyMulticast[3] = WPA_TKIP;
-        } else if (pMgmt->byCSSGK == KEY_CTL_CCMP) {
-            sFrame.pRSNWPA->abyMulticast[3] = WPA_AESCCMP;
-        } else {
-            sFrame.pRSNWPA->abyMulticast[3] = WPA_NONE;
-        }
-        // Pairwise Key Cipher Suite
-        sFrame.pRSNWPA->wPKCount = 1;
-        sFrame.pRSNWPA->PKSList[0].abyOUI[0] = 0x00;
-        sFrame.pRSNWPA->PKSList[0].abyOUI[1] = 0x50;
-        sFrame.pRSNWPA->PKSList[0].abyOUI[2] = 0xf2;
-        if (pMgmt->byCSSPK == KEY_CTL_TKIP) {
-            sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_TKIP;
-        } else if (pMgmt->byCSSPK == KEY_CTL_CCMP) {
-            sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_AESCCMP;
-        } else {
-            sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_NONE;
-        }
-        // Auth Key Management Suite
-        pbyRSN = (unsigned char *)(sFrame.pBuf + sFrame.len + 2 + sFrame.pRSNWPA->len);
-        *pbyRSN++=0x01;
-        *pbyRSN++=0x00;
-        *pbyRSN++=0x00;
-
-        *pbyRSN++=0x50;
-        *pbyRSN++=0xf2;
-        if (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) {
-            *pbyRSN++=WPA_AUTH_PSK;
-        }
-        else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA) {
-            *pbyRSN++=WPA_AUTH_IEEE802_1X;
-        }
-        else {
-            *pbyRSN++=WPA_NONE;
-        }
-
-        sFrame.pRSNWPA->len +=6;
-
-        // RSN Capabilities
-
-        *pbyRSN++=0x00;
-        *pbyRSN++=0x00;
-        sFrame.pRSNWPA->len +=2;
-
-        sFrame.len += sFrame.pRSNWPA->len + WLAN_IEHDR_LEN;
-        // copy to AssocInfo. for OID_802_11_ASSOCIATION_INFORMATION
-        pMgmt->sAssocInfo.AssocInfo.RequestIELength += sFrame.pRSNWPA->len + WLAN_IEHDR_LEN;
-        memcpy(pbyIEs, sFrame.pRSNWPA, sFrame.pRSNWPA->len + WLAN_IEHDR_LEN);
-        pbyIEs += sFrame.pRSNWPA->len + WLAN_IEHDR_LEN;
-
-    } else if (((pMgmt->eAuthenMode == WMAC_AUTH_WPA2) ||
-                (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) &&
-               (pMgmt->pCurrBSS != NULL)) {
-        unsigned int ii;
-        unsigned short *pwPMKID;
-
-        // WPA IE
-        sFrame.pRSN = (PWLAN_IE_RSN)(sFrame.pBuf + sFrame.len);
-        sFrame.pRSN->byElementID = WLAN_EID_RSN;
-        sFrame.pRSN->len = 6; //Version(2)+GK(4)
-        sFrame.pRSN->wVersion = 1;
-        //Group Key Cipher Suite
-        sFrame.pRSN->abyRSN[0] = 0x00;
-        sFrame.pRSN->abyRSN[1] = 0x0F;
-        sFrame.pRSN->abyRSN[2] = 0xAC;
-        if (pMgmt->byCSSGK == KEY_CTL_WEP) {
-            sFrame.pRSN->abyRSN[3] = pMgmt->pCurrBSS->byCSSGK;
-        } else if (pMgmt->byCSSGK == KEY_CTL_TKIP) {
-            sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_TKIP;
-        } else if (pMgmt->byCSSGK == KEY_CTL_CCMP) {
-            sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_CCMP;
-        } else {
-            sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_UNKNOWN;
-        }
-
-        // Pairwise Key Cipher Suite
-        sFrame.pRSN->abyRSN[4] = 1;
-        sFrame.pRSN->abyRSN[5] = 0;
-        sFrame.pRSN->abyRSN[6] = 0x00;
-        sFrame.pRSN->abyRSN[7] = 0x0F;
-        sFrame.pRSN->abyRSN[8] = 0xAC;
-        if (pMgmt->byCSSPK == KEY_CTL_TKIP) {
-            sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_TKIP;
-        } else if (pMgmt->byCSSPK == KEY_CTL_CCMP) {
-            sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_CCMP;
-        } else if (pMgmt->byCSSPK == KEY_CTL_NONE) {
-            sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_USE_GROUP;
-        } else {
-            sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_UNKNOWN;
-        }
-        sFrame.pRSN->len += 6;
-
-        // Auth Key Management Suite
-        sFrame.pRSN->abyRSN[10] = 1;
-        sFrame.pRSN->abyRSN[11] = 0;
-        sFrame.pRSN->abyRSN[12] = 0x00;
-        sFrame.pRSN->abyRSN[13] = 0x0F;
-        sFrame.pRSN->abyRSN[14] = 0xAC;
-        if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK) {
-            sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_PSK;
-        } else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2) {
-            sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_802_1X;
-        } else {
-            sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_UNKNOWN;
-        }
-        sFrame.pRSN->len +=6;
-
-        // RSN Capabilities
-        if (pMgmt->pCurrBSS->sRSNCapObj.bRSNCapExist == true) {
-            memcpy(&sFrame.pRSN->abyRSN[16], &pMgmt->pCurrBSS->sRSNCapObj.wRSNCap, 2);
-        } else {
-            sFrame.pRSN->abyRSN[16] = 0;
-            sFrame.pRSN->abyRSN[17] = 0;
-        }
-        sFrame.pRSN->len +=2;
-
-        if ((pDevice->gsPMKID.BSSIDInfoCount > 0) && (pDevice->bRoaming == true) && (pMgmt->eAuthenMode == WMAC_AUTH_WPA2)) {
-            // RSN PMKID
-            pbyRSN = &sFrame.pRSN->abyRSN[18];
-            pwPMKID = (unsigned short *)pbyRSN; // Point to PMKID count
-            *pwPMKID = 0;            // Initialize PMKID count
-            pbyRSN += 2;             // Point to PMKID list
-            for (ii = 0; ii < pDevice->gsPMKID.BSSIDInfoCount; ii++) {
-                if ( !memcmp(&pDevice->gsPMKID.BSSIDInfo[ii].BSSID[0], pMgmt->abyCurrBSSID, ETH_ALEN)) {
-                    (*pwPMKID) ++;
-                    memcpy(pbyRSN, pDevice->gsPMKID.BSSIDInfo[ii].PMKID, 16);
-                    pbyRSN += 16;
-                }
-            }
-            if (*pwPMKID != 0) {
-                sFrame.pRSN->len += (2 + (*pwPMKID)*16);
-            }
-        }
-
-        sFrame.len += sFrame.pRSN->len + WLAN_IEHDR_LEN;
-        // copy to AssocInfo. for OID_802_11_ASSOCIATION_INFORMATION
-        pMgmt->sAssocInfo.AssocInfo.RequestIELength += sFrame.pRSN->len + WLAN_IEHDR_LEN;
-        memcpy(pbyIEs, sFrame.pRSN, sFrame.pRSN->len + WLAN_IEHDR_LEN);
-        pbyIEs += sFrame.pRSN->len + WLAN_IEHDR_LEN;
-    }
-
-
-    // Adjust the length fields
-    pTxPacket->cbMPDULen = sFrame.len;
-    pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
-    return pTxPacket;
-}
-
+	PSTxMgmtPacket      pTxPacket = NULL;
+	WLAN_FR_ASSOCREQ    sFrame;
+	unsigned char *pbyIEs;
+	unsigned char *pbyRSN;
+
+	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
+	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_ASSOCREQ_FR_MAXLEN);
+	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
+	// Setup the sFrame structure.
+	sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
+	sFrame.len = WLAN_ASSOCREQ_FR_MAXLEN;
+	// format fixed field frame structure
+	vMgrEncodeAssocRequest(&sFrame);
+	// Setup the header
+	sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
+		(
+			WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
+			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_ASSOCREQ)
+));
+	memcpy(sFrame.pHdr->sA3.abyAddr1, pDAddr, WLAN_ADDR_LEN);
+	memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
+	memcpy(sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
+
+	// Set the capability and listen interval
+	*(sFrame.pwCapInfo) = cpu_to_le16(wCurrCapInfo);
+	*(sFrame.pwListenInterval) = cpu_to_le16(wListenInterval);
+
+	// sFrame.len point to end of fixed field
+	sFrame.pSSID = (PWLAN_IE_SSID)(sFrame.pBuf + sFrame.len);
+	sFrame.len += pCurrSSID->len + WLAN_IEHDR_LEN;
+	memcpy(sFrame.pSSID, pCurrSSID, pCurrSSID->len + WLAN_IEHDR_LEN);
+
+	pMgmt->sAssocInfo.AssocInfo.RequestIELength = pCurrSSID->len + WLAN_IEHDR_LEN;
+	pMgmt->sAssocInfo.AssocInfo.OffsetRequestIEs = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
+	pbyIEs = pMgmt->sAssocInfo.abyIEs;
+	memcpy(pbyIEs, pCurrSSID, pCurrSSID->len + WLAN_IEHDR_LEN);
+	pbyIEs += pCurrSSID->len + WLAN_IEHDR_LEN;
+
+	// Copy the rate set
+	sFrame.pSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
+	if ((pDevice->eCurrentPHYType == PHY_TYPE_11B) && (pCurrRates->len > 4))
+		sFrame.len += 4 + WLAN_IEHDR_LEN;
+	else
+		sFrame.len += pCurrRates->len + WLAN_IEHDR_LEN;
+	memcpy(sFrame.pSuppRates, pCurrRates, pCurrRates->len + WLAN_IEHDR_LEN);
+
+	// Copy the extension rate set
+	if ((pDevice->eCurrentPHYType == PHY_TYPE_11G) && (pCurrExtSuppRates->len > 0)) {
+		sFrame.pExtSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
+		sFrame.len += pCurrExtSuppRates->len + WLAN_IEHDR_LEN;
+		memcpy(sFrame.pExtSuppRates, pCurrExtSuppRates, pCurrExtSuppRates->len + WLAN_IEHDR_LEN);
+	}
 
+	pMgmt->sAssocInfo.AssocInfo.RequestIELength += pCurrRates->len + WLAN_IEHDR_LEN;
+	memcpy(pbyIEs, pCurrRates, pCurrRates->len + WLAN_IEHDR_LEN);
+	pbyIEs += pCurrRates->len + WLAN_IEHDR_LEN;
+
+	// for 802.11h
+	if (pMgmt->b11hEnable == true) {
+		if (sFrame.pCurrPowerCap == NULL) {
+			sFrame.pCurrPowerCap = (PWLAN_IE_PW_CAP)(sFrame.pBuf + sFrame.len);
+			sFrame.len += (2 + WLAN_IEHDR_LEN);
+			sFrame.pCurrPowerCap->byElementID = WLAN_EID_PWR_CAPABILITY;
+			sFrame.pCurrPowerCap->len = 2;
+			CARDvGetPowerCapability(pMgmt->pAdapter,
+						&(sFrame.pCurrPowerCap->byMinPower),
+						&(sFrame.pCurrPowerCap->byMaxPower)
+);
+		}
+		if (sFrame.pCurrSuppCh == NULL) {
+			sFrame.pCurrSuppCh = (PWLAN_IE_SUPP_CH)(sFrame.pBuf + sFrame.len);
+			sFrame.len += set_support_channels(pMgmt->pAdapter, (unsigned char *)sFrame.pCurrSuppCh);
+		}
+	}
 
+	if (((pMgmt->eAuthenMode == WMAC_AUTH_WPA) ||
+	     (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) ||
+	     (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE)) &&
+	    (pMgmt->pCurrBSS != NULL)) {
+		/* WPA IE */
+		sFrame.pRSNWPA = (PWLAN_IE_RSN_EXT)(sFrame.pBuf + sFrame.len);
+		sFrame.pRSNWPA->byElementID = WLAN_EID_RSN_WPA;
+		sFrame.pRSNWPA->len = 16;
+		sFrame.pRSNWPA->abyOUI[0] = 0x00;
+		sFrame.pRSNWPA->abyOUI[1] = 0x50;
+		sFrame.pRSNWPA->abyOUI[2] = 0xf2;
+		sFrame.pRSNWPA->abyOUI[3] = 0x01;
+		sFrame.pRSNWPA->wVersion = 1;
+		//Group Key Cipher Suite
+		sFrame.pRSNWPA->abyMulticast[0] = 0x00;
+		sFrame.pRSNWPA->abyMulticast[1] = 0x50;
+		sFrame.pRSNWPA->abyMulticast[2] = 0xf2;
+		if (pMgmt->byCSSGK == KEY_CTL_WEP) {
+			sFrame.pRSNWPA->abyMulticast[3] = pMgmt->pCurrBSS->byGKType;
+		} else if (pMgmt->byCSSGK == KEY_CTL_TKIP) {
+			sFrame.pRSNWPA->abyMulticast[3] = WPA_TKIP;
+		} else if (pMgmt->byCSSGK == KEY_CTL_CCMP) {
+			sFrame.pRSNWPA->abyMulticast[3] = WPA_AESCCMP;
+		} else {
+			sFrame.pRSNWPA->abyMulticast[3] = WPA_NONE;
+		}
+		// Pairwise Key Cipher Suite
+		sFrame.pRSNWPA->wPKCount = 1;
+		sFrame.pRSNWPA->PKSList[0].abyOUI[0] = 0x00;
+		sFrame.pRSNWPA->PKSList[0].abyOUI[1] = 0x50;
+		sFrame.pRSNWPA->PKSList[0].abyOUI[2] = 0xf2;
+		if (pMgmt->byCSSPK == KEY_CTL_TKIP) {
+			sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_TKIP;
+		} else if (pMgmt->byCSSPK == KEY_CTL_CCMP) {
+			sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_AESCCMP;
+		} else {
+			sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_NONE;
+		}
+		// Auth Key Management Suite
+		pbyRSN = (unsigned char *)(sFrame.pBuf + sFrame.len + 2 + sFrame.pRSNWPA->len);
+		*pbyRSN++ = 0x01;
+		*pbyRSN++ = 0x00;
+		*pbyRSN++ = 0x00;
+
+		*pbyRSN++ = 0x50;
+		*pbyRSN++ = 0xf2;
+		if (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) {
+			*pbyRSN++ = WPA_AUTH_PSK;
+		} else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA) {
+			*pbyRSN++ = WPA_AUTH_IEEE802_1X;
+		} else {
+			*pbyRSN++ = WPA_NONE;
+		}
 
+		sFrame.pRSNWPA->len += 6;
+
+		// RSN Capabilities
+
+		*pbyRSN++ = 0x00;
+		*pbyRSN++ = 0x00;
+		sFrame.pRSNWPA->len += 2;
+
+		sFrame.len += sFrame.pRSNWPA->len + WLAN_IEHDR_LEN;
+		// copy to AssocInfo. for OID_802_11_ASSOCIATION_INFORMATION
+		pMgmt->sAssocInfo.AssocInfo.RequestIELength += sFrame.pRSNWPA->len + WLAN_IEHDR_LEN;
+		memcpy(pbyIEs, sFrame.pRSNWPA, sFrame.pRSNWPA->len + WLAN_IEHDR_LEN);
+		pbyIEs += sFrame.pRSNWPA->len + WLAN_IEHDR_LEN;
+
+	} else if (((pMgmt->eAuthenMode == WMAC_AUTH_WPA2) ||
+		    (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) &&
+		   (pMgmt->pCurrBSS != NULL)) {
+		unsigned int ii;
+		unsigned short *pwPMKID;
+
+		// WPA IE
+		sFrame.pRSN = (PWLAN_IE_RSN)(sFrame.pBuf + sFrame.len);
+		sFrame.pRSN->byElementID = WLAN_EID_RSN;
+		sFrame.pRSN->len = 6; //Version(2)+GK(4)
+		sFrame.pRSN->wVersion = 1;
+		//Group Key Cipher Suite
+		sFrame.pRSN->abyRSN[0] = 0x00;
+		sFrame.pRSN->abyRSN[1] = 0x0F;
+		sFrame.pRSN->abyRSN[2] = 0xAC;
+		if (pMgmt->byCSSGK == KEY_CTL_WEP) {
+			sFrame.pRSN->abyRSN[3] = pMgmt->pCurrBSS->byCSSGK;
+		} else if (pMgmt->byCSSGK == KEY_CTL_TKIP) {
+			sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_TKIP;
+		} else if (pMgmt->byCSSGK == KEY_CTL_CCMP) {
+			sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_CCMP;
+		} else {
+			sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_UNKNOWN;
+		}
 
+		// Pairwise Key Cipher Suite
+		sFrame.pRSN->abyRSN[4] = 1;
+		sFrame.pRSN->abyRSN[5] = 0;
+		sFrame.pRSN->abyRSN[6] = 0x00;
+		sFrame.pRSN->abyRSN[7] = 0x0F;
+		sFrame.pRSN->abyRSN[8] = 0xAC;
+		if (pMgmt->byCSSPK == KEY_CTL_TKIP) {
+			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_TKIP;
+		} else if (pMgmt->byCSSPK == KEY_CTL_CCMP) {
+			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_CCMP;
+		} else if (pMgmt->byCSSPK == KEY_CTL_NONE) {
+			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_USE_GROUP;
+		} else {
+			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_UNKNOWN;
+		}
+		sFrame.pRSN->len += 6;
+
+		// Auth Key Management Suite
+		sFrame.pRSN->abyRSN[10] = 1;
+		sFrame.pRSN->abyRSN[11] = 0;
+		sFrame.pRSN->abyRSN[12] = 0x00;
+		sFrame.pRSN->abyRSN[13] = 0x0F;
+		sFrame.pRSN->abyRSN[14] = 0xAC;
+		if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK) {
+			sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_PSK;
+		} else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2) {
+			sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_802_1X;
+		} else {
+			sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_UNKNOWN;
+		}
+		sFrame.pRSN->len += 6;
+
+		// RSN Capabilities
+		if (pMgmt->pCurrBSS->sRSNCapObj.bRSNCapExist == true) {
+			memcpy(&sFrame.pRSN->abyRSN[16], &pMgmt->pCurrBSS->sRSNCapObj.wRSNCap, 2);
+		} else {
+			sFrame.pRSN->abyRSN[16] = 0;
+			sFrame.pRSN->abyRSN[17] = 0;
+		}
+		sFrame.pRSN->len += 2;
+
+		if ((pDevice->gsPMKID.BSSIDInfoCount > 0) && (pDevice->bRoaming == true) && (pMgmt->eAuthenMode == WMAC_AUTH_WPA2)) {
+			// RSN PMKID
+			pbyRSN = &sFrame.pRSN->abyRSN[18];
+			pwPMKID = (unsigned short *)pbyRSN; // Point to PMKID count
+			*pwPMKID = 0;            // Initialize PMKID count
+			pbyRSN += 2;             // Point to PMKID list
+			for (ii = 0; ii < pDevice->gsPMKID.BSSIDInfoCount; ii++) {
+				if (!memcmp(&pDevice->gsPMKID.BSSIDInfo[ii].BSSID[0], pMgmt->abyCurrBSSID, ETH_ALEN)) {
+					(*pwPMKID)++;
+					memcpy(pbyRSN, pDevice->gsPMKID.BSSIDInfo[ii].PMKID, 16);
+					pbyRSN += 16;
+				}
+			}
+			if (*pwPMKID != 0) {
+				sFrame.pRSN->len += (2 + (*pwPMKID)*16);
+			}
+		}
 
+		sFrame.len += sFrame.pRSN->len + WLAN_IEHDR_LEN;
+		// copy to AssocInfo. for OID_802_11_ASSOCIATION_INFORMATION
+		pMgmt->sAssocInfo.AssocInfo.RequestIELength += sFrame.pRSN->len + WLAN_IEHDR_LEN;
+		memcpy(pbyIEs, sFrame.pRSN, sFrame.pRSN->len + WLAN_IEHDR_LEN);
+		pbyIEs += sFrame.pRSN->len + WLAN_IEHDR_LEN;
+	}
 
+	// Adjust the length fields
+	pTxPacket->cbMPDULen = sFrame.len;
+	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
+	return pTxPacket;
+}
 
 /*+
  *
@@ -3888,248 +3697,243 @@ s_MgrMakeAssocRequest(
  * Return Value:
  *    A ptr to frame or NULL on allocation failure
  *
--*/
-
+ -*/
 
 PSTxMgmtPacket
 s_MgrMakeReAssocRequest(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    unsigned char *pDAddr,
-    unsigned short wCurrCapInfo,
-    unsigned short wListenInterval,
-    PWLAN_IE_SSID pCurrSSID,
-    PWLAN_IE_SUPP_RATES pCurrRates,
-    PWLAN_IE_SUPP_RATES pCurrExtSuppRates
-    )
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	unsigned char *pDAddr,
+	unsigned short wCurrCapInfo,
+	unsigned short wListenInterval,
+	PWLAN_IE_SSID pCurrSSID,
+	PWLAN_IE_SUPP_RATES pCurrRates,
+	PWLAN_IE_SUPP_RATES pCurrExtSuppRates
+)
 {
-    PSTxMgmtPacket      pTxPacket = NULL;
-    WLAN_FR_REASSOCREQ  sFrame;
-    unsigned char *pbyIEs;
-    unsigned char *pbyRSN;
-
-
-    pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
-    memset( pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_REASSOCREQ_FR_MAXLEN);
-    pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-    /* Setup the sFrame structure. */
-    sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
-    sFrame.len = WLAN_REASSOCREQ_FR_MAXLEN;
-
-    // format fixed field frame structure
-    vMgrEncodeReassocRequest(&sFrame);
-
-    /* Setup the header */
-    sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
-        (
-        WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
-        WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_REASSOCREQ)
-        ));
-    memcpy( sFrame.pHdr->sA3.abyAddr1, pDAddr, WLAN_ADDR_LEN);
-    memcpy( sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-    memcpy( sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-
-    /* Set the capability and listen interval */
-    *(sFrame.pwCapInfo) = cpu_to_le16(wCurrCapInfo);
-    *(sFrame.pwListenInterval) = cpu_to_le16(wListenInterval);
-
-    memcpy(sFrame.pAddrCurrAP, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-    /* Copy the SSID */
-    /* sFrame.len point to end of fixed field */
-    sFrame.pSSID = (PWLAN_IE_SSID)(sFrame.pBuf + sFrame.len);
-    sFrame.len += pCurrSSID->len + WLAN_IEHDR_LEN;
-    memcpy(sFrame.pSSID, pCurrSSID, pCurrSSID->len + WLAN_IEHDR_LEN);
-
-    pMgmt->sAssocInfo.AssocInfo.RequestIELength = pCurrSSID->len + WLAN_IEHDR_LEN;
-    pMgmt->sAssocInfo.AssocInfo.OffsetRequestIEs = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
-    pbyIEs = pMgmt->sAssocInfo.abyIEs;
-    memcpy(pbyIEs, pCurrSSID, pCurrSSID->len + WLAN_IEHDR_LEN);
-    pbyIEs += pCurrSSID->len + WLAN_IEHDR_LEN;
-
-    /* Copy the rate set */
-    /* sFrame.len point to end of SSID */
-    sFrame.pSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-    sFrame.len += pCurrRates->len + WLAN_IEHDR_LEN;
-    memcpy(sFrame.pSuppRates, pCurrRates, pCurrRates->len + WLAN_IEHDR_LEN);
-
-    // Copy the extension rate set
-    if ((pMgmt->eCurrentPHYMode == PHY_TYPE_11G) && (pCurrExtSuppRates->len > 0)) {
-        sFrame.pExtSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-        sFrame.len += pCurrExtSuppRates->len + WLAN_IEHDR_LEN;
-        memcpy(sFrame.pExtSuppRates, pCurrExtSuppRates, pCurrExtSuppRates->len + WLAN_IEHDR_LEN);
-    }
-
-    pMgmt->sAssocInfo.AssocInfo.RequestIELength += pCurrRates->len + WLAN_IEHDR_LEN;
-    memcpy(pbyIEs, pCurrRates, pCurrRates->len + WLAN_IEHDR_LEN);
-    pbyIEs += pCurrRates->len + WLAN_IEHDR_LEN;
-
-    if (((pMgmt->eAuthenMode == WMAC_AUTH_WPA) ||
-         (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) ||
-         (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE)) &&
-        (pMgmt->pCurrBSS != NULL)) {
-        /* WPA IE */
-        sFrame.pRSNWPA = (PWLAN_IE_RSN_EXT)(sFrame.pBuf + sFrame.len);
-        sFrame.pRSNWPA->byElementID = WLAN_EID_RSN_WPA;
-        sFrame.pRSNWPA->len = 16;
-        sFrame.pRSNWPA->abyOUI[0] = 0x00;
-        sFrame.pRSNWPA->abyOUI[1] = 0x50;
-        sFrame.pRSNWPA->abyOUI[2] = 0xf2;
-        sFrame.pRSNWPA->abyOUI[3] = 0x01;
-        sFrame.pRSNWPA->wVersion = 1;
-        //Group Key Cipher Suite
-        sFrame.pRSNWPA->abyMulticast[0] = 0x00;
-        sFrame.pRSNWPA->abyMulticast[1] = 0x50;
-        sFrame.pRSNWPA->abyMulticast[2] = 0xf2;
-        if (pMgmt->byCSSGK == KEY_CTL_WEP) {
-            sFrame.pRSNWPA->abyMulticast[3] = pMgmt->pCurrBSS->byGKType;
-        } else if (pMgmt->byCSSGK == KEY_CTL_TKIP) {
-            sFrame.pRSNWPA->abyMulticast[3] = WPA_TKIP;
-        } else if (pMgmt->byCSSGK == KEY_CTL_CCMP) {
-            sFrame.pRSNWPA->abyMulticast[3] = WPA_AESCCMP;
-        } else {
-            sFrame.pRSNWPA->abyMulticast[3] = WPA_NONE;
-        }
-        // Pairwise Key Cipher Suite
-        sFrame.pRSNWPA->wPKCount = 1;
-        sFrame.pRSNWPA->PKSList[0].abyOUI[0] = 0x00;
-        sFrame.pRSNWPA->PKSList[0].abyOUI[1] = 0x50;
-        sFrame.pRSNWPA->PKSList[0].abyOUI[2] = 0xf2;
-        if (pMgmt->byCSSPK == KEY_CTL_TKIP) {
-            sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_TKIP;
-        } else if (pMgmt->byCSSPK == KEY_CTL_CCMP) {
-            sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_AESCCMP;
-        } else {
-            sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_NONE;
-        }
-        // Auth Key Management Suite
-        pbyRSN = (unsigned char *)(sFrame.pBuf + sFrame.len + 2 + sFrame.pRSNWPA->len);
-        *pbyRSN++=0x01;
-        *pbyRSN++=0x00;
-        *pbyRSN++=0x00;
-
-        *pbyRSN++=0x50;
-        *pbyRSN++=0xf2;
-        if (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) {
-            *pbyRSN++=WPA_AUTH_PSK;
-        } else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA) {
-            *pbyRSN++=WPA_AUTH_IEEE802_1X;
-        } else {
-            *pbyRSN++=WPA_NONE;
-        }
-
-        sFrame.pRSNWPA->len +=6;
-
-        // RSN Capabilities
-        *pbyRSN++=0x00;
-        *pbyRSN++=0x00;
-        sFrame.pRSNWPA->len +=2;
-
-        sFrame.len += sFrame.pRSNWPA->len + WLAN_IEHDR_LEN;
-        // copy to AssocInfo. for OID_802_11_ASSOCIATION_INFORMATION
-        pMgmt->sAssocInfo.AssocInfo.RequestIELength += sFrame.pRSNWPA->len + WLAN_IEHDR_LEN;
-        memcpy(pbyIEs, sFrame.pRSNWPA, sFrame.pRSNWPA->len + WLAN_IEHDR_LEN);
-        pbyIEs += sFrame.pRSNWPA->len + WLAN_IEHDR_LEN;
-
-    } else if (((pMgmt->eAuthenMode == WMAC_AUTH_WPA2) ||
-                (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) &&
-               (pMgmt->pCurrBSS != NULL)) {
-        unsigned int ii;
-        unsigned short *pwPMKID;
-
-        /* WPA IE */
-        sFrame.pRSN = (PWLAN_IE_RSN)(sFrame.pBuf + sFrame.len);
-        sFrame.pRSN->byElementID = WLAN_EID_RSN;
-        sFrame.pRSN->len = 6; //Version(2)+GK(4)
-        sFrame.pRSN->wVersion = 1;
-        //Group Key Cipher Suite
-        sFrame.pRSN->abyRSN[0] = 0x00;
-        sFrame.pRSN->abyRSN[1] = 0x0F;
-        sFrame.pRSN->abyRSN[2] = 0xAC;
-        if (pMgmt->byCSSGK == KEY_CTL_WEP) {
-            sFrame.pRSN->abyRSN[3] = pMgmt->pCurrBSS->byCSSGK;
-        } else if (pMgmt->byCSSGK == KEY_CTL_TKIP) {
-            sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_TKIP;
-        } else if (pMgmt->byCSSGK == KEY_CTL_CCMP) {
-            sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_CCMP;
-        } else {
-            sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_UNKNOWN;
-        }
-
-        // Pairwise Key Cipher Suite
-        sFrame.pRSN->abyRSN[4] = 1;
-        sFrame.pRSN->abyRSN[5] = 0;
-        sFrame.pRSN->abyRSN[6] = 0x00;
-        sFrame.pRSN->abyRSN[7] = 0x0F;
-        sFrame.pRSN->abyRSN[8] = 0xAC;
-        if (pMgmt->byCSSPK == KEY_CTL_TKIP) {
-            sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_TKIP;
-        } else if (pMgmt->byCSSPK == KEY_CTL_CCMP) {
-            sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_CCMP;
-        } else if (pMgmt->byCSSPK == KEY_CTL_NONE) {
-            sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_USE_GROUP;
-        } else {
-            sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_UNKNOWN;
-        }
-        sFrame.pRSN->len += 6;
-
-        // Auth Key Management Suite
-        sFrame.pRSN->abyRSN[10] = 1;
-        sFrame.pRSN->abyRSN[11] = 0;
-        sFrame.pRSN->abyRSN[12] = 0x00;
-        sFrame.pRSN->abyRSN[13] = 0x0F;
-        sFrame.pRSN->abyRSN[14] = 0xAC;
-        if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK) {
-            sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_PSK;
-        } else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2) {
-            sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_802_1X;
-        } else {
-            sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_UNKNOWN;
-        }
-        sFrame.pRSN->len +=6;
-
-        // RSN Capabilities
-        if (pMgmt->pCurrBSS->sRSNCapObj.bRSNCapExist == true) {
-            memcpy(&sFrame.pRSN->abyRSN[16], &pMgmt->pCurrBSS->sRSNCapObj.wRSNCap, 2);
-        } else {
-            sFrame.pRSN->abyRSN[16] = 0;
-            sFrame.pRSN->abyRSN[17] = 0;
-        }
-        sFrame.pRSN->len +=2;
-
-        if ((pDevice->gsPMKID.BSSIDInfoCount > 0) && (pDevice->bRoaming == true) && (pMgmt->eAuthenMode == WMAC_AUTH_WPA2)) {
-            // RSN PMKID
-            pbyRSN = &sFrame.pRSN->abyRSN[18];
-            pwPMKID = (unsigned short *)pbyRSN; // Point to PMKID count
-            *pwPMKID = 0;            // Initialize PMKID count
-            pbyRSN += 2;             // Point to PMKID list
-            for (ii = 0; ii < pDevice->gsPMKID.BSSIDInfoCount; ii++) {
-                if ( !memcmp(&pDevice->gsPMKID.BSSIDInfo[ii].BSSID[0], pMgmt->abyCurrBSSID, ETH_ALEN)) {
-                    (*pwPMKID) ++;
-                    memcpy(pbyRSN, pDevice->gsPMKID.BSSIDInfo[ii].PMKID, 16);
-                    pbyRSN += 16;
-                }
-            }
-            if (*pwPMKID != 0) {
-                sFrame.pRSN->len += (2 + (*pwPMKID)*16);
-            }
-        }
-
-        sFrame.len += sFrame.pRSN->len + WLAN_IEHDR_LEN;
-        // copy to AssocInfo. for OID_802_11_ASSOCIATION_INFORMATION
-        pMgmt->sAssocInfo.AssocInfo.RequestIELength += sFrame.pRSN->len + WLAN_IEHDR_LEN;
-        memcpy(pbyIEs, sFrame.pRSN, sFrame.pRSN->len + WLAN_IEHDR_LEN);
-        pbyIEs += sFrame.pRSN->len + WLAN_IEHDR_LEN;
-    }
-
-
-    /* Adjust the length fields */
-    pTxPacket->cbMPDULen = sFrame.len;
-    pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
-
-    return pTxPacket;
-}
+	PSTxMgmtPacket      pTxPacket = NULL;
+	WLAN_FR_REASSOCREQ  sFrame;
+	unsigned char *pbyIEs;
+	unsigned char *pbyRSN;
+
+	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
+	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_REASSOCREQ_FR_MAXLEN);
+	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
+	/* Setup the sFrame structure. */
+	sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
+	sFrame.len = WLAN_REASSOCREQ_FR_MAXLEN;
+
+	// format fixed field frame structure
+	vMgrEncodeReassocRequest(&sFrame);
+
+	/* Setup the header */
+	sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
+		(
+			WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
+			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_REASSOCREQ)
+));
+	memcpy(sFrame.pHdr->sA3.abyAddr1, pDAddr, WLAN_ADDR_LEN);
+	memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
+	memcpy(sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
+
+	/* Set the capability and listen interval */
+	*(sFrame.pwCapInfo) = cpu_to_le16(wCurrCapInfo);
+	*(sFrame.pwListenInterval) = cpu_to_le16(wListenInterval);
+
+	memcpy(sFrame.pAddrCurrAP, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
+	/* Copy the SSID */
+	/* sFrame.len point to end of fixed field */
+	sFrame.pSSID = (PWLAN_IE_SSID)(sFrame.pBuf + sFrame.len);
+	sFrame.len += pCurrSSID->len + WLAN_IEHDR_LEN;
+	memcpy(sFrame.pSSID, pCurrSSID, pCurrSSID->len + WLAN_IEHDR_LEN);
+
+	pMgmt->sAssocInfo.AssocInfo.RequestIELength = pCurrSSID->len + WLAN_IEHDR_LEN;
+	pMgmt->sAssocInfo.AssocInfo.OffsetRequestIEs = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
+	pbyIEs = pMgmt->sAssocInfo.abyIEs;
+	memcpy(pbyIEs, pCurrSSID, pCurrSSID->len + WLAN_IEHDR_LEN);
+	pbyIEs += pCurrSSID->len + WLAN_IEHDR_LEN;
+
+	/* Copy the rate set */
+	/* sFrame.len point to end of SSID */
+	sFrame.pSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
+	sFrame.len += pCurrRates->len + WLAN_IEHDR_LEN;
+	memcpy(sFrame.pSuppRates, pCurrRates, pCurrRates->len + WLAN_IEHDR_LEN);
+
+	// Copy the extension rate set
+	if ((pMgmt->eCurrentPHYMode == PHY_TYPE_11G) && (pCurrExtSuppRates->len > 0)) {
+		sFrame.pExtSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
+		sFrame.len += pCurrExtSuppRates->len + WLAN_IEHDR_LEN;
+		memcpy(sFrame.pExtSuppRates, pCurrExtSuppRates, pCurrExtSuppRates->len + WLAN_IEHDR_LEN);
+	}
+
+	pMgmt->sAssocInfo.AssocInfo.RequestIELength += pCurrRates->len + WLAN_IEHDR_LEN;
+	memcpy(pbyIEs, pCurrRates, pCurrRates->len + WLAN_IEHDR_LEN);
+	pbyIEs += pCurrRates->len + WLAN_IEHDR_LEN;
+
+	if (((pMgmt->eAuthenMode == WMAC_AUTH_WPA) ||
+	     (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) ||
+	     (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE)) &&
+	    (pMgmt->pCurrBSS != NULL)) {
+		/* WPA IE */
+		sFrame.pRSNWPA = (PWLAN_IE_RSN_EXT)(sFrame.pBuf + sFrame.len);
+		sFrame.pRSNWPA->byElementID = WLAN_EID_RSN_WPA;
+		sFrame.pRSNWPA->len = 16;
+		sFrame.pRSNWPA->abyOUI[0] = 0x00;
+		sFrame.pRSNWPA->abyOUI[1] = 0x50;
+		sFrame.pRSNWPA->abyOUI[2] = 0xf2;
+		sFrame.pRSNWPA->abyOUI[3] = 0x01;
+		sFrame.pRSNWPA->wVersion = 1;
+		//Group Key Cipher Suite
+		sFrame.pRSNWPA->abyMulticast[0] = 0x00;
+		sFrame.pRSNWPA->abyMulticast[1] = 0x50;
+		sFrame.pRSNWPA->abyMulticast[2] = 0xf2;
+		if (pMgmt->byCSSGK == KEY_CTL_WEP) {
+			sFrame.pRSNWPA->abyMulticast[3] = pMgmt->pCurrBSS->byGKType;
+		} else if (pMgmt->byCSSGK == KEY_CTL_TKIP) {
+			sFrame.pRSNWPA->abyMulticast[3] = WPA_TKIP;
+		} else if (pMgmt->byCSSGK == KEY_CTL_CCMP) {
+			sFrame.pRSNWPA->abyMulticast[3] = WPA_AESCCMP;
+		} else {
+			sFrame.pRSNWPA->abyMulticast[3] = WPA_NONE;
+		}
+		// Pairwise Key Cipher Suite
+		sFrame.pRSNWPA->wPKCount = 1;
+		sFrame.pRSNWPA->PKSList[0].abyOUI[0] = 0x00;
+		sFrame.pRSNWPA->PKSList[0].abyOUI[1] = 0x50;
+		sFrame.pRSNWPA->PKSList[0].abyOUI[2] = 0xf2;
+		if (pMgmt->byCSSPK == KEY_CTL_TKIP) {
+			sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_TKIP;
+		} else if (pMgmt->byCSSPK == KEY_CTL_CCMP) {
+			sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_AESCCMP;
+		} else {
+			sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_NONE;
+		}
+		// Auth Key Management Suite
+		pbyRSN = (unsigned char *)(sFrame.pBuf + sFrame.len + 2 + sFrame.pRSNWPA->len);
+		*pbyRSN++ = 0x01;
+		*pbyRSN++ = 0x00;
+		*pbyRSN++ = 0x00;
+
+		*pbyRSN++ = 0x50;
+		*pbyRSN++ = 0xf2;
+		if (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) {
+			*pbyRSN++ = WPA_AUTH_PSK;
+		} else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA) {
+			*pbyRSN++ = WPA_AUTH_IEEE802_1X;
+		} else {
+			*pbyRSN++ = WPA_NONE;
+		}
 
+		sFrame.pRSNWPA->len += 6;
+
+		// RSN Capabilities
+		*pbyRSN++ = 0x00;
+		*pbyRSN++ = 0x00;
+		sFrame.pRSNWPA->len += 2;
+
+		sFrame.len += sFrame.pRSNWPA->len + WLAN_IEHDR_LEN;
+		// copy to AssocInfo. for OID_802_11_ASSOCIATION_INFORMATION
+		pMgmt->sAssocInfo.AssocInfo.RequestIELength += sFrame.pRSNWPA->len + WLAN_IEHDR_LEN;
+		memcpy(pbyIEs, sFrame.pRSNWPA, sFrame.pRSNWPA->len + WLAN_IEHDR_LEN);
+		pbyIEs += sFrame.pRSNWPA->len + WLAN_IEHDR_LEN;
+
+	} else if (((pMgmt->eAuthenMode == WMAC_AUTH_WPA2) ||
+		    (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) &&
+		   (pMgmt->pCurrBSS != NULL)) {
+		unsigned int ii;
+		unsigned short *pwPMKID;
+
+		/* WPA IE */
+		sFrame.pRSN = (PWLAN_IE_RSN)(sFrame.pBuf + sFrame.len);
+		sFrame.pRSN->byElementID = WLAN_EID_RSN;
+		sFrame.pRSN->len = 6; //Version(2)+GK(4)
+		sFrame.pRSN->wVersion = 1;
+		//Group Key Cipher Suite
+		sFrame.pRSN->abyRSN[0] = 0x00;
+		sFrame.pRSN->abyRSN[1] = 0x0F;
+		sFrame.pRSN->abyRSN[2] = 0xAC;
+		if (pMgmt->byCSSGK == KEY_CTL_WEP) {
+			sFrame.pRSN->abyRSN[3] = pMgmt->pCurrBSS->byCSSGK;
+		} else if (pMgmt->byCSSGK == KEY_CTL_TKIP) {
+			sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_TKIP;
+		} else if (pMgmt->byCSSGK == KEY_CTL_CCMP) {
+			sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_CCMP;
+		} else {
+			sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_UNKNOWN;
+		}
 
+		// Pairwise Key Cipher Suite
+		sFrame.pRSN->abyRSN[4] = 1;
+		sFrame.pRSN->abyRSN[5] = 0;
+		sFrame.pRSN->abyRSN[6] = 0x00;
+		sFrame.pRSN->abyRSN[7] = 0x0F;
+		sFrame.pRSN->abyRSN[8] = 0xAC;
+		if (pMgmt->byCSSPK == KEY_CTL_TKIP) {
+			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_TKIP;
+		} else if (pMgmt->byCSSPK == KEY_CTL_CCMP) {
+			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_CCMP;
+		} else if (pMgmt->byCSSPK == KEY_CTL_NONE) {
+			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_USE_GROUP;
+		} else {
+			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_UNKNOWN;
+		}
+		sFrame.pRSN->len += 6;
+
+		// Auth Key Management Suite
+		sFrame.pRSN->abyRSN[10] = 1;
+		sFrame.pRSN->abyRSN[11] = 0;
+		sFrame.pRSN->abyRSN[12] = 0x00;
+		sFrame.pRSN->abyRSN[13] = 0x0F;
+		sFrame.pRSN->abyRSN[14] = 0xAC;
+		if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK) {
+			sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_PSK;
+		} else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2) {
+			sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_802_1X;
+		} else {
+			sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_UNKNOWN;
+		}
+		sFrame.pRSN->len += 6;
+
+		// RSN Capabilities
+		if (pMgmt->pCurrBSS->sRSNCapObj.bRSNCapExist == true) {
+			memcpy(&sFrame.pRSN->abyRSN[16], &pMgmt->pCurrBSS->sRSNCapObj.wRSNCap, 2);
+		} else {
+			sFrame.pRSN->abyRSN[16] = 0;
+			sFrame.pRSN->abyRSN[17] = 0;
+		}
+		sFrame.pRSN->len += 2;
+
+		if ((pDevice->gsPMKID.BSSIDInfoCount > 0) && (pDevice->bRoaming == true) && (pMgmt->eAuthenMode == WMAC_AUTH_WPA2)) {
+			// RSN PMKID
+			pbyRSN = &sFrame.pRSN->abyRSN[18];
+			pwPMKID = (unsigned short *)pbyRSN; // Point to PMKID count
+			*pwPMKID = 0;            // Initialize PMKID count
+			pbyRSN += 2;             // Point to PMKID list
+			for (ii = 0; ii < pDevice->gsPMKID.BSSIDInfoCount; ii++) {
+				if (!memcmp(&pDevice->gsPMKID.BSSIDInfo[ii].BSSID[0], pMgmt->abyCurrBSSID, ETH_ALEN)) {
+					(*pwPMKID)++;
+					memcpy(pbyRSN, pDevice->gsPMKID.BSSIDInfo[ii].PMKID, 16);
+					pbyRSN += 16;
+				}
+			}
+			if (*pwPMKID != 0) {
+				sFrame.pRSN->len += (2 + (*pwPMKID) * 16);
+			}
+		}
+
+		sFrame.len += sFrame.pRSN->len + WLAN_IEHDR_LEN;
+		// copy to AssocInfo. for OID_802_11_ASSOCIATION_INFORMATION
+		pMgmt->sAssocInfo.AssocInfo.RequestIELength += sFrame.pRSN->len + WLAN_IEHDR_LEN;
+		memcpy(pbyIEs, sFrame.pRSN, sFrame.pRSN->len + WLAN_IEHDR_LEN);
+		pbyIEs += sFrame.pRSN->len + WLAN_IEHDR_LEN;
+	}
+
+	/* Adjust the length fields */
+	pTxPacket->cbMPDULen = sFrame.len;
+	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
+
+	return pTxPacket;
+}
 
 /*+
  *
@@ -4140,70 +3944,67 @@ s_MgrMakeReAssocRequest(
  * Return Value:
  *    PTR to frame; or NULL on allocation failure
  *
--*/
-
+ -*/
 
 PSTxMgmtPacket
 s_MgrMakeAssocResponse(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    unsigned short wCurrCapInfo,
-    unsigned short wAssocStatus,
-    unsigned short wAssocAID,
-    unsigned char *pDstAddr,
-    PWLAN_IE_SUPP_RATES pCurrSuppRates,
-    PWLAN_IE_SUPP_RATES pCurrExtSuppRates
-    )
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	unsigned short wCurrCapInfo,
+	unsigned short wAssocStatus,
+	unsigned short wAssocAID,
+	unsigned char *pDstAddr,
+	PWLAN_IE_SUPP_RATES pCurrSuppRates,
+	PWLAN_IE_SUPP_RATES pCurrExtSuppRates
+)
 {
-    PSTxMgmtPacket      pTxPacket = NULL;
-    WLAN_FR_ASSOCRESP   sFrame;
-
-
-    pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
-    memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_ASSOCREQ_FR_MAXLEN);
-    pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-    // Setup the sFrame structure
-    sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
-    sFrame.len = WLAN_REASSOCRESP_FR_MAXLEN;
-    vMgrEncodeAssocResponse(&sFrame);
-    // Setup the header
-    sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
-        (
-        WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
-        WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_ASSOCRESP)
-        ));
-    memcpy( sFrame.pHdr->sA3.abyAddr1, pDstAddr, WLAN_ADDR_LEN);
-    memcpy( sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-    memcpy( sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-
-    *sFrame.pwCapInfo = cpu_to_le16(wCurrCapInfo);
-    *sFrame.pwStatus = cpu_to_le16(wAssocStatus);
-    *sFrame.pwAid = cpu_to_le16((unsigned short)(wAssocAID | BIT14 | BIT15));
-
-    // Copy the rate set
-    sFrame.pSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-    sFrame.len += ((PWLAN_IE_SUPP_RATES)pCurrSuppRates)->len + WLAN_IEHDR_LEN;
-    memcpy(sFrame.pSuppRates,
-           pCurrSuppRates,
-           ((PWLAN_IE_SUPP_RATES)pCurrSuppRates)->len + WLAN_IEHDR_LEN
-          );
-
-    if (((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len != 0) {
-        sFrame.pExtSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-        sFrame.len += ((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len + WLAN_IEHDR_LEN;
-        memcpy(sFrame.pExtSuppRates,
-             pCurrExtSuppRates,
-             ((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len + WLAN_IEHDR_LEN
-             );
-    }
-
-    // Adjust the length fields
-    pTxPacket->cbMPDULen = sFrame.len;
-    pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
-
-    return pTxPacket;
-}
+	PSTxMgmtPacket      pTxPacket = NULL;
+	WLAN_FR_ASSOCRESP   sFrame;
+
+	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
+	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_ASSOCREQ_FR_MAXLEN);
+	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
+	// Setup the sFrame structure
+	sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
+	sFrame.len = WLAN_REASSOCRESP_FR_MAXLEN;
+	vMgrEncodeAssocResponse(&sFrame);
+	// Setup the header
+	sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
+		(
+			WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
+			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_ASSOCRESP)
+));
+	memcpy(sFrame.pHdr->sA3.abyAddr1, pDstAddr, WLAN_ADDR_LEN);
+	memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
+	memcpy(sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
+
+	*sFrame.pwCapInfo = cpu_to_le16(wCurrCapInfo);
+	*sFrame.pwStatus = cpu_to_le16(wAssocStatus);
+	*sFrame.pwAid = cpu_to_le16((unsigned short)(wAssocAID | BIT14 | BIT15));
+
+	// Copy the rate set
+	sFrame.pSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
+	sFrame.len += ((PWLAN_IE_SUPP_RATES)pCurrSuppRates)->len + WLAN_IEHDR_LEN;
+	memcpy(sFrame.pSuppRates,
+	       pCurrSuppRates,
+	       ((PWLAN_IE_SUPP_RATES)pCurrSuppRates)->len + WLAN_IEHDR_LEN
+);
+
+	if (((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len != 0) {
+		sFrame.pExtSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
+		sFrame.len += ((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len + WLAN_IEHDR_LEN;
+		memcpy(sFrame.pExtSuppRates,
+		       pCurrExtSuppRates,
+		       ((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len + WLAN_IEHDR_LEN
+);
+	}
 
+	// Adjust the length fields
+	pTxPacket->cbMPDULen = sFrame.len;
+	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
+
+	return pTxPacket;
+}
 
 /*+
  *
@@ -4214,70 +4015,67 @@ s_MgrMakeAssocResponse(
  * Return Value:
  *    PTR to frame; or NULL on allocation failure
  *
--*/
-
+ -*/
 
 PSTxMgmtPacket
 s_MgrMakeReAssocResponse(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    unsigned short wCurrCapInfo,
-    unsigned short wAssocStatus,
-    unsigned short wAssocAID,
-    unsigned char *pDstAddr,
-    PWLAN_IE_SUPP_RATES pCurrSuppRates,
-    PWLAN_IE_SUPP_RATES pCurrExtSuppRates
-    )
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	unsigned short wCurrCapInfo,
+	unsigned short wAssocStatus,
+	unsigned short wAssocAID,
+	unsigned char *pDstAddr,
+	PWLAN_IE_SUPP_RATES pCurrSuppRates,
+	PWLAN_IE_SUPP_RATES pCurrExtSuppRates
+)
 {
-    PSTxMgmtPacket      pTxPacket = NULL;
-    WLAN_FR_REASSOCRESP   sFrame;
-
-
-    pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
-    memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_ASSOCREQ_FR_MAXLEN);
-    pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-    // Setup the sFrame structure
-    sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
-    sFrame.len = WLAN_REASSOCRESP_FR_MAXLEN;
-    vMgrEncodeReassocResponse(&sFrame);
-    // Setup the header
-    sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
-        (
-        WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
-        WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_REASSOCRESP)
-        ));
-    memcpy( sFrame.pHdr->sA3.abyAddr1, pDstAddr, WLAN_ADDR_LEN);
-    memcpy( sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-    memcpy( sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-
-    *sFrame.pwCapInfo = cpu_to_le16(wCurrCapInfo);
-    *sFrame.pwStatus = cpu_to_le16(wAssocStatus);
-    *sFrame.pwAid = cpu_to_le16((unsigned short)(wAssocAID | BIT14 | BIT15));
-
-    // Copy the rate set
-    sFrame.pSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-    sFrame.len += ((PWLAN_IE_SUPP_RATES)pCurrSuppRates)->len + WLAN_IEHDR_LEN;
-    memcpy(sFrame.pSuppRates,
-             pCurrSuppRates,
-             ((PWLAN_IE_SUPP_RATES)pCurrSuppRates)->len + WLAN_IEHDR_LEN
-             );
-
-    if (((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len != 0) {
-        sFrame.pExtSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-        sFrame.len += ((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len + WLAN_IEHDR_LEN;
-        memcpy(sFrame.pExtSuppRates,
-             pCurrExtSuppRates,
-             ((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len + WLAN_IEHDR_LEN
-             );
-    }
-
-    // Adjust the length fields
-    pTxPacket->cbMPDULen = sFrame.len;
-    pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
-
-    return pTxPacket;
-}
+	PSTxMgmtPacket      pTxPacket = NULL;
+	WLAN_FR_REASSOCRESP   sFrame;
+
+	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
+	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_ASSOCREQ_FR_MAXLEN);
+	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
+	// Setup the sFrame structure
+	sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
+	sFrame.len = WLAN_REASSOCRESP_FR_MAXLEN;
+	vMgrEncodeReassocResponse(&sFrame);
+	// Setup the header
+	sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
+		(
+			WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
+			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_REASSOCRESP)
+));
+	memcpy(sFrame.pHdr->sA3.abyAddr1, pDstAddr, WLAN_ADDR_LEN);
+	memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
+	memcpy(sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
+
+	*sFrame.pwCapInfo = cpu_to_le16(wCurrCapInfo);
+	*sFrame.pwStatus = cpu_to_le16(wAssocStatus);
+	*sFrame.pwAid = cpu_to_le16((unsigned short)(wAssocAID | BIT14 | BIT15));
+
+	// Copy the rate set
+	sFrame.pSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
+	sFrame.len += ((PWLAN_IE_SUPP_RATES)pCurrSuppRates)->len + WLAN_IEHDR_LEN;
+	memcpy(sFrame.pSuppRates,
+	       pCurrSuppRates,
+	       ((PWLAN_IE_SUPP_RATES)pCurrSuppRates)->len + WLAN_IEHDR_LEN
+);
+
+	if (((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len != 0) {
+		sFrame.pExtSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
+		sFrame.len += ((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len + WLAN_IEHDR_LEN;
+		memcpy(sFrame.pExtSuppRates,
+		       pCurrExtSuppRates,
+		       ((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len + WLAN_IEHDR_LEN
+);
+	}
+
+	// Adjust the length fields
+	pTxPacket->cbMPDULen = sFrame.len;
+	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
 
+	return pTxPacket;
+}
 
 /*+
  *
@@ -4288,119 +4086,115 @@ s_MgrMakeReAssocResponse(
  * Return Value:
  *    none.
  *
--*/
+ -*/
 
 static
 void
 s_vMgrRxProbeResponse(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    PSRxMgmtPacket pRxPacket
-    )
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	PSRxMgmtPacket pRxPacket
+)
 {
-    PKnownBSS           pBSSList = NULL;
-    WLAN_FR_PROBERESP   sFrame;
-    unsigned char byCurrChannel = pRxPacket->byRxChannel;
-    ERPObject           sERP;
-    unsigned char byIEChannel = 0;
-    bool bChannelHit = true;
-
-
-    memset(&sFrame, 0, sizeof(WLAN_FR_PROBERESP));
-    // decode the frame
-    sFrame.len = pRxPacket->cbMPDULen;
-    sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
-    vMgrDecodeProbeResponse(&sFrame);
-
-    if ((sFrame.pqwTimestamp == 0) ||
-        (sFrame.pwBeaconInterval == 0) ||
-        (sFrame.pwCapInfo == 0) ||
-        (sFrame.pSSID == 0) ||
-        (sFrame.pSuppRates == 0)) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Probe resp:Fail addr:[%p] \n", pRxPacket->p80211Header);
-        DBG_PORT80(0xCC);
-        return;
-    }
-
-    if(sFrame.pSSID->len == 0)
-       DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Rx Probe resp: SSID len = 0 \n");
-
-    if (sFrame.pDSParms != 0) {
-        if (byCurrChannel > CB_MAX_CHANNEL_24G) {
-            // channel remapping to
-            byIEChannel = get_channel_mapping(pMgmt->pAdapter, sFrame.pDSParms->byCurrChannel, PHY_TYPE_11A);
-        } else {
-            byIEChannel = sFrame.pDSParms->byCurrChannel;
-        }
-        if (byCurrChannel != byIEChannel) {
-            // adjust channel info. bcs we rcv adjacent channel packets
-            bChannelHit = false;
-            byCurrChannel = byIEChannel;
-        }
-    } else {
-        // no DS channel info
-        bChannelHit = true;
-    }
+	PKnownBSS           pBSSList = NULL;
+	WLAN_FR_PROBERESP   sFrame;
+	unsigned char byCurrChannel = pRxPacket->byRxChannel;
+	ERPObject           sERP;
+	unsigned char byIEChannel = 0;
+	bool bChannelHit = true;
+
+	memset(&sFrame, 0, sizeof(WLAN_FR_PROBERESP));
+	// decode the frame
+	sFrame.len = pRxPacket->cbMPDULen;
+	sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
+	vMgrDecodeProbeResponse(&sFrame);
+
+	if ((sFrame.pqwTimestamp == 0) ||
+	    (sFrame.pwBeaconInterval == 0) ||
+	    (sFrame.pwCapInfo == 0) ||
+	    (sFrame.pSSID == 0) ||
+	    (sFrame.pSuppRates == 0)) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Probe resp:Fail addr:[%p] \n", pRxPacket->p80211Header);
+		DBG_PORT80(0xCC);
+		return;
+	}
+
+	if (sFrame.pSSID->len == 0)
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Rx Probe resp: SSID len = 0 \n");
+
+	if (sFrame.pDSParms != 0) {
+		if (byCurrChannel > CB_MAX_CHANNEL_24G) {
+			// channel remapping to
+			byIEChannel = get_channel_mapping(pMgmt->pAdapter, sFrame.pDSParms->byCurrChannel, PHY_TYPE_11A);
+		} else {
+			byIEChannel = sFrame.pDSParms->byCurrChannel;
+		}
+		if (byCurrChannel != byIEChannel) {
+			// adjust channel info. bcs we rcv adjacent channel packets
+			bChannelHit = false;
+			byCurrChannel = byIEChannel;
+		}
+	} else {
+		// no DS channel info
+		bChannelHit = true;
+	}
 
 //2008-0730-01<Add>by MikeLiu
-if(ChannelExceedZoneType(pDevice,byCurrChannel)==true)
-      return;
-
-    if (sFrame.pERP != NULL) {
-        sERP.byERP = sFrame.pERP->byContext;
-        sERP.bERPExist = true;
-    } else {
-        sERP.bERPExist = false;
-        sERP.byERP = 0;
-    }
-
-
-    // update or insert the bss
-    pBSSList = BSSpAddrIsInBSSList((void *)pDevice, sFrame.pHdr->sA3.abyAddr3, sFrame.pSSID);
-    if (pBSSList) {
-        BSSbUpdateToBSSList((void *)pDevice,
-                            *sFrame.pqwTimestamp,
-                            *sFrame.pwBeaconInterval,
-                            *sFrame.pwCapInfo,
-                            byCurrChannel,
-                            bChannelHit,
-                            sFrame.pSSID,
-                            sFrame.pSuppRates,
-                            sFrame.pExtSuppRates,
-                            &sERP,
-                            sFrame.pRSN,
-                            sFrame.pRSNWPA,
-                            sFrame.pIE_Country,
-                            sFrame.pIE_Quiet,
-                            pBSSList,
-                            sFrame.len - WLAN_HDR_ADDR3_LEN,
-                            sFrame.pHdr->sA4.abyAddr4,   // payload of probresponse
-                            (void *)pRxPacket
-                           );
-    }
-    else {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Probe resp/insert: RxChannel = : %d\n", byCurrChannel);
-        BSSbInsertToBSSList((void *)pDevice,
-                            sFrame.pHdr->sA3.abyAddr3,
-                            *sFrame.pqwTimestamp,
-                            *sFrame.pwBeaconInterval,
-                            *sFrame.pwCapInfo,
-                            byCurrChannel,
-                            sFrame.pSSID,
-                            sFrame.pSuppRates,
-                            sFrame.pExtSuppRates,
-                            &sERP,
-                            sFrame.pRSN,
-                            sFrame.pRSNWPA,
-                            sFrame.pIE_Country,
-                            sFrame.pIE_Quiet,
-                            sFrame.len - WLAN_HDR_ADDR3_LEN,
-                            sFrame.pHdr->sA4.abyAddr4,   // payload of beacon
-                            (void *)pRxPacket
-                           );
-    }
-    return;
+	if (ChannelExceedZoneType(pDevice, byCurrChannel) == true)
+		return;
+
+	if (sFrame.pERP != NULL) {
+		sERP.byERP = sFrame.pERP->byContext;
+		sERP.bERPExist = true;
+	} else {
+		sERP.bERPExist = false;
+		sERP.byERP = 0;
+	}
 
+	// update or insert the bss
+	pBSSList = BSSpAddrIsInBSSList((void *)pDevice, sFrame.pHdr->sA3.abyAddr3, sFrame.pSSID);
+	if (pBSSList) {
+		BSSbUpdateToBSSList((void *)pDevice,
+				    *sFrame.pqwTimestamp,
+				    *sFrame.pwBeaconInterval,
+				    *sFrame.pwCapInfo,
+				    byCurrChannel,
+				    bChannelHit,
+				    sFrame.pSSID,
+				    sFrame.pSuppRates,
+				    sFrame.pExtSuppRates,
+				    &sERP,
+				    sFrame.pRSN,
+				    sFrame.pRSNWPA,
+				    sFrame.pIE_Country,
+				    sFrame.pIE_Quiet,
+				    pBSSList,
+				    sFrame.len - WLAN_HDR_ADDR3_LEN,
+				    sFrame.pHdr->sA4.abyAddr4,   // payload of probresponse
+				    (void *)pRxPacket
+);
+	} else {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Probe resp/insert: RxChannel = : %d\n", byCurrChannel);
+		BSSbInsertToBSSList((void *)pDevice,
+				    sFrame.pHdr->sA3.abyAddr3,
+				    *sFrame.pqwTimestamp,
+				    *sFrame.pwBeaconInterval,
+				    *sFrame.pwCapInfo,
+				    byCurrChannel,
+				    sFrame.pSSID,
+				    sFrame.pSuppRates,
+				    sFrame.pExtSuppRates,
+				    &sERP,
+				    sFrame.pRSN,
+				    sFrame.pRSNWPA,
+				    sFrame.pIE_Country,
+				    sFrame.pIE_Quiet,
+				    sFrame.len - WLAN_HDR_ADDR3_LEN,
+				    sFrame.pHdr->sA4.abyAddr4,   // payload of beacon
+				    (void *)pRxPacket
+);
+	}
+	return;
 }
 
 /*+
@@ -4412,84 +4206,77 @@ if(ChannelExceedZoneType(pDevice,byCurrChannel)==true)
  * Return Value:
  *    none.
  *
--*/
-
+ -*/
 
 static
 void
 s_vMgrRxProbeRequest(
-    PSDevice pDevice,
-    PSMgmtObject pMgmt,
-    PSRxMgmtPacket pRxPacket
-    )
+	PSDevice pDevice,
+	PSMgmtObject pMgmt,
+	PSRxMgmtPacket pRxPacket
+)
 {
-    WLAN_FR_PROBEREQ    sFrame;
-    CMD_STATUS          Status;
-    PSTxMgmtPacket      pTxPacket;
-    unsigned char byPHYType = BB_TYPE_11B;
-
-    // STA in Ad-hoc mode: when latest TBTT beacon transmit success,
-    // STA have to response this request.
-    if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) ||
-        ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) && pDevice->bBeaconSent)) {
-
-        memset(&sFrame, 0, sizeof(WLAN_FR_PROBEREQ));
-        // decode the frame
-        sFrame.len = pRxPacket->cbMPDULen;
-        sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
-        vMgrDecodeProbeRequest(&sFrame);
+	WLAN_FR_PROBEREQ    sFrame;
+	CMD_STATUS          Status;
+	PSTxMgmtPacket      pTxPacket;
+	unsigned char byPHYType = BB_TYPE_11B;
+
+	// STA in Ad-hoc mode: when latest TBTT beacon transmit success,
+	// STA have to response this request.
+	if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) ||
+	    ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) && pDevice->bBeaconSent)) {
+		memset(&sFrame, 0, sizeof(WLAN_FR_PROBEREQ));
+		// decode the frame
+		sFrame.len = pRxPacket->cbMPDULen;
+		sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
+		vMgrDecodeProbeRequest(&sFrame);
 /*
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Probe request rx:MAC addr:%pM\n",
-		sFrame.pHdr->sA3.abyAddr2);
+  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Probe request rx:MAC addr:%pM\n",
+  sFrame.pHdr->sA3.abyAddr2);
 */
-        if (sFrame.pSSID->len != 0) {
-            if (sFrame.pSSID->len != ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len)
-                return;
-            if (memcmp(sFrame.pSSID->abySSID,
-                       ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->abySSID,
-                       ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len) != 0) {
-                       return;
-            }
-        }
-
-        if ((sFrame.pSuppRates->len > 4) || (sFrame.pExtSuppRates != NULL)) {
-            byPHYType = BB_TYPE_11G;
-        }
-
-        // Probe response reply..
-        pTxPacket = s_MgrMakeProbeResponse
-                    (
-                      pDevice,
-                      pMgmt,
-                      pMgmt->wCurrCapInfo,
-                      pMgmt->wCurrBeaconPeriod,
-                      pMgmt->uCurrChannel,
-                      0,
-                      sFrame.pHdr->sA3.abyAddr2,
-                      (PWLAN_IE_SSID)pMgmt->abyCurrSSID,
-                      (unsigned char *)pMgmt->abyCurrBSSID,
-                      (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-                      (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates,
-                       byPHYType
-                    );
-        if (pTxPacket != NULL ){
-            /* send the frame */
-            Status = csMgmt_xmit(pDevice, pTxPacket);
-            if (Status != CMD_STATUS_PENDING) {
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Probe response tx failed\n");
-            }
-            else {
-//                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Probe response tx sending..\n");
-            }
-        }
-    }
-
-    return;
-}
-
+		if (sFrame.pSSID->len != 0) {
+			if (sFrame.pSSID->len != ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len)
+				return;
+			if (memcmp(sFrame.pSSID->abySSID,
+				   ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->abySSID,
+				   ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len) != 0) {
+				return;
+			}
+		}
 
+		if ((sFrame.pSuppRates->len > 4) || (sFrame.pExtSuppRates != NULL)) {
+			byPHYType = BB_TYPE_11G;
+		}
 
+		// Probe response reply..
+		pTxPacket = s_MgrMakeProbeResponse
+			(
+				pDevice,
+				pMgmt,
+				pMgmt->wCurrCapInfo,
+				pMgmt->wCurrBeaconPeriod,
+				pMgmt->uCurrChannel,
+				0,
+				sFrame.pHdr->sA3.abyAddr2,
+				(PWLAN_IE_SSID)pMgmt->abyCurrSSID,
+				(unsigned char *)pMgmt->abyCurrBSSID,
+				(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
+				(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates,
+				byPHYType
+);
+		if (pTxPacket != NULL) {
+			/* send the frame */
+			Status = csMgmt_xmit(pDevice, pTxPacket);
+			if (Status != CMD_STATUS_PENDING) {
+				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Probe response tx failed\n");
+			} else {
+//                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Probe response tx sending..\n");
+			}
+		}
+	}
 
+	return;
+}
 
 /*+
  *
@@ -4503,146 +4290,139 @@ s_vMgrRxProbeRequest(
  * Return Value:
  *    none.
  *
--*/
-
+ -*/
 
 void
 vMgrRxManagePacket(
-    void *hDeviceContext,
-    PSMgmtObject pMgmt,
-    PSRxMgmtPacket pRxPacket
-     )
+	void *hDeviceContext,
+	PSMgmtObject pMgmt,
+	PSRxMgmtPacket pRxPacket
+)
 {
-    PSDevice    pDevice = (PSDevice)hDeviceContext;
-    bool bInScan = false;
-    unsigned int uNodeIndex = 0;
-    NODE_STATE  eNodeState = 0;
-    CMD_STATUS  Status;
-
-
-    if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-        if (BSSDBbIsSTAInNodeDB(pMgmt, pRxPacket->p80211Header->sA3.abyAddr2, &uNodeIndex))
-            eNodeState = pMgmt->sNodeDBTable[uNodeIndex].eNodeState;
-    }
-
-    switch( WLAN_GET_FC_FSTYPE((pRxPacket->p80211Header->sA3.wFrameCtl)) ){
-
-        case WLAN_FSTYPE_ASSOCREQ:
-            // Frame Clase = 2
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx assocreq\n");
-            if (eNodeState < NODE_AUTH) {
-                // send deauth notification
-                // reason = (6) class 2 received from nonauth sta
-                vMgrDeAuthenBeginSta(pDevice,
-                                     pMgmt,
-                                     pRxPacket->p80211Header->sA3.abyAddr2,
-                                     (6),
-                                     &Status
-                                     );
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wmgr: send vMgrDeAuthenBeginSta 1\n");
-            }
-            else {
-                s_vMgrRxAssocRequest(pDevice, pMgmt, pRxPacket, uNodeIndex);
-            }
-            break;
-
-        case WLAN_FSTYPE_ASSOCRESP:
-            // Frame Clase = 2
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx assocresp1\n");
-            s_vMgrRxAssocResponse(pDevice, pMgmt, pRxPacket, false);
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx assocresp2\n");
-            break;
-
-        case WLAN_FSTYPE_REASSOCREQ:
-            // Frame Clase = 2
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx reassocreq\n");
-            // Todo: reassoc
-            if (eNodeState < NODE_AUTH) {
-                // send deauth notification
-                // reason = (6) class 2 received from nonauth sta
-                vMgrDeAuthenBeginSta(pDevice,
-                                     pMgmt,
-                                     pRxPacket->p80211Header->sA3.abyAddr2,
-                                     (6),
-                                     &Status
-                                     );
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wmgr: send vMgrDeAuthenBeginSta 2\n");
-
-            }
-            s_vMgrRxReAssocRequest(pDevice, pMgmt, pRxPacket, uNodeIndex);
-            break;
-
-        case WLAN_FSTYPE_REASSOCRESP:
-            // Frame Clase = 2
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx reassocresp\n");
-            s_vMgrRxAssocResponse(pDevice, pMgmt, pRxPacket, true);
-            break;
-
-        case WLAN_FSTYPE_PROBEREQ:
-            // Frame Clase = 0
-            //DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx probereq\n");
-            s_vMgrRxProbeRequest(pDevice, pMgmt, pRxPacket);
-            break;
-
-        case WLAN_FSTYPE_PROBERESP:
-            // Frame Clase = 0
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx proberesp\n");
-
-            s_vMgrRxProbeResponse(pDevice, pMgmt, pRxPacket);
-            break;
-
-        case WLAN_FSTYPE_BEACON:
-            // Frame Clase = 0
-            //DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx beacon\n");
-            if (pMgmt->eScanState != WMAC_NO_SCANNING) {
-                bInScan = true;
-            }
-            s_vMgrRxBeacon(pDevice, pMgmt, pRxPacket, bInScan);
-            break;
-
-        case WLAN_FSTYPE_ATIM:
-            // Frame Clase = 1
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx atim\n");
-            break;
-
-        case WLAN_FSTYPE_DISASSOC:
-            // Frame Clase = 2
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx disassoc\n");
-            if (eNodeState < NODE_AUTH) {
-                // send deauth notification
-                // reason = (6) class 2 received from nonauth sta
-                vMgrDeAuthenBeginSta(pDevice,
-                                     pMgmt,
-                                     pRxPacket->p80211Header->sA3.abyAddr2,
-                                     (6),
-                                     &Status
-                                     );
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wmgr: send vMgrDeAuthenBeginSta 3\n");
-            }
-            s_vMgrRxDisassociation(pDevice, pMgmt, pRxPacket);
-            break;
-
-        case WLAN_FSTYPE_AUTHEN:
-            // Frame Clase = 1
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO  "rx authen\n");
-            s_vMgrRxAuthentication(pDevice, pMgmt, pRxPacket);
-            break;
-
-        case WLAN_FSTYPE_DEAUTHEN:
-            // Frame Clase = 1
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx deauthen\n");
-            s_vMgrRxDeauthentication(pDevice, pMgmt, pRxPacket);
-            break;
-
-        default:
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx unknown mgmt\n");
-    }
-
-    return;
-}
+	PSDevice    pDevice = (PSDevice)hDeviceContext;
+	bool bInScan = false;
+	unsigned int uNodeIndex = 0;
+	NODE_STATE  eNodeState = 0;
+	CMD_STATUS  Status;
+
+	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
+		if (BSSDBbIsSTAInNodeDB(pMgmt, pRxPacket->p80211Header->sA3.abyAddr2, &uNodeIndex))
+			eNodeState = pMgmt->sNodeDBTable[uNodeIndex].eNodeState;
+	}
+
+	switch (WLAN_GET_FC_FSTYPE((pRxPacket->p80211Header->sA3.wFrameCtl))) {
+	case WLAN_FSTYPE_ASSOCREQ:
+		// Frame Clase = 2
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx assocreq\n");
+		if (eNodeState < NODE_AUTH) {
+			// send deauth notification
+			// reason = (6) class 2 received from nonauth sta
+			vMgrDeAuthenBeginSta(pDevice,
+					     pMgmt,
+					     pRxPacket->p80211Header->sA3.abyAddr2,
+					     (6),
+					     &Status
+);
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wmgr: send vMgrDeAuthenBeginSta 1\n");
+		} else {
+			s_vMgrRxAssocRequest(pDevice, pMgmt, pRxPacket, uNodeIndex);
+		}
+		break;
+
+	case WLAN_FSTYPE_ASSOCRESP:
+		// Frame Clase = 2
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx assocresp1\n");
+		s_vMgrRxAssocResponse(pDevice, pMgmt, pRxPacket, false);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx assocresp2\n");
+		break;
+
+	case WLAN_FSTYPE_REASSOCREQ:
+		// Frame Clase = 2
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx reassocreq\n");
+		// Todo: reassoc
+		if (eNodeState < NODE_AUTH) {
+			// send deauth notification
+			// reason = (6) class 2 received from nonauth sta
+			vMgrDeAuthenBeginSta(pDevice,
+					     pMgmt,
+					     pRxPacket->p80211Header->sA3.abyAddr2,
+					     (6),
+					     &Status
+);
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wmgr: send vMgrDeAuthenBeginSta 2\n");
+
+		}
+		s_vMgrRxReAssocRequest(pDevice, pMgmt, pRxPacket, uNodeIndex);
+		break;
+
+	case WLAN_FSTYPE_REASSOCRESP:
+		// Frame Clase = 2
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx reassocresp\n");
+		s_vMgrRxAssocResponse(pDevice, pMgmt, pRxPacket, true);
+		break;
+
+	case WLAN_FSTYPE_PROBEREQ:
+		// Frame Clase = 0
+		//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx probereq\n");
+		s_vMgrRxProbeRequest(pDevice, pMgmt, pRxPacket);
+		break;
+
+	case WLAN_FSTYPE_PROBERESP:
+		// Frame Clase = 0
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx proberesp\n");
+
+		s_vMgrRxProbeResponse(pDevice, pMgmt, pRxPacket);
+		break;
 
+	case WLAN_FSTYPE_BEACON:
+		// Frame Clase = 0
+		//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx beacon\n");
+		if (pMgmt->eScanState != WMAC_NO_SCANNING) {
+			bInScan = true;
+		}
+		s_vMgrRxBeacon(pDevice, pMgmt, pRxPacket, bInScan);
+		break;
+
+	case WLAN_FSTYPE_ATIM:
+		// Frame Clase = 1
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx atim\n");
+		break;
 
+	case WLAN_FSTYPE_DISASSOC:
+		// Frame Clase = 2
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx disassoc\n");
+		if (eNodeState < NODE_AUTH) {
+			// send deauth notification
+			// reason = (6) class 2 received from nonauth sta
+			vMgrDeAuthenBeginSta(pDevice,
+					     pMgmt,
+					     pRxPacket->p80211Header->sA3.abyAddr2,
+					     (6),
+					     &Status
+);
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wmgr: send vMgrDeAuthenBeginSta 3\n");
+		}
+		s_vMgrRxDisassociation(pDevice, pMgmt, pRxPacket);
+		break;
 
+	case WLAN_FSTYPE_AUTHEN:
+		// Frame Clase = 1
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO  "rx authen\n");
+		s_vMgrRxAuthentication(pDevice, pMgmt, pRxPacket);
+		break;
+
+	case WLAN_FSTYPE_DEAUTHEN:
+		// Frame Clase = 1
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx deauthen\n");
+		s_vMgrRxDeauthentication(pDevice, pMgmt, pRxPacket);
+		break;
+
+	default:
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx unknown mgmt\n");
+	}
+
+	return;
+}
 
 /*+
  *
@@ -4654,49 +4434,45 @@ vMgrRxManagePacket(
  * Return Value:
  *    true if success; false if failed.
  *
--*/
+ -*/
 bool
 bMgrPrepareBeaconToSend(
-    void *hDeviceContext,
-    PSMgmtObject pMgmt
-    )
+	void *hDeviceContext,
+	PSMgmtObject pMgmt
+)
 {
-    PSDevice            pDevice = (PSDevice)hDeviceContext;
-    PSTxMgmtPacket      pTxPacket;
+	PSDevice            pDevice = (PSDevice)hDeviceContext;
+	PSTxMgmtPacket      pTxPacket;
 
 //    pDevice->bBeaconBufReady = false;
-    if (pDevice->bEncryptionEnable || pDevice->bEnable8021x){
-        pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_PRIVACY(1);
-    }
-    else {
-        pMgmt->wCurrCapInfo &= ~WLAN_SET_CAP_INFO_PRIVACY(1);
-    }
-    pTxPacket = s_MgrMakeBeacon
-                (
-                  pDevice,
-                  pMgmt,
-                  pMgmt->wCurrCapInfo,
-                  pMgmt->wCurrBeaconPeriod,
-                  pMgmt->uCurrChannel,
-                  pMgmt->wCurrATIMWindow, //0,
-                  (PWLAN_IE_SSID)pMgmt->abyCurrSSID,
-                  (unsigned char *)pMgmt->abyCurrBSSID,
-                  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-                  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates
-                );
-
-    if ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) &&
-        (pMgmt->abyCurrBSSID[0] == 0))
-        return false;
-
-    csBeacon_xmit(pDevice, pTxPacket);
-
-    return true;
+	if (pDevice->bEncryptionEnable || pDevice->bEnable8021x) {
+		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_PRIVACY(1);
+	} else {
+		pMgmt->wCurrCapInfo &= ~WLAN_SET_CAP_INFO_PRIVACY(1);
+	}
+	pTxPacket = s_MgrMakeBeacon
+		(
+			pDevice,
+			pMgmt,
+			pMgmt->wCurrCapInfo,
+			pMgmt->wCurrBeaconPeriod,
+			pMgmt->uCurrChannel,
+			pMgmt->wCurrATIMWindow, //0,
+			(PWLAN_IE_SSID)pMgmt->abyCurrSSID,
+			(unsigned char *)pMgmt->abyCurrBSSID,
+			(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
+			(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates
+);
+
+	if ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) &&
+	    (pMgmt->abyCurrBSSID[0] == 0))
+		return false;
+
+	csBeacon_xmit(pDevice, pTxPacket);
+
+	return true;
 }
 
-
-
-
 /*+
  *
  * Routine Description:
@@ -4708,61 +4484,60 @@ bMgrPrepareBeaconToSend(
  * Return Value:
  *    none.
  *
--*/
+ -*/
 static
 void
 s_vMgrLogStatus(
-    PSMgmtObject pMgmt,
-    unsigned short wStatus
-    )
+	PSMgmtObject pMgmt,
+	unsigned short wStatus
+)
 {
-    switch( wStatus ){
-        case WLAN_MGMT_STATUS_UNSPEC_FAILURE:
-            DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Unspecified error.\n");
-            break;
-        case WLAN_MGMT_STATUS_CAPS_UNSUPPORTED:
-            DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Can't support all requested capabilities.\n");
-            break;
-        case WLAN_MGMT_STATUS_REASSOC_NO_ASSOC:
-            DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Reassoc denied, can't confirm original Association.\n");
-            break;
-        case WLAN_MGMT_STATUS_ASSOC_DENIED_UNSPEC:
-            DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Assoc denied, undefine in spec\n");
-            break;
-        case WLAN_MGMT_STATUS_UNSUPPORTED_AUTHALG:
-            DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Peer doesn't support authen algorithm.\n");
-            break;
-        case WLAN_MGMT_STATUS_RX_AUTH_NOSEQ:
-            DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Authen frame received out of sequence.\n");
-            break;
-        case WLAN_MGMT_STATUS_CHALLENGE_FAIL:
-            DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Authen rejected, challenge  failure.\n");
-            break;
-        case WLAN_MGMT_STATUS_AUTH_TIMEOUT:
-            DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Authen rejected, timeout waiting for next frame.\n");
-            break;
-        case WLAN_MGMT_STATUS_ASSOC_DENIED_BUSY:
-            DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Assoc denied, AP too busy.\n");
-            break;
-        case WLAN_MGMT_STATUS_ASSOC_DENIED_RATES:
-            DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Assoc denied, we haven't enough basic rates.\n");
-            break;
-        case WLAN_MGMT_STATUS_ASSOC_DENIED_SHORTPREAMBLE:
-            DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Assoc denied, we do not support short preamble.\n");
-            break;
-        case WLAN_MGMT_STATUS_ASSOC_DENIED_PBCC:
-            DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Assoc denied, we do not support PBCC.\n");
-            break;
-        case WLAN_MGMT_STATUS_ASSOC_DENIED_AGILITY:
-            DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Assoc denied, we do not support channel agility.\n");
-            break;
-        default:
-            DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Unknown status code %d.\n", wStatus);
-            break;
-    }
+	switch (wStatus) {
+	case WLAN_MGMT_STATUS_UNSPEC_FAILURE:
+		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Unspecified error.\n");
+		break;
+	case WLAN_MGMT_STATUS_CAPS_UNSUPPORTED:
+		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Can't support all requested capabilities.\n");
+		break;
+	case WLAN_MGMT_STATUS_REASSOC_NO_ASSOC:
+		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Reassoc denied, can't confirm original Association.\n");
+		break;
+	case WLAN_MGMT_STATUS_ASSOC_DENIED_UNSPEC:
+		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Assoc denied, undefine in spec\n");
+		break;
+	case WLAN_MGMT_STATUS_UNSUPPORTED_AUTHALG:
+		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Peer doesn't support authen algorithm.\n");
+		break;
+	case WLAN_MGMT_STATUS_RX_AUTH_NOSEQ:
+		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Authen frame received out of sequence.\n");
+		break;
+	case WLAN_MGMT_STATUS_CHALLENGE_FAIL:
+		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Authen rejected, challenge  failure.\n");
+		break;
+	case WLAN_MGMT_STATUS_AUTH_TIMEOUT:
+		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Authen rejected, timeout waiting for next frame.\n");
+		break;
+	case WLAN_MGMT_STATUS_ASSOC_DENIED_BUSY:
+		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Assoc denied, AP too busy.\n");
+		break;
+	case WLAN_MGMT_STATUS_ASSOC_DENIED_RATES:
+		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Assoc denied, we haven't enough basic rates.\n");
+		break;
+	case WLAN_MGMT_STATUS_ASSOC_DENIED_SHORTPREAMBLE:
+		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Assoc denied, we do not support short preamble.\n");
+		break;
+	case WLAN_MGMT_STATUS_ASSOC_DENIED_PBCC:
+		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Assoc denied, we do not support PBCC.\n");
+		break;
+	case WLAN_MGMT_STATUS_ASSOC_DENIED_AGILITY:
+		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Assoc denied, we do not support channel agility.\n");
+		break;
+	default:
+		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Unknown status code %d.\n", wStatus);
+		break;
+	}
 }
 
-
 /*
  *
  * Description:
@@ -4778,52 +4553,50 @@ s_vMgrLogStatus(
  *
  * Return Value: none.
  *
--*/
+ -*/
 bool
-bAdd_PMKID_Candidate (
-    void *hDeviceContext,
-    unsigned char *pbyBSSID,
-    PSRSNCapObject psRSNCapObj
-    )
+bAdd_PMKID_Candidate(
+	void *hDeviceContext,
+	unsigned char *pbyBSSID,
+	PSRSNCapObject psRSNCapObj
+)
 {
-    PSDevice         pDevice = (PSDevice)hDeviceContext;
-    PPMKID_CANDIDATE pCandidateList;
-    unsigned int ii = 0;
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"bAdd_PMKID_Candidate START: (%d)\n", (int)pDevice->gsPMKIDCandidate.NumCandidates);
-
-    if ((pDevice == NULL) || (pbyBSSID == NULL) || (psRSNCapObj == NULL))
-        return false;
-
-    if (pDevice->gsPMKIDCandidate.NumCandidates >= MAX_PMKIDLIST)
-        return false;
-
-
-
-    // Update Old Candidate
-    for (ii = 0; ii < pDevice->gsPMKIDCandidate.NumCandidates; ii++) {
-        pCandidateList = &pDevice->gsPMKIDCandidate.CandidateList[ii];
-        if ( !memcmp(pCandidateList->BSSID, pbyBSSID, ETH_ALEN)) {
-            if ((psRSNCapObj->bRSNCapExist == true) && (psRSNCapObj->wRSNCap & BIT0)) {
-                pCandidateList->Flags |= NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED;
-            } else {
-                pCandidateList->Flags &= ~(NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED);
-            }
-            return true;
-        }
-    }
-
-    // New Candidate
-    pCandidateList = &pDevice->gsPMKIDCandidate.CandidateList[pDevice->gsPMKIDCandidate.NumCandidates];
-    if ((psRSNCapObj->bRSNCapExist == true) && (psRSNCapObj->wRSNCap & BIT0)) {
-        pCandidateList->Flags |= NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED;
-    } else {
-        pCandidateList->Flags &= ~(NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED);
-    }
-    memcpy(pCandidateList->BSSID, pbyBSSID, ETH_ALEN);
-    pDevice->gsPMKIDCandidate.NumCandidates++;
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"NumCandidates:%d\n", (int)pDevice->gsPMKIDCandidate.NumCandidates);
-    return true;
+	PSDevice         pDevice = (PSDevice)hDeviceContext;
+	PPMKID_CANDIDATE pCandidateList;
+	unsigned int ii = 0;
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "bAdd_PMKID_Candidate START: (%d)\n", (int)pDevice->gsPMKIDCandidate.NumCandidates);
+
+	if ((pDevice == NULL) || (pbyBSSID == NULL) || (psRSNCapObj == NULL))
+		return false;
+
+	if (pDevice->gsPMKIDCandidate.NumCandidates >= MAX_PMKIDLIST)
+		return false;
+
+	// Update Old Candidate
+	for (ii = 0; ii < pDevice->gsPMKIDCandidate.NumCandidates; ii++) {
+		pCandidateList = &pDevice->gsPMKIDCandidate.CandidateList[ii];
+		if (!memcmp(pCandidateList->BSSID, pbyBSSID, ETH_ALEN)) {
+			if ((psRSNCapObj->bRSNCapExist == true) && (psRSNCapObj->wRSNCap & BIT0)) {
+				pCandidateList->Flags |= NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED;
+			} else {
+				pCandidateList->Flags &= ~(NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED);
+			}
+			return true;
+		}
+	}
+
+	// New Candidate
+	pCandidateList = &pDevice->gsPMKIDCandidate.CandidateList[pDevice->gsPMKIDCandidate.NumCandidates];
+	if ((psRSNCapObj->bRSNCapExist == true) && (psRSNCapObj->wRSNCap & BIT0)) {
+		pCandidateList->Flags |= NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED;
+	} else {
+		pCandidateList->Flags &= ~(NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED);
+	}
+	memcpy(pCandidateList->BSSID, pbyBSSID, ETH_ALEN);
+	pDevice->gsPMKIDCandidate.NumCandidates++;
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "NumCandidates:%d\n", (int)pDevice->gsPMKIDCandidate.NumCandidates);
+	return true;
 }
 
 /*
@@ -4839,166 +4612,163 @@ bAdd_PMKID_Candidate (
  *
  * Return Value: none.
  *
--*/
+ -*/
 void
-vFlush_PMKID_Candidate (
-    void *hDeviceContext
-    )
+vFlush_PMKID_Candidate(
+	void *hDeviceContext
+)
 {
-    PSDevice        pDevice = (PSDevice)hDeviceContext;
+	PSDevice        pDevice = (PSDevice)hDeviceContext;
 
-    if (pDevice == NULL)
-        return;
+	if (pDevice == NULL)
+		return;
 
-    memset(&pDevice->gsPMKIDCandidate, 0, sizeof(SPMKIDCandidateEvent));
+	memset(&pDevice->gsPMKIDCandidate, 0, sizeof(SPMKIDCandidateEvent));
 }
 
 static bool
-s_bCipherMatch (
-    PKnownBSS                        pBSSNode,
-    NDIS_802_11_ENCRYPTION_STATUS    EncStatus,
-    unsigned char *pbyCCSPK,
-    unsigned char *pbyCCSGK
-    )
+s_bCipherMatch(
+	PKnownBSS                        pBSSNode,
+	NDIS_802_11_ENCRYPTION_STATUS    EncStatus,
+	unsigned char *pbyCCSPK,
+	unsigned char *pbyCCSGK
+)
 {
-    unsigned char byMulticastCipher = KEY_CTL_INVALID;
-    unsigned char byCipherMask = 0x00;
-    int i;
-
-    if (pBSSNode == NULL)
-        return false;
-
-    // check cap. of BSS
-    if ((WLAN_GET_CAP_INFO_PRIVACY(pBSSNode->wCapInfo) != 0) &&
-         (EncStatus == Ndis802_11Encryption1Enabled)) {
-        // default is WEP only
-        byMulticastCipher = KEY_CTL_WEP;
-    }
-
-    if ((WLAN_GET_CAP_INFO_PRIVACY(pBSSNode->wCapInfo) != 0) &&
-        (pBSSNode->bWPA2Valid == true) &&
-          //20080123-01,<Add> by Einsn Liu
-        ((EncStatus == Ndis802_11Encryption3Enabled)||(EncStatus == Ndis802_11Encryption2Enabled))) {
-        //WPA2
-        // check Group Key Cipher
-        if ((pBSSNode->byCSSGK == WLAN_11i_CSS_WEP40) ||
-            (pBSSNode->byCSSGK == WLAN_11i_CSS_WEP104)) {
-            byMulticastCipher = KEY_CTL_WEP;
-        } else if (pBSSNode->byCSSGK == WLAN_11i_CSS_TKIP) {
-            byMulticastCipher = KEY_CTL_TKIP;
-        } else if (pBSSNode->byCSSGK == WLAN_11i_CSS_CCMP) {
-            byMulticastCipher = KEY_CTL_CCMP;
-        } else {
-            byMulticastCipher = KEY_CTL_INVALID;
-        }
-
-        // check Pairwise Key Cipher
-        for(i=0;i<pBSSNode->wCSSPKCount;i++) {
-            if ((pBSSNode->abyCSSPK[i] == WLAN_11i_CSS_WEP40) ||
-                (pBSSNode->abyCSSPK[i] == WLAN_11i_CSS_WEP104)) {
-                // this should not happen as defined 802.11i
-                byCipherMask |= 0x01;
-            } else if (pBSSNode->abyCSSPK[i] == WLAN_11i_CSS_TKIP) {
-                byCipherMask |= 0x02;
-            } else if (pBSSNode->abyCSSPK[i] == WLAN_11i_CSS_CCMP) {
-                byCipherMask |= 0x04;
-            } else if (pBSSNode->abyCSSPK[i] == WLAN_11i_CSS_USE_GROUP) {
-                // use group key only ignore all others
-                byCipherMask = 0;
-                i = pBSSNode->wCSSPKCount;
-            }
-        }
-
-    } else if ((WLAN_GET_CAP_INFO_PRIVACY(pBSSNode->wCapInfo) != 0) &&
-                (pBSSNode->bWPAValid == true) &&
-                ((EncStatus == Ndis802_11Encryption3Enabled) || (EncStatus == Ndis802_11Encryption2Enabled))) {
-        //WPA
-        // check Group Key Cipher
-        if ((pBSSNode->byGKType == WPA_WEP40) ||
-            (pBSSNode->byGKType == WPA_WEP104)) {
-            byMulticastCipher = KEY_CTL_WEP;
-        } else if (pBSSNode->byGKType == WPA_TKIP) {
-            byMulticastCipher = KEY_CTL_TKIP;
-        } else if (pBSSNode->byGKType == WPA_AESCCMP) {
-            byMulticastCipher = KEY_CTL_CCMP;
-        } else {
-            byMulticastCipher = KEY_CTL_INVALID;
-        }
-
-        // check Pairwise Key Cipher
-        for(i=0;i<pBSSNode->wPKCount;i++) {
-            if (pBSSNode->abyPKType[i] == WPA_TKIP) {
-                byCipherMask |= 0x02;
-            } else if (pBSSNode->abyPKType[i] == WPA_AESCCMP) {
-                byCipherMask |= 0x04;
-            } else if (pBSSNode->abyPKType[i] == WPA_NONE) {
-                // use group key only ignore all others
-                byCipherMask = 0;
-                i = pBSSNode->wPKCount;
-            }
-        }
-    }
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"%d, %d, %d, %d, EncStatus:%d\n",
-        byMulticastCipher, byCipherMask, pBSSNode->bWPAValid, pBSSNode->bWPA2Valid, EncStatus);
-
-    // mask our cap. with BSS
-    if (EncStatus == Ndis802_11Encryption1Enabled) {
-
-        // For supporting Cisco migration mode, don't care pairwise key cipher
-        if ((byMulticastCipher == KEY_CTL_WEP) &&
-            (byCipherMask == 0)) {
-            *pbyCCSGK = KEY_CTL_WEP;
-            *pbyCCSPK = KEY_CTL_NONE;
-            return true;
-        } else {
-            return false;
-        }
-
-    } else if (EncStatus == Ndis802_11Encryption2Enabled) {
-        if ((byMulticastCipher == KEY_CTL_TKIP) &&
-            (byCipherMask == 0)) {
-            *pbyCCSGK = KEY_CTL_TKIP;
-            *pbyCCSPK = KEY_CTL_NONE;
-            return true;
-        } else if ((byMulticastCipher == KEY_CTL_WEP) &&
-                   ((byCipherMask & 0x02) != 0)) {
-            *pbyCCSGK = KEY_CTL_WEP;
-            *pbyCCSPK = KEY_CTL_TKIP;
-            return true;
-        } else if ((byMulticastCipher == KEY_CTL_TKIP) &&
-                   ((byCipherMask & 0x02) != 0)) {
-            *pbyCCSGK = KEY_CTL_TKIP;
-            *pbyCCSPK = KEY_CTL_TKIP;
-            return true;
-        } else {
-            return false;
-        }
-    } else if (EncStatus == Ndis802_11Encryption3Enabled) {
-        if ((byMulticastCipher == KEY_CTL_CCMP) &&
-            (byCipherMask == 0)) {
-            // When CCMP is enable, "Use group cipher suite" shall not be a valid option.
-            return false;
-        } else if ((byMulticastCipher == KEY_CTL_WEP) &&
-                   ((byCipherMask & 0x04) != 0)) {
-            *pbyCCSGK = KEY_CTL_WEP;
-            *pbyCCSPK = KEY_CTL_CCMP;
-            return true;
-        } else if ((byMulticastCipher == KEY_CTL_TKIP) &&
-                   ((byCipherMask & 0x04) != 0)) {
-            *pbyCCSGK = KEY_CTL_TKIP;
-            *pbyCCSPK = KEY_CTL_CCMP;
-            return true;
-        } else if ((byMulticastCipher == KEY_CTL_CCMP) &&
-                   ((byCipherMask & 0x04) != 0)) {
-            *pbyCCSGK = KEY_CTL_CCMP;
-            *pbyCCSPK = KEY_CTL_CCMP;
-            return true;
-        } else {
-            return false;
-        }
-    }
-    return true;
-}
+	unsigned char byMulticastCipher = KEY_CTL_INVALID;
+	unsigned char byCipherMask = 0x00;
+	int i;
+
+	if (pBSSNode == NULL)
+		return false;
+
+	// check cap. of BSS
+	if ((WLAN_GET_CAP_INFO_PRIVACY(pBSSNode->wCapInfo) != 0) &&
+	    (EncStatus == Ndis802_11Encryption1Enabled)) {
+		// default is WEP only
+		byMulticastCipher = KEY_CTL_WEP;
+	}
+
+	if ((WLAN_GET_CAP_INFO_PRIVACY(pBSSNode->wCapInfo) != 0) &&
+	    (pBSSNode->bWPA2Valid == true) &&
+	    //20080123-01,<Add> by Einsn Liu
+	    ((EncStatus == Ndis802_11Encryption3Enabled) || (EncStatus == Ndis802_11Encryption2Enabled))) {
+		//WPA2
+		// check Group Key Cipher
+		if ((pBSSNode->byCSSGK == WLAN_11i_CSS_WEP40) ||
+		    (pBSSNode->byCSSGK == WLAN_11i_CSS_WEP104)) {
+			byMulticastCipher = KEY_CTL_WEP;
+		} else if (pBSSNode->byCSSGK == WLAN_11i_CSS_TKIP) {
+			byMulticastCipher = KEY_CTL_TKIP;
+		} else if (pBSSNode->byCSSGK == WLAN_11i_CSS_CCMP) {
+			byMulticastCipher = KEY_CTL_CCMP;
+		} else {
+			byMulticastCipher = KEY_CTL_INVALID;
+		}
+
+		// check Pairwise Key Cipher
+		for (i = 0; i < pBSSNode->wCSSPKCount; i++) {
+			if ((pBSSNode->abyCSSPK[i] == WLAN_11i_CSS_WEP40) ||
+			    (pBSSNode->abyCSSPK[i] == WLAN_11i_CSS_WEP104)) {
+				// this should not happen as defined 802.11i
+				byCipherMask |= 0x01;
+			} else if (pBSSNode->abyCSSPK[i] == WLAN_11i_CSS_TKIP) {
+				byCipherMask |= 0x02;
+			} else if (pBSSNode->abyCSSPK[i] == WLAN_11i_CSS_CCMP) {
+				byCipherMask |= 0x04;
+			} else if (pBSSNode->abyCSSPK[i] == WLAN_11i_CSS_USE_GROUP) {
+				// use group key only ignore all others
+				byCipherMask = 0;
+				i = pBSSNode->wCSSPKCount;
+			}
+		}
 
+	} else if ((WLAN_GET_CAP_INFO_PRIVACY(pBSSNode->wCapInfo) != 0) &&
+		   (pBSSNode->bWPAValid == true) &&
+		   ((EncStatus == Ndis802_11Encryption3Enabled) || (EncStatus == Ndis802_11Encryption2Enabled))) {
+		//WPA
+		// check Group Key Cipher
+		if ((pBSSNode->byGKType == WPA_WEP40) ||
+		    (pBSSNode->byGKType == WPA_WEP104)) {
+			byMulticastCipher = KEY_CTL_WEP;
+		} else if (pBSSNode->byGKType == WPA_TKIP) {
+			byMulticastCipher = KEY_CTL_TKIP;
+		} else if (pBSSNode->byGKType == WPA_AESCCMP) {
+			byMulticastCipher = KEY_CTL_CCMP;
+		} else {
+			byMulticastCipher = KEY_CTL_INVALID;
+		}
 
+		// check Pairwise Key Cipher
+		for (i = 0; i < pBSSNode->wPKCount; i++) {
+			if (pBSSNode->abyPKType[i] == WPA_TKIP) {
+				byCipherMask |= 0x02;
+			} else if (pBSSNode->abyPKType[i] == WPA_AESCCMP) {
+				byCipherMask |= 0x04;
+			} else if (pBSSNode->abyPKType[i] == WPA_NONE) {
+				// use group key only ignore all others
+				byCipherMask = 0;
+				i = pBSSNode->wPKCount;
+			}
+		}
+	}
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%d, %d, %d, %d, EncStatus:%d\n",
+		byMulticastCipher, byCipherMask, pBSSNode->bWPAValid, pBSSNode->bWPA2Valid, EncStatus);
+
+	// mask our cap. with BSS
+	if (EncStatus == Ndis802_11Encryption1Enabled) {
+		// For supporting Cisco migration mode, don't care pairwise key cipher
+		if ((byMulticastCipher == KEY_CTL_WEP) &&
+		    (byCipherMask == 0)) {
+			*pbyCCSGK = KEY_CTL_WEP;
+			*pbyCCSPK = KEY_CTL_NONE;
+			return true;
+		} else {
+			return false;
+		}
+
+	} else if (EncStatus == Ndis802_11Encryption2Enabled) {
+		if ((byMulticastCipher == KEY_CTL_TKIP) &&
+		    (byCipherMask == 0)) {
+			*pbyCCSGK = KEY_CTL_TKIP;
+			*pbyCCSPK = KEY_CTL_NONE;
+			return true;
+		} else if ((byMulticastCipher == KEY_CTL_WEP) &&
+			   ((byCipherMask & 0x02) != 0)) {
+			*pbyCCSGK = KEY_CTL_WEP;
+			*pbyCCSPK = KEY_CTL_TKIP;
+			return true;
+		} else if ((byMulticastCipher == KEY_CTL_TKIP) &&
+			   ((byCipherMask & 0x02) != 0)) {
+			*pbyCCSGK = KEY_CTL_TKIP;
+			*pbyCCSPK = KEY_CTL_TKIP;
+			return true;
+		} else {
+			return false;
+		}
+	} else if (EncStatus == Ndis802_11Encryption3Enabled) {
+		if ((byMulticastCipher == KEY_CTL_CCMP) &&
+		    (byCipherMask == 0)) {
+			// When CCMP is enable, "Use group cipher suite" shall not be a valid option.
+			return false;
+		} else if ((byMulticastCipher == KEY_CTL_WEP) &&
+			   ((byCipherMask & 0x04) != 0)) {
+			*pbyCCSGK = KEY_CTL_WEP;
+			*pbyCCSPK = KEY_CTL_CCMP;
+			return true;
+		} else if ((byMulticastCipher == KEY_CTL_TKIP) &&
+			   ((byCipherMask & 0x04) != 0)) {
+			*pbyCCSGK = KEY_CTL_TKIP;
+			*pbyCCSPK = KEY_CTL_CCMP;
+			return true;
+		} else if ((byMulticastCipher == KEY_CTL_CCMP) &&
+			   ((byCipherMask & 0x04) != 0)) {
+			*pbyCCSGK = KEY_CTL_CCMP;
+			*pbyCCSPK = KEY_CTL_CCMP;
+			return true;
+		} else {
+			return false;
+		}
+	}
+	return true;
+}
diff --git a/drivers/staging/vt6655/wmgr.h b/drivers/staging/vt6655/wmgr.h
index bfa67ae5d40..b91f1f89ed0 100644
--- a/drivers/staging/vt6655/wmgr.h
+++ b/drivers/staging/vt6655/wmgr.h
@@ -45,8 +45,6 @@
 
 /*---------------------  Export Definitions -------------------------*/
 
-
-
 // Scan time
 #define PROBE_DELAY                  100  // (us)
 #define SWITCH_CHANNEL_DELAY         200 // (us)
@@ -58,7 +56,6 @@
 #define WCMD_ACTIVE_SCAN_TIME   50 //(ms)
 #define WCMD_PASSIVE_SCAN_TIME  100 //(ms)
 
-
 #define DEFAULT_MSDU_LIFETIME           512  // ms
 #define DEFAULT_MSDU_LIFETIME_RES_64us  8000 // 64us
 
@@ -67,7 +64,6 @@
 
 #define MAKE_BEACON_RESERVED            10  //(us)
 
-
 #define TIM_MULTICAST_MASK           0x01
 #define TIM_BITMAPOFFSET_MASK        0xFE
 #define DEFAULT_DTIM_PERIOD             1
@@ -76,107 +72,91 @@
 
 #define DEFAULT_IBSS_CHANNEL            6  //2.4G
 
-
 /*---------------------  Export Classes  ----------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
 
 /*---------------------  Export Types  ------------------------------*/
-#define timer_expire(timer,next_tick)   mod_timer(&timer, RUN_AT(next_tick))
+#define timer_expire(timer, next_tick)   mod_timer(&timer, RUN_AT(next_tick))
 typedef void (*TimerFunction)(unsigned long);
 
-
 //+++ NDIS related
 
 typedef unsigned char NDIS_802_11_MAC_ADDRESS[6];
 typedef struct _NDIS_802_11_AI_REQFI
 {
-    unsigned short Capabilities;
-    unsigned short ListenInterval;
-    NDIS_802_11_MAC_ADDRESS  CurrentAPAddress;
+	unsigned short Capabilities;
+	unsigned short ListenInterval;
+	NDIS_802_11_MAC_ADDRESS  CurrentAPAddress;
 } NDIS_802_11_AI_REQFI, *PNDIS_802_11_AI_REQFI;
 
 typedef struct _NDIS_802_11_AI_RESFI
 {
-    unsigned short Capabilities;
-    unsigned short StatusCode;
-    unsigned short AssociationId;
+	unsigned short Capabilities;
+	unsigned short StatusCode;
+	unsigned short AssociationId;
 } NDIS_802_11_AI_RESFI, *PNDIS_802_11_AI_RESFI;
 
 typedef struct _NDIS_802_11_ASSOCIATION_INFORMATION
 {
-    unsigned long Length;
-    unsigned short          AvailableRequestFixedIEs;
-    NDIS_802_11_AI_REQFI    RequestFixedIEs;
-    unsigned long RequestIELength;
-    unsigned long OffsetRequestIEs;
-    unsigned short          AvailableResponseFixedIEs;
-    NDIS_802_11_AI_RESFI    ResponseFixedIEs;
-    unsigned long ResponseIELength;
-    unsigned long OffsetResponseIEs;
+	unsigned long Length;
+	unsigned short          AvailableRequestFixedIEs;
+	NDIS_802_11_AI_REQFI    RequestFixedIEs;
+	unsigned long RequestIELength;
+	unsigned long OffsetRequestIEs;
+	unsigned short          AvailableResponseFixedIEs;
+	NDIS_802_11_AI_RESFI    ResponseFixedIEs;
+	unsigned long ResponseIELength;
+	unsigned long OffsetResponseIEs;
 } NDIS_802_11_ASSOCIATION_INFORMATION, *PNDIS_802_11_ASSOCIATION_INFORMATION;
 
-
-
 typedef struct tagSAssocInfo {
-    NDIS_802_11_ASSOCIATION_INFORMATION     AssocInfo;
-    unsigned char abyIEs[WLAN_BEACON_FR_MAXLEN+WLAN_BEACON_FR_MAXLEN];
-    // store ReqIEs set by OID_802_11_ASSOCIATION_INFORMATION
-    unsigned long RequestIELength;
-    unsigned char abyReqIEs[WLAN_BEACON_FR_MAXLEN];
+	NDIS_802_11_ASSOCIATION_INFORMATION     AssocInfo;
+	unsigned char abyIEs[WLAN_BEACON_FR_MAXLEN+WLAN_BEACON_FR_MAXLEN];
+	// store ReqIEs set by OID_802_11_ASSOCIATION_INFORMATION
+	unsigned long RequestIELength;
+	unsigned char abyReqIEs[WLAN_BEACON_FR_MAXLEN];
 } SAssocInfo, *PSAssocInfo;
 //---
 
-
 /*
-typedef enum tagWMAC_AUTHENTICATION_MODE {
-
-
-    WMAC_AUTH_OPEN,
-    WMAC_AUTH_SHAREKEY,
-    WMAC_AUTH_AUTO,
-    WMAC_AUTH_WPA,
-    WMAC_AUTH_WPAPSK,
-    WMAC_AUTH_WPANONE,
-    WMAC_AUTH_WPA2,
-    WMAC_AUTH_WPA2PSK,
-    WMAC_AUTH_MAX       // Not a real mode, defined as upper bound
-
-
-} WMAC_AUTHENTICATION_MODE, *PWMAC_AUTHENTICATION_MODE;
+  typedef enum tagWMAC_AUTHENTICATION_MODE {
+  WMAC_AUTH_OPEN,
+  WMAC_AUTH_SHAREKEY,
+  WMAC_AUTH_AUTO,
+  WMAC_AUTH_WPA,
+  WMAC_AUTH_WPAPSK,
+  WMAC_AUTH_WPANONE,
+  WMAC_AUTH_WPA2,
+  WMAC_AUTH_WPA2PSK,
+  WMAC_AUTH_MAX       // Not a real mode, defined as upper bound
+
+  } WMAC_AUTHENTICATION_MODE, *PWMAC_AUTHENTICATION_MODE;
 */
 
-
 // Pre-configured Mode (from XP)
 /*
-typedef enum tagWMAC_CONFIG_MODE {
-    WMAC_CONFIG_ESS_STA,
-    WMAC_CONFIG_IBSS_STA,
-    WMAC_CONFIG_AUTO,
-    WMAC_CONFIG_AP
+  typedef enum tagWMAC_CONFIG_MODE {
+  WMAC_CONFIG_ESS_STA,
+  WMAC_CONFIG_IBSS_STA,
+  WMAC_CONFIG_AUTO,
+  WMAC_CONFIG_AP
 
-} WMAC_CONFIG_MODE, *PWMAC_CONFIG_MODE;
+  } WMAC_CONFIG_MODE, *PWMAC_CONFIG_MODE;
 */
 
 typedef enum tagWMAC_SCAN_TYPE {
-
-    WMAC_SCAN_ACTIVE,
-    WMAC_SCAN_PASSIVE,
-    WMAC_SCAN_HYBRID
-
+	WMAC_SCAN_ACTIVE,
+	WMAC_SCAN_PASSIVE,
+	WMAC_SCAN_HYBRID
 } WMAC_SCAN_TYPE, *PWMAC_SCAN_TYPE;
 
-
 typedef enum tagWMAC_SCAN_STATE {
-
-    WMAC_NO_SCANNING,
-    WMAC_IS_SCANNING,
-    WMAC_IS_PROBEPENDING
-
+	WMAC_NO_SCANNING,
+	WMAC_IS_SCANNING,
+	WMAC_IS_PROBEPENDING
 } WMAC_SCAN_STATE, *PWMAC_SCAN_STATE;
 
-
-
 // Notes:
 // Basic Service Set state explained as following:
 // WMAC_STATE_IDLE          : no BSS is selected (Adhoc or Infra)
@@ -188,315 +168,287 @@ typedef enum tagWMAC_SCAN_STATE {
 // WMAC_STATE_ASSOC         : Associated (Infra)
 
 typedef enum tagWMAC_BSS_STATE {
-
-    WMAC_STATE_IDLE,
-    WMAC_STATE_STARTED,
-    WMAC_STATE_JOINTED,
-    WMAC_STATE_AUTHPENDING,
-    WMAC_STATE_AUTH,
-    WMAC_STATE_ASSOCPENDING,
-    WMAC_STATE_ASSOC
-
+	WMAC_STATE_IDLE,
+	WMAC_STATE_STARTED,
+	WMAC_STATE_JOINTED,
+	WMAC_STATE_AUTHPENDING,
+	WMAC_STATE_AUTH,
+	WMAC_STATE_ASSOCPENDING,
+	WMAC_STATE_ASSOC
 } WMAC_BSS_STATE, *PWMAC_BSS_STATE;
 
 // WMAC selected running mode
 typedef enum tagWMAC_CURRENT_MODE {
-
-    WMAC_MODE_STANDBY,
-    WMAC_MODE_ESS_STA,
-    WMAC_MODE_IBSS_STA,
-    WMAC_MODE_ESS_AP
-
+	WMAC_MODE_STANDBY,
+	WMAC_MODE_ESS_STA,
+	WMAC_MODE_IBSS_STA,
+	WMAC_MODE_ESS_AP
 } WMAC_CURRENT_MODE, *PWMAC_CURRENT_MODE;
 
 /*
-typedef enum tagWMAC_POWER_MODE {
+  typedef enum tagWMAC_POWER_MODE {
+  WMAC_POWER_CAM,
+  WMAC_POWER_FAST,
+  WMAC_POWER_MAX
 
-    WMAC_POWER_CAM,
-    WMAC_POWER_FAST,
-    WMAC_POWER_MAX
-
-} WMAC_POWER_MODE, *PWMAC_POWER_MODE;
+  } WMAC_POWER_MODE, *PWMAC_POWER_MODE;
 */
 
-
 // Tx Management Packet descriptor
 typedef struct tagSTxMgmtPacket {
-
-    PUWLAN_80211HDR     p80211Header;
-    unsigned int cbMPDULen;
-    unsigned int cbPayloadLen;
-
+	PUWLAN_80211HDR     p80211Header;
+	unsigned int cbMPDULen;
+	unsigned int cbPayloadLen;
 } STxMgmtPacket, *PSTxMgmtPacket;
 
-
 // Rx Management Packet descriptor
 typedef struct tagSRxMgmtPacket {
-
-    PUWLAN_80211HDR     p80211Header;
-    QWORD               qwLocalTSF;
-    unsigned int cbMPDULen;
-    unsigned int cbPayloadLen;
-    unsigned int uRSSI;
-    unsigned char bySQ;
-    unsigned char byRxRate;
-    unsigned char byRxChannel;
-
+	PUWLAN_80211HDR     p80211Header;
+	QWORD               qwLocalTSF;
+	unsigned int cbMPDULen;
+	unsigned int cbPayloadLen;
+	unsigned int uRSSI;
+	unsigned char bySQ;
+	unsigned char byRxRate;
+	unsigned char byRxChannel;
 } SRxMgmtPacket, *PSRxMgmtPacket;
 
-
-
 typedef struct tagSMgmtObject
 {
+	void *pAdapter;
+	// MAC address
+	unsigned char abyMACAddr[WLAN_ADDR_LEN];
 
-    void *                   pAdapter;
-    // MAC address
-    unsigned char abyMACAddr[WLAN_ADDR_LEN];
-
-    // Configuration Mode
-    WMAC_CONFIG_MODE        eConfigMode; // MAC pre-configed mode
-    CARD_PHY_TYPE           eCurrentPHYMode;
-    CARD_PHY_TYPE           eConfigPHYMode;
-
+	// Configuration Mode
+	WMAC_CONFIG_MODE        eConfigMode; // MAC pre-configed mode
+	CARD_PHY_TYPE           eCurrentPHYMode;
+	CARD_PHY_TYPE           eConfigPHYMode;
 
-    // Operation state variables
-    WMAC_CURRENT_MODE       eCurrMode;   // MAC current connection mode
-    WMAC_BSS_STATE          eCurrState;  // MAC current BSS state
+	// Operation state variables
+	WMAC_CURRENT_MODE       eCurrMode;   // MAC current connection mode
+	WMAC_BSS_STATE          eCurrState;  // MAC current BSS state
 
-    PKnownBSS               pCurrBSS;
-    unsigned char byCSSGK;
-    unsigned char byCSSPK;
+	PKnownBSS               pCurrBSS;
+	unsigned char byCSSGK;
+	unsigned char byCSSPK;
 
 //    unsigned char abyNewSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN];
 //    unsigned char abyNewExtSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN];
 
-    // Current state vars
-    unsigned int	uCurrChannel;
-    unsigned char abyCurrSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
-    unsigned char abyCurrExtSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
-    unsigned char abyCurrSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
-    unsigned char abyCurrBSSID[WLAN_BSSID_LEN];
-    unsigned short wCurrCapInfo;
-    unsigned short wCurrAID;
-    unsigned short wCurrATIMWindow;
-    unsigned short wCurrBeaconPeriod;
-    bool bIsDS;
-    unsigned char byERPContext;
-
-    CMD_STATE               eCommandState;
-    unsigned int	uScanChannel;
-
-    // Desire joining BSS vars
-    unsigned char abyDesireSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
-    unsigned char abyDesireBSSID[WLAN_BSSID_LEN];
-
-    // Adhoc or AP configuration vars
-  //unsigned char abyAdHocSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
-    unsigned short wIBSSBeaconPeriod;
-    unsigned short wIBSSATIMWindow;
-    unsigned int	uIBSSChannel;
-    unsigned char abyIBSSSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
-    unsigned char byAPBBType;
-    unsigned char abyWPAIE[MAX_WPA_IE_LEN];
-    unsigned short wWPAIELen;
-
-    unsigned int	uAssocCount;
-    bool bMoreData;
-
-    // Scan state vars
-    WMAC_SCAN_STATE         eScanState;
-    WMAC_SCAN_TYPE          eScanType;
-    unsigned int	uScanStartCh;
-    unsigned int	uScanEndCh;
-    unsigned short wScanSteps;
-    unsigned int	uScanBSSType;
-    // Desire scanning vars
-    unsigned char abyScanSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
-    unsigned char abyScanBSSID[WLAN_BSSID_LEN];
-
-    // Privacy
-    WMAC_AUTHENTICATION_MODE eAuthenMode;
-    WMAC_ENCRYPTION_MODE    eEncryptionMode;
-    bool bShareKeyAlgorithm;
-    unsigned char abyChallenge[WLAN_CHALLENGE_LEN];
-    bool bPrivacyInvoked;
-
-    // Received beacon state vars
-    bool bInTIM;
-    bool bMulticastTIM;
-    unsigned char byDTIMCount;
-    unsigned char byDTIMPeriod;
-
-    // Power saving state vars
-    WMAC_POWER_MODE         ePSMode;
-    unsigned short wListenInterval;
-    unsigned short wCountToWakeUp;
-    bool bInTIMWake;
-    unsigned char *pbyPSPacketPool;
-    unsigned char byPSPacketPool[sizeof(STxMgmtPacket) + WLAN_NULLDATA_FR_MAXLEN];
-    bool bRxBeaconInTBTTWake;
-    unsigned char abyPSTxMap[MAX_NODE_NUM + 1];
-
-    // management command related
-    unsigned int	uCmdBusy;
-    unsigned int	uCmdHostAPBusy;
-
-    // management packet pool
-    unsigned char *pbyMgmtPacketPool;
-    unsigned char byMgmtPacketPool[sizeof(STxMgmtPacket) + WLAN_A3FR_MAXLEN];
-
-
-    // One second callback timer
-    struct timer_list	    sTimerSecondCallback;
-
-    // Temporarily Rx Mgmt Packet Descriptor
-    SRxMgmtPacket           sRxPacket;
-
-    // link list of known bss's (scan results)
-    KnownBSS                sBSSList[MAX_BSS_NUM];
-
-
-
-    // table list of known node
-    // sNodeDBList[0] is reserved for AP under Infra mode
-    // sNodeDBList[0] is reserved for Multicast under adhoc/AP mode
-    KnownNodeDB             sNodeDBTable[MAX_NODE_NUM + 1];
-
-
-
-    // WPA2 PMKID Cache
-    SPMKIDCache             gsPMKIDCache;
-    bool bRoaming;
-
-    // rate fall back vars
-
-
-
-    // associate info
-    SAssocInfo              sAssocInfo;
-
-
-    // for 802.11h
-    bool b11hEnable;
-    bool bSwitchChannel;
-    unsigned char byNewChannel;
-    PWLAN_IE_MEASURE_REP    pCurrMeasureEIDRep;
-    unsigned int	uLengthOfRepEIDs;
-    unsigned char abyCurrentMSRReq[sizeof(STxMgmtPacket) + WLAN_A3FR_MAXLEN];
-    unsigned char abyCurrentMSRRep[sizeof(STxMgmtPacket) + WLAN_A3FR_MAXLEN];
-    unsigned char abyIECountry[WLAN_A3FR_MAXLEN];
-    unsigned char abyIBSSDFSOwner[6];
-    unsigned char byIBSSDFSRecovery;
-
-    struct sk_buff  skb;
-
+	// Current state vars
+	unsigned int	uCurrChannel;
+	unsigned char abyCurrSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
+	unsigned char abyCurrExtSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
+	unsigned char abyCurrSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
+	unsigned char abyCurrBSSID[WLAN_BSSID_LEN];
+	unsigned short wCurrCapInfo;
+	unsigned short wCurrAID;
+	unsigned short wCurrATIMWindow;
+	unsigned short wCurrBeaconPeriod;
+	bool bIsDS;
+	unsigned char byERPContext;
+
+	CMD_STATE               eCommandState;
+	unsigned int	uScanChannel;
+
+	// Desire joining BSS vars
+	unsigned char abyDesireSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
+	unsigned char abyDesireBSSID[WLAN_BSSID_LEN];
+
+	// Adhoc or AP configuration vars
+	//unsigned char abyAdHocSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
+	unsigned short wIBSSBeaconPeriod;
+	unsigned short wIBSSATIMWindow;
+	unsigned int	uIBSSChannel;
+	unsigned char abyIBSSSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
+	unsigned char byAPBBType;
+	unsigned char abyWPAIE[MAX_WPA_IE_LEN];
+	unsigned short wWPAIELen;
+
+	unsigned int	uAssocCount;
+	bool bMoreData;
+
+	// Scan state vars
+	WMAC_SCAN_STATE         eScanState;
+	WMAC_SCAN_TYPE          eScanType;
+	unsigned int	uScanStartCh;
+	unsigned int	uScanEndCh;
+	unsigned short wScanSteps;
+	unsigned int	uScanBSSType;
+	// Desire scanning vars
+	unsigned char abyScanSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
+	unsigned char abyScanBSSID[WLAN_BSSID_LEN];
+
+	// Privacy
+	WMAC_AUTHENTICATION_MODE eAuthenMode;
+	WMAC_ENCRYPTION_MODE    eEncryptionMode;
+	bool bShareKeyAlgorithm;
+	unsigned char abyChallenge[WLAN_CHALLENGE_LEN];
+	bool bPrivacyInvoked;
+
+	// Received beacon state vars
+	bool bInTIM;
+	bool bMulticastTIM;
+	unsigned char byDTIMCount;
+	unsigned char byDTIMPeriod;
+
+	// Power saving state vars
+	WMAC_POWER_MODE         ePSMode;
+	unsigned short wListenInterval;
+	unsigned short wCountToWakeUp;
+	bool bInTIMWake;
+	unsigned char *pbyPSPacketPool;
+	unsigned char byPSPacketPool[sizeof(STxMgmtPacket) + WLAN_NULLDATA_FR_MAXLEN];
+	bool bRxBeaconInTBTTWake;
+	unsigned char abyPSTxMap[MAX_NODE_NUM + 1];
+
+	// management command related
+	unsigned int	uCmdBusy;
+	unsigned int	uCmdHostAPBusy;
+
+	// management packet pool
+	unsigned char *pbyMgmtPacketPool;
+	unsigned char byMgmtPacketPool[sizeof(STxMgmtPacket) + WLAN_A3FR_MAXLEN];
+
+	// One second callback timer
+	struct timer_list	    sTimerSecondCallback;
+
+	// Temporarily Rx Mgmt Packet Descriptor
+	SRxMgmtPacket           sRxPacket;
+
+	// link list of known bss's (scan results)
+	KnownBSS                sBSSList[MAX_BSS_NUM];
+
+	// table list of known node
+	// sNodeDBList[0] is reserved for AP under Infra mode
+	// sNodeDBList[0] is reserved for Multicast under adhoc/AP mode
+	KnownNodeDB             sNodeDBTable[MAX_NODE_NUM + 1];
+
+	// WPA2 PMKID Cache
+	SPMKIDCache             gsPMKIDCache;
+	bool bRoaming;
+
+	// rate fall back vars
+
+	// associate info
+	SAssocInfo              sAssocInfo;
+
+	// for 802.11h
+	bool b11hEnable;
+	bool bSwitchChannel;
+	unsigned char byNewChannel;
+	PWLAN_IE_MEASURE_REP    pCurrMeasureEIDRep;
+	unsigned int	uLengthOfRepEIDs;
+	unsigned char abyCurrentMSRReq[sizeof(STxMgmtPacket) + WLAN_A3FR_MAXLEN];
+	unsigned char abyCurrentMSRRep[sizeof(STxMgmtPacket) + WLAN_A3FR_MAXLEN];
+	unsigned char abyIECountry[WLAN_A3FR_MAXLEN];
+	unsigned char abyIBSSDFSOwner[6];
+	unsigned char byIBSSDFSRecovery;
+
+	struct sk_buff  skb;
 } SMgmtObject, *PSMgmtObject;
 
-
 /*---------------------  Export Macros ------------------------------*/
 
-
 /*---------------------  Export Functions  --------------------------*/
 
-
 void
 vMgrObjectInit(
-    void *hDeviceContext
-    );
+	void *hDeviceContext
+);
 
 void
 vMgrTimerInit(
-    void *hDeviceContext
-    );
+	void *hDeviceContext
+);
 
 void
 vMgrObjectReset(
-    void *hDeviceContext
-    );
+	void *hDeviceContext
+);
 
 void
 vMgrAssocBeginSta(
-    void *hDeviceContext,
-    PSMgmtObject pMgmt,
-    PCMD_STATUS pStatus
-    );
+	void *hDeviceContext,
+	PSMgmtObject pMgmt,
+	PCMD_STATUS pStatus
+);
 
 void
 vMgrReAssocBeginSta(
-    void *hDeviceContext,
-    PSMgmtObject pMgmt,
-    PCMD_STATUS pStatus
-    );
+	void *hDeviceContext,
+	PSMgmtObject pMgmt,
+	PCMD_STATUS pStatus
+);
 
 void
 vMgrDisassocBeginSta(
-    void *hDeviceContext,
-    PSMgmtObject pMgmt,
-    unsigned char *abyDestAddress,
-    unsigned short wReason,
-    PCMD_STATUS pStatus
-    );
+	void *hDeviceContext,
+	PSMgmtObject pMgmt,
+	unsigned char *abyDestAddress,
+	unsigned short wReason,
+	PCMD_STATUS pStatus
+);
 
 void
 vMgrAuthenBeginSta(
-    void *hDeviceContext,
-    PSMgmtObject pMgmt,
-    PCMD_STATUS pStatus
-    );
+	void *hDeviceContext,
+	PSMgmtObject pMgmt,
+	PCMD_STATUS pStatus
+);
 
 void
 vMgrCreateOwnIBSS(
-    void *hDeviceContext,
-    PCMD_STATUS pStatus
-    );
+	void *hDeviceContext,
+	PCMD_STATUS pStatus
+);
 
 void
 vMgrJoinBSSBegin(
-    void *hDeviceContext,
-    PCMD_STATUS pStatus
-    );
+	void *hDeviceContext,
+	PCMD_STATUS pStatus
+);
 
 void
 vMgrRxManagePacket(
-    void *hDeviceContext,
-    PSMgmtObject pMgmt,
-    PSRxMgmtPacket pRxPacket
-    );
+	void *hDeviceContext,
+	PSMgmtObject pMgmt,
+	PSRxMgmtPacket pRxPacket
+);
 
 /*
-void
-vMgrScanBegin(
-    void *hDeviceContext,
-    PCMD_STATUS pStatus
-    );
+  void
+  vMgrScanBegin(
+  void *hDeviceContext,
+  PCMD_STATUS pStatus
+);
 */
 
 void
 vMgrDeAuthenBeginSta(
-    void *hDeviceContext,
-    PSMgmtObject  pMgmt,
-    unsigned char *abyDestAddress,
-    unsigned short wReason,
-    PCMD_STATUS pStatus
-    );
+	void *hDeviceContext,
+	PSMgmtObject  pMgmt,
+	unsigned char *abyDestAddress,
+	unsigned short wReason,
+	PCMD_STATUS pStatus
+);
 
 bool
 bMgrPrepareBeaconToSend(
-    void *hDeviceContext,
-    PSMgmtObject pMgmt
-    );
-
+	void *hDeviceContext,
+	PSMgmtObject pMgmt
+);
 
 bool
-bAdd_PMKID_Candidate (
-    void *hDeviceContext,
-    unsigned char *pbyBSSID,
-    PSRSNCapObject psRSNCapObj
-    );
+bAdd_PMKID_Candidate(
+	void *hDeviceContext,
+	unsigned char *pbyBSSID,
+	PSRSNCapObject psRSNCapObj
+);
 
 void
-vFlush_PMKID_Candidate (
-    void *hDeviceContext
-    );
+vFlush_PMKID_Candidate(
+	void *hDeviceContext
+);
 
 #endif // __WMGR_H__
diff --git a/drivers/staging/vt6655/wpa.c b/drivers/staging/vt6655/wpa.c
index 4412fe9396a..c5293bbcdab 100644
--- a/drivers/staging/vt6655/wpa.c
+++ b/drivers/staging/vt6655/wpa.c
@@ -43,7 +43,7 @@
 #include "80211mgr.h"
 
 /*---------------------  Static Variables  --------------------------*/
-static int          msglevel                =MSG_LEVEL_INFO;
+static int msglevel = MSG_LEVEL_INFO;
 
 const unsigned char abyOUI00[4] = { 0x00, 0x50, 0xf2, 0x00 };
 const unsigned char abyOUI01[4] = { 0x00, 0x50, 0xf2, 0x01 };
@@ -52,7 +52,6 @@ const unsigned char abyOUI03[4] = { 0x00, 0x50, 0xf2, 0x03 };
 const unsigned char abyOUI04[4] = { 0x00, 0x50, 0xf2, 0x04 };
 const unsigned char abyOUI05[4] = { 0x00, 0x50, 0xf2, 0x05 };
 
-
 /*+
  *
  * Description:
@@ -66,29 +65,28 @@ const unsigned char abyOUI05[4] = { 0x00, 0x50, 0xf2, 0x05 };
  *
  * Return Value: none.
  *
--*/
+ -*/
 
 void
-WPA_ClearRSN (
-    PKnownBSS        pBSSList
-    )
+WPA_ClearRSN(
+	PKnownBSS        pBSSList
+)
 {
-    int ii;
-    pBSSList->byGKType = WPA_TKIP;
-    for (ii=0; ii < 4; ii ++)
-        pBSSList->abyPKType[ii] = WPA_TKIP;
-    pBSSList->wPKCount = 0;
-    for (ii=0; ii < 4; ii ++)
-        pBSSList->abyAuthType[ii] = WPA_AUTH_IEEE802_1X;
-    pBSSList->wAuthCount = 0;
-    pBSSList->byDefaultK_as_PK = 0;
-    pBSSList->byReplayIdx = 0;
-    pBSSList->sRSNCapObj.bRSNCapExist = false;
-    pBSSList->sRSNCapObj.wRSNCap = 0;
-    pBSSList->bWPAValid = false;
+	int ii;
+	pBSSList->byGKType = WPA_TKIP;
+	for (ii = 0; ii < 4; ii++)
+		pBSSList->abyPKType[ii] = WPA_TKIP;
+	pBSSList->wPKCount = 0;
+	for (ii = 0; ii < 4; ii++)
+		pBSSList->abyAuthType[ii] = WPA_AUTH_IEEE802_1X;
+	pBSSList->wAuthCount = 0;
+	pBSSList->byDefaultK_as_PK = 0;
+	pBSSList->byReplayIdx = 0;
+	pBSSList->sRSNCapObj.bRSNCapExist = false;
+	pBSSList->sRSNCapObj.wRSNCap = 0;
+	pBSSList->bWPAValid = false;
 }
 
-
 /*+
  *
  * Description:
@@ -103,122 +101,118 @@ WPA_ClearRSN (
  *
  * Return Value: none.
  *
--*/
+ -*/
 void
-WPA_ParseRSN (
-    PKnownBSS        pBSSList,
-    PWLAN_IE_RSN_EXT pRSN
-    )
+WPA_ParseRSN(
+	PKnownBSS        pBSSList,
+	PWLAN_IE_RSN_EXT pRSN
+)
 {
-    PWLAN_IE_RSN_AUTH  pIE_RSN_Auth = NULL;
-    int                i, j, m, n = 0;
-    unsigned char *pbyCaps;
-
-    WPA_ClearRSN(pBSSList);
+	PWLAN_IE_RSN_AUTH  pIE_RSN_Auth = NULL;
+	int                i, j, m, n = 0;
+	unsigned char *pbyCaps;
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"WPA_ParseRSN: [%d]\n", pRSN->len);
+	WPA_ClearRSN(pBSSList);
 
-    // information element header makes sense
-    if ((pRSN->len >= 6) // oui1(4)+ver(2)
-         && (pRSN->byElementID == WLAN_EID_RSN_WPA) &&  !memcmp(pRSN->abyOUI, abyOUI01, 4)
-         && (pRSN->wVersion == 1)) {
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "WPA_ParseRSN: [%d]\n", pRSN->len);
 
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Legal RSN\n");
-        // update each variable if pRSN is long enough to contain the variable
-        if (pRSN->len >= 10) //oui1(4)+ver(2)+GKSuite(4)
-        {
-            if ( !memcmp(pRSN->abyMulticast, abyOUI01, 4))
-                pBSSList->byGKType = WPA_WEP40;
-            else if ( !memcmp(pRSN->abyMulticast, abyOUI02, 4))
-                pBSSList->byGKType = WPA_TKIP;
-            else if ( !memcmp(pRSN->abyMulticast, abyOUI03, 4))
-                pBSSList->byGKType = WPA_AESWRAP;
-            else if ( !memcmp(pRSN->abyMulticast, abyOUI04, 4))
-                pBSSList->byGKType = WPA_AESCCMP;
-            else if ( !memcmp(pRSN->abyMulticast, abyOUI05, 4))
-                pBSSList->byGKType = WPA_WEP104;
-            else
-                // any vendor checks here
-                pBSSList->byGKType = WPA_NONE;
+	// information element header makes sense
+	if ((pRSN->len >= 6) // oui1(4)+ver(2)
+	    && (pRSN->byElementID == WLAN_EID_RSN_WPA) && !memcmp(pRSN->abyOUI, abyOUI01, 4)
+	    && (pRSN->wVersion == 1)) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Legal RSN\n");
+		// update each variable if pRSN is long enough to contain the variable
+		if (pRSN->len >= 10) //oui1(4)+ver(2)+GKSuite(4)
+		{
+			if (!memcmp(pRSN->abyMulticast, abyOUI01, 4))
+				pBSSList->byGKType = WPA_WEP40;
+			else if (!memcmp(pRSN->abyMulticast, abyOUI02, 4))
+				pBSSList->byGKType = WPA_TKIP;
+			else if (!memcmp(pRSN->abyMulticast, abyOUI03, 4))
+				pBSSList->byGKType = WPA_AESWRAP;
+			else if (!memcmp(pRSN->abyMulticast, abyOUI04, 4))
+				pBSSList->byGKType = WPA_AESCCMP;
+			else if (!memcmp(pRSN->abyMulticast, abyOUI05, 4))
+				pBSSList->byGKType = WPA_WEP104;
+			else
+				// any vendor checks here
+				pBSSList->byGKType = WPA_NONE;
 
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"byGKType: %x\n", pBSSList->byGKType);
-        }
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "byGKType: %x\n", pBSSList->byGKType);
+		}
 
-        if (pRSN->len >= 12) //oui1(4)+ver(2)+GKS(4)+PKSCnt(2)
-        {
-            j = 0;
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"wPKCount: %d, sizeof(pBSSList->abyPKType): %zu\n", pRSN->wPKCount, sizeof(pBSSList->abyPKType));
-            for(i = 0; (i < pRSN->wPKCount) && (j < ARRAY_SIZE(pBSSList->abyPKType)); i++) {
-                if(pRSN->len >= 12+i*4+4) { //oui1(4)+ver(2)+GKS(4)+PKSCnt(2)+PKS(4*i)
-                    if ( !memcmp(pRSN->PKSList[i].abyOUI, abyOUI00, 4))
-                        pBSSList->abyPKType[j++] = WPA_NONE;
-                    else if ( !memcmp(pRSN->PKSList[i].abyOUI, abyOUI02, 4))
-                        pBSSList->abyPKType[j++] = WPA_TKIP;
-                    else if ( !memcmp(pRSN->PKSList[i].abyOUI, abyOUI03, 4))
-                        pBSSList->abyPKType[j++] = WPA_AESWRAP;
-                    else if ( !memcmp(pRSN->PKSList[i].abyOUI, abyOUI04, 4))
-                        pBSSList->abyPKType[j++] = WPA_AESCCMP;
-                    else
-                        // any vendor checks here
-                        ;
-                }
-                else
-                    break;
-                //DBG_PRN_GRP14(("abyPKType[%d]: %X\n", j-1, pBSSList->abyPKType[j-1]));
-            } //for
-            pBSSList->wPKCount = (unsigned short)j;
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"wPKCount: %d\n", pBSSList->wPKCount);
-        }
+		if (pRSN->len >= 12) //oui1(4)+ver(2)+GKS(4)+PKSCnt(2)
+		{
+			j = 0;
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wPKCount: %d, sizeof(pBSSList->abyPKType): %zu\n", pRSN->wPKCount, sizeof(pBSSList->abyPKType));
+			for (i = 0; (i < pRSN->wPKCount) && (j < ARRAY_SIZE(pBSSList->abyPKType)); i++) {
+				if (pRSN->len >= 12+i*4+4) { //oui1(4)+ver(2)+GKS(4)+PKSCnt(2)+PKS(4*i)
+					if (!memcmp(pRSN->PKSList[i].abyOUI, abyOUI00, 4))
+						pBSSList->abyPKType[j++] = WPA_NONE;
+					else if (!memcmp(pRSN->PKSList[i].abyOUI, abyOUI02, 4))
+						pBSSList->abyPKType[j++] = WPA_TKIP;
+					else if (!memcmp(pRSN->PKSList[i].abyOUI, abyOUI03, 4))
+						pBSSList->abyPKType[j++] = WPA_AESWRAP;
+					else if (!memcmp(pRSN->PKSList[i].abyOUI, abyOUI04, 4))
+						pBSSList->abyPKType[j++] = WPA_AESCCMP;
+					else
+						// any vendor checks here
+						;
+				} else
+					break;
+				//DBG_PRN_GRP14(("abyPKType[%d]: %X\n", j-1, pBSSList->abyPKType[j-1]));
+			} //for
+			pBSSList->wPKCount = (unsigned short)j;
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wPKCount: %d\n", pBSSList->wPKCount);
+		}
 
-        m = pRSN->wPKCount;
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"m: %d\n", m);
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"14+m*4: %d\n", 14+m*4);
+		m = pRSN->wPKCount;
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "m: %d\n", m);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "14+m*4: %d\n", 14+m*4);
 
-        if (pRSN->len >= 14+m*4) { //oui1(4)+ver(2)+GKS(4)+PKSCnt(2)+PKS(4*m)+AKC(2)
-            // overlay IE_RSN_Auth structure into correct place
-            pIE_RSN_Auth = (PWLAN_IE_RSN_AUTH) pRSN->PKSList[m].abyOUI;
-            j = 0;
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"wAuthCount: %d, sizeof(pBSSList->abyAuthType): %zu\n",
-                          pIE_RSN_Auth->wAuthCount, sizeof(pBSSList->abyAuthType));
-            for(i = 0; (i < pIE_RSN_Auth->wAuthCount) && (j < ARRAY_SIZE(pBSSList->abyAuthType)); i++) {
-                if(pRSN->len >= 14+4+(m+i)*4) { //oui1(4)+ver(2)+GKS(4)+PKSCnt(2)+PKS(4*m)+AKC(2)+AKS(4*i)
-                    if ( !memcmp(pIE_RSN_Auth->AuthKSList[i].abyOUI, abyOUI01, 4))
-                        pBSSList->abyAuthType[j++] = WPA_AUTH_IEEE802_1X;
-                    else if ( !memcmp(pIE_RSN_Auth->AuthKSList[i].abyOUI, abyOUI02, 4))
-                        pBSSList->abyAuthType[j++] = WPA_AUTH_PSK;
-                    else
-                    // any vendor checks here
-                    ;
-                }
-                else
-                    break;
-                //DBG_PRN_GRP14(("abyAuthType[%d]: %X\n", j-1, pBSSList->abyAuthType[j-1]));
-            }
-            if(j > 0)
-                pBSSList->wAuthCount = (unsigned short)j;
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"wAuthCount: %d\n", pBSSList->wAuthCount);
-        }
+		if (pRSN->len >= 14+m*4) { //oui1(4)+ver(2)+GKS(4)+PKSCnt(2)+PKS(4*m)+AKC(2)
+			// overlay IE_RSN_Auth structure into correct place
+			pIE_RSN_Auth = (PWLAN_IE_RSN_AUTH) pRSN->PKSList[m].abyOUI;
+			j = 0;
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wAuthCount: %d, sizeof(pBSSList->abyAuthType): %zu\n",
+				pIE_RSN_Auth->wAuthCount, sizeof(pBSSList->abyAuthType));
+			for (i = 0; (i < pIE_RSN_Auth->wAuthCount) && (j < ARRAY_SIZE(pBSSList->abyAuthType)); i++) {
+				if (pRSN->len >= 14+4+(m+i)*4) { //oui1(4)+ver(2)+GKS(4)+PKSCnt(2)+PKS(4*m)+AKC(2)+AKS(4*i)
+					if (!memcmp(pIE_RSN_Auth->AuthKSList[i].abyOUI, abyOUI01, 4))
+						pBSSList->abyAuthType[j++] = WPA_AUTH_IEEE802_1X;
+					else if (!memcmp(pIE_RSN_Auth->AuthKSList[i].abyOUI, abyOUI02, 4))
+						pBSSList->abyAuthType[j++] = WPA_AUTH_PSK;
+					else
+						// any vendor checks here
+						;
+				} else
+					break;
+				//DBG_PRN_GRP14(("abyAuthType[%d]: %X\n", j-1, pBSSList->abyAuthType[j-1]));
+			}
+			if (j > 0)
+				pBSSList->wAuthCount = (unsigned short)j;
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wAuthCount: %d\n", pBSSList->wAuthCount);
+		}
 
-        if (pIE_RSN_Auth != NULL) {
+		if (pIE_RSN_Auth != NULL) {
+			n = pIE_RSN_Auth->wAuthCount;
 
-            n = pIE_RSN_Auth->wAuthCount;
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "n: %d\n", n);
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "14+4+(m+n)*4: %d\n", 14+4+(m+n)*4);
 
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"n: %d\n", n);
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"14+4+(m+n)*4: %d\n", 14+4+(m+n)*4);
-
-            if(pRSN->len+2 >= 14+4+(m+n)*4) { //oui1(4)+ver(2)+GKS(4)+PKSCnt(2)+PKS(4*m)+AKC(2)+AKS(4*n)+Cap(2)
-                pbyCaps = (unsigned char *)pIE_RSN_Auth->AuthKSList[n].abyOUI;
-                pBSSList->byDefaultK_as_PK = (*pbyCaps) & WPA_GROUPFLAG;
-                pBSSList->byReplayIdx = 2 << ((*pbyCaps >> WPA_REPLAYBITSSHIFT) & WPA_REPLAYBITS);
-                pBSSList->sRSNCapObj.bRSNCapExist = true;
-                pBSSList->sRSNCapObj.wRSNCap = *(unsigned short *)pbyCaps;
-                //DBG_PRN_GRP14(("pbyCaps: %X\n", *pbyCaps));
-                //DBG_PRN_GRP14(("byDefaultK_as_PK: %X\n", pBSSList->byDefaultK_as_PK));
-                //DBG_PRN_GRP14(("byReplayIdx: %X\n", pBSSList->byReplayIdx));
-            }
-        }
-        pBSSList->bWPAValid = true;
-    }
+			if (pRSN->len+2 >= 14+4+(m+n)*4) { //oui1(4)+ver(2)+GKS(4)+PKSCnt(2)+PKS(4*m)+AKC(2)+AKS(4*n)+Cap(2)
+				pbyCaps = (unsigned char *)pIE_RSN_Auth->AuthKSList[n].abyOUI;
+				pBSSList->byDefaultK_as_PK = (*pbyCaps) & WPA_GROUPFLAG;
+				pBSSList->byReplayIdx = 2 << ((*pbyCaps >> WPA_REPLAYBITSSHIFT) & WPA_REPLAYBITS);
+				pBSSList->sRSNCapObj.bRSNCapExist = true;
+				pBSSList->sRSNCapObj.wRSNCap = *(unsigned short *)pbyCaps;
+				//DBG_PRN_GRP14(("pbyCaps: %X\n", *pbyCaps));
+				//DBG_PRN_GRP14(("byDefaultK_as_PK: %X\n", pBSSList->byDefaultK_as_PK));
+				//DBG_PRN_GRP14(("byReplayIdx: %X\n", pBSSList->byReplayIdx));
+			}
+		}
+		pBSSList->bWPAValid = true;
+	}
 }
 
 /*+
@@ -236,51 +230,47 @@ WPA_ParseRSN (
  *
  * Return Value: none.
  *
--*/
+ -*/
 bool
-WPA_SearchRSN (
-    unsigned char byCmd,
-    unsigned char byEncrypt,
-    PKnownBSS        pBSSList
-    )
+WPA_SearchRSN(
+	unsigned char byCmd,
+	unsigned char byEncrypt,
+	PKnownBSS        pBSSList
+)
 {
-    int ii;
-    unsigned char byPKType = WPA_NONE;
+	int ii;
+	unsigned char byPKType = WPA_NONE;
 
-    if (pBSSList->bWPAValid == false)
-        return false;
+	if (pBSSList->bWPAValid == false)
+		return false;
 
-    switch(byCmd) {
-    case 0:
+	switch (byCmd) {
+	case 0:
 
-        if (byEncrypt != pBSSList->byGKType)
-            return false;
+		if (byEncrypt != pBSSList->byGKType)
+			return false;
 
-        if (pBSSList->wPKCount > 0) {
-            for (ii = 0; ii < pBSSList->wPKCount; ii ++) {
-                if (pBSSList->abyPKType[ii] == WPA_AESCCMP)
-                    byPKType = WPA_AESCCMP;
-                else if ((pBSSList->abyPKType[ii] == WPA_TKIP) && (byPKType != WPA_AESCCMP))
-                     byPKType = WPA_TKIP;
-                else if ((pBSSList->abyPKType[ii] == WPA_WEP40) && (byPKType != WPA_AESCCMP) && (byPKType != WPA_TKIP))
-                     byPKType = WPA_WEP40;
-                else if ((pBSSList->abyPKType[ii] == WPA_WEP104) && (byPKType != WPA_AESCCMP) && (byPKType != WPA_TKIP))
-                     byPKType = WPA_WEP104;
-            }
-            if (byEncrypt != byPKType)
-                return false;
-        }
-        return true;
-//        if (pBSSList->wAuthCount > 0)
-//            for (ii=0; ii < pBSSList->wAuthCount; ii ++)
-//                if (byAuth == pBSSList->abyAuthType[ii])
-//                    break;
-        break;
+		if (pBSSList->wPKCount > 0) {
+			for (ii = 0; ii < pBSSList->wPKCount; ii++) {
+				if (pBSSList->abyPKType[ii] == WPA_AESCCMP)
+					byPKType = WPA_AESCCMP;
+				else if ((pBSSList->abyPKType[ii] == WPA_TKIP) && (byPKType != WPA_AESCCMP))
+					byPKType = WPA_TKIP;
+				else if ((pBSSList->abyPKType[ii] == WPA_WEP40) && (byPKType != WPA_AESCCMP) && (byPKType != WPA_TKIP))
+					byPKType = WPA_WEP40;
+				else if ((pBSSList->abyPKType[ii] == WPA_WEP104) && (byPKType != WPA_AESCCMP) && (byPKType != WPA_TKIP))
+					byPKType = WPA_WEP104;
+			}
+			if (byEncrypt != byPKType)
+				return false;
+		}
+		return true;
+		break;
 
-    default:
-        break;
-    }
-    return false;
+	default:
+		break;
+	}
+	return false;
 }
 
 /*+
@@ -296,21 +286,19 @@ WPA_SearchRSN (
  *
  * Return Value: none.
  *
--*/
+ -*/
 bool
-WPAb_Is_RSN (
-    PWLAN_IE_RSN_EXT pRSN
-    )
+WPAb_Is_RSN(
+	PWLAN_IE_RSN_EXT pRSN
+)
 {
-    if (pRSN == NULL)
-        return false;
+	if (pRSN == NULL)
+		return false;
 
-    if ((pRSN->len >= 6) && // oui1(4)+ver(2)
-        (pRSN->byElementID == WLAN_EID_RSN_WPA) &&  !memcmp(pRSN->abyOUI, abyOUI01, 4) &&
-        (pRSN->wVersion == 1)) {
-        return true;
-    }
-    else
-        return false;
+	if ((pRSN->len >= 6) && // oui1(4)+ver(2)
+	    (pRSN->byElementID == WLAN_EID_RSN_WPA) &&  !memcmp(pRSN->abyOUI, abyOUI01, 4) &&
+	    (pRSN->wVersion == 1)) {
+		return true;
+	} else
+		return false;
 }
-
diff --git a/drivers/staging/vt6655/wpa.h b/drivers/staging/vt6655/wpa.h
index 921fd7ae9d3..1d1918a1264 100644
--- a/drivers/staging/vt6655/wpa.h
+++ b/drivers/staging/vt6655/wpa.h
@@ -55,30 +55,29 @@
 
 /*---------------------  Export Types  ------------------------------*/
 
-
 /*---------------------  Export Functions  --------------------------*/
 
 void
 WPA_ClearRSN(
-    PKnownBSS        pBSSList
-    );
+	PKnownBSS        pBSSList
+);
 
 void
 WPA_ParseRSN(
-    PKnownBSS        pBSSList,
-    PWLAN_IE_RSN_EXT pRSN
-    );
+	PKnownBSS        pBSSList,
+	PWLAN_IE_RSN_EXT pRSN
+);
 
 bool
 WPA_SearchRSN(
-    unsigned char byCmd,
-    unsigned char byEncrypt,
-    PKnownBSS        pBSSList
-    );
+	unsigned char byCmd,
+	unsigned char byEncrypt,
+	PKnownBSS        pBSSList
+);
 
 bool
 WPAb_Is_RSN(
-    PWLAN_IE_RSN_EXT pRSN
-    );
+	PWLAN_IE_RSN_EXT pRSN
+);
 
 #endif // __WPA_H__
diff --git a/drivers/staging/vt6655/wpa2.c b/drivers/staging/vt6655/wpa2.c
index 884db1abe12..089788dfba6 100644
--- a/drivers/staging/vt6655/wpa2.c
+++ b/drivers/staging/vt6655/wpa2.c
@@ -36,7 +36,7 @@
 #include "wmgr.h"
 
 /*---------------------  Static Definitions -------------------------*/
-static int          msglevel                =MSG_LEVEL_INFO;
+static int msglevel = MSG_LEVEL_INFO;
 //static int          msglevel                =MSG_LEVEL_DEBUG;
 /*---------------------  Static Classes  ----------------------------*/
 
@@ -51,7 +51,6 @@ const unsigned char abyOUICCMP[4]    = { 0x00, 0x0F, 0xAC, 0x04 };
 const unsigned char abyOUI8021X[4]   = { 0x00, 0x0F, 0xAC, 0x01 };
 const unsigned char abyOUIPSK[4]     = { 0x00, 0x0F, 0xAC, 0x02 };
 
-
 /*---------------------  Static Functions  --------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
@@ -71,25 +70,25 @@ const unsigned char abyOUIPSK[4]     = { 0x00, 0x0F, 0xAC, 0x02 };
  *
  * Return Value: none.
  *
--*/
+ -*/
 void
-WPA2_ClearRSN (
-    PKnownBSS        pBSSNode
-    )
+WPA2_ClearRSN(
+	PKnownBSS        pBSSNode
+)
 {
-    int ii;
-
-    pBSSNode->bWPA2Valid = false;
-
-    pBSSNode->byCSSGK = WLAN_11i_CSS_CCMP;
-    for (ii=0; ii < 4; ii ++)
-        pBSSNode->abyCSSPK[ii] = WLAN_11i_CSS_CCMP;
-    pBSSNode->wCSSPKCount = 1;
-    for (ii=0; ii < 4; ii ++)
-        pBSSNode->abyAKMSSAuthType[ii] = WLAN_11i_AKMSS_802_1X;
-    pBSSNode->wAKMSSAuthCount = 1;
-    pBSSNode->sRSNCapObj.bRSNCapExist = false;
-    pBSSNode->sRSNCapObj.wRSNCap = 0;
+	int ii;
+
+	pBSSNode->bWPA2Valid = false;
+
+	pBSSNode->byCSSGK = WLAN_11i_CSS_CCMP;
+	for (ii = 0; ii < 4; ii++)
+		pBSSNode->abyCSSPK[ii] = WLAN_11i_CSS_CCMP;
+	pBSSNode->wCSSPKCount = 1;
+	for (ii = 0; ii < 4; ii++)
+		pBSSNode->abyAKMSSAuthType[ii] = WLAN_11i_AKMSS_802_1X;
+	pBSSNode->wAKMSSAuthCount = 1;
+	pBSSNode->sRSNCapObj.bRSNCapExist = false;
+	pBSSNode->sRSNCapObj.wRSNCap = 0;
 }
 
 /*+
@@ -106,147 +105,144 @@ WPA2_ClearRSN (
  *
  * Return Value: none.
  *
--*/
+ -*/
 void
-WPA2vParseRSN (
-    PKnownBSS        pBSSNode,
-    PWLAN_IE_RSN     pRSN
-    )
+WPA2vParseRSN(
+	PKnownBSS        pBSSNode,
+	PWLAN_IE_RSN     pRSN
+)
 {
-    int                 i, j;
-    unsigned short m = 0, n = 0;
-    unsigned char *pbyOUI;
-    bool bUseGK = false;
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"WPA2_ParseRSN: [%d]\n", pRSN->len);
-
-    WPA2_ClearRSN(pBSSNode);
-
-    if (pRSN->len == 2) { // ver(2)
-        if ((pRSN->byElementID == WLAN_EID_RSN) && (pRSN->wVersion == 1)) {
-            pBSSNode->bWPA2Valid = true;
-        }
-        return;
-    }
-
-    if (pRSN->len < 6) { // ver(2) + GK(4)
-        // invalid CSS, P802.11i/D10.0, p31
-        return;
-    }
-
-    // information element header makes sense
-    if ((pRSN->byElementID == WLAN_EID_RSN) &&
-        (pRSN->wVersion == 1)) {
-
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Legal 802.11i RSN\n");
-
-        pbyOUI = &(pRSN->abyRSN[0]);
-        if ( !memcmp(pbyOUI, abyOUIWEP40, 4))
-            pBSSNode->byCSSGK = WLAN_11i_CSS_WEP40;
-        else if ( !memcmp(pbyOUI, abyOUITKIP, 4))
-            pBSSNode->byCSSGK = WLAN_11i_CSS_TKIP;
-        else if ( !memcmp(pbyOUI, abyOUICCMP, 4))
-            pBSSNode->byCSSGK = WLAN_11i_CSS_CCMP;
-        else if ( !memcmp(pbyOUI, abyOUIWEP104, 4))
-            pBSSNode->byCSSGK = WLAN_11i_CSS_WEP104;
-        else if ( !memcmp(pbyOUI, abyOUIGK, 4)) {
-            // invalid CSS, P802.11i/D10.0, p32
-            return;
-        } else
-            // any vendor checks here
-            pBSSNode->byCSSGK = WLAN_11i_CSS_UNKNOWN;
-
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"802.11i CSS: %X\n", pBSSNode->byCSSGK);
-
-        if (pRSN->len == 6) {
-            pBSSNode->bWPA2Valid = true;
-            return;
-        }
-
-        if (pRSN->len >= 8) { // ver(2) + GK(4) + PK count(2)
-            pBSSNode->wCSSPKCount = *((unsigned short *) &(pRSN->abyRSN[4]));
-            j = 0;
-            pbyOUI = &(pRSN->abyRSN[6]);
-
-            for (i = 0; (i < pBSSNode->wCSSPKCount) && (j < sizeof(pBSSNode->abyCSSPK)/sizeof(unsigned char)); i++) {
-
-                if (pRSN->len >= 8+i*4+4) { // ver(2)+GK(4)+PKCnt(2)+PKS(4*i)
-                    if ( !memcmp(pbyOUI, abyOUIGK, 4)) {
-                        pBSSNode->abyCSSPK[j++] = WLAN_11i_CSS_USE_GROUP;
-                        bUseGK = true;
-                    } else if ( !memcmp(pbyOUI, abyOUIWEP40, 4)) {
-                        // Invalid CSS, continue to parsing
-                    } else if ( !memcmp(pbyOUI, abyOUITKIP, 4)) {
-                        if (pBSSNode->byCSSGK != WLAN_11i_CSS_CCMP)
-                            pBSSNode->abyCSSPK[j++] = WLAN_11i_CSS_TKIP;
-                        else
-                            ; // Invalid CSS, continue to parsing
-                    } else if ( !memcmp(pbyOUI, abyOUICCMP, 4)) {
-                        pBSSNode->abyCSSPK[j++] = WLAN_11i_CSS_CCMP;
-                    } else if ( !memcmp(pbyOUI, abyOUIWEP104, 4)) {
-                        // Invalid CSS, continue to parsing
-                    } else {
-                        // any vendor checks here
-                        pBSSNode->abyCSSPK[j++] = WLAN_11i_CSS_UNKNOWN;
-                    }
-                    pbyOUI += 4;
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"abyCSSPK[%d]: %X\n", j-1, pBSSNode->abyCSSPK[j-1]);
-                } else
-                    break;
-            } //for
-
-            if (bUseGK == true) {
-                if (j != 1) {
-                    // invalid CSS, This should be only PK CSS.
-                    return;
-                }
-                if (pBSSNode->byCSSGK == WLAN_11i_CSS_CCMP) {
-                    // invalid CSS, If CCMP is enable , PK can't be CSSGK.
-                    return;
-                }
-            }
-            if ((pBSSNode->wCSSPKCount != 0) && (j == 0)) {
-                // invalid CSS, No valid PK.
-                return;
-            }
-            pBSSNode->wCSSPKCount = (unsigned short)j;
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"wCSSPKCount: %d\n", pBSSNode->wCSSPKCount);
-        }
-
-        m = *((unsigned short *) &(pRSN->abyRSN[4]));
-
-        if (pRSN->len >= 10+m*4) { // ver(2) + GK(4) + PK count(2) + PKS(4*m) + AKMSS count(2)
-            pBSSNode->wAKMSSAuthCount = *((unsigned short *) &(pRSN->abyRSN[6+4*m]));
-            j = 0;
-            pbyOUI = &(pRSN->abyRSN[8+4*m]);
-            for (i = 0; (i < pBSSNode->wAKMSSAuthCount) && (j < sizeof(pBSSNode->abyAKMSSAuthType)/sizeof(unsigned char)); i++) {
-                if (pRSN->len >= 10+(m+i)*4+4) { // ver(2)+GK(4)+PKCnt(2)+PKS(4*m)+AKMSS(2)+AKS(4*i)
-                    if ( !memcmp(pbyOUI, abyOUI8021X, 4))
-                        pBSSNode->abyAKMSSAuthType[j++] = WLAN_11i_AKMSS_802_1X;
-                    else if ( !memcmp(pbyOUI, abyOUIPSK, 4))
-                        pBSSNode->abyAKMSSAuthType[j++] = WLAN_11i_AKMSS_PSK;
-                    else
-                        // any vendor checks here
-                        pBSSNode->abyAKMSSAuthType[j++] = WLAN_11i_AKMSS_UNKNOWN;
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"abyAKMSSAuthType[%d]: %X\n", j-1, pBSSNode->abyAKMSSAuthType[j-1]);
-                } else
-                    break;
-            }
-            pBSSNode->wAKMSSAuthCount = (unsigned short)j;
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"wAKMSSAuthCount: %d\n", pBSSNode->wAKMSSAuthCount);
-
-            n = *((unsigned short *) &(pRSN->abyRSN[6+4*m]));
-            if (pRSN->len >= 12+4*m+4*n) { // ver(2)+GK(4)+PKCnt(2)+PKS(4*m)+AKMSSCnt(2)+AKMSS(4*n)+Cap(2)
-                pBSSNode->sRSNCapObj.bRSNCapExist = true;
-                pBSSNode->sRSNCapObj.wRSNCap = *((unsigned short *) &(pRSN->abyRSN[8+4*m+4*n]));
-            }
-        }
-        //ignore PMKID lists bcs only (Re)Assocrequest has this field
-        pBSSNode->bWPA2Valid = true;
-    }
+	int                 i, j;
+	unsigned short m = 0, n = 0;
+	unsigned char *pbyOUI;
+	bool bUseGK = false;
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "WPA2_ParseRSN: [%d]\n", pRSN->len);
+
+	WPA2_ClearRSN(pBSSNode);
+
+	if (pRSN->len == 2) { // ver(2)
+		if ((pRSN->byElementID == WLAN_EID_RSN) && (pRSN->wVersion == 1)) {
+			pBSSNode->bWPA2Valid = true;
+		}
+		return;
+	}
+
+	if (pRSN->len < 6) { // ver(2) + GK(4)
+		// invalid CSS, P802.11i/D10.0, p31
+		return;
+	}
+
+	// information element header makes sense
+	if ((pRSN->byElementID == WLAN_EID_RSN) &&
+	    (pRSN->wVersion == 1)) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Legal 802.11i RSN\n");
+
+		pbyOUI = &(pRSN->abyRSN[0]);
+		if (!memcmp(pbyOUI, abyOUIWEP40, 4))
+			pBSSNode->byCSSGK = WLAN_11i_CSS_WEP40;
+		else if (!memcmp(pbyOUI, abyOUITKIP, 4))
+			pBSSNode->byCSSGK = WLAN_11i_CSS_TKIP;
+		else if (!memcmp(pbyOUI, abyOUICCMP, 4))
+			pBSSNode->byCSSGK = WLAN_11i_CSS_CCMP;
+		else if (!memcmp(pbyOUI, abyOUIWEP104, 4))
+			pBSSNode->byCSSGK = WLAN_11i_CSS_WEP104;
+		else if (!memcmp(pbyOUI, abyOUIGK, 4)) {
+			// invalid CSS, P802.11i/D10.0, p32
+			return;
+		} else
+			// any vendor checks here
+			pBSSNode->byCSSGK = WLAN_11i_CSS_UNKNOWN;
+
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "802.11i CSS: %X\n", pBSSNode->byCSSGK);
+
+		if (pRSN->len == 6) {
+			pBSSNode->bWPA2Valid = true;
+			return;
+		}
+
+		if (pRSN->len >= 8) { // ver(2) + GK(4) + PK count(2)
+			pBSSNode->wCSSPKCount = *((unsigned short *)&(pRSN->abyRSN[4]));
+			j = 0;
+			pbyOUI = &(pRSN->abyRSN[6]);
+
+			for (i = 0; (i < pBSSNode->wCSSPKCount) && (j < sizeof(pBSSNode->abyCSSPK)/sizeof(unsigned char)); i++) {
+				if (pRSN->len >= 8+i*4+4) { // ver(2)+GK(4)+PKCnt(2)+PKS(4*i)
+					if (!memcmp(pbyOUI, abyOUIGK, 4)) {
+						pBSSNode->abyCSSPK[j++] = WLAN_11i_CSS_USE_GROUP;
+						bUseGK = true;
+					} else if (!memcmp(pbyOUI, abyOUIWEP40, 4)) {
+						// Invalid CSS, continue to parsing
+					} else if (!memcmp(pbyOUI, abyOUITKIP, 4)) {
+						if (pBSSNode->byCSSGK != WLAN_11i_CSS_CCMP)
+							pBSSNode->abyCSSPK[j++] = WLAN_11i_CSS_TKIP;
+						else
+							; // Invalid CSS, continue to parsing
+					} else if (!memcmp(pbyOUI, abyOUICCMP, 4)) {
+						pBSSNode->abyCSSPK[j++] = WLAN_11i_CSS_CCMP;
+					} else if (!memcmp(pbyOUI, abyOUIWEP104, 4)) {
+						// Invalid CSS, continue to parsing
+					} else {
+						// any vendor checks here
+						pBSSNode->abyCSSPK[j++] = WLAN_11i_CSS_UNKNOWN;
+					}
+					pbyOUI += 4;
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "abyCSSPK[%d]: %X\n", j-1, pBSSNode->abyCSSPK[j-1]);
+				} else
+					break;
+			} //for
+
+			if (bUseGK == true) {
+				if (j != 1) {
+					// invalid CSS, This should be only PK CSS.
+					return;
+				}
+				if (pBSSNode->byCSSGK == WLAN_11i_CSS_CCMP) {
+					// invalid CSS, If CCMP is enable , PK can't be CSSGK.
+					return;
+				}
+			}
+			if ((pBSSNode->wCSSPKCount != 0) && (j == 0)) {
+				// invalid CSS, No valid PK.
+				return;
+			}
+			pBSSNode->wCSSPKCount = (unsigned short)j;
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wCSSPKCount: %d\n", pBSSNode->wCSSPKCount);
+		}
+
+		m = *((unsigned short *)&(pRSN->abyRSN[4]));
+
+		if (pRSN->len >= 10+m*4) { // ver(2) + GK(4) + PK count(2) + PKS(4*m) + AKMSS count(2)
+			pBSSNode->wAKMSSAuthCount = *((unsigned short *)&(pRSN->abyRSN[6+4*m]));
+			j = 0;
+			pbyOUI = &(pRSN->abyRSN[8+4*m]);
+			for (i = 0; (i < pBSSNode->wAKMSSAuthCount) && (j < sizeof(pBSSNode->abyAKMSSAuthType)/sizeof(unsigned char)); i++) {
+				if (pRSN->len >= 10+(m+i)*4+4) { // ver(2)+GK(4)+PKCnt(2)+PKS(4*m)+AKMSS(2)+AKS(4*i)
+					if (!memcmp(pbyOUI, abyOUI8021X, 4))
+						pBSSNode->abyAKMSSAuthType[j++] = WLAN_11i_AKMSS_802_1X;
+					else if (!memcmp(pbyOUI, abyOUIPSK, 4))
+						pBSSNode->abyAKMSSAuthType[j++] = WLAN_11i_AKMSS_PSK;
+					else
+						// any vendor checks here
+						pBSSNode->abyAKMSSAuthType[j++] = WLAN_11i_AKMSS_UNKNOWN;
+					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "abyAKMSSAuthType[%d]: %X\n", j-1, pBSSNode->abyAKMSSAuthType[j-1]);
+				} else
+					break;
+			}
+			pBSSNode->wAKMSSAuthCount = (unsigned short)j;
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wAKMSSAuthCount: %d\n", pBSSNode->wAKMSSAuthCount);
+
+			n = *((unsigned short *)&(pRSN->abyRSN[6+4*m]));
+			if (pRSN->len >= 12 + 4 * m + 4 * n) { // ver(2)+GK(4)+PKCnt(2)+PKS(4*m)+AKMSSCnt(2)+AKMSS(4*n)+Cap(2)
+				pBSSNode->sRSNCapObj.bRSNCapExist = true;
+				pBSSNode->sRSNCapObj.wRSNCap = *((unsigned short *)&(pRSN->abyRSN[8+4*m+4*n]));
+			}
+		}
+		//ignore PMKID lists bcs only (Re)Assocrequest has this field
+		pBSSNode->bWPA2Valid = true;
+	}
 }
 
-
 /*+
  *
  * Description:
@@ -260,105 +256,105 @@ WPA2vParseRSN (
  *
  * Return Value: length of IEs.
  *
--*/
+ -*/
 unsigned int
 WPA2uSetIEs(
-    void *pMgmtHandle,
-    PWLAN_IE_RSN pRSNIEs
-    )
+	void *pMgmtHandle,
+	PWLAN_IE_RSN pRSNIEs
+)
 {
-    PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtHandle;
-    unsigned char *pbyBuffer = NULL;
-    unsigned int ii = 0;
-    unsigned short *pwPMKID = NULL;
-
-    if (pRSNIEs == NULL) {
-        return(0);
-    }
-    if (((pMgmt->eAuthenMode == WMAC_AUTH_WPA2) ||
-         (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) &&
-        (pMgmt->pCurrBSS != NULL)) {
-        /* WPA2 IE */
-        pbyBuffer = (unsigned char *) pRSNIEs;
-        pRSNIEs->byElementID = WLAN_EID_RSN;
-        pRSNIEs->len = 6; //Version(2)+GK(4)
-        pRSNIEs->wVersion = 1;
-        //Group Key Cipher Suite
-        pRSNIEs->abyRSN[0] = 0x00;
-        pRSNIEs->abyRSN[1] = 0x0F;
-        pRSNIEs->abyRSN[2] = 0xAC;
-        if (pMgmt->byCSSGK == KEY_CTL_WEP) {
-            pRSNIEs->abyRSN[3] = pMgmt->pCurrBSS->byCSSGK;
-        } else if (pMgmt->byCSSGK == KEY_CTL_TKIP) {
-            pRSNIEs->abyRSN[3] = WLAN_11i_CSS_TKIP;
-        } else if (pMgmt->byCSSGK == KEY_CTL_CCMP) {
-            pRSNIEs->abyRSN[3] = WLAN_11i_CSS_CCMP;
-        } else {
-            pRSNIEs->abyRSN[3] = WLAN_11i_CSS_UNKNOWN;
-        }
-
-        // Pairwise Key Cipher Suite
-        pRSNIEs->abyRSN[4] = 1;
-        pRSNIEs->abyRSN[5] = 0;
-        pRSNIEs->abyRSN[6] = 0x00;
-        pRSNIEs->abyRSN[7] = 0x0F;
-        pRSNIEs->abyRSN[8] = 0xAC;
-        if (pMgmt->byCSSPK == KEY_CTL_TKIP) {
-            pRSNIEs->abyRSN[9] = WLAN_11i_CSS_TKIP;
-        } else if (pMgmt->byCSSPK == KEY_CTL_CCMP) {
-            pRSNIEs->abyRSN[9] = WLAN_11i_CSS_CCMP;
-        } else if (pMgmt->byCSSPK == KEY_CTL_NONE) {
-            pRSNIEs->abyRSN[9] = WLAN_11i_CSS_USE_GROUP;
-        } else {
-            pRSNIEs->abyRSN[9] = WLAN_11i_CSS_UNKNOWN;
-        }
-        pRSNIEs->len += 6;
-
-        // Auth Key Management Suite
-        pRSNIEs->abyRSN[10] = 1;
-        pRSNIEs->abyRSN[11] = 0;
-        pRSNIEs->abyRSN[12] = 0x00;
-        pRSNIEs->abyRSN[13] = 0x0F;
-        pRSNIEs->abyRSN[14] = 0xAC;
-        if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK) {
-            pRSNIEs->abyRSN[15] = WLAN_11i_AKMSS_PSK;
-        } else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2) {
-            pRSNIEs->abyRSN[15] = WLAN_11i_AKMSS_802_1X;
-        } else {
-            pRSNIEs->abyRSN[15] = WLAN_11i_AKMSS_UNKNOWN;
-        }
-        pRSNIEs->len +=6;
-
-        // RSN Capabilities
-        if (pMgmt->pCurrBSS->sRSNCapObj.bRSNCapExist == true) {
-            memcpy(&pRSNIEs->abyRSN[16], &pMgmt->pCurrBSS->sRSNCapObj.wRSNCap, 2);
-        } else {
-            pRSNIEs->abyRSN[16] = 0;
-            pRSNIEs->abyRSN[17] = 0;
-        }
-        pRSNIEs->len +=2;
-
-        if ((pMgmt->gsPMKIDCache.BSSIDInfoCount > 0) &&
-            (pMgmt->bRoaming == true) &&
-            (pMgmt->eAuthenMode == WMAC_AUTH_WPA2)) {
-            // RSN PMKID
-            pwPMKID = (unsigned short *)(&pRSNIEs->abyRSN[18]);  // Point to PMKID count
-            *pwPMKID = 0;                               // Initialize PMKID count
-            pbyBuffer = &pRSNIEs->abyRSN[20];           // Point to PMKID list
-            for (ii = 0; ii < pMgmt->gsPMKIDCache.BSSIDInfoCount; ii++) {
-                if ( !memcmp(&pMgmt->gsPMKIDCache.BSSIDInfo[ii].abyBSSID[0], pMgmt->abyCurrBSSID, ETH_ALEN)) {
-                    (*pwPMKID) ++;
-                    memcpy(pbyBuffer, pMgmt->gsPMKIDCache.BSSIDInfo[ii].abyPMKID, 16);
-                    pbyBuffer += 16;
-                }
-            }
-            if (*pwPMKID != 0) {
-                pRSNIEs->len += (2 + (*pwPMKID)*16);
-            } else {
-                pbyBuffer = &pRSNIEs->abyRSN[18];
-            }
-        }
-        return(pRSNIEs->len + WLAN_IEHDR_LEN);
-    }
-    return(0);
+	PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtHandle;
+	unsigned char *pbyBuffer = NULL;
+	unsigned int ii = 0;
+	unsigned short *pwPMKID = NULL;
+
+	if (pRSNIEs == NULL) {
+		return 0;
+	}
+	if (((pMgmt->eAuthenMode == WMAC_AUTH_WPA2) ||
+	     (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) &&
+	    (pMgmt->pCurrBSS != NULL)) {
+		/* WPA2 IE */
+		pbyBuffer = (unsigned char *)pRSNIEs;
+		pRSNIEs->byElementID = WLAN_EID_RSN;
+		pRSNIEs->len = 6; //Version(2)+GK(4)
+		pRSNIEs->wVersion = 1;
+		//Group Key Cipher Suite
+		pRSNIEs->abyRSN[0] = 0x00;
+		pRSNIEs->abyRSN[1] = 0x0F;
+		pRSNIEs->abyRSN[2] = 0xAC;
+		if (pMgmt->byCSSGK == KEY_CTL_WEP) {
+			pRSNIEs->abyRSN[3] = pMgmt->pCurrBSS->byCSSGK;
+		} else if (pMgmt->byCSSGK == KEY_CTL_TKIP) {
+			pRSNIEs->abyRSN[3] = WLAN_11i_CSS_TKIP;
+		} else if (pMgmt->byCSSGK == KEY_CTL_CCMP) {
+			pRSNIEs->abyRSN[3] = WLAN_11i_CSS_CCMP;
+		} else {
+			pRSNIEs->abyRSN[3] = WLAN_11i_CSS_UNKNOWN;
+		}
+
+		// Pairwise Key Cipher Suite
+		pRSNIEs->abyRSN[4] = 1;
+		pRSNIEs->abyRSN[5] = 0;
+		pRSNIEs->abyRSN[6] = 0x00;
+		pRSNIEs->abyRSN[7] = 0x0F;
+		pRSNIEs->abyRSN[8] = 0xAC;
+		if (pMgmt->byCSSPK == KEY_CTL_TKIP) {
+			pRSNIEs->abyRSN[9] = WLAN_11i_CSS_TKIP;
+		} else if (pMgmt->byCSSPK == KEY_CTL_CCMP) {
+			pRSNIEs->abyRSN[9] = WLAN_11i_CSS_CCMP;
+		} else if (pMgmt->byCSSPK == KEY_CTL_NONE) {
+			pRSNIEs->abyRSN[9] = WLAN_11i_CSS_USE_GROUP;
+		} else {
+			pRSNIEs->abyRSN[9] = WLAN_11i_CSS_UNKNOWN;
+		}
+		pRSNIEs->len += 6;
+
+		// Auth Key Management Suite
+		pRSNIEs->abyRSN[10] = 1;
+		pRSNIEs->abyRSN[11] = 0;
+		pRSNIEs->abyRSN[12] = 0x00;
+		pRSNIEs->abyRSN[13] = 0x0F;
+		pRSNIEs->abyRSN[14] = 0xAC;
+		if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK) {
+			pRSNIEs->abyRSN[15] = WLAN_11i_AKMSS_PSK;
+		} else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2) {
+			pRSNIEs->abyRSN[15] = WLAN_11i_AKMSS_802_1X;
+		} else {
+			pRSNIEs->abyRSN[15] = WLAN_11i_AKMSS_UNKNOWN;
+		}
+		pRSNIEs->len += 6;
+
+		// RSN Capabilities
+		if (pMgmt->pCurrBSS->sRSNCapObj.bRSNCapExist == true) {
+			memcpy(&pRSNIEs->abyRSN[16], &pMgmt->pCurrBSS->sRSNCapObj.wRSNCap, 2);
+		} else {
+			pRSNIEs->abyRSN[16] = 0;
+			pRSNIEs->abyRSN[17] = 0;
+		}
+		pRSNIEs->len += 2;
+
+		if ((pMgmt->gsPMKIDCache.BSSIDInfoCount > 0) &&
+		    (pMgmt->bRoaming == true) &&
+		    (pMgmt->eAuthenMode == WMAC_AUTH_WPA2)) {
+			// RSN PMKID
+			pwPMKID = (unsigned short *)(&pRSNIEs->abyRSN[18]);  // Point to PMKID count
+			*pwPMKID = 0;                               // Initialize PMKID count
+			pbyBuffer = &pRSNIEs->abyRSN[20];           // Point to PMKID list
+			for (ii = 0; ii < pMgmt->gsPMKIDCache.BSSIDInfoCount; ii++) {
+				if (!memcmp(&pMgmt->gsPMKIDCache.BSSIDInfo[ii].abyBSSID[0], pMgmt->abyCurrBSSID, ETH_ALEN)) {
+					(*pwPMKID)++;
+					memcpy(pbyBuffer, pMgmt->gsPMKIDCache.BSSIDInfo[ii].abyPMKID, 16);
+					pbyBuffer += 16;
+				}
+			}
+			if (*pwPMKID != 0) {
+				pRSNIEs->len += (2 + (*pwPMKID)*16);
+			} else {
+				pbyBuffer = &pRSNIEs->abyRSN[18];
+			}
+		}
+		return pRSNIEs->len + WLAN_IEHDR_LEN;
+	}
+	return 0;
 }
diff --git a/drivers/staging/vt6655/wpa2.h b/drivers/staging/vt6655/wpa2.h
index 718208beb72..2d0bd2e515f 100644
--- a/drivers/staging/vt6655/wpa2.h
+++ b/drivers/staging/vt6655/wpa2.h
@@ -40,16 +40,15 @@
 #define MAX_PMKID_CACHE         16
 
 typedef struct tagsPMKIDInfo {
-    unsigned char abyBSSID[6];
-    unsigned char abyPMKID[16];
+	unsigned char abyBSSID[6];
+	unsigned char abyPMKID[16];
 } PMKIDInfo, *PPMKIDInfo;
 
 typedef struct tagSPMKIDCache {
-    unsigned long BSSIDInfoCount;
-    PMKIDInfo   BSSIDInfo[MAX_PMKID_CACHE];
+	unsigned long BSSIDInfoCount;
+	PMKIDInfo   BSSIDInfo[MAX_PMKID_CACHE];
 } SPMKIDCache, *PSPMKIDCache;
 
-
 /*---------------------  Export Classes  ----------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
@@ -59,20 +58,20 @@ typedef struct tagSPMKIDCache {
 /*---------------------  Export Functions  --------------------------*/
 
 void
-WPA2_ClearRSN (
-    PKnownBSS        pBSSNode
-    );
+WPA2_ClearRSN(
+	PKnownBSS        pBSSNode
+);
 
 void
-WPA2vParseRSN (
-    PKnownBSS        pBSSNode,
-    PWLAN_IE_RSN     pRSN
-    );
+WPA2vParseRSN(
+	PKnownBSS        pBSSNode,
+	PWLAN_IE_RSN     pRSN
+);
 
 unsigned int
 WPA2uSetIEs(
-    void *pMgmtHandle,
-    PWLAN_IE_RSN pRSNIEs
-    );
+	void *pMgmtHandle,
+	PWLAN_IE_RSN pRSNIEs
+);
 
 #endif // __WPA2_H__
diff --git a/drivers/staging/vt6655/wpactl.c b/drivers/staging/vt6655/wpactl.c
index 2b6ae1e403b..869f62c678e 100644
--- a/drivers/staging/vt6655/wpactl.c
+++ b/drivers/staging/vt6655/wpactl.c
@@ -44,8 +44,6 @@
 
 #define VIAWGET_WPA_MAX_BUF_SIZE 1024
 
-
-
 static const int frequency_list[] = {
 	2412, 2417, 2422, 2427, 2432, 2437, 2442,
 	2447, 2452, 2457, 2462, 2467, 2472, 2484
@@ -54,13 +52,10 @@ static const int frequency_list[] = {
 
 /*---------------------  Static Variables  --------------------------*/
 //static int          msglevel                =MSG_LEVEL_DEBUG;
-static int          msglevel                =MSG_LEVEL_INFO;
+static int msglevel = MSG_LEVEL_INFO;
 
 /*---------------------  Static Functions  --------------------------*/
 
-
-
-
 /*---------------------  Export Variables  --------------------------*/
 static void wpadev_setup(struct net_device *dev)
 {
@@ -70,7 +65,7 @@ static void wpadev_setup(struct net_device *dev)
 	dev->addr_len           = ETH_ALEN;
 	dev->tx_queue_len       = 1000;
 
-	memset(dev->broadcast,0xFF, ETH_ALEN);
+	memset(dev->broadcast, 0xFF, ETH_ALEN);
 
 	dev->flags              = IFF_BROADCAST|IFF_MULTICAST;
 }
@@ -91,42 +86,41 @@ static void wpadev_setup(struct net_device *dev)
 
 static int wpa_init_wpadev(PSDevice pDevice)
 {
-    PSDevice wpadev_priv;
+	PSDevice wpadev_priv;
 	struct net_device *dev = pDevice->dev;
-         int ret=0;
+	int ret = 0;
 
 	pDevice->wpadev = alloc_netdev(sizeof(PSDevice), "vntwpa", wpadev_setup);
 	if (pDevice->wpadev == NULL)
 		return -ENOMEM;
 
-    wpadev_priv = netdev_priv(pDevice->wpadev);
-    *wpadev_priv = *pDevice;
+	wpadev_priv = netdev_priv(pDevice->wpadev);
+	*wpadev_priv = *pDevice;
 	memcpy(pDevice->wpadev->dev_addr, dev->dev_addr, ETH_ALEN);
-         pDevice->wpadev->base_addr = dev->base_addr;
+	pDevice->wpadev->base_addr = dev->base_addr;
 	pDevice->wpadev->irq = dev->irq;
 	pDevice->wpadev->mem_start = dev->mem_start;
 	pDevice->wpadev->mem_end = dev->mem_end;
 	ret = register_netdev(pDevice->wpadev);
 	if (ret) {
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: register_netdev(WPA) failed!\n",
-		       dev->name);
+			dev->name);
 		free_netdev(pDevice->wpadev);
 		return -1;
 	}
 
 	if (pDevice->skb == NULL) {
-        pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
-        if (pDevice->skb == NULL)
-		    return -ENOMEM;
-    }
+		pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
+		if (pDevice->skb == NULL)
+			return -ENOMEM;
+	}
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: Registered netdev %s for WPA management\n",
-	       dev->name, pDevice->wpadev->name);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: Registered netdev %s for WPA management\n",
+		dev->name, pDevice->wpadev->name);
 
 	return 0;
 }
 
-
 /*
  * Description:
  *      unregister net_device (wpadev)
@@ -142,26 +136,22 @@ static int wpa_init_wpadev(PSDevice pDevice)
 
 static int wpa_release_wpadev(PSDevice pDevice)
 {
-    if (pDevice->skb) {
-        dev_kfree_skb(pDevice->skb);
-        pDevice->skb = NULL;
-    }
-
-    if (pDevice->wpadev) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: Netdevice %s unregistered\n",
-	       pDevice->dev->name, pDevice->wpadev->name);
-	unregister_netdev(pDevice->wpadev);
-	free_netdev(pDevice->wpadev);
-         pDevice->wpadev = NULL;
-    }
+	if (pDevice->skb) {
+		dev_kfree_skb(pDevice->skb);
+		pDevice->skb = NULL;
+	}
+
+	if (pDevice->wpadev) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: Netdevice %s unregistered\n",
+			pDevice->dev->name, pDevice->wpadev->name);
+		unregister_netdev(pDevice->wpadev);
+		free_netdev(pDevice->wpadev);
+		pDevice->wpadev = NULL;
+	}
 
 	return 0;
 }
 
-
-
-
-
 /*
  * Description:
  *      Set enable/disable dev for wpa supplicant daemon
@@ -184,7 +174,6 @@ int wpa_set_wpadev(PSDevice pDevice, int val)
 		return wpa_release_wpadev(pDevice);
 }
 
-
 /*
  * Description:
  *      Set WPA algorithm & keys
@@ -199,252 +188,245 @@ int wpa_set_wpadev(PSDevice pDevice, int val)
  *
  */
 
- int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel)
+int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel)
 {
-    struct viawget_wpa_param *param=ctx;
-    PSMgmtObject pMgmt = pDevice->pMgmt;
-    unsigned long dwKeyIndex = 0;
-    unsigned char abyKey[MAX_KEY_LEN];
-    unsigned char abySeq[MAX_KEY_LEN];
-    QWORD   KeyRSC;
+	struct viawget_wpa_param *param = ctx;
+	PSMgmtObject pMgmt = pDevice->pMgmt;
+	unsigned long dwKeyIndex = 0;
+	unsigned char abyKey[MAX_KEY_LEN];
+	unsigned char abySeq[MAX_KEY_LEN];
+	QWORD   KeyRSC;
 //    NDIS_802_11_KEY_RSC KeyRSC;
-    unsigned char byKeyDecMode = KEY_CTL_WEP;
+	unsigned char byKeyDecMode = KEY_CTL_WEP;
 	int ret = 0;
 	int uu, ii;
 
-
 	if (param->u.wpa_key.alg_name > WPA_ALG_CCMP ||
-			param->u.wpa_key.key_len >= MAX_KEY_LEN ||
-			param->u.wpa_key.seq_len >= MAX_KEY_LEN)
+	    param->u.wpa_key.key_len >= MAX_KEY_LEN ||
+	    param->u.wpa_key.seq_len >= MAX_KEY_LEN)
 		return -EINVAL;
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "param->u.wpa_key.alg_name = %d \n", param->u.wpa_key.alg_name);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "param->u.wpa_key.alg_name = %d \n", param->u.wpa_key.alg_name);
 	if (param->u.wpa_key.alg_name == WPA_ALG_NONE) {
-        pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
-        pDevice->bEncryptionEnable = false;
-        pDevice->byKeyIndex = 0;
-        pDevice->bTransmitKey = false;
-        KeyvRemoveAllWEPKey(&(pDevice->sKey), pDevice->PortOffset);
-        for (uu=0; uu<MAX_KEY_TABLE; uu++) {
-            MACvDisableKeyEntry(pDevice->PortOffset, uu);
-        }
-        return ret;
-    }
-
-    //spin_unlock_irq(&pDevice->lock);
-    if(param->u.wpa_key.key && fcpfkernel) {
-       memcpy(&abyKey[0], param->u.wpa_key.key, param->u.wpa_key.key_len);
-     }
-    else {
-	spin_unlock_irq(&pDevice->lock);
-	if (param->u.wpa_key.key &&
-	    copy_from_user(&abyKey[0], param->u.wpa_key.key, param->u.wpa_key.key_len)) {
-	    spin_lock_irq(&pDevice->lock);
-	    return -EINVAL;
-    	}
-spin_lock_irq(&pDevice->lock);
-    	}
+		pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
+		pDevice->bEncryptionEnable = false;
+		pDevice->byKeyIndex = 0;
+		pDevice->bTransmitKey = false;
+		KeyvRemoveAllWEPKey(&(pDevice->sKey), pDevice->PortOffset);
+		for (uu = 0; uu < MAX_KEY_TABLE; uu++) {
+			MACvDisableKeyEntry(pDevice->PortOffset, uu);
+		}
+		return ret;
+	}
+
+	//spin_unlock_irq(&pDevice->lock);
+	if (param->u.wpa_key.key && fcpfkernel) {
+		memcpy(&abyKey[0], param->u.wpa_key.key, param->u.wpa_key.key_len);
+	} else {
+		spin_unlock_irq(&pDevice->lock);
+		if (param->u.wpa_key.key &&
+		    copy_from_user(&abyKey[0], param->u.wpa_key.key, param->u.wpa_key.key_len)) {
+			spin_lock_irq(&pDevice->lock);
+			return -EINVAL;
+		}
+		spin_lock_irq(&pDevice->lock);
+	}
 
-    dwKeyIndex = (unsigned long)(param->u.wpa_key.key_index);
+	dwKeyIndex = (unsigned long)(param->u.wpa_key.key_index);
 
 	if (param->u.wpa_key.alg_name == WPA_ALG_WEP) {
-        if (dwKeyIndex > 3) {
-            return -EINVAL;
-        }
-        else {
-            if (param->u.wpa_key.set_tx) {
-                pDevice->byKeyIndex = (unsigned char)dwKeyIndex;
-                pDevice->bTransmitKey = true;
-		        dwKeyIndex |= (1 << 31);
-            }
-            KeybSetDefaultKey(&(pDevice->sKey),
-                                dwKeyIndex & ~(BIT30 | USE_KEYRSC),
-                                param->u.wpa_key.key_len,
-                                NULL,
-                                abyKey,
-                                KEY_CTL_WEP,
-                                pDevice->PortOffset,
-                                pDevice->byLocalID);
-
-        }
-        pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
-        pDevice->bEncryptionEnable = true;
-        return ret;
-	}
-
-	    //spin_unlock_irq(&pDevice->lock);
-        if(param->u.wpa_key.seq && fcpfkernel) {
-           memcpy(&abySeq[0], param->u.wpa_key.seq, param->u.wpa_key.seq_len);
-        	}
-       else {
-	   	spin_unlock_irq(&pDevice->lock);
-	if (param->u.wpa_key.seq &&
-	    copy_from_user(&abySeq[0], param->u.wpa_key.seq, param->u.wpa_key.seq_len)) {
-	    spin_lock_irq(&pDevice->lock);
-	    return -EINVAL;
-       	}
-spin_lock_irq(&pDevice->lock);
-}
+		if (dwKeyIndex > 3) {
+			return -EINVAL;
+		} else {
+			if (param->u.wpa_key.set_tx) {
+				pDevice->byKeyIndex = (unsigned char)dwKeyIndex;
+				pDevice->bTransmitKey = true;
+				dwKeyIndex |= (1 << 31);
+			}
+			KeybSetDefaultKey(&(pDevice->sKey),
+					  dwKeyIndex & ~(BIT30 | USE_KEYRSC),
+					  param->u.wpa_key.key_len,
+					  NULL,
+					  abyKey,
+					  KEY_CTL_WEP,
+					  pDevice->PortOffset,
+					  pDevice->byLocalID);
+
+		}
+		pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
+		pDevice->bEncryptionEnable = true;
+		return ret;
+	}
+
+	//spin_unlock_irq(&pDevice->lock);
+	if (param->u.wpa_key.seq && fcpfkernel) {
+		memcpy(&abySeq[0], param->u.wpa_key.seq, param->u.wpa_key.seq_len);
+	} else {
+		spin_unlock_irq(&pDevice->lock);
+		if (param->u.wpa_key.seq &&
+		    copy_from_user(&abySeq[0], param->u.wpa_key.seq, param->u.wpa_key.seq_len)) {
+			spin_lock_irq(&pDevice->lock);
+			return -EINVAL;
+		}
+		spin_lock_irq(&pDevice->lock);
+	}
 
 	if (param->u.wpa_key.seq_len > 0) {
-		for (ii = 0 ; ii < param->u.wpa_key.seq_len ; ii++) {
-		     if (ii < 4)
-			    LODWORD(KeyRSC) |= (abySeq[ii] << (ii * 8));
-			 else
-			    HIDWORD(KeyRSC) |= (abySeq[ii] << ((ii-4) * 8));
-	         //KeyRSC |= (abySeq[ii] << (ii * 8));
+		for (ii = 0; ii < param->u.wpa_key.seq_len; ii++) {
+			if (ii < 4)
+				LODWORD(KeyRSC) |= (abySeq[ii] << (ii * 8));
+			else
+				HIDWORD(KeyRSC) |= (abySeq[ii] << ((ii-4) * 8));
+			//KeyRSC |= (abySeq[ii] << (ii * 8));
 		}
 		dwKeyIndex |= 1 << 29;
 	}
 
-    if (param->u.wpa_key.key_index >= MAX_GROUP_KEY) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "return  dwKeyIndex > 3\n");
-        return -EINVAL;
-    }
+	if (param->u.wpa_key.key_index >= MAX_GROUP_KEY) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "return  dwKeyIndex > 3\n");
+		return -EINVAL;
+	}
 
 	if (param->u.wpa_key.alg_name == WPA_ALG_TKIP) {
-        pDevice->eEncryptionStatus = Ndis802_11Encryption2Enabled;
-    }
+		pDevice->eEncryptionStatus = Ndis802_11Encryption2Enabled;
+	}
 
 	if (param->u.wpa_key.alg_name == WPA_ALG_CCMP) {
-        pDevice->eEncryptionStatus = Ndis802_11Encryption3Enabled;
-    }
+		pDevice->eEncryptionStatus = Ndis802_11Encryption3Enabled;
+	}
 
 	if (param->u.wpa_key.set_tx)
 		dwKeyIndex |= (1 << 31);
 
+	if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled)
+		byKeyDecMode = KEY_CTL_CCMP;
+	else if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled)
+		byKeyDecMode = KEY_CTL_TKIP;
+	else
+		byKeyDecMode = KEY_CTL_WEP;
+
+	// Fix HCT test that set 256 bits KEY and Ndis802_11Encryption3Enabled
+	if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled) {
+		if (param->u.wpa_key.key_len == MAX_KEY_LEN)
+			byKeyDecMode = KEY_CTL_TKIP;
+		else if (param->u.wpa_key.key_len == WLAN_WEP40_KEYLEN)
+			byKeyDecMode = KEY_CTL_WEP;
+		else if (param->u.wpa_key.key_len == WLAN_WEP104_KEYLEN)
+			byKeyDecMode = KEY_CTL_WEP;
+	} else if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled) {
+		if (param->u.wpa_key.key_len == WLAN_WEP40_KEYLEN)
+			byKeyDecMode = KEY_CTL_WEP;
+		else if (param->u.wpa_key.key_len == WLAN_WEP104_KEYLEN)
+			byKeyDecMode = KEY_CTL_WEP;
+	}
+
+	// Check TKIP key length
+	if ((byKeyDecMode == KEY_CTL_TKIP) &&
+	    (param->u.wpa_key.key_len != MAX_KEY_LEN)) {
+		// TKIP Key must be 256 bits
+		//DBG_PRN_WLAN03(("return NDIS_STATUS_INVALID_DATA - TKIP Key must be 256 bits\n"));
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "return- TKIP Key must be 256 bits!\n");
+		return -EINVAL;
+	}
+	// Check AES key length
+	if ((byKeyDecMode == KEY_CTL_CCMP) &&
+	    (param->u.wpa_key.key_len != AES_KEY_LEN)) {
+		// AES Key must be 128 bits
+		//DBG_PRN_WLAN03(("return NDIS_STATUS_INVALID_DATA - AES Key must be 128 bits\n"));
+		return -EINVAL;
+	}
+
+	// spin_lock_irq(&pDevice->lock);
+	if (is_broadcast_ether_addr(&param->addr[0]) || (param->addr == NULL)) {
+		// If is_broadcast_ether_addr, set the key as every key entry's group key.
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Groupe Key Assign.\n");
+
+		if ((KeybSetAllGroupKey(&(pDevice->sKey),
+					dwKeyIndex,
+					param->u.wpa_key.key_len,
+					(PQWORD) &(KeyRSC),
+					(unsigned char *)abyKey,
+					byKeyDecMode,
+					pDevice->PortOffset,
+					pDevice->byLocalID) == true) &&
+		    (KeybSetDefaultKey(&(pDevice->sKey),
+				       dwKeyIndex,
+				       param->u.wpa_key.key_len,
+				       (PQWORD) &(KeyRSC),
+				       (unsigned char *)abyKey,
+				       byKeyDecMode,
+				       pDevice->PortOffset,
+				       pDevice->byLocalID) == true)) {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "GROUP Key Assign.\n");
+
+		} else {
+			//DBG_PRN_WLAN03(("return NDIS_STATUS_INVALID_DATA -KeybSetDefaultKey Fail.0\n"));
+			// spin_unlock_irq(&pDevice->lock);
+			return -EINVAL;
+		}
+
+	} else {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Pairwise Key Assign.\n");
+		// BSSID not 0xffffffffffff
+		// Pairwise Key can't be WEP
+		if (byKeyDecMode == KEY_CTL_WEP) {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Pairwise Key can't be WEP\n");
+			//spin_unlock_irq(&pDevice->lock);
+			return -EINVAL;
+		}
 
-    if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled)
-        byKeyDecMode = KEY_CTL_CCMP;
-    else if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled)
-        byKeyDecMode = KEY_CTL_TKIP;
-    else
-        byKeyDecMode = KEY_CTL_WEP;
-
-    // Fix HCT test that set 256 bits KEY and Ndis802_11Encryption3Enabled
-    if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled) {
-        if (param->u.wpa_key.key_len == MAX_KEY_LEN)
-            byKeyDecMode = KEY_CTL_TKIP;
-        else if (param->u.wpa_key.key_len == WLAN_WEP40_KEYLEN)
-            byKeyDecMode = KEY_CTL_WEP;
-        else if (param->u.wpa_key.key_len == WLAN_WEP104_KEYLEN)
-            byKeyDecMode = KEY_CTL_WEP;
-    } else if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled) {
-        if (param->u.wpa_key.key_len == WLAN_WEP40_KEYLEN)
-            byKeyDecMode = KEY_CTL_WEP;
-        else if (param->u.wpa_key.key_len == WLAN_WEP104_KEYLEN)
-            byKeyDecMode = KEY_CTL_WEP;
-    }
-
-    // Check TKIP key length
-    if ((byKeyDecMode == KEY_CTL_TKIP) &&
-        (param->u.wpa_key.key_len != MAX_KEY_LEN)) {
-        // TKIP Key must be 256 bits
-        //DBG_PRN_WLAN03(("return NDIS_STATUS_INVALID_DATA - TKIP Key must be 256 bits\n"));
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "return- TKIP Key must be 256 bits!\n");
-        return -EINVAL;
-    }
-    // Check AES key length
-    if ((byKeyDecMode == KEY_CTL_CCMP) &&
-        (param->u.wpa_key.key_len != AES_KEY_LEN)) {
-        // AES Key must be 128 bits
-        //DBG_PRN_WLAN03(("return NDIS_STATUS_INVALID_DATA - AES Key must be 128 bits\n"));
-        return -EINVAL;
-    }
-
-   // spin_lock_irq(&pDevice->lock);
-    if (is_broadcast_ether_addr(&param->addr[0]) || (param->addr == NULL)) {
-        // If is_broadcast_ether_addr, set the key as every key entry's group key.
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Groupe Key Assign.\n");
-
-        if ((KeybSetAllGroupKey(&(pDevice->sKey),
-                            dwKeyIndex,
-                            param->u.wpa_key.key_len,
-                            (PQWORD) &(KeyRSC),
-                            (unsigned char *)abyKey,
-                            byKeyDecMode,
-                            pDevice->PortOffset,
-                            pDevice->byLocalID) == true) &&
-            (KeybSetDefaultKey(&(pDevice->sKey),
-                            dwKeyIndex,
-                            param->u.wpa_key.key_len,
-                            (PQWORD) &(KeyRSC),
-                            (unsigned char *)abyKey,
-                            byKeyDecMode,
-                            pDevice->PortOffset,
-                            pDevice->byLocalID) == true) ) {
-             DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "GROUP Key Assign.\n");
-
-        } else {
-            //DBG_PRN_WLAN03(("return NDIS_STATUS_INVALID_DATA -KeybSetDefaultKey Fail.0\n"));
-           // spin_unlock_irq(&pDevice->lock);
-            return -EINVAL;
-        }
-
-    } else {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Pairwise Key Assign.\n");
-        // BSSID not 0xffffffffffff
-        // Pairwise Key can't be WEP
-        if (byKeyDecMode == KEY_CTL_WEP) {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Pairwise Key can't be WEP\n");
-            //spin_unlock_irq(&pDevice->lock);
-            return -EINVAL;
-        }
-
-        dwKeyIndex |= (1 << 30); // set pairwise key
-        if (pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA) {
-            //DBG_PRN_WLAN03(("return NDIS_STATUS_INVALID_DATA - WMAC_CONFIG_IBSS_STA\n"));
-            //spin_unlock_irq(&pDevice->lock);
-            return -EINVAL;
-        }
-        if (KeybSetKey(&(pDevice->sKey),
-                       &param->addr[0],
-                       dwKeyIndex,
-                       param->u.wpa_key.key_len,
-                       (PQWORD) &(KeyRSC),
-                       (unsigned char *)abyKey,
-                        byKeyDecMode,
-                        pDevice->PortOffset,
-                        pDevice->byLocalID) == true) {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Pairwise Key Set\n");
-
-        } else {
-            // Key Table Full
-            if (!compare_ether_addr(&param->addr[0], pDevice->abyBSSID)) {
-                //DBG_PRN_WLAN03(("return NDIS_STATUS_INVALID_DATA -Key Table Full.2\n"));
-                //spin_unlock_irq(&pDevice->lock);
-                return -EINVAL;
-
-            } else {
-                // Save Key and configure just before associate/reassociate to BSSID
-                // we do not implement now
-                //spin_unlock_irq(&pDevice->lock);
-                return -EINVAL;
-            }
-        }
-    } // BSSID not 0xffffffffffff
-    if ((ret == 0) && ((param->u.wpa_key.set_tx) != 0)) {
-        pDevice->byKeyIndex = (unsigned char)param->u.wpa_key.key_index;
-        pDevice->bTransmitKey = true;
-    }
-    pDevice->bEncryptionEnable = true;
-    //spin_unlock_irq(&pDevice->lock);
+		dwKeyIndex |= (1 << 30); // set pairwise key
+		if (pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA) {
+			//DBG_PRN_WLAN03(("return NDIS_STATUS_INVALID_DATA - WMAC_CONFIG_IBSS_STA\n"));
+			//spin_unlock_irq(&pDevice->lock);
+			return -EINVAL;
+		}
+		if (KeybSetKey(&(pDevice->sKey),
+			       &param->addr[0],
+			       dwKeyIndex,
+			       param->u.wpa_key.key_len,
+			       (PQWORD) &(KeyRSC),
+			       (unsigned char *)abyKey,
+			       byKeyDecMode,
+			       pDevice->PortOffset,
+			       pDevice->byLocalID) == true) {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Pairwise Key Set\n");
+
+		} else {
+			// Key Table Full
+			if (!compare_ether_addr(&param->addr[0], pDevice->abyBSSID)) {
+				//DBG_PRN_WLAN03(("return NDIS_STATUS_INVALID_DATA -Key Table Full.2\n"));
+				//spin_unlock_irq(&pDevice->lock);
+				return -EINVAL;
+
+			} else {
+				// Save Key and configure just before associate/reassociate to BSSID
+				// we do not implement now
+				//spin_unlock_irq(&pDevice->lock);
+				return -EINVAL;
+			}
+		}
+	} // BSSID not 0xffffffffffff
+	if ((ret == 0) && ((param->u.wpa_key.set_tx) != 0)) {
+		pDevice->byKeyIndex = (unsigned char)param->u.wpa_key.key_index;
+		pDevice->bTransmitKey = true;
+	}
+	pDevice->bEncryptionEnable = true;
+	//spin_unlock_irq(&pDevice->lock);
 
 /*
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " key=%x-%x-%x-%x-%x-xxxxx \n",
-               pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[byKeyIndex][0],
-               pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[byKeyIndex][1],
-               pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[byKeyIndex][2],
-               pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[byKeyIndex][3],
-               pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[byKeyIndex][4]
-              );
+  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " key=%x-%x-%x-%x-%x-xxxxx \n",
+  pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[byKeyIndex][0],
+  pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[byKeyIndex][1],
+  pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[byKeyIndex][2],
+  pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[byKeyIndex][3],
+  pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[byKeyIndex][4]
+);
 */
 
 	return ret;
-
 }
 
-
 /*
  * Description:
  *      enable wpa auth & mode
@@ -460,22 +442,18 @@ spin_lock_irq(&pDevice->lock);
  */
 
 static int wpa_set_wpa(PSDevice pDevice,
-				     struct viawget_wpa_param *param)
+		       struct viawget_wpa_param *param)
 {
-
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	int ret = 0;
 
-    pMgmt->eAuthenMode = WMAC_AUTH_OPEN;
-    pMgmt->bShareKeyAlgorithm = false;
+	pMgmt->eAuthenMode = WMAC_AUTH_OPEN;
+	pMgmt->bShareKeyAlgorithm = false;
 
-    return ret;
+	return ret;
 }
 
-
-
-
- /*
+/*
  * Description:
  *      set disassociate
  *
@@ -490,23 +468,21 @@ static int wpa_set_wpa(PSDevice pDevice,
  */
 
 static int wpa_set_disassociate(PSDevice pDevice,
-				     struct viawget_wpa_param *param)
+				struct viawget_wpa_param *param)
 {
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	int ret = 0;
 
-    spin_lock_irq(&pDevice->lock);
-    if (pDevice->bLinkPass) {
-        if (!memcmp(param->addr, pMgmt->abyCurrBSSID, 6))
-            bScheduleCommand((void *)pDevice, WLAN_CMD_DISASSOCIATE, NULL);
-    }
-    spin_unlock_irq(&pDevice->lock);
+	spin_lock_irq(&pDevice->lock);
+	if (pDevice->bLinkPass) {
+		if (!memcmp(param->addr, pMgmt->abyCurrBSSID, 6))
+			bScheduleCommand((void *)pDevice, WLAN_CMD_DISASSOCIATE, NULL);
+	}
+	spin_unlock_irq(&pDevice->lock);
 
-    return ret;
+	return ret;
 }
 
-
-
 /*
  * Description:
  *      enable scan process
@@ -522,20 +498,18 @@ static int wpa_set_disassociate(PSDevice pDevice,
  */
 
 static int wpa_set_scan(PSDevice pDevice,
-				     struct viawget_wpa_param *param)
+			struct viawget_wpa_param *param)
 {
 	int ret = 0;
 
-    spin_lock_irq(&pDevice->lock);
-    BSSvClearBSSList((void *)pDevice, pDevice->bLinkPass);
-    bScheduleCommand((void *) pDevice, WLAN_CMD_BSSID_SCAN, NULL);
-    spin_unlock_irq(&pDevice->lock);
+	spin_lock_irq(&pDevice->lock);
+	BSSvClearBSSList((void *)pDevice, pDevice->bLinkPass);
+	bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, NULL);
+	spin_unlock_irq(&pDevice->lock);
 
-    return ret;
+	return ret;
 }
 
-
-
 /*
  * Description:
  *      get bssid
@@ -551,18 +525,16 @@ static int wpa_set_scan(PSDevice pDevice,
  */
 
 static int wpa_get_bssid(PSDevice pDevice,
-				     struct viawget_wpa_param *param)
+			 struct viawget_wpa_param *param)
 {
-    PSMgmtObject        pMgmt = pDevice->pMgmt;
+	PSMgmtObject        pMgmt = pDevice->pMgmt;
 	int ret = 0;
 
 	memcpy(param->u.wpa_associate.bssid, pMgmt->abyCurrBSSID , 6);
 
-    return ret;
-
+	return ret;
 }
 
-
 /*
  * Description:
  *      get bssid
@@ -578,22 +550,20 @@ static int wpa_get_bssid(PSDevice pDevice,
  */
 
 static int wpa_get_ssid(PSDevice pDevice,
-				     struct viawget_wpa_param *param)
+			struct viawget_wpa_param *param)
 {
-    PSMgmtObject        pMgmt = pDevice->pMgmt;
+	PSMgmtObject        pMgmt = pDevice->pMgmt;
 	PWLAN_IE_SSID       pItemSSID;
 	int ret = 0;
 
-    pItemSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
+	pItemSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
 
 	memcpy(param->u.wpa_associate.ssid, pItemSSID->abySSID , pItemSSID->len);
 	param->u.wpa_associate.ssid_len = pItemSSID->len;
 
-    return ret;
+	return ret;
 }
 
-
-
 /*
  * Description:
  *      get scan results
@@ -609,132 +579,112 @@ static int wpa_get_ssid(PSDevice pDevice,
  */
 
 static int wpa_get_scan(PSDevice pDevice,
-				     struct viawget_wpa_param *param)
+			struct viawget_wpa_param *param)
 {
 	struct viawget_scan_result *scan_buf;
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    PWLAN_IE_SSID   pItemSSID;
-    PKnownBSS pBSS;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	PWLAN_IE_SSID   pItemSSID;
+	PKnownBSS pBSS;
 	unsigned char *pBuf;
 	int ret = 0;
 	u16 count = 0;
 	u16 ii, jj;
 #if 1
 
-    unsigned char *ptempBSS;
-
-
-
-    ptempBSS = kmalloc(sizeof(KnownBSS), (int)GFP_ATOMIC);
-
-    if (ptempBSS == NULL) {
-
-       printk("bubble sort kmalloc memory fail@@@\n");
+	unsigned char *ptempBSS;
 
-        ret = -ENOMEM;
+	ptempBSS = kmalloc(sizeof(KnownBSS), (int)GFP_ATOMIC);
 
-        return ret;
+	if (ptempBSS == NULL) {
+		printk("bubble sort kmalloc memory fail@@@\n");
 
-    }
+		ret = -ENOMEM;
 
-    for (ii = 0; ii < MAX_BSS_NUM; ii++) {
+		return ret;
 
-         for(jj=0;jj<MAX_BSS_NUM-ii-1;jj++) {
-
-           if((pMgmt->sBSSList[jj].bActive!=true) ||
-
-                ((pMgmt->sBSSList[jj].uRSSI>pMgmt->sBSSList[jj+1].uRSSI) &&(pMgmt->sBSSList[jj+1].bActive!=false))) {
-
-                 memcpy(ptempBSS,&pMgmt->sBSSList[jj],sizeof(KnownBSS));
-
-                 memcpy(&pMgmt->sBSSList[jj],&pMgmt->sBSSList[jj+1],sizeof(KnownBSS));
-
-                 memcpy(&pMgmt->sBSSList[jj+1],ptempBSS,sizeof(KnownBSS));
-
-              }
+	}
 
-         }
+	for (ii = 0; ii < MAX_BSS_NUM; ii++) {
+		for (jj = 0; jj < MAX_BSS_NUM - ii - 1; jj++) {
+			if ((pMgmt->sBSSList[jj].bActive != true) ||
 
-    }
+			    ((pMgmt->sBSSList[jj].uRSSI > pMgmt->sBSSList[jj + 1].uRSSI) && (pMgmt->sBSSList[jj + 1].bActive != false))) {
+				memcpy(ptempBSS, &pMgmt->sBSSList[jj], sizeof(KnownBSS));
 
-  kfree(ptempBSS);
+				memcpy(&pMgmt->sBSSList[jj], &pMgmt->sBSSList[jj + 1], sizeof(KnownBSS));
 
- // printk("bubble sort result:\n");
+				memcpy(&pMgmt->sBSSList[jj + 1], ptempBSS, sizeof(KnownBSS));
 
-  //for (ii = 0; ii < MAX_BSS_NUM; ii++)
+			}
 
-  //    printk("%d [%s]:RSSI=%d\n",ii,((PWLAN_IE_SSID)(pMgmt->sBSSList[ii].abySSID))->abySSID,
+		}
 
-  //                                                                 pMgmt->sBSSList[ii].uRSSI);
+	}
 
- #endif
+	kfree(ptempBSS);
+#endif
 
 //******mike:bubble sort by stronger RSSI*****//
 
-
-
-
 	count = 0;
 	pBSS = &(pMgmt->sBSSList[0]);
-    for (ii = 0; ii < MAX_BSS_NUM; ii++) {
-        pBSS = &(pMgmt->sBSSList[ii]);
-        if (!pBSS->bActive)
-            continue;
-        count++;
-    }
-
-    pBuf = kcalloc(count, sizeof(struct viawget_scan_result), (int)GFP_ATOMIC);
-
-    if (pBuf == NULL) {
-        ret = -ENOMEM;
-        return ret;
-    }
-    scan_buf = (struct viawget_scan_result *)pBuf;
+	for (ii = 0; ii < MAX_BSS_NUM; ii++) {
+		pBSS = &(pMgmt->sBSSList[ii]);
+		if (!pBSS->bActive)
+			continue;
+		count++;
+	}
+
+	pBuf = kcalloc(count, sizeof(struct viawget_scan_result), (int)GFP_ATOMIC);
+
+	if (pBuf == NULL) {
+		ret = -ENOMEM;
+		return ret;
+	}
+	scan_buf = (struct viawget_scan_result *)pBuf;
 	pBSS = &(pMgmt->sBSSList[0]);
-    for (ii = 0, jj = 0; ii < MAX_BSS_NUM ; ii++) {
-        pBSS = &(pMgmt->sBSSList[ii]);
-        if (pBSS->bActive) {
-            if (jj >= count)
-                break;
-            memcpy(scan_buf->bssid, pBSS->abyBSSID, WLAN_BSSID_LEN);
-            pItemSSID = (PWLAN_IE_SSID)pBSS->abySSID;
-   		    memcpy(scan_buf->ssid, pItemSSID->abySSID, pItemSSID->len);
-   		    scan_buf->ssid_len = pItemSSID->len;
-            scan_buf->freq = frequency_list[pBSS->uChannel-1];
-	  scan_buf->caps = pBSS->wCapInfo;
-            //scan_buf->caps = pBSS->wCapInfo;
-            //scan_buf->qual =
-            //scan_buf->noise =
-            //scan_buf->level =
-            //scan_buf->maxrate =
-            if (pBSS->wWPALen != 0) {
-                scan_buf->wpa_ie_len = pBSS->wWPALen;
-                memcpy(scan_buf->wpa_ie, pBSS->byWPAIE, pBSS->wWPALen);
-            }
-            if (pBSS->wRSNLen != 0) {
-                scan_buf->rsn_ie_len = pBSS->wRSNLen;
-                memcpy(scan_buf->rsn_ie, pBSS->byRSNIE, pBSS->wRSNLen);
-            }
-            scan_buf = (struct viawget_scan_result *)((unsigned char *)scan_buf + sizeof(struct viawget_scan_result));
-            jj ++;
-        }
-    }
-
-    if (jj < count)
-        count = jj;
-
-    if (copy_to_user(param->u.scan_results.buf, pBuf, sizeof(struct viawget_scan_result) * count)) {
+	for (ii = 0, jj = 0; ii < MAX_BSS_NUM; ii++) {
+		pBSS = &(pMgmt->sBSSList[ii]);
+		if (pBSS->bActive) {
+			if (jj >= count)
+				break;
+			memcpy(scan_buf->bssid, pBSS->abyBSSID, WLAN_BSSID_LEN);
+			pItemSSID = (PWLAN_IE_SSID)pBSS->abySSID;
+			memcpy(scan_buf->ssid, pItemSSID->abySSID, pItemSSID->len);
+			scan_buf->ssid_len = pItemSSID->len;
+			scan_buf->freq = frequency_list[pBSS->uChannel-1];
+			scan_buf->caps = pBSS->wCapInfo;
+			//scan_buf->caps = pBSS->wCapInfo;
+			//scan_buf->qual =
+			//scan_buf->noise =
+			//scan_buf->level =
+			//scan_buf->maxrate =
+			if (pBSS->wWPALen != 0) {
+				scan_buf->wpa_ie_len = pBSS->wWPALen;
+				memcpy(scan_buf->wpa_ie, pBSS->byWPAIE, pBSS->wWPALen);
+			}
+			if (pBSS->wRSNLen != 0) {
+				scan_buf->rsn_ie_len = pBSS->wRSNLen;
+				memcpy(scan_buf->rsn_ie, pBSS->byRSNIE, pBSS->wRSNLen);
+			}
+			scan_buf = (struct viawget_scan_result *)((unsigned char *)scan_buf + sizeof(struct viawget_scan_result));
+			jj++;
+		}
+	}
+
+	if (jj < count)
+		count = jj;
+
+	if (copy_to_user(param->u.scan_results.buf, pBuf, sizeof(struct viawget_scan_result) * count)) {
 		ret = -EFAULT;
 	}
 	param->u.scan_results.scan_count = count;
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " param->u.scan_results.scan_count = %d\n", count)
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " param->u.scan_results.scan_count = %d\n", count)
 
-    kfree(pBuf);
-    return ret;
+		kfree(pBuf);
+	return ret;
 }
 
-
-
 /*
  * Description:
  *      set associate with AP
@@ -750,23 +700,22 @@ static int wpa_get_scan(PSDevice pDevice,
  */
 
 static int wpa_set_associate(PSDevice pDevice,
-				     struct viawget_wpa_param *param)
+			     struct viawget_wpa_param *param)
 {
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    PWLAN_IE_SSID   pItemSSID;
-    unsigned char abyNullAddr[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
-    unsigned char abyWPAIE[64];
-    int ret = 0;
-    bool bWepEnabled=false;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	PWLAN_IE_SSID   pItemSSID;
+	unsigned char abyNullAddr[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+	unsigned char abyWPAIE[64];
+	int ret = 0;
+	bool bWepEnabled = false;
 
 	// set key type & algorithm
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pairwise_suite = %d\n", param->u.wpa_associate.pairwise_suite);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "group_suite = %d\n", param->u.wpa_associate.group_suite);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "key_mgmt_suite = %d\n", param->u.wpa_associate.key_mgmt_suite);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "auth_alg = %d\n", param->u.wpa_associate.auth_alg);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "mode = %d\n", param->u.wpa_associate.mode);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wpa_ie_len = %d\n", param->u.wpa_associate.wpa_ie_len);
-
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pairwise_suite = %d\n", param->u.wpa_associate.pairwise_suite);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "group_suite = %d\n", param->u.wpa_associate.group_suite);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "key_mgmt_suite = %d\n", param->u.wpa_associate.key_mgmt_suite);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "auth_alg = %d\n", param->u.wpa_associate.auth_alg);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "mode = %d\n", param->u.wpa_associate.mode);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wpa_ie_len = %d\n", param->u.wpa_associate.wpa_ie_len);
 
 	if (param->u.wpa_associate.wpa_ie_len) {
 		if (!param->u.wpa_associate.wpa_ie)
@@ -778,28 +727,27 @@ static int wpa_set_associate(PSDevice pDevice,
 	}
 
 	if (param->u.wpa_associate.mode == 1)
-	    pMgmt->eConfigMode = WMAC_CONFIG_IBSS_STA;
+		pMgmt->eConfigMode = WMAC_CONFIG_IBSS_STA;
 	else
-	    pMgmt->eConfigMode = WMAC_CONFIG_ESS_STA;
-    // set ssid
+		pMgmt->eConfigMode = WMAC_CONFIG_ESS_STA;
+	// set ssid
 	memset(pMgmt->abyDesireSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-    pItemSSID = (PWLAN_IE_SSID)pMgmt->abyDesireSSID;
-    pItemSSID->byElementID = WLAN_EID_SSID;
+	pItemSSID = (PWLAN_IE_SSID)pMgmt->abyDesireSSID;
+	pItemSSID->byElementID = WLAN_EID_SSID;
 	pItemSSID->len = param->u.wpa_associate.ssid_len;
 	memcpy(pItemSSID->abySSID, param->u.wpa_associate.ssid, pItemSSID->len);
 	// set bssid
-    if (memcmp(param->u.wpa_associate.bssid, &abyNullAddr[0], 6) != 0)
-        memcpy(pMgmt->abyDesireBSSID, param->u.wpa_associate.bssid, 6);
-else
-{
-   bScheduleCommand((void *) pDevice, WLAN_CMD_BSSID_SCAN, pItemSSID->abySSID);
-}
+	if (memcmp(param->u.wpa_associate.bssid, &abyNullAddr[0], 6) != 0)
+		memcpy(pMgmt->abyDesireBSSID, param->u.wpa_associate.bssid, 6);
+	else {
+		bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, pItemSSID->abySSID);
+	}
 
-    if (param->u.wpa_associate.wpa_ie_len == 0) {
-	    if (param->u.wpa_associate.auth_alg & AUTH_ALG_SHARED_KEY)
-            pMgmt->eAuthenMode = WMAC_AUTH_SHAREKEY;
-	    else
-            pMgmt->eAuthenMode = WMAC_AUTH_OPEN;
+	if (param->u.wpa_associate.wpa_ie_len == 0) {
+		if (param->u.wpa_associate.auth_alg & AUTH_ALG_SHARED_KEY)
+			pMgmt->eAuthenMode = WMAC_AUTH_SHAREKEY;
+		else
+			pMgmt->eAuthenMode = WMAC_AUTH_OPEN;
 	} else if (abyWPAIE[0] == RSN_INFO_ELEM) {
 		if (param->u.wpa_associate.key_mgmt_suite == KEY_MGMT_PSK)
 			pMgmt->eAuthenMode = WMAC_AUTH_WPA2PSK;
@@ -809,9 +757,9 @@ else
 		if (param->u.wpa_associate.key_mgmt_suite == KEY_MGMT_WPA_NONE)
 			pMgmt->eAuthenMode = WMAC_AUTH_WPANONE;
 		else if (param->u.wpa_associate.key_mgmt_suite == KEY_MGMT_PSK)
-		    pMgmt->eAuthenMode = WMAC_AUTH_WPAPSK;
+			pMgmt->eAuthenMode = WMAC_AUTH_WPAPSK;
 		else
-		    pMgmt->eAuthenMode = WMAC_AUTH_WPA;
+			pMgmt->eAuthenMode = WMAC_AUTH_WPA;
 	}
 
 	switch (param->u.wpa_associate.pairwise_suite) {
@@ -824,7 +772,7 @@ else
 	case CIPHER_WEP40:
 	case CIPHER_WEP104:
 		pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
-		bWepEnabled=true;
+		bWepEnabled = true;
 		break;
 	case CIPHER_NONE:
 		if (param->u.wpa_associate.group_suite == CIPHER_CCMP)
@@ -838,55 +786,53 @@ else
 
 //DavidWang add for WPA_supplicant support open/share mode
 
-      if (pMgmt->eAuthenMode == WMAC_AUTH_SHAREKEY) {
-            pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
-            //pMgmt->eAuthenMode = WMAC_AUTH_SHAREKEY;
-            pMgmt->bShareKeyAlgorithm = true;
-             }
-     else if (pMgmt->eAuthenMode == WMAC_AUTH_OPEN) {
-          if(!bWepEnabled)  pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
-	else pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
-            //pMgmt->eAuthenMode = WMAC_AUTH_OPEN;
-            //pMgmt->bShareKeyAlgorithm = false; //20080717-06,<Modify> by chester//Fix Open mode, WEP encryption
-           }
+	if (pMgmt->eAuthenMode == WMAC_AUTH_SHAREKEY) {
+		pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
+		//pMgmt->eAuthenMode = WMAC_AUTH_SHAREKEY;
+		pMgmt->bShareKeyAlgorithm = true;
+	} else if (pMgmt->eAuthenMode == WMAC_AUTH_OPEN) {
+		if (!bWepEnabled)  pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
+		else pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
+		//pMgmt->eAuthenMode = WMAC_AUTH_OPEN;
+		//pMgmt->bShareKeyAlgorithm = false; //20080717-06,<Modify> by chester//Fix Open mode, WEP encryption
+	}
 //mike save old encryption status
 	pDevice->eOldEncryptionStatus = pDevice->eEncryptionStatus;
 
-    if (pDevice->eEncryptionStatus !=  Ndis802_11EncryptionDisabled)
-        pDevice->bEncryptionEnable = true;
-    else
-        pDevice->bEncryptionEnable = false;
-if (!((pMgmt->eAuthenMode == WMAC_AUTH_SHAREKEY) ||
-      ((pMgmt->eAuthenMode == WMAC_AUTH_OPEN) && (bWepEnabled==true))) )  //DavidWang  //20080717-06,<Modify> by chester//Not to initial WEP
-    KeyvInitTable(&pDevice->sKey, pDevice->PortOffset);
-    spin_lock_irq(&pDevice->lock);
-    pDevice->bLinkPass = false;
-    memset(pMgmt->abyCurrBSSID, 0, 6);
-    pMgmt->eCurrState = WMAC_STATE_IDLE;
-    netif_stop_queue(pDevice->dev);
+	if (pDevice->eEncryptionStatus !=  Ndis802_11EncryptionDisabled)
+		pDevice->bEncryptionEnable = true;
+	else
+		pDevice->bEncryptionEnable = false;
+	if (!((pMgmt->eAuthenMode == WMAC_AUTH_SHAREKEY) ||
+	      ((pMgmt->eAuthenMode == WMAC_AUTH_OPEN) && (bWepEnabled == true))))  //DavidWang  //20080717-06,<Modify> by chester//Not to initial WEP
+		KeyvInitTable(&pDevice->sKey, pDevice->PortOffset);
+	spin_lock_irq(&pDevice->lock);
+	pDevice->bLinkPass = false;
+	memset(pMgmt->abyCurrBSSID, 0, 6);
+	pMgmt->eCurrState = WMAC_STATE_IDLE;
+	netif_stop_queue(pDevice->dev);
 	//20080701-02,<Add> by Mike Liu
 /*******search if ap_scan=2 ,which is associating request in hidden ssid mode ****/
-{
-   PKnownBSS       pCurr = NULL;
-    pCurr = BSSpSearchBSSList(pDevice,
-                              pMgmt->abyDesireBSSID,
-                              pMgmt->abyDesireSSID,
-                              pMgmt->eConfigPHYMode
-                              );
-
-    if (pCurr == NULL){
-    printk("wpa_set_associate---->hidden mode site survey before associate.......\n");
-    bScheduleCommand((void *) pDevice, WLAN_CMD_BSSID_SCAN, pMgmt->abyDesireSSID);
-  }
-}
+	{
+		PKnownBSS       pCurr = NULL;
+		pCurr = BSSpSearchBSSList(pDevice,
+					  pMgmt->abyDesireBSSID,
+					  pMgmt->abyDesireSSID,
+					  pMgmt->eConfigPHYMode
+);
+
+		if (pCurr == NULL) {
+			printk("wpa_set_associate---->hidden mode site survey before associate.......\n");
+			bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, pMgmt->abyDesireSSID);
+		}
+	}
 /****************************************************************/
-    bScheduleCommand((void *) pDevice, WLAN_CMD_SSID, NULL);
-    spin_unlock_irq(&pDevice->lock);
+	bScheduleCommand((void *)pDevice, WLAN_CMD_SSID, NULL);
+	spin_unlock_irq(&pDevice->lock);
 
-    return ret;
+	return ret;
 }
 
-
 /*
  * Description:
  *      wpa_ioctl main function supported for wpa supplicant
@@ -922,61 +868,61 @@ int wpa_ioctl(PSDevice pDevice, struct iw_point *p)
 
 	switch (param->cmd) {
 	case VIAWGET_SET_WPA:
-        ret = wpa_set_wpa(pDevice, param);
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_SET_WPA \n");
+		ret = wpa_set_wpa(pDevice, param);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_SET_WPA \n");
 		break;
 
 	case VIAWGET_SET_KEY:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_SET_KEY \n");
-	    spin_lock_irq(&pDevice->lock);
-        ret = wpa_set_keys(pDevice, param, false);
-        spin_unlock_irq(&pDevice->lock);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_SET_KEY \n");
+		spin_lock_irq(&pDevice->lock);
+		ret = wpa_set_keys(pDevice, param, false);
+		spin_unlock_irq(&pDevice->lock);
 		break;
 
 	case VIAWGET_SET_SCAN:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_SET_SCAN \n");
-        ret = wpa_set_scan(pDevice, param);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_SET_SCAN \n");
+		ret = wpa_set_scan(pDevice, param);
 		break;
 
 	case VIAWGET_GET_SCAN:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_GET_SCAN\n");
-        ret = wpa_get_scan(pDevice, param);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_GET_SCAN\n");
+		ret = wpa_get_scan(pDevice, param);
 		wpa_ioctl = 1;
 		break;
 
 	case VIAWGET_GET_SSID:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_GET_SSID \n");
-        ret = wpa_get_ssid(pDevice, param);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_GET_SSID \n");
+		ret = wpa_get_ssid(pDevice, param);
 		wpa_ioctl = 1;
 		break;
 
 	case VIAWGET_GET_BSSID:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_GET_BSSID \n");
-        ret = wpa_get_bssid(pDevice, param);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_GET_BSSID \n");
+		ret = wpa_get_bssid(pDevice, param);
 		wpa_ioctl = 1;
 		break;
 
 	case VIAWGET_SET_ASSOCIATE:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_SET_ASSOCIATE \n");
-        ret = wpa_set_associate(pDevice, param);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_SET_ASSOCIATE \n");
+		ret = wpa_set_associate(pDevice, param);
 		break;
 
 	case VIAWGET_SET_DISASSOCIATE:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_SET_DISASSOCIATE \n");
-        ret = wpa_set_disassociate(pDevice, param);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_SET_DISASSOCIATE \n");
+		ret = wpa_set_disassociate(pDevice, param);
 		break;
 
 	case VIAWGET_SET_DROP_UNENCRYPT:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_SET_DROP_UNENCRYPT \n");
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_SET_DROP_UNENCRYPT \n");
 		break;
 
-    case VIAWGET_SET_DEAUTHENTICATE:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_SET_DEAUTHENTICATE \n");
+	case VIAWGET_SET_DEAUTHENTICATE:
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_SET_DEAUTHENTICATE \n");
 		break;
 
 	default:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wpa_ioctl: unknown cmd=%d\n",
-		       param->cmd);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wpa_ioctl: unknown cmd=%d\n",
+			param->cmd);
 		return -EOPNOTSUPP;
 		break;
 	}
@@ -993,4 +939,3 @@ out:
 
 	return ret;
 }
-
diff --git a/drivers/staging/vt6655/wpactl.h b/drivers/staging/vt6655/wpactl.h
index dbe8e861d99..b9e2ab231f1 100644
--- a/drivers/staging/vt6655/wpactl.h
+++ b/drivers/staging/vt6655/wpactl.h
@@ -36,13 +36,12 @@
 
 /*---------------------  Export Definitions -------------------------*/
 
-
 //WPA related
 
 typedef enum { WPA_ALG_NONE, WPA_ALG_WEP, WPA_ALG_TKIP, WPA_ALG_CCMP } wpa_alg;
 typedef enum { CIPHER_NONE, CIPHER_WEP40, CIPHER_TKIP, CIPHER_CCMP,
 	       CIPHER_WEP104 } wpa_cipher;
-typedef enum { KEY_MGMT_802_1X, KEY_MGMT_CCKM,KEY_MGMT_PSK, KEY_MGMT_NONE,
+typedef enum { KEY_MGMT_802_1X, KEY_MGMT_CCKM, KEY_MGMT_PSK, KEY_MGMT_NONE,
 	       KEY_MGMT_802_1X_NO_WPA, KEY_MGMT_WPA_NONE } wpa_key_mgmt;
 
 #define AUTH_ALG_OPEN_SYSTEM	0x01
@@ -52,8 +51,6 @@ typedef enum { KEY_MGMT_802_1X, KEY_MGMT_CCKM,KEY_MGMT_PSK, KEY_MGMT_NONE,
 #define GENERIC_INFO_ELEM 0xdd
 #define RSN_INFO_ELEM 0x30
 
-
-
 typedef unsigned long long   NDIS_802_11_KEY_RSC;
 
 /*---------------------  Export Classes  ----------------------------*/
@@ -67,6 +64,3 @@ int wpa_ioctl(PSDevice pDevice, struct iw_point *p);
 int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel);
 
 #endif // __WPACL_H__
-
-
-
diff --git a/drivers/staging/vt6655/wroute.c b/drivers/staging/vt6655/wroute.c
index 82e93cb8205..b61328fbee8 100644
--- a/drivers/staging/vt6655/wroute.c
+++ b/drivers/staging/vt6655/wroute.c
@@ -43,14 +43,12 @@
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
-static int          msglevel                =MSG_LEVEL_INFO;
+static int msglevel = MSG_LEVEL_INFO;
 //static int          msglevel                =MSG_LEVEL_DEBUG;
 /*---------------------  Static Functions  --------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
 
-
-
 /*
  * Description:
  *      Relay packet.  Return true if packet is copy to DMA1
@@ -65,135 +63,128 @@ static int          msglevel                =MSG_LEVEL_INFO;
  * Return Value: true if packet duplicate; otherwise false
  *
  */
-bool ROUTEbRelay (PSDevice pDevice, unsigned char *pbySkbData, unsigned int uDataLen, unsigned int uNodeIndex)
+bool ROUTEbRelay(PSDevice pDevice, unsigned char *pbySkbData, unsigned int uDataLen, unsigned int uNodeIndex)
 {
-    PSMgmtObject    pMgmt = pDevice->pMgmt;
-    PSTxDesc        pHeadTD, pLastTD;
-    unsigned int cbFrameBodySize;
-    unsigned int uMACfragNum;
-    unsigned char byPktType;
-    bool bNeedEncryption = false;
-    SKeyItem        STempKey;
-    PSKeyItem       pTransmitKey = NULL;
-    unsigned int cbHeaderSize;
-    unsigned int ii;
-    unsigned char *pbyBSSID;
-
-
-
-
-    if (AVAIL_TD(pDevice, TYPE_AC0DMA)<=0) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Relay can't allocate TD1..\n");
-        return false;
-    }
-
-    pHeadTD = pDevice->apCurrTD[TYPE_AC0DMA];
-
-    pHeadTD->m_td1TD1.byTCR = (TCR_EDP|TCR_STP);
-
-    memcpy(pDevice->sTxEthHeader.abyDstAddr, (unsigned char *)pbySkbData, ETH_HLEN);
-
-    cbFrameBodySize = uDataLen - ETH_HLEN;
-
-    if (ntohs(pDevice->sTxEthHeader.wType) > ETH_DATA_LEN) {
-        cbFrameBodySize += 8;
-    }
-
-    if (pDevice->bEncryptionEnable == true) {
-        bNeedEncryption = true;
-
-        // get group key
-        pbyBSSID = pDevice->abyBroadcastAddr;
-        if(KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, GROUP_KEY, &pTransmitKey) == false) {
-            pTransmitKey = NULL;
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG"KEY is NULL. [%d]\n", pDevice->pMgmt->eCurrMode);
-        } else {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG"Get GTK.\n");
-        }
-    }
-
-    if (pDevice->bEnableHostWEP) {
-	if (uNodeIndex < MAX_NODE_NUM + 1) {
-            pTransmitKey = &STempKey;
-            pTransmitKey->byCipherSuite = pMgmt->sNodeDBTable[uNodeIndex].byCipherSuite;
-            pTransmitKey->dwKeyIndex = pMgmt->sNodeDBTable[uNodeIndex].dwKeyIndex;
-            pTransmitKey->uKeyLength = pMgmt->sNodeDBTable[uNodeIndex].uWepKeyLength;
-            pTransmitKey->dwTSC47_16 = pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16;
-            pTransmitKey->wTSC15_0 = pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0;
-            memcpy(pTransmitKey->abyKey,
-                &pMgmt->sNodeDBTable[uNodeIndex].abyWepKey[0],
-                pTransmitKey->uKeyLength
-                );
-        }
-    }
-
-    uMACfragNum = cbGetFragCount(pDevice, pTransmitKey, cbFrameBodySize, &pDevice->sTxEthHeader);
-
-    if (uMACfragNum > AVAIL_TD(pDevice,TYPE_AC0DMA)) {
-        return false;
-    }
-    byPktType = (unsigned char)pDevice->byPacketType;
-
-    if (pDevice->bFixRate) {
-        if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
-            if (pDevice->uConnectionRate >= RATE_11M) {
-                pDevice->wCurrentRate = RATE_11M;
-            } else {
-                pDevice->wCurrentRate = (unsigned short)pDevice->uConnectionRate;
-            }
-        } else {
-            if ((pDevice->eCurrentPHYType == PHY_TYPE_11A) &&
-                (pDevice->uConnectionRate <= RATE_6M)) {
-                pDevice->wCurrentRate = RATE_6M;
-            } else {
-                if (pDevice->uConnectionRate >= RATE_54M)
-                    pDevice->wCurrentRate = RATE_54M;
-                else
-                    pDevice->wCurrentRate = (unsigned short)pDevice->uConnectionRate;
-            }
-        }
-    }
-    else {
-        pDevice->wCurrentRate = pDevice->pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate;
-    }
-
-    if (pDevice->wCurrentRate <= RATE_11M)
-        byPktType = PK_TYPE_11B;
-
-    vGenerateFIFOHeader(pDevice, byPktType, pDevice->pbyTmpBuff, bNeedEncryption,
-                        cbFrameBodySize, TYPE_AC0DMA, pHeadTD,
-                        &pDevice->sTxEthHeader, pbySkbData, pTransmitKey, uNodeIndex,
-                        &uMACfragNum,
-                        &cbHeaderSize
-                        );
-
-    if (MACbIsRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PS)) {
-        // Disable PS
-        MACbPSWakeup(pDevice->PortOffset);
-    }
-
-    pDevice->bPWBitOn = false;
-
-    pLastTD = pHeadTD;
-    for (ii = 0; ii < uMACfragNum; ii++) {
-        // Poll Transmit the adapter
-        wmb();
-        pHeadTD->m_td0TD0.f1Owner=OWNED_BY_NIC;
-        wmb();
-        if (ii == (uMACfragNum - 1))
-            pLastTD = pHeadTD;
-        pHeadTD = pHeadTD->next;
-    }
-
-    pLastTD->pTDInfo->skb = 0;
-    pLastTD->pTDInfo->byFlags = 0;
-
-    pDevice->apCurrTD[TYPE_AC0DMA] = pHeadTD;
-
-    MACvTransmitAC0(pDevice->PortOffset);
-
-    return true;
+	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	PSTxDesc        pHeadTD, pLastTD;
+	unsigned int cbFrameBodySize;
+	unsigned int uMACfragNum;
+	unsigned char byPktType;
+	bool bNeedEncryption = false;
+	SKeyItem        STempKey;
+	PSKeyItem       pTransmitKey = NULL;
+	unsigned int cbHeaderSize;
+	unsigned int ii;
+	unsigned char *pbyBSSID;
+
+	if (AVAIL_TD(pDevice, TYPE_AC0DMA) <= 0) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Relay can't allocate TD1..\n");
+		return false;
+	}
+
+	pHeadTD = pDevice->apCurrTD[TYPE_AC0DMA];
+
+	pHeadTD->m_td1TD1.byTCR = (TCR_EDP | TCR_STP);
+
+	memcpy(pDevice->sTxEthHeader.abyDstAddr, (unsigned char *)pbySkbData, ETH_HLEN);
+
+	cbFrameBodySize = uDataLen - ETH_HLEN;
+
+	if (ntohs(pDevice->sTxEthHeader.wType) > ETH_DATA_LEN) {
+		cbFrameBodySize += 8;
+	}
+
+	if (pDevice->bEncryptionEnable == true) {
+		bNeedEncryption = true;
+
+		// get group key
+		pbyBSSID = pDevice->abyBroadcastAddr;
+		if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, GROUP_KEY, &pTransmitKey) == false) {
+			pTransmitKey = NULL;
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG "KEY is NULL. [%d]\n", pDevice->pMgmt->eCurrMode);
+		} else {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG "Get GTK.\n");
+		}
+	}
+
+	if (pDevice->bEnableHostWEP) {
+		if (uNodeIndex < MAX_NODE_NUM + 1) {
+			pTransmitKey = &STempKey;
+			pTransmitKey->byCipherSuite = pMgmt->sNodeDBTable[uNodeIndex].byCipherSuite;
+			pTransmitKey->dwKeyIndex = pMgmt->sNodeDBTable[uNodeIndex].dwKeyIndex;
+			pTransmitKey->uKeyLength = pMgmt->sNodeDBTable[uNodeIndex].uWepKeyLength;
+			pTransmitKey->dwTSC47_16 = pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16;
+			pTransmitKey->wTSC15_0 = pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0;
+			memcpy(pTransmitKey->abyKey,
+			       &pMgmt->sNodeDBTable[uNodeIndex].abyWepKey[0],
+			       pTransmitKey->uKeyLength
+);
+		}
+	}
+
+	uMACfragNum = cbGetFragCount(pDevice, pTransmitKey, cbFrameBodySize, &pDevice->sTxEthHeader);
+
+	if (uMACfragNum > AVAIL_TD(pDevice, TYPE_AC0DMA)) {
+		return false;
+	}
+	byPktType = (unsigned char)pDevice->byPacketType;
+
+	if (pDevice->bFixRate) {
+		if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
+			if (pDevice->uConnectionRate >= RATE_11M) {
+				pDevice->wCurrentRate = RATE_11M;
+			} else {
+				pDevice->wCurrentRate = (unsigned short)pDevice->uConnectionRate;
+			}
+		} else {
+			if ((pDevice->eCurrentPHYType == PHY_TYPE_11A) &&
+			    (pDevice->uConnectionRate <= RATE_6M)) {
+				pDevice->wCurrentRate = RATE_6M;
+			} else {
+				if (pDevice->uConnectionRate >= RATE_54M)
+					pDevice->wCurrentRate = RATE_54M;
+				else
+					pDevice->wCurrentRate = (unsigned short)pDevice->uConnectionRate;
+			}
+		}
+	} else {
+		pDevice->wCurrentRate = pDevice->pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate;
+	}
+
+	if (pDevice->wCurrentRate <= RATE_11M)
+		byPktType = PK_TYPE_11B;
+
+	vGenerateFIFOHeader(pDevice, byPktType, pDevice->pbyTmpBuff, bNeedEncryption,
+			    cbFrameBodySize, TYPE_AC0DMA, pHeadTD,
+			    &pDevice->sTxEthHeader, pbySkbData, pTransmitKey, uNodeIndex,
+			    &uMACfragNum,
+			    &cbHeaderSize
+);
+
+	if (MACbIsRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PS)) {
+		// Disable PS
+		MACbPSWakeup(pDevice->PortOffset);
+	}
+
+	pDevice->bPWBitOn = false;
+
+	pLastTD = pHeadTD;
+	for (ii = 0; ii < uMACfragNum; ii++) {
+		// Poll Transmit the adapter
+		wmb();
+		pHeadTD->m_td0TD0.f1Owner = OWNED_BY_NIC;
+		wmb();
+		if (ii == (uMACfragNum - 1))
+			pLastTD = pHeadTD;
+		pHeadTD = pHeadTD->next;
+	}
+
+	pLastTD->pTDInfo->skb = 0;
+	pLastTD->pTDInfo->byFlags = 0;
+
+	pDevice->apCurrTD[TYPE_AC0DMA] = pHeadTD;
+
+	MACvTransmitAC0(pDevice->PortOffset);
+
+	return true;
 }
-
-
-
diff --git a/drivers/staging/vt6655/wroute.h b/drivers/staging/vt6655/wroute.h
index 34f9e43a6cc..5ecc190ae77 100644
--- a/drivers/staging/vt6655/wroute.h
+++ b/drivers/staging/vt6655/wroute.h
@@ -39,9 +39,6 @@
 
 /*---------------------  Export Functions  --------------------------*/
 
-bool ROUTEbRelay (PSDevice pDevice, unsigned char *pbySkbData, unsigned int uDataLen, unsigned int uNodeIndex);
+bool ROUTEbRelay(PSDevice pDevice, unsigned char *pbySkbData, unsigned int uDataLen, unsigned int uNodeIndex);
 
 #endif // __WROUTE_H__
-
-
-
diff --git a/drivers/staging/vt6656/80211hdr.h b/drivers/staging/vt6656/80211hdr.h
index b87d5434077..000304ffad6 100644
--- a/drivers/staging/vt6656/80211hdr.h
+++ b/drivers/staging/vt6656/80211hdr.h
@@ -28,10 +28,6 @@
 #ifndef __80211HDR_H__
 #define __80211HDR_H__
 
-#include "ttype.h"
-
-/*---------------------  Export Definitions -------------------------*/
-
 /* bit type */
 #define BIT0	0x00000001
 #define BIT1	0x00000002
@@ -155,22 +151,22 @@
 #ifdef __BIG_ENDIAN
 
 /* GET & SET Frame Control bit */
-#define WLAN_GET_FC_PRVER(n)    ((((WORD)(n) >> 8) & (BIT0 | BIT1))
-#define WLAN_GET_FC_FTYPE(n)    ((((WORD)(n) >> 8) & (BIT2 | BIT3)) >> 2)
-#define WLAN_GET_FC_FSTYPE(n)   ((((WORD)(n) >> 8) \
+#define WLAN_GET_FC_PRVER(n)    ((((u16)(n) >> 8) & (BIT0 | BIT1))
+#define WLAN_GET_FC_FTYPE(n)    ((((u16)(n) >> 8) & (BIT2 | BIT3)) >> 2)
+#define WLAN_GET_FC_FSTYPE(n)   ((((u16)(n) >> 8) \
 				  & (BIT4|BIT5|BIT6|BIT7)) >> 4)
-#define WLAN_GET_FC_TODS(n)     ((((WORD)(n) << 8) & (BIT8)) >> 8)
-#define WLAN_GET_FC_FROMDS(n)   ((((WORD)(n) << 8) & (BIT9)) >> 9)
-#define WLAN_GET_FC_MOREFRAG(n) ((((WORD)(n) << 8) & (BIT10)) >> 10)
-#define WLAN_GET_FC_RETRY(n)    ((((WORD)(n) << 8) & (BIT11)) >> 11)
-#define WLAN_GET_FC_PWRMGT(n)   ((((WORD)(n) << 8) & (BIT12)) >> 12)
-#define WLAN_GET_FC_MOREDATA(n) ((((WORD)(n) << 8) & (BIT13)) >> 13)
-#define WLAN_GET_FC_ISWEP(n)    ((((WORD)(n) << 8) & (BIT14)) >> 14)
-#define WLAN_GET_FC_ORDER(n)    ((((WORD)(n) << 8) & (BIT15)) >> 15)
+#define WLAN_GET_FC_TODS(n)     ((((u16)(n) << 8) & (BIT8)) >> 8)
+#define WLAN_GET_FC_FROMDS(n)   ((((u16)(n) << 8) & (BIT9)) >> 9)
+#define WLAN_GET_FC_MOREFRAG(n) ((((u16)(n) << 8) & (BIT10)) >> 10)
+#define WLAN_GET_FC_RETRY(n)    ((((u16)(n) << 8) & (BIT11)) >> 11)
+#define WLAN_GET_FC_PWRMGT(n)   ((((u16)(n) << 8) & (BIT12)) >> 12)
+#define WLAN_GET_FC_MOREDATA(n) ((((u16)(n) << 8) & (BIT13)) >> 13)
+#define WLAN_GET_FC_ISWEP(n)    ((((u16)(n) << 8) & (BIT14)) >> 14)
+#define WLAN_GET_FC_ORDER(n)    ((((u16)(n) << 8) & (BIT15)) >> 15)
 
 /* Sequence Field bit */
-#define WLAN_GET_SEQ_FRGNUM(n) (((WORD)(n) >> 8) & (BIT0|BIT1|BIT2|BIT3))
-#define WLAN_GET_SEQ_SEQNUM(n) ((((WORD)(n) >> 8) \
+#define WLAN_GET_SEQ_FRGNUM(n) (((u16)(n) >> 8) & (BIT0|BIT1|BIT2|BIT3))
+#define WLAN_GET_SEQ_SEQNUM(n) ((((u16)(n) >> 8) \
 				 & (~(BIT0|BIT1|BIT2|BIT3))) >> 4)
 
 /* Capability Field bit */
@@ -190,21 +186,21 @@
 #else
 
 /* GET & SET Frame Control bit */
-#define WLAN_GET_FC_PRVER(n)    (((WORD)(n)) & (BIT0 | BIT1))
-#define WLAN_GET_FC_FTYPE(n)    ((((WORD)(n)) & (BIT2 | BIT3)) >> 2)
-#define WLAN_GET_FC_FSTYPE(n)   ((((WORD)(n)) & (BIT4|BIT5|BIT6|BIT7)) >> 4)
-#define WLAN_GET_FC_TODS(n)     ((((WORD)(n)) & (BIT8)) >> 8)
-#define WLAN_GET_FC_FROMDS(n)   ((((WORD)(n)) & (BIT9)) >> 9)
-#define WLAN_GET_FC_MOREFRAG(n) ((((WORD)(n)) & (BIT10)) >> 10)
-#define WLAN_GET_FC_RETRY(n)    ((((WORD)(n)) & (BIT11)) >> 11)
-#define WLAN_GET_FC_PWRMGT(n)   ((((WORD)(n)) & (BIT12)) >> 12)
-#define WLAN_GET_FC_MOREDATA(n) ((((WORD)(n)) & (BIT13)) >> 13)
-#define WLAN_GET_FC_ISWEP(n)    ((((WORD)(n)) & (BIT14)) >> 14)
-#define WLAN_GET_FC_ORDER(n)    ((((WORD)(n)) & (BIT15)) >> 15)
+#define WLAN_GET_FC_PRVER(n)    (((u16)(n)) & (BIT0 | BIT1))
+#define WLAN_GET_FC_FTYPE(n)    ((((u16)(n)) & (BIT2 | BIT3)) >> 2)
+#define WLAN_GET_FC_FSTYPE(n)   ((((u16)(n)) & (BIT4|BIT5|BIT6|BIT7)) >> 4)
+#define WLAN_GET_FC_TODS(n)     ((((u16)(n)) & (BIT8)) >> 8)
+#define WLAN_GET_FC_FROMDS(n)   ((((u16)(n)) & (BIT9)) >> 9)
+#define WLAN_GET_FC_MOREFRAG(n) ((((u16)(n)) & (BIT10)) >> 10)
+#define WLAN_GET_FC_RETRY(n)    ((((u16)(n)) & (BIT11)) >> 11)
+#define WLAN_GET_FC_PWRMGT(n)   ((((u16)(n)) & (BIT12)) >> 12)
+#define WLAN_GET_FC_MOREDATA(n) ((((u16)(n)) & (BIT13)) >> 13)
+#define WLAN_GET_FC_ISWEP(n)    ((((u16)(n)) & (BIT14)) >> 14)
+#define WLAN_GET_FC_ORDER(n)    ((((u16)(n)) & (BIT15)) >> 15)
 
 /* Sequence Field bit */
-#define WLAN_GET_SEQ_FRGNUM(n) (((WORD)(n)) & (BIT0|BIT1|BIT2|BIT3))
-#define WLAN_GET_SEQ_SEQNUM(n) ((((WORD)(n)) & (~(BIT0|BIT1|BIT2|BIT3))) >> 4)
+#define WLAN_GET_SEQ_FRGNUM(n) (((u16)(n)) & (BIT0|BIT1|BIT2|BIT3))
+#define WLAN_GET_SEQ_SEQNUM(n) ((((u16)(n)) & (~(BIT0|BIT1|BIT2|BIT3))) >> 4)
 
 /* Capability Field bit */
 #define WLAN_GET_CAP_INFO_ESS(n)           ((n) & BIT0)
@@ -235,20 +231,20 @@
 #define WLAN_SET_CAP_INFO_DSSSOFDM(n)      ((n) << 13)
 #define WLAN_SET_CAP_INFO_GRPACK(n)        ((n) << 14)
 
-#define WLAN_SET_FC_PRVER(n)    ((WORD)(n))
-#define WLAN_SET_FC_FTYPE(n)    (((WORD)(n)) << 2)
-#define WLAN_SET_FC_FSTYPE(n)   (((WORD)(n)) << 4)
-#define WLAN_SET_FC_TODS(n)     (((WORD)(n)) << 8)
-#define WLAN_SET_FC_FROMDS(n)   (((WORD)(n)) << 9)
-#define WLAN_SET_FC_MOREFRAG(n) (((WORD)(n)) << 10)
-#define WLAN_SET_FC_RETRY(n)    (((WORD)(n)) << 11)
-#define WLAN_SET_FC_PWRMGT(n)   (((WORD)(n)) << 12)
-#define WLAN_SET_FC_MOREDATA(n) (((WORD)(n)) << 13)
-#define WLAN_SET_FC_ISWEP(n)    (((WORD)(n)) << 14)
-#define WLAN_SET_FC_ORDER(n)    (((WORD)(n)) << 15)
-
-#define WLAN_SET_SEQ_FRGNUM(n) ((WORD)(n))
-#define WLAN_SET_SEQ_SEQNUM(n) (((WORD)(n)) << 4)
+#define WLAN_SET_FC_PRVER(n)    ((u16)(n))
+#define WLAN_SET_FC_FTYPE(n)    (((u16)(n)) << 2)
+#define WLAN_SET_FC_FSTYPE(n)   (((u16)(n)) << 4)
+#define WLAN_SET_FC_TODS(n)     (((u16)(n)) << 8)
+#define WLAN_SET_FC_FROMDS(n)   (((u16)(n)) << 9)
+#define WLAN_SET_FC_MOREFRAG(n) (((u16)(n)) << 10)
+#define WLAN_SET_FC_RETRY(n)    (((u16)(n)) << 11)
+#define WLAN_SET_FC_PWRMGT(n)   (((u16)(n)) << 12)
+#define WLAN_SET_FC_MOREDATA(n) (((u16)(n)) << 13)
+#define WLAN_SET_FC_ISWEP(n)    (((u16)(n)) << 14)
+#define WLAN_SET_FC_ORDER(n)    (((u16)(n)) << 15)
+
+#define WLAN_SET_SEQ_FRGNUM(n) ((u16)(n))
+#define WLAN_SET_SEQ_SEQNUM(n) (((u16)(n)) << 4)
 
 /* ERP Field bit */
 
@@ -269,50 +265,50 @@
 #define WLAN_MGMT_GET_TIM_OFFSET(b)     (((b) & ~BIT0) >> 1)
 
 /* 3-Addr & 4-Addr */
-#define WLAN_HDR_A3_DATA_PTR(p) (((PBYTE)(p)) + WLAN_HDR_ADDR3_LEN)
-#define WLAN_HDR_A4_DATA_PTR(p) (((PBYTE)(p)) + WLAN_HDR_ADDR4_LEN)
+#define WLAN_HDR_A3_DATA_PTR(p) (((u8 *)(p)) + WLAN_HDR_ADDR3_LEN)
+#define WLAN_HDR_A4_DATA_PTR(p) (((u8 *)(p)) + WLAN_HDR_ADDR4_LEN)
 
 /* IEEE ADDR */
 #define IEEE_ADDR_UNIVERSAL         0x02
 #define IEEE_ADDR_GROUP             0x01
 
 typedef struct {
-    BYTE            abyAddr[6];
+    u8            abyAddr[6];
 } IEEE_ADDR, *PIEEE_ADDR;
 
 /* 802.11 Header Format */
 
 typedef struct tagWLAN_80211HDR_A2 {
 
-    WORD    wFrameCtl;
-    WORD    wDurationID;
-    BYTE    abyAddr1[WLAN_ADDR_LEN];
-    BYTE    abyAddr2[WLAN_ADDR_LEN];
+    u16    wFrameCtl;
+    u16    wDurationID;
+    u8    abyAddr1[WLAN_ADDR_LEN];
+    u8    abyAddr2[WLAN_ADDR_LEN];
 
 } __attribute__ ((__packed__))
 WLAN_80211HDR_A2, *PWLAN_80211HDR_A2;
 
 typedef struct tagWLAN_80211HDR_A3 {
 
-    WORD    wFrameCtl;
-    WORD    wDurationID;
-    BYTE    abyAddr1[WLAN_ADDR_LEN];
-    BYTE    abyAddr2[WLAN_ADDR_LEN];
-    BYTE    abyAddr3[WLAN_ADDR_LEN];
-    WORD    wSeqCtl;
+    u16    wFrameCtl;
+    u16    wDurationID;
+    u8    abyAddr1[WLAN_ADDR_LEN];
+    u8    abyAddr2[WLAN_ADDR_LEN];
+    u8    abyAddr3[WLAN_ADDR_LEN];
+    u16    wSeqCtl;
 
 } __attribute__ ((__packed__))
 WLAN_80211HDR_A3, *PWLAN_80211HDR_A3;
 
 typedef struct tagWLAN_80211HDR_A4 {
 
-    WORD    wFrameCtl;
-    WORD    wDurationID;
-    BYTE    abyAddr1[WLAN_ADDR_LEN];
-    BYTE    abyAddr2[WLAN_ADDR_LEN];
-    BYTE    abyAddr3[WLAN_ADDR_LEN];
-    WORD    wSeqCtl;
-    BYTE    abyAddr4[WLAN_ADDR_LEN];
+    u16    wFrameCtl;
+    u16    wDurationID;
+    u8    abyAddr1[WLAN_ADDR_LEN];
+    u8    abyAddr2[WLAN_ADDR_LEN];
+    u8    abyAddr3[WLAN_ADDR_LEN];
+    u16    wSeqCtl;
+    u8    abyAddr4[WLAN_ADDR_LEN];
 
 } __attribute__ ((__packed__))
 WLAN_80211HDR_A4, *PWLAN_80211HDR_A4;
@@ -325,10 +321,4 @@ typedef union tagUWLAN_80211HDR {
 
 } UWLAN_80211HDR, *PUWLAN_80211HDR;
 
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
 #endif /* __80211HDR_H__ */
diff --git a/drivers/staging/vt6656/80211mgr.c b/drivers/staging/vt6656/80211mgr.c
index 534d490539b..61edb51f383 100644
--- a/drivers/staging/vt6656/80211mgr.c
+++ b/drivers/staging/vt6656/80211mgr.c
@@ -27,8 +27,6 @@
  * Functions:
  *      vMgrEncodeBeacon - Encode the Beacon frame
  *      vMgrDecodeBeacon - Decode the Beacon frame
- *      vMgrEncodeIBSSATIM - Encode the IBSS ATIM frame
- *      vMgrDecodeIBSSATIM - Decode the IBSS ATIM frame
  *      vMgrEncodeDisassociation - Encode the Disassociation frame
  *      vMgrDecodeDisassociation - Decode the Disassociation frame
  *      vMgrEncodeAssocRequest - Encode the Association request frame
@@ -46,7 +44,6 @@
  *      vMgrEncodeDeauthen - Encode the DeAuthentication frame
  *      vMgrDecodeDeauthen - Decode the DeAuthentication frame
  *      vMgrEncodeReassocResponse - Encode the Reassociation response frame
- *      vMgrDecodeReassocResponse - Decode the Reassociation response frame
  *
  * Revision History:
  *
@@ -59,25 +56,8 @@
 #include "80211hdr.h"
 #include "wpa.h"
 
-/*---------------------  Static Definitions -------------------------*/
-
-
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
 static int          msglevel                = MSG_LEVEL_INFO;
 /*static int          msglevel                =MSG_LEVEL_DEBUG;*/
-/*---------------------  Static Functions  --------------------------*/
-
-
-
-/*---------------------  Export Variables  --------------------------*/
-
-
-/*---------------------  Export Functions  --------------------------*/
-
 
 /*+
  *
@@ -100,9 +80,9 @@ vMgrEncodeBeacon(
 	pFrame->pqwTimestamp =
 		(u64 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
 			WLAN_BEACON_OFF_TS);
-    pFrame->pwBeaconInterval = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwBeaconInterval = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                        + WLAN_BEACON_OFF_BCN_INT);
-    pFrame->pwCapInfo = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwCapInfo = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                 + WLAN_BEACON_OFF_CAPINFO);
 
     pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_BEACON_OFF_SSID;
@@ -121,7 +101,6 @@ vMgrEncodeBeacon(
  *
 -*/
 
-
 void
 vMgrDecodeBeacon(
       PWLAN_FR_BEACON  pFrame
@@ -135,15 +114,15 @@ vMgrDecodeBeacon(
 	pFrame->pqwTimestamp =
 		(u64 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
 			WLAN_BEACON_OFF_TS);
-    pFrame->pwBeaconInterval = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwBeaconInterval = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                        + WLAN_BEACON_OFF_BCN_INT);
-    pFrame->pwCapInfo = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwCapInfo = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                 + WLAN_BEACON_OFF_CAPINFO);
 
     /* Information elements */
-    pItem = (PWLAN_IE)((PBYTE)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)))
+    pItem = (PWLAN_IE)((u8 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)))
                        + WLAN_BEACON_OFF_SSID);
-    while (((PBYTE)pItem) < (pFrame->pBuf + pFrame->len)) {
+    while (((u8 *)pItem) < (pFrame->pBuf + pFrame->len)) {
 
         switch (pItem->byElementID) {
         case WLAN_EID_SSID:
@@ -224,53 +203,10 @@ vMgrDecodeBeacon(
                 break;
 
         }
-        pItem = (PWLAN_IE)(((PBYTE)pItem) + 2 + pItem->len);
+        pItem = (PWLAN_IE)(((u8 *)pItem) + 2 + pItem->len);
     }
 }
 
-
-/*+
- *
- * Routine Description:
- *  Encode IBSS ATIM
- *
- *
- * Return Value:
- *    None.
- *
--*/
-
-
-void
-vMgrEncodeIBSSATIM(
-      PWLAN_FR_IBSSATIM   pFrame
-    )
-{
-    pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-    pFrame->len = WLAN_HDR_ADDR3_LEN;
-}
-
-
-/*+
- *
- * Routine Description:
- *  Decode IBSS ATIM
- *
- *
- * Return Value:
- *    None.
- *
--*/
-
-void
-vMgrDecodeIBSSATIM(
-      PWLAN_FR_IBSSATIM   pFrame
-    )
-{
-    pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-}
-
-
 /*+
  *
  * Routine Description:
@@ -289,14 +225,12 @@ vMgrEncodeDisassociation(
 {
     pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
 
-
     /* Fixed Fields */
-    pFrame->pwReason = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwReason = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                + WLAN_DISASSOC_OFF_REASON);
     pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_DISASSOC_OFF_REASON + sizeof(*(pFrame->pwReason));
 }
 
-
 /*+
  *
  * Routine Description:
@@ -316,7 +250,7 @@ vMgrDecodeDisassociation(
     pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
 
     /* Fixed Fields */
-    pFrame->pwReason = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwReason = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                + WLAN_DISASSOC_OFF_REASON);
 }
 
@@ -331,7 +265,6 @@ vMgrDecodeDisassociation(
  *
 -*/
 
-
 void
 vMgrEncodeAssocRequest(
       PWLAN_FR_ASSOCREQ  pFrame
@@ -339,14 +272,13 @@ vMgrEncodeAssocRequest(
 {
     pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
     /* Fixed Fields */
-    pFrame->pwCapInfo = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwCapInfo = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                 + WLAN_ASSOCREQ_OFF_CAP_INFO);
-    pFrame->pwListenInterval = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwListenInterval = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                        + WLAN_ASSOCREQ_OFF_LISTEN_INT);
     pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_ASSOCREQ_OFF_LISTEN_INT + sizeof(*(pFrame->pwListenInterval));
 }
 
-
 /*+
  *
  * Routine Description: (AP)
@@ -367,16 +299,16 @@ vMgrDecodeAssocRequest(
 
     pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
     /* Fixed Fields */
-    pFrame->pwCapInfo = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwCapInfo = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                 + WLAN_ASSOCREQ_OFF_CAP_INFO);
-    pFrame->pwListenInterval = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwListenInterval = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                 + WLAN_ASSOCREQ_OFF_LISTEN_INT);
 
     /* Information elements */
     pItem = (PWLAN_IE)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                             + WLAN_ASSOCREQ_OFF_SSID);
 
-    while (((PBYTE)pItem) < (pFrame->pBuf + pFrame->len)) {
+    while (((u8 *)pItem) < (pFrame->pBuf + pFrame->len)) {
         switch (pItem->byElementID) {
         case WLAN_EID_SSID:
             if (pFrame->pSSID == NULL)
@@ -407,7 +339,7 @@ vMgrDecodeAssocRequest(
                     pItem->byElementID);
             break;
         }
-        pItem = (PWLAN_IE)(((PBYTE)pItem) + 2 + pItem->len);
+        pItem = (PWLAN_IE)(((u8 *)pItem) + 2 + pItem->len);
     }
 }
 
@@ -430,17 +362,16 @@ vMgrEncodeAssocResponse(
     pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
 
     /* Fixed Fields */
-    pFrame->pwCapInfo = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwCapInfo = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                 + WLAN_ASSOCRESP_OFF_CAP_INFO);
-    pFrame->pwStatus = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwStatus = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                + WLAN_ASSOCRESP_OFF_STATUS);
-    pFrame->pwAid = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwAid = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                             + WLAN_ASSOCRESP_OFF_AID);
     pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_ASSOCRESP_OFF_AID
                   + sizeof(*(pFrame->pwAid));
 }
 
-
 /*+
  *
  * Routine Description:
@@ -462,11 +393,11 @@ vMgrDecodeAssocResponse(
     pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
 
     /* Fixed Fields */
-    pFrame->pwCapInfo = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwCapInfo = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                 + WLAN_ASSOCRESP_OFF_CAP_INFO);
-    pFrame->pwStatus = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwStatus = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                + WLAN_ASSOCRESP_OFF_STATUS);
-    pFrame->pwAid = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwAid = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                             + WLAN_ASSOCRESP_OFF_AID);
 
     /* Information elements */
@@ -474,16 +405,15 @@ vMgrDecodeAssocResponse(
                            + WLAN_ASSOCRESP_OFF_SUPP_RATES);
 
     pItem = (PWLAN_IE)(pFrame->pSuppRates);
-    pItem = (PWLAN_IE)(((PBYTE)pItem) + 2 + pItem->len);
+    pItem = (PWLAN_IE)(((u8 *)pItem) + 2 + pItem->len);
 
-    if ((((PBYTE)pItem) < (pFrame->pBuf + pFrame->len)) && (pItem->byElementID == WLAN_EID_EXTSUPP_RATES)) {
+    if ((((u8 *)pItem) < (pFrame->pBuf + pFrame->len)) && (pItem->byElementID == WLAN_EID_EXTSUPP_RATES)) {
         pFrame->pExtSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
         DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pFrame->pExtSuppRates=[%p].\n", pItem);
 	} else
         pFrame->pExtSuppRates = NULL;
 }
 
-
 /*+
  *
  * Routine Description:
@@ -503,16 +433,15 @@ vMgrEncodeReassocRequest(
     pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
 
     /* Fixed Fields */
-    pFrame->pwCapInfo = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwCapInfo = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                 + WLAN_REASSOCREQ_OFF_CAP_INFO);
-    pFrame->pwListenInterval = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwListenInterval = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                       + WLAN_REASSOCREQ_OFF_LISTEN_INT);
     pFrame->pAddrCurrAP = (PIEEE_ADDR)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                        + WLAN_REASSOCREQ_OFF_CURR_AP);
     pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_REASSOCREQ_OFF_CURR_AP + sizeof(*(pFrame->pAddrCurrAP));
 }
 
-
 /*+
  *
  * Routine Description: (AP)
@@ -524,7 +453,6 @@ vMgrEncodeReassocRequest(
  *
 -*/
 
-
 void
 vMgrDecodeReassocRequest(
       PWLAN_FR_REASSOCREQ  pFrame
@@ -534,9 +462,9 @@ vMgrDecodeReassocRequest(
     pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
 
     /* Fixed Fields */
-    pFrame->pwCapInfo = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwCapInfo = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                 + WLAN_REASSOCREQ_OFF_CAP_INFO);
-    pFrame->pwListenInterval = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwListenInterval = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                       + WLAN_REASSOCREQ_OFF_LISTEN_INT);
     pFrame->pAddrCurrAP = (PIEEE_ADDR)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                        + WLAN_REASSOCREQ_OFF_CURR_AP);
@@ -545,7 +473,7 @@ vMgrDecodeReassocRequest(
     pItem = (PWLAN_IE)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                        + WLAN_REASSOCREQ_OFF_SSID);
 
-    while (((PBYTE)pItem) < (pFrame->pBuf + pFrame->len)) {
+    while (((u8 *)pItem) < (pFrame->pBuf + pFrame->len)) {
 
         switch (pItem->byElementID) {
         case WLAN_EID_SSID:
@@ -576,12 +504,10 @@ vMgrDecodeReassocRequest(
                         pItem->byElementID);
             break;
         }
-        pItem = (PWLAN_IE)(((PBYTE)pItem) + 2 + pItem->len);
+        pItem = (PWLAN_IE)(((u8 *)pItem) + 2 + pItem->len);
     }
 }
 
-
-
 /*+
  *
  * Routine Description:
@@ -593,7 +519,6 @@ vMgrDecodeReassocRequest(
  *
 -*/
 
-
 void
 vMgrEncodeProbeRequest(
      PWLAN_FR_PROBEREQ  pFrame
@@ -626,7 +551,7 @@ vMgrDecodeProbeRequest(
     /* Information elements */
     pItem = (PWLAN_IE)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)));
 
-    while (((PBYTE)pItem) < (pFrame->pBuf + pFrame->len)) {
+    while (((u8 *)pItem) < (pFrame->pBuf + pFrame->len)) {
 
         switch (pItem->byElementID) {
         case WLAN_EID_SSID:
@@ -649,11 +574,10 @@ vMgrDecodeProbeRequest(
             break;
         }
 
-        pItem = (PWLAN_IE)(((PBYTE)pItem) + 2 +  pItem->len);
+        pItem = (PWLAN_IE)(((u8 *)pItem) + 2 +  pItem->len);
     }
 }
 
-
 /*+
  *
  * Routine Description:
@@ -665,7 +589,6 @@ vMgrDecodeProbeRequest(
  *
 -*/
 
-
 void
 vMgrEncodeProbeResponse(
      PWLAN_FR_PROBERESP  pFrame
@@ -677,17 +600,15 @@ vMgrEncodeProbeResponse(
 	pFrame->pqwTimestamp =
 		(u64 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
 			WLAN_PROBERESP_OFF_TS);
-    pFrame->pwBeaconInterval = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwBeaconInterval = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                        + WLAN_PROBERESP_OFF_BCN_INT);
-    pFrame->pwCapInfo = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwCapInfo = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                 + WLAN_PROBERESP_OFF_CAP_INFO);
 
     pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_PROBERESP_OFF_CAP_INFO +
                   sizeof(*(pFrame->pwCapInfo));
 }
 
-
-
 /*+
  *
  * Routine Description:
@@ -706,23 +627,22 @@ vMgrDecodeProbeResponse(
 {
     PWLAN_IE    pItem;
 
-
     pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
 
 	/* Fixed Fields */
 	pFrame->pqwTimestamp =
 		(u64 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
 			WLAN_PROBERESP_OFF_TS);
-    pFrame->pwBeaconInterval = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwBeaconInterval = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                        + WLAN_PROBERESP_OFF_BCN_INT);
-    pFrame->pwCapInfo = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwCapInfo = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                 + WLAN_PROBERESP_OFF_CAP_INFO);
 
     /* Information elements */
     pItem = (PWLAN_IE)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                        + WLAN_PROBERESP_OFF_SSID);
 
-    while (((PBYTE)pItem) < (pFrame->pBuf + pFrame->len)) {
+    while (((u8 *)pItem) < (pFrame->pBuf + pFrame->len)) {
         switch (pItem->byElementID) {
         case WLAN_EID_SSID:
             if (pFrame->pSSID == NULL)
@@ -796,11 +716,10 @@ vMgrDecodeProbeResponse(
             break;
         }
 
-        pItem = (PWLAN_IE)(((PBYTE)pItem) + 2 +  pItem->len);
+        pItem = (PWLAN_IE)(((u8 *)pItem) + 2 +  pItem->len);
     }
 }
 
-
 /*+
  *
  * Routine Description:
@@ -820,16 +739,15 @@ vMgrEncodeAuthen(
     pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
 
     /* Fixed Fields */
-    pFrame->pwAuthAlgorithm = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwAuthAlgorithm = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                       + WLAN_AUTHEN_OFF_AUTH_ALG);
-    pFrame->pwAuthSequence = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwAuthSequence = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                      + WLAN_AUTHEN_OFF_AUTH_SEQ);
-    pFrame->pwStatus = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwStatus = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                + WLAN_AUTHEN_OFF_STATUS);
     pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_AUTHEN_OFF_STATUS + sizeof(*(pFrame->pwStatus));
 }
 
-
 /*+
  *
  * Routine Description:
@@ -851,22 +769,21 @@ vMgrDecodeAuthen(
     pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
 
     /* Fixed Fields */
-    pFrame->pwAuthAlgorithm = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwAuthAlgorithm = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                       + WLAN_AUTHEN_OFF_AUTH_ALG);
-    pFrame->pwAuthSequence = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwAuthSequence = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                      + WLAN_AUTHEN_OFF_AUTH_SEQ);
-    pFrame->pwStatus = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwStatus = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                + WLAN_AUTHEN_OFF_STATUS);
 
     /* Information elements */
     pItem = (PWLAN_IE)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                        + WLAN_AUTHEN_OFF_CHALLENGE);
 
-    if ((((PBYTE)pItem) < (pFrame->pBuf + pFrame->len)) && (pItem->byElementID == WLAN_EID_CHALLENGE))
+    if ((((u8 *)pItem) < (pFrame->pBuf + pFrame->len)) && (pItem->byElementID == WLAN_EID_CHALLENGE))
         pFrame->pChallenge = (PWLAN_IE_CHALLENGE)pItem;
 }
 
-
 /*+
  *
  * Routine Description:
@@ -886,12 +803,11 @@ vMgrEncodeDeauthen(
     pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
 
     /* Fixed Fields */
-    pFrame->pwReason = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwReason = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                + WLAN_DEAUTHEN_OFF_REASON);
     pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_DEAUTHEN_OFF_REASON + sizeof(*(pFrame->pwReason));
 }
 
-
 /*+
  *
  * Routine Description:
@@ -911,11 +827,10 @@ vMgrDecodeDeauthen(
     pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
 
     /* Fixed Fields */
-    pFrame->pwReason = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwReason = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                + WLAN_DEAUTHEN_OFF_REASON);
 }
 
-
 /*+
  *
  * Routine Description: (AP)
@@ -935,53 +850,12 @@ vMgrEncodeReassocResponse(
     pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
 
     /* Fixed Fields */
-    pFrame->pwCapInfo = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwCapInfo = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                 + WLAN_REASSOCRESP_OFF_CAP_INFO);
-    pFrame->pwStatus = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwStatus = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                + WLAN_REASSOCRESP_OFF_STATUS);
-    pFrame->pwAid = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
+    pFrame->pwAid = (u16 *)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                             + WLAN_REASSOCRESP_OFF_AID);
 
     pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_REASSOCRESP_OFF_AID + sizeof(*(pFrame->pwAid));
 }
-
-
-/*+
- *
- * Routine Description:
- *   Decode Reassociation Response
- *
- *
- * Return Value:
- *    None.
- *
--*/
-
-
-void
-vMgrDecodeReassocResponse(
-      PWLAN_FR_REASSOCRESP  pFrame
-     )
-{
-    PWLAN_IE   pItem;
-
-    pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-    /* Fixed Fields */
-    pFrame->pwCapInfo = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                + WLAN_REASSOCRESP_OFF_CAP_INFO);
-    pFrame->pwStatus = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                               + WLAN_REASSOCRESP_OFF_STATUS);
-    pFrame->pwAid = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                            + WLAN_REASSOCRESP_OFF_AID);
-
-    /* Information elements */
-    pFrame->pSuppRates = (PWLAN_IE_SUPP_RATES)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-                                               + WLAN_REASSOCRESP_OFF_SUPP_RATES);
-
-    pItem = (PWLAN_IE)(pFrame->pSuppRates);
-    pItem = (PWLAN_IE)(((PBYTE)pItem) + 2 + pItem->len);
-
-    if ((((PBYTE)pItem) < (pFrame->pBuf + pFrame->len)) && (pItem->byElementID == WLAN_EID_EXTSUPP_RATES))
-        pFrame->pExtSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
-}
diff --git a/drivers/staging/vt6656/80211mgr.h b/drivers/staging/vt6656/80211mgr.h
index f8e16d8989e..39cde1a0b66 100644
--- a/drivers/staging/vt6656/80211mgr.h
+++ b/drivers/staging/vt6656/80211mgr.h
@@ -31,11 +31,8 @@
 #ifndef __80211MGR_H__
 #define __80211MGR_H__
 
-#include "ttype.h"
 #include "80211hdr.h"
 
-/*---------------------  Export Definitions -------------------------*/
-
 #define WLAN_MIN_ARRAY          1
 
 /* Information Element ID value */
@@ -213,192 +210,186 @@
 #define MEASURE_MODE_INCAPABLE  0x02
 #define MEASURE_MODE_REFUSED    0x04
 
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Types  ------------------------------*/
-
 /* Information Element types */
 
 #pragma pack(1)
 typedef struct tagWLAN_IE {
-    BYTE   byElementID;
-    BYTE   len;
+    u8   byElementID;
+    u8   len;
 } __attribute__ ((__packed__))
 WLAN_IE, *PWLAN_IE;
 
 /* Service Set IDentity (SSID) */
 #pragma pack(1)
 typedef struct tagWLAN_IE_SSID {
-    BYTE   byElementID;
-    BYTE   len;
-    BYTE   abySSID[1];
+    u8   byElementID;
+    u8   len;
+    u8   abySSID[1];
 } __attribute__ ((__packed__))
 WLAN_IE_SSID, *PWLAN_IE_SSID;
 
 /* Supported Rates */
 #pragma pack(1)
 typedef struct tagWLAN_IE_SUPP_RATES {
-    BYTE   byElementID;
-    BYTE   len;
-    BYTE   abyRates[1];
+    u8   byElementID;
+    u8   len;
+    u8   abyRates[1];
 } __attribute__ ((__packed__))
 WLAN_IE_SUPP_RATES,  *PWLAN_IE_SUPP_RATES;
 
 /* FH Parameter Set */
 #pragma pack(1)
 typedef struct _WLAN_IE_FH_PARMS {
-    BYTE    byElementID;
-    BYTE    len;
-    WORD    wDwellTime;
-    BYTE    byHopSet;
-    BYTE    byHopPattern;
-    BYTE    byHopIndex;
+    u8    byElementID;
+    u8    len;
+    u16    wDwellTime;
+    u8    byHopSet;
+    u8    byHopPattern;
+    u8    byHopIndex;
 } WLAN_IE_FH_PARMS,  *PWLAN_IE_FH_PARMS;
 
 /* DS Parameter Set */
 #pragma pack(1)
 typedef struct tagWLAN_IE_DS_PARMS {
-    BYTE   byElementID;
-    BYTE   len;
-    BYTE   byCurrChannel;
+    u8   byElementID;
+    u8   len;
+    u8   byCurrChannel;
 } __attribute__ ((__packed__))
 WLAN_IE_DS_PARMS,  *PWLAN_IE_DS_PARMS;
 
 /* CF Parameter Set */
 #pragma pack(1)
 typedef struct tagWLAN_IE_CF_PARMS {
-    BYTE   byElementID;
-    BYTE   len;
-    BYTE   byCFPCount;
-    BYTE   byCFPPeriod;
-    WORD   wCFPMaxDuration;
-    WORD   wCFPDurRemaining;
+    u8   byElementID;
+    u8   len;
+    u8   byCFPCount;
+    u8   byCFPPeriod;
+    u16   wCFPMaxDuration;
+    u16   wCFPDurRemaining;
 } __attribute__ ((__packed__))
 WLAN_IE_CF_PARMS,  *PWLAN_IE_CF_PARMS;
 
 /* TIM */
 #pragma pack(1)
 typedef struct tagWLAN_IE_TIM {
-    BYTE   byElementID;
-    BYTE   len;
-    BYTE   byDTIMCount;
-    BYTE   byDTIMPeriod;
-    BYTE   byBitMapCtl;
-    BYTE   byVirtBitMap[1];
+    u8   byElementID;
+    u8   len;
+    u8   byDTIMCount;
+    u8   byDTIMPeriod;
+    u8   byBitMapCtl;
+    u8   byVirtBitMap[1];
 } __attribute__ ((__packed__))
 WLAN_IE_TIM,  *PWLAN_IE_TIM;
 
 /* IBSS Parameter Set */
 #pragma pack(1)
 typedef struct tagWLAN_IE_IBSS_PARMS {
-    BYTE   byElementID;
-    BYTE   len;
-    WORD   wATIMWindow;
+    u8   byElementID;
+    u8   len;
+    u16   wATIMWindow;
 } __attribute__ ((__packed__))
 WLAN_IE_IBSS_PARMS, *PWLAN_IE_IBSS_PARMS;
 
 /* Challenge Text */
 #pragma pack(1)
 typedef struct tagWLAN_IE_CHALLENGE {
-    BYTE   byElementID;
-    BYTE   len;
-    BYTE   abyChallenge[1];
+    u8   byElementID;
+    u8   len;
+    u8   abyChallenge[1];
 } __attribute__ ((__packed__))
 WLAN_IE_CHALLENGE,  *PWLAN_IE_CHALLENGE;
 
 #pragma pack(1)
 typedef struct tagWLAN_IE_RSN_EXT {
-    BYTE byElementID;
-    BYTE len;
-    BYTE abyOUI[4];
-    WORD wVersion;
-    BYTE abyMulticast[4];
-    WORD wPKCount;
+    u8 byElementID;
+    u8 len;
+    u8 abyOUI[4];
+    u16 wVersion;
+    u8 abyMulticast[4];
+    u16 wPKCount;
 	struct {
-		BYTE abyOUI[4];
+		u8 abyOUI[4];
 	} PKSList[1];
 	/* the rest is variable so need to overlay ieauth structure */
 } WLAN_IE_RSN_EXT, *PWLAN_IE_RSN_EXT;
 
 #pragma pack(1)
 typedef struct tagWLAN_IE_RSN_AUTH {
-    WORD wAuthCount;
+    u16 wAuthCount;
     struct {
-        BYTE abyOUI[4];
+        u8 abyOUI[4];
     } AuthKSList[1];
 } WLAN_IE_RSN_AUTH, *PWLAN_IE_RSN_AUTH;
 
 /* RSN Identity */
 #pragma pack(1)
 typedef struct tagWLAN_IE_RSN {
-    BYTE   byElementID;
-    BYTE   len;
-    WORD   wVersion;
-    BYTE   abyRSN[WLAN_MIN_ARRAY];
+    u8   byElementID;
+    u8   len;
+    u16   wVersion;
+    u8   abyRSN[WLAN_MIN_ARRAY];
 } WLAN_IE_RSN, *PWLAN_IE_RSN;
 
 /* CCX Identity DavidWang */
 #pragma pack(1)
 typedef struct tagWLAN_IE_CCX {
-BYTE   byElementID;
-BYTE   len;
-BYTE   abyCCX[30];
+u8   byElementID;
+u8   len;
+u8   abyCCX[30];
 } WLAN_IE_CCX, *PWLAN_IE_CCX;
 #pragma pack(1)
 typedef struct tagWLAN_IE_CCX_IP {
-BYTE   byElementID;
-BYTE   len;
-BYTE   abyCCXOUI[4];
-BYTE   abyCCXIP[4];
-BYTE   abyCCXREV[2];
+u8   byElementID;
+u8   len;
+u8   abyCCXOUI[4];
+u8   abyCCXIP[4];
+u8   abyCCXREV[2];
 } WLAN_IE_CCX_IP, *PWLAN_IE_CCX_IP;
 #pragma pack(1)
 typedef struct tagWLAN_IE_CCX_Ver {
-BYTE   byElementID;
-BYTE   len;
-BYTE   abyCCXVer[5];
+u8   byElementID;
+u8   len;
+u8   abyCCXVer[5];
 } WLAN_IE_CCX_Ver, *PWLAN_IE_CCX_Ver;
 
 /* ERP */
 #pragma pack(1)
 typedef struct tagWLAN_IE_ERP {
-    BYTE   byElementID;
-    BYTE   len;
-    BYTE   byContext;
+    u8   byElementID;
+    u8   len;
+    u8   byContext;
 } __attribute__ ((__packed__))
 WLAN_IE_ERP,  *PWLAN_IE_ERP;
 
 #pragma pack(1)
 typedef struct _MEASEURE_REQ {
-    BYTE                byChannel;
-    BYTE                abyStartTime[8];
-    BYTE                abyDuration[2];
+    u8                byChannel;
+    u8                abyStartTime[8];
+    u8                abyDuration[2];
 } MEASEURE_REQ, *PMEASEURE_REQ,
   MEASEURE_REQ_BASIC, *PMEASEURE_REQ_BASIC,
   MEASEURE_REQ_CCA, *PMEASEURE_REQ_CCA,
   MEASEURE_REQ_RPI, *PMEASEURE_REQ_RPI;
 
 typedef struct _MEASEURE_REP_BASIC {
-    BYTE                byChannel;
-    BYTE                abyStartTime[8];
-    BYTE                abyDuration[2];
-    BYTE                byMap;
+    u8                byChannel;
+    u8                abyStartTime[8];
+    u8                abyDuration[2];
+    u8                byMap;
 } MEASEURE_REP_BASIC, *PMEASEURE_REP_BASIC;
 
 typedef struct _MEASEURE_REP_CCA {
-    BYTE                byChannel;
-    BYTE                abyStartTime[8];
-    BYTE                abyDuration[2];
-    BYTE                byCCABusyFraction;
+    u8                byChannel;
+    u8                abyStartTime[8];
+    u8                abyDuration[2];
+    u8                byCCABusyFraction;
 } MEASEURE_REP_CCA, *PMEASEURE_REP_CCA;
 
 typedef struct _MEASEURE_REP_RPI {
-    BYTE                byChannel;
-    BYTE                abyStartTime[8];
-    BYTE                abyDuration[2];
-    BYTE                abyRPIdensity[8];
+    u8                byChannel;
+    u8                abyStartTime[8];
+    u8                abyDuration[2];
+    u8                abyRPIdensity[8];
 } MEASEURE_REP_RPI, *PMEASEURE_REP_RPI;
 
 typedef union _MEASEURE_REP {
@@ -410,85 +401,84 @@ typedef union _MEASEURE_REP {
 } MEASEURE_REP, *PMEASEURE_REP;
 
 typedef struct _WLAN_IE_MEASURE_REQ {
-    BYTE                byElementID;
-    BYTE                len;
-    BYTE                byToken;
-    BYTE                byMode;
-    BYTE                byType;
+    u8                byElementID;
+    u8                len;
+    u8                byToken;
+    u8                byMode;
+    u8                byType;
     MEASEURE_REQ        sReq;
 } WLAN_IE_MEASURE_REQ, *PWLAN_IE_MEASURE_REQ;
 
 typedef struct _WLAN_IE_MEASURE_REP {
-    BYTE                byElementID;
-    BYTE                len;
-    BYTE                byToken;
-    BYTE                byMode;
-    BYTE                byType;
+    u8                byElementID;
+    u8                len;
+    u8                byToken;
+    u8                byMode;
+    u8                byType;
     MEASEURE_REP        sRep;
 } WLAN_IE_MEASURE_REP, *PWLAN_IE_MEASURE_REP;
 
 typedef struct _WLAN_IE_CH_SW {
-    BYTE                byElementID;
-    BYTE                len;
-    BYTE                byMode;
-    BYTE                byChannel;
-    BYTE                byCount;
+    u8                byElementID;
+    u8                len;
+    u8                byMode;
+    u8                byChannel;
+    u8                byCount;
 } WLAN_IE_CH_SW, *PWLAN_IE_CH_SW;
 
 typedef struct _WLAN_IE_QUIET {
-    BYTE                byElementID;
-    BYTE                len;
-    BYTE                byQuietCount;
-    BYTE                byQuietPeriod;
-    BYTE                abyQuietDuration[2];
-    BYTE                abyQuietOffset[2];
+    u8                byElementID;
+    u8                len;
+    u8                byQuietCount;
+    u8                byQuietPeriod;
+    u8                abyQuietDuration[2];
+    u8                abyQuietOffset[2];
 } WLAN_IE_QUIET, *PWLAN_IE_QUIET;
 
 typedef struct _WLAN_IE_COUNTRY {
-    BYTE                byElementID;
-    BYTE                len;
-    BYTE                abyCountryString[3];
-    BYTE                abyCountryInfo[3];
+    u8                byElementID;
+    u8                len;
+    u8                abyCountryString[3];
+    u8                abyCountryInfo[3];
 } WLAN_IE_COUNTRY, *PWLAN_IE_COUNTRY;
 
 typedef struct _WLAN_IE_PW_CONST {
-    BYTE                byElementID;
-    BYTE                len;
-    BYTE                byPower;
+    u8                byElementID;
+    u8                len;
+    u8                byPower;
 } WLAN_IE_PW_CONST, *PWLAN_IE_PW_CONST;
 
 typedef struct _WLAN_IE_PW_CAP {
-    BYTE                byElementID;
-    BYTE                len;
-    BYTE                byMinPower;
-    BYTE                byMaxPower;
+    u8                byElementID;
+    u8                len;
+    u8                byMinPower;
+    u8                byMaxPower;
 } WLAN_IE_PW_CAP, *PWLAN_IE_PW_CAP;
 
 typedef struct _WLAN_IE_SUPP_CH {
-    BYTE                byElementID;
-    BYTE                len;
-    BYTE                abyChannelTuple[2];
+    u8                byElementID;
+    u8                len;
+    u8                abyChannelTuple[2];
 } WLAN_IE_SUPP_CH, *PWLAN_IE_SUPP_CH;
 
 typedef struct _WLAN_IE_TPC_REQ {
-    BYTE                byElementID;
-    BYTE                len;
+    u8                byElementID;
+    u8                len;
 } WLAN_IE_TPC_REQ, *PWLAN_IE_TPC_REQ;
 
 typedef struct _WLAN_IE_TPC_REP {
-    BYTE                byElementID;
-    BYTE                len;
-    BYTE                byTxPower;
-    BYTE                byLinkMargin;
+    u8                byElementID;
+    u8                len;
+    u8                byTxPower;
+    u8                byLinkMargin;
 } WLAN_IE_TPC_REP, *PWLAN_IE_TPC_REP;
 
-
 typedef struct _WLAN_IE_IBSS_DFS {
-    BYTE                byElementID;
-    BYTE                len;
-    BYTE                abyDFSOwner[6];
-    BYTE                byDFSRecovery;
-    BYTE                abyChannelMap[2];
+    u8                byElementID;
+    u8                len;
+    u8                abyDFSOwner[6];
+    u8                byDFSRecovery;
+    u8                abyChannelMap[2];
 } WLAN_IE_IBSS_DFS, *PWLAN_IE_IBSS_DFS;
 
 #pragma pack()
@@ -500,7 +490,7 @@ typedef struct tagWLAN_FR_MGMT {
 
     unsigned int                  uType;
     unsigned int                  len;
-    PBYTE                 pBuf;
+    u8 *                 pBuf;
     PUWLAN_80211HDR       pHdr;
 
 } WLAN_FR_MGMT,  *PWLAN_FR_MGMT;
@@ -510,12 +500,12 @@ typedef struct tagWLAN_FR_BEACON {
 
     unsigned int                    uType;
     unsigned int                    len;
-    PBYTE                   pBuf;
+    u8 *                   pBuf;
     PUWLAN_80211HDR         pHdr;
 	/* fixed fields */
 	u64 *pqwTimestamp;
-    PWORD                   pwBeaconInterval;
-    PWORD                   pwCapInfo;
+    u16 *                   pwBeaconInterval;
+    u16 *                   pwCapInfo;
     /* info elements */
     PWLAN_IE_SSID           pSSID;
     PWLAN_IE_SUPP_RATES     pSuppRates;
@@ -541,7 +531,7 @@ typedef struct tagWLAN_FR_IBSSATIM {
 
     unsigned int                    uType;
     unsigned int                    len;
-    PBYTE                   pBuf;
+    u8 *                   pBuf;
     PUWLAN_80211HDR         pHdr;
 
 	/* fixed fields */
@@ -555,10 +545,10 @@ typedef struct tagWLAN_FR_DISASSOC {
 
     unsigned int                    uType;
     unsigned int                    len;
-    PBYTE                   pBuf;
+    u8 *                   pBuf;
     PUWLAN_80211HDR         pHdr;
     /* fixed fields */
-    PWORD                   pwReason;
+    u16 *                   pwReason;
     /* info elements */
 
 } WLAN_FR_DISASSOC, *PWLAN_FR_DISASSOC;
@@ -568,11 +558,11 @@ typedef struct tagWLAN_FR_ASSOCREQ {
 
     unsigned int                    uType;
     unsigned int                    len;
-    PBYTE                   pBuf;
+    u8 *                   pBuf;
     PUWLAN_80211HDR         pHdr;
     /* fixed fields */
-    PWORD                   pwCapInfo;
-    PWORD                   pwListenInterval;
+    u16 *                   pwCapInfo;
+    u16 *                   pwListenInterval;
     /* info elements */
     PWLAN_IE_SSID           pSSID;
     PWLAN_IE_SUPP_RATES     pSuppRates;
@@ -592,12 +582,12 @@ typedef struct tagWLAN_FR_ASSOCRESP {
 
     unsigned int                    uType;
     unsigned int                    len;
-    PBYTE                   pBuf;
+    u8 *                   pBuf;
     PUWLAN_80211HDR         pHdr;
     /* fixed fields */
-    PWORD                   pwCapInfo;
-    PWORD                   pwStatus;
-    PWORD                   pwAid;
+    u16 *                   pwCapInfo;
+    u16 *                   pwStatus;
+    u16 *                   pwAid;
     /* info elements */
     PWLAN_IE_SUPP_RATES     pSuppRates;
     PWLAN_IE_SUPP_RATES     pExtSuppRates;
@@ -609,12 +599,12 @@ typedef struct tagWLAN_FR_REASSOCREQ {
 
     unsigned int                    uType;
     unsigned int                    len;
-    PBYTE                   pBuf;
+    u8 *                   pBuf;
     PUWLAN_80211HDR         pHdr;
 
     /* fixed fields */
-    PWORD                   pwCapInfo;
-    PWORD                   pwListenInterval;
+    u16 *                   pwCapInfo;
+    u16 *                   pwListenInterval;
     PIEEE_ADDR              pAddrCurrAP;
 
     /* info elements */
@@ -634,12 +624,12 @@ typedef struct tagWLAN_FR_REASSOCRESP {
 
     unsigned int                    uType;
     unsigned int                    len;
-    PBYTE                   pBuf;
+    u8 *                   pBuf;
     PUWLAN_80211HDR         pHdr;
     /* fixed fields */
-    PWORD                   pwCapInfo;
-    PWORD                   pwStatus;
-    PWORD                   pwAid;
+    u16 *                   pwCapInfo;
+    u16 *                   pwStatus;
+    u16 *                   pwAid;
     /* info elements */
     PWLAN_IE_SUPP_RATES     pSuppRates;
     PWLAN_IE_SUPP_RATES     pExtSuppRates;
@@ -651,7 +641,7 @@ typedef struct tagWLAN_FR_PROBEREQ {
 
     unsigned int                    uType;
     unsigned int                    len;
-    PBYTE                   pBuf;
+    u8 *                   pBuf;
     PUWLAN_80211HDR         pHdr;
     /* fixed fields */
     /* info elements */
@@ -666,12 +656,12 @@ typedef struct tagWLAN_FR_PROBERESP {
 
     unsigned int                    uType;
     unsigned int                    len;
-    PBYTE                   pBuf;
+    u8 *                   pBuf;
     PUWLAN_80211HDR         pHdr;
     /* fixed fields */
 	u64 *pqwTimestamp;
-    PWORD                   pwBeaconInterval;
-    PWORD                   pwCapInfo;
+    u16 *                   pwBeaconInterval;
+    u16 *                   pwCapInfo;
     /* info elements */
     PWLAN_IE_SSID           pSSID;
     PWLAN_IE_SUPP_RATES     pSuppRates;
@@ -695,12 +685,12 @@ typedef struct tagWLAN_FR_AUTHEN {
 
     unsigned int                    uType;
     unsigned int                    len;
-    PBYTE                   pBuf;
+    u8 *                   pBuf;
     PUWLAN_80211HDR         pHdr;
     /* fixed fields */
-    PWORD                   pwAuthAlgorithm;
-    PWORD                   pwAuthSequence;
-    PWORD                   pwStatus;
+    u16 *                   pwAuthAlgorithm;
+    u16 *                   pwAuthSequence;
+    u16 *                   pwStatus;
     /* info elements */
     PWLAN_IE_CHALLENGE      pChallenge;
 
@@ -711,17 +701,15 @@ typedef struct tagWLAN_FR_DEAUTHEN {
 
     unsigned int                    uType;
     unsigned int                    len;
-    PBYTE                   pBuf;
+    u8 *                   pBuf;
     PUWLAN_80211HDR         pHdr;
     /* fixed fields */
-    PWORD                   pwReason;
+    u16 *                   pwReason;
 
     /* info elements */
 
 } WLAN_FR_DEAUTHEN, *PWLAN_FR_DEAUTHEN;
 
-/*---------------------  Export Functions  --------------------------*/
-
 void
 vMgrEncodeBeacon(
       PWLAN_FR_BEACON  pFrame
@@ -733,16 +721,6 @@ vMgrDecodeBeacon(
     );
 
 void
-vMgrEncodeIBSSATIM(
-      PWLAN_FR_IBSSATIM   pFrame
-    );
-
-void
-vMgrDecodeIBSSATIM(
-      PWLAN_FR_IBSSATIM   pFrame
-    );
-
-void
 vMgrEncodeDisassociation(
       PWLAN_FR_DISASSOC  pFrame
     );
@@ -827,9 +805,4 @@ vMgrEncodeReassocResponse(
       PWLAN_FR_REASSOCRESP  pFrame
     );
 
-void
-vMgrDecodeReassocResponse(
-      PWLAN_FR_REASSOCRESP  pFrame
-    );
-
 #endif /* __80211MGR_H__ */
diff --git a/drivers/staging/vt6656/TODO b/drivers/staging/vt6656/TODO
index a318995ba07..e154b2f3b24 100644
--- a/drivers/staging/vt6656/TODO
+++ b/drivers/staging/vt6656/TODO
@@ -7,7 +7,7 @@ TODO:
   - split rf.c
   - abstract VT3184 chipset specific code
 - add common vt665x infrastructure
-- kill ttype.h
+- kill ttype.h -- done
 - switch to use LIB80211
 - switch to use MAC80211
 - use kernel coding style
diff --git a/drivers/staging/vt6656/aes_ccmp.c b/drivers/staging/vt6656/aes_ccmp.c
index fb6124d9082..28a4c4c3041 100644
--- a/drivers/staging/vt6656/aes_ccmp.c
+++ b/drivers/staging/vt6656/aes_ccmp.c
@@ -33,17 +33,11 @@
 #include "device.h"
 #include "80211hdr.h"
 
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
 /*
  * SBOX Table
  */
 
-BYTE sbox_table[256] = {
+u8 sbox_table[256] = {
 	0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5, 0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76,
 	0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0, 0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0,
 	0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc, 0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15,
@@ -62,7 +56,7 @@ BYTE sbox_table[256] = {
 	0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68, 0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16
 };
 
-BYTE dot2_table[256] = {
+u8 dot2_table[256] = {
 	0x00, 0x02, 0x04, 0x06, 0x08, 0x0a, 0x0c, 0x0e, 0x10, 0x12, 0x14, 0x16, 0x18, 0x1a, 0x1c, 0x1e,
 	0x20, 0x22, 0x24, 0x26, 0x28, 0x2a, 0x2c, 0x2e, 0x30, 0x32, 0x34, 0x36, 0x38, 0x3a, 0x3c, 0x3e,
 	0x40, 0x42, 0x44, 0x46, 0x48, 0x4a, 0x4c, 0x4e, 0x50, 0x52, 0x54, 0x56, 0x58, 0x5a, 0x5c, 0x5e,
@@ -81,7 +75,7 @@ BYTE dot2_table[256] = {
 	0xfb, 0xf9, 0xff, 0xfd, 0xf3, 0xf1, 0xf7, 0xf5, 0xeb, 0xe9, 0xef, 0xed, 0xe3, 0xe1, 0xe7, 0xe5
 };
 
-BYTE dot3_table[256] = {
+u8 dot3_table[256] = {
 	0x00, 0x03, 0x06, 0x05, 0x0c, 0x0f, 0x0a, 0x09, 0x18, 0x1b, 0x1e, 0x1d, 0x14, 0x17, 0x12, 0x11,
 	0x30, 0x33, 0x36, 0x35, 0x3c, 0x3f, 0x3a, 0x39, 0x28, 0x2b, 0x2e, 0x2d, 0x24, 0x27, 0x22, 0x21,
 	0x60, 0x63, 0x66, 0x65, 0x6c, 0x6f, 0x6a, 0x69, 0x78, 0x7b, 0x7e, 0x7d, 0x74, 0x77, 0x72, 0x71,
@@ -100,17 +94,11 @@ BYTE dot3_table[256] = {
 	0x0b, 0x08, 0x0d, 0x0e, 0x07, 0x04, 0x01, 0x02, 0x13, 0x10, 0x15, 0x16, 0x1f, 0x1c, 0x19, 0x1a
 };
 
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-static void xor_128(BYTE *a, BYTE *b, BYTE *out)
+static void xor_128(u8 *a, u8 *b, u8 *out)
 {
-	PDWORD dwPtrA = (PDWORD) a;
-	PDWORD dwPtrB = (PDWORD) b;
-	PDWORD dwPtrOut = (PDWORD) out;
+	u32 * dwPtrA = (u32 *) a;
+	u32 * dwPtrB = (u32 *) b;
+	u32 * dwPtrOut = (u32 *) out;
 
 	(*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
 	(*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
@@ -118,20 +106,19 @@ static void xor_128(BYTE *a, BYTE *b, BYTE *out)
 	(*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
 }
 
-
-static void xor_32(BYTE *a, BYTE *b, BYTE *out)
+static void xor_32(u8 *a, u8 *b, u8 *out)
 {
-	PDWORD dwPtrA = (PDWORD) a;
-	PDWORD dwPtrB = (PDWORD) b;
-	PDWORD dwPtrOut = (PDWORD) out;
+	u32 * dwPtrA = (u32 *) a;
+	u32 * dwPtrB = (u32 *) b;
+	u32 * dwPtrOut = (u32 *) out;
 
 	(*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
 }
 
-void AddRoundKey(BYTE *key, int round)
+void AddRoundKey(u8 *key, int round)
 {
-	BYTE sbox_key[4];
-	BYTE rcon_table[10] = { 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80, 0x1b, 0x36};
+	u8 sbox_key[4];
+	u8 rcon_table[10] = { 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80, 0x1b, 0x36};
 
 	sbox_key[0] = sbox_table[key[13]];
 	sbox_key[1] = sbox_table[key[14]];
@@ -146,7 +133,7 @@ void AddRoundKey(BYTE *key, int round)
 	xor_32(&key[12], &key[8], &key[12]);
 }
 
-void SubBytes(BYTE *in, BYTE *out)
+void SubBytes(u8 *in, u8 *out)
 {
 	int i;
 
@@ -154,7 +141,7 @@ void SubBytes(BYTE *in, BYTE *out)
 		out[i] = sbox_table[in[i]];
 }
 
-void ShiftRows(BYTE *in, BYTE *out)
+void ShiftRows(u8 *in, u8 *out)
 {
 	out[0]  = in[0];
 	out[1]  = in[5];
@@ -174,7 +161,7 @@ void ShiftRows(BYTE *in, BYTE *out)
 	out[15] = in[11];
 }
 
-void MixColumns(BYTE *in, BYTE *out)
+void MixColumns(u8 *in, u8 *out)
 {
 
 	out[0] = dot2_table[in[0]] ^ dot3_table[in[1]] ^ in[2] ^ in[3];
@@ -183,13 +170,13 @@ void MixColumns(BYTE *in, BYTE *out)
 	out[3] = dot3_table[in[0]] ^ in[1] ^ in[2] ^ dot2_table[in[3]];
 }
 
-void AESv128(BYTE *key, BYTE *data, BYTE *ciphertext)
+void AESv128(u8 *key, u8 *data, u8 *ciphertext)
 {
 	int  i;
 	int  round;
-	BYTE TmpdataA[16];
-	BYTE TmpdataB[16];
-	BYTE abyRoundKey[16];
+	u8 TmpdataA[16];
+	u8 TmpdataB[16];
+	u8 abyRoundKey[16];
 
 	for (i = 0; i < 16; i++)
 		abyRoundKey[i] = key[i];
@@ -231,32 +218,32 @@ void AESv128(BYTE *key, BYTE *data, BYTE *ciphertext)
  *
  */
 
-bool AESbGenCCMP(PBYTE pbyRxKey, PBYTE pbyFrame, WORD wFrameSize)
+bool AESbGenCCMP(u8 * pbyRxKey, u8 * pbyFrame, u16 wFrameSize)
 {
-	BYTE            abyNonce[13];
-	BYTE            MIC_IV[16];
-	BYTE            MIC_HDR1[16];
-	BYTE            MIC_HDR2[16];
-	BYTE            abyMIC[16];
-	BYTE            abyCTRPLD[16];
-	BYTE            abyTmp[16];
-	BYTE            abyPlainText[16];
-	BYTE            abyLastCipher[16];
-
-	PS802_11Header  pMACHeader = (PS802_11Header) pbyFrame;
-	PBYTE           pbyIV;
-	PBYTE           pbyPayload;
-	WORD            wHLen = 22;
+	u8            abyNonce[13];
+	u8            MIC_IV[16];
+	u8            MIC_HDR1[16];
+	u8            MIC_HDR2[16];
+	u8            abyMIC[16];
+	u8            abyCTRPLD[16];
+	u8            abyTmp[16];
+	u8            abyPlainText[16];
+	u8            abyLastCipher[16];
+
+	struct ieee80211_hdr *pMACHeader = (struct ieee80211_hdr *) pbyFrame;
+	u8 *           pbyIV;
+	u8 *           pbyPayload;
+	u16            wHLen = 22;
 	/* 8 is IV, 8 is MIC, 4 is CRC */
-	WORD            wPayloadSize = wFrameSize - 8 - 8 - 4 - WLAN_HDR_ADDR3_LEN;
+	u16            wPayloadSize = wFrameSize - 8 - 8 - 4 - WLAN_HDR_ADDR3_LEN;
 	bool            bA4 = false;
-	BYTE            byTmp;
-	WORD            wCnt;
+	u8            byTmp;
+	u16            wCnt;
 	int             ii, jj, kk;
 
 	pbyIV = pbyFrame + WLAN_HDR_ADDR3_LEN;
-	if (WLAN_GET_FC_TODS(*(PWORD) pbyFrame) &&
-	    WLAN_GET_FC_FROMDS(*(PWORD) pbyFrame)) {
+	if (WLAN_GET_FC_TODS(*(u16 *) pbyFrame) &&
+	    WLAN_GET_FC_FROMDS(*(u16 *) pbyFrame)) {
 		bA4 = true;
 		pbyIV += 6;             /* 6 is 802.11 address4 */
 		wHLen += 6;
@@ -265,7 +252,7 @@ bool AESbGenCCMP(PBYTE pbyRxKey, PBYTE pbyFrame, WORD wFrameSize)
 	pbyPayload = pbyIV + 8; /* IV-length */
 
 	abyNonce[0]  = 0x00; /* now is 0, if Qos here will be priority */
-	memcpy(&(abyNonce[1]), pMACHeader->abyAddr2, ETH_ALEN);
+	memcpy(&(abyNonce[1]), pMACHeader->addr2, ETH_ALEN);
 	abyNonce[7]  = pbyIV[7];
 	abyNonce[8]  = pbyIV[6];
 	abyNonce[9]  = pbyIV[5];
@@ -276,28 +263,28 @@ bool AESbGenCCMP(PBYTE pbyRxKey, PBYTE pbyFrame, WORD wFrameSize)
 	/* MIC_IV */
 	MIC_IV[0] = 0x59;
 	memcpy(&(MIC_IV[1]), &(abyNonce[0]), 13);
-	MIC_IV[14] = (BYTE)(wPayloadSize >> 8);
-	MIC_IV[15] = (BYTE)(wPayloadSize & 0xff);
+	MIC_IV[14] = (u8)(wPayloadSize >> 8);
+	MIC_IV[15] = (u8)(wPayloadSize & 0xff);
 
 	/* MIC_HDR1 */
-	MIC_HDR1[0] = (BYTE)(wHLen >> 8);
-	MIC_HDR1[1] = (BYTE)(wHLen & 0xff);
-	byTmp = (BYTE)(pMACHeader->wFrameCtl & 0xff);
+	MIC_HDR1[0] = (u8)(wHLen >> 8);
+	MIC_HDR1[1] = (u8)(wHLen & 0xff);
+	byTmp = (u8)(pMACHeader->frame_control & 0xff);
 	MIC_HDR1[2] = byTmp & 0x8f;
-	byTmp = (BYTE)(pMACHeader->wFrameCtl >> 8);
+	byTmp = (u8)(pMACHeader->frame_control >> 8);
 	byTmp &= 0x87;
 	MIC_HDR1[3] = byTmp | 0x40;
-	memcpy(&(MIC_HDR1[4]), pMACHeader->abyAddr1, ETH_ALEN);
-	memcpy(&(MIC_HDR1[10]), pMACHeader->abyAddr2, ETH_ALEN);
+	memcpy(&(MIC_HDR1[4]), pMACHeader->addr1, ETH_ALEN);
+	memcpy(&(MIC_HDR1[10]), pMACHeader->addr2, ETH_ALEN);
 
 	/* MIC_HDR2 */
-	memcpy(&(MIC_HDR2[0]), pMACHeader->abyAddr3, ETH_ALEN);
-	byTmp = (BYTE)(pMACHeader->wSeqCtl & 0xff);
+	memcpy(&(MIC_HDR2[0]), pMACHeader->addr3, ETH_ALEN);
+	byTmp = (u8)(pMACHeader->seq_ctrl & 0xff);
 	MIC_HDR2[6] = byTmp & 0x0f;
 	MIC_HDR2[7] = 0;
 
 	if (bA4) {
-		memcpy(&(MIC_HDR2[8]), pMACHeader->abyAddr4, ETH_ALEN);
+		memcpy(&(MIC_HDR2[8]), pMACHeader->addr4, ETH_ALEN);
 	} else {
 		MIC_HDR2[8]  = 0x00;
 		MIC_HDR2[9]  = 0x00;
@@ -326,8 +313,8 @@ bool AESbGenCCMP(PBYTE pbyRxKey, PBYTE pbyFrame, WORD wFrameSize)
 
 	for (jj = wPayloadSize; jj > 16; jj = jj-16) {
 
-		abyCTRPLD[14] = (BYTE) (wCnt >> 8);
-		abyCTRPLD[15] = (BYTE) (wCnt & 0xff);
+		abyCTRPLD[14] = (u8) (wCnt >> 8);
+		abyCTRPLD[15] = (u8) (wCnt & 0xff);
 
 		AESv128(pbyRxKey, abyCTRPLD, abyTmp);
 
@@ -349,8 +336,8 @@ bool AESbGenCCMP(PBYTE pbyRxKey, PBYTE pbyFrame, WORD wFrameSize)
 	for (ii = jj; ii < 16; ii++)
 		abyLastCipher[ii] = 0x00;
 
-	abyCTRPLD[14] = (BYTE) (wCnt >> 8);
-	abyCTRPLD[15] = (BYTE) (wCnt & 0xff);
+	abyCTRPLD[14] = (u8) (wCnt >> 8);
+	abyCTRPLD[15] = (u8) (wCnt & 0xff);
 
 	AESv128(pbyRxKey, abyCTRPLD, abyTmp);
 	for (kk = 0; kk < 16; kk++)
@@ -370,8 +357,8 @@ bool AESbGenCCMP(PBYTE pbyRxKey, PBYTE pbyFrame, WORD wFrameSize)
 	/* => above is the calculated MIC */
 
 	wCnt = 0;
-	abyCTRPLD[14] = (BYTE) (wCnt >> 8);
-	abyCTRPLD[15] = (BYTE) (wCnt & 0xff);
+	abyCTRPLD[14] = (u8) (wCnt >> 8);
+	abyCTRPLD[15] = (u8) (wCnt & 0xff);
 	AESv128(pbyRxKey, abyCTRPLD, abyTmp);
 
 	for (kk = 0; kk < 8; kk++)
diff --git a/drivers/staging/vt6656/aes_ccmp.h b/drivers/staging/vt6656/aes_ccmp.h
index a2e2c4e9a5c..ed6a9ae315d 100644
--- a/drivers/staging/vt6656/aes_ccmp.h
+++ b/drivers/staging/vt6656/aes_ccmp.h
@@ -30,17 +30,6 @@
 #ifndef __AES_H__
 #define __AES_H__
 
-#include "ttype.h"
-
-/*---------------------  Export Definitions -------------------------*/
-
-/*---------------------  Export Types  ------------------------------*/
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-bool AESbGenCCMP(PBYTE pbyRxKey, PBYTE pbyFrame, WORD wFrameSize);
+bool AESbGenCCMP(u8 * pbyRxKey, u8 * pbyFrame, u16 wFrameSize);
 
 #endif /* __AES_CCMP_H__ */
diff --git a/drivers/staging/vt6656/baseband.c b/drivers/staging/vt6656/baseband.c
index a9f525e9d16..33fa76759bf 100644
--- a/drivers/staging/vt6656/baseband.c
+++ b/drivers/staging/vt6656/baseband.c
@@ -29,8 +29,6 @@
  *      BBuGetFrameTime        - Calculate data frame transmitting time
  *      BBvCalculateParameter   - Calculate PhyLength, PhyService and Phy Signal parameter for baseband Tx
  *      BBbVT3184Init          - VIA VT3184 baseband chip init code
- *      BBvLoopbackOn          - Turn on BaseBand Loopback mode
- *      BBvLoopbackOff         - Turn off BaseBand Loopback mode
  *
  * Revision History:
  *
@@ -47,26 +45,10 @@
 #include "datarate.h"
 #include "rndis.h"
 
-/*---------------------  Static Definitions -------------------------*/
 static int          msglevel                =MSG_LEVEL_INFO;
 //static int          msglevel                =MSG_LEVEL_DEBUG;
 
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
-
-BYTE abyVT3184_AGC[] = {
+u8 abyVT3184_AGC[] = {
     0x00,   //0
     0x00,   //1
     0x02,   //2
@@ -133,8 +115,7 @@ BYTE abyVT3184_AGC[] = {
     0x3E    //3F
 };
 
-
-BYTE abyVT3184_AL2230[] = {
+u8 abyVT3184_AL2230[] = {
         0x31,//00
         0x00,
         0x00,
@@ -393,10 +374,8 @@ BYTE abyVT3184_AL2230[] = {
         0x00
 };
 
-
-
 //{{RobertYu:20060515, new BB setting for VT3226D0
-BYTE abyVT3184_VT3226D0[] = {
+u8 abyVT3184_VT3226D0[] = {
         0x31,//00
         0x00,
         0x00,
@@ -655,11 +634,9 @@ BYTE abyVT3184_VT3226D0[] = {
         0x00,
 };
 
-const WORD awcFrameTime[MAX_RATE] =
+const u16 awcFrameTime[MAX_RATE] =
 {10, 20, 55, 110, 24, 36, 48, 72, 96, 144, 192, 216};
 
-/*---------------------  Static Functions  --------------------------*/
-
 /*
 static
 unsigned long
@@ -674,7 +651,6 @@ void
 s_vClearSQ3Value(PSDevice pDevice);
 */
 
-/*---------------------  Export Variables  --------------------------*/
 /*
  * Description: Calculate data frame transmitting time
  *
@@ -691,10 +667,10 @@ s_vClearSQ3Value(PSDevice pDevice);
  */
 unsigned int
 BBuGetFrameTime(
-     BYTE byPreambleType,
-     BYTE byPktType,
+     u8 byPreambleType,
+     u8 byPktType,
      unsigned int cbFrameLength,
-     WORD wRate
+     u16 wRate
     )
 {
     unsigned int uFrameTime;
@@ -703,7 +679,6 @@ BBuGetFrameTime(
     unsigned int uRateIdx = (unsigned int)wRate;
     unsigned int uRate = 0;
 
-
     if (uRateIdx > RATE_54M) {
         ASSERT(0);
         return 0;
@@ -900,15 +875,14 @@ void BBvCalculateParameter(struct vnt_private *pDevice, u32 cbFrameLength,
         *pbyPhySrv = 0x00;
         if (bExtBit)
             *pbyPhySrv = *pbyPhySrv | 0x80;
-        *pwPhyLen = (WORD) cbUsCount;
+        *pwPhyLen = (u16) cbUsCount;
     }
     else {
         *pbyPhySrv = 0x00;
-        *pwPhyLen = (WORD)cbFrameLength;
+        *pwPhyLen = (u16)cbFrameLength;
     }
 }
 
-
 /*
  * Description: Set Antenna mode
  *
@@ -937,10 +911,9 @@ void BBvSetAntennaMode(struct vnt_private *pDevice, u8 byAntennaMode)
             break;
     }
 
-
     CONTROLnsRequestOut(pDevice,
                     MESSAGE_TYPE_SET_ANTMD,
-                    (WORD) byAntennaMode,
+                    (u16) byAntennaMode,
                     0,
                     0,
                     NULL);
@@ -963,11 +936,11 @@ void BBvSetAntennaMode(struct vnt_private *pDevice, u8 byAntennaMode)
 int BBbVT3184Init(struct vnt_private *pDevice)
 {
 	int ntStatus;
-    WORD                    wLength;
-    PBYTE                   pbyAddr;
-    PBYTE                   pbyAgc;
-    WORD                    wLengthAgc;
-    BYTE                    abyArray[256];
+    u16                    wLength;
+    u8 *                   pbyAddr;
+    u8 *                   pbyAgc;
+    u16                    wLengthAgc;
+    u8                    abyArray[256];
 
     ntStatus = CONTROLnsRequestIn(pDevice,
                                   MESSAGE_TYPE_READ,
@@ -979,7 +952,6 @@ int BBbVT3184Init(struct vnt_private *pDevice)
         return false;
     }
 
-
 //    if ((pDevice->abyEEPROM[EEP_OFS_RADIOCTL]&0x06)==0x04)
 //        return false;
 
@@ -1118,7 +1090,6 @@ else {
                     abyArray
                     );
 
-
     if ((pDevice->byRFType == RF_VT3226) || //RobertYu:20051116, 20060111 remove VT3226D0
          (pDevice->byRFType == RF_VT3342A0)  //RobertYu:20060609
          ) {
@@ -1131,7 +1102,6 @@ else {
         MACvRegBitsOn(pDevice,MAC_REG_PAPEDELAY,0x01);
     }
 
-
     ControlvWriteByte(pDevice,MESSAGE_REQUEST_BBREG,0x04,0x7F);
     ControlvWriteByte(pDevice,MESSAGE_REQUEST_BBREG,0x0D,0x01);
 
@@ -1139,96 +1109,6 @@ else {
     return true;//ntStatus;
 }
 
-
-/*
- * Description: Turn on BaseBand Loopback mode
- *
- * Parameters:
- *  In:
- *      pDevice         - Device Structure
- *
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void BBvLoopbackOn(struct vnt_private *pDevice)
-{
-    BYTE      byData;
-
-    //CR C9 = 0x00
-    ControlvReadByte (pDevice, MESSAGE_REQUEST_BBREG, 0xC9, &pDevice->byBBCRc9);//CR201
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0);
-    ControlvReadByte (pDevice, MESSAGE_REQUEST_BBREG, 0x4D, &pDevice->byBBCR4d);//CR77
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x4D, 0x90);
-
-    //CR 88 = 0x02(CCK), 0x03(OFDM)
-    ControlvReadByte (pDevice, MESSAGE_REQUEST_BBREG, 0x88, &pDevice->byBBCR88);//CR136
-
-    if (pDevice->wCurrentRate <= RATE_11M) { //CCK
-        // Enable internal digital loopback: CR33 |= 0000 0001
-        ControlvReadByte (pDevice, MESSAGE_REQUEST_BBREG, 0x21, &byData);//CR33
-        ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x21, (BYTE)(byData | 0x01));//CR33
-        // CR154 = 0x00
-        ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x9A, 0);   //CR154
-
-        ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x88, 0x02);//CR239
-    }
-    else { //OFDM
-        // Enable internal digital loopback:CR154 |= 0000 0001
-        ControlvReadByte (pDevice, MESSAGE_REQUEST_BBREG, 0x9A, &byData);//CR154
-        ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x9A, (BYTE)(byData | 0x01));//CR154
-        // CR33 = 0x00
-        ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x21, 0);   //CR33
-
-        ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x88, 0x03);//CR239
-    }
-
-    //CR14 = 0x00
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x0E, 0);//CR14
-
-    // Disable TX_IQUN
-    ControlvReadByte (pDevice, MESSAGE_REQUEST_BBREG, 0x09, &pDevice->byBBCR09);
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x09, (BYTE)(pDevice->byBBCR09 & 0xDE));
-}
-
-/*
- * Description: Turn off BaseBand Loopback mode
- *
- * Parameters:
- *  In:
- *      pDevice         - Device Structure
- *
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void BBvLoopbackOff(struct vnt_private *pDevice)
-{
-	u8 byData;
-
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, pDevice->byBBCRc9);//CR201
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x88, pDevice->byBBCR88);//CR136
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x09, pDevice->byBBCR09);//CR136
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x4D, pDevice->byBBCR4d);//CR77
-
-    if (pDevice->wCurrentRate <= RATE_11M) { // CCK
-        // Set the CR33 Bit2 to disable internal Loopback.
-        ControlvReadByte (pDevice, MESSAGE_REQUEST_BBREG, 0x21, &byData);//CR33
-        ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x21, (BYTE)(byData & 0xFE));//CR33
-	} else { /* OFDM */
-        ControlvReadByte (pDevice, MESSAGE_REQUEST_BBREG, 0x9A, &byData);//CR154
-        ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x9A, (BYTE)(byData & 0xFE));//CR154
-    }
-    ControlvReadByte (pDevice, MESSAGE_REQUEST_BBREG, 0x0E, &byData);//CR14
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x0E, (BYTE)(byData | 0x80));//CR14
-
-}
-
-
 /*
  * Description: Set ShortSlotTime mode
  *
@@ -1243,7 +1123,7 @@ void BBvLoopbackOff(struct vnt_private *pDevice)
  */
 void BBvSetShortSlotTime(struct vnt_private *pDevice)
 {
-    BYTE byBBVGA=0;
+    u8 byBBVGA=0;
 
 	if (pDevice->bShortSlotTime)
         pDevice->byBBRxConf &= 0xDF;//1101 1111
@@ -1257,8 +1137,7 @@ void BBvSetShortSlotTime(struct vnt_private *pDevice)
     ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x0A, pDevice->byBBRxConf);
 }
 
-
-void BBvSetVGAGainOffset(struct vnt_private *pDevice, BYTE byData)
+void BBvSetVGAGainOffset(struct vnt_private *pDevice, u8 byData)
 {
 
     ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xE7, byData);
@@ -1272,27 +1151,6 @@ void BBvSetVGAGainOffset(struct vnt_private *pDevice, BYTE byData)
     ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x0A, pDevice->byBBRxConf);//CR10
 }
 
-
-/*
- * Description: Baseband SoftwareReset
- *
- * Parameters:
- *  In:
- *      dwIoBase    - I/O base address
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void BBvSoftwareReset(struct vnt_private *pDevice)
-{
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x50, 0x40);
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x50, 0);
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x9C, 0x01);
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x9C, 0);
-}
-
 /*
  * Description: BBvSetDeepSleep
  *
@@ -1317,7 +1175,6 @@ void BBvExitDeepSleep(struct vnt_private *pDevice)
     ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x0D, 0x01);//CR13
 }
 
-
 static unsigned long s_ulGetLowSQ3(struct vnt_private *pDevice)
 {
 	int ii;
@@ -1364,7 +1221,6 @@ static unsigned long s_ulGetRatio(struct vnt_private *pDevice)
     return ulRatio;
 }
 
-
 static void s_vClearSQ3Value(struct vnt_private *pDevice)
 {
     int ii;
@@ -1376,7 +1232,6 @@ static void s_vClearSQ3Value(struct vnt_private *pDevice)
     }
 }
 
-
 /*
  * Description: Antenna Diversity
  *
@@ -1508,7 +1363,6 @@ void BBvAntennaDiversity(struct vnt_private *pDevice,
     } //byAntennaState
 }
 
-
 /*+
  *
  * Description:
@@ -1541,11 +1395,9 @@ void TimerSQ3CallBack(struct vnt_private *pDevice)
     add_timer(&pDevice->TimerSQ3Tmax3);
     add_timer(&pDevice->TimerSQ3Tmax2);
 
-
     spin_unlock_irq(&pDevice->lock);
 }
 
-
 /*+
  *
  * Description:
@@ -1594,7 +1446,6 @@ void TimerSQ3Tmax3CallBack(struct vnt_private *pDevice)
 void BBvUpdatePreEDThreshold(struct vnt_private *pDevice, int bScanning)
 {
 
-
     switch(pDevice->byRFType)
     {
         case RF_AL2230:
diff --git a/drivers/staging/vt6656/baseband.h b/drivers/staging/vt6656/baseband.h
index fba61605a69..0a634adabf0 100644
--- a/drivers/staging/vt6656/baseband.h
+++ b/drivers/staging/vt6656/baseband.h
@@ -33,12 +33,9 @@
 #ifndef __BASEBAND_H__
 #define __BASEBAND_H__
 
-#include "ttype.h"
 #include "tether.h"
 #include "device.h"
 
-/*---------------------  Export Definitions -------------------------*/
-
 #define PREAMBLE_LONG   0
 #define PREAMBLE_SHORT  1
 
@@ -84,23 +81,12 @@
 #define TOP_RATE_2M         0x00200000
 #define TOP_RATE_1M         0x00100000
 
-
-/*---------------------  Export Types  ------------------------------*/
-
-/*---------------------  Export Macros ------------------------------*/
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
 unsigned int
 BBuGetFrameTime(
-     BYTE byPreambleType,
-     BYTE byFreqType,
+     u8 byPreambleType,
+     u8 byFreqType,
      unsigned int cbFrameLength,
-     WORD wRate
+     u16 wRate
     );
 
 void BBvCalculateParameter(struct vnt_private *, u32 cbFrameLength,
@@ -113,9 +99,6 @@ void TimerSQ3CallBack(struct vnt_private *);
 void TimerSQ3Tmax3CallBack(struct vnt_private *);
 
 void BBvAntennaDiversity(struct vnt_private *, u8 byRxRate, u8 bySQ3);
-void BBvLoopbackOn(struct vnt_private *);
-void BBvLoopbackOff(struct vnt_private *);
-void BBvSoftwareReset(struct vnt_private *);
 
 void BBvSetShortSlotTime(struct vnt_private *);
 void BBvSetVGAGainOffset(struct vnt_private *, u8 byData);
diff --git a/drivers/staging/vt6656/bssdb.c b/drivers/staging/vt6656/bssdb.c
index e214fcf8386..ee79bbdf1a0 100644
--- a/drivers/staging/vt6656/bssdb.c
+++ b/drivers/staging/vt6656/bssdb.c
@@ -39,7 +39,6 @@
  *
  */
 
-#include "ttype.h"
 #include "tmacro.h"
 #include "tether.h"
 #include "device.h"
@@ -59,27 +58,17 @@
 #include "rndis.h"
 #include "iowpa.h"
 
-/*---------------------  Static Definitions -------------------------*/
-
-
-
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
 static int          msglevel                =MSG_LEVEL_INFO;
 //static int          msglevel                =MSG_LEVEL_DEBUG;
 
-
-
-const WORD             awHWRetry0[5][5] = {
+const u16             awHWRetry0[5][5] = {
                                             {RATE_18M, RATE_18M, RATE_12M, RATE_12M, RATE_12M},
                                             {RATE_24M, RATE_24M, RATE_18M, RATE_12M, RATE_12M},
                                             {RATE_36M, RATE_36M, RATE_24M, RATE_18M, RATE_18M},
                                             {RATE_48M, RATE_48M, RATE_36M, RATE_24M, RATE_24M},
                                             {RATE_54M, RATE_54M, RATE_48M, RATE_36M, RATE_36M}
                                            };
-const WORD             awHWRetry1[5][5] = {
+const u16             awHWRetry1[5][5] = {
                                             {RATE_18M, RATE_18M, RATE_12M, RATE_6M, RATE_6M},
                                             {RATE_24M, RATE_24M, RATE_18M, RATE_6M, RATE_6M},
                                             {RATE_36M, RATE_36M, RATE_24M, RATE_12M, RATE_12M},
@@ -87,23 +76,10 @@ const WORD             awHWRetry1[5][5] = {
                                             {RATE_54M, RATE_54M, RATE_36M, RATE_18M, RATE_18M}
                                            };
 
-
-
-/*---------------------  Static Functions  --------------------------*/
-
 static void s_vCheckSensitivity(struct vnt_private *pDevice);
 static void s_vCheckPreEDThreshold(struct vnt_private *pDevice);
 static void s_uCalculateLinkQual(struct vnt_private *pDevice);
 
-/*---------------------  Export Variables  --------------------------*/
-
-
-/*---------------------  Export Functions  --------------------------*/
-
-
-
-
-
 /*+
  *
  * Routine Description:
@@ -221,7 +197,6 @@ PKnownBSS BSSpSearchBSSList(struct vnt_private *pDevice,
 			pCurrBSS->abyBSSID);
         jj++;
 
-
                 if (pSelect == NULL) {
                     pSelect = pCurrBSS;
                 } else {
@@ -249,7 +224,6 @@ pDevice->bSameBSSMaxNum = jj;
 
 }
 
-
 /*+
  *
  * Routine Description:
@@ -260,7 +234,6 @@ pDevice->bSameBSSMaxNum = jj;
  *
 -*/
 
-
 void BSSvClearBSSList(struct vnt_private *pDevice, int bKeepCurrBSSID)
 {
 	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
@@ -285,8 +258,6 @@ void BSSvClearBSSList(struct vnt_private *pDevice, int bKeepCurrBSSID)
     BSSvClearAnyBSSJoinRecord(pDevice);
 }
 
-
-
 /*+
  *
  * Routine Description:
@@ -320,8 +291,6 @@ PKnownBSS BSSpAddrIsInBSSList(struct vnt_private *pDevice,
     return NULL;
 };
 
-
-
 /*+
  *
  * Routine Description:
@@ -357,7 +326,6 @@ int BSSbInsertToBSSList(struct vnt_private *pDevice,
 	unsigned int ii;
 	bool bParsingQuiet = false;
 
-
     pBSSList = (PKnownBSS)&(pMgmt->sBSSList[0]);
 
     for (ii = 0; ii < MAX_BSS_NUM; ii++) {
@@ -430,7 +398,7 @@ int BSSbInsertToBSSList(struct vnt_private *pDevice,
 	unsigned int uLen = pRSNWPA->len + 2;
 
 	if (uLen <= (uIELength -
-		     (unsigned int) (ULONG_PTR) ((PBYTE) pRSNWPA - pbyIEs))) {
+		     (unsigned int) (u32) ((u8 *) pRSNWPA - pbyIEs))) {
 		pBSSList->wWPALen = uLen;
 		memcpy(pBSSList->byWPAIE, pRSNWPA, uLen);
 		WPA_ParseRSN(pBSSList, pRSNWPA);
@@ -443,7 +411,7 @@ int BSSbInsertToBSSList(struct vnt_private *pDevice,
 	unsigned int uLen = pRSN->len + 2;
 
 	if (uLen <= (uIELength -
-		     (unsigned int) (ULONG_PTR) ((PBYTE) pRSN - pbyIEs))) {
+		     (unsigned int) (u32) ((u8 *) pRSN - pbyIEs))) {
 		pBSSList->wRSNLen = uLen;
 		memcpy(pBSSList->byRSNIE, pRSN, uLen);
 		WPA2vParseRSN(pBSSList, pRSN);
@@ -483,7 +451,7 @@ int BSSbInsertToBSSList(struct vnt_private *pDevice,
     if (pDevice->bUpdateBBVGA) {
         // Monitor if RSSI is too strong.
         pBSSList->byRSSIStatCnt = 0;
-        RFvRSSITodBm(pDevice, (BYTE)(pRxPacket->uRSSI), &pBSSList->ldBmMAX);
+        RFvRSSITodBm(pDevice, (u8)(pRxPacket->uRSSI), &pBSSList->ldBmMAX);
         pBSSList->ldBmAverage[0] = pBSSList->ldBmMAX;
         pBSSList->ldBmAverRange = pBSSList->ldBmMAX;
         for (ii = 1; ii < RSSI_STAT_COUNT; ii++)
@@ -498,7 +466,6 @@ int BSSbInsertToBSSList(struct vnt_private *pDevice,
     return true;
 }
 
-
 /*+
  *
  * Routine Description:
@@ -539,7 +506,6 @@ int BSSbUpdateToBSSList(struct vnt_private *pDevice,
     if (pBSSList == NULL)
         return false;
 
-
 	pBSSList->qwBSSTimestamp = cpu_to_le64(qwTimestamp);
 
     pBSSList->wBeaconInterval = cpu_to_le16(wBeaconInterval);
@@ -592,7 +558,7 @@ int BSSbUpdateToBSSList(struct vnt_private *pDevice,
    if (pRSNWPA != NULL) {
 	unsigned int uLen = pRSNWPA->len + 2;
 	if (uLen <= (uIELength -
-		     (unsigned int) (ULONG_PTR) ((PBYTE) pRSNWPA - pbyIEs))) {
+		     (unsigned int) (u32) ((u8 *) pRSNWPA - pbyIEs))) {
 		pBSSList->wWPALen = uLen;
 		memcpy(pBSSList->byWPAIE, pRSNWPA, uLen);
 		WPA_ParseRSN(pBSSList, pRSNWPA);
@@ -604,7 +570,7 @@ int BSSbUpdateToBSSList(struct vnt_private *pDevice,
     if (pRSN != NULL) {
 	unsigned int uLen = pRSN->len + 2;
 	if (uLen <= (uIELength -
-			(unsigned int) (ULONG_PTR) ((PBYTE) pRSN - pbyIEs))) {
+			(unsigned int) (u32) ((u8 *) pRSN - pbyIEs))) {
 		pBSSList->wRSNLen = uLen;
 		memcpy(pBSSList->byRSNIE, pRSN, uLen);
 		WPA2vParseRSN(pBSSList, pRSN);
@@ -612,7 +578,7 @@ int BSSbUpdateToBSSList(struct vnt_private *pDevice,
     }
 
     if (pRxPacket->uRSSI != 0) {
-        RFvRSSITodBm(pDevice, (BYTE)(pRxPacket->uRSSI), &ldBm);
+        RFvRSSITodBm(pDevice, (u8)(pRxPacket->uRSSI), &ldBm);
         // Monitor if RSSI is too strong.
         pBSSList->byRSSIStatCnt++;
         pBSSList->byRSSIStatCnt %= RSSI_STAT_COUNT;
@@ -638,10 +604,6 @@ int BSSbUpdateToBSSList(struct vnt_private *pDevice,
     return true;
 }
 
-
-
-
-
 /*+
  *
  * Routine Description:
@@ -672,8 +634,6 @@ int BSSbIsSTAInNodeDB(struct vnt_private *pDevice,
    return false;
 };
 
-
-
 /*+
  *
  * Routine Description:
@@ -731,8 +691,6 @@ void BSSvCreateOneNode(struct vnt_private *pDevice, u32 *puNodeIndex)
     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Create node index = %d\n", ii);
 };
 
-
-
 /*+
  *
  * Routine Description:
@@ -750,7 +708,6 @@ void BSSvRemoveOneNode(struct vnt_private *pDevice, u32 uNodeIndex)
 	u8 byMask[8] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80};
 	struct sk_buff  *skb;
 
-
     while ((skb = skb_dequeue(&pMgmt->sNodeDBTable[uNodeIndex].sTxPSQueue)) != NULL)
             dev_kfree_skb(skb);
     // clear context
@@ -948,7 +905,6 @@ if(pDevice->byReAssocCount > 0) {
                 if (pMgmt->sNodeDBTable[ii].bPSEnable)
                     uSleepySTACnt++;
 
-
             }
 
             // Rate fallback check
@@ -981,7 +937,6 @@ if(pDevice->byReAssocCount > 0) {
 
     }
 
-
     if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) && (pDevice->byBBType == BB_TYPE_11G)) {
 
         // on/off protect mode
@@ -1031,7 +986,6 @@ if(pDevice->byReAssocCount > 0) {
 
     }
 
-
     // Check if any STA in PS mode, enable DTIM multicast deliver
     if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
         if (uSleepySTACnt > 0)
@@ -1205,9 +1159,9 @@ void BSSvUpdateNodeTxCounter(struct vnt_private *pDevice,
 
     byPktNum = (byPktNO & 0x0F) >> 4;
     byTxRetry = (byTSR & 0xF0) >> 4;
-    wRate = (WORD) (byPktNO & 0xF0) >> 4;
+    wRate = (u16) (byPktNO & 0xF0) >> 4;
     wFIFOCtl = pStatistic->abyTxPktInfo[byPktNum].wFIFOCtl;
-    pbyDestAddr = (PBYTE) &( pStatistic->abyTxPktInfo[byPktNum].abyDestAddr[0]);
+    pbyDestAddr = (u8 *) &( pStatistic->abyTxPktInfo[byPktNum].abyDestAddr[0]);
 
     if (wFIFOCtl & FIFOCTL_AUTO_FB_0) {
         byFallBack = AUTO_FB_0;
@@ -1433,7 +1387,7 @@ if(pDevice->bLinkPass !=true)
 }
 else
 {
-   RFvRSSITodBm(pDevice, (BYTE)(pDevice->uCurrRSSI), &ldBm);
+   RFvRSSITodBm(pDevice, (u8)(pDevice->uCurrRSSI), &ldBm);
    if(-ldBm < 50)  {
    	RssiRatio = 4000;
      }
@@ -1473,7 +1427,7 @@ static void s_vCheckPreEDThreshold(struct vnt_private *pDevice)
         ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) && (pMgmt->eCurrState == WMAC_STATE_JOINTED))) {
         pBSSList = BSSpAddrIsInBSSList(pDevice, pMgmt->abyCurrBSSID, (PWLAN_IE_SSID)pMgmt->abyCurrSSID);
         if (pBSSList != NULL) {
-            pDevice->byBBPreEDRSSI = (BYTE) (~(pBSSList->ldBmAverRange) + 1);
+            pDevice->byBBPreEDRSSI = (u8) (~(pBSSList->ldBmAverRange) + 1);
             BBvUpdatePreEDThreshold(pDevice, false);
         }
     }
diff --git a/drivers/staging/vt6656/bssdb.h b/drivers/staging/vt6656/bssdb.h
index 08091a0a7c4..bce3b465416 100644
--- a/drivers/staging/vt6656/bssdb.h
+++ b/drivers/staging/vt6656/bssdb.h
@@ -36,8 +36,6 @@
 #include "card.h"
 #include "mib.h"
 
-/*---------------------  Export Definitions -------------------------*/
-
 #define MAX_NODE_NUM             64
 #define MAX_BSS_NUM              42
 #define LOST_BEACON_COUNT        10   /* 10 sec, XP defined */
@@ -66,46 +64,37 @@
 
 #define MAX_WPA_IE_LEN      64
 
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-
-/*---------------------  Export Types  ------------------------------*/
-
 //
 // IEEE 802.11 Structures and definitions
 //
 
 typedef struct tagSERPObject {
     bool    bERPExist;
-    BYTE    byERP;
+    u8    byERP;
 } ERPObject, *PERPObject;
 
-
 typedef struct tagSRSNCapObject {
     bool    bRSNCapExist;
-    WORD    wRSNCap;
+    u16    wRSNCap;
 } SRSNCapObject, *PSRSNCapObject;
 
 // BSS info(AP)
 typedef struct tagKnownBSS {
     // BSS info
     bool            bActive;
-    BYTE            abyBSSID[WLAN_BSSID_LEN];
+    u8            abyBSSID[WLAN_BSSID_LEN];
     unsigned int            uChannel;
-    BYTE            abySuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
-    BYTE            abyExtSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
+    u8            abySuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
+    u8            abyExtSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
     unsigned int            uRSSI;
-    BYTE            bySQ;
-    WORD            wBeaconInterval;
-    WORD            wCapInfo;
-    BYTE            abySSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
-    BYTE            byRxRate;
-
-//    WORD            wATIMWindow;
-    BYTE            byRSSIStatCnt;
+    u8            bySQ;
+    u16            wBeaconInterval;
+    u16            wCapInfo;
+    u8            abySSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
+    u8            byRxRate;
+
+//    u16            wATIMWindow;
+    u8            byRSSIStatCnt;
     signed long            ldBmMAX;
     signed long            ldBmAverage[RSSI_STAT_COUNT];
     signed long            ldBmAverRange;
@@ -114,32 +103,32 @@ typedef struct tagKnownBSS {
 
     //++ WPA informations
     bool            bWPAValid;
-    BYTE            byGKType;
-    BYTE            abyPKType[4];
-    WORD            wPKCount;
-    BYTE            abyAuthType[4];
-    WORD            wAuthCount;
-    BYTE            byDefaultK_as_PK;
-    BYTE            byReplayIdx;
+    u8            byGKType;
+    u8            abyPKType[4];
+    u16            wPKCount;
+    u8            abyAuthType[4];
+    u16            wAuthCount;
+    u8            byDefaultK_as_PK;
+    u8            byReplayIdx;
     //--
 
     //++ WPA2 informations
     bool            bWPA2Valid;
-    BYTE            byCSSGK;
-    WORD            wCSSPKCount;
-    BYTE            abyCSSPK[4];
-    WORD            wAKMSSAuthCount;
-    BYTE            abyAKMSSAuthType[4];
+    u8            byCSSGK;
+    u16            wCSSPKCount;
+    u8            abyCSSPK[4];
+    u16            wAKMSSAuthCount;
+    u8            abyAKMSSAuthType[4];
 
     //++  wpactl
-    BYTE            byWPAIE[MAX_WPA_IE_LEN];
-    BYTE            byRSNIE[MAX_WPA_IE_LEN];
-    WORD            wWPALen;
-    WORD            wRSNLen;
+    u8            byWPAIE[MAX_WPA_IE_LEN];
+    u8            byRSNIE[MAX_WPA_IE_LEN];
+    u16            wWPALen;
+    u16            wRSNLen;
 
     // Clear count
     unsigned int            uClearCount;
-//    BYTE            abyIEs[WLAN_BEACON_FR_MAXLEN];
+//    u8            abyIEs[WLAN_BEACON_FR_MAXLEN];
     unsigned int            uIELength;
 	u64 qwBSSTimestamp;
 	u64 qwLocalTSF;/* local TSF timer */
@@ -148,13 +137,11 @@ typedef struct tagKnownBSS {
 
     ERPObject       sERP;
     SRSNCapObject   sRSNCapObj;
-    BYTE            abyIEs[1024];   // don't move this field !!
+    u8            abyIEs[1024];   // don't move this field !!
 
 } __attribute__ ((__packed__))
 KnownBSS , *PKnownBSS;
 
-
-
 typedef enum tagNODE_STATE {
     NODE_FREE,
     NODE_AGED,
@@ -163,48 +150,47 @@ typedef enum tagNODE_STATE {
     NODE_ASSOC
 } NODE_STATE, *PNODE_STATE;
 
-
 // STA node info
 typedef struct tagKnownNodeDB {
     // STA info
     bool            bActive;
-    BYTE            abyMACAddr[WLAN_ADDR_LEN];
-    BYTE            abyCurrSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN];
-    BYTE            abyCurrExtSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN];
-    WORD            wTxDataRate;
+    u8            abyMACAddr[WLAN_ADDR_LEN];
+    u8            abyCurrSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN];
+    u8            abyCurrExtSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN];
+    u16            wTxDataRate;
     bool            bShortPreamble;
     bool            bERPExist;
     bool            bShortSlotTime;
     unsigned int            uInActiveCount;
-    WORD            wMaxBasicRate;     //Get from byTopOFDMBasicRate or byTopCCKBasicRate which depends on packetTyp.
-    WORD            wMaxSuppRate;      //Records the highest supported rate getting from SuppRates IE and ExtSuppRates IE in Beacon.
-    WORD            wSuppRate;
-    BYTE            byTopOFDMBasicRate;//Records the highest basic rate in OFDM mode
-    BYTE            byTopCCKBasicRate; //Records the highest basic rate in CCK mode
+    u16            wMaxBasicRate;     //Get from byTopOFDMBasicRate or byTopCCKBasicRate which depends on packetTyp.
+    u16            wMaxSuppRate;      //Records the highest supported rate getting from SuppRates IE and ExtSuppRates IE in Beacon.
+    u16            wSuppRate;
+    u8            byTopOFDMBasicRate;//Records the highest basic rate in OFDM mode
+    u8            byTopCCKBasicRate; //Records the highest basic rate in CCK mode
 
     // For AP mode
     struct sk_buff_head sTxPSQueue;
-    WORD            wCapInfo;
-    WORD            wListenInterval;
-    WORD            wAID;
+    u16            wCapInfo;
+    u16            wListenInterval;
+    u16            wAID;
     NODE_STATE      eNodeState;
     bool            bPSEnable;
     bool            bRxPSPoll;
-    BYTE            byAuthSequence;
+    u8            byAuthSequence;
     unsigned long           ulLastRxJiffer;
-    BYTE            bySuppRate;
-    DWORD           dwFlags;
-    WORD            wEnQueueCnt;
+    u8            bySuppRate;
+    u32           dwFlags;
+    u16            wEnQueueCnt;
 
     bool            bOnFly;
     unsigned long long       KeyRSC;
-    BYTE            byKeyIndex;
-    DWORD           dwKeyIndex;
-    BYTE            byCipherSuite;
-    DWORD           dwTSC47_16;
-    WORD            wTSC15_0;
+    u8            byKeyIndex;
+    u32           dwKeyIndex;
+    u8            byCipherSuite;
+    u32           dwTSC47_16;
+    u16            wTSC15_0;
     unsigned int            uWepKeyLength;
-    BYTE            abyWepKey[WLAN_WEPMAX_KEYLEN];
+    u8            abyWepKey[WLAN_WEPMAX_KEYLEN];
     //
     // Auto rate fallback vars
     bool            bIsInFallback;
@@ -223,8 +209,6 @@ typedef struct tagKnownNodeDB {
 
 } KnownNodeDB, *PKnownNodeDB;
 
-/*---------------------  Export Functions  --------------------------*/
-
 PKnownBSS BSSpSearchBSSList(struct vnt_private *, u8 *pbyDesireBSSID,
 	u8 *pbyDesireSSID, CARD_PHY_TYPE ePhyType);
 
@@ -270,7 +254,7 @@ int BSSbUpdateToBSSList(struct vnt_private *,
 			u8 *pbyIEs,
 			void *pRxPacketContext);
 
-int BSSbIsSTAInNodeDB(struct vnt_private *, PBYTE abyDstAddr,
+int BSSbIsSTAInNodeDB(struct vnt_private *, u8 * abyDstAddr,
 	u32 *puNodeIndex);
 
 void BSSvCreateOneNode(struct vnt_private *, u32 *puNodeIndex);
diff --git a/drivers/staging/vt6656/card.c b/drivers/staging/vt6656/card.c
index d2479b76645..24291aee58b 100644
--- a/drivers/staging/vt6656/card.c
+++ b/drivers/staging/vt6656/card.c
@@ -60,28 +60,15 @@
 #include "rndis.h"
 #include "control.h"
 
-/*---------------------  Static Definitions -------------------------*/
-
 //static int          msglevel                =MSG_LEVEL_DEBUG;
 static int          msglevel                =MSG_LEVEL_INFO;
 
-
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-//const WORD cwRXBCNTSFOff[MAX_RATE] =
+//const u16 cwRXBCNTSFOff[MAX_RATE] =
 //{17, 34, 96, 192, 34, 23, 17, 11, 8, 5, 4, 3};
 
-const WORD cwRXBCNTSFOff[MAX_RATE] =
+const u16 cwRXBCNTSFOff[MAX_RATE] =
 {192, 96, 34, 17, 34, 23, 17, 11, 8, 5, 4, 3};
 
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
 /*
  * Description: Set NIC media channel
  *
@@ -114,7 +101,7 @@ void CARDbSetMediaChannel(struct vnt_private *pDevice, u32 uConnectionChannel)
 
     CONTROLnsRequestOut(pDevice,
                         MESSAGE_TYPE_SELECT_CHANNLE,
-                        (WORD) uConnectionChannel,
+                        (u16) uConnectionChannel,
                         0,
                         0,
                         NULL
@@ -133,7 +120,7 @@ void CARDbSetMediaChannel(struct vnt_private *pDevice, u32 uConnectionChannel)
         pDevice->byCurPwr = 0xFF;
         RFbRawSetPower(pDevice, pDevice->abyCCKPwrTbl[uConnectionChannel-1], RATE_1M);
     }
-    ControlvWriteByte(pDevice,MESSAGE_REQUEST_MACREG,MAC_REG_CHANNEL,(BYTE)(uConnectionChannel|0x80));
+    ControlvWriteByte(pDevice,MESSAGE_REQUEST_MACREG,MAC_REG_CHANNEL,(u8)(uConnectionChannel|0x80));
 }
 
 /*
@@ -220,10 +207,10 @@ static u16 swGetOFDMControlRate(struct vnt_private *pDevice, u16 wRateIdx)
  */
 void
 CARDvCalculateOFDMRParameter (
-      WORD wRate,
-      BYTE byBBType,
-     PBYTE pbyTxRate,
-     PBYTE pbyRsvTime
+      u16 wRate,
+      u8 byBBType,
+     u8 * pbyTxRate,
+     u8 * pbyRsvTime
     )
 {
     switch (wRate) {
@@ -434,23 +421,23 @@ void CARDvSetRSPINF(struct vnt_private *pDevice, u8 byBBType)
                                  &abyTxRate[8],
                                  &abyRsvTime[8]);
 
-    abyData[0] = (BYTE)(awLen[0]&0xFF);
-    abyData[1] = (BYTE)(awLen[0]>>8);
+    abyData[0] = (u8)(awLen[0]&0xFF);
+    abyData[1] = (u8)(awLen[0]>>8);
     abyData[2] = abySignal[0];
     abyData[3] = abyServ[0];
 
-    abyData[4] = (BYTE)(awLen[1]&0xFF);
-    abyData[5] = (BYTE)(awLen[1]>>8);
+    abyData[4] = (u8)(awLen[1]&0xFF);
+    abyData[5] = (u8)(awLen[1]>>8);
     abyData[6] = abySignal[1];
     abyData[7] = abyServ[1];
 
-    abyData[8] = (BYTE)(awLen[2]&0xFF);
-    abyData[9] = (BYTE)(awLen[2]>>8);
+    abyData[8] = (u8)(awLen[2]&0xFF);
+    abyData[9] = (u8)(awLen[2]>>8);
     abyData[10] = abySignal[2];
     abyData[11] = abyServ[2];
 
-    abyData[12] = (BYTE)(awLen[3]&0xFF);
-    abyData[13] = (BYTE)(awLen[3]>>8);
+    abyData[12] = (u8)(awLen[3]&0xFF);
+    abyData[13] = (u8)(awLen[3]>>8);
     abyData[14] = abySignal[3];
     abyData[15] = abyServ[3];
 
@@ -500,7 +487,7 @@ void vUpdateIFS(struct vnt_private *pDevice)
         byMaxMin = 5;
     }
     else {// PK_TYPE_11GA & PK_TYPE_11GB
-        BYTE byRate = 0;
+        u8 byRate = 0;
         bool bOFDMRate = false;
 	unsigned int ii = 0;
         PWLAN_IE_SUPP_RATES pItemRates = NULL;
@@ -515,7 +502,7 @@ void vUpdateIFS(struct vnt_private *pDevice)
 
 	pItemRates = (PWLAN_IE_SUPP_RATES)pDevice->vnt_mgmt.abyCurrSuppRates;
         for (ii = 0; ii < pItemRates->len; ii++) {
-            byRate = (BYTE)(pItemRates->abyRates[ii]&0x7F);
+            byRate = (u8)(pItemRates->abyRates[ii]&0x7F);
             if (RATEwGetRateIdx(byRate) > RATE_11M) {
                 bOFDMRate = true;
                 break;
@@ -525,7 +512,7 @@ void vUpdateIFS(struct vnt_private *pDevice)
 		pItemRates = (PWLAN_IE_SUPP_RATES)pDevice->vnt_mgmt
 			.abyCurrExtSuppRates;
             for (ii = 0; ii < pItemRates->len; ii++) {
-                byRate = (BYTE)(pItemRates->abyRates[ii]&0x7F);
+                byRate = (u8)(pItemRates->abyRates[ii]&0x7F);
                 if (RATEwGetRateIdx(byRate) > RATE_11M) {
                     bOFDMRate = true;
                     break;
@@ -544,10 +531,10 @@ void vUpdateIFS(struct vnt_private *pDevice)
     pDevice->uCwMax = C_CWMAX;
     pDevice->uEIFS = C_EIFS;
 
-    byData[0] = (BYTE)pDevice->uSIFS;
-    byData[1] = (BYTE)pDevice->uDIFS;
-    byData[2] = (BYTE)pDevice->uEIFS;
-    byData[3] = (BYTE)pDevice->uSlot;
+    byData[0] = (u8)pDevice->uSIFS;
+    byData[1] = (u8)pDevice->uDIFS;
+    byData[2] = (u8)pDevice->uEIFS;
+    byData[3] = (u8)pDevice->uSlot;
     CONTROLnsRequestOut(pDevice,
                         MESSAGE_TYPE_WRITE,
                         MAC_REG_SIFS,
@@ -571,7 +558,7 @@ void CARDvUpdateBasicTopRate(struct vnt_private *pDevice)
 
      //Determines the highest basic rate.
      for (ii = RATE_54M; ii >= RATE_6M; ii --) {
-         if ( (pDevice->wBasicRate) & ((WORD)(1<<ii)) ) {
+         if ( (pDevice->wBasicRate) & ((u16)(1<<ii)) ) {
              byTopOFDM = ii;
              break;
          }
@@ -579,7 +566,7 @@ void CARDvUpdateBasicTopRate(struct vnt_private *pDevice)
      pDevice->byTopOFDMBasicRate = byTopOFDM;
 
      for (ii = RATE_11M;; ii --) {
-         if ( (pDevice->wBasicRate) & ((WORD)(1<<ii)) ) {
+         if ( (pDevice->wBasicRate) & ((u16)(1<<ii)) ) {
              byTopCCK = ii;
              break;
          }
@@ -617,7 +604,7 @@ int CARDbIsOFDMinBasicRate(struct vnt_private *pDevice)
 	int ii;
 
     for (ii = RATE_54M; ii >= RATE_6M; ii --) {
-        if ((pDevice->wBasicRate) & ((WORD)(1<<ii)))
+        if ((pDevice->wBasicRate) & ((u16)(1<<ii)))
             return true;
     }
     return false;
@@ -627,7 +614,7 @@ u8 CARDbyGetPktType(struct vnt_private *pDevice)
 {
 
     if (pDevice->byBBType == BB_TYPE_11A || pDevice->byBBType == BB_TYPE_11B) {
-        return (BYTE)pDevice->byBBType;
+        return (u8)pDevice->byBBType;
     }
     else if (CARDbIsOFDMinBasicRate(pDevice)) {
         return PK_TYPE_11GA;
@@ -637,7 +624,6 @@ u8 CARDbyGetPktType(struct vnt_private *pDevice)
     }
 }
 
-
 /*
  * Description: Calculate TSF offset of two TSF input
  *              Get TSF Offset from RxBCN's TSF and local TSF
@@ -653,10 +639,10 @@ u8 CARDbyGetPktType(struct vnt_private *pDevice)
  * Return Value: TSF Offset value
  *
  */
-u64 CARDqGetTSFOffset(BYTE byRxRate, u64 qwTSF1, u64 qwTSF2)
+u64 CARDqGetTSFOffset(u8 byRxRate, u64 qwTSF1, u64 qwTSF2)
 {
 	u64 qwTSFOffset = 0;
-	WORD wRxBcnTSFOffst = 0;
+	u16 wRxBcnTSFOffst = 0;
 
 	wRxBcnTSFOffst = cwRXBCNTSFOff[byRxRate % MAX_RATE];
 
@@ -667,8 +653,6 @@ u64 CARDqGetTSFOffset(BYTE byRxRate, u64 qwTSF1, u64 qwTSF2)
 	return qwTSFOffset;
 }
 
-
-
 /*
  * Description: Sync. TSF counter to BSS
  *              Get TSF offset and write to HW
@@ -690,7 +674,6 @@ void CARDvAdjustTSF(struct vnt_private *pDevice, u8 byRxRate,
 	u64 qwTSFOffset = 0;
 	u8 pbyData[8];
 
-
     qwTSFOffset = CARDqGetTSFOffset(byRxRate, qwBSSTimestamp, qwLocalTSF);
     // adjust TSF
     // HW's TSF add TSF Offset reg
@@ -734,7 +717,6 @@ bool CARDbGetCurrentTSF(struct vnt_private *pDevice, u64 *pqwCurrTSF)
 	return true;
 }
 
-
 /*
  * Description: Clear NIC TSF counter
  *              Clear local TSF counter
@@ -770,7 +752,7 @@ bool CARDbClearCurrentTSF(struct vnt_private *pDevice)
  * Return Value: TSF value of next Beacon
  *
  */
-u64 CARDqGetNextTBTT(u64 qwTSF, WORD wBeaconInterval)
+u64 CARDqGetNextTBTT(u64 qwTSF, u16 wBeaconInterval)
 {
 
     unsigned int    uLowNextTBTT;
@@ -796,7 +778,6 @@ u64 CARDqGetNextTBTT(u64 qwTSF, WORD wBeaconInterval)
     return (qwTSF);
 }
 
-
 /*
  * Description: Set NIC TSF counter for first Beacon time
  *              Get NEXTTBTT from adjusted TSF and Beacon Interval
@@ -811,7 +792,7 @@ u64 CARDqGetNextTBTT(u64 qwTSF, WORD wBeaconInterval)
  * Return Value: none
  *
  */
-void CARDvSetFirstNextTBTT(struct vnt_private *pDevice, WORD wBeaconInterval)
+void CARDvSetFirstNextTBTT(struct vnt_private *pDevice, u16 wBeaconInterval)
 {
 	u64 qwNextTBTT = 0;
 	u8 pbyData[8];
@@ -841,7 +822,6 @@ void CARDvSetFirstNextTBTT(struct vnt_private *pDevice, WORD wBeaconInterval)
     return;
 }
 
-
 /*
  * Description: Sync NIC TSF counter for Beacon time
  *              Get NEXTTBTT and write to HW
@@ -883,7 +863,6 @@ void CARDvUpdateNextTBTT(struct vnt_private *pDevice, u64 qwTSF,
                         pbyData
                         );
 
-
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
 		"Card:Update Next TBTT[%8lx]\n", (unsigned long)qwTSF);
 
@@ -929,7 +908,6 @@ int CARDbRadioPowerOff(struct vnt_private *pDevice)
     return bResult;
 }
 
-
 /*
  * Description: Turn on Radio power
  *
@@ -996,7 +974,7 @@ void CARDvSetBSSMode(struct vnt_private *pDevice)
     }
 
     vUpdateIFS(pDevice);
-    CARDvSetRSPINF(pDevice, (BYTE)pDevice->byBBType);
+    CARDvSetRSPINF(pDevice, (u8)pDevice->byBBType);
 
     if ( pDevice->byBBType == BB_TYPE_11A ) {
         //request by Jack 2005-04-26
@@ -1016,44 +994,3 @@ void CARDvSetBSSMode(struct vnt_private *pDevice)
         pDevice->abyBBVGA[3] = 0x0;
     }
 }
-
-/*
- *
- * Description:
- *    Do Channel Switch defined in 802.11h
- *
- * Parameters:
- *  In:
- *      hDeviceContext - device structure point
- *  Out:
- *      none
- *
- * Return Value: none.
- *
--*/
-int CARDbChannelSwitch(struct vnt_private *pDevice, u8 byMode,
-	u8 byNewChannel, u8 byCount)
-{
-	int bResult = true;
-
-	if (byCount == 0) {
-		pDevice->vnt_mgmt.uCurrChannel = byNewChannel;
-		CARDbSetMediaChannel(pDevice, byNewChannel);
-		return bResult;
-	}
-    pDevice->byChannelSwitchCount = byCount;
-    pDevice->byNewChannel = byNewChannel;
-    pDevice->bChannelSwitch = true;
-
-    if (byMode == 1) {
-        //bResult=CARDbStopTxPacket(pDevice, PKT_TYPE_802_11_ALL);
-        pDevice->bStopDataPkt = true;
-    }
-	return bResult;
-}
-
-
-
-
-
-
diff --git a/drivers/staging/vt6656/card.h b/drivers/staging/vt6656/card.h
index 5123bc7d0dc..c3017a74fa0 100644
--- a/drivers/staging/vt6656/card.h
+++ b/drivers/staging/vt6656/card.h
@@ -29,11 +29,6 @@
 #ifndef __CARD_H__
 #define __CARD_H__
 #include "device.h"
-#include "ttype.h"
-
-/*---------------------  Export Definitions -------------------------*/
-
-/*---------------------  Export Classes  ----------------------------*/
 
 /* init card type */
 
@@ -55,9 +50,6 @@ typedef enum _CARD_OP_MODE {
 #define CB_MAX_CHANNEL_5G       42 /* add channel9(5045MHz), 41==>42 */
 #define CB_MAX_CHANNEL      (CB_MAX_CHANNEL_24G+CB_MAX_CHANNEL_5G)
 
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
 struct vnt_private;
 
 void CARDbSetMediaChannel(struct vnt_private *pDevice, u32 uConnectionChannel);
@@ -70,16 +62,14 @@ void CARDvAdjustTSF(struct vnt_private *pDevice, u8 byRxRate,
 		u64 qwBSSTimestamp, u64 qwLocalTSF);
 bool CARDbGetCurrentTSF(struct vnt_private *pDevice, u64 *pqwCurrTSF);
 bool CARDbClearCurrentTSF(struct vnt_private *pDevice);
-void CARDvSetFirstNextTBTT(struct vnt_private *pDevice, WORD wBeaconInterval);
+void CARDvSetFirstNextTBTT(struct vnt_private *pDevice, u16 wBeaconInterval);
 void CARDvUpdateNextTBTT(struct vnt_private *pDevice, u64 qwTSF,
-			 WORD wBeaconInterval);
-u64 CARDqGetNextTBTT(u64 qwTSF, WORD wBeaconInterval);
-u64 CARDqGetTSFOffset(BYTE byRxRate, u64 qwTSF1, u64 qwTSF2);
+			 u16 wBeaconInterval);
+u64 CARDqGetNextTBTT(u64 qwTSF, u16 wBeaconInterval);
+u64 CARDqGetTSFOffset(u8 byRxRate, u64 qwTSF1, u64 qwTSF2);
 int CARDbRadioPowerOff(struct vnt_private *pDevice);
 int CARDbRadioPowerOn(struct vnt_private *pDevice);
 u8 CARDbyGetPktType(struct vnt_private *pDevice);
 void CARDvSetBSSMode(struct vnt_private *pDevice);
-int CARDbChannelSwitch(struct vnt_private *pDevice, u8 byMode,
-	u8 byNewChannel, u8 byCount);
 
 #endif /* __CARD_H__ */
diff --git a/drivers/staging/vt6656/channel.c b/drivers/staging/vt6656/channel.c
index 4181e3e12ea..5158ff4b346 100644
--- a/drivers/staging/vt6656/channel.c
+++ b/drivers/staging/vt6656/channel.c
@@ -39,15 +39,9 @@
 #include "channel.h"
 #include "rf.h"
 
-/*---------------------  Static Definitions -------------------------*/
 static int          msglevel                = MSG_LEVEL_INFO;
 //static int          msglevel                =MSG_LEVEL_DEBUG;
 
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Export Definitions -------------------------*/
-
-
 static SChannelTblElement sChannelTbl[CB_MAX_CHANNEL+1] =
 {
   {0,   0,    false},
@@ -109,17 +103,15 @@ static SChannelTblElement sChannelTbl[CB_MAX_CHANNEL+1] =
   {165, 5825, true}  //56
 };
 
-
-
 /************************************************************************
  * The Radar regulation rules for each country
  ************************************************************************/
 static  struct
 {
-    BYTE    byChannelCountryCode;             /* The country code         */
+    u8    byChannelCountryCode;             /* The country code         */
     char    chCountryCode[2];
-    BYTE    bChannelIdxList[CB_MAX_CHANNEL];  /* Available channels Index */
-    BYTE    byPower[CB_MAX_CHANNEL];
+    u8    bChannelIdxList[CB_MAX_CHANNEL];  /* Available channels Index */
+    u8    byPower[CB_MAX_CHANNEL];
 }   ChannelRuleTab[] =
 {
 /************************************************************************
@@ -368,7 +360,6 @@ static  struct
 /*                                           1   2   3   4   5   6   7   8   9  10  11  12  13  14  15  16  17  18  19  20  21  22  23  24  25  26  27  28  29  30  31  32  33  34  35  36  37  38  39  40  41  42  43  44  45  46  47  48  49  50  51  52  53  54  55  56  */
 };
 
-/*---------------------  Export function  -------------------------*/
 /************************************************************************
  * Country Channel Valid
  *  Input:  CountryCode, ChannelNum
@@ -410,32 +401,6 @@ exit:
 
 } /* end ChannelValid */
 
-/************************************************************************
- * CHvChannelGetList
- * Get Available Channel List for a given country
- * Input:
- *      CountryCode     =   The country code defined in country.h
- * Output:
- *      ChannelBitMask  =   (QWORD *) correspondent bit mask
- *                          of available channels
- *                          0x0000000000000001 means channel 1 is supported
- *                          0x0000000000000003 means channel 1,2 are supported
- *                          0x000000000000000F means channel 1,2,..15 are supported
- ************************************************************************/
-bool
-CHvChannelGetList (
-      unsigned int       uCountryCodeIdx,
-     PBYTE      pbyChannelTable
-    )
-{
-    if (uCountryCodeIdx >= CCODE_MAX) {
-        return (false);
-    }
-    memcpy(pbyChannelTable, ChannelRuleTab[uCountryCodeIdx].bChannelIdxList, CB_MAX_CHANNEL);
-    return (true);
-}
-
-
 void CHvInitChannelTable(struct vnt_private *pDevice)
 {
 	int bMultiBand = false;
@@ -507,15 +472,3 @@ void CHvInitChannelTable(struct vnt_private *pDevice)
         }*/
     }
 }
-
-BYTE CHbyGetChannelMapping(BYTE byChannelNumber)
-{
-BYTE    ii;
-BYTE    byCHMapping = 0;
-
-	for (ii = 1; ii <= CB_MAX_CHANNEL; ii++) {
-		if (sChannelTbl[ii].byChannelNumber == byChannelNumber)
-			byCHMapping = ii;
-    }
-    return byCHMapping;
-}
diff --git a/drivers/staging/vt6656/channel.h b/drivers/staging/vt6656/channel.h
index 9914dba0ba0..95701e0cbee 100644
--- a/drivers/staging/vt6656/channel.h
+++ b/drivers/staging/vt6656/channel.h
@@ -31,26 +31,14 @@
 #define _CHANNEL_H_
 
 #include "device.h"
-#include "ttype.h"
-
-/*---------------------  Export Definitions -------------------------*/
-
-/*---------------------  Export Classes  ----------------------------*/
 
 typedef struct tagSChannelTblElement {
-    BYTE    byChannelNumber;
+    u8    byChannelNumber;
     unsigned int    uFrequency;
     bool    bValid;
 } SChannelTblElement, *PSChannelTblElement;
 
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
 bool    ChannelValid(unsigned int CountryCode, unsigned int ChannelNum);
 void    CHvInitChannelTable(struct vnt_private *pDevice);
-BYTE    CHbyGetChannelMapping(BYTE byChannelNumber);
-
-bool CHvChannelGetList(unsigned int uCountryCodeIdx, PBYTE pbyChannelTable);
 
 #endif  /* _CHANNEL_H_ */
diff --git a/drivers/staging/vt6656/control.c b/drivers/staging/vt6656/control.c
index 743ef5fb7fe..026784fcde8 100644
--- a/drivers/staging/vt6656/control.c
+++ b/drivers/staging/vt6656/control.c
@@ -43,18 +43,8 @@
 #include "control.h"
 #include "rndis.h"
 
-/*---------------------  Static Definitions -------------------------*/
 /* static int          msglevel                =MSG_LEVEL_INFO;  */
 /* static int          msglevel                =MSG_LEVEL_DEBUG; */
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
 
 void ControlvWriteByte(struct vnt_private *pDevice, u8 reg, u8 reg_off,
 			u8 data)
diff --git a/drivers/staging/vt6656/control.h b/drivers/staging/vt6656/control.h
index 76ce0244e10..9da9b96add7 100644
--- a/drivers/staging/vt6656/control.h
+++ b/drivers/staging/vt6656/control.h
@@ -30,12 +30,9 @@
 #ifndef __CONTROL_H__
 #define __CONTROL_H__
 
-#include "ttype.h"
 #include "device.h"
 #include "usbpipe.h"
 
-/*---------------------  Export Definitions -------------------------*/
-
 #define CONTROLnsRequestOut(Device, Request, Value, Index, Length, Buffer) \
 	PIPEnsControlOut(Device, Request, Value, Index, Length, Buffer)
 
@@ -45,12 +42,6 @@
 #define CONTROLnsRequestIn(Device, Request, Value, Index, Length, Buffer) \
 	PIPEnsControlIn(Device, Request, Value, Index, Length, Buffer)
 
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
 void ControlvWriteByte(struct vnt_private *pDevice, u8 reg, u8 reg_off,
 			u8 data);
 
@@ -60,5 +51,4 @@ void ControlvReadByte(struct vnt_private *pDevice, u8 reg, u8 reg_off,
 void ControlvMaskByte(struct vnt_private *pDevice, u8 reg_type, u8 reg_off,
 			u8 reg_mask, u8 data);
 
-
 #endif /* __CONTROL_H__ */
diff --git a/drivers/staging/vt6656/country.h b/drivers/staging/vt6656/country.h
index 7bdc8d47aed..a0320d8f136 100644
--- a/drivers/staging/vt6656/country.h
+++ b/drivers/staging/vt6656/country.h
@@ -156,7 +156,6 @@ typedef enum _COUNTRY_CODE {
     CCODE_MAX
 } COUNTRY_CODE;
 
-
 /************************************************************************
  * Function prototype
  ************************************************************************/
diff --git a/drivers/staging/vt6656/datarate.c b/drivers/staging/vt6656/datarate.c
index 77464e819f6..17fbc35ebcb 100644
--- a/drivers/staging/vt6656/datarate.c
+++ b/drivers/staging/vt6656/datarate.c
@@ -33,7 +33,6 @@
  *
  */
 
-#include "ttype.h"
 #include "tmacro.h"
 #include "mac.h"
 #include "80211mgr.h"
@@ -44,33 +43,20 @@
 #include "srom.h"
 #include "rf.h"
 
-/*---------------------  Static Definitions -------------------------*/
-
-
-
-
-/*---------------------  Static Classes  ----------------------------*/
-
-
-
-/*---------------------  Static Variables  --------------------------*/
-
 /* static int msglevel = MSG_LEVEL_DEBUG; */
 static int          msglevel                =MSG_LEVEL_INFO;
-const BYTE acbyIERate[MAX_RATE] =
+const u8 acbyIERate[MAX_RATE] =
 {0x02, 0x04, 0x0B, 0x16, 0x0C, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C};
 
 #define AUTORATE_TXOK_CNT       0x0400
 #define AUTORATE_TXFAIL_CNT     0x0064
 #define AUTORATE_TIMEOUT        10
 
-/*---------------------  Static Functions  --------------------------*/
-
 void s_vResetCounter(PKnownNodeDB psNodeDBTable);
 
 void s_vResetCounter(PKnownNodeDB psNodeDBTable)
 {
-    BYTE            ii;
+    u8            ii;
 
     /* clear statistics counter for auto_rate */
     for (ii = 0; ii <= MAX_RATE; ii++) {
@@ -79,45 +65,6 @@ void s_vResetCounter(PKnownNodeDB psNodeDBTable)
     }
 }
 
-/*---------------------  Export Variables  --------------------------*/
-
-
-/*---------------------  Export Functions  --------------------------*/
-
-
-/*+
- *
- * Description:
- *      Get RateIdx from the value in SuppRates IE or ExtSuppRates IE
- *
- * Parameters:
- *  In:
- *      BYTE    - Rate value in SuppRates IE or ExtSuppRates IE
- *  Out:
- *      none
- *
- * Return Value: RateIdx
- *
--*/
-BYTE
-DATARATEbyGetRateIdx (
-     BYTE byRate
-    )
-{
-    BYTE    ii;
-
-    /* erase BasicRate flag */
-    byRate = byRate & 0x7F;
-
-    for (ii = 0; ii < MAX_RATE; ii ++) {
-        if (acbyIERate[ii] == byRate)
-            return ii;
-    }
-    return 0;
-}
-
-
-
 /*+
  *
  * Routine Description:
@@ -136,9 +83,6 @@ DATARATEbyGetRateIdx (
 #define AUTORATE_TXCNT_THRESHOLD        20
 #define AUTORATE_INC_THRESHOLD          30
 
-
-
-
 /*+
  *
  * Description:
@@ -146,19 +90,19 @@ DATARATEbyGetRateIdx (
  *
  * Parameters:
  *  In:
- *      BYTE    - Rate value in SuppRates IE or ExtSuppRates IE
+ *      u8    - Rate value in SuppRates IE or ExtSuppRates IE
  *  Out:
  *      none
  *
  * Return Value: RateIdx
  *
 -*/
-WORD
+u16
 RATEwGetRateIdx(
-     BYTE byRate
+     u8 byRate
     )
 {
-    WORD    ii;
+    u16    ii;
 
     /* erase BasicRate flag */
     byRate = byRate & 0x7F;
@@ -216,7 +160,7 @@ void RATEvParseMaxRate(struct vnt_private *pDevice,
     }
 
     for (ii = 0; ii < uRateLen; ii++) {
-    	byRate = (BYTE)(pItemRates->abyRates[ii]);
+    	byRate = (u8)(pItemRates->abyRates[ii]);
         if (WLAN_MGMT_IS_BASICRATE(byRate) &&
             (bUpdateBasicRate == true))  {
 	  /*
@@ -226,7 +170,7 @@ void RATEvParseMaxRate(struct vnt_private *pDevice,
 		CARDbAddBasicRate((void *)pDevice, RATEwGetRateIdx(byRate));
             DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ParseMaxRate AddBasicRate: %d\n", RATEwGetRateIdx(byRate));
         }
-        byRate = (BYTE)(pItemRates->abyRates[ii]&0x7F);
+        byRate = (u8)(pItemRates->abyRates[ii]&0x7F);
         if (byHighSuppRate == 0)
             byHighSuppRate = byRate;
         if (byRate > byHighSuppRate)
@@ -242,7 +186,7 @@ void RATEvParseMaxRate(struct vnt_private *pDevice,
             uExtRateLen = WLAN_RATES_MAXLEN;
 
         for (ii = 0; ii < uExtRateLen ; ii++) {
-            byRate = (BYTE)(pItemExtRates->abyRates[ii]);
+            byRate = (u8)(pItemExtRates->abyRates[ii]);
 	    /* select highest basic rate */
             if (WLAN_MGMT_IS_BASICRATE(pItemExtRates->abyRates[ii])) {
 	      /*
@@ -252,7 +196,7 @@ void RATEvParseMaxRate(struct vnt_private *pDevice,
 		    CARDbAddBasicRate((void *)pDevice, RATEwGetRateIdx(byRate));
                 DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ParseMaxRate AddBasicRate: %d\n", RATEwGetRateIdx(byRate));
             }
-            byRate = (BYTE)(pItemExtRates->abyRates[ii]&0x7F);
+            byRate = (u8)(pItemExtRates->abyRates[ii]&0x7F);
             if (byHighSuppRate == 0)
                 byHighSuppRate = byRate;
             if (byRate > byHighSuppRate)
@@ -282,7 +226,6 @@ void RATEvParseMaxRate(struct vnt_private *pDevice,
      DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Exit ParseMaxRate\n");
 }
 
-
 /*+
  *
  * Routine Description:
@@ -336,7 +279,7 @@ void RATEvTxRateFallBack(struct vnt_private *pDevice,
     for (ii = 0; ii < MAX_RATE; ii++) {
         if (psNodeDBTable->wSuppRate & (0x0001<<ii)) {
             if (bAutoRate[ii] == true) {
-                wIdxUpRate = (WORD) ii;
+                wIdxUpRate = (u16) ii;
             }
         } else {
             bAutoRate[ii] = false;
@@ -363,7 +306,7 @@ void RATEvTxRateFallBack(struct vnt_private *pDevice,
         if ( (dwThroughputTbl[ii] > dwThroughput) &&
              (bAutoRate[ii]==true) ) {
             dwThroughput = dwThroughputTbl[ii];
-            wIdxDownRate = (WORD) ii;
+            wIdxDownRate = (u16) ii;
         }
     }
     psNodeDBTable->wTxDataRate = wIdxDownRate;
@@ -400,7 +343,7 @@ void RATEvTxRateFallBack(struct vnt_private *pDevice,
  * Return Value: None
  *
 -*/
-BYTE
+u8
 RATEuSetIE (
      PWLAN_IE_SUPP_RATES pSrcRates,
      PWLAN_IE_SUPP_RATES pDstRates,
@@ -423,6 +366,6 @@ RATEuSetIE (
             }
         }
     }
-    return (BYTE)uRateCnt;
+    return (u8)uRateCnt;
 }
 
diff --git a/drivers/staging/vt6656/datarate.h b/drivers/staging/vt6656/datarate.h
index 8dc55bd6166..43cb77894b6 100644
--- a/drivers/staging/vt6656/datarate.h
+++ b/drivers/staging/vt6656/datarate.h
@@ -29,9 +29,6 @@
 #ifndef __DATARATE_H__
 #define __DATARATE_H__
 
-
-/*---------------------  Export Definitions -------------------------*/
-
 #define FALLBACK_PKT_COLLECT_TR_H  50   /* pkts */
 #define FALLBACK_PKT_COLLECT_TR_L  10   /* pkts */
 #define FALLBACK_POLL_SECOND       5    /* 5 sec */
@@ -57,18 +54,6 @@
 #define RATE_AUTO      12
 #define MAX_RATE       12
 
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-
-/*---------------------  Export Types  ------------------------------*/
-
-
-/*---------------------  Export Functions  --------------------------*/
-
-
-
 void RATEvParseMaxRate(struct vnt_private *, PWLAN_IE_SUPP_RATES pItemRates,
 	PWLAN_IE_SUPP_RATES pItemExtRates, int bUpdateBasicRate,
 	u16 *pwMaxBasicRate, u16 *pwMaxSuppRate, u16 *pwSuppRate,
@@ -77,22 +62,16 @@ void RATEvParseMaxRate(struct vnt_private *, PWLAN_IE_SUPP_RATES pItemRates,
 void RATEvTxRateFallBack(struct vnt_private *pDevice,
 	PKnownNodeDB psNodeDBTable);
 
-BYTE
+u8
 RATEuSetIE(
      PWLAN_IE_SUPP_RATES pSrcRates,
      PWLAN_IE_SUPP_RATES pDstRates,
      unsigned int                uRateLen
     );
 
-WORD
+u16
 RATEwGetRateIdx(
-     BYTE byRate
-    );
-
-
-BYTE
-DATARATEbyGetRateIdx(
-     BYTE byRate
+     u8 byRate
     );
 
 #endif /* __DATARATE_H__ */
diff --git a/drivers/staging/vt6656/desc.h b/drivers/staging/vt6656/desc.h
index 0c0b614aaa1..64cb046fe98 100644
--- a/drivers/staging/vt6656/desc.h
+++ b/drivers/staging/vt6656/desc.h
@@ -33,7 +33,7 @@
 
 #include <linux/types.h>
 #include <linux/mm.h>
-#include "ttype.h"
+
 #include "tether.h"
 
 /* max transmit or receive buffer size */
@@ -147,38 +147,38 @@
  * RsvTime buffer header
  */
 typedef struct tagSRrvTime_gRTS {
-    WORD        wRTSTxRrvTime_ba;
-    WORD        wRTSTxRrvTime_aa;
-    WORD        wRTSTxRrvTime_bb;
-    WORD        wReserved;
-    WORD        wTxRrvTime_b;
-    WORD        wTxRrvTime_a;
+    u16        wRTSTxRrvTime_ba;
+    u16        wRTSTxRrvTime_aa;
+    u16        wRTSTxRrvTime_bb;
+    u16        wReserved;
+    u16        wTxRrvTime_b;
+    u16        wTxRrvTime_a;
 } __attribute__ ((__packed__))
 SRrvTime_gRTS, *PSRrvTime_gRTS;
 
 typedef const SRrvTime_gRTS *PCSRrvTime_gRTS;
 
 typedef struct tagSRrvTime_gCTS {
-    WORD        wCTSTxRrvTime_ba;
-    WORD        wReserved;
-    WORD        wTxRrvTime_b;
-    WORD        wTxRrvTime_a;
+    u16        wCTSTxRrvTime_ba;
+    u16        wReserved;
+    u16        wTxRrvTime_b;
+    u16        wTxRrvTime_a;
 } __attribute__ ((__packed__))
 SRrvTime_gCTS, *PSRrvTime_gCTS;
 
 typedef const SRrvTime_gCTS *PCSRrvTime_gCTS;
 
 typedef struct tagSRrvTime_ab {
-    WORD        wRTSTxRrvTime;
-    WORD        wTxRrvTime;
+    u16        wRTSTxRrvTime;
+    u16        wTxRrvTime;
 } __attribute__ ((__packed__))
 SRrvTime_ab, *PSRrvTime_ab;
 
 typedef const SRrvTime_ab *PCSRrvTime_ab;
 
 typedef struct tagSRrvTime_atim {
-    WORD        wCTSTxRrvTime_ba;
-    WORD        wTxRrvTime_a;
+    u16        wCTSTxRrvTime_ba;
+    u16        wTxRrvTime_a;
 } __attribute__ ((__packed__))
 SRrvTime_atim, *PSRrvTime_atim;
 
@@ -188,46 +188,46 @@ typedef const SRrvTime_atim *PCSRrvTime_atim;
  * RTS buffer header
  */
 typedef struct tagSRTSData {
-    WORD    wFrameControl;
-    WORD    wDurationID;
-    BYTE    abyRA[ETH_ALEN];
-    BYTE    abyTA[ETH_ALEN];
+    u16    wFrameControl;
+    u16    wDurationID;
+    u8    abyRA[ETH_ALEN];
+    u8    abyTA[ETH_ALEN];
 } __attribute__ ((__packed__))
 SRTSData, *PSRTSData;
 
 typedef const SRTSData *PCSRTSData;
 
 typedef struct tagSRTS_g {
-    BYTE        bySignalField_b;
-    BYTE        byServiceField_b;
-    WORD        wTransmitLength_b;
-    BYTE        bySignalField_a;
-    BYTE        byServiceField_a;
-    WORD        wTransmitLength_a;
-    WORD        wDuration_ba;
-    WORD        wDuration_aa;
-    WORD        wDuration_bb;
-    WORD        wReserved;
+    u8        bySignalField_b;
+    u8        byServiceField_b;
+    u16        wTransmitLength_b;
+    u8        bySignalField_a;
+    u8        byServiceField_a;
+    u16        wTransmitLength_a;
+    u16        wDuration_ba;
+    u16        wDuration_aa;
+    u16        wDuration_bb;
+    u16        wReserved;
     SRTSData    Data;
 } __attribute__ ((__packed__))
 SRTS_g, *PSRTS_g;
 typedef const SRTS_g *PCSRTS_g;
 
 typedef struct tagSRTS_g_FB {
-    BYTE        bySignalField_b;
-    BYTE        byServiceField_b;
-    WORD        wTransmitLength_b;
-    BYTE        bySignalField_a;
-    BYTE        byServiceField_a;
-    WORD        wTransmitLength_a;
-    WORD        wDuration_ba;
-    WORD        wDuration_aa;
-    WORD        wDuration_bb;
-    WORD        wReserved;
-    WORD        wRTSDuration_ba_f0;
-    WORD        wRTSDuration_aa_f0;
-    WORD        wRTSDuration_ba_f1;
-    WORD        wRTSDuration_aa_f1;
+    u8        bySignalField_b;
+    u8        byServiceField_b;
+    u16        wTransmitLength_b;
+    u8        bySignalField_a;
+    u8        byServiceField_a;
+    u16        wTransmitLength_a;
+    u16        wDuration_ba;
+    u16        wDuration_aa;
+    u16        wDuration_bb;
+    u16        wReserved;
+    u16        wRTSDuration_ba_f0;
+    u16        wRTSDuration_aa_f0;
+    u16        wRTSDuration_ba_f1;
+    u16        wRTSDuration_aa_f1;
     SRTSData    Data;
 } __attribute__ ((__packed__))
 SRTS_g_FB, *PSRTS_g_FB;
@@ -235,11 +235,11 @@ SRTS_g_FB, *PSRTS_g_FB;
 typedef const SRTS_g_FB *PCSRTS_g_FB;
 
 typedef struct tagSRTS_ab {
-    BYTE        bySignalField;
-    BYTE        byServiceField;
-    WORD        wTransmitLength;
-    WORD        wDuration;
-    WORD        wReserved;
+    u8        bySignalField;
+    u8        byServiceField;
+    u16        wTransmitLength;
+    u16        wDuration;
+    u16        wReserved;
     SRTSData    Data;
 } __attribute__ ((__packed__))
 SRTS_ab, *PSRTS_ab;
@@ -247,13 +247,13 @@ SRTS_ab, *PSRTS_ab;
 typedef const SRTS_ab *PCSRTS_ab;
 
 typedef struct tagSRTS_a_FB {
-    BYTE        bySignalField;
-    BYTE        byServiceField;
-    WORD        wTransmitLength;
-    WORD        wDuration;
-    WORD        wReserved;
-    WORD        wRTSDuration_f0;
-    WORD        wRTSDuration_f1;
+    u8        bySignalField;
+    u8        byServiceField;
+    u16        wTransmitLength;
+    u16        wDuration;
+    u16        wReserved;
+    u16        wRTSDuration_f0;
+    u16        wRTSDuration_f1;
     SRTSData    Data;
 } __attribute__ ((__packed__))
 SRTS_a_FB, *PSRTS_a_FB;
@@ -264,19 +264,19 @@ typedef const SRTS_a_FB *PCSRTS_a_FB;
  * CTS buffer header
  */
 typedef struct tagSCTSData {
-    WORD    wFrameControl;
-    WORD    wDurationID;
-    BYTE    abyRA[ETH_ALEN];
-    WORD    wReserved;
+    u16    wFrameControl;
+    u16    wDurationID;
+    u8    abyRA[ETH_ALEN];
+    u16    wReserved;
 } __attribute__ ((__packed__))
 SCTSData, *PSCTSData;
 
 typedef struct tagSCTS {
-    BYTE        bySignalField_b;
-    BYTE        byServiceField_b;
-    WORD        wTransmitLength_b;
-    WORD        wDuration_ba;
-    WORD        wReserved;
+    u8        bySignalField_b;
+    u8        byServiceField_b;
+    u16        wTransmitLength_b;
+    u16        wDuration_ba;
+    u16        wReserved;
     SCTSData    Data;
 } __attribute__ ((__packed__))
 SCTS, *PSCTS;
@@ -284,13 +284,13 @@ SCTS, *PSCTS;
 typedef const SCTS *PCSCTS;
 
 typedef struct tagSCTS_FB {
-    BYTE        bySignalField_b;
-    BYTE        byServiceField_b;
-    WORD        wTransmitLength_b;
-    WORD        wDuration_ba;
-    WORD        wReserved;
-    WORD        wCTSDuration_ba_f0;
-    WORD        wCTSDuration_ba_f1;
+    u8        bySignalField_b;
+    u8        byServiceField_b;
+    u16        wTransmitLength_b;
+    u16        wDuration_ba;
+    u16        wReserved;
+    u16        wCTSDuration_ba_f0;
+    u16        wCTSDuration_ba_f1;
     SCTSData    Data;
 } __attribute__ ((__packed__))
 SCTS_FB, *PSCTS_FB;
@@ -302,17 +302,17 @@ typedef const SCTS_FB *PCSCTS_FB;
  */
 typedef struct tagSTxBufHead {
 	u32 adwTxKey[4];
-    WORD    wFIFOCtl;
-    WORD    wTimeStamp;
-    WORD    wFragCtl;
-    WORD    wReserved;
+    u16    wFIFOCtl;
+    u16    wTimeStamp;
+    u16    wFragCtl;
+    u16    wReserved;
 } __attribute__ ((__packed__))
 STxBufHead, *PSTxBufHead;
 typedef const STxBufHead *PCSTxBufHead;
 
 typedef struct tagSTxShortBufHead {
-    WORD    wFIFOCtl;
-    WORD    wTimeStamp;
+    u16    wFIFOCtl;
+    u16    wTimeStamp;
 } __attribute__ ((__packed__))
 STxShortBufHead, *PSTxShortBufHead;
 typedef const STxShortBufHead *PCSTxShortBufHead;
@@ -321,56 +321,56 @@ typedef const STxShortBufHead *PCSTxShortBufHead;
  * TX data header
  */
 typedef struct tagSTxDataHead_g {
-    BYTE    bySignalField_b;
-    BYTE    byServiceField_b;
-    WORD    wTransmitLength_b;
-    BYTE    bySignalField_a;
-    BYTE    byServiceField_a;
-    WORD    wTransmitLength_a;
-    WORD    wDuration_b;
-    WORD    wDuration_a;
-    WORD    wTimeStampOff_b;
-    WORD    wTimeStampOff_a;
+    u8    bySignalField_b;
+    u8    byServiceField_b;
+    u16    wTransmitLength_b;
+    u8    bySignalField_a;
+    u8    byServiceField_a;
+    u16    wTransmitLength_a;
+    u16    wDuration_b;
+    u16    wDuration_a;
+    u16    wTimeStampOff_b;
+    u16    wTimeStampOff_a;
 } __attribute__ ((__packed__))
 STxDataHead_g, *PSTxDataHead_g;
 
 typedef const STxDataHead_g *PCSTxDataHead_g;
 
 typedef struct tagSTxDataHead_g_FB {
-    BYTE    bySignalField_b;
-    BYTE    byServiceField_b;
-    WORD    wTransmitLength_b;
-    BYTE    bySignalField_a;
-    BYTE    byServiceField_a;
-    WORD    wTransmitLength_a;
-    WORD    wDuration_b;
-    WORD    wDuration_a;
-    WORD    wDuration_a_f0;
-    WORD    wDuration_a_f1;
-    WORD    wTimeStampOff_b;
-    WORD    wTimeStampOff_a;
+    u8    bySignalField_b;
+    u8    byServiceField_b;
+    u16    wTransmitLength_b;
+    u8    bySignalField_a;
+    u8    byServiceField_a;
+    u16    wTransmitLength_a;
+    u16    wDuration_b;
+    u16    wDuration_a;
+    u16    wDuration_a_f0;
+    u16    wDuration_a_f1;
+    u16    wTimeStampOff_b;
+    u16    wTimeStampOff_a;
 } __attribute__ ((__packed__))
 STxDataHead_g_FB, *PSTxDataHead_g_FB;
 typedef const STxDataHead_g_FB *PCSTxDataHead_g_FB;
 
 typedef struct tagSTxDataHead_ab {
-    BYTE    bySignalField;
-    BYTE    byServiceField;
-    WORD    wTransmitLength;
-    WORD    wDuration;
-    WORD    wTimeStampOff;
+    u8    bySignalField;
+    u8    byServiceField;
+    u16    wTransmitLength;
+    u16    wDuration;
+    u16    wTimeStampOff;
 } __attribute__ ((__packed__))
 STxDataHead_ab, *PSTxDataHead_ab;
 typedef const STxDataHead_ab *PCSTxDataHead_ab;
 
 typedef struct tagSTxDataHead_a_FB {
-    BYTE    bySignalField;
-    BYTE    byServiceField;
-    WORD    wTransmitLength;
-    WORD    wDuration;
-    WORD    wTimeStampOff;
-    WORD    wDuration_f0;
-    WORD    wDuration_f1;
+    u8    bySignalField;
+    u8    byServiceField;
+    u16    wTransmitLength;
+    u16    wDuration;
+    u16    wTimeStampOff;
+    u16    wDuration_f0;
+    u16    wDuration_f1;
 } __attribute__ ((__packed__))
 STxDataHead_a_FB, *PSTxDataHead_a_FB;
 typedef const STxDataHead_a_FB *PCSTxDataHead_a_FB;
@@ -397,14 +397,14 @@ SBEACONCtl;
 
 typedef struct tagSSecretKey {
 	u32 dwLowDword;
-    BYTE    byHighByte;
+    u8    byHighByte;
 } __attribute__ ((__packed__))
 SSecretKey;
 
 typedef struct tagSKeyEntry {
-    BYTE  abyAddrHi[2];
-    WORD  wKCTL;
-    BYTE  abyAddrLo[4];
+    u8  abyAddrHi[2];
+    u16  wKCTL;
+    u8  abyAddrLo[4];
 	u32 dwKey0[4];
 	u32 dwKey1[4];
 	u32 dwKey2[4];
diff --git a/drivers/staging/vt6656/device.h b/drivers/staging/vt6656/device.h
index 6bba2e06fa6..f07ba242811 100644
--- a/drivers/staging/vt6656/device.h
+++ b/drivers/staging/vt6656/device.h
@@ -45,7 +45,6 @@
 #include <linux/timer.h>
 #include <linux/usb.h>
 
-
 #ifdef SIOCETHTOOL
 #define DEVICE_ETHTOOL_IOCTL_SUPPORT
 #include <linux/ethtool.h>
@@ -66,7 +65,6 @@
  */
 
 #include "device_cfg.h"
-#include "ttype.h"
 #include "80211hdr.h"
 #include "tether.h"
 #include "wmgr.h"
@@ -78,7 +76,6 @@
 #include "key.h"
 #include "card.h"
 
-/*---------------------  Export Definitions -------------------------*/
 #define VNT_USB_VENDOR_ID                     0x160a
 #define VNT_USB_PRODUCT_ID                    0x3184
 
@@ -122,7 +119,6 @@
 #define ANT_RXA                 2
 #define ANT_RXB                 3
 
-
 #define MAXCHECKHANGCNT         4
 
 /* Packet type */
@@ -142,8 +138,6 @@
 
 #define PRIVATE_Message                 0
 
-/*---------------------  Export Types  ------------------------------*/
-
 #define DBG_PRT(l, p, args...) { if (l <= msglevel) printk(p, ##args); }
 #define PRINT_K(p, args...) { if (PRIVATE_Message) printk(p, ##args); }
 
@@ -191,7 +185,7 @@ typedef struct _USB_SEND_CONTEXT {
     struct urb      *pUrb;
     unsigned int            uBufLen;
     CONTEXT_TYPE    Type;
-    SEthernetHeader sEthHeader;
+    struct ethhdr sEthHeader;
     void *Next;
     bool            bBoolInUse;
     unsigned char           Data[MAX_TOTAL_SIZE_WITH_ALL_HEADERS];
@@ -212,7 +206,7 @@ typedef struct _DEFAULT_CONFIG {
  */
 typedef struct {
     unsigned int            uDataLen;
-    PBYTE           pDataBuf;
+    u8 *           pDataBuf;
   /* struct urb *pUrb; */
     bool            bInUse;
 } INT_BUFFER, *PINT_BUFFER;
@@ -251,7 +245,6 @@ typedef enum __DEVICE_NDIS_STATUS {
 /* PMKID Structures */
 typedef unsigned char   NDIS_802_11_PMKID_VALUE[16];
 
-
 typedef enum _NDIS_802_11_WEP_STATUS
 {
     Ndis802_11WEPEnabled,
@@ -269,7 +262,6 @@ typedef enum _NDIS_802_11_WEP_STATUS
 } NDIS_802_11_WEP_STATUS, *PNDIS_802_11_WEP_STATUS,
   NDIS_802_11_ENCRYPTION_STATUS, *PNDIS_802_11_ENCRYPTION_STATUS;
 
-
 typedef enum _NDIS_802_11_STATUS_TYPE
 {
 	Ndis802_11StatusType_Authentication,
@@ -284,7 +276,6 @@ typedef struct _PMKID_CANDIDATE {
     unsigned long Flags;
 } PMKID_CANDIDATE, *PPMKID_CANDIDATE;
 
-
 typedef struct _BSSID_INFO
 {
     NDIS_802_11_MAC_ADDRESS BSSID;
@@ -309,16 +300,16 @@ typedef struct tagSPMKIDCandidateEvent {
 
 typedef struct tagSQuietControl {
     bool        bEnable;
-    DWORD       dwStartTime;
-    BYTE        byPeriod;
-    WORD        wDuration;
+    u32       dwStartTime;
+    u8        byPeriod;
+    u16        wDuration;
 } SQuietControl, *PSQuietControl;
 
 /* The receive duplicate detection cache entry */
 typedef struct tagSCacheEntry{
-    WORD        wFmSequence;
-    BYTE        abyAddr2[ETH_ALEN];
-    WORD        wFrameCtl;
+    u16        wFmSequence;
+    u8        abyAddr2[ETH_ALEN];
+    u16        wFrameCtl;
 } SCacheEntry, *PSCacheEntry;
 
 typedef struct tagSCache{
@@ -335,12 +326,12 @@ typedef struct tagSCache{
  */
 typedef struct tagSDeFragControlBlock
 {
-    WORD            wSequence;
-    WORD            wFragNum;
-    BYTE            abyAddr2[ETH_ALEN];
+    u16            wSequence;
+    u16            wFragNum;
+    u8            abyAddr2[ETH_ALEN];
 	unsigned int            uLifetime;
     struct sk_buff* skb;
-    PBYTE           pbyRxBuffer;
+    u8 *           pbyRxBuffer;
     unsigned int            cbFrameLength;
     bool            bInUse;
 } SDeFragControlBlock, *PSDeFragControlBlock;
@@ -371,7 +362,6 @@ typedef struct tagSDeFragControlBlock
 /* for device_set_media_duplex */
 #define     DEVICE_LINK_CHANGE           0x00000001UL
 
-
 typedef struct __device_opt {
 	int nRxDescs0;  /* number of RX descriptors 0 */
 	int nTxDescs0;  /* number of TX descriptors 0, 1, 2, 3 */
@@ -386,7 +376,6 @@ typedef struct __device_opt {
     u32         flags;
 } OPTIONS, *POPTIONS;
 
-
 struct vnt_private {
 	/* netdev */
 	struct usb_device *usb;
@@ -420,7 +409,6 @@ struct vnt_private {
 	u32 cbFreeDFCB;
 	u32 uCurrentDFCBIdx;
 
-
 	/* USB */
 	struct urb *pControlURB;
 	struct urb *pInterruptURB;
@@ -453,7 +441,6 @@ struct vnt_private {
 	/* default config from file by user setting */
 	DEFAULT_CONFIG config_file;
 
-
 	/* Statistic for USB */
 	unsigned long ulBulkInPosted;
 	unsigned long ulBulkInError;
@@ -470,14 +457,12 @@ struct vnt_private {
 	unsigned long ulIntInContCRCError;
 	unsigned long ulIntInBytesRead;
 
-
 	/* Version control */
 	u16 wFirmwareVersion;
 	u8 byLocalID;
 	u8 byRFType;
 	u8 byBBRxConf;
 
-
 	u8 byZoneType;
 	int bZoneRegExist;
 
@@ -571,7 +556,6 @@ struct vnt_private {
 	u8 byTopOFDMBasicRate;
 	u8 byTopCCKBasicRate;
 
-
 	u32 dwAotoRateTxOkCnt;
 	u32 dwAotoRateTxFailCnt;
 	u32 dwErrorRateThreshold[13];
@@ -684,7 +668,6 @@ struct vnt_private {
 	SKeyManagement sKey;
 	u32 dwIVCounter;
 
-
 	RC4Ext SBox;
 	u8 abyPRNG[WLAN_WEPMAX_KEYLEN+3];
 	u8 byKeyIndex;
@@ -701,13 +684,11 @@ struct vnt_private {
 	/* QoS */
 	int bGrpAckPolicy;
 
-
 	u8 byAutoFBCtrl;
 
 	int bTxMICFail;
 	int bRxMICFail;
 
-
 	/* For Update BaseBand VGA Gain Offset */
 	int bUpdateBBVGA;
 	u32 uBBVGADiffCount;
@@ -719,7 +700,6 @@ struct vnt_private {
 	u8 byBBPreEDRSSI;
 	u8 byBBPreEDIndex;
 
-
 	int bRadioCmd;
 	u32 dwDiagRefCount;
 
@@ -747,8 +727,8 @@ struct vnt_private {
 	u8 byReAssocCount;
 	u8 byLinkWaitCount;
 
-	SEthernetHeader sTxEthHeader;
-	SEthernetHeader sRxEthHeader;
+	struct ethhdr sTxEthHeader;
+	struct ethhdr sRxEthHeader;
 	u8 abyBroadcastAddr[ETH_ALEN];
 	u8 abySNAP_RFC1042[ETH_ALEN];
 	u8 abySNAP_Bridgetunnel[ETH_ALEN];
@@ -757,7 +737,6 @@ struct vnt_private {
 	SPMKID gsPMKID;
 	SPMKIDCandidateEvent gsPMKIDCandidate;
 
-
 	/* for 802.11h */
 	int b11hEnable;
 
@@ -788,9 +767,6 @@ struct vnt_private {
 
 };
 
-
-
-
 #define EnqueueRCB(_Head, _Tail, _RCB)                  \
 {                                                       \
     if (!_Head) {                                       \
@@ -812,7 +788,6 @@ struct vnt_private {
     Head = RCB->Next;                                   \
 }
 
-
 #define ADD_ONE_WITH_WRAP_AROUND(uVar, uModulo) {   \
     if ((uVar) >= ((uModulo) - 1))                  \
         (uVar) = 0;                                 \
@@ -820,7 +795,6 @@ struct vnt_private {
         (uVar)++;                                   \
 }
 
-
 #define fMP_RESET_IN_PROGRESS               0x00000001
 #define fMP_DISCONNECTED                    0x00000002
 #define fMP_HALT_IN_PROGRESS                0x00000004
@@ -841,8 +815,6 @@ struct vnt_private {
 #define MP_IS_READY(_M)        (((_M)->Flags & \
                                  (fMP_DISCONNECTED | fMP_RESET_IN_PROGRESS | fMP_HALT_IN_PROGRESS | fMP_INIT_IN_PROGRESS | fMP_SURPRISE_REMOVED)) == 0)
 
-/*---------------------  Export Functions  --------------------------*/
-
 int device_alloc_frag_buf(struct vnt_private *, PSDeFragControlBlock pDeF);
 
 #endif
diff --git a/drivers/staging/vt6656/device_cfg.h b/drivers/staging/vt6656/device_cfg.h
index 62290d0ac19..ea66b975fa5 100644
--- a/drivers/staging/vt6656/device_cfg.h
+++ b/drivers/staging/vt6656/device_cfg.h
@@ -29,8 +29,6 @@
 
 #include <linux/types.h>
 
-#include "ttype.h"
-
 typedef
 struct _version {
     unsigned char   major;
diff --git a/drivers/staging/vt6656/dpc.c b/drivers/staging/vt6656/dpc.c
index e83f95e1d9a..7ec166a2ac8 100644
--- a/drivers/staging/vt6656/dpc.c
+++ b/drivers/staging/vt6656/dpc.c
@@ -56,32 +56,20 @@
 #include "datarate.h"
 #include "usbpipe.h"
 
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
 //static int          msglevel                =MSG_LEVEL_DEBUG;
 static int          msglevel                =MSG_LEVEL_INFO;
 
-const BYTE acbyRxRate[MAX_RATE] =
+const u8 acbyRxRate[MAX_RATE] =
 {2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108};
 
-
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
-
-static BYTE s_byGetRateIdx(BYTE byRate);
+static u8 s_byGetRateIdx(u8 byRate);
 
 static
 void
 s_vGetDASA(
-      PBYTE pbyRxBufferAddr,
+      u8 * pbyRxBufferAddr,
      unsigned int *pcbHeaderSize,
-     PSEthernetHeader psEthHeader
+     struct ethhdr *psEthHeader
     );
 
 static void s_vProcessRxMACHeader(struct vnt_private *pDevice,
@@ -102,8 +90,6 @@ static int s_bHostWepRxEncryption(struct vnt_private *pDevice, u8 *pbyFrame,
 	u32 FrameSize, u8 *pbyRsr, int bOnFly, PSKeyItem pKey, u8 *pbyNewRsr,
 	s32 *pbExtIV, u16 *pwRxTSC15_0, u32 *pdwRxTSC47_16);
 
-/*---------------------  Export Variables  --------------------------*/
-
 /*+
  *
  * Description:
@@ -129,13 +115,12 @@ static void s_vProcessRxMACHeader(struct vnt_private *pDevice,
 	u8 *pbyRxBuffer;
 	u32 cbHeaderSize = 0;
 	u16 *pwType;
-	PS802_11Header pMACHeader;
+	struct ieee80211_hdr *pMACHeader;
 	int ii;
 
+    pMACHeader = (struct ieee80211_hdr *) (pbyRxBufferAddr + cbHeaderSize);
 
-    pMACHeader = (PS802_11Header) (pbyRxBufferAddr + cbHeaderSize);
-
-    s_vGetDASA((PBYTE)pMACHeader, &cbHeaderSize, &pDevice->sRxEthHeader);
+    s_vGetDASA((u8 *)pMACHeader, &cbHeaderSize, &pDevice->sRxEthHeader);
 
     if (bIsWEP) {
         if (bExtIV) {
@@ -150,16 +135,16 @@ static void s_vProcessRxMACHeader(struct vnt_private *pDevice,
         cbHeaderSize += WLAN_HDR_ADDR3_LEN;
     };
 
-    pbyRxBuffer = (PBYTE) (pbyRxBufferAddr + cbHeaderSize);
+    pbyRxBuffer = (u8 *) (pbyRxBufferAddr + cbHeaderSize);
     if (!compare_ether_addr(pbyRxBuffer, &pDevice->abySNAP_Bridgetunnel[0])) {
         cbHeaderSize += 6;
     } else if (!compare_ether_addr(pbyRxBuffer, &pDevice->abySNAP_RFC1042[0])) {
         cbHeaderSize += 6;
-        pwType = (PWORD) (pbyRxBufferAddr + cbHeaderSize);
+        pwType = (u16 *) (pbyRxBufferAddr + cbHeaderSize);
 	if ((*pwType == cpu_to_be16(ETH_P_IPX)) ||
 	    (*pwType == cpu_to_le16(0xF380))) {
 		cbHeaderSize -= 8;
-            pwType = (PWORD) (pbyRxBufferAddr + cbHeaderSize);
+            pwType = (u16 *) (pbyRxBufferAddr + cbHeaderSize);
             if (bIsWEP) {
                 if (bExtIV) {
                     *pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN - 8);    // 8 is IV&ExtIV
@@ -174,7 +159,7 @@ static void s_vProcessRxMACHeader(struct vnt_private *pDevice,
     }
     else {
         cbHeaderSize -= 2;
-        pwType = (PWORD) (pbyRxBufferAddr + cbHeaderSize);
+        pwType = (u16 *) (pbyRxBufferAddr + cbHeaderSize);
         if (bIsWEP) {
             if (bExtIV) {
                 *pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN - 8);    // 8 is IV&ExtIV
@@ -188,21 +173,18 @@ static void s_vProcessRxMACHeader(struct vnt_private *pDevice,
     }
 
     cbHeaderSize -= (ETH_ALEN * 2);
-    pbyRxBuffer = (PBYTE) (pbyRxBufferAddr + cbHeaderSize);
+    pbyRxBuffer = (u8 *) (pbyRxBufferAddr + cbHeaderSize);
     for (ii = 0; ii < ETH_ALEN; ii++)
-        *pbyRxBuffer++ = pDevice->sRxEthHeader.abyDstAddr[ii];
+        *pbyRxBuffer++ = pDevice->sRxEthHeader.h_dest[ii];
     for (ii = 0; ii < ETH_ALEN; ii++)
-        *pbyRxBuffer++ = pDevice->sRxEthHeader.abySrcAddr[ii];
+        *pbyRxBuffer++ = pDevice->sRxEthHeader.h_source[ii];
 
     *pcbHeadSize = cbHeaderSize;
 }
 
-
-
-
-static BYTE s_byGetRateIdx(BYTE byRate)
+static u8 s_byGetRateIdx(u8 byRate)
 {
-    BYTE    byRateIdx;
+    u8    byRateIdx;
 
     for (byRateIdx = 0; byRateIdx <MAX_RATE ; byRateIdx++) {
         if (acbyRxRate[byRateIdx%MAX_RATE] == byRate)
@@ -211,61 +193,59 @@ static BYTE s_byGetRateIdx(BYTE byRate)
     return 0;
 }
 
-
 static
 void
 s_vGetDASA (
-      PBYTE pbyRxBufferAddr,
+      u8 * pbyRxBufferAddr,
      unsigned int *pcbHeaderSize,
-     PSEthernetHeader psEthHeader
+     struct ethhdr *psEthHeader
     )
 {
 	unsigned int            cbHeaderSize = 0;
-	PS802_11Header  pMACHeader;
+	struct ieee80211_hdr *pMACHeader;
 	int             ii;
 
-	pMACHeader = (PS802_11Header) (pbyRxBufferAddr + cbHeaderSize);
+	pMACHeader = (struct ieee80211_hdr *) (pbyRxBufferAddr + cbHeaderSize);
 
-	if ((pMACHeader->wFrameCtl & FC_TODS) == 0) {
-		if (pMACHeader->wFrameCtl & FC_FROMDS) {
+	if ((pMACHeader->frame_control & FC_TODS) == 0) {
+		if (pMACHeader->frame_control & FC_FROMDS) {
 			for (ii = 0; ii < ETH_ALEN; ii++) {
-				psEthHeader->abyDstAddr[ii] =
-					pMACHeader->abyAddr1[ii];
-				psEthHeader->abySrcAddr[ii] =
-					pMACHeader->abyAddr3[ii];
+				psEthHeader->h_dest[ii] =
+					pMACHeader->addr1[ii];
+				psEthHeader->h_source[ii] =
+					pMACHeader->addr3[ii];
 			}
 		} else {
 			/* IBSS mode */
 			for (ii = 0; ii < ETH_ALEN; ii++) {
-				psEthHeader->abyDstAddr[ii] =
-					pMACHeader->abyAddr1[ii];
-				psEthHeader->abySrcAddr[ii] =
-					pMACHeader->abyAddr2[ii];
+				psEthHeader->h_dest[ii] =
+					pMACHeader->addr1[ii];
+				psEthHeader->h_source[ii] =
+					pMACHeader->addr2[ii];
 			}
 		}
 	} else {
 		/* Is AP mode.. */
-		if (pMACHeader->wFrameCtl & FC_FROMDS) {
+		if (pMACHeader->frame_control & FC_FROMDS) {
 			for (ii = 0; ii < ETH_ALEN; ii++) {
-				psEthHeader->abyDstAddr[ii] =
-					pMACHeader->abyAddr3[ii];
-				psEthHeader->abySrcAddr[ii] =
-					pMACHeader->abyAddr4[ii];
+				psEthHeader->h_dest[ii] =
+					pMACHeader->addr3[ii];
+				psEthHeader->h_source[ii] =
+					pMACHeader->addr4[ii];
 				cbHeaderSize += 6;
 			}
 		} else {
 			for (ii = 0; ii < ETH_ALEN; ii++) {
-				psEthHeader->abyDstAddr[ii] =
-					pMACHeader->abyAddr3[ii];
-				psEthHeader->abySrcAddr[ii] =
-					pMACHeader->abyAddr2[ii];
+				psEthHeader->h_dest[ii] =
+					pMACHeader->addr3[ii];
+				psEthHeader->h_source[ii] =
+					pMACHeader->addr2[ii];
 			}
 		}
 	};
     *pcbHeaderSize = cbHeaderSize;
 }
 
-
 int RXbBulkInProcessData(struct vnt_private *pDevice, PRCB pRCB,
 	unsigned long BytesToIndicate)
 {
@@ -273,7 +253,7 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, PRCB pRCB,
 	struct sk_buff *skb;
 	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
 	struct vnt_rx_mgmt *pRxPacket = &pMgmt->sRxPacket;
-	PS802_11Header p802_11Header;
+	struct ieee80211_hdr *p802_11Header;
 	u8 *pbyRsr, *pbyNewRsr, *pbyRSSI, *pbyFrame;
 	u64 *pqwTSFTime;
 	u32 bDeFragRx = false;
@@ -297,10 +277,9 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, PRCB pRCB,
 	u8 abyVaildRate[MAX_RATE]
 		= {2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108};
 	u16 wPLCPwithPadding;
-	PS802_11Header pMACHeader;
+	struct ieee80211_hdr *pMACHeader;
 	int bRxeapol_key = false;
 
-
     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---------- RXbBulkInProcessData---\n");
 
     skb = pRCB->skb;
@@ -321,14 +300,14 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, PRCB pRCB,
         return false;
     }
 
-    pbyDAddress = (PBYTE)(skb->data);
+    pbyDAddress = (u8 *)(skb->data);
     pbyRxSts = pbyDAddress+4;
     pbyRxRate = pbyDAddress+5;
 
     //real Frame Size = USBFrameSize -4WbkStatus - 4RxStatus - 8TSF - 4RSR - 4SQ3 - ?Padding
     //if SQ3 the range is 24~27, if no SQ3 the range is 20~23
     //real Frame size in PLCPLength field.
-    pwPLCP_Length = (PWORD) (pbyDAddress + 6);
+    pwPLCP_Length = (u16 *) (pbyDAddress + 6);
     //Fix hardware bug => PLCP_Length error
     if ( ((BytesToIndicate - (*pwPLCP_Length)) > 27) ||
          ((BytesToIndicate - (*pwPLCP_Length)) < 24) ||
@@ -377,45 +356,43 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, PRCB pRCB,
                             FrameSize
                             );
 
-
-    pMACHeader = (PS802_11Header) pbyFrame;
+    pMACHeader = (struct ieee80211_hdr *) pbyFrame;
 
 //mike add: to judge if current AP is activated?
     if ((pMgmt->eCurrMode == WMAC_MODE_STANDBY) ||
         (pMgmt->eCurrMode == WMAC_MODE_ESS_STA)) {
        if (pMgmt->sNodeDBTable[0].bActive) {
-	 if (!compare_ether_addr(pMgmt->abyCurrBSSID, pMACHeader->abyAddr2)) {
+	 if (!compare_ether_addr(pMgmt->abyCurrBSSID, pMACHeader->addr2)) {
 	    if (pMgmt->sNodeDBTable[0].uInActiveCount != 0)
                   pMgmt->sNodeDBTable[0].uInActiveCount = 0;
            }
        }
     }
 
-    if (!is_multicast_ether_addr(pMACHeader->abyAddr1)) {
-        if ( WCTLbIsDuplicate(&(pDevice->sDupRxCache), (PS802_11Header) pbyFrame) ) {
+    if (!is_multicast_ether_addr(pMACHeader->addr1)) {
+        if (WCTLbIsDuplicate(&(pDevice->sDupRxCache), (struct ieee80211_hdr *) pbyFrame)) {
             pDevice->s802_11Counter.FrameDuplicateCount++;
             return false;
         }
 
 	if (compare_ether_addr(pDevice->abyCurrentNetAddr,
-			       pMACHeader->abyAddr1)) {
+			       pMACHeader->addr1)) {
 		return false;
         }
     }
 
-
     // Use for TKIP MIC
     s_vGetDASA(pbyFrame, &cbHeaderSize, &pDevice->sRxEthHeader);
 
-    if (!compare_ether_addr((PBYTE)&(pDevice->sRxEthHeader.abySrcAddr[0]),
+    if (!compare_ether_addr((u8 *)&(pDevice->sRxEthHeader.h_source[0]),
 			    pDevice->abyCurrentNetAddr))
         return false;
 
     if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) || (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA)) {
         if (IS_CTL_PSPOLL(pbyFrame) || !IS_TYPE_CONTROL(pbyFrame)) {
-            p802_11Header = (PS802_11Header) (pbyFrame);
+            p802_11Header = (struct ieee80211_hdr *) (pbyFrame);
             // get SA NodeIndex
-            if (BSSbIsSTAInNodeDB(pDevice, (PBYTE)(p802_11Header->abyAddr2), &iSANodeIndex)) {
+            if (BSSbIsSTAInNodeDB(pDevice, (u8 *)(p802_11Header->addr2), &iSANodeIndex)) {
                 pMgmt->sNodeDBTable[iSANodeIndex].ulLastRxJiffer = jiffies;
                 pMgmt->sNodeDBTable[iSANodeIndex].uInActiveCount = 0;
             }
@@ -428,7 +405,6 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, PRCB pRCB,
         }
     }
 
-
     if (IS_FC_WEP(pbyFrame)) {
         bool     bRxDecryOK = false;
 
@@ -497,7 +473,6 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, PRCB pRCB,
             FrameSize -= 4;         // 4 is ICV
     }
 
-
     //
     // RX OK
     //
@@ -508,7 +483,7 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, PRCB pRCB,
         (IS_FRAGMENT_PKT((pbyFrame)))
         ) {
         // defragment
-        bDeFragRx = WCTLbHandleFragment(pDevice, (PS802_11Header) (pbyFrame), FrameSize, bIsWEP, bExtIV);
+        bDeFragRx = WCTLbHandleFragment(pDevice, (struct ieee80211_hdr *) (pbyFrame), FrameSize, bIsWEP, bExtIV);
         pDevice->s802_11Counter.ReceivedFragmentCount++;
         if (bDeFragRx) {
             // defrag complete
@@ -529,8 +504,8 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, PRCB pRCB,
         // Handle Control & Manage Frame
 
         if (IS_TYPE_MGMT((pbyFrame))) {
-            PBYTE pbyData1;
-            PBYTE pbyData2;
+            u8 * pbyData1;
+            u8 * pbyData2;
 
             pRxPacket = &(pRCB->sMngPacket);
             pRxPacket->p80211Header = (PUWLAN_80211HDR)(pbyFrame);
@@ -622,10 +597,10 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, PRCB pRCB,
             }
    //mike add:station mode check eapol-key challenge--->
    	  {
-   	    BYTE  Protocol_Version;    //802.1x Authentication
-	    BYTE  Packet_Type;           //802.1x Authentication
-	    BYTE  Descriptor_type;
-             WORD Key_info;
+   	    u8  Protocol_Version;    //802.1x Authentication
+	    u8  Packet_Type;           //802.1x Authentication
+	    u8  Descriptor_type;
+             u16 Key_info;
               if (bIsWEP)
                   cbIVOffset = 8;
               else
@@ -653,10 +628,8 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, PRCB pRCB,
         }
     }
 
-
 // Data frame Handle
 
-
     if (pDevice->bEnablePSMode) {
         if (IS_FC_MOREDATA((pbyFrame))) {
             if (*pbyRsr & RSR_ADDROK) {
@@ -699,11 +672,10 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, PRCB pRCB,
     }
 */
 
-
     // -----------------------------------------------
 
     if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) && (pDevice->bEnable8021x == true)){
-        BYTE    abyMacHdr[24];
+        u8    abyMacHdr[24];
 
         // Only 802.1x packet incoming allowed
         if (bIsWEP)
@@ -739,7 +711,6 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, PRCB pRCB,
             return false;
     }
 
-
     if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_TKIP)) {
         if (bIsWEP) {
             FrameSize -= 8;  //MIC
@@ -750,44 +721,42 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, PRCB pRCB,
     // Soft MIC
     if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_TKIP)) {
         if (bIsWEP) {
-            PDWORD          pdwMIC_L;
-            PDWORD          pdwMIC_R;
-            DWORD           dwMIC_Priority;
-            DWORD           dwMICKey0 = 0, dwMICKey1 = 0;
-            DWORD           dwLocalMIC_L = 0;
-            DWORD           dwLocalMIC_R = 0;
-
+            u32 *          pdwMIC_L;
+            u32 *          pdwMIC_R;
+            u32           dwMIC_Priority;
+            u32           dwMICKey0 = 0, dwMICKey1 = 0;
+            u32           dwLocalMIC_L = 0;
+            u32           dwLocalMIC_R = 0;
 
             if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-                dwMICKey0 = cpu_to_le32(*(PDWORD)(&pKey->abyKey[24]));
-                dwMICKey1 = cpu_to_le32(*(PDWORD)(&pKey->abyKey[28]));
+                dwMICKey0 = cpu_to_le32(*(u32 *)(&pKey->abyKey[24]));
+                dwMICKey1 = cpu_to_le32(*(u32 *)(&pKey->abyKey[28]));
             }
             else {
                 if (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) {
-                    dwMICKey0 = cpu_to_le32(*(PDWORD)(&pKey->abyKey[16]));
-                    dwMICKey1 = cpu_to_le32(*(PDWORD)(&pKey->abyKey[20]));
+                    dwMICKey0 = cpu_to_le32(*(u32 *)(&pKey->abyKey[16]));
+                    dwMICKey1 = cpu_to_le32(*(u32 *)(&pKey->abyKey[20]));
                 } else if ((pKey->dwKeyIndex & BIT28) == 0) {
-                    dwMICKey0 = cpu_to_le32(*(PDWORD)(&pKey->abyKey[16]));
-                    dwMICKey1 = cpu_to_le32(*(PDWORD)(&pKey->abyKey[20]));
+                    dwMICKey0 = cpu_to_le32(*(u32 *)(&pKey->abyKey[16]));
+                    dwMICKey1 = cpu_to_le32(*(u32 *)(&pKey->abyKey[20]));
                 } else {
-                    dwMICKey0 = cpu_to_le32(*(PDWORD)(&pKey->abyKey[24]));
-                    dwMICKey1 = cpu_to_le32(*(PDWORD)(&pKey->abyKey[28]));
+                    dwMICKey0 = cpu_to_le32(*(u32 *)(&pKey->abyKey[24]));
+                    dwMICKey1 = cpu_to_le32(*(u32 *)(&pKey->abyKey[28]));
                 }
             }
 
             MIC_vInit(dwMICKey0, dwMICKey1);
-            MIC_vAppend((PBYTE)&(pDevice->sRxEthHeader.abyDstAddr[0]), 12);
+            MIC_vAppend((u8 *)&(pDevice->sRxEthHeader.h_dest[0]), 12);
             dwMIC_Priority = 0;
-            MIC_vAppend((PBYTE)&dwMIC_Priority, 4);
+            MIC_vAppend((u8 *)&dwMIC_Priority, 4);
             // 4 is Rcv buffer header, 24 is MAC Header, and 8 is IV and Ext IV.
-            MIC_vAppend((PBYTE)(skb->data + 8 + WLAN_HDR_ADDR3_LEN + 8),
+            MIC_vAppend((u8 *)(skb->data + 8 + WLAN_HDR_ADDR3_LEN + 8),
                         FrameSize - WLAN_HDR_ADDR3_LEN - 8);
             MIC_vGetMIC(&dwLocalMIC_L, &dwLocalMIC_R);
             MIC_vUnInit();
 
-            pdwMIC_L = (PDWORD)(skb->data + 8 + FrameSize);
-            pdwMIC_R = (PDWORD)(skb->data + 8 + FrameSize + 4);
-
+            pdwMIC_L = (u32 *)(skb->data + 8 + FrameSize);
+            pdwMIC_R = (u32 *)(skb->data + 8 + FrameSize + 4);
 
             if ((cpu_to_le32(*pdwMIC_L) != dwLocalMIC_L) || (cpu_to_le32(*pdwMIC_R) != dwLocalMIC_R) ||
                 (pDevice->bRxMICFail == true)) {
@@ -818,7 +787,7 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, PRCB pRCB,
 					}
 
 					ev.src_addr.sa_family = ARPHRD_ETHER;
-					memcpy(ev.src_addr.sa_data, pMACHeader->abyAddr2, ETH_ALEN);
+					memcpy(ev.src_addr.sa_data, pMACHeader->addr2, ETH_ALEN);
 					memset(&wrqu, 0, sizeof(wrqu));
 					wrqu.data.length = sizeof(ev);
 			PRINT_K("wireless_send_event--->IWEVMICHAELMICFAILURE\n");
@@ -837,13 +806,13 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, PRCB pRCB,
     if ((pKey != NULL) && ((pKey->byCipherSuite == KEY_CTL_TKIP) ||
                            (pKey->byCipherSuite == KEY_CTL_CCMP))) {
         if (bIsWEP) {
-            WORD        wLocalTSC15_0 = 0;
-            DWORD       dwLocalTSC47_16 = 0;
+            u16        wLocalTSC15_0 = 0;
+            u32       dwLocalTSC47_16 = 0;
 	    unsigned long long       RSC = 0;
             // endian issues
 	    RSC = *((unsigned long long *) &(pKey->KeyRSC));
-            wLocalTSC15_0 = (WORD) RSC;
-            dwLocalTSC47_16 = (DWORD) (RSC>>16);
+            wLocalTSC15_0 = (u16) RSC;
+            dwLocalTSC47_16 = (u32) (RSC>>16);
 
             RSC = dwRxTSC47_16;
             RSC <<= 16;
@@ -876,8 +845,7 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, PRCB pRCB,
         }
     } // ----- End of Reply Counter Check --------------------------
 
-
-    s_vProcessRxMACHeader(pDevice, (PBYTE)(skb->data+8), FrameSize, bIsWEP, bExtIV, &cbHeaderOffset);
+    s_vProcessRxMACHeader(pDevice, (u8 *)(skb->data+8), FrameSize, bIsWEP, bExtIV, &cbHeaderOffset);
     FrameSize -= cbHeaderOffset;
     cbHeaderOffset += 8;        // 8 is Rcv buffer header
 
@@ -928,13 +896,12 @@ static int s_bAPModeRxCtl(struct vnt_private *pDevice, u8 *pbyFrame,
 	s32 iSANodeIndex)
 {
 	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
-	PS802_11Header p802_11Header;
+	struct ieee80211_hdr *p802_11Header;
 	CMD_STATUS Status;
 
-
     if (IS_CTL_PSPOLL(pbyFrame) || !IS_TYPE_CONTROL(pbyFrame)) {
 
-        p802_11Header = (PS802_11Header) (pbyFrame);
+        p802_11Header = (struct ieee80211_hdr *) (pbyFrame);
         if (!IS_TYPE_MGMT(pbyFrame)) {
 
             // Data & PS-Poll packet
@@ -946,7 +913,7 @@ static int s_bAPModeRxCtl(struct vnt_private *pDevice, u8 *pbyFrame,
                     // reason = (6) class 2 received from nonauth sta
                     vMgrDeAuthenBeginSta(pDevice,
                                          pMgmt,
-                                         (PBYTE)(p802_11Header->abyAddr2),
+                                         (u8 *)(p802_11Header->addr2),
                                          (WLAN_MGMT_REASON_CLASS2_NONAUTH),
                                          &Status
                                          );
@@ -958,7 +925,7 @@ static int s_bAPModeRxCtl(struct vnt_private *pDevice, u8 *pbyFrame,
                     // reason = (7) class 3 received from nonassoc sta
                     vMgrDisassocBeginSta(pDevice,
                                          pMgmt,
-                                         (PBYTE)(p802_11Header->abyAddr2),
+                                         (u8 *)(p802_11Header->addr2),
                                          (WLAN_MGMT_REASON_CLASS3_NONASSOC),
                                          &Status
                                          );
@@ -1011,18 +978,18 @@ static int s_bAPModeRxCtl(struct vnt_private *pDevice, u8 *pbyFrame,
             else {
                   vMgrDeAuthenBeginSta(pDevice,
                                        pMgmt,
-                                       (PBYTE)(p802_11Header->abyAddr2),
+                                       (u8 *)(p802_11Header->addr2),
                                        (WLAN_MGMT_REASON_CLASS2_NONAUTH),
                                        &Status
                                        );
                     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: send vMgrDeAuthenBeginSta 3\n");
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BSSID:%pM\n",
-				p802_11Header->abyAddr3);
+				p802_11Header->addr3);
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ADDR2:%pM\n",
-				p802_11Header->abyAddr2);
+				p802_11Header->addr2);
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ADDR1:%pM\n",
-				p802_11Header->abyAddr1);
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: wFrameCtl= %x\n", p802_11Header->wFrameCtl );
+				p802_11Header->addr1);
+                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: frame_control= %x\n", p802_11Header->frame_control);
                     return true;
             }
         }
@@ -1042,13 +1009,12 @@ static int s_bHandleRxEncryption(struct vnt_private *pDevice, u8 *pbyFrame,
 	PSKeyItem pKey = NULL;
 	u8 byDecMode = KEY_CTL_WEP;
 
-
     *pwRxTSC15_0 = 0;
     *pdwRxTSC47_16 = 0;
 
     pbyIV = pbyFrame + WLAN_HDR_ADDR3_LEN;
-    if ( WLAN_GET_FC_TODS(*(PWORD)pbyFrame) &&
-         WLAN_GET_FC_FROMDS(*(PWORD)pbyFrame) ) {
+    if ( WLAN_GET_FC_TODS(*(u16 *)pbyFrame) &&
+         WLAN_GET_FC_FROMDS(*(u16 *)pbyFrame) ) {
          pbyIV += 6;             // 6 is 802.11 address4
          PayloadLen -= 6;
     }
@@ -1136,12 +1102,12 @@ static int s_bHandleRxEncryption(struct vnt_private *pDevice, u8 *pbyFrame,
         // TKIP/AES
 
         PayloadLen -= (WLAN_HDR_ADDR3_LEN + 8 + 4); // 24 is 802.11 header, 8 is IV&ExtIV, 4 is crc
-        *pdwRxTSC47_16 = cpu_to_le32(*(PDWORD)(pbyIV + 4));
+        *pdwRxTSC47_16 = cpu_to_le32(*(u32 *)(pbyIV + 4));
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ExtIV: %x\n", *pdwRxTSC47_16);
         if (byDecMode == KEY_CTL_TKIP) {
             *pwRxTSC15_0 = cpu_to_le16(MAKEWORD(*(pbyIV+2), *pbyIV));
         } else {
-            *pwRxTSC15_0 = cpu_to_le16(*(PWORD)pbyIV);
+            *pwRxTSC15_0 = cpu_to_le16(*(u16 *)pbyIV);
         }
         DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"TSC0_15: %x\n", *pwRxTSC15_0);
 
@@ -1149,8 +1115,8 @@ static int s_bHandleRxEncryption(struct vnt_private *pDevice, u8 *pbyFrame,
             (pDevice->byLocalID <= REV_ID_VT3253_A1)) {
             // Software TKIP
             // 1. 3253 A
-            PS802_11Header  pMACHeader = (PS802_11Header) (pbyFrame);
-            TKIPvMixKey(pKey->abyKey, pMACHeader->abyAddr2, *pwRxTSC15_0, *pdwRxTSC47_16, pDevice->abyPRNG);
+            struct ieee80211_hdr *pMACHeader = (struct ieee80211_hdr *) (pbyFrame);
+            TKIPvMixKey(pKey->abyKey, pMACHeader->addr2, *pwRxTSC15_0, *pdwRxTSC47_16, pDevice->abyPRNG);
             rc4_init(&pDevice->SBox, pDevice->abyPRNG, TKIP_KEY_LEN);
             rc4_encrypt(&pDevice->SBox, pbyIV+8, pbyIV+8, PayloadLen);
             if (ETHbIsBufferCrc32Ok(pbyIV+8, PayloadLen)) {
@@ -1173,7 +1139,7 @@ static int s_bHostWepRxEncryption(struct vnt_private *pDevice, u8 *pbyFrame,
 	s32 *pbExtIV, u16 *pwRxTSC15_0, u32 *pdwRxTSC47_16)
 {
 	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
-	PS802_11Header  pMACHeader;
+	struct ieee80211_hdr *pMACHeader;
 	u32 PayloadLen = FrameSize;
 	u8 *pbyIV;
 	u8 byKeyIdx;
@@ -1183,8 +1149,8 @@ static int s_bHostWepRxEncryption(struct vnt_private *pDevice, u8 *pbyFrame,
 	*pdwRxTSC47_16 = 0;
 
     pbyIV = pbyFrame + WLAN_HDR_ADDR3_LEN;
-    if ( WLAN_GET_FC_TODS(*(PWORD)pbyFrame) &&
-         WLAN_GET_FC_FROMDS(*(PWORD)pbyFrame) ) {
+    if ( WLAN_GET_FC_TODS(*(u16 *)pbyFrame) &&
+         WLAN_GET_FC_FROMDS(*(u16 *)pbyFrame) ) {
          pbyIV += 6;             // 6 is 802.11 address4
          PayloadLen -= 6;
     }
@@ -1192,7 +1158,6 @@ static int s_bHostWepRxEncryption(struct vnt_private *pDevice, u8 *pbyFrame,
     byKeyIdx >>= 6;
     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"\nKeyIdx: %d\n", byKeyIdx);
 
-
     if (pMgmt->byCSSGK == KEY_CTL_TKIP)
         byDecMode = KEY_CTL_TKIP;
     else if (pMgmt->byCSSGK == KEY_CTL_CCMP)
@@ -1235,13 +1200,13 @@ static int s_bHostWepRxEncryption(struct vnt_private *pDevice, u8 *pbyFrame,
         // TKIP/AES
 
         PayloadLen -= (WLAN_HDR_ADDR3_LEN + 8 + 4); // 24 is 802.11 header, 8 is IV&ExtIV, 4 is crc
-        *pdwRxTSC47_16 = cpu_to_le32(*(PDWORD)(pbyIV + 4));
+        *pdwRxTSC47_16 = cpu_to_le32(*(u32 *)(pbyIV + 4));
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ExtIV: %x\n", *pdwRxTSC47_16);
 
         if (byDecMode == KEY_CTL_TKIP) {
             *pwRxTSC15_0 = cpu_to_le16(MAKEWORD(*(pbyIV+2), *pbyIV));
         } else {
-            *pwRxTSC15_0 = cpu_to_le16(*(PWORD)pbyIV);
+            *pwRxTSC15_0 = cpu_to_le16(*(u16 *)pbyIV);
         }
         DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"TSC0_15: %x\n", *pwRxTSC15_0);
 
@@ -1252,8 +1217,8 @@ static int s_bHostWepRxEncryption(struct vnt_private *pDevice, u8 *pbyFrame,
                 // 1. 3253 A
                 // 2. NotOnFly
                 DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"soft KEY_CTL_TKIP \n");
-                pMACHeader = (PS802_11Header) (pbyFrame);
-                TKIPvMixKey(pKey->abyKey, pMACHeader->abyAddr2, *pwRxTSC15_0, *pdwRxTSC47_16, pDevice->abyPRNG);
+                pMACHeader = (struct ieee80211_hdr *) (pbyFrame);
+                TKIPvMixKey(pKey->abyKey, pMACHeader->addr2, *pwRxTSC15_0, *pdwRxTSC47_16, pDevice->abyPRNG);
                 rc4_init(&pDevice->SBox, pDevice->abyPRNG, TKIP_KEY_LEN);
                 rc4_encrypt(&pDevice->SBox, pbyIV+8, pbyIV+8, PayloadLen);
                 if (ETHbIsBufferCrc32Ok(pbyIV+8, PayloadLen)) {
@@ -1297,11 +1262,10 @@ static int s_bAPModeRxData(struct vnt_private *pDevice, struct sk_buff *skb,
 	u8 byMask[8] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80};
 	u16 wAID;
 
-
     if (FrameSize > CB_MAX_BUF_SIZE)
         return false;
     // check DA
-    if (is_multicast_ether_addr((PBYTE)(skb->data+cbHeaderOffset))) {
+    if (is_multicast_ether_addr((u8 *)(skb->data+cbHeaderOffset))) {
        if (pMgmt->sNodeDBTable[0].bPSEnable) {
 
            skbcpy = dev_alloc_skb((int)pDevice->rx_buf_sz);
@@ -1326,7 +1290,7 @@ static int s_bAPModeRxData(struct vnt_private *pDevice, struct sk_buff *skb,
     }
     else {
         // check if relay
-        if (BSSbIsSTAInNodeDB(pDevice, (PBYTE)(skb->data+cbHeaderOffset), &iDANodeIndex)) {
+        if (BSSbIsSTAInNodeDB(pDevice, (u8 *)(skb->data+cbHeaderOffset), &iDANodeIndex)) {
             if (pMgmt->sNodeDBTable[iDANodeIndex].eNodeState >= NODE_ASSOC) {
                 if (pMgmt->sNodeDBTable[iDANodeIndex].bPSEnable) {
                     // queue this skb until next PS tx, and then release.
@@ -1356,7 +1320,7 @@ static int s_bAPModeRxData(struct vnt_private *pDevice, struct sk_buff *skb,
             iDANodeIndex = 0;
 
         if ((pDevice->uAssocCount > 1) && (iDANodeIndex >= 0)) {
-		bRelayPacketSend(pDevice, (PBYTE) (skb->data + cbHeaderOffset),
+		bRelayPacketSend(pDevice, (u8 *) (skb->data + cbHeaderOffset),
 				 FrameSize, (unsigned int) iDANodeIndex);
         }
 
@@ -1370,9 +1334,6 @@ static int s_bAPModeRxData(struct vnt_private *pDevice, struct sk_buff *skb,
     return true;
 }
 
-
-
-
 void RXvWorkItem(struct vnt_private *pDevice)
 {
 	int ntStatus;
@@ -1395,12 +1356,10 @@ void RXvWorkItem(struct vnt_private *pDevice)
 
 }
 
-
 void RXvFreeRCB(PRCB pRCB, int bReAllocSkb)
 {
 	struct vnt_private *pDevice = pRCB->pDevice;
 
-
     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---->RXvFreeRCB\n");
 
     ASSERT(!pRCB->Ref);     // should be 0
@@ -1426,7 +1385,6 @@ void RXvFreeRCB(PRCB pRCB, int bReAllocSkb)
     EnqueueRCB(pDevice->FirstRecvFreeList, pDevice->LastRecvFreeList, pRCB);
     pDevice->NumRecvFreeList++;
 
-
     if ((pDevice->Flags & fMP_POST_READS) && MP_IS_READY(pDevice) &&
         (pDevice->bIsRxWorkItemQueued == false) ) {
 
@@ -1436,7 +1394,6 @@ void RXvFreeRCB(PRCB pRCB, int bReAllocSkb)
     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"<----RXFreeRCB %d %d\n",pDevice->NumRecvFreeList, pDevice->NumRecvMngList);
 }
 
-
 void RXvMngWorkItem(struct vnt_private *pDevice)
 {
 	PRCB pRCB = NULL;
@@ -1471,4 +1428,3 @@ void RXvMngWorkItem(struct vnt_private *pDevice)
 
 }
 
-
diff --git a/drivers/staging/vt6656/dpc.h b/drivers/staging/vt6656/dpc.h
index 786c523f547..876468f2c3d 100644
--- a/drivers/staging/vt6656/dpc.h
+++ b/drivers/staging/vt6656/dpc.h
@@ -29,18 +29,9 @@
 #ifndef __DPC_H__
 #define __DPC_H__
 
-#include "ttype.h"
 #include "device.h"
 #include "wcmd.h"
 
-/*---------------------  Export Definitions -------------------------*/
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
 void RXvWorkItem(void *Context);
 
 void RXvMngWorkItem(void *Context);
diff --git a/drivers/staging/vt6656/firmware.c b/drivers/staging/vt6656/firmware.c
index 4371a77e9ad..a1dc3a4cfd9 100644
--- a/drivers/staging/vt6656/firmware.c
+++ b/drivers/staging/vt6656/firmware.c
@@ -35,8 +35,6 @@
 #include "control.h"
 #include "rndis.h"
 
-/*---------------------  Static Definitions -------------------------*/
-
 static int          msglevel                =MSG_LEVEL_INFO;
 //static int          msglevel                =MSG_LEVEL_DEBUG;
 
@@ -45,17 +43,6 @@ static int          msglevel                =MSG_LEVEL_INFO;
 
 #define FIRMWARE_CHUNK_SIZE	0x400
 
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-
 int FIRMWAREbDownload(struct vnt_private *pDevice)
 {
 	struct device *dev = &pDevice->usb->dev;
@@ -66,7 +53,6 @@ int FIRMWAREbDownload(struct vnt_private *pDevice)
 	u16 wLength;
 	int ii, rc;
 
-
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---->Download firmware\n");
 	spin_unlock_irq(&pDevice->lock);
 
@@ -132,7 +118,6 @@ int FIRMWAREbBrach2Sram(struct vnt_private *pDevice)
     }
 }
 
-
 int FIRMWAREbCheckVersion(struct vnt_private *pDevice)
 {
 	int ntStatus;
@@ -142,7 +127,7 @@ int FIRMWAREbCheckVersion(struct vnt_private *pDevice)
                                     0,
                                     MESSAGE_REQUEST_VERSION,
                                     2,
-                                    (PBYTE) &(pDevice->wFirmwareVersion));
+                                    (u8 *) &(pDevice->wFirmwareVersion));
 
     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Firmware Version [%04x]\n", pDevice->wFirmwareVersion);
     if (ntStatus != STATUS_SUCCESS) {
diff --git a/drivers/staging/vt6656/firmware.h b/drivers/staging/vt6656/firmware.h
index ebab3a6351b..e3b08db6a73 100644
--- a/drivers/staging/vt6656/firmware.h
+++ b/drivers/staging/vt6656/firmware.h
@@ -30,17 +30,8 @@
 #ifndef __FIRMWARE_H__
 #define __FIRMWARE_H__
 
-#include "ttype.h"
 #include "device.h"
 
-/*---------------------  Export Definitions -------------------------*/
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
 int FIRMWAREbDownload(struct vnt_private *);
 int FIRMWAREbBrach2Sram(struct vnt_private *);
 int FIRMWAREbCheckVersion(struct vnt_private *);
diff --git a/drivers/staging/vt6656/hostap.c b/drivers/staging/vt6656/hostap.c
index bc5e9da4758..f4f1bf7a30f 100644
--- a/drivers/staging/vt6656/hostap.c
+++ b/drivers/staging/vt6656/hostap.c
@@ -142,7 +142,6 @@ static int hostap_disable_hostapd(struct vnt_private *pDevice, int rtnl_locked)
 	return 0;
 }
 
-
 /*
  * Description:
  *      Set enable/disable hostapd mode
@@ -174,7 +173,6 @@ int vt6656_hostap_set_hostapd(struct vnt_private *pDevice,
 		return hostap_disable_hostapd(pDevice, rtnl_locked);
 }
 
-
 /*
  * Description:
  *      remove station function supported for hostap daemon
@@ -193,7 +191,6 @@ static int hostap_remove_sta(struct vnt_private *pDevice,
 {
 	unsigned int uNodeIndex;
 
-
     if (BSSbIsSTAInNodeDB(pDevice, param->sta_addr, &uNodeIndex)) {
         BSSvRemoveOneNode(pDevice, uNodeIndex);
     }
@@ -242,7 +239,7 @@ static int hostap_add_sta(struct vnt_private *pDevice,
     pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble =
             WLAN_GET_CAP_INFO_SHORTPREAMBLE(pMgmt->sNodeDBTable[uNodeIndex].wCapInfo);
 
-    pMgmt->sNodeDBTable[uNodeIndex].wAID = (WORD)param->u.add_sta.aid;
+    pMgmt->sNodeDBTable[uNodeIndex].wAID = (u16)param->u.add_sta.aid;
 
     pMgmt->sNodeDBTable[uNodeIndex].ulLastRxJiffer = jiffies;
 
@@ -294,7 +291,6 @@ static int hostap_get_info_sta(struct vnt_private *pDevice,
 	return 0;
 }
 
-
 /*
  * Description:
  *      set station flag
@@ -327,8 +323,6 @@ static int hostap_set_flags_sta(struct vnt_private *pDevice,
 	return 0;
 }
 
-
-
 /*
  * Description:
  *      set generic element (wpa ie)
@@ -347,8 +341,6 @@ static int hostap_set_generic_element(struct vnt_private *pDevice,
 {
 	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
 
-
-
     memcpy( pMgmt->abyWPAIE,
             param->u.generic_elem.data,
             param->u.generic_elem.len
@@ -425,13 +417,11 @@ static int hostap_set_encryption(struct vnt_private *pDevice,
 	int bKeyTableFull = false;
 	u16 wKeyCtl = 0;
 
-
 	param->u.crypt.err = 0;
 
 	if (param->u.crypt.alg > WPA_ALG_CCMP)
 		return -EINVAL;
 
-
 	if ((param->u.crypt.idx > 3) || (param->u.crypt.key_len > MAX_KEY_LEN)) {
 		param->u.crypt.err = HOSTAP_CRYPT_ERR_KEY_SET_FAILED;
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " HOSTAP_CRYPT_ERR_KEY_SET_FAILED\n");
@@ -489,9 +479,9 @@ static int hostap_set_encryption(struct vnt_private *pDevice,
             param->u.crypt.key_len
            );
 
-    dwKeyIndex = (DWORD)(param->u.crypt.idx);
+    dwKeyIndex = (u32)(param->u.crypt.idx);
     if (param->u.crypt.flags & HOSTAP_CRYPT_FLAG_SET_TX_KEY) {
-        pDevice->byKeyIndex = (BYTE)dwKeyIndex;
+        pDevice->byKeyIndex = (u8)dwKeyIndex;
         pDevice->bTransmitKey = true;
         dwKeyIndex |= (1 << 31);
     }
@@ -515,11 +505,10 @@ static int hostap_set_encryption(struct vnt_private *pDevice,
 			&param->sta_addr[0],
 			dwKeyIndex & ~(USE_KEYRSC),
 			param->u.crypt.key_len,
-			&KeyRSC, (PBYTE)abyKey,
+			&KeyRSC, (u8 *)abyKey,
 			KEY_CTL_WEP
                            ) == true) {
 
-
                 pMgmt->sNodeDBTable[iNodeIndex].bOnFly = true;
 
             } else {
@@ -565,7 +554,6 @@ static int hostap_set_encryption(struct vnt_private *pDevice,
         pMgmt->byCSSGK = KEY_CTL_CCMP;
     }
 
-
     if (iNodeIndex == 0) {
        KeybSetDefaultKey(  pDevice,
                            &(pDevice->sKey),
@@ -585,7 +573,7 @@ static int hostap_set_encryption(struct vnt_private *pDevice,
                        dwKeyIndex,
                        param->u.crypt.key_len,
 			&KeyRSC,
-                       (PBYTE)abyKey,
+                       (u8 *)abyKey,
                         byKeyDecMode
                        ) == true) {
 
@@ -631,8 +619,6 @@ static int hostap_set_encryption(struct vnt_private *pDevice,
 	return ret;
 }
 
-
-
 /*
  * Description:
  *      get each stations encryption key
@@ -655,7 +641,6 @@ static int hostap_get_encryption(struct vnt_private *pDevice,
 	int ii;
 	s32 iNodeIndex = 0;
 
-
 	param->u.crypt.err = 0;
 
 	if (is_broadcast_ether_addr(param->sta_addr)) {
@@ -670,13 +655,12 @@ static int hostap_get_encryption(struct vnt_private *pDevice,
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "hostap_get_encryption: %d\n", iNodeIndex);
     memset(param->u.crypt.seq, 0, 8);
     for (ii = 0 ; ii < 8 ; ii++) {
-        param->u.crypt.seq[ii] = (BYTE)pMgmt->sNodeDBTable[iNodeIndex].KeyRSC >> (ii * 8);
+        param->u.crypt.seq[ii] = (u8)pMgmt->sNodeDBTable[iNodeIndex].KeyRSC >> (ii * 8);
     }
 
 	return ret;
 }
 
-
 /*
  * Description:
  *      vt6656_hostap_ioctl main function supported for hostap daemon.
@@ -779,7 +763,6 @@ int vt6656_hostap_ioctl(struct vnt_private *pDevice, struct iw_point *p)
 		goto out;
 	}
 
-
 	if ((ret == 0) && ap_ioctl) {
 		if (copy_to_user(p->pointer, param, p->length)) {
 			ret = -EFAULT;
diff --git a/drivers/staging/vt6656/hostap.h b/drivers/staging/vt6656/hostap.h
index f5656cd96a8..6a68f7e69cf 100644
--- a/drivers/staging/vt6656/hostap.h
+++ b/drivers/staging/vt6656/hostap.h
@@ -31,8 +31,6 @@
 
 #include "device.h"
 
-/*---------------------  Export Definitions -------------------------*/
-
 #define WLAN_RATE_1M    BIT0
 #define WLAN_RATE_2M    BIT1
 #define WLAN_RATE_5M5   BIT2
@@ -46,13 +44,6 @@
 #define WLAN_RATE_48M   BIT10
 #define WLAN_RATE_54M   BIT11
 
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
 #ifndef ETH_P_PAE
 #define ETH_P_PAE 0x888E /* Port Access Entity (IEEE 802.1X) */
 #endif /* ETH_P_PAE */
diff --git a/drivers/staging/vt6656/int.c b/drivers/staging/vt6656/int.c
index 51990bd3dd4..a2b4ba6d4f0 100644
--- a/drivers/staging/vt6656/int.c
+++ b/drivers/staging/vt6656/int.c
@@ -40,19 +40,8 @@
 #include "bssdb.h"
 #include "usbpipe.h"
 
-/*---------------------  Static Definitions -------------------------*/
 static int msglevel = MSG_LEVEL_INFO; /* MSG_LEVEL_DEBUG */
 
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
 /*+
  *
  *  Function:   InterruptPollingThread
@@ -98,8 +87,8 @@ void INTnsProcessData(struct vnt_private *pDevice)
 	pINTData = (PSINTData) pDevice->intBuf.pDataBuf;
 	if (pINTData->byTSR0 & TSR_VALID) {
 		STAvUpdateTDStatCounter(&(pDevice->scStatistic),
-					(BYTE)(pINTData->byPkt0 & 0x0F),
-					(BYTE)(pINTData->byPkt0>>4),
+					(u8)(pINTData->byPkt0 & 0x0F),
+					(u8)(pINTData->byPkt0>>4),
 					pINTData->byTSR0);
 		BSSvUpdateNodeTxCounter(pDevice,
 					&(pDevice->scStatistic),
@@ -109,8 +98,8 @@ void INTnsProcessData(struct vnt_private *pDevice)
 	}
 	if (pINTData->byTSR1 & TSR_VALID) {
 		STAvUpdateTDStatCounter(&(pDevice->scStatistic),
-					(BYTE)(pINTData->byPkt1 & 0x0F),
-					(BYTE)(pINTData->byPkt1>>4),
+					(u8)(pINTData->byPkt1 & 0x0F),
+					(u8)(pINTData->byPkt1>>4),
 					pINTData->byTSR1);
 		BSSvUpdateNodeTxCounter(pDevice,
 					&(pDevice->scStatistic),
@@ -120,8 +109,8 @@ void INTnsProcessData(struct vnt_private *pDevice)
 	}
 	if (pINTData->byTSR2 & TSR_VALID) {
 		STAvUpdateTDStatCounter(&(pDevice->scStatistic),
-					(BYTE)(pINTData->byPkt2 & 0x0F),
-					(BYTE)(pINTData->byPkt2>>4),
+					(u8)(pINTData->byPkt2 & 0x0F),
+					(u8)(pINTData->byPkt2>>4),
 					pINTData->byTSR2);
 		BSSvUpdateNodeTxCounter(pDevice,
 					&(pDevice->scStatistic),
@@ -131,8 +120,8 @@ void INTnsProcessData(struct vnt_private *pDevice)
 	}
 	if (pINTData->byTSR3 & TSR_VALID) {
 		STAvUpdateTDStatCounter(&(pDevice->scStatistic),
-					(BYTE)(pINTData->byPkt3 & 0x0F),
-					(BYTE)(pINTData->byPkt3>>4),
+					(u8)(pINTData->byPkt3 & 0x0F),
+					(u8)(pINTData->byPkt3>>4),
 					pINTData->byTSR3);
 		BSSvUpdateNodeTxCounter(pDevice,
 					&(pDevice->scStatistic),
diff --git a/drivers/staging/vt6656/int.h b/drivers/staging/vt6656/int.h
index 27c725f1ce1..8e6e217ba4f 100644
--- a/drivers/staging/vt6656/int.h
+++ b/drivers/staging/vt6656/int.h
@@ -30,40 +30,32 @@
 #ifndef __INT_H__
 #define __INT_H__
 
-#include "ttype.h"
 #include "device.h"
 
-/*---------------------  Export Definitions -------------------------*/
 typedef struct tagSINTData {
-	BYTE byTSR0;
-	BYTE byPkt0;
-	WORD wTime0;
-	BYTE byTSR1;
-	BYTE byPkt1;
-	WORD wTime1;
-	BYTE byTSR2;
-	BYTE byPkt2;
-	WORD wTime2;
-	BYTE byTSR3;
-	BYTE byPkt3;
-	WORD wTime3;
+	u8 byTSR0;
+	u8 byPkt0;
+	u16 wTime0;
+	u8 byTSR1;
+	u8 byPkt1;
+	u16 wTime1;
+	u8 byTSR2;
+	u8 byPkt2;
+	u16 wTime2;
+	u8 byTSR3;
+	u8 byPkt3;
+	u16 wTime3;
 	u64 qwTSF;
-	BYTE byISR0;
-	BYTE byISR1;
-	BYTE byRTSSuccess;
-	BYTE byRTSFail;
-	BYTE byACKFail;
-	BYTE byFCSErr;
-	BYTE abySW[2];
+	u8 byISR0;
+	u8 byISR1;
+	u8 byRTSSuccess;
+	u8 byRTSFail;
+	u8 byACKFail;
+	u8 byFCSErr;
+	u8 abySW[2];
 } __attribute__ ((__packed__))
 SINTData, *PSINTData;
 
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
 void INTvWorkItem(struct vnt_private *);
 void INTnsProcessData(struct vnt_private *);
 
diff --git a/drivers/staging/vt6656/iocmd.h b/drivers/staging/vt6656/iocmd.h
index c354a77964d..f3406da7336 100644
--- a/drivers/staging/vt6656/iocmd.h
+++ b/drivers/staging/vt6656/iocmd.h
@@ -29,10 +29,6 @@
 #ifndef __IOCMD_H__
 #define __IOCMD_H__
 
-#include "ttype.h"
-
-/*---------------------  Export Definitions -------------------------*/
-
 // ioctl Command code
 #define MAGIC_CODE	                 0x3142
 #define IOCTL_CMD_TEST	            (SIOCDEVPRIVATE + 0)
@@ -181,14 +177,12 @@ typedef struct tagSBSSIDItem {
 
 } __packed SBSSIDItem;
 
-
 typedef struct tagSBSSIDList {
 
 	u32		    uItem;
 	SBSSIDItem	sBSSIDList[0];
 } __packed SBSSIDList, *PSBSSIDList;
 
-
 typedef struct tagSNodeItem {
     // STA info
     u16            wAID;
@@ -209,7 +203,6 @@ typedef struct tagSNodeItem {
 
 } __packed SNodeItem;
 
-
 typedef struct tagSNodeList {
 
 	u32		    uItem;
@@ -217,7 +210,6 @@ typedef struct tagSNodeList {
 
 } __packed SNodeList, *PSNodeList;
 
-
 typedef struct tagSCmdLinkStatus {
 
     bool    bLink;
@@ -248,8 +240,6 @@ typedef struct tagSDot11MIBCount {
     u32 FCSErrorCount;
 } __packed SDot11MIBCount, *PSDot11MIBCount;
 
-
-
 //
 // statistic counter
 //
@@ -432,12 +422,4 @@ struct viawget_hostapd_param {
 	} u;
 } __packed;
 
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Types  ------------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
 #endif /* __IOCMD_H__ */
diff --git a/drivers/staging/vt6656/iowpa.h b/drivers/staging/vt6656/iowpa.h
index 2522ddec718..97af32e1fa4 100644
--- a/drivers/staging/vt6656/iowpa.h
+++ b/drivers/staging/vt6656/iowpa.h
@@ -29,8 +29,6 @@
 #ifndef __IOWPA_H__
 #define __IOWPA_H__
 
-/*---------------------  Export Definitions -------------------------*/
-
 #define WPA_IE_LEN 64
 
 //WPA related
@@ -55,7 +53,6 @@ enum {
 	VIAWGET_SET_DISASSOCIATE = 10
 };
 
-
 enum {
 	VIAWGET_ASSOC_MSG = 1,
 	VIAWGET_DISASSOC_MSG = 2,
@@ -65,8 +62,6 @@ enum {
 	VIAWGET_DEVICECLOSE_MSG = 6
 };
 
-
-
 typedef struct viawget_wpa_header {
 	u8 type;
 	u16 req_ie_len;
@@ -130,12 +125,4 @@ struct viawget_scan_result {
 	int maxrate;
 } __packed;
 
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Types  ------------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
 #endif /* __IOWPA_H__ */
diff --git a/drivers/staging/vt6656/iwctl.c b/drivers/staging/vt6656/iwctl.c
index 69971f35e49..c335808211e 100644
--- a/drivers/staging/vt6656/iwctl.c
+++ b/drivers/staging/vt6656/iwctl.c
@@ -42,7 +42,6 @@
 #include "control.h"
 #include "rndis.h"
 
-
 static const long frequency_list[] = {
 	2412, 2417, 2422, 2427, 2432, 2437, 2442, 2447, 2452, 2457, 2462, 2467, 2472, 2484,
 	4915, 4920, 4925, 4935, 4940, 4945, 4960, 4980,
@@ -61,8 +60,8 @@ struct iw_statistics *iwctl_get_wireless_stats(struct net_device *dev)
 	pDevice->wstats.status = pDevice->eOPMode;
 	if (pDevice->scStatistic.LinkQuality > 100)
 		pDevice->scStatistic.LinkQuality = 100;
-	pDevice->wstats.qual.qual =(BYTE)pDevice->scStatistic.LinkQuality;
-	RFvRSSITodBm(pDevice, (BYTE)(pDevice->uCurrRSSI), &ldBm);
+	pDevice->wstats.qual.qual =(u8)pDevice->scStatistic.LinkQuality;
+	RFvRSSITodBm(pDevice, (u8)(pDevice->uCurrRSSI), &ldBm);
 	pDevice->wstats.qual.level = ldBm;
 	pDevice->wstats.qual.noise = 0;
 	pDevice->wstats.qual.updated = 1;
@@ -95,7 +94,7 @@ int iwctl_siwscan(struct net_device *dev, struct iw_request_info *info,
 	struct iw_point *wrq = &wrqu->data;
 	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
 	struct iw_scan_req *req = (struct iw_scan_req *)extra;
-	BYTE abyScanSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
+	u8 abyScanSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
 	PWLAN_IE_SSID pItemSSID = NULL;
 
 	if (!(pDevice->flags & DEVICE_FLAGS_OPENED))
@@ -238,7 +237,7 @@ int iwctl_giwscan(struct net_device *dev, struct iw_request_info *info,
 			// ADD quality
 			memset(&iwe, 0, sizeof(iwe));
 			iwe.cmd = IWEVQUAL;
-			RFvRSSITodBm(pDevice, (BYTE)(pBSS->uRSSI), &ldBm);
+			RFvRSSITodBm(pDevice, (u8)(pBSS->uRSSI), &ldBm);
 			iwe.u.qual.level = ldBm;
 			iwe.u.qual.noise = 0;
 
@@ -357,7 +356,6 @@ int iwctl_giwfreq(struct net_device *dev, struct iw_request_info *info,
 	if (pMgmt == NULL)
 		return -EFAULT;
 
-
 #ifdef WEXT_USECHANNELS
 	wrq->m = (int)pMgmt->uCurrChannel;
 	wrq->e = 0;
@@ -482,7 +480,6 @@ int iwctl_siwmode(struct net_device *dev, struct iw_request_info *info,
 		pDevice->bCommit = false;
 	}
 
-
 	return rc;
 }
 
@@ -532,7 +529,7 @@ int iwctl_giwrange(struct net_device *dev, struct iw_request_info *info,
 	struct iw_range *range = (struct iw_range *)extra;
 	int i;
 	int k;
-	BYTE abySupportedRates[13] = {
+	u8 abySupportedRates[13] = {
 		0x02, 0x04, 0x0B, 0x16, 0x0c, 0x12, 0x18, 0x24, 0x30, 0x48,
 		0x60, 0x6C, 0x90
 	};
@@ -635,7 +632,7 @@ int iwctl_siwap(struct net_device *dev, struct iw_request_info *info,
 	struct sockaddr *wrq = &wrqu->ap_addr;
 	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
 	int rc = 0;
-	BYTE ZeroBSSID[WLAN_BSSID_LEN] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
+	u8 ZeroBSSID[WLAN_BSSID_LEN] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
 
 	PRINT_K(" SIOCSIWAP\n");
 
@@ -819,7 +816,7 @@ int iwctl_siwessid(struct net_device *dev, struct iw_request_info *info,
 		if (pDevice->bWPASuppWextEnabled == true)  {
 			/*******search if  in hidden ssid mode ****/
 			PKnownBSS pCurr = NULL;
-			BYTE abyTmpDesireSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
+			u8 abyTmpDesireSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
 			unsigned ii;
 			unsigned uSameBssidNum = 0;
 
@@ -913,7 +910,7 @@ int iwctl_siwrate(struct net_device *dev, struct iw_request_info *info,
 	int rc = 0;
 	u8 brate = 0;
 	int i;
-	BYTE abySupportedRates[13] = {
+	u8 abySupportedRates[13] = {
 		0x02, 0x04, 0x0B, 0x16, 0x0c, 0x12, 0x18, 0x24, 0x30, 0x48,
 		0x60, 0x6C, 0x90
 	};
@@ -996,7 +993,7 @@ int iwctl_giwrate(struct net_device *dev, struct iw_request_info *info,
 		return -EFAULT;
 
 	{
-		BYTE abySupportedRates[13] = {
+		u8 abySupportedRates[13] = {
 			0x02, 0x04, 0x0B, 0x16, 0x0c, 0x12, 0x18, 0x24, 0x30,
 			0x48, 0x60, 0x6C, 0x90
 		};
@@ -1227,7 +1224,7 @@ int iwctl_siwencode(struct net_device *dev, struct iw_request_info *info,
 					KEY_CTL_WEP);
 			spin_unlock_irq(&pDevice->lock);
 		}
-		pDevice->byKeyIndex = (BYTE)dwKeyIndex;
+		pDevice->byKeyIndex = (u8)dwKeyIndex;
 		pDevice->uKeyLength = wrq->length;
 		pDevice->bTransmitKey = true;
 		pDevice->bEncryptionEnable = true;
@@ -1317,7 +1314,7 @@ int iwctl_giwencode(struct net_device *dev, struct iw_request_info *info,
 			memcpy(abyKey, pKey->abyKey,	pKey->uKeyLength);
 			memcpy(extra,  abyKey, WLAN_WEP232_KEYLEN);
 		}
-	} else if (KeybGetKey(&(pDevice->sKey), pDevice->abyBroadcastAddr, (BYTE)index, &pKey)) {
+	} else if (KeybGetKey(&(pDevice->sKey), pDevice->abyBroadcastAddr, (u8)index, &pKey)) {
 		wrq->length = pKey->uKeyLength;
 		memcpy(abyKey, pKey->abyKey, pKey->uKeyLength);
 		memcpy(extra, abyKey, WLAN_WEP232_KEYLEN);
@@ -1424,7 +1421,7 @@ int iwctl_giwsens(struct net_device *dev, struct iw_request_info *info,
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWSENS\n");
 	if (pDevice->bLinkPass == true) {
-		RFvRSSITodBm(pDevice, (BYTE)(pDevice->uCurrRSSI), &ldBm);
+		RFvRSSITodBm(pDevice, (u8)(pDevice->uCurrRSSI), &ldBm);
 		wrq->value = ldBm;
 	} else {
 		wrq->value = 0;
diff --git a/drivers/staging/vt6656/iwctl.h b/drivers/staging/vt6656/iwctl.h
index b594a10db31..dceda0dbdfe 100644
--- a/drivers/staging/vt6656/iwctl.h
+++ b/drivers/staging/vt6656/iwctl.h
@@ -31,14 +31,6 @@
 
 #include "device.h"
 
-/*---------------------  Export Definitions -------------------------*/
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
 struct iw_statistics *iwctl_get_wireless_stats(struct net_device *dev);
 
 int iwctl_siwap(struct net_device *dev, struct iw_request_info *info,
diff --git a/drivers/staging/vt6656/key.c b/drivers/staging/vt6656/key.c
index 416175e8ba5..205590b0e9c 100644
--- a/drivers/staging/vt6656/key.c
+++ b/drivers/staging/vt6656/key.c
@@ -42,24 +42,9 @@
 #include "rndis.h"
 #include "control.h"
 
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
 static int          msglevel                =MSG_LEVEL_INFO;
 //static int          msglevel                =MSG_LEVEL_DEBUG;
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
 
-/*---------------------  Static Variables  --------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
 static void s_vCheckKeyTableValid(struct vnt_private *pDevice,
 	PSKeyManagement pTable)
 {
@@ -79,7 +64,7 @@ static void s_vCheckKeyTableValid(struct vnt_private *pDevice,
             pTable->KeyTable[i].bInUse = false;
             pTable->KeyTable[i].wKeyCtl = 0;
             pTable->KeyTable[i].bSoftWEP = false;
-            pbyData[wLength++] = (BYTE) i;
+            pbyData[wLength++] = (u8) i;
             //MACvDisableKeyEntry(pDevice, i);
         }
     }
@@ -95,10 +80,6 @@ static void s_vCheckKeyTableValid(struct vnt_private *pDevice,
 
 }
 
-
-/*---------------------  Export Functions  --------------------------*/
-
-
 /*
  * Description: Init Key management table
  *
@@ -130,9 +111,9 @@ void KeyvInitTable(struct vnt_private *pDevice, PSKeyManagement pTable)
         pTable->KeyTable[i].wKeyCtl = 0;
         pTable->KeyTable[i].dwGTKeyIndex = 0;
         pTable->KeyTable[i].bSoftWEP = false;
-        pbyData[i] = (BYTE) i;
+        pbyData[i] = (u8) i;
     }
-    pbyData[i] = (BYTE) i;
+    pbyData[i] = (u8) i;
     CONTROLnsRequestOut(pDevice,
                         MESSAGE_TYPE_CLRKEYENTRY,
                         0,
@@ -146,7 +127,6 @@ void KeyvInitTable(struct vnt_private *pDevice, PSKeyManagement pTable)
     return;
 }
 
-
 /*
  * Description: Get Key from table
  *
@@ -197,7 +177,6 @@ int KeybGetKey(PSKeyManagement pTable, u8 *pbyBSSID, u32 dwKeyIndex,
     return (false);
 }
 
-
 /*
  * Description: Set Key to table
  *
@@ -272,7 +251,7 @@ int KeybSetKey(struct vnt_private *pDevice, PSKeyManagement pTable,
                 if (uKeyLength == WLAN_WEP104_KEYLEN)
                     pKey->abyKey[15] |= 0x80;
             }
-            MACvSetKeyEntry(pDevice, pTable->KeyTable[i].wKeyCtl, i, uKeyIdx, pbyBSSID, (PDWORD)pKey->abyKey);
+            MACvSetKeyEntry(pDevice, pTable->KeyTable[i].wKeyCtl, i, uKeyIdx, pbyBSSID, (u32 *)pKey->abyKey);
 
 		if ((dwKeyIndex & USE_KEYRSC) == 0)
 			pKey->KeyRSC = 0; /* RSC set by NIC */
@@ -340,7 +319,7 @@ int KeybSetKey(struct vnt_private *pDevice, PSKeyManagement pTable,
             if (uKeyLength == WLAN_WEP104_KEYLEN)
                 pKey->abyKey[15] |= 0x80;
         }
-        MACvSetKeyEntry(pDevice, pTable->KeyTable[j].wKeyCtl, j, uKeyIdx, pbyBSSID, (PDWORD)pKey->abyKey);
+        MACvSetKeyEntry(pDevice, pTable->KeyTable[j].wKeyCtl, j, uKeyIdx, pbyBSSID, (u32 *)pKey->abyKey);
 
 		if ((dwKeyIndex & USE_KEYRSC) == 0)
 			pKey->KeyRSC = 0; /* RSC set by NIC */
@@ -370,7 +349,6 @@ int KeybSetKey(struct vnt_private *pDevice, PSKeyManagement pTable,
     return (false);
 }
 
-
 /*
  * Description: Remove Key from table
  *
@@ -445,10 +423,8 @@ int KeybRemoveKey(struct vnt_private *pDevice, PSKeyManagement pTable,
     s_vCheckKeyTableValid(pDevice,pTable);
     return bReturnValue;
 
-
 }
 
-
 /*
  * Description: Remove Key from table
  *
@@ -483,45 +459,6 @@ int KeybRemoveAllKey(struct vnt_private *pDevice, PSKeyManagement pTable,
 }
 
 /*
- * Description: Remove WEP Key from table
- *
- * Parameters:
- *  In:
- *      pTable          - Pointer to Key table
- *  Out:
- *      none
- *
- * Return Value: true if success otherwise false
- *
- */
-void KeyvRemoveWEPKey(struct vnt_private *pDevice, PSKeyManagement pTable,
-	u32 dwKeyIndex)
-{
-
-   if ((dwKeyIndex & 0x000000FF) < MAX_GROUP_KEY) {
-        if (pTable->KeyTable[MAX_KEY_TABLE-1].bInUse == true) {
-            if (pTable->KeyTable[MAX_KEY_TABLE-1].GroupKey[dwKeyIndex & 0x000000FF].byCipherSuite == KEY_CTL_WEP) {
-                pTable->KeyTable[MAX_KEY_TABLE-1].GroupKey[dwKeyIndex & 0x000000FF].bKeyValid = false;
-                if ((dwKeyIndex & 0x7FFFFFFF) == (pTable->KeyTable[MAX_KEY_TABLE-1].dwGTKeyIndex & 0x7FFFFFFF)) {
-                    // remove Group transmit key
-                    pTable->KeyTable[MAX_KEY_TABLE-1].dwGTKeyIndex = 0;
-                }
-            }
-        }
-        s_vCheckKeyTableValid(pDevice, pTable);
-    }
-    return;
-}
-
-void KeyvRemoveAllWEPKey(struct vnt_private *pDevice, PSKeyManagement pTable)
-{
-	int i;
-
-	for (i = 0; i < MAX_GROUP_KEY; i++)
-		KeyvRemoveWEPKey(pDevice, pTable, i);
-}
-
-/*
  * Description: Get Transmit Key from table
  *
  * Parameters:
@@ -557,7 +494,6 @@ int KeybGetTransmitKey(PSKeyManagement pTable, u8 *pbyBSSID, u32 dwKeyType,
                     }
                     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"\n");
 
-
                     return (true);
                 }
                 else {
@@ -599,35 +535,6 @@ int KeybGetTransmitKey(PSKeyManagement pTable, u8 *pbyBSSID, u32 dwKeyType,
     return (false);
 }
 
-
-/*
- * Description: Check Pairwise Key
- *
- * Parameters:
- *  In:
- *      pTable          - Pointer to Key table
- *  Out:
- *      none
- *
- * Return Value: true if found otherwise false
- *
- */
-int KeybCheckPairewiseKey(PSKeyManagement pTable, PSKeyItem *pKey)
-{
-	int i;
-
-	*pKey = NULL;
-
-    for (i=0;i<MAX_KEY_TABLE;i++) {
-        if ((pTable->KeyTable[i].bInUse == true) &&
-            (pTable->KeyTable[i].PairwiseKey.bKeyValid == true)) {
-            *pKey = &(pTable->KeyTable[i].PairwiseKey);
-            return (true);
-        }
-    }
-    return (false);
-}
-
 /*
  * Description: Set Key to table
  *
@@ -707,18 +614,16 @@ int KeybSetDefaultKey(struct vnt_private *pDevice, PSKeyManagement pTable,
             pKey->abyKey[15] |= 0x80;
     }
 
-    MACvSetKeyEntry(pDevice, pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl, MAX_KEY_TABLE-1, uKeyIdx, pTable->KeyTable[MAX_KEY_TABLE-1].abyBSSID, (PDWORD) pKey->abyKey);
+    MACvSetKeyEntry(pDevice, pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl, MAX_KEY_TABLE-1, uKeyIdx, pTable->KeyTable[MAX_KEY_TABLE-1].abyBSSID, (u32 *) pKey->abyKey);
 
 		if ((dwKeyIndex & USE_KEYRSC) == 0)
 			pKey->KeyRSC = 0; /* RSC set by NIC */
 		else
 			pKey->KeyRSC = *KeyRSC;
 
-
     pKey->dwTSC47_16 = 0;
     pKey->wTSC15_0 = 0;
 
-
     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"KeybSetKey(R): \n");
     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pKey->bKeyValid: %d\n", pKey->bKeyValid);
     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pKey->uKeyLength: %d\n", (int)pKey->uKeyLength);
@@ -737,7 +642,6 @@ int KeybSetDefaultKey(struct vnt_private *pDevice, PSKeyManagement pTable,
     return (true);
 }
 
-
 /*
  * Description: Set Key to table
  *
@@ -766,7 +670,6 @@ int KeybSetAllGroupKey(struct vnt_private *pDevice, PSKeyManagement pTable,
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Enter KeybSetAllGroupKey: %X\n",
 		dwKeyIndex);
 
-
     if ((dwKeyIndex & PAIRWISE_KEY) != 0) {                  // Pairwise key
         return (false);
     } else if ((dwKeyIndex & 0x000000FF) >= MAX_GROUP_KEY) {
@@ -805,7 +708,7 @@ int KeybSetAllGroupKey(struct vnt_private *pDevice, PSKeyManagement pTable,
                     pKey->abyKey[15] |= 0x80;
             }
 
-            MACvSetKeyEntry(pDevice, pTable->KeyTable[i].wKeyCtl, i, uKeyIdx, pTable->KeyTable[i].abyBSSID, (PDWORD) pKey->abyKey);
+            MACvSetKeyEntry(pDevice, pTable->KeyTable[i].wKeyCtl, i, uKeyIdx, pTable->KeyTable[i].abyBSSID, (u32 *) pKey->abyKey);
 
 		if ((dwKeyIndex & USE_KEYRSC) == 0)
 			pKey->KeyRSC = 0; /* RSC set by NIC */
diff --git a/drivers/staging/vt6656/key.h b/drivers/staging/vt6656/key.h
index 7ecddcd6bcf..23e188d125b 100644
--- a/drivers/staging/vt6656/key.h
+++ b/drivers/staging/vt6656/key.h
@@ -30,17 +30,14 @@
 #ifndef __KEY_H__
 #define __KEY_H__
 
-#include "ttype.h"
 #include "tether.h"
 #include "80211mgr.h"
 
-/*---------------------  Export Definitions -------------------------*/
 #define MAX_GROUP_KEY       4
 #define MAX_KEY_TABLE       11
 #define MAX_KEY_LEN         32
 #define AES_KEY_LEN         16
 
-
 #define AUTHENTICATOR_KEY   0x10000000
 #define USE_KEYRSC          0x20000000
 #define PAIRWISE_KEY        0x40000000
@@ -54,32 +51,31 @@
 #define KEY_CTL_CCMP        0x03
 #define KEY_CTL_INVALID     0xFF
 
-
 typedef struct tagSKeyItem
 {
     bool        bKeyValid;
 	u32 uKeyLength;
-    BYTE        abyKey[MAX_KEY_LEN];
+    u8        abyKey[MAX_KEY_LEN];
 	u64 KeyRSC;
-    DWORD       dwTSC47_16;
-    WORD        wTSC15_0;
-    BYTE        byCipherSuite;
-    BYTE        byReserved0;
-    DWORD       dwKeyIndex;
+    u32       dwTSC47_16;
+    u16        wTSC15_0;
+    u8        byCipherSuite;
+    u8        byReserved0;
+    u32       dwKeyIndex;
     void *pvKeyTable;
 } SKeyItem, *PSKeyItem; //64
 
 typedef struct tagSKeyTable
 {
-    BYTE        abyBSSID[ETH_ALEN];  /* 6 */
-    BYTE        byReserved0[2];              //8
+    u8        abyBSSID[ETH_ALEN];  /* 6 */
+    u8        byReserved0[2];              //8
     SKeyItem    PairwiseKey;
     SKeyItem    GroupKey[MAX_GROUP_KEY]; //64*5 = 320, 320+8=328
-    DWORD       dwGTKeyIndex;            // GroupTransmitKey Index
+    u32       dwGTKeyIndex;            // GroupTransmitKey Index
     bool        bInUse;
-    WORD        wKeyCtl;
+    u16        wKeyCtl;
     bool        bSoftWEP;
-    BYTE        byReserved1[6];
+    u8        byReserved1[6];
 } SKeyTable, *PSKeyTable; //352
 
 typedef struct tagSKeyManagement
@@ -87,16 +83,6 @@ typedef struct tagSKeyManagement
     SKeyTable   KeyTable[MAX_KEY_TABLE];
 } SKeyManagement, *PSKeyManagement;
 
-/*---------------------  Export Types  ------------------------------*/
-
-/*---------------------  Export Macros ------------------------------*/
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
 void KeyvInitTable(struct vnt_private *, PSKeyManagement pTable);
 
 int KeybGetKey(PSKeyManagement pTable, u8 *pbyBSSID, u32 dwKeyIndex,
@@ -112,16 +98,9 @@ int KeybRemoveKey(struct vnt_private *, PSKeyManagement pTable,
 int KeybRemoveAllKey(struct vnt_private *, PSKeyManagement pTable,
 	u8 *pbyBSSID);
 
-void KeyvRemoveWEPKey(struct vnt_private *, PSKeyManagement pTable,
-	u32 dwKeyIndex);
-
-void KeyvRemoveAllWEPKey(struct vnt_private *, PSKeyManagement pTable);
-
 int KeybGetTransmitKey(PSKeyManagement pTable, u8 *pbyBSSID, u32 dwKeyType,
 	PSKeyItem *pKey);
 
-int KeybCheckPairewiseKey(PSKeyManagement pTable, PSKeyItem *pKey);
-
 int KeybSetDefaultKey(struct vnt_private *, PSKeyManagement pTable,
 	u32 dwKeyIndex, u32 uKeyLength, u64 *KeyRSC, u8 *pbyKey,
 	u8 byKeyDecMode);
diff --git a/drivers/staging/vt6656/mac.c b/drivers/staging/vt6656/mac.c
index 76d307b58d5..343db19283a 100644
--- a/drivers/staging/vt6656/mac.c
+++ b/drivers/staging/vt6656/mac.c
@@ -38,63 +38,8 @@
 #include "rndis.h"
 #include "control.h"
 
-/*---------------------  Static Definitions -------------------------*/
 //static int          msglevel                =MSG_LEVEL_DEBUG;
 static int          msglevel                =MSG_LEVEL_INFO;
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-
-
-
-
-/*
- * Description:
- *      Set this hash index into multicast address register bit
- *
- * Parameters:
- *  In:
- *      byHashIdx   - Hash index to set
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void MACvSetMultiAddrByHash(struct vnt_private *pDevice, u8 byHashIdx)
-{
-	u8 uByteIdx;
-	u8 byBitMask;
-	u8 pbyData[2];
-
-
-    // calculate byte position
-    uByteIdx = byHashIdx / 8;
-
-    // calculate bit position
-    byBitMask = 1;
-    byBitMask <<= (byHashIdx % 8);
-    // turn on the bit
-
-    pbyData[0] = byBitMask;
-    pbyData[1] = byBitMask;
-
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_WRITE_MASK,
-                        (WORD) (MAC_REG_MAR0 + uByteIdx),
-                        MESSAGE_REQUEST_MACREG,
-                        2,
-                        pbyData);
-}
-
-
 
 /*
  * Description:
@@ -117,13 +62,12 @@ void MACvWriteMultiAddr(struct vnt_private *pDevice, u32 uByteIdx, u8 byData)
     byData1 = byData;
     CONTROLnsRequestOut(pDevice,
                         MESSAGE_TYPE_WRITE,
-                        (WORD) (MAC_REG_MAR0 + uByteIdx),
+                        (u16) (MAC_REG_MAR0 + uByteIdx),
                         MESSAGE_REQUEST_MACREG,
                         1,
                         &byData1);
 }
 
-
 /*
  * Description:
  *      Shut Down MAC
@@ -150,7 +94,6 @@ void MACvSetBBType(struct vnt_private *pDevice, u8 byType)
 {
 	u8 pbyData[2];
 
-
     pbyData[0] = byType;
     pbyData[1] = EnCFG_BBType_MASK;
 
@@ -163,26 +106,6 @@ void MACvSetBBType(struct vnt_private *pDevice, u8 byType)
                         );
 }
 
-void MACvSetMISCFifo(struct vnt_private *pDevice, u16 wOffset, u32 dwData)
-{
-	u8 pbyData[4];
-
-    if (wOffset > 273)
-        return;
-    pbyData[0] = (BYTE)dwData;
-    pbyData[1] = (BYTE)(dwData>>8);
-    pbyData[2] = (BYTE)(dwData>>16);
-    pbyData[3] = (BYTE)(dwData>>24);
-
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_WRITE_MISCFF,
-                        wOffset,
-                        0,
-                        4,
-                        pbyData
-                        );
-}
-
 /*
  * Description:
  *      Disable the Key Entry by MISCFIFO
@@ -202,8 +125,7 @@ void MACvDisableKeyEntry(struct vnt_private *pDevice, u32 uEntryIdx)
 	u16 wOffset;
 	u8 byData;
 
-
-    byData = (BYTE) uEntryIdx;
+    byData = (u8) uEntryIdx;
 
     wOffset = MISCFIFO_KEYETRY0;
     wOffset += (uEntryIdx * MISCFIFO_KEYENTRYSIZE);
@@ -222,7 +144,6 @@ void MACvDisableKeyEntry(struct vnt_private *pDevice, u32 uEntryIdx)
                         );
 }
 
-
 /*
  * Description:
  *      Set the Key by MISCFIFO
@@ -294,31 +215,29 @@ void MACvSetKeyEntry(struct vnt_private *pDevice, u16 wKeyCtl, u32 uEntryIdx,
         VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
     }
 */
-    pbyKey = (PBYTE)pdwKey;
-
-    pbyData[0] = (BYTE)dwData1;
-    pbyData[1] = (BYTE)(dwData1>>8);
-    pbyData[2] = (BYTE)(dwData1>>16);
-    pbyData[3] = (BYTE)(dwData1>>24);
-    pbyData[4] = (BYTE)dwData2;
-    pbyData[5] = (BYTE)(dwData2>>8);
-    pbyData[6] = (BYTE)(dwData2>>16);
-    pbyData[7] = (BYTE)(dwData2>>24);
+    pbyKey = (u8 *)pdwKey;
+
+    pbyData[0] = (u8)dwData1;
+    pbyData[1] = (u8)(dwData1>>8);
+    pbyData[2] = (u8)(dwData1>>16);
+    pbyData[3] = (u8)(dwData1>>24);
+    pbyData[4] = (u8)dwData2;
+    pbyData[5] = (u8)(dwData2>>8);
+    pbyData[6] = (u8)(dwData2>>16);
+    pbyData[7] = (u8)(dwData2>>24);
     for (ii = 8; ii < 24; ii++)
 	pbyData[ii] = *pbyKey++;
 
     CONTROLnsRequestOut(pDevice,
                         MESSAGE_TYPE_SETKEY,
                         wOffset,
-                        (WORD)uKeyIdx,
+                        (u16)uKeyIdx,
                         24,
                         pbyData
                         );
 
-
 }
 
-
 void MACvRegBitsOff(struct vnt_private *pDevice, u8 byRegOfs, u8 byBits)
 {
 	u8 pbyData[2];
@@ -335,12 +254,10 @@ void MACvRegBitsOff(struct vnt_private *pDevice, u8 byRegOfs, u8 byBits)
                         );
 }
 
-
 void MACvRegBitsOn(struct vnt_private *pDevice, u8 byRegOfs, u8 byBits)
 {
 	u8 pbyData[2];
 
-
     pbyData[0] = byBits;
     pbyData[1] = byBits;
 
@@ -357,9 +274,8 @@ void MACvWriteWord(struct vnt_private *pDevice, u8 byRegOfs, u16 wData)
 {
 	u8 pbyData[2];
 
-
-    pbyData[0] = (BYTE)(wData & 0xff);
-    pbyData[1] = (BYTE)(wData >> 8);
+    pbyData[0] = (u8)(wData & 0xff);
+    pbyData[1] = (u8)(wData >> 8);
 
     CONTROLnsRequestOut(pDevice,
                         MESSAGE_TYPE_WRITE,
@@ -375,13 +291,12 @@ void MACvWriteBSSIDAddress(struct vnt_private *pDevice, u8 *pbyEtherAddr)
 {
 	u8 pbyData[6];
 
-
-    pbyData[0] = *((PBYTE)pbyEtherAddr);
-    pbyData[1] = *((PBYTE)pbyEtherAddr+1);
-    pbyData[2] = *((PBYTE)pbyEtherAddr+2);
-    pbyData[3] = *((PBYTE)pbyEtherAddr+3);
-    pbyData[4] = *((PBYTE)pbyEtherAddr+4);
-    pbyData[5] = *((PBYTE)pbyEtherAddr+5);
+    pbyData[0] = *((u8 *)pbyEtherAddr);
+    pbyData[1] = *((u8 *)pbyEtherAddr+1);
+    pbyData[2] = *((u8 *)pbyEtherAddr+2);
+    pbyData[3] = *((u8 *)pbyEtherAddr+3);
+    pbyData[4] = *((u8 *)pbyEtherAddr+4);
+    pbyData[5] = *((u8 *)pbyEtherAddr+5);
 
     CONTROLnsRequestOut(pDevice,
                         MESSAGE_TYPE_WRITE,
@@ -396,7 +311,6 @@ void MACvEnableProtectMD(struct vnt_private *pDevice)
 {
 	u8 pbyData[2];
 
-
     pbyData[0] = EnCFG_ProtectMd;
     pbyData[1] = EnCFG_ProtectMd;
 
@@ -413,7 +327,6 @@ void MACvDisableProtectMD(struct vnt_private *pDevice)
 {
 	u8 pbyData[2];
 
-
     pbyData[0] = 0;
     pbyData[1] = EnCFG_ProtectMd;
 
@@ -430,7 +343,6 @@ void MACvEnableBarkerPreambleMd(struct vnt_private *pDevice)
 {
 	u8 pbyData[2];
 
-
     pbyData[0] = EnCFG_BarkerPream;
     pbyData[1] = EnCFG_BarkerPream;
 
@@ -447,7 +359,6 @@ void MACvDisableBarkerPreambleMd(struct vnt_private *pDevice)
 {
 	u8 pbyData[2];
 
-
     pbyData[0] = 0;
     pbyData[1] = EnCFG_BarkerPream;
 
@@ -460,7 +371,6 @@ void MACvDisableBarkerPreambleMd(struct vnt_private *pDevice)
                         );
 }
 
-
 void MACvWriteBeaconInterval(struct vnt_private *pDevice, u16 wInterval)
 {
 	u8 pbyData[2];
diff --git a/drivers/staging/vt6656/mac.h b/drivers/staging/vt6656/mac.h
index 6e28500ae5f..0db1be5b01c 100644
--- a/drivers/staging/vt6656/mac.h
+++ b/drivers/staging/vt6656/mac.h
@@ -34,12 +34,9 @@
 #ifndef __MAC_H__
 #define __MAC_H__
 
-#include "ttype.h"
 #include "device.h"
 #include "tmacro.h"
 
-/*---------------------  Export Definitions -------------------------*/
-
 #define REV_ID_VT3253_A0    0x00
 #define REV_ID_VT3253_A1    0x01
 #define REV_ID_VT3253_B0    0x08
@@ -156,7 +153,6 @@
 #define MAC_REG_RSPINF_A_54 0xFA
 #define MAC_REG_RSPINF_A_72 0xFC
 
-
 //
 // Bits in the I2MCFG EEPROM register
 //
@@ -281,7 +277,6 @@
 #define TCR_SYNCDCFOPT      0x02        //
 #define TCR_AUTOBCNTX       0x01        // Beacon automatically transmit enable
 
-
 //ISR1
 #define ISR_GPIO3           0x40
 #define ISR_RXNOBUF         0x08
@@ -377,7 +372,6 @@
 //
 #define MISCFFCTL_WRITE     0x0001      //
 
-
 // Loopback mode
 #define MAC_LB_EXT          0x02        //
 #define MAC_LB_INTERNAL     0x01        //
@@ -409,22 +403,9 @@
 #define MAC_REVISION_A0     0x00
 #define MAC_REVISION_A1     0x01
 
-
-/*---------------------  Export Types  ------------------------------*/
-
-/*---------------------  Export Macros ------------------------------*/
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-void MACvSetMultiAddrByHash(struct vnt_private *, u8);
 void MACvWriteMultiAddr(struct vnt_private *, u32, u8);
 void MACbShutdown(struct vnt_private *);
 void MACvSetBBType(struct vnt_private *, u8);
-void MACvSetMISCFifo(struct vnt_private *pDevice, u16, u32);
 void MACvDisableKeyEntry(struct vnt_private *, u32);
 void MACvSetKeyEntry(struct vnt_private *, u16, u32, u32, u8 *, u32 *);
 void MACvRegBitsOff(struct vnt_private *, u8, u8);
diff --git a/drivers/staging/vt6656/main_usb.c b/drivers/staging/vt6656/main_usb.c
index a5063a6f64d..2161af83eaa 100644
--- a/drivers/staging/vt6656/main_usb.c
+++ b/drivers/staging/vt6656/main_usb.c
@@ -72,7 +72,6 @@
 #include "int.h"
 #include "iowpa.h"
 
-/*---------------------  Static Definitions -------------------------*/
 /* static int msglevel = MSG_LEVEL_DEBUG; */
 static int          msglevel                =MSG_LEVEL_INFO;
 
@@ -95,14 +94,12 @@ MODULE_DESCRIPTION(DEVICE_FULL_DRV_NAM);
 #define RX_DESC_DEF0     64
 DEVICE_PARAM(RxDescriptors0,"Number of receive usb desc buffer");
 
-
 #define TX_DESC_DEF0     64
 DEVICE_PARAM(TxDescriptors0,"Number of transmit usb desc buffer");
 
 #define CHANNEL_DEF     6
 DEVICE_PARAM(Channel, "Channel number");
 
-
 /* PreambleType[] is the preamble length used for transmit.
    0: indicate allows long preamble type
    1: indicate allows short preamble type
@@ -118,7 +115,6 @@ DEVICE_PARAM(RTSThreshold, "RTS threshold");
 #define FRAG_THRESH_DEF     2346
 DEVICE_PARAM(FragThreshold, "Fragmentation threshold");
 
-
 #define DATA_RATE_DEF     13
 /* datarate[] index
    0: indicate 1 Mbps   0x02
@@ -148,7 +144,6 @@ DEVICE_PARAM(OPMode, "Infrastruct, adhoc, AP mode ");
    2: indicate AP mode used
 */
 
-
 /* PSMode[]
    0: indicate disable power saving mode
    1: indicate enable power saving mode
@@ -157,7 +152,6 @@ DEVICE_PARAM(OPMode, "Infrastruct, adhoc, AP mode ");
 #define PS_MODE_DEF     0
 DEVICE_PARAM(PSMode, "Power saving mode");
 
-
 #define SHORT_RETRY_DEF     8
 DEVICE_PARAM(ShortRetryLimit, "Short frame retry limits");
 
@@ -173,8 +167,6 @@ DEVICE_PARAM(LongRetryLimit, "long frame retry limits");
 #define BBP_TYPE_DEF     2
 DEVICE_PARAM(BasebandType, "baseband type");
 
-
-
 /* 80211hEnable[]
    0: indicate disable 802.11h
    1: indicate enable 802.11h
@@ -184,7 +176,6 @@ DEVICE_PARAM(BasebandType, "baseband type");
 
 DEVICE_PARAM(b80211hEnable, "802.11h mode");
 
-
 /*
  * Static vars definitions
  */
@@ -204,12 +195,9 @@ static const long frequency_list[] = {
     5700, 5745, 5765, 5785, 5805, 5825
 	};
 
-
 static const struct iw_handler_def	iwctl_handler_def;
 */
 
-/*---------------------  Static Functions  --------------------------*/
-
 static int vt6656_probe(struct usb_interface *intf,
 			const struct usb_device_id *id);
 static void vt6656_disconnect(struct usb_interface *intf);
@@ -245,21 +233,13 @@ static int Config_FileGetParameter(unsigned char *string,
 				   unsigned char *dest,
 				   unsigned char *source);
 
-
 static void usb_device_reset(struct vnt_private *pDevice);
 
-
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-
 static void
 device_set_options(struct vnt_private *pDevice) {
 
-    BYTE    abyBroadcastAddr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
-    BYTE    abySNAP_RFC1042[ETH_ALEN] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00};
+    u8    abyBroadcastAddr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+    u8    abySNAP_RFC1042[ETH_ALEN] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00};
     u8 abySNAP_Bridgetunnel[ETH_ALEN] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0xF8};
 
     memcpy(pDevice->abyBroadcastAddr, abyBroadcastAddr, ETH_ALEN);
@@ -293,7 +273,6 @@ device_set_options(struct vnt_private *pDevice) {
     pDevice->bDiversityRegCtlON = false;
 }
 
-
 static void device_init_diversity_timer(struct vnt_private *pDevice)
 {
     init_timer(&pDevice->TimerSQ3Tmax1);
@@ -314,7 +293,6 @@ static void device_init_diversity_timer(struct vnt_private *pDevice)
     return;
 }
 
-
 /*
  * initialization of MAC & BBP registers
  */
@@ -366,8 +344,8 @@ static int device_init_registers(struct vnt_private *pDevice,
         }
     }
 
-    sInitCmd.byInitClass = (BYTE)InitType;
-    sInitCmd.bExistSWNetAddr = (BYTE) pDevice->bExistSWNetAddr;
+    sInitCmd.byInitClass = (u8)InitType;
+    sInitCmd.bExistSWNetAddr = (u8) pDevice->bExistSWNetAddr;
     for (ii = 0; ii < 6; ii++)
 	sInitCmd.bySWNetAddr[ii] = pDevice->abyCurrentNetAddr[ii];
     sInitCmd.byShortRetryLimit = pDevice->byShortRetryLimit;
@@ -379,7 +357,7 @@ static int device_init_registers(struct vnt_private *pDevice,
                                     0,
                                     0,
                                     sizeof(CMD_CARD_INIT),
-                                    (PBYTE) &(sInitCmd));
+                                    (u8 *) &(sInitCmd));
 
     if ( ntStatus != STATUS_SUCCESS ) {
         DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" Issue Card init fail \n");
@@ -388,7 +366,7 @@ static int device_init_registers(struct vnt_private *pDevice,
     }
     if (InitType == DEVICE_INIT_COLD) {
 
-        ntStatus = CONTROLnsRequestIn(pDevice,MESSAGE_TYPE_INIT_RSP,0,0,sizeof(RSP_CARD_INIT), (PBYTE) &(sInitRsp));
+        ntStatus = CONTROLnsRequestIn(pDevice,MESSAGE_TYPE_INIT_RSP,0,0,sizeof(RSP_CARD_INIT), (u8 *) &(sInitRsp));
 
         if (ntStatus != STATUS_SUCCESS) {
             DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Cardinit request in status fail!\n");
@@ -418,7 +396,7 @@ static int device_init_registers(struct vnt_private *pDevice,
         pDevice->bNonERPPresent = false;
         pDevice->bBarkerPreambleMd = false;
         if ( pDevice->bFixRate ) {
-            pDevice->wCurrentRate = (WORD) pDevice->uConnectionRate;
+            pDevice->wCurrentRate = (u16) pDevice->uConnectionRate;
         } else {
             if ( pDevice->byBBType == BB_TYPE_11B )
                 pDevice->wCurrentRate = RATE_11M;
@@ -771,7 +749,6 @@ static void device_free_tx_bufs(struct vnt_private *pDevice)
     return;
 }
 
-
 static void device_free_rx_bufs(struct vnt_private *pDevice)
 {
     PRCB pRCB;
@@ -809,7 +786,6 @@ static void device_free_int_bufs(struct vnt_private *pDevice)
     return;
 }
 
-
 static bool device_alloc_bufs(struct vnt_private *pDevice)
 {
 
@@ -817,7 +793,6 @@ static bool device_alloc_bufs(struct vnt_private *pDevice)
     PRCB pRCB;
     int ii;
 
-
     for (ii = 0; ii < pDevice->cbTD; ii++) {
 
         pTxContext = kmalloc(sizeof(USB_SEND_CONTEXT), GFP_KERNEL);
@@ -843,7 +818,6 @@ static bool device_alloc_bufs(struct vnt_private *pDevice)
         goto free_tx;
     }
 
-
     pDevice->FirstRecvFreeList = NULL;
     pDevice->LastRecvFreeList = NULL;
     pDevice->FirstRecvMngList = NULL;
@@ -874,7 +848,6 @@ static bool device_alloc_bufs(struct vnt_private *pDevice)
         pRCB++;
     }
 
-
 	pDevice->pControlURB = usb_alloc_urb(0, GFP_ATOMIC);
 	if (pDevice->pControlURB == NULL) {
 	    DBG_PRT(MSG_LEVEL_ERR,KERN_ERR"Failed to alloc control urb\n");
@@ -907,9 +880,6 @@ free_tx:
 	return false;
 }
 
-
-
-
 static bool device_init_defrag_cb(struct vnt_private *pDevice)
 {
 	int i;
@@ -933,8 +903,6 @@ free_frag:
     return false;
 }
 
-
-
 static void device_free_frag_bufs(struct vnt_private *pDevice)
 {
 	PSDeFragControlBlock pDeF;
@@ -949,8 +917,6 @@ static void device_free_frag_bufs(struct vnt_private *pDevice)
     }
 }
 
-
-
 int device_alloc_frag_buf(struct vnt_private *pDevice,
 		PSDeFragControlBlock pDeF)
 {
@@ -964,9 +930,6 @@ int device_alloc_frag_buf(struct vnt_private *pDevice,
     return true;
 }
 
-
-/*-----------------------------------------------------------------*/
-
 static int  device_open(struct net_device *dev)
 {
 	struct vnt_private *pDevice = netdev_priv(dev);
@@ -975,7 +938,6 @@ static int  device_open(struct net_device *dev)
 
     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " device_open...\n");
 
-
     pDevice->rx_buf_sz = MAX_TOTAL_SIZE_WITH_ALL_HEADERS;
 
     if (device_alloc_bufs(pDevice) == false) {
@@ -1061,7 +1023,6 @@ static int  device_open(struct net_device *dev)
 	else
 		bScheduleCommand((void *) pDevice, WLAN_CMD_BSSID_SCAN, NULL);
 
-
     netif_stop_queue(pDevice->dev);
     pDevice->flags |= DEVICE_FLAGS_OPENED;
 
@@ -1083,8 +1044,6 @@ free_rx_tx:
     return -ENOMEM;
 }
 
-
-
 static int device_close(struct net_device *dev)
 {
 	struct vnt_private *pDevice = netdev_priv(dev);
@@ -1100,7 +1059,6 @@ static int device_close(struct net_device *dev)
         mdelay(30);
     }
 
-
         memset(pMgmt->abyDesireSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
         pMgmt->bShareKeyAlgorithm = false;
         pDevice->bEncryptionEnable = false;
@@ -1165,7 +1123,6 @@ static void vt6656_disconnect(struct usb_interface *intf)
 	if (!device)
 		return;
 
-
 	usb_set_intfdata(intf, NULL);
 	usb_put_dev(interface_to_usbdev(intf));
 
@@ -1430,7 +1387,6 @@ static void device_set_multi(struct net_device *dev)
 	u8 byTmpMode = 0;
 	int rc;
 
-
 	spin_lock_irq(&pDevice->lock);
     rc = CONTROLnsRequestIn(pDevice,
                             MESSAGE_TYPE_READ,
@@ -1466,8 +1422,8 @@ static void device_set_multi(struct net_device *dev)
             mc_filter[bit_nr >> 5] |= cpu_to_le32(1 << (bit_nr & 31));
         }
         for (ii = 0; ii < 4; ii++) {
-             MACvWriteMultiAddr(pDevice, ii, *((PBYTE)&mc_filter[0] + ii));
-             MACvWriteMultiAddr(pDevice, ii+ 4, *((PBYTE)&mc_filter[1] + ii));
+             MACvWriteMultiAddr(pDevice, ii, *((u8 *)&mc_filter[0] + ii));
+             MACvWriteMultiAddr(pDevice, ii+ 4, *((u8 *)&mc_filter[1] + ii));
         }
         pDevice->byRxMode &= ~(RCR_UNICAST);
         pDevice->byRxMode |= (RCR_MULTICAST|RCR_BROADCAST);
@@ -1518,7 +1474,6 @@ static int device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 	return rc;
 }
 
-
 static int ethtool_ioctl(struct net_device *dev, void *useraddr)
 {
 	u32 ethcmd;
diff --git a/drivers/staging/vt6656/mib.c b/drivers/staging/vt6656/mib.c
index d4c7b0cc7ec..12333cdcbc6 100644
--- a/drivers/staging/vt6656/mib.c
+++ b/drivers/staging/vt6656/mib.c
@@ -25,10 +25,8 @@
  * Date: May 21, 1996
  *
  * Functions:
- *      STAvClearAllCounter - Clear All MIB Counter
  *      STAvUpdateIstStatCounter - Update ISR statistic counter
  *      STAvUpdateRDStatCounter - Update Rx statistic counter
- *      STAvUpdateRDStatCounterEx - Update Rx statistic counter and copy rcv data
  *      STAvUpdateTDStatCounter - Update Tx statistic counter
  *      STAvUpdateTDStatCounterEx - Update Tx statistic counter and copy tx data
  *      STAvUpdate802_11Counter - Update 802.11 mib counter
@@ -43,38 +41,7 @@
 #include "wctl.h"
 #include "baseband.h"
 
-/*---------------------  Static Definitions -------------------------*/
 static int          msglevel                =MSG_LEVEL_INFO;
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-
-
-/*
- * Description: Clear All Statistic Counter
- *
- * Parameters:
- *  In:
- *      pStatistic  - Pointer to Statistic Counter Data Structure
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void STAvClearAllCounter (PSStatCounter pStatistic)
-{
-    // set memory to zero
-	memset(pStatistic, 0, sizeof(SStatCounter));
-}
-
 
 /*
  * Description: Update Isr Statistic Counter
@@ -89,7 +56,7 @@ void STAvClearAllCounter (PSStatCounter pStatistic)
  * Return Value: none
  *
  */
-void STAvUpdateIsrStatCounter (PSStatCounter pStatistic, BYTE byIsr0, BYTE byIsr1)
+void STAvUpdateIsrStatCounter (PSStatCounter pStatistic, u8 byIsr0, u8 byIsr1)
 {
     /**********************/
     /* ABNORMAL interrupt */
@@ -100,7 +67,6 @@ void STAvUpdateIsrStatCounter (PSStatCounter pStatistic, BYTE byIsr0, BYTE byIsr
         return;
     }
 
-
     if (byIsr0 & ISR_ACTX)              // ISR, bit0
         pStatistic->ISRStat.dwIsrTx0OK++;           // TXDMA0 successful
 
@@ -119,7 +85,6 @@ void STAvUpdateIsrStatCounter (PSStatCounter pStatistic, BYTE byIsr0, BYTE byIsr
     if (byIsr0 & ISR_WATCHDOG)          // ISR, bit7
         pStatistic->ISRStat.dwIsrWatchDog++;
 
-
     if (byIsr1 & ISR_FETALERR)              // ISR, bit8
         pStatistic->ISRStat.dwIsrUnrecoverableError++;
 
@@ -134,7 +99,6 @@ void STAvUpdateIsrStatCounter (PSStatCounter pStatistic, BYTE byIsr0, BYTE byIsr
 
 }
 
-
 /*
  * Description: Update Rx Statistic Counter
  *
@@ -152,12 +116,12 @@ void STAvUpdateIsrStatCounter (PSStatCounter pStatistic, BYTE byIsr0, BYTE byIsr
  *
  */
 void STAvUpdateRDStatCounter(PSStatCounter pStatistic,
-			     BYTE byRSR, BYTE byNewRSR,
-			     BYTE byRxSts, BYTE byRxRate,
-			     PBYTE pbyBuffer, unsigned int cbFrameLength)
+			     u8 byRSR, u8 byNewRSR,
+			     u8 byRxSts, u8 byRxRate,
+			     u8 * pbyBuffer, unsigned int cbFrameLength)
 {
 	/* need change */
-	PS802_11Header pHeader = (PS802_11Header)pbyBuffer;
+	struct ieee80211_hdr *pHeader = (struct ieee80211_hdr *)pbyBuffer;
 
 	if (byRSR & RSR_ADDROK)
 		pStatistic->dwRsrADDROk++;
@@ -327,7 +291,6 @@ void STAvUpdateRDStatCounter(PSStatCounter pStatistic,
     pStatistic->dwRsrRxPacket++;
     pStatistic->dwRsrRxOctet += cbFrameLength;
 
-
     if (IS_TYPE_DATA(pbyBuffer)) {
         pStatistic->dwRsrRxData++;
     } else if (IS_TYPE_MGMT(pbyBuffer)){
@@ -343,7 +306,7 @@ void STAvUpdateRDStatCounter(PSStatCounter pStatistic,
     else
         pStatistic->dwRsrDirected++;
 
-    if (WLAN_GET_FC_MOREFRAG(pHeader->wFrameCtl))
+    if (WLAN_GET_FC_MOREFRAG(pHeader->frame_control))
         pStatistic->dwRsrRxFragment++;
 
     if (cbFrameLength < ETH_ZLEN + 4) {
@@ -371,51 +334,6 @@ void STAvUpdateRDStatCounter(PSStatCounter pStatistic,
 }
 
 /*
- * Description: Update Rx Statistic Counter and copy Rx buffer
- *
- * Parameters:
- *  In:
- *      pStatistic      - Pointer to Statistic Counter Data Structure
- *      byRSR           - Rx Status
- *      byNewRSR        - Rx Status
- *      pbyBuffer       - Rx Buffer
- *      cbFrameLength   - Rx Length
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-
-void
-STAvUpdateRDStatCounterEx (
-    PSStatCounter   pStatistic,
-    BYTE            byRSR,
-    BYTE            byNewRSR,
-    BYTE            byRxSts,
-    BYTE            byRxRate,
-    PBYTE           pbyBuffer,
-    unsigned int            cbFrameLength
-    )
-{
-    STAvUpdateRDStatCounter(
-                    pStatistic,
-                    byRSR,
-                    byNewRSR,
-                    byRxSts,
-                    byRxRate,
-                    pbyBuffer,
-                    cbFrameLength
-                    );
-
-    // rx length
-    pStatistic->dwCntRxFrmLength = cbFrameLength;
-    // rx pattern, we just see 10 bytes for sample
-    memcpy(pStatistic->abyCntRxPattern, (PBYTE)pbyBuffer, 10);
-}
-
-
-/*
  * Description: Update Tx Statistic Counter
  *
  * Parameters:
@@ -435,12 +353,12 @@ STAvUpdateRDStatCounterEx (
 void
 STAvUpdateTDStatCounter (
     PSStatCounter   pStatistic,
-    BYTE            byPktNum,
-    BYTE            byRate,
-    BYTE            byTSR
+    u8            byPktNum,
+    u8            byRate,
+    u8            byTSR
     )
 {
-    BYTE    byRetyCnt;
+    u8    byRetyCnt;
     // increase tx packet count
     pStatistic->dwTsrTxPacket++;
 
@@ -504,8 +422,6 @@ STAvUpdateTDStatCounter (
     }
 }
 
-
-
 /*
  * Description: Update 802.11 mib counter
  *
@@ -524,10 +440,10 @@ void
 STAvUpdate802_11Counter(
     PSDot11Counters         p802_11Counter,
     PSStatCounter           pStatistic,
-    BYTE                    byRTSSuccess,
-    BYTE                    byRTSFail,
-    BYTE                    byACKFail,
-    BYTE                    byFCSErr
+    u8                    byRTSSuccess,
+    u8                    byRTSFail,
+    u8                    byACKFail,
+    u8                    byFCSErr
     )
 {
     //p802_11Counter->TransmittedFragmentCount
@@ -554,25 +470,6 @@ STAvUpdate802_11Counter(
  *
  * Parameters:
  *  In:
- *      p802_11Counter  - Pointer to 802.11 mib counter
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void
-STAvClear802_11Counter(PSDot11Counters p802_11Counter)
-{
-    // set memory to zero
-	memset(p802_11Counter, 0, sizeof(SDot11Counters));
-}
-
-/*
- * Description: Clear 802.11 mib counter
- *
- * Parameters:
- *  In:
  *      pUsbCounter  - Pointer to USB mib counter
  *      ntStatus - URB status
  *  Out:
diff --git a/drivers/staging/vt6656/mib.h b/drivers/staging/vt6656/mib.h
index 85c28e92366..35375325a77 100644
--- a/drivers/staging/vt6656/mib.h
+++ b/drivers/staging/vt6656/mib.h
@@ -29,28 +29,21 @@
 #ifndef __MIB_H__
 #define __MIB_H__
 
-#include "ttype.h"
 #include "tether.h"
 #include "desc.h"
 
-/*---------------------  Export Definitions -------------------------*/
-
-
 //
 // USB counter
 //
 typedef struct tagSUSBCounter {
-    DWORD dwCrc;
+    u32 dwCrc;
 
 } SUSBCounter, *PSUSBCounter;
 
-
-
 //
 // 802.11 counter
 //
 
-
 typedef struct tagSDot11Counters {
   /* unsigned long Length; // Length of structure */
     unsigned long long   TransmittedFragmentCount;
@@ -81,7 +74,6 @@ typedef struct tagSDot11Counters {
    */
 } SDot11Counters, *PSDot11Counters;
 
-
 //
 // MIB2 counter
 //
@@ -91,24 +83,24 @@ typedef struct tagSMib2Counter {
                                         // e.g. "interface 1"
     signed long    ifType;
     signed long    ifMtu;
-    DWORD   ifSpeed;
-    BYTE    ifPhysAddress[ETH_ALEN];
+    u32   ifSpeed;
+    u8    ifPhysAddress[ETH_ALEN];
     signed long    ifAdminStatus;
     signed long    ifOperStatus;
-    DWORD   ifLastChange;
-    DWORD   ifInOctets;
-    DWORD   ifInUcastPkts;
-    DWORD   ifInNUcastPkts;
-    DWORD   ifInDiscards;
-    DWORD   ifInErrors;
-    DWORD   ifInUnknownProtos;
-    DWORD   ifOutOctets;
-    DWORD   ifOutUcastPkts;
-    DWORD   ifOutNUcastPkts;
-    DWORD   ifOutDiscards;
-    DWORD   ifOutErrors;
-    DWORD   ifOutQLen;
-    DWORD   ifSpecific;
+    u32   ifLastChange;
+    u32   ifInOctets;
+    u32   ifInUcastPkts;
+    u32   ifInNUcastPkts;
+    u32   ifInDiscards;
+    u32   ifInErrors;
+    u32   ifInUnknownProtos;
+    u32   ifOutOctets;
+    u32   ifOutUcastPkts;
+    u32   ifOutNUcastPkts;
+    u32   ifOutDiscards;
+    u32   ifOutErrors;
+    u32   ifOutQLen;
+    u32   ifSpecific;
 } SMib2Counter, *PSMib2Counter;
 
 // Value in the ifType entry
@@ -119,32 +111,31 @@ typedef struct tagSMib2Counter {
 #define DOWN                2           //
 #define TESTING             3           //
 
-
 //
 // RMON counter
 //
 typedef struct tagSRmonCounter {
     signed long    etherStatsIndex;
-    DWORD   etherStatsDataSource;
-    DWORD   etherStatsDropEvents;
-    DWORD   etherStatsOctets;
-    DWORD   etherStatsPkts;
-    DWORD   etherStatsBroadcastPkts;
-    DWORD   etherStatsMulticastPkts;
-    DWORD   etherStatsCRCAlignErrors;
-    DWORD   etherStatsUndersizePkts;
-    DWORD   etherStatsOversizePkts;
-    DWORD   etherStatsFragments;
-    DWORD   etherStatsJabbers;
-    DWORD   etherStatsCollisions;
-    DWORD   etherStatsPkt64Octets;
-    DWORD   etherStatsPkt65to127Octets;
-    DWORD   etherStatsPkt128to255Octets;
-    DWORD   etherStatsPkt256to511Octets;
-    DWORD   etherStatsPkt512to1023Octets;
-    DWORD   etherStatsPkt1024to1518Octets;
-    DWORD   etherStatsOwners;
-    DWORD   etherStatsStatus;
+    u32   etherStatsDataSource;
+    u32   etherStatsDropEvents;
+    u32   etherStatsOctets;
+    u32   etherStatsPkts;
+    u32   etherStatsBroadcastPkts;
+    u32   etherStatsMulticastPkts;
+    u32   etherStatsCRCAlignErrors;
+    u32   etherStatsUndersizePkts;
+    u32   etherStatsOversizePkts;
+    u32   etherStatsFragments;
+    u32   etherStatsJabbers;
+    u32   etherStatsCollisions;
+    u32   etherStatsPkt64Octets;
+    u32   etherStatsPkt65to127Octets;
+    u32   etherStatsPkt128to255Octets;
+    u32   etherStatsPkt256to511Octets;
+    u32   etherStatsPkt512to1023Octets;
+    u32   etherStatsPkt1024to1518Octets;
+    u32   etherStatsOwners;
+    u32   etherStatsStatus;
 } SRmonCounter, *PSRmonCounter;
 
 //
@@ -185,56 +176,52 @@ typedef struct tagSCustomCounters {
 
 } SCustomCounters, *PSCustomCounters;
 
-
 //
 // Custom counter
 //
 typedef struct tagSISRCounters {
     unsigned long   Length;
 
-    DWORD   dwIsrTx0OK;
-    DWORD   dwIsrAC0TxOK;
-    DWORD   dwIsrBeaconTxOK;
-    DWORD   dwIsrRx0OK;
-    DWORD   dwIsrTBTTInt;
-    DWORD   dwIsrSTIMERInt;
-    DWORD   dwIsrWatchDog;
-    DWORD   dwIsrUnrecoverableError;
-    DWORD   dwIsrSoftInterrupt;
-    DWORD   dwIsrMIBNearfull;
-    DWORD   dwIsrRxNoBuf;
-
-    DWORD   dwIsrUnknown;               // unknown interrupt count
-
-    DWORD   dwIsrRx1OK;
-    DWORD   dwIsrATIMTxOK;
-    DWORD   dwIsrSYNCTxOK;
-    DWORD   dwIsrCFPEnd;
-    DWORD   dwIsrATIMEnd;
-    DWORD   dwIsrSYNCFlushOK;
-    DWORD   dwIsrSTIMER1Int;
+    u32   dwIsrTx0OK;
+    u32   dwIsrAC0TxOK;
+    u32   dwIsrBeaconTxOK;
+    u32   dwIsrRx0OK;
+    u32   dwIsrTBTTInt;
+    u32   dwIsrSTIMERInt;
+    u32   dwIsrWatchDog;
+    u32   dwIsrUnrecoverableError;
+    u32   dwIsrSoftInterrupt;
+    u32   dwIsrMIBNearfull;
+    u32   dwIsrRxNoBuf;
+
+    u32   dwIsrUnknown;               // unknown interrupt count
+
+    u32   dwIsrRx1OK;
+    u32   dwIsrATIMTxOK;
+    u32   dwIsrSYNCTxOK;
+    u32   dwIsrCFPEnd;
+    u32   dwIsrATIMEnd;
+    u32   dwIsrSYNCFlushOK;
+    u32   dwIsrSTIMER1Int;
     /////////////////////////////////////
 } SISRCounters, *PSISRCounters;
 
-
 // Value in the etherStatsStatus entry
 #define VALID               1           //
 #define CREATE_REQUEST      2           //
 #define UNDER_CREATION      3           //
 #define INVALID             4           //
 
-
 //
 // Tx packet information
 //
 typedef struct tagSTxPktInfo {
-    BYTE    byBroadMultiUni;
-    WORD    wLength;
-    WORD    wFIFOCtl;
-    BYTE    abyDestAddr[ETH_ALEN];
+    u8    byBroadMultiUni;
+    u16    wLength;
+    u16    wFIFOCtl;
+    u8    abyDestAddr[ETH_ALEN];
 } STxPktInfo, *PSTxPktInfo;
 
-
 #define MAX_RATE            12
 //
 // statistic counter
@@ -248,34 +235,34 @@ typedef struct tagSStatCounter {
 
     // RSR status count
     //
-    DWORD   dwRsrFrmAlgnErr;
-    DWORD   dwRsrErr;
-    DWORD   dwRsrCRCErr;
-    DWORD   dwRsrCRCOk;
-    DWORD   dwRsrBSSIDOk;
-    DWORD   dwRsrADDROk;
-    DWORD   dwRsrBCNSSIDOk;
-    DWORD   dwRsrLENErr;
-    DWORD   dwRsrTYPErr;
-
-    DWORD   dwNewRsrDECRYPTOK;
-    DWORD   dwNewRsrCFP;
-    DWORD   dwNewRsrUTSF;
-    DWORD   dwNewRsrHITAID;
-    DWORD   dwNewRsrHITAID0;
-
-    DWORD   dwRsrLong;
-    DWORD   dwRsrRunt;
-
-    DWORD   dwRsrRxControl;
-    DWORD   dwRsrRxData;
-    DWORD   dwRsrRxManage;
-
-    DWORD   dwRsrRxPacket;
-    DWORD   dwRsrRxOctet;
-    DWORD   dwRsrBroadcast;
-    DWORD   dwRsrMulticast;
-    DWORD   dwRsrDirected;
+    u32   dwRsrFrmAlgnErr;
+    u32   dwRsrErr;
+    u32   dwRsrCRCErr;
+    u32   dwRsrCRCOk;
+    u32   dwRsrBSSIDOk;
+    u32   dwRsrADDROk;
+    u32   dwRsrBCNSSIDOk;
+    u32   dwRsrLENErr;
+    u32   dwRsrTYPErr;
+
+    u32   dwNewRsrDECRYPTOK;
+    u32   dwNewRsrCFP;
+    u32   dwNewRsrUTSF;
+    u32   dwNewRsrHITAID;
+    u32   dwNewRsrHITAID0;
+
+    u32   dwRsrLong;
+    u32   dwRsrRunt;
+
+    u32   dwRsrRxControl;
+    u32   dwRsrRxData;
+    u32   dwRsrRxManage;
+
+    u32   dwRsrRxPacket;
+    u32   dwRsrRxOctet;
+    u32   dwRsrBroadcast;
+    u32   dwRsrMulticast;
+    u32   dwRsrDirected;
     // 64-bit OID
     unsigned long long   ullRsrOK;
 
@@ -287,47 +274,44 @@ typedef struct tagSStatCounter {
     unsigned long long   ullRxMulticastFrames;
     unsigned long long   ullRxDirectedFrames;
 
-    DWORD   dwRsrRxFragment;
-    DWORD   dwRsrRxFrmLen64;
-    DWORD   dwRsrRxFrmLen65_127;
-    DWORD   dwRsrRxFrmLen128_255;
-    DWORD   dwRsrRxFrmLen256_511;
-    DWORD   dwRsrRxFrmLen512_1023;
-    DWORD   dwRsrRxFrmLen1024_1518;
+    u32   dwRsrRxFragment;
+    u32   dwRsrRxFrmLen64;
+    u32   dwRsrRxFrmLen65_127;
+    u32   dwRsrRxFrmLen128_255;
+    u32   dwRsrRxFrmLen256_511;
+    u32   dwRsrRxFrmLen512_1023;
+    u32   dwRsrRxFrmLen1024_1518;
 
     // TSR status count
     //
-    DWORD   dwTsrTotalRetry;        // total collision retry count
-    DWORD   dwTsrOnceRetry;         // this packet only occur one collision
-    DWORD   dwTsrMoreThanOnceRetry; // this packet occur more than one collision
-    DWORD   dwTsrRetry;             // this packet has ever occur collision,
+    u32   dwTsrTotalRetry;        // total collision retry count
+    u32   dwTsrOnceRetry;         // this packet only occur one collision
+    u32   dwTsrMoreThanOnceRetry; // this packet occur more than one collision
+    u32   dwTsrRetry;             // this packet has ever occur collision,
                                          // that is (dwTsrOnceCollision0 + dwTsrMoreThanOnceCollision0)
-    DWORD   dwTsrACKData;
-    DWORD   dwTsrErr;
-    DWORD   dwAllTsrOK;
-    DWORD   dwTsrRetryTimeout;
-    DWORD   dwTsrTransmitTimeout;
-
-    DWORD   dwTsrTxPacket;
-    DWORD   dwTsrTxOctet;
-    DWORD   dwTsrBroadcast;
-    DWORD   dwTsrMulticast;
-    DWORD   dwTsrDirected;
+    u32   dwTsrACKData;
+    u32   dwTsrErr;
+    u32   dwAllTsrOK;
+    u32   dwTsrRetryTimeout;
+    u32   dwTsrTransmitTimeout;
+
+    u32   dwTsrTxPacket;
+    u32   dwTsrTxOctet;
+    u32   dwTsrBroadcast;
+    u32   dwTsrMulticast;
+    u32   dwTsrDirected;
 
     // RD/TD count
-    DWORD   dwCntRxFrmLength;
-    DWORD   dwCntTxBufLength;
-
-    BYTE    abyCntRxPattern[16];
-    BYTE    abyCntTxPattern[16];
-
+    u32   dwCntRxFrmLength;
+    u32   dwCntTxBufLength;
 
+    u8    abyCntRxPattern[16];
+    u8    abyCntTxPattern[16];
 
     // Software check....
-    DWORD   dwCntRxDataErr;             // rx buffer data software compare CRC err count
-    DWORD   dwCntDecryptErr;            // rx buffer data software compare CRC err count
-    DWORD   dwCntRxICVErr;              // rx buffer data software compare CRC err count
-
+    u32   dwCntRxDataErr;             // rx buffer data software compare CRC err count
+    u32   dwCntDecryptErr;            // rx buffer data software compare CRC err count
+    u32   dwCntRxICVErr;              // rx buffer data software compare CRC err count
 
     // 64-bit OID
     unsigned long long   ullTsrOK;
@@ -341,9 +325,9 @@ typedef struct tagSStatCounter {
     unsigned long long   ullTxDirectedBytes;
 
     // for autorate
-    DWORD   dwTxOk[MAX_RATE+1];
-    DWORD   dwTxFail[MAX_RATE+1];
-    DWORD   dwTxRetryCount[8];
+    u32   dwTxOk[MAX_RATE+1];
+    u32   dwTxFail[MAX_RATE+1];
+    u32   dwTxRetryCount[8];
 
     STxPktInfo  abyTxPktInfo[16];
 
@@ -367,42 +351,28 @@ typedef struct tagSStatCounter {
 
 } SStatCounter, *PSStatCounter;
 
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-void STAvClearAllCounter(PSStatCounter pStatistic);
-
 void STAvUpdateIsrStatCounter(PSStatCounter pStatistic,
-			      BYTE byIsr0,
-			      BYTE byIsr1);
+			      u8 byIsr0,
+			      u8 byIsr1);
 
 void STAvUpdateRDStatCounter(PSStatCounter pStatistic,
-			     BYTE byRSR, BYTE byNewRSR, BYTE byRxSts,
-			     BYTE byRxRate, PBYTE pbyBuffer,
+			     u8 byRSR, u8 byNewRSR, u8 byRxSts,
+			     u8 byRxRate, u8 * pbyBuffer,
 			     unsigned int cbFrameLength);
 
-void STAvUpdateRDStatCounterEx(PSStatCounter pStatistic,
-			       BYTE byRSR, BYTE byNewRSR, BYTE byRxSts,
-			       BYTE byRxRate, PBYTE pbyBuffer,
-			       unsigned int cbFrameLength);
-
-void STAvUpdateTDStatCounter(PSStatCounter pStatistic, BYTE byPktNum,
-			     BYTE byRate, BYTE byTSR);
+void STAvUpdateTDStatCounter(PSStatCounter pStatistic, u8 byPktNum,
+			     u8 byRate, u8 byTSR);
 
 void
 STAvUpdate802_11Counter(
     PSDot11Counters         p802_11Counter,
     PSStatCounter           pStatistic,
-    BYTE                    byRTSSuccess,
-    BYTE                    byRTSFail,
-    BYTE                    byACKFail,
-    BYTE                    byFCSErr
+    u8                    byRTSSuccess,
+    u8                    byRTSFail,
+    u8                    byACKFail,
+    u8                    byFCSErr
     );
 
-void STAvClear802_11Counter(PSDot11Counters p802_11Counter);
 void STAvUpdateUSBCounter(PSUSBCounter pUsbCounter, int ntStatus);
 
 #endif /* __MIB_H__ */
diff --git a/drivers/staging/vt6656/michael.c b/drivers/staging/vt6656/michael.c
index 4d419814f27..9a5a0b6761e 100644
--- a/drivers/staging/vt6656/michael.c
+++ b/drivers/staging/vt6656/michael.c
@@ -26,8 +26,8 @@
  * Date: Sep 4, 2002
  *
  * Functions:
- *      s_dwGetUINT32 - Convert from BYTE[] to DWORD in a portable way
- *      s_vPutUINT32 - Convert from DWORD to BYTE[] in a portable way
+ *      s_dwGetUINT32 - Convert from u8[] to u32 in a portable way
+ *      s_vPutUINT32 - Convert from u32 to u8[] in a portable way
  *      s_vClear - Reset the state to the empty message.
  *      s_vSetKey - Set the key.
  *      MIC_vInit - Set the key.
@@ -42,49 +42,41 @@
 #include "tmacro.h"
 #include "michael.h"
 
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
 /*
- * static DWORD s_dwGetUINT32(BYTE * p);         Get DWORD from
+ * static u32 s_dwGetUINT32(u8 * p);         Get u32 from
  *							4 bytes LSByte first
- * static void s_vPutUINT32(BYTE* p, DWORD val); Put DWORD into
+ * static void s_vPutUINT32(u8* p, u32 val); Put u32 into
  *							4 bytes LSByte first
  */
 static void s_vClear(void);		/* Clear the internal message,
 					 * resets the object to the
 					 * state just after construction. */
-static void s_vSetKey(DWORD dwK0, DWORD dwK1);
-static void s_vAppendByte(BYTE b);	/* Add a single byte to the internal
+static void s_vSetKey(u32 dwK0, u32 dwK1);
+static void s_vAppendByte(u8 b);	/* Add a single byte to the internal
 					 * message */
 
-/*---------------------  Export Variables  --------------------------*/
-static DWORD  L, R;		/* Current state */
-static DWORD  K0, K1;		/* Key */
-static DWORD  M;		/* Message accumulator (single word) */
+static u32  L, R;		/* Current state */
+static u32  K0, K1;		/* Key */
+static u32  M;		/* Message accumulator (single word) */
 static unsigned int   nBytesInM;	/* # bytes in M */
 
-/*---------------------  Export Functions  --------------------------*/
-
 /*
-static DWORD s_dwGetUINT32 (BYTE * p)
-// Convert from BYTE[] to DWORD in a portable way
+static u32 s_dwGetUINT32 (u8 * p)
+// Convert from u8[] to u32 in a portable way
 {
-	DWORD res = 0;
+	u32 res = 0;
 	unsigned int i;
 	for (i = 0; i < 4; i++)
 		res |= (*p++) << (8*i);
 	return res;
 }
 
-static void s_vPutUINT32(BYTE *p, DWORD val)
-// Convert from DWORD to BYTE[] in a portable way
+static void s_vPutUINT32(u8 *p, u32 val)
+// Convert from u32 to u8[] in a portable way
 {
 	unsigned int i;
 	for (i = 0; i < 4; i++) {
-		*p++ = (BYTE) (val & 0xff);
+		*p++ = (u8) (val & 0xff);
 		val >>= 8;
 	}
 }
@@ -99,7 +91,7 @@ static void s_vClear(void)
 	M = 0;
 }
 
-static void s_vSetKey(DWORD dwK0, DWORD dwK1)
+static void s_vSetKey(u32 dwK0, u32 dwK1)
 {
 	/* Set the key */
 	K0 = dwK0;
@@ -108,7 +100,7 @@ static void s_vSetKey(DWORD dwK0, DWORD dwK1)
 	s_vClear();
 }
 
-static void s_vAppendByte(BYTE b)
+static void s_vAppendByte(u8 b)
 {
 	/* Append the byte to our word-sized buffer */
 	M |= b << (8*nBytesInM);
@@ -130,13 +122,12 @@ static void s_vAppendByte(BYTE b)
 	}
 }
 
-void MIC_vInit(DWORD dwK0, DWORD dwK1)
+void MIC_vInit(u32 dwK0, u32 dwK1)
 {
 	/* Set the key */
 	s_vSetKey(dwK0, dwK1);
 }
 
-
 void MIC_vUnInit(void)
 {
 	/* Wipe the key material */
@@ -148,7 +139,7 @@ void MIC_vUnInit(void)
 	s_vClear();
 }
 
-void MIC_vAppend(PBYTE src, unsigned int nBytes)
+void MIC_vAppend(u8 * src, unsigned int nBytes)
 {
     /* This is simple */
 	while (nBytes > 0) {
@@ -157,7 +148,7 @@ void MIC_vAppend(PBYTE src, unsigned int nBytes)
 	}
 }
 
-void MIC_vGetMIC(PDWORD pdwL, PDWORD pdwR)
+void MIC_vGetMIC(u32 * pdwL, u32 * pdwR)
 {
 	/* Append the minimum padding */
 	s_vAppendByte(0x5a);
diff --git a/drivers/staging/vt6656/michael.h b/drivers/staging/vt6656/michael.h
index 81351f50623..9c69a42640a 100644
--- a/drivers/staging/vt6656/michael.h
+++ b/drivers/staging/vt6656/michael.h
@@ -31,22 +31,18 @@
 #ifndef __MICHAEL_H__
 #define __MICHAEL_H__
 
-/*---------------------  Export Definitions -------------------------*/
+#include <linux/types.h>
 
-/*---------------------  Export Types  ------------------------------*/
-
-void MIC_vInit(DWORD dwK0, DWORD dwK1);
+void MIC_vInit(u32 dwK0, u32 dwK1);
 
 void MIC_vUnInit(void);
 
 // Append bytes to the message to be MICed
-void MIC_vAppend(PBYTE src, unsigned int nBytes);
+void MIC_vAppend(u8 * src, unsigned int nBytes);
 
 // Get the MIC result. Destination should accept 8 bytes of result.
 // This also resets the message to empty.
-void MIC_vGetMIC(PDWORD pdwL, PDWORD pdwR);
-
-/*---------------------  Export Macros ------------------------------*/
+void MIC_vGetMIC(u32 * pdwL, u32 * pdwR);
 
 // Rotation functions on 32 bit values
 #define ROL32(A, n) \
diff --git a/drivers/staging/vt6656/power.c b/drivers/staging/vt6656/power.c
index 527c259f675..edc8975b2e2 100644
--- a/drivers/staging/vt6656/power.c
+++ b/drivers/staging/vt6656/power.c
@@ -37,7 +37,6 @@
  *
  */
 
-#include "ttype.h"
 #include "mac.h"
 #include "device.h"
 #include "wmgr.h"
@@ -48,17 +47,7 @@
 #include "control.h"
 #include "rndis.h"
 
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
 static int msglevel = MSG_LEVEL_INFO;
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
 
 /*
  *
@@ -290,7 +279,7 @@ int PSbSendNullPacket(struct vnt_private *pDevice)
 	pTxPacket->p80211Header->sA3.wFrameCtl = cpu_to_le16(flags);
 
 	if (pMgmt->eCurrMode != WMAC_MODE_IBSS_STA)
-		pTxPacket->p80211Header->sA3.wFrameCtl |= cpu_to_le16((WORD)WLAN_SET_FC_TODS(1));
+		pTxPacket->p80211Header->sA3.wFrameCtl |= cpu_to_le16((u16)WLAN_SET_FC_TODS(1));
 
 	memcpy(pTxPacket->p80211Header->sA3.abyAddr1, pMgmt->abyCurrBSSID, WLAN_ADDR_LEN);
 	memcpy(pTxPacket->p80211Header->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
diff --git a/drivers/staging/vt6656/power.h b/drivers/staging/vt6656/power.h
index 879b10c0d42..77835823943 100644
--- a/drivers/staging/vt6656/power.h
+++ b/drivers/staging/vt6656/power.h
@@ -29,22 +29,10 @@
 #ifndef __POWER_H__
 #define __POWER_H__
 
-
-/*---------------------  Export Definitions -------------------------*/
 #define     C_PWBT                   1000      // micro sec. power up before TBTT
 #define     PS_FAST_INTERVAL         1         // Fast power saving listen interval
 #define     PS_MAX_INTERVAL          4         // MAX power saving listen interval
 
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-
-/*---------------------  Export Types  ------------------------------*/
-
-
-/*---------------------  Export Functions  --------------------------*/
-
 /*  PSDevice pDevice */
 /*  PSDevice hDeviceContext */
 
diff --git a/drivers/staging/vt6656/rc4.c b/drivers/staging/vt6656/rc4.c
index 5c3c2d0552b..2fd836f0753 100644
--- a/drivers/staging/vt6656/rc4.c
+++ b/drivers/staging/vt6656/rc4.c
@@ -32,27 +32,27 @@
 
 #include "rc4.h"
 
-void rc4_init(PRC4Ext pRC4, PBYTE pbyKey, unsigned int cbKey_len)
+void rc4_init(PRC4Ext pRC4, u8 * pbyKey, unsigned int cbKey_len)
 {
 	unsigned int  ust1, ust2;
 	unsigned int  keyindex;
 	unsigned int  stateindex;
-	PBYTE pbyst;
+	u8 * pbyst;
 	unsigned int  idx;
 
 	pbyst = pRC4->abystate;
 	pRC4->ux = 0;
 	pRC4->uy = 0;
 	for (idx = 0; idx < 256; idx++)
-		pbyst[idx] = (BYTE)idx;
+		pbyst[idx] = (u8)idx;
 	keyindex = 0;
 	stateindex = 0;
 	for (idx = 0; idx < 256; idx++) {
 		ust1 = pbyst[idx];
 		stateindex = (stateindex + pbyKey[keyindex] + ust1) & 0xff;
 		ust2 = pbyst[stateindex];
-		pbyst[stateindex] = (BYTE)ust1;
-		pbyst[idx] = (BYTE)ust2;
+		pbyst[stateindex] = (u8)ust1;
+		pbyst[idx] = (u8)ust2;
 		if (++keyindex >= cbKey_len)
 			keyindex = 0;
 	}
@@ -63,7 +63,7 @@ unsigned int rc4_byte(PRC4Ext pRC4)
 	unsigned int ux;
 	unsigned int uy;
 	unsigned int ustx, usty;
-	PBYTE pbyst;
+	u8 * pbyst;
 
 	pbyst = pRC4->abystate;
 	ux = (pRC4->ux + 1) & 0xff;
@@ -72,16 +72,16 @@ unsigned int rc4_byte(PRC4Ext pRC4)
 	usty = pbyst[uy];
 	pRC4->ux = ux;
 	pRC4->uy = uy;
-	pbyst[uy] = (BYTE)ustx;
-	pbyst[ux] = (BYTE)usty;
+	pbyst[uy] = (u8)ustx;
+	pbyst[ux] = (u8)usty;
 
 	return pbyst[(ustx + usty) & 0xff];
 }
 
-void rc4_encrypt(PRC4Ext pRC4, PBYTE pbyDest,
-			PBYTE pbySrc, unsigned int cbData_len)
+void rc4_encrypt(PRC4Ext pRC4, u8 * pbyDest,
+			u8 * pbySrc, unsigned int cbData_len)
 {
 	unsigned int ii;
 	for (ii = 0; ii < cbData_len; ii++)
-		pbyDest[ii] = (BYTE)(pbySrc[ii] ^ rc4_byte(pRC4));
+		pbyDest[ii] = (u8)(pbySrc[ii] ^ rc4_byte(pRC4));
 }
diff --git a/drivers/staging/vt6656/rc4.h b/drivers/staging/vt6656/rc4.h
index d447879c8f9..d376e1adcc1 100644
--- a/drivers/staging/vt6656/rc4.h
+++ b/drivers/staging/vt6656/rc4.h
@@ -30,19 +30,17 @@
 #ifndef __RC4_H__
 #define __RC4_H__
 
-#include "ttype.h"
+#include <linux/types.h>
 
-/*---------------------  Export Definitions -------------------------*/
-/*---------------------  Export Types  ------------------------------*/
 typedef struct {
     unsigned int ux;
     unsigned int uy;
-    BYTE abystate[256];
+    u8 abystate[256];
 } RC4Ext, *PRC4Ext;
 
-void rc4_init(PRC4Ext pRC4, PBYTE pbyKey, unsigned int cbKey_len);
+void rc4_init(PRC4Ext pRC4, u8 * pbyKey, unsigned int cbKey_len);
 unsigned int rc4_byte(PRC4Ext pRC4);
-void rc4_encrypt(PRC4Ext pRC4, PBYTE pbyDest, PBYTE pbySrc,
+void rc4_encrypt(PRC4Ext pRC4, u8 * pbyDest, u8 * pbySrc,
 		 unsigned int cbData_len);
 
 #endif /* __RC4_H__ */
diff --git a/drivers/staging/vt6656/rf.c b/drivers/staging/vt6656/rf.c
index a415705297b..44cfe0b1418 100644
--- a/drivers/staging/vt6656/rf.c
+++ b/drivers/staging/vt6656/rf.c
@@ -41,7 +41,6 @@
 
 static int          msglevel                =MSG_LEVEL_INFO;
 //static int          msglevel                =MSG_LEVEL_DEBUG;
-/*---------------------  Static Definitions -------------------------*/
 #define BY_AL2230_REG_LEN     23 //24bit
 #define CB_AL2230_INIT_SEQ    15
 #define AL2230_PWR_IDX_LEN    64
@@ -62,13 +61,6 @@ static int          msglevel                =MSG_LEVEL_INFO;
 #define VT3342_PWR_IDX_LEN    64
 //}}
 
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
-
-
-
 u8 abyAL2230InitTable[CB_AL2230_INIT_SEQ][3] = {
     {0x03, 0xF7, 0x90},
     {0x03, 0x33, 0x31},
@@ -388,7 +380,6 @@ u8 abyVT3226D0_InitTable[CB_VT3226_INIT_SEQ][3] = {
     {0x02, 0x01, 0xAA}  //RobertYu:20060523
     };
 
-
 u8 abyVT3226_ChannelTable0[CB_MAX_CHANNEL_24G][3] = {
     {0x01, 0x97, 0x83}, // channel = 1, Tf = 2412MHz
     {0x01, 0x97, 0x83}, // channel = 2, Tf = 2417MHz
@@ -424,7 +415,6 @@ u8 abyVT3226_ChannelTable1[CB_MAX_CHANNEL_24G][3] = {
     };
 ///}}RobertYu
 
-
 //{{RobertYu:20060502, TWIF 1.14, LO Current for 11b mode
 u32 dwVT3226D0LoCurrentTable[CB_MAX_CHANNEL_24G] = {
     0x0135C600+(BY_VT3226_REG_LEN<<3)+IFREGCTL_REGW, // channel = 1, Tf = 2412MHz
@@ -444,7 +434,6 @@ u32 dwVT3226D0LoCurrentTable[CB_MAX_CHANNEL_24G] = {
 };
 //}}
 
-
 //{{RobertYu:20060609
 u8 abyVT3342A0_InitTable[CB_VT3342_INIT_SEQ][3] = { /* 11b/g mode */
     {0x03, 0xFF, 0x80}, //update for mode//
@@ -599,7 +588,6 @@ u8 abyVT3342_ChannelTable1[CB_MAX_CHANNEL][3] = {
     {0x03, 0x00, 0x04}  // channel = 165, Tf = 5825MHz (56), TBD
     };
 
-
 /*+
  *
  * Power Table
@@ -673,10 +661,6 @@ const u32 dwAL2230PowerTable[AL2230_PWR_IDX_LEN] = {
     0x0407F900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW
     };
 
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
 //{{ RobertYu:20050103, Channel 11a Number To Index
 // 4.9G => Ch 183, 184, 185, 187, 188, 189, 192, 196  (Value:15 ~ 22)
 // 5G => Ch 7, 8, 9, 11, 12, 16, 34, 36, 38, 40, 42, 44, 46, 48, 52, 56, 60, 64,
@@ -708,8 +692,6 @@ const u8 RFaby11aChannelIndex[200] = {
 };
 //}} RobertYu
 
-/*---------------------  Export Functions  --------------------------*/
-
 /*
  * Description: Write to IF/RF, by embedded programming
  *
@@ -734,11 +716,9 @@ int IFRFbWriteEmbedded(struct vnt_private *pDevice, u32 dwData)
 	CONTROLnsRequestOut(pDevice,
 		MESSAGE_TYPE_WRITE_IFRF, 0, 0, 4, pbyData);
 
-
 	return true;
 }
 
-
 /*
  * Description: Set Tx power
  *
@@ -790,7 +770,6 @@ int RFbSetPower(struct vnt_private *pDevice, u32 uRATE, u32 uCH)
     return bResult;
 }
 
-
 /*
  * Description: Set Tx power
  *
@@ -839,10 +818,9 @@ int RFbRawSetPower(struct vnt_private *pDevice, u8 byPwr, u32 uRATE)
             }
             break;
 
-
         case RF_AIROHA7230:
             {
-                DWORD       dwMax7230Pwr;
+                u32       dwMax7230Pwr;
 
                 if (uRATE <= RATE_11M) { //RobertYu:20060426, for better 11b mask
                     bResult &= IFRFbWriteEmbedded(pDevice, 0x111BB900+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW);
@@ -864,7 +842,7 @@ int RFbRawSetPower(struct vnt_private *pDevice, u8 byPwr, u32 uRATE)
 
         case RF_VT3226: //RobertYu:20051111, VT3226C0 and before
         {
-            DWORD       dwVT3226Pwr;
+            u32       dwVT3226Pwr;
 
             if (pDevice->byCurPwr >= VT3226_PWR_IDX_LEN)
                 return false;
@@ -876,7 +854,7 @@ int RFbRawSetPower(struct vnt_private *pDevice, u8 byPwr, u32 uRATE)
 
         case RF_VT3226D0: //RobertYu:20051228
         {
-            DWORD       dwVT3226Pwr;
+            u32       dwVT3226Pwr;
 
             if (pDevice->byCurPwr >= VT3226_PWR_IDX_LEN)
                 return false;
@@ -921,7 +899,7 @@ int RFbRawSetPower(struct vnt_private *pDevice, u8 byPwr, u32 uRATE)
         //{{RobertYu:20060609
         case RF_VT3342A0:
         {
-            DWORD       dwVT3342Pwr;
+            u32       dwVT3342Pwr;
 
             if (pDevice->byCurPwr >= VT3342_PWR_IDX_LEN)
                 return false;
@@ -976,8 +954,6 @@ void RFvRSSITodBm(struct vnt_private *pDevice, u8 byCurrRSSI, long *pldBm)
     *pldBm = -1 * (a + b * 2);
 }
 
-
-
 void RFbRFTableDownload(struct vnt_private *pDevice)
 {
 	u16 wLength1 = 0, wLength2 = 0, wLength3 = 0;
@@ -1121,20 +1097,3 @@ void RFbRFTableDownload(struct vnt_private *pDevice)
     }
 
 }
-
-int s_bVT3226D0_11bLoCurrentAdjust(struct vnt_private *pDevice, u8 byChannel,
-	int b11bMode)
-{
-	int bResult = true;
-
-	if (b11bMode)
-		bResult &= IFRFbWriteEmbedded(pDevice,
-			dwVT3226D0LoCurrentTable[byChannel-1]);
-	else
-		bResult &= IFRFbWriteEmbedded(pDevice, 0x016bc600 +
-			(BY_VT3226_REG_LEN << 3) + IFREGCTL_REGW);
-
-	return bResult;
-}
-
-
diff --git a/drivers/staging/vt6656/rf.h b/drivers/staging/vt6656/rf.h
index 9f70cf740ba..de5c613b4ef 100644
--- a/drivers/staging/vt6656/rf.h
+++ b/drivers/staging/vt6656/rf.h
@@ -30,10 +30,8 @@
 #ifndef __RF_H__
 #define __RF_H__
 
-#include "ttype.h"
 #include "device.h"
 
-/*---------------------  Export Definitions -------------------------*/
 //
 // Baseband RF pair definition in eeprom (Bits 6..0)
 //
@@ -55,20 +53,12 @@
 #define RF_EMU              0x80
 #define RF_MASK             0x7F
 
-
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
 extern const u8 RFaby11aChannelIndex[200];
-/*---------------------  Export Functions  --------------------------*/
 
 int IFRFbWriteEmbedded(struct vnt_private *, u32 dwData);
 int RFbSetPower(struct vnt_private *, u32 uRATE, u32 uCH);
 int RFbRawSetPower(struct vnt_private *, u8 byPwr, u32 uRATE);
 void RFvRSSITodBm(struct vnt_private *, u8 byCurrRSSI, long *pldBm);
 void RFbRFTableDownload(struct vnt_private *pDevice);
-int s_bVT3226D0_11bLoCurrentAdjust(struct vnt_private *, u8 byChannel,
-	int b11bMode);
 
 #endif /* __RF_H__ */
diff --git a/drivers/staging/vt6656/rndis.h b/drivers/staging/vt6656/rndis.h
index fccf7e98eb6..5e073062017 100644
--- a/drivers/staging/vt6656/rndis.h
+++ b/drivers/staging/vt6656/rndis.h
@@ -27,11 +27,9 @@
  *
  */
 
-
 #ifndef __RNDIS_H__
 #define __RNDIS_H__
 
-/*---------------------  Export Definitions -------------------------*/
 #define MESSAGE_TYPE_READ               0x01
 #define MESSAGE_TYPE_WRITE              0x00
 #define MESSAGE_TYPE_LOCK_OR            0x02
@@ -66,96 +64,86 @@
 #define MESSAGE_REQUEST_RF_CH1          0x0C
 #define MESSAGE_REQUEST_RF_CH2          0x0D
 
-
 #define VIAUSB20_PACKET_HEADER          0x04
 
-
-/*---------------------  Export Classes  ----------------------------*/
-
 typedef struct _CMD_MESSAGE
 {
-    BYTE        byData[256];
+    u8        byData[256];
 } CMD_MESSAGE, *PCMD_MESSAGE;
 
 typedef struct _CMD_WRITE_MASK
 {
-    BYTE        byData;
-    BYTE        byMask;
+    u8        byData;
+    u8        byMask;
 } CMD_WRITE_MASK, *PCMD_WRITE_MASK;
 
 typedef struct _CMD_CARD_INIT
 {
-    BYTE        byInitClass;
-    BYTE        bExistSWNetAddr;
-    BYTE        bySWNetAddr[6];
-    BYTE        byShortRetryLimit;
-    BYTE        byLongRetryLimit;
+    u8        byInitClass;
+    u8        bExistSWNetAddr;
+    u8        bySWNetAddr[6];
+    u8        byShortRetryLimit;
+    u8        byLongRetryLimit;
 } CMD_CARD_INIT, *PCMD_CARD_INIT;
 
 typedef struct _RSP_CARD_INIT
 {
-    BYTE        byStatus;
-    BYTE        byNetAddr[6];
-    BYTE        byRFType;
-    BYTE        byMinChannel;
-    BYTE        byMaxChannel;
+    u8        byStatus;
+    u8        byNetAddr[6];
+    u8        byRFType;
+    u8        byMinChannel;
+    u8        byMaxChannel;
 } RSP_CARD_INIT, *PRSP_CARD_INIT;
 
 typedef struct _CMD_SET_KEY
 {
-    WORD        wKCTL;
-    BYTE        abyMacAddr[6];
-    BYTE        abyKey[16];
+    u16        wKCTL;
+    u8        abyMacAddr[6];
+    u8        abyKey[16];
 } CMD_SET_KEY, *PCMD_SET_KEY;
 
 typedef struct _CMD_CLRKEY_ENTRY
 {
-    BYTE        abyKeyEntry[11];
+    u8        abyKeyEntry[11];
 } CMD_CLRKEY_ENTRY, *PCMD_CLRKEY_ENTRY;
 
 typedef struct _CMD_WRITE_MISCFF
 {
-    DWORD       adwMiscFFData[22][4];  //a key entry has only 22 dwords
+    u32       adwMiscFFData[22][4];  //a key entry has only 22 dwords
 } CMD_WRITE_MISCFF, *PCMD_WRITE_MISCFF;
 
 typedef struct _CMD_SET_TSFTBTT
 {
-    BYTE        abyTSF_TBTT[8];
+    u8        abyTSF_TBTT[8];
 } CMD_SET_TSFTBTT, *PCMD_SET_TSFTBTT;
 
 typedef struct _CMD_SET_SSTIFS
 {
-    BYTE        bySIFS;
-    BYTE        byDIFS;
-    BYTE        byEIFS;
-    BYTE        bySlotTime;
-    BYTE        byCwMax_Min;
-    BYTE        byBBCR10;
+    u8        bySIFS;
+    u8        byDIFS;
+    u8        byEIFS;
+    u8        bySlotTime;
+    u8        byCwMax_Min;
+    u8        byBBCR10;
 } CMD_SET_SSTIFS, *PCMD_SET_SSTIFS;
 
 typedef struct _CMD_CHANGE_BBTYPE
 {
-    BYTE        bySIFS;
-    BYTE        byDIFS;
-    BYTE        byEIFS;
-    BYTE        bySlotTime;
-    BYTE        byCwMax_Min;
-    BYTE        byBBCR10;
-    BYTE        byBB_BBType;    //CR88
-    BYTE        byMAC_BBType;
-    DWORD       dwRSPINF_b_1;
-    DWORD       dwRSPINF_b_2;
-    DWORD       dwRSPINF_b_55;
-    DWORD       dwRSPINF_b_11;
-    WORD        wRSPINF_a[9];
+    u8        bySIFS;
+    u8        byDIFS;
+    u8        byEIFS;
+    u8        bySlotTime;
+    u8        byCwMax_Min;
+    u8        byBBCR10;
+    u8        byBB_BBType;    //CR88
+    u8        byMAC_BBType;
+    u32       dwRSPINF_b_1;
+    u32       dwRSPINF_b_2;
+    u32       dwRSPINF_b_55;
+    u32       dwRSPINF_b_11;
+    u16        wRSPINF_a[9];
 } CMD_CHANGE_BBTYPE, *PCMD_CHANGE_BBTYPE;
 
-/*---------------------  Export Macros -------------------------*/
-
-#define EXCH_WORD(w) ((WORD)((WORD)(w)<<8) | (WORD)((WORD)(w)>>8))
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
+#define EXCH_WORD(w) ((u16)((u16)(w)<<8) | (u16)((u16)(w)>>8))
 
 #endif /* _RNDIS_H_ */
diff --git a/drivers/staging/vt6656/rxtx.c b/drivers/staging/vt6656/rxtx.c
index b939dcf689d..9bf2f8d562c 100644
--- a/drivers/staging/vt6656/rxtx.c
+++ b/drivers/staging/vt6656/rxtx.c
@@ -63,32 +63,22 @@
 #include "usbpipe.h"
 #include "iocmd.h"
 
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
 static int          msglevel                = MSG_LEVEL_INFO;
 
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Static Definitions -------------------------*/
-
-const WORD wTimeStampOff[2][MAX_RATE] = {
+const u16 wTimeStampOff[2][MAX_RATE] = {
         {384, 288, 226, 209, 54, 43, 37, 31, 28, 25, 24, 23}, // Long Preamble
         {384, 192, 130, 113, 54, 43, 37, 31, 28, 25, 24, 23}, // Short Preamble
     };
 
-const WORD wFB_Opt0[2][5] = {
+const u16 wFB_Opt0[2][5] = {
         {RATE_12M, RATE_18M, RATE_24M, RATE_36M, RATE_48M}, // fallback_rate0
         {RATE_12M, RATE_12M, RATE_18M, RATE_24M, RATE_36M}, // fallback_rate1
     };
-const WORD wFB_Opt1[2][5] = {
+const u16 wFB_Opt1[2][5] = {
         {RATE_12M, RATE_18M, RATE_24M, RATE_24M, RATE_36M}, // fallback_rate0
         {RATE_6M , RATE_6M,  RATE_12M, RATE_12M, RATE_18M}, // fallback_rate1
     };
 
-
 #define RTSDUR_BB       0
 #define RTSDUR_BA       1
 #define RTSDUR_AA       2
@@ -104,8 +94,6 @@ const WORD wFB_Opt1[2][5] = {
 #define DATADUR_A_F0    12
 #define DATADUR_A_F1    13
 
-/*---------------------  Static Functions  --------------------------*/
-
 static void s_vSaveTxPktInfo(struct vnt_private *pDevice, u8 byPktNum,
 	u8 *pbyDestAddr, u16 wPktLength, u16 wFIFOCtl);
 
@@ -114,16 +102,15 @@ static void *s_vGetFreeContext(struct vnt_private *pDevice);
 static void s_vGenerateTxParameter(struct vnt_private *pDevice,
 	u8 byPktType, u16 wCurrentRate,	void *pTxBufHead, void *pvRrvTime,
 	void *pvRTS, void *pvCTS, u32 cbFrameSize, int bNeedACK, u32 uDMAIdx,
-	PSEthernetHeader psEthHeader);
+	struct ethhdr *psEthHeader);
 
 static u32 s_uFillDataHead(struct vnt_private *pDevice,
 	u8 byPktType, u16 wCurrentRate, void *pTxDataHead, u32 cbFrameLength,
 	u32 uDMAIdx, int bNeedAck, u32 uFragIdx, u32 cbLastFragmentSize,
 	u32 uMACfragNum, u8 byFBOption);
 
-
 static void s_vGenerateMACHeader(struct vnt_private *pDevice,
-	u8 *pbyBufferAddr, u16 wDuration, PSEthernetHeader psEthHeader,
+	u8 *pbyBufferAddr, u16 wDuration, struct ethhdr *psEthHeader,
 	int bNeedEncrypt, u16 wFragType, u32 uDMAIdx, u32 uFragIdx);
 
 static void s_vFillTxKey(struct vnt_private *pDevice, u8 *pbyBuf,
@@ -145,7 +132,7 @@ static void s_vFillCTSHead(struct vnt_private *pDevice, u32 uDMAIdx,
 
 static void s_vFillRTSHead(struct vnt_private *pDevice, u8 byPktType,
 	void *pvRTS, u32 cbFrameLength, int bNeedAck, int bDisCRC,
-	PSEthernetHeader psEthHeader, u16 wCurrentRate, u8 byFBOption);
+	struct ethhdr *psEthHeader, u16 wCurrentRate, u8 byFBOption);
 
 static u32 s_uGetDataDuration(struct vnt_private *pDevice, u8 byDurType,
 	u32 cbFrameLength, u8 byPktType, u16 wRate, int bNeedAck,
@@ -156,9 +143,6 @@ static unsigned int s_uGetRTSCTSDuration(struct vnt_private *pDevice,
 	u8 byDurType, u32 cbFrameLength, u8 byPktType, u16 wRate,
 	int bNeedAck, u8 byFBOption);
 
-
-/*---------------------  Export Variables  --------------------------*/
-
 static void *s_vGetFreeContext(struct vnt_private *pDevice)
 {
 	PUSB_SEND_CONTEXT pContext = NULL;
@@ -181,7 +165,6 @@ static void *s_vGetFreeContext(struct vnt_private *pDevice)
     return (void *) pReturnContext;
 }
 
-
 static void s_vSaveTxPktInfo(struct vnt_private *pDevice, u8 byPktNum,
 	u8 *pbyDestAddr, u16 wPktLength, u16 wFIFOCtl)
 {
@@ -208,10 +191,9 @@ static void s_vFillTxKey(struct vnt_private *pDevice, u8 *pbyBuf,
 	u32 *pdwIV = (u32 *)pbyIVHead;
 	u32 *pdwExtIV = (u32 *)((u8 *)pbyIVHead + 4);
 	u16 wValue;
-	PS802_11Header pMACHeader = (PS802_11Header)pbyHdrBuf;
+	struct ieee80211_hdr *pMACHeader = (struct ieee80211_hdr *)pbyHdrBuf;
 	u32 dwRevIVCounter;
 
-
     //Fill TXKEY
     if (pTransmitKey == NULL)
         return;
@@ -222,13 +204,13 @@ static void s_vFillTxKey(struct vnt_private *pDevice, u8 *pbyBuf,
 
     if (pTransmitKey->byCipherSuite == KEY_CTL_WEP) {
         if (pTransmitKey->uKeyLength == WLAN_WEP232_KEYLEN ){
-            memcpy(pDevice->abyPRNG, (PBYTE)&(dwRevIVCounter), 3);
+            memcpy(pDevice->abyPRNG, (u8 *)&(dwRevIVCounter), 3);
             memcpy(pDevice->abyPRNG+3, pTransmitKey->abyKey, pTransmitKey->uKeyLength);
         } else {
-            memcpy(pbyBuf, (PBYTE)&(dwRevIVCounter), 3);
+            memcpy(pbyBuf, (u8 *)&(dwRevIVCounter), 3);
             memcpy(pbyBuf+3, pTransmitKey->abyKey, pTransmitKey->uKeyLength);
             if(pTransmitKey->uKeyLength == WLAN_WEP40_KEYLEN) {
-                memcpy(pbyBuf+8, (PBYTE)&(dwRevIVCounter), 3);
+                memcpy(pbyBuf+8, (u8 *)&(dwRevIVCounter), 3);
                 memcpy(pbyBuf+11, pTransmitKey->abyKey, pTransmitKey->uKeyLength);
             }
             memcpy(pDevice->abyPRNG, pbyBuf, 16);
@@ -252,7 +234,7 @@ static void s_vFillTxKey(struct vnt_private *pDevice, u8 *pbyBuf,
         // Make IV
         memcpy(pdwIV, pDevice->abyPRNG, 3);
 
-        *(pbyIVHead+3) = (BYTE)(((pDevice->byKeyIndex << 6) & 0xc0) | 0x20); // 0x20 is ExtIV
+        *(pbyIVHead+3) = (u8)(((pDevice->byKeyIndex << 6) & 0xc0) | 0x20); // 0x20 is ExtIV
         // Append IV&ExtIV after Mac Header
         *pdwExtIV = cpu_to_le32(pTransmitKey->dwTSC47_16);
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"vFillTxKey()---- pdwExtIV: %x\n",
@@ -267,49 +249,48 @@ static void s_vFillTxKey(struct vnt_private *pDevice, u8 *pbyBuf,
 
         // Make IV
         *pdwIV = 0;
-        *(pbyIVHead+3) = (BYTE)(((pDevice->byKeyIndex << 6) & 0xc0) | 0x20); // 0x20 is ExtIV
-        *pdwIV |= cpu_to_le16((WORD)(pTransmitKey->wTSC15_0));
+        *(pbyIVHead+3) = (u8)(((pDevice->byKeyIndex << 6) & 0xc0) | 0x20); // 0x20 is ExtIV
+        *pdwIV |= cpu_to_le16((u16)(pTransmitKey->wTSC15_0));
         //Append IV&ExtIV after Mac Header
         *pdwExtIV = cpu_to_le32(pTransmitKey->dwTSC47_16);
 
         //Fill MICHDR0
         *pMICHDR = 0x59;
-        *((PBYTE)(pMICHDR+1)) = 0; // TxPriority
-        memcpy(pMICHDR+2, &(pMACHeader->abyAddr2[0]), 6);
-        *((PBYTE)(pMICHDR+8)) = HIBYTE(HIWORD(pTransmitKey->dwTSC47_16));
-        *((PBYTE)(pMICHDR+9)) = LOBYTE(HIWORD(pTransmitKey->dwTSC47_16));
-        *((PBYTE)(pMICHDR+10)) = HIBYTE(LOWORD(pTransmitKey->dwTSC47_16));
-        *((PBYTE)(pMICHDR+11)) = LOBYTE(LOWORD(pTransmitKey->dwTSC47_16));
-        *((PBYTE)(pMICHDR+12)) = HIBYTE(pTransmitKey->wTSC15_0);
-        *((PBYTE)(pMICHDR+13)) = LOBYTE(pTransmitKey->wTSC15_0);
-        *((PBYTE)(pMICHDR+14)) = HIBYTE(wPayloadLen);
-        *((PBYTE)(pMICHDR+15)) = LOBYTE(wPayloadLen);
+        *((u8 *)(pMICHDR+1)) = 0; // TxPriority
+        memcpy(pMICHDR+2, &(pMACHeader->addr2[0]), 6);
+        *((u8 *)(pMICHDR+8)) = HIBYTE(HIWORD(pTransmitKey->dwTSC47_16));
+        *((u8 *)(pMICHDR+9)) = LOBYTE(HIWORD(pTransmitKey->dwTSC47_16));
+        *((u8 *)(pMICHDR+10)) = HIBYTE(LOWORD(pTransmitKey->dwTSC47_16));
+        *((u8 *)(pMICHDR+11)) = LOBYTE(LOWORD(pTransmitKey->dwTSC47_16));
+        *((u8 *)(pMICHDR+12)) = HIBYTE(pTransmitKey->wTSC15_0);
+        *((u8 *)(pMICHDR+13)) = LOBYTE(pTransmitKey->wTSC15_0);
+        *((u8 *)(pMICHDR+14)) = HIBYTE(wPayloadLen);
+        *((u8 *)(pMICHDR+15)) = LOBYTE(wPayloadLen);
 
         //Fill MICHDR1
-        *((PBYTE)(pMICHDR+16)) = 0; // HLEN[15:8]
+        *((u8 *)(pMICHDR+16)) = 0; // HLEN[15:8]
         if (pDevice->bLongHeader) {
-            *((PBYTE)(pMICHDR+17)) = 28; // HLEN[7:0]
+            *((u8 *)(pMICHDR+17)) = 28; // HLEN[7:0]
         } else {
-            *((PBYTE)(pMICHDR+17)) = 22; // HLEN[7:0]
+            *((u8 *)(pMICHDR+17)) = 22; // HLEN[7:0]
         }
-        wValue = cpu_to_le16(pMACHeader->wFrameCtl & 0xC78F);
-        memcpy(pMICHDR+18, (PBYTE)&wValue, 2); // MSKFRACTL
-        memcpy(pMICHDR+20, &(pMACHeader->abyAddr1[0]), 6);
-        memcpy(pMICHDR+26, &(pMACHeader->abyAddr2[0]), 6);
+        wValue = cpu_to_le16(pMACHeader->frame_control & 0xC78F);
+        memcpy(pMICHDR+18, (u8 *)&wValue, 2); // MSKFRACTL
+        memcpy(pMICHDR+20, &(pMACHeader->addr1[0]), 6);
+        memcpy(pMICHDR+26, &(pMACHeader->addr2[0]), 6);
 
         //Fill MICHDR2
-        memcpy(pMICHDR+32, &(pMACHeader->abyAddr3[0]), 6);
-        wValue = pMACHeader->wSeqCtl;
+        memcpy(pMICHDR+32, &(pMACHeader->addr3[0]), 6);
+        wValue = pMACHeader->seq_ctrl;
         wValue &= 0x000F;
         wValue = cpu_to_le16(wValue);
-        memcpy(pMICHDR+38, (PBYTE)&wValue, 2); // MSKSEQCTL
+        memcpy(pMICHDR+38, (u8 *)&wValue, 2); // MSKSEQCTL
         if (pDevice->bLongHeader) {
-            memcpy(pMICHDR+40, &(pMACHeader->abyAddr4[0]), 6);
+            memcpy(pMICHDR+40, &(pMACHeader->addr4[0]), 6);
         }
     }
 }
 
-
 static void s_vSWencryption(struct vnt_private *pDevice,
 	PSKeyItem pTransmitKey, u8 *pbyPayloadHead, u16 wPayloadSize)
 {
@@ -324,7 +305,7 @@ static void s_vSWencryption(struct vnt_private *pDevice,
         //=======================================================================
         // Append ICV after payload
         dwICV = CRCdwGetCrc32Ex(pbyPayloadHead, wPayloadSize, dwICV);//ICV(Payload)
-        pdwICV = (PDWORD)(pbyPayloadHead + wPayloadSize);
+        pdwICV = (u32 *)(pbyPayloadHead + wPayloadSize);
         // finally, we must invert dwCRC to get the correct answer
         *pdwICV = cpu_to_le32(~dwICV);
         // RC4 encryption
@@ -335,7 +316,7 @@ static void s_vSWencryption(struct vnt_private *pDevice,
         //=======================================================================
         //Append ICV after payload
         dwICV = CRCdwGetCrc32Ex(pbyPayloadHead, wPayloadSize, dwICV);//ICV(Payload)
-        pdwICV = (PDWORD)(pbyPayloadHead + wPayloadSize);
+        pdwICV = (u32 *)(pbyPayloadHead + wPayloadSize);
         // finally, we must invert dwCRC to get the correct answer
         *pdwICV = cpu_to_le32(~dwICV);
         // RC4 encryption
@@ -345,9 +326,6 @@ static void s_vSWencryption(struct vnt_private *pDevice,
     }
 }
 
-
-
-
 /*byPktType : PK_TYPE_11A     0
              PK_TYPE_11B     1
              PK_TYPE_11GB    2
@@ -360,9 +338,9 @@ static u32 s_uGetTxRsvTime(struct vnt_private *pDevice, u8 byPktType,
 
     uDataTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, cbFrameLength, wRate);
     if (byPktType == PK_TYPE_11B) {//llb,CCK mode
-        uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, (WORD)pDevice->byTopCCKBasicRate);
+        uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, (u16)pDevice->byTopCCKBasicRate);
     } else {//11g 2.4G OFDM mode & 11a 5G OFDM mode
-        uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, (WORD)pDevice->byTopOFDMBasicRate);
+        uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, (u16)pDevice->byTopOFDMBasicRate);
     }
 
     if (bNeedAck) {
@@ -381,7 +359,6 @@ static u32 s_uGetRTSCTSRsvTime(struct vnt_private *pDevice,
 
     uRrvTime = uRTSTime = uCTSTime = uAckTime = uDataTime = 0;
 
-
     uDataTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, cbFrameLength, wCurrentRate);
     if (byRTSRsvType == 0) { //RTSTxRrvTime_bb
         uRTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 20, pDevice->byTopCCKBasicRate);
@@ -447,7 +424,6 @@ static u32 s_uGetDataDuration(struct vnt_private *pDevice, u8 byDurType,
         }
         break;
 
-
     case DATADUR_A:    //DATADUR_A
         if (((uMACfragNum==1)) || (bLastFrag==1)) {//Non Frag or Last Frag
             if(bNeedAck){
@@ -566,7 +542,6 @@ static u32 s_uGetDataDuration(struct vnt_private *pDevice, u8 byDurType,
 	return 0;
 }
 
-
 //byFreqType: 0=>5GHZ 1=>2.4GHZ
 static u32 s_uGetRTSCTSDuration(struct vnt_private *pDevice, u8 byDurType,
 	u32 cbFrameLength, u8 byPktType, u16 wRate, int bNeedAck,
@@ -574,7 +549,6 @@ static u32 s_uGetRTSCTSDuration(struct vnt_private *pDevice, u8 byDurType,
 {
 	u32 uCTSTime = 0, uDurTime = 0;
 
-
     switch (byDurType) {
 
     case RTSDUR_BB:    //RTSDuration_bb
@@ -671,10 +645,10 @@ static u32 s_uFillDataHead(struct vnt_private *pDevice,
 		PSTxDataHead_ab pBuf = (PSTxDataHead_ab) pTxDataHead;
             //Get SignalField,ServiceField,Length
             BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
-                (PWORD)&(pBuf->wTransmitLength), (PBYTE)&(pBuf->byServiceField), (PBYTE)&(pBuf->bySignalField)
+                (u16 *)&(pBuf->wTransmitLength), (u8 *)&(pBuf->byServiceField), (u8 *)&(pBuf->bySignalField)
             );
             //Get Duration and TimeStampOff
-            pBuf->wDuration = (WORD)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength, byPktType,
+            pBuf->wDuration = (u16)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength, byPktType,
                                                        wCurrentRate, bNeedAck, uFragIdx,
                                                        cbLastFragmentSize, uMACfragNum,
                                                        byFBOption); //1: 2.4GHz
@@ -688,17 +662,17 @@ static u32 s_uFillDataHead(struct vnt_private *pDevice,
                 PSTxDataHead_g pBuf = (PSTxDataHead_g)pTxDataHead;
                 //Get SignalField,ServiceField,Length
                 BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
-                    (PWORD)&(pBuf->wTransmitLength_a), (PBYTE)&(pBuf->byServiceField_a), (PBYTE)&(pBuf->bySignalField_a)
+                    (u16 *)&(pBuf->wTransmitLength_a), (u8 *)&(pBuf->byServiceField_a), (u8 *)&(pBuf->bySignalField_a)
                 );
                 BBvCalculateParameter(pDevice, cbFrameLength, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
-                    (PWORD)&(pBuf->wTransmitLength_b), (PBYTE)&(pBuf->byServiceField_b), (PBYTE)&(pBuf->bySignalField_b)
+                    (u16 *)&(pBuf->wTransmitLength_b), (u8 *)&(pBuf->byServiceField_b), (u8 *)&(pBuf->bySignalField_b)
                 );
                 //Get Duration and TimeStamp
-                pBuf->wDuration_a = (WORD)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength,
+                pBuf->wDuration_a = (u16)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength,
                                                              byPktType, wCurrentRate, bNeedAck, uFragIdx,
                                                              cbLastFragmentSize, uMACfragNum,
                                                              byFBOption); //1: 2.4GHz
-                pBuf->wDuration_b = (WORD)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameLength,
+                pBuf->wDuration_b = (u16)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameLength,
                                                              PK_TYPE_11B, pDevice->byTopCCKBasicRate,
                                                              bNeedAck, uFragIdx, cbLastFragmentSize,
                                                              uMACfragNum, byFBOption); //1: 2.4GHz
@@ -711,19 +685,19 @@ static u32 s_uFillDataHead(struct vnt_private *pDevice,
                 PSTxDataHead_g_FB pBuf = (PSTxDataHead_g_FB)pTxDataHead;
                 //Get SignalField,ServiceField,Length
                 BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
-                    (PWORD)&(pBuf->wTransmitLength_a), (PBYTE)&(pBuf->byServiceField_a), (PBYTE)&(pBuf->bySignalField_a)
+                    (u16 *)&(pBuf->wTransmitLength_a), (u8 *)&(pBuf->byServiceField_a), (u8 *)&(pBuf->bySignalField_a)
                 );
                 BBvCalculateParameter(pDevice, cbFrameLength, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
-                    (PWORD)&(pBuf->wTransmitLength_b), (PBYTE)&(pBuf->byServiceField_b), (PBYTE)&(pBuf->bySignalField_b)
+                    (u16 *)&(pBuf->wTransmitLength_b), (u8 *)&(pBuf->byServiceField_b), (u8 *)&(pBuf->bySignalField_b)
                 );
                 //Get Duration and TimeStamp
-                pBuf->wDuration_a = (WORD)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength, byPktType,
+                pBuf->wDuration_a = (u16)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength, byPktType,
                                              wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption); //1: 2.4GHz
-                pBuf->wDuration_b = (WORD)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameLength, PK_TYPE_11B,
+                pBuf->wDuration_b = (u16)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameLength, PK_TYPE_11B,
                                              pDevice->byTopCCKBasicRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption); //1: 2.4GHz
-                pBuf->wDuration_a_f0 = (WORD)s_uGetDataDuration(pDevice, DATADUR_A_F0, cbFrameLength, byPktType,
+                pBuf->wDuration_a_f0 = (u16)s_uGetDataDuration(pDevice, DATADUR_A_F0, cbFrameLength, byPktType,
                                              wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption); //1: 2.4GHz
-                pBuf->wDuration_a_f1 = (WORD)s_uGetDataDuration(pDevice, DATADUR_A_F1, cbFrameLength, byPktType,
+                pBuf->wDuration_a_f1 = (u16)s_uGetDataDuration(pDevice, DATADUR_A_F1, cbFrameLength, byPktType,
                                              wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption); //1: 2.4GHz
                 pBuf->wTimeStampOff_a = wTimeStampOff[pDevice->byPreambleType%2][wCurrentRate%MAX_RATE];
                 pBuf->wTimeStampOff_b = wTimeStampOff[pDevice->byPreambleType%2][pDevice->byTopCCKBasicRate%MAX_RATE];
@@ -737,14 +711,14 @@ static u32 s_uFillDataHead(struct vnt_private *pDevice,
             PSTxDataHead_a_FB pBuf = (PSTxDataHead_a_FB)pTxDataHead;
             //Get SignalField,ServiceField,Length
             BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
-                (PWORD)&(pBuf->wTransmitLength), (PBYTE)&(pBuf->byServiceField), (PBYTE)&(pBuf->bySignalField)
+                (u16 *)&(pBuf->wTransmitLength), (u8 *)&(pBuf->byServiceField), (u8 *)&(pBuf->bySignalField)
             );
             //Get Duration and TimeStampOff
-            pBuf->wDuration = (WORD)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength, byPktType,
+            pBuf->wDuration = (u16)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength, byPktType,
                                         wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption); //0: 5GHz
-            pBuf->wDuration_f0 = (WORD)s_uGetDataDuration(pDevice, DATADUR_A_F0, cbFrameLength, byPktType,
+            pBuf->wDuration_f0 = (u16)s_uGetDataDuration(pDevice, DATADUR_A_F0, cbFrameLength, byPktType,
                                         wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption); //0: 5GHz
-            pBuf->wDuration_f1 = (WORD)s_uGetDataDuration(pDevice, DATADUR_A_F1, cbFrameLength, byPktType,
+            pBuf->wDuration_f1 = (u16)s_uGetDataDuration(pDevice, DATADUR_A_F1, cbFrameLength, byPktType,
                                         wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption); //0: 5GHz
             if(uDMAIdx!=TYPE_ATIMDMA) {
                 pBuf->wTimeStampOff = wTimeStampOff[pDevice->byPreambleType%2][wCurrentRate%MAX_RATE];
@@ -754,10 +728,10 @@ static u32 s_uFillDataHead(struct vnt_private *pDevice,
             PSTxDataHead_ab pBuf = (PSTxDataHead_ab)pTxDataHead;
             //Get SignalField,ServiceField,Length
             BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
-                (PWORD)&(pBuf->wTransmitLength), (PBYTE)&(pBuf->byServiceField), (PBYTE)&(pBuf->bySignalField)
+                (u16 *)&(pBuf->wTransmitLength), (u8 *)&(pBuf->byServiceField), (u8 *)&(pBuf->bySignalField)
             );
             //Get Duration and TimeStampOff
-            pBuf->wDuration = (WORD)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength, byPktType,
+            pBuf->wDuration = (u16)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength, byPktType,
                                                        wCurrentRate, bNeedAck, uFragIdx,
                                                        cbLastFragmentSize, uMACfragNum,
                                                        byFBOption);
@@ -772,10 +746,10 @@ static u32 s_uFillDataHead(struct vnt_private *pDevice,
             PSTxDataHead_ab pBuf = (PSTxDataHead_ab)pTxDataHead;
             //Get SignalField,ServiceField,Length
             BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
-                (PWORD)&(pBuf->wTransmitLength), (PBYTE)&(pBuf->byServiceField), (PBYTE)&(pBuf->bySignalField)
+                (u16 *)&(pBuf->wTransmitLength), (u8 *)&(pBuf->byServiceField), (u8 *)&(pBuf->bySignalField)
             );
             //Get Duration and TimeStampOff
-            pBuf->wDuration = (WORD)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameLength, byPktType,
+            pBuf->wDuration = (u16)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameLength, byPktType,
                                                        wCurrentRate, bNeedAck, uFragIdx,
                                                        cbLastFragmentSize, uMACfragNum,
                                                        byFBOption);
@@ -789,7 +763,7 @@ static u32 s_uFillDataHead(struct vnt_private *pDevice,
 
 static void s_vFillRTSHead(struct vnt_private *pDevice, u8 byPktType,
 	void *pvRTS, u32 cbFrameLength, int bNeedAck, int bDisCRC,
-	PSEthernetHeader psEthHeader, u16 wCurrentRate, u8 byFBOption)
+	struct ethhdr *psEthHeader, u16 wCurrentRate, u8 byFBOption)
 {
 	u32 uRTSFrameLen = 20;
 	u16 wLen = 0;
@@ -810,17 +784,17 @@ static void s_vFillRTSHead(struct vnt_private *pDevice, u8 byPktType,
             PSRTS_g pBuf = (PSRTS_g)pvRTS;
             //Get SignalField,ServiceField,Length
             BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
-                (PWORD)&(wLen), (PBYTE)&(pBuf->byServiceField_b), (PBYTE)&(pBuf->bySignalField_b)
+                (u16 *)&(wLen), (u8 *)&(pBuf->byServiceField_b), (u8 *)&(pBuf->bySignalField_b)
             );
             pBuf->wTransmitLength_b = cpu_to_le16(wLen);
             BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
-                (PWORD)&(wLen), (PBYTE)&(pBuf->byServiceField_a), (PBYTE)&(pBuf->bySignalField_a)
+                (u16 *)&(wLen), (u8 *)&(pBuf->byServiceField_a), (u8 *)&(pBuf->bySignalField_a)
             );
             pBuf->wTransmitLength_a = cpu_to_le16(wLen);
             //Get Duration
-            pBuf->wDuration_bb = cpu_to_le16((WORD)s_uGetRTSCTSDuration(pDevice, RTSDUR_BB, cbFrameLength, PK_TYPE_11B, pDevice->byTopCCKBasicRate, bNeedAck, byFBOption));    //0:RTSDuration_bb, 1:2.4G, 1:CCKData
-            pBuf->wDuration_aa = cpu_to_le16((WORD)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //2:RTSDuration_aa, 1:2.4G, 2,3: 2.4G OFDMData
-            pBuf->wDuration_ba = cpu_to_le16((WORD)s_uGetRTSCTSDuration(pDevice, RTSDUR_BA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //1:RTSDuration_ba, 1:2.4G, 2,3:2.4G OFDM Data
+            pBuf->wDuration_bb = cpu_to_le16((u16)s_uGetRTSCTSDuration(pDevice, RTSDUR_BB, cbFrameLength, PK_TYPE_11B, pDevice->byTopCCKBasicRate, bNeedAck, byFBOption));    //0:RTSDuration_bb, 1:2.4G, 1:CCKData
+            pBuf->wDuration_aa = cpu_to_le16((u16)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //2:RTSDuration_aa, 1:2.4G, 2,3: 2.4G OFDMData
+            pBuf->wDuration_ba = cpu_to_le16((u16)s_uGetRTSCTSDuration(pDevice, RTSDUR_BA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //1:RTSDuration_ba, 1:2.4G, 2,3:2.4G OFDM Data
 
             pBuf->Data.wDurationID = pBuf->wDuration_aa;
             //Get RTS Frame body
@@ -829,7 +803,7 @@ static void s_vFillRTSHead(struct vnt_private *pDevice, u8 byPktType,
 	if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
 	    (pDevice->eOPMode == OP_MODE_AP)) {
 		memcpy(&(pBuf->Data.abyRA[0]),
-		       &(psEthHeader->abyDstAddr[0]),
+		       &(psEthHeader->h_dest[0]),
 		       ETH_ALEN);
 	}
             else {
@@ -844,7 +818,7 @@ static void s_vFillRTSHead(struct vnt_private *pDevice, u8 byPktType,
 	}
             else {
 		    memcpy(&(pBuf->Data.abyTA[0]),
-			   &(psEthHeader->abySrcAddr[0]),
+			   &(psEthHeader->h_source[0]),
 			   ETH_ALEN);
             }
         }
@@ -852,21 +826,21 @@ static void s_vFillRTSHead(struct vnt_private *pDevice, u8 byPktType,
            PSRTS_g_FB pBuf = (PSRTS_g_FB)pvRTS;
             //Get SignalField,ServiceField,Length
             BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
-                (PWORD)&(wLen), (PBYTE)&(pBuf->byServiceField_b), (PBYTE)&(pBuf->bySignalField_b)
+                (u16 *)&(wLen), (u8 *)&(pBuf->byServiceField_b), (u8 *)&(pBuf->bySignalField_b)
             );
             pBuf->wTransmitLength_b = cpu_to_le16(wLen);
             BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
-                (PWORD)&(wLen), (PBYTE)&(pBuf->byServiceField_a), (PBYTE)&(pBuf->bySignalField_a)
+                (u16 *)&(wLen), (u8 *)&(pBuf->byServiceField_a), (u8 *)&(pBuf->bySignalField_a)
             );
             pBuf->wTransmitLength_a = cpu_to_le16(wLen);
             //Get Duration
-            pBuf->wDuration_bb = cpu_to_le16((WORD)s_uGetRTSCTSDuration(pDevice, RTSDUR_BB, cbFrameLength, PK_TYPE_11B, pDevice->byTopCCKBasicRate, bNeedAck, byFBOption));    //0:RTSDuration_bb, 1:2.4G, 1:CCKData
-            pBuf->wDuration_aa = cpu_to_le16((WORD)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //2:RTSDuration_aa, 1:2.4G, 2,3:2.4G OFDMData
-            pBuf->wDuration_ba = cpu_to_le16((WORD)s_uGetRTSCTSDuration(pDevice, RTSDUR_BA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //1:RTSDuration_ba, 1:2.4G, 2,3:2.4G OFDMData
-            pBuf->wRTSDuration_ba_f0 = cpu_to_le16((WORD)s_uGetRTSCTSDuration(pDevice, RTSDUR_BA_F0, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption));    //4:wRTSDuration_ba_f0, 1:2.4G, 1:CCKData
-            pBuf->wRTSDuration_aa_f0 = cpu_to_le16((WORD)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F0, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption));    //5:wRTSDuration_aa_f0, 1:2.4G, 1:CCKData
-            pBuf->wRTSDuration_ba_f1 = cpu_to_le16((WORD)s_uGetRTSCTSDuration(pDevice, RTSDUR_BA_F1, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption));    //6:wRTSDuration_ba_f1, 1:2.4G, 1:CCKData
-            pBuf->wRTSDuration_aa_f1 = cpu_to_le16((WORD)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F1, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption));    //7:wRTSDuration_aa_f1, 1:2.4G, 1:CCKData
+            pBuf->wDuration_bb = cpu_to_le16((u16)s_uGetRTSCTSDuration(pDevice, RTSDUR_BB, cbFrameLength, PK_TYPE_11B, pDevice->byTopCCKBasicRate, bNeedAck, byFBOption));    //0:RTSDuration_bb, 1:2.4G, 1:CCKData
+            pBuf->wDuration_aa = cpu_to_le16((u16)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //2:RTSDuration_aa, 1:2.4G, 2,3:2.4G OFDMData
+            pBuf->wDuration_ba = cpu_to_le16((u16)s_uGetRTSCTSDuration(pDevice, RTSDUR_BA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //1:RTSDuration_ba, 1:2.4G, 2,3:2.4G OFDMData
+            pBuf->wRTSDuration_ba_f0 = cpu_to_le16((u16)s_uGetRTSCTSDuration(pDevice, RTSDUR_BA_F0, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption));    //4:wRTSDuration_ba_f0, 1:2.4G, 1:CCKData
+            pBuf->wRTSDuration_aa_f0 = cpu_to_le16((u16)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F0, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption));    //5:wRTSDuration_aa_f0, 1:2.4G, 1:CCKData
+            pBuf->wRTSDuration_ba_f1 = cpu_to_le16((u16)s_uGetRTSCTSDuration(pDevice, RTSDUR_BA_F1, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption));    //6:wRTSDuration_ba_f1, 1:2.4G, 1:CCKData
+            pBuf->wRTSDuration_aa_f1 = cpu_to_le16((u16)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F1, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption));    //7:wRTSDuration_aa_f1, 1:2.4G, 1:CCKData
             pBuf->Data.wDurationID = pBuf->wDuration_aa;
             //Get RTS Frame body
             pBuf->Data.wFrameControl = TYPE_CTL_RTS;//0x00B4
@@ -874,7 +848,7 @@ static void s_vFillRTSHead(struct vnt_private *pDevice, u8 byPktType,
 	if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
 	    (pDevice->eOPMode == OP_MODE_AP)) {
 		memcpy(&(pBuf->Data.abyRA[0]),
-		       &(psEthHeader->abyDstAddr[0]),
+		       &(psEthHeader->h_dest[0]),
 		       ETH_ALEN);
 	}
             else {
@@ -890,7 +864,7 @@ static void s_vFillRTSHead(struct vnt_private *pDevice, u8 byPktType,
 	}
             else {
 		    memcpy(&(pBuf->Data.abyTA[0]),
-			   &(psEthHeader->abySrcAddr[0]),
+			   &(psEthHeader->h_source[0]),
 			   ETH_ALEN);
             }
 
@@ -901,11 +875,11 @@ static void s_vFillRTSHead(struct vnt_private *pDevice, u8 byPktType,
             PSRTS_ab pBuf = (PSRTS_ab)pvRTS;
             //Get SignalField,ServiceField,Length
             BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
-                (PWORD)&(wLen), (PBYTE)&(pBuf->byServiceField), (PBYTE)&(pBuf->bySignalField)
+                (u16 *)&(wLen), (u8 *)&(pBuf->byServiceField), (u8 *)&(pBuf->bySignalField)
             );
             pBuf->wTransmitLength = cpu_to_le16(wLen);
             //Get Duration
-            pBuf->wDuration = cpu_to_le16((WORD)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //0:RTSDuration_aa, 0:5G, 0: 5G OFDMData
+            pBuf->wDuration = cpu_to_le16((u16)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //0:RTSDuration_aa, 0:5G, 0: 5G OFDMData
     	    pBuf->Data.wDurationID = pBuf->wDuration;
             //Get RTS Frame body
             pBuf->Data.wFrameControl = TYPE_CTL_RTS;//0x00B4
@@ -913,7 +887,7 @@ static void s_vFillRTSHead(struct vnt_private *pDevice, u8 byPktType,
 	if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
 	    (pDevice->eOPMode == OP_MODE_AP)) {
 		memcpy(&(pBuf->Data.abyRA[0]),
-		       &(psEthHeader->abyDstAddr[0]),
+		       &(psEthHeader->h_dest[0]),
 		       ETH_ALEN);
 	} else {
 		memcpy(&(pBuf->Data.abyRA[0]),
@@ -927,7 +901,7 @@ static void s_vFillRTSHead(struct vnt_private *pDevice, u8 byPktType,
 		       ETH_ALEN);
 	} else {
 		memcpy(&(pBuf->Data.abyTA[0]),
-		       &(psEthHeader->abySrcAddr[0]),
+		       &(psEthHeader->h_source[0]),
 		       ETH_ALEN);
 	}
 
@@ -936,13 +910,13 @@ static void s_vFillRTSHead(struct vnt_private *pDevice, u8 byPktType,
             PSRTS_a_FB pBuf = (PSRTS_a_FB)pvRTS;
             //Get SignalField,ServiceField,Length
             BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
-                (PWORD)&(wLen), (PBYTE)&(pBuf->byServiceField), (PBYTE)&(pBuf->bySignalField)
+                (u16 *)&(wLen), (u8 *)&(pBuf->byServiceField), (u8 *)&(pBuf->bySignalField)
             );
             pBuf->wTransmitLength = cpu_to_le16(wLen);
             //Get Duration
-            pBuf->wDuration = cpu_to_le16((WORD)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //0:RTSDuration_aa, 0:5G, 0: 5G OFDMData
-    	    pBuf->wRTSDuration_f0 = cpu_to_le16((WORD)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F0, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //5:RTSDuration_aa_f0, 0:5G, 0: 5G OFDMData
-    	    pBuf->wRTSDuration_f1 = cpu_to_le16((WORD)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F1, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //7:RTSDuration_aa_f1, 0:5G, 0:
+            pBuf->wDuration = cpu_to_le16((u16)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //0:RTSDuration_aa, 0:5G, 0: 5G OFDMData
+    	    pBuf->wRTSDuration_f0 = cpu_to_le16((u16)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F0, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //5:RTSDuration_aa_f0, 0:5G, 0: 5G OFDMData
+    	    pBuf->wRTSDuration_f1 = cpu_to_le16((u16)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F1, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //7:RTSDuration_aa_f1, 0:5G, 0:
     	    pBuf->Data.wDurationID = pBuf->wDuration;
     	    //Get RTS Frame body
             pBuf->Data.wFrameControl = TYPE_CTL_RTS;//0x00B4
@@ -950,7 +924,7 @@ static void s_vFillRTSHead(struct vnt_private *pDevice, u8 byPktType,
 	if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
 	    (pDevice->eOPMode == OP_MODE_AP)) {
 		memcpy(&(pBuf->Data.abyRA[0]),
-		       &(psEthHeader->abyDstAddr[0]),
+		       &(psEthHeader->h_dest[0]),
 		       ETH_ALEN);
 	} else {
 		memcpy(&(pBuf->Data.abyRA[0]),
@@ -963,7 +937,7 @@ static void s_vFillRTSHead(struct vnt_private *pDevice, u8 byPktType,
 		       ETH_ALEN);
 	} else {
 		memcpy(&(pBuf->Data.abyTA[0]),
-		       &(psEthHeader->abySrcAddr[0]),
+		       &(psEthHeader->h_source[0]),
 		       ETH_ALEN);
 	}
         }
@@ -972,11 +946,11 @@ static void s_vFillRTSHead(struct vnt_private *pDevice, u8 byPktType,
         PSRTS_ab pBuf = (PSRTS_ab)pvRTS;
         //Get SignalField,ServiceField,Length
         BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
-            (PWORD)&(wLen), (PBYTE)&(pBuf->byServiceField), (PBYTE)&(pBuf->bySignalField)
+            (u16 *)&(wLen), (u8 *)&(pBuf->byServiceField), (u8 *)&(pBuf->bySignalField)
         );
         pBuf->wTransmitLength = cpu_to_le16(wLen);
         //Get Duration
-        pBuf->wDuration = cpu_to_le16((WORD)s_uGetRTSCTSDuration(pDevice, RTSDUR_BB, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //0:RTSDuration_bb, 1:2.4G, 1:CCKData
+        pBuf->wDuration = cpu_to_le16((u16)s_uGetRTSCTSDuration(pDevice, RTSDUR_BB, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //0:RTSDuration_bb, 1:2.4G, 1:CCKData
         pBuf->Data.wDurationID = pBuf->wDuration;
         //Get RTS Frame body
         pBuf->Data.wFrameControl = TYPE_CTL_RTS;//0x00B4
@@ -984,7 +958,7 @@ static void s_vFillRTSHead(struct vnt_private *pDevice, u8 byPktType,
 	if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
             (pDevice->eOPMode == OP_MODE_AP)) {
 		memcpy(&(pBuf->Data.abyRA[0]),
-		       &(psEthHeader->abyDstAddr[0]),
+		       &(psEthHeader->h_dest[0]),
 		       ETH_ALEN);
         }
         else {
@@ -999,7 +973,7 @@ static void s_vFillRTSHead(struct vnt_private *pDevice, u8 byPktType,
 		       ETH_ALEN);
 	} else {
 		memcpy(&(pBuf->Data.abyTA[0]),
-		       &(psEthHeader->abySrcAddr[0]),
+		       &(psEthHeader->h_source[0]),
 		       ETH_ALEN);
         }
     }
@@ -1028,18 +1002,18 @@ static void s_vFillCTSHead(struct vnt_private *pDevice, u32 uDMAIdx,
             PSCTS_FB pBuf = (PSCTS_FB)pvCTS;
             //Get SignalField,ServiceField,Length
             BBvCalculateParameter(pDevice, uCTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
-                (PWORD)&(wLen), (PBYTE)&(pBuf->byServiceField_b), (PBYTE)&(pBuf->bySignalField_b)
+                (u16 *)&(wLen), (u8 *)&(pBuf->byServiceField_b), (u8 *)&(pBuf->bySignalField_b)
             );
             pBuf->wTransmitLength_b = cpu_to_le16(wLen);
-            pBuf->wDuration_ba = (WORD)s_uGetRTSCTSDuration(pDevice, CTSDUR_BA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption); //3:CTSDuration_ba, 1:2.4G, 2,3:2.4G OFDM Data
+            pBuf->wDuration_ba = (u16)s_uGetRTSCTSDuration(pDevice, CTSDUR_BA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption); //3:CTSDuration_ba, 1:2.4G, 2,3:2.4G OFDM Data
             pBuf->wDuration_ba += pDevice->wCTSDuration;
             pBuf->wDuration_ba = cpu_to_le16(pBuf->wDuration_ba);
             //Get CTSDuration_ba_f0
-            pBuf->wCTSDuration_ba_f0 = (WORD)s_uGetRTSCTSDuration(pDevice, CTSDUR_BA_F0, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption); //8:CTSDuration_ba_f0, 1:2.4G, 2,3:2.4G OFDM Data
+            pBuf->wCTSDuration_ba_f0 = (u16)s_uGetRTSCTSDuration(pDevice, CTSDUR_BA_F0, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption); //8:CTSDuration_ba_f0, 1:2.4G, 2,3:2.4G OFDM Data
             pBuf->wCTSDuration_ba_f0 += pDevice->wCTSDuration;
             pBuf->wCTSDuration_ba_f0 = cpu_to_le16(pBuf->wCTSDuration_ba_f0);
             //Get CTSDuration_ba_f1
-            pBuf->wCTSDuration_ba_f1 = (WORD)s_uGetRTSCTSDuration(pDevice, CTSDUR_BA_F1, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption); //9:CTSDuration_ba_f1, 1:2.4G, 2,3:2.4G OFDM Data
+            pBuf->wCTSDuration_ba_f1 = (u16)s_uGetRTSCTSDuration(pDevice, CTSDUR_BA_F1, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption); //9:CTSDuration_ba_f1, 1:2.4G, 2,3:2.4G OFDM Data
             pBuf->wCTSDuration_ba_f1 += pDevice->wCTSDuration;
             pBuf->wCTSDuration_ba_f1 = cpu_to_le16(pBuf->wCTSDuration_ba_f1);
             //Get CTS Frame body
@@ -1053,11 +1027,11 @@ static void s_vFillCTSHead(struct vnt_private *pDevice, u32 uDMAIdx,
             PSCTS pBuf = (PSCTS)pvCTS;
             //Get SignalField,ServiceField,Length
             BBvCalculateParameter(pDevice, uCTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
-                (PWORD)&(wLen), (PBYTE)&(pBuf->byServiceField_b), (PBYTE)&(pBuf->bySignalField_b)
+                (u16 *)&(wLen), (u8 *)&(pBuf->byServiceField_b), (u8 *)&(pBuf->bySignalField_b)
             );
             pBuf->wTransmitLength_b = cpu_to_le16(wLen);
             //Get CTSDuration_ba
-            pBuf->wDuration_ba = cpu_to_le16((WORD)s_uGetRTSCTSDuration(pDevice, CTSDUR_BA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //3:CTSDuration_ba, 1:2.4G, 2,3:2.4G OFDM Data
+            pBuf->wDuration_ba = cpu_to_le16((u16)s_uGetRTSCTSDuration(pDevice, CTSDUR_BA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //3:CTSDuration_ba, 1:2.4G, 2,3:2.4G OFDM Data
             pBuf->wDuration_ba += pDevice->wCTSDuration;
             pBuf->wDuration_ba = cpu_to_le16(pBuf->wDuration_ba);
 
@@ -1098,7 +1072,7 @@ static void s_vFillCTSHead(struct vnt_private *pDevice, u32 uDMAIdx,
 static void s_vGenerateTxParameter(struct vnt_private *pDevice,
 	u8 byPktType, u16 wCurrentRate,	void *pTxBufHead, void *pvRrvTime,
 	void *pvRTS, void *pvCTS, u32 cbFrameSize, int bNeedACK, u32 uDMAIdx,
-	PSEthernetHeader psEthHeader)
+	struct ethhdr *psEthHeader)
 {
 	u32 cbMACHdLen = WLAN_HDR_ADDR3_LEN; /* 24 */
 	u16 wFifoCtl;
@@ -1130,11 +1104,11 @@ static void s_vGenerateTxParameter(struct vnt_private *pDevice,
             //Fill RsvTime
             if (pvRrvTime) {
                 PSRrvTime_gRTS pBuf = (PSRrvTime_gRTS)pvRrvTime;
-                pBuf->wRTSTxRrvTime_aa = cpu_to_le16((WORD)s_uGetRTSCTSRsvTime(pDevice, 2, byPktType, cbFrameSize, wCurrentRate));//2:RTSTxRrvTime_aa, 1:2.4GHz
-                pBuf->wRTSTxRrvTime_ba = cpu_to_le16((WORD)s_uGetRTSCTSRsvTime(pDevice, 1, byPktType, cbFrameSize, wCurrentRate));//1:RTSTxRrvTime_ba, 1:2.4GHz
-                pBuf->wRTSTxRrvTime_bb = cpu_to_le16((WORD)s_uGetRTSCTSRsvTime(pDevice, 0, byPktType, cbFrameSize, wCurrentRate));//0:RTSTxRrvTime_bb, 1:2.4GHz
-                pBuf->wTxRrvTime_a = cpu_to_le16((WORD) s_uGetTxRsvTime(pDevice, byPktType, cbFrameSize, wCurrentRate, bNeedACK));//2.4G OFDM
-                pBuf->wTxRrvTime_b = cpu_to_le16((WORD) s_uGetTxRsvTime(pDevice, PK_TYPE_11B, cbFrameSize, pDevice->byTopCCKBasicRate, bNeedACK));//1:CCK
+                pBuf->wRTSTxRrvTime_aa = cpu_to_le16((u16)s_uGetRTSCTSRsvTime(pDevice, 2, byPktType, cbFrameSize, wCurrentRate));//2:RTSTxRrvTime_aa, 1:2.4GHz
+                pBuf->wRTSTxRrvTime_ba = cpu_to_le16((u16)s_uGetRTSCTSRsvTime(pDevice, 1, byPktType, cbFrameSize, wCurrentRate));//1:RTSTxRrvTime_ba, 1:2.4GHz
+                pBuf->wRTSTxRrvTime_bb = cpu_to_le16((u16)s_uGetRTSCTSRsvTime(pDevice, 0, byPktType, cbFrameSize, wCurrentRate));//0:RTSTxRrvTime_bb, 1:2.4GHz
+                pBuf->wTxRrvTime_a = cpu_to_le16((u16) s_uGetTxRsvTime(pDevice, byPktType, cbFrameSize, wCurrentRate, bNeedACK));//2.4G OFDM
+                pBuf->wTxRrvTime_b = cpu_to_le16((u16) s_uGetTxRsvTime(pDevice, PK_TYPE_11B, cbFrameSize, pDevice->byTopCCKBasicRate, bNeedACK));//1:CCK
             }
             //Fill RTS
             s_vFillRTSHead(pDevice, byPktType, pvRTS, cbFrameSize, bNeedACK, bDisCRC, psEthHeader, wCurrentRate, byFBOption);
@@ -1144,9 +1118,9 @@ static void s_vGenerateTxParameter(struct vnt_private *pDevice,
             //Fill RsvTime
             if (pvRrvTime) {
                 PSRrvTime_gCTS pBuf = (PSRrvTime_gCTS)pvRrvTime;
-                pBuf->wTxRrvTime_a = cpu_to_le16((WORD)s_uGetTxRsvTime(pDevice, byPktType, cbFrameSize, wCurrentRate, bNeedACK));//2.4G OFDM
-                pBuf->wTxRrvTime_b = cpu_to_le16((WORD)s_uGetTxRsvTime(pDevice, PK_TYPE_11B, cbFrameSize, pDevice->byTopCCKBasicRate, bNeedACK));//1:CCK
-                pBuf->wCTSTxRrvTime_ba = cpu_to_le16((WORD)s_uGetRTSCTSRsvTime(pDevice, 3, byPktType, cbFrameSize, wCurrentRate));//3:CTSTxRrvTime_Ba, 1:2.4GHz
+                pBuf->wTxRrvTime_a = cpu_to_le16((u16)s_uGetTxRsvTime(pDevice, byPktType, cbFrameSize, wCurrentRate, bNeedACK));//2.4G OFDM
+                pBuf->wTxRrvTime_b = cpu_to_le16((u16)s_uGetTxRsvTime(pDevice, PK_TYPE_11B, cbFrameSize, pDevice->byTopCCKBasicRate, bNeedACK));//1:CCK
+                pBuf->wCTSTxRrvTime_ba = cpu_to_le16((u16)s_uGetRTSCTSRsvTime(pDevice, 3, byPktType, cbFrameSize, wCurrentRate));//3:CTSTxRrvTime_Ba, 1:2.4GHz
             }
             //Fill CTS
             s_vFillCTSHead(pDevice, uDMAIdx, byPktType, pvCTS, cbFrameSize, bNeedACK, bDisCRC, wCurrentRate, byFBOption);
@@ -1158,8 +1132,8 @@ static void s_vGenerateTxParameter(struct vnt_private *pDevice,
             //Fill RsvTime
             if (pvRrvTime) {
                 PSRrvTime_ab pBuf = (PSRrvTime_ab)pvRrvTime;
-                pBuf->wRTSTxRrvTime = cpu_to_le16((WORD)s_uGetRTSCTSRsvTime(pDevice, 2, byPktType, cbFrameSize, wCurrentRate));//2:RTSTxRrvTime_aa, 0:5GHz
-                pBuf->wTxRrvTime = cpu_to_le16((WORD)s_uGetTxRsvTime(pDevice, byPktType, cbFrameSize, wCurrentRate, bNeedACK));//0:OFDM
+                pBuf->wRTSTxRrvTime = cpu_to_le16((u16)s_uGetRTSCTSRsvTime(pDevice, 2, byPktType, cbFrameSize, wCurrentRate));//2:RTSTxRrvTime_aa, 0:5GHz
+                pBuf->wTxRrvTime = cpu_to_le16((u16)s_uGetTxRsvTime(pDevice, byPktType, cbFrameSize, wCurrentRate, bNeedACK));//0:OFDM
             }
             //Fill RTS
             s_vFillRTSHead(pDevice, byPktType, pvRTS, cbFrameSize, bNeedACK, bDisCRC, psEthHeader, wCurrentRate, byFBOption);
@@ -1168,7 +1142,7 @@ static void s_vGenerateTxParameter(struct vnt_private *pDevice,
             //Fill RsvTime
             if (pvRrvTime) {
                 PSRrvTime_ab pBuf = (PSRrvTime_ab)pvRrvTime;
-                pBuf->wTxRrvTime = cpu_to_le16((WORD)s_uGetTxRsvTime(pDevice, PK_TYPE_11A, cbFrameSize, wCurrentRate, bNeedACK)); //0:OFDM
+                pBuf->wTxRrvTime = cpu_to_le16((u16)s_uGetTxRsvTime(pDevice, PK_TYPE_11A, cbFrameSize, wCurrentRate, bNeedACK)); //0:OFDM
             }
         }
     }
@@ -1178,8 +1152,8 @@ static void s_vGenerateTxParameter(struct vnt_private *pDevice,
             //Fill RsvTime
             if (pvRrvTime) {
                 PSRrvTime_ab pBuf = (PSRrvTime_ab)pvRrvTime;
-                pBuf->wRTSTxRrvTime = cpu_to_le16((WORD)s_uGetRTSCTSRsvTime(pDevice, 0, byPktType, cbFrameSize, wCurrentRate));//0:RTSTxRrvTime_bb, 1:2.4GHz
-                pBuf->wTxRrvTime = cpu_to_le16((WORD)s_uGetTxRsvTime(pDevice, PK_TYPE_11B, cbFrameSize, wCurrentRate, bNeedACK));//1:CCK
+                pBuf->wRTSTxRrvTime = cpu_to_le16((u16)s_uGetRTSCTSRsvTime(pDevice, 0, byPktType, cbFrameSize, wCurrentRate));//0:RTSTxRrvTime_bb, 1:2.4GHz
+                pBuf->wTxRrvTime = cpu_to_le16((u16)s_uGetTxRsvTime(pDevice, PK_TYPE_11B, cbFrameSize, wCurrentRate, bNeedACK));//1:CCK
             }
             //Fill RTS
             s_vFillRTSHead(pDevice, byPktType, pvRTS, cbFrameSize, bNeedACK, bDisCRC, psEthHeader, wCurrentRate, byFBOption);
@@ -1188,21 +1162,21 @@ static void s_vGenerateTxParameter(struct vnt_private *pDevice,
             //Fill RsvTime
             if (pvRrvTime) {
                 PSRrvTime_ab pBuf = (PSRrvTime_ab)pvRrvTime;
-                pBuf->wTxRrvTime = cpu_to_le16((WORD)s_uGetTxRsvTime(pDevice, PK_TYPE_11B, cbFrameSize, wCurrentRate, bNeedACK)); //1:CCK
+                pBuf->wTxRrvTime = cpu_to_le16((u16)s_uGetTxRsvTime(pDevice, PK_TYPE_11B, cbFrameSize, wCurrentRate, bNeedACK)); //1:CCK
             }
         }
     }
     //DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"s_vGenerateTxParameter END.\n");
 }
 /*
-    PBYTE pbyBuffer,//point to pTxBufHead
-    WORD  wFragType,//00:Non-Frag, 01:Start, 02:Mid, 03:Last
+    u8 * pbyBuffer,//point to pTxBufHead
+    u16  wFragType,//00:Non-Frag, 01:Start, 02:Mid, 03:Last
     unsigned int  cbFragmentSize,//Hdr+payoad+FCS
 */
 
 static int s_bPacketToWirelessUsb(struct vnt_private *pDevice, u8 byPktType,
 	u8 *usbPacketBuf, int bNeedEncryption, u32 uSkbPacketLen, u32 uDMAIdx,
-	PSEthernetHeader psEthHeader, u8 *pPacket, PSKeyItem pTransmitKey,
+	struct ethhdr *psEthHeader, u8 *pPacket, PSKeyItem pTransmitKey,
 	u32 uNodeIndex, u16 wCurrentRate, u32 *pcbHeaderLen, u32 *pcbTotalLen)
 {
 	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
@@ -1240,7 +1214,7 @@ static int s_bPacketToWirelessUsb(struct vnt_private *pDevice, u8 byPktType,
     memset(pTxBufHead, 0, sizeof(TX_BUFFER));
 
     // Get pkt type
-    if (ntohs(psEthHeader->wType) > ETH_DATA_LEN) {
+    if (ntohs(psEthHeader->h_proto) > ETH_DATA_LEN) {
         if (pDevice->dwDiagRefCount == 0) {
             cb802_1_H_len = 8;
         } else {
@@ -1253,7 +1227,7 @@ static int s_bPacketToWirelessUsb(struct vnt_private *pDevice, u8 byPktType,
     cbFrameBodySize = uSkbPacketLen - ETH_HLEN + cb802_1_H_len;
 
     //Set packet type
-    pTxBufHead->wFIFOCtl |= (WORD)(byPktType<<8);
+    pTxBufHead->wFIFOCtl |= (u16)(byPktType<<8);
 
     if (pDevice->dwDiagRefCount != 0) {
         bNeedACK = false;
@@ -1261,7 +1235,7 @@ static int s_bPacketToWirelessUsb(struct vnt_private *pDevice, u8 byPktType,
     } else { //if (pDevice->dwDiagRefCount != 0) {
 	if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
 	    (pDevice->eOPMode == OP_MODE_AP)) {
-		if (is_multicast_ether_addr(psEthHeader->abyDstAddr)) {
+		if (is_multicast_ether_addr(psEthHeader->h_dest)) {
 			bNeedACK = false;
 			pTxBufHead->wFIFOCtl =
 				pTxBufHead->wFIFOCtl & (~FIFOCTL_NEEDACK);
@@ -1293,7 +1267,7 @@ static int s_bPacketToWirelessUsb(struct vnt_private *pDevice, u8 byPktType,
     } else {
         cbMACHdLen = WLAN_HDR_ADDR3_LEN;
     }
-    pTxBufHead->wFragCtl |= (WORD)(cbMACHdLen << 10);
+    pTxBufHead->wFragCtl |= (u16)(cbMACHdLen << 10);
 
     //Set FIFOCTL_GrpAckPolicy
     if (pDevice->bGrpAckPolicy == true) {//0000 0100 0000 0000
@@ -1326,7 +1300,6 @@ static int s_bPacketToWirelessUsb(struct vnt_private *pDevice, u8 byPktType,
         }
     }
 
-
     if ((bNeedEncryption) && (pTransmitKey != NULL))  {
         if (pTransmitKey->byCipherSuite == KEY_CTL_WEP) {
             cbIVlen = 4;
@@ -1358,7 +1331,7 @@ static int s_bPacketToWirelessUsb(struct vnt_private *pDevice, u8 byPktType,
         pTxBufHead->wFIFOCtl |= (FIFOCTL_RTS | FIFOCTL_LRETRY);
     }
 
-    pbyTxBufferAddr = (PBYTE) &(pTxBufHead->adwTxKey[0]);
+    pbyTxBufferAddr = (u8 *) &(pTxBufHead->adwTxKey[0]);
     wTxBufSize = sizeof(STxBufHead);
     if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {//802.11g packet
         if (byFBOption == AUTO_FB_NONE) {
@@ -1437,10 +1410,9 @@ static int s_bPacketToWirelessUsb(struct vnt_private *pDevice, u8 byPktType,
         } // Auto Fall Back
     }
 
-    pbyMacHdr = (PBYTE)(pbyTxBufferAddr + cbHeaderLength);
-    pbyIVHead = (PBYTE)(pbyMacHdr + cbMACHdLen + uPadding);
-    pbyPayloadHead = (PBYTE)(pbyMacHdr + cbMACHdLen + uPadding + cbIVlen);
-
+    pbyMacHdr = (u8 *)(pbyTxBufferAddr + cbHeaderLength);
+    pbyIVHead = (u8 *)(pbyMacHdr + cbMACHdLen + uPadding);
+    pbyPayloadHead = (u8 *)(pbyMacHdr + cbMACHdLen + uPadding + cbIVlen);
 
     //=========================
     //    No Fragmentation
@@ -1450,7 +1422,6 @@ static int s_bPacketToWirelessUsb(struct vnt_private *pDevice, u8 byPktType,
     //uDMAIdx = TYPE_AC0DMA;
     //pTxBufHead = (PSTxBufHead) &(pTxBufHead->adwTxKey[0]);
 
-
     //Fill FIFO,RrvTime,RTS,and CTS
     s_vGenerateTxParameter(pDevice, byPktType, wCurrentRate,
 			   (void *)pbyTxBufferAddr, pvRrvTime, pvRTS, pvCTS,
@@ -1459,13 +1430,13 @@ static int s_bPacketToWirelessUsb(struct vnt_private *pDevice, u8 byPktType,
     uDuration = s_uFillDataHead(pDevice, byPktType, wCurrentRate, pvTxDataHd, cbFrameSize, uDMAIdx, bNeedACK,
                                     0, 0, 1/*uMACfragNum*/, byFBOption);
     // Generate TX MAC Header
-    s_vGenerateMACHeader(pDevice, pbyMacHdr, (WORD)uDuration, psEthHeader, bNeedEncryption,
+    s_vGenerateMACHeader(pDevice, pbyMacHdr, (u16)uDuration, psEthHeader, bNeedEncryption,
                            byFragType, uDMAIdx, 0);
 
     if (bNeedEncryption == true) {
         //Fill TXKEY
-        s_vFillTxKey(pDevice, (PBYTE)(pTxBufHead->adwTxKey), pbyIVHead, pTransmitKey,
-                         pbyMacHdr, (WORD)cbFrameBodySize, (PBYTE)pMICHDR);
+        s_vFillTxKey(pDevice, (u8 *)(pTxBufHead->adwTxKey), pbyIVHead, pTransmitKey,
+                         pbyMacHdr, (u16)cbFrameBodySize, (u8 *)pMICHDR);
 
         if (pDevice->bEnableHostWEP) {
             pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16 = pTransmitKey->dwTSC47_16;
@@ -1474,25 +1445,24 @@ static int s_bPacketToWirelessUsb(struct vnt_private *pDevice, u8 byPktType,
     }
 
     // 802.1H
-    if (ntohs(psEthHeader->wType) > ETH_DATA_LEN) {
+    if (ntohs(psEthHeader->h_proto) > ETH_DATA_LEN) {
 	if (pDevice->dwDiagRefCount == 0) {
-		if ((psEthHeader->wType == cpu_to_be16(ETH_P_IPX)) ||
-		    (psEthHeader->wType == cpu_to_le16(0xF380))) {
-			memcpy((PBYTE) (pbyPayloadHead),
+		if ((psEthHeader->h_proto == cpu_to_be16(ETH_P_IPX)) ||
+		    (psEthHeader->h_proto == cpu_to_le16(0xF380))) {
+			memcpy((u8 *) (pbyPayloadHead),
 			       abySNAP_Bridgetunnel, 6);
             } else {
-                memcpy((PBYTE) (pbyPayloadHead), &abySNAP_RFC1042[0], 6);
+                memcpy((u8 *) (pbyPayloadHead), &abySNAP_RFC1042[0], 6);
             }
-            pbyType = (PBYTE) (pbyPayloadHead + 6);
-            memcpy(pbyType, &(psEthHeader->wType), sizeof(WORD));
+            pbyType = (u8 *) (pbyPayloadHead + 6);
+            memcpy(pbyType, &(psEthHeader->h_proto), sizeof(u16));
         } else {
-            memcpy((PBYTE) (pbyPayloadHead), &(psEthHeader->wType), sizeof(WORD));
+            memcpy((u8 *) (pbyPayloadHead), &(psEthHeader->h_proto), sizeof(u16));
 
         }
 
     }
 
-
     if (pPacket != NULL) {
         // Copy the Packet into a tx Buffer
         memcpy((pbyPayloadHead + cb802_1_H_len),
@@ -1502,7 +1472,7 @@ static int s_bPacketToWirelessUsb(struct vnt_private *pDevice, u8 byPktType,
 
     } else {
         // while bRelayPacketSend psEthHeader is point to header+payload
-        memcpy((pbyPayloadHead + cb802_1_H_len), ((PBYTE)psEthHeader) + ETH_HLEN, uSkbPacketLen - ETH_HLEN);
+        memcpy((pbyPayloadHead + cb802_1_H_len), ((u8 *)psEthHeader) + ETH_HLEN, uSkbPacketLen - ETH_HLEN);
     }
 
     ASSERT(uLength == cbNdisBodySize);
@@ -1516,18 +1486,18 @@ static int s_bPacketToWirelessUsb(struct vnt_private *pDevice, u8 byPktType,
 		dwMICKey1 = *(u32 *)(&pTransmitKey->abyKey[20]);
 	}
         else if ((pTransmitKey->dwKeyIndex & AUTHENTICATOR_KEY) != 0) {
-            dwMICKey0 = *(PDWORD)(&pTransmitKey->abyKey[16]);
-            dwMICKey1 = *(PDWORD)(&pTransmitKey->abyKey[20]);
+            dwMICKey0 = *(u32 *)(&pTransmitKey->abyKey[16]);
+            dwMICKey1 = *(u32 *)(&pTransmitKey->abyKey[20]);
         }
         else {
-            dwMICKey0 = *(PDWORD)(&pTransmitKey->abyKey[24]);
-            dwMICKey1 = *(PDWORD)(&pTransmitKey->abyKey[28]);
+            dwMICKey0 = *(u32 *)(&pTransmitKey->abyKey[24]);
+            dwMICKey1 = *(u32 *)(&pTransmitKey->abyKey[28]);
         }
         // DO Software Michael
         MIC_vInit(dwMICKey0, dwMICKey1);
-        MIC_vAppend((PBYTE)&(psEthHeader->abyDstAddr[0]), 12);
+        MIC_vAppend((u8 *)&(psEthHeader->h_dest[0]), 12);
         dwMIC_Priority = 0;
-        MIC_vAppend((PBYTE)&dwMIC_Priority, 4);
+        MIC_vAppend((u8 *)&dwMIC_Priority, 4);
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"MIC KEY: %X, %X\n",
 		dwMICKey0, dwMICKey1);
 
@@ -1535,14 +1505,14 @@ static int s_bPacketToWirelessUsb(struct vnt_private *pDevice, u8 byPktType,
 
         //DBG_PRN_GRP12(("Length:%d, %d\n", cbFrameBodySize, uFromHDtoPLDLength));
         //for (ii = 0; ii < cbFrameBodySize; ii++) {
-        //    DBG_PRN_GRP12(("%02x ", *((PBYTE)((pbyPayloadHead + cb802_1_H_len) + ii))));
+        //    DBG_PRN_GRP12(("%02x ", *((u8 *)((pbyPayloadHead + cb802_1_H_len) + ii))));
         //}
         //DBG_PRN_GRP12(("\n\n\n"));
 
         MIC_vAppend(pbyPayloadHead, cbFrameBodySize);
 
-        pdwMIC_L = (PDWORD)(pbyPayloadHead + cbFrameBodySize);
-        pdwMIC_R = (PDWORD)(pbyPayloadHead + cbFrameBodySize + 4);
+        pdwMIC_L = (u32 *)(pbyPayloadHead + cbFrameBodySize);
+        pdwMIC_R = (u32 *)(pbyPayloadHead + cbFrameBodySize + 4);
 
         MIC_vGetMIC(pdwMIC_L, pdwMIC_R);
         MIC_vUnInit();
@@ -1557,10 +1527,9 @@ static int s_bPacketToWirelessUsb(struct vnt_private *pDevice, u8 byPktType,
         //DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"MIC:%lX, %lX\n", *pdwMIC_L, *pdwMIC_R);
     }
 
-
     if (bSoftWEP == true) {
 
-        s_vSWencryption(pDevice, pTransmitKey, (pbyPayloadHead), (WORD)(cbFrameBodySize + cbMIClen));
+        s_vSWencryption(pDevice, pTransmitKey, (pbyPayloadHead), (u16)(cbFrameBodySize + cbMIClen));
 
     } else if (  ((pDevice->eEncryptionStatus == Ndis802_11Encryption1Enabled) && (bNeedEncryption == true))  ||
           ((pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled) && (bNeedEncryption == true))   ||
@@ -1570,13 +1539,13 @@ static int s_bPacketToWirelessUsb(struct vnt_private *pDevice, u8 byPktType,
 
     if (pDevice->bSoftwareGenCrcErr == true) {
 	unsigned int cbLen;
-        PDWORD pdwCRC;
+        u32 * pdwCRC;
 
         dwCRC = 0xFFFFFFFFL;
         cbLen = cbFrameSize - cbFCSlen;
         // calculate CRC, and wrtie CRC value to end of TD
         dwCRC = CRCdwGetCrc32Ex(pbyMacHdr, cbLen, dwCRC);
-        pdwCRC = (PDWORD)(pbyMacHdr + cbLen);
+        pdwCRC = (u32 *)(pbyMacHdr + cbLen);
         // finally, we must invert dwCRC to get the correct answer
         *pdwCRC = ~dwCRC;
         // Force Error
@@ -1588,16 +1557,13 @@ static int s_bPacketToWirelessUsb(struct vnt_private *pDevice, u8 byPktType,
     *pcbHeaderLen = cbHeaderLength;
     *pcbTotalLen = cbHeaderLength + cbFrameSize ;
 
-
     //Set FragCtl in TxBufferHead
-    pTxBufHead->wFragCtl |= (WORD)byFragType;
-
+    pTxBufHead->wFragCtl |= (u16)byFragType;
 
     return true;
 
 }
 
-
 /*+
  *
  * Description:
@@ -1618,67 +1584,67 @@ static int s_bPacketToWirelessUsb(struct vnt_private *pDevice, u8 byPktType,
 -*/
 
 static void s_vGenerateMACHeader(struct vnt_private *pDevice,
-	u8 *pbyBufferAddr, u16 wDuration, PSEthernetHeader psEthHeader,
+	u8 *pbyBufferAddr, u16 wDuration, struct ethhdr *psEthHeader,
 	int bNeedEncrypt, u16 wFragType, u32 uDMAIdx, u32 uFragIdx)
 {
-	PS802_11Header pMACHeader = (PS802_11Header)pbyBufferAddr;
+	struct ieee80211_hdr *pMACHeader = (struct ieee80211_hdr *)pbyBufferAddr;
 
-    memset(pMACHeader, 0, (sizeof(S802_11Header)));  //- sizeof(pMACHeader->dwIV)));
+    memset(pMACHeader, 0, (sizeof(struct ieee80211_hdr)));
 
     if (uDMAIdx == TYPE_ATIMDMA) {
-    	pMACHeader->wFrameCtl = TYPE_802_11_ATIM;
+    	pMACHeader->frame_control = TYPE_802_11_ATIM;
     } else {
-        pMACHeader->wFrameCtl = TYPE_802_11_DATA;
+        pMACHeader->frame_control = TYPE_802_11_DATA;
     }
 
     if (pDevice->eOPMode == OP_MODE_AP) {
-	memcpy(&(pMACHeader->abyAddr1[0]),
-	       &(psEthHeader->abyDstAddr[0]),
+	memcpy(&(pMACHeader->addr1[0]),
+	       &(psEthHeader->h_dest[0]),
 	       ETH_ALEN);
-	memcpy(&(pMACHeader->abyAddr2[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-	memcpy(&(pMACHeader->abyAddr3[0]),
-	       &(psEthHeader->abySrcAddr[0]),
+	memcpy(&(pMACHeader->addr2[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
+	memcpy(&(pMACHeader->addr3[0]),
+	       &(psEthHeader->h_source[0]),
 	       ETH_ALEN);
-        pMACHeader->wFrameCtl |= FC_FROMDS;
+        pMACHeader->frame_control |= FC_FROMDS;
     } else {
 	if (pDevice->eOPMode == OP_MODE_ADHOC) {
-		memcpy(&(pMACHeader->abyAddr1[0]),
-		       &(psEthHeader->abyDstAddr[0]),
+		memcpy(&(pMACHeader->addr1[0]),
+		       &(psEthHeader->h_dest[0]),
 		       ETH_ALEN);
-		memcpy(&(pMACHeader->abyAddr2[0]),
-		       &(psEthHeader->abySrcAddr[0]),
+		memcpy(&(pMACHeader->addr2[0]),
+		       &(psEthHeader->h_source[0]),
 		       ETH_ALEN);
-		memcpy(&(pMACHeader->abyAddr3[0]),
+		memcpy(&(pMACHeader->addr3[0]),
 		       &(pDevice->abyBSSID[0]),
 		       ETH_ALEN);
 	} else {
-		memcpy(&(pMACHeader->abyAddr3[0]),
-		       &(psEthHeader->abyDstAddr[0]),
+		memcpy(&(pMACHeader->addr3[0]),
+		       &(psEthHeader->h_dest[0]),
 		       ETH_ALEN);
-		memcpy(&(pMACHeader->abyAddr2[0]),
-		       &(psEthHeader->abySrcAddr[0]),
+		memcpy(&(pMACHeader->addr2[0]),
+		       &(psEthHeader->h_source[0]),
 		       ETH_ALEN);
-		memcpy(&(pMACHeader->abyAddr1[0]),
+		memcpy(&(pMACHeader->addr1[0]),
 		       &(pDevice->abyBSSID[0]),
 		       ETH_ALEN);
-            pMACHeader->wFrameCtl |= FC_TODS;
+            pMACHeader->frame_control |= FC_TODS;
         }
     }
 
     if (bNeedEncrypt)
-        pMACHeader->wFrameCtl |= cpu_to_le16((WORD)WLAN_SET_FC_ISWEP(1));
+        pMACHeader->frame_control |= cpu_to_le16((u16)WLAN_SET_FC_ISWEP(1));
 
-    pMACHeader->wDurationID = cpu_to_le16(wDuration);
+    pMACHeader->duration_id = cpu_to_le16(wDuration);
 
     if (pDevice->bLongHeader) {
         PWLAN_80211HDR_A4 pMACA4Header  = (PWLAN_80211HDR_A4) pbyBufferAddr;
-        pMACHeader->wFrameCtl |= (FC_TODS | FC_FROMDS);
+        pMACHeader->frame_control |= (FC_TODS | FC_FROMDS);
         memcpy(pMACA4Header->abyAddr4, pDevice->abyBSSID, WLAN_ADDR_LEN);
     }
-    pMACHeader->wSeqCtl = cpu_to_le16(pDevice->wSeqCounter << 4);
+    pMACHeader->seq_ctrl = cpu_to_le16(pDevice->wSeqCounter << 4);
 
     //Set FragNumber in Sequence Control
-    pMACHeader->wSeqCtl |= cpu_to_le16((WORD)uFragIdx);
+    pMACHeader->seq_ctrl |= cpu_to_le16((u16)uFragIdx);
 
     if ((wFragType == FRAGCTL_ENDFRAG) || (wFragType == FRAGCTL_NONFRAG)) {
         pDevice->wSeqCounter++;
@@ -1687,12 +1653,10 @@ static void s_vGenerateMACHeader(struct vnt_private *pDevice,
     }
 
     if ((wFragType == FRAGCTL_STAFRAG) || (wFragType == FRAGCTL_MIDFRAG)) { //StartFrag or MidFrag
-        pMACHeader->wFrameCtl |= FC_MOREFRAG;
+        pMACHeader->frame_control |= FC_MOREFRAG;
     }
 }
 
-
-
 /*+
  *
  * Description:
@@ -1717,9 +1681,9 @@ CMD_STATUS csMgmt_xmit(struct vnt_private *pDevice,
 	PTX_BUFFER pTX_Buffer;
 	PSTxBufHead pTxBufHead;
 	PUSB_SEND_CONTEXT pContext;
-	PS802_11Header pMACHeader;
+	struct ieee80211_hdr *pMACHeader;
 	PSCTS pCTS;
-	SEthernetHeader sEthHeader;
+	struct ethhdr sEthHeader;
 	u8 byPktType, *pbyTxBufferAddr;
 	void *pvRTS, *pvTxDataHd, *pvRrvTime, *pMICHDR;
 	u32 uDuration, cbReqCount, cbHeaderSize, cbFrameBodySize, cbFrameSize;
@@ -1730,8 +1694,6 @@ CMD_STATUS csMgmt_xmit(struct vnt_private *pDevice,
 	u32 cbMacHdLen;
 	u16 wCurrentRate = RATE_1M;
 
-
-
     pContext = (PUSB_SEND_CONTEXT)s_vGetFreeContext(pDevice);
 
     if (NULL == pContext) {
@@ -1740,7 +1702,7 @@ CMD_STATUS csMgmt_xmit(struct vnt_private *pDevice,
     }
 
     pTX_Buffer = (PTX_BUFFER) (&pContext->Data[0]);
-    pbyTxBufferAddr = (PBYTE)&(pTX_Buffer->adwTxKey[0]);
+    pbyTxBufferAddr = (u8 *)&(pTX_Buffer->adwTxKey[0]);
     cbFrameBodySize = pPacket->cbPayloadLen;
     pTxBufHead = (PSTxBufHead) pbyTxBufferAddr;
     wTxBufSize = sizeof(STxBufHead);
@@ -1765,7 +1727,6 @@ CMD_STATUS csMgmt_xmit(struct vnt_private *pDevice,
     }
     pDevice->wCurrentRate = wCurrentRate;
 
-
     //Set packet type
     if (byPktType == PK_TYPE_11A) {//0000 0000 0000 0000
         pTxBufHead->wFIFOCtl = 0;
@@ -1814,7 +1775,7 @@ CMD_STATUS csMgmt_xmit(struct vnt_private *pDevice,
     }
 
     //Set FRAGCTL_MACHDCNT
-    pTxBufHead->wFragCtl |= cpu_to_le16((WORD)(cbMacHdLen << 10));
+    pTxBufHead->wFragCtl |= cpu_to_le16((u16)(cbMacHdLen << 10));
 
     // Notes:
     // Although spec says MMPDU can be fragmented; In most case,
@@ -1877,17 +1838,16 @@ CMD_STATUS csMgmt_xmit(struct vnt_private *pDevice,
     memset((void *)(pbyTxBufferAddr + wTxBufSize), 0,
 	   (cbHeaderSize - wTxBufSize));
 
-    memcpy(&(sEthHeader.abyDstAddr[0]),
+    memcpy(&(sEthHeader.h_dest[0]),
 	   &(pPacket->p80211Header->sA3.abyAddr1[0]),
 	   ETH_ALEN);
-    memcpy(&(sEthHeader.abySrcAddr[0]),
+    memcpy(&(sEthHeader.h_source[0]),
 	   &(pPacket->p80211Header->sA3.abyAddr2[0]),
 	   ETH_ALEN);
     //=========================
     //    No Fragmentation
     //=========================
-    pTxBufHead->wFragCtl |= (WORD)FRAGCTL_NONFRAG;
-
+    pTxBufHead->wFragCtl |= (u16)FRAGCTL_NONFRAG;
 
     //Fill FIFO,RrvTime,RTS,and CTS
     s_vGenerateTxParameter(pDevice, byPktType, wCurrentRate,  pbyTxBufferAddr, pvRrvTime, pvRTS, pCTS,
@@ -1897,18 +1857,18 @@ CMD_STATUS csMgmt_xmit(struct vnt_private *pDevice,
     uDuration = s_uFillDataHead(pDevice, byPktType, wCurrentRate, pvTxDataHd, cbFrameSize, TYPE_TXDMA0, bNeedACK,
                                 0, 0, 1, AUTO_FB_NONE);
 
-    pMACHeader = (PS802_11Header) (pbyTxBufferAddr + cbHeaderSize);
+    pMACHeader = (struct ieee80211_hdr *) (pbyTxBufferAddr + cbHeaderSize);
 
     cbReqCount = cbHeaderSize + cbMacHdLen + uPadding + cbIVlen + cbFrameBodySize;
 
     if (WLAN_GET_FC_ISWEP(pPacket->p80211Header->sA4.wFrameCtl) != 0) {
-        PBYTE           pbyIVHead;
-        PBYTE           pbyPayloadHead;
-        PBYTE           pbyBSSID;
+        u8 *           pbyIVHead;
+        u8 *           pbyPayloadHead;
+        u8 *           pbyBSSID;
         PSKeyItem       pTransmitKey = NULL;
 
-        pbyIVHead = (PBYTE)(pbyTxBufferAddr + cbHeaderSize + cbMacHdLen + uPadding);
-        pbyPayloadHead = (PBYTE)(pbyTxBufferAddr + cbHeaderSize + cbMacHdLen + uPadding + cbIVlen);
+        pbyIVHead = (u8 *)(pbyTxBufferAddr + cbHeaderSize + cbMacHdLen + uPadding);
+        pbyPayloadHead = (u8 *)(pbyTxBufferAddr + cbHeaderSize + cbMacHdLen + uPadding + cbIVlen);
         do {
             if ((pDevice->eOPMode == OP_MODE_INFRASTRUCTURE) &&
                 (pDevice->bLinkPass == true)) {
@@ -1935,11 +1895,11 @@ CMD_STATUS csMgmt_xmit(struct vnt_private *pDevice,
             }
         } while(false);
         //Fill TXKEY
-        s_vFillTxKey(pDevice, (PBYTE)(pTxBufHead->adwTxKey), pbyIVHead, pTransmitKey,
-                     (PBYTE)pMACHeader, (WORD)cbFrameBodySize, NULL);
+        s_vFillTxKey(pDevice, (u8 *)(pTxBufHead->adwTxKey), pbyIVHead, pTransmitKey,
+                     (u8 *)pMACHeader, (u16)cbFrameBodySize, NULL);
 
         memcpy(pMACHeader, pPacket->p80211Header, cbMacHdLen);
-        memcpy(pbyPayloadHead, ((PBYTE)(pPacket->p80211Header) + cbMacHdLen),
+        memcpy(pbyPayloadHead, ((u8 *)(pPacket->p80211Header) + cbMacHdLen),
                  cbFrameBodySize);
     }
     else {
@@ -1947,7 +1907,7 @@ CMD_STATUS csMgmt_xmit(struct vnt_private *pDevice,
         memcpy(pMACHeader, pPacket->p80211Header, pPacket->cbMPDULen);
     }
 
-    pMACHeader->wSeqCtl = cpu_to_le16(pDevice->wSeqCounter << 4);
+    pMACHeader->seq_ctrl = cpu_to_le16(pDevice->wSeqCounter << 4);
     pDevice->wSeqCounter++ ;
     if (pDevice->wSeqCounter > 0x0fff)
         pDevice->wSeqCounter = 0;
@@ -1966,27 +1926,25 @@ CMD_STATUS csMgmt_xmit(struct vnt_private *pDevice,
         }
     }
 
-
-    pTX_Buffer->wTxByteCount = cpu_to_le16((WORD)(cbReqCount));
-    pTX_Buffer->byPKTNO = (BYTE) (((wCurrentRate<<4) &0x00F0) | ((pDevice->wSeqCounter - 1) & 0x000F));
+    pTX_Buffer->wTxByteCount = cpu_to_le16((u16)(cbReqCount));
+    pTX_Buffer->byPKTNO = (u8) (((wCurrentRate<<4) &0x00F0) | ((pDevice->wSeqCounter - 1) & 0x000F));
     pTX_Buffer->byType = 0x00;
 
     pContext->pPacket = NULL;
     pContext->Type = CONTEXT_MGMT_PACKET;
-    pContext->uBufLen = (WORD)cbReqCount + 4;  //USB header
+    pContext->uBufLen = (u16)cbReqCount + 4;  //USB header
 
-    if (WLAN_GET_FC_TODS(pMACHeader->wFrameCtl) == 0) {
-        s_vSaveTxPktInfo(pDevice, (BYTE) (pTX_Buffer->byPKTNO & 0x0F), &(pMACHeader->abyAddr1[0]),(WORD)cbFrameSize,pTX_Buffer->wFIFOCtl);
+    if (WLAN_GET_FC_TODS(pMACHeader->frame_control) == 0) {
+        s_vSaveTxPktInfo(pDevice, (u8) (pTX_Buffer->byPKTNO & 0x0F), &(pMACHeader->addr1[0]), (u16)cbFrameSize, pTX_Buffer->wFIFOCtl);
     }
     else {
-        s_vSaveTxPktInfo(pDevice, (BYTE) (pTX_Buffer->byPKTNO & 0x0F), &(pMACHeader->abyAddr3[0]),(WORD)cbFrameSize,pTX_Buffer->wFIFOCtl);
+        s_vSaveTxPktInfo(pDevice, (u8) (pTX_Buffer->byPKTNO & 0x0F), &(pMACHeader->addr3[0]), (u16)cbFrameSize, pTX_Buffer->wFIFOCtl);
     }
 
     PIPEnsSendBulkOut(pDevice,pContext);
     return CMD_STATUS_PENDING;
 }
 
-
 CMD_STATUS csBeacon_xmit(struct vnt_private *pDevice,
 	struct vnt_tx_mgmt *pPacket)
 {
@@ -1994,7 +1952,7 @@ CMD_STATUS csBeacon_xmit(struct vnt_private *pDevice,
 	u32 cbHeaderSize = 0;
 	u16 wTxBufSize = sizeof(STxShortBufHead);
 	PSTxShortBufHead pTxBufHead;
-	PS802_11Header pMACHeader;
+	struct ieee80211_hdr *pMACHeader;
 	PSTxDataHead_ab pTxDataHead;
 	u16 wCurrentRate;
 	u32 cbFrameBodySize;
@@ -2004,7 +1962,6 @@ CMD_STATUS csBeacon_xmit(struct vnt_private *pDevice,
 	PUSB_SEND_CONTEXT pContext;
 	CMD_STATUS status;
 
-
     pContext = (PUSB_SEND_CONTEXT)s_vGetFreeContext(pDevice);
     if (NULL == pContext) {
         status = CMD_STATUS_RESOURCES;
@@ -2012,7 +1969,7 @@ CMD_STATUS csBeacon_xmit(struct vnt_private *pDevice,
         return status ;
     }
     pTX_Buffer = (PBEACON_BUFFER) (&pContext->Data[0]);
-    pbyTxBufferAddr = (PBYTE)&(pTX_Buffer->wFIFOCtl);
+    pbyTxBufferAddr = (u8 *)&(pTX_Buffer->wFIFOCtl);
 
     cbFrameBodySize = pPacket->cbPayloadLen;
 
@@ -2025,10 +1982,10 @@ CMD_STATUS csBeacon_xmit(struct vnt_private *pDevice,
         pTxDataHead = (PSTxDataHead_ab) (pbyTxBufferAddr + wTxBufSize);
         //Get SignalField,ServiceField,Length
         BBvCalculateParameter(pDevice, cbFrameSize, wCurrentRate, PK_TYPE_11A,
-            (PWORD)&(pTxDataHead->wTransmitLength), (PBYTE)&(pTxDataHead->byServiceField), (PBYTE)&(pTxDataHead->bySignalField)
+            (u16 *)&(pTxDataHead->wTransmitLength), (u8 *)&(pTxDataHead->byServiceField), (u8 *)&(pTxDataHead->bySignalField)
         );
         //Get Duration and TimeStampOff
-        pTxDataHead->wDuration = cpu_to_le16((WORD)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameSize, PK_TYPE_11A,
+        pTxDataHead->wDuration = cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameSize, PK_TYPE_11A,
                                                           wCurrentRate, false, 0, 0, 1, AUTO_FB_NONE));
         pTxDataHead->wTimeStampOff = wTimeStampOff[pDevice->byPreambleType%2][wCurrentRate%MAX_RATE];
         cbHeaderSize = wTxBufSize + sizeof(STxDataHead_ab);
@@ -2038,41 +1995,40 @@ CMD_STATUS csBeacon_xmit(struct vnt_private *pDevice,
         pTxDataHead = (PSTxDataHead_ab) (pbyTxBufferAddr + wTxBufSize);
         //Get SignalField,ServiceField,Length
         BBvCalculateParameter(pDevice, cbFrameSize, wCurrentRate, PK_TYPE_11B,
-            (PWORD)&(pTxDataHead->wTransmitLength), (PBYTE)&(pTxDataHead->byServiceField), (PBYTE)&(pTxDataHead->bySignalField)
+            (u16 *)&(pTxDataHead->wTransmitLength), (u8 *)&(pTxDataHead->byServiceField), (u8 *)&(pTxDataHead->bySignalField)
         );
         //Get Duration and TimeStampOff
-        pTxDataHead->wDuration = cpu_to_le16((WORD)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameSize, PK_TYPE_11B,
+        pTxDataHead->wDuration = cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameSize, PK_TYPE_11B,
                                                           wCurrentRate, false, 0, 0, 1, AUTO_FB_NONE));
         pTxDataHead->wTimeStampOff = wTimeStampOff[pDevice->byPreambleType%2][wCurrentRate%MAX_RATE];
         cbHeaderSize = wTxBufSize + sizeof(STxDataHead_ab);
     }
 
     //Generate Beacon Header
-    pMACHeader = (PS802_11Header)(pbyTxBufferAddr + cbHeaderSize);
+    pMACHeader = (struct ieee80211_hdr *)(pbyTxBufferAddr + cbHeaderSize);
     memcpy(pMACHeader, pPacket->p80211Header, pPacket->cbMPDULen);
 
-    pMACHeader->wDurationID = 0;
-    pMACHeader->wSeqCtl = cpu_to_le16(pDevice->wSeqCounter << 4);
+    pMACHeader->duration_id = 0;
+    pMACHeader->seq_ctrl = cpu_to_le16(pDevice->wSeqCounter << 4);
     pDevice->wSeqCounter++ ;
     if (pDevice->wSeqCounter > 0x0fff)
         pDevice->wSeqCounter = 0;
 
     cbReqCount = cbHeaderSize + WLAN_HDR_ADDR3_LEN + cbFrameBodySize;
 
-    pTX_Buffer->wTxByteCount = (WORD)cbReqCount;
-    pTX_Buffer->byPKTNO = (BYTE) (((wCurrentRate<<4) &0x00F0) | ((pDevice->wSeqCounter - 1) & 0x000F));
+    pTX_Buffer->wTxByteCount = (u16)cbReqCount;
+    pTX_Buffer->byPKTNO = (u8) (((wCurrentRate<<4) &0x00F0) | ((pDevice->wSeqCounter - 1) & 0x000F));
     pTX_Buffer->byType = 0x01;
 
     pContext->pPacket = NULL;
     pContext->Type = CONTEXT_MGMT_PACKET;
-    pContext->uBufLen = (WORD)cbReqCount + 4;  //USB header
+    pContext->uBufLen = (u16)cbReqCount + 4;  //USB header
 
     PIPEnsSendBulkOut(pDevice,pContext);
     return CMD_STATUS_PENDING;
 
 }
 
-
 void vDMA0_tx_80211(struct vnt_private *pDevice, struct sk_buff *skb)
 {
 	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
@@ -2080,7 +2036,7 @@ void vDMA0_tx_80211(struct vnt_private *pDevice, struct sk_buff *skb)
 	u8 *pbyTxBufferAddr;
 	void *pvRTS, *pvCTS, *pvTxDataHd;
 	u32 uDuration, cbReqCount;
-	PS802_11Header  pMACHeader;
+	struct ieee80211_hdr *pMACHeader;
 	u32 cbHeaderSize, cbFrameBodySize;
 	int bNeedACK, bIsPSPOLL = false;
 	PSTxBufHead pTxBufHead;
@@ -2093,7 +2049,7 @@ void vDMA0_tx_80211(struct vnt_private *pDevice, struct sk_buff *skb)
 	u32 *pdwMIC_L, *pdwMIC_R;
 	u16 wTxBufSize;
 	u32 cbMacHdLen;
-	SEthernetHeader sEthHeader;
+	struct ethhdr sEthHeader;
 	void *pvRrvTime, *pMICHDR;
 	u32 wCurrentRate = RATE_1M;
 	PUWLAN_80211HDR  p80211Header;
@@ -2106,7 +2062,6 @@ void vDMA0_tx_80211(struct vnt_private *pDevice, struct sk_buff *skb)
 	PTX_BUFFER pTX_Buffer;
 	PUSB_SEND_CONTEXT pContext;
 
-
     pvRrvTime = pMICHDR = pvRTS = pvCTS = pvTxDataHd = NULL;
 
     if(skb->len <= WLAN_HDR_ADDR3_LEN) {
@@ -2126,7 +2081,7 @@ void vDMA0_tx_80211(struct vnt_private *pDevice, struct sk_buff *skb)
     }
 
     pTX_Buffer = (PTX_BUFFER)(&pContext->Data[0]);
-    pbyTxBufferAddr = (PBYTE)(&pTX_Buffer->adwTxKey[0]);
+    pbyTxBufferAddr = (u8 *)(&pTX_Buffer->adwTxKey[0]);
     pTxBufHead = (PSTxBufHead) pbyTxBufferAddr;
     wTxBufSize = sizeof(STxBufHead);
     memset(pTxBufHead, 0, wTxBufSize);
@@ -2177,7 +2132,7 @@ void vDMA0_tx_80211(struct vnt_private *pDevice, struct sk_buff *skb)
     }
     else {
         if (pDevice->bEnableHostWEP) {
-            if (BSSbIsSTAInNodeDB(pDevice, (PBYTE)(p80211Header->sA3.abyAddr1), &uNodeIndex))
+            if (BSSbIsSTAInNodeDB(pDevice, (u8 *)(p80211Header->sA3.abyAddr1), &uNodeIndex))
                 bNodeExist = true;
         }
         bNeedACK = true;
@@ -2223,16 +2178,14 @@ void vDMA0_tx_80211(struct vnt_private *pDevice, struct sk_buff *skb)
          }
     }
 
-
     //Set FRAGCTL_MACHDCNT
-    pTxBufHead->wFragCtl |= cpu_to_le16((WORD)cbMacHdLen << 10);
+    pTxBufHead->wFragCtl |= cpu_to_le16((u16)cbMacHdLen << 10);
 
     // Notes:
     // Although spec says MMPDU can be fragmented; In most case,
     // no one will send a MMPDU under fragmentation. With RTS may occur.
     pDevice->bAES = false;  //Set FRAGCTL_WEPTYP
 
-
     if (WLAN_GET_FC_ISWEP(p80211Header->sA4.wFrameCtl) != 0) {
         if (pDevice->eEncryptionStatus == Ndis802_11Encryption1Enabled) {
             cbIVlen = 4;
@@ -2268,7 +2221,6 @@ void vDMA0_tx_80211(struct vnt_private *pDevice, struct sk_buff *skb)
     }
     //the rest of pTxBufHead->wFragCtl:FragTyp will be set later in s_vFillFragParameter()
 
-
     if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {//802.11g packet
 
         pvRrvTime = (PSRrvTime_gCTS) (pbyTxBufferAddr + wTxBufSize);
@@ -2290,17 +2242,16 @@ void vDMA0_tx_80211(struct vnt_private *pDevice, struct sk_buff *skb)
     }
     memset((void *)(pbyTxBufferAddr + wTxBufSize), 0,
 	   (cbHeaderSize - wTxBufSize));
-    memcpy(&(sEthHeader.abyDstAddr[0]),
+    memcpy(&(sEthHeader.h_dest[0]),
 	   &(p80211Header->sA3.abyAddr1[0]),
 	   ETH_ALEN);
-    memcpy(&(sEthHeader.abySrcAddr[0]),
+    memcpy(&(sEthHeader.h_source[0]),
 	   &(p80211Header->sA3.abyAddr2[0]),
 	   ETH_ALEN);
     //=========================
     //    No Fragmentation
     //=========================
-    pTxBufHead->wFragCtl |= (WORD)FRAGCTL_NONFRAG;
-
+    pTxBufHead->wFragCtl |= (u16)FRAGCTL_NONFRAG;
 
     //Fill FIFO,RrvTime,RTS,and CTS
     s_vGenerateTxParameter(pDevice, byPktType, wCurrentRate, pbyTxBufferAddr, pvRrvTime, pvRTS, pvCTS,
@@ -2310,19 +2261,19 @@ void vDMA0_tx_80211(struct vnt_private *pDevice, struct sk_buff *skb)
     uDuration = s_uFillDataHead(pDevice, byPktType, wCurrentRate, pvTxDataHd, cbFrameSize, TYPE_TXDMA0, bNeedACK,
                                 0, 0, 1, AUTO_FB_NONE);
 
-    pMACHeader = (PS802_11Header) (pbyTxBufferAddr + cbHeaderSize);
+    pMACHeader = (struct ieee80211_hdr *) (pbyTxBufferAddr + cbHeaderSize);
 
     cbReqCount = cbHeaderSize + cbMacHdLen + uPadding + cbIVlen + (cbFrameBodySize + cbMIClen) + cbExtSuppRate;
 
-    pbyMacHdr = (PBYTE)(pbyTxBufferAddr + cbHeaderSize);
-    pbyPayloadHead = (PBYTE)(pbyMacHdr + cbMacHdLen + uPadding + cbIVlen);
-    pbyIVHead = (PBYTE)(pbyMacHdr + cbMacHdLen + uPadding);
+    pbyMacHdr = (u8 *)(pbyTxBufferAddr + cbHeaderSize);
+    pbyPayloadHead = (u8 *)(pbyMacHdr + cbMacHdLen + uPadding + cbIVlen);
+    pbyIVHead = (u8 *)(pbyMacHdr + cbMacHdLen + uPadding);
 
     // Copy the Packet into a tx Buffer
     memcpy(pbyMacHdr, skb->data, cbMacHdLen);
 
     // version set to 0, patch for hostapd deamon
-    pMACHeader->wFrameCtl &= cpu_to_le16(0xfffc);
+    pMACHeader->frame_control &= cpu_to_le16(0xfffc);
     memcpy(pbyPayloadHead, (skb->data + cbMacHdLen), cbFrameBodySize);
 
     // replace support rate, patch for hostapd daemon( only support 11M)
@@ -2359,14 +2310,14 @@ void vDMA0_tx_80211(struct vnt_private *pDevice, struct sk_buff *skb)
 
         if ((pTransmitKey != NULL) && (pTransmitKey->byCipherSuite == KEY_CTL_TKIP)) {
 
-            dwMICKey0 = *(PDWORD)(&pTransmitKey->abyKey[16]);
-            dwMICKey1 = *(PDWORD)(&pTransmitKey->abyKey[20]);
+            dwMICKey0 = *(u32 *)(&pTransmitKey->abyKey[16]);
+            dwMICKey1 = *(u32 *)(&pTransmitKey->abyKey[20]);
 
             // DO Software Michael
             MIC_vInit(dwMICKey0, dwMICKey1);
-            MIC_vAppend((PBYTE)&(sEthHeader.abyDstAddr[0]), 12);
+            MIC_vAppend((u8 *)&(sEthHeader.h_dest[0]), 12);
             dwMIC_Priority = 0;
-            MIC_vAppend((PBYTE)&dwMIC_Priority, 4);
+            MIC_vAppend((u8 *)&dwMIC_Priority, 4);
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"DMA0_tx_8021:MIC KEY:"\
 			" %X, %X\n", dwMICKey0, dwMICKey1);
 
@@ -2374,8 +2325,8 @@ void vDMA0_tx_80211(struct vnt_private *pDevice, struct sk_buff *skb)
 
             MIC_vAppend((pbyTxBufferAddr + uLength), cbFrameBodySize);
 
-            pdwMIC_L = (PDWORD)(pbyTxBufferAddr + uLength + cbFrameBodySize);
-            pdwMIC_R = (PDWORD)(pbyTxBufferAddr + uLength + cbFrameBodySize + 4);
+            pdwMIC_L = (u32 *)(pbyTxBufferAddr + uLength + cbFrameBodySize);
+            pdwMIC_R = (u32 *)(pbyTxBufferAddr + uLength + cbFrameBodySize + 4);
 
             MIC_vGetMIC(pdwMIC_L, pdwMIC_R);
             MIC_vUnInit();
@@ -2393,8 +2344,8 @@ void vDMA0_tx_80211(struct vnt_private *pDevice, struct sk_buff *skb)
 
         }
 
-        s_vFillTxKey(pDevice, (PBYTE)(pTxBufHead->adwTxKey), pbyIVHead, pTransmitKey,
-                     pbyMacHdr, (WORD)cbFrameBodySize, (PBYTE)pMICHDR);
+        s_vFillTxKey(pDevice, (u8 *)(pTxBufHead->adwTxKey), pbyIVHead, pTransmitKey,
+                     pbyMacHdr, (u16)cbFrameBodySize, (u8 *)pMICHDR);
 
         if (pDevice->bEnableHostWEP) {
             pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16 = pTransmitKey->dwTSC47_16;
@@ -2402,16 +2353,15 @@ void vDMA0_tx_80211(struct vnt_private *pDevice, struct sk_buff *skb)
         }
 
         if ((pDevice->byLocalID <= REV_ID_VT3253_A1)) {
-            s_vSWencryption(pDevice, pTransmitKey, pbyPayloadHead, (WORD)(cbFrameBodySize + cbMIClen));
+            s_vSWencryption(pDevice, pTransmitKey, pbyPayloadHead, (u16)(cbFrameBodySize + cbMIClen));
         }
     }
 
-    pMACHeader->wSeqCtl = cpu_to_le16(pDevice->wSeqCounter << 4);
+    pMACHeader->seq_ctrl = cpu_to_le16(pDevice->wSeqCounter << 4);
     pDevice->wSeqCounter++ ;
     if (pDevice->wSeqCounter > 0x0fff)
         pDevice->wSeqCounter = 0;
 
-
     if (bIsPSPOLL) {
         // The MAC will automatically replace the Duration-field of MAC header by Duration-field
         // of  FIFO control header.
@@ -2426,28 +2376,25 @@ void vDMA0_tx_80211(struct vnt_private *pDevice, struct sk_buff *skb)
         }
     }
 
-    pTX_Buffer->wTxByteCount = cpu_to_le16((WORD)(cbReqCount));
-    pTX_Buffer->byPKTNO = (BYTE) (((wCurrentRate<<4) &0x00F0) | ((pDevice->wSeqCounter - 1) & 0x000F));
+    pTX_Buffer->wTxByteCount = cpu_to_le16((u16)(cbReqCount));
+    pTX_Buffer->byPKTNO = (u8) (((wCurrentRate<<4) &0x00F0) | ((pDevice->wSeqCounter - 1) & 0x000F));
     pTX_Buffer->byType = 0x00;
 
     pContext->pPacket = skb;
     pContext->Type = CONTEXT_MGMT_PACKET;
-    pContext->uBufLen = (WORD)cbReqCount + 4;  //USB header
+    pContext->uBufLen = (u16)cbReqCount + 4;  //USB header
 
-    if (WLAN_GET_FC_TODS(pMACHeader->wFrameCtl) == 0) {
-        s_vSaveTxPktInfo(pDevice, (BYTE) (pTX_Buffer->byPKTNO & 0x0F), &(pMACHeader->abyAddr1[0]),(WORD)cbFrameSize,pTX_Buffer->wFIFOCtl);
+    if (WLAN_GET_FC_TODS(pMACHeader->frame_control) == 0) {
+        s_vSaveTxPktInfo(pDevice, (u8) (pTX_Buffer->byPKTNO & 0x0F), &(pMACHeader->addr1[0]), (u16)cbFrameSize, pTX_Buffer->wFIFOCtl);
     }
     else {
-        s_vSaveTxPktInfo(pDevice, (BYTE) (pTX_Buffer->byPKTNO & 0x0F), &(pMACHeader->abyAddr3[0]),(WORD)cbFrameSize,pTX_Buffer->wFIFOCtl);
+        s_vSaveTxPktInfo(pDevice, (u8) (pTX_Buffer->byPKTNO & 0x0F), &(pMACHeader->addr3[0]), (u16)cbFrameSize, pTX_Buffer->wFIFOCtl);
     }
     PIPEnsSendBulkOut(pDevice,pContext);
     return ;
 
 }
 
-
-
-
 //TYPE_AC0DMA data tx
 /*
  * Description:
@@ -2488,7 +2435,6 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
 	u16 wKeepRate = pDevice->wCurrentRate;
 	int bTxeapol_key = false;
 
-
     if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
 
         if (pDevice->uAssocCount == 0) {
@@ -2496,7 +2442,7 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
             return 0;
         }
 
-	if (is_multicast_ether_addr((PBYTE)(skb->data))) {
+	if (is_multicast_ether_addr((u8 *)(skb->data))) {
             uNodeIndex = 0;
             bNodeExist = true;
             if (pMgmt->sNodeDBTable[0].bPSEnable) {
@@ -2518,7 +2464,7 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
 
         }else {
 
-            if (BSSbIsSTAInNodeDB(pDevice, (PBYTE)(skb->data), &uNodeIndex)) {
+            if (BSSbIsSTAInNodeDB(pDevice, (u8 *)(skb->data), &uNodeIndex)) {
 
                 if (pMgmt->sNodeDBTable[uNodeIndex].bPSEnable) {
 
@@ -2562,20 +2508,20 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
         return STATUS_RESOURCES;
     }
 
-    memcpy(pDevice->sTxEthHeader.abyDstAddr, (PBYTE)(skb->data), ETH_HLEN);
+    memcpy(pDevice->sTxEthHeader.h_dest, (u8 *)(skb->data), ETH_HLEN);
 
 //mike add:station mode check eapol-key challenge--->
 {
-    BYTE  Protocol_Version;    //802.1x Authentication
-    BYTE  Packet_Type;           //802.1x Authentication
-    BYTE  Descriptor_type;
-    WORD Key_info;
+    u8  Protocol_Version;    //802.1x Authentication
+    u8  Packet_Type;           //802.1x Authentication
+    u8  Descriptor_type;
+    u16 Key_info;
 
     Protocol_Version = skb->data[ETH_HLEN];
     Packet_Type = skb->data[ETH_HLEN+1];
     Descriptor_type = skb->data[ETH_HLEN+1+1+2];
     Key_info = (skb->data[ETH_HLEN+1+1+2+1] << 8)|(skb->data[ETH_HLEN+1+1+2+2]);
-	if (pDevice->sTxEthHeader.wType == cpu_to_be16(ETH_P_PAE)) {
+	if (pDevice->sTxEthHeader.h_proto == cpu_to_be16(ETH_P_PAE)) {
 		/* 802.1x OR eapol-key challenge frame transfer */
 		if (((Protocol_Version == 1) || (Protocol_Version == 2)) &&
 			(Packet_Type == 3)) {
@@ -2622,8 +2568,8 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
                     break;
                 }
             }else if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
-
-                pbyBSSID = pDevice->sTxEthHeader.abyDstAddr;  //TO_DS = 0 and FROM_DS = 0 --> 802.11 MAC Address1
+	      /* TO_DS = 0 and FROM_DS = 0 --> 802.11 MAC Address1 */
+                pbyBSSID = pDevice->sTxEthHeader.h_dest;
                 DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG"IBSS Serach Key: \n");
                 for (ii = 0; ii< 6; ii++)
                     DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG"%x \n", *(pbyBSSID+ii));
@@ -2665,14 +2611,14 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
          }
     }
 
-    byPktType = (BYTE)pDevice->byPacketType;
+    byPktType = (u8)pDevice->byPacketType;
 
     if (pDevice->bFixRate) {
         if (pDevice->byBBType == BB_TYPE_11B) {
             if (pDevice->uConnectionRate >= RATE_11M) {
                 pDevice->wCurrentRate = RATE_11M;
             } else {
-                pDevice->wCurrentRate = (WORD)pDevice->uConnectionRate;
+                pDevice->wCurrentRate = (u16)pDevice->uConnectionRate;
             }
         } else {
             if ((pDevice->byBBType == BB_TYPE_11A) &&
@@ -2682,21 +2628,21 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
                 if (pDevice->uConnectionRate >= RATE_54M)
                     pDevice->wCurrentRate = RATE_54M;
                 else
-                    pDevice->wCurrentRate = (WORD)pDevice->uConnectionRate;
+                    pDevice->wCurrentRate = (u16)pDevice->uConnectionRate;
             }
         }
     }
     else {
         if (pDevice->eOPMode == OP_MODE_ADHOC) {
             // Adhoc Tx rate decided from node DB
-	    if (is_multicast_ether_addr(pDevice->sTxEthHeader.abyDstAddr)) {
+	    if (is_multicast_ether_addr(pDevice->sTxEthHeader.h_dest)) {
                 // Multicast use highest data rate
                 pDevice->wCurrentRate = pMgmt->sNodeDBTable[0].wTxDataRate;
                 // preamble type
                 pDevice->byPreambleType = pDevice->byShortPreamble;
             }
             else {
-                if(BSSbIsSTAInNodeDB(pDevice, &(pDevice->sTxEthHeader.abyDstAddr[0]), &uNodeIndex)) {
+                if (BSSbIsSTAInNodeDB(pDevice, &(pDevice->sTxEthHeader.h_dest[0]), &uNodeIndex)) {
                     pDevice->wCurrentRate = pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate;
                     if (pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble) {
                         pDevice->byPreambleType = pDevice->byShortPreamble;
@@ -2724,7 +2670,7 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
         }
     }
 
-	if (pDevice->sTxEthHeader.wType == cpu_to_be16(ETH_P_PAE)) {
+	if (pDevice->sTxEthHeader.h_proto == cpu_to_be16(ETH_P_PAE)) {
 		if (pDevice->byBBType != BB_TYPE_11A) {
 			pDevice->wCurrentRate = RATE_1M;
 			pDevice->byACKRate = RATE_1M;
@@ -2751,10 +2697,10 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
     }
 
     if (bNeedEncryption == true) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ntohs Pkt Type=%04x\n", ntohs(pDevice->sTxEthHeader.wType));
-	if ((pDevice->sTxEthHeader.wType) == cpu_to_be16(ETH_P_PAE)) {
+        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ntohs Pkt Type=%04x\n", ntohs(pDevice->sTxEthHeader.h_proto));
+	if ((pDevice->sTxEthHeader.h_proto) == cpu_to_be16(ETH_P_PAE)) {
 		bNeedEncryption = false;
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Pkt Type=%04x\n", (pDevice->sTxEthHeader.wType));
+            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Pkt Type=%04x\n", (pDevice->sTxEthHeader.h_proto));
             if ((pMgmt->eCurrMode == WMAC_MODE_ESS_STA) && (pMgmt->eCurrState == WMAC_STATE_ASSOC)) {
                 if (pTransmitKey == NULL) {
                     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Don't Find TX KEY\n");
@@ -2793,9 +2739,9 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
     }
 
     fConvertedPacket = s_bPacketToWirelessUsb(pDevice, byPktType,
-                        (PBYTE)(&pContext->Data[0]), bNeedEncryption,
+                        (u8 *)(&pContext->Data[0]), bNeedEncryption,
                         skb->len, uDMAIdx, &pDevice->sTxEthHeader,
-                        (PBYTE)skb->data, pTransmitKey, uNodeIndex,
+                        (u8 *)skb->data, pTransmitKey, uNodeIndex,
                         pDevice->wCurrentRate,
                         &uHeaderLen, &BytesToWrite
                        );
@@ -2816,21 +2762,21 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
     }
 
     pTX_Buffer = (PTX_BUFFER)&(pContext->Data[0]);
-    pTX_Buffer->byPKTNO = (BYTE) (((pDevice->wCurrentRate<<4) &0x00F0) | ((pDevice->wSeqCounter - 1) & 0x000F));
-    pTX_Buffer->wTxByteCount = (WORD)BytesToWrite;
+    pTX_Buffer->byPKTNO = (u8) (((pDevice->wCurrentRate<<4) &0x00F0) | ((pDevice->wSeqCounter - 1) & 0x000F));
+    pTX_Buffer->wTxByteCount = (u16)BytesToWrite;
 
     pContext->pPacket = skb;
     pContext->Type = CONTEXT_DATA_PACKET;
-    pContext->uBufLen = (WORD)BytesToWrite + 4 ; //USB header
+    pContext->uBufLen = (u16)BytesToWrite + 4 ; //USB header
 
-    s_vSaveTxPktInfo(pDevice, (BYTE) (pTX_Buffer->byPKTNO & 0x0F), &(pContext->sEthHeader.abyDstAddr[0]),(WORD) (BytesToWrite-uHeaderLen),pTX_Buffer->wFIFOCtl);
+    s_vSaveTxPktInfo(pDevice, (u8) (pTX_Buffer->byPKTNO & 0x0F), &(pContext->sEthHeader.h_dest[0]), (u16) (BytesToWrite-uHeaderLen), pTX_Buffer->wFIFOCtl);
 
     status = PIPEnsSendBulkOut(pDevice,pContext);
 
     if (bNeedDeAuth == true) {
-        WORD wReason = WLAN_MGMT_REASON_MIC_FAILURE;
+        u16 wReason = WLAN_MGMT_REASON_MIC_FAILURE;
 
-	bScheduleCommand((void *) pDevice, WLAN_CMD_DEAUTH, (PBYTE) &wReason);
+	bScheduleCommand((void *) pDevice, WLAN_CMD_DEAUTH, (u8 *) &wReason);
     }
 
   if(status!=STATUS_PENDING) {
@@ -2843,8 +2789,6 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
 
 }
 
-
-
 /*
  * Description:
  *      Relay packet send (AC1DMA) from rx dpc.
@@ -2877,15 +2821,13 @@ int bRelayPacketSend(struct vnt_private *pDevice, u8 *pbySkbData, u32 uDataLen,
 	u32 status;
 	u16 wKeepRate = pDevice->wCurrentRate;
 
-
-
     pContext = (PUSB_SEND_CONTEXT)s_vGetFreeContext(pDevice);
 
     if (NULL == pContext) {
         return false;
     }
 
-    memcpy(pDevice->sTxEthHeader.abyDstAddr, (PBYTE)pbySkbData, ETH_HLEN);
+    memcpy(pDevice->sTxEthHeader.h_dest, (u8 *)pbySkbData, ETH_HLEN);
 
     if (pDevice->bEncryptionEnable == true) {
         bNeedEncryption = true;
@@ -2919,14 +2861,14 @@ int bRelayPacketSend(struct vnt_private *pDevice, u8 *pbySkbData, u32 uDataLen,
         return false;
     }
 
-    byPktTyp = (BYTE)pDevice->byPacketType;
+    byPktTyp = (u8)pDevice->byPacketType;
 
     if (pDevice->bFixRate) {
         if (pDevice->byBBType == BB_TYPE_11B) {
             if (pDevice->uConnectionRate >= RATE_11M) {
                 pDevice->wCurrentRate = RATE_11M;
             } else {
-                pDevice->wCurrentRate = (WORD)pDevice->uConnectionRate;
+                pDevice->wCurrentRate = (u16)pDevice->uConnectionRate;
             }
         } else {
             if ((pDevice->byBBType == BB_TYPE_11A) &&
@@ -2936,7 +2878,7 @@ int bRelayPacketSend(struct vnt_private *pDevice, u8 *pbySkbData, u32 uDataLen,
                 if (pDevice->uConnectionRate >= RATE_54M)
                     pDevice->wCurrentRate = RATE_54M;
                 else
-                    pDevice->wCurrentRate = (WORD)pDevice->uConnectionRate;
+                    pDevice->wCurrentRate = (u16)pDevice->uConnectionRate;
             }
         }
     }
@@ -2957,7 +2899,7 @@ int bRelayPacketSend(struct vnt_private *pDevice, u8 *pbySkbData, u32 uDataLen,
     // and send the irp.
 
     fConvertedPacket = s_bPacketToWirelessUsb(pDevice, byPktType,
-                         (PBYTE)(&pContext->Data[0]), bNeedEncryption,
+                         (u8 *)(&pContext->Data[0]), bNeedEncryption,
                          uDataLen, TYPE_AC0DMA, &pDevice->sTxEthHeader,
                          pbySkbData, pTransmitKey, uNodeIndex,
                          pDevice->wCurrentRate,
@@ -2970,14 +2912,14 @@ int bRelayPacketSend(struct vnt_private *pDevice, u8 *pbySkbData, u32 uDataLen,
     }
 
     pTX_Buffer = (PTX_BUFFER)&(pContext->Data[0]);
-    pTX_Buffer->byPKTNO = (BYTE) (((pDevice->wCurrentRate<<4) &0x00F0) | ((pDevice->wSeqCounter - 1) & 0x000F));
-    pTX_Buffer->wTxByteCount = (WORD)BytesToWrite;
+    pTX_Buffer->byPKTNO = (u8) (((pDevice->wCurrentRate<<4) &0x00F0) | ((pDevice->wSeqCounter - 1) & 0x000F));
+    pTX_Buffer->wTxByteCount = (u16)BytesToWrite;
 
     pContext->pPacket = NULL;
     pContext->Type = CONTEXT_DATA_PACKET;
-    pContext->uBufLen = (WORD)BytesToWrite + 4 ; //USB header
+    pContext->uBufLen = (u16)BytesToWrite + 4 ; //USB header
 
-    s_vSaveTxPktInfo(pDevice, (BYTE) (pTX_Buffer->byPKTNO & 0x0F), &(pContext->sEthHeader.abyDstAddr[0]),(WORD) (BytesToWrite-uHeaderLen),pTX_Buffer->wFIFOCtl);
+    s_vSaveTxPktInfo(pDevice, (u8) (pTX_Buffer->byPKTNO & 0x0F), &(pContext->sEthHeader.h_dest[0]), (u16) (BytesToWrite-uHeaderLen), pTX_Buffer->wFIFOCtl);
 
     status = PIPEnsSendBulkOut(pDevice,pContext);
 
diff --git a/drivers/staging/vt6656/rxtx.h b/drivers/staging/vt6656/rxtx.h
index 9f537022cdd..dd7e85dde1a 100644
--- a/drivers/staging/vt6656/rxtx.h
+++ b/drivers/staging/vt6656/rxtx.h
@@ -29,32 +29,27 @@
 #ifndef __RXTX_H__
 #define __RXTX_H__
 
-#include "ttype.h"
 #include "device.h"
 #include "wcmd.h"
 
-/*---------------------  Export Definitions -------------------------*/
-
-/*---------------------  Export Classes  ----------------------------*/
-
 //
 // RTS buffer header
 //
 typedef struct tagSRTSDataF {
-    WORD    wFrameControl;
-    WORD    wDurationID;
-    BYTE    abyRA[ETH_ALEN];
-    BYTE    abyTA[ETH_ALEN];
+    u16    wFrameControl;
+    u16    wDurationID;
+    u8    abyRA[ETH_ALEN];
+    u8    abyTA[ETH_ALEN];
 } SRTSDataF, *PSRTSDataF;
 
 //
 // CTS buffer header
 //
 typedef struct tagSCTSDataF {
-    WORD    wFrameControl;
-    WORD    wDurationID;
-    BYTE    abyRA[ETH_ALEN];
-    WORD    wReserved;
+    u16    wFrameControl;
+    u16    wDurationID;
+    u8    abyRA[ETH_ALEN];
+    u16    wReserved;
 } SCTSDataF, *PSCTSDataF;
 
 //
@@ -66,534 +61,510 @@ typedef struct tagSMICHDR {
 	u32 adwHDR2[4];
 } SMICHDR, *PSMICHDR;
 
-
 typedef struct tagSTX_NAF_G_RTS
 {
     //RsvTime
-    WORD            wRTSTxRrvTime_ba;
-    WORD            wRTSTxRrvTime_aa;
-    WORD            wRTSTxRrvTime_bb;
-    WORD            wReserved2;
-    WORD            wTxRrvTime_b;
-    WORD            wTxRrvTime_a;
+    u16            wRTSTxRrvTime_ba;
+    u16            wRTSTxRrvTime_aa;
+    u16            wRTSTxRrvTime_bb;
+    u16            wReserved2;
+    u16            wTxRrvTime_b;
+    u16            wTxRrvTime_a;
 
     //RTS
-    BYTE            byRTSSignalField_b;
-    BYTE            byRTSServiceField_b;
-    WORD            wRTSTransmitLength_b;
-    BYTE            byRTSSignalField_a;
-    BYTE            byRTSServiceField_a;
-    WORD            wRTSTransmitLength_a;
-    WORD            wRTSDuration_ba;
-    WORD            wRTSDuration_aa;
-    WORD            wRTSDuration_bb;
-    WORD            wReserved3;
+    u8            byRTSSignalField_b;
+    u8            byRTSServiceField_b;
+    u16            wRTSTransmitLength_b;
+    u8            byRTSSignalField_a;
+    u8            byRTSServiceField_a;
+    u16            wRTSTransmitLength_a;
+    u16            wRTSDuration_ba;
+    u16            wRTSDuration_aa;
+    u16            wRTSDuration_bb;
+    u16            wReserved3;
     SRTSDataF       sRTS;
 
     //Data
-    BYTE            bySignalField_b;
-    BYTE            byServiceField_b;
-    WORD            wTransmitLength_b;
-    BYTE            bySignalField_a;
-    BYTE            byServiceField_a;
-    WORD            wTransmitLength_a;
-    WORD            wDuration_b;
-    WORD            wDuration_a;
-    WORD            wTimeStampOff_b;
-    WORD            wTimeStampOff_a;
+    u8            bySignalField_b;
+    u8            byServiceField_b;
+    u16            wTransmitLength_b;
+    u8            bySignalField_a;
+    u8            byServiceField_a;
+    u16            wTransmitLength_a;
+    u16            wDuration_b;
+    u16            wDuration_a;
+    u16            wTimeStampOff_b;
+    u16            wTimeStampOff_a;
 
 } TX_NAF_G_RTS, *PTX_NAF_G_RTS;
 
 typedef struct tagSTX_NAF_G_RTS_MIC
 {
     //RsvTime
-    WORD            wRTSTxRrvTime_ba;
-    WORD            wRTSTxRrvTime_aa;
-    WORD            wRTSTxRrvTime_bb;
-    WORD            wReserved2;
-    WORD            wTxRrvTime_b;
-    WORD            wTxRrvTime_a;
+    u16            wRTSTxRrvTime_ba;
+    u16            wRTSTxRrvTime_aa;
+    u16            wRTSTxRrvTime_bb;
+    u16            wReserved2;
+    u16            wTxRrvTime_b;
+    u16            wTxRrvTime_a;
 
     SMICHDR         sMICHDR;
 
     //RTS
-    BYTE            byRTSSignalField_b;
-    BYTE            byRTSServiceField_b;
-    WORD            wRTSTransmitLength_b;
-    BYTE            byRTSSignalField_a;
-    BYTE            byRTSServiceField_a;
-    WORD            wRTSTransmitLength_a;
-    WORD            wRTSDuration_ba;
-    WORD            wRTSDuration_aa;
-    WORD            wRTSDuration_bb;
-    WORD            wReserved3;
+    u8            byRTSSignalField_b;
+    u8            byRTSServiceField_b;
+    u16            wRTSTransmitLength_b;
+    u8            byRTSSignalField_a;
+    u8            byRTSServiceField_a;
+    u16            wRTSTransmitLength_a;
+    u16            wRTSDuration_ba;
+    u16            wRTSDuration_aa;
+    u16            wRTSDuration_bb;
+    u16            wReserved3;
     SRTSDataF       sRTS;
 
     //Data
-    BYTE            bySignalField_b;
-    BYTE            byServiceField_b;
-    WORD            wTransmitLength_b;
-    BYTE            bySignalField_a;
-    BYTE            byServiceField_a;
-    WORD            wTransmitLength_a;
-    WORD            wDuration_b;
-    WORD            wDuration_a;
-    WORD            wTimeStampOff_b;
-    WORD            wTimeStampOff_a;
+    u8            bySignalField_b;
+    u8            byServiceField_b;
+    u16            wTransmitLength_b;
+    u8            bySignalField_a;
+    u8            byServiceField_a;
+    u16            wTransmitLength_a;
+    u16            wDuration_b;
+    u16            wDuration_a;
+    u16            wTimeStampOff_b;
+    u16            wTimeStampOff_a;
 
 } TX_NAF_G_RTS_MIC, *PTX_NAF_G_RTS_MIC;
 
 typedef struct tagSTX_NAF_G_CTS
 {
     //RsvTime
-    WORD            wCTSTxRrvTime_ba;
-    WORD            wReserved2;
-    WORD            wTxRrvTime_b;
-    WORD            wTxRrvTime_a;
+    u16            wCTSTxRrvTime_ba;
+    u16            wReserved2;
+    u16            wTxRrvTime_b;
+    u16            wTxRrvTime_a;
 
     //CTS
-    BYTE            byCTSSignalField_b;
-    BYTE            byCTSServiceField_b;
-    WORD            wCTSTransmitLength_b;
-    WORD            wCTSDuration_ba;
-    WORD            wReserved3;
+    u8            byCTSSignalField_b;
+    u8            byCTSServiceField_b;
+    u16            wCTSTransmitLength_b;
+    u16            wCTSDuration_ba;
+    u16            wReserved3;
     SCTSDataF       sCTS;
 
     //Data
-    BYTE            bySignalField_b;
-    BYTE            byServiceField_b;
-    WORD            wTransmitLength_b;
-    BYTE            bySignalField_a;
-    BYTE            byServiceField_a;
-    WORD            wTransmitLength_a;
-    WORD            wDuration_b;
-    WORD            wDuration_a;
-    WORD            wTimeStampOff_b;
-    WORD            wTimeStampOff_a;
+    u8            bySignalField_b;
+    u8            byServiceField_b;
+    u16            wTransmitLength_b;
+    u8            bySignalField_a;
+    u8            byServiceField_a;
+    u16            wTransmitLength_a;
+    u16            wDuration_b;
+    u16            wDuration_a;
+    u16            wTimeStampOff_b;
+    u16            wTimeStampOff_a;
 
 } TX_NAF_G_CTS, *PTX_NAF_G_CTS;
 
-
 typedef struct tagSTX_NAF_G_CTS_MIC
 {
     //RsvTime
-    WORD            wCTSTxRrvTime_ba;
-    WORD            wReserved2;
-    WORD            wTxRrvTime_b;
-    WORD            wTxRrvTime_a;
-
+    u16            wCTSTxRrvTime_ba;
+    u16            wReserved2;
+    u16            wTxRrvTime_b;
+    u16            wTxRrvTime_a;
 
     SMICHDR         sMICHDR;
 
     //CTS
-    BYTE            byCTSSignalField_b;
-    BYTE            byCTSServiceField_b;
-    WORD            wCTSTransmitLength_b;
-    WORD            wCTSDuration_ba;
-    WORD            wReserved3;
+    u8            byCTSSignalField_b;
+    u8            byCTSServiceField_b;
+    u16            wCTSTransmitLength_b;
+    u16            wCTSDuration_ba;
+    u16            wReserved3;
     SCTSDataF       sCTS;
 
     //Data
-    BYTE            bySignalField_b;
-    BYTE            byServiceField_b;
-    WORD            wTransmitLength_b;
-    BYTE            bySignalField_a;
-    BYTE            byServiceField_a;
-    WORD            wTransmitLength_a;
-    WORD            wDuration_b;
-    WORD            wDuration_a;
-    WORD            wTimeStampOff_b;
-    WORD            wTimeStampOff_a;
+    u8            bySignalField_b;
+    u8            byServiceField_b;
+    u16            wTransmitLength_b;
+    u8            bySignalField_a;
+    u8            byServiceField_a;
+    u16            wTransmitLength_a;
+    u16            wDuration_b;
+    u16            wDuration_a;
+    u16            wTimeStampOff_b;
+    u16            wTimeStampOff_a;
 
 } TX_NAF_G_CTS_MIC, *PTX_NAF_G_CTS_MIC;
 
-
 typedef struct tagSTX_NAF_G_BEACON
 {
-    WORD            wFIFOCtl;
-    WORD            wTimeStamp;
+    u16            wFIFOCtl;
+    u16            wTimeStamp;
 
     //CTS
-    BYTE            byCTSSignalField_b;
-    BYTE            byCTSServiceField_b;
-    WORD            wCTSTransmitLength_b;
-    WORD            wCTSDuration_ba;
-    WORD            wReserved1;
+    u8            byCTSSignalField_b;
+    u8            byCTSServiceField_b;
+    u16            wCTSTransmitLength_b;
+    u16            wCTSDuration_ba;
+    u16            wReserved1;
     SCTSDataF       sCTS;
 
     //Data
-    BYTE            bySignalField_a;
-    BYTE            byServiceField_a;
-    WORD            wTransmitLength_a;
-    WORD            wDuration_a;
-    WORD            wTimeStampOff_a;
-
+    u8            bySignalField_a;
+    u8            byServiceField_a;
+    u16            wTransmitLength_a;
+    u16            wDuration_a;
+    u16            wTimeStampOff_a;
 
 } TX_NAF_G_BEACON, *PTX_NAF_G_BEACON;
 
-
 typedef struct tagSTX_NAF_AB_RTS
 {
     //RsvTime
-    WORD            wRTSTxRrvTime_ab;
-    WORD            wTxRrvTime_ab;
+    u16            wRTSTxRrvTime_ab;
+    u16            wTxRrvTime_ab;
 
     //RTS
-    BYTE            byRTSSignalField_ab;
-    BYTE            byRTSServiceField_ab;
-    WORD            wRTSTransmitLength_ab;
-    WORD            wRTSDuration_ab;
-    WORD            wReserved2;
+    u8            byRTSSignalField_ab;
+    u8            byRTSServiceField_ab;
+    u16            wRTSTransmitLength_ab;
+    u16            wRTSDuration_ab;
+    u16            wReserved2;
     SRTSDataF       sRTS;
 
     //Data
-    BYTE            bySignalField_ab;
-    BYTE            byServiceField_ab;
-    WORD            wTransmitLength_ab;
-    WORD            wDuration_ab;
-    WORD            wTimeStampOff_ab;
-
+    u8            bySignalField_ab;
+    u8            byServiceField_ab;
+    u16            wTransmitLength_ab;
+    u16            wDuration_ab;
+    u16            wTimeStampOff_ab;
 
 } TX_NAF_AB_RTS, *PTX_NAF_AB_RTS;
 
-
 typedef struct tagSTX_NAF_AB_RTS_MIC
 {
     //RsvTime
-    WORD            wRTSTxRrvTime_ab;
-    WORD            wTxRrvTime_ab;
+    u16            wRTSTxRrvTime_ab;
+    u16            wTxRrvTime_ab;
 
     SMICHDR         sMICHDR;
 
     //RTS
-    BYTE            byRTSSignalField_ab;
-    BYTE            byRTSServiceField_ab;
-    WORD            wRTSTransmitLength_ab;
-    WORD            wRTSDuration_ab;
-    WORD            wReserved2;
+    u8            byRTSSignalField_ab;
+    u8            byRTSServiceField_ab;
+    u16            wRTSTransmitLength_ab;
+    u16            wRTSDuration_ab;
+    u16            wReserved2;
     SRTSDataF       sRTS;
 
     //Data
-    BYTE            bySignalField_ab;
-    BYTE            byServiceField_ab;
-    WORD            wTransmitLength_ab;
-    WORD            wDuration_ab;
-    WORD            wTimeStampOff_ab;
-
+    u8            bySignalField_ab;
+    u8            byServiceField_ab;
+    u16            wTransmitLength_ab;
+    u16            wDuration_ab;
+    u16            wTimeStampOff_ab;
 
 } TX_NAF_AB_RTS_MIC, *PTX_NAF_AB_RTS_MIC;
 
-
-
 typedef struct tagSTX_NAF_AB_CTS
 {
     //RsvTime
-    WORD            wReserved2;
-    WORD            wTxRrvTime_ab;
+    u16            wReserved2;
+    u16            wTxRrvTime_ab;
 
     //Data
-    BYTE            bySignalField_ab;
-    BYTE            byServiceField_ab;
-    WORD            wTransmitLength_ab;
-    WORD            wDuration_ab;
-    WORD            wTimeStampOff_ab;
+    u8            bySignalField_ab;
+    u8            byServiceField_ab;
+    u16            wTransmitLength_ab;
+    u16            wDuration_ab;
+    u16            wTimeStampOff_ab;
 
 } TX_NAF_AB_CTS, *PTX_NAF_AB_CTS;
 
 typedef struct tagSTX_NAF_AB_CTS_MIC
 {
     //RsvTime
-    WORD            wReserved2;
-    WORD            wTxRrvTime_ab;
+    u16            wReserved2;
+    u16            wTxRrvTime_ab;
 
     SMICHDR         sMICHDR;
 
     //Data
-    BYTE            bySignalField_ab;
-    BYTE            byServiceField_ab;
-    WORD            wTransmitLength_ab;
-    WORD            wDuration_ab;
-    WORD            wTimeStampOff_ab;
+    u8            bySignalField_ab;
+    u8            byServiceField_ab;
+    u16            wTransmitLength_ab;
+    u16            wDuration_ab;
+    u16            wTimeStampOff_ab;
 
 } TX_NAF_AB_CTS_MIC, *PTX_NAF_AB_CTS_MIC;
 
-
 typedef struct tagSTX_NAF_AB_BEACON
 {
-    WORD            wFIFOCtl;
-    WORD            wTimeStamp;
+    u16            wFIFOCtl;
+    u16            wTimeStamp;
 
    //Data
-    BYTE            bySignalField_ab;
-    BYTE            byServiceField_ab;
-    WORD            wTransmitLength_ab;
-    WORD            wDuration_ab;
-    WORD            wTimeStampOff_ab;
+    u8            bySignalField_ab;
+    u8            byServiceField_ab;
+    u16            wTransmitLength_ab;
+    u16            wDuration_ab;
+    u16            wTimeStampOff_ab;
 
 } TX_NAF_AB_BEACON, *PTX_NAF_AB_BEACON;
 
 typedef struct tagSTX_AF_G_RTS
 {
     //RsvTime
-    WORD            wRTSTxRrvTime_ba;
-    WORD            wRTSTxRrvTime_aa;
-    WORD            wRTSTxRrvTime_bb;
-    WORD            wReserved2;
-    WORD            wTxRrvTime_b;
-    WORD            wTxRrvTime_a;
+    u16            wRTSTxRrvTime_ba;
+    u16            wRTSTxRrvTime_aa;
+    u16            wRTSTxRrvTime_bb;
+    u16            wReserved2;
+    u16            wTxRrvTime_b;
+    u16            wTxRrvTime_a;
 
     //RTS
-    BYTE            byRTSSignalField_b;
-    BYTE            byRTSServiceField_b;
-    WORD            wRTSTransmitLength_b;
-    BYTE            byRTSSignalField_a;
-    BYTE            byRTSServiceField_a;
-    WORD            wRTSTransmitLength_a;
-    WORD            wRTSDuration_ba;
-    WORD            wRTSDuration_aa;
-    WORD            wRTSDuration_bb;
-    WORD            wReserved3;
-    WORD            wRTSDuration_ba_f0;
-    WORD            wRTSDuration_aa_f0;
-    WORD            wRTSDuration_ba_f1;
-    WORD            wRTSDuration_aa_f1;
+    u8            byRTSSignalField_b;
+    u8            byRTSServiceField_b;
+    u16            wRTSTransmitLength_b;
+    u8            byRTSSignalField_a;
+    u8            byRTSServiceField_a;
+    u16            wRTSTransmitLength_a;
+    u16            wRTSDuration_ba;
+    u16            wRTSDuration_aa;
+    u16            wRTSDuration_bb;
+    u16            wReserved3;
+    u16            wRTSDuration_ba_f0;
+    u16            wRTSDuration_aa_f0;
+    u16            wRTSDuration_ba_f1;
+    u16            wRTSDuration_aa_f1;
     SRTSDataF       sRTS;
 
     //Data
-    BYTE            bySignalField_b;
-    BYTE            byServiceField_b;
-    WORD            wTransmitLength_b;
-    BYTE            bySignalField_a;
-    BYTE            byServiceField_a;
-    WORD            wTransmitLength_a;
-    WORD            wDuration_b;
-    WORD            wDuration_a;
-    WORD            wDuration_a_f0;
-    WORD            wDuration_a_f1;
-    WORD            wTimeStampOff_b;
-    WORD            wTimeStampOff_a;
+    u8            bySignalField_b;
+    u8            byServiceField_b;
+    u16            wTransmitLength_b;
+    u8            bySignalField_a;
+    u8            byServiceField_a;
+    u16            wTransmitLength_a;
+    u16            wDuration_b;
+    u16            wDuration_a;
+    u16            wDuration_a_f0;
+    u16            wDuration_a_f1;
+    u16            wTimeStampOff_b;
+    u16            wTimeStampOff_a;
 
 } TX_AF_G_RTS, *PTX_AF_G_RTS;
 
-
 typedef struct tagSTX_AF_G_RTS_MIC
 {
     //RsvTime
-    WORD            wRTSTxRrvTime_ba;
-    WORD            wRTSTxRrvTime_aa;
-    WORD            wRTSTxRrvTime_bb;
-    WORD            wReserved2;
-    WORD            wTxRrvTime_b;
-    WORD            wTxRrvTime_a;
+    u16            wRTSTxRrvTime_ba;
+    u16            wRTSTxRrvTime_aa;
+    u16            wRTSTxRrvTime_bb;
+    u16            wReserved2;
+    u16            wTxRrvTime_b;
+    u16            wTxRrvTime_a;
 
     SMICHDR         sMICHDR;
 
     //RTS
-    BYTE            byRTSSignalField_b;
-    BYTE            byRTSServiceField_b;
-    WORD            wRTSTransmitLength_b;
-    BYTE            byRTSSignalField_a;
-    BYTE            byRTSServiceField_a;
-    WORD            wRTSTransmitLength_a;
-    WORD            wRTSDuration_ba;
-    WORD            wRTSDuration_aa;
-    WORD            wRTSDuration_bb;
-    WORD            wReserved3;
-    WORD            wRTSDuration_ba_f0;
-    WORD            wRTSDuration_aa_f0;
-    WORD            wRTSDuration_ba_f1;
-    WORD            wRTSDuration_aa_f1;
+    u8            byRTSSignalField_b;
+    u8            byRTSServiceField_b;
+    u16            wRTSTransmitLength_b;
+    u8            byRTSSignalField_a;
+    u8            byRTSServiceField_a;
+    u16            wRTSTransmitLength_a;
+    u16            wRTSDuration_ba;
+    u16            wRTSDuration_aa;
+    u16            wRTSDuration_bb;
+    u16            wReserved3;
+    u16            wRTSDuration_ba_f0;
+    u16            wRTSDuration_aa_f0;
+    u16            wRTSDuration_ba_f1;
+    u16            wRTSDuration_aa_f1;
     SRTSDataF       sRTS;
 
     //Data
-    BYTE            bySignalField_b;
-    BYTE            byServiceField_b;
-    WORD            wTransmitLength_b;
-    BYTE            bySignalField_a;
-    BYTE            byServiceField_a;
-    WORD            wTransmitLength_a;
-    WORD            wDuration_b;
-    WORD            wDuration_a;
-    WORD            wDuration_a_f0;
-    WORD            wDuration_a_f1;
-    WORD            wTimeStampOff_b;
-    WORD            wTimeStampOff_a;
+    u8            bySignalField_b;
+    u8            byServiceField_b;
+    u16            wTransmitLength_b;
+    u8            bySignalField_a;
+    u8            byServiceField_a;
+    u16            wTransmitLength_a;
+    u16            wDuration_b;
+    u16            wDuration_a;
+    u16            wDuration_a_f0;
+    u16            wDuration_a_f1;
+    u16            wTimeStampOff_b;
+    u16            wTimeStampOff_a;
 
 } TX_AF_G_RTS_MIC, *PTX_AF_G_RTS_MIC;
 
-
-
 typedef struct tagSTX_AF_G_CTS
 {
     //RsvTime
-    WORD            wCTSTxRrvTime_ba;
-    WORD            wReserved2;
-    WORD            wTxRrvTime_b;
-    WORD            wTxRrvTime_a;
+    u16            wCTSTxRrvTime_ba;
+    u16            wReserved2;
+    u16            wTxRrvTime_b;
+    u16            wTxRrvTime_a;
 
     //CTS
-    BYTE            byCTSSignalField_b;
-    BYTE            byCTSServiceField_b;
-    WORD            wCTSTransmitLength_b;
-    WORD            wCTSDuration_ba;
-    WORD            wReserved3;
-    WORD            wCTSDuration_ba_f0;
-    WORD            wCTSDuration_ba_f1;
+    u8            byCTSSignalField_b;
+    u8            byCTSServiceField_b;
+    u16            wCTSTransmitLength_b;
+    u16            wCTSDuration_ba;
+    u16            wReserved3;
+    u16            wCTSDuration_ba_f0;
+    u16            wCTSDuration_ba_f1;
     SCTSDataF       sCTS;
 
     //Data
-    BYTE            bySignalField_b;
-    BYTE            byServiceField_b;
-    WORD            wTransmitLength_b;
-    BYTE            bySignalField_a;
-    BYTE            byServiceField_a;
-    WORD            wTransmitLength_a;
-    WORD            wDuration_b;
-    WORD            wDuration_a;
-    WORD            wDuration_a_f0;
-    WORD            wDuration_a_f1;
-    WORD            wTimeStampOff_b;
-    WORD            wTimeStampOff_a;
+    u8            bySignalField_b;
+    u8            byServiceField_b;
+    u16            wTransmitLength_b;
+    u8            bySignalField_a;
+    u8            byServiceField_a;
+    u16            wTransmitLength_a;
+    u16            wDuration_b;
+    u16            wDuration_a;
+    u16            wDuration_a_f0;
+    u16            wDuration_a_f1;
+    u16            wTimeStampOff_b;
+    u16            wTimeStampOff_a;
 
 } TX_AF_G_CTS, *PTX_AF_G_CTS;
 
-
 typedef struct tagSTX_AF_G_CTS_MIC
 {
     //RsvTime
-    WORD            wCTSTxRrvTime_ba;
-    WORD            wReserved2;
-    WORD            wTxRrvTime_b;
-    WORD            wTxRrvTime_a;
-
+    u16            wCTSTxRrvTime_ba;
+    u16            wReserved2;
+    u16            wTxRrvTime_b;
+    u16            wTxRrvTime_a;
 
     SMICHDR         sMICHDR;
 
     //CTS
-    BYTE            byCTSSignalField_b;
-    BYTE            byCTSServiceField_b;
-    WORD            wCTSTransmitLength_b;
-    WORD            wCTSDuration_ba;
-    WORD            wReserved3;
-    WORD            wCTSDuration_ba_f0;
-    WORD            wCTSDuration_ba_f1;
+    u8            byCTSSignalField_b;
+    u8            byCTSServiceField_b;
+    u16            wCTSTransmitLength_b;
+    u16            wCTSDuration_ba;
+    u16            wReserved3;
+    u16            wCTSDuration_ba_f0;
+    u16            wCTSDuration_ba_f1;
     SCTSDataF       sCTS;
 
     //Data
-    BYTE            bySignalField_b;
-    BYTE            byServiceField_b;
-    WORD            wTransmitLength_b;
-    BYTE            bySignalField_a;
-    BYTE            byServiceField_a;
-    WORD            wTransmitLength_a;
-    WORD            wDuration_b;
-    WORD            wDuration_a;
-    WORD            wDuration_a_f0;
-    WORD            wDuration_a_f1;
-    WORD            wTimeStampOff_b;
-    WORD            wTimeStampOff_a;
+    u8            bySignalField_b;
+    u8            byServiceField_b;
+    u16            wTransmitLength_b;
+    u8            bySignalField_a;
+    u8            byServiceField_a;
+    u16            wTransmitLength_a;
+    u16            wDuration_b;
+    u16            wDuration_a;
+    u16            wDuration_a_f0;
+    u16            wDuration_a_f1;
+    u16            wTimeStampOff_b;
+    u16            wTimeStampOff_a;
 
 } TX_AF_G_CTS_MIC, *PTX_AF_G_CTS_MIC;
 
-
-
 typedef struct tagSTX_AF_A_RTS
 {
     //RsvTime
-    WORD            wRTSTxRrvTime_a;
-    WORD            wTxRrvTime_a;
+    u16            wRTSTxRrvTime_a;
+    u16            wTxRrvTime_a;
 
     //RTS
-    BYTE            byRTSSignalField_a;
-    BYTE            byRTSServiceField_a;
-    WORD            wRTSTransmitLength_a;
-    WORD            wRTSDuration_a;
-    WORD            wReserved2;
-    WORD            wRTSDuration_a_f0;
-    WORD            wRTSDuration_a_f1;
+    u8            byRTSSignalField_a;
+    u8            byRTSServiceField_a;
+    u16            wRTSTransmitLength_a;
+    u16            wRTSDuration_a;
+    u16            wReserved2;
+    u16            wRTSDuration_a_f0;
+    u16            wRTSDuration_a_f1;
     SRTSDataF       sRTS;
 
     //Data
-    BYTE            bySignalField_a;
-    BYTE            byServiceField_a;
-    WORD            wTransmitLength_a;
-    WORD            wDuration_a;
-    WORD            wTimeStampOff_a;
-    WORD            wDuration_a_f0;
-    WORD            wDuration_a_f1;
+    u8            bySignalField_a;
+    u8            byServiceField_a;
+    u16            wTransmitLength_a;
+    u16            wDuration_a;
+    u16            wTimeStampOff_a;
+    u16            wDuration_a_f0;
+    u16            wDuration_a_f1;
 
 } TX_AF_A_RTS, *PTX_AF_A_RTS;
 
-
 typedef struct tagSTX_AF_A_RTS_MIC
 {
     //RsvTime
-    WORD            wRTSTxRrvTime_a;
-    WORD            wTxRrvTime_a;
+    u16            wRTSTxRrvTime_a;
+    u16            wTxRrvTime_a;
 
     SMICHDR         sMICHDR;
 
     //RTS
-    BYTE            byRTSSignalField_a;
-    BYTE            byRTSServiceField_a;
-    WORD            wRTSTransmitLength_a;
-    WORD            wRTSDuration_a;
-    WORD            wReserved2;
-    WORD            wRTSDuration_a_f0;
-    WORD            wRTSDuration_a_f1;
+    u8            byRTSSignalField_a;
+    u8            byRTSServiceField_a;
+    u16            wRTSTransmitLength_a;
+    u16            wRTSDuration_a;
+    u16            wReserved2;
+    u16            wRTSDuration_a_f0;
+    u16            wRTSDuration_a_f1;
     SRTSDataF       sRTS;
 
     //Data
-    BYTE            bySignalField_a;
-    BYTE            byServiceField_a;
-    WORD            wTransmitLength_a;
-    WORD            wDuration_a;
-    WORD            wTimeStampOff_a;
-    WORD            wDuration_a_f0;
-    WORD            wDuration_a_f1;
+    u8            bySignalField_a;
+    u8            byServiceField_a;
+    u16            wTransmitLength_a;
+    u16            wDuration_a;
+    u16            wTimeStampOff_a;
+    u16            wDuration_a_f0;
+    u16            wDuration_a_f1;
 
 } TX_AF_A_RTS_MIC, *PTX_AF_A_RTS_MIC;
 
-
-
 typedef struct tagSTX_AF_A_CTS
 {
     //RsvTime
-    WORD            wReserved2;
-    WORD            wTxRrvTime_a;
+    u16            wReserved2;
+    u16            wTxRrvTime_a;
 
     //Data
-    BYTE            bySignalField_a;
-    BYTE            byServiceField_a;
-    WORD            wTransmitLength_a;
-    WORD            wDuration_a;
-    WORD            wTimeStampOff_a;
-    WORD            wDuration_a_f0;
-    WORD            wDuration_a_f1;
+    u8            bySignalField_a;
+    u8            byServiceField_a;
+    u16            wTransmitLength_a;
+    u16            wDuration_a;
+    u16            wTimeStampOff_a;
+    u16            wDuration_a_f0;
+    u16            wDuration_a_f1;
 
 } TX_AF_A_CTS, *PTX_AF_A_CTS;
 
-
 typedef struct tagSTX_AF_A_CTS_MIC
 {
     //RsvTime
-    WORD            wReserved2;
-    WORD            wTxRrvTime_a;
+    u16            wReserved2;
+    u16            wTxRrvTime_a;
 
     SMICHDR         sMICHDR;
 
     //Data
-    BYTE            bySignalField_a;
-    BYTE            byServiceField_a;
-    WORD            wTransmitLength_a;
-    WORD            wDuration_a;
-    WORD            wTimeStampOff_a;
-    WORD            wDuration_a_f0;
-    WORD            wDuration_a_f1;
+    u8            bySignalField_a;
+    u8            byServiceField_a;
+    u16            wTransmitLength_a;
+    u16            wDuration_a;
+    u16            wTimeStampOff_a;
+    u16            wDuration_a_f0;
+    u16            wDuration_a_f1;
 
 } TX_AF_A_CTS_MIC, *PTX_AF_A_CTS_MIC;
 
-
 //
 // union with all of the TX Buffer Type
 //
@@ -620,51 +591,43 @@ typedef union tagUTX_BUFFER_CONTAINER
 
 } TX_BUFFER_CONTAINER, *PTX_BUFFER_CONTAINER;
 
-
 //
 // Remote NDIS message format
 //
 typedef struct tagSTX_BUFFER
 {
-    BYTE                            byType;
-    BYTE                            byPKTNO;
-    WORD                            wTxByteCount;
+    u8                            byType;
+    u8                            byPKTNO;
+    u16                            wTxByteCount;
 
 	u32 adwTxKey[4];
-    WORD                            wFIFOCtl;
-    WORD                            wTimeStamp;
-    WORD                            wFragCtl;
-    WORD                            wReserved;
-
+    u16                            wFIFOCtl;
+    u16                            wTimeStamp;
+    u16                            wFragCtl;
+    u16                            wReserved;
 
     // Actual message
     TX_BUFFER_CONTAINER             BufferHeader;
 
 } TX_BUFFER, *PTX_BUFFER;
 
-
 //
 // Remote NDIS message format
 //
 typedef struct tagSBEACON_BUFFER
 {
-    BYTE                            byType;
-    BYTE                            byPKTNO;
-    WORD                            wTxByteCount;
+    u8                            byType;
+    u8                            byPKTNO;
+    u16                            wTxByteCount;
 
-    WORD                            wFIFOCtl;
-    WORD                            wTimeStamp;
+    u16                            wFIFOCtl;
+    u16                            wTimeStamp;
 
     // Actual message
     TX_BUFFER_CONTAINER             BufferHeader;
 
 } BEACON_BUFFER, *PBEACON_BUFFER;
 
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
 void vDMA0_tx_80211(struct vnt_private *, struct sk_buff *skb);
 int nsDMA_tx_packet(struct vnt_private *, u32 uDMAIdx, struct sk_buff *skb);
 CMD_STATUS csMgmt_xmit(struct vnt_private *, struct vnt_tx_mgmt *);
diff --git a/drivers/staging/vt6656/srom.h b/drivers/staging/vt6656/srom.h
index dba21a54414..488192d79ac 100644
--- a/drivers/staging/vt6656/srom.h
+++ b/drivers/staging/vt6656/srom.h
@@ -30,10 +30,6 @@
 #ifndef __SROM_H__
 #define __SROM_H__
 
-#include "ttype.h"
-
-/*---------------------  Export Definitions -------------------------*/
-
 #define EEP_MAX_CONTEXT_SIZE    256
 
 #define CB_EEPROM_READBYTE_WAIT 900     //us
@@ -56,7 +52,6 @@
 #define EEP_OFS_SETPT_CCK   0x21
 #define EEP_OFS_PWR_OFDMG   0x23
 
-
 #define EEP_OFS_CALIB_TX_IQ 0x24
 #define EEP_OFS_CALIB_TX_DC 0x25
 #define EEP_OFS_CALIB_RX_IQ 0x26
@@ -80,48 +75,38 @@
 //
 #define EEP_RADIOCTL_ENABLE 0x80
 
-/*---------------------  Export Types  ------------------------------*/
-
 // AT24C02 eeprom contents
 //      2048 bits = 256 bytes = 128 words
 //
 typedef struct tagSSromReg {
-    BYTE    abyPAR[6];                  // 0x00 (WORD)
-
-    WORD    wSUB_VID;                   // 0x03 (WORD)
-    WORD    wSUB_SID;
-
-    BYTE    byBCFG0;                    // 0x05 (WORD)
-    BYTE    byBCFG1;
-
-    BYTE    byFCR0;                     // 0x06 (WORD)
-    BYTE    byFCR1;
-    BYTE    byPMC0;                     // 0x07 (WORD)
-    BYTE    byPMC1;
-    BYTE    byMAXLAT;                   // 0x08 (WORD)
-    BYTE    byMINGNT;
-    BYTE    byCFG0;                     // 0x09 (WORD)
-    BYTE    byCFG1;
-    WORD    wCISPTR;                    // 0x0A (WORD)
-    WORD    wRsv0;                      // 0x0B (WORD)
-    WORD    wRsv1;                      // 0x0C (WORD)
-    BYTE    byBBPAIR;                   // 0x0D (WORD)
-    BYTE    byRFTYPE;
-    BYTE    byMinChannel;               // 0x0E (WORD)
-    BYTE    byMaxChannel;
-    BYTE    bySignature;                // 0x0F (WORD)
-    BYTE    byCheckSum;
-
-    BYTE    abyReserved0[96];           // 0x10 (WORD)
-    BYTE    abyCIS[128];                // 0x80 (WORD)
+    u8    abyPAR[6];                  // 0x00 (u16)
+
+    u16    wSUB_VID;                   // 0x03 (u16)
+    u16    wSUB_SID;
+
+    u8    byBCFG0;                    // 0x05 (u16)
+    u8    byBCFG1;
+
+    u8    byFCR0;                     // 0x06 (u16)
+    u8    byFCR1;
+    u8    byPMC0;                     // 0x07 (u16)
+    u8    byPMC1;
+    u8    byMAXLAT;                   // 0x08 (u16)
+    u8    byMINGNT;
+    u8    byCFG0;                     // 0x09 (u16)
+    u8    byCFG1;
+    u16    wCISPTR;                    // 0x0A (u16)
+    u16    wRsv0;                      // 0x0B (u16)
+    u16    wRsv1;                      // 0x0C (u16)
+    u8    byBBPAIR;                   // 0x0D (u16)
+    u8    byRFTYPE;
+    u8    byMinChannel;               // 0x0E (u16)
+    u8    byMaxChannel;
+    u8    bySignature;                // 0x0F (u16)
+    u8    byCheckSum;
+
+    u8    abyReserved0[96];           // 0x10 (u16)
+    u8    abyCIS[128];                // 0x80 (u16)
 } SSromReg, *PSSromReg;
 
-/*---------------------  Export Macros ------------------------------*/
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
 #endif /* __EEPROM_H__ */
diff --git a/drivers/staging/vt6656/tcrc.c b/drivers/staging/vt6656/tcrc.c
index 2237eeb5ec5..7229f26c052 100644
--- a/drivers/staging/vt6656/tcrc.c
+++ b/drivers/staging/vt6656/tcrc.c
@@ -35,14 +35,8 @@
 
 #include "tcrc.h"
 
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
 /* 32-bit CRC table */
-static const DWORD s_adwCrc32Table[256] = {
+static const u32 s_adwCrc32Table[256] = {
     0x00000000L, 0x77073096L, 0xEE0E612CL, 0x990951BAL,
     0x076DC419L, 0x706AF48FL, 0xE963A535L, 0x9E6495A3L,
     0x0EDB8832L, 0x79DCB8A4L, 0xE0D5E91EL, 0x97D2D988L,
@@ -109,13 +103,6 @@ static const DWORD s_adwCrc32Table[256] = {
     0xB40BBE37L, 0xC30C8EA1L, 0x5A05DF1BL, 0x2D02EF8DL
 };
 
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-
-
-
 /*+
  *
  * Description:
@@ -132,13 +119,13 @@ static const DWORD s_adwCrc32Table[256] = {
  * Return Value: CRC-32
  *
 -*/
-DWORD CRCdwCrc32(PBYTE pbyData, unsigned int cbByte, DWORD dwCrcSeed)
+u32 CRCdwCrc32(u8 * pbyData, unsigned int cbByte, u32 dwCrcSeed)
 {
-	DWORD dwCrc;
+	u32 dwCrc;
 
 	dwCrc = dwCrcSeed;
 	while (cbByte--) {
-		dwCrc = s_adwCrc32Table[(BYTE)((dwCrc ^ (*pbyData)) & 0xFF)] ^
+		dwCrc = s_adwCrc32Table[(u8)((dwCrc ^ (*pbyData)) & 0xFF)] ^
 			(dwCrc >> 8);
 		pbyData++;
 	}
@@ -146,7 +133,6 @@ DWORD CRCdwCrc32(PBYTE pbyData, unsigned int cbByte, DWORD dwCrcSeed)
 	return dwCrc;
 }
 
-
 /*+
  *
  * Description:
@@ -165,12 +151,11 @@ DWORD CRCdwCrc32(PBYTE pbyData, unsigned int cbByte, DWORD dwCrcSeed)
  * Return Value: CRC-32
  *
 -*/
-DWORD CRCdwGetCrc32(PBYTE pbyData, unsigned int cbByte)
+u32 CRCdwGetCrc32(u8 * pbyData, unsigned int cbByte)
 {
     return ~CRCdwCrc32(pbyData, cbByte, 0xFFFFFFFFL);
 }
 
-
 /*+
  *
  * Description:
@@ -191,9 +176,8 @@ DWORD CRCdwGetCrc32(PBYTE pbyData, unsigned int cbByte)
  * Return Value: CRC-32
  *
 -*/
-DWORD CRCdwGetCrc32Ex(PBYTE pbyData, unsigned int cbByte, DWORD dwPreCRC)
+u32 CRCdwGetCrc32Ex(u8 * pbyData, unsigned int cbByte, u32 dwPreCRC)
 {
     return CRCdwCrc32(pbyData, cbByte, dwPreCRC);
 }
 
-
diff --git a/drivers/staging/vt6656/tcrc.h b/drivers/staging/vt6656/tcrc.h
index dc54bd8fc4f..5b1f368964e 100644
--- a/drivers/staging/vt6656/tcrc.h
+++ b/drivers/staging/vt6656/tcrc.h
@@ -29,22 +29,10 @@
 #ifndef __TCRC_H__
 #define __TCRC_H__
 
-#include "ttype.h"
+#include <linux/types.h>
 
-/*---------------------  Export Definitions -------------------------*/
-
-/*---------------------  Export Types  ------------------------------*/
-
-/*---------------------  Export Macros ------------------------------*/
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-DWORD CRCdwCrc32(PBYTE pbyData, unsigned int cbByte, DWORD dwCrcSeed);
-DWORD CRCdwGetCrc32(PBYTE pbyData, unsigned int cbByte);
-DWORD CRCdwGetCrc32Ex(PBYTE pbyData, unsigned int cbByte, DWORD dwPreCRC);
+u32 CRCdwCrc32(u8 * pbyData, unsigned int cbByte, u32 dwCrcSeed);
+u32 CRCdwGetCrc32(u8 * pbyData, unsigned int cbByte);
+u32 CRCdwGetCrc32Ex(u8 * pbyData, unsigned int cbByte, u32 dwPreCRC);
 
 #endif /* __TCRC_H__ */
diff --git a/drivers/staging/vt6656/tether.c b/drivers/staging/vt6656/tether.c
index 95286c4d557..1db1e8419a9 100644
--- a/drivers/staging/vt6656/tether.c
+++ b/drivers/staging/vt6656/tether.c
@@ -25,7 +25,6 @@
  * Date: May 21, 1996
  *
  * Functions:
- *      ETHbyGetHashIndexByCrc32 - Calculate multicast hash value by CRC32
  *      ETHbIsBufferCrc32Ok - Check CRC value of the buffer if Ok or not
  *
  * Revision History:
@@ -37,52 +36,6 @@
 #include "tcrc.h"
 #include "tether.h"
 
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-
-
-/*
- * Description: Calculate multicast hash value by CRC32
- *
- * Parameters:
- *  In:
- *		pbyMultiAddr    - Multicast Address
- *  Out:
- *      none
- *
- * Return Value: Hash value
- *
- */
-BYTE ETHbyGetHashIndexByCrc32(PBYTE pbyMultiAddr)
-{
-	int     ii;
-	BYTE    byTmpHash;
-	BYTE    byHash = 0;
-
-	/* get the least 6-bits from CRC generator */
-	byTmpHash = (BYTE)(CRCdwCrc32(pbyMultiAddr, ETH_ALEN,
-			0xFFFFFFFFL) & 0x3F);
-	/* reverse most bit to least bit */
-	for (ii = 0; ii < (sizeof(byTmpHash) * 8); ii++) {
-		byHash <<= 1;
-		if (byTmpHash & 0x01)
-			byHash |= 1;
-		byTmpHash >>= 1;
-	}
-
-	/* adjust 6-bits to the right most */
-	return byHash >> 2;
-}
-
-
 /*
  * Description: Check CRC value of the buffer if Ok or not
  *
@@ -96,12 +49,12 @@ BYTE ETHbyGetHashIndexByCrc32(PBYTE pbyMultiAddr)
  * Return Value: true if ok; false if error.
  *
  */
-bool ETHbIsBufferCrc32Ok(PBYTE pbyBuffer, unsigned int cbFrameLength)
+bool ETHbIsBufferCrc32Ok(u8 * pbyBuffer, unsigned int cbFrameLength)
 {
-	DWORD dwCRC;
+	u32 dwCRC;
 
 	dwCRC = CRCdwGetCrc32(pbyBuffer, cbFrameLength - 4);
-	if (cpu_to_le32(*((PDWORD)(pbyBuffer + cbFrameLength - 4))) != dwCRC)
+	if (cpu_to_le32(*((u32 *)(pbyBuffer + cbFrameLength - 4))) != dwCRC)
 		return false;
 	return true;
 }
diff --git a/drivers/staging/vt6656/tether.h b/drivers/staging/vt6656/tether.h
index 2f8f4853fd9..24465cfe3e6 100644
--- a/drivers/staging/vt6656/tether.h
+++ b/drivers/staging/vt6656/tether.h
@@ -30,9 +30,7 @@
 #define __TETHER_H__
 
 #include <linux/if_ether.h>
-#include "ttype.h"
 
-/*---------------------  Export Definitions -------------------------*/
 //
 // constants
 //
@@ -44,11 +42,6 @@
 
 #define TYPE_MGMT_PROBE_RSP 0x5000
 
-//
-// wFrameCtl field in the S802_11Header
-//
-// NOTE....
-//   in network byte order, high byte is going first
 #define FC_TODS             0x0001
 #define FC_FROMDS           0x0002
 #define FC_MOREFRAG         0x0004
@@ -71,8 +64,6 @@
 #define TYPE_CTL_CTS        0xc400
 #define TYPE_CTL_ACK        0xd400
 
-
-
 #else //if LITTLE_ENDIAN
 //
 // wType field in the SEthernetHeader
@@ -82,11 +73,6 @@
 
 #define TYPE_MGMT_PROBE_RSP 0x0050
 
-//
-// wFrameCtl field in the S802_11Header
-//
-// NOTE....
-//   in network byte order, high byte is going first
 #define FC_TODS             0x0100
 #define FC_FROMDS           0x0200
 #define FC_MOREFRAG         0x0400
@@ -109,58 +95,21 @@
 #define TYPE_CTL_CTS        0x00c4
 #define TYPE_CTL_ACK        0x00d4
 
-
-
 #endif //#ifdef __BIG_ENDIAN
 
 #define WEP_IV_MASK         0x00FFFFFF
 
-/*---------------------  Export Types  ------------------------------*/
-//
-// Ethernet packet
-//
-typedef struct tagSEthernetHeader {
-    BYTE    abyDstAddr[ETH_ALEN];
-    BYTE    abySrcAddr[ETH_ALEN];
-    WORD    wType;
-} __attribute__ ((__packed__))
-SEthernetHeader, *PSEthernetHeader;
-
-
 //
 // 802_3 packet
 //
 typedef struct tagS802_3Header {
-    BYTE    abyDstAddr[ETH_ALEN];
-    BYTE    abySrcAddr[ETH_ALEN];
-    WORD    wLen;
+    u8    abyDstAddr[ETH_ALEN];
+    u8    abySrcAddr[ETH_ALEN];
+    u16    wLen;
 } __attribute__ ((__packed__))
 S802_3Header, *PS802_3Header;
 
-//
-// 802_11 packet
-//
-typedef struct tagS802_11Header {
-    WORD    wFrameCtl;
-    WORD    wDurationID;
-    BYTE    abyAddr1[ETH_ALEN];
-    BYTE    abyAddr2[ETH_ALEN];
-    BYTE    abyAddr3[ETH_ALEN];
-    WORD    wSeqCtl;
-    BYTE    abyAddr4[ETH_ALEN];
-} __attribute__ ((__packed__))
-S802_11Header, *PS802_11Header;
-
-/*---------------------  Export Macros ------------------------------*/
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-BYTE ETHbyGetHashIndexByCrc32(PBYTE pbyMultiAddr);
-//BYTE ETHbyGetHashIndexByCrc(PBYTE pbyMultiAddr);
-bool ETHbIsBufferCrc32Ok(PBYTE pbyBuffer, unsigned int cbFrameLength);
+//u8 ETHbyGetHashIndexByCrc(u8 * pbyMultiAddr);
+bool ETHbIsBufferCrc32Ok(u8 * pbyBuffer, unsigned int cbFrameLength);
 
 #endif /* __TETHER_H__ */
diff --git a/drivers/staging/vt6656/tkip.c b/drivers/staging/vt6656/tkip.c
index 282c08d6576..9d643e449ac 100644
--- a/drivers/staging/vt6656/tkip.c
+++ b/drivers/staging/vt6656/tkip.c
@@ -35,27 +35,11 @@
 #include "tmacro.h"
 #include "tkip.h"
 
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
 /* The Sbox is reduced to 2 16-bit wide tables, each with 256 entries. */
 /* The 2nd table is the same as the 1st but with the upper and lower   */
 /* bytes swapped. To allow an endian tolerant implementation, the byte */
 /* halves have been expressed independently here.                      */
-const BYTE TKIP_Sbox_Lower[256] = {
+const u8 TKIP_Sbox_Lower[256] = {
     0xA5,0x84,0x99,0x8D,0x0D,0xBD,0xB1,0x54,
     0x50,0x03,0xA9,0x7D,0x19,0x62,0xE6,0x9A,
     0x45,0x9D,0x40,0x87,0x15,0xEB,0xC9,0x0B,
@@ -90,7 +74,7 @@ const BYTE TKIP_Sbox_Lower[256] = {
     0xC3,0xB0,0x77,0x11,0xCB,0xFC,0xD6,0x3A
 };
 
-const BYTE TKIP_Sbox_Upper[256] = {
+const u8 TKIP_Sbox_Upper[256] = {
     0xC6,0xF8,0xEE,0xF6,0xFF,0xD6,0xDE,0x91,
     0x60,0x02,0xCE,0x56,0xE7,0xB5,0x4D,0xEC,
     0x8F,0x1F,0x89,0xFA,0xEF,0xB2,0x8E,0xFB,
@@ -125,13 +109,8 @@ const BYTE TKIP_Sbox_Upper[256] = {
     0x82,0x29,0x5A,0x1E,0x7B,0xA8,0x6D,0x2C
 };
 
-
 //STKIPKeyManagement  sTKIPKeyTable[MAX_TKIP_KEY];
 
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
 /************************************************************/
 /* tkip_sbox()                                              */
 /* Returns a 16 bit value from a 64K entry table. The Table */
@@ -152,7 +131,6 @@ static unsigned int tkip_sbox(unsigned int index)
     return (left ^ right);
 };
 
-
 static unsigned int rotr1(unsigned int a)
 {
     unsigned int b;
@@ -166,7 +144,6 @@ static unsigned int rotr1(unsigned int a)
     return b;
 }
 
-
 /*
  * Description: Calculate RC4Key fom TK, TA, and TSC
  *
@@ -182,11 +159,11 @@ static unsigned int rotr1(unsigned int a)
  *
  */
 void TKIPvMixKey(
-    PBYTE   pbyTKey,
-    PBYTE   pbyTA,
-    WORD    wTSC15_0,
-    DWORD   dwTSC47_16,
-    PBYTE   pbyRC4Key
+    u8 *   pbyTKey,
+    u8 *   pbyTA,
+    u16    wTSC15_0,
+    u32   dwTSC47_16,
+    u8 *   pbyRC4Key
     )
 {
 	u32 p1k[5];
diff --git a/drivers/staging/vt6656/tkip.h b/drivers/staging/vt6656/tkip.h
index 47c3a853b92..4fba7ef3826 100644
--- a/drivers/staging/vt6656/tkip.h
+++ b/drivers/staging/vt6656/tkip.h
@@ -30,28 +30,16 @@
 #ifndef __TKIP_H__
 #define __TKIP_H__
 
-#include "ttype.h"
 #include "tether.h"
 
-/*---------------------  Export Definitions -------------------------*/
 #define TKIP_KEY_LEN        16
 
-/*---------------------  Export Types  ------------------------------*/
-
-/*---------------------  Export Macros ------------------------------*/
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
 void TKIPvMixKey(
-    PBYTE   pbyTKey,
-    PBYTE   pbyTA,
-    WORD    wTSC15_0,
-    DWORD   dwTSC47_16,
-    PBYTE   pbyRC4Key
+    u8 *   pbyTKey,
+    u8 *   pbyTA,
+    u16    wTSC15_0,
+    u32   dwTSC47_16,
+    u8 *   pbyRC4Key
     );
 
 #endif /* __TKIP_H__ */
diff --git a/drivers/staging/vt6656/tmacro.h b/drivers/staging/vt6656/tmacro.h
index 3c81e2b0791..15cd5abb800 100644
--- a/drivers/staging/vt6656/tmacro.h
+++ b/drivers/staging/vt6656/tmacro.h
@@ -29,32 +29,30 @@
 #ifndef __TMACRO_H__
 #define __TMACRO_H__
 
-#include "ttype.h"
-
 /****** Common helper macros ***********************************************/
 
 #if !defined(LOBYTE)
-#define LOBYTE(w)           ((BYTE)(w))
+#define LOBYTE(w)           ((u8)(w))
 #endif
 #if !defined(HIBYTE)
-#define HIBYTE(w)           ((BYTE)(((WORD)(w) >> 8) & 0xFF))
+#define HIBYTE(w)           ((u8)(((u16)(w) >> 8) & 0xFF))
 #endif
 
 #if !defined(LOWORD)
-#define LOWORD(d)           ((WORD)(d))
+#define LOWORD(d)           ((u16)(d))
 #endif
 #if !defined(HIWORD)
-#define HIWORD(d)           ((WORD)((((DWORD)(d)) >> 16) & 0xFFFF))
+#define HIWORD(d)           ((u16)((((u32)(d)) >> 16) & 0xFFFF))
 #endif
 
 #define LODWORD(q)          ((q).u.dwLowDword)
 #define HIDWORD(q)          ((q).u.dwHighDword)
 
 #if !defined(MAKEWORD)
-#define MAKEWORD(lb, hb)    ((WORD)(((BYTE)(lb)) | (((WORD)((BYTE)(hb))) << 8)))
+#define MAKEWORD(lb, hb)    ((u16)(((u8)(lb)) | (((u16)((u8)(hb))) << 8)))
 #endif
 #if !defined(MAKEDWORD)
-#define MAKEDWORD(lw, hw)   ((DWORD)(((WORD)(lw)) | (((DWORD)((WORD)(hw))) << 16)))
+#define MAKEDWORD(lw, hw)   ((u32)(((u16)(lw)) | (((u32)((u16)(hw))) << 16)))
 #endif
 
 #endif /* __TMACRO_H__ */
diff --git a/drivers/staging/vt6656/ttype.h b/drivers/staging/vt6656/ttype.h
deleted file mode 100644
index d7b64894531..00000000000
--- a/drivers/staging/vt6656/ttype.h
+++ /dev/null
@@ -1,55 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * 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.
- *
- * File: ttype.h
- *
- * Purpose: define basic common types and macros
- *
- * Author: Tevin Chen
- *
- * Date: May 21, 1996
- *
- */
-
-#ifndef __TTYPE_H__
-#define __TTYPE_H__
-
-#include <linux/types.h>
-
-/******* Common definitions and typedefs ***********************************/
-
-/****** Simple typedefs  ***************************************************/
-
-typedef u8 BYTE;
-typedef u16 WORD;
-typedef u32 DWORD;
-
-/****** Common pointer types ***********************************************/
-
-typedef u32 ULONG_PTR;
-typedef u32 DWORD_PTR;
-
-// boolean pointer
-
-typedef BYTE *           PBYTE;
-
-typedef WORD *           PWORD;
-
-typedef DWORD *          PDWORD;
-
-#endif /* __TTYPE_H__ */
diff --git a/drivers/staging/vt6656/usbpipe.c b/drivers/staging/vt6656/usbpipe.c
index 00fd0f8a58c..098be609107 100644
--- a/drivers/staging/vt6656/usbpipe.c
+++ b/drivers/staging/vt6656/usbpipe.c
@@ -45,7 +45,6 @@
 #include "desc.h"
 #include "device.h"
 
-/*---------------------  Static Definitions -------------------------*/
 //endpoint def
 //endpoint 0: control
 //endpoint 1: interrupt
@@ -55,28 +54,18 @@
 //static int          msglevel                =MSG_LEVEL_DEBUG;
 static int          msglevel                =MSG_LEVEL_INFO;
 
-
 #define USB_CTL_WAIT   500 //ms
 
 #ifndef URB_ASYNC_UNLINK
 #define URB_ASYNC_UNLINK    0
 #endif
 
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
 static void s_nsInterruptUsbIoCompleteRead(struct urb *urb);
 static void s_nsBulkInUsbIoCompleteRead(struct urb *urb);
 static void s_nsBulkOutIoCompleteWrite(struct urb *urb);
 static void s_nsControlInUsbIoCompleteRead(struct urb *urb);
 static void s_nsControlInUsbIoCompleteWrite(struct urb *urb);
 
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
 int PIPEnsControlOutAsyn(struct vnt_private *pDevice, u8 byRequest,
 	u16 wValue, u16 wIndex, u16 wLength, u8 *pbyBuffer)
 {
@@ -251,8 +240,6 @@ static void s_nsControlInUsbIoCompleteWrite(struct urb *urb)
     MP_CLEAR_FLAG(pDevice, fMP_CONTROL_WRITES);
 }
 
-
-
 /*
  * Description:
  *      Complete function of usb Control callback
@@ -288,9 +275,6 @@ static void s_nsControlInUsbIoCompleteRead(struct urb *urb)
     MP_CLEAR_FLAG(pDevice, fMP_CONTROL_READS);
 }
 
-
-
-
 /*
  * Description:
  *      Allocates an usb interrupt in irp and calls USBD.
@@ -341,7 +325,6 @@ usb_fill_bulk_urb(pDevice->pInterruptURB,
     return ntStatus;
 }
 
-
 /*
  * Description:
  *      Complete function of usb interrupt in irp.
@@ -403,7 +386,6 @@ static void s_nsInterruptUsbIoCompleteRead(struct urb *urb)
 
     STAvUpdateUSBCounter(&pDevice->scStatistic.USB_InterruptStat, ntStatus);
 
-
     if (pDevice->fKillEventPollingThread != true) {
        usb_fill_bulk_urb(pDevice->pInterruptURB,
 		      pDevice->usb,
@@ -444,7 +426,6 @@ int PIPEnsBulkInUsbRead(struct vnt_private *pDevice, PRCB pRCB)
 	int ntStatus = 0;
 	struct urb *pUrb;
 
-
     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---->s_nsStartBulkInUsbRead\n");
 
     if (pDevice->Flags & fMP_DISCONNECTED)
@@ -452,7 +433,6 @@ int PIPEnsBulkInUsbRead(struct vnt_private *pDevice, PRCB pRCB)
 
     pDevice->ulBulkInPosted++;
 
-
 	pUrb = pRCB->pUrb;
     //
     // Now that we have created the urb, we will send a
@@ -482,9 +462,6 @@ int PIPEnsBulkInUsbRead(struct vnt_private *pDevice, PRCB pRCB)
     return ntStatus;
 }
 
-
-
-
 /*
  * Description:
  *      Complete function of usb BulkIn irp.
@@ -535,7 +512,6 @@ static void s_nsBulkInUsbIoCompleteRead(struct urb *urb)
            pDevice->scStatistic.RxOkCnt ++;
     }
 
-
     STAvUpdateUSBCounter(&pDevice->scStatistic.USB_BulkInStat, status);
 
     if (bIndicateReceive) {
@@ -553,7 +529,6 @@ static void s_nsBulkInUsbIoCompleteRead(struct urb *urb)
         spin_unlock(&pDevice->lock);
     }
 
-
     return;
 }
 
@@ -576,8 +551,6 @@ int PIPEnsSendBulkOut(struct vnt_private *pDevice, PUSB_SEND_CONTEXT pContext)
 	int status;
 	struct urb          *pUrb;
 
-
-
     pDevice->bPWBitOn = false;
 
 /*
@@ -657,7 +630,6 @@ static void s_nsBulkOutIoCompleteWrite(struct urb *urb)
 	unsigned long ulBufLen;
 	PUSB_SEND_CONTEXT pContext;
 
-
     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---->s_nsBulkOutIoCompleteWrite\n");
     //
     // The context given to IoSetCompletionRoutine is an USB_CONTEXT struct
@@ -705,7 +677,6 @@ static void s_nsBulkOutIoCompleteWrite(struct urb *urb)
 
         pDevice->dev->trans_start = jiffies;
 
-
         if (status == STATUS_SUCCESS) {
             pDevice->packetsSent++;
         }
diff --git a/drivers/staging/vt6656/usbpipe.h b/drivers/staging/vt6656/usbpipe.h
index b3023559c15..bb7a61111a6 100644
--- a/drivers/staging/vt6656/usbpipe.h
+++ b/drivers/staging/vt6656/usbpipe.h
@@ -30,17 +30,8 @@
 #ifndef __USBPIPE_H__
 #define __USBPIPE_H__
 
-#include "ttype.h"
 #include "device.h"
 
-/*---------------------  Export Definitions -------------------------*/
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
 int PIPEnsControlOut(struct vnt_private *, u8 byRequest, u16 wValue,
 		u16 wIndex, u16 wLength, u8 *pbyBuffer);
 int PIPEnsControlOutAsyn(struct vnt_private *, u8 byRequest,
diff --git a/drivers/staging/vt6656/wcmd.c b/drivers/staging/vt6656/wcmd.c
index 4bb652bf7cf..0013cb73d83 100644
--- a/drivers/staging/vt6656/wcmd.c
+++ b/drivers/staging/vt6656/wcmd.c
@@ -38,7 +38,6 @@
  *
  */
 
-#include "ttype.h"
 #include "tmacro.h"
 #include "device.h"
 #include "mac.h"
@@ -56,17 +55,8 @@
 #include "channel.h"
 #include "iowpa.h"
 
-/*---------------------  Static Definitions -------------------------*/
-
-
-
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
 static int          msglevel                =MSG_LEVEL_INFO;
 //static int          msglevel                =MSG_LEVEL_DEBUG;
-/*---------------------  Static Functions  --------------------------*/
 
 static void s_vProbeChannel(struct vnt_private *);
 
@@ -74,16 +64,10 @@ static struct vnt_tx_mgmt *s_MgrMakeProbeRequest(struct vnt_private *,
 	struct vnt_manager *pMgmt, u8 *pScanBSSID, PWLAN_IE_SSID pSSID,
 	PWLAN_IE_SUPP_RATES pCurrRates, PWLAN_IE_SUPP_RATES pCurrExtSuppRates);
 
-
 static int s_bCommandComplete(struct vnt_private *);
 
-
 static int s_bClearBSSID_SCAN(struct vnt_private *);
 
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
 /*
  * Description:
  *      Stop AdHoc beacon during scan process
@@ -138,7 +122,6 @@ static void vAdHocBeaconStop(struct vnt_private *pDevice)
 
 } /* vAdHocBeaconStop */
 
-
 /*
  * Description:
  *      Restart AdHoc beacon after scan process complete
@@ -171,7 +154,6 @@ static void vAdHocBeaconRestart(struct vnt_private *pDevice)
 
 }
 
-
 /*+
  *
  * Routine Description:
@@ -200,7 +182,6 @@ static void s_vProbeChannel(struct vnt_private *pDevice)
 	u8 *pbyRate;
 	int ii;
 
-
     if (pDevice->byBBType == BB_TYPE_11A) {
         pbyRate = &abyCurrSuppRatesA[0];
     } else if (pDevice->byBBType == BB_TYPE_11B) {
@@ -232,9 +213,6 @@ static void s_vProbeChannel(struct vnt_private *pDevice)
 
 }
 
-
-
-
 /*+
  *
  * Routine Description:
@@ -246,7 +224,6 @@ static void s_vProbeChannel(struct vnt_private *pDevice)
  *
 -*/
 
-
 struct vnt_tx_mgmt *s_MgrMakeProbeRequest(struct vnt_private *pDevice,
 	struct vnt_manager *pMgmt, u8 *pScanBSSID, PWLAN_IE_SSID pSSID,
 	PWLAN_IE_SUPP_RATES pCurrRates, PWLAN_IE_SUPP_RATES pCurrExtSuppRates)
@@ -254,13 +231,12 @@ struct vnt_tx_mgmt *s_MgrMakeProbeRequest(struct vnt_private *pDevice,
 	struct vnt_tx_mgmt *pTxPacket = NULL;
 	WLAN_FR_PROBEREQ sFrame;
 
-
 	pTxPacket = (struct vnt_tx_mgmt *)pMgmt->pbyMgmtPacketPool;
 	memset(pTxPacket, 0, sizeof(struct vnt_tx_mgmt)
 		+ WLAN_PROBEREQ_FR_MAXLEN);
 	pTxPacket->p80211Header = (PUWLAN_80211HDR)((u8 *)pTxPacket
 		+ sizeof(struct vnt_tx_mgmt));
-    sFrame.pBuf = (PBYTE)pTxPacket->p80211Header;
+    sFrame.pBuf = (u8 *)pTxPacket->p80211Header;
     sFrame.len = WLAN_PROBEREQ_FR_MAXLEN;
     vMgrEncodeProbeRequest(&sFrame);
     sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
@@ -316,7 +292,6 @@ void vRunCommand(struct vnt_private *pDevice)
 	u8 byMask[8] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80};
 	u8 byData;
 
-
     if (pDevice->dwDiagRefCount != 0)
         return;
     if (pDevice->bCmdRunning != true)
@@ -347,29 +322,7 @@ void vRunCommand(struct vnt_private *pDevice)
                 pMgmt->uScanChannel = pDevice->byMinChannel;
             }
             if (pMgmt->uScanChannel > pDevice->byMaxChannel) {
-                pMgmt->eScanState = WMAC_NO_SCANNING;
-
-                if (pDevice->byBBType != pDevice->byScanBBType) {
-                    pDevice->byBBType = pDevice->byScanBBType;
-                    CARDvSetBSSMode(pDevice);
-                }
-
-                if (pDevice->bUpdateBBVGA) {
-                    BBvSetShortSlotTime(pDevice);
-                    BBvSetVGAGainOffset(pDevice, pDevice->byBBVGACurrent);
-                    BBvUpdatePreEDThreshold(pDevice, false);
-                }
-                // Set channel back
-                vAdHocBeaconRestart(pDevice);
-                // Set channel back
-                CARDbSetMediaChannel(pDevice, pMgmt->uCurrChannel);
-                // Set Filter
-                if (pMgmt->bCurrBSSIDFilterOn) {
-                    MACvRegBitsOn(pDevice, MAC_REG_RCR, RCR_BSSID);
-                    pDevice->byRxMode |= RCR_BSSID;
-                }
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Scanning, set back to channel: [%d]\n", pMgmt->uCurrChannel);
-                pDevice->bStopDataPkt = false;
+		pDevice->eCommandState = WLAN_CMD_SCAN_END;
                 s_bCommandComplete(pDevice);
                 spin_unlock_irq(&pDevice->lock);
                 return;
@@ -377,6 +330,7 @@ void vRunCommand(struct vnt_private *pDevice)
             } else {
                 if (!ChannelValid(pDevice->byZoneType, pMgmt->uScanChannel)) {
                     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Invalid channel pMgmt->uScanChannel = %d \n",pMgmt->uScanChannel);
+			pMgmt->uScanChannel++;
                     s_bCommandComplete(pDevice);
                     spin_unlock_irq(&pDevice->lock);
                     return;
@@ -473,6 +427,7 @@ void vRunCommand(struct vnt_private *pDevice)
                 pDevice->byRxMode |= RCR_BSSID;
             }
             DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Scanning, set back to channel: [%d]\n", pMgmt->uCurrChannel);
+		pMgmt->uScanChannel = 0;
             pMgmt->eScanState = WMAC_NO_SCANNING;
             pDevice->bStopDataPkt = false;
 
@@ -523,7 +478,6 @@ void vRunCommand(struct vnt_private *pDevice)
             s_bCommandComplete(pDevice);
             break;
 
-
         case WLAN_CMD_SSID_START:
 
 		pDevice->byReAssocCount = 0;
@@ -845,7 +799,7 @@ void vRunCommand(struct vnt_private *pDevice)
 
        {
 	       int ntStatus = STATUS_SUCCESS;
-        BYTE            byTmp;
+        u8            byTmp;
 
         ntStatus = CONTROLnsRequestIn(pDevice,
                                     MESSAGE_TYPE_READ,
@@ -930,7 +884,6 @@ void vRunCommand(struct vnt_private *pDevice)
             s_bCommandComplete(pDevice);
             break;
 
-
         case WLAN_CMD_CHANGE_BBSENSITIVITY_START:
 
             pDevice->bStopDataPkt = true;
@@ -981,7 +934,6 @@ void vRunCommand(struct vnt_private *pDevice)
             s_bCommandComplete(pDevice);
             break;
 
-
         case WLAN_CMD_MAC_DISPOWERSAVING_START:
             ControlvReadByte (pDevice, MESSAGE_REQUEST_MACREG, MAC_REG_PSCTL, &byData);
             if ( (byData & PSCTL_PS) != 0 ) {
@@ -1014,7 +966,6 @@ void vRunCommand(struct vnt_private *pDevice)
     return;
 }
 
-
 static int s_bCommandComplete(struct vnt_private *pDevice)
 {
 	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
@@ -1022,7 +973,6 @@ static int s_bCommandComplete(struct vnt_private *pDevice)
 	int bRadioCmd = false;
 	int bForceSCAN = true;
 
-
     pDevice->eCommandState = WLAN_CMD_IDLE;
     if (pDevice->cbFreeCmdQueue == CMD_Q_SIZE) {
         //Command Queue Empty
@@ -1147,7 +1097,7 @@ int bScheduleCommand(struct vnt_private *pDevice,
                 break;
 /*
             case WLAN_CMD_DEAUTH:
-                pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].wDeAuthenReason = *((PWORD)pbyItem0);
+                pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].wDeAuthenReason = *((u16 *)pbyItem0);
                 break;
 */
 
@@ -1203,7 +1153,6 @@ static int s_bClearBSSID_SCAN(struct vnt_private *pDevice)
     return true;
 }
 
-
 //mike add:reset command timer
 void vResetCommandTimer(struct vnt_private *pDevice)
 {
diff --git a/drivers/staging/vt6656/wcmd.h b/drivers/staging/vt6656/wcmd.h
index c40e6baa0b5..db8b4cf7fd6 100644
--- a/drivers/staging/vt6656/wcmd.h
+++ b/drivers/staging/vt6656/wcmd.h
@@ -28,18 +28,13 @@
 
 #ifndef __WCMD_H__
 #define __WCMD_H__
-#include "ttype.h"
+
 #include "80211hdr.h"
 #include "80211mgr.h"
 
-/*---------------------  Export Definitions -------------------------*/
-
-
-
 #define AUTHENTICATE_TIMEOUT   1000 //ms
 #define ASSOCIATE_TIMEOUT      1000 //ms
 
-
 // Command code
 typedef enum tagCMD_CODE {
     WLAN_CMD_BSSID_SCAN,
@@ -73,11 +68,11 @@ typedef enum tagCMD_STATUS {
 
 typedef struct tagCMD_ITEM {
     CMD_CODE eCmd;
-    BYTE     abyCmdDesireSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
+    u8     abyCmdDesireSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
     bool     bNeedRadioOFF;
     bool     bRadioCmd;
     bool     bForceSCAN;
-    WORD     wDeAuthenReason;
+    u16     wDeAuthenReason;
 } CMD_ITEM, *PCMD_ITEM;
 
 // Command state
@@ -104,13 +99,6 @@ typedef enum tagCMD_STATE {
     WLAN_CMD_IDLE
 } CMD_STATE, *PCMD_STATE;
 
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Types  ------------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
 struct vnt_private;
 
 void vResetCommandTimer(struct vnt_private *);
diff --git a/drivers/staging/vt6656/wctl.c b/drivers/staging/vt6656/wctl.c
index baa48a1f0d3..47a655db51e 100644
--- a/drivers/staging/vt6656/wctl.c
+++ b/drivers/staging/vt6656/wctl.c
@@ -39,17 +39,7 @@
 #include "card.h"
 #include "tmacro.h"
 
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
 // static int          msglevel                =MSG_LEVEL_INFO;
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-
 
 /*
  * Description:
@@ -67,7 +57,7 @@
  *
  */
 
-bool WCTLbIsDuplicate (PSCache pCache, PS802_11Header pMACHeader)
+bool WCTLbIsDuplicate (PSCache pCache, struct ieee80211_hdr *pMACHeader)
 {
     unsigned int            uIndex;
     unsigned int            ii;
@@ -78,10 +68,10 @@ bool WCTLbIsDuplicate (PSCache pCache, PS802_11Header pMACHeader)
         uIndex = pCache->uInPtr;
         for (ii = 0; ii < DUPLICATE_RX_CACHE_LENGTH; ii++) {
             pCacheEntry = &(pCache->asCacheEntry[uIndex]);
-            if ((pCacheEntry->wFmSequence == pMACHeader->wSeqCtl) &&
+            if ((pCacheEntry->wFmSequence == pMACHeader->seq_ctrl) &&
 		(!compare_ether_addr(&(pCacheEntry->abyAddr2[0]),
-				     &(pMACHeader->abyAddr2[0]))) &&
-                (LOBYTE(pCacheEntry->wFrameCtl) == LOBYTE(pMACHeader->wFrameCtl))
+				     &(pMACHeader->addr2[0]))) &&
+                (LOBYTE(pCacheEntry->wFrameCtl) == LOBYTE(pMACHeader->frame_control))
                 ) {
                 /* Duplicate match */
                 return true;
@@ -91,9 +81,9 @@ bool WCTLbIsDuplicate (PSCache pCache, PS802_11Header pMACHeader)
     }
     /* Not found in cache - insert */
     pCacheEntry = &pCache->asCacheEntry[pCache->uInPtr];
-    pCacheEntry->wFmSequence = pMACHeader->wSeqCtl;
-    memcpy(&(pCacheEntry->abyAddr2[0]), &(pMACHeader->abyAddr2[0]), ETH_ALEN);
-    pCacheEntry->wFrameCtl = pMACHeader->wFrameCtl;
+    pCacheEntry->wFmSequence = pMACHeader->seq_ctrl;
+    memcpy(&(pCacheEntry->abyAddr2[0]), &(pMACHeader->addr2[0]), ETH_ALEN);
+    pCacheEntry->wFrameCtl = pMACHeader->frame_control;
     ADD_ONE_WITH_WRAP_AROUND(pCache->uInPtr, DUPLICATE_RX_CACHE_LENGTH);
     return false;
 }
@@ -114,14 +104,14 @@ bool WCTLbIsDuplicate (PSCache pCache, PS802_11Header pMACHeader)
  */
 
 unsigned int WCTLuSearchDFCB(struct vnt_private *pDevice,
-	PS802_11Header pMACHeader)
+			     struct ieee80211_hdr *pMACHeader)
 {
 	unsigned int ii;
 
 	for (ii = 0; ii < pDevice->cbDFCB; ii++) {
 		if ((pDevice->sRxDFCB[ii].bInUse == true) &&
 		    (!compare_ether_addr(&(pDevice->sRxDFCB[ii].abyAddr2[0]),
-					  &(pMACHeader->abyAddr2[0])))) {
+					  &(pMACHeader->addr2[0])))) {
 			return ii;
 		}
 	}
@@ -143,7 +133,7 @@ unsigned int WCTLuSearchDFCB(struct vnt_private *pDevice,
  *
  */
 unsigned int WCTLuInsertDFCB(struct vnt_private *pDevice,
-	PS802_11Header pMACHeader)
+			     struct ieee80211_hdr *pMACHeader)
 {
 	unsigned int ii;
 
@@ -154,10 +144,10 @@ unsigned int WCTLuInsertDFCB(struct vnt_private *pDevice,
             pDevice->cbFreeDFCB--;
             pDevice->sRxDFCB[ii].uLifetime = pDevice->dwMaxReceiveLifetime;
             pDevice->sRxDFCB[ii].bInUse = true;
-            pDevice->sRxDFCB[ii].wSequence = (pMACHeader->wSeqCtl >> 4);
-            pDevice->sRxDFCB[ii].wFragNum = (pMACHeader->wSeqCtl & 0x000F);
+            pDevice->sRxDFCB[ii].wSequence = (pMACHeader->seq_ctrl >> 4);
+            pDevice->sRxDFCB[ii].wFragNum = (pMACHeader->seq_ctrl & 0x000F);
 	    memcpy(&(pDevice->sRxDFCB[ii].abyAddr2[0]),
-		   &(pMACHeader->abyAddr2[0]),
+		   &(pMACHeader->addr2[0]),
 		   ETH_ALEN);
             return(ii);
         }
@@ -165,7 +155,6 @@ unsigned int WCTLuInsertDFCB(struct vnt_private *pDevice,
     return(pDevice->cbDFCB);
 }
 
-
 /*
  * Description:
  *      Handle received fragment packet
@@ -182,12 +171,10 @@ unsigned int WCTLuInsertDFCB(struct vnt_private *pDevice,
  * Return Value: true if it is valid fragment packet and we have resource to defragment; otherwise false
  *
  */
-bool WCTLbHandleFragment(struct vnt_private *pDevice, PS802_11Header pMACHeader,
-	unsigned int cbFrameLength, bool bWEP, bool bExtIV)
+bool WCTLbHandleFragment(struct vnt_private *pDevice, struct ieee80211_hdr *pMACHeader,	unsigned int cbFrameLength, bool bWEP, bool bExtIV)
 {
 	unsigned int uHeaderSize;
 
-
     if (bWEP == true) {
         uHeaderSize = 28;
         if (bExtIV)
@@ -203,8 +190,8 @@ bool WCTLbHandleFragment(struct vnt_private *pDevice, PS802_11Header pMACHeader,
         if (pDevice->uCurrentDFCBIdx < pDevice->cbDFCB) {
             // duplicate, we must flush previous DCB
             pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].uLifetime = pDevice->dwMaxReceiveLifetime;
-            pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wSequence = (pMACHeader->wSeqCtl >> 4);
-            pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wFragNum = (pMACHeader->wSeqCtl & 0x000F);
+            pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wSequence = (pMACHeader->seq_ctrl >> 4);
+            pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wFragNum = (pMACHeader->seq_ctrl & 0x000F);
         }
         else {
             pDevice->uCurrentDFCBIdx = WCTLuInsertDFCB(pDevice, pMACHeader);
@@ -213,8 +200,8 @@ bool WCTLbHandleFragment(struct vnt_private *pDevice, PS802_11Header pMACHeader,
             }
         }
         // reserve 8 byte to match MAC RX Buffer
-        pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer = (PBYTE) (pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].skb->data + 8);
-//        pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer = (PBYTE) (pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].skb->data + 4);
+        pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer = (u8 *) (pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].skb->data + 8);
+//        pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer = (u8 *) (pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].skb->data + 4);
         memcpy(pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer, pMACHeader, cbFrameLength);
         pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].cbFrameLength = cbFrameLength;
         pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer += cbFrameLength;
@@ -225,11 +212,11 @@ bool WCTLbHandleFragment(struct vnt_private *pDevice, PS802_11Header pMACHeader,
     else {
         pDevice->uCurrentDFCBIdx = WCTLuSearchDFCB(pDevice, pMACHeader);
         if (pDevice->uCurrentDFCBIdx != pDevice->cbDFCB) {
-            if ((pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wSequence == (pMACHeader->wSeqCtl >> 4)) &&
-                (pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wFragNum == (pMACHeader->wSeqCtl & 0x000F)) &&
+            if ((pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wSequence == (pMACHeader->seq_ctrl >> 4)) &&
+                (pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wFragNum == (pMACHeader->seq_ctrl & 0x000F)) &&
                 ((pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].cbFrameLength + cbFrameLength - uHeaderSize) < 2346)) {
 
-                memcpy(pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer, ((PBYTE) (pMACHeader) + uHeaderSize), (cbFrameLength - uHeaderSize));
+                memcpy(pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer, ((u8 *) (pMACHeader) + uHeaderSize), (cbFrameLength - uHeaderSize));
                 pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].cbFrameLength += (cbFrameLength - uHeaderSize);
                 pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer += (cbFrameLength - uHeaderSize);
                 pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wFragNum++;
@@ -256,4 +243,3 @@ bool WCTLbHandleFragment(struct vnt_private *pDevice, PS802_11Header pMACHeader,
     }
 }
 
-
diff --git a/drivers/staging/vt6656/wctl.h b/drivers/staging/vt6656/wctl.h
index 1b21e32e99e..14cb4117704 100644
--- a/drivers/staging/vt6656/wctl.h
+++ b/drivers/staging/vt6656/wctl.h
@@ -29,59 +29,55 @@
 #ifndef __WCTL_H__
 #define __WCTL_H__
 
-#include "ttype.h"
 #include "tether.h"
 #include "device.h"
 
-/*---------------------  Export Definitions -------------------------*/
-
 #define IS_TYPE_DATA(pMACHeader)                                                        \
-    ((((PS802_11Header) pMACHeader)->wFrameCtl & TYPE_802_11_MASK) == TYPE_802_11_DATA)
+    ((((struct ieee80211_hdr *) pMACHeader)->frame_control & TYPE_802_11_MASK) == TYPE_802_11_DATA)
 
 #define IS_TYPE_MGMT(pMACHeader)                                                        \
-    ((((PS802_11Header) pMACHeader)->wFrameCtl & TYPE_802_11_MASK) == TYPE_802_11_MGMT)
+    ((((struct ieee80211_hdr *) pMACHeader)->frame_control & TYPE_802_11_MASK) == TYPE_802_11_MGMT)
 
 #define IS_TYPE_CONTROL(pMACHeader)                                                     \
-    ((((PS802_11Header) pMACHeader)->wFrameCtl & TYPE_802_11_MASK) == TYPE_802_11_CTL)
+    ((((struct ieee80211_hdr *) pMACHeader)->frame_control & TYPE_802_11_MASK) == TYPE_802_11_CTL)
 
 #define IS_FC_MOREDATA(pMACHeader)                                                      \
-    ((((PS802_11Header) pMACHeader)->wFrameCtl & FC_MOREDATA) == FC_MOREDATA)
+    ((((struct ieee80211_hdr *) pMACHeader)->frame_control & FC_MOREDATA) == FC_MOREDATA)
 
 #define IS_FC_POWERMGT(pMACHeader)                                                      \
-    ((((PS802_11Header) pMACHeader)->wFrameCtl & FC_POWERMGT) == FC_POWERMGT)
+    ((((struct ieee80211_hdr *) pMACHeader)->frame_control & FC_POWERMGT) == FC_POWERMGT)
 
 #define IS_FC_RETRY(pMACHeader)                                                         \
-    ((((PS802_11Header) pMACHeader)->wFrameCtl & FC_RETRY) == FC_RETRY)
+    ((((struct ieee80211_hdr *) pMACHeader)->frame_control & FC_RETRY) == FC_RETRY)
 
 #define IS_FC_WEP(pMACHeader)                                                           \
-    ((((PS802_11Header) pMACHeader)->wFrameCtl & FC_WEP) == FC_WEP)
+    ((((struct ieee80211_hdr *) pMACHeader)->frame_control & FC_WEP) == FC_WEP)
 
 #ifdef __BIG_ENDIAN
 
 #define IS_FRAGMENT_PKT(pMACHeader)                                                     \
-    (((((PS802_11Header) pMACHeader)->wFrameCtl & FC_MOREFRAG) != 0) |                  \
-     ((((PS802_11Header) pMACHeader)->wSeqCtl & 0x0F00) != 0))
+    (((((struct ieee80211_hdr *) pMACHeader)->frame_control & FC_MOREFRAG) != 0) |                  \
+     ((((struct ieee80211_hdr *) pMACHeader)->seq_ctrl & 0x0F00) != 0))
 
 #define IS_FIRST_FRAGMENT_PKT(pMACHeader)                                               \
-    ((((PS802_11Header) pMACHeader)->wSeqCtl & 0x0F00) == 0)
+    ((((struct ieee80211_hdr *) pMACHeader)->seq_ctrl & 0x0F00) == 0)
 
 #else
 
 #define IS_FRAGMENT_PKT(pMACHeader)                                                     \
-    (((((PS802_11Header) pMACHeader)->wFrameCtl & FC_MOREFRAG) != 0) |                  \
-     ((((PS802_11Header) pMACHeader)->wSeqCtl & 0x000F) != 0))
+    (((((struct ieee80211_hdr *) pMACHeader)->frame_control & FC_MOREFRAG) != 0) |                  \
+     ((((struct ieee80211_hdr *) pMACHeader)->seq_ctrl & 0x000F) != 0))
 
 #define IS_FIRST_FRAGMENT_PKT(pMACHeader)                                               \
-    ((((PS802_11Header) pMACHeader)->wSeqCtl & 0x000F) == 0)
+    ((((struct ieee80211_hdr *) pMACHeader)->seq_ctrl & 0x000F) == 0)
 
 #endif//#ifdef __BIG_ENDIAN
 
 #define IS_LAST_FRAGMENT_PKT(pMACHeader)                                                \
-    ((((PS802_11Header) pMACHeader)->wFrameCtl & FC_MOREFRAG) == 0)
+    ((((struct ieee80211_hdr *) pMACHeader)->frame_control & FC_MOREFRAG) == 0)
 
 #define IS_CTL_PSPOLL(pMACHeader)                                                       \
-    ((((PS802_11Header) pMACHeader)->wFrameCtl & TYPE_SUBTYPE_MASK) == TYPE_CTL_PSPOLL)
-
+    ((((struct ieee80211_hdr *) pMACHeader)->frame_control & TYPE_SUBTYPE_MASK) == TYPE_CTL_PSPOLL)
 
 #define ADD_ONE_WITH_WRAP_AROUND(uVar, uModulo) {   \
     if ((uVar) >= ((uModulo) - 1))                  \
@@ -90,16 +86,9 @@
         (uVar)++;                                   \
 }
 
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-bool WCTLbIsDuplicate(PSCache pCache, PS802_11Header pMACHeader);
-bool WCTLbHandleFragment(struct vnt_private *, PS802_11Header pMACHeader,
-	unsigned int cbFrameLength, bool bWEP, bool bExtIV);
-unsigned int WCTLuSearchDFCB(struct vnt_private *, PS802_11Header pMACHeader);
-unsigned int WCTLuInsertDFCB(struct vnt_private *, PS802_11Header pMACHeader);
+bool WCTLbIsDuplicate(PSCache pCache, struct ieee80211_hdr *pMACHeader);
+bool WCTLbHandleFragment(struct vnt_private *, struct ieee80211_hdr *pMACHeader, unsigned int cbFrameLength, bool bWEP, bool bExtIV);
+unsigned int WCTLuSearchDFCB(struct vnt_private *, struct ieee80211_hdr *pMACHeader);
+unsigned int WCTLuInsertDFCB(struct vnt_private *, struct ieee80211_hdr *pMACHeader);
 
 #endif /* __WCTL_H__ */
diff --git a/drivers/staging/vt6656/wmgr.c b/drivers/staging/vt6656/wmgr.c
index 5dced0a4379..6d1ff5eeafa 100644
--- a/drivers/staging/vt6656/wmgr.c
+++ b/drivers/staging/vt6656/wmgr.c
@@ -81,18 +81,9 @@
 #include "control.h"
 #include "rndis.h"
 
-/*---------------------  Static Definitions -------------------------*/
-
-
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
 static int          msglevel                =MSG_LEVEL_INFO;
 //static int          msglevel                =MSG_LEVEL_DEBUG;
 
-/*---------------------  Static Functions  --------------------------*/
-
 static int ChannelExceedZoneType(struct vnt_private *, u8 byCurrChannel);
 
 /* Association/diassociation functions */
@@ -158,7 +149,6 @@ static struct vnt_tx_mgmt *s_MgrMakeBeacon(struct vnt_private *pDevice,
 	u8 *pCurrBSSID, PWLAN_IE_SUPP_RATES pCurrSuppRates,
 	PWLAN_IE_SUPP_RATES pCurrExtSuppRates);
 
-
 /* Association response */
 static struct vnt_tx_mgmt *s_MgrMakeAssocResponse(struct vnt_private *,
 	struct vnt_manager *pMgmt, u16 wCurrCapInfo, u16 wAssocStatus,
@@ -182,25 +172,19 @@ static struct vnt_tx_mgmt *s_MgrMakeProbeResponse(struct vnt_private *,
 /* received status */
 static void s_vMgrLogStatus(struct vnt_manager *pMgmt, u16 wStatus);
 
-
 static void s_vMgrSynchBSS(struct vnt_private *, u32 uBSSMode,
 	PKnownBSS pCurr, PCMD_STATUS  pStatus);
 
-
 static bool
 s_bCipherMatch (
      PKnownBSS                        pBSSNode,
      NDIS_802_11_ENCRYPTION_STATUS    EncStatus,
-     PBYTE                           pbyCCSPK,
-     PBYTE                           pbyCCSGK
+     u8 *                           pbyCCSPK,
+     u8 *                           pbyCCSGK
     );
 
 static void Encyption_Rebuild(struct vnt_private *, PKnownBSS pCurr);
 
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
 /*+
  *
  * Routine Description:
@@ -216,7 +200,6 @@ void vMgrObjectInit(struct vnt_private *pDevice)
 	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
 	int ii;
 
-
     pMgmt->pbyPSPacketPool = &pMgmt->byPSPacketPool[0];
     pMgmt->pbyMgmtPacketPool = &pMgmt->byMgmtPacketPool[0];
     pMgmt->uCurrChannel = pDevice->uChannel;
@@ -274,7 +257,6 @@ void vMgrAssocBeginSta(struct vnt_private *pDevice,
 {
 	struct vnt_tx_mgmt *pTxPacket;
 
-
     pMgmt->wCurrCapInfo = 0;
     pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_ESS(1);
     if (pDevice->bEncryptionEnable) {
@@ -329,7 +311,6 @@ void vMgrAssocBeginSta(struct vnt_private *pDevice,
     return ;
 }
 
-
 /*+
  *
  * Routine Description:
@@ -359,7 +340,6 @@ void vMgrReAssocBeginSta(struct vnt_private *pDevice,
     if (pMgmt->wListenInterval == 0)
         pMgmt->wListenInterval = 1;    // at least one.
 
-
     // ERP Phy (802.11g) should support short preamble.
     if (pMgmt->eCurrentPHYMode == PHY_TYPE_11G) {
         pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
@@ -374,7 +354,6 @@ void vMgrReAssocBeginSta(struct vnt_private *pDevice,
     if (pMgmt->b11hEnable == true)
         pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SPECTRUMMNG(1);
 
-
     pTxPacket = s_MgrMakeReAssocRequest
                 (
                   pDevice,
@@ -398,7 +377,6 @@ void vMgrReAssocBeginSta(struct vnt_private *pDevice,
         }
     }
 
-
     return ;
 }
 
@@ -426,7 +404,7 @@ void vMgrDisassocBeginSta(struct vnt_private *pDevice,
 		+ sizeof(struct vnt_tx_mgmt));
 
     // Setup the sFrame structure
-    sFrame.pBuf = (PBYTE)pTxPacket->p80211Header;
+    sFrame.pBuf = (u8 *)pTxPacket->p80211Header;
     sFrame.len = WLAN_DISASSOC_FR_MAXLEN;
 
     // format fixed field frame structure
@@ -458,8 +436,6 @@ void vMgrDisassocBeginSta(struct vnt_private *pDevice,
     return;
 }
 
-
-
 /*+
  *
  * Routine Description:(AP function)
@@ -483,7 +459,6 @@ static void s_vMgrRxAssocRequest(struct vnt_private *pDevice,
 	u8 abyCurrSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
 	u8 abyCurrExtSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
 
-
     if (pMgmt->eCurrMode != WMAC_MODE_ESS_AP)
         return;
     //  node index not found
@@ -496,7 +471,7 @@ static void s_vMgrRxAssocRequest(struct vnt_private *pDevice,
     memset(abyCurrSuppRates, 0, WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1);
     memset(abyCurrExtSuppRates, 0, WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1);
     sFrame.len = pRxPacket->cbMPDULen;
-    sFrame.pBuf = (PBYTE)pRxPacket->p80211Header;
+    sFrame.pBuf = (u8 *)pRxPacket->p80211Header;
 
     vMgrDecodeAssocRequest(&sFrame);
 
@@ -523,7 +498,6 @@ static void s_vMgrRxAssocRequest(struct vnt_private *pDevice,
             abyCurrExtSuppRates[1] = 0;
         }
 
-
 	RATEvParseMaxRate((void *)pDevice,
                            (PWLAN_IE_SUPP_RATES)abyCurrSuppRates,
                            (PWLAN_IE_SUPP_RATES)abyCurrExtSuppRates,
@@ -543,9 +517,9 @@ static void s_vMgrRxAssocRequest(struct vnt_private *pDevice,
                 WLAN_GET_CAP_INFO_SHORTPREAMBLE(*sFrame.pwCapInfo);
         pMgmt->sNodeDBTable[uNodeIndex].bShortSlotTime =
                 WLAN_GET_CAP_INFO_SHORTSLOTTIME(*sFrame.pwCapInfo);
-        pMgmt->sNodeDBTable[uNodeIndex].wAID = (WORD)uNodeIndex;
+        pMgmt->sNodeDBTable[uNodeIndex].wAID = (u16)uNodeIndex;
         wAssocStatus = WLAN_MGMT_STATUS_SUCCESS;
-        wAssocAID = (WORD)uNodeIndex;
+        wAssocAID = (u16)uNodeIndex;
         // check if ERP support
         if(pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate > RATE_11M)
            pMgmt->sNodeDBTable[uNodeIndex].bERPExist = true;
@@ -572,7 +546,6 @@ static void s_vMgrRxAssocRequest(struct vnt_private *pDevice,
                    pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate);
     }
 
-
     // assoc response reply..
     pTxPacket = s_MgrMakeAssocResponse
                 (
@@ -604,7 +577,6 @@ static void s_vMgrRxAssocRequest(struct vnt_private *pDevice,
     return;
 }
 
-
 /*+
  *
  * Description:(AP function)
@@ -643,7 +615,7 @@ static void s_vMgrRxReAssocRequest(struct vnt_private *pDevice,
     //decode the frame
     memset(&sFrame, 0, sizeof(WLAN_FR_REASSOCREQ));
     sFrame.len = pRxPacket->cbMPDULen;
-    sFrame.pBuf = (PBYTE)pRxPacket->p80211Header;
+    sFrame.pBuf = (u8 *)pRxPacket->p80211Header;
     vMgrDecodeReassocRequest(&sFrame);
 
     if (pMgmt->sNodeDBTable[uNodeIndex].eNodeState >= NODE_AUTH) {
@@ -671,7 +643,6 @@ static void s_vMgrRxReAssocRequest(struct vnt_private *pDevice,
             abyCurrExtSuppRates[1] = 0;
         }
 
-
 	RATEvParseMaxRate((void *)pDevice,
                           (PWLAN_IE_SUPP_RATES)abyCurrSuppRates,
                           (PWLAN_IE_SUPP_RATES)abyCurrExtSuppRates,
@@ -691,9 +662,9 @@ static void s_vMgrRxReAssocRequest(struct vnt_private *pDevice,
                 WLAN_GET_CAP_INFO_SHORTPREAMBLE(*sFrame.pwCapInfo);
         pMgmt->sNodeDBTable[uNodeIndex].bShortSlotTime =
                 WLAN_GET_CAP_INFO_SHORTSLOTTIME(*sFrame.pwCapInfo);
-        pMgmt->sNodeDBTable[uNodeIndex].wAID = (WORD)uNodeIndex;
+        pMgmt->sNodeDBTable[uNodeIndex].wAID = (u16)uNodeIndex;
         wAssocStatus = WLAN_MGMT_STATUS_SUCCESS;
-        wAssocAID = (WORD)uNodeIndex;
+        wAssocAID = (u16)uNodeIndex;
 
         // if suppurt ERP
         if(pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate > RATE_11M)
@@ -722,7 +693,6 @@ static void s_vMgrRxReAssocRequest(struct vnt_private *pDevice,
 
     }
 
-
     // assoc response reply..
     pTxPacket = s_MgrMakeReAssocResponse
                 (
@@ -752,7 +722,6 @@ static void s_vMgrRxReAssocRequest(struct vnt_private *pDevice,
     return;
 }
 
-
 /*+
  *
  * Routine Description:
@@ -771,13 +740,11 @@ static void s_vMgrRxAssocResponse(struct vnt_private *pDevice,
 	PWLAN_IE_SSID   pItemSSID;
 	u8 *pbyIEs;
 
-
-
     if (pMgmt->eCurrState == WMAC_STATE_ASSOCPENDING ||
          pMgmt->eCurrState == WMAC_STATE_ASSOC) {
 
         sFrame.len = pRxPacket->cbMPDULen;
-        sFrame.pBuf = (PBYTE)pRxPacket->p80211Header;
+        sFrame.pBuf = (u8 *)pRxPacket->p80211Header;
         // decode the frame
         vMgrDecodeAssocResponse(&sFrame);
 	if ((sFrame.pwCapInfo == NULL)
@@ -820,7 +787,7 @@ static void s_vMgrRxAssocResponse(struct vnt_private *pDevice,
 
 	//if(pDevice->bWPASuppWextEnabled == true)
 	   {
-		BYTE buf[512];
+		u8 buf[512];
 		size_t len;
 		union iwreq_data  wrqu;
 		int we_event;
@@ -906,7 +873,7 @@ void vMgrAuthenBeginSta(struct vnt_private *pDevice,
 		+ WLAN_AUTHEN_FR_MAXLEN);
 	pTxPacket->p80211Header = (PUWLAN_80211HDR)((u8 *)pTxPacket
 		+ sizeof(struct vnt_tx_mgmt));
-    sFrame.pBuf = (PBYTE)pTxPacket->p80211Header;
+    sFrame.pBuf = (u8 *)pTxPacket->p80211Header;
     sFrame.len = WLAN_AUTHEN_FR_MAXLEN;
     vMgrEncodeAuthen(&sFrame);
     /* insert values */
@@ -960,7 +927,7 @@ void vMgrDeAuthenBeginSta(struct vnt_private *pDevice,
 		+ WLAN_DEAUTHEN_FR_MAXLEN);
 	pTxPacket->p80211Header = (PUWLAN_80211HDR)((u8 *)pTxPacket
 		+ sizeof(struct vnt_tx_mgmt));
-    sFrame.pBuf = (PBYTE)pTxPacket->p80211Header;
+    sFrame.pBuf = (u8 *)pTxPacket->p80211Header;
     sFrame.len = WLAN_DEAUTHEN_FR_MAXLEN;
     vMgrEncodeDeauthen(&sFrame);
     /* insert values */
@@ -984,11 +951,9 @@ void vMgrDeAuthenBeginSta(struct vnt_private *pDevice,
         *pStatus = CMD_STATUS_SUCCESS;
     }
 
-
     return ;
 }
 
-
 /*+
  *
  * Routine Description:
@@ -1012,7 +977,7 @@ static void s_vMgrRxAuthentication(struct vnt_private *pDevice,
 
     // decode the frame
     sFrame.len = pRxPacket->cbMPDULen;
-    sFrame.pBuf = (PBYTE)pRxPacket->p80211Header;
+    sFrame.pBuf = (u8 *)pRxPacket->p80211Header;
     vMgrDecodeAuthen(&sFrame);
     switch (cpu_to_le16((*(sFrame.pwAuthSequence )))){
         case 1:
@@ -1037,8 +1002,6 @@ static void s_vMgrRxAuthentication(struct vnt_private *pDevice,
     return;
 }
 
-
-
 /*+
  *
  * Routine Description:
@@ -1051,7 +1014,6 @@ static void s_vMgrRxAuthentication(struct vnt_private *pDevice,
  *
 -*/
 
-
 static void s_vMgrRxAuthenSequence_1(struct vnt_private *pDevice,
 	struct vnt_manager *pMgmt, PWLAN_FR_AUTHEN pFrame)
 {
@@ -1082,7 +1044,7 @@ static void s_vMgrRxAuthenSequence_1(struct vnt_private *pDevice,
 		+ WLAN_AUTHEN_FR_MAXLEN);
 	pTxPacket->p80211Header = (PUWLAN_80211HDR)((u8 *)pTxPacket
 		+ sizeof(struct vnt_tx_mgmt));
-    sFrame.pBuf = (PBYTE)pTxPacket->p80211Header;
+    sFrame.pBuf = (u8 *)pTxPacket->p80211Header;
     sFrame.len = WLAN_AUTHEN_FR_MAXLEN;
     // format buffer structure
     vMgrEncodeAuthen(&sFrame);
@@ -1142,8 +1104,6 @@ static void s_vMgrRxAuthenSequence_1(struct vnt_private *pDevice,
     return;
 }
 
-
-
 /*+
  *
  * Routine Description:
@@ -1162,7 +1122,6 @@ static void s_vMgrRxAuthenSequence_2(struct vnt_private *pDevice,
 	WLAN_FR_AUTHEN sFrame;
 	struct vnt_tx_mgmt *pTxPacket = NULL;
 
-
     switch (cpu_to_le16((*(pFrame->pwAuthAlgorithm))))
     {
         case WLAN_AUTH_ALG_OPENSYSTEM:
@@ -1193,7 +1152,7 @@ static void s_vMgrRxAuthenSequence_2(struct vnt_private *pDevice,
 			pTxPacket->p80211Header
 				= (PUWLAN_80211HDR)((u8 *)pTxPacket
 					+ sizeof(struct vnt_tx_mgmt));
-                sFrame.pBuf = (PBYTE)pTxPacket->p80211Header;
+                sFrame.pBuf = (u8 *)pTxPacket->p80211Header;
                 sFrame.len = WLAN_AUTHEN_FR_MAXLEN;
                 // format buffer structure
                 vMgrEncodeAuthen(&sFrame);
@@ -1241,8 +1200,6 @@ static void s_vMgrRxAuthenSequence_2(struct vnt_private *pDevice,
     return;
 }
 
-
-
 /*+
  *
  * Routine Description:
@@ -1297,7 +1254,7 @@ reply:
 		+ WLAN_AUTHEN_FR_MAXLEN);
 	pTxPacket->p80211Header = (PUWLAN_80211HDR)((u8 *)pTxPacket
 		+ sizeof(struct vnt_tx_mgmt));
-    sFrame.pBuf = (PBYTE)pTxPacket->p80211Header;
+    sFrame.pBuf = (u8 *)pTxPacket->p80211Header;
     sFrame.len = WLAN_AUTHEN_FR_MAXLEN;
     // format buffer structure
     vMgrEncodeAuthen(&sFrame);
@@ -1329,8 +1286,6 @@ reply:
 
 }
 
-
-
 /*+
  *
  * Routine Description:
@@ -1385,7 +1340,7 @@ static void s_vMgrRxDisassociation(struct vnt_private *pDevice,
         // if is acting an AP..
         // a STA is leaving this BSS..
         sFrame.len = pRxPacket->cbMPDULen;
-        sFrame.pBuf = (PBYTE)pRxPacket->p80211Header;
+        sFrame.pBuf = (u8 *)pRxPacket->p80211Header;
         if (BSSbIsSTAInNodeDB(pDevice, pRxPacket->p80211Header->sA3.abyAddr2, &uNodeIndex)) {
             BSSvRemoveOneNode(pDevice, uNodeIndex);
         }
@@ -1395,7 +1350,7 @@ static void s_vMgrRxDisassociation(struct vnt_private *pDevice,
     }
     else if (pMgmt->eCurrMode == WMAC_MODE_ESS_STA ){
         sFrame.len = pRxPacket->cbMPDULen;
-        sFrame.pBuf = (PBYTE)pRxPacket->p80211Header;
+        sFrame.pBuf = (u8 *)pRxPacket->p80211Header;
         vMgrDecodeDisassociation(&sFrame);
         DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "AP disassociated me, reason=%d.\n", cpu_to_le16(*(sFrame.pwReason)));
 
@@ -1430,7 +1385,6 @@ static void s_vMgrRxDisassociation(struct vnt_private *pDevice,
     return;
 }
 
-
 /*+
  *
  * Routine Description:
@@ -1448,13 +1402,12 @@ static void s_vMgrRxDeauthentication(struct vnt_private *pDevice,
 	WLAN_FR_DEAUTHEN sFrame;
 	u32 uNodeIndex = 0;
 
-
     if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP ){
         //Todo:
         // if is acting an AP..
         // a STA is leaving this BSS..
         sFrame.len = pRxPacket->cbMPDULen;
-        sFrame.pBuf = (PBYTE)pRxPacket->p80211Header;
+        sFrame.pBuf = (u8 *)pRxPacket->p80211Header;
         if (BSSbIsSTAInNodeDB(pDevice, pRxPacket->p80211Header->sA3.abyAddr2, &uNodeIndex)) {
             BSSvRemoveOneNode(pDevice, uNodeIndex);
         }
@@ -1465,7 +1418,7 @@ static void s_vMgrRxDeauthentication(struct vnt_private *pDevice,
     else {
         if (pMgmt->eCurrMode == WMAC_MODE_ESS_STA ) {
             sFrame.len = pRxPacket->cbMPDULen;
-            sFrame.pBuf = (PBYTE)pRxPacket->p80211Header;
+            sFrame.pBuf = (u8 *)pRxPacket->p80211Header;
             vMgrDecodeDeauthen(&sFrame);
 	   pDevice->fWPA_Authened = false;
             DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO  "AP deauthed me, reason=%d.\n", cpu_to_le16((*(sFrame.pwReason))));
@@ -1569,14 +1522,12 @@ static void s_vMgrRxBeacon(struct vnt_private *pDevice,
 	int bChannelHit = false;
 	u8 byOldPreambleType;
 
-
-
      if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP)
         return;
 
     memset(&sFrame, 0, sizeof(WLAN_FR_BEACON));
     sFrame.len = pRxPacket->cbMPDULen;
-    sFrame.pBuf = (PBYTE)pRxPacket->p80211Header;
+    sFrame.pBuf = (u8 *)pRxPacket->p80211Header;
 
     // decode the beacon frame
     vMgrDecodeBeacon(&sFrame);
@@ -1672,7 +1623,7 @@ if(ChannelExceedZoneType(pDevice,byCurrChannel)==true)
         return;
     }
 
-    if(byCurrChannel == (BYTE)pMgmt->uCurrChannel)
+    if(byCurrChannel == (u8)pMgmt->uCurrChannel)
        bIsChannelEqual = true;
 
     if (bIsChannelEqual && (pMgmt->eCurrMode == WMAC_MODE_ESS_AP)) {
@@ -1785,7 +1736,7 @@ if(ChannelExceedZoneType(pDevice,byCurrChannel)==true)
                     pDevice->byPreambleType = 0;
                 }
                 if (pDevice->byPreambleType != byOldPreambleType)
-                    CARDvSetRSPINF(pDevice, (BYTE)pDevice->byBBType);
+                    CARDvSetRSPINF(pDevice, (u8)pDevice->byBBType);
             //
             // Basic Rate Set may change dynamically
             //
@@ -1839,7 +1790,6 @@ if(ChannelExceedZoneType(pDevice,byCurrChannel)==true)
 	if (qwTSFOffset > TRIVIAL_SYNC_DIFFERENCE)
 		bTSFLargeDiff = true;
 
-
     // if infra mode
     if (bIsAPBeacon == true) {
 
@@ -2009,8 +1959,7 @@ if(ChannelExceedZoneType(pDevice,byCurrChannel)==true)
                         pDevice->byPreambleType = 0;
                     }
                     if (pDevice->byPreambleType != byOldPreambleType)
-                        CARDvSetRSPINF(pDevice, (BYTE)pDevice->byBBType);
-
+                        CARDvSetRSPINF(pDevice, (u8)pDevice->byBBType);
 
                      // MACvRegBitsOff(pDevice->PortOffset, MAC_REG_RCR, RCR_BSSID);
                      // set highest basic rate
@@ -2061,8 +2010,6 @@ void vMgrCreateOwnIBSS(struct vnt_private *pDevice, PCMD_STATUS pStatus)
 	u8 abyOFDM_RATE[] = {0x0C, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C};
 	u16 wSuppRate;
 
-
-
     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Create Basic Service Set .......\n");
 
     if (pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA) {
@@ -2101,7 +2048,6 @@ void vMgrCreateOwnIBSS(struct vnt_private *pDevice, PCMD_STATUS pStatus)
             pMgmt->abyCurrSuppRates[2+ii] = abyRATE[ii];
     }
 
-
     if (pMgmt->eCurrentPHYMode == PHY_TYPE_11G) {
         pMgmt->abyCurrSuppRates[1] = 8;
         pMgmt->abyCurrExtSuppRates[1] = 4;
@@ -2113,7 +2059,6 @@ void vMgrCreateOwnIBSS(struct vnt_private *pDevice, PCMD_STATUS pStatus)
             pMgmt->abyCurrExtSuppRates[2+ii] =  abyOFDM_RATE[ii+4];
     }
 
-
     // Disable Protect Mode
     pDevice->bProtectMode = 0;
     MACvDisableProtectMD(pDevice);
@@ -2156,8 +2101,6 @@ void vMgrCreateOwnIBSS(struct vnt_private *pDevice, PCMD_STATUS pStatus)
                       &wMaxBasicRate, &wMaxSuppRate, &wSuppRate,
                       &byTopCCKBasicRate, &byTopOFDMBasicRate);
 
-
-
     if (pDevice->byBBType == BB_TYPE_11A) {
         pDevice->bShortSlotTime = true;
     } else {
@@ -2221,7 +2164,6 @@ void vMgrCreateOwnIBSS(struct vnt_private *pDevice, PCMD_STATUS pStatus)
         pMgmt->abyCurrBSSID[0] &= ~IEEE_ADDR_GROUP;
         pMgmt->abyCurrBSSID[0] |= IEEE_ADDR_UNIVERSAL;
 
-
 		DBG_PRT(MSG_LEVEL_INFO, KERN_INFO"Adhoc beacon created bssid:"
 			"%pM\n", pMgmt->abyCurrBSSID);
     }
@@ -2313,7 +2255,6 @@ void vMgrJoinBSSBegin(struct vnt_private *pDevice, PCMD_STATUS pStatus)
 	u8 byTopOFDMBasicRate = RATE_1M;
 	u8 bShortSlotTime = false;
 
-
     for (ii = 0; ii < MAX_BSS_NUM; ii++) {
         if (pMgmt->sBSSList[ii].bActive == true)
             break;
@@ -2411,9 +2352,9 @@ void vMgrJoinBSSBegin(struct vnt_private *pDevice, PCMD_STATUS pStatus)
                     if (pItemExtRates->len <= ii)
                         break;
                 }
-                pItemRates->len += (BYTE)ii;
+                pItemRates->len += (u8)ii;
                 if (pItemExtRates->len - ii > 0) {
-                    pItemExtRates->len -= (BYTE)ii;
+                    pItemExtRates->len -= (u8)ii;
                     for (uu = 0; uu < pItemExtRates->len; uu ++) {
                         pItemExtRates->abyRates[uu] = pItemExtRates->abyRates[uu + ii];
                     }
@@ -2466,7 +2407,7 @@ void vMgrJoinBSSBegin(struct vnt_private *pDevice, PCMD_STATUS pStatus)
                 pDevice->byPreambleType = 0;
             }
             // Change PreambleType must set RSPINF again
-            CARDvSetRSPINF(pDevice, (BYTE)pDevice->byBBType);
+            CARDvSetRSPINF(pDevice, (u8)pDevice->byBBType);
 
             DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Join ESS\n");
 
@@ -2519,7 +2460,6 @@ void vMgrJoinBSSBegin(struct vnt_private *pDevice, PCMD_STATUS pStatus)
             pMgmt->eCurrState = WMAC_STATE_IDLE;
         };
 
-
      }
      else {
         // ad-hoc mode BSS
@@ -2559,7 +2499,6 @@ void vMgrJoinBSSBegin(struct vnt_private *pDevice, PCMD_STATUS pStatus)
             // TODO: check if CapInfo privacy on, but we don't..
             pMgmt->uCurrChannel = pCurr->uChannel;
 
-
             // Parse Support Rate IE
             pMgmt->abyCurrSuppRates[0] = WLAN_EID_SUPP_RATES;
             pMgmt->abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)pCurr->abySuppRates,
@@ -2597,7 +2536,7 @@ void vMgrJoinBSSBegin(struct vnt_private *pDevice, PCMD_STATUS pStatus)
                 pDevice->byPreambleType = 0;
             }
             // Change PreambleType must set RSPINF again
-            CARDvSetRSPINF(pDevice, (BYTE)pDevice->byBBType);
+            CARDvSetRSPINF(pDevice, (u8)pDevice->byBBType);
 
             // Prepare beacon
 		bMgrPrepareBeaconToSend((void *) pDevice, pMgmt);
@@ -2609,8 +2548,6 @@ void vMgrJoinBSSBegin(struct vnt_private *pDevice, PCMD_STATUS pStatus)
     return;
 }
 
-
-
 /*+
  *
  * Routine Description:
@@ -2806,7 +2743,6 @@ static void Encyption_Rebuild(struct vnt_private *pDevice, PKnownBSS pCurr)
       return;
  }
 
-
 /*+
  *
  * Routine Description:
@@ -2828,7 +2764,6 @@ static void s_vMgrFormatTIM(struct vnt_manager *pMgmt, PWLAN_IE_TIM pTIM)
 	u16 wStartIndex = 0;
 	u16 wEndIndex = 0;
 
-
     // Find size of partial virtual bitmap
     for (ii = 0; ii < (MAX_NODE_NUM + 1); ii++) {
         byMap = pMgmt->abyPSTxMap[ii];
@@ -2843,13 +2778,12 @@ static void s_vMgrFormatTIM(struct vnt_manager *pMgmt, PWLAN_IE_TIM pTIM)
         if (byMap) {
             if (!bStartFound) {
                 bStartFound = true;
-                wStartIndex = (WORD)ii;
+                wStartIndex = (u16)ii;
             }
-            wEndIndex = (WORD)ii;
+            wEndIndex = (u16)ii;
         }
     }
 
-
     // Round start index down to nearest even number
     wStartIndex &=  ~BIT0;
 
@@ -2876,7 +2810,6 @@ static void s_vMgrFormatTIM(struct vnt_manager *pMgmt, PWLAN_IE_TIM pTIM)
     pTIM->byVirtBitMap[0]  &= ~BIT0;
 }
 
-
 /*+
  *
  * Routine Description:
@@ -2898,7 +2831,6 @@ static struct vnt_tx_mgmt *s_MgrMakeBeacon(struct vnt_private *pDevice,
 	WLAN_FR_BEACON sFrame;
 	u8 abyBroadcastAddr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 
-
 	/* prepare beacon frame */
 	pTxPacket = (struct vnt_tx_mgmt *)pMgmt->pbyMgmtPacketPool;
 	memset(pTxPacket, 0, sizeof(struct vnt_tx_mgmt)
@@ -2906,7 +2838,7 @@ static struct vnt_tx_mgmt *s_MgrMakeBeacon(struct vnt_private *pDevice,
 	pTxPacket->p80211Header = (PUWLAN_80211HDR)((u8 *)pTxPacket
 		+ sizeof(struct vnt_tx_mgmt));
     // Setup the sFrame structure.
-    sFrame.pBuf = (PBYTE)pTxPacket->p80211Header;
+    sFrame.pBuf = (u8 *)pTxPacket->p80211Header;
     sFrame.len = WLAN_BEACON_FR_MAXLEN;
     vMgrEncodeBeacon(&sFrame);
     // Setup the header
@@ -2917,7 +2849,7 @@ static struct vnt_tx_mgmt *s_MgrMakeBeacon(struct vnt_private *pDevice,
         ));
 
     if (pDevice->bEnablePSMode) {
-        sFrame.pHdr->sA3.wFrameCtl |= cpu_to_le16((WORD)WLAN_SET_FC_PWRMGT(1));
+        sFrame.pHdr->sA3.wFrameCtl |= cpu_to_le16((u16)WLAN_SET_FC_PWRMGT(1));
     }
 
     memcpy( sFrame.pHdr->sA3.abyAddr1, abyBroadcastAddr, WLAN_ADDR_LEN);
@@ -2945,7 +2877,7 @@ static struct vnt_tx_mgmt *s_MgrMakeBeacon(struct vnt_private *pDevice,
         sFrame.len += (1) + WLAN_IEHDR_LEN;
         sFrame.pDSParms->byElementID = WLAN_EID_DS_PARMS;
         sFrame.pDSParms->len = 1;
-        sFrame.pDSParms->byCurrChannel = (BYTE)uCurrChannel;
+        sFrame.pDSParms->byCurrChannel = (u8)uCurrChannel;
     }
     // TIM field
     if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
@@ -2988,17 +2920,16 @@ static struct vnt_tx_mgmt *s_MgrMakeBeacon(struct vnt_private *pDevice,
             // Pairwise Key Cipher Suite
             sFrame.pRSNWPA->wPKCount = 0;
             // Auth Key Management Suite
-            *((PWORD)(sFrame.pBuf + sFrame.len + sFrame.pRSNWPA->len))=0;
+            *((u16 *)(sFrame.pBuf + sFrame.len + sFrame.pRSNWPA->len))=0;
             sFrame.pRSNWPA->len +=2;
 
             // RSN Capabilites
-            *((PWORD)(sFrame.pBuf + sFrame.len + sFrame.pRSNWPA->len))=0;
+            *((u16 *)(sFrame.pBuf + sFrame.len + sFrame.pRSNWPA->len))=0;
             sFrame.pRSNWPA->len +=2;
             sFrame.len += sFrame.pRSNWPA->len + WLAN_IEHDR_LEN;
         }
     }
 
-
     if (pMgmt->eCurrentPHYMode == PHY_TYPE_11G) {
         sFrame.pERP = (PWLAN_IE_ERP)(sFrame.pBuf + sFrame.len);
         sFrame.len += 1 + WLAN_IEHDR_LEN;
@@ -3038,10 +2969,6 @@ static struct vnt_tx_mgmt *s_MgrMakeBeacon(struct vnt_private *pDevice,
     return pTxPacket;
 }
 
-
-
-
-
 /*+
  *
  * Routine Description:
@@ -3053,9 +2980,6 @@ static struct vnt_tx_mgmt *s_MgrMakeBeacon(struct vnt_private *pDevice,
  *
 -*/
 
-
-
-
 struct vnt_tx_mgmt *s_MgrMakeProbeResponse(struct vnt_private *pDevice,
 	struct vnt_manager *pMgmt, u16 wCurrCapInfo, u16 wCurrBeaconPeriod,
 	u32 uCurrChannel, u16 wCurrATIMWinodw, u8 *pDstAddr,
@@ -3066,14 +2990,13 @@ struct vnt_tx_mgmt *s_MgrMakeProbeResponse(struct vnt_private *pDevice,
 	struct vnt_tx_mgmt *pTxPacket = NULL;
 	WLAN_FR_PROBERESP sFrame;
 
-
 	pTxPacket = (struct vnt_tx_mgmt *)pMgmt->pbyMgmtPacketPool;
 	memset(pTxPacket, 0, sizeof(struct vnt_tx_mgmt)
 		+ WLAN_PROBERESP_FR_MAXLEN);
 	pTxPacket->p80211Header = (PUWLAN_80211HDR)((u8 *)pTxPacket
 		+ sizeof(struct vnt_tx_mgmt));
     // Setup the sFrame structure.
-    sFrame.pBuf = (PBYTE)pTxPacket->p80211Header;
+    sFrame.pBuf = (u8 *)pTxPacket->p80211Header;
     sFrame.len = WLAN_PROBERESP_FR_MAXLEN;
     vMgrEncodeProbeResponse(&sFrame);
     // Setup the header
@@ -3089,7 +3012,7 @@ struct vnt_tx_mgmt *s_MgrMakeProbeResponse(struct vnt_private *pDevice,
     *sFrame.pwCapInfo = cpu_to_le16(wCurrCapInfo);
 
     if (byPHYType == BB_TYPE_11B) {
-        *sFrame.pwCapInfo &= cpu_to_le16((WORD)~(WLAN_SET_CAP_INFO_SHORTSLOTTIME(1)));
+        *sFrame.pwCapInfo &= cpu_to_le16((u16)~(WLAN_SET_CAP_INFO_SHORTSLOTTIME(1)));
     }
 
     // Copy SSID
@@ -3114,7 +3037,7 @@ struct vnt_tx_mgmt *s_MgrMakeProbeResponse(struct vnt_private *pDevice,
         sFrame.len += (1) + WLAN_IEHDR_LEN;
         sFrame.pDSParms->byElementID = WLAN_EID_DS_PARMS;
         sFrame.pDSParms->len = 1;
-        sFrame.pDSParms->byCurrChannel = (BYTE)uCurrChannel;
+        sFrame.pDSParms->byCurrChannel = (u8)uCurrChannel;
     }
 
     if (pMgmt->eCurrMode != WMAC_MODE_ESS_AP) {
@@ -3166,8 +3089,6 @@ struct vnt_tx_mgmt *s_MgrMakeProbeResponse(struct vnt_private *pDevice,
     return pTxPacket;
 }
 
-
-
 /*+
  *
  * Routine Description:
@@ -3179,7 +3100,6 @@ struct vnt_tx_mgmt *s_MgrMakeProbeResponse(struct vnt_private *pDevice,
  *
 -*/
 
-
 struct vnt_tx_mgmt *s_MgrMakeAssocRequest(struct vnt_private *pDevice,
 	struct vnt_manager *pMgmt, u8 *pDAddr, u16 wCurrCapInfo,
 	u16 wListenInterval,
@@ -3192,14 +3112,13 @@ struct vnt_tx_mgmt *s_MgrMakeAssocRequest(struct vnt_private *pDevice,
 	u8 *pbyIEs;
 	u8 *pbyRSN;
 
-
 	pTxPacket = (struct vnt_tx_mgmt *)pMgmt->pbyMgmtPacketPool;
 	memset(pTxPacket, 0, sizeof(struct vnt_tx_mgmt)
 		+ WLAN_ASSOCREQ_FR_MAXLEN);
 	pTxPacket->p80211Header = (PUWLAN_80211HDR)((u8 *)pTxPacket
 		+ sizeof(struct vnt_tx_mgmt));
     // Setup the sFrame structure.
-    sFrame.pBuf = (PBYTE)pTxPacket->p80211Header;
+    sFrame.pBuf = (u8 *)pTxPacket->p80211Header;
     sFrame.len = WLAN_ASSOCREQ_FR_MAXLEN;
     // format fixed field frame structure
     vMgrEncodeAssocRequest(&sFrame);
@@ -3247,7 +3166,6 @@ struct vnt_tx_mgmt *s_MgrMakeAssocRequest(struct vnt_private *pDevice,
     memcpy(pbyIEs, pCurrRates, pCurrRates->len + WLAN_IEHDR_LEN);
     pbyIEs += pCurrRates->len + WLAN_IEHDR_LEN;
 
-
     if (((pMgmt->eAuthenMode == WMAC_AUTH_WPA) ||
          (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) ||
          (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE)) &&
@@ -3287,7 +3205,7 @@ struct vnt_tx_mgmt *s_MgrMakeAssocRequest(struct vnt_private *pDevice,
             sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_NONE;
         }
         // Auth Key Management Suite
-        pbyRSN = (PBYTE)(sFrame.pBuf + sFrame.len + 2 + sFrame.pRSNWPA->len);
+        pbyRSN = (u8 *)(sFrame.pBuf + sFrame.len + 2 + sFrame.pRSNWPA->len);
         *pbyRSN++=0x01;
         *pbyRSN++=0x00;
         *pbyRSN++=0x00;
@@ -3322,7 +3240,7 @@ struct vnt_tx_mgmt *s_MgrMakeAssocRequest(struct vnt_private *pDevice,
                 (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) &&
                (pMgmt->pCurrBSS != NULL)) {
 	unsigned int ii;
-        PWORD               pwPMKID;
+        u16 *               pwPMKID;
 
         // WPA IE
         sFrame.pRSN = (PWLAN_IE_RSN)(sFrame.pBuf + sFrame.len);
@@ -3387,7 +3305,7 @@ struct vnt_tx_mgmt *s_MgrMakeAssocRequest(struct vnt_private *pDevice,
         if ((pDevice->gsPMKID.BSSIDInfoCount > 0) && (pDevice->bRoaming == true) && (pMgmt->eAuthenMode == WMAC_AUTH_WPA2)) {
             // RSN PMKID
             pbyRSN = &sFrame.pRSN->abyRSN[18];
-            pwPMKID = (PWORD)pbyRSN; // Point to PMKID count
+            pwPMKID = (u16 *)pbyRSN; // Point to PMKID count
             *pwPMKID = 0;            // Initialize PMKID count
             pbyRSN += 2;             // Point to PMKID list
 	for (ii = 0; ii < pDevice->gsPMKID.BSSIDInfoCount; ii++) {
@@ -3413,20 +3331,12 @@ struct vnt_tx_mgmt *s_MgrMakeAssocRequest(struct vnt_private *pDevice,
         pbyIEs += sFrame.pRSN->len + WLAN_IEHDR_LEN;
     }
 
-
     // Adjust the length fields
     pTxPacket->cbMPDULen = sFrame.len;
     pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
     return pTxPacket;
 }
 
-
-
-
-
-
-
-
 /*+
  *
  * Routine Description:
@@ -3438,7 +3348,6 @@ struct vnt_tx_mgmt *s_MgrMakeAssocRequest(struct vnt_private *pDevice,
  *
 -*/
 
-
 struct vnt_tx_mgmt *s_MgrMakeReAssocRequest(struct vnt_private *pDevice,
 	struct vnt_manager *pMgmt, u8 *pDAddr, u16 wCurrCapInfo,
 	u16 wListenInterval, PWLAN_IE_SSID pCurrSSID,
@@ -3450,14 +3359,13 @@ struct vnt_tx_mgmt *s_MgrMakeReAssocRequest(struct vnt_private *pDevice,
 	u8 *pbyIEs;
 	u8 *pbyRSN;
 
-
 	pTxPacket = (struct vnt_tx_mgmt *)pMgmt->pbyMgmtPacketPool;
 	memset(pTxPacket, 0, sizeof(struct vnt_tx_mgmt)
 		+ WLAN_REASSOCREQ_FR_MAXLEN);
 	pTxPacket->p80211Header = (PUWLAN_80211HDR)((u8 *)pTxPacket
 		+ sizeof(struct vnt_tx_mgmt));
     /* Setup the sFrame structure. */
-    sFrame.pBuf = (PBYTE)pTxPacket->p80211Header;
+    sFrame.pBuf = (u8 *)pTxPacket->p80211Header;
     sFrame.len = WLAN_REASSOCREQ_FR_MAXLEN;
 
     // format fixed field frame structure
@@ -3546,7 +3454,7 @@ struct vnt_tx_mgmt *s_MgrMakeReAssocRequest(struct vnt_private *pDevice,
             sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_NONE;
         }
         // Auth Key Management Suite
-        pbyRSN = (PBYTE)(sFrame.pBuf + sFrame.len + 2 + sFrame.pRSNWPA->len);
+        pbyRSN = (u8 *)(sFrame.pBuf + sFrame.len + 2 + sFrame.pRSNWPA->len);
         *pbyRSN++=0x01;
         *pbyRSN++=0x00;
         *pbyRSN++=0x00;
@@ -3578,7 +3486,7 @@ struct vnt_tx_mgmt *s_MgrMakeReAssocRequest(struct vnt_private *pDevice,
                 (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) &&
                (pMgmt->pCurrBSS != NULL)) {
 	unsigned int ii;
-        PWORD               pwPMKID;
+        u16 *               pwPMKID;
 
         /* WPA IE */
         sFrame.pRSN = (PWLAN_IE_RSN)(sFrame.pBuf + sFrame.len);
@@ -3643,7 +3551,7 @@ struct vnt_tx_mgmt *s_MgrMakeReAssocRequest(struct vnt_private *pDevice,
         if ((pDevice->gsPMKID.BSSIDInfoCount > 0) && (pDevice->bRoaming == true) && (pMgmt->eAuthenMode == WMAC_AUTH_WPA2)) {
             // RSN PMKID
             pbyRSN = &sFrame.pRSN->abyRSN[18];
-            pwPMKID = (PWORD)pbyRSN; // Point to PMKID count
+            pwPMKID = (u16 *)pbyRSN; // Point to PMKID count
             *pwPMKID = 0;            // Initialize PMKID count
             pbyRSN += 2;             // Point to PMKID list
             for (ii = 0; ii < pDevice->gsPMKID.BSSIDInfoCount; ii++) {
@@ -3669,8 +3577,6 @@ struct vnt_tx_mgmt *s_MgrMakeReAssocRequest(struct vnt_private *pDevice,
         pbyIEs += sFrame.pRSN->len + WLAN_IEHDR_LEN;
     }
 
-
-
     /* Adjust the length fields */
     pTxPacket->cbMPDULen = sFrame.len;
     pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
@@ -3697,14 +3603,13 @@ struct vnt_tx_mgmt *s_MgrMakeAssocResponse(struct vnt_private *pDevice,
 	struct vnt_tx_mgmt *pTxPacket = NULL;
 	WLAN_FR_ASSOCRESP   sFrame;
 
-
 	pTxPacket = (struct vnt_tx_mgmt *)pMgmt->pbyMgmtPacketPool;
 	memset(pTxPacket, 0, sizeof(struct vnt_tx_mgmt)
 		+ WLAN_ASSOCREQ_FR_MAXLEN);
 	pTxPacket->p80211Header = (PUWLAN_80211HDR)((u8 *)pTxPacket
 		+ sizeof(struct vnt_tx_mgmt));
     // Setup the sFrame structure
-    sFrame.pBuf = (PBYTE)pTxPacket->p80211Header;
+    sFrame.pBuf = (u8 *)pTxPacket->p80211Header;
     sFrame.len = WLAN_REASSOCRESP_FR_MAXLEN;
     vMgrEncodeAssocResponse(&sFrame);
     // Setup the header
@@ -3719,7 +3624,7 @@ struct vnt_tx_mgmt *s_MgrMakeAssocResponse(struct vnt_private *pDevice,
 
     *sFrame.pwCapInfo = cpu_to_le16(wCurrCapInfo);
     *sFrame.pwStatus = cpu_to_le16(wAssocStatus);
-    *sFrame.pwAid = cpu_to_le16((WORD)(wAssocAID | BIT14 | BIT15));
+    *sFrame.pwAid = cpu_to_le16((u16)(wAssocAID | BIT14 | BIT15));
 
     // Copy the rate set
     sFrame.pSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
@@ -3745,7 +3650,6 @@ struct vnt_tx_mgmt *s_MgrMakeAssocResponse(struct vnt_private *pDevice,
     return pTxPacket;
 }
 
-
 /*+
  *
  * Routine Description:
@@ -3757,7 +3661,6 @@ struct vnt_tx_mgmt *s_MgrMakeAssocResponse(struct vnt_private *pDevice,
  *
 -*/
 
-
 struct vnt_tx_mgmt *s_MgrMakeReAssocResponse(struct vnt_private *pDevice,
 	struct vnt_manager *pMgmt, u16 wCurrCapInfo, u16 wAssocStatus,
 	u16 wAssocAID, u8 *pDstAddr, PWLAN_IE_SUPP_RATES pCurrSuppRates,
@@ -3766,14 +3669,13 @@ struct vnt_tx_mgmt *s_MgrMakeReAssocResponse(struct vnt_private *pDevice,
 	struct vnt_tx_mgmt *pTxPacket = NULL;
 	WLAN_FR_REASSOCRESP sFrame;
 
-
 	pTxPacket = (struct vnt_tx_mgmt *)pMgmt->pbyMgmtPacketPool;
 	memset(pTxPacket, 0, sizeof(struct vnt_tx_mgmt)
 		+ WLAN_ASSOCREQ_FR_MAXLEN);
 	pTxPacket->p80211Header = (PUWLAN_80211HDR)((u8 *)pTxPacket
 		+ sizeof(struct vnt_tx_mgmt));
     // Setup the sFrame structure
-    sFrame.pBuf = (PBYTE)pTxPacket->p80211Header;
+    sFrame.pBuf = (u8 *)pTxPacket->p80211Header;
     sFrame.len = WLAN_REASSOCRESP_FR_MAXLEN;
     vMgrEncodeReassocResponse(&sFrame);
     // Setup the header
@@ -3788,7 +3690,7 @@ struct vnt_tx_mgmt *s_MgrMakeReAssocResponse(struct vnt_private *pDevice,
 
     *sFrame.pwCapInfo = cpu_to_le16(wCurrCapInfo);
     *sFrame.pwStatus = cpu_to_le16(wAssocStatus);
-    *sFrame.pwAid = cpu_to_le16((WORD)(wAssocAID | BIT14 | BIT15));
+    *sFrame.pwAid = cpu_to_le16((u16)(wAssocAID | BIT14 | BIT15));
 
     // Copy the rate set
     sFrame.pSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
@@ -3814,7 +3716,6 @@ struct vnt_tx_mgmt *s_MgrMakeReAssocResponse(struct vnt_private *pDevice,
     return pTxPacket;
 }
 
-
 /*+
  *
  * Routine Description:
@@ -3835,11 +3736,10 @@ static void s_vMgrRxProbeResponse(struct vnt_private *pDevice,
 	ERPObject sERP;
 	int bChannelHit = true;
 
-
     memset(&sFrame, 0, sizeof(WLAN_FR_PROBERESP));
     // decode the frame
     sFrame.len = pRxPacket->cbMPDULen;
-    sFrame.pBuf = (PBYTE)pRxPacket->p80211Header;
+    sFrame.pBuf = (u8 *)pRxPacket->p80211Header;
     vMgrDecodeProbeResponse(&sFrame);
 
     if ((sFrame.pqwTimestamp == NULL)
@@ -3857,7 +3757,6 @@ static void s_vMgrRxProbeResponse(struct vnt_private *pDevice,
     if(sFrame.pSSID->len == 0)
        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Rx Probe resp: SSID len = 0 \n");
 
-
     //{{ RobertYu:20050201, 11a  byCurrChannel != sFrame.pDSParms->byCurrChannel mapping
     if( byCurrChannel > CB_MAX_CHANNEL_24G )
     {
@@ -3892,7 +3791,6 @@ if(ChannelExceedZoneType(pDevice,byCurrChannel)==true)
         sERP.byERP = 0;
     }
 
-
     // update or insert the bss
     pBSSList = BSSpAddrIsInBSSList((void *) pDevice,
 				   sFrame.pHdr->sA3.abyAddr3,
@@ -3952,7 +3850,6 @@ if(ChannelExceedZoneType(pDevice,byCurrChannel)==true)
  *
 -*/
 
-
 static void s_vMgrRxProbeRequest(struct vnt_private *pDevice,
 	struct vnt_manager *pMgmt, struct vnt_rx_mgmt *pRxPacket)
 {
@@ -3969,7 +3866,7 @@ static void s_vMgrRxProbeRequest(struct vnt_private *pDevice,
         memset(&sFrame, 0, sizeof(WLAN_FR_PROBEREQ));
         // decode the frame
         sFrame.len = pRxPacket->cbMPDULen;
-        sFrame.pBuf = (PBYTE)pRxPacket->p80211Header;
+        sFrame.pBuf = (u8 *)pRxPacket->p80211Header;
         vMgrDecodeProbeRequest(&sFrame);
 /*
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Probe request rx:MAC addr:%pM\n",
@@ -4000,7 +3897,7 @@ static void s_vMgrRxProbeRequest(struct vnt_private *pDevice,
                       0,
                       sFrame.pHdr->sA3.abyAddr2,
                       (PWLAN_IE_SSID)pMgmt->abyCurrSSID,
-                      (PBYTE)pMgmt->abyCurrBSSID,
+                      (u8 *)pMgmt->abyCurrBSSID,
                       (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
                       (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates,
                        byPHYType
@@ -4042,7 +3939,6 @@ void vMgrRxManagePacket(struct vnt_private *pDevice, struct vnt_manager *pMgmt,
 	NODE_STATE eNodeState = 0;
 	CMD_STATUS Status;
 
-
     if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
         if (BSSbIsSTAInNodeDB(pDevice, pRxPacket->p80211Header->sA3.abyAddr2, &uNodeIndex))
             eNodeState = pMgmt->sNodeDBTable[uNodeIndex].eNodeState;
@@ -4199,7 +4095,7 @@ int bMgrPrepareBeaconToSend(struct vnt_private *pDevice,
                   pMgmt->uCurrChannel,
                   pMgmt->wCurrATIMWindow, //0,
                   (PWLAN_IE_SSID)pMgmt->abyCurrSSID,
-                  (PBYTE)pMgmt->abyCurrBSSID,
+                  (u8 *)pMgmt->abyCurrBSSID,
                   (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
                   (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates
                 );
@@ -4214,9 +4110,6 @@ int bMgrPrepareBeaconToSend(struct vnt_private *pDevice,
     return true;
 }
 
-
-
-
 /*+
  *
  * Routine Description:
@@ -4308,8 +4201,6 @@ int bAdd_PMKID_Candidate(struct vnt_private *pDevice, u8 *pbyBSSID,
     if (pDevice->gsPMKIDCandidate.NumCandidates >= MAX_PMKIDLIST)
         return false;
 
-
-
     // Update Old Candidate
     for (ii = 0; ii < pDevice->gsPMKIDCandidate.NumCandidates; ii++) {
 	pCandidateList = &pDevice->gsPMKIDCandidate.CandidateList[ii];
@@ -4368,12 +4259,12 @@ static bool
 s_bCipherMatch (
      PKnownBSS                        pBSSNode,
      NDIS_802_11_ENCRYPTION_STATUS    EncStatus,
-     PBYTE                           pbyCCSPK,
-     PBYTE                           pbyCCSGK
+     u8 *                           pbyCCSPK,
+     u8 *                           pbyCCSGK
     )
 {
-    BYTE byMulticastCipher = KEY_CTL_INVALID;
-    BYTE byCipherMask = 0x00;
+    u8 byMulticastCipher = KEY_CTL_INVALID;
+    u8 byCipherMask = 0x00;
     int i;
 
     if (pBSSNode == NULL)
@@ -4515,4 +4406,3 @@ s_bCipherMatch (
     return true;
 }
 
-
diff --git a/drivers/staging/vt6656/wmgr.h b/drivers/staging/vt6656/wmgr.h
index 83aed45f68a..5424c7f820a 100644
--- a/drivers/staging/vt6656/wmgr.h
+++ b/drivers/staging/vt6656/wmgr.h
@@ -34,7 +34,6 @@
 #ifndef __WMGR_H__
 #define __WMGR_H__
 
-#include "ttype.h"
 #include "80211mgr.h"
 #include "80211hdr.h"
 #include "wcmd.h"
@@ -42,10 +41,6 @@
 #include "wpa2.h"
 #include "card.h"
 
-/*---------------------  Export Definitions -------------------------*/
-
-
-
 // Scan time
 #define PROBE_DELAY                  100  // (us)
 #define SWITCH_CHANNEL_DELAY         200 // (us)
@@ -57,7 +52,6 @@
 #define WCMD_ACTIVE_SCAN_TIME   20 //(ms)
 #define WCMD_PASSIVE_SCAN_TIME  100 //(ms)
 
-
 #define DEFAULT_MSDU_LIFETIME           512  // ms
 #define DEFAULT_MSDU_LIFETIME_RES_64us  8000 // 64us
 
@@ -66,7 +60,6 @@
 
 #define MAKE_BEACON_RESERVED            10  //(us)
 
-
 #define TIM_MULTICAST_MASK           0x01
 #define TIM_BITMAPOFFSET_MASK        0xFE
 #define DEFAULT_DTIM_PERIOD             1
@@ -75,18 +68,11 @@
 
 #define DEFAULT_IBSS_CHANNEL            6  //2.4G
 
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Types  ------------------------------*/
 //mike define: make timer  to expire after desired times
 #define timer_expire(timer, next_tick) mod_timer(&timer, RUN_AT(next_tick))
 
 typedef void (*TimerFunction)(unsigned long);
 
-
 //+++ NDIS related
 
 typedef u8 NDIS_802_11_MAC_ADDRESS[ETH_ALEN];
@@ -117,8 +103,6 @@ typedef struct _NDIS_802_11_ASSOCIATION_INFORMATION
 	u32 OffsetResponseIEs;
 } NDIS_802_11_ASSOCIATION_INFORMATION, *PNDIS_802_11_ASSOCIATION_INFORMATION;
 
-
-
 typedef struct tagSAssocInfo {
 	NDIS_802_11_ASSOCIATION_INFORMATION AssocInfo;
 	u8 abyIEs[WLAN_BEACON_FR_MAXLEN+WLAN_BEACON_FR_MAXLEN];
@@ -127,9 +111,6 @@ typedef struct tagSAssocInfo {
 	u8 abyReqIEs[WLAN_BEACON_FR_MAXLEN];
 } SAssocInfo, *PSAssocInfo;
 
-
-
-
 typedef enum tagWMAC_AUTHENTICATION_MODE {
 
     WMAC_AUTH_OPEN,
@@ -143,8 +124,6 @@ typedef enum tagWMAC_AUTHENTICATION_MODE {
     WMAC_AUTH_MAX       // Not a real mode, defined as upper bound
 } WMAC_AUTHENTICATION_MODE, *PWMAC_AUTHENTICATION_MODE;
 
-
-
 // Pre-configured Mode (from XP)
 
 typedef enum tagWMAC_CONFIG_MODE {
@@ -155,7 +134,6 @@ typedef enum tagWMAC_CONFIG_MODE {
 
 } WMAC_CONFIG_MODE, *PWMAC_CONFIG_MODE;
 
-
 typedef enum tagWMAC_SCAN_TYPE {
 
     WMAC_SCAN_ACTIVE,
@@ -164,7 +142,6 @@ typedef enum tagWMAC_SCAN_TYPE {
 
 } WMAC_SCAN_TYPE, *PWMAC_SCAN_TYPE;
 
-
 typedef enum tagWMAC_SCAN_STATE {
 
     WMAC_NO_SCANNING,
@@ -173,8 +150,6 @@ typedef enum tagWMAC_SCAN_STATE {
 
 } WMAC_SCAN_STATE, *PWMAC_SCAN_STATE;
 
-
-
 // Notes:
 // Basic Service Set state explained as following:
 // WMAC_STATE_IDLE          : no BSS is selected (Adhoc or Infra)
@@ -207,7 +182,6 @@ typedef enum tagWMAC_CURRENT_MODE {
 
 } WMAC_CURRENT_MODE, *PWMAC_CURRENT_MODE;
 
-
 typedef enum tagWMAC_POWER_MODE {
 
     WMAC_POWER_CAM,
@@ -216,8 +190,6 @@ typedef enum tagWMAC_POWER_MODE {
 
 } WMAC_POWER_MODE, *PWMAC_POWER_MODE;
 
-
-
 /* Tx Management Packet descriptor */
 struct vnt_tx_mgmt {
 	PUWLAN_80211HDR p80211Header;
@@ -225,7 +197,6 @@ struct vnt_tx_mgmt {
 	u32 cbPayloadLen;
 };
 
-
 /* Rx Management Packet descriptor */
 struct vnt_rx_mgmt {
 	PUWLAN_80211HDR p80211Header;
@@ -238,7 +209,6 @@ struct vnt_rx_mgmt {
 	u8 byRxChannel;
 };
 
-
 struct vnt_manager {
 	void *pAdapter;
 
@@ -340,7 +310,6 @@ struct vnt_manager {
 	u8 byMgmtPacketPool[sizeof(struct vnt_tx_mgmt)
 		+ WLAN_A3FR_MAXLEN];
 
-
 	/* One second callback timer */
 	struct timer_list sTimerSecondCallback;
 
@@ -384,10 +353,6 @@ struct vnt_manager {
 
 };
 
-/*---------------------  Export Macros ------------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
 void vMgrObjectInit(struct vnt_private *pDevice);
 
 void vMgrAssocBeginSta(struct vnt_private *pDevice,
diff --git a/drivers/staging/vt6656/wpa.c b/drivers/staging/vt6656/wpa.c
index f037be3aa16..01db4e7154d 100644
--- a/drivers/staging/vt6656/wpa.c
+++ b/drivers/staging/vt6656/wpa.c
@@ -32,7 +32,6 @@
  *
  */
 
-#include "ttype.h"
 #include "tmacro.h"
 #include "tether.h"
 #include "device.h"
@@ -42,16 +41,14 @@
 #include "wpa.h"
 #include "80211mgr.h"
 
-/*---------------------  Static Variables  --------------------------*/
 static int          msglevel                =MSG_LEVEL_INFO;
 
-const BYTE abyOUI00[4] = { 0x00, 0x50, 0xf2, 0x00 };
-const BYTE abyOUI01[4] = { 0x00, 0x50, 0xf2, 0x01 };
-const BYTE abyOUI02[4] = { 0x00, 0x50, 0xf2, 0x02 };
-const BYTE abyOUI03[4] = { 0x00, 0x50, 0xf2, 0x03 };
-const BYTE abyOUI04[4] = { 0x00, 0x50, 0xf2, 0x04 };
-const BYTE abyOUI05[4] = { 0x00, 0x50, 0xf2, 0x05 };
-
+const u8 abyOUI00[4] = { 0x00, 0x50, 0xf2, 0x00 };
+const u8 abyOUI01[4] = { 0x00, 0x50, 0xf2, 0x01 };
+const u8 abyOUI02[4] = { 0x00, 0x50, 0xf2, 0x02 };
+const u8 abyOUI03[4] = { 0x00, 0x50, 0xf2, 0x03 };
+const u8 abyOUI04[4] = { 0x00, 0x50, 0xf2, 0x04 };
+const u8 abyOUI05[4] = { 0x00, 0x50, 0xf2, 0x05 };
 
 /*+
  *
@@ -88,7 +85,6 @@ WPA_ClearRSN(
     pBSSList->bWPAValid = false;
 }
 
-
 /*+
  *
  * Description:
@@ -112,7 +108,7 @@ WPA_ParseRSN(
 {
     PWLAN_IE_RSN_AUTH  pIE_RSN_Auth = NULL;
     int                i, j, m, n = 0;
-    PBYTE              pbyCaps;
+    u8 *              pbyCaps;
 
     WPA_ClearRSN(pBSSList);
 
@@ -167,7 +163,7 @@ WPA_ParseRSN(
                     break;
                 //DBG_PRN_GRP14(("abyPKType[%d]: %X\n", j-1, pBSSList->abyPKType[j-1]));
             } //for
-            pBSSList->wPKCount = (WORD)j;
+            pBSSList->wPKCount = (u16)j;
             DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"wPKCount: %d\n", pBSSList->wPKCount);
         }
 
@@ -197,7 +193,7 @@ WPA_ParseRSN(
                 //DBG_PRN_GRP14(("abyAuthType[%d]: %X\n", j-1, pBSSList->abyAuthType[j-1]));
             }
             if(j > 0)
-                pBSSList->wAuthCount = (WORD)j;
+                pBSSList->wAuthCount = (u16)j;
             DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"wAuthCount: %d\n", pBSSList->wAuthCount);
         }
 
@@ -209,11 +205,11 @@ WPA_ParseRSN(
             DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"14+4+(m+n)*4: %d\n", 14+4+(m+n)*4);
 
             if(pRSN->len+2 >= 14+4+(m+n)*4) { //oui1(4)+ver(2)+GKS(4)+PKSCnt(2)+PKS(4*m)+AKC(2)+AKS(4*n)+Cap(2)
-                pbyCaps = (PBYTE)pIE_RSN_Auth->AuthKSList[n].abyOUI;
+                pbyCaps = (u8 *)pIE_RSN_Auth->AuthKSList[n].abyOUI;
                 pBSSList->byDefaultK_as_PK = (*pbyCaps) & WPA_GROUPFLAG;
                 pBSSList->byReplayIdx = 2 << ((*pbyCaps >> WPA_REPLAYBITSSHIFT) & WPA_REPLAYBITS);
                 pBSSList->sRSNCapObj.bRSNCapExist = true;
-                pBSSList->sRSNCapObj.wRSNCap = *(PWORD)pbyCaps;
+                pBSSList->sRSNCapObj.wRSNCap = *(u16 *)pbyCaps;
                 //DBG_PRN_GRP14(("pbyCaps: %X\n", *pbyCaps));
                 //DBG_PRN_GRP14(("byDefaultK_as_PK: %X\n", pBSSList->byDefaultK_as_PK));
                 //DBG_PRN_GRP14(("byReplayIdx: %X\n", pBSSList->byReplayIdx));
@@ -241,13 +237,13 @@ WPA_ParseRSN(
 -*/
 bool
 WPA_SearchRSN(
-    BYTE                byCmd,
-    BYTE                byEncrypt,
+    u8                byCmd,
+    u8                byEncrypt,
      PKnownBSS        pBSSList
     )
 {
     int ii;
-    BYTE byPKType = WPA_NONE;
+    u8 byPKType = WPA_NONE;
 
     if (pBSSList->bWPAValid == false)
         return false;
diff --git a/drivers/staging/vt6656/wpa.h b/drivers/staging/vt6656/wpa.h
index 0369cbf32c4..2a724c064e5 100644
--- a/drivers/staging/vt6656/wpa.h
+++ b/drivers/staging/vt6656/wpa.h
@@ -31,11 +31,8 @@
 #ifndef __WPA_H__
 #define __WPA_H__
 
-#include "ttype.h"
 #include "80211hdr.h"
 
-/*---------------------  Export Definitions -------------------------*/
-
 #define WPA_NONE            0
 #define WPA_WEP40           1
 #define WPA_TKIP            2
@@ -49,15 +46,6 @@
 #define WPA_REPLAYBITSSHIFT 2
 #define WPA_REPLAYBITS      0x03
 
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Types  ------------------------------*/
-
-
-/*---------------------  Export Functions  --------------------------*/
-
 void
 WPA_ClearRSN(
      PKnownBSS        pBSSList
@@ -71,8 +59,8 @@ WPA_ParseRSN(
 
 bool
 WPA_SearchRSN(
-    BYTE                byCmd,
-    BYTE                byEncrypt,
+    u8                byCmd,
+    u8                byEncrypt,
      PKnownBSS        pBSSList
     );
 
diff --git a/drivers/staging/vt6656/wpa2.c b/drivers/staging/vt6656/wpa2.c
index a89456a9137..aa221618434 100644
--- a/drivers/staging/vt6656/wpa2.c
+++ b/drivers/staging/vt6656/wpa2.c
@@ -34,28 +34,17 @@
 #include "device.h"
 #include "wpa2.h"
 
-/*---------------------  Static Definitions -------------------------*/
 static int          msglevel                =MSG_LEVEL_INFO;
 //static int          msglevel                =MSG_LEVEL_DEBUG;
-/*---------------------  Static Classes  ----------------------------*/
 
-/*---------------------  Static Variables  --------------------------*/
+const u8 abyOUIGK[4]      = { 0x00, 0x0F, 0xAC, 0x00 };
+const u8 abyOUIWEP40[4]   = { 0x00, 0x0F, 0xAC, 0x01 };
+const u8 abyOUIWEP104[4]  = { 0x00, 0x0F, 0xAC, 0x05 };
+const u8 abyOUITKIP[4]    = { 0x00, 0x0F, 0xAC, 0x02 };
+const u8 abyOUICCMP[4]    = { 0x00, 0x0F, 0xAC, 0x04 };
 
-const BYTE abyOUIGK[4]      = { 0x00, 0x0F, 0xAC, 0x00 };
-const BYTE abyOUIWEP40[4]   = { 0x00, 0x0F, 0xAC, 0x01 };
-const BYTE abyOUIWEP104[4]  = { 0x00, 0x0F, 0xAC, 0x05 };
-const BYTE abyOUITKIP[4]    = { 0x00, 0x0F, 0xAC, 0x02 };
-const BYTE abyOUICCMP[4]    = { 0x00, 0x0F, 0xAC, 0x04 };
-
-const BYTE abyOUI8021X[4]   = { 0x00, 0x0F, 0xAC, 0x01 };
-const BYTE abyOUIPSK[4]     = { 0x00, 0x0F, 0xAC, 0x02 };
-
-
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
+const u8 abyOUI8021X[4]   = { 0x00, 0x0F, 0xAC, 0x01 };
+const u8 abyOUIPSK[4]     = { 0x00, 0x0F, 0xAC, 0x02 };
 
 /*+
  *
@@ -113,8 +102,8 @@ WPA2vParseRSN (
     )
 {
     int                 i, j;
-    WORD                m = 0, n = 0;
-    PBYTE               pbyOUI;
+    u16                m = 0, n = 0;
+    u8 *               pbyOUI;
     bool                bUseGK = false;
 
     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"WPA2_ParseRSN: [%d]\n", pRSN->len);
@@ -163,11 +152,11 @@ WPA2vParseRSN (
         }
 
         if (pRSN->len >= 8) { // ver(2) + GK(4) + PK count(2)
-            pBSSNode->wCSSPKCount = *((PWORD) &(pRSN->abyRSN[4]));
+            pBSSNode->wCSSPKCount = *((u16 *) &(pRSN->abyRSN[4]));
             j = 0;
             pbyOUI = &(pRSN->abyRSN[6]);
 
-            for (i = 0; (i < pBSSNode->wCSSPKCount) && (j < sizeof(pBSSNode->abyCSSPK)/sizeof(BYTE)); i++) {
+            for (i = 0; (i < pBSSNode->wCSSPKCount) && (j < sizeof(pBSSNode->abyCSSPK)/sizeof(u8)); i++) {
 
                 if (pRSN->len >= 8+i*4+4) { // ver(2)+GK(4)+PKCnt(2)+PKS(4*i)
                     if ( !memcmp(pbyOUI, abyOUIGK, 4)) {
@@ -208,17 +197,17 @@ WPA2vParseRSN (
                 // invalid CSS, No valid PK.
                 return;
             }
-            pBSSNode->wCSSPKCount = (WORD)j;
+            pBSSNode->wCSSPKCount = (u16)j;
             DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"wCSSPKCount: %d\n", pBSSNode->wCSSPKCount);
         }
 
-        m = *((PWORD) &(pRSN->abyRSN[4]));
+        m = *((u16 *) &(pRSN->abyRSN[4]));
 
         if (pRSN->len >= 10+m*4) { // ver(2) + GK(4) + PK count(2) + PKS(4*m) + AKMSS count(2)
-            pBSSNode->wAKMSSAuthCount = *((PWORD) &(pRSN->abyRSN[6+4*m]));
+            pBSSNode->wAKMSSAuthCount = *((u16 *) &(pRSN->abyRSN[6+4*m]));
             j = 0;
             pbyOUI = &(pRSN->abyRSN[8+4*m]);
-            for (i = 0; (i < pBSSNode->wAKMSSAuthCount) && (j < sizeof(pBSSNode->abyAKMSSAuthType)/sizeof(BYTE)); i++) {
+            for (i = 0; (i < pBSSNode->wAKMSSAuthCount) && (j < sizeof(pBSSNode->abyAKMSSAuthType)/sizeof(u8)); i++) {
                 if (pRSN->len >= 10+(m+i)*4+4) { // ver(2)+GK(4)+PKCnt(2)+PKS(4*m)+AKMSS(2)+AKS(4*i)
                     if ( !memcmp(pbyOUI, abyOUI8021X, 4))
                         pBSSNode->abyAKMSSAuthType[j++] = WLAN_11i_AKMSS_802_1X;
@@ -231,134 +220,16 @@ WPA2vParseRSN (
                 } else
                     break;
             }
-            pBSSNode->wAKMSSAuthCount = (WORD)j;
+            pBSSNode->wAKMSSAuthCount = (u16)j;
             DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"wAKMSSAuthCount: %d\n", pBSSNode->wAKMSSAuthCount);
 
-            n = *((PWORD) &(pRSN->abyRSN[6+4*m]));
+            n = *((u16 *) &(pRSN->abyRSN[6+4*m]));
             if (pRSN->len >= 12+4*m+4*n) { // ver(2)+GK(4)+PKCnt(2)+PKS(4*m)+AKMSSCnt(2)+AKMSS(4*n)+Cap(2)
                 pBSSNode->sRSNCapObj.bRSNCapExist = true;
-                pBSSNode->sRSNCapObj.wRSNCap = *((PWORD) &(pRSN->abyRSN[8+4*m+4*n]));
+                pBSSNode->sRSNCapObj.wRSNCap = *((u16 *) &(pRSN->abyRSN[8+4*m+4*n]));
             }
         }
         //ignore PMKID lists bcs only (Re)Assocrequest has this field
         pBSSNode->bWPA2Valid = true;
     }
 }
-
-
-/*+
- *
- * Description:
- *    Set WPA IEs
- *
- * Parameters:
- *  In:
- *      pMgmtHandle - Pointer to management object
- *  Out:
- *      pRSNIEs     - Pointer to the RSN IE to set.
- *
- * Return Value: length of IEs.
- *
--*/
-unsigned int WPA2uSetIEs(void *pMgmtHandle, PWLAN_IE_RSN pRSNIEs)
-{
-	struct vnt_manager *pMgmt = (struct vnt_manager *)pMgmtHandle;
-	u8 *pbyBuffer = NULL;
-	int ii = 0;
-	u16 *pwPMKID = NULL;
-
-	if (pRSNIEs == NULL)
-		return 0;
-
-    if (((pMgmt->eAuthenMode == WMAC_AUTH_WPA2) ||
-         (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) &&
-        (pMgmt->pCurrBSS != NULL)) {
-        /* WPA2 IE */
-        pbyBuffer = (PBYTE) pRSNIEs;
-        pRSNIEs->byElementID = WLAN_EID_RSN;
-        pRSNIEs->len = 6; //Version(2)+GK(4)
-        pRSNIEs->wVersion = 1;
-        //Group Key Cipher Suite
-        pRSNIEs->abyRSN[0] = 0x00;
-        pRSNIEs->abyRSN[1] = 0x0F;
-        pRSNIEs->abyRSN[2] = 0xAC;
-        if (pMgmt->byCSSGK == KEY_CTL_WEP) {
-            pRSNIEs->abyRSN[3] = pMgmt->pCurrBSS->byCSSGK;
-        } else if (pMgmt->byCSSGK == KEY_CTL_TKIP) {
-            pRSNIEs->abyRSN[3] = WLAN_11i_CSS_TKIP;
-        } else if (pMgmt->byCSSGK == KEY_CTL_CCMP) {
-            pRSNIEs->abyRSN[3] = WLAN_11i_CSS_CCMP;
-        } else {
-            pRSNIEs->abyRSN[3] = WLAN_11i_CSS_UNKNOWN;
-        }
-
-        // Pairwise Key Cipher Suite
-        pRSNIEs->abyRSN[4] = 1;
-        pRSNIEs->abyRSN[5] = 0;
-        pRSNIEs->abyRSN[6] = 0x00;
-        pRSNIEs->abyRSN[7] = 0x0F;
-        pRSNIEs->abyRSN[8] = 0xAC;
-        if (pMgmt->byCSSPK == KEY_CTL_TKIP) {
-            pRSNIEs->abyRSN[9] = WLAN_11i_CSS_TKIP;
-        } else if (pMgmt->byCSSPK == KEY_CTL_CCMP) {
-            pRSNIEs->abyRSN[9] = WLAN_11i_CSS_CCMP;
-        } else if (pMgmt->byCSSPK == KEY_CTL_NONE) {
-            pRSNIEs->abyRSN[9] = WLAN_11i_CSS_USE_GROUP;
-        } else {
-            pRSNIEs->abyRSN[9] = WLAN_11i_CSS_UNKNOWN;
-        }
-        pRSNIEs->len += 6;
-
-        // Auth Key Management Suite
-        pRSNIEs->abyRSN[10] = 1;
-        pRSNIEs->abyRSN[11] = 0;
-        pRSNIEs->abyRSN[12] = 0x00;
-        pRSNIEs->abyRSN[13] = 0x0F;
-        pRSNIEs->abyRSN[14] = 0xAC;
-        if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK) {
-            pRSNIEs->abyRSN[15] = WLAN_11i_AKMSS_PSK;
-        } else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2) {
-            pRSNIEs->abyRSN[15] = WLAN_11i_AKMSS_802_1X;
-        } else {
-            pRSNIEs->abyRSN[15] = WLAN_11i_AKMSS_UNKNOWN;
-        }
-        pRSNIEs->len +=6;
-
-        // RSN Capabilites
-        if (pMgmt->pCurrBSS->sRSNCapObj.bRSNCapExist == true) {
-            memcpy(&pRSNIEs->abyRSN[16], &pMgmt->pCurrBSS->sRSNCapObj.wRSNCap, 2);
-        } else {
-            pRSNIEs->abyRSN[16] = 0;
-            pRSNIEs->abyRSN[17] = 0;
-        }
-        pRSNIEs->len +=2;
-
-	if ((pMgmt->gsPMKIDCache.BSSIDInfoCount > 0) &&
-	    (pMgmt->bRoaming == true) &&
-            (pMgmt->eAuthenMode == WMAC_AUTH_WPA2)) {
-		/* RSN PMKID, pointer to PMKID count */
-		pwPMKID = (PWORD)(&pRSNIEs->abyRSN[18]);
-		*pwPMKID = 0;                     /* Initialize PMKID count */
-		pbyBuffer = &pRSNIEs->abyRSN[20];    /* Point to PMKID list */
-		for (ii = 0; ii < pMgmt->gsPMKIDCache.BSSIDInfoCount; ii++) {
-			if (!memcmp(&pMgmt->
-				    gsPMKIDCache.BSSIDInfo[ii].abyBSSID[0],
-				    pMgmt->abyCurrBSSID,
-				    ETH_ALEN)) {
-				(*pwPMKID)++;
-				memcpy(pbyBuffer,
-			pMgmt->gsPMKIDCache.BSSIDInfo[ii].abyPMKID,
-				       16);
-				pbyBuffer += 16;
-			}
-		}
-            if (*pwPMKID != 0) {
-                pRSNIEs->len += (2 + (*pwPMKID)*16);
-            } else {
-                pbyBuffer = &pRSNIEs->abyRSN[18];
-            }
-        }
-        return(pRSNIEs->len + WLAN_IEHDR_LEN);
-    }
-    return(0);
-}
diff --git a/drivers/staging/vt6656/wpa2.h b/drivers/staging/vt6656/wpa2.h
index c359252a6b0..dc505ce7a7e 100644
--- a/drivers/staging/vt6656/wpa2.h
+++ b/drivers/staging/vt6656/wpa2.h
@@ -31,17 +31,15 @@
 #ifndef __WPA2_H__
 #define __WPA2_H__
 
-#include "ttype.h"
 #include "80211mgr.h"
 #include "80211hdr.h"
 #include "bssdb.h"
 
-/*---------------------  Export Definitions -------------------------*/
 #define MAX_PMKID_CACHE         16
 
 typedef struct tagsPMKIDInfo {
-    BYTE    abyBSSID[6];
-    BYTE    abyPMKID[16];
+    u8    abyBSSID[6];
+    u8    abyPMKID[16];
 } PMKIDInfo, *PPMKIDInfo;
 
 typedef struct tagSPMKIDCache {
@@ -49,18 +47,7 @@ typedef struct tagSPMKIDCache {
 	PMKIDInfo BSSIDInfo[MAX_PMKID_CACHE];
 } SPMKIDCache, *PSPMKIDCache;
 
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Types  ------------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
 void WPA2_ClearRSN(PKnownBSS pBSSNode);
 void WPA2vParseRSN(PKnownBSS pBSSNode, PWLAN_IE_RSN pRSN);
 
-unsigned int WPA2uSetIEs(void *pMgmtHandle, PWLAN_IE_RSN pRSNIEs);
-
 #endif /* __WPA2_H__ */
diff --git a/drivers/staging/vt6656/wpactl.c b/drivers/staging/vt6656/wpactl.c
index 53629b26f24..9f1b413ce86 100644
--- a/drivers/staging/vt6656/wpactl.c
+++ b/drivers/staging/vt6656/wpactl.c
@@ -42,18 +42,8 @@
 #include "rndis.h"
 #include "rf.h"
 
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
 static int msglevel = MSG_LEVEL_INFO;
 
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-
 /*
  * Description:
  *      Set WPA algorithm & keys
@@ -71,11 +61,11 @@ int wpa_set_keys(struct vnt_private *pDevice, void *ctx)
 {
 	struct viawget_wpa_param *param = ctx;
 	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
-	DWORD dwKeyIndex = 0;
-	BYTE abyKey[MAX_KEY_LEN];
-	BYTE abySeq[MAX_KEY_LEN];
+	u32 dwKeyIndex = 0;
+	u8 abyKey[MAX_KEY_LEN];
+	u8 abySeq[MAX_KEY_LEN];
 	u64 KeyRSC;
-	BYTE byKeyDecMode = KEY_CTL_WEP;
+	u8 byKeyDecMode = KEY_CTL_WEP;
 	int ret = 0;
 	int uu;
 	int ii;
@@ -101,14 +91,14 @@ int wpa_set_keys(struct vnt_private *pDevice, void *ctx)
 
 	memcpy(&abyKey[0], param->u.wpa_key.key, param->u.wpa_key.key_len);
 
-	dwKeyIndex = (DWORD)(param->u.wpa_key.key_index);
+	dwKeyIndex = (u32)(param->u.wpa_key.key_index);
 
 	if (param->u.wpa_key.alg_name == WPA_ALG_WEP) {
 		if (dwKeyIndex > 3) {
 			return -EINVAL;
 		} else {
 			if (param->u.wpa_key.set_tx) {
-				pDevice->byKeyIndex = (BYTE)dwKeyIndex;
+				pDevice->byKeyIndex = (u8)dwKeyIndex;
 				pDevice->bTransmitKey = true;
 				dwKeyIndex |= (1 << 31);
 			}
@@ -127,7 +117,6 @@ int wpa_set_keys(struct vnt_private *pDevice, void *ctx)
 		return ret;
 	}
 
-
 	if (param->u.wpa_key.seq && param->u.wpa_key.seq_len > sizeof(abySeq))
 		return -EINVAL;
 
@@ -159,7 +148,6 @@ int wpa_set_keys(struct vnt_private *pDevice, void *ctx)
 	if (param->u.wpa_key.set_tx)
 		dwKeyIndex |= (1 << 31);
 
-
 	if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled)
 		byKeyDecMode = KEY_CTL_CCMP;
 	else if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled)
@@ -204,7 +192,7 @@ int wpa_set_keys(struct vnt_private *pDevice, void *ctx)
 		if ((KeybSetAllGroupKey(pDevice, &(pDevice->sKey), dwKeyIndex,
 							param->u.wpa_key.key_len,
 							&KeyRSC,
-							(PBYTE)abyKey,
+							(u8 *)abyKey,
 							byKeyDecMode
 					) == true) &&
 			(KeybSetDefaultKey(pDevice,
@@ -212,7 +200,7 @@ int wpa_set_keys(struct vnt_private *pDevice, void *ctx)
 					dwKeyIndex,
 					param->u.wpa_key.key_len,
 					&KeyRSC,
-					(PBYTE)abyKey,
+					(u8 *)abyKey,
 					byKeyDecMode
 				) == true) ) {
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "GROUP Key Assign.\n");
@@ -234,7 +222,7 @@ int wpa_set_keys(struct vnt_private *pDevice, void *ctx)
 		}
 		if (KeybSetKey(pDevice, &(pDevice->sKey), &param->addr[0],
 				dwKeyIndex, param->u.wpa_key.key_len,
-				&KeyRSC, (PBYTE)abyKey, byKeyDecMode
+				&KeyRSC, (u8 *)abyKey, byKeyDecMode
 				) == true) {
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Pairwise Key Set\n");
 		} else {
@@ -250,7 +238,7 @@ int wpa_set_keys(struct vnt_private *pDevice, void *ctx)
 		}
 	} // BSSID not 0xffffffffffff
 	if ((ret == 0) && ((param->u.wpa_key.set_tx) != 0)) {
-		pDevice->byKeyIndex = (BYTE)param->u.wpa_key.key_index;
+		pDevice->byKeyIndex = (u8)param->u.wpa_key.key_index;
 		pDevice->bTransmitKey = true;
 	}
 	pDevice->bEncryptionEnable = true;
@@ -258,4 +246,3 @@ int wpa_set_keys(struct vnt_private *pDevice, void *ctx)
 	return ret;
 }
 
-
diff --git a/drivers/staging/vt6656/wpactl.h b/drivers/staging/vt6656/wpactl.h
index 2235ee97012..e032a1b94ae 100644
--- a/drivers/staging/vt6656/wpactl.h
+++ b/drivers/staging/vt6656/wpactl.h
@@ -32,9 +32,6 @@
 #include "device.h"
 #include "iowpa.h"
 
-/*---------------------  Export Definitions -------------------------*/
-
-
 //WPA related
 
 typedef enum { WPA_ALG_NONE, WPA_ALG_WEP, WPA_ALG_TKIP, WPA_ALG_CCMP } wpa_alg;
@@ -43,15 +40,8 @@ typedef enum { WPA_ALG_NONE, WPA_ALG_WEP, WPA_ALG_TKIP, WPA_ALG_CCMP } wpa_alg;
 #define AUTH_ALG_SHARED_KEY	0x02
 #define AUTH_ALG_LEAP		0x04
 
-
 typedef unsigned long long NDIS_802_11_KEY_RSC;
 
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
 int wpa_set_keys(struct vnt_private *, void *ctx);
 
 #endif /* __WPACL_H__ */
diff --git a/drivers/staging/wlan-ng/prism2sta.c b/drivers/staging/wlan-ng/prism2sta.c
index 8d2277bb898..428a9be2501 100644
--- a/drivers/staging/wlan-ng/prism2sta.c
+++ b/drivers/staging/wlan-ng/prism2sta.c
@@ -1160,30 +1160,33 @@ static void prism2sta_inf_chinforesults(wlandevice_t *wlandev,
 	    le16_to_cpu(inf->info.chinforesult.scanchannels);
 
 	for (i = 0, n = 0; i < HFA384x_CHINFORESULT_MAX; i++) {
-		if (hw->channel_info.results.scanchannels & (1 << i)) {
-			int channel =
-			    le16_to_cpu(inf->info.chinforesult.result[n].chid) -
-			    1;
-			hfa384x_ChInfoResultSub_t *chinforesult =
-			    &hw->channel_info.results.result[channel];
-			chinforesult->chid = channel;
-			chinforesult->anl =
-			    le16_to_cpu(inf->info.chinforesult.result[n].anl);
-			chinforesult->pnl =
-			    le16_to_cpu(inf->info.chinforesult.result[n].pnl);
-			chinforesult->active =
-			    le16_to_cpu(inf->info.chinforesult.result[n].
-					active);
-	pr_debug
-		("chinfo: channel %d, %s level (avg/peak)=%d/%d dB, pcf %d\n",
-			     channel + 1,
-			     chinforesult->
-			     active & HFA384x_CHINFORESULT_BSSACTIVE ? "signal"
-			     : "noise", chinforesult->anl, chinforesult->pnl,
-			     chinforesult->
-			     active & HFA384x_CHINFORESULT_PCFACTIVE ? 1 : 0);
-			n++;
-		}
+		hfa384x_ChInfoResultSub_t *result;
+		hfa384x_ChInfoResultSub_t *chinforesult;
+		int chan;
+
+		if (!(hw->channel_info.results.scanchannels & (1 << i)))
+			continue;
+
+		result = &inf->info.chinforesult.result[n];
+		chan = le16_to_cpu(result->chid) - 1;
+
+		if (chan < 0 || chan >= HFA384x_CHINFORESULT_MAX)
+			continue;
+
+		chinforesult = &hw->channel_info.results.result[chan];
+		chinforesult->chid = chan;
+		chinforesult->anl = le16_to_cpu(result->anl);
+		chinforesult->pnl = le16_to_cpu(result->pnl);
+		chinforesult->active = le16_to_cpu(result->active);
+
+		pr_debug("chinfo: channel %d, %s level (avg/peak)=%d/%d dB, pcf %d\n",
+			 chan + 1,
+			 (chinforesult->active & HFA384x_CHINFORESULT_BSSACTIVE)
+				? "signal" : "noise",
+			 chinforesult->anl, chinforesult->pnl,
+			 (chinforesult->active & HFA384x_CHINFORESULT_PCFACTIVE)
+				? 1 : 0);
+		n++;
 	}
 	atomic_set(&hw->channel_info.done, 2);
 
diff --git a/drivers/staging/wlan-ng/prism2usb.c b/drivers/staging/wlan-ng/prism2usb.c
index b1aed1f1f74..b401974fb28 100644
--- a/drivers/staging/wlan-ng/prism2usb.c
+++ b/drivers/staging/wlan-ng/prism2usb.c
@@ -113,14 +113,14 @@ static int prism2sta_probe_usb(struct usb_interface *interface,
 	dev = interface_to_usbdev(interface);
 	wlandev = create_wlan();
 	if (wlandev == NULL) {
-		printk(KERN_ERR "%s: Memory allocation failure.\n", dev_info);
+		dev_err(&interface->dev, "Memory allocation failure.\n");
 		result = -EIO;
 		goto failed;
 	}
 	hw = wlandev->priv;
 
 	if (wlan_setup(wlandev, &(interface->dev)) != 0) {
-		printk(KERN_ERR "%s: wlan_setup() failed.\n", dev_info);
+		dev_err(&interface->dev, "wlan_setup() failed.\n");
 		result = -EIO;
 		goto failed;
 	}
@@ -143,8 +143,7 @@ static int prism2sta_probe_usb(struct usb_interface *interface,
 			unregister_wlandev(wlandev);
 			hfa384x_destroy(hw);
 			result = -EIO;
-			printk(KERN_ERR
-			       "%s: hfa384x_corereset() failed.\n", dev_info);
+			dev_err(&interface->dev, "hfa384x_corereset() failed.\n");
 			goto failed;
 		}
 	}
@@ -158,7 +157,7 @@ static int prism2sta_probe_usb(struct usb_interface *interface,
 	prism2sta_ifstate(wlandev, P80211ENUM_ifstate_enable);
 
 	if (register_wlandev(wlandev) != 0) {
-		printk(KERN_ERR "%s: register_wlandev() failed.\n", dev_info);
+		dev_err(&interface->dev, "register_wlandev() failed.\n");
 		result = -EIO;
 		goto failed;
 	}
@@ -329,8 +328,7 @@ static int prism2sta_resume(struct usb_interface *interface)
 		if (result != 0) {
 			unregister_wlandev(wlandev);
 			hfa384x_destroy(hw);
-			printk(KERN_ERR
-			       "%s: hfa384x_corereset() failed.\n", dev_info);
+			dev_err(&interface->dev, "hfa384x_corereset() failed.\n");
 			kfree(wlandev);
 			kfree(hw);
 			wlandev = NULL;
diff --git a/drivers/staging/xgifb/vb_def.h b/drivers/staging/xgifb/vb_def.h
index 148f6373c9a..80c97230588 100644
--- a/drivers/staging/xgifb/vb_def.h
+++ b/drivers/staging/xgifb/vb_def.h
@@ -62,10 +62,6 @@
 #define EnableVBCLKDRVLOW    0x4000
 #define EnablePLLSPLOW       0x8000
 
-#define LCDBToA              0x20   /* LCD SetFlag */
-#define StLCDBToA            0x40
-#define LockLCDBToA          0x80
-#define   LCDToFull          0x10
 #define AVIDEOSense          0x01   /* CR32 */
 #define SVIDEOSense          0x02
 #define SCARTSense           0x04
diff --git a/drivers/staging/xgifb/vb_init.c b/drivers/staging/xgifb/vb_init.c
index df127e40695..19ce5a978ca 100644
--- a/drivers/staging/xgifb/vb_init.c
+++ b/drivers/staging/xgifb/vb_init.c
@@ -90,18 +90,14 @@ static void XGINew_DDR1x_MRS_340(unsigned long P3c4,
 	xgifb_reg_set(P3c4, 0x16, 0x80);
 
 	udelay(60);
-	xgifb_reg_set(P3c4,
-		      0x18,
-		      pVBInfo->SR15[2][pVBInfo->ram_type]); /* SR18 */
+	xgifb_reg_set(P3c4, 0x18, pVBInfo->SR18[pVBInfo->ram_type]); /* SR18 */
 	xgifb_reg_set(P3c4, 0x19, 0x01);
 	xgifb_reg_set(P3c4, 0x16, 0x03);
 	xgifb_reg_set(P3c4, 0x16, 0x83);
 	mdelay(1);
 	xgifb_reg_set(P3c4, 0x1B, 0x03);
 	udelay(500);
-	xgifb_reg_set(P3c4,
-		      0x18,
-		      pVBInfo->SR15[2][pVBInfo->ram_type]); /* SR18 */
+	xgifb_reg_set(P3c4, 0x18, pVBInfo->SR18[pVBInfo->ram_type]); /* SR18 */
 	xgifb_reg_set(P3c4, 0x19, 0x00);
 	xgifb_reg_set(P3c4, 0x16, 0x03);
 	xgifb_reg_set(P3c4, 0x16, 0x83);
@@ -265,18 +261,14 @@ static void XGINew_DDR1x_MRS_XG20(unsigned long P3c4,
 	xgifb_reg_set(P3c4, 0x16, 0x00);
 	xgifb_reg_set(P3c4, 0x16, 0x80);
 	udelay(60);
-	xgifb_reg_set(P3c4,
-		      0x18,
-		      pVBInfo->SR15[2][pVBInfo->ram_type]); /* SR18 */
+	xgifb_reg_set(P3c4, 0x18, pVBInfo->SR18[pVBInfo->ram_type]); /* SR18 */
 	xgifb_reg_set(P3c4, 0x19, 0x01);
 	xgifb_reg_set(P3c4, 0x16, 0x03);
 	xgifb_reg_set(P3c4, 0x16, 0x83);
 	mdelay(1);
 	xgifb_reg_set(P3c4, 0x1B, 0x03);
 	udelay(500);
-	xgifb_reg_set(P3c4,
-		      0x18,
-		      pVBInfo->SR15[2][pVBInfo->ram_type]); /* SR18 */
+	xgifb_reg_set(P3c4, 0x18, pVBInfo->SR18[pVBInfo->ram_type]); /* SR18 */
 	xgifb_reg_set(P3c4, 0x19, 0x00);
 	xgifb_reg_set(P3c4, 0x16, 0x03);
 	xgifb_reg_set(P3c4, 0x16, 0x83);
@@ -507,9 +499,7 @@ static void XGINew_SetDRAMDefaultRegister340(
 		xgifb_reg_set(P3d4, 0xB0, 0x80); /* DDRII Dual frequency mode */
 		XGINew_DDR2_DefaultRegister(HwDeviceExtension, P3d4, pVBInfo);
 	}
-	xgifb_reg_set(P3c4,
-		      0x1B,
-		      pVBInfo->SR15[3][pVBInfo->ram_type]); /* SR1B */
+	xgifb_reg_set(P3c4, 0x1B, 0x03); /* SR1B */
 }
 
 
@@ -888,7 +878,7 @@ done:
 	return rom_copy;
 }
 
-static void xgifb_read_vbios(struct pci_dev *pdev,
+static bool xgifb_read_vbios(struct pci_dev *pdev,
 			      struct vb_device_info *pVBInfo)
 {
 	struct xgifb_video_info *xgifb_info = pci_get_drvdata(pdev);
@@ -899,13 +889,10 @@ static void xgifb_read_vbios(struct pci_dev *pdev,
 	size_t vbios_size;
 	int entry;
 
-	if (xgifb_info->chip != XG21)
-		return;
-	pVBInfo->IF_DEF_LVDS = 0;
 	vbios = xgifb_copy_rom(pdev, &vbios_size);
 	if (vbios == NULL) {
 		dev_err(&pdev->dev, "Video BIOS not available\n");
-		return;
+		return false;
 	}
 	if (vbios_size <= 0x65)
 		goto error;
@@ -917,7 +904,7 @@ static void xgifb_read_vbios(struct pci_dev *pdev,
 	    (!xgifb_info->display2_force ||
 	     xgifb_info->display2 != XGIFB_DISP_LCD)) {
 		vfree(vbios);
-		return;
+		return false;
 	}
 	if (vbios_size <= 0x317)
 		goto error;
@@ -956,11 +943,11 @@ static void xgifb_read_vbios(struct pci_dev *pdev,
 	lvds->PSC_S4		= vbios[i + 23];
 	lvds->PSC_S5		= vbios[i + 24];
 	vfree(vbios);
-	pVBInfo->IF_DEF_LVDS = 1;
-	return;
+	return true;
 error:
 	dev_err(&pdev->dev, "Video BIOS corrupted\n");
 	vfree(vbios);
+	return false;
 }
 
 static void XGINew_ChkSenseStatus(struct xgi_hw_device_info *HwDeviceExtension,
@@ -1132,12 +1119,13 @@ static unsigned short XGINew_SenseLCD(struct xgi_hw_device_info
 	}
 }
 
-static void XGINew_GetXG21Sense(struct xgi_hw_device_info *HwDeviceExtension,
+static void XGINew_GetXG21Sense(struct pci_dev *pdev,
 		struct vb_device_info *pVBInfo)
 {
+	struct xgifb_video_info *xgifb_info = pci_get_drvdata(pdev);
 	unsigned char Temp;
 
-	if (pVBInfo->IF_DEF_LVDS) { /* For XG21 LVDS */
+	if (xgifb_read_vbios(pdev, pVBInfo)) { /* For XG21 LVDS */
 		xgifb_reg_or(pVBInfo->P3d4, 0x32, LCDSense);
 		/* LVDS on chip */
 		xgifb_reg_and_or(pVBInfo->P3d4, 0x38, ~0xE0, 0xC0);
@@ -1146,7 +1134,7 @@ static void XGINew_GetXG21Sense(struct xgi_hw_device_info *HwDeviceExtension,
 		xgifb_reg_and_or(pVBInfo->P3d4, 0x4A, ~0x03, 0x03);
 		Temp = xgifb_reg_get(pVBInfo->P3d4, 0x48) & 0xC0;
 		if (Temp == 0xC0) { /* DVI & DVO GPIOA/B pull high */
-			XGINew_SenseLCD(HwDeviceExtension, pVBInfo);
+			XGINew_SenseLCD(&xgifb_info->hw_info, pVBInfo);
 			xgifb_reg_or(pVBInfo->P3d4, 0x32, LCDSense);
 			/* Enable read GPIOF */
 			xgifb_reg_and_or(pVBInfo->P3d4, 0x4A, ~0x20, 0x20);
@@ -1172,7 +1160,6 @@ static void XGINew_GetXG27Sense(struct xgi_hw_device_info *HwDeviceExtension,
 {
 	unsigned char Temp, bCR4A;
 
-	pVBInfo->IF_DEF_LVDS = 0;
 	bCR4A = xgifb_reg_get(pVBInfo->P3d4, 0x4A);
 	/* Enable GPIOA/B/C read  */
 	xgifb_reg_and_or(pVBInfo->P3d4, 0x4A, ~0x07, 0x07);
@@ -1254,14 +1241,12 @@ unsigned char XGIInitNew(struct pci_dev *pdev)
 
 	InitTo330Pointer(HwDeviceExtension->jChipType, pVBInfo);
 
-	xgifb_read_vbios(pdev, pVBInfo);
-
 	/* Openkey */
 	xgifb_reg_set(pVBInfo->P3c4, 0x05, 0x86);
 
 	/* GetXG21Sense (GPIO) */
 	if (HwDeviceExtension->jChipType == XG21)
-		XGINew_GetXG21Sense(HwDeviceExtension, pVBInfo);
+		XGINew_GetXG21Sense(pdev, pVBInfo);
 
 	if (HwDeviceExtension->jChipType == XG27)
 		XGINew_GetXG27Sense(HwDeviceExtension, pVBInfo);
@@ -1369,17 +1354,14 @@ unsigned char XGIInitNew(struct pci_dev *pdev)
 
 	if (HwDeviceExtension->jChipType < XG20) {
 		if (XGI_BridgeIsOn(pVBInfo) == 1) {
-			if (pVBInfo->IF_DEF_LVDS == 0) {
-				xgifb_reg_set(pVBInfo->Part2Port, 0x00, 0x1C);
-				xgifb_reg_set(pVBInfo->Part4Port,
-					      0x0D, XGI330_CRT2Data_4_D);
-				xgifb_reg_set(pVBInfo->Part4Port,
-					      0x0E, XGI330_CRT2Data_4_E);
-				xgifb_reg_set(pVBInfo->Part4Port,
-					      0x10, XGI330_CRT2Data_4_10);
-				xgifb_reg_set(pVBInfo->Part4Port, 0x0F, 0x3F);
-			}
-
+			xgifb_reg_set(pVBInfo->Part2Port, 0x00, 0x1C);
+			xgifb_reg_set(pVBInfo->Part4Port,
+				      0x0D, XGI330_CRT2Data_4_D);
+			xgifb_reg_set(pVBInfo->Part4Port,
+				      0x0E, XGI330_CRT2Data_4_E);
+			xgifb_reg_set(pVBInfo->Part4Port,
+				      0x10, XGI330_CRT2Data_4_10);
+			xgifb_reg_set(pVBInfo->Part4Port, 0x0F, 0x3F);
 			XGI_LockCRT2(HwDeviceExtension, pVBInfo);
 		}
 	} /* != XG20 */
diff --git a/drivers/staging/xgifb/vb_setmode.c b/drivers/staging/xgifb/vb_setmode.c
index dfa5303379e..3adec3f1846 100644
--- a/drivers/staging/xgifb/vb_setmode.c
+++ b/drivers/staging/xgifb/vb_setmode.c
@@ -32,7 +32,7 @@ void InitTo330Pointer(unsigned char ChipType, struct vb_device_info *pVBInfo)
 	pVBInfo->VBInfo = 0;
 	pVBInfo->TVInfo = 0;
 
-	pVBInfo->SR15 = XGI340_SR13;
+	pVBInfo->SR18 = XGI340_SR18;
 	pVBInfo->CR40 = XGI340_cr41;
 
 	/* 310 customization related */
@@ -49,7 +49,7 @@ void InitTo330Pointer(unsigned char ChipType, struct vb_device_info *pVBInfo)
 		pVBInfo->MCLKData = XGI27New_MCLKData;
 		pVBInfo->CR40 = XGI27_cr41;
 		pVBInfo->XGINew_CR97 = 0xc1;
-		pVBInfo->SR15 = XG27_SR13;
+		pVBInfo->SR18 = XG27_SR18;
 
 		/*Z11m DDR*/
 		temp = xgifb_reg_get(pVBInfo->P3c4, 0x3B);
diff --git a/drivers/staging/xgifb/vb_struct.h b/drivers/staging/xgifb/vb_struct.h
index ae0c18b320a..c08ff5b2d6e 100644
--- a/drivers/staging/xgifb/vb_struct.h
+++ b/drivers/staging/xgifb/vb_struct.h
@@ -81,7 +81,6 @@ struct XGI_XG21CRT1Struct {
 struct XGI330_LCDCapStruct {
 	unsigned char	LCD_ID;
 	unsigned short	LCD_Capability;
-	unsigned char	LCD_SetFlag;
 	unsigned char	LCD_HSyncWidth;
 	unsigned char	LCD_VSyncWidth;
 	unsigned char	LCD_VCLK;
@@ -89,16 +88,6 @@ struct XGI330_LCDCapStruct {
 	unsigned char	LCDA_VCLKData2;
 	unsigned char	LCUCHAR_VCLKData1;
 	unsigned char	LCUCHAR_VCLKData2;
-	unsigned char	PSC_S1;
-	unsigned char	PSC_S2;
-	unsigned char	PSC_S3;
-	unsigned char	PSC_S4;
-	unsigned char	PSC_S5;
-	unsigned char	PWD_2B;
-	unsigned char	PWD_2C;
-	unsigned char	PWD_2D;
-	unsigned char	PWD_2E;
-	unsigned char	PWD_2F;
 	unsigned char	Spectrum_31;
 	unsigned char	Spectrum_32;
 	unsigned char	Spectrum_33;
@@ -145,7 +134,7 @@ struct vb_device_info {
 	unsigned short   LCDHRS, LCDVRS, LCDHDES, LCDVDES;
 
 	unsigned short   ModeType;
-	unsigned short   IF_DEF_LVDS, IF_DEF_TRUMPION, IF_DEF_DSTN;
+	unsigned short   IF_DEF_LVDS;
 	unsigned short   IF_DEF_CRT2Monitor;
 	unsigned short   IF_DEF_YPbPr;
 	unsigned short   IF_DEF_HiVision;
@@ -157,12 +146,11 @@ struct vb_device_info {
 
 	void __iomem *FBAddr;
 
-	unsigned char const (*SR15)[3];
+	unsigned char const *SR18;
 	unsigned char const (*CR40)[3];
 
 	struct SiS_MCLKData const *MCLKData;
 
-	unsigned char   *pXGINew_DRAMTypeDefinition;
 	unsigned char   XGINew_CR97;
 
 	struct XGI330_LCDCapStruct const *LCDCapList;
diff --git a/drivers/staging/xgifb/vb_table.h b/drivers/staging/xgifb/vb_table.h
index b4c05c80b93..7168eedbd96 100644
--- a/drivers/staging/xgifb/vb_table.h
+++ b/drivers/staging/xgifb/vb_table.h
@@ -18,18 +18,12 @@ const struct XGI_ECLKDataStruct XGI340_ECLKData[] = {
 	{0x7C, 0x08, 0x01, 200},
 };
 
-static const unsigned char XG27_SR13[4][3] = {
-	{0x35, 0x45, 0xb1}, /* SR13 */
-	{0x41, 0x51, 0x5c}, /* SR14 */
-	{0x32, 0x32, 0x42}, /* SR18 */
-	{0x03, 0x03, 0x03}  /* SR1B */
+static const unsigned char XG27_SR18[3] = {
+	0x32, 0x32, 0x42 /* SR18 */
 };
 
-static const unsigned char XGI340_SR13[4][3] = {
-	{0x35, 0x45, 0xb1}, /* SR13 */
-	{0x41, 0x51, 0x5c}, /* SR14 */
-	{0x31, 0x42, 0x42}, /* SR18 */
-	{0x03, 0x03, 0x03}  /* SR1B */
+static const unsigned char XGI340_SR18[3] = {
+	0x31, 0x42, 0x42 /* SR18 */
 };
 
 static const unsigned char XGI340_cr41[24][3] = {
@@ -1867,72 +1861,72 @@ static const struct XGI330_TVDataTablStruct XGI_TVDataTable[] = {
 /* Dual link only */
 static const struct XGI330_LCDCapStruct XGI_LCDDLCapList[] = {
 /* LCDCap1024x768 */
-	{Panel_1024x768, DefaultLCDCap, 0, 0x88, 0x06, VCLK65_315,
-	0x6C, 0xC3, 0x35, 0x62, 0x02, 0x14, 0x0A, 0x02, 0x00,
-	0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x28, 0x10},
+	{Panel_1024x768, DefaultLCDCap, 0x88, 0x06, VCLK65_315,
+	0x6C, 0xC3, 0x35, 0x62,
+	0x0A, 0xC0, 0x28, 0x10},
 /* LCDCap1280x1024 */
-	{Panel_1280x1024, XGI_LCDDualLink+DefaultLCDCap, StLCDBToA,
+	{Panel_1280x1024, XGI_LCDDualLink+DefaultLCDCap,
 	0x70, 0x03, VCLK108_2_315,
-	0x70, 0x44, 0xF8, 0x2F, 0x02, 0x14, 0x0A, 0x02, 0x00,
-	0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
+	0x70, 0x44, 0xF8, 0x2F,
+	0x0A, 0xC0, 0x30, 0x10},
 /* LCDCap1400x1050 */
-	{Panel_1400x1050, XGI_LCDDualLink+DefaultLCDCap, StLCDBToA,
+	{Panel_1400x1050, XGI_LCDDualLink+DefaultLCDCap,
 	0x70, 0x03, VCLK108_2_315,
-	 0x70, 0x44, 0xF8, 0x2F, 0x02, 0x14, 0x0A, 0x02, 0x00,
-	 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
+	 0x70, 0x44, 0xF8, 0x2F,
+	 0x0A, 0xC0, 0x30, 0x10},
 /* LCDCap1600x1200 */
-	{Panel_1600x1200, XGI_LCDDualLink+DefaultLCDCap, LCDToFull,
+	{Panel_1600x1200, XGI_LCDDualLink+DefaultLCDCap,
 	0xC0, 0x03, VCLK162,
-	 0x43, 0x22, 0x70, 0x24, 0x02, 0x14, 0x0A, 0x02, 0x00,
-	 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
+	 0x43, 0x22, 0x70, 0x24,
+	 0x0A, 0xC0, 0x30, 0x10},
 /* LCDCap1024x768x75 */
-	{Panel_1024x768x75, DefaultLCDCap, 0, 0x60, 0, VCLK78_75,
-	 0x2B, 0x61, 0x2B, 0x61, 0x02, 0x14, 0x0A, 0x02, 0x00,
-	 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x28, 0x10},
+	{Panel_1024x768x75, DefaultLCDCap, 0x60, 0, VCLK78_75,
+	 0x2B, 0x61, 0x2B, 0x61,
+	 0x0A, 0xC0, 0x28, 0x10},
 /* LCDCap1280x1024x75 */
-	{Panel_1280x1024x75, XGI_LCDDualLink+DefaultLCDCap, StLCDBToA,
+	{Panel_1280x1024x75, XGI_LCDDualLink+DefaultLCDCap,
 	 0x90, 0x03, VCLK135_5,
-	 0x54, 0x42, 0x4A, 0x61, 0x02, 0x14, 0x0A, 0x02, 0x00,
-	 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
+	 0x54, 0x42, 0x4A, 0x61,
+	 0x0A, 0xC0, 0x30, 0x10},
 /* LCDCapDefault */
-	{0xFF, DefaultLCDCap, 0, 0x88, 0x06, VCLK65_315,
-	0x6C, 0xC3, 0x35, 0x62, 0x02, 0x14, 0x0A, 0x02, 0x00,
-	0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x28, 0x10}
+	{0xFF, DefaultLCDCap, 0x88, 0x06, VCLK65_315,
+	0x6C, 0xC3, 0x35, 0x62,
+	0x0A, 0xC0, 0x28, 0x10}
 };
 
 static const struct XGI330_LCDCapStruct XGI_LCDCapList[] = {
 /* LCDCap1024x768 */
-	{Panel_1024x768, DefaultLCDCap, 0, 0x88, 0x06, VCLK65_315,
-	0x6C, 0xC3, 0x35, 0x62, 0x02, 0x14, 0x0A, 0x02, 0x00,
-	0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x28, 0x10},
+	{Panel_1024x768, DefaultLCDCap, 0x88, 0x06, VCLK65_315,
+	0x6C, 0xC3, 0x35, 0x62,
+	0x0A, 0xC0, 0x28, 0x10},
 /* LCDCap1280x1024 */
-	{Panel_1280x1024, DefaultLCDCap, StLCDBToA,
+	{Panel_1280x1024, DefaultLCDCap,
 	0x70, 0x03, VCLK108_2_315,
-	0x70, 0x44, 0xF8, 0x2F, 0x02, 0x14, 0x0A, 0x02, 0x00,
-	0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
+	0x70, 0x44, 0xF8, 0x2F,
+	0x0A, 0xC0, 0x30, 0x10},
 /* LCDCap1400x1050 */
-	{Panel_1400x1050, DefaultLCDCap, StLCDBToA,
+	{Panel_1400x1050, DefaultLCDCap,
 	 0x70, 0x03, VCLK108_2_315,
-	 0x70, 0x44, 0xF8, 0x2F, 0x02, 0x14, 0x0A, 0x02, 0x00,
-	 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
+	 0x70, 0x44, 0xF8, 0x2F,
+	 0x0A, 0xC0, 0x30, 0x10},
 /* LCDCap1600x1200 */
-	{Panel_1600x1200, DefaultLCDCap, LCDToFull,
+	{Panel_1600x1200, DefaultLCDCap,
 	 0xC0, 0x03, VCLK162,
-	 0x5A, 0x23, 0x5A, 0x23, 0x02, 0x14, 0x0A, 0x02, 0x00,
-	 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
+	 0x5A, 0x23, 0x5A, 0x23,
+	 0x0A, 0xC0, 0x30, 0x10},
 /* LCDCap1024x768x75 */
-	{Panel_1024x768x75, DefaultLCDCap, 0, 0x60, 0, VCLK78_75,
-	 0x2B, 0x61, 0x2B, 0x61, 0x02, 0x14, 0x0A, 0x02, 0x00,
-	 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x28, 0x10},
+	{Panel_1024x768x75, DefaultLCDCap, 0x60, 0, VCLK78_75,
+	 0x2B, 0x61, 0x2B, 0x61,
+	 0x0A, 0xC0, 0x28, 0x10},
 /* LCDCap1280x1024x75 */
-	{Panel_1280x1024x75, DefaultLCDCap, StLCDBToA,
+	{Panel_1280x1024x75, DefaultLCDCap,
 	 0x90, 0x03, VCLK135_5,
-	 0x54, 0x42, 0x4A, 0x61, 0x02, 0x14, 0x0A, 0x02, 0x00,
-	 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
+	 0x54, 0x42, 0x4A, 0x61,
+	 0x0A, 0xC0, 0x30, 0x10},
 /* LCDCapDefault */
-	{0xFF, DefaultLCDCap, 0, 0x88, 0x06, VCLK65_315,
-	0x6C, 0xC3, 0x35, 0x62, 0x02, 0x14, 0x0A, 0x02, 0x00,
-	0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x28, 0x10}
+	{0xFF, DefaultLCDCap, 0x88, 0x06, VCLK65_315,
+	0x6C, 0xC3, 0x35, 0x62,
+	0x0A, 0xC0, 0x28, 0x10}
 };
 
 const struct XGI_Ext2Struct XGI330_RefIndex[] = {
diff --git a/drivers/staging/zcache/Kconfig b/drivers/staging/zcache/Kconfig
index 73582705e8c..05e87a1e5d9 100644
--- a/drivers/staging/zcache/Kconfig
+++ b/drivers/staging/zcache/Kconfig
@@ -10,12 +10,20 @@ config ZCACHE
 	  memory to store clean page cache pages and swap in RAM,
 	  providing a noticeable reduction in disk I/O.
 
+config ZCACHE_DEBUG
+	bool "Enable debug statistics"
+	depends on DEBUG_FS && ZCACHE
+	default n
+	help
+	  This is used to provide an debugfs directory with counters of
+	  how zcache is doing. You probably want to set this to 'N'.
+
 config RAMSTER
 	bool "Cross-machine RAM capacity sharing, aka peer-to-peer tmem"
 	depends on CONFIGFS_FS=y && SYSFS=y && !HIGHMEM && ZCACHE=y
 	depends on NET
 	# must ensure struct page is 8-byte aligned
-	select HAVE_ALIGNED_STRUCT_PAGE if !64_BIT
+	select HAVE_ALIGNED_STRUCT_PAGE if !64BIT
 	default n
 	help
 	  RAMster allows RAM on other machines in a cluster to be utilized
@@ -25,6 +33,14 @@ config RAMSTER
 	  zcache2, compresses swap pages into local RAM, but then remotifies
 	  the compressed pages to another node in the RAMster cluster.
 
+config RAMSTER_DEBUG
+        bool "Enable ramster debug statistics"
+        depends on DEBUG_FS && RAMSTER
+        default n
+        help
+          This is used to provide an debugfs directory with counters of
+          how ramster is doing. You probably want to set this to 'N'.
+
 # Depends on not-yet-upstreamed mm patches to export end_swap_bio_write and
 # __add_to_swap_cache, and implement __swap_writepage (which is swap_writepage
 # without the frontswap call. When these are in-tree, the dependency on
diff --git a/drivers/staging/zcache/Makefile b/drivers/staging/zcache/Makefile
index 471104957da..845a5c2721b 100644
--- a/drivers/staging/zcache/Makefile
+++ b/drivers/staging/zcache/Makefile
@@ -1,4 +1,6 @@
 zcache-y	:=		zcache-main.o tmem.o zbud.o
+zcache-$(CONFIG_ZCACHE_DEBUG) += debug.o
+zcache-$(CONFIG_RAMSTER_DEBUG) += ramster/debug.o
 zcache-$(CONFIG_RAMSTER)	+=	ramster/ramster.o ramster/r2net.o
 zcache-$(CONFIG_RAMSTER)	+=	ramster/nodemanager.o ramster/tcp.o
 zcache-$(CONFIG_RAMSTER)	+=	ramster/heartbeat.o ramster/masklog.o
diff --git a/drivers/staging/zcache/TODO b/drivers/staging/zcache/TODO
index c1e26d4973d..d0c18fa9574 100644
--- a/drivers/staging/zcache/TODO
+++ b/drivers/staging/zcache/TODO
@@ -41,14 +41,10 @@ STATUS/OWNERSHIP
     for 3.9, see https://lkml.org/lkml/2013/2/6/437;
 7. PROTOTYPED as part of "new" zcache; in staging/zcache for 3.9;
     needs more review (plan to discuss at LSF/MM 2013)
-8. IN PROGRESS; owned by Konrad Wilk; v2 recently posted
-   http://lkml.org/lkml/2013/2/1/542
 9. IN PROGRESS; owned by Konrad Wilk; Mel Gorman provided
    great feedback in August 2012 (unfortunately of "old"
    zcache)
-10. Konrad posted series of fixes (that now need rebasing)
-    https://lkml.org/lkml/2013/2/1/566 
-11. NOT DONE; owned by Konrad Wilk
+11. NOT DONE; owned by Konrad Wilk and Bob Liu
 12. TBD (depends on quantity of feedback)
 13. PROPOSED; one suggestion proposed by Dan; needs more ideas/feedback
 14. TBD (depends on feedback)
@@ -65,5 +61,4 @@ ZCACHE FUTURE NEW FUNCTIONALITY
 
 A. Support zsmalloc as an alternative high-density allocator
     (See https://lkml.org/lkml/2013/1/23/511)
-B. Support zero-filled pages more efficiently
-C. Possibly support three zbuds per pageframe when space allows
+B. Possibly support three zbuds per pageframe when space allows
diff --git a/drivers/staging/zcache/debug.c b/drivers/staging/zcache/debug.c
new file mode 100644
index 00000000000..daa26919b88
--- /dev/null
+++ b/drivers/staging/zcache/debug.c
@@ -0,0 +1,107 @@
+#include <linux/atomic.h>
+#include "debug.h"
+
+#ifdef CONFIG_ZCACHE_DEBUG
+#include <linux/debugfs.h>
+
+ssize_t zcache_obj_count;
+ssize_t zcache_obj_count_max;
+ssize_t zcache_objnode_count;
+ssize_t zcache_objnode_count_max;
+u64 zcache_eph_zbytes;
+u64 zcache_eph_zbytes_max;
+u64 zcache_pers_zbytes_max;
+ssize_t zcache_eph_pageframes_max;
+ssize_t zcache_pers_pageframes_max;
+ssize_t zcache_pageframes_alloced;
+ssize_t zcache_pageframes_freed;
+ssize_t zcache_eph_zpages;
+ssize_t zcache_eph_zpages_max;
+ssize_t zcache_pers_zpages_max;
+ssize_t zcache_flush_total;
+ssize_t zcache_flush_found;
+ssize_t zcache_flobj_total;
+ssize_t zcache_flobj_found;
+ssize_t zcache_failed_eph_puts;
+ssize_t zcache_failed_pers_puts;
+ssize_t zcache_failed_getfreepages;
+ssize_t zcache_failed_alloc;
+ssize_t zcache_put_to_flush;
+ssize_t zcache_compress_poor;
+ssize_t zcache_mean_compress_poor;
+ssize_t zcache_eph_ate_tail;
+ssize_t zcache_eph_ate_tail_failed;
+ssize_t zcache_pers_ate_eph;
+ssize_t zcache_pers_ate_eph_failed;
+ssize_t zcache_evicted_eph_zpages;
+ssize_t zcache_evicted_eph_pageframes;
+ssize_t zcache_zero_filled_pages;
+ssize_t zcache_zero_filled_pages_max;
+
+#define ATTR(x)  { .name = #x, .val = &zcache_##x, }
+static struct debug_entry {
+	const char *name;
+	ssize_t *val;
+} attrs[] = {
+	ATTR(obj_count), ATTR(obj_count_max),
+	ATTR(objnode_count), ATTR(objnode_count_max),
+	ATTR(flush_total), ATTR(flush_found),
+	ATTR(flobj_total), ATTR(flobj_found),
+	ATTR(failed_eph_puts), ATTR(failed_pers_puts),
+	ATTR(failed_getfreepages), ATTR(failed_alloc),
+	ATTR(put_to_flush),
+	ATTR(compress_poor), ATTR(mean_compress_poor),
+	ATTR(eph_ate_tail), ATTR(eph_ate_tail_failed),
+	ATTR(pers_ate_eph), ATTR(pers_ate_eph_failed),
+	ATTR(evicted_eph_zpages), ATTR(evicted_eph_pageframes),
+	ATTR(eph_pageframes), ATTR(eph_pageframes_max),
+	ATTR(pers_pageframes), ATTR(pers_pageframes_max),
+	ATTR(eph_zpages), ATTR(eph_zpages_max),
+	ATTR(pers_zpages), ATTR(pers_zpages_max),
+	ATTR(last_active_file_pageframes),
+	ATTR(last_inactive_file_pageframes),
+	ATTR(last_active_anon_pageframes),
+	ATTR(last_inactive_anon_pageframes),
+	ATTR(eph_nonactive_puts_ignored),
+	ATTR(pers_nonactive_puts_ignored),
+	ATTR(zero_filled_pages),
+#ifdef CONFIG_ZCACHE_WRITEBACK
+	ATTR(outstanding_writeback_pages),
+	ATTR(writtenback_pages),
+#endif
+};
+#undef ATTR
+int zcache_debugfs_init(void)
+{
+	unsigned int i;
+	struct dentry *root = debugfs_create_dir("zcache", NULL);
+	if (root == NULL)
+		return -ENXIO;
+
+	for (i = 0; i < ARRAY_SIZE(attrs); i++)
+		if (!debugfs_create_size_t(attrs[i].name, S_IRUGO, root, attrs[i].val))
+			goto out;
+
+	debugfs_create_u64("eph_zbytes", S_IRUGO, root, &zcache_eph_zbytes);
+	debugfs_create_u64("eph_zbytes_max", S_IRUGO, root, &zcache_eph_zbytes_max);
+	debugfs_create_u64("pers_zbytes", S_IRUGO, root, &zcache_pers_zbytes);
+	debugfs_create_u64("pers_zbytes_max", S_IRUGO, root, &zcache_pers_zbytes_max);
+
+	return 0;
+out:
+	return -ENODEV;
+}
+
+/* developers can call this in case of ooms, e.g. to find memory leaks */
+void zcache_dump(void)
+{
+	unsigned int i;
+	for (i = 0; i < ARRAY_SIZE(attrs); i++)
+		pr_debug("zcache: %s=%zu\n", attrs[i].name, *attrs[i].val);
+
+	pr_debug("zcache: eph_zbytes=%llu\n", (unsigned long long)zcache_eph_zbytes);
+	pr_debug("zcache: eph_zbytes_max=%llu\n", (unsigned long long)zcache_eph_zbytes_max);
+	pr_debug("zcache: pers_zbytes=%llu\n", (unsigned long long)zcache_pers_zbytes);
+	pr_debug("zcache: pers_zbytes_max=%llu\n", (unsigned long long)zcache_pers_zbytes_max);
+}
+#endif
diff --git a/drivers/staging/zcache/debug.h b/drivers/staging/zcache/debug.h
new file mode 100644
index 00000000000..8088d28f2dc
--- /dev/null
+++ b/drivers/staging/zcache/debug.h
@@ -0,0 +1,305 @@
+#include <linux/bug.h>
+
+#ifdef CONFIG_ZCACHE_DEBUG
+
+/* we try to keep these statistics SMP-consistent */
+extern ssize_t zcache_obj_count;
+static atomic_t zcache_obj_atomic = ATOMIC_INIT(0);
+extern ssize_t zcache_obj_count_max;
+static inline void inc_zcache_obj_count(void)
+{
+	zcache_obj_count = atomic_inc_return(&zcache_obj_atomic);
+	if (zcache_obj_count > zcache_obj_count_max)
+		zcache_obj_count_max = zcache_obj_count;
+}
+static inline void dec_zcache_obj_count(void)
+{
+	zcache_obj_count = atomic_dec_return(&zcache_obj_atomic);
+	BUG_ON(zcache_obj_count < 0);
+};
+extern ssize_t zcache_objnode_count;
+static atomic_t zcache_objnode_atomic = ATOMIC_INIT(0);
+extern ssize_t zcache_objnode_count_max;
+static inline void inc_zcache_objnode_count(void)
+{
+	zcache_objnode_count = atomic_inc_return(&zcache_objnode_atomic);
+	if (zcache_objnode_count > zcache_objnode_count_max)
+		zcache_objnode_count_max = zcache_objnode_count;
+};
+static inline void dec_zcache_objnode_count(void)
+{
+	zcache_objnode_count = atomic_dec_return(&zcache_objnode_atomic);
+	BUG_ON(zcache_objnode_count < 0);
+};
+extern u64 zcache_eph_zbytes;
+static atomic_long_t zcache_eph_zbytes_atomic = ATOMIC_INIT(0);
+extern u64 zcache_eph_zbytes_max;
+static inline void inc_zcache_eph_zbytes(unsigned clen)
+{
+	zcache_eph_zbytes = atomic_long_add_return(clen, &zcache_eph_zbytes_atomic);
+	if (zcache_eph_zbytes > zcache_eph_zbytes_max)
+		zcache_eph_zbytes_max = zcache_eph_zbytes;
+};
+static inline void dec_zcache_eph_zbytes(unsigned zsize)
+{
+	zcache_eph_zbytes = atomic_long_sub_return(zsize, &zcache_eph_zbytes_atomic);
+};
+extern  u64 zcache_pers_zbytes;
+static atomic_long_t zcache_pers_zbytes_atomic = ATOMIC_INIT(0);
+extern u64 zcache_pers_zbytes_max;
+static inline void inc_zcache_pers_zbytes(unsigned clen)
+{
+	zcache_pers_zbytes = atomic_long_add_return(clen, &zcache_pers_zbytes_atomic);
+	if (zcache_pers_zbytes > zcache_pers_zbytes_max)
+		zcache_pers_zbytes_max = zcache_pers_zbytes;
+}
+static inline void dec_zcache_pers_zbytes(unsigned zsize)
+{
+	zcache_pers_zbytes = atomic_long_sub_return(zsize, &zcache_pers_zbytes_atomic);
+}
+extern ssize_t zcache_eph_pageframes;
+static atomic_t zcache_eph_pageframes_atomic = ATOMIC_INIT(0);
+extern ssize_t zcache_eph_pageframes_max;
+static inline void inc_zcache_eph_pageframes(void)
+{
+	zcache_eph_pageframes = atomic_inc_return(&zcache_eph_pageframes_atomic);
+	if (zcache_eph_pageframes > zcache_eph_pageframes_max)
+		zcache_eph_pageframes_max = zcache_eph_pageframes;
+};
+static inline void dec_zcache_eph_pageframes(void)
+{
+	zcache_eph_pageframes = atomic_dec_return(&zcache_eph_pageframes_atomic);
+};
+extern ssize_t zcache_pers_pageframes;
+static atomic_t zcache_pers_pageframes_atomic = ATOMIC_INIT(0);
+extern ssize_t zcache_pers_pageframes_max;
+static inline void inc_zcache_pers_pageframes(void)
+{
+	zcache_pers_pageframes = atomic_inc_return(&zcache_pers_pageframes_atomic);
+	if (zcache_pers_pageframes > zcache_pers_pageframes_max)
+		zcache_pers_pageframes_max = zcache_pers_pageframes;
+}
+static inline void dec_zcache_pers_pageframes(void)
+{
+	zcache_pers_pageframes = atomic_dec_return(&zcache_pers_pageframes_atomic);
+}
+extern ssize_t zcache_pageframes_alloced;
+static atomic_t zcache_pageframes_alloced_atomic = ATOMIC_INIT(0);
+static inline void inc_zcache_pageframes_alloced(void)
+{
+	zcache_pageframes_alloced = atomic_inc_return(&zcache_pageframes_alloced_atomic);
+};
+extern ssize_t zcache_pageframes_freed;
+static atomic_t zcache_pageframes_freed_atomic = ATOMIC_INIT(0);
+static inline void inc_zcache_pageframes_freed(void)
+{
+	zcache_pageframes_freed = atomic_inc_return(&zcache_pageframes_freed_atomic);
+}
+extern ssize_t zcache_eph_zpages;
+static atomic_t zcache_eph_zpages_atomic = ATOMIC_INIT(0);
+extern ssize_t zcache_eph_zpages_max;
+static inline void inc_zcache_eph_zpages(void)
+{
+	zcache_eph_zpages = atomic_inc_return(&zcache_eph_zpages_atomic);
+	if (zcache_eph_zpages > zcache_eph_zpages_max)
+		zcache_eph_zpages_max = zcache_eph_zpages;
+}
+static inline void dec_zcache_eph_zpages(unsigned zpages)
+{
+	zcache_eph_zpages = atomic_sub_return(zpages, &zcache_eph_zpages_atomic);
+}
+extern ssize_t zcache_pers_zpages;
+static atomic_t zcache_pers_zpages_atomic = ATOMIC_INIT(0);
+extern ssize_t zcache_pers_zpages_max;
+static inline void inc_zcache_pers_zpages(void)
+{
+	zcache_pers_zpages = atomic_inc_return(&zcache_pers_zpages_atomic);
+	if (zcache_pers_zpages > zcache_pers_zpages_max)
+		zcache_pers_zpages_max = zcache_pers_zpages;
+}
+static inline void dec_zcache_pers_zpages(unsigned zpages)
+{
+	zcache_pers_zpages = atomic_sub_return(zpages, &zcache_pers_zpages_atomic);
+}
+
+extern ssize_t zcache_zero_filled_pages;
+static atomic_t zcache_zero_filled_pages_atomic = ATOMIC_INIT(0);
+extern ssize_t zcache_zero_filled_pages_max;
+static inline void inc_zcache_zero_filled_pages(void)
+{
+	zcache_zero_filled_pages = atomic_inc_return(
+					&zcache_zero_filled_pages_atomic);
+	if (zcache_zero_filled_pages > zcache_zero_filled_pages_max)
+		zcache_zero_filled_pages_max = zcache_zero_filled_pages;
+}
+static inline void dec_zcache_zero_filled_pages(void)
+{
+	zcache_zero_filled_pages = atomic_dec_return(
+					&zcache_zero_filled_pages_atomic);
+}
+static inline unsigned long curr_pageframes_count(void)
+{
+	return zcache_pageframes_alloced -
+		atomic_read(&zcache_pageframes_freed_atomic) -
+		atomic_read(&zcache_eph_pageframes_atomic) -
+		atomic_read(&zcache_pers_pageframes_atomic);
+};
+/* but for the rest of these, counting races are ok */
+extern ssize_t zcache_flush_total;
+extern ssize_t zcache_flush_found;
+extern ssize_t zcache_flobj_total;
+extern ssize_t zcache_flobj_found;
+extern ssize_t zcache_failed_eph_puts;
+extern ssize_t zcache_failed_pers_puts;
+extern ssize_t zcache_failed_getfreepages;
+extern ssize_t zcache_failed_alloc;
+extern ssize_t zcache_put_to_flush;
+extern ssize_t zcache_compress_poor;
+extern ssize_t zcache_mean_compress_poor;
+extern ssize_t zcache_eph_ate_tail;
+extern ssize_t zcache_eph_ate_tail_failed;
+extern ssize_t zcache_pers_ate_eph;
+extern ssize_t zcache_pers_ate_eph_failed;
+extern ssize_t zcache_evicted_eph_zpages;
+extern ssize_t zcache_evicted_eph_pageframes;
+
+extern ssize_t zcache_last_active_file_pageframes;
+extern ssize_t zcache_last_inactive_file_pageframes;
+extern ssize_t zcache_last_active_anon_pageframes;
+extern ssize_t zcache_last_inactive_anon_pageframes;
+static ssize_t zcache_eph_nonactive_puts_ignored;
+static ssize_t zcache_pers_nonactive_puts_ignored;
+#ifdef CONFIG_ZCACHE_WRITEBACK
+extern ssize_t zcache_writtenback_pages;
+extern ssize_t zcache_outstanding_writeback_pages;
+#endif
+
+static inline void inc_zcache_flush_total(void)
+{
+	zcache_flush_total++;
+};
+static inline void inc_zcache_flush_found(void)
+{
+	zcache_flush_found++;
+};
+static inline void inc_zcache_flobj_total(void)
+{
+	zcache_flobj_total++;
+};
+static inline void inc_zcache_flobj_found(void)
+{
+	zcache_flobj_found++;
+};
+static inline void inc_zcache_failed_eph_puts(void)
+{
+	zcache_failed_eph_puts++;
+};
+static inline void inc_zcache_failed_pers_puts(void)
+{
+	zcache_failed_pers_puts++;
+};
+static inline void inc_zcache_failed_getfreepages(void)
+{
+	zcache_failed_getfreepages++;
+};
+static inline void inc_zcache_failed_alloc(void)
+{
+	zcache_failed_alloc++;
+};
+static inline void inc_zcache_put_to_flush(void)
+{
+	zcache_put_to_flush++;
+};
+static inline void inc_zcache_compress_poor(void)
+{
+	zcache_compress_poor++;
+};
+static inline void inc_zcache_mean_compress_poor(void)
+{
+	zcache_mean_compress_poor++;
+};
+static inline void inc_zcache_eph_ate_tail(void)
+{
+	zcache_eph_ate_tail++;
+};
+static inline void inc_zcache_eph_ate_tail_failed(void)
+{
+	zcache_eph_ate_tail_failed++;
+};
+static inline void inc_zcache_pers_ate_eph(void)
+{
+	zcache_pers_ate_eph++;
+};
+static inline void inc_zcache_pers_ate_eph_failed(void)
+{
+	zcache_pers_ate_eph_failed++;
+};
+static inline void inc_zcache_evicted_eph_zpages(unsigned zpages)
+{
+	zcache_evicted_eph_zpages += zpages;
+};
+static inline void inc_zcache_evicted_eph_pageframes(void)
+{
+	zcache_evicted_eph_pageframes++;
+};
+
+static inline void inc_zcache_eph_nonactive_puts_ignored(void)
+{
+	zcache_eph_nonactive_puts_ignored++;
+};
+static inline void inc_zcache_pers_nonactive_puts_ignored(void)
+{
+	zcache_pers_nonactive_puts_ignored++;
+};
+
+int zcache_debugfs_init(void);
+#else
+static inline void inc_zcache_obj_count(void) { };
+static inline void dec_zcache_obj_count(void) { };
+static inline void inc_zcache_objnode_count(void) { };
+static inline void dec_zcache_objnode_count(void) { };
+static inline void inc_zcache_eph_zbytes(unsigned clen) { };
+static inline void dec_zcache_eph_zbytes(unsigned zsize) { };
+static inline void inc_zcache_pers_zbytes(unsigned clen) { };
+static inline void dec_zcache_pers_zbytes(unsigned zsize) { };
+static inline void inc_zcache_eph_pageframes(void) { };
+static inline void dec_zcache_eph_pageframes(void) { };
+static inline void inc_zcache_pers_pageframes(void) { };
+static inline void dec_zcache_pers_pageframes(void) { };
+static inline void inc_zcache_pageframes_alloced(void) { };
+static inline void inc_zcache_pageframes_freed(void) { };
+static inline void inc_zcache_eph_zpages(void) { };
+static inline void dec_zcache_eph_zpages(unsigned zpages) { };
+static inline void inc_zcache_pers_zpages(void) { };
+static inline void dec_zcache_pers_zpages(unsigned zpages) { };
+static inline void inc_zcache_zero_filled_pages(void) { };
+static inline void dec_zcache_zero_filled_pages(void) { };
+static inline unsigned long curr_pageframes_count(void)
+{
+	return 0;
+};
+static inline int zcache_debugfs_init(void)
+{
+	return 0;
+};
+static inline void inc_zcache_flush_total(void) { };
+static inline void inc_zcache_flush_found(void) { };
+static inline void inc_zcache_flobj_total(void) { };
+static inline void inc_zcache_flobj_found(void) { };
+static inline void inc_zcache_failed_eph_puts(void) { };
+static inline void inc_zcache_failed_pers_puts(void) { };
+static inline void inc_zcache_failed_getfreepages(void) { };
+static inline void inc_zcache_failed_alloc(void) { };
+static inline void inc_zcache_put_to_flush(void) { };
+static inline void inc_zcache_compress_poor(void) { };
+static inline void inc_zcache_mean_compress_poor(void) { };
+static inline void inc_zcache_eph_ate_tail(void) { };
+static inline void inc_zcache_eph_ate_tail_failed(void) { };
+static inline void inc_zcache_pers_ate_eph(void) { };
+static inline void inc_zcache_pers_ate_eph_failed(void) { };
+static inline void inc_zcache_evicted_eph_zpages(unsigned zpages) { };
+static inline void inc_zcache_evicted_eph_pageframes(void) { };
+
+static inline void inc_zcache_eph_nonactive_puts_ignored(void) { };
+static inline void inc_zcache_pers_nonactive_puts_ignored(void) { };
+#endif
diff --git a/drivers/staging/zcache/ramster/debug.c b/drivers/staging/zcache/ramster/debug.c
new file mode 100644
index 00000000000..bf34133cc63
--- /dev/null
+++ b/drivers/staging/zcache/ramster/debug.c
@@ -0,0 +1,66 @@
+#include <linux/atomic.h>
+#include "debug.h"
+
+#ifdef CONFIG_DEBUG_FS
+#include <linux/debugfs.h>
+
+ssize_t ramster_eph_pages_remoted;
+ssize_t ramster_pers_pages_remoted;
+ssize_t ramster_eph_pages_remote_failed;
+ssize_t ramster_pers_pages_remote_failed;
+ssize_t ramster_remote_eph_pages_succ_get;
+ssize_t ramster_remote_pers_pages_succ_get;
+ssize_t ramster_remote_eph_pages_unsucc_get;
+ssize_t ramster_remote_pers_pages_unsucc_get;
+ssize_t ramster_pers_pages_remote_nomem;
+ssize_t ramster_remote_objects_flushed;
+ssize_t ramster_remote_object_flushes_failed;
+ssize_t ramster_remote_pages_flushed;
+ssize_t ramster_remote_page_flushes_failed;
+
+#define ATTR(x)  { .name = #x, .val = &ramster_##x, }
+static struct debug_entry {
+	const char *name;
+	ssize_t *val;
+} attrs[] = {
+	ATTR(eph_pages_remoted),
+	ATTR(pers_pages_remoted),
+	ATTR(eph_pages_remote_failed),
+	ATTR(pers_pages_remote_failed),
+	ATTR(remote_eph_pages_succ_get),
+	ATTR(remote_pers_pages_succ_get),
+	ATTR(remote_eph_pages_unsucc_get),
+	ATTR(remote_pers_pages_unsucc_get),
+	ATTR(pers_pages_remote_nomem),
+	ATTR(remote_objects_flushed),
+	ATTR(remote_pages_flushed),
+	ATTR(remote_object_flushes_failed),
+	ATTR(remote_page_flushes_failed),
+	ATTR(foreign_eph_pages),
+	ATTR(foreign_eph_pages_max),
+	ATTR(foreign_pers_pages),
+	ATTR(foreign_pers_pages_max),
+};
+#undef ATTR
+
+int __init ramster_debugfs_init(void)
+{
+	int i;
+	struct dentry *root = debugfs_create_dir("ramster", NULL);
+	if (root == NULL)
+		return -ENXIO;
+
+	for (i = 0; i < ARRAY_SIZE(attrs); i++)
+		if (!debugfs_create_size_t(attrs[i].name,
+				S_IRUGO, root, attrs[i].val))
+			goto out;
+	return 0;
+out:
+	return -ENODEV;
+}
+#else
+static inline int ramster_debugfs_init(void)
+{
+	return 0;
+}
+#endif
diff --git a/drivers/staging/zcache/ramster/debug.h b/drivers/staging/zcache/ramster/debug.h
new file mode 100644
index 00000000000..5ffab50807d
--- /dev/null
+++ b/drivers/staging/zcache/ramster/debug.h
@@ -0,0 +1,145 @@
+#include <linux/bug.h>
+
+#ifdef CONFIG_RAMSTER_DEBUG
+
+extern long ramster_flnodes;
+static atomic_t ramster_flnodes_atomic = ATOMIC_INIT(0);
+static unsigned long ramster_flnodes_max;
+static inline void inc_ramster_flnodes(void)
+{
+	ramster_flnodes = atomic_inc_return(&ramster_flnodes_atomic);
+	if (ramster_flnodes > ramster_flnodes_max)
+		ramster_flnodes_max = ramster_flnodes;
+}
+static inline void dec_ramster_flnodes(void)
+{
+	ramster_flnodes = atomic_dec_return(&ramster_flnodes_atomic);
+}
+extern ssize_t ramster_foreign_eph_pages;
+static atomic_t ramster_foreign_eph_pages_atomic = ATOMIC_INIT(0);
+static ssize_t ramster_foreign_eph_pages_max;
+static inline void inc_ramster_foreign_eph_pages(void)
+{
+	ramster_foreign_eph_pages = atomic_inc_return(
+		&ramster_foreign_eph_pages_atomic);
+	if (ramster_foreign_eph_pages > ramster_foreign_eph_pages_max)
+		ramster_foreign_eph_pages_max = ramster_foreign_eph_pages;
+}
+static inline void dec_ramster_foreign_eph_pages(void)
+{
+	ramster_foreign_eph_pages = atomic_dec_return(
+		&ramster_foreign_eph_pages_atomic);
+}
+extern ssize_t ramster_foreign_pers_pages;
+static atomic_t ramster_foreign_pers_pages_atomic = ATOMIC_INIT(0);
+static ssize_t ramster_foreign_pers_pages_max;
+static inline void inc_ramster_foreign_pers_pages(void)
+{
+	ramster_foreign_pers_pages = atomic_inc_return(
+		&ramster_foreign_pers_pages_atomic);
+	if (ramster_foreign_pers_pages > ramster_foreign_pers_pages_max)
+		ramster_foreign_pers_pages_max = ramster_foreign_pers_pages;
+}
+static inline void dec_ramster_foreign_pers_pages(void)
+{
+	ramster_foreign_pers_pages = atomic_dec_return(
+		&ramster_foreign_pers_pages_atomic);
+}
+
+extern ssize_t ramster_eph_pages_remoted;
+extern ssize_t ramster_pers_pages_remoted;
+extern ssize_t ramster_eph_pages_remote_failed;
+extern ssize_t ramster_pers_pages_remote_failed;
+extern ssize_t ramster_remote_eph_pages_succ_get;
+extern ssize_t ramster_remote_pers_pages_succ_get;
+extern ssize_t ramster_remote_eph_pages_unsucc_get;
+extern ssize_t ramster_remote_pers_pages_unsucc_get;
+extern ssize_t ramster_pers_pages_remote_nomem;
+extern ssize_t ramster_remote_objects_flushed;
+extern ssize_t ramster_remote_object_flushes_failed;
+extern ssize_t ramster_remote_pages_flushed;
+extern ssize_t ramster_remote_page_flushes_failed;
+
+int ramster_debugfs_init(void);
+
+static inline void inc_ramster_eph_pages_remoted(void)
+{
+	ramster_eph_pages_remoted++;
+};
+static inline void inc_ramster_pers_pages_remoted(void)
+{
+	ramster_pers_pages_remoted++;
+};
+static inline void inc_ramster_eph_pages_remote_failed(void)
+{
+	ramster_eph_pages_remote_failed++;
+};
+static inline void inc_ramster_pers_pages_remote_failed(void)
+{
+	ramster_pers_pages_remote_failed++;
+};
+static inline void inc_ramster_remote_eph_pages_succ_get(void)
+{
+	ramster_remote_eph_pages_succ_get++;
+};
+static inline void inc_ramster_remote_pers_pages_succ_get(void)
+{
+	ramster_remote_pers_pages_succ_get++;
+};
+static inline void inc_ramster_remote_eph_pages_unsucc_get(void)
+{
+	ramster_remote_eph_pages_unsucc_get++;
+};
+static inline void inc_ramster_remote_pers_pages_unsucc_get(void)
+{
+	ramster_remote_pers_pages_unsucc_get++;
+};
+static inline void inc_ramster_pers_pages_remote_nomem(void)
+{
+	ramster_pers_pages_remote_nomem++;
+};
+static inline void inc_ramster_remote_objects_flushed(void)
+{
+	ramster_remote_objects_flushed++;
+};
+static inline void inc_ramster_remote_object_flushes_failed(void)
+{
+	ramster_remote_object_flushes_failed++;
+};
+static inline void inc_ramster_remote_pages_flushed(void)
+{
+	ramster_remote_pages_flushed++;
+};
+static inline void inc_ramster_remote_page_flushes_failed(void)
+{
+	ramster_remote_page_flushes_failed++;
+};
+
+#else
+
+static inline void inc_ramster_flnodes(void) { };
+static inline void dec_ramster_flnodes(void) { };
+static inline void inc_ramster_foreign_eph_pages(void) { };
+static inline void dec_ramster_foreign_eph_pages(void) { };
+static inline void inc_ramster_foreign_pers_pages(void) { };
+static inline void dec_ramster_foreign_pers_pages(void) { };
+
+static inline void inc_ramster_eph_pages_remoted(void) { };
+static inline void inc_ramster_pers_pages_remoted(void) { };
+static inline void inc_ramster_eph_pages_remote_failed(void) { };
+static inline void inc_ramster_pers_pages_remote_failed(void) { };
+static inline void inc_ramster_remote_eph_pages_succ_get(void) { };
+static inline void inc_ramster_remote_pers_pages_succ_get(void) { };
+static inline void inc_ramster_remote_eph_pages_unsucc_get(void) { };
+static inline void inc_ramster_remote_pers_pages_unsucc_get(void) { };
+static inline void inc_ramster_pers_pages_remote_nomem(void) { };
+static inline void inc_ramster_remote_objects_flushed(void) { };
+static inline void inc_ramster_remote_object_flushes_failed(void) { };
+static inline void inc_ramster_remote_pages_flushed(void) { };
+static inline void inc_ramster_remote_page_flushes_failed(void) { };
+
+static inline int ramster_debugfs_init(void)
+{
+	return 0;
+}
+#endif
diff --git a/drivers/staging/zcache/ramster/ramster.c b/drivers/staging/zcache/ramster/ramster.c
index bf96a1cbf7c..87816279ce3 100644
--- a/drivers/staging/zcache/ramster/ramster.c
+++ b/drivers/staging/zcache/ramster/ramster.c
@@ -42,6 +42,7 @@
 #include "ramster.h"
 #include "ramster_nodemanager.h"
 #include "tcp.h"
+#include "debug.h"
 
 #define RAMSTER_TESTING
 
@@ -63,79 +64,12 @@ static atomic_t ramster_remote_pers_pages = ATOMIC_INIT(0);
 static bool ramster_nodes_manual_up[MANUAL_NODES] __read_mostly;
 static int ramster_remote_target_nodenum __read_mostly = -1;
 
-/* these counters are made available via debugfs */
-static long ramster_flnodes;
-static atomic_t ramster_flnodes_atomic = ATOMIC_INIT(0);
-static unsigned long ramster_flnodes_max;
-static ssize_t ramster_foreign_eph_pages;
-static atomic_t ramster_foreign_eph_pages_atomic = ATOMIC_INIT(0);
-static ssize_t ramster_foreign_eph_pages_max;
-static ssize_t ramster_foreign_pers_pages;
-static atomic_t ramster_foreign_pers_pages_atomic = ATOMIC_INIT(0);
-static ssize_t ramster_foreign_pers_pages_max;
-static ssize_t ramster_eph_pages_remoted;
-static ssize_t ramster_pers_pages_remoted;
-static ssize_t ramster_eph_pages_remote_failed;
-static ssize_t ramster_pers_pages_remote_failed;
-static ssize_t ramster_remote_eph_pages_succ_get;
-static ssize_t ramster_remote_pers_pages_succ_get;
-static ssize_t ramster_remote_eph_pages_unsucc_get;
-static ssize_t ramster_remote_pers_pages_unsucc_get;
-static ssize_t ramster_pers_pages_remote_nomem;
-static ssize_t ramster_remote_objects_flushed;
-static ssize_t ramster_remote_object_flushes_failed;
-static ssize_t ramster_remote_pages_flushed;
-static ssize_t ramster_remote_page_flushes_failed;
+/* Used by this code. */
+long ramster_flnodes;
+ssize_t ramster_foreign_eph_pages;
+ssize_t ramster_foreign_pers_pages;
 /* FIXME frontswap selfshrinking knobs in debugfs? */
 
-#ifdef CONFIG_DEBUG_FS
-#include <linux/debugfs.h>
-#define	zdfs	debugfs_create_size_t
-#define	zdfs64	debugfs_create_u64
-static int __init ramster_debugfs_init(void)
-{
-	struct dentry *root = debugfs_create_dir("ramster", NULL);
-	if (root == NULL)
-		return -ENXIO;
-
-	zdfs("eph_pages_remoted", S_IRUGO, root, &ramster_eph_pages_remoted);
-	zdfs("pers_pages_remoted", S_IRUGO, root, &ramster_pers_pages_remoted);
-	zdfs("eph_pages_remote_failed", S_IRUGO, root,
-			&ramster_eph_pages_remote_failed);
-	zdfs("pers_pages_remote_failed", S_IRUGO, root,
-			&ramster_pers_pages_remote_failed);
-	zdfs("remote_eph_pages_succ_get", S_IRUGO, root,
-			&ramster_remote_eph_pages_succ_get);
-	zdfs("remote_pers_pages_succ_get", S_IRUGO, root,
-			&ramster_remote_pers_pages_succ_get);
-	zdfs("remote_eph_pages_unsucc_get", S_IRUGO, root,
-			&ramster_remote_eph_pages_unsucc_get);
-	zdfs("remote_pers_pages_unsucc_get", S_IRUGO, root,
-			&ramster_remote_pers_pages_unsucc_get);
-	zdfs("pers_pages_remote_nomem", S_IRUGO, root,
-			&ramster_pers_pages_remote_nomem);
-	zdfs("remote_objects_flushed", S_IRUGO, root,
-			&ramster_remote_objects_flushed);
-	zdfs("remote_pages_flushed", S_IRUGO, root,
-			&ramster_remote_pages_flushed);
-	zdfs("remote_object_flushes_failed", S_IRUGO, root,
-			&ramster_remote_object_flushes_failed);
-	zdfs("remote_page_flushes_failed", S_IRUGO, root,
-			&ramster_remote_page_flushes_failed);
-	zdfs("foreign_eph_pages", S_IRUGO, root,
-			&ramster_foreign_eph_pages);
-	zdfs("foreign_eph_pages_max", S_IRUGO, root,
-			&ramster_foreign_eph_pages_max);
-	zdfs("foreign_pers_pages", S_IRUGO, root,
-			&ramster_foreign_pers_pages);
-	zdfs("foreign_pers_pages_max", S_IRUGO, root,
-			&ramster_foreign_pers_pages_max);
-	return 0;
-}
-#undef	zdebugfs
-#undef	zdfs64
-#endif
-
 static LIST_HEAD(ramster_rem_op_list);
 static DEFINE_SPINLOCK(ramster_rem_op_list_lock);
 static DEFINE_PER_CPU(struct ramster_preload, ramster_preloads);
@@ -154,9 +88,7 @@ static struct flushlist_node *ramster_flnode_alloc(struct tmem_pool *pool)
 	flnode = kp->flnode;
 	BUG_ON(flnode == NULL);
 	kp->flnode = NULL;
-	ramster_flnodes = atomic_inc_return(&ramster_flnodes_atomic);
-	if (ramster_flnodes > ramster_flnodes_max)
-		ramster_flnodes_max = ramster_flnodes;
+	inc_ramster_flnodes();
 	return flnode;
 }
 
@@ -165,10 +97,8 @@ static struct flushlist_node *ramster_flnode_alloc(struct tmem_pool *pool)
 static void ramster_flnode_free(struct flushlist_node *flnode,
 				struct tmem_pool *pool)
 {
-	int flnodes;
-
-	flnodes = atomic_dec_return(&ramster_flnodes_atomic);
-	BUG_ON(flnodes < 0);
+	dec_ramster_flnodes();
+	BUG_ON(ramster_flnodes < 0);
 	kmem_cache_free(ramster_flnode_cache, flnode);
 }
 
@@ -226,9 +156,9 @@ int ramster_localify(int pool_id, struct tmem_oid *oidp, uint32_t index,
 		pr_err("UNTESTED pampd==NULL in ramster_localify\n");
 #endif
 		if (eph)
-			ramster_remote_eph_pages_unsucc_get++;
+			inc_ramster_remote_eph_pages_unsucc_get();
 		else
-			ramster_remote_pers_pages_unsucc_get++;
+			inc_ramster_remote_pers_pages_unsucc_get();
 		obj = NULL;
 		goto finish;
 	} else if (unlikely(!pampd_is_remote(pampd))) {
@@ -237,9 +167,9 @@ int ramster_localify(int pool_id, struct tmem_oid *oidp, uint32_t index,
 		pr_err("UNTESTED dup while waiting in ramster_localify\n");
 #endif
 		if (eph)
-			ramster_remote_eph_pages_unsucc_get++;
+			inc_ramster_remote_eph_pages_unsucc_get();
 		else
-			ramster_remote_pers_pages_unsucc_get++;
+			inc_ramster_remote_pers_pages_unsucc_get();
 		obj = NULL;
 		pampd = NULL;
 		ret = -EEXIST;
@@ -248,7 +178,7 @@ int ramster_localify(int pool_id, struct tmem_oid *oidp, uint32_t index,
 		/* no remote data, delete the local is_remote pampd */
 		pampd = NULL;
 		if (eph)
-			ramster_remote_eph_pages_unsucc_get++;
+			inc_ramster_remote_eph_pages_unsucc_get();
 		else
 			BUG();
 		delete = true;
@@ -279,9 +209,9 @@ int ramster_localify(int pool_id, struct tmem_oid *oidp, uint32_t index,
 	BUG_ON(extra == NULL);
 	zcache_decompress_to_page(data, size, (struct page *)extra);
 	if (eph)
-		ramster_remote_eph_pages_succ_get++;
+		inc_ramster_remote_eph_pages_succ_get();
 	else
-		ramster_remote_pers_pages_succ_get++;
+		inc_ramster_remote_pers_pages_succ_get();
 	ret = 0;
 finish:
 	tmem_localify_finish(obj, index, pampd, saved_hb, delete);
@@ -366,7 +296,7 @@ void *ramster_pampd_repatriate_preload(void *pampd, struct tmem_pool *pool,
 		c = atomic_dec_return(&ramster_remote_pers_pages);
 		WARN_ON_ONCE(c < 0);
 	} else {
-		ramster_pers_pages_remote_nomem++;
+		inc_ramster_pers_pages_remote_nomem();
 	}
 	local_irq_restore(flags);
 out:
@@ -461,32 +391,21 @@ void *ramster_pampd_free(void *pampd, struct tmem_pool *pool,
 
 void ramster_count_foreign_pages(bool eph, int count)
 {
-	int c;
-
 	BUG_ON(count != 1 && count != -1);
 	if (eph) {
 		if (count > 0) {
-			c = atomic_inc_return(
-					&ramster_foreign_eph_pages_atomic);
-			if (c > ramster_foreign_eph_pages_max)
-				ramster_foreign_eph_pages_max = c;
+			inc_ramster_foreign_eph_pages();
 		} else {
-			c = atomic_dec_return(&ramster_foreign_eph_pages_atomic);
-			WARN_ON_ONCE(c < 0);
+			dec_ramster_foreign_eph_pages();
+			WARN_ON_ONCE(ramster_foreign_eph_pages < 0);
 		}
-		ramster_foreign_eph_pages = c;
 	} else {
 		if (count > 0) {
-			c = atomic_inc_return(
-					&ramster_foreign_pers_pages_atomic);
-			if (c > ramster_foreign_pers_pages_max)
-				ramster_foreign_pers_pages_max = c;
+			inc_ramster_foreign_pers_pages();
 		} else {
-			c = atomic_dec_return(
-					&ramster_foreign_pers_pages_atomic);
-			WARN_ON_ONCE(c < 0);
+			dec_ramster_foreign_pers_pages();
+			WARN_ON_ONCE(ramster_foreign_pers_pages < 0);
 		}
-		ramster_foreign_pers_pages = c;
 	}
 }
 
@@ -516,9 +435,9 @@ static void ramster_remote_flush_page(struct flushlist_node *flnode)
 	remotenode = flnode->xh.client_id;
 	ret = r2net_remote_flush(xh, remotenode);
 	if (ret >= 0)
-		ramster_remote_pages_flushed++;
+		inc_ramster_remote_pages_flushed();
 	else
-		ramster_remote_page_flushes_failed++;
+		inc_ramster_remote_page_flushes_failed();
 	preempt_enable_no_resched();
 	ramster_flnode_free(flnode, NULL);
 }
@@ -533,9 +452,9 @@ static void ramster_remote_flush_object(struct flushlist_node *flnode)
 	remotenode = flnode->xh.client_id;
 	ret = r2net_remote_flush_object(xh, remotenode);
 	if (ret >= 0)
-		ramster_remote_objects_flushed++;
+		inc_ramster_remote_objects_flushed();
 	else
-		ramster_remote_object_flushes_failed++;
+		inc_ramster_remote_object_flushes_failed();
 	preempt_enable_no_resched();
 	ramster_flnode_free(flnode, NULL);
 }
@@ -586,18 +505,18 @@ int ramster_remotify_pageframe(bool eph)
 		 * But count them so we know if it becomes a problem.
 		 */
 			if (eph)
-				ramster_eph_pages_remote_failed++;
+				inc_ramster_eph_pages_remote_failed();
 			else
-				ramster_pers_pages_remote_failed++;
+				inc_ramster_pers_pages_remote_failed();
 			break;
 		} else {
 			if (!eph)
 				atomic_inc(&ramster_remote_pers_pages);
 		}
 		if (eph)
-			ramster_eph_pages_remoted++;
+			inc_ramster_eph_pages_remoted();
 		else
-			ramster_pers_pages_remoted++;
+			inc_ramster_pers_pages_remoted();
 		/*
 		 * data was successfully remoted so change the local version to
 		 * point to the remote node where it landed
diff --git a/drivers/staging/zcache/zbud.c b/drivers/staging/zcache/zbud.c
index fdff5c6a023..6cda4ed9ed3 100644
--- a/drivers/staging/zcache/zbud.c
+++ b/drivers/staging/zcache/zbud.c
@@ -342,6 +342,11 @@ static int zbud_debugfs_init(void)
 }
 #undef	zdfs
 #undef	zdfs64
+#else
+static inline int zbud_debugfs_init(void)
+{
+	return 0;
+}
 #endif
 
 /* protects the buddied list and all unbuddied lists */
@@ -1051,9 +1056,7 @@ void zbud_init(void)
 {
 	int i;
 
-#ifdef CONFIG_DEBUG_FS
 	zbud_debugfs_init();
-#endif
 	BUG_ON((sizeof(struct tmem_handle) * 2 > CHUNK_SIZE));
 	BUG_ON(sizeof(struct zbudpage) > sizeof(struct page));
 	for (i = 0; i < NCHUNKS; i++) {
diff --git a/drivers/staging/zcache/zcache-main.c b/drivers/staging/zcache/zcache-main.c
index 328898ea76c..e23d814b539 100644
--- a/drivers/staging/zcache/zcache-main.c
+++ b/drivers/staging/zcache/zcache-main.c
@@ -19,6 +19,7 @@
 #include <linux/slab.h>
 #include <linux/spinlock.h>
 #include <linux/types.h>
+#include <linux/string.h>
 #include <linux/atomic.h>
 #include <linux/math64.h>
 #include <linux/crypto.h>
@@ -33,10 +34,11 @@
 #include "zcache.h"
 #include "zbud.h"
 #include "ramster.h"
+#include "debug.h"
 #ifdef CONFIG_RAMSTER
-static int ramster_enabled;
+static bool ramster_enabled __read_mostly;
 #else
-#define ramster_enabled 0
+#define ramster_enabled false
 #endif
 
 #ifndef __PG_WAS_ACTIVE
@@ -59,21 +61,25 @@ static inline void frontswap_tmem_exclusive_gets(bool b)
 }
 #endif
 
+/*
+ * mark pampd to special value in order that later
+ * retrieve will identify zero-filled pages
+ */
+#define ZERO_FILLED 0x2
+
 /* enable (or fix code) when Seth's patches are accepted upstream */
 #define zcache_writeback_enabled 0
 
-static int zcache_enabled __read_mostly;
-static int disable_cleancache __read_mostly;
-static int disable_frontswap __read_mostly;
-static int disable_frontswap_ignore_nonactive __read_mostly;
-static int disable_cleancache_ignore_nonactive __read_mostly;
+static bool zcache_enabled __read_mostly;
+static bool disable_cleancache __read_mostly;
+static bool disable_frontswap __read_mostly;
+static bool disable_frontswap_ignore_nonactive __read_mostly;
+static bool disable_cleancache_ignore_nonactive __read_mostly;
 static char *namestr __read_mostly = "zcache";
 
 #define ZCACHE_GFP_MASK \
 	(__GFP_FS | __GFP_NORETRY | __GFP_NOWARN | __GFP_NOMEMALLOC)
 
-MODULE_LICENSE("GPL");
-
 /* crypto API for zcache  */
 #define ZCACHE_COMP_NAME_SZ CRYPTO_MAX_ALG_NAME
 static char zcache_comp_name[ZCACHE_COMP_NAME_SZ] __read_mostly;
@@ -134,196 +140,21 @@ static struct kmem_cache *zcache_obj_cache;
 
 static DEFINE_PER_CPU(struct zcache_preload, zcache_preloads) = { 0, };
 
-/* we try to keep these statistics SMP-consistent */
-static ssize_t zcache_obj_count;
-static atomic_t zcache_obj_atomic = ATOMIC_INIT(0);
-static ssize_t zcache_obj_count_max;
-static ssize_t zcache_objnode_count;
-static atomic_t zcache_objnode_atomic = ATOMIC_INIT(0);
-static ssize_t zcache_objnode_count_max;
-static u64 zcache_eph_zbytes;
-static atomic_long_t zcache_eph_zbytes_atomic = ATOMIC_INIT(0);
-static u64 zcache_eph_zbytes_max;
-static u64 zcache_pers_zbytes;
-static atomic_long_t zcache_pers_zbytes_atomic = ATOMIC_INIT(0);
-static u64 zcache_pers_zbytes_max;
-static ssize_t zcache_eph_pageframes;
-static atomic_t zcache_eph_pageframes_atomic = ATOMIC_INIT(0);
-static ssize_t zcache_eph_pageframes_max;
-static ssize_t zcache_pers_pageframes;
-static atomic_t zcache_pers_pageframes_atomic = ATOMIC_INIT(0);
-static ssize_t zcache_pers_pageframes_max;
-static ssize_t zcache_pageframes_alloced;
-static atomic_t zcache_pageframes_alloced_atomic = ATOMIC_INIT(0);
-static ssize_t zcache_pageframes_freed;
-static atomic_t zcache_pageframes_freed_atomic = ATOMIC_INIT(0);
-static ssize_t zcache_eph_zpages;
-static ssize_t zcache_eph_zpages;
-static atomic_t zcache_eph_zpages_atomic = ATOMIC_INIT(0);
-static ssize_t zcache_eph_zpages_max;
-static ssize_t zcache_pers_zpages;
-static atomic_t zcache_pers_zpages_atomic = ATOMIC_INIT(0);
-static ssize_t zcache_pers_zpages_max;
-
-/* but for the rest of these, counting races are ok */
-static ssize_t zcache_flush_total;
-static ssize_t zcache_flush_found;
-static ssize_t zcache_flobj_total;
-static ssize_t zcache_flobj_found;
-static ssize_t zcache_failed_eph_puts;
-static ssize_t zcache_failed_pers_puts;
-static ssize_t zcache_failed_getfreepages;
-static ssize_t zcache_failed_alloc;
-static ssize_t zcache_put_to_flush;
-static ssize_t zcache_compress_poor;
-static ssize_t zcache_mean_compress_poor;
-static ssize_t zcache_eph_ate_tail;
-static ssize_t zcache_eph_ate_tail_failed;
-static ssize_t zcache_pers_ate_eph;
-static ssize_t zcache_pers_ate_eph_failed;
-static ssize_t zcache_evicted_eph_zpages;
-static ssize_t zcache_evicted_eph_pageframes;
-static ssize_t zcache_last_active_file_pageframes;
-static ssize_t zcache_last_inactive_file_pageframes;
-static ssize_t zcache_last_active_anon_pageframes;
-static ssize_t zcache_last_inactive_anon_pageframes;
-static ssize_t zcache_eph_nonactive_puts_ignored;
-static ssize_t zcache_pers_nonactive_puts_ignored;
-static ssize_t zcache_writtenback_pages;
-static ssize_t zcache_outstanding_writeback_pages;
-
-#ifdef CONFIG_DEBUG_FS
-#include <linux/debugfs.h>
-#define	zdfs	debugfs_create_size_t
-#define	zdfs64	debugfs_create_u64
-static int zcache_debugfs_init(void)
-{
-	struct dentry *root = debugfs_create_dir("zcache", NULL);
-	if (root == NULL)
-		return -ENXIO;
-
-	zdfs("obj_count", S_IRUGO, root, &zcache_obj_count);
-	zdfs("obj_count_max", S_IRUGO, root, &zcache_obj_count_max);
-	zdfs("objnode_count", S_IRUGO, root, &zcache_objnode_count);
-	zdfs("objnode_count_max", S_IRUGO, root, &zcache_objnode_count_max);
-	zdfs("flush_total", S_IRUGO, root, &zcache_flush_total);
-	zdfs("flush_found", S_IRUGO, root, &zcache_flush_found);
-	zdfs("flobj_total", S_IRUGO, root, &zcache_flobj_total);
-	zdfs("flobj_found", S_IRUGO, root, &zcache_flobj_found);
-	zdfs("failed_eph_puts", S_IRUGO, root, &zcache_failed_eph_puts);
-	zdfs("failed_pers_puts", S_IRUGO, root, &zcache_failed_pers_puts);
-	zdfs("failed_get_free_pages", S_IRUGO, root,
-				&zcache_failed_getfreepages);
-	zdfs("failed_alloc", S_IRUGO, root, &zcache_failed_alloc);
-	zdfs("put_to_flush", S_IRUGO, root, &zcache_put_to_flush);
-	zdfs("compress_poor", S_IRUGO, root, &zcache_compress_poor);
-	zdfs("mean_compress_poor", S_IRUGO, root, &zcache_mean_compress_poor);
-	zdfs("eph_ate_tail", S_IRUGO, root, &zcache_eph_ate_tail);
-	zdfs("eph_ate_tail_failed", S_IRUGO, root, &zcache_eph_ate_tail_failed);
-	zdfs("pers_ate_eph", S_IRUGO, root, &zcache_pers_ate_eph);
-	zdfs("pers_ate_eph_failed", S_IRUGO, root, &zcache_pers_ate_eph_failed);
-	zdfs("evicted_eph_zpages", S_IRUGO, root, &zcache_evicted_eph_zpages);
-	zdfs("evicted_eph_pageframes", S_IRUGO, root,
-				&zcache_evicted_eph_pageframes);
-	zdfs("eph_pageframes", S_IRUGO, root, &zcache_eph_pageframes);
-	zdfs("eph_pageframes_max", S_IRUGO, root, &zcache_eph_pageframes_max);
-	zdfs("pers_pageframes", S_IRUGO, root, &zcache_pers_pageframes);
-	zdfs("pers_pageframes_max", S_IRUGO, root, &zcache_pers_pageframes_max);
-	zdfs("eph_zpages", S_IRUGO, root, &zcache_eph_zpages);
-	zdfs("eph_zpages_max", S_IRUGO, root, &zcache_eph_zpages_max);
-	zdfs("pers_zpages", S_IRUGO, root, &zcache_pers_zpages);
-	zdfs("pers_zpages_max", S_IRUGO, root, &zcache_pers_zpages_max);
-	zdfs("last_active_file_pageframes", S_IRUGO, root,
-				&zcache_last_active_file_pageframes);
-	zdfs("last_inactive_file_pageframes", S_IRUGO, root,
-				&zcache_last_inactive_file_pageframes);
-	zdfs("last_active_anon_pageframes", S_IRUGO, root,
-				&zcache_last_active_anon_pageframes);
-	zdfs("last_inactive_anon_pageframes", S_IRUGO, root,
-				&zcache_last_inactive_anon_pageframes);
-	zdfs("eph_nonactive_puts_ignored", S_IRUGO, root,
-				&zcache_eph_nonactive_puts_ignored);
-	zdfs("pers_nonactive_puts_ignored", S_IRUGO, root,
-				&zcache_pers_nonactive_puts_ignored);
-	zdfs64("eph_zbytes", S_IRUGO, root, &zcache_eph_zbytes);
-	zdfs64("eph_zbytes_max", S_IRUGO, root, &zcache_eph_zbytes_max);
-	zdfs64("pers_zbytes", S_IRUGO, root, &zcache_pers_zbytes);
-	zdfs64("pers_zbytes_max", S_IRUGO, root, &zcache_pers_zbytes_max);
-	zdfs("outstanding_writeback_pages", S_IRUGO, root,
-				&zcache_outstanding_writeback_pages);
-	zdfs("writtenback_pages", S_IRUGO, root, &zcache_writtenback_pages);
-	return 0;
-}
-#undef	zdebugfs
-#undef	zdfs64
-#endif
-
-#define ZCACHE_DEBUG
-#ifdef ZCACHE_DEBUG
-/* developers can call this in case of ooms, e.g. to find memory leaks */
-void zcache_dump(void)
-{
-	pr_info("zcache: obj_count=%zd\n", zcache_obj_count);
-	pr_info("zcache: obj_count_max=%zd\n", zcache_obj_count_max);
-	pr_info("zcache: objnode_count=%zd\n", zcache_objnode_count);
-	pr_info("zcache: objnode_count_max=%zd\n", zcache_objnode_count_max);
-	pr_info("zcache: flush_total=%zd\n", zcache_flush_total);
-	pr_info("zcache: flush_found=%zd\n", zcache_flush_found);
-	pr_info("zcache: flobj_total=%zd\n", zcache_flobj_total);
-	pr_info("zcache: flobj_found=%zd\n", zcache_flobj_found);
-	pr_info("zcache: failed_eph_puts=%zd\n", zcache_failed_eph_puts);
-	pr_info("zcache: failed_pers_puts=%zd\n", zcache_failed_pers_puts);
-	pr_info("zcache: failed_get_free_pages=%zd\n",
-				zcache_failed_getfreepages);
-	pr_info("zcache: failed_alloc=%zd\n", zcache_failed_alloc);
-	pr_info("zcache: put_to_flush=%zd\n", zcache_put_to_flush);
-	pr_info("zcache: compress_poor=%zd\n", zcache_compress_poor);
-	pr_info("zcache: mean_compress_poor=%zd\n",
-				zcache_mean_compress_poor);
-	pr_info("zcache: eph_ate_tail=%zd\n", zcache_eph_ate_tail);
-	pr_info("zcache: eph_ate_tail_failed=%zd\n",
-				zcache_eph_ate_tail_failed);
-	pr_info("zcache: pers_ate_eph=%zd\n", zcache_pers_ate_eph);
-	pr_info("zcache: pers_ate_eph_failed=%zd\n",
-				zcache_pers_ate_eph_failed);
-	pr_info("zcache: evicted_eph_zpages=%zd\n", zcache_evicted_eph_zpages);
-	pr_info("zcache: evicted_eph_pageframes=%zd\n",
-				zcache_evicted_eph_pageframes);
-	pr_info("zcache: eph_pageframes=%zd\n", zcache_eph_pageframes);
-	pr_info("zcache: eph_pageframes_max=%zd\n", zcache_eph_pageframes_max);
-	pr_info("zcache: pers_pageframes=%zd\n", zcache_pers_pageframes);
-	pr_info("zcache: pers_pageframes_max=%zd\n",
-				zcache_pers_pageframes_max);
-	pr_info("zcache: eph_zpages=%zd\n", zcache_eph_zpages);
-	pr_info("zcache: eph_zpages_max=%zd\n", zcache_eph_zpages_max);
-	pr_info("zcache: pers_zpages=%zd\n", zcache_pers_zpages);
-	pr_info("zcache: pers_zpages_max=%zd\n", zcache_pers_zpages_max);
-	pr_info("zcache: last_active_file_pageframes=%zd\n",
-				zcache_last_active_file_pageframes);
-	pr_info("zcache: last_inactive_file_pageframes=%zd\n",
-				zcache_last_inactive_file_pageframes);
-	pr_info("zcache: last_active_anon_pageframes=%zd\n",
-				zcache_last_active_anon_pageframes);
-	pr_info("zcache: last_inactive_anon_pageframes=%zd\n",
-				zcache_last_inactive_anon_pageframes);
-	pr_info("zcache: eph_nonactive_puts_ignored=%zd\n",
-				zcache_eph_nonactive_puts_ignored);
-	pr_info("zcache: pers_nonactive_puts_ignored=%zd\n",
-				zcache_pers_nonactive_puts_ignored);
-	pr_info("zcache: eph_zbytes=%llu\n",
-				zcache_eph_zbytes);
-	pr_info("zcache: eph_zbytes_max=%llu\n",
-				zcache_eph_zbytes_max);
-	pr_info("zcache: pers_zbytes=%llu\n",
-				zcache_pers_zbytes);
-	pr_info("zcache: pers_zbytes_max=%llu\n",
-				zcache_pers_zbytes_max);
-	pr_info("zcache: outstanding_writeback_pages=%zd\n",
-				zcache_outstanding_writeback_pages);
-	pr_info("zcache: writtenback_pages=%zd\n", zcache_writtenback_pages);
-}
+/* Used by debug.c */
+ssize_t zcache_pers_zpages;
+u64 zcache_pers_zbytes;
+ssize_t zcache_eph_pageframes;
+ssize_t zcache_pers_pageframes;
+
+/* Used by this code. */
+ssize_t zcache_last_active_file_pageframes;
+ssize_t zcache_last_inactive_file_pageframes;
+ssize_t zcache_last_active_anon_pageframes;
+ssize_t zcache_last_inactive_anon_pageframes;
+#ifdef CONFIG_ZCACHE_WRITEBACK
+ssize_t zcache_writtenback_pages;
+ssize_t zcache_outstanding_writeback_pages;
 #endif
-
 /*
  * zcache core code starts here
  */
@@ -422,18 +253,14 @@ static struct tmem_objnode *zcache_objnode_alloc(struct tmem_pool *pool)
 		}
 	}
 	BUG_ON(objnode == NULL);
-	zcache_objnode_count = atomic_inc_return(&zcache_objnode_atomic);
-	if (zcache_objnode_count > zcache_objnode_count_max)
-		zcache_objnode_count_max = zcache_objnode_count;
+	inc_zcache_objnode_count();
 	return objnode;
 }
 
 static void zcache_objnode_free(struct tmem_objnode *objnode,
 					struct tmem_pool *pool)
 {
-	zcache_objnode_count =
-		atomic_dec_return(&zcache_objnode_atomic);
-	BUG_ON(zcache_objnode_count < 0);
+	dec_zcache_objnode_count();
 	kmem_cache_free(zcache_objnode_cache, objnode);
 }
 
@@ -446,20 +273,49 @@ static struct tmem_obj *zcache_obj_alloc(struct tmem_pool *pool)
 	obj = kp->obj;
 	BUG_ON(obj == NULL);
 	kp->obj = NULL;
-	zcache_obj_count = atomic_inc_return(&zcache_obj_atomic);
-	if (zcache_obj_count > zcache_obj_count_max)
-		zcache_obj_count_max = zcache_obj_count;
+	inc_zcache_obj_count();
 	return obj;
 }
 
 static void zcache_obj_free(struct tmem_obj *obj, struct tmem_pool *pool)
 {
-	zcache_obj_count =
-		atomic_dec_return(&zcache_obj_atomic);
-	BUG_ON(zcache_obj_count < 0);
+	dec_zcache_obj_count();
 	kmem_cache_free(zcache_obj_cache, obj);
 }
 
+/*
+ * Compressing zero-filled pages will waste memory and introduce
+ * serious fragmentation, skip it to avoid overhead.
+ */
+static bool page_is_zero_filled(struct page *p)
+{
+	unsigned int pos;
+	char *page;
+
+	page = kmap_atomic(p);
+	for (pos = 0; pos < PAGE_SIZE / sizeof(*page); pos++) {
+		if (page[pos]) {
+			kunmap_atomic(page);
+			return false;
+		}
+	}
+	kunmap_atomic(page);
+
+	return true;
+}
+
+static void handle_zero_filled_page(void *p)
+{
+	void *user_mem;
+	struct page *page = (struct page *)p;
+
+	user_mem = kmap_atomic(page);
+	memset(user_mem, 0, PAGE_SIZE);
+	kunmap_atomic(user_mem);
+
+	flush_dcache_page(page);
+}
+
 static struct tmem_hostops zcache_hostops = {
 	.obj_alloc = zcache_obj_alloc,
 	.obj_free = zcache_obj_free,
@@ -472,8 +328,7 @@ static struct page *zcache_alloc_page(void)
 	struct page *page = alloc_page(ZCACHE_GFP_MASK);
 
 	if (page != NULL)
-		zcache_pageframes_alloced =
-			atomic_inc_return(&zcache_pageframes_alloced_atomic);
+		inc_zcache_pageframes_alloced();
 	return page;
 }
 
@@ -485,17 +340,13 @@ static void zcache_free_page(struct page *page)
 	if (page == NULL)
 		BUG();
 	__free_page(page);
-	zcache_pageframes_freed =
-		atomic_inc_return(&zcache_pageframes_freed_atomic);
-	curr_pageframes = zcache_pageframes_alloced -
-			atomic_read(&zcache_pageframes_freed_atomic) -
-			atomic_read(&zcache_eph_pageframes_atomic) -
-			atomic_read(&zcache_pers_pageframes_atomic);
+	inc_zcache_pageframes_freed();
+	curr_pageframes = curr_pageframes_count();
 	if (curr_pageframes > max_pageframes)
 		max_pageframes = curr_pageframes;
 	if (curr_pageframes < min_pageframes)
 		min_pageframes = curr_pageframes;
-#ifdef ZCACHE_DEBUG
+#ifdef CONFIG_ZCACHE_DEBUG
 	if (curr_pageframes > 2L || curr_pageframes < -2L) {
 		/* pr_info here */
 	}
@@ -517,12 +368,20 @@ static void *zcache_pampd_eph_create(char *data, size_t size, bool raw,
 {
 	void *pampd = NULL, *cdata = data;
 	unsigned clen = size;
+	bool zero_filled = false;
 	struct page *page = (struct page *)(data), *newpage;
 
+	if (page_is_zero_filled(page)) {
+		clen = 0;
+		zero_filled = true;
+		inc_zcache_zero_filled_pages();
+		goto got_pampd;
+	}
+
 	if (!raw) {
 		zcache_compress(page, &cdata, &clen);
 		if (clen > zbud_max_buddy_size()) {
-			zcache_compress_poor++;
+			inc_zcache_compress_poor();
 			goto out;
 		}
 	} else {
@@ -539,33 +398,27 @@ static void *zcache_pampd_eph_create(char *data, size_t size, bool raw,
 	if (newpage != NULL)
 		goto create_in_new_page;
 
-	zcache_failed_getfreepages++;
+	inc_zcache_failed_getfreepages();
 	/* can't allocate a page, evict an ephemeral page via LRU */
 	newpage = zcache_evict_eph_pageframe();
 	if (newpage == NULL) {
-		zcache_eph_ate_tail_failed++;
+		inc_zcache_eph_ate_tail_failed();
 		goto out;
 	}
-	zcache_eph_ate_tail++;
+	inc_zcache_eph_ate_tail();
 
 create_in_new_page:
 	pampd = (void *)zbud_create_prep(th, true, cdata, clen, newpage);
 	BUG_ON(pampd == NULL);
-	zcache_eph_pageframes =
-		atomic_inc_return(&zcache_eph_pageframes_atomic);
-	if (zcache_eph_pageframes > zcache_eph_pageframes_max)
-		zcache_eph_pageframes_max = zcache_eph_pageframes;
+	inc_zcache_eph_pageframes();
 
 got_pampd:
-	zcache_eph_zbytes =
-		atomic_long_add_return(clen, &zcache_eph_zbytes_atomic);
-	if (zcache_eph_zbytes > zcache_eph_zbytes_max)
-		zcache_eph_zbytes_max = zcache_eph_zbytes;
-	zcache_eph_zpages = atomic_inc_return(&zcache_eph_zpages_atomic);
-	if (zcache_eph_zpages > zcache_eph_zpages_max)
-		zcache_eph_zpages_max = zcache_eph_zpages;
-	if (ramster_enabled && raw)
+	inc_zcache_eph_zbytes(clen);
+	inc_zcache_eph_zpages();
+	if (ramster_enabled && raw && !zero_filled)
 		ramster_count_foreign_pages(true, 1);
+	if (zero_filled)
+		pampd = (void *)ZERO_FILLED;
 out:
 	return pampd;
 }
@@ -575,6 +428,7 @@ static void *zcache_pampd_pers_create(char *data, size_t size, bool raw,
 {
 	void *pampd = NULL, *cdata = data;
 	unsigned clen = size;
+	bool zero_filled = false;
 	struct page *page = (struct page *)(data), *newpage;
 	unsigned long zbud_mean_zsize;
 	unsigned long curr_pers_zpages, total_zsize;
@@ -583,13 +437,21 @@ static void *zcache_pampd_pers_create(char *data, size_t size, bool raw,
 		BUG_ON(!ramster_enabled);
 		goto create_pampd;
 	}
+
+	if (page_is_zero_filled(page)) {
+		clen = 0;
+		zero_filled = true;
+		inc_zcache_zero_filled_pages();
+		goto got_pampd;
+	}
+
 	curr_pers_zpages = zcache_pers_zpages;
 /* FIXME CONFIG_RAMSTER... subtract atomic remote_pers_pages here? */
 	if (!raw)
 		zcache_compress(page, &cdata, &clen);
 	/* reject if compression is too poor */
 	if (clen > zbud_max_zsize) {
-		zcache_compress_poor++;
+		inc_zcache_compress_poor();
 		goto out;
 	}
 	/* reject if mean compression is too poor */
@@ -600,7 +462,7 @@ static void *zcache_pampd_pers_create(char *data, size_t size, bool raw,
 		zbud_mean_zsize = div_u64(total_zsize,
 					curr_pers_zpages);
 		if (zbud_mean_zsize > zbud_max_mean_zsize) {
-			zcache_mean_compress_poor++;
+			inc_zcache_mean_compress_poor();
 			goto out;
 		}
 	}
@@ -621,33 +483,27 @@ create_pampd:
 	 * (global_page_state(NR_LRU_BASE + LRU_ACTIVE_FILE) +
 	 * global_page_state(NR_LRU_BASE + LRU_INACTIVE_FILE)))
 	 */
-	zcache_failed_getfreepages++;
+	inc_zcache_failed_getfreepages();
 	/* can't allocate a page, evict an ephemeral page via LRU */
 	newpage = zcache_evict_eph_pageframe();
 	if (newpage == NULL) {
-		zcache_pers_ate_eph_failed++;
+		inc_zcache_pers_ate_eph_failed();
 		goto out;
 	}
-	zcache_pers_ate_eph++;
+	inc_zcache_pers_ate_eph();
 
 create_in_new_page:
 	pampd = (void *)zbud_create_prep(th, false, cdata, clen, newpage);
 	BUG_ON(pampd == NULL);
-	zcache_pers_pageframes =
-		atomic_inc_return(&zcache_pers_pageframes_atomic);
-	if (zcache_pers_pageframes > zcache_pers_pageframes_max)
-		zcache_pers_pageframes_max = zcache_pers_pageframes;
+	inc_zcache_pers_pageframes();
 
 got_pampd:
-	zcache_pers_zpages = atomic_inc_return(&zcache_pers_zpages_atomic);
-	if (zcache_pers_zpages > zcache_pers_zpages_max)
-		zcache_pers_zpages_max = zcache_pers_zpages;
-	zcache_pers_zbytes =
-		atomic_long_add_return(clen, &zcache_pers_zbytes_atomic);
-	if (zcache_pers_zbytes > zcache_pers_zbytes_max)
-		zcache_pers_zbytes_max = zcache_pers_zbytes;
-	if (ramster_enabled && raw)
+	inc_zcache_pers_zpages();
+	inc_zcache_pers_zbytes(clen);
+	if (ramster_enabled && raw && !zero_filled)
 		ramster_count_foreign_pages(false, 1);
+	if (zero_filled)
+		pampd = (void *)ZERO_FILLED;
 out:
 	return pampd;
 }
@@ -676,7 +532,7 @@ void *zcache_pampd_create(char *data, unsigned int size, bool raw,
 			objnode = kmem_cache_alloc(zcache_objnode_cache,
 							ZCACHE_GFP_MASK);
 			if (unlikely(objnode == NULL)) {
-				zcache_failed_alloc++;
+				inc_zcache_failed_alloc();
 				goto out;
 			}
 			kp->objnodes[i] = objnode;
@@ -687,7 +543,7 @@ void *zcache_pampd_create(char *data, unsigned int size, bool raw,
 		kp->obj = obj;
 	}
 	if (unlikely(kp->obj == NULL)) {
-		zcache_failed_alloc++;
+		inc_zcache_failed_alloc();
 		goto out;
 	}
 	/*
@@ -709,7 +565,8 @@ out:
  */
 void zcache_pampd_create_finish(void *pampd, bool eph)
 {
-	zbud_create_finish((struct zbudref *)pampd, eph);
+	if (pampd != (void *)ZERO_FILLED)
+		zbud_create_finish((struct zbudref *)pampd, eph);
 }
 
 /*
@@ -754,6 +611,14 @@ static int zcache_pampd_get_data(char *data, size_t *sizep, bool raw,
 	BUG_ON(preemptible());
 	BUG_ON(eph);	/* fix later if shared pools get implemented */
 	BUG_ON(pampd_is_remote(pampd));
+
+	if (pampd == (void *)ZERO_FILLED) {
+		handle_zero_filled_page(data);
+		if (!raw)
+			*sizep = PAGE_SIZE;
+		return 0;
+	}
+
 	if (raw)
 		ret = zbud_copy_from_zbud(data, (struct zbudref *)pampd,
 						sizep, eph);
@@ -774,13 +639,25 @@ static int zcache_pampd_get_data_and_free(char *data, size_t *sizep, bool raw,
 					void *pampd, struct tmem_pool *pool,
 					struct tmem_oid *oid, uint32_t index)
 {
-	int ret;
-	bool eph = !is_persistent(pool);
+	int ret = 0;
+	bool eph = !is_persistent(pool), zero_filled = false;
 	struct page *page = NULL;
 	unsigned int zsize, zpages;
 
 	BUG_ON(preemptible());
 	BUG_ON(pampd_is_remote(pampd));
+
+	if (pampd == (void *)ZERO_FILLED) {
+		handle_zero_filled_page(data);
+		zero_filled = true;
+		zsize = 0;
+		zpages = 1;
+		if (!raw)
+			*sizep = PAGE_SIZE;
+		dec_zcache_zero_filled_pages();
+		goto zero_fill;
+	}
+
 	if (raw)
 		ret = zbud_copy_from_zbud(data, (struct zbudref *)pampd,
 						sizep, eph);
@@ -792,26 +669,21 @@ static int zcache_pampd_get_data_and_free(char *data, size_t *sizep, bool raw,
 	}
 	page = zbud_free_and_delist((struct zbudref *)pampd, eph,
 					&zsize, &zpages);
+zero_fill:
 	if (eph) {
 		if (page)
-			zcache_eph_pageframes =
-			    atomic_dec_return(&zcache_eph_pageframes_atomic);
-		zcache_eph_zpages =
-		    atomic_sub_return(zpages, &zcache_eph_zpages_atomic);
-		zcache_eph_zbytes =
-		    atomic_long_sub_return(zsize, &zcache_eph_zbytes_atomic);
+			dec_zcache_eph_pageframes();
+		dec_zcache_eph_zpages(zpages);
+		dec_zcache_eph_zbytes(zsize);
 	} else {
 		if (page)
-			zcache_pers_pageframes =
-			    atomic_dec_return(&zcache_pers_pageframes_atomic);
-		zcache_pers_zpages =
-		    atomic_sub_return(zpages, &zcache_pers_zpages_atomic);
-		zcache_pers_zbytes =
-		    atomic_long_sub_return(zsize, &zcache_pers_zbytes_atomic);
+			dec_zcache_pers_pageframes();
+		dec_zcache_pers_zpages(zpages);
+		dec_zcache_pers_zbytes(zsize);
 	}
-	if (!is_local_client(pool->client))
+	if (!is_local_client(pool->client) && !zero_filled)
 		ramster_count_foreign_pages(eph, -1);
-	if (page)
+	if (page && !zero_filled)
 		zcache_free_page(page);
 	return ret;
 }
@@ -825,39 +697,44 @@ static void zcache_pampd_free(void *pampd, struct tmem_pool *pool,
 {
 	struct page *page = NULL;
 	unsigned int zsize, zpages;
+	bool zero_filled = false;
 
 	BUG_ON(preemptible());
-	if (pampd_is_remote(pampd)) {
+
+	if (pampd == (void *)ZERO_FILLED) {
+		zero_filled = true;
+		zsize = 0;
+		zpages = 1;
+		dec_zcache_zero_filled_pages();
+	}
+
+	if (pampd_is_remote(pampd) && !zero_filled) {
 		BUG_ON(!ramster_enabled);
 		pampd = ramster_pampd_free(pampd, pool, oid, index, acct);
 		if (pampd == NULL)
 			return;
 	}
 	if (is_ephemeral(pool)) {
-		page = zbud_free_and_delist((struct zbudref *)pampd,
+		if (!zero_filled)
+			page = zbud_free_and_delist((struct zbudref *)pampd,
 						true, &zsize, &zpages);
 		if (page)
-			zcache_eph_pageframes =
-			    atomic_dec_return(&zcache_eph_pageframes_atomic);
-		zcache_eph_zpages =
-		    atomic_sub_return(zpages, &zcache_eph_zpages_atomic);
-		zcache_eph_zbytes =
-		    atomic_long_sub_return(zsize, &zcache_eph_zbytes_atomic);
+			dec_zcache_eph_pageframes();
+		dec_zcache_eph_zpages(zpages);
+		dec_zcache_eph_zbytes(zsize);
 		/* FIXME CONFIG_RAMSTER... check acct parameter? */
 	} else {
-		page = zbud_free_and_delist((struct zbudref *)pampd,
+		if (!zero_filled)
+			page = zbud_free_and_delist((struct zbudref *)pampd,
 						false, &zsize, &zpages);
 		if (page)
-			zcache_pers_pageframes =
-			    atomic_dec_return(&zcache_pers_pageframes_atomic);
-		zcache_pers_zpages =
-		     atomic_sub_return(zpages, &zcache_pers_zpages_atomic);
-		zcache_pers_zbytes =
-		    atomic_long_sub_return(zsize, &zcache_pers_zbytes_atomic);
+			dec_zcache_pers_pageframes();
+		dec_zcache_pers_zpages(zpages);
+		dec_zcache_pers_zbytes(zsize);
 	}
-	if (!is_local_client(pool->client))
+	if (!is_local_client(pool->client) && !zero_filled)
 		ramster_count_foreign_pages(is_ephemeral(pool), -1);
-	if (page)
+	if (page && !zero_filled)
 		zcache_free_page(page);
 }
 
@@ -975,14 +852,11 @@ static struct page *zcache_evict_eph_pageframe(void)
 	page = zbud_evict_pageframe_lru(&zsize, &zpages);
 	if (page == NULL)
 		goto out;
-	zcache_eph_zbytes = atomic_long_sub_return(zsize,
-					&zcache_eph_zbytes_atomic);
-	zcache_eph_zpages = atomic_sub_return(zpages,
-					&zcache_eph_zpages_atomic);
-	zcache_evicted_eph_zpages += zpages;
-	zcache_eph_pageframes =
-		atomic_dec_return(&zcache_eph_pageframes_atomic);
-	zcache_evicted_eph_pageframes++;
+	dec_zcache_eph_zbytes(zsize);
+	dec_zcache_eph_zpages(zpages);
+	inc_zcache_evicted_eph_zpages(zpages);
+	dec_zcache_eph_pageframes();
+	inc_zcache_evicted_eph_pageframes();
 out:
 	return page;
 }
@@ -991,6 +865,16 @@ out:
 
 static atomic_t zcache_outstanding_writeback_pages_atomic = ATOMIC_INIT(0);
 
+static inline void inc_zcache_outstanding_writeback_pages(void)
+{
+	zcache_outstanding_writeback_pages =
+	    atomic_inc_return(&zcache_outstanding_writeback_pages_atomic);
+}
+static inline void dec_zcache_outstanding_writeback_pages(void)
+{
+	zcache_outstanding_writeback_pages =
+	  atomic_dec_return(&zcache_outstanding_writeback_pages_atomic);
+};
 static void unswiz(struct tmem_oid oid, u32 index,
 				unsigned *type, pgoff_t *offset);
 
@@ -1004,8 +888,7 @@ static void unswiz(struct tmem_oid oid, u32 index,
 static void zcache_end_swap_write(struct bio *bio, int err)
 {
 	end_swap_bio_write(bio, err);
-	zcache_outstanding_writeback_pages =
-	  atomic_dec_return(&zcache_outstanding_writeback_pages_atomic);
+	dec_zcache_outstanding_writeback_pages();
 	zcache_writtenback_pages++;
 }
 
@@ -1120,8 +1003,7 @@ static int zcache_frontswap_writeback_zpage(int type, pgoff_t offset,
 	 */
 	(void)__swap_writepage(page, &wbc, zcache_end_swap_write);
 	page_cache_release(page);
-	zcache_outstanding_writeback_pages =
-	    atomic_inc_return(&zcache_outstanding_writeback_pages_atomic);
+	inc_zcache_outstanding_writeback_pages();
 
 	return 0;
 }
@@ -1357,9 +1239,9 @@ int zcache_put_page(int cli_id, int pool_id, struct tmem_oid *oidp,
 		if (pampd == NULL) {
 			ret = -ENOMEM;
 			if (ephemeral)
-				zcache_failed_eph_puts++;
+				inc_zcache_failed_eph_puts();
 			else
-				zcache_failed_pers_puts++;
+				inc_zcache_failed_pers_puts();
 		} else {
 			if (ramster_enabled)
 				ramster_do_preload_flnode(pool);
@@ -1369,7 +1251,7 @@ int zcache_put_page(int cli_id, int pool_id, struct tmem_oid *oidp,
 		}
 		zcache_put_pool(pool);
 	} else {
-		zcache_put_to_flush++;
+		inc_zcache_put_to_flush();
 		if (ramster_enabled)
 			ramster_do_preload_flnode(pool);
 		if (atomic_read(&pool->obj_count) > 0)
@@ -1419,7 +1301,7 @@ int zcache_flush_page(int cli_id, int pool_id,
 	unsigned long flags;
 
 	local_irq_save(flags);
-	zcache_flush_total++;
+	inc_zcache_flush_total();
 	pool = zcache_get_pool_by_id(cli_id, pool_id);
 	if (ramster_enabled)
 		ramster_do_preload_flnode(pool);
@@ -1429,7 +1311,7 @@ int zcache_flush_page(int cli_id, int pool_id,
 		zcache_put_pool(pool);
 	}
 	if (ret >= 0)
-		zcache_flush_found++;
+		inc_zcache_flush_found();
 	local_irq_restore(flags);
 	return ret;
 }
@@ -1442,7 +1324,7 @@ int zcache_flush_object(int cli_id, int pool_id,
 	unsigned long flags;
 
 	local_irq_save(flags);
-	zcache_flobj_total++;
+	inc_zcache_flobj_total();
 	pool = zcache_get_pool_by_id(cli_id, pool_id);
 	if (ramster_enabled)
 		ramster_do_preload_flnode(pool);
@@ -1452,7 +1334,7 @@ int zcache_flush_object(int cli_id, int pool_id,
 		zcache_put_pool(pool);
 	}
 	if (ret >= 0)
-		zcache_flobj_found++;
+		inc_zcache_flobj_found();
 	local_irq_restore(flags);
 	return ret;
 }
@@ -1546,7 +1428,7 @@ static int zcache_local_new_pool(uint32_t flags)
 int zcache_autocreate_pool(unsigned int cli_id, unsigned int pool_id, bool eph)
 {
 	struct tmem_pool *pool;
-	struct zcache_client *cli;
+	struct zcache_client *cli = NULL;
 	uint32_t flags = eph ? 0 : TMEM_POOL_PERSIST;
 	int ret = -1;
 
@@ -1615,7 +1497,7 @@ static void zcache_cleancache_put_page(int pool_id,
 	struct tmem_oid oid = *(struct tmem_oid *)&key;
 
 	if (!disable_cleancache_ignore_nonactive && !PageWasActive(page)) {
-		zcache_eph_nonactive_puts_ignored++;
+		inc_zcache_eph_nonactive_puts_ignored();
 		return;
 	}
 	if (likely(ind == index))
@@ -1744,7 +1626,7 @@ static int zcache_frontswap_put_page(unsigned type, pgoff_t offset,
 
 	BUG_ON(!PageLocked(page));
 	if (!disable_frontswap_ignore_nonactive && !PageWasActive(page)) {
-		zcache_pers_nonactive_puts_ignored++;
+		inc_zcache_pers_nonactive_puts_ignored();
 		ret = -ERANGE;
 		goto out;
 	}
@@ -1841,16 +1723,16 @@ struct frontswap_ops zcache_frontswap_register_ops(void)
 
 static int __init enable_zcache(char *s)
 {
-	zcache_enabled = 1;
+	zcache_enabled = true;
 	return 1;
 }
 __setup("zcache", enable_zcache);
 
 static int __init enable_ramster(char *s)
 {
-	zcache_enabled = 1;
+	zcache_enabled = true;
 #ifdef CONFIG_RAMSTER
-	ramster_enabled = 1;
+	ramster_enabled = true;
 #endif
 	return 1;
 }
@@ -1860,7 +1742,7 @@ __setup("ramster", enable_ramster);
 
 static int __init no_cleancache(char *s)
 {
-	disable_cleancache = 1;
+	disable_cleancache = true;
 	return 1;
 }
 
@@ -1868,7 +1750,7 @@ __setup("nocleancache", no_cleancache);
 
 static int __init no_frontswap(char *s)
 {
-	disable_frontswap = 1;
+	disable_frontswap = true;
 	return 1;
 }
 
@@ -1884,7 +1766,7 @@ __setup("nofrontswapexclusivegets", no_frontswap_exclusive_gets);
 
 static int __init no_frontswap_ignore_nonactive(char *s)
 {
-	disable_frontswap_ignore_nonactive = 1;
+	disable_frontswap_ignore_nonactive = true;
 	return 1;
 }
 
@@ -1892,7 +1774,7 @@ __setup("nofrontswapignorenonactive", no_frontswap_ignore_nonactive);
 
 static int __init no_cleancache_ignore_nonactive(char *s)
 {
-	disable_cleancache_ignore_nonactive = 1;
+	disable_cleancache_ignore_nonactive = true;
 	return 1;
 }
 
@@ -1900,8 +1782,8 @@ __setup("nocleancacheignorenonactive", no_cleancache_ignore_nonactive);
 
 static int __init enable_zcache_compressor(char *s)
 {
-	strncpy(zcache_comp_name, s, ZCACHE_COMP_NAME_SZ);
-	zcache_enabled = 1;
+	strlcpy(zcache_comp_name, s, sizeof(zcache_comp_name));
+	zcache_enabled = true;
 	return 1;
 }
 __setup("zcache=", enable_zcache_compressor);
@@ -1944,9 +1826,7 @@ static int __init zcache_init(void)
 		namestr = "ramster";
 		ramster_register_pamops(&zcache_pamops);
 	}
-#ifdef CONFIG_DEBUG_FS
 	zcache_debugfs_init();
-#endif
 	if (zcache_enabled) {
 		unsigned int cpu;
 
@@ -1986,7 +1866,7 @@ static int __init zcache_init(void)
 		old_ops = zcache_cleancache_register_ops();
 		pr_info("%s: cleancache enabled using kernel transcendent "
 			"memory and compression buddies\n", namestr);
-#ifdef ZCACHE_DEBUG
+#ifdef CONFIG_ZCACHE_DEBUG
 		pr_info("%s: cleancache: ignorenonactive = %d\n",
 			namestr, !disable_cleancache_ignore_nonactive);
 #endif
@@ -2001,7 +1881,7 @@ static int __init zcache_init(void)
 			frontswap_tmem_exclusive_gets(true);
 		pr_info("%s: frontswap enabled using kernel transcendent "
 			"memory and compression buddies\n", namestr);
-#ifdef ZCACHE_DEBUG
+#ifdef CONFIG_ZCACHE_DEBUG
 		pr_info("%s: frontswap: excl gets = %d active only = %d\n",
 			namestr, frontswap_has_exclusive_gets,
 			!disable_frontswap_ignore_nonactive);
diff --git a/drivers/staging/zram/zram_drv.c b/drivers/staging/zram/zram_drv.c
index 5918fd7d7e3..e34e3fe0ae2 100644
--- a/drivers/staging/zram/zram_drv.c
+++ b/drivers/staging/zram/zram_drv.c
@@ -207,11 +207,8 @@ static int zram_bvec_read(struct zram *zram, struct bio_vec *bvec,
 
 	ret = zram_decompress_page(zram, uncmem, index);
 	/* Should NEVER happen. Return bio error if it does. */
-	if (unlikely(ret != LZO_E_OK)) {
-		pr_err("Decompression failed! err=%d, page=%u\n", ret, index);
-		zram_stat64_inc(zram, &zram->stats.failed_reads);
+	if (unlikely(ret != LZO_E_OK))
 		goto out_cleanup;
-	}
 
 	if (is_partial_io(bvec))
 		memcpy(user_mem + bvec->bv_offset, uncmem + offset,
diff --git a/drivers/staging/zsmalloc/Kconfig b/drivers/staging/zsmalloc/Kconfig
index 90845656579..7fab032298f 100644
--- a/drivers/staging/zsmalloc/Kconfig
+++ b/drivers/staging/zsmalloc/Kconfig
@@ -1,5 +1,5 @@
 config ZSMALLOC
-	tristate "Memory allocator for compressed pages"
+	bool "Memory allocator for compressed pages"
 	default n
 	help
 	  zsmalloc is a slab-based memory allocator designed to store
diff --git a/drivers/staging/zsmalloc/zsmalloc-main.c b/drivers/staging/zsmalloc/zsmalloc-main.c
index e78d262c524..f82f7e69c8a 100644
--- a/drivers/staging/zsmalloc/zsmalloc-main.c
+++ b/drivers/staging/zsmalloc/zsmalloc-main.c
@@ -225,7 +225,7 @@ struct zs_pool {
  * so that USE_PGTABLE_MAPPING is defined. This causes zsmalloc to use
  * page table mapping rather than copying for object mapping.
 */
-#if defined(CONFIG_ARM)
+#if defined(CONFIG_ARM) && !defined(MODULE)
 #define USE_PGTABLE_MAPPING
 #endif
 
@@ -656,11 +656,8 @@ static inline void __zs_unmap_object(struct mapping_area *area,
 				struct page *pages[2], int off, int size)
 {
 	unsigned long addr = (unsigned long)area->vm_addr;
-	unsigned long end = addr + (PAGE_SIZE * 2);
 
-	flush_cache_vunmap(addr, end);
-	unmap_kernel_range_noflush(addr, PAGE_SIZE * 2);
-	flush_tlb_kernel_range(addr, end);
+	unmap_kernel_range(addr, PAGE_SIZE * 2);
 }
 
 #else /* USE_PGTABLE_MAPPING */
diff --git a/drivers/target/iscsi/Makefile b/drivers/target/iscsi/Makefile
index 5b9a2cf7f0a..13a92403fe3 100644
--- a/drivers/target/iscsi/Makefile
+++ b/drivers/target/iscsi/Makefile
@@ -15,6 +15,7 @@ iscsi_target_mod-y +=		iscsi_target_parameters.o \
 				iscsi_target_util.o \
 				iscsi_target.o \
 				iscsi_target_configfs.o \
-				iscsi_target_stat.o
+				iscsi_target_stat.o \
+				iscsi_target_transport.o
 
 obj-$(CONFIG_ISCSI_TARGET)	+= iscsi_target_mod.o
diff --git a/drivers/target/iscsi/iscsi_target.c b/drivers/target/iscsi/iscsi_target.c
index 7ea246a0773..ffbc6a94be5 100644
--- a/drivers/target/iscsi/iscsi_target.c
+++ b/drivers/target/iscsi/iscsi_target.c
@@ -49,6 +49,8 @@
 #include "iscsi_target_device.h"
 #include "iscsi_target_stat.h"
 
+#include <target/iscsi/iscsi_transport.h>
+
 static LIST_HEAD(g_tiqn_list);
 static LIST_HEAD(g_np_list);
 static DEFINE_SPINLOCK(tiqn_lock);
@@ -68,8 +70,7 @@ struct kmem_cache *lio_ooo_cache;
 struct kmem_cache *lio_r2t_cache;
 
 static int iscsit_handle_immediate_data(struct iscsi_cmd *,
-			unsigned char *buf, u32);
-static int iscsit_logout_post_handler(struct iscsi_cmd *, struct iscsi_conn *);
+			struct iscsi_scsi_req *, u32);
 
 struct iscsi_tiqn *iscsit_get_tiqn_for_login(unsigned char *buf)
 {
@@ -401,8 +402,7 @@ struct iscsi_np *iscsit_add_np(
 	spin_unlock_bh(&np_lock);
 
 	pr_debug("CORE[0] - Added Network Portal: %s:%hu on %s\n",
-		np->np_ip, np->np_port, (np->np_network_transport == ISCSI_TCP) ?
-		"TCP" : "SCTP");
+		np->np_ip, np->np_port, np->np_transport->name);
 
 	return np;
 }
@@ -441,11 +441,10 @@ int iscsit_reset_np_thread(
 	return 0;
 }
 
-static int iscsit_del_np_comm(struct iscsi_np *np)
+static void iscsit_free_np(struct iscsi_np *np)
 {
 	if (np->np_socket)
 		sock_release(np->np_socket);
-	return 0;
 }
 
 int iscsit_del_np(struct iscsi_np *np)
@@ -467,20 +466,47 @@ int iscsit_del_np(struct iscsi_np *np)
 		send_sig(SIGINT, np->np_thread, 1);
 		kthread_stop(np->np_thread);
 	}
-	iscsit_del_np_comm(np);
+
+	np->np_transport->iscsit_free_np(np);
 
 	spin_lock_bh(&np_lock);
 	list_del(&np->np_list);
 	spin_unlock_bh(&np_lock);
 
 	pr_debug("CORE[0] - Removed Network Portal: %s:%hu on %s\n",
-		np->np_ip, np->np_port, (np->np_network_transport == ISCSI_TCP) ?
-		"TCP" : "SCTP");
+		np->np_ip, np->np_port, np->np_transport->name);
 
+	iscsit_put_transport(np->np_transport);
 	kfree(np);
 	return 0;
 }
 
+static int iscsit_immediate_queue(struct iscsi_conn *, struct iscsi_cmd *, int);
+static int iscsit_response_queue(struct iscsi_conn *, struct iscsi_cmd *, int);
+
+static int iscsit_queue_rsp(struct iscsi_conn *conn, struct iscsi_cmd *cmd)
+{
+	iscsit_add_cmd_to_response_queue(cmd, cmd->conn, cmd->i_state);
+	return 0;
+}
+
+static struct iscsit_transport iscsi_target_transport = {
+	.name			= "iSCSI/TCP",
+	.transport_type		= ISCSI_TCP,
+	.owner			= NULL,
+	.iscsit_setup_np	= iscsit_setup_np,
+	.iscsit_accept_np	= iscsit_accept_np,
+	.iscsit_free_np		= iscsit_free_np,
+	.iscsit_alloc_cmd	= iscsit_alloc_cmd,
+	.iscsit_get_login_rx	= iscsit_get_login_rx,
+	.iscsit_put_login_tx	= iscsit_put_login_tx,
+	.iscsit_get_dataout	= iscsit_build_r2ts_for_cmd,
+	.iscsit_immediate_queue	= iscsit_immediate_queue,
+	.iscsit_response_queue	= iscsit_response_queue,
+	.iscsit_queue_data_in	= iscsit_queue_rsp,
+	.iscsit_queue_status	= iscsit_queue_rsp,
+};
+
 static int __init iscsi_target_init_module(void)
 {
 	int ret = 0;
@@ -557,6 +583,8 @@ static int __init iscsi_target_init_module(void)
 		goto ooo_out;
 	}
 
+	iscsit_register_transport(&iscsi_target_transport);
+
 	if (iscsit_load_discovery_tpg() < 0)
 		goto r2t_out;
 
@@ -587,6 +615,7 @@ static void __exit iscsi_target_cleanup_module(void)
 	iscsi_deallocate_thread_sets();
 	iscsi_thread_set_free();
 	iscsit_release_discovery_tpg();
+	iscsit_unregister_transport(&iscsi_target_transport);
 	kmem_cache_destroy(lio_cmd_cache);
 	kmem_cache_destroy(lio_qr_cache);
 	kmem_cache_destroy(lio_dr_cache);
@@ -682,11 +711,20 @@ int iscsit_add_reject_from_cmd(
 	iscsit_add_cmd_to_response_queue(cmd, conn, cmd->i_state);
 
 	ret = wait_for_completion_interruptible(&cmd->reject_comp);
+	/*
+	 * Perform the kref_put now if se_cmd has already been setup by
+	 * scsit_setup_scsi_cmd()
+	 */
+	if (cmd->se_cmd.se_tfo != NULL) {
+		pr_debug("iscsi reject: calling target_put_sess_cmd >>>>>>\n");
+		target_put_sess_cmd(conn->sess->se_sess, &cmd->se_cmd);
+	}
 	if (ret != 0)
 		return -1;
 
 	return (!fail_conn) ? 0 : -1;
 }
+EXPORT_SYMBOL(iscsit_add_reject_from_cmd);
 
 /*
  * Map some portion of the allocated scatterlist to an iovec, suitable for
@@ -745,6 +783,9 @@ static void iscsit_ack_from_expstatsn(struct iscsi_conn *conn, u32 exp_statsn)
 
 	conn->exp_statsn = exp_statsn;
 
+	if (conn->sess->sess_ops->RDMAExtensions)
+		return;
+
 	spin_lock_bh(&conn->cmd_lock);
 	list_for_each_entry(cmd, &conn->conn_cmd_list, i_conn_node) {
 		spin_lock(&cmd->istate_lock);
@@ -777,12 +818,10 @@ static int iscsit_allocate_iovecs(struct iscsi_cmd *cmd)
 	return 0;
 }
 
-static int iscsit_handle_scsi_cmd(
-	struct iscsi_conn *conn,
-	unsigned char *buf)
+int iscsit_setup_scsi_cmd(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
+			  unsigned char *buf)
 {
-	int data_direction, payload_length, cmdsn_ret = 0, immed_ret;
-	struct iscsi_cmd *cmd = NULL;
+	int data_direction, payload_length;
 	struct iscsi_scsi_req *hdr;
 	int iscsi_task_attr;
 	int sam_task_attr;
@@ -805,8 +844,8 @@ static int iscsit_handle_scsi_cmd(
 	    !(hdr->flags & ISCSI_FLAG_CMD_FINAL)) {
 		pr_err("ISCSI_FLAG_CMD_WRITE & ISCSI_FLAG_CMD_FINAL"
 				" not set. Bad iSCSI Initiator.\n");
-		return iscsit_add_reject(ISCSI_REASON_BOOKMARK_INVALID, 1,
-				buf, conn);
+		return iscsit_add_reject_from_cmd(ISCSI_REASON_BOOKMARK_INVALID,
+				1, 1, buf, cmd);
 	}
 
 	if (((hdr->flags & ISCSI_FLAG_CMD_READ) ||
@@ -826,8 +865,8 @@ static int iscsit_handle_scsi_cmd(
 		pr_err("ISCSI_FLAG_CMD_READ or ISCSI_FLAG_CMD_WRITE"
 			" set when Expected Data Transfer Length is 0 for"
 			" CDB: 0x%02x. Bad iSCSI Initiator.\n", hdr->cdb[0]);
-		return iscsit_add_reject(ISCSI_REASON_BOOKMARK_INVALID, 1,
-				buf, conn);
+		return iscsit_add_reject_from_cmd(ISCSI_REASON_BOOKMARK_INVALID,
+				1, 1, buf, cmd);
 	}
 done:
 
@@ -836,29 +875,29 @@ done:
 		pr_err("ISCSI_FLAG_CMD_READ and/or ISCSI_FLAG_CMD_WRITE"
 			" MUST be set if Expected Data Transfer Length is not 0."
 			" Bad iSCSI Initiator\n");
-		return iscsit_add_reject(ISCSI_REASON_BOOKMARK_INVALID, 1,
-				buf, conn);
+		return iscsit_add_reject_from_cmd(ISCSI_REASON_BOOKMARK_INVALID,
+				1, 1, buf, cmd);
 	}
 
 	if ((hdr->flags & ISCSI_FLAG_CMD_READ) &&
 	    (hdr->flags & ISCSI_FLAG_CMD_WRITE)) {
 		pr_err("Bidirectional operations not supported!\n");
-		return iscsit_add_reject(ISCSI_REASON_BOOKMARK_INVALID, 1,
-				buf, conn);
+		return iscsit_add_reject_from_cmd(ISCSI_REASON_BOOKMARK_INVALID,
+				1, 1, buf, cmd);
 	}
 
 	if (hdr->opcode & ISCSI_OP_IMMEDIATE) {
 		pr_err("Illegally set Immediate Bit in iSCSI Initiator"
 				" Scsi Command PDU.\n");
-		return iscsit_add_reject(ISCSI_REASON_BOOKMARK_INVALID, 1,
-				buf, conn);
+		return iscsit_add_reject_from_cmd(ISCSI_REASON_BOOKMARK_INVALID,
+				1, 1, buf, cmd);
 	}
 
 	if (payload_length && !conn->sess->sess_ops->ImmediateData) {
 		pr_err("ImmediateData=No but DataSegmentLength=%u,"
 			" protocol error.\n", payload_length);
-		return iscsit_add_reject(ISCSI_REASON_PROTOCOL_ERROR, 1,
-				buf, conn);
+		return iscsit_add_reject_from_cmd(ISCSI_REASON_PROTOCOL_ERROR,
+				1, 1, buf, cmd);
 	}
 
 	if ((be32_to_cpu(hdr->data_length )== payload_length) &&
@@ -866,43 +905,38 @@ done:
 		pr_err("Expected Data Transfer Length and Length of"
 			" Immediate Data are the same, but ISCSI_FLAG_CMD_FINAL"
 			" bit is not set protocol error\n");
-		return iscsit_add_reject(ISCSI_REASON_PROTOCOL_ERROR, 1,
-				buf, conn);
+		return iscsit_add_reject_from_cmd(ISCSI_REASON_PROTOCOL_ERROR,
+				1, 1, buf, cmd);
 	}
 
 	if (payload_length > be32_to_cpu(hdr->data_length)) {
 		pr_err("DataSegmentLength: %u is greater than"
 			" EDTL: %u, protocol error.\n", payload_length,
 				hdr->data_length);
-		return iscsit_add_reject(ISCSI_REASON_PROTOCOL_ERROR, 1,
-				buf, conn);
+		return iscsit_add_reject_from_cmd(ISCSI_REASON_PROTOCOL_ERROR,
+				1, 1, buf, cmd);
 	}
 
 	if (payload_length > conn->conn_ops->MaxXmitDataSegmentLength) {
 		pr_err("DataSegmentLength: %u is greater than"
 			" MaxXmitDataSegmentLength: %u, protocol error.\n",
 			payload_length, conn->conn_ops->MaxXmitDataSegmentLength);
-		return iscsit_add_reject(ISCSI_REASON_PROTOCOL_ERROR, 1,
-				buf, conn);
+		return iscsit_add_reject_from_cmd(ISCSI_REASON_PROTOCOL_ERROR,
+				1, 1, buf, cmd);
 	}
 
 	if (payload_length > conn->sess->sess_ops->FirstBurstLength) {
 		pr_err("DataSegmentLength: %u is greater than"
 			" FirstBurstLength: %u, protocol error.\n",
 			payload_length, conn->sess->sess_ops->FirstBurstLength);
-		return iscsit_add_reject(ISCSI_REASON_BOOKMARK_INVALID, 1,
-					buf, conn);
+		return iscsit_add_reject_from_cmd(ISCSI_REASON_BOOKMARK_INVALID,
+				1, 1, buf, cmd);
 	}
 
 	data_direction = (hdr->flags & ISCSI_FLAG_CMD_WRITE) ? DMA_TO_DEVICE :
 			 (hdr->flags & ISCSI_FLAG_CMD_READ) ? DMA_FROM_DEVICE :
 			  DMA_NONE;
 
-	cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
-	if (!cmd)
-		return iscsit_add_reject(ISCSI_REASON_BOOKMARK_NO_RESOURCES, 1,
-					 buf, conn);
-
 	cmd->data_direction = data_direction;
 	iscsi_task_attr = hdr->flags & ISCSI_FLAG_CMD_ATTR_MASK;
 	/*
@@ -945,7 +979,8 @@ done:
 	cmd->exp_stat_sn	= be32_to_cpu(hdr->exp_statsn);
 	cmd->first_burst_len	= payload_length;
 
-	if (cmd->data_direction == DMA_FROM_DEVICE) {
+	if (!conn->sess->sess_ops->RDMAExtensions &&
+	     cmd->data_direction == DMA_FROM_DEVICE) {
 		struct iscsi_datain_req *dr;
 
 		dr = iscsit_allocate_datain_req();
@@ -967,7 +1002,10 @@ done:
 
 	pr_debug("Got SCSI Command, ITT: 0x%08x, CmdSN: 0x%08x,"
 		" ExpXferLen: %u, Length: %u, CID: %hu\n", hdr->itt,
-		hdr->cmdsn, hdr->data_length, payload_length, conn->cid);
+		hdr->cmdsn, be32_to_cpu(hdr->data_length), payload_length,
+		conn->cid);
+
+	target_get_sess_cmd(conn->sess->se_sess, &cmd->se_cmd, true);
 
 	cmd->sense_reason = transport_lookup_cmd_lun(&cmd->se_cmd,
 						     scsilun_to_int(&hdr->lun));
@@ -1001,12 +1039,24 @@ attach_cmd:
 	 */
 	core_alua_check_nonop_delay(&cmd->se_cmd);
 
-	if (iscsit_allocate_iovecs(cmd) < 0) {
-		return iscsit_add_reject_from_cmd(
-				ISCSI_REASON_BOOKMARK_NO_RESOURCES,
-				1, 0, buf, cmd);
-	}
+	return 0;
+}
+EXPORT_SYMBOL(iscsit_setup_scsi_cmd);
+
+void iscsit_set_unsoliticed_dataout(struct iscsi_cmd *cmd)
+{
+	iscsit_set_dataout_sequence_values(cmd);
+
+	spin_lock_bh(&cmd->dataout_timeout_lock);
+	iscsit_start_dataout_timer(cmd, cmd->conn);
+	spin_unlock_bh(&cmd->dataout_timeout_lock);
+}
+EXPORT_SYMBOL(iscsit_set_unsoliticed_dataout);
 
+int iscsit_process_scsi_cmd(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
+			    struct iscsi_scsi_req *hdr)
+{
+	int cmdsn_ret = 0;
 	/*
 	 * Check the CmdSN against ExpCmdSN/MaxCmdSN here if
 	 * the Immediate Bit is not set, and no Immediate
@@ -1019,12 +1069,17 @@ attach_cmd:
 	 */
 	if (!cmd->immediate_data) {
 		cmdsn_ret = iscsit_sequence_cmd(conn, cmd, hdr->cmdsn);
-		if (cmdsn_ret == CMDSN_LOWER_THAN_EXP)
+		if (cmdsn_ret == CMDSN_LOWER_THAN_EXP) {
+			if (!cmd->sense_reason)
+				return 0;
+
+			target_put_sess_cmd(conn->sess->se_sess, &cmd->se_cmd);
 			return 0;
-		else if (cmdsn_ret == CMDSN_ERROR_CANNOT_RECOVER)
+		} else if (cmdsn_ret == CMDSN_ERROR_CANNOT_RECOVER) {
 			return iscsit_add_reject_from_cmd(
 				ISCSI_REASON_PROTOCOL_ERROR,
-				1, 0, buf, cmd);
+				1, 0, (unsigned char *)hdr, cmd);
+		}
 	}
 
 	iscsit_ack_from_expstatsn(conn, be32_to_cpu(hdr->exp_statsn));
@@ -1033,25 +1088,23 @@ attach_cmd:
 	 * If no Immediate Data is attached, it's OK to return now.
 	 */
 	if (!cmd->immediate_data) {
-		if (!cmd->sense_reason && cmd->unsolicited_data) {
-			iscsit_set_dataout_sequence_values(cmd);
-
-			spin_lock_bh(&cmd->dataout_timeout_lock);
-			iscsit_start_dataout_timer(cmd, cmd->conn);
-			spin_unlock_bh(&cmd->dataout_timeout_lock);
-		}
+		if (!cmd->sense_reason && cmd->unsolicited_data)
+			iscsit_set_unsoliticed_dataout(cmd);
+		if (!cmd->sense_reason)
+			return 0;
 
+		target_put_sess_cmd(conn->sess->se_sess, &cmd->se_cmd);
 		return 0;
 	}
 
 	/*
-	 * Early CHECK_CONDITIONs never make it to the transport processing
-	 * thread.  They are processed in CmdSN order by
-	 * iscsit_check_received_cmdsn() below.
+	 * Early CHECK_CONDITIONs with ImmediateData never make it to command
+	 * execution.  These exceptions are processed in CmdSN order using
+	 * iscsit_check_received_cmdsn() in iscsit_get_immediate_data() below.
 	 */
 	if (cmd->sense_reason) {
-		immed_ret = IMMEDIATE_DATA_NORMAL_OPERATION;
-		goto after_immediate_data;
+		target_put_sess_cmd(conn->sess->se_sess, &cmd->se_cmd);
+		return 1;
 	}
 	/*
 	 * Call directly into transport_generic_new_cmd() to perform
@@ -1059,11 +1112,27 @@ attach_cmd:
 	 */
 	cmd->sense_reason = transport_generic_new_cmd(&cmd->se_cmd);
 	if (cmd->sense_reason) {
-		immed_ret = IMMEDIATE_DATA_NORMAL_OPERATION;
-		goto after_immediate_data;
+		target_put_sess_cmd(conn->sess->se_sess, &cmd->se_cmd);
+		return 1;
 	}
 
-	immed_ret = iscsit_handle_immediate_data(cmd, buf, payload_length);
+	return 0;
+}
+EXPORT_SYMBOL(iscsit_process_scsi_cmd);
+
+static int
+iscsit_get_immediate_data(struct iscsi_cmd *cmd, struct iscsi_scsi_req *hdr,
+			  bool dump_payload)
+{
+	int cmdsn_ret = 0, immed_ret = IMMEDIATE_DATA_NORMAL_OPERATION;
+	/*
+	 * Special case for Unsupported SAM WRITE Opcodes and ImmediateData=Yes.
+	 */
+	if (dump_payload == true)
+		goto after_immediate_data;
+
+	immed_ret = iscsit_handle_immediate_data(cmd, hdr,
+					cmd->first_burst_len);
 after_immediate_data:
 	if (immed_ret == IMMEDIATE_DATA_NORMAL_OPERATION) {
 		/*
@@ -1071,26 +1140,19 @@ after_immediate_data:
 		 * DataCRC, check against ExpCmdSN/MaxCmdSN if
 		 * Immediate Bit is not set.
 		 */
-		cmdsn_ret = iscsit_sequence_cmd(conn, cmd, hdr->cmdsn);
-		/*
-		 * Special case for Unsupported SAM WRITE Opcodes
-		 * and ImmediateData=Yes.
-		 */
+		cmdsn_ret = iscsit_sequence_cmd(cmd->conn, cmd, hdr->cmdsn);
+
 		if (cmd->sense_reason) {
-			if (iscsit_dump_data_payload(conn, payload_length, 1) < 0)
+			if (iscsit_dump_data_payload(cmd->conn,
+					cmd->first_burst_len, 1) < 0)
 				return -1;
-		} else if (cmd->unsolicited_data) {
-			iscsit_set_dataout_sequence_values(cmd);
-
-			spin_lock_bh(&cmd->dataout_timeout_lock);
-			iscsit_start_dataout_timer(cmd, cmd->conn);
-			spin_unlock_bh(&cmd->dataout_timeout_lock);
-		}
+		} else if (cmd->unsolicited_data)
+			iscsit_set_unsoliticed_dataout(cmd);
 
 		if (cmdsn_ret == CMDSN_ERROR_CANNOT_RECOVER)
 			return iscsit_add_reject_from_cmd(
 				ISCSI_REASON_PROTOCOL_ERROR,
-				1, 0, buf, cmd);
+				1, 0, (unsigned char *)hdr, cmd);
 
 	} else if (immed_ret == IMMEDIATE_DATA_ERL1_CRC_FAILURE) {
 		/*
@@ -1105,13 +1167,47 @@ after_immediate_data:
 		 * CmdSN and issue a retry to plug the sequence.
 		 */
 		cmd->i_state = ISTATE_REMOVE;
-		iscsit_add_cmd_to_immediate_queue(cmd, conn, cmd->i_state);
+		iscsit_add_cmd_to_immediate_queue(cmd, cmd->conn, cmd->i_state);
 	} else /* immed_ret == IMMEDIATE_DATA_CANNOT_RECOVER */
 		return -1;
 
 	return 0;
 }
 
+static int
+iscsit_handle_scsi_cmd(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
+			   unsigned char *buf)
+{
+	struct iscsi_scsi_req *hdr = (struct iscsi_scsi_req *)buf;
+	int rc, immed_data;
+	bool dump_payload = false;
+
+	rc = iscsit_setup_scsi_cmd(conn, cmd, buf);
+	if (rc < 0)
+		return rc;
+	/*
+	 * Allocation iovecs needed for struct socket operations for
+	 * traditional iSCSI block I/O.
+	 */
+	if (iscsit_allocate_iovecs(cmd) < 0) {
+		return iscsit_add_reject_from_cmd(
+				ISCSI_REASON_BOOKMARK_NO_RESOURCES,
+				1, 0, buf, cmd);
+	}
+	immed_data = cmd->immediate_data;
+
+	rc = iscsit_process_scsi_cmd(conn, cmd, hdr);
+	if (rc < 0)
+		return rc;
+	else if (rc > 0)
+		dump_payload = true;
+
+	if (!immed_data)
+		return 0;
+
+	return iscsit_get_immediate_data(cmd, hdr, dump_payload);
+}
+
 static u32 iscsit_do_crypto_hash_sg(
 	struct hash_desc *hash,
 	struct iscsi_cmd *cmd,
@@ -1174,20 +1270,16 @@ static void iscsit_do_crypto_hash_buf(
 	crypto_hash_final(hash, data_crc);
 }
 
-static int iscsit_handle_data_out(struct iscsi_conn *conn, unsigned char *buf)
+int
+iscsit_check_dataout_hdr(struct iscsi_conn *conn, unsigned char *buf,
+			  struct iscsi_cmd **out_cmd)
 {
-	int iov_ret, ooo_cmdsn = 0, ret;
-	u8 data_crc_failed = 0;
-	u32 checksum, iov_count = 0, padding = 0, rx_got = 0;
-	u32 rx_size = 0, payload_length;
+	struct iscsi_data *hdr = (struct iscsi_data *)buf;
 	struct iscsi_cmd *cmd = NULL;
 	struct se_cmd *se_cmd;
-	struct iscsi_data *hdr;
-	struct kvec *iov;
 	unsigned long flags;
-
-	hdr			= (struct iscsi_data *) buf;
-	payload_length		= ntoh24(hdr->dlength);
+	u32 payload_length = ntoh24(hdr->dlength);
+	int rc;
 
 	if (!payload_length) {
 		pr_err("DataOUT payload is ZERO, protocol error.\n");
@@ -1220,7 +1312,7 @@ static int iscsit_handle_data_out(struct iscsi_conn *conn, unsigned char *buf)
 
 	pr_debug("Got DataOut ITT: 0x%08x, TTT: 0x%08x,"
 		" DataSN: 0x%08x, Offset: %u, Length: %u, CID: %hu\n",
-		hdr->itt, hdr->ttt, hdr->datasn, hdr->offset,
+		hdr->itt, hdr->ttt, hdr->datasn, ntohl(hdr->offset),
 		payload_length, conn->cid);
 
 	if (cmd->cmd_flags & ICF_GOT_LAST_DATAOUT) {
@@ -1312,12 +1404,26 @@ static int iscsit_handle_data_out(struct iscsi_conn *conn, unsigned char *buf)
 	 * Preform DataSN, DataSequenceInOrder, DataPDUInOrder, and
 	 * within-command recovery checks before receiving the payload.
 	 */
-	ret = iscsit_check_pre_dataout(cmd, buf);
-	if (ret == DATAOUT_WITHIN_COMMAND_RECOVERY)
+	rc = iscsit_check_pre_dataout(cmd, buf);
+	if (rc == DATAOUT_WITHIN_COMMAND_RECOVERY)
 		return 0;
-	else if (ret == DATAOUT_CANNOT_RECOVER)
+	else if (rc == DATAOUT_CANNOT_RECOVER)
 		return -1;
 
+	*out_cmd = cmd;
+	return 0;
+}
+EXPORT_SYMBOL(iscsit_check_dataout_hdr);
+
+static int
+iscsit_get_dataout(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
+		   struct iscsi_data *hdr)
+{
+	struct kvec *iov;
+	u32 checksum, iov_count = 0, padding = 0, rx_got = 0, rx_size = 0;
+	u32 payload_length = ntoh24(hdr->dlength);
+	int iov_ret, data_crc_failed = 0;
+
 	rx_size += payload_length;
 	iov = &cmd->iov_data[0];
 
@@ -1370,17 +1476,27 @@ static int iscsit_handle_data_out(struct iscsi_conn *conn, unsigned char *buf)
 				payload_length);
 		}
 	}
+
+	return data_crc_failed;
+}
+
+int
+iscsit_check_dataout_payload(struct iscsi_cmd *cmd, struct iscsi_data *hdr,
+			     bool data_crc_failed)
+{
+	struct iscsi_conn *conn = cmd->conn;
+	int rc, ooo_cmdsn;
 	/*
 	 * Increment post receive data and CRC values or perform
 	 * within-command recovery.
 	 */
-	ret = iscsit_check_post_dataout(cmd, buf, data_crc_failed);
-	if ((ret == DATAOUT_NORMAL) || (ret == DATAOUT_WITHIN_COMMAND_RECOVERY))
+	rc = iscsit_check_post_dataout(cmd, (unsigned char *)hdr, data_crc_failed);
+	if ((rc == DATAOUT_NORMAL) || (rc == DATAOUT_WITHIN_COMMAND_RECOVERY))
 		return 0;
-	else if (ret == DATAOUT_SEND_R2T) {
+	else if (rc == DATAOUT_SEND_R2T) {
 		iscsit_set_dataout_sequence_values(cmd);
-		iscsit_build_r2ts_for_cmd(cmd, conn, false);
-	} else if (ret == DATAOUT_SEND_TO_TRANSPORT) {
+		conn->conn_transport->iscsit_get_dataout(conn, cmd, false);
+	} else if (rc == DATAOUT_SEND_TO_TRANSPORT) {
 		/*
 		 * Handle extra special case for out of order
 		 * Unsolicited Data Out.
@@ -1401,15 +1517,37 @@ static int iscsit_handle_data_out(struct iscsi_conn *conn, unsigned char *buf)
 
 	return 0;
 }
+EXPORT_SYMBOL(iscsit_check_dataout_payload);
 
-static int iscsit_handle_nop_out(
-	struct iscsi_conn *conn,
-	unsigned char *buf)
+static int iscsit_handle_data_out(struct iscsi_conn *conn, unsigned char *buf)
+{
+	struct iscsi_cmd *cmd;
+	struct iscsi_data *hdr = (struct iscsi_data *)buf;
+	int rc;
+	bool data_crc_failed = false;
+
+	rc = iscsit_check_dataout_hdr(conn, buf, &cmd);
+	if (rc < 0)
+		return rc;
+	else if (!cmd)
+		return 0;
+
+	rc = iscsit_get_dataout(conn, cmd, hdr);
+	if (rc < 0)
+		return rc;
+	else if (rc > 0)
+		data_crc_failed = true;
+
+	return iscsit_check_dataout_payload(cmd, hdr, data_crc_failed);
+}
+
+int iscsit_handle_nop_out(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
+			unsigned char *buf)
 {
 	unsigned char *ping_data = NULL;
 	int cmdsn_ret, niov = 0, ret = 0, rx_got, rx_size;
 	u32 checksum, data_crc, padding = 0, payload_length;
-	struct iscsi_cmd *cmd = NULL;
+	struct iscsi_cmd *cmd_p = NULL;
 	struct kvec *iov = NULL;
 	struct iscsi_nopout *hdr;
 
@@ -1432,7 +1570,7 @@ static int iscsit_handle_nop_out(
 					buf, conn);
 	}
 
-	pr_debug("Got NOPOUT Ping %s ITT: 0x%08x, TTT: 0x%09x,"
+	pr_debug("Got NOPOUT Ping %s ITT: 0x%08x, TTT: 0x%08x,"
 		" CmdSN: 0x%08x, ExpStatSN: 0x%08x, Length: %u\n",
 		hdr->itt == RESERVED_ITT ? "Response" : "Request",
 		hdr->itt, hdr->ttt, hdr->cmdsn, hdr->exp_statsn,
@@ -1445,7 +1583,6 @@ static int iscsit_handle_nop_out(
 	 * can contain ping data.
 	 */
 	if (hdr->ttt == cpu_to_be32(0xFFFFFFFF)) {
-		cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
 		if (!cmd)
 			return iscsit_add_reject(
 					ISCSI_REASON_BOOKMARK_NO_RESOURCES,
@@ -1580,14 +1717,14 @@ static int iscsit_handle_nop_out(
 		/*
 		 * This was a response to a unsolicited NOPIN ping.
 		 */
-		cmd = iscsit_find_cmd_from_ttt(conn, be32_to_cpu(hdr->ttt));
-		if (!cmd)
+		cmd_p = iscsit_find_cmd_from_ttt(conn, be32_to_cpu(hdr->ttt));
+		if (!cmd_p)
 			return -1;
 
 		iscsit_stop_nopin_response_timer(conn);
 
-		cmd->i_state = ISTATE_REMOVE;
-		iscsit_add_cmd_to_immediate_queue(cmd, conn, cmd->i_state);
+		cmd_p->i_state = ISTATE_REMOVE;
+		iscsit_add_cmd_to_immediate_queue(cmd_p, conn, cmd_p->i_state);
 		iscsit_start_nopin_timer(conn);
 	} else {
 		/*
@@ -1611,12 +1748,12 @@ ping_out:
 	kfree(ping_data);
 	return ret;
 }
+EXPORT_SYMBOL(iscsit_handle_nop_out);
 
-static int iscsit_handle_task_mgt_cmd(
-	struct iscsi_conn *conn,
-	unsigned char *buf)
+int
+iscsit_handle_task_mgt_cmd(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
+			   unsigned char *buf)
 {
-	struct iscsi_cmd *cmd;
 	struct se_tmr_req *se_tmr;
 	struct iscsi_tmr_req *tmr_req;
 	struct iscsi_tm *hdr;
@@ -1645,18 +1782,13 @@ static int iscsit_handle_task_mgt_cmd(
 		pr_err("Task Management Request TASK_REASSIGN not"
 			" issued as immediate command, bad iSCSI Initiator"
 				"implementation\n");
-		return iscsit_add_reject(ISCSI_REASON_PROTOCOL_ERROR, 1,
-					buf, conn);
+		return iscsit_add_reject_from_cmd(ISCSI_REASON_PROTOCOL_ERROR,
+					1, 1, buf, cmd);
 	}
 	if ((function != ISCSI_TM_FUNC_ABORT_TASK) &&
 	    be32_to_cpu(hdr->refcmdsn) != ISCSI_RESERVED_TAG)
 		hdr->refcmdsn = cpu_to_be32(ISCSI_RESERVED_TAG);
 
-	cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
-	if (!cmd)
-		return iscsit_add_reject(ISCSI_REASON_BOOKMARK_NO_RESOURCES,
-					 1, buf, conn);
-
 	cmd->data_direction = DMA_NONE;
 
 	cmd->tmr_req = kzalloc(sizeof(struct iscsi_tmr_req), GFP_KERNEL);
@@ -1827,6 +1959,7 @@ attach:
 	iscsit_add_cmd_to_response_queue(cmd, conn, cmd->i_state);
 	return 0;
 }
+EXPORT_SYMBOL(iscsit_handle_task_mgt_cmd);
 
 /* #warning FIXME: Support Text Command parameters besides SendTargets */
 static int iscsit_handle_text_cmd(
@@ -2089,13 +2222,12 @@ int iscsit_logout_removeconnforrecovery(struct iscsi_cmd *cmd, struct iscsi_conn
 	return 0;
 }
 
-static int iscsit_handle_logout_cmd(
-	struct iscsi_conn *conn,
-	unsigned char *buf)
+int
+iscsit_handle_logout_cmd(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
+			unsigned char *buf)
 {
 	int cmdsn_ret, logout_remove = 0;
 	u8 reason_code = 0;
-	struct iscsi_cmd *cmd;
 	struct iscsi_logout *hdr;
 	struct iscsi_tiqn *tiqn = iscsit_snmp_get_tiqn(conn);
 
@@ -2119,14 +2251,10 @@ static int iscsit_handle_logout_cmd(
 	if (conn->conn_state != TARG_CONN_STATE_LOGGED_IN) {
 		pr_err("Received logout request on connection that"
 			" is not in logged in state, ignoring request.\n");
+		iscsit_release_cmd(cmd);
 		return 0;
 	}
 
-	cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
-	if (!cmd)
-		return iscsit_add_reject(ISCSI_REASON_BOOKMARK_NO_RESOURCES, 1,
-					buf, conn);
-
 	cmd->iscsi_opcode       = ISCSI_OP_LOGOUT;
 	cmd->i_state            = ISTATE_SEND_LOGOUTRSP;
 	cmd->immediate_cmd      = ((hdr->opcode & ISCSI_OP_IMMEDIATE) ? 1 : 0);
@@ -2176,6 +2304,7 @@ static int iscsit_handle_logout_cmd(
 
 	return logout_remove;
 }
+EXPORT_SYMBOL(iscsit_handle_logout_cmd);
 
 static int iscsit_handle_snack(
 	struct iscsi_conn *conn,
@@ -2243,7 +2372,7 @@ static void iscsit_rx_thread_wait_for_tcp(struct iscsi_conn *conn)
 
 static int iscsit_handle_immediate_data(
 	struct iscsi_cmd *cmd,
-	unsigned char *buf,
+	struct iscsi_scsi_req *hdr,
 	u32 length)
 {
 	int iov_ret, rx_got = 0, rx_size = 0;
@@ -2299,12 +2428,12 @@ static int iscsit_handle_immediate_data(
 					" in ERL=0.\n");
 				iscsit_add_reject_from_cmd(
 						ISCSI_REASON_DATA_DIGEST_ERROR,
-						1, 0, buf, cmd);
+						1, 0, (unsigned char *)hdr, cmd);
 				return IMMEDIATE_DATA_CANNOT_RECOVER;
 			} else {
 				iscsit_add_reject_from_cmd(
 						ISCSI_REASON_DATA_DIGEST_ERROR,
-						0, 0, buf, cmd);
+						0, 0, (unsigned char *)hdr, cmd);
 				return IMMEDIATE_DATA_ERL1_CRC_FAILURE;
 			}
 		} else {
@@ -2424,18 +2553,60 @@ static void iscsit_tx_thread_wait_for_tcp(struct iscsi_conn *conn)
 	}
 }
 
-static int iscsit_send_data_in(
-	struct iscsi_cmd *cmd,
-	struct iscsi_conn *conn)
+static void
+iscsit_build_datain_pdu(struct iscsi_cmd *cmd, struct iscsi_conn *conn,
+			struct iscsi_datain *datain, struct iscsi_data_rsp *hdr,
+			bool set_statsn)
 {
-	int iov_ret = 0, set_statsn = 0;
-	u32 iov_count = 0, tx_size = 0;
+	hdr->opcode		= ISCSI_OP_SCSI_DATA_IN;
+	hdr->flags		= datain->flags;
+	if (hdr->flags & ISCSI_FLAG_DATA_STATUS) {
+		if (cmd->se_cmd.se_cmd_flags & SCF_OVERFLOW_BIT) {
+			hdr->flags |= ISCSI_FLAG_DATA_OVERFLOW;
+			hdr->residual_count = cpu_to_be32(cmd->se_cmd.residual_count);
+		} else if (cmd->se_cmd.se_cmd_flags & SCF_UNDERFLOW_BIT) {
+			hdr->flags |= ISCSI_FLAG_DATA_UNDERFLOW;
+			hdr->residual_count = cpu_to_be32(cmd->se_cmd.residual_count);
+		}
+	}
+	hton24(hdr->dlength, datain->length);
+	if (hdr->flags & ISCSI_FLAG_DATA_ACK)
+		int_to_scsilun(cmd->se_cmd.orig_fe_lun,
+				(struct scsi_lun *)&hdr->lun);
+	else
+		put_unaligned_le64(0xFFFFFFFFFFFFFFFFULL, &hdr->lun);
+
+	hdr->itt		= cmd->init_task_tag;
+
+	if (hdr->flags & ISCSI_FLAG_DATA_ACK)
+		hdr->ttt		= cpu_to_be32(cmd->targ_xfer_tag);
+	else
+		hdr->ttt		= cpu_to_be32(0xFFFFFFFF);
+	if (set_statsn)
+		hdr->statsn		= cpu_to_be32(cmd->stat_sn);
+	else
+		hdr->statsn		= cpu_to_be32(0xFFFFFFFF);
+
+	hdr->exp_cmdsn		= cpu_to_be32(conn->sess->exp_cmd_sn);
+	hdr->max_cmdsn		= cpu_to_be32(conn->sess->max_cmd_sn);
+	hdr->datasn		= cpu_to_be32(datain->data_sn);
+	hdr->offset		= cpu_to_be32(datain->offset);
+
+	pr_debug("Built DataIN ITT: 0x%08x, StatSN: 0x%08x,"
+		" DataSN: 0x%08x, Offset: %u, Length: %u, CID: %hu\n",
+		cmd->init_task_tag, ntohl(hdr->statsn), ntohl(hdr->datasn),
+		ntohl(hdr->offset), datain->length, conn->cid);
+}
+
+static int iscsit_send_datain(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
+{
+	struct iscsi_data_rsp *hdr = (struct iscsi_data_rsp *)&cmd->pdu[0];
 	struct iscsi_datain datain;
 	struct iscsi_datain_req *dr;
-	struct iscsi_data_rsp *hdr;
 	struct kvec *iov;
-	int eodr = 0;
-	int ret;
+	u32 iov_count = 0, tx_size = 0;
+	int eodr = 0, ret, iov_ret;
+	bool set_statsn = false;
 
 	memset(&datain, 0, sizeof(struct iscsi_datain));
 	dr = iscsit_get_datain_values(cmd, &datain);
@@ -2444,7 +2615,6 @@ static int iscsit_send_data_in(
 				cmd->init_task_tag);
 		return -1;
 	}
-
 	/*
 	 * Be paranoid and double check the logic for now.
 	 */
@@ -2452,7 +2622,7 @@ static int iscsit_send_data_in(
 		pr_err("Command ITT: 0x%08x, datain.offset: %u and"
 			" datain.length: %u exceeds cmd->data_length: %u\n",
 			cmd->init_task_tag, datain.offset, datain.length,
-				cmd->se_cmd.data_length);
+			cmd->se_cmd.data_length);
 		return -1;
 	}
 
@@ -2476,47 +2646,13 @@ static int iscsit_send_data_in(
 		    (dr->dr_complete == DATAIN_COMPLETE_CONNECTION_RECOVERY)) {
 			iscsit_increment_maxcmdsn(cmd, conn->sess);
 			cmd->stat_sn = conn->stat_sn++;
-			set_statsn = 1;
+			set_statsn = true;
 		} else if (dr->dr_complete ==
-				DATAIN_COMPLETE_WITHIN_COMMAND_RECOVERY)
-			set_statsn = 1;
+			   DATAIN_COMPLETE_WITHIN_COMMAND_RECOVERY)
+			set_statsn = true;
 	}
 
-	hdr	= (struct iscsi_data_rsp *) cmd->pdu;
-	memset(hdr, 0, ISCSI_HDR_LEN);
-	hdr->opcode		= ISCSI_OP_SCSI_DATA_IN;
-	hdr->flags		= datain.flags;
-	if (hdr->flags & ISCSI_FLAG_DATA_STATUS) {
-		if (cmd->se_cmd.se_cmd_flags & SCF_OVERFLOW_BIT) {
-			hdr->flags |= ISCSI_FLAG_DATA_OVERFLOW;
-			hdr->residual_count = cpu_to_be32(cmd->se_cmd.residual_count);
-		} else if (cmd->se_cmd.se_cmd_flags & SCF_UNDERFLOW_BIT) {
-			hdr->flags |= ISCSI_FLAG_DATA_UNDERFLOW;
-			hdr->residual_count = cpu_to_be32(cmd->se_cmd.residual_count);
-		}
-	}
-	hton24(hdr->dlength, datain.length);
-	if (hdr->flags & ISCSI_FLAG_DATA_ACK)
-		int_to_scsilun(cmd->se_cmd.orig_fe_lun,
-				(struct scsi_lun *)&hdr->lun);
-	else
-		put_unaligned_le64(0xFFFFFFFFFFFFFFFFULL, &hdr->lun);
-
-	hdr->itt		= cmd->init_task_tag;
-
-	if (hdr->flags & ISCSI_FLAG_DATA_ACK)
-		hdr->ttt		= cpu_to_be32(cmd->targ_xfer_tag);
-	else
-		hdr->ttt		= cpu_to_be32(0xFFFFFFFF);
-	if (set_statsn)
-		hdr->statsn		= cpu_to_be32(cmd->stat_sn);
-	else
-		hdr->statsn		= cpu_to_be32(0xFFFFFFFF);
-
-	hdr->exp_cmdsn		= cpu_to_be32(conn->sess->exp_cmd_sn);
-	hdr->max_cmdsn		= cpu_to_be32(conn->sess->max_cmd_sn);
-	hdr->datasn		= cpu_to_be32(datain.data_sn);
-	hdr->offset		= cpu_to_be32(datain.offset);
+	iscsit_build_datain_pdu(cmd, conn, &datain, hdr, set_statsn);
 
 	iov = &cmd->iov_data[0];
 	iov[iov_count].iov_base	= cmd->pdu;
@@ -2527,7 +2663,7 @@ static int iscsit_send_data_in(
 		u32 *header_digest = (u32 *)&cmd->pdu[ISCSI_HDR_LEN];
 
 		iscsit_do_crypto_hash_buf(&conn->conn_tx_hash,
-				(unsigned char *)hdr, ISCSI_HDR_LEN,
+				(unsigned char *)cmd->pdu, ISCSI_HDR_LEN,
 				0, NULL, (u8 *)header_digest);
 
 		iov[0].iov_len += ISCSI_CRC_LEN;
@@ -2537,7 +2673,8 @@ static int iscsit_send_data_in(
 			" for DataIN PDU 0x%08x\n", *header_digest);
 	}
 
-	iov_ret = iscsit_map_iovec(cmd, &cmd->iov_data[1], datain.offset, datain.length);
+	iov_ret = iscsit_map_iovec(cmd, &cmd->iov_data[1],
+				datain.offset, datain.length);
 	if (iov_ret < 0)
 		return -1;
 
@@ -2568,11 +2705,6 @@ static int iscsit_send_data_in(
 	cmd->iov_data_count = iov_count;
 	cmd->tx_size = tx_size;
 
-	pr_debug("Built DataIN ITT: 0x%08x, StatSN: 0x%08x,"
-		" DataSN: 0x%08x, Offset: %u, Length: %u, CID: %hu\n",
-		cmd->init_task_tag, ntohl(hdr->statsn), ntohl(hdr->datasn),
-		ntohl(hdr->offset), datain.length, conn->cid);
-
 	/* sendpage is preferred but can't insert markers */
 	if (!conn->conn_ops->IFMarker)
 		ret = iscsit_fe_sendpage_sg(cmd, conn);
@@ -2595,16 +2727,13 @@ static int iscsit_send_data_in(
 	return eodr;
 }
 
-static int iscsit_send_logout_response(
-	struct iscsi_cmd *cmd,
-	struct iscsi_conn *conn)
+int
+iscsit_build_logout_rsp(struct iscsi_cmd *cmd, struct iscsi_conn *conn,
+			struct iscsi_logout_rsp *hdr)
 {
-	int niov = 0, tx_size;
 	struct iscsi_conn *logout_conn = NULL;
 	struct iscsi_conn_recovery *cr = NULL;
 	struct iscsi_session *sess = conn->sess;
-	struct kvec *iov;
-	struct iscsi_logout_rsp *hdr;
 	/*
 	 * The actual shutting down of Sessions and/or Connections
 	 * for CLOSESESSION and CLOSECONNECTION Logout Requests
@@ -2673,9 +2802,6 @@ static int iscsit_send_logout_response(
 		return -1;
 	}
 
-	tx_size = ISCSI_HDR_LEN;
-	hdr			= (struct iscsi_logout_rsp *)cmd->pdu;
-	memset(hdr, 0, ISCSI_HDR_LEN);
 	hdr->opcode		= ISCSI_OP_LOGOUT_RSP;
 	hdr->flags		|= ISCSI_FLAG_CMD_FINAL;
 	hdr->response		= cmd->logout_response;
@@ -2687,6 +2813,27 @@ static int iscsit_send_logout_response(
 	hdr->exp_cmdsn		= cpu_to_be32(conn->sess->exp_cmd_sn);
 	hdr->max_cmdsn		= cpu_to_be32(conn->sess->max_cmd_sn);
 
+	pr_debug("Built Logout Response ITT: 0x%08x StatSN:"
+		" 0x%08x Response: 0x%02x CID: %hu on CID: %hu\n",
+		cmd->init_task_tag, cmd->stat_sn, hdr->response,
+		cmd->logout_cid, conn->cid);
+
+	return 0;
+}
+EXPORT_SYMBOL(iscsit_build_logout_rsp);
+
+static int
+iscsit_send_logout(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
+{
+	struct kvec *iov;
+	int niov = 0, tx_size, rc;
+
+	rc = iscsit_build_logout_rsp(cmd, conn,
+			(struct iscsi_logout_rsp *)&cmd->pdu[0]);
+	if (rc < 0)
+		return rc;
+
+	tx_size = ISCSI_HDR_LEN;
 	iov = &cmd->iov_misc[0];
 	iov[niov].iov_base	= cmd->pdu;
 	iov[niov++].iov_len	= ISCSI_HDR_LEN;
@@ -2695,7 +2842,7 @@ static int iscsit_send_logout_response(
 		u32 *header_digest = (u32 *)&cmd->pdu[ISCSI_HDR_LEN];
 
 		iscsit_do_crypto_hash_buf(&conn->conn_tx_hash,
-				(unsigned char *)hdr, ISCSI_HDR_LEN,
+				(unsigned char *)&cmd->pdu[0], ISCSI_HDR_LEN,
 				0, NULL, (u8 *)header_digest);
 
 		iov[0].iov_len += ISCSI_CRC_LEN;
@@ -2706,14 +2853,37 @@ static int iscsit_send_logout_response(
 	cmd->iov_misc_count = niov;
 	cmd->tx_size = tx_size;
 
-	pr_debug("Sending Logout Response ITT: 0x%08x StatSN:"
-		" 0x%08x Response: 0x%02x CID: %hu on CID: %hu\n",
-		cmd->init_task_tag, cmd->stat_sn, hdr->response,
-		cmd->logout_cid, conn->cid);
-
 	return 0;
 }
 
+void
+iscsit_build_nopin_rsp(struct iscsi_cmd *cmd, struct iscsi_conn *conn,
+		       struct iscsi_nopin *hdr, bool nopout_response)
+{
+	hdr->opcode		= ISCSI_OP_NOOP_IN;
+	hdr->flags		|= ISCSI_FLAG_CMD_FINAL;
+        hton24(hdr->dlength, cmd->buf_ptr_size);
+	if (nopout_response)
+		put_unaligned_le64(0xFFFFFFFFFFFFFFFFULL, &hdr->lun);
+	hdr->itt		= cmd->init_task_tag;
+	hdr->ttt		= cpu_to_be32(cmd->targ_xfer_tag);
+	cmd->stat_sn		= (nopout_response) ? conn->stat_sn++ :
+				  conn->stat_sn;
+	hdr->statsn		= cpu_to_be32(cmd->stat_sn);
+
+	if (nopout_response)
+		iscsit_increment_maxcmdsn(cmd, conn->sess);
+
+	hdr->exp_cmdsn		= cpu_to_be32(conn->sess->exp_cmd_sn);
+	hdr->max_cmdsn		= cpu_to_be32(conn->sess->max_cmd_sn);
+
+	pr_debug("Built NOPIN %s Response ITT: 0x%08x, TTT: 0x%08x,"
+		" StatSN: 0x%08x, Length %u\n", (nopout_response) ?
+		"Solicitied" : "Unsolicitied", cmd->init_task_tag,
+		cmd->targ_xfer_tag, cmd->stat_sn, cmd->buf_ptr_size);
+}
+EXPORT_SYMBOL(iscsit_build_nopin_rsp);
+
 /*
  *	Unsolicited NOPIN, either requesting a response or not.
  */
@@ -2722,20 +2892,10 @@ static int iscsit_send_unsolicited_nopin(
 	struct iscsi_conn *conn,
 	int want_response)
 {
-	int tx_size = ISCSI_HDR_LEN;
-	struct iscsi_nopin *hdr;
-	int ret;
+	struct iscsi_nopin *hdr = (struct iscsi_nopin *)&cmd->pdu[0];
+	int tx_size = ISCSI_HDR_LEN, ret;
 
-	hdr			= (struct iscsi_nopin *) cmd->pdu;
-	memset(hdr, 0, ISCSI_HDR_LEN);
-	hdr->opcode		= ISCSI_OP_NOOP_IN;
-	hdr->flags		|= ISCSI_FLAG_CMD_FINAL;
-	hdr->itt		= cmd->init_task_tag;
-	hdr->ttt		= cpu_to_be32(cmd->targ_xfer_tag);
-	cmd->stat_sn		= conn->stat_sn;
-	hdr->statsn		= cpu_to_be32(cmd->stat_sn);
-	hdr->exp_cmdsn		= cpu_to_be32(conn->sess->exp_cmd_sn);
-	hdr->max_cmdsn		= cpu_to_be32(conn->sess->max_cmd_sn);
+	iscsit_build_nopin_rsp(cmd, conn, hdr, false);
 
 	if (conn->conn_ops->HeaderDigest) {
 		u32 *header_digest = (u32 *)&cmd->pdu[ISCSI_HDR_LEN];
@@ -2771,31 +2931,17 @@ static int iscsit_send_unsolicited_nopin(
 	return 0;
 }
 
-static int iscsit_send_nopin_response(
-	struct iscsi_cmd *cmd,
-	struct iscsi_conn *conn)
+static int
+iscsit_send_nopin(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
 {
-	int niov = 0, tx_size;
-	u32 padding = 0;
+	struct iscsi_nopin *hdr = (struct iscsi_nopin *)&cmd->pdu[0];
 	struct kvec *iov;
-	struct iscsi_nopin *hdr;
-
-	tx_size = ISCSI_HDR_LEN;
-	hdr			= (struct iscsi_nopin *) cmd->pdu;
-	memset(hdr, 0, ISCSI_HDR_LEN);
-	hdr->opcode		= ISCSI_OP_NOOP_IN;
-	hdr->flags		|= ISCSI_FLAG_CMD_FINAL;
-	hton24(hdr->dlength, cmd->buf_ptr_size);
-	put_unaligned_le64(0xFFFFFFFFFFFFFFFFULL, &hdr->lun);
-	hdr->itt		= cmd->init_task_tag;
-	hdr->ttt		= cpu_to_be32(cmd->targ_xfer_tag);
-	cmd->stat_sn		= conn->stat_sn++;
-	hdr->statsn		= cpu_to_be32(cmd->stat_sn);
+	u32 padding = 0;
+	int niov = 0, tx_size;
 
-	iscsit_increment_maxcmdsn(cmd, conn->sess);
-	hdr->exp_cmdsn		= cpu_to_be32(conn->sess->exp_cmd_sn);
-	hdr->max_cmdsn		= cpu_to_be32(conn->sess->max_cmd_sn);
+	iscsit_build_nopin_rsp(cmd, conn, hdr, true);
 
+	tx_size = ISCSI_HDR_LEN;
 	iov = &cmd->iov_misc[0];
 	iov[niov].iov_base	= cmd->pdu;
 	iov[niov++].iov_len	= ISCSI_HDR_LEN;
@@ -2851,10 +2997,6 @@ static int iscsit_send_nopin_response(
 	cmd->iov_misc_count = niov;
 	cmd->tx_size = tx_size;
 
-	pr_debug("Sending NOPIN Response ITT: 0x%08x, TTT:"
-		" 0x%08x, StatSN: 0x%08x, Length %u\n", cmd->init_task_tag,
-		cmd->targ_xfer_tag, cmd->stat_sn, cmd->buf_ptr_size);
-
 	return 0;
 }
 
@@ -2939,8 +3081,8 @@ static int iscsit_send_r2t(
  *		connection recovery.
  */
 int iscsit_build_r2ts_for_cmd(
-	struct iscsi_cmd *cmd,
 	struct iscsi_conn *conn,
+	struct iscsi_cmd *cmd,
 	bool recovery)
 {
 	int first_r2t = 1;
@@ -3015,24 +3157,16 @@ int iscsit_build_r2ts_for_cmd(
 	return 0;
 }
 
-static int iscsit_send_status(
-	struct iscsi_cmd *cmd,
-	struct iscsi_conn *conn)
+void iscsit_build_rsp_pdu(struct iscsi_cmd *cmd, struct iscsi_conn *conn,
+			bool inc_stat_sn, struct iscsi_scsi_rsp *hdr)
 {
-	u8 iov_count = 0, recovery;
-	u32 padding = 0, tx_size = 0;
-	struct iscsi_scsi_rsp *hdr;
-	struct kvec *iov;
-
-	recovery = (cmd->i_state != ISTATE_SEND_STATUS);
-	if (!recovery)
+	if (inc_stat_sn)
 		cmd->stat_sn = conn->stat_sn++;
 
 	spin_lock_bh(&conn->sess->session_stats_lock);
 	conn->sess->rsp_pdus++;
 	spin_unlock_bh(&conn->sess->session_stats_lock);
 
-	hdr			= (struct iscsi_scsi_rsp *) cmd->pdu;
 	memset(hdr, 0, ISCSI_HDR_LEN);
 	hdr->opcode		= ISCSI_OP_SCSI_CMD_RSP;
 	hdr->flags		|= ISCSI_FLAG_CMD_FINAL;
@@ -3052,6 +3186,23 @@ static int iscsit_send_status(
 	hdr->exp_cmdsn		= cpu_to_be32(conn->sess->exp_cmd_sn);
 	hdr->max_cmdsn		= cpu_to_be32(conn->sess->max_cmd_sn);
 
+	pr_debug("Built SCSI Response, ITT: 0x%08x, StatSN: 0x%08x,"
+		" Response: 0x%02x, SAM Status: 0x%02x, CID: %hu\n",
+		cmd->init_task_tag, cmd->stat_sn, cmd->se_cmd.scsi_status,
+		cmd->se_cmd.scsi_status, conn->cid);
+}
+EXPORT_SYMBOL(iscsit_build_rsp_pdu);
+
+static int iscsit_send_response(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
+{
+	struct iscsi_scsi_rsp *hdr = (struct iscsi_scsi_rsp *)&cmd->pdu[0];
+	struct kvec *iov;
+	u32 padding = 0, tx_size = 0;
+	int iov_count = 0;
+	bool inc_stat_sn = (cmd->i_state == ISTATE_SEND_STATUS);
+
+	iscsit_build_rsp_pdu(cmd, conn, inc_stat_sn, hdr);
+
 	iov = &cmd->iov_misc[0];
 	iov[iov_count].iov_base	= cmd->pdu;
 	iov[iov_count++].iov_len = ISCSI_HDR_LEN;
@@ -3106,7 +3257,7 @@ static int iscsit_send_status(
 		u32 *header_digest = (u32 *)&cmd->pdu[ISCSI_HDR_LEN];
 
 		iscsit_do_crypto_hash_buf(&conn->conn_tx_hash,
-				(unsigned char *)hdr, ISCSI_HDR_LEN,
+				(unsigned char *)cmd->pdu, ISCSI_HDR_LEN,
 				0, NULL, (u8 *)header_digest);
 
 		iov[0].iov_len += ISCSI_CRC_LEN;
@@ -3118,11 +3269,6 @@ static int iscsit_send_status(
 	cmd->iov_misc_count = iov_count;
 	cmd->tx_size = tx_size;
 
-	pr_debug("Built %sSCSI Response, ITT: 0x%08x, StatSN: 0x%08x,"
-		" Response: 0x%02x, SAM Status: 0x%02x, CID: %hu\n",
-		(!recovery) ? "" : "Recovery ", cmd->init_task_tag,
-		cmd->stat_sn, 0x00, cmd->se_cmd.scsi_status, conn->cid);
-
 	return 0;
 }
 
@@ -3145,16 +3291,12 @@ static u8 iscsit_convert_tcm_tmr_rsp(struct se_tmr_req *se_tmr)
 	}
 }
 
-static int iscsit_send_task_mgt_rsp(
-	struct iscsi_cmd *cmd,
-	struct iscsi_conn *conn)
+void
+iscsit_build_task_mgt_rsp(struct iscsi_cmd *cmd, struct iscsi_conn *conn,
+			  struct iscsi_tm_rsp *hdr)
 {
 	struct se_tmr_req *se_tmr = cmd->se_cmd.se_tmr_req;
-	struct iscsi_tm_rsp *hdr;
-	u32 tx_size = 0;
 
-	hdr			= (struct iscsi_tm_rsp *) cmd->pdu;
-	memset(hdr, 0, ISCSI_HDR_LEN);
 	hdr->opcode		= ISCSI_OP_SCSI_TMFUNC_RSP;
 	hdr->flags		= ISCSI_FLAG_CMD_FINAL;
 	hdr->response		= iscsit_convert_tcm_tmr_rsp(se_tmr);
@@ -3166,6 +3308,20 @@ static int iscsit_send_task_mgt_rsp(
 	hdr->exp_cmdsn		= cpu_to_be32(conn->sess->exp_cmd_sn);
 	hdr->max_cmdsn		= cpu_to_be32(conn->sess->max_cmd_sn);
 
+	pr_debug("Built Task Management Response ITT: 0x%08x,"
+		" StatSN: 0x%08x, Response: 0x%02x, CID: %hu\n",
+		cmd->init_task_tag, cmd->stat_sn, hdr->response, conn->cid);
+}
+EXPORT_SYMBOL(iscsit_build_task_mgt_rsp);
+
+static int
+iscsit_send_task_mgt_rsp(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
+{
+	struct iscsi_tm_rsp *hdr = (struct iscsi_tm_rsp *)&cmd->pdu[0];
+	u32 tx_size = 0;
+
+	iscsit_build_task_mgt_rsp(cmd, conn, hdr);
+
 	cmd->iov_misc[0].iov_base	= cmd->pdu;
 	cmd->iov_misc[0].iov_len	= ISCSI_HDR_LEN;
 	tx_size += ISCSI_HDR_LEN;
@@ -3186,10 +3342,6 @@ static int iscsit_send_task_mgt_rsp(
 	cmd->iov_misc_count = 1;
 	cmd->tx_size = tx_size;
 
-	pr_debug("Built Task Management Response ITT: 0x%08x,"
-		" StatSN: 0x%08x, Response: 0x%02x, CID: %hu\n",
-		cmd->init_task_tag, cmd->stat_sn, hdr->response, conn->cid);
-
 	return 0;
 }
 
@@ -3385,6 +3537,22 @@ static int iscsit_send_text_rsp(
 	return 0;
 }
 
+void
+iscsit_build_reject(struct iscsi_cmd *cmd, struct iscsi_conn *conn,
+		    struct iscsi_reject *hdr)
+{
+	hdr->opcode		= ISCSI_OP_REJECT;
+	hdr->flags		|= ISCSI_FLAG_CMD_FINAL;
+	hton24(hdr->dlength, ISCSI_HDR_LEN);
+	hdr->ffffffff		= cpu_to_be32(0xffffffff);
+	cmd->stat_sn		= conn->stat_sn++;
+	hdr->statsn		= cpu_to_be32(cmd->stat_sn);
+	hdr->exp_cmdsn		= cpu_to_be32(conn->sess->exp_cmd_sn);
+	hdr->max_cmdsn		= cpu_to_be32(conn->sess->max_cmd_sn);
+
+}
+EXPORT_SYMBOL(iscsit_build_reject);
+
 static int iscsit_send_reject(
 	struct iscsi_cmd *cmd,
 	struct iscsi_conn *conn)
@@ -3393,18 +3561,9 @@ static int iscsit_send_reject(
 	struct iscsi_reject *hdr;
 	struct kvec *iov;
 
-	hdr			= (struct iscsi_reject *) cmd->pdu;
-	hdr->opcode		= ISCSI_OP_REJECT;
-	hdr->flags		|= ISCSI_FLAG_CMD_FINAL;
-	hton24(hdr->dlength, ISCSI_HDR_LEN);
-	hdr->ffffffff		= cpu_to_be32(0xffffffff);
-	cmd->stat_sn		= conn->stat_sn++;
-	hdr->statsn		= cpu_to_be32(cmd->stat_sn);
-	hdr->exp_cmdsn	= cpu_to_be32(conn->sess->exp_cmd_sn);
-	hdr->max_cmdsn	= cpu_to_be32(conn->sess->max_cmd_sn);
+	iscsit_build_reject(cmd, conn, (struct iscsi_reject *)&cmd->pdu[0]);
 
 	iov = &cmd->iov_misc[0];
-
 	iov[iov_count].iov_base = cmd->pdu;
 	iov[iov_count++].iov_len = ISCSI_HDR_LEN;
 	iov[iov_count].iov_base = cmd->buf_ptr;
@@ -3501,55 +3660,41 @@ static inline void iscsit_thread_check_cpumask(
 	set_cpus_allowed_ptr(p, conn->conn_cpumask);
 }
 
-static int handle_immediate_queue(struct iscsi_conn *conn)
+static int
+iscsit_immediate_queue(struct iscsi_conn *conn, struct iscsi_cmd *cmd, int state)
 {
-	struct iscsi_queue_req *qr;
-	struct iscsi_cmd *cmd;
-	u8 state;
 	int ret;
 
-	while ((qr = iscsit_get_cmd_from_immediate_queue(conn))) {
-		atomic_set(&conn->check_immediate_queue, 0);
-		cmd = qr->cmd;
-		state = qr->state;
-		kmem_cache_free(lio_qr_cache, qr);
-
-		switch (state) {
-		case ISTATE_SEND_R2T:
-			ret = iscsit_send_r2t(cmd, conn);
-			if (ret < 0)
-				goto err;
-			break;
-		case ISTATE_REMOVE:
-			if (cmd->data_direction == DMA_TO_DEVICE)
-				iscsit_stop_dataout_timer(cmd);
-
-			spin_lock_bh(&conn->cmd_lock);
-			list_del(&cmd->i_conn_node);
-			spin_unlock_bh(&conn->cmd_lock);
+	switch (state) {
+	case ISTATE_SEND_R2T:
+		ret = iscsit_send_r2t(cmd, conn);
+		if (ret < 0)
+			goto err;
+		break;
+	case ISTATE_REMOVE:
+		spin_lock_bh(&conn->cmd_lock);
+		list_del(&cmd->i_conn_node);
+		spin_unlock_bh(&conn->cmd_lock);
 
-			iscsit_free_cmd(cmd);
-			continue;
-		case ISTATE_SEND_NOPIN_WANT_RESPONSE:
-			iscsit_mod_nopin_response_timer(conn);
-			ret = iscsit_send_unsolicited_nopin(cmd,
-							    conn, 1);
-			if (ret < 0)
-				goto err;
-			break;
-		case ISTATE_SEND_NOPIN_NO_RESPONSE:
-			ret = iscsit_send_unsolicited_nopin(cmd,
-							    conn, 0);
-			if (ret < 0)
-				goto err;
-			break;
-		default:
-			pr_err("Unknown Opcode: 0x%02x ITT:"
-			       " 0x%08x, i_state: %d on CID: %hu\n",
-			       cmd->iscsi_opcode, cmd->init_task_tag, state,
-			       conn->cid);
+		iscsit_free_cmd(cmd);
+		break;
+	case ISTATE_SEND_NOPIN_WANT_RESPONSE:
+		iscsit_mod_nopin_response_timer(conn);
+		ret = iscsit_send_unsolicited_nopin(cmd, conn, 1);
+		if (ret < 0)
 			goto err;
-		}
+		break;
+	case ISTATE_SEND_NOPIN_NO_RESPONSE:
+		ret = iscsit_send_unsolicited_nopin(cmd, conn, 0);
+		if (ret < 0)
+			goto err;
+		break;
+	default:
+		pr_err("Unknown Opcode: 0x%02x ITT:"
+		       " 0x%08x, i_state: %d on CID: %hu\n",
+		       cmd->iscsi_opcode, cmd->init_task_tag, state,
+		       conn->cid);
+		goto err;
 	}
 
 	return 0;
@@ -3558,128 +3703,143 @@ err:
 	return -1;
 }
 
-static int handle_response_queue(struct iscsi_conn *conn)
+static int
+iscsit_handle_immediate_queue(struct iscsi_conn *conn)
 {
+	struct iscsit_transport *t = conn->conn_transport;
 	struct iscsi_queue_req *qr;
 	struct iscsi_cmd *cmd;
 	u8 state;
 	int ret;
 
-	while ((qr = iscsit_get_cmd_from_response_queue(conn))) {
+	while ((qr = iscsit_get_cmd_from_immediate_queue(conn))) {
+		atomic_set(&conn->check_immediate_queue, 0);
 		cmd = qr->cmd;
 		state = qr->state;
 		kmem_cache_free(lio_qr_cache, qr);
 
-check_rsp_state:
-		switch (state) {
-		case ISTATE_SEND_DATAIN:
-			ret = iscsit_send_data_in(cmd, conn);
-			if (ret < 0)
-				goto err;
-			else if (!ret)
-				/* more drs */
-				goto check_rsp_state;
-			else if (ret == 1) {
-				/* all done */
-				spin_lock_bh(&cmd->istate_lock);
-				cmd->i_state = ISTATE_SENT_STATUS;
-				spin_unlock_bh(&cmd->istate_lock);
-
-				if (atomic_read(&conn->check_immediate_queue))
-					return 1;
+		ret = t->iscsit_immediate_queue(conn, cmd, state);
+		if (ret < 0)
+			return ret;
+	}
 
-				continue;
-			} else if (ret == 2) {
-				/* Still must send status,
-				   SCF_TRANSPORT_TASK_SENSE was set */
-				spin_lock_bh(&cmd->istate_lock);
-				cmd->i_state = ISTATE_SEND_STATUS;
-				spin_unlock_bh(&cmd->istate_lock);
-				state = ISTATE_SEND_STATUS;
-				goto check_rsp_state;
-			}
+	return 0;
+}
 
-			break;
-		case ISTATE_SEND_STATUS:
-		case ISTATE_SEND_STATUS_RECOVERY:
-			ret = iscsit_send_status(cmd, conn);
-			break;
-		case ISTATE_SEND_LOGOUTRSP:
-			ret = iscsit_send_logout_response(cmd, conn);
-			break;
-		case ISTATE_SEND_ASYNCMSG:
-			ret = iscsit_send_conn_drop_async_message(
-				cmd, conn);
-			break;
-		case ISTATE_SEND_NOPIN:
-			ret = iscsit_send_nopin_response(cmd, conn);
-			break;
-		case ISTATE_SEND_REJECT:
-			ret = iscsit_send_reject(cmd, conn);
-			break;
-		case ISTATE_SEND_TASKMGTRSP:
-			ret = iscsit_send_task_mgt_rsp(cmd, conn);
-			if (ret != 0)
-				break;
-			ret = iscsit_tmr_post_handler(cmd, conn);
-			if (ret != 0)
-				iscsit_fall_back_to_erl0(conn->sess);
-			break;
-		case ISTATE_SEND_TEXTRSP:
-			ret = iscsit_send_text_rsp(cmd, conn);
-			break;
-		default:
-			pr_err("Unknown Opcode: 0x%02x ITT:"
-			       " 0x%08x, i_state: %d on CID: %hu\n",
-			       cmd->iscsi_opcode, cmd->init_task_tag,
-			       state, conn->cid);
-			goto err;
-		}
+static int
+iscsit_response_queue(struct iscsi_conn *conn, struct iscsi_cmd *cmd, int state)
+{
+	int ret;
+
+check_rsp_state:
+	switch (state) {
+	case ISTATE_SEND_DATAIN:
+		ret = iscsit_send_datain(cmd, conn);
 		if (ret < 0)
 			goto err;
+		else if (!ret)
+			/* more drs */
+			goto check_rsp_state;
+		else if (ret == 1) {
+			/* all done */
+			spin_lock_bh(&cmd->istate_lock);
+			cmd->i_state = ISTATE_SENT_STATUS;
+			spin_unlock_bh(&cmd->istate_lock);
 
-		if (iscsit_send_tx_data(cmd, conn, 1) < 0) {
-			iscsit_tx_thread_wait_for_tcp(conn);
-			iscsit_unmap_iovec(cmd);
-			goto err;
-		}
-		iscsit_unmap_iovec(cmd);
+			if (atomic_read(&conn->check_immediate_queue))
+				return 1;
 
-		switch (state) {
-		case ISTATE_SEND_LOGOUTRSP:
-			if (!iscsit_logout_post_handler(cmd, conn))
-				goto restart;
-			/* fall through */
-		case ISTATE_SEND_STATUS:
-		case ISTATE_SEND_ASYNCMSG:
-		case ISTATE_SEND_NOPIN:
-		case ISTATE_SEND_STATUS_RECOVERY:
-		case ISTATE_SEND_TEXTRSP:
-		case ISTATE_SEND_TASKMGTRSP:
+			return 0;
+		} else if (ret == 2) {
+			/* Still must send status,
+			   SCF_TRANSPORT_TASK_SENSE was set */
 			spin_lock_bh(&cmd->istate_lock);
-			cmd->i_state = ISTATE_SENT_STATUS;
+			cmd->i_state = ISTATE_SEND_STATUS;
 			spin_unlock_bh(&cmd->istate_lock);
+			state = ISTATE_SEND_STATUS;
+			goto check_rsp_state;
+		}
+
+		break;
+	case ISTATE_SEND_STATUS:
+	case ISTATE_SEND_STATUS_RECOVERY:
+		ret = iscsit_send_response(cmd, conn);
+		break;
+	case ISTATE_SEND_LOGOUTRSP:
+		ret = iscsit_send_logout(cmd, conn);
+		break;
+	case ISTATE_SEND_ASYNCMSG:
+		ret = iscsit_send_conn_drop_async_message(
+			cmd, conn);
+		break;
+	case ISTATE_SEND_NOPIN:
+		ret = iscsit_send_nopin(cmd, conn);
+		break;
+	case ISTATE_SEND_REJECT:
+		ret = iscsit_send_reject(cmd, conn);
+		break;
+	case ISTATE_SEND_TASKMGTRSP:
+		ret = iscsit_send_task_mgt_rsp(cmd, conn);
+		if (ret != 0)
 			break;
-		case ISTATE_SEND_REJECT:
-			if (cmd->cmd_flags & ICF_REJECT_FAIL_CONN) {
-				cmd->cmd_flags &= ~ICF_REJECT_FAIL_CONN;
-				complete(&cmd->reject_comp);
-				goto err;
-			}
+		ret = iscsit_tmr_post_handler(cmd, conn);
+		if (ret != 0)
+			iscsit_fall_back_to_erl0(conn->sess);
+		break;
+	case ISTATE_SEND_TEXTRSP:
+		ret = iscsit_send_text_rsp(cmd, conn);
+		break;
+	default:
+		pr_err("Unknown Opcode: 0x%02x ITT:"
+		       " 0x%08x, i_state: %d on CID: %hu\n",
+		       cmd->iscsi_opcode, cmd->init_task_tag,
+		       state, conn->cid);
+		goto err;
+	}
+	if (ret < 0)
+		goto err;
+
+	if (iscsit_send_tx_data(cmd, conn, 1) < 0) {
+		iscsit_tx_thread_wait_for_tcp(conn);
+		iscsit_unmap_iovec(cmd);
+		goto err;
+	}
+	iscsit_unmap_iovec(cmd);
+
+	switch (state) {
+	case ISTATE_SEND_LOGOUTRSP:
+		if (!iscsit_logout_post_handler(cmd, conn))
+			goto restart;
+		/* fall through */
+	case ISTATE_SEND_STATUS:
+	case ISTATE_SEND_ASYNCMSG:
+	case ISTATE_SEND_NOPIN:
+	case ISTATE_SEND_STATUS_RECOVERY:
+	case ISTATE_SEND_TEXTRSP:
+	case ISTATE_SEND_TASKMGTRSP:
+		spin_lock_bh(&cmd->istate_lock);
+		cmd->i_state = ISTATE_SENT_STATUS;
+		spin_unlock_bh(&cmd->istate_lock);
+		break;
+	case ISTATE_SEND_REJECT:
+		if (cmd->cmd_flags & ICF_REJECT_FAIL_CONN) {
+			cmd->cmd_flags &= ~ICF_REJECT_FAIL_CONN;
 			complete(&cmd->reject_comp);
-			break;
-		default:
-			pr_err("Unknown Opcode: 0x%02x ITT:"
-			       " 0x%08x, i_state: %d on CID: %hu\n",
-			       cmd->iscsi_opcode, cmd->init_task_tag,
-			       cmd->i_state, conn->cid);
 			goto err;
 		}
-
-		if (atomic_read(&conn->check_immediate_queue))
-			return 1;
+		complete(&cmd->reject_comp);
+		break;
+	default:
+		pr_err("Unknown Opcode: 0x%02x ITT:"
+		       " 0x%08x, i_state: %d on CID: %hu\n",
+		       cmd->iscsi_opcode, cmd->init_task_tag,
+		       cmd->i_state, conn->cid);
+		goto err;
 	}
 
+	if (atomic_read(&conn->check_immediate_queue))
+		return 1;
+
 	return 0;
 
 err:
@@ -3688,6 +3848,27 @@ restart:
 	return -EAGAIN;
 }
 
+static int iscsit_handle_response_queue(struct iscsi_conn *conn)
+{
+	struct iscsit_transport *t = conn->conn_transport;
+	struct iscsi_queue_req *qr;
+	struct iscsi_cmd *cmd;
+	u8 state;
+	int ret;
+
+	while ((qr = iscsit_get_cmd_from_response_queue(conn))) {
+		cmd = qr->cmd;
+		state = qr->state;
+		kmem_cache_free(lio_qr_cache, qr);
+
+		ret = t->iscsit_response_queue(conn, cmd, state);
+		if (ret == 1 || ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
 int iscsi_target_tx_thread(void *arg)
 {
 	int ret = 0;
@@ -3722,11 +3903,11 @@ restart:
 			goto transport_err;
 
 get_immediate:
-		ret = handle_immediate_queue(conn);
+		ret = iscsit_handle_immediate_queue(conn);
 		if (ret < 0)
 			goto transport_err;
 
-		ret = handle_response_queue(conn);
+		ret = iscsit_handle_response_queue(conn);
 		if (ret == 1)
 			goto get_immediate;
 		else if (ret == -EAGAIN)
@@ -3742,6 +3923,83 @@ out:
 	return 0;
 }
 
+static int iscsi_target_rx_opcode(struct iscsi_conn *conn, unsigned char *buf)
+{
+	struct iscsi_hdr *hdr = (struct iscsi_hdr *)buf;
+	struct iscsi_cmd *cmd;
+	int ret = 0;
+
+	switch (hdr->opcode & ISCSI_OPCODE_MASK) {
+	case ISCSI_OP_SCSI_CMD:
+		cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
+		if (!cmd)
+			return iscsit_add_reject(ISCSI_REASON_BOOKMARK_NO_RESOURCES,
+						1, buf, conn);
+
+		ret = iscsit_handle_scsi_cmd(conn, cmd, buf);
+		break;
+	case ISCSI_OP_SCSI_DATA_OUT:
+		ret = iscsit_handle_data_out(conn, buf);
+		break;
+	case ISCSI_OP_NOOP_OUT:
+		cmd = NULL;
+		if (hdr->ttt == cpu_to_be32(0xFFFFFFFF)) {
+			cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
+			if (!cmd)
+				return iscsit_add_reject(ISCSI_REASON_BOOKMARK_NO_RESOURCES,
+						1, buf, conn);
+		}
+		ret = iscsit_handle_nop_out(conn, cmd, buf);
+		break;
+	case ISCSI_OP_SCSI_TMFUNC:
+		cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
+		if (!cmd)
+			return iscsit_add_reject(ISCSI_REASON_BOOKMARK_NO_RESOURCES,
+						1, buf, conn);
+
+		ret = iscsit_handle_task_mgt_cmd(conn, cmd, buf);
+		break;
+	case ISCSI_OP_TEXT:
+		ret = iscsit_handle_text_cmd(conn, buf);
+		break;
+	case ISCSI_OP_LOGOUT:
+		cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
+		if (!cmd)
+			return iscsit_add_reject(ISCSI_REASON_BOOKMARK_NO_RESOURCES,
+						1, buf, conn);
+
+		ret = iscsit_handle_logout_cmd(conn, cmd, buf);
+		if (ret > 0)
+			wait_for_completion_timeout(&conn->conn_logout_comp,
+					SECONDS_FOR_LOGOUT_COMP * HZ);
+		break;
+	case ISCSI_OP_SNACK:
+		ret = iscsit_handle_snack(conn, buf);
+		break;
+	default:
+		pr_err("Got unknown iSCSI OpCode: 0x%02x\n", hdr->opcode);
+		if (!conn->sess->sess_ops->ErrorRecoveryLevel) {
+			pr_err("Cannot recover from unknown"
+			" opcode while ERL=0, closing iSCSI connection.\n");
+			return -1;
+		}
+		if (!conn->conn_ops->OFMarker) {
+			pr_err("Unable to recover from unknown"
+			" opcode while OFMarker=No, closing iSCSI"
+				" connection.\n");
+			return -1;
+		}
+		if (iscsit_recover_from_unknown_opcode(conn) < 0) {
+			pr_err("Unable to recover from unknown"
+				" opcode, closing iSCSI connection.\n");
+			return -1;
+		}
+		break;
+	}
+
+	return ret;
+}
+
 int iscsi_target_rx_thread(void *arg)
 {
 	int ret;
@@ -3761,6 +4019,18 @@ restart:
 	if (!conn)
 		goto out;
 
+	if (conn->conn_transport->transport_type == ISCSI_INFINIBAND) {
+		struct completion comp;
+		int rc;
+
+		init_completion(&comp);
+		rc = wait_for_completion_interruptible(&comp);
+		if (rc < 0)
+			goto transport_err;
+
+		goto out;
+	}
+
 	while (!kthread_should_stop()) {
 		/*
 		 * Ensure that both TX and RX per connection kthreads
@@ -3832,62 +4102,9 @@ restart:
 			goto transport_err;
 		}
 
-		switch (opcode) {
-		case ISCSI_OP_SCSI_CMD:
-			if (iscsit_handle_scsi_cmd(conn, buffer) < 0)
-				goto transport_err;
-			break;
-		case ISCSI_OP_SCSI_DATA_OUT:
-			if (iscsit_handle_data_out(conn, buffer) < 0)
-				goto transport_err;
-			break;
-		case ISCSI_OP_NOOP_OUT:
-			if (iscsit_handle_nop_out(conn, buffer) < 0)
-				goto transport_err;
-			break;
-		case ISCSI_OP_SCSI_TMFUNC:
-			if (iscsit_handle_task_mgt_cmd(conn, buffer) < 0)
-				goto transport_err;
-			break;
-		case ISCSI_OP_TEXT:
-			if (iscsit_handle_text_cmd(conn, buffer) < 0)
-				goto transport_err;
-			break;
-		case ISCSI_OP_LOGOUT:
-			ret = iscsit_handle_logout_cmd(conn, buffer);
-			if (ret > 0) {
-				wait_for_completion_timeout(&conn->conn_logout_comp,
-						SECONDS_FOR_LOGOUT_COMP * HZ);
-				goto transport_err;
-			} else if (ret < 0)
-				goto transport_err;
-			break;
-		case ISCSI_OP_SNACK:
-			if (iscsit_handle_snack(conn, buffer) < 0)
-				goto transport_err;
-			break;
-		default:
-			pr_err("Got unknown iSCSI OpCode: 0x%02x\n",
-					opcode);
-			if (!conn->sess->sess_ops->ErrorRecoveryLevel) {
-				pr_err("Cannot recover from unknown"
-				" opcode while ERL=0, closing iSCSI connection"
-				".\n");
-				goto transport_err;
-			}
-			if (!conn->conn_ops->OFMarker) {
-				pr_err("Unable to recover from unknown"
-				" opcode while OFMarker=No, closing iSCSI"
-					" connection.\n");
-				goto transport_err;
-			}
-			if (iscsit_recover_from_unknown_opcode(conn) < 0) {
-				pr_err("Unable to recover from unknown"
-					" opcode, closing iSCSI connection.\n");
-				goto transport_err;
-			}
-			break;
-		}
+		ret = iscsi_target_rx_opcode(conn, buffer);
+		if (ret < 0)
+			goto transport_err;
 	}
 
 transport_err:
@@ -4053,6 +4270,12 @@ int iscsit_close_connection(
 
 	if (conn->sock)
 		sock_release(conn->sock);
+
+	if (conn->conn_transport->iscsit_free_conn)
+		conn->conn_transport->iscsit_free_conn(conn);
+
+	iscsit_put_transport(conn->conn_transport);
+
 	conn->thread_set = NULL;
 
 	pr_debug("Moving to TARG_CONN_STATE_FREE.\n");
@@ -4284,7 +4507,7 @@ static void iscsit_logout_post_handler_diffcid(
 /*
  *	Return of 0 causes the TX thread to restart.
  */
-static int iscsit_logout_post_handler(
+int iscsit_logout_post_handler(
 	struct iscsi_cmd *cmd,
 	struct iscsi_conn *conn)
 {
@@ -4342,6 +4565,7 @@ static int iscsit_logout_post_handler(
 	}
 	return ret;
 }
+EXPORT_SYMBOL(iscsit_logout_post_handler);
 
 void iscsit_fail_session(struct iscsi_session *sess)
 {
diff --git a/drivers/target/iscsi/iscsi_target.h b/drivers/target/iscsi/iscsi_target.h
index b1a1e635070..a0050b2f294 100644
--- a/drivers/target/iscsi/iscsi_target.h
+++ b/drivers/target/iscsi/iscsi_target.h
@@ -16,11 +16,12 @@ extern int iscsit_reset_np_thread(struct iscsi_np *, struct iscsi_tpg_np *,
 				struct iscsi_portal_group *);
 extern int iscsit_del_np(struct iscsi_np *);
 extern int iscsit_add_reject_from_cmd(u8, int, int, unsigned char *, struct iscsi_cmd *);
+extern void iscsit_set_unsoliticed_dataout(struct iscsi_cmd *);
 extern int iscsit_logout_closesession(struct iscsi_cmd *, struct iscsi_conn *);
 extern int iscsit_logout_closeconnection(struct iscsi_cmd *, struct iscsi_conn *);
 extern int iscsit_logout_removeconnforrecovery(struct iscsi_cmd *, struct iscsi_conn *);
 extern int iscsit_send_async_msg(struct iscsi_conn *, u16, u8, u8);
-extern int iscsit_build_r2ts_for_cmd(struct iscsi_cmd *, struct iscsi_conn *, bool recovery);
+extern int iscsit_build_r2ts_for_cmd(struct iscsi_conn *, struct iscsi_cmd *, bool recovery);
 extern void iscsit_thread_get_cpumask(struct iscsi_conn *);
 extern int iscsi_target_tx_thread(void *);
 extern int iscsi_target_rx_thread(void *);
diff --git a/drivers/target/iscsi/iscsi_target_auth.c b/drivers/target/iscsi/iscsi_target_auth.c
index a0fc7b9eea6..cee17543278 100644
--- a/drivers/target/iscsi/iscsi_target_auth.c
+++ b/drivers/target/iscsi/iscsi_target_auth.c
@@ -49,32 +49,6 @@ static void chap_binaryhex_to_asciihex(char *dst, char *src, int src_len)
 	}
 }
 
-static void chap_set_random(char *data, int length)
-{
-	long r;
-	unsigned n;
-
-	while (length > 0) {
-		get_random_bytes(&r, sizeof(long));
-		r = r ^ (r >> 8);
-		r = r ^ (r >> 4);
-		n = r & 0x7;
-
-		get_random_bytes(&r, sizeof(long));
-		r = r ^ (r >> 8);
-		r = r ^ (r >> 5);
-		n = (n << 3) | (r & 0x7);
-
-		get_random_bytes(&r, sizeof(long));
-		r = r ^ (r >> 8);
-		r = r ^ (r >> 5);
-		n = (n << 2) | (r & 0x3);
-
-		*data++ = n;
-		length--;
-	}
-}
-
 static void chap_gen_challenge(
 	struct iscsi_conn *conn,
 	int caller,
@@ -86,7 +60,7 @@ static void chap_gen_challenge(
 
 	memset(challenge_asciihex, 0, CHAP_CHALLENGE_LENGTH * 2 + 1);
 
-	chap_set_random(chap->challenge, CHAP_CHALLENGE_LENGTH);
+	get_random_bytes(chap->challenge, CHAP_CHALLENGE_LENGTH);
 	chap_binaryhex_to_asciihex(challenge_asciihex, chap->challenge,
 				CHAP_CHALLENGE_LENGTH);
 	/*
diff --git a/drivers/target/iscsi/iscsi_target_configfs.c b/drivers/target/iscsi/iscsi_target_configfs.c
index 78d75c8567d..13e9e715ad2 100644
--- a/drivers/target/iscsi/iscsi_target_configfs.c
+++ b/drivers/target/iscsi/iscsi_target_configfs.c
@@ -27,6 +27,7 @@
 #include <target/target_core_fabric_configfs.h>
 #include <target/target_core_configfs.h>
 #include <target/configfs_macros.h>
+#include <target/iscsi/iscsi_transport.h>
 
 #include "iscsi_target_core.h"
 #include "iscsi_target_parameters.h"
@@ -124,8 +125,87 @@ out:
 
 TF_NP_BASE_ATTR(lio_target, sctp, S_IRUGO | S_IWUSR);
 
+static ssize_t lio_target_np_show_iser(
+	struct se_tpg_np *se_tpg_np,
+	char *page)
+{
+	struct iscsi_tpg_np *tpg_np = container_of(se_tpg_np,
+				struct iscsi_tpg_np, se_tpg_np);
+	struct iscsi_tpg_np *tpg_np_iser;
+	ssize_t rb;
+
+	tpg_np_iser = iscsit_tpg_locate_child_np(tpg_np, ISCSI_INFINIBAND);
+	if (tpg_np_iser)
+		rb = sprintf(page, "1\n");
+	else
+		rb = sprintf(page, "0\n");
+
+	return rb;
+}
+
+static ssize_t lio_target_np_store_iser(
+	struct se_tpg_np *se_tpg_np,
+	const char *page,
+	size_t count)
+{
+	struct iscsi_np *np;
+	struct iscsi_portal_group *tpg;
+	struct iscsi_tpg_np *tpg_np = container_of(se_tpg_np,
+				struct iscsi_tpg_np, se_tpg_np);
+	struct iscsi_tpg_np *tpg_np_iser = NULL;
+	char *endptr;
+	u32 op;
+	int rc;
+
+	op = simple_strtoul(page, &endptr, 0);
+	if ((op != 1) && (op != 0)) {
+		pr_err("Illegal value for tpg_enable: %u\n", op);
+		return -EINVAL;
+	}
+	np = tpg_np->tpg_np;
+	if (!np) {
+		pr_err("Unable to locate struct iscsi_np from"
+				" struct iscsi_tpg_np\n");
+		return -EINVAL;
+	}
+
+	tpg = tpg_np->tpg;
+	if (iscsit_get_tpg(tpg) < 0)
+		return -EINVAL;
+
+	if (op) {
+		int rc = request_module("ib_isert");
+		if (rc != 0)
+			pr_warn("Unable to request_module for ib_isert\n");
+
+		tpg_np_iser = iscsit_tpg_add_network_portal(tpg, &np->np_sockaddr,
+				np->np_ip, tpg_np, ISCSI_INFINIBAND);
+		if (!tpg_np_iser || IS_ERR(tpg_np_iser))
+			goto out;
+	} else {
+		tpg_np_iser = iscsit_tpg_locate_child_np(tpg_np, ISCSI_INFINIBAND);
+		if (!tpg_np_iser)
+			goto out;
+
+		rc = iscsit_tpg_del_network_portal(tpg, tpg_np_iser);
+		if (rc < 0)
+			goto out;
+	}
+
+	printk("lio_target_np_store_iser() done, op: %d\n", op);
+
+	iscsit_put_tpg(tpg);
+	return count;
+out:
+	iscsit_put_tpg(tpg);
+	return -EINVAL;
+}
+
+TF_NP_BASE_ATTR(lio_target, iser, S_IRUGO | S_IWUSR);
+
 static struct configfs_attribute *lio_target_portal_attrs[] = {
 	&lio_target_np_sctp.attr,
+	&lio_target_np_iser.attr,
 	NULL,
 };
 
@@ -1536,16 +1616,18 @@ static int lio_queue_data_in(struct se_cmd *se_cmd)
 	struct iscsi_cmd *cmd = container_of(se_cmd, struct iscsi_cmd, se_cmd);
 
 	cmd->i_state = ISTATE_SEND_DATAIN;
-	iscsit_add_cmd_to_response_queue(cmd, cmd->conn, cmd->i_state);
+	cmd->conn->conn_transport->iscsit_queue_data_in(cmd->conn, cmd);
+
 	return 0;
 }
 
 static int lio_write_pending(struct se_cmd *se_cmd)
 {
 	struct iscsi_cmd *cmd = container_of(se_cmd, struct iscsi_cmd, se_cmd);
+	struct iscsi_conn *conn = cmd->conn;
 
 	if (!cmd->immediate_data && !cmd->unsolicited_data)
-		return iscsit_build_r2ts_for_cmd(cmd, cmd->conn, false);
+		return conn->conn_transport->iscsit_get_dataout(conn, cmd, false);
 
 	return 0;
 }
@@ -1567,7 +1649,8 @@ static int lio_queue_status(struct se_cmd *se_cmd)
 	struct iscsi_cmd *cmd = container_of(se_cmd, struct iscsi_cmd, se_cmd);
 
 	cmd->i_state = ISTATE_SEND_STATUS;
-	iscsit_add_cmd_to_response_queue(cmd, cmd->conn, cmd->i_state);
+	cmd->conn->conn_transport->iscsit_queue_status(cmd->conn, cmd);
+
 	return 0;
 }
 
@@ -1696,11 +1779,17 @@ static void lio_set_default_node_attributes(struct se_node_acl *se_acl)
 	iscsit_set_default_node_attribues(acl);
 }
 
+static int lio_check_stop_free(struct se_cmd *se_cmd)
+{
+	return target_put_sess_cmd(se_cmd->se_sess, se_cmd);
+}
+
 static void lio_release_cmd(struct se_cmd *se_cmd)
 {
 	struct iscsi_cmd *cmd = container_of(se_cmd, struct iscsi_cmd, se_cmd);
 
-	iscsit_release_cmd(cmd);
+	pr_debug("Entering lio_release_cmd for se_cmd: %p\n", se_cmd);
+	cmd->release_cmd(cmd);
 }
 
 /* End functions for target_core_fabric_ops */
@@ -1740,6 +1829,7 @@ int iscsi_target_register_configfs(void)
 	fabric->tf_ops.tpg_alloc_fabric_acl = &lio_tpg_alloc_fabric_acl;
 	fabric->tf_ops.tpg_release_fabric_acl = &lio_tpg_release_fabric_acl;
 	fabric->tf_ops.tpg_get_inst_index = &lio_tpg_get_inst_index;
+	fabric->tf_ops.check_stop_free = &lio_check_stop_free,
 	fabric->tf_ops.release_cmd = &lio_release_cmd;
 	fabric->tf_ops.shutdown_session = &lio_tpg_shutdown_session;
 	fabric->tf_ops.close_session = &lio_tpg_close_session;
diff --git a/drivers/target/iscsi/iscsi_target_core.h b/drivers/target/iscsi/iscsi_target_core.h
index 7a333d28d9a..60ec4b92be0 100644
--- a/drivers/target/iscsi/iscsi_target_core.h
+++ b/drivers/target/iscsi/iscsi_target_core.h
@@ -60,7 +60,7 @@
 
 #define ISCSI_IOV_DATA_BUFFER		5
 
-enum tpg_np_network_transport_table {
+enum iscsit_transport_type {
 	ISCSI_TCP				= 0,
 	ISCSI_SCTP_TCP				= 1,
 	ISCSI_SCTP_UDP				= 2,
@@ -244,6 +244,11 @@ struct iscsi_conn_ops {
 	u8	IFMarker;			/* [0,1] == [No,Yes] */
 	u32	OFMarkInt;			/* [1..65535] */
 	u32	IFMarkInt;			/* [1..65535] */
+	/*
+	 * iSER specific connection parameters
+	 */
+	u32	InitiatorRecvDataSegmentLength;	/* [512..2**24-1] */
+	u32	TargetRecvDataSegmentLength;	/* [512..2**24-1] */
 };
 
 struct iscsi_sess_ops {
@@ -265,6 +270,10 @@ struct iscsi_sess_ops {
 	u8	DataSequenceInOrder;		/* [0,1] == [No,Yes] */
 	u8	ErrorRecoveryLevel;		/* [0..2] */
 	u8	SessionType;			/* [0,1] == [Normal,Discovery]*/
+	/*
+	 * iSER specific session parameters
+	 */
+	u8	RDMAExtensions;			/* [0,1] == [No,Yes] */
 };
 
 struct iscsi_queue_req {
@@ -284,6 +293,7 @@ struct iscsi_data_count {
 };
 
 struct iscsi_param_list {
+	bool			iser;
 	struct list_head	param_list;
 	struct list_head	extra_response_list;
 };
@@ -475,6 +485,7 @@ struct iscsi_cmd {
 	u32			first_data_sg_off;
 	u32			kmapped_nents;
 	sense_reason_t		sense_reason;
+	void (*release_cmd)(struct iscsi_cmd *);
 }  ____cacheline_aligned;
 
 struct iscsi_tmr_req {
@@ -503,6 +514,7 @@ struct iscsi_conn {
 	u16			login_port;
 	u16			local_port;
 	int			net_size;
+	int			login_family;
 	u32			auth_id;
 	u32			conn_flags;
 	/* Used for iscsi_tx_login_rsp() */
@@ -562,9 +574,12 @@ struct iscsi_conn {
 	struct list_head	immed_queue_list;
 	struct list_head	response_queue_list;
 	struct iscsi_conn_ops	*conn_ops;
+	struct iscsi_login	*conn_login;
+	struct iscsit_transport *conn_transport;
 	struct iscsi_param_list	*param_list;
 	/* Used for per connection auth state machine */
 	void			*auth_protocol;
+	void			*context;
 	struct iscsi_login_thread_s *login_thread;
 	struct iscsi_portal_group *tpg;
 	/* Pointer to parent session */
@@ -663,6 +678,8 @@ struct iscsi_login {
 	u8 first_request;
 	u8 version_min;
 	u8 version_max;
+	u8 login_complete;
+	u8 login_failed;
 	char isid[6];
 	u32 cmd_sn;
 	itt_t init_task_tag;
@@ -670,10 +687,11 @@ struct iscsi_login {
 	u32 rsp_length;
 	u16 cid;
 	u16 tsih;
-	char *req;
-	char *rsp;
+	char req[ISCSI_HDR_LEN];
+	char rsp[ISCSI_HDR_LEN];
 	char *req_buf;
 	char *rsp_buf;
+	struct iscsi_conn *conn;
 } ____cacheline_aligned;
 
 struct iscsi_node_attrib {
@@ -754,6 +772,8 @@ struct iscsi_np {
 	struct task_struct	*np_thread;
 	struct timer_list	np_login_timer;
 	struct iscsi_portal_group *np_login_tpg;
+	void			*np_context;
+	struct iscsit_transport *np_transport;
 	struct list_head	np_list;
 } ____cacheline_aligned;
 
diff --git a/drivers/target/iscsi/iscsi_target_device.c b/drivers/target/iscsi/iscsi_target_device.c
index bcc409853a6..1b74033510a 100644
--- a/drivers/target/iscsi/iscsi_target_device.c
+++ b/drivers/target/iscsi/iscsi_target_device.c
@@ -60,8 +60,13 @@ void iscsit_increment_maxcmdsn(struct iscsi_cmd *cmd, struct iscsi_session *sess
 
 	cmd->maxcmdsn_inc = 1;
 
-	mutex_lock(&sess->cmdsn_mutex);
+	if (!mutex_trylock(&sess->cmdsn_mutex)) {
+		sess->max_cmd_sn += 1;
+		pr_debug("Updated MaxCmdSN to 0x%08x\n", sess->max_cmd_sn);
+		return;
+	}
 	sess->max_cmd_sn += 1;
 	pr_debug("Updated MaxCmdSN to 0x%08x\n", sess->max_cmd_sn);
 	mutex_unlock(&sess->cmdsn_mutex);
 }
+EXPORT_SYMBOL(iscsit_increment_maxcmdsn);
diff --git a/drivers/target/iscsi/iscsi_target_erl1.c b/drivers/target/iscsi/iscsi_target_erl1.c
index 0b52a237130..7816af6cdd1 100644
--- a/drivers/target/iscsi/iscsi_target_erl1.c
+++ b/drivers/target/iscsi/iscsi_target_erl1.c
@@ -22,6 +22,7 @@
 #include <scsi/iscsi_proto.h>
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
+#include <target/iscsi/iscsi_transport.h>
 
 #include "iscsi_target_core.h"
 #include "iscsi_target_seq_pdu_list.h"
@@ -53,6 +54,9 @@ int iscsit_dump_data_payload(
 	u32 length, padding, offset = 0, size;
 	struct kvec iov;
 
+	if (conn->sess->sess_ops->RDMAExtensions)
+		return 0;
+
 	length = (buf_len > OFFLOAD_BUF_SIZE) ? OFFLOAD_BUF_SIZE : buf_len;
 
 	buf = kzalloc(length, GFP_ATOMIC);
@@ -919,6 +923,7 @@ int iscsit_execute_ooo_cmdsns(struct iscsi_session *sess)
 int iscsit_execute_cmd(struct iscsi_cmd *cmd, int ooo)
 {
 	struct se_cmd *se_cmd = &cmd->se_cmd;
+	struct iscsi_conn *conn = cmd->conn;
 	int lr = 0;
 
 	spin_lock_bh(&cmd->istate_lock);
@@ -981,7 +986,7 @@ int iscsit_execute_cmd(struct iscsi_cmd *cmd, int ooo)
 					return 0;
 
 				iscsit_set_dataout_sequence_values(cmd);
-				iscsit_build_r2ts_for_cmd(cmd, cmd->conn, false);
+				conn->conn_transport->iscsit_get_dataout(conn, cmd, false);
 			}
 			return 0;
 		}
@@ -999,10 +1004,7 @@ int iscsit_execute_cmd(struct iscsi_cmd *cmd, int ooo)
 			if (transport_check_aborted_status(se_cmd, 1) != 0)
 				return 0;
 
-			iscsit_set_dataout_sequence_values(cmd);
-			spin_lock_bh(&cmd->dataout_timeout_lock);
-			iscsit_start_dataout_timer(cmd, cmd->conn);
-			spin_unlock_bh(&cmd->dataout_timeout_lock);
+			iscsit_set_unsoliticed_dataout(cmd);
 		}
 		return transport_handle_cdb_direct(&cmd->se_cmd);
 
@@ -1290,3 +1292,4 @@ void iscsit_stop_dataout_timer(struct iscsi_cmd *cmd)
 			cmd->init_task_tag);
 	spin_unlock_bh(&cmd->dataout_timeout_lock);
 }
+EXPORT_SYMBOL(iscsit_stop_dataout_timer);
diff --git a/drivers/target/iscsi/iscsi_target_login.c b/drivers/target/iscsi/iscsi_target_login.c
index 2535d4d46c0..bb5d5c5bce6 100644
--- a/drivers/target/iscsi/iscsi_target_login.c
+++ b/drivers/target/iscsi/iscsi_target_login.c
@@ -39,8 +39,39 @@
 #include "iscsi_target.h"
 #include "iscsi_target_parameters.h"
 
-static int iscsi_login_init_conn(struct iscsi_conn *conn)
+#include <target/iscsi/iscsi_transport.h>
+
+static struct iscsi_login *iscsi_login_init_conn(struct iscsi_conn *conn)
 {
+	struct iscsi_login *login;
+
+	login = kzalloc(sizeof(struct iscsi_login), GFP_KERNEL);
+	if (!login) {
+		pr_err("Unable to allocate memory for struct iscsi_login.\n");
+		return NULL;
+	}
+	login->conn = conn;
+	login->first_request = 1;
+
+	login->req_buf = kzalloc(MAX_KEY_VALUE_PAIRS, GFP_KERNEL);
+	if (!login->req_buf) {
+		pr_err("Unable to allocate memory for response buffer.\n");
+		goto out_login;
+	}
+
+	login->rsp_buf = kzalloc(MAX_KEY_VALUE_PAIRS, GFP_KERNEL);
+	if (!login->rsp_buf) {
+		pr_err("Unable to allocate memory for request buffer.\n");
+		goto out_req_buf;
+	}
+
+	conn->conn_ops = kzalloc(sizeof(struct iscsi_conn_ops), GFP_KERNEL);
+	if (!conn->conn_ops) {
+		pr_err("Unable to allocate memory for"
+			" struct iscsi_conn_ops.\n");
+		goto out_rsp_buf;
+	}
+
 	init_waitqueue_head(&conn->queues_wq);
 	INIT_LIST_HEAD(&conn->conn_list);
 	INIT_LIST_HEAD(&conn->conn_cmd_list);
@@ -62,10 +93,21 @@ static int iscsi_login_init_conn(struct iscsi_conn *conn)
 
 	if (!zalloc_cpumask_var(&conn->conn_cpumask, GFP_KERNEL)) {
 		pr_err("Unable to allocate conn->conn_cpumask\n");
-		return -ENOMEM;
+		goto out_conn_ops;
 	}
+	conn->conn_login = login;
 
-	return 0;
+	return login;
+
+out_conn_ops:
+	kfree(conn->conn_ops);
+out_rsp_buf:
+	kfree(login->rsp_buf);
+out_req_buf:
+	kfree(login->req_buf);
+out_login:
+	kfree(login);
+	return NULL;
 }
 
 /*
@@ -298,6 +340,7 @@ static int iscsi_login_zero_tsih_s2(
 	struct iscsi_node_attrib *na;
 	struct iscsi_session *sess = conn->sess;
 	unsigned char buf[32];
+	bool iser = false;
 
 	sess->tpg = conn->tpg;
 
@@ -319,7 +362,10 @@ static int iscsi_login_zero_tsih_s2(
 		return -1;
 	}
 
-	iscsi_set_keys_to_negotiate(0, conn->param_list);
+	if (conn->conn_transport->transport_type == ISCSI_INFINIBAND)
+		iser = true;
+
+	iscsi_set_keys_to_negotiate(conn->param_list, iser);
 
 	if (sess->sess_ops->SessionType)
 		return iscsi_set_keys_irrelevant_for_discovery(
@@ -357,6 +403,56 @@ static int iscsi_login_zero_tsih_s2(
 
 	if (iscsi_login_disable_FIM_keys(conn->param_list, conn) < 0)
 		return -1;
+	/*
+	 * Set RDMAExtensions=Yes by default for iSER enabled network portals
+	 */
+	if (iser) {
+		struct iscsi_param *param;
+		unsigned long mrdsl, off;
+		int rc;
+
+		sprintf(buf, "RDMAExtensions=Yes");
+		if (iscsi_change_param_value(buf, conn->param_list, 0) < 0) {
+			iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
+				ISCSI_LOGIN_STATUS_NO_RESOURCES);
+			return -1;
+		}
+		/*
+		 * Make MaxRecvDataSegmentLength PAGE_SIZE aligned for
+		 * Immediate Data + Unsolicitied Data-OUT if necessary..
+		 */
+		param = iscsi_find_param_from_key("MaxRecvDataSegmentLength",
+						  conn->param_list);
+		if (!param) {
+			iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
+				ISCSI_LOGIN_STATUS_NO_RESOURCES);
+			return -1;
+		}
+		rc = strict_strtoul(param->value, 0, &mrdsl);
+		if (rc < 0) {
+			iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
+				ISCSI_LOGIN_STATUS_NO_RESOURCES);
+			return -1;
+		}
+		off = mrdsl % PAGE_SIZE;
+		if (!off)
+			return 0;
+
+		if (mrdsl < PAGE_SIZE)
+			mrdsl = PAGE_SIZE;
+		else
+			mrdsl -= off;
+
+		pr_warn("Aligning ISER MaxRecvDataSegmentLength: %lu down"
+			" to PAGE_SIZE\n", mrdsl);
+
+		sprintf(buf, "MaxRecvDataSegmentLength=%lu\n", mrdsl);
+		if (iscsi_change_param_value(buf, conn->param_list, 0) < 0) {
+			iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
+				ISCSI_LOGIN_STATUS_NO_RESOURCES);
+			return -1;
+		}
+	}
 
 	return 0;
 }
@@ -436,6 +532,7 @@ static int iscsi_login_non_zero_tsih_s2(
 	struct se_portal_group *se_tpg = &tpg->tpg_se_tpg;
 	struct se_session *se_sess, *se_sess_tmp;
 	struct iscsi_login_req *pdu = (struct iscsi_login_req *)buf;
+	bool iser = false;
 
 	spin_lock_bh(&se_tpg->session_lock);
 	list_for_each_entry_safe(se_sess, se_sess_tmp, &se_tpg->tpg_sess_list,
@@ -485,7 +582,10 @@ static int iscsi_login_non_zero_tsih_s2(
 		return -1;
 	}
 
-	iscsi_set_keys_to_negotiate(0, conn->param_list);
+	if (conn->conn_transport->transport_type == ISCSI_INFINIBAND)
+		iser = true;
+
+	iscsi_set_keys_to_negotiate(conn->param_list, iser);
 	/*
 	 * Need to send TargetPortalGroupTag back in first login response
 	 * on any iSCSI connection where the Initiator provides TargetName.
@@ -574,6 +674,11 @@ int iscsi_login_post_auth_non_zero_tsih(
 static void iscsi_post_login_start_timers(struct iscsi_conn *conn)
 {
 	struct iscsi_session *sess = conn->sess;
+	/*
+	 * FIXME: Unsolicitied NopIN support for ISER
+	 */
+	if (conn->conn_transport->transport_type == ISCSI_INFINIBAND)
+		return;
 
 	if (!sess->sess_ops->SessionType)
 		iscsit_start_nopin_timer(conn);
@@ -632,6 +737,7 @@ static int iscsi_post_login_handler(
 		spin_unlock_bh(&sess->conn_lock);
 
 		iscsi_post_login_start_timers(conn);
+
 		iscsi_activate_thread_set(conn, ts);
 		/*
 		 * Determine CPU mask to ensure connection's RX and TX kthreads
@@ -761,11 +867,11 @@ static void iscsi_stop_login_thread_timer(struct iscsi_np *np)
 	spin_unlock_bh(&np->np_thread_lock);
 }
 
-int iscsi_target_setup_login_socket(
+int iscsit_setup_np(
 	struct iscsi_np *np,
 	struct __kernel_sockaddr_storage *sockaddr)
 {
-	struct socket *sock;
+	struct socket *sock = NULL;
 	int backlog = 5, ret, opt = 0, len;
 
 	switch (np->np_network_transport) {
@@ -781,15 +887,15 @@ int iscsi_target_setup_login_socket(
 		np->np_ip_proto = IPPROTO_SCTP;
 		np->np_sock_type = SOCK_SEQPACKET;
 		break;
-	case ISCSI_IWARP_TCP:
-	case ISCSI_IWARP_SCTP:
-	case ISCSI_INFINIBAND:
 	default:
 		pr_err("Unsupported network_transport: %d\n",
 				np->np_network_transport);
 		return -EINVAL;
 	}
 
+	np->np_ip_proto = IPPROTO_TCP;
+	np->np_sock_type = SOCK_STREAM;
+
 	ret = sock_create(sockaddr->ss_family, np->np_sock_type,
 			np->np_ip_proto, &sock);
 	if (ret < 0) {
@@ -853,7 +959,6 @@ int iscsi_target_setup_login_socket(
 	}
 
 	return 0;
-
 fail:
 	np->np_socket = NULL;
 	if (sock)
@@ -861,21 +966,169 @@ fail:
 	return ret;
 }
 
+int iscsi_target_setup_login_socket(
+	struct iscsi_np *np,
+	struct __kernel_sockaddr_storage *sockaddr)
+{
+	struct iscsit_transport *t;
+	int rc;
+
+	t = iscsit_get_transport(np->np_network_transport);
+	if (!t)
+		return -EINVAL;
+
+	rc = t->iscsit_setup_np(np, sockaddr);
+	if (rc < 0) {
+		iscsit_put_transport(t);
+		return rc;
+	}
+
+	np->np_transport = t;
+	printk("Set np->np_transport to %p -> %s\n", np->np_transport,
+				np->np_transport->name);
+	return 0;
+}
+
+int iscsit_accept_np(struct iscsi_np *np, struct iscsi_conn *conn)
+{
+	struct socket *new_sock, *sock = np->np_socket;
+	struct sockaddr_in sock_in;
+	struct sockaddr_in6 sock_in6;
+	int rc, err;
+
+	rc = kernel_accept(sock, &new_sock, 0);
+	if (rc < 0)
+		return rc;
+
+	conn->sock = new_sock;
+	conn->login_family = np->np_sockaddr.ss_family;
+	printk("iSCSI/TCP: Setup conn->sock from new_sock: %p\n", new_sock);
+
+	if (np->np_sockaddr.ss_family == AF_INET6) {
+		memset(&sock_in6, 0, sizeof(struct sockaddr_in6));
+
+		rc = conn->sock->ops->getname(conn->sock,
+				(struct sockaddr *)&sock_in6, &err, 1);
+		if (!rc) {
+			snprintf(conn->login_ip, sizeof(conn->login_ip), "%pI6c",
+				&sock_in6.sin6_addr.in6_u);
+			conn->login_port = ntohs(sock_in6.sin6_port);
+		}
+
+		rc = conn->sock->ops->getname(conn->sock,
+				(struct sockaddr *)&sock_in6, &err, 0);
+		if (!rc) {
+			snprintf(conn->local_ip, sizeof(conn->local_ip), "%pI6c",
+				&sock_in6.sin6_addr.in6_u);
+			conn->local_port = ntohs(sock_in6.sin6_port);
+		}
+	} else {
+		memset(&sock_in, 0, sizeof(struct sockaddr_in));
+
+		rc = conn->sock->ops->getname(conn->sock,
+				(struct sockaddr *)&sock_in, &err, 1);
+		if (!rc) {
+			sprintf(conn->login_ip, "%pI4",
+					&sock_in.sin_addr.s_addr);
+			conn->login_port = ntohs(sock_in.sin_port);
+		}
+
+		rc = conn->sock->ops->getname(conn->sock,
+				(struct sockaddr *)&sock_in, &err, 0);
+		if (!rc) {
+			sprintf(conn->local_ip, "%pI4",
+					&sock_in.sin_addr.s_addr);
+			conn->local_port = ntohs(sock_in.sin_port);
+		}
+	}
+
+	return 0;
+}
+
+int iscsit_get_login_rx(struct iscsi_conn *conn, struct iscsi_login *login)
+{
+	struct iscsi_login_req *login_req;
+	u32 padding = 0, payload_length;
+
+	if (iscsi_login_rx_data(conn, login->req, ISCSI_HDR_LEN) < 0)
+		return -1;
+
+	login_req = (struct iscsi_login_req *)login->req;
+	payload_length	= ntoh24(login_req->dlength);
+	padding = ((-payload_length) & 3);
+
+	pr_debug("Got Login Command, Flags 0x%02x, ITT: 0x%08x,"
+		" CmdSN: 0x%08x, ExpStatSN: 0x%08x, CID: %hu, Length: %u\n",
+		login_req->flags, login_req->itt, login_req->cmdsn,
+		login_req->exp_statsn, login_req->cid, payload_length);
+	/*
+	 * Setup the initial iscsi_login values from the leading
+	 * login request PDU.
+	 */
+	if (login->first_request) {
+		login_req = (struct iscsi_login_req *)login->req;
+		login->leading_connection = (!login_req->tsih) ? 1 : 0;
+		login->current_stage	= ISCSI_LOGIN_CURRENT_STAGE(login_req->flags);
+		login->version_min	= login_req->min_version;
+		login->version_max	= login_req->max_version;
+		memcpy(login->isid, login_req->isid, 6);
+		login->cmd_sn		= be32_to_cpu(login_req->cmdsn);
+		login->init_task_tag	= login_req->itt;
+		login->initial_exp_statsn = be32_to_cpu(login_req->exp_statsn);
+		login->cid		= be16_to_cpu(login_req->cid);
+		login->tsih		= be16_to_cpu(login_req->tsih);
+	}
+
+	if (iscsi_target_check_login_request(conn, login) < 0)
+		return -1;
+
+	memset(login->req_buf, 0, MAX_KEY_VALUE_PAIRS);
+	if (iscsi_login_rx_data(conn, login->req_buf,
+				payload_length + padding) < 0)
+		return -1;
+
+	return 0;
+}
+
+int iscsit_put_login_tx(struct iscsi_conn *conn, struct iscsi_login *login,
+			u32 length)
+{
+	if (iscsi_login_tx_data(conn, login->rsp, login->rsp_buf, length) < 0)
+		return -1;
+
+	return 0;
+}
+
+static int
+iscsit_conn_set_transport(struct iscsi_conn *conn, struct iscsit_transport *t)
+{
+	int rc;
+
+	if (!t->owner) {
+		conn->conn_transport = t;
+		return 0;
+	}
+
+	rc = try_module_get(t->owner);
+	if (!rc) {
+		pr_err("try_module_get() failed for %s\n", t->name);
+		return -EINVAL;
+	}
+
+	conn->conn_transport = t;
+	return 0;
+}
+
 static int __iscsi_target_login_thread(struct iscsi_np *np)
 {
-	u8 buffer[ISCSI_HDR_LEN], iscsi_opcode, zero_tsih = 0;
-	int err, ret = 0, stop;
+	u8 *buffer, zero_tsih = 0;
+	int ret = 0, rc, stop;
 	struct iscsi_conn *conn = NULL;
 	struct iscsi_login *login;
 	struct iscsi_portal_group *tpg = NULL;
-	struct socket *new_sock, *sock;
-	struct kvec iov;
 	struct iscsi_login_req *pdu;
-	struct sockaddr_in sock_in;
-	struct sockaddr_in6 sock_in6;
 
 	flush_signals(current);
-	sock = np->np_socket;
 
 	spin_lock_bh(&np->np_thread_lock);
 	if (np->np_thread_state == ISCSI_NP_THREAD_RESET) {
@@ -886,75 +1139,76 @@ static int __iscsi_target_login_thread(struct iscsi_np *np)
 	}
 	spin_unlock_bh(&np->np_thread_lock);
 
-	if (kernel_accept(sock, &new_sock, 0) < 0) {
-		spin_lock_bh(&np->np_thread_lock);
-		if (np->np_thread_state == ISCSI_NP_THREAD_RESET) {
-			spin_unlock_bh(&np->np_thread_lock);
-			complete(&np->np_restart_comp);
-			/* Get another socket */
-			return 1;
-		}
-		spin_unlock_bh(&np->np_thread_lock);
-		goto out;
-	}
-	iscsi_start_login_thread_timer(np);
-
 	conn = kzalloc(sizeof(struct iscsi_conn), GFP_KERNEL);
 	if (!conn) {
 		pr_err("Could not allocate memory for"
 			" new connection\n");
-		sock_release(new_sock);
 		/* Get another socket */
 		return 1;
 	}
-
 	pr_debug("Moving to TARG_CONN_STATE_FREE.\n");
 	conn->conn_state = TARG_CONN_STATE_FREE;
-	conn->sock = new_sock;
 
-	pr_debug("Moving to TARG_CONN_STATE_XPT_UP.\n");
-	conn->conn_state = TARG_CONN_STATE_XPT_UP;
+	if (iscsit_conn_set_transport(conn, np->np_transport) < 0) {
+		kfree(conn);
+		return 1;
+	}
 
-	/*
-	 * Allocate conn->conn_ops early as a failure calling
-	 * iscsit_tx_login_rsp() below will call tx_data().
-	 */
-	conn->conn_ops = kzalloc(sizeof(struct iscsi_conn_ops), GFP_KERNEL);
-	if (!conn->conn_ops) {
-		pr_err("Unable to allocate memory for"
-			" struct iscsi_conn_ops.\n");
-		goto new_sess_out;
+	rc = np->np_transport->iscsit_accept_np(np, conn);
+	if (rc == -ENOSYS) {
+		complete(&np->np_restart_comp);
+		iscsit_put_transport(conn->conn_transport);
+		kfree(conn);
+		conn = NULL;
+		goto exit;
+	} else if (rc < 0) {
+		spin_lock_bh(&np->np_thread_lock);
+		if (np->np_thread_state == ISCSI_NP_THREAD_RESET) {
+			spin_unlock_bh(&np->np_thread_lock);
+			complete(&np->np_restart_comp);
+			if (ret == -ENODEV) {
+				iscsit_put_transport(conn->conn_transport);
+				kfree(conn);
+				conn = NULL;
+				goto out;
+			}
+			/* Get another socket */
+			return 1;
+		}
+		spin_unlock_bh(&np->np_thread_lock);
+		iscsit_put_transport(conn->conn_transport);
+		kfree(conn);
+		conn = NULL;
+		goto out;
 	}
 	/*
 	 * Perform the remaining iSCSI connection initialization items..
 	 */
-	if (iscsi_login_init_conn(conn) < 0)
-		goto new_sess_out;
-
-	memset(buffer, 0, ISCSI_HDR_LEN);
-	memset(&iov, 0, sizeof(struct kvec));
-	iov.iov_base	= buffer;
-	iov.iov_len	= ISCSI_HDR_LEN;
-
-	if (rx_data(conn, &iov, 1, ISCSI_HDR_LEN) <= 0) {
-		pr_err("rx_data() returned an error.\n");
+	login = iscsi_login_init_conn(conn);
+	if (!login) {
 		goto new_sess_out;
 	}
 
-	iscsi_opcode = (buffer[0] & ISCSI_OPCODE_MASK);
-	if (!(iscsi_opcode & ISCSI_OP_LOGIN)) {
-		pr_err("First opcode is not login request,"
-			" failing login request.\n");
-		goto new_sess_out;
-	}
+	iscsi_start_login_thread_timer(np);
 
-	pdu			= (struct iscsi_login_req *) buffer;
+	pr_debug("Moving to TARG_CONN_STATE_XPT_UP.\n");
+	conn->conn_state = TARG_CONN_STATE_XPT_UP;
+	/*
+	 * This will process the first login request + payload..
+	 */
+	rc = np->np_transport->iscsit_get_login_rx(conn, login);
+	if (rc == 1)
+		return 1;
+	else if (rc < 0)
+		goto new_sess_out;
 
+	buffer = &login->req[0];
+	pdu = (struct iscsi_login_req *)buffer;
 	/*
 	 * Used by iscsit_tx_login_rsp() for Login Resonses PDUs
 	 * when Status-Class != 0.
 	*/
-	conn->login_itt		= pdu->itt;
+	conn->login_itt	= pdu->itt;
 
 	spin_lock_bh(&np->np_thread_lock);
 	if (np->np_thread_state != ISCSI_NP_THREAD_ACTIVE) {
@@ -967,61 +1221,11 @@ static int __iscsi_target_login_thread(struct iscsi_np *np)
 	}
 	spin_unlock_bh(&np->np_thread_lock);
 
-	if (np->np_sockaddr.ss_family == AF_INET6) {
-		memset(&sock_in6, 0, sizeof(struct sockaddr_in6));
-
-		if (conn->sock->ops->getname(conn->sock,
-				(struct sockaddr *)&sock_in6, &err, 1) < 0) {
-			pr_err("sock_ops->getname() failed.\n");
-			iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-					ISCSI_LOGIN_STATUS_TARGET_ERROR);
-			goto new_sess_out;
-		}
-		snprintf(conn->login_ip, sizeof(conn->login_ip), "%pI6c",
-				&sock_in6.sin6_addr.in6_u);
-		conn->login_port = ntohs(sock_in6.sin6_port);
-
-		if (conn->sock->ops->getname(conn->sock,
-				(struct sockaddr *)&sock_in6, &err, 0) < 0) {
-			pr_err("sock_ops->getname() failed.\n");
-			iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-					ISCSI_LOGIN_STATUS_TARGET_ERROR);
-			goto new_sess_out;
-		}
-		snprintf(conn->local_ip, sizeof(conn->local_ip), "%pI6c",
-				&sock_in6.sin6_addr.in6_u);
-		conn->local_port = ntohs(sock_in6.sin6_port);
-
-	} else {
-		memset(&sock_in, 0, sizeof(struct sockaddr_in));
-
-		if (conn->sock->ops->getname(conn->sock,
-				(struct sockaddr *)&sock_in, &err, 1) < 0) {
-			pr_err("sock_ops->getname() failed.\n");
-			iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-					ISCSI_LOGIN_STATUS_TARGET_ERROR);
-			goto new_sess_out;
-		}
-		sprintf(conn->login_ip, "%pI4", &sock_in.sin_addr.s_addr);
-		conn->login_port = ntohs(sock_in.sin_port);
-
-		if (conn->sock->ops->getname(conn->sock,
-				(struct sockaddr *)&sock_in, &err, 0) < 0) {
-			pr_err("sock_ops->getname() failed.\n");
-			iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-					ISCSI_LOGIN_STATUS_TARGET_ERROR);
-			goto new_sess_out;
-		}
-		sprintf(conn->local_ip, "%pI4", &sock_in.sin_addr.s_addr);
-		conn->local_port = ntohs(sock_in.sin_port);
-	}
-
 	conn->network_transport = np->np_network_transport;
 
 	pr_debug("Received iSCSI login request from %s on %s Network"
-			" Portal %s:%hu\n", conn->login_ip,
-		(conn->network_transport == ISCSI_TCP) ? "TCP" : "SCTP",
-			conn->local_ip, conn->local_port);
+		" Portal %s:%hu\n", conn->login_ip, np->np_transport->name,
+		conn->local_ip, conn->local_port);
 
 	pr_debug("Moving to TARG_CONN_STATE_IN_LOGIN.\n");
 	conn->conn_state	= TARG_CONN_STATE_IN_LOGIN;
@@ -1050,13 +1254,17 @@ static int __iscsi_target_login_thread(struct iscsi_np *np)
 		if (iscsi_login_non_zero_tsih_s1(conn, buffer) < 0)
 			goto new_sess_out;
 	}
-
 	/*
-	 * This will process the first login request, and call
-	 * iscsi_target_locate_portal(), and return a valid struct iscsi_login.
+	 * SessionType: Discovery
+	 *
+	 * 	Locates Default Portal
+	 *
+	 * SessionType: Normal
+	 *
+	 * 	Locates Target Portal from NP -> Target IQN
 	 */
-	login = iscsi_target_init_negotiation(np, conn, buffer);
-	if (!login) {
+	rc = iscsi_target_locate_portal(np, conn, login);
+	if (rc < 0) {
 		tpg = conn->tpg;
 		goto new_sess_out;
 	}
@@ -1068,15 +1276,11 @@ static int __iscsi_target_login_thread(struct iscsi_np *np)
 	}
 
 	if (zero_tsih) {
-		if (iscsi_login_zero_tsih_s2(conn) < 0) {
-			iscsi_target_nego_release(login, conn);
+		if (iscsi_login_zero_tsih_s2(conn) < 0)
 			goto new_sess_out;
-		}
 	} else {
-		if (iscsi_login_non_zero_tsih_s2(conn, buffer) < 0) {
-			iscsi_target_nego_release(login, conn);
+		if (iscsi_login_non_zero_tsih_s2(conn, buffer) < 0)
 			goto old_sess_out;
-		}
 	}
 
 	if (iscsi_target_start_negotiation(login, conn) < 0)
@@ -1153,8 +1357,18 @@ old_sess_out:
 		iscsi_release_param_list(conn->param_list);
 		conn->param_list = NULL;
 	}
-	if (conn->sock)
+	iscsi_target_nego_release(conn);
+
+	if (conn->sock) {
 		sock_release(conn->sock);
+		conn->sock = NULL;
+	}
+
+	if (conn->conn_transport->iscsit_free_conn)
+		conn->conn_transport->iscsit_free_conn(conn);
+
+	iscsit_put_transport(conn->conn_transport);
+
 	kfree(conn);
 
 	if (tpg) {
@@ -1172,11 +1386,13 @@ out:
 	/* Wait for another socket.. */
 	if (!stop)
 		return 1;
-
+exit:
 	iscsi_stop_login_thread_timer(np);
 	spin_lock_bh(&np->np_thread_lock);
 	np->np_thread_state = ISCSI_NP_THREAD_EXIT;
+	np->np_thread = NULL;
 	spin_unlock_bh(&np->np_thread_lock);
+
 	return 0;
 }
 
diff --git a/drivers/target/iscsi/iscsi_target_login.h b/drivers/target/iscsi/iscsi_target_login.h
index 091dcae2532..63efd287845 100644
--- a/drivers/target/iscsi/iscsi_target_login.h
+++ b/drivers/target/iscsi/iscsi_target_login.h
@@ -4,8 +4,14 @@
 extern int iscsi_login_setup_crypto(struct iscsi_conn *);
 extern int iscsi_check_for_session_reinstatement(struct iscsi_conn *);
 extern int iscsi_login_post_auth_non_zero_tsih(struct iscsi_conn *, u16, u32);
+extern int iscsit_setup_np(struct iscsi_np *,
+				struct __kernel_sockaddr_storage *);
 extern int iscsi_target_setup_login_socket(struct iscsi_np *,
 				struct __kernel_sockaddr_storage *);
+extern int iscsit_accept_np(struct iscsi_np *, struct iscsi_conn *);
+extern int iscsit_get_login_rx(struct iscsi_conn *, struct iscsi_login *);
+extern int iscsit_put_login_tx(struct iscsi_conn *, struct iscsi_login *, u32);
+extern void iscsit_free_conn(struct iscsi_np *, struct iscsi_conn *);
 extern int iscsi_target_login_thread(void *);
 extern int iscsi_login_disable_FIM_keys(struct iscsi_param_list *, struct iscsi_conn *);
 
diff --git a/drivers/target/iscsi/iscsi_target_nego.c b/drivers/target/iscsi/iscsi_target_nego.c
index 9d902aefe01..7ad912060e2 100644
--- a/drivers/target/iscsi/iscsi_target_nego.c
+++ b/drivers/target/iscsi/iscsi_target_nego.c
@@ -22,6 +22,7 @@
 #include <scsi/iscsi_proto.h>
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
+#include <target/iscsi/iscsi_transport.h>
 
 #include "iscsi_target_core.h"
 #include "iscsi_target_parameters.h"
@@ -169,7 +170,7 @@ static void iscsi_remove_failed_auth_entry(struct iscsi_conn *conn)
 	kfree(conn->auth_protocol);
 }
 
-static int iscsi_target_check_login_request(
+int iscsi_target_check_login_request(
 	struct iscsi_conn *conn,
 	struct iscsi_login *login)
 {
@@ -200,8 +201,8 @@ static int iscsi_target_check_login_request(
 		return -1;
 	}
 
-	req_csg = (login_req->flags & ISCSI_FLAG_LOGIN_CURRENT_STAGE_MASK) >> 2;
-	req_nsg = (login_req->flags & ISCSI_FLAG_LOGIN_NEXT_STAGE_MASK);
+	req_csg = ISCSI_LOGIN_CURRENT_STAGE(login_req->flags);
+	req_nsg = ISCSI_LOGIN_NEXT_STAGE(login_req->flags);
 
 	if (req_csg != login->current_stage) {
 		pr_err("Initiator unexpectedly changed login stage"
@@ -352,11 +353,8 @@ static int iscsi_target_do_tx_login_io(struct iscsi_conn *conn, struct iscsi_log
 
 	padding = ((-login->rsp_length) & 3);
 
-	if (iscsi_login_tx_data(
-			conn,
-			login->rsp,
-			login->rsp_buf,
-			login->rsp_length + padding) < 0)
+	if (conn->conn_transport->iscsit_put_login_tx(conn, login,
+					login->rsp_length + padding) < 0)
 		return -1;
 
 	login->rsp_length		= 0;
@@ -368,72 +366,12 @@ static int iscsi_target_do_tx_login_io(struct iscsi_conn *conn, struct iscsi_log
 	return 0;
 }
 
-static int iscsi_target_do_rx_login_io(struct iscsi_conn *conn, struct iscsi_login *login)
-{
-	u32 padding = 0, payload_length;
-	struct iscsi_login_req *login_req;
-
-	if (iscsi_login_rx_data(conn, login->req, ISCSI_HDR_LEN) < 0)
-		return -1;
-
-	login_req = (struct iscsi_login_req *) login->req;
-	payload_length			= ntoh24(login_req->dlength);
-
-	pr_debug("Got Login Command, Flags 0x%02x, ITT: 0x%08x,"
-		" CmdSN: 0x%08x, ExpStatSN: 0x%08x, CID: %hu, Length: %u\n",
-		 login_req->flags, login_req->itt, login_req->cmdsn,
-		 login_req->exp_statsn, login_req->cid, payload_length);
-
-	if (iscsi_target_check_login_request(conn, login) < 0)
-		return -1;
-
-	padding = ((-payload_length) & 3);
-	memset(login->req_buf, 0, MAX_KEY_VALUE_PAIRS);
-
-	if (iscsi_login_rx_data(
-			conn,
-			login->req_buf,
-			payload_length + padding) < 0)
-		return -1;
-
-	return 0;
-}
-
 static int iscsi_target_do_login_io(struct iscsi_conn *conn, struct iscsi_login *login)
 {
 	if (iscsi_target_do_tx_login_io(conn, login) < 0)
 		return -1;
 
-	if (iscsi_target_do_rx_login_io(conn, login) < 0)
-		return -1;
-
-	return 0;
-}
-
-static int iscsi_target_get_initial_payload(
-	struct iscsi_conn *conn,
-	struct iscsi_login *login)
-{
-	u32 padding = 0, payload_length;
-	struct iscsi_login_req *login_req;
-
-	login_req = (struct iscsi_login_req *) login->req;
-	payload_length = ntoh24(login_req->dlength);
-
-	pr_debug("Got Login Command, Flags 0x%02x, ITT: 0x%08x,"
-		" CmdSN: 0x%08x, ExpStatSN: 0x%08x, Length: %u\n",
-		login_req->flags, login_req->itt, login_req->cmdsn,
-		login_req->exp_statsn, payload_length);
-
-	if (iscsi_target_check_login_request(conn, login) < 0)
-		return -1;
-
-	padding = ((-payload_length) & 3);
-
-	if (iscsi_login_rx_data(
-			conn,
-			login->req_buf,
-			payload_length + padding) < 0)
+	if (conn->conn_transport->iscsit_get_login_rx(conn, login) < 0)
 		return -1;
 
 	return 0;
@@ -681,9 +619,9 @@ static int iscsi_target_do_login(struct iscsi_conn *conn, struct iscsi_login *lo
 			return -1;
 		}
 
-		switch ((login_req->flags & ISCSI_FLAG_LOGIN_CURRENT_STAGE_MASK) >> 2) {
+		switch (ISCSI_LOGIN_CURRENT_STAGE(login_req->flags)) {
 		case 0:
-			login_rsp->flags |= (0 & ISCSI_FLAG_LOGIN_CURRENT_STAGE_MASK);
+			login_rsp->flags &= ~ISCSI_FLAG_LOGIN_CURRENT_STAGE_MASK;
 			if (iscsi_target_handle_csg_zero(conn, login) < 0)
 				return -1;
 			break;
@@ -693,6 +631,7 @@ static int iscsi_target_do_login(struct iscsi_conn *conn, struct iscsi_login *lo
 				return -1;
 			if (login_rsp->flags & ISCSI_FLAG_LOGIN_TRANSIT) {
 				login->tsih = conn->sess->tsih;
+				login->login_complete = 1;
 				if (iscsi_target_do_tx_login_io(conn,
 						login) < 0)
 					return -1;
@@ -702,8 +641,7 @@ static int iscsi_target_do_login(struct iscsi_conn *conn, struct iscsi_login *lo
 		default:
 			pr_err("Illegal CSG: %d received from"
 				" Initiator, protocol error.\n",
-				(login_req->flags & ISCSI_FLAG_LOGIN_CURRENT_STAGE_MASK)
-				>> 2);
+				ISCSI_LOGIN_CURRENT_STAGE(login_req->flags));
 			break;
 		}
 
@@ -737,7 +675,7 @@ static void iscsi_initiatorname_tolower(
 /*
  * Processes the first Login Request..
  */
-static int iscsi_target_locate_portal(
+int iscsi_target_locate_portal(
 	struct iscsi_np *np,
 	struct iscsi_conn *conn,
 	struct iscsi_login *login)
@@ -753,22 +691,6 @@ static int iscsi_target_locate_portal(
 	login_req = (struct iscsi_login_req *) login->req;
 	payload_length = ntoh24(login_req->dlength);
 
-	login->first_request	= 1;
-	login->leading_connection = (!login_req->tsih) ? 1 : 0;
-	login->current_stage	=
-		(login_req->flags & ISCSI_FLAG_LOGIN_CURRENT_STAGE_MASK) >> 2;
-	login->version_min	= login_req->min_version;
-	login->version_max	= login_req->max_version;
-	memcpy(login->isid, login_req->isid, 6);
-	login->cmd_sn		= be32_to_cpu(login_req->cmdsn);
-	login->init_task_tag	= login_req->itt;
-	login->initial_exp_statsn = be32_to_cpu(login_req->exp_statsn);
-	login->cid		= be16_to_cpu(login_req->cid);
-	login->tsih		= be16_to_cpu(login_req->tsih);
-
-	if (iscsi_target_get_initial_payload(conn, login) < 0)
-		return -1;
-
 	tmpbuf = kzalloc(payload_length + 1, GFP_KERNEL);
 	if (!tmpbuf) {
 		pr_err("Unable to allocate memory for tmpbuf.\n");
@@ -800,6 +722,8 @@ static int iscsi_target_locate_portal(
 		start += strlen(key) + strlen(value) + 2;
 	}
 
+	printk("i_buf: %s, s_buf: %s, t_buf: %s\n", i_buf, s_buf, t_buf);
+
 	/*
 	 * See 5.3.  Login Phase.
 	 */
@@ -958,100 +882,30 @@ out:
 	return ret;
 }
 
-struct iscsi_login *iscsi_target_init_negotiation(
-	struct iscsi_np *np,
-	struct iscsi_conn *conn,
-	char *login_pdu)
-{
-	struct iscsi_login *login;
-
-	login = kzalloc(sizeof(struct iscsi_login), GFP_KERNEL);
-	if (!login) {
-		pr_err("Unable to allocate memory for struct iscsi_login.\n");
-		iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-				ISCSI_LOGIN_STATUS_NO_RESOURCES);
-		return NULL;
-	}
-
-	login->req = kmemdup(login_pdu, ISCSI_HDR_LEN, GFP_KERNEL);
-	if (!login->req) {
-		pr_err("Unable to allocate memory for Login Request.\n");
-		iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-				ISCSI_LOGIN_STATUS_NO_RESOURCES);
-		goto out;
-	}
-
-	login->req_buf = kzalloc(MAX_KEY_VALUE_PAIRS, GFP_KERNEL);
-	if (!login->req_buf) {
-		pr_err("Unable to allocate memory for response buffer.\n");
-		iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-				ISCSI_LOGIN_STATUS_NO_RESOURCES);
-		goto out;
-	}
-	/*
-	 * SessionType: Discovery
-	 *
-	 *	Locates Default Portal
-	 *
-	 * SessionType: Normal
-	 *
-	 *	Locates Target Portal from NP -> Target IQN
-	 */
-	if (iscsi_target_locate_portal(np, conn, login) < 0) {
-		goto out;
-	}
-
-	return login;
-out:
-	kfree(login->req);
-	kfree(login->req_buf);
-	kfree(login);
-
-	return NULL;
-}
-
 int iscsi_target_start_negotiation(
 	struct iscsi_login *login,
 	struct iscsi_conn *conn)
 {
-	int ret = -1;
-
-	login->rsp = kzalloc(ISCSI_HDR_LEN, GFP_KERNEL);
-	if (!login->rsp) {
-		pr_err("Unable to allocate memory for"
-				" Login Response.\n");
-		iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-				ISCSI_LOGIN_STATUS_NO_RESOURCES);
-		ret = -1;
-		goto out;
-	}
-
-	login->rsp_buf = kzalloc(MAX_KEY_VALUE_PAIRS, GFP_KERNEL);
-	if (!login->rsp_buf) {
-		pr_err("Unable to allocate memory for"
-			" request buffer.\n");
-		iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-				ISCSI_LOGIN_STATUS_NO_RESOURCES);
-		ret = -1;
-		goto out;
-	}
+	int ret;
 
 	ret = iscsi_target_do_login(conn, login);
-out:
 	if (ret != 0)
 		iscsi_remove_failed_auth_entry(conn);
 
-	iscsi_target_nego_release(login, conn);
+	iscsi_target_nego_release(conn);
 	return ret;
 }
 
-void iscsi_target_nego_release(
-	struct iscsi_login *login,
-	struct iscsi_conn *conn)
+void iscsi_target_nego_release(struct iscsi_conn *conn)
 {
-	kfree(login->req);
-	kfree(login->rsp);
+	struct iscsi_login *login = conn->conn_login;
+
+	if (!login)
+		return;
+
 	kfree(login->req_buf);
 	kfree(login->rsp_buf);
 	kfree(login);
+
+	conn->conn_login = NULL;
 }
diff --git a/drivers/target/iscsi/iscsi_target_nego.h b/drivers/target/iscsi/iscsi_target_nego.h
index 92e133a5158..f021cbd330e 100644
--- a/drivers/target/iscsi/iscsi_target_nego.h
+++ b/drivers/target/iscsi/iscsi_target_nego.h
@@ -7,11 +7,14 @@
 extern void convert_null_to_semi(char *, int);
 extern int extract_param(const char *, const char *, unsigned int, char *,
 		unsigned char *);
-extern struct iscsi_login *iscsi_target_init_negotiation(
-		struct iscsi_np *, struct iscsi_conn *, char *);
+extern int iscsi_target_check_login_request(struct iscsi_conn *,
+		struct iscsi_login *);
+extern int iscsi_target_get_initial_payload(struct iscsi_conn *,
+		struct iscsi_login *);
+extern int iscsi_target_locate_portal(struct iscsi_np *, struct iscsi_conn *,
+		struct iscsi_login *);
 extern int iscsi_target_start_negotiation(
 		struct iscsi_login *, struct iscsi_conn *);
-extern void iscsi_target_nego_release(
-		struct iscsi_login *, struct iscsi_conn *);
+extern void iscsi_target_nego_release(struct iscsi_conn *);
 
 #endif /* ISCSI_TARGET_NEGO_H */
diff --git a/drivers/target/iscsi/iscsi_target_parameters.c b/drivers/target/iscsi/iscsi_target_parameters.c
index ca2be406f14..f690be9e529 100644
--- a/drivers/target/iscsi/iscsi_target_parameters.c
+++ b/drivers/target/iscsi/iscsi_target_parameters.c
@@ -59,7 +59,7 @@ int iscsi_login_tx_data(
 	char *text_buf,
 	int text_length)
 {
-	int length, tx_sent;
+	int length, tx_sent, iov_cnt = 1;
 	struct kvec iov[2];
 
 	length = (ISCSI_HDR_LEN + text_length);
@@ -67,8 +67,12 @@ int iscsi_login_tx_data(
 	memset(&iov[0], 0, 2 * sizeof(struct kvec));
 	iov[0].iov_len		= ISCSI_HDR_LEN;
 	iov[0].iov_base		= pdu_buf;
-	iov[1].iov_len		= text_length;
-	iov[1].iov_base		= text_buf;
+
+	if (text_buf && text_length) {
+		iov[1].iov_len	= text_length;
+		iov[1].iov_base	= text_buf;
+		iov_cnt++;
+	}
 
 	/*
 	 * Initial Marker-less Interval.
@@ -77,7 +81,7 @@ int iscsi_login_tx_data(
 	 */
 	conn->if_marker += length;
 
-	tx_sent = tx_data(conn, &iov[0], 2, length);
+	tx_sent = tx_data(conn, &iov[0], iov_cnt, length);
 	if (tx_sent != length) {
 		pr_err("tx_data returned %d, expecting %d.\n",
 				tx_sent, length);
@@ -429,6 +433,28 @@ int iscsi_create_default_params(struct iscsi_param_list **param_list_ptr)
 			TYPERANGE_MARKINT, USE_INITIAL_ONLY);
 	if (!param)
 		goto out;
+	/*
+	 * Extra parameters for ISER from RFC-5046
+	 */
+	param = iscsi_set_default_param(pl, RDMAEXTENTIONS, INITIAL_RDMAEXTENTIONS,
+			PHASE_OPERATIONAL, SCOPE_SESSION_WIDE, SENDER_BOTH,
+			TYPERANGE_BOOL_AND, USE_LEADING_ONLY);
+	if (!param)
+		goto out;
+
+	param = iscsi_set_default_param(pl, INITIATORRECVDATASEGMENTLENGTH,
+			INITIAL_INITIATORRECVDATASEGMENTLENGTH,
+			PHASE_OPERATIONAL, SCOPE_CONNECTION_ONLY, SENDER_BOTH,
+			TYPERANGE_512_TO_16777215, USE_ALL);
+	if (!param)
+		goto out;
+
+	param = iscsi_set_default_param(pl, TARGETRECVDATASEGMENTLENGTH,
+			INITIAL_TARGETRECVDATASEGMENTLENGTH,
+			PHASE_OPERATIONAL, SCOPE_CONNECTION_ONLY, SENDER_BOTH,
+			TYPERANGE_512_TO_16777215, USE_ALL);
+	if (!param)
+		goto out;
 
 	*param_list_ptr = pl;
 	return 0;
@@ -438,19 +464,23 @@ out:
 }
 
 int iscsi_set_keys_to_negotiate(
-	int sessiontype,
-	struct iscsi_param_list *param_list)
+	struct iscsi_param_list *param_list,
+	bool iser)
 {
 	struct iscsi_param *param;
 
+	param_list->iser = iser;
+
 	list_for_each_entry(param, &param_list->param_list, p_list) {
 		param->state = 0;
 		if (!strcmp(param->name, AUTHMETHOD)) {
 			SET_PSTATE_NEGOTIATE(param);
 		} else if (!strcmp(param->name, HEADERDIGEST)) {
-			SET_PSTATE_NEGOTIATE(param);
+			if (iser == false)
+				SET_PSTATE_NEGOTIATE(param);
 		} else if (!strcmp(param->name, DATADIGEST)) {
-			SET_PSTATE_NEGOTIATE(param);
+			if (iser == false)
+				SET_PSTATE_NEGOTIATE(param);
 		} else if (!strcmp(param->name, MAXCONNECTIONS)) {
 			SET_PSTATE_NEGOTIATE(param);
 		} else if (!strcmp(param->name, TARGETNAME)) {
@@ -469,7 +499,8 @@ int iscsi_set_keys_to_negotiate(
 		} else if (!strcmp(param->name, IMMEDIATEDATA)) {
 			SET_PSTATE_NEGOTIATE(param);
 		} else if (!strcmp(param->name, MAXRECVDATASEGMENTLENGTH)) {
-			SET_PSTATE_NEGOTIATE(param);
+			if (iser == false)
+				SET_PSTATE_NEGOTIATE(param);
 		} else if (!strcmp(param->name, MAXXMITDATASEGMENTLENGTH)) {
 			continue;
 		} else if (!strcmp(param->name, MAXBURSTLENGTH)) {
@@ -498,6 +529,15 @@ int iscsi_set_keys_to_negotiate(
 			SET_PSTATE_NEGOTIATE(param);
 		} else if (!strcmp(param->name, OFMARKINT)) {
 			SET_PSTATE_NEGOTIATE(param);
+		} else if (!strcmp(param->name, RDMAEXTENTIONS)) {
+			if (iser == true)
+				SET_PSTATE_NEGOTIATE(param);
+		} else if (!strcmp(param->name, INITIATORRECVDATASEGMENTLENGTH)) {
+			if (iser == true)
+				SET_PSTATE_NEGOTIATE(param);
+		} else if (!strcmp(param->name, TARGETRECVDATASEGMENTLENGTH)) {
+			if (iser == true)
+				SET_PSTATE_NEGOTIATE(param);
 		}
 	}
 
@@ -540,6 +580,12 @@ int iscsi_set_keys_irrelevant_for_discovery(
 			param->state &= ~PSTATE_NEGOTIATE;
 		else if (!strcmp(param->name, OFMARKINT))
 			param->state &= ~PSTATE_NEGOTIATE;
+		else if (!strcmp(param->name, RDMAEXTENTIONS))
+			param->state &= ~PSTATE_NEGOTIATE;
+		else if (!strcmp(param->name, INITIATORRECVDATASEGMENTLENGTH))
+			param->state &= ~PSTATE_NEGOTIATE;
+		else if (!strcmp(param->name, TARGETRECVDATASEGMENTLENGTH))
+			param->state &= ~PSTATE_NEGOTIATE;
 	}
 
 	return 0;
@@ -1755,6 +1801,9 @@ void iscsi_set_connection_parameters(
 		 * this key is not sent over the wire.
 		 */
 		if (!strcmp(param->name, MAXXMITDATASEGMENTLENGTH)) {
+			if (param_list->iser == true)
+				continue;
+
 			ops->MaxXmitDataSegmentLength =
 				simple_strtoul(param->value, &tmpptr, 0);
 			pr_debug("MaxXmitDataSegmentLength:     %s\n",
@@ -1800,6 +1849,22 @@ void iscsi_set_connection_parameters(
 				simple_strtoul(param->value, &tmpptr, 0);
 			pr_debug("IFMarkInt:                    %s\n",
 				param->value);
+		} else if (!strcmp(param->name, INITIATORRECVDATASEGMENTLENGTH)) {
+			ops->InitiatorRecvDataSegmentLength =
+				simple_strtoul(param->value, &tmpptr, 0);
+			pr_debug("InitiatorRecvDataSegmentLength: %s\n",
+				param->value);
+			ops->MaxRecvDataSegmentLength =
+					ops->InitiatorRecvDataSegmentLength;
+			pr_debug("Set MRDSL from InitiatorRecvDataSegmentLength\n");
+		} else if (!strcmp(param->name, TARGETRECVDATASEGMENTLENGTH)) {
+			ops->TargetRecvDataSegmentLength =
+				simple_strtoul(param->value, &tmpptr, 0);
+			pr_debug("TargetRecvDataSegmentLength:  %s\n",
+				param->value);
+			ops->MaxXmitDataSegmentLength =
+					ops->TargetRecvDataSegmentLength;
+			pr_debug("Set MXDSL from TargetRecvDataSegmentLength\n");
 		}
 	}
 	pr_debug("----------------------------------------------------"
@@ -1912,6 +1977,10 @@ void iscsi_set_session_parameters(
 			ops->SessionType = !strcmp(param->value, DISCOVERY);
 			pr_debug("SessionType:                  %s\n",
 				param->value);
+		} else if (!strcmp(param->name, RDMAEXTENTIONS)) {
+			ops->RDMAExtensions = !strcmp(param->value, YES);
+			pr_debug("RDMAExtensions:               %s\n",
+				param->value);
 		}
 	}
 	pr_debug("----------------------------------------------------"
diff --git a/drivers/target/iscsi/iscsi_target_parameters.h b/drivers/target/iscsi/iscsi_target_parameters.h
index 1e1b7504a76..f31b9c4b83f 100644
--- a/drivers/target/iscsi/iscsi_target_parameters.h
+++ b/drivers/target/iscsi/iscsi_target_parameters.h
@@ -27,7 +27,7 @@ extern void iscsi_dump_conn_ops(struct iscsi_conn_ops *);
 extern void iscsi_dump_sess_ops(struct iscsi_sess_ops *);
 extern void iscsi_print_params(struct iscsi_param_list *);
 extern int iscsi_create_default_params(struct iscsi_param_list **);
-extern int iscsi_set_keys_to_negotiate(int, struct iscsi_param_list *);
+extern int iscsi_set_keys_to_negotiate(struct iscsi_param_list *, bool);
 extern int iscsi_set_keys_irrelevant_for_discovery(struct iscsi_param_list *);
 extern int iscsi_copy_param_list(struct iscsi_param_list **,
 			struct iscsi_param_list *, int);
@@ -89,6 +89,13 @@ extern void iscsi_set_session_parameters(struct iscsi_sess_ops *,
 #define X_EXTENSIONKEY_CISCO_OLD	"X-com.cisco.iscsi.draft"
 
 /*
+ * Parameter names of iSCSI Extentions for RDMA (iSER).  See RFC-5046
+ */
+#define RDMAEXTENTIONS			"RDMAExtensions"
+#define INITIATORRECVDATASEGMENTLENGTH	"InitiatorRecvDataSegmentLength"
+#define TARGETRECVDATASEGMENTLENGTH	"TargetRecvDataSegmentLength"
+
+/*
  * For AuthMethod.
  */
 #define KRB5				"KRB5"
@@ -133,6 +140,13 @@ extern void iscsi_set_session_parameters(struct iscsi_sess_ops *,
 #define INITIAL_OFMARKINT			"2048~65535"
 
 /*
+ * Initial values for iSER parameters following RFC-5046 Section 6
+ */
+#define INITIAL_RDMAEXTENTIONS			NO
+#define INITIAL_INITIATORRECVDATASEGMENTLENGTH	"262144"
+#define INITIAL_TARGETRECVDATASEGMENTLENGTH	"8192"
+
+/*
  * For [Header,Data]Digests.
  */
 #define CRC32C				"CRC32C"
diff --git a/drivers/target/iscsi/iscsi_target_tmr.c b/drivers/target/iscsi/iscsi_target_tmr.c
index 9d4417aae92..b997e5da47d 100644
--- a/drivers/target/iscsi/iscsi_target_tmr.c
+++ b/drivers/target/iscsi/iscsi_target_tmr.c
@@ -23,6 +23,7 @@
 #include <scsi/iscsi_proto.h>
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
+#include <target/iscsi/iscsi_transport.h>
 
 #include "iscsi_target_core.h"
 #include "iscsi_target_seq_pdu_list.h"
@@ -301,7 +302,7 @@ static int iscsit_task_reassign_complete_write(
 	/*
 	 * iscsit_build_r2ts_for_cmd() can handle the rest from here.
 	 */
-	return iscsit_build_r2ts_for_cmd(cmd, conn, true);
+	return conn->conn_transport->iscsit_get_dataout(conn, cmd, true);
 }
 
 static int iscsit_task_reassign_complete_read(
@@ -471,6 +472,7 @@ int iscsit_tmr_post_handler(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
 
 	return 0;
 }
+EXPORT_SYMBOL(iscsit_tmr_post_handler);
 
 /*
  *	Nothing to do here, but leave it for good measure. :-)
diff --git a/drivers/target/iscsi/iscsi_target_tpg.c b/drivers/target/iscsi/iscsi_target_tpg.c
index ee8f8c66248..439260b7d87 100644
--- a/drivers/target/iscsi/iscsi_target_tpg.c
+++ b/drivers/target/iscsi/iscsi_target_tpg.c
@@ -31,6 +31,8 @@
 #include "iscsi_target.h"
 #include "iscsi_target_parameters.h"
 
+#include <target/iscsi/iscsi_transport.h>
+
 struct iscsi_portal_group *iscsit_alloc_portal_group(struct iscsi_tiqn *tiqn, u16 tpgt)
 {
 	struct iscsi_portal_group *tpg;
@@ -508,7 +510,7 @@ struct iscsi_tpg_np *iscsit_tpg_add_network_portal(
 
 	pr_debug("CORE[%s] - Added Network Portal: %s:%hu,%hu on %s\n",
 		tpg->tpg_tiqn->tiqn, np->np_ip, np->np_port, tpg->tpgt,
-		(np->np_network_transport == ISCSI_TCP) ? "TCP" : "SCTP");
+		np->np_transport->name);
 
 	return tpg_np;
 }
@@ -522,7 +524,7 @@ static int iscsit_tpg_release_np(
 
 	pr_debug("CORE[%s] - Removed Network Portal: %s:%hu,%hu on %s\n",
 		tpg->tpg_tiqn->tiqn, np->np_ip, np->np_port, tpg->tpgt,
-		(np->np_network_transport == ISCSI_TCP) ? "TCP" : "SCTP");
+		np->np_transport->name);
 
 	tpg_np->tpg_np = NULL;
 	tpg_np->tpg = NULL;
diff --git a/drivers/target/iscsi/iscsi_target_transport.c b/drivers/target/iscsi/iscsi_target_transport.c
new file mode 100644
index 00000000000..882728fac30
--- /dev/null
+++ b/drivers/target/iscsi/iscsi_target_transport.c
@@ -0,0 +1,55 @@
+#include <linux/spinlock.h>
+#include <linux/list.h>
+#include <target/iscsi/iscsi_transport.h>
+
+static LIST_HEAD(g_transport_list);
+static DEFINE_MUTEX(transport_mutex);
+
+struct iscsit_transport *iscsit_get_transport(int type)
+{
+	struct iscsit_transport *t;
+
+	mutex_lock(&transport_mutex);
+	list_for_each_entry(t, &g_transport_list, t_node) {
+		if (t->transport_type == type) {
+			if (t->owner && !try_module_get(t->owner)) {
+				t = NULL;
+			}
+			mutex_unlock(&transport_mutex);
+			return t;
+		}
+	}
+	mutex_unlock(&transport_mutex);
+
+	return NULL;
+}
+
+void iscsit_put_transport(struct iscsit_transport *t)
+{
+	if (t->owner)
+		module_put(t->owner);
+}
+
+int iscsit_register_transport(struct iscsit_transport *t)
+{
+	INIT_LIST_HEAD(&t->t_node);
+
+	mutex_lock(&transport_mutex);
+	list_add_tail(&t->t_node, &g_transport_list);
+	mutex_unlock(&transport_mutex);
+
+	pr_debug("Registered iSCSI transport: %s\n", t->name);
+
+	return 0;
+}
+EXPORT_SYMBOL(iscsit_register_transport);
+
+void iscsit_unregister_transport(struct iscsit_transport *t)
+{
+	mutex_lock(&transport_mutex);
+	list_del(&t->t_node);
+	mutex_unlock(&transport_mutex);
+
+	pr_debug("Unregistered iSCSI transport: %s\n", t->name);
+}
+EXPORT_SYMBOL(iscsit_unregister_transport);
diff --git a/drivers/target/iscsi/iscsi_target_util.c b/drivers/target/iscsi/iscsi_target_util.c
index 7ce350578c8..2cc6c9a3ffb 100644
--- a/drivers/target/iscsi/iscsi_target_util.c
+++ b/drivers/target/iscsi/iscsi_target_util.c
@@ -24,6 +24,7 @@
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
 #include <target/target_core_configfs.h>
+#include <target/iscsi/iscsi_transport.h>
 
 #include "iscsi_target_core.h"
 #include "iscsi_target_parameters.h"
@@ -148,6 +149,18 @@ void iscsit_free_r2ts_from_list(struct iscsi_cmd *cmd)
 	spin_unlock_bh(&cmd->r2t_lock);
 }
 
+struct iscsi_cmd *iscsit_alloc_cmd(struct iscsi_conn *conn, gfp_t gfp_mask)
+{
+	struct iscsi_cmd *cmd;
+
+	cmd = kmem_cache_zalloc(lio_cmd_cache, gfp_mask);
+	if (!cmd)
+		return NULL;
+
+	cmd->release_cmd = &iscsit_release_cmd;
+	return cmd;
+}
+
 /*
  * May be called from software interrupt (timer) context for allocating
  * iSCSI NopINs.
@@ -156,13 +169,12 @@ struct iscsi_cmd *iscsit_allocate_cmd(struct iscsi_conn *conn, gfp_t gfp_mask)
 {
 	struct iscsi_cmd *cmd;
 
-	cmd = kmem_cache_zalloc(lio_cmd_cache, gfp_mask);
+	cmd = conn->conn_transport->iscsit_alloc_cmd(conn, gfp_mask);
 	if (!cmd) {
 		pr_err("Unable to allocate memory for struct iscsi_cmd.\n");
 		return NULL;
 	}
-
-	cmd->conn	= conn;
+	cmd->conn = conn;
 	INIT_LIST_HEAD(&cmd->i_conn_node);
 	INIT_LIST_HEAD(&cmd->datain_list);
 	INIT_LIST_HEAD(&cmd->cmd_r2t_list);
@@ -175,6 +187,7 @@ struct iscsi_cmd *iscsit_allocate_cmd(struct iscsi_conn *conn, gfp_t gfp_mask)
 
 	return cmd;
 }
+EXPORT_SYMBOL(iscsit_allocate_cmd);
 
 struct iscsi_seq *iscsit_get_seq_holder_for_datain(
 	struct iscsi_cmd *cmd,
@@ -304,6 +317,7 @@ int iscsit_sequence_cmd(
 
 	return ret;
 }
+EXPORT_SYMBOL(iscsit_sequence_cmd);
 
 int iscsit_check_unsolicited_dataout(struct iscsi_cmd *cmd, unsigned char *buf)
 {
@@ -689,6 +703,11 @@ void iscsit_free_cmd(struct iscsi_cmd *cmd)
 	 */
 	switch (cmd->iscsi_opcode) {
 	case ISCSI_OP_SCSI_CMD:
+		if (cmd->data_direction == DMA_TO_DEVICE)
+			iscsit_stop_dataout_timer(cmd);
+		/*
+		 * Fallthrough
+		 */
 	case ISCSI_OP_SCSI_TMFUNC:
 		transport_generic_free_cmd(&cmd->se_cmd, 1);
 		break;
@@ -704,7 +723,7 @@ void iscsit_free_cmd(struct iscsi_cmd *cmd)
 		}
 		/* Fall-through */
 	default:
-		iscsit_release_cmd(cmd);
+		cmd->release_cmd(cmd);
 		break;
 	}
 }
@@ -1226,34 +1245,19 @@ send_datacrc:
  */
 int iscsit_tx_login_rsp(struct iscsi_conn *conn, u8 status_class, u8 status_detail)
 {
-	u8 iscsi_hdr[ISCSI_HDR_LEN];
-	int err;
-	struct kvec iov;
 	struct iscsi_login_rsp *hdr;
+	struct iscsi_login *login = conn->conn_login;
 
+	login->login_failed = 1;
 	iscsit_collect_login_stats(conn, status_class, status_detail);
 
-	memset(&iov, 0, sizeof(struct kvec));
-	memset(&iscsi_hdr, 0x0, ISCSI_HDR_LEN);
-
-	hdr	= (struct iscsi_login_rsp *)&iscsi_hdr;
+	hdr	= (struct iscsi_login_rsp *)&login->rsp[0];
 	hdr->opcode		= ISCSI_OP_LOGIN_RSP;
 	hdr->status_class	= status_class;
 	hdr->status_detail	= status_detail;
 	hdr->itt		= conn->login_itt;
 
-	iov.iov_base		= &iscsi_hdr;
-	iov.iov_len		= ISCSI_HDR_LEN;
-
-	PRINT_BUFF(iscsi_hdr, ISCSI_HDR_LEN);
-
-	err = tx_data(conn, &iov, 1, ISCSI_HDR_LEN);
-	if (err != ISCSI_HDR_LEN) {
-		pr_err("tx_data returned less than expected\n");
-		return -1;
-	}
-
-	return 0;
+	return conn->conn_transport->iscsit_put_login_tx(conn, login, 0);
 }
 
 void iscsit_print_session_params(struct iscsi_session *sess)
@@ -1432,7 +1436,8 @@ void iscsit_collect_login_stats(
 		strcpy(ls->last_intr_fail_name,
 		       (intrname ? intrname->value : "Unknown"));
 
-		ls->last_intr_fail_ip_family = conn->sock->sk->sk_family;
+		ls->last_intr_fail_ip_family = conn->login_family;
+
 		snprintf(ls->last_intr_fail_ip_addr, IPV6_ADDRESS_SPACE,
 				"%s", conn->login_ip);
 		ls->last_fail_time = get_jiffies_64();
diff --git a/drivers/target/iscsi/iscsi_target_util.h b/drivers/target/iscsi/iscsi_target_util.h
index 894d0f83792..4f8e01a4708 100644
--- a/drivers/target/iscsi/iscsi_target_util.h
+++ b/drivers/target/iscsi/iscsi_target_util.h
@@ -8,6 +8,7 @@ extern struct iscsi_r2t *iscsit_get_r2t_for_eos(struct iscsi_cmd *, u32, u32);
 extern struct iscsi_r2t *iscsit_get_r2t_from_list(struct iscsi_cmd *);
 extern void iscsit_free_r2t(struct iscsi_r2t *, struct iscsi_cmd *);
 extern void iscsit_free_r2ts_from_list(struct iscsi_cmd *);
+extern struct iscsi_cmd *iscsit_alloc_cmd(struct iscsi_conn *, gfp_t);
 extern struct iscsi_cmd *iscsit_allocate_cmd(struct iscsi_conn *, gfp_t);
 extern struct iscsi_seq *iscsit_get_seq_holder_for_datain(struct iscsi_cmd *, u32);
 extern struct iscsi_seq *iscsit_get_seq_holder_for_r2t(struct iscsi_cmd *);
diff --git a/drivers/target/target_core_alua.c b/drivers/target/target_core_alua.c
index ff1c5ee352c..cbe48ab4174 100644
--- a/drivers/target/target_core_alua.c
+++ b/drivers/target/target_core_alua.c
@@ -409,6 +409,7 @@ static inline int core_alua_state_standby(
 	case REPORT_LUNS:
 	case RECEIVE_DIAGNOSTIC:
 	case SEND_DIAGNOSTIC:
+		return 0;
 	case MAINTENANCE_IN:
 		switch (cdb[1] & 0x1f) {
 		case MI_REPORT_TARGET_PGS:
@@ -451,6 +452,7 @@ static inline int core_alua_state_unavailable(
 	switch (cdb[0]) {
 	case INQUIRY:
 	case REPORT_LUNS:
+		return 0;
 	case MAINTENANCE_IN:
 		switch (cdb[1] & 0x1f) {
 		case MI_REPORT_TARGET_PGS:
@@ -491,6 +493,7 @@ static inline int core_alua_state_transition(
 	switch (cdb[0]) {
 	case INQUIRY:
 	case REPORT_LUNS:
+		return 0;
 	case MAINTENANCE_IN:
 		switch (cdb[1] & 0x1f) {
 		case MI_REPORT_TARGET_PGS:
diff --git a/drivers/target/target_core_file.c b/drivers/target/target_core_file.c
index 17a6acbc3ab..58ed683e04a 100644
--- a/drivers/target/target_core_file.c
+++ b/drivers/target/target_core_file.c
@@ -30,8 +30,10 @@
 #include <linux/slab.h>
 #include <linux/spinlock.h>
 #include <linux/module.h>
+#include <linux/falloc.h>
 #include <scsi/scsi.h>
 #include <scsi/scsi_host.h>
+#include <asm/unaligned.h>
 
 #include <target/target_core_base.h>
 #include <target/target_core_backend.h>
@@ -166,6 +168,33 @@ static int fd_configure_device(struct se_device *dev)
 			" block_device blocks: %llu logical_block_size: %d\n",
 			dev_size, div_u64(dev_size, fd_dev->fd_block_size),
 			fd_dev->fd_block_size);
+		/*
+		 * Check if the underlying struct block_device request_queue supports
+		 * the QUEUE_FLAG_DISCARD bit for UNMAP/WRITE_SAME in SCSI + TRIM
+		 * in ATA and we need to set TPE=1
+		 */
+		if (blk_queue_discard(q)) {
+			dev->dev_attrib.max_unmap_lba_count =
+				q->limits.max_discard_sectors;
+			/*
+			 * Currently hardcoded to 1 in Linux/SCSI code..
+			 */
+			dev->dev_attrib.max_unmap_block_desc_count = 1;
+			dev->dev_attrib.unmap_granularity =
+				q->limits.discard_granularity >> 9;
+			dev->dev_attrib.unmap_granularity_alignment =
+				q->limits.discard_alignment;
+			pr_debug("IFILE: BLOCK Discard support available,"
+					" disabled by default\n");
+		}
+		/*
+		 * Enable write same emulation for IBLOCK and use 0xFFFF as
+		 * the smaller WRITE_SAME(10) only has a two-byte block count.
+		 */
+		dev->dev_attrib.max_write_same_len = 0xFFFF;
+
+		if (blk_queue_nonrot(q))
+			dev->dev_attrib.is_nonrot = 1;
 	} else {
 		if (!(fd_dev->fbd_flags & FBDF_HAS_SIZE)) {
 			pr_err("FILEIO: Missing fd_dev_size="
@@ -176,6 +205,23 @@ static int fd_configure_device(struct se_device *dev)
 
 		dev->dev_attrib.hw_block_size = FD_BLOCKSIZE;
 		dev->dev_attrib.hw_max_sectors = FD_MAX_SECTORS;
+
+		/*
+		 * Limit UNMAP emulation to 8k Number of LBAs (NoLB)
+		 */
+		dev->dev_attrib.max_unmap_lba_count = 0x2000;
+		/*
+		 * Currently hardcoded to 1 in Linux/SCSI code..
+		 */
+		dev->dev_attrib.max_unmap_block_desc_count = 1;
+		dev->dev_attrib.unmap_granularity = 1;
+		dev->dev_attrib.unmap_granularity_alignment = 0;
+
+		/*
+		 * Limit WRITE_SAME w/ UNMAP=0 emulation to 8k Number of LBAs (NoLB)
+		 * based upon struct iovec limit for vfs_writev()
+		 */
+		dev->dev_attrib.max_write_same_len = 0x1000;
 	}
 
 	fd_dev->fd_block_size = dev->dev_attrib.hw_block_size;
@@ -190,11 +236,6 @@ static int fd_configure_device(struct se_device *dev)
 
 	fd_dev->fd_dev_id = fd_host->fd_host_dev_id_count++;
 	fd_dev->fd_queue_depth = dev->queue_depth;
-	/*
-	 * Limit WRITE_SAME w/ UNMAP=0 emulation to 8k Number of LBAs (NoLB)
-	 * based upon struct iovec limit for vfs_writev()
-	 */
-	dev->dev_attrib.max_write_same_len = 0x1000;
 
 	pr_debug("CORE_FILE[%u] - Added TCM FILEIO Device ID: %u at %s,"
 		" %llu total bytes\n", fd_host->fd_host_id, fd_dev->fd_dev_id,
@@ -442,6 +483,75 @@ fd_execute_write_same(struct se_cmd *cmd)
 }
 
 static sense_reason_t
+fd_do_unmap(struct se_cmd *cmd, void *priv, sector_t lba, sector_t nolb)
+{
+	struct file *file = priv;
+	struct inode *inode = file->f_mapping->host;
+	int ret;
+
+	if (S_ISBLK(inode->i_mode)) {
+		/* The backend is block device, use discard */
+		struct block_device *bdev = inode->i_bdev;
+
+		ret = blkdev_issue_discard(bdev, lba,
+				nolb, GFP_KERNEL, 0);
+		if (ret < 0) {
+			pr_warn("FILEIO: blkdev_issue_discard() failed: %d\n",
+				ret);
+			return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
+		}
+	} else {
+		/* The backend is normal file, use fallocate */
+		struct se_device *se_dev = cmd->se_dev;
+		loff_t pos = lba * se_dev->dev_attrib.block_size;
+		unsigned int len = nolb * se_dev->dev_attrib.block_size;
+		int mode = FALLOC_FL_PUNCH_HOLE | FALLOC_FL_KEEP_SIZE;
+
+		if (!file->f_op->fallocate)
+			return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
+
+		ret = file->f_op->fallocate(file, mode, pos, len);
+		if (ret < 0) {
+			pr_warn("FILEIO: fallocate() failed: %d\n", ret);
+			return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
+		}
+	}
+
+	return 0;
+}
+
+static sense_reason_t
+fd_execute_write_same_unmap(struct se_cmd *cmd)
+{
+	struct se_device *se_dev = cmd->se_dev;
+	struct fd_dev *fd_dev = FD_DEV(se_dev);
+	struct file *file = fd_dev->fd_file;
+	sector_t lba = cmd->t_task_lba;
+	sector_t nolb = sbc_get_write_same_sectors(cmd);
+	int ret;
+
+	if (!nolb) {
+		target_complete_cmd(cmd, SAM_STAT_GOOD);
+		return 0;
+	}
+
+	ret = fd_do_unmap(cmd, file, lba, nolb);
+	if (ret)
+		return ret;
+
+	target_complete_cmd(cmd, GOOD);
+	return 0;
+}
+
+static sense_reason_t
+fd_execute_unmap(struct se_cmd *cmd)
+{
+	struct file *file = FD_DEV(cmd->se_dev)->fd_file;
+
+	return sbc_execute_unmap(cmd, fd_do_unmap, file);
+}
+
+static sense_reason_t
 fd_execute_rw(struct se_cmd *cmd)
 {
 	struct scatterlist *sgl = cmd->t_data_sg;
@@ -600,6 +710,8 @@ static struct sbc_ops fd_sbc_ops = {
 	.execute_rw		= fd_execute_rw,
 	.execute_sync_cache	= fd_execute_sync_cache,
 	.execute_write_same	= fd_execute_write_same,
+	.execute_write_same_unmap = fd_execute_write_same_unmap,
+	.execute_unmap		= fd_execute_unmap,
 };
 
 static sense_reason_t
diff --git a/drivers/target/target_core_iblock.c b/drivers/target/target_core_iblock.c
index 8bcc514ec8b..07f5f94634b 100644
--- a/drivers/target/target_core_iblock.c
+++ b/drivers/target/target_core_iblock.c
@@ -380,104 +380,40 @@ iblock_execute_sync_cache(struct se_cmd *cmd)
 }
 
 static sense_reason_t
-iblock_execute_unmap(struct se_cmd *cmd)
+iblock_do_unmap(struct se_cmd *cmd, void *priv,
+		sector_t lba, sector_t nolb)
 {
-	struct se_device *dev = cmd->se_dev;
-	struct iblock_dev *ib_dev = IBLOCK_DEV(dev);
-	unsigned char *buf, *ptr = NULL;
-	sector_t lba;
-	int size;
-	u32 range;
-	sense_reason_t ret = 0;
-	int dl, bd_dl, err;
-
-	/* We never set ANC_SUP */
-	if (cmd->t_task_cdb[1])
-		return TCM_INVALID_CDB_FIELD;
-
-	if (cmd->data_length == 0) {
-		target_complete_cmd(cmd, SAM_STAT_GOOD);
-		return 0;
-	}
+	struct block_device *bdev = priv;
+	int ret;
 
-	if (cmd->data_length < 8) {
-		pr_warn("UNMAP parameter list length %u too small\n",
-			cmd->data_length);
-		return TCM_PARAMETER_LIST_LENGTH_ERROR;
-	}
-
-	buf = transport_kmap_data_sg(cmd);
-	if (!buf)
+	ret = blkdev_issue_discard(bdev, lba, nolb, GFP_KERNEL, 0);
+	if (ret < 0) {
+		pr_err("blkdev_issue_discard() failed: %d\n", ret);
 		return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
-
-	dl = get_unaligned_be16(&buf[0]);
-	bd_dl = get_unaligned_be16(&buf[2]);
-
-	size = cmd->data_length - 8;
-	if (bd_dl > size)
-		pr_warn("UNMAP parameter list length %u too small, ignoring bd_dl %u\n",
-			cmd->data_length, bd_dl);
-	else
-		size = bd_dl;
-
-	if (size / 16 > dev->dev_attrib.max_unmap_block_desc_count) {
-		ret = TCM_INVALID_PARAMETER_LIST;
-		goto err;
 	}
 
-	/* First UNMAP block descriptor starts at 8 byte offset */
-	ptr = &buf[8];
-	pr_debug("UNMAP: Sub: %s Using dl: %u bd_dl: %u size: %u"
-		" ptr: %p\n", dev->transport->name, dl, bd_dl, size, ptr);
-
-	while (size >= 16) {
-		lba = get_unaligned_be64(&ptr[0]);
-		range = get_unaligned_be32(&ptr[8]);
-		pr_debug("UNMAP: Using lba: %llu and range: %u\n",
-				 (unsigned long long)lba, range);
-
-		if (range > dev->dev_attrib.max_unmap_lba_count) {
-			ret = TCM_INVALID_PARAMETER_LIST;
-			goto err;
-		}
-
-		if (lba + range > dev->transport->get_blocks(dev) + 1) {
-			ret = TCM_ADDRESS_OUT_OF_RANGE;
-			goto err;
-		}
-
-		err = blkdev_issue_discard(ib_dev->ibd_bd, lba, range,
-					   GFP_KERNEL, 0);
-		if (err < 0) {
-			pr_err("blkdev_issue_discard() failed: %d\n",
-					err);
-			ret = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
-			goto err;
-		}
+	return 0;
+}
 
-		ptr += 16;
-		size -= 16;
-	}
+static sense_reason_t
+iblock_execute_unmap(struct se_cmd *cmd)
+{
+	struct block_device *bdev = IBLOCK_DEV(cmd->se_dev)->ibd_bd;
 
-err:
-	transport_kunmap_data_sg(cmd);
-	if (!ret)
-		target_complete_cmd(cmd, GOOD);
-	return ret;
+	return sbc_execute_unmap(cmd, iblock_do_unmap, bdev);
 }
 
 static sense_reason_t
 iblock_execute_write_same_unmap(struct se_cmd *cmd)
 {
-	struct iblock_dev *ib_dev = IBLOCK_DEV(cmd->se_dev);
-	int rc;
-
-	rc = blkdev_issue_discard(ib_dev->ibd_bd, cmd->t_task_lba,
-			sbc_get_write_same_sectors(cmd), GFP_KERNEL, 0);
-	if (rc < 0) {
-		pr_warn("blkdev_issue_discard() failed: %d\n", rc);
-		return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
-	}
+	struct block_device *bdev = IBLOCK_DEV(cmd->se_dev)->ibd_bd;
+	sector_t lba = cmd->t_task_lba;
+	sector_t nolb = sbc_get_write_same_sectors(cmd);
+	int ret;
+
+	ret = iblock_do_unmap(cmd, bdev, lba, nolb);
+	if (ret)
+		return ret;
 
 	target_complete_cmd(cmd, GOOD);
 	return 0;
diff --git a/drivers/target/target_core_sbc.c b/drivers/target/target_core_sbc.c
index 60d4b5185f3..bbc5b0ee2bd 100644
--- a/drivers/target/target_core_sbc.c
+++ b/drivers/target/target_core_sbc.c
@@ -596,3 +596,88 @@ u32 sbc_get_device_type(struct se_device *dev)
 	return TYPE_DISK;
 }
 EXPORT_SYMBOL(sbc_get_device_type);
+
+sense_reason_t
+sbc_execute_unmap(struct se_cmd *cmd,
+	sense_reason_t (*do_unmap_fn)(struct se_cmd *, void *,
+				      sector_t, sector_t),
+	void *priv)
+{
+	struct se_device *dev = cmd->se_dev;
+	unsigned char *buf, *ptr = NULL;
+	sector_t lba;
+	int size;
+	u32 range;
+	sense_reason_t ret = 0;
+	int dl, bd_dl;
+
+	/* We never set ANC_SUP */
+	if (cmd->t_task_cdb[1])
+		return TCM_INVALID_CDB_FIELD;
+
+	if (cmd->data_length == 0) {
+		target_complete_cmd(cmd, SAM_STAT_GOOD);
+		return 0;
+	}
+
+	if (cmd->data_length < 8) {
+		pr_warn("UNMAP parameter list length %u too small\n",
+			cmd->data_length);
+		return TCM_PARAMETER_LIST_LENGTH_ERROR;
+	}
+
+	buf = transport_kmap_data_sg(cmd);
+	if (!buf)
+		return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
+
+	dl = get_unaligned_be16(&buf[0]);
+	bd_dl = get_unaligned_be16(&buf[2]);
+
+	size = cmd->data_length - 8;
+	if (bd_dl > size)
+		pr_warn("UNMAP parameter list length %u too small, ignoring bd_dl %u\n",
+			cmd->data_length, bd_dl);
+	else
+		size = bd_dl;
+
+	if (size / 16 > dev->dev_attrib.max_unmap_block_desc_count) {
+		ret = TCM_INVALID_PARAMETER_LIST;
+		goto err;
+	}
+
+	/* First UNMAP block descriptor starts at 8 byte offset */
+	ptr = &buf[8];
+	pr_debug("UNMAP: Sub: %s Using dl: %u bd_dl: %u size: %u"
+		" ptr: %p\n", dev->transport->name, dl, bd_dl, size, ptr);
+
+	while (size >= 16) {
+		lba = get_unaligned_be64(&ptr[0]);
+		range = get_unaligned_be32(&ptr[8]);
+		pr_debug("UNMAP: Using lba: %llu and range: %u\n",
+				 (unsigned long long)lba, range);
+
+		if (range > dev->dev_attrib.max_unmap_lba_count) {
+			ret = TCM_INVALID_PARAMETER_LIST;
+			goto err;
+		}
+
+		if (lba + range > dev->transport->get_blocks(dev) + 1) {
+			ret = TCM_ADDRESS_OUT_OF_RANGE;
+			goto err;
+		}
+
+		ret = do_unmap_fn(cmd, priv, lba, range);
+		if (ret)
+			goto err;
+
+		ptr += 16;
+		size -= 16;
+	}
+
+err:
+	transport_kunmap_data_sg(cmd);
+	if (!ret)
+		target_complete_cmd(cmd, GOOD);
+	return ret;
+}
+EXPORT_SYMBOL(sbc_execute_unmap);
diff --git a/drivers/target/target_core_transport.c b/drivers/target/target_core_transport.c
index 2030b608136..f8388b4024a 100644
--- a/drivers/target/target_core_transport.c
+++ b/drivers/target/target_core_transport.c
@@ -65,7 +65,6 @@ static void transport_complete_task_attr(struct se_cmd *cmd);
 static void transport_handle_queue_full(struct se_cmd *cmd,
 		struct se_device *dev);
 static int transport_generic_get_mem(struct se_cmd *cmd);
-static int target_get_sess_cmd(struct se_session *, struct se_cmd *, bool);
 static void transport_put_cmd(struct se_cmd *cmd);
 static void target_complete_ok_work(struct work_struct *work);
 
@@ -1139,8 +1138,10 @@ target_setup_cmd_from_cdb(struct se_cmd *cmd, unsigned char *cdb)
 		return ret;
 
 	ret = target_check_reservation(cmd);
-	if (ret)
+	if (ret) {
+		cmd->scsi_status = SAM_STAT_RESERVATION_CONFLICT;
 		return ret;
+	}
 
 	ret = dev->transport->parse_cdb(cmd);
 	if (ret)
@@ -2177,7 +2178,7 @@ EXPORT_SYMBOL(transport_generic_free_cmd);
  * @se_cmd:	command descriptor to add
  * @ack_kref:	Signal that fabric will perform an ack target_put_sess_cmd()
  */
-static int target_get_sess_cmd(struct se_session *se_sess, struct se_cmd *se_cmd,
+int target_get_sess_cmd(struct se_session *se_sess, struct se_cmd *se_cmd,
 			       bool ack_kref)
 {
 	unsigned long flags;
@@ -2206,6 +2207,7 @@ out:
 	spin_unlock_irqrestore(&se_sess->sess_cmd_lock, flags);
 	return ret;
 }
+EXPORT_SYMBOL(target_get_sess_cmd);
 
 static void target_release_cmd_kref(struct kref *kref)
 {
@@ -2763,8 +2765,13 @@ transport_send_check_condition_and_sense(struct se_cmd *cmd,
 		/* CURRENT ERROR */
 		buffer[0] = 0x70;
 		buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
-		/* ILLEGAL REQUEST */
-		buffer[SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
+		/*
+		 * Returning ILLEGAL REQUEST would cause immediate IO errors on
+		 * Solaris initiators.  Returning NOT READY instead means the
+		 * operations will be retried a finite number of times and we
+		 * can survive intermittent errors.
+		 */
+		buffer[SPC_SENSE_KEY_OFFSET] = NOT_READY;
 		/* LOGICAL UNIT COMMUNICATION FAILURE */
 		buffer[SPC_ASC_KEY_OFFSET] = 0x08;
 		break;
diff --git a/drivers/target/tcm_fc/tfc_io.c b/drivers/target/tcm_fc/tfc_io.c
index b6fd4cf4284..e415af32115 100644
--- a/drivers/target/tcm_fc/tfc_io.c
+++ b/drivers/target/tcm_fc/tfc_io.c
@@ -103,6 +103,13 @@ int ft_queue_data_in(struct se_cmd *se_cmd)
 	use_sg = !(remaining % 4);
 
 	while (remaining) {
+		struct fc_seq *seq = cmd->seq;
+
+		if (!seq) {
+			pr_debug("%s: Command aborted, xid 0x%x\n",
+				 __func__, ep->xid);
+			break;
+		}
 		if (!mem_len) {
 			sg = sg_next(sg);
 			mem_len = min((size_t)sg->length, remaining);
@@ -169,7 +176,7 @@ int ft_queue_data_in(struct se_cmd *se_cmd)
 			f_ctl |= FC_FC_END_SEQ;
 		fc_fill_fc_hdr(fp, FC_RCTL_DD_SOL_DATA, ep->did, ep->sid,
 			       FC_TYPE_FCP, f_ctl, fh_off);
-		error = lport->tt.seq_send(lport, cmd->seq, fp);
+		error = lport->tt.seq_send(lport, seq, fp);
 		if (error) {
 			/* XXX For now, initiator will retry */
 			pr_err_ratelimited("%s: Failed to send frame %p, "
diff --git a/drivers/target/tcm_fc/tfc_sess.c b/drivers/target/tcm_fc/tfc_sess.c
index 113f33598b9..4859505ae2e 100644
--- a/drivers/target/tcm_fc/tfc_sess.c
+++ b/drivers/target/tcm_fc/tfc_sess.c
@@ -428,19 +428,12 @@ static int ft_prli(struct fc_rport_priv *rdata, u32 spp_len,
 	return ret;
 }
 
-static void ft_sess_rcu_free(struct rcu_head *rcu)
-{
-	struct ft_sess *sess = container_of(rcu, struct ft_sess, rcu);
-
-	kfree(sess);
-}
-
 static void ft_sess_free(struct kref *kref)
 {
 	struct ft_sess *sess = container_of(kref, struct ft_sess, kref);
 
 	transport_deregister_session(sess->se_sess);
-	call_rcu(&sess->rcu, ft_sess_rcu_free);
+	kfree_rcu(sess, rcu);
 }
 
 void ft_sess_put(struct ft_sess *sess)
diff --git a/drivers/tty/amiserial.c b/drivers/tty/amiserial.c
index fc700342d43..083710e0236 100644
--- a/drivers/tty/amiserial.c
+++ b/drivers/tty/amiserial.c
@@ -1798,19 +1798,7 @@ static struct platform_driver amiga_serial_driver = {
 	},
 };
 
-static int __init amiga_serial_init(void)
-{
-	return platform_driver_probe(&amiga_serial_driver, amiga_serial_probe);
-}
-
-module_init(amiga_serial_init);
-
-static void __exit amiga_serial_exit(void)
-{
-	platform_driver_unregister(&amiga_serial_driver);
-}
-
-module_exit(amiga_serial_exit);
+module_platform_driver_probe(amiga_serial_driver, amiga_serial_probe);
 
 
 #if defined(CONFIG_SERIAL_CONSOLE) && !defined(MODULE)
diff --git a/drivers/tty/cyclades.c b/drivers/tty/cyclades.c
index 345bd0e0884..33f83fee9fa 100644
--- a/drivers/tty/cyclades.c
+++ b/drivers/tty/cyclades.c
@@ -1124,14 +1124,8 @@ static void cyz_handle_cmd(struct cyclades_card *cinfo)
 					readl(&info->u.cyz.ch_ctrl->rs_status);
 				if (dcd & C_RS_DCD)
 					wake_up_interruptible(&info->port.open_wait);
-				else {
-					struct tty_struct *tty;
-					tty = tty_port_tty_get(&info->port);
-					if (tty) {
-						tty_hangup(tty);
-						tty_kref_put(tty);
-					}
-				}
+				else
+					tty_port_tty_hangup(&info->port, false);
 			}
 			break;
 		case C_CM_MCTS:
diff --git a/drivers/tty/ehv_bytechan.c b/drivers/tty/ehv_bytechan.c
index ed92622b894..6d0c27cd03d 100644
--- a/drivers/tty/ehv_bytechan.c
+++ b/drivers/tty/ehv_bytechan.c
@@ -472,13 +472,9 @@ static void ehv_bc_tx_dequeue(struct ehv_bc_data *bc)
 static irqreturn_t ehv_bc_tty_tx_isr(int irq, void *data)
 {
 	struct ehv_bc_data *bc = data;
-	struct tty_struct *ttys = tty_port_tty_get(&bc->port);
 
 	ehv_bc_tx_dequeue(bc);
-	if (ttys) {
-		tty_wakeup(ttys);
-		tty_kref_put(ttys);
-	}
+	tty_port_tty_wakeup(&bc->port);
 
 	return IRQ_HANDLED;
 }
diff --git a/drivers/tty/hvc/hvsi.c b/drivers/tty/hvc/hvsi.c
index ef95a154854..41901997c0d 100644
--- a/drivers/tty/hvc/hvsi.c
+++ b/drivers/tty/hvc/hvsi.c
@@ -861,7 +861,6 @@ static void hvsi_write_worker(struct work_struct *work)
 {
 	struct hvsi_struct *hp =
 		container_of(work, struct hvsi_struct, writer.work);
-	struct tty_struct *tty;
 	unsigned long flags;
 #ifdef DEBUG
 	static long start_j = 0;
@@ -895,11 +894,7 @@ static void hvsi_write_worker(struct work_struct *work)
 		start_j = 0;
 #endif /* DEBUG */
 		wake_up_all(&hp->emptyq);
-		tty = tty_port_tty_get(&hp->port);
-		if (tty) {
-			tty_wakeup(tty);
-			tty_kref_put(tty);
-		}
+		tty_port_tty_wakeup(&hp->port);
 	}
 
 out:
diff --git a/drivers/tty/ipwireless/hardware.c b/drivers/tty/ipwireless/hardware.c
index 97a511f4185..2c14842541d 100644
--- a/drivers/tty/ipwireless/hardware.c
+++ b/drivers/tty/ipwireless/hardware.c
@@ -1732,8 +1732,7 @@ void ipwireless_hardware_free(struct ipw_hardware *hw)
 	flush_work(&hw->work_rx);
 
 	for (i = 0; i < NL_NUM_OF_ADDRESSES; i++)
-		if (hw->packet_assembler[i] != NULL)
-			kfree(hw->packet_assembler[i]);
+		kfree(hw->packet_assembler[i]);
 
 	for (i = 0; i < NL_NUM_OF_PRIORITIES; i++)
 		list_for_each_entry_safe(tp, tq, &hw->tx_queue[i], queue) {
diff --git a/drivers/tty/moxa.c b/drivers/tty/moxa.c
index adeac255e52..1deaca4674e 100644
--- a/drivers/tty/moxa.c
+++ b/drivers/tty/moxa.c
@@ -913,16 +913,12 @@ static void moxa_board_deinit(struct moxa_board_conf *brd)
 
 	/* pci hot-un-plug support */
 	for (a = 0; a < brd->numPorts; a++)
-		if (brd->ports[a].port.flags & ASYNC_INITIALIZED) {
-			struct tty_struct *tty = tty_port_tty_get(
-						&brd->ports[a].port);
-			if (tty) {
-				tty_hangup(tty);
-				tty_kref_put(tty);
-			}
-		}
+		if (brd->ports[a].port.flags & ASYNC_INITIALIZED)
+			tty_port_tty_hangup(&brd->ports[a].port, false);
+
 	for (a = 0; a < MAX_PORTS_PER_BOARD; a++)
 		tty_port_destroy(&brd->ports[a].port);
+
 	while (1) {
 		opened = 0;
 		for (a = 0; a < brd->numPorts; a++)
@@ -1365,7 +1361,6 @@ static void moxa_hangup(struct tty_struct *tty)
 
 static void moxa_new_dcdstate(struct moxa_port *p, u8 dcd)
 {
-	struct tty_struct *tty;
 	unsigned long flags;
 	dcd = !!dcd;
 
@@ -1373,10 +1368,8 @@ static void moxa_new_dcdstate(struct moxa_port *p, u8 dcd)
 	if (dcd != p->DCDState) {
         	p->DCDState = dcd;
         	spin_unlock_irqrestore(&p->port.lock, flags);
-		tty = tty_port_tty_get(&p->port);
-		if (tty && !C_CLOCAL(tty) && !dcd)
-			tty_hangup(tty);
-		tty_kref_put(tty);
+		if (!dcd)
+			tty_port_tty_hangup(&p->port, true);
 	}
 	else
 		spin_unlock_irqrestore(&p->port.lock, flags);
diff --git a/drivers/tty/mxser.c b/drivers/tty/mxser.c
index 484b6a3c9b0..71d6eb2c93b 100644
--- a/drivers/tty/mxser.c
+++ b/drivers/tty/mxser.c
@@ -1084,6 +1084,10 @@ static void mxser_close(struct tty_struct *tty, struct file *filp)
 	mutex_lock(&port->mutex);
 	mxser_close_port(port);
 	mxser_flush_buffer(tty);
+	if (test_bit(ASYNCB_INITIALIZED, &port->flags)) {
+		if (C_HUPCL(tty))
+			tty_port_lower_dtr_rts(port);
+	}
 	mxser_shutdown_port(port);
 	clear_bit(ASYNCB_INITIALIZED, &port->flags);
 	mutex_unlock(&port->mutex);
@@ -2643,9 +2647,9 @@ static int mxser_probe(struct pci_dev *pdev,
 				mxvar_sdriver, brd->idx + i, &pdev->dev);
 		if (IS_ERR(tty_dev)) {
 			retval = PTR_ERR(tty_dev);
-			for (i--; i >= 0; i--)
+			for (; i > 0; i--)
 				tty_unregister_device(mxvar_sdriver,
-					brd->idx + i);
+					brd->idx + i - 1);
 			goto err_relbrd;
 		}
 	}
@@ -2751,9 +2755,9 @@ static int __init mxser_module_init(void)
 			tty_dev = tty_port_register_device(&brd->ports[i].port,
 					mxvar_sdriver, brd->idx + i, NULL);
 			if (IS_ERR(tty_dev)) {
-				for (i--; i >= 0; i--)
+				for (; i > 0; i--)
 					tty_unregister_device(mxvar_sdriver,
-						brd->idx + i);
+						brd->idx + i - 1);
 				for (i = 0; i < brd->info->nports; i++)
 					tty_port_destroy(&brd->ports[i].port);
 				free_irq(brd->irq, brd);
diff --git a/drivers/tty/n_gsm.c b/drivers/tty/n_gsm.c
index 4a43ef5d796..642239015b4 100644
--- a/drivers/tty/n_gsm.c
+++ b/drivers/tty/n_gsm.c
@@ -1418,11 +1418,7 @@ static void gsm_dlci_close(struct gsm_dlci *dlci)
 		pr_debug("DLCI %d goes closed.\n", dlci->addr);
 	dlci->state = DLCI_CLOSED;
 	if (dlci->addr != 0) {
-		struct tty_struct  *tty = tty_port_tty_get(&dlci->port);
-		if (tty) {
-			tty_hangup(tty);
-			tty_kref_put(tty);
-		}
+		tty_port_tty_hangup(&dlci->port, false);
 		kfifo_reset(dlci->fifo);
 	} else
 		dlci->gsm->dead = 1;
@@ -2968,6 +2964,10 @@ static void gsmtty_close(struct tty_struct *tty, struct file *filp)
 	if (tty_port_close_start(&dlci->port, tty, filp) == 0)
 		goto out;
 	gsm_dlci_begin_close(dlci);
+	if (test_bit(ASYNCB_INITIALIZED, &dlci->port.flags)) {
+		if (C_HUPCL(tty))
+			tty_port_lower_dtr_rts(&dlci->port);
+	}
 	tty_port_close_end(&dlci->port, tty);
 	tty_port_tty_set(&dlci->port, NULL);
 out:
diff --git a/drivers/tty/n_tty.c b/drivers/tty/n_tty.c
index 05e72bea9b0..d655416087b 100644
--- a/drivers/tty/n_tty.c
+++ b/drivers/tty/n_tty.c
@@ -153,6 +153,12 @@ static void n_tty_set_room(struct tty_struct *tty)
 	if (left && !old_left) {
 		WARN_RATELIMIT(tty->port->itty == NULL,
 				"scheduling with invalid itty\n");
+		/* see if ldisc has been killed - if so, this means that
+		 * even though the ldisc has been halted and ->buf.work
+		 * cancelled, ->buf.work is about to be rescheduled
+		 */
+		WARN_RATELIMIT(test_bit(TTY_LDISC_HALTED, &tty->flags),
+			       "scheduling buffer work for halted ldisc\n");
 		schedule_work(&tty->port->buf.work);
 	}
 }
@@ -189,34 +195,17 @@ static void put_tty_queue(unsigned char c, struct n_tty_data *ldata)
 }
 
 /**
- *	check_unthrottle	-	allow new receive data
- *	@tty; tty device
- *
- *	Check whether to call the driver unthrottle functions
- *
- *	Can sleep, may be called under the atomic_read_lock mutex but
- *	this is not guaranteed.
- */
-static void check_unthrottle(struct tty_struct *tty)
-{
-	if (tty->count)
-		tty_unthrottle(tty);
-}
-
-/**
  *	reset_buffer_flags	-	reset buffer state
  *	@tty: terminal to reset
  *
- *	Reset the read buffer counters, clear the flags,
- *	and make sure the driver is unthrottled. Called
- *	from n_tty_open() and n_tty_flush_buffer().
+ *	Reset the read buffer counters and clear the flags.
+ *	Called from n_tty_open() and n_tty_flush_buffer().
  *
  *	Locking: tty_read_lock for read fields.
  */
 
-static void reset_buffer_flags(struct tty_struct *tty)
+static void reset_buffer_flags(struct n_tty_data *ldata)
 {
-	struct n_tty_data *ldata = tty->disc_data;
 	unsigned long flags;
 
 	raw_spin_lock_irqsave(&ldata->read_lock, flags);
@@ -229,36 +218,38 @@ static void reset_buffer_flags(struct tty_struct *tty)
 
 	ldata->canon_head = ldata->canon_data = ldata->erasing = 0;
 	bitmap_zero(ldata->read_flags, N_TTY_BUF_SIZE);
-	n_tty_set_room(tty);
+}
+
+static void n_tty_packet_mode_flush(struct tty_struct *tty)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&tty->ctrl_lock, flags);
+	if (tty->link->packet) {
+		tty->ctrl_status |= TIOCPKT_FLUSHREAD;
+		wake_up_interruptible(&tty->link->read_wait);
+	}
+	spin_unlock_irqrestore(&tty->ctrl_lock, flags);
 }
 
 /**
  *	n_tty_flush_buffer	-	clean input queue
  *	@tty:	terminal device
  *
- *	Flush the input buffer. Called when the line discipline is
- *	being closed, when the tty layer wants the buffer flushed (eg
- *	at hangup) or when the N_TTY line discipline internally has to
- *	clean the pending queue (for example some signals).
+ *	Flush the input buffer. Called when the tty layer wants the
+ *	buffer flushed (eg at hangup) or when the N_TTY line discipline
+ *	internally has to clean the pending queue (for example some signals).
  *
  *	Locking: ctrl_lock, read_lock.
  */
 
 static void n_tty_flush_buffer(struct tty_struct *tty)
 {
-	unsigned long flags;
-	/* clear everything and unthrottle the driver */
-	reset_buffer_flags(tty);
-
-	if (!tty->link)
-		return;
+	reset_buffer_flags(tty->disc_data);
+	n_tty_set_room(tty);
 
-	spin_lock_irqsave(&tty->ctrl_lock, flags);
-	if (tty->link->packet) {
-		tty->ctrl_status |= TIOCPKT_FLUSHREAD;
-		wake_up_interruptible(&tty->link->read_wait);
-	}
-	spin_unlock_irqrestore(&tty->ctrl_lock, flags);
+	if (tty->link)
+		n_tty_packet_mode_flush(tty);
 }
 
 /**
@@ -1032,23 +1023,19 @@ static void eraser(unsigned char c, struct tty_struct *tty)
  *	isig		-	handle the ISIG optio
  *	@sig: signal
  *	@tty: terminal
- *	@flush: force flush
  *
- *	Called when a signal is being sent due to terminal input. This
- *	may caus terminal flushing to take place according to the termios
- *	settings and character used. Called from the driver receive_buf
- *	path so serialized.
+ *	Called when a signal is being sent due to terminal input.
+ *	Called from the driver receive_buf path so serialized.
  *
- *	Locking: ctrl_lock, read_lock (both via flush buffer)
+ *	Locking: ctrl_lock
  */
 
-static inline void isig(int sig, struct tty_struct *tty, int flush)
+static inline void isig(int sig, struct tty_struct *tty)
 {
-	if (tty->pgrp)
-		kill_pgrp(tty->pgrp, sig, 1);
-	if (flush || !L_NOFLSH(tty)) {
-		n_tty_flush_buffer(tty);
-		tty_driver_flush_buffer(tty);
+	struct pid *tty_pgrp = tty_get_pgrp(tty);
+	if (tty_pgrp) {
+		kill_pgrp(tty_pgrp, sig, 1);
+		put_pid(tty_pgrp);
 	}
 }
 
@@ -1069,7 +1056,11 @@ static inline void n_tty_receive_break(struct tty_struct *tty)
 	if (I_IGNBRK(tty))
 		return;
 	if (I_BRKINT(tty)) {
-		isig(SIGINT, tty, 1);
+		isig(SIGINT, tty);
+		if (!L_NOFLSH(tty)) {
+			n_tty_flush_buffer(tty);
+			tty_driver_flush_buffer(tty);
+		}
 		return;
 	}
 	if (I_PARMRK(tty)) {
@@ -1236,11 +1227,6 @@ static inline void n_tty_receive_char(struct tty_struct *tty, unsigned char c)
 		signal = SIGTSTP;
 		if (c == SUSP_CHAR(tty)) {
 send_signal:
-			/*
-			 * Note that we do not use isig() here because we want
-			 * the order to be:
-			 * 1) flush, 2) echo, 3) signal
-			 */
 			if (!L_NOFLSH(tty)) {
 				n_tty_flush_buffer(tty);
 				tty_driver_flush_buffer(tty);
@@ -1251,8 +1237,7 @@ send_signal:
 				echo_char(c, tty);
 				process_echoes(tty);
 			}
-			if (tty->pgrp)
-				kill_pgrp(tty->pgrp, signal, 1);
+			isig(signal, tty);
 			return;
 		}
 	}
@@ -1483,14 +1468,14 @@ static void n_tty_receive_buf(struct tty_struct *tty, const unsigned char *cp,
 	 * mode.  We don't want to throttle the driver if we're in
 	 * canonical mode and don't have a newline yet!
 	 */
-	if (tty->receive_room < TTY_THRESHOLD_THROTTLE)
-		tty_throttle(tty);
-
-        /* FIXME: there is a tiny race here if the receive room check runs
-           before the other work executes and empties the buffer (upping
-           the receiving room and unthrottling. We then throttle and get
-           stuck. This has been observed and traced down by Vincent Pillet/
-           We need to address this when we sort out out the rx path locking */
+	while (1) {
+		tty_set_flow_change(tty, TTY_THROTTLE_SAFE);
+		if (tty->receive_room >= TTY_THRESHOLD_THROTTLE)
+			break;
+		if (!tty_throttle_safe(tty))
+			break;
+	}
+	__tty_set_flow_change(tty, 0);
 }
 
 int is_ignored(int sig)
@@ -1607,7 +1592,9 @@ static void n_tty_close(struct tty_struct *tty)
 {
 	struct n_tty_data *ldata = tty->disc_data;
 
-	n_tty_flush_buffer(tty);
+	if (tty->link)
+		n_tty_packet_mode_flush(tty);
+
 	kfree(ldata->read_buf);
 	kfree(ldata->echo_buf);
 	kfree(ldata);
@@ -1645,12 +1632,14 @@ static int n_tty_open(struct tty_struct *tty)
 		goto err_free_bufs;
 
 	tty->disc_data = ldata;
-	reset_buffer_flags(tty);
-	tty_unthrottle(tty);
+	reset_buffer_flags(tty->disc_data);
 	ldata->column = 0;
-	n_tty_set_termios(tty, NULL);
 	tty->minimum_to_wake = 1;
 	tty->closing = 0;
+	/* indicate buffer work may resume */
+	clear_bit(TTY_LDISC_HALTED, &tty->flags);
+	n_tty_set_termios(tty, NULL);
+	tty_unthrottle(tty);
 
 	return 0;
 err_free_bufs:
@@ -1740,10 +1729,9 @@ extern ssize_t redirected_tty_write(struct file *, const char __user *,
  *	and if appropriate send any needed signals and return a negative
  *	error code if action should be taken.
  *
- *	FIXME:
- *	Locking: None - redirected write test is safe, testing
- *	current->signal should possibly lock current->sighand
- *	pgrp locking ?
+ *	Locking: redirected write test is safe
+ *		 current->signal->tty check is safe
+ *		 ctrl_lock to safely reference tty->pgrp
  */
 
 static int job_control(struct tty_struct *tty, struct file *file)
@@ -1753,19 +1741,22 @@ static int job_control(struct tty_struct *tty, struct file *file)
 	/* NOTE: not yet done after every sleep pending a thorough
 	   check of the logic of this change. -- jlc */
 	/* don't stop on /dev/console */
-	if (file->f_op->write != redirected_tty_write &&
-	    current->signal->tty == tty) {
-		if (!tty->pgrp)
-			printk(KERN_ERR "n_tty_read: no tty->pgrp!\n");
-		else if (task_pgrp(current) != tty->pgrp) {
-			if (is_ignored(SIGTTIN) ||
-			    is_current_pgrp_orphaned())
-				return -EIO;
-			kill_pgrp(task_pgrp(current), SIGTTIN, 1);
-			set_thread_flag(TIF_SIGPENDING);
-			return -ERESTARTSYS;
-		}
+	if (file->f_op->write == redirected_tty_write ||
+	    current->signal->tty != tty)
+		return 0;
+
+	spin_lock_irq(&tty->ctrl_lock);
+	if (!tty->pgrp)
+		printk(KERN_ERR "n_tty_read: no tty->pgrp!\n");
+	else if (task_pgrp(current) != tty->pgrp) {
+		spin_unlock_irq(&tty->ctrl_lock);
+		if (is_ignored(SIGTTIN) || is_current_pgrp_orphaned())
+			return -EIO;
+		kill_pgrp(task_pgrp(current), SIGTTIN, 1);
+		set_thread_flag(TIF_SIGPENDING);
+		return -ERESTARTSYS;
 	}
+	spin_unlock_irq(&tty->ctrl_lock);
 	return 0;
 }
 
@@ -1959,10 +1950,17 @@ do_it_again:
 		 * longer than TTY_THRESHOLD_UNTHROTTLE in canonical mode,
 		 * we won't get any more characters.
 		 */
-		if (n_tty_chars_in_buffer(tty) <= TTY_THRESHOLD_UNTHROTTLE) {
+		while (1) {
+			tty_set_flow_change(tty, TTY_UNTHROTTLE_SAFE);
+			if (n_tty_chars_in_buffer(tty) > TTY_THRESHOLD_UNTHROTTLE)
+				break;
+			if (!tty->count)
+				break;
 			n_tty_set_room(tty);
-			check_unthrottle(tty);
+			if (!tty_unthrottle_safe(tty))
+				break;
 		}
+		__tty_set_flow_change(tty, 0);
 
 		if (b - buf >= minimum)
 			break;
diff --git a/drivers/tty/nozomi.c b/drivers/tty/nozomi.c
index 2dff1979615..d6080c3831e 100644
--- a/drivers/tty/nozomi.c
+++ b/drivers/tty/nozomi.c
@@ -791,7 +791,6 @@ static int send_data(enum port_type index, struct nozomi *dc)
 	const u8 toggle = port->toggle_ul;
 	void __iomem *addr = port->ul_addr[toggle];
 	const u32 ul_size = port->ul_size[toggle];
-	struct tty_struct *tty = tty_port_tty_get(&port->port);
 
 	/* Get data from tty and place in buf for now */
 	size = kfifo_out(&port->fifo_ul, dc->send_buf,
@@ -799,7 +798,6 @@ static int send_data(enum port_type index, struct nozomi *dc)
 
 	if (size == 0) {
 		DBG4("No more data to send, disable link:");
-		tty_kref_put(tty);
 		return 0;
 	}
 
@@ -809,10 +807,8 @@ static int send_data(enum port_type index, struct nozomi *dc)
 	write_mem32(addr, (u32 *) &size, 4);
 	write_mem32(addr + 4, (u32 *) dc->send_buf, size);
 
-	if (tty)
-		tty_wakeup(tty);
+	tty_port_tty_wakeup(&port->port);
 
-	tty_kref_put(tty);
 	return 1;
 }
 
@@ -1505,12 +1501,9 @@ static void tty_exit(struct nozomi *dc)
 
 	DBG1(" ");
 
-	for (i = 0; i < MAX_PORT; ++i) {
-		struct tty_struct *tty = tty_port_tty_get(&dc->port[i].port);
-		if (tty && list_empty(&tty->hangup_work.entry))
-			tty_hangup(tty);
-		tty_kref_put(tty);
-	}
+	for (i = 0; i < MAX_PORT; ++i)
+		tty_port_tty_hangup(&dc->port[i].port, false);
+
 	/* Racy below - surely should wait for scheduled work to be done or
 	   complete off a hangup method ? */
 	while (dc->open_ttys)
diff --git a/drivers/tty/pty.c b/drivers/tty/pty.c
index c24b4db243b..a62798fcc01 100644
--- a/drivers/tty/pty.c
+++ b/drivers/tty/pty.c
@@ -405,15 +405,8 @@ err:
 	return retval;
 }
 
-/* this is called once with whichever end is closed last */
-static void pty_unix98_shutdown(struct tty_struct *tty)
-{
-	devpts_kill_index(tty->driver_data, tty->index);
-}
-
 static void pty_cleanup(struct tty_struct *tty)
 {
-	tty->port->itty = NULL;
 	tty_port_put(tty->port);
 }
 
@@ -627,6 +620,12 @@ static void pty_unix98_remove(struct tty_driver *driver, struct tty_struct *tty)
 {
 }
 
+/* this is called once with whichever end is closed last */
+static void pty_unix98_shutdown(struct tty_struct *tty)
+{
+	devpts_kill_index(tty->driver_data, tty->index);
+}
+
 static const struct tty_operations ptm_unix98_ops = {
 	.lookup = ptm_unix98_lookup,
 	.install = pty_unix98_install,
diff --git a/drivers/tty/rocket.c b/drivers/tty/rocket.c
index 1d270034bfc..82d35c5a58f 100644
--- a/drivers/tty/rocket.c
+++ b/drivers/tty/rocket.c
@@ -449,7 +449,7 @@ static void rp_do_transmit(struct r_port *info)
 
 	/*  Loop sending data to FIFO until done or FIFO full */
 	while (1) {
-		if (tty->stopped || tty->hw_stopped)
+		if (tty->stopped)
 			break;
 		c = min(info->xmit_fifo_room, info->xmit_cnt);
 		c = min(c, XMIT_BUF_SIZE - info->xmit_tail);
@@ -521,15 +521,10 @@ static void rp_handle_port(struct r_port *info)
 		       (ChanStatus & CD_ACT) ? "on" : "off");
 #endif
 		if (!(ChanStatus & CD_ACT) && info->cd_status) {
-			struct tty_struct *tty;
 #ifdef ROCKET_DEBUG_HANGUP
 			printk(KERN_INFO "CD drop, calling hangup.\n");
 #endif
-			tty = tty_port_tty_get(&info->port);
-			if (tty) {
-				tty_hangup(tty);
-				tty_kref_put(tty);
-			}
+			tty_port_tty_hangup(&info->port, false);
 		}
 		info->cd_status = (ChanStatus & CD_ACT) ? 1 : 0;
 		wake_up_interruptible(&info->port.open_wait);
@@ -1111,15 +1106,12 @@ static void rp_set_termios(struct tty_struct *tty,
 
 	/* Handle transition away from B0 status */
 	if (!(old_termios->c_cflag & CBAUD) && (tty->termios.c_cflag & CBAUD)) {
-		if (!tty->hw_stopped || !(tty->termios.c_cflag & CRTSCTS))
-			sSetRTS(cp);
+		sSetRTS(cp);
 		sSetDTR(cp);
 	}
 
-	if ((old_termios->c_cflag & CRTSCTS) && !(tty->termios.c_cflag & CRTSCTS)) {
-		tty->hw_stopped = 0;
+	if ((old_termios->c_cflag & CRTSCTS) && !(tty->termios.c_cflag & CRTSCTS))
 		rp_start(tty);
-	}
 }
 
 static int rp_break(struct tty_struct *tty, int break_state)
@@ -1575,10 +1567,10 @@ static int rp_put_char(struct tty_struct *tty, unsigned char ch)
 	spin_lock_irqsave(&info->slock, flags);
 	cp = &info->channel;
 
-	if (!tty->stopped && !tty->hw_stopped && info->xmit_fifo_room == 0)
+	if (!tty->stopped && info->xmit_fifo_room == 0)
 		info->xmit_fifo_room = TXFIFO_SIZE - sGetTxCnt(cp);
 
-	if (tty->stopped || tty->hw_stopped || info->xmit_fifo_room == 0 || info->xmit_cnt != 0) {
+	if (tty->stopped || info->xmit_fifo_room == 0 || info->xmit_cnt != 0) {
 		info->xmit_buf[info->xmit_head++] = ch;
 		info->xmit_head &= XMIT_BUF_SIZE - 1;
 		info->xmit_cnt++;
@@ -1619,14 +1611,14 @@ static int rp_write(struct tty_struct *tty,
 #endif
 	cp = &info->channel;
 
-	if (!tty->stopped && !tty->hw_stopped && info->xmit_fifo_room < count)
+	if (!tty->stopped && info->xmit_fifo_room < count)
 		info->xmit_fifo_room = TXFIFO_SIZE - sGetTxCnt(cp);
 
         /*
 	 *  If the write queue for the port is empty, and there is FIFO space, stuff bytes 
 	 *  into FIFO.  Use the write queue for temp storage.
          */
-	if (!tty->stopped && !tty->hw_stopped && info->xmit_cnt == 0 && info->xmit_fifo_room > 0) {
+	if (!tty->stopped && info->xmit_cnt == 0 && info->xmit_fifo_room > 0) {
 		c = min(count, info->xmit_fifo_room);
 		b = buf;
 
@@ -1674,7 +1666,7 @@ static int rp_write(struct tty_struct *tty,
 		retval += c;
 	}
 
-	if ((retval > 0) && !tty->stopped && !tty->hw_stopped)
+	if ((retval > 0) && !tty->stopped)
 		set_bit((info->aiop * 8) + info->chan, (void *) &xmit_flags[info->board]);
 	
 end:
@@ -2527,6 +2519,7 @@ static int sInitController(CONTROLLER_T * CtlP, int CtlNum, ByteIO_t MudbacIO,
 		return (CtlP->NumAiop);
 }
 
+#ifdef CONFIG_PCI
 /***************************************************************************
 Function: sPCIInitController
 Purpose:  Initialization of controller global registers and controller
@@ -2647,6 +2640,26 @@ static int sPCIInitController(CONTROLLER_T * CtlP, int CtlNum,
 		return (CtlP->NumAiop);
 }
 
+/*  Resets the speaker controller on RocketModem II and III devices */
+static void rmSpeakerReset(CONTROLLER_T * CtlP, unsigned long model)
+{
+	ByteIO_t addr;
+
+	/* RocketModem II speaker control is at the 8th port location of offset 0x40 */
+	if ((model == MODEL_RP4M) || (model == MODEL_RP6M)) {
+		addr = CtlP->AiopIO[0] + 0x4F;
+		sOutB(addr, 0);
+	}
+
+	/* RocketModem III speaker control is at the 1st port location of offset 0x80 */
+	if ((model == MODEL_UPCI_RM3_8PORT)
+	    || (model == MODEL_UPCI_RM3_4PORT)) {
+		addr = CtlP->AiopIO[0] + 0x88;
+		sOutB(addr, 0);
+	}
+}
+#endif
+
 /***************************************************************************
 Function: sReadAiopID
 Purpose:  Read the AIOP idenfication number directly from an AIOP.
@@ -3136,25 +3149,6 @@ static void sPCIModemReset(CONTROLLER_T * CtlP, int chan, int on)
 	sOutB(addr + chan, 0);	/* apply or remove reset */
 }
 
-/*  Resets the speaker controller on RocketModem II and III devices */
-static void rmSpeakerReset(CONTROLLER_T * CtlP, unsigned long model)
-{
-	ByteIO_t addr;
-
-	/* RocketModem II speaker control is at the 8th port location of offset 0x40 */
-	if ((model == MODEL_RP4M) || (model == MODEL_RP6M)) {
-		addr = CtlP->AiopIO[0] + 0x4F;
-		sOutB(addr, 0);
-	}
-
-	/* RocketModem III speaker control is at the 1st port location of offset 0x80 */
-	if ((model == MODEL_UPCI_RM3_8PORT)
-	    || (model == MODEL_UPCI_RM3_4PORT)) {
-		addr = CtlP->AiopIO[0] + 0x88;
-		sOutB(addr, 0);
-	}
-}
-
 /*  Returns the line number given the controller (board), aiop and channel number */
 static unsigned char GetLineNumber(int ctrl, int aiop, int ch)
 {
diff --git a/drivers/tty/serial/68328serial.c b/drivers/tty/serial/68328serial.c
index 49399470794..ef2e08e9b59 100644
--- a/drivers/tty/serial/68328serial.c
+++ b/drivers/tty/serial/68328serial.c
@@ -630,8 +630,7 @@ static void rs_flush_chars(struct tty_struct *tty)
 	/* Enable transmitter */
 	local_irq_save(flags);
 
-	if (info->xmit_cnt <= 0 || tty->stopped || tty->hw_stopped ||
-			!info->xmit_buf) {
+	if (info->xmit_cnt <= 0 || tty->stopped || !info->xmit_buf) {
 		local_irq_restore(flags);
 		return;
 	}
@@ -697,7 +696,7 @@ static int rs_write(struct tty_struct * tty,
 		total += c;
 	}
 
-	if (info->xmit_cnt && !tty->stopped && !tty->hw_stopped) {
+	if (info->xmit_cnt && !tty->stopped) {
 		/* Enable transmitter */
 		local_irq_disable();		
 #ifndef USE_INTS
@@ -978,10 +977,8 @@ static void rs_set_termios(struct tty_struct *tty, struct ktermios *old_termios)
 	change_speed(info, tty);
 
 	if ((old_termios->c_cflag & CRTSCTS) &&
-	    !(tty->termios.c_cflag & CRTSCTS)) {
-		tty->hw_stopped = 0;
+	    !(tty->termios.c_cflag & CRTSCTS))
 		rs_start(tty);
-	}
 	
 }
 
diff --git a/drivers/tty/serial/8250/8250.h b/drivers/tty/serial/8250/8250.h
index 34eb676916f..1ebf8538b4f 100644
--- a/drivers/tty/serial/8250/8250.h
+++ b/drivers/tty/serial/8250/8250.h
@@ -117,13 +117,6 @@ static inline void serial_dl_write(struct uart_8250_port *up, int value)
  * is cleared, the machine locks up with endless interrupts.
  */
 #define ALPHA_KLUDGE_MCR  (UART_MCR_OUT2 | UART_MCR_OUT1)
-#elif defined(CONFIG_SBC8560)
-/*
- * WindRiver did something similarly broken on their SBC8560 board. The
- * UART tristates its IRQ output while OUT2 is clear, but they pulled
- * the interrupt line _up_ instead of down, so if we register the IRQ
- * while the UART is in that state, we die in an IRQ storm. */
-#define ALPHA_KLUDGE_MCR (UART_MCR_OUT2)
 #else
 #define ALPHA_KLUDGE_MCR 0
 #endif
diff --git a/drivers/tty/serial/8250/8250.c b/drivers/tty/serial/8250/8250_core.c
index cf6a5383748..46528d57be7 100644
--- a/drivers/tty/serial/8250/8250.c
+++ b/drivers/tty/serial/8250/8250_core.c
@@ -2755,7 +2755,7 @@ static void __init serial8250_isa_init_ports(void)
 	if (nr_uarts > UART_NR)
 		nr_uarts = UART_NR;
 
-	for (i = 0; i < nr_uarts; i++) {
+	for (i = 0; i < UART_NR; i++) {
 		struct uart_8250_port *up = &serial8250_ports[i];
 		struct uart_port *port = &up->port;
 
@@ -2916,7 +2916,7 @@ static int __init serial8250_console_setup(struct console *co, char *options)
 	 * if so, search for the first available port that does have
 	 * console support.
 	 */
-	if (co->index >= nr_uarts)
+	if (co->index >= UART_NR)
 		co->index = 0;
 	port = &serial8250_ports[co->index].port;
 	if (!port->iobase && !port->membase)
@@ -2957,7 +2957,7 @@ int serial8250_find_port(struct uart_port *p)
 	int line;
 	struct uart_port *port;
 
-	for (line = 0; line < nr_uarts; line++) {
+	for (line = 0; line < UART_NR; line++) {
 		port = &serial8250_ports[line].port;
 		if (uart_match_port(p, port))
 			return line;
@@ -3110,7 +3110,7 @@ static int serial8250_remove(struct platform_device *dev)
 {
 	int i;
 
-	for (i = 0; i < nr_uarts; i++) {
+	for (i = 0; i < UART_NR; i++) {
 		struct uart_8250_port *up = &serial8250_ports[i];
 
 		if (up->port.dev == &dev->dev)
@@ -3178,7 +3178,7 @@ static struct uart_8250_port *serial8250_find_match_or_unused(struct uart_port *
 	/*
 	 * First, find a port entry which matches.
 	 */
-	for (i = 0; i < nr_uarts; i++)
+	for (i = 0; i < UART_NR; i++)
 		if (uart_match_port(&serial8250_ports[i].port, port))
 			return &serial8250_ports[i];
 
@@ -3187,7 +3187,7 @@ static struct uart_8250_port *serial8250_find_match_or_unused(struct uart_port *
 	 * free entry.  We look for one which hasn't been previously
 	 * used (indicated by zero iobase).
 	 */
-	for (i = 0; i < nr_uarts; i++)
+	for (i = 0; i < UART_NR; i++)
 		if (serial8250_ports[i].port.type == PORT_UNKNOWN &&
 		    serial8250_ports[i].port.iobase == 0)
 			return &serial8250_ports[i];
@@ -3196,7 +3196,7 @@ static struct uart_8250_port *serial8250_find_match_or_unused(struct uart_port *
 	 * That also failed.  Last resort is to find any entry which
 	 * doesn't have a real port associated with it.
 	 */
-	for (i = 0; i < nr_uarts; i++)
+	for (i = 0; i < UART_NR; i++)
 		if (serial8250_ports[i].port.type == PORT_UNKNOWN)
 			return &serial8250_ports[i];
 
@@ -3247,6 +3247,10 @@ int serial8250_register_8250_port(struct uart_8250_port *up)
 		uart->tx_loadsz		= up->tx_loadsz;
 		uart->capabilities	= up->capabilities;
 
+		/* Take tx_loadsz from fifosize if it wasn't set separately */
+		if (uart->port.fifosize && !uart->tx_loadsz)
+			uart->tx_loadsz = uart->port.fifosize;
+
 		if (up->port.dev)
 			uart->port.dev = up->port.dev;
 
@@ -3418,6 +3422,7 @@ MODULE_PARM_DESC(probe_rsa, "Probe I/O ports for RSA");
 #endif
 MODULE_ALIAS_CHARDEV_MAJOR(TTY_MAJOR);
 
+#ifdef CONFIG_SERIAL_8250_DEPRECATED_OPTIONS
 #ifndef MODULE
 /* This module was renamed to 8250_core in 3.7.  Keep the old "8250" name
  * working as well for the module options so we don't break people.  We
@@ -3432,7 +3437,7 @@ MODULE_ALIAS_CHARDEV_MAJOR(TTY_MAJOR);
 static void __used s8250_options(void)
 {
 #undef MODULE_PARAM_PREFIX
-#define MODULE_PARAM_PREFIX "8250."
+#define MODULE_PARAM_PREFIX "8250_core."
 
 	module_param_cb(share_irqs, &param_ops_uint, &share_irqs, 0644);
 	module_param_cb(nr_uarts, &param_ops_uint, &nr_uarts, 0644);
@@ -3444,5 +3449,6 @@ static void __used s8250_options(void)
 #endif
 }
 #else
-MODULE_ALIAS("8250");
+MODULE_ALIAS("8250_core");
+#endif
 #endif
diff --git a/drivers/tty/serial/8250/8250_dma.c b/drivers/tty/serial/8250/8250_dma.c
index b9f7fd28112..7046769608d 100644
--- a/drivers/tty/serial/8250/8250_dma.c
+++ b/drivers/tty/serial/8250/8250_dma.c
@@ -33,10 +33,8 @@ static void __dma_tx_complete(void *param)
 	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
 		uart_write_wakeup(&p->port);
 
-	if (!uart_circ_empty(xmit) && !uart_tx_stopped(&p->port)) {
+	if (!uart_circ_empty(xmit) && !uart_tx_stopped(&p->port))
 		serial8250_tx_dma(p);
-		uart_write_wakeup(&p->port);
-	}
 }
 
 static void __dma_rx_complete(void *param)
@@ -67,12 +65,11 @@ int serial8250_tx_dma(struct uart_8250_port *p)
 	struct circ_buf			*xmit = &p->port.state->xmit;
 	struct dma_async_tx_descriptor	*desc;
 
-	if (dma->tx_running)
-		return -EBUSY;
+	if (uart_tx_stopped(&p->port) || dma->tx_running ||
+	    uart_circ_empty(xmit))
+		return 0;
 
 	dma->tx_size = CIRC_CNT_TO_END(xmit->head, xmit->tail, UART_XMIT_SIZE);
-	if (!dma->tx_size)
-		return -EINVAL;
 
 	desc = dmaengine_prep_slave_single(dma->txchan,
 					   dma->tx_addr + xmit->tail,
@@ -104,20 +101,29 @@ int serial8250_rx_dma(struct uart_8250_port *p, unsigned int iir)
 	struct dma_tx_state		state;
 	int				dma_status;
 
-	/*
-	 * If RCVR FIFO trigger level was not reached, complete the transfer and
-	 * let 8250.c copy the remaining data.
-	 */
-	if ((iir & 0x3f) == UART_IIR_RX_TIMEOUT) {
-		dma_status = dmaengine_tx_status(dma->rxchan, dma->rx_cookie,
-						&state);
+	dma_status = dmaengine_tx_status(dma->rxchan, dma->rx_cookie, &state);
+
+	switch (iir & 0x3f) {
+	case UART_IIR_RLSI:
+		/* 8250_core handles errors and break interrupts */
+		return -EIO;
+	case UART_IIR_RX_TIMEOUT:
+		/*
+		 * If RCVR FIFO trigger level was not reached, complete the
+		 * transfer and let 8250_core copy the remaining data.
+		 */
 		if (dma_status == DMA_IN_PROGRESS) {
 			dmaengine_pause(dma->rxchan);
 			__dma_rx_complete(p);
 		}
 		return -ETIMEDOUT;
+	default:
+		break;
 	}
 
+	if (dma_status)
+		return 0;
+
 	desc = dmaengine_prep_slave_single(dma->rxchan, dma->rx_addr,
 					   dma->rx_size, DMA_DEV_TO_MEM,
 					   DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
@@ -143,21 +149,31 @@ int serial8250_request_dma(struct uart_8250_port *p)
 	struct uart_8250_dma	*dma = p->dma;
 	dma_cap_mask_t		mask;
 
-	dma->rxconf.src_addr = p->port.mapbase + UART_RX;
-	dma->txconf.dst_addr = p->port.mapbase + UART_TX;
+	/* Default slave configuration parameters */
+	dma->rxconf.direction		= DMA_DEV_TO_MEM;
+	dma->rxconf.src_addr_width	= DMA_SLAVE_BUSWIDTH_1_BYTE;
+	dma->rxconf.src_addr		= p->port.mapbase + UART_RX;
+
+	dma->txconf.direction		= DMA_MEM_TO_DEV;
+	dma->txconf.dst_addr_width	= DMA_SLAVE_BUSWIDTH_1_BYTE;
+	dma->txconf.dst_addr		= p->port.mapbase + UART_TX;
 
 	dma_cap_zero(mask);
 	dma_cap_set(DMA_SLAVE, mask);
 
 	/* Get a channel for RX */
-	dma->rxchan = dma_request_channel(mask, dma->fn, dma->rx_param);
+	dma->rxchan = dma_request_slave_channel_compat(mask,
+						       dma->fn, dma->rx_param,
+						       p->port.dev, "rx");
 	if (!dma->rxchan)
 		return -ENODEV;
 
 	dmaengine_slave_config(dma->rxchan, &dma->rxconf);
 
 	/* Get a channel for TX */
-	dma->txchan = dma_request_channel(mask, dma->fn, dma->tx_param);
+	dma->txchan = dma_request_slave_channel_compat(mask,
+						       dma->fn, dma->tx_param,
+						       p->port.dev, "tx");
 	if (!dma->txchan) {
 		dma_release_channel(dma->rxchan);
 		return -ENODEV;
diff --git a/drivers/tty/serial/8250/8250_dw.c b/drivers/tty/serial/8250/8250_dw.c
index db0e66f6dd0..beaa283f5cc 100644
--- a/drivers/tty/serial/8250/8250_dw.c
+++ b/drivers/tty/serial/8250/8250_dw.c
@@ -26,6 +26,8 @@
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 #include <linux/acpi.h>
+#include <linux/clk.h>
+#include <linux/pm_runtime.h>
 
 #include "8250.h"
 
@@ -34,9 +36,6 @@
 #define DW_UART_CPR	0xf4 /* Component Parameter Register */
 #define DW_UART_UCV	0xf8 /* UART Component Version */
 
-/* Intel Low Power Subsystem specific */
-#define LPSS_PRV_CLOCK_PARAMS 0x800
-
 /* Component Parameter Register bits */
 #define DW_UART_CPR_ABP_DATA_WIDTH	(3 << 0)
 #define DW_UART_CPR_AFCE_MODE		(1 << 4)
@@ -55,8 +54,9 @@
 
 
 struct dw8250_data {
-	int	last_lcr;
-	int	line;
+	int		last_lcr;
+	int		line;
+	struct clk	*clk;
 };
 
 static void dw8250_serial_out(struct uart_port *p, int offset, int value)
@@ -113,6 +113,18 @@ static int dw8250_handle_irq(struct uart_port *p)
 	return 0;
 }
 
+static void
+dw8250_do_pm(struct uart_port *port, unsigned int state, unsigned int old)
+{
+	if (!state)
+		pm_runtime_get_sync(port->dev);
+
+	serial8250_do_pm(port, state, old);
+
+	if (state)
+		pm_runtime_put_sync_suspend(port->dev);
+}
+
 static int dw8250_probe_of(struct uart_port *p)
 {
 	struct device_node	*np = p->dev->of_node;
@@ -136,8 +148,13 @@ static int dw8250_probe_of(struct uart_port *p)
 	if (!of_property_read_u32(np, "reg-shift", &val))
 		p->regshift = val;
 
+	/* clock got configured through clk api, all done */
+	if (p->uartclk)
+		return 0;
+
+	/* try to find out clock frequency from DT as fallback */
 	if (of_property_read_u32(np, "clock-frequency", &val)) {
-		dev_err(p->dev, "no clock-frequency property set\n");
+		dev_err(p->dev, "clk or clock-frequency not defined\n");
 		return -EINVAL;
 	}
 	p->uartclk = val;
@@ -146,67 +163,10 @@ static int dw8250_probe_of(struct uart_port *p)
 }
 
 #ifdef CONFIG_ACPI
-static bool dw8250_acpi_dma_filter(struct dma_chan *chan, void *parm)
-{
-	return chan->chan_id == *(int *)parm;
-}
-
-static acpi_status
-dw8250_acpi_walk_resource(struct acpi_resource *res, void *data)
-{
-	struct uart_port		*p = data;
-	struct uart_8250_port		*port;
-	struct uart_8250_dma		*dma;
-	struct acpi_resource_fixed_dma	*fixed_dma;
-	struct dma_slave_config		*slave;
-
-	port = container_of(p, struct uart_8250_port, port);
-
-	switch (res->type) {
-	case ACPI_RESOURCE_TYPE_FIXED_DMA:
-		fixed_dma = &res->data.fixed_dma;
-
-		/* TX comes first */
-		if (!port->dma) {
-			dma = devm_kzalloc(p->dev, sizeof(*dma), GFP_KERNEL);
-			if (!dma)
-				return AE_NO_MEMORY;
-
-			port->dma = dma;
-			slave = &dma->txconf;
-
-			slave->direction	= DMA_MEM_TO_DEV;
-			slave->dst_addr_width	= DMA_SLAVE_BUSWIDTH_1_BYTE;
-			slave->slave_id		= fixed_dma->request_lines;
-			slave->dst_maxburst	= port->tx_loadsz / 4;
-
-			dma->tx_chan_id		= fixed_dma->channels;
-			dma->tx_param		= &dma->tx_chan_id;
-			dma->fn			= dw8250_acpi_dma_filter;
-		} else {
-			dma = port->dma;
-			slave = &dma->rxconf;
-
-			slave->direction	= DMA_DEV_TO_MEM;
-			slave->src_addr_width	= DMA_SLAVE_BUSWIDTH_1_BYTE;
-			slave->slave_id		= fixed_dma->request_lines;
-			slave->src_maxburst	= p->fifosize / 4;
-
-			dma->rx_chan_id		= fixed_dma->channels;
-			dma->rx_param		= &dma->rx_chan_id;
-		}
-
-		break;
-	}
-
-	return AE_OK;
-}
-
-static int dw8250_probe_acpi(struct uart_port *p)
+static int dw8250_probe_acpi(struct uart_8250_port *up)
 {
 	const struct acpi_device_id *id;
-	acpi_status status;
-	u32 reg;
+	struct uart_port *p = &up->port;
 
 	id = acpi_match_device(p->dev->driver->acpi_match_table, p->dev);
 	if (!id)
@@ -216,26 +176,21 @@ static int dw8250_probe_acpi(struct uart_port *p)
 	p->serial_in = dw8250_serial_in32;
 	p->serial_out = dw8250_serial_out32;
 	p->regshift = 2;
-	p->uartclk = (unsigned int)id->driver_data;
 
-	status = acpi_walk_resources(ACPI_HANDLE(p->dev), METHOD_NAME__CRS,
-				     dw8250_acpi_walk_resource, p);
-	if (ACPI_FAILURE(status)) {
-		dev_err_ratelimited(p->dev, "%s failed \"%s\"\n", __func__,
-				    acpi_format_exception(status));
-		return -ENODEV;
-	}
+	if (!p->uartclk)
+		p->uartclk = (unsigned int)id->driver_data;
 
-	/* Fix Haswell issue where the clocks do not get enabled */
-	if (!strcmp(id->id, "INT33C4") || !strcmp(id->id, "INT33C5")) {
-		reg = readl(p->membase + LPSS_PRV_CLOCK_PARAMS);
-		writel(reg | 1, p->membase + LPSS_PRV_CLOCK_PARAMS);
-	}
+	up->dma = devm_kzalloc(p->dev, sizeof(*up->dma), GFP_KERNEL);
+	if (!up->dma)
+		return -ENOMEM;
+
+	up->dma->rxconf.src_maxburst = p->fifosize / 4;
+	up->dma->txconf.dst_maxburst = p->fifosize / 4;
 
 	return 0;
 }
 #else
-static inline int dw8250_probe_acpi(struct uart_port *p)
+static inline int dw8250_probe_acpi(struct uart_8250_port *up)
 {
 	return -ENODEV;
 }
@@ -266,7 +221,11 @@ static void dw8250_setup_port(struct uart_8250_port *up)
 		p->flags |= UPF_FIXED_TYPE;
 		p->fifosize = DW_UART_CPR_FIFO_SIZE(reg);
 		up->tx_loadsz = p->fifosize;
+		up->capabilities = UART_CAP_FIFO;
 	}
+
+	if (reg & DW_UART_CPR_AFCE_MODE)
+		up->capabilities |= UART_CAP_AFE;
 }
 
 static int dw8250_probe(struct platform_device *pdev)
@@ -286,17 +245,30 @@ static int dw8250_probe(struct platform_device *pdev)
 	uart.port.mapbase = regs->start;
 	uart.port.irq = irq->start;
 	uart.port.handle_irq = dw8250_handle_irq;
+	uart.port.pm = dw8250_do_pm;
 	uart.port.type = PORT_8250;
 	uart.port.flags = UPF_SHARE_IRQ | UPF_BOOT_AUTOCONF | UPF_FIXED_PORT;
 	uart.port.dev = &pdev->dev;
 
-	uart.port.membase = ioremap(regs->start, resource_size(regs));
+	uart.port.membase = devm_ioremap(&pdev->dev, regs->start,
+					 resource_size(regs));
 	if (!uart.port.membase)
 		return -ENOMEM;
 
+	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	data->clk = devm_clk_get(&pdev->dev, NULL);
+	if (!IS_ERR(data->clk)) {
+		clk_prepare_enable(data->clk);
+		uart.port.uartclk = clk_get_rate(data->clk);
+	}
+
 	uart.port.iotype = UPIO_MEM;
 	uart.port.serial_in = dw8250_serial_in;
 	uart.port.serial_out = dw8250_serial_out;
+	uart.port.private_data = data;
 
 	dw8250_setup_port(&uart);
 
@@ -305,25 +277,22 @@ static int dw8250_probe(struct platform_device *pdev)
 		if (err)
 			return err;
 	} else if (ACPI_HANDLE(&pdev->dev)) {
-		err = dw8250_probe_acpi(&uart.port);
+		err = dw8250_probe_acpi(&uart);
 		if (err)
 			return err;
 	} else {
 		return -ENODEV;
 	}
 
-	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
-	if (!data)
-		return -ENOMEM;
-
-	uart.port.private_data = data;
-
 	data->line = serial8250_register_8250_port(&uart);
 	if (data->line < 0)
 		return data->line;
 
 	platform_set_drvdata(pdev, data);
 
+	pm_runtime_set_active(&pdev->dev);
+	pm_runtime_enable(&pdev->dev);
+
 	return 0;
 }
 
@@ -331,34 +300,64 @@ static int dw8250_remove(struct platform_device *pdev)
 {
 	struct dw8250_data *data = platform_get_drvdata(pdev);
 
+	pm_runtime_get_sync(&pdev->dev);
+
 	serial8250_unregister_port(data->line);
 
+	if (!IS_ERR(data->clk))
+		clk_disable_unprepare(data->clk);
+
+	pm_runtime_disable(&pdev->dev);
+	pm_runtime_put_noidle(&pdev->dev);
+
 	return 0;
 }
 
 #ifdef CONFIG_PM
-static int dw8250_suspend(struct platform_device *pdev, pm_message_t state)
+static int dw8250_suspend(struct device *dev)
 {
-	struct dw8250_data *data = platform_get_drvdata(pdev);
+	struct dw8250_data *data = dev_get_drvdata(dev);
 
 	serial8250_suspend_port(data->line);
 
 	return 0;
 }
 
-static int dw8250_resume(struct platform_device *pdev)
+static int dw8250_resume(struct device *dev)
 {
-	struct dw8250_data *data = platform_get_drvdata(pdev);
+	struct dw8250_data *data = dev_get_drvdata(dev);
 
 	serial8250_resume_port(data->line);
 
 	return 0;
 }
-#else
-#define dw8250_suspend NULL
-#define dw8250_resume NULL
 #endif /* CONFIG_PM */
 
+#ifdef CONFIG_PM_RUNTIME
+static int dw8250_runtime_suspend(struct device *dev)
+{
+	struct dw8250_data *data = dev_get_drvdata(dev);
+
+	clk_disable_unprepare(data->clk);
+
+	return 0;
+}
+
+static int dw8250_runtime_resume(struct device *dev)
+{
+	struct dw8250_data *data = dev_get_drvdata(dev);
+
+	clk_prepare_enable(data->clk);
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops dw8250_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(dw8250_suspend, dw8250_resume)
+	SET_RUNTIME_PM_OPS(dw8250_runtime_suspend, dw8250_runtime_resume, NULL)
+};
+
 static const struct of_device_id dw8250_of_match[] = {
 	{ .compatible = "snps,dw-apb-uart" },
 	{ /* Sentinel */ }
@@ -366,8 +365,8 @@ static const struct of_device_id dw8250_of_match[] = {
 MODULE_DEVICE_TABLE(of, dw8250_of_match);
 
 static const struct acpi_device_id dw8250_acpi_match[] = {
-	{ "INT33C4", 100000000 },
-	{ "INT33C5", 100000000 },
+	{ "INT33C4", 0 },
+	{ "INT33C5", 0 },
 	{ },
 };
 MODULE_DEVICE_TABLE(acpi, dw8250_acpi_match);
@@ -376,13 +375,12 @@ static struct platform_driver dw8250_platform_driver = {
 	.driver = {
 		.name		= "dw-apb-uart",
 		.owner		= THIS_MODULE,
+		.pm		= &dw8250_pm_ops,
 		.of_match_table	= dw8250_of_match,
 		.acpi_match_table = ACPI_PTR(dw8250_acpi_match),
 	},
 	.probe			= dw8250_probe,
 	.remove			= dw8250_remove,
-	.suspend		= dw8250_suspend,
-	.resume			= dw8250_resume,
 };
 
 module_platform_driver(dw8250_platform_driver);
diff --git a/drivers/tty/serial/8250/8250_pci.c b/drivers/tty/serial/8250/8250_pci.c
index aa76825229d..26e3a97ab15 100644
--- a/drivers/tty/serial/8250/8250_pci.c
+++ b/drivers/tty/serial/8250/8250_pci.c
@@ -1554,6 +1554,7 @@ pci_wch_ch353_setup(struct serial_private *priv,
 #define PCI_DEVICE_ID_PLX_CRONYX_OMEGA	0xc001
 #define PCI_DEVICE_ID_INTEL_PATSBURG_KT 0x1d3d
 #define PCI_VENDOR_ID_WCH		0x4348
+#define PCI_DEVICE_ID_WCH_CH352_2S	0x3253
 #define PCI_DEVICE_ID_WCH_CH353_4S	0x3453
 #define PCI_DEVICE_ID_WCH_CH353_2S1PF	0x5046
 #define PCI_DEVICE_ID_WCH_CH353_2S1P	0x7053
@@ -2172,6 +2173,14 @@ static struct pci_serial_quirk pci_serial_quirks[] __refdata = {
 		.subdevice      = PCI_ANY_ID,
 		.setup          = pci_wch_ch353_setup,
 	},
+	/* WCH CH352 2S card (16550 clone) */
+	{
+		.vendor		= PCI_VENDOR_ID_WCH,
+		.device		= PCI_DEVICE_ID_WCH_CH352_2S,
+		.subvendor	= PCI_ANY_ID,
+		.subdevice	= PCI_ANY_ID,
+		.setup		= pci_wch_ch353_setup,
+	},
 	/*
 	 * ASIX devices with FIFO bug
 	 */
@@ -4870,6 +4879,10 @@ static struct pci_device_id serial_pci_tbl[] = {
 		PCI_ANY_ID, PCI_ANY_ID,
 		0, 0, pbn_b0_bt_2_115200 },
 
+	{	PCI_VENDOR_ID_WCH, PCI_DEVICE_ID_WCH_CH352_2S,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0, pbn_b0_bt_2_115200 },
+
 	/*
 	 * Commtech, Inc. Fastcom adapters
 	 */
diff --git a/drivers/tty/serial/8250/8250_pnp.c b/drivers/tty/serial/8250/8250_pnp.c
index b3455a970a1..35d9ab95c5c 100644
--- a/drivers/tty/serial/8250/8250_pnp.c
+++ b/drivers/tty/serial/8250/8250_pnp.c
@@ -429,7 +429,6 @@ serial_pnp_probe(struct pnp_dev *dev, const struct pnp_device_id *dev_id)
 {
 	struct uart_8250_port uart;
 	int ret, line, flags = dev_id->driver_data;
-	struct resource *res = NULL;
 
 	if (flags & UNKNOWN_DEV) {
 		ret = serial_pnp_guess_board(dev);
@@ -440,12 +439,11 @@ serial_pnp_probe(struct pnp_dev *dev, const struct pnp_device_id *dev_id)
 	memset(&uart, 0, sizeof(uart));
 	if (pnp_irq_valid(dev, 0))
 		uart.port.irq = pnp_irq(dev, 0);
-	if ((flags & CIR_PORT) && pnp_port_valid(dev, 2))
-		res = pnp_get_resource(dev, IORESOURCE_IO, 2);
-	else if (pnp_port_valid(dev, 0))
-		res = pnp_get_resource(dev, IORESOURCE_IO, 0);
-	if (pnp_resource_enabled(res)) {
-		uart.port.iobase = res->start;
+	if ((flags & CIR_PORT) && pnp_port_valid(dev, 2)) {
+		uart.port.iobase = pnp_port_start(dev, 2);
+		uart.port.iotype = UPIO_PORT;
+	} else if (pnp_port_valid(dev, 0)) {
+		uart.port.iobase = pnp_port_start(dev, 0);
 		uart.port.iotype = UPIO_PORT;
 	} else if (pnp_mem_valid(dev, 0)) {
 		uart.port.mapbase = pnp_mem_start(dev, 0);
diff --git a/drivers/tty/serial/8250/Kconfig b/drivers/tty/serial/8250/Kconfig
index 2ef9537bcb2..80fe91e64a5 100644
--- a/drivers/tty/serial/8250/Kconfig
+++ b/drivers/tty/serial/8250/Kconfig
@@ -33,6 +33,23 @@ config SERIAL_8250
 	  Most people will say Y or M here, so that they can use serial mice,
 	  modems and similar devices connecting to the standard serial ports.
 
+config SERIAL_8250_DEPRECATED_OPTIONS
+	bool "Support 8250_core.* kernel options (DEPRECATED)"
+	depends on SERIAL_8250
+	default y
+	---help---
+	  In 3.7 we renamed 8250 to 8250_core by mistake, so now we have to
+	  accept kernel parameters in both forms like 8250_core.nr_uarts=4 and
+	  8250.nr_uarts=4. We now renamed the module back to 8250, but if
+	  anybody noticed in 3.7 and changed their userspace we still have to
+	  keep the 8350_core.* options around until they revert the changes
+	  they already did.
+
+	  If 8250 is built as a module, this adds 8250_core alias instead. 
+
+	  If you did not notice yet and/or you have userspace from pre-3.7, it
+	  is safe (and recommended) to say N here.
+
 config SERIAL_8250_PNP
 	bool "8250/16550 PNP device support" if EXPERT
 	depends on SERIAL_8250 && PNP
diff --git a/drivers/tty/serial/8250/Makefile b/drivers/tty/serial/8250/Makefile
index a23838a4d53..36d68d05430 100644
--- a/drivers/tty/serial/8250/Makefile
+++ b/drivers/tty/serial/8250/Makefile
@@ -2,10 +2,10 @@
 # Makefile for the 8250 serial device drivers.
 #
 
-obj-$(CONFIG_SERIAL_8250)		+= 8250_core.o
-8250_core-y				:= 8250.o
-8250_core-$(CONFIG_SERIAL_8250_PNP)	+= 8250_pnp.o
-8250_core-$(CONFIG_SERIAL_8250_DMA)	+= 8250_dma.o
+obj-$(CONFIG_SERIAL_8250)		+= 8250.o
+8250-y					:= 8250_core.o
+8250-$(CONFIG_SERIAL_8250_PNP)		+= 8250_pnp.o
+8250-$(CONFIG_SERIAL_8250_DMA)		+= 8250_dma.o
 obj-$(CONFIG_SERIAL_8250_GSC)		+= 8250_gsc.o
 obj-$(CONFIG_SERIAL_8250_PCI)		+= 8250_pci.o
 obj-$(CONFIG_SERIAL_8250_HP300)		+= 8250_hp300.o
diff --git a/drivers/tty/serial/8250/serial_cs.c b/drivers/tty/serial/8250/serial_cs.c
index b7d48b34639..1b74b88e1e1 100644
--- a/drivers/tty/serial/8250/serial_cs.c
+++ b/drivers/tty/serial/8250/serial_cs.c
@@ -852,18 +852,6 @@ static struct pcmcia_driver serial_cs_driver = {
 	.suspend	= serial_suspend,
 	.resume		= serial_resume,
 };
-
-static int __init init_serial_cs(void)
-{
-	return pcmcia_register_driver(&serial_cs_driver);
-}
-
-static void __exit exit_serial_cs(void)
-{
-	pcmcia_unregister_driver(&serial_cs_driver);
-}
-
-module_init(init_serial_cs);
-module_exit(exit_serial_cs);
+module_pcmcia_driver(serial_cs_driver);
 
 MODULE_LICENSE("GPL");
diff --git a/drivers/tty/serial/amba-pl011.c b/drivers/tty/serial/amba-pl011.c
index 3ea5408fcbe..b2e9e177a35 100644
--- a/drivers/tty/serial/amba-pl011.c
+++ b/drivers/tty/serial/amba-pl011.c
@@ -29,6 +29,7 @@
  * and hooked into this driver.
  */
 
+
 #if defined(CONFIG_SERIAL_AMBA_PL011_CONSOLE) && defined(CONFIG_MAGIC_SYSRQ)
 #define SUPPORT_SYSRQ
 #endif
@@ -72,32 +73,44 @@
 /* There is by now at least one vendor with differing details, so handle it */
 struct vendor_data {
 	unsigned int		ifls;
-	unsigned int		fifosize;
 	unsigned int		lcrh_tx;
 	unsigned int		lcrh_rx;
 	bool			oversampling;
 	bool			dma_threshold;
 	bool			cts_event_workaround;
+
+	unsigned int (*get_fifosize)(unsigned int periphid);
 };
 
+static unsigned int get_fifosize_arm(unsigned int periphid)
+{
+	unsigned int rev = (periphid >> 20) & 0xf;
+	return rev < 3 ? 16 : 32;
+}
+
 static struct vendor_data vendor_arm = {
 	.ifls			= UART011_IFLS_RX4_8|UART011_IFLS_TX4_8,
-	.fifosize		= 16,
 	.lcrh_tx		= UART011_LCRH,
 	.lcrh_rx		= UART011_LCRH,
 	.oversampling		= false,
 	.dma_threshold		= false,
 	.cts_event_workaround	= false,
+	.get_fifosize		= get_fifosize_arm,
 };
 
+static unsigned int get_fifosize_st(unsigned int periphid)
+{
+	return 64;
+}
+
 static struct vendor_data vendor_st = {
 	.ifls			= UART011_IFLS_RX_HALF|UART011_IFLS_TX_HALF,
-	.fifosize		= 64,
 	.lcrh_tx		= ST_UART011_LCRH_TX,
 	.lcrh_rx		= ST_UART011_LCRH_RX,
 	.oversampling		= true,
 	.dma_threshold		= true,
 	.cts_event_workaround	= true,
+	.get_fifosize		= get_fifosize_st,
 };
 
 static struct uart_amba_port *amba_ports[UART_NR];
@@ -117,6 +130,12 @@ struct pl011_dmarx_data {
 	struct pl011_sgbuf	sgbuf_b;
 	dma_cookie_t		cookie;
 	bool			running;
+	struct timer_list	timer;
+	unsigned int last_residue;
+	unsigned long last_jiffies;
+	bool auto_poll_rate;
+	unsigned int poll_rate;
+	unsigned int poll_timeout;
 };
 
 struct pl011_dmatx_data {
@@ -223,16 +242,18 @@ static int pl011_fifo_to_tty(struct uart_amba_port *uap)
 static int pl011_sgbuf_init(struct dma_chan *chan, struct pl011_sgbuf *sg,
 	enum dma_data_direction dir)
 {
-	sg->buf = kmalloc(PL011_DMA_BUFFER_SIZE, GFP_KERNEL);
+	dma_addr_t dma_addr;
+
+	sg->buf = dma_alloc_coherent(chan->device->dev,
+		PL011_DMA_BUFFER_SIZE, &dma_addr, GFP_KERNEL);
 	if (!sg->buf)
 		return -ENOMEM;
 
-	sg_init_one(&sg->sg, sg->buf, PL011_DMA_BUFFER_SIZE);
+	sg_init_table(&sg->sg, 1);
+	sg_set_page(&sg->sg, phys_to_page(dma_addr),
+		PL011_DMA_BUFFER_SIZE, offset_in_page(dma_addr));
+	sg_dma_address(&sg->sg) = dma_addr;
 
-	if (dma_map_sg(chan->device->dev, &sg->sg, 1, dir) != 1) {
-		kfree(sg->buf);
-		return -EINVAL;
-	}
 	return 0;
 }
 
@@ -240,8 +261,9 @@ static void pl011_sgbuf_free(struct dma_chan *chan, struct pl011_sgbuf *sg,
 	enum dma_data_direction dir)
 {
 	if (sg->buf) {
-		dma_unmap_sg(chan->device->dev, &sg->sg, 1, dir);
-		kfree(sg->buf);
+		dma_free_coherent(chan->device->dev,
+			PL011_DMA_BUFFER_SIZE, sg->buf,
+			sg_dma_address(&sg->sg));
 	}
 }
 
@@ -300,6 +322,29 @@ static void pl011_dma_probe_initcall(struct uart_amba_port *uap)
 		dmaengine_slave_config(chan, &rx_conf);
 		uap->dmarx.chan = chan;
 
+		if (plat->dma_rx_poll_enable) {
+			/* Set poll rate if specified. */
+			if (plat->dma_rx_poll_rate) {
+				uap->dmarx.auto_poll_rate = false;
+				uap->dmarx.poll_rate = plat->dma_rx_poll_rate;
+			} else {
+				/*
+				 * 100 ms defaults to poll rate if not
+				 * specified. This will be adjusted with
+				 * the baud rate at set_termios.
+				 */
+				uap->dmarx.auto_poll_rate = true;
+				uap->dmarx.poll_rate =  100;
+			}
+			/* 3 secs defaults poll_timeout if not specified. */
+			if (plat->dma_rx_poll_timeout)
+				uap->dmarx.poll_timeout =
+					plat->dma_rx_poll_timeout;
+			else
+				uap->dmarx.poll_timeout = 3000;
+		} else
+			uap->dmarx.auto_poll_rate = false;
+
 		dev_info(uap->port.dev, "DMA channel RX %s\n",
 			 dma_chan_name(uap->dmarx.chan));
 	}
@@ -701,24 +746,30 @@ static void pl011_dma_rx_chars(struct uart_amba_port *uap,
 	struct tty_port *port = &uap->port.state->port;
 	struct pl011_sgbuf *sgbuf = use_buf_b ?
 		&uap->dmarx.sgbuf_b : &uap->dmarx.sgbuf_a;
-	struct device *dev = uap->dmarx.chan->device->dev;
 	int dma_count = 0;
 	u32 fifotaken = 0; /* only used for vdbg() */
 
-	/* Pick everything from the DMA first */
+	struct pl011_dmarx_data *dmarx = &uap->dmarx;
+	int dmataken = 0;
+
+	if (uap->dmarx.poll_rate) {
+		/* The data can be taken by polling */
+		dmataken = sgbuf->sg.length - dmarx->last_residue;
+		/* Recalculate the pending size */
+		if (pending >= dmataken)
+			pending -= dmataken;
+	}
+
+	/* Pick the remain data from the DMA */
 	if (pending) {
-		/* Sync in buffer */
-		dma_sync_sg_for_cpu(dev, &sgbuf->sg, 1, DMA_FROM_DEVICE);
 
 		/*
 		 * First take all chars in the DMA pipe, then look in the FIFO.
 		 * Note that tty_insert_flip_buf() tries to take as many chars
 		 * as it can.
 		 */
-		dma_count = tty_insert_flip_string(port, sgbuf->buf, pending);
-
-		/* Return buffer to device */
-		dma_sync_sg_for_device(dev, &sgbuf->sg, 1, DMA_FROM_DEVICE);
+		dma_count = tty_insert_flip_string(port, sgbuf->buf + dmataken,
+				pending);
 
 		uap->port.icount.rx += dma_count;
 		if (dma_count < pending)
@@ -726,6 +777,10 @@ static void pl011_dma_rx_chars(struct uart_amba_port *uap,
 				 "couldn't insert all characters (TTY is full?)\n");
 	}
 
+	/* Reset the last_residue for Rx DMA poll */
+	if (uap->dmarx.poll_rate)
+		dmarx->last_residue = sgbuf->sg.length;
+
 	/*
 	 * Only continue with trying to read the FIFO if all DMA chars have
 	 * been taken first.
@@ -865,6 +920,57 @@ static inline void pl011_dma_rx_stop(struct uart_amba_port *uap)
 	writew(uap->dmacr, uap->port.membase + UART011_DMACR);
 }
 
+/*
+ * Timer handler for Rx DMA polling.
+ * Every polling, It checks the residue in the dma buffer and transfer
+ * data to the tty. Also, last_residue is updated for the next polling.
+ */
+static void pl011_dma_rx_poll(unsigned long args)
+{
+	struct uart_amba_port *uap = (struct uart_amba_port *)args;
+	struct tty_port *port = &uap->port.state->port;
+	struct pl011_dmarx_data *dmarx = &uap->dmarx;
+	struct dma_chan *rxchan = uap->dmarx.chan;
+	unsigned long flags = 0;
+	unsigned int dmataken = 0;
+	unsigned int size = 0;
+	struct pl011_sgbuf *sgbuf;
+	int dma_count;
+	struct dma_tx_state state;
+
+	sgbuf = dmarx->use_buf_b ? &uap->dmarx.sgbuf_b : &uap->dmarx.sgbuf_a;
+	rxchan->device->device_tx_status(rxchan, dmarx->cookie, &state);
+	if (likely(state.residue < dmarx->last_residue)) {
+		dmataken = sgbuf->sg.length - dmarx->last_residue;
+		size = dmarx->last_residue - state.residue;
+		dma_count = tty_insert_flip_string(port, sgbuf->buf + dmataken,
+				size);
+		if (dma_count == size)
+			dmarx->last_residue =  state.residue;
+		dmarx->last_jiffies = jiffies;
+	}
+	tty_flip_buffer_push(port);
+
+	/*
+	 * If no data is received in poll_timeout, the driver will fall back
+	 * to interrupt mode. We will retrigger DMA at the first interrupt.
+	 */
+	if (jiffies_to_msecs(jiffies - dmarx->last_jiffies)
+			> uap->dmarx.poll_timeout) {
+
+		spin_lock_irqsave(&uap->port.lock, flags);
+		pl011_dma_rx_stop(uap);
+		spin_unlock_irqrestore(&uap->port.lock, flags);
+
+		uap->dmarx.running = false;
+		dmaengine_terminate_all(rxchan);
+		del_timer(&uap->dmarx.timer);
+	} else {
+		mod_timer(&uap->dmarx.timer,
+			jiffies + msecs_to_jiffies(uap->dmarx.poll_rate));
+	}
+}
+
 static void pl011_dma_startup(struct uart_amba_port *uap)
 {
 	int ret;
@@ -927,6 +1033,16 @@ skip_rx:
 		if (pl011_dma_rx_trigger_dma(uap))
 			dev_dbg(uap->port.dev, "could not trigger initial "
 				"RX DMA job, fall back to interrupt mode\n");
+		if (uap->dmarx.poll_rate) {
+			init_timer(&(uap->dmarx.timer));
+			uap->dmarx.timer.function = pl011_dma_rx_poll;
+			uap->dmarx.timer.data = (unsigned long)uap;
+			mod_timer(&uap->dmarx.timer,
+				jiffies +
+				msecs_to_jiffies(uap->dmarx.poll_rate));
+			uap->dmarx.last_residue = PL011_DMA_BUFFER_SIZE;
+			uap->dmarx.last_jiffies = jiffies;
+		}
 	}
 }
 
@@ -962,6 +1078,8 @@ static void pl011_dma_shutdown(struct uart_amba_port *uap)
 		/* Clean up the RX DMA */
 		pl011_sgbuf_free(uap->dmarx.chan, &uap->dmarx.sgbuf_a, DMA_FROM_DEVICE);
 		pl011_sgbuf_free(uap->dmarx.chan, &uap->dmarx.sgbuf_b, DMA_FROM_DEVICE);
+		if (uap->dmarx.poll_rate)
+			del_timer_sync(&uap->dmarx.timer);
 		uap->using_rx_dma = false;
 	}
 }
@@ -976,7 +1094,6 @@ static inline bool pl011_dma_rx_running(struct uart_amba_port *uap)
 	return uap->using_rx_dma && uap->dmarx.running;
 }
 
-
 #else
 /* Blank functions if the DMA engine is not available */
 static inline void pl011_dma_probe(struct uart_amba_port *uap)
@@ -1088,8 +1205,20 @@ static void pl011_rx_chars(struct uart_amba_port *uap)
 			dev_dbg(uap->port.dev, "could not trigger RX DMA job "
 				"fall back to interrupt mode again\n");
 			uap->im |= UART011_RXIM;
-		} else
+		} else {
 			uap->im &= ~UART011_RXIM;
+#ifdef CONFIG_DMA_ENGINE
+			/* Start Rx DMA poll */
+			if (uap->dmarx.poll_rate) {
+				uap->dmarx.last_jiffies = jiffies;
+				uap->dmarx.last_residue	= PL011_DMA_BUFFER_SIZE;
+				mod_timer(&uap->dmarx.timer,
+					jiffies +
+					msecs_to_jiffies(uap->dmarx.poll_rate));
+			}
+#endif
+		}
+
 		writew(uap->im, uap->port.membase + UART011_IMSC);
 	}
 	spin_lock(&uap->port.lock);
@@ -1164,7 +1293,6 @@ static irqreturn_t pl011_int(int irq, void *dev_id)
 	unsigned int dummy_read;
 
 	spin_lock_irqsave(&uap->port.lock, flags);
-
 	status = readw(uap->port.membase + UART011_MIS);
 	if (status) {
 		do {
@@ -1551,6 +1679,13 @@ pl011_set_termios(struct uart_port *port, struct ktermios *termios,
 	 */
 	baud = uart_get_baud_rate(port, termios, old, 0,
 				  port->uartclk / clkdiv);
+#ifdef CONFIG_DMA_ENGINE
+	/*
+	 * Adjust RX DMA polling rate with baud rate if not specified.
+	 */
+	if (uap->dmarx.auto_poll_rate)
+		uap->dmarx.poll_rate = DIV_ROUND_UP(10000000, baud);
+#endif
 
 	if (baud > port->uartclk/16)
 		quot = DIV_ROUND_CLOSEST(port->uartclk * 8, baud);
@@ -2010,7 +2145,7 @@ static int pl011_probe(struct amba_device *dev, const struct amba_id *id)
 	uap->lcrh_rx = vendor->lcrh_rx;
 	uap->lcrh_tx = vendor->lcrh_tx;
 	uap->old_cr = 0;
-	uap->fifosize = vendor->fifosize;
+	uap->fifosize = vendor->get_fifosize(dev->periphid);
 	uap->port.dev = &dev->dev;
 	uap->port.mapbase = dev->res.start;
 	uap->port.membase = base;
diff --git a/drivers/tty/serial/arc_uart.c b/drivers/tty/serial/arc_uart.c
index d97e194b6bc..cbf1d155b7b 100644
--- a/drivers/tty/serial/arc_uart.c
+++ b/drivers/tty/serial/arc_uart.c
@@ -162,7 +162,7 @@ static unsigned int arc_serial_tx_empty(struct uart_port *port)
 /*
  * Driver internal routine, used by both tty(serial core) as well as tx-isr
  *  -Called under spinlock in either cases
- *  -also tty->stopped / tty->hw_stopped has already been checked
+ *  -also tty->stopped has already been checked
  *     = by uart_start( ) before calling us
  *     = tx_ist checks that too before calling
  */
diff --git a/drivers/tty/serial/atmel_serial.c b/drivers/tty/serial/atmel_serial.c
index d4a7c241b75..3467462869c 100644
--- a/drivers/tty/serial/atmel_serial.c
+++ b/drivers/tty/serial/atmel_serial.c
@@ -158,7 +158,7 @@ struct atmel_uart_port {
 };
 
 static struct atmel_uart_port atmel_ports[ATMEL_MAX_UART];
-static unsigned long atmel_ports_in_use;
+static DECLARE_BITMAP(atmel_ports_in_use, ATMEL_MAX_UART);
 
 #ifdef SUPPORT_SYSRQ
 static struct console atmel_console;
@@ -1769,15 +1769,14 @@ static int atmel_serial_probe(struct platform_device *pdev)
 	if (ret < 0)
 		/* port id not found in platform data nor device-tree aliases:
 		 * auto-enumerate it */
-		ret = find_first_zero_bit(&atmel_ports_in_use,
-				sizeof(atmel_ports_in_use));
+		ret = find_first_zero_bit(atmel_ports_in_use, ATMEL_MAX_UART);
 
-	if (ret > ATMEL_MAX_UART) {
+	if (ret >= ATMEL_MAX_UART) {
 		ret = -ENODEV;
 		goto err;
 	}
 
-	if (test_and_set_bit(ret, &atmel_ports_in_use)) {
+	if (test_and_set_bit(ret, atmel_ports_in_use)) {
 		/* port already in use */
 		ret = -EBUSY;
 		goto err;
@@ -1857,7 +1856,7 @@ static int atmel_serial_remove(struct platform_device *pdev)
 
 	/* "port" is allocated statically, so we shouldn't free it */
 
-	clear_bit(port->line, &atmel_ports_in_use);
+	clear_bit(port->line, atmel_ports_in_use);
 
 	clk_put(atmel_port->clk);
 
diff --git a/drivers/tty/serial/bfin_uart.c b/drivers/tty/serial/bfin_uart.c
index 12dceda9db3..26a3be7ced7 100644
--- a/drivers/tty/serial/bfin_uart.c
+++ b/drivers/tty/serial/bfin_uart.c
@@ -1011,24 +1011,6 @@ static int bfin_serial_poll_get_char(struct uart_port *port)
 }
 #endif
 
-#if defined(CONFIG_KGDB_SERIAL_CONSOLE) || \
-	defined(CONFIG_KGDB_SERIAL_CONSOLE_MODULE)
-static void bfin_kgdboc_port_shutdown(struct uart_port *port)
-{
-	if (kgdboc_break_enabled) {
-		kgdboc_break_enabled = 0;
-		bfin_serial_shutdown(port);
-	}
-}
-
-static int bfin_kgdboc_port_startup(struct uart_port *port)
-{
-	kgdboc_port_line = port->line;
-	kgdboc_break_enabled = !bfin_serial_startup(port);
-	return 0;
-}
-#endif
-
 static struct uart_ops bfin_serial_pops = {
 	.tx_empty	= bfin_serial_tx_empty,
 	.set_mctrl	= bfin_serial_set_mctrl,
@@ -1047,11 +1029,6 @@ static struct uart_ops bfin_serial_pops = {
 	.request_port	= bfin_serial_request_port,
 	.config_port	= bfin_serial_config_port,
 	.verify_port	= bfin_serial_verify_port,
-#if defined(CONFIG_KGDB_SERIAL_CONSOLE) || \
-	defined(CONFIG_KGDB_SERIAL_CONSOLE_MODULE)
-	.kgdboc_port_startup	= bfin_kgdboc_port_startup,
-	.kgdboc_port_shutdown	= bfin_kgdboc_port_shutdown,
-#endif
 #ifdef CONFIG_CONSOLE_POLL
 	.poll_put_char	= bfin_serial_poll_put_char,
 	.poll_get_char	= bfin_serial_poll_get_char,
diff --git a/drivers/tty/serial/crisv10.c b/drivers/tty/serial/crisv10.c
index 5f37c31e32b..477f22f773f 100644
--- a/drivers/tty/serial/crisv10.c
+++ b/drivers/tty/serial/crisv10.c
@@ -169,7 +169,6 @@ static int get_lsr_info(struct e100_serial *info, unsigned int *value);
 
 
 #define DEF_BAUD 115200   /* 115.2 kbit/s */
-#define STD_FLAGS (ASYNC_BOOT_AUTOCONF | ASYNC_SKIP_TEST)
 #define DEF_RX 0x20  /* or SERIAL_CTRL_W >> 8 */
 /* Default value of tx_ctrl register: has txd(bit 7)=1 (idle) as default */
 #define DEF_TX 0x80  /* or SERIAL_CTRL_B */
@@ -246,7 +245,6 @@ static struct e100_serial rs_table[] = {
 	  .ifirstadr   = R_DMA_CH7_FIRST,
 	  .icmdadr     = R_DMA_CH7_CMD,
 	  .idescradr   = R_DMA_CH7_DESCR,
-	  .flags       = STD_FLAGS,
 	  .rx_ctrl     = DEF_RX,
 	  .tx_ctrl     = DEF_TX,
 	  .iseteop     = 2,
@@ -300,7 +298,6 @@ static struct e100_serial rs_table[] = {
 	  .ifirstadr   = R_DMA_CH9_FIRST,
 	  .icmdadr     = R_DMA_CH9_CMD,
 	  .idescradr   = R_DMA_CH9_DESCR,
-	  .flags       = STD_FLAGS,
 	  .rx_ctrl     = DEF_RX,
 	  .tx_ctrl     = DEF_TX,
 	  .iseteop     = 3,
@@ -356,7 +353,6 @@ static struct e100_serial rs_table[] = {
 	  .ifirstadr   = R_DMA_CH3_FIRST,
 	  .icmdadr     = R_DMA_CH3_CMD,
 	  .idescradr   = R_DMA_CH3_DESCR,
-	  .flags       = STD_FLAGS,
 	  .rx_ctrl     = DEF_RX,
 	  .tx_ctrl     = DEF_TX,
 	  .iseteop     = 0,
@@ -410,7 +406,6 @@ static struct e100_serial rs_table[] = {
 	  .ifirstadr   = R_DMA_CH5_FIRST,
 	  .icmdadr     = R_DMA_CH5_CMD,
 	  .idescradr   = R_DMA_CH5_DESCR,
-	  .flags       = STD_FLAGS,
 	  .rx_ctrl     = DEF_RX,
 	  .tx_ctrl     = DEF_TX,
 	  .iseteop     = 1,
@@ -2263,8 +2258,7 @@ TODO: The break will be delayed until an F or V character is received.
 
 */
 
-static
-struct e100_serial * handle_ser_rx_interrupt_no_dma(struct e100_serial *info)
+static void handle_ser_rx_interrupt_no_dma(struct e100_serial *info)
 {
 	unsigned long data_read;
 
@@ -2370,10 +2364,9 @@ more_data:
 	}
 
 	tty_flip_buffer_push(&info->port);
-	return info;
 }
 
-static struct e100_serial* handle_ser_rx_interrupt(struct e100_serial *info)
+static void handle_ser_rx_interrupt(struct e100_serial *info)
 {
 	unsigned char rstat;
 
@@ -2382,7 +2375,8 @@ static struct e100_serial* handle_ser_rx_interrupt(struct e100_serial *info)
 #endif
 /*	DEBUG_LOG(info->line, "ser_interrupt stat %03X\n", rstat | (i << 8)); */
 	if (!info->uses_dma_in) {
-		return handle_ser_rx_interrupt_no_dma(info);
+		handle_ser_rx_interrupt_no_dma(info);
+		return;
 	}
 	/* DMA is used */
 	rstat = info->ioport[REG_STATUS];
@@ -2489,7 +2483,6 @@ static struct e100_serial* handle_ser_rx_interrupt(struct e100_serial *info)
 	/* Restarting the DMA never hurts */
 	*info->icmdadr = IO_STATE(R_DMA_CH6_CMD, cmd, restart);
 	START_FLUSH_FAST_TIMER(info, "ser_int");
-	return info;
 } /* handle_ser_rx_interrupt */
 
 static void handle_ser_tx_interrupt(struct e100_serial *info)
@@ -2534,8 +2527,7 @@ static void handle_ser_tx_interrupt(struct e100_serial *info)
 	}
 	/* Normal char-by-char interrupt */
 	if (info->xmit.head == info->xmit.tail
-	    || info->port.tty->stopped
-	    || info->port.tty->hw_stopped) {
+	    || info->port.tty->stopped) {
 		DFLOW(DEBUG_LOG(info->line, "tx_int: stopped %i\n",
 				info->port.tty->stopped));
 		e100_disable_serial_tx_ready_irq(info);
@@ -2722,7 +2714,7 @@ startup(struct e100_serial * info)
 
 	/* if it was already initialized, skip this */
 
-	if (info->flags & ASYNC_INITIALIZED) {
+	if (info->port.flags & ASYNC_INITIALIZED) {
 		local_irq_restore(flags);
 		free_page(xmit_page);
 		return 0;
@@ -2847,7 +2839,7 @@ startup(struct e100_serial * info)
 
 #endif /* CONFIG_SVINTO_SIM */
 
-	info->flags |= ASYNC_INITIALIZED;
+	info->port.flags |= ASYNC_INITIALIZED;
 
 	local_irq_restore(flags);
 	return 0;
@@ -2892,7 +2884,7 @@ shutdown(struct e100_serial * info)
 
 #endif /* CONFIG_SVINTO_SIM */
 
-	if (!(info->flags & ASYNC_INITIALIZED))
+	if (!(info->port.flags & ASYNC_INITIALIZED))
 		return;
 
 #ifdef SERIAL_DEBUG_OPEN
@@ -2923,7 +2915,7 @@ shutdown(struct e100_serial * info)
 	if (info->port.tty)
 		set_bit(TTY_IO_ERROR, &info->port.tty->flags);
 
-	info->flags &= ~ASYNC_INITIALIZED;
+	info->port.flags &= ~ASYNC_INITIALIZED;
 	local_irq_restore(flags);
 }
 
@@ -2948,7 +2940,7 @@ change_speed(struct e100_serial *info)
 	/* possibly, the tx/rx should be disabled first to do this safely */
 
 	/* change baud-rate and write it to the hardware */
-	if ((info->flags & ASYNC_SPD_MASK) == ASYNC_SPD_CUST) {
+	if ((info->port.flags & ASYNC_SPD_MASK) == ASYNC_SPD_CUST) {
 		/* Special baudrate */
 		u32 mask = 0xFF << (info->line*8); /* Each port has 8 bits */
 		unsigned long alt_source =
@@ -3098,7 +3090,6 @@ rs_flush_chars(struct tty_struct *tty)
 	if (info->tr_running ||
 	    info->xmit.head == info->xmit.tail ||
 	    tty->stopped ||
-	    tty->hw_stopped ||
 	    !info->xmit.buf)
 		return;
 
@@ -3176,7 +3167,6 @@ static int rs_raw_write(struct tty_struct *tty,
 
 	if (info->xmit.head != info->xmit.tail &&
 	    !tty->stopped &&
-	    !tty->hw_stopped &&
 	    !info->tr_running) {
 		start_transmit(info);
 	}
@@ -3400,10 +3390,10 @@ get_serial_info(struct e100_serial * info,
 	tmp.line = info->line;
 	tmp.port = (int)info->ioport;
 	tmp.irq = info->irq;
-	tmp.flags = info->flags;
+	tmp.flags = info->port.flags;
 	tmp.baud_base = info->baud_base;
-	tmp.close_delay = info->close_delay;
-	tmp.closing_wait = info->closing_wait;
+	tmp.close_delay = info->port.close_delay;
+	tmp.closing_wait = info->port.closing_wait;
 	tmp.custom_divisor = info->custom_divisor;
 	if (copy_to_user(retinfo, &tmp, sizeof(*retinfo)))
 		return -EFAULT;
@@ -3425,16 +3415,16 @@ set_serial_info(struct e100_serial *info,
 
 	if (!capable(CAP_SYS_ADMIN)) {
 		if ((new_serial.type != info->type) ||
-		    (new_serial.close_delay != info->close_delay) ||
+		    (new_serial.close_delay != info->port.close_delay) ||
 		    ((new_serial.flags & ~ASYNC_USR_MASK) !=
-		     (info->flags & ~ASYNC_USR_MASK)))
+		     (info->port.flags & ~ASYNC_USR_MASK)))
 			return -EPERM;
-		info->flags = ((info->flags & ~ASYNC_USR_MASK) |
+		info->port.flags = ((info->port.flags & ~ASYNC_USR_MASK) |
 			       (new_serial.flags & ASYNC_USR_MASK));
 		goto check_and_exit;
 	}
 
-	if (info->count > 1)
+	if (info->port.count > 1)
 		return -EBUSY;
 
 	/*
@@ -3443,16 +3433,16 @@ set_serial_info(struct e100_serial *info,
 	 */
 
 	info->baud_base = new_serial.baud_base;
-	info->flags = ((info->flags & ~ASYNC_FLAGS) |
+	info->port.flags = ((info->port.flags & ~ASYNC_FLAGS) |
 		       (new_serial.flags & ASYNC_FLAGS));
 	info->custom_divisor = new_serial.custom_divisor;
 	info->type = new_serial.type;
-	info->close_delay = new_serial.close_delay;
-	info->closing_wait = new_serial.closing_wait;
-	info->port.low_latency = (info->flags & ASYNC_LOW_LATENCY) ? 1 : 0;
+	info->port.close_delay = new_serial.close_delay;
+	info->port.closing_wait = new_serial.closing_wait;
+	info->port.low_latency = (info->port.flags & ASYNC_LOW_LATENCY) ? 1 : 0;
 
  check_and_exit:
-	if (info->flags & ASYNC_INITIALIZED) {
+	if (info->port.flags & ASYNC_INITIALIZED) {
 		change_speed(info);
 	} else
 		retval = startup(info);
@@ -3733,10 +3723,8 @@ rs_set_termios(struct tty_struct *tty, struct ktermios *old_termios)
 
 	/* Handle turning off CRTSCTS */
 	if ((old_termios->c_cflag & CRTSCTS) &&
-	    !(tty->termios.c_cflag & CRTSCTS)) {
-		tty->hw_stopped = 0;
+	    !(tty->termios.c_cflag & CRTSCTS))
 		rs_start(tty);
-	}
 
 }
 
@@ -3772,7 +3760,7 @@ rs_close(struct tty_struct *tty, struct file * filp)
 	printk("[%d] rs_close ttyS%d, count = %d\n", current->pid,
 	       info->line, info->count);
 #endif
-	if ((tty->count == 1) && (info->count != 1)) {
+	if ((tty->count == 1) && (info->port.count != 1)) {
 		/*
 		 * Uh, oh.  tty->count is 1, which means that the tty
 		 * structure will be freed.  Info->count should always
@@ -3782,32 +3770,32 @@ rs_close(struct tty_struct *tty, struct file * filp)
 		 */
 		printk(KERN_ERR
 		       "rs_close: bad serial port count; tty->count is 1, "
-		       "info->count is %d\n", info->count);
-		info->count = 1;
+		       "info->count is %d\n", info->port.count);
+		info->port.count = 1;
 	}
-	if (--info->count < 0) {
+	if (--info->port.count < 0) {
 		printk(KERN_ERR "rs_close: bad serial port count for ttyS%d: %d\n",
-		       info->line, info->count);
-		info->count = 0;
+		       info->line, info->port.count);
+		info->port.count = 0;
 	}
-	if (info->count) {
+	if (info->port.count) {
 		local_irq_restore(flags);
 		return;
 	}
-	info->flags |= ASYNC_CLOSING;
+	info->port.flags |= ASYNC_CLOSING;
 	/*
 	 * Save the termios structure, since this port may have
 	 * separate termios for callout and dialin.
 	 */
-	if (info->flags & ASYNC_NORMAL_ACTIVE)
+	if (info->port.flags & ASYNC_NORMAL_ACTIVE)
 		info->normal_termios = tty->termios;
 	/*
 	 * Now we wait for the transmit buffer to clear; and we notify
 	 * the line discipline to only process XON/XOFF characters.
 	 */
 	tty->closing = 1;
-	if (info->closing_wait != ASYNC_CLOSING_WAIT_NONE)
-		tty_wait_until_sent(tty, info->closing_wait);
+	if (info->port.closing_wait != ASYNC_CLOSING_WAIT_NONE)
+		tty_wait_until_sent(tty, info->port.closing_wait);
 	/*
 	 * At this point we stop accepting input.  To do this, we
 	 * disable the serial receiver and the DMA receive interrupt.
@@ -3820,7 +3808,7 @@ rs_close(struct tty_struct *tty, struct file * filp)
 	e100_disable_rx(info);
 	e100_disable_rx_irq(info);
 
-	if (info->flags & ASYNC_INITIALIZED) {
+	if (info->port.flags & ASYNC_INITIALIZED) {
 		/*
 		 * Before we drop DTR, make sure the UART transmitter
 		 * has completely drained; this is especially
@@ -3836,13 +3824,13 @@ rs_close(struct tty_struct *tty, struct file * filp)
 	tty->closing = 0;
 	info->event = 0;
 	info->port.tty = NULL;
-	if (info->blocked_open) {
-		if (info->close_delay)
-			schedule_timeout_interruptible(info->close_delay);
-		wake_up_interruptible(&info->open_wait);
+	if (info->port.blocked_open) {
+		if (info->port.close_delay)
+			schedule_timeout_interruptible(info->port.close_delay);
+		wake_up_interruptible(&info->port.open_wait);
 	}
-	info->flags &= ~(ASYNC_NORMAL_ACTIVE|ASYNC_CLOSING);
-	wake_up_interruptible(&info->close_wait);
+	info->port.flags &= ~(ASYNC_NORMAL_ACTIVE|ASYNC_CLOSING);
+	wake_up_interruptible(&info->port.close_wait);
 	local_irq_restore(flags);
 
 	/* port closed */
@@ -3935,10 +3923,10 @@ rs_hangup(struct tty_struct *tty)
 	rs_flush_buffer(tty);
 	shutdown(info);
 	info->event = 0;
-	info->count = 0;
-	info->flags &= ~ASYNC_NORMAL_ACTIVE;
+	info->port.count = 0;
+	info->port.flags &= ~ASYNC_NORMAL_ACTIVE;
 	info->port.tty = NULL;
-	wake_up_interruptible(&info->open_wait);
+	wake_up_interruptible(&info->port.open_wait);
 }
 
 /*
@@ -3960,11 +3948,11 @@ block_til_ready(struct tty_struct *tty, struct file * filp,
 	 * until it's done, and then try again.
 	 */
 	if (tty_hung_up_p(filp) ||
-	    (info->flags & ASYNC_CLOSING)) {
-		wait_event_interruptible_tty(tty, info->close_wait,
-			!(info->flags & ASYNC_CLOSING));
+	    (info->port.flags & ASYNC_CLOSING)) {
+		wait_event_interruptible_tty(tty, info->port.close_wait,
+			!(info->port.flags & ASYNC_CLOSING));
 #ifdef SERIAL_DO_RESTART
-		if (info->flags & ASYNC_HUP_NOTIFY)
+		if (info->port.flags & ASYNC_HUP_NOTIFY)
 			return -EAGAIN;
 		else
 			return -ERESTARTSYS;
@@ -3979,7 +3967,7 @@ block_til_ready(struct tty_struct *tty, struct file * filp,
 	 */
 	if ((filp->f_flags & O_NONBLOCK) ||
 	    (tty->flags & (1 << TTY_IO_ERROR))) {
-		info->flags |= ASYNC_NORMAL_ACTIVE;
+		info->port.flags |= ASYNC_NORMAL_ACTIVE;
 		return 0;
 	}
 
@@ -3990,23 +3978,23 @@ block_til_ready(struct tty_struct *tty, struct file * filp,
 	/*
 	 * Block waiting for the carrier detect and the line to become
 	 * free (i.e., not in use by the callout).  While we are in
-	 * this loop, info->count is dropped by one, so that
+	 * this loop, info->port.count is dropped by one, so that
 	 * rs_close() knows when to free things.  We restore it upon
 	 * exit, either normal or abnormal.
 	 */
 	retval = 0;
-	add_wait_queue(&info->open_wait, &wait);
+	add_wait_queue(&info->port.open_wait, &wait);
 #ifdef SERIAL_DEBUG_OPEN
 	printk("block_til_ready before block: ttyS%d, count = %d\n",
-	       info->line, info->count);
+	       info->line, info->port.count);
 #endif
 	local_irq_save(flags);
 	if (!tty_hung_up_p(filp)) {
 		extra_count++;
-		info->count--;
+		info->port.count--;
 	}
 	local_irq_restore(flags);
-	info->blocked_open++;
+	info->port.blocked_open++;
 	while (1) {
 		local_irq_save(flags);
 		/* assert RTS and DTR */
@@ -4015,9 +4003,9 @@ block_til_ready(struct tty_struct *tty, struct file * filp,
 		local_irq_restore(flags);
 		set_current_state(TASK_INTERRUPTIBLE);
 		if (tty_hung_up_p(filp) ||
-		    !(info->flags & ASYNC_INITIALIZED)) {
+		    !(info->port.flags & ASYNC_INITIALIZED)) {
 #ifdef SERIAL_DO_RESTART
-			if (info->flags & ASYNC_HUP_NOTIFY)
+			if (info->port.flags & ASYNC_HUP_NOTIFY)
 				retval = -EAGAIN;
 			else
 				retval = -ERESTARTSYS;
@@ -4026,7 +4014,7 @@ block_til_ready(struct tty_struct *tty, struct file * filp,
 #endif
 			break;
 		}
-		if (!(info->flags & ASYNC_CLOSING) && do_clocal)
+		if (!(info->port.flags & ASYNC_CLOSING) && do_clocal)
 			/* && (do_clocal || DCD_IS_ASSERTED) */
 			break;
 		if (signal_pending(current)) {
@@ -4035,24 +4023,24 @@ block_til_ready(struct tty_struct *tty, struct file * filp,
 		}
 #ifdef SERIAL_DEBUG_OPEN
 		printk("block_til_ready blocking: ttyS%d, count = %d\n",
-		       info->line, info->count);
+		       info->line, info->port.count);
 #endif
 		tty_unlock(tty);
 		schedule();
 		tty_lock(tty);
 	}
 	set_current_state(TASK_RUNNING);
-	remove_wait_queue(&info->open_wait, &wait);
+	remove_wait_queue(&info->port.open_wait, &wait);
 	if (extra_count)
-		info->count++;
-	info->blocked_open--;
+		info->port.count++;
+	info->port.blocked_open--;
 #ifdef SERIAL_DEBUG_OPEN
 	printk("block_til_ready after blocking: ttyS%d, count = %d\n",
-	       info->line, info->count);
+	       info->line, info->port.count);
 #endif
 	if (retval)
 		return retval;
-	info->flags |= ASYNC_NORMAL_ACTIVE;
+	info->port.flags |= ASYNC_NORMAL_ACTIVE;
 	return 0;
 }
 
@@ -4086,24 +4074,24 @@ rs_open(struct tty_struct *tty, struct file * filp)
 
 #ifdef SERIAL_DEBUG_OPEN
         printk("[%d] rs_open %s, count = %d\n", current->pid, tty->name,
- 	       info->count);
+ 	       info->port.count);
 #endif
 
-	info->count++;
+	info->port.count++;
 	tty->driver_data = info;
 	info->port.tty = tty;
 
-	info->port.low_latency = !!(info->flags & ASYNC_LOW_LATENCY);
+	info->port.low_latency = !!(info->port.flags & ASYNC_LOW_LATENCY);
 
 	/*
 	 * If the port is in the middle of closing, bail out now
 	 */
 	if (tty_hung_up_p(filp) ||
-	    (info->flags & ASYNC_CLOSING)) {
-		wait_event_interruptible_tty(tty, info->close_wait,
-			!(info->flags & ASYNC_CLOSING));
+	    (info->port.flags & ASYNC_CLOSING)) {
+		wait_event_interruptible_tty(tty, info->port.close_wait,
+			!(info->port.flags & ASYNC_CLOSING));
 #ifdef SERIAL_DO_RESTART
-		return ((info->flags & ASYNC_HUP_NOTIFY) ?
+		return ((info->port.flags & ASYNC_HUP_NOTIFY) ?
 			-EAGAIN : -ERESTARTSYS);
 #else
 		return -EAGAIN;
@@ -4113,7 +4101,7 @@ rs_open(struct tty_struct *tty, struct file * filp)
 	/*
 	 * If DMA is enabled try to allocate the irq's.
 	 */
-	if (info->count == 1) {
+	if (info->port.count == 1) {
 		allocated_resources = 1;
 		if (info->dma_in_enabled) {
 			if (request_irq(info->dma_in_irq_nbr,
@@ -4186,7 +4174,7 @@ rs_open(struct tty_struct *tty, struct file * filp)
 		if (allocated_resources)
 			deinit_port(info);
 
-		/* FIXME Decrease count info->count here too? */
+		/* FIXME Decrease count info->port.count here too? */
 		return retval;
 	}
 
@@ -4203,7 +4191,7 @@ rs_open(struct tty_struct *tty, struct file * filp)
 		return retval;
 	}
 
-	if ((info->count == 1) && (info->flags & ASYNC_SPLIT_TERMIOS)) {
+	if ((info->port.count == 1) && (info->port.flags & ASYNC_SPLIT_TERMIOS)) {
 		tty->termios = info->normal_termios;
 		change_speed(info);
 	}
@@ -4256,9 +4244,6 @@ static void seq_line_info(struct seq_file *m, struct e100_serial *info)
 		if (info->port.tty->stopped)
 			seq_printf(m, " stopped:%i",
 				   (int)info->port.tty->stopped);
-		if (info->port.tty->hw_stopped)
-			seq_printf(m, " hw_stopped:%i",
-				   (int)info->port.tty->hw_stopped);
 	}
 
 	{
@@ -4455,16 +4440,9 @@ static int __init rs_init(void)
 		info->forced_eop = 0;
 		info->baud_base = DEF_BAUD_BASE;
 		info->custom_divisor = 0;
-		info->flags = 0;
-		info->close_delay = 5*HZ/10;
-		info->closing_wait = 30*HZ;
 		info->x_char = 0;
 		info->event = 0;
-		info->count = 0;
-		info->blocked_open = 0;
 		info->normal_termios = driver->init_termios;
-		init_waitqueue_head(&info->open_wait);
-		init_waitqueue_head(&info->close_wait);
 		info->xmit.buf = NULL;
 		info->xmit.tail = info->xmit.head = 0;
 		info->first_recv_buffer = info->last_recv_buffer = NULL;
diff --git a/drivers/tty/serial/crisv10.h b/drivers/tty/serial/crisv10.h
index ea0beb46a10..7599014ae03 100644
--- a/drivers/tty/serial/crisv10.h
+++ b/drivers/tty/serial/crisv10.h
@@ -53,8 +53,6 @@ struct e100_serial {
 	volatile u8 *icmdadr;		/* adr to R_DMA_CHx_CMD */
 	volatile u32 *idescradr;	/* adr to R_DMA_CHx_DESCR */
 
-	int flags;	/* defined in tty.h */
-
 	u8 rx_ctrl;	/* shadow for R_SERIALx_REC_CTRL */
 	u8 tx_ctrl;	/* shadow for R_SERIALx_TR_CTRL */
 	u8 iseteop;	/* bit number for R_SET_EOP for the input dma */
@@ -88,19 +86,10 @@ struct e100_serial {
 
 	volatile int		tr_running; /* 1 if output is running */
 
-	struct tty_struct	*tty;
-	int			read_status_mask;
-	int			ignore_status_mask;
 	int			x_char;	/* xon/xoff character */
-	int			close_delay;
-	unsigned short		closing_wait;
-	unsigned short		closing_wait2;
 	unsigned long		event;
-	unsigned long		last_active;
 	int			line;
 	int			type;  /* PORT_ETRAX */
-	int			count;	    /* # of fd on device */
-	int			blocked_open; /* # of blocked opens */
 	struct circ_buf		xmit;
 	struct etrax_recv_buffer *first_recv_buffer;
 	struct etrax_recv_buffer *last_recv_buffer;
@@ -110,9 +99,6 @@ struct e100_serial {
 	struct work_struct	work;
 	struct async_icount	icount;   /* error-statistics etc.*/
 	struct ktermios		normal_termios;
-	struct ktermios		callout_termios;
-	wait_queue_head_t	open_wait;
-	wait_queue_head_t	close_wait;
 
 	unsigned long char_time_usec;       /* The time for 1 char, in usecs */
 	unsigned long flush_time_usec;      /* How often we should flush */
diff --git a/drivers/tty/serial/icom.c b/drivers/tty/serial/icom.c
index bc9e6b017b0..18ed5aebb16 100644
--- a/drivers/tty/serial/icom.c
+++ b/drivers/tty/serial/icom.c
@@ -1415,8 +1415,7 @@ static int icom_alloc_adapter(struct icom_adapter
 	struct icom_adapter *cur_adapter_entry;
 	struct list_head *tmp;
 
-	icom_adapter = (struct icom_adapter *)
-	    kzalloc(sizeof(struct icom_adapter), GFP_KERNEL);
+	icom_adapter = kzalloc(sizeof(struct icom_adapter), GFP_KERNEL);
 
 	if (!icom_adapter) {
 		return -ENOMEM;
diff --git a/drivers/tty/serial/ifx6x60.c b/drivers/tty/serial/ifx6x60.c
index 68d7ce997ed..8b1534c424a 100644
--- a/drivers/tty/serial/ifx6x60.c
+++ b/drivers/tty/serial/ifx6x60.c
@@ -270,23 +270,6 @@ static void mrdy_assert(struct ifx_spi_device *ifx_dev)
 }
 
 /**
- *	ifx_spi_hangup		-	hang up an IFX device
- *	@ifx_dev: our SPI device
- *
- *	Hang up the tty attached to the IFX device if one is currently
- *	open. If not take no action
- */
-static void ifx_spi_ttyhangup(struct ifx_spi_device *ifx_dev)
-{
-	struct tty_port *pport = &ifx_dev->tty_port;
-	struct tty_struct *tty = tty_port_tty_get(pport);
-	if (tty) {
-		tty_hangup(tty);
-		tty_kref_put(tty);
-	}
-}
-
-/**
  *	ifx_spi_timeout		-	SPI timeout
  *	@arg: our SPI device
  *
@@ -298,7 +281,7 @@ static void ifx_spi_timeout(unsigned long arg)
 	struct ifx_spi_device *ifx_dev = (struct ifx_spi_device *)arg;
 
 	dev_warn(&ifx_dev->spi_dev->dev, "*** SPI Timeout ***");
-	ifx_spi_ttyhangup(ifx_dev);
+	tty_port_tty_hangup(&ifx_dev->tty_port, false);
 	mrdy_set_low(ifx_dev);
 	clear_bit(IFX_SPI_STATE_TIMER_PENDING, &ifx_dev->flags);
 }
@@ -443,25 +426,6 @@ static void ifx_spi_setup_spi_header(unsigned char *txbuffer, int tx_count,
 }
 
 /**
- *	ifx_spi_wakeup_serial	-	SPI space made
- *	@port_data: our SPI device
- *
- *	We have emptied the FIFO enough that we want to get more data
- *	queued into it. Poke the line discipline via tty_wakeup so that
- *	it will feed us more bits
- */
-static void ifx_spi_wakeup_serial(struct ifx_spi_device *ifx_dev)
-{
-	struct tty_struct *tty;
-
-	tty = tty_port_tty_get(&ifx_dev->tty_port);
-	if (!tty)
-		return;
-	tty_wakeup(tty);
-	tty_kref_put(tty);
-}
-
-/**
  *	ifx_spi_prepare_tx_buffer	-	prepare transmit frame
  *	@ifx_dev: our SPI device
  *
@@ -506,7 +470,7 @@ static int ifx_spi_prepare_tx_buffer(struct ifx_spi_device *ifx_dev)
 			tx_count += temp_count;
 			if (temp_count == queue_length)
 				/* poke port to get more data */
-				ifx_spi_wakeup_serial(ifx_dev);
+				tty_port_tty_wakeup(&ifx_dev->tty_port);
 			else /* more data in port, use next SPI message */
 				ifx_dev->spi_more = 1;
 		}
@@ -683,8 +647,6 @@ static void ifx_spi_insert_flip_string(struct ifx_spi_device *ifx_dev,
 static void ifx_spi_complete(void *ctx)
 {
 	struct ifx_spi_device *ifx_dev = ctx;
-	struct tty_struct *tty;
-	struct tty_ldisc *ldisc = NULL;
 	int length;
 	int actual_length;
 	unsigned char more;
@@ -762,15 +724,7 @@ complete_exit:
 			 */
 			ifx_spi_power_state_clear(ifx_dev,
 						  IFX_SPI_POWER_DATA_PENDING);
-			tty = tty_port_tty_get(&ifx_dev->tty_port);
-			if (tty) {
-				ldisc = tty_ldisc_ref(tty);
-				if (ldisc) {
-					ldisc->ops->write_wakeup(tty);
-					tty_ldisc_deref(ldisc);
-				}
-				tty_kref_put(tty);
-			}
+			tty_port_tty_wakeup(&ifx_dev->tty_port);
 		}
 	}
 }
@@ -962,7 +916,7 @@ static irqreturn_t ifx_spi_reset_interrupt(int irq, void *dev)
 		set_bit(MR_INPROGRESS, &ifx_dev->mdm_reset_state);
 		if (!solreset) {
 			/* unsolicited reset  */
-			ifx_spi_ttyhangup(ifx_dev);
+			tty_port_tty_hangup(&ifx_dev->tty_port, false);
 		}
 	} else {
 		/* exited reset */
@@ -1325,30 +1279,6 @@ static void ifx_spi_spi_shutdown(struct spi_device *spi)
  */
 
 /**
- *	ifx_spi_spi_suspend	-	suspend SPI on system suspend
- *	@dev: device being suspended
- *
- *	Suspend the SPI side. No action needed on Intel MID platforms, may
- *	need extending for other systems.
- */
-static int ifx_spi_spi_suspend(struct spi_device *spi, pm_message_t msg)
-{
-	return 0;
-}
-
-/**
- *	ifx_spi_spi_resume	-	resume SPI side on system resume
- *	@dev: device being suspended
- *
- *	Suspend the SPI side. No action needed on Intel MID platforms, may
- *	need extending for other systems.
- */
-static int ifx_spi_spi_resume(struct spi_device *spi)
-{
-	return 0;
-}
-
-/**
  *	ifx_spi_pm_suspend	-	suspend modem on system suspend
  *	@dev: device being suspended
  *
@@ -1437,8 +1367,6 @@ static struct spi_driver ifx_spi_driver = {
 	.probe = ifx_spi_spi_probe,
 	.shutdown = ifx_spi_spi_shutdown,
 	.remove = ifx_spi_spi_remove,
-	.suspend = ifx_spi_spi_suspend,
-	.resume = ifx_spi_spi_resume,
 	.id_table = ifx_id_table
 };
 
diff --git a/drivers/tty/serial/jsm/jsm_tty.c b/drivers/tty/serial/jsm/jsm_tty.c
index 00f250ae14c..27bb75070c9 100644
--- a/drivers/tty/serial/jsm/jsm_tty.c
+++ b/drivers/tty/serial/jsm/jsm_tty.c
@@ -596,12 +596,6 @@ void jsm_input(struct jsm_channel *ch)
 
 	jsm_dbg(READ, &ch->ch_bd->pci_dev, "start 2\n");
 
-	if (data_len <= 0) {
-		spin_unlock_irqrestore(&ch->ch_lock, lock_flags);
-		jsm_dbg(READ, &ch->ch_bd->pci_dev, "jsm_input 1\n");
-		return;
-	}
-
 	len = tty_buffer_request_room(port, data_len);
 	n = len;
 
diff --git a/drivers/tty/serial/max3100.c b/drivers/tty/serial/max3100.c
index 32517d4bcea..35866d5872a 100644
--- a/drivers/tty/serial/max3100.c
+++ b/drivers/tty/serial/max3100.c
@@ -778,7 +778,7 @@ static int max3100_probe(struct spi_device *spi)
 	max3100s[i]->spi = spi;
 	max3100s[i]->irq = spi->irq;
 	spin_lock_init(&max3100s[i]->conf_lock);
-	dev_set_drvdata(&spi->dev, max3100s[i]);
+	spi_set_drvdata(spi, max3100s[i]);
 	pdata = spi->dev.platform_data;
 	max3100s[i]->crystal = pdata->crystal;
 	max3100s[i]->loopback = pdata->loopback;
@@ -819,7 +819,7 @@ static int max3100_probe(struct spi_device *spi)
 
 static int max3100_remove(struct spi_device *spi)
 {
-	struct max3100_port *s = dev_get_drvdata(&spi->dev);
+	struct max3100_port *s = spi_get_drvdata(spi);
 	int i;
 
 	mutex_lock(&max3100s_lock);
@@ -849,11 +849,11 @@ static int max3100_remove(struct spi_device *spi)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 
-static int max3100_suspend(struct spi_device *spi, pm_message_t state)
+static int max3100_suspend(struct device *dev)
 {
-	struct max3100_port *s = dev_get_drvdata(&spi->dev);
+	struct max3100_port *s = dev_get_drvdata(dev);
 
 	dev_dbg(&s->spi->dev, "%s\n", __func__);
 
@@ -874,9 +874,9 @@ static int max3100_suspend(struct spi_device *spi, pm_message_t state)
 	return 0;
 }
 
-static int max3100_resume(struct spi_device *spi)
+static int max3100_resume(struct device *dev)
 {
-	struct max3100_port *s = dev_get_drvdata(&spi->dev);
+	struct max3100_port *s = dev_get_drvdata(dev);
 
 	dev_dbg(&s->spi->dev, "%s\n", __func__);
 
@@ -894,21 +894,21 @@ static int max3100_resume(struct spi_device *spi)
 	return 0;
 }
 
+static SIMPLE_DEV_PM_OPS(max3100_pm_ops, max3100_suspend, max3100_resume);
+#define MAX3100_PM_OPS (&max3100_pm_ops)
+
 #else
-#define max3100_suspend NULL
-#define max3100_resume  NULL
+#define MAX3100_PM_OPS NULL
 #endif
 
 static struct spi_driver max3100_driver = {
 	.driver = {
 		.name		= "max3100",
 		.owner		= THIS_MODULE,
+		.pm		= MAX3100_PM_OPS,
 	},
-
 	.probe		= max3100_probe,
 	.remove		= max3100_remove,
-	.suspend	= max3100_suspend,
-	.resume		= max3100_resume,
 };
 
 module_spi_driver(max3100_driver);
diff --git a/drivers/tty/serial/max310x.c b/drivers/tty/serial/max310x.c
index 0c2422cb04e..8941e641894 100644
--- a/drivers/tty/serial/max310x.c
+++ b/drivers/tty/serial/max310x.c
@@ -881,12 +881,14 @@ static struct uart_ops max310x_ops = {
 	.verify_port	= max310x_verify_port,
 };
 
-static int max310x_suspend(struct spi_device *spi, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+
+static int max310x_suspend(struct device *dev)
 {
 	int ret;
-	struct max310x_port *s = dev_get_drvdata(&spi->dev);
+	struct max310x_port *s = dev_get_drvdata(dev);
 
-	dev_dbg(&spi->dev, "Suspend\n");
+	dev_dbg(dev, "Suspend\n");
 
 	ret = uart_suspend_port(&s->uart, &s->port);
 
@@ -905,11 +907,11 @@ static int max310x_suspend(struct spi_device *spi, pm_message_t state)
 	return ret;
 }
 
-static int max310x_resume(struct spi_device *spi)
+static int max310x_resume(struct device *dev)
 {
-	struct max310x_port *s = dev_get_drvdata(&spi->dev);
+	struct max310x_port *s = dev_get_drvdata(dev);
 
-	dev_dbg(&spi->dev, "Resume\n");
+	dev_dbg(dev, "Resume\n");
 
 	if (s->pdata->suspend)
 		s->pdata->suspend(0);
@@ -928,6 +930,13 @@ static int max310x_resume(struct spi_device *spi)
 	return uart_resume_port(&s->uart, &s->port);
 }
 
+static SIMPLE_DEV_PM_OPS(max310x_pm_ops, max310x_suspend, max310x_resume);
+#define MAX310X_PM_OPS (&max310x_pm_ops)
+
+#else
+#define MAX310X_PM_OPS NULL
+#endif
+
 #ifdef CONFIG_GPIOLIB
 static int max310x_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
@@ -1242,11 +1251,10 @@ static struct spi_driver max310x_driver = {
 	.driver = {
 		.name	= "max310x",
 		.owner	= THIS_MODULE,
+		.pm	= MAX310X_PM_OPS,
 	},
 	.probe		= max310x_probe,
 	.remove		= max310x_remove,
-	.suspend	= max310x_suspend,
-	.resume		= max310x_resume,
 	.id_table	= max310x_id_table,
 };
 module_spi_driver(max310x_driver);
diff --git a/drivers/tty/serial/mrst_max3110.c b/drivers/tty/serial/mrst_max3110.c
index f641c232bec..9b6ef20420c 100644
--- a/drivers/tty/serial/mrst_max3110.c
+++ b/drivers/tty/serial/mrst_max3110.c
@@ -743,9 +743,10 @@ static struct uart_driver serial_m3110_reg = {
 	.cons		= &serial_m3110_console,
 };
 
-#ifdef CONFIG_PM
-static int serial_m3110_suspend(struct spi_device *spi, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int serial_m3110_suspend(struct device *dev)
 {
+	struct spi_device *spi = to_spi_device(dev);
 	struct uart_max3110 *max = spi_get_drvdata(spi);
 
 	disable_irq(max->irq);
@@ -754,8 +755,9 @@ static int serial_m3110_suspend(struct spi_device *spi, pm_message_t state)
 	return 0;
 }
 
-static int serial_m3110_resume(struct spi_device *spi)
+static int serial_m3110_resume(struct device *dev)
 {
+	struct spi_device *spi = to_spi_device(dev);
 	struct uart_max3110 *max = spi_get_drvdata(spi);
 
 	max3110_out(max, max->cur_conf);
@@ -763,9 +765,13 @@ static int serial_m3110_resume(struct spi_device *spi)
 	enable_irq(max->irq);
 	return 0;
 }
+
+static SIMPLE_DEV_PM_OPS(serial_m3110_pm_ops, serial_m3110_suspend,
+			serial_m3110_resume);
+#define SERIAL_M3110_PM_OPS (&serial_m3110_pm_ops)
+
 #else
-#define serial_m3110_suspend	NULL
-#define serial_m3110_resume	NULL
+#define SERIAL_M3110_PM_OPS NULL
 #endif
 
 static int serial_m3110_probe(struct spi_device *spi)
@@ -872,11 +878,10 @@ static struct spi_driver uart_max3110_driver = {
 	.driver = {
 			.name	= "spi_max3111",
 			.owner	= THIS_MODULE,
+			.pm	= SERIAL_M3110_PM_OPS,
 	},
 	.probe		= serial_m3110_probe,
 	.remove		= serial_m3110_remove,
-	.suspend	= serial_m3110_suspend,
-	.resume		= serial_m3110_resume,
 };
 
 static int __init serial_m3110_init(void)
diff --git a/drivers/tty/serial/msm_serial_hs.c b/drivers/tty/serial/msm_serial_hs.c
index 4a942c78347..4ca2f64861e 100644
--- a/drivers/tty/serial/msm_serial_hs.c
+++ b/drivers/tty/serial/msm_serial_hs.c
@@ -907,7 +907,6 @@ static void msm_hs_dmov_rx_callback(struct msm_dmov_cmd *cmd_ptr,
 	unsigned int error_f = 0;
 	unsigned long flags;
 	unsigned int flush;
-	struct tty_struct *tty;
 	struct tty_port *port;
 	struct uart_port *uport;
 	struct msm_hs_port *msm_uport;
@@ -919,7 +918,6 @@ static void msm_hs_dmov_rx_callback(struct msm_dmov_cmd *cmd_ptr,
 	clk_enable(msm_uport->clk);
 
 	port = &uport->state->port;
-	tty = port->tty;
 
 	msm_hs_write(uport, UARTDM_CR_ADDR, STALE_EVENT_DISABLE);
 
diff --git a/drivers/tty/serial/msm_smd_tty.c b/drivers/tty/serial/msm_smd_tty.c
index e722ff163d9..1238ac370bf 100644
--- a/drivers/tty/serial/msm_smd_tty.c
+++ b/drivers/tty/serial/msm_smd_tty.c
@@ -90,13 +90,13 @@ static void smd_tty_notify(void *priv, unsigned event)
 
 static int smd_tty_port_activate(struct tty_port *tport, struct tty_struct *tty)
 {
+	struct smd_tty_info *info = container_of(tport, struct smd_tty_info,
+			port);
 	int i, res = 0;
-	int n = tty->index;
 	const char *name = NULL;
-	struct smd_tty_info *info = smd_tty + n;
 
 	for (i = 0; i < smd_tty_channels_len; i++) {
-		if (smd_tty_channels[i].id == n) {
+		if (smd_tty_channels[i].id == tty->index) {
 			name = smd_tty_channels[i].name;
 			break;
 		}
@@ -117,17 +117,13 @@ static int smd_tty_port_activate(struct tty_port *tport, struct tty_struct *tty)
 
 static void smd_tty_port_shutdown(struct tty_port *tport)
 {
-	struct smd_tty_info *info;
-	struct tty_struct *tty = tty_port_tty_get(tport);
+	struct smd_tty_info *info = container_of(tport, struct smd_tty_info,
+			port);
 
-	info = tty->driver_data;
 	if (info->ch) {
 		smd_close(info->ch);
 		info->ch = 0;
 	}
-
-	tty->driver_data = 0;
-	tty_kref_put(tty);
 }
 
 static int smd_tty_open(struct tty_struct *tty, struct file *f)
diff --git a/drivers/tty/serial/mxs-auart.c b/drivers/tty/serial/mxs-auart.c
index d549fe1fa42..62e7d3b015a 100644
--- a/drivers/tty/serial/mxs-auart.c
+++ b/drivers/tty/serial/mxs-auart.c
@@ -883,7 +883,7 @@ auart_console_write(struct console *co, const char *str, unsigned int count)
 	unsigned int old_ctrl0, old_ctrl2;
 	unsigned int to = 1000;
 
-	if (co->index >	MXS_AUART_PORTS || co->index < 0)
+	if (co->index >= MXS_AUART_PORTS || co->index < 0)
 		return;
 
 	s = auart_port[co->index];
@@ -1103,7 +1103,7 @@ static int mxs_auart_probe(struct platform_device *pdev)
 	s->port.fifosize = 16;
 	s->port.uartclk = clk_get_rate(s->clk);
 	s->port.type = PORT_IMX;
-	s->port.dev = s->dev = get_device(&pdev->dev);
+	s->port.dev = s->dev = &pdev->dev;
 
 	s->ctrl = 0;
 
@@ -1134,7 +1134,6 @@ out_free_irq:
 	auart_port[pdev->id] = NULL;
 	free_irq(s->irq, s);
 out_free_clk:
-	put_device(s->dev);
 	clk_put(s->clk);
 out_free:
 	kfree(s);
@@ -1150,7 +1149,6 @@ static int mxs_auart_remove(struct platform_device *pdev)
 
 	auart_port[pdev->id] = NULL;
 
-	put_device(s->dev);
 	clk_put(s->clk);
 	free_irq(s->irq, s);
 	kfree(s);
diff --git a/drivers/tty/serial/of_serial.c b/drivers/tty/serial/of_serial.c
index b025d543827..39c7ea4cb14 100644
--- a/drivers/tty/serial/of_serial.c
+++ b/drivers/tty/serial/of_serial.c
@@ -14,7 +14,6 @@
 #include <linux/slab.h>
 #include <linux/delay.h>
 #include <linux/serial_core.h>
-#include <linux/serial_8250.h>
 #include <linux/serial_reg.h>
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
@@ -22,6 +21,8 @@
 #include <linux/nwpserial.h>
 #include <linux/clk.h>
 
+#include "8250/8250.h"
+
 struct of_serial_info {
 	struct clk *clk;
 	int type;
@@ -97,6 +98,10 @@ static int of_platform_serial_setup(struct platform_device *ofdev,
 	if (of_property_read_u32(np, "reg-shift", &prop) == 0)
 		port->regshift = prop;
 
+	/* Check for fifo size */
+	if (of_property_read_u32(np, "fifo-size", &prop) == 0)
+		port->fifosize = prop;
+
 	port->irq = irq_of_parse_and_map(np, 0);
 	port->iotype = UPIO_MEM;
 	if (of_property_read_u32(np, "reg-io-width", &prop) == 0) {
@@ -167,11 +172,17 @@ static int of_platform_serial_probe(struct platform_device *ofdev)
 #ifdef CONFIG_SERIAL_8250
 	case PORT_8250 ... PORT_MAX_8250:
 	{
-		/* For now the of bindings don't support the extra
-		   8250 specific bits */
 		struct uart_8250_port port8250;
 		memset(&port8250, 0, sizeof(port8250));
 		port8250.port = port;
+
+		if (port.fifosize)
+			port8250.capabilities = UART_CAP_FIFO;
+
+		if (of_property_read_bool(ofdev->dev.of_node,
+					  "auto-flow-control"))
+			port8250.capabilities |= UART_CAP_AFE;
+
 		ret = serial8250_register_8250_port(&port8250);
 		break;
 	}
diff --git a/drivers/tty/serial/omap-serial.c b/drivers/tty/serial/omap-serial.c
index 4dc41408ecb..30d4f7a783c 100644
--- a/drivers/tty/serial/omap-serial.c
+++ b/drivers/tty/serial/omap-serial.c
@@ -886,6 +886,17 @@ serial_omap_set_termios(struct uart_port *port, struct ktermios *termios,
 	serial_out(up, UART_MCR, up->mcr | UART_MCR_TCRTLR);
 	/* FIFO ENABLE, DMA MODE */
 
+	up->scr |= OMAP_UART_SCR_RX_TRIG_GRANU1_MASK;
+	/*
+	 * NOTE: Setting OMAP_UART_SCR_RX_TRIG_GRANU1_MASK
+	 * sets Enables the granularity of 1 for TRIGGER RX
+	 * level. Along with setting RX FIFO trigger level
+	 * to 1 (as noted below, 16 characters) and TLR[3:0]
+	 * to zero this will result RX FIFO threshold level
+	 * to 1 character, instead of 16 as noted in comment
+	 * below.
+	 */
+
 	/* Set receive FIFO threshold to 16 characters and
 	 * transmit FIFO threshold to 16 spaces
 	 */
diff --git a/drivers/tty/serial/pch_uart.c b/drivers/tty/serial/pch_uart.c
index 7a6c989924b..21a7e179edf 100644
--- a/drivers/tty/serial/pch_uart.c
+++ b/drivers/tty/serial/pch_uart.c
@@ -1493,29 +1493,6 @@ static int pch_uart_verify_port(struct uart_port *port,
 	return 0;
 }
 
-static struct uart_ops pch_uart_ops = {
-	.tx_empty = pch_uart_tx_empty,
-	.set_mctrl = pch_uart_set_mctrl,
-	.get_mctrl = pch_uart_get_mctrl,
-	.stop_tx = pch_uart_stop_tx,
-	.start_tx = pch_uart_start_tx,
-	.stop_rx = pch_uart_stop_rx,
-	.enable_ms = pch_uart_enable_ms,
-	.break_ctl = pch_uart_break_ctl,
-	.startup = pch_uart_startup,
-	.shutdown = pch_uart_shutdown,
-	.set_termios = pch_uart_set_termios,
-/*	.pm		= pch_uart_pm,		Not supported yet */
-/*	.set_wake	= pch_uart_set_wake,	Not supported yet */
-	.type = pch_uart_type,
-	.release_port = pch_uart_release_port,
-	.request_port = pch_uart_request_port,
-	.config_port = pch_uart_config_port,
-	.verify_port = pch_uart_verify_port
-};
-
-#ifdef CONFIG_SERIAL_PCH_UART_CONSOLE
-
 /*
  *	Wait for transmitter & holding register to empty
  */
@@ -1547,6 +1524,84 @@ static void wait_for_xmitr(struct eg20t_port *up, int bits)
 	}
 }
 
+#ifdef CONFIG_CONSOLE_POLL
+/*
+ * Console polling routines for communicate via uart while
+ * in an interrupt or debug context.
+ */
+static int pch_uart_get_poll_char(struct uart_port *port)
+{
+	struct eg20t_port *priv =
+		container_of(port, struct eg20t_port, port);
+	u8 lsr = ioread8(priv->membase + UART_LSR);
+
+	if (!(lsr & UART_LSR_DR))
+		return NO_POLL_CHAR;
+
+	return ioread8(priv->membase + PCH_UART_RBR);
+}
+
+
+static void pch_uart_put_poll_char(struct uart_port *port,
+			 unsigned char c)
+{
+	unsigned int ier;
+	struct eg20t_port *priv =
+		container_of(port, struct eg20t_port, port);
+
+	/*
+	 * First save the IER then disable the interrupts
+	 */
+	ier = ioread8(priv->membase + UART_IER);
+	pch_uart_hal_disable_interrupt(priv, PCH_UART_HAL_ALL_INT);
+
+	wait_for_xmitr(priv, UART_LSR_THRE);
+	/*
+	 * Send the character out.
+	 * If a LF, also do CR...
+	 */
+	iowrite8(c, priv->membase + PCH_UART_THR);
+	if (c == 10) {
+		wait_for_xmitr(priv, UART_LSR_THRE);
+		iowrite8(13, priv->membase + PCH_UART_THR);
+	}
+
+	/*
+	 * Finally, wait for transmitter to become empty
+	 * and restore the IER
+	 */
+	wait_for_xmitr(priv, BOTH_EMPTY);
+	iowrite8(ier, priv->membase + UART_IER);
+}
+#endif /* CONFIG_CONSOLE_POLL */
+
+static struct uart_ops pch_uart_ops = {
+	.tx_empty = pch_uart_tx_empty,
+	.set_mctrl = pch_uart_set_mctrl,
+	.get_mctrl = pch_uart_get_mctrl,
+	.stop_tx = pch_uart_stop_tx,
+	.start_tx = pch_uart_start_tx,
+	.stop_rx = pch_uart_stop_rx,
+	.enable_ms = pch_uart_enable_ms,
+	.break_ctl = pch_uart_break_ctl,
+	.startup = pch_uart_startup,
+	.shutdown = pch_uart_shutdown,
+	.set_termios = pch_uart_set_termios,
+/*	.pm		= pch_uart_pm,		Not supported yet */
+/*	.set_wake	= pch_uart_set_wake,	Not supported yet */
+	.type = pch_uart_type,
+	.release_port = pch_uart_release_port,
+	.request_port = pch_uart_request_port,
+	.config_port = pch_uart_config_port,
+	.verify_port = pch_uart_verify_port,
+#ifdef CONFIG_CONSOLE_POLL
+	.poll_get_char = pch_uart_get_poll_char,
+	.poll_put_char = pch_uart_put_poll_char,
+#endif
+};
+
+#ifdef CONFIG_SERIAL_PCH_UART_CONSOLE
+
 static void pch_console_putchar(struct uart_port *port, int ch)
 {
 	struct eg20t_port *priv =
@@ -1655,7 +1710,7 @@ static struct console pch_console = {
 #define PCH_CONSOLE	(&pch_console)
 #else
 #define PCH_CONSOLE	NULL
-#endif
+#endif	/* CONFIG_SERIAL_PCH_UART_CONSOLE */
 
 static struct uart_driver pch_uart_driver = {
 	.owner = THIS_MODULE,
diff --git a/drivers/tty/serial/samsung.c b/drivers/tty/serial/samsung.c
index 2769a38d15b..074b9194144 100644
--- a/drivers/tty/serial/samsung.c
+++ b/drivers/tty/serial/samsung.c
@@ -39,6 +39,7 @@
 #include <linux/tty_flip.h>
 #include <linux/serial_core.h>
 #include <linux/serial.h>
+#include <linux/serial_s3c.h>
 #include <linux/delay.h>
 #include <linux/clk.h>
 #include <linux/cpufreq.h>
@@ -46,10 +47,9 @@
 
 #include <asm/irq.h>
 
-#include <mach/hardware.h>
-
-#include <plat/regs-serial.h>
+#ifdef CONFIG_SAMSUNG_CLOCK
 #include <plat/clock.h>
+#endif
 
 #include "samsung.h"
 
@@ -446,6 +446,8 @@ static void s3c24xx_serial_shutdown(struct uart_port *port)
 
 	/* Clear pending interrupts and mask all interrupts */
 	if (s3c24xx_serial_has_interrupt_mask(port)) {
+		free_irq(port->irq, ourport);
+
 		wr_regl(port, S3C64XX_UINTP, 0xf);
 		wr_regl(port, S3C64XX_UINTM, 0xf);
 	}
@@ -505,6 +507,8 @@ static int s3c64xx_serial_startup(struct uart_port *port)
 	dbg("s3c64xx_serial_startup: port=%p (%08lx,%p)\n",
 	    port->mapbase, port->membase);
 
+	wr_regl(port, S3C64XX_UINTM, 0xf);
+
 	ret = request_irq(port->irq, s3c64xx_serial_handle_irq, IRQF_SHARED,
 			  s3c24xx_serial_portname(port), ourport);
 	if (ret) {
@@ -894,7 +898,7 @@ console_initcall(s3c24xx_serial_console_init);
 #define S3C24XX_SERIAL_CONSOLE NULL
 #endif
 
-#ifdef CONFIG_CONSOLE_POLL
+#if defined(CONFIG_SERIAL_SAMSUNG_CONSOLE) && defined(CONFIG_CONSOLE_POLL)
 static int s3c24xx_serial_get_poll_char(struct uart_port *port);
 static void s3c24xx_serial_put_poll_char(struct uart_port *port,
 			 unsigned char c);
@@ -918,7 +922,7 @@ static struct uart_ops s3c24xx_serial_ops = {
 	.request_port	= s3c24xx_serial_request_port,
 	.config_port	= s3c24xx_serial_config_port,
 	.verify_port	= s3c24xx_serial_verify_port,
-#ifdef CONFIG_CONSOLE_POLL
+#if defined(CONFIG_SERIAL_SAMSUNG_CONSOLE) && defined(CONFIG_CONSOLE_POLL)
 	.poll_get_char = s3c24xx_serial_get_poll_char,
 	.poll_put_char = s3c24xx_serial_put_poll_char,
 #endif
@@ -1179,6 +1183,7 @@ static int s3c24xx_serial_init_port(struct s3c24xx_uart_port *ourport,
 	return 0;
 }
 
+#ifdef CONFIG_SAMSUNG_CLOCK
 static ssize_t s3c24xx_serial_show_clksrc(struct device *dev,
 					  struct device_attribute *attr,
 					  char *buf)
@@ -1194,7 +1199,7 @@ static ssize_t s3c24xx_serial_show_clksrc(struct device *dev,
 }
 
 static DEVICE_ATTR(clock_source, S_IRUGO, s3c24xx_serial_show_clksrc, NULL);
-
+#endif
 
 /* Device driver serial port probe */
 
@@ -1252,9 +1257,11 @@ static int s3c24xx_serial_probe(struct platform_device *pdev)
 	uart_add_one_port(&s3c24xx_uart_drv, &ourport->port);
 	platform_set_drvdata(pdev, &ourport->port);
 
+#ifdef CONFIG_SAMSUNG_CLOCK
 	ret = device_create_file(&pdev->dev, &dev_attr_clock_source);
 	if (ret < 0)
 		dev_err(&pdev->dev, "failed to add clock source attr.\n");
+#endif
 
 	ret = s3c24xx_serial_cpufreq_register(ourport);
 	if (ret < 0)
@@ -1272,7 +1279,9 @@ static int s3c24xx_serial_remove(struct platform_device *dev)
 
 	if (port) {
 		s3c24xx_serial_cpufreq_deregister(to_ourport(port));
+#ifdef CONFIG_SAMSUNG_CLOCK
 		device_remove_file(&dev->dev, &dev_attr_clock_source);
+#endif
 		uart_remove_one_port(&s3c24xx_uart_drv, port);
 	}
 
@@ -1307,9 +1316,29 @@ static int s3c24xx_serial_resume(struct device *dev)
 	return 0;
 }
 
+static int s3c24xx_serial_resume_noirq(struct device *dev)
+{
+	struct uart_port *port = s3c24xx_dev_to_port(dev);
+
+	if (port) {
+		/* restore IRQ mask */
+		if (s3c24xx_serial_has_interrupt_mask(port)) {
+			unsigned int uintm = 0xf;
+			if (tx_enabled(port))
+				uintm &= ~S3C64XX_UINTM_TXD_MSK;
+			if (rx_enabled(port))
+				uintm &= ~S3C64XX_UINTM_RXD_MSK;
+			wr_regl(port, S3C64XX_UINTM, uintm);
+		}
+	}
+
+	return 0;
+}
+
 static const struct dev_pm_ops s3c24xx_serial_pm_ops = {
 	.suspend = s3c24xx_serial_suspend,
 	.resume = s3c24xx_serial_resume,
+	.resume_noirq = s3c24xx_serial_resume_noirq,
 };
 #define SERIAL_SAMSUNG_PM_OPS	(&s3c24xx_serial_pm_ops)
 
@@ -1343,6 +1372,13 @@ s3c24xx_serial_console_txrdy(struct uart_port *port, unsigned int ufcon)
 	return (utrstat & S3C2410_UTRSTAT_TXE) ? 1 : 0;
 }
 
+static bool
+s3c24xx_port_configured(unsigned int ucon)
+{
+	/* consider the serial port configured if the tx/rx mode set */
+	return (ucon & 0xf) != 0;
+}
+
 #ifdef CONFIG_CONSOLE_POLL
 /*
  * Console polling routines for writing and reading from the uart while
@@ -1365,6 +1401,11 @@ static void s3c24xx_serial_put_poll_char(struct uart_port *port,
 		unsigned char c)
 {
 	unsigned int ufcon = rd_regl(cons_uart, S3C2410_UFCON);
+	unsigned int ucon = rd_regl(cons_uart, S3C2410_UCON);
+
+	/* not possible to xmit on unconfigured port */
+	if (!s3c24xx_port_configured(ucon))
+		return;
 
 	while (!s3c24xx_serial_console_txrdy(port, ufcon))
 		cpu_relax();
@@ -1377,6 +1418,12 @@ static void
 s3c24xx_serial_console_putchar(struct uart_port *port, int ch)
 {
 	unsigned int ufcon = rd_regl(cons_uart, S3C2410_UFCON);
+	unsigned int ucon = rd_regl(cons_uart, S3C2410_UCON);
+
+	/* not possible to xmit on unconfigured port */
+	if (!s3c24xx_port_configured(ucon))
+		return;
+
 	while (!s3c24xx_serial_console_txrdy(port, ufcon))
 		barrier();
 	wr_regb(cons_uart, S3C2410_UTXH, ch);
@@ -1409,9 +1456,7 @@ s3c24xx_serial_get_options(struct uart_port *port, int *baud,
 	    "registers: ulcon=%08x, ucon=%08x, ubdriv=%08x\n",
 	    port, ulcon, ucon, ubrdiv);
 
-	if ((ucon & 0xf) != 0) {
-		/* consider the serial port configured if the tx/rx mode set */
-
+	if (s3c24xx_port_configured(ucon)) {
 		switch (ulcon & S3C2410_LCON_CSMASK) {
 		case S3C2410_LCON_CS5:
 			*bits = 5;
diff --git a/drivers/tty/serial/samsung.h b/drivers/tty/serial/samsung.h
index 1a4bca3e417..00a499ecd38 100644
--- a/drivers/tty/serial/samsung.h
+++ b/drivers/tty/serial/samsung.h
@@ -76,7 +76,9 @@ struct s3c24xx_uart_port {
 #define wr_regb(port, reg, val) __raw_writeb(val, portaddr(port, reg))
 #define wr_regl(port, reg, val) __raw_writel(val, portaddr(port, reg))
 
-#ifdef CONFIG_SERIAL_SAMSUNG_DEBUG
+#if defined(CONFIG_SERIAL_SAMSUNG_DEBUG) && \
+    defined(CONFIG_DEBUG_LL) && \
+    !defined(MODULE)
 
 extern void printascii(const char *);
 
diff --git a/drivers/tty/serial/sccnxp.c b/drivers/tty/serial/sccnxp.c
index 08dbfb88d42..c7730415541 100644
--- a/drivers/tty/serial/sccnxp.c
+++ b/drivers/tty/serial/sccnxp.c
@@ -27,6 +27,7 @@
 #include <linux/spinlock.h>
 #include <linux/platform_device.h>
 #include <linux/platform_data/serial-sccnxp.h>
+#include <linux/regulator/consumer.h>
 
 #define SCCNXP_NAME			"uart-sccnxp"
 #define SCCNXP_MAJOR			204
@@ -131,6 +132,8 @@ struct sccnxp_port {
 	struct timer_list	timer;
 
 	struct sccnxp_pdata	pdata;
+
+	struct regulator	*regulator;
 };
 
 static inline u8 sccnxp_raw_read(void __iomem *base, u8 reg, u8 shift)
@@ -789,8 +792,6 @@ static int sccnxp_probe(struct platform_device *pdev)
 		return -EADDRNOTAVAIL;
 	}
 
-	dev_set_name(&pdev->dev, SCCNXP_NAME);
-
 	s = devm_kzalloc(&pdev->dev, sizeof(struct sccnxp_port), GFP_KERNEL);
 	if (!s) {
 		dev_err(&pdev->dev, "Error allocating port structure\n");
@@ -918,6 +919,16 @@ static int sccnxp_probe(struct platform_device *pdev)
 		goto err_out;
 	}
 
+	s->regulator = devm_regulator_get(&pdev->dev, "VCC");
+	if (!IS_ERR(s->regulator)) {
+		ret = regulator_enable(s->regulator);
+		if (ret) {
+			dev_err(&pdev->dev,
+				"Failed to enable regulator: %i\n", ret);
+			return ret;
+		}
+	}
+
 	membase = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(membase)) {
 		ret = PTR_ERR(membase);
@@ -967,10 +978,6 @@ static int sccnxp_probe(struct platform_device *pdev)
 	s->imr = 0;
 	sccnxp_write(&s->port[0], SCCNXP_IMR_REG, 0);
 
-	/* Board specific configure */
-	if (s->pdata.init)
-		s->pdata.init();
-
 	if (!s->poll) {
 		ret = devm_request_threaded_irq(&pdev->dev, s->irq, NULL,
 						sccnxp_ist,
@@ -1011,8 +1018,8 @@ static int sccnxp_remove(struct platform_device *pdev)
 	uart_unregister_driver(&s->uart);
 	platform_set_drvdata(pdev, NULL);
 
-	if (s->pdata.exit)
-		s->pdata.exit();
+	if (!IS_ERR(s->regulator))
+		return regulator_disable(s->regulator);
 
 	return 0;
 }
diff --git a/drivers/tty/serial/serial-tegra.c b/drivers/tty/serial/serial-tegra.c
index 372de8ade76..9799d043a9b 100644
--- a/drivers/tty/serial/serial-tegra.c
+++ b/drivers/tty/serial/serial-tegra.c
@@ -26,6 +26,7 @@
 #include <linux/dmaengine.h>
 #include <linux/dma-mapping.h>
 #include <linux/dmapool.h>
+#include <linux/err.h>
 #include <linux/io.h>
 #include <linux/irq.h>
 #include <linux/module.h>
@@ -1301,11 +1302,9 @@ static int tegra_uart_probe(struct platform_device *pdev)
 	}
 
 	u->mapbase = resource->start;
-	u->membase = devm_request_and_ioremap(&pdev->dev, resource);
-	if (!u->membase) {
-		dev_err(&pdev->dev, "memregion/iomap address req failed\n");
-		return -EADDRNOTAVAIL;
-	}
+	u->membase = devm_ioremap_resource(&pdev->dev, resource);
+	if (IS_ERR(u->membase))
+		return PTR_ERR(u->membase);
 
 	tup->uart_clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(tup->uart_clk)) {
diff --git a/drivers/tty/serial/serial_core.c b/drivers/tty/serial/serial_core.c
index a400002dfa8..8fbb6d22cdc 100644
--- a/drivers/tty/serial/serial_core.c
+++ b/drivers/tty/serial/serial_core.c
@@ -1941,6 +1941,8 @@ int uart_suspend_port(struct uart_driver *drv, struct uart_port *uport)
 		mutex_unlock(&port->mutex);
 		return 0;
 	}
+	put_device(tty_dev);
+
 	if (console_suspend_enabled || !uart_console(uport))
 		uport->suspended = 1;
 
@@ -2006,9 +2008,11 @@ int uart_resume_port(struct uart_driver *drv, struct uart_port *uport)
 			disable_irq_wake(uport->irq);
 			uport->irq_wake = 0;
 		}
+		put_device(tty_dev);
 		mutex_unlock(&port->mutex);
 		return 0;
 	}
+	put_device(tty_dev);
 	uport->suspended = 0;
 
 	/*
diff --git a/drivers/tty/serial/sh-sci.h b/drivers/tty/serial/sh-sci.h
index 4c22a1529aa..5aca7364634 100644
--- a/drivers/tty/serial/sh-sci.h
+++ b/drivers/tty/serial/sh-sci.h
@@ -15,8 +15,6 @@
     defined(CONFIG_CPU_SUBTYPE_SH7720) || \
     defined(CONFIG_CPU_SUBTYPE_SH7721) || \
     defined(CONFIG_ARCH_SH73A0) || \
-    defined(CONFIG_ARCH_SH7367) || \
-    defined(CONFIG_ARCH_SH7377) || \
     defined(CONFIG_ARCH_SH7372) || \
     defined(CONFIG_ARCH_R8A7740)
 
diff --git a/drivers/tty/serial/sirfsoc_uart.c b/drivers/tty/serial/sirfsoc_uart.c
index 6bbfe9934a4..03465b67394 100644
--- a/drivers/tty/serial/sirfsoc_uart.c
+++ b/drivers/tty/serial/sirfsoc_uart.c
@@ -758,7 +758,7 @@ static struct of_device_id sirfsoc_uart_ids[] = {
 	{ .compatible = "sirf,marco-uart", },
 	{}
 };
-MODULE_DEVICE_TABLE(of, sirfsoc_serial_of_match);
+MODULE_DEVICE_TABLE(of, sirfsoc_uart_ids);
 
 static struct platform_driver sirfsoc_uart_driver = {
 	.probe		= sirfsoc_uart_probe,
diff --git a/drivers/tty/serial/sunsab.c b/drivers/tty/serial/sunsab.c
index 8de2213664e..a422c8b55a4 100644
--- a/drivers/tty/serial/sunsab.c
+++ b/drivers/tty/serial/sunsab.c
@@ -203,7 +203,7 @@ receive_chars(struct uart_sunsab_port *up,
 				flag = TTY_FRAME;
 		}
 
-		if (uart_handle_sysrq_char(&up->port, ch))
+		if (uart_handle_sysrq_char(&up->port, ch) || !port)
 			continue;
 
 		if ((stat->sreg.isr0 & (up->port.ignore_status_mask & 0xff)) == 0 &&
diff --git a/drivers/tty/serial/sunzilog.c b/drivers/tty/serial/sunzilog.c
index 27669ff3d44..813ef8eb8ef 100644
--- a/drivers/tty/serial/sunzilog.c
+++ b/drivers/tty/serial/sunzilog.c
@@ -388,7 +388,7 @@ sunzilog_receive_chars(struct uart_sunzilog_port *up,
 			else if (r1 & CRC_ERR)
 				flag = TTY_FRAME;
 		}
-		if (uart_handle_sysrq_char(&up->port, ch))
+		if (uart_handle_sysrq_char(&up->port, ch) || !port)
 			continue;
 
 		if (up->port.ignore_status_mask == 0xff ||
diff --git a/drivers/tty/serial/vt8500_serial.c b/drivers/tty/serial/vt8500_serial.c
index 705240e6c4e..1a8bc2275ea 100644
--- a/drivers/tty/serial/vt8500_serial.c
+++ b/drivers/tty/serial/vt8500_serial.c
@@ -35,6 +35,7 @@
 #include <linux/clk.h>
 #include <linux/platform_device.h>
 #include <linux/of.h>
+#include <linux/err.h>
 
 /*
  * UART Register offsets
@@ -585,9 +586,9 @@ static int vt8500_serial_probe(struct platform_device *pdev)
 	if (!vt8500_port)
 		return -ENOMEM;
 
-	vt8500_port->uart.membase = devm_request_and_ioremap(&pdev->dev, mmres);
-	if (!vt8500_port->uart.membase)
-		return -EADDRNOTAVAIL;
+	vt8500_port->uart.membase = devm_ioremap_resource(&pdev->dev, mmres);
+	if (IS_ERR(vt8500_port->uart.membase))
+		return PTR_ERR(vt8500_port->uart.membase);
 
 	vt8500_port->clk = of_clk_get(pdev->dev.of_node, 0);
 	if (IS_ERR(vt8500_port->clk)) {
diff --git a/drivers/tty/serial/xilinx_uartps.c b/drivers/tty/serial/xilinx_uartps.c
index ba451c7209f..4e5c77834c5 100644
--- a/drivers/tty/serial/xilinx_uartps.c
+++ b/drivers/tty/serial/xilinx_uartps.c
@@ -578,14 +578,13 @@ static int xuartps_startup(struct uart_port *port)
 	/* Receive Timeout register is enabled with value of 10 */
 	xuartps_writel(10, XUARTPS_RXTOUT_OFFSET);
 
+	/* Clear out any pending interrupts before enabling them */
+	xuartps_writel(xuartps_readl(XUARTPS_ISR_OFFSET), XUARTPS_ISR_OFFSET);
 
 	/* Set the Interrupt Registers with desired interrupts */
 	xuartps_writel(XUARTPS_IXR_TXEMPTY | XUARTPS_IXR_PARITY |
 		XUARTPS_IXR_FRAMING | XUARTPS_IXR_OVERRUN |
 		XUARTPS_IXR_RXTRIG | XUARTPS_IXR_TOUT, XUARTPS_IER_OFFSET);
-	xuartps_writel(~(XUARTPS_IXR_TXEMPTY | XUARTPS_IXR_PARITY |
-		XUARTPS_IXR_FRAMING | XUARTPS_IXR_OVERRUN |
-		XUARTPS_IXR_RXTRIG | XUARTPS_IXR_TOUT), XUARTPS_IDR_OFFSET);
 
 	return retval;
 }
diff --git a/drivers/tty/synclink.c b/drivers/tty/synclink.c
index 8983276aa35..8eaf1ab8add 100644
--- a/drivers/tty/synclink.c
+++ b/drivers/tty/synclink.c
@@ -1058,9 +1058,6 @@ static void mgsl_bh_handler(struct work_struct *work)
 		container_of(work, struct mgsl_struct, task);
 	int action;
 
-	if (!info)
-		return;
-		
 	if ( debug_level >= DEBUG_LEVEL_BH )
 		printk( "%s(%d):mgsl_bh_handler(%s) entry\n",
 			__FILE__,__LINE__,info->device_name);
@@ -3311,7 +3308,7 @@ static int block_til_ready(struct tty_struct *tty, struct file * filp,
 	port->blocked_open++;
 	
 	while (1) {
-		if (tty->termios.c_cflag & CBAUD)
+		if (C_BAUD(tty) && test_bit(ASYNCB_INITIALIZED, &port->flags))
 			tty_port_raise_dtr_rts(port);
 		
 		set_current_state(TASK_INTERRUPTIBLE);
diff --git a/drivers/tty/synclink_gt.c b/drivers/tty/synclink_gt.c
index aa9eece35c3..1abf946463f 100644
--- a/drivers/tty/synclink_gt.c
+++ b/drivers/tty/synclink_gt.c
@@ -3308,7 +3308,7 @@ static int block_til_ready(struct tty_struct *tty, struct file *filp,
 	port->blocked_open++;
 
 	while (1) {
-		if ((tty->termios.c_cflag & CBAUD))
+		if (C_BAUD(tty) && test_bit(ASYNCB_INITIALIZED, &port->flags))
 			tty_port_raise_dtr_rts(port);
 
 		set_current_state(TASK_INTERRUPTIBLE);
diff --git a/drivers/tty/synclinkmp.c b/drivers/tty/synclinkmp.c
index 6d5780cf1d5..ff171384ea5 100644
--- a/drivers/tty/synclinkmp.c
+++ b/drivers/tty/synclinkmp.c
@@ -3329,7 +3329,7 @@ static int block_til_ready(struct tty_struct *tty, struct file *filp,
 	port->blocked_open++;
 
 	while (1) {
-		if (tty->termios.c_cflag & CBAUD)
+		if (C_BAUD(tty) && test_bit(ASYNCB_INITIALIZED, &port->flags))
 			tty_port_raise_dtr_rts(port);
 
 		set_current_state(TASK_INTERRUPTIBLE);
diff --git a/drivers/tty/sysrq.c b/drivers/tty/sysrq.c
index 3687f0cad64..0a0de333c76 100644
--- a/drivers/tty/sysrq.c
+++ b/drivers/tty/sysrq.c
@@ -101,7 +101,7 @@ static void sysrq_handle_SAK(int key)
 }
 static struct sysrq_key_op sysrq_SAK_op = {
 	.handler	= sysrq_handle_SAK,
-	.help_msg	= "saK",
+	.help_msg	= "sak(k)",
 	.action_msg	= "SAK",
 	.enable_mask	= SYSRQ_ENABLE_KEYBOARD,
 };
@@ -117,7 +117,7 @@ static void sysrq_handle_unraw(int key)
 
 static struct sysrq_key_op sysrq_unraw_op = {
 	.handler	= sysrq_handle_unraw,
-	.help_msg	= "unRaw",
+	.help_msg	= "unraw(r)",
 	.action_msg	= "Keyboard mode set to system default",
 	.enable_mask	= SYSRQ_ENABLE_KEYBOARD,
 };
@@ -135,7 +135,7 @@ static void sysrq_handle_crash(int key)
 }
 static struct sysrq_key_op sysrq_crash_op = {
 	.handler	= sysrq_handle_crash,
-	.help_msg	= "Crash",
+	.help_msg	= "crash(c)",
 	.action_msg	= "Trigger a crash",
 	.enable_mask	= SYSRQ_ENABLE_DUMP,
 };
@@ -148,7 +148,7 @@ static void sysrq_handle_reboot(int key)
 }
 static struct sysrq_key_op sysrq_reboot_op = {
 	.handler	= sysrq_handle_reboot,
-	.help_msg	= "reBoot",
+	.help_msg	= "reboot(b)",
 	.action_msg	= "Resetting",
 	.enable_mask	= SYSRQ_ENABLE_BOOT,
 };
@@ -159,7 +159,7 @@ static void sysrq_handle_sync(int key)
 }
 static struct sysrq_key_op sysrq_sync_op = {
 	.handler	= sysrq_handle_sync,
-	.help_msg	= "Sync",
+	.help_msg	= "sync(s)",
 	.action_msg	= "Emergency Sync",
 	.enable_mask	= SYSRQ_ENABLE_SYNC,
 };
@@ -171,7 +171,7 @@ static void sysrq_handle_show_timers(int key)
 
 static struct sysrq_key_op sysrq_show_timers_op = {
 	.handler	= sysrq_handle_show_timers,
-	.help_msg	= "show-all-timers(Q)",
+	.help_msg	= "show-all-timers(q)",
 	.action_msg	= "Show clockevent devices & pending hrtimers (no others)",
 };
 
@@ -181,7 +181,7 @@ static void sysrq_handle_mountro(int key)
 }
 static struct sysrq_key_op sysrq_mountro_op = {
 	.handler	= sysrq_handle_mountro,
-	.help_msg	= "Unmount",
+	.help_msg	= "unmount(u)",
 	.action_msg	= "Emergency Remount R/O",
 	.enable_mask	= SYSRQ_ENABLE_REMOUNT,
 };
@@ -194,7 +194,7 @@ static void sysrq_handle_showlocks(int key)
 
 static struct sysrq_key_op sysrq_showlocks_op = {
 	.handler	= sysrq_handle_showlocks,
-	.help_msg	= "show-all-locks(D)",
+	.help_msg	= "show-all-locks(d)",
 	.action_msg	= "Show Locks Held",
 };
 #else
@@ -245,7 +245,7 @@ static void sysrq_handle_showallcpus(int key)
 
 static struct sysrq_key_op sysrq_showallcpus_op = {
 	.handler	= sysrq_handle_showallcpus,
-	.help_msg	= "show-backtrace-all-active-cpus(L)",
+	.help_msg	= "show-backtrace-all-active-cpus(l)",
 	.action_msg	= "Show backtrace of all active CPUs",
 	.enable_mask	= SYSRQ_ENABLE_DUMP,
 };
@@ -260,7 +260,7 @@ static void sysrq_handle_showregs(int key)
 }
 static struct sysrq_key_op sysrq_showregs_op = {
 	.handler	= sysrq_handle_showregs,
-	.help_msg	= "show-registers(P)",
+	.help_msg	= "show-registers(p)",
 	.action_msg	= "Show Regs",
 	.enable_mask	= SYSRQ_ENABLE_DUMP,
 };
@@ -271,7 +271,7 @@ static void sysrq_handle_showstate(int key)
 }
 static struct sysrq_key_op sysrq_showstate_op = {
 	.handler	= sysrq_handle_showstate,
-	.help_msg	= "show-task-states(T)",
+	.help_msg	= "show-task-states(t)",
 	.action_msg	= "Show State",
 	.enable_mask	= SYSRQ_ENABLE_DUMP,
 };
@@ -282,7 +282,7 @@ static void sysrq_handle_showstate_blocked(int key)
 }
 static struct sysrq_key_op sysrq_showstate_blocked_op = {
 	.handler	= sysrq_handle_showstate_blocked,
-	.help_msg	= "show-blocked-tasks(W)",
+	.help_msg	= "show-blocked-tasks(w)",
 	.action_msg	= "Show Blocked State",
 	.enable_mask	= SYSRQ_ENABLE_DUMP,
 };
@@ -296,7 +296,7 @@ static void sysrq_ftrace_dump(int key)
 }
 static struct sysrq_key_op sysrq_ftrace_dump_op = {
 	.handler	= sysrq_ftrace_dump,
-	.help_msg	= "dump-ftrace-buffer(Z)",
+	.help_msg	= "dump-ftrace-buffer(z)",
 	.action_msg	= "Dump ftrace buffer",
 	.enable_mask	= SYSRQ_ENABLE_DUMP,
 };
@@ -310,7 +310,7 @@ static void sysrq_handle_showmem(int key)
 }
 static struct sysrq_key_op sysrq_showmem_op = {
 	.handler	= sysrq_handle_showmem,
-	.help_msg	= "show-memory-usage(M)",
+	.help_msg	= "show-memory-usage(m)",
 	.action_msg	= "Show Memory",
 	.enable_mask	= SYSRQ_ENABLE_DUMP,
 };
@@ -341,7 +341,7 @@ static void sysrq_handle_term(int key)
 }
 static struct sysrq_key_op sysrq_term_op = {
 	.handler	= sysrq_handle_term,
-	.help_msg	= "terminate-all-tasks(E)",
+	.help_msg	= "terminate-all-tasks(e)",
 	.action_msg	= "Terminate All Tasks",
 	.enable_mask	= SYSRQ_ENABLE_SIGNAL,
 };
@@ -360,7 +360,7 @@ static void sysrq_handle_moom(int key)
 }
 static struct sysrq_key_op sysrq_moom_op = {
 	.handler	= sysrq_handle_moom,
-	.help_msg	= "memory-full-oom-kill(F)",
+	.help_msg	= "memory-full-oom-kill(f)",
 	.action_msg	= "Manual OOM execution",
 	.enable_mask	= SYSRQ_ENABLE_SIGNAL,
 };
@@ -372,7 +372,7 @@ static void sysrq_handle_thaw(int key)
 }
 static struct sysrq_key_op sysrq_thaw_op = {
 	.handler	= sysrq_handle_thaw,
-	.help_msg	= "thaw-filesystems(J)",
+	.help_msg	= "thaw-filesystems(j)",
 	.action_msg	= "Emergency Thaw of all frozen filesystems",
 	.enable_mask	= SYSRQ_ENABLE_SIGNAL,
 };
@@ -385,7 +385,7 @@ static void sysrq_handle_kill(int key)
 }
 static struct sysrq_key_op sysrq_kill_op = {
 	.handler	= sysrq_handle_kill,
-	.help_msg	= "kill-all-tasks(I)",
+	.help_msg	= "kill-all-tasks(i)",
 	.action_msg	= "Kill All Tasks",
 	.enable_mask	= SYSRQ_ENABLE_SIGNAL,
 };
@@ -396,7 +396,7 @@ static void sysrq_handle_unrt(int key)
 }
 static struct sysrq_key_op sysrq_unrt_op = {
 	.handler	= sysrq_handle_unrt,
-	.help_msg	= "nice-all-RT-tasks(N)",
+	.help_msg	= "nice-all-RT-tasks(n)",
 	.action_msg	= "Nice All RT Tasks",
 	.enable_mask	= SYSRQ_ENABLE_RTNICE,
 };
diff --git a/drivers/tty/tty_buffer.c b/drivers/tty/tty_buffer.c
index 578aa7594b1..9121c1f7aee 100644
--- a/drivers/tty/tty_buffer.c
+++ b/drivers/tty/tty_buffer.c
@@ -449,11 +449,6 @@ static void flush_to_ldisc(struct work_struct *work)
 				tty_buffer_free(port, head);
 				continue;
 			}
-			/* Ldisc or user is trying to flush the buffers
-			   we are feeding to the ldisc, stop feeding the
-			   line discipline as we want to empty the queue */
-			if (test_bit(TTYP_FLUSHPENDING, &port->iflags))
-				break;
 			if (!tty->receive_room)
 				break;
 			if (count > tty->receive_room)
@@ -465,17 +460,20 @@ static void flush_to_ldisc(struct work_struct *work)
 			disc->ops->receive_buf(tty, char_buf,
 							flag_buf, count);
 			spin_lock_irqsave(&buf->lock, flags);
+			/* Ldisc or user is trying to flush the buffers.
+			   We may have a deferred request to flush the
+			   input buffer, if so pull the chain under the lock
+			   and empty the queue */
+			if (test_bit(TTYP_FLUSHPENDING, &port->iflags)) {
+				__tty_buffer_flush(port);
+				clear_bit(TTYP_FLUSHPENDING, &port->iflags);
+				wake_up(&tty->read_wait);
+				break;
+			}
 		}
 		clear_bit(TTYP_FLUSHING, &port->iflags);
 	}
 
-	/* We may have a deferred request to flush the input buffer,
-	   if so pull the chain under the lock and empty the queue */
-	if (test_bit(TTYP_FLUSHPENDING, &port->iflags)) {
-		__tty_buffer_flush(port);
-		clear_bit(TTYP_FLUSHPENDING, &port->iflags);
-		wake_up(&tty->read_wait);
-	}
 	spin_unlock_irqrestore(&buf->lock, flags);
 
 	tty_ldisc_deref(disc);
diff --git a/drivers/tty/tty_io.c b/drivers/tty/tty_io.c
index 05400acbc45..97ebc8c5864 100644
--- a/drivers/tty/tty_io.c
+++ b/drivers/tty/tty_io.c
@@ -533,6 +533,60 @@ void tty_wakeup(struct tty_struct *tty)
 EXPORT_SYMBOL_GPL(tty_wakeup);
 
 /**
+ *	tty_signal_session_leader	- sends SIGHUP to session leader
+ *	@tty		controlling tty
+ *	@exit_session	if non-zero, signal all foreground group processes
+ *
+ *	Send SIGHUP and SIGCONT to the session leader and its process group.
+ *	Optionally, signal all processes in the foreground process group.
+ *
+ *	Returns the number of processes in the session with this tty
+ *	as their controlling terminal. This value is used to drop
+ *	tty references for those processes.
+ */
+static int tty_signal_session_leader(struct tty_struct *tty, int exit_session)
+{
+	struct task_struct *p;
+	int refs = 0;
+	struct pid *tty_pgrp = NULL;
+
+	read_lock(&tasklist_lock);
+	if (tty->session) {
+		do_each_pid_task(tty->session, PIDTYPE_SID, p) {
+			spin_lock_irq(&p->sighand->siglock);
+			if (p->signal->tty == tty) {
+				p->signal->tty = NULL;
+				/* We defer the dereferences outside fo
+				   the tasklist lock */
+				refs++;
+			}
+			if (!p->signal->leader) {
+				spin_unlock_irq(&p->sighand->siglock);
+				continue;
+			}
+			__group_send_sig_info(SIGHUP, SEND_SIG_PRIV, p);
+			__group_send_sig_info(SIGCONT, SEND_SIG_PRIV, p);
+			put_pid(p->signal->tty_old_pgrp);  /* A noop */
+			spin_lock(&tty->ctrl_lock);
+			tty_pgrp = get_pid(tty->pgrp);
+			if (tty->pgrp)
+				p->signal->tty_old_pgrp = get_pid(tty->pgrp);
+			spin_unlock(&tty->ctrl_lock);
+			spin_unlock_irq(&p->sighand->siglock);
+		} while_each_pid_task(tty->session, PIDTYPE_SID, p);
+	}
+	read_unlock(&tasklist_lock);
+
+	if (tty_pgrp) {
+		if (exit_session)
+			kill_pgrp(tty_pgrp, SIGHUP, exit_session);
+		put_pid(tty_pgrp);
+	}
+
+	return refs;
+}
+
+/**
  *	__tty_hangup		-	actual handler for hangup events
  *	@work: tty device
  *
@@ -554,15 +608,13 @@ EXPORT_SYMBOL_GPL(tty_wakeup);
  *		  tasklist_lock to walk task list for hangup event
  *		    ->siglock to protect ->signal/->sighand
  */
-static void __tty_hangup(struct tty_struct *tty)
+static void __tty_hangup(struct tty_struct *tty, int exit_session)
 {
 	struct file *cons_filp = NULL;
 	struct file *filp, *f = NULL;
-	struct task_struct *p;
 	struct tty_file_private *priv;
 	int    closecount = 0, n;
-	unsigned long flags;
-	int refs = 0;
+	int refs;
 
 	if (!tty)
 		return;
@@ -599,39 +651,18 @@ static void __tty_hangup(struct tty_struct *tty)
 	}
 	spin_unlock(&tty_files_lock);
 
+	refs = tty_signal_session_leader(tty, exit_session);
+	/* Account for the p->signal references we killed */
+	while (refs--)
+		tty_kref_put(tty);
+
 	/*
 	 * it drops BTM and thus races with reopen
 	 * we protect the race by TTY_HUPPING
 	 */
 	tty_ldisc_hangup(tty);
 
-	read_lock(&tasklist_lock);
-	if (tty->session) {
-		do_each_pid_task(tty->session, PIDTYPE_SID, p) {
-			spin_lock_irq(&p->sighand->siglock);
-			if (p->signal->tty == tty) {
-				p->signal->tty = NULL;
-				/* We defer the dereferences outside fo
-				   the tasklist lock */
-				refs++;
-			}
-			if (!p->signal->leader) {
-				spin_unlock_irq(&p->sighand->siglock);
-				continue;
-			}
-			__group_send_sig_info(SIGHUP, SEND_SIG_PRIV, p);
-			__group_send_sig_info(SIGCONT, SEND_SIG_PRIV, p);
-			put_pid(p->signal->tty_old_pgrp);  /* A noop */
-			spin_lock_irqsave(&tty->ctrl_lock, flags);
-			if (tty->pgrp)
-				p->signal->tty_old_pgrp = get_pid(tty->pgrp);
-			spin_unlock_irqrestore(&tty->ctrl_lock, flags);
-			spin_unlock_irq(&p->sighand->siglock);
-		} while_each_pid_task(tty->session, PIDTYPE_SID, p);
-	}
-	read_unlock(&tasklist_lock);
-
-	spin_lock_irqsave(&tty->ctrl_lock, flags);
+	spin_lock_irq(&tty->ctrl_lock);
 	clear_bit(TTY_THROTTLED, &tty->flags);
 	clear_bit(TTY_PUSH, &tty->flags);
 	clear_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
@@ -640,11 +671,7 @@ static void __tty_hangup(struct tty_struct *tty)
 	tty->session = NULL;
 	tty->pgrp = NULL;
 	tty->ctrl_status = 0;
-	spin_unlock_irqrestore(&tty->ctrl_lock, flags);
-
-	/* Account for the p->signal references we killed */
-	while (refs--)
-		tty_kref_put(tty);
+	spin_unlock_irq(&tty->ctrl_lock);
 
 	/*
 	 * If one of the devices matches a console pointer, we
@@ -666,7 +693,6 @@ static void __tty_hangup(struct tty_struct *tty)
 	 */
 	set_bit(TTY_HUPPED, &tty->flags);
 	clear_bit(TTY_HUPPING, &tty->flags);
-	tty_ldisc_enable(tty);
 
 	tty_unlock(tty);
 
@@ -679,7 +705,7 @@ static void do_tty_hangup(struct work_struct *work)
 	struct tty_struct *tty =
 		container_of(work, struct tty_struct, hangup_work);
 
-	__tty_hangup(tty);
+	__tty_hangup(tty, 0);
 }
 
 /**
@@ -717,7 +743,7 @@ void tty_vhangup(struct tty_struct *tty)
 
 	printk(KERN_DEBUG "%s vhangup...\n", tty_name(tty, buf));
 #endif
-	__tty_hangup(tty);
+	__tty_hangup(tty, 0);
 }
 
 EXPORT_SYMBOL(tty_vhangup);
@@ -741,6 +767,27 @@ void tty_vhangup_self(void)
 }
 
 /**
+ *	tty_vhangup_session		-	hangup session leader exit
+ *	@tty: tty to hangup
+ *
+ *	The session leader is exiting and hanging up its controlling terminal.
+ *	Every process in the foreground process group is signalled SIGHUP.
+ *
+ *	We do this synchronously so that when the syscall returns the process
+ *	is complete. That guarantee is necessary for security reasons.
+ */
+
+static void tty_vhangup_session(struct tty_struct *tty)
+{
+#ifdef TTY_DEBUG_HANGUP
+	char	buf[64];
+
+	printk(KERN_DEBUG "%s vhangup session...\n", tty_name(tty, buf));
+#endif
+	__tty_hangup(tty, 1);
+}
+
+/**
  *	tty_hung_up_p		-	was tty hung up
  *	@filp: file pointer of tty
  *
@@ -797,18 +844,18 @@ void disassociate_ctty(int on_exit)
 
 	tty = get_current_tty();
 	if (tty) {
-		struct pid *tty_pgrp = get_pid(tty->pgrp);
-		if (on_exit) {
-			if (tty->driver->type != TTY_DRIVER_TYPE_PTY)
-				tty_vhangup(tty);
-		}
-		tty_kref_put(tty);
-		if (tty_pgrp) {
-			kill_pgrp(tty_pgrp, SIGHUP, on_exit);
-			if (!on_exit)
+		if (on_exit && tty->driver->type != TTY_DRIVER_TYPE_PTY) {
+			tty_vhangup_session(tty);
+		} else {
+			struct pid *tty_pgrp = tty_get_pgrp(tty);
+			if (tty_pgrp) {
+				kill_pgrp(tty_pgrp, SIGHUP, on_exit);
 				kill_pgrp(tty_pgrp, SIGCONT, on_exit);
-			put_pid(tty_pgrp);
+				put_pid(tty_pgrp);
+			}
 		}
+		tty_kref_put(tty);
+
 	} else if (on_exit) {
 		struct pid *old_pgrp;
 		spin_lock_irq(&current->sighand->siglock);
@@ -941,6 +988,14 @@ void start_tty(struct tty_struct *tty)
 
 EXPORT_SYMBOL(start_tty);
 
+static void tty_update_time(struct timespec *time)
+{
+	unsigned long sec = get_seconds();
+	sec -= sec % 60;
+	if ((long)(sec - time->tv_sec) > 0)
+		time->tv_sec = sec;
+}
+
 /**
  *	tty_read	-	read method for tty device files
  *	@file: pointer to tty file
@@ -960,10 +1015,11 @@ static ssize_t tty_read(struct file *file, char __user *buf, size_t count,
 			loff_t *ppos)
 {
 	int i;
+	struct inode *inode = file_inode(file);
 	struct tty_struct *tty = file_tty(file);
 	struct tty_ldisc *ld;
 
-	if (tty_paranoia_check(tty, file_inode(file), "tty_read"))
+	if (tty_paranoia_check(tty, inode, "tty_read"))
 		return -EIO;
 	if (!tty || (test_bit(TTY_IO_ERROR, &tty->flags)))
 		return -EIO;
@@ -977,6 +1033,9 @@ static ssize_t tty_read(struct file *file, char __user *buf, size_t count,
 		i = -EIO;
 	tty_ldisc_deref(ld);
 
+	if (i > 0)
+		tty_update_time(&inode->i_atime);
+
 	return i;
 }
 
@@ -1077,8 +1136,10 @@ static inline ssize_t do_tty_write(
 			break;
 		cond_resched();
 	}
-	if (written)
+	if (written) {
+		tty_update_time(&file_inode(file)->i_mtime);
 		ret = written;
+	}
 out:
 	tty_write_unlock(tty);
 	return ret;
@@ -1344,9 +1405,7 @@ static int tty_reopen(struct tty_struct *tty)
 	}
 	tty->count++;
 
-	mutex_lock(&tty->ldisc_mutex);
 	WARN_ON(!test_bit(TTY_LDISC, &tty->flags));
-	mutex_unlock(&tty->ldisc_mutex);
 
 	return 0;
 }
@@ -1463,6 +1522,17 @@ void tty_free_termios(struct tty_struct *tty)
 }
 EXPORT_SYMBOL(tty_free_termios);
 
+/**
+ *	tty_flush_works		-	flush all works of a tty
+ *	@tty: tty device to flush works for
+ *
+ *	Sync flush all works belonging to @tty.
+ */
+static void tty_flush_works(struct tty_struct *tty)
+{
+	flush_work(&tty->SAK_work);
+	flush_work(&tty->hangup_work);
+}
 
 /**
  *	release_one_tty		-	release tty structure memory
@@ -1548,6 +1618,7 @@ static void release_tty(struct tty_struct *tty, int idx)
 	tty_free_termios(tty);
 	tty_driver_remove_tty(tty->driver, tty);
 	tty->port->itty = NULL;
+	cancel_work_sync(&tty->port->buf.work);
 
 	if (tty->link)
 		tty_kref_put(tty->link);
@@ -1777,12 +1848,21 @@ int tty_release(struct inode *inode, struct file *filp)
 		return 0;
 
 #ifdef TTY_DEBUG_HANGUP
-	printk(KERN_DEBUG "%s: freeing tty structure...\n", __func__);
+	printk(KERN_DEBUG "%s: %s: final close\n", __func__, tty_name(tty, buf));
 #endif
 	/*
 	 * Ask the line discipline code to release its structures
 	 */
 	tty_ldisc_release(tty, o_tty);
+
+	/* Wait for pending work before tty destruction commmences */
+	tty_flush_works(tty);
+	if (o_tty)
+		tty_flush_works(o_tty);
+
+#ifdef TTY_DEBUG_HANGUP
+	printk(KERN_DEBUG "%s: %s: freeing structure...\n", __func__, tty_name(tty, buf));
+#endif
 	/*
 	 * The release_tty function takes care of the details of clearing
 	 * the slots and preserving the termios structure. The tty_unlock_pair
diff --git a/drivers/tty/tty_ioctl.c b/drivers/tty/tty_ioctl.c
index d58b92cc187..3500d411414 100644
--- a/drivers/tty/tty_ioctl.c
+++ b/drivers/tty/tty_ioctl.c
@@ -106,6 +106,7 @@ void tty_throttle(struct tty_struct *tty)
 	if (!test_and_set_bit(TTY_THROTTLED, &tty->flags) &&
 	    tty->ops->throttle)
 		tty->ops->throttle(tty);
+	tty->flow_change = 0;
 	mutex_unlock(&tty->termios_mutex);
 }
 EXPORT_SYMBOL(tty_throttle);
@@ -129,11 +130,74 @@ void tty_unthrottle(struct tty_struct *tty)
 	if (test_and_clear_bit(TTY_THROTTLED, &tty->flags) &&
 	    tty->ops->unthrottle)
 		tty->ops->unthrottle(tty);
+	tty->flow_change = 0;
 	mutex_unlock(&tty->termios_mutex);
 }
 EXPORT_SYMBOL(tty_unthrottle);
 
 /**
+ *	tty_throttle_safe	-	flow control
+ *	@tty: terminal
+ *
+ *	Similar to tty_throttle() but will only attempt throttle
+ *	if tty->flow_change is TTY_THROTTLE_SAFE. Prevents an accidental
+ *	throttle due to race conditions when throttling is conditional
+ *	on factors evaluated prior to throttling.
+ *
+ *	Returns 0 if tty is throttled (or was already throttled)
+ */
+
+int tty_throttle_safe(struct tty_struct *tty)
+{
+	int ret = 0;
+
+	mutex_lock(&tty->termios_mutex);
+	if (!test_bit(TTY_THROTTLED, &tty->flags)) {
+		if (tty->flow_change != TTY_THROTTLE_SAFE)
+			ret = 1;
+		else {
+			set_bit(TTY_THROTTLED, &tty->flags);
+			if (tty->ops->throttle)
+				tty->ops->throttle(tty);
+		}
+	}
+	mutex_unlock(&tty->termios_mutex);
+
+	return ret;
+}
+
+/**
+ *	tty_unthrottle_safe	-	flow control
+ *	@tty: terminal
+ *
+ *	Similar to tty_unthrottle() but will only attempt unthrottle
+ *	if tty->flow_change is TTY_UNTHROTTLE_SAFE. Prevents an accidental
+ *	unthrottle due to race conditions when unthrottling is conditional
+ *	on factors evaluated prior to unthrottling.
+ *
+ *	Returns 0 if tty is unthrottled (or was already unthrottled)
+ */
+
+int tty_unthrottle_safe(struct tty_struct *tty)
+{
+	int ret = 0;
+
+	mutex_lock(&tty->termios_mutex);
+	if (test_bit(TTY_THROTTLED, &tty->flags)) {
+		if (tty->flow_change != TTY_UNTHROTTLE_SAFE)
+			ret = 1;
+		else {
+			clear_bit(TTY_THROTTLED, &tty->flags);
+			if (tty->ops->unthrottle)
+				tty->ops->unthrottle(tty);
+		}
+	}
+	mutex_unlock(&tty->termios_mutex);
+
+	return ret;
+}
+
+/**
  *	tty_wait_until_sent	-	wait for I/O to finish
  *	@tty: tty we are waiting for
  *	@timeout: how long we will wait
@@ -415,34 +479,6 @@ void tty_encode_baud_rate(struct tty_struct *tty, speed_t ibaud, speed_t obaud)
 EXPORT_SYMBOL_GPL(tty_encode_baud_rate);
 
 /**
- *	tty_get_baud_rate	-	get tty bit rates
- *	@tty: tty to query
- *
- *	Returns the baud rate as an integer for this terminal. The
- *	termios lock must be held by the caller and the terminal bit
- *	flags may be updated.
- *
- *	Locking: none
- */
-
-speed_t tty_get_baud_rate(struct tty_struct *tty)
-{
-	speed_t baud = tty_termios_baud_rate(&tty->termios);
-
-	if (baud == 38400 && tty->alt_speed) {
-		if (!tty->warned) {
-			printk(KERN_WARNING "Use of setserial/setrocket to "
-					    "set SPD_* flags is deprecated\n");
-			tty->warned = 1;
-		}
-		baud = tty->alt_speed;
-	}
-
-	return baud;
-}
-EXPORT_SYMBOL(tty_get_baud_rate);
-
-/**
  *	tty_termios_copy_hw	-	copy hardware settings
  *	@new: New termios
  *	@old: Old termios
@@ -1086,14 +1122,12 @@ int tty_mode_ioctl(struct tty_struct *tty, struct file *file,
 }
 EXPORT_SYMBOL_GPL(tty_mode_ioctl);
 
-int tty_perform_flush(struct tty_struct *tty, unsigned long arg)
+
+/* Caller guarantees ldisc reference is held */
+static int __tty_perform_flush(struct tty_struct *tty, unsigned long arg)
 {
-	struct tty_ldisc *ld;
-	int retval = tty_check_change(tty);
-	if (retval)
-		return retval;
+	struct tty_ldisc *ld = tty->ldisc;
 
-	ld = tty_ldisc_ref_wait(tty);
 	switch (arg) {
 	case TCIFLUSH:
 		if (ld && ld->ops->flush_buffer) {
@@ -1111,12 +1145,24 @@ int tty_perform_flush(struct tty_struct *tty, unsigned long arg)
 		tty_driver_flush_buffer(tty);
 		break;
 	default:
-		tty_ldisc_deref(ld);
 		return -EINVAL;
 	}
-	tty_ldisc_deref(ld);
 	return 0;
 }
+
+int tty_perform_flush(struct tty_struct *tty, unsigned long arg)
+{
+	struct tty_ldisc *ld;
+	int retval = tty_check_change(tty);
+	if (retval)
+		return retval;
+
+	ld = tty_ldisc_ref_wait(tty);
+	retval = __tty_perform_flush(tty, arg);
+	if (ld)
+		tty_ldisc_deref(ld);
+	return retval;
+}
 EXPORT_SYMBOL_GPL(tty_perform_flush);
 
 int n_tty_ioctl_helper(struct tty_struct *tty, struct file *file,
@@ -1155,7 +1201,7 @@ int n_tty_ioctl_helper(struct tty_struct *tty, struct file *file,
 		}
 		return 0;
 	case TCFLSH:
-		return tty_perform_flush(tty, arg);
+		return __tty_perform_flush(tty, arg);
 	default:
 		/* Try the mode commands */
 		return tty_mode_ioctl(tty, file, cmd, arg);
diff --git a/drivers/tty/tty_ldisc.c b/drivers/tty/tty_ldisc.c
index d794087c327..1afe192bef6 100644
--- a/drivers/tty/tty_ldisc.c
+++ b/drivers/tty/tty_ldisc.c
@@ -20,6 +20,17 @@
 #include <linux/uaccess.h>
 #include <linux/ratelimit.h>
 
+#undef LDISC_DEBUG_HANGUP
+
+#ifdef LDISC_DEBUG_HANGUP
+#define tty_ldisc_debug(tty, f, args...) ({				       \
+	char __b[64];							       \
+	printk(KERN_DEBUG "%s: %s: " f, __func__, tty_name(tty, __b), ##args); \
+})
+#else
+#define tty_ldisc_debug(tty, f, args...)
+#endif
+
 /*
  *	This guards the refcounted line discipline lists. The lock
  *	must be taken with irqs off because there are hangup path
@@ -31,44 +42,6 @@ static DECLARE_WAIT_QUEUE_HEAD(tty_ldisc_wait);
 /* Line disc dispatch table */
 static struct tty_ldisc_ops *tty_ldiscs[NR_LDISCS];
 
-static inline struct tty_ldisc *get_ldisc(struct tty_ldisc *ld)
-{
-	if (ld)
-		atomic_inc(&ld->users);
-	return ld;
-}
-
-static void put_ldisc(struct tty_ldisc *ld)
-{
-	unsigned long flags;
-
-	if (WARN_ON_ONCE(!ld))
-		return;
-
-	/*
-	 * If this is the last user, free the ldisc, and
-	 * release the ldisc ops.
-	 *
-	 * We really want an "atomic_dec_and_raw_lock_irqsave()",
-	 * but we don't have it, so this does it by hand.
-	 */
-	raw_spin_lock_irqsave(&tty_ldisc_lock, flags);
-	if (atomic_dec_and_test(&ld->users)) {
-		struct tty_ldisc_ops *ldo = ld->ops;
-
-		ldo->refcount--;
-		module_put(ldo->owner);
-		raw_spin_unlock_irqrestore(&tty_ldisc_lock, flags);
-
-		kfree(ld);
-		return;
-	}
-	raw_spin_unlock_irqrestore(&tty_ldisc_lock, flags);
-
-	if (waitqueue_active(&ld->wq_idle))
-		wake_up(&ld->wq_idle);
-}
-
 /**
  *	tty_register_ldisc	-	install a line discipline
  *	@disc: ldisc number
@@ -206,6 +179,29 @@ static struct tty_ldisc *tty_ldisc_get(int disc)
 	return ld;
 }
 
+/**
+ *	tty_ldisc_put		-	release the ldisc
+ *
+ *	Complement of tty_ldisc_get().
+ */
+static inline void tty_ldisc_put(struct tty_ldisc *ld)
+{
+	unsigned long flags;
+
+	if (WARN_ON_ONCE(!ld))
+		return;
+
+	raw_spin_lock_irqsave(&tty_ldisc_lock, flags);
+
+	/* unreleased reader reference(s) will cause this WARN */
+	WARN_ON(!atomic_dec_and_test(&ld->users));
+
+	ld->ops->refcount--;
+	module_put(ld->ops->owner);
+	kfree(ld);
+	raw_spin_unlock_irqrestore(&tty_ldisc_lock, flags);
+}
+
 static void *tty_ldiscs_seq_start(struct seq_file *m, loff_t *pos)
 {
 	return (*pos < NR_LDISCS) ? pos : NULL;
@@ -255,24 +251,6 @@ const struct file_operations tty_ldiscs_proc_fops = {
 };
 
 /**
- *	tty_ldisc_assign	-	set ldisc on a tty
- *	@tty: tty to assign
- *	@ld: line discipline
- *
- *	Install an instance of a line discipline into a tty structure. The
- *	ldisc must have a reference count above zero to ensure it remains.
- *	The tty instance refcount starts at zero.
- *
- *	Locking:
- *		Caller must hold references
- */
-
-static void tty_ldisc_assign(struct tty_struct *tty, struct tty_ldisc *ld)
-{
-	tty->ldisc = ld;
-}
-
-/**
  *	tty_ldisc_try		-	internal helper
  *	@tty: the tty
  *
@@ -289,10 +267,13 @@ static struct tty_ldisc *tty_ldisc_try(struct tty_struct *tty)
 	unsigned long flags;
 	struct tty_ldisc *ld;
 
+	/* FIXME: this allows reference acquire after TTY_LDISC is cleared */
 	raw_spin_lock_irqsave(&tty_ldisc_lock, flags);
 	ld = NULL;
-	if (test_bit(TTY_LDISC, &tty->flags))
-		ld = get_ldisc(tty->ldisc);
+	if (test_bit(TTY_LDISC, &tty->flags) && tty->ldisc) {
+		ld = tty->ldisc;
+		atomic_inc(&ld->users);
+	}
 	raw_spin_unlock_irqrestore(&tty_ldisc_lock, flags);
 	return ld;
 }
@@ -352,14 +333,23 @@ EXPORT_SYMBOL_GPL(tty_ldisc_ref);
 
 void tty_ldisc_deref(struct tty_ldisc *ld)
 {
-	put_ldisc(ld);
-}
-EXPORT_SYMBOL_GPL(tty_ldisc_deref);
+	unsigned long flags;
 
-static inline void tty_ldisc_put(struct tty_ldisc *ld)
-{
-	put_ldisc(ld);
+	if (WARN_ON_ONCE(!ld))
+		return;
+
+	raw_spin_lock_irqsave(&tty_ldisc_lock, flags);
+	/*
+	 * WARNs if one-too-many reader references were released
+	 * - the last reference must be released with tty_ldisc_put
+	 */
+	WARN_ON(atomic_dec_and_test(&ld->users));
+	raw_spin_unlock_irqrestore(&tty_ldisc_lock, flags);
+
+	if (waitqueue_active(&ld->wq_idle))
+		wake_up(&ld->wq_idle);
 }
+EXPORT_SYMBOL_GPL(tty_ldisc_deref);
 
 /**
  *	tty_ldisc_enable	-	allow ldisc use
@@ -373,8 +363,9 @@ static inline void tty_ldisc_put(struct tty_ldisc *ld)
  *	Clearing directly is allowed.
  */
 
-void tty_ldisc_enable(struct tty_struct *tty)
+static void tty_ldisc_enable(struct tty_struct *tty)
 {
+	clear_bit(TTY_LDISC_HALTED, &tty->flags);
 	set_bit(TTY_LDISC, &tty->flags);
 	clear_bit(TTY_LDISC_CHANGING, &tty->flags);
 	wake_up(&tty_ldisc_wait);
@@ -479,7 +470,7 @@ static void tty_ldisc_restore(struct tty_struct *tty, struct tty_ldisc *old)
 	/* There is an outstanding reference here so this is safe */
 	old = tty_ldisc_get(old->ops->num);
 	WARN_ON(IS_ERR(old));
-	tty_ldisc_assign(tty, old);
+	tty->ldisc = old;
 	tty_set_termios_ldisc(tty, old->ops->num);
 	if (tty_ldisc_open(tty, old) < 0) {
 		tty_ldisc_put(old);
@@ -487,7 +478,7 @@ static void tty_ldisc_restore(struct tty_struct *tty, struct tty_ldisc *old)
 		new_ldisc = tty_ldisc_get(N_TTY);
 		if (IS_ERR(new_ldisc))
 			panic("n_tty: get");
-		tty_ldisc_assign(tty, new_ldisc);
+		tty->ldisc = new_ldisc;
 		tty_set_termios_ldisc(tty, N_TTY);
 		r = tty_ldisc_open(tty, new_ldisc);
 		if (r < 0)
@@ -498,52 +489,98 @@ static void tty_ldisc_restore(struct tty_struct *tty, struct tty_ldisc *old)
 }
 
 /**
+ *	tty_ldisc_wait_idle	-	wait for the ldisc to become idle
+ *	@tty: tty to wait for
+ *	@timeout: for how long to wait at most
+ *
+ *	Wait for the line discipline to become idle. The discipline must
+ *	have been halted for this to guarantee it remains idle.
+ */
+static int tty_ldisc_wait_idle(struct tty_struct *tty, long timeout)
+{
+	long ret;
+	ret = wait_event_timeout(tty->ldisc->wq_idle,
+			atomic_read(&tty->ldisc->users) == 1, timeout);
+	return ret > 0 ? 0 : -EBUSY;
+}
+
+/**
  *	tty_ldisc_halt		-	shut down the line discipline
  *	@tty: tty device
+ *	@o_tty: paired pty device (can be NULL)
+ *	@timeout: # of jiffies to wait for ldisc refs to be released
  *
- *	Shut down the line discipline and work queue for this tty device.
- *	The TTY_LDISC flag being cleared ensures no further references can
- *	be obtained while the delayed work queue halt ensures that no more
- *	data is fed to the ldisc.
+ *	Shut down the line discipline and work queue for this tty device and
+ *	its paired pty (if exists). Clearing the TTY_LDISC flag ensures
+ *	no further references can be obtained, while waiting for existing
+ *	references to be released ensures no more data is fed to the ldisc.
  *
  *	You need to do a 'flush_scheduled_work()' (outside the ldisc_mutex)
  *	in order to make sure any currently executing ldisc work is also
  *	flushed.
  */
 
-static int tty_ldisc_halt(struct tty_struct *tty)
+static int tty_ldisc_halt(struct tty_struct *tty, struct tty_struct *o_tty,
+			  long timeout)
 {
+	int retval;
+
 	clear_bit(TTY_LDISC, &tty->flags);
-	return cancel_work_sync(&tty->port->buf.work);
-}
+	if (o_tty)
+		clear_bit(TTY_LDISC, &o_tty->flags);
 
-/**
- *	tty_ldisc_flush_works	-	flush all works of a tty
- *	@tty: tty device to flush works for
- *
- *	Sync flush all works belonging to @tty.
- */
-static void tty_ldisc_flush_works(struct tty_struct *tty)
-{
-	flush_work(&tty->hangup_work);
-	flush_work(&tty->SAK_work);
-	flush_work(&tty->port->buf.work);
+	retval = tty_ldisc_wait_idle(tty, timeout);
+	if (!retval && o_tty)
+		retval = tty_ldisc_wait_idle(o_tty, timeout);
+	if (retval)
+		return retval;
+
+	set_bit(TTY_LDISC_HALTED, &tty->flags);
+	if (o_tty)
+		set_bit(TTY_LDISC_HALTED, &o_tty->flags);
+
+	return 0;
 }
 
 /**
- *	tty_ldisc_wait_idle	-	wait for the ldisc to become idle
- *	@tty: tty to wait for
- *	@timeout: for how long to wait at most
+ *	tty_ldisc_hangup_halt - halt the line discipline for hangup
+ *	@tty: tty being hung up
  *
- *	Wait for the line discipline to become idle. The discipline must
- *	have been halted for this to guarantee it remains idle.
+ *	Shut down the line discipline and work queue for the tty device
+ *	being hungup. Clear the TTY_LDISC flag to ensure no further
+ *	references can be obtained and wait for remaining references to be
+ *	released to ensure no more data is fed to this ldisc.
+ *	Caller must hold legacy and ->ldisc_mutex.
+ *
+ *	NB: tty_set_ldisc() is prevented from changing the ldisc concurrently
+ *	with this function by checking the TTY_HUPPING flag.
  */
-static int tty_ldisc_wait_idle(struct tty_struct *tty, long timeout)
+static bool tty_ldisc_hangup_halt(struct tty_struct *tty)
 {
-	long ret;
-	ret = wait_event_timeout(tty->ldisc->wq_idle,
-			atomic_read(&tty->ldisc->users) == 1, timeout);
-	return ret > 0 ? 0 : -EBUSY;
+	char cur_n[TASK_COMM_LEN], tty_n[64];
+	long timeout = 3 * HZ;
+
+	clear_bit(TTY_LDISC, &tty->flags);
+
+	if (tty->ldisc) {	/* Not yet closed */
+		tty_unlock(tty);
+
+		while (tty_ldisc_wait_idle(tty, timeout) == -EBUSY) {
+			timeout = MAX_SCHEDULE_TIMEOUT;
+			printk_ratelimited(KERN_WARNING
+				"%s: waiting (%s) for %s took too long, but we keep waiting...\n",
+				__func__, get_task_comm(cur_n, current),
+				tty_name(tty, tty_n));
+		}
+
+		set_bit(TTY_LDISC_HALTED, &tty->flags);
+
+		/* must reacquire both locks and preserve lock order */
+		mutex_unlock(&tty->ldisc_mutex);
+		tty_lock(tty);
+		mutex_lock(&tty->ldisc_mutex);
+	}
+	return !!tty->ldisc;
 }
 
 /**
@@ -563,7 +600,6 @@ int tty_set_ldisc(struct tty_struct *tty, int ldisc)
 {
 	int retval;
 	struct tty_ldisc *o_ldisc, *new_ldisc;
-	int work, o_work = 0;
 	struct tty_struct *o_tty;
 
 	new_ldisc = tty_ldisc_get(ldisc);
@@ -589,15 +625,6 @@ int tty_set_ldisc(struct tty_struct *tty, int ldisc)
 		return 0;
 	}
 
-	tty_unlock(tty);
-	/*
-	 *	Problem: What do we do if this blocks ?
-	 *	We could deadlock here
-	 */
-
-	tty_wait_until_sent(tty, 0);
-
-	tty_lock(tty);
 	mutex_lock(&tty->ldisc_mutex);
 
 	/*
@@ -637,20 +664,16 @@ int tty_set_ldisc(struct tty_struct *tty, int ldisc)
 	 *	parallel to the change and re-referencing the tty.
 	 */
 
-	work = tty_ldisc_halt(tty);
-	if (o_tty)
-		o_work = tty_ldisc_halt(o_tty);
+	retval = tty_ldisc_halt(tty, o_tty, 5 * HZ);
 
 	/*
-	 * Wait for ->hangup_work and ->buf.work handlers to terminate.
+	 * Wait for hangup to complete, if pending.
 	 * We must drop the mutex here in case a hangup is also in process.
 	 */
 
 	mutex_unlock(&tty->ldisc_mutex);
 
-	tty_ldisc_flush_works(tty);
-
-	retval = tty_ldisc_wait_idle(tty, 5 * HZ);
+	flush_work(&tty->hangup_work);
 
 	tty_lock(tty);
 	mutex_lock(&tty->ldisc_mutex);
@@ -675,7 +698,7 @@ int tty_set_ldisc(struct tty_struct *tty, int ldisc)
 	tty_ldisc_close(tty, o_ldisc);
 
 	/* Now set up the new line discipline. */
-	tty_ldisc_assign(tty, new_ldisc);
+	tty->ldisc = new_ldisc;
 	tty_set_termios_ldisc(tty, ldisc);
 
 	retval = tty_ldisc_open(tty, new_ldisc);
@@ -705,10 +728,10 @@ enable:
 
 	/* Restart the work queue in case no characters kick it off. Safe if
 	   already running */
-	if (work)
-		schedule_work(&tty->port->buf.work);
-	if (o_work)
+	schedule_work(&tty->port->buf.work);
+	if (o_tty)
 		schedule_work(&o_tty->port->buf.work);
+
 	mutex_unlock(&tty->ldisc_mutex);
 	tty_unlock(tty);
 	return retval;
@@ -749,11 +772,10 @@ static int tty_ldisc_reinit(struct tty_struct *tty, int ldisc)
 
 	tty_ldisc_close(tty, tty->ldisc);
 	tty_ldisc_put(tty->ldisc);
-	tty->ldisc = NULL;
 	/*
 	 *	Switch the line discipline back
 	 */
-	tty_ldisc_assign(tty, ld);
+	tty->ldisc = ld;
 	tty_set_termios_ldisc(tty, ldisc);
 
 	return 0;
@@ -780,6 +802,8 @@ void tty_ldisc_hangup(struct tty_struct *tty)
 	int reset = tty->driver->flags & TTY_DRIVER_RESET_TERMIOS;
 	int err = 0;
 
+	tty_ldisc_debug(tty, "closing ldisc: %p\n", tty->ldisc);
+
 	/*
 	 * FIXME! What are the locking issues here? This may me overdoing
 	 * things... This question is especially important now that we've
@@ -812,40 +836,12 @@ void tty_ldisc_hangup(struct tty_struct *tty)
 	 */
 	mutex_lock(&tty->ldisc_mutex);
 
-	/*
-	 * this is like tty_ldisc_halt, but we need to give up
-	 * the BTM before calling cancel_work_sync, which may
-	 * need to wait for another function taking the BTM
-	 */
-	clear_bit(TTY_LDISC, &tty->flags);
-	tty_unlock(tty);
-	cancel_work_sync(&tty->port->buf.work);
-	mutex_unlock(&tty->ldisc_mutex);
-retry:
-	tty_lock(tty);
-	mutex_lock(&tty->ldisc_mutex);
-
-	/* At this point we have a closed ldisc and we want to
-	   reopen it. We could defer this to the next open but
-	   it means auditing a lot of other paths so this is
-	   a FIXME */
-	if (tty->ldisc) {	/* Not yet closed */
-		if (atomic_read(&tty->ldisc->users) != 1) {
-			char cur_n[TASK_COMM_LEN], tty_n[64];
-			long timeout = 3 * HZ;
-			tty_unlock(tty);
-
-			while (tty_ldisc_wait_idle(tty, timeout) == -EBUSY) {
-				timeout = MAX_SCHEDULE_TIMEOUT;
-				printk_ratelimited(KERN_WARNING
-					"%s: waiting (%s) for %s took too long, but we keep waiting...\n",
-					__func__, get_task_comm(cur_n, current),
-					tty_name(tty, tty_n));
-			}
-			mutex_unlock(&tty->ldisc_mutex);
-			goto retry;
-		}
+	if (tty_ldisc_hangup_halt(tty)) {
 
+		/* At this point we have a halted ldisc; we want to close it and
+		   reopen a new ldisc. We could defer the reopen to the next
+		   open but it means auditing a lot of other paths so this is
+		   a FIXME */
 		if (reset == 0) {
 
 			if (!tty_ldisc_reinit(tty, tty->termios.c_line))
@@ -864,6 +860,8 @@ retry:
 	mutex_unlock(&tty->ldisc_mutex);
 	if (reset)
 		tty_reset_termios(tty);
+
+	tty_ldisc_debug(tty, "re-opened ldisc: %p\n", tty->ldisc);
 }
 
 /**
@@ -899,11 +897,6 @@ int tty_ldisc_setup(struct tty_struct *tty, struct tty_struct *o_tty)
 
 static void tty_ldisc_kill(struct tty_struct *tty)
 {
-	/* There cannot be users from userspace now. But there still might be
-	 * drivers holding a reference via tty_ldisc_ref. Do not steal them the
-	 * ldisc until they are done. */
-	tty_ldisc_wait_idle(tty, MAX_SCHEDULE_TIMEOUT);
-
 	mutex_lock(&tty->ldisc_mutex);
 	/*
 	 * Now kill off the ldisc
@@ -931,18 +924,13 @@ static void tty_ldisc_kill(struct tty_struct *tty)
 void tty_ldisc_release(struct tty_struct *tty, struct tty_struct *o_tty)
 {
 	/*
-	 * Prevent flush_to_ldisc() from rescheduling the work for later.  Then
-	 * kill any delayed work. As this is the final close it does not
-	 * race with the set_ldisc code path.
+	 * Shutdown this line discipline. As this is the final close,
+	 * it does not race with the set_ldisc code path.
 	 */
 
-	tty_ldisc_halt(tty);
-	if (o_tty)
-		tty_ldisc_halt(o_tty);
+	tty_ldisc_debug(tty, "closing ldisc: %p\n", tty->ldisc);
 
-	tty_ldisc_flush_works(tty);
-	if (o_tty)
-		tty_ldisc_flush_works(o_tty);
+	tty_ldisc_halt(tty, o_tty, MAX_SCHEDULE_TIMEOUT);
 
 	tty_lock_pair(tty, o_tty);
 	/* This will need doing differently if we need to lock */
@@ -953,6 +941,8 @@ void tty_ldisc_release(struct tty_struct *tty, struct tty_struct *o_tty)
 	tty_unlock_pair(tty, o_tty);
 	/* And the memory resources remaining (buffers, termios) will be
 	   disposed of when the kref hits zero */
+
+	tty_ldisc_debug(tty, "ldisc closed\n");
 }
 
 /**
@@ -968,7 +958,7 @@ void tty_ldisc_init(struct tty_struct *tty)
 	struct tty_ldisc *ld = tty_ldisc_get(N_TTY);
 	if (IS_ERR(ld))
 		panic("n_tty: init_tty");
-	tty_ldisc_assign(tty, ld);
+	tty->ldisc = ld;
 }
 
 /**
@@ -980,8 +970,8 @@ void tty_ldisc_init(struct tty_struct *tty)
  */
 void tty_ldisc_deinit(struct tty_struct *tty)
 {
-	put_ldisc(tty->ldisc);
-	tty_ldisc_assign(tty, NULL);
+	tty_ldisc_put(tty->ldisc);
+	tty->ldisc = NULL;
 }
 
 void tty_ldisc_begin(void)
diff --git a/drivers/tty/tty_port.c b/drivers/tty/tty_port.c
index b7ff59d3db8..121aeb9393e 100644
--- a/drivers/tty/tty_port.c
+++ b/drivers/tty/tty_port.c
@@ -132,6 +132,7 @@ EXPORT_SYMBOL(tty_port_free_xmit_buf);
  */
 void tty_port_destroy(struct tty_port *port)
 {
+	cancel_work_sync(&port->buf.work);
 	tty_buffer_free_all(port);
 }
 EXPORT_SYMBOL(tty_port_destroy);
@@ -196,12 +197,24 @@ void tty_port_tty_set(struct tty_port *port, struct tty_struct *tty)
 }
 EXPORT_SYMBOL(tty_port_tty_set);
 
-static void tty_port_shutdown(struct tty_port *port)
+static void tty_port_shutdown(struct tty_port *port, struct tty_struct *tty)
 {
 	mutex_lock(&port->mutex);
-	if (port->ops->shutdown && !port->console &&
-		test_and_clear_bit(ASYNCB_INITIALIZED, &port->flags))
+	if (port->console)
+		goto out;
+
+	if (test_and_clear_bit(ASYNCB_INITIALIZED, &port->flags)) {
+		/*
+		 * Drop DTR/RTS if HUPCL is set. This causes any attached
+		 * modem to hang up the line.
+		 */
+		if (tty && C_HUPCL(tty))
+			tty_port_lower_dtr_rts(port);
+
+		if (port->ops->shutdown)
 			port->ops->shutdown(port);
+	}
+out:
 	mutex_unlock(&port->mutex);
 }
 
@@ -215,24 +228,58 @@ static void tty_port_shutdown(struct tty_port *port)
 
 void tty_port_hangup(struct tty_port *port)
 {
+	struct tty_struct *tty;
 	unsigned long flags;
 
 	spin_lock_irqsave(&port->lock, flags);
 	port->count = 0;
 	port->flags &= ~ASYNC_NORMAL_ACTIVE;
-	if (port->tty) {
-		set_bit(TTY_IO_ERROR, &port->tty->flags);
-		tty_kref_put(port->tty);
-	}
+	tty = port->tty;
+	if (tty)
+		set_bit(TTY_IO_ERROR, &tty->flags);
 	port->tty = NULL;
 	spin_unlock_irqrestore(&port->lock, flags);
+	tty_port_shutdown(port, tty);
+	tty_kref_put(tty);
 	wake_up_interruptible(&port->open_wait);
 	wake_up_interruptible(&port->delta_msr_wait);
-	tty_port_shutdown(port);
 }
 EXPORT_SYMBOL(tty_port_hangup);
 
 /**
+ * tty_port_tty_hangup - helper to hang up a tty
+ *
+ * @port: tty port
+ * @check_clocal: hang only ttys with CLOCAL unset?
+ */
+void tty_port_tty_hangup(struct tty_port *port, bool check_clocal)
+{
+	struct tty_struct *tty = tty_port_tty_get(port);
+
+	if (tty && (!check_clocal || !C_CLOCAL(tty))) {
+		tty_hangup(tty);
+		tty_kref_put(tty);
+	}
+}
+EXPORT_SYMBOL_GPL(tty_port_tty_hangup);
+
+/**
+ * tty_port_tty_wakeup - helper to wake up a tty
+ *
+ * @port: tty port
+ */
+void tty_port_tty_wakeup(struct tty_port *port)
+{
+	struct tty_struct *tty = tty_port_tty_get(port);
+
+	if (tty) {
+		tty_wakeup(tty);
+		tty_kref_put(tty);
+	}
+}
+EXPORT_SYMBOL_GPL(tty_port_tty_wakeup);
+
+/**
  *	tty_port_carrier_raised	-	carrier raised check
  *	@port: tty port
  *
@@ -350,7 +397,7 @@ int tty_port_block_til_ready(struct tty_port *port,
 
 	while (1) {
 		/* Indicate we are open */
-		if (tty->termios.c_cflag & CBAUD)
+		if (C_BAUD(tty) && test_bit(ASYNCB_INITIALIZED, &port->flags))
 			tty_port_raise_dtr_rts(port);
 
 		prepare_to_wait(&port->open_wait, &wait, TASK_INTERRUPTIBLE);
@@ -395,6 +442,20 @@ int tty_port_block_til_ready(struct tty_port *port,
 }
 EXPORT_SYMBOL(tty_port_block_til_ready);
 
+static void tty_port_drain_delay(struct tty_port *port, struct tty_struct *tty)
+{
+	unsigned int bps = tty_get_baud_rate(tty);
+	long timeout;
+
+	if (bps > 1200) {
+		timeout = (HZ * 10 * port->drain_delay) / bps;
+		timeout = max_t(long, timeout, HZ / 10);
+	} else {
+		timeout = 2 * HZ;
+	}
+	schedule_timeout_interruptible(timeout);
+}
+
 int tty_port_close_start(struct tty_port *port,
 				struct tty_struct *tty, struct file *filp)
 {
@@ -427,31 +488,19 @@ int tty_port_close_start(struct tty_port *port,
 	set_bit(ASYNCB_CLOSING, &port->flags);
 	tty->closing = 1;
 	spin_unlock_irqrestore(&port->lock, flags);
-	/* Don't block on a stalled port, just pull the chain */
-	if (tty->flow_stopped)
-		tty_driver_flush_buffer(tty);
-	if (test_bit(ASYNCB_INITIALIZED, &port->flags) &&
-			port->closing_wait != ASYNC_CLOSING_WAIT_NONE)
-		tty_wait_until_sent_from_close(tty, port->closing_wait);
-	if (port->drain_delay) {
-		unsigned int bps = tty_get_baud_rate(tty);
-		long timeout;
-
-		if (bps > 1200)
-			timeout = max_t(long,
-				(HZ * 10 * port->drain_delay) / bps, HZ / 10);
-		else
-			timeout = 2 * HZ;
-		schedule_timeout_interruptible(timeout);
+
+	if (test_bit(ASYNCB_INITIALIZED, &port->flags)) {
+		/* Don't block on a stalled port, just pull the chain */
+		if (tty->flow_stopped)
+			tty_driver_flush_buffer(tty);
+		if (port->closing_wait != ASYNC_CLOSING_WAIT_NONE)
+			tty_wait_until_sent_from_close(tty, port->closing_wait);
+		if (port->drain_delay)
+			tty_port_drain_delay(port, tty);
 	}
 	/* Flush the ldisc buffering */
 	tty_ldisc_flush(tty);
 
-	/* Drop DTR/RTS if HUPCL is set. This causes any attached modem to
-	   hang up the line */
-	if (tty->termios.c_cflag & HUPCL)
-		tty_port_lower_dtr_rts(port);
-
 	/* Don't call port->drop for the last reference. Callers will want
 	   to drop the last active reference in ->shutdown() or the tty
 	   shutdown path */
@@ -486,7 +535,7 @@ void tty_port_close(struct tty_port *port, struct tty_struct *tty,
 {
 	if (tty_port_close_start(port, tty, filp) == 0)
 		return;
-	tty_port_shutdown(port);
+	tty_port_shutdown(port, tty);
 	set_bit(TTY_IO_ERROR, &tty->flags);
 	tty_port_close_end(port, tty);
 	tty_port_tty_set(port, NULL);
diff --git a/drivers/tty/vt/consolemap.c b/drivers/tty/vt/consolemap.c
index 248381b3072..2978ca596a7 100644
--- a/drivers/tty/vt/consolemap.c
+++ b/drivers/tty/vt/consolemap.c
@@ -194,8 +194,7 @@ static void set_inverse_transl(struct vc_data *conp, struct uni_pagedir *p, int
 	q = p->inverse_translations[i];
 
 	if (!q) {
-		q = p->inverse_translations[i] = (unsigned char *) 
-			kmalloc(MAX_GLYPH, GFP_KERNEL);
+		q = p->inverse_translations[i] = kmalloc(MAX_GLYPH, GFP_KERNEL);
 		if (!q) return;
 	}
 	memset(q, 0, MAX_GLYPH);
diff --git a/drivers/tty/vt/vc_screen.c b/drivers/tty/vt/vc_screen.c
index e4ca345873c..d7799deacb2 100644
--- a/drivers/tty/vt/vc_screen.c
+++ b/drivers/tty/vt/vc_screen.c
@@ -93,7 +93,7 @@ vcs_poll_data_free(struct vcs_poll_data *poll)
 static struct vcs_poll_data *
 vcs_poll_data_get(struct file *file)
 {
-	struct vcs_poll_data *poll = file->private_data;
+	struct vcs_poll_data *poll = file->private_data, *kill = NULL;
 
 	if (poll)
 		return poll;
@@ -122,10 +122,12 @@ vcs_poll_data_get(struct file *file)
 		file->private_data = poll;
 	} else {
 		/* someone else raced ahead of us */
-		vcs_poll_data_free(poll);
+		kill = poll;
 		poll = file->private_data;
 	}
 	spin_unlock(&file->f_lock);
+	if (kill)
+		vcs_poll_data_free(kill);
 
 	return poll;
 }
diff --git a/drivers/uio/uio.c b/drivers/uio/uio.c
index c8b926291e2..b645c47501b 100644
--- a/drivers/uio/uio.c
+++ b/drivers/uio/uio.c
@@ -374,6 +374,7 @@ static int uio_get_minor(struct uio_device *idev)
 	retval = idr_alloc(&uio_idr, idev, 0, UIO_MAX_DEVICES, GFP_KERNEL);
 	if (retval >= 0) {
 		idev->minor = retval;
+		retval = 0;
 	} else if (retval == -ENOSPC) {
 		dev_err(idev->dev, "too many uio devices\n");
 		retval = -EINVAL;
diff --git a/drivers/usb/Kconfig b/drivers/usb/Kconfig
index 640ae6c6d2d..92e1dc94ecc 100644
--- a/drivers/usb/Kconfig
+++ b/drivers/usb/Kconfig
@@ -122,9 +122,9 @@ config USB
 	  To compile this driver as a module, choose M here: the
 	  module will be called usbcore.
 
-source "drivers/usb/core/Kconfig"
+if USB
 
-source "drivers/usb/dwc3/Kconfig"
+source "drivers/usb/core/Kconfig"
 
 source "drivers/usb/mon/Kconfig"
 
@@ -134,8 +134,6 @@ source "drivers/usb/host/Kconfig"
 
 source "drivers/usb/musb/Kconfig"
 
-source "drivers/usb/chipidea/Kconfig"
-
 source "drivers/usb/renesas_usbhs/Kconfig"
 
 source "drivers/usb/class/Kconfig"
@@ -144,12 +142,19 @@ source "drivers/usb/storage/Kconfig"
 
 source "drivers/usb/image/Kconfig"
 
+endif
+
+source "drivers/usb/dwc3/Kconfig"
+
+source "drivers/usb/chipidea/Kconfig"
+
 comment "USB port drivers"
-	depends on USB
+
+if USB
 
 config USB_USS720
 	tristate "USS720 parport driver"
-	depends on USB && PARPORT
+	depends on PARPORT
 	select PARPORT_NOT_PC
 	---help---
 	  This driver is for USB parallel port adapters that use the Lucent
@@ -180,12 +185,12 @@ source "drivers/usb/serial/Kconfig"
 
 source "drivers/usb/misc/Kconfig"
 
-source "drivers/usb/phy/Kconfig"
-
 source "drivers/usb/atm/Kconfig"
 
-source "drivers/usb/gadget/Kconfig"
+endif # USB
+
+source "drivers/usb/phy/Kconfig"
 
-source "drivers/usb/otg/Kconfig"
+source "drivers/usb/gadget/Kconfig"
 
 endif # USB_SUPPORT
diff --git a/drivers/usb/Makefile b/drivers/usb/Makefile
index 8f5ebced5df..c41feba8d5c 100644
--- a/drivers/usb/Makefile
+++ b/drivers/usb/Makefile
@@ -6,8 +6,6 @@
 
 obj-$(CONFIG_USB)		+= core/
 
-obj-$(CONFIG_USB_OTG_UTILS)	+= otg/
-
 obj-$(CONFIG_USB_DWC3)		+= dwc3/
 
 obj-$(CONFIG_USB_MON)		+= mon/
@@ -46,7 +44,7 @@ obj-$(CONFIG_USB_MICROTEK)	+= image/
 obj-$(CONFIG_USB_SERIAL)	+= serial/
 
 obj-$(CONFIG_USB)		+= misc/
-obj-$(CONFIG_USB_OTG_UTILS)	+= phy/
+obj-$(CONFIG_USB_PHY)		+= phy/
 obj-$(CONFIG_EARLY_PRINTK_DBGP)	+= early/
 
 obj-$(CONFIG_USB_ATM)		+= atm/
diff --git a/drivers/usb/atm/Kconfig b/drivers/usb/atm/Kconfig
index be0b8daac9c..0f922942a07 100644
--- a/drivers/usb/atm/Kconfig
+++ b/drivers/usb/atm/Kconfig
@@ -4,7 +4,7 @@
 
 menuconfig USB_ATM
 	tristate "USB DSL modem support"
-	depends on USB && ATM
+	depends on ATM
 	select CRC32
 	default n
 	help
diff --git a/drivers/usb/atm/usbatm.c b/drivers/usb/atm/usbatm.c
index 35f10bfe15d..d3527dd8b90 100644
--- a/drivers/usb/atm/usbatm.c
+++ b/drivers/usb/atm/usbatm.c
@@ -672,9 +672,6 @@ static int usbatm_atm_send(struct atm_vcc *vcc, struct sk_buff *skb)
 	struct usbatm_control *ctrl = UDSL_SKB(skb);
 	int err;
 
-	vdbg(&instance->usb_intf->dev, "%s called (skb 0x%p, len %u)", __func__,
-	     skb, skb->len);
-
 	/* racy disconnection check - fine */
 	if (!instance || instance->disconnected) {
 #ifdef DEBUG
@@ -684,6 +681,9 @@ static int usbatm_atm_send(struct atm_vcc *vcc, struct sk_buff *skb)
 		goto fail;
 	}
 
+	vdbg(&instance->usb_intf->dev, "%s called (skb 0x%p, len %u)", __func__,
+	     skb, skb->len);
+
 	if (vcc->qos.aal != ATM_AAL5) {
 		atm_rldbg(instance, "%s: unsupported ATM type %d!\n", __func__, vcc->qos.aal);
 		err = -EINVAL;
diff --git a/drivers/usb/chipidea/Makefile b/drivers/usb/chipidea/Makefile
index d92ca325b10..4ab83e98219 100644
--- a/drivers/usb/chipidea/Makefile
+++ b/drivers/usb/chipidea/Makefile
@@ -17,5 +17,5 @@ ifneq ($(CONFIG_PCI),)
 endif
 
 ifneq ($(CONFIG_OF_DEVICE),)
-	obj-$(CONFIG_USB_CHIPIDEA)	+= ci13xxx_imx.o usbmisc_imx6q.o
+	obj-$(CONFIG_USB_CHIPIDEA)	+= ci13xxx_imx.o usbmisc_imx.o
 endif
diff --git a/drivers/usb/chipidea/ci.h b/drivers/usb/chipidea/ci.h
index e25d1263da1..b0a6bce064c 100644
--- a/drivers/usb/chipidea/ci.h
+++ b/drivers/usb/chipidea/ci.h
@@ -21,6 +21,7 @@
 /******************************************************************************
  * DEFINE
  *****************************************************************************/
+#define TD_PAGE_COUNT      5
 #define CI13XXX_PAGE_SIZE  4096ul /* page size for TD's */
 #define ENDPT_MAX          32
 
@@ -129,6 +130,7 @@ struct hw_bank {
  * @vbus_active: is VBUS active
  * @transceiver: pointer to USB PHY, if any
  * @hcd: pointer to usb_hcd for ehci host driver
+ * @debugfs: root dentry for this controller in debugfs
  */
 struct ci13xxx {
 	struct device			*dev;
@@ -139,7 +141,6 @@ struct ci13xxx {
 	enum ci_role			role;
 	bool				is_otg;
 	struct work_struct		work;
-	struct work_struct		vbus_work;
 	struct workqueue_struct		*wq;
 
 	struct dma_pool			*qh_pool;
@@ -165,6 +166,7 @@ struct ci13xxx {
 	bool				global_phy;
 	struct usb_phy			*transceiver;
 	struct usb_hcd			*hcd;
+	struct dentry			*debugfs;
 };
 
 static inline struct ci_role_driver *ci_role(struct ci13xxx *ci)
@@ -234,19 +236,6 @@ enum ci13xxx_regs {
 };
 
 /**
- * ffs_nr: find first (least significant) bit set
- * @x: the word to search
- *
- * This function returns bit number (instead of position)
- */
-static inline int ffs_nr(u32 x)
-{
-	int n = ffs(x);
-
-	return n ? n-1 : 32;
-}
-
-/**
  * hw_read: reads from a hw register
  * @reg:  register index
  * @mask: bitfield mask
@@ -304,7 +293,7 @@ static inline u32 hw_test_and_write(struct ci13xxx *ci, enum ci13xxx_regs reg,
 	u32 val = hw_read(ci, reg, ~0);
 
 	hw_write(ci, reg, mask, data);
-	return (val & mask) >> ffs_nr(mask);
+	return (val & mask) >> __ffs(mask);
 }
 
 int hw_device_reset(struct ci13xxx *ci, u32 mode);
diff --git a/drivers/usb/chipidea/ci13xxx_imx.c b/drivers/usb/chipidea/ci13xxx_imx.c
index 8c291220be7..8faec9dbbb8 100644
--- a/drivers/usb/chipidea/ci13xxx_imx.c
+++ b/drivers/usb/chipidea/ci13xxx_imx.c
@@ -79,6 +79,9 @@ int usbmisc_get_init_data(struct device *dev, struct usbmisc_usb_device *usbdev)
 	if (of_find_property(np, "disable-over-current", NULL))
 		usbdev->disable_oc = 1;
 
+	if (of_find_property(np, "external-vbus-divider", NULL))
+		usbdev->evdo = 1;
+
 	return 0;
 }
 EXPORT_SYMBOL_GPL(usbmisc_get_init_data);
@@ -202,6 +205,15 @@ static int ci13xxx_imx_probe(struct platform_device *pdev)
 		goto err;
 	}
 
+	if (usbmisc_ops && usbmisc_ops->post) {
+		ret = usbmisc_ops->post(&pdev->dev);
+		if (ret) {
+			dev_err(&pdev->dev,
+				"usbmisc post failed, ret=%d\n", ret);
+			goto put_np;
+		}
+	}
+
 	data->ci_pdev = plat_ci;
 	platform_set_drvdata(pdev, data);
 
diff --git a/drivers/usb/chipidea/ci13xxx_imx.h b/drivers/usb/chipidea/ci13xxx_imx.h
index 9cd2e910b1c..550bfa45762 100644
--- a/drivers/usb/chipidea/ci13xxx_imx.h
+++ b/drivers/usb/chipidea/ci13xxx_imx.h
@@ -13,6 +13,8 @@
 struct usbmisc_ops {
 	/* It's called once when probe a usb device */
 	int (*init)(struct device *dev);
+	/* It's called once after adding a usb device */
+	int (*post)(struct device *dev);
 };
 
 struct usbmisc_usb_device {
@@ -20,6 +22,7 @@ struct usbmisc_usb_device {
 	int index;
 
 	unsigned int disable_oc:1; /* over current detect disabled */
+	unsigned int evdo:1; /* set external vbus divider option */
 };
 
 int usbmisc_set_ops(const struct usbmisc_ops *ops);
diff --git a/drivers/usb/chipidea/ci13xxx_pci.c b/drivers/usb/chipidea/ci13xxx_pci.c
index 9b227e39299..4e1fc61b9d9 100644
--- a/drivers/usb/chipidea/ci13xxx_pci.c
+++ b/drivers/usb/chipidea/ci13xxx_pci.c
@@ -23,17 +23,17 @@
 /******************************************************************************
  * PCI block
  *****************************************************************************/
-struct ci13xxx_platform_data pci_platdata = {
+static struct ci13xxx_platform_data pci_platdata = {
 	.name		= UDC_DRIVER_NAME,
 	.capoffset	= DEF_CAPOFFSET,
 };
 
-struct ci13xxx_platform_data langwell_pci_platdata = {
+static struct ci13xxx_platform_data langwell_pci_platdata = {
 	.name		= UDC_DRIVER_NAME,
 	.capoffset	= 0,
 };
 
-struct ci13xxx_platform_data penwell_pci_platdata = {
+static struct ci13xxx_platform_data penwell_pci_platdata = {
 	.name		= UDC_DRIVER_NAME,
 	.capoffset	= 0,
 	.power_budget	= 200,
diff --git a/drivers/usb/chipidea/core.c b/drivers/usb/chipidea/core.c
index 57cae1f897b..450107e5f65 100644
--- a/drivers/usb/chipidea/core.c
+++ b/drivers/usb/chipidea/core.c
@@ -51,15 +51,12 @@
  */
 #include <linux/delay.h>
 #include <linux/device.h>
-#include <linux/dmapool.h>
 #include <linux/dma-mapping.h>
-#include <linux/init.h>
 #include <linux/platform_device.h>
 #include <linux/module.h>
 #include <linux/idr.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
-#include <linux/irq.h>
 #include <linux/kernel.h>
 #include <linux/slab.h>
 #include <linux/pm_runtime.h>
@@ -158,7 +155,7 @@ int hw_port_test_set(struct ci13xxx *ci, u8 mode)
 	if (mode > TEST_MODE_MAX)
 		return -EINVAL;
 
-	hw_write(ci, OP_PORTSC, PORTSC_PTC, mode << ffs_nr(PORTSC_PTC));
+	hw_write(ci, OP_PORTSC, PORTSC_PTC, mode << __ffs(PORTSC_PTC));
 	return 0;
 }
 
@@ -169,7 +166,7 @@ int hw_port_test_set(struct ci13xxx *ci, u8 mode)
  */
 u8 hw_port_test_get(struct ci13xxx *ci)
 {
-	return hw_read(ci, OP_PORTSC, PORTSC_PTC) >> ffs_nr(PORTSC_PTC);
+	return hw_read(ci, OP_PORTSC, PORTSC_PTC) >> __ffs(PORTSC_PTC);
 }
 
 static int hw_device_init(struct ci13xxx *ci, void __iomem *base)
@@ -181,11 +178,11 @@ static int hw_device_init(struct ci13xxx *ci, void __iomem *base)
 
 	ci->hw_bank.cap = ci->hw_bank.abs;
 	ci->hw_bank.cap += ci->platdata->capoffset;
-	ci->hw_bank.op = ci->hw_bank.cap + ioread8(ci->hw_bank.cap);
+	ci->hw_bank.op = ci->hw_bank.cap + (ioread32(ci->hw_bank.cap) & 0xff);
 
 	hw_alloc_regmap(ci, false);
 	reg = hw_read(ci, CAP_HCCPARAMS, HCCPARAMS_LEN) >>
-		ffs_nr(HCCPARAMS_LEN);
+		__ffs(HCCPARAMS_LEN);
 	ci->hw_bank.lpm  = reg;
 	hw_alloc_regmap(ci, !!reg);
 	ci->hw_bank.size = ci->hw_bank.op - ci->hw_bank.abs;
@@ -193,7 +190,7 @@ static int hw_device_init(struct ci13xxx *ci, void __iomem *base)
 	ci->hw_bank.size /= sizeof(u32);
 
 	reg = hw_read(ci, CAP_DCCPARAMS, DCCPARAMS_DEN) >>
-		ffs_nr(DCCPARAMS_DEN);
+		__ffs(DCCPARAMS_DEN);
 	ci->hw_ep_max = reg * 2;   /* cache hw ENDPT_MAX */
 
 	if (ci->hw_ep_max > ENDPT_MAX)
@@ -283,38 +280,6 @@ static void ci_role_work(struct work_struct *work)
 	}
 }
 
-static ssize_t show_role(struct device *dev, struct device_attribute *attr,
-			 char *buf)
-{
-	struct ci13xxx *ci = dev_get_drvdata(dev);
-
-	return sprintf(buf, "%s\n", ci_role(ci)->name);
-}
-
-static ssize_t store_role(struct device *dev, struct device_attribute *attr,
-			  const char *buf, size_t count)
-{
-	struct ci13xxx *ci = dev_get_drvdata(dev);
-	enum ci_role role;
-	int ret;
-
-	for (role = CI_ROLE_HOST; role < CI_ROLE_END; role++)
-		if (ci->roles[role] && !strcmp(buf, ci->roles[role]->name))
-			break;
-
-	if (role == CI_ROLE_END || role == ci->role)
-		return -EINVAL;
-
-	ci_role_stop(ci);
-	ret = ci_role_start(ci, role);
-	if (ret)
-		return ret;
-
-	return count;
-}
-
-static DEVICE_ATTR(role, S_IRUSR | S_IWUSR, show_role, store_role);
-
 static irqreturn_t ci_irq(int irq, void *data)
 {
 	struct ci13xxx *ci = data;
@@ -410,11 +375,9 @@ static int ci_hdrc_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	base = devm_request_and_ioremap(dev, res);
-	if (!base) {
-		dev_err(dev, "can't request and ioremap resource\n");
-		return -ENOMEM;
-	}
+	base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
 
 	ci = devm_kzalloc(dev, sizeof(*ci), GFP_KERNEL);
 	if (!ci) {
@@ -489,17 +452,14 @@ static int ci_hdrc_probe(struct platform_device *pdev)
 	if (ret)
 		goto stop;
 
-	ret = device_create_file(dev, &dev_attr_role);
-	if (ret)
-		goto rm_attr;
-
 	if (ci->is_otg)
 		hw_write(ci, OP_OTGSC, OTGSC_IDIE, OTGSC_IDIE);
 
-	return ret;
+	ret = dbg_create_files(ci);
+	if (!ret)
+		return 0;
 
-rm_attr:
-	device_remove_file(dev, &dev_attr_role);
+	free_irq(ci->irq, ci);
 stop:
 	ci_role_stop(ci);
 rm_wq:
@@ -513,9 +473,9 @@ static int ci_hdrc_remove(struct platform_device *pdev)
 {
 	struct ci13xxx *ci = platform_get_drvdata(pdev);
 
+	dbg_remove_files(ci);
 	flush_workqueue(ci->wq);
 	destroy_workqueue(ci->wq);
-	device_remove_file(ci->dev, &dev_attr_role);
 	free_irq(ci->irq, ci);
 	ci_role_stop(ci);
 
diff --git a/drivers/usb/chipidea/debug.c b/drivers/usb/chipidea/debug.c
index a62c4a47d52..36a7063a6cb 100644
--- a/drivers/usb/chipidea/debug.c
+++ b/drivers/usb/chipidea/debug.c
@@ -1,590 +1,126 @@
-#include <linux/delay.h>
-#include <linux/device.h>
-#include <linux/dmapool.h>
-#include <linux/dma-mapping.h>
-#include <linux/init.h>
-#include <linux/platform_device.h>
-#include <linux/module.h>
-#include <linux/interrupt.h>
-#include <linux/io.h>
-#include <linux/irq.h>
 #include <linux/kernel.h>
-#include <linux/slab.h>
-#include <linux/pm_runtime.h>
+#include <linux/device.h>
+#include <linux/types.h>
+#include <linux/spinlock.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/uaccess.h>
 #include <linux/usb/ch9.h>
 #include <linux/usb/gadget.h>
-#include <linux/usb/otg.h>
-#include <linux/usb/chipidea.h>
 
 #include "ci.h"
 #include "udc.h"
 #include "bits.h"
 #include "debug.h"
 
-/* Interrupt statistics */
-#define ISR_MASK   0x1F
-static struct isr_statistics {
-	u32 test;
-	u32 ui;
-	u32 uei;
-	u32 pci;
-	u32 uri;
-	u32 sli;
-	u32 none;
-	struct {
-		u32 cnt;
-		u32 buf[ISR_MASK+1];
-		u32 idx;
-	} hndl;
-} isr_statistics;
-
-void dbg_interrupt(u32 intmask)
-{
-	if (!intmask) {
-		isr_statistics.none++;
-		return;
-	}
-
-	isr_statistics.hndl.buf[isr_statistics.hndl.idx++] = intmask;
-	isr_statistics.hndl.idx &= ISR_MASK;
-	isr_statistics.hndl.cnt++;
-
-	if (USBi_URI & intmask)
-		isr_statistics.uri++;
-	if (USBi_PCI & intmask)
-		isr_statistics.pci++;
-	if (USBi_UEI & intmask)
-		isr_statistics.uei++;
-	if (USBi_UI  & intmask)
-		isr_statistics.ui++;
-	if (USBi_SLI & intmask)
-		isr_statistics.sli++;
-}
-
 /**
- * hw_register_read: reads all device registers (execute without interruption)
- * @buf:  destination buffer
- * @size: buffer size
- *
- * This function returns number of registers read
+ * ci_device_show: prints information about device capabilities and status
  */
-static size_t hw_register_read(struct ci13xxx *ci, u32 *buf, size_t size)
+static int ci_device_show(struct seq_file *s, void *data)
 {
-	unsigned i;
-
-	if (size > ci->hw_bank.size)
-		size = ci->hw_bank.size;
-
-	for (i = 0; i < size; i++)
-		buf[i] = hw_read(ci, i * sizeof(u32), ~0);
-
-	return size;
-}
-
-/**
- * hw_register_write: writes to register
- * @addr: register address
- * @data: register value
- *
- * This function returns an error code
- */
-static int hw_register_write(struct ci13xxx *ci, u16 addr, u32 data)
-{
-	/* align */
-	addr /= sizeof(u32);
-
-	if (addr >= ci->hw_bank.size)
-		return -EINVAL;
-
-	/* align */
-	addr *= sizeof(u32);
-
-	hw_write(ci, addr, ~0, data);
-	return 0;
-}
-
-/**
- * hw_intr_clear: disables interrupt & clears interrupt status (execute without
- *                interruption)
- * @n: interrupt bit
- *
- * This function returns an error code
- */
-static int hw_intr_clear(struct ci13xxx *ci, int n)
-{
-	if (n >= REG_BITS)
-		return -EINVAL;
-
-	hw_write(ci, OP_USBINTR, BIT(n), 0);
-	hw_write(ci, OP_USBSTS,  BIT(n), BIT(n));
-	return 0;
-}
-
-/**
- * hw_intr_force: enables interrupt & forces interrupt status (execute without
- *                interruption)
- * @n: interrupt bit
- *
- * This function returns an error code
- */
-static int hw_intr_force(struct ci13xxx *ci, int n)
-{
-	if (n >= REG_BITS)
-		return -EINVAL;
-
-	hw_write(ci, CAP_TESTMODE, TESTMODE_FORCE, TESTMODE_FORCE);
-	hw_write(ci, OP_USBINTR,  BIT(n), BIT(n));
-	hw_write(ci, OP_USBSTS,   BIT(n), BIT(n));
-	hw_write(ci, CAP_TESTMODE, TESTMODE_FORCE, 0);
-	return 0;
-}
-
-/**
- * show_device: prints information about device capabilities and status
- *
- * Check "device.h" for details
- */
-static ssize_t show_device(struct device *dev, struct device_attribute *attr,
-			   char *buf)
-{
-	struct ci13xxx *ci = container_of(dev, struct ci13xxx, gadget.dev);
+	struct ci13xxx *ci = s->private;
 	struct usb_gadget *gadget = &ci->gadget;
-	int n = 0;
 
-	if (attr == NULL || buf == NULL) {
-		dev_err(ci->dev, "[%s] EINVAL\n", __func__);
+	seq_printf(s, "speed             = %d\n", gadget->speed);
+	seq_printf(s, "max_speed         = %d\n", gadget->max_speed);
+	seq_printf(s, "is_otg            = %d\n", gadget->is_otg);
+	seq_printf(s, "is_a_peripheral   = %d\n", gadget->is_a_peripheral);
+	seq_printf(s, "b_hnp_enable      = %d\n", gadget->b_hnp_enable);
+	seq_printf(s, "a_hnp_support     = %d\n", gadget->a_hnp_support);
+	seq_printf(s, "a_alt_hnp_support = %d\n", gadget->a_alt_hnp_support);
+	seq_printf(s, "name              = %s\n",
+		   (gadget->name ? gadget->name : ""));
+
+	if (!ci->driver)
 		return 0;
-	}
-
-	n += scnprintf(buf + n, PAGE_SIZE - n, "speed             = %d\n",
-		       gadget->speed);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "max_speed         = %d\n",
-		       gadget->max_speed);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "is_otg            = %d\n",
-		       gadget->is_otg);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "is_a_peripheral   = %d\n",
-		       gadget->is_a_peripheral);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "b_hnp_enable      = %d\n",
-		       gadget->b_hnp_enable);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "a_hnp_support     = %d\n",
-		       gadget->a_hnp_support);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "a_alt_hnp_support = %d\n",
-		       gadget->a_alt_hnp_support);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "name              = %s\n",
-		       (gadget->name ? gadget->name : ""));
-
-	return n;
-}
-static DEVICE_ATTR(device, S_IRUSR, show_device, NULL);
-
-/**
- * show_driver: prints information about attached gadget (if any)
- *
- * Check "device.h" for details
- */
-static ssize_t show_driver(struct device *dev, struct device_attribute *attr,
-			   char *buf)
-{
-	struct ci13xxx *ci = container_of(dev, struct ci13xxx, gadget.dev);
-	struct usb_gadget_driver *driver = ci->driver;
-	int n = 0;
-
-	if (attr == NULL || buf == NULL) {
-		dev_err(dev, "[%s] EINVAL\n", __func__);
-		return 0;
-	}
-
-	if (driver == NULL)
-		return scnprintf(buf, PAGE_SIZE,
-				 "There is no gadget attached!\n");
 
-	n += scnprintf(buf + n, PAGE_SIZE - n, "function  = %s\n",
-		       (driver->function ? driver->function : ""));
-	n += scnprintf(buf + n, PAGE_SIZE - n, "max speed = %d\n",
-		       driver->max_speed);
+	seq_printf(s, "gadget function   = %s\n",
+		       (ci->driver->function ? ci->driver->function : ""));
+	seq_printf(s, "gadget max speed  = %d\n", ci->driver->max_speed);
 
-	return n;
-}
-static DEVICE_ATTR(driver, S_IRUSR, show_driver, NULL);
-
-/* Maximum event message length */
-#define DBG_DATA_MSG   64UL
-
-/* Maximum event messages */
-#define DBG_DATA_MAX   128UL
-
-/* Event buffer descriptor */
-static struct {
-	char     (buf[DBG_DATA_MAX])[DBG_DATA_MSG];   /* buffer */
-	unsigned idx;   /* index */
-	unsigned tty;   /* print to console? */
-	rwlock_t lck;   /* lock */
-} dbg_data = {
-	.idx = 0,
-	.tty = 0,
-	.lck = __RW_LOCK_UNLOCKED(dbg_data.lck)
-};
-
-/**
- * dbg_dec: decrements debug event index
- * @idx: buffer index
- */
-static void dbg_dec(unsigned *idx)
-{
-	*idx = (*idx - 1) & (DBG_DATA_MAX-1);
-}
-
-/**
- * dbg_inc: increments debug event index
- * @idx: buffer index
- */
-static void dbg_inc(unsigned *idx)
-{
-	*idx = (*idx + 1) & (DBG_DATA_MAX-1);
-}
-
-/**
- * dbg_print:  prints the common part of the event
- * @addr:   endpoint address
- * @name:   event name
- * @status: status
- * @extra:  extra information
- */
-static void dbg_print(u8 addr, const char *name, int status, const char *extra)
-{
-	struct timeval tval;
-	unsigned int stamp;
-	unsigned long flags;
-
-	write_lock_irqsave(&dbg_data.lck, flags);
-
-	do_gettimeofday(&tval);
-	stamp = tval.tv_sec & 0xFFFF;	/* 2^32 = 4294967296. Limit to 4096s */
-	stamp = stamp * 1000000 + tval.tv_usec;
-
-	scnprintf(dbg_data.buf[dbg_data.idx], DBG_DATA_MSG,
-		  "%04X\t? %02X %-7.7s %4i ?\t%s\n",
-		  stamp, addr, name, status, extra);
-
-	dbg_inc(&dbg_data.idx);
-
-	write_unlock_irqrestore(&dbg_data.lck, flags);
-
-	if (dbg_data.tty != 0)
-		pr_notice("%04X\t? %02X %-7.7s %4i ?\t%s\n",
-			  stamp, addr, name, status, extra);
-}
-
-/**
- * dbg_done: prints a DONE event
- * @addr:   endpoint address
- * @td:     transfer descriptor
- * @status: status
- */
-void dbg_done(u8 addr, const u32 token, int status)
-{
-	char msg[DBG_DATA_MSG];
-
-	scnprintf(msg, sizeof(msg), "%d %02X",
-		  (int)(token & TD_TOTAL_BYTES) >> ffs_nr(TD_TOTAL_BYTES),
-		  (int)(token & TD_STATUS)      >> ffs_nr(TD_STATUS));
-	dbg_print(addr, "DONE", status, msg);
-}
-
-/**
- * dbg_event: prints a generic event
- * @addr:   endpoint address
- * @name:   event name
- * @status: status
- */
-void dbg_event(u8 addr, const char *name, int status)
-{
-	if (name != NULL)
-		dbg_print(addr, name, status, "");
-}
-
-/*
- * dbg_queue: prints a QUEUE event
- * @addr:   endpoint address
- * @req:    USB request
- * @status: status
- */
-void dbg_queue(u8 addr, const struct usb_request *req, int status)
-{
-	char msg[DBG_DATA_MSG];
-
-	if (req != NULL) {
-		scnprintf(msg, sizeof(msg),
-			  "%d %d", !req->no_interrupt, req->length);
-		dbg_print(addr, "QUEUE", status, msg);
-	}
-}
-
-/**
- * dbg_setup: prints a SETUP event
- * @addr: endpoint address
- * @req:  setup request
- */
-void dbg_setup(u8 addr, const struct usb_ctrlrequest *req)
-{
-	char msg[DBG_DATA_MSG];
-
-	if (req != NULL) {
-		scnprintf(msg, sizeof(msg),
-			  "%02X %02X %04X %04X %d", req->bRequestType,
-			  req->bRequest, le16_to_cpu(req->wValue),
-			  le16_to_cpu(req->wIndex), le16_to_cpu(req->wLength));
-		dbg_print(addr, "SETUP", 0, msg);
-	}
+	return 0;
 }
 
-/**
- * show_events: displays the event buffer
- *
- * Check "device.h" for details
- */
-static ssize_t show_events(struct device *dev, struct device_attribute *attr,
-			   char *buf)
+static int ci_device_open(struct inode *inode, struct file *file)
 {
-	unsigned long flags;
-	unsigned i, j, n = 0;
-
-	if (attr == NULL || buf == NULL) {
-		dev_err(dev->parent, "[%s] EINVAL\n", __func__);
-		return 0;
-	}
-
-	read_lock_irqsave(&dbg_data.lck, flags);
-
-	i = dbg_data.idx;
-	for (dbg_dec(&i); i != dbg_data.idx; dbg_dec(&i)) {
-		n += strlen(dbg_data.buf[i]);
-		if (n >= PAGE_SIZE) {
-			n -= strlen(dbg_data.buf[i]);
-			break;
-		}
-	}
-	for (j = 0, dbg_inc(&i); j < n; dbg_inc(&i))
-		j += scnprintf(buf + j, PAGE_SIZE - j,
-			       "%s", dbg_data.buf[i]);
-
-	read_unlock_irqrestore(&dbg_data.lck, flags);
-
-	return n;
+	return single_open(file, ci_device_show, inode->i_private);
 }
 
-/**
- * store_events: configure if events are going to be also printed to console
- *
- * Check "device.h" for details
- */
-static ssize_t store_events(struct device *dev, struct device_attribute *attr,
-			    const char *buf, size_t count)
-{
-	unsigned tty;
-
-	if (attr == NULL || buf == NULL) {
-		dev_err(dev, "[%s] EINVAL\n", __func__);
-		goto done;
-	}
-
-	if (sscanf(buf, "%u", &tty) != 1 || tty > 1) {
-		dev_err(dev, "<1|0>: enable|disable console log\n");
-		goto done;
-	}
-
-	dbg_data.tty = tty;
-	dev_info(dev, "tty = %u", dbg_data.tty);
-
- done:
-	return count;
-}
-static DEVICE_ATTR(events, S_IRUSR | S_IWUSR, show_events, store_events);
+static const struct file_operations ci_device_fops = {
+	.open		= ci_device_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
 
 /**
- * show_inters: interrupt status, enable status and historic
- *
- * Check "device.h" for details
+ * ci_port_test_show: reads port test mode
  */
-static ssize_t show_inters(struct device *dev, struct device_attribute *attr,
-			   char *buf)
+static int ci_port_test_show(struct seq_file *s, void *data)
 {
-	struct ci13xxx *ci = container_of(dev, struct ci13xxx, gadget.dev);
+	struct ci13xxx *ci = s->private;
 	unsigned long flags;
-	u32 intr;
-	unsigned i, j, n = 0;
-
-	if (attr == NULL || buf == NULL) {
-		dev_err(ci->dev, "[%s] EINVAL\n", __func__);
-		return 0;
-	}
+	unsigned mode;
 
 	spin_lock_irqsave(&ci->lock, flags);
-
-	/*n += scnprintf(buf + n, PAGE_SIZE - n,
-		       "status = %08x\n", hw_read_intr_status(ci));
-	n += scnprintf(buf + n, PAGE_SIZE - n,
-	"enable = %08x\n", hw_read_intr_enable(ci));*/
-
-	n += scnprintf(buf + n, PAGE_SIZE - n, "*test = %d\n",
-		       isr_statistics.test);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "? ui  = %d\n",
-		       isr_statistics.ui);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "? uei = %d\n",
-		       isr_statistics.uei);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "? pci = %d\n",
-		       isr_statistics.pci);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "? uri = %d\n",
-		       isr_statistics.uri);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "? sli = %d\n",
-		       isr_statistics.sli);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "*none = %d\n",
-		       isr_statistics.none);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "*hndl = %d\n",
-		       isr_statistics.hndl.cnt);
-
-	for (i = isr_statistics.hndl.idx, j = 0; j <= ISR_MASK; j++, i++) {
-		i   &= ISR_MASK;
-		intr = isr_statistics.hndl.buf[i];
-
-		if (USBi_UI  & intr)
-			n += scnprintf(buf + n, PAGE_SIZE - n, "ui  ");
-		intr &= ~USBi_UI;
-		if (USBi_UEI & intr)
-			n += scnprintf(buf + n, PAGE_SIZE - n, "uei ");
-		intr &= ~USBi_UEI;
-		if (USBi_PCI & intr)
-			n += scnprintf(buf + n, PAGE_SIZE - n, "pci ");
-		intr &= ~USBi_PCI;
-		if (USBi_URI & intr)
-			n += scnprintf(buf + n, PAGE_SIZE - n, "uri ");
-		intr &= ~USBi_URI;
-		if (USBi_SLI & intr)
-			n += scnprintf(buf + n, PAGE_SIZE - n, "sli ");
-		intr &= ~USBi_SLI;
-		if (intr)
-			n += scnprintf(buf + n, PAGE_SIZE - n, "??? ");
-		if (isr_statistics.hndl.buf[i])
-			n += scnprintf(buf + n, PAGE_SIZE - n, "\n");
-	}
-
+	mode = hw_port_test_get(ci);
 	spin_unlock_irqrestore(&ci->lock, flags);
 
-	return n;
-}
-
-/**
- * store_inters: enable & force or disable an individual interrutps
- *                   (to be used for test purposes only)
- *
- * Check "device.h" for details
- */
-static ssize_t store_inters(struct device *dev, struct device_attribute *attr,
-			    const char *buf, size_t count)
-{
-	struct ci13xxx *ci = container_of(dev, struct ci13xxx, gadget.dev);
-	unsigned long flags;
-	unsigned en, bit;
-
-	if (attr == NULL || buf == NULL) {
-		dev_err(ci->dev, "EINVAL\n");
-		goto done;
-	}
+	seq_printf(s, "mode = %u\n", mode);
 
-	if (sscanf(buf, "%u %u", &en, &bit) != 2 || en > 1) {
-		dev_err(ci->dev, "<1|0> <bit>: enable|disable interrupt\n");
-		goto done;
-	}
-
-	spin_lock_irqsave(&ci->lock, flags);
-	if (en) {
-		if (hw_intr_force(ci, bit))
-			dev_err(dev, "invalid bit number\n");
-		else
-			isr_statistics.test++;
-	} else {
-		if (hw_intr_clear(ci, bit))
-			dev_err(dev, "invalid bit number\n");
-	}
-	spin_unlock_irqrestore(&ci->lock, flags);
-
- done:
-	return count;
+	return 0;
 }
-static DEVICE_ATTR(inters, S_IRUSR | S_IWUSR, show_inters, store_inters);
 
 /**
- * show_port_test: reads port test mode
- *
- * Check "device.h" for details
+ * ci_port_test_write: writes port test mode
  */
-static ssize_t show_port_test(struct device *dev,
-			      struct device_attribute *attr, char *buf)
+static ssize_t ci_port_test_write(struct file *file, const char __user *ubuf,
+				  size_t count, loff_t *ppos)
 {
-	struct ci13xxx *ci = container_of(dev, struct ci13xxx, gadget.dev);
+	struct seq_file *s = file->private_data;
+	struct ci13xxx *ci = s->private;
 	unsigned long flags;
 	unsigned mode;
+	char buf[32];
+	int ret;
 
-	if (attr == NULL || buf == NULL) {
-		dev_err(ci->dev, "EINVAL\n");
-		return 0;
-	}
+	if (copy_from_user(buf, ubuf, min_t(size_t, sizeof(buf) - 1, count)))
+		return -EFAULT;
+
+	if (sscanf(buf, "%u", &mode) != 1)
+		return -EINVAL;
 
 	spin_lock_irqsave(&ci->lock, flags);
-	mode = hw_port_test_get(ci);
+	ret = hw_port_test_set(ci, mode);
 	spin_unlock_irqrestore(&ci->lock, flags);
 
-	return scnprintf(buf, PAGE_SIZE, "mode = %u\n", mode);
+	return ret ? ret : count;
 }
 
-/**
- * store_port_test: writes port test mode
- *
- * Check "device.h" for details
- */
-static ssize_t store_port_test(struct device *dev,
-			       struct device_attribute *attr,
-			       const char *buf, size_t count)
+static int ci_port_test_open(struct inode *inode, struct file *file)
 {
-	struct ci13xxx *ci = container_of(dev, struct ci13xxx, gadget.dev);
-	unsigned long flags;
-	unsigned mode;
-
-	if (attr == NULL || buf == NULL) {
-		dev_err(ci->dev, "[%s] EINVAL\n", __func__);
-		goto done;
-	}
-
-	if (sscanf(buf, "%u", &mode) != 1) {
-		dev_err(ci->dev, "<mode>: set port test mode");
-		goto done;
-	}
-
-	spin_lock_irqsave(&ci->lock, flags);
-	if (hw_port_test_set(ci, mode))
-		dev_err(ci->dev, "invalid mode\n");
-	spin_unlock_irqrestore(&ci->lock, flags);
-
- done:
-	return count;
+	return single_open(file, ci_port_test_show, inode->i_private);
 }
-static DEVICE_ATTR(port_test, S_IRUSR | S_IWUSR,
-		   show_port_test, store_port_test);
+
+static const struct file_operations ci_port_test_fops = {
+	.open		= ci_port_test_open,
+	.write		= ci_port_test_write,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
 
 /**
- * show_qheads: DMA contents of all queue heads
- *
- * Check "device.h" for details
+ * ci_qheads_show: DMA contents of all queue heads
  */
-static ssize_t show_qheads(struct device *dev, struct device_attribute *attr,
-			   char *buf)
+static int ci_qheads_show(struct seq_file *s, void *data)
 {
-	struct ci13xxx *ci = container_of(dev, struct ci13xxx, gadget.dev);
+	struct ci13xxx *ci = s->private;
 	unsigned long flags;
-	unsigned i, j, n = 0;
+	unsigned i, j;
 
-	if (attr == NULL || buf == NULL) {
-		dev_err(ci->dev, "[%s] EINVAL\n", __func__);
+	if (ci->role != CI_ROLE_GADGET) {
+		seq_printf(s, "not in gadget mode\n");
 		return 0;
 	}
 
@@ -593,209 +129,173 @@ static ssize_t show_qheads(struct device *dev, struct device_attribute *attr,
 		struct ci13xxx_ep *mEpRx = &ci->ci13xxx_ep[i];
 		struct ci13xxx_ep *mEpTx =
 			&ci->ci13xxx_ep[i + ci->hw_ep_max/2];
-		n += scnprintf(buf + n, PAGE_SIZE - n,
-			       "EP=%02i: RX=%08X TX=%08X\n",
-			       i, (u32)mEpRx->qh.dma, (u32)mEpTx->qh.dma);
-		for (j = 0; j < (sizeof(struct ci13xxx_qh)/sizeof(u32)); j++) {
-			n += scnprintf(buf + n, PAGE_SIZE - n,
-				       " %04X:    %08X    %08X\n", j,
-				       *((u32 *)mEpRx->qh.ptr + j),
-				       *((u32 *)mEpTx->qh.ptr + j));
-		}
+		seq_printf(s, "EP=%02i: RX=%08X TX=%08X\n",
+			   i, (u32)mEpRx->qh.dma, (u32)mEpTx->qh.dma);
+		for (j = 0; j < (sizeof(struct ci13xxx_qh)/sizeof(u32)); j++)
+			seq_printf(s, " %04X:    %08X    %08X\n", j,
+				   *((u32 *)mEpRx->qh.ptr + j),
+				   *((u32 *)mEpTx->qh.ptr + j));
 	}
 	spin_unlock_irqrestore(&ci->lock, flags);
 
-	return n;
+	return 0;
+}
+
+static int ci_qheads_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ci_qheads_show, inode->i_private);
 }
-static DEVICE_ATTR(qheads, S_IRUSR, show_qheads, NULL);
+
+static const struct file_operations ci_qheads_fops = {
+	.open		= ci_qheads_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
 
 /**
- * show_registers: dumps all registers
- *
- * Check "device.h" for details
+ * ci_requests_show: DMA contents of all requests currently queued (all endpts)
  */
-#define DUMP_ENTRIES	512
-static ssize_t show_registers(struct device *dev,
-			      struct device_attribute *attr, char *buf)
+static int ci_requests_show(struct seq_file *s, void *data)
 {
-	struct ci13xxx *ci = container_of(dev, struct ci13xxx, gadget.dev);
+	struct ci13xxx *ci = s->private;
 	unsigned long flags;
-	u32 *dump;
-	unsigned i, k, n = 0;
-
-	if (attr == NULL || buf == NULL) {
-		dev_err(ci->dev, "[%s] EINVAL\n", __func__);
-		return 0;
-	}
+	struct list_head   *ptr = NULL;
+	struct ci13xxx_req *req = NULL;
+	unsigned i, j, qsize = sizeof(struct ci13xxx_td)/sizeof(u32);
 
-	dump = kmalloc(sizeof(u32) * DUMP_ENTRIES, GFP_KERNEL);
-	if (!dump) {
-		dev_err(ci->dev, "%s: out of memory\n", __func__);
+	if (ci->role != CI_ROLE_GADGET) {
+		seq_printf(s, "not in gadget mode\n");
 		return 0;
 	}
 
 	spin_lock_irqsave(&ci->lock, flags);
-	k = hw_register_read(ci, dump, DUMP_ENTRIES);
-	spin_unlock_irqrestore(&ci->lock, flags);
+	for (i = 0; i < ci->hw_ep_max; i++)
+		list_for_each(ptr, &ci->ci13xxx_ep[i].qh.queue) {
+			req = list_entry(ptr, struct ci13xxx_req, queue);
 
-	for (i = 0; i < k; i++) {
-		n += scnprintf(buf + n, PAGE_SIZE - n,
-			       "reg[0x%04X] = 0x%08X\n",
-			       i * (unsigned)sizeof(u32), dump[i]);
-	}
-	kfree(dump);
+			seq_printf(s, "EP=%02i: TD=%08X %s\n",
+				   i % (ci->hw_ep_max / 2), (u32)req->dma,
+				   ((i < ci->hw_ep_max/2) ? "RX" : "TX"));
+
+			for (j = 0; j < qsize; j++)
+				seq_printf(s, " %04X:    %08X\n", j,
+					   *((u32 *)req->ptr + j));
+		}
+	spin_unlock_irqrestore(&ci->lock, flags);
 
-	return n;
+	return 0;
 }
 
-/**
- * store_registers: writes value to register address
- *
- * Check "device.h" for details
- */
-static ssize_t store_registers(struct device *dev,
-			       struct device_attribute *attr,
-			       const char *buf, size_t count)
+static int ci_requests_open(struct inode *inode, struct file *file)
 {
-	struct ci13xxx *ci = container_of(dev, struct ci13xxx, gadget.dev);
-	unsigned long addr, data, flags;
+	return single_open(file, ci_requests_show, inode->i_private);
+}
 
-	if (attr == NULL || buf == NULL) {
-		dev_err(ci->dev, "[%s] EINVAL\n", __func__);
-		goto done;
-	}
+static const struct file_operations ci_requests_fops = {
+	.open		= ci_requests_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
 
-	if (sscanf(buf, "%li %li", &addr, &data) != 2) {
-		dev_err(ci->dev,
-			"<addr> <data>: write data to register address\n");
-		goto done;
-	}
+static int ci_role_show(struct seq_file *s, void *data)
+{
+	struct ci13xxx *ci = s->private;
 
-	spin_lock_irqsave(&ci->lock, flags);
-	if (hw_register_write(ci, addr, data))
-		dev_err(ci->dev, "invalid address range\n");
-	spin_unlock_irqrestore(&ci->lock, flags);
+	seq_printf(s, "%s\n", ci_role(ci)->name);
 
- done:
-	return count;
+	return 0;
 }
-static DEVICE_ATTR(registers, S_IRUSR | S_IWUSR,
-		   show_registers, store_registers);
 
-/**
- * show_requests: DMA contents of all requests currently queued (all endpts)
- *
- * Check "device.h" for details
- */
-static ssize_t show_requests(struct device *dev, struct device_attribute *attr,
-			     char *buf)
+static ssize_t ci_role_write(struct file *file, const char __user *ubuf,
+			     size_t count, loff_t *ppos)
 {
-	struct ci13xxx *ci = container_of(dev, struct ci13xxx, gadget.dev);
-	unsigned long flags;
-	struct list_head   *ptr = NULL;
-	struct ci13xxx_req *req = NULL;
-	unsigned i, j, n = 0, qSize = sizeof(struct ci13xxx_td)/sizeof(u32);
-
-	if (attr == NULL || buf == NULL) {
-		dev_err(ci->dev, "[%s] EINVAL\n", __func__);
-		return 0;
-	}
+	struct seq_file *s = file->private_data;
+	struct ci13xxx *ci = s->private;
+	enum ci_role role;
+	char buf[8];
+	int ret;
+
+	if (copy_from_user(buf, ubuf, min_t(size_t, sizeof(buf) - 1, count)))
+		return -EFAULT;
+
+	for (role = CI_ROLE_HOST; role < CI_ROLE_END; role++)
+		if (ci->roles[role] &&
+		    !strncmp(buf, ci->roles[role]->name,
+			     strlen(ci->roles[role]->name)))
+			break;
 
-	spin_lock_irqsave(&ci->lock, flags);
-	for (i = 0; i < ci->hw_ep_max; i++)
-		list_for_each(ptr, &ci->ci13xxx_ep[i].qh.queue)
-		{
-			req = list_entry(ptr, struct ci13xxx_req, queue);
+	if (role == CI_ROLE_END || role == ci->role)
+		return -EINVAL;
 
-			n += scnprintf(buf + n, PAGE_SIZE - n,
-					"EP=%02i: TD=%08X %s\n",
-					i % ci->hw_ep_max/2, (u32)req->dma,
-					((i < ci->hw_ep_max/2) ? "RX" : "TX"));
+	ci_role_stop(ci);
+	ret = ci_role_start(ci, role);
 
-			for (j = 0; j < qSize; j++)
-				n += scnprintf(buf + n, PAGE_SIZE - n,
-						" %04X:    %08X\n", j,
-						*((u32 *)req->ptr + j));
-		}
-	spin_unlock_irqrestore(&ci->lock, flags);
+	return ret ? ret : count;
+}
 
-	return n;
+static int ci_role_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ci_role_show, inode->i_private);
 }
-static DEVICE_ATTR(requests, S_IRUSR, show_requests, NULL);
+
+static const struct file_operations ci_role_fops = {
+	.open		= ci_role_open,
+	.write		= ci_role_write,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
 
 /**
  * dbg_create_files: initializes the attribute interface
- * @dev: device
+ * @ci: device
  *
  * This function returns an error code
  */
-int dbg_create_files(struct device *dev)
+int dbg_create_files(struct ci13xxx *ci)
 {
-	int retval = 0;
-
-	if (dev == NULL)
-		return -EINVAL;
-	retval = device_create_file(dev, &dev_attr_device);
-	if (retval)
-		goto done;
-	retval = device_create_file(dev, &dev_attr_driver);
-	if (retval)
-		goto rm_device;
-	retval = device_create_file(dev, &dev_attr_events);
-	if (retval)
-		goto rm_driver;
-	retval = device_create_file(dev, &dev_attr_inters);
-	if (retval)
-		goto rm_events;
-	retval = device_create_file(dev, &dev_attr_port_test);
-	if (retval)
-		goto rm_inters;
-	retval = device_create_file(dev, &dev_attr_qheads);
-	if (retval)
-		goto rm_port_test;
-	retval = device_create_file(dev, &dev_attr_registers);
-	if (retval)
-		goto rm_qheads;
-	retval = device_create_file(dev, &dev_attr_requests);
-	if (retval)
-		goto rm_registers;
-	return 0;
-
- rm_registers:
-	device_remove_file(dev, &dev_attr_registers);
- rm_qheads:
-	device_remove_file(dev, &dev_attr_qheads);
- rm_port_test:
-	device_remove_file(dev, &dev_attr_port_test);
- rm_inters:
-	device_remove_file(dev, &dev_attr_inters);
- rm_events:
-	device_remove_file(dev, &dev_attr_events);
- rm_driver:
-	device_remove_file(dev, &dev_attr_driver);
- rm_device:
-	device_remove_file(dev, &dev_attr_device);
- done:
-	return retval;
+	struct dentry *dent;
+
+	ci->debugfs = debugfs_create_dir(dev_name(ci->dev), NULL);
+	if (!ci->debugfs)
+		return -ENOMEM;
+
+	dent = debugfs_create_file("device", S_IRUGO, ci->debugfs, ci,
+				   &ci_device_fops);
+	if (!dent)
+		goto err;
+
+	dent = debugfs_create_file("port_test", S_IRUGO | S_IWUSR, ci->debugfs,
+				   ci, &ci_port_test_fops);
+	if (!dent)
+		goto err;
+
+	dent = debugfs_create_file("qheads", S_IRUGO, ci->debugfs, ci,
+				   &ci_qheads_fops);
+	if (!dent)
+		goto err;
+
+	dent = debugfs_create_file("requests", S_IRUGO, ci->debugfs, ci,
+				   &ci_requests_fops);
+	if (!dent)
+		goto err;
+
+	dent = debugfs_create_file("role", S_IRUGO | S_IWUSR, ci->debugfs, ci,
+				   &ci_role_fops);
+	if (dent)
+		return 0;
+err:
+	debugfs_remove_recursive(ci->debugfs);
+	return -ENOMEM;
 }
 
 /**
  * dbg_remove_files: destroys the attribute interface
- * @dev: device
- *
- * This function returns an error code
+ * @ci: device
  */
-int dbg_remove_files(struct device *dev)
+void dbg_remove_files(struct ci13xxx *ci)
 {
-	if (dev == NULL)
-		return -EINVAL;
-	device_remove_file(dev, &dev_attr_requests);
-	device_remove_file(dev, &dev_attr_registers);
-	device_remove_file(dev, &dev_attr_qheads);
-	device_remove_file(dev, &dev_attr_port_test);
-	device_remove_file(dev, &dev_attr_inters);
-	device_remove_file(dev, &dev_attr_events);
-	device_remove_file(dev, &dev_attr_driver);
-	device_remove_file(dev, &dev_attr_device);
-	return 0;
+	debugfs_remove_recursive(ci->debugfs);
 }
diff --git a/drivers/usb/chipidea/debug.h b/drivers/usb/chipidea/debug.h
index 80d96865775..7ca6ca0a24a 100644
--- a/drivers/usb/chipidea/debug.h
+++ b/drivers/usb/chipidea/debug.h
@@ -14,42 +14,16 @@
 #define __DRIVERS_USB_CHIPIDEA_DEBUG_H
 
 #ifdef CONFIG_USB_CHIPIDEA_DEBUG
-void dbg_interrupt(u32 intmask);
-void dbg_done(u8 addr, const u32 token, int status);
-void dbg_event(u8 addr, const char *name, int status);
-void dbg_queue(u8 addr, const struct usb_request *req, int status);
-void dbg_setup(u8 addr, const struct usb_ctrlrequest *req);
-int dbg_create_files(struct device *dev);
-int dbg_remove_files(struct device *dev);
+int dbg_create_files(struct ci13xxx *ci);
+void dbg_remove_files(struct ci13xxx *ci);
 #else
-static inline void dbg_interrupt(u32 intmask)
-{
-}
-
-static inline void dbg_done(u8 addr, const u32 token, int status)
-{
-}
-
-static inline void dbg_event(u8 addr, const char *name, int status)
-{
-}
-
-static inline void dbg_queue(u8 addr, const struct usb_request *req, int status)
-{
-}
-
-static inline void dbg_setup(u8 addr, const struct usb_ctrlrequest *req)
-{
-}
-
-static inline int dbg_create_files(struct device *dev)
+static inline int dbg_create_files(struct ci13xxx *ci)
 {
 	return 0;
 }
 
-static inline int dbg_remove_files(struct device *dev)
+static inline void dbg_remove_files(struct ci13xxx *ci)
 {
-	return 0;
 }
 #endif
 
diff --git a/drivers/usb/chipidea/udc.c b/drivers/usb/chipidea/udc.c
index f64fbea1cf2..519ead2443c 100644
--- a/drivers/usb/chipidea/udc.c
+++ b/drivers/usb/chipidea/udc.c
@@ -13,14 +13,8 @@
 #include <linux/delay.h>
 #include <linux/device.h>
 #include <linux/dmapool.h>
-#include <linux/dma-mapping.h>
 #include <linux/err.h>
-#include <linux/init.h>
-#include <linux/platform_device.h>
-#include <linux/module.h>
-#include <linux/interrupt.h>
-#include <linux/io.h>
-#include <linux/irq.h>
+#include <linux/irqreturn.h>
 #include <linux/kernel.h>
 #include <linux/slab.h>
 #include <linux/pm_runtime.h>
@@ -146,7 +140,7 @@ static int hw_ep_enable(struct ci13xxx *ci, int num, int dir, int type)
 
 	if (dir) {
 		mask  = ENDPTCTRL_TXT;  /* type    */
-		data  = type << ffs_nr(mask);
+		data  = type << __ffs(mask);
 
 		mask |= ENDPTCTRL_TXS;  /* unstall */
 		mask |= ENDPTCTRL_TXR;  /* reset data toggle */
@@ -155,7 +149,7 @@ static int hw_ep_enable(struct ci13xxx *ci, int num, int dir, int type)
 		data |= ENDPTCTRL_TXE;
 	} else {
 		mask  = ENDPTCTRL_RXT;  /* type    */
-		data  = type << ffs_nr(mask);
+		data  = type << __ffs(mask);
 
 		mask |= ENDPTCTRL_RXS;  /* unstall */
 		mask |= ENDPTCTRL_RXR;  /* reset data toggle */
@@ -305,18 +299,6 @@ static u32 hw_test_and_clear_intr_active(struct ci13xxx *ci)
 	return reg;
 }
 
-static void hw_enable_vbus_intr(struct ci13xxx *ci)
-{
-	hw_write(ci, OP_OTGSC, OTGSC_AVVIS, OTGSC_AVVIS);
-	hw_write(ci, OP_OTGSC, OTGSC_AVVIE, OTGSC_AVVIE);
-	queue_work(ci->wq, &ci->vbus_work);
-}
-
-static void hw_disable_vbus_intr(struct ci13xxx *ci)
-{
-	hw_write(ci, OP_OTGSC, OTGSC_AVVIE, 0);
-}
-
 /**
  * hw_test_and_clear_setup_guard: test & clear setup guard (execute without
  *                                interruption)
@@ -349,7 +331,7 @@ static int hw_test_and_set_setup_guard(struct ci13xxx *ci)
 static void hw_usb_set_address(struct ci13xxx *ci, u8 value)
 {
 	hw_write(ci, OP_DEVICEADDR, DEVICEADDR_USBADR,
-		 value << ffs_nr(DEVICEADDR_USBADR));
+		 value << __ffs(DEVICEADDR_USBADR));
 }
 
 /**
@@ -383,16 +365,6 @@ static int hw_usb_reset(struct ci13xxx *ci)
 	return 0;
 }
 
-static void vbus_work(struct work_struct *work)
-{
-	struct ci13xxx *ci = container_of(work, struct ci13xxx, vbus_work);
-
-	if (hw_read(ci, OP_OTGSC, OTGSC_AVV))
-		usb_gadget_vbus_connect(&ci->gadget);
-	else
-		usb_gadget_vbus_disconnect(&ci->gadget);
-}
-
 /******************************************************************************
  * UTIL block
  *****************************************************************************/
@@ -432,10 +404,10 @@ static int _hardware_enqueue(struct ci13xxx_ep *mEp, struct ci13xxx_req *mReq)
 			return -ENOMEM;
 
 		memset(mReq->zptr, 0, sizeof(*mReq->zptr));
-		mReq->zptr->next    = TD_TERMINATE;
-		mReq->zptr->token   = TD_STATUS_ACTIVE;
+		mReq->zptr->next    = cpu_to_le32(TD_TERMINATE);
+		mReq->zptr->token   = cpu_to_le32(TD_STATUS_ACTIVE);
 		if (!mReq->req.no_interrupt)
-			mReq->zptr->token   |= TD_IOC;
+			mReq->zptr->token   |= cpu_to_le32(TD_IOC);
 	}
 	ret = usb_gadget_map_request(&ci->gadget, &mReq->req, mEp->dir);
 	if (ret)
@@ -446,32 +418,37 @@ static int _hardware_enqueue(struct ci13xxx_ep *mEp, struct ci13xxx_req *mReq)
 	 * TODO - handle requests which spawns into several TDs
 	 */
 	memset(mReq->ptr, 0, sizeof(*mReq->ptr));
-	mReq->ptr->token    = length << ffs_nr(TD_TOTAL_BYTES);
-	mReq->ptr->token   &= TD_TOTAL_BYTES;
-	mReq->ptr->token   |= TD_STATUS_ACTIVE;
+	mReq->ptr->token    = cpu_to_le32(length << __ffs(TD_TOTAL_BYTES));
+	mReq->ptr->token   &= cpu_to_le32(TD_TOTAL_BYTES);
+	mReq->ptr->token   |= cpu_to_le32(TD_STATUS_ACTIVE);
 	if (mReq->zptr) {
-		mReq->ptr->next    = mReq->zdma;
+		mReq->ptr->next    = cpu_to_le32(mReq->zdma);
 	} else {
-		mReq->ptr->next    = TD_TERMINATE;
+		mReq->ptr->next    = cpu_to_le32(TD_TERMINATE);
 		if (!mReq->req.no_interrupt)
-			mReq->ptr->token  |= TD_IOC;
+			mReq->ptr->token  |= cpu_to_le32(TD_IOC);
+	}
+	mReq->ptr->page[0]  = cpu_to_le32(mReq->req.dma);
+	for (i = 1; i < TD_PAGE_COUNT; i++) {
+		u32 page = mReq->req.dma + i * CI13XXX_PAGE_SIZE;
+		page &= ~TD_RESERVED_MASK;
+		mReq->ptr->page[i] = cpu_to_le32(page);
 	}
-	mReq->ptr->page[0]  = mReq->req.dma;
-	for (i = 1; i < 5; i++)
-		mReq->ptr->page[i] =
-			(mReq->req.dma + i * CI13XXX_PAGE_SIZE) & ~TD_RESERVED_MASK;
+
+	wmb();
 
 	if (!list_empty(&mEp->qh.queue)) {
 		struct ci13xxx_req *mReqPrev;
 		int n = hw_ep_bit(mEp->num, mEp->dir);
 		int tmp_stat;
+		u32 next = mReq->dma & TD_ADDR_MASK;
 
 		mReqPrev = list_entry(mEp->qh.queue.prev,
 				struct ci13xxx_req, queue);
 		if (mReqPrev->zptr)
-			mReqPrev->zptr->next = mReq->dma & TD_ADDR_MASK;
+			mReqPrev->zptr->next = cpu_to_le32(next);
 		else
-			mReqPrev->ptr->next = mReq->dma & TD_ADDR_MASK;
+			mReqPrev->ptr->next = cpu_to_le32(next);
 		wmb();
 		if (hw_read(ci, OP_ENDPTPRIME, BIT(n)))
 			goto done;
@@ -485,9 +462,9 @@ static int _hardware_enqueue(struct ci13xxx_ep *mEp, struct ci13xxx_req *mReq)
 	}
 
 	/*  QH configuration */
-	mEp->qh.ptr->td.next   = mReq->dma;    /* TERMINATE = 0 */
-	mEp->qh.ptr->td.token &= ~TD_STATUS;   /* clear status */
-	mEp->qh.ptr->cap |=  QH_ZLT;
+	mEp->qh.ptr->td.next   = cpu_to_le32(mReq->dma);    /* TERMINATE = 0 */
+	mEp->qh.ptr->td.token &=
+		cpu_to_le32(~(TD_STATUS_HALTED|TD_STATUS_ACTIVE));
 
 	wmb();   /* synchronize before ep prime */
 
@@ -506,14 +483,16 @@ done:
  */
 static int _hardware_dequeue(struct ci13xxx_ep *mEp, struct ci13xxx_req *mReq)
 {
+	u32 tmptoken = le32_to_cpu(mReq->ptr->token);
+
 	if (mReq->req.status != -EALREADY)
 		return -EINVAL;
 
-	if ((TD_STATUS_ACTIVE & mReq->ptr->token) != 0)
+	if ((TD_STATUS_ACTIVE & tmptoken) != 0)
 		return -EBUSY;
 
 	if (mReq->zptr) {
-		if ((TD_STATUS_ACTIVE & mReq->zptr->token) != 0)
+		if ((cpu_to_le32(TD_STATUS_ACTIVE) & mReq->zptr->token) != 0)
 			return -EBUSY;
 		dma_pool_free(mEp->td_pool, mReq->zptr, mReq->zdma);
 		mReq->zptr = NULL;
@@ -523,7 +502,7 @@ static int _hardware_dequeue(struct ci13xxx_ep *mEp, struct ci13xxx_req *mReq)
 
 	usb_gadget_unmap_request(&mEp->ci->gadget, &mReq->req, mEp->dir);
 
-	mReq->req.status = mReq->ptr->token & TD_STATUS;
+	mReq->req.status = tmptoken & TD_STATUS;
 	if ((TD_STATUS_HALTED & mReq->req.status) != 0)
 		mReq->req.status = -1;
 	else if ((TD_STATUS_DT_ERR & mReq->req.status) != 0)
@@ -531,8 +510,8 @@ static int _hardware_dequeue(struct ci13xxx_ep *mEp, struct ci13xxx_req *mReq)
 	else if ((TD_STATUS_TR_ERR & mReq->req.status) != 0)
 		mReq->req.status = -1;
 
-	mReq->req.actual   = mReq->ptr->token & TD_TOTAL_BYTES;
-	mReq->req.actual >>= ffs_nr(TD_TOTAL_BYTES);
+	mReq->req.actual   = tmptoken & TD_TOTAL_BYTES;
+	mReq->req.actual >>= __ffs(TD_TOTAL_BYTES);
 	mReq->req.actual   = mReq->req.length - mReq->req.actual;
 	mReq->req.actual   = mReq->req.status ? 0 : mReq->req.actual;
 
@@ -561,6 +540,12 @@ __acquires(mEp->lock)
 		struct ci13xxx_req *mReq = \
 			list_entry(mEp->qh.queue.next,
 				   struct ci13xxx_req, queue);
+
+		if (mReq->zptr) {
+			dma_pool_free(mEp->td_pool, mReq->zptr, mReq->zdma);
+			mReq->zptr = NULL;
+		}
+
 		list_del_init(&mReq->queue);
 		mReq->req.status = -ESHUTDOWN;
 
@@ -629,8 +614,6 @@ __acquires(ci->lock)
 {
 	int retval;
 
-	dbg_event(0xFF, "BUS RST", 0);
-
 	spin_unlock(&ci->lock);
 	retval = _gadget_stop_activity(&ci->gadget);
 	if (retval)
@@ -668,6 +651,59 @@ static void isr_get_status_complete(struct usb_ep *ep, struct usb_request *req)
 }
 
 /**
+ * _ep_queue: queues (submits) an I/O request to an endpoint
+ *
+ * Caller must hold lock
+ */
+static int _ep_queue(struct usb_ep *ep, struct usb_request *req,
+		    gfp_t __maybe_unused gfp_flags)
+{
+	struct ci13xxx_ep  *mEp  = container_of(ep,  struct ci13xxx_ep, ep);
+	struct ci13xxx_req *mReq = container_of(req, struct ci13xxx_req, req);
+	struct ci13xxx *ci = mEp->ci;
+	int retval = 0;
+
+	if (ep == NULL || req == NULL || mEp->ep.desc == NULL)
+		return -EINVAL;
+
+	if (mEp->type == USB_ENDPOINT_XFER_CONTROL) {
+		if (req->length)
+			mEp = (ci->ep0_dir == RX) ?
+			       ci->ep0out : ci->ep0in;
+		if (!list_empty(&mEp->qh.queue)) {
+			_ep_nuke(mEp);
+			retval = -EOVERFLOW;
+			dev_warn(mEp->ci->dev, "endpoint ctrl %X nuked\n",
+				 _usb_addr(mEp));
+		}
+	}
+
+	/* first nuke then test link, e.g. previous status has not sent */
+	if (!list_empty(&mReq->queue)) {
+		dev_err(mEp->ci->dev, "request already in queue\n");
+		return -EBUSY;
+	}
+
+	if (req->length > (TD_PAGE_COUNT - 1) * CI13XXX_PAGE_SIZE) {
+		dev_err(mEp->ci->dev, "request bigger than one td\n");
+		return -EMSGSIZE;
+	}
+
+	/* push request */
+	mReq->req.status = -EINPROGRESS;
+	mReq->req.actual = 0;
+
+	retval = _hardware_enqueue(mEp, mReq);
+
+	if (retval == -EALREADY)
+		retval = 0;
+	if (!retval)
+		list_add_tail(&mReq->queue, &mEp->qh.queue);
+
+	return retval;
+}
+
+/**
  * isr_get_status_response: get_status request response
  * @ci: ci struct
  * @setup: setup request packet
@@ -714,9 +750,7 @@ __acquires(mEp->lock)
 	}
 	/* else do nothing; reserved for future use */
 
-	spin_unlock(mEp->lock);
-	retval = usb_ep_queue(&mEp->ep, req, gfp_flags);
-	spin_lock(mEp->lock);
+	retval = _ep_queue(&mEp->ep, req, gfp_flags);
 	if (retval)
 		goto err_free_buf;
 
@@ -763,8 +797,6 @@ isr_setup_status_complete(struct usb_ep *ep, struct usb_request *req)
  * This function returns an error code
  */
 static int isr_setup_status_phase(struct ci13xxx *ci)
-__releases(mEp->lock)
-__acquires(mEp->lock)
 {
 	int retval;
 	struct ci13xxx_ep *mEp;
@@ -773,9 +805,7 @@ __acquires(mEp->lock)
 	ci->status->context = ci;
 	ci->status->complete = isr_setup_status_complete;
 
-	spin_unlock(mEp->lock);
-	retval = usb_ep_queue(&mEp->ep, ci->status, GFP_ATOMIC);
-	spin_lock(mEp->lock);
+	retval = _ep_queue(&mEp->ep, ci->status, GFP_ATOMIC);
 
 	return retval;
 }
@@ -801,7 +831,6 @@ __acquires(mEp->lock)
 		if (retval < 0)
 			break;
 		list_del_init(&mReq->queue);
-		dbg_done(_usb_addr(mEp), mReq->ptr->token, retval);
 		if (mReq->req.complete != NULL) {
 			spin_unlock(mEp->lock);
 			if ((mEp->type == USB_ENDPOINT_XFER_CONTROL) &&
@@ -814,8 +843,6 @@ __acquires(mEp->lock)
 
 	if (retval == -EBUSY)
 		retval = 0;
-	if (retval < 0)
-		dbg_event(_usb_addr(mEp), "DONE", retval);
 
 	return retval;
 }
@@ -847,8 +874,6 @@ __acquires(ci->lock)
 				if (err > 0)   /* needs status phase */
 					err = isr_setup_status_phase(ci);
 				if (err < 0) {
-					dbg_event(_usb_addr(mEp),
-						  "ERROR", err);
 					spin_unlock(&ci->lock);
 					if (usb_ep_set_halt(&mEp->ep))
 						dev_err(ci->dev,
@@ -884,8 +909,6 @@ __acquires(ci->lock)
 
 		ci->ep0_dir = (type & USB_DIR_IN) ? TX : RX;
 
-		dbg_setup(_usb_addr(mEp), &req);
-
 		switch (req.bRequest) {
 		case USB_REQ_CLEAR_FEATURE:
 			if (type == (USB_DIR_OUT|USB_RECIP_ENDPOINT) &&
@@ -997,8 +1020,6 @@ delegate:
 		}
 
 		if (err < 0) {
-			dbg_event(_usb_addr(mEp), "ERROR", err);
-
 			spin_unlock(&ci->lock);
 			if (usb_ep_set_halt(&mEp->ep))
 				dev_err(ci->dev, "error: ep_set_halt\n");
@@ -1021,6 +1042,7 @@ static int ep_enable(struct usb_ep *ep,
 	struct ci13xxx_ep *mEp = container_of(ep, struct ci13xxx_ep, ep);
 	int retval = 0;
 	unsigned long flags;
+	u32 cap = 0;
 
 	if (ep == NULL || desc == NULL)
 		return -EINVAL;
@@ -1040,20 +1062,15 @@ static int ep_enable(struct usb_ep *ep,
 
 	mEp->ep.maxpacket = usb_endpoint_maxp(desc);
 
-	dbg_event(_usb_addr(mEp), "ENABLE", 0);
-
-	mEp->qh.ptr->cap = 0;
-
 	if (mEp->type == USB_ENDPOINT_XFER_CONTROL)
-		mEp->qh.ptr->cap |=  QH_IOS;
-	else if (mEp->type == USB_ENDPOINT_XFER_ISOC)
-		mEp->qh.ptr->cap &= ~QH_MULT;
-	else
-		mEp->qh.ptr->cap &= ~QH_ZLT;
+		cap |= QH_IOS;
+	if (mEp->num)
+		cap |= QH_ZLT;
+	cap |= (mEp->ep.maxpacket << __ffs(QH_MAX_PKT)) & QH_MAX_PKT;
+
+	mEp->qh.ptr->cap = cpu_to_le32(cap);
 
-	mEp->qh.ptr->cap |=
-		(mEp->ep.maxpacket << ffs_nr(QH_MAX_PKT)) & QH_MAX_PKT;
-	mEp->qh.ptr->td.next |= TD_TERMINATE;   /* needed? */
+	mEp->qh.ptr->td.next |= cpu_to_le32(TD_TERMINATE);   /* needed? */
 
 	/*
 	 * Enable endpoints in the HW other than ep0 as ep0
@@ -1088,8 +1105,6 @@ static int ep_disable(struct usb_ep *ep)
 
 	direction = mEp->dir;
 	do {
-		dbg_event(_usb_addr(mEp), "DISABLE", 0);
-
 		retval |= _ep_nuke(mEp);
 		retval |= hw_ep_disable(mEp->ci, mEp->num, mEp->dir);
 
@@ -1129,8 +1144,6 @@ static struct usb_request *ep_alloc_request(struct usb_ep *ep, gfp_t gfp_flags)
 		}
 	}
 
-	dbg_event(_usb_addr(mEp), "ALLOC", mReq == NULL);
-
 	return (mReq == NULL) ? NULL : &mReq->req;
 }
 
@@ -1158,8 +1171,6 @@ static void ep_free_request(struct usb_ep *ep, struct usb_request *req)
 		dma_pool_free(mEp->td_pool, mReq->ptr, mReq->dma);
 	kfree(mReq);
 
-	dbg_event(_usb_addr(mEp), "FREE", 0);
-
 	spin_unlock_irqrestore(mEp->lock, flags);
 }
 
@@ -1172,8 +1183,6 @@ static int ep_queue(struct usb_ep *ep, struct usb_request *req,
 		    gfp_t __maybe_unused gfp_flags)
 {
 	struct ci13xxx_ep  *mEp  = container_of(ep,  struct ci13xxx_ep, ep);
-	struct ci13xxx_req *mReq = container_of(req, struct ci13xxx_req, req);
-	struct ci13xxx *ci = mEp->ci;
 	int retval = 0;
 	unsigned long flags;
 
@@ -1181,48 +1190,7 @@ static int ep_queue(struct usb_ep *ep, struct usb_request *req,
 		return -EINVAL;
 
 	spin_lock_irqsave(mEp->lock, flags);
-
-	if (mEp->type == USB_ENDPOINT_XFER_CONTROL) {
-		if (req->length)
-			mEp = (ci->ep0_dir == RX) ?
-			       ci->ep0out : ci->ep0in;
-		if (!list_empty(&mEp->qh.queue)) {
-			_ep_nuke(mEp);
-			retval = -EOVERFLOW;
-			dev_warn(mEp->ci->dev, "endpoint ctrl %X nuked\n",
-				 _usb_addr(mEp));
-		}
-	}
-
-	/* first nuke then test link, e.g. previous status has not sent */
-	if (!list_empty(&mReq->queue)) {
-		retval = -EBUSY;
-		dev_err(mEp->ci->dev, "request already in queue\n");
-		goto done;
-	}
-
-	if (req->length > 4 * CI13XXX_PAGE_SIZE) {
-		req->length = 4 * CI13XXX_PAGE_SIZE;
-		retval = -EMSGSIZE;
-		dev_warn(mEp->ci->dev, "request length truncated\n");
-	}
-
-	dbg_queue(_usb_addr(mEp), req, retval);
-
-	/* push request */
-	mReq->req.status = -EINPROGRESS;
-	mReq->req.actual = 0;
-
-	retval = _hardware_enqueue(mEp, mReq);
-
-	if (retval == -EALREADY) {
-		dbg_event(_usb_addr(mEp), "QUEUE", retval);
-		retval = 0;
-	}
-	if (!retval)
-		list_add_tail(&mReq->queue, &mEp->qh.queue);
-
- done:
+	retval = _ep_queue(ep, req, gfp_flags);
 	spin_unlock_irqrestore(mEp->lock, flags);
 	return retval;
 }
@@ -1245,8 +1213,6 @@ static int ep_dequeue(struct usb_ep *ep, struct usb_request *req)
 
 	spin_lock_irqsave(mEp->lock, flags);
 
-	dbg_event(_usb_addr(mEp), "DEQUEUE", 0);
-
 	hw_ep_flush(mEp->ci, mEp->num, mEp->dir);
 
 	/* pop request */
@@ -1293,7 +1259,6 @@ static int ep_set_halt(struct usb_ep *ep, int value)
 
 	direction = mEp->dir;
 	do {
-		dbg_event(_usb_addr(mEp), "HALT", value);
 		retval |= hw_ep_set_halt(mEp->ci, mEp->num, mEp->dir, value);
 
 		if (!value)
@@ -1322,10 +1287,7 @@ static int ep_set_wedge(struct usb_ep *ep)
 		return -EINVAL;
 
 	spin_lock_irqsave(mEp->lock, flags);
-
-	dbg_event(_usb_addr(mEp), "WEDGE", 0);
 	mEp->wedge = 1;
-
 	spin_unlock_irqrestore(mEp->lock, flags);
 
 	return usb_ep_set_halt(ep);
@@ -1348,7 +1310,6 @@ static void ep_fifo_flush(struct usb_ep *ep)
 
 	spin_lock_irqsave(mEp->lock, flags);
 
-	dbg_event(_usb_addr(mEp), "FFLUSH", 0);
 	hw_ep_flush(mEp->ci, mEp->num, mEp->dir);
 
 	spin_unlock_irqrestore(mEp->lock, flags);
@@ -1392,7 +1353,6 @@ static int ci13xxx_vbus_session(struct usb_gadget *_gadget, int is_active)
 		if (is_active) {
 			pm_runtime_get_sync(&_gadget->dev);
 			hw_device_reset(ci, USBMODE_CM_DC);
-			hw_enable_vbus_intr(ci);
 			hw_device_state(ci, ci->ep0out->qh.dma);
 		} else {
 			hw_device_state(ci, 0);
@@ -1567,10 +1527,8 @@ static int ci13xxx_start(struct usb_gadget *gadget,
 	pm_runtime_get_sync(&ci->gadget.dev);
 	if (ci->platdata->flags & CI13XXX_PULLUP_ON_VBUS) {
 		if (ci->vbus_active) {
-			if (ci->platdata->flags & CI13XXX_REGS_SHARED) {
+			if (ci->platdata->flags & CI13XXX_REGS_SHARED)
 				hw_device_reset(ci, USBMODE_CM_DC);
-				hw_enable_vbus_intr(ci);
-			}
 		} else {
 			pm_runtime_put_sync(&ci->gadget.dev);
 			goto done;
@@ -1642,7 +1600,6 @@ static irqreturn_t udc_irq(struct ci13xxx *ci)
 		}
 	}
 	intr = hw_test_and_clear_intr_active(ci);
-	dbg_interrupt(intr);
 
 	if (intr) {
 		/* order defines priority - do NOT change it */
@@ -1676,29 +1633,12 @@ static irqreturn_t udc_irq(struct ci13xxx *ci)
 	} else {
 		retval = IRQ_NONE;
 	}
-
-	intr = hw_read(ci, OP_OTGSC, ~0);
-	hw_write(ci, OP_OTGSC, ~0, intr);
-
-	if (intr & (OTGSC_AVVIE & OTGSC_AVVIS))
-		queue_work(ci->wq, &ci->vbus_work);
-
 	spin_unlock(&ci->lock);
 
 	return retval;
 }
 
 /**
- * udc_release: driver release function
- * @dev: device
- *
- * Currently does nothing
- */
-static void udc_release(struct device *dev)
-{
-}
-
-/**
  * udc_start: initialize gadget role
  * @ci: chipidea controller
  */
@@ -1717,12 +1657,6 @@ static int udc_start(struct ci13xxx *ci)
 
 	INIT_LIST_HEAD(&ci->gadget.ep_list);
 
-	dev_set_name(&ci->gadget.dev, "gadget");
-	ci->gadget.dev.dma_mask = dev->dma_mask;
-	ci->gadget.dev.coherent_dma_mask = dev->coherent_dma_mask;
-	ci->gadget.dev.parent   = dev;
-	ci->gadget.dev.release  = udc_release;
-
 	/* alloc resources */
 	ci->qh_pool = dma_pool_create("ci13xxx_qh", dev,
 				       sizeof(struct ci13xxx_qh),
@@ -1758,24 +1692,13 @@ static int udc_start(struct ci13xxx *ci)
 		retval = hw_device_reset(ci, USBMODE_CM_DC);
 		if (retval)
 			goto put_transceiver;
-		hw_enable_vbus_intr(ci);
-	}
-
-	retval = device_register(&ci->gadget.dev);
-	if (retval) {
-		put_device(&ci->gadget.dev);
-		goto put_transceiver;
 	}
 
-	retval = dbg_create_files(ci->dev);
-	if (retval)
-		goto unreg_device;
-
 	if (!IS_ERR_OR_NULL(ci->transceiver)) {
 		retval = otg_set_peripheral(ci->transceiver->otg,
 						&ci->gadget);
 		if (retval)
-			goto remove_dbg;
+			goto put_transceiver;
 	}
 
 	retval = usb_add_gadget_udc(dev, &ci->gadget);
@@ -1795,10 +1718,6 @@ remove_trans:
 	}
 
 	dev_err(dev, "error = %i\n", retval);
-remove_dbg:
-	dbg_remove_files(ci->dev);
-unreg_device:
-	device_unregister(&ci->gadget.dev);
 put_transceiver:
 	if (!IS_ERR_OR_NULL(ci->transceiver) && ci->global_phy)
 		usb_put_phy(ci->transceiver);
@@ -1821,9 +1740,6 @@ static void udc_stop(struct ci13xxx *ci)
 	if (ci == NULL)
 		return;
 
-	hw_disable_vbus_intr(ci);
-	cancel_work_sync(&ci->vbus_work);
-
 	usb_del_gadget_udc(&ci->gadget);
 
 	destroy_eps(ci);
@@ -1836,8 +1752,6 @@ static void udc_stop(struct ci13xxx *ci)
 		if (ci->global_phy)
 			usb_put_phy(ci->transceiver);
 	}
-	dbg_remove_files(ci->dev);
-	device_unregister(&ci->gadget.dev);
 	/* my kobject is dynamic, I swear! */
 	memset(&ci->gadget, 0, sizeof(ci->gadget));
 }
@@ -1864,7 +1778,6 @@ int ci_hdrc_gadget_init(struct ci13xxx *ci)
 	rdrv->irq	= udc_irq;
 	rdrv->name	= "gadget";
 	ci->roles[CI_ROLE_GADGET] = rdrv;
-	INIT_WORK(&ci->vbus_work, vbus_work);
 
 	return 0;
 }
diff --git a/drivers/usb/chipidea/udc.h b/drivers/usb/chipidea/udc.h
index 4ff2384d7ca..d12e8b59b11 100644
--- a/drivers/usb/chipidea/udc.h
+++ b/drivers/usb/chipidea/udc.h
@@ -40,7 +40,7 @@ struct ci13xxx_td {
 #define TD_CURR_OFFSET        (0x0FFFUL <<  0)
 #define TD_FRAME_NUM          (0x07FFUL <<  0)
 #define TD_RESERVED_MASK      (0x0FFFUL <<  0)
-} __attribute__ ((packed));
+} __attribute__ ((packed, aligned(4)));
 
 /* DMA layout of queue heads */
 struct ci13xxx_qh {
@@ -57,7 +57,7 @@ struct ci13xxx_qh {
 	/* 9 */
 	u32 RESERVED;
 	struct usb_ctrlrequest   setup;
-} __attribute__ ((packed));
+} __attribute__ ((packed, aligned(4)));
 
 /**
  * struct ci13xxx_req - usb request representation
diff --git a/drivers/usb/chipidea/usbmisc_imx.c b/drivers/usb/chipidea/usbmisc_imx.c
new file mode 100644
index 00000000000..714a6bd810e
--- /dev/null
+++ b/drivers/usb/chipidea/usbmisc_imx.c
@@ -0,0 +1,261 @@
+/*
+ * Copyright 2012 Freescale Semiconductor, Inc.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/module.h>
+#include <linux/of_platform.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+
+#include "ci13xxx_imx.h"
+
+#define USB_DEV_MAX 4
+
+#define MX25_USB_PHY_CTRL_OFFSET	0x08
+#define MX25_BM_EXTERNAL_VBUS_DIVIDER	BIT(23)
+
+#define MX53_USB_OTG_PHY_CTRL_0_OFFSET	0x08
+#define MX53_USB_UH2_CTRL_OFFSET	0x14
+#define MX53_USB_UH3_CTRL_OFFSET	0x18
+#define MX53_BM_OVER_CUR_DIS_H1		BIT(5)
+#define MX53_BM_OVER_CUR_DIS_OTG	BIT(8)
+#define MX53_BM_OVER_CUR_DIS_UHx	BIT(30)
+
+#define MX6_BM_OVER_CUR_DIS		BIT(7)
+
+struct imx_usbmisc {
+	void __iomem *base;
+	spinlock_t lock;
+	struct clk *clk;
+	struct usbmisc_usb_device usbdev[USB_DEV_MAX];
+	const struct usbmisc_ops *ops;
+};
+
+static struct imx_usbmisc *usbmisc;
+
+static struct usbmisc_usb_device *get_usbdev(struct device *dev)
+{
+	int i, ret;
+
+	for (i = 0; i < USB_DEV_MAX; i++) {
+		if (usbmisc->usbdev[i].dev == dev)
+			return &usbmisc->usbdev[i];
+		else if (!usbmisc->usbdev[i].dev)
+			break;
+	}
+
+	if (i >= USB_DEV_MAX)
+		return ERR_PTR(-EBUSY);
+
+	ret = usbmisc_get_init_data(dev, &usbmisc->usbdev[i]);
+	if (ret)
+		return ERR_PTR(ret);
+
+	return &usbmisc->usbdev[i];
+}
+
+static int usbmisc_imx25_post(struct device *dev)
+{
+	struct usbmisc_usb_device *usbdev;
+	void __iomem *reg;
+	unsigned long flags;
+	u32 val;
+
+	usbdev = get_usbdev(dev);
+	if (IS_ERR(usbdev))
+		return PTR_ERR(usbdev);
+
+	reg = usbmisc->base + MX25_USB_PHY_CTRL_OFFSET;
+
+	if (usbdev->evdo) {
+		spin_lock_irqsave(&usbmisc->lock, flags);
+		val = readl(reg);
+		writel(val | MX25_BM_EXTERNAL_VBUS_DIVIDER, reg);
+		spin_unlock_irqrestore(&usbmisc->lock, flags);
+		usleep_range(5000, 10000); /* needed to stabilize voltage */
+	}
+
+	return 0;
+}
+
+static int usbmisc_imx53_init(struct device *dev)
+{
+	struct usbmisc_usb_device *usbdev;
+	void __iomem *reg = NULL;
+	unsigned long flags;
+	u32 val = 0;
+
+	usbdev = get_usbdev(dev);
+	if (IS_ERR(usbdev))
+		return PTR_ERR(usbdev);
+
+	if (usbdev->disable_oc) {
+		spin_lock_irqsave(&usbmisc->lock, flags);
+		switch (usbdev->index) {
+		case 0:
+			reg = usbmisc->base + MX53_USB_OTG_PHY_CTRL_0_OFFSET;
+			val = readl(reg) | MX53_BM_OVER_CUR_DIS_OTG;
+			break;
+		case 1:
+			reg = usbmisc->base + MX53_USB_OTG_PHY_CTRL_0_OFFSET;
+			val = readl(reg) | MX53_BM_OVER_CUR_DIS_H1;
+			break;
+		case 2:
+			reg = usbmisc->base + MX53_USB_UH2_CTRL_OFFSET;
+			val = readl(reg) | MX53_BM_OVER_CUR_DIS_UHx;
+			break;
+		case 3:
+			reg = usbmisc->base + MX53_USB_UH3_CTRL_OFFSET;
+			val = readl(reg) | MX53_BM_OVER_CUR_DIS_UHx;
+			break;
+		}
+		if (reg && val)
+			writel(val, reg);
+		spin_unlock_irqrestore(&usbmisc->lock, flags);
+	}
+
+	return 0;
+}
+
+static int usbmisc_imx6q_init(struct device *dev)
+{
+
+	struct usbmisc_usb_device *usbdev;
+	unsigned long flags;
+	u32 reg;
+
+	usbdev = get_usbdev(dev);
+	if (IS_ERR(usbdev))
+		return PTR_ERR(usbdev);
+
+	if (usbdev->disable_oc) {
+		spin_lock_irqsave(&usbmisc->lock, flags);
+		reg = readl(usbmisc->base + usbdev->index * 4);
+		writel(reg | MX6_BM_OVER_CUR_DIS,
+			usbmisc->base + usbdev->index * 4);
+		spin_unlock_irqrestore(&usbmisc->lock, flags);
+	}
+
+	return 0;
+}
+
+static const struct usbmisc_ops imx25_usbmisc_ops = {
+	.post = usbmisc_imx25_post,
+};
+
+static const struct usbmisc_ops imx53_usbmisc_ops = {
+	.init = usbmisc_imx53_init,
+};
+
+static const struct usbmisc_ops imx6q_usbmisc_ops = {
+	.init = usbmisc_imx6q_init,
+};
+
+static const struct of_device_id usbmisc_imx_dt_ids[] = {
+	{
+		.compatible = "fsl,imx25-usbmisc",
+		.data = &imx25_usbmisc_ops,
+	},
+	{
+		.compatible = "fsl,imx53-usbmisc",
+		.data = &imx53_usbmisc_ops,
+	},
+	{
+		.compatible = "fsl,imx6q-usbmisc",
+		.data = &imx6q_usbmisc_ops,
+	},
+	{ /* sentinel */ }
+};
+
+static int usbmisc_imx_probe(struct platform_device *pdev)
+{
+	struct resource	*res;
+	struct imx_usbmisc *data;
+	int ret;
+	struct of_device_id *tmp_dev;
+
+	if (usbmisc)
+		return -EBUSY;
+
+	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	spin_lock_init(&data->lock);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	data->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(data->base))
+		return PTR_ERR(data->base);
+
+	data->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(data->clk)) {
+		dev_err(&pdev->dev,
+			"failed to get clock, err=%ld\n", PTR_ERR(data->clk));
+		return PTR_ERR(data->clk);
+	}
+
+	ret = clk_prepare_enable(data->clk);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"clk_prepare_enable failed, err=%d\n", ret);
+		return ret;
+	}
+
+	tmp_dev = (struct of_device_id *)
+		of_match_device(usbmisc_imx_dt_ids, &pdev->dev);
+	data->ops = (const struct usbmisc_ops *)tmp_dev->data;
+	usbmisc = data;
+	ret = usbmisc_set_ops(data->ops);
+	if (ret) {
+		usbmisc = NULL;
+		clk_disable_unprepare(data->clk);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int usbmisc_imx_remove(struct platform_device *pdev)
+{
+	usbmisc_unset_ops(usbmisc->ops);
+	clk_disable_unprepare(usbmisc->clk);
+	usbmisc = NULL;
+	return 0;
+}
+
+static struct platform_driver usbmisc_imx_driver = {
+	.probe = usbmisc_imx_probe,
+	.remove = usbmisc_imx_remove,
+	.driver = {
+		.name = "usbmisc_imx",
+		.owner = THIS_MODULE,
+		.of_match_table = usbmisc_imx_dt_ids,
+	 },
+};
+
+int usbmisc_imx_drv_init(void)
+{
+	return platform_driver_register(&usbmisc_imx_driver);
+}
+subsys_initcall(usbmisc_imx_drv_init);
+
+void usbmisc_imx_drv_exit(void)
+{
+	platform_driver_unregister(&usbmisc_imx_driver);
+}
+module_exit(usbmisc_imx_drv_exit);
+
+MODULE_ALIAS("platform:usbmisc-imx");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("driver for imx usb non-core registers");
+MODULE_AUTHOR("Richard Zhao <richard.zhao@freescale.com>");
diff --git a/drivers/usb/chipidea/usbmisc_imx6q.c b/drivers/usb/chipidea/usbmisc_imx6q.c
deleted file mode 100644
index a1bce391e82..00000000000
--- a/drivers/usb/chipidea/usbmisc_imx6q.c
+++ /dev/null
@@ -1,162 +0,0 @@
-/*
- * Copyright 2012 Freescale Semiconductor, Inc.
- *
- * The code contained herein is licensed under the GNU General Public
- * License. You may obtain a copy of the GNU General Public License
- * Version 2 or later at the following locations:
- *
- * http://www.opensource.org/licenses/gpl-license.html
- * http://www.gnu.org/copyleft/gpl.html
- */
-
-#include <linux/module.h>
-#include <linux/of_platform.h>
-#include <linux/clk.h>
-#include <linux/err.h>
-#include <linux/io.h>
-
-#include "ci13xxx_imx.h"
-
-#define USB_DEV_MAX 4
-
-#define BM_OVER_CUR_DIS		BIT(7)
-
-struct imx6q_usbmisc {
-	void __iomem *base;
-	spinlock_t lock;
-	struct clk *clk;
-	struct usbmisc_usb_device usbdev[USB_DEV_MAX];
-};
-
-static struct imx6q_usbmisc *usbmisc;
-
-static struct usbmisc_usb_device *get_usbdev(struct device *dev)
-{
-	int i, ret;
-
-	for (i = 0; i < USB_DEV_MAX; i++) {
-		if (usbmisc->usbdev[i].dev == dev)
-			return &usbmisc->usbdev[i];
-		else if (!usbmisc->usbdev[i].dev)
-			break;
-	}
-
-	if (i >= USB_DEV_MAX)
-		return ERR_PTR(-EBUSY);
-
-	ret = usbmisc_get_init_data(dev, &usbmisc->usbdev[i]);
-	if (ret)
-		return ERR_PTR(ret);
-
-	return &usbmisc->usbdev[i];
-}
-
-static int usbmisc_imx6q_init(struct device *dev)
-{
-
-	struct usbmisc_usb_device *usbdev;
-	unsigned long flags;
-	u32 reg;
-
-	usbdev = get_usbdev(dev);
-	if (IS_ERR(usbdev))
-		return PTR_ERR(usbdev);
-
-	if (usbdev->disable_oc) {
-		spin_lock_irqsave(&usbmisc->lock, flags);
-		reg = readl(usbmisc->base + usbdev->index * 4);
-		writel(reg | BM_OVER_CUR_DIS,
-			usbmisc->base + usbdev->index * 4);
-		spin_unlock_irqrestore(&usbmisc->lock, flags);
-	}
-
-	return 0;
-}
-
-static const struct usbmisc_ops imx6q_usbmisc_ops = {
-	.init = usbmisc_imx6q_init,
-};
-
-static const struct of_device_id usbmisc_imx6q_dt_ids[] = {
-	{ .compatible = "fsl,imx6q-usbmisc"},
-	{ /* sentinel */ }
-};
-
-static int usbmisc_imx6q_probe(struct platform_device *pdev)
-{
-	struct resource	*res;
-	struct imx6q_usbmisc *data;
-	int ret;
-
-	if (usbmisc)
-		return -EBUSY;
-
-	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
-	if (!data)
-		return -ENOMEM;
-
-	spin_lock_init(&data->lock);
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	data->base = devm_ioremap_resource(&pdev->dev, res);
-	if (IS_ERR(data->base))
-		return PTR_ERR(data->base);
-
-	data->clk = devm_clk_get(&pdev->dev, NULL);
-	if (IS_ERR(data->clk)) {
-		dev_err(&pdev->dev,
-			"failed to get clock, err=%ld\n", PTR_ERR(data->clk));
-		return PTR_ERR(data->clk);
-	}
-
-	ret = clk_prepare_enable(data->clk);
-	if (ret) {
-		dev_err(&pdev->dev,
-			"clk_prepare_enable failed, err=%d\n", ret);
-		return ret;
-	}
-
-	ret = usbmisc_set_ops(&imx6q_usbmisc_ops);
-	if (ret) {
-		clk_disable_unprepare(data->clk);
-		return ret;
-	}
-
-	usbmisc = data;
-
-	return 0;
-}
-
-static int usbmisc_imx6q_remove(struct platform_device *pdev)
-{
-	usbmisc_unset_ops(&imx6q_usbmisc_ops);
-	clk_disable_unprepare(usbmisc->clk);
-	return 0;
-}
-
-static struct platform_driver usbmisc_imx6q_driver = {
-	.probe = usbmisc_imx6q_probe,
-	.remove = usbmisc_imx6q_remove,
-	.driver = {
-		.name = "usbmisc_imx6q",
-		.owner = THIS_MODULE,
-		.of_match_table = usbmisc_imx6q_dt_ids,
-	 },
-};
-
-int __init usbmisc_imx6q_drv_init(void)
-{
-	return platform_driver_register(&usbmisc_imx6q_driver);
-}
-subsys_initcall(usbmisc_imx6q_drv_init);
-
-void __exit usbmisc_imx6q_drv_exit(void)
-{
-	platform_driver_unregister(&usbmisc_imx6q_driver);
-}
-module_exit(usbmisc_imx6q_drv_exit);
-
-MODULE_ALIAS("platform:usbmisc-imx6q");
-MODULE_LICENSE("GPL v2");
-MODULE_DESCRIPTION("driver for imx6q usb non-core registers");
-MODULE_AUTHOR("Richard Zhao <richard.zhao@freescale.com>");
diff --git a/drivers/usb/class/Kconfig b/drivers/usb/class/Kconfig
index 316aac8e4ca..bb8b73682a7 100644
--- a/drivers/usb/class/Kconfig
+++ b/drivers/usb/class/Kconfig
@@ -2,11 +2,10 @@
 # USB Class driver configuration
 #
 comment "USB Device Class drivers"
-	depends on USB
 
 config USB_ACM
 	tristate "USB Modem (CDC ACM) support"
-	depends on USB && TTY
+	depends on TTY
 	---help---
 	  This driver supports USB modems and ISDN adapters which support the
 	  Communication Device Class Abstract Control Model interface.
@@ -21,7 +20,6 @@ config USB_ACM
 
 config USB_PRINTER
 	tristate "USB Printer support"
-	depends on USB
 	help
 	  Say Y here if you want to connect a USB printer to your computer's
 	  USB port.
@@ -31,7 +29,6 @@ config USB_PRINTER
 
 config USB_WDM
 	tristate "USB Wireless Device Management support"
-	depends on USB
 	---help---
 	  This driver supports the WMC Device Management functionality
 	  of cell phones compliant to the CDC WMC specification. You can use
@@ -42,7 +39,6 @@ config USB_WDM
 
 config USB_TMC
 	tristate "USB Test and Measurement Class support"
-	depends on USB
 	help
 	  Say Y here if you want to connect a USB device that follows
 	  the USB.org specification for USB Test and Measurement devices
diff --git a/drivers/usb/class/cdc-acm.c b/drivers/usb/class/cdc-acm.c
index 387dc6c8ad2..171d7a9df3a 100644
--- a/drivers/usb/class/cdc-acm.c
+++ b/drivers/usb/class/cdc-acm.c
@@ -292,7 +292,6 @@ static void acm_ctrl_irq(struct urb *urb)
 {
 	struct acm *acm = urb->context;
 	struct usb_cdc_notification *dr = urb->transfer_buffer;
-	struct tty_struct *tty;
 	unsigned char *data;
 	int newctrl;
 	int retval;
@@ -327,17 +326,12 @@ static void acm_ctrl_irq(struct urb *urb)
 		break;
 
 	case USB_CDC_NOTIFY_SERIAL_STATE:
-		tty = tty_port_tty_get(&acm->port);
 		newctrl = get_unaligned_le16(data);
 
-		if (tty) {
-			if (!acm->clocal &&
-				(acm->ctrlin & ~newctrl & ACM_CTRL_DCD)) {
-				dev_dbg(&acm->control->dev,
-					"%s - calling hangup\n", __func__);
-				tty_hangup(tty);
-			}
-			tty_kref_put(tty);
+		if (!acm->clocal && (acm->ctrlin & ~newctrl & ACM_CTRL_DCD)) {
+			dev_dbg(&acm->control->dev, "%s - calling hangup\n",
+					__func__);
+			tty_port_tty_hangup(&acm->port, false);
 		}
 
 		acm->ctrlin = newctrl;
@@ -475,15 +469,10 @@ static void acm_write_bulk(struct urb *urb)
 static void acm_softint(struct work_struct *work)
 {
 	struct acm *acm = container_of(work, struct acm, work);
-	struct tty_struct *tty;
 
 	dev_vdbg(&acm->data->dev, "%s\n", __func__);
 
-	tty = tty_port_tty_get(&acm->port);
-	if (!tty)
-		return;
-	tty_wakeup(tty);
-	tty_kref_put(tty);
+	tty_port_tty_wakeup(&acm->port);
 }
 
 /*
@@ -839,14 +828,6 @@ static int acm_tty_ioctl(struct tty_struct *tty,
 	return rv;
 }
 
-static const __u32 acm_tty_speed[] = {
-	0, 50, 75, 110, 134, 150, 200, 300, 600,
-	1200, 1800, 2400, 4800, 9600, 19200, 38400,
-	57600, 115200, 230400, 460800, 500000, 576000,
-	921600, 1000000, 1152000, 1500000, 2000000,
-	2500000, 3000000, 3500000, 4000000
-};
-
 static void acm_tty_set_termios(struct tty_struct *tty,
 						struct ktermios *termios_old)
 {
@@ -1519,15 +1500,9 @@ err_out:
 static int acm_reset_resume(struct usb_interface *intf)
 {
 	struct acm *acm = usb_get_intfdata(intf);
-	struct tty_struct *tty;
 
-	if (test_bit(ASYNCB_INITIALIZED, &acm->port.flags)) {
-		tty = tty_port_tty_get(&acm->port);
-		if (tty) {
-			tty_hangup(tty);
-			tty_kref_put(tty);
-		}
-	}
+	if (test_bit(ASYNCB_INITIALIZED, &acm->port.flags))
+		tty_port_tty_hangup(&acm->port, false);
 
 	return acm_resume(intf);
 }
diff --git a/drivers/usb/class/cdc-wdm.c b/drivers/usb/class/cdc-wdm.c
index 122d056d96d..8a230f0ef77 100644
--- a/drivers/usb/class/cdc-wdm.c
+++ b/drivers/usb/class/cdc-wdm.c
@@ -13,6 +13,7 @@
  */
 #include <linux/kernel.h>
 #include <linux/errno.h>
+#include <linux/ioctl.h>
 #include <linux/slab.h>
 #include <linux/module.h>
 #include <linux/mutex.h>
@@ -644,6 +645,22 @@ static int wdm_release(struct inode *inode, struct file *file)
 	return 0;
 }
 
+static long wdm_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	struct wdm_device *desc = file->private_data;
+	int rv = 0;
+
+	switch (cmd) {
+	case IOCTL_WDM_MAX_COMMAND:
+		if (copy_to_user((void __user *)arg, &desc->wMaxCommand, sizeof(desc->wMaxCommand)))
+			rv = -EFAULT;
+		break;
+	default:
+		rv = -ENOTTY;
+	}
+	return rv;
+}
+
 static const struct file_operations wdm_fops = {
 	.owner =	THIS_MODULE,
 	.read =		wdm_read,
@@ -652,6 +669,8 @@ static const struct file_operations wdm_fops = {
 	.flush =	wdm_flush,
 	.release =	wdm_release,
 	.poll =		wdm_poll,
+	.unlocked_ioctl = wdm_ioctl,
+	.compat_ioctl = wdm_ioctl,
 	.llseek =	noop_llseek,
 };
 
diff --git a/drivers/usb/class/usbtmc.c b/drivers/usb/class/usbtmc.c
index 70d69d06054..4c5506ae5e4 100644
--- a/drivers/usb/class/usbtmc.c
+++ b/drivers/usb/class/usbtmc.c
@@ -718,50 +718,32 @@ exit:
 
 static int usbtmc_ioctl_clear_out_halt(struct usbtmc_device_data *data)
 {
-	u8 *buffer;
 	int rv;
 
-	buffer = kmalloc(2, GFP_KERNEL);
-	if (!buffer)
-		return -ENOMEM;
-
 	rv = usb_clear_halt(data->usb_dev,
 			    usb_sndbulkpipe(data->usb_dev, data->bulk_out));
 
 	if (rv < 0) {
 		dev_err(&data->usb_dev->dev, "usb_control_msg returned %d\n",
 			rv);
-		goto exit;
+		return rv;
 	}
-	rv = 0;
-
-exit:
-	kfree(buffer);
-	return rv;
+	return 0;
 }
 
 static int usbtmc_ioctl_clear_in_halt(struct usbtmc_device_data *data)
 {
-	u8 *buffer;
 	int rv;
 
-	buffer = kmalloc(2, GFP_KERNEL);
-	if (!buffer)
-		return -ENOMEM;
-
 	rv = usb_clear_halt(data->usb_dev,
 			    usb_rcvbulkpipe(data->usb_dev, data->bulk_in));
 
 	if (rv < 0) {
 		dev_err(&data->usb_dev->dev, "usb_control_msg returned %d\n",
 			rv);
-		goto exit;
+		return rv;
 	}
-	rv = 0;
-
-exit:
-	kfree(buffer);
-	return rv;
+	return 0;
 }
 
 static int get_capabilities(struct usbtmc_device_data *data)
diff --git a/drivers/usb/core/Kconfig b/drivers/usb/core/Kconfig
index f70c1a1694a..8772b365929 100644
--- a/drivers/usb/core/Kconfig
+++ b/drivers/usb/core/Kconfig
@@ -3,7 +3,6 @@
 #
 config USB_DEBUG
 	bool "USB verbose debug messages"
-	depends on USB
 	help
 	  Say Y here if you want the USB core & hub drivers to produce a bunch
 	  of debug messages to the system log. Select this if you are having a
@@ -11,7 +10,6 @@ config USB_DEBUG
 
 config USB_ANNOUNCE_NEW_DEVICES
 	bool "USB announce new devices"
-	depends on USB
 	default N
 	help
 	  Say Y here if you want the USB core to always announce the
@@ -25,11 +23,24 @@ config USB_ANNOUNCE_NEW_DEVICES
 	  log, or have any doubts about this, say N here.
 
 comment "Miscellaneous USB options"
-	depends on USB
+
+config USB_DEFAULT_PERSIST
+	bool "Enable USB persist by default"
+	default y
+	help
+	  Say N here if you don't want USB power session persistance
+	  enabled by default.  If you say N it will make suspended USB
+	  devices that lose power get reenumerated as if they had been
+	  unplugged, causing any mounted filesystems to be lost.  The
+	  persist feature can still be enabled for individual devices
+	  through the power/persist sysfs node. See
+	  Documentation/usb/persist.txt for more info.
+
+	  If you have any questions about this, say Y here, only say N
+	  if you know exactly what you are doing.
 
 config USB_DYNAMIC_MINORS
 	bool "Dynamic USB minor allocation"
-	depends on USB
 	help
 	  If you say Y here, the USB subsystem will use dynamic minor
 	  allocation for any device that uses the USB major number.
@@ -38,25 +49,8 @@ config USB_DYNAMIC_MINORS
 
 	  If you are unsure about this, say N here.
 
-config USB_SUSPEND
-	bool "USB runtime power management (autosuspend) and wakeup"
-	depends on USB && PM_RUNTIME
-	help
-	  If you say Y here, you can use driver calls or the sysfs
-	  "power/control" file to enable or disable autosuspend for
-	  individual USB peripherals (see
-	  Documentation/usb/power-management.txt for more details).
-
-	  Also, USB "remote wakeup" signaling is supported, whereby some
-	  USB devices (like keyboards and network adapters) can wake up
-	  their parent hub.  That wakeup cascades up the USB tree, and
-	  could wake the system from states like suspend-to-RAM.
-
-	  If you are unsure about this, say N here.
-
 config USB_OTG
 	bool "OTG support"
-	depends on USB
 	depends on USB_SUSPEND
 	default n
 	help
diff --git a/drivers/usb/core/devio.c b/drivers/usb/core/devio.c
index 8823e98989f..caefc800f29 100644
--- a/drivers/usb/core/devio.c
+++ b/drivers/usb/core/devio.c
@@ -739,6 +739,8 @@ static int check_ctrlrecip(struct dev_state *ps, unsigned int requesttype,
 	index &= 0xff;
 	switch (requesttype & USB_RECIP_MASK) {
 	case USB_RECIP_ENDPOINT:
+		if ((index & ~USB_DIR_IN) == 0)
+			return 0;
 		ret = findintfep(ps->dev, index);
 		if (ret >= 0)
 			ret = checkintf(ps, ret);
diff --git a/drivers/usb/core/driver.c b/drivers/usb/core/driver.c
index d938b2b99e3..6eab440e154 100644
--- a/drivers/usb/core/driver.c
+++ b/drivers/usb/core/driver.c
@@ -1196,9 +1196,14 @@ done:
  *
  * This is the central routine for suspending USB devices.  It calls the
  * suspend methods for all the interface drivers in @udev and then calls
- * the suspend method for @udev itself.  If an error occurs at any stage,
- * all the interfaces which were suspended are resumed so that they remain
- * in the same state as the device.
+ * the suspend method for @udev itself.  When the routine is called in
+ * autosuspend, if an error occurs at any stage, all the interfaces
+ * which were suspended are resumed so that they remain in the same
+ * state as the device, but when called from system sleep, all error
+ * from suspend methods of interfaces and the non-root-hub device itself
+ * are simply ignored, so all suspended interfaces are only resumed
+ * to the device's state when @udev is root-hub and its suspend method
+ * returns failure.
  *
  * Autosuspend requests originating from a child device or an interface
  * driver may be made without the protection of @udev's device lock, but
@@ -1248,10 +1253,12 @@ static int usb_suspend_both(struct usb_device *udev, pm_message_t msg)
 
 	/* If the suspend failed, resume interfaces that did get suspended */
 	if (status != 0) {
-		msg.event ^= (PM_EVENT_SUSPEND | PM_EVENT_RESUME);
-		while (++i < n) {
-			intf = udev->actconfig->interface[i];
-			usb_resume_interface(udev, intf, msg, 0);
+		if (udev->actconfig) {
+			msg.event ^= (PM_EVENT_SUSPEND | PM_EVENT_RESUME);
+			while (++i < n) {
+				intf = udev->actconfig->interface[i];
+				usb_resume_interface(udev, intf, msg, 0);
+			}
 		}
 
 	/* If the suspend succeeded then prevent any more URB submissions
@@ -1407,7 +1414,7 @@ int usb_resume(struct device *dev, pm_message_t msg)
 
 #endif /* CONFIG_PM */
 
-#ifdef CONFIG_USB_SUSPEND
+#ifdef CONFIG_PM_RUNTIME
 
 /**
  * usb_enable_autosuspend - allow a USB device to be autosuspended
@@ -1775,7 +1782,7 @@ int usb_set_usb2_hardware_lpm(struct usb_device *udev, int enable)
 	return ret;
 }
 
-#endif /* CONFIG_USB_SUSPEND */
+#endif /* CONFIG_PM_RUNTIME */
 
 struct bus_type usb_bus_type = {
 	.name =		"usb",
diff --git a/drivers/usb/core/generic.c b/drivers/usb/core/generic.c
index 271e761f563..acbfeb0a011 100644
--- a/drivers/usb/core/generic.c
+++ b/drivers/usb/core/generic.c
@@ -169,7 +169,7 @@ static int generic_probe(struct usb_device *udev)
 		c = usb_choose_configuration(udev);
 		if (c >= 0) {
 			err = usb_set_configuration(udev, c);
-			if (err) {
+			if (err && err != -ENODEV) {
 				dev_err(&udev->dev, "can't set config #%d, error %d\n",
 					c, err);
 				/* This need not be fatal.  The user can try to
diff --git a/drivers/usb/core/hcd-pci.c b/drivers/usb/core/hcd-pci.c
index 2b487d4797b..caeb8d6d39f 100644
--- a/drivers/usb/core/hcd-pci.c
+++ b/drivers/usb/core/hcd-pci.c
@@ -37,119 +37,123 @@
 
 /* PCI-based HCs are common, but plenty of non-PCI HCs are used too */
 
-#ifdef CONFIG_PM_SLEEP
-
-/* Coordinate handoffs between EHCI and companion controllers
- * during system resume
+/*
+ * Coordinate handoffs between EHCI and companion controllers
+ * during EHCI probing and system resume.
  */
 
-static DEFINE_MUTEX(companions_mutex);
+static DECLARE_RWSEM(companions_rwsem);
 
 #define CL_UHCI		PCI_CLASS_SERIAL_USB_UHCI
 #define CL_OHCI		PCI_CLASS_SERIAL_USB_OHCI
 #define CL_EHCI		PCI_CLASS_SERIAL_USB_EHCI
 
-enum companion_action {
-	SET_HS_COMPANION, CLEAR_HS_COMPANION, WAIT_FOR_COMPANIONS
-};
+static inline int is_ohci_or_uhci(struct pci_dev *pdev)
+{
+	return pdev->class == CL_OHCI || pdev->class == CL_UHCI;
+}
+
+typedef void (*companion_fn)(struct pci_dev *pdev, struct usb_hcd *hcd,
+		struct pci_dev *companion, struct usb_hcd *companion_hcd);
 
-static void companion_common(struct pci_dev *pdev, struct usb_hcd *hcd,
-		enum companion_action action)
+/* Iterate over PCI devices in the same slot as pdev and call fn for each */
+static void for_each_companion(struct pci_dev *pdev, struct usb_hcd *hcd,
+		companion_fn fn)
 {
 	struct pci_dev		*companion;
 	struct usb_hcd		*companion_hcd;
 	unsigned int		slot = PCI_SLOT(pdev->devfn);
 
-	/* Iterate through other PCI functions in the same slot.
-	 * If pdev is OHCI or UHCI then we are looking for EHCI, and
-	 * vice versa.
+	/*
+	 * Iterate through other PCI functions in the same slot.
+	 * If the function's drvdata isn't set then it isn't bound to
+	 * a USB host controller driver, so skip it.
 	 */
 	companion = NULL;
 	for_each_pci_dev(companion) {
 		if (companion->bus != pdev->bus ||
 				PCI_SLOT(companion->devfn) != slot)
 			continue;
-
 		companion_hcd = pci_get_drvdata(companion);
 		if (!companion_hcd)
 			continue;
-
-		/* For SET_HS_COMPANION, store a pointer to the EHCI bus in
-		 * the OHCI/UHCI companion bus structure.
-		 * For CLEAR_HS_COMPANION, clear the pointer to the EHCI bus
-		 * in the OHCI/UHCI companion bus structure.
-		 * For WAIT_FOR_COMPANIONS, wait until the OHCI/UHCI
-		 * companion controllers have fully resumed.
-		 */
-
-		if ((pdev->class == CL_OHCI || pdev->class == CL_UHCI) &&
-				companion->class == CL_EHCI) {
-			/* action must be SET_HS_COMPANION */
-			dev_dbg(&companion->dev, "HS companion for %s\n",
-					dev_name(&pdev->dev));
-			hcd->self.hs_companion = &companion_hcd->self;
-
-		} else if (pdev->class == CL_EHCI &&
-				(companion->class == CL_OHCI ||
-				companion->class == CL_UHCI)) {
-			switch (action) {
-			case SET_HS_COMPANION:
-				dev_dbg(&pdev->dev, "HS companion for %s\n",
-						dev_name(&companion->dev));
-				companion_hcd->self.hs_companion = &hcd->self;
-				break;
-			case CLEAR_HS_COMPANION:
-				companion_hcd->self.hs_companion = NULL;
-				break;
-			case WAIT_FOR_COMPANIONS:
-				device_pm_wait_for_dev(&pdev->dev,
-						&companion->dev);
-				break;
-			}
-		}
+		fn(pdev, hcd, companion, companion_hcd);
 	}
 }
 
-static void set_hs_companion(struct pci_dev *pdev, struct usb_hcd *hcd)
+/*
+ * We're about to add an EHCI controller, which will unceremoniously grab
+ * all the port connections away from its companions.  To prevent annoying
+ * error messages, lock the companion's root hub and gracefully unconfigure
+ * it beforehand.  Leave it locked until the EHCI controller is all set.
+ */
+static void ehci_pre_add(struct pci_dev *pdev, struct usb_hcd *hcd,
+		struct pci_dev *companion, struct usb_hcd *companion_hcd)
 {
-	mutex_lock(&companions_mutex);
-	dev_set_drvdata(&pdev->dev, hcd);
-	companion_common(pdev, hcd, SET_HS_COMPANION);
-	mutex_unlock(&companions_mutex);
+	struct usb_device *udev;
+
+	if (is_ohci_or_uhci(companion)) {
+		udev = companion_hcd->self.root_hub;
+		usb_lock_device(udev);
+		usb_set_configuration(udev, 0);
+	}
 }
 
-static void clear_hs_companion(struct pci_dev *pdev, struct usb_hcd *hcd)
+/*
+ * Adding the EHCI controller has either succeeded or failed.  Set the
+ * companion pointer accordingly, and in either case, reconfigure and
+ * unlock the root hub.
+ */
+static void ehci_post_add(struct pci_dev *pdev, struct usb_hcd *hcd,
+		struct pci_dev *companion, struct usb_hcd *companion_hcd)
 {
-	mutex_lock(&companions_mutex);
-	dev_set_drvdata(&pdev->dev, NULL);
+	struct usb_device *udev;
 
-	/* If pdev is OHCI or UHCI, just clear its hs_companion pointer */
-	if (pdev->class == CL_OHCI || pdev->class == CL_UHCI)
-		hcd->self.hs_companion = NULL;
+	if (is_ohci_or_uhci(companion)) {
+		if (dev_get_drvdata(&pdev->dev)) {	/* Succeeded */
+			dev_dbg(&pdev->dev, "HS companion for %s\n",
+					dev_name(&companion->dev));
+			companion_hcd->self.hs_companion = &hcd->self;
+		}
+		udev = companion_hcd->self.root_hub;
+		usb_set_configuration(udev, 1);
+		usb_unlock_device(udev);
+	}
+}
 
-	/* Otherwise search for companion buses and clear their pointers */
-	else
-		companion_common(pdev, hcd, CLEAR_HS_COMPANION);
-	mutex_unlock(&companions_mutex);
+/*
+ * We just added a non-EHCI controller.  Find the EHCI controller to
+ * which it is a companion, and store a pointer to the bus structure.
+ */
+static void non_ehci_add(struct pci_dev *pdev, struct usb_hcd *hcd,
+		struct pci_dev *companion, struct usb_hcd *companion_hcd)
+{
+	if (is_ohci_or_uhci(pdev) && companion->class == CL_EHCI) {
+		dev_dbg(&pdev->dev, "FS/LS companion for %s\n",
+				dev_name(&companion->dev));
+		hcd->self.hs_companion = &companion_hcd->self;
+	}
 }
 
-static void wait_for_companions(struct pci_dev *pdev, struct usb_hcd *hcd)
+/* We are removing an EHCI controller.  Clear the companions' pointers. */
+static void ehci_remove(struct pci_dev *pdev, struct usb_hcd *hcd,
+		struct pci_dev *companion, struct usb_hcd *companion_hcd)
 {
-	/* Only EHCI controllers need to wait.
-	 * No locking is needed because a controller cannot be resumed
-	 * while one of its companions is getting unbound.
-	 */
-	if (pdev->class == CL_EHCI)
-		companion_common(pdev, hcd, WAIT_FOR_COMPANIONS);
+	if (is_ohci_or_uhci(companion))
+		companion_hcd->self.hs_companion = NULL;
 }
 
-#else /* !CONFIG_PM_SLEEP */
+#ifdef	CONFIG_PM
 
-static inline void set_hs_companion(struct pci_dev *d, struct usb_hcd *h) {}
-static inline void clear_hs_companion(struct pci_dev *d, struct usb_hcd *h) {}
-static inline void wait_for_companions(struct pci_dev *d, struct usb_hcd *h) {}
+/* An EHCI controller must wait for its companions before resuming. */
+static void ehci_wait_for_companions(struct pci_dev *pdev, struct usb_hcd *hcd,
+		struct pci_dev *companion, struct usb_hcd *companion_hcd)
+{
+	if (is_ohci_or_uhci(companion))
+		device_pm_wait_for_dev(&pdev->dev, &companion->dev);
+}
 
-#endif /* !CONFIG_PM_SLEEP */
+#endif	/* CONFIG_PM */
 
 /*-------------------------------------------------------------------------*/
 
@@ -217,7 +221,7 @@ int usb_hcd_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
 				driver->description)) {
 			dev_dbg(&dev->dev, "controller already in use\n");
 			retval = -EBUSY;
-			goto clear_companion;
+			goto put_hcd;
 		}
 		hcd->regs = ioremap_nocache(hcd->rsrc_start, hcd->rsrc_len);
 		if (hcd->regs == NULL) {
@@ -244,16 +248,35 @@ int usb_hcd_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
 		if (region == PCI_ROM_RESOURCE) {
 			dev_dbg(&dev->dev, "no i/o regions available\n");
 			retval = -EBUSY;
-			goto clear_companion;
+			goto put_hcd;
 		}
 	}
 
 	pci_set_master(dev);
 
-	retval = usb_add_hcd(hcd, hcd_irq, IRQF_SHARED);
+	/* Note: dev_set_drvdata must be called while holding the rwsem */
+	if (dev->class == CL_EHCI) {
+		down_write(&companions_rwsem);
+		dev_set_drvdata(&dev->dev, hcd);
+		for_each_companion(dev, hcd, ehci_pre_add);
+		retval = usb_add_hcd(hcd, hcd_irq, IRQF_SHARED);
+		if (retval != 0)
+			dev_set_drvdata(&dev->dev, NULL);
+		for_each_companion(dev, hcd, ehci_post_add);
+		up_write(&companions_rwsem);
+	} else {
+		down_read(&companions_rwsem);
+		dev_set_drvdata(&dev->dev, hcd);
+		retval = usb_add_hcd(hcd, hcd_irq, IRQF_SHARED);
+		if (retval != 0)
+			dev_set_drvdata(&dev->dev, NULL);
+		else
+			for_each_companion(dev, hcd, non_ehci_add);
+		up_read(&companions_rwsem);
+	}
+
 	if (retval != 0)
 		goto unmap_registers;
-	set_hs_companion(dev, hcd);
 
 	if (pci_dev_run_wake(dev))
 		pm_runtime_put_noidle(&dev->dev);
@@ -266,8 +289,7 @@ release_mem_region:
 		release_mem_region(hcd->rsrc_start, hcd->rsrc_len);
 	} else
 		release_region(hcd->rsrc_start, hcd->rsrc_len);
-clear_companion:
-	clear_hs_companion(dev, hcd);
+put_hcd:
 	usb_put_hcd(hcd);
 disable_pci:
 	pci_disable_device(dev);
@@ -310,14 +332,29 @@ void usb_hcd_pci_remove(struct pci_dev *dev)
 	usb_hcd_irq(0, hcd);
 	local_irq_enable();
 
-	usb_remove_hcd(hcd);
+	/* Note: dev_set_drvdata must be called while holding the rwsem */
+	if (dev->class == CL_EHCI) {
+		down_write(&companions_rwsem);
+		for_each_companion(dev, hcd, ehci_remove);
+		usb_remove_hcd(hcd);
+		dev_set_drvdata(&dev->dev, NULL);
+		up_write(&companions_rwsem);
+	} else {
+		/* Not EHCI; just clear the companion pointer */
+		down_read(&companions_rwsem);
+		hcd->self.hs_companion = NULL;
+		usb_remove_hcd(hcd);
+		dev_set_drvdata(&dev->dev, NULL);
+		up_read(&companions_rwsem);
+	}
+
 	if (hcd->driver->flags & HCD_MEMORY) {
 		iounmap(hcd->regs);
 		release_mem_region(hcd->rsrc_start, hcd->rsrc_len);
 	} else {
 		release_region(hcd->rsrc_start, hcd->rsrc_len);
 	}
-	clear_hs_companion(dev, hcd);
+
 	usb_put_hcd(hcd);
 	pci_disable_device(dev);
 }
@@ -463,8 +500,15 @@ static int resume_common(struct device *dev, int event)
 	pci_set_master(pci_dev);
 
 	if (hcd->driver->pci_resume && !HCD_DEAD(hcd)) {
-		if (event != PM_EVENT_AUTO_RESUME)
-			wait_for_companions(pci_dev, hcd);
+
+		/*
+		 * Only EHCI controllers have to wait for their companions.
+		 * No locking is needed because PCI controller drivers do not
+		 * get unbound during system resume.
+		 */
+		if (pci_dev->class == CL_EHCI && event != PM_EVENT_AUTO_RESUME)
+			for_each_companion(pci_dev, hcd,
+					ehci_wait_for_companions);
 
 		retval = hcd->driver->pci_resume(hcd,
 				event == PM_EVENT_RESTORE);
diff --git a/drivers/usb/core/hcd.c b/drivers/usb/core/hcd.c
index 99b34a30354..d53547d2e4c 100644
--- a/drivers/usb/core/hcd.c
+++ b/drivers/usb/core/hcd.c
@@ -2125,7 +2125,7 @@ int hcd_bus_resume(struct usb_device *rhdev, pm_message_t msg)
 
 #endif	/* CONFIG_PM */
 
-#ifdef	CONFIG_USB_SUSPEND
+#ifdef	CONFIG_PM_RUNTIME
 
 /* Workqueue routine for root-hub remote wakeup */
 static void hcd_resume_work(struct work_struct *work)
@@ -2160,7 +2160,7 @@ void usb_hcd_resume_root_hub (struct usb_hcd *hcd)
 }
 EXPORT_SYMBOL_GPL(usb_hcd_resume_root_hub);
 
-#endif	/* CONFIG_USB_SUSPEND */
+#endif	/* CONFIG_PM_RUNTIME */
 
 /*-------------------------------------------------------------------------*/
 
@@ -2336,7 +2336,7 @@ struct usb_hcd *usb_create_shared_hcd(const struct hc_driver *driver,
 	init_timer(&hcd->rh_timer);
 	hcd->rh_timer.function = rh_timer_func;
 	hcd->rh_timer.data = (unsigned long) hcd;
-#ifdef CONFIG_USB_SUSPEND
+#ifdef CONFIG_PM_RUNTIME
 	INIT_WORK(&hcd->wakeup_work, hcd_resume_work);
 #endif
 
@@ -2412,6 +2412,14 @@ int usb_hcd_is_primary_hcd(struct usb_hcd *hcd)
 }
 EXPORT_SYMBOL_GPL(usb_hcd_is_primary_hcd);
 
+int usb_hcd_find_raw_port_number(struct usb_hcd *hcd, int port1)
+{
+	if (!hcd->driver->find_raw_port_number)
+		return port1;
+
+	return hcd->driver->find_raw_port_number(hcd, port1);
+}
+
 static int usb_hcd_request_irqs(struct usb_hcd *hcd,
 		unsigned int irqnum, unsigned long irqflags)
 {
@@ -2582,7 +2590,7 @@ error_create_attr_group:
 	hcd->rh_registered = 0;
 	spin_unlock_irq(&hcd_root_hub_lock);
 
-#ifdef CONFIG_USB_SUSPEND
+#ifdef CONFIG_PM_RUNTIME
 	cancel_work_sync(&hcd->wakeup_work);
 #endif
 	mutex_lock(&usb_bus_list_lock);
@@ -2637,7 +2645,7 @@ void usb_remove_hcd(struct usb_hcd *hcd)
 	hcd->rh_registered = 0;
 	spin_unlock_irq (&hcd_root_hub_lock);
 
-#ifdef CONFIG_USB_SUSPEND
+#ifdef CONFIG_PM_RUNTIME
 	cancel_work_sync(&hcd->wakeup_work);
 #endif
 
diff --git a/drivers/usb/core/hub.c b/drivers/usb/core/hub.c
index 5480352f984..feef9351463 100644
--- a/drivers/usb/core/hub.c
+++ b/drivers/usb/core/hub.c
@@ -555,8 +555,9 @@ static int hub_port_status(struct usb_hub *hub, int port1,
 	mutex_lock(&hub->status_mutex);
 	ret = get_port_status(hub->hdev, port1, &hub->status->port);
 	if (ret < 4) {
-		dev_err(hub->intfdev,
-			"%s failed (err = %d)\n", __func__, ret);
+		if (ret != -ENODEV)
+			dev_err(hub->intfdev,
+				"%s failed (err = %d)\n", __func__, ret);
 		if (ret >= 0)
 			ret = -EIO;
 	} else {
@@ -699,7 +700,7 @@ static void hub_tt_work(struct work_struct *work)
 		/* drop lock so HCD can concurrently report other TT errors */
 		spin_unlock_irqrestore (&hub->tt.lock, flags);
 		status = hub_clear_tt_buffer (hdev, clear->devinfo, clear->tt);
-		if (status)
+		if (status && status != -ENODEV)
 			dev_err (&hdev->dev,
 				"clear tt %d (%04x) error %d\n",
 				clear->tt, clear->devinfo, status);
@@ -837,10 +838,11 @@ static int hub_hub_status(struct usb_hub *hub,
 
 	mutex_lock(&hub->status_mutex);
 	ret = get_hub_status(hub->hdev, &hub->status->hub);
-	if (ret < 0)
-		dev_err (hub->intfdev,
-			"%s failed (err = %d)\n", __func__, ret);
-	else {
+	if (ret < 0) {
+		if (ret != -ENODEV)
+			dev_err(hub->intfdev,
+				"%s failed (err = %d)\n", __func__, ret);
+	} else {
 		*status = le16_to_cpu(hub->status->hub.wHubStatus);
 		*change = le16_to_cpu(hub->status->hub.wHubChange); 
 		ret = 0;
@@ -877,11 +879,8 @@ static int hub_usb3_port_disable(struct usb_hub *hub, int port1)
 		return -EINVAL;
 
 	ret = hub_set_port_link_state(hub, port1, USB_SS_PORT_LS_SS_DISABLED);
-	if (ret) {
-		dev_err(hub->intfdev, "cannot disable port %d (err = %d)\n",
-				port1, ret);
+	if (ret)
 		return ret;
-	}
 
 	/* Wait for the link to enter the disabled state. */
 	for (total_time = 0; ; total_time += HUB_DEBOUNCE_STEP) {
@@ -918,7 +917,7 @@ static int hub_port_disable(struct usb_hub *hub, int port1, int set_state)
 			ret = usb_clear_port_feature(hdev, port1,
 					USB_PORT_FEAT_ENABLE);
 	}
-	if (ret)
+	if (ret && ret != -ENODEV)
 		dev_err(hub->intfdev, "cannot disable port %d (err = %d)\n",
 				port1, ret);
 	return ret;
@@ -1317,6 +1316,10 @@ static int hub_configure(struct usb_hub *hub,
 		message = "hub has too many ports!";
 		ret = -ENODEV;
 		goto fail;
+	} else if (hub->descriptor->bNbrPorts == 0) {
+		message = "hub doesn't have any ports!";
+		ret = -ENODEV;
+		goto fail;
 	}
 
 	hdev->maxchild = hub->descriptor->bNbrPorts;
@@ -2192,8 +2195,9 @@ static int usb_enumerate_device(struct usb_device *udev)
 	if (udev->config == NULL) {
 		err = usb_get_configuration(udev);
 		if (err < 0) {
-			dev_err(&udev->dev, "can't read configurations, error %d\n",
-				err);
+			if (err != -ENODEV)
+				dev_err(&udev->dev, "can't read configurations, error %d\n",
+						err);
 			return err;
 		}
 	}
@@ -2640,14 +2644,16 @@ static int hub_port_reset(struct usb_hub *hub, int port1,
 		status = set_port_feature(hub->hdev, port1, (warm ?
 					USB_PORT_FEAT_BH_PORT_RESET :
 					USB_PORT_FEAT_RESET));
-		if (status) {
+		if (status == -ENODEV) {
+			;	/* The hub is gone */
+		} else if (status) {
 			dev_err(hub->intfdev,
 					"cannot %sreset port %d (err = %d)\n",
 					warm ? "warm " : "", port1, status);
 		} else {
 			status = hub_port_wait_reset(hub, port1, udev, delay,
 								warm);
-			if (status && status != -ENOTCONN)
+			if (status && status != -ENOTCONN && status != -ENODEV)
 				dev_dbg(hub->intfdev,
 						"port_wait_reset: err = %d\n",
 						status);
@@ -2821,7 +2827,7 @@ void usb_enable_ltm(struct usb_device *udev)
 }
 EXPORT_SYMBOL_GPL(usb_enable_ltm);
 
-#ifdef	CONFIG_USB_SUSPEND
+#ifdef	CONFIG_PM
 /*
  * usb_disable_function_remotewakeup - disable usb3.0
  * device's function remote wakeup
@@ -2880,9 +2886,11 @@ static int usb_disable_function_remotewakeup(struct usb_device *udev)
  * Linux (2.6) currently has NO mechanisms to initiate that:  no khubd
  * timer, no SRP, no requests through sysfs.
  *
- * If CONFIG_USB_SUSPEND isn't enabled, devices only really suspend when
- * the root hub for their bus goes into global suspend ... so we don't
- * (falsely) update the device power state to say it suspended.
+ * If Runtime PM isn't enabled or used, non-SuperSpeed devices really get
+ * suspended only when their bus goes into global suspend (i.e., the root
+ * hub is suspended).  Nevertheless, we change @udev->state to
+ * USB_STATE_SUSPENDED as this is the device's "logical" state.  The actual
+ * upstream port setting is stored in @udev->port_is_suspended.
  *
  * Returns 0 on success, else negative errno.
  */
@@ -2893,6 +2901,7 @@ int usb_port_suspend(struct usb_device *udev, pm_message_t msg)
 	enum pm_qos_flags_status pm_qos_stat;
 	int		port1 = udev->portnum;
 	int		status;
+	bool		really_suspend = true;
 
 	/* enable remote wakeup when appropriate; this lets the device
 	 * wake up the upstream hub (including maybe the root hub).
@@ -2949,9 +2958,19 @@ int usb_port_suspend(struct usb_device *udev, pm_message_t msg)
 	/* see 7.1.7.6 */
 	if (hub_is_superspeed(hub->hdev))
 		status = hub_set_port_link_state(hub, port1, USB_SS_PORT_LS_U3);
-	else
+	else if (PMSG_IS_AUTO(msg))
 		status = set_port_feature(hub->hdev, port1,
 						USB_PORT_FEAT_SUSPEND);
+	/*
+	 * For system suspend, we do not need to enable the suspend feature
+	 * on individual USB-2 ports.  The devices will automatically go
+	 * into suspend a few ms after the root hub stops sending packets.
+	 * The USB 2.0 spec calls this "global suspend".
+	 */
+	else {
+		really_suspend = false;
+		status = 0;
+	}
 	if (status) {
 		dev_dbg(hub->intfdev, "can't suspend port %d, status %d\n",
 				port1, status);
@@ -2987,8 +3006,10 @@ int usb_port_suspend(struct usb_device *udev, pm_message_t msg)
 				(PMSG_IS_AUTO(msg) ? "auto-" : ""),
 				udev->do_remote_wakeup);
 		usb_set_device_state(udev, USB_STATE_SUSPENDED);
-		udev->port_is_suspended = 1;
-		msleep(10);
+		if (really_suspend) {
+			udev->port_is_suspended = 1;
+			msleep(10);
+		}
 	}
 
 	/*
@@ -3226,6 +3247,10 @@ int usb_port_resume(struct usb_device *udev, pm_message_t msg)
 	return status;
 }
 
+#endif	/* CONFIG_PM */
+
+#ifdef	CONFIG_PM_RUNTIME
+
 /* caller has locked udev */
 int usb_remote_wakeup(struct usb_device *udev)
 {
@@ -3242,38 +3267,6 @@ int usb_remote_wakeup(struct usb_device *udev)
 	return status;
 }
 
-#else	/* CONFIG_USB_SUSPEND */
-
-/* When CONFIG_USB_SUSPEND isn't set, we never suspend or resume any ports. */
-
-int usb_port_suspend(struct usb_device *udev, pm_message_t msg)
-{
-	return 0;
-}
-
-/* However we may need to do a reset-resume */
-
-int usb_port_resume(struct usb_device *udev, pm_message_t msg)
-{
-	struct usb_hub	*hub = usb_hub_to_struct_hub(udev->parent);
-	int		port1 = udev->portnum;
-	int		status;
-	u16		portchange, portstatus;
-
-	status = hub_port_status(hub, port1, &portstatus, &portchange);
-	status = check_port_resume_type(udev,
-			hub, port1, status, portchange, portstatus);
-
-	if (status) {
-		dev_dbg(&udev->dev, "can't resume, status %d\n", status);
-		hub_port_logical_disconnect(hub, port1);
-	} else if (udev->reset_resume) {
-		dev_dbg(&udev->dev, "reset-resume\n");
-		status = usb_reset_and_verify_device(udev);
-	}
-	return status;
-}
-
 #endif
 
 static int check_ports_changed(struct usb_hub *hub)
@@ -4090,9 +4083,9 @@ hub_port_init (struct usb_hub *hub, struct usb_device *udev, int port1,
 				goto fail;
 			}
 			if (r) {
-				dev_err(&udev->dev,
-					"device descriptor read/64, error %d\n",
-					r);
+				if (r != -ENODEV)
+					dev_err(&udev->dev, "device descriptor read/64, error %d\n",
+							r);
 				retval = -EMSGSIZE;
 				continue;
 			}
@@ -4112,9 +4105,9 @@ hub_port_init (struct usb_hub *hub, struct usb_device *udev, int port1,
 				msleep(200);
 			}
 			if (retval < 0) {
-				dev_err(&udev->dev,
-					"device not accepting address %d, error %d\n",
-					devnum, retval);
+				if (retval != -ENODEV)
+					dev_err(&udev->dev, "device not accepting address %d, error %d\n",
+							devnum, retval);
 				goto fail;
 			}
 			if (udev->speed == USB_SPEED_SUPER) {
@@ -4136,7 +4129,8 @@ hub_port_init (struct usb_hub *hub, struct usb_device *udev, int port1,
 
 		retval = usb_get_device_descriptor(udev, 8);
 		if (retval < 8) {
-			dev_err(&udev->dev,
+			if (retval != -ENODEV)
+				dev_err(&udev->dev,
 					"device descriptor read/8, error %d\n",
 					retval);
 			if (retval >= 0)
@@ -4190,8 +4184,9 @@ hub_port_init (struct usb_hub *hub, struct usb_device *udev, int port1,
   
 	retval = usb_get_device_descriptor(udev, USB_DT_DEVICE_SIZE);
 	if (retval < (signed)sizeof(udev->descriptor)) {
-		dev_err(&udev->dev, "device descriptor read/all, error %d\n",
-			retval);
+		if (retval != -ENODEV)
+			dev_err(&udev->dev, "device descriptor read/all, error %d\n",
+					retval);
 		if (retval >= 0)
 			retval = -ENOMSG;
 		goto fail;
@@ -4333,7 +4328,7 @@ static void hub_port_connect_change(struct usb_hub *hub, int port1,
 		if (portstatus & USB_PORT_STAT_ENABLE) {
 			status = 0;		/* Nothing to do */
 
-#ifdef CONFIG_USB_SUSPEND
+#ifdef CONFIG_PM_RUNTIME
 		} else if (udev->state == USB_STATE_SUSPENDED &&
 				udev->persist_enabled) {
 			/* For a suspended device, treat this as a
@@ -4373,7 +4368,7 @@ static void hub_port_connect_change(struct usb_hub *hub, int port1,
 				USB_PORT_STAT_C_ENABLE)) {
 		status = hub_port_debounce_be_stable(hub, port1);
 		if (status < 0) {
-			if (printk_ratelimit())
+			if (status != -ENODEV && printk_ratelimit())
 				dev_err(hub_dev, "connect-debounce failed, "
 						"port %d disabled\n", port1);
 			portstatus &= ~USB_PORT_STAT_CONNECTION;
@@ -4402,6 +4397,7 @@ static void hub_port_connect_change(struct usb_hub *hub, int port1,
 	else
 		unit_load = 100;
 
+	status = 0;
 	for (i = 0; i < SET_CONFIG_TRIES; i++) {
 
 		/* reallocate for each attempt, since references
@@ -4526,9 +4522,11 @@ loop:
 	}
 	if (hub->hdev->parent ||
 			!hcd->driver->port_handed_over ||
-			!(hcd->driver->port_handed_over)(hcd, port1))
-		dev_err(hub_dev, "unable to enumerate USB device on port %d\n",
-				port1);
+			!(hcd->driver->port_handed_over)(hcd, port1)) {
+		if (status != -ENOTCONN && status != -ENODEV)
+			dev_err(hub_dev, "unable to enumerate USB device on port %d\n",
+					port1);
+	}
  
 done:
 	hub_port_disable(hub, port1, 1);
diff --git a/drivers/usb/core/port.c b/drivers/usb/core/port.c
index 797f9d51473..b8bad294eeb 100644
--- a/drivers/usb/core/port.c
+++ b/drivers/usb/core/port.c
@@ -67,11 +67,10 @@ static void usb_port_device_release(struct device *dev)
 {
 	struct usb_port *port_dev = to_usb_port(dev);
 
-	dev_pm_qos_hide_flags(dev);
 	kfree(port_dev);
 }
 
-#ifdef CONFIG_USB_SUSPEND
+#ifdef CONFIG_PM_RUNTIME
 static int usb_port_runtime_resume(struct device *dev)
 {
 	struct usb_port *port_dev = to_usb_port(dev);
@@ -139,7 +138,7 @@ static int usb_port_runtime_suspend(struct device *dev)
 #endif
 
 static const struct dev_pm_ops usb_port_pm_ops = {
-#ifdef CONFIG_USB_SUSPEND
+#ifdef CONFIG_PM_RUNTIME
 	.runtime_suspend =	usb_port_runtime_suspend,
 	.runtime_resume =	usb_port_runtime_resume,
 	.runtime_idle =		pm_generic_runtime_idle,
diff --git a/drivers/usb/core/quirks.c b/drivers/usb/core/quirks.c
index 3113c1d7144..ab5638d9c70 100644
--- a/drivers/usb/core/quirks.c
+++ b/drivers/usb/core/quirks.c
@@ -201,20 +201,14 @@ void usb_detect_quirks(struct usb_device *udev)
 		dev_dbg(&udev->dev, "USB quirks for this device: %x\n",
 			udev->quirks);
 
-	/* For the present, all devices default to USB-PERSIST enabled */
-#if 0		/* was: #ifdef CONFIG_PM */
-	/* Hubs are automatically enabled for USB-PERSIST */
-	if (udev->descriptor.bDeviceClass == USB_CLASS_HUB)
+#ifdef CONFIG_USB_DEFAULT_PERSIST
+	if (!(udev->quirks & USB_QUIRK_RESET))
 		udev->persist_enabled = 1;
-
 #else
-	/* In the absence of PM, we can safely enable USB-PERSIST
-	 * for all devices.  It will affect things like hub resets
-	 * and EMF-related port disables.
-	 */
-	if (!(udev->quirks & USB_QUIRK_RESET))
+	/* Hubs are automatically enabled for USB-PERSIST */
+	if (udev->descriptor.bDeviceClass == USB_CLASS_HUB)
 		udev->persist_enabled = 1;
-#endif	/* CONFIG_PM */
+#endif	/* CONFIG_USB_DEFAULT_PERSIST */
 }
 
 void usb_detect_interface_quirks(struct usb_device *udev)
diff --git a/drivers/usb/core/sysfs.c b/drivers/usb/core/sysfs.c
index 3f81a3dc686..aa38db44818 100644
--- a/drivers/usb/core/sysfs.c
+++ b/drivers/usb/core/sysfs.c
@@ -338,7 +338,7 @@ static void remove_persist_attributes(struct device *dev)
 
 #endif	/* CONFIG_PM */
 
-#ifdef	CONFIG_USB_SUSPEND
+#ifdef	CONFIG_PM_RUNTIME
 
 static ssize_t
 show_connected_duration(struct device *dev, struct device_attribute *attr,
@@ -544,7 +544,7 @@ static void remove_power_attributes(struct device *dev)
 #define add_power_attributes(dev)	0
 #define remove_power_attributes(dev)	do {} while (0)
 
-#endif	/* CONFIG_USB_SUSPEND */
+#endif	/* CONFIG_PM_RUNTIME */
 
 
 /* Descriptor fields */
diff --git a/drivers/usb/core/urb.c b/drivers/usb/core/urb.c
index e0d9d948218..16927fa88fb 100644
--- a/drivers/usb/core/urb.c
+++ b/drivers/usb/core/urb.c
@@ -683,10 +683,13 @@ EXPORT_SYMBOL_GPL(usb_kill_urb);
 void usb_poison_urb(struct urb *urb)
 {
 	might_sleep();
-	if (!(urb && urb->dev && urb->ep))
+	if (!urb)
 		return;
 	atomic_inc(&urb->reject);
 
+	if (!urb->dev || !urb->ep)
+		return;
+
 	usb_hcd_unlink_urb(urb, -ENOENT);
 	wait_event(usb_kill_urb_queue, atomic_read(&urb->use_count) == 0);
 }
diff --git a/drivers/usb/core/usb-acpi.c b/drivers/usb/core/usb-acpi.c
index b6f4bad3f75..255c14464bf 100644
--- a/drivers/usb/core/usb-acpi.c
+++ b/drivers/usb/core/usb-acpi.c
@@ -15,6 +15,7 @@
 #include <linux/kernel.h>
 #include <linux/acpi.h>
 #include <linux/pci.h>
+#include <linux/usb/hcd.h>
 #include <acpi/acpi_bus.h>
 
 #include "usb.h"
@@ -188,8 +189,13 @@ static int usb_acpi_find_device(struct device *dev, acpi_handle *handle)
 		 * connected to.
 		 */
 		if (!udev->parent) {
-			*handle = acpi_get_child(DEVICE_ACPI_HANDLE(&udev->dev),
+			struct usb_hcd *hcd = bus_to_hcd(udev->bus);
+			int raw_port_num;
+
+			raw_port_num = usb_hcd_find_raw_port_number(hcd,
 				port_num);
+			*handle = acpi_get_child(DEVICE_ACPI_HANDLE(&udev->dev),
+				raw_port_num);
 			if (!*handle)
 				return -ENODEV;
 		} else {
diff --git a/drivers/usb/core/usb.c b/drivers/usb/core/usb.c
index f81b9257273..b10da720f2b 100644
--- a/drivers/usb/core/usb.c
+++ b/drivers/usb/core/usb.c
@@ -49,7 +49,7 @@ const char *usbcore_name = "usbcore";
 
 static bool nousb;	/* Disable USB when built into kernel image */
 
-#ifdef	CONFIG_USB_SUSPEND
+#ifdef	CONFIG_PM_RUNTIME
 static int usb_autosuspend_delay = 2;		/* Default delay value,
 						 * in seconds */
 module_param_named(autosuspend, usb_autosuspend_delay, int, 0644);
@@ -307,7 +307,7 @@ static const struct dev_pm_ops usb_device_pm_ops = {
 	.thaw =		usb_dev_thaw,
 	.poweroff =	usb_dev_poweroff,
 	.restore =	usb_dev_restore,
-#ifdef CONFIG_USB_SUSPEND
+#ifdef CONFIG_PM_RUNTIME
 	.runtime_suspend =	usb_runtime_suspend,
 	.runtime_resume =	usb_runtime_resume,
 	.runtime_idle =		usb_runtime_idle,
@@ -317,7 +317,8 @@ static const struct dev_pm_ops usb_device_pm_ops = {
 #endif	/* CONFIG_PM */
 
 
-static char *usb_devnode(struct device *dev, umode_t *mode)
+static char *usb_devnode(struct device *dev,
+			 umode_t *mode, kuid_t *uid, kgid_t *gid)
 {
 	struct usb_device *usb_dev;
 
diff --git a/drivers/usb/core/usb.h b/drivers/usb/core/usb.h
index a7f20bde0e5..823857767a1 100644
--- a/drivers/usb/core/usb.h
+++ b/drivers/usb/core/usb.h
@@ -93,7 +93,7 @@ static inline int usb_port_resume(struct usb_device *udev, pm_message_t msg)
 
 #endif
 
-#ifdef CONFIG_USB_SUSPEND
+#ifdef CONFIG_PM_RUNTIME
 
 extern void usb_autosuspend_device(struct usb_device *udev);
 extern int usb_autoresume_device(struct usb_device *udev);
diff --git a/drivers/usb/dwc3/Kconfig b/drivers/usb/dwc3/Kconfig
index 68e9a2c5a01..ea5ee9c21c3 100644
--- a/drivers/usb/dwc3/Kconfig
+++ b/drivers/usb/dwc3/Kconfig
@@ -1,7 +1,6 @@
 config USB_DWC3
 	tristate "DesignWare USB3 DRD Core Support"
 	depends on (USB || USB_GADGET) && GENERIC_HARDIRQS
-	select USB_OTG_UTILS
 	select USB_XHCI_PLATFORM if USB_SUPPORT && USB_XHCI_HCD
 	help
 	  Say Y or M here if your system has a Dual Role SuperSpeed
diff --git a/drivers/usb/dwc3/core.c b/drivers/usb/dwc3/core.c
index ffa6b004a84..c35d49d39b7 100644
--- a/drivers/usb/dwc3/core.c
+++ b/drivers/usb/dwc3/core.c
@@ -140,7 +140,8 @@ static void dwc3_free_one_event_buffer(struct dwc3 *dwc,
  * Returns a pointer to the allocated event buffer structure on success
  * otherwise ERR_PTR(errno).
  */
-static struct dwc3_event_buffer *dwc3_alloc_one_event_buffer(struct dwc3 *dwc, unsigned length)
+static struct dwc3_event_buffer *dwc3_alloc_one_event_buffer(struct dwc3 *dwc,
+		unsigned length)
 {
 	struct dwc3_event_buffer	*evt;
 
@@ -259,6 +260,17 @@ static void dwc3_event_buffers_cleanup(struct dwc3 *dwc)
 	}
 }
 
+static void dwc3_core_num_eps(struct dwc3 *dwc)
+{
+	struct dwc3_hwparams	*parms = &dwc->hwparams;
+
+	dwc->num_in_eps = DWC3_NUM_IN_EPS(parms);
+	dwc->num_out_eps = DWC3_NUM_EPS(parms) - dwc->num_in_eps;
+
+	dev_vdbg(dwc->dev, "found %d IN and %d OUT endpoints\n",
+			dwc->num_in_eps, dwc->num_out_eps);
+}
+
 static void dwc3_cache_hwparams(struct dwc3 *dwc)
 {
 	struct dwc3_hwparams	*parms = &dwc->hwparams;
@@ -335,13 +347,9 @@ static int dwc3_core_init(struct dwc3 *dwc)
 	if (dwc->revision < DWC3_REVISION_190A)
 		reg |= DWC3_GCTL_U2RSTECN;
 
-	dwc3_writel(dwc->regs, DWC3_GCTL, reg);
+	dwc3_core_num_eps(dwc);
 
-	ret = dwc3_event_buffers_setup(dwc);
-	if (ret) {
-		dev_err(dwc->dev, "failed to setup event buffers\n");
-		goto err0;
-	}
+	dwc3_writel(dwc->regs, DWC3_GCTL, reg);
 
 	return 0;
 
@@ -351,8 +359,6 @@ err0:
 
 static void dwc3_core_exit(struct dwc3 *dwc)
 {
-	dwc3_event_buffers_cleanup(dwc);
-
 	usb_phy_shutdown(dwc->usb2_phy);
 	usb_phy_shutdown(dwc->usb3_phy);
 }
@@ -428,12 +434,32 @@ static int dwc3_probe(struct platform_device *pdev)
 		dwc->usb3_phy = devm_usb_get_phy(dev, USB_PHY_TYPE_USB3);
 	}
 
-	if (IS_ERR_OR_NULL(dwc->usb2_phy)) {
+	if (IS_ERR(dwc->usb2_phy)) {
+		ret = PTR_ERR(dwc->usb2_phy);
+
+		/*
+		 * if -ENXIO is returned, it means PHY layer wasn't
+		 * enabled, so it makes no sense to return -EPROBE_DEFER
+		 * in that case, since no PHY driver will ever probe.
+		 */
+		if (ret == -ENXIO)
+			return ret;
+
 		dev_err(dev, "no usb2 phy configured\n");
 		return -EPROBE_DEFER;
 	}
 
-	if (IS_ERR_OR_NULL(dwc->usb3_phy)) {
+	if (IS_ERR(dwc->usb3_phy)) {
+		ret = PTR_ERR(dwc->usb2_phy);
+
+		/*
+		 * if -ENXIO is returned, it means PHY layer wasn't
+		 * enabled, so it makes no sense to return -EPROBE_DEFER
+		 * in that case, since no PHY driver will ever probe.
+		 */
+		if (ret == -ENXIO)
+			return ret;
+
 		dev_err(dev, "no usb3 phy configured\n");
 		return -EPROBE_DEFER;
 	}
@@ -448,6 +474,10 @@ static int dwc3_probe(struct platform_device *pdev)
 	dwc->regs_size	= resource_size(res);
 	dwc->dev	= dev;
 
+	dev->dma_mask	= dev->parent->dma_mask;
+	dev->dma_parms	= dev->parent->dma_parms;
+	dma_set_coherent_mask(dev, dev->parent->coherent_dma_mask);
+
 	if (!strncmp("super", maximum_speed, 5))
 		dwc->maximum_speed = DWC3_DCFG_SUPERSPEED;
 	else if (!strncmp("high", maximum_speed, 4))
@@ -480,7 +510,18 @@ static int dwc3_probe(struct platform_device *pdev)
 		goto err0;
 	}
 
-	mode = DWC3_MODE(dwc->hwparams.hwparams0);
+	ret = dwc3_event_buffers_setup(dwc);
+	if (ret) {
+		dev_err(dwc->dev, "failed to setup event buffers\n");
+		goto err1;
+	}
+
+	if (IS_ENABLED(CONFIG_USB_DWC3_HOST))
+		mode = DWC3_MODE_HOST;
+	else if (IS_ENABLED(CONFIG_USB_DWC3_GADGET))
+		mode = DWC3_MODE_DEVICE;
+	else
+		mode = DWC3_MODE_DRD;
 
 	switch (mode) {
 	case DWC3_MODE_DEVICE:
@@ -488,7 +529,7 @@ static int dwc3_probe(struct platform_device *pdev)
 		ret = dwc3_gadget_init(dwc);
 		if (ret) {
 			dev_err(dev, "failed to initialize gadget\n");
-			goto err1;
+			goto err2;
 		}
 		break;
 	case DWC3_MODE_HOST:
@@ -496,7 +537,7 @@ static int dwc3_probe(struct platform_device *pdev)
 		ret = dwc3_host_init(dwc);
 		if (ret) {
 			dev_err(dev, "failed to initialize host\n");
-			goto err1;
+			goto err2;
 		}
 		break;
 	case DWC3_MODE_DRD:
@@ -504,32 +545,32 @@ static int dwc3_probe(struct platform_device *pdev)
 		ret = dwc3_host_init(dwc);
 		if (ret) {
 			dev_err(dev, "failed to initialize host\n");
-			goto err1;
+			goto err2;
 		}
 
 		ret = dwc3_gadget_init(dwc);
 		if (ret) {
 			dev_err(dev, "failed to initialize gadget\n");
-			goto err1;
+			goto err2;
 		}
 		break;
 	default:
 		dev_err(dev, "Unsupported mode of operation %d\n", mode);
-		goto err1;
+		goto err2;
 	}
 	dwc->mode = mode;
 
 	ret = dwc3_debugfs_init(dwc);
 	if (ret) {
 		dev_err(dev, "failed to initialize debugfs\n");
-		goto err2;
+		goto err3;
 	}
 
 	pm_runtime_allow(dev);
 
 	return 0;
 
-err2:
+err3:
 	switch (mode) {
 	case DWC3_MODE_DEVICE:
 		dwc3_gadget_exit(dwc);
@@ -546,6 +587,9 @@ err2:
 		break;
 	}
 
+err2:
+	dwc3_event_buffers_cleanup(dwc);
+
 err1:
 	dwc3_core_exit(dwc);
 
@@ -583,12 +627,130 @@ static int dwc3_remove(struct platform_device *pdev)
 		break;
 	}
 
+	dwc3_event_buffers_cleanup(dwc);
 	dwc3_free_event_buffers(dwc);
 	dwc3_core_exit(dwc);
 
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
+static int dwc3_prepare(struct device *dev)
+{
+	struct dwc3	*dwc = dev_get_drvdata(dev);
+	unsigned long	flags;
+
+	spin_lock_irqsave(&dwc->lock, flags);
+
+	switch (dwc->mode) {
+	case DWC3_MODE_DEVICE:
+	case DWC3_MODE_DRD:
+		dwc3_gadget_prepare(dwc);
+		/* FALLTHROUGH */
+	case DWC3_MODE_HOST:
+	default:
+		dwc3_event_buffers_cleanup(dwc);
+		break;
+	}
+
+	spin_unlock_irqrestore(&dwc->lock, flags);
+
+	return 0;
+}
+
+static void dwc3_complete(struct device *dev)
+{
+	struct dwc3	*dwc = dev_get_drvdata(dev);
+	unsigned long	flags;
+
+	spin_lock_irqsave(&dwc->lock, flags);
+
+	switch (dwc->mode) {
+	case DWC3_MODE_DEVICE:
+	case DWC3_MODE_DRD:
+		dwc3_gadget_complete(dwc);
+		/* FALLTHROUGH */
+	case DWC3_MODE_HOST:
+	default:
+		dwc3_event_buffers_setup(dwc);
+		break;
+	}
+
+	spin_unlock_irqrestore(&dwc->lock, flags);
+}
+
+static int dwc3_suspend(struct device *dev)
+{
+	struct dwc3	*dwc = dev_get_drvdata(dev);
+	unsigned long	flags;
+
+	spin_lock_irqsave(&dwc->lock, flags);
+
+	switch (dwc->mode) {
+	case DWC3_MODE_DEVICE:
+	case DWC3_MODE_DRD:
+		dwc3_gadget_suspend(dwc);
+		/* FALLTHROUGH */
+	case DWC3_MODE_HOST:
+	default:
+		/* do nothing */
+		break;
+	}
+
+	dwc->gctl = dwc3_readl(dwc->regs, DWC3_GCTL);
+	spin_unlock_irqrestore(&dwc->lock, flags);
+
+	usb_phy_shutdown(dwc->usb3_phy);
+	usb_phy_shutdown(dwc->usb2_phy);
+
+	return 0;
+}
+
+static int dwc3_resume(struct device *dev)
+{
+	struct dwc3	*dwc = dev_get_drvdata(dev);
+	unsigned long	flags;
+
+	usb_phy_init(dwc->usb3_phy);
+	usb_phy_init(dwc->usb2_phy);
+	msleep(100);
+
+	spin_lock_irqsave(&dwc->lock, flags);
+
+	dwc3_writel(dwc->regs, DWC3_GCTL, dwc->gctl);
+
+	switch (dwc->mode) {
+	case DWC3_MODE_DEVICE:
+	case DWC3_MODE_DRD:
+		dwc3_gadget_resume(dwc);
+		/* FALLTHROUGH */
+	case DWC3_MODE_HOST:
+	default:
+		/* do nothing */
+		break;
+	}
+
+	spin_unlock_irqrestore(&dwc->lock, flags);
+
+	pm_runtime_disable(dev);
+	pm_runtime_set_active(dev);
+	pm_runtime_enable(dev);
+
+	return 0;
+}
+
+static const struct dev_pm_ops dwc3_dev_pm_ops = {
+	.prepare	= dwc3_prepare,
+	.complete	= dwc3_complete,
+
+	SET_SYSTEM_SLEEP_PM_OPS(dwc3_suspend, dwc3_resume)
+};
+
+#define DWC3_PM_OPS	&(dwc3_dev_pm_ops)
+#else
+#define DWC3_PM_OPS	NULL
+#endif
+
 #ifdef CONFIG_OF
 static const struct of_device_id of_dwc3_match[] = {
 	{
@@ -605,6 +767,7 @@ static struct platform_driver dwc3_driver = {
 	.driver		= {
 		.name	= "dwc3",
 		.of_match_table	= of_match_ptr(of_dwc3_match),
+		.pm	= DWC3_PM_OPS,
 	},
 };
 
diff --git a/drivers/usb/dwc3/core.h b/drivers/usb/dwc3/core.h
index b4175066023..b69d322e3ca 100644
--- a/drivers/usb/dwc3/core.h
+++ b/drivers/usb/dwc3/core.h
@@ -154,8 +154,9 @@
 /* OTG Registers */
 #define DWC3_OCFG		0xcc00
 #define DWC3_OCTL		0xcc04
-#define DWC3_OEVTEN		0xcc08
-#define DWC3_OSTS		0xcc0C
+#define DWC3_OEVT		0xcc08
+#define DWC3_OEVTEN		0xcc0C
+#define DWC3_OSTS		0xcc10
 
 /* Bit fields */
 
@@ -369,6 +370,9 @@ struct dwc3_trb;
  * @list: a list of event buffers
  * @buf: _THE_ buffer
  * @length: size of this buffer
+ * @lpos: event offset
+ * @count: cache of last read event count register
+ * @flags: flags related to this event buffer
  * @dma: dma_addr_t
  * @dwc: pointer to DWC controller
  */
@@ -376,6 +380,10 @@ struct dwc3_event_buffer {
 	void			*buf;
 	unsigned		length;
 	unsigned int		lpos;
+	unsigned int		count;
+	unsigned int		flags;
+
+#define DWC3_EVENT_PENDING	BIT(0)
 
 	dma_addr_t		dma;
 
@@ -487,12 +495,6 @@ enum dwc3_link_state {
 	DWC3_LINK_STATE_MASK		= 0x0f,
 };
 
-enum dwc3_device_state {
-	DWC3_DEFAULT_STATE,
-	DWC3_ADDRESS_STATE,
-	DWC3_CONFIGURED_STATE,
-};
-
 /* TRB Length, PCM and Status */
 #define DWC3_TRB_SIZE_MASK	(0x00ffffff)
 #define DWC3_TRB_SIZE_LENGTH(n)	((n) & DWC3_TRB_SIZE_MASK)
@@ -574,6 +576,14 @@ struct dwc3_hwparams {
 /* HWPARAMS1 */
 #define DWC3_NUM_INT(n)		(((n) & (0x3f << 15)) >> 15)
 
+/* HWPARAMS3 */
+#define DWC3_NUM_IN_EPS_MASK	(0x1f << 18)
+#define DWC3_NUM_EPS_MASK	(0x3f << 12)
+#define DWC3_NUM_EPS(p)		(((p)->hwparams3 &		\
+			(DWC3_NUM_EPS_MASK)) >> 12)
+#define DWC3_NUM_IN_EPS(p)	(((p)->hwparams3 &		\
+			(DWC3_NUM_IN_EPS_MASK)) >> 18)
+
 /* HWPARAMS7 */
 #define DWC3_RAM1_DEPTH(n)	((n) & 0xffff)
 
@@ -618,7 +628,6 @@ struct dwc3_scratchpad_array {
  * @gadget_driver: pointer to the gadget driver
  * @regs: base address for our registers
  * @regs_size: address space size
- * @irq: IRQ number
  * @num_event_buffers: calculated number of event buffers
  * @u1u2: only used on revisions <1.83a for workaround
  * @maximum_speed: maximum speed requested (mainly for testing purposes)
@@ -626,6 +635,8 @@ struct dwc3_scratchpad_array {
  * @mode: mode of operation
  * @usb2_phy: pointer to USB2 PHY
  * @usb3_phy: pointer to USB3 PHY
+ * @dcfg: saved contents of DCFG register
+ * @gctl: saved contents of GCTL register
  * @is_selfpowered: true when we are selfpowered
  * @three_stage_setup: set if we perform a three phase setup
  * @ep0_bounced: true when we used bounce buffer
@@ -639,6 +650,8 @@ struct dwc3_scratchpad_array {
  * @u2pel: parameter from Set SEL request.
  * @u1sel: parameter from Set SEL request.
  * @u1pel: parameter from Set SEL request.
+ * @num_out_eps: number of out endpoints
+ * @num_in_eps: number of in endpoints
  * @ep0_next_event: hold the next expected event
  * @ep0state: state of endpoint zero
  * @link_state: link state
@@ -656,8 +669,10 @@ struct dwc3 {
 	dma_addr_t		ep0_trb_addr;
 	dma_addr_t		ep0_bounce_addr;
 	struct dwc3_request	ep0_usb_req;
+
 	/* device lock */
 	spinlock_t		lock;
+
 	struct device		*dev;
 
 	struct platform_device	*xhci;
@@ -675,6 +690,10 @@ struct dwc3 {
 	void __iomem		*regs;
 	size_t			regs_size;
 
+	/* used for suspend/resume */
+	u32			dcfg;
+	u32			gctl;
+
 	u32			num_event_buffers;
 	u32			u1u2;
 	u32			maximum_speed;
@@ -694,6 +713,9 @@ struct dwc3 {
 #define DWC3_REVISION_202A	0x5533202a
 #define DWC3_REVISION_210A	0x5533210a
 #define DWC3_REVISION_220A	0x5533220a
+#define DWC3_REVISION_230A	0x5533230a
+#define DWC3_REVISION_240A	0x5533240a
+#define DWC3_REVISION_250A	0x5533250a
 
 	unsigned		is_selfpowered:1;
 	unsigned		three_stage_setup:1;
@@ -704,11 +726,11 @@ struct dwc3 {
 	unsigned		delayed_status:1;
 	unsigned		needs_fifo_resize:1;
 	unsigned		resize_fifos:1;
+	unsigned		pullups_connected:1;
 
 	enum dwc3_ep0_next	ep0_next_event;
 	enum dwc3_ep0_state	ep0state;
 	enum dwc3_link_state	link_state;
-	enum dwc3_device_state	dev_state;
 
 	u16			isoch_delay;
 	u16			u2sel;
@@ -718,6 +740,9 @@ struct dwc3 {
 
 	u8			speed;
 
+	u8			num_out_eps;
+	u8			num_in_eps;
+
 	void			*mem;
 
 	struct dwc3_hwparams	hwparams;
@@ -884,4 +909,31 @@ static inline void dwc3_gadget_exit(struct dwc3 *dwc)
 { }
 #endif
 
+/* power management interface */
+#if !IS_ENABLED(CONFIG_USB_DWC3_HOST)
+int dwc3_gadget_prepare(struct dwc3 *dwc);
+void dwc3_gadget_complete(struct dwc3 *dwc);
+int dwc3_gadget_suspend(struct dwc3 *dwc);
+int dwc3_gadget_resume(struct dwc3 *dwc);
+#else
+static inline int dwc3_gadget_prepare(struct dwc3 *dwc)
+{
+	return 0;
+}
+
+static inline void dwc3_gadget_complete(struct dwc3 *dwc)
+{
+}
+
+static inline int dwc3_gadget_suspend(struct dwc3 *dwc)
+{
+	return 0;
+}
+
+static inline int dwc3_gadget_resume(struct dwc3 *dwc)
+{
+	return 0;
+}
+#endif /* !IS_ENABLED(CONFIG_USB_DWC3_HOST) */
+
 #endif /* __DRIVERS_USB_DWC3_CORE_H */
diff --git a/drivers/usb/dwc3/debugfs.c b/drivers/usb/dwc3/debugfs.c
index 4a752e730c5..9e9f122162f 100644
--- a/drivers/usb/dwc3/debugfs.c
+++ b/drivers/usb/dwc3/debugfs.c
@@ -59,7 +59,7 @@
 	.offset	= DWC3_ ##nm - DWC3_GLOBALS_REGS_START,	\
 }
 
-static struct debugfs_reg32 dwc3_regs[] = {
+static const struct debugfs_reg32 dwc3_regs[] = {
 	dump_register(GSBUSCFG0),
 	dump_register(GSBUSCFG1),
 	dump_register(GTXTHRCFG),
@@ -372,6 +372,7 @@ static struct debugfs_reg32 dwc3_regs[] = {
 
 	dump_register(OCFG),
 	dump_register(OCTL),
+	dump_register(OEVT),
 	dump_register(OEVTEN),
 	dump_register(OSTS),
 };
@@ -577,8 +578,14 @@ static int dwc3_link_state_show(struct seq_file *s, void *unused)
 	case DWC3_LINK_STATE_LPBK:
 		seq_printf(s, "Loopback\n");
 		break;
+	case DWC3_LINK_STATE_RESET:
+		seq_printf(s, "Reset\n");
+		break;
+	case DWC3_LINK_STATE_RESUME:
+		seq_printf(s, "Resume\n");
+		break;
 	default:
-		seq_printf(s, "UNKNOWN %d\n", reg);
+		seq_printf(s, "UNKNOWN %d\n", state);
 	}
 
 	return 0;
@@ -661,28 +668,31 @@ int dwc3_debugfs_init(struct dwc3 *dwc)
 		goto err1;
 	}
 
-#if IS_ENABLED(CONFIG_USB_DWC3_GADGET)
-	file = debugfs_create_file("mode", S_IRUGO | S_IWUSR, root,
-			dwc, &dwc3_mode_fops);
-	if (!file) {
-		ret = -ENOMEM;
-		goto err1;
-	}
-
-	file = debugfs_create_file("testmode", S_IRUGO | S_IWUSR, root,
-			dwc, &dwc3_testmode_fops);
-	if (!file) {
-		ret = -ENOMEM;
-		goto err1;
+	if (IS_ENABLED(CONFIG_USB_DWC3_DUAL_ROLE)) {
+		file = debugfs_create_file("mode", S_IRUGO | S_IWUSR, root,
+				dwc, &dwc3_mode_fops);
+		if (!file) {
+			ret = -ENOMEM;
+			goto err1;
+		}
 	}
 
-	file = debugfs_create_file("link_state", S_IRUGO | S_IWUSR, root,
-			dwc, &dwc3_link_state_fops);
-	if (!file) {
-		ret = -ENOMEM;
-		goto err1;
+	if (IS_ENABLED(CONFIG_USB_DWC3_DUAL_ROLE) ||
+			IS_ENABLED(CONFIG_USB_DWC3_GADGET)) {
+		file = debugfs_create_file("testmode", S_IRUGO | S_IWUSR, root,
+				dwc, &dwc3_testmode_fops);
+		if (!file) {
+			ret = -ENOMEM;
+			goto err1;
+		}
+
+		file = debugfs_create_file("link_state", S_IRUGO | S_IWUSR, root,
+				dwc, &dwc3_link_state_fops);
+		if (!file) {
+			ret = -ENOMEM;
+			goto err1;
+		}
 	}
-#endif
 
 	return 0;
 
diff --git a/drivers/usb/dwc3/dwc3-exynos.c b/drivers/usb/dwc3/dwc3-exynos.c
index b082bec7343..a8afe6e2662 100644
--- a/drivers/usb/dwc3/dwc3-exynos.c
+++ b/drivers/usb/dwc3/dwc3-exynos.c
@@ -22,9 +22,9 @@
 #include <linux/usb/otg.h>
 #include <linux/usb/nop-usb-xceiv.h>
 #include <linux/of.h>
+#include <linux/of_platform.h>
 
 struct dwc3_exynos {
-	struct platform_device	*dwc3;
 	struct platform_device	*usb2_phy;
 	struct platform_device	*usb3_phy;
 	struct device		*dev;
@@ -86,21 +86,30 @@ err1:
 	return ret;
 }
 
+static int dwc3_exynos_remove_child(struct device *dev, void *unused)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+
+	platform_device_unregister(pdev);
+
+	return 0;
+}
+
 static u64 dwc3_exynos_dma_mask = DMA_BIT_MASK(32);
 
 static int dwc3_exynos_probe(struct platform_device *pdev)
 {
-	struct platform_device	*dwc3;
 	struct dwc3_exynos	*exynos;
 	struct clk		*clk;
 	struct device		*dev = &pdev->dev;
+	struct device_node	*node = dev->of_node;
 
 	int			ret = -ENOMEM;
 
 	exynos = devm_kzalloc(dev, sizeof(*exynos), GFP_KERNEL);
 	if (!exynos) {
 		dev_err(dev, "not enough memory\n");
-		return -ENOMEM;
+		goto err1;
 	}
 
 	/*
@@ -108,21 +117,15 @@ static int dwc3_exynos_probe(struct platform_device *pdev)
 	 * Since shared usb code relies on it, set it here for now.
 	 * Once we move to full device tree support this will vanish off.
 	 */
-	if (!pdev->dev.dma_mask)
-		pdev->dev.dma_mask = &dwc3_exynos_dma_mask;
+	if (!dev->dma_mask)
+		dev->dma_mask = &dwc3_exynos_dma_mask;
 
 	platform_set_drvdata(pdev, exynos);
 
 	ret = dwc3_exynos_register_phys(exynos);
 	if (ret) {
 		dev_err(dev, "couldn't register PHYs\n");
-		return ret;
-	}
-
-	dwc3 = platform_device_alloc("dwc3", PLATFORM_DEVID_AUTO);
-	if (!dwc3) {
-		dev_err(dev, "couldn't allocate dwc3 device\n");
-		return -ENOMEM;
+		goto err1;
 	}
 
 	clk = devm_clk_get(dev, "usbdrd30");
@@ -132,37 +135,28 @@ static int dwc3_exynos_probe(struct platform_device *pdev)
 		goto err1;
 	}
 
-	dma_set_coherent_mask(&dwc3->dev, dev->coherent_dma_mask);
-
-	dwc3->dev.parent = dev;
-	dwc3->dev.dma_mask = dev->dma_mask;
-	dwc3->dev.dma_parms = dev->dma_parms;
-	exynos->dwc3	= dwc3;
 	exynos->dev	= dev;
 	exynos->clk	= clk;
 
-	clk_enable(exynos->clk);
-
-	ret = platform_device_add_resources(dwc3, pdev->resource,
-			pdev->num_resources);
-	if (ret) {
-		dev_err(dev, "couldn't add resources to dwc3 device\n");
-		goto err2;
-	}
-
-	ret = platform_device_add(dwc3);
-	if (ret) {
-		dev_err(dev, "failed to register dwc3 device\n");
+	clk_prepare_enable(exynos->clk);
+
+	if (node) {
+		ret = of_platform_populate(node, NULL, NULL, dev);
+		if (ret) {
+			dev_err(dev, "failed to add dwc3 core\n");
+			goto err2;
+		}
+	} else {
+		dev_err(dev, "no device node, failed to add dwc3 core\n");
+		ret = -ENODEV;
 		goto err2;
 	}
 
 	return 0;
 
 err2:
-	clk_disable(clk);
+	clk_disable_unprepare(clk);
 err1:
-	platform_device_put(dwc3);
-
 	return ret;
 }
 
@@ -170,11 +164,11 @@ static int dwc3_exynos_remove(struct platform_device *pdev)
 {
 	struct dwc3_exynos	*exynos = platform_get_drvdata(pdev);
 
-	platform_device_unregister(exynos->dwc3);
 	platform_device_unregister(exynos->usb2_phy);
 	platform_device_unregister(exynos->usb3_phy);
+	device_for_each_child(&pdev->dev, NULL, dwc3_exynos_remove_child);
 
-	clk_disable(exynos->clk);
+	clk_disable_unprepare(exynos->clk);
 
 	return 0;
 }
@@ -187,12 +181,46 @@ static const struct of_device_id exynos_dwc3_match[] = {
 MODULE_DEVICE_TABLE(of, exynos_dwc3_match);
 #endif
 
+#ifdef CONFIG_PM_SLEEP
+static int dwc3_exynos_suspend(struct device *dev)
+{
+	struct dwc3_exynos *exynos = dev_get_drvdata(dev);
+
+	clk_disable(exynos->clk);
+
+	return 0;
+}
+
+static int dwc3_exynos_resume(struct device *dev)
+{
+	struct dwc3_exynos *exynos = dev_get_drvdata(dev);
+
+	clk_enable(exynos->clk);
+
+	/* runtime set active to reflect active state. */
+	pm_runtime_disable(dev);
+	pm_runtime_set_active(dev);
+	pm_runtime_enable(dev);
+
+	return 0;
+}
+
+static const struct dev_pm_ops dwc3_exynos_dev_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(dwc3_exynos_suspend, dwc3_exynos_resume)
+};
+
+#define DEV_PM_OPS	(&dwc3_exynos_dev_pm_ops)
+#else
+#define DEV_PM_OPS	NULL
+#endif /* CONFIG_PM_SLEEP */
+
 static struct platform_driver dwc3_exynos_driver = {
 	.probe		= dwc3_exynos_probe,
 	.remove		= dwc3_exynos_remove,
 	.driver		= {
 		.name	= "exynos-dwc3",
 		.of_match_table = of_match_ptr(exynos_dwc3_match),
+		.pm	= DEV_PM_OPS,
 	},
 };
 
diff --git a/drivers/usb/dwc3/dwc3-omap.c b/drivers/usb/dwc3/dwc3-omap.c
index afa05e3c9cf..34638b92500 100644
--- a/drivers/usb/dwc3/dwc3-omap.c
+++ b/drivers/usb/dwc3/dwc3-omap.c
@@ -52,7 +52,6 @@
 #include <linux/of_platform.h>
 
 #include <linux/usb/otg.h>
-#include <linux/usb/nop-usb-xceiv.h>
 
 /*
  * All these registers belong to OMAP's Wrapper around the
@@ -117,20 +116,17 @@ struct dwc3_omap {
 	/* device lock */
 	spinlock_t		lock;
 
-	struct platform_device	*usb2_phy;
-	struct platform_device	*usb3_phy;
 	struct device		*dev;
 
 	int			irq;
 	void __iomem		*base;
 
-	void			*context;
-	u32			resource_size;
+	u32			utmi_otg_status;
 
 	u32			dma_status:1;
 };
 
-struct dwc3_omap		*_omap;
+static struct dwc3_omap		*_omap;
 
 static inline u32 dwc3_omap_readl(void __iomem *base, u32 offset)
 {
@@ -142,11 +138,14 @@ static inline void dwc3_omap_writel(void __iomem *base, u32 offset, u32 value)
 	writel(value, base + offset);
 }
 
-void dwc3_omap_mailbox(enum omap_dwc3_vbus_id_status status)
+int dwc3_omap_mailbox(enum omap_dwc3_vbus_id_status status)
 {
 	u32			val;
 	struct dwc3_omap	*omap = _omap;
 
+	if (!omap)
+		return -EPROBE_DEFER;
+
 	switch (status) {
 	case OMAP_DWC3_ID_GROUND:
 		dev_dbg(omap->dev, "ID GND\n");
@@ -189,63 +188,9 @@ void dwc3_omap_mailbox(enum omap_dwc3_vbus_id_status status)
 		dev_dbg(omap->dev, "ID float\n");
 	}
 
-	return;
-}
-EXPORT_SYMBOL_GPL(dwc3_omap_mailbox);
-
-static int dwc3_omap_register_phys(struct dwc3_omap *omap)
-{
-	struct nop_usb_xceiv_platform_data pdata;
-	struct platform_device	*pdev;
-	int			ret;
-
-	memset(&pdata, 0x00, sizeof(pdata));
-
-	pdev = platform_device_alloc("nop_usb_xceiv", PLATFORM_DEVID_AUTO);
-	if (!pdev)
-		return -ENOMEM;
-
-	omap->usb2_phy = pdev;
-	pdata.type = USB_PHY_TYPE_USB2;
-
-	ret = platform_device_add_data(omap->usb2_phy, &pdata, sizeof(pdata));
-	if (ret)
-		goto err1;
-
-	pdev = platform_device_alloc("nop_usb_xceiv", PLATFORM_DEVID_AUTO);
-	if (!pdev) {
-		ret = -ENOMEM;
-		goto err1;
-	}
-
-	omap->usb3_phy = pdev;
-	pdata.type = USB_PHY_TYPE_USB3;
-
-	ret = platform_device_add_data(omap->usb3_phy, &pdata, sizeof(pdata));
-	if (ret)
-		goto err2;
-
-	ret = platform_device_add(omap->usb2_phy);
-	if (ret)
-		goto err2;
-
-	ret = platform_device_add(omap->usb3_phy);
-	if (ret)
-		goto err3;
-
 	return 0;
-
-err3:
-	platform_device_del(omap->usb2_phy);
-
-err2:
-	platform_device_put(omap->usb3_phy);
-
-err1:
-	platform_device_put(omap->usb2_phy);
-
-	return ret;
 }
+EXPORT_SYMBOL_GPL(dwc3_omap_mailbox);
 
 static irqreturn_t dwc3_omap_interrupt(int irq, void *_omap)
 {
@@ -307,24 +252,57 @@ static int dwc3_omap_remove_core(struct device *dev, void *c)
 	return 0;
 }
 
+static void dwc3_omap_enable_irqs(struct dwc3_omap *omap)
+{
+	u32			reg;
+
+	/* enable all IRQs */
+	reg = USBOTGSS_IRQO_COREIRQ_ST;
+	dwc3_omap_writel(omap->base, USBOTGSS_IRQENABLE_SET_0, reg);
+
+	reg = (USBOTGSS_IRQ1_OEVT |
+			USBOTGSS_IRQ1_DRVVBUS_RISE |
+			USBOTGSS_IRQ1_CHRGVBUS_RISE |
+			USBOTGSS_IRQ1_DISCHRGVBUS_RISE |
+			USBOTGSS_IRQ1_IDPULLUP_RISE |
+			USBOTGSS_IRQ1_DRVVBUS_FALL |
+			USBOTGSS_IRQ1_CHRGVBUS_FALL |
+			USBOTGSS_IRQ1_DISCHRGVBUS_FALL |
+			USBOTGSS_IRQ1_IDPULLUP_FALL);
+
+	dwc3_omap_writel(omap->base, USBOTGSS_IRQENABLE_SET_1, reg);
+}
+
+static void dwc3_omap_disable_irqs(struct dwc3_omap *omap)
+{
+	/* disable all IRQs */
+	dwc3_omap_writel(omap->base, USBOTGSS_IRQENABLE_SET_1, 0x00);
+	dwc3_omap_writel(omap->base, USBOTGSS_IRQENABLE_SET_0, 0x00);
+}
+
+static u64 dwc3_omap_dma_mask = DMA_BIT_MASK(32);
+
 static int dwc3_omap_probe(struct platform_device *pdev)
 {
-	struct dwc3_omap_data	*pdata = pdev->dev.platform_data;
 	struct device_node	*node = pdev->dev.of_node;
 
 	struct dwc3_omap	*omap;
 	struct resource		*res;
 	struct device		*dev = &pdev->dev;
 
-	int			size;
 	int			ret = -ENOMEM;
 	int			irq;
 
-	const u32		*utmi_mode;
+	int			utmi_mode = 0;
+
 	u32			reg;
 
 	void __iomem		*base;
-	void			*context;
+
+	if (!node) {
+		dev_err(dev, "device node not found\n");
+		return -EINVAL;
+	}
 
 	omap = devm_kzalloc(dev, sizeof(*omap), GFP_KERNEL);
 	if (!omap) {
@@ -334,13 +312,13 @@ static int dwc3_omap_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, omap);
 
-	irq = platform_get_irq(pdev, 1);
+	irq = platform_get_irq(pdev, 0);
 	if (irq < 0) {
 		dev_err(dev, "missing IRQ resource\n");
 		return -EINVAL;
 	}
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
 		dev_err(dev, "missing memory base resource\n");
 		return -EINVAL;
@@ -352,25 +330,12 @@ static int dwc3_omap_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
-	ret = dwc3_omap_register_phys(omap);
-	if (ret) {
-		dev_err(dev, "couldn't register PHYs\n");
-		return ret;
-	}
-
-	context = devm_kzalloc(dev, resource_size(res), GFP_KERNEL);
-	if (!context) {
-		dev_err(dev, "couldn't allocate dwc3 context memory\n");
-		return -ENOMEM;
-	}
-
 	spin_lock_init(&omap->lock);
 
-	omap->resource_size = resource_size(res);
-	omap->context	= context;
 	omap->dev	= dev;
 	omap->irq	= irq;
 	omap->base	= base;
+	dev->dma_mask	= &dwc3_omap_dma_mask;
 
 	/*
 	 * REVISIT if we ever have two instances of the wrapper, we will be
@@ -387,25 +352,17 @@ static int dwc3_omap_probe(struct platform_device *pdev)
 
 	reg = dwc3_omap_readl(omap->base, USBOTGSS_UTMI_OTG_STATUS);
 
-	utmi_mode = of_get_property(node, "utmi-mode", &size);
-	if (utmi_mode && size == sizeof(*utmi_mode)) {
-		reg |= *utmi_mode;
-	} else {
-		if (!pdata) {
-			dev_dbg(dev, "missing platform data\n");
-		} else {
-			switch (pdata->utmi_mode) {
-			case DWC3_OMAP_UTMI_MODE_SW:
-				reg |= USBOTGSS_UTMI_OTG_STATUS_SW_MODE;
-				break;
-			case DWC3_OMAP_UTMI_MODE_HW:
-				reg &= ~USBOTGSS_UTMI_OTG_STATUS_SW_MODE;
-				break;
-			default:
-				dev_dbg(dev, "UNKNOWN utmi mode %d\n",
-						pdata->utmi_mode);
-			}
-		}
+	of_property_read_u32(node, "utmi-mode", &utmi_mode);
+
+	switch (utmi_mode) {
+	case DWC3_OMAP_UTMI_MODE_SW:
+		reg |= USBOTGSS_UTMI_OTG_STATUS_SW_MODE;
+		break;
+	case DWC3_OMAP_UTMI_MODE_HW:
+		reg &= ~USBOTGSS_UTMI_OTG_STATUS_SW_MODE;
+		break;
+	default:
+		dev_dbg(dev, "UNKNOWN utmi mode %d\n", utmi_mode);
 	}
 
 	dwc3_omap_writel(omap->base, USBOTGSS_UTMI_OTG_STATUS, reg);
@@ -422,29 +379,12 @@ static int dwc3_omap_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	/* enable all IRQs */
-	reg = USBOTGSS_IRQO_COREIRQ_ST;
-	dwc3_omap_writel(omap->base, USBOTGSS_IRQENABLE_SET_0, reg);
+	dwc3_omap_enable_irqs(omap);
 
-	reg = (USBOTGSS_IRQ1_OEVT |
-			USBOTGSS_IRQ1_DRVVBUS_RISE |
-			USBOTGSS_IRQ1_CHRGVBUS_RISE |
-			USBOTGSS_IRQ1_DISCHRGVBUS_RISE |
-			USBOTGSS_IRQ1_IDPULLUP_RISE |
-			USBOTGSS_IRQ1_DRVVBUS_FALL |
-			USBOTGSS_IRQ1_CHRGVBUS_FALL |
-			USBOTGSS_IRQ1_DISCHRGVBUS_FALL |
-			USBOTGSS_IRQ1_IDPULLUP_FALL);
-
-	dwc3_omap_writel(omap->base, USBOTGSS_IRQENABLE_SET_1, reg);
-
-	if (node) {
-		ret = of_platform_populate(node, NULL, NULL, dev);
-		if (ret) {
-			dev_err(&pdev->dev,
-				"failed to add create dwc3 core\n");
-			return ret;
-		}
+	ret = of_platform_populate(node, NULL, NULL, dev);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to create dwc3 core\n");
+		return ret;
 	}
 
 	return 0;
@@ -454,8 +394,7 @@ static int dwc3_omap_remove(struct platform_device *pdev)
 {
 	struct dwc3_omap	*omap = platform_get_drvdata(pdev);
 
-	platform_device_unregister(omap->usb2_phy);
-	platform_device_unregister(omap->usb3_phy);
+	dwc3_omap_disable_irqs(omap);
 	pm_runtime_put_sync(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
 	device_for_each_child(&pdev->dev, NULL, dwc3_omap_remove_core);
@@ -465,18 +404,72 @@ static int dwc3_omap_remove(struct platform_device *pdev)
 
 static const struct of_device_id of_dwc3_match[] = {
 	{
-		"ti,dwc3",
+		.compatible =	"ti,dwc3"
 	},
 	{ },
 };
 MODULE_DEVICE_TABLE(of, of_dwc3_match);
 
+#ifdef CONFIG_PM_SLEEP
+static int dwc3_omap_prepare(struct device *dev)
+{
+	struct dwc3_omap	*omap = dev_get_drvdata(dev);
+
+	dwc3_omap_disable_irqs(omap);
+
+	return 0;
+}
+
+static void dwc3_omap_complete(struct device *dev)
+{
+	struct dwc3_omap	*omap = dev_get_drvdata(dev);
+
+	dwc3_omap_enable_irqs(omap);
+}
+
+static int dwc3_omap_suspend(struct device *dev)
+{
+	struct dwc3_omap	*omap = dev_get_drvdata(dev);
+
+	omap->utmi_otg_status = dwc3_omap_readl(omap->base,
+			USBOTGSS_UTMI_OTG_STATUS);
+
+	return 0;
+}
+
+static int dwc3_omap_resume(struct device *dev)
+{
+	struct dwc3_omap	*omap = dev_get_drvdata(dev);
+
+	dwc3_omap_writel(omap->base, USBOTGSS_UTMI_OTG_STATUS,
+			omap->utmi_otg_status);
+
+	pm_runtime_disable(dev);
+	pm_runtime_set_active(dev);
+	pm_runtime_enable(dev);
+
+	return 0;
+}
+
+static const struct dev_pm_ops dwc3_omap_dev_pm_ops = {
+	.prepare	= dwc3_omap_prepare,
+	.complete	= dwc3_omap_complete,
+
+	SET_SYSTEM_SLEEP_PM_OPS(dwc3_omap_suspend, dwc3_omap_resume)
+};
+
+#define DEV_PM_OPS	(&dwc3_omap_dev_pm_ops)
+#else
+#define DEV_PM_OPS	NULL
+#endif /* CONFIG_PM_SLEEP */
+
 static struct platform_driver dwc3_omap_driver = {
 	.probe		= dwc3_omap_probe,
 	.remove		= dwc3_omap_remove,
 	.driver		= {
 		.name	= "omap-dwc3",
 		.of_match_table	= of_dwc3_match,
+		.pm	= DEV_PM_OPS,
 	},
 };
 
diff --git a/drivers/usb/dwc3/dwc3-pci.c b/drivers/usb/dwc3/dwc3-pci.c
index e8d77689a32..227d4a7acad 100644
--- a/drivers/usb/dwc3/dwc3-pci.c
+++ b/drivers/usb/dwc3/dwc3-pci.c
@@ -212,11 +212,49 @@ static DEFINE_PCI_DEVICE_TABLE(dwc3_pci_id_table) = {
 };
 MODULE_DEVICE_TABLE(pci, dwc3_pci_id_table);
 
+#ifdef CONFIG_PM
+static int dwc3_pci_suspend(struct device *dev)
+{
+	struct pci_dev	*pci = to_pci_dev(dev);
+
+	pci_disable_device(pci);
+
+	return 0;
+}
+
+static int dwc3_pci_resume(struct device *dev)
+{
+	struct pci_dev	*pci = to_pci_dev(dev);
+	int		ret;
+
+	ret = pci_enable_device(pci);
+	if (ret) {
+		dev_err(dev, "can't re-enable device --> %d\n", ret);
+		return ret;
+	}
+
+	pci_set_master(pci);
+
+	return 0;
+}
+
+static const struct dev_pm_ops dwc3_pci_dev_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(dwc3_pci_suspend, dwc3_pci_resume)
+};
+
+#define DEV_PM_OPS	(&dwc3_pci_dev_pm_ops)
+#else
+#define DEV_PM_OPS	NULL
+#endif /* CONFIG_PM */
+
 static struct pci_driver dwc3_pci_driver = {
 	.name		= "dwc3-pci",
 	.id_table	= dwc3_pci_id_table,
 	.probe		= dwc3_pci_probe,
 	.remove		= dwc3_pci_remove,
+	.driver		= {
+		.pm	= DEV_PM_OPS,
+	},
 };
 
 MODULE_AUTHOR("Felipe Balbi <balbi@ti.com>");
diff --git a/drivers/usb/dwc3/ep0.c b/drivers/usb/dwc3/ep0.c
index 1d139ca05ef..5acbb948b70 100644
--- a/drivers/usb/dwc3/ep0.c
+++ b/drivers/usb/dwc3/ep0.c
@@ -394,10 +394,13 @@ static int dwc3_ep0_handle_feature(struct dwc3 *dwc,
 	u32			wIndex;
 	u32			reg;
 	int			ret;
+	enum usb_device_state	state;
 
 	wValue = le16_to_cpu(ctrl->wValue);
 	wIndex = le16_to_cpu(ctrl->wIndex);
 	recip = ctrl->bRequestType & USB_RECIP_MASK;
+	state = dwc->gadget.state;
+
 	switch (recip) {
 	case USB_RECIP_DEVICE:
 
@@ -409,7 +412,7 @@ static int dwc3_ep0_handle_feature(struct dwc3 *dwc,
 		 * default control pipe
 		 */
 		case USB_DEVICE_U1_ENABLE:
-			if (dwc->dev_state != DWC3_CONFIGURED_STATE)
+			if (state != USB_STATE_CONFIGURED)
 				return -EINVAL;
 			if (dwc->speed != DWC3_DSTS_SUPERSPEED)
 				return -EINVAL;
@@ -423,7 +426,7 @@ static int dwc3_ep0_handle_feature(struct dwc3 *dwc,
 			break;
 
 		case USB_DEVICE_U2_ENABLE:
-			if (dwc->dev_state != DWC3_CONFIGURED_STATE)
+			if (state != USB_STATE_CONFIGURED)
 				return -EINVAL;
 			if (dwc->speed != DWC3_DSTS_SUPERSPEED)
 				return -EINVAL;
@@ -493,6 +496,7 @@ static int dwc3_ep0_handle_feature(struct dwc3 *dwc,
 
 static int dwc3_ep0_set_address(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
 {
+	enum usb_device_state state = dwc->gadget.state;
 	u32 addr;
 	u32 reg;
 
@@ -502,7 +506,7 @@ static int dwc3_ep0_set_address(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
 		return -EINVAL;
 	}
 
-	if (dwc->dev_state == DWC3_CONFIGURED_STATE) {
+	if (state == USB_STATE_CONFIGURED) {
 		dev_dbg(dwc->dev, "trying to set address when configured\n");
 		return -EINVAL;
 	}
@@ -513,9 +517,9 @@ static int dwc3_ep0_set_address(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
 	dwc3_writel(dwc->regs, DWC3_DCFG, reg);
 
 	if (addr)
-		dwc->dev_state = DWC3_ADDRESS_STATE;
+		usb_gadget_set_state(&dwc->gadget, USB_STATE_ADDRESS);
 	else
-		dwc->dev_state = DWC3_DEFAULT_STATE;
+		usb_gadget_set_state(&dwc->gadget, USB_STATE_DEFAULT);
 
 	return 0;
 }
@@ -532,6 +536,7 @@ static int dwc3_ep0_delegate_req(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
 
 static int dwc3_ep0_set_config(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
 {
+	enum usb_device_state state = dwc->gadget.state;
 	u32 cfg;
 	int ret;
 	u32 reg;
@@ -539,16 +544,18 @@ static int dwc3_ep0_set_config(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
 	dwc->start_config_issued = false;
 	cfg = le16_to_cpu(ctrl->wValue);
 
-	switch (dwc->dev_state) {
-	case DWC3_DEFAULT_STATE:
+	switch (state) {
+	case USB_STATE_DEFAULT:
 		return -EINVAL;
 		break;
 
-	case DWC3_ADDRESS_STATE:
+	case USB_STATE_ADDRESS:
 		ret = dwc3_ep0_delegate_req(dwc, ctrl);
 		/* if the cfg matches and the cfg is non zero */
 		if (cfg && (!ret || (ret == USB_GADGET_DELAYED_STATUS))) {
-			dwc->dev_state = DWC3_CONFIGURED_STATE;
+			usb_gadget_set_state(&dwc->gadget,
+					USB_STATE_CONFIGURED);
+
 			/*
 			 * Enable transition to U1/U2 state when
 			 * nothing is pending from application.
@@ -562,10 +569,11 @@ static int dwc3_ep0_set_config(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
 		}
 		break;
 
-	case DWC3_CONFIGURED_STATE:
+	case USB_STATE_CONFIGURED:
 		ret = dwc3_ep0_delegate_req(dwc, ctrl);
 		if (!cfg)
-			dwc->dev_state = DWC3_ADDRESS_STATE;
+			usb_gadget_set_state(&dwc->gadget,
+					USB_STATE_ADDRESS);
 		break;
 	default:
 		ret = -EINVAL;
@@ -620,10 +628,11 @@ static void dwc3_ep0_set_sel_cmpl(struct usb_ep *ep, struct usb_request *req)
 static int dwc3_ep0_set_sel(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
 {
 	struct dwc3_ep	*dep;
+	enum usb_device_state state = dwc->gadget.state;
 	u16		wLength;
 	u16		wValue;
 
-	if (dwc->dev_state == DWC3_DEFAULT_STATE)
+	if (state == USB_STATE_DEFAULT)
 		return -EINVAL;
 
 	wValue = le16_to_cpu(ctrl->wValue);
diff --git a/drivers/usb/dwc3/gadget.c b/drivers/usb/dwc3/gadget.c
index 82e160e96fc..2b6e7e00120 100644
--- a/drivers/usb/dwc3/gadget.c
+++ b/drivers/usb/dwc3/gadget.c
@@ -1425,8 +1425,10 @@ static int dwc3_gadget_run_stop(struct dwc3 *dwc, int is_on)
 		if (dwc->revision >= DWC3_REVISION_194A)
 			reg &= ~DWC3_DCTL_KEEP_CONNECT;
 		reg |= DWC3_DCTL_RUN_STOP;
+		dwc->pullups_connected = true;
 	} else {
 		reg &= ~DWC3_DCTL_RUN_STOP;
+		dwc->pullups_connected = false;
 	}
 
 	dwc3_writel(dwc->regs, DWC3_DCTL, reg);
@@ -1469,6 +1471,33 @@ static int dwc3_gadget_pullup(struct usb_gadget *g, int is_on)
 	return ret;
 }
 
+static void dwc3_gadget_enable_irq(struct dwc3 *dwc)
+{
+	u32			reg;
+
+	/* Enable all but Start and End of Frame IRQs */
+	reg = (DWC3_DEVTEN_VNDRDEVTSTRCVEDEN |
+			DWC3_DEVTEN_EVNTOVERFLOWEN |
+			DWC3_DEVTEN_CMDCMPLTEN |
+			DWC3_DEVTEN_ERRTICERREN |
+			DWC3_DEVTEN_WKUPEVTEN |
+			DWC3_DEVTEN_ULSTCNGEN |
+			DWC3_DEVTEN_CONNECTDONEEN |
+			DWC3_DEVTEN_USBRSTEN |
+			DWC3_DEVTEN_DISCONNEVTEN);
+
+	dwc3_writel(dwc->regs, DWC3_DEVTEN, reg);
+}
+
+static void dwc3_gadget_disable_irq(struct dwc3 *dwc)
+{
+	/* mask all interrupts */
+	dwc3_writel(dwc->regs, DWC3_DEVTEN, 0x00);
+}
+
+static irqreturn_t dwc3_interrupt(int irq, void *_dwc);
+static irqreturn_t dwc3_thread_interrupt(int irq, void *_dwc);
+
 static int dwc3_gadget_start(struct usb_gadget *g,
 		struct usb_gadget_driver *driver)
 {
@@ -1476,6 +1505,7 @@ static int dwc3_gadget_start(struct usb_gadget *g,
 	struct dwc3_ep		*dep;
 	unsigned long		flags;
 	int			ret = 0;
+	int			irq;
 	u32			reg;
 
 	spin_lock_irqsave(&dwc->lock, flags);
@@ -1489,7 +1519,6 @@ static int dwc3_gadget_start(struct usb_gadget *g,
 	}
 
 	dwc->gadget_driver	= driver;
-	dwc->gadget.dev.driver	= &driver->driver;
 
 	reg = dwc3_readl(dwc->regs, DWC3_DCFG);
 	reg &= ~(DWC3_DCFG_SPEED_MASK);
@@ -1536,6 +1565,17 @@ static int dwc3_gadget_start(struct usb_gadget *g,
 	dwc->ep0state = EP0_SETUP_PHASE;
 	dwc3_ep0_out_start(dwc);
 
+	irq = platform_get_irq(to_platform_device(dwc->dev), 0);
+	ret = request_threaded_irq(irq, dwc3_interrupt, dwc3_thread_interrupt,
+			IRQF_SHARED | IRQF_ONESHOT, "dwc3", dwc);
+	if (ret) {
+		dev_err(dwc->dev, "failed to request irq #%d --> %d\n",
+				irq, ret);
+		goto err1;
+	}
+
+	dwc3_gadget_enable_irq(dwc);
+
 	spin_unlock_irqrestore(&dwc->lock, flags);
 
 	return 0;
@@ -1554,14 +1594,18 @@ static int dwc3_gadget_stop(struct usb_gadget *g,
 {
 	struct dwc3		*dwc = gadget_to_dwc(g);
 	unsigned long		flags;
+	int			irq;
 
 	spin_lock_irqsave(&dwc->lock, flags);
 
+	dwc3_gadget_disable_irq(dwc);
+	irq = platform_get_irq(to_platform_device(dwc->dev), 0);
+	free_irq(irq, dwc);
+
 	__dwc3_gadget_ep_disable(dwc->eps[0]);
 	__dwc3_gadget_ep_disable(dwc->eps[1]);
 
 	dwc->gadget_driver	= NULL;
-	dwc->gadget.dev.driver	= NULL;
 
 	spin_unlock_irqrestore(&dwc->lock, flags);
 
@@ -1579,14 +1623,15 @@ static const struct usb_gadget_ops dwc3_gadget_ops = {
 
 /* -------------------------------------------------------------------------- */
 
-static int dwc3_gadget_init_endpoints(struct dwc3 *dwc)
+static int dwc3_gadget_init_hw_endpoints(struct dwc3 *dwc,
+		u8 num, u32 direction)
 {
 	struct dwc3_ep			*dep;
-	u8				epnum;
+	u8				i;
 
-	INIT_LIST_HEAD(&dwc->gadget.ep_list);
+	for (i = 0; i < num; i++) {
+		u8 epnum = (i << 1) | (!!direction);
 
-	for (epnum = 0; epnum < DWC3_ENDPOINTS_NUM; epnum++) {
 		dep = kzalloc(sizeof(*dep), GFP_KERNEL);
 		if (!dep) {
 			dev_err(dwc->dev, "can't allocate endpoint %d\n",
@@ -1600,6 +1645,7 @@ static int dwc3_gadget_init_endpoints(struct dwc3 *dwc)
 
 		snprintf(dep->name, sizeof(dep->name), "ep%d%s", epnum >> 1,
 				(epnum & 1) ? "in" : "out");
+
 		dep->endpoint.name = dep->name;
 		dep->direction = (epnum & 1);
 
@@ -1630,6 +1676,27 @@ static int dwc3_gadget_init_endpoints(struct dwc3 *dwc)
 	return 0;
 }
 
+static int dwc3_gadget_init_endpoints(struct dwc3 *dwc)
+{
+	int				ret;
+
+	INIT_LIST_HEAD(&dwc->gadget.ep_list);
+
+	ret = dwc3_gadget_init_hw_endpoints(dwc, dwc->num_out_eps, 0);
+	if (ret < 0) {
+		dev_vdbg(dwc->dev, "failed to allocate OUT endpoints\n");
+		return ret;
+	}
+
+	ret = dwc3_gadget_init_hw_endpoints(dwc, dwc->num_in_eps, 1);
+	if (ret < 0) {
+		dev_vdbg(dwc->dev, "failed to allocate IN endpoints\n");
+		return ret;
+	}
+
+	return 0;
+}
+
 static void dwc3_gadget_free_endpoints(struct dwc3 *dwc)
 {
 	struct dwc3_ep			*dep;
@@ -1637,6 +1704,9 @@ static void dwc3_gadget_free_endpoints(struct dwc3 *dwc)
 
 	for (epnum = 0; epnum < DWC3_ENDPOINTS_NUM; epnum++) {
 		dep = dwc->eps[epnum];
+		if (!dep)
+			continue;
+
 		dwc3_free_trb_pool(dep);
 
 		if (epnum != 0 && epnum != 1)
@@ -1646,12 +1716,8 @@ static void dwc3_gadget_free_endpoints(struct dwc3 *dwc)
 	}
 }
 
-static void dwc3_gadget_release(struct device *dev)
-{
-	dev_dbg(dev, "%s\n", __func__);
-}
-
 /* -------------------------------------------------------------------------- */
+
 static int __dwc3_cleanup_done_trbs(struct dwc3 *dwc, struct dwc3_ep *dep,
 		struct dwc3_request *req, struct dwc3_trb *trb,
 		const struct dwc3_event_depevt *event, int status)
@@ -1975,6 +2041,9 @@ static void dwc3_stop_active_transfers(struct dwc3 *dwc)
 		struct dwc3_ep *dep;
 
 		dep = dwc->eps[epnum];
+		if (!dep)
+			continue;
+
 		if (!(dep->flags & DWC3_EP_ENABLED))
 			continue;
 
@@ -1992,6 +2061,8 @@ static void dwc3_clear_stall_all_ep(struct dwc3 *dwc)
 		int ret;
 
 		dep = dwc->eps[epnum];
+		if (!dep)
+			continue;
 
 		if (!(dep->flags & DWC3_EP_STALL))
 			continue;
@@ -2091,7 +2162,7 @@ static void dwc3_gadget_reset_interrupt(struct dwc3 *dwc)
 	}
 
 	/* after reset -> Default State */
-	dwc->dev_state = DWC3_DEFAULT_STATE;
+	usb_gadget_set_state(&dwc->gadget, USB_STATE_DEFAULT);
 
 	/* Recent versions support automatic phy suspend and don't need this */
 	if (dwc->revision < DWC3_REVISION_194A) {
@@ -2277,6 +2348,34 @@ static void dwc3_gadget_linksts_change_interrupt(struct dwc3 *dwc,
 		unsigned int evtinfo)
 {
 	enum dwc3_link_state	next = evtinfo & DWC3_LINK_STATE_MASK;
+	unsigned int		pwropt;
+
+	/*
+	 * WORKAROUND: DWC3 < 2.50a have an issue when configured without
+	 * Hibernation mode enabled which would show up when device detects
+	 * host-initiated U3 exit.
+	 *
+	 * In that case, device will generate a Link State Change Interrupt
+	 * from U3 to RESUME which is only necessary if Hibernation is
+	 * configured in.
+	 *
+	 * There are no functional changes due to such spurious event and we
+	 * just need to ignore it.
+	 *
+	 * Refers to:
+	 *
+	 * STAR#9000570034 RTL: SS Resume event generated in non-Hibernation
+	 * operational mode
+	 */
+	pwropt = DWC3_GHWPARAMS1_EN_PWROPT(dwc->hwparams.hwparams1);
+	if ((dwc->revision < DWC3_REVISION_250A) &&
+			(pwropt != DWC3_GHWPARAMS1_EN_PWROPT_HIB)) {
+		if ((dwc->link_state == DWC3_LINK_STATE_U3) &&
+				(next == DWC3_LINK_STATE_RESUME)) {
+			dev_vdbg(dwc->dev, "ignoring transition U3 -> Resume\n");
+			return;
+		}
+	}
 
 	/*
 	 * WORKAROUND: DWC3 Revisions <1.83a have an issue which, depending
@@ -2387,40 +2486,73 @@ static void dwc3_process_event_entry(struct dwc3 *dwc,
 	}
 }
 
+static irqreturn_t dwc3_thread_interrupt(int irq, void *_dwc)
+{
+	struct dwc3 *dwc = _dwc;
+	unsigned long flags;
+	irqreturn_t ret = IRQ_NONE;
+	int i;
+
+	spin_lock_irqsave(&dwc->lock, flags);
+
+	for (i = 0; i < dwc->num_event_buffers; i++) {
+		struct dwc3_event_buffer *evt;
+		int			left;
+
+		evt = dwc->ev_buffs[i];
+		left = evt->count;
+
+		if (!(evt->flags & DWC3_EVENT_PENDING))
+			continue;
+
+		while (left > 0) {
+			union dwc3_event event;
+
+			event.raw = *(u32 *) (evt->buf + evt->lpos);
+
+			dwc3_process_event_entry(dwc, &event);
+
+			/*
+			 * FIXME we wrap around correctly to the next entry as
+			 * almost all entries are 4 bytes in size. There is one
+			 * entry which has 12 bytes which is a regular entry
+			 * followed by 8 bytes data. ATM I don't know how
+			 * things are organized if we get next to the a
+			 * boundary so I worry about that once we try to handle
+			 * that.
+			 */
+			evt->lpos = (evt->lpos + 4) % DWC3_EVENT_BUFFERS_SIZE;
+			left -= 4;
+
+			dwc3_writel(dwc->regs, DWC3_GEVNTCOUNT(i), 4);
+		}
+
+		evt->count = 0;
+		evt->flags &= ~DWC3_EVENT_PENDING;
+		ret = IRQ_HANDLED;
+	}
+
+	spin_unlock_irqrestore(&dwc->lock, flags);
+
+	return ret;
+}
+
 static irqreturn_t dwc3_process_event_buf(struct dwc3 *dwc, u32 buf)
 {
 	struct dwc3_event_buffer *evt;
-	int left;
 	u32 count;
 
+	evt = dwc->ev_buffs[buf];
+
 	count = dwc3_readl(dwc->regs, DWC3_GEVNTCOUNT(buf));
 	count &= DWC3_GEVNTCOUNT_MASK;
 	if (!count)
 		return IRQ_NONE;
 
-	evt = dwc->ev_buffs[buf];
-	left = count;
-
-	while (left > 0) {
-		union dwc3_event event;
-
-		event.raw = *(u32 *) (evt->buf + evt->lpos);
+	evt->count = count;
+	evt->flags |= DWC3_EVENT_PENDING;
 
-		dwc3_process_event_entry(dwc, &event);
-		/*
-		 * XXX we wrap around correctly to the next entry as almost all
-		 * entries are 4 bytes in size. There is one entry which has 12
-		 * bytes which is a regular entry followed by 8 bytes data. ATM
-		 * I don't know how things are organized if were get next to the
-		 * a boundary so I worry about that once we try to handle that.
-		 */
-		evt->lpos = (evt->lpos + 4) % DWC3_EVENT_BUFFERS_SIZE;
-		left -= 4;
-
-		dwc3_writel(dwc->regs, DWC3_GEVNTCOUNT(buf), 4);
-	}
-
-	return IRQ_HANDLED;
+	return IRQ_WAKE_THREAD;
 }
 
 static irqreturn_t dwc3_interrupt(int irq, void *_dwc)
@@ -2435,7 +2567,7 @@ static irqreturn_t dwc3_interrupt(int irq, void *_dwc)
 		irqreturn_t status;
 
 		status = dwc3_process_event_buf(dwc, i);
-		if (status == IRQ_HANDLED)
+		if (status == IRQ_WAKE_THREAD)
 			ret = status;
 	}
 
@@ -2454,7 +2586,6 @@ int dwc3_gadget_init(struct dwc3 *dwc)
 {
 	u32					reg;
 	int					ret;
-	int					irq;
 
 	dwc->ctrl_req = dma_alloc_coherent(dwc->dev, sizeof(*dwc->ctrl_req),
 			&dwc->ctrl_req_addr, GFP_KERNEL);
@@ -2488,19 +2619,10 @@ int dwc3_gadget_init(struct dwc3 *dwc)
 		goto err3;
 	}
 
-	dev_set_name(&dwc->gadget.dev, "gadget");
-
 	dwc->gadget.ops			= &dwc3_gadget_ops;
 	dwc->gadget.max_speed		= USB_SPEED_SUPER;
 	dwc->gadget.speed		= USB_SPEED_UNKNOWN;
-	dwc->gadget.dev.parent		= dwc->dev;
 	dwc->gadget.sg_supported	= true;
-
-	dma_set_coherent_mask(&dwc->gadget.dev, dwc->dev->coherent_dma_mask);
-
-	dwc->gadget.dev.dma_parms	= dwc->dev->dma_parms;
-	dwc->gadget.dev.dma_mask	= dwc->dev->dma_mask;
-	dwc->gadget.dev.release		= dwc3_gadget_release;
 	dwc->gadget.name		= "dwc3-gadget";
 
 	/*
@@ -2512,60 +2634,24 @@ int dwc3_gadget_init(struct dwc3 *dwc)
 	if (ret)
 		goto err4;
 
-	irq = platform_get_irq(to_platform_device(dwc->dev), 0);
-
-	ret = request_irq(irq, dwc3_interrupt, IRQF_SHARED,
-			"dwc3", dwc);
-	if (ret) {
-		dev_err(dwc->dev, "failed to request irq #%d --> %d\n",
-				irq, ret);
-		goto err5;
-	}
-
 	reg = dwc3_readl(dwc->regs, DWC3_DCFG);
 	reg |= DWC3_DCFG_LPM_CAP;
 	dwc3_writel(dwc->regs, DWC3_DCFG, reg);
 
-	/* Enable all but Start and End of Frame IRQs */
-	reg = (DWC3_DEVTEN_VNDRDEVTSTRCVEDEN |
-			DWC3_DEVTEN_EVNTOVERFLOWEN |
-			DWC3_DEVTEN_CMDCMPLTEN |
-			DWC3_DEVTEN_ERRTICERREN |
-			DWC3_DEVTEN_WKUPEVTEN |
-			DWC3_DEVTEN_ULSTCNGEN |
-			DWC3_DEVTEN_CONNECTDONEEN |
-			DWC3_DEVTEN_USBRSTEN |
-			DWC3_DEVTEN_DISCONNEVTEN);
-	dwc3_writel(dwc->regs, DWC3_DEVTEN, reg);
-
-	/* automatic phy suspend only on recent versions */
+	/* Enable USB2 LPM and automatic phy suspend only on recent versions */
 	if (dwc->revision >= DWC3_REVISION_194A) {
 		dwc3_gadget_usb2_phy_suspend(dwc, false);
 		dwc3_gadget_usb3_phy_suspend(dwc, false);
 	}
 
-	ret = device_register(&dwc->gadget.dev);
-	if (ret) {
-		dev_err(dwc->dev, "failed to register gadget device\n");
-		put_device(&dwc->gadget.dev);
-		goto err6;
-	}
-
 	ret = usb_add_gadget_udc(dwc->dev, &dwc->gadget);
 	if (ret) {
 		dev_err(dwc->dev, "failed to register udc\n");
-		goto err7;
+		goto err5;
 	}
 
 	return 0;
 
-err7:
-	device_unregister(&dwc->gadget.dev);
-
-err6:
-	dwc3_writel(dwc->regs, DWC3_DEVTEN, 0x00);
-	free_irq(irq, dwc);
-
 err5:
 	dwc3_gadget_free_endpoints(dwc);
 
@@ -2588,15 +2674,11 @@ err0:
 	return ret;
 }
 
+/* -------------------------------------------------------------------------- */
+
 void dwc3_gadget_exit(struct dwc3 *dwc)
 {
-	int			irq;
-
 	usb_del_gadget_udc(&dwc->gadget);
-	irq = platform_get_irq(to_platform_device(dwc->dev), 0);
-
-	dwc3_writel(dwc->regs, DWC3_DEVTEN, 0x00);
-	free_irq(irq, dwc);
 
 	dwc3_gadget_free_endpoints(dwc);
 
@@ -2610,6 +2692,63 @@ void dwc3_gadget_exit(struct dwc3 *dwc)
 
 	dma_free_coherent(dwc->dev, sizeof(*dwc->ctrl_req),
 			dwc->ctrl_req, dwc->ctrl_req_addr);
+}
 
-	device_unregister(&dwc->gadget.dev);
+int dwc3_gadget_prepare(struct dwc3 *dwc)
+{
+	if (dwc->pullups_connected)
+		dwc3_gadget_disable_irq(dwc);
+
+	return 0;
+}
+
+void dwc3_gadget_complete(struct dwc3 *dwc)
+{
+	if (dwc->pullups_connected) {
+		dwc3_gadget_enable_irq(dwc);
+		dwc3_gadget_run_stop(dwc, true);
+	}
+}
+
+int dwc3_gadget_suspend(struct dwc3 *dwc)
+{
+	__dwc3_gadget_ep_disable(dwc->eps[0]);
+	__dwc3_gadget_ep_disable(dwc->eps[1]);
+
+	dwc->dcfg = dwc3_readl(dwc->regs, DWC3_DCFG);
+
+	return 0;
+}
+
+int dwc3_gadget_resume(struct dwc3 *dwc)
+{
+	struct dwc3_ep		*dep;
+	int			ret;
+
+	/* Start with SuperSpeed Default */
+	dwc3_gadget_ep0_desc.wMaxPacketSize = cpu_to_le16(512);
+
+	dep = dwc->eps[0];
+	ret = __dwc3_gadget_ep_enable(dep, &dwc3_gadget_ep0_desc, NULL, false);
+	if (ret)
+		goto err0;
+
+	dep = dwc->eps[1];
+	ret = __dwc3_gadget_ep_enable(dep, &dwc3_gadget_ep0_desc, NULL, false);
+	if (ret)
+		goto err1;
+
+	/* begin to receive SETUP packets */
+	dwc->ep0state = EP0_SETUP_PHASE;
+	dwc3_ep0_out_start(dwc);
+
+	dwc3_writel(dwc->regs, DWC3_DCFG, dwc->dcfg);
+
+	return 0;
+
+err1:
+	__dwc3_gadget_ep_disable(dwc->eps[0]);
+
+err0:
+	return ret;
 }
diff --git a/drivers/usb/gadget/Kconfig b/drivers/usb/gadget/Kconfig
index 5a0c541daf8..83300d94a89 100644
--- a/drivers/usb/gadget/Kconfig
+++ b/drivers/usb/gadget/Kconfig
@@ -144,7 +144,9 @@ config USB_AT91
 config USB_LPC32XX
 	tristate "LPC32XX USB Peripheral Controller"
 	depends on ARCH_LPC32XX
+	depends on USB_PHY
 	select USB_ISP1301
+	select USB_OTG_UTILS
 	help
 	   This option selects the USB device controller in the LPC32xx SoC.
 
@@ -194,8 +196,8 @@ config USB_FUSB300
 config USB_OMAP
 	tristate "OMAP USB Device Controller"
 	depends on ARCH_OMAP1
+	depends on USB_PHY
 	select ISP1301_OMAP if MACH_OMAP_H2 || MACH_OMAP_H3 || MACH_OMAP_H4_OTG
-	select USB_OTG_UTILS if ARCH_OMAP
 	help
 	   Many Texas Instruments OMAP processors have flexible full
 	   speed USB device controllers, with support for up to 30
@@ -210,7 +212,6 @@ config USB_OMAP
 config USB_PXA25X
 	tristate "PXA 25x or IXP 4xx"
 	depends on (ARCH_PXA && PXA25x) || ARCH_IXP4XX
-	select USB_OTG_UTILS
 	help
 	   Intel's PXA 25x series XScale ARM-5TE processors include
 	   an integrated full speed USB 1.1 device controller.  The
@@ -258,8 +259,6 @@ config USB_RENESAS_USBHS_UDC
 
 config USB_PXA27X
 	tristate "PXA 27x"
-	depends on ARCH_PXA && (PXA27x || PXA3xx)
-	select USB_OTG_UTILS
 	help
 	   Intel's PXA 27x series XScale ARM v5TE processors include
 	   an integrated full speed USB 1.1 device controller.
@@ -328,9 +327,6 @@ config USB_MV_UDC
 
 config USB_MV_U3D
 	tristate "MARVELL PXA2128 USB 3.0 controller"
-	depends on CPU_MMP3
-	select USB_GADGET_DUALSPEED
-	select USB_GADGET_SUPERSPEED
 	help
 	  MARVELL PXA2128 Processor series include a super speed USB3.0 device
 	  controller, which support super speed USB peripheral.
@@ -500,6 +496,7 @@ endmenu
 # composite based drivers
 config USB_LIBCOMPOSITE
 	tristate
+	select CONFIGFS_FS
 	depends on USB_GADGET
 
 config USB_F_ACM
@@ -511,6 +508,12 @@ config USB_F_SS_LB
 config USB_U_SERIAL
 	tristate
 
+config USB_F_SERIAL
+	tristate
+
+config USB_F_OBEX
+	tristate
+
 choice
 	tristate "USB Gadget Drivers"
 	default USB_ETH
@@ -765,6 +768,8 @@ config USB_G_SERIAL
 	depends on TTY
 	select USB_U_SERIAL
 	select USB_F_ACM
+	select USB_F_SERIAL
+	select USB_F_OBEX
 	select USB_LIBCOMPOSITE
 	help
 	  The Serial Gadget talks to the Linux-USB generic serial driver.
@@ -838,6 +843,7 @@ config USB_G_NOKIA
 	depends on PHONET
 	select USB_LIBCOMPOSITE
 	select USB_U_SERIAL
+	select USB_F_ACM
 	help
 	  The Nokia composite gadget provides support for acm, obex
 	  and phonet in only one composite gadget driver.
@@ -956,6 +962,7 @@ config USB_G_WEBCAM
 	tristate "USB Webcam Gadget"
 	depends on VIDEO_DEV
 	select USB_LIBCOMPOSITE
+	select VIDEOBUF2_VMALLOC
 	help
 	  The Webcam Gadget acts as a composite USB Audio and Video Class
 	  device. It provides a userspace API to process UVC control requests
diff --git a/drivers/usb/gadget/Makefile b/drivers/usb/gadget/Makefile
index 82fb2251135..6afd16659e7 100644
--- a/drivers/usb/gadget/Makefile
+++ b/drivers/usb/gadget/Makefile
@@ -6,7 +6,7 @@ ccflags-$(CONFIG_USB_GADGET_DEBUG) := -DDEBUG
 obj-$(CONFIG_USB_GADGET)	+= udc-core.o
 obj-$(CONFIG_USB_LIBCOMPOSITE)	+= libcomposite.o
 libcomposite-y			:= usbstring.o config.o epautoconf.o
-libcomposite-y			+= composite.o functions.o
+libcomposite-y			+= composite.o functions.o configfs.o
 obj-$(CONFIG_USB_DUMMY_HCD)	+= dummy_hcd.o
 obj-$(CONFIG_USB_NET2272)	+= net2272.o
 obj-$(CONFIG_USB_NET2280)	+= net2280.o
@@ -36,10 +36,15 @@ obj-$(CONFIG_USB_FUSB300)	+= fusb300_udc.o
 obj-$(CONFIG_USB_MV_U3D)	+= mv_u3d_core.o
 
 # USB Functions
-obj-$(CONFIG_USB_F_ACM)		+= f_acm.o
-f_ss_lb-y			:= f_loopback.o f_sourcesink.o
-obj-$(CONFIG_USB_F_SS_LB)	+= f_ss_lb.o
+usb_f_acm-y			:= f_acm.o
+obj-$(CONFIG_USB_F_ACM)		+= usb_f_acm.o
+usb_f_ss_lb-y			:= f_loopback.o f_sourcesink.o
+obj-$(CONFIG_USB_F_SS_LB)	+= usb_f_ss_lb.o
 obj-$(CONFIG_USB_U_SERIAL)	+= u_serial.o
+usb_f_serial-y			:= f_serial.o
+obj-$(CONFIG_USB_F_SERIAL)	+= usb_f_serial.o
+usb_f_obex-y			:= f_obex.o
+obj-$(CONFIG_USB_F_OBEX)	+= usb_f_obex.o
 
 #
 # USB gadget drivers
diff --git a/drivers/usb/gadget/acm_ms.c b/drivers/usb/gadget/acm_ms.c
index 8f2b0e39153..4b947bb50f6 100644
--- a/drivers/usb/gadget/acm_ms.c
+++ b/drivers/usb/gadget/acm_ms.c
@@ -109,7 +109,6 @@ FSG_MODULE_PARAMETERS(/* no prefix */, fsg_mod_data);
 static struct fsg_common fsg_common;
 
 /*-------------------------------------------------------------------------*/
-static unsigned char tty_line;
 static struct usb_function *f_acm;
 static struct usb_function_instance *f_acm_inst;
 /*
@@ -117,7 +116,6 @@ static struct usb_function_instance *f_acm_inst;
  */
 static int __init acm_ms_do_config(struct usb_configuration *c)
 {
-	struct f_serial_opts *opts;
 	int	status;
 
 	if (gadget_is_otg(c->cdev->gadget)) {
@@ -129,9 +127,6 @@ static int __init acm_ms_do_config(struct usb_configuration *c)
 	if (IS_ERR(f_acm_inst))
 		return PTR_ERR(f_acm_inst);
 
-	opts = container_of(f_acm_inst, struct f_serial_opts, func_inst);
-	opts->port_num = tty_line;
-
 	f_acm = usb_get_function(f_acm_inst);
 	if (IS_ERR(f_acm)) {
 		status = PTR_ERR(f_acm);
@@ -171,16 +166,11 @@ static int __init acm_ms_bind(struct usb_composite_dev *cdev)
 	int			status;
 	void			*retp;
 
-	/* set up serial link layer */
-	status = gserial_alloc_line(&tty_line);
-	if (status < 0)
-		return status;
-
 	/* set up mass storage function */
 	retp = fsg_common_from_params(&fsg_common, cdev, &fsg_mod_data);
 	if (IS_ERR(retp)) {
 		status = PTR_ERR(retp);
-		goto fail0;
+		return PTR_ERR(retp);
 	}
 
 	/*
@@ -207,8 +197,6 @@ static int __init acm_ms_bind(struct usb_composite_dev *cdev)
 	/* error recovery */
 fail1:
 	fsg_common_put(&fsg_common);
-fail0:
-	gserial_free_line(tty_line);
 	return status;
 }
 
@@ -216,7 +204,6 @@ static int __exit acm_ms_unbind(struct usb_composite_dev *cdev)
 {
 	usb_put_function(f_acm);
 	usb_put_function_instance(f_acm_inst);
-	gserial_free_line(tty_line);
 	return 0;
 }
 
diff --git a/drivers/usb/gadget/amd5536udc.c b/drivers/usb/gadget/amd5536udc.c
index 75973f33a4c..f52dcfe8f54 100644
--- a/drivers/usb/gadget/amd5536udc.c
+++ b/drivers/usb/gadget/amd5536udc.c
@@ -1922,7 +1922,6 @@ static int amd5536_udc_start(struct usb_gadget *g,
 
 	driver->driver.bus = NULL;
 	dev->driver = driver;
-	dev->gadget.dev.driver = &driver->driver;
 
 	/* Some gadget drivers use both ep0 directions.
 	 * NOTE: to gadget driver, ep0 is just one endpoint...
@@ -1973,7 +1972,6 @@ static int amd5536_udc_stop(struct usb_gadget *g,
 	shutdown(dev, driver);
 	spin_unlock_irqrestore(&dev->lock, flags);
 
-	dev->gadget.dev.driver = NULL;
 	dev->driver = NULL;
 
 	/* set SD */
@@ -3080,7 +3078,6 @@ static void udc_pci_remove(struct pci_dev *pdev)
 	if (dev->active)
 		pci_disable_device(pdev);
 
-	device_unregister(&dev->gadget.dev);
 	pci_set_drvdata(pdev, NULL);
 
 	udc_remove(dev);
@@ -3245,8 +3242,6 @@ static int udc_pci_probe(
 	dev->phys_addr = resource;
 	dev->irq = pdev->irq;
 	dev->pdev = pdev;
-	dev->gadget.dev.parent = &pdev->dev;
-	dev->gadget.dev.dma_mask = pdev->dev.dma_mask;
 
 	/* general probing */
 	if (udc_probe(dev) == 0)
@@ -3273,7 +3268,6 @@ static int udc_probe(struct udc *dev)
 	dev->gadget.ops = &udc_ops;
 
 	dev_set_name(&dev->gadget.dev, "gadget");
-	dev->gadget.dev.release = gadget_release;
 	dev->gadget.name = name;
 	dev->gadget.max_speed = USB_SPEED_HIGH;
 
@@ -3297,17 +3291,11 @@ static int udc_probe(struct udc *dev)
 		"driver version: %s(for Geode5536 B1)\n", tmp);
 	udc = dev;
 
-	retval = usb_add_gadget_udc(&udc->pdev->dev, &dev->gadget);
+	retval = usb_add_gadget_udc_release(&udc->pdev->dev, &dev->gadget,
+			gadget_release);
 	if (retval)
 		goto finished;
 
-	retval = device_register(&dev->gadget.dev);
-	if (retval) {
-		usb_del_gadget_udc(&dev->gadget);
-		put_device(&dev->gadget.dev);
-		goto finished;
-	}
-
 	/* timer init */
 	init_timer(&udc_timer);
 	udc_timer.function = udc_timer_function;
diff --git a/drivers/usb/gadget/amd5536udc.h b/drivers/usb/gadget/amd5536udc.h
index f1bf32e6b8d..6744d3b8310 100644
--- a/drivers/usb/gadget/amd5536udc.h
+++ b/drivers/usb/gadget/amd5536udc.h
@@ -472,7 +472,6 @@ struct udc_request {
 
 	/* flags */
 	unsigned			dma_going : 1,
-					dma_mapping : 1,
 					dma_done : 1;
 	/* phys. address */
 	dma_addr_t			td_phys;
diff --git a/drivers/usb/gadget/at91_udc.c b/drivers/usb/gadget/at91_udc.c
index 45dd2929a67..a690d64217f 100644
--- a/drivers/usb/gadget/at91_udc.c
+++ b/drivers/usb/gadget/at91_udc.c
@@ -1631,7 +1631,6 @@ static int at91_start(struct usb_gadget *gadget,
 
 	udc = container_of(gadget, struct at91_udc, gadget);
 	udc->driver = driver;
-	udc->gadget.dev.driver = &driver->driver;
 	udc->gadget.dev.of_node = udc->pdev->dev.of_node;
 	udc->enabled = 1;
 	udc->selfpowered = 1;
@@ -1652,7 +1651,6 @@ static int at91_stop(struct usb_gadget *gadget,
 	at91_udp_write(udc, AT91_UDP_IDR, ~0);
 	spin_unlock_irqrestore(&udc->lock, flags);
 
-	udc->gadget.dev.driver = NULL;
 	udc->driver = NULL;
 
 	DBG("unbound from %s\n", driver->driver.name);
@@ -1780,13 +1778,7 @@ static int at91udc_probe(struct platform_device *pdev)
 		DBG("clocks missing\n");
 		retval = -ENODEV;
 		/* NOTE: we "know" here that refcounts on these are NOPs */
-		goto fail0b;
-	}
-
-	retval = device_register(&udc->gadget.dev);
-	if (retval < 0) {
-		put_device(&udc->gadget.dev);
-		goto fail0b;
+		goto fail1;
 	}
 
 	/* don't do anything until we have both gadget driver and VBUS */
@@ -1857,8 +1849,6 @@ fail3:
 fail2:
 	free_irq(udc->udp_irq, udc);
 fail1:
-	device_unregister(&udc->gadget.dev);
-fail0b:
 	iounmap(udc->udp_baseaddr);
 fail0a:
 	if (cpu_is_at91rm9200())
@@ -1892,8 +1882,6 @@ static int __exit at91udc_remove(struct platform_device *pdev)
 		gpio_free(udc->board.vbus_pin);
 	}
 	free_irq(udc->udp_irq, udc);
-	device_unregister(&udc->gadget.dev);
-
 	iounmap(udc->udp_baseaddr);
 
 	if (cpu_is_at91rm9200())
diff --git a/drivers/usb/gadget/atmel_usba_udc.c b/drivers/usb/gadget/atmel_usba_udc.c
index b66130c9726..f2a970f75bf 100644
--- a/drivers/usb/gadget/atmel_usba_udc.c
+++ b/drivers/usb/gadget/atmel_usba_udc.c
@@ -489,13 +489,8 @@ request_complete(struct usba_ep *ep, struct usba_request *req, int status)
 	if (req->req.status == -EINPROGRESS)
 		req->req.status = status;
 
-	if (req->mapped) {
-		dma_unmap_single(
-			&udc->pdev->dev, req->req.dma, req->req.length,
-			ep->is_in ? DMA_TO_DEVICE : DMA_FROM_DEVICE);
-		req->req.dma = DMA_ADDR_INVALID;
-		req->mapped = 0;
-	}
+	if (req->using_dma)
+		usb_gadget_unmap_request(&udc->gadget, &req->req, ep->is_in);
 
 	DBG(DBG_GADGET | DBG_REQ,
 		"%s: req %p complete: status %d, actual %u\n",
@@ -684,7 +679,6 @@ usba_ep_alloc_request(struct usb_ep *_ep, gfp_t gfp_flags)
 		return NULL;
 
 	INIT_LIST_HEAD(&req->queue);
-	req->req.dma = DMA_ADDR_INVALID;
 
 	return &req->req;
 }
@@ -717,20 +711,11 @@ static int queue_dma(struct usba_udc *udc, struct usba_ep *ep,
 		return -EINVAL;
 	}
 
-	req->using_dma = 1;
-
-	if (req->req.dma == DMA_ADDR_INVALID) {
-		req->req.dma = dma_map_single(
-			&udc->pdev->dev, req->req.buf, req->req.length,
-			ep->is_in ? DMA_TO_DEVICE : DMA_FROM_DEVICE);
-		req->mapped = 1;
-	} else {
-		dma_sync_single_for_device(
-			&udc->pdev->dev, req->req.dma, req->req.length,
-			ep->is_in ? DMA_TO_DEVICE : DMA_FROM_DEVICE);
-		req->mapped = 0;
-	}
+	ret = usb_gadget_map_request(&udc->gadget, &req->req, ep->is_in);
+	if (ret)
+		return ret;
 
+	req->using_dma = 1;
 	req->ctrl = USBA_BF(DMA_BUF_LEN, req->req.length)
 			| USBA_DMA_CH_EN | USBA_DMA_END_BUF_IE
 			| USBA_DMA_END_TR_EN | USBA_DMA_END_TR_IE;
@@ -1799,7 +1784,6 @@ static int atmel_usba_start(struct usb_gadget *gadget,
 
 	udc->devstatus = 1 << USB_DEVICE_SELF_POWERED;
 	udc->driver = driver;
-	udc->gadget.dev.driver = &driver->driver;
 	spin_unlock_irqrestore(&udc->lock, flags);
 
 	clk_enable(udc->pclk);
@@ -1841,7 +1825,6 @@ static int atmel_usba_stop(struct usb_gadget *gadget,
 	toggle_bias(0);
 	usba_writel(udc, CTRL, USBA_DISABLE_MASK);
 
-	udc->gadget.dev.driver = NULL;
 	udc->driver = NULL;
 
 	clk_disable(udc->hclk);
@@ -1900,10 +1883,6 @@ static int __init usba_udc_probe(struct platform_device *pdev)
 	dev_info(&pdev->dev, "FIFO at 0x%08lx mapped at %p\n",
 		 (unsigned long)fifo->start, udc->fifo);
 
-	device_initialize(&udc->gadget.dev);
-	udc->gadget.dev.parent = &pdev->dev;
-	udc->gadget.dev.dma_mask = pdev->dev.dma_mask;
-
 	platform_set_drvdata(pdev, udc);
 
 	/* Make sure we start from a clean slate */
@@ -1962,12 +1941,6 @@ static int __init usba_udc_probe(struct platform_device *pdev)
 	}
 	udc->irq = irq;
 
-	ret = device_add(&udc->gadget.dev);
-	if (ret) {
-		dev_dbg(&pdev->dev, "Could not add gadget: %d\n", ret);
-		goto err_device_add;
-	}
-
 	if (gpio_is_valid(pdata->vbus_pin)) {
 		if (!gpio_request(pdata->vbus_pin, "atmel_usba_udc")) {
 			udc->vbus_pin = pdata->vbus_pin;
@@ -2007,9 +1980,6 @@ err_add_udc:
 		gpio_free(udc->vbus_pin);
 	}
 
-	device_unregister(&udc->gadget.dev);
-
-err_device_add:
 	free_irq(irq, udc);
 err_request_irq:
 	kfree(usba_ep);
@@ -2053,8 +2023,6 @@ static int __exit usba_udc_remove(struct platform_device *pdev)
 	clk_put(udc->hclk);
 	clk_put(udc->pclk);
 
-	device_unregister(&udc->gadget.dev);
-
 	return 0;
 }
 
diff --git a/drivers/usb/gadget/atmel_usba_udc.h b/drivers/usb/gadget/atmel_usba_udc.h
index 9791259cbda..d65a61851d3 100644
--- a/drivers/usb/gadget/atmel_usba_udc.h
+++ b/drivers/usb/gadget/atmel_usba_udc.h
@@ -216,12 +216,6 @@
 #define EP0_EPT_SIZE		USBA_EPT_SIZE_64
 #define EP0_NR_BANKS		1
 
-/*
- * REVISIT: Try to eliminate this value. Can we rely on req->mapped to
- * provide this information?
- */
-#define DMA_ADDR_INVALID (~(dma_addr_t)0)
-
 #define FIFO_IOMEM_ID	0
 #define CTRL_IOMEM_ID	1
 
diff --git a/drivers/usb/gadget/bcm63xx_udc.c b/drivers/usb/gadget/bcm63xx_udc.c
index 8cc8253f110..6e6518264c4 100644
--- a/drivers/usb/gadget/bcm63xx_udc.c
+++ b/drivers/usb/gadget/bcm63xx_udc.c
@@ -1819,7 +1819,6 @@ static int bcm63xx_udc_start(struct usb_gadget *gadget,
 
 	udc->driver = driver;
 	driver->driver.bus = NULL;
-	udc->gadget.dev.driver = &driver->driver;
 	udc->gadget.dev.of_node = udc->dev->of_node;
 
 	spin_unlock_irqrestore(&udc->lock, flags);
@@ -1841,7 +1840,6 @@ static int bcm63xx_udc_stop(struct usb_gadget *gadget,
 	spin_lock_irqsave(&udc->lock, flags);
 
 	udc->driver = NULL;
-	udc->gadget.dev.driver = NULL;
 
 	/*
 	 * If we switch the PHY too abruptly after dropping D+, the host
@@ -2306,17 +2304,6 @@ static void bcm63xx_udc_cleanup_debugfs(struct bcm63xx_udc *udc)
  ***********************************************************************/
 
 /**
- * bcm63xx_udc_gadget_release - Called from device_release().
- * @dev: Unused.
- *
- * We get a warning if this function doesn't exist, but it's empty because
- * we don't have to free any of the memory allocated with the devm_* APIs.
- */
-static void bcm63xx_udc_gadget_release(struct device *dev)
-{
-}
-
-/**
  * bcm63xx_udc_probe - Initialize a new instance of the UDC.
  * @pdev: Platform device struct from the bcm63xx BSP code.
  *
@@ -2368,13 +2355,9 @@ static int bcm63xx_udc_probe(struct platform_device *pdev)
 
 	spin_lock_init(&udc->lock);
 	INIT_WORK(&udc->ep0_wq, bcm63xx_ep0_process);
-	dev_set_name(&udc->gadget.dev, "gadget");
 
 	udc->gadget.ops = &bcm63xx_udc_ops;
 	udc->gadget.name = dev_name(dev);
-	udc->gadget.dev.parent = dev;
-	udc->gadget.dev.release = bcm63xx_udc_gadget_release;
-	udc->gadget.dev.dma_mask = dev->dma_mask;
 
 	if (!pd->use_fullspeed && !use_fullspeed)
 		udc->gadget.max_speed = USB_SPEED_HIGH;
@@ -2414,17 +2397,12 @@ static int bcm63xx_udc_probe(struct platform_device *pdev)
 		}
 	}
 
-	rc = device_register(&udc->gadget.dev);
-	if (rc)
-		goto out_uninit;
-
 	bcm63xx_udc_init_debugfs(udc);
 	rc = usb_add_gadget_udc(dev, &udc->gadget);
 	if (!rc)
 		return 0;
 
 	bcm63xx_udc_cleanup_debugfs(udc);
-	device_unregister(&udc->gadget.dev);
 out_uninit:
 	bcm63xx_uninit_udc_hw(udc);
 	return rc;
@@ -2440,7 +2418,6 @@ static int bcm63xx_udc_remove(struct platform_device *pdev)
 
 	bcm63xx_udc_cleanup_debugfs(udc);
 	usb_del_gadget_udc(&udc->gadget);
-	device_unregister(&udc->gadget.dev);
 	BUG_ON(udc->driver);
 
 	platform_set_drvdata(pdev, NULL);
diff --git a/drivers/usb/gadget/cdc2.c b/drivers/usb/gadget/cdc2.c
index a7d6f702675..2c525518276 100644
--- a/drivers/usb/gadget/cdc2.c
+++ b/drivers/usb/gadget/cdc2.c
@@ -103,18 +103,16 @@ static struct usb_gadget_strings *dev_strings[] = {
 };
 
 static u8 hostaddr[ETH_ALEN];
-
+static struct eth_dev *the_dev;
 /*-------------------------------------------------------------------------*/
 static struct usb_function *f_acm;
 static struct usb_function_instance *fi_serial;
 
-static unsigned char tty_line;
 /*
  * We _always_ have both CDC ECM and CDC ACM functions.
  */
 static int __init cdc_do_config(struct usb_configuration *c)
 {
-	struct f_serial_opts *opts;
 	int	status;
 
 	if (gadget_is_otg(c->cdev->gadget)) {
@@ -122,7 +120,7 @@ static int __init cdc_do_config(struct usb_configuration *c)
 		c->bmAttributes |= USB_CONFIG_ATT_WAKEUP;
 	}
 
-	status = ecm_bind_config(c, hostaddr);
+	status = ecm_bind_config(c, hostaddr, the_dev);
 	if (status < 0)
 		return status;
 
@@ -130,12 +128,11 @@ static int __init cdc_do_config(struct usb_configuration *c)
 	if (IS_ERR(fi_serial))
 		return PTR_ERR(fi_serial);
 
-	opts = container_of(fi_serial, struct f_serial_opts, func_inst);
-	opts->port_num = tty_line;
-
 	f_acm = usb_get_function(fi_serial);
-	if (IS_ERR(f_acm))
+	if (IS_ERR(f_acm)) {
+		status = PTR_ERR(f_acm);
 		goto err_func_acm;
+	}
 
 	status = usb_add_function(c, f_acm);
 	if (status)
@@ -169,14 +166,9 @@ static int __init cdc_bind(struct usb_composite_dev *cdev)
 	}
 
 	/* set up network link layer */
-	status = gether_setup(cdev->gadget, hostaddr);
-	if (status < 0)
-		return status;
-
-	/* set up serial link layer */
-	status = gserial_alloc_line(&tty_line);
-	if (status < 0)
-		goto fail0;
+	the_dev = gether_setup(cdev->gadget, hostaddr);
+	if (IS_ERR(the_dev))
+		return PTR_ERR(the_dev);
 
 	/* Allocate string descriptor numbers ... note that string
 	 * contents can be overridden by the composite_dev glue.
@@ -200,9 +192,7 @@ static int __init cdc_bind(struct usb_composite_dev *cdev)
 	return 0;
 
 fail1:
-	gserial_free_line(tty_line);
-fail0:
-	gether_cleanup();
+	gether_cleanup(the_dev);
 	return status;
 }
 
@@ -210,8 +200,7 @@ static int __exit cdc_unbind(struct usb_composite_dev *cdev)
 {
 	usb_put_function(f_acm);
 	usb_put_function_instance(fi_serial);
-	gserial_free_line(tty_line);
-	gether_cleanup();
+	gether_cleanup(the_dev);
 	return 0;
 }
 
diff --git a/drivers/usb/gadget/composite.c b/drivers/usb/gadget/composite.c
index c0d62b27861..55f4df60f32 100644
--- a/drivers/usb/gadget/composite.c
+++ b/drivers/usb/gadget/composite.c
@@ -1637,6 +1637,7 @@ void composite_dev_cleanup(struct usb_composite_dev *cdev)
 		kfree(cdev->req->buf);
 		usb_ep_free_request(cdev->gadget->ep0, cdev->req);
 	}
+	cdev->next_string_id = 0;
 	device_remove_file(&cdev->gadget->dev, &dev_attr_suspended);
 }
 
diff --git a/drivers/usb/gadget/configfs.c b/drivers/usb/gadget/configfs.c
new file mode 100644
index 00000000000..3d5cfc9c2c7
--- /dev/null
+++ b/drivers/usb/gadget/configfs.c
@@ -0,0 +1,1003 @@
+#include <linux/configfs.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/device.h>
+#include <linux/usb/composite.h>
+#include <linux/usb/gadget_configfs.h>
+
+int check_user_usb_string(const char *name,
+		struct usb_gadget_strings *stringtab_dev)
+{
+	unsigned primary_lang;
+	unsigned sub_lang;
+	u16 num;
+	int ret;
+
+	ret = kstrtou16(name, 0, &num);
+	if (ret)
+		return ret;
+
+	primary_lang = num & 0x3ff;
+	sub_lang = num >> 10;
+
+	/* simple sanity check for valid langid */
+	switch (primary_lang) {
+	case 0:
+	case 0x62 ... 0xfe:
+	case 0x100 ... 0x3ff:
+		return -EINVAL;
+	}
+	if (!sub_lang)
+		return -EINVAL;
+
+	stringtab_dev->language = num;
+	return 0;
+}
+
+#define MAX_NAME_LEN	40
+#define MAX_USB_STRING_LANGS 2
+
+struct gadget_info {
+	struct config_group group;
+	struct config_group functions_group;
+	struct config_group configs_group;
+	struct config_group strings_group;
+	struct config_group *default_groups[4];
+
+	struct mutex lock;
+	struct usb_gadget_strings *gstrings[MAX_USB_STRING_LANGS + 1];
+	struct list_head string_list;
+	struct list_head available_func;
+
+	const char *udc_name;
+#ifdef CONFIG_USB_OTG
+	struct usb_otg_descriptor otg;
+#endif
+	struct usb_composite_driver composite;
+	struct usb_composite_dev cdev;
+};
+
+struct config_usb_cfg {
+	struct config_group group;
+	struct config_group strings_group;
+	struct config_group *default_groups[2];
+	struct list_head string_list;
+	struct usb_configuration c;
+	struct list_head func_list;
+	struct usb_gadget_strings *gstrings[MAX_USB_STRING_LANGS + 1];
+};
+
+struct gadget_strings {
+	struct usb_gadget_strings stringtab_dev;
+	struct usb_string strings[USB_GADGET_FIRST_AVAIL_IDX];
+	char *manufacturer;
+	char *product;
+	char *serialnumber;
+
+	struct config_group group;
+	struct list_head list;
+};
+
+struct gadget_config_name {
+	struct usb_gadget_strings stringtab_dev;
+	struct usb_string strings;
+	char *configuration;
+
+	struct config_group group;
+	struct list_head list;
+};
+
+static int usb_string_copy(const char *s, char **s_copy)
+{
+	int ret;
+	char *str;
+	char *copy = *s_copy;
+	ret = strlen(s);
+	if (ret > 126)
+		return -EOVERFLOW;
+
+	str = kstrdup(s, GFP_KERNEL);
+	if (!str)
+		return -ENOMEM;
+	if (str[ret - 1] == '\n')
+		str[ret - 1] = '\0';
+	kfree(copy);
+	*s_copy = str;
+	return 0;
+}
+
+CONFIGFS_ATTR_STRUCT(gadget_info);
+CONFIGFS_ATTR_STRUCT(config_usb_cfg);
+
+#define GI_DEVICE_DESC_ITEM_ATTR(name)	\
+	static struct gadget_info_attribute gadget_cdev_desc_##name = \
+		__CONFIGFS_ATTR(name,  S_IRUGO | S_IWUSR,		\
+				gadget_dev_desc_##name##_show,		\
+				gadget_dev_desc_##name##_store)
+
+#define GI_DEVICE_DESC_SIMPLE_R_u8(__name)	\
+	static ssize_t gadget_dev_desc_##__name##_show(struct gadget_info *gi, \
+			char *page)	\
+{	\
+	return sprintf(page, "0x%02x\n", gi->cdev.desc.__name);	\
+}
+
+#define GI_DEVICE_DESC_SIMPLE_R_u16(__name)	\
+	static ssize_t gadget_dev_desc_##__name##_show(struct gadget_info *gi, \
+			char *page)	\
+{	\
+	return sprintf(page, "0x%04x\n", le16_to_cpup(&gi->cdev.desc.__name)); \
+}
+
+
+#define GI_DEVICE_DESC_SIMPLE_W_u8(_name)		\
+	static ssize_t gadget_dev_desc_##_name##_store(struct gadget_info *gi, \
+		const char *page, size_t len)		\
+{							\
+	u8 val;						\
+	int ret;					\
+	ret = kstrtou8(page, 0, &val);			\
+	if (ret)					\
+		return ret;				\
+	gi->cdev.desc._name = val;			\
+	return len;					\
+}
+
+#define GI_DEVICE_DESC_SIMPLE_W_u16(_name)	\
+	static ssize_t gadget_dev_desc_##_name##_store(struct gadget_info *gi, \
+		const char *page, size_t len)		\
+{							\
+	u16 val;					\
+	int ret;					\
+	ret = kstrtou16(page, 0, &val);			\
+	if (ret)					\
+		return ret;				\
+	gi->cdev.desc._name = cpu_to_le16p(&val);	\
+	return len;					\
+}
+
+#define GI_DEVICE_DESC_SIMPLE_RW(_name, _type)	\
+	GI_DEVICE_DESC_SIMPLE_R_##_type(_name)	\
+	GI_DEVICE_DESC_SIMPLE_W_##_type(_name)
+
+GI_DEVICE_DESC_SIMPLE_R_u16(bcdUSB);
+GI_DEVICE_DESC_SIMPLE_RW(bDeviceClass, u8);
+GI_DEVICE_DESC_SIMPLE_RW(bDeviceSubClass, u8);
+GI_DEVICE_DESC_SIMPLE_RW(bDeviceProtocol, u8);
+GI_DEVICE_DESC_SIMPLE_RW(bMaxPacketSize0, u8);
+GI_DEVICE_DESC_SIMPLE_RW(idVendor, u16);
+GI_DEVICE_DESC_SIMPLE_RW(idProduct, u16);
+GI_DEVICE_DESC_SIMPLE_R_u16(bcdDevice);
+
+static ssize_t is_valid_bcd(u16 bcd_val)
+{
+	if ((bcd_val & 0xf) > 9)
+		return -EINVAL;
+	if (((bcd_val >> 4) & 0xf) > 9)
+		return -EINVAL;
+	if (((bcd_val >> 8) & 0xf) > 9)
+		return -EINVAL;
+	if (((bcd_val >> 12) & 0xf) > 9)
+		return -EINVAL;
+	return 0;
+}
+
+static ssize_t gadget_dev_desc_bcdDevice_store(struct gadget_info *gi,
+		const char *page, size_t len)
+{
+	u16 bcdDevice;
+	int ret;
+
+	ret = kstrtou16(page, 0, &bcdDevice);
+	if (ret)
+		return ret;
+	ret = is_valid_bcd(bcdDevice);
+	if (ret)
+		return ret;
+
+	gi->cdev.desc.bcdDevice = cpu_to_le16(bcdDevice);
+	return len;
+}
+
+static ssize_t gadget_dev_desc_bcdUSB_store(struct gadget_info *gi,
+		const char *page, size_t len)
+{
+	u16 bcdUSB;
+	int ret;
+
+	ret = kstrtou16(page, 0, &bcdUSB);
+	if (ret)
+		return ret;
+	ret = is_valid_bcd(bcdUSB);
+	if (ret)
+		return ret;
+
+	gi->cdev.desc.bcdUSB = cpu_to_le16(bcdUSB);
+	return len;
+}
+
+static ssize_t gadget_dev_desc_UDC_show(struct gadget_info *gi, char *page)
+{
+	return sprintf(page, "%s\n", gi->udc_name ?: "");
+}
+
+static int unregister_gadget(struct gadget_info *gi)
+{
+	int ret;
+
+	if (!gi->udc_name)
+		return -ENODEV;
+
+	ret = usb_gadget_unregister_driver(&gi->composite.gadget_driver);
+	if (ret)
+		return ret;
+	kfree(gi->udc_name);
+	gi->udc_name = NULL;
+	return 0;
+}
+
+static ssize_t gadget_dev_desc_UDC_store(struct gadget_info *gi,
+		const char *page, size_t len)
+{
+	char *name;
+	int ret;
+
+	name = kstrdup(page, GFP_KERNEL);
+	if (!name)
+		return -ENOMEM;
+	if (name[len - 1] == '\n')
+		name[len - 1] = '\0';
+
+	mutex_lock(&gi->lock);
+
+	if (!strlen(name)) {
+		ret = unregister_gadget(gi);
+		if (ret)
+			goto err;
+	} else {
+		if (gi->udc_name) {
+			ret = -EBUSY;
+			goto err;
+		}
+		ret = udc_attach_driver(name, &gi->composite.gadget_driver);
+		if (ret)
+			goto err;
+		gi->udc_name = name;
+	}
+	mutex_unlock(&gi->lock);
+	return len;
+err:
+	kfree(name);
+	mutex_unlock(&gi->lock);
+	return ret;
+}
+
+GI_DEVICE_DESC_ITEM_ATTR(bDeviceClass);
+GI_DEVICE_DESC_ITEM_ATTR(bDeviceSubClass);
+GI_DEVICE_DESC_ITEM_ATTR(bDeviceProtocol);
+GI_DEVICE_DESC_ITEM_ATTR(bMaxPacketSize0);
+GI_DEVICE_DESC_ITEM_ATTR(idVendor);
+GI_DEVICE_DESC_ITEM_ATTR(idProduct);
+GI_DEVICE_DESC_ITEM_ATTR(bcdDevice);
+GI_DEVICE_DESC_ITEM_ATTR(bcdUSB);
+GI_DEVICE_DESC_ITEM_ATTR(UDC);
+
+static struct configfs_attribute *gadget_root_attrs[] = {
+	&gadget_cdev_desc_bDeviceClass.attr,
+	&gadget_cdev_desc_bDeviceSubClass.attr,
+	&gadget_cdev_desc_bDeviceProtocol.attr,
+	&gadget_cdev_desc_bMaxPacketSize0.attr,
+	&gadget_cdev_desc_idVendor.attr,
+	&gadget_cdev_desc_idProduct.attr,
+	&gadget_cdev_desc_bcdDevice.attr,
+	&gadget_cdev_desc_bcdUSB.attr,
+	&gadget_cdev_desc_UDC.attr,
+	NULL,
+};
+
+static inline struct gadget_info *to_gadget_info(struct config_item *item)
+{
+	 return container_of(to_config_group(item), struct gadget_info, group);
+}
+
+static inline struct gadget_strings *to_gadget_strings(struct config_item *item)
+{
+	 return container_of(to_config_group(item), struct gadget_strings,
+			 group);
+}
+
+static inline struct gadget_config_name *to_gadget_config_name(
+		struct config_item *item)
+{
+	 return container_of(to_config_group(item), struct gadget_config_name,
+			 group);
+}
+
+static inline struct config_usb_cfg *to_config_usb_cfg(struct config_item *item)
+{
+	return container_of(to_config_group(item), struct config_usb_cfg,
+			group);
+}
+
+static inline struct usb_function_instance *to_usb_function_instance(
+		struct config_item *item)
+{
+	 return container_of(to_config_group(item),
+			 struct usb_function_instance, group);
+}
+
+static void gadget_info_attr_release(struct config_item *item)
+{
+	struct gadget_info *gi = to_gadget_info(item);
+
+	WARN_ON(!list_empty(&gi->cdev.configs));
+	WARN_ON(!list_empty(&gi->string_list));
+	WARN_ON(!list_empty(&gi->available_func));
+	kfree(gi->composite.gadget_driver.function);
+	kfree(gi);
+}
+
+CONFIGFS_ATTR_OPS(gadget_info);
+
+static struct configfs_item_operations gadget_root_item_ops = {
+	.release                = gadget_info_attr_release,
+	.show_attribute         = gadget_info_attr_show,
+	.store_attribute        = gadget_info_attr_store,
+};
+
+static void gadget_config_attr_release(struct config_item *item)
+{
+	struct config_usb_cfg *cfg = to_config_usb_cfg(item);
+
+	WARN_ON(!list_empty(&cfg->c.functions));
+	list_del(&cfg->c.list);
+	kfree(cfg->c.label);
+	kfree(cfg);
+}
+
+static int config_usb_cfg_link(
+	struct config_item *usb_cfg_ci,
+	struct config_item *usb_func_ci)
+{
+	struct config_usb_cfg *cfg = to_config_usb_cfg(usb_cfg_ci);
+	struct usb_composite_dev *cdev = cfg->c.cdev;
+	struct gadget_info *gi = container_of(cdev, struct gadget_info, cdev);
+
+	struct config_group *group = to_config_group(usb_func_ci);
+	struct usb_function_instance *fi = container_of(group,
+			struct usb_function_instance, group);
+	struct usb_function_instance *a_fi;
+	struct usb_function *f;
+	int ret;
+
+	mutex_lock(&gi->lock);
+	/*
+	 * Make sure this function is from within our _this_ gadget and not
+	 * from another gadget or a random directory.
+	 * Also a function instance can only be linked once.
+	 */
+	list_for_each_entry(a_fi, &gi->available_func, cfs_list) {
+		if (a_fi == fi)
+			break;
+	}
+	if (a_fi != fi) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	list_for_each_entry(f, &cfg->func_list, list) {
+		if (f->fi == fi) {
+			ret = -EEXIST;
+			goto out;
+		}
+	}
+
+	f = usb_get_function(fi);
+	if (IS_ERR(f)) {
+		ret = PTR_ERR(f);
+		goto out;
+	}
+
+	/* stash the function until we bind it to the gadget */
+	list_add_tail(&f->list, &cfg->func_list);
+	ret = 0;
+out:
+	mutex_unlock(&gi->lock);
+	return ret;
+}
+
+static int config_usb_cfg_unlink(
+	struct config_item *usb_cfg_ci,
+	struct config_item *usb_func_ci)
+{
+	struct config_usb_cfg *cfg = to_config_usb_cfg(usb_cfg_ci);
+	struct usb_composite_dev *cdev = cfg->c.cdev;
+	struct gadget_info *gi = container_of(cdev, struct gadget_info, cdev);
+
+	struct config_group *group = to_config_group(usb_func_ci);
+	struct usb_function_instance *fi = container_of(group,
+			struct usb_function_instance, group);
+	struct usb_function *f;
+
+	/*
+	 * ideally I would like to forbid to unlink functions while a gadget is
+	 * bound to an UDC. Since this isn't possible at the moment, we simply
+	 * force an unbind, the function is available here and then we can
+	 * remove the function.
+	 */
+	mutex_lock(&gi->lock);
+	if (gi->udc_name)
+		unregister_gadget(gi);
+	WARN_ON(gi->udc_name);
+
+	list_for_each_entry(f, &cfg->func_list, list) {
+		if (f->fi == fi) {
+			list_del(&f->list);
+			usb_put_function(f);
+			mutex_unlock(&gi->lock);
+			return 0;
+		}
+	}
+	mutex_unlock(&gi->lock);
+	WARN(1, "Unable to locate function to unbind\n");
+	return 0;
+}
+
+CONFIGFS_ATTR_OPS(config_usb_cfg);
+
+static struct configfs_item_operations gadget_config_item_ops = {
+	.release                = gadget_config_attr_release,
+	.show_attribute         = config_usb_cfg_attr_show,
+	.store_attribute        = config_usb_cfg_attr_store,
+	.allow_link             = config_usb_cfg_link,
+	.drop_link              = config_usb_cfg_unlink,
+};
+
+
+static ssize_t gadget_config_desc_MaxPower_show(struct config_usb_cfg *cfg,
+		char *page)
+{
+	return sprintf(page, "%u\n", cfg->c.MaxPower);
+}
+
+static ssize_t gadget_config_desc_MaxPower_store(struct config_usb_cfg *cfg,
+		const char *page, size_t len)
+{
+	u16 val;
+	int ret;
+	ret = kstrtou16(page, 0, &val);
+	if (ret)
+		return ret;
+	if (DIV_ROUND_UP(val, 8) > 0xff)
+		return -ERANGE;
+	cfg->c.MaxPower = val;
+	return len;
+}
+
+static ssize_t gadget_config_desc_bmAttributes_show(struct config_usb_cfg *cfg,
+		char *page)
+{
+	return sprintf(page, "0x%02x\n", cfg->c.bmAttributes);
+}
+
+static ssize_t gadget_config_desc_bmAttributes_store(struct config_usb_cfg *cfg,
+		const char *page, size_t len)
+{
+	u8 val;
+	int ret;
+	ret = kstrtou8(page, 0, &val);
+	if (ret)
+		return ret;
+	if (!(val & USB_CONFIG_ATT_ONE))
+		return -EINVAL;
+	if (val & ~(USB_CONFIG_ATT_ONE | USB_CONFIG_ATT_SELFPOWER |
+				USB_CONFIG_ATT_WAKEUP))
+		return -EINVAL;
+	cfg->c.bmAttributes = val;
+	return len;
+}
+
+#define CFG_CONFIG_DESC_ITEM_ATTR(name)	\
+	static struct config_usb_cfg_attribute gadget_usb_cfg_##name = \
+		__CONFIGFS_ATTR(name,  S_IRUGO | S_IWUSR,		\
+				gadget_config_desc_##name##_show,	\
+				gadget_config_desc_##name##_store)
+
+CFG_CONFIG_DESC_ITEM_ATTR(MaxPower);
+CFG_CONFIG_DESC_ITEM_ATTR(bmAttributes);
+
+static struct configfs_attribute *gadget_config_attrs[] = {
+	&gadget_usb_cfg_MaxPower.attr,
+	&gadget_usb_cfg_bmAttributes.attr,
+	NULL,
+};
+
+static struct config_item_type gadget_config_type = {
+	.ct_item_ops	= &gadget_config_item_ops,
+	.ct_attrs	= gadget_config_attrs,
+	.ct_owner	= THIS_MODULE,
+};
+
+static struct config_item_type gadget_root_type = {
+	.ct_item_ops	= &gadget_root_item_ops,
+	.ct_attrs	= gadget_root_attrs,
+	.ct_owner	= THIS_MODULE,
+};
+
+static void composite_init_dev(struct usb_composite_dev *cdev)
+{
+	spin_lock_init(&cdev->lock);
+	INIT_LIST_HEAD(&cdev->configs);
+	INIT_LIST_HEAD(&cdev->gstrings);
+}
+
+static struct config_group *function_make(
+		struct config_group *group,
+		const char *name)
+{
+	struct gadget_info *gi;
+	struct usb_function_instance *fi;
+	char buf[MAX_NAME_LEN];
+	char *func_name;
+	char *instance_name;
+	int ret;
+
+	ret = snprintf(buf, MAX_NAME_LEN, "%s", name);
+	if (ret >= MAX_NAME_LEN)
+		return ERR_PTR(-ENAMETOOLONG);
+
+	func_name = buf;
+	instance_name = strchr(func_name, '.');
+	if (!instance_name) {
+		pr_err("Unable to locate . in FUNC.INSTANCE\n");
+		return ERR_PTR(-EINVAL);
+	}
+	*instance_name = '\0';
+	instance_name++;
+
+	fi = usb_get_function_instance(func_name);
+	if (IS_ERR(fi))
+		return ERR_PTR(PTR_ERR(fi));
+
+	ret = config_item_set_name(&fi->group.cg_item, name);
+	if (ret) {
+		usb_put_function_instance(fi);
+		return ERR_PTR(ret);
+	}
+
+	gi = container_of(group, struct gadget_info, functions_group);
+
+	mutex_lock(&gi->lock);
+	list_add_tail(&fi->cfs_list, &gi->available_func);
+	mutex_unlock(&gi->lock);
+	return &fi->group;
+}
+
+static void function_drop(
+		struct config_group *group,
+		struct config_item *item)
+{
+	struct usb_function_instance *fi = to_usb_function_instance(item);
+	struct gadget_info *gi;
+
+	gi = container_of(group, struct gadget_info, functions_group);
+
+	mutex_lock(&gi->lock);
+	list_del(&fi->cfs_list);
+	mutex_unlock(&gi->lock);
+	config_item_put(item);
+}
+
+static struct configfs_group_operations functions_ops = {
+	.make_group     = &function_make,
+	.drop_item      = &function_drop,
+};
+
+static struct config_item_type functions_type = {
+	.ct_group_ops   = &functions_ops,
+	.ct_owner       = THIS_MODULE,
+};
+
+CONFIGFS_ATTR_STRUCT(gadget_config_name);
+GS_STRINGS_RW(gadget_config_name, configuration);
+
+static struct configfs_attribute *gadget_config_name_langid_attrs[] = {
+	&gadget_config_name_configuration.attr,
+	NULL,
+};
+
+static void gadget_config_name_attr_release(struct config_item *item)
+{
+	struct gadget_config_name *cn = to_gadget_config_name(item);
+
+	kfree(cn->configuration);
+
+	list_del(&cn->list);
+	kfree(cn);
+}
+
+USB_CONFIG_STRING_RW_OPS(gadget_config_name);
+USB_CONFIG_STRINGS_LANG(gadget_config_name, config_usb_cfg);
+
+static struct config_group *config_desc_make(
+		struct config_group *group,
+		const char *name)
+{
+	struct gadget_info *gi;
+	struct config_usb_cfg *cfg;
+	char buf[MAX_NAME_LEN];
+	char *num_str;
+	u8 num;
+	int ret;
+
+	gi = container_of(group, struct gadget_info, configs_group);
+	ret = snprintf(buf, MAX_NAME_LEN, "%s", name);
+	if (ret >= MAX_NAME_LEN)
+		return ERR_PTR(-ENAMETOOLONG);
+
+	num_str = strchr(buf, '.');
+	if (!num_str) {
+		pr_err("Unable to locate . in name.bConfigurationValue\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	*num_str = '\0';
+	num_str++;
+
+	if (!strlen(buf))
+		return ERR_PTR(-EINVAL);
+
+	ret = kstrtou8(num_str, 0, &num);
+	if (ret)
+		return ERR_PTR(ret);
+
+	cfg = kzalloc(sizeof(*cfg), GFP_KERNEL);
+	if (!cfg)
+		return ERR_PTR(-ENOMEM);
+	cfg->c.label = kstrdup(buf, GFP_KERNEL);
+	if (!cfg->c.label) {
+		ret = -ENOMEM;
+		goto err;
+	}
+	cfg->c.bConfigurationValue = num;
+	cfg->c.MaxPower = CONFIG_USB_GADGET_VBUS_DRAW;
+	cfg->c.bmAttributes = USB_CONFIG_ATT_ONE;
+	INIT_LIST_HEAD(&cfg->string_list);
+	INIT_LIST_HEAD(&cfg->func_list);
+
+	cfg->group.default_groups = cfg->default_groups;
+	cfg->default_groups[0] = &cfg->strings_group;
+
+	config_group_init_type_name(&cfg->group, name,
+				&gadget_config_type);
+	config_group_init_type_name(&cfg->strings_group, "strings",
+			&gadget_config_name_strings_type);
+
+	ret = usb_add_config_only(&gi->cdev, &cfg->c);
+	if (ret)
+		goto err;
+
+	return &cfg->group;
+err:
+	kfree(cfg->c.label);
+	kfree(cfg);
+	return ERR_PTR(ret);
+}
+
+static void config_desc_drop(
+		struct config_group *group,
+		struct config_item *item)
+{
+	config_item_put(item);
+}
+
+static struct configfs_group_operations config_desc_ops = {
+	.make_group     = &config_desc_make,
+	.drop_item      = &config_desc_drop,
+};
+
+static struct config_item_type config_desc_type = {
+	.ct_group_ops   = &config_desc_ops,
+	.ct_owner       = THIS_MODULE,
+};
+
+CONFIGFS_ATTR_STRUCT(gadget_strings);
+GS_STRINGS_RW(gadget_strings, manufacturer);
+GS_STRINGS_RW(gadget_strings, product);
+GS_STRINGS_RW(gadget_strings, serialnumber);
+
+static struct configfs_attribute *gadget_strings_langid_attrs[] = {
+	&gadget_strings_manufacturer.attr,
+	&gadget_strings_product.attr,
+	&gadget_strings_serialnumber.attr,
+	NULL,
+};
+
+static void gadget_strings_attr_release(struct config_item *item)
+{
+	struct gadget_strings *gs = to_gadget_strings(item);
+
+	kfree(gs->manufacturer);
+	kfree(gs->product);
+	kfree(gs->serialnumber);
+
+	list_del(&gs->list);
+	kfree(gs);
+}
+
+USB_CONFIG_STRING_RW_OPS(gadget_strings);
+USB_CONFIG_STRINGS_LANG(gadget_strings, gadget_info);
+
+static int configfs_do_nothing(struct usb_composite_dev *cdev)
+{
+	WARN_ON(1);
+	return -EINVAL;
+}
+
+int composite_dev_prepare(struct usb_composite_driver *composite,
+		struct usb_composite_dev *dev);
+
+static void purge_configs_funcs(struct gadget_info *gi)
+{
+	struct usb_configuration	*c;
+
+	list_for_each_entry(c, &gi->cdev.configs, list) {
+		struct usb_function *f, *tmp;
+		struct config_usb_cfg *cfg;
+
+		cfg = container_of(c, struct config_usb_cfg, c);
+
+		list_for_each_entry_safe(f, tmp, &c->functions, list) {
+
+			list_move_tail(&f->list, &cfg->func_list);
+			if (f->unbind) {
+				dev_err(&gi->cdev.gadget->dev, "unbind function"
+						" '%s'/%p\n", f->name, f);
+				f->unbind(c, f);
+			}
+		}
+		c->next_interface_id = 0;
+		c->superspeed = 0;
+		c->highspeed = 0;
+		c->fullspeed = 0;
+	}
+}
+
+static int configfs_composite_bind(struct usb_gadget *gadget,
+		struct usb_gadget_driver *gdriver)
+{
+	struct usb_composite_driver     *composite = to_cdriver(gdriver);
+	struct gadget_info		*gi = container_of(composite,
+						struct gadget_info, composite);
+	struct usb_composite_dev	*cdev = &gi->cdev;
+	struct usb_configuration	*c;
+	struct usb_string		*s;
+	unsigned			i;
+	int				ret;
+
+	/* the gi->lock is hold by the caller */
+	cdev->gadget = gadget;
+	set_gadget_data(gadget, cdev);
+	ret = composite_dev_prepare(composite, cdev);
+	if (ret)
+		return ret;
+	/* and now the gadget bind */
+	ret = -EINVAL;
+
+	if (list_empty(&gi->cdev.configs)) {
+		pr_err("Need atleast one configuration in %s.\n",
+				gi->composite.name);
+		goto err_comp_cleanup;
+	}
+
+
+	list_for_each_entry(c, &gi->cdev.configs, list) {
+		struct config_usb_cfg *cfg;
+
+		cfg = container_of(c, struct config_usb_cfg, c);
+		if (list_empty(&cfg->func_list)) {
+			pr_err("Config %s/%d of %s needs atleast one function.\n",
+			      c->label, c->bConfigurationValue,
+			      gi->composite.name);
+			goto err_comp_cleanup;
+		}
+	}
+
+	/* init all strings */
+	if (!list_empty(&gi->string_list)) {
+		struct gadget_strings *gs;
+
+		i = 0;
+		list_for_each_entry(gs, &gi->string_list, list) {
+
+			gi->gstrings[i] = &gs->stringtab_dev;
+			gs->stringtab_dev.strings = gs->strings;
+			gs->strings[USB_GADGET_MANUFACTURER_IDX].s =
+				gs->manufacturer;
+			gs->strings[USB_GADGET_PRODUCT_IDX].s = gs->product;
+			gs->strings[USB_GADGET_SERIAL_IDX].s = gs->serialnumber;
+			i++;
+		}
+		gi->gstrings[i] = NULL;
+		s = usb_gstrings_attach(&gi->cdev, gi->gstrings,
+				USB_GADGET_FIRST_AVAIL_IDX);
+		if (IS_ERR(s))
+			goto err_comp_cleanup;
+
+		gi->cdev.desc.iManufacturer = s[USB_GADGET_MANUFACTURER_IDX].id;
+		gi->cdev.desc.iProduct = s[USB_GADGET_PRODUCT_IDX].id;
+		gi->cdev.desc.iSerialNumber = s[USB_GADGET_SERIAL_IDX].id;
+	}
+
+	/* Go through all configs, attach all functions */
+	list_for_each_entry(c, &gi->cdev.configs, list) {
+		struct config_usb_cfg *cfg;
+		struct usb_function *f;
+		struct usb_function *tmp;
+		struct gadget_config_name *cn;
+
+		cfg = container_of(c, struct config_usb_cfg, c);
+		if (!list_empty(&cfg->string_list)) {
+			i = 0;
+			list_for_each_entry(cn, &cfg->string_list, list) {
+				cfg->gstrings[i] = &cn->stringtab_dev;
+				cn->stringtab_dev.strings = &cn->strings;
+				cn->strings.s = cn->configuration;
+				i++;
+			}
+			cfg->gstrings[i] = NULL;
+			s = usb_gstrings_attach(&gi->cdev, cfg->gstrings, 1);
+			if (IS_ERR(s))
+				goto err_comp_cleanup;
+			c->iConfiguration = s[0].id;
+		}
+
+		list_for_each_entry_safe(f, tmp, &cfg->func_list, list) {
+			list_del(&f->list);
+			ret = usb_add_function(c, f);
+			if (ret)
+				goto err_purge_funcs;
+		}
+		usb_ep_autoconfig_reset(cdev->gadget);
+	}
+	usb_ep_autoconfig_reset(cdev->gadget);
+	return 0;
+
+err_purge_funcs:
+	purge_configs_funcs(gi);
+err_comp_cleanup:
+	composite_dev_cleanup(cdev);
+	return ret;
+}
+
+static void configfs_composite_unbind(struct usb_gadget *gadget)
+{
+	struct usb_composite_dev	*cdev;
+	struct gadget_info		*gi;
+
+	/* the gi->lock is hold by the caller */
+
+	cdev = get_gadget_data(gadget);
+	gi = container_of(cdev, struct gadget_info, cdev);
+
+	purge_configs_funcs(gi);
+	composite_dev_cleanup(cdev);
+	usb_ep_autoconfig_reset(cdev->gadget);
+	cdev->gadget = NULL;
+	set_gadget_data(gadget, NULL);
+}
+
+static const struct usb_gadget_driver configfs_driver_template = {
+	.bind           = configfs_composite_bind,
+	.unbind         = configfs_composite_unbind,
+
+	.setup          = composite_setup,
+	.disconnect     = composite_disconnect,
+
+	.max_speed	= USB_SPEED_SUPER,
+	.driver = {
+		.owner          = THIS_MODULE,
+		.name		= "configfs-gadget",
+	},
+};
+
+static struct config_group *gadgets_make(
+		struct config_group *group,
+		const char *name)
+{
+	struct gadget_info *gi;
+
+	gi = kzalloc(sizeof(*gi), GFP_KERNEL);
+	if (!gi)
+		return ERR_PTR(-ENOMEM);
+
+	gi->group.default_groups = gi->default_groups;
+	gi->group.default_groups[0] = &gi->functions_group;
+	gi->group.default_groups[1] = &gi->configs_group;
+	gi->group.default_groups[2] = &gi->strings_group;
+
+	config_group_init_type_name(&gi->functions_group, "functions",
+			&functions_type);
+	config_group_init_type_name(&gi->configs_group, "configs",
+			&config_desc_type);
+	config_group_init_type_name(&gi->strings_group, "strings",
+			&gadget_strings_strings_type);
+
+	gi->composite.bind = configfs_do_nothing;
+	gi->composite.unbind = configfs_do_nothing;
+	gi->composite.suspend = NULL;
+	gi->composite.resume = NULL;
+	gi->composite.max_speed = USB_SPEED_SUPER;
+
+	mutex_init(&gi->lock);
+	INIT_LIST_HEAD(&gi->string_list);
+	INIT_LIST_HEAD(&gi->available_func);
+
+	composite_init_dev(&gi->cdev);
+	gi->cdev.desc.bLength = USB_DT_DEVICE_SIZE;
+	gi->cdev.desc.bDescriptorType = USB_DT_DEVICE;
+	gi->cdev.desc.bcdDevice = cpu_to_le16(get_default_bcdDevice());
+
+	gi->composite.gadget_driver = configfs_driver_template;
+
+	gi->composite.gadget_driver.function = kstrdup(name, GFP_KERNEL);
+	gi->composite.name = gi->composite.gadget_driver.function;
+
+	if (!gi->composite.gadget_driver.function)
+		goto err;
+
+#ifdef CONFIG_USB_OTG
+	gi->otg.bLength = sizeof(struct usb_otg_descriptor);
+	gi->otg.bDescriptorType = USB_DT_OTG;
+	gi->otg.bmAttributes = USB_OTG_SRP | USB_OTG_HNP;
+#endif
+
+	config_group_init_type_name(&gi->group, name,
+				&gadget_root_type);
+	return &gi->group;
+err:
+	kfree(gi);
+	return ERR_PTR(-ENOMEM);
+}
+
+static void gadgets_drop(struct config_group *group, struct config_item *item)
+{
+	config_item_put(item);
+}
+
+static struct configfs_group_operations gadgets_ops = {
+	.make_group     = &gadgets_make,
+	.drop_item      = &gadgets_drop,
+};
+
+static struct config_item_type gadgets_type = {
+	.ct_group_ops   = &gadgets_ops,
+	.ct_owner       = THIS_MODULE,
+};
+
+static struct configfs_subsystem gadget_subsys = {
+	.su_group = {
+		.cg_item = {
+			.ci_namebuf = "usb_gadget",
+			.ci_type = &gadgets_type,
+		},
+	},
+	.su_mutex = __MUTEX_INITIALIZER(gadget_subsys.su_mutex),
+};
+
+static int __init gadget_cfs_init(void)
+{
+	int ret;
+
+	config_group_init(&gadget_subsys.su_group);
+
+	ret = configfs_register_subsystem(&gadget_subsys);
+	return ret;
+}
+module_init(gadget_cfs_init);
+
+static void __exit gadget_cfs_exit(void)
+{
+	configfs_unregister_subsystem(&gadget_subsys);
+}
+module_exit(gadget_cfs_exit);
diff --git a/drivers/usb/gadget/dummy_hcd.c b/drivers/usb/gadget/dummy_hcd.c
index 8cf0c0f6fa1..a792e322f4f 100644
--- a/drivers/usb/gadget/dummy_hcd.c
+++ b/drivers/usb/gadget/dummy_hcd.c
@@ -912,7 +912,6 @@ static int dummy_udc_start(struct usb_gadget *g,
 	dum->devstatus = 0;
 
 	dum->driver = driver;
-	dum->gadget.dev.driver = &driver->driver;
 	dev_dbg(udc_dev(dum), "binding gadget driver '%s'\n",
 			driver->driver.name);
 	return 0;
@@ -927,7 +926,6 @@ static int dummy_udc_stop(struct usb_gadget *g,
 	dev_dbg(udc_dev(dum), "unregister gadget driver '%s'\n",
 			driver->driver.name);
 
-	dum->gadget.dev.driver = NULL;
 	dum->driver = NULL;
 
 	return 0;
@@ -937,11 +935,6 @@ static int dummy_udc_stop(struct usb_gadget *g,
 
 /* The gadget structure is stored inside the hcd structure and will be
  * released along with it. */
-static void dummy_gadget_release(struct device *dev)
-{
-	return;
-}
-
 static void init_dummy_udc_hw(struct dummy *dum)
 {
 	int i;
@@ -984,15 +977,7 @@ static int dummy_udc_probe(struct platform_device *pdev)
 	dum->gadget.ops = &dummy_ops;
 	dum->gadget.max_speed = USB_SPEED_SUPER;
 
-	dev_set_name(&dum->gadget.dev, "gadget");
 	dum->gadget.dev.parent = &pdev->dev;
-	dum->gadget.dev.release = dummy_gadget_release;
-	rc = device_register(&dum->gadget.dev);
-	if (rc < 0) {
-		put_device(&dum->gadget.dev);
-		return rc;
-	}
-
 	init_dummy_udc_hw(dum);
 
 	rc = usb_add_gadget_udc(&pdev->dev, &dum->gadget);
@@ -1008,7 +993,6 @@ static int dummy_udc_probe(struct platform_device *pdev)
 err_dev:
 	usb_del_gadget_udc(&dum->gadget);
 err_udc:
-	device_unregister(&dum->gadget.dev);
 	return rc;
 }
 
@@ -1019,7 +1003,6 @@ static int dummy_udc_remove(struct platform_device *pdev)
 	usb_del_gadget_udc(&dum->gadget);
 	platform_set_drvdata(pdev, NULL);
 	device_remove_file(&dum->gadget.dev, &dev_attr_function);
-	device_unregister(&dum->gadget.dev);
 	return 0;
 }
 
@@ -1923,7 +1906,7 @@ done:
 }
 
 /* usb 3.0 root hub device descriptor */
-struct {
+static struct {
 	struct usb_bos_descriptor bos;
 	struct usb_ss_cap_descriptor ss_cap;
 } __packed usb3_bos_desc = {
diff --git a/drivers/usb/gadget/ether.c b/drivers/usb/gadget/ether.c
index 18c3f423706..56c8ecae9bc 100644
--- a/drivers/usb/gadget/ether.c
+++ b/drivers/usb/gadget/ether.c
@@ -207,7 +207,7 @@ static struct usb_gadget_strings *dev_strings[] = {
 };
 
 static u8 hostaddr[ETH_ALEN];
-
+static struct eth_dev *the_dev;
 /*-------------------------------------------------------------------------*/
 
 /*
@@ -224,7 +224,7 @@ static int __init rndis_do_config(struct usb_configuration *c)
 		c->bmAttributes |= USB_CONFIG_ATT_WAKEUP;
 	}
 
-	return rndis_bind_config(c, hostaddr);
+	return rndis_bind_config(c, hostaddr, the_dev);
 }
 
 static struct usb_configuration rndis_config_driver = {
@@ -257,11 +257,11 @@ static int __init eth_do_config(struct usb_configuration *c)
 	}
 
 	if (use_eem)
-		return eem_bind_config(c);
+		return eem_bind_config(c, the_dev);
 	else if (can_support_ecm(c->cdev->gadget))
-		return ecm_bind_config(c, hostaddr);
+		return ecm_bind_config(c, hostaddr, the_dev);
 	else
-		return geth_bind_config(c, hostaddr);
+		return geth_bind_config(c, hostaddr, the_dev);
 }
 
 static struct usb_configuration eth_config_driver = {
@@ -279,9 +279,9 @@ static int __init eth_bind(struct usb_composite_dev *cdev)
 	int			status;
 
 	/* set up network link layer */
-	status = gether_setup(cdev->gadget, hostaddr);
-	if (status < 0)
-		return status;
+	the_dev = gether_setup(cdev->gadget, hostaddr);
+	if (IS_ERR(the_dev))
+		return PTR_ERR(the_dev);
 
 	/* set up main config label and device descriptor */
 	if (use_eem) {
@@ -338,13 +338,13 @@ static int __init eth_bind(struct usb_composite_dev *cdev)
 	return 0;
 
 fail:
-	gether_cleanup();
+	gether_cleanup(the_dev);
 	return status;
 }
 
 static int __exit eth_unbind(struct usb_composite_dev *cdev)
 {
-	gether_cleanup();
+	gether_cleanup(the_dev);
 	return 0;
 }
 
diff --git a/drivers/usb/gadget/f_acm.c b/drivers/usb/gadget/f_acm.c
index 1ae180baa59..4b7e33e5d9c 100644
--- a/drivers/usb/gadget/f_acm.c
+++ b/drivers/usb/gadget/f_acm.c
@@ -715,13 +715,31 @@ fail:
 	return status;
 }
 
-static struct f_acm *acm_alloc_basic_func(void)
+static void acm_unbind(struct usb_configuration *c, struct usb_function *f)
+{
+	struct f_acm		*acm = func_to_acm(f);
+
+	acm_string_defs[0].id = 0;
+	usb_free_all_descriptors(f);
+	if (acm->notify_req)
+		gs_free_req(acm->notify, acm->notify_req);
+}
+
+static void acm_free_func(struct usb_function *f)
+{
+	struct f_acm		*acm = func_to_acm(f);
+
+	kfree(acm);
+}
+
+static struct usb_function *acm_alloc_func(struct usb_function_instance *fi)
 {
-	struct f_acm	*acm;
+	struct f_serial_opts *opts;
+	struct f_acm *acm;
 
 	acm = kzalloc(sizeof(*acm), GFP_KERNEL);
 	if (!acm)
-		return NULL;
+		return ERR_PTR(-ENOMEM);
 
 	spin_lock_init(&acm->lock);
 
@@ -730,109 +748,100 @@ static struct f_acm *acm_alloc_basic_func(void)
 	acm->port.send_break = acm_send_break;
 
 	acm->port.func.name = "acm";
+	acm->port.func.strings = acm_strings;
 	/* descriptors are per-instance copies */
 	acm->port.func.bind = acm_bind;
 	acm->port.func.set_alt = acm_set_alt;
 	acm->port.func.setup = acm_setup;
 	acm->port.func.disable = acm_disable;
 
-	return acm;
+	opts = container_of(fi, struct f_serial_opts, func_inst);
+	acm->port_num = opts->port_num;
+	acm->port.func.unbind = acm_unbind;
+	acm->port.func.free_func = acm_free_func;
+
+	return &acm->port.func;
 }
 
-#ifdef USB_FACM_INCLUDED
-static void
-acm_old_unbind(struct usb_configuration *c, struct usb_function *f)
+static inline struct f_serial_opts *to_f_serial_opts(struct config_item *item)
 {
-	struct f_acm		*acm = func_to_acm(f);
-
-	usb_free_all_descriptors(f);
-	if (acm->notify_req)
-		gs_free_req(acm->notify, acm->notify_req);
-	kfree(acm);
+	return container_of(to_config_group(item), struct f_serial_opts,
+			func_inst.group);
 }
 
-/**
- * acm_bind_config - add a CDC ACM function to a configuration
- * @c: the configuration to support the CDC ACM instance
- * @port_num: /dev/ttyGS* port this interface will use
- * Context: single threaded during gadget setup
- *
- * Returns zero on success, else negative errno.
- *
- */
-int acm_bind_config(struct usb_configuration *c, u8 port_num)
+CONFIGFS_ATTR_STRUCT(f_serial_opts);
+static ssize_t f_acm_attr_show(struct config_item *item,
+				 struct configfs_attribute *attr,
+				 char *page)
 {
-	struct f_acm	*acm;
-	int		status;
-
-	/* allocate and initialize one new instance */
-	acm = acm_alloc_basic_func();
-	if (!acm)
-		return -ENOMEM;
-
-	acm->port_num = port_num;
-	acm->port.func.unbind = acm_old_unbind;
-
-	status = usb_add_function(c, &acm->port.func);
-	if (status)
-		kfree(acm);
-	return status;
+	struct f_serial_opts *opts = to_f_serial_opts(item);
+	struct f_serial_opts_attribute *f_serial_opts_attr =
+		container_of(attr, struct f_serial_opts_attribute, attr);
+	ssize_t ret = 0;
+
+	if (f_serial_opts_attr->show)
+		ret = f_serial_opts_attr->show(opts, page);
+	return ret;
 }
 
-#else
-
-static void acm_unbind(struct usb_configuration *c, struct usb_function *f)
+static void acm_attr_release(struct config_item *item)
 {
-	struct f_acm		*acm = func_to_acm(f);
+	struct f_serial_opts *opts = to_f_serial_opts(item);
 
-	acm_string_defs[0].id = 0;
-	usb_free_all_descriptors(f);
-	if (acm->notify_req)
-		gs_free_req(acm->notify, acm->notify_req);
+	usb_put_function_instance(&opts->func_inst);
 }
 
-static void acm_free_func(struct usb_function *f)
-{
-	struct f_acm		*acm = func_to_acm(f);
+static struct configfs_item_operations acm_item_ops = {
+	.release                = acm_attr_release,
+	.show_attribute		= f_acm_attr_show,
+};
 
-	kfree(acm);
+static ssize_t f_acm_port_num_show(struct f_serial_opts *opts, char *page)
+{
+	return sprintf(page, "%u\n", opts->port_num);
 }
 
-static struct usb_function *acm_alloc_func(struct usb_function_instance *fi)
-{
-	struct f_serial_opts *opts;
-	struct f_acm *acm;
+static struct f_serial_opts_attribute f_acm_port_num =
+	__CONFIGFS_ATTR_RO(port_num, f_acm_port_num_show);
 
-	acm = acm_alloc_basic_func();
-	if (!acm)
-		return ERR_PTR(-ENOMEM);
 
-	opts = container_of(fi, struct f_serial_opts, func_inst);
-	acm->port_num = opts->port_num;
-	acm->port.func.unbind = acm_unbind;
-	acm->port.func.free_func = acm_free_func;
+static struct configfs_attribute *acm_attrs[] = {
+	&f_acm_port_num.attr,
+	NULL,
+};
 
-	return &acm->port.func;
-}
+static struct config_item_type acm_func_type = {
+	.ct_item_ops    = &acm_item_ops,
+	.ct_attrs	= acm_attrs,
+	.ct_owner       = THIS_MODULE,
+};
 
 static void acm_free_instance(struct usb_function_instance *fi)
 {
 	struct f_serial_opts *opts;
 
 	opts = container_of(fi, struct f_serial_opts, func_inst);
+	gserial_free_line(opts->port_num);
 	kfree(opts);
 }
 
 static struct usb_function_instance *acm_alloc_instance(void)
 {
 	struct f_serial_opts *opts;
+	int ret;
 
 	opts = kzalloc(sizeof(*opts), GFP_KERNEL);
 	if (!opts)
 		return ERR_PTR(-ENOMEM);
 	opts->func_inst.free_func_inst = acm_free_instance;
+	ret = gserial_alloc_line(&opts->port_num);
+	if (ret) {
+		kfree(opts);
+		return ERR_PTR(ret);
+	}
+	config_group_init_type_name(&opts->func_inst.group, "",
+			&acm_func_type);
 	return &opts->func_inst;
 }
 DECLARE_USB_FUNCTION_INIT(acm, acm_alloc_instance, acm_alloc_func);
 MODULE_LICENSE("GPL");
-#endif
diff --git a/drivers/usb/gadget/f_ecm.c b/drivers/usb/gadget/f_ecm.c
index 83420a310fb..d893d692907 100644
--- a/drivers/usb/gadget/f_ecm.c
+++ b/drivers/usb/gadget/f_ecm.c
@@ -824,7 +824,8 @@ ecm_unbind(struct usb_configuration *c, struct usb_function *f)
  * for calling @gether_cleanup() before module unload.
  */
 int
-ecm_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN])
+ecm_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN],
+		struct eth_dev *dev)
 {
 	struct f_ecm	*ecm;
 	int		status;
@@ -852,6 +853,7 @@ ecm_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN])
 	snprintf(ecm->ethaddr, sizeof ecm->ethaddr, "%pm", ethaddr);
 	ecm_string_defs[1].s = ecm->ethaddr;
 
+	ecm->port.ioport = dev;
 	ecm->port.cdc_filter = DEFAULT_FILTER;
 
 	ecm->port.func.name = "cdc_ethernet";
diff --git a/drivers/usb/gadget/f_eem.c b/drivers/usb/gadget/f_eem.c
index cf0ebee8556..f4e0bbef602 100644
--- a/drivers/usb/gadget/f_eem.c
+++ b/drivers/usb/gadget/f_eem.c
@@ -528,7 +528,7 @@ error:
  * Caller must have called @gether_setup().  Caller is also responsible
  * for calling @gether_cleanup() before module unload.
  */
-int __init eem_bind_config(struct usb_configuration *c)
+int __init eem_bind_config(struct usb_configuration *c, struct eth_dev *dev)
 {
 	struct f_eem	*eem;
 	int		status;
@@ -549,6 +549,7 @@ int __init eem_bind_config(struct usb_configuration *c)
 	if (!eem)
 		return -ENOMEM;
 
+	eem->port.ioport = dev;
 	eem->port.cdc_filter = DEFAULT_FILTER;
 
 	eem->port.func.name = "cdc_eem";
diff --git a/drivers/usb/gadget/f_ncm.c b/drivers/usb/gadget/f_ncm.c
index 5e7557e23ec..ee19bc8d004 100644
--- a/drivers/usb/gadget/f_ncm.c
+++ b/drivers/usb/gadget/f_ncm.c
@@ -1287,7 +1287,8 @@ ncm_unbind(struct usb_configuration *c, struct usb_function *f)
  * Caller must have called @gether_setup().  Caller is also responsible
  * for calling @gether_cleanup() before module unload.
  */
-int __init ncm_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN])
+int __init ncm_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN],
+		struct eth_dev *dev)
 {
 	struct f_ncm	*ncm;
 	int		status;
@@ -1321,6 +1322,7 @@ int __init ncm_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN])
 
 	spin_lock_init(&ncm->lock);
 	ncm_reset_values(ncm);
+	ncm->port.ioport = dev;
 	ncm->port.is_fixed = true;
 
 	ncm->port.func.name = "cdc_network";
diff --git a/drivers/usb/gadget/f_obex.c b/drivers/usb/gadget/f_obex.c
index 36a004563b8..8aa2be5329b 100644
--- a/drivers/usb/gadget/f_obex.c
+++ b/drivers/usb/gadget/f_obex.c
@@ -72,7 +72,7 @@ static struct usb_gadget_strings *obex_strings[] = {
 
 /*-------------------------------------------------------------------------*/
 
-static struct usb_interface_descriptor obex_control_intf __initdata = {
+static struct usb_interface_descriptor obex_control_intf = {
 	.bLength		= sizeof(obex_control_intf),
 	.bDescriptorType	= USB_DT_INTERFACE,
 	.bInterfaceNumber	= 0,
@@ -83,7 +83,7 @@ static struct usb_interface_descriptor obex_control_intf __initdata = {
 	.bInterfaceSubClass	= USB_CDC_SUBCLASS_OBEX,
 };
 
-static struct usb_interface_descriptor obex_data_nop_intf __initdata = {
+static struct usb_interface_descriptor obex_data_nop_intf = {
 	.bLength		= sizeof(obex_data_nop_intf),
 	.bDescriptorType	= USB_DT_INTERFACE,
 	.bInterfaceNumber	= 1,
@@ -93,7 +93,7 @@ static struct usb_interface_descriptor obex_data_nop_intf __initdata = {
 	.bInterfaceClass	= USB_CLASS_CDC_DATA,
 };
 
-static struct usb_interface_descriptor obex_data_intf __initdata = {
+static struct usb_interface_descriptor obex_data_intf = {
 	.bLength		= sizeof(obex_data_intf),
 	.bDescriptorType	= USB_DT_INTERFACE,
 	.bInterfaceNumber	= 2,
@@ -103,14 +103,14 @@ static struct usb_interface_descriptor obex_data_intf __initdata = {
 	.bInterfaceClass	= USB_CLASS_CDC_DATA,
 };
 
-static struct usb_cdc_header_desc obex_cdc_header_desc __initdata = {
+static struct usb_cdc_header_desc obex_cdc_header_desc = {
 	.bLength		= sizeof(obex_cdc_header_desc),
 	.bDescriptorType	= USB_DT_CS_INTERFACE,
 	.bDescriptorSubType	= USB_CDC_HEADER_TYPE,
 	.bcdCDC			= cpu_to_le16(0x0120),
 };
 
-static struct usb_cdc_union_desc obex_cdc_union_desc __initdata = {
+static struct usb_cdc_union_desc obex_cdc_union_desc = {
 	.bLength		= sizeof(obex_cdc_union_desc),
 	.bDescriptorType	= USB_DT_CS_INTERFACE,
 	.bDescriptorSubType	= USB_CDC_UNION_TYPE,
@@ -118,7 +118,7 @@ static struct usb_cdc_union_desc obex_cdc_union_desc __initdata = {
 	.bSlaveInterface0	= 2,
 };
 
-static struct usb_cdc_obex_desc obex_desc __initdata = {
+static struct usb_cdc_obex_desc obex_desc = {
 	.bLength		= sizeof(obex_desc),
 	.bDescriptorType	= USB_DT_CS_INTERFACE,
 	.bDescriptorSubType	= USB_CDC_OBEX_TYPE,
@@ -127,7 +127,7 @@ static struct usb_cdc_obex_desc obex_desc __initdata = {
 
 /* High-Speed Support */
 
-static struct usb_endpoint_descriptor obex_hs_ep_out_desc __initdata = {
+static struct usb_endpoint_descriptor obex_hs_ep_out_desc = {
 	.bLength		= USB_DT_ENDPOINT_SIZE,
 	.bDescriptorType	= USB_DT_ENDPOINT,
 
@@ -136,7 +136,7 @@ static struct usb_endpoint_descriptor obex_hs_ep_out_desc __initdata = {
 	.wMaxPacketSize		= cpu_to_le16(512),
 };
 
-static struct usb_endpoint_descriptor obex_hs_ep_in_desc __initdata = {
+static struct usb_endpoint_descriptor obex_hs_ep_in_desc = {
 	.bLength		= USB_DT_ENDPOINT_SIZE,
 	.bDescriptorType	= USB_DT_ENDPOINT,
 
@@ -145,7 +145,7 @@ static struct usb_endpoint_descriptor obex_hs_ep_in_desc __initdata = {
 	.wMaxPacketSize		= cpu_to_le16(512),
 };
 
-static struct usb_descriptor_header *hs_function[] __initdata = {
+static struct usb_descriptor_header *hs_function[] = {
 	(struct usb_descriptor_header *) &obex_control_intf,
 	(struct usb_descriptor_header *) &obex_cdc_header_desc,
 	(struct usb_descriptor_header *) &obex_desc,
@@ -160,7 +160,7 @@ static struct usb_descriptor_header *hs_function[] __initdata = {
 
 /* Full-Speed Support */
 
-static struct usb_endpoint_descriptor obex_fs_ep_in_desc __initdata = {
+static struct usb_endpoint_descriptor obex_fs_ep_in_desc = {
 	.bLength		= USB_DT_ENDPOINT_SIZE,
 	.bDescriptorType	= USB_DT_ENDPOINT,
 
@@ -168,7 +168,7 @@ static struct usb_endpoint_descriptor obex_fs_ep_in_desc __initdata = {
 	.bmAttributes		= USB_ENDPOINT_XFER_BULK,
 };
 
-static struct usb_endpoint_descriptor obex_fs_ep_out_desc __initdata = {
+static struct usb_endpoint_descriptor obex_fs_ep_out_desc = {
 	.bLength		= USB_DT_ENDPOINT_SIZE,
 	.bDescriptorType	= USB_DT_ENDPOINT,
 
@@ -176,7 +176,7 @@ static struct usb_endpoint_descriptor obex_fs_ep_out_desc __initdata = {
 	.bmAttributes		= USB_ENDPOINT_XFER_BULK,
 };
 
-static struct usb_descriptor_header *fs_function[] __initdata = {
+static struct usb_descriptor_header *fs_function[] = {
 	(struct usb_descriptor_header *) &obex_control_intf,
 	(struct usb_descriptor_header *) &obex_cdc_header_desc,
 	(struct usb_descriptor_header *) &obex_desc,
@@ -290,14 +290,43 @@ static void obex_disconnect(struct gserial *g)
 
 /*-------------------------------------------------------------------------*/
 
-static int __init
-obex_bind(struct usb_configuration *c, struct usb_function *f)
+/* Some controllers can't support CDC OBEX ... */
+static inline bool can_support_obex(struct usb_configuration *c)
+{
+	/* Since the first interface is a NOP, we can ignore the
+	 * issue of multi-interface support on most controllers.
+	 *
+	 * Altsettings are mandatory, however...
+	 */
+	if (!gadget_supports_altsettings(c->cdev->gadget))
+		return false;
+
+	/* everything else is *probably* fine ... */
+	return true;
+}
+
+static int obex_bind(struct usb_configuration *c, struct usb_function *f)
 {
 	struct usb_composite_dev *cdev = c->cdev;
 	struct f_obex		*obex = func_to_obex(f);
 	int			status;
 	struct usb_ep		*ep;
 
+	if (!can_support_obex(c))
+		return -EINVAL;
+
+	if (obex_string_defs[OBEX_CTRL_IDX].id == 0) {
+		status = usb_string_ids_tab(c->cdev, obex_string_defs);
+		if (status < 0)
+			return status;
+		obex_control_intf.iInterface =
+			obex_string_defs[OBEX_CTRL_IDX].id;
+
+		status = obex_string_defs[OBEX_DATA_IDX].id;
+		obex_data_nop_intf.iInterface = status;
+		obex_data_intf.iInterface = status;
+	}
+
 	/* allocate instance-specific interface IDs, and patch descriptors */
 
 	status = usb_interface_id(c, f);
@@ -319,6 +348,7 @@ obex_bind(struct usb_configuration *c, struct usb_function *f)
 
 	/* allocate instance-specific endpoints */
 
+	status = -ENODEV;
 	ep = usb_ep_autoconfig(cdev->gadget, &obex_fs_ep_in_desc);
 	if (!ep)
 		goto fail;
@@ -376,29 +406,16 @@ fail:
 	return status;
 }
 
+#ifdef USBF_OBEX_INCLUDED
+
 static void
-obex_unbind(struct usb_configuration *c, struct usb_function *f)
+obex_old_unbind(struct usb_configuration *c, struct usb_function *f)
 {
 	obex_string_defs[OBEX_CTRL_IDX].id = 0;
 	usb_free_all_descriptors(f);
 	kfree(func_to_obex(f));
 }
 
-/* Some controllers can't support CDC OBEX ... */
-static inline bool can_support_obex(struct usb_configuration *c)
-{
-	/* Since the first interface is a NOP, we can ignore the
-	 * issue of multi-interface support on most controllers.
-	 *
-	 * Altsettings are mandatory, however...
-	 */
-	if (!gadget_supports_altsettings(c->cdev->gadget))
-		return false;
-
-	/* everything else is *probably* fine ... */
-	return true;
-}
-
 /**
  * obex_bind_config - add a CDC OBEX function to a configuration
  * @c: the configuration to support the CDC OBEX instance
@@ -412,21 +429,6 @@ int __init obex_bind_config(struct usb_configuration *c, u8 port_num)
 	struct f_obex	*obex;
 	int		status;
 
-	if (!can_support_obex(c))
-		return -EINVAL;
-
-	if (obex_string_defs[OBEX_CTRL_IDX].id == 0) {
-		status = usb_string_ids_tab(c->cdev, obex_string_defs);
-		if (status < 0)
-			return status;
-		obex_control_intf.iInterface =
-			obex_string_defs[OBEX_CTRL_IDX].id;
-
-		status = obex_string_defs[OBEX_DATA_IDX].id;
-		obex_data_nop_intf.iInterface = status;
-		obex_data_intf.iInterface = status;
-	}
-
 	/* allocate and initialize one new instance */
 	obex = kzalloc(sizeof *obex, GFP_KERNEL);
 	if (!obex)
@@ -441,7 +443,7 @@ int __init obex_bind_config(struct usb_configuration *c, u8 port_num)
 	obex->port.func.strings = obex_strings;
 	/* descriptors are per-instance copies */
 	obex->port.func.bind = obex_bind;
-	obex->port.func.unbind = obex_unbind;
+	obex->port.func.unbind = obex_old_unbind;
 	obex->port.func.set_alt = obex_set_alt;
 	obex->port.func.get_alt = obex_get_alt;
 	obex->port.func.disable = obex_disable;
@@ -453,5 +455,138 @@ int __init obex_bind_config(struct usb_configuration *c, u8 port_num)
 	return status;
 }
 
+#else
+
+static inline struct f_serial_opts *to_f_serial_opts(struct config_item *item)
+{
+	return container_of(to_config_group(item), struct f_serial_opts,
+			    func_inst.group);
+}
+
+CONFIGFS_ATTR_STRUCT(f_serial_opts);
+static ssize_t f_obex_attr_show(struct config_item *item,
+				struct configfs_attribute *attr,
+				char *page)
+{
+	struct f_serial_opts *opts = to_f_serial_opts(item);
+	struct f_serial_opts_attribute *f_serial_opts_attr =
+		container_of(attr, struct f_serial_opts_attribute, attr);
+	ssize_t ret = 0;
+
+	if (f_serial_opts_attr->show)
+		ret = f_serial_opts_attr->show(opts, page);
+
+	return ret;
+}
+
+static void obex_attr_release(struct config_item *item)
+{
+	struct f_serial_opts *opts = to_f_serial_opts(item);
+
+	usb_put_function_instance(&opts->func_inst);
+}
+
+static struct configfs_item_operations obex_item_ops = {
+	.release	= obex_attr_release,
+	.show_attribute = f_obex_attr_show,
+};
+
+static ssize_t f_obex_port_num_show(struct f_serial_opts *opts, char *page)
+{
+	return sprintf(page, "%u\n", opts->port_num);
+}
+
+static struct f_serial_opts_attribute f_obex_port_num =
+	__CONFIGFS_ATTR_RO(port_num, f_obex_port_num_show);
+
+static struct configfs_attribute *acm_attrs[] = {
+	&f_obex_port_num.attr,
+	NULL,
+};
+
+static struct config_item_type obex_func_type = {
+	.ct_item_ops	= &obex_item_ops,
+	.ct_attrs	= acm_attrs,
+	.ct_owner	= THIS_MODULE,
+};
+
+static void obex_free_inst(struct usb_function_instance *f)
+{
+	struct f_serial_opts *opts;
+
+	opts = container_of(f, struct f_serial_opts, func_inst);
+	gserial_free_line(opts->port_num);
+	kfree(opts);
+}
+
+static struct usb_function_instance *obex_alloc_inst(void)
+{
+	struct f_serial_opts *opts;
+	int ret;
+
+	opts = kzalloc(sizeof(*opts), GFP_KERNEL);
+	if (!opts)
+		return ERR_PTR(-ENOMEM);
+
+	opts->func_inst.free_func_inst = obex_free_inst;
+	ret = gserial_alloc_line(&opts->port_num);
+	if (ret) {
+		kfree(opts);
+		return ERR_PTR(ret);
+	}
+	config_group_init_type_name(&opts->func_inst.group, "",
+				    &obex_func_type);
+
+	return &opts->func_inst;
+}
+
+static void obex_free(struct usb_function *f)
+{
+	struct f_obex *obex;
+
+	obex = func_to_obex(f);
+	kfree(obex);
+}
+
+static void obex_unbind(struct usb_configuration *c, struct usb_function *f)
+{
+	obex_string_defs[OBEX_CTRL_IDX].id = 0;
+	usb_free_all_descriptors(f);
+}
+
+struct usb_function *obex_alloc(struct usb_function_instance *fi)
+{
+	struct f_obex	*obex;
+	struct f_serial_opts *opts;
+
+	/* allocate and initialize one new instance */
+	obex = kzalloc(sizeof(*obex), GFP_KERNEL);
+	if (!obex)
+		return ERR_PTR(-ENOMEM);
+
+	opts = container_of(fi, struct f_serial_opts, func_inst);
+
+	obex->port_num = opts->port_num;
+
+	obex->port.connect = obex_connect;
+	obex->port.disconnect = obex_disconnect;
+
+	obex->port.func.name = "obex";
+	obex->port.func.strings = obex_strings;
+	/* descriptors are per-instance copies */
+	obex->port.func.bind = obex_bind;
+	obex->port.func.unbind = obex_unbind;
+	obex->port.func.set_alt = obex_set_alt;
+	obex->port.func.get_alt = obex_get_alt;
+	obex->port.func.disable = obex_disable;
+	obex->port.func.free_func = obex_free;
+
+	return &obex->port.func;
+}
+
+DECLARE_USB_FUNCTION_INIT(obex, obex_alloc_inst, obex_alloc);
+
+#endif
+
 MODULE_AUTHOR("Felipe Balbi");
 MODULE_LICENSE("GPL");
diff --git a/drivers/usb/gadget/f_rndis.c b/drivers/usb/gadget/f_rndis.c
index cc9c49c57c8..36e8c44d8e5 100644
--- a/drivers/usb/gadget/f_rndis.c
+++ b/drivers/usb/gadget/f_rndis.c
@@ -813,7 +813,7 @@ static inline bool can_support_rndis(struct usb_configuration *c)
 
 int
 rndis_bind_config_vendor(struct usb_configuration *c, u8 ethaddr[ETH_ALEN],
-				u32 vendorID, const char *manufacturer)
+		u32 vendorID, const char *manufacturer, struct eth_dev *dev)
 {
 	struct f_rndis	*rndis;
 	int		status;
@@ -846,6 +846,7 @@ rndis_bind_config_vendor(struct usb_configuration *c, u8 ethaddr[ETH_ALEN],
 	rndis->vendorID = vendorID;
 	rndis->manufacturer = manufacturer;
 
+	rndis->port.ioport = dev;
 	/* RNDIS activates when the host changes this filter */
 	rndis->port.cdc_filter = 0;
 
diff --git a/drivers/usb/gadget/f_serial.c b/drivers/usb/gadget/f_serial.c
index da33cfb3031..981113c9924 100644
--- a/drivers/usb/gadget/f_serial.c
+++ b/drivers/usb/gadget/f_serial.c
@@ -12,6 +12,7 @@
 
 #include <linux/slab.h>
 #include <linux/kernel.h>
+#include <linux/module.h>
 #include <linux/device.h>
 
 #include "u_serial.h"
@@ -42,7 +43,7 @@ static inline struct f_gser *func_to_gser(struct usb_function *f)
 
 /* interface descriptor: */
 
-static struct usb_interface_descriptor gser_interface_desc __initdata = {
+static struct usb_interface_descriptor gser_interface_desc = {
 	.bLength =		USB_DT_INTERFACE_SIZE,
 	.bDescriptorType =	USB_DT_INTERFACE,
 	/* .bInterfaceNumber = DYNAMIC */
@@ -55,21 +56,21 @@ static struct usb_interface_descriptor gser_interface_desc __initdata = {
 
 /* full speed support: */
 
-static struct usb_endpoint_descriptor gser_fs_in_desc __initdata = {
+static struct usb_endpoint_descriptor gser_fs_in_desc = {
 	.bLength =		USB_DT_ENDPOINT_SIZE,
 	.bDescriptorType =	USB_DT_ENDPOINT,
 	.bEndpointAddress =	USB_DIR_IN,
 	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
 };
 
-static struct usb_endpoint_descriptor gser_fs_out_desc __initdata = {
+static struct usb_endpoint_descriptor gser_fs_out_desc = {
 	.bLength =		USB_DT_ENDPOINT_SIZE,
 	.bDescriptorType =	USB_DT_ENDPOINT,
 	.bEndpointAddress =	USB_DIR_OUT,
 	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
 };
 
-static struct usb_descriptor_header *gser_fs_function[] __initdata = {
+static struct usb_descriptor_header *gser_fs_function[] = {
 	(struct usb_descriptor_header *) &gser_interface_desc,
 	(struct usb_descriptor_header *) &gser_fs_in_desc,
 	(struct usb_descriptor_header *) &gser_fs_out_desc,
@@ -78,47 +79,47 @@ static struct usb_descriptor_header *gser_fs_function[] __initdata = {
 
 /* high speed support: */
 
-static struct usb_endpoint_descriptor gser_hs_in_desc __initdata = {
+static struct usb_endpoint_descriptor gser_hs_in_desc = {
 	.bLength =		USB_DT_ENDPOINT_SIZE,
 	.bDescriptorType =	USB_DT_ENDPOINT,
 	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
 	.wMaxPacketSize =	cpu_to_le16(512),
 };
 
-static struct usb_endpoint_descriptor gser_hs_out_desc __initdata = {
+static struct usb_endpoint_descriptor gser_hs_out_desc = {
 	.bLength =		USB_DT_ENDPOINT_SIZE,
 	.bDescriptorType =	USB_DT_ENDPOINT,
 	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
 	.wMaxPacketSize =	cpu_to_le16(512),
 };
 
-static struct usb_descriptor_header *gser_hs_function[] __initdata = {
+static struct usb_descriptor_header *gser_hs_function[] = {
 	(struct usb_descriptor_header *) &gser_interface_desc,
 	(struct usb_descriptor_header *) &gser_hs_in_desc,
 	(struct usb_descriptor_header *) &gser_hs_out_desc,
 	NULL,
 };
 
-static struct usb_endpoint_descriptor gser_ss_in_desc __initdata = {
+static struct usb_endpoint_descriptor gser_ss_in_desc = {
 	.bLength =		USB_DT_ENDPOINT_SIZE,
 	.bDescriptorType =	USB_DT_ENDPOINT,
 	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
 	.wMaxPacketSize =	cpu_to_le16(1024),
 };
 
-static struct usb_endpoint_descriptor gser_ss_out_desc __initdata = {
+static struct usb_endpoint_descriptor gser_ss_out_desc = {
 	.bLength =		USB_DT_ENDPOINT_SIZE,
 	.bDescriptorType =	USB_DT_ENDPOINT,
 	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
 	.wMaxPacketSize =	cpu_to_le16(1024),
 };
 
-static struct usb_ss_ep_comp_descriptor gser_ss_bulk_comp_desc __initdata = {
+static struct usb_ss_ep_comp_descriptor gser_ss_bulk_comp_desc = {
 	.bLength =              sizeof gser_ss_bulk_comp_desc,
 	.bDescriptorType =      USB_DT_SS_ENDPOINT_COMP,
 };
 
-static struct usb_descriptor_header *gser_ss_function[] __initdata = {
+static struct usb_descriptor_header *gser_ss_function[] = {
 	(struct usb_descriptor_header *) &gser_interface_desc,
 	(struct usb_descriptor_header *) &gser_ss_in_desc,
 	(struct usb_descriptor_header *) &gser_ss_bulk_comp_desc,
@@ -183,14 +184,25 @@ static void gser_disable(struct usb_function *f)
 
 /* serial function driver setup/binding */
 
-static int __init
-gser_bind(struct usb_configuration *c, struct usb_function *f)
+static int gser_bind(struct usb_configuration *c, struct usb_function *f)
 {
 	struct usb_composite_dev *cdev = c->cdev;
 	struct f_gser		*gser = func_to_gser(f);
 	int			status;
 	struct usb_ep		*ep;
 
+	/* REVISIT might want instance-specific strings to help
+	 * distinguish instances ...
+	 */
+
+	/* maybe allocate device-global string ID */
+	if (gser_string_defs[0].id == 0) {
+		status = usb_string_id(c->cdev);
+		if (status < 0)
+			return status;
+		gser_string_defs[0].id = status;
+	}
+
 	/* allocate instance-specific interface IDs */
 	status = usb_interface_id(c, f);
 	if (status < 0)
@@ -246,44 +258,115 @@ fail:
 	return status;
 }
 
-static void
-gser_unbind(struct usb_configuration *c, struct usb_function *f)
+static inline struct f_serial_opts *to_f_serial_opts(struct config_item *item)
 {
-	usb_free_all_descriptors(f);
-	kfree(func_to_gser(f));
+	return container_of(to_config_group(item), struct f_serial_opts,
+			    func_inst.group);
 }
 
-/**
- * gser_bind_config - add a generic serial function to a configuration
- * @c: the configuration to support the serial instance
- * @port_num: /dev/ttyGS* port this interface will use
- * Context: single threaded during gadget setup
- *
- * Returns zero on success, else negative errno.
- */
-int __init gser_bind_config(struct usb_configuration *c, u8 port_num)
+CONFIGFS_ATTR_STRUCT(f_serial_opts);
+static ssize_t f_serial_attr_show(struct config_item *item,
+				  struct configfs_attribute *attr,
+				  char *page)
 {
-	struct f_gser	*gser;
-	int		status;
+	struct f_serial_opts *opts = to_f_serial_opts(item);
+	struct f_serial_opts_attribute *f_serial_opts_attr =
+		container_of(attr, struct f_serial_opts_attribute, attr);
+	ssize_t ret = 0;
 
-	/* REVISIT might want instance-specific strings to help
-	 * distinguish instances ...
-	 */
+	if (f_serial_opts_attr->show)
+		ret = f_serial_opts_attr->show(opts, page);
 
-	/* maybe allocate device-global string ID */
-	if (gser_string_defs[0].id == 0) {
-		status = usb_string_id(c->cdev);
-		if (status < 0)
-			return status;
-		gser_string_defs[0].id = status;
+	return ret;
+}
+
+static void serial_attr_release(struct config_item *item)
+{
+	struct f_serial_opts *opts = to_f_serial_opts(item);
+
+	usb_put_function_instance(&opts->func_inst);
+}
+
+static struct configfs_item_operations serial_item_ops = {
+	.release	= serial_attr_release,
+	.show_attribute = f_serial_attr_show,
+};
+
+static ssize_t f_serial_port_num_show(struct f_serial_opts *opts, char *page)
+{
+	return sprintf(page, "%u\n", opts->port_num);
+}
+
+static struct f_serial_opts_attribute f_serial_port_num =
+	__CONFIGFS_ATTR_RO(port_num, f_serial_port_num_show);
+
+static struct configfs_attribute *acm_attrs[] = {
+	&f_serial_port_num.attr,
+	NULL,
+};
+
+static struct config_item_type serial_func_type = {
+	.ct_item_ops	= &serial_item_ops,
+	.ct_attrs	= acm_attrs,
+	.ct_owner	= THIS_MODULE,
+};
+
+static void gser_free_inst(struct usb_function_instance *f)
+{
+	struct f_serial_opts *opts;
+
+	opts = container_of(f, struct f_serial_opts, func_inst);
+	gserial_free_line(opts->port_num);
+	kfree(opts);
+}
+
+static struct usb_function_instance *gser_alloc_inst(void)
+{
+	struct f_serial_opts *opts;
+	int ret;
+
+	opts = kzalloc(sizeof(*opts), GFP_KERNEL);
+	if (!opts)
+		return ERR_PTR(-ENOMEM);
+
+	opts->func_inst.free_func_inst = gser_free_inst;
+	ret = gserial_alloc_line(&opts->port_num);
+	if (ret) {
+		kfree(opts);
+		return ERR_PTR(ret);
 	}
+	config_group_init_type_name(&opts->func_inst.group, "",
+				    &serial_func_type);
+
+	return &opts->func_inst;
+}
+
+static void gser_free(struct usb_function *f)
+{
+	struct f_gser *serial;
+
+	serial = func_to_gser(f);
+	kfree(serial);
+}
+
+static void gser_unbind(struct usb_configuration *c, struct usb_function *f)
+{
+	usb_free_all_descriptors(f);
+}
+
+struct usb_function *gser_alloc(struct usb_function_instance *fi)
+{
+	struct f_gser	*gser;
+	struct f_serial_opts *opts;
 
 	/* allocate and initialize one new instance */
-	gser = kzalloc(sizeof *gser, GFP_KERNEL);
+	gser = kzalloc(sizeof(*gser), GFP_KERNEL);
 	if (!gser)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
-	gser->port_num = port_num;
+	opts = container_of(fi, struct f_serial_opts, func_inst);
+
+	gser->port_num = opts->port_num;
 
 	gser->port.func.name = "gser";
 	gser->port.func.strings = gser_strings;
@@ -291,9 +374,12 @@ int __init gser_bind_config(struct usb_configuration *c, u8 port_num)
 	gser->port.func.unbind = gser_unbind;
 	gser->port.func.set_alt = gser_set_alt;
 	gser->port.func.disable = gser_disable;
+	gser->port.func.free_func = gser_free;
 
-	status = usb_add_function(c, &gser->port.func);
-	if (status)
-		kfree(gser);
-	return status;
+	return &gser->port.func;
 }
+
+DECLARE_USB_FUNCTION_INIT(gser, gser_alloc_inst, gser_alloc);
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Al Borchers");
+MODULE_AUTHOR("David Brownell");
diff --git a/drivers/usb/gadget/f_sourcesink.c b/drivers/usb/gadget/f_sourcesink.c
index 41adf3ef96c..a8895859a22 100644
--- a/drivers/usb/gadget/f_sourcesink.c
+++ b/drivers/usb/gadget/f_sourcesink.c
@@ -898,7 +898,7 @@ static struct usb_function *source_sink_alloc_func(
 	return &ss->function;
 }
 
-static void acm_free_instance(struct usb_function_instance *fi)
+static void source_sink_free_instance(struct usb_function_instance *fi)
 {
 	struct f_ss_opts *ss_opts;
 
@@ -913,7 +913,7 @@ static struct usb_function_instance *source_sink_alloc_inst(void)
 	ss_opts = kzalloc(sizeof(*ss_opts), GFP_KERNEL);
 	if (!ss_opts)
 		return ERR_PTR(-ENOMEM);
-	ss_opts->func_inst.free_func_inst = acm_free_instance;
+	ss_opts->func_inst.free_func_inst = source_sink_free_instance;
 	return &ss_opts->func_inst;
 }
 DECLARE_USB_FUNCTION(SourceSink, source_sink_alloc_inst,
diff --git a/drivers/usb/gadget/f_subset.c b/drivers/usb/gadget/f_subset.c
index f172bd152fb..185d6f5e4e4 100644
--- a/drivers/usb/gadget/f_subset.c
+++ b/drivers/usb/gadget/f_subset.c
@@ -380,7 +380,8 @@ geth_unbind(struct usb_configuration *c, struct usb_function *f)
  * Caller must have called @gether_setup().  Caller is also responsible
  * for calling @gether_cleanup() before module unload.
  */
-int geth_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN])
+int geth_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN],
+		struct eth_dev *dev)
 {
 	struct f_gether	*geth;
 	int		status;
@@ -406,6 +407,7 @@ int geth_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN])
 	snprintf(geth->ethaddr, sizeof geth->ethaddr, "%pm", ethaddr);
 	geth_string_defs[1].s = geth->ethaddr;
 
+	geth->port.ioport = dev;
 	geth->port.cdc_filter = DEFAULT_FILTER;
 
 	geth->port.func.name = "cdc_subset";
diff --git a/drivers/usb/gadget/f_uvc.c b/drivers/usb/gadget/f_uvc.c
index 92efd6ec48a..38dcedddc52 100644
--- a/drivers/usb/gadget/f_uvc.c
+++ b/drivers/usb/gadget/f_uvc.c
@@ -33,19 +33,15 @@ unsigned int uvc_gadget_trace_param;
 /*-------------------------------------------------------------------------*/
 
 /* module parameters specific to the Video streaming endpoint */
-static unsigned streaming_interval = 1;
+static unsigned int streaming_interval = 1;
 module_param(streaming_interval, uint, S_IRUGO|S_IWUSR);
 MODULE_PARM_DESC(streaming_interval, "1 - 16");
 
-static unsigned streaming_maxpacket = 1024;
+static unsigned int streaming_maxpacket = 1024;
 module_param(streaming_maxpacket, uint, S_IRUGO|S_IWUSR);
-MODULE_PARM_DESC(streaming_maxpacket, "0 - 1023 (fs), 0 - 1024 (hs/ss)");
+MODULE_PARM_DESC(streaming_maxpacket, "1 - 1023 (FS), 1 - 3072 (hs/ss)");
 
-static unsigned streaming_mult;
-module_param(streaming_mult, uint, S_IRUGO|S_IWUSR);
-MODULE_PARM_DESC(streaming_mult, "0 - 2 (hs/ss only)");
-
-static unsigned streaming_maxburst;
+static unsigned int streaming_maxburst;
 module_param(streaming_maxburst, uint, S_IRUGO|S_IWUSR);
 MODULE_PARM_DESC(streaming_maxburst, "0 - 15 (ss only)");
 
@@ -55,13 +51,11 @@ MODULE_PARM_DESC(streaming_maxburst, "0 - 15 (ss only)");
 
 /* string IDs are assigned dynamically */
 
-#define UVC_STRING_ASSOCIATION_IDX		0
-#define UVC_STRING_CONTROL_IDX			1
-#define UVC_STRING_STREAMING_IDX		2
+#define UVC_STRING_CONTROL_IDX			0
+#define UVC_STRING_STREAMING_IDX		1
 
 static struct usb_string uvc_en_us_strings[] = {
-	[UVC_STRING_ASSOCIATION_IDX].s = "UVC Camera",
-	[UVC_STRING_CONTROL_IDX].s = "Video Control",
+	[UVC_STRING_CONTROL_IDX].s = "UVC Camera",
 	[UVC_STRING_STREAMING_IDX].s = "Video Streaming",
 	{  }
 };
@@ -79,7 +73,7 @@ static struct usb_gadget_strings *uvc_function_strings[] = {
 #define UVC_INTF_VIDEO_CONTROL			0
 #define UVC_INTF_VIDEO_STREAMING		1
 
-#define STATUS_BYTECOUNT			16	/* 16 bytes status */
+#define UVC_STATUS_MAX_PACKET_SIZE		16	/* 16 bytes status */
 
 static struct usb_interface_assoc_descriptor uvc_iad __initdata = {
 	.bLength		= sizeof(uvc_iad),
@@ -104,20 +98,29 @@ static struct usb_interface_descriptor uvc_control_intf __initdata = {
 	.iInterface		= 0,
 };
 
-static struct usb_endpoint_descriptor uvc_fs_control_ep __initdata = {
+static struct usb_endpoint_descriptor uvc_control_ep __initdata = {
 	.bLength		= USB_DT_ENDPOINT_SIZE,
 	.bDescriptorType	= USB_DT_ENDPOINT,
 	.bEndpointAddress	= USB_DIR_IN,
 	.bmAttributes		= USB_ENDPOINT_XFER_INT,
-	.wMaxPacketSize		= cpu_to_le16(STATUS_BYTECOUNT),
+	.wMaxPacketSize		= cpu_to_le16(UVC_STATUS_MAX_PACKET_SIZE),
 	.bInterval		= 8,
 };
 
+static struct usb_ss_ep_comp_descriptor uvc_ss_control_comp __initdata = {
+	.bLength		= sizeof(uvc_ss_control_comp),
+	.bDescriptorType	= USB_DT_SS_ENDPOINT_COMP,
+	/* The following 3 values can be tweaked if necessary. */
+	.bMaxBurst		= 0,
+	.bmAttributes		= 0,
+	.wBytesPerInterval	= cpu_to_le16(UVC_STATUS_MAX_PACKET_SIZE),
+};
+
 static struct uvc_control_endpoint_descriptor uvc_control_cs_ep __initdata = {
 	.bLength		= UVC_DT_CONTROL_ENDPOINT_SIZE,
 	.bDescriptorType	= USB_DT_CS_ENDPOINT,
 	.bDescriptorSubType	= UVC_EP_INTERRUPT,
-	.wMaxTransferSize	= cpu_to_le16(STATUS_BYTECOUNT),
+	.wMaxTransferSize	= cpu_to_le16(UVC_STATUS_MAX_PACKET_SIZE),
 };
 
 static struct usb_interface_descriptor uvc_streaming_intf_alt0 __initdata = {
@@ -144,63 +147,53 @@ static struct usb_interface_descriptor uvc_streaming_intf_alt1 __initdata = {
 	.iInterface		= 0,
 };
 
-static struct usb_endpoint_descriptor uvc_fs_streaming_ep = {
+static struct usb_endpoint_descriptor uvc_fs_streaming_ep __initdata = {
 	.bLength		= USB_DT_ENDPOINT_SIZE,
 	.bDescriptorType	= USB_DT_ENDPOINT,
 	.bEndpointAddress	= USB_DIR_IN,
-	.bmAttributes		= USB_ENDPOINT_XFER_ISOC,
-	.wMaxPacketSize		= cpu_to_le16(512),
-	.bInterval		= 1,
+	.bmAttributes		= USB_ENDPOINT_SYNC_ASYNC
+				| USB_ENDPOINT_XFER_ISOC,
+	/* The wMaxPacketSize and bInterval values will be initialized from
+	 * module parameters.
+	 */
+	.wMaxPacketSize		= 0,
+	.bInterval		= 0,
 };
 
-static struct usb_endpoint_descriptor uvc_hs_streaming_ep = {
+static struct usb_endpoint_descriptor uvc_hs_streaming_ep __initdata = {
 	.bLength		= USB_DT_ENDPOINT_SIZE,
 	.bDescriptorType	= USB_DT_ENDPOINT,
 	.bEndpointAddress	= USB_DIR_IN,
-	.bmAttributes		= USB_ENDPOINT_XFER_ISOC,
-	.wMaxPacketSize		= cpu_to_le16(1024),
-	.bInterval		= 1,
-};
-
-/* super speed support */
-static struct usb_endpoint_descriptor uvc_ss_control_ep __initdata = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-
-	.bEndpointAddress =	USB_DIR_IN,
-	.bmAttributes =		USB_ENDPOINT_XFER_INT,
-	.wMaxPacketSize =	cpu_to_le16(STATUS_BYTECOUNT),
-	.bInterval =		8,
-};
-
-static struct usb_ss_ep_comp_descriptor uvc_ss_control_comp __initdata = {
-	.bLength =		sizeof uvc_ss_control_comp,
-	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
-
-	/* the following 3 values can be tweaked if necessary */
-	/* .bMaxBurst =		0, */
-	/* .bmAttributes =	0, */
-	.wBytesPerInterval =	cpu_to_le16(STATUS_BYTECOUNT),
+	.bmAttributes		= USB_ENDPOINT_SYNC_ASYNC
+				| USB_ENDPOINT_XFER_ISOC,
+	/* The wMaxPacketSize and bInterval values will be initialized from
+	 * module parameters.
+	 */
+	.wMaxPacketSize		= 0,
+	.bInterval		= 0,
 };
 
 static struct usb_endpoint_descriptor uvc_ss_streaming_ep __initdata = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bLength		= USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType	= USB_DT_ENDPOINT,
 
-	.bEndpointAddress =	USB_DIR_IN,
-	.bmAttributes =		USB_ENDPOINT_XFER_ISOC,
-	.wMaxPacketSize =	cpu_to_le16(1024),
-	.bInterval =		4,
+	.bEndpointAddress	= USB_DIR_IN,
+	.bmAttributes		= USB_ENDPOINT_SYNC_ASYNC
+				| USB_ENDPOINT_XFER_ISOC,
+	/* The wMaxPacketSize and bInterval values will be initialized from
+	 * module parameters.
+	 */
+	.wMaxPacketSize		= 0,
+	.bInterval		= 0,
 };
 
-static struct usb_ss_ep_comp_descriptor uvc_ss_streaming_comp = {
-	.bLength =		sizeof uvc_ss_streaming_comp,
-	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
-
-	/* the following 3 values can be tweaked if necessary */
-	.bMaxBurst =		0,
-	.bmAttributes =	0,
-	.wBytesPerInterval =	cpu_to_le16(1024),
+static struct usb_ss_ep_comp_descriptor uvc_ss_streaming_comp __initdata = {
+	.bLength		= sizeof(uvc_ss_streaming_comp),
+	.bDescriptorType	= USB_DT_SS_ENDPOINT_COMP,
+	/* The following 3 values can be tweaked if necessary. */
+	.bMaxBurst		= 0,
+	.bmAttributes		= 0,
+	.wBytesPerInterval	= cpu_to_le16(1024),
 };
 
 static const struct usb_descriptor_header * const uvc_fs_streaming[] = {
@@ -273,6 +266,13 @@ uvc_function_setup(struct usb_function *f, const struct usb_ctrlrequest *ctrl)
 	return 0;
 }
 
+void uvc_function_setup_continue(struct uvc_device *uvc)
+{
+	struct usb_composite_dev *cdev = uvc->func.config->cdev;
+
+	usb_composite_setup_continue(cdev);
+}
+
 static int
 uvc_function_get_alt(struct usb_function *f, unsigned interface)
 {
@@ -335,7 +335,7 @@ uvc_function_set_alt(struct usb_function *f, unsigned interface, unsigned alt)
 		v4l2_event_queue(uvc->vdev, &v4l2_event);
 
 		uvc->state = UVC_STATE_CONNECTED;
-		break;
+		return 0;
 
 	case 1:
 		if (uvc->state != UVC_STATE_CONNECTED)
@@ -352,15 +352,11 @@ uvc_function_set_alt(struct usb_function *f, unsigned interface, unsigned alt)
 		memset(&v4l2_event, 0, sizeof(v4l2_event));
 		v4l2_event.type = UVC_EVENT_STREAMON;
 		v4l2_event_queue(uvc->vdev, &v4l2_event);
-
-		uvc->state = UVC_STATE_STREAMING;
-		break;
+		return USB_GADGET_DELAYED_STATUS;
 
 	default:
 		return -EINVAL;
 	}
-
-	return 0;
 }
 
 static void
@@ -454,7 +450,6 @@ uvc_copy_descriptors(struct uvc_device *uvc, enum usb_device_speed speed)
 	const struct uvc_descriptor_header * const *uvc_streaming_cls;
 	const struct usb_descriptor_header * const *uvc_streaming_std;
 	const struct usb_descriptor_header * const *src;
-	static struct usb_endpoint_descriptor *uvc_control_ep;
 	struct usb_descriptor_header **dst;
 	struct usb_descriptor_header **hdr;
 	unsigned int control_size;
@@ -468,14 +463,12 @@ uvc_copy_descriptors(struct uvc_device *uvc, enum usb_device_speed speed)
 		uvc_control_desc = uvc->desc.ss_control;
 		uvc_streaming_cls = uvc->desc.ss_streaming;
 		uvc_streaming_std = uvc_ss_streaming;
-		uvc_control_ep = &uvc_ss_control_ep;
 		break;
 
 	case USB_SPEED_HIGH:
 		uvc_control_desc = uvc->desc.fs_control;
 		uvc_streaming_cls = uvc->desc.hs_streaming;
 		uvc_streaming_std = uvc_hs_streaming;
-		uvc_control_ep = &uvc_fs_control_ep;
 		break;
 
 	case USB_SPEED_FULL:
@@ -483,7 +476,6 @@ uvc_copy_descriptors(struct uvc_device *uvc, enum usb_device_speed speed)
 		uvc_control_desc = uvc->desc.fs_control;
 		uvc_streaming_cls = uvc->desc.fs_streaming;
 		uvc_streaming_std = uvc_fs_streaming;
-		uvc_control_ep = &uvc_fs_control_ep;
 		break;
 	}
 
@@ -494,6 +486,7 @@ uvc_copy_descriptors(struct uvc_device *uvc, enum usb_device_speed speed)
 	 * Class-specific UVC control descriptors
 	 * uvc_control_ep
 	 * uvc_control_cs_ep
+	 * uvc_ss_control_comp (for SS only)
 	 * uvc_streaming_intf_alt0
 	 * Class-specific UVC streaming descriptors
 	 * uvc_{fs|hs}_streaming
@@ -503,7 +496,7 @@ uvc_copy_descriptors(struct uvc_device *uvc, enum usb_device_speed speed)
 	control_size = 0;
 	streaming_size = 0;
 	bytes = uvc_iad.bLength + uvc_control_intf.bLength
-	      + uvc_control_ep->bLength + uvc_control_cs_ep.bLength
+	      + uvc_control_ep.bLength + uvc_control_cs_ep.bLength
 	      + uvc_streaming_intf_alt0.bLength;
 
 	if (speed == USB_SPEED_SUPER) {
@@ -514,13 +507,13 @@ uvc_copy_descriptors(struct uvc_device *uvc, enum usb_device_speed speed)
 	}
 
 	for (src = (const struct usb_descriptor_header **)uvc_control_desc;
-			*src; ++src) {
+	     *src; ++src) {
 		control_size += (*src)->bLength;
 		bytes += (*src)->bLength;
 		n_desc++;
 	}
 	for (src = (const struct usb_descriptor_header **)uvc_streaming_cls;
-			*src; ++src) {
+	     *src; ++src) {
 		streaming_size += (*src)->bLength;
 		bytes += (*src)->bLength;
 		n_desc++;
@@ -549,7 +542,7 @@ uvc_copy_descriptors(struct uvc_device *uvc, enum usb_device_speed speed)
 	uvc_control_header->bInCollection = 1;
 	uvc_control_header->baInterfaceNr[0] = uvc->streaming_intf;
 
-	UVC_COPY_DESCRIPTOR(mem, dst, uvc_control_ep);
+	UVC_COPY_DESCRIPTOR(mem, dst, &uvc_control_ep);
 	if (speed == USB_SPEED_SUPER)
 		UVC_COPY_DESCRIPTOR(mem, dst, &uvc_ss_control_comp);
 
@@ -560,8 +553,7 @@ uvc_copy_descriptors(struct uvc_device *uvc, enum usb_device_speed speed)
 	UVC_COPY_DESCRIPTORS(mem, dst,
 		(const struct usb_descriptor_header**)uvc_streaming_cls);
 	uvc_streaming_header->wTotalLength = cpu_to_le16(streaming_size);
-	uvc_streaming_header->bEndpointAddress =
-		uvc_fs_streaming_ep.bEndpointAddress;
+	uvc_streaming_header->bEndpointAddress = uvc->video.ep->address;
 
 	UVC_COPY_DESCRIPTORS(mem, dst, uvc_streaming_std);
 
@@ -581,7 +573,7 @@ uvc_function_unbind(struct usb_configuration *c, struct usb_function *f)
 	uvc->control_ep->driver_data = NULL;
 	uvc->video.ep->driver_data = NULL;
 
-	uvc_en_us_strings[UVC_STRING_ASSOCIATION_IDX].id = 0;
+	uvc_en_us_strings[UVC_STRING_CONTROL_IDX].id = 0;
 	usb_ep_free_request(cdev->gadget->ep0, uvc->control_req);
 	kfree(uvc->control_buf);
 
@@ -595,31 +587,52 @@ uvc_function_bind(struct usb_configuration *c, struct usb_function *f)
 {
 	struct usb_composite_dev *cdev = c->cdev;
 	struct uvc_device *uvc = to_uvc(f);
+	unsigned int max_packet_mult;
+	unsigned int max_packet_size;
 	struct usb_ep *ep;
 	int ret = -EINVAL;
 
 	INFO(cdev, "uvc_function_bind\n");
 
-	/* sanity check the streaming endpoint module parameters */
-	if (streaming_interval < 1)
-		streaming_interval = 1;
-	if (streaming_interval > 16)
-		streaming_interval = 16;
-	if (streaming_mult > 2)
-		streaming_mult = 2;
-	if (streaming_maxburst > 15)
-		streaming_maxburst = 15;
-
-	/*
-	 * fill in the FS video streaming specific descriptors from the
-	 * module parameters
+	/* Sanity check the streaming endpoint module parameters.
 	 */
-	uvc_fs_streaming_ep.wMaxPacketSize = streaming_maxpacket > 1023 ?
-						1023 : streaming_maxpacket;
+	streaming_interval = clamp(streaming_interval, 1U, 16U);
+	streaming_maxpacket = clamp(streaming_maxpacket, 1U, 3072U);
+	streaming_maxburst = min(streaming_maxburst, 15U);
+
+	/* Fill in the FS/HS/SS Video Streaming specific descriptors from the
+	 * module parameters.
+	 *
+	 * NOTE: We assume that the user knows what they are doing and won't
+	 * give parameters that their UDC doesn't support.
+	 */
+	if (streaming_maxpacket <= 1024) {
+		max_packet_mult = 1;
+		max_packet_size = streaming_maxpacket;
+	} else if (streaming_maxpacket <= 2048) {
+		max_packet_mult = 2;
+		max_packet_size = streaming_maxpacket / 2;
+	} else {
+		max_packet_mult = 3;
+		max_packet_size = streaming_maxpacket / 3;
+	}
+
+	uvc_fs_streaming_ep.wMaxPacketSize = min(streaming_maxpacket, 1023U);
 	uvc_fs_streaming_ep.bInterval = streaming_interval;
 
+	uvc_hs_streaming_ep.wMaxPacketSize = max_packet_size;
+	uvc_hs_streaming_ep.wMaxPacketSize |= ((max_packet_mult - 1) << 11);
+	uvc_hs_streaming_ep.bInterval = streaming_interval;
+
+	uvc_ss_streaming_ep.wMaxPacketSize = max_packet_size;
+	uvc_ss_streaming_ep.bInterval = streaming_interval;
+	uvc_ss_streaming_comp.bmAttributes = max_packet_mult - 1;
+	uvc_ss_streaming_comp.bMaxBurst = streaming_maxburst;
+	uvc_ss_streaming_comp.wBytesPerInterval =
+		max_packet_size * max_packet_mult * streaming_maxburst;
+
 	/* Allocate endpoints. */
-	ep = usb_ep_autoconfig(cdev->gadget, &uvc_fs_control_ep);
+	ep = usb_ep_autoconfig(cdev->gadget, &uvc_control_ep);
 	if (!ep) {
 		INFO(cdev, "Unable to allocate control EP\n");
 		goto error;
@@ -627,7 +640,14 @@ uvc_function_bind(struct usb_configuration *c, struct usb_function *f)
 	uvc->control_ep = ep;
 	ep->driver_data = uvc;
 
-	ep = usb_ep_autoconfig(cdev->gadget, &uvc_fs_streaming_ep);
+	if (gadget_is_superspeed(c->cdev->gadget))
+		ep = usb_ep_autoconfig_ss(cdev->gadget, &uvc_ss_streaming_ep,
+					  &uvc_ss_streaming_comp);
+	else if (gadget_is_dualspeed(cdev->gadget))
+		ep = usb_ep_autoconfig(cdev->gadget, &uvc_hs_streaming_ep);
+	else
+		ep = usb_ep_autoconfig(cdev->gadget, &uvc_fs_streaming_ep);
+
 	if (!ep) {
 		INFO(cdev, "Unable to allocate streaming EP\n");
 		goto error;
@@ -635,6 +655,10 @@ uvc_function_bind(struct usb_configuration *c, struct usb_function *f)
 	uvc->video.ep = ep;
 	ep->driver_data = uvc;
 
+	uvc_fs_streaming_ep.bEndpointAddress = uvc->video.ep->address;
+	uvc_hs_streaming_ep.bEndpointAddress = uvc->video.ep->address;
+	uvc_ss_streaming_ep.bEndpointAddress = uvc->video.ep->address;
+
 	/* Allocate interface IDs. */
 	if ((ret = usb_interface_id(c, f)) < 0)
 		goto error;
@@ -648,37 +672,6 @@ uvc_function_bind(struct usb_configuration *c, struct usb_function *f)
 	uvc_streaming_intf_alt1.bInterfaceNumber = ret;
 	uvc->streaming_intf = ret;
 
-	/* sanity check the streaming endpoint module parameters */
-	if (streaming_maxpacket > 1024)
-		streaming_maxpacket = 1024;
-	/*
-	 * Fill in the HS descriptors from the module parameters for the Video
-	 * Streaming endpoint.
-	 * NOTE: We assume that the user knows what they are doing and won't
-	 * give parameters that their UDC doesn't support.
-	 */
-	uvc_hs_streaming_ep.wMaxPacketSize = streaming_maxpacket;
-	uvc_hs_streaming_ep.wMaxPacketSize |= streaming_mult << 11;
-	uvc_hs_streaming_ep.bInterval = streaming_interval;
-	uvc_hs_streaming_ep.bEndpointAddress =
-		uvc_fs_streaming_ep.bEndpointAddress;
-
-	/*
-	 * Fill in the SS descriptors from the module parameters for the Video
-	 * Streaming endpoint.
-	 * NOTE: We assume that the user knows what they are doing and won't
-	 * give parameters that their UDC doesn't support.
-	 */
-	uvc_ss_streaming_ep.wMaxPacketSize = streaming_maxpacket;
-	uvc_ss_streaming_ep.bInterval = streaming_interval;
-	uvc_ss_streaming_comp.bmAttributes = streaming_mult;
-	uvc_ss_streaming_comp.bMaxBurst = streaming_maxburst;
-	uvc_ss_streaming_comp.wBytesPerInterval =
-		streaming_maxpacket * (streaming_mult + 1) *
-		(streaming_maxburst + 1);
-	uvc_ss_streaming_ep.bEndpointAddress =
-		uvc_fs_streaming_ep.bEndpointAddress;
-
 	/* Copy descriptors */
 	f->fs_descriptors = uvc_copy_descriptors(uvc, USB_SPEED_FULL);
 	if (gadget_is_dualspeed(cdev->gadget))
@@ -775,23 +768,23 @@ uvc_bind_config(struct usb_configuration *c,
 
 	/* Validate the descriptors. */
 	if (fs_control == NULL || fs_control[0] == NULL ||
-		fs_control[0]->bDescriptorSubType != UVC_VC_HEADER)
+	    fs_control[0]->bDescriptorSubType != UVC_VC_HEADER)
 		goto error;
 
 	if (ss_control == NULL || ss_control[0] == NULL ||
-		ss_control[0]->bDescriptorSubType != UVC_VC_HEADER)
+	    ss_control[0]->bDescriptorSubType != UVC_VC_HEADER)
 		goto error;
 
 	if (fs_streaming == NULL || fs_streaming[0] == NULL ||
-		fs_streaming[0]->bDescriptorSubType != UVC_VS_INPUT_HEADER)
+	    fs_streaming[0]->bDescriptorSubType != UVC_VS_INPUT_HEADER)
 		goto error;
 
 	if (hs_streaming == NULL || hs_streaming[0] == NULL ||
-		hs_streaming[0]->bDescriptorSubType != UVC_VS_INPUT_HEADER)
+	    hs_streaming[0]->bDescriptorSubType != UVC_VS_INPUT_HEADER)
 		goto error;
 
 	if (ss_streaming == NULL || ss_streaming[0] == NULL ||
-		ss_streaming[0]->bDescriptorSubType != UVC_VS_INPUT_HEADER)
+	    ss_streaming[0]->bDescriptorSubType != UVC_VS_INPUT_HEADER)
 		goto error;
 
 	uvc->desc.fs_control = fs_control;
@@ -800,13 +793,16 @@ uvc_bind_config(struct usb_configuration *c,
 	uvc->desc.hs_streaming = hs_streaming;
 	uvc->desc.ss_streaming = ss_streaming;
 
-	/* Allocate string descriptor numbers. */
-	if (uvc_en_us_strings[UVC_STRING_ASSOCIATION_IDX].id == 0) {
+	/* String descriptors are global, we only need to allocate string IDs
+	 * for the first UVC function. UVC functions beyond the first (if any)
+	 * will reuse the same IDs.
+	 */
+	if (uvc_en_us_strings[UVC_STRING_CONTROL_IDX].id == 0) {
 		ret = usb_string_ids_tab(c->cdev, uvc_en_us_strings);
 		if (ret)
 			goto error;
 		uvc_iad.iFunction =
-			uvc_en_us_strings[UVC_STRING_ASSOCIATION_IDX].id;
+			uvc_en_us_strings[UVC_STRING_CONTROL_IDX].id;
 		uvc_control_intf.iInterface =
 			uvc_en_us_strings[UVC_STRING_CONTROL_IDX].id;
 		ret = uvc_en_us_strings[UVC_STRING_STREAMING_IDX].id;
diff --git a/drivers/usb/gadget/f_uvc.h b/drivers/usb/gadget/f_uvc.h
index c3d258d3018..ec52752f732 100644
--- a/drivers/usb/gadget/f_uvc.h
+++ b/drivers/usb/gadget/f_uvc.h
@@ -16,12 +16,12 @@
 #include <linux/usb/composite.h>
 #include <linux/usb/video.h>
 
-extern int uvc_bind_config(struct usb_configuration *c,
-		   const struct uvc_descriptor_header * const *fs_control,
-		   const struct uvc_descriptor_header * const *hs_control,
-		   const struct uvc_descriptor_header * const *fs_streaming,
-		   const struct uvc_descriptor_header * const *hs_streaming,
-		   const struct uvc_descriptor_header * const *ss_streaming);
+int uvc_bind_config(struct usb_configuration *c,
+		    const struct uvc_descriptor_header * const *fs_control,
+		    const struct uvc_descriptor_header * const *hs_control,
+		    const struct uvc_descriptor_header * const *fs_streaming,
+		    const struct uvc_descriptor_header * const *hs_streaming,
+		    const struct uvc_descriptor_header * const *ss_streaming);
 
 #endif /* _F_UVC_H_ */
 
diff --git a/drivers/usb/gadget/fsl_qe_udc.c b/drivers/usb/gadget/fsl_qe_udc.c
index 034477ce77c..9a7ee3347e4 100644
--- a/drivers/usb/gadget/fsl_qe_udc.c
+++ b/drivers/usb/gadget/fsl_qe_udc.c
@@ -2296,7 +2296,6 @@ static int fsl_qe_start(struct usb_gadget *gadget,
 	driver->driver.bus = NULL;
 	/* hook up the driver */
 	udc->driver = driver;
-	udc->gadget.dev.driver = &driver->driver;
 	udc->gadget.speed = driver->max_speed;
 
 	/* Enable IRQ reg and Set usbcmd reg EN bit */
@@ -2338,7 +2337,6 @@ static int fsl_qe_stop(struct usb_gadget *gadget,
 		nuke(loop_ep, -ESHUTDOWN);
 	spin_unlock_irqrestore(&udc->lock, flags);
 
-	udc->gadget.dev.driver = NULL;
 	udc->driver = NULL;
 
 	dev_info(udc->dev, "unregistered gadget driver '%s'\r\n",
@@ -2523,12 +2521,6 @@ static int qe_udc_probe(struct platform_device *ofdev)
 
 	/* name: Identifies the controller hardware type. */
 	udc->gadget.name = driver_name;
-
-	device_initialize(&udc->gadget.dev);
-
-	dev_set_name(&udc->gadget.dev, "gadget");
-
-	udc->gadget.dev.release = qe_udc_release;
 	udc->gadget.dev.parent = &ofdev->dev;
 
 	/* initialize qe_ep struct */
@@ -2592,22 +2584,17 @@ static int qe_udc_probe(struct platform_device *ofdev)
 		goto err5;
 	}
 
-	ret = device_add(&udc->gadget.dev);
+	ret = usb_add_gadget_udc_release(&ofdev->dev, &udc->gadget,
+			qe_udc_release);
 	if (ret)
 		goto err6;
 
-	ret = usb_add_gadget_udc(&ofdev->dev, &udc->gadget);
-	if (ret)
-		goto err7;
-
 	dev_set_drvdata(&ofdev->dev, udc);
 	dev_info(udc->dev,
 			"%s USB controller initialized as device\n",
 			(udc->soc_type == PORT_QE) ? "QE" : "CPM");
 	return 0;
 
-err7:
-	device_unregister(&udc->gadget.dev);
 err6:
 	free_irq(udc->usb_irq, udc);
 err5:
@@ -2702,7 +2689,6 @@ static int qe_udc_remove(struct platform_device *ofdev)
 
 	iounmap(udc->usb_regs);
 
-	device_unregister(&udc->gadget.dev);
 	/* wait for release() of gadget.dev to free udc */
 	wait_for_completion(&done);
 
diff --git a/drivers/usb/gadget/fsl_udc_core.c b/drivers/usb/gadget/fsl_udc_core.c
index 04d5fef1440..7c2a101d19a 100644
--- a/drivers/usb/gadget/fsl_udc_core.c
+++ b/drivers/usb/gadget/fsl_udc_core.c
@@ -185,20 +185,7 @@ static void done(struct fsl_ep *ep, struct fsl_req *req, int status)
 		dma_pool_free(udc->td_pool, curr_td, curr_td->td_dma);
 	}
 
-	if (req->mapped) {
-		dma_unmap_single(ep->udc->gadget.dev.parent,
-			req->req.dma, req->req.length,
-			ep_is_in(ep)
-				? DMA_TO_DEVICE
-				: DMA_FROM_DEVICE);
-		req->req.dma = DMA_ADDR_INVALID;
-		req->mapped = 0;
-	} else
-		dma_sync_single_for_cpu(ep->udc->gadget.dev.parent,
-			req->req.dma, req->req.length,
-			ep_is_in(ep)
-				? DMA_TO_DEVICE
-				: DMA_FROM_DEVICE);
+	usb_gadget_unmap_request(&ep->udc->gadget, &req->req, ep_is_in(ep));
 
 	if (status && (status != -ESHUTDOWN))
 		VDBG("complete %s req %p stat %d len %u/%u",
@@ -888,6 +875,7 @@ fsl_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 	struct fsl_req *req = container_of(_req, struct fsl_req, req);
 	struct fsl_udc *udc;
 	unsigned long flags;
+	int ret;
 
 	/* catch various bogus parameters */
 	if (!_req || !req->req.complete || !req->req.buf
@@ -910,22 +898,9 @@ fsl_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 
 	req->ep = ep;
 
-	/* map virtual address to hardware */
-	if (req->req.dma == DMA_ADDR_INVALID) {
-		req->req.dma = dma_map_single(ep->udc->gadget.dev.parent,
-					req->req.buf,
-					req->req.length, ep_is_in(ep)
-						? DMA_TO_DEVICE
-						: DMA_FROM_DEVICE);
-		req->mapped = 1;
-	} else {
-		dma_sync_single_for_device(ep->udc->gadget.dev.parent,
-					req->req.dma, req->req.length,
-					ep_is_in(ep)
-						? DMA_TO_DEVICE
-						: DMA_FROM_DEVICE);
-		req->mapped = 0;
-	}
+	ret = usb_gadget_map_request(&ep->udc->gadget, &req->req, ep_is_in(ep));
+	if (ret)
+		return ret;
 
 	req->req.status = -EINPROGRESS;
 	req->req.actual = 0;
@@ -1290,6 +1265,7 @@ static int ep0_prime_status(struct fsl_udc *udc, int direction)
 {
 	struct fsl_req *req = udc->status_req;
 	struct fsl_ep *ep;
+	int ret;
 
 	if (direction == EP_DIR_IN)
 		udc->ep0_dir = USB_DIR_IN;
@@ -1307,10 +1283,9 @@ static int ep0_prime_status(struct fsl_udc *udc, int direction)
 	req->req.complete = NULL;
 	req->dtd_count = 0;
 
-	req->req.dma = dma_map_single(ep->udc->gadget.dev.parent,
-			req->req.buf, req->req.length,
-			ep_is_in(ep) ? DMA_TO_DEVICE : DMA_FROM_DEVICE);
-	req->mapped = 1;
+	ret = usb_gadget_map_request(&ep->udc->gadget, &req->req, ep_is_in(ep));
+	if (ret)
+		return ret;
 
 	if (fsl_req_to_dtd(req, GFP_ATOMIC) == 0)
 		fsl_queue_td(ep, req);
@@ -1353,6 +1328,7 @@ static void ch9getstatus(struct fsl_udc *udc, u8 request_type, u16 value,
 	u16 tmp = 0;		/* Status, cpu endian */
 	struct fsl_req *req;
 	struct fsl_ep *ep;
+	int ret;
 
 	ep = &udc->eps[0];
 
@@ -1390,10 +1366,9 @@ static void ch9getstatus(struct fsl_udc *udc, u8 request_type, u16 value,
 	req->req.complete = NULL;
 	req->dtd_count = 0;
 
-	req->req.dma = dma_map_single(ep->udc->gadget.dev.parent,
-				req->req.buf, req->req.length,
-				ep_is_in(ep) ? DMA_TO_DEVICE : DMA_FROM_DEVICE);
-	req->mapped = 1;
+	ret = usb_gadget_map_request(&ep->udc->gadget, &req->req, ep_is_in(ep));
+	if (ret)
+		goto stall;
 
 	/* prime the data phase */
 	if ((fsl_req_to_dtd(req, GFP_ATOMIC) == 0))
@@ -1964,7 +1939,6 @@ static int fsl_udc_start(struct usb_gadget *g,
 	driver->driver.bus = NULL;
 	/* hook up the driver */
 	udc_controller->driver = driver;
-	udc_controller->gadget.dev.driver = &driver->driver;
 	spin_unlock_irqrestore(&udc_controller->lock, flags);
 
 	if (!IS_ERR_OR_NULL(udc_controller->transceiver)) {
@@ -1980,7 +1954,6 @@ static int fsl_udc_start(struct usb_gadget *g,
 			if (retval < 0) {
 				ERR("can't bind to transceiver\n");
 				driver->unbind(&udc_controller->gadget);
-				udc_controller->gadget.dev.driver = 0;
 				udc_controller->driver = 0;
 				return retval;
 			}
@@ -2023,7 +1996,6 @@ static int fsl_udc_stop(struct usb_gadget *g,
 		nuke(loop_ep, -ESHUTDOWN);
 	spin_unlock_irqrestore(&udc_controller->lock, flags);
 
-	udc_controller->gadget.dev.driver = NULL;
 	udc_controller->driver = NULL;
 
 	return 0;
@@ -2521,12 +2493,7 @@ static int __init fsl_udc_probe(struct platform_device *pdev)
 
 	/* Setup gadget.dev and register with kernel */
 	dev_set_name(&udc_controller->gadget.dev, "gadget");
-	udc_controller->gadget.dev.release = fsl_udc_release;
-	udc_controller->gadget.dev.parent = &pdev->dev;
 	udc_controller->gadget.dev.of_node = pdev->dev.of_node;
-	ret = device_register(&udc_controller->gadget.dev);
-	if (ret < 0)
-		goto err_free_irq;
 
 	if (!IS_ERR_OR_NULL(udc_controller->transceiver))
 		udc_controller->gadget.is_otg = 1;
@@ -2559,10 +2526,11 @@ static int __init fsl_udc_probe(struct platform_device *pdev)
 			DTD_ALIGNMENT, UDC_DMA_BOUNDARY);
 	if (udc_controller->td_pool == NULL) {
 		ret = -ENOMEM;
-		goto err_unregister;
+		goto err_free_irq;
 	}
 
-	ret = usb_add_gadget_udc(&pdev->dev, &udc_controller->gadget);
+	ret = usb_add_gadget_udc_release(&pdev->dev, &udc_controller->gadget,
+			fsl_udc_release);
 	if (ret)
 		goto err_del_udc;
 
@@ -2571,8 +2539,6 @@ static int __init fsl_udc_probe(struct platform_device *pdev)
 
 err_del_udc:
 	dma_pool_destroy(udc_controller->td_pool);
-err_unregister:
-	device_unregister(&udc_controller->gadget.dev);
 err_free_irq:
 	free_irq(udc_controller->irq, udc_controller);
 err_iounmap:
@@ -2622,7 +2588,6 @@ static int __exit fsl_udc_remove(struct platform_device *pdev)
 	if (pdata->operating_mode == FSL_USB2_DR_DEVICE)
 		release_mem_region(res->start, resource_size(res));
 
-	device_unregister(&udc_controller->gadget.dev);
 	/* free udc --wait for the release() finished */
 	wait_for_completion(&done);
 
@@ -2747,21 +2712,7 @@ static struct platform_driver udc_driver = {
 	},
 };
 
-static int __init udc_init(void)
-{
-	printk(KERN_INFO "%s (%s)\n", driver_desc, DRIVER_VERSION);
-	return platform_driver_probe(&udc_driver, fsl_udc_probe);
-}
-
-module_init(udc_init);
-
-static void __exit udc_exit(void)
-{
-	platform_driver_unregister(&udc_driver);
-	printk(KERN_WARNING "%s unregistered\n", driver_desc);
-}
-
-module_exit(udc_exit);
+module_platform_driver_probe(udc_driver, fsl_udc_probe);
 
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_AUTHOR(DRIVER_AUTHOR);
diff --git a/drivers/usb/gadget/fusb300_udc.c b/drivers/usb/gadget/fusb300_udc.c
index 066cb89376d..cec8871b77f 100644
--- a/drivers/usb/gadget/fusb300_udc.c
+++ b/drivers/usb/gadget/fusb300_udc.c
@@ -394,7 +394,7 @@ static void fusb300_clear_epnstall(struct fusb300 *fusb300, u8 ep)
 
 	if (reg & FUSB300_EPSET0_STL) {
 		printk(KERN_DEBUG "EP%d stall... Clear!!\n", ep);
-		reg &= ~FUSB300_EPSET0_STL;
+		reg |= FUSB300_EPSET0_STL_CLR;
 		iowrite32(reg, fusb300->reg + FUSB300_OFFSET_EPSET0(ep));
 	}
 }
@@ -930,33 +930,33 @@ static void fusb300_wait_idma_finished(struct fusb300_ep *ep)
 
 	fusb300_clear_int(ep->fusb300, FUSB300_OFFSET_IGR0,
 		FUSB300_IGR0_EPn_PRD_INT(ep->epnum));
+	return;
+
 IDMA_RESET:
-	fusb300_clear_int(ep->fusb300, FUSB300_OFFSET_IGER0,
-		FUSB300_IGER0_EEPn_PRD_INT(ep->epnum));
+	reg = ioread32(ep->fusb300->reg + FUSB300_OFFSET_IGER0);
+	reg &= ~FUSB300_IGER0_EEPn_PRD_INT(ep->epnum);
+	iowrite32(reg, ep->fusb300->reg + FUSB300_OFFSET_IGER0);
 }
 
-static void  fusb300_set_idma(struct fusb300_ep *ep,
+static void fusb300_set_idma(struct fusb300_ep *ep,
 			struct fusb300_request *req)
 {
-	dma_addr_t d;
-
-	d = dma_map_single(NULL, req->req.buf, req->req.length, DMA_TO_DEVICE);
+	int ret;
 
-	if (dma_mapping_error(NULL, d)) {
-		printk(KERN_DEBUG "dma_mapping_error\n");
+	ret = usb_gadget_map_request(&ep->fusb300->gadget,
+			&req->req, DMA_TO_DEVICE);
+	if (ret)
 		return;
-	}
-
-	dma_sync_single_for_device(NULL, d, req->req.length, DMA_TO_DEVICE);
 
 	fusb300_enable_bit(ep->fusb300, FUSB300_OFFSET_IGER0,
 		FUSB300_IGER0_EEPn_PRD_INT(ep->epnum));
 
-	fusb300_fill_idma_prdtbl(ep, d, req->req.length);
+	fusb300_fill_idma_prdtbl(ep, req->req.dma, req->req.length);
 	/* check idma is done */
 	fusb300_wait_idma_finished(ep);
 
-	dma_unmap_single(NULL, d, req->req.length, DMA_TO_DEVICE);
+	usb_gadget_unmap_request(&ep->fusb300->gadget,
+			&req->req, DMA_TO_DEVICE);
 }
 
 static void in_ep_fifo_handler(struct fusb300_ep *ep)
@@ -1316,7 +1316,6 @@ static int fusb300_udc_start(struct usb_gadget *g,
 	/* hook up the driver */
 	driver->driver.bus = NULL;
 	fusb300->driver = driver;
-	fusb300->gadget.dev.driver = &driver->driver;
 
 	return 0;
 }
@@ -1327,7 +1326,6 @@ static int fusb300_udc_stop(struct usb_gadget *g,
 	struct fusb300 *fusb300 = to_fusb300(g);
 
 	driver->unbind(&fusb300->gadget);
-	fusb300->gadget.dev.driver = NULL;
 
 	init_controller(fusb300);
 	fusb300->driver = NULL;
@@ -1422,14 +1420,7 @@ static int __init fusb300_probe(struct platform_device *pdev)
 
 	fusb300->gadget.ops = &fusb300_gadget_ops;
 
-	device_initialize(&fusb300->gadget.dev);
-
-	dev_set_name(&fusb300->gadget.dev, "gadget");
-
 	fusb300->gadget.max_speed = USB_SPEED_HIGH;
-	fusb300->gadget.dev.parent = &pdev->dev;
-	fusb300->gadget.dev.dma_mask = pdev->dev.dma_mask;
-	fusb300->gadget.dev.release = pdev->dev.release;
 	fusb300->gadget.name = udc_name;
 	fusb300->reg = reg;
 
@@ -1478,19 +1469,10 @@ static int __init fusb300_probe(struct platform_device *pdev)
 	if (ret)
 		goto err_add_udc;
 
-	ret = device_add(&fusb300->gadget.dev);
-	if (ret) {
-		pr_err("device_add error (%d)\n", ret);
-		goto err_add_device;
-	}
-
 	dev_info(&pdev->dev, "version %s\n", DRIVER_VERSION);
 
 	return 0;
 
-err_add_device:
-	usb_del_gadget_udc(&fusb300->gadget);
-
 err_add_udc:
 	fusb300_free_request(&fusb300->ep[0]->ep, fusb300->ep0_req);
 
diff --git a/drivers/usb/gadget/fusb300_udc.h b/drivers/usb/gadget/fusb300_udc.h
index 6ba444ae8dd..ae811d8d38b 100644
--- a/drivers/usb/gadget/fusb300_udc.h
+++ b/drivers/usb/gadget/fusb300_udc.h
@@ -111,8 +111,8 @@
 /*
  * * EPn Setting 0 (EPn_SET0, offset = 020H+(n-1)*30H, n=1~15 )
  * */
+#define FUSB300_EPSET0_STL_CLR		(1 << 3)
 #define FUSB300_EPSET0_CLRSEQNUM	(1 << 2)
-#define FUSB300_EPSET0_EPn_TX0BYTE	(1 << 1)
 #define FUSB300_EPSET0_STL		(1 << 0)
 
 /*
diff --git a/drivers/usb/gadget/g_ffs.c b/drivers/usb/gadget/g_ffs.c
index 3b343b23e4b..787a78e92aa 100644
--- a/drivers/usb/gadget/g_ffs.c
+++ b/drivers/usb/gadget/g_ffs.c
@@ -13,7 +13,6 @@
 #define pr_fmt(fmt) "g_ffs: " fmt
 
 #include <linux/module.h>
-
 /*
  * kbuild is not very cooperative with respect to linking separately
  * compiled library objects into one module.  So for now we won't use
@@ -38,13 +37,16 @@
 #  include "u_ether.c"
 
 static u8 gfs_hostaddr[ETH_ALEN];
+static struct eth_dev *the_dev;
 #  ifdef CONFIG_USB_FUNCTIONFS_ETH
-static int eth_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN]);
+static int eth_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN],
+		struct eth_dev *dev);
 #  endif
 #else
-#  define gether_cleanup() do { } while (0)
-#  define gether_setup(gadget, hostaddr)   ((int)0)
+#  define the_dev	NULL
+#  define gether_cleanup(dev) do { } while (0)
 #  define gfs_hostaddr NULL
+struct eth_dev;
 #endif
 
 #include "f_fs.c"
@@ -137,7 +139,8 @@ static struct usb_gadget_strings *gfs_dev_strings[] = {
 
 struct gfs_configuration {
 	struct usb_configuration c;
-	int (*eth)(struct usb_configuration *c, u8 *ethaddr);
+	int (*eth)(struct usb_configuration *c, u8 *ethaddr,
+			struct eth_dev *dev);
 } gfs_configurations[] = {
 #ifdef CONFIG_USB_FUNCTIONFS_RNDIS
 	{
@@ -346,10 +349,13 @@ static int gfs_bind(struct usb_composite_dev *cdev)
 
 	if (missing_funcs)
 		return -ENODEV;
-
-	ret = gether_setup(cdev->gadget, gfs_hostaddr);
-	if (unlikely(ret < 0))
+#if defined CONFIG_USB_FUNCTIONFS_ETH || defined CONFIG_USB_FUNCTIONFS_RNDIS
+	the_dev = gether_setup(cdev->gadget, gfs_hostaddr);
+#endif
+	if (IS_ERR(the_dev)) {
+		ret = PTR_ERR(the_dev);
 		goto error_quick;
+	}
 	gfs_ether_setup = true;
 
 	ret = usb_string_ids_tab(cdev, gfs_strings);
@@ -386,7 +392,7 @@ error_unbind:
 	for (i = 0; i < func_num; i++)
 		functionfs_unbind(ffs_tab[i].ffs_data);
 error:
-	gether_cleanup();
+	gether_cleanup(the_dev);
 error_quick:
 	gfs_ether_setup = false;
 	return ret;
@@ -410,7 +416,7 @@ static int gfs_unbind(struct usb_composite_dev *cdev)
 	 * do...?
 	 */
 	if (gfs_ether_setup)
-		gether_cleanup();
+		gether_cleanup(the_dev);
 	gfs_ether_setup = false;
 
 	for (i = func_num; i--; )
@@ -440,7 +446,7 @@ static int gfs_do_config(struct usb_configuration *c)
 	}
 
 	if (gc->eth) {
-		ret = gc->eth(c, gfs_hostaddr);
+		ret = gc->eth(c, gfs_hostaddr, the_dev);
 		if (unlikely(ret < 0))
 			return ret;
 	}
@@ -469,11 +475,12 @@ static int gfs_do_config(struct usb_configuration *c)
 
 #ifdef CONFIG_USB_FUNCTIONFS_ETH
 
-static int eth_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN])
+static int eth_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN],
+		struct eth_dev *dev)
 {
 	return can_support_ecm(c->cdev->gadget)
-		? ecm_bind_config(c, ethaddr)
-		: geth_bind_config(c, ethaddr);
+		? ecm_bind_config(c, ethaddr, dev)
+		: geth_bind_config(c, ethaddr, dev);
 }
 
 #endif
diff --git a/drivers/usb/gadget/goku_udc.c b/drivers/usb/gadget/goku_udc.c
index 85742d4c67d..991aba390d9 100644
--- a/drivers/usb/gadget/goku_udc.c
+++ b/drivers/usb/gadget/goku_udc.c
@@ -51,8 +51,6 @@
 #define	DRIVER_DESC		"TC86C001 USB Device Controller"
 #define	DRIVER_VERSION		"30-Oct 2003"
 
-#define	DMA_ADDR_INVALID	(~(dma_addr_t)0)
-
 static const char driver_name [] = "goku_udc";
 static const char driver_desc [] = DRIVER_DESC;
 
@@ -275,7 +273,6 @@ goku_alloc_request(struct usb_ep *_ep, gfp_t gfp_flags)
 	if (!req)
 		return NULL;
 
-	req->req.dma = DMA_ADDR_INVALID;
 	INIT_LIST_HEAD(&req->queue);
 	return &req->req;
 }
@@ -1354,7 +1351,6 @@ static int goku_udc_start(struct usb_gadget *g,
 	/* hook up the driver */
 	driver->driver.bus = NULL;
 	dev->driver = driver;
-	dev->gadget.dev.driver = &driver->driver;
 
 	/*
 	 * then enable host detection and ep0; and we're ready
@@ -1394,7 +1390,6 @@ static int goku_udc_stop(struct usb_gadget *g,
 	dev->driver = NULL;
 	stop_activity(dev, driver);
 	spin_unlock_irqrestore(&dev->lock, flags);
-	dev->gadget.dev.driver = NULL;
 
 	return 0;
 }
@@ -1716,8 +1711,6 @@ static void goku_remove(struct pci_dev *pdev)
 				pci_resource_len (pdev, 0));
 	if (dev->enabled)
 		pci_disable_device(pdev);
-	if (dev->registered)
-		device_unregister(&dev->gadget.dev);
 
 	pci_set_drvdata(pdev, NULL);
 	dev->regs = NULL;
@@ -1756,10 +1749,6 @@ static int goku_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	dev->gadget.max_speed = USB_SPEED_FULL;
 
 	/* the "gadget" abstracts/virtualizes the controller */
-	dev_set_name(&dev->gadget.dev, "gadget");
-	dev->gadget.dev.parent = &pdev->dev;
-	dev->gadget.dev.dma_mask = pdev->dev.dma_mask;
-	dev->gadget.dev.release = gadget_release;
 	dev->gadget.name = driver_name;
 
 	/* now all the pci goodies ... */
@@ -1810,13 +1799,8 @@ static int goku_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	create_proc_read_entry(proc_node_name, 0, NULL, udc_proc_read, dev);
 #endif
 
-	retval = device_register(&dev->gadget.dev);
-	if (retval) {
-		put_device(&dev->gadget.dev);
-		goto err;
-	}
-	dev->registered = 1;
-	retval = usb_add_gadget_udc(&pdev->dev, &dev->gadget);
+	retval = usb_add_gadget_udc_release(&pdev->dev, &dev->gadget,
+			gadget_release);
 	if (retval)
 		goto err;
 
diff --git a/drivers/usb/gadget/goku_udc.h b/drivers/usb/gadget/goku_udc.h
index b4470d2b1d8..86d2adafe14 100644
--- a/drivers/usb/gadget/goku_udc.h
+++ b/drivers/usb/gadget/goku_udc.h
@@ -250,8 +250,7 @@ struct goku_udc {
 					got_region:1,
 					req_config:1,
 					configured:1,
-					enabled:1,
-					registered:1;
+					enabled:1;
 
 	/* pci state used to access those endpoints */
 	struct pci_dev			*pdev;
diff --git a/drivers/usb/gadget/imx_udc.c b/drivers/usb/gadget/imx_udc.c
index 5bd930d779b..b5cebd6b0d7 100644
--- a/drivers/usb/gadget/imx_udc.c
+++ b/drivers/usb/gadget/imx_udc.c
@@ -1338,7 +1338,6 @@ static int imx_udc_start(struct usb_gadget *gadget,
 	imx_usb = container_of(gadget, struct imx_udc_struct, gadget);
 	/* first hook up the driver ... */
 	imx_usb->driver = driver;
-	imx_usb->gadget.dev.driver = &driver->driver;
 
 	D_INI(imx_usb->dev, "<%s> registered gadget driver '%s'\n",
 		__func__, driver->driver.name);
@@ -1358,7 +1357,6 @@ static int imx_udc_stop(struct usb_gadget *gadget,
 	imx_udc_disable(imx_usb);
 	del_timer(&imx_usb->timer);
 
-	imx_usb->gadget.dev.driver = NULL;
 	imx_usb->driver = NULL;
 
 	D_INI(imx_usb->dev, "<%s> unregistered gadget driver '%s'\n",
@@ -1461,15 +1459,6 @@ static int __init imx_udc_probe(struct platform_device *pdev)
 	imx_usb->clk = clk;
 	imx_usb->dev = &pdev->dev;
 
-	device_initialize(&imx_usb->gadget.dev);
-
-	imx_usb->gadget.dev.parent = &pdev->dev;
-	imx_usb->gadget.dev.dma_mask = pdev->dev.dma_mask;
-
-	ret = device_add(&imx_usb->gadget.dev);
-	if (retval)
-		goto fail4;
-
 	platform_set_drvdata(pdev, imx_usb);
 
 	usb_init_data(imx_usb);
@@ -1481,11 +1470,9 @@ static int __init imx_udc_probe(struct platform_device *pdev)
 
 	ret = usb_add_gadget_udc(&pdev->dev, &imx_usb->gadget);
 	if (ret)
-		goto fail5;
+		goto fail4;
 
 	return 0;
-fail5:
-	device_unregister(&imx_usb->gadget.dev);
 fail4:
 	for (i = 0; i < IMX_USB_NB_EP + 1; i++)
 		free_irq(imx_usb->usbd_int[i], imx_usb);
@@ -1509,7 +1496,6 @@ static int __exit imx_udc_remove(struct platform_device *pdev)
 	int i;
 
 	usb_del_gadget_udc(&imx_usb->gadget);
-	device_unregister(&imx_usb->gadget.dev);
 	imx_udc_disable(imx_usb);
 	del_timer(&imx_usb->timer);
 
diff --git a/drivers/usb/gadget/lpc32xx_udc.c b/drivers/usb/gadget/lpc32xx_udc.c
index aa04089d689..b943d8cdfbf 100644
--- a/drivers/usb/gadget/lpc32xx_udc.c
+++ b/drivers/usb/gadget/lpc32xx_udc.c
@@ -1469,23 +1469,7 @@ static void done(struct lpc32xx_ep *ep, struct lpc32xx_request *req, int status)
 		status = req->req.status;
 
 	if (ep->lep) {
-		enum dma_data_direction direction;
-
-		if (ep->is_in)
-			direction = DMA_TO_DEVICE;
-		else
-			direction = DMA_FROM_DEVICE;
-
-		if (req->mapped) {
-			dma_unmap_single(ep->udc->gadget.dev.parent,
-					req->req.dma, req->req.length,
-					direction);
-			req->req.dma = 0;
-			req->mapped = 0;
-		} else
-			dma_sync_single_for_cpu(ep->udc->gadget.dev.parent,
-						req->req.dma, req->req.length,
-						direction);
+		usb_gadget_unmap_request(&udc->gadget, &req->req, ep->is_in);
 
 		/* Free DDs */
 		udc_dd_free(udc, req->dd_desc_ptr);
@@ -1841,26 +1825,11 @@ static int lpc32xx_ep_queue(struct usb_ep *_ep,
 	}
 
 	if (ep->lep) {
-		enum dma_data_direction direction;
 		struct lpc32xx_usbd_dd_gad *dd;
 
-		/* Map DMA pointer */
-		if (ep->is_in)
-			direction = DMA_TO_DEVICE;
-		else
-			direction = DMA_FROM_DEVICE;
-
-		if (req->req.dma == 0) {
-			req->req.dma = dma_map_single(
-				ep->udc->gadget.dev.parent,
-				req->req.buf, req->req.length, direction);
-			req->mapped = 1;
-		} else {
-			dma_sync_single_for_device(
-				ep->udc->gadget.dev.parent, req->req.dma,
-				req->req.length, direction);
-			req->mapped = 0;
-		}
+		status = usb_gadget_map_request(&udc->gadget, _req, ep->is_in);
+		if (status)
+			return status;
 
 		/* For the request, build a list of DDs */
 		dd = udc_dd_alloc(udc);
@@ -2977,7 +2946,6 @@ static int lpc32xx_start(struct usb_gadget *gadget,
 	}
 
 	udc->driver = driver;
-	udc->gadget.dev.driver = &driver->driver;
 	udc->gadget.dev.of_node = udc->dev->of_node;
 	udc->enabled = 1;
 	udc->selfpowered = 1;
@@ -3026,7 +2994,6 @@ static int lpc32xx_stop(struct usb_gadget *gadget,
 	}
 
 	udc->enabled = 0;
-	udc->gadget.dev.driver = NULL;
 	udc->driver = NULL;
 
 	return 0;
@@ -3248,12 +3215,6 @@ static int __init lpc32xx_udc_probe(struct platform_device *pdev)
 	udc_disable(udc);
 	udc_reinit(udc);
 
-	retval = device_register(&udc->gadget.dev);
-	if (retval < 0) {
-		dev_err(udc->dev, "Device registration failure\n");
-		goto dev_register_fail;
-	}
-
 	/* Request IRQs - low and high priority USB device IRQs are routed to
 	 * the same handler, while the DMA interrupt is routed elsewhere */
 	retval = request_irq(udc->udp_irq[IRQ_USB_LP], lpc32xx_usb_lp_irq,
@@ -3320,8 +3281,6 @@ irq_dev_fail:
 irq_hp_fail:
 	free_irq(udc->udp_irq[IRQ_USB_LP], udc);
 irq_lp_fail:
-	device_unregister(&udc->gadget.dev);
-dev_register_fail:
 	dma_pool_destroy(udc->dd_cache);
 dma_alloc_fail:
 	dma_free_coherent(&pdev->dev, UDCA_BUFF_SIZE,
@@ -3376,8 +3335,6 @@ static int lpc32xx_udc_remove(struct platform_device *pdev)
 	free_irq(udc->udp_irq[IRQ_USB_HP], udc);
 	free_irq(udc->udp_irq[IRQ_USB_LP], udc);
 
-	device_unregister(&udc->gadget.dev);
-
 	clk_disable(udc->usb_otg_clk);
 	clk_put(udc->usb_otg_clk);
 	clk_disable(udc->usb_slv_clk);
diff --git a/drivers/usb/gadget/m66592-udc.c b/drivers/usb/gadget/m66592-udc.c
index c1b8c2dd808..866ef099924 100644
--- a/drivers/usb/gadget/m66592-udc.c
+++ b/drivers/usb/gadget/m66592-udc.c
@@ -1471,7 +1471,6 @@ static int m66592_udc_start(struct usb_gadget *g,
 	/* hook up the driver */
 	driver->driver.bus = NULL;
 	m66592->driver = driver;
-	m66592->gadget.dev.driver = &driver->driver;
 
 	m66592_bset(m66592, M66592_VBSE | M66592_URST, M66592_INTENB0);
 	if (m66592_read(m66592, M66592_INTSTS0) & M66592_VBSTS) {
@@ -1494,7 +1493,6 @@ static int m66592_udc_stop(struct usb_gadget *g,
 	m66592_bclr(m66592, M66592_VBSE | M66592_URST, M66592_INTENB0);
 
 	driver->unbind(&m66592->gadget);
-	m66592->gadget.dev.driver = NULL;
 
 	init_controller(m66592);
 	disable_controller(m66592);
@@ -1538,7 +1536,6 @@ static int __exit m66592_remove(struct platform_device *pdev)
 	struct m66592		*m66592 = dev_get_drvdata(&pdev->dev);
 
 	usb_del_gadget_udc(&m66592->gadget);
-	device_del(&m66592->gadget.dev);
 
 	del_timer_sync(&m66592->timer);
 	iounmap(m66592->reg);
@@ -1608,12 +1605,7 @@ static int __init m66592_probe(struct platform_device *pdev)
 	dev_set_drvdata(&pdev->dev, m66592);
 
 	m66592->gadget.ops = &m66592_gadget_ops;
-	device_initialize(&m66592->gadget.dev);
-	dev_set_name(&m66592->gadget.dev, "gadget");
 	m66592->gadget.max_speed = USB_SPEED_HIGH;
-	m66592->gadget.dev.parent = &pdev->dev;
-	m66592->gadget.dev.dma_mask = pdev->dev.dma_mask;
-	m66592->gadget.dev.release = pdev->dev.release;
 	m66592->gadget.name = udc_name;
 
 	init_timer(&m66592->timer);
@@ -1674,12 +1666,6 @@ static int __init m66592_probe(struct platform_device *pdev)
 
 	init_controller(m66592);
 
-	ret = device_add(&m66592->gadget.dev);
-	if (ret) {
-		pr_err("device_add error (%d)\n", ret);
-		goto err_device_add;
-	}
-
 	ret = usb_add_gadget_udc(&pdev->dev, &m66592->gadget);
 	if (ret)
 		goto err_add_udc;
@@ -1688,9 +1674,6 @@ static int __init m66592_probe(struct platform_device *pdev)
 	return 0;
 
 err_add_udc:
-	device_del(&m66592->gadget.dev);
-
-err_device_add:
 	m66592_free_request(&m66592->ep[0].ep, m66592->ep0_req);
 
 clean_up3:
diff --git a/drivers/usb/gadget/multi.c b/drivers/usb/gadget/multi.c
index 20bbbf917fc..4a45e80c6e3 100644
--- a/drivers/usb/gadget/multi.c
+++ b/drivers/usb/gadget/multi.c
@@ -135,8 +135,8 @@ static struct fsg_common fsg_common;
 
 static u8 hostaddr[ETH_ALEN];
 
-static unsigned char tty_line;
 static struct usb_function_instance *fi_acm;
+static struct eth_dev *the_dev;
 
 /********** RNDIS **********/
 
@@ -152,13 +152,15 @@ static __init int rndis_do_config(struct usb_configuration *c)
 		c->bmAttributes |= USB_CONFIG_ATT_WAKEUP;
 	}
 
-	ret = rndis_bind_config(c, hostaddr);
+	ret = rndis_bind_config(c, hostaddr, the_dev);
 	if (ret < 0)
 		return ret;
 
 	f_acm_rndis = usb_get_function(fi_acm);
-	if (IS_ERR(f_acm_rndis))
+	if (IS_ERR(f_acm_rndis)) {
+		ret = PTR_ERR(f_acm_rndis);
 		goto err_func_acm;
+	}
 
 	ret = usb_add_function(c, f_acm_rndis);
 	if (ret)
@@ -214,7 +216,7 @@ static __init int cdc_do_config(struct usb_configuration *c)
 		c->bmAttributes |= USB_CONFIG_ATT_WAKEUP;
 	}
 
-	ret = ecm_bind_config(c, hostaddr);
+	ret = ecm_bind_config(c, hostaddr, the_dev);
 	if (ret < 0)
 		return ret;
 
@@ -269,7 +271,6 @@ static int cdc_config_register(struct usb_composite_dev *cdev)
 static int __ref multi_bind(struct usb_composite_dev *cdev)
 {
 	struct usb_gadget *gadget = cdev->gadget;
-	struct f_serial_opts *opts;
 	int status;
 
 	if (!can_support_ecm(cdev->gadget)) {
@@ -279,24 +280,17 @@ static int __ref multi_bind(struct usb_composite_dev *cdev)
 	}
 
 	/* set up network link layer */
-	status = gether_setup(cdev->gadget, hostaddr);
-	if (status < 0)
-		return status;
+	the_dev = gether_setup(cdev->gadget, hostaddr);
+	if (IS_ERR(the_dev))
+		return PTR_ERR(the_dev);
 
 	/* set up serial link layer */
-	status = gserial_alloc_line(&tty_line);
-	if (status < 0)
-		goto fail0;
-
 	fi_acm = usb_get_function_instance("acm");
 	if (IS_ERR(fi_acm)) {
 		status = PTR_ERR(fi_acm);
-		goto fail0dot5;
+		goto fail0;
 	}
 
-	opts = container_of(fi_acm, struct f_serial_opts, func_inst);
-	opts->port_num = tty_line;
-
 	/* set up mass storage function */
 	{
 		void *retp;
@@ -334,10 +328,8 @@ fail2:
 	fsg_common_put(&fsg_common);
 fail1:
 	usb_put_function_instance(fi_acm);
-fail0dot5:
-	gserial_free_line(tty_line);
 fail0:
-	gether_cleanup();
+	gether_cleanup(the_dev);
 	return status;
 }
 
@@ -350,8 +342,7 @@ static int __exit multi_unbind(struct usb_composite_dev *cdev)
 	usb_put_function(f_acm_rndis);
 #endif
 	usb_put_function_instance(fi_acm);
-	gserial_free_line(tty_line);
-	gether_cleanup();
+	gether_cleanup(the_dev);
 	return 0;
 }
 
diff --git a/drivers/usb/gadget/mv_u3d_core.c b/drivers/usb/gadget/mv_u3d_core.c
index b5cea273c95..58288e9cf72 100644
--- a/drivers/usb/gadget/mv_u3d_core.c
+++ b/drivers/usb/gadget/mv_u3d_core.c
@@ -30,9 +30,6 @@
 #include <linux/platform_device.h>
 #include <linux/platform_data/mv_usb.h>
 #include <linux/clk.h>
-#include <asm/system.h>
-#include <asm/unaligned.h>
-#include <asm/byteorder.h>
 
 #include "mv_u3d.h"
 
@@ -125,7 +122,7 @@ static int mv_u3d_process_ep_req(struct mv_u3d *u3d, int index,
 	struct mv_u3d_trb	*curr_trb;
 	dma_addr_t cur_deq_lo;
 	struct mv_u3d_ep_context	*curr_ep_context;
-	int trb_complete, actual, remaining_length;
+	int trb_complete, actual, remaining_length = 0;
 	int direction, ep_num;
 	int retval = 0;
 	u32 tmp, status, length;
@@ -189,6 +186,8 @@ static int mv_u3d_process_ep_req(struct mv_u3d *u3d, int index,
  */
 static
 void mv_u3d_done(struct mv_u3d_ep *ep, struct mv_u3d_req *req, int status)
+	__releases(&ep->udc->lock)
+	__acquires(&ep->udc->lock)
 {
 	struct mv_u3d *u3d = (struct mv_u3d *)ep->u3d;
 
@@ -812,19 +811,19 @@ mv_u3d_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 		return 0;
 	}
 
-	dev_dbg(u3d->dev, "%s: %s, req: 0x%x\n",
-			__func__, _ep->name, (u32)req);
+	dev_dbg(u3d->dev, "%s: %s, req: 0x%p\n",
+			__func__, _ep->name, req);
 
 	/* catch various bogus parameters */
 	if (!req->req.complete || !req->req.buf
 			|| !list_empty(&req->queue)) {
 		dev_err(u3d->dev,
-			"%s, bad params, _req: 0x%x,"
-			"req->req.complete: 0x%x, req->req.buf: 0x%x,"
+			"%s, bad params, _req: 0x%p,"
+			"req->req.complete: 0x%p, req->req.buf: 0x%p,"
 			"list_empty: 0x%x\n",
-			__func__, (u32)_req,
-			(u32)req->req.complete, (u32)req->req.buf,
-			(u32)list_empty(&req->queue));
+			__func__, _req,
+			req->req.complete, req->req.buf,
+			list_empty(&req->queue));
 		return -EINVAL;
 	}
 	if (unlikely(!ep->ep.desc)) {
@@ -905,7 +904,7 @@ static int mv_u3d_ep_dequeue(struct usb_ep *_ep, struct usb_request *_req)
 					struct mv_u3d_req, queue);
 
 			/* Point first TRB of next request to the EP context. */
-			iowrite32((u32) next_req->trb_head,
+			iowrite32((unsigned long) next_req->trb_head,
 					&ep_context->trb_addr_lo);
 		} else {
 			struct mv_u3d_ep_context *ep_context;
@@ -1264,7 +1263,6 @@ static int mv_u3d_start(struct usb_gadget *g,
 	/* hook up the driver ... */
 	driver->driver.bus = NULL;
 	u3d->driver = driver;
-	u3d->gadget.dev.driver = &driver->driver;
 
 	u3d->ep0_dir = USB_DIR_OUT;
 
@@ -1302,7 +1300,6 @@ static int mv_u3d_stop(struct usb_gadget *g,
 
 	spin_unlock_irqrestore(&u3d->lock, flags);
 
-	u3d->gadget.dev.driver = NULL;
 	u3d->driver = NULL;
 
 	return 0;
@@ -1525,6 +1522,8 @@ static int mv_u3d_is_set_configuration(struct usb_ctrlrequest *setup)
 
 static void mv_u3d_handle_setup_packet(struct mv_u3d *u3d, u8 ep_num,
 	struct usb_ctrlrequest *setup)
+	__releases(&u3c->lock)
+	__acquires(&u3c->lock)
 {
 	bool delegate = false;
 
@@ -1758,11 +1757,6 @@ static irqreturn_t mv_u3d_irq(int irq, void *dev)
 	return IRQ_HANDLED;
 }
 
-static void mv_u3d_gadget_release(struct device *dev)
-{
-	dev_dbg(dev, "%s\n", __func__);
-}
-
 static int mv_u3d_remove(struct platform_device *dev)
 {
 	struct mv_u3d *u3d = platform_get_drvdata(dev);
@@ -1792,8 +1786,6 @@ static int mv_u3d_remove(struct platform_device *dev)
 
 	clk_put(u3d->clk);
 
-	device_unregister(&u3d->gadget.dev);
-
 	platform_set_drvdata(dev, NULL);
 
 	kfree(u3d);
@@ -1829,7 +1821,7 @@ static int mv_u3d_probe(struct platform_device *dev)
 	u3d->dev = &dev->dev;
 	u3d->vbus = pdata->vbus;
 
-	u3d->clk = clk_get(&dev->dev, pdata->clkname[0]);
+	u3d->clk = clk_get(&dev->dev, NULL);
 	if (IS_ERR(u3d->clk)) {
 		retval = PTR_ERR(u3d->clk);
 		goto err_get_clk;
@@ -1849,8 +1841,9 @@ static int mv_u3d_probe(struct platform_device *dev)
 		retval = -EBUSY;
 		goto err_map_cap_regs;
 	} else {
-		dev_dbg(&dev->dev, "cap_regs address: 0x%x/0x%x\n",
-			(unsigned int)r->start, (unsigned int)u3d->cap_regs);
+		dev_dbg(&dev->dev, "cap_regs address: 0x%lx/0x%lx\n",
+			(unsigned long) r->start,
+			(unsigned long) u3d->cap_regs);
 	}
 
 	/* we will access controller register, so enable the u3d controller */
@@ -1864,10 +1857,10 @@ static int mv_u3d_probe(struct platform_device *dev)
 		}
 	}
 
-	u3d->op_regs = (struct mv_u3d_op_regs __iomem *)((u32)u3d->cap_regs
+	u3d->op_regs = (struct mv_u3d_op_regs __iomem *)(u3d->cap_regs
 		+ MV_U3D_USB3_OP_REGS_OFFSET);
 
-	u3d->vuc_regs = (struct mv_u3d_vuc_regs __iomem *)((u32)u3d->cap_regs
+	u3d->vuc_regs = (struct mv_u3d_vuc_regs __iomem *)(u3d->cap_regs
 		+ ioread32(&u3d->cap_regs->vuoff));
 
 	u3d->max_eps = 16;
@@ -1957,16 +1950,8 @@ static int mv_u3d_probe(struct platform_device *dev)
 	u3d->gadget.speed = USB_SPEED_UNKNOWN;	/* speed */
 
 	/* the "gadget" abstracts/virtualizes the controller */
-	dev_set_name(&u3d->gadget.dev, "gadget");
-	u3d->gadget.dev.parent = &dev->dev;
-	u3d->gadget.dev.dma_mask = dev->dev.dma_mask;
-	u3d->gadget.dev.release = mv_u3d_gadget_release;
 	u3d->gadget.name = driver_name;		/* gadget name */
 
-	retval = device_register(&u3d->gadget.dev);
-	if (retval)
-		goto err_register_gadget_device;
-
 	mv_u3d_eps_init(u3d);
 
 	/* external vbus detection */
@@ -1991,8 +1976,6 @@ static int mv_u3d_probe(struct platform_device *dev)
 	return 0;
 
 err_unregister:
-	device_unregister(&u3d->gadget.dev);
-err_register_gadget_device:
 	free_irq(u3d->irq, &dev->dev);
 err_request_irq:
 err_get_irq:
@@ -2021,7 +2004,7 @@ err_pdata:
 	return retval;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int mv_u3d_suspend(struct device *dev)
 {
 	struct mv_u3d *u3d = dev_get_drvdata(dev);
@@ -2064,10 +2047,10 @@ static int mv_u3d_resume(struct device *dev)
 
 	return 0;
 }
-
-SIMPLE_DEV_PM_OPS(mv_u3d_pm_ops, mv_u3d_suspend, mv_u3d_resume);
 #endif
 
+static SIMPLE_DEV_PM_OPS(mv_u3d_pm_ops, mv_u3d_suspend, mv_u3d_resume);
+
 static void mv_u3d_shutdown(struct platform_device *dev)
 {
 	struct mv_u3d *u3d = dev_get_drvdata(&dev->dev);
@@ -2080,14 +2063,12 @@ static void mv_u3d_shutdown(struct platform_device *dev)
 
 static struct platform_driver mv_u3d_driver = {
 	.probe		= mv_u3d_probe,
-	.remove		= __exit_p(mv_u3d_remove),
+	.remove		= mv_u3d_remove,
 	.shutdown	= mv_u3d_shutdown,
 	.driver		= {
 		.owner	= THIS_MODULE,
 		.name	= "mv-u3d",
-#ifdef CONFIG_PM
 		.pm	= &mv_u3d_pm_ops,
-#endif
 	},
 };
 
diff --git a/drivers/usb/gadget/mv_udc.h b/drivers/usb/gadget/mv_udc.h
index 9073436d8b2..be77f207dba 100644
--- a/drivers/usb/gadget/mv_udc.h
+++ b/drivers/usb/gadget/mv_udc.h
@@ -222,8 +222,7 @@ struct mv_udc {
 	struct mv_usb_platform_data     *pdata;
 
 	/* some SOC has mutiple clock sources for USB*/
-	unsigned int    clknum;
-	struct clk      *clk[0];
+	struct clk      *clk;
 };
 
 /* endpoint data structure */
diff --git a/drivers/usb/gadget/mv_udc_core.c b/drivers/usb/gadget/mv_udc_core.c
index c8cf959057f..c2a57023e46 100644
--- a/drivers/usb/gadget/mv_udc_core.c
+++ b/drivers/usb/gadget/mv_udc_core.c
@@ -212,6 +212,8 @@ static int process_ep_req(struct mv_udc *udc, int index,
  * request is still in progress.
  */
 static void done(struct mv_ep *ep, struct mv_req *req, int status)
+	__releases(&ep->udc->lock)
+	__acquires(&ep->udc->lock)
 {
 	struct mv_udc *udc = NULL;
 	unsigned char stopped = ep->stopped;
@@ -237,18 +239,7 @@ static void done(struct mv_ep *ep, struct mv_req *req, int status)
 		dma_pool_free(udc->dtd_pool, curr_td, curr_td->td_dma);
 	}
 
-	if (req->mapped) {
-		dma_unmap_single(ep->udc->gadget.dev.parent,
-			req->req.dma, req->req.length,
-			((ep_dir(ep) == EP_DIR_IN) ?
-				DMA_TO_DEVICE : DMA_FROM_DEVICE));
-		req->req.dma = DMA_ADDR_INVALID;
-		req->mapped = 0;
-	} else
-		dma_sync_single_for_cpu(ep->udc->gadget.dev.parent,
-			req->req.dma, req->req.length,
-			((ep_dir(ep) == EP_DIR_IN) ?
-				DMA_TO_DEVICE : DMA_FROM_DEVICE));
+	usb_gadget_unmap_request(&udc->gadget, &req->req, ep_dir(ep));
 
 	if (status && (status != -ESHUTDOWN))
 		dev_info(&udc->dev->dev, "complete %s req %p stat %d len %u/%u",
@@ -732,21 +723,9 @@ mv_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 	req->ep = ep;
 
 	/* map virtual address to hardware */
-	if (req->req.dma == DMA_ADDR_INVALID) {
-		req->req.dma = dma_map_single(ep->udc->gadget.dev.parent,
-					req->req.buf,
-					req->req.length, ep_dir(ep)
-						? DMA_TO_DEVICE
-						: DMA_FROM_DEVICE);
-		req->mapped = 1;
-	} else {
-		dma_sync_single_for_device(ep->udc->gadget.dev.parent,
-					req->req.dma, req->req.length,
-					ep_dir(ep)
-						? DMA_TO_DEVICE
-						: DMA_FROM_DEVICE);
-		req->mapped = 0;
-	}
+	retval = usb_gadget_map_request(&udc->gadget, _req, ep_dir(ep));
+	if (retval)
+		return retval;
 
 	req->req.status = -EINPROGRESS;
 	req->req.actual = 0;
@@ -780,18 +759,7 @@ mv_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 	return 0;
 
 err_unmap_dma:
-	if (req->mapped) {
-		dma_unmap_single(ep->udc->gadget.dev.parent,
-				req->req.dma, req->req.length,
-				((ep_dir(ep) == EP_DIR_IN) ?
-				DMA_TO_DEVICE : DMA_FROM_DEVICE));
-		req->req.dma = DMA_ADDR_INVALID;
-		req->mapped = 0;
-	} else
-		dma_sync_single_for_cpu(ep->udc->gadget.dev.parent,
-				req->req.dma, req->req.length,
-				((ep_dir(ep) == EP_DIR_IN) ?
-				DMA_TO_DEVICE : DMA_FROM_DEVICE));
+	usb_gadget_unmap_request(&udc->gadget, _req, ep_dir(ep));
 
 	return retval;
 }
@@ -1006,18 +974,12 @@ static struct usb_ep_ops mv_ep_ops = {
 
 static void udc_clock_enable(struct mv_udc *udc)
 {
-	unsigned int i;
-
-	for (i = 0; i < udc->clknum; i++)
-		clk_prepare_enable(udc->clk[i]);
+	clk_prepare_enable(udc->clk);
 }
 
 static void udc_clock_disable(struct mv_udc *udc)
 {
-	unsigned int i;
-
-	for (i = 0; i < udc->clknum; i++)
-		clk_disable_unprepare(udc->clk[i]);
+	clk_disable_unprepare(udc->clk);
 }
 
 static void udc_stop(struct mv_udc *udc)
@@ -1386,7 +1348,6 @@ static int mv_udc_start(struct usb_gadget *gadget,
 	/* hook up the driver ... */
 	driver->driver.bus = NULL;
 	udc->driver = driver;
-	udc->gadget.dev.driver = &driver->driver;
 
 	udc->usb_state = USB_STATE_ATTACHED;
 	udc->ep0_state = WAIT_FOR_SETUP;
@@ -1401,7 +1362,6 @@ static int mv_udc_start(struct usb_gadget *gadget,
 			dev_err(&udc->dev->dev,
 				"unable to register peripheral to otg\n");
 			udc->driver = NULL;
-			udc->gadget.dev.driver = NULL;
 			return retval;
 		}
 	}
@@ -1437,7 +1397,6 @@ static int mv_udc_stop(struct usb_gadget *gadget,
 	spin_unlock_irqrestore(&udc->lock, flags);
 
 	/* unbind gadget driver */
-	udc->gadget.dev.driver = NULL;
 	udc->driver = NULL;
 
 	return 0;
@@ -1528,14 +1487,7 @@ udc_prime_status(struct mv_udc *udc, u8 direction, u16 status, bool empty)
 
 	return 0;
 out:
-	if (req->mapped) {
-		dma_unmap_single(ep->udc->gadget.dev.parent,
-				req->req.dma, req->req.length,
-				((ep_dir(ep) == EP_DIR_IN) ?
-				DMA_TO_DEVICE : DMA_FROM_DEVICE));
-		req->req.dma = DMA_ADDR_INVALID;
-		req->mapped = 0;
-	}
+	usb_gadget_unmap_request(&udc->gadget, &req->req, ep_dir(ep));
 
 	return retval;
 }
@@ -1695,6 +1647,8 @@ out:
 
 static void handle_setup_packet(struct mv_udc *udc, u8 ep_num,
 	struct usb_ctrlrequest *setup)
+	__releases(&ep->udc->lock)
+	__acquires(&ep->udc->lock)
 {
 	bool delegate = false;
 
@@ -1891,7 +1845,7 @@ static void irq_process_tr_complete(struct mv_udc *udc)
 	}
 }
 
-void irq_process_reset(struct mv_udc *udc)
+static void irq_process_reset(struct mv_udc *udc)
 {
 	u32 tmp;
 	unsigned int loops;
@@ -2138,8 +2092,6 @@ static int mv_udc_remove(struct platform_device *pdev)
 
 	mv_udc_disable(udc);
 
-	device_unregister(&udc->gadget.dev);
-
 	/* free dev, wait for the release() finished */
 	wait_for_completion(udc->done);
 
@@ -2151,7 +2103,6 @@ static int mv_udc_probe(struct platform_device *pdev)
 	struct mv_usb_platform_data *pdata = pdev->dev.platform_data;
 	struct mv_udc *udc;
 	int retval = 0;
-	int clk_i = 0;
 	struct resource *r;
 	size_t size;
 
@@ -2160,8 +2111,7 @@ static int mv_udc_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	size = sizeof(*udc) + sizeof(struct clk *) * pdata->clknum;
-	udc = devm_kzalloc(&pdev->dev, size, GFP_KERNEL);
+	udc = devm_kzalloc(&pdev->dev, sizeof(*udc), GFP_KERNEL);
 	if (udc == NULL) {
 		dev_err(&pdev->dev, "failed to allocate memory for udc\n");
 		return -ENOMEM;
@@ -2173,26 +2123,24 @@ static int mv_udc_probe(struct platform_device *pdev)
 
 	udc->dev = pdev;
 
-#ifdef CONFIG_USB_OTG_UTILS
 	if (pdata->mode == MV_USB_MODE_OTG) {
 		udc->transceiver = devm_usb_get_phy(&pdev->dev,
 					USB_PHY_TYPE_USB2);
-		if (IS_ERR_OR_NULL(udc->transceiver)) {
+		if (IS_ERR(udc->transceiver)) {
+			retval = PTR_ERR(udc->transceiver);
+
+			if (retval == -ENXIO)
+				return retval;
+
 			udc->transceiver = NULL;
-			return -ENODEV;
+			return -EPROBE_DEFER;
 		}
 	}
-#endif
 
-	udc->clknum = pdata->clknum;
-	for (clk_i = 0; clk_i < udc->clknum; clk_i++) {
-		udc->clk[clk_i] = devm_clk_get(&pdev->dev,
-					pdata->clkname[clk_i]);
-		if (IS_ERR(udc->clk[clk_i])) {
-			retval = PTR_ERR(udc->clk[clk_i]);
-			return retval;
-		}
-	}
+	/* udc only have one sysclk. */
+	udc->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(udc->clk))
+		return PTR_ERR(udc->clk);
 
 	r = platform_get_resource_byname(udc->dev, IORESOURCE_MEM, "capregs");
 	if (r == NULL) {
@@ -2311,16 +2259,8 @@ static int mv_udc_probe(struct platform_device *pdev)
 	udc->gadget.max_speed = USB_SPEED_HIGH;	/* support dual speed */
 
 	/* the "gadget" abstracts/virtualizes the controller */
-	dev_set_name(&udc->gadget.dev, "gadget");
-	udc->gadget.dev.parent = &pdev->dev;
-	udc->gadget.dev.dma_mask = pdev->dev.dma_mask;
-	udc->gadget.dev.release = gadget_release;
 	udc->gadget.name = driver_name;		/* gadget name */
 
-	retval = device_register(&udc->gadget.dev);
-	if (retval)
-		goto err_destroy_dma;
-
 	eps_init(udc);
 
 	/* VBUS detect: we can disable/enable clock on demand.*/
@@ -2342,7 +2282,7 @@ static int mv_udc_probe(struct platform_device *pdev)
 		if (!udc->qwork) {
 			dev_err(&pdev->dev, "cannot create workqueue\n");
 			retval = -ENOMEM;
-			goto err_unregister;
+			goto err_destroy_dma;
 		}
 
 		INIT_WORK(&udc->vbus_work, mv_udc_vbus_work);
@@ -2358,7 +2298,8 @@ static int mv_udc_probe(struct platform_device *pdev)
 	else
 		udc->vbus_active = 1;
 
-	retval = usb_add_gadget_udc(&pdev->dev, &udc->gadget);
+	retval = usb_add_gadget_udc_release(&pdev->dev, &udc->gadget,
+			gadget_release);
 	if (retval)
 		goto err_create_workqueue;
 
@@ -2370,8 +2311,6 @@ static int mv_udc_probe(struct platform_device *pdev)
 
 err_create_workqueue:
 	destroy_workqueue(udc->qwork);
-err_unregister:
-	device_unregister(&udc->gadget.dev);
 err_destroy_dma:
 	dma_pool_destroy(udc->dtd_pool);
 err_free_dma:
diff --git a/drivers/usb/gadget/ncm.c b/drivers/usb/gadget/ncm.c
index a22ad9af056..3b02fd4649c 100644
--- a/drivers/usb/gadget/ncm.c
+++ b/drivers/usb/gadget/ncm.c
@@ -111,6 +111,7 @@ static struct usb_gadget_strings *dev_strings[] = {
 	NULL,
 };
 
+struct eth_dev *the_dev;
 static u8 hostaddr[ETH_ALEN];
 
 /*-------------------------------------------------------------------------*/
@@ -124,7 +125,7 @@ static int __init ncm_do_config(struct usb_configuration *c)
 		c->bmAttributes |= USB_CONFIG_ATT_WAKEUP;
 	}
 
-	return ncm_bind_config(c, hostaddr);
+	return ncm_bind_config(c, hostaddr, the_dev);
 }
 
 static struct usb_configuration ncm_config_driver = {
@@ -143,9 +144,9 @@ static int __init gncm_bind(struct usb_composite_dev *cdev)
 	int			status;
 
 	/* set up network link layer */
-	status = gether_setup(cdev->gadget, hostaddr);
-	if (status < 0)
-		return status;
+	the_dev = gether_setup(cdev->gadget, hostaddr);
+	if (IS_ERR(the_dev))
+		return PTR_ERR(the_dev);
 
 	/* Allocate string descriptor numbers ... note that string
 	 * contents can be overridden by the composite_dev glue.
@@ -168,13 +169,13 @@ static int __init gncm_bind(struct usb_composite_dev *cdev)
 	return 0;
 
 fail:
-	gether_cleanup();
+	gether_cleanup(the_dev);
 	return status;
 }
 
 static int __exit gncm_unbind(struct usb_composite_dev *cdev)
 {
-	gether_cleanup();
+	gether_cleanup(the_dev);
 	return 0;
 }
 
diff --git a/drivers/usb/gadget/net2272.c b/drivers/usb/gadget/net2272.c
index 32524b63195..f1e50a3e322 100644
--- a/drivers/usb/gadget/net2272.c
+++ b/drivers/usb/gadget/net2272.c
@@ -58,7 +58,6 @@ static const char * const ep_name[] = {
 	"ep-a", "ep-b", "ep-c",
 };
 
-#define DMA_ADDR_INVALID	(~(dma_addr_t)0)
 #ifdef CONFIG_USB_NET2272_DMA
 /*
  * use_dma: the NET2272 can use an external DMA controller.
@@ -341,7 +340,6 @@ net2272_alloc_request(struct usb_ep *_ep, gfp_t gfp_flags)
 	if (!req)
 		return NULL;
 
-	req->req.dma = DMA_ADDR_INVALID;
 	INIT_LIST_HEAD(&req->queue);
 
 	return &req->req;
@@ -913,7 +911,7 @@ net2272_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 			}
 		}
 	}
-	if (likely(req != 0))
+	if (likely(req))
 		list_add_tail(&req->queue, &ep->queue);
 
 	if (likely(!list_empty(&ep->queue)))
@@ -1467,7 +1465,6 @@ static int net2272_start(struct usb_gadget *_gadget,
 	dev->softconnect = 1;
 	driver->driver.bus = NULL;
 	dev->driver = driver;
-	dev->gadget.dev.driver = &driver->driver;
 
 	/* ... then enable host detection and ep0; and we're ready
 	 * for set_configuration as well as eventual disconnect.
@@ -1517,7 +1514,6 @@ static int net2272_stop(struct usb_gadget *_gadget,
 	stop_activity(dev, driver);
 	spin_unlock_irqrestore(&dev->lock, flags);
 
-	dev->gadget.dev.driver = NULL;
 	dev->driver = NULL;
 
 	dev_dbg(dev->dev, "unregistered driver '%s'\n", driver->driver.name);
@@ -1549,7 +1545,7 @@ net2272_handle_dma(struct net2272_ep *ep)
 	      | (ep->dev->dma_eot_polarity << EOT_POLARITY)
 	      | (ep->dev->dma_dack_polarity << DACK_POLARITY)
 	      | (ep->dev->dma_dreq_polarity << DREQ_POLARITY)
-	      | ((ep->dma >> 1) << DMA_ENDPOINT_SELECT));
+	      | (ep->dma << DMA_ENDPOINT_SELECT));
 
 	ep->dev->dma_busy = 0;
 
@@ -1622,7 +1618,7 @@ net2272_handle_ep(struct net2272_ep *ep)
 	ep->irqs++;
 
 	dev_vdbg(ep->dev->dev, "%s ack ep_stat0 %02x, ep_stat1 %02x, req %p\n",
-		ep->ep.name, stat0, stat1, req ? &req->req : 0);
+		ep->ep.name, stat0, stat1, req ? &req->req : NULL);
 
 	net2272_ep_write(ep, EP_STAT0, stat0 &
 		~((1 << NAK_OUT_PACKETS)
@@ -2216,7 +2212,6 @@ net2272_remove(struct net2272 *dev)
 	free_irq(dev->irq, dev);
 	iounmap(dev->base_addr);
 
-	device_unregister(&dev->gadget.dev);
 	device_remove_file(dev->dev, &dev_attr_registers);
 
 	dev_info(dev->dev, "unbind\n");
@@ -2243,10 +2238,6 @@ static struct net2272 *net2272_probe_init(struct device *dev, unsigned int irq)
 	ret->gadget.max_speed = USB_SPEED_HIGH;
 
 	/* the "gadget" abstracts/virtualizes the controller */
-	dev_set_name(&ret->gadget.dev, "gadget");
-	ret->gadget.dev.parent = dev;
-	ret->gadget.dev.dma_mask = dev->dma_mask;
-	ret->gadget.dev.release = net2272_gadget_release;
 	ret->gadget.name = driver_name;
 
 	return ret;
@@ -2282,14 +2273,12 @@ net2272_probe_fin(struct net2272 *dev, unsigned int irqflags)
 		dma_mode_string());
 	dev_info(dev->dev, "version: %s\n", driver_vers);
 
-	ret = device_register(&dev->gadget.dev);
-	if (ret)
-		goto err_irq;
 	ret = device_create_file(dev->dev, &dev_attr_registers);
 	if (ret)
-		goto err_dev_reg;
+		goto err_irq;
 
-	ret = usb_add_gadget_udc(dev->dev, &dev->gadget);
+	ret = usb_add_gadget_udc_release(dev->dev, &dev->gadget,
+			net2272_gadget_release);
 	if (ret)
 		goto err_add_udc;
 
@@ -2297,8 +2286,6 @@ net2272_probe_fin(struct net2272 *dev, unsigned int irqflags)
 
 err_add_udc:
 	device_remove_file(dev->dev, &dev_attr_registers);
- err_dev_reg:
-	device_unregister(&dev->gadget.dev);
  err_irq:
 	free_irq(dev->irq, dev);
  err:
diff --git a/drivers/usb/gadget/net2280.c b/drivers/usb/gadget/net2280.c
index 3bd0f992fb4..fbd006ab31d 100644
--- a/drivers/usb/gadget/net2280.c
+++ b/drivers/usb/gadget/net2280.c
@@ -65,7 +65,6 @@
 #define	DRIVER_DESC		"PLX NET228x USB Peripheral Controller"
 #define	DRIVER_VERSION		"2005 Sept 27"
 
-#define	DMA_ADDR_INVALID	(~(dma_addr_t)0)
 #define	EP_DONTUSE		13	/* nonzero */
 
 #define USE_RDK_LEDS		/* GPIO pins control three LEDs */
@@ -406,7 +405,6 @@ net2280_alloc_request (struct usb_ep *_ep, gfp_t gfp_flags)
 	if (!req)
 		return NULL;
 
-	req->req.dma = DMA_ADDR_INVALID;
 	INIT_LIST_HEAD (&req->queue);
 
 	/* this dma descriptor may be swapped with the previous dummy */
@@ -420,7 +418,6 @@ net2280_alloc_request (struct usb_ep *_ep, gfp_t gfp_flags)
 			return NULL;
 		}
 		td->dmacount = 0;	/* not VALID */
-		td->dmaaddr = cpu_to_le32 (DMA_ADDR_INVALID);
 		td->dmadesc = td->dmaaddr;
 		req->td = td;
 	}
@@ -1896,7 +1893,6 @@ static int net2280_start(struct usb_gadget *_gadget,
 	dev->softconnect = 1;
 	driver->driver.bus = NULL;
 	dev->driver = driver;
-	dev->gadget.dev.driver = &driver->driver;
 
 	retval = device_create_file (&dev->pdev->dev, &dev_attr_function);
 	if (retval) goto err_unbind;
@@ -1924,7 +1920,6 @@ static int net2280_start(struct usb_gadget *_gadget,
 err_func:
 	device_remove_file (&dev->pdev->dev, &dev_attr_function);
 err_unbind:
-	dev->gadget.dev.driver = NULL;
 	dev->driver = NULL;
 	return retval;
 }
@@ -1967,7 +1962,6 @@ static int net2280_stop(struct usb_gadget *_gadget,
 	stop_activity (dev, driver);
 	spin_unlock_irqrestore (&dev->lock, flags);
 
-	dev->gadget.dev.driver = NULL;
 	dev->driver = NULL;
 
 	net2280_led_active (dev, 0);
@@ -2072,7 +2066,7 @@ static void handle_ep_small (struct net2280_ep *ep)
 		return;
 
 	/* manual DMA queue advance after short OUT */
-	if (likely (ep->dma != 0)) {
+	if (likely (ep->dma)) {
 		if (t & (1 << SHORT_PACKET_TRANSFERRED_INTERRUPT)) {
 			u32	count;
 			int	stopped = ep->stopped;
@@ -2330,7 +2324,7 @@ static void handle_stat0_irqs (struct net2280 *dev, u32 stat)
 			/* hw handles device and interface status */
 			if (u.r.bRequestType != (USB_DIR_IN|USB_RECIP_ENDPOINT))
 				goto delegate;
-			if ((e = get_ep_by_addr (dev, w_index)) == 0
+			if ((e = get_ep_by_addr (dev, w_index)) == NULL
 					|| w_length > 2)
 				goto do_stall;
 
@@ -2358,7 +2352,7 @@ static void handle_stat0_irqs (struct net2280 *dev, u32 stat)
 			if (w_value != USB_ENDPOINT_HALT
 					|| w_length != 0)
 				goto do_stall;
-			if ((e = get_ep_by_addr (dev, w_index)) == 0)
+			if ((e = get_ep_by_addr (dev, w_index)) == NULL)
 				goto do_stall;
 			if (e->wedged) {
 				VDEBUG(dev, "%s wedged, halt not cleared\n",
@@ -2380,7 +2374,7 @@ static void handle_stat0_irqs (struct net2280 *dev, u32 stat)
 			if (w_value != USB_ENDPOINT_HALT
 					|| w_length != 0)
 				goto do_stall;
-			if ((e = get_ep_by_addr (dev, w_index)) == 0)
+			if ((e = get_ep_by_addr (dev, w_index)) == NULL)
 				goto do_stall;
 			if (e->ep.name == ep0name)
 				goto do_stall;
@@ -2685,7 +2679,6 @@ static void net2280_remove (struct pci_dev *pdev)
 				pci_resource_len (pdev, 0));
 	if (dev->enabled)
 		pci_disable_device (pdev);
-	device_unregister (&dev->gadget.dev);
 	device_remove_file (&pdev->dev, &dev_attr_registers);
 	pci_set_drvdata (pdev, NULL);
 
@@ -2717,10 +2710,6 @@ static int net2280_probe (struct pci_dev *pdev, const struct pci_device_id *id)
 	dev->gadget.max_speed = USB_SPEED_HIGH;
 
 	/* the "gadget" abstracts/virtualizes the controller */
-	dev_set_name(&dev->gadget.dev, "gadget");
-	dev->gadget.dev.parent = &pdev->dev;
-	dev->gadget.dev.dma_mask = pdev->dev.dma_mask;
-	dev->gadget.dev.release = gadget_release;
 	dev->gadget.name = driver_name;
 
 	/* now all the pci goodies ... */
@@ -2802,7 +2791,6 @@ static int net2280_probe (struct pci_dev *pdev, const struct pci_device_id *id)
 			goto done;
 		}
 		td->dmacount = 0;	/* not VALID */
-		td->dmaaddr = cpu_to_le32 (DMA_ADDR_INVALID);
 		td->dmadesc = td->dmaaddr;
 		dev->ep [i].dummy = td;
 	}
@@ -2829,12 +2817,11 @@ static int net2280_probe (struct pci_dev *pdev, const struct pci_device_id *id)
 			use_dma
 				? (use_dma_chaining ? "chaining" : "enabled")
 				: "disabled");
-	retval = device_register (&dev->gadget.dev);
-	if (retval) goto done;
 	retval = device_create_file (&pdev->dev, &dev_attr_registers);
 	if (retval) goto done;
 
-	retval = usb_add_gadget_udc(&pdev->dev, &dev->gadget);
+	retval = usb_add_gadget_udc_release(&pdev->dev, &dev->gadget,
+			gadget_release);
 	if (retval)
 		goto done;
 	return 0;
diff --git a/drivers/usb/gadget/nokia.c b/drivers/usb/gadget/nokia.c
index def37403989..3b344b41a16 100644
--- a/drivers/usb/gadget/nokia.c
+++ b/drivers/usb/gadget/nokia.c
@@ -37,11 +37,9 @@
  * the runtime footprint, and giving us at least some parts of what
  * a "gcc --combine ... part1.c part2.c part3.c ... " build would.
  */
-#define USB_FACM_INCLUDED
-#include "f_acm.c"
+#define USBF_OBEX_INCLUDED
 #include "f_ecm.c"
 #include "f_obex.c"
-#include "f_serial.c"
 #include "f_phonet.c"
 #include "u_ether.c"
 
@@ -98,20 +96,40 @@ MODULE_AUTHOR("Felipe Balbi");
 MODULE_LICENSE("GPL");
 
 /*-------------------------------------------------------------------------*/
-
+static struct usb_function *f_acm_cfg1;
+static struct usb_function *f_acm_cfg2;
 static u8 hostaddr[ETH_ALEN];
+static struct eth_dev *the_dev;
 
 enum {
 	TTY_PORT_OBEX0,
 	TTY_PORT_OBEX1,
-	TTY_PORT_ACM,
 	TTY_PORTS_MAX,
 };
 
 static unsigned char tty_lines[TTY_PORTS_MAX];
 
+static struct usb_configuration nokia_config_500ma_driver = {
+	.label		= "Bus Powered",
+	.bConfigurationValue = 1,
+	/* .iConfiguration = DYNAMIC */
+	.bmAttributes	= USB_CONFIG_ATT_ONE,
+	.MaxPower	= 500,
+};
+
+static struct usb_configuration nokia_config_100ma_driver = {
+	.label		= "Self Powered",
+	.bConfigurationValue = 2,
+	/* .iConfiguration = DYNAMIC */
+	.bmAttributes	= USB_CONFIG_ATT_ONE | USB_CONFIG_ATT_SELFPOWER,
+	.MaxPower	= 100,
+};
+
+static struct usb_function_instance *fi_acm;
+
 static int __init nokia_bind_config(struct usb_configuration *c)
 {
+	struct usb_function *f_acm;
 	int status = 0;
 
 	status = phonet_bind_config(c);
@@ -126,33 +144,32 @@ static int __init nokia_bind_config(struct usb_configuration *c)
 	if (status)
 		printk(KERN_DEBUG "could not bind obex config %d\n", 0);
 
-	status = acm_bind_config(c, tty_lines[TTY_PORT_ACM]);
-	if (status)
-		printk(KERN_DEBUG "could not bind acm config\n");
+	f_acm = usb_get_function(fi_acm);
+	if (IS_ERR(f_acm))
+		return PTR_ERR(f_acm);
 
-	status = ecm_bind_config(c, hostaddr);
+	status = usb_add_function(c, f_acm);
 	if (status)
-		printk(KERN_DEBUG "could not bind ecm config\n");
+		goto err_conf;
+
+	status = ecm_bind_config(c, hostaddr, the_dev);
+	if (status) {
+		pr_debug("could not bind ecm config %d\n", status);
+		goto err_ecm;
+	}
+	if (c == &nokia_config_500ma_driver)
+		f_acm_cfg1 = f_acm;
+	else
+		f_acm_cfg2 = f_acm;
 
 	return status;
+err_ecm:
+	usb_remove_function(c, f_acm);
+err_conf:
+	usb_put_function(f_acm);
+	return status;
 }
 
-static struct usb_configuration nokia_config_500ma_driver = {
-	.label		= "Bus Powered",
-	.bConfigurationValue = 1,
-	/* .iConfiguration = DYNAMIC */
-	.bmAttributes	= USB_CONFIG_ATT_ONE,
-	.MaxPower	= 500,
-};
-
-static struct usb_configuration nokia_config_100ma_driver = {
-	.label		= "Self Powered",
-	.bConfigurationValue = 2,
-	/* .iConfiguration = DYNAMIC */
-	.bmAttributes	= USB_CONFIG_ATT_ONE | USB_CONFIG_ATT_SELFPOWER,
-	.MaxPower	= 100,
-};
-
 static int __init nokia_bind(struct usb_composite_dev *cdev)
 {
 	struct usb_gadget	*gadget = cdev->gadget;
@@ -169,9 +186,11 @@ static int __init nokia_bind(struct usb_composite_dev *cdev)
 			goto err_ether;
 	}
 
-	status = gether_setup(cdev->gadget, hostaddr);
-	if (status < 0)
+	the_dev = gether_setup(cdev->gadget, hostaddr);
+	if (IS_ERR(the_dev)) {
+		status = PTR_ERR(the_dev);
 		goto err_ether;
+	}
 
 	status = usb_string_ids_tab(cdev, strings_dev);
 	if (status < 0)
@@ -185,24 +204,32 @@ static int __init nokia_bind(struct usb_composite_dev *cdev)
 	if (!gadget_supports_altsettings(gadget))
 		goto err_usb;
 
+	fi_acm = usb_get_function_instance("acm");
+	if (IS_ERR(fi_acm))
+		goto err_usb;
+
 	/* finally register the configuration */
 	status = usb_add_config(cdev, &nokia_config_500ma_driver,
 			nokia_bind_config);
 	if (status < 0)
-		goto err_usb;
+		goto err_acm_inst;
 
 	status = usb_add_config(cdev, &nokia_config_100ma_driver,
 			nokia_bind_config);
 	if (status < 0)
-		goto err_usb;
+		goto err_put_cfg1;
 
 	usb_composite_overwrite_options(cdev, &coverwrite);
 	dev_info(&gadget->dev, "%s\n", NOKIA_LONG_NAME);
 
 	return 0;
 
+err_put_cfg1:
+	usb_put_function(f_acm_cfg1);
+err_acm_inst:
+	usb_put_function_instance(fi_acm);
 err_usb:
-	gether_cleanup();
+	gether_cleanup(the_dev);
 err_ether:
 	cur_line--;
 	while (cur_line >= 0)
@@ -217,12 +244,15 @@ static int __exit nokia_unbind(struct usb_composite_dev *cdev)
 {
 	int i;
 
+	usb_put_function(f_acm_cfg1);
+	usb_put_function(f_acm_cfg2);
+	usb_put_function_instance(fi_acm);
 	gphonet_cleanup();
 
 	for (i = 0; i < TTY_PORTS_MAX; i++)
 		gserial_free_line(tty_lines[i]);
 
-	gether_cleanup();
+	gether_cleanup(the_dev);
 
 	return 0;
 }
@@ -247,4 +277,3 @@ static void __exit nokia_cleanup(void)
 	usb_composite_unregister(&nokia_driver);
 }
 module_exit(nokia_cleanup);
-
diff --git a/drivers/usb/gadget/omap_udc.c b/drivers/usb/gadget/omap_udc.c
index f8445653577..b8ed74a823c 100644
--- a/drivers/usb/gadget/omap_udc.c
+++ b/drivers/usb/gadget/omap_udc.c
@@ -2067,7 +2067,6 @@ static int omap_udc_start(struct usb_gadget *g,
 	/* hook up the driver */
 	driver->driver.bus = NULL;
 	udc->driver = driver;
-	udc->gadget.dev.driver = &driver->driver;
 	spin_unlock_irqrestore(&udc->lock, flags);
 
 	if (udc->dc_clk != NULL)
@@ -2083,7 +2082,6 @@ static int omap_udc_start(struct usb_gadget *g,
 			ERR("can't bind to transceiver\n");
 			if (driver->unbind) {
 				driver->unbind(&udc->gadget);
-				udc->gadget.dev.driver = NULL;
 				udc->driver = NULL;
 			}
 			goto done;
@@ -2129,7 +2127,6 @@ static int omap_udc_stop(struct usb_gadget *g,
 	udc_quiesce(udc);
 	spin_unlock_irqrestore(&udc->lock, flags);
 
-	udc->gadget.dev.driver = NULL;
 	udc->driver = NULL;
 
 	if (udc->dc_clk != NULL)
@@ -2631,14 +2628,6 @@ omap_udc_setup(struct platform_device *odev, struct usb_phy *xceiv)
 	udc->gadget.speed = USB_SPEED_UNKNOWN;
 	udc->gadget.max_speed = USB_SPEED_FULL;
 	udc->gadget.name = driver_name;
-
-	device_initialize(&udc->gadget.dev);
-	dev_set_name(&udc->gadget.dev, "gadget");
-	udc->gadget.dev.release = omap_udc_release;
-	udc->gadget.dev.parent = &odev->dev;
-	if (use_dma)
-		udc->gadget.dev.dma_mask = odev->dev.dma_mask;
-
 	udc->transceiver = xceiv;
 
 	/* ep0 is special; put it right after the SETUP buffer */
@@ -2912,14 +2901,13 @@ bad_on_1710:
 	}
 
 	create_proc_file();
-	status = device_add(&udc->gadget.dev);
+	status = usb_add_gadget_udc_release(&pdev->dev, &udc->gadget,
+			omap_udc_release);
 	if (status)
 		goto cleanup4;
 
-	status = usb_add_gadget_udc(&pdev->dev, &udc->gadget);
-	if (!status)
-		return status;
-	/* If fail, fall through */
+	return 0;
+
 cleanup4:
 	remove_proc_file();
 
@@ -2990,7 +2978,6 @@ static int omap_udc_remove(struct platform_device *pdev)
 	release_mem_region(pdev->resource[0].start,
 			pdev->resource[0].end - pdev->resource[0].start + 1);
 
-	device_unregister(&udc->gadget.dev);
 	wait_for_completion(&done);
 
 	return 0;
diff --git a/drivers/usb/gadget/pch_udc.c b/drivers/usb/gadget/pch_udc.c
index a787a8ef672..24174e1d156 100644
--- a/drivers/usb/gadget/pch_udc.c
+++ b/drivers/usb/gadget/pch_udc.c
@@ -358,7 +358,6 @@ struct pch_udc_dev {
 			prot_stall:1,
 			irq_registered:1,
 			mem_region:1,
-			registered:1,
 			suspended:1,
 			connected:1,
 			vbus_session:1,
@@ -1441,6 +1440,8 @@ static void pch_vbus_gpio_free(struct pch_udc_dev *dev)
  */
 static void complete_req(struct pch_udc_ep *ep, struct pch_udc_request *req,
 								 int status)
+	__releases(&dev->lock)
+	__acquires(&dev->lock)
 {
 	struct pch_udc_dev	*dev;
 	unsigned halted = ep->halted;
@@ -2382,6 +2383,8 @@ static void pch_udc_svc_control_in(struct pch_udc_dev *dev)
  * @dev:	Reference to the device structure
  */
 static void pch_udc_svc_control_out(struct pch_udc_dev *dev)
+	__releases(&dev->lock)
+	__acquires(&dev->lock)
 {
 	u32	stat;
 	int setup_supported;
@@ -2989,7 +2992,6 @@ static int pch_udc_start(struct usb_gadget *g,
 
 	driver->driver.bus = NULL;
 	dev->driver = driver;
-	dev->gadget.dev.driver = &driver->driver;
 
 	/* get ready for ep0 traffic */
 	pch_udc_setup_ep0(dev);
@@ -3010,7 +3012,6 @@ static int pch_udc_stop(struct usb_gadget *g,
 	pch_udc_disable_interrupts(dev, UDC_DEVINT_MSK);
 
 	/* Assures that there are no pending requests with this driver */
-	dev->gadget.dev.driver = NULL;
 	dev->driver = NULL;
 	dev->connected = 0;
 
@@ -3078,8 +3079,6 @@ static void pch_udc_remove(struct pci_dev *pdev)
 				   pci_resource_len(pdev, PCH_UDC_PCI_BAR));
 	if (dev->active)
 		pci_disable_device(pdev);
-	if (dev->registered)
-		device_unregister(&dev->gadget.dev);
 	kfree(dev);
 	pci_set_drvdata(pdev, NULL);
 }
@@ -3196,21 +3195,13 @@ static int pch_udc_probe(struct pci_dev *pdev,
 	if (retval)
 		goto finished;
 
-	dev_set_name(&dev->gadget.dev, "gadget");
-	dev->gadget.dev.parent = &pdev->dev;
-	dev->gadget.dev.dma_mask = pdev->dev.dma_mask;
-	dev->gadget.dev.release = gadget_release;
 	dev->gadget.name = KBUILD_MODNAME;
 	dev->gadget.max_speed = USB_SPEED_HIGH;
 
-	retval = device_register(&dev->gadget.dev);
-	if (retval)
-		goto finished;
-	dev->registered = 1;
-
 	/* Put the device in disconnected state till a driver is bound */
 	pch_udc_set_disconnect(dev);
-	retval = usb_add_gadget_udc(&pdev->dev, &dev->gadget);
+	retval = usb_add_gadget_udc_release(&pdev->dev, &dev->gadget,
+			gadget_release);
 	if (retval)
 		goto finished;
 	return 0;
diff --git a/drivers/usb/gadget/pxa25x_udc.c b/drivers/usb/gadget/pxa25x_udc.c
index d0f37484b6b..ef47495dec8 100644
--- a/drivers/usb/gadget/pxa25x_udc.c
+++ b/drivers/usb/gadget/pxa25x_udc.c
@@ -1263,7 +1263,6 @@ static int pxa25x_udc_start(struct usb_gadget *g,
 
 	/* first hook up the driver ... */
 	dev->driver = driver;
-	dev->gadget.dev.driver = &driver->driver;
 	dev->pullup = 1;
 
 	/* ... then enable host detection and ep0; and we're ready
@@ -1325,7 +1324,6 @@ static int pxa25x_udc_stop(struct usb_gadget*g,
 	if (!IS_ERR_OR_NULL(dev->transceiver))
 		(void) otg_set_peripheral(dev->transceiver->otg, NULL);
 
-	dev->gadget.dev.driver = NULL;
 	dev->driver = NULL;
 
 	dump_state(dev);
@@ -2138,17 +2136,6 @@ static int __init pxa25x_udc_probe(struct platform_device *pdev)
 	dev->timer.function = udc_watchdog;
 	dev->timer.data = (unsigned long) dev;
 
-	device_initialize(&dev->gadget.dev);
-	dev->gadget.dev.parent = &pdev->dev;
-	dev->gadget.dev.dma_mask = pdev->dev.dma_mask;
-
-	retval = device_add(&dev->gadget.dev);
-	if (retval) {
-		dev->driver = NULL;
-		dev->gadget.dev.driver = NULL;
-		goto err_device_add;
-	}
-
 	the_controller = dev;
 	platform_set_drvdata(pdev, dev);
 
@@ -2199,8 +2186,6 @@ lubbock_fail0:
 	free_irq(irq, dev);
 #endif
  err_irq1:
-	device_unregister(&dev->gadget.dev);
- err_device_add:
 	if (gpio_is_valid(dev->mach->gpio_pullup))
 		gpio_free(dev->mach->gpio_pullup);
  err_gpio_pullup:
@@ -2226,7 +2211,6 @@ static int __exit pxa25x_udc_remove(struct platform_device *pdev)
 		return -EBUSY;
 
 	usb_del_gadget_udc(&dev->gadget);
-	device_unregister(&dev->gadget.dev);
 	dev->pullup = 0;
 	pullup(dev);
 
diff --git a/drivers/usb/gadget/pxa27x_udc.c b/drivers/usb/gadget/pxa27x_udc.c
index 2fc867652ef..6b4c7d95853 100644
--- a/drivers/usb/gadget/pxa27x_udc.c
+++ b/drivers/usb/gadget/pxa27x_udc.c
@@ -24,14 +24,12 @@
 #include <linux/gpio.h>
 #include <linux/slab.h>
 #include <linux/prefetch.h>
-
-#include <asm/byteorder.h>
-#include <mach/hardware.h>
+#include <linux/byteorder/generic.h>
+#include <linux/platform_data/pxa2xx_udc.h>
 
 #include <linux/usb.h>
 #include <linux/usb/ch9.h>
 #include <linux/usb/gadget.h>
-#include <mach/udc.h>
 
 #include "pxa27x_udc.h"
 
@@ -611,7 +609,7 @@ static void inc_ep_stats_bytes(struct pxa_ep *ep, int count, int is_in)
  *
  * Find the physical pxa27x ep, and setup its UDCCR
  */
-static __init void pxa_ep_setup(struct pxa_ep *ep)
+static void pxa_ep_setup(struct pxa_ep *ep)
 {
 	u32 new_udccr;
 
@@ -633,7 +631,7 @@ static __init void pxa_ep_setup(struct pxa_ep *ep)
  *
  * Setup all pxa physical endpoints, except ep0
  */
-static __init void pxa_eps_setup(struct pxa_udc *dev)
+static void pxa_eps_setup(struct pxa_udc *dev)
 {
 	unsigned int i;
 
@@ -1718,7 +1716,7 @@ static void udc_disable(struct pxa_udc *udc)
  * Initializes gadget endpoint list, endpoints locks. No action is taken
  * on the hardware.
  */
-static __init void udc_init_data(struct pxa_udc *dev)
+static void udc_init_data(struct pxa_udc *dev)
 {
 	int i;
 	struct pxa_ep *ep;
@@ -1811,7 +1809,6 @@ static int pxa27x_udc_start(struct usb_gadget *g,
 
 	/* first hook up the driver ... */
 	udc->driver = driver;
-	udc->gadget.dev.driver = &driver->driver;
 	dplus_pullup(udc, 1);
 
 	if (!IS_ERR_OR_NULL(udc->transceiver)) {
@@ -1829,7 +1826,6 @@ static int pxa27x_udc_start(struct usb_gadget *g,
 
 fail:
 	udc->driver = NULL;
-	udc->gadget.dev.driver = NULL;
 	return retval;
 }
 
@@ -1871,7 +1867,6 @@ static int pxa27x_udc_stop(struct usb_gadget *g,
 
 	udc->driver = NULL;
 
-
 	if (!IS_ERR_OR_NULL(udc->transceiver))
 		return otg_set_peripheral(udc->transceiver->otg, NULL);
 	return 0;
@@ -2413,7 +2408,7 @@ static struct pxa_udc memory = {
  * Perform basic init : allocates udc clock, creates sysfs files, requests
  * irq.
  */
-static int __init pxa_udc_probe(struct platform_device *pdev)
+static int pxa_udc_probe(struct platform_device *pdev)
 {
 	struct resource *regs;
 	struct pxa_udc *udc = &memory;
@@ -2456,9 +2451,6 @@ static int __init pxa_udc_probe(struct platform_device *pdev)
 		goto err_map;
 	}
 
-	device_initialize(&udc->gadget.dev);
-	udc->gadget.dev.parent = &pdev->dev;
-	udc->gadget.dev.dma_mask = NULL;
 	udc->vbus_sensed = 0;
 
 	the_controller = udc;
@@ -2475,12 +2467,6 @@ static int __init pxa_udc_probe(struct platform_device *pdev)
 		goto err_irq;
 	}
 
-	retval = device_add(&udc->gadget.dev);
-	if (retval) {
-		dev_err(udc->dev, "device_add error %d\n", retval);
-		goto err_dev_add;
-	}
-
 	retval = usb_add_gadget_udc(&pdev->dev, &udc->gadget);
 	if (retval)
 		goto err_add_udc;
@@ -2490,8 +2476,6 @@ static int __init pxa_udc_probe(struct platform_device *pdev)
 	return 0;
 
 err_add_udc:
-	device_unregister(&udc->gadget.dev);
-err_dev_add:
 	free_irq(udc->irq, udc);
 err_irq:
 	iounmap(udc->regs);
@@ -2506,13 +2490,12 @@ err_clk:
  * pxa_udc_remove - removes the udc device driver
  * @_dev: platform device
  */
-static int __exit pxa_udc_remove(struct platform_device *_dev)
+static int pxa_udc_remove(struct platform_device *_dev)
 {
 	struct pxa_udc *udc = platform_get_drvdata(_dev);
 	int gpio = udc->mach->gpio_pullup;
 
 	usb_del_gadget_udc(&udc->gadget);
-	device_del(&udc->gadget.dev);
 	usb_gadget_unregister_driver(udc->driver);
 	free_irq(udc->irq, udc);
 	pxa_cleanup_debugfs(udc);
@@ -2625,7 +2608,8 @@ static struct platform_driver udc_driver = {
 		.name	= "pxa27x-udc",
 		.owner	= THIS_MODULE,
 	},
-	.remove		= __exit_p(pxa_udc_remove),
+	.probe		= pxa_udc_probe,
+	.remove		= pxa_udc_remove,
 	.shutdown	= pxa_udc_shutdown,
 #ifdef CONFIG_PM
 	.suspend	= pxa_udc_suspend,
@@ -2633,22 +2617,7 @@ static struct platform_driver udc_driver = {
 #endif
 };
 
-static int __init udc_init(void)
-{
-	if (!cpu_is_pxa27x() && !cpu_is_pxa3xx())
-		return -ENODEV;
-
-	printk(KERN_INFO "%s: version %s\n", driver_name, DRIVER_VERSION);
-	return platform_driver_probe(&udc_driver, pxa_udc_probe);
-}
-module_init(udc_init);
-
-
-static void __exit udc_exit(void)
-{
-	platform_driver_unregister(&udc_driver);
-}
-module_exit(udc_exit);
+module_platform_driver(udc_driver);
 
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_AUTHOR("Robert Jarzmik");
diff --git a/drivers/usb/gadget/r8a66597-udc.c b/drivers/usb/gadget/r8a66597-udc.c
index f46a1b77ce3..0b742d17184 100644
--- a/drivers/usb/gadget/r8a66597-udc.c
+++ b/drivers/usb/gadget/r8a66597-udc.c
@@ -1837,7 +1837,6 @@ static int __exit r8a66597_remove(struct platform_device *pdev)
 		clk_put(r8a66597->clk);
 	}
 
-	device_unregister(&r8a66597->gadget.dev);
 	kfree(r8a66597);
 	return 0;
 }
@@ -1915,17 +1914,8 @@ static int __init r8a66597_probe(struct platform_device *pdev)
 	r8a66597->irq_sense_low = irq_trigger == IRQF_TRIGGER_LOW;
 
 	r8a66597->gadget.ops = &r8a66597_gadget_ops;
-	dev_set_name(&r8a66597->gadget.dev, "gadget");
 	r8a66597->gadget.max_speed = USB_SPEED_HIGH;
-	r8a66597->gadget.dev.parent = &pdev->dev;
-	r8a66597->gadget.dev.dma_mask = pdev->dev.dma_mask;
-	r8a66597->gadget.dev.release = pdev->dev.release;
 	r8a66597->gadget.name = udc_name;
-	ret = device_register(&r8a66597->gadget.dev);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "device_register failed\n");
-		goto clean_up;
-	}
 
 	init_timer(&r8a66597->timer);
 	r8a66597->timer.function = r8a66597_timer;
@@ -1939,7 +1929,7 @@ static int __init r8a66597_probe(struct platform_device *pdev)
 			dev_err(&pdev->dev, "cannot get clock \"%s\"\n",
 				clk_name);
 			ret = PTR_ERR(r8a66597->clk);
-			goto clean_up_dev;
+			goto clean_up;
 		}
 		clk_enable(r8a66597->clk);
 	}
@@ -2007,8 +1997,6 @@ clean_up2:
 		clk_disable(r8a66597->clk);
 		clk_put(r8a66597->clk);
 	}
-clean_up_dev:
-	device_unregister(&r8a66597->gadget.dev);
 clean_up:
 	if (r8a66597) {
 		if (r8a66597->sudmac_reg)
diff --git a/drivers/usb/gadget/s3c-hsotg.c b/drivers/usb/gadget/s3c-hsotg.c
index c26564f29a2..a3cdc32115d 100644
--- a/drivers/usb/gadget/s3c-hsotg.c
+++ b/drivers/usb/gadget/s3c-hsotg.c
@@ -39,8 +39,6 @@
 
 #include "s3c-hsotg.h"
 
-#define DMA_ADDR_INVALID (~((dma_addr_t)0))
-
 static const char * const s3c_hsotg_supply_names[] = {
 	"vusb_d",		/* digital USB supply, 1.2V */
 	"vusb_a",		/* analog USB supply, 1.1V */
@@ -405,7 +403,6 @@ static struct usb_request *s3c_hsotg_ep_alloc_request(struct usb_ep *ep,
 
 	INIT_LIST_HEAD(&req->queue);
 
-	req->req.dma = DMA_ADDR_INVALID;
 	return &req->req;
 }
 
@@ -435,24 +432,12 @@ static void s3c_hsotg_unmap_dma(struct s3c_hsotg *hsotg,
 				struct s3c_hsotg_req *hs_req)
 {
 	struct usb_request *req = &hs_req->req;
-	enum dma_data_direction dir;
-
-	dir = hs_ep->dir_in ? DMA_TO_DEVICE : DMA_FROM_DEVICE;
 
 	/* ignore this if we're not moving any data */
 	if (hs_req->req.length == 0)
 		return;
 
-	if (hs_req->mapped) {
-		/* we mapped this, so unmap and remove the dma */
-
-		dma_unmap_single(hsotg->dev, req->dma, req->length, dir);
-
-		req->dma = DMA_ADDR_INVALID;
-		hs_req->mapped = 0;
-	} else {
-		dma_sync_single_for_cpu(hsotg->dev, req->dma, req->length, dir);
-	}
+	usb_gadget_unmap_request(&hsotg->gadget, hs_req, hs_ep->dir_in);
 }
 
 /**
@@ -852,37 +837,16 @@ static int s3c_hsotg_map_dma(struct s3c_hsotg *hsotg,
 			     struct s3c_hsotg_ep *hs_ep,
 			     struct usb_request *req)
 {
-	enum dma_data_direction dir;
 	struct s3c_hsotg_req *hs_req = our_req(req);
-
-	dir = hs_ep->dir_in ? DMA_TO_DEVICE : DMA_FROM_DEVICE;
+	int ret;
 
 	/* if the length is zero, ignore the DMA data */
 	if (hs_req->req.length == 0)
 		return 0;
 
-	if (req->dma == DMA_ADDR_INVALID) {
-		dma_addr_t dma;
-
-		dma = dma_map_single(hsotg->dev, req->buf, req->length, dir);
-
-		if (unlikely(dma_mapping_error(hsotg->dev, dma)))
-			goto dma_error;
-
-		if (dma & 3) {
-			dev_err(hsotg->dev, "%s: unaligned dma buffer\n",
-				__func__);
-
-			dma_unmap_single(hsotg->dev, dma, req->length, dir);
-			return -EINVAL;
-		}
-
-		hs_req->mapped = 1;
-		req->dma = dma;
-	} else {
-		dma_sync_single_for_cpu(hsotg->dev, req->dma, req->length, dir);
-		hs_req->mapped = 0;
-	}
+	ret = usb_gadget_map_request(&hsotg->gadget, req, hs_ep->dir_in);
+	if (ret)
+		goto dma_error;
 
 	return 0;
 
@@ -2961,9 +2925,7 @@ static int s3c_hsotg_udc_start(struct usb_gadget *gadget,
 
 	driver->driver.bus = NULL;
 	hsotg->driver = driver;
-	hsotg->gadget.dev.driver = &driver->driver;
 	hsotg->gadget.dev.of_node = hsotg->dev->of_node;
-	hsotg->gadget.dev.dma_mask = hsotg->dev->dma_mask;
 	hsotg->gadget.speed = USB_SPEED_UNKNOWN;
 
 	ret = regulator_bulk_enable(ARRAY_SIZE(hsotg->supplies),
@@ -2979,7 +2941,6 @@ static int s3c_hsotg_udc_start(struct usb_gadget *gadget,
 
 err:
 	hsotg->driver = NULL;
-	hsotg->gadget.dev.driver = NULL;
 	return ret;
 }
 
@@ -3014,7 +2975,6 @@ static int s3c_hsotg_udc_stop(struct usb_gadget *gadget,
 
 	hsotg->driver = NULL;
 	hsotg->gadget.speed = USB_SPEED_UNKNOWN;
-	hsotg->gadget.dev.driver = NULL;
 
 	spin_unlock_irqrestore(&hsotg->lock, flags);
 
@@ -3484,16 +3444,6 @@ static void s3c_hsotg_delete_debug(struct s3c_hsotg *hsotg)
 }
 
 /**
- * s3c_hsotg_release - release callback for hsotg device
- * @dev: Device to for which release is called
- *
- * Nothing to do as the resource is allocated using devm_ API.
- */
-static void s3c_hsotg_release(struct device *dev)
-{
-}
-
-/**
  * s3c_hsotg_probe - probe function for hsotg driver
  * @pdev: The platform information for the driver
  */
@@ -3517,7 +3467,7 @@ static int s3c_hsotg_probe(struct platform_device *pdev)
 	}
 
 	phy = devm_usb_get_phy(dev, USB_PHY_TYPE_USB2);
-	if (IS_ERR_OR_NULL(phy)) {
+	if (IS_ERR(phy)) {
 		/* Fallback for pdata */
 		plat = pdev->dev.platform_data;
 		if (!plat) {
@@ -3567,18 +3517,10 @@ static int s3c_hsotg_probe(struct platform_device *pdev)
 
 	dev_info(dev, "regs %p, irq %d\n", hsotg->regs, hsotg->irq);
 
-	device_initialize(&hsotg->gadget.dev);
-
-	dev_set_name(&hsotg->gadget.dev, "gadget");
-
 	hsotg->gadget.max_speed = USB_SPEED_HIGH;
 	hsotg->gadget.ops = &s3c_hsotg_gadget_ops;
 	hsotg->gadget.name = dev_name(dev);
 
-	hsotg->gadget.dev.parent = dev;
-	hsotg->gadget.dev.dma_mask = dev->dma_mask;
-	hsotg->gadget.dev.release = s3c_hsotg_release;
-
 	/* reset the system */
 
 	clk_prepare_enable(hsotg->clk);
@@ -3658,12 +3600,6 @@ static int s3c_hsotg_probe(struct platform_device *pdev)
 
 	s3c_hsotg_phy_disable(hsotg);
 
-	ret = device_add(&hsotg->gadget.dev);
-	if (ret) {
-		put_device(&hsotg->gadget.dev);
-		goto err_ep_mem;
-	}
-
 	ret = usb_add_gadget_udc(&pdev->dev, &hsotg->gadget);
 	if (ret)
 		goto err_ep_mem;
@@ -3702,10 +3638,8 @@ static int s3c_hsotg_remove(struct platform_device *pdev)
 	}
 
 	s3c_hsotg_phy_disable(hsotg);
-
 	clk_disable_unprepare(hsotg->clk);
 
-	device_unregister(&hsotg->gadget.dev);
 	return 0;
 }
 
diff --git a/drivers/usb/gadget/s3c-hsudc.c b/drivers/usb/gadget/s3c-hsudc.c
index 458965a1b13..b1f0771fbd3 100644
--- a/drivers/usb/gadget/s3c-hsudc.c
+++ b/drivers/usb/gadget/s3c-hsudc.c
@@ -283,7 +283,6 @@ static void s3c_hsudc_nuke_ep(struct s3c_hsudc_ep *hsep, int status)
 /**
  * s3c_hsudc_stop_activity - Stop activity on all endpoints.
  * @hsudc: Device controller for which EP activity is to be stopped.
- * @driver: Reference to the gadget driver which is currently active.
  *
  * All the endpoints are stopped and any pending transfer requests if any on
  * the endpoint are terminated.
@@ -1154,7 +1153,6 @@ static int s3c_hsudc_start(struct usb_gadget *gadget,
 		return -EBUSY;
 
 	hsudc->driver = driver;
-	hsudc->gadget.dev.driver = &driver->driver;
 
 	ret = regulator_bulk_enable(ARRAY_SIZE(hsudc->supplies),
 				    hsudc->supplies);
@@ -1190,7 +1188,6 @@ err_otg:
 	regulator_bulk_disable(ARRAY_SIZE(hsudc->supplies), hsudc->supplies);
 err_supplies:
 	hsudc->driver = NULL;
-	hsudc->gadget.dev.driver = NULL;
 	return ret;
 }
 
@@ -1208,7 +1205,6 @@ static int s3c_hsudc_stop(struct usb_gadget *gadget,
 
 	spin_lock_irqsave(&hsudc->lock, flags);
 	hsudc->driver = NULL;
-	hsudc->gadget.dev.driver = NULL;
 	hsudc->gadget.speed = USB_SPEED_UNKNOWN;
 	s3c_hsudc_uninit_phy();
 
@@ -1303,15 +1299,10 @@ static int s3c_hsudc_probe(struct platform_device *pdev)
 
 	spin_lock_init(&hsudc->lock);
 
-	dev_set_name(&hsudc->gadget.dev, "gadget");
-
 	hsudc->gadget.max_speed = USB_SPEED_HIGH;
 	hsudc->gadget.ops = &s3c_hsudc_gadget_ops;
 	hsudc->gadget.name = dev_name(dev);
-	hsudc->gadget.dev.parent = dev;
-	hsudc->gadget.dev.dma_mask = dev->dma_mask;
 	hsudc->gadget.ep0 = &hsudc->ep[0].ep;
-
 	hsudc->gadget.is_otg = 0;
 	hsudc->gadget.is_a_peripheral = 0;
 	hsudc->gadget.speed = USB_SPEED_UNKNOWN;
@@ -1345,12 +1336,6 @@ static int s3c_hsudc_probe(struct platform_device *pdev)
 	disable_irq(hsudc->irq);
 	local_irq_enable();
 
-	ret = device_register(&hsudc->gadget.dev);
-	if (ret) {
-		put_device(&hsudc->gadget.dev);
-		goto err_add_device;
-	}
-
 	ret = usb_add_gadget_udc(&pdev->dev, &hsudc->gadget);
 	if (ret)
 		goto err_add_udc;
@@ -1359,7 +1344,6 @@ static int s3c_hsudc_probe(struct platform_device *pdev)
 
 	return 0;
 err_add_udc:
-	device_unregister(&hsudc->gadget.dev);
 err_add_device:
 	clk_disable(hsudc->uclk);
 err_res:
diff --git a/drivers/usb/gadget/s3c2410_udc.c b/drivers/usb/gadget/s3c2410_udc.c
index 08f89652533..d0e75e1b3cc 100644
--- a/drivers/usb/gadget/s3c2410_udc.c
+++ b/drivers/usb/gadget/s3c2410_udc.c
@@ -1674,7 +1674,6 @@ static int s3c2410_udc_start(struct usb_gadget *g,
 
 	/* Hook the driver */
 	udc->driver = driver;
-	udc->gadget.dev.driver = &driver->driver;
 
 	/* Enable udc */
 	s3c2410_udc_enable(udc);
@@ -1824,17 +1823,6 @@ static int s3c2410_udc_probe(struct platform_device *pdev)
 		goto err_mem;
 	}
 
-	device_initialize(&udc->gadget.dev);
-	udc->gadget.dev.parent = &pdev->dev;
-	udc->gadget.dev.dma_mask = pdev->dev.dma_mask;
-
-	/* Bind the driver */
-	retval = device_add(&udc->gadget.dev);
-	if (retval) {
-		dev_err(&udc->gadget.dev, "Error in device_add() : %d\n", retval);
-		goto err_device_add;
-	}
-
 	the_controller = udc;
 	platform_set_drvdata(pdev, udc);
 
@@ -1923,8 +1911,6 @@ err_gpio_claim:
 err_int:
 	free_irq(IRQ_USBD, udc);
 err_map:
-	device_unregister(&udc->gadget.dev);
-err_device_add:
 	iounmap(base_addr);
 err_mem:
 	release_mem_region(rsrc_start, rsrc_len);
@@ -1946,7 +1932,6 @@ static int s3c2410_udc_remove(struct platform_device *pdev)
 		return -EBUSY;
 
 	usb_del_gadget_udc(&udc->gadget);
-	device_unregister(&udc->gadget.dev);
 	debugfs_remove(udc->regs_info);
 
 	if (udc_info && !udc_info->udc_command &&
diff --git a/drivers/usb/gadget/serial.c b/drivers/usb/gadget/serial.c
index 68d7bb06ebc..1f5f978d35d 100644
--- a/drivers/usb/gadget/serial.c
+++ b/drivers/usb/gadget/serial.c
@@ -12,6 +12,7 @@
 
 #include <linux/kernel.h>
 #include <linux/device.h>
+#include <linux/module.h>
 #include <linux/tty.h>
 #include <linux/tty_flip.h>
 
@@ -28,18 +29,6 @@
 #define GS_VERSION_NAME			GS_LONG_NAME " " GS_VERSION_STR
 
 /*-------------------------------------------------------------------------*/
-
-/*
- * Kbuild is not very cooperative with respect to linking separately
- * compiled library objects into one module.  So for now we won't use
- * separate compilation ... ensuring init/exit sections work to shrink
- * the runtime footprint, and giving us at least some parts of what
- * a "gcc --combine ... part1.c part2.c part3.c ... " build would.
- */
-#include "f_obex.c"
-#include "f_serial.c"
-
-/*-------------------------------------------------------------------------*/
 USB_GADGET_COMPOSITE_OPTIONS();
 
 /* Thanks to NetChip Technologies for donating this product ID.
@@ -126,27 +115,6 @@ module_param(n_ports, uint, 0);
 MODULE_PARM_DESC(n_ports, "number of ports to create, default=1");
 
 /*-------------------------------------------------------------------------*/
-static unsigned char tty_lines[MAX_U_SERIAL_PORTS];
-
-static int __init serial_bind_obex_config(struct usb_configuration *c)
-{
-	unsigned i;
-	int status = 0;
-
-	for (i = 0; i < n_ports && status == 0; i++)
-		status = obex_bind_config(c, tty_lines[i]);
-	return status;
-}
-
-static int __init serial_bind_gser_config(struct usb_configuration *c)
-{
-	unsigned i;
-	int status = 0;
-
-	for (i = 0; i < n_ports && status == 0; i++)
-		status = gser_bind_config(c, tty_lines[i]);
-	return status;
-}
 
 static struct usb_configuration serial_config_driver = {
 	/* .label = f(use_acm) */
@@ -169,15 +137,12 @@ static int serial_register_ports(struct usb_composite_dev *cdev,
 		goto out;
 
 	for (i = 0; i < n_ports; i++) {
-		struct f_serial_opts *opts;
 
 		fi_serial[i] = usb_get_function_instance(f_name);
 		if (IS_ERR(fi_serial[i])) {
 			ret = PTR_ERR(fi_serial[i]);
 			goto fail;
 		}
-		opts = container_of(fi_serial[i], struct f_serial_opts, func_inst);
-		opts->port_num = tty_lines[i];
 
 		f_serial[i] = usb_get_function(fi_serial[i]);
 		if (IS_ERR(f_serial[i])) {
@@ -212,13 +177,6 @@ out:
 static int __init gs_bind(struct usb_composite_dev *cdev)
 {
 	int			status;
-	int			cur_line;
-
-	for (cur_line = 0; cur_line < n_ports; cur_line++) {
-		status = gserial_alloc_line(&tty_lines[cur_line]);
-		if (status)
-			goto fail;
-	}
 
 	/* Allocate string descriptor numbers ... note that string
 	 * contents can be overridden by the composite_dev glue.
@@ -243,11 +201,12 @@ static int __init gs_bind(struct usb_composite_dev *cdev)
 				"acm");
 		usb_ep_autoconfig_reset(cdev->gadget);
 	} else if (use_obex)
-		status = usb_add_config(cdev, &serial_config_driver,
-				serial_bind_obex_config);
-	else
-		status = usb_add_config(cdev, &serial_config_driver,
-				serial_bind_gser_config);
+		status = serial_register_ports(cdev, &serial_config_driver,
+				"obex");
+	else {
+		status = serial_register_ports(cdev, &serial_config_driver,
+				"gser");
+	}
 	if (status < 0)
 		goto fail;
 
@@ -257,9 +216,6 @@ static int __init gs_bind(struct usb_composite_dev *cdev)
 	return 0;
 
 fail:
-	cur_line--;
-	while (cur_line >= 0)
-		gserial_free_line(tty_lines[cur_line--]);
 	return status;
 }
 
@@ -270,7 +226,6 @@ static int gs_unbind(struct usb_composite_dev *cdev)
 	for (i = 0; i < n_ports; i++) {
 		usb_put_function(f_serial[i]);
 		usb_put_function_instance(fi_serial[i]);
-		gserial_free_line(tty_lines[i]);
 	}
 	return 0;
 }
diff --git a/drivers/usb/gadget/u_ether.c b/drivers/usb/gadget/u_ether.c
index a0aa721d8b2..4b76124ce96 100644
--- a/drivers/usb/gadget/u_ether.c
+++ b/drivers/usb/gadget/u_ether.c
@@ -50,7 +50,6 @@
 
 struct eth_dev {
 	/* lock is held while accessing port_usb
-	 * or updating its backlink port_usb->ioport
 	 */
 	spinlock_t		lock;
 	struct gether		*port_usb;
@@ -729,8 +728,6 @@ static int get_ether_addr(const char *str, u8 *dev_addr)
 	return 1;
 }
 
-static struct eth_dev *the_dev;
-
 static const struct net_device_ops eth_netdev_ops = {
 	.ndo_open		= eth_open,
 	.ndo_stop		= eth_stop,
@@ -758,19 +755,16 @@ static struct device_type gadget_type = {
  *
  * Returns negative errno, or zero on success
  */
-int gether_setup_name(struct usb_gadget *g, u8 ethaddr[ETH_ALEN],
+struct eth_dev *gether_setup_name(struct usb_gadget *g, u8 ethaddr[ETH_ALEN],
 		const char *netname)
 {
 	struct eth_dev		*dev;
 	struct net_device	*net;
 	int			status;
 
-	if (the_dev)
-		return -EBUSY;
-
 	net = alloc_etherdev(sizeof *dev);
 	if (!net)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	dev = netdev_priv(net);
 	spin_lock_init(&dev->lock);
@@ -807,12 +801,11 @@ int gether_setup_name(struct usb_gadget *g, u8 ethaddr[ETH_ALEN],
 	if (status < 0) {
 		dev_dbg(&g->dev, "register_netdev failed, %d\n", status);
 		free_netdev(net);
+		dev = ERR_PTR(status);
 	} else {
 		INFO(dev, "MAC %pM\n", net->dev_addr);
 		INFO(dev, "HOST MAC %pM\n", dev->host_mac);
 
-		the_dev = dev;
-
 		/* two kinds of host-initiated state changes:
 		 *  - iff DATA transfer is active, carrier is "on"
 		 *  - tx queueing enabled if open *and* carrier is "on"
@@ -820,7 +813,7 @@ int gether_setup_name(struct usb_gadget *g, u8 ethaddr[ETH_ALEN],
 		netif_carrier_off(net);
 	}
 
-	return status;
+	return dev;
 }
 
 /**
@@ -829,19 +822,16 @@ int gether_setup_name(struct usb_gadget *g, u8 ethaddr[ETH_ALEN],
  *
  * This is called to free all resources allocated by @gether_setup().
  */
-void gether_cleanup(void)
+void gether_cleanup(struct eth_dev *dev)
 {
-	if (!the_dev)
+	if (!dev)
 		return;
 
-	unregister_netdev(the_dev->net);
-	flush_work(&the_dev->work);
-	free_netdev(the_dev->net);
-
-	the_dev = NULL;
+	unregister_netdev(dev->net);
+	flush_work(&dev->work);
+	free_netdev(dev->net);
 }
 
-
 /**
  * gether_connect - notify network layer that USB link is active
  * @link: the USB link, set up with endpoints, descriptors matching
@@ -860,7 +850,7 @@ void gether_cleanup(void)
  */
 struct net_device *gether_connect(struct gether *link)
 {
-	struct eth_dev		*dev = the_dev;
+	struct eth_dev		*dev = link->ioport;
 	int			result = 0;
 
 	if (!dev)
@@ -895,7 +885,6 @@ struct net_device *gether_connect(struct gether *link)
 
 		spin_lock(&dev->lock);
 		dev->port_usb = link;
-		link->ioport = dev;
 		if (netif_running(dev->net)) {
 			if (link->open)
 				link->open(link);
@@ -989,6 +978,5 @@ void gether_disconnect(struct gether *link)
 
 	spin_lock(&dev->lock);
 	dev->port_usb = NULL;
-	link->ioport = NULL;
 	spin_unlock(&dev->lock);
 }
diff --git a/drivers/usb/gadget/u_ether.h b/drivers/usb/gadget/u_ether.h
index 6f4a1623d85..02522338a70 100644
--- a/drivers/usb/gadget/u_ether.h
+++ b/drivers/usb/gadget/u_ether.h
@@ -21,6 +21,7 @@
 
 #include "gadget_chips.h"
 
+struct eth_dev;
 
 /*
  * This represents the USB side of an "ethernet" link, managed by a USB
@@ -70,7 +71,7 @@ struct gether {
 			|USB_CDC_PACKET_TYPE_DIRECTED)
 
 /* variant of gether_setup that allows customizing network device name */
-int gether_setup_name(struct usb_gadget *g, u8 ethaddr[ETH_ALEN],
+struct eth_dev *gether_setup_name(struct usb_gadget *g, u8 ethaddr[ETH_ALEN],
 		const char *netname);
 
 /* netdev setup/teardown as directed by the gadget driver */
@@ -86,12 +87,13 @@ int gether_setup_name(struct usb_gadget *g, u8 ethaddr[ETH_ALEN],
  *
  * Returns negative errno, or zero on success
  */
-static inline int gether_setup(struct usb_gadget *g, u8 ethaddr[ETH_ALEN])
+static inline struct eth_dev *gether_setup(struct usb_gadget *g,
+		u8 ethaddr[ETH_ALEN])
 {
 	return gether_setup_name(g, ethaddr, "usb");
 }
 
-void gether_cleanup(void);
+void gether_cleanup(struct eth_dev *dev);
 
 /* connect/disconnect is handled by individual functions */
 struct net_device *gether_connect(struct gether *);
@@ -111,21 +113,24 @@ static inline bool can_support_ecm(struct usb_gadget *gadget)
 }
 
 /* each configuration may bind one instance of an ethernet link */
-int geth_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN]);
-int ecm_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN]);
-int ncm_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN]);
-int eem_bind_config(struct usb_configuration *c);
+int geth_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN],
+		struct eth_dev *dev);
+int ecm_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN],
+		struct eth_dev *dev);
+int ncm_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN],
+		struct eth_dev *dev);
+int eem_bind_config(struct usb_configuration *c, struct eth_dev *dev);
 
 #ifdef USB_ETH_RNDIS
 
 int rndis_bind_config_vendor(struct usb_configuration *c, u8 ethaddr[ETH_ALEN],
-				u32 vendorID, const char *manufacturer);
+		u32 vendorID, const char *manufacturer, struct eth_dev *dev);
 
 #else
 
 static inline int
 rndis_bind_config_vendor(struct usb_configuration *c, u8 ethaddr[ETH_ALEN],
-				u32 vendorID, const char *manufacturer)
+		u32 vendorID, const char *manufacturer, struct eth_dev *dev)
 {
 	return 0;
 }
@@ -145,9 +150,9 @@ rndis_bind_config_vendor(struct usb_configuration *c, u8 ethaddr[ETH_ALEN],
  * for calling @gether_cleanup() before module unload.
  */
 static inline int rndis_bind_config(struct usb_configuration *c,
-				    u8 ethaddr[ETH_ALEN])
+		u8 ethaddr[ETH_ALEN], struct eth_dev *dev)
 {
-	return rndis_bind_config_vendor(c, ethaddr, 0, NULL);
+	return rndis_bind_config_vendor(c, ethaddr, 0, NULL, dev);
 }
 
 
diff --git a/drivers/usb/gadget/u_serial.h b/drivers/usb/gadget/u_serial.h
index 66ce73a0050..c20210c0bab 100644
--- a/drivers/usb/gadget/u_serial.h
+++ b/drivers/usb/gadget/u_serial.h
@@ -65,7 +65,6 @@ int gserial_connect(struct gserial *, u8 port_num);
 void gserial_disconnect(struct gserial *);
 
 /* functions are bound to configurations by a config or gadget driver */
-int acm_bind_config(struct usb_configuration *c, u8 port_num);
 int gser_bind_config(struct usb_configuration *c, u8 port_num);
 int obex_bind_config(struct usb_configuration *c, u8 port_num);
 
diff --git a/drivers/usb/gadget/udc-core.c b/drivers/usb/gadget/udc-core.c
index f8f62c3ed65..ffd8fa54110 100644
--- a/drivers/usb/gadget/udc-core.c
+++ b/drivers/usb/gadget/udc-core.c
@@ -101,6 +101,16 @@ EXPORT_SYMBOL_GPL(usb_gadget_unmap_request);
 
 /* ------------------------------------------------------------------------- */
 
+void usb_gadget_set_state(struct usb_gadget *gadget,
+		enum usb_device_state state)
+{
+	gadget->state = state;
+	sysfs_notify(&gadget->dev.kobj, NULL, "status");
+}
+EXPORT_SYMBOL_GPL(usb_gadget_set_state);
+
+/* ------------------------------------------------------------------------- */
+
 /**
  * usb_gadget_udc_start - tells usb device controller to start up
  * @gadget: The gadget we want to get started
@@ -156,15 +166,23 @@ static void usb_udc_release(struct device *dev)
 }
 
 static const struct attribute_group *usb_udc_attr_groups[];
+
+static void usb_udc_nop_release(struct device *dev)
+{
+	dev_vdbg(dev, "%s\n", __func__);
+}
+
 /**
- * usb_add_gadget_udc - adds a new gadget to the udc class driver list
- * @parent: the parent device to this udc. Usually the controller
- * driver's device.
- * @gadget: the gadget to be added to the list
+ * usb_add_gadget_udc_release - adds a new gadget to the udc class driver list
+ * @parent: the parent device to this udc. Usually the controller driver's
+ * device.
+ * @gadget: the gadget to be added to the list.
+ * @release: a gadget release function.
  *
  * Returns zero on success, negative errno otherwise.
  */
-int usb_add_gadget_udc(struct device *parent, struct usb_gadget *gadget)
+int usb_add_gadget_udc_release(struct device *parent, struct usb_gadget *gadget,
+		void (*release)(struct device *dev))
 {
 	struct usb_udc		*udc;
 	int			ret = -ENOMEM;
@@ -173,6 +191,22 @@ int usb_add_gadget_udc(struct device *parent, struct usb_gadget *gadget)
 	if (!udc)
 		goto err1;
 
+	dev_set_name(&gadget->dev, "gadget");
+	gadget->dev.parent = parent;
+
+	dma_set_coherent_mask(&gadget->dev, parent->coherent_dma_mask);
+	gadget->dev.dma_parms = parent->dma_parms;
+	gadget->dev.dma_mask = parent->dma_mask;
+
+	if (release)
+		gadget->dev.release = release;
+	else
+		gadget->dev.release = usb_udc_nop_release;
+
+	ret = device_register(&gadget->dev);
+	if (ret)
+		goto err2;
+
 	device_initialize(&udc->dev);
 	udc->dev.release = usb_udc_release;
 	udc->dev.class = udc_class;
@@ -180,7 +214,7 @@ int usb_add_gadget_udc(struct device *parent, struct usb_gadget *gadget)
 	udc->dev.parent = parent;
 	ret = dev_set_name(&udc->dev, "%s", kobject_name(&parent->kobj));
 	if (ret)
-		goto err2;
+		goto err3;
 
 	udc->gadget = gadget;
 
@@ -189,21 +223,42 @@ int usb_add_gadget_udc(struct device *parent, struct usb_gadget *gadget)
 
 	ret = device_add(&udc->dev);
 	if (ret)
-		goto err3;
+		goto err4;
+
+	usb_gadget_set_state(gadget, USB_STATE_NOTATTACHED);
 
 	mutex_unlock(&udc_lock);
 
 	return 0;
-err3:
+
+err4:
 	list_del(&udc->list);
 	mutex_unlock(&udc_lock);
 
-err2:
+err3:
 	put_device(&udc->dev);
 
+err2:
+	put_device(&gadget->dev);
+	kfree(udc);
+
 err1:
 	return ret;
 }
+EXPORT_SYMBOL_GPL(usb_add_gadget_udc_release);
+
+/**
+ * usb_add_gadget_udc - adds a new gadget to the udc class driver list
+ * @parent: the parent device to this udc. Usually the controller
+ * driver's device.
+ * @gadget: the gadget to be added to the list
+ *
+ * Returns zero on success, negative errno otherwise.
+ */
+int usb_add_gadget_udc(struct device *parent, struct usb_gadget *gadget)
+{
+	return usb_add_gadget_udc_release(parent, gadget, NULL);
+}
 EXPORT_SYMBOL_GPL(usb_add_gadget_udc);
 
 static void usb_gadget_remove_driver(struct usb_udc *udc)
@@ -220,6 +275,7 @@ static void usb_gadget_remove_driver(struct usb_udc *udc)
 
 	udc->driver = NULL;
 	udc->dev.driver = NULL;
+	udc->gadget->dev.driver = NULL;
 }
 
 /**
@@ -254,6 +310,7 @@ found:
 
 	kobject_uevent(&udc->dev.kobj, KOBJ_REMOVE);
 	device_unregister(&udc->dev);
+	device_unregister(&gadget->dev);
 }
 EXPORT_SYMBOL_GPL(usb_del_gadget_udc);
 
@@ -268,6 +325,7 @@ static int udc_bind_to_driver(struct usb_udc *udc, struct usb_gadget_driver *dri
 
 	udc->driver = driver;
 	udc->dev.driver = &driver->driver;
+	udc->gadget->dev.driver = &driver->driver;
 
 	ret = driver->bind(udc->gadget, driver);
 	if (ret)
@@ -286,6 +344,7 @@ err1:
 			udc->driver->function, ret);
 	udc->driver = NULL;
 	udc->dev.driver = NULL;
+	udc->gadget->dev.driver = NULL;
 	return ret;
 }
 
@@ -395,6 +454,16 @@ static ssize_t usb_udc_softconn_store(struct device *dev,
 }
 static DEVICE_ATTR(soft_connect, S_IWUSR, NULL, usb_udc_softconn_store);
 
+static ssize_t usb_gadget_state_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct usb_udc		*udc = container_of(dev, struct usb_udc, dev);
+	struct usb_gadget	*gadget = udc->gadget;
+
+	return sprintf(buf, "%s\n", usb_state_string(gadget->state));
+}
+static DEVICE_ATTR(state, S_IRUGO, usb_gadget_state_show, NULL);
+
 #define USB_UDC_SPEED_ATTR(name, param)					\
 ssize_t usb_udc_##param##_show(struct device *dev,			\
 		struct device_attribute *attr, char *buf)		\
@@ -403,7 +472,7 @@ ssize_t usb_udc_##param##_show(struct device *dev,			\
 	return snprintf(buf, PAGE_SIZE, "%s\n",				\
 			usb_speed_string(udc->gadget->param));		\
 }									\
-static DEVICE_ATTR(name, S_IRUSR, usb_udc_##param##_show, NULL)
+static DEVICE_ATTR(name, S_IRUGO, usb_udc_##param##_show, NULL)
 
 static USB_UDC_SPEED_ATTR(current_speed, speed);
 static USB_UDC_SPEED_ATTR(maximum_speed, max_speed);
@@ -428,6 +497,7 @@ static USB_UDC_ATTR(a_alt_hnp_support);
 static struct attribute *usb_udc_attrs[] = {
 	&dev_attr_srp.attr,
 	&dev_attr_soft_connect.attr,
+	&dev_attr_state.attr,
 	&dev_attr_current_speed.attr,
 	&dev_attr_maximum_speed.attr,
 
diff --git a/drivers/usb/gadget/uvc.h b/drivers/usb/gadget/uvc.h
index 93b0c119111..817e9e19cec 100644
--- a/drivers/usb/gadget/uvc.h
+++ b/drivers/usb/gadget/uvc.h
@@ -98,8 +98,6 @@ extern unsigned int uvc_gadget_trace_param;
 #define DRIVER_VERSION				"0.1.0"
 #define DRIVER_VERSION_NUMBER			KERNEL_VERSION(0, 1, 0)
 
-#define DMA_ADDR_INVALID			(~(dma_addr_t)0)
-
 #define UVC_NUM_REQUESTS			4
 #define UVC_MAX_REQUEST_SIZE			64
 #define UVC_MAX_EVENTS				4
@@ -190,6 +188,7 @@ struct uvc_file_handle
  * Functions
  */
 
+extern void uvc_function_setup_continue(struct uvc_device *uvc);
 extern void uvc_endpoint_stream(struct uvc_device *dev);
 
 extern void uvc_function_connect(struct uvc_device *uvc);
diff --git a/drivers/usb/gadget/uvc_queue.c b/drivers/usb/gadget/uvc_queue.c
index 104ae9c8125..7ce27e35550 100644
--- a/drivers/usb/gadget/uvc_queue.c
+++ b/drivers/usb/gadget/uvc_queue.c
@@ -10,6 +10,7 @@
  *	(at your option) any later version.
  */
 
+#include <linux/atomic.h>
 #include <linux/kernel.h>
 #include <linux/mm.h>
 #include <linux/list.h>
@@ -18,7 +19,8 @@
 #include <linux/videodev2.h>
 #include <linux/vmalloc.h>
 #include <linux/wait.h>
-#include <linux/atomic.h>
+
+#include <media/videobuf2-vmalloc.h>
 
 #include "uvc.h"
 
@@ -28,330 +30,175 @@
  * Video queues is initialized by uvc_queue_init(). The function performs
  * basic initialization of the uvc_video_queue struct and never fails.
  *
- * Video buffer allocation and freeing are performed by uvc_alloc_buffers and
- * uvc_free_buffers respectively. The former acquires the video queue lock,
- * while the later must be called with the lock held (so that allocation can
- * free previously allocated buffers). Trying to free buffers that are mapped
- * to user space will return -EBUSY.
- *
- * Video buffers are managed using two queues. However, unlike most USB video
- * drivers that use an in queue and an out queue, we use a main queue to hold
- * all queued buffers (both 'empty' and 'done' buffers), and an irq queue to
- * hold empty buffers. This design (copied from video-buf) minimizes locking
- * in interrupt, as only one queue is shared between interrupt and user
- * contexts.
- *
- * Use cases
- * ---------
- *
- * Unless stated otherwise, all operations that modify the irq buffers queue
- * are protected by the irq spinlock.
- *
- * 1. The user queues the buffers, starts streaming and dequeues a buffer.
- *
- *    The buffers are added to the main and irq queues. Both operations are
- *    protected by the queue lock, and the later is protected by the irq
- *    spinlock as well.
- *
- *    The completion handler fetches a buffer from the irq queue and fills it
- *    with video data. If no buffer is available (irq queue empty), the handler
- *    returns immediately.
- *
- *    When the buffer is full, the completion handler removes it from the irq
- *    queue, marks it as ready (UVC_BUF_STATE_DONE) and wakes its wait queue.
- *    At that point, any process waiting on the buffer will be woken up. If a
- *    process tries to dequeue a buffer after it has been marked ready, the
- *    dequeing will succeed immediately.
- *
- * 2. Buffers are queued, user is waiting on a buffer and the device gets
- *    disconnected.
- *
- *    When the device is disconnected, the kernel calls the completion handler
- *    with an appropriate status code. The handler marks all buffers in the
- *    irq queue as being erroneous (UVC_BUF_STATE_ERROR) and wakes them up so
- *    that any process waiting on a buffer gets woken up.
- *
- *    Waking up up the first buffer on the irq list is not enough, as the
- *    process waiting on the buffer might restart the dequeue operation
- *    immediately.
- *
+ * Video buffers are managed by videobuf2. The driver uses a mutex to protect
+ * the videobuf2 queue operations by serializing calls to videobuf2 and a
+ * spinlock to protect the IRQ queue that holds the buffers to be processed by
+ * the driver.
  */
 
-static void
-uvc_queue_init(struct uvc_video_queue *queue, enum v4l2_buf_type type)
-{
-	mutex_init(&queue->mutex);
-	spin_lock_init(&queue->irqlock);
-	INIT_LIST_HEAD(&queue->mainqueue);
-	INIT_LIST_HEAD(&queue->irqqueue);
-	queue->type = type;
-}
-
-/*
- * Free the video buffers.
- *
- * This function must be called with the queue lock held.
+/* -----------------------------------------------------------------------------
+ * videobuf2 queue operations
  */
-static int uvc_free_buffers(struct uvc_video_queue *queue)
+
+static int uvc_queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
+			   unsigned int *nbuffers, unsigned int *nplanes,
+			   unsigned int sizes[], void *alloc_ctxs[])
 {
-	unsigned int i;
+	struct uvc_video_queue *queue = vb2_get_drv_priv(vq);
+	struct uvc_video *video = container_of(queue, struct uvc_video, queue);
 
-	for (i = 0; i < queue->count; ++i) {
-		if (queue->buffer[i].vma_use_count != 0)
-			return -EBUSY;
-	}
+	if (*nbuffers > UVC_MAX_VIDEO_BUFFERS)
+		*nbuffers = UVC_MAX_VIDEO_BUFFERS;
 
-	if (queue->count) {
-		vfree(queue->mem);
-		queue->count = 0;
-	}
+	*nplanes = 1;
+
+	sizes[0] = video->imagesize;
 
 	return 0;
 }
 
-/*
- * Allocate the video buffers.
- *
- * Pages are reserved to make sure they will not be swapped, as they will be
- * filled in the URB completion handler.
- *
- * Buffers will be individually mapped, so they must all be page aligned.
- */
-static int
-uvc_alloc_buffers(struct uvc_video_queue *queue, unsigned int nbuffers,
-		  unsigned int buflength)
+static int uvc_buffer_prepare(struct vb2_buffer *vb)
 {
-	unsigned int bufsize = PAGE_ALIGN(buflength);
-	unsigned int i;
-	void *mem = NULL;
-	int ret;
+	struct uvc_video_queue *queue = vb2_get_drv_priv(vb->vb2_queue);
+	struct uvc_buffer *buf = container_of(vb, struct uvc_buffer, buf);
 
-	if (nbuffers > UVC_MAX_VIDEO_BUFFERS)
-		nbuffers = UVC_MAX_VIDEO_BUFFERS;
+	if (vb->v4l2_buf.type == V4L2_BUF_TYPE_VIDEO_OUTPUT &&
+	    vb2_get_plane_payload(vb, 0) > vb2_plane_size(vb, 0)) {
+		uvc_trace(UVC_TRACE_CAPTURE, "[E] Bytes used out of bounds.\n");
+		return -EINVAL;
+	}
 
-	mutex_lock(&queue->mutex);
+	if (unlikely(queue->flags & UVC_QUEUE_DISCONNECTED))
+		return -ENODEV;
 
-	if ((ret = uvc_free_buffers(queue)) < 0)
-		goto done;
+	buf->state = UVC_BUF_STATE_QUEUED;
+	buf->mem = vb2_plane_vaddr(vb, 0);
+	buf->length = vb2_plane_size(vb, 0);
+	if (vb->v4l2_buf.type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		buf->bytesused = 0;
+	else
+		buf->bytesused = vb2_get_plane_payload(vb, 0);
 
-	/* Bail out if no buffers should be allocated. */
-	if (nbuffers == 0)
-		goto done;
+	return 0;
+}
 
-	/* Decrement the number of buffers until allocation succeeds. */
-	for (; nbuffers > 0; --nbuffers) {
-		mem = vmalloc_32(nbuffers * bufsize);
-		if (mem != NULL)
-			break;
-	}
+static void uvc_buffer_queue(struct vb2_buffer *vb)
+{
+	struct uvc_video_queue *queue = vb2_get_drv_priv(vb->vb2_queue);
+	struct uvc_buffer *buf = container_of(vb, struct uvc_buffer, buf);
+	unsigned long flags;
 
-	if (mem == NULL) {
-		ret = -ENOMEM;
-		goto done;
-	}
+	spin_lock_irqsave(&queue->irqlock, flags);
 
-	for (i = 0; i < nbuffers; ++i) {
-		memset(&queue->buffer[i], 0, sizeof queue->buffer[i]);
-		queue->buffer[i].buf.index = i;
-		queue->buffer[i].buf.m.offset = i * bufsize;
-		queue->buffer[i].buf.length = buflength;
-		queue->buffer[i].buf.type = queue->type;
-		queue->buffer[i].buf.sequence = 0;
-		queue->buffer[i].buf.field = V4L2_FIELD_NONE;
-		queue->buffer[i].buf.memory = V4L2_MEMORY_MMAP;
-		queue->buffer[i].buf.flags = 0;
-		init_waitqueue_head(&queue->buffer[i].wait);
+	if (likely(!(queue->flags & UVC_QUEUE_DISCONNECTED))) {
+		list_add_tail(&buf->queue, &queue->irqqueue);
+	} else {
+		/* If the device is disconnected return the buffer to userspace
+		 * directly. The next QBUF call will fail with -ENODEV.
+		 */
+		buf->state = UVC_BUF_STATE_ERROR;
+		vb2_buffer_done(&buf->buf, VB2_BUF_STATE_ERROR);
 	}
 
-	queue->mem = mem;
-	queue->count = nbuffers;
-	queue->buf_size = bufsize;
-	ret = nbuffers;
-
-done:
-	mutex_unlock(&queue->mutex);
-	return ret;
+	spin_unlock_irqrestore(&queue->irqlock, flags);
 }
 
-static void __uvc_query_buffer(struct uvc_buffer *buf,
-		struct v4l2_buffer *v4l2_buf)
-{
-	memcpy(v4l2_buf, &buf->buf, sizeof *v4l2_buf);
-
-	if (buf->vma_use_count)
-		v4l2_buf->flags |= V4L2_BUF_FLAG_MAPPED;
-
-	switch (buf->state) {
-	case UVC_BUF_STATE_ERROR:
-	case UVC_BUF_STATE_DONE:
-		v4l2_buf->flags |= V4L2_BUF_FLAG_DONE;
-		break;
-	case UVC_BUF_STATE_QUEUED:
-	case UVC_BUF_STATE_ACTIVE:
-		v4l2_buf->flags |= V4L2_BUF_FLAG_QUEUED;
-		break;
-	case UVC_BUF_STATE_IDLE:
-	default:
-		break;
-	}
-}
+static struct vb2_ops uvc_queue_qops = {
+	.queue_setup = uvc_queue_setup,
+	.buf_prepare = uvc_buffer_prepare,
+	.buf_queue = uvc_buffer_queue,
+};
 
-static int
-uvc_query_buffer(struct uvc_video_queue *queue, struct v4l2_buffer *v4l2_buf)
+static int uvc_queue_init(struct uvc_video_queue *queue,
+			  enum v4l2_buf_type type)
 {
-	int ret = 0;
+	int ret;
 
-	mutex_lock(&queue->mutex);
-	if (v4l2_buf->index >= queue->count) {
-		ret = -EINVAL;
-		goto done;
-	}
+	queue->queue.type = type;
+	queue->queue.io_modes = VB2_MMAP | VB2_USERPTR;
+	queue->queue.drv_priv = queue;
+	queue->queue.buf_struct_size = sizeof(struct uvc_buffer);
+	queue->queue.ops = &uvc_queue_qops;
+	queue->queue.mem_ops = &vb2_vmalloc_memops;
+	ret = vb2_queue_init(&queue->queue);
+	if (ret)
+		return ret;
+
+	mutex_init(&queue->mutex);
+	spin_lock_init(&queue->irqlock);
+	INIT_LIST_HEAD(&queue->irqqueue);
+	queue->flags = 0;
 
-	__uvc_query_buffer(&queue->buffer[v4l2_buf->index], v4l2_buf);
+	return 0;
+}
 
-done:
+/*
+ * Free the video buffers.
+ */
+static void uvc_free_buffers(struct uvc_video_queue *queue)
+{
+	mutex_lock(&queue->mutex);
+	vb2_queue_release(&queue->queue);
 	mutex_unlock(&queue->mutex);
-	return ret;
 }
 
 /*
- * Queue a video buffer. Attempting to queue a buffer that has already been
- * queued will return -EINVAL.
+ * Allocate the video buffers.
  */
-static int
-uvc_queue_buffer(struct uvc_video_queue *queue, struct v4l2_buffer *v4l2_buf)
+static int uvc_alloc_buffers(struct uvc_video_queue *queue,
+			     struct v4l2_requestbuffers *rb)
 {
-	struct uvc_buffer *buf;
-	unsigned long flags;
-	int ret = 0;
+	int ret;
 
-	uvc_trace(UVC_TRACE_CAPTURE, "Queuing buffer %u.\n", v4l2_buf->index);
+	mutex_lock(&queue->mutex);
+	ret = vb2_reqbufs(&queue->queue, rb);
+	mutex_unlock(&queue->mutex);
 
-	if (v4l2_buf->type != queue->type ||
-	    v4l2_buf->memory != V4L2_MEMORY_MMAP) {
-		uvc_trace(UVC_TRACE_CAPTURE, "[E] Invalid buffer type (%u) "
-			"and/or memory (%u).\n", v4l2_buf->type,
-			v4l2_buf->memory);
-		return -EINVAL;
-	}
+	return ret ? ret : rb->count;
+}
 
-	mutex_lock(&queue->mutex);
-	if (v4l2_buf->index >= queue->count) {
-		uvc_trace(UVC_TRACE_CAPTURE, "[E] Out of range index.\n");
-		ret = -EINVAL;
-		goto done;
-	}
+static int uvc_query_buffer(struct uvc_video_queue *queue,
+			    struct v4l2_buffer *buf)
+{
+	int ret;
 
-	buf = &queue->buffer[v4l2_buf->index];
-	if (buf->state != UVC_BUF_STATE_IDLE) {
-		uvc_trace(UVC_TRACE_CAPTURE, "[E] Invalid buffer state "
-			"(%u).\n", buf->state);
-		ret = -EINVAL;
-		goto done;
-	}
+	mutex_lock(&queue->mutex);
+	ret = vb2_querybuf(&queue->queue, buf);
+	mutex_unlock(&queue->mutex);
 
-	if (v4l2_buf->type == V4L2_BUF_TYPE_VIDEO_OUTPUT &&
-	    v4l2_buf->bytesused > buf->buf.length) {
-		uvc_trace(UVC_TRACE_CAPTURE, "[E] Bytes used out of bounds.\n");
-		ret = -EINVAL;
-		goto done;
-	}
+	return ret;
+}
 
-	if (v4l2_buf->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		buf->buf.bytesused = 0;
-	else
-		buf->buf.bytesused = v4l2_buf->bytesused;
+static int uvc_queue_buffer(struct uvc_video_queue *queue,
+			    struct v4l2_buffer *buf)
+{
+	unsigned long flags;
+	int ret;
 
+	mutex_lock(&queue->mutex);
+	ret = vb2_qbuf(&queue->queue, buf);
 	spin_lock_irqsave(&queue->irqlock, flags);
-	if (queue->flags & UVC_QUEUE_DISCONNECTED) {
-		spin_unlock_irqrestore(&queue->irqlock, flags);
-		ret = -ENODEV;
-		goto done;
-	}
-	buf->state = UVC_BUF_STATE_QUEUED;
-
 	ret = (queue->flags & UVC_QUEUE_PAUSED) != 0;
 	queue->flags &= ~UVC_QUEUE_PAUSED;
-
-	list_add_tail(&buf->stream, &queue->mainqueue);
-	list_add_tail(&buf->queue, &queue->irqqueue);
 	spin_unlock_irqrestore(&queue->irqlock, flags);
-
-done:
 	mutex_unlock(&queue->mutex);
-	return ret;
-}
 
-static int uvc_queue_waiton(struct uvc_buffer *buf, int nonblocking)
-{
-	if (nonblocking) {
-		return (buf->state != UVC_BUF_STATE_QUEUED &&
-			buf->state != UVC_BUF_STATE_ACTIVE)
-			? 0 : -EAGAIN;
-	}
-
-	return wait_event_interruptible(buf->wait,
-		buf->state != UVC_BUF_STATE_QUEUED &&
-		buf->state != UVC_BUF_STATE_ACTIVE);
+	return ret;
 }
 
 /*
  * Dequeue a video buffer. If nonblocking is false, block until a buffer is
  * available.
  */
-static int
-uvc_dequeue_buffer(struct uvc_video_queue *queue, struct v4l2_buffer *v4l2_buf,
-		   int nonblocking)
+static int uvc_dequeue_buffer(struct uvc_video_queue *queue,
+			      struct v4l2_buffer *buf, int nonblocking)
 {
-	struct uvc_buffer *buf;
-	int ret = 0;
-
-	if (v4l2_buf->type != queue->type ||
-	    v4l2_buf->memory != V4L2_MEMORY_MMAP) {
-		uvc_trace(UVC_TRACE_CAPTURE, "[E] Invalid buffer type (%u) "
-			"and/or memory (%u).\n", v4l2_buf->type,
-			v4l2_buf->memory);
-		return -EINVAL;
-	}
+	int ret;
 
 	mutex_lock(&queue->mutex);
-	if (list_empty(&queue->mainqueue)) {
-		uvc_trace(UVC_TRACE_CAPTURE, "[E] Empty buffer queue.\n");
-		ret = -EINVAL;
-		goto done;
-	}
-
-	buf = list_first_entry(&queue->mainqueue, struct uvc_buffer, stream);
-	if ((ret = uvc_queue_waiton(buf, nonblocking)) < 0)
-		goto done;
-
-	uvc_trace(UVC_TRACE_CAPTURE, "Dequeuing buffer %u (%u, %u bytes).\n",
-		buf->buf.index, buf->state, buf->buf.bytesused);
-
-	switch (buf->state) {
-	case UVC_BUF_STATE_ERROR:
-		uvc_trace(UVC_TRACE_CAPTURE, "[W] Corrupted data "
-			"(transmission error).\n");
-		ret = -EIO;
-	case UVC_BUF_STATE_DONE:
-		buf->state = UVC_BUF_STATE_IDLE;
-		break;
-
-	case UVC_BUF_STATE_IDLE:
-	case UVC_BUF_STATE_QUEUED:
-	case UVC_BUF_STATE_ACTIVE:
-	default:
-		uvc_trace(UVC_TRACE_CAPTURE, "[E] Invalid buffer state %u "
-			"(driver bug?).\n", buf->state);
-		ret = -EINVAL;
-		goto done;
-	}
-
-	list_del(&buf->stream);
-	__uvc_query_buffer(buf, v4l2_buf);
-
-done:
+	ret = vb2_dqbuf(&queue->queue, buf, nonblocking);
 	mutex_unlock(&queue->mutex);
+
 	return ret;
 }
 
@@ -361,105 +208,47 @@ done:
  * This function implements video queue polling and is intended to be used by
  * the device poll handler.
  */
-static unsigned int
-uvc_queue_poll(struct uvc_video_queue *queue, struct file *file,
-	       poll_table *wait)
+static unsigned int uvc_queue_poll(struct uvc_video_queue *queue,
+				   struct file *file, poll_table *wait)
 {
-	struct uvc_buffer *buf;
-	unsigned int mask = 0;
+	unsigned int ret;
 
 	mutex_lock(&queue->mutex);
-	if (list_empty(&queue->mainqueue))
-		goto done;
-
-	buf = list_first_entry(&queue->mainqueue, struct uvc_buffer, stream);
-
-	poll_wait(file, &buf->wait, wait);
-	if (buf->state == UVC_BUF_STATE_DONE ||
-	    buf->state == UVC_BUF_STATE_ERROR)
-		mask |= POLLOUT | POLLWRNORM;
-
-done:
+	ret = vb2_poll(&queue->queue, file, wait);
 	mutex_unlock(&queue->mutex);
-	return mask;
-}
 
-/*
- * VMA operations.
- */
-static void uvc_vm_open(struct vm_area_struct *vma)
-{
-	struct uvc_buffer *buffer = vma->vm_private_data;
-	buffer->vma_use_count++;
+	return ret;
 }
 
-static void uvc_vm_close(struct vm_area_struct *vma)
+static int uvc_queue_mmap(struct uvc_video_queue *queue,
+			  struct vm_area_struct *vma)
 {
-	struct uvc_buffer *buffer = vma->vm_private_data;
-	buffer->vma_use_count--;
-}
+	int ret;
 
-static struct vm_operations_struct uvc_vm_ops = {
-	.open		= uvc_vm_open,
-	.close		= uvc_vm_close,
-};
+	mutex_lock(&queue->mutex);
+	ret = vb2_mmap(&queue->queue, vma);
+	mutex_unlock(&queue->mutex);
+
+	return ret;
+}
 
+#ifndef CONFIG_MMU
 /*
- * Memory-map a buffer.
+ * Get unmapped area.
  *
- * This function implements video buffer memory mapping and is intended to be
- * used by the device mmap handler.
+ * NO-MMU arch need this function to make mmap() work correctly.
  */
-static int
-uvc_queue_mmap(struct uvc_video_queue *queue, struct vm_area_struct *vma)
+static unsigned long uvc_queue_get_unmapped_area(struct uvc_video_queue *queue,
+		unsigned long pgoff)
 {
-	struct uvc_buffer *uninitialized_var(buffer);
-	struct page *page;
-	unsigned long addr, start, size;
-	unsigned int i;
-	int ret = 0;
-
-	start = vma->vm_start;
-	size = vma->vm_end - vma->vm_start;
+	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) == vma->vm_pgoff)
-			break;
-	}
-
-	if (i == queue->count || size != queue->buf_size) {
-		ret = -EINVAL;
-		goto done;
-	}
-
-	/*
-	 * VM_IO marks the area as being an mmaped region for I/O to a
-	 * device. It also prevents the region from being core dumped.
-	 */
-	vma->vm_flags |= VM_IO;
-
-	addr = (unsigned long)queue->mem + buffer->buf.m.offset;
-	while (size > 0) {
-		page = vmalloc_to_page((void *)addr);
-		if ((ret = vm_insert_page(vma, start, page)) < 0)
-			goto done;
-
-		start += PAGE_SIZE;
-		addr += PAGE_SIZE;
-		size -= PAGE_SIZE;
-	}
-
-	vma->vm_ops = &uvc_vm_ops;
-	vma->vm_private_data = buffer;
-	uvc_vm_open(vma);
-
-done:
+	ret = vb2_get_unmapped_area(&queue->queue, 0, 0, pgoff, 0);
 	mutex_unlock(&queue->mutex);
 	return ret;
 }
+#endif
 
 /*
  * Cancel the video buffers queue.
@@ -484,7 +273,7 @@ static void uvc_queue_cancel(struct uvc_video_queue *queue, int disconnect)
 				       queue);
 		list_del(&buf->queue);
 		buf->state = UVC_BUF_STATE_ERROR;
-		wake_up(&buf->wait);
+		vb2_buffer_done(&buf->buf, VB2_BUF_STATE_ERROR);
 	}
 	/* This must be protected by the irqlock spinlock to avoid race
 	 * conditions between uvc_queue_buffer and the disconnection event that
@@ -516,26 +305,33 @@ static void uvc_queue_cancel(struct uvc_video_queue *queue, int disconnect)
  */
 static int uvc_queue_enable(struct uvc_video_queue *queue, int enable)
 {
-	unsigned int i;
+	unsigned long flags;
 	int ret = 0;
 
 	mutex_lock(&queue->mutex);
 	if (enable) {
-		if (uvc_queue_streaming(queue)) {
-			ret = -EBUSY;
+		ret = vb2_streamon(&queue->queue, queue->queue.type);
+		if (ret < 0)
 			goto done;
-		}
+
 		queue->sequence = 0;
-		queue->flags |= UVC_QUEUE_STREAMING;
 		queue->buf_used = 0;
 	} else {
-		uvc_queue_cancel(queue, 0);
-		INIT_LIST_HEAD(&queue->mainqueue);
+		ret = vb2_streamoff(&queue->queue, queue->queue.type);
+		if (ret < 0)
+			goto done;
 
-		for (i = 0; i < queue->count; ++i)
-			queue->buffer[i].state = UVC_BUF_STATE_IDLE;
+		spin_lock_irqsave(&queue->irqlock, flags);
+		INIT_LIST_HEAD(&queue->irqqueue);
 
-		queue->flags &= ~UVC_QUEUE_STREAMING;
+		/*
+		 * FIXME: We need to clear the DISCONNECTED flag to ensure that
+		 * applications will be able to queue buffers for the next
+		 * streaming run. However, clearing it here doesn't guarantee
+		 * that the device will be reconnected in the meantime.
+		 */
+		queue->flags &= ~UVC_QUEUE_DISCONNECTED;
+		spin_unlock_irqrestore(&queue->irqlock, flags);
 	}
 
 done:
@@ -544,15 +340,15 @@ done:
 }
 
 /* called with &queue_irqlock held.. */
-static struct uvc_buffer *
-uvc_queue_next_buffer(struct uvc_video_queue *queue, struct uvc_buffer *buf)
+static struct uvc_buffer *uvc_queue_next_buffer(struct uvc_video_queue *queue,
+						struct uvc_buffer *buf)
 {
 	struct uvc_buffer *nextbuf;
 
 	if ((queue->flags & UVC_QUEUE_DROP_INCOMPLETE) &&
-	    buf->buf.length != buf->buf.bytesused) {
+	     buf->length != buf->bytesused) {
 		buf->state = UVC_BUF_STATE_QUEUED;
-		buf->buf.bytesused = 0;
+		vb2_set_plane_payload(&buf->buf, 0, 0);
 		return buf;
 	}
 
@@ -563,10 +359,18 @@ uvc_queue_next_buffer(struct uvc_video_queue *queue, struct uvc_buffer *buf)
 	else
 		nextbuf = NULL;
 
-	buf->buf.sequence = queue->sequence++;
-	do_gettimeofday(&buf->buf.timestamp);
+	/*
+	 * FIXME: with videobuf2, the sequence number or timestamp fields
+	 * are valid only for video capture devices and the UVC gadget usually
+	 * is a video output device. Keeping these until the specs are clear on
+	 * this aspect.
+	 */
+	buf->buf.v4l2_buf.sequence = queue->sequence++;
+	do_gettimeofday(&buf->buf.v4l2_buf.timestamp);
+
+	vb2_set_plane_payload(&buf->buf, 0, buf->bytesused);
+	vb2_buffer_done(&buf->buf, VB2_BUF_STATE_DONE);
 
-	wake_up(&buf->wait);
 	return nextbuf;
 }
 
diff --git a/drivers/usb/gadget/uvc_queue.h b/drivers/usb/gadget/uvc_queue.h
index 1812a8ecc5d..8e76ce982f1 100644
--- a/drivers/usb/gadget/uvc_queue.h
+++ b/drivers/usb/gadget/uvc_queue.h
@@ -6,6 +6,7 @@
 #include <linux/kernel.h>
 #include <linux/poll.h>
 #include <linux/videodev2.h>
+#include <media/videobuf2-core.h>
 
 /* Maximum frame size in bytes, for sanity checking. */
 #define UVC_MAX_FRAME_SIZE	(16*1024*1024)
@@ -25,42 +26,35 @@ enum uvc_buffer_state {
 };
 
 struct uvc_buffer {
-	unsigned long vma_use_count;
-	struct list_head stream;
-
-	/* Touched by interrupt handler. */
-	struct v4l2_buffer buf;
+	struct vb2_buffer buf;
 	struct list_head queue;
-	wait_queue_head_t wait;
+
 	enum uvc_buffer_state state;
+	void *mem;
+	unsigned int length;
+	unsigned int bytesused;
 };
 
-#define UVC_QUEUE_STREAMING		(1 << 0)
-#define UVC_QUEUE_DISCONNECTED		(1 << 1)
-#define UVC_QUEUE_DROP_INCOMPLETE	(1 << 2)
-#define UVC_QUEUE_PAUSED		(1 << 3)
+#define UVC_QUEUE_DISCONNECTED		(1 << 0)
+#define UVC_QUEUE_DROP_INCOMPLETE	(1 << 1)
+#define UVC_QUEUE_PAUSED		(1 << 2)
 
 struct uvc_video_queue {
-	enum v4l2_buf_type type;
+	struct vb2_queue queue;
+	struct mutex mutex;	/* Protects queue */
 
-	void *mem;
 	unsigned int flags;
 	__u32 sequence;
 
-	unsigned int count;
-	unsigned int buf_size;
 	unsigned int buf_used;
-	struct uvc_buffer buffer[UVC_MAX_VIDEO_BUFFERS];
-	struct mutex mutex;	/* protects buffers and mainqueue */
-	spinlock_t irqlock;	/* protects irqqueue */
 
-	struct list_head mainqueue;
+	spinlock_t irqlock;	/* Protects flags and irqqueue */
 	struct list_head irqqueue;
 };
 
 static inline int uvc_queue_streaming(struct uvc_video_queue *queue)
 {
-	return queue->flags & UVC_QUEUE_STREAMING;
+	return vb2_is_streaming(&queue->queue);
 }
 
 #endif /* __KERNEL__ */
diff --git a/drivers/usb/gadget/uvc_v4l2.c b/drivers/usb/gadget/uvc_v4l2.c
index 2ca9386d655..ad48e81155e 100644
--- a/drivers/usb/gadget/uvc_v4l2.c
+++ b/drivers/usb/gadget/uvc_v4l2.c
@@ -41,9 +41,8 @@ uvc_send_response(struct uvc_device *uvc, struct uvc_request_data *data)
 
 	req->length = min_t(unsigned int, uvc->event_length, data->length);
 	req->zero = data->length < uvc->event_length;
-	req->dma = DMA_ADDR_INVALID;
 
-	memcpy(req->buf, data->data, data->length);
+	memcpy(req->buf, data->data, req->length);
 
 	return usb_ep_queue(cdev->gadget->ep0, req, GFP_KERNEL);
 }
@@ -148,16 +147,13 @@ uvc_v4l2_release(struct file *file)
 	uvc_function_disconnect(uvc);
 
 	uvc_video_enable(video, 0);
-	mutex_lock(&video->queue.mutex);
-	if (uvc_free_buffers(&video->queue) < 0)
-		printk(KERN_ERR "uvc_v4l2_release: Unable to free "
-				"buffers.\n");
-	mutex_unlock(&video->queue.mutex);
+	uvc_free_buffers(&video->queue);
 
 	file->private_data = NULL;
 	v4l2_fh_del(&handle->vfh);
 	v4l2_fh_exit(&handle->vfh);
 	kfree(handle);
+
 	return 0;
 }
 
@@ -178,9 +174,9 @@ uvc_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 		struct v4l2_capability *cap = arg;
 
 		memset(cap, 0, sizeof *cap);
-		strncpy(cap->driver, "g_uvc", sizeof(cap->driver));
-		strncpy(cap->card, cdev->gadget->name, sizeof(cap->card));
-		strncpy(cap->bus_info, dev_name(&cdev->gadget->dev),
+		strlcpy(cap->driver, "g_uvc", sizeof(cap->driver));
+		strlcpy(cap->card, cdev->gadget->name, sizeof(cap->card));
+		strlcpy(cap->bus_info, dev_name(&cdev->gadget->dev),
 			sizeof cap->bus_info);
 		cap->version = DRIVER_VERSION_NUMBER;
 		cap->capabilities = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
@@ -192,7 +188,7 @@ uvc_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 	{
 		struct v4l2_format *fmt = arg;
 
-		if (fmt->type != video->queue.type)
+		if (fmt->type != video->queue.queue.type)
 			return -EINVAL;
 
 		return uvc_v4l2_get_format(video, fmt);
@@ -202,7 +198,7 @@ uvc_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 	{
 		struct v4l2_format *fmt = arg;
 
-		if (fmt->type != video->queue.type)
+		if (fmt->type != video->queue.queue.type)
 			return -EINVAL;
 
 		return uvc_v4l2_set_format(video, fmt);
@@ -213,16 +209,13 @@ uvc_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 	{
 		struct v4l2_requestbuffers *rb = arg;
 
-		if (rb->type != video->queue.type ||
-		    rb->memory != V4L2_MEMORY_MMAP)
+		if (rb->type != video->queue.queue.type)
 			return -EINVAL;
 
-		ret = uvc_alloc_buffers(&video->queue, rb->count,
-					video->imagesize);
+		ret = uvc_alloc_buffers(&video->queue, rb);
 		if (ret < 0)
 			return ret;
 
-		rb->count = ret;
 		ret = 0;
 		break;
 	}
@@ -231,9 +224,6 @@ uvc_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 	{
 		struct v4l2_buffer *buf = arg;
 
-		if (buf->type != video->queue.type)
-			return -EINVAL;
-
 		return uvc_query_buffer(&video->queue, buf);
 	}
 
@@ -251,24 +241,36 @@ uvc_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 	{
 		int *type = arg;
 
-		if (*type != video->queue.type)
+		if (*type != video->queue.queue.type)
 			return -EINVAL;
 
-		return uvc_video_enable(video, 1);
+		/* Enable UVC video. */
+		ret = uvc_video_enable(video, 1);
+		if (ret < 0)
+			return ret;
+
+		/*
+		 * Complete the alternate setting selection setup phase now that
+		 * userspace is ready to provide video frames.
+		 */
+		uvc_function_setup_continue(uvc);
+		uvc->state = UVC_STATE_STREAMING;
+
+		return 0;
 	}
 
 	case VIDIOC_STREAMOFF:
 	{
 		int *type = arg;
 
-		if (*type != video->queue.type)
+		if (*type != video->queue.queue.type)
 			return -EINVAL;
 
 		return uvc_video_enable(video, 0);
 	}
 
 	/* Events */
-        case VIDIOC_DQEVENT:
+	case VIDIOC_DQEVENT:
 	{
 		struct v4l2_event *event = arg;
 
@@ -333,17 +335,21 @@ uvc_v4l2_poll(struct file *file, poll_table *wait)
 {
 	struct video_device *vdev = video_devdata(file);
 	struct uvc_device *uvc = video_get_drvdata(vdev);
-	struct uvc_file_handle *handle = to_uvc_file_handle(file->private_data);
-	unsigned int mask = 0;
 
-	poll_wait(file, &handle->vfh.wait, wait);
-	if (v4l2_event_pending(&handle->vfh))
-		mask |= POLLPRI;
+	return uvc_queue_poll(&uvc->video.queue, file, wait);
+}
 
-	mask |= uvc_queue_poll(&uvc->video.queue, file, wait);
+#ifndef CONFIG_MMU
+static unsigned long uvc_v4l2_get_unmapped_area(struct file *file,
+		unsigned long addr, unsigned long len, unsigned long pgoff,
+		unsigned long flags)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct uvc_device *uvc = video_get_drvdata(vdev);
 
-	return mask;
+	return uvc_queue_get_unmapped_area(&uvc->video.queue, pgoff);
 }
+#endif
 
 static struct v4l2_file_operations uvc_v4l2_fops = {
 	.owner		= THIS_MODULE,
@@ -352,5 +358,8 @@ static struct v4l2_file_operations uvc_v4l2_fops = {
 	.ioctl		= uvc_v4l2_ioctl,
 	.mmap		= uvc_v4l2_mmap,
 	.poll		= uvc_v4l2_poll,
+#ifndef CONFIG_MMU
+	.get_unmapped_area = uvc_v4l2_get_unmapped_area,
+#endif
 };
 
diff --git a/drivers/usb/gadget/uvc_video.c b/drivers/usb/gadget/uvc_video.c
index b0e53a8ea4f..71e896d4c5a 100644
--- a/drivers/usb/gadget/uvc_video.c
+++ b/drivers/usb/gadget/uvc_video.c
@@ -32,7 +32,7 @@ uvc_video_encode_header(struct uvc_video *video, struct uvc_buffer *buf,
 	data[0] = 2;
 	data[1] = UVC_STREAM_EOH | video->fid;
 
-	if (buf->buf.bytesused - video->queue.buf_used <= len - 2)
+	if (buf->bytesused - video->queue.buf_used <= len - 2)
 		data[1] |= UVC_STREAM_EOF;
 
 	return 2;
@@ -47,8 +47,8 @@ uvc_video_encode_data(struct uvc_video *video, struct uvc_buffer *buf,
 	void *mem;
 
 	/* Copy video data to the USB buffer. */
-	mem = queue->mem + buf->buf.m.offset + queue->buf_used;
-	nbytes = min((unsigned int)len, buf->buf.bytesused - queue->buf_used);
+	mem = buf->mem + queue->buf_used;
+	nbytes = min((unsigned int)len, buf->bytesused - queue->buf_used);
 
 	memcpy(data, mem, nbytes);
 	queue->buf_used += nbytes;
@@ -82,7 +82,7 @@ uvc_video_encode_bulk(struct usb_request *req, struct uvc_video *video,
 	req->length = video->req_size - len;
 	req->zero = video->payload_size == video->max_payload_size;
 
-	if (buf->buf.bytesused == video->queue.buf_used) {
+	if (buf->bytesused == video->queue.buf_used) {
 		video->queue.buf_used = 0;
 		buf->state = UVC_BUF_STATE_DONE;
 		uvc_queue_next_buffer(&video->queue, buf);
@@ -92,7 +92,7 @@ uvc_video_encode_bulk(struct usb_request *req, struct uvc_video *video,
 	}
 
 	if (video->payload_size == video->max_payload_size ||
-	    buf->buf.bytesused == video->queue.buf_used)
+	    buf->bytesused == video->queue.buf_used)
 		video->payload_size = 0;
 }
 
@@ -115,7 +115,7 @@ uvc_video_encode_isoc(struct usb_request *req, struct uvc_video *video,
 
 	req->length = video->req_size - len;
 
-	if (buf->buf.bytesused == video->queue.buf_used) {
+	if (buf->bytesused == video->queue.buf_used) {
 		video->queue.buf_used = 0;
 		buf->state = UVC_BUF_STATE_DONE;
 		uvc_queue_next_buffer(&video->queue, buf);
@@ -161,6 +161,7 @@ static void
 uvc_video_complete(struct usb_ep *ep, struct usb_request *req)
 {
 	struct uvc_video *video = req->context;
+	struct uvc_video_queue *queue = &video->queue;
 	struct uvc_buffer *buf;
 	unsigned long flags;
 	int ret;
@@ -169,13 +170,15 @@ uvc_video_complete(struct usb_ep *ep, struct usb_request *req)
 	case 0:
 		break;
 
-	case -ESHUTDOWN:
+	case -ESHUTDOWN:	/* disconnect from host. */
 		printk(KERN_INFO "VS request cancelled.\n");
+		uvc_queue_cancel(queue, 1);
 		goto requeue;
 
 	default:
 		printk(KERN_INFO "VS request completed with status %d.\n",
 			req->status);
+		uvc_queue_cancel(queue, 0);
 		goto requeue;
 	}
 
@@ -229,13 +232,18 @@ uvc_video_free_requests(struct uvc_video *video)
 static int
 uvc_video_alloc_requests(struct uvc_video *video)
 {
+	unsigned int req_size;
 	unsigned int i;
 	int ret = -ENOMEM;
 
 	BUG_ON(video->req_size);
 
+	req_size = video->ep->maxpacket
+		 * max_t(unsigned int, video->ep->maxburst, 1)
+		 * (video->ep->mult + 1);
+
 	for (i = 0; i < UVC_NUM_REQUESTS; ++i) {
-		video->req_buffer[i] = kmalloc(video->ep->maxpacket, GFP_KERNEL);
+		video->req_buffer[i] = kmalloc(req_size, GFP_KERNEL);
 		if (video->req_buffer[i] == NULL)
 			goto error;
 
@@ -245,14 +253,14 @@ uvc_video_alloc_requests(struct uvc_video *video)
 
 		video->req[i]->buf = video->req_buffer[i];
 		video->req[i]->length = 0;
-		video->req[i]->dma = DMA_ADDR_INVALID;
 		video->req[i]->complete = uvc_video_complete;
 		video->req[i]->context = video;
 
 		list_add_tail(&video->req[i]->list, &video->req_free);
 	}
 
-	video->req_size = video->ep->maxpacket;
+	video->req_size = req_size;
+
 	return 0;
 
 error:
@@ -309,7 +317,8 @@ uvc_video_pump(struct uvc_video *video)
 		video->encode(req, video, buf);
 
 		/* Queue the USB request */
-		if ((ret = usb_ep_queue(video->ep, req, GFP_KERNEL)) < 0) {
+		ret = usb_ep_queue(video->ep, req, GFP_ATOMIC);
+		if (ret < 0) {
 			printk(KERN_INFO "Failed to queue request (%d)\n", ret);
 			usb_ep_set_halt(video->ep);
 			spin_unlock_irqrestore(&video->queue.irqlock, flags);
diff --git a/drivers/usb/gadget/zero.c b/drivers/usb/gadget/zero.c
index 685fa681cb6..2cd6262e8b7 100644
--- a/drivers/usb/gadget/zero.c
+++ b/drivers/usb/gadget/zero.c
@@ -368,8 +368,10 @@ static int zero_unbind(struct usb_composite_dev *cdev)
 	del_timer_sync(&autoresume_timer);
 	if (!IS_ERR_OR_NULL(func_ss))
 		usb_put_function(func_ss);
+	usb_put_function_instance(func_inst_ss);
 	if (!IS_ERR_OR_NULL(func_lb))
 		usb_put_function(func_lb);
+	usb_put_function_instance(func_inst_lb);
 	return 0;
 }
 
diff --git a/drivers/usb/host/Kconfig b/drivers/usb/host/Kconfig
index c59a1126926..de94f269906 100644
--- a/drivers/usb/host/Kconfig
+++ b/drivers/usb/host/Kconfig
@@ -2,11 +2,9 @@
 # USB Host Controller Drivers
 #
 comment "USB Host Controller Drivers"
-	depends on USB
 
 config USB_C67X00_HCD
 	tristate "Cypress C67x00 HCD support"
-	depends on USB
 	help
 	  The Cypress C67x00 (EZ-Host/EZ-OTG) chips are dual-role
 	  host/peripheral/OTG USB controllers.
@@ -19,7 +17,7 @@ config USB_C67X00_HCD
 
 config USB_XHCI_HCD
 	tristate "xHCI HCD (USB 3.0) support"
-	depends on USB && USB_ARCH_HAS_XHCI
+	depends on USB_ARCH_HAS_XHCI
 	---help---
 	  The eXtensible Host Controller Interface (xHCI) is standard for USB 3.0
 	  "SuperSpeed" host controller hardware.
@@ -27,13 +25,13 @@ config USB_XHCI_HCD
 	  To compile this driver as a module, choose M here: the
 	  module will be called xhci-hcd.
 
+if USB_XHCI_HCD
+
 config USB_XHCI_PLATFORM
 	tristate
-	depends on USB_XHCI_HCD
 
 config USB_XHCI_HCD_DEBUGGING
 	bool "Debugging for the xHCI host controller"
-	depends on USB_XHCI_HCD
 	---help---
 	  Say 'Y' to turn on debugging for the xHCI host controller driver.
 	  This will spew debugging output, even in interrupt context.
@@ -41,9 +39,11 @@ config USB_XHCI_HCD_DEBUGGING
 
 	  If unsure, say N.
 
+endif # USB_XHCI_HCD
+
 config USB_EHCI_HCD
 	tristate "EHCI HCD (USB 2.0) support"
-	depends on USB && USB_ARCH_HAS_EHCI
+	depends on USB_ARCH_HAS_EHCI
 	---help---
 	  The Enhanced Host Controller Interface (EHCI) is standard for USB 2.0
 	  "high speed" (480 Mbit/sec, 60 Mbyte/sec) host controller hardware.
@@ -95,14 +95,19 @@ config USB_EHCI_TT_NEWSCHED
 
 	  If unsure, say Y.
 
+config USB_FSL_MPH_DR_OF
+	tristate
+
+if USB_EHCI_HCD
+
 config USB_EHCI_PCI
 	tristate
-	depends on USB_EHCI_HCD && PCI
+	depends on PCI
 	default y
 
 config USB_EHCI_HCD_PMC_MSP
 	tristate "EHCI support for on-chip PMC MSP71xx USB controller"
-	depends on USB_EHCI_HCD && MSP_HAS_USB
+	depends on MSP_HAS_USB
 	default n
 	select USB_EHCI_BIG_ENDIAN_DESC
 	select USB_EHCI_BIG_ENDIAN_MMIO
@@ -112,22 +117,13 @@ config USB_EHCI_HCD_PMC_MSP
 
 config USB_EHCI_BIG_ENDIAN_MMIO
 	bool
-	depends on USB_EHCI_HCD && (PPC_CELLEB || PPC_PS3 || 440EPX || \
-				    ARCH_IXP4XX || XPS_USB_HCD_XILINX || \
-				    PPC_MPC512x || CPU_CAVIUM_OCTEON || \
-				    PMC_MSP || SPARC_LEON || MIPS_SEAD3)
-	default y
 
 config USB_EHCI_BIG_ENDIAN_DESC
 	bool
-	depends on USB_EHCI_HCD && (440EPX || ARCH_IXP4XX || XPS_USB_HCD_XILINX || \
-				    PPC_MPC512x || PMC_MSP || SPARC_LEON || \
-				    MIPS_SEAD3)
-	default y
 
 config XPS_USB_HCD_XILINX
 	bool "Use Xilinx usb host EHCI controller core"
-	depends on USB_EHCI_HCD && (PPC32 || MICROBLAZE)
+	depends on (PPC32 || MICROBLAZE)
 	select USB_EHCI_BIG_ENDIAN_DESC
 	select USB_EHCI_BIG_ENDIAN_MMIO
 	---help---
@@ -136,12 +132,9 @@ config XPS_USB_HCD_XILINX
 		support both high speed and full speed devices, or high speed
 		devices only.
 
-config USB_FSL_MPH_DR_OF
-	tristate
-
 config USB_EHCI_FSL
 	bool "Support for Freescale PPC on-chip EHCI USB controller"
-	depends on USB_EHCI_HCD && FSL_SOC
+	depends on FSL_SOC
 	select USB_EHCI_ROOT_HUB_TT
 	select USB_FSL_MPH_DR_OF if OF
 	---help---
@@ -149,22 +142,53 @@ config USB_EHCI_FSL
 
 config USB_EHCI_MXC
 	tristate "Support for Freescale i.MX on-chip EHCI USB controller"
-	depends on USB_EHCI_HCD && ARCH_MXC
+	depends on ARCH_MXC
 	select USB_EHCI_ROOT_HUB_TT
 	---help---
 	  Variation of ARC USB block used in some Freescale chips.
 
 config USB_EHCI_HCD_OMAP
-	bool "EHCI support for OMAP3 and later chips"
-	depends on USB_EHCI_HCD && ARCH_OMAP
+	tristate "EHCI support for OMAP3 and later chips"
+	depends on ARCH_OMAP
 	default y
 	---help---
 	  Enables support for the on-chip EHCI controller on
 	  OMAP3 and later chips.
+	  If your system uses a PHY on the USB port, you will need to
+	  enable USB_PHY and the appropriate PHY driver as well. Most
+	  boards need the NOP_USB_XCEIV PHY driver.
+
+config USB_EHCI_HCD_ORION
+	tristate  "Support for Marvell EBU on-chip EHCI USB controller"
+	depends on USB_EHCI_HCD && PLAT_ORION
+	default y
+	---help---
+	  Enables support for the on-chip EHCI controller on Marvell's
+	  embedded ARM SoCs, including Orion, Kirkwood, Dove, Armada XP,
+	  Armada 370.  This is different from the EHCI implementation
+	  on Marvell's mobile PXA and MMP SoC, see "EHCI support for
+	  Marvell PXA/MMP USB controller" for those.
+
+config USB_EHCI_HCD_SPEAR
+        tristate "Support for ST SPEAr on-chip EHCI USB controller"
+        depends on USB_EHCI_HCD && PLAT_SPEAR
+        default y
+        ---help---
+          Enables support for the on-chip EHCI controller on
+          ST SPEAr chips.
+
+config USB_EHCI_HCD_AT91
+        tristate  "Support for Atmel on-chip EHCI USB controller"
+        depends on USB_EHCI_HCD && ARCH_AT91
+        default y
+        ---help---
+          Enables support for the on-chip EHCI controller on
+          Atmel chips.
 
 config USB_EHCI_MSM
-	bool "Support for MSM on-chip EHCI USB controller"
-	depends on USB_EHCI_HCD && ARCH_MSM
+	tristate "Support for Qualcomm QSD/MSM on-chip EHCI USB controller"
+	depends on ARCH_MSM
+	depends on USB_PHY
 	select USB_EHCI_ROOT_HUB_TT
 	select USB_MSM_OTG
 	---help---
@@ -177,15 +201,16 @@ config USB_EHCI_MSM
 
 config USB_EHCI_TEGRA
        boolean "NVIDIA Tegra HCD support"
-       depends on USB_EHCI_HCD && ARCH_TEGRA
+       depends on ARCH_TEGRA
        select USB_EHCI_ROOT_HUB_TT
+       select USB_PHY
        help
          This driver enables support for the internal USB Host Controllers
          found in NVIDIA Tegra SoCs. The controllers are EHCI compliant.
 
 config USB_EHCI_HCD_PPC_OF
 	bool "EHCI support for PPC USB controller on OF platform bus"
-	depends on USB_EHCI_HCD && PPC_OF
+	depends on PPC_OF
 	default y
 	---help---
 	  Enables support for the USB controller present on the PowerPC
@@ -193,35 +218,40 @@ config USB_EHCI_HCD_PPC_OF
 
 config USB_EHCI_SH
 	bool "EHCI support for SuperH USB controller"
-	depends on USB_EHCI_HCD && SUPERH
+	depends on SUPERH
 	---help---
 	  Enables support for the on-chip EHCI controller on the SuperH.
 	  If you use the PCI EHCI controller, this option is not necessary.
 
 config USB_EHCI_S5P
-       boolean "S5P EHCI support"
-       depends on USB_EHCI_HCD && PLAT_S5P
+       tristate "EHCI support for Samsung S5P/EXYNOS SoC Series"
+       depends on PLAT_S5P
        help
-	 Enable support for the S5P SOC's on-chip EHCI controller.
+	Enable support for the Samsung S5Pxxxx and Exynos3/4/5 SOC's
+	on-chip EHCI controller.
 
 config USB_EHCI_MV
-	bool "EHCI support for Marvell on-chip controller"
-	depends on USB_EHCI_HCD && (ARCH_PXA || ARCH_MMP)
+	bool "EHCI support for Marvell PXA/MMP USB controller"
+	depends on (ARCH_PXA || ARCH_MMP)
 	select USB_EHCI_ROOT_HUB_TT
 	---help---
 	  Enables support for Marvell (including PXA and MMP series) on-chip
 	  USB SPH and OTG controller. SPH is a single port host, and it can
 	  only be EHCI host. OTG is controller that can switch to host mode.
+	  Note that this driver will not work on Marvell's other EHCI
+	  controller used by the EBU-type SoCs including Orion, Kirkwood,
+	  Dova, Armada 370 and Armada XP. See "Support for Marvell EBU
+	  on-chip EHCI USB controller" for those.
 
 config USB_W90X900_EHCI
 	bool "W90X900(W90P910) EHCI support"
-	depends on USB_EHCI_HCD && ARCH_W90X900
+	depends on ARCH_W90X900
 	---help---
 		Enables support for the W90X900 USB controller
 
 config USB_CNS3XXX_EHCI
 	bool "Cavium CNS3XXX EHCI Module (DEPRECATED)"
-	depends on USB_EHCI_HCD && ARCH_CNS3XXX
+	depends on ARCH_CNS3XXX
 	select USB_EHCI_HCD_PLATFORM
 	---help---
 	  This option is deprecated now and the driver was removed, use
@@ -233,7 +263,7 @@ config USB_CNS3XXX_EHCI
 
 config USB_EHCI_ATH79
 	bool "EHCI support for AR7XXX/AR9XXX SoCs (DEPRECATED)"
-	depends on USB_EHCI_HCD && (SOC_AR71XX || SOC_AR724X || SOC_AR913X || SOC_AR933X)
+	depends on (SOC_AR71XX || SOC_AR724X || SOC_AR913X || SOC_AR933X)
 	select USB_EHCI_ROOT_HUB_TT
 	select USB_EHCI_HCD_PLATFORM
 	default y
@@ -244,9 +274,31 @@ config USB_EHCI_ATH79
 	  Enables support for the built-in EHCI controller present
 	  on the Atheros AR7XXX/AR9XXX SoCs.
 
+config USB_EHCI_HCD_PLATFORM
+	tristate "Generic EHCI driver for a platform device"
+	default n
+	---help---
+	  Adds an EHCI host driver for a generic platform device, which
+	  provides a memory space and an irq.
+
+	  If unsure, say N.
+
+config USB_OCTEON_EHCI
+	bool "Octeon on-chip EHCI support"
+	depends on CPU_CAVIUM_OCTEON
+	default n
+	select USB_EHCI_BIG_ENDIAN_MMIO
+	help
+	  Enable support for the Octeon II SOC's on-chip EHCI
+	  controller.  It is needed for high-speed (480Mbit/sec)
+	  USB 2.0 device support.  All CN6XXX based chips with USB are
+	  supported.
+
+endif # USB_EHCI_HCD
+
 config USB_OXU210HP_HCD
 	tristate "OXU210HP HCD support"
-	depends on USB && GENERIC_HARDIRQS
+	depends on GENERIC_HARDIRQS
 	---help---
 	  The OXU210HP is an USB host/OTG/device controller. Enable this
 	  option if your board has this chip. If unsure, say N.
@@ -259,7 +311,6 @@ config USB_OXU210HP_HCD
 
 config USB_ISP116X_HCD
 	tristate "ISP116X HCD support"
-	depends on USB
 	---help---
 	  The ISP1160 and ISP1161 chips are USB host controllers. Enable this
 	  option if your board has this chip. If unsure, say N.
@@ -271,7 +322,6 @@ config USB_ISP116X_HCD
 
 config USB_ISP1760_HCD
 	tristate "ISP 1760 HCD support"
-	depends on USB
 	---help---
 	  The ISP1760 chip is a USB 2.0 host controller.
 
@@ -286,7 +336,6 @@ config USB_ISP1760_HCD
 
 config USB_ISP1362_HCD
 	tristate "ISP1362 HCD support"
-	depends on USB
 	default N
 	---help---
 	  Supports the Philips ISP1362 chip as a host controller
@@ -298,9 +347,8 @@ config USB_ISP1362_HCD
 
 config USB_OHCI_HCD
 	tristate "OHCI HCD support"
-	depends on USB && USB_ARCH_HAS_OHCI
+	depends on USB_ARCH_HAS_OHCI
 	select ISP1301_OMAP if MACH_OMAP_H2 || MACH_OMAP_H3
-	select USB_OTG_UTILS if ARCH_OMAP
 	depends on USB_ISP1301 || !ARCH_LPC32XX
 	---help---
 	  The Open Host Controller Interface (OHCI) is a standard for accessing
@@ -315,16 +363,18 @@ config USB_OHCI_HCD
 	  To compile this driver as a module, choose M here: the
 	  module will be called ohci-hcd.
 
+if USB_OHCI_HCD
+
 config USB_OHCI_HCD_OMAP1
 	bool "OHCI support for OMAP1/2 chips"
-	depends on USB_OHCI_HCD && ARCH_OMAP1
+	depends on ARCH_OMAP1
 	default y
 	---help---
 	  Enables support for the OHCI controller on OMAP1/2 chips.
 
 config USB_OHCI_HCD_OMAP3
 	bool "OHCI support for OMAP3 and later chips"
-	depends on USB_OHCI_HCD && (ARCH_OMAP3 || ARCH_OMAP4)
+	depends on (ARCH_OMAP3 || ARCH_OMAP4)
 	default y
 	---help---
 	  Enables support for the on-chip OHCI controller on
@@ -332,7 +382,7 @@ config USB_OHCI_HCD_OMAP3
 
 config USB_OHCI_ATH79
 	bool "USB OHCI support for the Atheros AR71XX/AR7240 SoCs (DEPRECATED)"
-	depends on USB_OHCI_HCD && (SOC_AR71XX || SOC_AR724X)
+	depends on (SOC_AR71XX || SOC_AR724X)
 	select USB_OHCI_HCD_PLATFORM
 	default y
 	help
@@ -344,7 +394,7 @@ config USB_OHCI_ATH79
 
 config USB_OHCI_HCD_PPC_OF_BE
 	bool "OHCI support for OF platform bus (big endian)"
-	depends on USB_OHCI_HCD && PPC_OF
+	depends on PPC_OF
 	select USB_OHCI_BIG_ENDIAN_DESC
 	select USB_OHCI_BIG_ENDIAN_MMIO
 	---help---
@@ -353,7 +403,7 @@ config USB_OHCI_HCD_PPC_OF_BE
 
 config USB_OHCI_HCD_PPC_OF_LE
 	bool "OHCI support for OF platform bus (little endian)"
-	depends on USB_OHCI_HCD && PPC_OF
+	depends on PPC_OF
 	select USB_OHCI_LITTLE_ENDIAN
 	---help---
 	  Enables support for little-endian USB controllers present on the
@@ -361,12 +411,12 @@ config USB_OHCI_HCD_PPC_OF_LE
 
 config USB_OHCI_HCD_PPC_OF
 	bool
-	depends on USB_OHCI_HCD && PPC_OF
+	depends on PPC_OF
 	default USB_OHCI_HCD_PPC_OF_BE || USB_OHCI_HCD_PPC_OF_LE
 
 config USB_OHCI_HCD_PCI
 	bool "OHCI support for PCI-bus USB controllers"
-	depends on USB_OHCI_HCD && PCI && (STB03xxx || PPC_MPC52xx || USB_OHCI_HCD_PPC_OF)
+	depends on PCI && (STB03xxx || PPC_MPC52xx || USB_OHCI_HCD_PPC_OF)
 	default y
 	select USB_OHCI_LITTLE_ENDIAN
 	---help---
@@ -375,7 +425,7 @@ config USB_OHCI_HCD_PCI
 
 config USB_OHCI_HCD_SSB
 	bool "OHCI support for Broadcom SSB OHCI core (DEPRECATED)"
-	depends on USB_OHCI_HCD && (SSB = y || SSB = USB_OHCI_HCD)
+	depends on (SSB = y || SSB = USB_OHCI_HCD)
 	select USB_HCD_SSB
 	select USB_OHCI_HCD_PLATFORM
 	default n
@@ -393,7 +443,7 @@ config USB_OHCI_HCD_SSB
 
 config USB_OHCI_SH
 	bool "OHCI support for SuperH USB controller (DEPRECATED)"
-	depends on USB_OHCI_HCD && SUPERH
+	depends on SUPERH
 	select USB_OHCI_HCD_PLATFORM
 	---help---
 	  This option is deprecated now and the driver was removed, use
@@ -404,13 +454,13 @@ config USB_OHCI_SH
 
 config USB_OHCI_EXYNOS
 	boolean "OHCI support for Samsung EXYNOS SoC Series"
-	depends on USB_OHCI_HCD && ARCH_EXYNOS
+	depends on ARCH_EXYNOS
 	help
 	 Enable support for the Samsung Exynos SOC's on-chip OHCI controller.
 
 config USB_CNS3XXX_OHCI
 	bool "Cavium CNS3XXX OHCI Module (DEPRECATED)"
-	depends on USB_OHCI_HCD && ARCH_CNS3XXX
+	depends on ARCH_CNS3XXX
 	select USB_OHCI_HCD_PLATFORM
 	---help---
 	  This option is deprecated now and the driver was removed, use
@@ -421,7 +471,6 @@ config USB_CNS3XXX_OHCI
 
 config USB_OHCI_HCD_PLATFORM
 	bool "Generic OHCI driver for a platform device"
-	depends on USB_OHCI_HCD
 	default n
 	---help---
 	  Adds an OHCI host driver for a generic platform device, which
@@ -429,35 +478,36 @@ config USB_OHCI_HCD_PLATFORM
 
 	  If unsure, say N.
 
-config USB_EHCI_HCD_PLATFORM
-	tristate "Generic EHCI driver for a platform device"
-	depends on USB_EHCI_HCD
-	default n
-	---help---
-	  Adds an EHCI host driver for a generic platform device, which
-	  provides a memory space and an irq.
+config USB_OCTEON_OHCI
+	bool "Octeon on-chip OHCI support"
+	depends on CPU_CAVIUM_OCTEON
+	default USB_OCTEON_EHCI
+	select USB_OHCI_BIG_ENDIAN_MMIO
+	select USB_OHCI_LITTLE_ENDIAN
+	help
+	  Enable support for the Octeon II SOC's on-chip OHCI
+	  controller.  It is needed for low-speed USB 1.0 device
+	  support.  All CN6XXX based chips with USB are supported.
 
-	  If unsure, say N.
 
 config USB_OHCI_BIG_ENDIAN_DESC
 	bool
-	depends on USB_OHCI_HCD
 	default n
 
 config USB_OHCI_BIG_ENDIAN_MMIO
 	bool
-	depends on USB_OHCI_HCD
 	default n
 
 config USB_OHCI_LITTLE_ENDIAN
 	bool
-	depends on USB_OHCI_HCD
 	default n if STB03xxx || PPC_MPC52xx
 	default y
 
+endif # USB_OHCI_HCD
+
 config USB_UHCI_HCD
 	tristate "UHCI HCD (most Intel and VIA) support"
-	depends on USB && (PCI || SPARC_LEON || ARCH_VT8500)
+	depends on PCI || SPARC_LEON || ARCH_VT8500
 	---help---
 	  The Universal Host Controller Interface is a standard by Intel for
 	  accessing the USB hardware in the PC (which is also called the USB
@@ -497,7 +547,7 @@ config USB_UHCI_BIG_ENDIAN_DESC
 
 config USB_FHCI_HCD
 	tristate "Freescale QE USB Host Controller support"
-	depends on USB && OF_GPIO && QE_GPIO && QUICC_ENGINE
+	depends on OF_GPIO && QE_GPIO && QUICC_ENGINE
 	select FSL_GTM
 	select QE_USB
 	help
@@ -514,7 +564,7 @@ config FHCI_DEBUG
 
 config USB_U132_HCD
 	tristate "Elan U132 Adapter Host Controller"
-	depends on USB && USB_FTDI_ELAN
+	depends on USB_FTDI_ELAN
 	default M
 	help
 	  The U132 adapter is a USB to CardBus adapter specifically designed
@@ -542,7 +592,6 @@ config USB_U132_HCD
 
 config USB_SL811_HCD
 	tristate "SL811HS HCD support"
-	depends on USB
 	help
 	  The SL811HS is a single-port USB controller that supports either
 	  host side or peripheral side roles.  Enable this option if your
@@ -574,7 +623,6 @@ config USB_SL811_CS
 
 config USB_R8A66597_HCD
 	tristate "R8A66597 HCD support"
-	depends on USB
 	help
 	  The R8A66597 is a USB 2.0 host and peripheral controller.
 
@@ -586,7 +634,6 @@ config USB_R8A66597_HCD
 
 config USB_RENESAS_USBHS_HCD
 	tristate "Renesas USBHS HCD support"
-	depends on USB
 	depends on USB_RENESAS_USBHS
 	help
 	  The Renesas USBHS is a USB 2.0 host and peripheral controller.
@@ -611,7 +658,7 @@ config USB_WHCI_HCD
 
 config USB_HWA_HCD
 	tristate "Host Wire Adapter (HWA) driver"
-	depends on USB && UWB
+	depends on UWB
 	select USB_WUSB
 	select UWB_HWA
 	help
@@ -625,7 +672,7 @@ config USB_HWA_HCD
 
 config USB_IMX21_HCD
        tristate "i.MX21 HCD support"
-       depends on USB && ARM && ARCH_MXC
+       depends on ARM && ARCH_MXC
        help
          This driver enables support for the on-chip USB host in the
          i.MX21 processor.
@@ -633,27 +680,7 @@ config USB_IMX21_HCD
          To compile this driver as a module, choose M here: the
          module will be called "imx21-hcd".
 
-config USB_OCTEON_EHCI
-	bool "Octeon on-chip EHCI support"
-	depends on USB && USB_EHCI_HCD && CPU_CAVIUM_OCTEON
-	default n
-	select USB_EHCI_BIG_ENDIAN_MMIO
-	help
-	  Enable support for the Octeon II SOC's on-chip EHCI
-	  controller.  It is needed for high-speed (480Mbit/sec)
-	  USB 2.0 device support.  All CN6XXX based chips with USB are
-	  supported.
 
-config USB_OCTEON_OHCI
-	bool "Octeon on-chip OHCI support"
-	depends on USB && USB_OHCI_HCD && CPU_CAVIUM_OCTEON
-	default USB_OCTEON_EHCI
-	select USB_OHCI_BIG_ENDIAN_MMIO
-	select USB_OHCI_LITTLE_ENDIAN
-	help
-	  Enable support for the Octeon II SOC's on-chip OHCI
-	  controller.  It is needed for low-speed USB 1.0 device
-	  support.  All CN6XXX based chips with USB are supported.
 
 config USB_OCTEON2_COMMON
 	bool
diff --git a/drivers/usb/host/Makefile b/drivers/usb/host/Makefile
index 001fbff2fde..4fb73c156d7 100644
--- a/drivers/usb/host/Makefile
+++ b/drivers/usb/host/Makefile
@@ -27,6 +27,12 @@ obj-$(CONFIG_USB_EHCI_HCD)	+= ehci-hcd.o
 obj-$(CONFIG_USB_EHCI_PCI)	+= ehci-pci.o
 obj-$(CONFIG_USB_EHCI_HCD_PLATFORM)	+= ehci-platform.o
 obj-$(CONFIG_USB_EHCI_MXC)	+= ehci-mxc.o
+obj-$(CONFIG_USB_EHCI_HCD_OMAP)	+= ehci-omap.o
+obj-$(CONFIG_USB_EHCI_HCD_ORION)	+= ehci-orion.o
+obj-$(CONFIG_USB_EHCI_HCD_SPEAR)	+= ehci-spear.o
+obj-$(CONFIG_USB_EHCI_S5P)	+= ehci-s5p.o
+obj-$(CONFIG_USB_EHCI_HCD_AT91) += ehci-atmel.o
+obj-$(CONFIG_USB_EHCI_MSM)	+= ehci-msm.o
 
 obj-$(CONFIG_USB_OXU210HP_HCD)	+= oxu210hp-hcd.o
 obj-$(CONFIG_USB_ISP116X_HCD)	+= isp116x-hcd.o
diff --git a/drivers/usb/host/ehci-atmel.c b/drivers/usb/host/ehci-atmel.c
index f3beac4d06b..66420097c24 100644
--- a/drivers/usb/host/ehci-atmel.c
+++ b/drivers/usb/host/ehci-atmel.c
@@ -12,9 +12,22 @@
  */
 
 #include <linux/clk.h>
-#include <linux/platform_device.h>
+#include <linux/dma-mapping.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
 #include <linux/of.h>
 #include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/usb.h>
+#include <linux/usb/hcd.h>
+
+#include "ehci.h"
+
+#define DRIVER_DESC "EHCI Atmel driver"
+
+static const char hcd_name[] = "ehci-atmel";
+static struct hc_driver __read_mostly ehci_atmel_hc_driver;
 
 /* interface and function clocks */
 static struct clk *iclk, *fclk;
@@ -50,51 +63,6 @@ static void atmel_stop_ehci(struct platform_device *pdev)
 
 /*-------------------------------------------------------------------------*/
 
-static int ehci_atmel_setup(struct usb_hcd *hcd)
-{
-	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
-
-	/* registers start at offset 0x0 */
-	ehci->caps = hcd->regs;
-
-	return ehci_setup(hcd);
-}
-
-static const struct hc_driver ehci_atmel_hc_driver = {
-	.description		= hcd_name,
-	.product_desc		= "Atmel EHCI UHP HS",
-	.hcd_priv_size		= sizeof(struct ehci_hcd),
-
-	/* generic hardware linkage */
-	.irq			= ehci_irq,
-	.flags			= HCD_MEMORY | HCD_USB2,
-
-	/* basic lifecycle operations */
-	.reset			= ehci_atmel_setup,
-	.start			= ehci_run,
-	.stop			= ehci_stop,
-	.shutdown		= ehci_shutdown,
-
-	/* managing i/o requests and associated device resources */
-	.urb_enqueue		= ehci_urb_enqueue,
-	.urb_dequeue		= ehci_urb_dequeue,
-	.endpoint_disable	= ehci_endpoint_disable,
-	.endpoint_reset		= ehci_endpoint_reset,
-
-	/* scheduling support */
-	.get_frame_number	= ehci_get_frame,
-
-	/* root hub support */
-	.hub_status_data	= ehci_hub_status_data,
-	.hub_control		= ehci_hub_control,
-	.bus_suspend		= ehci_bus_suspend,
-	.bus_resume		= ehci_bus_resume,
-	.relinquish_port	= ehci_relinquish_port,
-	.port_handed_over	= ehci_port_handed_over,
-
-	.clear_tt_buffer_complete	= ehci_clear_tt_buffer_complete,
-};
-
 static u64 at91_ehci_dma_mask = DMA_BIT_MASK(32);
 
 static int ehci_atmel_drv_probe(struct platform_device *pdev)
@@ -102,6 +70,7 @@ static int ehci_atmel_drv_probe(struct platform_device *pdev)
 	struct usb_hcd *hcd;
 	const struct hc_driver *driver = &ehci_atmel_hc_driver;
 	struct resource *res;
+	struct ehci_hcd *ehci;
 	int irq;
 	int retval;
 
@@ -162,6 +131,10 @@ static int ehci_atmel_drv_probe(struct platform_device *pdev)
 		goto fail_request_resource;
 	}
 
+	ehci = hcd_to_ehci(hcd);
+	/* registers start at offset 0x0 */
+	ehci->caps = hcd->regs;
+
 	atmel_start_ehci(pdev);
 
 	retval = usb_add_hcd(hcd, irq, IRQF_SHARED);
@@ -185,7 +158,6 @@ static int ehci_atmel_drv_remove(struct platform_device *pdev)
 {
 	struct usb_hcd *hcd = platform_get_drvdata(pdev);
 
-	ehci_shutdown(hcd);
 	usb_remove_hcd(hcd);
 	usb_put_hcd(hcd);
 
@@ -213,3 +185,25 @@ static struct platform_driver ehci_atmel_driver = {
 		.of_match_table	= of_match_ptr(atmel_ehci_dt_ids),
 	},
 };
+
+static int __init ehci_atmel_init(void)
+{
+	if (usb_disabled())
+		return -ENODEV;
+
+	pr_info("%s: " DRIVER_DESC "\n", hcd_name);
+	ehci_init_driver(&ehci_atmel_hc_driver, NULL);
+	return platform_driver_register(&ehci_atmel_driver);
+}
+module_init(ehci_atmel_init);
+
+static void __exit ehci_atmel_cleanup(void)
+{
+	platform_driver_unregister(&ehci_atmel_driver);
+}
+module_exit(ehci_atmel_cleanup);
+
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_ALIAS("platform:atmel-ehci");
+MODULE_AUTHOR("Nicolas Ferre");
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/host/ehci-dbg.c b/drivers/usb/host/ehci-dbg.c
index 70b496dc18a..5429d2645bb 100644
--- a/drivers/usb/host/ehci-dbg.c
+++ b/drivers/usb/host/ehci-dbg.c
@@ -510,14 +510,16 @@ static ssize_t fill_async_buffer(struct debug_buffer *buf)
 	spin_lock_irqsave (&ehci->lock, flags);
 	for (qh = ehci->async->qh_next.qh; size > 0 && qh; qh = qh->qh_next.qh)
 		qh_lines (ehci, qh, &next, &size);
-	if (ehci->async_unlink && size > 0) {
+	if (!list_empty(&ehci->async_unlink) && size > 0) {
 		temp = scnprintf(next, size, "\nunlink =\n");
 		size -= temp;
 		next += temp;
 
-		for (qh = ehci->async_unlink; size > 0 && qh;
-				qh = qh->unlink_next)
-			qh_lines (ehci, qh, &next, &size);
+		list_for_each_entry(qh, &ehci->async_unlink, unlink_node) {
+			if (size <= 0)
+				break;
+			qh_lines(ehci, qh, &next, &size);
+		}
 	}
 	spin_unlock_irqrestore (&ehci->lock, flags);
 
@@ -814,9 +816,10 @@ static ssize_t fill_registers_buffer(struct debug_buffer *buf)
 		}
 	}
 
-	if (ehci->async_unlink) {
+	if (!list_empty(&ehci->async_unlink)) {
 		temp = scnprintf(next, size, "async unlink qh %p\n",
-				ehci->async_unlink);
+				list_first_entry(&ehci->async_unlink,
+						struct ehci_qh, unlink_node));
 		size -= temp;
 		next += temp;
 	}
diff --git a/drivers/usb/host/ehci-fsl.c b/drivers/usb/host/ehci-fsl.c
index d81d2fcbff1..3be3df233a0 100644
--- a/drivers/usb/host/ehci-fsl.c
+++ b/drivers/usb/host/ehci-fsl.c
@@ -370,6 +370,15 @@ static int ehci_fsl_setup(struct usb_hcd *hcd)
 	/* EHCI registers start at offset 0x100 */
 	ehci->caps = hcd->regs + 0x100;
 
+#ifdef CONFIG_PPC_83xx
+	/*
+	 * Deal with MPC834X that need port power to be cycled after the power
+	 * fault condition is removed. Otherwise the state machine does not
+	 * reflect PORTSC[CSC] correctly.
+	 */
+	ehci->need_oc_pp_cycle = 1;
+#endif
+
 	hcd->has_tt = 1;
 
 	retval = ehci_setup(hcd);
diff --git a/drivers/usb/host/ehci-hcd.c b/drivers/usb/host/ehci-hcd.c
index 416a6dce5e1..312fc10da3c 100644
--- a/drivers/usb/host/ehci-hcd.c
+++ b/drivers/usb/host/ehci-hcd.c
@@ -482,6 +482,9 @@ static int ehci_init(struct usb_hcd *hcd)
 	 * periodic_size can shrink by USBCMD update if hcc_params allows.
 	 */
 	ehci->periodic_size = DEFAULT_I_TDPS;
+	INIT_LIST_HEAD(&ehci->async_unlink);
+	INIT_LIST_HEAD(&ehci->async_idle);
+	INIT_LIST_HEAD(&ehci->intr_unlink);
 	INIT_LIST_HEAD(&ehci->intr_qh_list);
 	INIT_LIST_HEAD(&ehci->cached_itd_list);
 	INIT_LIST_HEAD(&ehci->cached_sitd_list);
@@ -670,9 +673,6 @@ int ehci_setup(struct usb_hcd *hcd)
 	if (retval)
 		return retval;
 
-	if (ehci_is_TDI(ehci))
-		tdi_reset(ehci);
-
 	ehci_reset(ehci);
 
 	return 0;
@@ -749,7 +749,7 @@ static irqreturn_t ehci_irq (struct usb_hcd *hcd)
 		/* guard against (alleged) silicon errata */
 		if (cmd & CMD_IAAD)
 			ehci_dbg(ehci, "IAA with IAAD still set?\n");
-		if (ehci->async_iaa)
+		if (ehci->iaa_in_progress)
 			COUNT(ehci->stats.iaa);
 		end_unlink_async(ehci);
 	}
@@ -757,7 +757,7 @@ static irqreturn_t ehci_irq (struct usb_hcd *hcd)
 	/* remote wakeup [4.3.1] */
 	if (status & STS_PCD) {
 		unsigned	i = HCS_N_PORTS (ehci->hcs_params);
-		u32		ppcd = 0;
+		u32		ppcd = ~0;
 
 		/* kick root hub later */
 		pcd_status = status;
@@ -774,7 +774,7 @@ static irqreturn_t ehci_irq (struct usb_hcd *hcd)
 			int pstatus;
 
 			/* leverage per-port change bits feature */
-			if (ehci->has_ppcd && !(ppcd & (1 << i)))
+			if (!(ppcd & (1 << i)))
 				continue;
 			pstatus = ehci_readl(ehci,
 					 &ehci->regs->port_status[i]);
@@ -896,17 +896,24 @@ static int ehci_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
 	if (rc)
 		goto done;
 
-	switch (usb_pipetype (urb->pipe)) {
-	// case PIPE_CONTROL:
-	// case PIPE_BULK:
-	default:
+	if (usb_pipetype(urb->pipe) == PIPE_ISOCHRONOUS) {
+		/*
+		 * We don't expedite dequeue for isochronous URBs.
+		 * Just wait until they complete normally or their
+		 * time slot expires.
+		 */
+	} else {
 		qh = (struct ehci_qh *) urb->hcpriv;
-		if (!qh)
-			break;
+		qh->exception = 1;
 		switch (qh->qh_state) {
 		case QH_STATE_LINKED:
+			if (usb_pipetype(urb->pipe) == PIPE_INTERRUPT)
+				start_unlink_intr(ehci, qh);
+			else
+				start_unlink_async(ehci, qh);
+			break;
 		case QH_STATE_COMPLETING:
-			start_unlink_async(ehci, qh);
+			qh->dequeue_during_giveback = 1;
 			break;
 		case QH_STATE_UNLINK:
 		case QH_STATE_UNLINK_WAIT:
@@ -917,33 +924,6 @@ static int ehci_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
 			qh_completions(ehci, qh);
 			break;
 		}
-		break;
-
-	case PIPE_INTERRUPT:
-		qh = (struct ehci_qh *) urb->hcpriv;
-		if (!qh)
-			break;
-		switch (qh->qh_state) {
-		case QH_STATE_LINKED:
-		case QH_STATE_COMPLETING:
-			start_unlink_intr(ehci, qh);
-			break;
-		case QH_STATE_IDLE:
-			qh_completions (ehci, qh);
-			break;
-		default:
-			ehci_dbg (ehci, "bogus qh %p state %d\n",
-					qh, qh->qh_state);
-			goto done;
-		}
-		break;
-
-	case PIPE_ISOCHRONOUS:
-		// itd or sitd ...
-
-		// wait till next completion, do it then.
-		// completion irqs can wait up to 1024 msec,
-		break;
 	}
 done:
 	spin_unlock_irqrestore (&ehci->lock, flags);
@@ -984,6 +964,7 @@ rescan:
 		goto done;
 	}
 
+	qh->exception = 1;
 	if (ehci->rh_state < EHCI_RH_RUNNING)
 		qh->qh_state = QH_STATE_IDLE;
 	switch (qh->qh_state) {
@@ -1052,13 +1033,12 @@ ehci_endpoint_reset(struct usb_hcd *hcd, struct usb_host_endpoint *ep)
 		usb_settoggle(qh->dev, epnum, is_out, 0);
 		if (!list_empty(&qh->qtd_list)) {
 			WARN_ONCE(1, "clear_halt for a busy endpoint\n");
-		} else if (qh->qh_state == QH_STATE_LINKED ||
-				qh->qh_state == QH_STATE_COMPLETING) {
-
+		} else {
 			/* The toggle value in the QH can't be updated
 			 * while the QH is active.  Unlink it now;
 			 * re-linking will call qh_refresh().
 			 */
+			qh->exception = 1;
 			if (eptype == USB_ENDPOINT_XFER_BULK)
 				start_unlink_async(ehci, qh);
 			else
@@ -1251,11 +1231,6 @@ MODULE_LICENSE ("GPL");
 #define PLATFORM_DRIVER		ehci_hcd_sh_driver
 #endif
 
-#ifdef CONFIG_USB_EHCI_HCD_OMAP
-#include "ehci-omap.c"
-#define        PLATFORM_DRIVER         ehci_hcd_omap_driver
-#endif
-
 #ifdef CONFIG_PPC_PS3
 #include "ehci-ps3.c"
 #define	PS3_SYSTEM_BUS_DRIVER	ps3_ehci_driver
@@ -1271,41 +1246,16 @@ MODULE_LICENSE ("GPL");
 #define XILINX_OF_PLATFORM_DRIVER	ehci_hcd_xilinx_of_driver
 #endif
 
-#ifdef CONFIG_PLAT_ORION
-#include "ehci-orion.c"
-#define	PLATFORM_DRIVER		ehci_orion_driver
-#endif
-
 #ifdef CONFIG_USB_W90X900_EHCI
 #include "ehci-w90x900.c"
 #define	PLATFORM_DRIVER		ehci_hcd_w90x900_driver
 #endif
 
-#ifdef CONFIG_ARCH_AT91
-#include "ehci-atmel.c"
-#define	PLATFORM_DRIVER		ehci_atmel_driver
-#endif
-
 #ifdef CONFIG_USB_OCTEON_EHCI
 #include "ehci-octeon.c"
 #define PLATFORM_DRIVER		ehci_octeon_driver
 #endif
 
-#ifdef CONFIG_ARCH_VT8500
-#include "ehci-vt8500.c"
-#define	PLATFORM_DRIVER		vt8500_ehci_driver
-#endif
-
-#ifdef CONFIG_PLAT_SPEAR
-#include "ehci-spear.c"
-#define PLATFORM_DRIVER		spear_ehci_hcd_driver
-#endif
-
-#ifdef CONFIG_USB_EHCI_MSM
-#include "ehci-msm.c"
-#define PLATFORM_DRIVER		ehci_msm_driver
-#endif
-
 #ifdef CONFIG_TILE_USB
 #include "ehci-tilegx.c"
 #define	PLATFORM_DRIVER		ehci_hcd_tilegx_driver
@@ -1321,11 +1271,6 @@ MODULE_LICENSE ("GPL");
 #define PLATFORM_DRIVER		tegra_ehci_driver
 #endif
 
-#ifdef CONFIG_USB_EHCI_S5P
-#include "ehci-s5p.c"
-#define PLATFORM_DRIVER		s5p_ehci_driver
-#endif
-
 #ifdef CONFIG_SPARC_LEON
 #include "ehci-grlib.c"
 #define PLATFORM_DRIVER		ehci_grlib_driver
@@ -1345,6 +1290,12 @@ MODULE_LICENSE ("GPL");
 	!IS_ENABLED(CONFIG_USB_EHCI_HCD_PLATFORM) && \
 	!IS_ENABLED(CONFIG_USB_CHIPIDEA_HOST) && \
 	!IS_ENABLED(CONFIG_USB_EHCI_MXC) && \
+	!IS_ENABLED(CONFIG_USB_EHCI_HCD_OMAP) && \
+	!IS_ENABLED(CONFIG_USB_EHCI_HCD_ORION) && \
+	!IS_ENABLED(CONFIG_USB_EHCI_HCD_SPEAR) && \
+	!IS_ENABLED(CONFIG_USB_EHCI_S5P) && \
+	!IS_ENABLED(CONFIG_USB_EHCI_HCD_AT91) && \
+	!IS_ENABLED(CONFIG_USB_EHCI_MSM) && \
 	!defined(PLATFORM_DRIVER) && \
 	!defined(PS3_SYSTEM_BUS_DRIVER) && \
 	!defined(OF_PLATFORM_DRIVER) && \
diff --git a/drivers/usb/host/ehci-hub.c b/drivers/usb/host/ehci-hub.c
index 7d06e77f6c4..9ab4a4d9768 100644
--- a/drivers/usb/host/ehci-hub.c
+++ b/drivers/usb/host/ehci-hub.c
@@ -464,7 +464,7 @@ static int ehci_bus_resume (struct usb_hcd *hcd)
 	while (i--) {
 		temp = ehci_readl(ehci, &ehci->regs->port_status [i]);
 		if (test_bit(i, &resume_needed)) {
-			temp &= ~(PORT_RWC_BITS | PORT_RESUME);
+			temp &= ~(PORT_RWC_BITS | PORT_SUSPEND | PORT_RESUME);
 			ehci_writel(ehci, temp, &ehci->regs->port_status [i]);
 			ehci_vdbg (ehci, "resumed port %d\n", i + 1);
 		}
@@ -590,7 +590,7 @@ ehci_hub_status_data (struct usb_hcd *hcd, char *buf)
 	u32		mask;
 	int		ports, i, retval = 1;
 	unsigned long	flags;
-	u32		ppcd = 0;
+	u32		ppcd = ~0;
 
 	/* init status to no-changes */
 	buf [0] = 0;
@@ -628,9 +628,10 @@ ehci_hub_status_data (struct usb_hcd *hcd, char *buf)
 
 	for (i = 0; i < ports; i++) {
 		/* leverage per-port change bits feature */
-		if (ehci->has_ppcd && !(ppcd & (1 << i)))
-			continue;
-		temp = ehci_readl(ehci, &ehci->regs->port_status [i]);
+		if (ppcd & (1 << i))
+			temp = ehci_readl(ehci, &ehci->regs->port_status[i]);
+		else
+			temp = 0;
 
 		/*
 		 * Return status information even for ports with OWNER set.
@@ -839,7 +840,8 @@ static int ehci_hub_control (
 			 * power switching; they're allowed to just limit the
 			 * current.  khubd will turn the power back on.
 			 */
-			if ((temp & PORT_OC) && HCS_PPC(ehci->hcs_params)) {
+			if (((temp & PORT_OC) || (ehci->need_oc_pp_cycle))
+					&& HCS_PPC(ehci->hcs_params)) {
 				ehci_writel(ehci,
 					temp & ~(PORT_RWC_BITS | PORT_POWER),
 					status_reg);
@@ -870,10 +872,9 @@ static int ehci_hub_control (
 				usb_hcd_end_port_resume(&hcd->self, wIndex);
 
 				/* stop resume signaling */
-				temp = ehci_readl(ehci, status_reg);
-				ehci_writel(ehci,
-					temp & ~(PORT_RWC_BITS | PORT_RESUME),
-					status_reg);
+				temp &= ~(PORT_RWC_BITS |
+						PORT_SUSPEND | PORT_RESUME);
+				ehci_writel(ehci, temp, status_reg);
 				clear_bit(wIndex, &ehci->resuming_ports);
 				retval = handshake(ehci, status_reg,
 					   PORT_RESUME, 0, 2000 /* 2msec */);
@@ -883,7 +884,7 @@ static int ehci_hub_control (
 						wIndex + 1, retval);
 					goto error;
 				}
-				temp &= ~(PORT_SUSPEND|PORT_RESUME|(3<<10));
+				temp = ehci_readl(ehci, status_reg);
 			}
 		}
 
diff --git a/drivers/usb/host/ehci-msm.c b/drivers/usb/host/ehci-msm.c
index 88a49c87e74..0f717dc688b 100644
--- a/drivers/usb/host/ehci-msm.c
+++ b/drivers/usb/host/ehci-msm.c
@@ -22,16 +22,26 @@
  * along with this program; if not, you can find it at http://www.fsf.org
  */
 
-#include <linux/platform_device.h>
 #include <linux/clk.h>
 #include <linux/err.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
-
 #include <linux/usb/otg.h>
 #include <linux/usb/msm_hsusb_hw.h>
+#include <linux/usb.h>
+#include <linux/usb/hcd.h>
+
+#include "ehci.h"
 
 #define MSM_USB_BASE (hcd->regs)
 
+#define DRIVER_DESC "Qualcomm On-Chip EHCI Host Controller"
+
+static const char hcd_name[] = "ehci-msm";
+static struct hc_driver __read_mostly msm_hc_driver;
 static struct usb_phy *phy;
 
 static int ehci_msm_reset(struct usb_hcd *hcd)
@@ -56,52 +66,6 @@ static int ehci_msm_reset(struct usb_hcd *hcd)
 	return 0;
 }
 
-static struct hc_driver msm_hc_driver = {
-	.description		= hcd_name,
-	.product_desc		= "Qualcomm On-Chip EHCI Host Controller",
-	.hcd_priv_size		= sizeof(struct ehci_hcd),
-
-	/*
-	 * generic hardware linkage
-	 */
-	.irq			= ehci_irq,
-	.flags			= HCD_USB2 | HCD_MEMORY,
-
-	.reset			= ehci_msm_reset,
-	.start			= ehci_run,
-
-	.stop			= ehci_stop,
-	.shutdown		= ehci_shutdown,
-
-	/*
-	 * managing i/o requests and associated device resources
-	 */
-	.urb_enqueue		= ehci_urb_enqueue,
-	.urb_dequeue		= ehci_urb_dequeue,
-	.endpoint_disable	= ehci_endpoint_disable,
-	.endpoint_reset		= ehci_endpoint_reset,
-	.clear_tt_buffer_complete = ehci_clear_tt_buffer_complete,
-
-	/*
-	 * scheduling support
-	 */
-	.get_frame_number	= ehci_get_frame,
-
-	/*
-	 * root hub support
-	 */
-	.hub_status_data	= ehci_hub_status_data,
-	.hub_control		= ehci_hub_control,
-	.relinquish_port	= ehci_relinquish_port,
-	.port_handed_over	= ehci_port_handed_over,
-
-	/*
-	 * PM support
-	 */
-	.bus_suspend		= ehci_bus_suspend,
-	.bus_resume		= ehci_bus_resume,
-};
-
 static int ehci_msm_probe(struct platform_device *pdev)
 {
 	struct usb_hcd *hcd;
@@ -145,7 +109,7 @@ static int ehci_msm_probe(struct platform_device *pdev)
 	 * management.
 	 */
 	phy = devm_usb_get_phy(&pdev->dev, USB_PHY_TYPE_USB2);
-	if (IS_ERR_OR_NULL(phy)) {
+	if (IS_ERR(phy)) {
 		dev_err(&pdev->dev, "unable to find transceiver\n");
 		ret = -ENODEV;
 		goto put_hcd;
@@ -165,6 +129,8 @@ static int ehci_msm_probe(struct platform_device *pdev)
 	pm_runtime_no_callbacks(&pdev->dev);
 	pm_runtime_enable(&pdev->dev);
 
+	/* FIXME: need to call usb_add_hcd() here? */
+
 	return 0;
 
 put_hcd:
@@ -183,6 +149,8 @@ static int ehci_msm_remove(struct platform_device *pdev)
 
 	otg_set_host(phy->otg, NULL);
 
+	/* FIXME: need to call usb_remove_hcd() here? */
+
 	usb_put_hcd(hcd);
 
 	return 0;
@@ -226,3 +194,28 @@ static struct platform_driver ehci_msm_driver = {
 		   .pm = &ehci_msm_dev_pm_ops,
 	},
 };
+
+static const struct ehci_driver_overrides msm_overrides __initdata = {
+	.reset = ehci_msm_reset,
+};
+
+static int __init ehci_msm_init(void)
+{
+	if (usb_disabled())
+		return -ENODEV;
+
+	pr_info("%s: " DRIVER_DESC "\n", hcd_name);
+	ehci_init_driver(&msm_hc_driver, &msm_overrides);
+	return platform_driver_register(&ehci_msm_driver);
+}
+module_init(ehci_msm_init);
+
+static void __exit ehci_msm_cleanup(void)
+{
+	platform_driver_unregister(&ehci_msm_driver);
+}
+module_exit(ehci_msm_cleanup);
+
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_ALIAS("platform:msm-ehci");
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/host/ehci-mv.c b/drivers/usb/host/ehci-mv.c
index 3065809546b..402062973f0 100644
--- a/drivers/usb/host/ehci-mv.c
+++ b/drivers/usb/host/ehci-mv.c
@@ -33,25 +33,17 @@ struct ehci_hcd_mv {
 
 	struct mv_usb_platform_data *pdata;
 
-	/* clock source and total clock number */
-	unsigned int clknum;
-	struct clk *clk[0];
+	struct clk *clk;
 };
 
 static void ehci_clock_enable(struct ehci_hcd_mv *ehci_mv)
 {
-	unsigned int i;
-
-	for (i = 0; i < ehci_mv->clknum; i++)
-		clk_prepare_enable(ehci_mv->clk[i]);
+	clk_prepare_enable(ehci_mv->clk);
 }
 
 static void ehci_clock_disable(struct ehci_hcd_mv *ehci_mv)
 {
-	unsigned int i;
-
-	for (i = 0; i < ehci_mv->clknum; i++)
-		clk_disable_unprepare(ehci_mv->clk[i]);
+	clk_disable_unprepare(ehci_mv->clk);
 }
 
 static int mv_ehci_enable(struct ehci_hcd_mv *ehci_mv)
@@ -144,9 +136,8 @@ static int mv_ehci_probe(struct platform_device *pdev)
 	struct ehci_hcd *ehci;
 	struct ehci_hcd_mv *ehci_mv;
 	struct resource *r;
-	int clk_i, retval = -ENODEV;
+	int retval = -ENODEV;
 	u32 offset;
-	size_t size;
 
 	if (!pdata) {
 		dev_err(&pdev->dev, "missing platform_data\n");
@@ -160,8 +151,7 @@ static int mv_ehci_probe(struct platform_device *pdev)
 	if (!hcd)
 		return -ENOMEM;
 
-	size = sizeof(*ehci_mv) + sizeof(struct clk *) * pdata->clknum;
-	ehci_mv = devm_kzalloc(&pdev->dev, size, GFP_KERNEL);
+	ehci_mv = devm_kzalloc(&pdev->dev, sizeof(*ehci_mv), GFP_KERNEL);
 	if (ehci_mv == NULL) {
 		dev_err(&pdev->dev, "cannot allocate ehci_hcd_mv\n");
 		retval = -ENOMEM;
@@ -172,16 +162,11 @@ static int mv_ehci_probe(struct platform_device *pdev)
 	ehci_mv->pdata = pdata;
 	ehci_mv->hcd = hcd;
 
-	ehci_mv->clknum = pdata->clknum;
-	for (clk_i = 0; clk_i < ehci_mv->clknum; clk_i++) {
-		ehci_mv->clk[clk_i] =
-		    devm_clk_get(&pdev->dev, pdata->clkname[clk_i]);
-		if (IS_ERR(ehci_mv->clk[clk_i])) {
-			dev_err(&pdev->dev, "error get clck \"%s\"\n",
-				pdata->clkname[clk_i]);
-			retval = PTR_ERR(ehci_mv->clk[clk_i]);
-			goto err_clear_drvdata;
-		}
+	ehci_mv->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(ehci_mv->clk)) {
+		dev_err(&pdev->dev, "error getting clock\n");
+		retval = PTR_ERR(ehci_mv->clk);
+		goto err_clear_drvdata;
 	}
 
 	r = platform_get_resource_byname(pdev, IORESOURCE_MEM, "phyregs");
@@ -225,7 +210,7 @@ static int mv_ehci_probe(struct platform_device *pdev)
 		(void __iomem *) ((unsigned long) ehci_mv->cap_regs + offset);
 
 	hcd->rsrc_start = r->start;
-	hcd->rsrc_len = r->end - r->start + 1;
+	hcd->rsrc_len = resource_size(r);
 	hcd->regs = ehci_mv->op_regs;
 
 	hcd->irq = platform_get_irq(pdev, 0);
@@ -240,12 +225,16 @@ static int mv_ehci_probe(struct platform_device *pdev)
 
 	ehci_mv->mode = pdata->mode;
 	if (ehci_mv->mode == MV_USB_MODE_OTG) {
-#ifdef CONFIG_USB_OTG_UTILS
 		ehci_mv->otg = devm_usb_get_phy(&pdev->dev, USB_PHY_TYPE_USB2);
-		if (IS_ERR_OR_NULL(ehci_mv->otg)) {
-			dev_err(&pdev->dev,
-				"unable to find transceiver\n");
-			retval = -ENODEV;
+		if (IS_ERR(ehci_mv->otg)) {
+			retval = PTR_ERR(ehci_mv->otg);
+
+			if (retval == -ENXIO)
+				dev_info(&pdev->dev, "MV_USB_MODE_OTG "
+						"must have CONFIG_USB_PHY enabled\n");
+			else
+				dev_err(&pdev->dev,
+						"unable to find transceiver\n");
 			goto err_disable_clk;
 		}
 
@@ -258,11 +247,6 @@ static int mv_ehci_probe(struct platform_device *pdev)
 		}
 		/* otg will enable clock before use as host */
 		mv_ehci_disable(ehci_mv);
-#else
-		dev_info(&pdev->dev, "MV_USB_MODE_OTG "
-			 "must have CONFIG_USB_OTG_UTILS enabled\n");
-		goto err_disable_clk;
-#endif
 	} else {
 		if (pdata->set_vbus)
 			pdata->set_vbus(1);
diff --git a/drivers/usb/host/ehci-mxc.c b/drivers/usb/host/ehci-mxc.c
index e9301fb97ea..c369767b00e 100644
--- a/drivers/usb/host/ehci-mxc.c
+++ b/drivers/usb/host/ehci-mxc.c
@@ -28,11 +28,7 @@
 #include <linux/slab.h>
 #include <linux/usb.h>
 #include <linux/usb/hcd.h>
-
 #include <linux/platform_data/usb-ehci-mxc.h>
-
-#include <asm/mach-types.h>
-
 #include "ehci.h"
 
 #define DRIVER_DESC "Freescale On-Chip EHCI Host driver"
@@ -47,7 +43,7 @@ struct ehci_mxc_priv {
 
 static struct hc_driver __read_mostly ehci_mxc_hc_driver;
 
-static const struct ehci_driver_overrides ehci_mxc_overrides __initdata = {
+static const struct ehci_driver_overrides ehci_mxc_overrides __initconst = {
 	.extra_priv_size =	sizeof(struct ehci_mxc_priv),
 };
 
@@ -61,8 +57,6 @@ static int ehci_mxc_drv_probe(struct platform_device *pdev)
 	struct device *dev = &pdev->dev;
 	struct ehci_hcd *ehci;
 
-	dev_info(&pdev->dev, "initializing i.MX USB Controller\n");
-
 	if (!pdata) {
 		dev_err(dev, "No platform data given, bailing out.\n");
 		return -EINVAL;
@@ -178,7 +172,7 @@ err_alloc:
 	return ret;
 }
 
-static int __exit ehci_mxc_drv_remove(struct platform_device *pdev)
+static int ehci_mxc_drv_remove(struct platform_device *pdev)
 {
 	struct mxc_usbh_platform_data *pdata = pdev->dev.platform_data;
 	struct usb_hcd *hcd = platform_get_drvdata(pdev);
diff --git a/drivers/usb/host/ehci-omap.c b/drivers/usb/host/ehci-omap.c
index 0555ee42d7c..3d1491b5f36 100644
--- a/drivers/usb/host/ehci-omap.c
+++ b/drivers/usb/host/ehci-omap.c
@@ -4,10 +4,11 @@
  * Bus Glue for the EHCI controllers in OMAP3/4
  * Tested on several OMAP3 boards, and OMAP4 Pandaboard
  *
- * Copyright (C) 2007-2011 Texas Instruments, Inc.
+ * Copyright (C) 2007-2013 Texas Instruments, Inc.
  *	Author: Vikram Pandita <vikram.pandita@ti.com>
  *	Author: Anand Gadiyar <gadiyar@ti.com>
  *	Author: Keshava Munegowda <keshava_mgowda@ti.com>
+ *	Author: Roger Quadros <rogerq@ti.com>
  *
  * Copyright (C) 2009 Nokia Corporation
  *	Contact: Felipe Balbi <felipe.balbi@nokia.com>
@@ -28,21 +29,23 @@
  * along with this program; if not, write to the Free Software
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
  *
- * TODO (last updated Feb 27, 2010):
- *	- add kernel-doc
- *	- enable AUTOIDLE
- *	- add suspend/resume
- *	- add HSIC and TLL support
- *	- convert to use hwmod and runtime PM
  */
 
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/io.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 #include <linux/usb/ulpi.h>
-#include <linux/regulator/consumer.h>
 #include <linux/pm_runtime.h>
 #include <linux/gpio.h>
 #include <linux/clk.h>
+#include <linux/usb.h>
+#include <linux/usb/hcd.h>
+#include <linux/of.h>
+#include <linux/dma-mapping.h>
+
+#include "ehci.h"
 
 #include <linux/platform_data/usb-omap.h>
 
@@ -57,10 +60,16 @@
 #define	EHCI_INSNREG05_ULPI_EXTREGADD_SHIFT		8
 #define	EHCI_INSNREG05_ULPI_WRDATA_SHIFT		0
 
-/*-------------------------------------------------------------------------*/
+#define DRIVER_DESC "OMAP-EHCI Host Controller driver"
+
+static const char hcd_name[] = "ehci-omap";
 
-static const struct hc_driver ehci_omap_hc_driver;
+/*-------------------------------------------------------------------------*/
 
+struct omap_hcd {
+	struct usb_phy *phy[OMAP3_HS_USB_PORTS]; /* one PHY for each port */
+	int nports;
+};
 
 static inline void ehci_write(void __iomem *base, u32 reg, u32 val)
 {
@@ -72,99 +81,16 @@ static inline u32 ehci_read(void __iomem *base, u32 reg)
 	return __raw_readl(base + reg);
 }
 
+/* configure so an HC device and id are always provided */
+/* always called with process context; sleeping is OK */
 
-static void omap_ehci_soft_phy_reset(struct usb_hcd *hcd, u8 port)
-{
-	unsigned long timeout = jiffies + msecs_to_jiffies(1000);
-	unsigned reg = 0;
-
-	reg = ULPI_FUNC_CTRL_RESET
-		/* FUNCTION_CTRL_SET register */
-		| (ULPI_SET(ULPI_FUNC_CTRL) << EHCI_INSNREG05_ULPI_REGADD_SHIFT)
-		/* Write */
-		| (2 << EHCI_INSNREG05_ULPI_OPSEL_SHIFT)
-		/* PORTn */
-		| ((port + 1) << EHCI_INSNREG05_ULPI_PORTSEL_SHIFT)
-		/* start ULPI access*/
-		| (1 << EHCI_INSNREG05_ULPI_CONTROL_SHIFT);
-
-	ehci_write(hcd->regs, EHCI_INSNREG05_ULPI, reg);
-
-	/* Wait for ULPI access completion */
-	while ((ehci_read(hcd->regs, EHCI_INSNREG05_ULPI)
-			& (1 << EHCI_INSNREG05_ULPI_CONTROL_SHIFT))) {
-		cpu_relax();
-
-		if (time_after(jiffies, timeout)) {
-			dev_dbg(hcd->self.controller,
-					"phy reset operation timed out\n");
-			break;
-		}
-	}
-}
-
-static int omap_ehci_init(struct usb_hcd *hcd)
-{
-	struct ehci_hcd		*ehci = hcd_to_ehci(hcd);
-	int			rc;
-	struct usbhs_omap_platform_data	*pdata;
-
-	pdata = hcd->self.controller->platform_data;
-
-	/* Hold PHYs in reset while initializing EHCI controller */
-	if (pdata->phy_reset) {
-		if (gpio_is_valid(pdata->reset_gpio_port[0]))
-			gpio_set_value_cansleep(pdata->reset_gpio_port[0], 0);
-
-		if (gpio_is_valid(pdata->reset_gpio_port[1]))
-			gpio_set_value_cansleep(pdata->reset_gpio_port[1], 0);
-
-		/* Hold the PHY in RESET for enough time till DIR is high */
-		udelay(10);
-	}
-
-	/* Soft reset the PHY using PHY reset command over ULPI */
-	if (pdata->port_mode[0] == OMAP_EHCI_PORT_MODE_PHY)
-		omap_ehci_soft_phy_reset(hcd, 0);
-	if (pdata->port_mode[1] == OMAP_EHCI_PORT_MODE_PHY)
-		omap_ehci_soft_phy_reset(hcd, 1);
-
-	/* we know this is the memory we want, no need to ioremap again */
-	ehci->caps = hcd->regs;
-
-	rc = ehci_setup(hcd);
-
-	if (pdata->phy_reset) {
-		/* Hold the PHY in RESET for enough time till
-		 * PHY is settled and ready
-		 */
-		udelay(10);
-
-		if (gpio_is_valid(pdata->reset_gpio_port[0]))
-			gpio_set_value_cansleep(pdata->reset_gpio_port[0], 1);
-
-		if (gpio_is_valid(pdata->reset_gpio_port[1]))
-			gpio_set_value_cansleep(pdata->reset_gpio_port[1], 1);
-	}
-
-	return rc;
-}
+static struct hc_driver __read_mostly ehci_omap_hc_driver;
 
-static void disable_put_regulator(
-		struct usbhs_omap_platform_data *pdata)
-{
-	int i;
-
-	for (i = 0 ; i < OMAP3_HS_USB_PORTS ; i++) {
-		if (pdata->regulator[i]) {
-			regulator_disable(pdata->regulator[i]);
-			regulator_put(pdata->regulator[i]);
-		}
-	}
-}
+static const struct ehci_driver_overrides ehci_omap_overrides __initdata = {
+	.extra_priv_size = sizeof(struct omap_hcd),
+};
 
-/* configure so an HC device and id are always provided */
-/* always called with process context; sleeping is OK */
+static u64 omap_ehci_dma_mask = DMA_BIT_MASK(32);
 
 /**
  * ehci_hcd_omap_probe - initialize TI-based HCDs
@@ -175,15 +101,15 @@ static void disable_put_regulator(
  */
 static int ehci_hcd_omap_probe(struct platform_device *pdev)
 {
-	struct device				*dev = &pdev->dev;
-	struct usbhs_omap_platform_data		*pdata = dev->platform_data;
-	struct resource				*res;
-	struct usb_hcd				*hcd;
-	void __iomem				*regs;
-	int					ret = -ENODEV;
-	int					irq;
-	int					i;
-	char					supply[7];
+	struct device *dev = &pdev->dev;
+	struct usbhs_omap_platform_data *pdata = dev->platform_data;
+	struct resource	*res;
+	struct usb_hcd	*hcd;
+	void __iomem *regs;
+	int ret = -ENODEV;
+	int irq;
+	int i;
+	struct omap_hcd	*omap;
 
 	if (usb_disabled())
 		return -ENODEV;
@@ -193,52 +119,74 @@ static int ehci_hcd_omap_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	irq = platform_get_irq_byname(pdev, "ehci-irq");
-	if (irq < 0) {
-		dev_err(dev, "EHCI irq failed\n");
-		return -ENODEV;
+	/* For DT boot, get platform data from parent. i.e. usbhshost */
+	if (dev->of_node) {
+		pdata = dev->parent->platform_data;
+		dev->platform_data = pdata;
 	}
 
-	res =  platform_get_resource_byname(pdev,
-				IORESOURCE_MEM, "ehci");
-	if (!res) {
-		dev_err(dev, "UHH EHCI get resource failed\n");
+	if (!pdata) {
+		dev_err(dev, "Missing platform data\n");
 		return -ENODEV;
 	}
 
-	regs = ioremap(res->start, resource_size(res));
-	if (!regs) {
-		dev_err(dev, "UHH EHCI ioremap failed\n");
-		return -ENOMEM;
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(dev, "EHCI irq failed\n");
+		return -ENODEV;
 	}
 
+	res =  platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	regs = devm_ioremap_resource(dev, res);
+	if (IS_ERR(regs))
+		return PTR_ERR(regs);
+
+	/*
+	 * Right now device-tree probed devices don't get dma_mask set.
+	 * Since shared usb code relies on it, set it here for now.
+	 * Once we have dma capability bindings this can go away.
+	 */
+	if (!pdev->dev.dma_mask)
+		pdev->dev.dma_mask = &omap_ehci_dma_mask;
+
 	hcd = usb_create_hcd(&ehci_omap_hc_driver, dev,
 			dev_name(dev));
 	if (!hcd) {
-		dev_err(dev, "failed to create hcd with err %d\n", ret);
-		ret = -ENOMEM;
-		goto err_io;
+		dev_err(dev, "Failed to create HCD\n");
+		return -ENOMEM;
 	}
 
 	hcd->rsrc_start = res->start;
 	hcd->rsrc_len = resource_size(res);
 	hcd->regs = regs;
-
-	/* get ehci regulator and enable */
-	for (i = 0 ; i < OMAP3_HS_USB_PORTS ; i++) {
-		if (pdata->port_mode[i] != OMAP_EHCI_PORT_MODE_PHY) {
-			pdata->regulator[i] = NULL;
-			continue;
-		}
-		snprintf(supply, sizeof(supply), "hsusb%d", i);
-		pdata->regulator[i] = regulator_get(dev, supply);
-		if (IS_ERR(pdata->regulator[i])) {
-			pdata->regulator[i] = NULL;
-			dev_dbg(dev,
-			"failed to get ehci port%d regulator\n", i);
-		} else {
-			regulator_enable(pdata->regulator[i]);
+	hcd_to_ehci(hcd)->caps = regs;
+
+	omap = (struct omap_hcd *)hcd_to_ehci(hcd)->priv;
+	omap->nports = pdata->nports;
+
+	platform_set_drvdata(pdev, hcd);
+
+	/* get the PHY devices if needed */
+	for (i = 0 ; i < omap->nports ; i++) {
+		struct usb_phy *phy;
+
+		/* get the PHY device */
+		if (dev->of_node)
+			phy = devm_usb_get_phy_by_phandle(dev, "phys", i);
+		else
+			phy = devm_usb_get_phy_dev(dev, i);
+		if (IS_ERR(phy)) {
+			/* Don't bail out if PHY is not absolutely necessary */
+			if (pdata->port_mode[i] != OMAP_EHCI_PORT_MODE_PHY)
+				continue;
+
+			ret = PTR_ERR(phy);
+			dev_err(dev, "Can't get PHY device for port %d: %d\n",
+					i, ret);
+			goto err_phy;
 		}
+
+		omap->phy[i] = phy;
 	}
 
 	pm_runtime_enable(dev);
@@ -262,16 +210,34 @@ static int ehci_hcd_omap_probe(struct platform_device *pdev)
 		goto err_pm_runtime;
 	}
 
+	/*
+	 * Bring PHYs out of reset.
+	 * Even though HSIC mode is a PHY-less mode, the reset
+	 * line exists between the chips and can be modelled
+	 * as a PHY device for reset control.
+	 */
+	for (i = 0; i < omap->nports; i++) {
+		if (!omap->phy[i])
+			continue;
+
+		usb_phy_init(omap->phy[i]);
+		/* bring PHY out of suspend */
+		usb_phy_set_suspend(omap->phy[i], 0);
+	}
 
 	return 0;
 
 err_pm_runtime:
-	disable_put_regulator(pdata);
 	pm_runtime_put_sync(dev);
+
+err_phy:
+	for (i = 0; i < omap->nports; i++) {
+		if (omap->phy[i])
+			usb_phy_shutdown(omap->phy[i]);
+	}
+
 	usb_put_hcd(hcd);
 
-err_io:
-	iounmap(regs);
 	return ret;
 }
 
@@ -286,14 +252,19 @@ err_io:
  */
 static int ehci_hcd_omap_remove(struct platform_device *pdev)
 {
-	struct device *dev				= &pdev->dev;
-	struct usb_hcd *hcd				= dev_get_drvdata(dev);
+	struct device *dev = &pdev->dev;
+	struct usb_hcd *hcd = dev_get_drvdata(dev);
+	struct omap_hcd *omap = (struct omap_hcd *)hcd_to_ehci(hcd)->priv;
+	int i;
 
 	usb_remove_hcd(hcd);
-	disable_put_regulator(dev->platform_data);
-	iounmap(hcd->regs);
-	usb_put_hcd(hcd);
 
+	for (i = 0; i < omap->nports; i++) {
+		if (omap->phy[i])
+			usb_phy_shutdown(omap->phy[i]);
+	}
+
+	usb_put_hcd(hcd);
 	pm_runtime_put_sync(dev);
 	pm_runtime_disable(dev);
 
@@ -308,6 +279,13 @@ static void ehci_hcd_omap_shutdown(struct platform_device *pdev)
 		hcd->driver->shutdown(hcd);
 }
 
+static const struct of_device_id omap_ehci_dt_ids[] = {
+	{ .compatible = "ti,ehci-omap" },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(of, omap_ehci_dt_ids);
+
 static struct platform_driver ehci_hcd_omap_driver = {
 	.probe			= ehci_hcd_omap_probe,
 	.remove			= ehci_hcd_omap_remove,
@@ -315,56 +293,35 @@ static struct platform_driver ehci_hcd_omap_driver = {
 	/*.suspend		= ehci_hcd_omap_suspend, */
 	/*.resume		= ehci_hcd_omap_resume, */
 	.driver = {
-		.name		= "ehci-omap",
+		.name		= hcd_name,
+		.of_match_table = of_match_ptr(omap_ehci_dt_ids),
 	}
 };
 
 /*-------------------------------------------------------------------------*/
 
-static const struct hc_driver ehci_omap_hc_driver = {
-	.description		= hcd_name,
-	.product_desc		= "OMAP-EHCI Host Controller",
-	.hcd_priv_size		= sizeof(struct ehci_hcd),
-
-	/*
-	 * generic hardware linkage
-	 */
-	.irq			= ehci_irq,
-	.flags			= HCD_MEMORY | HCD_USB2,
-
-	/*
-	 * basic lifecycle operations
-	 */
-	.reset			= omap_ehci_init,
-	.start			= ehci_run,
-	.stop			= ehci_stop,
-	.shutdown		= ehci_shutdown,
-
-	/*
-	 * managing i/o requests and associated device resources
-	 */
-	.urb_enqueue		= ehci_urb_enqueue,
-	.urb_dequeue		= ehci_urb_dequeue,
-	.endpoint_disable	= ehci_endpoint_disable,
-	.endpoint_reset		= ehci_endpoint_reset,
+static int __init ehci_omap_init(void)
+{
+	if (usb_disabled())
+		return -ENODEV;
 
-	/*
-	 * scheduling support
-	 */
-	.get_frame_number	= ehci_get_frame,
+	pr_info("%s: " DRIVER_DESC "\n", hcd_name);
 
-	/*
-	 * root hub support
-	 */
-	.hub_status_data	= ehci_hub_status_data,
-	.hub_control		= ehci_hub_control,
-	.bus_suspend		= ehci_bus_suspend,
-	.bus_resume		= ehci_bus_resume,
+	ehci_init_driver(&ehci_omap_hc_driver, &ehci_omap_overrides);
+	return platform_driver_register(&ehci_hcd_omap_driver);
+}
+module_init(ehci_omap_init);
 
-	.clear_tt_buffer_complete = ehci_clear_tt_buffer_complete,
-};
+static void __exit ehci_omap_cleanup(void)
+{
+	platform_driver_unregister(&ehci_hcd_omap_driver);
+}
+module_exit(ehci_omap_cleanup);
 
 MODULE_ALIAS("platform:ehci-omap");
 MODULE_AUTHOR("Texas Instruments, Inc.");
 MODULE_AUTHOR("Felipe Balbi <felipe.balbi@nokia.com>");
+MODULE_AUTHOR("Roger Quadros <rogerq@ti.com>");
 
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/host/ehci-orion.c b/drivers/usb/host/ehci-orion.c
index 914a3ecfb5d..54c57948515 100644
--- a/drivers/usb/host/ehci-orion.c
+++ b/drivers/usb/host/ehci-orion.c
@@ -17,6 +17,12 @@
 #include <linux/of.h>
 #include <linux/of_device.h>
 #include <linux/of_irq.h>
+#include <linux/usb.h>
+#include <linux/usb/hcd.h>
+#include <linux/io.h>
+#include <linux/dma-mapping.h>
+
+#include "ehci.h"
 
 #define rdl(off)	__raw_readl(hcd->regs + (off))
 #define wrl(off, val)	__raw_writel((val), hcd->regs + (off))
@@ -34,6 +40,12 @@
 #define USB_PHY_IVREF_CTRL	0x440
 #define USB_PHY_TST_GRP_CTRL	0x450
 
+#define DRIVER_DESC "EHCI orion driver"
+
+static const char hcd_name[] = "ehci-orion";
+
+static struct hc_driver __read_mostly ehci_orion_hc_driver;
+
 /*
  * Implement Orion USB controller specification guidelines
  */
@@ -104,51 +116,6 @@ static void orion_usb_phy_v1_setup(struct usb_hcd *hcd)
 	wrl(USB_MODE, 0x13);
 }
 
-static const struct hc_driver ehci_orion_hc_driver = {
-	.description = hcd_name,
-	.product_desc = "Marvell Orion EHCI",
-	.hcd_priv_size = sizeof(struct ehci_hcd),
-
-	/*
-	 * generic hardware linkage
-	 */
-	.irq = ehci_irq,
-	.flags = HCD_MEMORY | HCD_USB2,
-
-	/*
-	 * basic lifecycle operations
-	 */
-	.reset = ehci_setup,
-	.start = ehci_run,
-	.stop = ehci_stop,
-	.shutdown = ehci_shutdown,
-
-	/*
-	 * managing i/o requests and associated device resources
-	 */
-	.urb_enqueue = ehci_urb_enqueue,
-	.urb_dequeue = ehci_urb_dequeue,
-	.endpoint_disable = ehci_endpoint_disable,
-	.endpoint_reset = ehci_endpoint_reset,
-
-	/*
-	 * scheduling support
-	 */
-	.get_frame_number = ehci_get_frame,
-
-	/*
-	 * root hub support
-	 */
-	.hub_status_data = ehci_hub_status_data,
-	.hub_control = ehci_hub_control,
-	.bus_suspend = ehci_bus_suspend,
-	.bus_resume = ehci_bus_resume,
-	.relinquish_port = ehci_relinquish_port,
-	.port_handed_over = ehci_port_handed_over,
-
-	.clear_tt_buffer_complete = ehci_clear_tt_buffer_complete,
-};
-
 static void
 ehci_orion_conf_mbus_windows(struct usb_hcd *hcd,
 			     const struct mbus_dram_target_info *dram)
@@ -305,7 +272,7 @@ err1:
 	return err;
 }
 
-static int __exit ehci_orion_drv_remove(struct platform_device *pdev)
+static int ehci_orion_drv_remove(struct platform_device *pdev)
 {
 	struct usb_hcd *hcd = platform_get_drvdata(pdev);
 	struct clk *clk;
@@ -323,8 +290,6 @@ static int __exit ehci_orion_drv_remove(struct platform_device *pdev)
 	return 0;
 }
 
-MODULE_ALIAS("platform:orion-ehci");
-
 static const struct of_device_id ehci_orion_dt_ids[] = {
 	{ .compatible = "marvell,orion-ehci", },
 	{},
@@ -333,7 +298,7 @@ MODULE_DEVICE_TABLE(of, ehci_orion_dt_ids);
 
 static struct platform_driver ehci_orion_driver = {
 	.probe		= ehci_orion_drv_probe,
-	.remove		= __exit_p(ehci_orion_drv_remove),
+	.remove		= ehci_orion_drv_remove,
 	.shutdown	= usb_hcd_platform_shutdown,
 	.driver = {
 		.name	= "orion-ehci",
@@ -341,3 +306,26 @@ static struct platform_driver ehci_orion_driver = {
 		.of_match_table = of_match_ptr(ehci_orion_dt_ids),
 	},
 };
+
+static int __init ehci_orion_init(void)
+{
+	if (usb_disabled())
+		return -ENODEV;
+
+	pr_info("%s: " DRIVER_DESC "\n", hcd_name);
+
+	ehci_init_driver(&ehci_orion_hc_driver, NULL);
+	return platform_driver_register(&ehci_orion_driver);
+}
+module_init(ehci_orion_init);
+
+static void __exit ehci_orion_cleanup(void)
+{
+	platform_driver_unregister(&ehci_orion_driver);
+}
+module_exit(ehci_orion_cleanup);
+
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_ALIAS("platform:orion-ehci");
+MODULE_AUTHOR("Tzachi Perelstein");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/usb/host/ehci-pci.c b/drivers/usb/host/ehci-pci.c
index 170b9399e09..595d210655b 100644
--- a/drivers/usb/host/ehci-pci.c
+++ b/drivers/usb/host/ehci-pci.c
@@ -292,17 +292,7 @@ static int ehci_pci_setup(struct usb_hcd *hcd)
 		}
 	}
 
-#ifdef	CONFIG_USB_SUSPEND
-	/* REVISIT: the controller works fine for wakeup iff the root hub
-	 * itself is "globally" suspended, but usbcore currently doesn't
-	 * understand such things.
-	 *
-	 * System suspend currently expects to be able to suspend the entire
-	 * device tree, device-at-a-time.  If we failed selective suspend
-	 * reports, system suspend would fail; so the root hub code must claim
-	 * success.  That's lying to usbcore, and it matters for runtime
-	 * PM scenarios with selective suspend and remote wakeup...
-	 */
+#ifdef	CONFIG_PM_RUNTIME
 	if (ehci->no_selective_suspend && device_can_wakeup(&pdev->dev))
 		ehci_warn(ehci, "selective suspend/wakeup unavailable\n");
 #endif
@@ -385,7 +375,7 @@ static int ehci_pci_resume(struct usb_hcd *hcd, bool hibernated)
 
 static struct hc_driver __read_mostly ehci_pci_hc_driver;
 
-static const struct ehci_driver_overrides pci_overrides __initdata = {
+static const struct ehci_driver_overrides pci_overrides __initconst = {
 	.reset =		ehci_pci_setup,
 };
 
diff --git a/drivers/usb/host/ehci-platform.c b/drivers/usb/host/ehci-platform.c
index ca750639054..f47f2594c9d 100644
--- a/drivers/usb/host/ehci-platform.c
+++ b/drivers/usb/host/ehci-platform.c
@@ -18,11 +18,13 @@
  *
  * Licensed under the GNU/GPL. See COPYING for details.
  */
+#include <linux/dma-mapping.h>
 #include <linux/err.h>
 #include <linux/kernel.h>
 #include <linux/hrtimer.h>
 #include <linux/io.h>
 #include <linux/module.h>
+#include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/usb.h>
 #include <linux/usb/hcd.h>
@@ -58,26 +60,36 @@ static int ehci_platform_reset(struct usb_hcd *hcd)
 
 static struct hc_driver __read_mostly ehci_platform_hc_driver;
 
-static const struct ehci_driver_overrides platform_overrides __initdata = {
+static const struct ehci_driver_overrides platform_overrides __initconst = {
 	.reset =	ehci_platform_reset,
 };
 
+static struct usb_ehci_pdata ehci_platform_defaults;
+
 static int ehci_platform_probe(struct platform_device *dev)
 {
 	struct usb_hcd *hcd;
 	struct resource *res_mem;
-	struct usb_ehci_pdata *pdata = dev->dev.platform_data;
+	struct usb_ehci_pdata *pdata;
 	int irq;
 	int err = -ENOMEM;
 
-	if (!pdata) {
-		WARN_ON(1);
-		return -ENODEV;
-	}
-
 	if (usb_disabled())
 		return -ENODEV;
 
+	/*
+	 * use reasonable defaults so platforms don't have to provide these.
+	 * with DT probing on ARM, none of these are set.
+	 */
+	if (!dev->dev.platform_data)
+		dev->dev.platform_data = &ehci_platform_defaults;
+	if (!dev->dev.dma_mask)
+		dev->dev.dma_mask = &dev->dev.coherent_dma_mask;
+	if (!dev->dev.coherent_dma_mask)
+		dev->dev.coherent_dma_mask = DMA_BIT_MASK(32);
+
+	pdata = dev->dev.platform_data;
+
 	irq = platform_get_irq(dev, 0);
 	if (irq < 0) {
 		dev_err(&dev->dev, "no irq provided");
@@ -139,6 +151,9 @@ static int ehci_platform_remove(struct platform_device *dev)
 	if (pdata->power_off)
 		pdata->power_off(dev);
 
+	if (pdata == &ehci_platform_defaults)
+		dev->dev.platform_data = NULL;
+
 	return 0;
 }
 
@@ -183,6 +198,12 @@ static int ehci_platform_resume(struct device *dev)
 #define ehci_platform_resume	NULL
 #endif /* CONFIG_PM */
 
+static const struct of_device_id vt8500_ehci_ids[] = {
+	{ .compatible = "via,vt8500-ehci", },
+	{ .compatible = "wm,prizm-ehci", },
+	{}
+};
+
 static const struct platform_device_id ehci_platform_table[] = {
 	{ "ehci-platform", 0 },
 	{ }
@@ -203,6 +224,7 @@ static struct platform_driver ehci_platform_driver = {
 		.owner	= THIS_MODULE,
 		.name	= "ehci-platform",
 		.pm	= &ehci_platform_pm_ops,
+		.of_match_table = of_match_ptr(vt8500_ehci_ids),
 	}
 };
 
diff --git a/drivers/usb/host/ehci-ps3.c b/drivers/usb/host/ehci-ps3.c
index df5925a4f0d..fd983771b02 100644
--- a/drivers/usb/host/ehci-ps3.c
+++ b/drivers/usb/host/ehci-ps3.c
@@ -221,7 +221,6 @@ static int ps3_ehci_remove(struct ps3_system_bus_device *dev)
 
 	tmp = hcd->irq;
 
-	ehci_shutdown(hcd);
 	usb_remove_hcd(hcd);
 
 	ps3_system_bus_set_drvdata(dev, NULL);
diff --git a/drivers/usb/host/ehci-q.c b/drivers/usb/host/ehci-q.c
index 23d13690428..d34b399b78e 100644
--- a/drivers/usb/host/ehci-q.c
+++ b/drivers/usb/host/ehci-q.c
@@ -90,7 +90,7 @@ qh_update (struct ehci_hcd *ehci, struct ehci_qh *qh, struct ehci_qtd *qtd)
 	struct ehci_qh_hw *hw = qh->hw;
 
 	/* writes to an active overlay are unsafe */
-	BUG_ON(qh->qh_state != QH_STATE_IDLE);
+	WARN_ON(qh->qh_state != QH_STATE_IDLE);
 
 	hw->hw_qtd_next = QTD_NEXT(ehci, qtd->qtd_dma);
 	hw->hw_alt_next = EHCI_LIST_END(ehci);
@@ -123,26 +123,19 @@ qh_refresh (struct ehci_hcd *ehci, struct ehci_qh *qh)
 {
 	struct ehci_qtd *qtd;
 
-	if (list_empty (&qh->qtd_list))
-		qtd = qh->dummy;
-	else {
-		qtd = list_entry (qh->qtd_list.next,
-				struct ehci_qtd, qtd_list);
-		/*
-		 * first qtd may already be partially processed.
-		 * If we come here during unlink, the QH overlay region
-		 * might have reference to the just unlinked qtd. The
-		 * qtd is updated in qh_completions(). Update the QH
-		 * overlay here.
-		 */
-		if (qh->hw->hw_token & ACTIVE_BIT(ehci)) {
-			qh->hw->hw_qtd_next = qtd->hw_next;
-			qtd = NULL;
-		}
-	}
+	qtd = list_entry(qh->qtd_list.next, struct ehci_qtd, qtd_list);
 
-	if (qtd)
-		qh_update (ehci, qh, qtd);
+	/*
+	 * first qtd may already be partially processed.
+	 * If we come here during unlink, the QH overlay region
+	 * might have reference to the just unlinked qtd. The
+	 * qtd is updated in qh_completions(). Update the QH
+	 * overlay here.
+	 */
+	if (qh->hw->hw_token & ACTIVE_BIT(ehci))
+		qh->hw->hw_qtd_next = qtd->hw_next;
+	else
+		qh_update(ehci, qh, qtd);
 }
 
 /*-------------------------------------------------------------------------*/
@@ -299,8 +292,8 @@ static int qh_schedule (struct ehci_hcd *ehci, struct ehci_qh *qh);
 
 /*
  * Process and free completed qtds for a qh, returning URBs to drivers.
- * Chases up to qh->hw_current.  Returns number of completions called,
- * indicating how much "real" work we did.
+ * Chases up to qh->hw_current.  Returns nonzero if the caller should
+ * unlink qh.
  */
 static unsigned
 qh_completions (struct ehci_hcd *ehci, struct ehci_qh *qh)
@@ -309,13 +302,9 @@ qh_completions (struct ehci_hcd *ehci, struct ehci_qh *qh)
 	struct list_head	*entry, *tmp;
 	int			last_status;
 	int			stopped;
-	unsigned		count = 0;
 	u8			state;
 	struct ehci_qh_hw	*hw = qh->hw;
 
-	if (unlikely (list_empty (&qh->qtd_list)))
-		return count;
-
 	/* completions (or tasks on other cpus) must never clobber HALT
 	 * till we've gone through and cleaned everything up, even when
 	 * they add urbs to this qh's queue or mark them for unlinking.
@@ -333,7 +322,7 @@ qh_completions (struct ehci_hcd *ehci, struct ehci_qh *qh)
  rescan:
 	last = NULL;
 	last_status = -EINPROGRESS;
-	qh->needs_rescan = 0;
+	qh->dequeue_during_giveback = 0;
 
 	/* remove de-activated QTDs from front of queue.
 	 * after faults (including short reads), cleanup this urb
@@ -352,7 +341,6 @@ qh_completions (struct ehci_hcd *ehci, struct ehci_qh *qh)
 		if (last) {
 			if (likely (last->urb != urb)) {
 				ehci_urb_done(ehci, last->urb, last_status);
-				count++;
 				last_status = -EINPROGRESS;
 			}
 			ehci_qtd_free (ehci, last);
@@ -526,23 +514,16 @@ qh_completions (struct ehci_hcd *ehci, struct ehci_qh *qh)
 	/* last urb's completion might still need calling */
 	if (likely (last != NULL)) {
 		ehci_urb_done(ehci, last->urb, last_status);
-		count++;
 		ehci_qtd_free (ehci, last);
 	}
 
 	/* Do we need to rescan for URBs dequeued during a giveback? */
-	if (unlikely(qh->needs_rescan)) {
+	if (unlikely(qh->dequeue_during_giveback)) {
 		/* If the QH is already unlinked, do the rescan now. */
 		if (state == QH_STATE_IDLE)
 			goto rescan;
 
-		/* Otherwise we have to wait until the QH is fully unlinked.
-		 * Our caller will start an unlink if qh->needs_rescan is
-		 * set.  But if an unlink has already started, nothing needs
-		 * to be done.
-		 */
-		if (state != QH_STATE_LINKED)
-			qh->needs_rescan = 0;
+		/* Otherwise the caller must unlink the QH. */
 	}
 
 	/* restore original state; caller must unlink or relink */
@@ -551,33 +532,23 @@ qh_completions (struct ehci_hcd *ehci, struct ehci_qh *qh)
 	/* be sure the hardware's done with the qh before refreshing
 	 * it after fault cleanup, or recovering from silicon wrongly
 	 * overlaying the dummy qtd (which reduces DMA chatter).
+	 *
+	 * We won't refresh a QH that's linked (after the HC
+	 * stopped the queue).  That avoids a race:
+	 *  - HC reads first part of QH;
+	 *  - CPU updates that first part and the token;
+	 *  - HC reads rest of that QH, including token
+	 * Result:  HC gets an inconsistent image, and then
+	 * DMAs to/from the wrong memory (corrupting it).
+	 *
+	 * That should be rare for interrupt transfers,
+	 * except maybe high bandwidth ...
 	 */
-	if (stopped != 0 || hw->hw_qtd_next == EHCI_LIST_END(ehci)) {
-		switch (state) {
-		case QH_STATE_IDLE:
-			qh_refresh(ehci, qh);
-			break;
-		case QH_STATE_LINKED:
-			/* We won't refresh a QH that's linked (after the HC
-			 * stopped the queue).  That avoids a race:
-			 *  - HC reads first part of QH;
-			 *  - CPU updates that first part and the token;
-			 *  - HC reads rest of that QH, including token
-			 * Result:  HC gets an inconsistent image, and then
-			 * DMAs to/from the wrong memory (corrupting it).
-			 *
-			 * That should be rare for interrupt transfers,
-			 * except maybe high bandwidth ...
-			 */
+	if (stopped != 0 || hw->hw_qtd_next == EHCI_LIST_END(ehci))
+		qh->exception = 1;
 
-			/* Tell the caller to start an unlink */
-			qh->needs_rescan = 1;
-			break;
-		/* otherwise, unlink already started */
-		}
-	}
-
-	return count;
+	/* Let the caller know if the QH needs to be unlinked. */
+	return qh->exception;
 }
 
 /*-------------------------------------------------------------------------*/
@@ -957,14 +928,13 @@ done:
 
 	/* NOTE:  if (PIPE_INTERRUPT) { scheduler sets s-mask } */
 
-	/* init as live, toggle clear, advance to dummy */
+	/* init as live, toggle clear */
 	qh->qh_state = QH_STATE_IDLE;
 	hw = qh->hw;
 	hw->hw_info1 = cpu_to_hc32(ehci, info1);
 	hw->hw_info2 = cpu_to_hc32(ehci, info2);
 	qh->is_out = !is_input;
 	usb_settoggle (urb->dev, usb_pipeendpoint (urb->pipe), !is_input, 1);
-	qh_refresh (ehci, qh);
 	return qh;
 }
 
@@ -988,8 +958,9 @@ static void disable_async(struct ehci_hcd *ehci)
 	if (--ehci->async_count)
 		return;
 
-	/* The async schedule and async_unlink list are supposed to be empty */
-	WARN_ON(ehci->async->qh_next.qh || ehci->async_unlink);
+	/* The async schedule and unlink lists are supposed to be empty */
+	WARN_ON(ehci->async->qh_next.qh || !list_empty(&ehci->async_unlink) ||
+			!list_empty(&ehci->async_idle));
 
 	/* Don't turn off the schedule until ASS is 1 */
 	ehci_poll_ASS(ehci);
@@ -1020,8 +991,9 @@ static void qh_link_async (struct ehci_hcd *ehci, struct ehci_qh *qh)
 	head->qh_next.qh = qh;
 	head->hw->hw_next = dma;
 
-	qh->xacterrs = 0;
 	qh->qh_state = QH_STATE_LINKED;
+	qh->xacterrs = 0;
+	qh->exception = 0;
 	/* qtd completions reported later by interrupt */
 
 	enable_async(ehci);
@@ -1179,11 +1151,7 @@ static void single_unlink_async(struct ehci_hcd *ehci, struct ehci_qh *qh)
 
 	/* Add to the end of the list of QHs waiting for the next IAAD */
 	qh->qh_state = QH_STATE_UNLINK_WAIT;
-	if (ehci->async_unlink)
-		ehci->async_unlink_last->unlink_next = qh;
-	else
-		ehci->async_unlink = qh;
-	ehci->async_unlink_last = qh;
+	list_add_tail(&qh->unlink_node, &ehci->async_unlink);
 
 	/* Unlink it from the schedule */
 	prev = ehci->async;
@@ -1196,44 +1164,19 @@ static void single_unlink_async(struct ehci_hcd *ehci, struct ehci_qh *qh)
 		ehci->qh_scan_next = qh->qh_next.qh;
 }
 
-static void start_iaa_cycle(struct ehci_hcd *ehci, bool nested)
+static void start_iaa_cycle(struct ehci_hcd *ehci)
 {
-	/*
-	 * Do nothing if an IAA cycle is already running or
-	 * if one will be started shortly.
-	 */
-	if (ehci->async_iaa || ehci->async_unlinking)
+	/* Do nothing if an IAA cycle is already running */
+	if (ehci->iaa_in_progress)
 		return;
+	ehci->iaa_in_progress = true;
 
 	/* If the controller isn't running, we don't have to wait for it */
 	if (unlikely(ehci->rh_state < EHCI_RH_RUNNING)) {
-
-		/* Do all the waiting QHs */
-		ehci->async_iaa = ehci->async_unlink;
-		ehci->async_unlink = NULL;
-
-		if (!nested)		/* Avoid recursion */
-			end_unlink_async(ehci);
+		end_unlink_async(ehci);
 
 	/* Otherwise start a new IAA cycle */
 	} else if (likely(ehci->rh_state == EHCI_RH_RUNNING)) {
-		struct ehci_qh		*qh;
-
-		/* Do only the first waiting QH (nVidia bug?) */
-		qh = ehci->async_unlink;
-
-		/*
-		 * Intel (?) bug: The HC can write back the overlay region
-		 * even after the IAA interrupt occurs.  In self-defense,
-		 * always go through two IAA cycles for each QH.
-		 */
-		if (qh->qh_state == QH_STATE_UNLINK_WAIT) {
-			qh->qh_state = QH_STATE_UNLINK;
-		} else {
-			ehci->async_iaa = qh;
-			ehci->async_unlink = qh->unlink_next;
-			qh->unlink_next = NULL;
-		}
 
 		/* Make sure the unlinks are all visible to the hardware */
 		wmb();
@@ -1250,36 +1193,73 @@ static void start_iaa_cycle(struct ehci_hcd *ehci, bool nested)
 static void end_unlink_async(struct ehci_hcd *ehci)
 {
 	struct ehci_qh		*qh;
+	bool			early_exit;
 
 	if (ehci->has_synopsys_hc_bug)
 		ehci_writel(ehci, (u32) ehci->async->qh_dma,
 			    &ehci->regs->async_next);
 
+	/* The current IAA cycle has ended */
+	ehci->iaa_in_progress = false;
+
+	if (list_empty(&ehci->async_unlink))
+		return;
+	qh = list_first_entry(&ehci->async_unlink, struct ehci_qh,
+			unlink_node);	/* QH whose IAA cycle just ended */
+
+	/*
+	 * If async_unlinking is set then this routine is already running,
+	 * either on the stack or on another CPU.
+	 */
+	early_exit = ehci->async_unlinking;
+
+	/* If the controller isn't running, process all the waiting QHs */
+	if (ehci->rh_state < EHCI_RH_RUNNING)
+		list_splice_tail_init(&ehci->async_unlink, &ehci->async_idle);
+
+	/*
+	 * Intel (?) bug: The HC can write back the overlay region even
+	 * after the IAA interrupt occurs.  In self-defense, always go
+	 * through two IAA cycles for each QH.
+	 */
+	else if (qh->qh_state == QH_STATE_UNLINK_WAIT) {
+		qh->qh_state = QH_STATE_UNLINK;
+		early_exit = true;
+	}
+
+	/* Otherwise process only the first waiting QH (NVIDIA bug?) */
+	else
+		list_move_tail(&qh->unlink_node, &ehci->async_idle);
+
+	/* Start a new IAA cycle if any QHs are waiting for it */
+	if (!list_empty(&ehci->async_unlink))
+		start_iaa_cycle(ehci);
+
+	/*
+	 * Don't allow nesting or concurrent calls,
+	 * or wait for the second IAA cycle for the next QH.
+	 */
+	if (early_exit)
+		return;
+
 	/* Process the idle QHs */
- restart:
 	ehci->async_unlinking = true;
-	while (ehci->async_iaa) {
-		qh = ehci->async_iaa;
-		ehci->async_iaa = qh->unlink_next;
-		qh->unlink_next = NULL;
+	while (!list_empty(&ehci->async_idle)) {
+		qh = list_first_entry(&ehci->async_idle, struct ehci_qh,
+				unlink_node);
+		list_del(&qh->unlink_node);
 
 		qh->qh_state = QH_STATE_IDLE;
 		qh->qh_next.qh = NULL;
 
-		qh_completions(ehci, qh);
+		if (!list_empty(&qh->qtd_list))
+			qh_completions(ehci, qh);
 		if (!list_empty(&qh->qtd_list) &&
 				ehci->rh_state == EHCI_RH_RUNNING)
 			qh_link_async(ehci, qh);
 		disable_async(ehci);
 	}
 	ehci->async_unlinking = false;
-
-	/* Start a new IAA cycle if any QHs are waiting for it */
-	if (ehci->async_unlink) {
-		start_iaa_cycle(ehci, true);
-		if (unlikely(ehci->rh_state < EHCI_RH_RUNNING))
-			goto restart;
-	}
 }
 
 static void start_unlink_async(struct ehci_hcd *ehci, struct ehci_qh *qh);
@@ -1288,7 +1268,6 @@ static void unlink_empty_async(struct ehci_hcd *ehci)
 {
 	struct ehci_qh		*qh;
 	struct ehci_qh		*qh_to_unlink = NULL;
-	bool			check_unlinks_later = false;
 	int			count = 0;
 
 	/* Find the last async QH which has been empty for a timer cycle */
@@ -1296,15 +1275,13 @@ static void unlink_empty_async(struct ehci_hcd *ehci)
 		if (list_empty(&qh->qtd_list) &&
 				qh->qh_state == QH_STATE_LINKED) {
 			++count;
-			if (qh->unlink_cycle == ehci->async_unlink_cycle)
-				check_unlinks_later = true;
-			else
+			if (qh->unlink_cycle != ehci->async_unlink_cycle)
 				qh_to_unlink = qh;
 		}
 	}
 
 	/* If nothing else is being unlinked, unlink the last empty QH */
-	if (!ehci->async_iaa && !ehci->async_unlink && qh_to_unlink) {
+	if (list_empty(&ehci->async_unlink) && qh_to_unlink) {
 		start_unlink_async(ehci, qh_to_unlink);
 		--count;
 	}
@@ -1317,7 +1294,7 @@ static void unlink_empty_async(struct ehci_hcd *ehci)
 }
 
 /* The root hub is suspended; unlink all the async QHs */
-static void unlink_empty_async_suspended(struct ehci_hcd *ehci)
+static void __maybe_unused unlink_empty_async_suspended(struct ehci_hcd *ehci)
 {
 	struct ehci_qh		*qh;
 
@@ -1326,7 +1303,7 @@ static void unlink_empty_async_suspended(struct ehci_hcd *ehci)
 		WARN_ON(!list_empty(&qh->qtd_list));
 		single_unlink_async(ehci, qh);
 	}
-	start_iaa_cycle(ehci, false);
+	start_iaa_cycle(ehci);
 }
 
 /* makes sure the async qh will become idle */
@@ -1334,19 +1311,12 @@ static void unlink_empty_async_suspended(struct ehci_hcd *ehci)
 
 static void start_unlink_async(struct ehci_hcd *ehci, struct ehci_qh *qh)
 {
-	/*
-	 * If the QH isn't linked then there's nothing we can do
-	 * unless we were called during a giveback, in which case
-	 * qh_completions() has to deal with it.
-	 */
-	if (qh->qh_state != QH_STATE_LINKED) {
-		if (qh->qh_state == QH_STATE_COMPLETING)
-			qh->needs_rescan = 1;
+	/* If the QH isn't linked then there's nothing we can do. */
+	if (qh->qh_state != QH_STATE_LINKED)
 		return;
-	}
 
 	single_unlink_async(ehci, qh);
-	start_iaa_cycle(ehci, false);
+	start_iaa_cycle(ehci);
 }
 
 /*-------------------------------------------------------------------------*/
@@ -1360,7 +1330,7 @@ static void scan_async (struct ehci_hcd *ehci)
 	while (ehci->qh_scan_next) {
 		qh = ehci->qh_scan_next;
 		ehci->qh_scan_next = qh->qh_next.qh;
- rescan:
+
 		/* clean any finished work for this qh */
 		if (!list_empty(&qh->qtd_list)) {
 			int temp;
@@ -1373,14 +1343,13 @@ static void scan_async (struct ehci_hcd *ehci)
 			 * in single_unlink_async().
 			 */
 			temp = qh_completions(ehci, qh);
-			if (qh->needs_rescan) {
+			if (unlikely(temp)) {
 				start_unlink_async(ehci, qh);
 			} else if (list_empty(&qh->qtd_list)
 					&& qh->qh_state == QH_STATE_LINKED) {
 				qh->unlink_cycle = ehci->async_unlink_cycle;
 				check_unlinks_later = true;
-			} else if (temp != 0)
-				goto rescan;
+			}
 		}
 	}
 
diff --git a/drivers/usb/host/ehci-s5p.c b/drivers/usb/host/ehci-s5p.c
index 20ebf6a8b7f..635775278c7 100644
--- a/drivers/usb/host/ehci-s5p.c
+++ b/drivers/usb/host/ehci-s5p.c
@@ -13,13 +13,23 @@
  */
 
 #include <linux/clk.h>
+#include <linux/dma-mapping.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
 #include <linux/of.h>
-#include <linux/platform_device.h>
 #include <linux/of_gpio.h>
+#include <linux/platform_device.h>
 #include <linux/platform_data/usb-ehci-s5p.h>
 #include <linux/usb/phy.h>
 #include <linux/usb/samsung_usb_phy.h>
-#include <plat/usb-phy.h>
+#include <linux/usb.h>
+#include <linux/usb/hcd.h>
+#include <linux/usb/otg.h>
+
+#include "ehci.h"
+
+#define DRIVER_DESC "EHCI s5p driver"
 
 #define EHCI_INSNREG00(base)			(base + 0x90)
 #define EHCI_INSNREG00_ENA_INCR16		(0x1 << 25)
@@ -30,82 +40,35 @@
 	(EHCI_INSNREG00_ENA_INCR16 | EHCI_INSNREG00_ENA_INCR8 |	\
 	 EHCI_INSNREG00_ENA_INCR4 | EHCI_INSNREG00_ENA_INCRX_ALIGN)
 
+static const char hcd_name[] = "ehci-s5p";
+static struct hc_driver __read_mostly s5p_ehci_hc_driver;
+
 struct s5p_ehci_hcd {
-	struct device *dev;
-	struct usb_hcd *hcd;
 	struct clk *clk;
 	struct usb_phy *phy;
 	struct usb_otg *otg;
 	struct s5p_ehci_platdata *pdata;
 };
 
-static const struct hc_driver s5p_ehci_hc_driver = {
-	.description		= hcd_name,
-	.product_desc		= "S5P EHCI Host Controller",
-	.hcd_priv_size		= sizeof(struct ehci_hcd),
-
-	.irq			= ehci_irq,
-	.flags			= HCD_MEMORY | HCD_USB2,
-
-	.reset			= ehci_setup,
-	.start			= ehci_run,
-	.stop			= ehci_stop,
-	.shutdown		= ehci_shutdown,
-
-	.get_frame_number	= ehci_get_frame,
-
-	.urb_enqueue		= ehci_urb_enqueue,
-	.urb_dequeue		= ehci_urb_dequeue,
-	.endpoint_disable	= ehci_endpoint_disable,
-	.endpoint_reset		= ehci_endpoint_reset,
-
-	.hub_status_data	= ehci_hub_status_data,
-	.hub_control		= ehci_hub_control,
-	.bus_suspend		= ehci_bus_suspend,
-	.bus_resume		= ehci_bus_resume,
-
-	.relinquish_port	= ehci_relinquish_port,
-	.port_handed_over	= ehci_port_handed_over,
-
-	.clear_tt_buffer_complete	= ehci_clear_tt_buffer_complete,
-};
-
-static void s5p_ehci_phy_enable(struct s5p_ehci_hcd *s5p_ehci)
-{
-	struct platform_device *pdev = to_platform_device(s5p_ehci->dev);
-
-	if (s5p_ehci->phy)
-		usb_phy_init(s5p_ehci->phy);
-	else if (s5p_ehci->pdata->phy_init)
-		s5p_ehci->pdata->phy_init(pdev, USB_PHY_TYPE_HOST);
-}
-
-static void s5p_ehci_phy_disable(struct s5p_ehci_hcd *s5p_ehci)
-{
-	struct platform_device *pdev = to_platform_device(s5p_ehci->dev);
-
-	if (s5p_ehci->phy)
-		usb_phy_shutdown(s5p_ehci->phy);
-	else if (s5p_ehci->pdata->phy_exit)
-		s5p_ehci->pdata->phy_exit(pdev, USB_PHY_TYPE_HOST);
-}
+#define to_s5p_ehci(hcd)      (struct s5p_ehci_hcd *)(hcd_to_ehci(hcd)->priv)
 
 static void s5p_setup_vbus_gpio(struct platform_device *pdev)
 {
+	struct device *dev = &pdev->dev;
 	int err;
 	int gpio;
 
-	if (!pdev->dev.of_node)
+	if (!dev->of_node)
 		return;
 
-	gpio = of_get_named_gpio(pdev->dev.of_node,
-			"samsung,vbus-gpio", 0);
+	gpio = of_get_named_gpio(dev->of_node, "samsung,vbus-gpio", 0);
 	if (!gpio_is_valid(gpio))
 		return;
 
-	err = gpio_request_one(gpio, GPIOF_OUT_INIT_HIGH, "ehci_vbus_gpio");
+	err = devm_gpio_request_one(dev, gpio, GPIOF_OUT_INIT_HIGH,
+				    "ehci_vbus_gpio");
 	if (err)
-		dev_err(&pdev->dev, "can't request ehci vbus gpio %d", gpio);
+		dev_err(dev, "can't request ehci vbus gpio %d", gpio);
 }
 
 static u64 ehci_s5p_dma_mask = DMA_BIT_MASK(32);
@@ -133,13 +96,15 @@ static int s5p_ehci_probe(struct platform_device *pdev)
 
 	s5p_setup_vbus_gpio(pdev);
 
-	s5p_ehci = devm_kzalloc(&pdev->dev, sizeof(struct s5p_ehci_hcd),
-				GFP_KERNEL);
-	if (!s5p_ehci)
+	hcd = usb_create_hcd(&s5p_ehci_hc_driver,
+			     &pdev->dev, dev_name(&pdev->dev));
+	if (!hcd) {
+		dev_err(&pdev->dev, "Unable to create HCD\n");
 		return -ENOMEM;
-
+	}
+	s5p_ehci = to_s5p_ehci(hcd);
 	phy = devm_usb_get_phy(&pdev->dev, USB_PHY_TYPE_USB2);
-	if (IS_ERR_OR_NULL(phy)) {
+	if (IS_ERR(phy)) {
 		/* Fallback to pdata */
 		if (!pdata) {
 			dev_warn(&pdev->dev, "no platform data or transceiver defined\n");
@@ -152,16 +117,6 @@ static int s5p_ehci_probe(struct platform_device *pdev)
 		s5p_ehci->otg = phy->otg;
 	}
 
-	s5p_ehci->dev = &pdev->dev;
-
-	hcd = usb_create_hcd(&s5p_ehci_hc_driver, &pdev->dev,
-					dev_name(&pdev->dev));
-	if (!hcd) {
-		dev_err(&pdev->dev, "Unable to create HCD\n");
-		return -ENOMEM;
-	}
-
-	s5p_ehci->hcd = hcd;
 	s5p_ehci->clk = devm_clk_get(&pdev->dev, "usbhost");
 
 	if (IS_ERR(s5p_ehci->clk)) {
@@ -198,9 +153,12 @@ static int s5p_ehci_probe(struct platform_device *pdev)
 	}
 
 	if (s5p_ehci->otg)
-		s5p_ehci->otg->set_host(s5p_ehci->otg, &s5p_ehci->hcd->self);
+		s5p_ehci->otg->set_host(s5p_ehci->otg, &hcd->self);
 
-	s5p_ehci_phy_enable(s5p_ehci);
+	if (s5p_ehci->phy)
+		usb_phy_init(s5p_ehci->phy);
+	else if (s5p_ehci->pdata->phy_init)
+		s5p_ehci->pdata->phy_init(pdev, USB_PHY_TYPE_HOST);
 
 	ehci = hcd_to_ehci(hcd);
 	ehci->caps = hcd->regs;
@@ -214,12 +172,15 @@ static int s5p_ehci_probe(struct platform_device *pdev)
 		goto fail_add_hcd;
 	}
 
-	platform_set_drvdata(pdev, s5p_ehci);
+	platform_set_drvdata(pdev, hcd);
 
 	return 0;
 
 fail_add_hcd:
-	s5p_ehci_phy_disable(s5p_ehci);
+	if (s5p_ehci->phy)
+		usb_phy_shutdown(s5p_ehci->phy);
+	else if (s5p_ehci->pdata->phy_exit)
+		s5p_ehci->pdata->phy_exit(pdev, USB_PHY_TYPE_HOST);
 fail_io:
 	clk_disable_unprepare(s5p_ehci->clk);
 fail_clk:
@@ -229,15 +190,18 @@ fail_clk:
 
 static int s5p_ehci_remove(struct platform_device *pdev)
 {
-	struct s5p_ehci_hcd *s5p_ehci = platform_get_drvdata(pdev);
-	struct usb_hcd *hcd = s5p_ehci->hcd;
+	struct usb_hcd *hcd = platform_get_drvdata(pdev);
+	struct s5p_ehci_hcd *s5p_ehci = to_s5p_ehci(hcd);
 
 	usb_remove_hcd(hcd);
 
 	if (s5p_ehci->otg)
-		s5p_ehci->otg->set_host(s5p_ehci->otg, &s5p_ehci->hcd->self);
+		s5p_ehci->otg->set_host(s5p_ehci->otg, &hcd->self);
 
-	s5p_ehci_phy_disable(s5p_ehci);
+	if (s5p_ehci->phy)
+		usb_phy_shutdown(s5p_ehci->phy);
+	else if (s5p_ehci->pdata->phy_exit)
+		s5p_ehci->pdata->phy_exit(pdev, USB_PHY_TYPE_HOST);
 
 	clk_disable_unprepare(s5p_ehci->clk);
 
@@ -248,8 +212,7 @@ static int s5p_ehci_remove(struct platform_device *pdev)
 
 static void s5p_ehci_shutdown(struct platform_device *pdev)
 {
-	struct s5p_ehci_hcd *s5p_ehci = platform_get_drvdata(pdev);
-	struct usb_hcd *hcd = s5p_ehci->hcd;
+	struct usb_hcd *hcd = platform_get_drvdata(pdev);
 
 	if (hcd->driver->shutdown)
 		hcd->driver->shutdown(hcd);
@@ -258,17 +221,22 @@ static void s5p_ehci_shutdown(struct platform_device *pdev)
 #ifdef CONFIG_PM
 static int s5p_ehci_suspend(struct device *dev)
 {
-	struct s5p_ehci_hcd *s5p_ehci = dev_get_drvdata(dev);
-	struct usb_hcd *hcd = s5p_ehci->hcd;
+	struct usb_hcd *hcd = dev_get_drvdata(dev);
+	struct s5p_ehci_hcd *s5p_ehci = to_s5p_ehci(hcd);
+	struct platform_device *pdev = to_platform_device(dev);
+
 	bool do_wakeup = device_may_wakeup(dev);
 	int rc;
 
 	rc = ehci_suspend(hcd, do_wakeup);
 
 	if (s5p_ehci->otg)
-		s5p_ehci->otg->set_host(s5p_ehci->otg, &s5p_ehci->hcd->self);
+		s5p_ehci->otg->set_host(s5p_ehci->otg, &hcd->self);
 
-	s5p_ehci_phy_disable(s5p_ehci);
+	if (s5p_ehci->phy)
+		usb_phy_shutdown(s5p_ehci->phy);
+	else if (s5p_ehci->pdata->phy_exit)
+		s5p_ehci->pdata->phy_exit(pdev, USB_PHY_TYPE_HOST);
 
 	clk_disable_unprepare(s5p_ehci->clk);
 
@@ -277,15 +245,19 @@ static int s5p_ehci_suspend(struct device *dev)
 
 static int s5p_ehci_resume(struct device *dev)
 {
-	struct s5p_ehci_hcd *s5p_ehci = dev_get_drvdata(dev);
-	struct usb_hcd *hcd = s5p_ehci->hcd;
+	struct usb_hcd *hcd = dev_get_drvdata(dev);
+	struct  s5p_ehci_hcd *s5p_ehci = to_s5p_ehci(hcd);
+	struct platform_device *pdev = to_platform_device(dev);
 
 	clk_prepare_enable(s5p_ehci->clk);
 
 	if (s5p_ehci->otg)
-		s5p_ehci->otg->set_host(s5p_ehci->otg, &s5p_ehci->hcd->self);
+		s5p_ehci->otg->set_host(s5p_ehci->otg, &hcd->self);
 
-	s5p_ehci_phy_enable(s5p_ehci);
+	if (s5p_ehci->phy)
+		usb_phy_init(s5p_ehci->phy);
+	else if (s5p_ehci->pdata->phy_init)
+		s5p_ehci->pdata->phy_init(pdev, USB_PHY_TYPE_HOST);
 
 	/* DMA burst Enable */
 	writel(EHCI_INSNREG00_ENABLE_DMA_BURST, EHCI_INSNREG00(hcd->regs));
@@ -322,5 +294,29 @@ static struct platform_driver s5p_ehci_driver = {
 		.of_match_table = of_match_ptr(exynos_ehci_match),
 	}
 };
+static const struct ehci_driver_overrides s5p_overrides __initdata = {
+	.extra_priv_size = sizeof(struct s5p_ehci_hcd),
+};
+
+static int __init ehci_s5p_init(void)
+{
+	if (usb_disabled())
+		return -ENODEV;
+
+	pr_info("%s: " DRIVER_DESC "\n", hcd_name);
+	ehci_init_driver(&s5p_ehci_hc_driver, &s5p_overrides);
+	return platform_driver_register(&s5p_ehci_driver);
+}
+module_init(ehci_s5p_init);
+
+static void __exit ehci_s5p_cleanup(void)
+{
+	platform_driver_unregister(&s5p_ehci_driver);
+}
+module_exit(ehci_s5p_cleanup);
 
+MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_ALIAS("platform:s5p-ehci");
+MODULE_AUTHOR("Jingoo Han");
+MODULE_AUTHOR("Joonyoung Shim");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/usb/host/ehci-sched.c b/drivers/usb/host/ehci-sched.c
index b476daf49f6..acff5b8f6e8 100644
--- a/drivers/usb/host/ehci-sched.c
+++ b/drivers/usb/host/ehci-sched.c
@@ -539,6 +539,7 @@ static void qh_link_periodic(struct ehci_hcd *ehci, struct ehci_qh *qh)
 	}
 	qh->qh_state = QH_STATE_LINKED;
 	qh->xacterrs = 0;
+	qh->exception = 0;
 
 	/* update per-qh bandwidth for usbfs */
 	ehci_to_hcd(ehci)->self.bandwidth_allocated += qh->period
@@ -602,15 +603,9 @@ static void qh_unlink_periodic(struct ehci_hcd *ehci, struct ehci_qh *qh)
 
 static void start_unlink_intr(struct ehci_hcd *ehci, struct ehci_qh *qh)
 {
-	/* If the QH isn't linked then there's nothing we can do
-	 * unless we were called during a giveback, in which case
-	 * qh_completions() has to deal with it.
-	 */
-	if (qh->qh_state != QH_STATE_LINKED) {
-		if (qh->qh_state == QH_STATE_COMPLETING)
-			qh->needs_rescan = 1;
+	/* If the QH isn't linked then there's nothing we can do. */
+	if (qh->qh_state != QH_STATE_LINKED)
 		return;
-	}
 
 	qh_unlink_periodic (ehci, qh);
 
@@ -625,17 +620,13 @@ static void start_unlink_intr(struct ehci_hcd *ehci, struct ehci_qh *qh)
 	qh->unlink_cycle = ehci->intr_unlink_cycle;
 
 	/* New entries go at the end of the intr_unlink list */
-	if (ehci->intr_unlink)
-		ehci->intr_unlink_last->unlink_next = qh;
-	else
-		ehci->intr_unlink = qh;
-	ehci->intr_unlink_last = qh;
+	list_add_tail(&qh->unlink_node, &ehci->intr_unlink);
 
 	if (ehci->intr_unlinking)
 		;	/* Avoid recursive calls */
 	else if (ehci->rh_state < EHCI_RH_RUNNING)
 		ehci_handle_intr_unlinks(ehci);
-	else if (ehci->intr_unlink == qh) {
+	else if (ehci->intr_unlink.next == &qh->unlink_node) {
 		ehci_enable_event(ehci, EHCI_HRTIMER_UNLINK_INTR, true);
 		++ehci->intr_unlink_cycle;
 	}
@@ -649,7 +640,8 @@ static void end_unlink_intr(struct ehci_hcd *ehci, struct ehci_qh *qh)
 	qh->qh_state = QH_STATE_IDLE;
 	hw->hw_next = EHCI_LIST_END(ehci);
 
-	qh_completions(ehci, qh);
+	if (!list_empty(&qh->qtd_list))
+		qh_completions(ehci, qh);
 
 	/* reschedule QH iff another request is queued */
 	if (!list_empty(&qh->qtd_list) && ehci->rh_state == EHCI_RH_RUNNING) {
@@ -792,7 +784,6 @@ static int qh_schedule(struct ehci_hcd *ehci, struct ehci_qh *qh)
 	unsigned	frame;		/* 0..(qh->period - 1), or NO_FRAME */
 	struct ehci_qh_hw	*hw = qh->hw;
 
-	qh_refresh(ehci, qh);
 	hw->hw_next = EHCI_LIST_END(ehci);
 	frame = qh->start;
 
@@ -844,8 +835,6 @@ static int qh_schedule(struct ehci_hcd *ehci, struct ehci_qh *qh)
 	} else
 		ehci_dbg (ehci, "reused qh %p schedule\n", qh);
 
-	/* stuff into the periodic schedule */
-	qh_link_periodic(ehci, qh);
 done:
 	return status;
 }
@@ -891,6 +880,12 @@ static int intr_submit (
 	qh = qh_append_tds(ehci, urb, qtd_list, epnum, &urb->ep->hcpriv);
 	BUG_ON (qh == NULL);
 
+	/* stuff into the periodic schedule */
+	if (qh->qh_state == QH_STATE_IDLE) {
+		qh_refresh(ehci, qh);
+		qh_link_periodic(ehci, qh);
+	}
+
 	/* ... update usbfs periodic stats */
 	ehci_to_hcd(ehci)->self.bandwidth_int_reqs++;
 
@@ -911,7 +906,7 @@ static void scan_intr(struct ehci_hcd *ehci)
 
 	list_for_each_entry_safe(qh, ehci->qh_scan_next, &ehci->intr_qh_list,
 			intr_node) {
- rescan:
+
 		/* clean any finished work for this qh */
 		if (!list_empty(&qh->qtd_list)) {
 			int temp;
@@ -924,12 +919,9 @@ static void scan_intr(struct ehci_hcd *ehci)
 			 * in qh_unlink_periodic().
 			 */
 			temp = qh_completions(ehci, qh);
-			if (unlikely(qh->needs_rescan ||
-					(list_empty(&qh->qtd_list) &&
-						qh->qh_state == QH_STATE_LINKED)))
+			if (unlikely(temp || (list_empty(&qh->qtd_list) &&
+					qh->qh_state == QH_STATE_LINKED)))
 				start_unlink_intr(ehci, qh);
-			else if (temp != 0)
-				goto rescan;
 		}
 	}
 }
@@ -1214,6 +1206,7 @@ itd_urb_transaction (
 
 		memset (itd, 0, sizeof *itd);
 		itd->itd_dma = itd_dma;
+		itd->frame = 9999;		/* an invalid value */
 		list_add (&itd->itd_list, &sched->td_list);
 	}
 	spin_unlock_irqrestore (&ehci->lock, flags);
@@ -1915,6 +1908,7 @@ sitd_urb_transaction (
 
 		memset (sitd, 0, sizeof *sitd);
 		sitd->sitd_dma = sitd_dma;
+		sitd->frame = 9999;		/* an invalid value */
 		list_add (&sitd->sitd_list, &iso_sched->td_list);
 	}
 
diff --git a/drivers/usb/host/ehci-sh.c b/drivers/usb/host/ehci-sh.c
index 3565a300f40..b44d716ddc8 100644
--- a/drivers/usb/host/ehci-sh.c
+++ b/drivers/usb/host/ehci-sh.c
@@ -77,7 +77,6 @@ static const struct hc_driver ehci_sh_hc_driver = {
 
 static int ehci_hcd_sh_probe(struct platform_device *pdev)
 {
-	const struct hc_driver *driver = &ehci_sh_hc_driver;
 	struct resource *res;
 	struct ehci_sh_priv *priv;
 	struct ehci_sh_platdata *pdata;
@@ -170,7 +169,7 @@ fail_create_hcd:
 	return ret;
 }
 
-static int __exit ehci_hcd_sh_remove(struct platform_device *pdev)
+static int ehci_hcd_sh_remove(struct platform_device *pdev)
 {
 	struct ehci_sh_priv *priv = platform_get_drvdata(pdev);
 	struct usb_hcd *hcd = priv->hcd;
@@ -196,7 +195,7 @@ static void ehci_hcd_sh_shutdown(struct platform_device *pdev)
 
 static struct platform_driver ehci_hcd_sh_driver = {
 	.probe		= ehci_hcd_sh_probe,
-	.remove		= __exit_p(ehci_hcd_sh_remove),
+	.remove		= ehci_hcd_sh_remove,
 	.shutdown	= ehci_hcd_sh_shutdown,
 	.driver		= {
 		.name	= "sh_ehci",
diff --git a/drivers/usb/host/ehci-spear.c b/drivers/usb/host/ehci-spear.c
index 466c1bb5b96..61ecfb3d52f 100644
--- a/drivers/usb/host/ehci-spear.c
+++ b/drivers/usb/host/ehci-spear.c
@@ -1,5 +1,5 @@
 /*
-* Driver for EHCI HCD on SPEAR SOC
+* Driver for EHCI HCD on SPEAr SOC
 *
 * Copyright (C) 2010 ST Micro Electronics,
 * Deepak Sikri <deepak.sikri@st.com>
@@ -12,73 +12,32 @@
 */
 
 #include <linux/clk.h>
+#include <linux/dma-mapping.h>
+#include <linux/io.h>
 #include <linux/jiffies.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/pm.h>
+#include <linux/usb.h>
+#include <linux/usb/hcd.h>
 
-struct spear_ehci {
-	struct ehci_hcd ehci;
-	struct clk *clk;
-};
-
-#define to_spear_ehci(hcd)	(struct spear_ehci *)hcd_to_ehci(hcd)
+#include "ehci.h"
 
-static void spear_start_ehci(struct spear_ehci *ehci)
-{
-	clk_prepare_enable(ehci->clk);
-}
+#define DRIVER_DESC "EHCI SPEAr driver"
 
-static void spear_stop_ehci(struct spear_ehci *ehci)
-{
-	clk_disable_unprepare(ehci->clk);
-}
+static const char hcd_name[] = "SPEAr-ehci";
 
-static int ehci_spear_setup(struct usb_hcd *hcd)
-{
-	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
-
-	/* registers start at offset 0x0 */
-	ehci->caps = hcd->regs;
+struct spear_ehci {
+	struct clk *clk;
+};
 
-	return ehci_setup(hcd);
-}
+#define to_spear_ehci(hcd)	(struct spear_ehci *)(hcd_to_ehci(hcd)->priv)
 
-static const struct hc_driver ehci_spear_hc_driver = {
-	.description			= hcd_name,
-	.product_desc			= "SPEAr EHCI",
-	.hcd_priv_size			= sizeof(struct spear_ehci),
-
-	/* generic hardware linkage */
-	.irq				= ehci_irq,
-	.flags				= HCD_MEMORY | HCD_USB2,
-
-	/* basic lifecycle operations */
-	.reset				= ehci_spear_setup,
-	.start				= ehci_run,
-	.stop				= ehci_stop,
-	.shutdown			= ehci_shutdown,
-
-	/* managing i/o requests and associated device resources */
-	.urb_enqueue			= ehci_urb_enqueue,
-	.urb_dequeue			= ehci_urb_dequeue,
-	.endpoint_disable		= ehci_endpoint_disable,
-	.endpoint_reset			= ehci_endpoint_reset,
-
-	/* scheduling support */
-	.get_frame_number		= ehci_get_frame,
-
-	/* root hub support */
-	.hub_status_data		= ehci_hub_status_data,
-	.hub_control			= ehci_hub_control,
-	.bus_suspend			= ehci_bus_suspend,
-	.bus_resume			= ehci_bus_resume,
-	.relinquish_port		= ehci_relinquish_port,
-	.port_handed_over		= ehci_port_handed_over,
-	.clear_tt_buffer_complete	= ehci_clear_tt_buffer_complete,
-};
+static struct hc_driver __read_mostly ehci_spear_hc_driver;
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int ehci_spear_drv_suspend(struct device *dev)
 {
 	struct usb_hcd *hcd = dev_get_drvdata(dev);
@@ -94,7 +53,7 @@ static int ehci_spear_drv_resume(struct device *dev)
 	ehci_resume(hcd, false);
 	return 0;
 }
-#endif /* CONFIG_PM */
+#endif /* CONFIG_PM_SLEEP */
 
 static SIMPLE_DEV_PM_OPS(ehci_spear_pm_ops, ehci_spear_drv_suspend,
 		ehci_spear_drv_resume);
@@ -104,7 +63,7 @@ static u64 spear_ehci_dma_mask = DMA_BIT_MASK(32);
 static int spear_ehci_hcd_drv_probe(struct platform_device *pdev)
 {
 	struct usb_hcd *hcd ;
-	struct spear_ehci *ehci;
+	struct spear_ehci *sehci;
 	struct resource *res;
 	struct clk *usbh_clk;
 	const struct hc_driver *driver = &ehci_spear_hc_driver;
@@ -161,10 +120,13 @@ static int spear_ehci_hcd_drv_probe(struct platform_device *pdev)
 		goto err_put_hcd;
 	}
 
-	ehci = (struct spear_ehci *)hcd_to_ehci(hcd);
-	ehci->clk = usbh_clk;
+	sehci = to_spear_ehci(hcd);
+	sehci->clk = usbh_clk;
+
+	/* registers start at offset 0x0 */
+	hcd_to_ehci(hcd)->caps = hcd->regs;
 
-	spear_start_ehci(ehci);
+	clk_prepare_enable(sehci->clk);
 	retval = usb_add_hcd(hcd, irq, IRQF_SHARED);
 	if (retval)
 		goto err_stop_ehci;
@@ -172,7 +134,7 @@ static int spear_ehci_hcd_drv_probe(struct platform_device *pdev)
 	return retval;
 
 err_stop_ehci:
-	spear_stop_ehci(ehci);
+	clk_disable_unprepare(sehci->clk);
 err_put_hcd:
 	usb_put_hcd(hcd);
 fail:
@@ -184,7 +146,7 @@ fail:
 static int spear_ehci_hcd_drv_remove(struct platform_device *pdev)
 {
 	struct usb_hcd *hcd = platform_get_drvdata(pdev);
-	struct spear_ehci *ehci_p = to_spear_ehci(hcd);
+	struct spear_ehci *sehci = to_spear_ehci(hcd);
 
 	if (!hcd)
 		return 0;
@@ -192,8 +154,8 @@ static int spear_ehci_hcd_drv_remove(struct platform_device *pdev)
 		BUG();
 	usb_remove_hcd(hcd);
 
-	if (ehci_p->clk)
-		spear_stop_ehci(ehci_p);
+	if (sehci->clk)
+		clk_disable_unprepare(sehci->clk);
 	usb_put_hcd(hcd);
 
 	return 0;
@@ -216,4 +178,29 @@ static struct platform_driver spear_ehci_hcd_driver = {
 	}
 };
 
+static const struct ehci_driver_overrides spear_overrides __initdata = {
+	.extra_priv_size = sizeof(struct spear_ehci),
+};
+
+static int __init ehci_spear_init(void)
+{
+	if (usb_disabled())
+		return -ENODEV;
+
+	pr_info("%s: " DRIVER_DESC "\n", hcd_name);
+
+	ehci_init_driver(&ehci_spear_hc_driver, &spear_overrides);
+	return platform_driver_register(&spear_ehci_hcd_driver);
+}
+module_init(ehci_spear_init);
+
+static void __exit ehci_spear_cleanup(void)
+{
+	platform_driver_unregister(&spear_ehci_hcd_driver);
+}
+module_exit(ehci_spear_cleanup);
+
+MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_ALIAS("platform:spear-ehci");
+MODULE_AUTHOR("Deepak Sikri");
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/host/ehci-tegra.c b/drivers/usb/host/ehci-tegra.c
index 568aecc7075..e3eddc31ac8 100644
--- a/drivers/usb/host/ehci-tegra.c
+++ b/drivers/usb/host/ehci-tegra.c
@@ -28,6 +28,7 @@
 #include <linux/pm_runtime.h>
 #include <linux/usb/ehci_def.h>
 #include <linux/usb/tegra_usb_phy.h>
+#include <linux/clk/tegra.h>
 
 #define TEGRA_USB_BASE			0xC5000000
 #define TEGRA_USB2_BASE			0xC5004000
@@ -610,7 +611,7 @@ static const struct dev_pm_ops tegra_ehci_pm_ops = {
 /* Bits of PORTSC1, which will get cleared by writing 1 into them */
 #define TEGRA_PORTSC1_RWC_BITS (PORT_CSC | PORT_PEC | PORT_OCC)
 
-void tegra_ehci_set_pts(struct usb_phy *x, u8 pts_val)
+static void tegra_ehci_set_pts(struct usb_phy *x, u8 pts_val)
 {
 	unsigned long val;
 	struct usb_hcd *hcd = bus_to_hcd(x->otg->host);
@@ -621,9 +622,8 @@ void tegra_ehci_set_pts(struct usb_phy *x, u8 pts_val)
 	val |= TEGRA_USB_PORTSC1_PTS(pts_val & 3);
 	writel(val, base + TEGRA_USB_PORTSC1);
 }
-EXPORT_SYMBOL_GPL(tegra_ehci_set_pts);
 
-void tegra_ehci_set_phcd(struct usb_phy *x, bool enable)
+static void tegra_ehci_set_phcd(struct usb_phy *x, bool enable)
 {
 	unsigned long val;
 	struct usb_hcd *hcd = bus_to_hcd(x->otg->host);
@@ -636,7 +636,6 @@ void tegra_ehci_set_phcd(struct usb_phy *x, bool enable)
 		val &= ~TEGRA_USB_PORTSC1_PHCD;
 	writel(val, base + TEGRA_USB_PORTSC1);
 }
-EXPORT_SYMBOL_GPL(tegra_ehci_set_phcd);
 
 static u64 tegra_ehci_dma_mask = DMA_BIT_MASK(32);
 
@@ -691,6 +690,10 @@ static int tegra_ehci_probe(struct platform_device *pdev)
 	if (err)
 		goto fail_clk;
 
+	tegra_periph_reset_assert(tegra->clk);
+	udelay(1);
+	tegra_periph_reset_deassert(tegra->clk);
+
 	tegra->needs_double_reset = of_property_read_bool(pdev->dev.of_node,
 		"nvidia,needs-double-reset");
 
@@ -733,7 +736,9 @@ static int tegra_ehci_probe(struct platform_device *pdev)
 
 	tegra->phy = tegra_usb_phy_open(&pdev->dev, instance, hcd->regs,
 					pdata->phy_config,
-					TEGRA_USB_PHY_MODE_HOST);
+					TEGRA_USB_PHY_MODE_HOST,
+					tegra_ehci_set_pts,
+					tegra_ehci_set_phcd);
 	if (IS_ERR(tegra->phy)) {
 		dev_err(&pdev->dev, "Failed to open USB phy\n");
 		err = -ENXIO;
@@ -755,7 +760,7 @@ static int tegra_ehci_probe(struct platform_device *pdev)
 	err = usb_phy_set_suspend(hcd->phy, 0);
 	if (err) {
 		dev_err(&pdev->dev, "Failed to power on the phy\n");
-		goto fail;
+		goto fail_phy;
 	}
 
 	tegra->host_resumed = 1;
@@ -765,17 +770,17 @@ static int tegra_ehci_probe(struct platform_device *pdev)
 	if (!irq) {
 		dev_err(&pdev->dev, "Failed to get IRQ\n");
 		err = -ENODEV;
-		goto fail;
+		goto fail_phy;
 	}
 
-#ifdef CONFIG_USB_OTG_UTILS
 	if (pdata->operating_mode == TEGRA_USB_OTG) {
 		tegra->transceiver =
 			devm_usb_get_phy(&pdev->dev, USB_PHY_TYPE_USB2);
-		if (!IS_ERR_OR_NULL(tegra->transceiver))
+		if (!IS_ERR(tegra->transceiver))
 			otg_set_host(tegra->transceiver->otg, &hcd->self);
+	} else {
+		tegra->transceiver = ERR_PTR(-ENODEV);
 	}
-#endif
 
 	err = usb_add_hcd(hcd, irq, IRQF_SHARED);
 	if (err) {
@@ -794,10 +799,9 @@ static int tegra_ehci_probe(struct platform_device *pdev)
 	return err;
 
 fail:
-#ifdef CONFIG_USB_OTG_UTILS
-	if (!IS_ERR_OR_NULL(tegra->transceiver))
+	if (!IS_ERR(tegra->transceiver))
 		otg_set_host(tegra->transceiver->otg, NULL);
-#endif
+fail_phy:
 	usb_phy_shutdown(hcd->phy);
 fail_io:
 	clk_disable_unprepare(tegra->clk);
@@ -815,10 +819,8 @@ static int tegra_ehci_remove(struct platform_device *pdev)
 	pm_runtime_disable(&pdev->dev);
 	pm_runtime_put_noidle(&pdev->dev);
 
-#ifdef CONFIG_USB_OTG_UTILS
-	if (!IS_ERR_OR_NULL(tegra->transceiver))
+	if (!IS_ERR(tegra->transceiver))
 		otg_set_host(tegra->transceiver->otg, NULL);
-#endif
 
 	usb_phy_shutdown(hcd->phy);
 	usb_remove_hcd(hcd);
diff --git a/drivers/usb/host/ehci-timer.c b/drivers/usb/host/ehci-timer.c
index c3fa1305f83..11e5b32f73e 100644
--- a/drivers/usb/host/ehci-timer.c
+++ b/drivers/usb/host/ehci-timer.c
@@ -113,8 +113,8 @@ static void ehci_poll_ASS(struct ehci_hcd *ehci)
 
 	if (want != actual) {
 
-		/* Poll again later, but give up after about 20 ms */
-		if (ehci->ASS_poll_count++ < 20) {
+		/* Poll again later, but give up after about 2-4 ms */
+		if (ehci->ASS_poll_count++ < 2) {
 			ehci_enable_event(ehci, EHCI_HRTIMER_POLL_ASS, true);
 			return;
 		}
@@ -159,8 +159,8 @@ static void ehci_poll_PSS(struct ehci_hcd *ehci)
 
 	if (want != actual) {
 
-		/* Poll again later, but give up after about 20 ms */
-		if (ehci->PSS_poll_count++ < 20) {
+		/* Poll again later, but give up after about 2-4 ms */
+		if (ehci->PSS_poll_count++ < 2) {
 			ehci_enable_event(ehci, EHCI_HRTIMER_POLL_PSS, true);
 			return;
 		}
@@ -229,18 +229,19 @@ static void ehci_handle_intr_unlinks(struct ehci_hcd *ehci)
 	 * process all the QHs on the list.
 	 */
 	ehci->intr_unlinking = true;
-	while (ehci->intr_unlink) {
-		struct ehci_qh	*qh = ehci->intr_unlink;
+	while (!list_empty(&ehci->intr_unlink)) {
+		struct ehci_qh	*qh;
 
+		qh = list_first_entry(&ehci->intr_unlink, struct ehci_qh,
+				unlink_node);
 		if (!stopped && qh->unlink_cycle == ehci->intr_unlink_cycle)
 			break;
-		ehci->intr_unlink = qh->unlink_next;
-		qh->unlink_next = NULL;
+		list_del(&qh->unlink_node);
 		end_unlink_intr(ehci, qh);
 	}
 
 	/* Handle remaining entries later */
-	if (ehci->intr_unlink) {
+	if (!list_empty(&ehci->intr_unlink)) {
 		ehci_enable_event(ehci, EHCI_HRTIMER_UNLINK_INTR, true);
 		++ehci->intr_unlink_cycle;
 	}
@@ -295,8 +296,7 @@ static void end_free_itds(struct ehci_hcd *ehci)
 /* Handle lost (or very late) IAA interrupts */
 static void ehci_iaa_watchdog(struct ehci_hcd *ehci)
 {
-	if (ehci->rh_state != EHCI_RH_RUNNING)
-		return;
+	u32 cmd, status;
 
 	/*
 	 * Lost IAA irqs wedge things badly; seen first with a vt8235.
@@ -304,34 +304,32 @@ static void ehci_iaa_watchdog(struct ehci_hcd *ehci)
 	 * (a) SMP races against real IAA firing and retriggering, and
 	 * (b) clean HC shutdown, when IAA watchdog was pending.
 	 */
-	if (1) {
-		u32 cmd, status;
-
-		/* If we get here, IAA is *REALLY* late.  It's barely
-		 * conceivable that the system is so busy that CMD_IAAD
-		 * is still legitimately set, so let's be sure it's
-		 * clear before we read STS_IAA.  (The HC should clear
-		 * CMD_IAAD when it sets STS_IAA.)
-		 */
-		cmd = ehci_readl(ehci, &ehci->regs->command);
-
-		/*
-		 * If IAA is set here it either legitimately triggered
-		 * after the watchdog timer expired (_way_ late, so we'll
-		 * still count it as lost) ... or a silicon erratum:
-		 * - VIA seems to set IAA without triggering the IRQ;
-		 * - IAAD potentially cleared without setting IAA.
-		 */
-		status = ehci_readl(ehci, &ehci->regs->status);
-		if ((status & STS_IAA) || !(cmd & CMD_IAAD)) {
-			COUNT(ehci->stats.lost_iaa);
-			ehci_writel(ehci, STS_IAA, &ehci->regs->status);
-		}
+	if (!ehci->iaa_in_progress || ehci->rh_state != EHCI_RH_RUNNING)
+		return;
+
+	/* If we get here, IAA is *REALLY* late.  It's barely
+	 * conceivable that the system is so busy that CMD_IAAD
+	 * is still legitimately set, so let's be sure it's
+	 * clear before we read STS_IAA.  (The HC should clear
+	 * CMD_IAAD when it sets STS_IAA.)
+	 */
+	cmd = ehci_readl(ehci, &ehci->regs->command);
 
-		ehci_vdbg(ehci, "IAA watchdog: status %x cmd %x\n",
-				status, cmd);
-		end_unlink_async(ehci);
+	/*
+	 * If IAA is set here it either legitimately triggered
+	 * after the watchdog timer expired (_way_ late, so we'll
+	 * still count it as lost) ... or a silicon erratum:
+	 * - VIA seems to set IAA without triggering the IRQ;
+	 * - IAAD potentially cleared without setting IAA.
+	 */
+	status = ehci_readl(ehci, &ehci->regs->status);
+	if ((status & STS_IAA) || !(cmd & CMD_IAAD)) {
+		COUNT(ehci->stats.lost_iaa);
+		ehci_writel(ehci, STS_IAA, &ehci->regs->status);
 	}
+
+	ehci_dbg(ehci, "IAA watchdog: status %x cmd %x\n", status, cmd);
+	end_unlink_async(ehci);
 }
 
 
diff --git a/drivers/usb/host/ehci-vt8500.c b/drivers/usb/host/ehci-vt8500.c
deleted file mode 100644
index 7ecf709610b..00000000000
--- a/drivers/usb/host/ehci-vt8500.c
+++ /dev/null
@@ -1,150 +0,0 @@
-/*
- * drivers/usb/host/ehci-vt8500.c
- *
- * Copyright (C) 2010 Alexey Charkov <alchark@gmail.com>
- *
- * Based on ehci-au1xxx.c
- *
- * 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/err.h>
-#include <linux/of.h>
-#include <linux/platform_device.h>
-
-static const struct hc_driver vt8500_ehci_hc_driver = {
-	.description		= hcd_name,
-	.product_desc		= "VT8500 EHCI",
-	.hcd_priv_size		= sizeof(struct ehci_hcd),
-
-	/*
-	 * generic hardware linkage
-	 */
-	.irq			= ehci_irq,
-	.flags			= HCD_MEMORY | HCD_USB2,
-
-	/*
-	 * basic lifecycle operations
-	 */
-	.reset			= ehci_setup,
-	.start			= ehci_run,
-	.stop			= ehci_stop,
-	.shutdown		= ehci_shutdown,
-
-	/*
-	 * managing i/o requests and associated device resources
-	 */
-	.urb_enqueue		= ehci_urb_enqueue,
-	.urb_dequeue		= ehci_urb_dequeue,
-	.endpoint_disable	= ehci_endpoint_disable,
-	.endpoint_reset		= ehci_endpoint_reset,
-
-	/*
-	 * scheduling support
-	 */
-	.get_frame_number	= ehci_get_frame,
-
-	/*
-	 * root hub support
-	 */
-	.hub_status_data	= ehci_hub_status_data,
-	.hub_control		= ehci_hub_control,
-	.bus_suspend		= ehci_bus_suspend,
-	.bus_resume		= ehci_bus_resume,
-	.relinquish_port	= ehci_relinquish_port,
-	.port_handed_over	= ehci_port_handed_over,
-
-	.clear_tt_buffer_complete	= ehci_clear_tt_buffer_complete,
-};
-
-static u64 vt8500_ehci_dma_mask = DMA_BIT_MASK(32);
-
-static int vt8500_ehci_drv_probe(struct platform_device *pdev)
-{
-	struct usb_hcd *hcd;
-	struct ehci_hcd *ehci;
-	struct resource *res;
-	int ret;
-
-	if (usb_disabled())
-		return -ENODEV;
-
-	/*
-	 * Right now device-tree probed devices don't get dma_mask set.
-	 * Since shared usb code relies on it, set it here for now.
-	 * Once we have dma capability bindings this can go away.
-	 */
-	if (!pdev->dev.dma_mask)
-		pdev->dev.dma_mask = &vt8500_ehci_dma_mask;
-
-	if (pdev->resource[1].flags != IORESOURCE_IRQ) {
-		pr_debug("resource[1] is not IORESOURCE_IRQ");
-		return -ENOMEM;
-	}
-	hcd = usb_create_hcd(&vt8500_ehci_hc_driver, &pdev->dev, "VT8500");
-	if (!hcd)
-		return -ENOMEM;
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	hcd->rsrc_start = res->start;
-	hcd->rsrc_len = resource_size(res);
-
-	hcd->regs = devm_ioremap_resource(&pdev->dev, res);
-	if (IS_ERR(hcd->regs)) {
-		ret = PTR_ERR(hcd->regs);
-		goto err1;
-	}
-
-	ehci = hcd_to_ehci(hcd);
-	ehci->caps = hcd->regs;
-
-	ret = usb_add_hcd(hcd, pdev->resource[1].start,
-			  IRQF_SHARED);
-	if (ret == 0) {
-		platform_set_drvdata(pdev, hcd);
-		return ret;
-	}
-
-err1:
-	usb_put_hcd(hcd);
-	return ret;
-}
-
-static int vt8500_ehci_drv_remove(struct platform_device *pdev)
-{
-	struct usb_hcd *hcd = platform_get_drvdata(pdev);
-
-	usb_remove_hcd(hcd);
-	usb_put_hcd(hcd);
-	platform_set_drvdata(pdev, NULL);
-
-	return 0;
-}
-
-static const struct of_device_id vt8500_ehci_ids[] = {
-	{ .compatible = "via,vt8500-ehci", },
-	{ .compatible = "wm,prizm-ehci", },
-	{}
-};
-
-static struct platform_driver vt8500_ehci_driver = {
-	.probe		= vt8500_ehci_drv_probe,
-	.remove		= vt8500_ehci_drv_remove,
-	.shutdown	= usb_hcd_platform_shutdown,
-	.driver = {
-		.name	= "vt8500-ehci",
-		.owner	= THIS_MODULE,
-		.of_match_table = of_match_ptr(vt8500_ehci_ids),
-	}
-};
-
-MODULE_ALIAS("platform:vt8500-ehci");
-MODULE_DEVICE_TABLE(of, vt8500_ehci_ids);
diff --git a/drivers/usb/host/ehci.h b/drivers/usb/host/ehci.h
index 36c3a821059..7c978b23520 100644
--- a/drivers/usb/host/ehci.h
+++ b/drivers/usb/host/ehci.h
@@ -121,6 +121,7 @@ struct ehci_hcd {			/* one per controller */
 	bool			scanning:1;
 	bool			need_rescan:1;
 	bool			intr_unlinking:1;
+	bool			iaa_in_progress:1;
 	bool			async_unlinking:1;
 	bool			shutdown:1;
 	struct ehci_qh		*qh_scan_next;
@@ -128,9 +129,8 @@ struct ehci_hcd {			/* one per controller */
 	/* async schedule support */
 	struct ehci_qh		*async;
 	struct ehci_qh		*dummy;		/* For AMD quirk use */
-	struct ehci_qh		*async_unlink;
-	struct ehci_qh		*async_unlink_last;
-	struct ehci_qh		*async_iaa;
+	struct list_head	async_unlink;
+	struct list_head	async_idle;
 	unsigned		async_unlink_cycle;
 	unsigned		async_count;	/* async activity count */
 
@@ -143,8 +143,7 @@ struct ehci_hcd {			/* one per controller */
 	unsigned		i_thresh;	/* uframes HC might cache */
 
 	union ehci_shadow	*pshadow;	/* mirror hw periodic table */
-	struct ehci_qh		*intr_unlink;
-	struct ehci_qh		*intr_unlink_last;
+	struct list_head	intr_unlink;
 	unsigned		intr_unlink_cycle;
 	unsigned		now_frame;	/* frame from HC hardware */
 	unsigned		last_iso_frame;	/* last frame scanned for iso */
@@ -200,6 +199,7 @@ struct ehci_hcd {			/* one per controller */
 	unsigned		use_dummy_qh:1;	/* AMD Frame List table quirk*/
 	unsigned		has_synopsys_hc_bug:1; /* Synopsys HC */
 	unsigned		frame_index_bug:1; /* MosChip (AKA NetMos) */
+	unsigned		need_oc_pp_cycle:1; /* MPC834X port power */
 
 	/* required for usb32 quirk */
 	#define OHCI_CTRL_HCFS          (3 << 6)
@@ -380,11 +380,10 @@ struct ehci_qh {
 	struct list_head	qtd_list;	/* sw qtd list */
 	struct list_head	intr_node;	/* list of intr QHs */
 	struct ehci_qtd		*dummy;
-	struct ehci_qh		*unlink_next;	/* next on unlink list */
+	struct list_head	unlink_node;
 
 	unsigned		unlink_cycle;
 
-	u8			needs_rescan;	/* Dequeue during giveback */
 	u8			qh_state;
 #define	QH_STATE_LINKED		1		/* HC sees this */
 #define	QH_STATE_UNLINK		2		/* HC may still see this */
@@ -407,6 +406,9 @@ struct ehci_qh {
 	struct usb_device	*dev;		/* access to TT */
 	unsigned		is_out:1;	/* bulk or intr OUT */
 	unsigned		clearing_tt:1;	/* Clear-TT-Buf in progress */
+	unsigned		dequeue_during_giveback:1;
+	unsigned		exception:1;	/* got a fault, or an unlink
+						   was requested */
 };
 
 /*-------------------------------------------------------------------------*/
diff --git a/drivers/usb/host/ohci-exynos.c b/drivers/usb/host/ohci-exynos.c
index e3b7e85120e..114583a8e92 100644
--- a/drivers/usb/host/ohci-exynos.c
+++ b/drivers/usb/host/ohci-exynos.c
@@ -17,7 +17,6 @@
 #include <linux/platform_data/usb-exynos.h>
 #include <linux/usb/phy.h>
 #include <linux/usb/samsung_usb_phy.h>
-#include <plat/usb-phy.h>
 
 struct exynos_ohci_hcd {
 	struct device *dev;
@@ -34,7 +33,7 @@ static void exynos_ohci_phy_enable(struct exynos_ohci_hcd *exynos_ohci)
 
 	if (exynos_ohci->phy)
 		usb_phy_init(exynos_ohci->phy);
-	else if (exynos_ohci->pdata->phy_init)
+	else if (exynos_ohci->pdata && exynos_ohci->pdata->phy_init)
 		exynos_ohci->pdata->phy_init(pdev, USB_PHY_TYPE_HOST);
 }
 
@@ -44,7 +43,7 @@ static void exynos_ohci_phy_disable(struct exynos_ohci_hcd *exynos_ohci)
 
 	if (exynos_ohci->phy)
 		usb_phy_shutdown(exynos_ohci->phy);
-	else if (exynos_ohci->pdata->phy_exit)
+	else if (exynos_ohci->pdata && exynos_ohci->pdata->phy_exit)
 		exynos_ohci->pdata->phy_exit(pdev, USB_PHY_TYPE_HOST);
 }
 
@@ -127,8 +126,12 @@ static int exynos_ohci_probe(struct platform_device *pdev)
 	if (!exynos_ohci)
 		return -ENOMEM;
 
+	if (of_device_is_compatible(pdev->dev.of_node,
+					"samsung,exynos5440-ohci"))
+		goto skip_phy;
+
 	phy = devm_usb_get_phy(&pdev->dev, USB_PHY_TYPE_USB2);
-	if (IS_ERR_OR_NULL(phy)) {
+	if (IS_ERR(phy)) {
 		/* Fallback to pdata */
 		if (!pdata) {
 			dev_warn(&pdev->dev, "no platform data or transceiver defined\n");
@@ -141,6 +144,8 @@ static int exynos_ohci_probe(struct platform_device *pdev)
 		exynos_ohci->otg = phy->otg;
 	}
 
+skip_phy:
+
 	exynos_ohci->dev = &pdev->dev;
 
 	hcd = usb_create_hcd(&exynos_ohci_hc_driver, &pdev->dev,
@@ -311,6 +316,7 @@ static const struct dev_pm_ops exynos_ohci_pm_ops = {
 #ifdef CONFIG_OF
 static const struct of_device_id exynos_ohci_match[] = {
 	{ .compatible = "samsung,exynos4210-ohci" },
+	{ .compatible = "samsung,exynos5440-ohci" },
 	{},
 };
 MODULE_DEVICE_TABLE(of, exynos_ohci_match);
diff --git a/drivers/usb/host/ohci-hcd.c b/drivers/usb/host/ohci-hcd.c
index 180a2b01db5..9e6de9586ae 100644
--- a/drivers/usb/host/ohci-hcd.c
+++ b/drivers/usb/host/ohci-hcd.c
@@ -1102,12 +1102,12 @@ MODULE_LICENSE ("GPL");
 
 #if defined(CONFIG_ARCH_S3C24XX) || defined(CONFIG_ARCH_S3C64XX)
 #include "ohci-s3c2410.c"
-#define PLATFORM_DRIVER		ohci_hcd_s3c2410_driver
+#define S3C2410_PLATFORM_DRIVER	ohci_hcd_s3c2410_driver
 #endif
 
 #ifdef CONFIG_USB_OHCI_EXYNOS
 #include "ohci-exynos.c"
-#define PLATFORM_DRIVER		exynos_ohci_driver
+#define EXYNOS_PLATFORM_DRIVER	exynos_ohci_driver
 #endif
 
 #ifdef CONFIG_USB_OHCI_HCD_OMAP1
@@ -1127,25 +1127,24 @@ MODULE_LICENSE ("GPL");
 
 #ifdef CONFIG_ARCH_EP93XX
 #include "ohci-ep93xx.c"
-#define PLATFORM_DRIVER		ohci_hcd_ep93xx_driver
+#define EP93XX_PLATFORM_DRIVER	ohci_hcd_ep93xx_driver
 #endif
 
 #ifdef CONFIG_ARCH_AT91
 #include "ohci-at91.c"
-#define PLATFORM_DRIVER		ohci_hcd_at91_driver
+#define AT91_PLATFORM_DRIVER	ohci_hcd_at91_driver
 #endif
 
 #ifdef CONFIG_ARCH_LPC32XX
 #include "ohci-nxp.c"
-#define PLATFORM_DRIVER		usb_hcd_nxp_driver
+#define NXP_PLATFORM_DRIVER	usb_hcd_nxp_driver
 #endif
 
 #ifdef CONFIG_ARCH_DAVINCI_DA8XX
 #include "ohci-da8xx.c"
-#define PLATFORM_DRIVER		ohci_hcd_da8xx_driver
+#define DAVINCI_PLATFORM_DRIVER	ohci_hcd_da8xx_driver
 #endif
 
-
 #ifdef CONFIG_USB_OHCI_HCD_PPC_OF
 #include "ohci-ppc-of.c"
 #define OF_PLATFORM_DRIVER	ohci_hcd_ppc_of_driver
@@ -1153,7 +1152,7 @@ MODULE_LICENSE ("GPL");
 
 #ifdef CONFIG_PLAT_SPEAR
 #include "ohci-spear.c"
-#define PLATFORM_DRIVER		spear_ohci_hcd_driver
+#define SPEAR_PLATFORM_DRIVER	spear_ohci_hcd_driver
 #endif
 
 #ifdef CONFIG_PPC_PS3
@@ -1199,7 +1198,14 @@ MODULE_LICENSE ("GPL");
 	!defined(SA1111_DRIVER) &&	\
 	!defined(PS3_SYSTEM_BUS_DRIVER) && \
 	!defined(SM501_OHCI_DRIVER) && \
-	!defined(TMIO_OHCI_DRIVER)
+	!defined(TMIO_OHCI_DRIVER) && \
+	!defined(S3C2410_PLATFORM_DRIVER) && \
+	!defined(EXYNOS_PLATFORM_DRIVER) && \
+	!defined(EP93XX_PLATFORM_DRIVER) && \
+	!defined(AT91_PLATFORM_DRIVER) && \
+	!defined(NXP_PLATFORM_DRIVER) && \
+	!defined(DAVINCI_PLATFORM_DRIVER) && \
+	!defined(SPEAR_PLATFORM_DRIVER)
 #error "missing bus glue for ohci-hcd"
 #endif
 
@@ -1277,9 +1283,79 @@ static int __init ohci_hcd_mod_init(void)
 		goto error_tmio;
 #endif
 
+#ifdef S3C2410_PLATFORM_DRIVER
+	retval = platform_driver_register(&S3C2410_PLATFORM_DRIVER);
+	if (retval < 0)
+		goto error_s3c2410;
+#endif
+
+#ifdef EXYNOS_PLATFORM_DRIVER
+	retval = platform_driver_register(&EXYNOS_PLATFORM_DRIVER);
+	if (retval < 0)
+		goto error_exynos;
+#endif
+
+#ifdef EP93XX_PLATFORM_DRIVER
+	retval = platform_driver_register(&EP93XX_PLATFORM_DRIVER);
+	if (retval < 0)
+		goto error_ep93xx;
+#endif
+
+#ifdef AT91_PLATFORM_DRIVER
+	retval = platform_driver_register(&AT91_PLATFORM_DRIVER);
+	if (retval < 0)
+		goto error_at91;
+#endif
+
+#ifdef NXP_PLATFORM_DRIVER
+	retval = platform_driver_register(&NXP_PLATFORM_DRIVER);
+	if (retval < 0)
+		goto error_nxp;
+#endif
+
+#ifdef DAVINCI_PLATFORM_DRIVER
+	retval = platform_driver_register(&DAVINCI_PLATFORM_DRIVER);
+	if (retval < 0)
+		goto error_davinci;
+#endif
+
+#ifdef SPEAR_PLATFORM_DRIVER
+	retval = platform_driver_register(&SPEAR_PLATFORM_DRIVER);
+	if (retval < 0)
+		goto error_spear;
+#endif
+
 	return retval;
 
 	/* Error path */
+#ifdef SPEAR_PLATFORM_DRIVER
+	platform_driver_unregister(&SPEAR_PLATFORM_DRIVER);
+ error_spear:
+#endif
+#ifdef DAVINCI_PLATFORM_DRIVER
+	platform_driver_unregister(&DAVINCI_PLATFORM_DRIVER);
+ error_davinci:
+#endif
+#ifdef NXP_PLATFORM_DRIVER
+	platform_driver_unregister(&NXP_PLATFORM_DRIVER);
+ error_nxp:
+#endif
+#ifdef AT91_PLATFORM_DRIVER
+	platform_driver_unregister(&AT91_PLATFORM_DRIVER);
+ error_at91:
+#endif
+#ifdef EP93XX_PLATFORM_DRIVER
+	platform_driver_unregister(&EP93XX_PLATFORM_DRIVER);
+ error_ep93xx:
+#endif
+#ifdef EXYNOS_PLATFORM_DRIVER
+	platform_driver_unregister(&EXYNOS_PLATFORM_DRIVER);
+ error_exynos:
+#endif
+#ifdef S3C2410_PLATFORM_DRIVER
+	platform_driver_unregister(&S3C2410_PLATFORM_DRIVER);
+ error_s3c2410:
+#endif
 #ifdef TMIO_OHCI_DRIVER
 	platform_driver_unregister(&TMIO_OHCI_DRIVER);
  error_tmio:
@@ -1300,17 +1376,17 @@ static int __init ohci_hcd_mod_init(void)
 	platform_driver_unregister(&OF_PLATFORM_DRIVER);
  error_of_platform:
 #endif
-#ifdef PLATFORM_DRIVER
-	platform_driver_unregister(&PLATFORM_DRIVER);
- error_platform:
+#ifdef OMAP3_PLATFORM_DRIVER
+	platform_driver_unregister(&OMAP3_PLATFORM_DRIVER);
+ error_omap3_platform:
 #endif
 #ifdef OMAP1_PLATFORM_DRIVER
 	platform_driver_unregister(&OMAP1_PLATFORM_DRIVER);
  error_omap1_platform:
 #endif
-#ifdef OMAP3_PLATFORM_DRIVER
-	platform_driver_unregister(&OMAP3_PLATFORM_DRIVER);
- error_omap3_platform:
+#ifdef PLATFORM_DRIVER
+	platform_driver_unregister(&PLATFORM_DRIVER);
+ error_platform:
 #endif
 #ifdef PS3_SYSTEM_BUS_DRIVER
 	ps3_ohci_driver_unregister(&PS3_SYSTEM_BUS_DRIVER);
@@ -1329,6 +1405,27 @@ module_init(ohci_hcd_mod_init);
 
 static void __exit ohci_hcd_mod_exit(void)
 {
+#ifdef SPEAR_PLATFORM_DRIVER
+	platform_driver_unregister(&SPEAR_PLATFORM_DRIVER);
+#endif
+#ifdef DAVINCI_PLATFORM_DRIVER
+	platform_driver_unregister(&DAVINCI_PLATFORM_DRIVER);
+#endif
+#ifdef NXP_PLATFORM_DRIVER
+	platform_driver_unregister(&NXP_PLATFORM_DRIVER);
+#endif
+#ifdef AT91_PLATFORM_DRIVER
+	platform_driver_unregister(&AT91_PLATFORM_DRIVER);
+#endif
+#ifdef EP93XX_PLATFORM_DRIVER
+	platform_driver_unregister(&EP93XX_PLATFORM_DRIVER);
+#endif
+#ifdef EXYNOS_PLATFORM_DRIVER
+	platform_driver_unregister(&EXYNOS_PLATFORM_DRIVER);
+#endif
+#ifdef S3C2410_PLATFORM_DRIVER
+	platform_driver_unregister(&S3C2410_PLATFORM_DRIVER);
+#endif
 #ifdef TMIO_OHCI_DRIVER
 	platform_driver_unregister(&TMIO_OHCI_DRIVER);
 #endif
@@ -1344,12 +1441,15 @@ static void __exit ohci_hcd_mod_exit(void)
 #ifdef OF_PLATFORM_DRIVER
 	platform_driver_unregister(&OF_PLATFORM_DRIVER);
 #endif
-#ifdef PLATFORM_DRIVER
-	platform_driver_unregister(&PLATFORM_DRIVER);
-#endif
 #ifdef OMAP3_PLATFORM_DRIVER
 	platform_driver_unregister(&OMAP3_PLATFORM_DRIVER);
 #endif
+#ifdef OMAP1_PLATFORM_DRIVER
+	platform_driver_unregister(&OMAP1_PLATFORM_DRIVER);
+#endif
+#ifdef PLATFORM_DRIVER
+	platform_driver_unregister(&PLATFORM_DRIVER);
+#endif
 #ifdef PS3_SYSTEM_BUS_DRIVER
 	ps3_ohci_driver_unregister(&PS3_SYSTEM_BUS_DRIVER);
 #endif
diff --git a/drivers/usb/host/ohci-hub.c b/drivers/usb/host/ohci-hub.c
index db09dae7b55..60ff4220e8b 100644
--- a/drivers/usb/host/ohci-hub.c
+++ b/drivers/usb/host/ohci-hub.c
@@ -580,14 +580,8 @@ static int ohci_start_port_reset (struct usb_hcd *hcd, unsigned port)
 
 /* See usb 7.1.7.5:  root hubs must issue at least 50 msec reset signaling,
  * not necessarily continuous ... to guard against resume signaling.
- * The short timeout is safe for non-root hubs, and is backward-compatible
- * with earlier Linux hosts.
  */
-#ifdef	CONFIG_USB_SUSPEND
 #define	PORT_RESET_MSEC		50
-#else
-#define	PORT_RESET_MSEC		10
-#endif
 
 /* this timer value might be vendor-specific ... */
 #define	PORT_RESET_HW_MSEC	10
diff --git a/drivers/usb/host/ohci-omap3.c b/drivers/usb/host/ohci-omap3.c
index eb35d963023..ddfc31427bc 100644
--- a/drivers/usb/host/ohci-omap3.c
+++ b/drivers/usb/host/ohci-omap3.c
@@ -31,6 +31,8 @@
 
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
+#include <linux/of.h>
+#include <linux/dma-mapping.h>
 
 /*-------------------------------------------------------------------------*/
 
@@ -112,6 +114,8 @@ static const struct hc_driver ohci_omap3_hc_driver = {
 
 /*-------------------------------------------------------------------------*/
 
+static u64 omap_ohci_dma_mask = DMA_BIT_MASK(32);
+
 /*
  * configure so an HC device and id are always provided
  * always called with process context; sleeping is OK
@@ -141,14 +145,13 @@ static int ohci_hcd_omap3_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	irq = platform_get_irq_byname(pdev, "ohci-irq");
+	irq = platform_get_irq(pdev, 0);
 	if (irq < 0) {
 		dev_err(dev, "OHCI irq failed\n");
 		return -ENODEV;
 	}
 
-	res = platform_get_resource_byname(pdev,
-				IORESOURCE_MEM, "ohci");
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
 		dev_err(dev, "UHH OHCI get resource failed\n");
 		return -ENOMEM;
@@ -160,6 +163,13 @@ static int ohci_hcd_omap3_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
+	/*
+	 * Right now device-tree probed devices don't get dma_mask set.
+	 * Since shared usb code relies on it, set it here for now.
+	 * Once we have dma capability bindings this can go away.
+	 */
+	if (!pdev->dev.dma_mask)
+		pdev->dev.dma_mask = &omap_ohci_dma_mask;
 
 	hcd = usb_create_hcd(&ohci_omap3_hc_driver, dev,
 			dev_name(dev));
@@ -229,12 +239,20 @@ static void ohci_hcd_omap3_shutdown(struct platform_device *pdev)
 		hcd->driver->shutdown(hcd);
 }
 
+static const struct of_device_id omap_ohci_dt_ids[] = {
+	{ .compatible = "ti,ohci-omap3" },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(of, omap_ohci_dt_ids);
+
 static struct platform_driver ohci_hcd_omap3_driver = {
 	.probe		= ohci_hcd_omap3_probe,
 	.remove		= ohci_hcd_omap3_remove,
 	.shutdown	= ohci_hcd_omap3_shutdown,
 	.driver		= {
 		.name	= "ohci-omap3",
+		.of_match_table = of_match_ptr(omap_ohci_dt_ids),
 	},
 };
 
diff --git a/drivers/usb/host/sl811-hcd.c b/drivers/usb/host/sl811-hcd.c
index d62f0404baa..15ed7e8d887 100644
--- a/drivers/usb/host/sl811-hcd.c
+++ b/drivers/usb/host/sl811-hcd.c
@@ -1755,7 +1755,7 @@ sl811h_probe(struct platform_device *dev)
 
 /* for this device there's no useful distinction between the controller
  * and its root hub, except that the root hub only gets direct PM calls
- * when CONFIG_USB_SUSPEND is enabled.
+ * when CONFIG_PM_RUNTIME is enabled.
  */
 
 static int
diff --git a/drivers/usb/host/sl811_cs.c b/drivers/usb/host/sl811_cs.c
index 3b6f50eaec9..469564e57a5 100644
--- a/drivers/usb/host/sl811_cs.c
+++ b/drivers/usb/host/sl811_cs.c
@@ -200,17 +200,4 @@ static struct pcmcia_driver sl811_cs_driver = {
 	.remove		= sl811_cs_detach,
 	.id_table	= sl811_ids,
 };
-
-/*====================================================================*/
-
-static int __init init_sl811_cs(void)
-{
-	return pcmcia_register_driver(&sl811_cs_driver);
-}
-module_init(init_sl811_cs);
-
-static void __exit exit_sl811_cs(void)
-{
-	pcmcia_unregister_driver(&sl811_cs_driver);
-}
-module_exit(exit_sl811_cs);
+module_pcmcia_driver(sl811_cs_driver);
diff --git a/drivers/usb/host/u132-hcd.c b/drivers/usb/host/u132-hcd.c
index 5efdffe3236..5c124bf5d01 100644
--- a/drivers/usb/host/u132-hcd.c
+++ b/drivers/usb/host/u132-hcd.c
@@ -3141,10 +3141,11 @@ static int u132_probe(struct platform_device *pdev)
 
 
 #ifdef CONFIG_PM
-/* for this device there's no useful distinction between the controller
-* and its root hub, except that the root hub only gets direct PM calls
-* when CONFIG_USB_SUSPEND is enabled.
-*/
+/*
+ * for this device there's no useful distinction between the controller
+ * and its root hub, except that the root hub only gets direct PM calls
+ * when CONFIG_PM_RUNTIME is enabled.
+ */
 static int u132_suspend(struct platform_device *pdev, pm_message_t state)
 {
 	struct usb_hcd *hcd = platform_get_drvdata(pdev);
diff --git a/drivers/usb/host/xhci-hub.c b/drivers/usb/host/xhci-hub.c
index 68914429482..187a3ec1069 100644
--- a/drivers/usb/host/xhci-hub.c
+++ b/drivers/usb/host/xhci-hub.c
@@ -1075,7 +1075,7 @@ int xhci_bus_suspend(struct usb_hcd *hcd)
 			set_bit(port_index, &bus_state->bus_suspended);
 		}
 		/* USB core sets remote wake mask for USB 3.0 hubs,
-		 * including the USB 3.0 roothub, but only if CONFIG_USB_SUSPEND
+		 * including the USB 3.0 roothub, but only if CONFIG_PM_RUNTIME
 		 * is enabled, so also enable remote wake here.
 		 */
 		if (hcd->self.root_hub->do_remote_wakeup) {
diff --git a/drivers/usb/host/xhci-mem.c b/drivers/usb/host/xhci-mem.c
index 35616ffbe3a..965b539bc47 100644
--- a/drivers/usb/host/xhci-mem.c
+++ b/drivers/usb/host/xhci-mem.c
@@ -51,7 +51,7 @@ static struct xhci_segment *xhci_segment_alloc(struct xhci_hcd *xhci,
 		return NULL;
 	}
 
-	memset(seg->trbs, 0, SEGMENT_SIZE);
+	memset(seg->trbs, 0, TRB_SEGMENT_SIZE);
 	/* If the cycle state is 0, set the cycle bit to 1 for all the TRBs */
 	if (cycle_state == 0) {
 		for (i = 0; i < TRBS_PER_SEGMENT; i++)
@@ -467,7 +467,7 @@ struct xhci_ring *xhci_dma_to_transfer_ring(
 {
 	if (ep->ep_state & EP_HAS_STREAMS)
 		return radix_tree_lookup(&ep->stream_info->trb_address_map,
-				address >> SEGMENT_SHIFT);
+				address >> TRB_SEGMENT_SHIFT);
 	return ep->ring;
 }
 
@@ -478,7 +478,7 @@ static struct xhci_ring *dma_to_stream_ring(
 		u64 address)
 {
 	return radix_tree_lookup(&stream_info->trb_address_map,
-			address >> SEGMENT_SHIFT);
+			address >> TRB_SEGMENT_SHIFT);
 }
 #endif	/* CONFIG_USB_XHCI_HCD_DEBUGGING */
 
@@ -514,7 +514,7 @@ static int xhci_test_radix_tree(struct xhci_hcd *xhci,
 
 		cur_ring = stream_info->stream_rings[cur_stream];
 		for (addr = cur_ring->first_seg->dma;
-				addr < cur_ring->first_seg->dma + SEGMENT_SIZE;
+				addr < cur_ring->first_seg->dma + TRB_SEGMENT_SIZE;
 				addr += trb_size) {
 			mapped_ring = dma_to_stream_ring(stream_info, addr);
 			if (cur_ring != mapped_ring) {
@@ -662,7 +662,7 @@ struct xhci_stream_info *xhci_alloc_stream_info(struct xhci_hcd *xhci,
 				cur_stream, (unsigned long long) addr);
 
 		key = (unsigned long)
-			(cur_ring->first_seg->dma >> SEGMENT_SHIFT);
+			(cur_ring->first_seg->dma >> TRB_SEGMENT_SHIFT);
 		ret = radix_tree_insert(&stream_info->trb_address_map,
 				key, cur_ring);
 		if (ret) {
@@ -693,7 +693,7 @@ cleanup_rings:
 		if (cur_ring) {
 			addr = cur_ring->first_seg->dma;
 			radix_tree_delete(&stream_info->trb_address_map,
-					addr >> SEGMENT_SHIFT);
+					addr >> TRB_SEGMENT_SHIFT);
 			xhci_ring_free(xhci, cur_ring);
 			stream_info->stream_rings[cur_stream] = NULL;
 		}
@@ -764,7 +764,7 @@ void xhci_free_stream_info(struct xhci_hcd *xhci,
 		if (cur_ring) {
 			addr = cur_ring->first_seg->dma;
 			radix_tree_delete(&stream_info->trb_address_map,
-					addr >> SEGMENT_SHIFT);
+					addr >> TRB_SEGMENT_SHIFT);
 			xhci_ring_free(xhci, cur_ring);
 			stream_info->stream_rings[cur_stream] = NULL;
 		}
@@ -1022,44 +1022,24 @@ void xhci_copy_ep0_dequeue_into_input_ctx(struct xhci_hcd *xhci,
  * is attached to (or the roothub port its ancestor hub is attached to).  All we
  * know is the index of that port under either the USB 2.0 or the USB 3.0
  * roothub, but that doesn't give us the real index into the HW port status
- * registers.  Scan through the xHCI roothub port array, looking for the Nth
- * entry of the correct port speed.  Return the port number of that entry.
+ * registers. Call xhci_find_raw_port_number() to get real index.
  */
 static u32 xhci_find_real_port_number(struct xhci_hcd *xhci,
 		struct usb_device *udev)
 {
 	struct usb_device *top_dev;
-	unsigned int num_similar_speed_ports;
-	unsigned int faked_port_num;
-	int i;
+	struct usb_hcd *hcd;
+
+	if (udev->speed == USB_SPEED_SUPER)
+		hcd = xhci->shared_hcd;
+	else
+		hcd = xhci->main_hcd;
 
 	for (top_dev = udev; top_dev->parent && top_dev->parent->parent;
 			top_dev = top_dev->parent)
 		/* Found device below root hub */;
-	faked_port_num = top_dev->portnum;
-	for (i = 0, num_similar_speed_ports = 0;
-			i < HCS_MAX_PORTS(xhci->hcs_params1); i++) {
-		u8 port_speed = xhci->port_array[i];
-
-		/*
-		 * Skip ports that don't have known speeds, or have duplicate
-		 * Extended Capabilities port speed entries.
-		 */
-		if (port_speed == 0 || port_speed == DUPLICATE_ENTRY)
-			continue;
 
-		/*
-		 * USB 3.0 ports are always under a USB 3.0 hub.  USB 2.0 and
-		 * 1.1 ports are under the USB 2.0 hub.  If the port speed
-		 * matches the device speed, it's a similar speed port.
-		 */
-		if ((port_speed == 0x03) == (udev->speed == USB_SPEED_SUPER))
-			num_similar_speed_ports++;
-		if (num_similar_speed_ports == faked_port_num)
-			/* Roothub ports are numbered from 1 to N */
-			return i+1;
-	}
-	return 0;
+	return	xhci_find_raw_port_number(hcd, top_dev->portnum);
 }
 
 /* Setup an xHCI virtual device for a Set Address command */
@@ -2325,7 +2305,7 @@ int xhci_mem_init(struct xhci_hcd *xhci, gfp_t flags)
 	 * so we pick the greater alignment need.
 	 */
 	xhci->segment_pool = dma_pool_create("xHCI ring segments", dev,
-			SEGMENT_SIZE, 64, xhci->page_size);
+			TRB_SEGMENT_SIZE, 64, xhci->page_size);
 
 	/* See Table 46 and Note on Figure 55 */
 	xhci->device_pool = dma_pool_create("xHCI input/output contexts", dev,
diff --git a/drivers/usb/host/xhci-pci.c b/drivers/usb/host/xhci-pci.c
index af259e0ec17..1a30c380043 100644
--- a/drivers/usb/host/xhci-pci.c
+++ b/drivers/usb/host/xhci-pci.c
@@ -313,6 +313,7 @@ static const struct hc_driver xhci_pci_hc_driver = {
 	.set_usb2_hw_lpm =	xhci_set_usb2_hardware_lpm,
 	.enable_usb3_lpm_timeout =	xhci_enable_usb3_lpm_timeout,
 	.disable_usb3_lpm_timeout =	xhci_disable_usb3_lpm_timeout,
+	.find_raw_port_number =	xhci_find_raw_port_number,
 };
 
 /*-------------------------------------------------------------------------*/
diff --git a/drivers/usb/host/xhci-ring.c b/drivers/usb/host/xhci-ring.c
index 88287546530..1969c001b3f 100644
--- a/drivers/usb/host/xhci-ring.c
+++ b/drivers/usb/host/xhci-ring.c
@@ -1599,14 +1599,20 @@ static void handle_port_status(struct xhci_hcd *xhci,
 	max_ports = HCS_MAX_PORTS(xhci->hcs_params1);
 	if ((port_id <= 0) || (port_id > max_ports)) {
 		xhci_warn(xhci, "Invalid port id %d\n", port_id);
-		bogus_port_status = true;
-		goto cleanup;
+		inc_deq(xhci, xhci->event_ring);
+		return;
 	}
 
 	/* Figure out which usb_hcd this port is attached to:
 	 * is it a USB 3.0 port or a USB 2.0/1.1 port?
 	 */
 	major_revision = xhci->port_array[port_id - 1];
+
+	/* Find the right roothub. */
+	hcd = xhci_to_hcd(xhci);
+	if ((major_revision == 0x03) != (hcd->speed == HCD_USB3))
+		hcd = xhci->shared_hcd;
+
 	if (major_revision == 0) {
 		xhci_warn(xhci, "Event for port %u not in "
 				"Extended Capabilities, ignoring.\n",
@@ -1629,10 +1635,6 @@ static void handle_port_status(struct xhci_hcd *xhci,
 	 * into the index into the ports on the correct split roothub, and the
 	 * correct bus_state structure.
 	 */
-	/* Find the right roothub. */
-	hcd = xhci_to_hcd(xhci);
-	if ((major_revision == 0x03) != (hcd->speed == HCD_USB3))
-		hcd = xhci->shared_hcd;
 	bus_state = &xhci->bus_state[hcd_index(hcd)];
 	if (hcd->speed == HCD_USB3)
 		port_array = xhci->usb3_ports;
@@ -2027,8 +2029,8 @@ static int process_ctrl_td(struct xhci_hcd *xhci, struct xhci_td *td,
 		if (event_trb != ep_ring->dequeue &&
 				event_trb != td->last_trb)
 			td->urb->actual_length =
-				td->urb->transfer_buffer_length
-				- TRB_LEN(le32_to_cpu(event->transfer_len));
+				td->urb->transfer_buffer_length -
+				EVENT_TRB_LEN(le32_to_cpu(event->transfer_len));
 		else
 			td->urb->actual_length = 0;
 
@@ -2060,7 +2062,7 @@ static int process_ctrl_td(struct xhci_hcd *xhci, struct xhci_td *td,
 		/* Maybe the event was for the data stage? */
 			td->urb->actual_length =
 				td->urb->transfer_buffer_length -
-				TRB_LEN(le32_to_cpu(event->transfer_len));
+				EVENT_TRB_LEN(le32_to_cpu(event->transfer_len));
 			xhci_dbg(xhci, "Waiting for status "
 					"stage event\n");
 			return 0;
@@ -2096,7 +2098,7 @@ static int process_isoc_td(struct xhci_hcd *xhci, struct xhci_td *td,
 	/* handle completion code */
 	switch (trb_comp_code) {
 	case COMP_SUCCESS:
-		if (TRB_LEN(le32_to_cpu(event->transfer_len)) == 0) {
+		if (EVENT_TRB_LEN(le32_to_cpu(event->transfer_len)) == 0) {
 			frame->status = 0;
 			break;
 		}
@@ -2141,7 +2143,7 @@ static int process_isoc_td(struct xhci_hcd *xhci, struct xhci_td *td,
 				len += TRB_LEN(le32_to_cpu(cur_trb->generic.field[2]));
 		}
 		len += TRB_LEN(le32_to_cpu(cur_trb->generic.field[2])) -
-			TRB_LEN(le32_to_cpu(event->transfer_len));
+			EVENT_TRB_LEN(le32_to_cpu(event->transfer_len));
 
 		if (trb_comp_code != COMP_STOP_INVAL) {
 			frame->actual_length = len;
@@ -2199,7 +2201,7 @@ static int process_bulk_intr_td(struct xhci_hcd *xhci, struct xhci_td *td,
 	case COMP_SUCCESS:
 		/* Double check that the HW transferred everything. */
 		if (event_trb != td->last_trb ||
-				TRB_LEN(le32_to_cpu(event->transfer_len)) != 0) {
+		    EVENT_TRB_LEN(le32_to_cpu(event->transfer_len)) != 0) {
 			xhci_warn(xhci, "WARN Successful completion "
 					"on short TX\n");
 			if (td->urb->transfer_flags & URB_SHORT_NOT_OK)
@@ -2227,18 +2229,18 @@ static int process_bulk_intr_td(struct xhci_hcd *xhci, struct xhci_td *td,
 				"%d bytes untransferred\n",
 				td->urb->ep->desc.bEndpointAddress,
 				td->urb->transfer_buffer_length,
-				TRB_LEN(le32_to_cpu(event->transfer_len)));
+				EVENT_TRB_LEN(le32_to_cpu(event->transfer_len)));
 	/* Fast path - was this the last TRB in the TD for this URB? */
 	if (event_trb == td->last_trb) {
-		if (TRB_LEN(le32_to_cpu(event->transfer_len)) != 0) {
+		if (EVENT_TRB_LEN(le32_to_cpu(event->transfer_len)) != 0) {
 			td->urb->actual_length =
 				td->urb->transfer_buffer_length -
-				TRB_LEN(le32_to_cpu(event->transfer_len));
+				EVENT_TRB_LEN(le32_to_cpu(event->transfer_len));
 			if (td->urb->transfer_buffer_length <
 					td->urb->actual_length) {
 				xhci_warn(xhci, "HC gave bad length "
 						"of %d bytes left\n",
-					  TRB_LEN(le32_to_cpu(event->transfer_len)));
+					  EVENT_TRB_LEN(le32_to_cpu(event->transfer_len)));
 				td->urb->actual_length = 0;
 				if (td->urb->transfer_flags & URB_SHORT_NOT_OK)
 					*status = -EREMOTEIO;
@@ -2280,7 +2282,7 @@ static int process_bulk_intr_td(struct xhci_hcd *xhci, struct xhci_td *td,
 		if (trb_comp_code != COMP_STOP_INVAL)
 			td->urb->actual_length +=
 				TRB_LEN(le32_to_cpu(cur_trb->generic.field[2])) -
-				TRB_LEN(le32_to_cpu(event->transfer_len));
+				EVENT_TRB_LEN(le32_to_cpu(event->transfer_len));
 	}
 
 	return finish_td(xhci, td, event_trb, event, ep, status, false);
@@ -2368,7 +2370,7 @@ static int handle_tx_event(struct xhci_hcd *xhci,
 	 * transfer type
 	 */
 	case COMP_SUCCESS:
-		if (TRB_LEN(le32_to_cpu(event->transfer_len)) == 0)
+		if (EVENT_TRB_LEN(le32_to_cpu(event->transfer_len)) == 0)
 			break;
 		if (xhci->quirks & XHCI_TRUST_TX_LENGTH)
 			trb_comp_code = COMP_SHORT_TX;
@@ -2461,14 +2463,21 @@ static int handle_tx_event(struct xhci_hcd *xhci,
 		 * TD list.
 		 */
 		if (list_empty(&ep_ring->td_list)) {
-			xhci_warn(xhci, "WARN Event TRB for slot %d ep %d "
-					"with no TDs queued?\n",
-				  TRB_TO_SLOT_ID(le32_to_cpu(event->flags)),
-				  ep_index);
-			xhci_dbg(xhci, "Event TRB with TRB type ID %u\n",
-				 (le32_to_cpu(event->flags) &
-				  TRB_TYPE_BITMASK)>>10);
-			xhci_print_trb_offsets(xhci, (union xhci_trb *) event);
+			/*
+			 * A stopped endpoint may generate an extra completion
+			 * event if the device was suspended.  Don't print
+			 * warnings.
+			 */
+			if (!(trb_comp_code == COMP_STOP ||
+						trb_comp_code == COMP_STOP_INVAL)) {
+				xhci_warn(xhci, "WARN Event TRB for slot %d ep %d with no TDs queued?\n",
+						TRB_TO_SLOT_ID(le32_to_cpu(event->flags)),
+						ep_index);
+				xhci_dbg(xhci, "Event TRB with TRB type ID %u\n",
+						(le32_to_cpu(event->flags) &
+						 TRB_TYPE_BITMASK)>>10);
+				xhci_print_trb_offsets(xhci, (union xhci_trb *) event);
+			}
 			if (ep->skip) {
 				ep->skip = false;
 				xhci_dbg(xhci, "td_list is empty while skip "
diff --git a/drivers/usb/host/xhci.c b/drivers/usb/host/xhci.c
index 849470b1883..b4aa79d154b 100644
--- a/drivers/usb/host/xhci.c
+++ b/drivers/usb/host/xhci.c
@@ -417,9 +417,9 @@ static void compliance_mode_recovery(unsigned long arg)
 			 * Compliance Mode Detected. Letting USB Core
 			 * handle the Warm Reset
 			 */
-			xhci_dbg(xhci, "Compliance Mode Detected->Port %d!\n",
+			xhci_dbg(xhci, "Compliance mode detected->port %d\n",
 					i + 1);
-			xhci_dbg(xhci, "Attempting Recovery routine!\n");
+			xhci_dbg(xhci, "Attempting compliance mode recovery\n");
 			hcd = xhci->shared_hcd;
 
 			if (hcd->state == HC_STATE_SUSPENDED)
@@ -457,7 +457,7 @@ static void compliance_mode_recovery_timer_init(struct xhci_hcd *xhci)
 	set_timer_slack(&xhci->comp_mode_recovery_timer,
 			msecs_to_jiffies(COMP_MODE_RCVRY_MSECS));
 	add_timer(&xhci->comp_mode_recovery_timer);
-	xhci_dbg(xhci, "Compliance Mode Recovery Timer Initialized.\n");
+	xhci_dbg(xhci, "Compliance mode recovery timer initialized\n");
 }
 
 /*
@@ -733,8 +733,11 @@ void xhci_stop(struct usb_hcd *hcd)
 
 	/* Deleting Compliance Mode Recovery Timer */
 	if ((xhci->quirks & XHCI_COMP_MODE_QUIRK) &&
-			(!(xhci_all_ports_seen_u0(xhci))))
+			(!(xhci_all_ports_seen_u0(xhci)))) {
 		del_timer_sync(&xhci->comp_mode_recovery_timer);
+		xhci_dbg(xhci, "%s: compliance mode recovery timer deleted\n",
+				__func__);
+	}
 
 	if (xhci->quirks & XHCI_AMD_PLL_FIX)
 		usb_amd_dev_put();
@@ -930,7 +933,8 @@ int xhci_suspend(struct xhci_hcd *xhci)
 	if ((xhci->quirks & XHCI_COMP_MODE_QUIRK) &&
 			(!(xhci_all_ports_seen_u0(xhci)))) {
 		del_timer_sync(&xhci->comp_mode_recovery_timer);
-		xhci_dbg(xhci, "Compliance Mode Recovery Timer Deleted!\n");
+		xhci_dbg(xhci, "%s: compliance mode recovery timer deleted\n",
+				__func__);
 	}
 
 	/* step 5: remove core well power */
@@ -3779,7 +3783,29 @@ int xhci_address_device(struct usb_hcd *hcd, struct usb_device *udev)
 	return 0;
 }
 
-#ifdef CONFIG_USB_SUSPEND
+/*
+ * Transfer the port index into real index in the HW port status
+ * registers. Caculate offset between the port's PORTSC register
+ * and port status base. Divide the number of per port register
+ * to get the real index. The raw port number bases 1.
+ */
+int xhci_find_raw_port_number(struct usb_hcd *hcd, int port1)
+{
+	struct xhci_hcd *xhci = hcd_to_xhci(hcd);
+	__le32 __iomem *base_addr = &xhci->op_regs->port_status_base;
+	__le32 __iomem *addr;
+	int raw_port;
+
+	if (hcd->speed != HCD_USB3)
+		addr = xhci->usb2_ports[port1 - 1];
+	else
+		addr = xhci->usb3_ports[port1 - 1];
+
+	raw_port = (addr - base_addr)/NUM_PORT_REGS + 1;
+	return raw_port;
+}
+
+#ifdef CONFIG_PM_RUNTIME
 
 /* BESL to HIRD Encoding array for USB2 LPM */
 static int xhci_besl_encoding[16] = {125, 150, 200, 300, 400, 500, 1000, 2000,
@@ -4029,7 +4055,7 @@ int xhci_update_device(struct usb_hcd *hcd, struct usb_device *udev)
 	return 0;
 }
 
-#endif /* CONFIG_USB_SUSPEND */
+#endif /* CONFIG_PM_RUNTIME */
 
 /*---------------------- USB 3.0 Link PM functions ------------------------*/
 
diff --git a/drivers/usb/host/xhci.h b/drivers/usb/host/xhci.h
index 2c510e4a7d4..29c978e3713 100644
--- a/drivers/usb/host/xhci.h
+++ b/drivers/usb/host/xhci.h
@@ -972,6 +972,10 @@ struct xhci_transfer_event {
 	__le32	flags;
 };
 
+/* Transfer event TRB length bit mask */
+/* bits 0:23 */
+#define	EVENT_TRB_LEN(p)		((p) & 0xffffff)
+
 /** Transfer Event bit fields **/
 #define	TRB_TO_EP_ID(p)	(((p) >> 16) & 0x1f)
 
@@ -1234,8 +1238,8 @@ union xhci_trb {
 #define TRBS_PER_SEGMENT	64
 /* Allow two commands + a link TRB, along with any reserved command TRBs */
 #define MAX_RSVD_CMD_TRBS	(TRBS_PER_SEGMENT - 3)
-#define SEGMENT_SIZE		(TRBS_PER_SEGMENT*16)
-#define SEGMENT_SHIFT		(__ffs(SEGMENT_SIZE))
+#define TRB_SEGMENT_SIZE	(TRBS_PER_SEGMENT*16)
+#define TRB_SEGMENT_SHIFT	(ilog2(TRB_SEGMENT_SIZE))
 /* TRB buffer pointers can't cross 64KB boundaries */
 #define TRB_MAX_BUFF_SHIFT		16
 #define TRB_MAX_BUFF_SIZE	(1 << TRB_MAX_BUFF_SHIFT)
@@ -1829,6 +1833,7 @@ void xhci_test_and_clear_bit(struct xhci_hcd *xhci, __le32 __iomem **port_array,
 int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue, u16 wIndex,
 		char *buf, u16 wLength);
 int xhci_hub_status_data(struct usb_hcd *hcd, char *buf);
+int xhci_find_raw_port_number(struct usb_hcd *hcd, int port1);
 
 #ifdef CONFIG_PM
 int xhci_bus_suspend(struct usb_hcd *hcd);
diff --git a/drivers/usb/image/Kconfig b/drivers/usb/image/Kconfig
index 33350f9dd34..320d368c8da 100644
--- a/drivers/usb/image/Kconfig
+++ b/drivers/usb/image/Kconfig
@@ -2,11 +2,9 @@
 # USB Imaging devices configuration
 #
 comment "USB Imaging devices"
-	depends on USB
 
 config USB_MDC800
 	tristate "USB Mustek MDC800 Digital Camera support"
-	depends on USB
 	---help---
 	  Say Y here if you want to connect this type of still camera to
 	  your computer's USB port. This driver can be used with gphoto 0.4.3
@@ -19,7 +17,7 @@ config USB_MDC800
 
 config USB_MICROTEK
 	tristate "Microtek X6USB scanner support"
-	depends on USB && SCSI
+	depends on SCSI
 	help
 	  Say Y here if you want support for the Microtek X6USB and
 	  possibly the Phantom 336CX, Phantom C6 and ScanMaker V6U(S)L.
diff --git a/drivers/usb/misc/Kconfig b/drivers/usb/misc/Kconfig
index 3b1a3f4ec5e..a51e7d6afda 100644
--- a/drivers/usb/misc/Kconfig
+++ b/drivers/usb/misc/Kconfig
@@ -2,11 +2,9 @@
 # USB Miscellaneous driver configuration
 #
 comment "USB Miscellaneous drivers"
-	depends on USB
 
 config USB_EMI62
 	tristate "EMI 6|2m USB Audio interface support"
-	depends on USB
 	---help---
 	  This driver loads firmware to Emagic EMI 6|2m low latency USB
 	  Audio and Midi interface.
@@ -21,7 +19,6 @@ config USB_EMI62
 
 config USB_EMI26
 	tristate "EMI 2|6 USB Audio interface support"
-	depends on USB
 	---help---
 	  This driver loads firmware to Emagic EMI 2|6 low latency USB
 	  Audio interface.
@@ -34,7 +31,6 @@ config USB_EMI26
 
 config USB_ADUTUX
 	tristate "ADU devices from Ontrak Control Systems"
-	depends on USB
 	help
 	  Say Y if you want to use an ADU device from Ontrak Control
 	  Systems.
@@ -44,7 +40,6 @@ config USB_ADUTUX
 
 config USB_SEVSEG
 	tristate "USB 7-Segment LED Display"
-	depends on USB
 	help
 	  Say Y here if you have a USB 7-Segment Display by Delcom
 
@@ -53,7 +48,6 @@ config USB_SEVSEG
 
 config USB_RIO500
 	tristate "USB Diamond Rio500 support"
-	depends on USB
 	help
 	  Say Y here if you want to connect a USB Rio500 mp3 player to your
 	  computer's USB port. Please read <file:Documentation/usb/rio.txt>
@@ -64,7 +58,6 @@ config USB_RIO500
 
 config USB_LEGOTOWER
 	tristate "USB Lego Infrared Tower support"
-	depends on USB
 	help
 	  Say Y here if you want to connect a USB Lego Infrared Tower to your
 	  computer's USB port.
@@ -77,7 +70,6 @@ config USB_LEGOTOWER
 
 config USB_LCD
 	tristate "USB LCD driver support"
-	depends on USB
 	help
 	  Say Y here if you want to connect an USBLCD to your computer's
 	  USB port. The USBLCD is a small USB interface board for
@@ -89,7 +81,6 @@ config USB_LCD
 
 config USB_LED
 	tristate "USB LED driver support"
-	depends on USB
 	help
 	  Say Y here if you want to connect an USBLED device to your 
 	  computer's USB port.
@@ -99,7 +90,6 @@ config USB_LED
 
 config USB_CYPRESS_CY7C63
 	tristate "Cypress CY7C63xxx USB driver support"
-	depends on USB
 	help
 	  Say Y here if you want to connect a Cypress CY7C63xxx
 	  micro controller to your computer's USB port. Currently this
@@ -113,7 +103,6 @@ config USB_CYPRESS_CY7C63
 
 config USB_CYTHERM
 	tristate "Cypress USB thermometer driver support"
-	depends on USB
 	help
 	  Say Y here if you want to connect a Cypress USB thermometer
 	  device to your computer's USB port. This device is also known
@@ -126,7 +115,6 @@ config USB_CYTHERM
 
 config USB_IDMOUSE
 	tristate "Siemens ID USB Mouse Fingerprint sensor support"
-	depends on USB
 	help
 	  Say Y here if you want to use the fingerprint sensor on
 	  the Siemens ID Mouse. There is also a Siemens ID Mouse
@@ -140,7 +128,6 @@ config USB_IDMOUSE
 
 config USB_FTDI_ELAN
 	tristate "Elan PCMCIA CardBus Adapter USB Client"
-	depends on USB
 	default M
 	help
 	  ELAN's Uxxx series of adapters are USB to PCMCIA CardBus adapters.
@@ -164,7 +151,6 @@ config USB_FTDI_ELAN
 
 config USB_APPLEDISPLAY
 	tristate "Apple Cinema Display support"
-	depends on USB
 	select BACKLIGHT_LCD_SUPPORT
 	select BACKLIGHT_CLASS_DEVICE
 	help
@@ -175,7 +161,6 @@ source "drivers/usb/misc/sisusbvga/Kconfig"
 
 config USB_LD
 	tristate "USB LD driver"
-	depends on USB
 	help
 	  This driver is for generic USB devices that use interrupt transfers,
 	  like LD Didactic's USB devices.
@@ -185,7 +170,6 @@ config USB_LD
 
 config USB_TRANCEVIBRATOR
 	tristate "PlayStation 2 Trance Vibrator driver support"
-	depends on USB
 	help
 	  Say Y here if you want to connect a PlayStation 2 Trance Vibrator
 	  device to your computer's USB port.
@@ -195,7 +179,6 @@ config USB_TRANCEVIBRATOR
 
 config USB_IOWARRIOR
 	tristate "IO Warrior driver support"
-	depends on USB
 	help
 	  Say Y here if you want to support the IO Warrior devices from Code
 	  Mercenaries.  This includes support for the following devices:
@@ -209,7 +192,6 @@ config USB_IOWARRIOR
 
 config USB_TEST
 	tristate "USB testing driver"
-	depends on USB
 	help
 	  This driver is for testing host controller software.  It is used
 	  with specialized device firmware for regression and stress testing,
@@ -220,7 +202,6 @@ config USB_TEST
 
 config USB_ISIGHTFW
 	tristate "iSight firmware loading support"
-	depends on USB
 	select FW_LOADER
 	help
 	  This driver loads firmware for USB Apple iSight cameras, allowing
@@ -233,7 +214,6 @@ config USB_ISIGHTFW
 
 config USB_YUREX
 	tristate "USB YUREX driver support"
-	depends on USB
 	help
 	  Say Y here if you want to connect a YUREX to your computer's
 	  USB port. The YUREX is a leg-shakes sensor. See
@@ -246,7 +226,6 @@ config USB_YUREX
 
 config USB_EZUSB_FX2
 	tristate "Functions for loading firmware on EZUSB chips"
-	depends on USB
 	help
 	  Say Y here if you need EZUSB device support.
 	  (Cypress FX/FX2/FX2LP microcontrollers)
diff --git a/drivers/usb/misc/appledisplay.c b/drivers/usb/misc/appledisplay.c
index 0fc6e5fc745..ba6a5d6e618 100644
--- a/drivers/usb/misc/appledisplay.c
+++ b/drivers/usb/misc/appledisplay.c
@@ -63,6 +63,7 @@ static const struct usb_device_id appledisplay_table[] = {
 	{ APPLEDISPLAY_DEVICE(0x9219) },
 	{ APPLEDISPLAY_DEVICE(0x921c) },
 	{ APPLEDISPLAY_DEVICE(0x921d) },
+	{ APPLEDISPLAY_DEVICE(0x9236) },
 
 	/* Terminating entry */
 	{ }
diff --git a/drivers/usb/misc/iowarrior.c b/drivers/usb/misc/iowarrior.c
index db46143c67a..d36f34e25be 100644
--- a/drivers/usb/misc/iowarrior.c
+++ b/drivers/usb/misc/iowarrior.c
@@ -672,7 +672,7 @@ static int iowarrior_release(struct inode *inode, struct file *file)
 		retval = -ENODEV;	/* close called more than once */
 		mutex_unlock(&dev->mutex);
 	} else {
-		dev->opened = 0;	/* we're closeing now */
+		dev->opened = 0;	/* we're closing now */
 		retval = 0;
 		if (dev->present) {
 			/*
@@ -802,7 +802,7 @@ static int iowarrior_probe(struct usb_interface *interface,
 			/* this one will match for the IOWarrior56 only */
 			dev->int_out_endpoint = endpoint;
 	}
-	/* we have to check the report_size often, so remember it in the endianess suitable for our machine */
+	/* we have to check the report_size often, so remember it in the endianness suitable for our machine */
 	dev->report_size = usb_endpoint_maxp(dev->int_in_endpoint);
 	if ((dev->interface->cur_altsetting->desc.bInterfaceNumber == 0) &&
 	    (dev->product_id == USB_DEVICE_ID_CODEMERCS_IOW56))
diff --git a/drivers/usb/misc/sisusbvga/Kconfig b/drivers/usb/misc/sisusbvga/Kconfig
index 30ea7ca6846..0d03a520048 100644
--- a/drivers/usb/misc/sisusbvga/Kconfig
+++ b/drivers/usb/misc/sisusbvga/Kconfig
@@ -1,7 +1,7 @@
 
 config USB_SISUSBVGA
 	tristate "USB 2.0 SVGA dongle support (Net2280/SiS315)"
-	depends on USB && (USB_MUSB_HDRC || USB_EHCI_HCD)
+	depends on (USB_MUSB_HDRC || USB_EHCI_HCD)
         ---help---
 	  Say Y here if you intend to attach a USB2VGA dongle based on a
 	  Net2280 and a SiS315 chip.
diff --git a/drivers/usb/misc/sisusbvga/sisusb.c b/drivers/usb/misc/sisusbvga/sisusb.c
index dd573abd2d1..c21386ec5d3 100644
--- a/drivers/usb/misc/sisusbvga/sisusb.c
+++ b/drivers/usb/misc/sisusbvga/sisusb.c
@@ -3084,7 +3084,7 @@ static int sisusb_probe(struct usb_interface *intf,
 
 	/* Allocate memory for our private */
 	if (!(sisusb = kzalloc(sizeof(*sisusb), GFP_KERNEL))) {
-		dev_err(&sisusb->sisusb_dev->dev, "Failed to allocate memory for private data\n");
+		dev_err(&dev->dev, "Failed to allocate memory for private data\n");
 		return -ENOMEM;
 	}
 	kref_init(&sisusb->kref);
diff --git a/drivers/usb/misc/usb3503.c b/drivers/usb/misc/usb3503.c
index f713f6aeb6e..d3a1cce1bf9 100644
--- a/drivers/usb/misc/usb3503.c
+++ b/drivers/usb/misc/usb3503.c
@@ -307,18 +307,7 @@ static struct i2c_driver usb3503_driver = {
 	.id_table	= usb3503_id,
 };
 
-static int __init usb3503_init(void)
-{
-	return i2c_add_driver(&usb3503_driver);
-}
-
-static void __exit usb3503_exit(void)
-{
-	i2c_del_driver(&usb3503_driver);
-}
-
-module_init(usb3503_init);
-module_exit(usb3503_exit);
+module_i2c_driver(usb3503_driver);
 
 MODULE_AUTHOR("Dongjin Kim <tobetter@gmail.com>");
 MODULE_DESCRIPTION("USB3503 USB HUB driver");
diff --git a/drivers/usb/misc/uss720.c b/drivers/usb/misc/uss720.c
index 29cad9e0a7a..e129cf66122 100644
--- a/drivers/usb/misc/uss720.c
+++ b/drivers/usb/misc/uss720.c
@@ -705,7 +705,7 @@ static int uss720_probe(struct usb_interface *intf,
 		return -ENODEV;
 	}
 	i = usb_set_interface(usbdev, intf->altsetting->desc.bInterfaceNumber, 2);
-	dev_dbg(&intf->dev, "set inteface result %d\n", i);
+	dev_dbg(&intf->dev, "set interface result %d\n", i);
 
 	interface = intf->cur_altsetting;
 
diff --git a/drivers/usb/mon/Kconfig b/drivers/usb/mon/Kconfig
index 635745f57fb..5c6ffa2a612 100644
--- a/drivers/usb/mon/Kconfig
+++ b/drivers/usb/mon/Kconfig
@@ -4,7 +4,6 @@
 
 config USB_MON
 	tristate "USB Monitor"
-	depends on USB
 	help
 	  If you select this option, a component which captures the USB traffic
 	  between peripheral-specific drivers and HC drivers will be built.
diff --git a/drivers/usb/musb/Kconfig b/drivers/usb/musb/Kconfig
index 05e51432dd2..06f8d29af1e 100644
--- a/drivers/usb/musb/Kconfig
+++ b/drivers/usb/musb/Kconfig
@@ -6,8 +6,7 @@
 # (M)HDRC = (Multipoint) Highspeed Dual-Role Controller
 config USB_MUSB_HDRC
 	tristate 'Inventra Highspeed Dual Role Controller (TI, ADI, ...)'
-	depends on USB && USB_GADGET
-	select USB_OTG_UTILS
+	depends on USB_GADGET
 	help
 	  Say Y here if your system has a dual role high speed USB
 	  controller based on the Mentor Graphics silicon IP.  Then
@@ -34,10 +33,12 @@ choice
 config USB_MUSB_DAVINCI
 	tristate "DaVinci"
 	depends on ARCH_DAVINCI_DMx
+	depends on BROKEN
 
 config USB_MUSB_DA8XX
 	tristate "DA8xx/OMAP-L1x"
 	depends on ARCH_DAVINCI_DA8XX
+	depends on BROKEN
 
 config USB_MUSB_TUSB6010
 	tristate "TUSB6010"
@@ -53,7 +54,6 @@ config USB_MUSB_AM35X
 
 config USB_MUSB_DSPS
 	tristate "TI DSPS platforms"
-	depends on SOC_TI81XX || SOC_AM33XX
 
 config USB_MUSB_BLACKFIN
 	tristate "Blackfin"
@@ -61,12 +61,12 @@ config USB_MUSB_BLACKFIN
 
 config USB_MUSB_UX500
 	tristate "U8500 and U5500"
-	depends on (ARCH_U8500 && AB8500_USB)
 
 endchoice
 
 choice
 	prompt 'MUSB DMA mode'
+	default MUSB_PIO_ONLY if ARCH_MULTIPLATFORM
 	default USB_UX500_DMA if USB_MUSB_UX500
 	default USB_INVENTRA_DMA if USB_MUSB_OMAP2PLUS || USB_MUSB_BLACKFIN
 	default USB_TI_CPPI_DMA if USB_MUSB_DAVINCI
diff --git a/drivers/usb/musb/am35x.c b/drivers/usb/musb/am35x.c
index 59eea219034..2231850c062 100644
--- a/drivers/usb/musb/am35x.c
+++ b/drivers/usb/musb/am35x.c
@@ -149,7 +149,7 @@ static void otg_timer(unsigned long _musb)
 	 */
 	devctl = musb_readb(mregs, MUSB_DEVCTL);
 	dev_dbg(musb->controller, "Poll devctl %02x (%s)\n", devctl,
-		otg_state_string(musb->xceiv->state));
+		usb_otg_state_string(musb->xceiv->state));
 
 	spin_lock_irqsave(&musb->lock, flags);
 	switch (musb->xceiv->state) {
@@ -195,7 +195,7 @@ static void am35x_musb_try_idle(struct musb *musb, unsigned long timeout)
 	if (musb->is_active || (musb->a_wait_bcon == 0 &&
 				musb->xceiv->state == OTG_STATE_A_WAIT_BCON)) {
 		dev_dbg(musb->controller, "%s active, deleting timer\n",
-			otg_state_string(musb->xceiv->state));
+			usb_otg_state_string(musb->xceiv->state));
 		del_timer(&otg_workaround);
 		last_timer = jiffies;
 		return;
@@ -208,7 +208,7 @@ static void am35x_musb_try_idle(struct musb *musb, unsigned long timeout)
 	last_timer = timeout;
 
 	dev_dbg(musb->controller, "%s inactive, starting idle timer for %u ms\n",
-		otg_state_string(musb->xceiv->state),
+		usb_otg_state_string(musb->xceiv->state),
 		jiffies_to_msecs(timeout - jiffies));
 	mod_timer(&otg_workaround, timeout);
 }
@@ -298,7 +298,7 @@ static irqreturn_t am35x_musb_interrupt(int irq, void *hci)
 		/* NOTE: this must complete power-on within 100 ms. */
 		dev_dbg(musb->controller, "VBUS %s (%s)%s, devctl %02x\n",
 				drvvbus ? "on" : "off",
-				otg_state_string(musb->xceiv->state),
+				usb_otg_state_string(musb->xceiv->state),
 				err ? " ERROR" : "",
 				devctl);
 		ret = IRQ_HANDLED;
diff --git a/drivers/usb/musb/blackfin.c b/drivers/usb/musb/blackfin.c
index dbb31b30c7f..5e63b160db0 100644
--- a/drivers/usb/musb/blackfin.c
+++ b/drivers/usb/musb/blackfin.c
@@ -280,13 +280,13 @@ static void musb_conn_timer_handler(unsigned long _musb)
 		break;
 	default:
 		dev_dbg(musb->controller, "%s state not handled\n",
-			otg_state_string(musb->xceiv->state));
+			usb_otg_state_string(musb->xceiv->state));
 		break;
 	}
 	spin_unlock_irqrestore(&musb->lock, flags);
 
 	dev_dbg(musb->controller, "state is %s\n",
-		otg_state_string(musb->xceiv->state));
+		usb_otg_state_string(musb->xceiv->state));
 }
 
 static void bfin_musb_enable(struct musb *musb)
@@ -307,7 +307,7 @@ static void bfin_musb_set_vbus(struct musb *musb, int is_on)
 
 	dev_dbg(musb->controller, "VBUS %s, devctl %02x "
 		/* otg %3x conf %08x prcm %08x */ "\n",
-		otg_state_string(musb->xceiv->state),
+		usb_otg_state_string(musb->xceiv->state),
 		musb_readb(musb->mregs, MUSB_DEVCTL));
 }
 
diff --git a/drivers/usb/musb/cppi_dma.c b/drivers/usb/musb/cppi_dma.c
index f522000e8f0..9db211ee15b 100644
--- a/drivers/usb/musb/cppi_dma.c
+++ b/drivers/usb/musb/cppi_dma.c
@@ -435,7 +435,6 @@ cppi_rndis_update(struct cppi_channel *c, int is_rx,
 	}
 }
 
-#ifdef CONFIG_USB_MUSB_DEBUG
 static void cppi_dump_rxbd(const char *tag, struct cppi_descriptor *bd)
 {
 	pr_debug("RXBD/%s %08x: "
@@ -444,21 +443,16 @@ static void cppi_dump_rxbd(const char *tag, struct cppi_descriptor *bd)
 			bd->hw_next, bd->hw_bufp, bd->hw_off_len,
 			bd->hw_options);
 }
-#endif
 
 static void cppi_dump_rxq(int level, const char *tag, struct cppi_channel *rx)
 {
-#ifdef CONFIG_USB_MUSB_DEBUG
 	struct cppi_descriptor	*bd;
 
-	if (!_dbg_level(level))
-		return;
 	cppi_dump_rx(level, rx, tag);
 	if (rx->last_processed)
 		cppi_dump_rxbd("last", rx->last_processed);
 	for (bd = rx->head; bd; bd = bd->next)
 		cppi_dump_rxbd("active", bd);
-#endif
 }
 
 
@@ -784,6 +778,7 @@ cppi_next_rx_segment(struct musb *musb, struct cppi_channel *rx, int onepacket)
 	void __iomem		*tibase = musb->ctrl_base;
 	int			is_rndis = 0;
 	struct cppi_rx_stateram	__iomem *rx_ram = rx->state_ram;
+	struct cppi_descriptor	*d;
 
 	if (onepacket) {
 		/* almost every USB driver, host or peripheral side */
@@ -897,14 +892,8 @@ cppi_next_rx_segment(struct musb *musb, struct cppi_channel *rx, int onepacket)
 	bd->hw_options |= CPPI_SOP_SET;
 	tail->hw_options |= CPPI_EOP_SET;
 
-#ifdef CONFIG_USB_MUSB_DEBUG
-	if (_dbg_level(5)) {
-		struct cppi_descriptor	*d;
-
-		for (d = rx->head; d; d = d->next)
-			cppi_dump_rxbd("S", d);
-	}
-#endif
+	for (d = rx->head; d; d = d->next)
+		cppi_dump_rxbd("S", d);
 
 	/* in case the preceding transfer left some state... */
 	tail = rx->last_processed;
diff --git a/drivers/usb/musb/da8xx.c b/drivers/usb/musb/da8xx.c
index 41613a2b35e..b903b744a22 100644
--- a/drivers/usb/musb/da8xx.c
+++ b/drivers/usb/musb/da8xx.c
@@ -198,7 +198,7 @@ static void otg_timer(unsigned long _musb)
 	 */
 	devctl = musb_readb(mregs, MUSB_DEVCTL);
 	dev_dbg(musb->controller, "Poll devctl %02x (%s)\n", devctl,
-		otg_state_string(musb->xceiv->state));
+		usb_otg_state_string(musb->xceiv->state));
 
 	spin_lock_irqsave(&musb->lock, flags);
 	switch (musb->xceiv->state) {
@@ -267,7 +267,7 @@ static void da8xx_musb_try_idle(struct musb *musb, unsigned long timeout)
 	if (musb->is_active || (musb->a_wait_bcon == 0 &&
 				musb->xceiv->state == OTG_STATE_A_WAIT_BCON)) {
 		dev_dbg(musb->controller, "%s active, deleting timer\n",
-			otg_state_string(musb->xceiv->state));
+			usb_otg_state_string(musb->xceiv->state));
 		del_timer(&otg_workaround);
 		last_timer = jiffies;
 		return;
@@ -280,7 +280,7 @@ static void da8xx_musb_try_idle(struct musb *musb, unsigned long timeout)
 	last_timer = timeout;
 
 	dev_dbg(musb->controller, "%s inactive, starting idle timer for %u ms\n",
-		otg_state_string(musb->xceiv->state),
+		usb_otg_state_string(musb->xceiv->state),
 		jiffies_to_msecs(timeout - jiffies));
 	mod_timer(&otg_workaround, timeout);
 }
@@ -360,7 +360,7 @@ static irqreturn_t da8xx_musb_interrupt(int irq, void *hci)
 
 		dev_dbg(musb->controller, "VBUS %s (%s)%s, devctl %02x\n",
 				drvvbus ? "on" : "off",
-				otg_state_string(musb->xceiv->state),
+				usb_otg_state_string(musb->xceiv->state),
 				err ? " ERROR" : "",
 				devctl);
 		ret = IRQ_HANDLED;
diff --git a/drivers/usb/musb/davinci.c b/drivers/usb/musb/davinci.c
index e040d910373..bea6cc35471 100644
--- a/drivers/usb/musb/davinci.c
+++ b/drivers/usb/musb/davinci.c
@@ -215,7 +215,7 @@ static void otg_timer(unsigned long _musb)
 	 */
 	devctl = musb_readb(mregs, MUSB_DEVCTL);
 	dev_dbg(musb->controller, "poll devctl %02x (%s)\n", devctl,
-		otg_state_string(musb->xceiv->state));
+		usb_otg_state_string(musb->xceiv->state));
 
 	spin_lock_irqsave(&musb->lock, flags);
 	switch (musb->xceiv->state) {
@@ -349,7 +349,7 @@ static irqreturn_t davinci_musb_interrupt(int irq, void *__hci)
 		davinci_musb_source_power(musb, drvvbus, 0);
 		dev_dbg(musb->controller, "VBUS %s (%s)%s, devctl %02x\n",
 				drvvbus ? "on" : "off",
-				otg_state_string(musb->xceiv->state),
+				usb_otg_state_string(musb->xceiv->state),
 				err ? " ERROR" : "",
 				devctl);
 		retval = IRQ_HANDLED;
diff --git a/drivers/usb/musb/musb_core.c b/drivers/usb/musb/musb_core.c
index daec6e0f7e3..37a261a6bb6 100644
--- a/drivers/usb/musb/musb_core.c
+++ b/drivers/usb/musb/musb_core.c
@@ -372,13 +372,13 @@ static void musb_otg_timer_func(unsigned long data)
 	case OTG_STATE_A_SUSPEND:
 	case OTG_STATE_A_WAIT_BCON:
 		dev_dbg(musb->controller, "HNP: %s timeout\n",
-			otg_state_string(musb->xceiv->state));
+			usb_otg_state_string(musb->xceiv->state));
 		musb_platform_set_vbus(musb, 0);
 		musb->xceiv->state = OTG_STATE_A_WAIT_VFALL;
 		break;
 	default:
 		dev_dbg(musb->controller, "HNP: Unhandled mode %s\n",
-			otg_state_string(musb->xceiv->state));
+			usb_otg_state_string(musb->xceiv->state));
 	}
 	musb->ignore_disconnect = 0;
 	spin_unlock_irqrestore(&musb->lock, flags);
@@ -393,13 +393,14 @@ void musb_hnp_stop(struct musb *musb)
 	void __iomem	*mbase = musb->mregs;
 	u8	reg;
 
-	dev_dbg(musb->controller, "HNP: stop from %s\n", otg_state_string(musb->xceiv->state));
+	dev_dbg(musb->controller, "HNP: stop from %s\n",
+			usb_otg_state_string(musb->xceiv->state));
 
 	switch (musb->xceiv->state) {
 	case OTG_STATE_A_PERIPHERAL:
 		musb_g_disconnect(musb);
 		dev_dbg(musb->controller, "HNP: back to %s\n",
-			otg_state_string(musb->xceiv->state));
+			usb_otg_state_string(musb->xceiv->state));
 		break;
 	case OTG_STATE_B_HOST:
 		dev_dbg(musb->controller, "HNP: Disabling HR\n");
@@ -413,7 +414,7 @@ void musb_hnp_stop(struct musb *musb)
 		break;
 	default:
 		dev_dbg(musb->controller, "HNP: Stopping in unknown state %s\n",
-			otg_state_string(musb->xceiv->state));
+			usb_otg_state_string(musb->xceiv->state));
 	}
 
 	/*
@@ -451,7 +452,7 @@ static irqreturn_t musb_stage0_irq(struct musb *musb, u8 int_usb,
 	 */
 	if (int_usb & MUSB_INTR_RESUME) {
 		handled = IRQ_HANDLED;
-		dev_dbg(musb->controller, "RESUME (%s)\n", otg_state_string(musb->xceiv->state));
+		dev_dbg(musb->controller, "RESUME (%s)\n", usb_otg_state_string(musb->xceiv->state));
 
 		if (devctl & MUSB_DEVCTL_HM) {
 			void __iomem *mbase = musb->mregs;
@@ -493,7 +494,7 @@ static irqreturn_t musb_stage0_irq(struct musb *musb, u8 int_usb,
 			default:
 				WARNING("bogus %s RESUME (%s)\n",
 					"host",
-					otg_state_string(musb->xceiv->state));
+					usb_otg_state_string(musb->xceiv->state));
 			}
 		} else {
 			switch (musb->xceiv->state) {
@@ -522,7 +523,7 @@ static irqreturn_t musb_stage0_irq(struct musb *musb, u8 int_usb,
 			default:
 				WARNING("bogus %s RESUME (%s)\n",
 					"peripheral",
-					otg_state_string(musb->xceiv->state));
+					usb_otg_state_string(musb->xceiv->state));
 			}
 		}
 	}
@@ -538,7 +539,7 @@ static irqreturn_t musb_stage0_irq(struct musb *musb, u8 int_usb,
 		}
 
 		dev_dbg(musb->controller, "SESSION_REQUEST (%s)\n",
-			otg_state_string(musb->xceiv->state));
+			usb_otg_state_string(musb->xceiv->state));
 
 		/* IRQ arrives from ID pin sense or (later, if VBUS power
 		 * is removed) SRP.  responses are time critical:
@@ -602,8 +603,9 @@ static irqreturn_t musb_stage0_irq(struct musb *musb, u8 int_usb,
 			break;
 		}
 
-		dev_dbg(musb->controller, "VBUS_ERROR in %s (%02x, %s), retry #%d, port1 %08x\n",
-				otg_state_string(musb->xceiv->state),
+		dev_printk(ignore ? KERN_DEBUG : KERN_ERR, musb->controller,
+				"VBUS_ERROR in %s (%02x, %s), retry #%d, port1 %08x\n",
+				usb_otg_state_string(musb->xceiv->state),
 				devctl,
 				({ char *s;
 				switch (devctl & MUSB_DEVCTL_VBUS) {
@@ -628,7 +630,7 @@ static irqreturn_t musb_stage0_irq(struct musb *musb, u8 int_usb,
 
 	if (int_usb & MUSB_INTR_SUSPEND) {
 		dev_dbg(musb->controller, "SUSPEND (%s) devctl %02x\n",
-			otg_state_string(musb->xceiv->state), devctl);
+			usb_otg_state_string(musb->xceiv->state), devctl);
 		handled = IRQ_HANDLED;
 
 		switch (musb->xceiv->state) {
@@ -745,12 +747,12 @@ b_host:
 			usb_hcd_resume_root_hub(hcd);
 
 		dev_dbg(musb->controller, "CONNECT (%s) devctl %02x\n",
-				otg_state_string(musb->xceiv->state), devctl);
+				usb_otg_state_string(musb->xceiv->state), devctl);
 	}
 
 	if ((int_usb & MUSB_INTR_DISCONNECT) && !musb->ignore_disconnect) {
 		dev_dbg(musb->controller, "DISCONNECT (%s) as %s, devctl %02x\n",
-				otg_state_string(musb->xceiv->state),
+				usb_otg_state_string(musb->xceiv->state),
 				MUSB_MODE(musb), devctl);
 		handled = IRQ_HANDLED;
 
@@ -787,7 +789,7 @@ b_host:
 			break;
 		default:
 			WARNING("unhandled DISCONNECT transition (%s)\n",
-				otg_state_string(musb->xceiv->state));
+				usb_otg_state_string(musb->xceiv->state));
 			break;
 		}
 	}
@@ -813,7 +815,7 @@ b_host:
 			}
 		} else {
 			dev_dbg(musb->controller, "BUS RESET as %s\n",
-				otg_state_string(musb->xceiv->state));
+				usb_otg_state_string(musb->xceiv->state));
 			switch (musb->xceiv->state) {
 			case OTG_STATE_A_SUSPEND:
 				/* We need to ignore disconnect on suspend
@@ -826,7 +828,7 @@ b_host:
 			case OTG_STATE_A_WAIT_BCON:	/* OPT TD.4.7-900ms */
 				/* never use invalid T(a_wait_bcon) */
 				dev_dbg(musb->controller, "HNP: in %s, %d msec timeout\n",
-					otg_state_string(musb->xceiv->state),
+					usb_otg_state_string(musb->xceiv->state),
 					TA_WAIT_BCON(musb));
 				mod_timer(&musb->otg_timer, jiffies
 					+ msecs_to_jiffies(TA_WAIT_BCON(musb)));
@@ -838,7 +840,7 @@ b_host:
 				break;
 			case OTG_STATE_B_WAIT_ACON:
 				dev_dbg(musb->controller, "HNP: RESET (%s), to b_peripheral\n",
-					otg_state_string(musb->xceiv->state));
+					usb_otg_state_string(musb->xceiv->state));
 				musb->xceiv->state = OTG_STATE_B_PERIPHERAL;
 				musb_g_reset(musb);
 				break;
@@ -850,7 +852,7 @@ b_host:
 				break;
 			default:
 				dev_dbg(musb->controller, "Unhandled BUS RESET as %s\n",
-					otg_state_string(musb->xceiv->state));
+					usb_otg_state_string(musb->xceiv->state));
 			}
 		}
 	}
@@ -1632,7 +1634,7 @@ musb_mode_show(struct device *dev, struct device_attribute *attr, char *buf)
 	int ret = -EINVAL;
 
 	spin_lock_irqsave(&musb->lock, flags);
-	ret = sprintf(buf, "%s\n", otg_state_string(musb->xceiv->state));
+	ret = sprintf(buf, "%s\n", usb_otg_state_string(musb->xceiv->state));
 	spin_unlock_irqrestore(&musb->lock, flags);
 
 	return ret;
@@ -1951,9 +1953,13 @@ musb_init_controller(struct device *dev, int nIrq, void __iomem *ctrl)
 		musb_write_ulpi_buscontrol(musb->mregs, busctl);
 	}
 
-	MUSB_DEV_MODE(musb);
-	musb->xceiv->otg->default_a = 0;
-	musb->xceiv->state = OTG_STATE_B_IDLE;
+	if (musb->xceiv->otg->default_a) {
+		MUSB_HST_MODE(musb);
+		musb->xceiv->state = OTG_STATE_A_IDLE;
+	} else {
+		MUSB_DEV_MODE(musb);
+		musb->xceiv->state = OTG_STATE_B_IDLE;
+	}
 
 	status = musb_gadget_setup(musb);
 
@@ -2008,7 +2014,6 @@ static int musb_probe(struct platform_device *pdev)
 {
 	struct device	*dev = &pdev->dev;
 	int		irq = platform_get_irq_byname(pdev, "mc");
-	int		status;
 	struct resource	*iomem;
 	void __iomem	*base;
 
@@ -2016,24 +2021,17 @@ static int musb_probe(struct platform_device *pdev)
 	if (!iomem || irq <= 0)
 		return -ENODEV;
 
-	base = ioremap(iomem->start, resource_size(iomem));
-	if (!base) {
-		dev_err(dev, "ioremap failed\n");
-		return -ENOMEM;
-	}
+	base = devm_ioremap_resource(dev, iomem);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
 
-	status = musb_init_controller(dev, irq, base);
-	if (status < 0)
-		iounmap(base);
-
-	return status;
+	return musb_init_controller(dev, irq, base);
 }
 
 static int musb_remove(struct platform_device *pdev)
 {
 	struct device	*dev = &pdev->dev;
 	struct musb	*musb = dev_to_musb(dev);
-	void __iomem	*ctrl_base = musb->ctrl_base;
 
 	/* this gets called on rmmod.
 	 *  - Host mode: host may still be active
@@ -2044,7 +2042,6 @@ static int musb_remove(struct platform_device *pdev)
 	musb_shutdown(pdev);
 
 	musb_free(musb);
-	iounmap(ctrl_base);
 	device_init_wakeup(dev, 0);
 #ifndef CONFIG_MUSB_PIO_ONLY
 	dma_set_mask(dev, *dev->parent->dma_mask);
@@ -2293,8 +2290,6 @@ static int __init musb_init(void)
 	if (usb_disabled())
 		return 0;
 
-	pr_info("%s: version " MUSB_VERSION ", ?dma?, otg (peripheral+host)\n",
-		musb_driver_name);
 	return platform_driver_register(&musb_driver);
 }
 module_init(musb_init);
diff --git a/drivers/usb/musb/musb_dsps.c b/drivers/usb/musb/musb_dsps.c
index 6bb89715b63..3a18e44e939 100644
--- a/drivers/usb/musb/musb_dsps.c
+++ b/drivers/usb/musb/musb_dsps.c
@@ -38,6 +38,7 @@
 #include <linux/module.h>
 #include <linux/usb/nop-usb-xceiv.h>
 #include <linux/platform_data/usb-omap.h>
+#include <linux/sizes.h>
 
 #include <linux/of.h>
 #include <linux/of_device.h>
@@ -224,7 +225,7 @@ static void otg_timer(unsigned long _musb)
 	 */
 	devctl = dsps_readb(mregs, MUSB_DEVCTL);
 	dev_dbg(musb->controller, "Poll devctl %02x (%s)\n", devctl,
-				otg_state_string(musb->xceiv->state));
+				usb_otg_state_string(musb->xceiv->state));
 
 	spin_lock_irqsave(&musb->lock, flags);
 	switch (musb->xceiv->state) {
@@ -273,7 +274,7 @@ static void dsps_musb_try_idle(struct musb *musb, unsigned long timeout)
 	if (musb->is_active || (musb->a_wait_bcon == 0 &&
 				musb->xceiv->state == OTG_STATE_A_WAIT_BCON)) {
 		dev_dbg(musb->controller, "%s active, deleting timer\n",
-				otg_state_string(musb->xceiv->state));
+				usb_otg_state_string(musb->xceiv->state));
 		del_timer(&glue->timer[pdev->id]);
 		glue->last_timer[pdev->id] = jiffies;
 		return;
@@ -288,7 +289,7 @@ static void dsps_musb_try_idle(struct musb *musb, unsigned long timeout)
 	glue->last_timer[pdev->id] = timeout;
 
 	dev_dbg(musb->controller, "%s inactive, starting idle timer for %u ms\n",
-		otg_state_string(musb->xceiv->state),
+		usb_otg_state_string(musb->xceiv->state),
 			jiffies_to_msecs(timeout - jiffies));
 	mod_timer(&glue->timer[pdev->id], timeout);
 }
@@ -334,7 +335,7 @@ static irqreturn_t dsps_interrupt(int irq, void *hci)
 	 * value but DEVCTL.BDEVICE is invalid without DEVCTL.SESSION set.
 	 * Also, DRVVBUS pulses for SRP (but not at 5V) ...
 	 */
-	if (usbintr & MUSB_INTR_BABBLE)
+	if (is_host_active(musb) && usbintr & MUSB_INTR_BABBLE)
 		pr_info("CAUTION: musb: Babble Interrupt Occurred\n");
 
 	if (usbintr & ((1 << wrp->drvvbus) << wrp->usb_shift)) {
@@ -377,7 +378,7 @@ static irqreturn_t dsps_interrupt(int irq, void *hci)
 		/* NOTE: this must complete power-on within 100 ms. */
 		dev_dbg(musb->controller, "VBUS %s (%s)%s, devctl %02x\n",
 				drvvbus ? "on" : "off",
-				otg_state_string(musb->xceiv->state),
+				usb_otg_state_string(musb->xceiv->state),
 				err ? " ERROR" : "",
 				devctl);
 		ret = IRQ_HANDLED;
@@ -596,14 +597,13 @@ err0:
 
 static int dsps_probe(struct platform_device *pdev)
 {
-	struct device_node *np = pdev->dev.of_node;
 	const struct of_device_id *match;
 	const struct dsps_musb_wrapper *wrp;
 	struct dsps_glue *glue;
 	struct resource *iomem;
 	int ret, i;
 
-	match = of_match_node(musb_dsps_of_match, np);
+	match = of_match_node(musb_dsps_of_match, pdev->dev.of_node);
 	if (!match) {
 		dev_err(&pdev->dev, "fail to get matching of_match struct\n");
 		ret = -EINVAL;
diff --git a/drivers/usb/musb/musb_gadget.c b/drivers/usb/musb/musb_gadget.c
index 83eddedcd9b..ba7092349fa 100644
--- a/drivers/usb/musb/musb_gadget.c
+++ b/drivers/usb/musb/musb_gadget.c
@@ -46,48 +46,6 @@
 #include "musb_core.h"
 
 
-/* MUSB PERIPHERAL status 3-mar-2006:
- *
- * - EP0 seems solid.  It passes both USBCV and usbtest control cases.
- *   Minor glitches:
- *
- *     + remote wakeup to Linux hosts work, but saw USBCV failures;
- *       in one test run (operator error?)
- *     + endpoint halt tests -- in both usbtest and usbcv -- seem
- *       to break when dma is enabled ... is something wrongly
- *       clearing SENDSTALL?
- *
- * - Mass storage behaved ok when last tested.  Network traffic patterns
- *   (with lots of short transfers etc) need retesting; they turn up the
- *   worst cases of the DMA, since short packets are typical but are not
- *   required.
- *
- * - TX/IN
- *     + both pio and dma behave in with network and g_zero tests
- *     + no cppi throughput issues other than no-hw-queueing
- *     + failed with FLAT_REG (DaVinci)
- *     + seems to behave with double buffering, PIO -and- CPPI
- *     + with gadgetfs + AIO, requests got lost?
- *
- * - RX/OUT
- *     + both pio and dma behave in with network and g_zero tests
- *     + dma is slow in typical case (short_not_ok is clear)
- *     + double buffering ok with PIO
- *     + double buffering *FAILS* with CPPI, wrong data bytes sometimes
- *     + request lossage observed with gadgetfs
- *
- * - ISO not tested ... might work, but only weakly isochronous
- *
- * - Gadget driver disabling of softconnect during bind() is ignored; so
- *   drivers can't hold off host requests until userspace is ready.
- *   (Workaround:  they can turn it off later.)
- *
- * - PORTABILITY (assumes PIO works):
- *     + DaVinci, basically works with cppi dma
- *     + OMAP 2430, ditto with mentor dma
- *     + TUSB 6010, platform-specific dma in the works
- */
-
 /* ----------------------------------------------------------------------- */
 
 #define is_buffer_mapped(req) (is_dma_capable() && \
@@ -280,41 +238,6 @@ static inline int max_ep_writesize(struct musb *musb, struct musb_ep *ep)
 		return ep->packet_sz;
 }
 
-
-#ifdef CONFIG_USB_INVENTRA_DMA
-
-/* Peripheral tx (IN) using Mentor DMA works as follows:
-	Only mode 0 is used for transfers <= wPktSize,
-	mode 1 is used for larger transfers,
-
-	One of the following happens:
-	- Host sends IN token which causes an endpoint interrupt
-		-> TxAvail
-			-> if DMA is currently busy, exit.
-			-> if queue is non-empty, txstate().
-
-	- Request is queued by the gadget driver.
-		-> if queue was previously empty, txstate()
-
-	txstate()
-		-> start
-		  /\	-> setup DMA
-		  |     (data is transferred to the FIFO, then sent out when
-		  |	IN token(s) are recd from Host.
-		  |		-> DMA interrupt on completion
-		  |		   calls TxAvail.
-		  |		      -> stop DMA, ~DMAENAB,
-		  |		      -> set TxPktRdy for last short pkt or zlp
-		  |		      -> Complete Request
-		  |		      -> Continue next request (call txstate)
-		  |___________________________________|
-
- * Non-Mentor DMA engines can of course work differently, such as by
- * upleveling from irq-per-packet to irq-per-buffer.
- */
-
-#endif
-
 /*
  * An endpoint is transmitting data. This can be called either from
  * the IRQ routine or from ep.queue() to kickstart a request on an
@@ -621,37 +544,6 @@ void musb_g_tx(struct musb *musb, u8 epnum)
 
 /* ------------------------------------------------------------ */
 
-#ifdef CONFIG_USB_INVENTRA_DMA
-
-/* Peripheral rx (OUT) using Mentor DMA works as follows:
-	- Only mode 0 is used.
-
-	- Request is queued by the gadget class driver.
-		-> if queue was previously empty, rxstate()
-
-	- Host sends OUT token which causes an endpoint interrupt
-	  /\      -> RxReady
-	  |	      -> if request queued, call rxstate
-	  |		/\	-> setup DMA
-	  |		|	     -> DMA interrupt on completion
-	  |		|		-> RxReady
-	  |		|		      -> stop DMA
-	  |		|		      -> ack the read
-	  |		|		      -> if data recd = max expected
-	  |		|				by the request, or host
-	  |		|				sent a short packet,
-	  |		|				complete the request,
-	  |		|				and start the next one.
-	  |		|_____________________________________|
-	  |					 else just wait for the host
-	  |					    to send the next OUT token.
-	  |__________________________________________________|
-
- * Non-Mentor DMA engines can of course work differently.
- */
-
-#endif
-
 /*
  * Context: controller locked, IRQs blocked, endpoint selected
  */
@@ -740,7 +632,7 @@ static void rxstate(struct musb *musb, struct musb_request *req)
 				struct dma_controller	*c;
 				struct dma_channel	*channel;
 				int			use_dma = 0;
-				int transfer_size;
+				unsigned int transfer_size;
 
 				c = musb->dma_controller;
 				channel = musb_ep->dma;
@@ -782,10 +674,11 @@ static void rxstate(struct musb *musb, struct musb_request *req)
 						csr | MUSB_RXCSR_DMAMODE);
 					musb_writew(epio, MUSB_RXCSR, csr);
 
-					transfer_size = min(request->length - request->actual,
+					transfer_size = min_t(unsigned int,
+							request->length -
+							request->actual,
 							channel->max_len);
 					musb_ep->dma->desired_mode = 1;
-
 				} else {
 					if (!musb_ep->hb_mult &&
 						musb_ep->hw_ep->rx_double_buffered)
@@ -815,7 +708,7 @@ static void rxstate(struct musb *musb, struct musb_request *req)
 
 				struct dma_controller *c;
 				struct dma_channel *channel;
-				int transfer_size = 0;
+				unsigned int transfer_size = 0;
 
 				c = musb->dma_controller;
 				channel = musb_ep->dma;
@@ -824,11 +717,13 @@ static void rxstate(struct musb *musb, struct musb_request *req)
 				if (fifo_count < musb_ep->packet_sz)
 					transfer_size = fifo_count;
 				else if (request->short_not_ok)
-					transfer_size =	min(request->length -
+					transfer_size =	min_t(unsigned int,
+							request->length -
 							request->actual,
 							channel->max_len);
 				else
-					transfer_size = min(request->length -
+					transfer_size = min_t(unsigned int,
+							request->length -
 							request->actual,
 							(unsigned)fifo_count);
 
@@ -1681,7 +1576,7 @@ static int musb_gadget_wakeup(struct usb_gadget *gadget)
 		goto done;
 	default:
 		dev_dbg(musb->controller, "Unhandled wake: %s\n",
-			otg_state_string(musb->xceiv->state));
+			usb_otg_state_string(musb->xceiv->state));
 		goto done;
 	}
 
@@ -1801,13 +1696,6 @@ static const struct usb_gadget_ops musb_gadget_operations = {
  * all peripheral ports are external...
  */
 
-static void musb_gadget_release(struct device *dev)
-{
-	/* kref_put(WHAT) */
-	dev_dbg(dev, "%s\n", __func__);
-}
-
-
 static void
 init_peripheral_ep(struct musb *musb, struct musb_ep *ep, u8 epnum, int is_in)
 {
@@ -1892,12 +1780,7 @@ int musb_gadget_setup(struct musb *musb)
 	musb->g.speed = USB_SPEED_UNKNOWN;
 
 	/* this "gadget" abstracts/virtualizes the controller */
-	dev_set_name(&musb->g.dev, "gadget");
-	musb->g.dev.parent = musb->controller;
-	musb->g.dev.dma_mask = musb->controller->dma_mask;
-	musb->g.dev.release = musb_gadget_release;
 	musb->g.name = musb_driver_name;
-
 	musb->g.is_otg = 1;
 
 	musb_g_init_endpoints(musb);
@@ -1905,11 +1788,6 @@ int musb_gadget_setup(struct musb *musb)
 	musb->is_active = 0;
 	musb_platform_try_idle(musb, 0);
 
-	status = device_register(&musb->g.dev);
-	if (status != 0) {
-		put_device(&musb->g.dev);
-		return status;
-	}
 	status = usb_add_gadget_udc(musb->controller, &musb->g);
 	if (status)
 		goto err;
@@ -1924,8 +1802,6 @@ err:
 void musb_gadget_cleanup(struct musb *musb)
 {
 	usb_del_gadget_udc(&musb->g);
-	if (musb->g.dev.parent)
-		device_unregister(&musb->g.dev);
 }
 
 /*
@@ -1977,9 +1853,8 @@ static int musb_gadget_start(struct usb_gadget *g,
 		goto err;
 	}
 
-	if ((musb->xceiv->last_event == USB_EVENT_ID)
-				&& otg->set_vbus)
-		otg_set_vbus(otg, 1);
+	if (musb->xceiv->last_event == USB_EVENT_ID)
+		musb_platform_set_vbus(musb, 1);
 
 	hcd->self.uses_pio_for_control = 1;
 
@@ -2063,6 +1938,7 @@ static int musb_gadget_stop(struct usb_gadget *g,
 	dev_dbg(musb->controller, "unregistering driver %s\n", driver->function);
 
 	musb->is_active = 0;
+	musb->gadget_driver = NULL;
 	musb_platform_try_idle(musb, 0);
 	spin_unlock_irqrestore(&musb->lock, flags);
 
@@ -2099,7 +1975,7 @@ void musb_g_resume(struct musb *musb)
 		break;
 	default:
 		WARNING("unhandled RESUME transition (%s)\n",
-				otg_state_string(musb->xceiv->state));
+				usb_otg_state_string(musb->xceiv->state));
 	}
 }
 
@@ -2129,7 +2005,7 @@ void musb_g_suspend(struct musb *musb)
 		 * A_PERIPHERAL may need care too
 		 */
 		WARNING("unhandled SUSPEND transition (%s)\n",
-				otg_state_string(musb->xceiv->state));
+				usb_otg_state_string(musb->xceiv->state));
 	}
 }
 
@@ -2163,7 +2039,7 @@ void musb_g_disconnect(struct musb *musb)
 	switch (musb->xceiv->state) {
 	default:
 		dev_dbg(musb->controller, "Unhandled disconnect %s, setting a_idle\n",
-			otg_state_string(musb->xceiv->state));
+			usb_otg_state_string(musb->xceiv->state));
 		musb->xceiv->state = OTG_STATE_A_IDLE;
 		MUSB_HST_MODE(musb);
 		break;
diff --git a/drivers/usb/musb/musb_gadget_ep0.c b/drivers/usb/musb/musb_gadget_ep0.c
index c9c1ac4e075..2af45a0c893 100644
--- a/drivers/usb/musb/musb_gadget_ep0.c
+++ b/drivers/usb/musb/musb_gadget_ep0.c
@@ -505,8 +505,10 @@ static void ep0_rxstate(struct musb *musb)
 			req->status = -EOVERFLOW;
 			count = len;
 		}
-		musb_read_fifo(&musb->endpoints[0], count, buf);
-		req->actual += count;
+		if (count > 0) {
+			musb_read_fifo(&musb->endpoints[0], count, buf);
+			req->actual += count;
+		}
 		csr = MUSB_CSR0_P_SVDRXPKTRDY;
 		if (count < 64 || req->actual == req->length) {
 			musb->ep0_state = MUSB_EP0_STAGE_STATUSIN;
diff --git a/drivers/usb/musb/musb_host.c b/drivers/usb/musb/musb_host.c
index 1ce1fcf3f3e..8914dec49f0 100644
--- a/drivers/usb/musb/musb_host.c
+++ b/drivers/usb/musb/musb_host.c
@@ -2453,7 +2453,7 @@ static int musb_bus_suspend(struct usb_hcd *hcd)
 
 	if (musb->is_active) {
 		WARNING("trying to suspend as %s while active\n",
-				otg_state_string(musb->xceiv->state));
+				usb_otg_state_string(musb->xceiv->state));
 		return -EBUSY;
 	} else
 		return 0;
@@ -2465,6 +2465,118 @@ static int musb_bus_resume(struct usb_hcd *hcd)
 	return 0;
 }
 
+
+#ifndef CONFIG_MUSB_PIO_ONLY
+
+#define MUSB_USB_DMA_ALIGN 4
+
+struct musb_temp_buffer {
+	void *kmalloc_ptr;
+	void *old_xfer_buffer;
+	u8 data[0];
+};
+
+static void musb_free_temp_buffer(struct urb *urb)
+{
+	enum dma_data_direction dir;
+	struct musb_temp_buffer *temp;
+
+	if (!(urb->transfer_flags & URB_ALIGNED_TEMP_BUFFER))
+		return;
+
+	dir = usb_urb_dir_in(urb) ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
+
+	temp = container_of(urb->transfer_buffer, struct musb_temp_buffer,
+			    data);
+
+	if (dir == DMA_FROM_DEVICE) {
+		memcpy(temp->old_xfer_buffer, temp->data,
+		       urb->transfer_buffer_length);
+	}
+	urb->transfer_buffer = temp->old_xfer_buffer;
+	kfree(temp->kmalloc_ptr);
+
+	urb->transfer_flags &= ~URB_ALIGNED_TEMP_BUFFER;
+}
+
+static int musb_alloc_temp_buffer(struct urb *urb, gfp_t mem_flags)
+{
+	enum dma_data_direction dir;
+	struct musb_temp_buffer *temp;
+	void *kmalloc_ptr;
+	size_t kmalloc_size;
+
+	if (urb->num_sgs || urb->sg ||
+	    urb->transfer_buffer_length == 0 ||
+	    !((uintptr_t)urb->transfer_buffer & (MUSB_USB_DMA_ALIGN - 1)))
+		return 0;
+
+	dir = usb_urb_dir_in(urb) ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
+
+	/* Allocate a buffer with enough padding for alignment */
+	kmalloc_size = urb->transfer_buffer_length +
+		sizeof(struct musb_temp_buffer) + MUSB_USB_DMA_ALIGN - 1;
+
+	kmalloc_ptr = kmalloc(kmalloc_size, mem_flags);
+	if (!kmalloc_ptr)
+		return -ENOMEM;
+
+	/* Position our struct temp_buffer such that data is aligned */
+	temp = PTR_ALIGN(kmalloc_ptr, MUSB_USB_DMA_ALIGN);
+
+
+	temp->kmalloc_ptr = kmalloc_ptr;
+	temp->old_xfer_buffer = urb->transfer_buffer;
+	if (dir == DMA_TO_DEVICE)
+		memcpy(temp->data, urb->transfer_buffer,
+		       urb->transfer_buffer_length);
+	urb->transfer_buffer = temp->data;
+
+	urb->transfer_flags |= URB_ALIGNED_TEMP_BUFFER;
+
+	return 0;
+}
+
+static int musb_map_urb_for_dma(struct usb_hcd *hcd, struct urb *urb,
+				      gfp_t mem_flags)
+{
+	struct musb	*musb = hcd_to_musb(hcd);
+	int ret;
+
+	/*
+	 * The DMA engine in RTL1.8 and above cannot handle
+	 * DMA addresses that are not aligned to a 4 byte boundary.
+	 * For such engine implemented (un)map_urb_for_dma hooks.
+	 * Do not use these hooks for RTL<1.8
+	 */
+	if (musb->hwvers < MUSB_HWVERS_1800)
+		return usb_hcd_map_urb_for_dma(hcd, urb, mem_flags);
+
+	ret = musb_alloc_temp_buffer(urb, mem_flags);
+	if (ret)
+		return ret;
+
+	ret = usb_hcd_map_urb_for_dma(hcd, urb, mem_flags);
+	if (ret)
+		musb_free_temp_buffer(urb);
+
+	return ret;
+}
+
+static void musb_unmap_urb_for_dma(struct usb_hcd *hcd, struct urb *urb)
+{
+	struct musb	*musb = hcd_to_musb(hcd);
+
+	usb_hcd_unmap_urb_for_dma(hcd, urb);
+
+	/* Do not use this hook for RTL<1.8 (see description above) */
+	if (musb->hwvers < MUSB_HWVERS_1800)
+		return;
+
+	musb_free_temp_buffer(urb);
+}
+#endif /* !CONFIG_MUSB_PIO_ONLY */
+
 const struct hc_driver musb_hc_driver = {
 	.description		= "musb-hcd",
 	.product_desc		= "MUSB HDRC host driver",
@@ -2484,6 +2596,11 @@ const struct hc_driver musb_hc_driver = {
 	.urb_dequeue		= musb_urb_dequeue,
 	.endpoint_disable	= musb_h_disable,
 
+#ifndef CONFIG_MUSB_PIO_ONLY
+	.map_urb_for_dma	= musb_map_urb_for_dma,
+	.unmap_urb_for_dma	= musb_unmap_urb_for_dma,
+#endif
+
 	.hub_status_data	= musb_hub_status_data,
 	.hub_control		= musb_hub_control,
 	.bus_suspend		= musb_bus_suspend,
diff --git a/drivers/usb/musb/musb_virthub.c b/drivers/usb/musb/musb_virthub.c
index f70579154de..ef7d11045f5 100644
--- a/drivers/usb/musb/musb_virthub.c
+++ b/drivers/usb/musb/musb_virthub.c
@@ -95,7 +95,7 @@ static void musb_port_suspend(struct musb *musb, bool do_suspend)
 			break;
 		default:
 			dev_dbg(musb->controller, "bogus rh suspend? %s\n",
-				otg_state_string(musb->xceiv->state));
+				usb_otg_state_string(musb->xceiv->state));
 		}
 	} else if (power & MUSB_POWER_SUSPENDM) {
 		power &= ~MUSB_POWER_SUSPENDM;
@@ -203,7 +203,7 @@ void musb_root_disconnect(struct musb *musb)
 		break;
 	default:
 		dev_dbg(musb->controller, "host disconnect (%s)\n",
-			otg_state_string(musb->xceiv->state));
+			usb_otg_state_string(musb->xceiv->state));
 	}
 }
 
diff --git a/drivers/usb/musb/omap2430.c b/drivers/usb/musb/omap2430.c
index 1a42a458f2c..3551f1a30c6 100644
--- a/drivers/usb/musb/omap2430.c
+++ b/drivers/usb/musb/omap2430.c
@@ -117,7 +117,7 @@ static void omap2430_musb_try_idle(struct musb *musb, unsigned long timeout)
 	if (musb->is_active || ((musb->a_wait_bcon == 0)
 			&& (musb->xceiv->state == OTG_STATE_A_WAIT_BCON))) {
 		dev_dbg(musb->controller, "%s active, deleting timer\n",
-			otg_state_string(musb->xceiv->state));
+			usb_otg_state_string(musb->xceiv->state));
 		del_timer(&musb_idle_timer);
 		last_timer = jiffies;
 		return;
@@ -134,7 +134,7 @@ static void omap2430_musb_try_idle(struct musb *musb, unsigned long timeout)
 	last_timer = timeout;
 
 	dev_dbg(musb->controller, "%s inactive, for idle timer for %lu ms\n",
-		otg_state_string(musb->xceiv->state),
+		usb_otg_state_string(musb->xceiv->state),
 		(unsigned long)jiffies_to_msecs(timeout - jiffies));
 	mod_timer(&musb_idle_timer, timeout);
 }
@@ -174,8 +174,7 @@ static void omap2430_musb_set_vbus(struct musb *musb, int is_on)
 				}
 			}
 
-			if (otg->set_vbus)
-				otg_set_vbus(otg, 1);
+			otg_set_vbus(otg, 1);
 		} else {
 			musb->is_active = 1;
 			otg->default_a = 1;
@@ -200,7 +199,7 @@ static void omap2430_musb_set_vbus(struct musb *musb, int is_on)
 
 	dev_dbg(musb->controller, "VBUS %s, devctl %02x "
 		/* otg %3x conf %08x prcm %08x */ "\n",
-		otg_state_string(musb->xceiv->state),
+		usb_otg_state_string(musb->xceiv->state),
 		musb_readb(musb->mregs, MUSB_DEVCTL));
 }
 
@@ -292,14 +291,14 @@ static void omap_musb_set_mailbox(struct omap2430_glue *glue)
 
 		musb->xceiv->last_event = USB_EVENT_NONE;
 		if (musb->gadget_driver) {
+			omap2430_musb_set_vbus(musb, 0);
 			pm_runtime_mark_last_busy(dev);
 			pm_runtime_put_autosuspend(dev);
 		}
 
-		if (data->interface_type == MUSB_INTERFACE_UTMI) {
-			if (musb->xceiv->otg->set_vbus)
-				otg_set_vbus(musb->xceiv->otg, 0);
-		}
+		if (data->interface_type == MUSB_INTERFACE_UTMI)
+			otg_set_vbus(musb->xceiv->otg, 0);
+
 		omap_control_usb_set_mode(glue->control_otghs,
 			USB_MODE_DISCONNECT);
 		break;
@@ -355,7 +354,12 @@ static int omap2430_musb_init(struct musb *musb)
 	else
 		musb->xceiv = devm_usb_get_phy_dev(dev, 0);
 
-	if (IS_ERR_OR_NULL(musb->xceiv)) {
+	if (IS_ERR(musb->xceiv)) {
+		status = PTR_ERR(musb->xceiv);
+
+		if (status == -ENXIO)
+			return status;
+
 		pr_err("HS USB OTG: no transceiver configured\n");
 		return -EPROBE_DEFER;
 	}
@@ -393,6 +397,8 @@ static int omap2430_musb_init(struct musb *musb)
 	if (glue->status != OMAP_MUSB_UNKNOWN)
 		omap_musb_set_mailbox(glue);
 
+	usb_phy_init(musb->xceiv);
+
 	pm_runtime_put_noidle(musb->controller);
 	return 0;
 
@@ -526,10 +532,10 @@ static int omap2430_probe(struct platform_device *pdev)
 		}
 
 		of_property_read_u32(np, "mode", (u32 *)&pdata->mode);
-		of_property_read_u32(np, "interface_type",
+		of_property_read_u32(np, "interface-type",
 						(u32 *)&data->interface_type);
-		of_property_read_u32(np, "num_eps", (u32 *)&config->num_eps);
-		of_property_read_u32(np, "ram_bits", (u32 *)&config->ram_bits);
+		of_property_read_u32(np, "num-eps", (u32 *)&config->num_eps);
+		of_property_read_u32(np, "ram-bits", (u32 *)&config->ram_bits);
 		of_property_read_u32(np, "power", (u32 *)&pdata->power);
 		config->multipoint = of_property_read_bool(np, "multipoint");
 		pdata->has_mailbox = of_property_read_bool(np,
diff --git a/drivers/usb/musb/tusb6010.c b/drivers/usb/musb/tusb6010.c
index 464bd23cccd..7369ba33c94 100644
--- a/drivers/usb/musb/tusb6010.c
+++ b/drivers/usb/musb/tusb6010.c
@@ -423,7 +423,7 @@ static void musb_do_idle(unsigned long _musb)
 			&& (musb->idle_timeout == 0
 				|| time_after(jiffies, musb->idle_timeout))) {
 			dev_dbg(musb->controller, "Nothing connected %s, turning off VBUS\n",
-					otg_state_string(musb->xceiv->state));
+					usb_otg_state_string(musb->xceiv->state));
 		}
 		/* FALLTHROUGH */
 	case OTG_STATE_A_IDLE:
@@ -478,7 +478,7 @@ static void tusb_musb_try_idle(struct musb *musb, unsigned long timeout)
 	if (musb->is_active || ((musb->a_wait_bcon == 0)
 			&& (musb->xceiv->state == OTG_STATE_A_WAIT_BCON))) {
 		dev_dbg(musb->controller, "%s active, deleting timer\n",
-			otg_state_string(musb->xceiv->state));
+			usb_otg_state_string(musb->xceiv->state));
 		del_timer(&musb_idle_timer);
 		last_timer = jiffies;
 		return;
@@ -495,7 +495,7 @@ static void tusb_musb_try_idle(struct musb *musb, unsigned long timeout)
 	last_timer = timeout;
 
 	dev_dbg(musb->controller, "%s inactive, for idle timer for %lu ms\n",
-		otg_state_string(musb->xceiv->state),
+		usb_otg_state_string(musb->xceiv->state),
 		(unsigned long)jiffies_to_msecs(timeout - jiffies));
 	mod_timer(&musb_idle_timer, timeout);
 }
@@ -571,7 +571,7 @@ static void tusb_musb_set_vbus(struct musb *musb, int is_on)
 	musb_writeb(musb->mregs, MUSB_DEVCTL, devctl);
 
 	dev_dbg(musb->controller, "VBUS %s, devctl %02x otg %3x conf %08x prcm %08x\n",
-		otg_state_string(musb->xceiv->state),
+		usb_otg_state_string(musb->xceiv->state),
 		musb_readb(musb->mregs, MUSB_DEVCTL),
 		musb_readl(tbase, TUSB_DEV_OTG_STAT),
 		conf, prcm);
@@ -678,13 +678,13 @@ tusb_otg_ints(struct musb *musb, u32 int_src, void __iomem *tbase)
 				musb->is_active = 0;
 			}
 			dev_dbg(musb->controller, "vbus change, %s, otg %03x\n",
-				otg_state_string(musb->xceiv->state), otg_stat);
+				usb_otg_state_string(musb->xceiv->state), otg_stat);
 			idle_timeout = jiffies + (1 * HZ);
 			schedule_work(&musb->irq_work);
 
 		} else /* A-dev state machine */ {
 			dev_dbg(musb->controller, "vbus change, %s, otg %03x\n",
-				otg_state_string(musb->xceiv->state), otg_stat);
+				usb_otg_state_string(musb->xceiv->state), otg_stat);
 
 			switch (musb->xceiv->state) {
 			case OTG_STATE_A_IDLE:
@@ -733,7 +733,7 @@ tusb_otg_ints(struct musb *musb, u32 int_src, void __iomem *tbase)
 		u8	devctl;
 
 		dev_dbg(musb->controller, "%s timer, %03x\n",
-			otg_state_string(musb->xceiv->state), otg_stat);
+			usb_otg_state_string(musb->xceiv->state), otg_stat);
 
 		switch (musb->xceiv->state) {
 		case OTG_STATE_A_WAIT_VRISE:
diff --git a/drivers/usb/musb/ux500.c b/drivers/usb/musb/ux500.c
index 13a39291376..2c80004e0a8 100644
--- a/drivers/usb/musb/ux500.c
+++ b/drivers/usb/musb/ux500.c
@@ -26,6 +26,7 @@
 #include <linux/err.h>
 #include <linux/io.h>
 #include <linux/platform_device.h>
+#include <linux/usb/musb-ux500.h>
 
 #include "musb_core.h"
 
@@ -36,6 +37,98 @@ struct ux500_glue {
 };
 #define glue_to_musb(g)	platform_get_drvdata(g->musb)
 
+static void ux500_musb_set_vbus(struct musb *musb, int is_on)
+{
+	u8            devctl;
+	unsigned long timeout = jiffies + msecs_to_jiffies(1000);
+	/* HDRC controls CPEN, but beware current surges during device
+	 * connect.  They can trigger transient overcurrent conditions
+	 * that must be ignored.
+	 */
+
+	devctl = musb_readb(musb->mregs, MUSB_DEVCTL);
+
+	if (is_on) {
+		if (musb->xceiv->state == OTG_STATE_A_IDLE) {
+			/* start the session */
+			devctl |= MUSB_DEVCTL_SESSION;
+			musb_writeb(musb->mregs, MUSB_DEVCTL, devctl);
+			/*
+			 * Wait for the musb to set as A device to enable the
+			 * VBUS
+			 */
+			while (musb_readb(musb->mregs, MUSB_DEVCTL) & 0x80) {
+
+				if (time_after(jiffies, timeout)) {
+					dev_err(musb->controller,
+					"configured as A device timeout");
+					break;
+				}
+			}
+
+		} else {
+			musb->is_active = 1;
+			musb->xceiv->otg->default_a = 1;
+			musb->xceiv->state = OTG_STATE_A_WAIT_VRISE;
+			devctl |= MUSB_DEVCTL_SESSION;
+			MUSB_HST_MODE(musb);
+		}
+	} else {
+		musb->is_active = 0;
+
+		/* NOTE: we're skipping A_WAIT_VFALL -> A_IDLE and jumping
+		 * right to B_IDLE...
+		 */
+		musb->xceiv->otg->default_a = 0;
+		devctl &= ~MUSB_DEVCTL_SESSION;
+		MUSB_DEV_MODE(musb);
+	}
+	musb_writeb(musb->mregs, MUSB_DEVCTL, devctl);
+
+	/*
+	 * Devctl values will be updated after vbus goes below
+	 * session_valid. The time taken depends on the capacitance
+	 * on VBUS line. The max discharge time can be upto 1 sec
+	 * as per the spec. Typically on our platform, it is 200ms
+	 */
+	if (!is_on)
+		mdelay(200);
+
+	dev_dbg(musb->controller, "VBUS %s, devctl %02x\n",
+		usb_otg_state_string(musb->xceiv->state),
+		musb_readb(musb->mregs, MUSB_DEVCTL));
+}
+
+static int musb_otg_notifications(struct notifier_block *nb,
+		unsigned long event, void *unused)
+{
+	struct musb *musb = container_of(nb, struct musb, nb);
+
+	dev_dbg(musb->controller, "musb_otg_notifications %ld %s\n",
+			event, usb_otg_state_string(musb->xceiv->state));
+
+	switch (event) {
+	case UX500_MUSB_ID:
+		dev_dbg(musb->controller, "ID GND\n");
+		ux500_musb_set_vbus(musb, 1);
+		break;
+	case UX500_MUSB_VBUS:
+		dev_dbg(musb->controller, "VBUS Connect\n");
+		break;
+	case UX500_MUSB_NONE:
+		dev_dbg(musb->controller, "VBUS Disconnect\n");
+		if (is_host_active(musb))
+			ux500_musb_set_vbus(musb, 0);
+		else
+			musb->xceiv->state = OTG_STATE_B_IDLE;
+		break;
+	default:
+		dev_dbg(musb->controller, "ID float\n");
+		return NOTIFY_DONE;
+	}
+	return NOTIFY_OK;
+}
+
 static irqreturn_t ux500_musb_interrupt(int irq, void *__hci)
 {
 	unsigned long   flags;
@@ -58,12 +151,21 @@ static irqreturn_t ux500_musb_interrupt(int irq, void *__hci)
 
 static int ux500_musb_init(struct musb *musb)
 {
+	int status;
+
 	musb->xceiv = usb_get_phy(USB_PHY_TYPE_USB2);
 	if (IS_ERR_OR_NULL(musb->xceiv)) {
 		pr_err("HS USB OTG: no transceiver configured\n");
 		return -EPROBE_DEFER;
 	}
 
+	musb->nb.notifier_call = musb_otg_notifications;
+	status = usb_register_notifier(musb->xceiv, &musb->nb);
+	if (status < 0) {
+		dev_dbg(musb->controller, "notification register failed\n");
+		return status;
+	}
+
 	musb->isr = ux500_musb_interrupt;
 
 	return 0;
@@ -71,6 +173,8 @@ static int ux500_musb_init(struct musb *musb)
 
 static int ux500_musb_exit(struct musb *musb)
 {
+	usb_unregister_notifier(musb->xceiv, &musb->nb);
+
 	usb_put_phy(musb->xceiv);
 
 	return 0;
@@ -79,6 +183,8 @@ static int ux500_musb_exit(struct musb *musb)
 static const struct musb_platform_ops ux500_ops = {
 	.init		= ux500_musb_init,
 	.exit		= ux500_musb_exit,
+
+	.set_vbus	= ux500_musb_set_vbus,
 };
 
 static int ux500_probe(struct platform_device *pdev)
diff --git a/drivers/usb/musb/ux500_dma.c b/drivers/usb/musb/ux500_dma.c
index 039e567dd3b..33812064114 100644
--- a/drivers/usb/musb/ux500_dma.c
+++ b/drivers/usb/musb/ux500_dma.c
@@ -1,7 +1,7 @@
 /*
  * drivers/usb/musb/ux500_dma.c
  *
- * U8500 and U5500 DMA support code
+ * U8500 DMA support code
  *
  * Copyright (C) 2009 STMicroelectronics
  * Copyright (C) 2011 ST-Ericsson SA
@@ -30,6 +30,7 @@
 #include <linux/dma-mapping.h>
 #include <linux/dmaengine.h>
 #include <linux/pfn.h>
+#include <linux/sizes.h>
 #include <linux/platform_data/usb-musb-ux500.h>
 #include "musb_core.h"
 
@@ -56,7 +57,7 @@ struct ux500_dma_controller {
 };
 
 /* Work function invoked from DMA callback to handle rx transfers. */
-void ux500_dma_callback(void *private_data)
+static void ux500_dma_callback(void *private_data)
 {
 	struct dma_channel *channel = private_data;
 	struct ux500_dma_channel *ux500_channel = channel->private_data;
@@ -93,8 +94,9 @@ static bool ux500_configure_channel(struct dma_channel *channel,
 	struct musb *musb = ux500_channel->controller->private_data;
 
 	dev_dbg(musb->controller,
-		"packet_sz=%d, mode=%d, dma_addr=0x%x, len=%d is_tx=%d\n",
-		packet_sz, mode, dma_addr, len, ux500_channel->is_tx);
+		"packet_sz=%d, mode=%d, dma_addr=0x%llu, len=%d is_tx=%d\n",
+		packet_sz, mode, (unsigned long long) dma_addr,
+		len, ux500_channel->is_tx);
 
 	ux500_channel->cur_len = len;
 
@@ -191,7 +193,7 @@ static int ux500_dma_is_compatible(struct dma_channel *channel,
 		u16 maxpacket, void *buf, u32 length)
 {
 	if ((maxpacket & 0x3)		||
-		((int)buf & 0x3)	||
+		((unsigned long int) buf & 0x3)	||
 		(length < 512)		||
 		(length & 0x3))
 		return false;
@@ -372,12 +374,17 @@ struct dma_controller *dma_controller_create(struct musb *musb, void __iomem *ba
 
 	controller = kzalloc(sizeof(*controller), GFP_KERNEL);
 	if (!controller)
-		return NULL;
+		goto kzalloc_fail;
 
 	controller->private_data = musb;
 
 	/* Save physical address for DMA controller. */
 	iomem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!iomem) {
+		dev_err(musb->controller, "no memory resource defined\n");
+		goto plat_get_fail;
+	}
+
 	controller->phy_base = (dma_addr_t) iomem->start;
 
 	controller->controller.start = ux500_dma_controller_start;
@@ -389,4 +396,9 @@ struct dma_controller *dma_controller_create(struct musb *musb, void __iomem *ba
 	controller->controller.is_compatible = ux500_dma_is_compatible;
 
 	return &controller->controller;
+
+plat_get_fail:
+	kfree(controller);
+kzalloc_fail:
+	return NULL;
 }
diff --git a/drivers/usb/otg/Kconfig b/drivers/usb/otg/Kconfig
deleted file mode 100644
index 37962c99ff1..00000000000
--- a/drivers/usb/otg/Kconfig
+++ /dev/null
@@ -1,141 +0,0 @@
-#
-# USB OTG infrastructure may be needed for peripheral-only, host-only,
-# or OTG-capable configurations when OTG transceivers or controllers
-# are used.
-#
-
-comment "OTG and related infrastructure"
-
-config USB_OTG_UTILS
-	bool
-	help
-	  Select this to make sure the build includes objects from
-	  the OTG infrastructure directory.
-
-if USB || USB_GADGET
-
-#
-# USB Transceiver Drivers
-#
-config USB_GPIO_VBUS
-	tristate "GPIO based peripheral-only VBUS sensing 'transceiver'"
-	depends on GENERIC_GPIO
-	select USB_OTG_UTILS
-	help
-	  Provides simple GPIO VBUS sensing for controllers with an
-	  internal transceiver via the usb_phy interface, and
-	  optionally control of a D+ pullup GPIO as well as a VBUS
-	  current limit regulator.
-
-config ISP1301_OMAP
-	tristate "Philips ISP1301 with OMAP OTG"
-	depends on I2C && ARCH_OMAP_OTG
-	select USB_OTG_UTILS
-	help
-	  If you say yes here you get support for the Philips ISP1301
-	  USB-On-The-Go transceiver working with the OMAP OTG controller.
-	  The ISP1301 is a full speed USB  transceiver which is used in
-	  products including H2, H3, and H4 development boards for Texas
-	  Instruments OMAP processors.
-
-	  This driver can also be built as a module.  If so, the module
-	  will be called isp1301_omap.
-
-config USB_ULPI
-	bool "Generic ULPI Transceiver Driver"
-	depends on ARM
-	select USB_OTG_UTILS
-	help
-	  Enable this to support ULPI connected USB OTG transceivers which
-	  are likely found on embedded boards.
-
-config USB_ULPI_VIEWPORT
-	bool
-	depends on USB_ULPI
-	help
-	  Provides read/write operations to the ULPI phy register set for
-	  controllers with a viewport register (e.g. Chipidea/ARC controllers).
-
-config TWL4030_USB
-	tristate "TWL4030 USB Transceiver Driver"
-	depends on TWL4030_CORE && REGULATOR_TWL4030 && USB_MUSB_OMAP2PLUS
-	select USB_OTG_UTILS
-	help
-	  Enable this to support the USB OTG transceiver on TWL4030
-	  family chips (including the TWL5030 and TPS659x0 devices).
-	  This transceiver supports high and full speed devices plus,
-	  in host mode, low speed.
-
-config TWL6030_USB
-	tristate "TWL6030 USB Transceiver Driver"
-	depends on TWL4030_CORE && OMAP_USB2 && USB_MUSB_OMAP2PLUS
-	select USB_OTG_UTILS
-	help
-	  Enable this to support the USB OTG transceiver on TWL6030
-	  family chips. This TWL6030 transceiver has the VBUS and ID GND
-	  and OTG SRP events capabilities. For all other transceiver functionality
-	  UTMI PHY is embedded in OMAP4430. The internal PHY configurations APIs
-	  are hooked to this driver through platform_data structure.
-	  The definition of internal PHY APIs are in the mach-omap2 layer.
-
-config NOP_USB_XCEIV
-	tristate "NOP USB Transceiver Driver"
-	select USB_OTG_UTILS
-	help
-	  This driver is to be used by all the usb transceiver which are either
-	  built-in with usb ip or which are autonomous and doesn't require any
-	  phy programming such as ISP1x04 etc.
-
-config USB_MSM_OTG
-	tristate "OTG support for Qualcomm on-chip USB controller"
-	depends on (USB || USB_GADGET) && ARCH_MSM
-	select USB_OTG_UTILS
-	help
-	  Enable this to support the USB OTG transceiver on MSM chips. It
-	  handles PHY initialization, clock management, and workarounds
-	  required after resetting the hardware and power management.
-	  This driver is required even for peripheral only or host only
-	  mode configurations.
-	  This driver is not supported on boards like trout which
-	  has an external PHY.
-
-config AB8500_USB
-	tristate "AB8500 USB Transceiver Driver"
-	depends on AB8500_CORE
-	select USB_OTG_UTILS
-	help
-	  Enable this to support the USB OTG transceiver in AB8500 chip.
-	  This transceiver supports high and full speed devices plus,
-	  in host mode, low speed.
-
-config FSL_USB2_OTG
-	bool "Freescale USB OTG Transceiver Driver"
-	depends on USB_EHCI_FSL && USB_FSL_USB2 && USB_SUSPEND
-	select USB_OTG
-	select USB_OTG_UTILS
-	help
-	  Enable this to support Freescale USB OTG transceiver.
-
-config USB_MXS_PHY
-	tristate "Freescale MXS USB PHY support"
-	depends on ARCH_MXC || ARCH_MXS
-	select STMP_DEVICE
-	select USB_OTG_UTILS
-	help
-	  Enable this to support the Freescale MXS USB PHY.
-
-	  MXS Phy is used by some of the i.MX SoCs, for example imx23/28/6x.
-
-config USB_MV_OTG
-	tristate "Marvell USB OTG support"
-	depends on USB_EHCI_MV && USB_MV_UDC && USB_SUSPEND
-	select USB_OTG
-	select USB_OTG_UTILS
-	help
-	  Say Y here if you want to build Marvell USB OTG transciever
-	  driver in kernel (including PXA and MMP series). This driver
-	  implements role switch between EHCI host driver and gadget driver.
-
-	  To compile this driver as a module, choose M here.
-
-endif # USB || OTG
diff --git a/drivers/usb/otg/Makefile b/drivers/usb/otg/Makefile
deleted file mode 100644
index a844b8d35d1..00000000000
--- a/drivers/usb/otg/Makefile
+++ /dev/null
@@ -1,24 +0,0 @@
-#
-# OTG infrastructure and transceiver drivers
-#
-
-ccflags-$(CONFIG_USB_DEBUG)		:= -DDEBUG
-ccflags-$(CONFIG_USB_GADGET_DEBUG)	+= -DDEBUG
-
-# infrastructure
-obj-$(CONFIG_USB_OTG_UTILS)	+= otg.o
-
-# transceiver drivers
-obj-$(CONFIG_USB_GPIO_VBUS)	+= gpio_vbus.o
-obj-$(CONFIG_ISP1301_OMAP)	+= isp1301_omap.o
-obj-$(CONFIG_TWL4030_USB)	+= twl4030-usb.o
-obj-$(CONFIG_TWL6030_USB)	+= twl6030-usb.o
-obj-$(CONFIG_NOP_USB_XCEIV)	+= nop-usb-xceiv.o
-obj-$(CONFIG_USB_ULPI)		+= ulpi.o
-obj-$(CONFIG_USB_ULPI_VIEWPORT)	+= ulpi_viewport.o
-obj-$(CONFIG_USB_MSM_OTG)	+= msm_otg.o
-obj-$(CONFIG_AB8500_USB)	+= ab8500-usb.o
-fsl_usb2_otg-objs		:= fsl_otg.o otg_fsm.o
-obj-$(CONFIG_FSL_USB2_OTG)	+= fsl_usb2_otg.o
-obj-$(CONFIG_USB_MXS_PHY)	+= mxs-phy.o
-obj-$(CONFIG_USB_MV_OTG)	+= mv_otg.o
diff --git a/drivers/usb/otg/ab8500-usb.c b/drivers/usb/otg/ab8500-usb.c
deleted file mode 100644
index 2d86f26a018..00000000000
--- a/drivers/usb/otg/ab8500-usb.c
+++ /dev/null
@@ -1,596 +0,0 @@
-/*
- * drivers/usb/otg/ab8500_usb.c
- *
- * USB transceiver driver for AB8500 chip
- *
- * Copyright (C) 2010 ST-Ericsson AB
- * Mian Yousaf Kaukab <mian.yousaf.kaukab@stericsson.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., 675 Mass Ave, Cambridge, MA 02139, USA.
- *
- */
-
-#include <linux/module.h>
-#include <linux/platform_device.h>
-#include <linux/usb/otg.h>
-#include <linux/slab.h>
-#include <linux/notifier.h>
-#include <linux/interrupt.h>
-#include <linux/delay.h>
-#include <linux/mfd/abx500.h>
-#include <linux/mfd/abx500/ab8500.h>
-
-#define AB8500_MAIN_WD_CTRL_REG 0x01
-#define AB8500_USB_LINE_STAT_REG 0x80
-#define AB8500_USB_PHY_CTRL_REG 0x8A
-
-#define AB8500_BIT_OTG_STAT_ID (1 << 0)
-#define AB8500_BIT_PHY_CTRL_HOST_EN (1 << 0)
-#define AB8500_BIT_PHY_CTRL_DEVICE_EN (1 << 1)
-#define AB8500_BIT_WD_CTRL_ENABLE (1 << 0)
-#define AB8500_BIT_WD_CTRL_KICK (1 << 1)
-
-#define AB8500_V1x_LINK_STAT_WAIT (HZ/10)
-#define AB8500_WD_KICK_DELAY_US 100 /* usec */
-#define AB8500_WD_V11_DISABLE_DELAY_US 100 /* usec */
-#define AB8500_WD_V10_DISABLE_DELAY_MS 100 /* ms */
-
-/* Usb line status register */
-enum ab8500_usb_link_status {
-	USB_LINK_NOT_CONFIGURED = 0,
-	USB_LINK_STD_HOST_NC,
-	USB_LINK_STD_HOST_C_NS,
-	USB_LINK_STD_HOST_C_S,
-	USB_LINK_HOST_CHG_NM,
-	USB_LINK_HOST_CHG_HS,
-	USB_LINK_HOST_CHG_HS_CHIRP,
-	USB_LINK_DEDICATED_CHG,
-	USB_LINK_ACA_RID_A,
-	USB_LINK_ACA_RID_B,
-	USB_LINK_ACA_RID_C_NM,
-	USB_LINK_ACA_RID_C_HS,
-	USB_LINK_ACA_RID_C_HS_CHIRP,
-	USB_LINK_HM_IDGND,
-	USB_LINK_RESERVED,
-	USB_LINK_NOT_VALID_LINK
-};
-
-struct ab8500_usb {
-	struct usb_phy phy;
-	struct device *dev;
-	int irq_num_id_rise;
-	int irq_num_id_fall;
-	int irq_num_vbus_rise;
-	int irq_num_vbus_fall;
-	int irq_num_link_status;
-	unsigned vbus_draw;
-	struct delayed_work dwork;
-	struct work_struct phy_dis_work;
-	unsigned long link_status_wait;
-	int rev;
-};
-
-static inline struct ab8500_usb *phy_to_ab(struct usb_phy *x)
-{
-	return container_of(x, struct ab8500_usb, phy);
-}
-
-static void ab8500_usb_wd_workaround(struct ab8500_usb *ab)
-{
-	abx500_set_register_interruptible(ab->dev,
-		AB8500_SYS_CTRL2_BLOCK,
-		AB8500_MAIN_WD_CTRL_REG,
-		AB8500_BIT_WD_CTRL_ENABLE);
-
-	udelay(AB8500_WD_KICK_DELAY_US);
-
-	abx500_set_register_interruptible(ab->dev,
-		AB8500_SYS_CTRL2_BLOCK,
-		AB8500_MAIN_WD_CTRL_REG,
-		(AB8500_BIT_WD_CTRL_ENABLE
-		| AB8500_BIT_WD_CTRL_KICK));
-
-	if (ab->rev > 0x10) /* v1.1 v2.0 */
-		udelay(AB8500_WD_V11_DISABLE_DELAY_US);
-	else /* v1.0 */
-		msleep(AB8500_WD_V10_DISABLE_DELAY_MS);
-
-	abx500_set_register_interruptible(ab->dev,
-		AB8500_SYS_CTRL2_BLOCK,
-		AB8500_MAIN_WD_CTRL_REG,
-		0);
-}
-
-static void ab8500_usb_phy_ctrl(struct ab8500_usb *ab, bool sel_host,
-					bool enable)
-{
-	u8 ctrl_reg;
-	abx500_get_register_interruptible(ab->dev,
-				AB8500_USB,
-				AB8500_USB_PHY_CTRL_REG,
-				&ctrl_reg);
-	if (sel_host) {
-		if (enable)
-			ctrl_reg |= AB8500_BIT_PHY_CTRL_HOST_EN;
-		else
-			ctrl_reg &= ~AB8500_BIT_PHY_CTRL_HOST_EN;
-	} else {
-		if (enable)
-			ctrl_reg |= AB8500_BIT_PHY_CTRL_DEVICE_EN;
-		else
-			ctrl_reg &= ~AB8500_BIT_PHY_CTRL_DEVICE_EN;
-	}
-
-	abx500_set_register_interruptible(ab->dev,
-				AB8500_USB,
-				AB8500_USB_PHY_CTRL_REG,
-				ctrl_reg);
-
-	/* Needed to enable the phy.*/
-	if (enable)
-		ab8500_usb_wd_workaround(ab);
-}
-
-#define ab8500_usb_host_phy_en(ab)	ab8500_usb_phy_ctrl(ab, true, true)
-#define ab8500_usb_host_phy_dis(ab)	ab8500_usb_phy_ctrl(ab, true, false)
-#define ab8500_usb_peri_phy_en(ab)	ab8500_usb_phy_ctrl(ab, false, true)
-#define ab8500_usb_peri_phy_dis(ab)	ab8500_usb_phy_ctrl(ab, false, false)
-
-static int ab8500_usb_link_status_update(struct ab8500_usb *ab)
-{
-	u8 reg;
-	enum ab8500_usb_link_status lsts;
-	void *v = NULL;
-	enum usb_phy_events event;
-
-	abx500_get_register_interruptible(ab->dev,
-			AB8500_USB,
-			AB8500_USB_LINE_STAT_REG,
-			&reg);
-
-	lsts = (reg >> 3) & 0x0F;
-
-	switch (lsts) {
-	case USB_LINK_NOT_CONFIGURED:
-	case USB_LINK_RESERVED:
-	case USB_LINK_NOT_VALID_LINK:
-		/* TODO: Disable regulators. */
-		ab8500_usb_host_phy_dis(ab);
-		ab8500_usb_peri_phy_dis(ab);
-		ab->phy.state = OTG_STATE_B_IDLE;
-		ab->phy.otg->default_a = false;
-		ab->vbus_draw = 0;
-		event = USB_EVENT_NONE;
-		break;
-
-	case USB_LINK_STD_HOST_NC:
-	case USB_LINK_STD_HOST_C_NS:
-	case USB_LINK_STD_HOST_C_S:
-	case USB_LINK_HOST_CHG_NM:
-	case USB_LINK_HOST_CHG_HS:
-	case USB_LINK_HOST_CHG_HS_CHIRP:
-		if (ab->phy.otg->gadget) {
-			/* TODO: Enable regulators. */
-			ab8500_usb_peri_phy_en(ab);
-			v = ab->phy.otg->gadget;
-		}
-		event = USB_EVENT_VBUS;
-		break;
-
-	case USB_LINK_HM_IDGND:
-		if (ab->phy.otg->host) {
-			/* TODO: Enable regulators. */
-			ab8500_usb_host_phy_en(ab);
-			v = ab->phy.otg->host;
-		}
-		ab->phy.state = OTG_STATE_A_IDLE;
-		ab->phy.otg->default_a = true;
-		event = USB_EVENT_ID;
-		break;
-
-	case USB_LINK_ACA_RID_A:
-	case USB_LINK_ACA_RID_B:
-		/* TODO */
-	case USB_LINK_ACA_RID_C_NM:
-	case USB_LINK_ACA_RID_C_HS:
-	case USB_LINK_ACA_RID_C_HS_CHIRP:
-	case USB_LINK_DEDICATED_CHG:
-		/* TODO: vbus_draw */
-		event = USB_EVENT_CHARGER;
-		break;
-	}
-
-	atomic_notifier_call_chain(&ab->phy.notifier, event, v);
-
-	return 0;
-}
-
-static void ab8500_usb_delayed_work(struct work_struct *work)
-{
-	struct ab8500_usb *ab = container_of(work, struct ab8500_usb,
-						dwork.work);
-
-	ab8500_usb_link_status_update(ab);
-}
-
-static irqreturn_t ab8500_usb_v1x_common_irq(int irq, void *data)
-{
-	struct ab8500_usb *ab = (struct ab8500_usb *) data;
-
-	/* Wait for link status to become stable. */
-	schedule_delayed_work(&ab->dwork, ab->link_status_wait);
-
-	return IRQ_HANDLED;
-}
-
-static irqreturn_t ab8500_usb_v1x_vbus_fall_irq(int irq, void *data)
-{
-	struct ab8500_usb *ab = (struct ab8500_usb *) data;
-
-	/* Link status will not be updated till phy is disabled. */
-	ab8500_usb_peri_phy_dis(ab);
-
-	/* Wait for link status to become stable. */
-	schedule_delayed_work(&ab->dwork, ab->link_status_wait);
-
-	return IRQ_HANDLED;
-}
-
-static irqreturn_t ab8500_usb_v20_irq(int irq, void *data)
-{
-	struct ab8500_usb *ab = (struct ab8500_usb *) data;
-
-	ab8500_usb_link_status_update(ab);
-
-	return IRQ_HANDLED;
-}
-
-static void ab8500_usb_phy_disable_work(struct work_struct *work)
-{
-	struct ab8500_usb *ab = container_of(work, struct ab8500_usb,
-						phy_dis_work);
-
-	if (!ab->phy.otg->host)
-		ab8500_usb_host_phy_dis(ab);
-
-	if (!ab->phy.otg->gadget)
-		ab8500_usb_peri_phy_dis(ab);
-}
-
-static int ab8500_usb_set_power(struct usb_phy *phy, unsigned mA)
-{
-	struct ab8500_usb *ab;
-
-	if (!phy)
-		return -ENODEV;
-
-	ab = phy_to_ab(phy);
-
-	ab->vbus_draw = mA;
-
-	if (mA)
-		atomic_notifier_call_chain(&ab->phy.notifier,
-				USB_EVENT_ENUMERATED, ab->phy.otg->gadget);
-	return 0;
-}
-
-/* TODO: Implement some way for charging or other drivers to read
- * ab->vbus_draw.
- */
-
-static int ab8500_usb_set_suspend(struct usb_phy *x, int suspend)
-{
-	/* TODO */
-	return 0;
-}
-
-static int ab8500_usb_set_peripheral(struct usb_otg *otg,
-					struct usb_gadget *gadget)
-{
-	struct ab8500_usb *ab;
-
-	if (!otg)
-		return -ENODEV;
-
-	ab = phy_to_ab(otg->phy);
-
-	/* Some drivers call this function in atomic context.
-	 * Do not update ab8500 registers directly till this
-	 * is fixed.
-	 */
-
-	if (!gadget) {
-		/* TODO: Disable regulators. */
-		otg->gadget = NULL;
-		schedule_work(&ab->phy_dis_work);
-	} else {
-		otg->gadget = gadget;
-		otg->phy->state = OTG_STATE_B_IDLE;
-
-		/* Phy will not be enabled if cable is already
-		 * plugged-in. Schedule to enable phy.
-		 * Use same delay to avoid any race condition.
-		 */
-		schedule_delayed_work(&ab->dwork, ab->link_status_wait);
-	}
-
-	return 0;
-}
-
-static int ab8500_usb_set_host(struct usb_otg *otg, struct usb_bus *host)
-{
-	struct ab8500_usb *ab;
-
-	if (!otg)
-		return -ENODEV;
-
-	ab = phy_to_ab(otg->phy);
-
-	/* Some drivers call this function in atomic context.
-	 * Do not update ab8500 registers directly till this
-	 * is fixed.
-	 */
-
-	if (!host) {
-		/* TODO: Disable regulators. */
-		otg->host = NULL;
-		schedule_work(&ab->phy_dis_work);
-	} else {
-		otg->host = host;
-		/* Phy will not be enabled if cable is already
-		 * plugged-in. Schedule to enable phy.
-		 * Use same delay to avoid any race condition.
-		 */
-		schedule_delayed_work(&ab->dwork, ab->link_status_wait);
-	}
-
-	return 0;
-}
-
-static void ab8500_usb_irq_free(struct ab8500_usb *ab)
-{
-	if (ab->rev < 0x20) {
-		free_irq(ab->irq_num_id_rise, ab);
-		free_irq(ab->irq_num_id_fall, ab);
-		free_irq(ab->irq_num_vbus_rise, ab);
-		free_irq(ab->irq_num_vbus_fall, ab);
-	} else {
-		free_irq(ab->irq_num_link_status, ab);
-	}
-}
-
-static int ab8500_usb_v1x_res_setup(struct platform_device *pdev,
-				struct ab8500_usb *ab)
-{
-	int err;
-
-	ab->irq_num_id_rise = platform_get_irq_byname(pdev, "ID_WAKEUP_R");
-	if (ab->irq_num_id_rise < 0) {
-		dev_err(&pdev->dev, "ID rise irq not found\n");
-		return ab->irq_num_id_rise;
-	}
-	err = request_threaded_irq(ab->irq_num_id_rise, NULL,
-		ab8500_usb_v1x_common_irq,
-		IRQF_NO_SUSPEND | IRQF_SHARED,
-		"usb-id-rise", ab);
-	if (err < 0) {
-		dev_err(ab->dev, "request_irq failed for ID rise irq\n");
-		goto fail0;
-	}
-
-	ab->irq_num_id_fall = platform_get_irq_byname(pdev, "ID_WAKEUP_F");
-	if (ab->irq_num_id_fall < 0) {
-		dev_err(&pdev->dev, "ID fall irq not found\n");
-		return ab->irq_num_id_fall;
-	}
-	err = request_threaded_irq(ab->irq_num_id_fall, NULL,
-		ab8500_usb_v1x_common_irq,
-		IRQF_NO_SUSPEND | IRQF_SHARED,
-		"usb-id-fall", ab);
-	if (err < 0) {
-		dev_err(ab->dev, "request_irq failed for ID fall irq\n");
-		goto fail1;
-	}
-
-	ab->irq_num_vbus_rise = platform_get_irq_byname(pdev, "VBUS_DET_R");
-	if (ab->irq_num_vbus_rise < 0) {
-		dev_err(&pdev->dev, "VBUS rise irq not found\n");
-		return ab->irq_num_vbus_rise;
-	}
-	err = request_threaded_irq(ab->irq_num_vbus_rise, NULL,
-		ab8500_usb_v1x_common_irq,
-		IRQF_NO_SUSPEND | IRQF_SHARED,
-		"usb-vbus-rise", ab);
-	if (err < 0) {
-		dev_err(ab->dev, "request_irq failed for Vbus rise irq\n");
-		goto fail2;
-	}
-
-	ab->irq_num_vbus_fall = platform_get_irq_byname(pdev, "VBUS_DET_F");
-	if (ab->irq_num_vbus_fall < 0) {
-		dev_err(&pdev->dev, "VBUS fall irq not found\n");
-		return ab->irq_num_vbus_fall;
-	}
-	err = request_threaded_irq(ab->irq_num_vbus_fall, NULL,
-		ab8500_usb_v1x_vbus_fall_irq,
-		IRQF_NO_SUSPEND | IRQF_SHARED,
-		"usb-vbus-fall", ab);
-	if (err < 0) {
-		dev_err(ab->dev, "request_irq failed for Vbus fall irq\n");
-		goto fail3;
-	}
-
-	return 0;
-fail3:
-	free_irq(ab->irq_num_vbus_rise, ab);
-fail2:
-	free_irq(ab->irq_num_id_fall, ab);
-fail1:
-	free_irq(ab->irq_num_id_rise, ab);
-fail0:
-	return err;
-}
-
-static int ab8500_usb_v2_res_setup(struct platform_device *pdev,
-				struct ab8500_usb *ab)
-{
-	int err;
-
-	ab->irq_num_link_status = platform_get_irq_byname(pdev,
-						"USB_LINK_STATUS");
-	if (ab->irq_num_link_status < 0) {
-		dev_err(&pdev->dev, "Link status irq not found\n");
-		return ab->irq_num_link_status;
-	}
-
-	err = request_threaded_irq(ab->irq_num_link_status, NULL,
-		ab8500_usb_v20_irq,
-		IRQF_NO_SUSPEND | IRQF_SHARED,
-		"usb-link-status", ab);
-	if (err < 0) {
-		dev_err(ab->dev,
-			"request_irq failed for link status irq\n");
-		return err;
-	}
-
-	return 0;
-}
-
-static int ab8500_usb_probe(struct platform_device *pdev)
-{
-	struct ab8500_usb	*ab;
-	struct usb_otg		*otg;
-	int err;
-	int rev;
-
-	rev = abx500_get_chip_id(&pdev->dev);
-	if (rev < 0) {
-		dev_err(&pdev->dev, "Chip id read failed\n");
-		return rev;
-	} else if (rev < 0x10) {
-		dev_err(&pdev->dev, "Unsupported AB8500 chip\n");
-		return -ENODEV;
-	}
-
-	ab = kzalloc(sizeof *ab, GFP_KERNEL);
-	if (!ab)
-		return -ENOMEM;
-
-	otg = kzalloc(sizeof *otg, GFP_KERNEL);
-	if (!otg) {
-		kfree(ab);
-		return -ENOMEM;
-	}
-
-	ab->dev			= &pdev->dev;
-	ab->rev			= rev;
-	ab->phy.dev		= ab->dev;
-	ab->phy.otg		= otg;
-	ab->phy.label		= "ab8500";
-	ab->phy.set_suspend	= ab8500_usb_set_suspend;
-	ab->phy.set_power	= ab8500_usb_set_power;
-	ab->phy.state		= OTG_STATE_UNDEFINED;
-
-	otg->phy		= &ab->phy;
-	otg->set_host		= ab8500_usb_set_host;
-	otg->set_peripheral	= ab8500_usb_set_peripheral;
-
-	platform_set_drvdata(pdev, ab);
-
-	ATOMIC_INIT_NOTIFIER_HEAD(&ab->phy.notifier);
-
-	/* v1: Wait for link status to become stable.
-	 * all: Updates form set_host and set_peripheral as they are atomic.
-	 */
-	INIT_DELAYED_WORK(&ab->dwork, ab8500_usb_delayed_work);
-
-	/* all: Disable phy when called from set_host and set_peripheral */
-	INIT_WORK(&ab->phy_dis_work, ab8500_usb_phy_disable_work);
-
-	if (ab->rev < 0x20) {
-		err = ab8500_usb_v1x_res_setup(pdev, ab);
-		ab->link_status_wait = AB8500_V1x_LINK_STAT_WAIT;
-	} else {
-		err = ab8500_usb_v2_res_setup(pdev, ab);
-	}
-
-	if (err < 0)
-		goto fail0;
-
-	err = usb_add_phy(&ab->phy, USB_PHY_TYPE_USB2);
-	if (err) {
-		dev_err(&pdev->dev, "Can't register transceiver\n");
-		goto fail1;
-	}
-
-	dev_info(&pdev->dev, "AB8500 usb driver initialized\n");
-
-	return 0;
-fail1:
-	ab8500_usb_irq_free(ab);
-fail0:
-	kfree(otg);
-	kfree(ab);
-	return err;
-}
-
-static int ab8500_usb_remove(struct platform_device *pdev)
-{
-	struct ab8500_usb *ab = platform_get_drvdata(pdev);
-
-	ab8500_usb_irq_free(ab);
-
-	cancel_delayed_work_sync(&ab->dwork);
-
-	cancel_work_sync(&ab->phy_dis_work);
-
-	usb_remove_phy(&ab->phy);
-
-	ab8500_usb_host_phy_dis(ab);
-	ab8500_usb_peri_phy_dis(ab);
-
-	platform_set_drvdata(pdev, NULL);
-
-	kfree(ab->phy.otg);
-	kfree(ab);
-
-	return 0;
-}
-
-static struct platform_driver ab8500_usb_driver = {
-	.probe		= ab8500_usb_probe,
-	.remove		= ab8500_usb_remove,
-	.driver		= {
-		.name	= "ab8500-usb",
-		.owner	= THIS_MODULE,
-	},
-};
-
-static int __init ab8500_usb_init(void)
-{
-	return platform_driver_register(&ab8500_usb_driver);
-}
-subsys_initcall(ab8500_usb_init);
-
-static void __exit ab8500_usb_exit(void)
-{
-	platform_driver_unregister(&ab8500_usb_driver);
-}
-module_exit(ab8500_usb_exit);
-
-MODULE_ALIAS("platform:ab8500_usb");
-MODULE_AUTHOR("ST-Ericsson AB");
-MODULE_DESCRIPTION("AB8500 usb transceiver driver");
-MODULE_LICENSE("GPL");
diff --git a/drivers/usb/phy/Kconfig b/drivers/usb/phy/Kconfig
index 65217a59006..aab2ab2fbc9 100644
--- a/drivers/usb/phy/Kconfig
+++ b/drivers/usb/phy/Kconfig
@@ -1,13 +1,74 @@
 #
 # Physical Layer USB driver configuration
 #
-comment "USB Physical Layer drivers"
-	depends on USB || USB_GADGET
+menuconfig USB_PHY
+	bool "USB Physical Layer drivers"
+	help
+	  USB controllers (those which are host, device or DRD) need a
+	  device to handle the physical layer signalling, commonly called
+	  a PHY.
+
+	  The following drivers add support for such PHY devices.
+
+if USB_PHY
+
+#
+# USB Transceiver Drivers
+#
+config AB8500_USB
+	tristate "AB8500 USB Transceiver Driver"
+	depends on AB8500_CORE
+	help
+	  Enable this to support the USB OTG transceiver in AB8500 chip.
+	  This transceiver supports high and full speed devices plus,
+	  in host mode, low speed.
+
+config FSL_USB2_OTG
+	bool "Freescale USB OTG Transceiver Driver"
+	depends on USB_EHCI_FSL && USB_FSL_USB2 && USB_SUSPEND
+	select USB_OTG
+	help
+	  Enable this to support Freescale USB OTG transceiver.
+
+config ISP1301_OMAP
+	tristate "Philips ISP1301 with OMAP OTG"
+	depends on I2C && ARCH_OMAP_OTG
+	help
+	  If you say yes here you get support for the Philips ISP1301
+	  USB-On-The-Go transceiver working with the OMAP OTG controller.
+	  The ISP1301 is a full speed USB  transceiver which is used in
+	  products including H2, H3, and H4 development boards for Texas
+	  Instruments OMAP processors.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called isp1301_omap.
+
+config MV_U3D_PHY
+	bool "Marvell USB 3.0 PHY controller Driver"
+	depends on CPU_MMP3
+	help
+	  Enable this to support Marvell USB 3.0 phy controller for Marvell
+	  SoC.
+
+config NOP_USB_XCEIV
+	tristate "NOP USB Transceiver Driver"
+	help
+	  This driver is to be used by all the usb transceiver which are either
+	  built-in with usb ip or which are autonomous and doesn't require any
+	  phy programming such as ISP1x04 etc.
+
+config OMAP_CONTROL_USB
+	tristate "OMAP CONTROL USB Driver"
+	help
+	  Enable this to add support for the USB part present in the control
+	  module. This driver has API to power on the USB2 PHY and to write to
+	  the mailbox. The mailbox is present only in omap4 and the register to
+	  power on the USB2 PHY is present in OMAP4 and OMAP5. OMAP5 has an
+	  additional register to power on USB3 PHY.
 
 config OMAP_USB2
 	tristate "OMAP USB2 PHY Driver"
 	depends on ARCH_OMAP2PLUS
-	select USB_OTG_UTILS
 	select OMAP_CONTROL_USB
 	help
 	  Enable this to support the transceiver that is part of SOC. This
@@ -17,7 +78,6 @@ config OMAP_USB2
 
 config OMAP_USB3
 	tristate "OMAP USB3 PHY Driver"
-	select USB_OTG_UTILS
 	select OMAP_CONTROL_USB
 	help
 	  Enable this to support the USB3 PHY that is part of SOC. This
@@ -25,19 +85,61 @@ config OMAP_USB3
 	  This driver interacts with the "OMAP Control USB Driver" to power
 	  on/off the PHY.
 
-config OMAP_CONTROL_USB
-	tristate "OMAP CONTROL USB Driver"
+config SAMSUNG_USBPHY
+	tristate "Samsung USB PHY Driver"
 	help
-	  Enable this to add support for the USB part present in the control
-	  module. This driver has API to power on the USB2 PHY and to write to
-	  the mailbox. The mailbox is present only in omap4 and the register to
-	  power on the USB2 PHY is present in OMAP4 and OMAP5. OMAP5 has an
-	  additional register to power on USB3 PHY.
+	  Enable this to support Samsung USB phy helper driver for Samsung SoCs.
+	  This driver provides common interface to interact, for Samsung USB 2.0 PHY
+	  driver and later for Samsung USB 3.0 PHY driver.
+
+config SAMSUNG_USB2PHY
+	tristate "Samsung USB 2.0 PHY controller Driver"
+	select SAMSUNG_USBPHY
+	help
+	  Enable this to support Samsung USB 2.0 (High Speed) PHY controller
+	  driver for Samsung SoCs.
+
+config SAMSUNG_USB3PHY
+	tristate "Samsung USB 3.0 PHY controller Driver"
+	select SAMSUNG_USBPHY
+	help
+	  Enable this to support Samsung USB 3.0 (Super Speed) phy controller
+	  for samsung SoCs.
+
+config TWL4030_USB
+	tristate "TWL4030 USB Transceiver Driver"
+	depends on TWL4030_CORE && REGULATOR_TWL4030 && USB_MUSB_OMAP2PLUS
+	help
+	  Enable this to support the USB OTG transceiver on TWL4030
+	  family chips (including the TWL5030 and TPS659x0 devices).
+	  This transceiver supports high and full speed devices plus,
+	  in host mode, low speed.
+
+config TWL6030_USB
+	tristate "TWL6030 USB Transceiver Driver"
+	depends on TWL4030_CORE && OMAP_USB2 && USB_MUSB_OMAP2PLUS
+	help
+	  Enable this to support the USB OTG transceiver on TWL6030
+	  family chips. This TWL6030 transceiver has the VBUS and ID GND
+	  and OTG SRP events capabilities. For all other transceiver functionality
+	  UTMI PHY is embedded in OMAP4430. The internal PHY configurations APIs
+	  are hooked to this driver through platform_data structure.
+	  The definition of internal PHY APIs are in the mach-omap2 layer.
+
+config USB_GPIO_VBUS
+	tristate "GPIO based peripheral-only VBUS sensing 'transceiver'"
+	depends on GENERIC_GPIO
+	help
+	  Provides simple GPIO VBUS sensing for controllers with an
+	  internal transceiver via the usb_phy interface, and
+	  optionally control of a D+ pullup GPIO as well as a VBUS
+	  current limit regulator.
 
 config USB_ISP1301
 	tristate "NXP ISP1301 USB transceiver support"
 	depends on USB || USB_GADGET
 	depends on I2C
+	select USB_OTG_UTILS
 	help
 	  Say Y here to add support for the NXP ISP1301 USB transceiver driver.
 	  This chip is typically used as USB transceiver for USB host, gadget
@@ -46,18 +148,41 @@ config USB_ISP1301
 	  To compile this driver as a module, choose M here: the
 	  module will be called isp1301.
 
-config MV_U3D_PHY
-	bool "Marvell USB 3.0 PHY controller Driver"
-	depends on USB_MV_U3D
-	select USB_OTG_UTILS
+config USB_MSM_OTG
+	tristate "OTG support for Qualcomm on-chip USB controller"
+	depends on (USB || USB_GADGET) && ARCH_MSM
 	help
-	  Enable this to support Marvell USB 3.0 phy controller for Marvell
-	  SoC.
+	  Enable this to support the USB OTG transceiver on MSM chips. It
+	  handles PHY initialization, clock management, and workarounds
+	  required after resetting the hardware and power management.
+	  This driver is required even for peripheral only or host only
+	  mode configurations.
+	  This driver is not supported on boards like trout which
+	  has an external PHY.
+
+config USB_MV_OTG
+	tristate "Marvell USB OTG support"
+	depends on USB_EHCI_MV && USB_MV_UDC && USB_SUSPEND
+	select USB_OTG
+	help
+	  Say Y here if you want to build Marvell USB OTG transciever
+	  driver in kernel (including PXA and MMP series). This driver
+	  implements role switch between EHCI host driver and gadget driver.
+
+	  To compile this driver as a module, choose M here.
+
+config USB_MXS_PHY
+	tristate "Freescale MXS USB PHY support"
+	depends on ARCH_MXC || ARCH_MXS
+	select STMP_DEVICE
+	help
+	  Enable this to support the Freescale MXS USB PHY.
+
+	  MXS Phy is used by some of the i.MX SoCs, for example imx23/28/6x.
 
 config USB_RCAR_PHY
 	tristate "Renesas R-Car USB phy support"
 	depends on USB || USB_GADGET
-	select USB_OTG_UTILS
 	help
 	  Say Y here to add support for the Renesas R-Car USB phy driver.
 	  This chip is typically used as USB phy for USB host, gadget.
@@ -66,10 +191,18 @@ config USB_RCAR_PHY
 	  To compile this driver as a module, choose M here: the
 	  module will be called rcar-phy.
 
-config SAMSUNG_USBPHY
-	bool "Samsung USB PHY controller Driver"
-	depends on USB_S3C_HSOTG || USB_EHCI_S5P || USB_OHCI_EXYNOS
-	select USB_OTG_UTILS
+config USB_ULPI
+	bool "Generic ULPI Transceiver Driver"
+	depends on ARM
+	help
+	  Enable this to support ULPI connected USB OTG transceivers which
+	  are likely found on embedded boards.
+
+config USB_ULPI_VIEWPORT
+	bool
+	depends on USB_ULPI
 	help
-	  Enable this to support Samsung USB phy controller for samsung
-	  SoCs.
+	  Provides read/write operations to the ULPI phy register set for
+	  controllers with a viewport register (e.g. Chipidea/ARC controllers).
+
+endif # USB_PHY
diff --git a/drivers/usb/phy/Makefile b/drivers/usb/phy/Makefile
index b13faa193e0..a9169cb1e6f 100644
--- a/drivers/usb/phy/Makefile
+++ b/drivers/usb/phy/Makefile
@@ -4,11 +4,30 @@
 
 ccflags-$(CONFIG_USB_DEBUG) := -DDEBUG
 
-obj-$(CONFIG_OMAP_USB2)			+= omap-usb2.o
-obj-$(CONFIG_OMAP_USB3)			+= omap-usb3.o
-obj-$(CONFIG_OMAP_CONTROL_USB)		+= omap-control-usb.o
-obj-$(CONFIG_USB_ISP1301)		+= isp1301.o
-obj-$(CONFIG_MV_U3D_PHY)		+= mv_u3d_phy.o
-obj-$(CONFIG_USB_EHCI_TEGRA)	+= tegra_usb_phy.o
-obj-$(CONFIG_USB_RCAR_PHY)		+= rcar-phy.o
-obj-$(CONFIG_SAMSUNG_USBPHY)		+= samsung-usbphy.o
+obj-$(CONFIG_USB_PHY)			+= phy.o
+
+# transceiver drivers, keep the list sorted
+
+obj-$(CONFIG_AB8500_USB)		+= phy-ab8500-usb.o
+phy-fsl-usb2-objs			:= phy-fsl-usb.o phy-fsm-usb.o
+obj-$(CONFIG_FSL_USB2_OTG)		+= phy-fsl-usb2.o
+obj-$(CONFIG_ISP1301_OMAP)		+= phy-isp1301-omap.o
+obj-$(CONFIG_MV_U3D_PHY)		+= phy-mv-u3d-usb.o
+obj-$(CONFIG_NOP_USB_XCEIV)		+= phy-nop.o
+obj-$(CONFIG_OMAP_CONTROL_USB)		+= phy-omap-control.o
+obj-$(CONFIG_OMAP_USB2)			+= phy-omap-usb2.o
+obj-$(CONFIG_OMAP_USB3)			+= phy-omap-usb3.o
+obj-$(CONFIG_SAMSUNG_USBPHY)		+= phy-samsung-usb.o
+obj-$(CONFIG_SAMSUNG_USB2PHY)		+= phy-samsung-usb2.o
+obj-$(CONFIG_SAMSUNG_USB3PHY)		+= phy-samsung-usb3.o
+obj-$(CONFIG_TWL4030_USB)		+= phy-twl4030-usb.o
+obj-$(CONFIG_TWL6030_USB)		+= phy-twl6030-usb.o
+obj-$(CONFIG_USB_EHCI_TEGRA)		+= phy-tegra-usb.o
+obj-$(CONFIG_USB_GPIO_VBUS)		+= phy-gpio-vbus-usb.o
+obj-$(CONFIG_USB_ISP1301)		+= phy-isp1301.o
+obj-$(CONFIG_USB_MSM_OTG)		+= phy-msm-usb.o
+obj-$(CONFIG_USB_MV_OTG)		+= phy-mv-usb.o
+obj-$(CONFIG_USB_MXS_PHY)		+= phy-mxs-usb.o
+obj-$(CONFIG_USB_RCAR_PHY)		+= phy-rcar-usb.o
+obj-$(CONFIG_USB_ULPI)			+= phy-ulpi.o
+obj-$(CONFIG_USB_ULPI_VIEWPORT)		+= phy-ulpi-viewport.o
diff --git a/drivers/usb/phy/isp1301.c b/drivers/usb/phy/isp1301.c
deleted file mode 100644
index 18dbf7e3760..00000000000
--- a/drivers/usb/phy/isp1301.c
+++ /dev/null
@@ -1,71 +0,0 @@
-/*
- * NXP ISP1301 USB transceiver driver
- *
- * Copyright (C) 2012 Roland Stigge
- *
- * Author: Roland Stigge <stigge@antcom.de>
- *
- * 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/module.h>
-#include <linux/i2c.h>
-
-#define DRV_NAME		"isp1301"
-
-static const struct i2c_device_id isp1301_id[] = {
-	{ "isp1301", 0 },
-	{ }
-};
-
-static struct i2c_client *isp1301_i2c_client;
-
-static int isp1301_probe(struct i2c_client *client,
-			 const struct i2c_device_id *i2c_id)
-{
-	isp1301_i2c_client = client;
-	return 0;
-}
-
-static int isp1301_remove(struct i2c_client *client)
-{
-	return 0;
-}
-
-static struct i2c_driver isp1301_driver = {
-	.driver = {
-		.name = DRV_NAME,
-	},
-	.probe = isp1301_probe,
-	.remove = isp1301_remove,
-	.id_table = isp1301_id,
-};
-
-module_i2c_driver(isp1301_driver);
-
-static int match(struct device *dev, void *data)
-{
-	struct device_node *node = (struct device_node *)data;
-	return (dev->of_node == node) &&
-		(dev->driver == &isp1301_driver.driver);
-}
-
-struct i2c_client *isp1301_get_client(struct device_node *node)
-{
-	if (node) { /* reference of ISP1301 I2C node via DT */
-		struct device *dev = bus_find_device(&i2c_bus_type, NULL,
-						     node, match);
-		if (!dev)
-			return NULL;
-		return to_i2c_client(dev);
-	} else { /* non-DT: only one ISP1301 chip supported */
-		return isp1301_i2c_client;
-	}
-}
-EXPORT_SYMBOL_GPL(isp1301_get_client);
-
-MODULE_AUTHOR("Roland Stigge <stigge@antcom.de>");
-MODULE_DESCRIPTION("NXP ISP1301 USB transceiver driver");
-MODULE_LICENSE("GPL");
diff --git a/drivers/usb/phy/phy-ab8500-usb.c b/drivers/usb/phy/phy-ab8500-usb.c
new file mode 100644
index 00000000000..4acef26a2ef
--- /dev/null
+++ b/drivers/usb/phy/phy-ab8500-usb.c
@@ -0,0 +1,924 @@
+/*
+ * drivers/usb/otg/ab8500_usb.c
+ *
+ * USB transceiver driver for AB8500 chip
+ *
+ * Copyright (C) 2010 ST-Ericsson AB
+ * Mian Yousaf Kaukab <mian.yousaf.kaukab@stericsson.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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/usb/otg.h>
+#include <linux/slab.h>
+#include <linux/notifier.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/abx500/ab8500.h>
+#include <linux/usb/musb-ux500.h>
+#include <linux/regulator/consumer.h>
+#include <linux/pinctrl/consumer.h>
+
+/* Bank AB8500_SYS_CTRL2_BLOCK */
+#define AB8500_MAIN_WD_CTRL_REG 0x01
+
+/* Bank AB8500_USB */
+#define AB8500_USB_LINE_STAT_REG 0x80
+#define AB8505_USB_LINE_STAT_REG 0x94
+#define AB8500_USB_PHY_CTRL_REG 0x8A
+
+/* Bank AB8500_DEVELOPMENT */
+#define AB8500_BANK12_ACCESS 0x00
+
+/* Bank AB8500_DEBUG */
+#define AB8500_USB_PHY_TUNE1 0x05
+#define AB8500_USB_PHY_TUNE2 0x06
+#define AB8500_USB_PHY_TUNE3 0x07
+
+#define AB8500_BIT_OTG_STAT_ID (1 << 0)
+#define AB8500_BIT_PHY_CTRL_HOST_EN (1 << 0)
+#define AB8500_BIT_PHY_CTRL_DEVICE_EN (1 << 1)
+#define AB8500_BIT_WD_CTRL_ENABLE (1 << 0)
+#define AB8500_BIT_WD_CTRL_KICK (1 << 1)
+
+#define AB8500_WD_KICK_DELAY_US 100 /* usec */
+#define AB8500_WD_V11_DISABLE_DELAY_US 100 /* usec */
+#define AB8500_V20_31952_DISABLE_DELAY_US 100 /* usec */
+
+/* Usb line status register */
+enum ab8500_usb_link_status {
+	USB_LINK_NOT_CONFIGURED_8500 = 0,
+	USB_LINK_STD_HOST_NC_8500,
+	USB_LINK_STD_HOST_C_NS_8500,
+	USB_LINK_STD_HOST_C_S_8500,
+	USB_LINK_HOST_CHG_NM_8500,
+	USB_LINK_HOST_CHG_HS_8500,
+	USB_LINK_HOST_CHG_HS_CHIRP_8500,
+	USB_LINK_DEDICATED_CHG_8500,
+	USB_LINK_ACA_RID_A_8500,
+	USB_LINK_ACA_RID_B_8500,
+	USB_LINK_ACA_RID_C_NM_8500,
+	USB_LINK_ACA_RID_C_HS_8500,
+	USB_LINK_ACA_RID_C_HS_CHIRP_8500,
+	USB_LINK_HM_IDGND_8500,
+	USB_LINK_RESERVED_8500,
+	USB_LINK_NOT_VALID_LINK_8500,
+};
+
+enum ab8505_usb_link_status {
+	USB_LINK_NOT_CONFIGURED_8505 = 0,
+	USB_LINK_STD_HOST_NC_8505,
+	USB_LINK_STD_HOST_C_NS_8505,
+	USB_LINK_STD_HOST_C_S_8505,
+	USB_LINK_CDP_8505,
+	USB_LINK_RESERVED0_8505,
+	USB_LINK_RESERVED1_8505,
+	USB_LINK_DEDICATED_CHG_8505,
+	USB_LINK_ACA_RID_A_8505,
+	USB_LINK_ACA_RID_B_8505,
+	USB_LINK_ACA_RID_C_NM_8505,
+	USB_LINK_RESERVED2_8505,
+	USB_LINK_RESERVED3_8505,
+	USB_LINK_HM_IDGND_8505,
+	USB_LINK_CHARGERPORT_NOT_OK_8505,
+	USB_LINK_CHARGER_DM_HIGH_8505,
+	USB_LINK_PHYEN_NO_VBUS_NO_IDGND_8505,
+	USB_LINK_STD_UPSTREAM_NO_IDGNG_NO_VBUS_8505,
+	USB_LINK_STD_UPSTREAM_8505,
+	USB_LINK_CHARGER_SE1_8505,
+	USB_LINK_CARKIT_CHGR_1_8505,
+	USB_LINK_CARKIT_CHGR_2_8505,
+	USB_LINK_ACA_DOCK_CHGR_8505,
+	USB_LINK_SAMSUNG_BOOT_CBL_PHY_EN_8505,
+	USB_LINK_SAMSUNG_BOOT_CBL_PHY_DISB_8505,
+	USB_LINK_SAMSUNG_UART_CBL_PHY_EN_8505,
+	USB_LINK_SAMSUNG_UART_CBL_PHY_DISB_8505,
+	USB_LINK_MOTOROLA_FACTORY_CBL_PHY_EN_8505,
+};
+
+enum ab8500_usb_mode {
+	USB_IDLE = 0,
+	USB_PERIPHERAL,
+	USB_HOST,
+	USB_DEDICATED_CHG
+};
+
+struct ab8500_usb {
+	struct usb_phy phy;
+	struct device *dev;
+	struct ab8500 *ab8500;
+	unsigned vbus_draw;
+	struct work_struct phy_dis_work;
+	enum ab8500_usb_mode mode;
+	struct regulator *v_ape;
+	struct regulator *v_musb;
+	struct regulator *v_ulpi;
+	int saved_v_ulpi;
+	int previous_link_status_state;
+	struct pinctrl *pinctrl;
+	struct pinctrl_state *pins_sleep;
+};
+
+static inline struct ab8500_usb *phy_to_ab(struct usb_phy *x)
+{
+	return container_of(x, struct ab8500_usb, phy);
+}
+
+static void ab8500_usb_wd_workaround(struct ab8500_usb *ab)
+{
+	abx500_set_register_interruptible(ab->dev,
+		AB8500_SYS_CTRL2_BLOCK,
+		AB8500_MAIN_WD_CTRL_REG,
+		AB8500_BIT_WD_CTRL_ENABLE);
+
+	udelay(AB8500_WD_KICK_DELAY_US);
+
+	abx500_set_register_interruptible(ab->dev,
+		AB8500_SYS_CTRL2_BLOCK,
+		AB8500_MAIN_WD_CTRL_REG,
+		(AB8500_BIT_WD_CTRL_ENABLE
+		| AB8500_BIT_WD_CTRL_KICK));
+
+	udelay(AB8500_WD_V11_DISABLE_DELAY_US);
+
+	abx500_set_register_interruptible(ab->dev,
+		AB8500_SYS_CTRL2_BLOCK,
+		AB8500_MAIN_WD_CTRL_REG,
+		0);
+}
+
+static void ab8500_usb_regulator_enable(struct ab8500_usb *ab)
+{
+	int ret, volt;
+
+	ret = regulator_enable(ab->v_ape);
+	if (ret)
+		dev_err(ab->dev, "Failed to enable v-ape\n");
+
+	if (!is_ab8500_2p0_or_earlier(ab->ab8500)) {
+		ab->saved_v_ulpi = regulator_get_voltage(ab->v_ulpi);
+		if (ab->saved_v_ulpi < 0)
+			dev_err(ab->dev, "Failed to get v_ulpi voltage\n");
+
+		ret = regulator_set_voltage(ab->v_ulpi, 1300000, 1350000);
+		if (ret < 0)
+			dev_err(ab->dev, "Failed to set the Vintcore to 1.3V, ret=%d\n",
+					ret);
+
+		ret = regulator_set_optimum_mode(ab->v_ulpi, 28000);
+		if (ret < 0)
+			dev_err(ab->dev, "Failed to set optimum mode (ret=%d)\n",
+					ret);
+	}
+
+	ret = regulator_enable(ab->v_ulpi);
+	if (ret)
+		dev_err(ab->dev, "Failed to enable vddulpivio18\n");
+
+	if (!is_ab8500_2p0_or_earlier(ab->ab8500)) {
+		volt = regulator_get_voltage(ab->v_ulpi);
+		if ((volt != 1300000) && (volt != 1350000))
+			dev_err(ab->dev, "Vintcore is not set to 1.3V volt=%d\n",
+					volt);
+	}
+
+	ret = regulator_enable(ab->v_musb);
+	if (ret)
+		dev_err(ab->dev, "Failed to enable musb_1v8\n");
+}
+
+static void ab8500_usb_regulator_disable(struct ab8500_usb *ab)
+{
+	int ret;
+
+	regulator_disable(ab->v_musb);
+
+	regulator_disable(ab->v_ulpi);
+
+	/* USB is not the only consumer of Vintcore, restore old settings */
+	if (!is_ab8500_2p0_or_earlier(ab->ab8500)) {
+		if (ab->saved_v_ulpi > 0) {
+			ret = regulator_set_voltage(ab->v_ulpi,
+					ab->saved_v_ulpi, ab->saved_v_ulpi);
+			if (ret < 0)
+				dev_err(ab->dev, "Failed to set the Vintcore to %duV, ret=%d\n",
+						ab->saved_v_ulpi, ret);
+		}
+
+		ret = regulator_set_optimum_mode(ab->v_ulpi, 0);
+		if (ret < 0)
+			dev_err(ab->dev, "Failed to set optimum mode (ret=%d)\n",
+					ret);
+	}
+
+	regulator_disable(ab->v_ape);
+}
+
+static void ab8500_usb_wd_linkstatus(struct ab8500_usb *ab, u8 bit)
+{
+	/* Workaround for v2.0 bug # 31952 */
+	if (is_ab8500_2p0(ab->ab8500)) {
+		abx500_mask_and_set_register_interruptible(ab->dev,
+				AB8500_USB, AB8500_USB_PHY_CTRL_REG,
+				bit, bit);
+		udelay(AB8500_V20_31952_DISABLE_DELAY_US);
+	}
+}
+
+static void ab8500_usb_phy_enable(struct ab8500_usb *ab, bool sel_host)
+{
+	u8 bit;
+	bit = sel_host ? AB8500_BIT_PHY_CTRL_HOST_EN :
+		AB8500_BIT_PHY_CTRL_DEVICE_EN;
+
+	/* mux and configure USB pins to DEFAULT state */
+	ab->pinctrl = pinctrl_get_select(ab->dev, PINCTRL_STATE_DEFAULT);
+	if (IS_ERR(ab->pinctrl))
+		dev_err(ab->dev, "could not get/set default pinstate\n");
+
+	ab8500_usb_regulator_enable(ab);
+
+	abx500_mask_and_set_register_interruptible(ab->dev,
+			AB8500_USB, AB8500_USB_PHY_CTRL_REG,
+			bit, bit);
+}
+
+static void ab8500_usb_phy_disable(struct ab8500_usb *ab, bool sel_host)
+{
+	u8 bit;
+	bit = sel_host ? AB8500_BIT_PHY_CTRL_HOST_EN :
+		AB8500_BIT_PHY_CTRL_DEVICE_EN;
+
+	ab8500_usb_wd_linkstatus(ab, bit);
+
+	abx500_mask_and_set_register_interruptible(ab->dev,
+			AB8500_USB, AB8500_USB_PHY_CTRL_REG,
+			bit, 0);
+
+	/* Needed to disable the phy.*/
+	ab8500_usb_wd_workaround(ab);
+
+	ab8500_usb_regulator_disable(ab);
+
+	if (!IS_ERR(ab->pinctrl)) {
+		/* configure USB pins to SLEEP state */
+		ab->pins_sleep = pinctrl_lookup_state(ab->pinctrl,
+				PINCTRL_STATE_SLEEP);
+
+		if (IS_ERR(ab->pins_sleep))
+			dev_dbg(ab->dev, "could not get sleep pinstate\n");
+		else if (pinctrl_select_state(ab->pinctrl, ab->pins_sleep))
+			dev_err(ab->dev, "could not set pins to sleep state\n");
+
+		/* as USB pins are shared with idddet, release them to allow
+		 * iddet to request them
+		 */
+		pinctrl_put(ab->pinctrl);
+	}
+}
+
+#define ab8500_usb_host_phy_en(ab)	ab8500_usb_phy_enable(ab, true)
+#define ab8500_usb_host_phy_dis(ab)	ab8500_usb_phy_disable(ab, true)
+#define ab8500_usb_peri_phy_en(ab)	ab8500_usb_phy_enable(ab, false)
+#define ab8500_usb_peri_phy_dis(ab)	ab8500_usb_phy_disable(ab, false)
+
+static int ab8505_usb_link_status_update(struct ab8500_usb *ab,
+		enum ab8505_usb_link_status lsts)
+{
+	enum ux500_musb_vbus_id_status event = 0;
+
+	dev_dbg(ab->dev, "ab8505_usb_link_status_update %d\n", lsts);
+
+	/*
+	 * Spurious link_status interrupts are seen at the time of
+	 * disconnection of a device in RIDA state
+	 */
+	if (ab->previous_link_status_state == USB_LINK_ACA_RID_A_8505 &&
+			(lsts == USB_LINK_STD_HOST_NC_8505))
+		return 0;
+
+	ab->previous_link_status_state = lsts;
+
+	switch (lsts) {
+	case USB_LINK_ACA_RID_B_8505:
+		event = UX500_MUSB_RIDB;
+	case USB_LINK_NOT_CONFIGURED_8505:
+	case USB_LINK_RESERVED0_8505:
+	case USB_LINK_RESERVED1_8505:
+	case USB_LINK_RESERVED2_8505:
+	case USB_LINK_RESERVED3_8505:
+		ab->mode = USB_IDLE;
+		ab->phy.otg->default_a = false;
+		ab->vbus_draw = 0;
+		if (event != UX500_MUSB_RIDB)
+			event = UX500_MUSB_NONE;
+		/*
+		 * Fallback to default B_IDLE as nothing
+		 * is connected
+		 */
+		ab->phy.state = OTG_STATE_B_IDLE;
+		break;
+
+	case USB_LINK_ACA_RID_C_NM_8505:
+		event = UX500_MUSB_RIDC;
+	case USB_LINK_STD_HOST_NC_8505:
+	case USB_LINK_STD_HOST_C_NS_8505:
+	case USB_LINK_STD_HOST_C_S_8505:
+	case USB_LINK_CDP_8505:
+		if (ab->mode == USB_IDLE) {
+			ab->mode = USB_PERIPHERAL;
+			ab8500_usb_peri_phy_en(ab);
+			atomic_notifier_call_chain(&ab->phy.notifier,
+					UX500_MUSB_PREPARE, &ab->vbus_draw);
+		}
+		if (event != UX500_MUSB_RIDC)
+			event = UX500_MUSB_VBUS;
+		break;
+
+	case USB_LINK_ACA_RID_A_8505:
+	case USB_LINK_ACA_DOCK_CHGR_8505:
+		event = UX500_MUSB_RIDA;
+	case USB_LINK_HM_IDGND_8505:
+		if (ab->mode == USB_IDLE) {
+			ab->mode = USB_HOST;
+			ab8500_usb_host_phy_en(ab);
+			atomic_notifier_call_chain(&ab->phy.notifier,
+					UX500_MUSB_PREPARE, &ab->vbus_draw);
+		}
+		ab->phy.otg->default_a = true;
+		if (event != UX500_MUSB_RIDA)
+			event = UX500_MUSB_ID;
+		atomic_notifier_call_chain(&ab->phy.notifier,
+				event, &ab->vbus_draw);
+		break;
+
+	case USB_LINK_DEDICATED_CHG_8505:
+		ab->mode = USB_DEDICATED_CHG;
+		event = UX500_MUSB_CHARGER;
+		atomic_notifier_call_chain(&ab->phy.notifier,
+				event, &ab->vbus_draw);
+		break;
+
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static int ab8500_usb_link_status_update(struct ab8500_usb *ab,
+		enum ab8500_usb_link_status lsts)
+{
+	enum ux500_musb_vbus_id_status event = 0;
+
+	dev_dbg(ab->dev, "ab8500_usb_link_status_update %d\n", lsts);
+
+	/*
+	 * Spurious link_status interrupts are seen in case of a
+	 * disconnection of a device in IDGND and RIDA stage
+	 */
+	if (ab->previous_link_status_state == USB_LINK_HM_IDGND_8500 &&
+			(lsts == USB_LINK_STD_HOST_C_NS_8500 ||
+			 lsts == USB_LINK_STD_HOST_NC_8500))
+		return 0;
+
+	if (ab->previous_link_status_state == USB_LINK_ACA_RID_A_8500 &&
+			lsts == USB_LINK_STD_HOST_NC_8500)
+		return 0;
+
+	ab->previous_link_status_state = lsts;
+
+	switch (lsts) {
+	case USB_LINK_ACA_RID_B_8500:
+		event = UX500_MUSB_RIDB;
+	case USB_LINK_NOT_CONFIGURED_8500:
+	case USB_LINK_NOT_VALID_LINK_8500:
+		ab->mode = USB_IDLE;
+		ab->phy.otg->default_a = false;
+		ab->vbus_draw = 0;
+		if (event != UX500_MUSB_RIDB)
+			event = UX500_MUSB_NONE;
+		/* Fallback to default B_IDLE as nothing is connected */
+		ab->phy.state = OTG_STATE_B_IDLE;
+		break;
+
+	case USB_LINK_ACA_RID_C_NM_8500:
+	case USB_LINK_ACA_RID_C_HS_8500:
+	case USB_LINK_ACA_RID_C_HS_CHIRP_8500:
+		event = UX500_MUSB_RIDC;
+	case USB_LINK_STD_HOST_NC_8500:
+	case USB_LINK_STD_HOST_C_NS_8500:
+	case USB_LINK_STD_HOST_C_S_8500:
+	case USB_LINK_HOST_CHG_NM_8500:
+	case USB_LINK_HOST_CHG_HS_8500:
+	case USB_LINK_HOST_CHG_HS_CHIRP_8500:
+		if (ab->mode == USB_IDLE) {
+			ab->mode = USB_PERIPHERAL;
+			ab8500_usb_peri_phy_en(ab);
+			atomic_notifier_call_chain(&ab->phy.notifier,
+					UX500_MUSB_PREPARE, &ab->vbus_draw);
+		}
+		if (event != UX500_MUSB_RIDC)
+			event = UX500_MUSB_VBUS;
+		break;
+
+	case USB_LINK_ACA_RID_A_8500:
+		event = UX500_MUSB_RIDA;
+	case USB_LINK_HM_IDGND_8500:
+		if (ab->mode == USB_IDLE) {
+			ab->mode = USB_HOST;
+			ab8500_usb_host_phy_en(ab);
+			atomic_notifier_call_chain(&ab->phy.notifier,
+					UX500_MUSB_PREPARE, &ab->vbus_draw);
+		}
+		ab->phy.otg->default_a = true;
+		if (event != UX500_MUSB_RIDA)
+			event = UX500_MUSB_ID;
+		atomic_notifier_call_chain(&ab->phy.notifier,
+				event, &ab->vbus_draw);
+		break;
+
+	case USB_LINK_DEDICATED_CHG_8500:
+		ab->mode = USB_DEDICATED_CHG;
+		event = UX500_MUSB_CHARGER;
+		atomic_notifier_call_chain(&ab->phy.notifier,
+				event, &ab->vbus_draw);
+		break;
+
+	case USB_LINK_RESERVED_8500:
+		break;
+	}
+
+	return 0;
+}
+
+/*
+ * Connection Sequence:
+ *   1. Link Status Interrupt
+ *   2. Enable AB clock
+ *   3. Enable AB regulators
+ *   4. Enable USB phy
+ *   5. Reset the musb controller
+ *   6. Switch the ULPI GPIO pins to fucntion mode
+ *   7. Enable the musb Peripheral5 clock
+ *   8. Restore MUSB context
+ */
+static int abx500_usb_link_status_update(struct ab8500_usb *ab)
+{
+	u8 reg;
+	int ret = 0;
+
+	if (is_ab8500(ab->ab8500)) {
+		enum ab8500_usb_link_status lsts;
+
+		abx500_get_register_interruptible(ab->dev,
+				AB8500_USB, AB8500_USB_LINE_STAT_REG, &reg);
+		lsts = (reg >> 3) & 0x0F;
+		ret = ab8500_usb_link_status_update(ab, lsts);
+	} else if (is_ab8505(ab->ab8500)) {
+		enum ab8505_usb_link_status lsts;
+
+		abx500_get_register_interruptible(ab->dev,
+				AB8500_USB, AB8505_USB_LINE_STAT_REG, &reg);
+		lsts = (reg >> 3) & 0x1F;
+		ret = ab8505_usb_link_status_update(ab, lsts);
+	}
+
+	return ret;
+}
+
+/*
+ * Disconnection Sequence:
+ *   1. Disconect Interrupt
+ *   2. Disable regulators
+ *   3. Disable AB clock
+ *   4. Disable the Phy
+ *   5. Link Status Interrupt
+ *   6. Disable Musb Clock
+ */
+static irqreturn_t ab8500_usb_disconnect_irq(int irq, void *data)
+{
+	struct ab8500_usb *ab = (struct ab8500_usb *) data;
+	enum usb_phy_events event = UX500_MUSB_NONE;
+
+	/* Link status will not be updated till phy is disabled. */
+	if (ab->mode == USB_HOST) {
+		ab->phy.otg->default_a = false;
+		ab->vbus_draw = 0;
+		atomic_notifier_call_chain(&ab->phy.notifier,
+				event, &ab->vbus_draw);
+		ab8500_usb_host_phy_dis(ab);
+		ab->mode = USB_IDLE;
+	}
+
+	if (ab->mode == USB_PERIPHERAL) {
+		atomic_notifier_call_chain(&ab->phy.notifier,
+				event, &ab->vbus_draw);
+		ab8500_usb_peri_phy_dis(ab);
+		atomic_notifier_call_chain(&ab->phy.notifier,
+				UX500_MUSB_CLEAN, &ab->vbus_draw);
+		ab->mode = USB_IDLE;
+		ab->phy.otg->default_a = false;
+		ab->vbus_draw = 0;
+	}
+
+	if (is_ab8500_2p0(ab->ab8500)) {
+		if (ab->mode == USB_DEDICATED_CHG) {
+			ab8500_usb_wd_linkstatus(ab,
+					AB8500_BIT_PHY_CTRL_DEVICE_EN);
+			abx500_mask_and_set_register_interruptible(ab->dev,
+					AB8500_USB, AB8500_USB_PHY_CTRL_REG,
+					AB8500_BIT_PHY_CTRL_DEVICE_EN, 0);
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t ab8500_usb_link_status_irq(int irq, void *data)
+{
+	struct ab8500_usb *ab = (struct ab8500_usb *) data;
+
+	abx500_usb_link_status_update(ab);
+
+	return IRQ_HANDLED;
+}
+
+static void ab8500_usb_phy_disable_work(struct work_struct *work)
+{
+	struct ab8500_usb *ab = container_of(work, struct ab8500_usb,
+						phy_dis_work);
+
+	if (!ab->phy.otg->host)
+		ab8500_usb_host_phy_dis(ab);
+
+	if (!ab->phy.otg->gadget)
+		ab8500_usb_peri_phy_dis(ab);
+}
+
+static unsigned ab8500_eyediagram_workaroud(struct ab8500_usb *ab, unsigned mA)
+{
+	/*
+	 * AB8500 V2 has eye diagram issues when drawing more than 100mA from
+	 * VBUS.  Set charging current to 100mA in case of standard host
+	 */
+	if (is_ab8500_2p0_or_earlier(ab->ab8500))
+		if (mA > 100)
+			mA = 100;
+
+	return mA;
+}
+
+static int ab8500_usb_set_power(struct usb_phy *phy, unsigned mA)
+{
+	struct ab8500_usb *ab;
+
+	if (!phy)
+		return -ENODEV;
+
+	ab = phy_to_ab(phy);
+
+	mA = ab8500_eyediagram_workaroud(ab, mA);
+
+	ab->vbus_draw = mA;
+
+	atomic_notifier_call_chain(&ab->phy.notifier,
+			UX500_MUSB_VBUS, &ab->vbus_draw);
+
+	return 0;
+}
+
+static int ab8500_usb_set_suspend(struct usb_phy *x, int suspend)
+{
+	/* TODO */
+	return 0;
+}
+
+static int ab8500_usb_set_peripheral(struct usb_otg *otg,
+					struct usb_gadget *gadget)
+{
+	struct ab8500_usb *ab;
+
+	if (!otg)
+		return -ENODEV;
+
+	ab = phy_to_ab(otg->phy);
+
+	ab->phy.otg->gadget = gadget;
+
+	/* Some drivers call this function in atomic context.
+	 * Do not update ab8500 registers directly till this
+	 * is fixed.
+	 */
+
+	if ((ab->mode != USB_IDLE) && (!gadget)) {
+		ab->mode = USB_IDLE;
+		schedule_work(&ab->phy_dis_work);
+	}
+
+	return 0;
+}
+
+static int ab8500_usb_set_host(struct usb_otg *otg, struct usb_bus *host)
+{
+	struct ab8500_usb *ab;
+
+	if (!otg)
+		return -ENODEV;
+
+	ab = phy_to_ab(otg->phy);
+
+	ab->phy.otg->host = host;
+
+	/* Some drivers call this function in atomic context.
+	 * Do not update ab8500 registers directly till this
+	 * is fixed.
+	 */
+
+	if ((ab->mode != USB_IDLE) && (!host)) {
+		ab->mode = USB_IDLE;
+		schedule_work(&ab->phy_dis_work);
+	}
+
+	return 0;
+}
+
+static int ab8500_usb_regulator_get(struct ab8500_usb *ab)
+{
+	int err;
+
+	ab->v_ape = devm_regulator_get(ab->dev, "v-ape");
+	if (IS_ERR(ab->v_ape)) {
+		dev_err(ab->dev, "Could not get v-ape supply\n");
+		err = PTR_ERR(ab->v_ape);
+		return err;
+	}
+
+	ab->v_ulpi = devm_regulator_get(ab->dev, "vddulpivio18");
+	if (IS_ERR(ab->v_ulpi)) {
+		dev_err(ab->dev, "Could not get vddulpivio18 supply\n");
+		err = PTR_ERR(ab->v_ulpi);
+		return err;
+	}
+
+	ab->v_musb = devm_regulator_get(ab->dev, "musb_1v8");
+	if (IS_ERR(ab->v_musb)) {
+		dev_err(ab->dev, "Could not get musb_1v8 supply\n");
+		err = PTR_ERR(ab->v_musb);
+		return err;
+	}
+
+	return 0;
+}
+
+static int ab8500_usb_irq_setup(struct platform_device *pdev,
+		struct ab8500_usb *ab)
+{
+	int err;
+	int irq;
+
+	irq = platform_get_irq_byname(pdev, "USB_LINK_STATUS");
+	if (irq < 0) {
+		dev_err(&pdev->dev, "Link status irq not found\n");
+		return irq;
+	}
+	err = devm_request_threaded_irq(&pdev->dev, irq, NULL,
+			ab8500_usb_link_status_irq,
+			IRQF_NO_SUSPEND | IRQF_SHARED, "usb-link-status", ab);
+	if (err < 0) {
+		dev_err(ab->dev, "request_irq failed for link status irq\n");
+		return err;
+	}
+
+	irq = platform_get_irq_byname(pdev, "ID_WAKEUP_F");
+	if (irq < 0) {
+		dev_err(&pdev->dev, "ID fall irq not found\n");
+		return irq;
+	}
+	err = devm_request_threaded_irq(&pdev->dev, irq, NULL,
+			ab8500_usb_disconnect_irq,
+			IRQF_NO_SUSPEND | IRQF_SHARED, "usb-id-fall", ab);
+	if (err < 0) {
+		dev_err(ab->dev, "request_irq failed for ID fall irq\n");
+		return err;
+	}
+
+	irq = platform_get_irq_byname(pdev, "VBUS_DET_F");
+	if (irq < 0) {
+		dev_err(&pdev->dev, "VBUS fall irq not found\n");
+		return irq;
+	}
+	err = devm_request_threaded_irq(&pdev->dev, irq, NULL,
+			ab8500_usb_disconnect_irq,
+			IRQF_NO_SUSPEND | IRQF_SHARED, "usb-vbus-fall", ab);
+	if (err < 0) {
+		dev_err(ab->dev, "request_irq failed for Vbus fall irq\n");
+		return err;
+	}
+
+	return 0;
+}
+
+static int ab8500_usb_probe(struct platform_device *pdev)
+{
+	struct ab8500_usb	*ab;
+	struct ab8500		*ab8500;
+	struct usb_otg		*otg;
+	int err;
+	int rev;
+
+	ab8500 = dev_get_drvdata(pdev->dev.parent);
+	rev = abx500_get_chip_id(&pdev->dev);
+
+	if (is_ab8500_1p1_or_earlier(ab8500)) {
+		dev_err(&pdev->dev, "Unsupported AB8500 chip rev=%d\n", rev);
+		return -ENODEV;
+	}
+
+	ab = devm_kzalloc(&pdev->dev, sizeof(*ab), GFP_KERNEL);
+	if (!ab)
+		return -ENOMEM;
+
+	otg = devm_kzalloc(&pdev->dev, sizeof(*otg), GFP_KERNEL);
+	if (!otg)
+		return -ENOMEM;
+
+	ab->dev			= &pdev->dev;
+	ab->ab8500		= ab8500;
+	ab->phy.dev		= ab->dev;
+	ab->phy.otg		= otg;
+	ab->phy.label		= "ab8500";
+	ab->phy.set_suspend	= ab8500_usb_set_suspend;
+	ab->phy.set_power	= ab8500_usb_set_power;
+	ab->phy.state		= OTG_STATE_UNDEFINED;
+
+	otg->phy		= &ab->phy;
+	otg->set_host		= ab8500_usb_set_host;
+	otg->set_peripheral	= ab8500_usb_set_peripheral;
+
+	platform_set_drvdata(pdev, ab);
+
+	ATOMIC_INIT_NOTIFIER_HEAD(&ab->phy.notifier);
+
+	/* all: Disable phy when called from set_host and set_peripheral */
+	INIT_WORK(&ab->phy_dis_work, ab8500_usb_phy_disable_work);
+
+	err = ab8500_usb_regulator_get(ab);
+	if (err)
+		return err;
+
+	err = ab8500_usb_irq_setup(pdev, ab);
+	if (err < 0)
+		return err;
+
+	err = usb_add_phy(&ab->phy, USB_PHY_TYPE_USB2);
+	if (err) {
+		dev_err(&pdev->dev, "Can't register transceiver\n");
+		return err;
+	}
+
+	/* Phy tuning values for AB8500 */
+	if (!is_ab8500_2p0_or_earlier(ab->ab8500)) {
+		/* Enable the PBT/Bank 0x12 access */
+		err = abx500_set_register_interruptible(ab->dev,
+				AB8500_DEVELOPMENT, AB8500_BANK12_ACCESS, 0x01);
+		if (err < 0)
+			dev_err(ab->dev, "Failed to enable bank12 access err=%d\n",
+					err);
+
+		err = abx500_set_register_interruptible(ab->dev,
+				AB8500_DEBUG, AB8500_USB_PHY_TUNE1, 0xC8);
+		if (err < 0)
+			dev_err(ab->dev, "Failed to set PHY_TUNE1 register err=%d\n",
+					err);
+
+		err = abx500_set_register_interruptible(ab->dev,
+				AB8500_DEBUG, AB8500_USB_PHY_TUNE2, 0x00);
+		if (err < 0)
+			dev_err(ab->dev, "Failed to set PHY_TUNE2 register err=%d\n",
+					err);
+
+		err = abx500_set_register_interruptible(ab->dev,
+				AB8500_DEBUG, AB8500_USB_PHY_TUNE3, 0x78);
+		if (err < 0)
+			dev_err(ab->dev, "Failed to set PHY_TUNE3 regester err=%d\n",
+					err);
+
+		/* Switch to normal mode/disable Bank 0x12 access */
+		err = abx500_set_register_interruptible(ab->dev,
+				AB8500_DEVELOPMENT, AB8500_BANK12_ACCESS, 0x00);
+		if (err < 0)
+			dev_err(ab->dev, "Failed to switch bank12 access err=%d\n",
+					err);
+	}
+
+	/* Phy tuning values for AB8505 */
+	if (is_ab8505(ab->ab8500)) {
+		/* Enable the PBT/Bank 0x12 access */
+		err = abx500_mask_and_set_register_interruptible(ab->dev,
+				AB8500_DEVELOPMENT, AB8500_BANK12_ACCESS,
+				0x01, 0x01);
+		if (err < 0)
+			dev_err(ab->dev, "Failed to enable bank12 access err=%d\n",
+					err);
+
+		err = abx500_mask_and_set_register_interruptible(ab->dev,
+				AB8500_DEBUG, AB8500_USB_PHY_TUNE1,
+				0xC8, 0xC8);
+		if (err < 0)
+			dev_err(ab->dev, "Failed to set PHY_TUNE1 register err=%d\n",
+					err);
+
+		err = abx500_mask_and_set_register_interruptible(ab->dev,
+				AB8500_DEBUG, AB8500_USB_PHY_TUNE2,
+				0x60, 0x60);
+		if (err < 0)
+			dev_err(ab->dev, "Failed to set PHY_TUNE2 register err=%d\n",
+					err);
+
+		err = abx500_mask_and_set_register_interruptible(ab->dev,
+				AB8500_DEBUG, AB8500_USB_PHY_TUNE3,
+				0xFC, 0x80);
+
+		if (err < 0)
+			dev_err(ab->dev, "Failed to set PHY_TUNE3 regester err=%d\n",
+					err);
+
+		/* Switch to normal mode/disable Bank 0x12 access */
+		err = abx500_mask_and_set_register_interruptible(ab->dev,
+				AB8500_DEVELOPMENT, AB8500_BANK12_ACCESS,
+				0x00, 0x00);
+		if (err < 0)
+			dev_err(ab->dev, "Failed to switch bank12 access err=%d\n",
+					err);
+	}
+
+	/* Needed to enable ID detection. */
+	ab8500_usb_wd_workaround(ab);
+
+	abx500_usb_link_status_update(ab);
+
+	dev_info(&pdev->dev, "revision 0x%2x driver initialized\n", rev);
+
+	return 0;
+}
+
+static int ab8500_usb_remove(struct platform_device *pdev)
+{
+	struct ab8500_usb *ab = platform_get_drvdata(pdev);
+
+	cancel_work_sync(&ab->phy_dis_work);
+
+	usb_remove_phy(&ab->phy);
+
+	if (ab->mode == USB_HOST)
+		ab8500_usb_host_phy_dis(ab);
+	else if (ab->mode == USB_PERIPHERAL)
+		ab8500_usb_peri_phy_dis(ab);
+
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static struct platform_driver ab8500_usb_driver = {
+	.probe		= ab8500_usb_probe,
+	.remove		= ab8500_usb_remove,
+	.driver		= {
+		.name	= "ab8500-usb",
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init ab8500_usb_init(void)
+{
+	return platform_driver_register(&ab8500_usb_driver);
+}
+subsys_initcall(ab8500_usb_init);
+
+static void __exit ab8500_usb_exit(void)
+{
+	platform_driver_unregister(&ab8500_usb_driver);
+}
+module_exit(ab8500_usb_exit);
+
+MODULE_ALIAS("platform:ab8500_usb");
+MODULE_AUTHOR("ST-Ericsson AB");
+MODULE_DESCRIPTION("AB8500 usb transceiver driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/otg/fsl_otg.c b/drivers/usb/phy/phy-fsl-usb.c
index d16adb41eb2..97b9308507c 100644
--- a/drivers/usb/otg/fsl_otg.c
+++ b/drivers/usb/phy/phy-fsl-usb.c
@@ -43,7 +43,7 @@
 
 #include <asm/unaligned.h>
 
-#include "fsl_otg.h"
+#include "phy-fsl-usb.h"
 
 #define DRIVER_VERSION "Rev. 1.55"
 #define DRIVER_AUTHOR "Jerry Huang/Li Yang"
@@ -361,28 +361,18 @@ int fsl_otg_init_timers(struct otg_fsm *fsm)
 void fsl_otg_uninit_timers(void)
 {
 	/* FSM used timers */
-	if (a_wait_vrise_tmr != NULL)
-		kfree(a_wait_vrise_tmr);
-	if (a_wait_bcon_tmr != NULL)
-		kfree(a_wait_bcon_tmr);
-	if (a_aidl_bdis_tmr != NULL)
-		kfree(a_aidl_bdis_tmr);
-	if (b_ase0_brst_tmr != NULL)
-		kfree(b_ase0_brst_tmr);
-	if (b_se0_srp_tmr != NULL)
-		kfree(b_se0_srp_tmr);
-	if (b_srp_fail_tmr != NULL)
-		kfree(b_srp_fail_tmr);
-	if (a_wait_enum_tmr != NULL)
-		kfree(a_wait_enum_tmr);
+	kfree(a_wait_vrise_tmr);
+	kfree(a_wait_bcon_tmr);
+	kfree(a_aidl_bdis_tmr);
+	kfree(b_ase0_brst_tmr);
+	kfree(b_se0_srp_tmr);
+	kfree(b_srp_fail_tmr);
+	kfree(a_wait_enum_tmr);
 
 	/* device driver used timers */
-	if (b_srp_wait_tmr != NULL)
-		kfree(b_srp_wait_tmr);
-	if (b_data_pulse_tmr != NULL)
-		kfree(b_data_pulse_tmr);
-	if (b_vbus_pulse_tmr != NULL)
-		kfree(b_vbus_pulse_tmr);
+	kfree(b_srp_wait_tmr);
+	kfree(b_data_pulse_tmr);
+	kfree(b_vbus_pulse_tmr);
 }
 
 /* Add timer to timer list */
@@ -1002,7 +992,7 @@ static int show_fsl_usb2_otg_state(struct device *dev,
 	/* State */
 	t = scnprintf(next, size,
 		      "OTG state: %s\n\n",
-		      otg_state_string(fsl_otg_dev->phy.state));
+		      usb_otg_state_string(fsl_otg_dev->phy.state));
 	size -= t;
 	next += t;
 
diff --git a/drivers/usb/otg/fsl_otg.h b/drivers/usb/phy/phy-fsl-usb.h
index ca266280895..ca266280895 100644
--- a/drivers/usb/otg/fsl_otg.h
+++ b/drivers/usb/phy/phy-fsl-usb.h
diff --git a/drivers/usb/otg/otg_fsm.c b/drivers/usb/phy/phy-fsm-usb.c
index ade131a8ae5..c520b3548e7 100644
--- a/drivers/usb/otg/otg_fsm.c
+++ b/drivers/usb/phy/phy-fsm-usb.c
@@ -29,7 +29,7 @@
 #include <linux/usb/gadget.h>
 #include <linux/usb/otg.h>
 
-#include "otg_fsm.h"
+#include "phy-otg-fsm.h"
 
 /* Change USB protocol when there is a protocol change */
 static int otg_set_protocol(struct otg_fsm *fsm, int protocol)
@@ -119,7 +119,7 @@ int otg_set_state(struct otg_fsm *fsm, enum usb_otg_state new_state)
 	state_changed = 1;
 	if (fsm->otg->phy->state == new_state)
 		return 0;
-	VDBG("Set state: %s\n", otg_state_string(new_state));
+	VDBG("Set state: %s\n", usb_otg_state_string(new_state));
 	otg_leave_state(fsm, fsm->otg->phy->state);
 	switch (new_state) {
 	case OTG_STATE_B_IDLE:
diff --git a/drivers/usb/otg/otg_fsm.h b/drivers/usb/phy/phy-fsm-usb.h
index c30a2e1d9e4..c30a2e1d9e4 100644
--- a/drivers/usb/otg/otg_fsm.h
+++ b/drivers/usb/phy/phy-fsm-usb.h
diff --git a/drivers/usb/otg/gpio_vbus.c b/drivers/usb/phy/phy-gpio-vbus-usb.c
index a7d4ac59198..4c76074e518 100644
--- a/drivers/usb/otg/gpio_vbus.c
+++ b/drivers/usb/phy/phy-gpio-vbus-usb.c
@@ -61,6 +61,7 @@ static void set_vbus_draw(struct gpio_vbus_data *gpio_vbus, unsigned mA)
 {
 	struct regulator *vbus_draw = gpio_vbus->vbus_draw;
 	int enabled;
+	int ret;
 
 	if (!vbus_draw)
 		return;
@@ -69,12 +70,16 @@ static void set_vbus_draw(struct gpio_vbus_data *gpio_vbus, unsigned mA)
 	if (mA) {
 		regulator_set_current_limit(vbus_draw, 0, 1000 * mA);
 		if (!enabled) {
-			regulator_enable(vbus_draw);
+			ret = regulator_enable(vbus_draw);
+			if (ret < 0)
+				return;
 			gpio_vbus->vbus_draw_enabled = 1;
 		}
 	} else {
 		if (enabled) {
-			regulator_disable(vbus_draw);
+			ret = regulator_disable(vbus_draw);
+			if (ret < 0)
+				return;
 			gpio_vbus->vbus_draw_enabled = 0;
 		}
 	}
diff --git a/drivers/usb/otg/isp1301_omap.c b/drivers/usb/phy/phy-isp1301-omap.c
index af9cb11626b..ae481afcb3e 100644
--- a/drivers/usb/otg/isp1301_omap.c
+++ b/drivers/usb/phy/phy-isp1301-omap.c
@@ -236,7 +236,7 @@ isp1301_clear_bits(struct isp1301 *isp, u8 reg, u8 bits)
 
 static inline const char *state_name(struct isp1301 *isp)
 {
-	return otg_state_string(isp->phy.state);
+	return usb_otg_state_string(isp->phy.state);
 }
 
 /*-------------------------------------------------------------------------*/
@@ -481,7 +481,7 @@ static void check_state(struct isp1301 *isp, const char *tag)
 	if (isp->phy.state == state && !extra)
 		return;
 	pr_debug("otg: %s FSM %s/%02x, %s, %06x\n", tag,
-		otg_state_string(state), fsm, state_name(isp),
+		usb_otg_state_string(state), fsm, state_name(isp),
 		omap_readl(OTG_CTRL));
 }
 
@@ -1077,7 +1077,7 @@ static void isp_update_otg(struct isp1301 *isp, u8 stat)
 
 	if (state != isp->phy.state)
 		pr_debug("  isp, %s -> %s\n",
-				otg_state_string(state), state_name(isp));
+				usb_otg_state_string(state), state_name(isp));
 
 #ifdef	CONFIG_USB_OTG
 	/* update the OTG controller state to match the isp1301; may
@@ -1212,7 +1212,7 @@ static void isp1301_release(struct device *dev)
 
 static struct isp1301 *the_transceiver;
 
-static int __exit isp1301_remove(struct i2c_client *i2c)
+static int isp1301_remove(struct i2c_client *i2c)
 {
 	struct isp1301	*isp;
 
@@ -1634,7 +1634,7 @@ static struct i2c_driver isp1301_driver = {
 		.name	= "isp1301_omap",
 	},
 	.probe		= isp1301_probe,
-	.remove		= __exit_p(isp1301_remove),
+	.remove		= isp1301_remove,
 	.id_table	= isp1301_id,
 };
 
diff --git a/drivers/usb/phy/phy-isp1301.c b/drivers/usb/phy/phy-isp1301.c
new file mode 100644
index 00000000000..225ae6c97ee
--- /dev/null
+++ b/drivers/usb/phy/phy-isp1301.c
@@ -0,0 +1,162 @@
+/*
+ * NXP ISP1301 USB transceiver driver
+ *
+ * Copyright (C) 2012 Roland Stigge
+ *
+ * Author: Roland Stigge <stigge@antcom.de>
+ *
+ * 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/module.h>
+#include <linux/mutex.h>
+#include <linux/i2c.h>
+#include <linux/usb/phy.h>
+#include <linux/usb/isp1301.h>
+
+#define DRV_NAME		"isp1301"
+
+struct isp1301 {
+	struct usb_phy		phy;
+	struct mutex		mutex;
+
+	struct i2c_client	*client;
+};
+
+#define phy_to_isp(p)		(container_of((p), struct isp1301, phy))
+
+static const struct i2c_device_id isp1301_id[] = {
+	{ "isp1301", 0 },
+	{ }
+};
+
+static struct i2c_client *isp1301_i2c_client;
+
+static int __isp1301_write(struct isp1301 *isp, u8 reg, u8 value, u8 clear)
+{
+	return i2c_smbus_write_byte_data(isp->client, reg | clear, value);
+}
+
+static int isp1301_write(struct isp1301 *isp, u8 reg, u8 value)
+{
+	return __isp1301_write(isp, reg, value, 0);
+}
+
+static int isp1301_clear(struct isp1301 *isp, u8 reg, u8 value)
+{
+	return __isp1301_write(isp, reg, value, ISP1301_I2C_REG_CLEAR_ADDR);
+}
+
+static int isp1301_phy_init(struct usb_phy *phy)
+{
+	struct isp1301 *isp = phy_to_isp(phy);
+
+	/* Disable transparent UART mode first */
+	isp1301_clear(isp, ISP1301_I2C_MODE_CONTROL_1, MC1_UART_EN);
+	isp1301_clear(isp, ISP1301_I2C_MODE_CONTROL_1, ~MC1_SPEED_REG);
+	isp1301_write(isp, ISP1301_I2C_MODE_CONTROL_1, MC1_SPEED_REG);
+	isp1301_clear(isp, ISP1301_I2C_MODE_CONTROL_2, ~0);
+	isp1301_write(isp, ISP1301_I2C_MODE_CONTROL_2, (MC2_BI_DI | MC2_PSW_EN
+				| MC2_SPD_SUSP_CTRL));
+
+	isp1301_clear(isp, ISP1301_I2C_OTG_CONTROL_1, ~0);
+	isp1301_write(isp, ISP1301_I2C_MODE_CONTROL_1, MC1_DAT_SE0);
+	isp1301_write(isp, ISP1301_I2C_OTG_CONTROL_1, (OTG1_DM_PULLDOWN
+				| OTG1_DP_PULLDOWN));
+	isp1301_clear(isp, ISP1301_I2C_OTG_CONTROL_1, (OTG1_DM_PULLUP
+				| OTG1_DP_PULLUP));
+
+	/* mask all interrupts */
+	isp1301_clear(isp, ISP1301_I2C_INTERRUPT_LATCH, ~0);
+	isp1301_clear(isp, ISP1301_I2C_INTERRUPT_FALLING, ~0);
+	isp1301_clear(isp, ISP1301_I2C_INTERRUPT_RISING, ~0);
+
+	return 0;
+}
+
+static int isp1301_phy_set_vbus(struct usb_phy *phy, int on)
+{
+	struct isp1301 *isp = phy_to_isp(phy);
+
+	if (on)
+		isp1301_write(isp, ISP1301_I2C_OTG_CONTROL_1, OTG1_VBUS_DRV);
+	else
+		isp1301_clear(isp, ISP1301_I2C_OTG_CONTROL_1, OTG1_VBUS_DRV);
+
+	return 0;
+}
+
+static int isp1301_probe(struct i2c_client *client,
+			 const struct i2c_device_id *i2c_id)
+{
+	struct isp1301 *isp;
+	struct usb_phy *phy;
+
+	isp = devm_kzalloc(&client->dev, sizeof(*isp), GFP_KERNEL);
+	if (!isp)
+		return -ENOMEM;
+
+	isp->client = client;
+	mutex_init(&isp->mutex);
+
+	phy = &isp->phy;
+	phy->label = DRV_NAME;
+	phy->init = isp1301_phy_init;
+	phy->set_vbus = isp1301_phy_set_vbus;
+	phy->type = USB_PHY_TYPE_USB2;
+
+	i2c_set_clientdata(client, isp);
+	usb_add_phy_dev(phy);
+
+	isp1301_i2c_client = client;
+
+	return 0;
+}
+
+static int isp1301_remove(struct i2c_client *client)
+{
+	struct isp1301 *isp = i2c_get_clientdata(client);
+
+	usb_remove_phy(&isp->phy);
+	isp1301_i2c_client = NULL;
+
+	return 0;
+}
+
+static struct i2c_driver isp1301_driver = {
+	.driver = {
+		.name = DRV_NAME,
+	},
+	.probe = isp1301_probe,
+	.remove = isp1301_remove,
+	.id_table = isp1301_id,
+};
+
+module_i2c_driver(isp1301_driver);
+
+static int match(struct device *dev, void *data)
+{
+	struct device_node *node = (struct device_node *)data;
+	return (dev->of_node == node) &&
+		(dev->driver == &isp1301_driver.driver);
+}
+
+struct i2c_client *isp1301_get_client(struct device_node *node)
+{
+	if (node) { /* reference of ISP1301 I2C node via DT */
+		struct device *dev = bus_find_device(&i2c_bus_type, NULL,
+						     node, match);
+		if (!dev)
+			return NULL;
+		return to_i2c_client(dev);
+	} else { /* non-DT: only one ISP1301 chip supported */
+		return isp1301_i2c_client;
+	}
+}
+EXPORT_SYMBOL_GPL(isp1301_get_client);
+
+MODULE_AUTHOR("Roland Stigge <stigge@antcom.de>");
+MODULE_DESCRIPTION("NXP ISP1301 USB transceiver driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/otg/msm_otg.c b/drivers/usb/phy/phy-msm-usb.c
index 749fbf41fb6..749fbf41fb6 100644
--- a/drivers/usb/otg/msm_otg.c
+++ b/drivers/usb/phy/phy-msm-usb.c
diff --git a/drivers/usb/phy/mv_u3d_phy.c b/drivers/usb/phy/phy-mv-u3d-usb.c
index 9d8599122aa..f7838a43347 100644
--- a/drivers/usb/phy/mv_u3d_phy.c
+++ b/drivers/usb/phy/phy-mv-u3d-usb.c
@@ -15,7 +15,7 @@
 #include <linux/usb/otg.h>
 #include <linux/platform_data/mv_usb.h>
 
-#include "mv_u3d_phy.h"
+#include "phy-mv-u3d-usb.h"
 
 /*
  * struct mv_u3d_phy - transceiver driver state
@@ -313,7 +313,7 @@ err:
 	return ret;
 }
 
-static int __exit mv_u3d_phy_remove(struct platform_device *pdev)
+static int mv_u3d_phy_remove(struct platform_device *pdev)
 {
 	struct mv_u3d_phy *mv_u3d_phy = platform_get_drvdata(pdev);
 
diff --git a/drivers/usb/phy/mv_u3d_phy.h b/drivers/usb/phy/phy-mv-u3d-usb.h
index 2a658cb9a52..2a658cb9a52 100644
--- a/drivers/usb/phy/mv_u3d_phy.h
+++ b/drivers/usb/phy/phy-mv-u3d-usb.h
diff --git a/drivers/usb/otg/mv_otg.c b/drivers/usb/phy/phy-mv-usb.c
index b6a9be31133..c987bbe2785 100644
--- a/drivers/usb/otg/mv_otg.c
+++ b/drivers/usb/phy/phy-mv-usb.c
@@ -27,7 +27,7 @@
 #include <linux/usb/hcd.h>
 #include <linux/platform_data/mv_usb.h>
 
-#include "mv_otg.h"
+#include "phy-mv-usb.h"
 
 #define	DRIVER_DESC	"Marvell USB OTG transceiver driver"
 #define	DRIVER_VERSION	"Jan 20, 2010"
@@ -237,18 +237,12 @@ static void mv_otg_start_periphrals(struct mv_otg *mvotg, int on)
 
 static void otg_clock_enable(struct mv_otg *mvotg)
 {
-	unsigned int i;
-
-	for (i = 0; i < mvotg->clknum; i++)
-		clk_prepare_enable(mvotg->clk[i]);
+	clk_prepare_enable(mvotg->clk);
 }
 
 static void otg_clock_disable(struct mv_otg *mvotg)
 {
-	unsigned int i;
-
-	for (i = 0; i < mvotg->clknum; i++)
-		clk_disable_unprepare(mvotg->clk[i]);
+	clk_disable_unprepare(mvotg->clk);
 }
 
 static int mv_otg_enable_internal(struct mv_otg *mvotg)
@@ -684,16 +678,14 @@ static int mv_otg_probe(struct platform_device *pdev)
 	struct mv_otg *mvotg;
 	struct usb_otg *otg;
 	struct resource *r;
-	int retval = 0, clk_i, i;
-	size_t size;
+	int retval = 0, i;
 
 	if (pdata == NULL) {
 		dev_err(&pdev->dev, "failed to get platform data\n");
 		return -ENODEV;
 	}
 
-	size = sizeof(*mvotg) + sizeof(struct clk *) * pdata->clknum;
-	mvotg = devm_kzalloc(&pdev->dev, size, GFP_KERNEL);
+	mvotg = devm_kzalloc(&pdev->dev, sizeof(*mvotg), GFP_KERNEL);
 	if (!mvotg) {
 		dev_err(&pdev->dev, "failed to allocate memory!\n");
 		return -ENOMEM;
@@ -708,15 +700,9 @@ static int mv_otg_probe(struct platform_device *pdev)
 	mvotg->pdev = pdev;
 	mvotg->pdata = pdata;
 
-	mvotg->clknum = pdata->clknum;
-	for (clk_i = 0; clk_i < mvotg->clknum; clk_i++) {
-		mvotg->clk[clk_i] = devm_clk_get(&pdev->dev,
-						pdata->clkname[clk_i]);
-		if (IS_ERR(mvotg->clk[clk_i])) {
-			retval = PTR_ERR(mvotg->clk[clk_i]);
-			return retval;
-		}
-	}
+	mvotg->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(mvotg->clk))
+		return PTR_ERR(mvotg->clk);
 
 	mvotg->qwork = create_singlethread_workqueue("mv_otg_queue");
 	if (!mvotg->qwork) {
diff --git a/drivers/usb/otg/mv_otg.h b/drivers/usb/phy/phy-mv-usb.h
index 8a9e351b36b..551da6eb0ba 100644
--- a/drivers/usb/otg/mv_otg.h
+++ b/drivers/usb/phy/phy-mv-usb.h
@@ -158,8 +158,7 @@ struct mv_otg {
 
 	unsigned int active;
 	unsigned int clock_gating;
-	unsigned int clknum;
-	struct clk *clk[0];
+	struct clk *clk;
 };
 
 #endif
diff --git a/drivers/usb/otg/mxs-phy.c b/drivers/usb/phy/phy-mxs-usb.c
index b0d9f119c74..9d4381e64d5 100644
--- a/drivers/usb/otg/mxs-phy.c
+++ b/drivers/usb/phy/phy-mxs-usb.c
@@ -48,12 +48,12 @@ static void mxs_phy_hw_init(struct mxs_phy *mxs_phy)
 	stmp_reset_block(base + HW_USBPHY_CTRL);
 
 	/* Power up the PHY */
-	writel_relaxed(0, base + HW_USBPHY_PWD);
+	writel(0, base + HW_USBPHY_PWD);
 
 	/* enable FS/LS device */
-	writel_relaxed(BM_USBPHY_CTRL_ENUTMILEVEL2 |
-			BM_USBPHY_CTRL_ENUTMILEVEL3,
-			base + HW_USBPHY_CTRL_SET);
+	writel(BM_USBPHY_CTRL_ENUTMILEVEL2 |
+	       BM_USBPHY_CTRL_ENUTMILEVEL3,
+	       base + HW_USBPHY_CTRL_SET);
 }
 
 static int mxs_phy_init(struct usb_phy *phy)
@@ -70,8 +70,8 @@ static void mxs_phy_shutdown(struct usb_phy *phy)
 {
 	struct mxs_phy *mxs_phy = to_mxs_phy(phy);
 
-	writel_relaxed(BM_USBPHY_CTRL_CLKGATE,
-			phy->io_priv + HW_USBPHY_CTRL_SET);
+	writel(BM_USBPHY_CTRL_CLKGATE,
+	       phy->io_priv + HW_USBPHY_CTRL_SET);
 
 	clk_disable_unprepare(mxs_phy->clk);
 }
@@ -81,15 +81,15 @@ static int mxs_phy_suspend(struct usb_phy *x, int suspend)
 	struct mxs_phy *mxs_phy = to_mxs_phy(x);
 
 	if (suspend) {
-		writel_relaxed(0xffffffff, x->io_priv + HW_USBPHY_PWD);
-		writel_relaxed(BM_USBPHY_CTRL_CLKGATE,
-			x->io_priv + HW_USBPHY_CTRL_SET);
+		writel(0xffffffff, x->io_priv + HW_USBPHY_PWD);
+		writel(BM_USBPHY_CTRL_CLKGATE,
+		       x->io_priv + HW_USBPHY_CTRL_SET);
 		clk_disable_unprepare(mxs_phy->clk);
 	} else {
 		clk_prepare_enable(mxs_phy->clk);
-		writel_relaxed(BM_USBPHY_CTRL_CLKGATE,
-			x->io_priv + HW_USBPHY_CTRL_CLR);
-		writel_relaxed(0, x->io_priv + HW_USBPHY_PWD);
+		writel(BM_USBPHY_CTRL_CLKGATE,
+		       x->io_priv + HW_USBPHY_CTRL_CLR);
+		writel(0, x->io_priv + HW_USBPHY_PWD);
 	}
 
 	return 0;
@@ -102,8 +102,8 @@ static int mxs_phy_on_connect(struct usb_phy *phy,
 		(speed == USB_SPEED_HIGH) ? "high" : "non-high");
 
 	if (speed == USB_SPEED_HIGH)
-		writel_relaxed(BM_USBPHY_CTRL_ENHOSTDISCONDETECT,
-				phy->io_priv + HW_USBPHY_CTRL_SET);
+		writel(BM_USBPHY_CTRL_ENHOSTDISCONDETECT,
+		       phy->io_priv + HW_USBPHY_CTRL_SET);
 
 	return 0;
 }
@@ -115,8 +115,8 @@ static int mxs_phy_on_disconnect(struct usb_phy *phy,
 		(speed == USB_SPEED_HIGH) ? "high" : "non-high");
 
 	if (speed == USB_SPEED_HIGH)
-		writel_relaxed(BM_USBPHY_CTRL_ENHOSTDISCONDETECT,
-				phy->io_priv + HW_USBPHY_CTRL_CLR);
+		writel(BM_USBPHY_CTRL_ENHOSTDISCONDETECT,
+		       phy->io_priv + HW_USBPHY_CTRL_CLR);
 
 	return 0;
 }
@@ -127,6 +127,7 @@ static int mxs_phy_probe(struct platform_device *pdev)
 	void __iomem *base;
 	struct clk *clk;
 	struct mxs_phy *mxs_phy;
+	int ret;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
@@ -166,11 +167,19 @@ static int mxs_phy_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, &mxs_phy->phy);
 
+	ret = usb_add_phy_dev(&mxs_phy->phy);
+	if (ret)
+		return ret;
+
 	return 0;
 }
 
 static int mxs_phy_remove(struct platform_device *pdev)
 {
+	struct mxs_phy *mxs_phy = platform_get_drvdata(pdev);
+
+	usb_remove_phy(&mxs_phy->phy);
+
 	platform_set_drvdata(pdev, NULL);
 
 	return 0;
diff --git a/drivers/usb/otg/nop-usb-xceiv.c b/drivers/usb/phy/phy-nop.c
index a3ce24b94a7..2b10cc969bb 100644
--- a/drivers/usb/otg/nop-usb-xceiv.c
+++ b/drivers/usb/phy/phy-nop.c
@@ -32,10 +32,16 @@
 #include <linux/usb/otg.h>
 #include <linux/usb/nop-usb-xceiv.h>
 #include <linux/slab.h>
+#include <linux/clk.h>
+#include <linux/regulator/consumer.h>
+#include <linux/of.h>
 
 struct nop_usb_xceiv {
-	struct usb_phy		phy;
-	struct device		*dev;
+	struct usb_phy phy;
+	struct device *dev;
+	struct clk *clk;
+	struct regulator *vcc;
+	struct regulator *reset;
 };
 
 static struct platform_device *pd;
@@ -64,6 +70,46 @@ static int nop_set_suspend(struct usb_phy *x, int suspend)
 	return 0;
 }
 
+static int nop_init(struct usb_phy *phy)
+{
+	struct nop_usb_xceiv *nop = dev_get_drvdata(phy->dev);
+
+	if (!IS_ERR(nop->vcc)) {
+		if (regulator_enable(nop->vcc))
+			dev_err(phy->dev, "Failed to enable power\n");
+	}
+
+	if (!IS_ERR(nop->clk))
+		clk_enable(nop->clk);
+
+	if (!IS_ERR(nop->reset)) {
+		/* De-assert RESET */
+		if (regulator_enable(nop->reset))
+			dev_err(phy->dev, "Failed to de-assert reset\n");
+	}
+
+	return 0;
+}
+
+static void nop_shutdown(struct usb_phy *phy)
+{
+	struct nop_usb_xceiv *nop = dev_get_drvdata(phy->dev);
+
+	if (!IS_ERR(nop->reset)) {
+		/* Assert RESET */
+		if (regulator_disable(nop->reset))
+			dev_err(phy->dev, "Failed to assert reset\n");
+	}
+
+	if (!IS_ERR(nop->clk))
+		clk_disable(nop->clk);
+
+	if (!IS_ERR(nop->vcc)) {
+		if (regulator_disable(nop->vcc))
+			dev_err(phy->dev, "Failed to disable power\n");
+	}
+}
+
 static int nop_set_peripheral(struct usb_otg *otg, struct usb_gadget *gadget)
 {
 	if (!otg)
@@ -95,39 +141,96 @@ static int nop_set_host(struct usb_otg *otg, struct usb_bus *host)
 
 static int nop_usb_xceiv_probe(struct platform_device *pdev)
 {
+	struct device *dev = &pdev->dev;
 	struct nop_usb_xceiv_platform_data *pdata = pdev->dev.platform_data;
 	struct nop_usb_xceiv	*nop;
 	enum usb_phy_type	type = USB_PHY_TYPE_USB2;
 	int err;
+	u32 clk_rate = 0;
+	bool needs_vcc = false;
+	bool needs_reset = false;
 
-	nop = kzalloc(sizeof *nop, GFP_KERNEL);
+	nop = devm_kzalloc(&pdev->dev, sizeof(*nop), GFP_KERNEL);
 	if (!nop)
 		return -ENOMEM;
 
-	nop->phy.otg = kzalloc(sizeof *nop->phy.otg, GFP_KERNEL);
-	if (!nop->phy.otg) {
-		kfree(nop);
+	nop->phy.otg = devm_kzalloc(&pdev->dev, sizeof(*nop->phy.otg),
+							GFP_KERNEL);
+	if (!nop->phy.otg)
 		return -ENOMEM;
-	}
 
-	if (pdata)
+	if (dev->of_node) {
+		struct device_node *node = dev->of_node;
+
+		if (of_property_read_u32(node, "clock-frequency", &clk_rate))
+			clk_rate = 0;
+
+		needs_vcc = of_property_read_bool(node, "vcc-supply");
+		needs_reset = of_property_read_bool(node, "reset-supply");
+
+	} else if (pdata) {
 		type = pdata->type;
+		clk_rate = pdata->clk_rate;
+		needs_vcc = pdata->needs_vcc;
+		needs_reset = pdata->needs_reset;
+	}
+
+	nop->clk = devm_clk_get(&pdev->dev, "main_clk");
+	if (IS_ERR(nop->clk)) {
+		dev_dbg(&pdev->dev, "Can't get phy clock: %ld\n",
+					PTR_ERR(nop->clk));
+	}
+
+	if (!IS_ERR(nop->clk) && clk_rate) {
+		err = clk_set_rate(nop->clk, clk_rate);
+		if (err) {
+			dev_err(&pdev->dev, "Error setting clock rate\n");
+			return err;
+		}
+	}
+
+	if (!IS_ERR(nop->clk)) {
+		err = clk_prepare(nop->clk);
+		if (err) {
+			dev_err(&pdev->dev, "Error preparing clock\n");
+			return err;
+		}
+	}
+
+	nop->vcc = devm_regulator_get(&pdev->dev, "vcc");
+	if (IS_ERR(nop->vcc)) {
+		dev_dbg(&pdev->dev, "Error getting vcc regulator: %ld\n",
+					PTR_ERR(nop->vcc));
+		if (needs_vcc)
+			return -EPROBE_DEFER;
+	}
+
+	nop->reset = devm_regulator_get(&pdev->dev, "reset");
+	if (IS_ERR(nop->reset)) {
+		dev_dbg(&pdev->dev, "Error getting reset regulator: %ld\n",
+					PTR_ERR(nop->reset));
+		if (needs_reset)
+			return -EPROBE_DEFER;
+	}
 
 	nop->dev		= &pdev->dev;
 	nop->phy.dev		= nop->dev;
 	nop->phy.label		= "nop-xceiv";
 	nop->phy.set_suspend	= nop_set_suspend;
+	nop->phy.init		= nop_init;
+	nop->phy.shutdown	= nop_shutdown;
 	nop->phy.state		= OTG_STATE_UNDEFINED;
+	nop->phy.type		= type;
 
 	nop->phy.otg->phy		= &nop->phy;
 	nop->phy.otg->set_host		= nop_set_host;
 	nop->phy.otg->set_peripheral	= nop_set_peripheral;
 
-	err = usb_add_phy(&nop->phy, type);
+	err = usb_add_phy_dev(&nop->phy);
 	if (err) {
 		dev_err(&pdev->dev, "can't register transceiver, err: %d\n",
 			err);
-		goto exit;
+		goto err_add;
 	}
 
 	platform_set_drvdata(pdev, nop);
@@ -135,9 +238,10 @@ static int nop_usb_xceiv_probe(struct platform_device *pdev)
 	ATOMIC_INIT_NOTIFIER_HEAD(&nop->phy.notifier);
 
 	return 0;
-exit:
-	kfree(nop->phy.otg);
-	kfree(nop);
+
+err_add:
+	if (!IS_ERR(nop->clk))
+		clk_unprepare(nop->clk);
 	return err;
 }
 
@@ -145,21 +249,30 @@ static int nop_usb_xceiv_remove(struct platform_device *pdev)
 {
 	struct nop_usb_xceiv *nop = platform_get_drvdata(pdev);
 
+	if (!IS_ERR(nop->clk))
+		clk_unprepare(nop->clk);
+
 	usb_remove_phy(&nop->phy);
 
 	platform_set_drvdata(pdev, NULL);
-	kfree(nop->phy.otg);
-	kfree(nop);
 
 	return 0;
 }
 
+static const struct of_device_id nop_xceiv_dt_ids[] = {
+	{ .compatible = "usb-nop-xceiv" },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(of, nop_xceiv_dt_ids);
+
 static struct platform_driver nop_usb_xceiv_driver = {
 	.probe		= nop_usb_xceiv_probe,
 	.remove		= nop_usb_xceiv_remove,
 	.driver		= {
 		.name	= "nop_usb_xceiv",
 		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(nop_xceiv_dt_ids),
 	},
 };
 
diff --git a/drivers/usb/phy/omap-control-usb.c b/drivers/usb/phy/phy-omap-control.c
index 1419ceda975..1419ceda975 100644
--- a/drivers/usb/phy/omap-control-usb.c
+++ b/drivers/usb/phy/phy-omap-control.c
diff --git a/drivers/usb/phy/omap-usb2.c b/drivers/usb/phy/phy-omap-usb2.c
index 844ab68f08d..844ab68f08d 100644
--- a/drivers/usb/phy/omap-usb2.c
+++ b/drivers/usb/phy/phy-omap-usb2.c
diff --git a/drivers/usb/phy/omap-usb3.c b/drivers/usb/phy/phy-omap-usb3.c
index a6e60b1e102..a6e60b1e102 100644
--- a/drivers/usb/phy/omap-usb3.c
+++ b/drivers/usb/phy/phy-omap-usb3.c
diff --git a/drivers/usb/phy/rcar-phy.c b/drivers/usb/phy/phy-rcar-usb.c
index a35681b0c50..a35681b0c50 100644
--- a/drivers/usb/phy/rcar-phy.c
+++ b/drivers/usb/phy/phy-rcar-usb.c
diff --git a/drivers/usb/phy/phy-samsung-usb.c b/drivers/usb/phy/phy-samsung-usb.c
new file mode 100644
index 00000000000..7b118ee5f5e
--- /dev/null
+++ b/drivers/usb/phy/phy-samsung-usb.c
@@ -0,0 +1,236 @@
+/* linux/drivers/usb/phy/phy-samsung-usb.c
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com
+ *
+ * Author: Praveen Paneri <p.paneri@samsung.com>
+ *
+ * Samsung USB-PHY helper driver with common function calls;
+ * interacts with Samsung USB 2.0 PHY controller driver and later
+ * with Samsung USB 3.0 PHY driver.
+ *
+ * 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.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/usb/samsung_usb_phy.h>
+
+#include "phy-samsung-usb.h"
+
+int samsung_usbphy_parse_dt(struct samsung_usbphy *sphy)
+{
+	struct device_node *usbphy_sys;
+
+	/* Getting node for system controller interface for usb-phy */
+	usbphy_sys = of_get_child_by_name(sphy->dev->of_node, "usbphy-sys");
+	if (!usbphy_sys) {
+		dev_err(sphy->dev, "No sys-controller interface for usb-phy\n");
+		return -ENODEV;
+	}
+
+	sphy->pmuregs = of_iomap(usbphy_sys, 0);
+
+	if (sphy->pmuregs == NULL) {
+		dev_err(sphy->dev, "Can't get usb-phy pmu control register\n");
+		goto err0;
+	}
+
+	sphy->sysreg = of_iomap(usbphy_sys, 1);
+
+	/*
+	 * Not returning error code here, since this situation is not fatal.
+	 * Few SoCs may not have this switch available
+	 */
+	if (sphy->sysreg == NULL)
+		dev_warn(sphy->dev, "Can't get usb-phy sysreg cfg register\n");
+
+	of_node_put(usbphy_sys);
+
+	return 0;
+
+err0:
+	of_node_put(usbphy_sys);
+	return -ENXIO;
+}
+EXPORT_SYMBOL_GPL(samsung_usbphy_parse_dt);
+
+/*
+ * Set isolation here for phy.
+ * Here 'on = true' would mean USB PHY block is isolated, hence
+ * de-activated and vice-versa.
+ */
+void samsung_usbphy_set_isolation(struct samsung_usbphy *sphy, bool on)
+{
+	void __iomem *reg = NULL;
+	u32 reg_val;
+	u32 en_mask = 0;
+
+	if (!sphy->pmuregs) {
+		dev_warn(sphy->dev, "Can't set pmu isolation\n");
+		return;
+	}
+
+	switch (sphy->drv_data->cpu_type) {
+	case TYPE_S3C64XX:
+		/*
+		 * Do nothing: We will add here once S3C64xx goes for DT support
+		 */
+		break;
+	case TYPE_EXYNOS4210:
+		/*
+		 * Fall through since exynos4210 and exynos5250 have similar
+		 * register architecture: two separate registers for host and
+		 * device phy control with enable bit at position 0.
+		 */
+	case TYPE_EXYNOS5250:
+		if (sphy->phy_type == USB_PHY_TYPE_DEVICE) {
+			reg = sphy->pmuregs +
+				sphy->drv_data->devphy_reg_offset;
+			en_mask = sphy->drv_data->devphy_en_mask;
+		} else if (sphy->phy_type == USB_PHY_TYPE_HOST) {
+			reg = sphy->pmuregs +
+				sphy->drv_data->hostphy_reg_offset;
+			en_mask = sphy->drv_data->hostphy_en_mask;
+		}
+		break;
+	default:
+		dev_err(sphy->dev, "Invalid SoC type\n");
+		return;
+	}
+
+	reg_val = readl(reg);
+
+	if (on)
+		reg_val &= ~en_mask;
+	else
+		reg_val |= en_mask;
+
+	writel(reg_val, reg);
+}
+EXPORT_SYMBOL_GPL(samsung_usbphy_set_isolation);
+
+/*
+ * Configure the mode of working of usb-phy here: HOST/DEVICE.
+ */
+void samsung_usbphy_cfg_sel(struct samsung_usbphy *sphy)
+{
+	u32 reg;
+
+	if (!sphy->sysreg) {
+		dev_warn(sphy->dev, "Can't configure specified phy mode\n");
+		return;
+	}
+
+	reg = readl(sphy->sysreg);
+
+	if (sphy->phy_type == USB_PHY_TYPE_DEVICE)
+		reg &= ~EXYNOS_USB20PHY_CFG_HOST_LINK;
+	else if (sphy->phy_type == USB_PHY_TYPE_HOST)
+		reg |= EXYNOS_USB20PHY_CFG_HOST_LINK;
+
+	writel(reg, sphy->sysreg);
+}
+EXPORT_SYMBOL_GPL(samsung_usbphy_cfg_sel);
+
+/*
+ * PHYs are different for USB Device and USB Host.
+ * This make sure that correct PHY type is selected before
+ * any operation on PHY.
+ */
+int samsung_usbphy_set_type(struct usb_phy *phy,
+				enum samsung_usb_phy_type phy_type)
+{
+	struct samsung_usbphy *sphy = phy_to_sphy(phy);
+
+	sphy->phy_type = phy_type;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(samsung_usbphy_set_type);
+
+/*
+ * Returns reference clock frequency selection value
+ */
+int samsung_usbphy_get_refclk_freq(struct samsung_usbphy *sphy)
+{
+	struct clk *ref_clk;
+	int refclk_freq = 0;
+
+	/*
+	 * In exynos5250 USB host and device PHY use
+	 * external crystal clock XXTI
+	 */
+	if (sphy->drv_data->cpu_type == TYPE_EXYNOS5250)
+		ref_clk = devm_clk_get(sphy->dev, "ext_xtal");
+	else
+		ref_clk = devm_clk_get(sphy->dev, "xusbxti");
+	if (IS_ERR(ref_clk)) {
+		dev_err(sphy->dev, "Failed to get reference clock\n");
+		return PTR_ERR(ref_clk);
+	}
+
+	if (sphy->drv_data->cpu_type == TYPE_EXYNOS5250) {
+		/* set clock frequency for PLL */
+		switch (clk_get_rate(ref_clk)) {
+		case 9600 * KHZ:
+			refclk_freq = FSEL_CLKSEL_9600K;
+			break;
+		case 10 * MHZ:
+			refclk_freq = FSEL_CLKSEL_10M;
+			break;
+		case 12 * MHZ:
+			refclk_freq = FSEL_CLKSEL_12M;
+			break;
+		case 19200 * KHZ:
+			refclk_freq = FSEL_CLKSEL_19200K;
+			break;
+		case 20 * MHZ:
+			refclk_freq = FSEL_CLKSEL_20M;
+			break;
+		case 50 * MHZ:
+			refclk_freq = FSEL_CLKSEL_50M;
+			break;
+		case 24 * MHZ:
+		default:
+			/* default reference clock */
+			refclk_freq = FSEL_CLKSEL_24M;
+			break;
+		}
+	} else {
+		switch (clk_get_rate(ref_clk)) {
+		case 12 * MHZ:
+			refclk_freq = PHYCLK_CLKSEL_12M;
+			break;
+		case 24 * MHZ:
+			refclk_freq = PHYCLK_CLKSEL_24M;
+			break;
+		case 48 * MHZ:
+			refclk_freq = PHYCLK_CLKSEL_48M;
+			break;
+		default:
+			if (sphy->drv_data->cpu_type == TYPE_S3C64XX)
+				refclk_freq = PHYCLK_CLKSEL_48M;
+			else
+				refclk_freq = PHYCLK_CLKSEL_24M;
+			break;
+		}
+	}
+	clk_put(ref_clk);
+
+	return refclk_freq;
+}
+EXPORT_SYMBOL_GPL(samsung_usbphy_get_refclk_freq);
diff --git a/drivers/usb/phy/phy-samsung-usb.h b/drivers/usb/phy/phy-samsung-usb.h
new file mode 100644
index 00000000000..70a9cae5e37
--- /dev/null
+++ b/drivers/usb/phy/phy-samsung-usb.h
@@ -0,0 +1,327 @@
+/* linux/drivers/usb/phy/phy-samsung-usb.h
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com
+ *
+ * Samsung USB-PHY transceiver; talks to S3C HS OTG controller, EHCI-S5P and
+ * OHCI-EXYNOS controllers.
+ *
+ * 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.
+ */
+
+#include <linux/usb/phy.h>
+
+/* Register definitions */
+
+#define SAMSUNG_PHYPWR				(0x00)
+
+#define PHYPWR_NORMAL_MASK			(0x19 << 0)
+#define PHYPWR_OTG_DISABLE			(0x1 << 4)
+#define PHYPWR_ANALOG_POWERDOWN			(0x1 << 3)
+#define PHYPWR_FORCE_SUSPEND			(0x1 << 1)
+/* For Exynos4 */
+#define PHYPWR_NORMAL_MASK_PHY0			(0x39 << 0)
+#define PHYPWR_SLEEP_PHY0			(0x1 << 5)
+
+#define SAMSUNG_PHYCLK				(0x04)
+
+#define PHYCLK_MODE_USB11			(0x1 << 6)
+#define PHYCLK_EXT_OSC				(0x1 << 5)
+#define PHYCLK_COMMON_ON_N			(0x1 << 4)
+#define PHYCLK_ID_PULL				(0x1 << 2)
+#define PHYCLK_CLKSEL_MASK			(0x3 << 0)
+#define PHYCLK_CLKSEL_48M			(0x0 << 0)
+#define PHYCLK_CLKSEL_12M			(0x2 << 0)
+#define PHYCLK_CLKSEL_24M			(0x3 << 0)
+
+#define SAMSUNG_RSTCON				(0x08)
+
+#define RSTCON_PHYLINK_SWRST			(0x1 << 2)
+#define RSTCON_HLINK_SWRST			(0x1 << 1)
+#define RSTCON_SWRST				(0x1 << 0)
+
+/* EXYNOS5 */
+#define EXYNOS5_PHY_HOST_CTRL0			(0x00)
+
+#define HOST_CTRL0_PHYSWRSTALL			(0x1 << 31)
+
+#define HOST_CTRL0_REFCLKSEL_MASK		(0x3 << 19)
+#define HOST_CTRL0_REFCLKSEL_XTAL		(0x0 << 19)
+#define HOST_CTRL0_REFCLKSEL_EXTL		(0x1 << 19)
+#define HOST_CTRL0_REFCLKSEL_CLKCORE		(0x2 << 19)
+
+#define HOST_CTRL0_FSEL_MASK			(0x7 << 16)
+#define HOST_CTRL0_FSEL(_x)			((_x) << 16)
+
+#define FSEL_CLKSEL_50M				(0x7)
+#define FSEL_CLKSEL_24M				(0x5)
+#define FSEL_CLKSEL_20M				(0x4)
+#define FSEL_CLKSEL_19200K			(0x3)
+#define FSEL_CLKSEL_12M				(0x2)
+#define FSEL_CLKSEL_10M				(0x1)
+#define FSEL_CLKSEL_9600K			(0x0)
+
+#define HOST_CTRL0_TESTBURNIN			(0x1 << 11)
+#define HOST_CTRL0_RETENABLE			(0x1 << 10)
+#define HOST_CTRL0_COMMONON_N			(0x1 << 9)
+#define HOST_CTRL0_SIDDQ			(0x1 << 6)
+#define HOST_CTRL0_FORCESLEEP			(0x1 << 5)
+#define HOST_CTRL0_FORCESUSPEND			(0x1 << 4)
+#define HOST_CTRL0_WORDINTERFACE		(0x1 << 3)
+#define HOST_CTRL0_UTMISWRST			(0x1 << 2)
+#define HOST_CTRL0_LINKSWRST			(0x1 << 1)
+#define HOST_CTRL0_PHYSWRST			(0x1 << 0)
+
+#define EXYNOS5_PHY_HOST_TUNE0			(0x04)
+
+#define EXYNOS5_PHY_HSIC_CTRL1			(0x10)
+
+#define EXYNOS5_PHY_HSIC_TUNE1			(0x14)
+
+#define EXYNOS5_PHY_HSIC_CTRL2			(0x20)
+
+#define EXYNOS5_PHY_HSIC_TUNE2			(0x24)
+
+#define HSIC_CTRL_REFCLKSEL_MASK		(0x3 << 23)
+#define HSIC_CTRL_REFCLKSEL			(0x2 << 23)
+
+#define HSIC_CTRL_REFCLKDIV_MASK		(0x7f << 16)
+#define HSIC_CTRL_REFCLKDIV(_x)			((_x) << 16)
+#define HSIC_CTRL_REFCLKDIV_12			(0x24 << 16)
+#define HSIC_CTRL_REFCLKDIV_15			(0x1c << 16)
+#define HSIC_CTRL_REFCLKDIV_16			(0x1a << 16)
+#define HSIC_CTRL_REFCLKDIV_19_2		(0x15 << 16)
+#define HSIC_CTRL_REFCLKDIV_20			(0x14 << 16)
+
+#define HSIC_CTRL_SIDDQ				(0x1 << 6)
+#define HSIC_CTRL_FORCESLEEP			(0x1 << 5)
+#define HSIC_CTRL_FORCESUSPEND			(0x1 << 4)
+#define HSIC_CTRL_WORDINTERFACE			(0x1 << 3)
+#define HSIC_CTRL_UTMISWRST			(0x1 << 2)
+#define HSIC_CTRL_PHYSWRST			(0x1 << 0)
+
+#define EXYNOS5_PHY_HOST_EHCICTRL		(0x30)
+
+#define HOST_EHCICTRL_ENAINCRXALIGN		(0x1 << 29)
+#define HOST_EHCICTRL_ENAINCR4			(0x1 << 28)
+#define HOST_EHCICTRL_ENAINCR8			(0x1 << 27)
+#define HOST_EHCICTRL_ENAINCR16			(0x1 << 26)
+
+#define EXYNOS5_PHY_HOST_OHCICTRL		(0x34)
+
+#define HOST_OHCICTRL_SUSPLGCY			(0x1 << 3)
+#define HOST_OHCICTRL_APPSTARTCLK		(0x1 << 2)
+#define HOST_OHCICTRL_CNTSEL			(0x1 << 1)
+#define HOST_OHCICTRL_CLKCKTRST			(0x1 << 0)
+
+#define EXYNOS5_PHY_OTG_SYS			(0x38)
+
+#define OTG_SYS_PHYLINK_SWRESET			(0x1 << 14)
+#define OTG_SYS_LINKSWRST_UOTG			(0x1 << 13)
+#define OTG_SYS_PHY0_SWRST			(0x1 << 12)
+
+#define OTG_SYS_REFCLKSEL_MASK			(0x3 << 9)
+#define OTG_SYS_REFCLKSEL_XTAL			(0x0 << 9)
+#define OTG_SYS_REFCLKSEL_EXTL			(0x1 << 9)
+#define OTG_SYS_REFCLKSEL_CLKCORE		(0x2 << 9)
+
+#define OTG_SYS_IDPULLUP_UOTG			(0x1 << 8)
+#define OTG_SYS_COMMON_ON			(0x1 << 7)
+
+#define OTG_SYS_FSEL_MASK			(0x7 << 4)
+#define OTG_SYS_FSEL(_x)			((_x) << 4)
+
+#define OTG_SYS_FORCESLEEP			(0x1 << 3)
+#define OTG_SYS_OTGDISABLE			(0x1 << 2)
+#define OTG_SYS_SIDDQ_UOTG			(0x1 << 1)
+#define OTG_SYS_FORCESUSPEND			(0x1 << 0)
+
+#define EXYNOS5_PHY_OTG_TUNE			(0x40)
+
+/* EXYNOS5: USB 3.0 DRD */
+#define EXYNOS5_DRD_LINKSYSTEM			(0x04)
+
+#define LINKSYSTEM_FLADJ_MASK			(0x3f << 1)
+#define LINKSYSTEM_FLADJ(_x)			((_x) << 1)
+#define LINKSYSTEM_XHCI_VERSION_CONTROL		(0x1 << 27)
+
+#define EXYNOS5_DRD_PHYUTMI			(0x08)
+
+#define PHYUTMI_OTGDISABLE			(0x1 << 6)
+#define PHYUTMI_FORCESUSPEND			(0x1 << 1)
+#define PHYUTMI_FORCESLEEP			(0x1 << 0)
+
+#define EXYNOS5_DRD_PHYPIPE			(0x0c)
+
+#define EXYNOS5_DRD_PHYCLKRST			(0x10)
+
+#define PHYCLKRST_SSC_REFCLKSEL_MASK		(0xff << 23)
+#define PHYCLKRST_SSC_REFCLKSEL(_x)		((_x) << 23)
+
+#define PHYCLKRST_SSC_RANGE_MASK		(0x03 << 21)
+#define PHYCLKRST_SSC_RANGE(_x)			((_x) << 21)
+
+#define PHYCLKRST_SSC_EN			(0x1 << 20)
+#define PHYCLKRST_REF_SSP_EN			(0x1 << 19)
+#define PHYCLKRST_REF_CLKDIV2			(0x1 << 18)
+
+#define PHYCLKRST_MPLL_MULTIPLIER_MASK		(0x7f << 11)
+#define PHYCLKRST_MPLL_MULTIPLIER_100MHZ_REF	(0x19 << 11)
+#define PHYCLKRST_MPLL_MULTIPLIER_50M_REF	(0x02 << 11)
+#define PHYCLKRST_MPLL_MULTIPLIER_24MHZ_REF	(0x68 << 11)
+#define PHYCLKRST_MPLL_MULTIPLIER_20MHZ_REF	(0x7d << 11)
+#define PHYCLKRST_MPLL_MULTIPLIER_19200KHZ_REF	(0x02 << 11)
+
+#define PHYCLKRST_FSEL_MASK			(0x3f << 5)
+#define PHYCLKRST_FSEL(_x)			((_x) << 5)
+#define PHYCLKRST_FSEL_PAD_100MHZ		(0x27 << 5)
+#define PHYCLKRST_FSEL_PAD_24MHZ		(0x2a << 5)
+#define PHYCLKRST_FSEL_PAD_20MHZ		(0x31 << 5)
+#define PHYCLKRST_FSEL_PAD_19_2MHZ		(0x38 << 5)
+
+#define PHYCLKRST_RETENABLEN			(0x1 << 4)
+
+#define PHYCLKRST_REFCLKSEL_MASK		(0x03 << 2)
+#define PHYCLKRST_REFCLKSEL_PAD_REFCLK		(0x2 << 2)
+#define PHYCLKRST_REFCLKSEL_EXT_REFCLK		(0x3 << 2)
+
+#define PHYCLKRST_PORTRESET			(0x1 << 1)
+#define PHYCLKRST_COMMONONN			(0x1 << 0)
+
+#define EXYNOS5_DRD_PHYREG0			(0x14)
+#define EXYNOS5_DRD_PHYREG1			(0x18)
+
+#define EXYNOS5_DRD_PHYPARAM0			(0x1c)
+
+#define PHYPARAM0_REF_USE_PAD			(0x1 << 31)
+#define PHYPARAM0_REF_LOSLEVEL_MASK		(0x1f << 26)
+#define PHYPARAM0_REF_LOSLEVEL			(0x9 << 26)
+
+#define EXYNOS5_DRD_PHYPARAM1			(0x20)
+
+#define PHYPARAM1_PCS_TXDEEMPH_MASK		(0x1f << 0)
+#define PHYPARAM1_PCS_TXDEEMPH			(0x1c)
+
+#define EXYNOS5_DRD_PHYTERM			(0x24)
+
+#define EXYNOS5_DRD_PHYTEST			(0x28)
+
+#define PHYTEST_POWERDOWN_SSP			(0x1 << 3)
+#define PHYTEST_POWERDOWN_HSP			(0x1 << 2)
+
+#define EXYNOS5_DRD_PHYADP			(0x2c)
+
+#define EXYNOS5_DRD_PHYBATCHG			(0x30)
+
+#define PHYBATCHG_UTMI_CLKSEL			(0x1 << 2)
+
+#define EXYNOS5_DRD_PHYRESUME			(0x34)
+#define EXYNOS5_DRD_LINKPORT			(0x44)
+
+#ifndef MHZ
+#define MHZ (1000*1000)
+#endif
+
+#ifndef KHZ
+#define KHZ (1000)
+#endif
+
+#define EXYNOS_USBHOST_PHY_CTRL_OFFSET		(0x4)
+#define S3C64XX_USBPHY_ENABLE			(0x1 << 16)
+#define EXYNOS_USBPHY_ENABLE			(0x1 << 0)
+#define EXYNOS_USB20PHY_CFG_HOST_LINK		(0x1 << 0)
+
+enum samsung_cpu_type {
+	TYPE_S3C64XX,
+	TYPE_EXYNOS4210,
+	TYPE_EXYNOS5250,
+};
+
+/*
+ * struct samsung_usbphy_drvdata - driver data for various SoC variants
+ * @cpu_type: machine identifier
+ * @devphy_en_mask: device phy enable mask for PHY CONTROL register
+ * @hostphy_en_mask: host phy enable mask for PHY CONTROL register
+ * @devphy_reg_offset: offset to DEVICE PHY CONTROL register from
+ *		       mapped address of system controller.
+ * @hostphy_reg_offset: offset to HOST PHY CONTROL register from
+ *		       mapped address of system controller.
+ *
+ *	Here we have a separate mask for device type phy.
+ *	Having different masks for host and device type phy helps
+ *	in setting independent masks in case of SoCs like S5PV210,
+ *	in which PHY0 and PHY1 enable bits belong to same register
+ *	placed at position 0 and 1 respectively.
+ *	Although for newer SoCs like exynos these bits belong to
+ *	different registers altogether placed at position 0.
+ */
+struct samsung_usbphy_drvdata {
+	int cpu_type;
+	int devphy_en_mask;
+	int hostphy_en_mask;
+	u32 devphy_reg_offset;
+	u32 hostphy_reg_offset;
+};
+
+/*
+ * struct samsung_usbphy - transceiver driver state
+ * @phy: transceiver structure
+ * @plat: platform data
+ * @dev: The parent device supplied to the probe function
+ * @clk: usb phy clock
+ * @regs: usb phy controller registers memory base
+ * @pmuregs: USB device PHY_CONTROL register memory base
+ * @sysreg: USB2.0 PHY_CFG register memory base
+ * @ref_clk_freq: reference clock frequency selection
+ * @drv_data: driver data available for different SoCs
+ * @phy_type: Samsung SoCs specific phy types:	#HOST
+ *						#DEVICE
+ * @phy_usage: usage count for phy
+ * @lock: lock for phy operations
+ */
+struct samsung_usbphy {
+	struct usb_phy	phy;
+	struct samsung_usbphy_data *plat;
+	struct device	*dev;
+	struct clk	*clk;
+	void __iomem	*regs;
+	void __iomem	*pmuregs;
+	void __iomem	*sysreg;
+	int		ref_clk_freq;
+	const struct samsung_usbphy_drvdata *drv_data;
+	enum samsung_usb_phy_type phy_type;
+	atomic_t	phy_usage;
+	spinlock_t	lock;
+};
+
+#define phy_to_sphy(x)		container_of((x), struct samsung_usbphy, phy)
+
+static const struct of_device_id samsung_usbphy_dt_match[];
+
+static inline const struct samsung_usbphy_drvdata
+*samsung_usbphy_get_driver_data(struct platform_device *pdev)
+{
+	if (pdev->dev.of_node) {
+		const struct of_device_id *match;
+		match = of_match_node(samsung_usbphy_dt_match,
+							pdev->dev.of_node);
+		return match->data;
+	}
+
+	return (struct samsung_usbphy_drvdata *)
+				platform_get_device_id(pdev)->driver_data;
+}
+
+extern int samsung_usbphy_parse_dt(struct samsung_usbphy *sphy);
+extern void samsung_usbphy_set_isolation(struct samsung_usbphy *sphy, bool on);
+extern void samsung_usbphy_cfg_sel(struct samsung_usbphy *sphy);
+extern int samsung_usbphy_set_type(struct usb_phy *phy,
+					enum samsung_usb_phy_type phy_type);
+extern int samsung_usbphy_get_refclk_freq(struct samsung_usbphy *sphy);
diff --git a/drivers/usb/phy/phy-samsung-usb2.c b/drivers/usb/phy/phy-samsung-usb2.c
new file mode 100644
index 00000000000..45ffe036dac
--- /dev/null
+++ b/drivers/usb/phy/phy-samsung-usb2.c
@@ -0,0 +1,509 @@
+/* linux/drivers/usb/phy/phy-samsung-usb2.c
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com
+ *
+ * Author: Praveen Paneri <p.paneri@samsung.com>
+ *
+ * Samsung USB2.0 PHY transceiver; talks to S3C HS OTG controller, EHCI-S5P and
+ * OHCI-EXYNOS controllers.
+ *
+ * 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.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/usb/otg.h>
+#include <linux/usb/samsung_usb_phy.h>
+#include <linux/platform_data/samsung-usbphy.h>
+
+#include "phy-samsung-usb.h"
+
+static int samsung_usbphy_set_host(struct usb_otg *otg, struct usb_bus *host)
+{
+	if (!otg)
+		return -ENODEV;
+
+	if (!otg->host)
+		otg->host = host;
+
+	return 0;
+}
+
+static bool exynos5_phyhost_is_on(void __iomem *regs)
+{
+	u32 reg;
+
+	reg = readl(regs + EXYNOS5_PHY_HOST_CTRL0);
+
+	return !(reg & HOST_CTRL0_SIDDQ);
+}
+
+static void samsung_exynos5_usb2phy_enable(struct samsung_usbphy *sphy)
+{
+	void __iomem *regs = sphy->regs;
+	u32 phyclk = sphy->ref_clk_freq;
+	u32 phyhost;
+	u32 phyotg;
+	u32 phyhsic;
+	u32 ehcictrl;
+	u32 ohcictrl;
+
+	/*
+	 * phy_usage helps in keeping usage count for phy
+	 * so that the first consumer enabling the phy is also
+	 * the last consumer to disable it.
+	 */
+
+	atomic_inc(&sphy->phy_usage);
+
+	if (exynos5_phyhost_is_on(regs)) {
+		dev_info(sphy->dev, "Already power on PHY\n");
+		return;
+	}
+
+	/* Host configuration */
+	phyhost = readl(regs + EXYNOS5_PHY_HOST_CTRL0);
+
+	/* phy reference clock configuration */
+	phyhost &= ~HOST_CTRL0_FSEL_MASK;
+	phyhost |= HOST_CTRL0_FSEL(phyclk);
+
+	/* host phy reset */
+	phyhost &= ~(HOST_CTRL0_PHYSWRST |
+			HOST_CTRL0_PHYSWRSTALL |
+			HOST_CTRL0_SIDDQ |
+			/* Enable normal mode of operation */
+			HOST_CTRL0_FORCESUSPEND |
+			HOST_CTRL0_FORCESLEEP);
+
+	/* Link reset */
+	phyhost |= (HOST_CTRL0_LINKSWRST |
+			HOST_CTRL0_UTMISWRST |
+			/* COMMON Block configuration during suspend */
+			HOST_CTRL0_COMMONON_N);
+	writel(phyhost, regs + EXYNOS5_PHY_HOST_CTRL0);
+	udelay(10);
+	phyhost &= ~(HOST_CTRL0_LINKSWRST |
+			HOST_CTRL0_UTMISWRST);
+	writel(phyhost, regs + EXYNOS5_PHY_HOST_CTRL0);
+
+	/* OTG configuration */
+	phyotg = readl(regs + EXYNOS5_PHY_OTG_SYS);
+
+	/* phy reference clock configuration */
+	phyotg &= ~OTG_SYS_FSEL_MASK;
+	phyotg |= OTG_SYS_FSEL(phyclk);
+
+	/* Enable normal mode of operation */
+	phyotg &= ~(OTG_SYS_FORCESUSPEND |
+			OTG_SYS_SIDDQ_UOTG |
+			OTG_SYS_FORCESLEEP |
+			OTG_SYS_REFCLKSEL_MASK |
+			/* COMMON Block configuration during suspend */
+			OTG_SYS_COMMON_ON);
+
+	/* OTG phy & link reset */
+	phyotg |= (OTG_SYS_PHY0_SWRST |
+			OTG_SYS_LINKSWRST_UOTG |
+			OTG_SYS_PHYLINK_SWRESET |
+			OTG_SYS_OTGDISABLE |
+			/* Set phy refclk */
+			OTG_SYS_REFCLKSEL_CLKCORE);
+
+	writel(phyotg, regs + EXYNOS5_PHY_OTG_SYS);
+	udelay(10);
+	phyotg &= ~(OTG_SYS_PHY0_SWRST |
+			OTG_SYS_LINKSWRST_UOTG |
+			OTG_SYS_PHYLINK_SWRESET);
+	writel(phyotg, regs + EXYNOS5_PHY_OTG_SYS);
+
+	/* HSIC phy configuration */
+	phyhsic = (HSIC_CTRL_REFCLKDIV_12 |
+			HSIC_CTRL_REFCLKSEL |
+			HSIC_CTRL_PHYSWRST);
+	writel(phyhsic, regs + EXYNOS5_PHY_HSIC_CTRL1);
+	writel(phyhsic, regs + EXYNOS5_PHY_HSIC_CTRL2);
+	udelay(10);
+	phyhsic &= ~HSIC_CTRL_PHYSWRST;
+	writel(phyhsic, regs + EXYNOS5_PHY_HSIC_CTRL1);
+	writel(phyhsic, regs + EXYNOS5_PHY_HSIC_CTRL2);
+
+	udelay(80);
+
+	/* enable EHCI DMA burst */
+	ehcictrl = readl(regs + EXYNOS5_PHY_HOST_EHCICTRL);
+	ehcictrl |= (HOST_EHCICTRL_ENAINCRXALIGN |
+				HOST_EHCICTRL_ENAINCR4 |
+				HOST_EHCICTRL_ENAINCR8 |
+				HOST_EHCICTRL_ENAINCR16);
+	writel(ehcictrl, regs + EXYNOS5_PHY_HOST_EHCICTRL);
+
+	/* set ohci_suspend_on_n */
+	ohcictrl = readl(regs + EXYNOS5_PHY_HOST_OHCICTRL);
+	ohcictrl |= HOST_OHCICTRL_SUSPLGCY;
+	writel(ohcictrl, regs + EXYNOS5_PHY_HOST_OHCICTRL);
+}
+
+static void samsung_usb2phy_enable(struct samsung_usbphy *sphy)
+{
+	void __iomem *regs = sphy->regs;
+	u32 phypwr;
+	u32 phyclk;
+	u32 rstcon;
+
+	/* set clock frequency for PLL */
+	phyclk = sphy->ref_clk_freq;
+	phypwr = readl(regs + SAMSUNG_PHYPWR);
+	rstcon = readl(regs + SAMSUNG_RSTCON);
+
+	switch (sphy->drv_data->cpu_type) {
+	case TYPE_S3C64XX:
+		phyclk &= ~PHYCLK_COMMON_ON_N;
+		phypwr &= ~PHYPWR_NORMAL_MASK;
+		rstcon |= RSTCON_SWRST;
+		break;
+	case TYPE_EXYNOS4210:
+		phypwr &= ~PHYPWR_NORMAL_MASK_PHY0;
+		rstcon |= RSTCON_SWRST;
+	default:
+		break;
+	}
+
+	writel(phyclk, regs + SAMSUNG_PHYCLK);
+	/* Configure PHY0 for normal operation*/
+	writel(phypwr, regs + SAMSUNG_PHYPWR);
+	/* reset all ports of PHY and Link */
+	writel(rstcon, regs + SAMSUNG_RSTCON);
+	udelay(10);
+	rstcon &= ~RSTCON_SWRST;
+	writel(rstcon, regs + SAMSUNG_RSTCON);
+}
+
+static void samsung_exynos5_usb2phy_disable(struct samsung_usbphy *sphy)
+{
+	void __iomem *regs = sphy->regs;
+	u32 phyhost;
+	u32 phyotg;
+	u32 phyhsic;
+
+	if (atomic_dec_return(&sphy->phy_usage) > 0) {
+		dev_info(sphy->dev, "still being used\n");
+		return;
+	}
+
+	phyhsic = (HSIC_CTRL_REFCLKDIV_12 |
+			HSIC_CTRL_REFCLKSEL |
+			HSIC_CTRL_SIDDQ |
+			HSIC_CTRL_FORCESLEEP |
+			HSIC_CTRL_FORCESUSPEND);
+	writel(phyhsic, regs + EXYNOS5_PHY_HSIC_CTRL1);
+	writel(phyhsic, regs + EXYNOS5_PHY_HSIC_CTRL2);
+
+	phyhost = readl(regs + EXYNOS5_PHY_HOST_CTRL0);
+	phyhost |= (HOST_CTRL0_SIDDQ |
+			HOST_CTRL0_FORCESUSPEND |
+			HOST_CTRL0_FORCESLEEP |
+			HOST_CTRL0_PHYSWRST |
+			HOST_CTRL0_PHYSWRSTALL);
+	writel(phyhost, regs + EXYNOS5_PHY_HOST_CTRL0);
+
+	phyotg = readl(regs + EXYNOS5_PHY_OTG_SYS);
+	phyotg |= (OTG_SYS_FORCESUSPEND |
+			OTG_SYS_SIDDQ_UOTG |
+			OTG_SYS_FORCESLEEP);
+	writel(phyotg, regs + EXYNOS5_PHY_OTG_SYS);
+}
+
+static void samsung_usb2phy_disable(struct samsung_usbphy *sphy)
+{
+	void __iomem *regs = sphy->regs;
+	u32 phypwr;
+
+	phypwr = readl(regs + SAMSUNG_PHYPWR);
+
+	switch (sphy->drv_data->cpu_type) {
+	case TYPE_S3C64XX:
+		phypwr |= PHYPWR_NORMAL_MASK;
+		break;
+	case TYPE_EXYNOS4210:
+		phypwr |= PHYPWR_NORMAL_MASK_PHY0;
+	default:
+		break;
+	}
+
+	/* Disable analog and otg block power */
+	writel(phypwr, regs + SAMSUNG_PHYPWR);
+}
+
+/*
+ * The function passed to the usb driver for phy initialization
+ */
+static int samsung_usb2phy_init(struct usb_phy *phy)
+{
+	struct samsung_usbphy *sphy;
+	struct usb_bus *host = NULL;
+	unsigned long flags;
+	int ret = 0;
+
+	sphy = phy_to_sphy(phy);
+
+	host = phy->otg->host;
+
+	/* Enable the phy clock */
+	ret = clk_prepare_enable(sphy->clk);
+	if (ret) {
+		dev_err(sphy->dev, "%s: clk_prepare_enable failed\n", __func__);
+		return ret;
+	}
+
+	spin_lock_irqsave(&sphy->lock, flags);
+
+	if (host) {
+		/* setting default phy-type for USB 2.0 */
+		if (!strstr(dev_name(host->controller), "ehci") ||
+				!strstr(dev_name(host->controller), "ohci"))
+			samsung_usbphy_set_type(&sphy->phy, USB_PHY_TYPE_HOST);
+	} else {
+		samsung_usbphy_set_type(&sphy->phy, USB_PHY_TYPE_DEVICE);
+	}
+
+	/* Disable phy isolation */
+	if (sphy->plat && sphy->plat->pmu_isolation)
+		sphy->plat->pmu_isolation(false);
+	else
+		samsung_usbphy_set_isolation(sphy, false);
+
+	/* Selecting Host/OTG mode; After reset USB2.0PHY_CFG: HOST */
+	samsung_usbphy_cfg_sel(sphy);
+
+	/* Initialize usb phy registers */
+	if (sphy->drv_data->cpu_type == TYPE_EXYNOS5250)
+		samsung_exynos5_usb2phy_enable(sphy);
+	else
+		samsung_usb2phy_enable(sphy);
+
+	spin_unlock_irqrestore(&sphy->lock, flags);
+
+	/* Disable the phy clock */
+	clk_disable_unprepare(sphy->clk);
+
+	return ret;
+}
+
+/*
+ * The function passed to the usb driver for phy shutdown
+ */
+static void samsung_usb2phy_shutdown(struct usb_phy *phy)
+{
+	struct samsung_usbphy *sphy;
+	struct usb_bus *host = NULL;
+	unsigned long flags;
+
+	sphy = phy_to_sphy(phy);
+
+	host = phy->otg->host;
+
+	if (clk_prepare_enable(sphy->clk)) {
+		dev_err(sphy->dev, "%s: clk_prepare_enable failed\n", __func__);
+		return;
+	}
+
+	spin_lock_irqsave(&sphy->lock, flags);
+
+	if (host) {
+		/* setting default phy-type for USB 2.0 */
+		if (!strstr(dev_name(host->controller), "ehci") ||
+				!strstr(dev_name(host->controller), "ohci"))
+			samsung_usbphy_set_type(&sphy->phy, USB_PHY_TYPE_HOST);
+	} else {
+		samsung_usbphy_set_type(&sphy->phy, USB_PHY_TYPE_DEVICE);
+	}
+
+	/* De-initialize usb phy registers */
+	if (sphy->drv_data->cpu_type == TYPE_EXYNOS5250)
+		samsung_exynos5_usb2phy_disable(sphy);
+	else
+		samsung_usb2phy_disable(sphy);
+
+	/* Enable phy isolation */
+	if (sphy->plat && sphy->plat->pmu_isolation)
+		sphy->plat->pmu_isolation(true);
+	else
+		samsung_usbphy_set_isolation(sphy, true);
+
+	spin_unlock_irqrestore(&sphy->lock, flags);
+
+	clk_disable_unprepare(sphy->clk);
+}
+
+static int samsung_usb2phy_probe(struct platform_device *pdev)
+{
+	struct samsung_usbphy *sphy;
+	struct usb_otg *otg;
+	struct samsung_usbphy_data *pdata = pdev->dev.platform_data;
+	const struct samsung_usbphy_drvdata *drv_data;
+	struct device *dev = &pdev->dev;
+	struct resource *phy_mem;
+	void __iomem	*phy_base;
+	struct clk *clk;
+	int ret;
+
+	phy_mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!phy_mem) {
+		dev_err(dev, "%s: missing mem resource\n", __func__);
+		return -ENODEV;
+	}
+
+	phy_base = devm_ioremap_resource(dev, phy_mem);
+	if (IS_ERR(phy_base))
+		return PTR_ERR(phy_base);
+
+	sphy = devm_kzalloc(dev, sizeof(*sphy), GFP_KERNEL);
+	if (!sphy)
+		return -ENOMEM;
+
+	otg = devm_kzalloc(dev, sizeof(*otg), GFP_KERNEL);
+	if (!otg)
+		return -ENOMEM;
+
+	drv_data = samsung_usbphy_get_driver_data(pdev);
+
+	if (drv_data->cpu_type == TYPE_EXYNOS5250)
+		clk = devm_clk_get(dev, "usbhost");
+	else
+		clk = devm_clk_get(dev, "otg");
+
+	if (IS_ERR(clk)) {
+		dev_err(dev, "Failed to get otg clock\n");
+		return PTR_ERR(clk);
+	}
+
+	sphy->dev = dev;
+
+	if (dev->of_node) {
+		ret = samsung_usbphy_parse_dt(sphy);
+		if (ret < 0)
+			return ret;
+	} else {
+		if (!pdata) {
+			dev_err(dev, "no platform data specified\n");
+			return -EINVAL;
+		}
+	}
+
+	sphy->plat		= pdata;
+	sphy->regs		= phy_base;
+	sphy->clk		= clk;
+	sphy->drv_data		= drv_data;
+	sphy->phy.dev		= sphy->dev;
+	sphy->phy.label		= "samsung-usb2phy";
+	sphy->phy.init		= samsung_usb2phy_init;
+	sphy->phy.shutdown	= samsung_usb2phy_shutdown;
+	sphy->ref_clk_freq	= samsung_usbphy_get_refclk_freq(sphy);
+
+	sphy->phy.otg		= otg;
+	sphy->phy.otg->phy	= &sphy->phy;
+	sphy->phy.otg->set_host = samsung_usbphy_set_host;
+
+	spin_lock_init(&sphy->lock);
+
+	platform_set_drvdata(pdev, sphy);
+
+	return usb_add_phy(&sphy->phy, USB_PHY_TYPE_USB2);
+}
+
+static int samsung_usb2phy_remove(struct platform_device *pdev)
+{
+	struct samsung_usbphy *sphy = platform_get_drvdata(pdev);
+
+	usb_remove_phy(&sphy->phy);
+
+	if (sphy->pmuregs)
+		iounmap(sphy->pmuregs);
+	if (sphy->sysreg)
+		iounmap(sphy->sysreg);
+
+	return 0;
+}
+
+static const struct samsung_usbphy_drvdata usb2phy_s3c64xx = {
+	.cpu_type		= TYPE_S3C64XX,
+	.devphy_en_mask		= S3C64XX_USBPHY_ENABLE,
+};
+
+static const struct samsung_usbphy_drvdata usb2phy_exynos4 = {
+	.cpu_type		= TYPE_EXYNOS4210,
+	.devphy_en_mask		= EXYNOS_USBPHY_ENABLE,
+	.hostphy_en_mask	= EXYNOS_USBPHY_ENABLE,
+};
+
+static struct samsung_usbphy_drvdata usb2phy_exynos5 = {
+	.cpu_type		= TYPE_EXYNOS5250,
+	.hostphy_en_mask	= EXYNOS_USBPHY_ENABLE,
+	.hostphy_reg_offset	= EXYNOS_USBHOST_PHY_CTRL_OFFSET,
+};
+
+#ifdef CONFIG_OF
+static const struct of_device_id samsung_usbphy_dt_match[] = {
+	{
+		.compatible = "samsung,s3c64xx-usb2phy",
+		.data = &usb2phy_s3c64xx,
+	}, {
+		.compatible = "samsung,exynos4210-usb2phy",
+		.data = &usb2phy_exynos4,
+	}, {
+		.compatible = "samsung,exynos5250-usb2phy",
+		.data = &usb2phy_exynos5
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, samsung_usbphy_dt_match);
+#endif
+
+static struct platform_device_id samsung_usbphy_driver_ids[] = {
+	{
+		.name		= "s3c64xx-usb2phy",
+		.driver_data	= (unsigned long)&usb2phy_s3c64xx,
+	}, {
+		.name		= "exynos4210-usb2phy",
+		.driver_data	= (unsigned long)&usb2phy_exynos4,
+	}, {
+		.name		= "exynos5250-usb2phy",
+		.driver_data	= (unsigned long)&usb2phy_exynos5,
+	},
+	{},
+};
+
+MODULE_DEVICE_TABLE(platform, samsung_usbphy_driver_ids);
+
+static struct platform_driver samsung_usb2phy_driver = {
+	.probe		= samsung_usb2phy_probe,
+	.remove		= samsung_usb2phy_remove,
+	.id_table	= samsung_usbphy_driver_ids,
+	.driver		= {
+		.name	= "samsung-usb2phy",
+		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(samsung_usbphy_dt_match),
+	},
+};
+
+module_platform_driver(samsung_usb2phy_driver);
+
+MODULE_DESCRIPTION("Samsung USB 2.0 phy controller");
+MODULE_AUTHOR("Praveen Paneri <p.paneri@samsung.com>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:samsung-usb2phy");
diff --git a/drivers/usb/phy/phy-samsung-usb3.c b/drivers/usb/phy/phy-samsung-usb3.c
new file mode 100644
index 00000000000..133f3d0c554
--- /dev/null
+++ b/drivers/usb/phy/phy-samsung-usb3.c
@@ -0,0 +1,347 @@
+/* linux/drivers/usb/phy/phy-samsung-usb3.c
+ *
+ * Copyright (c) 2013 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com
+ *
+ * Author: Vivek Gautam <gautam.vivek@samsung.com>
+ *
+ * Samsung USB 3.0 PHY transceiver; talks to DWC3 controller.
+ *
+ * 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.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/usb/samsung_usb_phy.h>
+#include <linux/platform_data/samsung-usbphy.h>
+
+#include "phy-samsung-usb.h"
+
+/*
+ * Sets the phy clk as EXTREFCLK (XXTI) which is internal clock from clock core.
+ */
+static u32 samsung_usb3phy_set_refclk(struct samsung_usbphy *sphy)
+{
+	u32 reg;
+	u32 refclk;
+
+	refclk = sphy->ref_clk_freq;
+
+	reg = PHYCLKRST_REFCLKSEL_EXT_REFCLK |
+		PHYCLKRST_FSEL(refclk);
+
+	switch (refclk) {
+	case FSEL_CLKSEL_50M:
+		reg |= (PHYCLKRST_MPLL_MULTIPLIER_50M_REF |
+			PHYCLKRST_SSC_REFCLKSEL(0x00));
+		break;
+	case FSEL_CLKSEL_20M:
+		reg |= (PHYCLKRST_MPLL_MULTIPLIER_20MHZ_REF |
+			PHYCLKRST_SSC_REFCLKSEL(0x00));
+		break;
+	case FSEL_CLKSEL_19200K:
+		reg |= (PHYCLKRST_MPLL_MULTIPLIER_19200KHZ_REF |
+			PHYCLKRST_SSC_REFCLKSEL(0x88));
+		break;
+	case FSEL_CLKSEL_24M:
+	default:
+		reg |= (PHYCLKRST_MPLL_MULTIPLIER_24MHZ_REF |
+			PHYCLKRST_SSC_REFCLKSEL(0x88));
+		break;
+	}
+
+	return reg;
+}
+
+static int samsung_exynos5_usb3phy_enable(struct samsung_usbphy *sphy)
+{
+	void __iomem *regs = sphy->regs;
+	u32 phyparam0;
+	u32 phyparam1;
+	u32 linksystem;
+	u32 phybatchg;
+	u32 phytest;
+	u32 phyclkrst;
+
+	/* Reset USB 3.0 PHY */
+	writel(0x0, regs + EXYNOS5_DRD_PHYREG0);
+
+	phyparam0 = readl(regs + EXYNOS5_DRD_PHYPARAM0);
+	/* Select PHY CLK source */
+	phyparam0 &= ~PHYPARAM0_REF_USE_PAD;
+	/* Set Loss-of-Signal Detector sensitivity */
+	phyparam0 &= ~PHYPARAM0_REF_LOSLEVEL_MASK;
+	phyparam0 |= PHYPARAM0_REF_LOSLEVEL;
+	writel(phyparam0, regs + EXYNOS5_DRD_PHYPARAM0);
+
+	writel(0x0, regs + EXYNOS5_DRD_PHYRESUME);
+
+	/*
+	 * Setting the Frame length Adj value[6:1] to default 0x20
+	 * See xHCI 1.0 spec, 5.2.4
+	 */
+	linksystem = LINKSYSTEM_XHCI_VERSION_CONTROL |
+			LINKSYSTEM_FLADJ(0x20);
+	writel(linksystem, regs + EXYNOS5_DRD_LINKSYSTEM);
+
+	phyparam1 = readl(regs + EXYNOS5_DRD_PHYPARAM1);
+	/* Set Tx De-Emphasis level */
+	phyparam1 &= ~PHYPARAM1_PCS_TXDEEMPH_MASK;
+	phyparam1 |= PHYPARAM1_PCS_TXDEEMPH;
+	writel(phyparam1, regs + EXYNOS5_DRD_PHYPARAM1);
+
+	phybatchg = readl(regs + EXYNOS5_DRD_PHYBATCHG);
+	phybatchg |= PHYBATCHG_UTMI_CLKSEL;
+	writel(phybatchg, regs + EXYNOS5_DRD_PHYBATCHG);
+
+	/* PHYTEST POWERDOWN Control */
+	phytest = readl(regs + EXYNOS5_DRD_PHYTEST);
+	phytest &= ~(PHYTEST_POWERDOWN_SSP |
+			PHYTEST_POWERDOWN_HSP);
+	writel(phytest, regs + EXYNOS5_DRD_PHYTEST);
+
+	/* UTMI Power Control */
+	writel(PHYUTMI_OTGDISABLE, regs + EXYNOS5_DRD_PHYUTMI);
+
+	phyclkrst = samsung_usb3phy_set_refclk(sphy);
+
+	phyclkrst |= PHYCLKRST_PORTRESET |
+			/* Digital power supply in normal operating mode */
+			PHYCLKRST_RETENABLEN |
+			/* Enable ref clock for SS function */
+			PHYCLKRST_REF_SSP_EN |
+			/* Enable spread spectrum */
+			PHYCLKRST_SSC_EN |
+			/* Power down HS Bias and PLL blocks in suspend mode */
+			PHYCLKRST_COMMONONN;
+
+	writel(phyclkrst, regs + EXYNOS5_DRD_PHYCLKRST);
+
+	udelay(10);
+
+	phyclkrst &= ~(PHYCLKRST_PORTRESET);
+	writel(phyclkrst, regs + EXYNOS5_DRD_PHYCLKRST);
+
+	return 0;
+}
+
+static void samsung_exynos5_usb3phy_disable(struct samsung_usbphy *sphy)
+{
+	u32 phyutmi;
+	u32 phyclkrst;
+	u32 phytest;
+	void __iomem *regs = sphy->regs;
+
+	phyutmi = PHYUTMI_OTGDISABLE |
+			PHYUTMI_FORCESUSPEND |
+			PHYUTMI_FORCESLEEP;
+	writel(phyutmi, regs + EXYNOS5_DRD_PHYUTMI);
+
+	/* Resetting the PHYCLKRST enable bits to reduce leakage current */
+	phyclkrst = readl(regs + EXYNOS5_DRD_PHYCLKRST);
+	phyclkrst &= ~(PHYCLKRST_REF_SSP_EN |
+			PHYCLKRST_SSC_EN |
+			PHYCLKRST_COMMONONN);
+	writel(phyclkrst, regs + EXYNOS5_DRD_PHYCLKRST);
+
+	/* Control PHYTEST to remove leakage current */
+	phytest = readl(regs + EXYNOS5_DRD_PHYTEST);
+	phytest |= (PHYTEST_POWERDOWN_SSP |
+			PHYTEST_POWERDOWN_HSP);
+	writel(phytest, regs + EXYNOS5_DRD_PHYTEST);
+}
+
+static int samsung_usb3phy_init(struct usb_phy *phy)
+{
+	struct samsung_usbphy *sphy;
+	unsigned long flags;
+	int ret = 0;
+
+	sphy = phy_to_sphy(phy);
+
+	/* Enable the phy clock */
+	ret = clk_prepare_enable(sphy->clk);
+	if (ret) {
+		dev_err(sphy->dev, "%s: clk_prepare_enable failed\n", __func__);
+		return ret;
+	}
+
+	spin_lock_irqsave(&sphy->lock, flags);
+
+	/* setting default phy-type for USB 3.0 */
+	samsung_usbphy_set_type(&sphy->phy, USB_PHY_TYPE_DEVICE);
+
+	/* Disable phy isolation */
+	samsung_usbphy_set_isolation(sphy, false);
+
+	/* Initialize usb phy registers */
+	samsung_exynos5_usb3phy_enable(sphy);
+
+	spin_unlock_irqrestore(&sphy->lock, flags);
+
+	/* Disable the phy clock */
+	clk_disable_unprepare(sphy->clk);
+
+	return ret;
+}
+
+/*
+ * The function passed to the usb driver for phy shutdown
+ */
+static void samsung_usb3phy_shutdown(struct usb_phy *phy)
+{
+	struct samsung_usbphy *sphy;
+	unsigned long flags;
+
+	sphy = phy_to_sphy(phy);
+
+	if (clk_prepare_enable(sphy->clk)) {
+		dev_err(sphy->dev, "%s: clk_prepare_enable failed\n", __func__);
+		return;
+	}
+
+	spin_lock_irqsave(&sphy->lock, flags);
+
+	/* setting default phy-type for USB 3.0 */
+	samsung_usbphy_set_type(&sphy->phy, USB_PHY_TYPE_DEVICE);
+
+	/* De-initialize usb phy registers */
+	samsung_exynos5_usb3phy_disable(sphy);
+
+	/* Enable phy isolation */
+	samsung_usbphy_set_isolation(sphy, true);
+
+	spin_unlock_irqrestore(&sphy->lock, flags);
+
+	clk_disable_unprepare(sphy->clk);
+}
+
+static int samsung_usb3phy_probe(struct platform_device *pdev)
+{
+	struct samsung_usbphy *sphy;
+	struct samsung_usbphy_data *pdata = pdev->dev.platform_data;
+	struct device *dev = &pdev->dev;
+	struct resource *phy_mem;
+	void __iomem	*phy_base;
+	struct clk *clk;
+	int ret;
+
+	phy_mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!phy_mem) {
+		dev_err(dev, "%s: missing mem resource\n", __func__);
+		return -ENODEV;
+	}
+
+	phy_base = devm_ioremap_resource(dev, phy_mem);
+	if (IS_ERR(phy_base))
+		return PTR_ERR(phy_base);
+
+	sphy = devm_kzalloc(dev, sizeof(*sphy), GFP_KERNEL);
+	if (!sphy)
+		return -ENOMEM;
+
+	clk = devm_clk_get(dev, "usbdrd30");
+	if (IS_ERR(clk)) {
+		dev_err(dev, "Failed to get device clock\n");
+		return PTR_ERR(clk);
+	}
+
+	sphy->dev = dev;
+
+	if (dev->of_node) {
+		ret = samsung_usbphy_parse_dt(sphy);
+		if (ret < 0)
+			return ret;
+	} else {
+		if (!pdata) {
+			dev_err(dev, "no platform data specified\n");
+			return -EINVAL;
+		}
+	}
+
+	sphy->plat		= pdata;
+	sphy->regs		= phy_base;
+	sphy->clk		= clk;
+	sphy->phy.dev		= sphy->dev;
+	sphy->phy.label		= "samsung-usb3phy";
+	sphy->phy.init		= samsung_usb3phy_init;
+	sphy->phy.shutdown	= samsung_usb3phy_shutdown;
+	sphy->drv_data		= samsung_usbphy_get_driver_data(pdev);
+	sphy->ref_clk_freq	= samsung_usbphy_get_refclk_freq(sphy);
+
+	spin_lock_init(&sphy->lock);
+
+	platform_set_drvdata(pdev, sphy);
+
+	return usb_add_phy(&sphy->phy, USB_PHY_TYPE_USB3);
+}
+
+static int samsung_usb3phy_remove(struct platform_device *pdev)
+{
+	struct samsung_usbphy *sphy = platform_get_drvdata(pdev);
+
+	usb_remove_phy(&sphy->phy);
+
+	if (sphy->pmuregs)
+		iounmap(sphy->pmuregs);
+	if (sphy->sysreg)
+		iounmap(sphy->sysreg);
+
+	return 0;
+}
+
+static struct samsung_usbphy_drvdata usb3phy_exynos5 = {
+	.cpu_type		= TYPE_EXYNOS5250,
+	.devphy_en_mask		= EXYNOS_USBPHY_ENABLE,
+};
+
+#ifdef CONFIG_OF
+static const struct of_device_id samsung_usbphy_dt_match[] = {
+	{
+		.compatible = "samsung,exynos5250-usb3phy",
+		.data = &usb3phy_exynos5
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, samsung_usbphy_dt_match);
+#endif
+
+static struct platform_device_id samsung_usbphy_driver_ids[] = {
+	{
+		.name		= "exynos5250-usb3phy",
+		.driver_data	= (unsigned long)&usb3phy_exynos5,
+	},
+	{},
+};
+
+MODULE_DEVICE_TABLE(platform, samsung_usbphy_driver_ids);
+
+static struct platform_driver samsung_usb3phy_driver = {
+	.probe		= samsung_usb3phy_probe,
+	.remove		= samsung_usb3phy_remove,
+	.id_table	= samsung_usbphy_driver_ids,
+	.driver		= {
+		.name	= "samsung-usb3phy",
+		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(samsung_usbphy_dt_match),
+	},
+};
+
+module_platform_driver(samsung_usb3phy_driver);
+
+MODULE_DESCRIPTION("Samsung USB 3.0 phy controller");
+MODULE_AUTHOR("Vivek Gautam <gautam.vivek@samsung.com>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:samsung-usb3phy");
diff --git a/drivers/usb/phy/tegra_usb_phy.c b/drivers/usb/phy/phy-tegra-usb.c
index 5487d38481a..17d811292f3 100644
--- a/drivers/usb/phy/tegra_usb_phy.c
+++ b/drivers/usb/phy/phy-tegra-usb.c
@@ -299,7 +299,7 @@ static void utmi_phy_clk_disable(struct tegra_usb_phy *phy)
 		val &= ~USB_SUSP_SET;
 		writel(val, base + USB_SUSP_CTRL);
 	} else
-		tegra_ehci_set_phcd(&phy->u_phy, true);
+		phy->set_phcd(&phy->u_phy, true);
 
 	if (utmi_wait_register(base + USB_SUSP_CTRL, USB_PHY_CLK_VALID, 0) < 0)
 		pr_err("%s: timeout waiting for phy to stabilize\n", __func__);
@@ -321,7 +321,7 @@ static void utmi_phy_clk_enable(struct tegra_usb_phy *phy)
 		val &= ~USB_SUSP_CLR;
 		writel(val, base + USB_SUSP_CTRL);
 	} else
-		tegra_ehci_set_phcd(&phy->u_phy, false);
+		phy->set_phcd(&phy->u_phy, false);
 
 	if (utmi_wait_register(base + USB_SUSP_CTRL, USB_PHY_CLK_VALID,
 						     USB_PHY_CLK_VALID))
@@ -444,7 +444,7 @@ static int utmi_phy_power_on(struct tegra_usb_phy *phy)
 	utmi_phy_clk_enable(phy);
 
 	if (!phy->is_legacy_phy)
-		tegra_ehci_set_pts(&phy->u_phy, 0);
+		phy->set_pts(&phy->u_phy, 0);
 
 	return 0;
 }
@@ -688,7 +688,10 @@ static int	tegra_usb_phy_suspend(struct usb_phy *x, int suspend)
 }
 
 struct tegra_usb_phy *tegra_usb_phy_open(struct device *dev, int instance,
-	void __iomem *regs, void *config, enum tegra_usb_phy_mode phy_mode)
+	void __iomem *regs, void *config, enum tegra_usb_phy_mode phy_mode,
+	void (*set_pts)(struct usb_phy *x, u8 pts_val),
+	void (*set_phcd)(struct usb_phy *x, bool enable))
+
 {
 	struct tegra_usb_phy *phy;
 	unsigned long parent_rate;
@@ -707,6 +710,8 @@ struct tegra_usb_phy *tegra_usb_phy_open(struct device *dev, int instance,
 	phy->dev = dev;
 	phy->is_legacy_phy =
 		of_property_read_bool(np, "nvidia,has-legacy-mode");
+	phy->set_pts = set_pts;
+	phy->set_phcd = set_phcd;
 	err = of_property_match_string(np, "phy_type", "ulpi");
 	if (err < 0)
 		phy->is_ulpi_phy = false;
diff --git a/drivers/usb/otg/twl4030-usb.c b/drivers/usb/phy/phy-twl4030-usb.c
index a994715a310..8f78d2d4072 100644
--- a/drivers/usb/otg/twl4030-usb.c
+++ b/drivers/usb/phy/phy-twl4030-usb.c
@@ -163,6 +163,8 @@ struct twl4030_usb {
 	bool			vbus_supplied;
 	u8			asleep;
 	bool			irq_enabled;
+
+	struct delayed_work	id_workaround_work;
 };
 
 /* internal define on top of container_of */
@@ -246,6 +248,25 @@ twl4030_usb_clear_bits(struct twl4030_usb *twl, u8 reg, u8 bits)
 
 /*-------------------------------------------------------------------------*/
 
+static bool twl4030_is_driving_vbus(struct twl4030_usb *twl)
+{
+	int ret;
+
+	ret = twl4030_usb_read(twl, PHY_CLK_CTRL_STS);
+	if (ret < 0 || !(ret & PHY_DPLL_CLK))
+		/*
+		 * if clocks are off, registers are not updated,
+		 * but we can assume we don't drive VBUS in this case
+		 */
+		return false;
+
+	ret = twl4030_usb_read(twl, ULPI_OTG_CTRL);
+	if (ret < 0)
+		return false;
+
+	return (ret & (ULPI_OTG_DRVVBUS | ULPI_OTG_CHRGVBUS)) ? true : false;
+}
+
 static enum omap_musb_vbus_id_status
 	twl4030_usb_linkstat(struct twl4030_usb *twl)
 {
@@ -268,13 +289,19 @@ static enum omap_musb_vbus_id_status
 	if (status < 0)
 		dev_err(twl->dev, "USB link status err %d\n", status);
 	else if (status & (BIT(7) | BIT(2))) {
-		if (status & (BIT(7)))
-                        twl->vbus_supplied = true;
+		if (status & BIT(7)) {
+			if (twl4030_is_driving_vbus(twl))
+				status &= ~BIT(7);
+			else
+				twl->vbus_supplied = true;
+		}
 
 		if (status & BIT(2))
 			linkstat = OMAP_MUSB_ID_GROUND;
-		else
+		else if (status & BIT(7))
 			linkstat = OMAP_MUSB_VBUS_VALID;
+		else
+			linkstat = OMAP_MUSB_VBUS_OFF;
 	} else {
 		if (twl->linkstat != OMAP_MUSB_UNKNOWN)
 			linkstat = OMAP_MUSB_VBUS_OFF;
@@ -287,10 +314,6 @@ static enum omap_musb_vbus_id_status
 	 * are registered, and that both are active...
 	 */
 
-	spin_lock_irq(&twl->lock);
-	twl->linkstat = linkstat;
-	spin_unlock_irq(&twl->lock);
-
 	return linkstat;
 }
 
@@ -361,9 +384,17 @@ static void __twl4030_phy_power(struct twl4030_usb *twl, int on)
 
 static void twl4030_phy_power(struct twl4030_usb *twl, int on)
 {
+	int ret;
+
 	if (on) {
-		regulator_enable(twl->usb3v1);
-		regulator_enable(twl->usb1v8);
+		ret = regulator_enable(twl->usb3v1);
+		if (ret)
+			dev_err(twl->dev, "Failed to enable usb3v1\n");
+
+		ret = regulator_enable(twl->usb1v8);
+		if (ret)
+			dev_err(twl->dev, "Failed to enable usb1v8\n");
+
 		/*
 		 * Disabling usb3v1 regulator (= writing 0 to VUSB3V1_DEV_GRP
 		 * in twl4030) resets the VUSB_DEDICATED2 register. This reset
@@ -372,7 +403,11 @@ static void twl4030_phy_power(struct twl4030_usb *twl, int on)
 		 * is re-activated. This ensures that VUSB3V1 is really active.
 		 */
 		twl_i2c_write_u8(TWL_MODULE_PM_RECEIVER, 0, VUSB_DEDICATED2);
-		regulator_enable(twl->usb1v5);
+
+		ret = regulator_enable(twl->usb1v5);
+		if (ret)
+			dev_err(twl->dev, "Failed to enable usb1v5\n");
+
 		__twl4030_phy_power(twl, 1);
 		twl4030_usb_write(twl, PHY_CLK_CTRL,
 				  twl4030_usb_read(twl, PHY_CLK_CTRL) |
@@ -412,6 +447,16 @@ static void twl4030_phy_resume(struct twl4030_usb *twl)
 	__twl4030_phy_resume(twl);
 	twl->asleep = 0;
 	dev_dbg(twl->dev, "%s\n", __func__);
+
+	/*
+	 * XXX When VBUS gets driven after musb goes to A mode,
+	 * ID_PRES related interrupts no longer arrive, why?
+	 * Register itself is updated fine though, so we must poll.
+	 */
+	if (twl->linkstat == OMAP_MUSB_ID_GROUND) {
+		cancel_delayed_work(&twl->id_workaround_work);
+		schedule_delayed_work(&twl->id_workaround_work, HZ);
+	}
 }
 
 static int twl4030_usb_ldo_init(struct twl4030_usb *twl)
@@ -432,7 +477,7 @@ static int twl4030_usb_ldo_init(struct twl4030_usb *twl)
 	/* Initialize 3.1V regulator */
 	twl_i2c_write_u8(TWL_MODULE_PM_RECEIVER, 0, VUSB3V1_DEV_GRP);
 
-	twl->usb3v1 = regulator_get(twl->dev, "usb3v1");
+	twl->usb3v1 = devm_regulator_get(twl->dev, "usb3v1");
 	if (IS_ERR(twl->usb3v1))
 		return -ENODEV;
 
@@ -441,18 +486,18 @@ static int twl4030_usb_ldo_init(struct twl4030_usb *twl)
 	/* Initialize 1.5V regulator */
 	twl_i2c_write_u8(TWL_MODULE_PM_RECEIVER, 0, VUSB1V5_DEV_GRP);
 
-	twl->usb1v5 = regulator_get(twl->dev, "usb1v5");
+	twl->usb1v5 = devm_regulator_get(twl->dev, "usb1v5");
 	if (IS_ERR(twl->usb1v5))
-		goto fail1;
+		return -ENODEV;
 
 	twl_i2c_write_u8(TWL_MODULE_PM_RECEIVER, 0, VUSB1V5_TYPE);
 
 	/* Initialize 1.8V regulator */
 	twl_i2c_write_u8(TWL_MODULE_PM_RECEIVER, 0, VUSB1V8_DEV_GRP);
 
-	twl->usb1v8 = regulator_get(twl->dev, "usb1v8");
+	twl->usb1v8 = devm_regulator_get(twl->dev, "usb1v8");
 	if (IS_ERR(twl->usb1v8))
-		goto fail2;
+		return -ENODEV;
 
 	twl_i2c_write_u8(TWL_MODULE_PM_RECEIVER, 0, VUSB1V8_TYPE);
 
@@ -461,14 +506,6 @@ static int twl4030_usb_ldo_init(struct twl4030_usb *twl)
 			 TWL4030_PM_MASTER_PROTECT_KEY);
 
 	return 0;
-
-fail2:
-	regulator_put(twl->usb1v5);
-	twl->usb1v5 = NULL;
-fail1:
-	regulator_put(twl->usb3v1);
-	twl->usb3v1 = NULL;
-	return -ENODEV;
 }
 
 static ssize_t twl4030_usb_vbus_show(struct device *dev,
@@ -491,9 +528,18 @@ static irqreturn_t twl4030_usb_irq(int irq, void *_twl)
 {
 	struct twl4030_usb *twl = _twl;
 	enum omap_musb_vbus_id_status status;
+	bool status_changed = false;
 
 	status = twl4030_usb_linkstat(twl);
-	if (status > 0) {
+
+	spin_lock_irq(&twl->lock);
+	if (status >= 0 && status != twl->linkstat) {
+		twl->linkstat = status;
+		status_changed = true;
+	}
+	spin_unlock_irq(&twl->lock);
+
+	if (status_changed) {
 		/* FIXME add a set_power() method so that B-devices can
 		 * configure the charger appropriately.  It's not always
 		 * correct to consume VBUS power, and how much current to
@@ -505,37 +551,62 @@ static irqreturn_t twl4030_usb_irq(int irq, void *_twl)
 		 * USB_LINK_VBUS state.  musb_hdrc won't care until it
 		 * starts to handle softconnect right.
 		 */
-		if (status == OMAP_MUSB_VBUS_OFF ||
-				status == OMAP_MUSB_ID_FLOAT)
-			twl4030_phy_suspend(twl, 0);
-		else
-			twl4030_phy_resume(twl);
-
-		omap_musb_mailbox(twl->linkstat);
+		omap_musb_mailbox(status);
 	}
 	sysfs_notify(&twl->dev->kobj, NULL, "vbus");
 
 	return IRQ_HANDLED;
 }
 
-static void twl4030_usb_phy_init(struct twl4030_usb *twl)
+static void twl4030_id_workaround_work(struct work_struct *work)
 {
+	struct twl4030_usb *twl = container_of(work, struct twl4030_usb,
+		id_workaround_work.work);
 	enum omap_musb_vbus_id_status status;
+	bool status_changed = false;
 
 	status = twl4030_usb_linkstat(twl);
-	if (status > 0) {
-		if (status == OMAP_MUSB_VBUS_OFF ||
-				status == OMAP_MUSB_ID_FLOAT) {
-			__twl4030_phy_power(twl, 0);
-			twl->asleep = 1;
-		} else {
-			__twl4030_phy_resume(twl);
-			twl->asleep = 0;
-		}
 
-		omap_musb_mailbox(twl->linkstat);
+	spin_lock_irq(&twl->lock);
+	if (status >= 0 && status != twl->linkstat) {
+		twl->linkstat = status;
+		status_changed = true;
+	}
+	spin_unlock_irq(&twl->lock);
+
+	if (status_changed) {
+		dev_dbg(twl->dev, "handle missing status change to %d\n",
+				status);
+		omap_musb_mailbox(status);
 	}
+
+	/* don't schedule during sleep - irq works right then */
+	if (status == OMAP_MUSB_ID_GROUND && !twl->asleep) {
+		cancel_delayed_work(&twl->id_workaround_work);
+		schedule_delayed_work(&twl->id_workaround_work, HZ);
+	}
+}
+
+static int twl4030_usb_phy_init(struct usb_phy *phy)
+{
+	struct twl4030_usb *twl = phy_to_twl(phy);
+	enum omap_musb_vbus_id_status status;
+
+	/*
+	 * Start in sleep state, we'll get called through set_suspend()
+	 * callback when musb is runtime resumed and it's time to start.
+	 */
+	__twl4030_phy_power(twl, 0);
+	twl->asleep = 1;
+
+	status = twl4030_usb_linkstat(twl);
+	twl->linkstat = status;
+
+	if (status == OMAP_MUSB_ID_GROUND || status == OMAP_MUSB_VBUS_VALID)
+		omap_musb_mailbox(twl->linkstat);
+
 	sysfs_notify(&twl->dev->kobj, NULL, "vbus");
+	return 0;
 }
 
 static int twl4030_set_suspend(struct usb_phy *x, int suspend)
@@ -612,6 +683,7 @@ static int twl4030_usb_probe(struct platform_device *pdev)
 	twl->phy.otg		= otg;
 	twl->phy.type		= USB_PHY_TYPE_USB2;
 	twl->phy.set_suspend	= twl4030_set_suspend;
+	twl->phy.init		= twl4030_usb_phy_init;
 
 	otg->phy		= &twl->phy;
 	otg->set_host		= twl4030_set_host;
@@ -620,6 +692,8 @@ static int twl4030_usb_probe(struct platform_device *pdev)
 	/* init spinlock for workqueue */
 	spin_lock_init(&twl->lock);
 
+	INIT_DELAYED_WORK(&twl->id_workaround_work, twl4030_id_workaround_work);
+
 	err = twl4030_usb_ldo_init(twl);
 	if (err) {
 		dev_err(&pdev->dev, "ldo init failed\n");
@@ -640,30 +714,25 @@ static int twl4030_usb_probe(struct platform_device *pdev)
 	 * need both handles, otherwise just one suffices.
 	 */
 	twl->irq_enabled = true;
-	status = request_threaded_irq(twl->irq, NULL, twl4030_usb_irq,
-			IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING |
-			IRQF_ONESHOT, "twl4030_usb", twl);
+	status = devm_request_threaded_irq(twl->dev, twl->irq, NULL,
+			twl4030_usb_irq, IRQF_TRIGGER_FALLING |
+			IRQF_TRIGGER_RISING | IRQF_ONESHOT, "twl4030_usb", twl);
 	if (status < 0) {
 		dev_dbg(&pdev->dev, "can't get IRQ %d, err %d\n",
 			twl->irq, status);
 		return status;
 	}
 
-	/* Power down phy or make it work according to
-	 * current link state.
-	 */
-	twl4030_usb_phy_init(twl);
-
 	dev_info(&pdev->dev, "Initialized TWL4030 USB module\n");
 	return 0;
 }
 
-static int __exit twl4030_usb_remove(struct platform_device *pdev)
+static int twl4030_usb_remove(struct platform_device *pdev)
 {
 	struct twl4030_usb *twl = platform_get_drvdata(pdev);
 	int val;
 
-	free_irq(twl->irq, twl);
+	cancel_delayed_work(&twl->id_workaround_work);
 	device_remove_file(twl->dev, &dev_attr_vbus);
 
 	/* set transceiver mode to power on defaults */
@@ -685,9 +754,6 @@ static int __exit twl4030_usb_remove(struct platform_device *pdev)
 
 	if (!twl->asleep)
 		twl4030_phy_power(twl, 0);
-	regulator_put(twl->usb1v5);
-	regulator_put(twl->usb1v8);
-	regulator_put(twl->usb3v1);
 
 	return 0;
 }
@@ -702,7 +768,7 @@ MODULE_DEVICE_TABLE(of, twl4030_usb_id_table);
 
 static struct platform_driver twl4030_usb_driver = {
 	.probe		= twl4030_usb_probe,
-	.remove		= __exit_p(twl4030_usb_remove),
+	.remove		= twl4030_usb_remove,
 	.driver		= {
 		.name	= "twl4030_usb",
 		.owner	= THIS_MODULE,
diff --git a/drivers/usb/otg/twl6030-usb.c b/drivers/usb/phy/phy-twl6030-usb.c
index 8cd6cf49bdb..9de7ada90a8 100644
--- a/drivers/usb/otg/twl6030-usb.c
+++ b/drivers/usb/phy/phy-twl6030-usb.c
@@ -211,6 +211,7 @@ static irqreturn_t twl6030_usb_irq(int irq, void *_twl)
 	struct twl6030_usb *twl = _twl;
 	enum omap_musb_vbus_id_status status = OMAP_MUSB_UNKNOWN;
 	u8 vbus_state, hw_state;
+	int ret;
 
 	hw_state = twl6030_readb(twl, TWL6030_MODULE_ID0, STS_HW_CONDITIONS);
 
@@ -218,7 +219,10 @@ static irqreturn_t twl6030_usb_irq(int irq, void *_twl)
 						CONTROLLER_STAT1);
 	if (!(hw_state & STS_USB_ID)) {
 		if (vbus_state & VBUS_DET) {
-			regulator_enable(twl->usb3v3);
+			ret = regulator_enable(twl->usb3v3);
+			if (ret)
+				dev_err(twl->dev, "Failed to enable usb3v3\n");
+
 			twl->asleep = 1;
 			status = OMAP_MUSB_VBUS_VALID;
 			twl->linkstat = status;
@@ -245,12 +249,15 @@ static irqreturn_t twl6030_usbotg_irq(int irq, void *_twl)
 	struct twl6030_usb *twl = _twl;
 	enum omap_musb_vbus_id_status status = OMAP_MUSB_UNKNOWN;
 	u8 hw_state;
+	int ret;
 
 	hw_state = twl6030_readb(twl, TWL6030_MODULE_ID0, STS_HW_CONDITIONS);
 
 	if (hw_state & STS_USB_ID) {
+		ret = regulator_enable(twl->usb3v3);
+		if (ret)
+			dev_err(twl->dev, "Failed to enable usb3v3\n");
 
-		regulator_enable(twl->usb3v3);
 		twl->asleep = 1;
 		twl6030_writeb(twl, TWL_MODULE_USB, 0x1, USB_ID_INT_EN_HI_CLR);
 		twl6030_writeb(twl, TWL_MODULE_USB, 0x10, USB_ID_INT_EN_HI_SET);
@@ -393,7 +400,7 @@ static int twl6030_usb_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static int __exit twl6030_usb_remove(struct platform_device *pdev)
+static int twl6030_usb_remove(struct platform_device *pdev)
 {
 	struct twl6030_usb *twl = platform_get_drvdata(pdev);
 
@@ -420,7 +427,7 @@ MODULE_DEVICE_TABLE(of, twl6030_usb_id_table);
 
 static struct platform_driver twl6030_usb_driver = {
 	.probe		= twl6030_usb_probe,
-	.remove		= __exit_p(twl6030_usb_remove),
+	.remove		= twl6030_usb_remove,
 	.driver		= {
 		.name	= "twl6030_usb",
 		.owner	= THIS_MODULE,
diff --git a/drivers/usb/otg/ulpi_viewport.c b/drivers/usb/phy/phy-ulpi-viewport.c
index c5ba7e5423f..c5ba7e5423f 100644
--- a/drivers/usb/otg/ulpi_viewport.c
+++ b/drivers/usb/phy/phy-ulpi-viewport.c
diff --git a/drivers/usb/otg/ulpi.c b/drivers/usb/phy/phy-ulpi.c
index 217339dd7a9..217339dd7a9 100644
--- a/drivers/usb/otg/ulpi.c
+++ b/drivers/usb/phy/phy-ulpi.c
diff --git a/drivers/usb/otg/otg.c b/drivers/usb/phy/phy.c
index 2bd03d261a5..a9984c700d2 100644
--- a/drivers/usb/otg/otg.c
+++ b/drivers/usb/phy/phy.c
@@ -1,14 +1,13 @@
 /*
- * otg.c -- USB OTG utility code
+ * phy.c -- USB phy handling
  *
- * Copyright (C) 2004 Texas Instruments
+ * Copyright (C) 2004-2013 Texas Instruments
  *
  * 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/kernel.h>
 #include <linux/export.h>
 #include <linux/err.h>
@@ -17,7 +16,7 @@
 #include <linux/slab.h>
 #include <linux/of.h>
 
-#include <linux/usb/otg.h>
+#include <linux/usb/phy.h>
 
 static LIST_HEAD(phy_list);
 static LIST_HEAD(phy_bind_list);
@@ -110,7 +109,7 @@ struct usb_phy *devm_usb_get_phy(struct device *dev, enum usb_phy_type type)
 
 	return phy;
 }
-EXPORT_SYMBOL(devm_usb_get_phy);
+EXPORT_SYMBOL_GPL(devm_usb_get_phy);
 
 /**
  * usb_get_phy - find the USB PHY
@@ -143,7 +142,7 @@ err0:
 
 	return phy;
 }
-EXPORT_SYMBOL(usb_get_phy);
+EXPORT_SYMBOL_GPL(usb_get_phy);
 
  /**
  * devm_usb_get_phy_by_phandle - find the USB PHY by phandle
@@ -207,7 +206,7 @@ err0:
 
 	return phy;
 }
-EXPORT_SYMBOL(devm_usb_get_phy_by_phandle);
+EXPORT_SYMBOL_GPL(devm_usb_get_phy_by_phandle);
 
 /**
  * usb_get_phy_dev - find the USB PHY
@@ -240,7 +239,7 @@ err0:
 
 	return phy;
 }
-EXPORT_SYMBOL(usb_get_phy_dev);
+EXPORT_SYMBOL_GPL(usb_get_phy_dev);
 
 /**
  * devm_usb_get_phy_dev - find the USB PHY using device ptr and index
@@ -270,7 +269,7 @@ struct usb_phy *devm_usb_get_phy_dev(struct device *dev, u8 index)
 
 	return phy;
 }
-EXPORT_SYMBOL(devm_usb_get_phy_dev);
+EXPORT_SYMBOL_GPL(devm_usb_get_phy_dev);
 
 /**
  * devm_usb_put_phy - release the USB PHY
@@ -289,7 +288,7 @@ void devm_usb_put_phy(struct device *dev, struct usb_phy *phy)
 	r = devres_destroy(dev, devm_usb_phy_release, devm_usb_phy_match, phy);
 	dev_WARN_ONCE(dev, r, "couldn't find PHY resource\n");
 }
-EXPORT_SYMBOL(devm_usb_put_phy);
+EXPORT_SYMBOL_GPL(devm_usb_put_phy);
 
 /**
  * usb_put_phy - release the USB PHY
@@ -308,7 +307,7 @@ void usb_put_phy(struct usb_phy *x)
 		module_put(owner);
 	}
 }
-EXPORT_SYMBOL(usb_put_phy);
+EXPORT_SYMBOL_GPL(usb_put_phy);
 
 /**
  * usb_add_phy - declare the USB PHY
@@ -348,7 +347,7 @@ out:
 	spin_unlock_irqrestore(&phy_lock, flags);
 	return ret;
 }
-EXPORT_SYMBOL(usb_add_phy);
+EXPORT_SYMBOL_GPL(usb_add_phy);
 
 /**
  * usb_add_phy_dev - declare the USB PHY
@@ -378,7 +377,7 @@ int usb_add_phy_dev(struct usb_phy *x)
 	spin_unlock_irqrestore(&phy_lock, flags);
 	return 0;
 }
-EXPORT_SYMBOL(usb_add_phy_dev);
+EXPORT_SYMBOL_GPL(usb_add_phy_dev);
 
 /**
  * usb_remove_phy - remove the OTG PHY
@@ -400,7 +399,7 @@ void usb_remove_phy(struct usb_phy *x)
 	}
 	spin_unlock_irqrestore(&phy_lock, flags);
 }
-EXPORT_SYMBOL(usb_remove_phy);
+EXPORT_SYMBOL_GPL(usb_remove_phy);
 
 /**
  * usb_bind_phy - bind the phy and the controller that uses the phy
@@ -414,7 +413,7 @@ EXPORT_SYMBOL(usb_remove_phy);
  *
  * To be used by platform specific initialization code.
  */
-int __init usb_bind_phy(const char *dev_name, u8 index,
+int usb_bind_phy(const char *dev_name, u8 index,
 				const char *phy_dev_name)
 {
 	struct usb_phy_bind *phy_bind;
@@ -437,38 +436,3 @@ int __init usb_bind_phy(const char *dev_name, u8 index,
 	return 0;
 }
 EXPORT_SYMBOL_GPL(usb_bind_phy);
-
-const char *otg_state_string(enum usb_otg_state state)
-{
-	switch (state) {
-	case OTG_STATE_A_IDLE:
-		return "a_idle";
-	case OTG_STATE_A_WAIT_VRISE:
-		return "a_wait_vrise";
-	case OTG_STATE_A_WAIT_BCON:
-		return "a_wait_bcon";
-	case OTG_STATE_A_HOST:
-		return "a_host";
-	case OTG_STATE_A_SUSPEND:
-		return "a_suspend";
-	case OTG_STATE_A_PERIPHERAL:
-		return "a_peripheral";
-	case OTG_STATE_A_WAIT_VFALL:
-		return "a_wait_vfall";
-	case OTG_STATE_A_VBUS_ERR:
-		return "a_vbus_err";
-	case OTG_STATE_B_IDLE:
-		return "b_idle";
-	case OTG_STATE_B_SRP_INIT:
-		return "b_srp_init";
-	case OTG_STATE_B_PERIPHERAL:
-		return "b_peripheral";
-	case OTG_STATE_B_WAIT_ACON:
-		return "b_wait_acon";
-	case OTG_STATE_B_HOST:
-		return "b_host";
-	default:
-		return "UNDEFINED";
-	}
-}
-EXPORT_SYMBOL(otg_state_string);
diff --git a/drivers/usb/phy/samsung-usbphy.c b/drivers/usb/phy/samsung-usbphy.c
deleted file mode 100644
index 967101ec15f..00000000000
--- a/drivers/usb/phy/samsung-usbphy.c
+++ /dev/null
@@ -1,928 +0,0 @@
-/* linux/drivers/usb/phy/samsung-usbphy.c
- *
- * Copyright (c) 2012 Samsung Electronics Co., Ltd.
- *              http://www.samsung.com
- *
- * Author: Praveen Paneri <p.paneri@samsung.com>
- *
- * Samsung USB2.0 PHY transceiver; talks to S3C HS OTG controller, EHCI-S5P and
- * OHCI-EXYNOS controllers.
- *
- * 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.
- */
-
-#include <linux/module.h>
-#include <linux/platform_device.h>
-#include <linux/clk.h>
-#include <linux/delay.h>
-#include <linux/device.h>
-#include <linux/err.h>
-#include <linux/io.h>
-#include <linux/of.h>
-#include <linux/of_address.h>
-#include <linux/usb/otg.h>
-#include <linux/usb/samsung_usb_phy.h>
-#include <linux/platform_data/samsung-usbphy.h>
-
-/* Register definitions */
-
-#define SAMSUNG_PHYPWR				(0x00)
-
-#define PHYPWR_NORMAL_MASK			(0x19 << 0)
-#define PHYPWR_OTG_DISABLE			(0x1 << 4)
-#define PHYPWR_ANALOG_POWERDOWN			(0x1 << 3)
-#define PHYPWR_FORCE_SUSPEND			(0x1 << 1)
-/* For Exynos4 */
-#define PHYPWR_NORMAL_MASK_PHY0			(0x39 << 0)
-#define PHYPWR_SLEEP_PHY0			(0x1 << 5)
-
-#define SAMSUNG_PHYCLK				(0x04)
-
-#define PHYCLK_MODE_USB11			(0x1 << 6)
-#define PHYCLK_EXT_OSC				(0x1 << 5)
-#define PHYCLK_COMMON_ON_N			(0x1 << 4)
-#define PHYCLK_ID_PULL				(0x1 << 2)
-#define PHYCLK_CLKSEL_MASK			(0x3 << 0)
-#define PHYCLK_CLKSEL_48M			(0x0 << 0)
-#define PHYCLK_CLKSEL_12M			(0x2 << 0)
-#define PHYCLK_CLKSEL_24M			(0x3 << 0)
-
-#define SAMSUNG_RSTCON				(0x08)
-
-#define RSTCON_PHYLINK_SWRST			(0x1 << 2)
-#define RSTCON_HLINK_SWRST			(0x1 << 1)
-#define RSTCON_SWRST				(0x1 << 0)
-
-/* EXYNOS5 */
-#define EXYNOS5_PHY_HOST_CTRL0			(0x00)
-
-#define HOST_CTRL0_PHYSWRSTALL			(0x1 << 31)
-
-#define HOST_CTRL0_REFCLKSEL_MASK		(0x3 << 19)
-#define HOST_CTRL0_REFCLKSEL_XTAL		(0x0 << 19)
-#define HOST_CTRL0_REFCLKSEL_EXTL		(0x1 << 19)
-#define HOST_CTRL0_REFCLKSEL_CLKCORE		(0x2 << 19)
-
-#define HOST_CTRL0_FSEL_MASK			(0x7 << 16)
-#define HOST_CTRL0_FSEL(_x)			((_x) << 16)
-
-#define FSEL_CLKSEL_50M				(0x7)
-#define FSEL_CLKSEL_24M				(0x5)
-#define FSEL_CLKSEL_20M				(0x4)
-#define FSEL_CLKSEL_19200K			(0x3)
-#define FSEL_CLKSEL_12M				(0x2)
-#define FSEL_CLKSEL_10M				(0x1)
-#define FSEL_CLKSEL_9600K			(0x0)
-
-#define HOST_CTRL0_TESTBURNIN			(0x1 << 11)
-#define HOST_CTRL0_RETENABLE			(0x1 << 10)
-#define HOST_CTRL0_COMMONON_N			(0x1 << 9)
-#define HOST_CTRL0_SIDDQ			(0x1 << 6)
-#define HOST_CTRL0_FORCESLEEP			(0x1 << 5)
-#define HOST_CTRL0_FORCESUSPEND			(0x1 << 4)
-#define HOST_CTRL0_WORDINTERFACE		(0x1 << 3)
-#define HOST_CTRL0_UTMISWRST			(0x1 << 2)
-#define HOST_CTRL0_LINKSWRST			(0x1 << 1)
-#define HOST_CTRL0_PHYSWRST			(0x1 << 0)
-
-#define EXYNOS5_PHY_HOST_TUNE0			(0x04)
-
-#define EXYNOS5_PHY_HSIC_CTRL1			(0x10)
-
-#define EXYNOS5_PHY_HSIC_TUNE1			(0x14)
-
-#define EXYNOS5_PHY_HSIC_CTRL2			(0x20)
-
-#define EXYNOS5_PHY_HSIC_TUNE2			(0x24)
-
-#define HSIC_CTRL_REFCLKSEL_MASK		(0x3 << 23)
-#define HSIC_CTRL_REFCLKSEL			(0x2 << 23)
-
-#define HSIC_CTRL_REFCLKDIV_MASK		(0x7f << 16)
-#define HSIC_CTRL_REFCLKDIV(_x)			((_x) << 16)
-#define HSIC_CTRL_REFCLKDIV_12			(0x24 << 16)
-#define HSIC_CTRL_REFCLKDIV_15			(0x1c << 16)
-#define HSIC_CTRL_REFCLKDIV_16			(0x1a << 16)
-#define HSIC_CTRL_REFCLKDIV_19_2		(0x15 << 16)
-#define HSIC_CTRL_REFCLKDIV_20			(0x14 << 16)
-
-#define HSIC_CTRL_SIDDQ				(0x1 << 6)
-#define HSIC_CTRL_FORCESLEEP			(0x1 << 5)
-#define HSIC_CTRL_FORCESUSPEND			(0x1 << 4)
-#define HSIC_CTRL_WORDINTERFACE			(0x1 << 3)
-#define HSIC_CTRL_UTMISWRST			(0x1 << 2)
-#define HSIC_CTRL_PHYSWRST			(0x1 << 0)
-
-#define EXYNOS5_PHY_HOST_EHCICTRL		(0x30)
-
-#define HOST_EHCICTRL_ENAINCRXALIGN		(0x1 << 29)
-#define HOST_EHCICTRL_ENAINCR4			(0x1 << 28)
-#define HOST_EHCICTRL_ENAINCR8			(0x1 << 27)
-#define HOST_EHCICTRL_ENAINCR16			(0x1 << 26)
-
-#define EXYNOS5_PHY_HOST_OHCICTRL		(0x34)
-
-#define HOST_OHCICTRL_SUSPLGCY			(0x1 << 3)
-#define HOST_OHCICTRL_APPSTARTCLK		(0x1 << 2)
-#define HOST_OHCICTRL_CNTSEL			(0x1 << 1)
-#define HOST_OHCICTRL_CLKCKTRST			(0x1 << 0)
-
-#define EXYNOS5_PHY_OTG_SYS			(0x38)
-
-#define OTG_SYS_PHYLINK_SWRESET			(0x1 << 14)
-#define OTG_SYS_LINKSWRST_UOTG			(0x1 << 13)
-#define OTG_SYS_PHY0_SWRST			(0x1 << 12)
-
-#define OTG_SYS_REFCLKSEL_MASK			(0x3 << 9)
-#define OTG_SYS_REFCLKSEL_XTAL			(0x0 << 9)
-#define OTG_SYS_REFCLKSEL_EXTL			(0x1 << 9)
-#define OTG_SYS_REFCLKSEL_CLKCORE		(0x2 << 9)
-
-#define OTG_SYS_IDPULLUP_UOTG			(0x1 << 8)
-#define OTG_SYS_COMMON_ON			(0x1 << 7)
-
-#define OTG_SYS_FSEL_MASK			(0x7 << 4)
-#define OTG_SYS_FSEL(_x)			((_x) << 4)
-
-#define OTG_SYS_FORCESLEEP			(0x1 << 3)
-#define OTG_SYS_OTGDISABLE			(0x1 << 2)
-#define OTG_SYS_SIDDQ_UOTG			(0x1 << 1)
-#define OTG_SYS_FORCESUSPEND			(0x1 << 0)
-
-#define EXYNOS5_PHY_OTG_TUNE			(0x40)
-
-#ifndef MHZ
-#define MHZ (1000*1000)
-#endif
-
-#ifndef KHZ
-#define KHZ (1000)
-#endif
-
-#define EXYNOS_USBHOST_PHY_CTRL_OFFSET		(0x4)
-#define S3C64XX_USBPHY_ENABLE			(0x1 << 16)
-#define EXYNOS_USBPHY_ENABLE			(0x1 << 0)
-#define EXYNOS_USB20PHY_CFG_HOST_LINK		(0x1 << 0)
-
-enum samsung_cpu_type {
-	TYPE_S3C64XX,
-	TYPE_EXYNOS4210,
-	TYPE_EXYNOS5250,
-};
-
-/*
- * struct samsung_usbphy_drvdata - driver data for various SoC variants
- * @cpu_type: machine identifier
- * @devphy_en_mask: device phy enable mask for PHY CONTROL register
- * @hostphy_en_mask: host phy enable mask for PHY CONTROL register
- * @devphy_reg_offset: offset to DEVICE PHY CONTROL register from
- *		       mapped address of system controller.
- * @hostphy_reg_offset: offset to HOST PHY CONTROL register from
- *		       mapped address of system controller.
- *
- *	Here we have a separate mask for device type phy.
- *	Having different masks for host and device type phy helps
- *	in setting independent masks in case of SoCs like S5PV210,
- *	in which PHY0 and PHY1 enable bits belong to same register
- *	placed at position 0 and 1 respectively.
- *	Although for newer SoCs like exynos these bits belong to
- *	different registers altogether placed at position 0.
- */
-struct samsung_usbphy_drvdata {
-	int cpu_type;
-	int devphy_en_mask;
-	int hostphy_en_mask;
-	u32 devphy_reg_offset;
-	u32 hostphy_reg_offset;
-};
-
-/*
- * struct samsung_usbphy - transceiver driver state
- * @phy: transceiver structure
- * @plat: platform data
- * @dev: The parent device supplied to the probe function
- * @clk: usb phy clock
- * @regs: usb phy controller registers memory base
- * @pmuregs: USB device PHY_CONTROL register memory base
- * @sysreg: USB2.0 PHY_CFG register memory base
- * @ref_clk_freq: reference clock frequency selection
- * @drv_data: driver data available for different SoCs
- * @phy_type: Samsung SoCs specific phy types:	#HOST
- *						#DEVICE
- * @phy_usage: usage count for phy
- * @lock: lock for phy operations
- */
-struct samsung_usbphy {
-	struct usb_phy	phy;
-	struct samsung_usbphy_data *plat;
-	struct device	*dev;
-	struct clk	*clk;
-	void __iomem	*regs;
-	void __iomem	*pmuregs;
-	void __iomem	*sysreg;
-	int		ref_clk_freq;
-	const struct samsung_usbphy_drvdata *drv_data;
-	enum samsung_usb_phy_type phy_type;
-	atomic_t	phy_usage;
-	spinlock_t	lock;
-};
-
-#define phy_to_sphy(x)		container_of((x), struct samsung_usbphy, phy)
-
-int samsung_usbphy_set_host(struct usb_otg *otg, struct usb_bus *host)
-{
-	if (!otg)
-		return -ENODEV;
-
-	if (!otg->host)
-		otg->host = host;
-
-	return 0;
-}
-
-static int samsung_usbphy_parse_dt(struct samsung_usbphy *sphy)
-{
-	struct device_node *usbphy_sys;
-
-	/* Getting node for system controller interface for usb-phy */
-	usbphy_sys = of_get_child_by_name(sphy->dev->of_node, "usbphy-sys");
-	if (!usbphy_sys) {
-		dev_err(sphy->dev, "No sys-controller interface for usb-phy\n");
-		return -ENODEV;
-	}
-
-	sphy->pmuregs = of_iomap(usbphy_sys, 0);
-
-	if (sphy->pmuregs == NULL) {
-		dev_err(sphy->dev, "Can't get usb-phy pmu control register\n");
-		goto err0;
-	}
-
-	sphy->sysreg = of_iomap(usbphy_sys, 1);
-
-	/*
-	 * Not returning error code here, since this situation is not fatal.
-	 * Few SoCs may not have this switch available
-	 */
-	if (sphy->sysreg == NULL)
-		dev_warn(sphy->dev, "Can't get usb-phy sysreg cfg register\n");
-
-	of_node_put(usbphy_sys);
-
-	return 0;
-
-err0:
-	of_node_put(usbphy_sys);
-	return -ENXIO;
-}
-
-/*
- * Set isolation here for phy.
- * Here 'on = true' would mean USB PHY block is isolated, hence
- * de-activated and vice-versa.
- */
-static void samsung_usbphy_set_isolation(struct samsung_usbphy *sphy, bool on)
-{
-	void __iomem *reg = NULL;
-	u32 reg_val;
-	u32 en_mask = 0;
-
-	if (!sphy->pmuregs) {
-		dev_warn(sphy->dev, "Can't set pmu isolation\n");
-		return;
-	}
-
-	switch (sphy->drv_data->cpu_type) {
-	case TYPE_S3C64XX:
-		/*
-		 * Do nothing: We will add here once S3C64xx goes for DT support
-		 */
-		break;
-	case TYPE_EXYNOS4210:
-		/*
-		 * Fall through since exynos4210 and exynos5250 have similar
-		 * register architecture: two separate registers for host and
-		 * device phy control with enable bit at position 0.
-		 */
-	case TYPE_EXYNOS5250:
-		if (sphy->phy_type == USB_PHY_TYPE_DEVICE) {
-			reg = sphy->pmuregs +
-				sphy->drv_data->devphy_reg_offset;
-			en_mask = sphy->drv_data->devphy_en_mask;
-		} else if (sphy->phy_type == USB_PHY_TYPE_HOST) {
-			reg = sphy->pmuregs +
-				sphy->drv_data->hostphy_reg_offset;
-			en_mask = sphy->drv_data->hostphy_en_mask;
-		}
-		break;
-	default:
-		dev_err(sphy->dev, "Invalid SoC type\n");
-		return;
-	}
-
-	reg_val = readl(reg);
-
-	if (on)
-		reg_val &= ~en_mask;
-	else
-		reg_val |= en_mask;
-
-	writel(reg_val, reg);
-}
-
-/*
- * Configure the mode of working of usb-phy here: HOST/DEVICE.
- */
-static void samsung_usbphy_cfg_sel(struct samsung_usbphy *sphy)
-{
-	u32 reg;
-
-	if (!sphy->sysreg) {
-		dev_warn(sphy->dev, "Can't configure specified phy mode\n");
-		return;
-	}
-
-	reg = readl(sphy->sysreg);
-
-	if (sphy->phy_type == USB_PHY_TYPE_DEVICE)
-		reg &= ~EXYNOS_USB20PHY_CFG_HOST_LINK;
-	else if (sphy->phy_type == USB_PHY_TYPE_HOST)
-		reg |= EXYNOS_USB20PHY_CFG_HOST_LINK;
-
-	writel(reg, sphy->sysreg);
-}
-
-/*
- * PHYs are different for USB Device and USB Host.
- * This make sure that correct PHY type is selected before
- * any operation on PHY.
- */
-static int samsung_usbphy_set_type(struct usb_phy *phy,
-				enum samsung_usb_phy_type phy_type)
-{
-	struct samsung_usbphy *sphy = phy_to_sphy(phy);
-
-	sphy->phy_type = phy_type;
-
-	return 0;
-}
-
-/*
- * Returns reference clock frequency selection value
- */
-static int samsung_usbphy_get_refclk_freq(struct samsung_usbphy *sphy)
-{
-	struct clk *ref_clk;
-	int refclk_freq = 0;
-
-	/*
-	 * In exynos5250 USB host and device PHY use
-	 * external crystal clock XXTI
-	 */
-	if (sphy->drv_data->cpu_type == TYPE_EXYNOS5250)
-		ref_clk = clk_get(sphy->dev, "ext_xtal");
-	else
-		ref_clk = clk_get(sphy->dev, "xusbxti");
-	if (IS_ERR(ref_clk)) {
-		dev_err(sphy->dev, "Failed to get reference clock\n");
-		return PTR_ERR(ref_clk);
-	}
-
-	if (sphy->drv_data->cpu_type == TYPE_EXYNOS5250) {
-		/* set clock frequency for PLL */
-		switch (clk_get_rate(ref_clk)) {
-		case 9600 * KHZ:
-			refclk_freq = FSEL_CLKSEL_9600K;
-			break;
-		case 10 * MHZ:
-			refclk_freq = FSEL_CLKSEL_10M;
-			break;
-		case 12 * MHZ:
-			refclk_freq = FSEL_CLKSEL_12M;
-			break;
-		case 19200 * KHZ:
-			refclk_freq = FSEL_CLKSEL_19200K;
-			break;
-		case 20 * MHZ:
-			refclk_freq = FSEL_CLKSEL_20M;
-			break;
-		case 50 * MHZ:
-			refclk_freq = FSEL_CLKSEL_50M;
-			break;
-		case 24 * MHZ:
-		default:
-			/* default reference clock */
-			refclk_freq = FSEL_CLKSEL_24M;
-			break;
-		}
-	} else {
-		switch (clk_get_rate(ref_clk)) {
-		case 12 * MHZ:
-			refclk_freq = PHYCLK_CLKSEL_12M;
-			break;
-		case 24 * MHZ:
-			refclk_freq = PHYCLK_CLKSEL_24M;
-			break;
-		case 48 * MHZ:
-			refclk_freq = PHYCLK_CLKSEL_48M;
-			break;
-		default:
-			if (sphy->drv_data->cpu_type == TYPE_S3C64XX)
-				refclk_freq = PHYCLK_CLKSEL_48M;
-			else
-				refclk_freq = PHYCLK_CLKSEL_24M;
-			break;
-		}
-	}
-	clk_put(ref_clk);
-
-	return refclk_freq;
-}
-
-static bool exynos5_phyhost_is_on(void *regs)
-{
-	u32 reg;
-
-	reg = readl(regs + EXYNOS5_PHY_HOST_CTRL0);
-
-	return !(reg & HOST_CTRL0_SIDDQ);
-}
-
-static void samsung_exynos5_usbphy_enable(struct samsung_usbphy *sphy)
-{
-	void __iomem *regs = sphy->regs;
-	u32 phyclk = sphy->ref_clk_freq;
-	u32 phyhost;
-	u32 phyotg;
-	u32 phyhsic;
-	u32 ehcictrl;
-	u32 ohcictrl;
-
-	/*
-	 * phy_usage helps in keeping usage count for phy
-	 * so that the first consumer enabling the phy is also
-	 * the last consumer to disable it.
-	 */
-
-	atomic_inc(&sphy->phy_usage);
-
-	if (exynos5_phyhost_is_on(regs)) {
-		dev_info(sphy->dev, "Already power on PHY\n");
-		return;
-	}
-
-	/* Host configuration */
-	phyhost = readl(regs + EXYNOS5_PHY_HOST_CTRL0);
-
-	/* phy reference clock configuration */
-	phyhost &= ~HOST_CTRL0_FSEL_MASK;
-	phyhost |= HOST_CTRL0_FSEL(phyclk);
-
-	/* host phy reset */
-	phyhost &= ~(HOST_CTRL0_PHYSWRST |
-			HOST_CTRL0_PHYSWRSTALL |
-			HOST_CTRL0_SIDDQ |
-			/* Enable normal mode of operation */
-			HOST_CTRL0_FORCESUSPEND |
-			HOST_CTRL0_FORCESLEEP);
-
-	/* Link reset */
-	phyhost |= (HOST_CTRL0_LINKSWRST |
-			HOST_CTRL0_UTMISWRST |
-			/* COMMON Block configuration during suspend */
-			HOST_CTRL0_COMMONON_N);
-	writel(phyhost, regs + EXYNOS5_PHY_HOST_CTRL0);
-	udelay(10);
-	phyhost &= ~(HOST_CTRL0_LINKSWRST |
-			HOST_CTRL0_UTMISWRST);
-	writel(phyhost, regs + EXYNOS5_PHY_HOST_CTRL0);
-
-	/* OTG configuration */
-	phyotg = readl(regs + EXYNOS5_PHY_OTG_SYS);
-
-	/* phy reference clock configuration */
-	phyotg &= ~OTG_SYS_FSEL_MASK;
-	phyotg |= OTG_SYS_FSEL(phyclk);
-
-	/* Enable normal mode of operation */
-	phyotg &= ~(OTG_SYS_FORCESUSPEND |
-			OTG_SYS_SIDDQ_UOTG |
-			OTG_SYS_FORCESLEEP |
-			OTG_SYS_REFCLKSEL_MASK |
-			/* COMMON Block configuration during suspend */
-			OTG_SYS_COMMON_ON);
-
-	/* OTG phy & link reset */
-	phyotg |= (OTG_SYS_PHY0_SWRST |
-			OTG_SYS_LINKSWRST_UOTG |
-			OTG_SYS_PHYLINK_SWRESET |
-			OTG_SYS_OTGDISABLE |
-			/* Set phy refclk */
-			OTG_SYS_REFCLKSEL_CLKCORE);
-
-	writel(phyotg, regs + EXYNOS5_PHY_OTG_SYS);
-	udelay(10);
-	phyotg &= ~(OTG_SYS_PHY0_SWRST |
-			OTG_SYS_LINKSWRST_UOTG |
-			OTG_SYS_PHYLINK_SWRESET);
-	writel(phyotg, regs + EXYNOS5_PHY_OTG_SYS);
-
-	/* HSIC phy configuration */
-	phyhsic = (HSIC_CTRL_REFCLKDIV_12 |
-			HSIC_CTRL_REFCLKSEL |
-			HSIC_CTRL_PHYSWRST);
-	writel(phyhsic, regs + EXYNOS5_PHY_HSIC_CTRL1);
-	writel(phyhsic, regs + EXYNOS5_PHY_HSIC_CTRL2);
-	udelay(10);
-	phyhsic &= ~HSIC_CTRL_PHYSWRST;
-	writel(phyhsic, regs + EXYNOS5_PHY_HSIC_CTRL1);
-	writel(phyhsic, regs + EXYNOS5_PHY_HSIC_CTRL2);
-
-	udelay(80);
-
-	/* enable EHCI DMA burst */
-	ehcictrl = readl(regs + EXYNOS5_PHY_HOST_EHCICTRL);
-	ehcictrl |= (HOST_EHCICTRL_ENAINCRXALIGN |
-				HOST_EHCICTRL_ENAINCR4 |
-				HOST_EHCICTRL_ENAINCR8 |
-				HOST_EHCICTRL_ENAINCR16);
-	writel(ehcictrl, regs + EXYNOS5_PHY_HOST_EHCICTRL);
-
-	/* set ohci_suspend_on_n */
-	ohcictrl = readl(regs + EXYNOS5_PHY_HOST_OHCICTRL);
-	ohcictrl |= HOST_OHCICTRL_SUSPLGCY;
-	writel(ohcictrl, regs + EXYNOS5_PHY_HOST_OHCICTRL);
-}
-
-static void samsung_usbphy_enable(struct samsung_usbphy *sphy)
-{
-	void __iomem *regs = sphy->regs;
-	u32 phypwr;
-	u32 phyclk;
-	u32 rstcon;
-
-	/* set clock frequency for PLL */
-	phyclk = sphy->ref_clk_freq;
-	phypwr = readl(regs + SAMSUNG_PHYPWR);
-	rstcon = readl(regs + SAMSUNG_RSTCON);
-
-	switch (sphy->drv_data->cpu_type) {
-	case TYPE_S3C64XX:
-		phyclk &= ~PHYCLK_COMMON_ON_N;
-		phypwr &= ~PHYPWR_NORMAL_MASK;
-		rstcon |= RSTCON_SWRST;
-		break;
-	case TYPE_EXYNOS4210:
-		phypwr &= ~PHYPWR_NORMAL_MASK_PHY0;
-		rstcon |= RSTCON_SWRST;
-	default:
-		break;
-	}
-
-	writel(phyclk, regs + SAMSUNG_PHYCLK);
-	/* Configure PHY0 for normal operation*/
-	writel(phypwr, regs + SAMSUNG_PHYPWR);
-	/* reset all ports of PHY and Link */
-	writel(rstcon, regs + SAMSUNG_RSTCON);
-	udelay(10);
-	rstcon &= ~RSTCON_SWRST;
-	writel(rstcon, regs + SAMSUNG_RSTCON);
-}
-
-static void samsung_exynos5_usbphy_disable(struct samsung_usbphy *sphy)
-{
-	void __iomem *regs = sphy->regs;
-	u32 phyhost;
-	u32 phyotg;
-	u32 phyhsic;
-
-	if (atomic_dec_return(&sphy->phy_usage) > 0) {
-		dev_info(sphy->dev, "still being used\n");
-		return;
-	}
-
-	phyhsic = (HSIC_CTRL_REFCLKDIV_12 |
-			HSIC_CTRL_REFCLKSEL |
-			HSIC_CTRL_SIDDQ |
-			HSIC_CTRL_FORCESLEEP |
-			HSIC_CTRL_FORCESUSPEND);
-	writel(phyhsic, regs + EXYNOS5_PHY_HSIC_CTRL1);
-	writel(phyhsic, regs + EXYNOS5_PHY_HSIC_CTRL2);
-
-	phyhost = readl(regs + EXYNOS5_PHY_HOST_CTRL0);
-	phyhost |= (HOST_CTRL0_SIDDQ |
-			HOST_CTRL0_FORCESUSPEND |
-			HOST_CTRL0_FORCESLEEP |
-			HOST_CTRL0_PHYSWRST |
-			HOST_CTRL0_PHYSWRSTALL);
-	writel(phyhost, regs + EXYNOS5_PHY_HOST_CTRL0);
-
-	phyotg = readl(regs + EXYNOS5_PHY_OTG_SYS);
-	phyotg |= (OTG_SYS_FORCESUSPEND |
-			OTG_SYS_SIDDQ_UOTG |
-			OTG_SYS_FORCESLEEP);
-	writel(phyotg, regs + EXYNOS5_PHY_OTG_SYS);
-}
-
-static void samsung_usbphy_disable(struct samsung_usbphy *sphy)
-{
-	void __iomem *regs = sphy->regs;
-	u32 phypwr;
-
-	phypwr = readl(regs + SAMSUNG_PHYPWR);
-
-	switch (sphy->drv_data->cpu_type) {
-	case TYPE_S3C64XX:
-		phypwr |= PHYPWR_NORMAL_MASK;
-		break;
-	case TYPE_EXYNOS4210:
-		phypwr |= PHYPWR_NORMAL_MASK_PHY0;
-	default:
-		break;
-	}
-
-	/* Disable analog and otg block power */
-	writel(phypwr, regs + SAMSUNG_PHYPWR);
-}
-
-/*
- * The function passed to the usb driver for phy initialization
- */
-static int samsung_usbphy_init(struct usb_phy *phy)
-{
-	struct samsung_usbphy *sphy;
-	struct usb_bus *host = NULL;
-	unsigned long flags;
-	int ret = 0;
-
-	sphy = phy_to_sphy(phy);
-
-	host = phy->otg->host;
-
-	/* Enable the phy clock */
-	ret = clk_prepare_enable(sphy->clk);
-	if (ret) {
-		dev_err(sphy->dev, "%s: clk_prepare_enable failed\n", __func__);
-		return ret;
-	}
-
-	spin_lock_irqsave(&sphy->lock, flags);
-
-	if (host) {
-		/* setting default phy-type for USB 2.0 */
-		if (!strstr(dev_name(host->controller), "ehci") ||
-				!strstr(dev_name(host->controller), "ohci"))
-			samsung_usbphy_set_type(&sphy->phy, USB_PHY_TYPE_HOST);
-	} else {
-		samsung_usbphy_set_type(&sphy->phy, USB_PHY_TYPE_DEVICE);
-	}
-
-	/* Disable phy isolation */
-	if (sphy->plat && sphy->plat->pmu_isolation)
-		sphy->plat->pmu_isolation(false);
-	else
-		samsung_usbphy_set_isolation(sphy, false);
-
-	/* Selecting Host/OTG mode; After reset USB2.0PHY_CFG: HOST */
-	samsung_usbphy_cfg_sel(sphy);
-
-	/* Initialize usb phy registers */
-	if (sphy->drv_data->cpu_type == TYPE_EXYNOS5250)
-		samsung_exynos5_usbphy_enable(sphy);
-	else
-		samsung_usbphy_enable(sphy);
-
-	spin_unlock_irqrestore(&sphy->lock, flags);
-
-	/* Disable the phy clock */
-	clk_disable_unprepare(sphy->clk);
-
-	return ret;
-}
-
-/*
- * The function passed to the usb driver for phy shutdown
- */
-static void samsung_usbphy_shutdown(struct usb_phy *phy)
-{
-	struct samsung_usbphy *sphy;
-	struct usb_bus *host = NULL;
-	unsigned long flags;
-
-	sphy = phy_to_sphy(phy);
-
-	host = phy->otg->host;
-
-	if (clk_prepare_enable(sphy->clk)) {
-		dev_err(sphy->dev, "%s: clk_prepare_enable failed\n", __func__);
-		return;
-	}
-
-	spin_lock_irqsave(&sphy->lock, flags);
-
-	if (host) {
-		/* setting default phy-type for USB 2.0 */
-		if (!strstr(dev_name(host->controller), "ehci") ||
-				!strstr(dev_name(host->controller), "ohci"))
-			samsung_usbphy_set_type(&sphy->phy, USB_PHY_TYPE_HOST);
-	} else {
-		samsung_usbphy_set_type(&sphy->phy, USB_PHY_TYPE_DEVICE);
-	}
-
-	/* De-initialize usb phy registers */
-	if (sphy->drv_data->cpu_type == TYPE_EXYNOS5250)
-		samsung_exynos5_usbphy_disable(sphy);
-	else
-		samsung_usbphy_disable(sphy);
-
-	/* Enable phy isolation */
-	if (sphy->plat && sphy->plat->pmu_isolation)
-		sphy->plat->pmu_isolation(true);
-	else
-		samsung_usbphy_set_isolation(sphy, true);
-
-	spin_unlock_irqrestore(&sphy->lock, flags);
-
-	clk_disable_unprepare(sphy->clk);
-}
-
-static const struct of_device_id samsung_usbphy_dt_match[];
-
-static inline const struct samsung_usbphy_drvdata
-*samsung_usbphy_get_driver_data(struct platform_device *pdev)
-{
-	if (pdev->dev.of_node) {
-		const struct of_device_id *match;
-		match = of_match_node(samsung_usbphy_dt_match,
-							pdev->dev.of_node);
-		return match->data;
-	}
-
-	return (struct samsung_usbphy_drvdata *)
-				platform_get_device_id(pdev)->driver_data;
-}
-
-static int samsung_usbphy_probe(struct platform_device *pdev)
-{
-	struct samsung_usbphy *sphy;
-	struct usb_otg *otg;
-	struct samsung_usbphy_data *pdata = pdev->dev.platform_data;
-	const struct samsung_usbphy_drvdata *drv_data;
-	struct device *dev = &pdev->dev;
-	struct resource *phy_mem;
-	void __iomem	*phy_base;
-	struct clk *clk;
-	int ret;
-
-	phy_mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!phy_mem) {
-		dev_err(dev, "%s: missing mem resource\n", __func__);
-		return -ENODEV;
-	}
-
-	phy_base = devm_ioremap_resource(dev, phy_mem);
-	if (IS_ERR(phy_base))
-		return PTR_ERR(phy_base);
-
-	sphy = devm_kzalloc(dev, sizeof(*sphy), GFP_KERNEL);
-	if (!sphy)
-		return -ENOMEM;
-
-	otg = devm_kzalloc(dev, sizeof(*otg), GFP_KERNEL);
-	if (!otg)
-		return -ENOMEM;
-
-	drv_data = samsung_usbphy_get_driver_data(pdev);
-
-	if (drv_data->cpu_type == TYPE_EXYNOS5250)
-		clk = devm_clk_get(dev, "usbhost");
-	else
-		clk = devm_clk_get(dev, "otg");
-
-	if (IS_ERR(clk)) {
-		dev_err(dev, "Failed to get otg clock\n");
-		return PTR_ERR(clk);
-	}
-
-	sphy->dev = dev;
-
-	if (dev->of_node) {
-		ret = samsung_usbphy_parse_dt(sphy);
-		if (ret < 0)
-			return ret;
-	} else {
-		if (!pdata) {
-			dev_err(dev, "no platform data specified\n");
-			return -EINVAL;
-		}
-	}
-
-	sphy->plat		= pdata;
-	sphy->regs		= phy_base;
-	sphy->clk		= clk;
-	sphy->drv_data		= drv_data;
-	sphy->phy.dev		= sphy->dev;
-	sphy->phy.label		= "samsung-usbphy";
-	sphy->phy.init		= samsung_usbphy_init;
-	sphy->phy.shutdown	= samsung_usbphy_shutdown;
-	sphy->ref_clk_freq	= samsung_usbphy_get_refclk_freq(sphy);
-
-	sphy->phy.otg		= otg;
-	sphy->phy.otg->phy	= &sphy->phy;
-	sphy->phy.otg->set_host = samsung_usbphy_set_host;
-
-	spin_lock_init(&sphy->lock);
-
-	platform_set_drvdata(pdev, sphy);
-
-	return usb_add_phy(&sphy->phy, USB_PHY_TYPE_USB2);
-}
-
-static int samsung_usbphy_remove(struct platform_device *pdev)
-{
-	struct samsung_usbphy *sphy = platform_get_drvdata(pdev);
-
-	usb_remove_phy(&sphy->phy);
-
-	if (sphy->pmuregs)
-		iounmap(sphy->pmuregs);
-	if (sphy->sysreg)
-		iounmap(sphy->sysreg);
-
-	return 0;
-}
-
-static const struct samsung_usbphy_drvdata usbphy_s3c64xx = {
-	.cpu_type		= TYPE_S3C64XX,
-	.devphy_en_mask		= S3C64XX_USBPHY_ENABLE,
-};
-
-static const struct samsung_usbphy_drvdata usbphy_exynos4 = {
-	.cpu_type		= TYPE_EXYNOS4210,
-	.devphy_en_mask		= EXYNOS_USBPHY_ENABLE,
-	.hostphy_en_mask	= EXYNOS_USBPHY_ENABLE,
-};
-
-static struct samsung_usbphy_drvdata usbphy_exynos5 = {
-	.cpu_type		= TYPE_EXYNOS5250,
-	.hostphy_en_mask	= EXYNOS_USBPHY_ENABLE,
-	.hostphy_reg_offset	= EXYNOS_USBHOST_PHY_CTRL_OFFSET,
-};
-
-#ifdef CONFIG_OF
-static const struct of_device_id samsung_usbphy_dt_match[] = {
-	{
-		.compatible = "samsung,s3c64xx-usbphy",
-		.data = &usbphy_s3c64xx,
-	}, {
-		.compatible = "samsung,exynos4210-usbphy",
-		.data = &usbphy_exynos4,
-	}, {
-		.compatible = "samsung,exynos5250-usbphy",
-		.data = &usbphy_exynos5
-	},
-	{},
-};
-MODULE_DEVICE_TABLE(of, samsung_usbphy_dt_match);
-#endif
-
-static struct platform_device_id samsung_usbphy_driver_ids[] = {
-	{
-		.name		= "s3c64xx-usbphy",
-		.driver_data	= (unsigned long)&usbphy_s3c64xx,
-	}, {
-		.name		= "exynos4210-usbphy",
-		.driver_data	= (unsigned long)&usbphy_exynos4,
-	}, {
-		.name		= "exynos5250-usbphy",
-		.driver_data	= (unsigned long)&usbphy_exynos5,
-	},
-	{},
-};
-
-MODULE_DEVICE_TABLE(platform, samsung_usbphy_driver_ids);
-
-static struct platform_driver samsung_usbphy_driver = {
-	.probe		= samsung_usbphy_probe,
-	.remove		= samsung_usbphy_remove,
-	.id_table	= samsung_usbphy_driver_ids,
-	.driver		= {
-		.name	= "samsung-usbphy",
-		.owner	= THIS_MODULE,
-		.of_match_table = of_match_ptr(samsung_usbphy_dt_match),
-	},
-};
-
-module_platform_driver(samsung_usbphy_driver);
-
-MODULE_DESCRIPTION("Samsung USB phy controller");
-MODULE_AUTHOR("Praveen Paneri <p.paneri@samsung.com>");
-MODULE_LICENSE("GPL");
-MODULE_ALIAS("platform:samsung-usbphy");
diff --git a/drivers/usb/renesas_usbhs/Kconfig b/drivers/usb/renesas_usbhs/Kconfig
index 29feb00d7f3..019bf7e49ee 100644
--- a/drivers/usb/renesas_usbhs/Kconfig
+++ b/drivers/usb/renesas_usbhs/Kconfig
@@ -4,7 +4,7 @@
 
 config USB_RENESAS_USBHS
 	tristate 'Renesas USBHS controller'
-	depends on USB && USB_GADGET && GENERIC_HARDIRQS
+	depends on USB_GADGET && GENERIC_HARDIRQS
 	default n
 	help
 	  Renesas USBHS is a discrete USB host and peripheral controller chip
diff --git a/drivers/usb/renesas_usbhs/fifo.c b/drivers/usb/renesas_usbhs/fifo.c
index 9538f0feafe..45b94019aec 100644
--- a/drivers/usb/renesas_usbhs/fifo.c
+++ b/drivers/usb/renesas_usbhs/fifo.c
@@ -32,7 +32,6 @@
  */
 void usbhs_pkt_init(struct usbhs_pkt *pkt)
 {
-	pkt->dma = DMA_ADDR_INVALID;
 	INIT_LIST_HEAD(&pkt->node);
 }
 
diff --git a/drivers/usb/renesas_usbhs/fifo.h b/drivers/usb/renesas_usbhs/fifo.h
index c31731a843d..a168a1760fc 100644
--- a/drivers/usb/renesas_usbhs/fifo.h
+++ b/drivers/usb/renesas_usbhs/fifo.h
@@ -23,8 +23,6 @@
 #include <asm/dma.h>
 #include "pipe.h"
 
-#define	DMA_ADDR_INVALID	(~(dma_addr_t)0)
-
 struct usbhs_fifo {
 	char *name;
 	u32 port;	/* xFIFO */
diff --git a/drivers/usb/renesas_usbhs/mod_gadget.c b/drivers/usb/renesas_usbhs/mod_gadget.c
index 78fca978b2d..ed4949faa70 100644
--- a/drivers/usb/renesas_usbhs/mod_gadget.c
+++ b/drivers/usb/renesas_usbhs/mod_gadget.c
@@ -230,7 +230,7 @@ static int usbhsg_recip_handler_std_clear_endpoint(struct usbhs_priv *priv,
 	return 0;
 }
 
-struct usbhsg_recip_handle req_clear_feature = {
+static struct usbhsg_recip_handle req_clear_feature = {
 	.name		= "clear feature",
 	.device		= usbhsg_recip_handler_std_control_done,
 	.interface	= usbhsg_recip_handler_std_control_done,
@@ -271,7 +271,7 @@ static int usbhsg_recip_handler_std_set_endpoint(struct usbhs_priv *priv,
 	return 0;
 }
 
-struct usbhsg_recip_handle req_set_feature = {
+static struct usbhsg_recip_handle req_set_feature = {
 	.name		= "set feature",
 	.device		= usbhsg_recip_handler_std_set_device,
 	.interface	= usbhsg_recip_handler_std_control_done,
@@ -372,7 +372,7 @@ static int usbhsg_recip_handler_std_get_endpoint(struct usbhs_priv *priv,
 	return 0;
 }
 
-struct usbhsg_recip_handle req_get_status = {
+static struct usbhsg_recip_handle req_get_status = {
 	.name		= "get status",
 	.device		= usbhsg_recip_handler_std_get_device,
 	.interface	= usbhsg_recip_handler_std_get_interface,
@@ -845,7 +845,6 @@ static int usbhsg_gadget_start(struct usb_gadget *gadget,
 
 	/* first hook up the driver ... */
 	gpriv->driver = driver;
-	gpriv->gadget.dev.driver = &driver->driver;
 
 	return usbhsg_try_start(priv, USBHSG_STATUS_REGISTERD);
 }
@@ -861,7 +860,6 @@ static int usbhsg_gadget_stop(struct usb_gadget *gadget,
 		return -EINVAL;
 
 	usbhsg_try_stop(priv, USBHSG_STATUS_REGISTERD);
-	gpriv->gadget.dev.driver = NULL;
 	gpriv->driver = NULL;
 
 	return 0;
@@ -925,11 +923,6 @@ static int usbhsg_stop(struct usbhs_priv *priv)
 	return usbhsg_try_stop(priv, USBHSG_STATUS_STARTED);
 }
 
-static void usbhs_mod_gadget_release(struct device *pdev)
-{
-	/* do nothing */
-}
-
 int usbhs_mod_gadget_probe(struct usbhs_priv *priv)
 {
 	struct usbhsg_gpriv *gpriv;
@@ -976,15 +969,10 @@ int usbhs_mod_gadget_probe(struct usbhs_priv *priv)
 	/*
 	 * init gadget
 	 */
-	dev_set_name(&gpriv->gadget.dev, "gadget");
 	gpriv->gadget.dev.parent	= dev;
-	gpriv->gadget.dev.release	= usbhs_mod_gadget_release;
 	gpriv->gadget.name		= "renesas_usbhs_udc";
 	gpriv->gadget.ops		= &usbhsg_gadget_ops;
 	gpriv->gadget.max_speed		= USB_SPEED_HIGH;
-	ret = device_register(&gpriv->gadget.dev);
-	if (ret < 0)
-		goto err_add_udc;
 
 	INIT_LIST_HEAD(&gpriv->gadget.ep_list);
 
@@ -1014,15 +1002,13 @@ int usbhs_mod_gadget_probe(struct usbhs_priv *priv)
 
 	ret = usb_add_gadget_udc(dev, &gpriv->gadget);
 	if (ret)
-		goto err_register;
+		goto err_add_udc;
 
 
 	dev_info(dev, "gadget probed\n");
 
 	return 0;
 
-err_register:
-	device_unregister(&gpriv->gadget.dev);
 err_add_udc:
 	kfree(gpriv->uep);
 
@@ -1038,8 +1024,6 @@ void usbhs_mod_gadget_remove(struct usbhs_priv *priv)
 
 	usb_del_gadget_udc(&gpriv->gadget);
 
-	device_unregister(&gpriv->gadget.dev);
-
 	kfree(gpriv->uep);
 	kfree(gpriv);
 }
diff --git a/drivers/usb/serial/Kconfig b/drivers/usb/serial/Kconfig
index 17b7f9ae36a..1d55762afbb 100644
--- a/drivers/usb/serial/Kconfig
+++ b/drivers/usb/serial/Kconfig
@@ -4,7 +4,7 @@
 
 menuconfig USB_SERIAL
 	tristate "USB Serial Converter support"
-	depends on USB && TTY
+	depends on TTY
 	---help---
 	  Say Y here if you have a USB device that provides normal serial
 	  ports, or acts like a serial device, and you want to connect it to
@@ -667,6 +667,23 @@ config USB_SERIAL_ZIO
 	  To compile this driver as a module, choose M here: the
 	  module will be called zio.
 
+config USB_SERIAL_WISHBONE
+	tristate "USB-Wishbone adapter interface driver"
+	help
+	  Say Y here if you want to use a USB attached Wishbone bus.
+
+	  Wishbone is an open hardware SoC bus commonly used in FPGA
+	  designs. Bus access can be serialized using the Etherbone
+	  protocol <http://www.ohwr.org/projects/etherbone-core>.
+
+	  This driver is intended to be used with devices which attach
+	  their internal Wishbone bus to a USB serial interface using
+	  the Etherbone protocol. A userspace library is required to
+	  speak the protocol made available by this driver as ttyUSBx.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called wishbone-serial.
+
 config USB_SERIAL_ZTE
 	tristate "ZTE USB serial driver"
 	help
diff --git a/drivers/usb/serial/Makefile b/drivers/usb/serial/Makefile
index eaf5ca14dfe..cec63fa1910 100644
--- a/drivers/usb/serial/Makefile
+++ b/drivers/usb/serial/Makefile
@@ -58,6 +58,7 @@ obj-$(CONFIG_USB_SERIAL_SYMBOL)			+= symbolserial.o
 obj-$(CONFIG_USB_SERIAL_WWAN)			+= usb_wwan.o
 obj-$(CONFIG_USB_SERIAL_TI)			+= ti_usb_3410_5052.o
 obj-$(CONFIG_USB_SERIAL_VISOR)			+= visor.o
+obj-$(CONFIG_USB_SERIAL_WISHBONE)		+= wishbone-serial.o
 obj-$(CONFIG_USB_SERIAL_WHITEHEAT)		+= whiteheat.o
 obj-$(CONFIG_USB_SERIAL_XIRCOM)			+= keyspan_pda.o
 obj-$(CONFIG_USB_SERIAL_VIVOPAY_SERIAL)		+= vivopay-serial.o
diff --git a/drivers/usb/serial/ark3116.c b/drivers/usb/serial/ark3116.c
index 4775f8209e5..3b16118cbf6 100644
--- a/drivers/usb/serial/ark3116.c
+++ b/drivers/usb/serial/ark3116.c
@@ -62,7 +62,6 @@ static int is_irda(struct usb_serial *serial)
 }
 
 struct ark3116_private {
-	struct async_icount	icount;
 	int			irda;	/* 1 for irda device */
 
 	/* protects hw register updates */
@@ -341,18 +340,15 @@ static void ark3116_close(struct usb_serial_port *port)
 {
 	struct usb_serial *serial = port->serial;
 
-	if (serial->dev) {
-		/* disable DMA */
-		ark3116_write_reg(serial, UART_FCR, 0);
-
-		/* deactivate interrupts */
-		ark3116_write_reg(serial, UART_IER, 0);
+	/* disable DMA */
+	ark3116_write_reg(serial, UART_FCR, 0);
 
-		usb_serial_generic_close(port);
-		if (serial->num_interrupt_in)
-			usb_kill_urb(port->interrupt_in_urb);
-	}
+	/* deactivate interrupts */
+	ark3116_write_reg(serial, UART_IER, 0);
 
+	usb_serial_generic_close(port);
+	if (serial->num_interrupt_in)
+		usb_kill_urb(port->interrupt_in_urb);
 }
 
 static int ark3116_open(struct tty_struct *tty, struct usb_serial_port *port)
@@ -405,31 +401,10 @@ err_out:
 	return result;
 }
 
-static int ark3116_get_icount(struct tty_struct *tty,
-					struct serial_icounter_struct *icount)
-{
-	struct usb_serial_port *port = tty->driver_data;
-	struct ark3116_private *priv = usb_get_serial_port_data(port);
-	struct async_icount cnow = priv->icount;
-	icount->cts = cnow.cts;
-	icount->dsr = cnow.dsr;
-	icount->rng = cnow.rng;
-	icount->dcd = cnow.dcd;
-	icount->rx = cnow.rx;
-	icount->tx = cnow.tx;
-	icount->frame = cnow.frame;
-	icount->overrun = cnow.overrun;
-	icount->parity = cnow.parity;
-	icount->brk = cnow.brk;
-	icount->buf_overrun = cnow.buf_overrun;
-	return 0;
-}
-
 static int ark3116_ioctl(struct tty_struct *tty,
 			 unsigned int cmd, unsigned long arg)
 {
 	struct usb_serial_port *port = tty->driver_data;
-	struct ark3116_private *priv = usb_get_serial_port_data(port);
 	struct serial_struct serstruct;
 	void __user *user_arg = (void __user *)arg;
 
@@ -451,33 +426,6 @@ static int ark3116_ioctl(struct tty_struct *tty,
 		if (copy_from_user(&serstruct, user_arg, sizeof(serstruct)))
 			return -EFAULT;
 		return 0;
-	case TIOCMIWAIT:
-		for (;;) {
-			struct async_icount prev = priv->icount;
-			interruptible_sleep_on(&port->delta_msr_wait);
-			/* see if a signal did it */
-			if (signal_pending(current))
-				return -ERESTARTSYS;
-
-			if (port->serial->disconnected)
-				return -EIO;
-
-			if ((prev.rng == priv->icount.rng) &&
-			    (prev.dsr == priv->icount.dsr) &&
-			    (prev.dcd == priv->icount.dcd) &&
-			    (prev.cts == priv->icount.cts))
-				return -EIO;
-			if ((arg & TIOCM_RNG &&
-			     (prev.rng != priv->icount.rng)) ||
-			    (arg & TIOCM_DSR &&
-			     (prev.dsr != priv->icount.dsr)) ||
-			    (arg & TIOCM_CD  &&
-			     (prev.dcd != priv->icount.dcd)) ||
-			    (arg & TIOCM_CTS &&
-			     (prev.cts != priv->icount.cts)))
-				return 0;
-		}
-		break;
 	}
 
 	return -ENOIOCTLCMD;
@@ -575,14 +523,14 @@ static void ark3116_update_msr(struct usb_serial_port *port, __u8 msr)
 	if (msr & UART_MSR_ANY_DELTA) {
 		/* update input line counters */
 		if (msr & UART_MSR_DCTS)
-			priv->icount.cts++;
+			port->icount.cts++;
 		if (msr & UART_MSR_DDSR)
-			priv->icount.dsr++;
+			port->icount.dsr++;
 		if (msr & UART_MSR_DDCD)
-			priv->icount.dcd++;
+			port->icount.dcd++;
 		if (msr & UART_MSR_TERI)
-			priv->icount.rng++;
-		wake_up_interruptible(&port->delta_msr_wait);
+			port->icount.rng++;
+		wake_up_interruptible(&port->port.delta_msr_wait);
 	}
 }
 
@@ -598,13 +546,13 @@ static void ark3116_update_lsr(struct usb_serial_port *port, __u8 lsr)
 
 	if (lsr&UART_LSR_BRK_ERROR_BITS) {
 		if (lsr & UART_LSR_BI)
-			priv->icount.brk++;
+			port->icount.brk++;
 		if (lsr & UART_LSR_FE)
-			priv->icount.frame++;
+			port->icount.frame++;
 		if (lsr & UART_LSR_PE)
-			priv->icount.parity++;
+			port->icount.parity++;
 		if (lsr & UART_LSR_OE)
-			priv->icount.overrun++;
+			port->icount.overrun++;
 	}
 }
 
@@ -722,7 +670,8 @@ static struct usb_serial_driver ark3116_device = {
 	.ioctl =		ark3116_ioctl,
 	.tiocmget =		ark3116_tiocmget,
 	.tiocmset =		ark3116_tiocmset,
-	.get_icount =		ark3116_get_icount,
+	.tiocmiwait =		usb_serial_generic_tiocmiwait,
+	.get_icount =		usb_serial_generic_get_icount,
 	.open =			ark3116_open,
 	.close =		ark3116_close,
 	.break_ctl = 		ark3116_break_ctl,
diff --git a/drivers/usb/serial/bus.c b/drivers/usb/serial/bus.c
index 37decb13d7e..3c4db6d196c 100644
--- a/drivers/usb/serial/bus.c
+++ b/drivers/usb/serial/bus.c
@@ -106,14 +106,15 @@ static int usb_serial_device_remove(struct device *dev)
 	/* make sure suspend/resume doesn't race against port_remove */
 	usb_autopm_get_interface(port->serial->interface);
 
+	minor = port->number;
+	tty_unregister_device(usb_serial_tty_driver, minor);
+
 	device_remove_file(&port->dev, &dev_attr_port_number);
 
 	driver = port->serial->type;
 	if (driver->port_remove)
 		retval = driver->port_remove(port);
 
-	minor = port->number;
-	tty_unregister_device(usb_serial_tty_driver, minor);
 	dev_info(dev, "%s converter now disconnected from ttyUSB%d\n",
 		 driver->description, minor);
 
diff --git a/drivers/usb/serial/ch341.c b/drivers/usb/serial/ch341.c
index 07d4650a32a..c2a4171ab9c 100644
--- a/drivers/usb/serial/ch341.c
+++ b/drivers/usb/serial/ch341.c
@@ -296,7 +296,6 @@ static void ch341_dtr_rts(struct usb_serial_port *port, int on)
 		priv->line_control &= ~(CH341_BIT_RTS | CH341_BIT_DTR);
 	spin_unlock_irqrestore(&priv->lock, flags);
 	ch341_set_handshake(port->serial->dev, priv->line_control);
-	wake_up_interruptible(&port->delta_msr_wait);
 }
 
 static void ch341_close(struct usb_serial_port *port)
@@ -489,7 +488,7 @@ static void ch341_read_int_callback(struct urb *urb)
 			tty_kref_put(tty);
 		}
 
-		wake_up_interruptible(&port->delta_msr_wait);
+		wake_up_interruptible(&port->port.delta_msr_wait);
 	}
 
 exit:
@@ -500,8 +499,9 @@ exit:
 			__func__, status);
 }
 
-static int wait_modem_info(struct usb_serial_port *port, unsigned int arg)
+static int ch341_tiocmiwait(struct tty_struct *tty, unsigned long arg)
 {
+	struct usb_serial_port *port = tty->driver_data;
 	struct ch341_private *priv = usb_get_serial_port_data(port);
 	unsigned long flags;
 	u8 prevstatus;
@@ -515,7 +515,7 @@ static int wait_modem_info(struct usb_serial_port *port, unsigned int arg)
 	spin_unlock_irqrestore(&priv->lock, flags);
 
 	while (!multi_change) {
-		interruptible_sleep_on(&port->delta_msr_wait);
+		interruptible_sleep_on(&port->port.delta_msr_wait);
 		/* see if a signal did it */
 		if (signal_pending(current))
 			return -ERESTARTSYS;
@@ -542,26 +542,6 @@ static int wait_modem_info(struct usb_serial_port *port, unsigned int arg)
 	return 0;
 }
 
-static int ch341_ioctl(struct tty_struct *tty,
-			unsigned int cmd, unsigned long arg)
-{
-	struct usb_serial_port *port = tty->driver_data;
-
-	dev_dbg(&port->dev, "%s (%d) cmd = 0x%04x\n", __func__, port->number, cmd);
-
-	switch (cmd) {
-	case TIOCMIWAIT:
-		dev_dbg(&port->dev, "%s (%d) TIOCMIWAIT\n", __func__,  port->number);
-		return wait_modem_info(port, arg);
-
-	default:
-		dev_dbg(&port->dev, "%s not supported = 0x%04x\n", __func__, cmd);
-		break;
-	}
-
-	return -ENOIOCTLCMD;
-}
-
 static int ch341_tiocmget(struct tty_struct *tty)
 {
 	struct usb_serial_port *port = tty->driver_data;
@@ -611,11 +591,11 @@ static struct usb_serial_driver ch341_device = {
 	.dtr_rts	   = ch341_dtr_rts,
 	.carrier_raised	   = ch341_carrier_raised,
 	.close             = ch341_close,
-	.ioctl             = ch341_ioctl,
 	.set_termios       = ch341_set_termios,
 	.break_ctl         = ch341_break_ctl,
 	.tiocmget          = ch341_tiocmget,
 	.tiocmset          = ch341_tiocmset,
+	.tiocmiwait        = ch341_tiocmiwait,
 	.read_int_callback = ch341_read_int_callback,
 	.port_probe        = ch341_port_probe,
 	.port_remove       = ch341_port_remove,
diff --git a/drivers/usb/serial/cp210x.c b/drivers/usb/serial/cp210x.c
index 4747d1c328f..2c659553c07 100644
--- a/drivers/usb/serial/cp210x.c
+++ b/drivers/usb/serial/cp210x.c
@@ -462,11 +462,7 @@ static int cp210x_open(struct tty_struct *tty, struct usb_serial_port *port)
 static void cp210x_close(struct usb_serial_port *port)
 {
 	usb_serial_generic_close(port);
-
-	mutex_lock(&port->serial->disc_mutex);
-	if (!port->serial->disconnected)
-		cp210x_set_config_single(port, CP210X_IFC_ENABLE, UART_DISABLE);
-	mutex_unlock(&port->serial->disc_mutex);
+	cp210x_set_config_single(port, CP210X_IFC_ENABLE, UART_DISABLE);
 }
 
 /*
diff --git a/drivers/usb/serial/cyberjack.c b/drivers/usb/serial/cyberjack.c
index 629bd289450..781426230d6 100644
--- a/drivers/usb/serial/cyberjack.c
+++ b/drivers/usb/serial/cyberjack.c
@@ -51,7 +51,6 @@
 #define CYBERJACK_PRODUCT_ID	0x0100
 
 /* Function prototypes */
-static void cyberjack_disconnect(struct usb_serial *serial);
 static int cyberjack_port_probe(struct usb_serial_port *port);
 static int cyberjack_port_remove(struct usb_serial_port *port);
 static int  cyberjack_open(struct tty_struct *tty,
@@ -79,7 +78,6 @@ static struct usb_serial_driver cyberjack_device = {
 	.description =		"Reiner SCT Cyberjack USB card reader",
 	.id_table =		id_table,
 	.num_ports =		1,
-	.disconnect =		cyberjack_disconnect,
 	.port_probe =		cyberjack_port_probe,
 	.port_remove =		cyberjack_port_remove,
 	.open =			cyberjack_open,
@@ -130,20 +128,14 @@ static int cyberjack_port_remove(struct usb_serial_port *port)
 {
 	struct cyberjack_private *priv;
 
+	usb_kill_urb(port->interrupt_in_urb);
+
 	priv = usb_get_serial_port_data(port);
 	kfree(priv);
 
 	return 0;
 }
 
-static void cyberjack_disconnect(struct usb_serial *serial)
-{
-	int i;
-
-	for (i = 0; i < serial->num_ports; ++i)
-		usb_kill_urb(serial->port[i]->interrupt_in_urb);
-}
-
 static int  cyberjack_open(struct tty_struct *tty,
 					struct usb_serial_port *port)
 {
@@ -166,11 +158,8 @@ static int  cyberjack_open(struct tty_struct *tty,
 
 static void cyberjack_close(struct usb_serial_port *port)
 {
-	if (port->serial->dev) {
-		/* shutdown any bulk reads that might be going on */
-		usb_kill_urb(port->write_urb);
-		usb_kill_urb(port->read_urb);
-	}
+	usb_kill_urb(port->write_urb);
+	usb_kill_urb(port->read_urb);
 }
 
 static int cyberjack_write(struct tty_struct *tty,
diff --git a/drivers/usb/serial/cypress_m8.c b/drivers/usb/serial/cypress_m8.c
index ba7352e4187..d341555d37d 100644
--- a/drivers/usb/serial/cypress_m8.c
+++ b/drivers/usb/serial/cypress_m8.c
@@ -129,13 +129,12 @@ static int  cypress_write(struct tty_struct *tty, struct usb_serial_port *port,
 			const unsigned char *buf, int count);
 static void cypress_send(struct usb_serial_port *port);
 static int  cypress_write_room(struct tty_struct *tty);
-static int  cypress_ioctl(struct tty_struct *tty,
-			unsigned int cmd, unsigned long arg);
 static void cypress_set_termios(struct tty_struct *tty,
 			struct usb_serial_port *port, struct ktermios *old);
 static int  cypress_tiocmget(struct tty_struct *tty);
 static int  cypress_tiocmset(struct tty_struct *tty,
 			unsigned int set, unsigned int clear);
+static int  cypress_tiocmiwait(struct tty_struct *tty, unsigned long arg);
 static int  cypress_chars_in_buffer(struct tty_struct *tty);
 static void cypress_throttle(struct tty_struct *tty);
 static void cypress_unthrottle(struct tty_struct *tty);
@@ -158,10 +157,10 @@ static struct usb_serial_driver cypress_earthmate_device = {
 	.dtr_rts =			cypress_dtr_rts,
 	.write =			cypress_write,
 	.write_room =			cypress_write_room,
-	.ioctl =			cypress_ioctl,
 	.set_termios =			cypress_set_termios,
 	.tiocmget =			cypress_tiocmget,
 	.tiocmset =			cypress_tiocmset,
+	.tiocmiwait =			cypress_tiocmiwait,
 	.chars_in_buffer =		cypress_chars_in_buffer,
 	.throttle =		 	cypress_throttle,
 	.unthrottle =			cypress_unthrottle,
@@ -184,10 +183,10 @@ static struct usb_serial_driver cypress_hidcom_device = {
 	.dtr_rts =			cypress_dtr_rts,
 	.write =			cypress_write,
 	.write_room =			cypress_write_room,
-	.ioctl =			cypress_ioctl,
 	.set_termios =			cypress_set_termios,
 	.tiocmget =			cypress_tiocmget,
 	.tiocmset =			cypress_tiocmset,
+	.tiocmiwait =			cypress_tiocmiwait,
 	.chars_in_buffer =		cypress_chars_in_buffer,
 	.throttle =			cypress_throttle,
 	.unthrottle =			cypress_unthrottle,
@@ -210,10 +209,10 @@ static struct usb_serial_driver cypress_ca42v2_device = {
 	.dtr_rts =			cypress_dtr_rts,
 	.write =			cypress_write,
 	.write_room =			cypress_write_room,
-	.ioctl =			cypress_ioctl,
 	.set_termios =			cypress_set_termios,
 	.tiocmget =			cypress_tiocmget,
 	.tiocmset =			cypress_tiocmset,
+	.tiocmiwait =			cypress_tiocmiwait,
 	.chars_in_buffer =		cypress_chars_in_buffer,
 	.throttle =			cypress_throttle,
 	.unthrottle =			cypress_unthrottle,
@@ -633,12 +632,6 @@ static void cypress_close(struct usb_serial_port *port)
 	struct cypress_private *priv = usb_get_serial_port_data(port);
 	unsigned long flags;
 
-	/* writing is potentially harmful, lock must be taken */
-	mutex_lock(&port->serial->disc_mutex);
-	if (port->serial->disconnected) {
-		mutex_unlock(&port->serial->disc_mutex);
-		return;
-	}
 	spin_lock_irqsave(&priv->lock, flags);
 	kfifo_reset_out(&priv->write_fifo);
 	spin_unlock_irqrestore(&priv->lock, flags);
@@ -650,7 +643,6 @@ static void cypress_close(struct usb_serial_port *port)
 	if (stats)
 		dev_info(&port->dev, "Statistics: %d Bytes In | %d Bytes Out | %d Commands Issued\n",
 			priv->bytes_in, priv->bytes_out, priv->cmd_count);
-	mutex_unlock(&port->serial->disc_mutex);
 } /* cypress_close */
 
 
@@ -855,55 +847,43 @@ static int cypress_tiocmset(struct tty_struct *tty,
 }
 
 
-static int cypress_ioctl(struct tty_struct *tty,
-					unsigned int cmd, unsigned long arg)
+static int cypress_tiocmiwait(struct tty_struct *tty, unsigned long arg)
 {
 	struct usb_serial_port *port = tty->driver_data;
 	struct cypress_private *priv = usb_get_serial_port_data(port);
-
-	dev_dbg(&port->dev, "%s - port %d, cmd 0x%.4x\n", __func__, port->number, cmd);
-
-	switch (cmd) {
-	/* This code comes from drivers/char/serial.c and ftdi_sio.c */
-	case TIOCMIWAIT:
-		for (;;) {
-			interruptible_sleep_on(&port->delta_msr_wait);
-			/* see if a signal did it */
-			if (signal_pending(current))
-				return -ERESTARTSYS;
-
-			if (port->serial->disconnected)
-				return -EIO;
-
-			{
-				char diff = priv->diff_status;
-				if (diff == 0)
-					return -EIO; /* no change => error */
-
-				/* consume all events */
-				priv->diff_status = 0;
-
-				/* return 0 if caller wanted to know about
-				   these bits */
-				if (((arg & TIOCM_RNG) && (diff & UART_RI)) ||
-				    ((arg & TIOCM_DSR) && (diff & UART_DSR)) ||
-				    ((arg & TIOCM_CD) && (diff & UART_CD)) ||
-				    ((arg & TIOCM_CTS) && (diff & UART_CTS)))
-					return 0;
-				/* otherwise caller can't care less about what
-				 * happened, and so we continue to wait for
-				 * more events.
-				 */
-			}
-		}
-		return 0;
-	default:
-		break;
+	char diff;
+
+	for (;;) {
+		interruptible_sleep_on(&port->port.delta_msr_wait);
+		/* see if a signal did it */
+		if (signal_pending(current))
+			return -ERESTARTSYS;
+
+		if (port->serial->disconnected)
+			return -EIO;
+
+		diff = priv->diff_status;
+		if (diff == 0)
+			return -EIO; /* no change => error */
+
+		/* consume all events */
+		priv->diff_status = 0;
+
+		/* return 0 if caller wanted to know about
+		   these bits */
+		if (((arg & TIOCM_RNG) && (diff & UART_RI))  ||
+			((arg & TIOCM_DSR) && (diff & UART_DSR)) ||
+			((arg & TIOCM_CD)  && (diff & UART_CD))  ||
+			((arg & TIOCM_CTS) && (diff & UART_CTS)))
+			return 0;
+		/* otherwise caller can't care less about what
+		 * happened, and so we continue to wait for
+		 * more events.
+		 */
 	}
-	dev_dbg(&port->dev, "%s - arg not supported - it was 0x%04x - check include/asm/ioctls.h\n", __func__, cmd);
-	return -ENOIOCTLCMD;
-} /* cypress_ioctl */
 
+	return 0;
+}
 
 static void cypress_set_termios(struct tty_struct *tty,
 	struct usb_serial_port *port, struct ktermios *old_termios)
@@ -1189,7 +1169,7 @@ static void cypress_read_int_callback(struct urb *urb)
 	if (priv->current_status != priv->prev_status) {
 		priv->diff_status |= priv->current_status ^
 			priv->prev_status;
-		wake_up_interruptible(&port->delta_msr_wait);
+		wake_up_interruptible(&port->port.delta_msr_wait);
 		priv->prev_status = priv->current_status;
 	}
 	spin_unlock_irqrestore(&priv->lock, flags);
diff --git a/drivers/usb/serial/digi_acceleport.c b/drivers/usb/serial/digi_acceleport.c
index ebe45fa0ed5..7b807d38952 100644
--- a/drivers/usb/serial/digi_acceleport.c
+++ b/drivers/usb/serial/digi_acceleport.c
@@ -196,7 +196,6 @@ struct digi_port {
 	unsigned char dp_out_buf[DIGI_OUT_BUF_SIZE];
 	int dp_write_urb_in_use;
 	unsigned int dp_modem_signals;
-	wait_queue_head_t dp_modem_change_wait;
 	int dp_transmit_idle;
 	wait_queue_head_t dp_transmit_idle_wait;
 	int dp_throttled;
@@ -210,7 +209,6 @@ struct digi_port {
 
 /* Local Function Declarations */
 
-static void digi_wakeup_write(struct usb_serial_port *port);
 static void digi_wakeup_write_lock(struct work_struct *work);
 static int digi_write_oob_command(struct usb_serial_port *port,
 	unsigned char *buf, int count, int interruptible);
@@ -374,20 +372,10 @@ static void digi_wakeup_write_lock(struct work_struct *work)
 	unsigned long flags;
 
 	spin_lock_irqsave(&priv->dp_port_lock, flags);
-	digi_wakeup_write(port);
+	tty_port_tty_wakeup(&port->port);
 	spin_unlock_irqrestore(&priv->dp_port_lock, flags);
 }
 
-static void digi_wakeup_write(struct usb_serial_port *port)
-{
-	struct tty_struct *tty = tty_port_tty_get(&port->port);
-	if (tty) {
-		tty_wakeup(tty);
-		tty_kref_put(tty);
-	}
-}
-
-
 /*
  *  Digi Write OOB Command
  *
@@ -1044,7 +1032,7 @@ static void digi_write_bulk_callback(struct urb *urb)
 		}
 	}
 	/* wake up processes sleeping on writes immediately */
-	digi_wakeup_write(port);
+	tty_port_tty_wakeup(&port->port);
 	/* also queue up a wakeup at scheduler time, in case we */
 	/* lost the race in write_chan(). */
 	schedule_work(&priv->dp_wakeup_work);
@@ -1149,53 +1137,51 @@ static void digi_close(struct usb_serial_port *port)
 	if (port->serial->disconnected)
 		goto exit;
 
-	if (port->serial->dev) {
-		/* FIXME: Transmit idle belongs in the wait_unti_sent path */
-		digi_transmit_idle(port, DIGI_CLOSE_TIMEOUT);
-
-		/* disable input flow control */
-		buf[0] = DIGI_CMD_SET_INPUT_FLOW_CONTROL;
-		buf[1] = priv->dp_port_num;
-		buf[2] = DIGI_DISABLE;
-		buf[3] = 0;
-
-		/* disable output flow control */
-		buf[4] = DIGI_CMD_SET_OUTPUT_FLOW_CONTROL;
-		buf[5] = priv->dp_port_num;
-		buf[6] = DIGI_DISABLE;
-		buf[7] = 0;
-
-		/* disable reading modem signals automatically */
-		buf[8] = DIGI_CMD_READ_INPUT_SIGNALS;
-		buf[9] = priv->dp_port_num;
-		buf[10] = DIGI_DISABLE;
-		buf[11] = 0;
-
-		/* disable receive */
-		buf[12] = DIGI_CMD_RECEIVE_ENABLE;
-		buf[13] = priv->dp_port_num;
-		buf[14] = DIGI_DISABLE;
-		buf[15] = 0;
-
-		/* flush fifos */
-		buf[16] = DIGI_CMD_IFLUSH_FIFO;
-		buf[17] = priv->dp_port_num;
-		buf[18] = DIGI_FLUSH_TX | DIGI_FLUSH_RX;
-		buf[19] = 0;
-
-		ret = digi_write_oob_command(port, buf, 20, 0);
-		if (ret != 0)
-			dev_dbg(&port->dev, "digi_close: write oob failed, ret=%d\n", ret);
-
-		/* wait for final commands on oob port to complete */
-		prepare_to_wait(&priv->dp_flush_wait, &wait,
-							TASK_INTERRUPTIBLE);
-		schedule_timeout(DIGI_CLOSE_TIMEOUT);
-		finish_wait(&priv->dp_flush_wait, &wait);
-
-		/* shutdown any outstanding bulk writes */
-		usb_kill_urb(port->write_urb);
-	}
+	/* FIXME: Transmit idle belongs in the wait_unti_sent path */
+	digi_transmit_idle(port, DIGI_CLOSE_TIMEOUT);
+
+	/* disable input flow control */
+	buf[0] = DIGI_CMD_SET_INPUT_FLOW_CONTROL;
+	buf[1] = priv->dp_port_num;
+	buf[2] = DIGI_DISABLE;
+	buf[3] = 0;
+
+	/* disable output flow control */
+	buf[4] = DIGI_CMD_SET_OUTPUT_FLOW_CONTROL;
+	buf[5] = priv->dp_port_num;
+	buf[6] = DIGI_DISABLE;
+	buf[7] = 0;
+
+	/* disable reading modem signals automatically */
+	buf[8] = DIGI_CMD_READ_INPUT_SIGNALS;
+	buf[9] = priv->dp_port_num;
+	buf[10] = DIGI_DISABLE;
+	buf[11] = 0;
+
+	/* disable receive */
+	buf[12] = DIGI_CMD_RECEIVE_ENABLE;
+	buf[13] = priv->dp_port_num;
+	buf[14] = DIGI_DISABLE;
+	buf[15] = 0;
+
+	/* flush fifos */
+	buf[16] = DIGI_CMD_IFLUSH_FIFO;
+	buf[17] = priv->dp_port_num;
+	buf[18] = DIGI_FLUSH_TX | DIGI_FLUSH_RX;
+	buf[19] = 0;
+
+	ret = digi_write_oob_command(port, buf, 20, 0);
+	if (ret != 0)
+		dev_dbg(&port->dev, "digi_close: write oob failed, ret=%d\n",
+									ret);
+	/* wait for final commands on oob port to complete */
+	prepare_to_wait(&priv->dp_flush_wait, &wait,
+			TASK_INTERRUPTIBLE);
+	schedule_timeout(DIGI_CLOSE_TIMEOUT);
+	finish_wait(&priv->dp_flush_wait, &wait);
+
+	/* shutdown any outstanding bulk writes */
+	usb_kill_urb(port->write_urb);
 exit:
 	spin_lock_irq(&priv->dp_port_lock);
 	priv->dp_write_urb_in_use = 0;
@@ -1252,7 +1238,6 @@ static int digi_port_init(struct usb_serial_port *port, unsigned port_num)
 
 	spin_lock_init(&priv->dp_port_lock);
 	priv->dp_port_num = port_num;
-	init_waitqueue_head(&priv->dp_modem_change_wait);
 	init_waitqueue_head(&priv->dp_transmit_idle_wait);
 	init_waitqueue_head(&priv->dp_flush_wait);
 	init_waitqueue_head(&priv->dp_close_wait);
@@ -1522,7 +1507,7 @@ static int digi_read_oob_callback(struct urb *urb)
 				/* port must be open to use tty struct */
 				if (rts) {
 					tty->hw_stopped = 0;
-					digi_wakeup_write(port);
+					tty_port_tty_wakeup(&port->port);
 				}
 			} else {
 				priv->dp_modem_signals &= ~TIOCM_CTS;
@@ -1543,7 +1528,6 @@ static int digi_read_oob_callback(struct urb *urb)
 			else
 				priv->dp_modem_signals &= ~TIOCM_CD;
 
-			wake_up_interruptible(&priv->dp_modem_change_wait);
 			spin_unlock(&priv->dp_port_lock);
 		} else if (opcode == DIGI_CMD_TRANSMIT_IDLE) {
 			spin_lock(&priv->dp_port_lock);
diff --git a/drivers/usb/serial/f81232.c b/drivers/usb/serial/f81232.c
index a172ad5c5ce..090b411d893 100644
--- a/drivers/usb/serial/f81232.c
+++ b/drivers/usb/serial/f81232.c
@@ -110,7 +110,7 @@ static void f81232_process_read_urb(struct urb *urb)
 	line_status = priv->line_status;
 	priv->line_status &= ~UART_STATE_TRANSIENT_MASK;
 	spin_unlock_irqrestore(&priv->lock, flags);
-	wake_up_interruptible(&port->delta_msr_wait);
+	wake_up_interruptible(&port->port.delta_msr_wait);
 
 	if (!urb->actual_length)
 		return;
@@ -242,8 +242,9 @@ static int f81232_carrier_raised(struct usb_serial_port *port)
 	return 0;
 }
 
-static int wait_modem_info(struct usb_serial_port *port, unsigned int arg)
+static int f81232_tiocmiwait(struct tty_struct *tty, unsigned long arg)
 {
+	struct usb_serial_port *port = tty->driver_data;
 	struct f81232_private *priv = usb_get_serial_port_data(port);
 	unsigned long flags;
 	unsigned int prevstatus;
@@ -255,7 +256,7 @@ static int wait_modem_info(struct usb_serial_port *port, unsigned int arg)
 	spin_unlock_irqrestore(&priv->lock, flags);
 
 	while (1) {
-		interruptible_sleep_on(&port->delta_msr_wait);
+		interruptible_sleep_on(&port->port.delta_msr_wait);
 		/* see if a signal did it */
 		if (signal_pending(current))
 			return -ERESTARTSYS;
@@ -302,11 +303,6 @@ static int f81232_ioctl(struct tty_struct *tty,
 			return -EFAULT;
 
 		return 0;
-
-	case TIOCMIWAIT:
-		dev_dbg(&port->dev, "%s (%d) TIOCMIWAIT\n", __func__,
-			port->number);
-		return wait_modem_info(port, arg);
 	default:
 		dev_dbg(&port->dev, "%s not supported = 0x%04x\n",
 			__func__, cmd);
@@ -358,6 +354,7 @@ static struct usb_serial_driver f81232_device = {
 	.set_termios =		f81232_set_termios,
 	.tiocmget =		f81232_tiocmget,
 	.tiocmset =		f81232_tiocmset,
+	.tiocmiwait =		f81232_tiocmiwait,
 	.process_read_urb =	f81232_process_read_urb,
 	.read_int_callback =	f81232_read_int_callback,
 	.port_probe =		f81232_port_probe,
diff --git a/drivers/usb/serial/ftdi_sio.c b/drivers/usb/serial/ftdi_sio.c
index d4809d55147..242b5776648 100644
--- a/drivers/usb/serial/ftdi_sio.c
+++ b/drivers/usb/serial/ftdi_sio.c
@@ -1,7 +1,7 @@
 /*
  * USB FTDI SIO driver
  *
- *	Copyright (C) 2009 - 2010
+ *	Copyright (C) 2009 - 2013
  *	    Johan Hovold (jhovold@gmail.com)
  *	Copyright (C) 1999 - 2001
  *	    Greg Kroah-Hartman (greg@kroah.com)
@@ -55,7 +55,6 @@ static __u16 vendor = FTDI_VID;
 static __u16 product;
 
 struct ftdi_private {
-	struct kref kref;
 	enum ftdi_chip_type chip_type;
 				/* type of device, either SIO or FT8U232AM */
 	int baud_base;		/* baud base clock for divisor setting */
@@ -68,7 +67,6 @@ struct ftdi_private {
 				 */
 	int flags;		/* some ASYNC_xxxx flags are supported */
 	unsigned long last_dtr_rts;	/* saved modem control outputs */
-	struct async_icount	icount;
 	char prev_status;        /* Used for TIOCMIWAIT */
 	char transmit_empty;	/* If transmitter is empty or not */
 	__u16 interface;	/* FT2232C, FT2232H or FT4232H port interface
@@ -189,6 +187,7 @@ static struct usb_device_id id_table_combined [] = {
 	{ USB_DEVICE(FTDI_VID, FTDI_OPENDCC_THROTTLE_PID) },
 	{ USB_DEVICE(FTDI_VID, FTDI_OPENDCC_GATEWAY_PID) },
 	{ USB_DEVICE(FTDI_VID, FTDI_OPENDCC_GBM_PID) },
+	{ USB_DEVICE(FTDI_VID, FTDI_OPENDCC_GBM_BOOST_PID) },
 	{ USB_DEVICE(NEWPORT_VID, NEWPORT_AGILIS_PID) },
 	{ USB_DEVICE(INTERBIOMETRICS_VID, INTERBIOMETRICS_IOBOARD_PID) },
 	{ USB_DEVICE(INTERBIOMETRICS_VID, INTERBIOMETRICS_MINI_IOBOARD_PID) },
@@ -640,6 +639,7 @@ static struct usb_device_id id_table_combined [] = {
 	{ USB_DEVICE(FTDI_VID, FTDI_RM_CANVIEW_PID) },
 	{ USB_DEVICE(ACTON_VID, ACTON_SPECTRAPRO_PID) },
 	{ USB_DEVICE(CONTEC_VID, CONTEC_COM1USBH_PID) },
+	{ USB_DEVICE(MITSUBISHI_VID, MITSUBISHI_FXUSB_PID) },
 	{ USB_DEVICE(BANDB_VID, BANDB_USOTL4_PID) },
 	{ USB_DEVICE(BANDB_VID, BANDB_USTL4_PID) },
 	{ USB_DEVICE(BANDB_VID, BANDB_USO9ML2_PID) },
@@ -869,7 +869,9 @@ static struct usb_device_id id_table_combined [] = {
 	{ USB_DEVICE(FTDI_VID, FTDI_DOTEC_PID) },
 	{ USB_DEVICE(QIHARDWARE_VID, MILKYMISTONE_JTAGSERIAL_PID),
 		.driver_info = (kernel_ulong_t)&ftdi_jtag_quirk },
-	{ USB_DEVICE(ST_VID, ST_STMCLT1030_PID),
+	{ USB_DEVICE(ST_VID, ST_STMCLT_2232_PID),
+		.driver_info = (kernel_ulong_t)&ftdi_jtag_quirk },
+	{ USB_DEVICE(ST_VID, ST_STMCLT_4232_PID),
 		.driver_info = (kernel_ulong_t)&ftdi_stmclite_quirk },
 	{ USB_DEVICE(FTDI_VID, FTDI_RF_R106) },
 	{ USB_DEVICE(FTDI_VID, FTDI_DISTORTEC_JTAG_LOCK_PICK_PID),
@@ -910,7 +912,6 @@ static int  ftdi_sio_probe(struct usb_serial *serial,
 static int  ftdi_sio_port_probe(struct usb_serial_port *port);
 static int  ftdi_sio_port_remove(struct usb_serial_port *port);
 static int  ftdi_open(struct tty_struct *tty, struct usb_serial_port *port);
-static void ftdi_close(struct usb_serial_port *port);
 static void ftdi_dtr_rts(struct usb_serial_port *port, int on);
 static void ftdi_process_read_urb(struct urb *urb);
 static int ftdi_prepare_write_buffer(struct usb_serial_port *port,
@@ -920,8 +921,6 @@ static void ftdi_set_termios(struct tty_struct *tty,
 static int  ftdi_tiocmget(struct tty_struct *tty);
 static int  ftdi_tiocmset(struct tty_struct *tty,
 			unsigned int set, unsigned int clear);
-static int ftdi_get_icount(struct tty_struct *tty,
-			   struct serial_icounter_struct *icount);
 static int  ftdi_ioctl(struct tty_struct *tty,
 			unsigned int cmd, unsigned long arg);
 static void ftdi_break_ctl(struct tty_struct *tty, int break_state);
@@ -950,7 +949,6 @@ static struct usb_serial_driver ftdi_sio_device = {
 	.port_probe =		ftdi_sio_port_probe,
 	.port_remove =		ftdi_sio_port_remove,
 	.open =			ftdi_open,
-	.close =		ftdi_close,
 	.dtr_rts =		ftdi_dtr_rts,
 	.throttle =		usb_serial_generic_throttle,
 	.unthrottle =		usb_serial_generic_unthrottle,
@@ -958,7 +956,8 @@ static struct usb_serial_driver ftdi_sio_device = {
 	.prepare_write_buffer =	ftdi_prepare_write_buffer,
 	.tiocmget =		ftdi_tiocmget,
 	.tiocmset =		ftdi_tiocmset,
-	.get_icount =           ftdi_get_icount,
+	.tiocmiwait =		usb_serial_generic_tiocmiwait,
+	.get_icount =           usb_serial_generic_get_icount,
 	.ioctl =		ftdi_ioctl,
 	.set_termios =		ftdi_set_termios,
 	.break_ctl =		ftdi_break_ctl,
@@ -1687,7 +1686,6 @@ static int ftdi_sio_port_probe(struct usb_serial_port *port)
 		return -ENOMEM;
 	}
 
-	kref_init(&priv->kref);
 	mutex_init(&priv->cfg_lock);
 
 	priv->flags = ASYNC_LOW_LATENCY;
@@ -1791,20 +1789,24 @@ static int ftdi_8u2232c_probe(struct usb_serial *serial)
 }
 
 /*
- * First and second port on STMCLiteadaptors is reserved for JTAG interface
- * and the forth port for pio
+ * First two ports on JTAG adaptors using an FT4232 such as STMicroelectronics's
+ * ST Micro Connect Lite are reserved for JTAG or other non-UART interfaces and
+ * can be accessed from userspace.
+ * The next two ports are enabled as UARTs by default, where port 2 is
+ * a conventional RS-232 UART.
  */
 static int ftdi_stmclite_probe(struct usb_serial *serial)
 {
 	struct usb_device *udev = serial->dev;
 	struct usb_interface *interface = serial->interface;
 
-	if (interface == udev->actconfig->interface[2])
-		return 0;
-
-	dev_info(&udev->dev, "Ignoring serial port reserved for JTAG\n");
+	if (interface == udev->actconfig->interface[0] ||
+	    interface == udev->actconfig->interface[1]) {
+		dev_info(&udev->dev, "Ignoring serial port reserved for JTAG\n");
+		return -ENODEV;
+	}
 
-	return -ENODEV;
+	return 0;
 }
 
 /*
@@ -1825,22 +1827,13 @@ static int ftdi_mtxorb_hack_setup(struct usb_serial *serial)
 	return 0;
 }
 
-static void ftdi_sio_priv_release(struct kref *k)
-{
-	struct ftdi_private *priv = container_of(k, struct ftdi_private, kref);
-
-	kfree(priv);
-}
-
 static int ftdi_sio_port_remove(struct usb_serial_port *port)
 {
 	struct ftdi_private *priv = usb_get_serial_port_data(port);
 
-	wake_up_interruptible(&port->delta_msr_wait);
-
 	remove_sysfs_attrs(port);
 
-	kref_put(&priv->kref, ftdi_sio_priv_release);
+	kfree(priv);
 
 	return 0;
 }
@@ -1850,7 +1843,6 @@ static int ftdi_open(struct tty_struct *tty, struct usb_serial_port *port)
 	struct ktermios dummy;
 	struct usb_device *dev = port->serial->dev;
 	struct ftdi_private *priv = usb_get_serial_port_data(port);
-	int result;
 
 	/* No error checking for this (will get errors later anyway) */
 	/* See ftdi_sio.h for description of what is reset */
@@ -1869,12 +1861,7 @@ static int ftdi_open(struct tty_struct *tty, struct usb_serial_port *port)
 		ftdi_set_termios(tty, port, &dummy);
 	}
 
-	/* Start reading from the device */
-	result = usb_serial_generic_open(tty, port);
-	if (!result)
-		kref_get(&priv->kref);
-
-	return result;
+	return usb_serial_generic_open(tty, port);
 }
 
 static void ftdi_dtr_rts(struct usb_serial_port *port, int on)
@@ -1899,19 +1886,6 @@ static void ftdi_dtr_rts(struct usb_serial_port *port, int on)
 		clear_mctrl(port, TIOCM_DTR | TIOCM_RTS);
 }
 
-/*
- * usbserial:__serial_close  only calls ftdi_close if the point is open
- *
- *   This only gets called when it is the last close
- */
-static void ftdi_close(struct usb_serial_port *port)
-{
-	struct ftdi_private *priv = usb_get_serial_port_data(port);
-
-	usb_serial_generic_close(port);
-	kref_put(&priv->kref, ftdi_sio_priv_release);
-}
-
 /* The SIO requires the first byte to have:
  *  B0 1
  *  B1 0
@@ -1939,7 +1913,7 @@ static int ftdi_prepare_write_buffer(struct usb_serial_port *port,
 			c = kfifo_out(&port->write_fifo, &buffer[i + 1], len);
 			if (!c)
 				break;
-			priv->icount.tx += c;
+			port->icount.tx += c;
 			buffer[i] = (c << 2) + 1;
 			count += c + 1;
 		}
@@ -1947,7 +1921,7 @@ static int ftdi_prepare_write_buffer(struct usb_serial_port *port,
 	} else {
 		count = kfifo_out_locked(&port->write_fifo, dest, size,
 								&port->lock);
-		priv->icount.tx += count;
+		port->icount.tx += count;
 	}
 
 	return count;
@@ -1976,15 +1950,15 @@ static int ftdi_process_packet(struct usb_serial_port *port,
 		char diff_status = status ^ priv->prev_status;
 
 		if (diff_status & FTDI_RS0_CTS)
-			priv->icount.cts++;
+			port->icount.cts++;
 		if (diff_status & FTDI_RS0_DSR)
-			priv->icount.dsr++;
+			port->icount.dsr++;
 		if (diff_status & FTDI_RS0_RI)
-			priv->icount.rng++;
+			port->icount.rng++;
 		if (diff_status & FTDI_RS0_RLSD)
-			priv->icount.dcd++;
+			port->icount.dcd++;
 
-		wake_up_interruptible(&port->delta_msr_wait);
+		wake_up_interruptible(&port->port.delta_msr_wait);
 		priv->prev_status = status;
 	}
 
@@ -1994,18 +1968,18 @@ static int ftdi_process_packet(struct usb_serial_port *port,
 		 * over framing errors */
 		if (packet[1] & FTDI_RS_BI) {
 			flag = TTY_BREAK;
-			priv->icount.brk++;
+			port->icount.brk++;
 			usb_serial_handle_break(port);
 		} else if (packet[1] & FTDI_RS_PE) {
 			flag = TTY_PARITY;
-			priv->icount.parity++;
+			port->icount.parity++;
 		} else if (packet[1] & FTDI_RS_FE) {
 			flag = TTY_FRAME;
-			priv->icount.frame++;
+			port->icount.frame++;
 		}
 		/* Overrun is special, not associated with a char */
 		if (packet[1] & FTDI_RS_OE) {
-			priv->icount.overrun++;
+			port->icount.overrun++;
 			tty_insert_flip_char(&port->port, 0, TTY_OVERRUN);
 		}
 	}
@@ -2019,7 +1993,7 @@ static int ftdi_process_packet(struct usb_serial_port *port,
 	len -= 2;
 	if (!len)
 		return 0;	/* status only */
-	priv->icount.rx += len;
+	port->icount.rx += len;
 	ch = packet + 2;
 
 	if (port->port.console && port->sysrq) {
@@ -2383,34 +2357,10 @@ static int ftdi_tiocmset(struct tty_struct *tty,
 	return update_mctrl(port, set, clear);
 }
 
-static int ftdi_get_icount(struct tty_struct *tty,
-				struct serial_icounter_struct *icount)
-{
-	struct usb_serial_port *port = tty->driver_data;
-	struct ftdi_private *priv = usb_get_serial_port_data(port);
-	struct async_icount *ic = &priv->icount;
-
-	icount->cts = ic->cts;
-	icount->dsr = ic->dsr;
-	icount->rng = ic->rng;
-	icount->dcd = ic->dcd;
-	icount->tx = ic->tx;
-	icount->rx = ic->rx;
-	icount->frame = ic->frame;
-	icount->parity = ic->parity;
-	icount->overrun = ic->overrun;
-	icount->brk = ic->brk;
-	icount->buf_overrun = ic->buf_overrun;
-	return 0;
-}
-
 static int ftdi_ioctl(struct tty_struct *tty,
 					unsigned int cmd, unsigned long arg)
 {
 	struct usb_serial_port *port = tty->driver_data;
-	struct ftdi_private *priv = usb_get_serial_port_data(port);
-	struct async_icount cnow;
-	struct async_icount cprev;
 
 	dev_dbg(&port->dev, "%s cmd 0x%04x\n", __func__, cmd);
 
@@ -2424,35 +2374,6 @@ static int ftdi_ioctl(struct tty_struct *tty,
 	case TIOCSSERIAL: /* sets serial port data */
 		return set_serial_info(tty, port,
 					(struct serial_struct __user *) arg);
-
-	/*
-	 * Wait for any of the 4 modem inputs (DCD,RI,DSR,CTS) to change
-	 * - mask passed in arg for lines of interest
-	 *   (use |'ed TIOCM_RNG/DSR/CD/CTS for masking)
-	 * Caller should use TIOCGICOUNT to see which one it was.
-	 *
-	 * This code is borrowed from linux/drivers/char/serial.c
-	 */
-	case TIOCMIWAIT:
-		cprev = priv->icount;
-		for (;;) {
-			interruptible_sleep_on(&port->delta_msr_wait);
-			/* see if a signal did it */
-			if (signal_pending(current))
-				return -ERESTARTSYS;
-
-			if (port->serial->disconnected)
-				return -EIO;
-
-			cnow = priv->icount;
-			if (((arg & TIOCM_RNG) && (cnow.rng != cprev.rng)) ||
-			    ((arg & TIOCM_DSR) && (cnow.dsr != cprev.dsr)) ||
-			    ((arg & TIOCM_CD)  && (cnow.dcd != cprev.dcd)) ||
-			    ((arg & TIOCM_CTS) && (cnow.cts != cprev.cts))) {
-				return 0;
-			}
-			cprev = cnow;
-		}
 	case TIOCSERGETLSR:
 		return get_lsr_info(port, (struct serial_struct __user *)arg);
 		break;
diff --git a/drivers/usb/serial/ftdi_sio_ids.h b/drivers/usb/serial/ftdi_sio_ids.h
index 9d359e189a6..98528270c43 100644
--- a/drivers/usb/serial/ftdi_sio_ids.h
+++ b/drivers/usb/serial/ftdi_sio_ids.h
@@ -74,6 +74,7 @@
 #define FTDI_OPENDCC_THROTTLE_PID	0xBFDA
 #define FTDI_OPENDCC_GATEWAY_PID	0xBFDB
 #define FTDI_OPENDCC_GBM_PID	0xBFDC
+#define FTDI_OPENDCC_GBM_BOOST_PID	0xBFDD
 
 /* NZR SEM 16+ USB (http://www.nzr.de) */
 #define FTDI_NZR_SEM_USB_PID	0xC1E0	/* NZR SEM-LOG16+ */
@@ -584,6 +585,13 @@
 #define CONTEC_COM1USBH_PID	0x8311	/* COM-1(USB)H */
 
 /*
+ * Mitsubishi Electric Corp. (http://www.meau.com)
+ * Submitted by Konstantin Holoborodko
+ */
+#define MITSUBISHI_VID		0x06D3
+#define MITSUBISHI_FXUSB_PID	0x0284 /* USB/RS422 converters: FX-USB-AW/-BD */
+
+/*
  * Definitions for B&B Electronics products.
  */
 #define BANDB_VID		0x0856	/* B&B Electronics Vendor ID */
@@ -1143,7 +1151,8 @@
  * STMicroelectonics
  */
 #define ST_VID			0x0483
-#define ST_STMCLT1030_PID	0x3747 /* ST Micro Connect Lite STMCLT1030 */
+#define ST_STMCLT_2232_PID	0x3746
+#define ST_STMCLT_4232_PID	0x3747
 
 /*
  * Papouch products (http://www.papouch.com/)
diff --git a/drivers/usb/serial/garmin_gps.c b/drivers/usb/serial/garmin_gps.c
index 81caf5623ee..b110c573ea8 100644
--- a/drivers/usb/serial/garmin_gps.c
+++ b/drivers/usb/serial/garmin_gps.c
@@ -946,16 +946,12 @@ static int garmin_open(struct tty_struct *tty, struct usb_serial_port *port)
 
 static void garmin_close(struct usb_serial_port *port)
 {
-	struct usb_serial *serial = port->serial;
 	struct garmin_data *garmin_data_p = usb_get_serial_port_data(port);
 
 	dev_dbg(&port->dev, "%s - port %d - mode=%d state=%d flags=0x%X\n",
 		__func__, port->number, garmin_data_p->mode,
 		garmin_data_p->state, garmin_data_p->flags);
 
-	if (!serial)
-		return;
-
 	garmin_clear(garmin_data_p);
 
 	/* shutdown our urbs */
@@ -1185,17 +1181,11 @@ static void garmin_read_bulk_callback(struct urb *urb)
 {
 	unsigned long flags;
 	struct usb_serial_port *port = urb->context;
-	struct usb_serial *serial =  port->serial;
 	struct garmin_data *garmin_data_p = usb_get_serial_port_data(port);
 	unsigned char *data = urb->transfer_buffer;
 	int status = urb->status;
 	int retval;
 
-	if (!serial) {
-		dev_dbg(&urb->dev->dev, "%s - bad serial pointer, exiting\n", __func__);
-		return;
-	}
-
 	if (status) {
 		dev_dbg(&urb->dev->dev, "%s - nonzero read bulk status received: %d\n",
 			__func__, status);
diff --git a/drivers/usb/serial/generic.c b/drivers/usb/serial/generic.c
index 4c5c23f1cae..297665fdd16 100644
--- a/drivers/usb/serial/generic.c
+++ b/drivers/usb/serial/generic.c
@@ -1,7 +1,7 @@
 /*
  * USB Serial Converter Generic functions
  *
- * Copyright (C) 2010 - 2011 Johan Hovold (jhovold@gmail.com)
+ * Copyright (C) 2010 - 2013 Johan Hovold (jhovold@gmail.com)
  * Copyright (C) 1999 - 2002 Greg Kroah-Hartman (greg@kroah.com)
  *
  *	This program is free software; you can redistribute it and/or
@@ -45,8 +45,6 @@ struct usb_serial_driver usb_serial_generic_device = {
 	},
 	.id_table =		generic_device_ids,
 	.num_ports =		1,
-	.disconnect =		usb_serial_generic_disconnect,
-	.release =		usb_serial_generic_release,
 	.throttle =		usb_serial_generic_throttle,
 	.unthrottle =		usb_serial_generic_unthrottle,
 	.resume =		usb_serial_generic_resume,
@@ -102,32 +100,23 @@ int usb_serial_generic_open(struct tty_struct *tty, struct usb_serial_port *port
 }
 EXPORT_SYMBOL_GPL(usb_serial_generic_open);
 
-static void generic_cleanup(struct usb_serial_port *port)
+void usb_serial_generic_close(struct usb_serial_port *port)
 {
-	struct usb_serial *serial = port->serial;
 	unsigned long flags;
 	int i;
 
-	if (serial->dev) {
-		/* shutdown any bulk transfers that might be going on */
-		if (port->bulk_out_size) {
-			for (i = 0; i < ARRAY_SIZE(port->write_urbs); ++i)
-				usb_kill_urb(port->write_urbs[i]);
+	if (port->bulk_out_size) {
+		for (i = 0; i < ARRAY_SIZE(port->write_urbs); ++i)
+			usb_kill_urb(port->write_urbs[i]);
 
-			spin_lock_irqsave(&port->lock, flags);
-			kfifo_reset_out(&port->write_fifo);
-			spin_unlock_irqrestore(&port->lock, flags);
-		}
-		if (port->bulk_in_size) {
-			for (i = 0; i < ARRAY_SIZE(port->read_urbs); ++i)
-				usb_kill_urb(port->read_urbs[i]);
-		}
+		spin_lock_irqsave(&port->lock, flags);
+		kfifo_reset_out(&port->write_fifo);
+		spin_unlock_irqrestore(&port->lock, flags);
+	}
+	if (port->bulk_in_size) {
+		for (i = 0; i < ARRAY_SIZE(port->read_urbs); ++i)
+			usb_kill_urb(port->read_urbs[i]);
 	}
-}
-
-void usb_serial_generic_close(struct usb_serial_port *port)
-{
-	generic_cleanup(port);
 }
 EXPORT_SYMBOL_GPL(usb_serial_generic_close);
 
@@ -272,8 +261,7 @@ static int usb_serial_generic_submit_read_urb(struct usb_serial_port *port,
 	if (!test_and_clear_bit(index, &port->read_urbs_free))
 		return 0;
 
-	dev_dbg(&port->dev, "%s - port %d, urb %d\n", __func__,
-		port->number, index);
+	dev_dbg(&port->dev, "%s - urb %d\n", __func__, index);
 
 	res = usb_submit_urb(port->read_urbs[index], mem_flags);
 	if (res) {
@@ -347,8 +335,8 @@ void usb_serial_generic_read_bulk_callback(struct urb *urb)
 	}
 	set_bit(i, &port->read_urbs_free);
 
-	dev_dbg(&port->dev, "%s - port %d, urb %d, len %d\n",
-		__func__, port->number, i, urb->actual_length);
+	dev_dbg(&port->dev, "%s - urb %d, len %d\n", __func__, i,
+							urb->actual_length);
 
 	if (urb->status) {
 		dev_dbg(&port->dev, "%s - non-zero urb status: %d\n",
@@ -430,6 +418,91 @@ void usb_serial_generic_unthrottle(struct tty_struct *tty)
 }
 EXPORT_SYMBOL_GPL(usb_serial_generic_unthrottle);
 
+static bool usb_serial_generic_msr_changed(struct tty_struct *tty,
+				unsigned long arg, struct async_icount *cprev)
+{
+	struct usb_serial_port *port = tty->driver_data;
+	struct async_icount cnow;
+	unsigned long flags;
+	bool ret;
+
+	/*
+	 * Use tty-port initialised flag to detect all hangups including the
+	 * one generated at USB-device disconnect.
+	 *
+	 * FIXME: Remove hupping check once tty_port_hangup calls shutdown
+	 *        (which clears the initialised flag) before wake up.
+	 */
+	if (test_bit(TTY_HUPPING, &tty->flags))
+		return true;
+	if (!test_bit(ASYNCB_INITIALIZED, &port->port.flags))
+		return true;
+
+	spin_lock_irqsave(&port->lock, flags);
+	cnow = port->icount;				/* atomic copy*/
+	spin_unlock_irqrestore(&port->lock, flags);
+
+	ret =	((arg & TIOCM_RNG) && (cnow.rng != cprev->rng)) ||
+		((arg & TIOCM_DSR) && (cnow.dsr != cprev->dsr)) ||
+		((arg & TIOCM_CD)  && (cnow.dcd != cprev->dcd)) ||
+		((arg & TIOCM_CTS) && (cnow.cts != cprev->cts));
+
+	*cprev = cnow;
+
+	return ret;
+}
+
+int usb_serial_generic_tiocmiwait(struct tty_struct *tty, unsigned long arg)
+{
+	struct usb_serial_port *port = tty->driver_data;
+	struct async_icount cnow;
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&port->lock, flags);
+	cnow = port->icount;				/* atomic copy */
+	spin_unlock_irqrestore(&port->lock, flags);
+
+	ret = wait_event_interruptible(port->port.delta_msr_wait,
+			usb_serial_generic_msr_changed(tty, arg, &cnow));
+	if (!ret) {
+		if (test_bit(TTY_HUPPING, &tty->flags))
+			ret = -EIO;
+		if (!test_bit(ASYNCB_INITIALIZED, &port->port.flags))
+			ret = -EIO;
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(usb_serial_generic_tiocmiwait);
+
+int usb_serial_generic_get_icount(struct tty_struct *tty,
+					struct serial_icounter_struct *icount)
+{
+	struct usb_serial_port *port = tty->driver_data;
+	struct async_icount cnow;
+	unsigned long flags;
+
+	spin_lock_irqsave(&port->lock, flags);
+	cnow = port->icount;				/* atomic copy */
+	spin_unlock_irqrestore(&port->lock, flags);
+
+	icount->cts = cnow.cts;
+	icount->dsr = cnow.dsr;
+	icount->rng = cnow.rng;
+	icount->dcd = cnow.dcd;
+	icount->tx = cnow.tx;
+	icount->rx = cnow.rx;
+	icount->frame = cnow.frame;
+	icount->parity = cnow.parity;
+	icount->overrun = cnow.overrun;
+	icount->brk = cnow.brk;
+	icount->buf_overrun = cnow.buf_overrun;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(usb_serial_generic_get_icount);
+
 #ifdef CONFIG_MAGIC_SYSRQ
 int usb_serial_handle_sysrq_char(struct usb_serial_port *port, unsigned int ch)
 {
@@ -473,8 +546,7 @@ void usb_serial_handle_dcd_change(struct usb_serial_port *usb_port,
 {
 	struct tty_port *port = &usb_port->port;
 
-	dev_dbg(&usb_port->dev, "%s - port %d, status %d\n", __func__,
-		usb_port->number, status);
+	dev_dbg(&usb_port->dev, "%s - status %d\n", __func__, status);
 
 	if (status)
 		wake_up_interruptible(&port->open_wait);
@@ -510,17 +582,3 @@ int usb_serial_generic_resume(struct usb_serial *serial)
 	return c ? -EIO : 0;
 }
 EXPORT_SYMBOL_GPL(usb_serial_generic_resume);
-
-void usb_serial_generic_disconnect(struct usb_serial *serial)
-{
-	int i;
-
-	/* stop reads and writes on all ports */
-	for (i = 0; i < serial->num_ports; ++i)
-		generic_cleanup(serial->port[i]);
-}
-EXPORT_SYMBOL_GPL(usb_serial_generic_disconnect);
-
-void usb_serial_generic_release(struct usb_serial *serial)
-{
-}
diff --git a/drivers/usb/serial/io_edgeport.c b/drivers/usb/serial/io_edgeport.c
index efd8b978128..1477e859347 100644
--- a/drivers/usb/serial/io_edgeport.c
+++ b/drivers/usb/serial/io_edgeport.c
@@ -111,7 +111,6 @@ struct edgeport_port {
 	wait_queue_head_t	wait_open;		/* for handling sleeping while waiting for open to finish */
 	wait_queue_head_t	wait_command;		/* for handling sleeping while waiting for command to finish */
 
-	struct async_icount	icount;
 	struct usb_serial_port	*port;			/* loop back to the owner of this object */
 };
 
@@ -215,8 +214,6 @@ static void edge_break(struct tty_struct *tty, int break_state);
 static int  edge_tiocmget(struct tty_struct *tty);
 static int  edge_tiocmset(struct tty_struct *tty,
 					unsigned int set, unsigned int clear);
-static int  edge_get_icount(struct tty_struct *tty,
-				struct serial_icounter_struct *icount);
 static int  edge_startup(struct usb_serial *serial);
 static void edge_disconnect(struct usb_serial *serial);
 static void edge_release(struct usb_serial *serial);
@@ -564,7 +561,6 @@ static void edge_interrupt_callback(struct urb *urb)
 	struct device *dev;
 	struct edgeport_port *edge_port;
 	struct usb_serial_port *port;
-	struct tty_struct *tty;
 	unsigned char *data = urb->transfer_buffer;
 	int length = urb->actual_length;
 	int bytes_avail;
@@ -643,12 +639,7 @@ static void edge_interrupt_callback(struct urb *urb)
 
 					/* tell the tty driver that something
 					   has changed */
-					tty = tty_port_tty_get(
-						&edge_port->port->port);
-					if (tty) {
-						tty_wakeup(tty);
-						tty_kref_put(tty);
-					}
+					tty_port_tty_wakeup(&edge_port->port->port);
 					/* Since we have more credit, check
 					   if more data can be sent */
 					send_more_port_data(edge_serial,
@@ -737,7 +728,6 @@ static void edge_bulk_in_callback(struct urb *urb)
 static void edge_bulk_out_data_callback(struct urb *urb)
 {
 	struct edgeport_port *edge_port = urb->context;
-	struct tty_struct *tty;
 	int status = urb->status;
 
 	if (status) {
@@ -746,14 +736,8 @@ static void edge_bulk_out_data_callback(struct urb *urb)
 			__func__, status);
 	}
 
-	tty = tty_port_tty_get(&edge_port->port->port);
-
-	if (tty && edge_port->open) {
-		/* let the tty driver wakeup if it has a special
-		   write_wakeup function */
-		tty_wakeup(tty);
-	}
-	tty_kref_put(tty);
+	if (edge_port->open)
+		tty_port_tty_wakeup(&edge_port->port->port);
 
 	/* Release the Write URB */
 	edge_port->write_in_progress = false;
@@ -772,7 +756,6 @@ static void edge_bulk_out_data_callback(struct urb *urb)
 static void edge_bulk_out_cmd_callback(struct urb *urb)
 {
 	struct edgeport_port *edge_port = urb->context;
-	struct tty_struct *tty;
 	int status = urb->status;
 
 	atomic_dec(&CmdUrbs);
@@ -793,13 +776,9 @@ static void edge_bulk_out_cmd_callback(struct urb *urb)
 		return;
 	}
 
-	/* Get pointer to tty */
-	tty = tty_port_tty_get(&edge_port->port->port);
-
 	/* tell the tty driver that something has changed */
-	if (tty && edge_port->open)
-		tty_wakeup(tty);
-	tty_kref_put(tty);
+	if (edge_port->open)
+		tty_port_tty_wakeup(&edge_port->port->port);
 
 	/* we have completed the command */
 	edge_port->commandPending = false;
@@ -885,9 +864,6 @@ static int edge_open(struct tty_struct *tty, struct usb_serial_port *port)
 	init_waitqueue_head(&edge_port->wait_chase);
 	init_waitqueue_head(&edge_port->wait_command);
 
-	/* initialize our icount structure */
-	memset(&(edge_port->icount), 0x00, sizeof(edge_port->icount));
-
 	/* initialize our port settings */
 	edge_port->txCredits = 0;	/* Can't send any data yet */
 	/* Must always set this bit to enable ints! */
@@ -1314,7 +1290,7 @@ static void send_more_port_data(struct edgeport_serial *edge_serial,
 
 	/* decrement the number of credits we have by the number we just sent */
 	edge_port->txCredits -= count;
-	edge_port->icount.tx += count;
+	edge_port->port->icount.tx += count;
 
 	status = usb_submit_urb(urb, GFP_ATOMIC);
 	if (status) {
@@ -1326,7 +1302,7 @@ static void send_more_port_data(struct edgeport_serial *edge_serial,
 
 		/* revert the credits as something bad happened. */
 		edge_port->txCredits += count;
-		edge_port->icount.tx -= count;
+		edge_port->port->icount.tx -= count;
 	}
 	dev_dbg(dev, "%s wrote %d byte(s) TxCredit %d, Fifo %d\n",
 		__func__, count, edge_port->txCredits, fifo->count);
@@ -1588,31 +1564,6 @@ static int edge_tiocmget(struct tty_struct *tty)
 	return result;
 }
 
-static int edge_get_icount(struct tty_struct *tty,
-				struct serial_icounter_struct *icount)
-{
-	struct usb_serial_port *port = tty->driver_data;
-	struct edgeport_port *edge_port = usb_get_serial_port_data(port);
-	struct async_icount cnow;
-	cnow = edge_port->icount;
-
-	icount->cts = cnow.cts;
-	icount->dsr = cnow.dsr;
-	icount->rng = cnow.rng;
-	icount->dcd = cnow.dcd;
-	icount->rx = cnow.rx;
-	icount->tx = cnow.tx;
-	icount->frame = cnow.frame;
-	icount->overrun = cnow.overrun;
-	icount->parity = cnow.parity;
-	icount->brk = cnow.brk;
-	icount->buf_overrun = cnow.buf_overrun;
-
-	dev_dbg(&port->dev, "%s (%d) TIOCGICOUNT RX=%d, TX=%d\n", __func__,
-		port->number, icount->rx, icount->tx);
-	return 0;
-}
-
 static int get_serial_info(struct edgeport_port *edge_port,
 				struct serial_struct __user *retinfo)
 {
@@ -1649,8 +1600,6 @@ static int edge_ioctl(struct tty_struct *tty,
 	struct usb_serial_port *port = tty->driver_data;
 	DEFINE_WAIT(wait);
 	struct edgeport_port *edge_port = usb_get_serial_port_data(port);
-	struct async_icount cnow;
-	struct async_icount cprev;
 
 	dev_dbg(&port->dev, "%s - port %d, cmd = 0x%x\n", __func__, port->number, cmd);
 
@@ -1662,37 +1611,6 @@ static int edge_ioctl(struct tty_struct *tty,
 	case TIOCGSERIAL:
 		dev_dbg(&port->dev, "%s (%d) TIOCGSERIAL\n", __func__,  port->number);
 		return get_serial_info(edge_port, (struct serial_struct __user *) arg);
-
-	case TIOCMIWAIT:
-		dev_dbg(&port->dev, "%s (%d) TIOCMIWAIT\n", __func__,  port->number);
-		cprev = edge_port->icount;
-		while (1) {
-			prepare_to_wait(&port->delta_msr_wait,
-						&wait, TASK_INTERRUPTIBLE);
-			schedule();
-			finish_wait(&port->delta_msr_wait, &wait);
-			/* see if a signal did it */
-			if (signal_pending(current))
-				return -ERESTARTSYS;
-
-			if (port->serial->disconnected)
-				return -EIO;
-
-			cnow = edge_port->icount;
-			if (cnow.rng == cprev.rng && cnow.dsr == cprev.dsr &&
-			    cnow.dcd == cprev.dcd && cnow.cts == cprev.cts)
-				return -EIO; /* no change => error */
-			if (((arg & TIOCM_RNG) && (cnow.rng != cprev.rng)) ||
-			    ((arg & TIOCM_DSR) && (cnow.dsr != cprev.dsr)) ||
-			    ((arg & TIOCM_CD)  && (cnow.dcd != cprev.dcd)) ||
-			    ((arg & TIOCM_CTS) && (cnow.cts != cprev.cts))) {
-				return 0;
-			}
-			cprev = cnow;
-		}
-		/* NOTREACHED */
-		break;
-
 	}
 	return -ENOIOCTLCMD;
 }
@@ -1866,7 +1784,7 @@ static void process_rcvd_data(struct edgeport_serial *edge_serial,
 						edge_serial->rxPort);
 					edge_tty_recv(edge_port->port, buffer,
 							rxLen);
-					edge_port->icount.rx += rxLen;
+					edge_port->port->icount.rx += rxLen;
 				}
 				buffer += rxLen;
 			}
@@ -2042,7 +1960,7 @@ static void handle_new_msr(struct edgeport_port *edge_port, __u8 newMsr)
 
 	if (newMsr & (EDGEPORT_MSR_DELTA_CTS | EDGEPORT_MSR_DELTA_DSR |
 			EDGEPORT_MSR_DELTA_RI | EDGEPORT_MSR_DELTA_CD)) {
-		icount = &edge_port->icount;
+		icount = &edge_port->port->icount;
 
 		/* update input line counters */
 		if (newMsr & EDGEPORT_MSR_DELTA_CTS)
@@ -2053,7 +1971,7 @@ static void handle_new_msr(struct edgeport_port *edge_port, __u8 newMsr)
 			icount->dcd++;
 		if (newMsr & EDGEPORT_MSR_DELTA_RI)
 			icount->rng++;
-		wake_up_interruptible(&edge_port->port->delta_msr_wait);
+		wake_up_interruptible(&edge_port->port->port.delta_msr_wait);
 	}
 
 	/* Save the new modem status */
@@ -2088,7 +2006,7 @@ static void handle_new_lsr(struct edgeport_port *edge_port, __u8 lsrData,
 		edge_tty_recv(edge_port->port, &data, 1);
 
 	/* update input line counters */
-	icount = &edge_port->icount;
+	icount = &edge_port->port->icount;
 	if (newLsr & LSR_BREAK)
 		icount->brk++;
 	if (newLsr & LSR_OVER_ERR)
diff --git a/drivers/usb/serial/io_tables.h b/drivers/usb/serial/io_tables.h
index 1511dd0ad32..ae5fac5656c 100644
--- a/drivers/usb/serial/io_tables.h
+++ b/drivers/usb/serial/io_tables.h
@@ -116,7 +116,8 @@ static struct usb_serial_driver edgeport_2port_device = {
 	.set_termios		= edge_set_termios,
 	.tiocmget		= edge_tiocmget,
 	.tiocmset		= edge_tiocmset,
-	.get_icount		= edge_get_icount,
+	.tiocmiwait		= usb_serial_generic_tiocmiwait,
+	.get_icount		= usb_serial_generic_get_icount,
 	.write			= edge_write,
 	.write_room		= edge_write_room,
 	.chars_in_buffer	= edge_chars_in_buffer,
@@ -147,7 +148,8 @@ static struct usb_serial_driver edgeport_4port_device = {
 	.set_termios		= edge_set_termios,
 	.tiocmget		= edge_tiocmget,
 	.tiocmset		= edge_tiocmset,
-	.get_icount		= edge_get_icount,
+	.tiocmiwait		= usb_serial_generic_tiocmiwait,
+	.get_icount		= usb_serial_generic_get_icount,
 	.write			= edge_write,
 	.write_room		= edge_write_room,
 	.chars_in_buffer	= edge_chars_in_buffer,
@@ -178,7 +180,8 @@ static struct usb_serial_driver edgeport_8port_device = {
 	.set_termios		= edge_set_termios,
 	.tiocmget		= edge_tiocmget,
 	.tiocmset		= edge_tiocmset,
-	.get_icount		= edge_get_icount,
+	.tiocmiwait		= usb_serial_generic_tiocmiwait,
+	.get_icount		= usb_serial_generic_get_icount,
 	.write			= edge_write,
 	.write_room		= edge_write_room,
 	.chars_in_buffer	= edge_chars_in_buffer,
@@ -209,7 +212,8 @@ static struct usb_serial_driver epic_device = {
 	.set_termios		= edge_set_termios,
 	.tiocmget		= edge_tiocmget,
 	.tiocmset		= edge_tiocmset,
-	.get_icount		= edge_get_icount,
+	.tiocmiwait		= usb_serial_generic_tiocmiwait,
+	.get_icount		= usb_serial_generic_get_icount,
 	.write			= edge_write,
 	.write_room		= edge_write_room,
 	.chars_in_buffer	= edge_chars_in_buffer,
diff --git a/drivers/usb/serial/io_ti.c b/drivers/usb/serial/io_ti.c
index 7777172206d..158bf4bc29c 100644
--- a/drivers/usb/serial/io_ti.c
+++ b/drivers/usb/serial/io_ti.c
@@ -86,7 +86,7 @@ struct edgeport_port {
 	int baud_rate;
 	int close_pending;
 	int lsr_event;
-	struct async_icount	icount;
+
 	struct edgeport_serial	*edge_serial;
 	struct usb_serial_port	*port;
 	__u8 bUartMode;		/* Port type, 0: RS232, etc. */
@@ -206,7 +206,7 @@ static int restart_read(struct edgeport_port *edge_port);
 
 static void edge_set_termios(struct tty_struct *tty,
 		struct usb_serial_port *port, struct ktermios *old_termios);
-static void edge_send(struct tty_struct *tty);
+static void edge_send(struct usb_serial_port *port, struct tty_struct *tty);
 
 /* sysfs attributes */
 static int edge_create_sysfs_attrs(struct usb_serial_port *port);
@@ -1445,7 +1445,7 @@ static void handle_new_msr(struct edgeport_port *edge_port, __u8 msr)
 
 	if (msr & (EDGEPORT_MSR_DELTA_CTS | EDGEPORT_MSR_DELTA_DSR |
 			EDGEPORT_MSR_DELTA_RI | EDGEPORT_MSR_DELTA_CD)) {
-		icount = &edge_port->icount;
+		icount = &edge_port->port->icount;
 
 		/* update input line counters */
 		if (msr & EDGEPORT_MSR_DELTA_CTS)
@@ -1456,7 +1456,7 @@ static void handle_new_msr(struct edgeport_port *edge_port, __u8 msr)
 			icount->dcd++;
 		if (msr & EDGEPORT_MSR_DELTA_RI)
 			icount->rng++;
-		wake_up_interruptible(&edge_port->port->delta_msr_wait);
+		wake_up_interruptible(&edge_port->port->port.delta_msr_wait);
 	}
 
 	/* Save the new modem status */
@@ -1498,7 +1498,7 @@ static void handle_new_lsr(struct edgeport_port *edge_port, int lsr_data,
 		edge_tty_recv(edge_port->port, &data, 1);
 
 	/* update input line counters */
-	icount = &edge_port->icount;
+	icount = &edge_port->port->icount;
 	if (new_lsr & LSR_BREAK)
 		icount->brk++;
 	if (new_lsr & LSR_OVER_ERR)
@@ -1657,7 +1657,7 @@ static void edge_bulk_in_callback(struct urb *urb)
 		else
 			edge_tty_recv(edge_port->port, data,
 					urb->actual_length);
-		edge_port->icount.rx += urb->actual_length;
+		edge_port->port->icount.rx += urb->actual_length;
 	}
 
 exit:
@@ -1712,7 +1712,7 @@ static void edge_bulk_out_callback(struct urb *urb)
 
 	/* send any buffered data */
 	tty = tty_port_tty_get(&port->port);
-	edge_send(tty);
+	edge_send(port, tty);
 	tty_kref_put(tty);
 }
 
@@ -1750,8 +1750,6 @@ static int edge_open(struct tty_struct *tty, struct usb_serial_port *port)
 
 	dev = port->serial->dev;
 
-	memset(&(edge_port->icount), 0x00, sizeof(edge_port->icount));
-
 	/* turn off loopback */
 	status = ti_do_config(edge_port, UMPC_SET_CLR_LOOPBACK, 0);
 	if (status) {
@@ -1909,21 +1907,10 @@ static void edge_close(struct usb_serial_port *port)
 	kfifo_reset_out(&edge_port->write_fifo);
 	spin_unlock_irqrestore(&edge_port->ep_lock, flags);
 
-	/* assuming we can still talk to the device,
-	 * send a close port command to it */
 	dev_dbg(&port->dev, "%s - send umpc_close_port\n", __func__);
 	port_number = port->number - port->serial->minor;
-
-	mutex_lock(&serial->disc_mutex);
-	if (!serial->disconnected) {
-		send_cmd(serial->dev,
-				     UMPC_CLOSE_PORT,
-				     (__u8)(UMPM_UART1_PORT + port_number),
-				     0,
-				     NULL,
-				     0);
-	}
-	mutex_unlock(&serial->disc_mutex);
+	send_cmd(serial->dev, UMPC_CLOSE_PORT,
+		     (__u8)(UMPM_UART1_PORT + port_number), 0, NULL, 0);
 
 	mutex_lock(&edge_serial->es_lock);
 	--edge_port->edge_serial->num_ports_open;
@@ -1953,14 +1940,13 @@ static int edge_write(struct tty_struct *tty, struct usb_serial_port *port,
 
 	count = kfifo_in_locked(&edge_port->write_fifo, data, count,
 							&edge_port->ep_lock);
-	edge_send(tty);
+	edge_send(port, tty);
 
 	return count;
 }
 
-static void edge_send(struct tty_struct *tty)
+static void edge_send(struct usb_serial_port *port, struct tty_struct *tty)
 {
-	struct usb_serial_port *port = tty->driver_data;
 	int count, result;
 	struct edgeport_port *edge_port = usb_get_serial_port_data(port);
 	unsigned long flags;
@@ -1999,7 +1985,7 @@ static void edge_send(struct tty_struct *tty)
 		edge_port->ep_write_urb_in_use = 0;
 		/* TODO: reschedule edge_send */
 	} else
-		edge_port->icount.tx += count;
+		edge_port->port->icount.tx += count;
 
 	/* wakeup any process waiting for writes to complete */
 	/* there is now more room in the buffer for new writes */
@@ -2360,27 +2346,6 @@ static int edge_tiocmget(struct tty_struct *tty)
 	return result;
 }
 
-static int edge_get_icount(struct tty_struct *tty,
-				struct serial_icounter_struct *icount)
-{
-	struct usb_serial_port *port = tty->driver_data;
-	struct edgeport_port *edge_port = usb_get_serial_port_data(port);
-	struct async_icount *ic = &edge_port->icount;
-
-	icount->cts = ic->cts;
-	icount->dsr = ic->dsr;
-	icount->rng = ic->rng;
-	icount->dcd = ic->dcd;
-	icount->tx = ic->tx;
-        icount->rx = ic->rx;
-        icount->frame = ic->frame;
-        icount->parity = ic->parity;
-        icount->overrun = ic->overrun;
-        icount->brk = ic->brk;
-        icount->buf_overrun = ic->buf_overrun;
-	return 0;
-}
-
 static int get_serial_info(struct edgeport_port *edge_port,
 				struct serial_struct __user *retinfo)
 {
@@ -2392,7 +2357,7 @@ static int get_serial_info(struct edgeport_port *edge_port,
 
 	cwait = edge_port->port->port.closing_wait;
 	if (cwait != ASYNC_CLOSING_WAIT_NONE)
-		cwait = jiffies_to_msecs(closing_wait) / 10;
+		cwait = jiffies_to_msecs(cwait) / 10;
 
 	memset(&tmp, 0, sizeof(tmp));
 
@@ -2416,8 +2381,6 @@ static int edge_ioctl(struct tty_struct *tty,
 {
 	struct usb_serial_port *port = tty->driver_data;
 	struct edgeport_port *edge_port = usb_get_serial_port_data(port);
-	struct async_icount cnow;
-	struct async_icount cprev;
 
 	dev_dbg(&port->dev, "%s - port %d, cmd = 0x%x\n", __func__, port->number, cmd);
 
@@ -2426,32 +2389,6 @@ static int edge_ioctl(struct tty_struct *tty,
 		dev_dbg(&port->dev, "%s - TIOCGSERIAL\n", __func__);
 		return get_serial_info(edge_port,
 				(struct serial_struct __user *) arg);
-	case TIOCMIWAIT:
-		dev_dbg(&port->dev, "%s - TIOCMIWAIT\n", __func__);
-		cprev = edge_port->icount;
-		while (1) {
-			interruptible_sleep_on(&port->delta_msr_wait);
-			/* see if a signal did it */
-			if (signal_pending(current))
-				return -ERESTARTSYS;
-
-			if (port->serial->disconnected)
-				return -EIO;
-
-			cnow = edge_port->icount;
-			if (cnow.rng == cprev.rng && cnow.dsr == cprev.dsr &&
-			    cnow.dcd == cprev.dcd && cnow.cts == cprev.cts)
-				return -EIO; /* no change => error */
-			if (((arg & TIOCM_RNG) && (cnow.rng != cprev.rng)) ||
-			    ((arg & TIOCM_DSR) && (cnow.dsr != cprev.dsr)) ||
-			    ((arg & TIOCM_CD)  && (cnow.dcd != cprev.dcd)) ||
-			    ((arg & TIOCM_CTS) && (cnow.cts != cprev.cts))) {
-				return 0;
-			}
-			cprev = cnow;
-		}
-		/* not reached */
-		break;
 	}
 	return -ENOIOCTLCMD;
 }
@@ -2463,8 +2400,6 @@ static void edge_break(struct tty_struct *tty, int break_state)
 	int status;
 	int bv = 0;	/* Off */
 
-	tty_wait_until_sent(tty, 0);
-
 	if (break_state == -1)
 		bv = 1;	/* On */
 	status = ti_do_config(edge_port, UMPC_SET_CLR_BREAK, bv);
@@ -2546,7 +2481,6 @@ static int edge_port_remove(struct usb_serial_port *port)
 	struct edgeport_port *edge_port;
 
 	edge_port = usb_get_serial_port_data(port);
-
 	edge_remove_sysfs_attrs(port);
 	kfifo_free(&edge_port->write_fifo);
 	kfree(edge_port);
@@ -2618,7 +2552,8 @@ static struct usb_serial_driver edgeport_1port_device = {
 	.set_termios		= edge_set_termios,
 	.tiocmget		= edge_tiocmget,
 	.tiocmset		= edge_tiocmset,
-	.get_icount		= edge_get_icount,
+	.tiocmiwait		= usb_serial_generic_tiocmiwait,
+	.get_icount		= usb_serial_generic_get_icount,
 	.write			= edge_write,
 	.write_room		= edge_write_room,
 	.chars_in_buffer	= edge_chars_in_buffer,
@@ -2649,7 +2584,8 @@ static struct usb_serial_driver edgeport_2port_device = {
 	.set_termios		= edge_set_termios,
 	.tiocmget		= edge_tiocmget,
 	.tiocmset		= edge_tiocmset,
-	.get_icount		= edge_get_icount,
+	.tiocmiwait		= usb_serial_generic_tiocmiwait,
+	.get_icount		= usb_serial_generic_get_icount,
 	.write			= edge_write,
 	.write_room		= edge_write_room,
 	.chars_in_buffer	= edge_chars_in_buffer,
diff --git a/drivers/usb/serial/iuu_phoenix.c b/drivers/usb/serial/iuu_phoenix.c
index ff77027160a..9d74c278b7b 100644
--- a/drivers/usb/serial/iuu_phoenix.c
+++ b/drivers/usb/serial/iuu_phoenix.c
@@ -55,7 +55,6 @@ static void read_rxcmd_callback(struct urb *urb);
 
 struct iuu_private {
 	spinlock_t lock;	/* store irq state */
-	wait_queue_head_t delta_msr_wait;
 	u8 line_status;
 	int tiostatus;		/* store IUART SIGNAL for tiocmget call */
 	u8 reset;		/* if 1 reset is needed */
@@ -94,7 +93,6 @@ static int iuu_port_probe(struct usb_serial_port *port)
 
 	priv->vcc = vcc_default;
 	spin_lock_init(&priv->lock);
-	init_waitqueue_head(&priv->delta_msr_wait);
 
 	usb_set_serial_port_data(port, priv);
 
@@ -944,22 +942,13 @@ static void iuu_set_termios(struct tty_struct *tty,
 static void iuu_close(struct usb_serial_port *port)
 {
 	/* iuu_led (port,255,0,0,0); */
-	struct usb_serial *serial;
-
-	serial = port->serial;
-	if (!serial)
-		return;
 
 	iuu_uart_off(port);
-	if (serial->dev) {
-		/* free writebuf */
-		/* shutdown our urbs */
-		dev_dbg(&port->dev, "%s - shutting down urbs\n", __func__);
-		usb_kill_urb(port->write_urb);
-		usb_kill_urb(port->read_urb);
-		usb_kill_urb(port->interrupt_in_urb);
-		iuu_led(port, 0, 0, 0xF000, 0xFF);
-	}
+
+	usb_kill_urb(port->write_urb);
+	usb_kill_urb(port->read_urb);
+
+	iuu_led(port, 0, 0, 0xF000, 0xFF);
 }
 
 static void iuu_init_termios(struct tty_struct *tty)
diff --git a/drivers/usb/serial/keyspan.c b/drivers/usb/serial/keyspan.c
index 1fd1935c831..eb30d7b01f3 100644
--- a/drivers/usb/serial/keyspan.c
+++ b/drivers/usb/serial/keyspan.c
@@ -378,7 +378,6 @@ static void	usa26_instat_callback(struct urb *urb)
 	struct usb_serial			*serial;
 	struct usb_serial_port			*port;
 	struct keyspan_port_private	 	*p_priv;
-	struct tty_struct			*tty;
 	int old_dcd_state, err;
 	int status = urb->status;
 
@@ -421,12 +420,8 @@ static void	usa26_instat_callback(struct urb *urb)
 	p_priv->dcd_state = ((msg->gpia_dcd) ? 1 : 0);
 	p_priv->ri_state = ((msg->ri) ? 1 : 0);
 
-	if (old_dcd_state != p_priv->dcd_state) {
-		tty = tty_port_tty_get(&port->port);
-		if (tty && !C_CLOCAL(tty))
-			tty_hangup(tty);
-		tty_kref_put(tty);
-	}
+	if (old_dcd_state != p_priv->dcd_state)
+		tty_port_tty_hangup(&port->port, true);
 
 	/* Resubmit urb so we continue receiving */
 	err = usb_submit_urb(urb, GFP_ATOMIC);
@@ -510,7 +505,6 @@ static void	usa28_instat_callback(struct urb *urb)
 	struct usb_serial			*serial;
 	struct usb_serial_port			*port;
 	struct keyspan_port_private	 	*p_priv;
-	struct tty_struct			*tty;
 	int old_dcd_state;
 	int status = urb->status;
 
@@ -551,12 +545,8 @@ static void	usa28_instat_callback(struct urb *urb)
 	p_priv->dcd_state = ((msg->dcd) ? 1 : 0);
 	p_priv->ri_state = ((msg->ri) ? 1 : 0);
 
-	if (old_dcd_state != p_priv->dcd_state && old_dcd_state) {
-		tty = tty_port_tty_get(&port->port);
-		if (tty && !C_CLOCAL(tty))
-			tty_hangup(tty);
-		tty_kref_put(tty);
-	}
+	if (old_dcd_state != p_priv->dcd_state && old_dcd_state)
+		tty_port_tty_hangup(&port->port, true);
 
 		/* Resubmit urb so we continue receiving */
 	err = usb_submit_urb(urb, GFP_ATOMIC);
@@ -642,12 +632,8 @@ static void	usa49_instat_callback(struct urb *urb)
 	p_priv->dcd_state = ((msg->dcd) ? 1 : 0);
 	p_priv->ri_state = ((msg->ri) ? 1 : 0);
 
-	if (old_dcd_state != p_priv->dcd_state && old_dcd_state) {
-		struct tty_struct *tty = tty_port_tty_get(&port->port);
-		if (tty && !C_CLOCAL(tty))
-			tty_hangup(tty);
-		tty_kref_put(tty);
-	}
+	if (old_dcd_state != p_priv->dcd_state && old_dcd_state)
+		tty_port_tty_hangup(&port->port, true);
 
 	/* Resubmit urb so we continue receiving */
 	err = usb_submit_urb(urb, GFP_ATOMIC);
@@ -726,45 +712,45 @@ static void usa49wg_indat_callback(struct urb *urb)
 	i = 0;
 	len = 0;
 
-	if (urb->actual_length) {
-		while (i < urb->actual_length) {
+	while (i < urb->actual_length) {
 
-			/* Check port number from message*/
-			if (data[i] >= serial->num_ports) {
-				dev_dbg(&urb->dev->dev, "%s - Unexpected port number %d\n",
-					__func__, data[i]);
-				return;
-			}
-			port = serial->port[data[i++]];
-			len = data[i++];
+		/* Check port number from message */
+		if (data[i] >= serial->num_ports) {
+			dev_dbg(&urb->dev->dev, "%s - Unexpected port number %d\n",
+				__func__, data[i]);
+			return;
+		}
+		port = serial->port[data[i++]];
+		len = data[i++];
 
-			/* 0x80 bit is error flag */
-			if ((data[i] & 0x80) == 0) {
-				/* no error on any byte */
-				i++;
-				for (x = 1; x < len ; ++x)
-					tty_insert_flip_char(&port->port,
-							data[i++], 0);
-			} else {
-				/*
-				 * some bytes had errors, every byte has status
-				 */
-				for (x = 0; x + 1 < len; x += 2) {
-					int stat = data[i], flag = 0;
-					if (stat & RXERROR_OVERRUN)
-						flag |= TTY_OVERRUN;
-					if (stat & RXERROR_FRAMING)
-						flag |= TTY_FRAME;
-					if (stat & RXERROR_PARITY)
-						flag |= TTY_PARITY;
-					/* XXX should handle break (0x10) */
-					tty_insert_flip_char(&port->port,
-							data[i+1], flag);
-					i += 2;
-				}
+		/* 0x80 bit is error flag */
+		if ((data[i] & 0x80) == 0) {
+			/* no error on any byte */
+			i++;
+			for (x = 1; x < len && i < urb->actual_length; ++x)
+				tty_insert_flip_char(&port->port,
+						data[i++], 0);
+		} else {
+			/*
+			 * some bytes had errors, every byte has status
+			 */
+			for (x = 0; x + 1 < len &&
+				    i + 1 < urb->actual_length; x += 2) {
+				int stat = data[i], flag = 0;
+
+				if (stat & RXERROR_OVERRUN)
+					flag |= TTY_OVERRUN;
+				if (stat & RXERROR_FRAMING)
+					flag |= TTY_FRAME;
+				if (stat & RXERROR_PARITY)
+					flag |= TTY_PARITY;
+				/* XXX should handle break (0x10) */
+				tty_insert_flip_char(&port->port, data[i+1],
+						     flag);
+				i += 2;
 			}
-			tty_flip_buffer_push(&port->port);
 		}
+		tty_flip_buffer_push(&port->port);
 	}
 
 	/* Resubmit urb so we continue receiving */
@@ -851,7 +837,6 @@ static void	usa90_instat_callback(struct urb *urb)
 	struct usb_serial			*serial;
 	struct usb_serial_port			*port;
 	struct keyspan_port_private	 	*p_priv;
-	struct tty_struct			*tty;
 	int old_dcd_state, err;
 	int status = urb->status;
 
@@ -880,12 +865,8 @@ static void	usa90_instat_callback(struct urb *urb)
 	p_priv->dcd_state = ((msg->dcd) ? 1 : 0);
 	p_priv->ri_state = ((msg->ri) ? 1 : 0);
 
-	if (old_dcd_state != p_priv->dcd_state && old_dcd_state) {
-		tty = tty_port_tty_get(&port->port);
-		if (tty && !C_CLOCAL(tty))
-			tty_hangup(tty);
-		tty_kref_put(tty);
-	}
+	if (old_dcd_state != p_priv->dcd_state && old_dcd_state)
+		tty_port_tty_hangup(&port->port, true);
 
 	/* Resubmit urb so we continue receiving */
 	err = usb_submit_urb(urb, GFP_ATOMIC);
@@ -953,12 +934,8 @@ static void	usa67_instat_callback(struct urb *urb)
 	p_priv->cts_state = ((msg->hskia_cts) ? 1 : 0);
 	p_priv->dcd_state = ((msg->gpia_dcd) ? 1 : 0);
 
-	if (old_dcd_state != p_priv->dcd_state && old_dcd_state) {
-		struct tty_struct *tty = tty_port_tty_get(&port->port);
-		if (tty && !C_CLOCAL(tty))
-			tty_hangup(tty);
-		tty_kref_put(tty);
-	}
+	if (old_dcd_state != p_priv->dcd_state && old_dcd_state)
+		tty_port_tty_hangup(&port->port, true);
 
 	/* Resubmit urb so we continue receiving */
 	err = usb_submit_urb(urb, GFP_ATOMIC);
@@ -1115,7 +1092,6 @@ static void keyspan_dtr_rts(struct usb_serial_port *port, int on)
 static void keyspan_close(struct usb_serial_port *port)
 {
 	int			i;
-	struct usb_serial	*serial = port->serial;
 	struct keyspan_port_private 	*p_priv;
 
 	p_priv = usb_get_serial_port_data(port);
@@ -1123,28 +1099,17 @@ static void keyspan_close(struct usb_serial_port *port)
 	p_priv->rts_state = 0;
 	p_priv->dtr_state = 0;
 
-	if (serial->dev) {
-		keyspan_send_setup(port, 2);
-		/* pilot-xfer seems to work best with this delay */
-		mdelay(100);
-		/* keyspan_set_termios(port, NULL); */
-	}
-
-	/*while (p_priv->outcont_urb->status == -EINPROGRESS) {
-		dev_dbg(&port->dev, "%s - urb in progress\n", __func__);
-	}*/
+	keyspan_send_setup(port, 2);
+	/* pilot-xfer seems to work best with this delay */
+	mdelay(100);
 
 	p_priv->out_flip = 0;
 	p_priv->in_flip = 0;
 
-	if (serial->dev) {
-		/* Stop reading/writing urbs */
-		stop_urb(p_priv->inack_urb);
-		/* stop_urb(p_priv->outcont_urb); */
-		for (i = 0; i < 2; i++) {
-			stop_urb(p_priv->in_urbs[i]);
-			stop_urb(p_priv->out_urbs[i]);
-		}
+	stop_urb(p_priv->inack_urb);
+	for (i = 0; i < 2; i++) {
+		stop_urb(p_priv->in_urbs[i]);
+		stop_urb(p_priv->out_urbs[i]);
 	}
 }
 
diff --git a/drivers/usb/serial/keyspan_pda.c b/drivers/usb/serial/keyspan_pda.c
index 3b17d5d13dc..5f1d382e55c 100644
--- a/drivers/usb/serial/keyspan_pda.c
+++ b/drivers/usb/serial/keyspan_pda.c
@@ -104,10 +104,8 @@ static void keyspan_pda_wakeup_write(struct work_struct *work)
 	struct keyspan_pda_private *priv =
 		container_of(work, struct keyspan_pda_private, wakeup_work);
 	struct usb_serial_port *port = priv->port;
-	struct tty_struct *tty = tty_port_tty_get(&port->port);
-	if (tty)
-		tty_wakeup(tty);
-	tty_kref_put(tty);
+
+	tty_port_tty_wakeup(&port->port);
 }
 
 static void keyspan_pda_request_unthrottle(struct work_struct *work)
@@ -595,12 +593,10 @@ static void keyspan_pda_dtr_rts(struct usb_serial_port *port, int on)
 {
 	struct usb_serial *serial = port->serial;
 
-	if (serial->dev) {
-		if (on)
-			keyspan_pda_set_modem_info(serial, (1<<7) | (1<< 2));
-		else
-			keyspan_pda_set_modem_info(serial, 0);
-	}
+	if (on)
+		keyspan_pda_set_modem_info(serial, (1 << 7) | (1 << 2));
+	else
+		keyspan_pda_set_modem_info(serial, 0);
 }
 
 
@@ -651,13 +647,8 @@ error:
 }
 static void keyspan_pda_close(struct usb_serial_port *port)
 {
-	struct usb_serial *serial = port->serial;
-
-	if (serial->dev) {
-		/* shutdown our bulk reads and writes */
-		usb_kill_urb(port->write_urb);
-		usb_kill_urb(port->interrupt_in_urb);
-	}
+	usb_kill_urb(port->write_urb);
+	usb_kill_urb(port->interrupt_in_urb);
 }
 
 
diff --git a/drivers/usb/serial/kl5kusb105.c b/drivers/usb/serial/kl5kusb105.c
index 769d910ae0a..1b4054fe52a 100644
--- a/drivers/usb/serial/kl5kusb105.c
+++ b/drivers/usb/serial/kl5kusb105.c
@@ -341,28 +341,20 @@ static void klsi_105_close(struct usb_serial_port *port)
 {
 	int rc;
 
-	mutex_lock(&port->serial->disc_mutex);
-	if (!port->serial->disconnected) {
-		/* send READ_OFF */
-		rc = usb_control_msg(port->serial->dev,
-				     usb_sndctrlpipe(port->serial->dev, 0),
-				     KL5KUSB105A_SIO_CONFIGURE,
-				     USB_TYPE_VENDOR | USB_DIR_OUT,
-				     KL5KUSB105A_SIO_CONFIGURE_READ_OFF,
-				     0, /* index */
-				     NULL, 0,
-				     KLSI_TIMEOUT);
-		if (rc < 0)
-			dev_err(&port->dev,
-				"Disabling read failed (error = %d)\n", rc);
-	}
-	mutex_unlock(&port->serial->disc_mutex);
+	/* send READ_OFF */
+	rc = usb_control_msg(port->serial->dev,
+			     usb_sndctrlpipe(port->serial->dev, 0),
+			     KL5KUSB105A_SIO_CONFIGURE,
+			     USB_TYPE_VENDOR | USB_DIR_OUT,
+			     KL5KUSB105A_SIO_CONFIGURE_READ_OFF,
+			     0, /* index */
+			     NULL, 0,
+			     KLSI_TIMEOUT);
+	if (rc < 0)
+		dev_err(&port->dev, "failed to disable read: %d\n", rc);
 
 	/* shutdown our bulk reads and writes */
 	usb_serial_generic_close(port);
-
-	/* wgg - do I need this? I think so. */
-	usb_kill_urb(port->interrupt_in_urb);
 }
 
 /* We need to write a complete 64-byte data block and encode the
diff --git a/drivers/usb/serial/kobil_sct.c b/drivers/usb/serial/kobil_sct.c
index 903d938e174..78b48c31abf 100644
--- a/drivers/usb/serial/kobil_sct.c
+++ b/drivers/usb/serial/kobil_sct.c
@@ -65,7 +65,7 @@ static int  kobil_tiocmget(struct tty_struct *tty);
 static int  kobil_tiocmset(struct tty_struct *tty,
 			   unsigned int set, unsigned int clear);
 static void kobil_read_int_callback(struct urb *urb);
-static void kobil_write_callback(struct urb *purb);
+static void kobil_write_int_callback(struct urb *urb);
 static void kobil_set_termios(struct tty_struct *tty,
 			struct usb_serial_port *port, struct ktermios *old);
 static void kobil_init_termios(struct tty_struct *tty);
@@ -99,6 +99,7 @@ static struct usb_serial_driver kobil_device = {
 	.write =		kobil_write,
 	.write_room =		kobil_write_room,
 	.read_int_callback =	kobil_read_int_callback,
+	.write_int_callback =	kobil_write_int_callback,
 };
 
 static struct usb_serial_driver * const serial_drivers[] = {
@@ -106,8 +107,6 @@ static struct usb_serial_driver * const serial_drivers[] = {
 };
 
 struct kobil_private {
-	int write_int_endpoint_address;
-	int read_int_endpoint_address;
 	unsigned char buf[KOBIL_BUF_LENGTH]; /* buffer for the APDU to send */
 	int filled;  /* index of the last char in buf */
 	int cur_pos; /* index of the next char to send in buf */
@@ -117,14 +116,8 @@ struct kobil_private {
 
 static int kobil_port_probe(struct usb_serial_port *port)
 {
-	int i;
 	struct usb_serial *serial = port->serial;
 	struct kobil_private *priv;
-	struct usb_device *pdev;
-	struct usb_host_config *actconfig;
-	struct usb_interface *interface;
-	struct usb_host_interface *altsetting;
-	struct usb_host_endpoint *endpoint;
 
 	priv = kmalloc(sizeof(struct kobil_private), GFP_KERNEL);
 	if (!priv)
@@ -150,30 +143,6 @@ static int kobil_port_probe(struct usb_serial_port *port)
 	}
 	usb_set_serial_port_data(port, priv);
 
-	/* search for the necessary endpoints */
-	pdev = serial->dev;
-	actconfig = pdev->actconfig;
-	interface = actconfig->interface[0];
-	altsetting = interface->cur_altsetting;
-	endpoint = altsetting->endpoint;
-
-	for (i = 0; i < altsetting->desc.bNumEndpoints; i++) {
-		endpoint = &altsetting->endpoint[i];
-		if (usb_endpoint_is_int_out(&endpoint->desc)) {
-			dev_dbg(&serial->dev->dev,
-				"%s Found interrupt out endpoint. Address: %d\n",
-				__func__, endpoint->desc.bEndpointAddress);
-			priv->write_int_endpoint_address =
-				endpoint->desc.bEndpointAddress;
-		}
-		if (usb_endpoint_is_int_in(&endpoint->desc)) {
-			dev_dbg(&serial->dev->dev,
-				"%s Found interrupt in  endpoint. Address: %d\n",
-				__func__, endpoint->desc.bEndpointAddress);
-			priv->read_int_endpoint_address =
-				endpoint->desc.bEndpointAddress;
-		}
-	}
 	return 0;
 }
 
@@ -205,7 +174,6 @@ static int kobil_open(struct tty_struct *tty, struct usb_serial_port *port)
 	struct kobil_private *priv;
 	unsigned char *transfer_buffer;
 	int transfer_buffer_length = 8;
-	int write_urb_transfer_buffer_length = 8;
 
 	priv = usb_get_serial_port_data(port);
 
@@ -214,27 +182,6 @@ static int kobil_open(struct tty_struct *tty, struct usb_serial_port *port)
 	if (!transfer_buffer)
 		return -ENOMEM;
 
-	/* allocate write_urb */
-	if (!port->write_urb) {
-		dev_dbg(dev, "%s - Allocating port->write_urb\n", __func__);
-		port->write_urb = usb_alloc_urb(0, GFP_KERNEL);
-		if (!port->write_urb) {
-			dev_dbg(dev, "%s - usb_alloc_urb failed\n", __func__);
-			kfree(transfer_buffer);
-			return -ENOMEM;
-		}
-	}
-
-	/* allocate memory for write_urb transfer buffer */
-	port->write_urb->transfer_buffer =
-			kmalloc(write_urb_transfer_buffer_length, GFP_KERNEL);
-	if (!port->write_urb->transfer_buffer) {
-		kfree(transfer_buffer);
-		usb_free_urb(port->write_urb);
-		port->write_urb = NULL;
-		return -ENOMEM;
-	}
-
 	/* get hardware version */
 	result = usb_control_msg(port->serial->dev,
 			  usb_rcvctrlpipe(port->serial->dev, 0),
@@ -310,12 +257,7 @@ static int kobil_open(struct tty_struct *tty, struct usb_serial_port *port)
 static void kobil_close(struct usb_serial_port *port)
 {
 	/* FIXME: Add rts/dtr methods */
-	if (port->write_urb) {
-		usb_poison_urb(port->write_urb);
-		kfree(port->write_urb->transfer_buffer);
-		usb_free_urb(port->write_urb);
-		port->write_urb = NULL;
-	}
+	usb_kill_urb(port->interrupt_out_urb);
 	usb_kill_urb(port->interrupt_in_urb);
 }
 
@@ -333,24 +275,8 @@ static void kobil_read_int_callback(struct urb *urb)
 	}
 
 	if (urb->actual_length) {
-
-		/* BEGIN DEBUG */
-		/*
-		  char *dbg_data;
-
-		  dbg_data = kzalloc((3 *  purb->actual_length + 10)
-						* sizeof(char), GFP_KERNEL);
-		  if (! dbg_data) {
-			  return;
-		  }
-		  for (i = 0; i < purb->actual_length; i++) {
-			  sprintf(dbg_data +3*i, "%02X ", data[i]);
-		  }
-		  dev_dbg(&port->dev, " <-- %s\n", dbg_data);
-		  kfree(dbg_data);
-		*/
-		/* END DEBUG */
-
+		usb_serial_debug_data(&port->dev, __func__, urb->actual_length,
+									data);
 		tty_insert_flip_string(&port->port, data, urb->actual_length);
 		tty_flip_buffer_push(&port->port);
 	}
@@ -360,7 +286,7 @@ static void kobil_read_int_callback(struct urb *urb)
 }
 
 
-static void kobil_write_callback(struct urb *purb)
+static void kobil_write_int_callback(struct urb *urb)
 {
 }
 
@@ -403,23 +329,14 @@ static int kobil_write(struct tty_struct *tty, struct usb_serial_port *port,
 
 		while (todo > 0) {
 			/* max 8 byte in one urb (endpoint size) */
-			length = (todo < 8) ? todo : 8;
+			length = min(todo, port->interrupt_out_size);
 			/* copy data to transfer buffer */
-			memcpy(port->write_urb->transfer_buffer,
+			memcpy(port->interrupt_out_buffer,
 					priv->buf + priv->cur_pos, length);
-			usb_fill_int_urb(port->write_urb,
-				  port->serial->dev,
-				  usb_sndintpipe(port->serial->dev,
-					priv->write_int_endpoint_address),
-				  port->write_urb->transfer_buffer,
-				  length,
-				  kobil_write_callback,
-				  port,
-				  8
-			);
+			port->interrupt_out_urb->transfer_buffer_length = length;
 
 			priv->cur_pos = priv->cur_pos + length;
-			result = usb_submit_urb(port->write_urb, GFP_NOIO);
+			result = usb_submit_urb(port->interrupt_out_urb, GFP_NOIO);
 			dev_dbg(&port->dev, "%s - Send write URB returns: %i\n", __func__, result);
 			todo = priv->filled - priv->cur_pos;
 
diff --git a/drivers/usb/serial/mct_u232.c b/drivers/usb/serial/mct_u232.c
index 06d5a60be2c..6a15adf5336 100644
--- a/drivers/usb/serial/mct_u232.c
+++ b/drivers/usb/serial/mct_u232.c
@@ -35,7 +35,6 @@
 #include <linux/usb.h>
 #include <linux/usb/serial.h>
 #include <linux/serial.h>
-#include <linux/ioctl.h>
 #include "mct_u232.h"
 
 #define DRIVER_AUTHOR "Wolfgang Grandegger <wolfgang@ces.ch>"
@@ -44,7 +43,6 @@
 /*
  * Function prototypes
  */
-static int  mct_u232_startup(struct usb_serial *serial);
 static int  mct_u232_port_probe(struct usb_serial_port *port);
 static int  mct_u232_port_remove(struct usb_serial_port *remove);
 static int  mct_u232_open(struct tty_struct *tty, struct usb_serial_port *port);
@@ -57,10 +55,6 @@ static void mct_u232_break_ctl(struct tty_struct *tty, int break_state);
 static int  mct_u232_tiocmget(struct tty_struct *tty);
 static int  mct_u232_tiocmset(struct tty_struct *tty,
 			unsigned int set, unsigned int clear);
-static int  mct_u232_ioctl(struct tty_struct *tty,
-			unsigned int cmd, unsigned long arg);
-static int  mct_u232_get_icount(struct tty_struct *tty,
-			struct serial_icounter_struct *icount);
 static void mct_u232_throttle(struct tty_struct *tty);
 static void mct_u232_unthrottle(struct tty_struct *tty);
 
@@ -95,11 +89,10 @@ static struct usb_serial_driver mct_u232_device = {
 	.break_ctl =	     mct_u232_break_ctl,
 	.tiocmget =	     mct_u232_tiocmget,
 	.tiocmset =	     mct_u232_tiocmset,
-	.attach =	     mct_u232_startup,
+	.tiocmiwait =        usb_serial_generic_tiocmiwait,
 	.port_probe =        mct_u232_port_probe,
 	.port_remove =       mct_u232_port_remove,
-	.ioctl =             mct_u232_ioctl,
-	.get_icount =        mct_u232_get_icount,
+	.get_icount =        usb_serial_generic_get_icount,
 };
 
 static struct usb_serial_driver * const serial_drivers[] = {
@@ -107,13 +100,13 @@ static struct usb_serial_driver * const serial_drivers[] = {
 };
 
 struct mct_u232_private {
+	struct urb *read_urb;
 	spinlock_t lock;
 	unsigned int	     control_state; /* Modem Line Setting (TIOCM) */
 	unsigned char        last_lcr;      /* Line Control Register */
 	unsigned char	     last_lsr;      /* Line Status Register */
 	unsigned char	     last_msr;      /* Modem Status Register */
 	unsigned int	     rx_flags;      /* Throttling flags */
-	struct async_icount  icount;
 };
 
 #define THROTTLED		0x01
@@ -382,22 +375,6 @@ static void mct_u232_msr_to_state(struct usb_serial_port *port,
  * Driver's tty interface functions
  */
 
-static int mct_u232_startup(struct usb_serial *serial)
-{
-	struct usb_serial_port *port, *rport;
-
-	/* Puh, that's dirty */
-	port = serial->port[0];
-	rport = serial->port[1];
-	/* No unlinking, it wasn't submitted yet. */
-	usb_free_urb(port->read_urb);
-	port->read_urb = rport->interrupt_in_urb;
-	rport->interrupt_in_urb = NULL;
-	port->read_urb->context = port;
-
-	return 0;
-} /* mct_u232_startup */
-
 static int mct_u232_port_probe(struct usb_serial_port *port)
 {
 	struct mct_u232_private *priv;
@@ -406,6 +383,10 @@ static int mct_u232_port_probe(struct usb_serial_port *port)
 	if (!priv)
 		return -ENOMEM;
 
+	/* Use second interrupt-in endpoint for reading. */
+	priv->read_urb = port->serial->port[1]->interrupt_in_urb;
+	priv->read_urb->context = port;
+
 	spin_lock_init(&priv->lock);
 
 	usb_set_serial_port_data(port, priv);
@@ -469,17 +450,17 @@ static int  mct_u232_open(struct tty_struct *tty, struct usb_serial_port *port)
 	mct_u232_msr_to_state(port, &priv->control_state, priv->last_msr);
 	spin_unlock_irqrestore(&priv->lock, flags);
 
-	retval = usb_submit_urb(port->read_urb, GFP_KERNEL);
+	retval = usb_submit_urb(priv->read_urb, GFP_KERNEL);
 	if (retval) {
 		dev_err(&port->dev,
-			"usb_submit_urb(read bulk) failed pipe 0x%x err %d\n",
+			"usb_submit_urb(read) failed pipe 0x%x err %d\n",
 			port->read_urb->pipe, retval);
 		goto error;
 	}
 
 	retval = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL);
 	if (retval) {
-		usb_kill_urb(port->read_urb);
+		usb_kill_urb(priv->read_urb);
 		dev_err(&port->dev,
 			"usb_submit_urb(read int) failed pipe 0x%x err %d",
 			port->interrupt_in_urb->pipe, retval);
@@ -509,11 +490,9 @@ static void mct_u232_dtr_rts(struct usb_serial_port *port, int on)
 
 static void mct_u232_close(struct usb_serial_port *port)
 {
-	/*
-	 * Must kill the read urb as it is actually an interrupt urb, which
-	 * generic close thus fails to kill.
-	 */
-	usb_kill_urb(port->read_urb);
+	struct mct_u232_private *priv = usb_get_serial_port_data(port);
+
+	usb_kill_urb(priv->read_urb);
 	usb_kill_urb(port->interrupt_in_urb);
 
 	usb_serial_generic_close(port);
@@ -570,7 +549,7 @@ static void mct_u232_read_int_callback(struct urb *urb)
 	/* Record Control Line states */
 	mct_u232_msr_to_state(port, &priv->control_state, priv->last_msr);
 
-	mct_u232_msr_to_icount(&priv->icount, priv->last_msr);
+	mct_u232_msr_to_icount(&port->icount, priv->last_msr);
 
 #if 0
 	/* Not yet handled. See belkin_sa.c for further information */
@@ -598,7 +577,7 @@ static void mct_u232_read_int_callback(struct urb *urb)
 		tty_kref_put(tty);
 	}
 #endif
-	wake_up_interruptible(&port->delta_msr_wait);
+	wake_up_interruptible(&port->port.delta_msr_wait);
 	spin_unlock_irqrestore(&priv->lock, flags);
 exit:
 	retval = usb_submit_urb(urb, GFP_ATOMIC);
@@ -786,86 +765,6 @@ static void mct_u232_unthrottle(struct tty_struct *tty)
 	}
 }
 
-static int  mct_u232_ioctl(struct tty_struct *tty,
-			unsigned int cmd, unsigned long arg)
-{
-	DEFINE_WAIT(wait);
-	struct usb_serial_port *port = tty->driver_data;
-	struct mct_u232_private *mct_u232_port = usb_get_serial_port_data(port);
-	struct async_icount cnow, cprev;
-	unsigned long flags;
-
-	dev_dbg(&port->dev, "%s - cmd = 0x%x\n", __func__, cmd);
-
-	switch (cmd) {
-
-	case TIOCMIWAIT:
-
-		dev_dbg(&port->dev, "%s TIOCMIWAIT", __func__);
-
-		spin_lock_irqsave(&mct_u232_port->lock, flags);
-		cprev = mct_u232_port->icount;
-		spin_unlock_irqrestore(&mct_u232_port->lock, flags);
-		for ( ; ; ) {
-			prepare_to_wait(&port->delta_msr_wait,
-					&wait, TASK_INTERRUPTIBLE);
-			schedule();
-			finish_wait(&port->delta_msr_wait, &wait);
-			/* see if a signal did it */
-			if (signal_pending(current))
-				return -ERESTARTSYS;
-
-			if (port->serial->disconnected)
-				return -EIO;
-
-			spin_lock_irqsave(&mct_u232_port->lock, flags);
-			cnow = mct_u232_port->icount;
-			spin_unlock_irqrestore(&mct_u232_port->lock, flags);
-			if (cnow.rng == cprev.rng && cnow.dsr == cprev.dsr &&
-			    cnow.dcd == cprev.dcd && cnow.cts == cprev.cts)
-				return -EIO; /* no change => error */
-			if (((arg & TIOCM_RNG) && (cnow.rng != cprev.rng)) ||
-			    ((arg & TIOCM_DSR) && (cnow.dsr != cprev.dsr)) ||
-			    ((arg & TIOCM_CD)  && (cnow.dcd != cprev.dcd)) ||
-			    ((arg & TIOCM_CTS) && (cnow.cts != cprev.cts))) {
-				return 0;
-			}
-			cprev = cnow;
-		}
-
-	}
-	return -ENOIOCTLCMD;
-}
-
-static int  mct_u232_get_icount(struct tty_struct *tty,
-			struct serial_icounter_struct *icount)
-{
-	struct usb_serial_port *port = tty->driver_data;
-	struct mct_u232_private *mct_u232_port = usb_get_serial_port_data(port);
-	struct async_icount *ic = &mct_u232_port->icount;
-	unsigned long flags;
-
-	spin_lock_irqsave(&mct_u232_port->lock, flags);
-
-	icount->cts = ic->cts;
-	icount->dsr = ic->dsr;
-	icount->rng = ic->rng;
-	icount->dcd = ic->dcd;
-	icount->rx = ic->rx;
-	icount->tx = ic->tx;
-	icount->frame = ic->frame;
-	icount->overrun = ic->overrun;
-	icount->parity = ic->parity;
-	icount->brk = ic->brk;
-	icount->buf_overrun = ic->buf_overrun;
-
-	spin_unlock_irqrestore(&mct_u232_port->lock, flags);
-
-	dev_dbg(&port->dev, "%s TIOCGICOUNT RX=%d, TX=%d\n",
-		__func__,  icount->rx, icount->tx);
-	return 0;
-}
-
 module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
diff --git a/drivers/usb/serial/metro-usb.c b/drivers/usb/serial/metro-usb.c
index bf3c7a23553..47e247759eb 100644
--- a/drivers/usb/serial/metro-usb.c
+++ b/drivers/usb/serial/metro-usb.c
@@ -177,10 +177,7 @@ static void metrousb_cleanup(struct usb_serial_port *port)
 	usb_unlink_urb(port->interrupt_in_urb);
 	usb_kill_urb(port->interrupt_in_urb);
 
-	mutex_lock(&port->serial->disc_mutex);
-	if (!port->serial->disconnected)
-		metrousb_send_unidirectional_cmd(UNI_CMD_CLOSE, port);
-	mutex_unlock(&port->serial->disc_mutex);
+	metrousb_send_unidirectional_cmd(UNI_CMD_CLOSE, port);
 }
 
 static int metrousb_open(struct tty_struct *tty, struct usb_serial_port *port)
diff --git a/drivers/usb/serial/mos7720.c b/drivers/usb/serial/mos7720.c
index e0ebec3b5d6..cc0e54345df 100644
--- a/drivers/usb/serial/mos7720.c
+++ b/drivers/usb/serial/mos7720.c
@@ -62,7 +62,6 @@ struct moschip_port {
 	__u8	shadowMCR;		/* last MCR value received */
 	__u8	shadowMSR;		/* last MSR value received */
 	char			open;
-	struct async_icount	icount;
 	struct usb_serial_port	*port;	/* loop back to the owner */
 	struct urb		*write_urb_pool[NUM_URBS];
 };
@@ -932,7 +931,6 @@ static void mos7720_bulk_in_callback(struct urb *urb)
 static void mos7720_bulk_out_data_callback(struct urb *urb)
 {
 	struct moschip_port *mos7720_port;
-	struct tty_struct *tty;
 	int status = urb->status;
 
 	if (status) {
@@ -946,11 +944,8 @@ static void mos7720_bulk_out_data_callback(struct urb *urb)
 		return ;
 	}
 
-	tty = tty_port_tty_get(&mos7720_port->port->port);
-
-	if (tty && mos7720_port->open)
-		tty_wakeup(tty);
-	tty_kref_put(tty);
+	if (mos7720_port->open)
+		tty_port_tty_wakeup(&mos7720_port->port->port);
 }
 
 /*
@@ -1075,9 +1070,6 @@ static int mos7720_open(struct tty_struct *tty, struct usb_serial_port *port)
 		dev_err(&port->dev, "%s - Error %d submitting read urb\n",
 							__func__, response);
 
-	/* initialize our icount structure */
-	memset(&(mos7720_port->icount), 0x00, sizeof(mos7720_port->icount));
-
 	/* initialize our port settings */
 	mos7720_port->shadowMCR = UART_MCR_OUT2; /* Must set to enable ints! */
 
@@ -1144,16 +1136,9 @@ static void mos7720_close(struct usb_serial_port *port)
 	usb_kill_urb(port->write_urb);
 	usb_kill_urb(port->read_urb);
 
-	mutex_lock(&serial->disc_mutex);
-	/* these commands must not be issued if the device has
-	 * been disconnected */
-	if (!serial->disconnected) {
-		write_mos_reg(serial, port->number - port->serial->minor,
-			      MCR, 0x00);
-		write_mos_reg(serial, port->number - port->serial->minor,
-			      IER, 0x00);
-	}
-	mutex_unlock(&serial->disc_mutex);
+	write_mos_reg(serial, port->number - port->serial->minor, MCR, 0x00);
+	write_mos_reg(serial, port->number - port->serial->minor, IER, 0x00);
+
 	mos7720_port->open = 0;
 }
 
@@ -1803,33 +1788,6 @@ static int mos7720_tiocmset(struct tty_struct *tty,
 	return 0;
 }
 
-static int mos7720_get_icount(struct tty_struct *tty,
-				struct serial_icounter_struct *icount)
-{
-	struct usb_serial_port *port = tty->driver_data;
-	struct moschip_port *mos7720_port;
-	struct async_icount cnow;
-
-	mos7720_port = usb_get_serial_port_data(port);
-	cnow = mos7720_port->icount;
-
-	icount->cts = cnow.cts;
-	icount->dsr = cnow.dsr;
-	icount->rng = cnow.rng;
-	icount->dcd = cnow.dcd;
-	icount->rx = cnow.rx;
-	icount->tx = cnow.tx;
-	icount->frame = cnow.frame;
-	icount->overrun = cnow.overrun;
-	icount->parity = cnow.parity;
-	icount->brk = cnow.brk;
-	icount->buf_overrun = cnow.buf_overrun;
-
-	dev_dbg(&port->dev, "%s TIOCGICOUNT RX=%d, TX=%d\n", __func__,
-		icount->rx, icount->tx);
-	return 0;
-}
-
 static int set_modem_info(struct moschip_port *mos7720_port, unsigned int cmd,
 			  unsigned int __user *value)
 {
@@ -1905,8 +1863,6 @@ static int mos7720_ioctl(struct tty_struct *tty,
 {
 	struct usb_serial_port *port = tty->driver_data;
 	struct moschip_port *mos7720_port;
-	struct async_icount cnow;
-	struct async_icount cprev;
 
 	mos7720_port = usb_get_serial_port_data(port);
 	if (mos7720_port == NULL)
@@ -1931,27 +1887,6 @@ static int mos7720_ioctl(struct tty_struct *tty,
 		dev_dbg(&port->dev, "%s TIOCGSERIAL\n", __func__);
 		return get_serial_info(mos7720_port,
 				       (struct serial_struct __user *)arg);
-
-	case TIOCMIWAIT:
-		dev_dbg(&port->dev, "%s TIOCMIWAIT\n", __func__);
-		cprev = mos7720_port->icount;
-		while (1) {
-			if (signal_pending(current))
-				return -ERESTARTSYS;
-			cnow = mos7720_port->icount;
-			if (cnow.rng == cprev.rng && cnow.dsr == cprev.dsr &&
-			    cnow.dcd == cprev.dcd && cnow.cts == cprev.cts)
-				return -EIO; /* no change => error */
-			if (((arg & TIOCM_RNG) && (cnow.rng != cprev.rng)) ||
-			    ((arg & TIOCM_DSR) && (cnow.dsr != cprev.dsr)) ||
-			    ((arg & TIOCM_CD)  && (cnow.dcd != cprev.dcd)) ||
-			    ((arg & TIOCM_CTS) && (cnow.cts != cprev.cts))) {
-				return 0;
-			}
-			cprev = cnow;
-		}
-		/* NOTREACHED */
-		break;
 	}
 
 	return -ENOIOCTLCMD;
@@ -2107,7 +2042,6 @@ static struct usb_serial_driver moschip7720_2port_driver = {
 	.ioctl			= mos7720_ioctl,
 	.tiocmget		= mos7720_tiocmget,
 	.tiocmset		= mos7720_tiocmset,
-	.get_icount		= mos7720_get_icount,
 	.set_termios		= mos7720_set_termios,
 	.write			= mos7720_write,
 	.write_room		= mos7720_write_room,
diff --git a/drivers/usb/serial/mos7840.c b/drivers/usb/serial/mos7840.c
index b8051fa6191..a0d5ea54598 100644
--- a/drivers/usb/serial/mos7840.c
+++ b/drivers/usb/serial/mos7840.c
@@ -219,8 +219,6 @@ struct moschip_port {
 	char open;
 	char open_ports;
 	wait_queue_head_t wait_chase;	/* for handling sleeping while waiting for chase to finish */
-	int delta_msr_cond;
-	struct async_icount icount;
 	struct usb_serial_port *port;	/* loop back to the owner of this object */
 
 	/* Offsets */
@@ -399,32 +397,22 @@ static void mos7840_handle_new_msr(struct moschip_port *port, __u8 new_msr)
 	struct moschip_port *mos7840_port;
 	struct async_icount *icount;
 	mos7840_port = port;
-	icount = &mos7840_port->icount;
 	if (new_msr &
 	    (MOS_MSR_DELTA_CTS | MOS_MSR_DELTA_DSR | MOS_MSR_DELTA_RI |
 	     MOS_MSR_DELTA_CD)) {
-		icount = &mos7840_port->icount;
+		icount = &mos7840_port->port->icount;
 
 		/* update input line counters */
-		if (new_msr & MOS_MSR_DELTA_CTS) {
+		if (new_msr & MOS_MSR_DELTA_CTS)
 			icount->cts++;
-			smp_wmb();
-		}
-		if (new_msr & MOS_MSR_DELTA_DSR) {
+		if (new_msr & MOS_MSR_DELTA_DSR)
 			icount->dsr++;
-			smp_wmb();
-		}
-		if (new_msr & MOS_MSR_DELTA_CD) {
+		if (new_msr & MOS_MSR_DELTA_CD)
 			icount->dcd++;
-			smp_wmb();
-		}
-		if (new_msr & MOS_MSR_DELTA_RI) {
+		if (new_msr & MOS_MSR_DELTA_RI)
 			icount->rng++;
-			smp_wmb();
-		}
 
-		mos7840_port->delta_msr_cond = 1;
-		wake_up_interruptible(&port->port->delta_msr_wait);
+		wake_up_interruptible(&port->port->port.delta_msr_wait);
 	}
 }
 
@@ -442,23 +430,15 @@ static void mos7840_handle_new_lsr(struct moschip_port *port, __u8 new_lsr)
 	}
 
 	/* update input line counters */
-	icount = &port->icount;
-	if (new_lsr & SERIAL_LSR_BI) {
+	icount = &port->port->icount;
+	if (new_lsr & SERIAL_LSR_BI)
 		icount->brk++;
-		smp_wmb();
-	}
-	if (new_lsr & SERIAL_LSR_OE) {
+	if (new_lsr & SERIAL_LSR_OE)
 		icount->overrun++;
-		smp_wmb();
-	}
-	if (new_lsr & SERIAL_LSR_PE) {
+	if (new_lsr & SERIAL_LSR_PE)
 		icount->parity++;
-		smp_wmb();
-	}
-	if (new_lsr & SERIAL_LSR_FE) {
+	if (new_lsr & SERIAL_LSR_FE)
 		icount->frame++;
-		smp_wmb();
-	}
 }
 
 /************************************************************************/
@@ -777,9 +757,8 @@ static void mos7840_bulk_in_callback(struct urb *urb)
 		struct tty_port *tport = &mos7840_port->port->port;
 		tty_insert_flip_string(tport, data, urb->actual_length);
 		tty_flip_buffer_push(tport);
-		mos7840_port->icount.rx += urb->actual_length;
-		smp_wmb();
-		dev_dbg(&port->dev, "mos7840_port->icount.rx is %d:\n", mos7840_port->icount.rx);
+		port->icount.rx += urb->actual_length;
+		dev_dbg(&port->dev, "icount.rx is %d:\n", port->icount.rx);
 	}
 
 	if (!mos7840_port->read_urb) {
@@ -816,7 +795,6 @@ static void mos7840_bulk_out_data_callback(struct urb *urb)
 {
 	struct moschip_port *mos7840_port;
 	struct usb_serial_port *port;
-	struct tty_struct *tty;
 	int status = urb->status;
 	int i;
 
@@ -839,10 +817,8 @@ static void mos7840_bulk_out_data_callback(struct urb *urb)
 	if (mos7840_port_paranoia_check(port, __func__))
 		return;
 
-	tty = tty_port_tty_get(&port->port);
-	if (tty && mos7840_port->open)
-		tty_wakeup(tty);
-	tty_kref_put(tty);
+	if (mos7840_port->open)
+		tty_port_tty_wakeup(&port->port);
 
 }
 
@@ -1130,17 +1106,12 @@ static int mos7840_open(struct tty_struct *tty, struct usb_serial_port *port)
 	/* initialize our wait queues */
 	init_waitqueue_head(&mos7840_port->wait_chase);
 
-	/* initialize our icount structure */
-	memset(&(mos7840_port->icount), 0x00, sizeof(mos7840_port->icount));
-
 	/* initialize our port settings */
 	/* Must set to enable ints! */
 	mos7840_port->shadowMCR = MCR_MASTER_IE;
 	/* send a open port command */
 	mos7840_port->open = 1;
 	/* mos7840_change_port_settings(mos7840_port,old_termios); */
-	mos7840_port->icount.tx = 0;
-	mos7840_port->icount.rx = 0;
 
 	return 0;
 }
@@ -1223,25 +1194,10 @@ static void mos7840_close(struct usb_serial_port *port)
 		}
 	}
 
-	/* While closing port, shutdown all bulk read, write  *
-	 * and interrupt read if they exists                  */
-	if (serial->dev) {
-		if (mos7840_port->write_urb) {
-			dev_dbg(&port->dev, "%s", "Shutdown bulk write\n");
-			usb_kill_urb(mos7840_port->write_urb);
-		}
-		if (mos7840_port->read_urb) {
-			dev_dbg(&port->dev, "%s", "Shutdown bulk read\n");
-			usb_kill_urb(mos7840_port->read_urb);
-			mos7840_port->read_urb_busy = false;
-		}
-		if ((&mos7840_port->control_urb)) {
-			dev_dbg(&port->dev, "%s", "Shutdown control read\n");
-			/*/      usb_kill_urb (mos7840_port->control_urb); */
-		}
-	}
-/*      if(mos7840_port->ctrl_buf != NULL) */
-/*              kfree(mos7840_port->ctrl_buf); */
+	usb_kill_urb(mos7840_port->write_urb);
+	usb_kill_urb(mos7840_port->read_urb);
+	mos7840_port->read_urb_busy = false;
+
 	port0->open_ports--;
 	dev_dbg(&port->dev, "%s in close%d:in port%d\n", __func__, port0->open_ports, port->number);
 	if (port0->open_ports == 0) {
@@ -1253,8 +1209,7 @@ static void mos7840_close(struct usb_serial_port *port)
 
 	if (mos7840_port->write_urb) {
 		/* if this urb had a transfer buffer already (old tx) free it */
-		if (mos7840_port->write_urb->transfer_buffer != NULL)
-			kfree(mos7840_port->write_urb->transfer_buffer);
+		kfree(mos7840_port->write_urb->transfer_buffer);
 		usb_free_urb(mos7840_port->write_urb);
 	}
 
@@ -1331,9 +1286,8 @@ static void mos7840_break(struct tty_struct *tty, int break_state)
 	if (mos7840_port == NULL)
 		return;
 
-	if (serial->dev)
-		/* flush and block until tx is empty */
-		mos7840_block_until_chase_response(tty, mos7840_port);
+	/* flush and block until tx is empty */
+	mos7840_block_until_chase_response(tty, mos7840_port);
 
 	if (break_state == -1)
 		data = mos7840_port->shadowLCR | LCR_SET_BREAK;
@@ -1523,9 +1477,8 @@ static int mos7840_write(struct tty_struct *tty, struct usb_serial_port *port,
 		goto exit;
 	}
 	bytes_sent = transfer_size;
-	mos7840_port->icount.tx += transfer_size;
-	smp_wmb();
-	dev_dbg(&port->dev, "mos7840_port->icount.tx is %d:\n", mos7840_port->icount.tx);
+	port->icount.tx += transfer_size;
+	dev_dbg(&port->dev, "icount.tx is %d:\n", port->icount.tx);
 exit:
 	return bytes_sent;
 
@@ -2144,34 +2097,6 @@ static int mos7840_get_serial_info(struct moschip_port *mos7840_port,
 	return 0;
 }
 
-static int mos7840_get_icount(struct tty_struct *tty,
-			struct serial_icounter_struct *icount)
-{
-	struct usb_serial_port *port = tty->driver_data;
-	struct moschip_port *mos7840_port;
-	struct async_icount cnow;
-
-	mos7840_port = mos7840_get_port_private(port);
-	cnow = mos7840_port->icount;
-
-	smp_rmb();
-	icount->cts = cnow.cts;
-	icount->dsr = cnow.dsr;
-	icount->rng = cnow.rng;
-	icount->dcd = cnow.dcd;
-	icount->rx = cnow.rx;
-	icount->tx = cnow.tx;
-	icount->frame = cnow.frame;
-	icount->overrun = cnow.overrun;
-	icount->parity = cnow.parity;
-	icount->brk = cnow.brk;
-	icount->buf_overrun = cnow.buf_overrun;
-
-	dev_dbg(&port->dev, "%s TIOCGICOUNT RX=%d, TX=%d\n", __func__,
-		icount->rx, icount->tx);
-	return 0;
-}
-
 /*****************************************************************************
  * SerialIoctl
  *	this function handles any ioctl calls to the driver
@@ -2184,9 +2109,6 @@ static int mos7840_ioctl(struct tty_struct *tty,
 	void __user *argp = (void __user *)arg;
 	struct moschip_port *mos7840_port;
 
-	struct async_icount cnow;
-	struct async_icount cprev;
-
 	if (mos7840_port_paranoia_check(port, __func__))
 		return -1;
 
@@ -2211,41 +2133,6 @@ static int mos7840_ioctl(struct tty_struct *tty,
 	case TIOCSSERIAL:
 		dev_dbg(&port->dev, "%s TIOCSSERIAL\n", __func__);
 		break;
-
-	case TIOCMIWAIT:
-		dev_dbg(&port->dev, "%s  TIOCMIWAIT\n", __func__);
-		cprev = mos7840_port->icount;
-		while (1) {
-			/* interruptible_sleep_on(&mos7840_port->delta_msr_wait); */
-			mos7840_port->delta_msr_cond = 0;
-			wait_event_interruptible(port->delta_msr_wait,
-						 (port->serial->disconnected ||
-						  mos7840_port->
-						  delta_msr_cond == 1));
-
-			/* see if a signal did it */
-			if (signal_pending(current))
-				return -ERESTARTSYS;
-
-			if (port->serial->disconnected)
-				return -EIO;
-
-			cnow = mos7840_port->icount;
-			smp_rmb();
-			if (cnow.rng == cprev.rng && cnow.dsr == cprev.dsr &&
-			    cnow.dcd == cprev.dcd && cnow.cts == cprev.cts)
-				return -EIO;	/* no change => error */
-			if (((arg & TIOCM_RNG) && (cnow.rng != cprev.rng)) ||
-			    ((arg & TIOCM_DSR) && (cnow.dsr != cprev.dsr)) ||
-			    ((arg & TIOCM_CD) && (cnow.dcd != cprev.dcd)) ||
-			    ((arg & TIOCM_CTS) && (cnow.cts != cprev.cts))) {
-				return 0;
-			}
-			cprev = cnow;
-		}
-		/* NOTREACHED */
-		break;
-
 	default:
 		break;
 	}
@@ -2592,7 +2479,8 @@ static struct usb_serial_driver moschip7840_4port_device = {
 	.break_ctl = mos7840_break,
 	.tiocmget = mos7840_tiocmget,
 	.tiocmset = mos7840_tiocmset,
-	.get_icount = mos7840_get_icount,
+	.tiocmiwait = usb_serial_generic_tiocmiwait,
+	.get_icount = usb_serial_generic_get_icount,
 	.port_probe = mos7840_port_probe,
 	.port_remove = mos7840_port_remove,
 	.read_bulk_callback = mos7840_bulk_in_callback,
diff --git a/drivers/usb/serial/omninet.c b/drivers/usb/serial/omninet.c
index 1e1cafe287e..5739bf6f720 100644
--- a/drivers/usb/serial/omninet.c
+++ b/drivers/usb/serial/omninet.c
@@ -33,8 +33,7 @@
 
 /* function prototypes */
 static int  omninet_open(struct tty_struct *tty, struct usb_serial_port *port);
-static void omninet_close(struct usb_serial_port *port);
-static void omninet_read_bulk_callback(struct urb *urb);
+static void omninet_process_read_urb(struct urb *urb);
 static void omninet_write_bulk_callback(struct urb *urb);
 static int  omninet_write(struct tty_struct *tty, struct usb_serial_port *port,
 				const unsigned char *buf, int count);
@@ -61,11 +60,10 @@ static struct usb_serial_driver zyxel_omninet_device = {
 	.port_probe =		omninet_port_probe,
 	.port_remove =		omninet_port_remove,
 	.open =			omninet_open,
-	.close =		omninet_close,
 	.write =		omninet_write,
 	.write_room =		omninet_write_room,
-	.read_bulk_callback =	omninet_read_bulk_callback,
 	.write_bulk_callback =	omninet_write_bulk_callback,
+	.process_read_urb =	omninet_process_read_urb,
 	.disconnect =		omninet_disconnect,
 };
 
@@ -74,29 +72,28 @@ static struct usb_serial_driver * const serial_drivers[] = {
 };
 
 
-/* The protocol.
+/*
+ * The protocol.
  *
  * The omni.net always exchange 64 bytes of data with the host. The first
- * four bytes are the control header, you can see it in the above structure.
+ * four bytes are the control header.
  *
  * oh_seq is a sequence number. Don't know if/how it's used.
  * oh_len is the length of the data bytes in the packet.
  * oh_xxx Bit-mapped, related to handshaking and status info.
- *	I normally set it to 0x03 in trasmitted frames.
+ *	I normally set it to 0x03 in transmitted frames.
  *	7: Active when the TA is in a CONNECTed state.
  *	6: unknown
  *	5: handshaking, unknown
  *	4: handshaking, unknown
  *	3: unknown, usually 0
  *	2: unknown, usually 0
- *	1: handshaking, unknown, usually set to 1 in trasmitted frames
- *	0: handshaking, unknown, usually set to 1 in trasmitted frames
+ *	1: handshaking, unknown, usually set to 1 in transmitted frames
+ *	0: handshaking, unknown, usually set to 1 in transmitted frames
  * oh_pad Probably a pad byte.
  *
  * After the header you will find data bytes if oh_len was greater than zero.
- *
  */
-
 struct omninet_header {
 	__u8	oh_seq;
 	__u8	oh_len;
@@ -112,7 +109,7 @@ static int omninet_port_probe(struct usb_serial_port *port)
 {
 	struct omninet_data *od;
 
-	od = kmalloc(sizeof(struct omninet_data), GFP_KERNEL);
+	od = kzalloc(sizeof(*od), GFP_KERNEL);
 	if (!od)
 		return -ENOMEM;
 
@@ -135,56 +132,32 @@ static int omninet_open(struct tty_struct *tty, struct usb_serial_port *port)
 {
 	struct usb_serial	*serial = port->serial;
 	struct usb_serial_port	*wport;
-	int			result = 0;
 
 	wport = serial->port[1];
 	tty_port_tty_set(&wport->port, tty);
 
-	/* Start reading from the device */
-	result = usb_submit_urb(port->read_urb, GFP_KERNEL);
-	if (result)
-		dev_err(&port->dev,
-			"%s - failed submitting read urb, error %d\n",
-			__func__, result);
-	return result;
-}
-
-static void omninet_close(struct usb_serial_port *port)
-{
-	usb_kill_urb(port->read_urb);
+	return usb_serial_generic_open(tty, port);
 }
 
+#define OMNINET_HEADERLEN	4
+#define OMNINET_BULKOUTSIZE	64
+#define OMNINET_PAYLOADSIZE	(OMNINET_BULKOUTSIZE - OMNINET_HEADERLEN)
 
-#define OMNINET_DATAOFFSET	0x04
-#define OMNINET_HEADERLEN	sizeof(struct omninet_header)
-#define OMNINET_BULKOUTSIZE 	(64 - OMNINET_HEADERLEN)
-
-static void omninet_read_bulk_callback(struct urb *urb)
+static void omninet_process_read_urb(struct urb *urb)
 {
-	struct usb_serial_port 	*port 	= urb->context;
-	unsigned char 		*data 	= urb->transfer_buffer;
-	struct omninet_header 	*header = (struct omninet_header *) &data[0];
-	int status = urb->status;
-	int result;
+	struct usb_serial_port *port = urb->context;
+	const struct omninet_header *hdr = urb->transfer_buffer;
+	const unsigned char *data;
+	size_t data_len;
 
-	if (status) {
-		dev_dbg(&port->dev, "%s - nonzero read bulk status received: %d\n",
-			__func__, status);
+	if (urb->actual_length <= OMNINET_HEADERLEN || !hdr->oh_len)
 		return;
-	}
-
-	if (urb->actual_length && header->oh_len) {
-		tty_insert_flip_string(&port->port, data + OMNINET_DATAOFFSET,
-				header->oh_len);
-		tty_flip_buffer_push(&port->port);
-	}
 
-	/* Continue trying to always read  */
-	result = usb_submit_urb(urb, GFP_ATOMIC);
-	if (result)
-		dev_err(&port->dev,
-			"%s - failed resubmitting read urb, error %d\n",
-			__func__, result);
+	data = (char *)urb->transfer_buffer + OMNINET_HEADERLEN;
+	data_len = min_t(size_t, urb->actual_length - OMNINET_HEADERLEN,
+								hdr->oh_len);
+	tty_insert_flip_string(&port->port, data, data_len);
+	tty_flip_buffer_push(&port->port);
 }
 
 static int omninet_write(struct tty_struct *tty, struct usb_serial_port *port,
@@ -209,9 +182,9 @@ static int omninet_write(struct tty_struct *tty, struct usb_serial_port *port,
 		return 0;
 	}
 
-	count = (count > OMNINET_BULKOUTSIZE) ? OMNINET_BULKOUTSIZE : count;
+	count = (count > OMNINET_PAYLOADSIZE) ? OMNINET_PAYLOADSIZE : count;
 
-	memcpy(wport->write_urb->transfer_buffer + OMNINET_DATAOFFSET,
+	memcpy(wport->write_urb->transfer_buffer + OMNINET_HEADERLEN,
 								buf, count);
 
 	usb_serial_debug_data(&port->dev, __func__, count,
@@ -223,7 +196,7 @@ static int omninet_write(struct tty_struct *tty, struct usb_serial_port *port,
 	header->oh_pad 	= 0x00;
 
 	/* send the data out the bulk port, always 64 bytes */
-	wport->write_urb->transfer_buffer_length = 64;
+	wport->write_urb->transfer_buffer_length = OMNINET_BULKOUTSIZE;
 
 	result = usb_submit_urb(wport->write_urb, GFP_ATOMIC);
 	if (result) {
diff --git a/drivers/usb/serial/opticon.c b/drivers/usb/serial/opticon.c
index e13e1a4d3e1..5f4b0cd0f6e 100644
--- a/drivers/usb/serial/opticon.c
+++ b/drivers/usb/serial/opticon.c
@@ -120,7 +120,10 @@ static int send_control_msg(struct usb_serial_port *port, u8 requesttype,
 				0, 0, buffer, 1, 0);
 	kfree(buffer);
 
-	return retval;
+	if (retval < 0)
+		return retval;
+
+	return 0;
 }
 
 static int opticon_open(struct tty_struct *tty, struct usb_serial_port *port)
@@ -306,7 +309,6 @@ static int opticon_tiocmset(struct tty_struct *tty,
 			   unsigned int set, unsigned int clear)
 {
 	struct usb_serial_port *port = tty->driver_data;
-	struct usb_serial *serial = port->serial;
 	struct opticon_private *priv = usb_get_serial_port_data(port);
 	unsigned long flags;
 	bool rts;
@@ -327,15 +329,11 @@ static int opticon_tiocmset(struct tty_struct *tty,
 	if (!changed)
 		return 0;
 
-	/* Send the new RTS state to the connected device */
-	mutex_lock(&serial->disc_mutex);
-	if (!serial->disconnected)
-		ret = send_control_msg(port, CONTROL_RTS, !rts);
-	else
-		ret = -ENODEV;
-	mutex_unlock(&serial->disc_mutex);
+	ret = send_control_msg(port, CONTROL_RTS, !rts);
+	if (ret)
+		return usb_translate_errors(ret);
 
-	return ret;
+	return 0;
 }
 
 static int get_serial_info(struct usb_serial_port *port,
diff --git a/drivers/usb/serial/option.c b/drivers/usb/serial/option.c
index 558adfc0500..734372846ab 100644
--- a/drivers/usb/serial/option.c
+++ b/drivers/usb/serial/option.c
@@ -347,6 +347,7 @@ static void option_instat_callback(struct urb *urb);
 /* Olivetti products */
 #define OLIVETTI_VENDOR_ID			0x0b3c
 #define OLIVETTI_PRODUCT_OLICARD100		0xc000
+#define OLIVETTI_PRODUCT_OLICARD145		0xc003
 
 /* Celot products */
 #define CELOT_VENDOR_ID				0x211f
@@ -1273,6 +1274,7 @@ static const struct usb_device_id option_ids[] = {
 	{ USB_DEVICE(SIEMENS_VENDOR_ID, CINTERION_PRODUCT_HC28_MDMNET) },
 
 	{ USB_DEVICE(OLIVETTI_VENDOR_ID, OLIVETTI_PRODUCT_OLICARD100) },
+	{ USB_DEVICE(OLIVETTI_VENDOR_ID, OLIVETTI_PRODUCT_OLICARD145) },
 	{ USB_DEVICE(CELOT_VENDOR_ID, CELOT_PRODUCT_CT680M) }, /* CT-650 CDMA 450 1xEVDO modem */
 	{ USB_DEVICE(ONDA_VENDOR_ID, ONDA_MT825UP) }, /* ONDA MT825UP modem */
 	{ USB_DEVICE_AND_INTERFACE_INFO(SAMSUNG_VENDOR_ID, SAMSUNG_PRODUCT_GT_B3730, USB_CLASS_CDC_DATA, 0x00, 0x00) }, /* Samsung GT-B3730 LTE USB modem.*/
@@ -1350,6 +1352,12 @@ static const struct usb_device_id option_ids[] = {
 	{ USB_DEVICE(TPLINK_VENDOR_ID, TPLINK_PRODUCT_MA180),
 	  .driver_info = (kernel_ulong_t)&net_intf4_blacklist },
 	{ USB_DEVICE(CHANGHONG_VENDOR_ID, CHANGHONG_PRODUCT_CH690) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(0x2001, 0x7d01, 0xff, 0x02, 0x01) },	/* D-Link DWM-156 (variant) */
+	{ USB_DEVICE_AND_INTERFACE_INFO(0x2001, 0x7d01, 0xff, 0x00, 0x00) },	/* D-Link DWM-156 (variant) */
+	{ USB_DEVICE_AND_INTERFACE_INFO(0x2001, 0x7d02, 0xff, 0x02, 0x01) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(0x2001, 0x7d02, 0xff, 0x00, 0x00) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(0x2001, 0x7d03, 0xff, 0x02, 0x01) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(0x2001, 0x7d03, 0xff, 0x00, 0x00) },
 	{ } /* Terminating entry */
 };
 MODULE_DEVICE_TABLE(usb, option_ids);
@@ -1537,13 +1545,8 @@ static void option_instat_callback(struct urb *urb)
 			portdata->dsr_state = ((signals & 0x02) ? 1 : 0);
 			portdata->ri_state = ((signals & 0x08) ? 1 : 0);
 
-			if (old_dcd_state && !portdata->dcd_state) {
-				struct tty_struct *tty =
-						tty_port_tty_get(&port->port);
-				if (tty && !C_CLOCAL(tty))
-					tty_hangup(tty);
-				tty_kref_put(tty);
-			}
+			if (old_dcd_state && !portdata->dcd_state)
+				tty_port_tty_hangup(&port->port, true);
 		} else {
 			dev_dbg(dev, "%s: type %x req %x\n", __func__,
 				req_pkt->bRequestType, req_pkt->bRequest);
diff --git a/drivers/usb/serial/oti6858.c b/drivers/usb/serial/oti6858.c
index 87c71ccfee8..7e3e0782e51 100644
--- a/drivers/usb/serial/oti6858.c
+++ b/drivers/usb/serial/oti6858.c
@@ -124,8 +124,6 @@ static void oti6858_close(struct usb_serial_port *port);
 static void oti6858_set_termios(struct tty_struct *tty,
 			struct usb_serial_port *port, struct ktermios *old);
 static void oti6858_init_termios(struct tty_struct *tty);
-static int oti6858_ioctl(struct tty_struct *tty,
-			unsigned int cmd, unsigned long arg);
 static void oti6858_read_int_callback(struct urb *urb);
 static void oti6858_read_bulk_callback(struct urb *urb);
 static void oti6858_write_bulk_callback(struct urb *urb);
@@ -136,6 +134,7 @@ static int oti6858_chars_in_buffer(struct tty_struct *tty);
 static int oti6858_tiocmget(struct tty_struct *tty);
 static int oti6858_tiocmset(struct tty_struct *tty,
 				unsigned int set, unsigned int clear);
+static int oti6858_tiocmiwait(struct tty_struct *tty, unsigned long arg);
 static int oti6858_port_probe(struct usb_serial_port *port);
 static int oti6858_port_remove(struct usb_serial_port *port);
 
@@ -150,11 +149,11 @@ static struct usb_serial_driver oti6858_device = {
 	.open =			oti6858_open,
 	.close =		oti6858_close,
 	.write =		oti6858_write,
-	.ioctl =		oti6858_ioctl,
 	.set_termios =		oti6858_set_termios,
 	.init_termios = 	oti6858_init_termios,
 	.tiocmget =		oti6858_tiocmget,
 	.tiocmset =		oti6858_tiocmset,
+	.tiocmiwait =		oti6858_tiocmiwait,
 	.read_bulk_callback =	oti6858_read_bulk_callback,
 	.read_int_callback =	oti6858_read_int_callback,
 	.write_bulk_callback =	oti6858_write_bulk_callback,
@@ -650,8 +649,9 @@ static int oti6858_tiocmget(struct tty_struct *tty)
 	return result;
 }
 
-static int wait_modem_info(struct usb_serial_port *port, unsigned int arg)
+static int oti6858_tiocmiwait(struct tty_struct *tty, unsigned long arg)
 {
+	struct usb_serial_port *port = tty->driver_data;
 	struct oti6858_private *priv = usb_get_serial_port_data(port);
 	unsigned long flags;
 	unsigned int prev, status;
@@ -662,7 +662,7 @@ static int wait_modem_info(struct usb_serial_port *port, unsigned int arg)
 	spin_unlock_irqrestore(&priv->lock, flags);
 
 	while (1) {
-		wait_event_interruptible(port->delta_msr_wait,
+		wait_event_interruptible(port->port.delta_msr_wait,
 					port->serial->disconnected ||
 					priv->status.pin_state != prev);
 		if (signal_pending(current))
@@ -689,24 +689,6 @@ static int wait_modem_info(struct usb_serial_port *port, unsigned int arg)
 	return 0;
 }
 
-static int oti6858_ioctl(struct tty_struct *tty,
-			unsigned int cmd, unsigned long arg)
-{
-	struct usb_serial_port *port = tty->driver_data;
-
-	dev_dbg(&port->dev, "%s(cmd = 0x%04x, arg = 0x%08lx)\n", __func__, cmd, arg);
-
-	switch (cmd) {
-	case TIOCMIWAIT:
-		dev_dbg(&port->dev, "%s(): TIOCMIWAIT\n", __func__);
-		return wait_modem_info(port, arg);
-	default:
-		dev_dbg(&port->dev, "%s(): 0x%04x not supported\n", __func__, cmd);
-		break;
-	}
-	return -ENOIOCTLCMD;
-}
-
 static void oti6858_read_int_callback(struct urb *urb)
 {
 	struct usb_serial_port *port =  urb->context;
@@ -765,7 +747,7 @@ static void oti6858_read_int_callback(struct urb *urb)
 
 		if (!priv->transient) {
 			if (xs->pin_state != priv->status.pin_state)
-				wake_up_interruptible(&port->delta_msr_wait);
+				wake_up_interruptible(&port->port.delta_msr_wait);
 			memcpy(&priv->status, xs, OTI6858_CTRL_PKT_SIZE);
 		}
 
diff --git a/drivers/usb/serial/pl2303.c b/drivers/usb/serial/pl2303.c
index 3b10018d89a..7151659367a 100644
--- a/drivers/usb/serial/pl2303.c
+++ b/drivers/usb/serial/pl2303.c
@@ -149,7 +149,7 @@ static int pl2303_vendor_read(__u16 value, __u16 index,
 	int res = usb_control_msg(serial->dev, usb_rcvctrlpipe(serial->dev, 0),
 			VENDOR_READ_REQUEST, VENDOR_READ_REQUEST_TYPE,
 			value, index, buf, 1, 100);
-	dev_dbg(&serial->dev->dev, "0x%x:0x%x:0x%x:0x%x  %d - %x\n",
+	dev_dbg(&serial->interface->dev, "0x%x:0x%x:0x%x:0x%x  %d - %x\n",
 		VENDOR_READ_REQUEST_TYPE, VENDOR_READ_REQUEST, value, index,
 		res, buf[0]);
 	return res;
@@ -161,7 +161,7 @@ static int pl2303_vendor_write(__u16 value, __u16 index,
 	int res = usb_control_msg(serial->dev, usb_sndctrlpipe(serial->dev, 0),
 			VENDOR_WRITE_REQUEST, VENDOR_WRITE_REQUEST_TYPE,
 			value, index, NULL, 0, 100);
-	dev_dbg(&serial->dev->dev, "0x%x:0x%x:0x%x:0x%x  %d\n",
+	dev_dbg(&serial->interface->dev, "0x%x:0x%x:0x%x:0x%x  %d\n",
 		VENDOR_WRITE_REQUEST_TYPE, VENDOR_WRITE_REQUEST, value, index,
 		res);
 	return res;
@@ -248,14 +248,15 @@ static int pl2303_port_remove(struct usb_serial_port *port)
 	return 0;
 }
 
-static int set_control_lines(struct usb_device *dev, u8 value)
+static int pl2303_set_control_lines(struct usb_serial_port *port, u8 value)
 {
+	struct usb_device *dev = port->serial->dev;
 	int retval;
 
 	retval = usb_control_msg(dev, usb_sndctrlpipe(dev, 0),
 				 SET_CONTROL_REQUEST, SET_CONTROL_REQUEST_TYPE,
 				 value, 0, NULL, 0, 100);
-	dev_dbg(&dev->dev, "%s - value = %d, retval = %d\n", __func__,
+	dev_dbg(&port->dev, "%s - value = %d, retval = %d\n", __func__,
 		value, retval);
 	return retval;
 }
@@ -437,7 +438,7 @@ static void pl2303_set_termios(struct tty_struct *tty,
 	if (control != priv->line_control) {
 		control = priv->line_control;
 		spin_unlock_irqrestore(&priv->lock, flags);
-		set_control_lines(serial->dev, control);
+		pl2303_set_control_lines(port, control);
 	} else {
 		spin_unlock_irqrestore(&priv->lock, flags);
 	}
@@ -480,7 +481,7 @@ static void pl2303_dtr_rts(struct usb_serial_port *port, int on)
 		priv->line_control &= ~(CONTROL_DTR | CONTROL_RTS);
 	control = priv->line_control;
 	spin_unlock_irqrestore(&priv->lock, flags);
-	set_control_lines(port->serial->dev, control);
+	pl2303_set_control_lines(port, control);
 }
 
 static void pl2303_close(struct usb_serial_port *port)
@@ -530,7 +531,6 @@ static int pl2303_tiocmset(struct tty_struct *tty,
 			   unsigned int set, unsigned int clear)
 {
 	struct usb_serial_port *port = tty->driver_data;
-	struct usb_serial *serial = port->serial;
 	struct pl2303_private *priv = usb_get_serial_port_data(port);
 	unsigned long flags;
 	u8 control;
@@ -548,14 +548,11 @@ static int pl2303_tiocmset(struct tty_struct *tty,
 	control = priv->line_control;
 	spin_unlock_irqrestore(&priv->lock, flags);
 
-	mutex_lock(&serial->disc_mutex);
-	if (!serial->disconnected)
-		ret = set_control_lines(serial->dev, control);
-	else
-		ret = -ENODEV;
-	mutex_unlock(&serial->disc_mutex);
+	ret = pl2303_set_control_lines(port, control);
+	if (ret)
+		return usb_translate_errors(ret);
 
-	return ret;
+	return 0;
 }
 
 static int pl2303_tiocmget(struct tty_struct *tty)
@@ -592,8 +589,9 @@ static int pl2303_carrier_raised(struct usb_serial_port *port)
 	return 0;
 }
 
-static int wait_modem_info(struct usb_serial_port *port, unsigned int arg)
+static int pl2303_tiocmiwait(struct tty_struct *tty, unsigned long arg)
 {
+	struct usb_serial_port *port = tty->driver_data;
 	struct pl2303_private *priv = usb_get_serial_port_data(port);
 	unsigned long flags;
 	unsigned int prevstatus;
@@ -605,7 +603,7 @@ static int wait_modem_info(struct usb_serial_port *port, unsigned int arg)
 	spin_unlock_irqrestore(&priv->lock, flags);
 
 	while (1) {
-		interruptible_sleep_on(&port->delta_msr_wait);
+		interruptible_sleep_on(&port->port.delta_msr_wait);
 		/* see if a signal did it */
 		if (signal_pending(current))
 			return -ERESTARTSYS;
@@ -651,10 +649,6 @@ static int pl2303_ioctl(struct tty_struct *tty,
 			return -EFAULT;
 
 		return 0;
-
-	case TIOCMIWAIT:
-		dev_dbg(&port->dev, "%s TIOCMIWAIT\n", __func__);
-		return wait_modem_info(port, arg);
 	default:
 		dev_dbg(&port->dev, "%s not supported = 0x%04x\n", __func__, cmd);
 		break;
@@ -720,7 +714,7 @@ static void pl2303_update_line_status(struct usb_serial_port *port,
 	spin_unlock_irqrestore(&priv->lock, flags);
 	if (priv->line_status & UART_BREAK_ERROR)
 		usb_serial_handle_break(port);
-	wake_up_interruptible(&port->delta_msr_wait);
+	wake_up_interruptible(&port->port.delta_msr_wait);
 
 	tty = tty_port_tty_get(&port->port);
 	if (!tty)
@@ -784,7 +778,7 @@ static void pl2303_process_read_urb(struct urb *urb)
 	line_status = priv->line_status;
 	priv->line_status &= ~UART_STATE_TRANSIENT_MASK;
 	spin_unlock_irqrestore(&priv->lock, flags);
-	wake_up_interruptible(&port->delta_msr_wait);
+	wake_up_interruptible(&port->port.delta_msr_wait);
 
 	if (!urb->actual_length)
 		return;
@@ -835,6 +829,7 @@ static struct usb_serial_driver pl2303_device = {
 	.set_termios =		pl2303_set_termios,
 	.tiocmget =		pl2303_tiocmget,
 	.tiocmset =		pl2303_tiocmset,
+	.tiocmiwait =		pl2303_tiocmiwait,
 	.process_read_urb =	pl2303_process_read_urb,
 	.read_int_callback =	pl2303_read_int_callback,
 	.attach =		pl2303_startup,
diff --git a/drivers/usb/serial/quatech2.c b/drivers/usb/serial/quatech2.c
index 75f125ddb0c..02b0803425c 100644
--- a/drivers/usb/serial/quatech2.c
+++ b/drivers/usb/serial/quatech2.c
@@ -116,7 +116,6 @@ struct qt2_serial_private {
 };
 
 struct qt2_port_private {
-	bool is_open;
 	u8   device_port;
 
 	spinlock_t urb_lock;
@@ -128,8 +127,6 @@ struct qt2_port_private {
 	u8          shadowLSR;
 	u8          shadowMSR;
 
-	struct async_icount icount;
-
 	struct usb_serial_port *port;
 };
 
@@ -397,7 +394,6 @@ static int qt2_open(struct tty_struct *tty, struct usb_serial_port *port)
 		return status;
 	}
 
-	port_priv->is_open = true;
 	port_priv->device_port = (u8) device_port;
 
 	if (tty)
@@ -417,19 +413,11 @@ static void qt2_close(struct usb_serial_port *port)
 	serial = port->serial;
 	port_priv = usb_get_serial_port_data(port);
 
-	port_priv->is_open = false;
-
 	spin_lock_irqsave(&port_priv->urb_lock, flags);
 	usb_kill_urb(port_priv->write_urb);
 	port_priv->urb_in_use = false;
 	spin_unlock_irqrestore(&port_priv->urb_lock, flags);
 
-	mutex_lock(&port->serial->disc_mutex);
-	if (port->serial->disconnected) {
-		mutex_unlock(&port->serial->disc_mutex);
-		return;
-	}
-
 	/* flush the port transmit buffer */
 	i = usb_control_msg(serial->dev,
 			    usb_rcvctrlpipe(serial->dev, 0),
@@ -460,8 +448,6 @@ static void qt2_close(struct usb_serial_port *port)
 	if (i < 0)
 		dev_err(&port->dev, "%s - close port failed %i\n",
 			__func__, i);
-
-	mutex_unlock(&port->serial->disc_mutex);
 }
 
 static void qt2_disconnect(struct usb_serial *serial)
@@ -494,71 +480,6 @@ static int get_serial_info(struct usb_serial_port *port,
 	return 0;
 }
 
-static int wait_modem_info(struct usb_serial_port *port, unsigned int arg)
-{
-	struct qt2_port_private *priv = usb_get_serial_port_data(port);
-	struct async_icount prev, cur;
-	unsigned long flags;
-
-	spin_lock_irqsave(&priv->lock, flags);
-	prev = priv->icount;
-	spin_unlock_irqrestore(&priv->lock, flags);
-
-	while (1) {
-		wait_event_interruptible(port->delta_msr_wait,
-					 (port->serial->disconnected ||
-					  (priv->icount.rng != prev.rng) ||
-					  (priv->icount.dsr != prev.dsr) ||
-					  (priv->icount.dcd != prev.dcd) ||
-					  (priv->icount.cts != prev.cts)));
-
-		if (signal_pending(current))
-			return -ERESTARTSYS;
-
-		if (port->serial->disconnected)
-			return -EIO;
-
-		spin_lock_irqsave(&priv->lock, flags);
-		cur = priv->icount;
-		spin_unlock_irqrestore(&priv->lock, flags);
-
-		if ((prev.rng == cur.rng) &&
-		    (prev.dsr == cur.dsr) &&
-		    (prev.dcd == cur.dcd) &&
-		    (prev.cts == cur.cts))
-			return -EIO;
-
-		if ((arg & TIOCM_RNG && (prev.rng != cur.rng)) ||
-		    (arg & TIOCM_DSR && (prev.dsr != cur.dsr)) ||
-		    (arg & TIOCM_CD && (prev.dcd != cur.dcd)) ||
-		    (arg & TIOCM_CTS && (prev.cts != cur.cts)))
-			return 0;
-	}
-	return 0;
-}
-
-static int qt2_get_icount(struct tty_struct *tty,
-			  struct serial_icounter_struct *icount)
-{
-	struct usb_serial_port *port = tty->driver_data;
-	struct qt2_port_private *priv = usb_get_serial_port_data(port);
-	struct async_icount cnow = priv->icount;
-
-	icount->cts = cnow.cts;
-	icount->dsr = cnow.dsr;
-	icount->rng = cnow.rng;
-	icount->dcd = cnow.dcd;
-	icount->rx = cnow.rx;
-	icount->tx = cnow.tx;
-	icount->frame = cnow.frame;
-	icount->overrun = cnow.overrun;
-	icount->parity = cnow.parity;
-	icount->brk = cnow.brk;
-	icount->buf_overrun = cnow.buf_overrun;
-
-	return 0;
-}
-
 static int qt2_ioctl(struct tty_struct *tty,
 		     unsigned int cmd, unsigned long arg)
 {
@@ -568,10 +489,6 @@ static int qt2_ioctl(struct tty_struct *tty,
 	case TIOCGSERIAL:
 		return get_serial_info(port,
 				       (struct serial_struct __user *)arg);
-
-	case TIOCMIWAIT:
-		return wait_modem_info(port, arg);
-
 	default:
 		break;
 	}
@@ -664,9 +581,7 @@ void qt2_process_read_urb(struct urb *urb)
 						 __func__);
 					break;
 				}
-
-				if (port_priv->is_open)
-					tty_flip_buffer_push(&port->port);
+				tty_flip_buffer_push(&port->port);
 
 				newport = *(ch + 3);
 
@@ -709,8 +624,7 @@ void qt2_process_read_urb(struct urb *urb)
 		tty_insert_flip_string(&port->port, ch, 1);
 	}
 
-	if (port_priv->is_open)
-		tty_flip_buffer_push(&port->port);
+	tty_flip_buffer_push(&port->port);
 }
 
 static void qt2_write_bulk_callback(struct urb *urb)
@@ -910,12 +824,6 @@ static void qt2_break_ctl(struct tty_struct *tty, int break_state)
 
 	port_priv = usb_get_serial_port_data(port);
 
-	if (!port_priv->is_open) {
-		dev_err(&port->dev,
-			"%s - port is not open\n", __func__);
-		return;
-	}
-
 	val = (break_state == -1) ? 1 : 0;
 
 	status = qt2_control_msg(port->serial->dev, QT2_BREAK_CONTROL,
@@ -961,18 +869,15 @@ static void qt2_update_msr(struct usb_serial_port *port, unsigned char *ch)
 	if (newMSR & UART_MSR_ANY_DELTA) {
 		/* update input line counters */
 		if (newMSR & UART_MSR_DCTS)
-			port_priv->icount.cts++;
-
+			port->icount.cts++;
 		if (newMSR & UART_MSR_DDSR)
-			port_priv->icount.dsr++;
-
+			port->icount.dsr++;
 		if (newMSR & UART_MSR_DDCD)
-			port_priv->icount.dcd++;
-
+			port->icount.dcd++;
 		if (newMSR & UART_MSR_TERI)
-			port_priv->icount.rng++;
+			port->icount.rng++;
 
-		wake_up_interruptible(&port->delta_msr_wait);
+		wake_up_interruptible(&port->port.delta_msr_wait);
 	}
 }
 
@@ -992,7 +897,7 @@ static void qt2_update_lsr(struct usb_serial_port *port, unsigned char *ch)
 	port_priv->shadowLSR = newLSR;
 	spin_unlock_irqrestore(&port_priv->lock, flags);
 
-	icount = &port_priv->icount;
+	icount = &port->icount;
 
 	if (newLSR & UART_LSR_BRK_ERROR_BITS) {
 
@@ -1102,7 +1007,8 @@ static struct usb_serial_driver qt2_device = {
 	.break_ctl           = qt2_break_ctl,
 	.tiocmget            = qt2_tiocmget,
 	.tiocmset            = qt2_tiocmset,
-	.get_icount	     = qt2_get_icount,
+	.tiocmiwait          = usb_serial_generic_tiocmiwait,
+	.get_icount	     = usb_serial_generic_get_icount,
 	.ioctl               = qt2_ioctl,
 	.set_termios         = qt2_set_termios,
 };
diff --git a/drivers/usb/serial/sierra.c b/drivers/usb/serial/sierra.c
index c13f6e74774..8894665cd61 100644
--- a/drivers/usb/serial/sierra.c
+++ b/drivers/usb/serial/sierra.c
@@ -628,7 +628,6 @@ static void sierra_instat_callback(struct urb *urb)
 			unsigned char signals = *((unsigned char *)
 					urb->transfer_buffer +
 					sizeof(struct usb_ctrlrequest));
-			struct tty_struct *tty;
 
 			dev_dbg(&port->dev, "%s: signal x%x\n", __func__,
 				signals);
@@ -639,11 +638,8 @@ static void sierra_instat_callback(struct urb *urb)
 			portdata->dsr_state = ((signals & 0x02) ? 1 : 0);
 			portdata->ri_state = ((signals & 0x08) ? 1 : 0);
 
-			tty = tty_port_tty_get(&port->port);
-			if (tty && !C_CLOCAL(tty) &&
-					old_dcd_state && !portdata->dcd_state)
-				tty_hangup(tty);
-			tty_kref_put(tty);
+			if (old_dcd_state && !portdata->dcd_state)
+				tty_port_tty_hangup(&port->port, true);
 		} else {
 			dev_dbg(&port->dev, "%s: type %x req %x\n",
 				__func__, req_pkt->bRequestType,
@@ -778,30 +774,25 @@ static void sierra_close(struct usb_serial_port *port)
 	portdata->rts_state = 0;
 	portdata->dtr_state = 0;
 
-	if (serial->dev) {
-		mutex_lock(&serial->disc_mutex);
-		if (!serial->disconnected) {
-			serial->interface->needs_remote_wakeup = 0;
-			/* odd error handling due to pm counters */
-			if (!usb_autopm_get_interface(serial->interface))
-				sierra_send_setup(port);
-			else
-				usb_autopm_get_interface_no_resume(serial->interface);
-				
-		}
-		mutex_unlock(&serial->disc_mutex);
-		spin_lock_irq(&intfdata->susp_lock);
-		portdata->opened = 0;
-		spin_unlock_irq(&intfdata->susp_lock);
+	mutex_lock(&serial->disc_mutex);
+	if (!serial->disconnected) {
+		serial->interface->needs_remote_wakeup = 0;
+		/* odd error handling due to pm counters */
+		if (!usb_autopm_get_interface(serial->interface))
+			sierra_send_setup(port);
+		else
+			usb_autopm_get_interface_no_resume(serial->interface);
 
+	}
+	mutex_unlock(&serial->disc_mutex);
+	spin_lock_irq(&intfdata->susp_lock);
+	portdata->opened = 0;
+	spin_unlock_irq(&intfdata->susp_lock);
 
-		/* Stop reading urbs */
-		sierra_stop_rx_urbs(port);
-		/* .. and release them */
-		for (i = 0; i < portdata->num_in_urbs; i++) {
-			sierra_release_urb(portdata->in_urbs[i]);
-			portdata->in_urbs[i] = NULL;
-		}
+	sierra_stop_rx_urbs(port);
+	for (i = 0; i < portdata->num_in_urbs; i++) {
+		sierra_release_urb(portdata->in_urbs[i]);
+		portdata->in_urbs[i] = NULL;
 	}
 }
 
diff --git a/drivers/usb/serial/spcp8x5.c b/drivers/usb/serial/spcp8x5.c
index 549ef68ff5f..cf3df793c2b 100644
--- a/drivers/usb/serial/spcp8x5.c
+++ b/drivers/usb/serial/spcp8x5.c
@@ -1,7 +1,7 @@
 /*
  * spcp8x5 USB to serial adaptor driver
  *
- * Copyright (C) 2010 Johan Hovold (jhovold@gmail.com)
+ * Copyright (C) 2010-2013 Johan Hovold (jhovold@gmail.com)
  * Copyright (C) 2006 Linxb (xubin.lin@worldplus.com.cn)
  * Copyright (C) 2006 S1 Corp.
  *
@@ -13,8 +13,6 @@
  *	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/kernel.h>
 #include <linux/errno.h>
@@ -28,7 +26,10 @@
 #include <linux/usb.h>
 #include <linux/usb/serial.h>
 
-#define DRIVER_DESC 	"SPCP8x5 USB to serial adaptor driver"
+#define DRIVER_DESC	"SPCP8x5 USB to serial adaptor driver"
+
+#define SPCP825_QUIRK_NO_UART_STATUS	0x01
+#define SPCP825_QUIRK_NO_WORK_MODE	0x02
 
 #define SPCP8x5_007_VID		0x04FC
 #define SPCP8x5_007_PID		0x0201
@@ -46,13 +47,15 @@ static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE(SPCP8x5_INTERMATIC_VID, SPCP8x5_INTERMATIC_PID)},
 	{ USB_DEVICE(SPCP8x5_835_VID, SPCP8x5_835_PID)},
 	{ USB_DEVICE(SPCP8x5_008_VID, SPCP8x5_008_PID)},
-	{ USB_DEVICE(SPCP8x5_007_VID, SPCP8x5_007_PID)},
+	{ USB_DEVICE(SPCP8x5_007_VID, SPCP8x5_007_PID),
+	  .driver_info = SPCP825_QUIRK_NO_UART_STATUS |
+				SPCP825_QUIRK_NO_WORK_MODE },
 	{ }					/* Terminating entry */
 };
 MODULE_DEVICE_TABLE(usb, id_table);
 
 struct spcp8x5_usb_ctrl_arg {
-	u8 	type;
+	u8	type;
 	u8	cmd;
 	u8	cmd_type;
 	u16	value;
@@ -138,49 +141,33 @@ struct spcp8x5_usb_ctrl_arg {
 #define UART_OVERRUN_ERROR		0x40
 #define UART_CTS			0x80
 
-enum spcp8x5_type {
-	SPCP825_007_TYPE,
-	SPCP825_008_TYPE,
-	SPCP825_PHILIP_TYPE,
-	SPCP825_INTERMATIC_TYPE,
-	SPCP835_TYPE,
-};
-
 struct spcp8x5_private {
-	spinlock_t 	lock;
-	enum spcp8x5_type	type;
-	u8 			line_control;
-	u8 			line_status;
+	unsigned		quirks;
+	spinlock_t		lock;
+	u8			line_control;
 };
 
+static int spcp8x5_probe(struct usb_serial *serial,
+						const struct usb_device_id *id)
+{
+	usb_set_serial_data(serial, (void *)id);
+
+	return 0;
+}
+
 static int spcp8x5_port_probe(struct usb_serial_port *port)
 {
-	struct usb_serial *serial = port->serial;
+	const struct usb_device_id *id = usb_get_serial_data(port->serial);
 	struct spcp8x5_private *priv;
-	enum spcp8x5_type type = SPCP825_007_TYPE;
-	u16 product = le16_to_cpu(serial->dev->descriptor.idProduct);
-
-	if (product == 0x0201)
-		type = SPCP825_007_TYPE;
-	else if (product == 0x0231)
-		type = SPCP835_TYPE;
-	else if (product == 0x0235)
-		type = SPCP825_008_TYPE;
-	else if (product == 0x0204)
-		type = SPCP825_INTERMATIC_TYPE;
-	else if (product == 0x0471 &&
-		 serial->dev->descriptor.idVendor == cpu_to_le16(0x081e))
-		type = SPCP825_PHILIP_TYPE;
-	dev_dbg(&serial->dev->dev, "device type = %d\n", (int)type);
 
 	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
 
 	spin_lock_init(&priv->lock);
-	priv->type = type;
+	priv->quirks = id->driver_info;
 
-	usb_set_serial_port_data(port , priv);
+	usb_set_serial_port_data(port, priv);
 
 	return 0;
 }
@@ -195,86 +182,79 @@ static int spcp8x5_port_remove(struct usb_serial_port *port)
 	return 0;
 }
 
-/* set the modem control line of the device.
- * NOTE spcp825-007 not supported this */
-static int spcp8x5_set_ctrlLine(struct usb_device *dev, u8 value,
-				enum spcp8x5_type type)
+static int spcp8x5_set_ctrl_line(struct usb_serial_port *port, u8 mcr)
 {
+	struct spcp8x5_private *priv = usb_get_serial_port_data(port);
+	struct usb_device *dev = port->serial->dev;
 	int retval;
-	u8 mcr = 0 ;
 
-	if (type == SPCP825_007_TYPE)
+	if (priv->quirks & SPCP825_QUIRK_NO_UART_STATUS)
 		return -EPERM;
 
-	mcr = (unsigned short)value;
 	retval = usb_control_msg(dev, usb_sndctrlpipe(dev, 0),
 				 SET_UART_STATUS_TYPE, SET_UART_STATUS,
 				 mcr, 0x04, NULL, 0, 100);
-	if (retval != 0)
-		dev_dbg(&dev->dev, "usb_control_msg return %#x\n", retval);
+	if (retval != 0) {
+		dev_err(&port->dev, "failed to set control lines: %d\n",
+								retval);
+	}
 	return retval;
 }
 
-/* get the modem status register of the device
- * NOTE spcp825-007 not supported this */
-static int spcp8x5_get_msr(struct usb_device *dev, u8 *status,
-			   enum spcp8x5_type type)
+static int spcp8x5_get_msr(struct usb_serial_port *port, u8 *status)
 {
-	u8 *status_buffer;
+	struct spcp8x5_private *priv = usb_get_serial_port_data(port);
+	struct usb_device *dev = port->serial->dev;
+	u8 *buf;
 	int ret;
 
-	/* I return Permited not support here but seem inval device
-	 * is more fix */
-	if (type == SPCP825_007_TYPE)
+	if (priv->quirks & SPCP825_QUIRK_NO_UART_STATUS)
 		return -EPERM;
-	if (status == NULL)
-		return -EINVAL;
 
-	status_buffer = kmalloc(1, GFP_KERNEL);
-	if (!status_buffer)
+	buf = kzalloc(1, GFP_KERNEL);
+	if (!buf)
 		return -ENOMEM;
-	status_buffer[0] = status[0];
 
 	ret = usb_control_msg(dev, usb_rcvctrlpipe(dev, 0),
 			      GET_UART_STATUS, GET_UART_STATUS_TYPE,
-			      0, GET_UART_STATUS_MSR, status_buffer, 1, 100);
+			      0, GET_UART_STATUS_MSR, buf, 1, 100);
 	if (ret < 0)
-		dev_dbg(&dev->dev, "Get MSR = 0x%p failed (error = %d)",
-			status_buffer, ret);
+		dev_err(&port->dev, "failed to get modem status: %d", ret);
 
-	dev_dbg(&dev->dev, "0xc0:0x22:0:6  %d - 0x%p ", ret, status_buffer);
-	status[0] = status_buffer[0];
-	kfree(status_buffer);
+	dev_dbg(&port->dev, "0xc0:0x22:0:6  %d - 0x02%x", ret, *buf);
+	*status = *buf;
+	kfree(buf);
 
 	return ret;
 }
 
-/* select the work mode.
- * NOTE this function not supported by spcp825-007 */
-static void spcp8x5_set_workMode(struct usb_device *dev, u16 value,
-				 u16 index, enum spcp8x5_type type)
+static void spcp8x5_set_work_mode(struct usb_serial_port *port, u16 value,
+								 u16 index)
 {
+	struct spcp8x5_private *priv = usb_get_serial_port_data(port);
+	struct usb_device *dev = port->serial->dev;
 	int ret;
 
-	/* I return Permited not support here but seem inval device
-	 * is more fix */
-	if (type == SPCP825_007_TYPE)
+	if (priv->quirks & SPCP825_QUIRK_NO_WORK_MODE)
 		return;
 
 	ret = usb_control_msg(dev, usb_sndctrlpipe(dev, 0),
 			      SET_WORKING_MODE_TYPE, SET_WORKING_MODE,
 			      value, index, NULL, 0, 100);
-	dev_dbg(&dev->dev, "value = %#x , index = %#x\n", value, index);
+	dev_dbg(&port->dev, "value = %#x , index = %#x\n", value, index);
 	if (ret < 0)
-		dev_dbg(&dev->dev,
-			"RTSCTS usb_control_msg(enable flowctrl) = %d\n", ret);
+		dev_err(&port->dev, "failed to set work mode: %d\n", ret);
 }
 
 static int spcp8x5_carrier_raised(struct usb_serial_port *port)
 {
-	struct spcp8x5_private *priv = usb_get_serial_port_data(port);
-	if (priv->line_status & MSR_STATUS_LINE_DCD)
+	u8 msr;
+	int ret;
+
+	ret = spcp8x5_get_msr(port, &msr);
+	if (ret || msr & MSR_STATUS_LINE_DCD)
 		return 1;
+
 	return 0;
 }
 
@@ -293,20 +273,17 @@ static void spcp8x5_dtr_rts(struct usb_serial_port *port, int on)
 						| MCR_CONTROL_LINE_RTS);
 	control = priv->line_control;
 	spin_unlock_irqrestore(&priv->lock, flags);
-	spcp8x5_set_ctrlLine(port->serial->dev, control , priv->type);
+	spcp8x5_set_ctrl_line(port, control);
 }
 
 static void spcp8x5_init_termios(struct tty_struct *tty)
 {
-	/* for the 1st time call this function */
 	tty->termios = tty_std_termios;
 	tty->termios.c_cflag = B115200 | CS8 | CREAD | HUPCL | CLOCAL;
 	tty->termios.c_ispeed = 115200;
 	tty->termios.c_ospeed = 115200;
 }
 
-/* set the serial param for transfer. we should check if we really need to
- * transfer. if we set flow control we should do this too. */
 static void spcp8x5_set_termios(struct tty_struct *tty,
 		struct usb_serial_port *port, struct ktermios *old_termios)
 {
@@ -321,7 +298,6 @@ static void spcp8x5_set_termios(struct tty_struct *tty,
 	int i;
 	u8 control;
 
-
 	/* check that they really want us to change something */
 	if (!tty_termios_hw_change(&tty->termios, old_termios))
 		return;
@@ -337,7 +313,7 @@ static void spcp8x5_set_termios(struct tty_struct *tty,
 	if (control != priv->line_control) {
 		control = priv->line_control;
 		spin_unlock_irqrestore(&priv->lock, flags);
-		spcp8x5_set_ctrlLine(serial->dev, control , priv->type);
+		spcp8x5_set_ctrl_line(port, control);
 	} else {
 		spin_unlock_irqrestore(&priv->lock, flags);
 	}
@@ -397,9 +373,9 @@ static void spcp8x5_set_termios(struct tty_struct *tty,
 	if (cflag & PARENB) {
 		buf[1] |= (cflag & PARODD) ?
 		SET_UART_FORMAT_PAR_ODD : SET_UART_FORMAT_PAR_EVEN ;
-	} else
+	} else {
 		buf[1] |= SET_UART_FORMAT_PAR_NONE;
-
+	}
 	uartdata = buf[0] | buf[1]<<8;
 
 	i = usb_control_msg(serial->dev, usb_sndctrlpipe(serial->dev, 0),
@@ -412,22 +388,16 @@ static void spcp8x5_set_termios(struct tty_struct *tty,
 
 	if (cflag & CRTSCTS) {
 		/* enable hardware flow control */
-		spcp8x5_set_workMode(serial->dev, 0x000a,
-				     SET_WORKING_MODE_U2C, priv->type);
+		spcp8x5_set_work_mode(port, 0x000a, SET_WORKING_MODE_U2C);
 	}
 }
 
-/* open the serial port. do some usb system call. set termios and get the line
- * status of the device. */
 static int spcp8x5_open(struct tty_struct *tty, struct usb_serial_port *port)
 {
 	struct ktermios tmp_termios;
 	struct usb_serial *serial = port->serial;
 	struct spcp8x5_private *priv = usb_get_serial_port_data(port);
 	int ret;
-	unsigned long flags;
-	u8 status = 0x30;
-	/* status 0x30 means DSR and CTS = 1 other CDC RI and delta = 0 */
 
 	usb_clear_halt(serial->dev, port->write_urb->pipe);
 	usb_clear_halt(serial->dev, port->read_urb->pipe);
@@ -438,142 +408,16 @@ static int spcp8x5_open(struct tty_struct *tty, struct usb_serial_port *port)
 	if (ret)
 		return ret;
 
-	spcp8x5_set_ctrlLine(serial->dev, priv->line_control , priv->type);
+	spcp8x5_set_ctrl_line(port, priv->line_control);
 
-	/* Setup termios */
 	if (tty)
 		spcp8x5_set_termios(tty, port, &tmp_termios);
 
-	spcp8x5_get_msr(serial->dev, &status, priv->type);
-
-	/* may be we should update uart status here but now we did not do */
-	spin_lock_irqsave(&priv->lock, flags);
-	priv->line_status = status & 0xf0 ;
-	spin_unlock_irqrestore(&priv->lock, flags);
-
 	port->port.drain_delay = 256;
 
 	return usb_serial_generic_open(tty, port);
 }
 
-static void spcp8x5_process_read_urb(struct urb *urb)
-{
-	struct usb_serial_port *port = urb->context;
-	struct spcp8x5_private *priv = usb_get_serial_port_data(port);
-	unsigned char *data = urb->transfer_buffer;
-	unsigned long flags;
-	u8 status;
-	char tty_flag;
-
-	/* get tty_flag from status */
-	tty_flag = TTY_NORMAL;
-
-	spin_lock_irqsave(&priv->lock, flags);
-	status = priv->line_status;
-	priv->line_status &= ~UART_STATE_TRANSIENT_MASK;
-	spin_unlock_irqrestore(&priv->lock, flags);
-	/* wake up the wait for termios */
-	wake_up_interruptible(&port->delta_msr_wait);
-
-	if (!urb->actual_length)
-		return;
-
-
-	if (status & UART_STATE_TRANSIENT_MASK) {
-		/* break takes precedence over parity, which takes precedence
-		 * over framing errors */
-		if (status & UART_BREAK_ERROR)
-			tty_flag = TTY_BREAK;
-		else if (status & UART_PARITY_ERROR)
-			tty_flag = TTY_PARITY;
-		else if (status & UART_FRAME_ERROR)
-			tty_flag = TTY_FRAME;
-		dev_dbg(&port->dev, "tty_flag = %d\n", tty_flag);
-
-		/* overrun is special, not associated with a char */
-		if (status & UART_OVERRUN_ERROR)
-			tty_insert_flip_char(&port->port, 0, TTY_OVERRUN);
-
-		if (status & UART_DCD) {
-			struct tty_struct *tty = tty_port_tty_get(&port->port);
-			if (tty) {
-				usb_serial_handle_dcd_change(port, tty,
-				       priv->line_status & MSR_STATUS_LINE_DCD);
-				tty_kref_put(tty);
-			}
-		}
-	}
-
-	tty_insert_flip_string_fixed_flag(&port->port, data, tty_flag,
-							urb->actual_length);
-	tty_flip_buffer_push(&port->port);
-}
-
-static int spcp8x5_wait_modem_info(struct usb_serial_port *port,
-				   unsigned int arg)
-{
-	struct spcp8x5_private *priv = usb_get_serial_port_data(port);
-	unsigned long flags;
-	unsigned int prevstatus;
-	unsigned int status;
-	unsigned int changed;
-
-	spin_lock_irqsave(&priv->lock, flags);
-	prevstatus = priv->line_status;
-	spin_unlock_irqrestore(&priv->lock, flags);
-
-	while (1) {
-		/* wake up in bulk read */
-		interruptible_sleep_on(&port->delta_msr_wait);
-
-		/* see if a signal did it */
-		if (signal_pending(current))
-			return -ERESTARTSYS;
-
-		if (port->serial->disconnected)
-			return -EIO;
-
-		spin_lock_irqsave(&priv->lock, flags);
-		status = priv->line_status;
-		spin_unlock_irqrestore(&priv->lock, flags);
-
-		changed = prevstatus^status;
-
-		if (((arg & TIOCM_RNG) && (changed & MSR_STATUS_LINE_RI)) ||
-		    ((arg & TIOCM_DSR) && (changed & MSR_STATUS_LINE_DSR)) ||
-		    ((arg & TIOCM_CD)  && (changed & MSR_STATUS_LINE_DCD)) ||
-		    ((arg & TIOCM_CTS) && (changed & MSR_STATUS_LINE_CTS)))
-			return 0;
-
-		prevstatus = status;
-	}
-	/* NOTREACHED */
-	return 0;
-}
-
-static int spcp8x5_ioctl(struct tty_struct *tty,
-			 unsigned int cmd, unsigned long arg)
-{
-	struct usb_serial_port *port = tty->driver_data;
-
-	dev_dbg(&port->dev, "%s (%d) cmd = 0x%04x\n", __func__,
-		port->number, cmd);
-
-	switch (cmd) {
-	case TIOCMIWAIT:
-		dev_dbg(&port->dev, "%s (%d) TIOCMIWAIT\n", __func__,
-			port->number);
-		return spcp8x5_wait_modem_info(port, arg);
-
-	default:
-		dev_dbg(&port->dev, "%s not supported = 0x%04x", __func__,
-			cmd);
-		break;
-	}
-
-	return -ENOIOCTLCMD;
-}
-
 static int spcp8x5_tiocmset(struct tty_struct *tty,
 			    unsigned int set, unsigned int clear)
 {
@@ -594,7 +438,7 @@ static int spcp8x5_tiocmset(struct tty_struct *tty,
 	control = priv->line_control;
 	spin_unlock_irqrestore(&priv->lock, flags);
 
-	return spcp8x5_set_ctrlLine(port->serial->dev, control , priv->type);
+	return spcp8x5_set_ctrl_line(port, control);
 }
 
 static int spcp8x5_tiocmget(struct tty_struct *tty)
@@ -603,12 +447,15 @@ static int spcp8x5_tiocmget(struct tty_struct *tty)
 	struct spcp8x5_private *priv = usb_get_serial_port_data(port);
 	unsigned long flags;
 	unsigned int mcr;
-	unsigned int status;
+	u8 status;
 	unsigned int result;
 
+	result = spcp8x5_get_msr(port, &status);
+	if (result)
+		return result;
+
 	spin_lock_irqsave(&priv->lock, flags);
 	mcr = priv->line_control;
-	status = priv->line_status;
 	spin_unlock_irqrestore(&priv->lock, flags);
 
 	result = ((mcr & MCR_DTR)			? TIOCM_DTR : 0)
@@ -621,7 +468,6 @@ static int spcp8x5_tiocmget(struct tty_struct *tty)
 	return result;
 }
 
-/* All of the device info needed for the spcp8x5 SIO serial converter */
 static struct usb_serial_driver spcp8x5_device = {
 	.driver = {
 		.owner =	THIS_MODULE,
@@ -629,17 +475,16 @@ static struct usb_serial_driver spcp8x5_device = {
 	},
 	.id_table		= id_table,
 	.num_ports		= 1,
-	.open 			= spcp8x5_open,
+	.open			= spcp8x5_open,
 	.dtr_rts		= spcp8x5_dtr_rts,
 	.carrier_raised		= spcp8x5_carrier_raised,
-	.set_termios 		= spcp8x5_set_termios,
+	.set_termios		= spcp8x5_set_termios,
 	.init_termios		= spcp8x5_init_termios,
-	.ioctl 			= spcp8x5_ioctl,
-	.tiocmget 		= spcp8x5_tiocmget,
-	.tiocmset 		= spcp8x5_tiocmset,
+	.tiocmget		= spcp8x5_tiocmget,
+	.tiocmset		= spcp8x5_tiocmset,
+	.probe			= spcp8x5_probe,
 	.port_probe		= spcp8x5_port_probe,
 	.port_remove		= spcp8x5_port_remove,
-	.process_read_urb	= spcp8x5_process_read_urb,
 };
 
 static struct usb_serial_driver * const serial_drivers[] = {
diff --git a/drivers/usb/serial/ssu100.c b/drivers/usb/serial/ssu100.c
index 4b2a19757b4..5b62dbbdf99 100644
--- a/drivers/usb/serial/ssu100.c
+++ b/drivers/usb/serial/ssu100.c
@@ -61,7 +61,6 @@ struct ssu100_port_private {
 	spinlock_t status_lock;
 	u8 shadowLSR;
 	u8 shadowMSR;
-	struct async_icount icount;
 };
 
 static inline int ssu100_control_msg(struct usb_device *dev,
@@ -315,11 +314,6 @@ static int ssu100_open(struct tty_struct *tty, struct usb_serial_port *port)
 	return usb_serial_generic_open(tty, port);
 }
 
-static void ssu100_close(struct usb_serial_port *port)
-{
-	usb_serial_generic_close(port);
-}
-
 static int get_serial_info(struct usb_serial_port *port,
 			   struct serial_struct __user *retinfo)
 {
@@ -343,73 +337,6 @@ static int get_serial_info(struct usb_serial_port *port,
 	return 0;
 }
 
-static int wait_modem_info(struct usb_serial_port *port, unsigned int arg)
-{
-	struct ssu100_port_private *priv = usb_get_serial_port_data(port);
-	struct async_icount prev, cur;
-	unsigned long flags;
-
-	spin_lock_irqsave(&priv->status_lock, flags);
-	prev = priv->icount;
-	spin_unlock_irqrestore(&priv->status_lock, flags);
-
-	while (1) {
-		wait_event_interruptible(port->delta_msr_wait,
-					 (port->serial->disconnected ||
-					  (priv->icount.rng != prev.rng) ||
-					  (priv->icount.dsr != prev.dsr) ||
-					  (priv->icount.dcd != prev.dcd) ||
-					  (priv->icount.cts != prev.cts)));
-
-		if (signal_pending(current))
-			return -ERESTARTSYS;
-
-		if (port->serial->disconnected)
-			return -EIO;
-
-		spin_lock_irqsave(&priv->status_lock, flags);
-		cur = priv->icount;
-		spin_unlock_irqrestore(&priv->status_lock, flags);
-
-		if ((prev.rng == cur.rng) &&
-		    (prev.dsr == cur.dsr) &&
-		    (prev.dcd == cur.dcd) &&
-		    (prev.cts == cur.cts))
-			return -EIO;
-
-		if ((arg & TIOCM_RNG && (prev.rng != cur.rng)) ||
-		    (arg & TIOCM_DSR && (prev.dsr != cur.dsr)) ||
-		    (arg & TIOCM_CD  && (prev.dcd != cur.dcd)) ||
-		    (arg & TIOCM_CTS && (prev.cts != cur.cts)))
-			return 0;
-	}
-	return 0;
-}
-
-static int ssu100_get_icount(struct tty_struct *tty,
-			struct serial_icounter_struct *icount)
-{
-	struct usb_serial_port *port = tty->driver_data;
-	struct ssu100_port_private *priv = usb_get_serial_port_data(port);
-	struct async_icount cnow = priv->icount;
-
-	icount->cts = cnow.cts;
-	icount->dsr = cnow.dsr;
-	icount->rng = cnow.rng;
-	icount->dcd = cnow.dcd;
-	icount->rx = cnow.rx;
-	icount->tx = cnow.tx;
-	icount->frame = cnow.frame;
-	icount->overrun = cnow.overrun;
-	icount->parity = cnow.parity;
-	icount->brk = cnow.brk;
-	icount->buf_overrun = cnow.buf_overrun;
-
-	return 0;
-}
-
-
-
 static int ssu100_ioctl(struct tty_struct *tty,
 		    unsigned int cmd, unsigned long arg)
 {
@@ -421,10 +348,6 @@ static int ssu100_ioctl(struct tty_struct *tty,
 	case TIOCGSERIAL:
 		return get_serial_info(port,
 				       (struct serial_struct __user *) arg);
-
-	case TIOCMIWAIT:
-		return wait_modem_info(port, arg);
-
 	default:
 		break;
 	}
@@ -532,14 +455,14 @@ static void ssu100_update_msr(struct usb_serial_port *port, u8 msr)
 	if (msr & UART_MSR_ANY_DELTA) {
 		/* update input line counters */
 		if (msr & UART_MSR_DCTS)
-			priv->icount.cts++;
+			port->icount.cts++;
 		if (msr & UART_MSR_DDSR)
-			priv->icount.dsr++;
+			port->icount.dsr++;
 		if (msr & UART_MSR_DDCD)
-			priv->icount.dcd++;
+			port->icount.dcd++;
 		if (msr & UART_MSR_TERI)
-			priv->icount.rng++;
-		wake_up_interruptible(&port->delta_msr_wait);
+			port->icount.rng++;
+		wake_up_interruptible(&port->port.delta_msr_wait);
 	}
 }
 
@@ -558,22 +481,22 @@ static void ssu100_update_lsr(struct usb_serial_port *port, u8 lsr,
 		/* we always want to update icount, but we only want to
 		 * update tty_flag for one case */
 		if (lsr & UART_LSR_BI) {
-			priv->icount.brk++;
+			port->icount.brk++;
 			*tty_flag = TTY_BREAK;
 			usb_serial_handle_break(port);
 		}
 		if (lsr & UART_LSR_PE) {
-			priv->icount.parity++;
+			port->icount.parity++;
 			if (*tty_flag == TTY_NORMAL)
 				*tty_flag = TTY_PARITY;
 		}
 		if (lsr & UART_LSR_FE) {
-			priv->icount.frame++;
+			port->icount.frame++;
 			if (*tty_flag == TTY_NORMAL)
 				*tty_flag = TTY_FRAME;
 		}
 		if (lsr & UART_LSR_OE){
-			priv->icount.overrun++;
+			port->icount.overrun++;
 			if (*tty_flag == TTY_NORMAL)
 				*tty_flag = TTY_OVERRUN;
 		}
@@ -630,7 +553,6 @@ static struct usb_serial_driver ssu100_device = {
 	.id_table	     = id_table,
 	.num_ports	     = 1,
 	.open		     = ssu100_open,
-	.close		     = ssu100_close,
 	.attach              = ssu100_attach,
 	.port_probe          = ssu100_port_probe,
 	.port_remove         = ssu100_port_remove,
@@ -638,10 +560,10 @@ static struct usb_serial_driver ssu100_device = {
 	.process_read_urb    = ssu100_process_read_urb,
 	.tiocmget            = ssu100_tiocmget,
 	.tiocmset            = ssu100_tiocmset,
-	.get_icount	     = ssu100_get_icount,
+	.tiocmiwait          = usb_serial_generic_tiocmiwait,
+	.get_icount	     = usb_serial_generic_get_icount,
 	.ioctl               = ssu100_ioctl,
 	.set_termios         = ssu100_set_termios,
-	.disconnect          = usb_serial_generic_disconnect,
 };
 
 static struct usb_serial_driver * const serial_drivers[] = {
diff --git a/drivers/usb/serial/symbolserial.c b/drivers/usb/serial/symbolserial.c
index be05e6caf9a..9b1648945e7 100644
--- a/drivers/usb/serial/symbolserial.c
+++ b/drivers/usb/serial/symbolserial.c
@@ -1,6 +1,7 @@
 /*
  * Symbol USB barcode to serial driver
  *
+ * Copyright (C) 2013 Johan Hovold <jhovold@gmail.com>
  * Copyright (C) 2009 Greg Kroah-Hartman <gregkh@suse.de>
  * Copyright (C) 2009 Novell Inc.
  *
@@ -26,27 +27,17 @@ static const struct usb_device_id id_table[] = {
 };
 MODULE_DEVICE_TABLE(usb, id_table);
 
-/* This structure holds all of the individual device information */
 struct symbol_private {
-	struct usb_device *udev;
-	struct usb_serial *serial;
-	struct usb_serial_port *port;
-	unsigned char *int_buffer;
-	struct urb *int_urb;
-	int buffer_size;
-	u8 bInterval;
-	u8 int_address;
 	spinlock_t lock;	/* protects the following flags */
 	bool throttled;
 	bool actually_throttled;
-	bool rts;
 };
 
 static void symbol_int_callback(struct urb *urb)
 {
-	struct symbol_private *priv = urb->context;
+	struct usb_serial_port *port = urb->context;
+	struct symbol_private *priv = usb_get_serial_port_data(port);
 	unsigned char *data = urb->transfer_buffer;
-	struct usb_serial_port *port = priv->port;
 	int status = urb->status;
 	int result;
 	int data_length;
@@ -84,7 +75,7 @@ static void symbol_int_callback(struct urb *urb)
 		tty_insert_flip_string(&port->port, &data[1], data_length);
 		tty_flip_buffer_push(&port->port);
 	} else {
-		dev_dbg(&priv->udev->dev,
+		dev_dbg(&port->dev,
 			"Improper amount of data received from the device, "
 			"%d bytes", urb->actual_length);
 	}
@@ -94,12 +85,7 @@ exit:
 
 	/* Continue trying to always read if we should */
 	if (!priv->throttled) {
-		usb_fill_int_urb(priv->int_urb, priv->udev,
-				 usb_rcvintpipe(priv->udev,
-				 		priv->int_address),
-				 priv->int_buffer, priv->buffer_size,
-				 symbol_int_callback, priv, priv->bInterval);
-		result = usb_submit_urb(priv->int_urb, GFP_ATOMIC);
+		result = usb_submit_urb(port->interrupt_in_urb, GFP_ATOMIC);
 		if (result)
 			dev_err(&port->dev,
 			    "%s - failed resubmitting read urb, error %d\n",
@@ -118,15 +104,10 @@ static int symbol_open(struct tty_struct *tty, struct usb_serial_port *port)
 	spin_lock_irqsave(&priv->lock, flags);
 	priv->throttled = false;
 	priv->actually_throttled = false;
-	priv->port = port;
 	spin_unlock_irqrestore(&priv->lock, flags);
 
 	/* Start reading from the device */
-	usb_fill_int_urb(priv->int_urb, priv->udev,
-			 usb_rcvintpipe(priv->udev, priv->int_address),
-			 priv->int_buffer, priv->buffer_size,
-			 symbol_int_callback, priv, priv->bInterval);
-	result = usb_submit_urb(priv->int_urb, GFP_KERNEL);
+	result = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL);
 	if (result)
 		dev_err(&port->dev,
 			"%s - failed resubmitting read urb, error %d\n",
@@ -136,10 +117,7 @@ static int symbol_open(struct tty_struct *tty, struct usb_serial_port *port)
 
 static void symbol_close(struct usb_serial_port *port)
 {
-	struct symbol_private *priv = usb_get_serial_data(port->serial);
-
-	/* shutdown our urbs */
-	usb_kill_urb(priv->int_urb);
+	usb_kill_urb(port->interrupt_in_urb);
 }
 
 static void symbol_throttle(struct tty_struct *tty)
@@ -166,7 +144,7 @@ static void symbol_unthrottle(struct tty_struct *tty)
 	spin_unlock_irq(&priv->lock);
 
 	if (was_throttled) {
-		result = usb_submit_urb(priv->int_urb, GFP_KERNEL);
+		result = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL);
 		if (result)
 			dev_err(&port->dev,
 				"%s - failed submitting read urb, error %d\n",
@@ -176,89 +154,36 @@ static void symbol_unthrottle(struct tty_struct *tty)
 
 static int symbol_startup(struct usb_serial *serial)
 {
+	if (!serial->num_interrupt_in) {
+		dev_err(&serial->dev->dev, "no interrupt-in endpoint\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int symbol_port_probe(struct usb_serial_port *port)
+{
 	struct symbol_private *priv;
-	struct usb_host_interface *intf;
-	int i;
-	int retval = -ENOMEM;
-	bool int_in_found = false;
 
-	/* create our private serial structure */
 	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
-	if (priv == NULL) {
-		dev_err(&serial->dev->dev, "%s - Out of memory\n", __func__);
+	if (!priv)
 		return -ENOMEM;
-	}
+
 	spin_lock_init(&priv->lock);
-	priv->serial = serial;
-	priv->port = serial->port[0];
-	priv->udev = serial->dev;
-
-	/* find our interrupt endpoint */
-	intf = serial->interface->altsetting;
-	for (i = 0; i < intf->desc.bNumEndpoints; ++i) {
-		struct usb_endpoint_descriptor *endpoint;
-
-		endpoint = &intf->endpoint[i].desc;
-		if (!usb_endpoint_is_int_in(endpoint))
-			continue;
-
-		priv->int_urb = usb_alloc_urb(0, GFP_KERNEL);
-		if (!priv->int_urb) {
-			dev_err(&priv->udev->dev, "out of memory\n");
-			goto error;
-		}
-
-		priv->buffer_size = usb_endpoint_maxp(endpoint) * 2;
-		priv->int_buffer = kmalloc(priv->buffer_size, GFP_KERNEL);
-		if (!priv->int_buffer) {
-			dev_err(&priv->udev->dev, "out of memory\n");
-			goto error;
-		}
-
-		priv->int_address = endpoint->bEndpointAddress;
-		priv->bInterval = endpoint->bInterval;
-
-		/* set up our int urb */
-		usb_fill_int_urb(priv->int_urb, priv->udev,
-				 usb_rcvintpipe(priv->udev,
-				 		endpoint->bEndpointAddress),
-				 priv->int_buffer, priv->buffer_size,
-				 symbol_int_callback, priv, priv->bInterval);
-
-		int_in_found = true;
-		break;
-		}
 
-	if (!int_in_found) {
-		dev_err(&priv->udev->dev,
-			"Error - the proper endpoints were not found!\n");
-		goto error;
-	}
+	usb_set_serial_port_data(port, priv);
 
-	usb_set_serial_data(serial, priv);
 	return 0;
-
-error:
-	usb_free_urb(priv->int_urb);
-	kfree(priv->int_buffer);
-	kfree(priv);
-	return retval;
-}
-
-static void symbol_disconnect(struct usb_serial *serial)
-{
-	struct symbol_private *priv = usb_get_serial_data(serial);
-
-	usb_kill_urb(priv->int_urb);
-	usb_free_urb(priv->int_urb);
 }
 
-static void symbol_release(struct usb_serial *serial)
+static int symbol_port_remove(struct usb_serial_port *port)
 {
-	struct symbol_private *priv = usb_get_serial_data(serial);
+	struct symbol_private *priv = usb_get_serial_port_data(port);
 
-	kfree(priv->int_buffer);
 	kfree(priv);
+
+	return 0;
 }
 
 static struct usb_serial_driver symbol_device = {
@@ -269,12 +194,13 @@ static struct usb_serial_driver symbol_device = {
 	.id_table =		id_table,
 	.num_ports =		1,
 	.attach =		symbol_startup,
+	.port_probe =		symbol_port_probe,
+	.port_remove =		symbol_port_remove,
 	.open =			symbol_open,
 	.close =		symbol_close,
-	.disconnect =		symbol_disconnect,
-	.release =		symbol_release,
 	.throttle = 		symbol_throttle,
 	.unthrottle =		symbol_unthrottle,
+	.read_int_callback =	symbol_int_callback,
 };
 
 static struct usb_serial_driver * const serial_drivers[] = {
diff --git a/drivers/usb/serial/ti_usb_3410_5052.c b/drivers/usb/serial/ti_usb_3410_5052.c
index 73deb029fc0..cac47aef291 100644
--- a/drivers/usb/serial/ti_usb_3410_5052.c
+++ b/drivers/usb/serial/ti_usb_3410_5052.c
@@ -67,13 +67,10 @@
 struct ti_port {
 	int			tp_is_open;
 	__u8			tp_msr;
-	__u8			tp_lsr;
 	__u8			tp_shadow_mcr;
 	__u8			tp_uart_mode;	/* 232 or 485 modes */
 	unsigned int		tp_uart_base_addr;
 	int			tp_flags;
-	int			tp_closing_wait;/* in .01 secs */
-	struct async_icount	tp_icount;
 	wait_queue_head_t	tp_write_wait;
 	struct ti_device	*tp_tdev;
 	struct usb_serial_port	*tp_port;
@@ -108,8 +105,6 @@ static void ti_throttle(struct tty_struct *tty);
 static void ti_unthrottle(struct tty_struct *tty);
 static int ti_ioctl(struct tty_struct *tty,
 		unsigned int cmd, unsigned long arg);
-static int ti_get_icount(struct tty_struct *tty,
-		struct serial_icounter_struct *icount);
 static void ti_set_termios(struct tty_struct *tty,
 		struct usb_serial_port *port, struct ktermios *old_termios);
 static int ti_tiocmget(struct tty_struct *tty);
@@ -124,15 +119,13 @@ static void ti_recv(struct usb_serial_port *port, unsigned char *data,
 		int length);
 static void ti_send(struct ti_port *tport);
 static int ti_set_mcr(struct ti_port *tport, unsigned int mcr);
-static int ti_get_lsr(struct ti_port *tport);
+static int ti_get_lsr(struct ti_port *tport, u8 *lsr);
 static int ti_get_serial_info(struct ti_port *tport,
 	struct serial_struct __user *ret_arg);
 static int ti_set_serial_info(struct tty_struct *tty, struct ti_port *tport,
 	struct serial_struct __user *new_arg);
 static void ti_handle_new_msr(struct ti_port *tport, __u8 msr);
 
-static void ti_drain(struct ti_port *tport, unsigned long timeout, int flush);
-
 static void ti_stop_read(struct ti_port *tport, struct tty_struct *tty);
 static int ti_restart_read(struct ti_port *tport, struct tty_struct *tty);
 
@@ -235,7 +228,8 @@ static struct usb_serial_driver ti_1port_device = {
 	.set_termios		= ti_set_termios,
 	.tiocmget		= ti_tiocmget,
 	.tiocmset		= ti_tiocmset,
-	.get_icount		= ti_get_icount,
+	.tiocmiwait		= usb_serial_generic_tiocmiwait,
+	.get_icount		= usb_serial_generic_get_icount,
 	.break_ctl		= ti_break,
 	.read_int_callback	= ti_interrupt_callback,
 	.read_bulk_callback	= ti_bulk_in_callback,
@@ -265,7 +259,8 @@ static struct usb_serial_driver ti_2port_device = {
 	.set_termios		= ti_set_termios,
 	.tiocmget		= ti_tiocmget,
 	.tiocmset		= ti_tiocmset,
-	.get_icount		= ti_get_icount,
+	.tiocmiwait		= usb_serial_generic_tiocmiwait,
+	.get_icount		= usb_serial_generic_get_icount,
 	.break_ctl		= ti_break,
 	.read_int_callback	= ti_interrupt_callback,
 	.read_bulk_callback	= ti_bulk_in_callback,
@@ -430,7 +425,7 @@ static int ti_port_probe(struct usb_serial_port *port)
 		tport->tp_uart_base_addr = TI_UART1_BASE_ADDR;
 	else
 		tport->tp_uart_base_addr = TI_UART2_BASE_ADDR;
-	tport->tp_closing_wait = closing_wait;
+	port->port.closing_wait = msecs_to_jiffies(10 * closing_wait);
 	init_waitqueue_head(&tport->tp_write_wait);
 	if (kfifo_alloc(&tport->write_fifo, TI_WRITE_BUF_SIZE, GFP_KERNEL)) {
 		kfree(tport);
@@ -480,8 +475,6 @@ static int ti_open(struct tty_struct *tty, struct usb_serial_port *port)
 
 	port_number = port->number - port->serial->minor;
 
-	memset(&(tport->tp_icount), 0x00, sizeof(tport->tp_icount));
-
 	tport->tp_msr = 0;
 	tport->tp_shadow_mcr |= (TI_MCR_RTS | TI_MCR_DTR);
 
@@ -585,6 +578,8 @@ static int ti_open(struct tty_struct *tty, struct usb_serial_port *port)
 	tport->tp_is_open = 1;
 	++tdev->td_open_port_count;
 
+	port->port.drain_delay = 3;
+
 	goto release_lock;
 
 unlink_int_urb:
@@ -604,6 +599,7 @@ static void ti_close(struct usb_serial_port *port)
 	int port_number;
 	int status;
 	int do_unlock;
+	unsigned long flags;
 
 	tdev = usb_get_serial_data(port->serial);
 	tport = usb_get_serial_port_data(port);
@@ -612,11 +608,12 @@ static void ti_close(struct usb_serial_port *port)
 
 	tport->tp_is_open = 0;
 
-	ti_drain(tport, (tport->tp_closing_wait*HZ)/100, 1);
-
 	usb_kill_urb(port->read_urb);
 	usb_kill_urb(port->write_urb);
 	tport->tp_write_urb_in_use = 0;
+	spin_lock_irqsave(&tport->tp_lock, flags);
+	kfifo_reset_out(&tport->write_fifo);
+	spin_unlock_irqrestore(&tport->tp_lock, flags);
 
 	port_number = port->number - port->serial->minor;
 
@@ -687,6 +684,8 @@ static int ti_chars_in_buffer(struct tty_struct *tty)
 	struct ti_port *tport = usb_get_serial_port_data(port);
 	int chars = 0;
 	unsigned long flags;
+	int ret;
+	u8 lsr;
 
 	if (tport == NULL)
 		return 0;
@@ -695,6 +694,12 @@ static int ti_chars_in_buffer(struct tty_struct *tty)
 	chars = kfifo_len(&tport->write_fifo);
 	spin_unlock_irqrestore(&tport->tp_lock, flags);
 
+	if (!chars) {
+		ret = ti_get_lsr(tport, &lsr);
+		if (!ret && !(lsr & TI_LSR_TX_EMPTY))
+			chars = 1;
+	}
+
 	dev_dbg(&port->dev, "%s - returns %d\n", __func__, chars);
 	return chars;
 }
@@ -731,38 +736,11 @@ static void ti_unthrottle(struct tty_struct *tty)
 	}
 }
 
-static int ti_get_icount(struct tty_struct *tty,
-		struct serial_icounter_struct *icount)
-{
-	struct usb_serial_port *port = tty->driver_data;
-	struct ti_port *tport = usb_get_serial_port_data(port);
-	struct async_icount cnow = tport->tp_icount;
-
-	dev_dbg(&port->dev, "%s - TIOCGICOUNT RX=%d, TX=%d\n", __func__,
-		cnow.rx, cnow.tx);
-
-	icount->cts = cnow.cts;
-	icount->dsr = cnow.dsr;
-	icount->rng = cnow.rng;
-	icount->dcd = cnow.dcd;
-	icount->rx = cnow.rx;
-	icount->tx = cnow.tx;
-	icount->frame = cnow.frame;
-	icount->overrun = cnow.overrun;
-	icount->parity = cnow.parity;
-	icount->brk = cnow.brk;
-	icount->buf_overrun = cnow.buf_overrun;
-
-	return 0;
-}
-
 static int ti_ioctl(struct tty_struct *tty,
 	unsigned int cmd, unsigned long arg)
 {
 	struct usb_serial_port *port = tty->driver_data;
 	struct ti_port *tport = usb_get_serial_port_data(port);
-	struct async_icount cnow;
-	struct async_icount cprev;
 
 	dev_dbg(&port->dev, "%s - cmd = 0x%04X\n", __func__, cmd);
 
@@ -778,29 +756,6 @@ static int ti_ioctl(struct tty_struct *tty,
 		dev_dbg(&port->dev, "%s - TIOCSSERIAL\n", __func__);
 		return ti_set_serial_info(tty, tport,
 				(struct serial_struct __user *)arg);
-	case TIOCMIWAIT:
-		dev_dbg(&port->dev, "%s - TIOCMIWAIT\n", __func__);
-		cprev = tport->tp_icount;
-		while (1) {
-			interruptible_sleep_on(&port->delta_msr_wait);
-			if (signal_pending(current))
-				return -ERESTARTSYS;
-
-			if (port->serial->disconnected)
-				return -EIO;
-
-			cnow = tport->tp_icount;
-			if (cnow.rng == cprev.rng && cnow.dsr == cprev.dsr &&
-			    cnow.dcd == cprev.dcd && cnow.cts == cprev.cts)
-				return -EIO; /* no change => error */
-			if (((arg & TIOCM_RNG) && (cnow.rng != cprev.rng)) ||
-			    ((arg & TIOCM_DSR) && (cnow.dsr != cprev.dsr)) ||
-			    ((arg & TIOCM_CD)  && (cnow.dcd != cprev.dcd)) ||
-			    ((arg & TIOCM_CTS) && (cnow.cts != cprev.cts)))
-				return 0;
-			cprev = cnow;
-		}
-		break;
 	}
 	return -ENOIOCTLCMD;
 }
@@ -1018,8 +973,6 @@ static void ti_break(struct tty_struct *tty, int break_state)
 	if (tport == NULL)
 		return;
 
-	ti_drain(tport, (tport->tp_closing_wait*HZ)/100, 0);
-
 	status = ti_write_byte(port, tport->tp_tdev,
 		tport->tp_uart_base_addr + TI_UART_OFFSET_LCR,
 		TI_LCR_BREAK, break_state == -1 ? TI_LCR_BREAK : 0);
@@ -1156,7 +1109,7 @@ static void ti_bulk_in_callback(struct urb *urb)
 		else
 			ti_recv(port, urb->transfer_buffer, urb->actual_length);
 		spin_lock(&tport->tp_lock);
-		tport->tp_icount.rx += urb->actual_length;
+		port->icount.rx += urb->actual_length;
 		spin_unlock(&tport->tp_lock);
 	}
 
@@ -1229,7 +1182,6 @@ static void ti_send(struct ti_port *tport)
 {
 	int count, result;
 	struct usb_serial_port *port = tport->tp_port;
-	struct tty_struct *tty = tty_port_tty_get(&port->port);	/* FIXME */
 	unsigned long flags;
 
 	spin_lock_irqsave(&tport->tp_lock, flags);
@@ -1265,19 +1217,17 @@ static void ti_send(struct ti_port *tport)
 		/* TODO: reschedule ti_send */
 	} else {
 		spin_lock_irqsave(&tport->tp_lock, flags);
-		tport->tp_icount.tx += count;
+		port->icount.tx += count;
 		spin_unlock_irqrestore(&tport->tp_lock, flags);
 	}
 
 	/* more room in the buffer for new writes, wakeup */
-	if (tty)
-		tty_wakeup(tty);
-	tty_kref_put(tty);
+	tty_port_tty_wakeup(&port->port);
+
 	wake_up_interruptible(&tport->tp_write_wait);
 	return;
 unlock:
 	spin_unlock_irqrestore(&tport->tp_lock, flags);
-	tty_kref_put(tty);
 	return;
 }
 
@@ -1300,7 +1250,7 @@ static int ti_set_mcr(struct ti_port *tport, unsigned int mcr)
 }
 
 
-static int ti_get_lsr(struct ti_port *tport)
+static int ti_get_lsr(struct ti_port *tport, u8 *lsr)
 {
 	int size, status;
 	struct ti_device *tdev = tport->tp_tdev;
@@ -1326,7 +1276,7 @@ static int ti_get_lsr(struct ti_port *tport)
 
 	dev_dbg(&port->dev, "%s - lsr 0x%02X\n", __func__, data->bLSR);
 
-	tport->tp_lsr = data->bLSR;
+	*lsr = data->bLSR;
 
 free_data:
 	kfree(data);
@@ -1339,10 +1289,15 @@ static int ti_get_serial_info(struct ti_port *tport,
 {
 	struct usb_serial_port *port = tport->tp_port;
 	struct serial_struct ret_serial;
+	unsigned cwait;
 
 	if (!ret_arg)
 		return -EFAULT;
 
+	cwait = port->port.closing_wait;
+	if (cwait != ASYNC_CLOSING_WAIT_NONE)
+		cwait = jiffies_to_msecs(cwait) / 10;
+
 	memset(&ret_serial, 0, sizeof(ret_serial));
 
 	ret_serial.type = PORT_16550A;
@@ -1351,7 +1306,7 @@ static int ti_get_serial_info(struct ti_port *tport,
 	ret_serial.flags = tport->tp_flags;
 	ret_serial.xmit_fifo_size = TI_WRITE_BUF_SIZE;
 	ret_serial.baud_base = tport->tp_tdev->td_is_3410 ? 921600 : 460800;
-	ret_serial.closing_wait = tport->tp_closing_wait;
+	ret_serial.closing_wait = cwait;
 
 	if (copy_to_user(ret_arg, &ret_serial, sizeof(*ret_arg)))
 		return -EFAULT;
@@ -1364,12 +1319,17 @@ static int ti_set_serial_info(struct tty_struct *tty, struct ti_port *tport,
 	struct serial_struct __user *new_arg)
 {
 	struct serial_struct new_serial;
+	unsigned cwait;
 
 	if (copy_from_user(&new_serial, new_arg, sizeof(new_serial)))
 		return -EFAULT;
 
+	cwait = new_serial.closing_wait;
+	if (cwait != ASYNC_CLOSING_WAIT_NONE)
+		cwait = msecs_to_jiffies(10 * new_serial.closing_wait);
+
 	tport->tp_flags = new_serial.flags & TI_SET_SERIAL_FLAGS;
-	tport->tp_closing_wait = new_serial.closing_wait;
+	tport->tp_port->port.closing_wait = cwait;
 
 	return 0;
 }
@@ -1385,7 +1345,7 @@ static void ti_handle_new_msr(struct ti_port *tport, __u8 msr)
 
 	if (msr & TI_MSR_DELTA_MASK) {
 		spin_lock_irqsave(&tport->tp_lock, flags);
-		icount = &tport->tp_icount;
+		icount = &tport->tp_port->icount;
 		if (msr & TI_MSR_DELTA_CTS)
 			icount->cts++;
 		if (msr & TI_MSR_DELTA_DSR)
@@ -1394,7 +1354,7 @@ static void ti_handle_new_msr(struct ti_port *tport, __u8 msr)
 			icount->dcd++;
 		if (msr & TI_MSR_DELTA_RI)
 			icount->rng++;
-		wake_up_interruptible(&tport->tp_port->delta_msr_wait);
+		wake_up_interruptible(&tport->tp_port->port.delta_msr_wait);
 		spin_unlock_irqrestore(&tport->tp_lock, flags);
 	}
 
@@ -1414,56 +1374,6 @@ static void ti_handle_new_msr(struct ti_port *tport, __u8 msr)
 }
 
 
-static void ti_drain(struct ti_port *tport, unsigned long timeout, int flush)
-{
-	struct ti_device *tdev = tport->tp_tdev;
-	struct usb_serial_port *port = tport->tp_port;
-	wait_queue_t wait;
-
-	spin_lock_irq(&tport->tp_lock);
-
-	/* wait for data to drain from the buffer */
-	tdev->td_urb_error = 0;
-	init_waitqueue_entry(&wait, current);
-	add_wait_queue(&tport->tp_write_wait, &wait);
-	for (;;) {
-		set_current_state(TASK_INTERRUPTIBLE);
-		if (kfifo_len(&tport->write_fifo) == 0
-		|| timeout == 0 || signal_pending(current)
-		|| tdev->td_urb_error
-		|| port->serial->disconnected)  /* disconnect */
-			break;
-		spin_unlock_irq(&tport->tp_lock);
-		timeout = schedule_timeout(timeout);
-		spin_lock_irq(&tport->tp_lock);
-	}
-	set_current_state(TASK_RUNNING);
-	remove_wait_queue(&tport->tp_write_wait, &wait);
-
-	/* flush any remaining data in the buffer */
-	if (flush)
-		kfifo_reset_out(&tport->write_fifo);
-
-	spin_unlock_irq(&tport->tp_lock);
-
-	mutex_lock(&port->serial->disc_mutex);
-	/* wait for data to drain from the device */
-	/* wait for empty tx register, plus 20 ms */
-	timeout += jiffies;
-	tport->tp_lsr &= ~TI_LSR_TX_EMPTY;
-	while ((long)(jiffies - timeout) < 0 && !signal_pending(current)
-	&& !(tport->tp_lsr&TI_LSR_TX_EMPTY) && !tdev->td_urb_error
-	&& !port->serial->disconnected) {
-		if (ti_get_lsr(tport))
-			break;
-		mutex_unlock(&port->serial->disc_mutex);
-		msleep_interruptible(20);
-		mutex_lock(&port->serial->disc_mutex);
-	}
-	mutex_unlock(&port->serial->disc_mutex);
-}
-
-
 static void ti_stop_read(struct ti_port *tport, struct tty_struct *tty)
 {
 	unsigned long flags;
diff --git a/drivers/usb/serial/usb-serial.c b/drivers/usb/serial/usb-serial.c
index 2e70efa08b7..cf75beb1251 100644
--- a/drivers/usb/serial/usb-serial.c
+++ b/drivers/usb/serial/usb-serial.c
@@ -1,6 +1,7 @@
 /*
  * USB Serial Converter driver
  *
+ * Copyright (C) 2009 - 2013 Johan Hovold (jhovold@gmail.com)
  * Copyright (C) 1999 - 2012 Greg Kroah-Hartman (greg@kroah.com)
  * Copyright (C) 2000 Peter Berger (pberger@brimson.com)
  * Copyright (C) 2000 Al Borchers (borchers@steinerpoint.com)
@@ -14,7 +15,6 @@
  *
  * See Documentation/usb/usb-serial.txt for more information on using this
  * driver
- *
  */
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
@@ -49,7 +49,6 @@
    drivers depend on it.
 */
 
-/* initially all NULL */
 static struct usb_serial *serial_table[SERIAL_TTY_MINORS];
 static DEFINE_MUTEX(table_lock);
 static LIST_HEAD(usb_serial_driver_list);
@@ -139,7 +138,7 @@ static void destroy_serial(struct kref *kref)
 	if (serial->minor != SERIAL_TTY_NO_MINOR)
 		return_serial(serial);
 
-	if (serial->attached)
+	if (serial->attached && serial->type->release)
 		serial->type->release(serial);
 
 	/* Now that nothing is using the ports, they can be freed */
@@ -225,7 +224,7 @@ static int serial_install(struct tty_driver *driver, struct tty_struct *tty)
 	return retval;
 }
 
-static int serial_activate(struct tty_port *tport, struct tty_struct *tty)
+static int serial_port_activate(struct tty_port *tport, struct tty_struct *tty)
 {
 	struct usb_serial_port *port =
 		container_of(tport, struct usb_serial_port, port);
@@ -249,30 +248,27 @@ static int serial_open(struct tty_struct *tty, struct file *filp)
 {
 	struct usb_serial_port *port = tty->driver_data;
 
-	dev_dbg(tty->dev, "%s - port %d\n", __func__, port->number);
+	dev_dbg(tty->dev, "%s\n", __func__);
+
 	return tty_port_open(&port->port, tty, filp);
 }
 
 /**
- * serial_down - shut down hardware
+ * serial_port_shutdown - shut down hardware
  * @tport: tty port to shut down
  *
- * Shut down a USB serial port unless it is the console.  We never
- * shut down the console hardware as it will always be in use. Serialized
- * against activate by the tport mutex and kept to matching open/close pairs
+ * Shut down a USB serial port. Serialized against activate by the
+ * tport mutex and kept to matching open/close pairs
  * of calls by the ASYNCB_INITIALIZED flag.
+ *
+ * Not called if tty is console.
  */
-static void serial_down(struct tty_port *tport)
+static void serial_port_shutdown(struct tty_port *tport)
 {
 	struct usb_serial_port *port =
 		container_of(tport, struct usb_serial_port, port);
 	struct usb_serial_driver *drv = port->serial->type;
-	/*
-	 * The console is magical.  Do not hang up the console hardware
-	 * or there will be tears.
-	 */
-	if (port->port.console)
-		return;
+
 	if (drv->close)
 		drv->close(port);
 }
@@ -281,7 +277,8 @@ static void serial_hangup(struct tty_struct *tty)
 {
 	struct usb_serial_port *port = tty->driver_data;
 
-	dev_dbg(tty->dev, "%s - port %d\n", __func__, port->number);
+	dev_dbg(tty->dev, "%s\n", __func__);
+
 	tty_port_hangup(&port->port);
 }
 
@@ -289,7 +286,8 @@ static void serial_close(struct tty_struct *tty, struct file *filp)
 {
 	struct usb_serial_port *port = tty->driver_data;
 
-	dev_dbg(tty->dev, "%s - port %d\n", __func__, port->number);
+	dev_dbg(tty->dev, "%s\n", __func__);
+
 	tty_port_close(&port->port, tty, filp);
 }
 
@@ -308,14 +306,14 @@ static void serial_cleanup(struct tty_struct *tty)
 	struct usb_serial *serial;
 	struct module *owner;
 
+	dev_dbg(tty->dev, "%s\n", __func__);
+
 	/* The console is magical.  Do not hang up the console hardware
 	 * or there will be tears.
 	 */
 	if (port->port.console)
 		return;
 
-	dev_dbg(tty->dev, "%s - port %d\n", __func__, port->number);
-
 	tty->driver_data = NULL;
 
 	serial = port->serial;
@@ -339,10 +337,8 @@ static int serial_write(struct tty_struct *tty, const unsigned char *buf,
 	if (port->serial->dev->state == USB_STATE_NOTATTACHED)
 		goto exit;
 
-	dev_dbg(tty->dev, "%s - port %d, %d byte(s)\n", __func__,
-		port->number, count);
+	dev_dbg(tty->dev, "%s - %d byte(s)\n", __func__, count);
 
-	/* pass on to the driver specific version of this function */
 	retval = port->serial->type->write(tty, port, buf, count);
 	if (retval < 0)
 		retval = usb_translate_errors(retval);
@@ -354,8 +350,8 @@ static int serial_write_room(struct tty_struct *tty)
 {
 	struct usb_serial_port *port = tty->driver_data;
 
-	dev_dbg(tty->dev, "%s - port %d\n", __func__, port->number);
-	/* pass on to the driver specific version of this function */
+	dev_dbg(tty->dev, "%s\n", __func__);
+
 	return port->serial->type->write_room(tty);
 }
 
@@ -365,7 +361,7 @@ static int serial_chars_in_buffer(struct tty_struct *tty)
 	struct usb_serial *serial = port->serial;
 	int count = 0;
 
-	dev_dbg(tty->dev, "%s - port %d\n", __func__, port->number);
+	dev_dbg(tty->dev, "%s\n", __func__);
 
 	mutex_lock(&serial->disc_mutex);
 	/* if the device was unplugged then any remaining characters
@@ -383,9 +379,8 @@ static void serial_throttle(struct tty_struct *tty)
 {
 	struct usb_serial_port *port = tty->driver_data;
 
-	dev_dbg(tty->dev, "%s - port %d\n", __func__, port->number);
+	dev_dbg(tty->dev, "%s\n", __func__);
 
-	/* pass on to the driver specific version of this function */
 	if (port->serial->type->throttle)
 		port->serial->type->throttle(tty);
 }
@@ -394,9 +389,8 @@ static void serial_unthrottle(struct tty_struct *tty)
 {
 	struct usb_serial_port *port = tty->driver_data;
 
-	dev_dbg(tty->dev, "%s - port %d\n", __func__, port->number);
+	dev_dbg(tty->dev, "%s\n", __func__);
 
-	/* pass on to the driver specific version of this function */
 	if (port->serial->type->unthrottle)
 		port->serial->type->unthrottle(tty);
 }
@@ -407,15 +401,20 @@ static int serial_ioctl(struct tty_struct *tty,
 	struct usb_serial_port *port = tty->driver_data;
 	int retval = -ENODEV;
 
-	dev_dbg(tty->dev, "%s - port %d, cmd 0x%.4x\n", __func__,
-		port->number, cmd);
+	dev_dbg(tty->dev, "%s - cmd 0x%.4x\n", __func__, cmd);
+
+	switch (cmd) {
+	case TIOCMIWAIT:
+		if (port->serial->type->tiocmiwait)
+			retval = port->serial->type->tiocmiwait(tty, arg);
+		break;
+	default:
+		if (port->serial->type->ioctl)
+			retval = port->serial->type->ioctl(tty, cmd, arg);
+		else
+			retval = -ENOIOCTLCMD;
+	}
 
-	/* pass on to the driver specific version of this function
-	   if it is available */
-	if (port->serial->type->ioctl) {
-		retval = port->serial->type->ioctl(tty, cmd, arg);
-	} else
-		retval = -ENOIOCTLCMD;
 	return retval;
 }
 
@@ -423,10 +422,8 @@ static void serial_set_termios(struct tty_struct *tty, struct ktermios *old)
 {
 	struct usb_serial_port *port = tty->driver_data;
 
-	dev_dbg(tty->dev, "%s - port %d\n", __func__, port->number);
+	dev_dbg(tty->dev, "%s\n", __func__);
 
-	/* pass on to the driver specific version of this function
-	   if it is available */
 	if (port->serial->type->set_termios)
 		port->serial->type->set_termios(tty, port, old);
 	else
@@ -437,12 +434,11 @@ static int serial_break(struct tty_struct *tty, int break_state)
 {
 	struct usb_serial_port *port = tty->driver_data;
 
-	dev_dbg(tty->dev, "%s - port %d\n", __func__, port->number);
+	dev_dbg(tty->dev, "%s\n", __func__);
 
-	/* pass on to the driver specific version of this function
-	   if it is available */
 	if (port->serial->type->break_ctl)
 		port->serial->type->break_ctl(tty, break_state);
+
 	return 0;
 }
 
@@ -496,7 +492,7 @@ static int serial_tiocmget(struct tty_struct *tty)
 {
 	struct usb_serial_port *port = tty->driver_data;
 
-	dev_dbg(tty->dev, "%s - port %d\n", __func__, port->number);
+	dev_dbg(tty->dev, "%s\n", __func__);
 
 	if (port->serial->type->tiocmget)
 		return port->serial->type->tiocmget(tty);
@@ -508,7 +504,7 @@ static int serial_tiocmset(struct tty_struct *tty,
 {
 	struct usb_serial_port *port = tty->driver_data;
 
-	dev_dbg(tty->dev, "%s - port %d\n", __func__, port->number);
+	dev_dbg(tty->dev, "%s\n", __func__);
 
 	if (port->serial->type->tiocmset)
 		return port->serial->type->tiocmset(tty, set, clear);
@@ -520,7 +516,7 @@ static int serial_get_icount(struct tty_struct *tty,
 {
 	struct usb_serial_port *port = tty->driver_data;
 
-	dev_dbg(tty->dev, "%s - port %d\n", __func__, port->number);
+	dev_dbg(tty->dev, "%s\n", __func__);
 
 	if (port->serial->type->get_icount)
 		return port->serial->type->get_icount(tty, icount);
@@ -542,55 +538,43 @@ static void usb_serial_port_work(struct work_struct *work)
 {
 	struct usb_serial_port *port =
 		container_of(work, struct usb_serial_port, work);
-	struct tty_struct *tty;
 
-	tty = tty_port_tty_get(&port->port);
-	if (!tty)
-		return;
+	tty_port_tty_wakeup(&port->port);
+}
+
+static void usb_serial_port_poison_urbs(struct usb_serial_port *port)
+{
+	int i;
 
-	dev_dbg(tty->dev, "%s - port %d\n", __func__, port->number);
+	for (i = 0; i < ARRAY_SIZE(port->read_urbs); ++i)
+		usb_poison_urb(port->read_urbs[i]);
+	for (i = 0; i < ARRAY_SIZE(port->write_urbs); ++i)
+		usb_poison_urb(port->write_urbs[i]);
 
-	tty_wakeup(tty);
-	tty_kref_put(tty);
+	usb_poison_urb(port->interrupt_in_urb);
+	usb_poison_urb(port->interrupt_out_urb);
 }
 
-static void kill_traffic(struct usb_serial_port *port)
+static void usb_serial_port_unpoison_urbs(struct usb_serial_port *port)
 {
 	int i;
 
 	for (i = 0; i < ARRAY_SIZE(port->read_urbs); ++i)
-		usb_kill_urb(port->read_urbs[i]);
+		usb_unpoison_urb(port->read_urbs[i]);
 	for (i = 0; i < ARRAY_SIZE(port->write_urbs); ++i)
-		usb_kill_urb(port->write_urbs[i]);
-	/*
-	 * This is tricky.
-	 * Some drivers submit the read_urb in the
-	 * handler for the write_urb or vice versa
-	 * this order determines the order in which
-	 * usb_kill_urb() must be used to reliably
-	 * kill the URBs. As it is unknown here,
-	 * both orders must be used in turn.
-	 * The call below is not redundant.
-	 */
-	usb_kill_urb(port->read_urb);
-	usb_kill_urb(port->interrupt_in_urb);
-	usb_kill_urb(port->interrupt_out_urb);
+		usb_unpoison_urb(port->write_urbs[i]);
+
+	usb_unpoison_urb(port->interrupt_in_urb);
+	usb_unpoison_urb(port->interrupt_out_urb);
 }
 
-static void port_release(struct device *dev)
+static void usb_serial_port_release(struct device *dev)
 {
 	struct usb_serial_port *port = to_usb_serial_port(dev);
 	int i;
 
 	dev_dbg(dev, "%s\n", __func__);
 
-	/*
-	 * Stop all the traffic before cancelling the work, so that
-	 * nobody will restart it by calling usb_serial_port_softint.
-	 */
-	kill_traffic(port);
-	cancel_work_sync(&port->work);
-
 	usb_free_urb(port->interrupt_in_urb);
 	usb_free_urb(port->interrupt_out_urb);
 	for (i = 0; i < ARRAY_SIZE(port->read_urbs); ++i) {
@@ -615,10 +599,8 @@ static struct usb_serial *create_serial(struct usb_device *dev,
 	struct usb_serial *serial;
 
 	serial = kzalloc(sizeof(*serial), GFP_KERNEL);
-	if (!serial) {
-		dev_err(&dev->dev, "%s - out of memory\n", __func__);
+	if (!serial)
 		return NULL;
-	}
 	serial->dev = usb_get_dev(dev);
 	serial->type = driver;
 	serial->interface = usb_get_intf(interface);
@@ -681,7 +663,7 @@ static struct usb_serial_driver *search_serial_device(
 	return NULL;
 }
 
-static int serial_carrier_raised(struct tty_port *port)
+static int serial_port_carrier_raised(struct tty_port *port)
 {
 	struct usb_serial_port *p = container_of(port, struct usb_serial_port, port);
 	struct usb_serial_driver *drv = p->serial->type;
@@ -692,7 +674,7 @@ static int serial_carrier_raised(struct tty_port *port)
 	return 1;
 }
 
-static void serial_dtr_rts(struct tty_port *port, int on)
+static void serial_port_dtr_rts(struct tty_port *port, int on)
 {
 	struct usb_serial_port *p = container_of(port, struct usb_serial_port, port);
 	struct usb_serial *serial = p->serial;
@@ -712,10 +694,10 @@ static void serial_dtr_rts(struct tty_port *port, int on)
 }
 
 static const struct tty_port_operations serial_port_ops = {
-	.carrier_raised = serial_carrier_raised,
-	.dtr_rts = serial_dtr_rts,
-	.activate = serial_activate,
-	.shutdown = serial_down,
+	.carrier_raised		= serial_port_carrier_raised,
+	.dtr_rts		= serial_port_dtr_rts,
+	.activate		= serial_port_activate,
+	.shutdown		= serial_port_shutdown,
 };
 
 static int usb_serial_probe(struct usb_interface *interface,
@@ -762,7 +744,6 @@ static int usb_serial_probe(struct usb_interface *interface,
 	serial = create_serial(dev, interface, type);
 	if (!serial) {
 		module_put(type->driver.owner);
-		dev_err(ddev, "%s - out of memory\n", __func__);
 		return -ENOMEM;
 	}
 
@@ -910,7 +891,7 @@ static int usb_serial_probe(struct usb_interface *interface,
 		port->dev.parent = &interface->dev;
 		port->dev.driver = NULL;
 		port->dev.bus = &usb_serial_bus_type;
-		port->dev.release = &port_release;
+		port->dev.release = &usb_serial_port_release;
 		device_initialize(&port->dev);
 	}
 
@@ -926,16 +907,12 @@ static int usb_serial_probe(struct usb_interface *interface,
 		for (j = 0; j < ARRAY_SIZE(port->read_urbs); ++j) {
 			set_bit(j, &port->read_urbs_free);
 			port->read_urbs[j] = usb_alloc_urb(0, GFP_KERNEL);
-			if (!port->read_urbs[j]) {
-				dev_err(ddev, "No free urbs available\n");
+			if (!port->read_urbs[j])
 				goto probe_error;
-			}
 			port->bulk_in_buffers[j] = kmalloc(buffer_size,
 								GFP_KERNEL);
-			if (!port->bulk_in_buffers[j]) {
-				dev_err(ddev, "Couldn't allocate bulk_in_buffer\n");
+			if (!port->bulk_in_buffers[j])
 				goto probe_error;
-			}
 			usb_fill_bulk_urb(port->read_urbs[j], dev,
 					usb_rcvbulkpipe(dev,
 						endpoint->bEndpointAddress),
@@ -962,16 +939,12 @@ static int usb_serial_probe(struct usb_interface *interface,
 		for (j = 0; j < ARRAY_SIZE(port->write_urbs); ++j) {
 			set_bit(j, &port->write_urbs_free);
 			port->write_urbs[j] = usb_alloc_urb(0, GFP_KERNEL);
-			if (!port->write_urbs[j]) {
-				dev_err(ddev, "No free urbs available\n");
+			if (!port->write_urbs[j])
 				goto probe_error;
-			}
 			port->bulk_out_buffers[j] = kmalloc(buffer_size,
 								GFP_KERNEL);
-			if (!port->bulk_out_buffers[j]) {
-				dev_err(ddev, "Couldn't allocate bulk_out_buffer\n");
+			if (!port->bulk_out_buffers[j])
 				goto probe_error;
-			}
 			usb_fill_bulk_urb(port->write_urbs[j], dev,
 					usb_sndbulkpipe(dev,
 						endpoint->bEndpointAddress),
@@ -989,19 +962,15 @@ static int usb_serial_probe(struct usb_interface *interface,
 			endpoint = interrupt_in_endpoint[i];
 			port = serial->port[i];
 			port->interrupt_in_urb = usb_alloc_urb(0, GFP_KERNEL);
-			if (!port->interrupt_in_urb) {
-				dev_err(ddev, "No free urbs available\n");
+			if (!port->interrupt_in_urb)
 				goto probe_error;
-			}
 			buffer_size = usb_endpoint_maxp(endpoint);
 			port->interrupt_in_endpointAddress =
 						endpoint->bEndpointAddress;
 			port->interrupt_in_buffer = kmalloc(buffer_size,
 								GFP_KERNEL);
-			if (!port->interrupt_in_buffer) {
-				dev_err(ddev, "Couldn't allocate interrupt_in_buffer\n");
+			if (!port->interrupt_in_buffer)
 				goto probe_error;
-			}
 			usb_fill_int_urb(port->interrupt_in_urb, dev,
 				usb_rcvintpipe(dev,
 						endpoint->bEndpointAddress),
@@ -1018,20 +987,16 @@ static int usb_serial_probe(struct usb_interface *interface,
 			endpoint = interrupt_out_endpoint[i];
 			port = serial->port[i];
 			port->interrupt_out_urb = usb_alloc_urb(0, GFP_KERNEL);
-			if (!port->interrupt_out_urb) {
-				dev_err(ddev, "No free urbs available\n");
+			if (!port->interrupt_out_urb)
 				goto probe_error;
-			}
 			buffer_size = usb_endpoint_maxp(endpoint);
 			port->interrupt_out_size = buffer_size;
 			port->interrupt_out_endpointAddress =
 						endpoint->bEndpointAddress;
 			port->interrupt_out_buffer = kmalloc(buffer_size,
 								GFP_KERNEL);
-			if (!port->interrupt_out_buffer) {
-				dev_err(ddev, "Couldn't allocate interrupt_out_buffer\n");
+			if (!port->interrupt_out_buffer)
 				goto probe_error;
-			}
 			usb_fill_int_urb(port->interrupt_out_urb, dev,
 				usb_sndintpipe(dev,
 						  endpoint->bEndpointAddress),
@@ -1120,13 +1085,15 @@ static void usb_serial_disconnect(struct usb_interface *interface)
 				tty_vhangup(tty);
 				tty_kref_put(tty);
 			}
-			kill_traffic(port);
+			usb_serial_port_poison_urbs(port);
+			wake_up_interruptible(&port->port.delta_msr_wait);
 			cancel_work_sync(&port->work);
 			if (device_is_registered(&port->dev))
 				device_del(&port->dev);
 		}
 	}
-	serial->type->disconnect(serial);
+	if (serial->type->disconnect)
+		serial->type->disconnect(serial);
 
 	/* let the last holder of this object cause it to be cleaned up */
 	usb_serial_put(serial);
@@ -1141,6 +1108,11 @@ int usb_serial_suspend(struct usb_interface *intf, pm_message_t message)
 
 	serial->suspending = 1;
 
+	/*
+	 * serial->type->suspend() MUST return 0 in system sleep context,
+	 * otherwise, the resume callback has to recover device from
+	 * previous suspend failure.
+	 */
 	if (serial->type->suspend) {
 		r = serial->type->suspend(serial, message);
 		if (r < 0) {
@@ -1152,7 +1124,7 @@ int usb_serial_suspend(struct usb_interface *intf, pm_message_t message)
 	for (i = 0; i < serial->num_ports; ++i) {
 		port = serial->port[i];
 		if (port)
-			kill_traffic(port);
+			usb_serial_port_poison_urbs(port);
 	}
 
 err_out:
@@ -1160,11 +1132,25 @@ err_out:
 }
 EXPORT_SYMBOL(usb_serial_suspend);
 
+static void usb_serial_unpoison_port_urbs(struct usb_serial *serial)
+{
+	struct usb_serial_port *port;
+	int i;
+
+	for (i = 0; i < serial->num_ports; ++i) {
+		port = serial->port[i];
+		if (port)
+			usb_serial_port_unpoison_urbs(port);
+	}
+}
+
 int usb_serial_resume(struct usb_interface *intf)
 {
 	struct usb_serial *serial = usb_get_intfdata(intf);
 	int rv;
 
+	usb_serial_unpoison_port_urbs(serial);
+
 	serial->suspending = 0;
 	if (serial->type->resume)
 		rv = serial->type->resume(serial);
@@ -1180,6 +1166,8 @@ static int usb_serial_reset_resume(struct usb_interface *intf)
 	struct usb_serial *serial = usb_get_intfdata(intf);
 	int rv;
 
+	usb_serial_unpoison_port_urbs(serial);
+
 	serial->suspending = 0;
 	if (serial->type->reset_resume)
 		rv = serial->type->reset_resume(serial);
@@ -1316,12 +1304,12 @@ module_exit(usb_serial_exit);
 	do {								\
 		if (!type->function) {					\
 			type->function = usb_serial_generic_##function;	\
-			pr_debug("Had to override the " #function	\
-				" usb serial operation with the generic one.");\
-			}						\
+			pr_debug("%s: using generic " #function	"\n",	\
+						type->driver.name);	\
+		}							\
 	} while (0)
 
-static void fixup_generic(struct usb_serial_driver *device)
+static void usb_serial_operations_init(struct usb_serial_driver *device)
 {
 	set_to_generic_if_null(device, open);
 	set_to_generic_if_null(device, write);
@@ -1330,8 +1318,6 @@ static void fixup_generic(struct usb_serial_driver *device)
 	set_to_generic_if_null(device, chars_in_buffer);
 	set_to_generic_if_null(device, read_bulk_callback);
 	set_to_generic_if_null(device, write_bulk_callback);
-	set_to_generic_if_null(device, disconnect);
-	set_to_generic_if_null(device, release);
 	set_to_generic_if_null(device, process_read_urb);
 	set_to_generic_if_null(device, prepare_write_buffer);
 }
@@ -1343,8 +1329,6 @@ static int usb_serial_register(struct usb_serial_driver *driver)
 	if (usb_disabled())
 		return -ENODEV;
 
-	fixup_generic(driver);
-
 	if (!driver->description)
 		driver->description = driver->driver.name;
 	if (!driver->usb_driver) {
@@ -1353,6 +1337,8 @@ static int usb_serial_register(struct usb_serial_driver *driver)
 		return -EINVAL;
 	}
 
+	usb_serial_operations_init(driver);
+
 	/* Add this device to our list of devices */
 	mutex_lock(&table_lock);
 	list_add(&driver->driver_list, &usb_serial_driver_list);
@@ -1470,7 +1456,6 @@ void usb_serial_deregister_drivers(struct usb_serial_driver *const serial_driver
 }
 EXPORT_SYMBOL_GPL(usb_serial_deregister_drivers);
 
-/* Module information */
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL");
diff --git a/drivers/usb/serial/usb_wwan.c b/drivers/usb/serial/usb_wwan.c
index 571965aa1cc..ece326ef63a 100644
--- a/drivers/usb/serial/usb_wwan.c
+++ b/drivers/usb/serial/usb_wwan.c
@@ -421,20 +421,19 @@ void usb_wwan_close(struct usb_serial_port *port)
 
 	portdata = usb_get_serial_port_data(port);
 
-	if (serial->dev) {
-		/* Stop reading/writing urbs */
-		spin_lock_irq(&intfdata->susp_lock);
-		portdata->opened = 0;
-		spin_unlock_irq(&intfdata->susp_lock);
+	/* Stop reading/writing urbs */
+	spin_lock_irq(&intfdata->susp_lock);
+	portdata->opened = 0;
+	spin_unlock_irq(&intfdata->susp_lock);
 
-		for (i = 0; i < N_IN_URB; i++)
-			usb_kill_urb(portdata->in_urbs[i]);
-		for (i = 0; i < N_OUT_URB; i++)
-			usb_kill_urb(portdata->out_urbs[i]);
-		/* balancing - important as an error cannot be handled*/
-		usb_autopm_get_interface_no_resume(serial->interface);
-		serial->interface->needs_remote_wakeup = 0;
-	}
+	for (i = 0; i < N_IN_URB; i++)
+		usb_kill_urb(portdata->in_urbs[i]);
+	for (i = 0; i < N_OUT_URB; i++)
+		usb_kill_urb(portdata->out_urbs[i]);
+
+	/* balancing - important as an error cannot be handled*/
+	usb_autopm_get_interface_no_resume(serial->interface);
+	serial->interface->needs_remote_wakeup = 0;
 }
 EXPORT_SYMBOL(usb_wwan_close);
 
diff --git a/drivers/usb/serial/visor.c b/drivers/usb/serial/visor.c
index 1129aa73c23..7573ec8a084 100644
--- a/drivers/usb/serial/visor.c
+++ b/drivers/usb/serial/visor.c
@@ -257,24 +257,18 @@ static void visor_close(struct usb_serial_port *port)
 {
 	unsigned char *transfer_buffer;
 
-	/* shutdown our urbs */
 	usb_serial_generic_close(port);
 	usb_kill_urb(port->interrupt_in_urb);
 
-	mutex_lock(&port->serial->disc_mutex);
-	if (!port->serial->disconnected) {
-		/* Try to send shutdown message, unless the device is gone */
-		transfer_buffer =  kmalloc(0x12, GFP_KERNEL);
-		if (transfer_buffer) {
-			usb_control_msg(port->serial->dev,
+	transfer_buffer = kmalloc(0x12, GFP_KERNEL);
+	if (!transfer_buffer)
+		return;
+	usb_control_msg(port->serial->dev,
 					 usb_rcvctrlpipe(port->serial->dev, 0),
 					 VISOR_CLOSE_NOTIFICATION, 0xc2,
 					 0x0000, 0x0000,
 					 transfer_buffer, 0x12, 300);
-			kfree(transfer_buffer);
-		}
-	}
-	mutex_unlock(&port->serial->disc_mutex);
+	kfree(transfer_buffer);
 }
 
 static void visor_read_int_callback(struct urb *urb)
diff --git a/drivers/usb/serial/wishbone-serial.c b/drivers/usb/serial/wishbone-serial.c
new file mode 100644
index 00000000000..100573c6f19
--- /dev/null
+++ b/drivers/usb/serial/wishbone-serial.c
@@ -0,0 +1,95 @@
+/*
+ * USB Wishbone-Serial adapter driver
+ *
+ * Copyright (C) 2013 Wesley W. Terpstra <w.terpstra@gsi.de>
+ * Copyright (C) 2013 GSI Helmholtz Centre for Heavy Ion Research GmbH
+ *
+ * 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/kernel.h>
+#include <linux/init.h>
+#include <linux/tty.h>
+#include <linux/module.h>
+#include <linux/usb.h>
+#include <linux/usb/serial.h>
+#include <linux/uaccess.h>
+
+#define GSI_VENDOR_OPENCLOSE 0xB0
+
+static const struct usb_device_id id_table[] = {
+	{ USB_DEVICE_AND_INTERFACE_INFO(0x1D50, 0x6062, 0xFF, 0xFF, 0xFF) },
+	{ },
+};
+MODULE_DEVICE_TABLE(usb, id_table);
+
+/*
+ * Etherbone must be told that a new stream has begun before data arrives.
+ * This is necessary to restart the negotiation of Wishbone bus parameters.
+ * Similarly, when the stream ends, Etherbone must be told so that the cycle
+ * line can be driven low in the case that userspace failed to do so.
+ */
+static int usb_gsi_openclose(struct usb_serial_port *port, int value)
+{
+	struct usb_device *dev = port->serial->dev;
+
+	return usb_control_msg(
+		dev,
+		usb_sndctrlpipe(dev, 0), /* Send to EP0OUT */
+		GSI_VENDOR_OPENCLOSE,
+		USB_DIR_OUT|USB_TYPE_VENDOR|USB_RECIP_INTERFACE,
+		value, /* wValue = device is open(1) or closed(0) */
+		port->serial->interface->cur_altsetting->desc.bInterfaceNumber,
+		NULL, 0,  /* There is no data stage */
+		5000); /* Timeout till operation fails */
+}
+
+static int wishbone_serial_open(struct tty_struct *tty,
+				struct usb_serial_port *port)
+{
+	int retval;
+
+	retval = usb_gsi_openclose(port, 1);
+	if (retval) {
+		dev_err(&port->serial->dev->dev,
+		       "Could not mark device as open (%d)\n",
+		       retval);
+		return retval;
+	}
+
+	retval = usb_serial_generic_open(tty, port);
+	if (retval)
+		usb_gsi_openclose(port, 0);
+
+	return retval;
+}
+
+static void wishbone_serial_close(struct usb_serial_port *port)
+{
+	usb_serial_generic_close(port);
+	usb_gsi_openclose(port, 0);
+}
+
+static struct usb_serial_driver wishbone_serial_device = {
+	.driver = {
+		.owner =	THIS_MODULE,
+		.name =		"wishbone_serial",
+	},
+	.id_table =		id_table,
+	.num_ports =		1,
+	.open =			&wishbone_serial_open,
+	.close =		&wishbone_serial_close,
+};
+
+static struct usb_serial_driver * const serial_drivers[] = {
+	&wishbone_serial_device, NULL
+};
+
+module_usb_serial_driver(serial_drivers, id_table);
+
+MODULE_AUTHOR("Wesley W. Terpstra <w.terpstra@gsi.de>");
+MODULE_DESCRIPTION("USB Wishbone-Serial adapter");
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/storage/Kconfig b/drivers/usb/storage/Kconfig
index eab04a6b5fb..8470e1b114f 100644
--- a/drivers/usb/storage/Kconfig
+++ b/drivers/usb/storage/Kconfig
@@ -4,11 +4,10 @@
 
 comment "NOTE: USB_STORAGE depends on SCSI but BLK_DEV_SD may"
 comment "also be needed; see USB_STORAGE Help for more info"
-	depends on USB
 
 config USB_STORAGE
 	tristate "USB Mass Storage support"
-	depends on USB && SCSI
+	depends on SCSI
 	---help---
 	  Say Y here if you want to connect USB mass storage devices to your
 	  computer's USB port. This is the driver you need for USB
@@ -188,7 +187,7 @@ config USB_STORAGE_CYPRESS_ATACB
 
 config USB_STORAGE_ENE_UB6250
 	tristate "USB ENE card reader support"
-	depends on USB && SCSI
+	depends on SCSI
 	depends on USB_STORAGE
 	---help---
 	  Say Y here if you wish to control a ENE SD/MS Card reader.
@@ -203,7 +202,7 @@ config USB_STORAGE_ENE_UB6250
 
 config USB_UAS
 	tristate "USB Attached SCSI"
-	depends on USB && SCSI && BROKEN
+	depends on SCSI && BROKEN
 	help
 	  The USB Attached SCSI protocol is supported by some USB
 	  storage devices.  It permits higher performance by supporting
diff --git a/drivers/usb/storage/alauda.c b/drivers/usb/storage/alauda.c
index be5564cc8e0..77a2ddfe648 100644
--- a/drivers/usb/storage/alauda.c
+++ b/drivers/usb/storage/alauda.c
@@ -326,8 +326,7 @@ static int alauda_get_media_status(struct us_data *us, unsigned char *data)
 	rc = usb_stor_ctrl_transfer(us, us->recv_ctrl_pipe,
 		command, 0xc0, 0, 1, data, 2);
 
-	US_DEBUGP("alauda_get_media_status: Media status %02X %02X\n",
-		data[0], data[1]);
+	usb_stor_dbg(us, "Media status %02X %02X\n", data[0], data[1]);
 
 	return rc;
 }
@@ -402,7 +401,7 @@ static int alauda_init_media(struct us_data *us)
 			ready = 1;
 	}
 
-	US_DEBUGP("alauda_init_media: We are ready for action!\n");
+	usb_stor_dbg(us, "We are ready for action!\n");
 
 	if (alauda_ack_media(us) != USB_STOR_XFER_GOOD)
 		return USB_STOR_TRANSPORT_ERROR;
@@ -413,15 +412,15 @@ static int alauda_init_media(struct us_data *us)
 		return USB_STOR_TRANSPORT_ERROR;
 
 	if (data[0] != 0x14) {
-		US_DEBUGP("alauda_init_media: Media not ready after ack\n");
+		usb_stor_dbg(us, "Media not ready after ack\n");
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 
 	if (alauda_get_media_signature(us, data) != USB_STOR_XFER_GOOD)
 		return USB_STOR_TRANSPORT_ERROR;
 
-	US_DEBUGP("alauda_init_media: Media signature: %02X %02X %02X %02X\n",
-		data[0], data[1], data[2], data[3]);
+	usb_stor_dbg(us, "Media signature: %02X %02X %02X %02X\n",
+		     data[0], data[1], data[2], data[3]);
 	media_info = alauda_card_find_id(data[1]);
 	if (media_info == NULL) {
 		printk(KERN_WARNING
@@ -432,8 +431,8 @@ static int alauda_init_media(struct us_data *us)
 	}
 
 	MEDIA_INFO(us).capacity = 1 << media_info->chipshift;
-	US_DEBUGP("Found media with capacity: %ldMB\n",
-		MEDIA_INFO(us).capacity >> 20);
+	usb_stor_dbg(us, "Found media with capacity: %ldMB\n",
+		     MEDIA_INFO(us).capacity >> 20);
 
 	MEDIA_INFO(us).pageshift = media_info->pageshift;
 	MEDIA_INFO(us).blockshift = media_info->blockshift;
@@ -472,7 +471,7 @@ static int alauda_check_media(struct us_data *us)
 	/* Check for no media or door open */
 	if ((status[0] & 0x80) || ((status[0] & 0x1F) == 0x10)
 		|| ((status[1] & 0x01) == 0)) {
-		US_DEBUGP("alauda_check_media: No media, or door open\n");
+		usb_stor_dbg(us, "No media, or door open\n");
 		alauda_free_maps(&MEDIA_INFO(us));
 		info->sense_key = 0x02;
 		info->sense_asc = 0x3A;
@@ -482,7 +481,7 @@ static int alauda_check_media(struct us_data *us)
 
 	/* Check for media change */
 	if (status[0] & 0x08) {
-		US_DEBUGP("alauda_check_media: Media change detected\n");
+		usb_stor_dbg(us, "Media change detected\n");
 		alauda_free_maps(&MEDIA_INFO(us));
 		alauda_init_media(us);
 
@@ -518,7 +517,7 @@ static int alauda_check_status2(struct us_data *us)
 	if (rc != USB_STOR_XFER_GOOD)
 		return rc;
 
-	US_DEBUGP("alauda_check_status2: %02X %02X %02X\n", data[0], data[1], data[2]);
+	usb_stor_dbg(us, "%02X %02X %02X\n", data[0], data[1], data[2]);
 	if (data[0] & ALAUDA_STATUS_ERROR)
 		return USB_STOR_XFER_ERROR;
 
@@ -584,7 +583,7 @@ static int alauda_read_map(struct us_data *us, unsigned int zone)
 		goto error;
 	}
 
-	US_DEBUGP("alauda_read_map: Mapping blocks for zone %d\n", zone);
+	usb_stor_dbg(us, "Mapping blocks for zone %d\n", zone);
 
 	/* 1024 PBA's per zone */
 	for (i = 0; i < zonesize; i++)
@@ -604,7 +603,7 @@ static int alauda_read_map(struct us_data *us, unsigned int zone)
 			if (data[j] != 0)
 				goto nonz;
 		pba_to_lba[i] = UNUSABLE;
-		US_DEBUGP("alauda_read_map: PBA %d has no logical mapping\n", blocknum);
+		usb_stor_dbg(us, "PBA %d has no logical mapping\n", blocknum);
 		continue;
 
 	nonz:
@@ -617,19 +616,18 @@ static int alauda_read_map(struct us_data *us, unsigned int zone)
 	nonff:
 		/* normal PBAs start with six FFs */
 		if (j < 6) {
-			US_DEBUGP("alauda_read_map: PBA %d has no logical mapping: "
-			       "reserved area = %02X%02X%02X%02X "
-			       "data status %02X block status %02X\n",
-			       blocknum, data[0], data[1], data[2], data[3],
-			       data[4], data[5]);
+			usb_stor_dbg(us, "PBA %d has no logical mapping: reserved area = %02X%02X%02X%02X data status %02X block status %02X\n",
+				     blocknum,
+				     data[0], data[1], data[2], data[3],
+				     data[4], data[5]);
 			pba_to_lba[i] = UNUSABLE;
 			continue;
 		}
 
 		if ((data[6] >> 4) != 0x01) {
-			US_DEBUGP("alauda_read_map: PBA %d has invalid address "
-			       "field %02X%02X/%02X%02X\n",
-			       blocknum, data[6], data[7], data[11], data[12]);
+			usb_stor_dbg(us, "PBA %d has invalid address field %02X%02X/%02X%02X\n",
+				     blocknum, data[6], data[7],
+				     data[11], data[12]);
 			pba_to_lba[i] = UNUSABLE;
 			continue;
 		}
@@ -711,7 +709,7 @@ static int alauda_erase_block(struct us_data *us, u16 pba)
 	};
 	unsigned char buf[2];
 
-	US_DEBUGP("alauda_erase_block: Erasing PBA %d\n", pba);
+	usb_stor_dbg(us, "Erasing PBA %d\n", pba);
 
 	rc = usb_stor_bulk_transfer_buf(us, us->send_bulk_pipe,
 		command, 9, NULL);
@@ -723,8 +721,7 @@ static int alauda_erase_block(struct us_data *us, u16 pba)
 	if (rc != USB_STOR_XFER_GOOD)
 		return rc;
 
-	US_DEBUGP("alauda_erase_block: Erase result: %02X %02X\n",
-		buf[0], buf[1]);
+	usb_stor_dbg(us, "Erase result: %02X %02X\n", buf[0], buf[1]);
 	return rc;
 }
 
@@ -741,8 +738,7 @@ static int alauda_read_block_raw(struct us_data *us, u16 pba,
 		PBA_ZONE(pba), 0, PBA_LO(pba) + page, pages, 0, MEDIA_PORT(us)
 	};
 
-	US_DEBUGP("alauda_read_block: pba %d page %d count %d\n",
-		pba, page, pages);
+	usb_stor_dbg(us, "pba %d page %d count %d\n", pba, page, pages);
 
 	rc = usb_stor_bulk_transfer_buf(us, us->send_bulk_pipe,
 		command, 9, NULL);
@@ -793,7 +789,7 @@ static int alauda_write_block(struct us_data *us, u16 pba, unsigned char *data)
 		PBA_ZONE(pba), 0, PBA_LO(pba), 32, 0, MEDIA_PORT(us)
 	};
 
-	US_DEBUGP("alauda_write_block: pba %d\n", pba);
+	usb_stor_dbg(us, "pba %d\n", pba);
 
 	rc = usb_stor_bulk_transfer_buf(us, us->send_bulk_pipe,
 		command, 9, NULL);
@@ -866,14 +862,14 @@ static int alauda_write_lba(struct us_data *us, u16 lba,
 		cptr = bptr + pagesize;
 		nand_compute_ecc(bptr, ecc);
 		if (!nand_compare_ecc(cptr+13, ecc)) {
-			US_DEBUGP("Warning: bad ecc in page %d- of pba %d\n",
-				  i, pba);
+			usb_stor_dbg(us, "Warning: bad ecc in page %d- of pba %d\n",
+				     i, pba);
 			nand_store_ecc(cptr+13, ecc);
 		}
 		nand_compute_ecc(bptr + (pagesize / 2), ecc);
 		if (!nand_compare_ecc(cptr+8, ecc)) {
-			US_DEBUGP("Warning: bad ecc in page %d+ of pba %d\n",
-				  i, pba);
+			usb_stor_dbg(us, "Warning: bad ecc in page %d+ of pba %d\n",
+				     i, pba);
 			nand_store_ecc(cptr+8, ecc);
 		}
 		cptr[6] = cptr[11] = MSB_of(lbap);
@@ -900,8 +896,7 @@ static int alauda_write_lba(struct us_data *us, u16 lba,
 	new_pba_offset = new_pba - (zone * zonesize);
 	MEDIA_INFO(us).pba_to_lba[zone][new_pba_offset] = lba;
 	MEDIA_INFO(us).lba_to_pba[zone][lba_offset] = new_pba;
-	US_DEBUGP("alauda_write_lba: Remapped LBA %d to PBA %d\n",
-		lba, new_pba);
+	usb_stor_dbg(us, "Remapped LBA %d to PBA %d\n", lba, new_pba);
 
 	if (pba != UNDEF) {
 		unsigned int pba_offset = pba - (zone * zonesize);
@@ -964,8 +959,8 @@ static int alauda_read_data(struct us_data *us, unsigned long address,
 
 		/* Not overflowing capacity? */
 		if (lba >= max_lba) {
-			US_DEBUGP("Error: Requested lba %u exceeds "
-				  "maximum %u\n", lba, max_lba);
+			usb_stor_dbg(us, "Error: Requested lba %u exceeds maximum %u\n",
+				     lba, max_lba);
 			result = USB_STOR_TRANSPORT_ERROR;
 			break;
 		}
@@ -978,8 +973,8 @@ static int alauda_read_data(struct us_data *us, unsigned long address,
 		pba = MEDIA_INFO(us).lba_to_pba[zone][lba_offset];
 
 		if (pba == UNDEF) {	/* this lba was never written */
-			US_DEBUGP("Read %d zero pages (LBA %d) page %d\n",
-				  pages, lba, page);
+			usb_stor_dbg(us, "Read %d zero pages (LBA %d) page %d\n",
+				     pages, lba, page);
 
 			/* This is not really an error. It just means
 			   that the block has never been written.
@@ -988,9 +983,8 @@ static int alauda_read_data(struct us_data *us, unsigned long address,
 
 			memset(buffer, 0, len);
 		} else {
-			US_DEBUGP("Read %d pages, from PBA %d"
-				  " (LBA %d) page %d\n",
-				  pages, pba, lba, page);
+			usb_stor_dbg(us, "Read %d pages, from PBA %d (LBA %d) page %d\n",
+				     pages, pba, lba, page);
 
 			result = alauda_read_block(us, pba, page, pages, buffer);
 			if (result != USB_STOR_TRANSPORT_GOOD)
@@ -1066,8 +1060,8 @@ static int alauda_write_data(struct us_data *us, unsigned long address,
 
 		/* Not overflowing capacity? */
 		if (lba >= max_lba) {
-			US_DEBUGP("alauda_write_data: Requested lba %u exceeds "
-				  "maximum %u\n", lba, max_lba);
+			usb_stor_dbg(us, "Requested lba %u exceeds maximum %u\n",
+				     lba, max_lba);
 			result = USB_STOR_TRANSPORT_ERROR;
 			break;
 		}
@@ -1122,11 +1116,9 @@ static int init_alauda(struct us_data *us)
 	nand_init_ecc();
 
 	us->extra = kzalloc(sizeof(struct alauda_info), GFP_NOIO);
-	if (!us->extra) {
-		US_DEBUGP("init_alauda: Gah! Can't allocate storage for"
-			"alauda info struct!\n");
+	if (!us->extra)
 		return USB_STOR_TRANSPORT_ERROR;
-	}
+
 	info = (struct alauda_info *) us->extra;
 	us->extra_destructor = alauda_info_destructor;
 
@@ -1147,15 +1139,14 @@ static int alauda_transport(struct scsi_cmnd *srb, struct us_data *us)
 	};
 
 	if (srb->cmnd[0] == INQUIRY) {
-		US_DEBUGP("alauda_transport: INQUIRY. "
-			"Returning bogus response.\n");
+		usb_stor_dbg(us, "INQUIRY - Returning bogus response\n");
 		memcpy(ptr, inquiry_response, sizeof(inquiry_response));
 		fill_inquiry_response(us, ptr, 36);
 		return USB_STOR_TRANSPORT_GOOD;
 	}
 
 	if (srb->cmnd[0] == TEST_UNIT_READY) {
-		US_DEBUGP("alauda_transport: TEST_UNIT_READY.\n");
+		usb_stor_dbg(us, "TEST_UNIT_READY\n");
 		return alauda_check_media(us);
 	}
 
@@ -1193,8 +1184,7 @@ static int alauda_transport(struct scsi_cmnd *srb, struct us_data *us)
 		page |= short_pack(srb->cmnd[5], srb->cmnd[4]);
 		pages = short_pack(srb->cmnd[8], srb->cmnd[7]);
 
-		US_DEBUGP("alauda_transport: READ_10: page %d pagect %d\n",
-			  page, pages);
+		usb_stor_dbg(us, "READ_10: page %d pagect %d\n", page, pages);
 
 		return alauda_read_data(us, page, pages);
 	}
@@ -1211,14 +1201,13 @@ static int alauda_transport(struct scsi_cmnd *srb, struct us_data *us)
 		page |= short_pack(srb->cmnd[5], srb->cmnd[4]);
 		pages = short_pack(srb->cmnd[8], srb->cmnd[7]);
 
-		US_DEBUGP("alauda_transport: WRITE_10: page %d pagect %d\n",
-			  page, pages);
+		usb_stor_dbg(us, "WRITE_10: page %d pagect %d\n", page, pages);
 
 		return alauda_write_data(us, page, pages);
 	}
 
 	if (srb->cmnd[0] == REQUEST_SENSE) {
-		US_DEBUGP("alauda_transport: REQUEST_SENSE.\n");
+		usb_stor_dbg(us, "REQUEST_SENSE\n");
 
 		memset(ptr, 0, 18);
 		ptr[0] = 0xF0;
@@ -1237,8 +1226,8 @@ static int alauda_transport(struct scsi_cmnd *srb, struct us_data *us)
 		return USB_STOR_TRANSPORT_GOOD;
 	}
 
-	US_DEBUGP("alauda_transport: Gah! Unknown command: %d (0x%x)\n",
-		srb->cmnd[0], srb->cmnd[0]);
+	usb_stor_dbg(us, "Gah! Unknown command: %d (0x%x)\n",
+		     srb->cmnd[0], srb->cmnd[0]);
 	info->sense_key = 0x05;
 	info->sense_asc = 0x20;
 	info->sense_ascq = 0x00;
diff --git a/drivers/usb/storage/cypress_atacb.c b/drivers/usb/storage/cypress_atacb.c
index 070b5c0ebbf..8514a2d82b7 100644
--- a/drivers/usb/storage/cypress_atacb.c
+++ b/drivers/usb/storage/cypress_atacb.c
@@ -159,7 +159,7 @@ static void cypress_atacb_passthrough(struct scsi_cmnd *srb, struct us_data *us)
 	if (srb->result == SAM_STAT_CHECK_CONDITION &&
 			memcmp(srb->sense_buffer, usb_stor_sense_invalidCDB,
 				sizeof(usb_stor_sense_invalidCDB)) == 0) {
-		US_DEBUGP("cypress atacb not supported ???\n");
+		usb_stor_dbg(us, "cypress atacb not supported ???\n");
 		goto end;
 	}
 
@@ -248,14 +248,26 @@ static int cypress_probe(struct usb_interface *intf,
 {
 	struct us_data *us;
 	int result;
+	struct usb_device *device;
 
 	result = usb_stor_probe1(&us, intf, id,
 			(id - cypress_usb_ids) + cypress_unusual_dev_list);
 	if (result)
 		return result;
 
-	us->protocol_name = "Transparent SCSI with Cypress ATACB";
-	us->proto_handler = cypress_atacb_passthrough;
+	/* Among CY7C68300 chips, the A revision does not support Cypress ATACB
+	 * Filter out this revision from EEPROM default descriptor values
+	 */
+	device = interface_to_usbdev(intf);
+	if (device->descriptor.iManufacturer != 0x38 ||
+	    device->descriptor.iProduct != 0x4e ||
+	    device->descriptor.iSerialNumber != 0x64) {
+		us->protocol_name = "Transparent SCSI with Cypress ATACB";
+		us->proto_handler = cypress_atacb_passthrough;
+	} else {
+		us->protocol_name = "Transparent SCSI";
+		us->proto_handler = usb_stor_transparent_scsi_command;
+	}
 
 	result = usb_stor_probe2(us);
 	return result;
diff --git a/drivers/usb/storage/datafab.c b/drivers/usb/storage/datafab.c
index 494fee5af41..7b17c216981 100644
--- a/drivers/usb/storage/datafab.c
+++ b/drivers/usb/storage/datafab.c
@@ -123,7 +123,7 @@ datafab_bulk_read(struct us_data *us, unsigned char *data, unsigned int len) {
 	if (len == 0)
 		return USB_STOR_XFER_GOOD;
 
-	US_DEBUGP("datafab_bulk_read:  len = %d\n", len);
+	usb_stor_dbg(us, "len = %d\n", len);
 	return usb_stor_bulk_transfer_buf(us, us->recv_bulk_pipe,
 			data, len, NULL);
 }
@@ -134,7 +134,7 @@ datafab_bulk_write(struct us_data *us, unsigned char *data, unsigned int len) {
 	if (len == 0)
 		return USB_STOR_XFER_GOOD;
 
-	US_DEBUGP("datafab_bulk_write:  len = %d\n", len);
+	usb_stor_dbg(us, "len = %d\n", len);
 	return usb_stor_bulk_transfer_buf(us, us->send_bulk_pipe,
 			data, len, NULL);
 }
@@ -300,9 +300,8 @@ static int datafab_write_data(struct us_data *us,
 			goto leave;
 
 		if (reply[0] != 0x50 && reply[1] != 0) {
-			US_DEBUGP("datafab_write_data:  Gah! "
-				  "write return code: %02x %02x\n",
-				  reply[0], reply[1]);
+			usb_stor_dbg(us, "Gah! write return code: %02x %02x\n",
+				     reply[0], reply[1]);
 			result = USB_STOR_TRANSPORT_ERROR;
 			goto leave;
 		}
@@ -342,7 +341,7 @@ static int datafab_determine_lun(struct us_data *us,
 	if (!buf)
 		return USB_STOR_TRANSPORT_ERROR;
 
-	US_DEBUGP("datafab_determine_lun:  locating...\n");
+	usb_stor_dbg(us, "locating...\n");
 
 	// we'll try 3 times before giving up...
 	//
@@ -474,16 +473,16 @@ static int datafab_handle_mode_sense(struct us_data *us,
 
 	switch (pc) {
 	   case 0x0:
-		US_DEBUGP("datafab_handle_mode_sense:  Current values\n");
+		   usb_stor_dbg(us, "Current values\n");
 		break;
 	   case 0x1:
-		US_DEBUGP("datafab_handle_mode_sense:  Changeable values\n");
+		   usb_stor_dbg(us, "Changeable values\n");
 		break;
 	   case 0x2:
-		US_DEBUGP("datafab_handle_mode_sense:  Default values\n");
+		   usb_stor_dbg(us, "Default values\n");
 		break;
 	   case 0x3:
-		US_DEBUGP("datafab_handle_mode_sense:  Saves values\n");
+		   usb_stor_dbg(us, "Saves values\n");
 		break;
 	}
 
@@ -566,11 +565,9 @@ static int datafab_transport(struct scsi_cmnd *srb, struct us_data *us)
 
 	if (!us->extra) {
 		us->extra = kzalloc(sizeof(struct datafab_info), GFP_NOIO);
-		if (!us->extra) {
-			US_DEBUGP("datafab_transport:  Gah! "
-				  "Can't allocate storage for Datafab info struct!\n");
+		if (!us->extra)
 			return USB_STOR_TRANSPORT_ERROR;
-		}
+
 		us->extra_destructor = datafab_info_destructor;
   		((struct datafab_info *)us->extra)->lun = -1;
 	}
@@ -578,7 +575,7 @@ static int datafab_transport(struct scsi_cmnd *srb, struct us_data *us)
 	info = (struct datafab_info *) (us->extra);
 
 	if (srb->cmnd[0] == INQUIRY) {
-		US_DEBUGP("datafab_transport:  INQUIRY.  Returning bogus response");
+		usb_stor_dbg(us, "INQUIRY - Returning bogus response\n");
 		memcpy(ptr, inquiry_reply, sizeof(inquiry_reply));
 		fill_inquiry_response(us, ptr, 36);
 		return USB_STOR_TRANSPORT_GOOD;
@@ -590,8 +587,8 @@ static int datafab_transport(struct scsi_cmnd *srb, struct us_data *us)
 		if (rc != USB_STOR_TRANSPORT_GOOD)
 			return rc;
 
-		US_DEBUGP("datafab_transport:  READ_CAPACITY:  %ld sectors, %ld bytes per sector\n",
-			  info->sectors, info->ssize);
+		usb_stor_dbg(us, "READ_CAPACITY:  %ld sectors, %ld bytes per sector\n",
+			     info->sectors, info->ssize);
 
 		// build the reply
 		// we need the last sector, not the number of sectors
@@ -603,7 +600,7 @@ static int datafab_transport(struct scsi_cmnd *srb, struct us_data *us)
 	}
 
 	if (srb->cmnd[0] == MODE_SELECT_10) {
-		US_DEBUGP("datafab_transport:  Gah! MODE_SELECT_10.\n");
+		usb_stor_dbg(us, "Gah! MODE_SELECT_10\n");
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 
@@ -615,7 +612,8 @@ static int datafab_transport(struct scsi_cmnd *srb, struct us_data *us)
 
 		blocks = ((u32)(srb->cmnd[7]) << 8) | ((u32)(srb->cmnd[8]));
 
-		US_DEBUGP("datafab_transport:  READ_10: read block 0x%04lx  count %ld\n", block, blocks);
+		usb_stor_dbg(us, "READ_10: read block 0x%04lx  count %ld\n",
+			     block, blocks);
 		return datafab_read_data(us, info, block, blocks);
 	}
 
@@ -628,7 +626,8 @@ static int datafab_transport(struct scsi_cmnd *srb, struct us_data *us)
 		blocks = ((u32)(srb->cmnd[6]) << 24) | ((u32)(srb->cmnd[7]) << 16) |
 			 ((u32)(srb->cmnd[8]) <<  8) | ((u32)(srb->cmnd[9]));
 
-		US_DEBUGP("datafab_transport:  READ_12: read block 0x%04lx  count %ld\n", block, blocks);
+		usb_stor_dbg(us, "READ_12: read block 0x%04lx  count %ld\n",
+			     block, blocks);
 		return datafab_read_data(us, info, block, blocks);
 	}
 
@@ -638,7 +637,8 @@ static int datafab_transport(struct scsi_cmnd *srb, struct us_data *us)
 
 		blocks = ((u32)(srb->cmnd[7]) << 8) | ((u32)(srb->cmnd[8]));
 
-		US_DEBUGP("datafab_transport:  WRITE_10: write block 0x%04lx  count %ld\n", block, blocks);
+		usb_stor_dbg(us, "WRITE_10: write block 0x%04lx count %ld\n",
+			     block, blocks);
 		return datafab_write_data(us, info, block, blocks);
 	}
 
@@ -651,17 +651,18 @@ static int datafab_transport(struct scsi_cmnd *srb, struct us_data *us)
 		blocks = ((u32)(srb->cmnd[6]) << 24) | ((u32)(srb->cmnd[7]) << 16) |
 			 ((u32)(srb->cmnd[8]) <<  8) | ((u32)(srb->cmnd[9]));
 
-		US_DEBUGP("datafab_transport:  WRITE_12: write block 0x%04lx  count %ld\n", block, blocks);
+		usb_stor_dbg(us, "WRITE_12: write block 0x%04lx count %ld\n",
+			     block, blocks);
 		return datafab_write_data(us, info, block, blocks);
 	}
 
 	if (srb->cmnd[0] == TEST_UNIT_READY) {
-		US_DEBUGP("datafab_transport:  TEST_UNIT_READY.\n");
+		usb_stor_dbg(us, "TEST_UNIT_READY\n");
 		return datafab_id_device(us, info);
 	}
 
 	if (srb->cmnd[0] == REQUEST_SENSE) {
-		US_DEBUGP("datafab_transport:  REQUEST_SENSE.  Returning faked response\n");
+		usb_stor_dbg(us, "REQUEST_SENSE - Returning faked response\n");
 
 		// this response is pretty bogus right now.  eventually if necessary
 		// we can set the correct sense data.  so far though it hasn't been
@@ -679,12 +680,12 @@ static int datafab_transport(struct scsi_cmnd *srb, struct us_data *us)
 	}
 
 	if (srb->cmnd[0] == MODE_SENSE) {
-		US_DEBUGP("datafab_transport:  MODE_SENSE_6 detected\n");
+		usb_stor_dbg(us, "MODE_SENSE_6 detected\n");
 		return datafab_handle_mode_sense(us, srb, 1);
 	}
 
 	if (srb->cmnd[0] == MODE_SENSE_10) {
-		US_DEBUGP("datafab_transport:  MODE_SENSE_10 detected\n");
+		usb_stor_dbg(us, "MODE_SENSE_10 detected\n");
 		return datafab_handle_mode_sense(us, srb, 0);
 	}
 
@@ -698,7 +699,7 @@ static int datafab_transport(struct scsi_cmnd *srb, struct us_data *us)
 	if (srb->cmnd[0] == START_STOP) {
 		/* this is used by sd.c'check_scsidisk_media_change to detect
 		   media change */
-		US_DEBUGP("datafab_transport:  START_STOP.\n");
+		usb_stor_dbg(us, "START_STOP\n");
 		/* the first datafab_id_device after a media change returns
 		   an error (determined experimentally) */
 		rc = datafab_id_device(us, info);
@@ -712,8 +713,8 @@ static int datafab_transport(struct scsi_cmnd *srb, struct us_data *us)
 		return rc;
 	}
 
-	US_DEBUGP("datafab_transport:  Gah! Unknown command: %d (0x%x)\n",
-		  srb->cmnd[0], srb->cmnd[0]);
+	usb_stor_dbg(us, "Gah! Unknown command: %d (0x%x)\n",
+		     srb->cmnd[0], srb->cmnd[0]);
 	info->sense_key = 0x05;
 	info->sense_asc = 0x20;
 	info->sense_ascq = 0x00;
diff --git a/drivers/usb/storage/debug.c b/drivers/usb/storage/debug.c
index a2b5526c9fa..e08f64780e3 100644
--- a/drivers/usb/storage/debug.c
+++ b/drivers/usb/storage/debug.c
@@ -42,16 +42,19 @@
  * 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
+#include <linux/device.h>
 #include <linux/cdrom.h>
+#include <linux/export.h>
 #include <scsi/scsi.h>
 #include <scsi/scsi_cmnd.h>
 #include <scsi/scsi_dbg.h>
 
+#include "usb.h"
 #include "debug.h"
 #include "scsi.h"
 
 
-void usb_stor_show_command(struct scsi_cmnd *srb)
+void usb_stor_show_command(const struct us_data *us, struct scsi_cmnd *srb)
 {
 	char *what = NULL;
 	int i;
@@ -149,18 +152,18 @@ void usb_stor_show_command(struct scsi_cmnd *srb)
 	case WRITE_LONG_2: what = "WRITE_LONG_2"; break;
 	default: what = "(unknown command)"; break;
 	}
-	US_DEBUGP("Command %s (%d bytes)\n", what, srb->cmd_len);
-	US_DEBUGP("");
+	usb_stor_dbg(us, "Command %s (%d bytes)\n", what, srb->cmd_len);
+	usb_stor_dbg(us, "bytes: ");
 	for (i = 0; i < srb->cmd_len && i < 16; i++)
 		US_DEBUGPX(" %02x", srb->cmnd[i]);
 	US_DEBUGPX("\n");
 }
 
-void usb_stor_show_sense(
-		unsigned char key,
-		unsigned char asc,
-		unsigned char ascq) {
-
+void usb_stor_show_sense(const struct us_data *us,
+			 unsigned char key,
+			 unsigned char asc,
+			 unsigned char ascq)
+{
 	const char *what, *keystr;
 
 	keystr = scsi_sense_key_string(key);
@@ -171,7 +174,22 @@ void usb_stor_show_sense(
 	if (what == NULL)
 		what = "(unknown ASC/ASCQ)";
 
-	US_DEBUGP("%s: ", keystr);
+	usb_stor_dbg(us, "%s: ", keystr);
 	US_DEBUGPX(what, ascq);
 	US_DEBUGPX("\n");
 }
+
+int usb_stor_dbg(const struct us_data *us, const char *fmt, ...)
+{
+	va_list args;
+	int r;
+
+	va_start(args, fmt);
+
+	r = dev_vprintk_emit(7, &us->pusb_dev->dev, fmt, args);
+
+	va_end(args);
+
+	return r;
+}
+EXPORT_SYMBOL_GPL(usb_stor_dbg);
diff --git a/drivers/usb/storage/debug.h b/drivers/usb/storage/debug.h
index dbb985d5242..b1273f03e22 100644
--- a/drivers/usb/storage/debug.h
+++ b/drivers/usb/storage/debug.h
@@ -47,15 +47,22 @@
 #define USB_STORAGE "usb-storage: "
 
 #ifdef CONFIG_USB_STORAGE_DEBUG
-void usb_stor_show_command(struct scsi_cmnd *srb);
-void usb_stor_show_sense( unsigned char key,
-		unsigned char asc, unsigned char ascq );
-#define US_DEBUGP(x...) printk( KERN_DEBUG USB_STORAGE x )
-#define US_DEBUGPX(x...) printk( x )
-#define US_DEBUG(x) x 
+void usb_stor_show_command(const struct us_data *us, struct scsi_cmnd *srb);
+void usb_stor_show_sense(const struct us_data *us, unsigned char key,
+			 unsigned char asc, unsigned char ascq);
+__printf(2, 3) int usb_stor_dbg(const struct us_data *us,
+				const char *fmt, ...);
+
+#define US_DEBUGPX(fmt, ...)	printk(fmt, ##__VA_ARGS__)
+#define US_DEBUG(x)		x
 #else
-#define US_DEBUGP(x...)
-#define US_DEBUGPX(x...)
+__printf(2, 3)
+static inline int _usb_stor_dbg(const struct us_data *us,
+				const char *fmt, ...) {return 1;}
+#define usb_stor_dbg(us, fmt, ...)				\
+	do { if (0) _usb_stor_dbg(us, fmt, ##__VA_ARGS__); } while (0)
+#define US_DEBUGPX(fmt, ...)					\
+	do { if (0) printk(fmt, ##__VA_ARGS__); } while (0)
 #define US_DEBUG(x)
 #endif
 
diff --git a/drivers/usb/storage/ene_ub6250.c b/drivers/usb/storage/ene_ub6250.c
index 118b134a1da..1bfc9a6cab5 100644
--- a/drivers/usb/storage/ene_ub6250.c
+++ b/drivers/usb/storage/ene_ub6250.c
@@ -504,12 +504,12 @@ static int ene_send_scsi_cmd(struct us_data *us, u8 fDir, void *buf, int use_sg)
 	unsigned int cswlen = 0, partial = 0;
 	unsigned int transfer_length = bcb->DataTransferLength;
 
-	/* US_DEBUGP("transport --- ene_send_scsi_cmd\n"); */
+	/* usb_stor_dbg(us, "transport --- ene_send_scsi_cmd\n"); */
 	/* send cmd to out endpoint */
 	result = usb_stor_bulk_transfer_buf(us, us->send_bulk_pipe,
 					    bcb, US_BULK_CB_WRAP_LEN, NULL);
 	if (result != USB_STOR_XFER_GOOD) {
-		US_DEBUGP("send cmd to out endpoint fail ---\n");
+		usb_stor_dbg(us, "send cmd to out endpoint fail ---\n");
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 
@@ -529,7 +529,7 @@ static int ene_send_scsi_cmd(struct us_data *us, u8 fDir, void *buf, int use_sg)
 						transfer_length, 0, &partial);
 		}
 		if (result != USB_STOR_XFER_GOOD) {
-			US_DEBUGP("data transfer fail ---\n");
+			usb_stor_dbg(us, "data transfer fail ---\n");
 			return USB_STOR_TRANSPORT_ERROR;
 		}
 	}
@@ -539,14 +539,14 @@ static int ene_send_scsi_cmd(struct us_data *us, u8 fDir, void *buf, int use_sg)
 					    US_BULK_CS_WRAP_LEN, &cswlen);
 
 	if (result == USB_STOR_XFER_SHORT && cswlen == 0) {
-		US_DEBUGP("Received 0-length CSW; retrying...\n");
+		usb_stor_dbg(us, "Received 0-length CSW; retrying...\n");
 		result = usb_stor_bulk_transfer_buf(us, us->recv_bulk_pipe,
 					    bcs, US_BULK_CS_WRAP_LEN, &cswlen);
 	}
 
 	if (result == USB_STOR_XFER_STALLED) {
 		/* get the status again */
-		US_DEBUGP("Attempting to get CSW (2nd try)...\n");
+		usb_stor_dbg(us, "Attempting to get CSW (2nd try)...\n");
 		result = usb_stor_bulk_transfer_buf(us, us->recv_bulk_pipe,
 						bcs, US_BULK_CS_WRAP_LEN, NULL);
 	}
@@ -626,7 +626,7 @@ static int sd_scsi_read_capacity(struct us_data *us, struct scsi_cmnd *srb)
 	struct scatterlist *sg = NULL;
 	struct ene_ub6250_info *info = (struct ene_ub6250_info *) us->extra;
 
-	US_DEBUGP("sd_scsi_read_capacity\n");
+	usb_stor_dbg(us, "sd_scsi_read_capacity\n");
 	if (info->SD_Status.HiCapacity) {
 		bl_len = 0x200;
 		if (info->SD_Status.IsMMC)
@@ -639,8 +639,8 @@ static int sd_scsi_read_capacity(struct us_data *us, struct scsi_cmnd *srb)
 				* (1 << (info->SD_C_SIZE_MULT + 2)) - 1;
 	}
 	info->bl_num = bl_num;
-	US_DEBUGP("bl_len = %x\n", bl_len);
-	US_DEBUGP("bl_num = %x\n", bl_num);
+	usb_stor_dbg(us, "bl_len = %x\n", bl_len);
+	usb_stor_dbg(us, "bl_num = %x\n", bl_num);
 
 	/*srb->request_bufflen = 8; */
 	buf[0] = (bl_num >> 24) & 0xff;
@@ -675,7 +675,7 @@ static int sd_scsi_read(struct us_data *us, struct scsi_cmnd *srb)
 
 	result = ene_load_bincode(us, SD_RW_PATTERN);
 	if (result != USB_STOR_XFER_GOOD) {
-		US_DEBUGP("Load SD RW pattern Fail !!\n");
+		usb_stor_dbg(us, "Load SD RW pattern Fail !!\n");
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 
@@ -715,7 +715,7 @@ static int sd_scsi_write(struct us_data *us, struct scsi_cmnd *srb)
 
 	result = ene_load_bincode(us, SD_RW_PATTERN);
 	if (result != USB_STOR_XFER_GOOD) {
-		US_DEBUGP("Load SD RW pattern Fail !!\n");
+		usb_stor_dbg(us, "Load SD RW pattern Fail !!\n");
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 
@@ -1493,7 +1493,7 @@ static int ms_scsi_read_capacity(struct us_data *us, struct scsi_cmnd *srb)
 	struct scatterlist *sg = NULL;
 	struct ene_ub6250_info *info = (struct ene_ub6250_info *) us->extra;
 
-	US_DEBUGP("ms_scsi_read_capacity\n");
+	usb_stor_dbg(us, "ms_scsi_read_capacity\n");
 	bl_len = 0x200;
 	if (info->MS_Status.IsMSPro)
 		bl_num = info->MSP_TotalBlock - 1;
@@ -1501,8 +1501,8 @@ static int ms_scsi_read_capacity(struct us_data *us, struct scsi_cmnd *srb)
 		bl_num = info->MS_Lib.NumberOfLogBlock * info->MS_Lib.blockSize * 2 - 1;
 
 	info->bl_num = bl_num;
-	US_DEBUGP("bl_len = %x\n", bl_len);
-	US_DEBUGP("bl_num = %x\n", bl_num);
+	usb_stor_dbg(us, "bl_len = %x\n", bl_len);
+	usb_stor_dbg(us, "bl_num = %x\n", bl_num);
 
 	/*srb->request_bufflen = 8; */
 	buf[0] = (bl_num >> 24) & 0xff;
@@ -1654,7 +1654,7 @@ static int ms_scsi_read(struct us_data *us, struct scsi_cmnd *srb)
 	if (info->MS_Status.IsMSPro) {
 		result = ene_load_bincode(us, MSP_RW_PATTERN);
 		if (result != USB_STOR_XFER_GOOD) {
-			US_DEBUGP("Load MPS RW pattern Fail !!\n");
+			usb_stor_dbg(us, "Load MPS RW pattern Fail !!\n");
 			return USB_STOR_TRANSPORT_ERROR;
 		}
 
@@ -1854,7 +1854,7 @@ static int ene_get_card_status(struct us_data *us, u8 *buf)
 	u32 reg4b;
 	struct ene_ub6250_info *info = (struct ene_ub6250_info *) us->extra;
 
-	/*US_DEBUGP("transport --- ENE_ReadSDReg\n");*/
+	/*usb_stor_dbg(us, "transport --- ENE_ReadSDReg\n");*/
 	reg4b = *(u32 *)&buf[0x18];
 	info->SD_READ_BL_LEN = (u8)((reg4b >> 8) & 0x0f);
 
@@ -1894,45 +1894,44 @@ static int ene_load_bincode(struct us_data *us, unsigned char flag)
 	switch (flag) {
 	/* For SD */
 	case SD_INIT1_PATTERN:
-		US_DEBUGP("SD_INIT1_PATTERN\n");
+		usb_stor_dbg(us, "SD_INIT1_PATTERN\n");
 		fw_name = SD_INIT1_FIRMWARE;
 		break;
 	case SD_INIT2_PATTERN:
-		US_DEBUGP("SD_INIT2_PATTERN\n");
+		usb_stor_dbg(us, "SD_INIT2_PATTERN\n");
 		fw_name = SD_INIT2_FIRMWARE;
 		break;
 	case SD_RW_PATTERN:
-		US_DEBUGP("SD_RW_PATTERN\n");
+		usb_stor_dbg(us, "SD_RW_PATTERN\n");
 		fw_name = SD_RW_FIRMWARE;
 		break;
 	/* For MS */
 	case MS_INIT_PATTERN:
-		US_DEBUGP("MS_INIT_PATTERN\n");
+		usb_stor_dbg(us, "MS_INIT_PATTERN\n");
 		fw_name = MS_INIT_FIRMWARE;
 		break;
 	case MSP_RW_PATTERN:
-		US_DEBUGP("MSP_RW_PATTERN\n");
+		usb_stor_dbg(us, "MSP_RW_PATTERN\n");
 		fw_name = MSP_RW_FIRMWARE;
 		break;
 	case MS_RW_PATTERN:
-		US_DEBUGP("MS_RW_PATTERN\n");
+		usb_stor_dbg(us, "MS_RW_PATTERN\n");
 		fw_name = MS_RW_FIRMWARE;
 		break;
 	default:
-		US_DEBUGP("----------- Unknown PATTERN ----------\n");
+		usb_stor_dbg(us, "----------- Unknown PATTERN ----------\n");
 		goto nofw;
 	}
 
 	err = request_firmware(&sd_fw, fw_name, &us->pusb_dev->dev);
 	if (err) {
-		US_DEBUGP("load firmware %s failed\n", fw_name);
+		usb_stor_dbg(us, "load firmware %s failed\n", fw_name);
 		goto nofw;
 	}
 	buf = kmalloc(sd_fw->size, GFP_KERNEL);
-	if (buf == NULL) {
-		US_DEBUGP("Malloc memory for fireware failed!\n");
+	if (buf == NULL)
 		goto nofw;
-	}
+
 	memcpy(buf, sd_fw->data, sd_fw->size);
 	memset(bcb, 0, sizeof(struct bulk_cb_wrap));
 	bcb->Signature = cpu_to_le32(US_BULK_CB_SIGN);
@@ -2116,9 +2115,9 @@ static int ene_ms_init(struct us_data *us)
 		} else {
 			ms_card_init(us); /* Card is MS (to ms.c)*/
 		}
-		US_DEBUGP("MS Init Code OK !!\n");
+		usb_stor_dbg(us, "MS Init Code OK !!\n");
 	} else {
-		US_DEBUGP("MS Card Not Ready --- %x\n", buf[0]);
+		usb_stor_dbg(us, "MS Card Not Ready --- %x\n", buf[0]);
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 
@@ -2132,11 +2131,11 @@ static int ene_sd_init(struct us_data *us)
 	struct bulk_cb_wrap *bcb = (struct bulk_cb_wrap *) us->iobuf;
 	struct ene_ub6250_info *info = (struct ene_ub6250_info *) us->extra;
 
-	US_DEBUGP("transport --- ENE_SDInit\n");
+	usb_stor_dbg(us, "transport --- ENE_SDInit\n");
 	/* SD Init Part-1 */
 	result = ene_load_bincode(us, SD_INIT1_PATTERN);
 	if (result != USB_STOR_XFER_GOOD) {
-		US_DEBUGP("Load SD Init Code Part-1 Fail !!\n");
+		usb_stor_dbg(us, "Load SD Init Code Part-1 Fail !!\n");
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 
@@ -2147,14 +2146,14 @@ static int ene_sd_init(struct us_data *us)
 
 	result = ene_send_scsi_cmd(us, FDIR_READ, NULL, 0);
 	if (result != USB_STOR_XFER_GOOD) {
-		US_DEBUGP("Execution SD Init Code Fail !!\n");
+		usb_stor_dbg(us, "Execution SD Init Code Fail !!\n");
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 
 	/* SD Init Part-2 */
 	result = ene_load_bincode(us, SD_INIT2_PATTERN);
 	if (result != USB_STOR_XFER_GOOD) {
-		US_DEBUGP("Load SD Init Code Part-2 Fail !!\n");
+		usb_stor_dbg(us, "Load SD Init Code Part-2 Fail !!\n");
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 
@@ -2166,21 +2165,23 @@ static int ene_sd_init(struct us_data *us)
 
 	result = ene_send_scsi_cmd(us, FDIR_READ, &buf, 0);
 	if (result != USB_STOR_XFER_GOOD) {
-		US_DEBUGP("Execution SD Init Code Fail !!\n");
+		usb_stor_dbg(us, "Execution SD Init Code Fail !!\n");
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 
 	info->SD_Status =  *(struct SD_STATUS *)&buf[0];
 	if (info->SD_Status.Insert && info->SD_Status.Ready) {
+		struct SD_STATUS *s = &info->SD_Status;
+
 		ene_get_card_status(us, (unsigned char *)&buf);
-		US_DEBUGP("Insert     = %x\n", info->SD_Status.Insert);
-		US_DEBUGP("Ready      = %x\n", info->SD_Status.Ready);
-		US_DEBUGP("IsMMC      = %x\n", info->SD_Status.IsMMC);
-		US_DEBUGP("HiCapacity = %x\n", info->SD_Status.HiCapacity);
-		US_DEBUGP("HiSpeed    = %x\n", info->SD_Status.HiSpeed);
-		US_DEBUGP("WtP        = %x\n", info->SD_Status.WtP);
+		usb_stor_dbg(us, "Insert     = %x\n", s->Insert);
+		usb_stor_dbg(us, "Ready      = %x\n", s->Ready);
+		usb_stor_dbg(us, "IsMMC      = %x\n", s->IsMMC);
+		usb_stor_dbg(us, "HiCapacity = %x\n", s->HiCapacity);
+		usb_stor_dbg(us, "HiSpeed    = %x\n", s->HiSpeed);
+		usb_stor_dbg(us, "WtP        = %x\n", s->WtP);
 	} else {
-		US_DEBUGP("SD Card Not Ready --- %x\n", buf[0]);
+		usb_stor_dbg(us, "SD Card Not Ready --- %x\n", buf[0]);
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 	return USB_STOR_TRANSPORT_GOOD;
@@ -2293,7 +2294,7 @@ static int ene_transport(struct scsi_cmnd *srb, struct us_data *us)
 	int result = 0;
 	struct ene_ub6250_info *info = (struct ene_ub6250_info *)(us->extra);
 
-	/*US_DEBUG(usb_stor_show_command(srb)); */
+	/*US_DEBUG(usb_stor_show_command(us, srb)); */
 	scsi_set_resid(srb, 0);
 	if (unlikely(!(info->SD_Status.Ready || info->MS_Status.Ready))) {
 		result = ene_init(us);
@@ -2362,7 +2363,6 @@ static int ene_ub6250_resume(struct usb_interface *iface)
 
 	mutex_lock(&us->dev_mutex);
 
-	US_DEBUGP("%s\n", __func__);
 	if (us->suspend_resume_hook)
 		(us->suspend_resume_hook)(us, US_RESUME);
 
@@ -2382,7 +2382,7 @@ static int ene_ub6250_reset_resume(struct usb_interface *iface)
 	u8 tmp = 0;
 	struct us_data *us = usb_get_intfdata(iface);
 	struct ene_ub6250_info *info = (struct ene_ub6250_info *)(us->extra);
-	US_DEBUGP("%s\n", __func__);
+
 	/* Report the reset to the SCSI core */
 	usb_stor_reset_resume(iface);
 
diff --git a/drivers/usb/storage/freecom.c b/drivers/usb/storage/freecom.c
index e6df087dca9..ef16068b708 100644
--- a/drivers/usb/storage/freecom.c
+++ b/drivers/usb/storage/freecom.c
@@ -40,7 +40,7 @@ MODULE_AUTHOR("David Brown <usb-storage@davidb.org>");
 MODULE_LICENSE("GPL");
 
 #ifdef CONFIG_USB_STORAGE_DEBUG
-static void pdump (void *, int);
+static void pdump(struct us_data *us, void *ibuffer, int length);
 #endif
 
 /* Bits of HD_STATUS */
@@ -161,20 +161,20 @@ freecom_readdata (struct scsi_cmnd *srb, struct us_data *us,
 	fxfr->Count = cpu_to_le32 (count);
 	memset (fxfr->Pad, 0, sizeof (fxfr->Pad));
 
-	US_DEBUGP("Read data Freecom! (c=%d)\n", count);
+	usb_stor_dbg(us, "Read data Freecom! (c=%d)\n", count);
 
 	/* Issue the transfer command. */
 	result = usb_stor_bulk_transfer_buf (us, opipe, fxfr,
 			FCM_PACKET_LENGTH, NULL);
 	if (result != USB_STOR_XFER_GOOD) {
-		US_DEBUGP ("Freecom readdata transport error\n");
+		usb_stor_dbg(us, "Freecom readdata transport error\n");
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 
 	/* Now transfer all of our blocks. */
-	US_DEBUGP("Start of read\n");
+	usb_stor_dbg(us, "Start of read\n");
 	result = usb_stor_bulk_srb(us, ipipe, srb);
-	US_DEBUGP("freecom_readdata done!\n");
+	usb_stor_dbg(us, "freecom_readdata done!\n");
 
 	if (result > USB_STOR_XFER_SHORT)
 		return USB_STOR_TRANSPORT_ERROR;
@@ -194,21 +194,21 @@ freecom_writedata (struct scsi_cmnd *srb, struct us_data *us,
 	fxfr->Count = cpu_to_le32 (count);
 	memset (fxfr->Pad, 0, sizeof (fxfr->Pad));
 
-	US_DEBUGP("Write data Freecom! (c=%d)\n", count);
+	usb_stor_dbg(us, "Write data Freecom! (c=%d)\n", count);
 
 	/* Issue the transfer command. */
 	result = usb_stor_bulk_transfer_buf (us, opipe, fxfr,
 			FCM_PACKET_LENGTH, NULL);
 	if (result != USB_STOR_XFER_GOOD) {
-		US_DEBUGP ("Freecom writedata transport error\n");
+		usb_stor_dbg(us, "Freecom writedata transport error\n");
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 
 	/* Now transfer all of our blocks. */
-	US_DEBUGP("Start of write\n");
+	usb_stor_dbg(us, "Start of write\n");
 	result = usb_stor_bulk_srb(us, opipe, srb);
 
-	US_DEBUGP("freecom_writedata done!\n");
+	usb_stor_dbg(us, "freecom_writedata done!\n");
 	if (result > USB_STOR_XFER_SHORT)
 		return USB_STOR_TRANSPORT_ERROR;
 	return USB_STOR_TRANSPORT_GOOD;
@@ -230,7 +230,7 @@ static int freecom_transport(struct scsi_cmnd *srb, struct us_data *us)
 	fcb = (struct freecom_cb_wrap *) us->iobuf;
 	fst = (struct freecom_status *) us->iobuf;
 
-	US_DEBUGP("Freecom TRANSPORT STARTED\n");
+	usb_stor_dbg(us, "Freecom TRANSPORT STARTED\n");
 
 	/* Get handles for both transports. */
 	opipe = us->send_bulk_pipe;
@@ -242,7 +242,7 @@ static int freecom_transport(struct scsi_cmnd *srb, struct us_data *us)
 	memcpy (fcb->Atapi, srb->cmnd, 12);
 	memset (fcb->Filler, 0, sizeof (fcb->Filler));
 
-	US_DEBUG(pdump (srb->cmnd, 12));
+	US_DEBUG(pdump(us, srb->cmnd, 12));
 
 	/* Send it out. */
 	result = usb_stor_bulk_transfer_buf (us, opipe, fcb,
@@ -252,7 +252,7 @@ static int freecom_transport(struct scsi_cmnd *srb, struct us_data *us)
 	 * USB land.  It returns the status in its own registers, which
 	 * come back in the bulk pipe. */
 	if (result != USB_STOR_XFER_GOOD) {
-		US_DEBUGP ("freecom transport error\n");
+		usb_stor_dbg(us, "freecom transport error\n");
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 
@@ -260,11 +260,11 @@ static int freecom_transport(struct scsi_cmnd *srb, struct us_data *us)
 	 * doesn't hurt us to always do it now. */
 	result = usb_stor_bulk_transfer_buf (us, ipipe, fst,
 			FCM_STATUS_PACKET_LENGTH, &partial);
-	US_DEBUGP("foo Status result %d %u\n", result, partial);
+	usb_stor_dbg(us, "foo Status result %d %u\n", result, partial);
 	if (result != USB_STOR_XFER_GOOD)
 		return USB_STOR_TRANSPORT_ERROR;
 
-	US_DEBUG(pdump ((void *) fst, partial));
+	US_DEBUG(pdump(us, (void *)fst, partial));
 
 	/* The firmware will time-out commands after 20 seconds. Some commands
 	 * can legitimately take longer than this, so we use a different
@@ -275,8 +275,8 @@ static int freecom_transport(struct scsi_cmnd *srb, struct us_data *us)
 	 * may not work, but that is a condition that should never happen.
 	 */
 	while (fst->Status & FCM_STATUS_BUSY) {
-		US_DEBUGP("20 second USB/ATAPI bridge TIMEOUT occurred!\n");
-		US_DEBUGP("fst->Status is %x\n", fst->Status);
+		usb_stor_dbg(us, "20 second USB/ATAPI bridge TIMEOUT occurred!\n");
+		usb_stor_dbg(us, "fst->Status is %x\n", fst->Status);
 
 		/* Get the status again */
 		fcb->Type = FCM_PACKET_STATUS;
@@ -293,7 +293,7 @@ static int freecom_transport(struct scsi_cmnd *srb, struct us_data *us)
 		 * registers, which come back in the bulk pipe.
 		 */
 		if (result != USB_STOR_XFER_GOOD) {
-			US_DEBUGP ("freecom transport error\n");
+			usb_stor_dbg(us, "freecom transport error\n");
 			return USB_STOR_TRANSPORT_ERROR;
 		}
 
@@ -301,26 +301,26 @@ static int freecom_transport(struct scsi_cmnd *srb, struct us_data *us)
 		result = usb_stor_bulk_transfer_buf (us, ipipe, fst,
 				FCM_STATUS_PACKET_LENGTH, &partial);
 
-		US_DEBUGP("bar Status result %d %u\n", result, partial);
+		usb_stor_dbg(us, "bar Status result %d %u\n", result, partial);
 		if (result != USB_STOR_XFER_GOOD)
 			return USB_STOR_TRANSPORT_ERROR;
 
-		US_DEBUG(pdump ((void *) fst, partial));
+		US_DEBUG(pdump(us, (void *)fst, partial));
 	}
 
 	if (partial != 4)
 		return USB_STOR_TRANSPORT_ERROR;
 	if ((fst->Status & 1) != 0) {
-		US_DEBUGP("operation failed\n");
+		usb_stor_dbg(us, "operation failed\n");
 		return USB_STOR_TRANSPORT_FAILED;
 	}
 
 	/* The device might not have as much data available as we
 	 * requested.  If you ask for more than the device has, this reads
 	 * and such will hang. */
-	US_DEBUGP("Device indicates that it has %d bytes available\n",
-			le16_to_cpu (fst->Count));
-	US_DEBUGP("SCSI requested %d\n", scsi_bufflen(srb));
+	usb_stor_dbg(us, "Device indicates that it has %d bytes available\n",
+		     le16_to_cpu(fst->Count));
+	usb_stor_dbg(us, "SCSI requested %d\n", scsi_bufflen(srb));
 
 	/* Find the length we desire to read. */
 	switch (srb->cmnd[0]) {
@@ -337,7 +337,8 @@ static int freecom_transport(struct scsi_cmnd *srb, struct us_data *us)
 	/* verify that this amount is legal */
 	if (length > scsi_bufflen(srb)) {
 		length = scsi_bufflen(srb);
-		US_DEBUGP("Truncating request to match buffer length: %d\n", length);
+		usb_stor_dbg(us, "Truncating request to match buffer length: %d\n",
+			     length);
 	}
 
 	/* What we do now depends on what direction the data is supposed to
@@ -351,29 +352,29 @@ static int freecom_transport(struct scsi_cmnd *srb, struct us_data *us)
 		/* Make sure that the status indicates that the device
 		 * wants data as well. */
 		if ((fst->Status & DRQ_STAT) == 0 || (fst->Reason & 3) != 2) {
-			US_DEBUGP("SCSI wants data, drive doesn't have any\n");
+			usb_stor_dbg(us, "SCSI wants data, drive doesn't have any\n");
 			return USB_STOR_TRANSPORT_FAILED;
 		}
 		result = freecom_readdata (srb, us, ipipe, opipe, length);
 		if (result != USB_STOR_TRANSPORT_GOOD)
 			return result;
 
-		US_DEBUGP("FCM: Waiting for status\n");
+		usb_stor_dbg(us, "Waiting for status\n");
 		result = usb_stor_bulk_transfer_buf (us, ipipe, fst,
 				FCM_PACKET_LENGTH, &partial);
-		US_DEBUG(pdump ((void *) fst, partial));
+		US_DEBUG(pdump(us, (void *)fst, partial));
 
 		if (partial != 4 || result > USB_STOR_XFER_SHORT)
 			return USB_STOR_TRANSPORT_ERROR;
 		if ((fst->Status & ERR_STAT) != 0) {
-			US_DEBUGP("operation failed\n");
+			usb_stor_dbg(us, "operation failed\n");
 			return USB_STOR_TRANSPORT_FAILED;
 		}
 		if ((fst->Reason & 3) != 3) {
-			US_DEBUGP("Drive seems still hungry\n");
+			usb_stor_dbg(us, "Drive seems still hungry\n");
 			return USB_STOR_TRANSPORT_FAILED;
 		}
-		US_DEBUGP("Transfer happy\n");
+		usb_stor_dbg(us, "Transfer happy\n");
 		break;
 
 	case DMA_TO_DEVICE:
@@ -387,22 +388,22 @@ static int freecom_transport(struct scsi_cmnd *srb, struct us_data *us)
 		if (result != USB_STOR_TRANSPORT_GOOD)
 			return result;
 
-		US_DEBUGP("FCM: Waiting for status\n");
+		usb_stor_dbg(us, "Waiting for status\n");
 		result = usb_stor_bulk_transfer_buf (us, ipipe, fst,
 				FCM_PACKET_LENGTH, &partial);
 
 		if (partial != 4 || result > USB_STOR_XFER_SHORT)
 			return USB_STOR_TRANSPORT_ERROR;
 		if ((fst->Status & ERR_STAT) != 0) {
-			US_DEBUGP("operation failed\n");
+			usb_stor_dbg(us, "operation failed\n");
 			return USB_STOR_TRANSPORT_FAILED;
 		}
 		if ((fst->Reason & 3) != 3) {
-			US_DEBUGP("Drive seems still hungry\n");
+			usb_stor_dbg(us, "Drive seems still hungry\n");
 			return USB_STOR_TRANSPORT_FAILED;
 		}
 
-		US_DEBUGP("Transfer happy\n");
+		usb_stor_dbg(us, "Transfer happy\n");
 		break;
 
 
@@ -412,8 +413,8 @@ static int freecom_transport(struct scsi_cmnd *srb, struct us_data *us)
 
 	default:
 		/* should never hit here -- filtered in usb.c */
-		US_DEBUGP ("freecom unimplemented direction: %d\n",
-				us->srb->sc_data_direction);
+		usb_stor_dbg(us, "freecom unimplemented direction: %d\n",
+			     us->srb->sc_data_direction);
 		/* Return fail, SCSI seems to handle this better. */
 		return USB_STOR_TRANSPORT_FAILED;
 		break;
@@ -434,7 +435,7 @@ static int init_freecom(struct us_data *us)
 	result = usb_stor_control_msg(us, us->recv_ctrl_pipe,
 			0x4c, 0xc0, 0x4346, 0x0, buffer, 0x20, 3*HZ);
 	buffer[32] = '\0';
-	US_DEBUGP("String returned from FC init is: %s\n", buffer);
+	usb_stor_dbg(us, "String returned from FC init is: %s\n", buffer);
 
 	/* Special thanks to the people at Freecom for providing me with
 	 * this "magic sequence", which they use in their Windows and MacOS
@@ -445,7 +446,7 @@ static int init_freecom(struct us_data *us)
 	/* send reset */
 	result = usb_stor_control_msg(us, us->send_ctrl_pipe,
 			0x4d, 0x40, 0x24d8, 0x0, NULL, 0x0, 3*HZ);
-	US_DEBUGP("result from activate reset is %d\n", result);
+	usb_stor_dbg(us, "result from activate reset is %d\n", result);
 
 	/* wait 250ms */
 	mdelay(250);
@@ -453,7 +454,7 @@ static int init_freecom(struct us_data *us)
 	/* clear reset */
 	result = usb_stor_control_msg(us, us->send_ctrl_pipe,
 			0x4d, 0x40, 0x24f8, 0x0, NULL, 0x0, 3*HZ);
-	US_DEBUGP("result from clear reset is %d\n", result);
+	usb_stor_dbg(us, "result from clear reset is %d\n", result);
 
 	/* wait 3 seconds */
 	mdelay(3 * 1000);
@@ -470,7 +471,7 @@ static int usb_stor_freecom_reset(struct us_data *us)
 }
 
 #ifdef CONFIG_USB_STORAGE_DEBUG
-static void pdump (void *ibuffer, int length)
+static void pdump(struct us_data *us, void *ibuffer, int length)
 {
 	static char line[80];
 	int offset = 0;
@@ -490,7 +491,7 @@ static void pdump (void *ibuffer, int length)
 						line[offset++] = '.';
 				}
 				line[offset] = 0;
-				US_DEBUGP("%s\n", line);
+				usb_stor_dbg(us, "%s\n", line);
 				offset = 0;
 			}
 			offset += sprintf (line+offset, "%08x:", i);
@@ -517,7 +518,7 @@ static void pdump (void *ibuffer, int length)
 			line[offset++] = '.';
 	}
 	line[offset] = 0;
-	US_DEBUGP("%s\n", line);
+	usb_stor_dbg(us, "%s\n", line);
 	offset = 0;
 }
 #endif
diff --git a/drivers/usb/storage/initializers.c b/drivers/usb/storage/initializers.c
index 105d900150c..5a8b5ff1e45 100644
--- a/drivers/usb/storage/initializers.c
+++ b/drivers/usb/storage/initializers.c
@@ -48,12 +48,12 @@ int usb_stor_euscsi_init(struct us_data *us)
 {
 	int result;
 
-	US_DEBUGP("Attempting to init eUSCSI bridge...\n");
+	usb_stor_dbg(us, "Attempting to init eUSCSI bridge...\n");
 	us->iobuf[0] = 0x1;
 	result = usb_stor_control_msg(us, us->send_ctrl_pipe,
 			0x0C, USB_RECIP_INTERFACE | USB_TYPE_VENDOR,
 			0x01, 0x0, us->iobuf, 0x1, 5000);
-	US_DEBUGP("-- result is %d\n", result);
+	usb_stor_dbg(us, "-- result is %d\n", result);
 
 	return 0;
 }
@@ -68,7 +68,7 @@ int usb_stor_ucr61s2b_init(struct us_data *us)
 	unsigned int partial;
 	static char init_string[] = "\xec\x0a\x06\x00$PCCHIPS";
 
-	US_DEBUGP("Sending UCR-61S2B initialization packet...\n");
+	usb_stor_dbg(us, "Sending UCR-61S2B initialization packet...\n");
 
 	bcb->Signature = cpu_to_le32(US_BULK_CB_SIGN);
 	bcb->Tag = 0;
@@ -83,7 +83,7 @@ int usb_stor_ucr61s2b_init(struct us_data *us)
 	if (res)
 		return -EIO;
 
-	US_DEBUGP("Getting status packet...\n");
+	usb_stor_dbg(us, "Getting status packet...\n");
 	res = usb_stor_bulk_transfer_buf(us, us->recv_bulk_pipe, bcs,
 			US_BULK_CS_WRAP_LEN, &partial);
 	if (res)
@@ -101,6 +101,6 @@ int usb_stor_huawei_e220_init(struct us_data *us)
 				      USB_REQ_SET_FEATURE,
 				      USB_TYPE_STANDARD | USB_RECIP_DEVICE,
 				      0x01, 0x0, NULL, 0x0, 1000);
-	US_DEBUGP("Huawei mode set result is %d\n", result);
+	usb_stor_dbg(us, "Huawei mode set result is %d\n", result);
 	return 0;
 }
diff --git a/drivers/usb/storage/isd200.c b/drivers/usb/storage/isd200.c
index ecea4787736..599d8bff26c 100644
--- a/drivers/usb/storage/isd200.c
+++ b/drivers/usb/storage/isd200.c
@@ -503,7 +503,7 @@ static int isd200_action( struct us_data *us, int action,
 
 	switch ( action ) {
 	case ACTION_READ_STATUS:
-		US_DEBUGP("   isd200_action(READ_STATUS)\n");
+		usb_stor_dbg(us, "   isd200_action(READ_STATUS)\n");
 		ata.generic.ActionSelect = ACTION_SELECT_0|ACTION_SELECT_2;
 		ata.generic.RegisterSelect =
 		  REG_CYLINDER_LOW | REG_CYLINDER_HIGH |
@@ -512,7 +512,7 @@ static int isd200_action( struct us_data *us, int action,
 		break;
 
 	case ACTION_ENUM:
-		US_DEBUGP("   isd200_action(ENUM,0x%02x)\n",value);
+		usb_stor_dbg(us, "   isd200_action(ENUM,0x%02x)\n", value);
 		ata.generic.ActionSelect = ACTION_SELECT_1|ACTION_SELECT_2|
 					   ACTION_SELECT_3|ACTION_SELECT_4|
 					   ACTION_SELECT_5;
@@ -522,7 +522,7 @@ static int isd200_action( struct us_data *us, int action,
 		break;
 
 	case ACTION_RESET:
-		US_DEBUGP("   isd200_action(RESET)\n");
+		usb_stor_dbg(us, "   isd200_action(RESET)\n");
 		ata.generic.ActionSelect = ACTION_SELECT_1|ACTION_SELECT_2|
 					   ACTION_SELECT_3|ACTION_SELECT_4;
 		ata.generic.RegisterSelect = REG_DEVICE_CONTROL;
@@ -531,7 +531,7 @@ static int isd200_action( struct us_data *us, int action,
 		break;
 
 	case ACTION_REENABLE:
-		US_DEBUGP("   isd200_action(REENABLE)\n");
+		usb_stor_dbg(us, "   isd200_action(REENABLE)\n");
 		ata.generic.ActionSelect = ACTION_SELECT_1|ACTION_SELECT_2|
 					   ACTION_SELECT_3|ACTION_SELECT_4;
 		ata.generic.RegisterSelect = REG_DEVICE_CONTROL;
@@ -540,7 +540,7 @@ static int isd200_action( struct us_data *us, int action,
 		break;
 
 	case ACTION_SOFT_RESET:
-		US_DEBUGP("   isd200_action(SOFT_RESET)\n");
+		usb_stor_dbg(us, "   isd200_action(SOFT_RESET)\n");
 		ata.generic.ActionSelect = ACTION_SELECT_1|ACTION_SELECT_5;
 		ata.generic.RegisterSelect = REG_DEVICE_HEAD | REG_COMMAND;
 		ata.write.DeviceHeadByte = info->DeviceHead;
@@ -549,7 +549,7 @@ static int isd200_action( struct us_data *us, int action,
 		break;
 
 	case ACTION_IDENTIFY:
-		US_DEBUGP("   isd200_action(IDENTIFY)\n");
+		usb_stor_dbg(us, "   isd200_action(IDENTIFY)\n");
 		ata.generic.RegisterSelect = REG_COMMAND;
 		ata.write.CommandByte = ATA_CMD_ID_ATA;
 		isd200_set_srb(info, DMA_FROM_DEVICE, info->id,
@@ -557,7 +557,7 @@ static int isd200_action( struct us_data *us, int action,
 		break;
 
 	default:
-		US_DEBUGP("Error: Undefined action %d\n",action);
+		usb_stor_dbg(us, "Error: Undefined action %d\n", action);
 		return ISD200_ERROR;
 	}
 
@@ -567,7 +567,8 @@ static int isd200_action( struct us_data *us, int action,
 	if (status == USB_STOR_TRANSPORT_GOOD)
 		status = ISD200_GOOD;
 	else {
-		US_DEBUGP("   isd200_action(0x%02x) error: %d\n",action,status);
+		usb_stor_dbg(us, "   isd200_action(0x%02x) error: %d\n",
+			     action, status);
 		status = ISD200_ERROR;
 		/* need to reset device here */
 	}
@@ -589,17 +590,17 @@ static int isd200_read_regs( struct us_data *us )
 	int retStatus = ISD200_GOOD;
 	int transferStatus;
 
-	US_DEBUGP("Entering isd200_IssueATAReadRegs\n");
+	usb_stor_dbg(us, "Entering isd200_IssueATAReadRegs\n");
 
 	transferStatus = isd200_action( us, ACTION_READ_STATUS,
 				    info->RegsBuf, sizeof(info->ATARegs) );
 	if (transferStatus != ISD200_TRANSPORT_GOOD) {
-		US_DEBUGP("   Error reading ATA registers\n");
+		usb_stor_dbg(us, "   Error reading ATA registers\n");
 		retStatus = ISD200_ERROR;
 	} else {
 		memcpy(info->ATARegs, info->RegsBuf, sizeof(info->ATARegs));
-		US_DEBUGP("   Got ATA Register[ATA_REG_ERROR_OFFSET] = 0x%x\n",
-			  info->ATARegs[ATA_REG_ERROR_OFFSET]);
+		usb_stor_dbg(us, "   Got ATA Register[ATA_REG_ERROR_OFFSET] = 0x%x\n",
+			     info->ATARegs[ATA_REG_ERROR_OFFSET]);
 	}
 
 	return retStatus;
@@ -629,7 +630,7 @@ static void isd200_invoke_transport( struct us_data *us,
 	 * short-circuit all other processing
 	 */
 	if (test_bit(US_FLIDX_TIMED_OUT, &us->dflags)) {
-		US_DEBUGP("-- command was aborted\n");
+		usb_stor_dbg(us, "-- command was aborted\n");
 		goto Handle_Abort;
 	}
 
@@ -641,23 +642,23 @@ static void isd200_invoke_transport( struct us_data *us,
 		break;
 
 	case USB_STOR_TRANSPORT_NO_SENSE:
-		US_DEBUGP("-- transport indicates protocol failure\n");
+		usb_stor_dbg(us, "-- transport indicates protocol failure\n");
 		srb->result = SAM_STAT_CHECK_CONDITION;
 		return;
 
 	case USB_STOR_TRANSPORT_FAILED:
-		US_DEBUGP("-- transport indicates command failure\n");
+		usb_stor_dbg(us, "-- transport indicates command failure\n");
 		need_auto_sense = 1;
 		break;
 
 	case USB_STOR_TRANSPORT_ERROR:
-		US_DEBUGP("-- transport indicates transport error\n");
+		usb_stor_dbg(us, "-- transport indicates transport error\n");
 		srb->result = DID_ERROR << 16;
 		/* Need reset here */
 		return;
     
 	default:
-		US_DEBUGP("-- transport indicates unknown error\n");   
+		usb_stor_dbg(us, "-- transport indicates unknown error\n");
 		srb->result = DID_ERROR << 16;
 		/* Need reset here */
 		return;
@@ -669,14 +670,14 @@ static void isd200_invoke_transport( struct us_data *us,
 	      (srb->cmnd[0] == MODE_SENSE) ||
 	      (srb->cmnd[0] == LOG_SENSE) ||
 	      (srb->cmnd[0] == MODE_SENSE_10))) {
-		US_DEBUGP("-- unexpectedly short transfer\n");
+		usb_stor_dbg(us, "-- unexpectedly short transfer\n");
 		need_auto_sense = 1;
 	}
 
 	if (need_auto_sense) {
 		result = isd200_read_regs(us);
 		if (test_bit(US_FLIDX_TIMED_OUT, &us->dflags)) {
-			US_DEBUGP("-- auto-sense aborted\n");
+			usb_stor_dbg(us, "-- auto-sense aborted\n");
 			goto Handle_Abort;
 		}
 		if (result == ISD200_GOOD) {
@@ -710,40 +711,40 @@ static void isd200_invoke_transport( struct us_data *us,
 }
 
 #ifdef CONFIG_USB_STORAGE_DEBUG
-static void isd200_log_config( struct isd200_info* info )
+static void isd200_log_config(struct us_data *us, struct isd200_info *info)
 {
-	US_DEBUGP("      Event Notification: 0x%x\n", 
-		  info->ConfigData.EventNotification);
-	US_DEBUGP("      External Clock: 0x%x\n", 
-		  info->ConfigData.ExternalClock);
-	US_DEBUGP("      ATA Init Timeout: 0x%x\n", 
-		  info->ConfigData.ATAInitTimeout);
-	US_DEBUGP("      ATAPI Command Block Size: 0x%x\n", 
-		  (info->ConfigData.ATAConfig & ATACFG_BLOCKSIZE) >> 6);
-	US_DEBUGP("      Master/Slave Selection: 0x%x\n", 
-		  info->ConfigData.ATAConfig & ATACFG_MASTER);
-	US_DEBUGP("      ATAPI Reset: 0x%x\n",
-		  info->ConfigData.ATAConfig & ATACFG_ATAPI_RESET);
-	US_DEBUGP("      ATA Timing: 0x%x\n",
-		  info->ConfigData.ATAConfig & ATACFG_TIMING);
-	US_DEBUGP("      ATA Major Command: 0x%x\n", 
-		  info->ConfigData.ATAMajorCommand);
-	US_DEBUGP("      ATA Minor Command: 0x%x\n", 
-		  info->ConfigData.ATAMinorCommand);
-	US_DEBUGP("      Init Status: 0x%x\n", 
-		  info->ConfigData.ATAExtraConfig & ATACFGE_INIT_STATUS);
-	US_DEBUGP("      Config Descriptor 2: 0x%x\n", 
-		  info->ConfigData.ATAExtraConfig & ATACFGE_CONF_DESC2);
-	US_DEBUGP("      Skip Device Boot: 0x%x\n",
-		  info->ConfigData.ATAExtraConfig & ATACFGE_SKIP_BOOT);
-	US_DEBUGP("      ATA 3 State Supsend: 0x%x\n",
-		  info->ConfigData.ATAExtraConfig & ATACFGE_STATE_SUSPEND);
-	US_DEBUGP("      Descriptor Override: 0x%x\n", 
-		  info->ConfigData.ATAExtraConfig & ATACFGE_DESC_OVERRIDE);
-	US_DEBUGP("      Last LUN Identifier: 0x%x\n",
-		  info->ConfigData.ATAExtraConfig & ATACFGE_LAST_LUN);
-	US_DEBUGP("      SRST Enable: 0x%x\n", 
-		  info->ConfigData.ATAExtraConfig & CFG_CAPABILITY_SRST);
+	usb_stor_dbg(us, "      Event Notification: 0x%x\n",
+		     info->ConfigData.EventNotification);
+	usb_stor_dbg(us, "      External Clock: 0x%x\n",
+		     info->ConfigData.ExternalClock);
+	usb_stor_dbg(us, "      ATA Init Timeout: 0x%x\n",
+		     info->ConfigData.ATAInitTimeout);
+	usb_stor_dbg(us, "      ATAPI Command Block Size: 0x%x\n",
+		     (info->ConfigData.ATAConfig & ATACFG_BLOCKSIZE) >> 6);
+	usb_stor_dbg(us, "      Master/Slave Selection: 0x%x\n",
+		     info->ConfigData.ATAConfig & ATACFG_MASTER);
+	usb_stor_dbg(us, "      ATAPI Reset: 0x%x\n",
+		     info->ConfigData.ATAConfig & ATACFG_ATAPI_RESET);
+	usb_stor_dbg(us, "      ATA Timing: 0x%x\n",
+		     info->ConfigData.ATAConfig & ATACFG_TIMING);
+	usb_stor_dbg(us, "      ATA Major Command: 0x%x\n",
+		     info->ConfigData.ATAMajorCommand);
+	usb_stor_dbg(us, "      ATA Minor Command: 0x%x\n",
+		     info->ConfigData.ATAMinorCommand);
+	usb_stor_dbg(us, "      Init Status: 0x%x\n",
+		     info->ConfigData.ATAExtraConfig & ATACFGE_INIT_STATUS);
+	usb_stor_dbg(us, "      Config Descriptor 2: 0x%x\n",
+		     info->ConfigData.ATAExtraConfig & ATACFGE_CONF_DESC2);
+	usb_stor_dbg(us, "      Skip Device Boot: 0x%x\n",
+		     info->ConfigData.ATAExtraConfig & ATACFGE_SKIP_BOOT);
+	usb_stor_dbg(us, "      ATA 3 State Supsend: 0x%x\n",
+		     info->ConfigData.ATAExtraConfig & ATACFGE_STATE_SUSPEND);
+	usb_stor_dbg(us, "      Descriptor Override: 0x%x\n",
+		     info->ConfigData.ATAExtraConfig & ATACFGE_DESC_OVERRIDE);
+	usb_stor_dbg(us, "      Last LUN Identifier: 0x%x\n",
+		     info->ConfigData.ATAExtraConfig & ATACFGE_LAST_LUN);
+	usb_stor_dbg(us, "      SRST Enable: 0x%x\n",
+		     info->ConfigData.ATAExtraConfig & CFG_CAPABILITY_SRST);
 }
 #endif
 
@@ -762,9 +763,9 @@ static int isd200_write_config( struct us_data *us )
 	int result;
 
 #ifdef CONFIG_USB_STORAGE_DEBUG
-	US_DEBUGP("Entering isd200_write_config\n");
-	US_DEBUGP("   Writing the following ISD200 Config Data:\n");
-	isd200_log_config(info);
+	usb_stor_dbg(us, "Entering isd200_write_config\n");
+	usb_stor_dbg(us, "   Writing the following ISD200 Config Data:\n");
+	isd200_log_config(us, info);
 #endif
 
 	/* let's send the command via the control pipe */
@@ -779,13 +780,13 @@ static int isd200_write_config( struct us_data *us )
 		sizeof(info->ConfigData));
 
 	if (result >= 0) {
-		US_DEBUGP("   ISD200 Config Data was written successfully\n");
+		usb_stor_dbg(us, "   ISD200 Config Data was written successfully\n");
 	} else {
-		US_DEBUGP("   Request to write ISD200 Config Data failed!\n");
+		usb_stor_dbg(us, "   Request to write ISD200 Config Data failed!\n");
 		retStatus = ISD200_ERROR;
 	}
 
-	US_DEBUGP("Leaving isd200_write_config %08X\n", retStatus);
+	usb_stor_dbg(us, "Leaving isd200_write_config %08X\n", retStatus);
 	return retStatus;
 }
 
@@ -804,7 +805,7 @@ static int isd200_read_config( struct us_data *us )
 	int retStatus = ISD200_GOOD;
 	int result;
 
-	US_DEBUGP("Entering isd200_read_config\n");
+	usb_stor_dbg(us, "Entering isd200_read_config\n");
 
 	/* read the configuration information from ISD200.  Use this to */
 	/* determine what the special ATA CDB bytes are.		*/
@@ -821,16 +822,16 @@ static int isd200_read_config( struct us_data *us )
 
 
 	if (result >= 0) {
-		US_DEBUGP("   Retrieved the following ISD200 Config Data:\n");
+		usb_stor_dbg(us, "   Retrieved the following ISD200 Config Data:\n");
 #ifdef CONFIG_USB_STORAGE_DEBUG
-		isd200_log_config(info);
+		isd200_log_config(us, info);
 #endif
 	} else {
-		US_DEBUGP("   Request to get ISD200 Config Data failed!\n");
+		usb_stor_dbg(us, "   Request to get ISD200 Config Data failed!\n");
 		retStatus = ISD200_ERROR;
 	}
 
-	US_DEBUGP("Leaving isd200_read_config %08X\n", retStatus);
+	usb_stor_dbg(us, "Leaving isd200_read_config %08X\n", retStatus);
 	return retStatus;
 }
 
@@ -848,15 +849,15 @@ static int isd200_atapi_soft_reset( struct us_data *us )
 	int retStatus = ISD200_GOOD;
 	int transferStatus;
 
-	US_DEBUGP("Entering isd200_atapi_soft_reset\n");
+	usb_stor_dbg(us, "Entering isd200_atapi_soft_reset\n");
 
 	transferStatus = isd200_action( us, ACTION_SOFT_RESET, NULL, 0 );
 	if (transferStatus != ISD200_TRANSPORT_GOOD) {
-		US_DEBUGP("   Error issuing Atapi Soft Reset\n");
+		usb_stor_dbg(us, "   Error issuing Atapi Soft Reset\n");
 		retStatus = ISD200_ERROR;
 	}
 
-	US_DEBUGP("Leaving isd200_atapi_soft_reset %08X\n", retStatus);
+	usb_stor_dbg(us, "Leaving isd200_atapi_soft_reset %08X\n", retStatus);
 	return retStatus;
 }
 
@@ -874,13 +875,13 @@ static int isd200_srst( struct us_data *us )
 	int retStatus = ISD200_GOOD;
 	int transferStatus;
 
-	US_DEBUGP("Entering isd200_SRST\n");
+	usb_stor_dbg(us, "Entering isd200_SRST\n");
 
 	transferStatus = isd200_action( us, ACTION_RESET, NULL, 0 );
 
 	/* check to see if this request failed */
 	if (transferStatus != ISD200_TRANSPORT_GOOD) {
-		US_DEBUGP("   Error issuing SRST\n");
+		usb_stor_dbg(us, "   Error issuing SRST\n");
 		retStatus = ISD200_ERROR;
 	} else {
 		/* delay 10ms to give the drive a chance to see it */
@@ -888,7 +889,7 @@ static int isd200_srst( struct us_data *us )
 
 		transferStatus = isd200_action( us, ACTION_REENABLE, NULL, 0 );
 		if (transferStatus != ISD200_TRANSPORT_GOOD) {
-			US_DEBUGP("   Error taking drive out of reset\n");
+			usb_stor_dbg(us, "   Error taking drive out of reset\n");
 			retStatus = ISD200_ERROR;
 		} else {
 			/* delay 50ms to give the drive a chance to recover after SRST */
@@ -896,7 +897,7 @@ static int isd200_srst( struct us_data *us )
 		}
 	}
 
-	US_DEBUGP("Leaving isd200_srst %08X\n", retStatus);
+	usb_stor_dbg(us, "Leaving isd200_srst %08X\n", retStatus);
 	return retStatus;
 }
 
@@ -926,10 +927,6 @@ static int isd200_try_enum(struct us_data *us, unsigned char master_slave,
 
 	/* loop until we detect !BSY or timeout */
 	while(1) {
-#ifdef CONFIG_USB_STORAGE_DEBUG
-		char* mstr = master_slave == ATA_ADDRESS_DEVHEAD_STD ?
-			"Master" : "Slave";
-#endif
 
 		status = isd200_action( us, ACTION_ENUM, NULL, master_slave );
 		if ( status != ISD200_GOOD )
@@ -942,9 +939,13 @@ static int isd200_try_enum(struct us_data *us, unsigned char master_slave,
 
 		if (!detect) {
 			if (regs[ATA_REG_STATUS_OFFSET] & ATA_BUSY) {
-				US_DEBUGP("   %s status is still BSY, try again...\n",mstr);
+				usb_stor_dbg(us, "   %s status is still BSY, try again...\n",
+					     master_slave == ATA_ADDRESS_DEVHEAD_STD ?
+					     "Master" : "Slave");
 			} else {
-				US_DEBUGP("   %s status !BSY, continue with next operation\n",mstr);
+				usb_stor_dbg(us, "   %s status !BSY, continue with next operation\n",
+					     master_slave == ATA_ADDRESS_DEVHEAD_STD ?
+					     "Master" : "Slave");
 				break;
 			}
 		}
@@ -953,11 +954,11 @@ static int isd200_try_enum(struct us_data *us, unsigned char master_slave,
 		/* ATA_ERR (workaround for Archos CD-ROM) */
 		else if (regs[ATA_REG_STATUS_OFFSET] &
 			 (ATA_BUSY | ATA_DF | ATA_ERR)) {
-			US_DEBUGP("   Status indicates it is not ready, try again...\n");
+			usb_stor_dbg(us, "   Status indicates it is not ready, try again...\n");
 		}
 		/* check for DRDY, ATA devices set DRDY after SRST */
 		else if (regs[ATA_REG_STATUS_OFFSET] & ATA_DRDY) {
-			US_DEBUGP("   Identified ATA device\n");
+			usb_stor_dbg(us, "   Identified ATA device\n");
 			info->DeviceFlags |= DF_ATA_DEVICE;
 			info->DeviceHead = master_slave;
 			break;
@@ -978,27 +979,27 @@ static int isd200_try_enum(struct us_data *us, unsigned char master_slave,
 			*/
 			if ((master_slave & ATA_ADDRESS_DEVHEAD_SLAVE) &&
 			    !recheckAsMaster) {
-				US_DEBUGP("   Identified ATAPI device as slave.  Rechecking again as master\n");
+				usb_stor_dbg(us, "   Identified ATAPI device as slave.  Rechecking again as master\n");
 				recheckAsMaster = 1;
 				master_slave = ATA_ADDRESS_DEVHEAD_STD;
 			} else {
-				US_DEBUGP("   Identified ATAPI device\n");
+				usb_stor_dbg(us, "   Identified ATAPI device\n");
 				info->DeviceHead = master_slave;
 			      
 				status = isd200_atapi_soft_reset(us);
 				break;
 			}
 		} else {
- 			US_DEBUGP("   Not ATA, not ATAPI. Weird.\n");
+			usb_stor_dbg(us, "   Not ATA, not ATAPI - Weird\n");
 			break;
 		}
 
 		/* check for timeout on this request */
 		if (time_after_eq(jiffies, endTime)) {
 			if (!detect)
-				US_DEBUGP("   BSY check timeout, just continue with next operation...\n");
+				usb_stor_dbg(us, "   BSY check timeout, just continue with next operation...\n");
 			else
-				US_DEBUGP("   Device detect timeout!\n");
+				usb_stor_dbg(us, "   Device detect timeout!\n");
 			break;
 		}
 	}
@@ -1020,7 +1021,7 @@ static int isd200_manual_enum(struct us_data *us)
 	struct isd200_info *info = (struct isd200_info *)us->extra;
 	int retStatus = ISD200_GOOD;
 
-	US_DEBUGP("Entering isd200_manual_enum\n");
+	usb_stor_dbg(us, "Entering isd200_manual_enum\n");
 
 	retStatus = isd200_read_config(us);
 	if (retStatus == ISD200_GOOD) {
@@ -1039,14 +1040,15 @@ static int isd200_manual_enum(struct us_data *us)
 
 		isslave = (info->DeviceHead & ATA_ADDRESS_DEVHEAD_SLAVE) ? 1 : 0;
 		if (!(info->ConfigData.ATAConfig & ATACFG_MASTER)) {
-			US_DEBUGP("   Setting Master/Slave selection to %d\n", isslave);
+			usb_stor_dbg(us, "   Setting Master/Slave selection to %d\n",
+				     isslave);
 			info->ConfigData.ATAConfig &= 0x3f;
 			info->ConfigData.ATAConfig |= (isslave<<6);
 			retStatus = isd200_write_config(us);
 		}
 	}
 
-	US_DEBUGP("Leaving isd200_manual_enum %08X\n", retStatus);
+	usb_stor_dbg(us, "Leaving isd200_manual_enum %08X\n", retStatus);
 	return(retStatus);
 }
 
@@ -1064,35 +1066,35 @@ static void isd200_fix_driveid(u16 *id)
 #endif
 }
 
-static void isd200_dump_driveid(u16 *id)
+static void isd200_dump_driveid(struct us_data *us, u16 *id)
 {
-	US_DEBUGP("   Identify Data Structure:\n");
-	US_DEBUGP("      config = 0x%x\n",	  id[ATA_ID_CONFIG]);
-	US_DEBUGP("      cyls = 0x%x\n",	  id[ATA_ID_CYLS]);
-	US_DEBUGP("      heads = 0x%x\n",	  id[ATA_ID_HEADS]);
-	US_DEBUGP("      track_bytes = 0x%x\n",	  id[4]);
-	US_DEBUGP("      sector_bytes = 0x%x\n",  id[5]);
-	US_DEBUGP("      sectors = 0x%x\n",	  id[ATA_ID_SECTORS]);
-	US_DEBUGP("      serial_no[0] = 0x%x\n",  *(char *)&id[ATA_ID_SERNO]);
-	US_DEBUGP("      buf_type = 0x%x\n",	  id[20]);
-	US_DEBUGP("      buf_size = 0x%x\n",	  id[ATA_ID_BUF_SIZE]);
-	US_DEBUGP("      ecc_bytes = 0x%x\n",	  id[22]);
-	US_DEBUGP("      fw_rev[0] = 0x%x\n",	  *(char *)&id[ATA_ID_FW_REV]);
-	US_DEBUGP("      model[0] = 0x%x\n",	  *(char *)&id[ATA_ID_PROD]);
-	US_DEBUGP("      max_multsect = 0x%x\n",  id[ATA_ID_MAX_MULTSECT] & 0xff);
-	US_DEBUGP("      dword_io = 0x%x\n",	  id[ATA_ID_DWORD_IO]);
-	US_DEBUGP("      capability = 0x%x\n",	  id[ATA_ID_CAPABILITY] >> 8);
-	US_DEBUGP("      tPIO = 0x%x\n",	  id[ATA_ID_OLD_PIO_MODES] >> 8);
-	US_DEBUGP("      tDMA = 0x%x\n",	  id[ATA_ID_OLD_DMA_MODES] >> 8);
-	US_DEBUGP("      field_valid = 0x%x\n",	  id[ATA_ID_FIELD_VALID]);
-	US_DEBUGP("      cur_cyls = 0x%x\n",	  id[ATA_ID_CUR_CYLS]);
-	US_DEBUGP("      cur_heads = 0x%x\n",	  id[ATA_ID_CUR_HEADS]);
-	US_DEBUGP("      cur_sectors = 0x%x\n",	  id[ATA_ID_CUR_SECTORS]);
-	US_DEBUGP("      cur_capacity = 0x%x\n",  ata_id_u32(id, 57));
-	US_DEBUGP("      multsect = 0x%x\n",	  id[ATA_ID_MULTSECT] & 0xff);
-	US_DEBUGP("      lba_capacity = 0x%x\n",  ata_id_u32(id, ATA_ID_LBA_CAPACITY));
-	US_DEBUGP("      command_set_1 = 0x%x\n", id[ATA_ID_COMMAND_SET_1]);
-	US_DEBUGP("      command_set_2 = 0x%x\n", id[ATA_ID_COMMAND_SET_2]);
+	usb_stor_dbg(us, "   Identify Data Structure:\n");
+	usb_stor_dbg(us, "      config = 0x%x\n",	id[ATA_ID_CONFIG]);
+	usb_stor_dbg(us, "      cyls = 0x%x\n",		id[ATA_ID_CYLS]);
+	usb_stor_dbg(us, "      heads = 0x%x\n",	id[ATA_ID_HEADS]);
+	usb_stor_dbg(us, "      track_bytes = 0x%x\n",	id[4]);
+	usb_stor_dbg(us, "      sector_bytes = 0x%x\n", id[5]);
+	usb_stor_dbg(us, "      sectors = 0x%x\n",	id[ATA_ID_SECTORS]);
+	usb_stor_dbg(us, "      serial_no[0] = 0x%x\n", *(char *)&id[ATA_ID_SERNO]);
+	usb_stor_dbg(us, "      buf_type = 0x%x\n",	id[20]);
+	usb_stor_dbg(us, "      buf_size = 0x%x\n",	id[ATA_ID_BUF_SIZE]);
+	usb_stor_dbg(us, "      ecc_bytes = 0x%x\n",	id[22]);
+	usb_stor_dbg(us, "      fw_rev[0] = 0x%x\n",	*(char *)&id[ATA_ID_FW_REV]);
+	usb_stor_dbg(us, "      model[0] = 0x%x\n",	*(char *)&id[ATA_ID_PROD]);
+	usb_stor_dbg(us, "      max_multsect = 0x%x\n", id[ATA_ID_MAX_MULTSECT] & 0xff);
+	usb_stor_dbg(us, "      dword_io = 0x%x\n",	id[ATA_ID_DWORD_IO]);
+	usb_stor_dbg(us, "      capability = 0x%x\n",	id[ATA_ID_CAPABILITY] >> 8);
+	usb_stor_dbg(us, "      tPIO = 0x%x\n",	  id[ATA_ID_OLD_PIO_MODES] >> 8);
+	usb_stor_dbg(us, "      tDMA = 0x%x\n",	  id[ATA_ID_OLD_DMA_MODES] >> 8);
+	usb_stor_dbg(us, "      field_valid = 0x%x\n",	id[ATA_ID_FIELD_VALID]);
+	usb_stor_dbg(us, "      cur_cyls = 0x%x\n",	id[ATA_ID_CUR_CYLS]);
+	usb_stor_dbg(us, "      cur_heads = 0x%x\n",	id[ATA_ID_CUR_HEADS]);
+	usb_stor_dbg(us, "      cur_sectors = 0x%x\n",	id[ATA_ID_CUR_SECTORS]);
+	usb_stor_dbg(us, "      cur_capacity = 0x%x\n", ata_id_u32(id, 57));
+	usb_stor_dbg(us, "      multsect = 0x%x\n",	id[ATA_ID_MULTSECT] & 0xff);
+	usb_stor_dbg(us, "      lba_capacity = 0x%x\n", ata_id_u32(id, ATA_ID_LBA_CAPACITY));
+	usb_stor_dbg(us, "      command_set_1 = 0x%x\n", id[ATA_ID_COMMAND_SET_1]);
+	usb_stor_dbg(us, "      command_set_2 = 0x%x\n", id[ATA_ID_COMMAND_SET_2]);
 }
 
 /**************************************************************************
@@ -1109,7 +1111,7 @@ static int isd200_get_inquiry_data( struct us_data *us )
 	int retStatus = ISD200_GOOD;
 	u16 *id = info->id;
 
-	US_DEBUGP("Entering isd200_get_inquiry_data\n");
+	usb_stor_dbg(us, "Entering isd200_get_inquiry_data\n");
 
 	/* set default to Master */
 	info->DeviceHead = ATA_ADDRESS_DEVHEAD_STD;
@@ -1127,7 +1129,7 @@ static int isd200_get_inquiry_data( struct us_data *us )
 							id, ATA_ID_WORDS * 2);
 			if (transferStatus != ISD200_TRANSPORT_GOOD) {
 				/* Error issuing ATA Command Identify */
-				US_DEBUGP("   Error issuing ATA Command Identify\n");
+				usb_stor_dbg(us, "   Error issuing ATA Command Identify\n");
 				retStatus = ISD200_ERROR;
 			} else {
 				/* ATA Command Identify successful */
@@ -1136,7 +1138,7 @@ static int isd200_get_inquiry_data( struct us_data *us )
 				__u16 *dest;
 
 				isd200_fix_driveid(id);
-				isd200_dump_driveid(id);
+				isd200_dump_driveid(us, id);
 
 				memset(&info->InquiryData, 0, sizeof(info->InquiryData));
 
@@ -1170,7 +1172,7 @@ static int isd200_get_inquiry_data( struct us_data *us )
 
 				/* determine if it supports Media Status Notification */
 				if (id[ATA_ID_COMMAND_SET_2] & COMMANDSET_MEDIA_STATUS) {
-					US_DEBUGP("   Device supports Media Status Notification\n");
+					usb_stor_dbg(us, "   Device supports Media Status Notification\n");
 
 					/* Indicate that it is enabled, even though it is not
 					 * This allows the lock/unlock of the media to work
@@ -1190,7 +1192,8 @@ static int isd200_get_inquiry_data( struct us_data *us )
 			us->protocol_name = "Transparent SCSI";
 			us->proto_handler = usb_stor_transparent_scsi_command;
 
-			US_DEBUGP("Protocol changed to: %s\n", us->protocol_name);
+			usb_stor_dbg(us, "Protocol changed to: %s\n",
+				     us->protocol_name);
 	    
 			/* Free driver structure */	    
 			us->extra_destructor(info);
@@ -1200,7 +1203,7 @@ static int isd200_get_inquiry_data( struct us_data *us )
 		}
 	}
 
-	US_DEBUGP("Leaving isd200_get_inquiry_data %08X\n", retStatus);
+	usb_stor_dbg(us, "Leaving isd200_get_inquiry_data %08X\n", retStatus);
 
 	return(retStatus);
 }
@@ -1231,7 +1234,7 @@ static int isd200_scsi_to_ata(struct scsi_cmnd *srb, struct us_data *us,
 	/* SCSI Command */
 	switch (srb->cmnd[0]) {
 	case INQUIRY:
-		US_DEBUGP("   ATA OUT - INQUIRY\n");
+		usb_stor_dbg(us, "   ATA OUT - INQUIRY\n");
 
 		/* copy InquiryData */
 		usb_stor_set_xfer_buf((unsigned char *) &info->InquiryData,
@@ -1241,7 +1244,7 @@ static int isd200_scsi_to_ata(struct scsi_cmnd *srb, struct us_data *us,
 		break;
 
 	case MODE_SENSE:
-		US_DEBUGP("   ATA OUT - SCSIOP_MODE_SENSE\n");
+		usb_stor_dbg(us, "   ATA OUT - SCSIOP_MODE_SENSE\n");
 
 		/* Initialize the return buffer */
 		usb_stor_set_xfer_buf(senseData, sizeof(senseData), srb);
@@ -1255,14 +1258,14 @@ static int isd200_scsi_to_ata(struct scsi_cmnd *srb, struct us_data *us,
 			ataCdb->write.CommandByte = ATA_COMMAND_GET_MEDIA_STATUS;
 			isd200_srb_set_bufflen(srb, 0);
 		} else {
-			US_DEBUGP("   Media Status not supported, just report okay\n");
+			usb_stor_dbg(us, "   Media Status not supported, just report okay\n");
 			srb->result = SAM_STAT_GOOD;
 			sendToTransport = 0;
 		}
 		break;
 
 	case TEST_UNIT_READY:
-		US_DEBUGP("   ATA OUT - SCSIOP_TEST_UNIT_READY\n");
+		usb_stor_dbg(us, "   ATA OUT - SCSIOP_TEST_UNIT_READY\n");
 
 		if (info->DeviceFlags & DF_MEDIA_STATUS_ENABLED)
 		{
@@ -1273,7 +1276,7 @@ static int isd200_scsi_to_ata(struct scsi_cmnd *srb, struct us_data *us,
 			ataCdb->write.CommandByte = ATA_COMMAND_GET_MEDIA_STATUS;
 			isd200_srb_set_bufflen(srb, 0);
 		} else {
-			US_DEBUGP("   Media Status not supported, just report okay\n");
+			usb_stor_dbg(us, "   Media Status not supported, just report okay\n");
 			srb->result = SAM_STAT_GOOD;
 			sendToTransport = 0;
 		}
@@ -1284,7 +1287,7 @@ static int isd200_scsi_to_ata(struct scsi_cmnd *srb, struct us_data *us,
 		unsigned long capacity;
 		struct read_capacity_data readCapacityData;
 
-		US_DEBUGP("   ATA OUT - SCSIOP_READ_CAPACITY\n");
+		usb_stor_dbg(us, "   ATA OUT - SCSIOP_READ_CAPACITY\n");
 
 		if (ata_id_has_lba(id))
 			capacity = ata_id_u32(id, ATA_ID_LBA_CAPACITY) - 1;
@@ -1303,7 +1306,7 @@ static int isd200_scsi_to_ata(struct scsi_cmnd *srb, struct us_data *us,
 	break;
 
 	case READ_10:
-		US_DEBUGP("   ATA OUT - SCSIOP_READ\n");
+		usb_stor_dbg(us, "   ATA OUT - SCSIOP_READ\n");
 
 		lba = be32_to_cpu(*(__be32 *)&srb->cmnd[2]);
 		blockCount = (unsigned long)srb->cmnd[7]<<8 | (unsigned long)srb->cmnd[8];
@@ -1335,7 +1338,7 @@ static int isd200_scsi_to_ata(struct scsi_cmnd *srb, struct us_data *us,
 		break;
 
 	case WRITE_10:
-		US_DEBUGP("   ATA OUT - SCSIOP_WRITE\n");
+		usb_stor_dbg(us, "   ATA OUT - SCSIOP_WRITE\n");
 
 		lba = be32_to_cpu(*(__be32 *)&srb->cmnd[2]);
 		blockCount = (unsigned long)srb->cmnd[7]<<8 | (unsigned long)srb->cmnd[8];
@@ -1367,10 +1370,11 @@ static int isd200_scsi_to_ata(struct scsi_cmnd *srb, struct us_data *us,
 		break;
 
 	case ALLOW_MEDIUM_REMOVAL:
-		US_DEBUGP("   ATA OUT - SCSIOP_MEDIUM_REMOVAL\n");
+		usb_stor_dbg(us, "   ATA OUT - SCSIOP_MEDIUM_REMOVAL\n");
 
 		if (info->DeviceFlags & DF_REMOVABLE_MEDIA) {
-			US_DEBUGP("   srb->cmnd[4] = 0x%X\n", srb->cmnd[4]);
+			usb_stor_dbg(us, "   srb->cmnd[4] = 0x%X\n",
+				     srb->cmnd[4]);
 	    
 			ataCdb->generic.SignatureByte0 = info->ConfigData.ATAMajorCommand;
 			ataCdb->generic.SignatureByte1 = info->ConfigData.ATAMinorCommand;
@@ -1380,25 +1384,25 @@ static int isd200_scsi_to_ata(struct scsi_cmnd *srb, struct us_data *us,
 				ATA_CMD_MEDIA_LOCK : ATA_CMD_MEDIA_UNLOCK;
 			isd200_srb_set_bufflen(srb, 0);
 		} else {
-			US_DEBUGP("   Not removeable media, just report okay\n");
+			usb_stor_dbg(us, "   Not removeable media, just report okay\n");
 			srb->result = SAM_STAT_GOOD;
 			sendToTransport = 0;
 		}
 		break;
 
 	case START_STOP:    
-		US_DEBUGP("   ATA OUT - SCSIOP_START_STOP_UNIT\n");
-		US_DEBUGP("   srb->cmnd[4] = 0x%X\n", srb->cmnd[4]);
+		usb_stor_dbg(us, "   ATA OUT - SCSIOP_START_STOP_UNIT\n");
+		usb_stor_dbg(us, "   srb->cmnd[4] = 0x%X\n", srb->cmnd[4]);
 
 		if ((srb->cmnd[4] & 0x3) == 0x2) {
-			US_DEBUGP("   Media Eject\n");
+			usb_stor_dbg(us, "   Media Eject\n");
 			ataCdb->generic.SignatureByte0 = info->ConfigData.ATAMajorCommand;
 			ataCdb->generic.SignatureByte1 = info->ConfigData.ATAMinorCommand;
 			ataCdb->generic.TransferBlockSize = 0;
 			ataCdb->generic.RegisterSelect = REG_COMMAND;
 			ataCdb->write.CommandByte = ATA_COMMAND_MEDIA_EJECT;
 		} else if ((srb->cmnd[4] & 0x3) == 0x1) {
-			US_DEBUGP("   Get Media Status\n");
+			usb_stor_dbg(us, "   Get Media Status\n");
 			ataCdb->generic.SignatureByte0 = info->ConfigData.ATAMajorCommand;
 			ataCdb->generic.SignatureByte1 = info->ConfigData.ATAMinorCommand;
 			ataCdb->generic.TransferBlockSize = 1;
@@ -1406,14 +1410,15 @@ static int isd200_scsi_to_ata(struct scsi_cmnd *srb, struct us_data *us,
 			ataCdb->write.CommandByte = ATA_COMMAND_GET_MEDIA_STATUS;
 			isd200_srb_set_bufflen(srb, 0);
 		} else {
-			US_DEBUGP("   Nothing to do, just report okay\n");
+			usb_stor_dbg(us, "   Nothing to do, just report okay\n");
 			srb->result = SAM_STAT_GOOD;
 			sendToTransport = 0;
 		}
 		break;
 
 	default:
-		US_DEBUGP("Unsupported SCSI command - 0x%X\n", srb->cmnd[0]);
+		usb_stor_dbg(us, "Unsupported SCSI command - 0x%X\n",
+			     srb->cmnd[0]);
 		srb->result = DID_ERROR << 16;
 		sendToTransport = 0;
 		break;
@@ -1457,8 +1462,7 @@ static int isd200_init_info(struct us_data *us)
 		retStatus = ISD200_ERROR;
 	else {
 		info->id = kzalloc(ATA_ID_WORDS * 2, GFP_KERNEL);
-		info->RegsBuf = (unsigned char *)
-				kmalloc(sizeof(info->ATARegs), GFP_KERNEL);
+		info->RegsBuf = kmalloc(sizeof(info->ATARegs), GFP_KERNEL);
 		info->srb.sense_buffer =
 				kmalloc(SCSI_SENSE_BUFFERSIZE, GFP_KERNEL);
 		if (!info->id || !info->RegsBuf || !info->srb.sense_buffer) {
@@ -1471,8 +1475,7 @@ static int isd200_init_info(struct us_data *us)
 	if (retStatus == ISD200_GOOD) {
 		us->extra = info;
 		us->extra_destructor = isd200_free_info_ptrs;
-	} else
-		US_DEBUGP("ERROR - kmalloc failure\n");
+	}
 
 	return retStatus;
 }
@@ -1483,19 +1486,19 @@ static int isd200_init_info(struct us_data *us)
 
 static int isd200_Initialization(struct us_data *us)
 {
-	US_DEBUGP("ISD200 Initialization...\n");
+	usb_stor_dbg(us, "ISD200 Initialization...\n");
 
 	/* Initialize ISD200 info struct */
 
 	if (isd200_init_info(us) == ISD200_ERROR) {
-		US_DEBUGP("ERROR Initializing ISD200 Info struct\n");
+		usb_stor_dbg(us, "ERROR Initializing ISD200 Info struct\n");
 	} else {
 		/* Get device specific data */
 
 		if (isd200_get_inquiry_data(us) != ISD200_GOOD)
-			US_DEBUGP("ISD200 Initialization Failure\n");
+			usb_stor_dbg(us, "ISD200 Initialization Failure\n");
 		else
-			US_DEBUGP("ISD200 Initialization complete\n");
+			usb_stor_dbg(us, "ISD200 Initialization complete\n");
 	}
 
 	return 0;
@@ -1520,7 +1523,7 @@ static void isd200_ata_command(struct scsi_cmnd *srb, struct us_data *us)
 	/* Make sure driver was initialized */
 
 	if (us->extra == NULL)
-		US_DEBUGP("ERROR Driver not initialized\n");
+		usb_stor_dbg(us, "ERROR Driver not initialized\n");
 
 	scsi_set_resid(srb, 0);
 	/* scsi_bufflen might change in protocol translation to ata */
diff --git a/drivers/usb/storage/jumpshot.c b/drivers/usb/storage/jumpshot.c
index ddc78780b1a..563078be654 100644
--- a/drivers/usb/storage/jumpshot.c
+++ b/drivers/usb/storage/jumpshot.c
@@ -118,7 +118,7 @@ static inline int jumpshot_bulk_read(struct us_data *us,
 	if (len == 0)
 		return USB_STOR_XFER_GOOD;
 
-	US_DEBUGP("jumpshot_bulk_read:  len = %d\n", len);
+	usb_stor_dbg(us, "len = %d\n", len);
 	return usb_stor_bulk_transfer_buf(us, us->recv_bulk_pipe,
 			data, len, NULL);
 }
@@ -131,7 +131,7 @@ static inline int jumpshot_bulk_write(struct us_data *us,
 	if (len == 0)
 		return USB_STOR_XFER_GOOD;
 
-	US_DEBUGP("jumpshot_bulk_write:  len = %d\n", len);
+	usb_stor_dbg(us, "len = %d\n", len);
 	return usb_stor_bulk_transfer_buf(us, us->send_bulk_pipe,
 			data, len, NULL);
 }
@@ -152,8 +152,7 @@ static int jumpshot_get_status(struct us_data  *us)
 		return USB_STOR_TRANSPORT_ERROR;
 
 	if (us->iobuf[0] != 0x50) {
-		US_DEBUGP("jumpshot_get_status:  0x%2x\n",
-			  us->iobuf[0]);
+		usb_stor_dbg(us, "0x%2x\n", us->iobuf[0]);
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 
@@ -218,7 +217,7 @@ static int jumpshot_read_data(struct us_data *us,
 		if (result != USB_STOR_XFER_GOOD)
 			goto leave;
 
-		US_DEBUGP("jumpshot_read_data:  %d bytes\n", len);
+		usb_stor_dbg(us, "%d bytes\n", len);
 
 		// Store the data in the transfer buffer
 		usb_stor_access_xfer_buf(buffer, len, us->srb,
@@ -314,7 +313,7 @@ static int jumpshot_write_data(struct us_data *us,
 		} while ((result != USB_STOR_TRANSPORT_GOOD) && (waitcount < 10));
 
 		if (result != USB_STOR_TRANSPORT_GOOD)
-			US_DEBUGP("jumpshot_write_data:  Gah!  Waitcount = 10.  Bad write!?\n");
+			usb_stor_dbg(us, "Gah!  Waitcount = 10.  Bad write!?\n");
 
 		sector += thistime;
 		totallen -= len;
@@ -349,8 +348,7 @@ static int jumpshot_id_device(struct us_data *us,
 				   0, 0x20, 0, 6, command, 2);
 
 	if (rc != USB_STOR_XFER_GOOD) {
-		US_DEBUGP("jumpshot_id_device:  Gah! "
-			  "send_control for read_capacity failed\n");
+		usb_stor_dbg(us, "Gah! send_control for read_capacity failed\n");
 		rc = USB_STOR_TRANSPORT_ERROR;
 		goto leave;
 	}
@@ -400,17 +398,17 @@ static int jumpshot_handle_mode_sense(struct us_data *us,
 
 	switch (pc) {
 	   case 0x0:
-		US_DEBUGP("jumpshot_handle_mode_sense:  Current values\n");
-		break;
+		   usb_stor_dbg(us, "Current values\n");
+		   break;
 	   case 0x1:
-		US_DEBUGP("jumpshot_handle_mode_sense:  Changeable values\n");
-		break;
+		   usb_stor_dbg(us, "Changeable values\n");
+		   break;
 	   case 0x2:
-		US_DEBUGP("jumpshot_handle_mode_sense:  Default values\n");
-		break;
+		   usb_stor_dbg(us, "Default values\n");
+		   break;
 	   case 0x3:
-		US_DEBUGP("jumpshot_handle_mode_sense:  Saves values\n");
-		break;
+		   usb_stor_dbg(us, "Saves values\n");
+		   break;
 	}
 
 	memset(ptr, 0, 8);
@@ -494,17 +492,16 @@ static int jumpshot_transport(struct scsi_cmnd *srb, struct us_data *us)
 
 	if (!us->extra) {
 		us->extra = kzalloc(sizeof(struct jumpshot_info), GFP_NOIO);
-		if (!us->extra) {
-			US_DEBUGP("jumpshot_transport:  Gah! Can't allocate storage for jumpshot info struct!\n");
+		if (!us->extra)
 			return USB_STOR_TRANSPORT_ERROR;
-		}
+
 		us->extra_destructor = jumpshot_info_destructor;
 	}
 
 	info = (struct jumpshot_info *) (us->extra);
 
 	if (srb->cmnd[0] == INQUIRY) {
-		US_DEBUGP("jumpshot_transport:  INQUIRY.  Returning bogus response.\n");
+		usb_stor_dbg(us, "INQUIRY - Returning bogus response\n");
 		memcpy(ptr, inquiry_response, sizeof(inquiry_response));
 		fill_inquiry_response(us, ptr, 36);
 		return USB_STOR_TRANSPORT_GOOD;
@@ -521,8 +518,8 @@ static int jumpshot_transport(struct scsi_cmnd *srb, struct us_data *us)
 		if (rc != USB_STOR_TRANSPORT_GOOD)
 			return rc;
 
-		US_DEBUGP("jumpshot_transport:  READ_CAPACITY:  %ld sectors, %ld bytes per sector\n",
-			  info->sectors, info->ssize);
+		usb_stor_dbg(us, "READ_CAPACITY:  %ld sectors, %ld bytes per sector\n",
+			     info->sectors, info->ssize);
 
 		// build the reply
 		//
@@ -534,7 +531,7 @@ static int jumpshot_transport(struct scsi_cmnd *srb, struct us_data *us)
 	}
 
 	if (srb->cmnd[0] == MODE_SELECT_10) {
-		US_DEBUGP("jumpshot_transport:  Gah! MODE_SELECT_10.\n");
+		usb_stor_dbg(us, "Gah! MODE_SELECT_10\n");
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 
@@ -544,7 +541,8 @@ static int jumpshot_transport(struct scsi_cmnd *srb, struct us_data *us)
 
 		blocks = ((u32)(srb->cmnd[7]) << 8) | ((u32)(srb->cmnd[8]));
 
-		US_DEBUGP("jumpshot_transport:  READ_10: read block 0x%04lx  count %ld\n", block, blocks);
+		usb_stor_dbg(us, "READ_10: read block 0x%04lx  count %ld\n",
+			     block, blocks);
 		return jumpshot_read_data(us, info, block, blocks);
 	}
 
@@ -557,7 +555,8 @@ static int jumpshot_transport(struct scsi_cmnd *srb, struct us_data *us)
 		blocks = ((u32)(srb->cmnd[6]) << 24) | ((u32)(srb->cmnd[7]) << 16) |
 			 ((u32)(srb->cmnd[8]) <<  8) | ((u32)(srb->cmnd[9]));
 
-		US_DEBUGP("jumpshot_transport:  READ_12: read block 0x%04lx  count %ld\n", block, blocks);
+		usb_stor_dbg(us, "READ_12: read block 0x%04lx  count %ld\n",
+			     block, blocks);
 		return jumpshot_read_data(us, info, block, blocks);
 	}
 
@@ -567,7 +566,8 @@ static int jumpshot_transport(struct scsi_cmnd *srb, struct us_data *us)
 
 		blocks = ((u32)(srb->cmnd[7]) << 8) | ((u32)(srb->cmnd[8]));
 
-		US_DEBUGP("jumpshot_transport:  WRITE_10: write block 0x%04lx  count %ld\n", block, blocks);
+		usb_stor_dbg(us, "WRITE_10: write block 0x%04lx  count %ld\n",
+			     block, blocks);
 		return jumpshot_write_data(us, info, block, blocks);
 	}
 
@@ -580,18 +580,19 @@ static int jumpshot_transport(struct scsi_cmnd *srb, struct us_data *us)
 		blocks = ((u32)(srb->cmnd[6]) << 24) | ((u32)(srb->cmnd[7]) << 16) |
 			 ((u32)(srb->cmnd[8]) <<  8) | ((u32)(srb->cmnd[9]));
 
-		US_DEBUGP("jumpshot_transport:  WRITE_12: write block 0x%04lx  count %ld\n", block, blocks);
+		usb_stor_dbg(us, "WRITE_12: write block 0x%04lx  count %ld\n",
+			     block, blocks);
 		return jumpshot_write_data(us, info, block, blocks);
 	}
 
 
 	if (srb->cmnd[0] == TEST_UNIT_READY) {
-		US_DEBUGP("jumpshot_transport:  TEST_UNIT_READY.\n");
+		usb_stor_dbg(us, "TEST_UNIT_READY\n");
 		return jumpshot_get_status(us);
 	}
 
 	if (srb->cmnd[0] == REQUEST_SENSE) {
-		US_DEBUGP("jumpshot_transport:  REQUEST_SENSE.\n");
+		usb_stor_dbg(us, "REQUEST_SENSE\n");
 
 		memset(ptr, 0, 18);
 		ptr[0] = 0xF0;
@@ -605,12 +606,12 @@ static int jumpshot_transport(struct scsi_cmnd *srb, struct us_data *us)
 	}
 
 	if (srb->cmnd[0] == MODE_SENSE) {
-		US_DEBUGP("jumpshot_transport:  MODE_SENSE_6 detected\n");
+		usb_stor_dbg(us, "MODE_SENSE_6 detected\n");
 		return jumpshot_handle_mode_sense(us, srb, 1);
 	}
 
 	if (srb->cmnd[0] == MODE_SENSE_10) {
-		US_DEBUGP("jumpshot_transport:  MODE_SENSE_10 detected\n");
+		usb_stor_dbg(us, "MODE_SENSE_10 detected\n");
 		return jumpshot_handle_mode_sense(us, srb, 0);
 	}
 
@@ -624,7 +625,7 @@ static int jumpshot_transport(struct scsi_cmnd *srb, struct us_data *us)
 	if (srb->cmnd[0] == START_STOP) {
 		/* this is used by sd.c'check_scsidisk_media_change to detect
 		   media change */
-		US_DEBUGP("jumpshot_transport:  START_STOP.\n");
+		usb_stor_dbg(us, "START_STOP\n");
 		/* the first jumpshot_id_device after a media change returns
 		   an error (determined experimentally) */
 		rc = jumpshot_id_device(us, info);
@@ -638,8 +639,8 @@ static int jumpshot_transport(struct scsi_cmnd *srb, struct us_data *us)
 		return rc;
 	}
 
-	US_DEBUGP("jumpshot_transport:  Gah! Unknown command: %d (0x%x)\n",
-		  srb->cmnd[0], srb->cmnd[0]);
+	usb_stor_dbg(us, "Gah! Unknown command: %d (0x%x)\n",
+		     srb->cmnd[0], srb->cmnd[0]);
 	info->sense_key = 0x05;
 	info->sense_asc = 0x20;
 	info->sense_ascq = 0x00;
diff --git a/drivers/usb/storage/karma.c b/drivers/usb/storage/karma.c
index f085ffb606c..94d16ee5e84 100644
--- a/drivers/usb/storage/karma.c
+++ b/drivers/usb/storage/karma.c
@@ -106,7 +106,7 @@ static int rio_karma_send_command(char cmd, struct us_data *us)
 	static unsigned char seq = 1;
 	struct karma_data *data = (struct karma_data *) us->extra;
 
-	US_DEBUGP("karma: sending command %04x\n", cmd);
+	usb_stor_dbg(us, "sending command %04x\n", cmd);
 	memset(us->iobuf, 0, RIO_SEND_LEN);
 	memcpy(us->iobuf, RIO_PREFIX, RIO_PREFIX_LEN);
 	us->iobuf[5] = cmd;
@@ -139,10 +139,10 @@ static int rio_karma_send_command(char cmd, struct us_data *us)
 	if (seq == 0)
 		seq = 1;
 
-	US_DEBUGP("karma: sent command %04x\n", cmd);
+	usb_stor_dbg(us, "sent command %04x\n", cmd);
 	return 0;
 err:
-	US_DEBUGP("karma: command %04x failed\n", cmd);
+	usb_stor_dbg(us, "command %04x failed\n", cmd);
 	return USB_STOR_TRANSPORT_FAILED;
 }
 
diff --git a/drivers/usb/storage/onetouch.c b/drivers/usb/storage/onetouch.c
index cb79de61f4c..26964895c88 100644
--- a/drivers/usb/storage/onetouch.c
+++ b/drivers/usb/storage/onetouch.c
@@ -195,6 +195,7 @@ static int onetouch_connect_input(struct us_data *ss)
 
 	pipe = usb_rcvintpipe(udev, endpoint->bEndpointAddress);
 	maxp = usb_maxpacket(udev, pipe, usb_pipeout(pipe));
+	maxp = min(maxp, ONETOUCH_PKT_LEN);
 
 	onetouch = kzalloc(sizeof(struct usb_onetouch), GFP_KERNEL);
 	input_dev = input_allocate_device();
@@ -245,8 +246,7 @@ static int onetouch_connect_input(struct us_data *ss)
 	input_dev->open = usb_onetouch_open;
 	input_dev->close = usb_onetouch_close;
 
-	usb_fill_int_urb(onetouch->irq, udev, pipe, onetouch->data,
-			 (maxp > 8 ? 8 : maxp),
+	usb_fill_int_urb(onetouch->irq, udev, pipe, onetouch->data, maxp,
 			 usb_onetouch_irq, onetouch, endpoint->bInterval);
 	onetouch->irq->transfer_dma = onetouch->data_dma;
 	onetouch->irq->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
diff --git a/drivers/usb/storage/option_ms.c b/drivers/usb/storage/option_ms.c
index e0f76bb0591..b2b35b1d7de 100644
--- a/drivers/usb/storage/option_ms.c
+++ b/drivers/usb/storage/option_ms.c
@@ -50,7 +50,7 @@ static int option_rezero(struct us_data *us)
 	char *buffer;
 	int result;
 
-	US_DEBUGP("Option MS: %s", "DEVICE MODE SWITCH\n");
+	usb_stor_dbg(us, "Option MS: %s\n", "DEVICE MODE SWITCH");
 
 	buffer = kzalloc(RESPONSE_LEN, GFP_KERNEL);
 	if (buffer == NULL)
@@ -95,7 +95,7 @@ static int option_inquiry(struct us_data *us)
 	char *buffer;
 	int result;
 
-	US_DEBUGP("Option MS: %s", "device inquiry for vendor name\n");
+	usb_stor_dbg(us, "Option MS: %s\n", "device inquiry for vendor name");
 
 	buffer = kzalloc(0x24, GFP_KERNEL);
 	if (buffer == NULL)
@@ -138,31 +138,32 @@ int option_ms_init(struct us_data *us)
 {
 	int result;
 
-	US_DEBUGP("Option MS: option_ms_init called\n");
+	usb_stor_dbg(us, "Option MS: %s\n", "option_ms_init called");
 
 	/* Additional test for vendor information via INQUIRY,
 	 * because some vendor/product IDs are ambiguous
 	 */
 	result = option_inquiry(us);
 	if (result != 0) {
-		US_DEBUGP("Option MS: vendor is not Option or not determinable,"
-			  " no action taken\n");
+		usb_stor_dbg(us, "Option MS: %s\n",
+			     "vendor is not Option or not determinable, no action taken");
 		return 0;
 	} else
-		US_DEBUGP("Option MS: this is a genuine Option device,"
-			  " proceeding\n");
+		usb_stor_dbg(us, "Option MS: %s\n",
+			     "this is a genuine Option device, proceeding");
 
 	/* Force Modem mode */
 	if (option_zero_cd == ZCD_FORCE_MODEM) {
-		US_DEBUGP("Option MS: %s", "Forcing Modem Mode\n");
+		usb_stor_dbg(us, "Option MS: %s\n", "Forcing Modem Mode");
 		result = option_rezero(us);
 		if (result != USB_STOR_XFER_GOOD)
-			US_DEBUGP("Option MS: Failed to switch to modem mode.\n");
+			usb_stor_dbg(us, "Option MS: %s\n",
+				     "Failed to switch to modem mode");
 		return -EIO;
 	} else if (option_zero_cd == ZCD_ALLOW_MS) {
 		/* Allow Mass Storage mode (keep CD-Rom) */
-		US_DEBUGP("Option MS: %s", "Allowing Mass Storage Mode if device"
-		          " requests it\n");
+		usb_stor_dbg(us, "Option MS: %s\n",
+			     "Allowing Mass Storage Mode if device requests it");
 	}
 
 	return 0;
diff --git a/drivers/usb/storage/realtek_cr.c b/drivers/usb/storage/realtek_cr.c
index 6c3586a4c95..4797228747f 100644
--- a/drivers/usb/storage/realtek_cr.c
+++ b/drivers/usb/storage/realtek_cr.c
@@ -254,8 +254,8 @@ static int rts51x_bulk_transport(struct us_data *us, u8 lun,
 
 	/* check bulk status */
 	if (bcs->Signature != cpu_to_le32(US_BULK_CS_SIGN)) {
-		US_DEBUGP("Signature mismatch: got %08X, expecting %08X\n",
-			  le32_to_cpu(bcs->Signature), US_BULK_CS_SIGN);
+		usb_stor_dbg(us, "Signature mismatch: got %08X, expecting %08X\n",
+			     le32_to_cpu(bcs->Signature), US_BULK_CS_SIGN);
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 
@@ -351,8 +351,8 @@ static int rts51x_get_max_lun(struct us_data *us)
 				      USB_RECIP_INTERFACE,
 				      0, us->ifnum, us->iobuf, 1, 10 * HZ);
 
-	US_DEBUGP("GetMaxLUN command result is %d, data is %d\n",
-		  result, us->iobuf[0]);
+	usb_stor_dbg(us, "GetMaxLUN command result is %d, data is %d\n",
+		     result, us->iobuf[0]);
 
 	/* if we have a successful request, return the result */
 	if (result > 0)
@@ -371,7 +371,7 @@ static int rts51x_read_mem(struct us_data *us, u16 addr, u8 *data, u16 len)
 	if (buf == NULL)
 		return USB_STOR_TRANSPORT_ERROR;
 
-	US_DEBUGP("%s, addr = 0x%x, len = %d\n", __func__, addr, len);
+	usb_stor_dbg(us, "addr = 0x%x, len = %d\n", addr, len);
 
 	cmnd[0] = 0xF0;
 	cmnd[1] = 0x0D;
@@ -402,7 +402,7 @@ static int rts51x_write_mem(struct us_data *us, u16 addr, u8 *data, u16 len)
 	if (buf == NULL)
 		return USB_STOR_TRANSPORT_ERROR;
 
-	US_DEBUGP("%s, addr = 0x%x, len = %d\n", __func__, addr, len);
+	usb_stor_dbg(us, "addr = 0x%x, len = %d\n", addr, len);
 
 	cmnd[0] = 0xF0;
 	cmnd[1] = 0x0E;
@@ -431,7 +431,7 @@ static int rts51x_read_status(struct us_data *us,
 	if (buf == NULL)
 		return USB_STOR_TRANSPORT_ERROR;
 
-	US_DEBUGP("%s, lun = %d\n", __func__, lun);
+	usb_stor_dbg(us, "lun = %d\n", lun);
 
 	cmnd[0] = 0xF0;
 	cmnd[1] = 0x09;
@@ -458,7 +458,7 @@ static int rts51x_check_status(struct us_data *us, u8 lun)
 	if (retval != STATUS_SUCCESS)
 		return -EIO;
 
-	US_DEBUGP("chip->status_len = %d\n", chip->status_len);
+	usb_stor_dbg(us, "chip->status_len = %d\n", chip->status_len);
 
 	chip->status[lun].vid = ((u16) buf[0] << 8) | buf[1];
 	chip->status[lun].pid = ((u16) buf[2] << 8) | buf[3];
@@ -509,7 +509,7 @@ static int __do_config_autodelink(struct us_data *us, u8 *data, u16 len)
 	u8 cmnd[12] = {0};
 	u8 *buf;
 
-	US_DEBUGP("%s, addr = 0xfe47, len = %d\n", __FUNCTION__, len);
+	usb_stor_dbg(us, "addr = 0xfe47, len = %d\n", len);
 
 	buf = kmemdup(data, len, GFP_NOIO);
 	if (!buf)
@@ -549,7 +549,7 @@ static int do_config_autodelink(struct us_data *us, int enable, int force)
 		value &= ~0x03;
 	}
 
-	US_DEBUGP("In %s,set 0xfe47 to 0x%x\n", __func__, value);
+	usb_stor_dbg(us, "set 0xfe47 to 0x%x\n", value);
 
 	/* retval = rts51x_write_mem(us, 0xFE47, &value, 1); */
 	retval = __do_config_autodelink(us, &value, 1);
@@ -565,8 +565,6 @@ static int config_autodelink_after_power_on(struct us_data *us)
 	int retval;
 	u8 value;
 
-	US_DEBUGP("%s: <---\n", __func__);
-
 	if (!CHK_AUTO_DELINK(chip))
 		return 0;
 
@@ -624,8 +622,6 @@ static int config_autodelink_after_power_on(struct us_data *us)
 		}
 	}
 
-	US_DEBUGP("%s: --->\n", __func__);
-
 	return 0;
 }
 
@@ -635,8 +631,6 @@ static int config_autodelink_before_power_down(struct us_data *us)
 	int retval;
 	u8 value;
 
-	US_DEBUGP("%s: <---\n", __func__);
-
 	if (!CHK_AUTO_DELINK(chip))
 		return 0;
 
@@ -698,8 +692,6 @@ static int config_autodelink_before_power_down(struct us_data *us)
 		}
 	}
 
-	US_DEBUGP("%s: --->\n", __func__);
-
 	return 0;
 }
 
@@ -709,23 +701,19 @@ static void fw5895_init(struct us_data *us)
 	int retval;
 	u8 val;
 
-	US_DEBUGP("%s: <---\n", __func__);
-
 	if ((PRODUCT_ID(chip) != 0x0158) || (FW_VERSION(chip) != 0x5895)) {
-		US_DEBUGP("Not the specified device, return immediately!\n");
+		usb_stor_dbg(us, "Not the specified device, return immediately!\n");
 	} else {
 		retval = rts51x_read_mem(us, 0xFD6F, &val, 1);
 		if (retval == STATUS_SUCCESS && (val & 0x1F) == 0) {
 			val = 0x1F;
 			retval = rts51x_write_mem(us, 0xFD70, &val, 1);
 			if (retval != STATUS_SUCCESS)
-				US_DEBUGP("Write memory fail\n");
+				usb_stor_dbg(us, "Write memory fail\n");
 		} else {
-			US_DEBUGP("Read memory fail, OR (val & 0x1F) != 0\n");
+			usb_stor_dbg(us, "Read memory fail, OR (val & 0x1F) != 0\n");
 		}
 	}
-
-	US_DEBUGP("%s: --->\n", __func__);
 }
 
 #ifdef CONFIG_REALTEK_AUTOPM
@@ -735,10 +723,8 @@ static void fw5895_set_mmc_wp(struct us_data *us)
 	int retval;
 	u8 buf[13];
 
-	US_DEBUGP("%s: <---\n", __func__);
-
 	if ((PRODUCT_ID(chip) != 0x0158) || (FW_VERSION(chip) != 0x5895)) {
-		US_DEBUGP("Not the specified device, return immediately!\n");
+		usb_stor_dbg(us, "Not the specified device, return immediately!\n");
 	} else {
 		retval = rts51x_read_mem(us, 0xFD6F, buf, 1);
 		if (retval == STATUS_SUCCESS && (buf[0] & 0x24) == 0x24) {
@@ -748,26 +734,24 @@ static void fw5895_set_mmc_wp(struct us_data *us)
 				buf[0] |= 0x04;
 				retval = rts51x_write_mem(us, 0xFD70, buf, 1);
 				if (retval != STATUS_SUCCESS)
-					US_DEBUGP("Write memory fail\n");
+					usb_stor_dbg(us, "Write memory fail\n");
 			} else {
-				US_DEBUGP("Read memory fail\n");
+				usb_stor_dbg(us, "Read memory fail\n");
 			}
 		} else {
-			US_DEBUGP("Read memory fail, OR (buf[0]&0x24)!=0x24\n");
+			usb_stor_dbg(us, "Read memory fail, OR (buf[0]&0x24)!=0x24\n");
 		}
 	}
-
-	US_DEBUGP("%s: --->\n", __func__);
 }
 
 static void rts51x_modi_suspend_timer(struct rts51x_chip *chip)
 {
-	US_DEBUGP("%s: <---, state:%d\n", __func__, rts51x_get_stat(chip));
+	struct us_data *us = chip->us;
+
+	usb_stor_dbg(us, "state:%d\n", rts51x_get_stat(chip));
 
 	chip->timer_expires = jiffies + msecs_to_jiffies(1000*ss_delay);
 	mod_timer(&chip->rts51x_suspend_timer, chip->timer_expires);
-
-	US_DEBUGP("%s: --->\n", __func__);
 }
 
 static void rts51x_suspend_timer_fn(unsigned long data)
@@ -775,8 +759,6 @@ static void rts51x_suspend_timer_fn(unsigned long data)
 	struct rts51x_chip *chip = (struct rts51x_chip *)data;
 	struct us_data *us = chip->us;
 
-	US_DEBUGP("%s: <---\n", __func__);
-
 	switch (rts51x_get_stat(chip)) {
 	case RTS51X_STAT_INIT:
 	case RTS51X_STAT_RUN:
@@ -784,32 +766,25 @@ static void rts51x_suspend_timer_fn(unsigned long data)
 		break;
 	case RTS51X_STAT_IDLE:
 	case RTS51X_STAT_SS:
-		US_DEBUGP("%s: RTS51X_STAT_SS, intf->pm_usage_cnt:%d,"
-			"power.usage:%d\n", __func__,
-			atomic_read(&us->pusb_intf->pm_usage_cnt),
-			atomic_read(&us->pusb_intf->dev.power.usage_count));
+		usb_stor_dbg(us, "RTS51X_STAT_SS, intf->pm_usage_cnt:%d, power.usage:%d\n",
+			     atomic_read(&us->pusb_intf->pm_usage_cnt),
+			     atomic_read(&us->pusb_intf->dev.power.usage_count));
 
 		if (atomic_read(&us->pusb_intf->pm_usage_cnt) > 0) {
-			US_DEBUGP("%s: Ready to enter SS state.\n",
-				  __func__);
+			usb_stor_dbg(us, "Ready to enter SS state\n");
 			rts51x_set_stat(chip, RTS51X_STAT_SS);
 			/* ignore mass storage interface's children */
 			pm_suspend_ignore_children(&us->pusb_intf->dev, true);
 			usb_autopm_put_interface_async(us->pusb_intf);
-			US_DEBUGP("%s: RTS51X_STAT_SS 01,"
-				"intf->pm_usage_cnt:%d, power.usage:%d\n",
-				__func__,
-				atomic_read(&us->pusb_intf->pm_usage_cnt),
-				atomic_read(
-					&us->pusb_intf->dev.power.usage_count));
+			usb_stor_dbg(us, "RTS51X_STAT_SS 01, intf->pm_usage_cnt:%d, power.usage:%d\n",
+				     atomic_read(&us->pusb_intf->pm_usage_cnt),
+				     atomic_read(&us->pusb_intf->dev.power.usage_count));
 		}
 		break;
 	default:
-		US_DEBUGP("%s: Unknonwn state !!!\n", __func__);
+		usb_stor_dbg(us, "Unknown state !!!\n");
 		break;
 	}
-
-	US_DEBUGP("%s: --->\n", __func__);
 }
 
 static inline int working_scsi(struct scsi_cmnd *srb)
@@ -834,24 +809,21 @@ static void rts51x_invoke_transport(struct scsi_cmnd *srb, struct us_data *us)
 	};
 	int ret;
 
-	US_DEBUGP("%s: <---\n", __func__);
-
 	if (working_scsi(srb)) {
-		US_DEBUGP("%s: working scsi, intf->pm_usage_cnt:%d,"
-			"power.usage:%d\n", __func__,
-			atomic_read(&us->pusb_intf->pm_usage_cnt),
-			atomic_read(&us->pusb_intf->dev.power.usage_count));
+		usb_stor_dbg(us, "working scsi, intf->pm_usage_cnt:%d, power.usage:%d\n",
+			     atomic_read(&us->pusb_intf->pm_usage_cnt),
+			     atomic_read(&us->pusb_intf->dev.power.usage_count));
 
 		if (atomic_read(&us->pusb_intf->pm_usage_cnt) <= 0) {
 			ret = usb_autopm_get_interface(us->pusb_intf);
-			US_DEBUGP("%s: working scsi, ret=%d\n", __func__, ret);
+			usb_stor_dbg(us, "working scsi, ret=%d\n", ret);
 		}
 		if (rts51x_get_stat(chip) != RTS51X_STAT_RUN)
 			rts51x_set_stat(chip, RTS51X_STAT_RUN);
 		chip->proto_handler_backup(srb, us);
 	} else {
 		if (rts51x_get_stat(chip) == RTS51X_STAT_SS) {
-			US_DEBUGP("%s: NOT working scsi\n", __func__);
+			usb_stor_dbg(us, "NOT working scsi\n");
 			if ((srb->cmnd[0] == TEST_UNIT_READY) &&
 			    (chip->pwr_state == US_SUSPEND)) {
 				if (TST_LUN_READY(chip, srb->device->lun)) {
@@ -862,8 +834,7 @@ static void rts51x_invoke_transport(struct scsi_cmnd *srb, struct us_data *us)
 					       media_not_present,
 					       US_SENSE_SIZE);
 				}
-				US_DEBUGP("%s: TEST_UNIT_READY--->\n",
-					  __func__);
+				usb_stor_dbg(us, "TEST_UNIT_READY\n");
 				goto out;
 			}
 			if (srb->cmnd[0] == ALLOW_MEDIUM_REMOVAL) {
@@ -876,12 +847,11 @@ static void rts51x_invoke_transport(struct scsi_cmnd *srb, struct us_data *us)
 				} else {
 					srb->result = SAM_STAT_GOOD;
 				}
-				US_DEBUGP("%s: ALLOW_MEDIUM_REMOVAL--->\n",
-					  __func__);
+				usb_stor_dbg(us, "ALLOW_MEDIUM_REMOVAL\n");
 				goto out;
 			}
 		} else {
-			US_DEBUGP("%s: NOT working scsi, not SS\n", __func__);
+			usb_stor_dbg(us, "NOT working scsi, not SS\n");
 			chip->proto_handler_backup(srb, us);
 			/* Check whether card is plugged in */
 			if (srb->cmnd[0] == TEST_UNIT_READY) {
@@ -901,11 +871,9 @@ static void rts51x_invoke_transport(struct scsi_cmnd *srb, struct us_data *us)
 		}
 	}
 out:
-	US_DEBUGP("%s: state:%d\n", __func__, rts51x_get_stat(chip));
+	usb_stor_dbg(us, "state:%d\n", rts51x_get_stat(chip));
 	if (rts51x_get_stat(chip) == RTS51X_STAT_RUN)
 		rts51x_modi_suspend_timer(chip);
-
-	US_DEBUGP("%s: --->\n", __func__);
 }
 
 static int realtek_cr_autosuspend_setup(struct us_data *us)
@@ -923,7 +891,7 @@ static int realtek_cr_autosuspend_setup(struct us_data *us)
 
 	retval = rts51x_read_status(us, 0, buf, 16, &(chip->status_len));
 	if (retval != STATUS_SUCCESS) {
-		US_DEBUGP("Read status fail\n");
+		usb_stor_dbg(us, "Read status fail\n");
 		return -EIO;
 	}
 	status = chip->status;
@@ -966,11 +934,13 @@ static int realtek_cr_autosuspend_setup(struct us_data *us)
 static void realtek_cr_destructor(void *extra)
 {
 	struct rts51x_chip *chip = (struct rts51x_chip *)extra;
-
-	US_DEBUGP("%s: <---\n", __func__);
+	struct us_data *us;
 
 	if (!chip)
 		return;
+
+	us = chip->us;
+
 #ifdef CONFIG_REALTEK_AUTOPM
 	if (ss_en) {
 		del_timer(&chip->rts51x_suspend_timer);
@@ -985,8 +955,6 @@ static int realtek_cr_suspend(struct usb_interface *iface, pm_message_t message)
 {
 	struct us_data *us = usb_get_intfdata(iface);
 
-	US_DEBUGP("%s: <---\n", __func__);
-
 	/* wait until no command is running */
 	mutex_lock(&us->dev_mutex);
 
@@ -994,8 +962,6 @@ static int realtek_cr_suspend(struct usb_interface *iface, pm_message_t message)
 
 	mutex_unlock(&us->dev_mutex);
 
-	US_DEBUGP("%s: --->\n", __func__);
-
 	return 0;
 }
 
@@ -1003,13 +969,9 @@ static int realtek_cr_resume(struct usb_interface *iface)
 {
 	struct us_data *us = usb_get_intfdata(iface);
 
-	US_DEBUGP("%s: <---\n", __func__);
-
 	fw5895_init(us);
 	config_autodelink_after_power_on(us);
 
-	US_DEBUGP("%s: --->\n", __func__);
-
 	return 0;
 }
 #else
@@ -1030,7 +992,7 @@ static int init_realtek_cr(struct us_data *us)
 	us->extra_destructor = realtek_cr_destructor;
 	us->max_lun = chip->max_lun = rts51x_get_max_lun(us);
 
-	US_DEBUGP("chip->max_lun = %d\n", chip->max_lun);
+	usb_stor_dbg(us, "chip->max_lun = %d\n", chip->max_lun);
 
 	size = (chip->max_lun + 1) * sizeof(struct rts51x_status);
 	chip->status = kzalloc(size, GFP_KERNEL);
@@ -1057,7 +1019,7 @@ static int init_realtek_cr(struct us_data *us)
 	}
 #endif
 
-	US_DEBUGP("chip->flag = 0x%x\n", chip->flag);
+	usb_stor_dbg(us, "chip->flag = 0x%x\n", chip->flag);
 
 	(void)config_autodelink_after_power_on(us);
 
@@ -1079,7 +1041,7 @@ static int realtek_cr_probe(struct usb_interface *intf,
 	struct us_data *us;
 	int result;
 
-	US_DEBUGP("Probe Realtek Card Reader!\n");
+	dev_dbg(&intf->dev, "Probe Realtek Card Reader!\n");
 
 	result = usb_stor_probe1(&us, intf, id,
 				 (id - realtek_cr_ids) +
diff --git a/drivers/usb/storage/scsiglue.c b/drivers/usb/storage/scsiglue.c
index 92f35abee92..4faa982807f 100644
--- a/drivers/usb/storage/scsiglue.c
+++ b/drivers/usb/storage/scsiglue.c
@@ -313,8 +313,6 @@ static int queuecommand_lck(struct scsi_cmnd *srb,
 {
 	struct us_data *us = host_to_us(srb->device->host);
 
-	US_DEBUGP("%s called\n", __func__);
-
 	/* check for state-transition errors */
 	if (us->srb != NULL) {
 		printk(KERN_ERR USB_STORAGE "Error in %s: us->srb = %p\n",
@@ -324,7 +322,7 @@ static int queuecommand_lck(struct scsi_cmnd *srb,
 
 	/* fail the command if we are disconnecting */
 	if (test_bit(US_FLIDX_DISCONNECTING, &us->dflags)) {
-		US_DEBUGP("Fail command during disconnect\n");
+		usb_stor_dbg(us, "Fail command during disconnect\n");
 		srb->result = DID_NO_CONNECT << 16;
 		done(srb);
 		return 0;
@@ -349,7 +347,7 @@ static int command_abort(struct scsi_cmnd *srb)
 {
 	struct us_data *us = host_to_us(srb->device->host);
 
-	US_DEBUGP("%s called\n", __func__);
+	usb_stor_dbg(us, "%s called\n", __func__);
 
 	/* us->srb together with the TIMED_OUT, RESETTING, and ABORTING
 	 * bits are protected by the host lock. */
@@ -358,7 +356,7 @@ static int command_abort(struct scsi_cmnd *srb)
 	/* Is this command still active? */
 	if (us->srb != srb) {
 		scsi_unlock(us_to_host(us));
-		US_DEBUGP ("-- nothing to abort\n");
+		usb_stor_dbg(us, "-- nothing to abort\n");
 		return FAILED;
 	}
 
@@ -386,7 +384,7 @@ static int device_reset(struct scsi_cmnd *srb)
 	struct us_data *us = host_to_us(srb->device->host);
 	int result;
 
-	US_DEBUGP("%s called\n", __func__);
+	usb_stor_dbg(us, "%s called\n", __func__);
 
 	/* lock the device pointers and do the reset */
 	mutex_lock(&(us->dev_mutex));
@@ -402,7 +400,8 @@ static int bus_reset(struct scsi_cmnd *srb)
 	struct us_data *us = host_to_us(srb->device->host);
 	int result;
 
-	US_DEBUGP("%s called\n", __func__);
+	usb_stor_dbg(us, "%s called\n", __func__);
+
 	result = usb_stor_port_reset(us);
 	return result < 0 ? FAILED : SUCCESS;
 }
diff --git a/drivers/usb/storage/sddr09.c b/drivers/usb/storage/sddr09.c
index 7bd54e0d512..732027f3320 100644
--- a/drivers/usb/storage/sddr09.c
+++ b/drivers/usb/storage/sddr09.c
@@ -105,8 +105,6 @@ static struct us_unusual_dev sddr09_unusual_dev_list[] = {
 #define LSB_of(s) ((s)&0xFF)
 #define MSB_of(s) ((s)>>8)
 
-/* #define US_DEBUGP printk */
-
 /*
  * First some stuff that does not belong here:
  * data on SmartMedia and other cards, completely
@@ -347,7 +345,7 @@ sddr09_test_unit_ready(struct us_data *us) {
 
 	result = sddr09_send_scsi_command(us, command, 6);
 
-	US_DEBUGP("sddr09_test_unit_ready returns %d\n", result);
+	usb_stor_dbg(us, "sddr09_test_unit_ready returns %d\n", result);
 
 	return result;
 }
@@ -423,8 +421,8 @@ sddr09_readX(struct us_data *us, int x, unsigned long fromaddress,
 	result = sddr09_send_scsi_command(us, command, 12);
 
 	if (result) {
-		US_DEBUGP("Result for send_control in sddr09_read2%d %d\n",
-			  x, result);
+		usb_stor_dbg(us, "Result for send_control in sddr09_read2%d %d\n",
+			     x, result);
 		return result;
 	}
 
@@ -432,8 +430,8 @@ sddr09_readX(struct us_data *us, int x, unsigned long fromaddress,
 				       buf, bulklen, use_sg, NULL);
 
 	if (result != USB_STOR_XFER_GOOD) {
-		US_DEBUGP("Result for bulk_transfer in sddr09_read2%d %d\n",
-			  x, result);
+		usb_stor_dbg(us, "Result for bulk_transfer in sddr09_read2%d %d\n",
+			     x, result);
 		return -EIO;
 	}
 	return 0;
@@ -494,8 +492,7 @@ sddr09_read22(struct us_data *us, unsigned long fromaddress,
 	      int nr_of_pages, int pageshift, unsigned char *buf, int use_sg) {
 
 	int bulklen = (nr_of_pages << pageshift) + (nr_of_pages << CONTROL_SHIFT);
-	US_DEBUGP("sddr09_read22: reading %d pages, %d bytes\n",
-		  nr_of_pages, bulklen);
+	usb_stor_dbg(us, "reading %d pages, %d bytes\n", nr_of_pages, bulklen);
 	return sddr09_readX(us, 2, fromaddress, nr_of_pages, bulklen,
 			    buf, use_sg);
 }
@@ -538,7 +535,7 @@ sddr09_erase(struct us_data *us, unsigned long Eaddress) {
 	unsigned char *command = us->iobuf;
 	int result;
 
-	US_DEBUGP("sddr09_erase: erase address %lu\n", Eaddress);
+	usb_stor_dbg(us, "erase address %lu\n", Eaddress);
 
 	memset(command, 0, 12);
 	command[0] = 0xEA;
@@ -551,8 +548,8 @@ sddr09_erase(struct us_data *us, unsigned long Eaddress) {
 	result = sddr09_send_scsi_command(us, command, 12);
 
 	if (result)
-		US_DEBUGP("Result for send_control in sddr09_erase %d\n",
-			  result);
+		usb_stor_dbg(us, "Result for send_control in sddr09_erase %d\n",
+			     result);
 
 	return result;
 }
@@ -609,8 +606,8 @@ sddr09_writeX(struct us_data *us,
 	result = sddr09_send_scsi_command(us, command, 12);
 
 	if (result) {
-		US_DEBUGP("Result for send_control in sddr09_writeX %d\n",
-			  result);
+		usb_stor_dbg(us, "Result for send_control in sddr09_writeX %d\n",
+			     result);
 		return result;
 	}
 
@@ -618,8 +615,8 @@ sddr09_writeX(struct us_data *us,
 				       buf, bulklen, use_sg, NULL);
 
 	if (result != USB_STOR_XFER_GOOD) {
-		US_DEBUGP("Result for bulk_transfer in sddr09_writeX %d\n",
-			  result);
+		usb_stor_dbg(us, "Result for bulk_transfer in sddr09_writeX %d\n",
+			     result);
 		return -EIO;
 	}
 	return 0;
@@ -687,8 +684,8 @@ sddr09_read_sg_test_only(struct us_data *us) {
 	result = sddr09_send_scsi_command(us, command, 4*nsg+3);
 
 	if (result) {
-		US_DEBUGP("Result for send_control in sddr09_read_sg %d\n",
-			  result);
+		usb_stor_dbg(us, "Result for send_control in sddr09_read_sg %d\n",
+			     result);
 		return result;
 	}
 
@@ -700,8 +697,8 @@ sddr09_read_sg_test_only(struct us_data *us) {
 				       buf, bulklen, NULL);
 	kfree(buf);
 	if (result != USB_STOR_XFER_GOOD) {
-		US_DEBUGP("Result for bulk_transfer in sddr09_read_sg %d\n",
-			  result);
+		usb_stor_dbg(us, "Result for bulk_transfer in sddr09_read_sg %d\n",
+			     result);
 		return -EIO;
 	}
 
@@ -727,7 +724,7 @@ sddr09_read_status(struct us_data *us, unsigned char *status) {
 	unsigned char *data = us->iobuf;
 	int result;
 
-	US_DEBUGP("Reading status...\n");
+	usb_stor_dbg(us, "Reading status...\n");
 
 	memset(command, 0, 12);
 	command[0] = 0xEC;
@@ -789,8 +786,8 @@ sddr09_read_data(struct us_data *us,
 
 		/* Not overflowing capacity? */
 		if (lba >= maxlba) {
-			US_DEBUGP("Error: Requested lba %u exceeds "
-				  "maximum %u\n", lba, maxlba);
+			usb_stor_dbg(us, "Error: Requested lba %u exceeds maximum %u\n",
+				     lba, maxlba);
 			result = -EIO;
 			break;
 		}
@@ -800,8 +797,8 @@ sddr09_read_data(struct us_data *us,
 
 		if (pba == UNDEF) {	/* this lba was never written */
 
-			US_DEBUGP("Read %d zero pages (LBA %d) page %d\n",
-				  pages, lba, page);
+			usb_stor_dbg(us, "Read %d zero pages (LBA %d) page %d\n",
+				     pages, lba, page);
 
 			/* This is not really an error. It just means
 			   that the block has never been written.
@@ -811,9 +808,8 @@ sddr09_read_data(struct us_data *us,
 			memset(buffer, 0, len);
 
 		} else {
-			US_DEBUGP("Read %d pages, from PBA %d"
-				  " (LBA %d) page %d\n",
-				  pages, pba, lba, page);
+			usb_stor_dbg(us, "Read %d pages, from PBA %d (LBA %d) page %d\n",
+				     pages, pba, lba, page);
 
 			address = ((pba << info->blockshift) + page) << 
 				info->pageshift;
@@ -916,14 +912,14 @@ sddr09_write_lba(struct us_data *us, unsigned int lba,
 		cptr = bptr + info->pagesize;
 		nand_compute_ecc(bptr, ecc);
 		if (!nand_compare_ecc(cptr+13, ecc)) {
-			US_DEBUGP("Warning: bad ecc in page %d- of pba %d\n",
-				  i, pba);
+			usb_stor_dbg(us, "Warning: bad ecc in page %d- of pba %d\n",
+				     i, pba);
 			nand_store_ecc(cptr+13, ecc);
 		}
 		nand_compute_ecc(bptr+(info->pagesize / 2), ecc);
 		if (!nand_compare_ecc(cptr+8, ecc)) {
-			US_DEBUGP("Warning: bad ecc in page %d+ of pba %d\n",
-				  i, pba);
+			usb_stor_dbg(us, "Warning: bad ecc in page %d+ of pba %d\n",
+				     i, pba);
 			nand_store_ecc(cptr+8, ecc);
 		}
 		cptr[6] = cptr[11] = MSB_of(lbap);
@@ -943,22 +939,21 @@ sddr09_write_lba(struct us_data *us, unsigned int lba,
 		nand_store_ecc(cptr+8, ecc);
 	}
 
-	US_DEBUGP("Rewrite PBA %d (LBA %d)\n", pba, lba);
+	usb_stor_dbg(us, "Rewrite PBA %d (LBA %d)\n", pba, lba);
 
 	result = sddr09_write_inplace(us, address>>1, info->blocksize,
 				      info->pageshift, blockbuffer, 0);
 
-	US_DEBUGP("sddr09_write_inplace returns %d\n", result);
+	usb_stor_dbg(us, "sddr09_write_inplace returns %d\n", result);
 
 #if 0
 	{
 		unsigned char status = 0;
 		int result2 = sddr09_read_status(us, &status);
 		if (result2)
-			US_DEBUGP("sddr09_write_inplace: cannot read status\n");
+			usb_stor_dbg(us, "cannot read status\n");
 		else if (status != 0xc0)
-			US_DEBUGP("sddr09_write_inplace: status after write: 0x%x\n",
-				  status);
+			usb_stor_dbg(us, "status after write: 0x%x\n", status);
 	}
 #endif
 
@@ -1031,8 +1026,8 @@ sddr09_write_data(struct us_data *us,
 
 		/* Not overflowing capacity? */
 		if (lba >= maxlba) {
-			US_DEBUGP("Error: Requested lba %u exceeds "
-				  "maximum %u\n", lba, maxlba);
+			usb_stor_dbg(us, "Error: Requested lba %u exceeds maximum %u\n",
+				     lba, maxlba);
 			result = -EIO;
 			break;
 		}
@@ -1064,8 +1059,8 @@ sddr09_read_control(struct us_data *us,
 		unsigned char *content,
 		int use_sg) {
 
-	US_DEBUGP("Read control address %lu, blocks %d\n",
-		address, blocks);
+	usb_stor_dbg(us, "Read control address %lu, blocks %d\n",
+		     address, blocks);
 
 	return sddr09_read21(us, address, blocks,
 			     CONTROL_SHIFT, content, use_sg);
@@ -1111,21 +1106,21 @@ sddr09_get_wp(struct us_data *us, struct sddr09_card_info *info) {
 
 	result = sddr09_read_status(us, &status);
 	if (result) {
-		US_DEBUGP("sddr09_get_wp: read_status fails\n");
+		usb_stor_dbg(us, "read_status fails\n");
 		return result;
 	}
-	US_DEBUGP("sddr09_get_wp: status 0x%02X", status);
+	usb_stor_dbg(us, "status 0x%02X", status);
 	if ((status & 0x80) == 0) {
 		info->flags |= SDDR09_WP;	/* write protected */
-		US_DEBUGP(" WP");
+		US_DEBUGPX(" WP");
 	}
 	if (status & 0x40)
-		US_DEBUGP(" Ready");
+		US_DEBUGPX(" Ready");
 	if (status & LUNBITS)
-		US_DEBUGP(" Suspended");
+		US_DEBUGPX(" Suspended");
 	if (status & 0x1)
-		US_DEBUGP(" Error");
-	US_DEBUGP("\n");
+		US_DEBUGPX(" Error");
+	US_DEBUGPX("\n");
 	return 0;
 }
 
@@ -1154,12 +1149,12 @@ sddr09_get_cardinfo(struct us_data *us, unsigned char flags) {
 	char blurbtxt[256];
 	int result;
 
-	US_DEBUGP("Reading capacity...\n");
+	usb_stor_dbg(us, "Reading capacity...\n");
 
 	result = sddr09_read_deviceID(us, deviceID);
 
 	if (result) {
-		US_DEBUGP("Result of read_deviceID is %d\n", result);
+		usb_stor_dbg(us, "Result of read_deviceID is %d\n", result);
 		printk(KERN_WARNING "sddr09: could not read card info\n");
 		return NULL;
 	}
@@ -1392,7 +1387,7 @@ sddr09_read_map(struct us_data *us) {
 		lbact += ct;
 	}
 	info->lbact = lbact;
-	US_DEBUGP("Found %d LBA's\n", lbact);
+	usb_stor_dbg(us, "Found %d LBA's\n", lbact);
 	result = 0;
 
  done:
@@ -1423,18 +1418,18 @@ sddr09_common_init(struct us_data *us) {
 
 	/* set the configuration -- STALL is an acceptable response here */
 	if (us->pusb_dev->actconfig->desc.bConfigurationValue != 1) {
-		US_DEBUGP("active config #%d != 1 ??\n", us->pusb_dev
-				->actconfig->desc.bConfigurationValue);
+		usb_stor_dbg(us, "active config #%d != 1 ??\n",
+			     us->pusb_dev->actconfig->desc.bConfigurationValue);
 		return -EINVAL;
 	}
 
 	result = usb_reset_configuration(us->pusb_dev);
-	US_DEBUGP("Result of usb_reset_configuration is %d\n", result);
+	usb_stor_dbg(us, "Result of usb_reset_configuration is %d\n", result);
 	if (result == -EPIPE) {
-		US_DEBUGP("-- stall on control interface\n");
+		usb_stor_dbg(us, "-- stall on control interface\n");
 	} else if (result != 0) {
 		/* it's not a stall, but another error -- time to bail */
-		US_DEBUGP("-- Unknown error.  Rejecting device\n");
+		usb_stor_dbg(us, "-- Unknown error.  Rejecting device\n");
 		return -EINVAL;
 	}
 
@@ -1464,20 +1459,20 @@ usb_stor_sddr09_dpcm_init(struct us_data *us) {
 
 	result = sddr09_send_command(us, 0x01, USB_DIR_IN, data, 2);
 	if (result) {
-		US_DEBUGP("sddr09_init: send_command fails\n");
+		usb_stor_dbg(us, "send_command fails\n");
 		return result;
 	}
 
-	US_DEBUGP("SDDR09init: %02X %02X\n", data[0], data[1]);
+	usb_stor_dbg(us, "%02X %02X\n", data[0], data[1]);
 	// get 07 02
 
 	result = sddr09_send_command(us, 0x08, USB_DIR_IN, data, 2);
 	if (result) {
-		US_DEBUGP("sddr09_init: 2nd send_command fails\n");
+		usb_stor_dbg(us, "2nd send_command fails\n");
 		return result;
 	}
 
-	US_DEBUGP("SDDR09init: %02X %02X\n", data[0], data[1]);
+	usb_stor_dbg(us, "%02X %02X\n", data[0], data[1]);
 	// get 07 00
 
 	result = sddr09_request_sense(us, data, 18);
@@ -1507,7 +1502,7 @@ static int dpcm_transport(struct scsi_cmnd *srb, struct us_data *us)
 {
 	int ret;
 
-	US_DEBUGP("dpcm_transport: LUN=%d\n", srb->device->lun);
+	usb_stor_dbg(us, "LUN=%d\n", srb->device->lun);
 
 	switch (srb->device->lun) {
 	case 0:
@@ -1533,8 +1528,7 @@ static int dpcm_transport(struct scsi_cmnd *srb, struct us_data *us)
 		break;
 
 	default:
-		US_DEBUGP("dpcm_transport: Invalid LUN %d\n",
-				srb->device->lun);
+		usb_stor_dbg(us, "Invalid LUN %d\n", srb->device->lun);
 		ret = USB_STOR_TRANSPORT_ERROR;
 		break;
 	}
@@ -1640,8 +1634,8 @@ static int sddr09_transport(struct scsi_cmnd *srb, struct us_data *us)
 		   or for all pages. */
 		/* %% We should check DBD %% */
 		if (modepage == 0x01 || modepage == 0x3F) {
-			US_DEBUGP("SDDR09: Dummy up request for "
-				  "mode page 0x%x\n", modepage);
+			usb_stor_dbg(us, "Dummy up request for mode page 0x%x\n",
+				     modepage);
 
 			memcpy(ptr, mode_page_01, sizeof(mode_page_01));
 			((__be16*)ptr)[0] = cpu_to_be16(sizeof(mode_page_01) - 2);
@@ -1667,8 +1661,8 @@ static int sddr09_transport(struct scsi_cmnd *srb, struct us_data *us)
 		page |= short_pack(srb->cmnd[5], srb->cmnd[4]);
 		pages = short_pack(srb->cmnd[8], srb->cmnd[7]);
 
-		US_DEBUGP("READ_10: read page %d pagect %d\n",
-			  page, pages);
+		usb_stor_dbg(us, "READ_10: read page %d pagect %d\n",
+			     page, pages);
 
 		result = sddr09_read_data(us, page, pages);
 		return (result == 0 ? USB_STOR_TRANSPORT_GOOD :
@@ -1682,8 +1676,8 @@ static int sddr09_transport(struct scsi_cmnd *srb, struct us_data *us)
 		page |= short_pack(srb->cmnd[5], srb->cmnd[4]);
 		pages = short_pack(srb->cmnd[8], srb->cmnd[7]);
 
-		US_DEBUGP("WRITE_10: write page %d pagect %d\n",
-			  page, pages);
+		usb_stor_dbg(us, "WRITE_10: write page %d pagect %d\n",
+			     page, pages);
 
 		result = sddr09_write_data(us, page, pages);
 		return (result == 0 ? USB_STOR_TRANSPORT_GOOD :
@@ -1710,12 +1704,12 @@ static int sddr09_transport(struct scsi_cmnd *srb, struct us_data *us)
 	for (i=0; i<12; i++)
 		sprintf(ptr+strlen(ptr), "%02X ", srb->cmnd[i]);
 
-	US_DEBUGP("SDDR09: Send control for command %s\n", ptr);
+	usb_stor_dbg(us, "Send control for command %s\n", ptr);
 
 	result = sddr09_send_scsi_command(us, srb->cmnd, 12);
 	if (result) {
-		US_DEBUGP("sddr09_transport: sddr09_send_scsi_command "
-			  "returns %d\n", result);
+		usb_stor_dbg(us, "sddr09_send_scsi_command returns %d\n",
+			     result);
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 
@@ -1727,10 +1721,10 @@ static int sddr09_transport(struct scsi_cmnd *srb, struct us_data *us)
 		unsigned int pipe = (srb->sc_data_direction == DMA_TO_DEVICE)
 				? us->send_bulk_pipe : us->recv_bulk_pipe;
 
-		US_DEBUGP("SDDR09: %s %d bytes\n",
-			  (srb->sc_data_direction == DMA_TO_DEVICE) ?
-			  "sending" : "receiving",
-			  scsi_bufflen(srb));
+		usb_stor_dbg(us, "%s %d bytes\n",
+			     (srb->sc_data_direction == DMA_TO_DEVICE) ?
+			     "sending" : "receiving",
+			     scsi_bufflen(srb));
 
 		result = usb_stor_bulk_srb(us, pipe, srb);
 
diff --git a/drivers/usb/storage/sddr55.c b/drivers/usb/storage/sddr55.c
index d278c5a99b7..aacedef9667 100644
--- a/drivers/usb/storage/sddr55.c
+++ b/drivers/usb/storage/sddr55.c
@@ -145,8 +145,7 @@ static int sddr55_status(struct us_data *us)
 	result = sddr55_bulk_transport(us,
 		DMA_TO_DEVICE, command, 8);
 
-	US_DEBUGP("Result for send_command in status %d\n",
-		result);
+	usb_stor_dbg(us, "Result for send_command in status %d\n", result);
 
 	if (result != USB_STOR_XFER_GOOD) {
 		set_sense_info (4, 0, 0);	/* hardware error */
@@ -236,9 +235,8 @@ static int sddr55_read_data(struct us_data *us,
 				info->blocksize - page);
 		len = pages << info->pageshift;
 
-		US_DEBUGP("Read %02X pages, from PBA %04X"
-			" (LBA %04X) page %02X\n",
-			pages, pba, lba, page);
+		usb_stor_dbg(us, "Read %02X pages, from PBA %04X (LBA %04X) page %02X\n",
+			     pages, pba, lba, page);
 
 		if (pba == NOT_ALLOCATED) {
 			/* no pba for this lba, fill with zeroes */
@@ -261,8 +259,8 @@ static int sddr55_read_data(struct us_data *us,
 			result = sddr55_bulk_transport(us,
 				DMA_TO_DEVICE, command, 8);
 
-			US_DEBUGP("Result for send_command in read_data %d\n",
-				result);
+			usb_stor_dbg(us, "Result for send_command in read_data %d\n",
+				     result);
 
 			if (result != USB_STOR_XFER_GOOD) {
 				result = USB_STOR_TRANSPORT_ERROR;
@@ -368,9 +366,8 @@ static int sddr55_write_data(struct us_data *us,
 		usb_stor_access_xfer_buf(buffer, len, us->srb,
 				&sg, &offset, FROM_XFER_BUF);
 
-		US_DEBUGP("Write %02X pages, to PBA %04X"
-			" (LBA %04X) page %02X\n",
-			pages, pba, lba, page);
+		usb_stor_dbg(us, "Write %02X pages, to PBA %04X (LBA %04X) page %02X\n",
+			     pages, pba, lba, page);
 			
 		command[4] = 0;
 
@@ -384,7 +381,7 @@ static int sddr55_write_data(struct us_data *us,
 			/* set pba to first block in zone lba is in */
 			pba = (lba / 1000) * 1024;
 
-			US_DEBUGP("No PBA for LBA %04X\n",lba);
+			usb_stor_dbg(us, "No PBA for LBA %04X\n", lba);
 
 			if (max_pba > 1024)
 				max_pba = 1024;
@@ -407,14 +404,15 @@ static int sddr55_write_data(struct us_data *us,
 
 			if (pba == -1) {
 				/* oh dear */
-				US_DEBUGP("Couldn't find unallocated block\n");
+				usb_stor_dbg(us, "Couldn't find unallocated block\n");
 
 				set_sense_info (3, 0x31, 0);	/* medium error */
 				result = USB_STOR_TRANSPORT_FAILED;
 				goto leave;
 			}
 
-			US_DEBUGP("Allocating PBA %04X for LBA %04X\n", pba, lba);
+			usb_stor_dbg(us, "Allocating PBA %04X for LBA %04X\n",
+				     pba, lba);
 
 			/* set writing to unallocated block flag */
 			command[4] = 0x40;
@@ -439,8 +437,8 @@ static int sddr55_write_data(struct us_data *us,
 			DMA_TO_DEVICE, command, 8);
 
 		if (result != USB_STOR_XFER_GOOD) {
-			US_DEBUGP("Result for send_command in write_data %d\n",
-			result);
+			usb_stor_dbg(us, "Result for send_command in write_data %d\n",
+				     result);
 
 			/* set_sense_info is superfluous here? */
 			set_sense_info (3, 0x3, 0);/* peripheral write error */
@@ -453,8 +451,8 @@ static int sddr55_write_data(struct us_data *us,
 			DMA_TO_DEVICE, buffer, len);
 
 		if (result != USB_STOR_XFER_GOOD) {
-			US_DEBUGP("Result for send_data in write_data %d\n",
-				  result);
+			usb_stor_dbg(us, "Result for send_data in write_data %d\n",
+				     result);
 
 			/* set_sense_info is superfluous here? */
 			set_sense_info (3, 0x3, 0);/* peripheral write error */
@@ -466,8 +464,8 @@ static int sddr55_write_data(struct us_data *us,
 		result = sddr55_bulk_transport(us, DMA_FROM_DEVICE, status, 6);
 
 		if (result != USB_STOR_XFER_GOOD) {
-			US_DEBUGP("Result for get_status in write_data %d\n",
-				  result);
+			usb_stor_dbg(us, "Result for get_status in write_data %d\n",
+				     result);
 
 			/* set_sense_info is superfluous here? */
 			set_sense_info (3, 0x3, 0);/* peripheral write error */
@@ -487,8 +485,8 @@ static int sddr55_write_data(struct us_data *us,
 			goto leave;
 		}
 
-		US_DEBUGP("Updating maps for LBA %04X: old PBA %04X, new PBA %04X\n",
-			lba, pba, new_pba);
+		usb_stor_dbg(us, "Updating maps for LBA %04X: old PBA %04X, new PBA %04X\n",
+			     lba, pba, new_pba);
 
 		/* update the lba<->pba maps, note new_pba might be the same as pba */
 		info->lba_to_pba[lba] = new_pba;
@@ -531,8 +529,8 @@ static int sddr55_read_deviceID(struct us_data *us,
 	command[7] = 0x84;
 	result = sddr55_bulk_transport(us, DMA_TO_DEVICE, command, 8);
 
-	US_DEBUGP("Result of send_control for device ID is %d\n",
-		result);
+	usb_stor_dbg(us, "Result of send_control for device ID is %d\n",
+		     result);
 
 	if (result != USB_STOR_XFER_GOOD)
 		return USB_STOR_TRANSPORT_ERROR;
@@ -568,20 +566,19 @@ static unsigned long sddr55_get_capacity(struct us_data *us) {
 	int result;
 	struct sddr55_card_info *info = (struct sddr55_card_info *)us->extra;
 
-	US_DEBUGP("Reading capacity...\n");
+	usb_stor_dbg(us, "Reading capacity...\n");
 
 	result = sddr55_read_deviceID(us,
 		&manufacturerID,
 		&deviceID);
 
-	US_DEBUGP("Result of read_deviceID is %d\n",
-		result);
+	usb_stor_dbg(us, "Result of read_deviceID is %d\n", result);
 
 	if (result != USB_STOR_XFER_GOOD)
 		return 0;
 
-	US_DEBUGP("Device ID = %02X\n", deviceID);
-	US_DEBUGP("Manuf  ID = %02X\n", manufacturerID);
+	usb_stor_dbg(us, "Device ID = %02X\n", deviceID);
+	usb_stor_dbg(us, "Manuf  ID = %02X\n", manufacturerID);
 
 	info->pageshift = 9;
 	info->smallpageshift = 0;
@@ -753,7 +750,7 @@ static int sddr55_read_map(struct us_data *us) {
 		}
 
 		if (lba<0x10 || (lba>=0x3E0 && lba<0x3EF))
-			US_DEBUGP("LBA %04X <-> PBA %04X\n", lba, i);
+			usb_stor_dbg(us, "LBA %04X <-> PBA %04X\n", lba, i);
 
 		info->lba_to_pba[lba + zone * 1000] = i;
 	}
@@ -808,7 +805,10 @@ static int sddr55_transport(struct scsi_cmnd *srb, struct us_data *us)
 	info = (struct sddr55_card_info *)(us->extra);
 
 	if (srb->cmnd[0] == REQUEST_SENSE) {
-		US_DEBUGP("SDDR55: request sense %02x/%02x/%02x\n", info->sense_data[2], info->sense_data[12], info->sense_data[13]);
+		usb_stor_dbg(us, "request sense %02x/%02x/%02x\n",
+			     info->sense_data[2],
+			     info->sense_data[12],
+			     info->sense_data[13]);
 
 		memcpy (ptr, info->sense_data, sizeof info->sense_data);
 		ptr[0] = 0x70;
@@ -892,13 +892,11 @@ static int sddr55_transport(struct scsi_cmnd *srb, struct us_data *us)
 		usb_stor_set_xfer_buf(ptr, sizeof(mode_page_01), srb);
 
 		if ( (srb->cmnd[2] & 0x3F) == 0x01 ) {
-			US_DEBUGP(
-			  "SDDR55: Dummy up request for mode page 1\n");
+			usb_stor_dbg(us, "Dummy up request for mode page 1\n");
 			return USB_STOR_TRANSPORT_GOOD;
 
 		} else if ( (srb->cmnd[2] & 0x3F) == 0x3F ) {
-			US_DEBUGP(
-			  "SDDR55: Dummy up request for all mode pages\n");
+			usb_stor_dbg(us, "Dummy up request for all mode pages\n");
 			return USB_STOR_TRANSPORT_GOOD;
 		}
 
@@ -908,10 +906,8 @@ static int sddr55_transport(struct scsi_cmnd *srb, struct us_data *us)
 
 	if (srb->cmnd[0] == ALLOW_MEDIUM_REMOVAL) {
 
-		US_DEBUGP(
-		  "SDDR55: %s medium removal. Not that I can do"
-		  " anything about it...\n",
-		  (srb->cmnd[4]&0x03) ? "Prevent" : "Allow");
+		usb_stor_dbg(us, "%s medium removal. Not that I can do anything about it...\n",
+			     (srb->cmnd[4]&0x03) ? "Prevent" : "Allow");
 
 		return USB_STOR_TRANSPORT_GOOD;
 
@@ -935,8 +931,8 @@ static int sddr55_transport(struct scsi_cmnd *srb, struct us_data *us)
 
 		if (lba >= info->max_log_blks) {
 
-			US_DEBUGP("Error: Requested LBA %04X exceeds maximum "
-			  "block %04X\n", lba, info->max_log_blks-1);
+			usb_stor_dbg(us, "Error: Requested LBA %04X exceeds maximum block %04X\n",
+				     lba, info->max_log_blks - 1);
 
 			set_sense_info (5, 0x24, 0);	/* invalid field in command */
 
@@ -946,15 +942,13 @@ static int sddr55_transport(struct scsi_cmnd *srb, struct us_data *us)
 		pba = info->lba_to_pba[lba];
 
 		if (srb->cmnd[0] == WRITE_10) {
-			US_DEBUGP("WRITE_10: write block %04X (LBA %04X) page %01X"
-				" pages %d\n",
-				pba, lba, page, pages);
+			usb_stor_dbg(us, "WRITE_10: write block %04X (LBA %04X) page %01X pages %d\n",
+				     pba, lba, page, pages);
 
 			return sddr55_write_data(us, lba, page, pages);
 		} else {
-			US_DEBUGP("READ_10: read block %04X (LBA %04X) page %01X"
-				" pages %d\n",
-				pba, lba, page, pages);
+			usb_stor_dbg(us, "READ_10: read block %04X (LBA %04X) page %01X pages %d\n",
+				     pba, lba, page, pages);
 
 			return sddr55_read_data(us, lba, page, pages);
 		}
diff --git a/drivers/usb/storage/shuttle_usbat.c b/drivers/usb/storage/shuttle_usbat.c
index daf2fc58ae0..4ef2a80728f 100644
--- a/drivers/usb/storage/shuttle_usbat.c
+++ b/drivers/usb/storage/shuttle_usbat.c
@@ -271,7 +271,7 @@ static int usbat_bulk_read(struct us_data *us,
 	if (len == 0)
 		return USB_STOR_XFER_GOOD;
 
-	US_DEBUGP("usbat_bulk_read: len = %d\n", len);
+	usb_stor_dbg(us, "len = %d\n", len);
 	return usb_stor_bulk_transfer_sg(us, us->recv_bulk_pipe, buf, len, use_sg, NULL);
 }
 
@@ -286,7 +286,7 @@ static int usbat_bulk_write(struct us_data *us,
 	if (len == 0)
 		return USB_STOR_XFER_GOOD;
 
-	US_DEBUGP("usbat_bulk_write:  len = %d\n", len);
+	usb_stor_dbg(us, "len = %d\n", len);
 	return usb_stor_bulk_transfer_sg(us, us->send_bulk_pipe, buf, len, use_sg, NULL);
 }
 
@@ -312,7 +312,7 @@ static int usbat_get_status(struct us_data *us, unsigned char *status)
 	int rc;
 	rc = usbat_read(us, USBAT_ATA, USBAT_ATA_STATUS, status);
 
-	US_DEBUGP("usbat_get_status: 0x%02X\n", (unsigned short) (*status));
+	usb_stor_dbg(us, "0x%02X\n", *status);
 	return rc;
 }
 
@@ -425,7 +425,7 @@ static int usbat_wait_not_busy(struct us_data *us, int minutes)
 			return USB_STOR_TRANSPORT_FAILED;
 
 		if ((*status & 0x80)==0x00) { /* not busy */
-			US_DEBUGP("Waited not busy for %d steps\n", i);
+			usb_stor_dbg(us, "Waited not busy for %d steps\n", i);
 			return USB_STOR_TRANSPORT_GOOD;
 		}
 
@@ -439,8 +439,8 @@ static int usbat_wait_not_busy(struct us_data *us, int minutes)
 			msleep(1000); /* X minutes */
 	}
 
-	US_DEBUGP("Waited not busy for %d minutes, timing out.\n",
-		minutes);
+	usb_stor_dbg(us, "Waited not busy for %d minutes, timing out\n",
+		     minutes);
 	return USB_STOR_TRANSPORT_FAILED;
 }
 
@@ -657,8 +657,9 @@ static int usbat_hp8200e_rw_block_test(struct us_data *us,
 			if (*status & 0x20) /* device fault */
 				return USB_STOR_TRANSPORT_FAILED;
 
-			US_DEBUGP("Redoing %s\n",
-			  direction==DMA_TO_DEVICE ? "write" : "read");
+			usb_stor_dbg(us, "Redoing %s\n",
+				     direction == DMA_TO_DEVICE
+				     ? "write" : "read");
 
 		} else if (result != USB_STOR_XFER_GOOD)
 			return USB_STOR_TRANSPORT_ERROR;
@@ -667,8 +668,8 @@ static int usbat_hp8200e_rw_block_test(struct us_data *us,
 
 	}
 
-	US_DEBUGP("Bummer! %s bulk data 20 times failed.\n",
-		direction==DMA_TO_DEVICE ? "Writing" : "Reading");
+	usb_stor_dbg(us, "Bummer! %s bulk data 20 times failed\n",
+		     direction == DMA_TO_DEVICE ? "Writing" : "Reading");
 
 	return USB_STOR_TRANSPORT_FAILED;
 }
@@ -827,7 +828,7 @@ static int usbat_read_user_io(struct us_data *us, unsigned char *data_flags)
 		data_flags,
 		USBAT_UIO_READ);
 
-	US_DEBUGP("usbat_read_user_io: UIO register reads %02X\n", (unsigned short) (*data_flags));
+	usb_stor_dbg(us, "UIO register reads %02X\n", *data_flags);
 
 	return result;
 }
@@ -900,10 +901,11 @@ static int usbat_device_enable_cdt(struct us_data *us)
 /*
  * Determine if media is present.
  */
-static int usbat_flash_check_media_present(unsigned char *uio)
+static int usbat_flash_check_media_present(struct us_data *us,
+					   unsigned char *uio)
 {
 	if (*uio & USBAT_UIO_UI0) {
-		US_DEBUGP("usbat_flash_check_media_present: no media detected\n");
+		usb_stor_dbg(us, "no media detected\n");
 		return USBAT_FLASH_MEDIA_NONE;
 	}
 
@@ -913,10 +915,11 @@ static int usbat_flash_check_media_present(unsigned char *uio)
 /*
  * Determine if media has changed since last operation
  */
-static int usbat_flash_check_media_changed(unsigned char *uio)
+static int usbat_flash_check_media_changed(struct us_data *us,
+					   unsigned char *uio)
 {
 	if (*uio & USBAT_UIO_0) {
-		US_DEBUGP("usbat_flash_check_media_changed: media change detected\n");
+		usb_stor_dbg(us, "media change detected\n");
 		return USBAT_FLASH_MEDIA_CHANGED;
 	}
 
@@ -937,7 +940,7 @@ static int usbat_flash_check_media(struct us_data *us,
 		return USB_STOR_TRANSPORT_ERROR;
 
 	/* Check for media existence */
-	rc = usbat_flash_check_media_present(uio);
+	rc = usbat_flash_check_media_present(us, uio);
 	if (rc == USBAT_FLASH_MEDIA_NONE) {
 		info->sense_key = 0x02;
 		info->sense_asc = 0x3A;
@@ -946,7 +949,7 @@ static int usbat_flash_check_media(struct us_data *us,
 	}
 
 	/* Check for media change */
-	rc = usbat_flash_check_media_changed(uio);
+	rc = usbat_flash_check_media_changed(us, uio);
 	if (rc == USBAT_FLASH_MEDIA_CHANGED) {
 
 		/* Reset and re-enable card detect */
@@ -1008,11 +1011,11 @@ static int usbat_identify_device(struct us_data *us,
 	/* Check for error bit, or if the command 'fell through' */
 	if (status == 0xA1 || !(status & 0x01)) {
 		/* Device is HP 8200 */
-		US_DEBUGP("usbat_identify_device: Detected HP8200 CDRW\n");
+		usb_stor_dbg(us, "Detected HP8200 CDRW\n");
 		info->devicetype = USBAT_DEV_HP8200;
 	} else {
 		/* Device is a CompactFlash reader/writer */
-		US_DEBUGP("usbat_identify_device: Detected Flash reader/writer\n");
+		usb_stor_dbg(us, "Detected Flash reader/writer\n");
 		info->devicetype = USBAT_DEV_FLASH;
 	}
 
@@ -1075,7 +1078,7 @@ static int usbat_flash_get_sector_count(struct us_data *us,
 	/* ATA command : IDENTIFY DEVICE */
 	rc = usbat_multiple_write(us, registers, command, 3);
 	if (rc != USB_STOR_XFER_GOOD) {
-		US_DEBUGP("usbat_flash_get_sector_count: Gah! identify_device failed\n");
+		usb_stor_dbg(us, "Gah! identify_device failed\n");
 		rc = USB_STOR_TRANSPORT_ERROR;
 		goto leave;
 	}
@@ -1178,7 +1181,7 @@ static int usbat_flash_read_data(struct us_data *us,
 		if (result != USB_STOR_TRANSPORT_GOOD)
 			goto leave;
   	 
-		US_DEBUGP("usbat_flash_read_data:  %d bytes\n", len);
+		usb_stor_dbg(us, "%d bytes\n", len);
 	
 		/* Store the data in the transfer buffer */
 		usb_stor_access_xfer_buf(buffer, len, us->srb,
@@ -1301,8 +1304,7 @@ static int usbat_hp8200e_handle_read10(struct us_data *us,
 	unsigned int sg_offset = 0;
 	struct scatterlist *sg = NULL;
 
-	US_DEBUGP("handle_read10: transfersize %d\n",
-		srb->transfersize);
+	usb_stor_dbg(us, "transfersize %d\n", srb->transfersize);
 
 	if (scsi_bufflen(srb) < 0x10000) {
 
@@ -1329,14 +1331,14 @@ static int usbat_hp8200e_handle_read10(struct us_data *us,
 		len = short_pack(data[7+9], data[7+8]);
 		len <<= 16;
 		len |= data[7+7];
-		US_DEBUGP("handle_read10: GPCMD_READ_CD: len %d\n", len);
+		usb_stor_dbg(us, "GPCMD_READ_CD: len %d\n", len);
 		srb->transfersize = scsi_bufflen(srb)/len;
 	}
 
 	if (!srb->transfersize)  {
 		srb->transfersize = 2048; /* A guess */
-		US_DEBUGP("handle_read10: transfersize 0, forcing %d\n",
-			srb->transfersize);
+		usb_stor_dbg(us, "transfersize 0, forcing %d\n",
+			     srb->transfersize);
 	}
 
 	/*
@@ -1346,7 +1348,7 @@ static int usbat_hp8200e_handle_read10(struct us_data *us,
 	 */
 
 	len = (65535/srb->transfersize) * srb->transfersize;
-	US_DEBUGP("Max read is %d bytes\n", len);
+	usb_stor_dbg(us, "Max read is %d bytes\n", len);
 	len = min(len, scsi_bufflen(srb));
 	buffer = kmalloc(len, GFP_NOIO);
 	if (buffer == NULL) /* bloody hell! */
@@ -1460,10 +1462,9 @@ static int init_usbat(struct us_data *us, int devicetype)
 	unsigned char *status = us->iobuf;
 
 	us->extra = kzalloc(sizeof(struct usbat_info), GFP_NOIO);
-	if (!us->extra) {
-		US_DEBUGP("init_usbat: Gah! Can't allocate storage for usbat info struct!\n");
+	if (!us->extra)
 		return 1;
-	}
+
 	info = (struct usbat_info *) (us->extra);
 
 	/* Enable peripheral control signals */
@@ -1473,7 +1474,7 @@ static int init_usbat(struct us_data *us, int devicetype)
 	if (rc != USB_STOR_XFER_GOOD)
 		return USB_STOR_TRANSPORT_ERROR;
 
-	US_DEBUGP("INIT 1\n");
+	usb_stor_dbg(us, "INIT 1\n");
 
 	msleep(2000);
 
@@ -1481,7 +1482,7 @@ static int init_usbat(struct us_data *us, int devicetype)
 	if (rc != USB_STOR_TRANSPORT_GOOD)
 		return rc;
 
-	US_DEBUGP("INIT 2\n");
+	usb_stor_dbg(us, "INIT 2\n");
 
 	rc = usbat_read_user_io(us, status);
 	if (rc != USB_STOR_XFER_GOOD)
@@ -1491,32 +1492,32 @@ static int init_usbat(struct us_data *us, int devicetype)
 	if (rc != USB_STOR_XFER_GOOD)
 		return USB_STOR_TRANSPORT_ERROR;
 
-	US_DEBUGP("INIT 3\n");
+	usb_stor_dbg(us, "INIT 3\n");
 
 	rc = usbat_select_and_test_registers(us);
 	if (rc != USB_STOR_TRANSPORT_GOOD)
 		return rc;
 
-	US_DEBUGP("INIT 4\n");
+	usb_stor_dbg(us, "INIT 4\n");
 
 	rc = usbat_read_user_io(us, status);
 	if (rc != USB_STOR_XFER_GOOD)
 		return USB_STOR_TRANSPORT_ERROR;
 
-	US_DEBUGP("INIT 5\n");
+	usb_stor_dbg(us, "INIT 5\n");
 
 	/* Enable peripheral control signals and card detect */
 	rc = usbat_device_enable_cdt(us);
 	if (rc != USB_STOR_TRANSPORT_GOOD)
 		return rc;
 
-	US_DEBUGP("INIT 6\n");
+	usb_stor_dbg(us, "INIT 6\n");
 
 	rc = usbat_read_user_io(us, status);
 	if (rc != USB_STOR_XFER_GOOD)
 		return USB_STOR_TRANSPORT_ERROR;
 
-	US_DEBUGP("INIT 7\n");
+	usb_stor_dbg(us, "INIT 7\n");
 
 	msleep(1400);
 
@@ -1524,19 +1525,19 @@ static int init_usbat(struct us_data *us, int devicetype)
 	if (rc != USB_STOR_XFER_GOOD)
 		return USB_STOR_TRANSPORT_ERROR;
 
-	US_DEBUGP("INIT 8\n");
+	usb_stor_dbg(us, "INIT 8\n");
 
 	rc = usbat_select_and_test_registers(us);
 	if (rc != USB_STOR_TRANSPORT_GOOD)
 		return rc;
 
-	US_DEBUGP("INIT 9\n");
+	usb_stor_dbg(us, "INIT 9\n");
 
 	/* At this point, we need to detect which device we are using */
 	if (usbat_set_transport(us, info, devicetype))
 		return USB_STOR_TRANSPORT_ERROR;
 
-	US_DEBUGP("INIT 10\n");
+	usb_stor_dbg(us, "INIT 10\n");
 
 	if (usbat_get_device_type(us) == USBAT_DEV_FLASH) { 
 		subcountH = 0x02;
@@ -1547,7 +1548,7 @@ static int init_usbat(struct us_data *us, int devicetype)
 	if (rc != USB_STOR_XFER_GOOD)
 		return USB_STOR_TRANSPORT_ERROR;
 
-	US_DEBUGP("INIT 11\n");
+	usb_stor_dbg(us, "INIT 11\n");
 
 	return USB_STOR_TRANSPORT_GOOD;
 }
@@ -1592,7 +1593,7 @@ static int usbat_hp8200e_transport(struct scsi_cmnd *srb, struct us_data *us)
 	}
 
 	result = usbat_get_status(us, status);
-	US_DEBUGP("Status = %02X\n", *status);
+	usb_stor_dbg(us, "Status = %02X\n", *status);
 	if (result != USB_STOR_XFER_GOOD)
 		return USB_STOR_TRANSPORT_ERROR;
 	if (srb->cmnd[0] == TEST_UNIT_READY)
@@ -1610,7 +1611,7 @@ static int usbat_hp8200e_transport(struct scsi_cmnd *srb, struct us_data *us)
 
 		if (result == USB_STOR_TRANSPORT_GOOD) {
 			transferred += len;
-			US_DEBUGP("Wrote %08X bytes\n", transferred);
+			usb_stor_dbg(us, "Wrote %08X bytes\n", transferred);
 		}
 
 		return result;
@@ -1623,8 +1624,8 @@ static int usbat_hp8200e_transport(struct scsi_cmnd *srb, struct us_data *us)
 	}
 
 	if (len > 0xFFFF) {
-		US_DEBUGP("Error: len = %08X... what do I do now?\n",
-			len);
+		usb_stor_dbg(us, "Error: len = %08X... what do I do now?\n",
+			     len);
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 
@@ -1693,7 +1694,7 @@ static int usbat_flash_transport(struct scsi_cmnd * srb, struct us_data *us)
 	};
 
 	if (srb->cmnd[0] == INQUIRY) {
-		US_DEBUGP("usbat_flash_transport: INQUIRY. Returning bogus response.\n");
+		usb_stor_dbg(us, "INQUIRY - Returning bogus response\n");
 		memcpy(ptr, inquiry_response, sizeof(inquiry_response));
 		fill_inquiry_response(us, ptr, 36);
 		return USB_STOR_TRANSPORT_GOOD;
@@ -1710,8 +1711,8 @@ static int usbat_flash_transport(struct scsi_cmnd * srb, struct us_data *us)
 
 		/* hard coded 512 byte sectors as per ATA spec */
 		info->ssize = 0x200;
-		US_DEBUGP("usbat_flash_transport: READ_CAPACITY: %ld sectors, %ld bytes per sector\n",
-			  info->sectors, info->ssize);
+		usb_stor_dbg(us, "READ_CAPACITY: %ld sectors, %ld bytes per sector\n",
+			     info->sectors, info->ssize);
 
 		/*
 		 * build the reply
@@ -1726,7 +1727,7 @@ static int usbat_flash_transport(struct scsi_cmnd * srb, struct us_data *us)
 	}
 
 	if (srb->cmnd[0] == MODE_SELECT_10) {
-		US_DEBUGP("usbat_flash_transport:  Gah! MODE_SELECT_10.\n");
+		usb_stor_dbg(us, "Gah! MODE_SELECT_10\n");
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 
@@ -1736,7 +1737,8 @@ static int usbat_flash_transport(struct scsi_cmnd * srb, struct us_data *us)
 
 		blocks = ((u32)(srb->cmnd[7]) << 8) | ((u32)(srb->cmnd[8]));
 
-		US_DEBUGP("usbat_flash_transport:  READ_10: read block 0x%04lx  count %ld\n", block, blocks);
+		usb_stor_dbg(us, "READ_10: read block 0x%04lx  count %ld\n",
+			     block, blocks);
 		return usbat_flash_read_data(us, info, block, blocks);
 	}
 
@@ -1750,7 +1752,8 @@ static int usbat_flash_transport(struct scsi_cmnd * srb, struct us_data *us)
 		blocks = ((u32)(srb->cmnd[6]) << 24) | ((u32)(srb->cmnd[7]) << 16) |
 		         ((u32)(srb->cmnd[8]) <<  8) | ((u32)(srb->cmnd[9]));
 
-		US_DEBUGP("usbat_flash_transport: READ_12: read block 0x%04lx  count %ld\n", block, blocks);
+		usb_stor_dbg(us, "READ_12: read block 0x%04lx  count %ld\n",
+			     block, blocks);
 		return usbat_flash_read_data(us, info, block, blocks);
 	}
 
@@ -1760,7 +1763,8 @@ static int usbat_flash_transport(struct scsi_cmnd * srb, struct us_data *us)
 
 		blocks = ((u32)(srb->cmnd[7]) << 8) | ((u32)(srb->cmnd[8]));
 
-		US_DEBUGP("usbat_flash_transport: WRITE_10: write block 0x%04lx  count %ld\n", block, blocks);
+		usb_stor_dbg(us, "WRITE_10: write block 0x%04lx  count %ld\n",
+			     block, blocks);
 		return usbat_flash_write_data(us, info, block, blocks);
 	}
 
@@ -1774,13 +1778,14 @@ static int usbat_flash_transport(struct scsi_cmnd * srb, struct us_data *us)
 		blocks = ((u32)(srb->cmnd[6]) << 24) | ((u32)(srb->cmnd[7]) << 16) |
 		         ((u32)(srb->cmnd[8]) <<  8) | ((u32)(srb->cmnd[9]));
 
-		US_DEBUGP("usbat_flash_transport: WRITE_12: write block 0x%04lx  count %ld\n", block, blocks);
+		usb_stor_dbg(us, "WRITE_12: write block 0x%04lx  count %ld\n",
+			     block, blocks);
 		return usbat_flash_write_data(us, info, block, blocks);
 	}
 
 
 	if (srb->cmnd[0] == TEST_UNIT_READY) {
-		US_DEBUGP("usbat_flash_transport: TEST_UNIT_READY.\n");
+		usb_stor_dbg(us, "TEST_UNIT_READY\n");
 
 		rc = usbat_flash_check_media(us, info);
 		if (rc != USB_STOR_TRANSPORT_GOOD)
@@ -1790,7 +1795,7 @@ static int usbat_flash_transport(struct scsi_cmnd * srb, struct us_data *us)
 	}
 
 	if (srb->cmnd[0] == REQUEST_SENSE) {
-		US_DEBUGP("usbat_flash_transport: REQUEST_SENSE.\n");
+		usb_stor_dbg(us, "REQUEST_SENSE\n");
 
 		memset(ptr, 0, 18);
 		ptr[0] = 0xF0;
@@ -1811,8 +1816,8 @@ static int usbat_flash_transport(struct scsi_cmnd * srb, struct us_data *us)
 		return USB_STOR_TRANSPORT_GOOD;
 	}
 
-	US_DEBUGP("usbat_flash_transport: Gah! Unknown command: %d (0x%x)\n",
-			  srb->cmnd[0], srb->cmnd[0]);
+	usb_stor_dbg(us, "Gah! Unknown command: %d (0x%x)\n",
+		     srb->cmnd[0], srb->cmnd[0]);
 	info->sense_key = 0x05;
 	info->sense_asc = 0x20;
 	info->sense_ascq = 0x00;
diff --git a/drivers/usb/storage/sierra_ms.c b/drivers/usb/storage/sierra_ms.c
index 17e36952bce..2ea657be14c 100644
--- a/drivers/usb/storage/sierra_ms.c
+++ b/drivers/usb/storage/sierra_ms.c
@@ -47,7 +47,7 @@ static bool containsFullLinuxPackage(struct swoc_info *swocInfo)
 static int sierra_set_ms_mode(struct usb_device *udev, __u16 eSWocMode)
 {
 	int result;
-	US_DEBUGP("SWIMS: %s", "DEVICE MODE SWITCH\n");
+	dev_dbg(&udev->dev, "SWIMS: %s", "DEVICE MODE SWITCH\n");
 	result = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
 			SWIMS_USB_REQUEST_SetSwocMode,	/* __u8 request      */
 			USB_TYPE_VENDOR | USB_DIR_OUT,	/* __u8 request type */
@@ -65,7 +65,7 @@ static int sierra_get_swoc_info(struct usb_device *udev,
 {
 	int result;
 
-	US_DEBUGP("SWIMS: Attempting to get TRU-Install info.\n");
+	dev_dbg(&udev->dev, "SWIMS: Attempting to get TRU-Install info\n");
 
 	result = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0),
 			SWIMS_USB_REQUEST_GetSwocInfo,	/* __u8 request      */
@@ -81,11 +81,11 @@ static int sierra_get_swoc_info(struct usb_device *udev,
 	return result;
 }
 
-static void debug_swoc(struct swoc_info *swocInfo)
+static void debug_swoc(const struct device *dev, struct swoc_info *swocInfo)
 {
-	US_DEBUGP("SWIMS: SWoC Rev: %02d \n", swocInfo->rev);
-	US_DEBUGP("SWIMS: Linux SKU: %04X \n", swocInfo->LinuxSKU);
-	US_DEBUGP("SWIMS: Linux Version: %04X \n", swocInfo->LinuxVer);
+	dev_dbg(dev, "SWIMS: SWoC Rev: %02d\n", swocInfo->rev);
+	dev_dbg(dev, "SWIMS: Linux SKU: %04X\n", swocInfo->LinuxSKU);
+	dev_dbg(dev, "SWIMS: Linux Version: %04X\n", swocInfo->LinuxVer);
 }
 
 
@@ -101,18 +101,17 @@ static ssize_t show_truinst(struct device *dev, struct device_attribute *attr,
 	} else {
 		swocInfo = kmalloc(sizeof(struct swoc_info), GFP_KERNEL);
 		if (!swocInfo) {
-			US_DEBUGP("SWIMS: Allocation failure\n");
 			snprintf(buf, PAGE_SIZE, "Error\n");
 			return -ENOMEM;
 		}
 		result = sierra_get_swoc_info(udev, swocInfo);
 		if (result < 0) {
-			US_DEBUGP("SWIMS: failed SWoC query\n");
+			dev_dbg(dev, "SWIMS: failed SWoC query\n");
 			kfree(swocInfo);
 			snprintf(buf, PAGE_SIZE, "Error\n");
 			return -EIO;
 		}
-		debug_swoc(swocInfo);
+		debug_swoc(dev, swocInfo);
 		result = snprintf(buf, PAGE_SIZE,
 			"REV=%02d SKU=%04X VER=%04X\n",
 			swocInfo->rev,
@@ -138,61 +137,55 @@ int sierra_ms_init(struct us_data *us)
 	sh = us_to_host(us);
 	scsi_get_host_dev(sh);
 
-	US_DEBUGP("SWIMS: sierra_ms_init called\n");
-
 	/* Force Modem mode */
 	if (swi_tru_install == TRU_FORCE_MODEM) {
-		US_DEBUGP("SWIMS: %s", "Forcing Modem Mode\n");
+		usb_stor_dbg(us, "SWIMS: Forcing Modem Mode\n");
 		result = sierra_set_ms_mode(udev, SWIMS_SET_MODE_Modem);
 		if (result < 0)
-			US_DEBUGP("SWIMS: Failed to switch to modem mode.\n");
+			usb_stor_dbg(us, "SWIMS: Failed to switch to modem mode\n");
 		return -EIO;
 	}
 	/* Force Mass Storage mode (keep CD-Rom) */
 	else if (swi_tru_install == TRU_FORCE_MS) {
-		US_DEBUGP("SWIMS: %s", "Forcing Mass Storage Mode\n");
+		usb_stor_dbg(us, "SWIMS: Forcing Mass Storage Mode\n");
 		goto complete;
 	}
 	/* Normal TRU-Install Logic */
 	else {
-		US_DEBUGP("SWIMS: %s", "Normal SWoC Logic\n");
+		usb_stor_dbg(us, "SWIMS: Normal SWoC Logic\n");
 
 		swocInfo = kmalloc(sizeof(struct swoc_info),
 				GFP_KERNEL);
-		if (!swocInfo) {
-			US_DEBUGP("SWIMS: %s", "Allocation failure\n");
+		if (!swocInfo)
 			return -ENOMEM;
-		}
 
 		retries = 3;
 		do {
 			retries--;
 			result = sierra_get_swoc_info(udev, swocInfo);
 			if (result < 0) {
-				US_DEBUGP("SWIMS: %s", "Failed SWoC query\n");
+				usb_stor_dbg(us, "SWIMS: Failed SWoC query\n");
 				schedule_timeout_uninterruptible(2*HZ);
 			}
 		} while (retries && result < 0);
 
 		if (result < 0) {
-			US_DEBUGP("SWIMS: %s",
-				  "Completely failed SWoC query\n");
+			usb_stor_dbg(us, "SWIMS: Completely failed SWoC query\n");
 			kfree(swocInfo);
 			return -EIO;
 		}
 
-		debug_swoc(swocInfo);
+		debug_swoc(&us->pusb_dev->dev, swocInfo);
 
 		/* If there is not Linux software on the TRU-Install device
 		 * then switch to modem mode
 		 */
 		if (!containsFullLinuxPackage(swocInfo)) {
-			US_DEBUGP("SWIMS: %s",
-				"Switching to Modem Mode\n");
+			usb_stor_dbg(us, "SWIMS: Switching to Modem Mode\n");
 			result = sierra_set_ms_mode(udev,
 				SWIMS_SET_MODE_Modem);
 			if (result < 0)
-				US_DEBUGP("SWIMS: Failed to switch modem\n");
+				usb_stor_dbg(us, "SWIMS: Failed to switch modem\n");
 			kfree(swocInfo);
 			return -EIO;
 		}
diff --git a/drivers/usb/storage/transport.c b/drivers/usb/storage/transport.c
index c0543c83923..22c7d4360fa 100644
--- a/drivers/usb/storage/transport.c
+++ b/drivers/usb/storage/transport.c
@@ -166,7 +166,7 @@ static int usb_stor_msg_common(struct us_data *us, int timeout)
 
 		/* cancel the URB, if it hasn't been cancelled already */
 		if (test_and_clear_bit(US_FLIDX_URB_ACTIVE, &us->dflags)) {
-			US_DEBUGP("-- cancelling URB\n");
+			usb_stor_dbg(us, "-- cancelling URB\n");
 			usb_unlink_urb(us->current_urb);
 		}
 	}
@@ -178,8 +178,8 @@ static int usb_stor_msg_common(struct us_data *us, int timeout)
 	clear_bit(US_FLIDX_URB_ACTIVE, &us->dflags);
 
 	if (timeleft <= 0) {
-		US_DEBUGP("%s -- cancelling URB\n",
-			  timeleft == 0 ? "Timeout" : "Signal");
+		usb_stor_dbg(us, "%s -- cancelling URB\n",
+			     timeleft == 0 ? "Timeout" : "Signal");
 		usb_kill_urb(us->current_urb);
 	}
 
@@ -197,9 +197,8 @@ int usb_stor_control_msg(struct us_data *us, unsigned int pipe,
 {
 	int status;
 
-	US_DEBUGP("%s: rq=%02x rqtype=%02x value=%04x index=%02x len=%u\n",
-			__func__, request, requesttype,
-			value, index, size);
+	usb_stor_dbg(us, "rq=%02x rqtype=%02x value=%04x index=%02x len=%u\n",
+		     request, requesttype, value, index, size);
 
 	/* fill in the devrequest structure */
 	us->cr->bRequestType = requesttype;
@@ -249,7 +248,7 @@ int usb_stor_clear_halt(struct us_data *us, unsigned int pipe)
 	if (result >= 0)
 		usb_reset_endpoint(us->pusb_dev, endp);
 
-	US_DEBUGP("%s: result = %d\n", __func__, result);
+	usb_stor_dbg(us, "result = %d\n", result);
 	return result;
 }
 EXPORT_SYMBOL_GPL(usb_stor_clear_halt);
@@ -265,18 +264,18 @@ EXPORT_SYMBOL_GPL(usb_stor_clear_halt);
 static int interpret_urb_result(struct us_data *us, unsigned int pipe,
 		unsigned int length, int result, unsigned int partial)
 {
-	US_DEBUGP("Status code %d; transferred %u/%u\n",
-			result, partial, length);
+	usb_stor_dbg(us, "Status code %d; transferred %u/%u\n",
+		     result, partial, length);
 	switch (result) {
 
 	/* no error code; did we send all the data? */
 	case 0:
 		if (partial != length) {
-			US_DEBUGP("-- short transfer\n");
+			usb_stor_dbg(us, "-- short transfer\n");
 			return USB_STOR_XFER_SHORT;
 		}
 
-		US_DEBUGP("-- transfer complete\n");
+		usb_stor_dbg(us, "-- transfer complete\n");
 		return USB_STOR_XFER_GOOD;
 
 	/* stalled */
@@ -284,39 +283,40 @@ static int interpret_urb_result(struct us_data *us, unsigned int pipe,
 		/* for control endpoints, (used by CB[I]) a stall indicates
 		 * a failed command */
 		if (usb_pipecontrol(pipe)) {
-			US_DEBUGP("-- stall on control pipe\n");
+			usb_stor_dbg(us, "-- stall on control pipe\n");
 			return USB_STOR_XFER_STALLED;
 		}
 
 		/* for other sorts of endpoint, clear the stall */
-		US_DEBUGP("clearing endpoint halt for pipe 0x%x\n", pipe);
+		usb_stor_dbg(us, "clearing endpoint halt for pipe 0x%x\n",
+			     pipe);
 		if (usb_stor_clear_halt(us, pipe) < 0)
 			return USB_STOR_XFER_ERROR;
 		return USB_STOR_XFER_STALLED;
 
 	/* babble - the device tried to send more than we wanted to read */
 	case -EOVERFLOW:
-		US_DEBUGP("-- babble\n");
+		usb_stor_dbg(us, "-- babble\n");
 		return USB_STOR_XFER_LONG;
 
 	/* the transfer was cancelled by abort, disconnect, or timeout */
 	case -ECONNRESET:
-		US_DEBUGP("-- transfer cancelled\n");
+		usb_stor_dbg(us, "-- transfer cancelled\n");
 		return USB_STOR_XFER_ERROR;
 
 	/* short scatter-gather read transfer */
 	case -EREMOTEIO:
-		US_DEBUGP("-- short read transfer\n");
+		usb_stor_dbg(us, "-- short read transfer\n");
 		return USB_STOR_XFER_SHORT;
 
 	/* abort or disconnect in progress */
 	case -EIO:
-		US_DEBUGP("-- abort or disconnect in progress\n");
+		usb_stor_dbg(us, "-- abort or disconnect in progress\n");
 		return USB_STOR_XFER_ERROR;
 
 	/* the catch-all error case */
 	default:
-		US_DEBUGP("-- unknown error\n");
+		usb_stor_dbg(us, "-- unknown error\n");
 		return USB_STOR_XFER_ERROR;
 	}
 }
@@ -331,9 +331,8 @@ int usb_stor_ctrl_transfer(struct us_data *us, unsigned int pipe,
 {
 	int result;
 
-	US_DEBUGP("%s: rq=%02x rqtype=%02x value=%04x index=%02x len=%u\n",
-			__func__, request, requesttype,
-			value, index, size);
+	usb_stor_dbg(us, "rq=%02x rqtype=%02x value=%04x index=%02x len=%u\n",
+		     request, requesttype, value, index, size);
 
 	/* fill in the devrequest structure */
 	us->cr->bRequestType = requesttype;
@@ -367,7 +366,7 @@ static int usb_stor_intr_transfer(struct us_data *us, void *buf,
 	unsigned int pipe = us->recv_intr_pipe;
 	unsigned int maxp;
 
-	US_DEBUGP("%s: xfer %u bytes\n", __func__, length);
+	usb_stor_dbg(us, "xfer %u bytes\n", length);
 
 	/* calculate the max packet size */
 	maxp = usb_maxpacket(us->pusb_dev, pipe, usb_pipeout(pipe));
@@ -394,7 +393,7 @@ int usb_stor_bulk_transfer_buf(struct us_data *us, unsigned int pipe,
 {
 	int result;
 
-	US_DEBUGP("%s: xfer %u bytes\n", __func__, length);
+	usb_stor_dbg(us, "xfer %u bytes\n", length);
 
 	/* fill and submit the URB */
 	usb_fill_bulk_urb(us->current_urb, us->pusb_dev, pipe, buf, length,
@@ -426,12 +425,11 @@ static int usb_stor_bulk_transfer_sglist(struct us_data *us, unsigned int pipe,
 		return USB_STOR_XFER_ERROR;
 
 	/* initialize the scatter-gather request block */
-	US_DEBUGP("%s: xfer %u bytes, %d entries\n", __func__,
-			length, num_sg);
+	usb_stor_dbg(us, "xfer %u bytes, %d entries\n", length, num_sg);
 	result = usb_sg_init(&us->current_sg, us->pusb_dev, pipe, 0,
 			sg, num_sg, length, GFP_NOIO);
 	if (result) {
-		US_DEBUGP("usb_sg_init returned %d\n", result);
+		usb_stor_dbg(us, "usb_sg_init returned %d\n", result);
 		return USB_STOR_XFER_ERROR;
 	}
 
@@ -444,7 +442,7 @@ static int usb_stor_bulk_transfer_sglist(struct us_data *us, unsigned int pipe,
 
 		/* cancel the request, if it hasn't been cancelled already */
 		if (test_and_clear_bit(US_FLIDX_SG_ACTIVE, &us->dflags)) {
-			US_DEBUGP("-- cancelling sg request\n");
+			usb_stor_dbg(us, "-- cancelling sg request\n");
 			usb_sg_cancel(&us->current_sg);
 		}
 	}
@@ -609,14 +607,14 @@ void usb_stor_invoke_transport(struct scsi_cmnd *srb, struct us_data *us)
 	 * short-circuit all other processing
 	 */
 	if (test_bit(US_FLIDX_TIMED_OUT, &us->dflags)) {
-		US_DEBUGP("-- command was aborted\n");
+		usb_stor_dbg(us, "-- command was aborted\n");
 		srb->result = DID_ABORT << 16;
 		goto Handle_Errors;
 	}
 
 	/* if there is a transport error, reset and don't auto-sense */
 	if (result == USB_STOR_TRANSPORT_ERROR) {
-		US_DEBUGP("-- transport indicates error, resetting\n");
+		usb_stor_dbg(us, "-- transport indicates error, resetting\n");
 		srb->result = DID_ERROR << 16;
 		goto Handle_Errors;
 	}
@@ -645,7 +643,7 @@ void usb_stor_invoke_transport(struct scsi_cmnd *srb, struct us_data *us)
 	 */
 	if ((us->protocol == USB_PR_CB || us->protocol == USB_PR_DPCM_USB) &&
 			srb->sc_data_direction != DMA_FROM_DEVICE) {
-		US_DEBUGP("-- CB transport device requiring auto-sense\n");
+		usb_stor_dbg(us, "-- CB transport device requiring auto-sense\n");
 		need_auto_sense = 1;
 	}
 
@@ -655,7 +653,7 @@ void usb_stor_invoke_transport(struct scsi_cmnd *srb, struct us_data *us)
 	 * "failure" and an "error" in the transport mechanism.
 	 */
 	if (result == USB_STOR_TRANSPORT_FAILED) {
-		US_DEBUGP("-- transport indicates command failure\n");
+		usb_stor_dbg(us, "-- transport indicates command failure\n");
 		need_auto_sense = 1;
 	}
 
@@ -670,7 +668,7 @@ void usb_stor_invoke_transport(struct scsi_cmnd *srb, struct us_data *us)
 	    !(us->fflags & US_FL_SANE_SENSE) &&
 	    !(us->fflags & US_FL_BAD_SENSE) &&
 	    !(srb->cmnd[2] & 0x20))) {
-		US_DEBUGP("-- SAT supported, increasing auto-sense\n");
+		usb_stor_dbg(us, "-- SAT supported, increasing auto-sense\n");
 		us->fflags |= US_FL_SANE_SENSE;
 	}
 
@@ -684,7 +682,7 @@ void usb_stor_invoke_transport(struct scsi_cmnd *srb, struct us_data *us)
 	      (srb->cmnd[0] == MODE_SENSE) ||
 	      (srb->cmnd[0] == LOG_SENSE) ||
 	      (srb->cmnd[0] == MODE_SENSE_10))) {
-		US_DEBUGP("-- unexpectedly short transfer\n");
+		usb_stor_dbg(us, "-- unexpectedly short transfer\n");
 	}
 
 	/* Now, if we need to do the auto-sense, let's do it */
@@ -700,7 +698,7 @@ void usb_stor_invoke_transport(struct scsi_cmnd *srb, struct us_data *us)
 		if (us->fflags & US_FL_SANE_SENSE)
 			sense_size = ~0;
 Retry_Sense:
-		US_DEBUGP("Issuing auto-REQUEST_SENSE\n");
+		usb_stor_dbg(us, "Issuing auto-REQUEST_SENSE\n");
 
 		scsi_eh_prep_cmnd(srb, &ses, NULL, 0, sense_size);
 
@@ -719,7 +717,7 @@ Retry_Sense:
 		scsi_eh_restore_cmnd(srb, &ses);
 
 		if (test_bit(US_FLIDX_TIMED_OUT, &us->dflags)) {
-			US_DEBUGP("-- auto-sense aborted\n");
+			usb_stor_dbg(us, "-- auto-sense aborted\n");
 			srb->result = DID_ABORT << 16;
 
 			/* If SANE_SENSE caused this problem, disable it */
@@ -737,7 +735,7 @@ Retry_Sense:
 		 */
 		if (temp_result == USB_STOR_TRANSPORT_FAILED &&
 				sense_size != US_SENSE_SIZE) {
-			US_DEBUGP("-- auto-sense failure, retry small sense\n");
+			usb_stor_dbg(us, "-- auto-sense failure, retry small sense\n");
 			sense_size = US_SENSE_SIZE;
 			us->fflags &= ~US_FL_SANE_SENSE;
 			us->fflags |= US_FL_BAD_SENSE;
@@ -746,7 +744,7 @@ Retry_Sense:
 
 		/* Other failures */
 		if (temp_result != USB_STOR_TRANSPORT_GOOD) {
-			US_DEBUGP("-- auto-sense failure\n");
+			usb_stor_dbg(us, "-- auto-sense failure\n");
 
 			/* we skip the reset if this happens to be a
 			 * multi-target device, since failure of an
@@ -766,27 +764,28 @@ Retry_Sense:
 		    !(us->fflags & US_FL_SANE_SENSE) &&
 		    !(us->fflags & US_FL_BAD_SENSE) &&
 		    (srb->sense_buffer[0] & 0x7C) == 0x70) {
-			US_DEBUGP("-- SANE_SENSE support enabled\n");
+			usb_stor_dbg(us, "-- SANE_SENSE support enabled\n");
 			us->fflags |= US_FL_SANE_SENSE;
 
 			/* Indicate to the user that we truncated their sense
 			 * because we didn't know it supported larger sense.
 			 */
-			US_DEBUGP("-- Sense data truncated to %i from %i\n",
-			          US_SENSE_SIZE,
-			          srb->sense_buffer[7] + 8);
+			usb_stor_dbg(us, "-- Sense data truncated to %i from %i\n",
+				     US_SENSE_SIZE,
+				     srb->sense_buffer[7] + 8);
 			srb->sense_buffer[7] = (US_SENSE_SIZE - 8);
 		}
 
 		scsi_normalize_sense(srb->sense_buffer, SCSI_SENSE_BUFFERSIZE,
 				     &sshdr);
 
-		US_DEBUGP("-- Result from auto-sense is %d\n", temp_result);
-		US_DEBUGP("-- code: 0x%x, key: 0x%x, ASC: 0x%x, ASCQ: 0x%x\n",
-			  sshdr.response_code, sshdr.sense_key,
-			  sshdr.asc, sshdr.ascq);
+		usb_stor_dbg(us, "-- Result from auto-sense is %d\n",
+			     temp_result);
+		usb_stor_dbg(us, "-- code: 0x%x, key: 0x%x, ASC: 0x%x, ASCQ: 0x%x\n",
+			     sshdr.response_code, sshdr.sense_key,
+			     sshdr.asc, sshdr.ascq);
 #ifdef CONFIG_USB_STORAGE_DEBUG
-		usb_stor_show_sense(sshdr.sense_key, sshdr.asc, sshdr.ascq);
+		usb_stor_show_sense(us, sshdr.sense_key, sshdr.asc, sshdr.ascq);
 #endif
 
 		/* set the result so the higher layers expect this data */
@@ -892,20 +891,18 @@ Retry_Sense:
 /* Stop the current URB transfer */
 void usb_stor_stop_transport(struct us_data *us)
 {
-	US_DEBUGP("%s called\n", __func__);
-
 	/* If the state machine is blocked waiting for an URB,
 	 * let's wake it up.  The test_and_clear_bit() call
 	 * guarantees that if a URB has just been submitted,
 	 * it won't be cancelled more than once. */
 	if (test_and_clear_bit(US_FLIDX_URB_ACTIVE, &us->dflags)) {
-		US_DEBUGP("-- cancelling URB\n");
+		usb_stor_dbg(us, "-- cancelling URB\n");
 		usb_unlink_urb(us->current_urb);
 	}
 
 	/* If we are waiting for a scatter-gather operation, cancel it. */
 	if (test_and_clear_bit(US_FLIDX_SG_ACTIVE, &us->dflags)) {
-		US_DEBUGP("-- cancelling sg request\n");
+		usb_stor_dbg(us, "-- cancelling sg request\n");
 		usb_sg_cancel(&us->current_sg);
 	}
 }
@@ -928,7 +925,8 @@ int usb_stor_CB_transport(struct scsi_cmnd *srb, struct us_data *us)
 				      us->ifnum, srb->cmnd, srb->cmd_len);
 
 	/* check the return code for the command */
-	US_DEBUGP("Call to usb_stor_ctrl_transfer() returned %d\n", result);
+	usb_stor_dbg(us, "Call to usb_stor_ctrl_transfer() returned %d\n",
+		     result);
 
 	/* if we stalled the command, it means command failed */
 	if (result == USB_STOR_XFER_STALLED) {
@@ -946,7 +944,7 @@ int usb_stor_CB_transport(struct scsi_cmnd *srb, struct us_data *us)
 		pipe = srb->sc_data_direction == DMA_FROM_DEVICE ? 
 				us->recv_bulk_pipe : us->send_bulk_pipe;
 		result = usb_stor_bulk_srb(us, pipe, srb);
-		US_DEBUGP("CBI data stage result is 0x%x\n", result);
+		usb_stor_dbg(us, "CBI data stage result is 0x%x\n", result);
 
 		/* if we stalled the data transfer it means command failed */
 		if (result == USB_STOR_XFER_STALLED)
@@ -964,8 +962,8 @@ int usb_stor_CB_transport(struct scsi_cmnd *srb, struct us_data *us)
 		return USB_STOR_TRANSPORT_GOOD;
 
 	result = usb_stor_intr_transfer(us, us->iobuf, 2);
-	US_DEBUGP("Got interrupt data (0x%x, 0x%x)\n", 
-			us->iobuf[0], us->iobuf[1]);
+	usb_stor_dbg(us, "Got interrupt data (0x%x, 0x%x)\n",
+		     us->iobuf[0], us->iobuf[1]);
 	if (result != USB_STOR_XFER_GOOD)
 		return USB_STOR_TRANSPORT_ERROR;
 
@@ -992,8 +990,8 @@ int usb_stor_CB_transport(struct scsi_cmnd *srb, struct us_data *us)
 	 * into the first byte -- so if it's non-zero, call it a failure.
 	 */
 	if (us->iobuf[0]) {
-		US_DEBUGP("CBI IRQ data showed reserved bType 0x%x\n",
-				us->iobuf[0]);
+		usb_stor_dbg(us, "CBI IRQ data showed reserved bType 0x%x\n",
+			     us->iobuf[0]);
 		goto Failed;
 
 	}
@@ -1034,8 +1032,8 @@ int usb_stor_Bulk_max_lun(struct us_data *us)
 				 USB_RECIP_INTERFACE,
 				 0, us->ifnum, us->iobuf, 1, 10*HZ);
 
-	US_DEBUGP("GetMaxLUN command result is %d, data is %d\n", 
-		  result, us->iobuf[0]);
+	usb_stor_dbg(us, "GetMaxLUN command result is %d, data is %d\n",
+		     result, us->iobuf[0]);
 
 	/* if we have a successful request, return the result */
 	if (result > 0)
@@ -1084,14 +1082,14 @@ int usb_stor_Bulk_transport(struct scsi_cmnd *srb, struct us_data *us)
 	memcpy(bcb->CDB, srb->cmnd, bcb->Length);
 
 	/* send it to out endpoint */
-	US_DEBUGP("Bulk Command S 0x%x T 0x%x L %d F %d Trg %d LUN %d CL %d\n",
-			le32_to_cpu(bcb->Signature), bcb->Tag,
-			le32_to_cpu(bcb->DataTransferLength), bcb->Flags,
-			(bcb->Lun >> 4), (bcb->Lun & 0x0F), 
-			bcb->Length);
+	usb_stor_dbg(us, "Bulk Command S 0x%x T 0x%x L %d F %d Trg %d LUN %d CL %d\n",
+		     le32_to_cpu(bcb->Signature), bcb->Tag,
+		     le32_to_cpu(bcb->DataTransferLength), bcb->Flags,
+		     (bcb->Lun >> 4), (bcb->Lun & 0x0F),
+		     bcb->Length);
 	result = usb_stor_bulk_transfer_buf(us, us->send_bulk_pipe,
 				bcb, cbwlen, NULL);
-	US_DEBUGP("Bulk command transfer result=%d\n", result);
+	usb_stor_dbg(us, "Bulk command transfer result=%d\n", result);
 	if (result != USB_STOR_XFER_GOOD)
 		return USB_STOR_TRANSPORT_ERROR;
 
@@ -1108,7 +1106,7 @@ int usb_stor_Bulk_transport(struct scsi_cmnd *srb, struct us_data *us)
 		unsigned int pipe = srb->sc_data_direction == DMA_FROM_DEVICE ? 
 				us->recv_bulk_pipe : us->send_bulk_pipe;
 		result = usb_stor_bulk_srb(us, pipe, srb);
-		US_DEBUGP("Bulk data transfer result 0x%x\n", result);
+		usb_stor_dbg(us, "Bulk data transfer result 0x%x\n", result);
 		if (result == USB_STOR_XFER_ERROR)
 			return USB_STOR_TRANSPORT_ERROR;
 
@@ -1127,7 +1125,7 @@ int usb_stor_Bulk_transport(struct scsi_cmnd *srb, struct us_data *us)
 	 */
 
 	/* get CSW for device status */
-	US_DEBUGP("Attempting to get CSW...\n");
+	usb_stor_dbg(us, "Attempting to get CSW...\n");
 	result = usb_stor_bulk_transfer_buf(us, us->recv_bulk_pipe,
 				bcs, US_BULK_CS_WRAP_LEN, &cswlen);
 
@@ -1136,7 +1134,7 @@ int usb_stor_Bulk_transport(struct scsi_cmnd *srb, struct us_data *us)
 	 * CSWs.  If we encounter such a thing, try to read the CSW again.
 	 */
 	if (result == USB_STOR_XFER_SHORT && cswlen == 0) {
-		US_DEBUGP("Received 0-length CSW; retrying...\n");
+		usb_stor_dbg(us, "Received 0-length CSW; retrying...\n");
 		result = usb_stor_bulk_transfer_buf(us, us->recv_bulk_pipe,
 				bcs, US_BULK_CS_WRAP_LEN, &cswlen);
 	}
@@ -1145,24 +1143,24 @@ int usb_stor_Bulk_transport(struct scsi_cmnd *srb, struct us_data *us)
 	if (result == USB_STOR_XFER_STALLED) {
 
 		/* get the status again */
-		US_DEBUGP("Attempting to get CSW (2nd try)...\n");
+		usb_stor_dbg(us, "Attempting to get CSW (2nd try)...\n");
 		result = usb_stor_bulk_transfer_buf(us, us->recv_bulk_pipe,
 				bcs, US_BULK_CS_WRAP_LEN, NULL);
 	}
 
 	/* if we still have a failure at this point, we're in trouble */
-	US_DEBUGP("Bulk status result = %d\n", result);
+	usb_stor_dbg(us, "Bulk status result = %d\n", result);
 	if (result != USB_STOR_XFER_GOOD)
 		return USB_STOR_TRANSPORT_ERROR;
 
 	/* check bulk status */
 	residue = le32_to_cpu(bcs->Residue);
-	US_DEBUGP("Bulk Status S 0x%x T 0x%x R %u Stat 0x%x\n",
-			le32_to_cpu(bcs->Signature), bcs->Tag, 
-			residue, bcs->Status);
+	usb_stor_dbg(us, "Bulk Status S 0x%x T 0x%x R %u Stat 0x%x\n",
+		     le32_to_cpu(bcs->Signature), bcs->Tag,
+		     residue, bcs->Status);
 	if (!(bcs->Tag == us->tag || (us->fflags & US_FL_BULK_IGNORE_TAG)) ||
 		bcs->Status > US_BULK_STAT_PHASE) {
-		US_DEBUGP("Bulk logical error\n");
+		usb_stor_dbg(us, "Bulk logical error\n");
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 
@@ -1173,12 +1171,12 @@ int usb_stor_Bulk_transport(struct scsi_cmnd *srb, struct us_data *us)
 	if (!us->bcs_signature) {
 		us->bcs_signature = bcs->Signature;
 		if (us->bcs_signature != cpu_to_le32(US_BULK_CS_SIGN))
-			US_DEBUGP("Learnt BCS signature 0x%08X\n",
-					le32_to_cpu(us->bcs_signature));
+			usb_stor_dbg(us, "Learnt BCS signature 0x%08X\n",
+				     le32_to_cpu(us->bcs_signature));
 	} else if (bcs->Signature != us->bcs_signature) {
-		US_DEBUGP("Signature mismatch: got %08X, expecting %08X\n",
-			  le32_to_cpu(bcs->Signature),
-			  le32_to_cpu(us->bcs_signature));
+		usb_stor_dbg(us, "Signature mismatch: got %08X, expecting %08X\n",
+			     le32_to_cpu(bcs->Signature),
+			     le32_to_cpu(us->bcs_signature));
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 
@@ -1255,7 +1253,7 @@ static int usb_stor_reset_common(struct us_data *us,
 	int result2;
 
 	if (test_bit(US_FLIDX_DISCONNECTING, &us->dflags)) {
-		US_DEBUGP("No reset during disconnect\n");
+		usb_stor_dbg(us, "No reset during disconnect\n");
 		return -EIO;
 	}
 
@@ -1263,7 +1261,7 @@ static int usb_stor_reset_common(struct us_data *us,
 			request, requesttype, value, index, data, size,
 			5*HZ);
 	if (result < 0) {
-		US_DEBUGP("Soft reset failed: %d\n", result);
+		usb_stor_dbg(us, "Soft reset failed: %d\n", result);
 		return result;
 	}
 
@@ -1273,23 +1271,23 @@ static int usb_stor_reset_common(struct us_data *us,
 			test_bit(US_FLIDX_DISCONNECTING, &us->dflags),
 			HZ*6);
 	if (test_bit(US_FLIDX_DISCONNECTING, &us->dflags)) {
-		US_DEBUGP("Reset interrupted by disconnect\n");
+		usb_stor_dbg(us, "Reset interrupted by disconnect\n");
 		return -EIO;
 	}
 
-	US_DEBUGP("Soft reset: clearing bulk-in endpoint halt\n");
+	usb_stor_dbg(us, "Soft reset: clearing bulk-in endpoint halt\n");
 	result = usb_stor_clear_halt(us, us->recv_bulk_pipe);
 
-	US_DEBUGP("Soft reset: clearing bulk-out endpoint halt\n");
+	usb_stor_dbg(us, "Soft reset: clearing bulk-out endpoint halt\n");
 	result2 = usb_stor_clear_halt(us, us->send_bulk_pipe);
 
 	/* return a result code based on the result of the clear-halts */
 	if (result >= 0)
 		result = result2;
 	if (result < 0)
-		US_DEBUGP("Soft reset failed\n");
+		usb_stor_dbg(us, "Soft reset failed\n");
 	else
-		US_DEBUGP("Soft reset done\n");
+		usb_stor_dbg(us, "Soft reset done\n");
 	return result;
 }
 
@@ -1299,8 +1297,6 @@ static int usb_stor_reset_common(struct us_data *us,
 
 int usb_stor_CB_reset(struct us_data *us)
 {
-	US_DEBUGP("%s called\n", __func__);
-
 	memset(us->iobuf, 0xFF, CB_RESET_CMD_SIZE);
 	us->iobuf[0] = SEND_DIAGNOSTIC;
 	us->iobuf[1] = 4;
@@ -1315,8 +1311,6 @@ EXPORT_SYMBOL_GPL(usb_stor_CB_reset);
  */
 int usb_stor_Bulk_reset(struct us_data *us)
 {
-	US_DEBUGP("%s called\n", __func__);
-
 	return usb_stor_reset_common(us, US_BULK_RESET_REQUEST, 
 				 USB_TYPE_CLASS | USB_RECIP_INTERFACE,
 				 0, us->ifnum, NULL, 0);
@@ -1336,16 +1330,17 @@ int usb_stor_port_reset(struct us_data *us)
 
 	result = usb_lock_device_for_reset(us->pusb_dev, us->pusb_intf);
 	if (result < 0)
-		US_DEBUGP("unable to lock device for reset: %d\n", result);
+		usb_stor_dbg(us, "unable to lock device for reset: %d\n",
+			     result);
 	else {
 		/* Were we disconnected while waiting for the lock? */
 		if (test_bit(US_FLIDX_DISCONNECTING, &us->dflags)) {
 			result = -EIO;
-			US_DEBUGP("No reset during disconnect\n");
+			usb_stor_dbg(us, "No reset during disconnect\n");
 		} else {
 			result = usb_reset_device(us->pusb_dev);
-			US_DEBUGP("usb_reset_device returns %d\n",
-					result);
+			usb_stor_dbg(us, "usb_reset_device returns %d\n",
+				     result);
 		}
 		usb_unlock_device(us->pusb_dev);
 	}
diff --git a/drivers/usb/storage/usb.c b/drivers/usb/storage/usb.c
index d6bee407af0..5c4fe0749af 100644
--- a/drivers/usb/storage/usb.c
+++ b/drivers/usb/storage/usb.c
@@ -183,7 +183,6 @@ int usb_stor_suspend(struct usb_interface *iface, pm_message_t message)
 	/* Wait until no command is running */
 	mutex_lock(&us->dev_mutex);
 
-	US_DEBUGP("%s\n", __func__);
 	if (us->suspend_resume_hook)
 		(us->suspend_resume_hook)(us, US_SUSPEND);
 
@@ -201,7 +200,6 @@ int usb_stor_resume(struct usb_interface *iface)
 
 	mutex_lock(&us->dev_mutex);
 
-	US_DEBUGP("%s\n", __func__);
 	if (us->suspend_resume_hook)
 		(us->suspend_resume_hook)(us, US_RESUME);
 
@@ -214,8 +212,6 @@ int usb_stor_reset_resume(struct usb_interface *iface)
 {
 	struct us_data *us = usb_get_intfdata(iface);
 
-	US_DEBUGP("%s\n", __func__);
-
 	/* Report the reset to the SCSI core */
 	usb_stor_report_bus_reset(us);
 
@@ -236,8 +232,6 @@ int usb_stor_pre_reset(struct usb_interface *iface)
 {
 	struct us_data *us = usb_get_intfdata(iface);
 
-	US_DEBUGP("%s\n", __func__);
-
 	/* Make sure no command runs during the reset */
 	mutex_lock(&us->dev_mutex);
 	return 0;
@@ -248,8 +242,6 @@ int usb_stor_post_reset(struct usb_interface *iface)
 {
 	struct us_data *us = usb_get_intfdata(iface);
 
-	US_DEBUGP("%s\n", __func__);
-
 	/* Report the reset to the SCSI core */
 	usb_stor_report_bus_reset(us);
 
@@ -311,11 +303,11 @@ static int usb_stor_control_thread(void * __us)
 	struct Scsi_Host *host = us_to_host(us);
 
 	for (;;) {
-		US_DEBUGP("*** thread sleeping.\n");
+		usb_stor_dbg(us, "*** thread sleeping\n");
 		if (wait_for_completion_interruptible(&us->cmnd_ready))
 			break;
 
-		US_DEBUGP("*** thread awakened.\n");
+		usb_stor_dbg(us, "*** thread awakened\n");
 
 		/* lock the device pointers */
 		mutex_lock(&(us->dev_mutex));
@@ -327,7 +319,7 @@ static int usb_stor_control_thread(void * __us)
 		if (us->srb == NULL) {
 			scsi_unlock(host);
 			mutex_unlock(&us->dev_mutex);
-			US_DEBUGP("-- exiting\n");
+			usb_stor_dbg(us, "-- exiting\n");
 			break;
 		}
 
@@ -343,7 +335,7 @@ static int usb_stor_control_thread(void * __us)
 		 * is UNKNOWN
 		 */
 		if (us->srb->sc_data_direction == DMA_BIDIRECTIONAL) {
-			US_DEBUGP("UNKNOWN data direction\n");
+			usb_stor_dbg(us, "UNKNOWN data direction\n");
 			us->srb->result = DID_ERROR << 16;
 		}
 
@@ -352,14 +344,14 @@ static int usb_stor_control_thread(void * __us)
 		 */
 		else if (us->srb->device->id &&
 				!(us->fflags & US_FL_SCM_MULT_TARG)) {
-			US_DEBUGP("Bad target number (%d:%d)\n",
-				  us->srb->device->id, us->srb->device->lun);
+			usb_stor_dbg(us, "Bad target number (%d:%d)\n",
+				     us->srb->device->id, us->srb->device->lun);
 			us->srb->result = DID_BAD_TARGET << 16;
 		}
 
 		else if (us->srb->device->lun > us->max_lun) {
-			US_DEBUGP("Bad LUN (%d:%d)\n",
-				  us->srb->device->id, us->srb->device->lun);
+			usb_stor_dbg(us, "Bad LUN (%d:%d)\n",
+				     us->srb->device->id, us->srb->device->lun);
 			us->srb->result = DID_BAD_TARGET << 16;
 		}
 
@@ -371,14 +363,14 @@ static int usb_stor_control_thread(void * __us)
 			    0x00, 0x80, 0x02, 0x02,
 			    0x1F, 0x00, 0x00, 0x00};
 
-			US_DEBUGP("Faking INQUIRY command\n");
+			usb_stor_dbg(us, "Faking INQUIRY command\n");
 			fill_inquiry_response(us, data_ptr, 36);
 			us->srb->result = SAM_STAT_GOOD;
 		}
 
 		/* we've got a command, let's do it! */
 		else {
-			US_DEBUG(usb_stor_show_command(us->srb));
+			US_DEBUG(usb_stor_show_command(us, us->srb));
 			us->proto_handler(us->srb, us);
 			usb_mark_last_busy(us->pusb_dev);
 		}
@@ -388,12 +380,12 @@ static int usb_stor_control_thread(void * __us)
 
 		/* indicate that the command is done */
 		if (us->srb->result != DID_ABORT << 16) {
-			US_DEBUGP("scsi cmd done, result=0x%x\n",
-				   us->srb->result);
+			usb_stor_dbg(us, "scsi cmd done, result=0x%x\n",
+				     us->srb->result);
 			us->srb->scsi_done(us->srb);
 		} else {
 SkipForAbort:
-			US_DEBUGP("scsi command aborted\n");
+			usb_stor_dbg(us, "scsi command aborted\n");
 		}
 
 		/* If an abort request was received we need to signal that
@@ -435,34 +427,30 @@ SkipForAbort:
 /* Associate our private data with the USB device */
 static int associate_dev(struct us_data *us, struct usb_interface *intf)
 {
-	US_DEBUGP("-- %s\n", __func__);
-
 	/* Fill in the device-related fields */
 	us->pusb_dev = interface_to_usbdev(intf);
 	us->pusb_intf = intf;
 	us->ifnum = intf->cur_altsetting->desc.bInterfaceNumber;
-	US_DEBUGP("Vendor: 0x%04x, Product: 0x%04x, Revision: 0x%04x\n",
-			le16_to_cpu(us->pusb_dev->descriptor.idVendor),
-			le16_to_cpu(us->pusb_dev->descriptor.idProduct),
-			le16_to_cpu(us->pusb_dev->descriptor.bcdDevice));
-	US_DEBUGP("Interface Subclass: 0x%02x, Protocol: 0x%02x\n",
-			intf->cur_altsetting->desc.bInterfaceSubClass,
-			intf->cur_altsetting->desc.bInterfaceProtocol);
+	usb_stor_dbg(us, "Vendor: 0x%04x, Product: 0x%04x, Revision: 0x%04x\n",
+		     le16_to_cpu(us->pusb_dev->descriptor.idVendor),
+		     le16_to_cpu(us->pusb_dev->descriptor.idProduct),
+		     le16_to_cpu(us->pusb_dev->descriptor.bcdDevice));
+	usb_stor_dbg(us, "Interface Subclass: 0x%02x, Protocol: 0x%02x\n",
+		     intf->cur_altsetting->desc.bInterfaceSubClass,
+		     intf->cur_altsetting->desc.bInterfaceProtocol);
 
 	/* Store our private data in the interface */
 	usb_set_intfdata(intf, us);
 
 	/* Allocate the control/setup and DMA-mapped buffers */
 	us->cr = kmalloc(sizeof(*us->cr), GFP_KERNEL);
-	if (!us->cr) {
-		US_DEBUGP("usb_ctrlrequest allocation failed\n");
+	if (!us->cr)
 		return -ENOMEM;
-	}
 
 	us->iobuf = usb_alloc_coherent(us->pusb_dev, US_IOBUF_SIZE,
 			GFP_KERNEL, &us->iobuf_dma);
 	if (!us->iobuf) {
-		US_DEBUGP("I/O buffer allocation failed\n");
+		usb_stor_dbg(us, "I/O buffer allocation failed\n");
 		return -ENOMEM;
 	}
 	return 0;
@@ -738,7 +726,7 @@ static int get_pipes(struct us_data *us)
 	}
 
 	if (!ep_in || !ep_out || (us->protocol == USB_PR_CBI && !ep_int)) {
-		US_DEBUGP("Endpoint sanity check failed! Rejecting dev.\n");
+		usb_stor_dbg(us, "Endpoint sanity check failed! Rejecting dev.\n");
 		return -EIO;
 	}
 
@@ -765,7 +753,7 @@ static int usb_stor_acquire_resources(struct us_data *us)
 
 	us->current_urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!us->current_urb) {
-		US_DEBUGP("URB allocation failed\n");
+		usb_stor_dbg(us, "URB allocation failed\n");
 		return -ENOMEM;
 	}
 
@@ -792,20 +780,18 @@ static int usb_stor_acquire_resources(struct us_data *us)
 /* Release all our dynamic resources */
 static void usb_stor_release_resources(struct us_data *us)
 {
-	US_DEBUGP("-- %s\n", __func__);
-
 	/* Tell the control thread to exit.  The SCSI host must
 	 * already have been removed and the DISCONNECTING flag set
 	 * so that we won't accept any more commands.
 	 */
-	US_DEBUGP("-- sending exit command to thread\n");
+	usb_stor_dbg(us, "-- sending exit command to thread\n");
 	complete(&us->cmnd_ready);
 	if (us->ctl_thread)
 		kthread_stop(us->ctl_thread);
 
 	/* Call the destructor routine, if it exists */
 	if (us->extra_destructor) {
-		US_DEBUGP("-- calling extra_destructor()\n");
+		usb_stor_dbg(us, "-- calling extra_destructor()\n");
 		us->extra_destructor(us->extra);
 	}
 
@@ -817,8 +803,6 @@ static void usb_stor_release_resources(struct us_data *us)
 /* Dissociate from the USB device */
 static void dissociate_dev(struct us_data *us)
 {
-	US_DEBUGP("-- %s\n", __func__);
-
 	/* Free the buffers */
 	kfree(us->cr);
 	usb_free_coherent(us->pusb_dev, US_IOBUF_SIZE, us->iobuf, us->iobuf_dma);
@@ -918,7 +902,7 @@ int usb_stor_probe1(struct us_data **pus,
 	struct us_data *us;
 	int result;
 
-	US_DEBUGP("USB Mass Storage device detected\n");
+	dev_info(&intf->dev, "USB Mass Storage device detected\n");
 
 	/*
 	 * Ask the SCSI layer to allocate a host structure, with extra
@@ -926,8 +910,7 @@ int usb_stor_probe1(struct us_data **pus,
 	 */
 	host = scsi_host_alloc(&usb_stor_host_template, sizeof(*us));
 	if (!host) {
-		dev_warn(&intf->dev,
-				"Unable to allocate the scsi host\n");
+		dev_warn(&intf->dev, "Unable to allocate the scsi host\n");
 		return -ENOMEM;
 	}
 
@@ -964,7 +947,7 @@ int usb_stor_probe1(struct us_data **pus,
 	return 0;
 
 BadDevice:
-	US_DEBUGP("storage_probe() failed\n");
+	usb_stor_dbg(us, "storage_probe() failed\n");
 	release_everything(us);
 	return result;
 }
@@ -981,8 +964,8 @@ int usb_stor_probe2(struct us_data *us)
 		result = -ENXIO;
 		goto BadDevice;
 	}
-	US_DEBUGP("Transport: %s\n", us->transport_name);
-	US_DEBUGP("Protocol: %s\n", us->protocol_name);
+	usb_stor_dbg(us, "Transport: %s\n", us->transport_name);
+	usb_stor_dbg(us, "Protocol: %s\n", us->protocol_name);
 
 	/* fix for single-lun devices */
 	if (us->fflags & US_FL_SINGLE_LUN)
@@ -1028,7 +1011,7 @@ int usb_stor_probe2(struct us_data *us)
 
 	/* We come here if there are any problems */
 BadDevice:
-	US_DEBUGP("storage_probe() failed\n");
+	usb_stor_dbg(us, "storage_probe() failed\n");
 	release_everything(us);
 	return result;
 }
@@ -1039,7 +1022,6 @@ void usb_stor_disconnect(struct usb_interface *intf)
 {
 	struct us_data *us = usb_get_intfdata(intf);
 
-	US_DEBUGP("storage_disconnect() called\n");
 	quiesce_and_remove_host(us);
 	release_everything(us);
 }
@@ -1075,8 +1057,7 @@ static int storage_probe(struct usb_interface *intf,
 	} else {
 		unusual_dev = &for_dynamic_ids;
 
-		US_DEBUGP("%s %s 0x%04x 0x%04x\n", "Use Bulk-Only transport",
-			"with the Transparent SCSI protocol for dynamic id:",
+		dev_dbg(&intf->dev, "Use Bulk-Only transport with the Transparent SCSI protocol for dynamic id: 0x%04x 0x%04x\n",
 			id->idVendor, id->idProduct);
 	}
 
@@ -1090,10 +1071,6 @@ static int storage_probe(struct usb_interface *intf,
 	return result;
 }
 
-/***********************************************************************
- * Initialization and registration
- ***********************************************************************/
-
 static struct usb_driver usb_storage_driver = {
 	.name =		"usb-storage",
 	.probe =	storage_probe,
@@ -1108,30 +1085,4 @@ static struct usb_driver usb_storage_driver = {
 	.soft_unbind =	1,
 };
 
-static int __init usb_stor_init(void)
-{
-	int retval;
-
-	pr_info("Initializing USB Mass Storage driver...\n");
-
-	/* register the driver, return usb_register return code if error */
-	retval = usb_register(&usb_storage_driver);
-	if (retval == 0)
-		pr_info("USB Mass Storage support registered.\n");
-	return retval;
-}
-
-static void __exit usb_stor_exit(void)
-{
-	US_DEBUGP("usb_stor_exit() called\n");
-
-	/* Deregister the driver
-	 * This will cause disconnect() to be called for each
-	 * attached unit
-	 */
-	US_DEBUGP("-- calling usb_deregister()\n");
-	usb_deregister(&usb_storage_driver) ;
-}
-
-module_init(usb_stor_init);
-module_exit(usb_stor_exit);
+module_usb_driver(usb_storage_driver);
diff --git a/drivers/usb/usb-common.c b/drivers/usb/usb-common.c
index d29503e954a..0db0a919d72 100644
--- a/drivers/usb/usb-common.c
+++ b/drivers/usb/usb-common.c
@@ -14,6 +14,32 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/usb/ch9.h>
+#include <linux/usb/otg.h>
+
+const char *usb_otg_state_string(enum usb_otg_state state)
+{
+	static const char *const names[] = {
+		[OTG_STATE_A_IDLE] = "a_idle",
+		[OTG_STATE_A_WAIT_VRISE] = "a_wait_vrise",
+		[OTG_STATE_A_WAIT_BCON] = "a_wait_bcon",
+		[OTG_STATE_A_HOST] = "a_host",
+		[OTG_STATE_A_SUSPEND] = "a_suspend",
+		[OTG_STATE_A_PERIPHERAL] = "a_peripheral",
+		[OTG_STATE_A_WAIT_VFALL] = "a_wait_vfall",
+		[OTG_STATE_A_VBUS_ERR] = "a_vbus_err",
+		[OTG_STATE_B_IDLE] = "b_idle",
+		[OTG_STATE_B_SRP_INIT] = "b_srp_init",
+		[OTG_STATE_B_PERIPHERAL] = "b_peripheral",
+		[OTG_STATE_B_WAIT_ACON] = "b_wait_acon",
+		[OTG_STATE_B_HOST] = "b_host",
+	};
+
+	if (state < 0 || state >= ARRAY_SIZE(names))
+		return "UNDEFINED";
+
+	return names[state];
+}
+EXPORT_SYMBOL_GPL(usb_otg_state_string);
 
 const char *usb_speed_string(enum usb_device_speed speed)
 {
@@ -32,4 +58,25 @@ const char *usb_speed_string(enum usb_device_speed speed)
 }
 EXPORT_SYMBOL_GPL(usb_speed_string);
 
+const char *usb_state_string(enum usb_device_state state)
+{
+	static const char *const names[] = {
+		[USB_STATE_NOTATTACHED] = "not attached",
+		[USB_STATE_ATTACHED] = "attached",
+		[USB_STATE_POWERED] = "powered",
+		[USB_STATE_RECONNECTING] = "reconnecting",
+		[USB_STATE_UNAUTHENTICATED] = "unauthenticated",
+		[USB_STATE_DEFAULT] = "default",
+		[USB_STATE_ADDRESS] = "addresssed",
+		[USB_STATE_CONFIGURED] = "configured",
+		[USB_STATE_SUSPENDED] = "suspended",
+	};
+
+	if (state < 0 || state >= ARRAY_SIZE(names))
+		return "UNKNOWN";
+
+	return names[state];
+}
+EXPORT_SYMBOL_GPL(usb_state_string);
+
 MODULE_LICENSE("GPL");
diff --git a/drivers/usb/usb-skeleton.c b/drivers/usb/usb-skeleton.c
index ce310170829..7ed3b039dbe 100644
--- a/drivers/usb/usb-skeleton.c
+++ b/drivers/usb/usb-skeleton.c
@@ -61,11 +61,10 @@ struct usb_skel {
 	__u8			bulk_out_endpointAddr;	/* the address of the bulk out endpoint */
 	int			errors;			/* the last request tanked */
 	bool			ongoing_read;		/* a read is going on */
-	bool			processed_urb;		/* indicates we haven't processed the urb */
 	spinlock_t		err_lock;		/* lock for errors */
 	struct kref		kref;
 	struct mutex		io_mutex;		/* synchronize I/O with disconnect */
-	struct completion	bulk_in_completion;	/* to wait for an ongoing read */
+	wait_queue_head_t	bulk_in_wait;		/* to wait for an ongoing read */
 };
 #define to_skel_dev(d) container_of(d, struct usb_skel, kref)
 
@@ -185,7 +184,7 @@ static void skel_read_bulk_callback(struct urb *urb)
 	dev->ongoing_read = 0;
 	spin_unlock(&dev->err_lock);
 
-	complete(&dev->bulk_in_completion);
+	wake_up_interruptible(&dev->bulk_in_wait);
 }
 
 static int skel_do_read_io(struct usb_skel *dev, size_t count)
@@ -206,13 +205,16 @@ static int skel_do_read_io(struct usb_skel *dev, size_t count)
 	dev->ongoing_read = 1;
 	spin_unlock_irq(&dev->err_lock);
 
+	/* submit bulk in urb, which means no data to deliver */
+	dev->bulk_in_filled = 0;
+	dev->bulk_in_copied = 0;
+
 	/* do it */
 	rv = usb_submit_urb(dev->bulk_in_urb, GFP_KERNEL);
 	if (rv < 0) {
 		dev_err(&dev->interface->dev,
 			"%s - failed submitting read urb, error %d\n",
 			__func__, rv);
-		dev->bulk_in_filled = 0;
 		rv = (rv == -ENOMEM) ? rv : -EIO;
 		spin_lock_irq(&dev->err_lock);
 		dev->ongoing_read = 0;
@@ -261,25 +263,9 @@ retry:
 		 * IO may take forever
 		 * hence wait in an interruptible state
 		 */
-		rv = wait_for_completion_interruptible(&dev->bulk_in_completion);
+		rv = wait_event_interruptible(dev->bulk_in_wait, (!dev->ongoing_read));
 		if (rv < 0)
 			goto exit;
-		/*
-		 * by waiting we also semiprocessed the urb
-		 * we must finish now
-		 */
-		dev->bulk_in_copied = 0;
-		dev->processed_urb = 1;
-	}
-
-	if (!dev->processed_urb) {
-		/*
-		 * the URB hasn't been processed
-		 * do it now
-		 */
-		wait_for_completion(&dev->bulk_in_completion);
-		dev->bulk_in_copied = 0;
-		dev->processed_urb = 1;
 	}
 
 	/* errors must be reported */
@@ -289,8 +275,6 @@ retry:
 		dev->errors = 0;
 		/* to preserve notifications about reset */
 		rv = (rv == -EPIPE) ? rv : -EIO;
-		/* no data to deliver */
-		dev->bulk_in_filled = 0;
 		/* report it */
 		goto exit;
 	}
@@ -526,7 +510,7 @@ static int skel_probe(struct usb_interface *interface,
 	mutex_init(&dev->io_mutex);
 	spin_lock_init(&dev->err_lock);
 	init_usb_anchor(&dev->submitted);
-	init_completion(&dev->bulk_in_completion);
+	init_waitqueue_head(&dev->bulk_in_wait);
 
 	dev->udev = usb_get_dev(interface_to_usbdev(interface));
 	dev->interface = interface;
diff --git a/drivers/usb/wusbcore/Kconfig b/drivers/usb/wusbcore/Kconfig
index 8bf19760d44..0e17b966e1b 100644
--- a/drivers/usb/wusbcore/Kconfig
+++ b/drivers/usb/wusbcore/Kconfig
@@ -3,7 +3,6 @@
 #
 config USB_WUSB
 	tristate "Enable Wireless USB extensions"
-	depends on USB
 	depends on PCI
 	depends on UWB
         select CRYPTO
@@ -19,7 +18,6 @@ config USB_WUSB
 
 config USB_WUSB_CBAF
 	tristate "Support WUSB Cable Based Association (CBA)"
-	depends on USB
 	help
 	  Some WUSB devices support Cable Based Association. It's used to
 	  enable the secure communication between the host and the
diff --git a/drivers/uwb/rsv.c b/drivers/uwb/rsv.c
index 0b0d8bce842..f4ae05f78c4 100644
--- a/drivers/uwb/rsv.c
+++ b/drivers/uwb/rsv.c
@@ -231,7 +231,7 @@ void uwb_rsv_backoff_win_increment(struct uwb_rc *rc)
 		return;
 
 	bow->window <<= 1;
-	bow->n = random32() & (bow->window - 1);
+	bow->n = prandom_u32() & (bow->window - 1);
 	dev_dbg(dev, "new_window=%d, n=%d\n: ", bow->window, bow->n);
 
 	/* reset the timer associated variables */
@@ -557,7 +557,7 @@ int uwb_rsv_establish(struct uwb_rsv *rsv)
 	if (ret)
 		goto out;
 
-	rsv->tiebreaker = random32() & 1;
+	rsv->tiebreaker = prandom_u32() & 1;
 	/* get available mas bitmap */
 	uwb_drp_available(rc, &available);
 
diff --git a/drivers/vfio/pci/vfio_pci.c b/drivers/vfio/pci/vfio_pci.c
index 8189cb6a86a..09d2e3ffd6f 100644
--- a/drivers/vfio/pci/vfio_pci.c
+++ b/drivers/vfio/pci/vfio_pci.c
@@ -70,7 +70,7 @@ static int vfio_pci_enable(struct vfio_pci_device *vdev)
 		pci_write_config_word(pdev, PCI_COMMAND, cmd);
 	}
 
-	msix_pos = pci_find_capability(pdev, PCI_CAP_ID_MSIX);
+	msix_pos = pdev->msix_cap;
 	if (msix_pos) {
 		u16 flags;
 		u32 table;
@@ -78,8 +78,8 @@ static int vfio_pci_enable(struct vfio_pci_device *vdev)
 		pci_read_config_word(pdev, msix_pos + PCI_MSIX_FLAGS, &flags);
 		pci_read_config_dword(pdev, msix_pos + PCI_MSIX_TABLE, &table);
 
-		vdev->msix_bar = table & PCI_MSIX_FLAGS_BIRMASK;
-		vdev->msix_offset = table & ~PCI_MSIX_FLAGS_BIRMASK;
+		vdev->msix_bar = table & PCI_MSIX_TABLE_BIR;
+		vdev->msix_offset = table & PCI_MSIX_TABLE_OFFSET;
 		vdev->msix_size = ((flags & PCI_MSIX_FLAGS_QSIZE) + 1) * 16;
 	} else
 		vdev->msix_bar = 0xFF;
@@ -183,7 +183,7 @@ static int vfio_pci_get_irq_count(struct vfio_pci_device *vdev, int irq_type)
 		u8 pos;
 		u16 flags;
 
-		pos = pci_find_capability(vdev->pdev, PCI_CAP_ID_MSI);
+		pos = vdev->pdev->msi_cap;
 		if (pos) {
 			pci_read_config_word(vdev->pdev,
 					     pos + PCI_MSI_FLAGS, &flags);
@@ -194,7 +194,7 @@ static int vfio_pci_get_irq_count(struct vfio_pci_device *vdev, int irq_type)
 		u8 pos;
 		u16 flags;
 
-		pos = pci_find_capability(vdev->pdev, PCI_CAP_ID_MSIX);
+		pos = vdev->pdev->msix_cap;
 		if (pos) {
 			pci_read_config_word(vdev->pdev,
 					     pos + PCI_MSIX_FLAGS, &flags);
@@ -346,6 +346,7 @@ static long vfio_pci_ioctl(void *device_data,
 
 		if (!(hdr.flags & VFIO_IRQ_SET_DATA_NONE)) {
 			size_t size;
+			int max = vfio_pci_get_irq_count(vdev, hdr.index);
 
 			if (hdr.flags & VFIO_IRQ_SET_DATA_BOOL)
 				size = sizeof(uint8_t);
@@ -355,7 +356,7 @@ static long vfio_pci_ioctl(void *device_data,
 				return -EINVAL;
 
 			if (hdr.argsz - minsz < hdr.count * size ||
-			    hdr.count > vfio_pci_get_irq_count(vdev, hdr.index))
+			    hdr.start >= max || hdr.start + hdr.count > max)
 				return -EINVAL;
 
 			data = memdup_user((void __user *)(arg + minsz),
diff --git a/drivers/vhost/tcm_vhost.c b/drivers/vhost/tcm_vhost.c
index 43fb11ee2e8..1677238d281 100644
--- a/drivers/vhost/tcm_vhost.c
+++ b/drivers/vhost/tcm_vhost.c
@@ -60,20 +60,36 @@ enum {
 	VHOST_SCSI_VQ_IO = 2,
 };
 
+/*
+ * VIRTIO_RING_F_EVENT_IDX seems broken. Not sure the bug is in
+ * kernel but disabling it helps.
+ * TODO: debug and remove the workaround.
+ */
+enum {
+	VHOST_SCSI_FEATURES = (VHOST_FEATURES & (~VIRTIO_RING_F_EVENT_IDX)) |
+			      (1ULL << VIRTIO_SCSI_F_HOTPLUG)
+};
+
 #define VHOST_SCSI_MAX_TARGET	256
 #define VHOST_SCSI_MAX_VQ	128
+#define VHOST_SCSI_MAX_EVENT	128
 
 struct vhost_scsi {
 	/* Protected by vhost_scsi->dev.mutex */
-	struct tcm_vhost_tpg *vs_tpg[VHOST_SCSI_MAX_TARGET];
+	struct tcm_vhost_tpg **vs_tpg;
 	char vs_vhost_wwpn[TRANSPORT_IQN_LEN];
-	bool vs_endpoint;
 
 	struct vhost_dev dev;
 	struct vhost_virtqueue vqs[VHOST_SCSI_MAX_VQ];
 
 	struct vhost_work vs_completion_work; /* cmd completion work item */
 	struct llist_head vs_completion_list; /* cmd completion queue */
+
+	struct vhost_work vs_event_work; /* evt injection work item */
+	struct llist_head vs_event_list; /* evt injection queue */
+
+	bool vs_events_missed; /* any missed events, protected by vq->mutex */
+	int vs_events_nr; /* num of pending events, protected by vq->mutex */
 };
 
 /* Local pointer to allocated TCM configfs fabric module */
@@ -341,6 +357,37 @@ static int tcm_vhost_queue_tm_rsp(struct se_cmd *se_cmd)
 	return 0;
 }
 
+static void tcm_vhost_free_evt(struct vhost_scsi *vs, struct tcm_vhost_evt *evt)
+{
+	vs->vs_events_nr--;
+	kfree(evt);
+}
+
+static struct tcm_vhost_evt *tcm_vhost_allocate_evt(struct vhost_scsi *vs,
+	u32 event, u32 reason)
+{
+	struct vhost_virtqueue *vq = &vs->vqs[VHOST_SCSI_VQ_EVT];
+	struct tcm_vhost_evt *evt;
+
+	if (vs->vs_events_nr > VHOST_SCSI_MAX_EVENT) {
+		vs->vs_events_missed = true;
+		return NULL;
+	}
+
+	evt = kzalloc(sizeof(*evt), GFP_KERNEL);
+	if (!evt) {
+		vq_err(vq, "Failed to allocate tcm_vhost_evt\n");
+		vs->vs_events_missed = true;
+		return NULL;
+	}
+
+	evt->event.event = event;
+	evt->event.reason = reason;
+	vs->vs_events_nr++;
+
+	return evt;
+}
+
 static void vhost_scsi_free_cmd(struct tcm_vhost_cmd *tv_cmd)
 {
 	struct se_cmd *se_cmd = &tv_cmd->tvc_se_cmd;
@@ -359,6 +406,75 @@ static void vhost_scsi_free_cmd(struct tcm_vhost_cmd *tv_cmd)
 	kfree(tv_cmd);
 }
 
+static void tcm_vhost_do_evt_work(struct vhost_scsi *vs,
+	struct tcm_vhost_evt *evt)
+{
+	struct vhost_virtqueue *vq = &vs->vqs[VHOST_SCSI_VQ_EVT];
+	struct virtio_scsi_event *event = &evt->event;
+	struct virtio_scsi_event __user *eventp;
+	unsigned out, in;
+	int head, ret;
+
+	if (!vq->private_data) {
+		vs->vs_events_missed = true;
+		return;
+	}
+
+again:
+	vhost_disable_notify(&vs->dev, vq);
+	head = vhost_get_vq_desc(&vs->dev, vq, vq->iov,
+			ARRAY_SIZE(vq->iov), &out, &in,
+			NULL, NULL);
+	if (head < 0) {
+		vs->vs_events_missed = true;
+		return;
+	}
+	if (head == vq->num) {
+		if (vhost_enable_notify(&vs->dev, vq))
+			goto again;
+		vs->vs_events_missed = true;
+		return;
+	}
+
+	if ((vq->iov[out].iov_len != sizeof(struct virtio_scsi_event))) {
+		vq_err(vq, "Expecting virtio_scsi_event, got %zu bytes\n",
+				vq->iov[out].iov_len);
+		vs->vs_events_missed = true;
+		return;
+	}
+
+	if (vs->vs_events_missed) {
+		event->event |= VIRTIO_SCSI_T_EVENTS_MISSED;
+		vs->vs_events_missed = false;
+	}
+
+	eventp = vq->iov[out].iov_base;
+	ret = __copy_to_user(eventp, event, sizeof(*event));
+	if (!ret)
+		vhost_add_used_and_signal(&vs->dev, vq, head, 0);
+	else
+		vq_err(vq, "Faulted on tcm_vhost_send_event\n");
+}
+
+static void tcm_vhost_evt_work(struct vhost_work *work)
+{
+	struct vhost_scsi *vs = container_of(work, struct vhost_scsi,
+					vs_event_work);
+	struct vhost_virtqueue *vq = &vs->vqs[VHOST_SCSI_VQ_EVT];
+	struct tcm_vhost_evt *evt;
+	struct llist_node *llnode;
+
+	mutex_lock(&vq->mutex);
+	llnode = llist_del_all(&vs->vs_event_list);
+	while (llnode) {
+		evt = llist_entry(llnode, struct tcm_vhost_evt, list);
+		llnode = llist_next(llnode);
+		tcm_vhost_do_evt_work(vs, evt);
+		tcm_vhost_free_evt(vs, evt);
+	}
+	mutex_unlock(&vq->mutex);
+}
+
 /* Fill in status and signal that we are done processing this command
  *
  * This is scheduled in the vhost work queue so we are called with the owner
@@ -570,9 +686,27 @@ static void tcm_vhost_submission_work(struct work_struct *work)
 	}
 }
 
+static void vhost_scsi_send_bad_target(struct vhost_scsi *vs,
+	struct vhost_virtqueue *vq, int head, unsigned out)
+{
+	struct virtio_scsi_cmd_resp __user *resp;
+	struct virtio_scsi_cmd_resp rsp;
+	int ret;
+
+	memset(&rsp, 0, sizeof(rsp));
+	rsp.response = VIRTIO_SCSI_S_BAD_TARGET;
+	resp = vq->iov[out].iov_base;
+	ret = __copy_to_user(resp, &rsp, sizeof(rsp));
+	if (!ret)
+		vhost_add_used_and_signal(&vs->dev, vq, head, 0);
+	else
+		pr_err("Faulted on virtio_scsi_cmd_resp\n");
+}
+
 static void vhost_scsi_handle_vq(struct vhost_scsi *vs,
 	struct vhost_virtqueue *vq)
 {
+	struct tcm_vhost_tpg **vs_tpg;
 	struct virtio_scsi_cmd_req v_req;
 	struct tcm_vhost_tpg *tv_tpg;
 	struct tcm_vhost_cmd *tv_cmd;
@@ -581,8 +715,16 @@ static void vhost_scsi_handle_vq(struct vhost_scsi *vs,
 	int head, ret;
 	u8 target;
 
-	/* Must use ioctl VHOST_SCSI_SET_ENDPOINT */
-	if (unlikely(!vs->vs_endpoint))
+	/*
+	 * We can handle the vq only after the endpoint is setup by calling the
+	 * VHOST_SCSI_SET_ENDPOINT ioctl.
+	 *
+	 * TODO: Check that we are running from vhost_worker which acts
+	 * as read-side critical section for vhost kind of RCU.
+	 * See the comments in struct vhost_virtqueue in drivers/vhost/vhost.h
+	 */
+	vs_tpg = rcu_dereference_check(vq->private_data, 1);
+	if (!vs_tpg)
 		return;
 
 	mutex_lock(&vq->mutex);
@@ -652,23 +794,11 @@ static void vhost_scsi_handle_vq(struct vhost_scsi *vs,
 
 		/* Extract the tpgt */
 		target = v_req.lun[1];
-		tv_tpg = vs->vs_tpg[target];
+		tv_tpg = ACCESS_ONCE(vs_tpg[target]);
 
 		/* Target does not exist, fail the request */
 		if (unlikely(!tv_tpg)) {
-			struct virtio_scsi_cmd_resp __user *resp;
-			struct virtio_scsi_cmd_resp rsp;
-
-			memset(&rsp, 0, sizeof(rsp));
-			rsp.response = VIRTIO_SCSI_S_BAD_TARGET;
-			resp = vq->iov[out].iov_base;
-			ret = __copy_to_user(resp, &rsp, sizeof(rsp));
-			if (!ret)
-				vhost_add_used_and_signal(&vs->dev,
-							  vq, head, 0);
-			else
-				pr_err("Faulted on virtio_scsi_cmd_resp\n");
-
+			vhost_scsi_send_bad_target(vs, vq, head, out);
 			continue;
 		}
 
@@ -681,22 +811,13 @@ static void vhost_scsi_handle_vq(struct vhost_scsi *vs,
 		if (IS_ERR(tv_cmd)) {
 			vq_err(vq, "vhost_scsi_allocate_cmd failed %ld\n",
 					PTR_ERR(tv_cmd));
-			break;
+			goto err_cmd;
 		}
 		pr_debug("Allocated tv_cmd: %p exp_data_len: %d, data_direction"
 			": %d\n", tv_cmd, exp_data_len, data_direction);
 
 		tv_cmd->tvc_vhost = vs;
 		tv_cmd->tvc_vq = vq;
-
-		if (unlikely(vq->iov[out].iov_len !=
-				sizeof(struct virtio_scsi_cmd_resp))) {
-			vq_err(vq, "Expecting virtio_scsi_cmd_resp, got %zu"
-				" bytes, out: %d, in: %d\n",
-				vq->iov[out].iov_len, out, in);
-			break;
-		}
-
 		tv_cmd->tvc_resp = vq->iov[out].iov_base;
 
 		/*
@@ -716,7 +837,7 @@ static void vhost_scsi_handle_vq(struct vhost_scsi *vs,
 				" exceeds SCSI_MAX_VARLEN_CDB_SIZE: %d\n",
 				scsi_command_size(tv_cmd->tvc_cdb),
 				TCM_VHOST_MAX_CDB_SIZE);
-			break; /* TODO */
+			goto err_free;
 		}
 		tv_cmd->tvc_lun = ((v_req.lun[2] << 8) | v_req.lun[3]) & 0x3FFF;
 
@@ -729,7 +850,7 @@ static void vhost_scsi_handle_vq(struct vhost_scsi *vs,
 					data_direction == DMA_TO_DEVICE);
 			if (unlikely(ret)) {
 				vq_err(vq, "Failed to map iov to sgl\n");
-				break; /* TODO */
+				goto err_free;
 			}
 		}
 
@@ -750,6 +871,13 @@ static void vhost_scsi_handle_vq(struct vhost_scsi *vs,
 	}
 
 	mutex_unlock(&vq->mutex);
+	return;
+
+err_free:
+	vhost_scsi_free_cmd(tv_cmd);
+err_cmd:
+	vhost_scsi_send_bad_target(vs, vq, head, out);
+	mutex_unlock(&vq->mutex);
 }
 
 static void vhost_scsi_ctl_handle_kick(struct vhost_work *work)
@@ -757,9 +885,46 @@ static void vhost_scsi_ctl_handle_kick(struct vhost_work *work)
 	pr_debug("%s: The handling func for control queue.\n", __func__);
 }
 
+static void tcm_vhost_send_evt(struct vhost_scsi *vs, struct tcm_vhost_tpg *tpg,
+	struct se_lun *lun, u32 event, u32 reason)
+{
+	struct tcm_vhost_evt *evt;
+
+	evt = tcm_vhost_allocate_evt(vs, event, reason);
+	if (!evt)
+		return;
+
+	if (tpg && lun) {
+		/* TODO: share lun setup code with virtio-scsi.ko */
+		/*
+		 * Note: evt->event is zeroed when we allocate it and
+		 * lun[4-7] need to be zero according to virtio-scsi spec.
+		 */
+		evt->event.lun[0] = 0x01;
+		evt->event.lun[1] = tpg->tport_tpgt & 0xFF;
+		if (lun->unpacked_lun >= 256)
+			evt->event.lun[2] = lun->unpacked_lun >> 8 | 0x40 ;
+		evt->event.lun[3] = lun->unpacked_lun & 0xFF;
+	}
+
+	llist_add(&evt->list, &vs->vs_event_list);
+	vhost_work_queue(&vs->dev, &vs->vs_event_work);
+}
+
 static void vhost_scsi_evt_handle_kick(struct vhost_work *work)
 {
-	pr_debug("%s: The handling func for event queue.\n", __func__);
+	struct vhost_virtqueue *vq = container_of(work, struct vhost_virtqueue,
+						poll.work);
+	struct vhost_scsi *vs = container_of(vq->dev, struct vhost_scsi, dev);
+
+	mutex_lock(&vq->mutex);
+	if (!vq->private_data)
+		goto out;
+
+	if (vs->vs_events_missed)
+		tcm_vhost_send_evt(vs, NULL, NULL, VIRTIO_SCSI_T_NO_EVENT, 0);
+out:
+	mutex_unlock(&vq->mutex);
 }
 
 static void vhost_scsi_handle_kick(struct vhost_work *work)
@@ -771,9 +936,27 @@ static void vhost_scsi_handle_kick(struct vhost_work *work)
 	vhost_scsi_handle_vq(vs, vq);
 }
 
+static void vhost_scsi_flush_vq(struct vhost_scsi *vs, int index)
+{
+	vhost_poll_flush(&vs->dev.vqs[index].poll);
+}
+
+static void vhost_scsi_flush(struct vhost_scsi *vs)
+{
+	int i;
+
+	for (i = 0; i < VHOST_SCSI_MAX_VQ; i++)
+		vhost_scsi_flush_vq(vs, i);
+	vhost_work_flush(&vs->dev, &vs->vs_completion_work);
+	vhost_work_flush(&vs->dev, &vs->vs_event_work);
+}
+
 /*
  * Called from vhost_scsi_ioctl() context to walk the list of available
  * tcm_vhost_tpg with an active struct tcm_vhost_nexus
+ *
+ *  The lock nesting rule is:
+ *    tcm_vhost_mutex -> vs->dev.mutex -> tpg->tv_tpg_mutex -> vq->mutex
  */
 static int vhost_scsi_set_endpoint(
 	struct vhost_scsi *vs,
@@ -781,20 +964,32 @@ static int vhost_scsi_set_endpoint(
 {
 	struct tcm_vhost_tport *tv_tport;
 	struct tcm_vhost_tpg *tv_tpg;
+	struct tcm_vhost_tpg **vs_tpg;
+	struct vhost_virtqueue *vq;
+	int index, ret, i, len;
 	bool match = false;
-	int index, ret;
 
+	mutex_lock(&tcm_vhost_mutex);
 	mutex_lock(&vs->dev.mutex);
+
 	/* Verify that ring has been setup correctly. */
 	for (index = 0; index < vs->dev.nvqs; ++index) {
 		/* Verify that ring has been setup correctly. */
 		if (!vhost_vq_access_ok(&vs->vqs[index])) {
-			mutex_unlock(&vs->dev.mutex);
-			return -EFAULT;
+			ret = -EFAULT;
+			goto out;
 		}
 	}
 
-	mutex_lock(&tcm_vhost_mutex);
+	len = sizeof(vs_tpg[0]) * VHOST_SCSI_MAX_TARGET;
+	vs_tpg = kzalloc(len, GFP_KERNEL);
+	if (!vs_tpg) {
+		ret = -ENOMEM;
+		goto out;
+	}
+	if (vs->vs_tpg)
+		memcpy(vs_tpg, vs->vs_tpg, len);
+
 	list_for_each_entry(tv_tpg, &tcm_vhost_list, tv_tpg_list) {
 		mutex_lock(&tv_tpg->tv_tpg_mutex);
 		if (!tv_tpg->tpg_nexus) {
@@ -808,31 +1003,48 @@ static int vhost_scsi_set_endpoint(
 		tv_tport = tv_tpg->tport;
 
 		if (!strcmp(tv_tport->tport_name, t->vhost_wwpn)) {
-			if (vs->vs_tpg[tv_tpg->tport_tpgt]) {
+			if (vs->vs_tpg && vs->vs_tpg[tv_tpg->tport_tpgt]) {
+				kfree(vs_tpg);
 				mutex_unlock(&tv_tpg->tv_tpg_mutex);
-				mutex_unlock(&tcm_vhost_mutex);
-				mutex_unlock(&vs->dev.mutex);
-				return -EEXIST;
+				ret = -EEXIST;
+				goto out;
 			}
 			tv_tpg->tv_tpg_vhost_count++;
-			vs->vs_tpg[tv_tpg->tport_tpgt] = tv_tpg;
+			tv_tpg->vhost_scsi = vs;
+			vs_tpg[tv_tpg->tport_tpgt] = tv_tpg;
 			smp_mb__after_atomic_inc();
 			match = true;
 		}
 		mutex_unlock(&tv_tpg->tv_tpg_mutex);
 	}
-	mutex_unlock(&tcm_vhost_mutex);
 
 	if (match) {
 		memcpy(vs->vs_vhost_wwpn, t->vhost_wwpn,
 		       sizeof(vs->vs_vhost_wwpn));
-		vs->vs_endpoint = true;
+		for (i = 0; i < VHOST_SCSI_MAX_VQ; i++) {
+			vq = &vs->vqs[i];
+			/* Flushing the vhost_work acts as synchronize_rcu */
+			mutex_lock(&vq->mutex);
+			rcu_assign_pointer(vq->private_data, vs_tpg);
+			vhost_init_used(vq);
+			mutex_unlock(&vq->mutex);
+		}
 		ret = 0;
 	} else {
 		ret = -EEXIST;
 	}
 
+	/*
+	 * Act as synchronize_rcu to make sure access to
+	 * old vs->vs_tpg is finished.
+	 */
+	vhost_scsi_flush(vs);
+	kfree(vs->vs_tpg);
+	vs->vs_tpg = vs_tpg;
+
+out:
 	mutex_unlock(&vs->dev.mutex);
+	mutex_unlock(&tcm_vhost_mutex);
 	return ret;
 }
 
@@ -842,9 +1054,12 @@ static int vhost_scsi_clear_endpoint(
 {
 	struct tcm_vhost_tport *tv_tport;
 	struct tcm_vhost_tpg *tv_tpg;
+	struct vhost_virtqueue *vq;
+	bool match = false;
 	int index, ret, i;
 	u8 target;
 
+	mutex_lock(&tcm_vhost_mutex);
 	mutex_lock(&vs->dev.mutex);
 	/* Verify that ring has been setup correctly. */
 	for (index = 0; index < vs->dev.nvqs; ++index) {
@@ -853,9 +1068,14 @@ static int vhost_scsi_clear_endpoint(
 			goto err_dev;
 		}
 	}
+
+	if (!vs->vs_tpg) {
+		ret = 0;
+		goto err_dev;
+	}
+
 	for (i = 0; i < VHOST_SCSI_MAX_TARGET; i++) {
 		target = i;
-
 		tv_tpg = vs->vs_tpg[target];
 		if (!tv_tpg)
 			continue;
@@ -876,20 +1096,58 @@ static int vhost_scsi_clear_endpoint(
 			goto err_tpg;
 		}
 		tv_tpg->tv_tpg_vhost_count--;
+		tv_tpg->vhost_scsi = NULL;
 		vs->vs_tpg[target] = NULL;
-		vs->vs_endpoint = false;
+		match = true;
 		mutex_unlock(&tv_tpg->tv_tpg_mutex);
 	}
+	if (match) {
+		for (i = 0; i < VHOST_SCSI_MAX_VQ; i++) {
+			vq = &vs->vqs[i];
+			/* Flushing the vhost_work acts as synchronize_rcu */
+			mutex_lock(&vq->mutex);
+			rcu_assign_pointer(vq->private_data, NULL);
+			mutex_unlock(&vq->mutex);
+		}
+	}
+	/*
+	 * Act as synchronize_rcu to make sure access to
+	 * old vs->vs_tpg is finished.
+	 */
+	vhost_scsi_flush(vs);
+	kfree(vs->vs_tpg);
+	vs->vs_tpg = NULL;
+	WARN_ON(vs->vs_events_nr);
 	mutex_unlock(&vs->dev.mutex);
+	mutex_unlock(&tcm_vhost_mutex);
 	return 0;
 
 err_tpg:
 	mutex_unlock(&tv_tpg->tv_tpg_mutex);
 err_dev:
 	mutex_unlock(&vs->dev.mutex);
+	mutex_unlock(&tcm_vhost_mutex);
 	return ret;
 }
 
+static int vhost_scsi_set_features(struct vhost_scsi *vs, u64 features)
+{
+	if (features & ~VHOST_SCSI_FEATURES)
+		return -EOPNOTSUPP;
+
+	mutex_lock(&vs->dev.mutex);
+	if ((features & (1 << VHOST_F_LOG_ALL)) &&
+	    !vhost_log_access_ok(&vs->dev)) {
+		mutex_unlock(&vs->dev.mutex);
+		return -EFAULT;
+	}
+	vs->dev.acked_features = features;
+	smp_wmb();
+	vhost_scsi_flush(vs);
+	mutex_unlock(&vs->dev.mutex);
+	return 0;
+}
+
 static int vhost_scsi_open(struct inode *inode, struct file *f)
 {
 	struct vhost_scsi *s;
@@ -900,6 +1158,10 @@ static int vhost_scsi_open(struct inode *inode, struct file *f)
 		return -ENOMEM;
 
 	vhost_work_init(&s->vs_completion_work, vhost_scsi_complete_cmd_work);
+	vhost_work_init(&s->vs_event_work, tcm_vhost_evt_work);
+
+	s->vs_events_nr = 0;
+	s->vs_events_missed = false;
 
 	s->vqs[VHOST_SCSI_VQ_CTL].handle_kick = vhost_scsi_ctl_handle_kick;
 	s->vqs[VHOST_SCSI_VQ_EVT].handle_kick = vhost_scsi_evt_handle_kick;
@@ -926,42 +1188,12 @@ static int vhost_scsi_release(struct inode *inode, struct file *f)
 	vhost_scsi_clear_endpoint(s, &t);
 	vhost_dev_stop(&s->dev);
 	vhost_dev_cleanup(&s->dev, false);
+	/* Jobs can re-queue themselves in evt kick handler. Do extra flush. */
+	vhost_scsi_flush(s);
 	kfree(s);
 	return 0;
 }
 
-static void vhost_scsi_flush_vq(struct vhost_scsi *vs, int index)
-{
-	vhost_poll_flush(&vs->dev.vqs[index].poll);
-}
-
-static void vhost_scsi_flush(struct vhost_scsi *vs)
-{
-	int i;
-
-	for (i = 0; i < VHOST_SCSI_MAX_VQ; i++)
-		vhost_scsi_flush_vq(vs, i);
-	vhost_work_flush(&vs->dev, &vs->vs_completion_work);
-}
-
-static int vhost_scsi_set_features(struct vhost_scsi *vs, u64 features)
-{
-	if (features & ~VHOST_FEATURES)
-		return -EOPNOTSUPP;
-
-	mutex_lock(&vs->dev.mutex);
-	if ((features & (1 << VHOST_F_LOG_ALL)) &&
-	    !vhost_log_access_ok(&vs->dev)) {
-		mutex_unlock(&vs->dev.mutex);
-		return -EFAULT;
-	}
-	vs->dev.acked_features = features;
-	smp_wmb();
-	vhost_scsi_flush(vs);
-	mutex_unlock(&vs->dev.mutex);
-	return 0;
-}
-
 static long vhost_scsi_ioctl(struct file *f, unsigned int ioctl,
 				unsigned long arg)
 {
@@ -969,8 +1201,11 @@ static long vhost_scsi_ioctl(struct file *f, unsigned int ioctl,
 	struct vhost_scsi_target backend;
 	void __user *argp = (void __user *)arg;
 	u64 __user *featurep = argp;
+	u32 __user *eventsp = argp;
+	u32 events_missed;
 	u64 features;
 	int r, abi_version = VHOST_SCSI_ABI_VERSION;
+	struct vhost_virtqueue *vq = &vs->vqs[VHOST_SCSI_VQ_EVT];
 
 	switch (ioctl) {
 	case VHOST_SCSI_SET_ENDPOINT:
@@ -991,8 +1226,22 @@ static long vhost_scsi_ioctl(struct file *f, unsigned int ioctl,
 		if (copy_to_user(argp, &abi_version, sizeof abi_version))
 			return -EFAULT;
 		return 0;
+	case VHOST_SCSI_SET_EVENTS_MISSED:
+		if (get_user(events_missed, eventsp))
+			return -EFAULT;
+		mutex_lock(&vq->mutex);
+		vs->vs_events_missed = events_missed;
+		mutex_unlock(&vq->mutex);
+		return 0;
+	case VHOST_SCSI_GET_EVENTS_MISSED:
+		mutex_lock(&vq->mutex);
+		events_missed = vs->vs_events_missed;
+		mutex_unlock(&vq->mutex);
+		if (put_user(events_missed, eventsp))
+			return -EFAULT;
+		return 0;
 	case VHOST_GET_FEATURES:
-		features = VHOST_FEATURES;
+		features = VHOST_SCSI_FEATURES;
 		if (copy_to_user(featurep, &features, sizeof features))
 			return -EFAULT;
 		return 0;
@@ -1062,28 +1311,80 @@ static char *tcm_vhost_dump_proto_id(struct tcm_vhost_tport *tport)
 	return "Unknown";
 }
 
+static void tcm_vhost_do_plug(struct tcm_vhost_tpg *tpg,
+	struct se_lun *lun, bool plug)
+{
+
+	struct vhost_scsi *vs = tpg->vhost_scsi;
+	struct vhost_virtqueue *vq;
+	u32 reason;
+
+	if (!vs)
+		return;
+
+	mutex_lock(&vs->dev.mutex);
+	if (!vhost_has_feature(&vs->dev, VIRTIO_SCSI_F_HOTPLUG)) {
+		mutex_unlock(&vs->dev.mutex);
+		return;
+	}
+
+	if (plug)
+		reason = VIRTIO_SCSI_EVT_RESET_RESCAN;
+	else
+		reason = VIRTIO_SCSI_EVT_RESET_REMOVED;
+
+	vq = &vs->vqs[VHOST_SCSI_VQ_EVT];
+	mutex_lock(&vq->mutex);
+	tcm_vhost_send_evt(vs, tpg, lun,
+			VIRTIO_SCSI_T_TRANSPORT_RESET, reason);
+	mutex_unlock(&vq->mutex);
+	mutex_unlock(&vs->dev.mutex);
+}
+
+static void tcm_vhost_hotplug(struct tcm_vhost_tpg *tpg, struct se_lun *lun)
+{
+	tcm_vhost_do_plug(tpg, lun, true);
+}
+
+static void tcm_vhost_hotunplug(struct tcm_vhost_tpg *tpg, struct se_lun *lun)
+{
+	tcm_vhost_do_plug(tpg, lun, false);
+}
+
 static int tcm_vhost_port_link(struct se_portal_group *se_tpg,
 	struct se_lun *lun)
 {
 	struct tcm_vhost_tpg *tv_tpg = container_of(se_tpg,
 				struct tcm_vhost_tpg, se_tpg);
 
+	mutex_lock(&tcm_vhost_mutex);
+
 	mutex_lock(&tv_tpg->tv_tpg_mutex);
 	tv_tpg->tv_tpg_port_count++;
 	mutex_unlock(&tv_tpg->tv_tpg_mutex);
 
+	tcm_vhost_hotplug(tv_tpg, lun);
+
+	mutex_unlock(&tcm_vhost_mutex);
+
 	return 0;
 }
 
 static void tcm_vhost_port_unlink(struct se_portal_group *se_tpg,
-	struct se_lun *se_lun)
+	struct se_lun *lun)
 {
 	struct tcm_vhost_tpg *tv_tpg = container_of(se_tpg,
 				struct tcm_vhost_tpg, se_tpg);
 
+	mutex_lock(&tcm_vhost_mutex);
+
 	mutex_lock(&tv_tpg->tv_tpg_mutex);
 	tv_tpg->tv_tpg_port_count--;
 	mutex_unlock(&tv_tpg->tv_tpg_mutex);
+
+	tcm_vhost_hotunplug(tv_tpg, lun);
+
+	mutex_unlock(&tcm_vhost_mutex);
 }
 
 static struct se_node_acl *tcm_vhost_make_nodeacl(
diff --git a/drivers/vhost/tcm_vhost.h b/drivers/vhost/tcm_vhost.h
index 1d2ae7a60e1..514b9fda230 100644
--- a/drivers/vhost/tcm_vhost.h
+++ b/drivers/vhost/tcm_vhost.h
@@ -53,6 +53,7 @@ struct tcm_vhost_nacl {
 	struct se_node_acl se_node_acl;
 };
 
+struct vhost_scsi;
 struct tcm_vhost_tpg {
 	/* Vhost port target portal group tag for TCM */
 	u16 tport_tpgt;
@@ -70,6 +71,8 @@ struct tcm_vhost_tpg {
 	struct tcm_vhost_tport *tport;
 	/* Returned by tcm_vhost_make_tpg() */
 	struct se_portal_group se_tpg;
+	/* Pointer back to vhost_scsi, protected by tv_tpg_mutex */
+	struct vhost_scsi *vhost_scsi;
 };
 
 struct tcm_vhost_tport {
@@ -83,6 +86,13 @@ struct tcm_vhost_tport {
 	struct se_wwn tport_wwn;
 };
 
+struct tcm_vhost_evt {
+	/* event to be sent to guest */
+	struct virtio_scsi_event event;
+	/* event list, serviced from vhost worker thread */
+	struct llist_node list;
+};
+
 /*
  * As per request from MST, keep TCM_VHOST related ioctl defines out of
  * linux/vhost.h (user-space) for now..
@@ -113,3 +123,6 @@ struct vhost_scsi_target {
 #define VHOST_SCSI_CLEAR_ENDPOINT _IOW(VHOST_VIRTIO, 0x41, struct vhost_scsi_target)
 /* Changing this breaks userspace. */
 #define VHOST_SCSI_GET_ABI_VERSION _IOW(VHOST_VIRTIO, 0x42, int)
+/* Set and get the events missed flag */
+#define VHOST_SCSI_SET_EVENTS_MISSED _IOW(VHOST_VIRTIO, 0x43, __u32)
+#define VHOST_SCSI_GET_EVENTS_MISSED _IOW(VHOST_VIRTIO, 0x44, __u32)
diff --git a/drivers/video/Kconfig b/drivers/video/Kconfig
index 4c1546f71d5..76be61701c9 100644
--- a/drivers/video/Kconfig
+++ b/drivers/video/Kconfig
@@ -31,26 +31,8 @@ config VIDEO_OUTPUT_CONTROL
 	  This framework adds support for low-level control of the video 
 	  output switch.
 
-config DISPLAY_TIMING
-       bool
-
-config VIDEOMODE
-       bool
-
-config OF_DISPLAY_TIMING
-	bool "Enable device tree display timing support"
-	depends on OF
-	select DISPLAY_TIMING
-	help
-	  helper to parse display timings from the devicetree
-
-config OF_VIDEOMODE
-	bool "Enable device tree videomode support"
-	depends on OF
-	select VIDEOMODE
-	select OF_DISPLAY_TIMING
-	help
-	  helper to get videomodes from the devicetree
+config VIDEOMODE_HELPERS
+	bool
 
 config HDMI
 	bool
@@ -212,14 +194,6 @@ config FB_SYS_FOPS
        depends on FB
        default n
 
-config FB_WMT_GE_ROPS
-	tristate
-	depends on FB
-	default n
-	---help---
-	  Include functions for accelerated rectangle filling and area
-	  copying using WonderMedia Graphics Engine operations.
-
 config FB_DEFERRED_IO
 	bool
 	depends on FB
@@ -1797,22 +1771,37 @@ config FB_AU1200
 	  option au1200fb:panel=<name>.
 
 config FB_VT8500
-	bool "VT8500 LCD Driver"
+	bool "VIA VT8500 framebuffer support"
 	depends on (FB = y) && ARM && ARCH_VT8500
-	select FB_WMT_GE_ROPS
+	select FB_SYS_FILLRECT if (!FB_WMT_GE_ROPS)
+	select FB_SYS_COPYAREA if (!FB_WMT_GE_ROPS)
 	select FB_SYS_IMAGEBLIT
+	select FB_MODE_HELPERS
+	select VIDEOMODE_HELPERS
 	help
 	  This is the framebuffer driver for VIA VT8500 integrated LCD
 	  controller.
 
 config FB_WM8505
-	bool "WM8505 frame buffer support"
+	bool "Wondermedia WM8xxx-series frame buffer support"
 	depends on (FB = y) && ARM && ARCH_VT8500
-	select FB_WMT_GE_ROPS
+	select FB_SYS_FILLRECT if (!FB_WMT_GE_ROPS)
+	select FB_SYS_COPYAREA if (!FB_WMT_GE_ROPS)
 	select FB_SYS_IMAGEBLIT
+	select FB_MODE_HELPERS
+	select VIDEOMODE_HELPERS
+	help
+	  This is the framebuffer driver for WonderMedia WM8xxx-series
+	  integrated LCD controller. This driver covers the WM8505, WM8650
+	  and WM8850 SoCs.
+
+config FB_WMT_GE_ROPS
+	bool "VT8500/WM8xxx accelerated raster ops support"
+	depends on (FB = y) && (FB_VT8500 || FB_WM8505)
+	default n
 	help
-	  This is the framebuffer driver for WonderMedia WM8505/WM8650
-	  integrated LCD controller.
+	  This adds support for accelerated raster operations on the
+	  VIA VT8500 and Wondermedia 85xx series SoCs.
 
 source "drivers/video/geode/Kconfig"
 
@@ -2277,7 +2266,7 @@ config XEN_FBDEV_FRONTEND
 	select FB_SYS_IMAGEBLIT
 	select FB_SYS_FOPS
 	select FB_DEFERRED_IO
-	select INPUT_XEN_KBDDEV_FRONTEND
+	select INPUT_XEN_KBDDEV_FRONTEND if INPUT_MISC
 	select XEN_XENBUS_FRONTEND
 	default y
 	help
@@ -2451,6 +2440,15 @@ config FB_PUV3_UNIGFX
 	  Choose this option if you want to use the Unigfx device as a
 	  framebuffer device. Without the support of PCI & AGP.
 
+config FB_HYPERV
+	tristate "Microsoft Hyper-V Synthetic Video support"
+	depends on FB && HYPERV
+	select FB_CFB_FILLRECT
+	select FB_CFB_COPYAREA
+	select FB_CFB_IMAGEBLIT
+	help
+	  This framebuffer driver supports Microsoft Hyper-V Synthetic Video.
+
 source "drivers/video/omap/Kconfig"
 source "drivers/video/omap2/Kconfig"
 source "drivers/video/exynos/Kconfig"
diff --git a/drivers/video/Makefile b/drivers/video/Makefile
index 9df387334cb..7234e4a959e 100644
--- a/drivers/video/Makefile
+++ b/drivers/video/Makefile
@@ -149,6 +149,7 @@ obj-$(CONFIG_FB_MSM)              += msm/
 obj-$(CONFIG_FB_NUC900)           += nuc900fb.o
 obj-$(CONFIG_FB_JZ4740)		  += jz4740_fb.o
 obj-$(CONFIG_FB_PUV3_UNIGFX)      += fb-puv3.o
+obj-$(CONFIG_FB_HYPERV)		  += hyperv_fb.o
 
 # Platform or fallback drivers go here
 obj-$(CONFIG_FB_UVESA)            += uvesafb.o
@@ -171,7 +172,7 @@ obj-$(CONFIG_FB_VIRTUAL)          += vfb.o
 
 #video output switch sysfs driver
 obj-$(CONFIG_VIDEO_OUTPUT_CONTROL) += output.o
-obj-$(CONFIG_DISPLAY_TIMING) += display_timing.o
-obj-$(CONFIG_OF_DISPLAY_TIMING) += of_display_timing.o
-obj-$(CONFIG_VIDEOMODE) += videomode.o
-obj-$(CONFIG_OF_VIDEOMODE) += of_videomode.o
+obj-$(CONFIG_VIDEOMODE_HELPERS) += display_timing.o videomode.o
+ifeq ($(CONFIG_OF),y)
+obj-$(CONFIG_VIDEOMODE_HELPERS) += of_display_timing.o of_videomode.o
+endif
diff --git a/drivers/video/amifb.c b/drivers/video/amifb.c
index 7fa1bf82372..a6780eecff0 100644
--- a/drivers/video/amifb.c
+++ b/drivers/video/amifb.c
@@ -1181,7 +1181,7 @@ static int ami_decode_var(struct fb_var_screeninfo *var, struct amifb_par *par,
 	}
 
 	/*
-	 * FB_VMODE_SMOOTH_XPAN will be cleared, if one of the folloing
+	 * FB_VMODE_SMOOTH_XPAN will be cleared, if one of the following
 	 * checks failed and smooth scrolling is not possible
 	 */
 
@@ -3788,19 +3788,7 @@ static struct platform_driver amifb_driver = {
 	},
 };
 
-static int __init amifb_init(void)
-{
-	return platform_driver_probe(&amifb_driver, amifb_probe);
-}
-
-module_init(amifb_init);
-
-static void __exit amifb_exit(void)
-{
-	platform_driver_unregister(&amifb_driver);
-}
-
-module_exit(amifb_exit);
+module_platform_driver_probe(amifb_driver, amifb_probe);
 
 MODULE_LICENSE("GPL");
 MODULE_ALIAS("platform:amiga-video");
diff --git a/drivers/video/atmel_lcdfb.c b/drivers/video/atmel_lcdfb.c
index 025428e04c3..98348ec0b3c 100644
--- a/drivers/video/atmel_lcdfb.c
+++ b/drivers/video/atmel_lcdfb.c
@@ -1158,18 +1158,7 @@ static struct platform_driver atmel_lcdfb_driver = {
 	},
 };
 
-static int __init atmel_lcdfb_init(void)
-{
-	return platform_driver_probe(&atmel_lcdfb_driver, atmel_lcdfb_probe);
-}
-
-static void __exit atmel_lcdfb_exit(void)
-{
-	platform_driver_unregister(&atmel_lcdfb_driver);
-}
-
-module_init(atmel_lcdfb_init);
-module_exit(atmel_lcdfb_exit);
+module_platform_driver_probe(atmel_lcdfb_driver, atmel_lcdfb_probe);
 
 MODULE_DESCRIPTION("AT91/AT32 LCD Controller framebuffer driver");
 MODULE_AUTHOR("Nicolas Ferre <nicolas.ferre@atmel.com>");
diff --git a/drivers/video/auo_k1900fb.c b/drivers/video/auo_k1900fb.c
index 1a9ac6e1f4b..f5b668e77af 100644
--- a/drivers/video/auo_k1900fb.c
+++ b/drivers/video/auo_k1900fb.c
@@ -60,9 +60,12 @@
 
 static void auok1900_init(struct auok190xfb_par *par)
 {
+	struct device *dev = par->info->device;
 	struct auok190x_board *board = par->board;
 	u16 init_param = 0;
 
+	pm_runtime_get_sync(dev);
+
 	init_param |= AUOK1900_INIT_TEMP_AVERAGE;
 	init_param |= AUOK1900_INIT_ROTATE(par->rotation);
 	init_param |= AUOK190X_INIT_INVERSE_WHITE;
@@ -74,6 +77,9 @@ static void auok1900_init(struct auok190xfb_par *par)
 
 	/* let the controller finish */
 	board->wait_for_rdy(par);
+
+	pm_runtime_mark_last_busy(dev);
+	pm_runtime_put_autosuspend(dev);
 }
 
 static void auok1900_update_region(struct auok190xfb_par *par, int mode,
@@ -82,6 +88,7 @@ static void auok1900_update_region(struct auok190xfb_par *par, int mode,
 	struct device *dev = par->info->device;
 	unsigned char *buf = (unsigned char *)par->info->screen_base;
 	int xres = par->info->var.xres;
+	int line_length = par->info->fix.line_length;
 	u16 args[4];
 
 	pm_runtime_get_sync(dev);
@@ -100,9 +107,9 @@ static void auok1900_update_region(struct auok190xfb_par *par, int mode,
 	args[1] = y1 + 1;
 	args[2] = xres;
 	args[3] = y2 - y1;
-	buf += y1 * xres;
+	buf += y1 * line_length;
 	auok190x_send_cmdargs_pixels(par, AUOK1900_CMD_PARTIALDISP, 4, args,
-				     ((y2 - y1) * xres)/2, (u16 *) buf);
+				     ((y2 - y1) * line_length)/2, (u16 *) buf);
 	auok190x_send_command(par, AUOK190X_CMD_DATA_STOP);
 
 	par->update_cnt++;
diff --git a/drivers/video/auo_k1901fb.c b/drivers/video/auo_k1901fb.c
index d1db1653cd8..12b9adcb75c 100644
--- a/drivers/video/auo_k1901fb.c
+++ b/drivers/video/auo_k1901fb.c
@@ -101,9 +101,12 @@
 
 static void auok1901_init(struct auok190xfb_par *par)
 {
+	struct device *dev = par->info->device;
 	struct auok190x_board *board = par->board;
 	u16 init_param = 0;
 
+	pm_runtime_get_sync(dev);
+
 	init_param |= AUOK190X_INIT_INVERSE_WHITE;
 	init_param |= AUOK190X_INIT_FORMAT0;
 	init_param |= AUOK1901_INIT_RESOLUTION(par->resolution);
@@ -113,6 +116,9 @@ static void auok1901_init(struct auok190xfb_par *par)
 
 	/* let the controller finish */
 	board->wait_for_rdy(par);
+
+	pm_runtime_mark_last_busy(dev);
+	pm_runtime_put_autosuspend(dev);
 }
 
 static void auok1901_update_region(struct auok190xfb_par *par, int mode,
@@ -121,6 +127,7 @@ static void auok1901_update_region(struct auok190xfb_par *par, int mode,
 	struct device *dev = par->info->device;
 	unsigned char *buf = (unsigned char *)par->info->screen_base;
 	int xres = par->info->var.xres;
+	int line_length = par->info->fix.line_length;
 	u16 args[5];
 
 	pm_runtime_get_sync(dev);
@@ -139,9 +146,9 @@ static void auok1901_update_region(struct auok190xfb_par *par, int mode,
 	args[1] = y1 + 1;
 	args[2] = xres;
 	args[3] = y2 - y1;
-	buf += y1 * xres;
+	buf += y1 * line_length;
 	auok190x_send_cmdargs_pixels_nowait(par, AUOK1901_CMD_DMA_START, 4,
-					    args, ((y2 - y1) * xres)/2,
+					    args, ((y2 - y1) * line_length)/2,
 					    (u16 *) buf);
 	auok190x_send_command_nowait(par, AUOK190X_CMD_DATA_STOP);
 
diff --git a/drivers/video/auo_k190x.c b/drivers/video/auo_k190x.c
index 53846cb534d..8d2499d1caf 100644
--- a/drivers/video/auo_k190x.c
+++ b/drivers/video/auo_k190x.c
@@ -40,6 +40,14 @@ static struct panel_info panel_table[] = {
 		.w = 1024,
 		.h = 768,
 	},
+	[AUOK190X_RESOLUTION_600_800] = {
+		.w = 600,
+		.h = 800,
+	},
+	[AUOK190X_RESOLUTION_768_1024] = {
+		.w = 768,
+		.h = 1024,
+	},
 };
 
 /*
@@ -60,8 +68,48 @@ static void auok190x_issue_cmd(struct auok190xfb_par *par, u16 data)
 	par->board->set_ctl(par, AUOK190X_I80_DC, 1);
 }
 
-static int auok190x_issue_pixels(struct auok190xfb_par *par, int size,
-				 u16 *data)
+/**
+ * Conversion of 16bit color to 4bit grayscale
+ * does roughly (0.3 * R + 0.6 G + 0.1 B) / 2
+ */
+static inline int rgb565_to_gray4(u16 data, struct fb_var_screeninfo *var)
+{
+	return ((((data & 0xF800) >> var->red.offset) * 77 +
+		 ((data & 0x07E0) >> (var->green.offset + 1)) * 151 +
+		 ((data & 0x1F) >> var->blue.offset) * 28) >> 8 >> 1);
+}
+
+static int auok190x_issue_pixels_rgb565(struct auok190xfb_par *par, int size,
+					u16 *data)
+{
+	struct fb_var_screeninfo *var = &par->info->var;
+	struct device *dev = par->info->device;
+	int i;
+	u16 tmp;
+
+	if (size & 7) {
+		dev_err(dev, "issue_pixels: size %d must be a multiple of 8\n",
+			size);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < (size >> 2); i++) {
+		par->board->set_ctl(par, AUOK190X_I80_WR, 0);
+
+		tmp  = (rgb565_to_gray4(data[4*i], var) & 0x000F);
+		tmp |= (rgb565_to_gray4(data[4*i+1], var) << 4) & 0x00F0;
+		tmp |= (rgb565_to_gray4(data[4*i+2], var) << 8) & 0x0F00;
+		tmp |= (rgb565_to_gray4(data[4*i+3], var) << 12) & 0xF000;
+
+		par->board->set_hdb(par, tmp);
+		par->board->set_ctl(par, AUOK190X_I80_WR, 1);
+	}
+
+	return 0;
+}
+
+static int auok190x_issue_pixels_gray8(struct auok190xfb_par *par, int size,
+				       u16 *data)
 {
 	struct device *dev = par->info->device;
 	int i;
@@ -91,6 +139,23 @@ static int auok190x_issue_pixels(struct auok190xfb_par *par, int size,
 	return 0;
 }
 
+static int auok190x_issue_pixels(struct auok190xfb_par *par, int size,
+				 u16 *data)
+{
+	struct fb_info *info = par->info;
+	struct device *dev = par->info->device;
+
+	if (info->var.bits_per_pixel == 8 && info->var.grayscale)
+		auok190x_issue_pixels_gray8(par, size, data);
+	else if (info->var.bits_per_pixel == 16)
+		auok190x_issue_pixels_rgb565(par, size, data);
+	else
+		dev_err(dev, "unsupported color mode (bits: %d, gray: %d)\n",
+			info->var.bits_per_pixel, info->var.grayscale);
+
+	return 0;
+}
+
 static u16 auok190x_read_data(struct auok190xfb_par *par)
 {
 	u16 data;
@@ -224,8 +289,8 @@ static void auok190xfb_dpy_deferred_io(struct fb_info *info,
 {
 	struct fb_deferred_io *fbdefio = info->fbdefio;
 	struct auok190xfb_par *par = info->par;
+	u16 line_length = info->fix.line_length;
 	u16 yres = info->var.yres;
-	u16 xres = info->var.xres;
 	u16 y1 = 0, h = 0;
 	int prev_index = -1;
 	struct page *cur;
@@ -254,7 +319,7 @@ static void auok190xfb_dpy_deferred_io(struct fb_info *info,
 	}
 
 	/* height increment is fixed per page */
-	h_inc = DIV_ROUND_UP(PAGE_SIZE , xres);
+	h_inc = DIV_ROUND_UP(PAGE_SIZE , line_length);
 
 	/* calculate number of pages from pixel height */
 	threshold = par->consecutive_threshold / h_inc;
@@ -265,7 +330,7 @@ static void auok190xfb_dpy_deferred_io(struct fb_info *info,
 	list_for_each_entry(cur, &fbdefio->pagelist, lru) {
 		if (prev_index < 0) {
 			/* just starting so assign first page */
-			y1 = (cur->index << PAGE_SHIFT) / xres;
+			y1 = (cur->index << PAGE_SHIFT) / line_length;
 			h = h_inc;
 		} else if ((cur->index - prev_index) <= threshold) {
 			/* page is within our threshold for single updates */
@@ -275,7 +340,7 @@ static void auok190xfb_dpy_deferred_io(struct fb_info *info,
 			par->update_partial(par, y1, y1 + h);
 
 			/* start over with our non consecutive page */
-			y1 = (cur->index << PAGE_SHIFT) / xres;
+			y1 = (cur->index << PAGE_SHIFT) / line_length;
 			h = h_inc;
 		}
 		prev_index = cur->index;
@@ -376,27 +441,127 @@ static void auok190xfb_imageblit(struct fb_info *info,
 static int auok190xfb_check_var(struct fb_var_screeninfo *var,
 				   struct fb_info *info)
 {
-	if (info->var.xres != var->xres || info->var.yres != var->yres ||
-	    info->var.xres_virtual != var->xres_virtual ||
-	    info->var.yres_virtual != var->yres_virtual) {
-		pr_info("%s: Resolution not supported: X%u x Y%u\n",
-			 __func__, var->xres, var->yres);
+	struct device *dev = info->device;
+	struct auok190xfb_par *par = info->par;
+	struct panel_info *panel = &panel_table[par->resolution];
+	int size;
+
+	/*
+	 * Color depth
+	 */
+
+	if (var->bits_per_pixel == 8 && var->grayscale == 1) {
+		/*
+		 * For 8-bit grayscale, R, G, and B offset are equal.
+		 */
+		var->red.length = 8;
+		var->red.offset = 0;
+		var->red.msb_right = 0;
+
+		var->green.length = 8;
+		var->green.offset = 0;
+		var->green.msb_right = 0;
+
+		var->blue.length = 8;
+		var->blue.offset = 0;
+		var->blue.msb_right = 0;
+
+		var->transp.length = 0;
+		var->transp.offset = 0;
+		var->transp.msb_right = 0;
+	} else if (var->bits_per_pixel == 16) {
+		var->red.length = 5;
+		var->red.offset = 11;
+		var->red.msb_right = 0;
+
+		var->green.length = 6;
+		var->green.offset = 5;
+		var->green.msb_right = 0;
+
+		var->blue.length = 5;
+		var->blue.offset = 0;
+		var->blue.msb_right = 0;
+
+		var->transp.length = 0;
+		var->transp.offset = 0;
+		var->transp.msb_right = 0;
+	} else {
+		dev_warn(dev, "unsupported color mode (bits: %d, grayscale: %d)\n",
+			info->var.bits_per_pixel, info->var.grayscale);
 		return -EINVAL;
 	}
 
 	/*
+	 * Dimensions
+	 */
+
+	switch (var->rotate) {
+	case FB_ROTATE_UR:
+	case FB_ROTATE_UD:
+		var->xres = panel->w;
+		var->yres = panel->h;
+		break;
+	case FB_ROTATE_CW:
+	case FB_ROTATE_CCW:
+		var->xres = panel->h;
+		var->yres = panel->w;
+		break;
+	default:
+		dev_dbg(dev, "Invalid rotation request\n");
+		return -EINVAL;
+	}
+
+	var->xres_virtual = var->xres;
+	var->yres_virtual = var->yres;
+
+	/*
 	 *  Memory limit
 	 */
 
-	if ((info->fix.line_length * var->yres_virtual) > info->fix.smem_len) {
-		pr_info("%s: Memory Limit requested yres_virtual = %u\n",
-			 __func__, var->yres_virtual);
+	size = var->xres_virtual * var->yres_virtual * var->bits_per_pixel / 8;
+	if (size > info->fix.smem_len) {
+		dev_err(dev, "Memory limit exceeded, requested %dK\n",
+			size >> 10);
 		return -ENOMEM;
 	}
 
 	return 0;
 }
 
+static int auok190xfb_set_fix(struct fb_info *info)
+{
+	struct fb_fix_screeninfo *fix = &info->fix;
+	struct fb_var_screeninfo *var = &info->var;
+
+	fix->line_length = var->xres_virtual * var->bits_per_pixel / 8;
+
+	fix->type = FB_TYPE_PACKED_PIXELS;
+	fix->accel = FB_ACCEL_NONE;
+	fix->visual = (var->grayscale) ? FB_VISUAL_STATIC_PSEUDOCOLOR
+				       : FB_VISUAL_TRUECOLOR;
+	fix->xpanstep = 0;
+	fix->ypanstep = 0;
+	fix->ywrapstep = 0;
+
+	return 0;
+}
+
+static int auok190xfb_set_par(struct fb_info *info)
+{
+	struct auok190xfb_par *par = info->par;
+
+	par->rotation = info->var.rotate;
+	auok190xfb_set_fix(info);
+
+	/* reinit the controller to honor the rotation */
+	par->init(par);
+
+	/* wait for init to complete */
+	par->board->wait_for_rdy(par);
+
+	return 0;
+}
+
 static struct fb_ops auok190xfb_ops = {
 	.owner		= THIS_MODULE,
 	.fb_read	= fb_sys_read,
@@ -405,6 +570,7 @@ static struct fb_ops auok190xfb_ops = {
 	.fb_copyarea	= auok190xfb_copyarea,
 	.fb_imageblit	= auok190xfb_imageblit,
 	.fb_check_var	= auok190xfb_check_var,
+	.fb_set_par     = auok190xfb_set_par,
 };
 
 /*
@@ -588,10 +754,16 @@ static int auok190x_power(struct auok190xfb_par *par, bool on)
 
 static void auok190x_recover(struct auok190xfb_par *par)
 {
+	struct device *dev = par->info->device;
+
 	auok190x_power(par, 0);
 	msleep(100);
 	auok190x_power(par, 1);
 
+	/* after powercycling the device, it's always active */
+	pm_runtime_set_active(dev);
+	par->standby = 0;
+
 	par->init(par);
 
 	/* wait for init to complete */
@@ -875,42 +1047,17 @@ int auok190x_common_probe(struct platform_device *pdev,
 	/* initialise fix, var, resolution and rotation */
 
 	strlcpy(info->fix.id, init->id, 16);
-	info->fix.type = FB_TYPE_PACKED_PIXELS;
-	info->fix.visual = FB_VISUAL_STATIC_PSEUDOCOLOR;
-	info->fix.xpanstep = 0;
-	info->fix.ypanstep = 0;
-	info->fix.ywrapstep = 0;
-	info->fix.accel = FB_ACCEL_NONE;
-
 	info->var.bits_per_pixel = 8;
 	info->var.grayscale = 1;
-	info->var.red.length = 8;
-	info->var.green.length = 8;
-	info->var.blue.length = 8;
 
 	panel = &panel_table[board->resolution];
 
-	/* if 90 degree rotation, switch width and height */
-	if (board->rotation & 1) {
-		info->var.xres = panel->h;
-		info->var.yres = panel->w;
-		info->var.xres_virtual = panel->h;
-		info->var.yres_virtual = panel->w;
-		info->fix.line_length = panel->h;
-	} else {
-		info->var.xres = panel->w;
-		info->var.yres = panel->h;
-		info->var.xres_virtual = panel->w;
-		info->var.yres_virtual = panel->h;
-		info->fix.line_length = panel->w;
-	}
-
 	par->resolution = board->resolution;
-	par->rotation = board->rotation;
+	par->rotation = 0;
 
 	/* videomemory handling */
 
-	videomemorysize = roundup((panel->w * panel->h), PAGE_SIZE);
+	videomemorysize = roundup((panel->w * panel->h) * 2, PAGE_SIZE);
 	videomemory = vmalloc(videomemorysize);
 	if (!videomemory) {
 		ret = -ENOMEM;
@@ -924,6 +1071,12 @@ int auok190x_common_probe(struct platform_device *pdev,
 	info->flags = FBINFO_FLAG_DEFAULT | FBINFO_VIRTFB;
 	info->fbops = &auok190xfb_ops;
 
+	ret = auok190xfb_check_var(&info->var, info);
+	if (ret)
+		goto err_defio;
+
+	auok190xfb_set_fix(info);
+
 	/* deferred io init */
 
 	info->fbdefio = devm_kzalloc(info->device,
@@ -935,7 +1088,7 @@ int auok190x_common_probe(struct platform_device *pdev,
 		goto err_defio;
 	}
 
-	dev_dbg(info->device, "targetting %d frames per second\n", board->fps);
+	dev_dbg(info->device, "targeting %d frames per second\n", board->fps);
 	info->fbdefio->delay = HZ / board->fps;
 	info->fbdefio->first_io = auok190xfb_dpy_first_io,
 	info->fbdefio->deferred_io = auok190xfb_dpy_deferred_io,
diff --git a/drivers/video/backlight/Kconfig b/drivers/video/backlight/Kconfig
index db10d0120d2..2e166c3fc4c 100644
--- a/drivers/video/backlight/Kconfig
+++ b/drivers/video/backlight/Kconfig
@@ -59,6 +59,13 @@ config LCD_LTV350QV
 
 	  The LTV350QV panel is present on all ATSTK1000 boards.
 
+config LCD_ILI922X
+	tristate "ILI Technology ILI9221/ILI9222 support"
+	depends on SPI
+	help
+	  If you have a panel based on the ILI9221/9222 controller
+	  chip then say y to include a driver for it.
+
 config LCD_ILI9320
 	tristate "ILI Technology ILI9320 controller support"
 	depends on SPI
@@ -161,7 +168,7 @@ if BACKLIGHT_CLASS_DEVICE
 config BACKLIGHT_ATMEL_LCDC
 	bool "Atmel LCDC Contrast-as-Backlight control"
 	depends on FB_ATMEL
-	default y if MACH_SAM9261EK || MACH_SAM9G10EK || MACH_SAM9263EK
+	default y if MACH_AT91SAM9261EK || MACH_AT91SAM9G10EK || MACH_AT91SAM9263EK
 	help
 	  This provides a backlight control internal to the Atmel LCDC
 	  driver.  If the LCD "contrast control" on your board is wired
diff --git a/drivers/video/backlight/Makefile b/drivers/video/backlight/Makefile
index 96c4d620c5c..92711fe6046 100644
--- a/drivers/video/backlight/Makefile
+++ b/drivers/video/backlight/Makefile
@@ -5,6 +5,7 @@ obj-$(CONFIG_LCD_CLASS_DEVICE)		+= lcd.o
 obj-$(CONFIG_LCD_CORGI)			+= corgi_lcd.o
 obj-$(CONFIG_LCD_HP700)			+= jornada720_lcd.o
 obj-$(CONFIG_LCD_HX8357)		+= hx8357.o
+obj-$(CONFIG_LCD_ILI922X)		+= ili922x.o
 obj-$(CONFIG_LCD_ILI9320)		+= ili9320.o
 obj-$(CONFIG_LCD_L4F00242T03)		+= l4f00242t03.o
 obj-$(CONFIG_LCD_LD9040)		+= ld9040.o
diff --git a/drivers/video/backlight/adp5520_bl.c b/drivers/video/backlight/adp5520_bl.c
index a1e41d4faa7..c84701b7ca6 100644
--- a/drivers/video/backlight/adp5520_bl.c
+++ b/drivers/video/backlight/adp5520_bl.c
@@ -143,13 +143,16 @@ static int adp5520_bl_setup(struct backlight_device *bl)
 static ssize_t adp5520_show(struct device *dev, char *buf, int reg)
 {
 	struct adp5520_bl *data = dev_get_drvdata(dev);
-	int error;
+	int ret;
 	uint8_t reg_val;
 
 	mutex_lock(&data->lock);
-	error = adp5520_read(data->master, reg, &reg_val);
+	ret = adp5520_read(data->master, reg, &reg_val);
 	mutex_unlock(&data->lock);
 
+	if (ret < 0)
+		return ret;
+
 	return sprintf(buf, "%u\n", reg_val);
 }
 
@@ -349,35 +352,34 @@ static int adp5520_bl_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
-static int adp5520_bl_suspend(struct platform_device *pdev,
-				 pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int adp5520_bl_suspend(struct device *dev)
 {
-	struct backlight_device *bl = platform_get_drvdata(pdev);
+	struct backlight_device *bl = dev_get_drvdata(dev);
+
 	return adp5520_bl_set(bl, 0);
 }
 
-static int adp5520_bl_resume(struct platform_device *pdev)
+static int adp5520_bl_resume(struct device *dev)
 {
-	struct backlight_device *bl = platform_get_drvdata(pdev);
+	struct backlight_device *bl = dev_get_drvdata(dev);
 
 	backlight_update_status(bl);
 	return 0;
 }
-#else
-#define adp5520_bl_suspend	NULL
-#define adp5520_bl_resume	NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(adp5520_bl_pm_ops, adp5520_bl_suspend,
+			adp5520_bl_resume);
+
 static struct platform_driver adp5520_bl_driver = {
 	.driver		= {
 		.name	= "adp5520-backlight",
 		.owner	= THIS_MODULE,
+		.pm	= &adp5520_bl_pm_ops,
 	},
 	.probe		= adp5520_bl_probe,
 	.remove		= adp5520_bl_remove,
-	.suspend	= adp5520_bl_suspend,
-	.resume		= adp5520_bl_resume,
 };
 
 module_platform_driver(adp5520_bl_driver);
diff --git a/drivers/video/backlight/adp8860_bl.c b/drivers/video/backlight/adp8860_bl.c
index a77c9cad332..75b10f87612 100644
--- a/drivers/video/backlight/adp8860_bl.c
+++ b/drivers/video/backlight/adp8860_bl.c
@@ -249,12 +249,14 @@ static int adp8860_led_probe(struct i2c_client *client)
 		if (led_dat->id > 7 || led_dat->id < 1) {
 			dev_err(&client->dev, "Invalid LED ID %d\n",
 				led_dat->id);
+			ret = -EINVAL;
 			goto err;
 		}
 
 		if (pdata->bl_led_assign & (1 << (led_dat->id - 1))) {
 			dev_err(&client->dev, "LED %d used by Backlight\n",
 				led_dat->id);
+			ret = -EBUSY;
 			goto err;
 		}
 
@@ -773,25 +775,29 @@ static int adp8860_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM
-static int adp8860_i2c_suspend(struct i2c_client *client, pm_message_t message)
+#ifdef CONFIG_PM_SLEEP
+static int adp8860_i2c_suspend(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
+
 	adp8860_clr_bits(client, ADP8860_MDCR, NSTBY);
 
 	return 0;
 }
 
-static int adp8860_i2c_resume(struct i2c_client *client)
+static int adp8860_i2c_resume(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
+
 	adp8860_set_bits(client, ADP8860_MDCR, NSTBY | BLEN);
 
 	return 0;
 }
-#else
-#define adp8860_i2c_suspend NULL
-#define adp8860_i2c_resume NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(adp8860_i2c_pm_ops, adp8860_i2c_suspend,
+			adp8860_i2c_resume);
+
 static const struct i2c_device_id adp8860_id[] = {
 	{ "adp8860", adp8860 },
 	{ "adp8861", adp8861 },
@@ -802,12 +808,11 @@ MODULE_DEVICE_TABLE(i2c, adp8860_id);
 
 static struct i2c_driver adp8860_driver = {
 	.driver = {
-		.name = KBUILD_MODNAME,
+		.name	= KBUILD_MODNAME,
+		.pm	= &adp8860_i2c_pm_ops,
 	},
 	.probe    = adp8860_probe,
 	.remove   = adp8860_remove,
-	.suspend = adp8860_i2c_suspend,
-	.resume  = adp8860_i2c_resume,
 	.id_table = adp8860_id,
 };
 
diff --git a/drivers/video/backlight/adp8870_bl.c b/drivers/video/backlight/adp8870_bl.c
index 712c25a0d8f..90049d7b5c6 100644
--- a/drivers/video/backlight/adp8870_bl.c
+++ b/drivers/video/backlight/adp8870_bl.c
@@ -274,12 +274,14 @@ static int adp8870_led_probe(struct i2c_client *client)
 		if (led_dat->id > 7 || led_dat->id < 1) {
 			dev_err(&client->dev, "Invalid LED ID %d\n",
 				led_dat->id);
+			ret = -EINVAL;
 			goto err;
 		}
 
 		if (pdata->bl_led_assign & (1 << (led_dat->id - 1))) {
 			dev_err(&client->dev, "LED %d used by Backlight\n",
 				led_dat->id);
+			ret = -EBUSY;
 			goto err;
 		}
 
@@ -895,13 +897,13 @@ static int adp8870_probe(struct i2c_client *client,
 
 	data->bl = bl;
 
-	if (pdata->en_ambl_sens)
+	if (pdata->en_ambl_sens) {
 		ret = sysfs_create_group(&bl->dev.kobj,
 			&adp8870_bl_attr_group);
-
-	if (ret) {
-		dev_err(&client->dev, "failed to register sysfs\n");
-		goto out1;
+		if (ret) {
+			dev_err(&client->dev, "failed to register sysfs\n");
+			goto out1;
+		}
 	}
 
 	ret = adp8870_bl_setup(bl);
@@ -947,25 +949,29 @@ static int adp8870_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM
-static int adp8870_i2c_suspend(struct i2c_client *client, pm_message_t message)
+#ifdef CONFIG_PM_SLEEP
+static int adp8870_i2c_suspend(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
+
 	adp8870_clr_bits(client, ADP8870_MDCR, NSTBY);
 
 	return 0;
 }
 
-static int adp8870_i2c_resume(struct i2c_client *client)
+static int adp8870_i2c_resume(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
+
 	adp8870_set_bits(client, ADP8870_MDCR, NSTBY | BLEN);
 
 	return 0;
 }
-#else
-#define adp8870_i2c_suspend NULL
-#define adp8870_i2c_resume NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(adp8870_i2c_pm_ops, adp8870_i2c_suspend,
+			adp8870_i2c_resume);
+
 static const struct i2c_device_id adp8870_id[] = {
 	{ "adp8870", 0 },
 	{ }
@@ -974,12 +980,11 @@ MODULE_DEVICE_TABLE(i2c, adp8870_id);
 
 static struct i2c_driver adp8870_driver = {
 	.driver = {
-		.name = KBUILD_MODNAME,
+		.name	= KBUILD_MODNAME,
+		.pm	= &adp8870_i2c_pm_ops,
 	},
 	.probe    = adp8870_probe,
 	.remove   = adp8870_remove,
-	.suspend = adp8870_i2c_suspend,
-	.resume  = adp8870_i2c_resume,
 	.id_table = adp8870_id,
 };
 
diff --git a/drivers/video/backlight/ams369fg06.c b/drivers/video/backlight/ams369fg06.c
index c02aa2c2575..319fef6cb42 100644
--- a/drivers/video/backlight/ams369fg06.c
+++ b/drivers/video/backlight/ams369fg06.c
@@ -533,12 +533,12 @@ static int ams369fg06_remove(struct spi_device *spi)
 	return 0;
 }
 
-#if defined(CONFIG_PM)
-static int ams369fg06_suspend(struct spi_device *spi, pm_message_t mesg)
+#ifdef CONFIG_PM_SLEEP
+static int ams369fg06_suspend(struct device *dev)
 {
-	struct ams369fg06 *lcd = spi_get_drvdata(spi);
+	struct ams369fg06 *lcd = dev_get_drvdata(dev);
 
-	dev_dbg(&spi->dev, "lcd->power = %d\n", lcd->power);
+	dev_dbg(dev, "lcd->power = %d\n", lcd->power);
 
 	/*
 	 * when lcd panel is suspend, lcd panel becomes off
@@ -547,19 +547,19 @@ static int ams369fg06_suspend(struct spi_device *spi, pm_message_t mesg)
 	return ams369fg06_power(lcd, FB_BLANK_POWERDOWN);
 }
 
-static int ams369fg06_resume(struct spi_device *spi)
+static int ams369fg06_resume(struct device *dev)
 {
-	struct ams369fg06 *lcd = spi_get_drvdata(spi);
+	struct ams369fg06 *lcd = dev_get_drvdata(dev);
 
 	lcd->power = FB_BLANK_POWERDOWN;
 
 	return ams369fg06_power(lcd, FB_BLANK_UNBLANK);
 }
-#else
-#define ams369fg06_suspend	NULL
-#define ams369fg06_resume	NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(ams369fg06_pm_ops, ams369fg06_suspend,
+			ams369fg06_resume);
+
 static void ams369fg06_shutdown(struct spi_device *spi)
 {
 	struct ams369fg06 *lcd = spi_get_drvdata(spi);
@@ -571,12 +571,11 @@ static struct spi_driver ams369fg06_driver = {
 	.driver = {
 		.name	= "ams369fg06",
 		.owner	= THIS_MODULE,
+		.pm	= &ams369fg06_pm_ops,
 	},
 	.probe		= ams369fg06_probe,
 	.remove		= ams369fg06_remove,
 	.shutdown	= ams369fg06_shutdown,
-	.suspend	= ams369fg06_suspend,
-	.resume		= ams369fg06_resume,
 };
 
 module_spi_driver(ams369fg06_driver);
diff --git a/drivers/video/backlight/as3711_bl.c b/drivers/video/backlight/as3711_bl.c
index 41d52fe5254..123887cd76b 100644
--- a/drivers/video/backlight/as3711_bl.c
+++ b/drivers/video/backlight/as3711_bl.c
@@ -258,6 +258,109 @@ static int as3711_bl_register(struct platform_device *pdev,
 	return 0;
 }
 
+static int as3711_backlight_parse_dt(struct device *dev)
+{
+	struct as3711_bl_pdata *pdata = dev_get_platdata(dev);
+	struct device_node *bl =
+		of_find_node_by_name(dev->parent->of_node, "backlight"), *fb;
+	int ret;
+
+	if (!bl) {
+		dev_dbg(dev, "backlight node not found\n");
+		return -ENODEV;
+	}
+
+	fb = of_parse_phandle(bl, "su1-dev", 0);
+	if (fb) {
+		pdata->su1_fb = fb->full_name;
+
+		ret = of_property_read_u32(bl, "su1-max-uA", &pdata->su1_max_uA);
+		if (pdata->su1_max_uA <= 0)
+			ret = -EINVAL;
+		if (ret < 0)
+			return ret;
+	}
+
+	fb = of_parse_phandle(bl, "su2-dev", 0);
+	if (fb) {
+		int count = 0;
+
+		pdata->su2_fb = fb->full_name;
+
+		ret = of_property_read_u32(bl, "su2-max-uA", &pdata->su2_max_uA);
+		if (pdata->su2_max_uA <= 0)
+			ret = -EINVAL;
+		if (ret < 0)
+			return ret;
+
+		if (of_find_property(bl, "su2-feedback-voltage", NULL)) {
+			pdata->su2_feedback = AS3711_SU2_VOLTAGE;
+			count++;
+		}
+		if (of_find_property(bl, "su2-feedback-curr1", NULL)) {
+			pdata->su2_feedback = AS3711_SU2_CURR1;
+			count++;
+		}
+		if (of_find_property(bl, "su2-feedback-curr2", NULL)) {
+			pdata->su2_feedback = AS3711_SU2_CURR2;
+			count++;
+		}
+		if (of_find_property(bl, "su2-feedback-curr3", NULL)) {
+			pdata->su2_feedback = AS3711_SU2_CURR3;
+			count++;
+		}
+		if (of_find_property(bl, "su2-feedback-curr-auto", NULL)) {
+			pdata->su2_feedback = AS3711_SU2_CURR_AUTO;
+			count++;
+		}
+		if (count != 1)
+			return -EINVAL;
+
+		count = 0;
+		if (of_find_property(bl, "su2-fbprot-lx-sd4", NULL)) {
+			pdata->su2_fbprot = AS3711_SU2_LX_SD4;
+			count++;
+		}
+		if (of_find_property(bl, "su2-fbprot-gpio2", NULL)) {
+			pdata->su2_fbprot = AS3711_SU2_GPIO2;
+			count++;
+		}
+		if (of_find_property(bl, "su2-fbprot-gpio3", NULL)) {
+			pdata->su2_fbprot = AS3711_SU2_GPIO3;
+			count++;
+		}
+		if (of_find_property(bl, "su2-fbprot-gpio4", NULL)) {
+			pdata->su2_fbprot = AS3711_SU2_GPIO4;
+			count++;
+		}
+		if (count != 1)
+			return -EINVAL;
+
+		count = 0;
+		if (of_find_property(bl, "su2-auto-curr1", NULL)) {
+			pdata->su2_auto_curr1 = true;
+			count++;
+		}
+		if (of_find_property(bl, "su2-auto-curr2", NULL)) {
+			pdata->su2_auto_curr2 = true;
+			count++;
+		}
+		if (of_find_property(bl, "su2-auto-curr3", NULL)) {
+			pdata->su2_auto_curr3 = true;
+			count++;
+		}
+
+		/*
+		 * At least one su2-auto-curr* must be specified iff
+		 * AS3711_SU2_CURR_AUTO is used
+		 */
+		if (!count ^ (pdata->su2_feedback != AS3711_SU2_CURR_AUTO))
+			return -EINVAL;
+	}
+
+	return 0;
+}
+
 static int as3711_backlight_probe(struct platform_device *pdev)
 {
 	struct as3711_bl_pdata *pdata = dev_get_platdata(&pdev->dev);
@@ -267,11 +370,24 @@ static int as3711_backlight_probe(struct platform_device *pdev)
 	unsigned int max_brightness;
 	int ret;
 
-	if (!pdata || (!pdata->su1_fb && !pdata->su2_fb)) {
+	if (!pdata) {
 		dev_err(&pdev->dev, "No platform data, exiting...\n");
 		return -ENODEV;
 	}
 
+	if (pdev->dev.parent->of_node) {
+		ret = as3711_backlight_parse_dt(&pdev->dev);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "DT parsing failed: %d\n", ret);
+			return ret;
+		}
+	}
+
+	if (!pdata->su1_fb && !pdata->su2_fb) {
+		dev_err(&pdev->dev, "No framebuffer specified\n");
+		return -EINVAL;
+	}
+
 	/*
 	 * Due to possible hardware damage I chose to block all modes,
 	 * unsupported on my hardware. Anyone, wishing to use any of those modes
diff --git a/drivers/video/backlight/atmel-pwm-bl.c b/drivers/video/backlight/atmel-pwm-bl.c
index de5e5e74e2a..a60d6afca97 100644
--- a/drivers/video/backlight/atmel-pwm-bl.c
+++ b/drivers/video/backlight/atmel-pwm-bl.c
@@ -118,7 +118,7 @@ static const struct backlight_ops atmel_pwm_bl_ops = {
 	.update_status  = atmel_pwm_bl_set_intensity,
 };
 
-static int atmel_pwm_bl_probe(struct platform_device *pdev)
+static int __init atmel_pwm_bl_probe(struct platform_device *pdev)
 {
 	struct backlight_properties props;
 	const struct atmel_pwm_bl_platform_data *pdata;
@@ -225,17 +225,7 @@ static struct platform_driver atmel_pwm_bl_driver = {
 	.remove = __exit_p(atmel_pwm_bl_remove),
 };
 
-static int __init atmel_pwm_bl_init(void)
-{
-	return platform_driver_probe(&atmel_pwm_bl_driver, atmel_pwm_bl_probe);
-}
-module_init(atmel_pwm_bl_init);
-
-static void __exit atmel_pwm_bl_exit(void)
-{
-	platform_driver_unregister(&atmel_pwm_bl_driver);
-}
-module_exit(atmel_pwm_bl_exit);
+module_platform_driver_probe(atmel_pwm_bl_driver, atmel_pwm_bl_probe);
 
 MODULE_AUTHOR("Hans-Christian egtvedt <hans-christian.egtvedt@atmel.com>");
 MODULE_DESCRIPTION("Atmel PWM backlight driver");
diff --git a/drivers/video/backlight/corgi_lcd.c b/drivers/video/backlight/corgi_lcd.c
index aa782f30298..c97867a717a 100644
--- a/drivers/video/backlight/corgi_lcd.c
+++ b/drivers/video/backlight/corgi_lcd.c
@@ -457,10 +457,10 @@ static const struct backlight_ops corgi_bl_ops = {
 	.update_status  = corgi_bl_update_status,
 };
 
-#ifdef CONFIG_PM
-static int corgi_lcd_suspend(struct spi_device *spi, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int corgi_lcd_suspend(struct device *dev)
 {
-	struct corgi_lcd *lcd = spi_get_drvdata(spi);
+	struct corgi_lcd *lcd = dev_get_drvdata(dev);
 
 	corgibl_flags |= CORGIBL_SUSPENDED;
 	corgi_bl_set_intensity(lcd, 0);
@@ -468,20 +468,19 @@ static int corgi_lcd_suspend(struct spi_device *spi, pm_message_t state)
 	return 0;
 }
 
-static int corgi_lcd_resume(struct spi_device *spi)
+static int corgi_lcd_resume(struct device *dev)
 {
-	struct corgi_lcd *lcd = spi_get_drvdata(spi);
+	struct corgi_lcd *lcd = dev_get_drvdata(dev);
 
 	corgibl_flags &= ~CORGIBL_SUSPENDED;
 	corgi_lcd_set_power(lcd->lcd_dev, FB_BLANK_UNBLANK);
 	backlight_update_status(lcd->bl_dev);
 	return 0;
 }
-#else
-#define corgi_lcd_suspend	NULL
-#define corgi_lcd_resume	NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(corgi_lcd_pm_ops, corgi_lcd_suspend, corgi_lcd_resume);
+
 static int setup_gpio_backlight(struct corgi_lcd *lcd,
 				struct corgi_lcd_platform_data *pdata)
 {
@@ -611,11 +610,10 @@ static struct spi_driver corgi_lcd_driver = {
 	.driver		= {
 		.name	= "corgi-lcd",
 		.owner	= THIS_MODULE,
+		.pm	= &corgi_lcd_pm_ops,
 	},
 	.probe		= corgi_lcd_probe,
 	.remove		= corgi_lcd_remove,
-	.suspend	= corgi_lcd_suspend,
-	.resume		= corgi_lcd_resume,
 };
 
 module_spi_driver(corgi_lcd_driver);
diff --git a/drivers/video/backlight/da903x_bl.c b/drivers/video/backlight/da903x_bl.c
index 8179cef0730..67cadd30e27 100644
--- a/drivers/video/backlight/da903x_bl.c
+++ b/drivers/video/backlight/da903x_bl.c
@@ -88,16 +88,21 @@ static int da903x_backlight_update_status(struct backlight_device *bl)
 	if (bl->props.fb_blank != FB_BLANK_UNBLANK)
 		brightness = 0;
 
+	if (bl->props.state & BL_CORE_SUSPENDED)
+		brightness = 0;
+
 	return da903x_backlight_set(bl, brightness);
 }
 
 static int da903x_backlight_get_brightness(struct backlight_device *bl)
 {
 	struct da903x_backlight_data *data = bl_get_data(bl);
+
 	return data->current_brightness;
 }
 
 static const struct backlight_ops da903x_backlight_ops = {
+	.options	= BL_CORE_SUSPENDRESUME,
 	.update_status	= da903x_backlight_update_status,
 	.get_brightness	= da903x_backlight_get_brightness,
 };
@@ -161,35 +166,10 @@ static int da903x_backlight_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
-static int da903x_backlight_suspend(struct device *dev)
-{
-	struct backlight_device *bl = dev_get_drvdata(dev);
-
-	return da903x_backlight_set(bl, 0);
-}
-
-static int da903x_backlight_resume(struct device *dev)
-{
-	struct backlight_device *bl = dev_get_drvdata(dev);
-
-	backlight_update_status(bl);
-	return 0;
-}
-
-static const struct dev_pm_ops da903x_backlight_pm_ops = {
-	.suspend	= da903x_backlight_suspend,
-	.resume		= da903x_backlight_resume,
-};
-#endif
-
 static struct platform_driver da903x_backlight_driver = {
 	.driver		= {
 		.name	= "da903x-backlight",
 		.owner	= THIS_MODULE,
-#ifdef CONFIG_PM
-		.pm	= &da903x_backlight_pm_ops,
-#endif
 	},
 	.probe		= da903x_backlight_probe,
 	.remove		= da903x_backlight_remove,
diff --git a/drivers/video/backlight/ep93xx_bl.c b/drivers/video/backlight/ep93xx_bl.c
index ef3e21e8f82..33455821dd3 100644
--- a/drivers/video/backlight/ep93xx_bl.c
+++ b/drivers/video/backlight/ep93xx_bl.c
@@ -60,7 +60,7 @@ static const struct backlight_ops ep93xxbl_ops = {
 	.get_brightness	= ep93xxbl_get_brightness,
 };
 
-static int __init ep93xxbl_probe(struct platform_device *dev)
+static int ep93xxbl_probe(struct platform_device *dev)
 {
 	struct ep93xxbl *ep93xxbl;
 	struct backlight_device *bl;
@@ -115,35 +115,33 @@ static int ep93xxbl_remove(struct platform_device *dev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
-static int ep93xxbl_suspend(struct platform_device *dev, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int ep93xxbl_suspend(struct device *dev)
 {
-	struct backlight_device *bl = platform_get_drvdata(dev);
+	struct backlight_device *bl = dev_get_drvdata(dev);
 
 	return ep93xxbl_set(bl, 0);
 }
 
-static int ep93xxbl_resume(struct platform_device *dev)
+static int ep93xxbl_resume(struct device *dev)
 {
-	struct backlight_device *bl = platform_get_drvdata(dev);
+	struct backlight_device *bl = dev_get_drvdata(dev);
 
 	backlight_update_status(bl);
 	return 0;
 }
-#else
-#define ep93xxbl_suspend	NULL
-#define ep93xxbl_resume		NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(ep93xxbl_pm_ops, ep93xxbl_suspend, ep93xxbl_resume);
+
 static struct platform_driver ep93xxbl_driver = {
 	.driver		= {
 		.name	= "ep93xx-bl",
 		.owner	= THIS_MODULE,
+		.pm	= &ep93xxbl_pm_ops,
 	},
 	.probe		= ep93xxbl_probe,
 	.remove		= ep93xxbl_remove,
-	.suspend	= ep93xxbl_suspend,
-	.resume		= ep93xxbl_resume,
 };
 
 module_platform_driver(ep93xxbl_driver);
diff --git a/drivers/video/backlight/generic_bl.c b/drivers/video/backlight/generic_bl.c
index 0ae155be9c8..19e393b4143 100644
--- a/drivers/video/backlight/generic_bl.c
+++ b/drivers/video/backlight/generic_bl.c
@@ -9,8 +9,6 @@
  *
  */
 
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
@@ -108,7 +106,7 @@ static int genericbl_probe(struct platform_device *pdev)
 
 	generic_backlight_device = bd;
 
-	pr_info("Generic Backlight Driver Initialized.\n");
+	dev_info(&pdev->dev, "Generic Backlight Driver Initialized.\n");
 	return 0;
 }
 
@@ -122,7 +120,7 @@ static int genericbl_remove(struct platform_device *pdev)
 
 	backlight_device_unregister(bd);
 
-	pr_info("Generic Backlight Driver Unloaded\n");
+	dev_info(&pdev->dev, "Generic Backlight Driver Unloaded\n");
 	return 0;
 }
 
diff --git a/drivers/video/backlight/hp680_bl.c b/drivers/video/backlight/hp680_bl.c
index 5cefd73526f..00076ecfe9b 100644
--- a/drivers/video/backlight/hp680_bl.c
+++ b/drivers/video/backlight/hp680_bl.c
@@ -64,29 +64,28 @@ static void hp680bl_send_intensity(struct backlight_device *bd)
 }
 
 
-#ifdef CONFIG_PM
-static int hp680bl_suspend(struct platform_device *pdev, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int hp680bl_suspend(struct device *dev)
 {
-	struct backlight_device *bd = platform_get_drvdata(pdev);
+	struct backlight_device *bd = dev_get_drvdata(dev);
 
 	hp680bl_suspended = 1;
 	hp680bl_send_intensity(bd);
 	return 0;
 }
 
-static int hp680bl_resume(struct platform_device *pdev)
+static int hp680bl_resume(struct device *dev)
 {
-	struct backlight_device *bd = platform_get_drvdata(pdev);
+	struct backlight_device *bd = dev_get_drvdata(dev);
 
 	hp680bl_suspended = 0;
 	hp680bl_send_intensity(bd);
 	return 0;
 }
-#else
-#define hp680bl_suspend	NULL
-#define hp680bl_resume	NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(hp680bl_pm_ops, hp680bl_suspend, hp680bl_resume);
+
 static int hp680bl_set_intensity(struct backlight_device *bd)
 {
 	hp680bl_send_intensity(bd);
@@ -140,10 +139,9 @@ static int hp680bl_remove(struct platform_device *pdev)
 static struct platform_driver hp680bl_driver = {
 	.probe		= hp680bl_probe,
 	.remove		= hp680bl_remove,
-	.suspend	= hp680bl_suspend,
-	.resume		= hp680bl_resume,
 	.driver		= {
 		.name	= "hp680-bl",
+		.pm	= &hp680bl_pm_ops,
 	},
 };
 
diff --git a/drivers/video/backlight/ili922x.c b/drivers/video/backlight/ili922x.c
new file mode 100644
index 00000000000..d9f65c2d9b0
--- /dev/null
+++ b/drivers/video/backlight/ili922x.c
@@ -0,0 +1,555 @@
+/*
+ * (C) Copyright 2008
+ * Stefano Babic, DENX Software Engineering, sbabic@denx.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 driver implements a lcd device for the ILITEK 922x display
+ * controller. The interface to the display is SPI and the display's
+ * memory is cyclically updated over the RGB interface.
+ */
+
+#include <linux/fb.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/lcd.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/slab.h>
+#include <linux/spi/spi.h>
+#include <linux/string.h>
+
+/* Register offset, see manual section 8.2 */
+#define REG_START_OSCILLATION			0x00
+#define REG_DRIVER_CODE_READ			0x00
+#define REG_DRIVER_OUTPUT_CONTROL		0x01
+#define REG_LCD_AC_DRIVEING_CONTROL		0x02
+#define REG_ENTRY_MODE				0x03
+#define REG_COMPARE_1				0x04
+#define REG_COMPARE_2				0x05
+#define REG_DISPLAY_CONTROL_1			0x07
+#define REG_DISPLAY_CONTROL_2			0x08
+#define REG_DISPLAY_CONTROL_3			0x09
+#define REG_FRAME_CYCLE_CONTROL			0x0B
+#define REG_EXT_INTF_CONTROL			0x0C
+#define REG_POWER_CONTROL_1			0x10
+#define REG_POWER_CONTROL_2			0x11
+#define REG_POWER_CONTROL_3			0x12
+#define REG_POWER_CONTROL_4			0x13
+#define REG_RAM_ADDRESS_SET			0x21
+#define REG_WRITE_DATA_TO_GRAM			0x22
+#define REG_RAM_WRITE_MASK1			0x23
+#define REG_RAM_WRITE_MASK2			0x24
+#define REG_GAMMA_CONTROL_1			0x30
+#define REG_GAMMA_CONTROL_2			0x31
+#define REG_GAMMA_CONTROL_3			0x32
+#define REG_GAMMA_CONTROL_4			0x33
+#define REG_GAMMA_CONTROL_5			0x34
+#define REG_GAMMA_CONTROL_6			0x35
+#define REG_GAMMA_CONTROL_7			0x36
+#define REG_GAMMA_CONTROL_8			0x37
+#define REG_GAMMA_CONTROL_9			0x38
+#define REG_GAMMA_CONTROL_10			0x39
+#define REG_GATE_SCAN_CONTROL			0x40
+#define REG_VERT_SCROLL_CONTROL			0x41
+#define REG_FIRST_SCREEN_DRIVE_POS		0x42
+#define REG_SECOND_SCREEN_DRIVE_POS		0x43
+#define REG_RAM_ADDR_POS_H			0x44
+#define REG_RAM_ADDR_POS_V			0x45
+#define REG_OSCILLATOR_CONTROL			0x4F
+#define REG_GPIO				0x60
+#define REG_OTP_VCM_PROGRAMMING			0x61
+#define REG_OTP_VCM_STATUS_ENABLE		0x62
+#define REG_OTP_PROGRAMMING_ID_KEY		0x65
+
+/*
+ * maximum frequency for register access
+ * (not for the GRAM access)
+ */
+#define ILITEK_MAX_FREQ_REG	4000000
+
+/*
+ * Device ID as found in the datasheet (supports 9221 and 9222)
+ */
+#define ILITEK_DEVICE_ID	0x9220
+#define ILITEK_DEVICE_ID_MASK	0xFFF0
+
+/* Last two bits in the START BYTE */
+#define START_RS_INDEX		0
+#define START_RS_REG		1
+#define START_RW_WRITE		0
+#define START_RW_READ		1
+
+/**
+ * START_BYTE(id, rs, rw)
+ *
+ * Set the start byte according to the required operation.
+ * The start byte is defined as:
+ *   ----------------------------------
+ *  | 0 | 1 | 1 | 1 | 0 | ID | RS | RW |
+ *   ----------------------------------
+ * @id: display's id as set by the manufacturer
+ * @rs: operation type bit, one of:
+ *	  - START_RS_INDEX	set the index register
+ *	  - START_RS_REG	write/read registers/GRAM
+ * @rw: read/write operation
+ *	 - START_RW_WRITE	write
+ *	 - START_RW_READ	read
+ */
+#define START_BYTE(id, rs, rw)	\
+	(0x70 | (((id) & 0x01) << 2) | (((rs) & 0x01) << 1) | ((rw) & 0x01))
+
+/**
+ * CHECK_FREQ_REG(spi_device s, spi_transfer x) - Check the frequency
+ *	for the SPI transfer. According to the datasheet, the controller
+ *	accept higher frequency for the GRAM transfer, but it requires
+ *	lower frequency when the registers are read/written.
+ *	The macro sets the frequency in the spi_transfer structure if
+ *	the frequency exceeds the maximum value.
+ */
+#define CHECK_FREQ_REG(s, x)	\
+	do {			\
+		if (s->max_speed_hz > ILITEK_MAX_FREQ_REG)	\
+			((struct spi_transfer *)x)->speed_hz =	\
+					ILITEK_MAX_FREQ_REG;	\
+	} while (0)
+
+#define CMD_BUFSIZE		16
+
+#define POWER_IS_ON(pwr)	((pwr) <= FB_BLANK_NORMAL)
+
+#define set_tx_byte(b)		(tx_invert ? ~(b) : b)
+
+/**
+ * ili922x_id - id as set by manufacturer
+ */
+static int ili922x_id = 1;
+module_param(ili922x_id, int, 0);
+
+static int tx_invert;
+module_param(tx_invert, int, 0);
+
+/**
+ * driver's private structure
+ */
+struct ili922x {
+	struct spi_device *spi;
+	struct lcd_device *ld;
+	int power;
+};
+
+/**
+ * ili922x_read_status - read status register from display
+ * @spi: spi device
+ * @rs:  output value
+ */
+static int ili922x_read_status(struct spi_device *spi, u16 *rs)
+{
+	struct spi_message msg;
+	struct spi_transfer xfer;
+	unsigned char tbuf[CMD_BUFSIZE];
+	unsigned char rbuf[CMD_BUFSIZE];
+	int ret, i;
+
+	memset(&xfer, 0, sizeof(struct spi_transfer));
+	spi_message_init(&msg);
+	xfer.tx_buf = tbuf;
+	xfer.rx_buf = rbuf;
+	xfer.cs_change = 1;
+	CHECK_FREQ_REG(spi, &xfer);
+
+	tbuf[0] = set_tx_byte(START_BYTE(ili922x_id, START_RS_INDEX,
+					 START_RW_READ));
+	/*
+	 * we need 4-byte xfer here due to invalid dummy byte
+	 * received after start byte
+	 */
+	for (i = 1; i < 4; i++)
+		tbuf[i] = set_tx_byte(0);	/* dummy */
+
+	xfer.bits_per_word = 8;
+	xfer.len = 4;
+	spi_message_add_tail(&xfer, &msg);
+	ret = spi_sync(spi, &msg);
+	if (ret < 0) {
+		dev_dbg(&spi->dev, "Error sending SPI message 0x%x", ret);
+		return ret;
+	}
+
+	*rs = (rbuf[2] << 8) + rbuf[3];
+	return 0;
+}
+
+/**
+ * ili922x_read - read register from display
+ * @spi: spi device
+ * @reg: offset of the register to be read
+ * @rx:  output value
+ */
+static int ili922x_read(struct spi_device *spi, u8 reg, u16 *rx)
+{
+	struct spi_message msg;
+	struct spi_transfer xfer_regindex, xfer_regvalue;
+	unsigned char tbuf[CMD_BUFSIZE];
+	unsigned char rbuf[CMD_BUFSIZE];
+	int ret, len = 0, send_bytes;
+
+	memset(&xfer_regindex, 0, sizeof(struct spi_transfer));
+	memset(&xfer_regvalue, 0, sizeof(struct spi_transfer));
+	spi_message_init(&msg);
+	xfer_regindex.tx_buf = tbuf;
+	xfer_regindex.rx_buf = rbuf;
+	xfer_regindex.cs_change = 1;
+	CHECK_FREQ_REG(spi, &xfer_regindex);
+
+	tbuf[0] = set_tx_byte(START_BYTE(ili922x_id, START_RS_INDEX,
+					 START_RW_WRITE));
+	tbuf[1] = set_tx_byte(0);
+	tbuf[2] = set_tx_byte(reg);
+	xfer_regindex.bits_per_word = 8;
+	len = xfer_regindex.len = 3;
+	spi_message_add_tail(&xfer_regindex, &msg);
+
+	send_bytes = len;
+
+	tbuf[len++] = set_tx_byte(START_BYTE(ili922x_id, START_RS_REG,
+					     START_RW_READ));
+	tbuf[len++] = set_tx_byte(0);
+	tbuf[len] = set_tx_byte(0);
+
+	xfer_regvalue.cs_change = 1;
+	xfer_regvalue.len = 3;
+	xfer_regvalue.tx_buf = &tbuf[send_bytes];
+	xfer_regvalue.rx_buf = &rbuf[send_bytes];
+	CHECK_FREQ_REG(spi, &xfer_regvalue);
+
+	spi_message_add_tail(&xfer_regvalue, &msg);
+	ret = spi_sync(spi, &msg);
+	if (ret < 0) {
+		dev_dbg(&spi->dev, "Error sending SPI message 0x%x", ret);
+		return ret;
+	}
+
+	*rx = (rbuf[1 + send_bytes] << 8) + rbuf[2 + send_bytes];
+	return 0;
+}
+
+/**
+ * ili922x_write - write a controller register
+ * @spi: struct spi_device *
+ * @reg: offset of the register to be written
+ * @value: value to be written
+ */
+static int ili922x_write(struct spi_device *spi, u8 reg, u16 value)
+{
+	struct spi_message msg;
+	struct spi_transfer xfer_regindex, xfer_regvalue;
+	unsigned char tbuf[CMD_BUFSIZE];
+	unsigned char rbuf[CMD_BUFSIZE];
+	int ret, len = 0;
+
+	memset(&xfer_regindex, 0, sizeof(struct spi_transfer));
+	memset(&xfer_regvalue, 0, sizeof(struct spi_transfer));
+
+	spi_message_init(&msg);
+	xfer_regindex.tx_buf = tbuf;
+	xfer_regindex.rx_buf = rbuf;
+	xfer_regindex.cs_change = 1;
+	CHECK_FREQ_REG(spi, &xfer_regindex);
+
+	tbuf[0] = set_tx_byte(START_BYTE(ili922x_id, START_RS_INDEX,
+					 START_RW_WRITE));
+	tbuf[1] = set_tx_byte(0);
+	tbuf[2] = set_tx_byte(reg);
+	xfer_regindex.bits_per_word = 8;
+	xfer_regindex.len = 3;
+	spi_message_add_tail(&xfer_regindex, &msg);
+
+	ret = spi_sync(spi, &msg);
+
+	spi_message_init(&msg);
+	len = 0;
+	tbuf[0] = set_tx_byte(START_BYTE(ili922x_id, START_RS_REG,
+					 START_RW_WRITE));
+	tbuf[1] = set_tx_byte((value & 0xFF00) >> 8);
+	tbuf[2] = set_tx_byte(value & 0x00FF);
+
+	xfer_regvalue.cs_change = 1;
+	xfer_regvalue.len = 3;
+	xfer_regvalue.tx_buf = tbuf;
+	xfer_regvalue.rx_buf = rbuf;
+	CHECK_FREQ_REG(spi, &xfer_regvalue);
+
+	spi_message_add_tail(&xfer_regvalue, &msg);
+
+	ret = spi_sync(spi, &msg);
+	if (ret < 0) {
+		dev_err(&spi->dev, "Error sending SPI message 0x%x", ret);
+		return ret;
+	}
+	return 0;
+}
+
+#ifdef DEBUG
+/**
+ * ili922x_reg_dump - dump all registers
+ */
+static void ili922x_reg_dump(struct spi_device *spi)
+{
+	u8 reg;
+	u16 rx;
+
+	dev_dbg(&spi->dev, "ILI922x configuration registers:\n");
+	for (reg = REG_START_OSCILLATION;
+	     reg <= REG_OTP_PROGRAMMING_ID_KEY; reg++) {
+		ili922x_read(spi, reg, &rx);
+		dev_dbg(&spi->dev, "reg @ 0x%02X: 0x%04X\n", reg, rx);
+	}
+}
+#else
+static inline void ili922x_reg_dump(struct spi_device *spi) {}
+#endif
+
+/**
+ * set_write_to_gram_reg - initialize the display to write the GRAM
+ * @spi: spi device
+ */
+static void set_write_to_gram_reg(struct spi_device *spi)
+{
+	struct spi_message msg;
+	struct spi_transfer xfer;
+	unsigned char tbuf[CMD_BUFSIZE];
+
+	memset(&xfer, 0, sizeof(struct spi_transfer));
+
+	spi_message_init(&msg);
+	xfer.tx_buf = tbuf;
+	xfer.rx_buf = NULL;
+	xfer.cs_change = 1;
+
+	tbuf[0] = START_BYTE(ili922x_id, START_RS_INDEX, START_RW_WRITE);
+	tbuf[1] = 0;
+	tbuf[2] = REG_WRITE_DATA_TO_GRAM;
+
+	xfer.bits_per_word = 8;
+	xfer.len = 3;
+	spi_message_add_tail(&xfer, &msg);
+	spi_sync(spi, &msg);
+}
+
+/**
+ * ili922x_poweron - turn the display on
+ * @spi: spi device
+ *
+ * The sequence to turn on the display is taken from
+ * the datasheet and/or the example code provided by the
+ * manufacturer.
+ */
+static int ili922x_poweron(struct spi_device *spi)
+{
+	int ret;
+
+	/* Power on */
+	ret = ili922x_write(spi, REG_POWER_CONTROL_1, 0x0000);
+	usleep_range(10000, 10500);
+	ret += ili922x_write(spi, REG_POWER_CONTROL_2, 0x0000);
+	ret += ili922x_write(spi, REG_POWER_CONTROL_3, 0x0000);
+	msleep(40);
+	ret += ili922x_write(spi, REG_POWER_CONTROL_4, 0x0000);
+	msleep(40);
+	/* register 0x56 is not documented in the datasheet */
+	ret += ili922x_write(spi, 0x56, 0x080F);
+	ret += ili922x_write(spi, REG_POWER_CONTROL_1, 0x4240);
+	usleep_range(10000, 10500);
+	ret += ili922x_write(spi, REG_POWER_CONTROL_2, 0x0000);
+	ret += ili922x_write(spi, REG_POWER_CONTROL_3, 0x0014);
+	msleep(40);
+	ret += ili922x_write(spi, REG_POWER_CONTROL_4, 0x1319);
+	msleep(40);
+
+	return ret;
+}
+
+/**
+ * ili922x_poweroff - turn the display off
+ * @spi: spi device
+ */
+static int ili922x_poweroff(struct spi_device *spi)
+{
+	int ret;
+
+	/* Power off */
+	ret = ili922x_write(spi, REG_POWER_CONTROL_1, 0x0000);
+	usleep_range(10000, 10500);
+	ret += ili922x_write(spi, REG_POWER_CONTROL_2, 0x0000);
+	ret += ili922x_write(spi, REG_POWER_CONTROL_3, 0x0000);
+	msleep(40);
+	ret += ili922x_write(spi, REG_POWER_CONTROL_4, 0x0000);
+	msleep(40);
+
+	return ret;
+}
+
+/**
+ * ili922x_display_init - initialize the display by setting
+ *			  the configuration registers
+ * @spi: spi device
+ */
+static void ili922x_display_init(struct spi_device *spi)
+{
+	ili922x_write(spi, REG_START_OSCILLATION, 1);
+	usleep_range(10000, 10500);
+	ili922x_write(spi, REG_DRIVER_OUTPUT_CONTROL, 0x691B);
+	ili922x_write(spi, REG_LCD_AC_DRIVEING_CONTROL, 0x0700);
+	ili922x_write(spi, REG_ENTRY_MODE, 0x1030);
+	ili922x_write(spi, REG_COMPARE_1, 0x0000);
+	ili922x_write(spi, REG_COMPARE_2, 0x0000);
+	ili922x_write(spi, REG_DISPLAY_CONTROL_1, 0x0037);
+	ili922x_write(spi, REG_DISPLAY_CONTROL_2, 0x0202);
+	ili922x_write(spi, REG_DISPLAY_CONTROL_3, 0x0000);
+	ili922x_write(spi, REG_FRAME_CYCLE_CONTROL, 0x0000);
+
+	/* Set RGB interface */
+	ili922x_write(spi, REG_EXT_INTF_CONTROL, 0x0110);
+
+	ili922x_poweron(spi);
+
+	ili922x_write(spi, REG_GAMMA_CONTROL_1, 0x0302);
+	ili922x_write(spi, REG_GAMMA_CONTROL_2, 0x0407);
+	ili922x_write(spi, REG_GAMMA_CONTROL_3, 0x0304);
+	ili922x_write(spi, REG_GAMMA_CONTROL_4, 0x0203);
+	ili922x_write(spi, REG_GAMMA_CONTROL_5, 0x0706);
+	ili922x_write(spi, REG_GAMMA_CONTROL_6, 0x0407);
+	ili922x_write(spi, REG_GAMMA_CONTROL_7, 0x0706);
+	ili922x_write(spi, REG_GAMMA_CONTROL_8, 0x0000);
+	ili922x_write(spi, REG_GAMMA_CONTROL_9, 0x0C06);
+	ili922x_write(spi, REG_GAMMA_CONTROL_10, 0x0F00);
+	ili922x_write(spi, REG_RAM_ADDRESS_SET, 0x0000);
+	ili922x_write(spi, REG_GATE_SCAN_CONTROL, 0x0000);
+	ili922x_write(spi, REG_VERT_SCROLL_CONTROL, 0x0000);
+	ili922x_write(spi, REG_FIRST_SCREEN_DRIVE_POS, 0xDB00);
+	ili922x_write(spi, REG_SECOND_SCREEN_DRIVE_POS, 0xDB00);
+	ili922x_write(spi, REG_RAM_ADDR_POS_H, 0xAF00);
+	ili922x_write(spi, REG_RAM_ADDR_POS_V, 0xDB00);
+	ili922x_reg_dump(spi);
+	set_write_to_gram_reg(spi);
+}
+
+static int ili922x_lcd_power(struct ili922x *lcd, int power)
+{
+	int ret = 0;
+
+	if (POWER_IS_ON(power) && !POWER_IS_ON(lcd->power))
+		ret = ili922x_poweron(lcd->spi);
+	else if (!POWER_IS_ON(power) && POWER_IS_ON(lcd->power))
+		ret = ili922x_poweroff(lcd->spi);
+
+	if (!ret)
+		lcd->power = power;
+
+	return ret;
+}
+
+static int ili922x_set_power(struct lcd_device *ld, int power)
+{
+	struct ili922x *ili = lcd_get_data(ld);
+
+	return ili922x_lcd_power(ili, power);
+}
+
+static int ili922x_get_power(struct lcd_device *ld)
+{
+	struct ili922x *ili = lcd_get_data(ld);
+
+	return ili->power;
+}
+
+static struct lcd_ops ili922x_ops = {
+	.get_power = ili922x_get_power,
+	.set_power = ili922x_set_power,
+};
+
+static int ili922x_probe(struct spi_device *spi)
+{
+	struct ili922x *ili;
+	struct lcd_device *lcd;
+	int ret;
+	u16 reg = 0;
+
+	ili = devm_kzalloc(&spi->dev, sizeof(*ili), GFP_KERNEL);
+	if (!ili) {
+		dev_err(&spi->dev, "cannot alloc priv data\n");
+		return -ENOMEM;
+	}
+
+	ili->spi = spi;
+	spi_set_drvdata(spi, ili);
+
+	/* check if the device is connected */
+	ret = ili922x_read(spi, REG_DRIVER_CODE_READ, &reg);
+	if (ret || ((reg & ILITEK_DEVICE_ID_MASK) != ILITEK_DEVICE_ID)) {
+		dev_err(&spi->dev,
+			"no LCD found: Chip ID 0x%x, ret %d\n",
+			reg, ret);
+		return -ENODEV;
+	} else {
+		dev_info(&spi->dev, "ILI%x found, SPI freq %d, mode %d\n",
+			 reg, spi->max_speed_hz, spi->mode);
+	}
+
+	ret = ili922x_read_status(spi, &reg);
+	if (ret) {
+		dev_err(&spi->dev, "reading RS failed...\n");
+		return ret;
+	} else
+		dev_dbg(&spi->dev, "status: 0x%x\n", reg);
+
+	ili922x_display_init(spi);
+
+	ili->power = FB_BLANK_POWERDOWN;
+
+	lcd = lcd_device_register("ili922xlcd", &spi->dev, ili,
+				  &ili922x_ops);
+	if (IS_ERR(lcd)) {
+		dev_err(&spi->dev, "cannot register LCD\n");
+		return PTR_ERR(lcd);
+	}
+
+	ili->ld = lcd;
+	spi_set_drvdata(spi, ili);
+
+	ili922x_lcd_power(ili, FB_BLANK_UNBLANK);
+
+	return 0;
+}
+
+static int ili922x_remove(struct spi_device *spi)
+{
+	struct ili922x *ili = spi_get_drvdata(spi);
+
+	ili922x_poweroff(spi);
+	lcd_device_unregister(ili->ld);
+	return 0;
+}
+
+static struct spi_driver ili922x_driver = {
+	.driver = {
+		.name = "ili922x",
+		.owner = THIS_MODULE,
+	},
+	.probe = ili922x_probe,
+	.remove = ili922x_remove,
+};
+
+module_spi_driver(ili922x_driver);
+
+MODULE_AUTHOR("Stefano Babic <sbabic@denx.de>");
+MODULE_DESCRIPTION("ILI9221/9222 LCD driver");
+MODULE_LICENSE("GPL");
+MODULE_PARM_DESC(ili922x_id, "set controller identifier (default=1)");
+MODULE_PARM_DESC(tx_invert, "invert bytes before sending");
diff --git a/drivers/video/backlight/ili9320.c b/drivers/video/backlight/ili9320.c
index 1235bf9defc..f8be90c5ded 100644
--- a/drivers/video/backlight/ili9320.c
+++ b/drivers/video/backlight/ili9320.c
@@ -231,7 +231,7 @@ int ili9320_probe_spi(struct spi_device *spi,
 	ili->power = FB_BLANK_POWERDOWN;
 	ili->platdata = cfg;
 
-	dev_set_drvdata(&spi->dev, ili);
+	spi_set_drvdata(spi, ili);
 
 	ili9320_setup_spi(ili, spi);
 
@@ -270,27 +270,21 @@ int ili9320_remove(struct ili9320 *ili)
 }
 EXPORT_SYMBOL_GPL(ili9320_remove);
 
-#ifdef CONFIG_PM
-int ili9320_suspend(struct ili9320 *lcd, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+int ili9320_suspend(struct ili9320 *lcd)
 {
 	int ret;
 
-	dev_dbg(lcd->dev, "%s: event %d\n", __func__, state.event);
+	ret = ili9320_power(lcd, FB_BLANK_POWERDOWN);
 
-	if (state.event == PM_EVENT_SUSPEND) {
-		ret = ili9320_power(lcd, FB_BLANK_POWERDOWN);
-
-		if (lcd->platdata->suspend == ILI9320_SUSPEND_DEEP) {
-			ili9320_write(lcd, ILI9320_POWER1, lcd->power1 |
-				      ILI9320_POWER1_SLP |
-				      ILI9320_POWER1_DSTB);
-			lcd->initialised = 0;
-		}
-
-		return ret;
+	if (lcd->platdata->suspend == ILI9320_SUSPEND_DEEP) {
+		ili9320_write(lcd, ILI9320_POWER1, lcd->power1 |
+			      ILI9320_POWER1_SLP |
+			      ILI9320_POWER1_DSTB);
+		lcd->initialised = 0;
 	}
 
-	return 0;
+	return ret;
 }
 EXPORT_SYMBOL_GPL(ili9320_suspend);
 
diff --git a/drivers/video/backlight/ili9320.h b/drivers/video/backlight/ili9320.h
index e0db738f7bb..42329e7aa9a 100644
--- a/drivers/video/backlight/ili9320.h
+++ b/drivers/video/backlight/ili9320.h
@@ -76,5 +76,5 @@ extern void ili9320_shutdown(struct ili9320 *lcd);
 
 /* PM */
 
-extern int ili9320_suspend(struct ili9320 *lcd, pm_message_t state);
+extern int ili9320_suspend(struct ili9320 *lcd);
 extern int ili9320_resume(struct ili9320 *lcd);
diff --git a/drivers/video/backlight/jornada720_bl.c b/drivers/video/backlight/jornada720_bl.c
index fef6ce4fad7..3ccb89340f2 100644
--- a/drivers/video/backlight/jornada720_bl.c
+++ b/drivers/video/backlight/jornada720_bl.c
@@ -9,8 +9,6 @@
  *
  */
 
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
 #include <linux/backlight.h>
 #include <linux/device.h>
 #include <linux/fb.h>
@@ -40,11 +38,13 @@ static int jornada_bl_get_brightness(struct backlight_device *bd)
 	ret = jornada_ssp_byte(GETBRIGHTNESS);
 
 	if (jornada_ssp_byte(GETBRIGHTNESS) != TXDUMMY) {
-		pr_err("get brightness timeout\n");
+		dev_err(&bd->dev, "get brightness timeout\n");
 		jornada_ssp_end();
 		return -ETIMEDOUT;
-	} else /* exchange txdummy for value */
+	} else {
+		/* exchange txdummy for value */
 		ret = jornada_ssp_byte(TXDUMMY);
+	}
 
 	jornada_ssp_end();
 
@@ -61,7 +61,7 @@ static int jornada_bl_update_status(struct backlight_device *bd)
 	if ((bd->props.power != FB_BLANK_UNBLANK) || (bd->props.fb_blank != FB_BLANK_UNBLANK)) {
 		ret = jornada_ssp_byte(BRIGHTNESSOFF);
 		if (ret != TXDUMMY) {
-			pr_info("brightness off timeout\n");
+			dev_info(&bd->dev, "brightness off timeout\n");
 			/* turn off backlight */
 			PPSR &= ~PPC_LDD1;
 			PPDR |= PPC_LDD1;
@@ -72,7 +72,7 @@ static int jornada_bl_update_status(struct backlight_device *bd)
 
 		/* send command to our mcu */
 		if (jornada_ssp_byte(SETBRIGHTNESS) != TXDUMMY) {
-			pr_info("failed to set brightness\n");
+			dev_info(&bd->dev, "failed to set brightness\n");
 			ret = -ETIMEDOUT;
 			goto out;
 		}
@@ -86,7 +86,7 @@ static int jornada_bl_update_status(struct backlight_device *bd)
 		 */
 		if (jornada_ssp_byte(BL_MAX_BRIGHT - bd->props.brightness)
 			!= TXDUMMY) {
-			pr_err("set brightness failed\n");
+			dev_err(&bd->dev, "set brightness failed\n");
 			ret = -ETIMEDOUT;
 		}
 
@@ -120,7 +120,7 @@ static int jornada_bl_probe(struct platform_device *pdev)
 
 	if (IS_ERR(bd)) {
 		ret = PTR_ERR(bd);
-		pr_err("failed to register device, err=%x\n", ret);
+		dev_err(&pdev->dev, "failed to register device, err=%x\n", ret);
 		return ret;
 	}
 
@@ -134,7 +134,7 @@ static int jornada_bl_probe(struct platform_device *pdev)
 	jornada_bl_update_status(bd);
 
 	platform_set_drvdata(pdev, bd);
-	pr_info("HP Jornada 700 series backlight driver\n");
+	dev_info(&pdev->dev, "HP Jornada 700 series backlight driver\n");
 
 	return 0;
 }
diff --git a/drivers/video/backlight/jornada720_lcd.c b/drivers/video/backlight/jornada720_lcd.c
index 635b30523fd..b061413f1a6 100644
--- a/drivers/video/backlight/jornada720_lcd.c
+++ b/drivers/video/backlight/jornada720_lcd.c
@@ -9,8 +9,6 @@
  *
  */
 
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
 #include <linux/device.h>
 #include <linux/fb.h>
 #include <linux/kernel.h>
@@ -27,7 +25,7 @@
 #define LCD_MAX_CONTRAST	0xff
 #define LCD_DEF_CONTRAST	0x80
 
-static int jornada_lcd_get_power(struct lcd_device *dev)
+static int jornada_lcd_get_power(struct lcd_device *ld)
 {
 	/* LDD2 in PPC = LCD POWER */
 	if (PPSR & PPC_LDD2)
@@ -36,17 +34,17 @@ static int jornada_lcd_get_power(struct lcd_device *dev)
 		return FB_BLANK_POWERDOWN;	/* PW OFF */
 }
 
-static int jornada_lcd_get_contrast(struct lcd_device *dev)
+static int jornada_lcd_get_contrast(struct lcd_device *ld)
 {
 	int ret;
 
-	if (jornada_lcd_get_power(dev) != FB_BLANK_UNBLANK)
+	if (jornada_lcd_get_power(ld) != FB_BLANK_UNBLANK)
 		return 0;
 
 	jornada_ssp_start();
 
 	if (jornada_ssp_byte(GETCONTRAST) != TXDUMMY) {
-		pr_err("get contrast failed\n");
+		dev_err(&ld->dev, "get contrast failed\n");
 		jornada_ssp_end();
 		return -ETIMEDOUT;
 	} else {
@@ -56,7 +54,7 @@ static int jornada_lcd_get_contrast(struct lcd_device *dev)
 	}
 }
 
-static int jornada_lcd_set_contrast(struct lcd_device *dev, int value)
+static int jornada_lcd_set_contrast(struct lcd_device *ld, int value)
 {
 	int ret;
 
@@ -67,7 +65,7 @@ static int jornada_lcd_set_contrast(struct lcd_device *dev, int value)
 
 	/* push the new value */
 	if (jornada_ssp_byte(value) != TXDUMMY) {
-		pr_err("set contrast failed\n");
+		dev_err(&ld->dev, "set contrast failed\n");
 		jornada_ssp_end();
 		return -ETIMEDOUT;
 	}
@@ -78,13 +76,14 @@ static int jornada_lcd_set_contrast(struct lcd_device *dev, int value)
 	return 0;
 }
 
-static int jornada_lcd_set_power(struct lcd_device *dev, int power)
+static int jornada_lcd_set_power(struct lcd_device *ld, int power)
 {
 	if (power != FB_BLANK_UNBLANK) {
 		PPSR &= ~PPC_LDD2;
 		PPDR |= PPC_LDD2;
-	} else
+	} else {
 		PPSR |= PPC_LDD2;
+	}
 
 	return 0;
 }
@@ -105,7 +104,7 @@ static int jornada_lcd_probe(struct platform_device *pdev)
 
 	if (IS_ERR(lcd_device)) {
 		ret = PTR_ERR(lcd_device);
-		pr_err("failed to register device\n");
+		dev_err(&pdev->dev, "failed to register device\n");
 		return ret;
 	}
 
diff --git a/drivers/video/backlight/kb3886_bl.c b/drivers/video/backlight/kb3886_bl.c
index 6c5ed6b242c..bca6ccc74df 100644
--- a/drivers/video/backlight/kb3886_bl.c
+++ b/drivers/video/backlight/kb3886_bl.c
@@ -106,29 +106,28 @@ static int kb3886bl_send_intensity(struct backlight_device *bd)
 	return 0;
 }
 
-#ifdef CONFIG_PM
-static int kb3886bl_suspend(struct platform_device *pdev, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int kb3886bl_suspend(struct device *dev)
 {
-	struct backlight_device *bd = platform_get_drvdata(pdev);
+	struct backlight_device *bd = dev_get_drvdata(dev);
 
 	kb3886bl_flags |= KB3886BL_SUSPENDED;
 	backlight_update_status(bd);
 	return 0;
 }
 
-static int kb3886bl_resume(struct platform_device *pdev)
+static int kb3886bl_resume(struct device *dev)
 {
-	struct backlight_device *bd = platform_get_drvdata(pdev);
+	struct backlight_device *bd = dev_get_drvdata(dev);
 
 	kb3886bl_flags &= ~KB3886BL_SUSPENDED;
 	backlight_update_status(bd);
 	return 0;
 }
-#else
-#define kb3886bl_suspend	NULL
-#define kb3886bl_resume		NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(kb3886bl_pm_ops, kb3886bl_suspend, kb3886bl_resume);
+
 static int kb3886bl_get_intensity(struct backlight_device *bd)
 {
 	return kb3886bl_intensity;
@@ -179,10 +178,9 @@ static int kb3886bl_remove(struct platform_device *pdev)
 static struct platform_driver kb3886bl_driver = {
 	.probe		= kb3886bl_probe,
 	.remove		= kb3886bl_remove,
-	.suspend	= kb3886bl_suspend,
-	.resume		= kb3886bl_resume,
 	.driver		= {
 		.name	= "kb3886-bl",
+		.pm	= &kb3886bl_pm_ops,
 	},
 };
 
diff --git a/drivers/video/backlight/l4f00242t03.c b/drivers/video/backlight/l4f00242t03.c
index fb615577132..a35a38c709c 100644
--- a/drivers/video/backlight/l4f00242t03.c
+++ b/drivers/video/backlight/l4f00242t03.c
@@ -51,14 +51,33 @@ static void l4f00242t03_lcd_init(struct spi_device *spi)
 	struct l4f00242t03_pdata *pdata = spi->dev.platform_data;
 	struct l4f00242t03_priv *priv = spi_get_drvdata(spi);
 	const u16 cmd[] = { 0x36, param(0), 0x3A, param(0x60) };
+	int ret;
 
 	dev_dbg(&spi->dev, "initializing LCD\n");
 
-	regulator_set_voltage(priv->io_reg, 1800000, 1800000);
-	regulator_enable(priv->io_reg);
+	ret = regulator_set_voltage(priv->io_reg, 1800000, 1800000);
+	if (ret) {
+		dev_err(&spi->dev, "failed to set the IO regulator voltage.\n");
+		return;
+	}
+	ret = regulator_enable(priv->io_reg);
+	if (ret) {
+		dev_err(&spi->dev, "failed to enable the IO regulator.\n");
+		return;
+	}
 
-	regulator_set_voltage(priv->core_reg, 2800000, 2800000);
-	regulator_enable(priv->core_reg);
+	ret = regulator_set_voltage(priv->core_reg, 2800000, 2800000);
+	if (ret) {
+		dev_err(&spi->dev, "failed to set the core regulator voltage.\n");
+		regulator_disable(priv->io_reg);
+		return;
+	}
+	ret = regulator_enable(priv->core_reg);
+	if (ret) {
+		dev_err(&spi->dev, "failed to enable the core regulator.\n");
+		regulator_disable(priv->io_reg);
+		return;
+	}
 
 	l4f00242t03_reset(pdata->reset_gpio);
 
diff --git a/drivers/video/backlight/ld9040.c b/drivers/video/backlight/ld9040.c
index 1b642f5f381..1e0a3093ce5 100644
--- a/drivers/video/backlight/ld9040.c
+++ b/drivers/video/backlight/ld9040.c
@@ -775,12 +775,12 @@ static int ld9040_remove(struct spi_device *spi)
 	return 0;
 }
 
-#if defined(CONFIG_PM)
-static int ld9040_suspend(struct spi_device *spi, pm_message_t mesg)
+#ifdef CONFIG_PM_SLEEP
+static int ld9040_suspend(struct device *dev)
 {
-	struct ld9040 *lcd = spi_get_drvdata(spi);
+	struct ld9040 *lcd = dev_get_drvdata(dev);
 
-	dev_dbg(&spi->dev, "lcd->power = %d\n", lcd->power);
+	dev_dbg(dev, "lcd->power = %d\n", lcd->power);
 
 	/*
 	 * when lcd panel is suspend, lcd panel becomes off
@@ -789,19 +789,18 @@ static int ld9040_suspend(struct spi_device *spi, pm_message_t mesg)
 	return ld9040_power(lcd, FB_BLANK_POWERDOWN);
 }
 
-static int ld9040_resume(struct spi_device *spi)
+static int ld9040_resume(struct device *dev)
 {
-	struct ld9040 *lcd = spi_get_drvdata(spi);
+	struct ld9040 *lcd = dev_get_drvdata(dev);
 
 	lcd->power = FB_BLANK_POWERDOWN;
 
 	return ld9040_power(lcd, FB_BLANK_UNBLANK);
 }
-#else
-#define ld9040_suspend		NULL
-#define ld9040_resume		NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(ld9040_pm_ops, ld9040_suspend, ld9040_resume);
+
 /* Power down all displays on reboot, poweroff or halt. */
 static void ld9040_shutdown(struct spi_device *spi)
 {
@@ -814,12 +813,11 @@ static struct spi_driver ld9040_driver = {
 	.driver = {
 		.name	= "ld9040",
 		.owner	= THIS_MODULE,
+		.pm	= &ld9040_pm_ops,
 	},
 	.probe		= ld9040_probe,
 	.remove		= ld9040_remove,
 	.shutdown	= ld9040_shutdown,
-	.suspend	= ld9040_suspend,
-	.resume		= ld9040_resume,
 };
 
 module_spi_driver(ld9040_driver);
diff --git a/drivers/video/backlight/lm3533_bl.c b/drivers/video/backlight/lm3533_bl.c
index 5d18d4d7f47..1d1dbfb789e 100644
--- a/drivers/video/backlight/lm3533_bl.c
+++ b/drivers/video/backlight/lm3533_bl.c
@@ -368,29 +368,28 @@ static int lm3533_bl_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
-static int lm3533_bl_suspend(struct platform_device *pdev, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int lm3533_bl_suspend(struct device *dev)
 {
-	struct lm3533_bl *bl = platform_get_drvdata(pdev);
+	struct lm3533_bl *bl = dev_get_drvdata(dev);
 
-	dev_dbg(&pdev->dev, "%s\n", __func__);
+	dev_dbg(dev, "%s\n", __func__);
 
 	return lm3533_ctrlbank_disable(&bl->cb);
 }
 
-static int lm3533_bl_resume(struct platform_device *pdev)
+static int lm3533_bl_resume(struct device *dev)
 {
-	struct lm3533_bl *bl = platform_get_drvdata(pdev);
+	struct lm3533_bl *bl = dev_get_drvdata(dev);
 
-	dev_dbg(&pdev->dev, "%s\n", __func__);
+	dev_dbg(dev, "%s\n", __func__);
 
 	return lm3533_ctrlbank_enable(&bl->cb);
 }
-#else
-#define lm3533_bl_suspend	NULL
-#define lm3533_bl_resume	NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(lm3533_bl_pm_ops, lm3533_bl_suspend, lm3533_bl_resume);
+
 static void lm3533_bl_shutdown(struct platform_device *pdev)
 {
 	struct lm3533_bl *bl = platform_get_drvdata(pdev);
@@ -404,12 +403,11 @@ static struct platform_driver lm3533_bl_driver = {
 	.driver = {
 		.name	= "lm3533-backlight",
 		.owner	= THIS_MODULE,
+		.pm	= &lm3533_bl_pm_ops,
 	},
 	.probe		= lm3533_bl_probe,
 	.remove		= lm3533_bl_remove,
 	.shutdown	= lm3533_bl_shutdown,
-	.suspend	= lm3533_bl_suspend,
-	.resume		= lm3533_bl_resume,
 };
 module_platform_driver(lm3533_bl_driver);
 
diff --git a/drivers/video/backlight/lms501kf03.c b/drivers/video/backlight/lms501kf03.c
index b43882abefa..cf01b9ac813 100644
--- a/drivers/video/backlight/lms501kf03.c
+++ b/drivers/video/backlight/lms501kf03.c
@@ -387,13 +387,12 @@ static int lms501kf03_remove(struct spi_device *spi)
 	return 0;
 }
 
-#if defined(CONFIG_PM)
-
-static int lms501kf03_suspend(struct spi_device *spi, pm_message_t mesg)
+#ifdef CONFIG_PM_SLEEP
+static int lms501kf03_suspend(struct device *dev)
 {
-	struct lms501kf03 *lcd = spi_get_drvdata(spi);
+	struct lms501kf03 *lcd = dev_get_drvdata(dev);
 
-	dev_dbg(&spi->dev, "lcd->power = %d\n", lcd->power);
+	dev_dbg(dev, "lcd->power = %d\n", lcd->power);
 
 	/*
 	 * when lcd panel is suspend, lcd panel becomes off
@@ -402,19 +401,19 @@ static int lms501kf03_suspend(struct spi_device *spi, pm_message_t mesg)
 	return lms501kf03_power(lcd, FB_BLANK_POWERDOWN);
 }
 
-static int lms501kf03_resume(struct spi_device *spi)
+static int lms501kf03_resume(struct device *dev)
 {
-	struct lms501kf03 *lcd = spi_get_drvdata(spi);
+	struct lms501kf03 *lcd = dev_get_drvdata(dev);
 
 	lcd->power = FB_BLANK_POWERDOWN;
 
 	return lms501kf03_power(lcd, FB_BLANK_UNBLANK);
 }
-#else
-#define lms501kf03_suspend	NULL
-#define lms501kf03_resume	NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(lms501kf03_pm_ops, lms501kf03_suspend,
+			lms501kf03_resume);
+
 static void lms501kf03_shutdown(struct spi_device *spi)
 {
 	struct lms501kf03 *lcd = spi_get_drvdata(spi);
@@ -426,12 +425,11 @@ static struct spi_driver lms501kf03_driver = {
 	.driver = {
 		.name	= "lms501kf03",
 		.owner	= THIS_MODULE,
+		.pm	= &lms501kf03_pm_ops,
 	},
 	.probe		= lms501kf03_probe,
 	.remove		= lms501kf03_remove,
 	.shutdown	= lms501kf03_shutdown,
-	.suspend	= lms501kf03_suspend,
-	.resume		= lms501kf03_resume,
 };
 
 module_spi_driver(lms501kf03_driver);
diff --git a/drivers/video/backlight/locomolcd.c b/drivers/video/backlight/locomolcd.c
index 146fea8aa43..6c3ec4259a6 100644
--- a/drivers/video/backlight/locomolcd.c
+++ b/drivers/video/backlight/locomolcd.c
@@ -157,25 +157,24 @@ static const struct backlight_ops locomobl_data = {
 	.update_status  = locomolcd_set_intensity,
 };
 
-#ifdef CONFIG_PM
-static int locomolcd_suspend(struct locomo_dev *dev, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int locomolcd_suspend(struct device *dev)
 {
 	locomolcd_flags |= LOCOMOLCD_SUSPENDED;
 	locomolcd_set_intensity(locomolcd_bl_device);
 	return 0;
 }
 
-static int locomolcd_resume(struct locomo_dev *dev)
+static int locomolcd_resume(struct device *dev)
 {
 	locomolcd_flags &= ~LOCOMOLCD_SUSPENDED;
 	locomolcd_set_intensity(locomolcd_bl_device);
 	return 0;
 }
-#else
-#define locomolcd_suspend	NULL
-#define locomolcd_resume	NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(locomolcd_pm_ops, locomolcd_suspend, locomolcd_resume);
+
 static int locomolcd_probe(struct locomo_dev *ldev)
 {
 	struct backlight_properties props;
@@ -230,13 +229,12 @@ static int locomolcd_remove(struct locomo_dev *dev)
 
 static struct locomo_driver poodle_lcd_driver = {
 	.drv = {
-		.name = "locomo-backlight",
+		.name	= "locomo-backlight",
+		.pm	= &locomolcd_pm_ops,
 	},
 	.devid	= LOCOMO_DEVID_BACKLIGHT,
 	.probe	= locomolcd_probe,
 	.remove	= locomolcd_remove,
-	.suspend = locomolcd_suspend,
-	.resume = locomolcd_resume,
 };
 
 static int __init locomolcd_init(void)
diff --git a/drivers/video/backlight/lp855x_bl.c b/drivers/video/backlight/lp855x_bl.c
index 7ae9ae6f465..a0e1e02bdc2 100644
--- a/drivers/video/backlight/lp855x_bl.c
+++ b/drivers/video/backlight/lp855x_bl.c
@@ -14,6 +14,7 @@
 #include <linux/i2c.h>
 #include <linux/backlight.h>
 #include <linux/err.h>
+#include <linux/of.h>
 #include <linux/platform_data/lp855x.h>
 #include <linux/pwm.h>
 
@@ -35,10 +36,14 @@
 #define LP8557_EPROM_START		0x10
 #define LP8557_EPROM_END		0x1E
 
-#define BUF_SIZE		20
 #define DEFAULT_BL_NAME		"lcd-backlight"
 #define MAX_BRIGHTNESS		255
 
+enum lp855x_brightness_ctrl_mode {
+	PWM_BASED = 1,
+	REGISTER_BASED,
+};
+
 struct lp855x;
 
 /*
@@ -58,6 +63,7 @@ struct lp855x_device_config {
 struct lp855x {
 	const char *chipname;
 	enum lp855x_chip_id chip_id;
+	enum lp855x_brightness_ctrl_mode mode;
 	struct lp855x_device_config *cfg;
 	struct i2c_client *client;
 	struct backlight_device *bl;
@@ -187,7 +193,7 @@ static int lp855x_configure(struct lp855x *lp)
 	if (ret)
 		goto err;
 
-	if (pd->load_new_rom_data && pd->size_program) {
+	if (pd->size_program > 0) {
 		for (i = 0; i < pd->size_program; i++) {
 			addr = pd->rom_data[i].addr;
 			val = pd->rom_data[i].val;
@@ -239,18 +245,17 @@ static void lp855x_pwm_ctrl(struct lp855x *lp, int br, int max_br)
 static int lp855x_bl_update_status(struct backlight_device *bl)
 {
 	struct lp855x *lp = bl_get_data(bl);
-	enum lp855x_brightness_ctrl_mode mode = lp->pdata->mode;
 
 	if (bl->props.state & BL_CORE_SUSPENDED)
 		bl->props.brightness = 0;
 
-	if (mode == PWM_BASED) {
+	if (lp->mode == PWM_BASED) {
 		int br = bl->props.brightness;
 		int max_br = bl->props.max_brightness;
 
 		lp855x_pwm_ctrl(lp, br, max_br);
 
-	} else if (mode == REGISTER_BASED) {
+	} else if (lp->mode == REGISTER_BASED) {
 		u8 val = bl->props.brightness;
 		lp855x_write_byte(lp, lp->cfg->reg_brightness, val);
 	}
@@ -274,7 +279,7 @@ static int lp855x_backlight_register(struct lp855x *lp)
 	struct backlight_device *bl;
 	struct backlight_properties props;
 	struct lp855x_platform_data *pdata = lp->pdata;
-	char *name = pdata->name ? : DEFAULT_BL_NAME;
+	const char *name = pdata->name ? : DEFAULT_BL_NAME;
 
 	props.type = BACKLIGHT_PLATFORM;
 	props.max_brightness = MAX_BRIGHTNESS;
@@ -304,22 +309,21 @@ static ssize_t lp855x_get_chip_id(struct device *dev,
 				struct device_attribute *attr, char *buf)
 {
 	struct lp855x *lp = dev_get_drvdata(dev);
-	return scnprintf(buf, BUF_SIZE, "%s\n", lp->chipname);
+	return scnprintf(buf, PAGE_SIZE, "%s\n", lp->chipname);
 }
 
 static ssize_t lp855x_get_bl_ctl_mode(struct device *dev,
 				     struct device_attribute *attr, char *buf)
 {
 	struct lp855x *lp = dev_get_drvdata(dev);
-	enum lp855x_brightness_ctrl_mode mode = lp->pdata->mode;
 	char *strmode = NULL;
 
-	if (mode == PWM_BASED)
+	if (lp->mode == PWM_BASED)
 		strmode = "pwm based";
-	else if (mode == REGISTER_BASED)
+	else if (lp->mode == REGISTER_BASED)
 		strmode = "register based";
 
-	return scnprintf(buf, BUF_SIZE, "%s\n", strmode);
+	return scnprintf(buf, PAGE_SIZE, "%s\n", strmode);
 }
 
 static DEVICE_ATTR(chip_id, S_IRUGO, lp855x_get_chip_id, NULL);
@@ -335,16 +339,71 @@ static const struct attribute_group lp855x_attr_group = {
 	.attrs = lp855x_attributes,
 };
 
+#ifdef CONFIG_OF
+static int lp855x_parse_dt(struct device *dev, struct device_node *node)
+{
+	struct lp855x_platform_data *pdata;
+	int rom_length;
+
+	if (!node) {
+		dev_err(dev, "no platform data\n");
+		return -EINVAL;
+	}
+
+	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return -ENOMEM;
+
+	of_property_read_string(node, "bl-name", &pdata->name);
+	of_property_read_u8(node, "dev-ctrl", &pdata->device_control);
+	of_property_read_u8(node, "init-brt", &pdata->initial_brightness);
+	of_property_read_u32(node, "pwm-period", &pdata->period_ns);
+
+	/* Fill ROM platform data if defined */
+	rom_length = of_get_child_count(node);
+	if (rom_length > 0) {
+		struct lp855x_rom_data *rom;
+		struct device_node *child;
+		int i = 0;
+
+		rom = devm_kzalloc(dev, sizeof(*rom) * rom_length, GFP_KERNEL);
+		if (!rom)
+			return -ENOMEM;
+
+		for_each_child_of_node(node, child) {
+			of_property_read_u8(child, "rom-addr", &rom[i].addr);
+			of_property_read_u8(child, "rom-val", &rom[i].val);
+			i++;
+		}
+
+		pdata->size_program = rom_length;
+		pdata->rom_data = &rom[0];
+	}
+
+	dev->platform_data = pdata;
+
+	return 0;
+}
+#else
+static int lp855x_parse_dt(struct device *dev, struct device_node *node)
+{
+	return -EINVAL;
+}
+#endif
+
 static int lp855x_probe(struct i2c_client *cl, const struct i2c_device_id *id)
 {
 	struct lp855x *lp;
 	struct lp855x_platform_data *pdata = cl->dev.platform_data;
-	enum lp855x_brightness_ctrl_mode mode;
+	struct device_node *node = cl->dev.of_node;
 	int ret;
 
 	if (!pdata) {
-		dev_err(&cl->dev, "no platform data supplied\n");
-		return -EINVAL;
+		ret = lp855x_parse_dt(&cl->dev, node);
+		if (ret < 0)
+			return ret;
+
+		pdata = cl->dev.platform_data;
 	}
 
 	if (!i2c_check_functionality(cl->adapter, I2C_FUNC_SMBUS_I2C_BLOCK))
@@ -354,7 +413,11 @@ static int lp855x_probe(struct i2c_client *cl, const struct i2c_device_id *id)
 	if (!lp)
 		return -ENOMEM;
 
-	mode = pdata->mode;
+	if (pdata->period_ns > 0)
+		lp->mode = PWM_BASED;
+	else
+		lp->mode = REGISTER_BASED;
+
 	lp->client = cl;
 	lp->dev = &cl->dev;
 	lp->pdata = pdata;
@@ -402,6 +465,17 @@ static int lp855x_remove(struct i2c_client *cl)
 	return 0;
 }
 
+static const struct of_device_id lp855x_dt_ids[] = {
+	{ .compatible = "ti,lp8550", },
+	{ .compatible = "ti,lp8551", },
+	{ .compatible = "ti,lp8552", },
+	{ .compatible = "ti,lp8553", },
+	{ .compatible = "ti,lp8556", },
+	{ .compatible = "ti,lp8557", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, lp855x_dt_ids);
+
 static const struct i2c_device_id lp855x_ids[] = {
 	{"lp8550", LP8550},
 	{"lp8551", LP8551},
@@ -416,6 +490,7 @@ MODULE_DEVICE_TABLE(i2c, lp855x_ids);
 static struct i2c_driver lp855x_driver = {
 	.driver = {
 		   .name = "lp855x",
+		   .of_match_table = of_match_ptr(lp855x_dt_ids),
 		   },
 	.probe = lp855x_probe,
 	.remove = lp855x_remove,
diff --git a/drivers/video/backlight/ltv350qv.c b/drivers/video/backlight/ltv350qv.c
index c0b4b8f2de9..ed1b3926813 100644
--- a/drivers/video/backlight/ltv350qv.c
+++ b/drivers/video/backlight/ltv350qv.c
@@ -271,25 +271,24 @@ static int ltv350qv_remove(struct spi_device *spi)
 	return 0;
 }
 
-#ifdef CONFIG_PM
-static int ltv350qv_suspend(struct spi_device *spi, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int ltv350qv_suspend(struct device *dev)
 {
-	struct ltv350qv *lcd = spi_get_drvdata(spi);
+	struct ltv350qv *lcd = dev_get_drvdata(dev);
 
 	return ltv350qv_power(lcd, FB_BLANK_POWERDOWN);
 }
 
-static int ltv350qv_resume(struct spi_device *spi)
+static int ltv350qv_resume(struct device *dev)
 {
-	struct ltv350qv *lcd = spi_get_drvdata(spi);
+	struct ltv350qv *lcd = dev_get_drvdata(dev);
 
 	return ltv350qv_power(lcd, FB_BLANK_UNBLANK);
 }
-#else
-#define ltv350qv_suspend	NULL
-#define ltv350qv_resume		NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(ltv350qv_pm_ops, ltv350qv_suspend, ltv350qv_resume);
+
 /* Power down all displays on reboot, poweroff or halt */
 static void ltv350qv_shutdown(struct spi_device *spi)
 {
@@ -302,13 +301,12 @@ static struct spi_driver ltv350qv_driver = {
 	.driver = {
 		.name		= "ltv350qv",
 		.owner		= THIS_MODULE,
+		.pm		= &ltv350qv_pm_ops,
 	},
 
 	.probe		= ltv350qv_probe,
 	.remove		= ltv350qv_remove,
 	.shutdown	= ltv350qv_shutdown,
-	.suspend	= ltv350qv_suspend,
-	.resume		= ltv350qv_resume,
 };
 
 module_spi_driver(ltv350qv_driver);
diff --git a/drivers/video/backlight/omap1_bl.c b/drivers/video/backlight/omap1_bl.c
index 62711016306..812e22e35ca 100644
--- a/drivers/video/backlight/omap1_bl.c
+++ b/drivers/video/backlight/omap1_bl.c
@@ -18,8 +18,6 @@
  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  */
 
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
@@ -73,27 +71,24 @@ static void omapbl_blank(struct omap_backlight *bl, int mode)
 	}
 }
 
-#ifdef CONFIG_PM
-static int omapbl_suspend(struct platform_device *pdev, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int omapbl_suspend(struct device *dev)
 {
-	struct backlight_device *dev = platform_get_drvdata(pdev);
-	struct omap_backlight *bl = bl_get_data(dev);
+	struct backlight_device *bl_dev = dev_get_drvdata(dev);
+	struct omap_backlight *bl = bl_get_data(bl_dev);
 
 	omapbl_blank(bl, FB_BLANK_POWERDOWN);
 	return 0;
 }
 
-static int omapbl_resume(struct platform_device *pdev)
+static int omapbl_resume(struct device *dev)
 {
-	struct backlight_device *dev = platform_get_drvdata(pdev);
-	struct omap_backlight *bl = bl_get_data(dev);
+	struct backlight_device *bl_dev = dev_get_drvdata(dev);
+	struct omap_backlight *bl = bl_get_data(bl_dev);
 
 	omapbl_blank(bl, bl->powermode);
 	return 0;
 }
-#else
-#define omapbl_suspend	NULL
-#define omapbl_resume	NULL
 #endif
 
 static int omapbl_set_power(struct backlight_device *dev, int state)
@@ -170,7 +165,7 @@ static int omapbl_probe(struct platform_device *pdev)
 	dev->props.brightness = pdata->default_intensity;
 	omapbl_update_status(dev);
 
-	pr_info("OMAP LCD backlight initialised\n");
+	dev_info(&pdev->dev, "OMAP LCD backlight initialised\n");
 
 	return 0;
 }
@@ -184,13 +179,14 @@ static int omapbl_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static SIMPLE_DEV_PM_OPS(omapbl_pm_ops, omapbl_suspend, omapbl_resume);
+
 static struct platform_driver omapbl_driver = {
 	.probe		= omapbl_probe,
 	.remove		= omapbl_remove,
-	.suspend	= omapbl_suspend,
-	.resume		= omapbl_resume,
 	.driver		= {
 		.name	= "omap-bl",
+		.pm	= &omapbl_pm_ops,
 	},
 };
 
diff --git a/drivers/video/backlight/platform_lcd.c b/drivers/video/backlight/platform_lcd.c
index 17a6b83f97a..05683670670 100644
--- a/drivers/video/backlight/platform_lcd.c
+++ b/drivers/video/backlight/platform_lcd.c
@@ -86,6 +86,12 @@ static int platform_lcd_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
+	if (pdata->probe) {
+		err = pdata->probe(pdata);
+		if (err)
+			return err;
+	}
+
 	plcd = devm_kzalloc(&pdev->dev, sizeof(struct platform_lcd),
 			    GFP_KERNEL);
 	if (!plcd) {
@@ -121,7 +127,7 @@ static int platform_lcd_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int platform_lcd_suspend(struct device *dev)
 {
 	struct platform_lcd *plcd = dev_get_drvdata(dev);
@@ -141,10 +147,10 @@ static int platform_lcd_resume(struct device *dev)
 
 	return 0;
 }
+#endif
 
 static SIMPLE_DEV_PM_OPS(platform_lcd_pm_ops, platform_lcd_suspend,
 			platform_lcd_resume);
-#endif
 
 #ifdef CONFIG_OF
 static const struct of_device_id platform_lcd_of_match[] = {
@@ -158,9 +164,7 @@ static struct platform_driver platform_lcd_driver = {
 	.driver		= {
 		.name	= "platform-lcd",
 		.owner	= THIS_MODULE,
-#ifdef CONFIG_PM
 		.pm	= &platform_lcd_pm_ops,
-#endif
 		.of_match_table = of_match_ptr(platform_lcd_of_match),
 	},
 	.probe		= platform_lcd_probe,
diff --git a/drivers/video/backlight/s6e63m0.c b/drivers/video/backlight/s6e63m0.c
index 9c2677f0ef7..b37bb1854bf 100644
--- a/drivers/video/backlight/s6e63m0.c
+++ b/drivers/video/backlight/s6e63m0.c
@@ -817,12 +817,12 @@ static int s6e63m0_remove(struct spi_device *spi)
 	return 0;
 }
 
-#if defined(CONFIG_PM)
-static int s6e63m0_suspend(struct spi_device *spi, pm_message_t mesg)
+#ifdef CONFIG_PM_SLEEP
+static int s6e63m0_suspend(struct device *dev)
 {
-	struct s6e63m0 *lcd = spi_get_drvdata(spi);
+	struct s6e63m0 *lcd = dev_get_drvdata(dev);
 
-	dev_dbg(&spi->dev, "lcd->power = %d\n", lcd->power);
+	dev_dbg(dev, "lcd->power = %d\n", lcd->power);
 
 	/*
 	 * when lcd panel is suspend, lcd panel becomes off
@@ -831,19 +831,18 @@ static int s6e63m0_suspend(struct spi_device *spi, pm_message_t mesg)
 	return s6e63m0_power(lcd, FB_BLANK_POWERDOWN);
 }
 
-static int s6e63m0_resume(struct spi_device *spi)
+static int s6e63m0_resume(struct device *dev)
 {
-	struct s6e63m0 *lcd = spi_get_drvdata(spi);
+	struct s6e63m0 *lcd = dev_get_drvdata(dev);
 
 	lcd->power = FB_BLANK_POWERDOWN;
 
 	return s6e63m0_power(lcd, FB_BLANK_UNBLANK);
 }
-#else
-#define s6e63m0_suspend		NULL
-#define s6e63m0_resume		NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(s6e63m0_pm_ops, s6e63m0_suspend, s6e63m0_resume);
+
 /* Power down all displays on reboot, poweroff or halt. */
 static void s6e63m0_shutdown(struct spi_device *spi)
 {
@@ -856,12 +855,11 @@ static struct spi_driver s6e63m0_driver = {
 	.driver = {
 		.name	= "s6e63m0",
 		.owner	= THIS_MODULE,
+		.pm	= &s6e63m0_pm_ops,
 	},
 	.probe		= s6e63m0_probe,
 	.remove		= s6e63m0_remove,
 	.shutdown	= s6e63m0_shutdown,
-	.suspend	= s6e63m0_suspend,
-	.resume		= s6e63m0_resume,
 };
 
 module_spi_driver(s6e63m0_driver);
diff --git a/drivers/video/backlight/tdo24m.c b/drivers/video/backlight/tdo24m.c
index 00162085eec..18cdf466d50 100644
--- a/drivers/video/backlight/tdo24m.c
+++ b/drivers/video/backlight/tdo24m.c
@@ -412,25 +412,24 @@ static int tdo24m_remove(struct spi_device *spi)
 	return 0;
 }
 
-#ifdef CONFIG_PM
-static int tdo24m_suspend(struct spi_device *spi, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int tdo24m_suspend(struct device *dev)
 {
-	struct tdo24m *lcd = spi_get_drvdata(spi);
+	struct tdo24m *lcd = dev_get_drvdata(dev);
 
 	return tdo24m_power(lcd, FB_BLANK_POWERDOWN);
 }
 
-static int tdo24m_resume(struct spi_device *spi)
+static int tdo24m_resume(struct device *dev)
 {
-	struct tdo24m *lcd = spi_get_drvdata(spi);
+	struct tdo24m *lcd = dev_get_drvdata(dev);
 
 	return tdo24m_power(lcd, FB_BLANK_UNBLANK);
 }
-#else
-#define tdo24m_suspend	NULL
-#define tdo24m_resume	NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(tdo24m_pm_ops, tdo24m_suspend, tdo24m_resume);
+
 /* Power down all displays on reboot, poweroff or halt */
 static void tdo24m_shutdown(struct spi_device *spi)
 {
@@ -443,12 +442,11 @@ static struct spi_driver tdo24m_driver = {
 	.driver = {
 		.name		= "tdo24m",
 		.owner		= THIS_MODULE,
+		.pm		= &tdo24m_pm_ops,
 	},
 	.probe		= tdo24m_probe,
 	.remove		= tdo24m_remove,
 	.shutdown	= tdo24m_shutdown,
-	.suspend	= tdo24m_suspend,
-	.resume		= tdo24m_resume,
 };
 
 module_spi_driver(tdo24m_driver);
diff --git a/drivers/video/backlight/tosa_bl.c b/drivers/video/backlight/tosa_bl.c
index 2326fa810c5..9df66ac68b3 100644
--- a/drivers/video/backlight/tosa_bl.c
+++ b/drivers/video/backlight/tosa_bl.c
@@ -134,28 +134,27 @@ static int tosa_bl_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM
-static int tosa_bl_suspend(struct i2c_client *client, pm_message_t pm)
+#ifdef CONFIG_PM_SLEEP
+static int tosa_bl_suspend(struct device *dev)
 {
-	struct tosa_bl_data *data = i2c_get_clientdata(client);
+	struct tosa_bl_data *data = dev_get_drvdata(dev);
 
 	tosa_bl_set_backlight(data, 0);
 
 	return 0;
 }
 
-static int tosa_bl_resume(struct i2c_client *client)
+static int tosa_bl_resume(struct device *dev)
 {
-	struct tosa_bl_data *data = i2c_get_clientdata(client);
+	struct tosa_bl_data *data = dev_get_drvdata(dev);
 
 	backlight_update_status(data->bl);
 	return 0;
 }
-#else
-#define tosa_bl_suspend NULL
-#define tosa_bl_resume NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(tosa_bl_pm_ops, tosa_bl_suspend, tosa_bl_resume);
+
 static const struct i2c_device_id tosa_bl_id[] = {
 	{ "tosa-bl", 0 },
 	{ },
@@ -165,11 +164,10 @@ static struct i2c_driver tosa_bl_driver = {
 	.driver = {
 		.name		= "tosa-bl",
 		.owner		= THIS_MODULE,
+		.pm		= &tosa_bl_pm_ops,
 	},
 	.probe		= tosa_bl_probe,
 	.remove		= tosa_bl_remove,
-	.suspend	= tosa_bl_suspend,
-	.resume		= tosa_bl_resume,
 	.id_table	= tosa_bl_id,
 };
 
diff --git a/drivers/video/backlight/tosa_lcd.c b/drivers/video/backlight/tosa_lcd.c
index 666fe2593ea..bf081573e5b 100644
--- a/drivers/video/backlight/tosa_lcd.c
+++ b/drivers/video/backlight/tosa_lcd.c
@@ -240,19 +240,19 @@ static int tosa_lcd_remove(struct spi_device *spi)
 	return 0;
 }
 
-#ifdef CONFIG_PM
-static int tosa_lcd_suspend(struct spi_device *spi, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int tosa_lcd_suspend(struct device *dev)
 {
-	struct tosa_lcd_data *data = spi_get_drvdata(spi);
+	struct tosa_lcd_data *data = dev_get_drvdata(dev);
 
 	tosa_lcd_tg_off(data);
 
 	return 0;
 }
 
-static int tosa_lcd_resume(struct spi_device *spi)
+static int tosa_lcd_resume(struct device *dev)
 {
-	struct tosa_lcd_data *data = spi_get_drvdata(spi);
+	struct tosa_lcd_data *data = dev_get_drvdata(dev);
 
 	tosa_lcd_tg_init(data);
 	if (POWER_IS_ON(data->lcd_power))
@@ -262,20 +262,18 @@ static int tosa_lcd_resume(struct spi_device *spi)
 
 	return 0;
 }
-#else
-#define tosa_lcd_suspend	NULL
-#define tosa_lcd_resume NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(tosa_lcd_pm_ops, tosa_lcd_suspend, tosa_lcd_resume);
+
 static struct spi_driver tosa_lcd_driver = {
 	.driver = {
 		.name		= "tosa-lcd",
 		.owner		= THIS_MODULE,
+		.pm		= &tosa_lcd_pm_ops,
 	},
 	.probe		= tosa_lcd_probe,
 	.remove		= tosa_lcd_remove,
-	.suspend	= tosa_lcd_suspend,
-	.resume		= tosa_lcd_resume,
 };
 
 module_spi_driver(tosa_lcd_driver);
diff --git a/drivers/video/backlight/tps65217_bl.c b/drivers/video/backlight/tps65217_bl.c
index 70881633b45..05782312aeb 100644
--- a/drivers/video/backlight/tps65217_bl.c
+++ b/drivers/video/backlight/tps65217_bl.c
@@ -245,6 +245,18 @@ tps65217_bl_parse_dt(struct platform_device *pdev)
 		}
 	}
 
+	if (!of_property_read_u32(node, "default-brightness", &val)) {
+		if (val < 0 ||
+			val > 100) {
+			dev_err(&pdev->dev,
+				"invalid 'default-brightness' value in the device tree\n");
+			err = ERR_PTR(-EINVAL);
+			goto err;
+		}
+
+		pdata->dft_brightness = val;
+	}
+
 	of_node_put(node);
 
 	return pdata;
@@ -311,7 +323,8 @@ static int tps65217_bl_probe(struct platform_device *pdev)
 		return PTR_ERR(tps65217_bl->bl);
 	}
 
-	tps65217_bl->bl->props.brightness = 0;
+	tps65217_bl->bl->props.brightness = pdata->dft_brightness;
+	backlight_update_status(tps65217_bl->bl);
 	platform_set_drvdata(pdev, tps65217_bl);
 
 	return 0;
diff --git a/drivers/video/backlight/vgg2432a4.c b/drivers/video/backlight/vgg2432a4.c
index 84d582f591d..d538947a67d 100644
--- a/drivers/video/backlight/vgg2432a4.c
+++ b/drivers/video/backlight/vgg2432a4.c
@@ -205,18 +205,15 @@ static int vgg2432a4_lcd_init(struct ili9320 *lcd,
 	return ret;
 }
 
-#ifdef CONFIG_PM
-static int vgg2432a4_suspend(struct spi_device *spi, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int vgg2432a4_suspend(struct device *dev)
 {
-	return ili9320_suspend(spi_get_drvdata(spi), state);
+	return ili9320_suspend(dev_get_drvdata(dev));
 }
-static int vgg2432a4_resume(struct spi_device *spi)
+static int vgg2432a4_resume(struct device *dev)
 {
-	return ili9320_resume(spi_get_drvdata(spi));
+	return ili9320_resume(dev_get_drvdata(dev));
 }
-#else
-#define vgg2432a4_suspend	NULL
-#define vgg2432a4_resume	NULL
 #endif
 
 static struct ili9320_client vgg2432a4_client = {
@@ -249,16 +246,17 @@ static void vgg2432a4_shutdown(struct spi_device *spi)
 	ili9320_shutdown(spi_get_drvdata(spi));
 }
 
+static SIMPLE_DEV_PM_OPS(vgg2432a4_pm_ops, vgg2432a4_suspend, vgg2432a4_resume);
+
 static struct spi_driver vgg2432a4_driver = {
 	.driver = {
 		.name		= "VGG2432A4",
 		.owner		= THIS_MODULE,
+		.pm		= &vgg2432a4_pm_ops,
 	},
 	.probe		= vgg2432a4_probe,
 	.remove		= vgg2432a4_remove,
 	.shutdown	= vgg2432a4_shutdown,
-	.suspend	= vgg2432a4_suspend,
-	.resume		= vgg2432a4_resume,
 };
 
 module_spi_driver(vgg2432a4_driver);
diff --git a/drivers/video/console/fbcon_cw.c b/drivers/video/console/fbcon_cw.c
index 6a737827beb..a93670ef7f8 100644
--- a/drivers/video/console/fbcon_cw.c
+++ b/drivers/video/console/fbcon_cw.c
@@ -27,7 +27,7 @@ static void cw_update_attr(u8 *dst, u8 *src, int attribute,
 {
 	int i, j, offset = (vc->vc_font.height < 10) ? 1 : 2;
 	int width = (vc->vc_font.height + 7) >> 3;
-	u8 c, t = 0, msk = ~(0xff >> offset);
+	u8 c, msk = ~(0xff >> offset);
 
 	for (i = 0; i < vc->vc_font.width; i++) {
 		for (j = 0; j < width; j++) {
@@ -40,7 +40,6 @@ static void cw_update_attr(u8 *dst, u8 *src, int attribute,
 				c = ~c;
 			src++;
 			*dst++ = c;
-			t = c;
 		}
 	}
 }
diff --git a/drivers/video/controlfb.c b/drivers/video/controlfb.c
index 0c189b32a4c..67b77b40aa7 100644
--- a/drivers/video/controlfb.c
+++ b/drivers/video/controlfb.c
@@ -285,36 +285,26 @@ static int controlfb_pan_display(struct fb_var_screeninfo *var,
 static int controlfb_mmap(struct fb_info *info,
                        struct vm_area_struct *vma)
 {
-       unsigned long off, start;
-       u32 len;
-
-       off = vma->vm_pgoff << PAGE_SHIFT;
-
-       /* frame buffer memory */
-       start = info->fix.smem_start;
-       len = PAGE_ALIGN((start & ~PAGE_MASK)+info->fix.smem_len);
-       if (off >= len) {
-               /* memory mapped io */
-               off -= len;
-               if (info->var.accel_flags)
-                       return -EINVAL;
-               start = info->fix.mmio_start;
-               len = PAGE_ALIGN((start & ~PAGE_MASK)+info->fix.mmio_len);
-	       vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
-       } else {
-               /* framebuffer */
-	       vma->vm_page_prot = pgprot_cached_wthru(vma->vm_page_prot);
-       }
-       start &= PAGE_MASK;
-       if ((vma->vm_end - vma->vm_start + off) > len)
-       		return -EINVAL;
-       off += start;
-       vma->vm_pgoff = off >> PAGE_SHIFT;
-       if (io_remap_pfn_range(vma, vma->vm_start, off >> PAGE_SHIFT,
-           vma->vm_end - vma->vm_start, vma->vm_page_prot))
-               return -EAGAIN;
-
-       return 0;
+	unsigned long mmio_pgoff;
+	unsigned long start;
+	u32 len;
+
+	start = info->fix.smem_start;
+	len = info->fix.smem_len;
+	mmio_pgoff = PAGE_ALIGN((start & ~PAGE_MASK) + len) >> PAGE_SHIFT;
+	if (vma->vm_pgoff >= mmio_pgoff) {
+		if (info->var.accel_flags)
+			return -EINVAL;
+		vma->vm_pgoff -= mmio_pgoff;
+		start = info->fix.mmio_start;
+		len = info->fix.mmio_len;
+		vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+	} else {
+		/* framebuffer */
+		vma->vm_page_prot = pgprot_cached_wthru(vma->vm_page_prot);
+	}
+
+	return vm_iomap_memory(vma, start, len);
 }
 
 static int controlfb_blank(int blank_mode, struct fb_info *info)
diff --git a/drivers/video/ep93xx-fb.c b/drivers/video/ep93xx-fb.c
index e06cd5d90c9..ee1ee540154 100644
--- a/drivers/video/ep93xx-fb.c
+++ b/drivers/video/ep93xx-fb.c
@@ -419,7 +419,7 @@ static struct fb_ops ep93xxfb_ops = {
 	.fb_mmap	= ep93xxfb_mmap,
 };
 
-static int __init ep93xxfb_calc_fbsize(struct ep93xxfb_mach_info *mach_info)
+static int ep93xxfb_calc_fbsize(struct ep93xxfb_mach_info *mach_info)
 {
 	int i, fb_size = 0;
 
@@ -441,7 +441,7 @@ static int __init ep93xxfb_calc_fbsize(struct ep93xxfb_mach_info *mach_info)
 	return fb_size;
 }
 
-static int __init ep93xxfb_alloc_videomem(struct fb_info *info)
+static int ep93xxfb_alloc_videomem(struct fb_info *info)
 {
 	struct ep93xx_fbi *fbi = info->par;
 	char __iomem *virt_addr;
@@ -627,19 +627,7 @@ static struct platform_driver ep93xxfb_driver = {
 		.owner	= THIS_MODULE,
 	},
 };
-
-static int ep93xxfb_init(void)
-{
-	return platform_driver_register(&ep93xxfb_driver);
-}
-
-static void __exit ep93xxfb_exit(void)
-{
-	platform_driver_unregister(&ep93xxfb_driver);
-}
-
-module_init(ep93xxfb_init);
-module_exit(ep93xxfb_exit);
+module_platform_driver(ep93xxfb_driver);
 
 MODULE_DESCRIPTION("EP93XX Framebuffer Driver");
 MODULE_ALIAS("platform:ep93xx-fb");
diff --git a/drivers/video/exynos/exynos_dp_core.c b/drivers/video/exynos/exynos_dp_core.c
index de9d4da0e3d..12bbede3b09 100644
--- a/drivers/video/exynos/exynos_dp_core.c
+++ b/drivers/video/exynos/exynos_dp_core.c
@@ -976,14 +976,14 @@ static int exynos_dp_dt_parse_phydata(struct exynos_dp_device *dp)
 	}
 
 	if (of_property_read_u32(dp_phy_node, "reg", &phy_base)) {
-		dev_err(dp->dev, "faild to get reg for dptx-phy\n");
+		dev_err(dp->dev, "failed to get reg for dptx-phy\n");
 		ret = -EINVAL;
 		goto err;
 	}
 
 	if (of_property_read_u32(dp_phy_node, "samsung,enable-mask",
 				&dp->enable_mask)) {
-		dev_err(dp->dev, "faild to get enable-mask for dptx-phy\n");
+		dev_err(dp->dev, "failed to get enable-mask for dptx-phy\n");
 		ret = -EINVAL;
 		goto err;
 	}
diff --git a/drivers/video/exynos/exynos_mipi_dsi.c b/drivers/video/exynos/exynos_mipi_dsi.c
index fac7df6d1ab..32e540600f9 100644
--- a/drivers/video/exynos/exynos_mipi_dsi.c
+++ b/drivers/video/exynos/exynos_mipi_dsi.c
@@ -32,11 +32,10 @@
 #include <linux/notifier.h>
 #include <linux/regulator/consumer.h>
 #include <linux/pm_runtime.h>
+#include <linux/err.h>
 
 #include <video/exynos_mipi_dsim.h>
 
-#include <plat/fb.h>
-
 #include "exynos_mipi_dsi_common.h"
 #include "exynos_mipi_dsi_lowlevel.h"
 
@@ -384,10 +383,9 @@ static int exynos_mipi_dsi_probe(struct platform_device *pdev)
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 
-	dsim->reg_base = devm_request_and_ioremap(&pdev->dev, res);
-	if (!dsim->reg_base) {
-		dev_err(&pdev->dev, "failed to remap io region\n");
-		ret = -ENOMEM;
+	dsim->reg_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(dsim->reg_base)) {
+		ret = PTR_ERR(dsim->reg_base);
 		goto error;
 	}
 
diff --git a/drivers/video/exynos/exynos_mipi_dsi_common.c b/drivers/video/exynos/exynos_mipi_dsi_common.c
index c70cb8926df..520fc9bd887 100644
--- a/drivers/video/exynos/exynos_mipi_dsi_common.c
+++ b/drivers/video/exynos/exynos_mipi_dsi_common.c
@@ -31,8 +31,6 @@
 #include <video/mipi_display.h>
 #include <video/exynos_mipi_dsim.h>
 
-#include <mach/map.h>
-
 #include "exynos_mipi_dsi_regs.h"
 #include "exynos_mipi_dsi_lowlevel.h"
 #include "exynos_mipi_dsi_common.h"
diff --git a/drivers/video/exynos/exynos_mipi_dsi_lowlevel.c b/drivers/video/exynos/exynos_mipi_dsi_lowlevel.c
index 95cb99a1fe2..15c5abd408d 100644
--- a/drivers/video/exynos/exynos_mipi_dsi_lowlevel.c
+++ b/drivers/video/exynos/exynos_mipi_dsi_lowlevel.c
@@ -26,8 +26,6 @@
 
 #include <video/exynos_mipi_dsim.h>
 
-#include <mach/map.h>
-
 #include "exynos_mipi_dsi_regs.h"
 
 void exynos_mipi_dsi_func_reset(struct mipi_dsim_device *dsim)
diff --git a/drivers/video/fb-puv3.c b/drivers/video/fb-puv3.c
index 7d106f1f490..27fc956166f 100644
--- a/drivers/video/fb-puv3.c
+++ b/drivers/video/fb-puv3.c
@@ -640,21 +640,9 @@ static int unifb_pan_display(struct fb_var_screeninfo *var,
 int unifb_mmap(struct fb_info *info,
 		    struct vm_area_struct *vma)
 {
-	unsigned long size = vma->vm_end - vma->vm_start;
-	unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
-	unsigned long pos = info->fix.smem_start + offset;
-
-	if (offset + size > info->fix.smem_len)
-		return -EINVAL;
-
 	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
 
-	if (io_remap_pfn_range(vma, vma->vm_start, pos >> PAGE_SHIFT, size,
-				vma->vm_page_prot))
-		return -EAGAIN;
-
-	/* VM_IO | VM_DONTEXPAND | VM_DONTDUMP are set by remap_pfn_range() */
-	return 0;
+	return vm_iomap_memory(vma, info->fix.smem_start, info->fix.smem_len);
 }
 
 static struct fb_ops unifb_ops = {
diff --git a/drivers/video/fbmem.c b/drivers/video/fbmem.c
index 7c254084b6a..dcb669eb453 100644
--- a/drivers/video/fbmem.c
+++ b/drivers/video/fbmem.c
@@ -1373,15 +1373,12 @@ fb_mmap(struct file *file, struct vm_area_struct * vma)
 {
 	struct fb_info *info = file_fb_info(file);
 	struct fb_ops *fb;
-	unsigned long off;
+	unsigned long mmio_pgoff;
 	unsigned long start;
 	u32 len;
 
 	if (!info)
 		return -ENODEV;
-	if (vma->vm_pgoff > (~0UL >> PAGE_SHIFT))
-		return -EINVAL;
-	off = vma->vm_pgoff << PAGE_SHIFT;
 	fb = info->fbops;
 	if (!fb)
 		return -ENODEV;
@@ -1393,32 +1390,29 @@ fb_mmap(struct file *file, struct vm_area_struct * vma)
 		return res;
 	}
 
-	/* frame buffer memory */
+	/*
+	 * Ugh. This can be either the frame buffer mapping, or
+	 * if pgoff points past it, the mmio mapping.
+	 */
 	start = info->fix.smem_start;
-	len = PAGE_ALIGN((start & ~PAGE_MASK) + info->fix.smem_len);
-	if (off >= len) {
-		/* memory mapped io */
-		off -= len;
+	len = info->fix.smem_len;
+	mmio_pgoff = PAGE_ALIGN((start & ~PAGE_MASK) + len) >> PAGE_SHIFT;
+	if (vma->vm_pgoff >= mmio_pgoff) {
 		if (info->var.accel_flags) {
 			mutex_unlock(&info->mm_lock);
 			return -EINVAL;
 		}
+
+		vma->vm_pgoff -= mmio_pgoff;
 		start = info->fix.mmio_start;
-		len = PAGE_ALIGN((start & ~PAGE_MASK) + info->fix.mmio_len);
+		len = info->fix.mmio_len;
 	}
 	mutex_unlock(&info->mm_lock);
-	start &= PAGE_MASK;
-	if ((vma->vm_end - vma->vm_start + off) > len)
-		return -EINVAL;
-	off += start;
-	vma->vm_pgoff = off >> PAGE_SHIFT;
-	/* VM_IO | VM_DONTEXPAND | VM_DONTDUMP are set by io_remap_pfn_range()*/
+
 	vma->vm_page_prot = vm_get_page_prot(vma->vm_flags);
-	fb_pgprotect(file, vma, off);
-	if (io_remap_pfn_range(vma, vma->vm_start, off >> PAGE_SHIFT,
-			     vma->vm_end - vma->vm_start, vma->vm_page_prot))
-		return -EAGAIN;
-	return 0;
+	fb_pgprotect(file, vma, start);
+
+	return vm_iomap_memory(vma, start, len);
 }
 
 static int
diff --git a/drivers/video/fbmon.c b/drivers/video/fbmon.c
index 94ad0f71383..6103fa6fb54 100644
--- a/drivers/video/fbmon.c
+++ b/drivers/video/fbmon.c
@@ -1376,7 +1376,7 @@ int fb_get_mode(int flags, u32 val, struct fb_var_screeninfo *var, struct fb_inf
 	return err;
 }
 
-#if IS_ENABLED(CONFIG_VIDEOMODE)
+#ifdef CONFIG_VIDEOMODE_HELPERS
 int fb_videomode_from_videomode(const struct videomode *vm,
 				struct fb_videomode *fbmode)
 {
@@ -1398,13 +1398,13 @@ int fb_videomode_from_videomode(const struct videomode *vm,
 
 	fbmode->sync = 0;
 	fbmode->vmode = 0;
-	if (vm->dmt_flags & VESA_DMT_HSYNC_HIGH)
+	if (vm->flags & DISPLAY_FLAGS_HSYNC_HIGH)
 		fbmode->sync |= FB_SYNC_HOR_HIGH_ACT;
-	if (vm->dmt_flags & VESA_DMT_HSYNC_HIGH)
+	if (vm->flags & DISPLAY_FLAGS_VSYNC_HIGH)
 		fbmode->sync |= FB_SYNC_VERT_HIGH_ACT;
-	if (vm->data_flags & DISPLAY_FLAGS_INTERLACED)
+	if (vm->flags & DISPLAY_FLAGS_INTERLACED)
 		fbmode->vmode |= FB_VMODE_INTERLACED;
-	if (vm->data_flags & DISPLAY_FLAGS_DOUBLESCAN)
+	if (vm->flags & DISPLAY_FLAGS_DOUBLESCAN)
 		fbmode->vmode |= FB_VMODE_DOUBLE;
 	fbmode->flag = 0;
 
@@ -1424,9 +1424,8 @@ int fb_videomode_from_videomode(const struct videomode *vm,
 	return 0;
 }
 EXPORT_SYMBOL_GPL(fb_videomode_from_videomode);
-#endif
 
-#if IS_ENABLED(CONFIG_OF_VIDEOMODE)
+#ifdef CONFIG_OF
 static inline void dump_fb_videomode(const struct fb_videomode *m)
 {
 	pr_debug("fb_videomode = %ux%u@%uHz (%ukHz) %u %u %u %u %u %u %u %u %u\n",
@@ -1465,7 +1464,8 @@ int of_get_fb_videomode(struct device_node *np, struct fb_videomode *fb,
 	return 0;
 }
 EXPORT_SYMBOL_GPL(of_get_fb_videomode);
-#endif
+#endif /* CONFIG_OF */
+#endif /* CONFIG_VIDEOMODE_HELPERS */
 
 #else
 int fb_parse_edid(unsigned char *edid, struct fb_var_screeninfo *var)
diff --git a/drivers/video/fsl-diu-fb.c b/drivers/video/fsl-diu-fb.c
index 41fbd9453c5..6c278056fc6 100644
--- a/drivers/video/fsl-diu-fb.c
+++ b/drivers/video/fsl-diu-fb.c
@@ -375,7 +375,10 @@ struct fsl_diu_data {
 	struct diu_ad dummy_ad __aligned(8);
 	struct diu_ad ad[NUM_AOIS] __aligned(8);
 	u8 gamma[256 * 3] __aligned(32);
-	u8 cursor[MAX_CURS * MAX_CURS * 2] __aligned(32);
+	/* It's easier to parse the cursor data as little-endian */
+	__le16 cursor[MAX_CURS * MAX_CURS] __aligned(32);
+	/* Blank cursor data -- used to hide the cursor */
+	__le16 blank_cursor[MAX_CURS * MAX_CURS] __aligned(32);
 	uint8_t edid_data[EDID_LENGTH];
 	bool has_edid;
 } __aligned(32);
@@ -824,7 +827,6 @@ static void update_lcdc(struct fb_info *info)
 	/* Program DIU registers */
 
 	out_be32(&hw->gamma, DMA_ADDR(data, gamma));
-	out_be32(&hw->cursor, DMA_ADDR(data, cursor));
 
 	out_be32(&hw->bgnd, 0x007F7F7F); /* Set background to grey */
 	out_be32(&hw->disp_size, (var->yres << 16) | var->xres);
@@ -968,6 +970,156 @@ static u32 fsl_diu_get_pixel_format(unsigned int bits_per_pixel)
 }
 
 /*
+ * Copies a cursor image from user space to the proper place in driver
+ * memory so that the hardware can display the cursor image.
+ *
+ * Cursor data is represented as a sequence of 'width' bits packed into bytes.
+ * That is, the first 8 bits are in the first byte, the second 8 bits in the
+ * second byte, and so on.  Therefore, the each row of the cursor is (width +
+ * 7) / 8 bytes of 'data'
+ *
+ * The DIU only supports cursors up to 32x32 (MAX_CURS).  We reject cursors
+ * larger than this, so we already know that 'width' <= 32.  Therefore, we can
+ * simplify our code by using a 32-bit big-endian integer ("line") to read in
+ * a single line of pixels, and only look at the top 'width' bits of that
+ * integer.
+ *
+ * This could result in an unaligned 32-bit read.  For example, if the cursor
+ * is 24x24, then the first three bytes of 'image' contain the pixel data for
+ * the top line of the cursor.  We do a 32-bit read of 'image', but we look
+ * only at the top 24 bits.  Then we increment 'image' by 3 bytes.  The next
+ * read is unaligned.  The only problem is that we might read past the end of
+ * 'image' by 1-3 bytes, but that should not cause any problems.
+ */
+static void fsl_diu_load_cursor_image(struct fb_info *info,
+	const void *image, uint16_t bg, uint16_t fg,
+	unsigned int width, unsigned int height)
+{
+	struct mfb_info *mfbi = info->par;
+	struct fsl_diu_data *data = mfbi->parent;
+	__le16 *cursor = data->cursor;
+	__le16 _fg = cpu_to_le16(fg);
+	__le16 _bg = cpu_to_le16(bg);
+	unsigned int h, w;
+
+	for (h = 0; h < height; h++) {
+		uint32_t mask = 1 << 31;
+		uint32_t line = be32_to_cpup(image);
+
+		for (w = 0; w < width; w++) {
+			cursor[w] = (line & mask) ? _fg : _bg;
+			mask >>= 1;
+		}
+
+		cursor += MAX_CURS;
+		image += DIV_ROUND_UP(width, 8);
+	}
+}
+
+/*
+ * Set a hardware cursor.  The image data for the cursor is passed via the
+ * fb_cursor object.
+ */
+static int fsl_diu_cursor(struct fb_info *info, struct fb_cursor *cursor)
+{
+	struct mfb_info *mfbi = info->par;
+	struct fsl_diu_data *data = mfbi->parent;
+	struct diu __iomem *hw = data->diu_reg;
+
+	if (cursor->image.width > MAX_CURS || cursor->image.height > MAX_CURS)
+		return -EINVAL;
+
+	/* The cursor size has changed */
+	if (cursor->set & FB_CUR_SETSIZE) {
+		/*
+		 * The DIU cursor is a fixed size, so when we get this
+		 * message, instead of resizing the cursor, we just clear
+		 * all the image data, in expectation of new data.  However,
+		 * in tests this control does not appear to be normally
+		 * called.
+		 */
+		memset(data->cursor, 0, sizeof(data->cursor));
+	}
+
+	/* The cursor position has changed (cursor->image.dx|dy) */
+	if (cursor->set & FB_CUR_SETPOS) {
+		uint32_t xx, yy;
+
+		yy = (cursor->image.dy - info->var.yoffset) & 0x7ff;
+		xx = (cursor->image.dx - info->var.xoffset) & 0x7ff;
+
+		out_be32(&hw->curs_pos, yy << 16 | xx);
+	}
+
+	/*
+	 * FB_CUR_SETIMAGE - the cursor image has changed
+	 * FB_CUR_SETCMAP  - the cursor colors has changed
+	 * FB_CUR_SETSHAPE - the cursor bitmask has changed
+	 */
+	if (cursor->set & (FB_CUR_SETSHAPE | FB_CUR_SETCMAP | FB_CUR_SETIMAGE)) {
+		unsigned int image_size =
+			DIV_ROUND_UP(cursor->image.width, 8) * cursor->image.height;
+		unsigned int image_words =
+			DIV_ROUND_UP(image_size, sizeof(uint32_t));
+		unsigned int bg_idx = cursor->image.bg_color;
+		unsigned int fg_idx = cursor->image.fg_color;
+		uint8_t buffer[image_size];
+		uint32_t *image, *source, *mask;
+		uint16_t fg, bg;
+		unsigned int i;
+
+		if (info->state != FBINFO_STATE_RUNNING)
+			return 0;
+
+		/*
+		 * Determine the size of the cursor image data.  Normally,
+		 * it's 8x16.
+		 */
+		image_size = DIV_ROUND_UP(cursor->image.width, 8) *
+			cursor->image.height;
+
+		bg = ((info->cmap.red[bg_idx] & 0xf8) << 7) |
+		     ((info->cmap.green[bg_idx] & 0xf8) << 2) |
+		     ((info->cmap.blue[bg_idx] & 0xf8) >> 3) |
+		     1 << 15;
+
+		fg = ((info->cmap.red[fg_idx] & 0xf8) << 7) |
+		     ((info->cmap.green[fg_idx] & 0xf8) << 2) |
+		     ((info->cmap.blue[fg_idx] & 0xf8) >> 3) |
+		     1 << 15;
+
+		/* Use 32-bit operations on the data to improve performance */
+		image = (uint32_t *)buffer;
+		source = (uint32_t *)cursor->image.data;
+		mask = (uint32_t *)cursor->mask;
+
+		if (cursor->rop == ROP_XOR)
+			for (i = 0; i < image_words; i++)
+				image[i] = source[i] ^ mask[i];
+		else
+			for (i = 0; i < image_words; i++)
+				image[i] = source[i] & mask[i];
+
+		fsl_diu_load_cursor_image(info, image, bg, fg,
+			cursor->image.width, cursor->image.height);
+	};
+
+	/*
+	 * Show or hide the cursor.  The cursor data is always stored in the
+	 * 'cursor' memory block, and the actual cursor position is always in
+	 * the DIU's CURS_POS register.  To hide the cursor, we redirect the
+	 * CURSOR register to a blank cursor.  The show the cursor, we
+	 * redirect the CURSOR register to the real cursor data.
+	 */
+	if (cursor->enable)
+		out_be32(&hw->cursor, DMA_ADDR(data, cursor));
+	else
+		out_be32(&hw->cursor, DMA_ADDR(data, blank_cursor));
+
+	return 0;
+}
+
+/*
  * Using the fb_var_screeninfo in fb_info we set the resolution of this
  * particular framebuffer. This function alters the fb_fix_screeninfo stored
  * in fb_info. It does not alter var in fb_info since we are using that
@@ -1312,6 +1464,7 @@ static struct fb_ops fsl_diu_ops = {
 	.fb_ioctl = fsl_diu_ioctl,
 	.fb_open = fsl_diu_open,
 	.fb_release = fsl_diu_release,
+	.fb_cursor = fsl_diu_cursor,
 };
 
 static int install_fb(struct fb_info *info)
diff --git a/drivers/video/gbefb.c b/drivers/video/gbefb.c
index bda5e394151..ceab37020ff 100644
--- a/drivers/video/gbefb.c
+++ b/drivers/video/gbefb.c
@@ -1016,7 +1016,9 @@ static int gbefb_mmap(struct fb_info *info,
 	/* check range */
 	if (vma->vm_pgoff > (~0UL >> PAGE_SHIFT))
 		return -EINVAL;
-	if (offset + size > gbe_mem_size)
+	if (size > gbe_mem_size)
+		return -EINVAL;
+	if (offset > gbe_mem_size - size)
 		return -EINVAL;
 
 	/* remap using the fastest write-through mode on architecture */
diff --git a/drivers/video/goldfishfb.c b/drivers/video/goldfishfb.c
index 489abb32fc0..7f6c9e6cfc6 100644
--- a/drivers/video/goldfishfb.c
+++ b/drivers/video/goldfishfb.c
@@ -148,7 +148,7 @@ static int goldfish_fb_pan_display(struct fb_var_screeninfo *var,
 	wait_event_timeout(fb->wait,
 			fb->base_update_count != base_update_count, HZ / 15);
 	if (fb->base_update_count == base_update_count)
-		pr_err("goldfish_fb_pan_display: timeout wating for base update\n");
+		pr_err("goldfish_fb_pan_display: timeout waiting for base update\n");
 	return 0;
 }
 
diff --git a/drivers/video/hyperv_fb.c b/drivers/video/hyperv_fb.c
new file mode 100644
index 00000000000..d4d2c5fe248
--- /dev/null
+++ b/drivers/video/hyperv_fb.c
@@ -0,0 +1,829 @@
+/*
+ * Copyright (c) 2012, Microsoft Corporation.
+ *
+ * Author:
+ *   Haiyang Zhang <haiyangz@microsoft.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, GOOD TITLE or
+ * NON INFRINGEMENT.  See the GNU General Public License for more
+ * details.
+ */
+
+/*
+ * Hyper-V Synthetic Video Frame Buffer Driver
+ *
+ * This is the driver for the Hyper-V Synthetic Video, which supports
+ * screen resolution up to Full HD 1920x1080 with 32 bit color on Windows
+ * Server 2012, and 1600x1200 with 16 bit color on Windows Server 2008 R2
+ * or earlier.
+ *
+ * It also solves the double mouse cursor issue of the emulated video mode.
+ *
+ * The default screen resolution is 1152x864, which may be changed by a
+ * kernel parameter:
+ *     video=hyperv_fb:<width>x<height>
+ *     For example: video=hyperv_fb:1280x1024
+ *
+ * Portrait orientation is also supported:
+ *     For example: video=hyperv_fb:864x1152
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/completion.h>
+#include <linux/fb.h>
+#include <linux/pci.h>
+
+#include <linux/hyperv.h>
+
+
+/* Hyper-V Synthetic Video Protocol definitions and structures */
+#define MAX_VMBUS_PKT_SIZE 0x4000
+
+#define SYNTHVID_VERSION(major, minor) ((minor) << 16 | (major))
+#define SYNTHVID_VERSION_WIN7 SYNTHVID_VERSION(3, 0)
+#define SYNTHVID_VERSION_WIN8 SYNTHVID_VERSION(3, 2)
+
+#define SYNTHVID_DEPTH_WIN7 16
+#define SYNTHVID_DEPTH_WIN8 32
+
+#define SYNTHVID_FB_SIZE_WIN7 (4 * 1024 * 1024)
+#define SYNTHVID_WIDTH_MAX_WIN7 1600
+#define SYNTHVID_HEIGHT_MAX_WIN7 1200
+
+#define SYNTHVID_FB_SIZE_WIN8 (8 * 1024 * 1024)
+
+#define PCI_VENDOR_ID_MICROSOFT 0x1414
+#define PCI_DEVICE_ID_HYPERV_VIDEO 0x5353
+
+
+enum pipe_msg_type {
+	PIPE_MSG_INVALID,
+	PIPE_MSG_DATA,
+	PIPE_MSG_MAX
+};
+
+struct pipe_msg_hdr {
+	u32 type;
+	u32 size; /* size of message after this field */
+} __packed;
+
+
+enum synthvid_msg_type {
+	SYNTHVID_ERROR			= 0,
+	SYNTHVID_VERSION_REQUEST	= 1,
+	SYNTHVID_VERSION_RESPONSE	= 2,
+	SYNTHVID_VRAM_LOCATION		= 3,
+	SYNTHVID_VRAM_LOCATION_ACK	= 4,
+	SYNTHVID_SITUATION_UPDATE	= 5,
+	SYNTHVID_SITUATION_UPDATE_ACK	= 6,
+	SYNTHVID_POINTER_POSITION	= 7,
+	SYNTHVID_POINTER_SHAPE		= 8,
+	SYNTHVID_FEATURE_CHANGE		= 9,
+	SYNTHVID_DIRT			= 10,
+
+	SYNTHVID_MAX			= 11
+};
+
+struct synthvid_msg_hdr {
+	u32 type;
+	u32 size;  /* size of this header + payload after this field*/
+} __packed;
+
+
+struct synthvid_version_req {
+	u32 version;
+} __packed;
+
+struct synthvid_version_resp {
+	u32 version;
+	u8 is_accepted;
+	u8 max_video_outputs;
+} __packed;
+
+struct synthvid_vram_location {
+	u64 user_ctx;
+	u8 is_vram_gpa_specified;
+	u64 vram_gpa;
+} __packed;
+
+struct synthvid_vram_location_ack {
+	u64 user_ctx;
+} __packed;
+
+struct video_output_situation {
+	u8 active;
+	u32 vram_offset;
+	u8 depth_bits;
+	u32 width_pixels;
+	u32 height_pixels;
+	u32 pitch_bytes;
+} __packed;
+
+struct synthvid_situation_update {
+	u64 user_ctx;
+	u8 video_output_count;
+	struct video_output_situation video_output[1];
+} __packed;
+
+struct synthvid_situation_update_ack {
+	u64 user_ctx;
+} __packed;
+
+struct synthvid_pointer_position {
+	u8 is_visible;
+	u8 video_output;
+	s32 image_x;
+	s32 image_y;
+} __packed;
+
+
+#define CURSOR_MAX_X 96
+#define CURSOR_MAX_Y 96
+#define CURSOR_ARGB_PIXEL_SIZE 4
+#define CURSOR_MAX_SIZE (CURSOR_MAX_X * CURSOR_MAX_Y * CURSOR_ARGB_PIXEL_SIZE)
+#define CURSOR_COMPLETE (-1)
+
+struct synthvid_pointer_shape {
+	u8 part_idx;
+	u8 is_argb;
+	u32 width; /* CURSOR_MAX_X at most */
+	u32 height; /* CURSOR_MAX_Y at most */
+	u32 hot_x; /* hotspot relative to upper-left of pointer image */
+	u32 hot_y;
+	u8 data[4];
+} __packed;
+
+struct synthvid_feature_change {
+	u8 is_dirt_needed;
+	u8 is_ptr_pos_needed;
+	u8 is_ptr_shape_needed;
+	u8 is_situ_needed;
+} __packed;
+
+struct rect {
+	s32 x1, y1; /* top left corner */
+	s32 x2, y2; /* bottom right corner, exclusive */
+} __packed;
+
+struct synthvid_dirt {
+	u8 video_output;
+	u8 dirt_count;
+	struct rect rect[1];
+} __packed;
+
+struct synthvid_msg {
+	struct pipe_msg_hdr pipe_hdr;
+	struct synthvid_msg_hdr vid_hdr;
+	union {
+		struct synthvid_version_req ver_req;
+		struct synthvid_version_resp ver_resp;
+		struct synthvid_vram_location vram;
+		struct synthvid_vram_location_ack vram_ack;
+		struct synthvid_situation_update situ;
+		struct synthvid_situation_update_ack situ_ack;
+		struct synthvid_pointer_position ptr_pos;
+		struct synthvid_pointer_shape ptr_shape;
+		struct synthvid_feature_change feature_chg;
+		struct synthvid_dirt dirt;
+	};
+} __packed;
+
+
+
+/* FB driver definitions and structures */
+#define HVFB_WIDTH 1152 /* default screen width */
+#define HVFB_HEIGHT 864 /* default screen height */
+#define HVFB_WIDTH_MIN 640
+#define HVFB_HEIGHT_MIN 480
+
+#define RING_BUFSIZE (256 * 1024)
+#define VSP_TIMEOUT (10 * HZ)
+#define HVFB_UPDATE_DELAY (HZ / 20)
+
+struct hvfb_par {
+	struct fb_info *info;
+	bool fb_ready; /* fb device is ready */
+	struct completion wait;
+	u32 synthvid_version;
+
+	struct delayed_work dwork;
+	bool update;
+
+	u32 pseudo_palette[16];
+	u8 init_buf[MAX_VMBUS_PKT_SIZE];
+	u8 recv_buf[MAX_VMBUS_PKT_SIZE];
+};
+
+static uint screen_width = HVFB_WIDTH;
+static uint screen_height = HVFB_HEIGHT;
+static uint screen_depth;
+static uint screen_fb_size;
+
+/* Send message to Hyper-V host */
+static inline int synthvid_send(struct hv_device *hdev,
+				struct synthvid_msg *msg)
+{
+	static atomic64_t request_id = ATOMIC64_INIT(0);
+	int ret;
+
+	msg->pipe_hdr.type = PIPE_MSG_DATA;
+	msg->pipe_hdr.size = msg->vid_hdr.size;
+
+	ret = vmbus_sendpacket(hdev->channel, msg,
+			       msg->vid_hdr.size + sizeof(struct pipe_msg_hdr),
+			       atomic64_inc_return(&request_id),
+			       VM_PKT_DATA_INBAND, 0);
+
+	if (ret)
+		pr_err("Unable to send packet via vmbus\n");
+
+	return ret;
+}
+
+
+/* Send screen resolution info to host */
+static int synthvid_send_situ(struct hv_device *hdev)
+{
+	struct fb_info *info = hv_get_drvdata(hdev);
+	struct synthvid_msg msg;
+
+	if (!info)
+		return -ENODEV;
+
+	memset(&msg, 0, sizeof(struct synthvid_msg));
+
+	msg.vid_hdr.type = SYNTHVID_SITUATION_UPDATE;
+	msg.vid_hdr.size = sizeof(struct synthvid_msg_hdr) +
+		sizeof(struct synthvid_situation_update);
+	msg.situ.user_ctx = 0;
+	msg.situ.video_output_count = 1;
+	msg.situ.video_output[0].active = 1;
+	msg.situ.video_output[0].vram_offset = 0;
+	msg.situ.video_output[0].depth_bits = info->var.bits_per_pixel;
+	msg.situ.video_output[0].width_pixels = info->var.xres;
+	msg.situ.video_output[0].height_pixels = info->var.yres;
+	msg.situ.video_output[0].pitch_bytes = info->fix.line_length;
+
+	synthvid_send(hdev, &msg);
+
+	return 0;
+}
+
+/* Send mouse pointer info to host */
+static int synthvid_send_ptr(struct hv_device *hdev)
+{
+	struct synthvid_msg msg;
+
+	memset(&msg, 0, sizeof(struct synthvid_msg));
+	msg.vid_hdr.type = SYNTHVID_POINTER_POSITION;
+	msg.vid_hdr.size = sizeof(struct synthvid_msg_hdr) +
+		sizeof(struct synthvid_pointer_position);
+	msg.ptr_pos.is_visible = 1;
+	msg.ptr_pos.video_output = 0;
+	msg.ptr_pos.image_x = 0;
+	msg.ptr_pos.image_y = 0;
+	synthvid_send(hdev, &msg);
+
+	memset(&msg, 0, sizeof(struct synthvid_msg));
+	msg.vid_hdr.type = SYNTHVID_POINTER_SHAPE;
+	msg.vid_hdr.size = sizeof(struct synthvid_msg_hdr) +
+		sizeof(struct synthvid_pointer_shape);
+	msg.ptr_shape.part_idx = CURSOR_COMPLETE;
+	msg.ptr_shape.is_argb = 1;
+	msg.ptr_shape.width = 1;
+	msg.ptr_shape.height = 1;
+	msg.ptr_shape.hot_x = 0;
+	msg.ptr_shape.hot_y = 0;
+	msg.ptr_shape.data[0] = 0;
+	msg.ptr_shape.data[1] = 1;
+	msg.ptr_shape.data[2] = 1;
+	msg.ptr_shape.data[3] = 1;
+	synthvid_send(hdev, &msg);
+
+	return 0;
+}
+
+/* Send updated screen area (dirty rectangle) location to host */
+static int synthvid_update(struct fb_info *info)
+{
+	struct hv_device *hdev = device_to_hv_device(info->device);
+	struct synthvid_msg msg;
+
+	memset(&msg, 0, sizeof(struct synthvid_msg));
+
+	msg.vid_hdr.type = SYNTHVID_DIRT;
+	msg.vid_hdr.size = sizeof(struct synthvid_msg_hdr) +
+		sizeof(struct synthvid_dirt);
+	msg.dirt.video_output = 0;
+	msg.dirt.dirt_count = 1;
+	msg.dirt.rect[0].x1 = 0;
+	msg.dirt.rect[0].y1 = 0;
+	msg.dirt.rect[0].x2 = info->var.xres;
+	msg.dirt.rect[0].y2 = info->var.yres;
+
+	synthvid_send(hdev, &msg);
+
+	return 0;
+}
+
+
+/*
+ * Actions on received messages from host:
+ * Complete the wait event.
+ * Or, reply with screen and cursor info.
+ */
+static void synthvid_recv_sub(struct hv_device *hdev)
+{
+	struct fb_info *info = hv_get_drvdata(hdev);
+	struct hvfb_par *par;
+	struct synthvid_msg *msg;
+
+	if (!info)
+		return;
+
+	par = info->par;
+	msg = (struct synthvid_msg *)par->recv_buf;
+
+	/* Complete the wait event */
+	if (msg->vid_hdr.type == SYNTHVID_VERSION_RESPONSE ||
+	    msg->vid_hdr.type == SYNTHVID_VRAM_LOCATION_ACK) {
+		memcpy(par->init_buf, msg, MAX_VMBUS_PKT_SIZE);
+		complete(&par->wait);
+		return;
+	}
+
+	/* Reply with screen and cursor info */
+	if (msg->vid_hdr.type == SYNTHVID_FEATURE_CHANGE) {
+		if (par->fb_ready) {
+			synthvid_send_ptr(hdev);
+			synthvid_send_situ(hdev);
+		}
+
+		par->update = msg->feature_chg.is_dirt_needed;
+		if (par->update)
+			schedule_delayed_work(&par->dwork, HVFB_UPDATE_DELAY);
+	}
+}
+
+/* Receive callback for messages from the host */
+static void synthvid_receive(void *ctx)
+{
+	struct hv_device *hdev = ctx;
+	struct fb_info *info = hv_get_drvdata(hdev);
+	struct hvfb_par *par;
+	struct synthvid_msg *recv_buf;
+	u32 bytes_recvd;
+	u64 req_id;
+	int ret;
+
+	if (!info)
+		return;
+
+	par = info->par;
+	recv_buf = (struct synthvid_msg *)par->recv_buf;
+
+	do {
+		ret = vmbus_recvpacket(hdev->channel, recv_buf,
+				       MAX_VMBUS_PKT_SIZE,
+				       &bytes_recvd, &req_id);
+		if (bytes_recvd > 0 &&
+		    recv_buf->pipe_hdr.type == PIPE_MSG_DATA)
+			synthvid_recv_sub(hdev);
+	} while (bytes_recvd > 0 && ret == 0);
+}
+
+/* Check synthetic video protocol version with the host */
+static int synthvid_negotiate_ver(struct hv_device *hdev, u32 ver)
+{
+	struct fb_info *info = hv_get_drvdata(hdev);
+	struct hvfb_par *par = info->par;
+	struct synthvid_msg *msg = (struct synthvid_msg *)par->init_buf;
+	int t, ret = 0;
+
+	memset(msg, 0, sizeof(struct synthvid_msg));
+	msg->vid_hdr.type = SYNTHVID_VERSION_REQUEST;
+	msg->vid_hdr.size = sizeof(struct synthvid_msg_hdr) +
+		sizeof(struct synthvid_version_req);
+	msg->ver_req.version = ver;
+	synthvid_send(hdev, msg);
+
+	t = wait_for_completion_timeout(&par->wait, VSP_TIMEOUT);
+	if (!t) {
+		pr_err("Time out on waiting version response\n");
+		ret = -ETIMEDOUT;
+		goto out;
+	}
+	if (!msg->ver_resp.is_accepted) {
+		ret = -ENODEV;
+		goto out;
+	}
+
+	par->synthvid_version = ver;
+
+out:
+	return ret;
+}
+
+/* Connect to VSP (Virtual Service Provider) on host */
+static int synthvid_connect_vsp(struct hv_device *hdev)
+{
+	struct fb_info *info = hv_get_drvdata(hdev);
+	struct hvfb_par *par = info->par;
+	int ret;
+
+	ret = vmbus_open(hdev->channel, RING_BUFSIZE, RING_BUFSIZE,
+			 NULL, 0, synthvid_receive, hdev);
+	if (ret) {
+		pr_err("Unable to open vmbus channel\n");
+		return ret;
+	}
+
+	/* Negotiate the protocol version with host */
+	if (vmbus_proto_version == VERSION_WS2008 ||
+	    vmbus_proto_version == VERSION_WIN7)
+		ret = synthvid_negotiate_ver(hdev, SYNTHVID_VERSION_WIN7);
+	else
+		ret = synthvid_negotiate_ver(hdev, SYNTHVID_VERSION_WIN8);
+
+	if (ret) {
+		pr_err("Synthetic video device version not accepted\n");
+		goto error;
+	}
+
+	if (par->synthvid_version == SYNTHVID_VERSION_WIN7) {
+		screen_depth = SYNTHVID_DEPTH_WIN7;
+		screen_fb_size = SYNTHVID_FB_SIZE_WIN7;
+	} else {
+		screen_depth = SYNTHVID_DEPTH_WIN8;
+		screen_fb_size = SYNTHVID_FB_SIZE_WIN8;
+	}
+
+	return 0;
+
+error:
+	vmbus_close(hdev->channel);
+	return ret;
+}
+
+/* Send VRAM and Situation messages to the host */
+static int synthvid_send_config(struct hv_device *hdev)
+{
+	struct fb_info *info = hv_get_drvdata(hdev);
+	struct hvfb_par *par = info->par;
+	struct synthvid_msg *msg = (struct synthvid_msg *)par->init_buf;
+	int t, ret = 0;
+
+	/* Send VRAM location */
+	memset(msg, 0, sizeof(struct synthvid_msg));
+	msg->vid_hdr.type = SYNTHVID_VRAM_LOCATION;
+	msg->vid_hdr.size = sizeof(struct synthvid_msg_hdr) +
+		sizeof(struct synthvid_vram_location);
+	msg->vram.user_ctx = msg->vram.vram_gpa = info->fix.smem_start;
+	msg->vram.is_vram_gpa_specified = 1;
+	synthvid_send(hdev, msg);
+
+	t = wait_for_completion_timeout(&par->wait, VSP_TIMEOUT);
+	if (!t) {
+		pr_err("Time out on waiting vram location ack\n");
+		ret = -ETIMEDOUT;
+		goto out;
+	}
+	if (msg->vram_ack.user_ctx != info->fix.smem_start) {
+		pr_err("Unable to set VRAM location\n");
+		ret = -ENODEV;
+		goto out;
+	}
+
+	/* Send pointer and situation update */
+	synthvid_send_ptr(hdev);
+	synthvid_send_situ(hdev);
+
+out:
+	return ret;
+}
+
+
+/*
+ * Delayed work callback:
+ * It is called at HVFB_UPDATE_DELAY or longer time interval to process
+ * screen updates. It is re-scheduled if further update is necessary.
+ */
+static void hvfb_update_work(struct work_struct *w)
+{
+	struct hvfb_par *par = container_of(w, struct hvfb_par, dwork.work);
+	struct fb_info *info = par->info;
+
+	if (par->fb_ready)
+		synthvid_update(info);
+
+	if (par->update)
+		schedule_delayed_work(&par->dwork, HVFB_UPDATE_DELAY);
+}
+
+
+/* Framebuffer operation handlers */
+
+static int hvfb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+	if (var->xres < HVFB_WIDTH_MIN || var->yres < HVFB_HEIGHT_MIN ||
+	    var->xres > screen_width || var->yres >  screen_height ||
+	    var->bits_per_pixel != screen_depth)
+		return -EINVAL;
+
+	var->xres_virtual = var->xres;
+	var->yres_virtual = var->yres;
+
+	return 0;
+}
+
+static int hvfb_set_par(struct fb_info *info)
+{
+	struct hv_device *hdev = device_to_hv_device(info->device);
+
+	return synthvid_send_situ(hdev);
+}
+
+
+static inline u32 chan_to_field(u32 chan, struct fb_bitfield *bf)
+{
+	return ((chan & 0xffff) >> (16 - bf->length)) << bf->offset;
+}
+
+static int hvfb_setcolreg(unsigned regno, unsigned red, unsigned green,
+			  unsigned blue, unsigned transp, struct fb_info *info)
+{
+	u32 *pal = info->pseudo_palette;
+
+	if (regno > 15)
+		return -EINVAL;
+
+	pal[regno] = chan_to_field(red, &info->var.red)
+		| chan_to_field(green, &info->var.green)
+		| chan_to_field(blue, &info->var.blue)
+		| chan_to_field(transp, &info->var.transp);
+
+	return 0;
+}
+
+
+static struct fb_ops hvfb_ops = {
+	.owner = THIS_MODULE,
+	.fb_check_var = hvfb_check_var,
+	.fb_set_par = hvfb_set_par,
+	.fb_setcolreg = hvfb_setcolreg,
+	.fb_fillrect = cfb_fillrect,
+	.fb_copyarea = cfb_copyarea,
+	.fb_imageblit = cfb_imageblit,
+};
+
+
+/* Get options from kernel paramenter "video=" */
+static void hvfb_get_option(struct fb_info *info)
+{
+	struct hvfb_par *par = info->par;
+	char *opt = NULL, *p;
+	uint x = 0, y = 0;
+
+	if (fb_get_options(KBUILD_MODNAME, &opt) || !opt || !*opt)
+		return;
+
+	p = strsep(&opt, "x");
+	if (!*p || kstrtouint(p, 0, &x) ||
+	    !opt || !*opt || kstrtouint(opt, 0, &y)) {
+		pr_err("Screen option is invalid: skipped\n");
+		return;
+	}
+
+	if (x < HVFB_WIDTH_MIN || y < HVFB_HEIGHT_MIN ||
+	    (par->synthvid_version == SYNTHVID_VERSION_WIN8 &&
+	     x * y * screen_depth / 8 > SYNTHVID_FB_SIZE_WIN8) ||
+	    (par->synthvid_version == SYNTHVID_VERSION_WIN7 &&
+	     (x > SYNTHVID_WIDTH_MAX_WIN7 || y > SYNTHVID_HEIGHT_MAX_WIN7))) {
+		pr_err("Screen resolution option is out of range: skipped\n");
+		return;
+	}
+
+	screen_width = x;
+	screen_height = y;
+	return;
+}
+
+
+/* Get framebuffer memory from Hyper-V video pci space */
+static int hvfb_getmem(struct fb_info *info)
+{
+	struct pci_dev *pdev;
+	ulong fb_phys;
+	void __iomem *fb_virt;
+
+	pdev = pci_get_device(PCI_VENDOR_ID_MICROSOFT,
+			      PCI_DEVICE_ID_HYPERV_VIDEO, NULL);
+	if (!pdev) {
+		pr_err("Unable to find PCI Hyper-V video\n");
+		return -ENODEV;
+	}
+
+	if (!(pci_resource_flags(pdev, 0) & IORESOURCE_MEM) ||
+	    pci_resource_len(pdev, 0) < screen_fb_size)
+		goto err1;
+
+	fb_phys = pci_resource_end(pdev, 0) - screen_fb_size + 1;
+	if (!request_mem_region(fb_phys, screen_fb_size, KBUILD_MODNAME))
+		goto err1;
+
+	fb_virt = ioremap(fb_phys, screen_fb_size);
+	if (!fb_virt)
+		goto err2;
+
+	info->apertures = alloc_apertures(1);
+	if (!info->apertures)
+		goto err3;
+
+	info->apertures->ranges[0].base = pci_resource_start(pdev, 0);
+	info->apertures->ranges[0].size = pci_resource_len(pdev, 0);
+	info->fix.smem_start = fb_phys;
+	info->fix.smem_len = screen_fb_size;
+	info->screen_base = fb_virt;
+	info->screen_size = screen_fb_size;
+
+	pci_dev_put(pdev);
+	return 0;
+
+err3:
+	iounmap(fb_virt);
+err2:
+	release_mem_region(fb_phys, screen_fb_size);
+err1:
+	pci_dev_put(pdev);
+	return -ENOMEM;
+}
+
+/* Release the framebuffer */
+static void hvfb_putmem(struct fb_info *info)
+{
+	iounmap(info->screen_base);
+	release_mem_region(info->fix.smem_start, screen_fb_size);
+}
+
+
+static int hvfb_probe(struct hv_device *hdev,
+		      const struct hv_vmbus_device_id *dev_id)
+{
+	struct fb_info *info;
+	struct hvfb_par *par;
+	int ret;
+
+	info = framebuffer_alloc(sizeof(struct hvfb_par), &hdev->device);
+	if (!info) {
+		pr_err("No memory for framebuffer info\n");
+		return -ENOMEM;
+	}
+
+	par = info->par;
+	par->info = info;
+	par->fb_ready = false;
+	init_completion(&par->wait);
+	INIT_DELAYED_WORK(&par->dwork, hvfb_update_work);
+
+	/* Connect to VSP */
+	hv_set_drvdata(hdev, info);
+	ret = synthvid_connect_vsp(hdev);
+	if (ret) {
+		pr_err("Unable to connect to VSP\n");
+		goto error1;
+	}
+
+	ret = hvfb_getmem(info);
+	if (ret) {
+		pr_err("No memory for framebuffer\n");
+		goto error2;
+	}
+
+	hvfb_get_option(info);
+	pr_info("Screen resolution: %dx%d, Color depth: %d\n",
+		screen_width, screen_height, screen_depth);
+
+
+	/* Set up fb_info */
+	info->flags = FBINFO_DEFAULT;
+
+	info->var.xres_virtual = info->var.xres = screen_width;
+	info->var.yres_virtual = info->var.yres = screen_height;
+	info->var.bits_per_pixel = screen_depth;
+
+	if (info->var.bits_per_pixel == 16) {
+		info->var.red = (struct fb_bitfield){11, 5, 0};
+		info->var.green = (struct fb_bitfield){5, 6, 0};
+		info->var.blue = (struct fb_bitfield){0, 5, 0};
+		info->var.transp = (struct fb_bitfield){0, 0, 0};
+	} else {
+		info->var.red = (struct fb_bitfield){16, 8, 0};
+		info->var.green = (struct fb_bitfield){8, 8, 0};
+		info->var.blue = (struct fb_bitfield){0, 8, 0};
+		info->var.transp = (struct fb_bitfield){24, 8, 0};
+	}
+
+	info->var.activate = FB_ACTIVATE_NOW;
+	info->var.height = -1;
+	info->var.width = -1;
+	info->var.vmode = FB_VMODE_NONINTERLACED;
+
+	strcpy(info->fix.id, KBUILD_MODNAME);
+	info->fix.type = FB_TYPE_PACKED_PIXELS;
+	info->fix.visual = FB_VISUAL_TRUECOLOR;
+	info->fix.line_length = screen_width * screen_depth / 8;
+	info->fix.accel = FB_ACCEL_NONE;
+
+	info->fbops = &hvfb_ops;
+	info->pseudo_palette = par->pseudo_palette;
+
+	/* Send config to host */
+	ret = synthvid_send_config(hdev);
+	if (ret)
+		goto error;
+
+	ret = register_framebuffer(info);
+	if (ret) {
+		pr_err("Unable to register framebuffer\n");
+		goto error;
+	}
+
+	par->fb_ready = true;
+
+	return 0;
+
+error:
+	hvfb_putmem(info);
+error2:
+	vmbus_close(hdev->channel);
+error1:
+	cancel_delayed_work_sync(&par->dwork);
+	hv_set_drvdata(hdev, NULL);
+	framebuffer_release(info);
+	return ret;
+}
+
+
+static int hvfb_remove(struct hv_device *hdev)
+{
+	struct fb_info *info = hv_get_drvdata(hdev);
+	struct hvfb_par *par = info->par;
+
+	par->update = false;
+	par->fb_ready = false;
+
+	unregister_framebuffer(info);
+	cancel_delayed_work_sync(&par->dwork);
+
+	vmbus_close(hdev->channel);
+	hv_set_drvdata(hdev, NULL);
+
+	hvfb_putmem(info);
+	framebuffer_release(info);
+
+	return 0;
+}
+
+
+static const struct hv_vmbus_device_id id_table[] = {
+	/* Synthetic Video Device GUID */
+	{HV_SYNTHVID_GUID},
+	{}
+};
+
+MODULE_DEVICE_TABLE(vmbus, id_table);
+
+static struct hv_driver hvfb_drv = {
+	.name = KBUILD_MODNAME,
+	.id_table = id_table,
+	.probe = hvfb_probe,
+	.remove = hvfb_remove,
+};
+
+
+static int __init hvfb_drv_init(void)
+{
+	return vmbus_driver_register(&hvfb_drv);
+}
+
+static void __exit hvfb_drv_exit(void)
+{
+	vmbus_driver_unregister(&hvfb_drv);
+}
+
+module_init(hvfb_drv_init);
+module_exit(hvfb_drv_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_VERSION(HV_DRV_VERSION);
+MODULE_DESCRIPTION("Microsoft Hyper-V Synthetic Video Frame Buffer Driver");
diff --git a/drivers/video/matrox/matroxfb_maven.c b/drivers/video/matrox/matroxfb_maven.c
index 217678e0b98..fd289745569 100644
--- a/drivers/video/matrox/matroxfb_maven.c
+++ b/drivers/video/matrox/matroxfb_maven.c
@@ -137,8 +137,20 @@ static int* get_ctrl_ptr(struct maven_data* md, int idx) {
 
 static int maven_get_reg(struct i2c_client* c, char reg) {
 	char dst;
-	struct i2c_msg msgs[] = {{ c->addr, I2C_M_REV_DIR_ADDR, sizeof(reg), &reg },
-				 { c->addr, I2C_M_RD | I2C_M_NOSTART, sizeof(dst), &dst }};
+	struct i2c_msg msgs[] = {
+		{
+			.addr = c->addr,
+			.flags = I2C_M_REV_DIR_ADDR,
+			.len = sizeof(reg),
+			.buf = &reg
+		},
+		{
+			.addr = c->addr,
+			.flags = I2C_M_RD | I2C_M_NOSTART,
+			.len = sizeof(dst),
+			.buf = &dst
+		}
+	};
 	s32 err;
 
 	err = i2c_transfer(c->adapter, msgs, 2);
diff --git a/drivers/video/mmp/core.c b/drivers/video/mmp/core.c
index 9ed83419038..84de2632857 100644
--- a/drivers/video/mmp/core.c
+++ b/drivers/video/mmp/core.c
@@ -252,7 +252,5 @@ void mmp_unregister_path(struct mmp_path *path)
 
 	kfree(path);
 	mutex_unlock(&disp_lock);
-
-	dev_info(path->dev, "de-register %s\n", path->name);
 }
 EXPORT_SYMBOL_GPL(mmp_unregister_path);
diff --git a/drivers/video/mmp/hw/mmp_ctrl.h b/drivers/video/mmp/hw/mmp_ctrl.h
index 6408d8ef3ab..edd2002b0e9 100644
--- a/drivers/video/mmp/hw/mmp_ctrl.h
+++ b/drivers/video/mmp/hw/mmp_ctrl.h
@@ -961,56 +961,7 @@ struct lcd_regs {
 	LCD_TVG_CUTVLN : PN2_LCD_GRA_CUTVLN) : LCD_GRA_CUTVLN)
 
 /*
- * defined Video Memory Color format for DMA control 0 register
- * DMA0 bit[23:20]
- */
-#define VMODE_RGB565		0x0
-#define VMODE_RGB1555		0x1
-#define VMODE_RGB888PACKED	0x2
-#define VMODE_RGB888UNPACKED	0x3
-#define VMODE_RGBA888		0x4
-#define VMODE_YUV422PACKED	0x5
-#define VMODE_YUV422PLANAR	0x6
-#define VMODE_YUV420PLANAR	0x7
-#define VMODE_SMPNCMD		0x8
-#define VMODE_PALETTE4BIT	0x9
-#define VMODE_PALETTE8BIT	0xa
-#define VMODE_RESERVED		0xb
-
-/*
- * defined Graphic Memory Color format for DMA control 0 register
- * DMA0 bit[19:16]
- */
-#define GMODE_RGB565		0x0
-#define GMODE_RGB1555		0x1
-#define GMODE_RGB888PACKED	0x2
-#define GMODE_RGB888UNPACKED	0x3
-#define GMODE_RGBA888		0x4
-#define GMODE_YUV422PACKED	0x5
-#define GMODE_YUV422PLANAR	0x6
-#define GMODE_YUV420PLANAR	0x7
-#define GMODE_SMPNCMD		0x8
-#define GMODE_PALETTE4BIT	0x9
-#define GMODE_PALETTE8BIT	0xa
-#define GMODE_RESERVED		0xb
-
-/*
- * define for DMA control 1 register
- */
-#define DMA1_FRAME_TRIG		31 /* bit location */
-#define DMA1_VSYNC_MODE		28
-#define DMA1_VSYNC_INV		27
-#define DMA1_CKEY		24
-#define DMA1_CARRY		23
-#define DMA1_LNBUF_ENA		22
-#define DMA1_GATED_ENA		21
-#define DMA1_PWRDN_ENA		20
-#define DMA1_DSCALE		18
-#define DMA1_ALPHA_MODE		16
-#define DMA1_ALPHA		08
-#define DMA1_PXLCMD		00
-
-/*
+ * defined for Configure Dumb Mode
  * defined for Configure Dumb Mode
  * DUMB LCD Panel bit[31:28]
  */
@@ -1050,18 +1001,6 @@ struct lcd_regs {
 #define	 CFG_CYC_BURST_LEN16			(1<<4)
 #define	 CFG_CYC_BURST_LEN8			(0<<4)
 
-/*
- * defined Dumb Panel Clock Divider register
- * SCLK_Source bit[31]
- */
- /* 0: PLL clock select*/
-#define AXI_BUS_SEL			0x80000000
-#define CCD_CLK_SEL			0x40000000
-#define DCON_CLK_SEL			0x20000000
-#define ENA_CLK_INT_DIV			CONFIG_FB_DOVE_CLCD_SCLK_DIV
-#define IDLE_CLK_INT_DIV		0x1	  /* idle Integer Divider */
-#define DIS_CLK_INT_DIV			0x0	  /* Disable Integer Divider */
-
 /* SRAM ID */
 #define SRAMID_GAMMA_YR			0x0
 #define SRAMID_GAMMA_UG			0x1
@@ -1471,422 +1410,6 @@ struct dsi_regs {
 #define LVDS_FREQ_OFFSET_MODE_CK_DIV4_OUT	(0x1 << 1)
 #define LVDS_FREQ_OFFSET_MODE_EN		(0x1 << 0)
 
-/* VDMA */
-struct vdma_ch_regs {
-#define VDMA_DC_SADDR_1		0x320
-#define VDMA_DC_SADDR_2		0x3A0
-#define VDMA_DC_SZ_1		0x324
-#define VDMA_DC_SZ_2		0x3A4
-#define VDMA_CTRL_1		0x328
-#define VDMA_CTRL_2		0x3A8
-#define VDMA_SRC_SZ_1		0x32C
-#define VDMA_SRC_SZ_2		0x3AC
-#define VDMA_SA_1		0x330
-#define VDMA_SA_2		0x3B0
-#define VDMA_DA_1		0x334
-#define VDMA_DA_2		0x3B4
-#define VDMA_SZ_1		0x338
-#define VDMA_SZ_2		0x3B8
-	u32	dc_saddr;
-	u32	dc_size;
-	u32	ctrl;
-	u32	src_size;
-	u32	src_addr;
-	u32	dst_addr;
-	u32	dst_size;
-#define VDMA_PITCH_1		0x33C
-#define VDMA_PITCH_2		0x3BC
-#define VDMA_ROT_CTRL_1		0x340
-#define VDMA_ROT_CTRL_2		0x3C0
-#define VDMA_RAM_CTRL0_1	0x344
-#define VDMA_RAM_CTRL0_2	0x3C4
-#define VDMA_RAM_CTRL1_1	0x348
-#define VDMA_RAM_CTRL1_2	0x3C8
-	u32	pitch;
-	u32	rot_ctrl;
-	u32	ram_ctrl0;
-	u32	ram_ctrl1;
-
-};
-struct vdma_regs {
-#define VDMA_ARBR_CTRL		0x300
-#define VDMA_IRQR		0x304
-#define VDMA_IRQM		0x308
-#define VDMA_IRQS		0x30C
-#define VDMA_MDMA_ARBR_CTRL	0x310
-	u32	arbr_ctr;
-	u32	irq_raw;
-	u32	irq_mask;
-	u32	irq_status;
-	u32	mdma_arbr_ctrl;
-	u32	reserved[3];
-
-	struct vdma_ch_regs	ch1;
-	u32	reserved2[21];
-	struct vdma_ch_regs	ch2;
-};
-
-/* CMU */
-#define CMU_PIP_DE_H_CFG	0x0008
-#define CMU_PRI1_H_CFG		0x000C
-#define CMU_PRI2_H_CFG		0x0010
-#define CMU_ACE_MAIN_DE1_H_CFG	0x0014
-#define CMU_ACE_MAIN_DE2_H_CFG	0x0018
-#define CMU_ACE_PIP_DE1_H_CFG	0x001C
-#define CMU_ACE_PIP_DE2_H_CFG	0x0020
-#define CMU_PIP_DE_V_CFG	0x0024
-#define CMU_PRI_V_CFG		0x0028
-#define CMU_ACE_MAIN_DE_V_CFG	0x002C
-#define CMU_ACE_PIP_DE_V_CFG	0x0030
-#define CMU_BAR_0_CFG		0x0034
-#define CMU_BAR_1_CFG		0x0038
-#define CMU_BAR_2_CFG		0x003C
-#define CMU_BAR_3_CFG		0x0040
-#define CMU_BAR_4_CFG		0x0044
-#define CMU_BAR_5_CFG		0x0048
-#define CMU_BAR_6_CFG		0x004C
-#define CMU_BAR_7_CFG		0x0050
-#define CMU_BAR_8_CFG		0x0054
-#define CMU_BAR_9_CFG		0x0058
-#define CMU_BAR_10_CFG		0x005C
-#define CMU_BAR_11_CFG		0x0060
-#define CMU_BAR_12_CFG		0x0064
-#define CMU_BAR_13_CFG		0x0068
-#define CMU_BAR_14_CFG		0x006C
-#define CMU_BAR_15_CFG		0x0070
-#define CMU_BAR_CTRL		0x0074
-#define PATTERN_TOTAL		0x0078
-#define PATTERN_ACTIVE		0x007C
-#define PATTERN_FRONT_PORCH	0x0080
-#define PATTERN_BACK_PORCH	0x0084
-#define CMU_CLK_CTRL		0x0088
-
-#define CMU_ICSC_M_C0_L		0x0900
-#define CMU_ICSC_M_C0_H		0x0901
-#define CMU_ICSC_M_C1_L		0x0902
-#define CMU_ICSC_M_C1_H		0x0903
-#define CMU_ICSC_M_C2_L		0x0904
-#define CMU_ICSC_M_C2_H		0x0905
-#define CMU_ICSC_M_C3_L		0x0906
-#define CMU_ICSC_M_C3_H		0x0907
-#define CMU_ICSC_M_C4_L		0x0908
-#define CMU_ICSC_M_C4_H		0x0909
-#define CMU_ICSC_M_C5_L		0x090A
-#define CMU_ICSC_M_C5_H		0x090B
-#define CMU_ICSC_M_C6_L		0x090C
-#define CMU_ICSC_M_C6_H		0x090D
-#define CMU_ICSC_M_C7_L		0x090E
-#define CMU_ICSC_M_C7_H		0x090F
-#define CMU_ICSC_M_C8_L		0x0910
-#define CMU_ICSC_M_C8_H		0x0911
-#define CMU_ICSC_M_O1_0		0x0914
-#define CMU_ICSC_M_O1_1		0x0915
-#define CMU_ICSC_M_O1_2		0x0916
-#define CMU_ICSC_M_O2_0		0x0918
-#define CMU_ICSC_M_O2_1		0x0919
-#define CMU_ICSC_M_O2_2		0x091A
-#define CMU_ICSC_M_O3_0		0x091C
-#define CMU_ICSC_M_O3_1		0x091D
-#define CMU_ICSC_M_O3_2		0x091E
-#define CMU_ICSC_P_C0_L		0x0920
-#define CMU_ICSC_P_C0_H		0x0921
-#define CMU_ICSC_P_C1_L		0x0922
-#define CMU_ICSC_P_C1_H		0x0923
-#define CMU_ICSC_P_C2_L		0x0924
-#define CMU_ICSC_P_C2_H		0x0925
-#define CMU_ICSC_P_C3_L		0x0926
-#define CMU_ICSC_P_C3_H		0x0927
-#define CMU_ICSC_P_C4_L		0x0928
-#define CMU_ICSC_P_C4_H		0x0929
-#define CMU_ICSC_P_C5_L		0x092A
-#define CMU_ICSC_P_C5_H		0x092B
-#define CMU_ICSC_P_C6_L		0x092C
-#define CMU_ICSC_P_C6_H		0x092D
-#define CMU_ICSC_P_C7_L		0x092E
-#define CMU_ICSC_P_C7_H		0x092F
-#define CMU_ICSC_P_C8_L		0x0930
-#define CMU_ICSC_P_C8_H		0x0931
-#define CMU_ICSC_P_O1_0		0x0934
-#define CMU_ICSC_P_O1_1		0x0935
-#define CMU_ICSC_P_O1_2		0x0936
-#define CMU_ICSC_P_O2_0		0x0938
-#define CMU_ICSC_P_O2_1		0x0939
-#define CMU_ICSC_P_O2_2		0x093A
-#define CMU_ICSC_P_O3_0		0x093C
-#define CMU_ICSC_P_O3_1		0x093D
-#define CMU_ICSC_P_O3_2		0x093E
-#define CMU_BR_M_EN		0x0940
-#define CMU_BR_M_TH1_L		0x0942
-#define CMU_BR_M_TH1_H		0x0943
-#define CMU_BR_M_TH2_L		0x0944
-#define CMU_BR_M_TH2_H		0x0945
-#define CMU_ACE_M_EN		0x0950
-#define CMU_ACE_M_WFG1		0x0951
-#define CMU_ACE_M_WFG2		0x0952
-#define CMU_ACE_M_WFG3		0x0953
-#define CMU_ACE_M_TH0		0x0954
-#define CMU_ACE_M_TH1		0x0955
-#define CMU_ACE_M_TH2		0x0956
-#define CMU_ACE_M_TH3		0x0957
-#define CMU_ACE_M_TH4		0x0958
-#define CMU_ACE_M_TH5		0x0959
-#define CMU_ACE_M_OP0_L		0x095A
-#define CMU_ACE_M_OP0_H		0x095B
-#define CMU_ACE_M_OP5_L		0x095C
-#define CMU_ACE_M_OP5_H		0x095D
-#define CMU_ACE_M_GB2		0x095E
-#define CMU_ACE_M_GB3		0x095F
-#define CMU_ACE_M_MS1		0x0960
-#define CMU_ACE_M_MS2		0x0961
-#define CMU_ACE_M_MS3		0x0962
-#define CMU_BR_P_EN		0x0970
-#define CMU_BR_P_TH1_L		0x0972
-#define CMU_BR_P_TH1_H		0x0973
-#define CMU_BR_P_TH2_L		0x0974
-#define CMU_BR_P_TH2_H		0x0975
-#define CMU_ACE_P_EN		0x0980
-#define CMU_ACE_P_WFG1		0x0981
-#define CMU_ACE_P_WFG2		0x0982
-#define CMU_ACE_P_WFG3		0x0983
-#define CMU_ACE_P_TH0		0x0984
-#define CMU_ACE_P_TH1		0x0985
-#define CMU_ACE_P_TH2		0x0986
-#define CMU_ACE_P_TH3		0x0987
-#define CMU_ACE_P_TH4		0x0988
-#define CMU_ACE_P_TH5		0x0989
-#define CMU_ACE_P_OP0_L		0x098A
-#define CMU_ACE_P_OP0_H		0x098B
-#define CMU_ACE_P_OP5_L		0x098C
-#define CMU_ACE_P_OP5_H		0x098D
-#define CMU_ACE_P_GB2		0x098E
-#define CMU_ACE_P_GB3		0x098F
-#define CMU_ACE_P_MS1		0x0990
-#define CMU_ACE_P_MS2		0x0991
-#define CMU_ACE_P_MS3		0x0992
-#define CMU_FTDC_M_EN		0x09A0
-#define CMU_FTDC_P_EN		0x09A1
-#define CMU_FTDC_INLOW_L	0x09A2
-#define CMU_FTDC_INLOW_H	0x09A3
-#define CMU_FTDC_INHIGH_L	0x09A4
-#define CMU_FTDC_INHIGH_H	0x09A5
-#define CMU_FTDC_OUTLOW_L	0x09A6
-#define CMU_FTDC_OUTLOW_H	0x09A7
-#define CMU_FTDC_OUTHIGH_L	0x09A8
-#define CMU_FTDC_OUTHIGH_H	0x09A9
-#define CMU_FTDC_YLOW		0x09AA
-#define CMU_FTDC_YHIGH		0x09AB
-#define CMU_FTDC_CH1		0x09AC
-#define CMU_FTDC_CH2_L		0x09AE
-#define CMU_FTDC_CH2_H		0x09AF
-#define CMU_FTDC_CH3_L		0x09B0
-#define CMU_FTDC_CH3_H		0x09B1
-#define CMU_FTDC_1_C00_6	0x09B2
-#define CMU_FTDC_1_C01_6	0x09B8
-#define CMU_FTDC_1_C11_6	0x09BE
-#define CMU_FTDC_1_C10_6	0x09C4
-#define CMU_FTDC_1_OFF00_6	0x09CA
-#define CMU_FTDC_1_OFF10_6	0x09D0
-#define CMU_HS_M_EN		0x0A00
-#define CMU_HS_M_AX1_L		0x0A02
-#define CMU_HS_M_AX1_H		0x0A03
-#define CMU_HS_M_AX2_L		0x0A04
-#define CMU_HS_M_AX2_H		0x0A05
-#define CMU_HS_M_AX3_L		0x0A06
-#define CMU_HS_M_AX3_H		0x0A07
-#define CMU_HS_M_AX4_L		0x0A08
-#define CMU_HS_M_AX4_H		0x0A09
-#define CMU_HS_M_AX5_L		0x0A0A
-#define CMU_HS_M_AX5_H		0x0A0B
-#define CMU_HS_M_AX6_L		0x0A0C
-#define CMU_HS_M_AX6_H		0x0A0D
-#define CMU_HS_M_AX7_L		0x0A0E
-#define CMU_HS_M_AX7_H		0x0A0F
-#define CMU_HS_M_AX8_L		0x0A10
-#define CMU_HS_M_AX8_H		0x0A11
-#define CMU_HS_M_AX9_L		0x0A12
-#define CMU_HS_M_AX9_H		0x0A13
-#define CMU_HS_M_AX10_L		0x0A14
-#define CMU_HS_M_AX10_H		0x0A15
-#define CMU_HS_M_AX11_L		0x0A16
-#define CMU_HS_M_AX11_H		0x0A17
-#define CMU_HS_M_AX12_L		0x0A18
-#define CMU_HS_M_AX12_H		0x0A19
-#define CMU_HS_M_AX13_L		0x0A1A
-#define CMU_HS_M_AX13_H		0x0A1B
-#define CMU_HS_M_AX14_L		0x0A1C
-#define CMU_HS_M_AX14_H		0x0A1D
-#define CMU_HS_M_H1_H14		0x0A1E
-#define CMU_HS_M_S1_S14		0x0A2C
-#define CMU_HS_M_GL		0x0A3A
-#define CMU_HS_M_MAXSAT_RGB_Y_L	0x0A3C
-#define CMU_HS_M_MAXSAT_RGB_Y_H	0x0A3D
-#define CMU_HS_M_MAXSAT_RCR_L	0x0A3E
-#define CMU_HS_M_MAXSAT_RCR_H	0x0A3F
-#define CMU_HS_M_MAXSAT_RCB_L	0x0A40
-#define CMU_HS_M_MAXSAT_RCB_H	0x0A41
-#define CMU_HS_M_MAXSAT_GCR_L	0x0A42
-#define CMU_HS_M_MAXSAT_GCR_H	0x0A43
-#define CMU_HS_M_MAXSAT_GCB_L	0x0A44
-#define CMU_HS_M_MAXSAT_GCB_H	0x0A45
-#define CMU_HS_M_MAXSAT_BCR_L	0x0A46
-#define CMU_HS_M_MAXSAT_BCR_H	0x0A47
-#define CMU_HS_M_MAXSAT_BCB_L	0x0A48
-#define CMU_HS_M_MAXSAT_BCB_H	0x0A49
-#define CMU_HS_M_ROFF_L		0x0A4A
-#define CMU_HS_M_ROFF_H		0x0A4B
-#define CMU_HS_M_GOFF_L		0x0A4C
-#define CMU_HS_M_GOFF_H		0x0A4D
-#define CMU_HS_M_BOFF_L		0x0A4E
-#define CMU_HS_M_BOFF_H		0x0A4F
-#define CMU_HS_P_EN		0x0A50
-#define CMU_HS_P_AX1_L		0x0A52
-#define CMU_HS_P_AX1_H		0x0A53
-#define CMU_HS_P_AX2_L		0x0A54
-#define CMU_HS_P_AX2_H		0x0A55
-#define CMU_HS_P_AX3_L		0x0A56
-#define CMU_HS_P_AX3_H		0x0A57
-#define CMU_HS_P_AX4_L		0x0A58
-#define CMU_HS_P_AX4_H		0x0A59
-#define CMU_HS_P_AX5_L		0x0A5A
-#define CMU_HS_P_AX5_H		0x0A5B
-#define CMU_HS_P_AX6_L		0x0A5C
-#define CMU_HS_P_AX6_H		0x0A5D
-#define CMU_HS_P_AX7_L		0x0A5E
-#define CMU_HS_P_AX7_H		0x0A5F
-#define CMU_HS_P_AX8_L		0x0A60
-#define CMU_HS_P_AX8_H		0x0A61
-#define CMU_HS_P_AX9_L		0x0A62
-#define CMU_HS_P_AX9_H		0x0A63
-#define CMU_HS_P_AX10_L		0x0A64
-#define CMU_HS_P_AX10_H		0x0A65
-#define CMU_HS_P_AX11_L		0x0A66
-#define CMU_HS_P_AX11_H		0x0A67
-#define CMU_HS_P_AX12_L		0x0A68
-#define CMU_HS_P_AX12_H		0x0A69
-#define CMU_HS_P_AX13_L		0x0A6A
-#define CMU_HS_P_AX13_H		0x0A6B
-#define CMU_HS_P_AX14_L		0x0A6C
-#define CMU_HS_P_AX14_H		0x0A6D
-#define CMU_HS_P_H1_H14		0x0A6E
-#define CMU_HS_P_S1_S14		0x0A7C
-#define CMU_HS_P_GL		0x0A8A
-#define CMU_HS_P_MAXSAT_RGB_Y_L	0x0A8C
-#define CMU_HS_P_MAXSAT_RGB_Y_H	0x0A8D
-#define CMU_HS_P_MAXSAT_RCR_L	0x0A8E
-#define CMU_HS_P_MAXSAT_RCR_H	0x0A8F
-#define CMU_HS_P_MAXSAT_RCB_L	0x0A90
-#define CMU_HS_P_MAXSAT_RCB_H	0x0A91
-#define CMU_HS_P_MAXSAT_GCR_L	0x0A92
-#define CMU_HS_P_MAXSAT_GCR_H	0x0A93
-#define CMU_HS_P_MAXSAT_GCB_L	0x0A94
-#define CMU_HS_P_MAXSAT_GCB_H	0x0A95
-#define CMU_HS_P_MAXSAT_BCR_L	0x0A96
-#define CMU_HS_P_MAXSAT_BCR_H	0x0A97
-#define CMU_HS_P_MAXSAT_BCB_L	0x0A98
-#define CMU_HS_P_MAXSAT_BCB_H	0x0A99
-#define CMU_HS_P_ROFF_L		0x0A9A
-#define CMU_HS_P_ROFF_H		0x0A9B
-#define CMU_HS_P_GOFF_L		0x0A9C
-#define CMU_HS_P_GOFF_H		0x0A9D
-#define CMU_HS_P_BOFF_L		0x0A9E
-#define CMU_HS_P_BOFF_H		0x0A9F
-#define CMU_GLCSC_M_C0_L	0x0AA0
-#define CMU_GLCSC_M_C0_H	0x0AA1
-#define CMU_GLCSC_M_C1_L	0x0AA2
-#define CMU_GLCSC_M_C1_H	0x0AA3
-#define CMU_GLCSC_M_C2_L	0x0AA4
-#define CMU_GLCSC_M_C2_H	0x0AA5
-#define CMU_GLCSC_M_C3_L	0x0AA6
-#define CMU_GLCSC_M_C3_H	0x0AA7
-#define CMU_GLCSC_M_C4_L	0x0AA8
-#define CMU_GLCSC_M_C4_H	0x0AA9
-#define CMU_GLCSC_M_C5_L	0x0AAA
-#define CMU_GLCSC_M_C5_H	0x0AAB
-#define CMU_GLCSC_M_C6_L	0x0AAC
-#define CMU_GLCSC_M_C6_H	0x0AAD
-#define CMU_GLCSC_M_C7_L	0x0AAE
-#define CMU_GLCSC_M_C7_H	0x0AAF
-#define CMU_GLCSC_M_C8_L	0x0AB0
-#define CMU_GLCSC_M_C8_H	0x0AB1
-#define CMU_GLCSC_M_O1_1	0x0AB4
-#define CMU_GLCSC_M_O1_2	0x0AB5
-#define CMU_GLCSC_M_O1_3	0x0AB6
-#define CMU_GLCSC_M_O2_1	0x0AB8
-#define CMU_GLCSC_M_O2_2	0x0AB9
-#define CMU_GLCSC_M_O2_3	0x0ABA
-#define CMU_GLCSC_M_O3_1	0x0ABC
-#define CMU_GLCSC_M_O3_2	0x0ABD
-#define CMU_GLCSC_M_O3_3	0x0ABE
-#define CMU_GLCSC_P_C0_L	0x0AC0
-#define CMU_GLCSC_P_C0_H	0x0AC1
-#define CMU_GLCSC_P_C1_L	0x0AC2
-#define CMU_GLCSC_P_C1_H	0x0AC3
-#define CMU_GLCSC_P_C2_L	0x0AC4
-#define CMU_GLCSC_P_C2_H	0x0AC5
-#define CMU_GLCSC_P_C3_L	0x0AC6
-#define CMU_GLCSC_P_C3_H	0x0AC7
-#define CMU_GLCSC_P_C4_L	0x0AC8
-#define CMU_GLCSC_P_C4_H	0x0AC9
-#define CMU_GLCSC_P_C5_L	0x0ACA
-#define CMU_GLCSC_P_C5_H	0x0ACB
-#define CMU_GLCSC_P_C6_L	0x0ACC
-#define CMU_GLCSC_P_C6_H	0x0ACD
-#define CMU_GLCSC_P_C7_L	0x0ACE
-#define CMU_GLCSC_P_C7_H	0x0ACF
-#define CMU_GLCSC_P_C8_L	0x0AD0
-#define CMU_GLCSC_P_C8_H	0x0AD1
-#define CMU_GLCSC_P_O1_1	0x0AD4
-#define CMU_GLCSC_P_O1_2	0x0AD5
-#define CMU_GLCSC_P_O1_3	0x0AD6
-#define CMU_GLCSC_P_O2_1	0x0AD8
-#define CMU_GLCSC_P_O2_2	0x0AD9
-#define CMU_GLCSC_P_O2_3	0x0ADA
-#define CMU_GLCSC_P_O3_1	0x0ADC
-#define CMU_GLCSC_P_O3_2	0x0ADD
-#define CMU_GLCSC_P_O3_3	0x0ADE
-#define CMU_PIXVAL_M_EN		0x0AE0
-#define CMU_PIXVAL_P_EN		0x0AE1
-
-#define CMU_CLK_CTRL_TCLK	0x0
-#define CMU_CLK_CTRL_SCLK	0x2
-#define CMU_CLK_CTRL_MSK	0x2
-#define CMU_CLK_CTRL_ENABLE	0x1
-
-#define LCD_TOP_CTRL_TV		0x2
-#define LCD_TOP_CTRL_PN		0x0
-#define LCD_TOP_CTRL_SEL_MSK	0x2
-#define LCD_IO_CMU_IN_SEL_MSK	(0x3 << 20)
-#define LCD_IO_CMU_IN_SEL_TV	0
-#define LCD_IO_CMU_IN_SEL_PN	1
-#define LCD_IO_CMU_IN_SEL_PN2	2
-#define LCD_IO_TV_OUT_SEL_MSK	(0x3 << 26)
-#define LCD_IO_PN_OUT_SEL_MSK	(0x3 << 24)
-#define LCD_IO_PN2_OUT_SEL_MSK	(0x3 << 28)
-#define LCD_IO_TV_OUT_SEL_NON	3
-#define LCD_IO_PN_OUT_SEL_NON	3
-#define LCD_IO_PN2_OUT_SEL_NON	3
-#define LCD_TOP_CTRL_CMU_ENABLE 0x1
-#define LCD_IO_OVERL_MSK	0xC00000
-#define LCD_IO_OVERL_TV		0x0
-#define LCD_IO_OVERL_LCD1	0x400000
-#define LCD_IO_OVERL_LCD2	0xC00000
-#define HINVERT_MSK		0x4
-#define VINVERT_MSK		0x8
-#define HINVERT_LEN		0x2
-#define VINVERT_LEN		0x3
-
-#define CMU_CTRL		0x88
-#define CMU_CTRL_A0_MSK		0x6
-#define CMU_CTRL_A0_TV		0x0
-#define CMU_CTRL_A0_LCD1	0x1
-#define CMU_CTRL_A0_LCD2	0x2
-#define CMU_CTRL_A0_HDMI	0x3
-
-#define ICR_DRV_ROUTE_OFF	0x0
-#define ICR_DRV_ROUTE_TV	0x1
-#define ICR_DRV_ROUTE_LCD1	0x2
-#define ICR_DRV_ROUTE_LCD2	0x3
-
 enum {
 	PATH_PN = 0,
 	PATH_TV,
diff --git a/drivers/video/mxsfb.c b/drivers/video/mxsfb.c
index 755556ca5b2..45169cbaba6 100644
--- a/drivers/video/mxsfb.c
+++ b/drivers/video/mxsfb.c
@@ -169,6 +169,7 @@ struct mxsfb_info {
 	unsigned dotclk_delay;
 	const struct mxsfb_devdata *devdata;
 	int mapped;
+	u32 sync;
 };
 
 #define mxsfb_is_v3(host) (host->devdata->ipversion == 3)
@@ -456,9 +457,9 @@ static int mxsfb_set_par(struct fb_info *fb_info)
 		vdctrl0 |= VDCTRL0_HSYNC_ACT_HIGH;
 	if (fb_info->var.sync & FB_SYNC_VERT_HIGH_ACT)
 		vdctrl0 |= VDCTRL0_VSYNC_ACT_HIGH;
-	if (fb_info->var.sync & FB_SYNC_DATA_ENABLE_HIGH_ACT)
+	if (host->sync & MXSFB_SYNC_DATA_ENABLE_HIGH_ACT)
 		vdctrl0 |= VDCTRL0_ENABLE_ACT_HIGH;
-	if (fb_info->var.sync & FB_SYNC_DOTCLK_FAILING_ACT)
+	if (host->sync & MXSFB_SYNC_DOTCLK_FAILING_ACT)
 		vdctrl0 |= VDCTRL0_DOTCLK_ACT_FAILING;
 
 	writel(vdctrl0, host->base + LCDC_VDCTRL0);
@@ -861,6 +862,8 @@ static int mxsfb_probe(struct platform_device *pdev)
 
 	INIT_LIST_HEAD(&fb_info->modelist);
 
+	host->sync = pdata->sync;
+
 	ret = mxsfb_init_fbinfo(host);
 	if (ret != 0)
 		goto error_init_fb;
diff --git a/drivers/video/of_display_timing.c b/drivers/video/of_display_timing.c
index 13ecd989701..56009bc02b0 100644
--- a/drivers/video/of_display_timing.c
+++ b/drivers/video/of_display_timing.c
@@ -79,25 +79,24 @@ static struct display_timing *of_get_display_timing(struct device_node *np)
 	ret |= parse_timing_property(np, "vsync-len", &dt->vsync_len);
 	ret |= parse_timing_property(np, "clock-frequency", &dt->pixelclock);
 
-	dt->dmt_flags = 0;
-	dt->data_flags = 0;
+	dt->flags = 0;
 	if (!of_property_read_u32(np, "vsync-active", &val))
-		dt->dmt_flags |= val ? VESA_DMT_VSYNC_HIGH :
-				VESA_DMT_VSYNC_LOW;
+		dt->flags |= val ? DISPLAY_FLAGS_VSYNC_HIGH :
+				DISPLAY_FLAGS_VSYNC_LOW;
 	if (!of_property_read_u32(np, "hsync-active", &val))
-		dt->dmt_flags |= val ? VESA_DMT_HSYNC_HIGH :
-				VESA_DMT_HSYNC_LOW;
+		dt->flags |= val ? DISPLAY_FLAGS_HSYNC_HIGH :
+				DISPLAY_FLAGS_HSYNC_LOW;
 	if (!of_property_read_u32(np, "de-active", &val))
-		dt->data_flags |= val ? DISPLAY_FLAGS_DE_HIGH :
+		dt->flags |= val ? DISPLAY_FLAGS_DE_HIGH :
 				DISPLAY_FLAGS_DE_LOW;
 	if (!of_property_read_u32(np, "pixelclk-active", &val))
-		dt->data_flags |= val ? DISPLAY_FLAGS_PIXDATA_POSEDGE :
+		dt->flags |= val ? DISPLAY_FLAGS_PIXDATA_POSEDGE :
 				DISPLAY_FLAGS_PIXDATA_NEGEDGE;
 
 	if (of_property_read_bool(np, "interlaced"))
-		dt->data_flags |= DISPLAY_FLAGS_INTERLACED;
+		dt->flags |= DISPLAY_FLAGS_INTERLACED;
 	if (of_property_read_bool(np, "doublescan"))
-		dt->data_flags |= DISPLAY_FLAGS_DOUBLESCAN;
+		dt->flags |= DISPLAY_FLAGS_DOUBLESCAN;
 
 	if (ret) {
 		pr_err("%s: error reading timing properties\n",
diff --git a/drivers/video/of_videomode.c b/drivers/video/of_videomode.c
index 5b8066cd397..111c2d1911d 100644
--- a/drivers/video/of_videomode.c
+++ b/drivers/video/of_videomode.c
@@ -43,7 +43,7 @@ int of_get_videomode(struct device_node *np, struct videomode *vm,
 	if (index == OF_USE_NATIVE_MODE)
 		index = disp->native_mode;
 
-	ret = videomode_from_timing(disp, vm, index);
+	ret = videomode_from_timings(disp, vm, index);
 	if (ret)
 		return ret;
 
diff --git a/drivers/video/omap/Kconfig b/drivers/video/omap/Kconfig
index e512581300f..0bc3a936ce2 100644
--- a/drivers/video/omap/Kconfig
+++ b/drivers/video/omap/Kconfig
@@ -39,17 +39,6 @@ config FB_OMAP_LCD_MIPID
 	  the Mobile Industry Processor Interface DBI-C/DCS
 	  specification. (Supported LCDs: Philips LPH8923, Sharp LS041Y3)
 
-config FB_OMAP_CONSISTENT_DMA_SIZE
-	int "Consistent DMA memory size (MB)"
-	depends on FB_OMAP
-	range 1 14
-	default 2
-	help
-	  Increase the DMA consistent memory size according to your video
-	  memory needs, for example if you want to use multiple planes.
-	  The size must be 2MB aligned.
-	  If unsure say 1.
-
 config FB_OMAP_DMA_TUNE
         bool "Set DMA SDRAM access priority high"
         depends on FB_OMAP
diff --git a/drivers/video/omap/omapfb_main.c b/drivers/video/omap/omapfb_main.c
index e31f5b33b50..d40612c31a9 100644
--- a/drivers/video/omap/omapfb_main.c
+++ b/drivers/video/omap/omapfb_main.c
@@ -32,6 +32,8 @@
 
 #include <linux/omap-dma.h>
 
+#include <mach/hardware.h>
+
 #include "omapfb.h"
 #include "lcdc.h"
 
diff --git a/drivers/video/omap2/displays/panel-tpo-td043mtea1.c b/drivers/video/omap2/displays/panel-tpo-td043mtea1.c
index 6b6643911d2..048c98381ef 100644
--- a/drivers/video/omap2/displays/panel-tpo-td043mtea1.c
+++ b/drivers/video/omap2/displays/panel-tpo-td043mtea1.c
@@ -63,6 +63,9 @@ struct tpo_td043_device {
 	u32 power_on_resume:1;
 };
 
+/* used to pass spi_device from SPI to DSS portion of the driver */
+static struct tpo_td043_device *g_tpo_td043;
+
 static int tpo_td043_write(struct spi_device *spi, u8 addr, u8 data)
 {
 	struct spi_message	m;
@@ -403,7 +406,7 @@ static void tpo_td043_disable(struct omap_dss_device *dssdev)
 
 static int tpo_td043_probe(struct omap_dss_device *dssdev)
 {
-	struct tpo_td043_device *tpo_td043 = dev_get_drvdata(&dssdev->dev);
+	struct tpo_td043_device *tpo_td043 = g_tpo_td043;
 	int nreset_gpio = dssdev->reset_gpio;
 	int ret = 0;
 
@@ -440,6 +443,8 @@ static int tpo_td043_probe(struct omap_dss_device *dssdev)
 	if (ret)
 		dev_warn(&dssdev->dev, "failed to create sysfs files\n");
 
+	dev_set_drvdata(&dssdev->dev, tpo_td043);
+
 	return 0;
 
 fail_gpio_req:
@@ -505,6 +510,9 @@ static int tpo_td043_spi_probe(struct spi_device *spi)
 		return -ENODEV;
 	}
 
+	if (g_tpo_td043 != NULL)
+		return -EBUSY;
+
 	spi->bits_per_word = 16;
 	spi->mode = SPI_MODE_0;
 
@@ -521,7 +529,7 @@ static int tpo_td043_spi_probe(struct spi_device *spi)
 	tpo_td043->spi = spi;
 	tpo_td043->nreset_gpio = dssdev->reset_gpio;
 	dev_set_drvdata(&spi->dev, tpo_td043);
-	dev_set_drvdata(&dssdev->dev, tpo_td043);
+	g_tpo_td043 = tpo_td043;
 
 	omap_dss_register_driver(&tpo_td043_driver);
 
@@ -534,6 +542,7 @@ static int tpo_td043_spi_remove(struct spi_device *spi)
 
 	omap_dss_unregister_driver(&tpo_td043_driver);
 	kfree(tpo_td043);
+	g_tpo_td043 = NULL;
 
 	return 0;
 }
diff --git a/drivers/video/omap2/dss/dss_features.c b/drivers/video/omap2/dss/dss_features.c
index d7d66ef5cb5..7f791aeda4d 100644
--- a/drivers/video/omap2/dss/dss_features.c
+++ b/drivers/video/omap2/dss/dss_features.c
@@ -202,12 +202,10 @@ static const enum omap_dss_output_id omap3630_dss_supported_outputs[] = {
 
 static const enum omap_dss_output_id omap4_dss_supported_outputs[] = {
 	/* OMAP_DSS_CHANNEL_LCD */
-	OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
-	OMAP_DSS_OUTPUT_DSI1,
+	OMAP_DSS_OUTPUT_DBI | OMAP_DSS_OUTPUT_DSI1,
 
 	/* OMAP_DSS_CHANNEL_DIGIT */
-	OMAP_DSS_OUTPUT_VENC | OMAP_DSS_OUTPUT_HDMI |
-	OMAP_DSS_OUTPUT_DPI,
+	OMAP_DSS_OUTPUT_VENC | OMAP_DSS_OUTPUT_HDMI,
 
 	/* OMAP_DSS_CHANNEL_LCD2 */
 	OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
diff --git a/drivers/video/omap2/omapfb/omapfb-main.c b/drivers/video/omap2/omapfb/omapfb-main.c
index ca585ef37f2..717f13a9335 100644
--- a/drivers/video/omap2/omapfb/omapfb-main.c
+++ b/drivers/video/omap2/omapfb/omapfb-main.c
@@ -1101,41 +1101,25 @@ static int omapfb_mmap(struct fb_info *fbi, struct vm_area_struct *vma)
 	struct omapfb_info *ofbi = FB2OFB(fbi);
 	struct fb_fix_screeninfo *fix = &fbi->fix;
 	struct omapfb2_mem_region *rg;
-	unsigned long off;
 	unsigned long start;
 	u32 len;
-	int r = -EINVAL;
-
-	if (vma->vm_end - vma->vm_start == 0)
-		return 0;
-	if (vma->vm_pgoff > (~0UL >> PAGE_SHIFT))
-		return -EINVAL;
-	off = vma->vm_pgoff << PAGE_SHIFT;
+	int r;
 
 	rg = omapfb_get_mem_region(ofbi->region);
 
 	start = omapfb_get_region_paddr(ofbi);
 	len = fix->smem_len;
-	if (off >= len)
-		goto error;
-	if ((vma->vm_end - vma->vm_start + off) > len)
-		goto error;
-
-	off += start;
 
-	DBG("user mmap region start %lx, len %d, off %lx\n", start, len, off);
+	DBG("user mmap region start %lx, len %d, off %lx\n", start, len,
+			vma->vm_pgoff << PAGE_SHIFT);
 
-	vma->vm_pgoff = off >> PAGE_SHIFT;
-	/* VM_IO | VM_DONTEXPAND | VM_DONTDUMP are set by remap_pfn_range() */
 	vma->vm_page_prot = pgprot_writecombine(vma->vm_page_prot);
 	vma->vm_ops = &mmap_user_ops;
 	vma->vm_private_data = rg;
-	if (io_remap_pfn_range(vma, vma->vm_start, off >> PAGE_SHIFT,
-			       vma->vm_end - vma->vm_start,
-			       vma->vm_page_prot)) {
-		r = -EAGAIN;
+
+	r = vm_iomap_memory(vma, start, len);
+	if (r)
 		goto error;
-	}
 
 	/* vm_ops.open won't be called for mmap itself. */
 	atomic_inc(&rg->map_count);
@@ -1144,7 +1128,7 @@ static int omapfb_mmap(struct fb_info *fbi, struct vm_area_struct *vma)
 
 	return 0;
 
- error:
+error:
 	omapfb_put_mem_region(ofbi->region);
 
 	return r;
diff --git a/drivers/video/omap2/vrfb.c b/drivers/video/omap2/vrfb.c
index 10560efeb35..5261229c79a 100644
--- a/drivers/video/omap2/vrfb.c
+++ b/drivers/video/omap2/vrfb.c
@@ -397,18 +397,7 @@ static struct platform_driver vrfb_driver = {
 	.remove		= __exit_p(vrfb_remove),
 };
 
-static int __init vrfb_init(void)
-{
-	return platform_driver_probe(&vrfb_driver, &vrfb_probe);
-}
-
-static void __exit vrfb_exit(void)
-{
-	platform_driver_unregister(&vrfb_driver);
-}
-
-module_init(vrfb_init);
-module_exit(vrfb_exit);
+module_platform_driver_probe(vrfb_driver, vrfb_probe);
 
 MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ti.com>");
 MODULE_DESCRIPTION("OMAP VRFB");
diff --git a/drivers/video/ps3fb.c b/drivers/video/ps3fb.c
index 920c27bf394..d9f08c653d6 100644
--- a/drivers/video/ps3fb.c
+++ b/drivers/video/ps3fb.c
@@ -705,21 +705,15 @@ static int ps3fb_pan_display(struct fb_var_screeninfo *var,
 
 static int ps3fb_mmap(struct fb_info *info, struct vm_area_struct *vma)
 {
-	unsigned long size, offset;
+	int r;
 
-	size = vma->vm_end - vma->vm_start;
-	offset = vma->vm_pgoff << PAGE_SHIFT;
-	if (offset + size > info->fix.smem_len)
-		return -EINVAL;
-
-	offset += info->fix.smem_start;
-	if (remap_pfn_range(vma, vma->vm_start, offset >> PAGE_SHIFT,
-			    size, vma->vm_page_prot))
-		return -EAGAIN;
+	r = vm_iomap_memory(vma, info->fix.smem_start, info->fix.smem_len);
 
 	dev_dbg(info->device, "ps3fb: mmap framebuffer P(%lx)->V(%lx)\n",
-		offset, vma->vm_start);
-	return 0;
+		info->fix.smem_start + vma->vm_pgoff << PAGE_SHIFT,
+		vma->vm_start);
+
+	return r;
 }
 
     /*
diff --git a/drivers/video/s1d13xxxfb.c b/drivers/video/s1d13xxxfb.c
index 76d9053d88c..05c2dc3d4bc 100644
--- a/drivers/video/s1d13xxxfb.c
+++ b/drivers/video/s1d13xxxfb.c
@@ -862,7 +862,7 @@ static int s1d13xxxfb_probe(struct platform_device *pdev)
 		printk(KERN_INFO PFX
 			"unknown chip production id %i, revision %i\n",
 			prod_id, revision);
-		printk(KERN_INFO PFX "please contant maintainer\n");
+		printk(KERN_INFO PFX "please contact maintainer\n");
 		goto bail;
 	}
 
diff --git a/drivers/video/s3c-fb.c b/drivers/video/s3c-fb.c
index 968a62571df..2e7991c7ca0 100644
--- a/drivers/video/s3c-fb.c
+++ b/drivers/video/s3c-fb.c
@@ -24,10 +24,9 @@
 #include <linux/uaccess.h>
 #include <linux/interrupt.h>
 #include <linux/pm_runtime.h>
+#include <linux/platform_data/video_s3c.h>
 
 #include <video/samsung_fimd.h>
-#include <mach/map.h>
-#include <plat/fb.h>
 
 /* This driver will export a number of framebuffer interfaces depending
  * on the configuration passed in via the platform data. Each fb instance
diff --git a/drivers/video/sa1100fb.c b/drivers/video/sa1100fb.c
index cfbde5e85cb..f34c858642e 100644
--- a/drivers/video/sa1100fb.c
+++ b/drivers/video/sa1100fb.c
@@ -556,7 +556,7 @@ static int sa1100fb_mmap(struct fb_info *info,
 			 struct vm_area_struct *vma)
 {
 	struct sa1100fb_info *fbi = (struct sa1100fb_info *)info;
-	unsigned long start, len, off = vma->vm_pgoff << PAGE_SHIFT;
+	unsigned long off = vma->vm_pgoff << PAGE_SHIFT;
 
 	if (off < info->fix.smem_len) {
 		vma->vm_pgoff += 1; /* skip over the palette */
@@ -564,19 +564,9 @@ static int sa1100fb_mmap(struct fb_info *info,
 					     fbi->map_dma, fbi->map_size);
 	}
 
-	start = info->fix.mmio_start;
-	len = PAGE_ALIGN((start & ~PAGE_MASK) + info->fix.mmio_len);
-
-	if ((vma->vm_end - vma->vm_start + off) > len)
-		return -EINVAL;
-
-	off += start & PAGE_MASK;
-	vma->vm_pgoff = off >> PAGE_SHIFT;
-	vma->vm_flags |= VM_IO;
 	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
-	return io_remap_pfn_range(vma, vma->vm_start, off >> PAGE_SHIFT,
-				   vma->vm_end - vma->vm_start,
-				   vma->vm_page_prot);
+
+	return vm_iomap_memory(vma, info->fix.mmio_start, info->fix.mmio_len);
 }
 
 static struct fb_ops sa1100fb_ops = {
diff --git a/drivers/video/sgivwfb.c b/drivers/video/sgivwfb.c
index 2331fadc272..b2a8912f643 100644
--- a/drivers/video/sgivwfb.c
+++ b/drivers/video/sgivwfb.c
@@ -705,23 +705,17 @@ static int sgivwfb_setcolreg(u_int regno, u_int red, u_int green,
 static int sgivwfb_mmap(struct fb_info *info,
 			struct vm_area_struct *vma)
 {
-	unsigned long size = vma->vm_end - vma->vm_start;
-	unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
+	int r;
 
-	if (vma->vm_pgoff > (~0UL >> PAGE_SHIFT))
-		return -EINVAL;
-	if (offset + size > sgivwfb_mem_size)
-		return -EINVAL;
-	offset += sgivwfb_mem_phys;
 	pgprot_val(vma->vm_page_prot) =
-	    pgprot_val(vma->vm_page_prot) | _PAGE_PCD;
-	vma->vm_flags |= VM_IO;
-	if (remap_pfn_range(vma, vma->vm_start, offset >> PAGE_SHIFT,
-						size, vma->vm_page_prot))
-		return -EAGAIN;
+		pgprot_val(vma->vm_page_prot) | _PAGE_PCD;
+
+	r = vm_iomap_memory(vma, sgivwfb_mem_phys, sgivwfb_mem_size);
+
 	printk(KERN_DEBUG "sgivwfb: mmap framebuffer P(%lx)->V(%lx)\n",
 	       offset, vma->vm_start);
-	return 0;
+
+	return r;
 }
 
 int __init sgivwfb_setup(char *options)
diff --git a/drivers/video/sh_mipi_dsi.c b/drivers/video/sh_mipi_dsi.c
index 701b461cf8a..6cad53075e9 100644
--- a/drivers/video/sh_mipi_dsi.c
+++ b/drivers/video/sh_mipi_dsi.c
@@ -581,17 +581,7 @@ static struct platform_driver sh_mipi_driver = {
 	},
 };
 
-static int __init sh_mipi_init(void)
-{
-	return platform_driver_probe(&sh_mipi_driver, sh_mipi_probe);
-}
-module_init(sh_mipi_init);
-
-static void __exit sh_mipi_exit(void)
-{
-	platform_driver_unregister(&sh_mipi_driver);
-}
-module_exit(sh_mipi_exit);
+module_platform_driver_probe(sh_mipi_driver, sh_mipi_probe);
 
 MODULE_AUTHOR("Guennadi Liakhovetski <g.liakhovetski@gmx.de>");
 MODULE_DESCRIPTION("SuperH / ARM-shmobile MIPI DSI driver");
diff --git a/drivers/video/sh_mobile_hdmi.c b/drivers/video/sh_mobile_hdmi.c
index 930e550e752..bfe4728480f 100644
--- a/drivers/video/sh_mobile_hdmi.c
+++ b/drivers/video/sh_mobile_hdmi.c
@@ -1445,17 +1445,7 @@ static struct platform_driver sh_hdmi_driver = {
 	},
 };
 
-static int __init sh_hdmi_init(void)
-{
-	return platform_driver_probe(&sh_hdmi_driver, sh_hdmi_probe);
-}
-module_init(sh_hdmi_init);
-
-static void __exit sh_hdmi_exit(void)
-{
-	platform_driver_unregister(&sh_hdmi_driver);
-}
-module_exit(sh_hdmi_exit);
+module_platform_driver_probe(sh_hdmi_driver, sh_hdmi_probe);
 
 MODULE_AUTHOR("Guennadi Liakhovetski <g.liakhovetski@gmx.de>");
 MODULE_DESCRIPTION("SuperH / ARM-shmobile HDMI driver");
diff --git a/drivers/video/sh_mobile_lcdcfb.c b/drivers/video/sh_mobile_lcdcfb.c
index 63203acef81..0264704a52b 100644
--- a/drivers/video/sh_mobile_lcdcfb.c
+++ b/drivers/video/sh_mobile_lcdcfb.c
@@ -858,6 +858,7 @@ static void sh_mobile_lcdc_geometry(struct sh_mobile_lcdc_chan *ch)
 	tmp = ((mode->xres & 7) << 24) | ((display_h_total & 7) << 16)
 	    | ((mode->hsync_len & 7) << 8) | (hsync_pos & 7);
 	lcdc_write_chan(ch, LDHAJR, tmp);
+	lcdc_write_chan_mirror(ch, LDHAJR, tmp);
 }
 
 static void sh_mobile_lcdc_overlay_setup(struct sh_mobile_lcdc_overlay *ovl)
diff --git a/drivers/video/smscufx.c b/drivers/video/smscufx.c
index 97bd6620c36..b2b33fc1ac3 100644
--- a/drivers/video/smscufx.c
+++ b/drivers/video/smscufx.c
@@ -782,7 +782,11 @@ static int ufx_ops_mmap(struct fb_info *info, struct vm_area_struct *vma)
 	unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
 	unsigned long page, pos;
 
-	if (offset + size > info->fix.smem_len)
+	if (vma->vm_pgoff > (~0UL >> PAGE_SHIFT))
+		return -EINVAL;
+	if (size > info->fix.smem_len)
+		return -EINVAL;
+	if (offset > info->fix.smem_len - size)
 		return -EINVAL;
 
 	pos = (unsigned long)info->fix.smem_start + offset;
diff --git a/drivers/video/ssd1307fb.c b/drivers/video/ssd1307fb.c
index 395cb6a8d8f..9ef05d3ef68 100644
--- a/drivers/video/ssd1307fb.c
+++ b/drivers/video/ssd1307fb.c
@@ -1,5 +1,5 @@
 /*
- * Driver for the Solomon SSD1307 OLED controler
+ * Driver for the Solomon SSD1307 OLED controller
  *
  * Copyright 2012 Free Electrons
  *
@@ -392,6 +392,6 @@ static struct i2c_driver ssd1307fb_driver = {
 
 module_i2c_driver(ssd1307fb_driver);
 
-MODULE_DESCRIPTION("FB driver for the Solomon SSD1307 OLED controler");
+MODULE_DESCRIPTION("FB driver for the Solomon SSD1307 OLED controller");
 MODULE_AUTHOR("Maxime Ripard <maxime.ripard@free-electrons.com>");
 MODULE_LICENSE("GPL");
diff --git a/drivers/video/udlfb.c b/drivers/video/udlfb.c
index 86d449ea316..ec03e726c94 100644
--- a/drivers/video/udlfb.c
+++ b/drivers/video/udlfb.c
@@ -324,7 +324,11 @@ static int dlfb_ops_mmap(struct fb_info *info, struct vm_area_struct *vma)
 	unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
 	unsigned long page, pos;
 
-	if (offset + size > info->fix.smem_len)
+	if (vma->vm_pgoff > (~0UL >> PAGE_SHIFT))
+		return -EINVAL;
+	if (size > info->fix.smem_len)
+		return -EINVAL;
+	if (offset > info->fix.smem_len - size)
 		return -EINVAL;
 
 	pos = (unsigned long)info->fix.smem_start + offset;
diff --git a/drivers/video/uvesafb.c b/drivers/video/uvesafb.c
index b75db018648..e328a61b64b 100644
--- a/drivers/video/uvesafb.c
+++ b/drivers/video/uvesafb.c
@@ -166,7 +166,7 @@ static int uvesafb_exec(struct uvesafb_ktask *task)
 	memcpy(&m->id, &uvesafb_cn_id, sizeof(m->id));
 	m->seq = seq;
 	m->len = len;
-	m->ack = random32();
+	m->ack = prandom_u32();
 
 	/* uvesafb_task structure */
 	memcpy(m + 1, &task->t, sizeof(task->t));
@@ -1973,7 +1973,8 @@ static int uvesafb_init(void)
 			err = -ENOMEM;
 
 		if (err) {
-			platform_device_put(uvesafb_device);
+			if (uvesafb_device)
+				platform_device_put(uvesafb_device);
 			platform_driver_unregister(&uvesafb_driver);
 			cn_del_callback(&uvesafb_cn_id);
 			return err;
diff --git a/drivers/video/vermilion/vermilion.c b/drivers/video/vermilion/vermilion.c
index 0aa516fc59c..09a136633f3 100644
--- a/drivers/video/vermilion/vermilion.c
+++ b/drivers/video/vermilion/vermilion.c
@@ -1003,24 +1003,18 @@ static int vmlfb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
 static int vmlfb_mmap(struct fb_info *info, struct vm_area_struct *vma)
 {
 	struct vml_info *vinfo = container_of(info, struct vml_info, info);
-	unsigned long size = vma->vm_end - vma->vm_start;
 	unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
 	int ret;
 
-	if (vma->vm_pgoff > (~0UL >> PAGE_SHIFT))
-		return -EINVAL;
-	if (offset + size > vinfo->vram_contig_size)
-		return -EINVAL;
 	ret = vmlfb_vram_offset(vinfo, offset);
 	if (ret)
 		return -EINVAL;
-	offset += vinfo->vram_start;
+
 	pgprot_val(vma->vm_page_prot) |= _PAGE_PCD;
 	pgprot_val(vma->vm_page_prot) &= ~_PAGE_PWT;
-	if (remap_pfn_range(vma, vma->vm_start, offset >> PAGE_SHIFT,
-						size, vma->vm_page_prot))
-		return -EAGAIN;
-	return 0;
+
+	return vm_iomap_memory(vma, vinfo->vram_start,
+			vinfo->vram_contig_size);
 }
 
 static int vmlfb_sync(struct fb_info *info)
diff --git a/drivers/video/vfb.c b/drivers/video/vfb.c
index 8bc1f939894..ee5985efa15 100644
--- a/drivers/video/vfb.c
+++ b/drivers/video/vfb.c
@@ -420,9 +420,12 @@ static int vfb_mmap(struct fb_info *info,
 	unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
 	unsigned long page, pos;
 
-	if (offset + size > info->fix.smem_len) {
+	if (vma->vm_pgoff > (~0UL >> PAGE_SHIFT))
+		return -EINVAL;
+	if (size > info->fix.smem_len)
+		return -EINVAL;
+	if (offset > info->fix.smem_len - size)
 		return -EINVAL;
-	}
 
 	pos = (unsigned long)info->fix.smem_start + offset;
 
diff --git a/drivers/video/videomode.c b/drivers/video/videomode.c
index 21c47a202af..df375c96c5d 100644
--- a/drivers/video/videomode.c
+++ b/drivers/video/videomode.c
@@ -11,7 +11,25 @@
 #include <video/display_timing.h>
 #include <video/videomode.h>
 
-int videomode_from_timing(const struct display_timings *disp,
+void videomode_from_timing(const struct display_timing *dt,
+			  struct videomode *vm)
+{
+	vm->pixelclock = dt->pixelclock.typ;
+	vm->hactive = dt->hactive.typ;
+	vm->hfront_porch = dt->hfront_porch.typ;
+	vm->hback_porch = dt->hback_porch.typ;
+	vm->hsync_len = dt->hsync_len.typ;
+
+	vm->vactive = dt->vactive.typ;
+	vm->vfront_porch = dt->vfront_porch.typ;
+	vm->vback_porch = dt->vback_porch.typ;
+	vm->vsync_len = dt->vsync_len.typ;
+
+	vm->flags = dt->flags;
+}
+EXPORT_SYMBOL_GPL(videomode_from_timing);
+
+int videomode_from_timings(const struct display_timings *disp,
 			  struct videomode *vm, unsigned int index)
 {
 	struct display_timing *dt;
@@ -20,20 +38,8 @@ int videomode_from_timing(const struct display_timings *disp,
 	if (!dt)
 		return -EINVAL;
 
-	vm->pixelclock = display_timing_get_value(&dt->pixelclock, TE_TYP);
-	vm->hactive = display_timing_get_value(&dt->hactive, TE_TYP);
-	vm->hfront_porch = display_timing_get_value(&dt->hfront_porch, TE_TYP);
-	vm->hback_porch = display_timing_get_value(&dt->hback_porch, TE_TYP);
-	vm->hsync_len = display_timing_get_value(&dt->hsync_len, TE_TYP);
-
-	vm->vactive = display_timing_get_value(&dt->vactive, TE_TYP);
-	vm->vfront_porch = display_timing_get_value(&dt->vfront_porch, TE_TYP);
-	vm->vback_porch = display_timing_get_value(&dt->vback_porch, TE_TYP);
-	vm->vsync_len = display_timing_get_value(&dt->vsync_len, TE_TYP);
-
-	vm->dmt_flags = dt->dmt_flags;
-	vm->data_flags = dt->data_flags;
+	videomode_from_timing(dt, vm);
 
 	return 0;
 }
-EXPORT_SYMBOL_GPL(videomode_from_timing);
+EXPORT_SYMBOL_GPL(videomode_from_timings);
diff --git a/drivers/video/vt8500lcdfb.c b/drivers/video/vt8500lcdfb.c
index aa2579c2364..9547e1831e0 100644
--- a/drivers/video/vt8500lcdfb.c
+++ b/drivers/video/vt8500lcdfb.c
@@ -15,22 +15,21 @@
  * GNU General Public License for more details.
  */
 
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/errno.h>
-#include <linux/string.h>
-#include <linux/mm.h>
-#include <linux/slab.h>
 #include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/errno.h>
 #include <linux/fb.h>
 #include <linux/init.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
-#include <linux/dma-mapping.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/module.h>
 #include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/string.h>
 #include <linux/wait.h>
-
-#include <linux/platform_data/video-vt8500lcdfb.h>
+#include <video/of_display_timing.h>
 
 #include "vt8500lcdfb.h"
 #include "wmt_ge_rops.h"
@@ -277,11 +276,11 @@ static int vt8500lcd_probe(struct platform_device *pdev)
 {
 	struct vt8500lcd_info *fbi;
 	struct resource *res;
+	struct display_timings *disp_timing;
 	void *addr;
 	int irq, ret;
 
 	struct fb_videomode	of_mode;
-	struct device_node	*np;
 	u32			bpp;
 	dma_addr_t fb_mem_phys;
 	unsigned long fb_mem_len;
@@ -346,32 +345,18 @@ static int vt8500lcd_probe(struct platform_device *pdev)
 		goto failed_free_res;
 	}
 
-	np = of_parse_phandle(pdev->dev.of_node, "default-mode", 0);
-	if (!np) {
-		pr_err("%s: No display description in Device Tree\n", __func__);
-		ret = -EINVAL;
-		goto failed_free_res;
-	}
+	disp_timing = of_get_display_timings(pdev->dev.of_node);
+	if (!disp_timing)
+		return -EINVAL;
 
-	/*
-	 * This code is copied from Sascha Hauer's of_videomode helper
-	 * and can be replaced with a call to the helper once mainlined
-	 */
-	ret = 0;
-	ret |= of_property_read_u32(np, "hactive", &of_mode.xres);
-	ret |= of_property_read_u32(np, "vactive", &of_mode.yres);
-	ret |= of_property_read_u32(np, "hback-porch", &of_mode.left_margin);
-	ret |= of_property_read_u32(np, "hfront-porch", &of_mode.right_margin);
-	ret |= of_property_read_u32(np, "hsync-len", &of_mode.hsync_len);
-	ret |= of_property_read_u32(np, "vback-porch", &of_mode.upper_margin);
-	ret |= of_property_read_u32(np, "vfront-porch", &of_mode.lower_margin);
-	ret |= of_property_read_u32(np, "vsync-len", &of_mode.vsync_len);
-	ret |= of_property_read_u32(np, "bpp", &bpp);
-	if (ret) {
-		pr_err("%s: Unable to read display properties\n", __func__);
-		goto failed_free_res;
-	}
-	of_mode.vmode = FB_VMODE_NONINTERLACED;
+	ret = of_get_fb_videomode(pdev->dev.of_node, &of_mode,
+							OF_USE_NATIVE_MODE);
+	if (ret)
+		return ret;
+
+	ret = of_property_read_u32(pdev->dev.of_node, "bits-per-pixel", &bpp);
+	if (ret)
+		return ret;
 
 	/* try allocating the framebuffer */
 	fb_mem_len = of_mode.xres * of_mode.yres * 2 * (bpp / 8);
diff --git a/drivers/video/wm8505fb.c b/drivers/video/wm8505fb.c
index 4dd0580f96f..01f9ace068e 100644
--- a/drivers/video/wm8505fb.c
+++ b/drivers/video/wm8505fb.c
@@ -14,25 +14,25 @@
  * GNU General Public License for more details.
  */
 
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/errno.h>
-#include <linux/string.h>
-#include <linux/mm.h>
-#include <linux/slab.h>
 #include <linux/delay.h>
+#include <linux/dma-mapping.h>
 #include <linux/fb.h>
+#include <linux/errno.h>
+#include <linux/err.h>
 #include <linux/init.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
-#include <linux/dma-mapping.h>
-#include <linux/platform_device.h>
-#include <linux/wait.h>
+#include <linux/kernel.h>
+#include <linux/memblock.h>
+#include <linux/mm.h>
+#include <linux/module.h>
 #include <linux/of.h>
 #include <linux/of_fdt.h>
-#include <linux/memblock.h>
-
-#include <linux/platform_data/video-vt8500lcdfb.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <linux/wait.h>
+#include <video/of_display_timing.h>
 
 #include "wm8505fb_regs.h"
 #include "wmt_ge_rops.h"
@@ -263,26 +263,22 @@ static struct fb_ops wm8505fb_ops = {
 static int wm8505fb_probe(struct platform_device *pdev)
 {
 	struct wm8505fb_info	*fbi;
-	struct resource		*res;
+	struct resource	*res;
+	struct display_timings *disp_timing;
 	void			*addr;
 	int ret;
 
-	struct fb_videomode	of_mode;
-	struct device_node	*np;
+	struct fb_videomode	mode;
 	u32			bpp;
 	dma_addr_t fb_mem_phys;
 	unsigned long fb_mem_len;
 	void *fb_mem_virt;
 
-	ret = -ENOMEM;
-	fbi = NULL;
-
 	fbi = devm_kzalloc(&pdev->dev, sizeof(struct wm8505fb_info) +
 			sizeof(u32) * 16, GFP_KERNEL);
 	if (!fbi) {
 		dev_err(&pdev->dev, "Failed to initialize framebuffer device\n");
-		ret = -ENOMEM;
-		goto failed;
+		return -ENOMEM;
 	}
 
 	strcpy(fbi->fb.fix.id, DRIVER_NAME);
@@ -308,54 +304,23 @@ static int wm8505fb_probe(struct platform_device *pdev)
 	fbi->fb.pseudo_palette	= addr;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (res == NULL) {
-		dev_err(&pdev->dev, "no I/O memory resource defined\n");
-		ret = -ENODEV;
-		goto failed_fbi;
-	}
+	fbi->regbase = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(fbi->regbase))
+		return PTR_ERR(fbi->regbase);
 
-	res = request_mem_region(res->start, resource_size(res), DRIVER_NAME);
-	if (res == NULL) {
-		dev_err(&pdev->dev, "failed to request I/O memory\n");
-		ret = -EBUSY;
-		goto failed_fbi;
-	}
-
-	fbi->regbase = ioremap(res->start, resource_size(res));
-	if (fbi->regbase == NULL) {
-		dev_err(&pdev->dev, "failed to map I/O memory\n");
-		ret = -EBUSY;
-		goto failed_free_res;
-	}
+	disp_timing = of_get_display_timings(pdev->dev.of_node);
+	if (!disp_timing)
+		return -EINVAL;
 
-	np = of_parse_phandle(pdev->dev.of_node, "default-mode", 0);
-	if (!np) {
-		pr_err("%s: No display description in Device Tree\n", __func__);
-		ret = -EINVAL;
-		goto failed_free_res;
-	}
+	ret = of_get_fb_videomode(pdev->dev.of_node, &mode, OF_USE_NATIVE_MODE);
+	if (ret)
+		return ret;
 
-	/*
-	 * This code is copied from Sascha Hauer's of_videomode helper
-	 * and can be replaced with a call to the helper once mainlined
-	 */
-	ret = 0;
-	ret |= of_property_read_u32(np, "hactive", &of_mode.xres);
-	ret |= of_property_read_u32(np, "vactive", &of_mode.yres);
-	ret |= of_property_read_u32(np, "hback-porch", &of_mode.left_margin);
-	ret |= of_property_read_u32(np, "hfront-porch", &of_mode.right_margin);
-	ret |= of_property_read_u32(np, "hsync-len", &of_mode.hsync_len);
-	ret |= of_property_read_u32(np, "vback-porch", &of_mode.upper_margin);
-	ret |= of_property_read_u32(np, "vfront-porch", &of_mode.lower_margin);
-	ret |= of_property_read_u32(np, "vsync-len", &of_mode.vsync_len);
-	ret |= of_property_read_u32(np, "bpp", &bpp);
-	if (ret) {
-		pr_err("%s: Unable to read display properties\n", __func__);
-		goto failed_free_res;
-	}
+	ret = of_property_read_u32(pdev->dev.of_node, "bits-per-pixel", &bpp);
+	if (ret)
+		return ret;
 
-	of_mode.vmode = FB_VMODE_NONINTERLACED;
-	fb_videomode_to_var(&fbi->fb.var, &of_mode);
+	fb_videomode_to_var(&fbi->fb.var, &mode);
 
 	fbi->fb.var.nonstd		= 0;
 	fbi->fb.var.activate		= FB_ACTIVATE_NOW;
@@ -364,16 +329,16 @@ static int wm8505fb_probe(struct platform_device *pdev)
 	fbi->fb.var.width		= -1;
 
 	/* try allocating the framebuffer */
-	fb_mem_len = of_mode.xres * of_mode.yres * 2 * (bpp / 8);
-	fb_mem_virt = dma_alloc_coherent(&pdev->dev, fb_mem_len, &fb_mem_phys,
+	fb_mem_len = mode.xres * mode.yres * 2 * (bpp / 8);
+	fb_mem_virt = dmam_alloc_coherent(&pdev->dev, fb_mem_len, &fb_mem_phys,
 				GFP_KERNEL);
 	if (!fb_mem_virt) {
 		pr_err("%s: Failed to allocate framebuffer\n", __func__);
 		return -ENOMEM;
-	};
+	}
 
-	fbi->fb.var.xres_virtual	= of_mode.xres;
-	fbi->fb.var.yres_virtual	= of_mode.yres * 2;
+	fbi->fb.var.xres_virtual	= mode.xres;
+	fbi->fb.var.yres_virtual	= mode.yres * 2;
 	fbi->fb.var.bits_per_pixel	= bpp;
 
 	fbi->fb.fix.smem_start		= fb_mem_phys;
@@ -381,28 +346,29 @@ static int wm8505fb_probe(struct platform_device *pdev)
 	fbi->fb.screen_base		= fb_mem_virt;
 	fbi->fb.screen_size		= fb_mem_len;
 
+	fbi->contrast = 0x10;
+	ret = wm8505fb_set_par(&fbi->fb);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to set parameters\n");
+		return ret;
+	}
+
 	if (fb_alloc_cmap(&fbi->fb.cmap, 256, 0) < 0) {
 		dev_err(&pdev->dev, "Failed to allocate color map\n");
-		ret = -ENOMEM;
-		goto failed_free_io;
+		return -ENOMEM;
 	}
 
 	wm8505fb_init_hw(&fbi->fb);
 
-	fbi->contrast = 0x80;
-	ret = wm8505fb_set_par(&fbi->fb);
-	if (ret) {
-		dev_err(&pdev->dev, "Failed to set parameters\n");
-		goto failed_free_cmap;
-	}
-
 	platform_set_drvdata(pdev, fbi);
 
 	ret = register_framebuffer(&fbi->fb);
 	if (ret < 0) {
 		dev_err(&pdev->dev,
 			"Failed to register framebuffer device: %d\n", ret);
-		goto failed_free_cmap;
+		if (fbi->fb.cmap.len)
+			fb_dealloc_cmap(&fbi->fb.cmap);
+		return ret;
 	}
 
 	ret = device_create_file(&pdev->dev, &dev_attr_contrast);
@@ -416,25 +382,11 @@ static int wm8505fb_probe(struct platform_device *pdev)
 	       fbi->fb.fix.smem_start + fbi->fb.fix.smem_len - 1);
 
 	return 0;
-
-failed_free_cmap:
-	if (fbi->fb.cmap.len)
-		fb_dealloc_cmap(&fbi->fb.cmap);
-failed_free_io:
-	iounmap(fbi->regbase);
-failed_free_res:
-	release_mem_region(res->start, resource_size(res));
-failed_fbi:
-	platform_set_drvdata(pdev, NULL);
-	kfree(fbi);
-failed:
-	return ret;
 }
 
 static int wm8505fb_remove(struct platform_device *pdev)
 {
 	struct wm8505fb_info *fbi = platform_get_drvdata(pdev);
-	struct resource *res;
 
 	device_remove_file(&pdev->dev, &dev_attr_contrast);
 
@@ -445,13 +397,6 @@ static int wm8505fb_remove(struct platform_device *pdev)
 	if (fbi->fb.cmap.len)
 		fb_dealloc_cmap(&fbi->fb.cmap);
 
-	iounmap(fbi->regbase);
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	release_mem_region(res->start, resource_size(res));
-
-	kfree(fbi);
-
 	return 0;
 }
 
diff --git a/drivers/video/wmt_ge_rops.h b/drivers/video/wmt_ge_rops.h
index 87380751a44..f73ec6377a4 100644
--- a/drivers/video/wmt_ge_rops.h
+++ b/drivers/video/wmt_ge_rops.h
@@ -1,5 +1,28 @@
+#ifdef CONFIG_FB_WMT_GE_ROPS
+
 extern void wmt_ge_fillrect(struct fb_info *info,
 			    const struct fb_fillrect *rect);
 extern void wmt_ge_copyarea(struct fb_info *info,
 			    const struct fb_copyarea *area);
 extern int wmt_ge_sync(struct fb_info *info);
+
+#else
+
+static inline int wmt_ge_sync(struct fb_info *p)
+{
+	return 0;
+}
+
+static inline void wmt_ge_fillrect(struct fb_info *p,
+				    const struct fb_fillrect *rect)
+{
+	sys_fillrect(p, rect);
+}
+
+static inline void wmt_ge_copyarea(struct fb_info *p,
+				     const struct fb_copyarea *area)
+{
+	sys_copyarea(p, area);
+}
+
+#endif
diff --git a/drivers/w1/masters/mxc_w1.c b/drivers/w1/masters/mxc_w1.c
index 950d354d50e..47e12cfc2a5 100644
--- a/drivers/w1/masters/mxc_w1.c
+++ b/drivers/w1/masters/mxc_w1.c
@@ -121,9 +121,9 @@ static int mxc_w1_probe(struct platform_device *pdev)
 	mdev->clkdiv = (clk_get_rate(mdev->clk) / 1000000) - 1;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	mdev->regs = devm_request_and_ioremap(&pdev->dev, res);
-	if (!mdev->regs)
-		return -EBUSY;
+	mdev->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(mdev->regs))
+		return PTR_ERR(mdev->regs);
 
 	clk_prepare_enable(mdev->clk);
 	__raw_writeb(mdev->clkdiv, mdev->regs + MXC_W1_TIME_DIVIDER);
diff --git a/drivers/w1/slaves/Kconfig b/drivers/w1/slaves/Kconfig
index 762561fbabb..5e6a3c9e510 100644
--- a/drivers/w1/slaves/Kconfig
+++ b/drivers/w1/slaves/Kconfig
@@ -22,6 +22,16 @@ config W1_SLAVE_DS2408
 	  Say Y here if you want to use a 1-wire
 	  DS2408 8-Channel Addressable Switch device support
 
+config W1_SLAVE_DS2408_READBACK
+	bool "Read-back values written to DS2408's output register"
+	depends on W1_SLAVE_DS2408
+	default y
+	help
+	  Enabling this will cause the driver to read back the values written
+	  to the chip's output register in order to detect errors.
+
+	  This is slower but useful when debugging chips and/or busses.
+
 config W1_SLAVE_DS2413
 	tristate "Dual Channel Addressable Switch 0x3a family support (DS2413)"
 	help
diff --git a/drivers/w1/slaves/w1_ds2408.c b/drivers/w1/slaves/w1_ds2408.c
index 441ad3a3b58..e45eca1044b 100644
--- a/drivers/w1/slaves/w1_ds2408.c
+++ b/drivers/w1/slaves/w1_ds2408.c
@@ -178,6 +178,15 @@ static ssize_t w1_f29_write_output(
 		w1_write_block(sl->master, w1_buf, 3);
 
 		readBack = w1_read_8(sl->master);
+
+		if (readBack != W1_F29_SUCCESS_CONFIRM_BYTE) {
+			if (w1_reset_resume_command(sl->master))
+				goto error;
+			/* try again, the slave is ready for a command */
+			continue;
+		}
+
+#ifdef CONFIG_W1_SLAVE_DS2408_READBACK
 		/* here the master could read another byte which
 		   would be the PIO reg (the actual pin logic state)
 		   since in this driver we don't know which pins are
@@ -186,11 +195,6 @@ static ssize_t w1_f29_write_output(
 		if (w1_reset_resume_command(sl->master))
 			goto error;
 
-		if (readBack != 0xAA) {
-			/* try again, the slave is ready for a command */
-			continue;
-		}
-
 		/* go read back the output latches */
 		/* (the direct effect of the write above) */
 		w1_buf[0] = W1_F29_FUNC_READ_PIO_REGS;
@@ -198,7 +202,9 @@ static ssize_t w1_f29_write_output(
 		w1_buf[2] = 0;
 		w1_write_block(sl->master, w1_buf, 3);
 		/* read the result of the READ_PIO_REGS command */
-		if (w1_read_8(sl->master) == *buf) {
+		if (w1_read_8(sl->master) == *buf)
+#endif
+		{
 			/* success! */
 			mutex_unlock(&sl->master->bus_mutex);
 			dev_dbg(&sl->dev,
@@ -297,8 +303,7 @@ error:
 
 
 
-#define NB_SYSFS_BIN_FILES 6
-static struct bin_attribute w1_f29_sysfs_bin_files[NB_SYSFS_BIN_FILES] = {
+static struct bin_attribute w1_f29_sysfs_bin_files[] = {
 	{
 		.attr =	{
 			.name = "state",
@@ -357,7 +362,7 @@ static int w1_f29_add_slave(struct w1_slave *sl)
 	int err = 0;
 	int i;
 
-	for (i = 0; i < NB_SYSFS_BIN_FILES && !err; ++i)
+	for (i = 0; i < ARRAY_SIZE(w1_f29_sysfs_bin_files) && !err; ++i)
 		err = sysfs_create_bin_file(
 			&sl->dev.kobj,
 			&(w1_f29_sysfs_bin_files[i]));
@@ -371,7 +376,7 @@ static int w1_f29_add_slave(struct w1_slave *sl)
 static void w1_f29_remove_slave(struct w1_slave *sl)
 {
 	int i;
-	for (i = NB_SYSFS_BIN_FILES - 1; i >= 0; --i)
+	for (i = ARRAY_SIZE(w1_f29_sysfs_bin_files) - 1; i >= 0; --i)
 		sysfs_remove_bin_file(&sl->dev.kobj,
 			&(w1_f29_sysfs_bin_files[i]));
 }
diff --git a/drivers/watchdog/Kconfig b/drivers/watchdog/Kconfig
index 9fcc70c11ce..e89fc313397 100644
--- a/drivers/watchdog/Kconfig
+++ b/drivers/watchdog/Kconfig
@@ -117,7 +117,7 @@ config ARM_SP805_WATCHDOG
 
 config AT91RM9200_WATCHDOG
 	tristate "AT91RM9200 watchdog"
-	depends on ARCH_AT91
+	depends on ARCH_AT91RM9200
 	help
 	  Watchdog timer embedded into AT91RM9200 chips. This will reboot your
 	  system when the timeout is reached.
diff --git a/drivers/xen/Kconfig b/drivers/xen/Kconfig
index 5a32232cf7c..67af155cf60 100644
--- a/drivers/xen/Kconfig
+++ b/drivers/xen/Kconfig
@@ -182,7 +182,7 @@ config XEN_PRIVCMD
 
 config XEN_STUB
 	bool "Xen stub drivers"
-	depends on XEN && X86_64
+	depends on XEN && X86_64 && BROKEN
 	default n
 	help
 	  Allow kernel to install stub drivers, to reserve space for Xen drivers,
diff --git a/drivers/xen/events.c b/drivers/xen/events.c
index d17aa41a904..d8cc8127f19 100644
--- a/drivers/xen/events.c
+++ b/drivers/xen/events.c
@@ -85,8 +85,7 @@ enum xen_irq_type {
  * event channel - irq->event channel mapping
  * cpu - cpu this event channel is bound to
  * index - type-specific information:
- *    PIRQ - vector, with MSB being "needs EIO", or physical IRQ of the HVM
- *           guest, or GSI (real passthrough IRQ) of the device.
+ *    PIRQ - physical IRQ, GSI, flags, and owner domain
  *    VIRQ - virq number
  *    IPI - IPI vector
  *    EVTCHN -
@@ -105,7 +104,6 @@ struct irq_info {
 		struct {
 			unsigned short pirq;
 			unsigned short gsi;
-			unsigned char vector;
 			unsigned char flags;
 			uint16_t domid;
 		} pirq;
@@ -211,7 +209,6 @@ static void xen_irq_info_pirq_init(unsigned irq,
 				   unsigned short evtchn,
 				   unsigned short pirq,
 				   unsigned short gsi,
-				   unsigned short vector,
 				   uint16_t domid,
 				   unsigned char flags)
 {
@@ -221,7 +218,6 @@ static void xen_irq_info_pirq_init(unsigned irq,
 
 	info->u.pirq.pirq = pirq;
 	info->u.pirq.gsi = gsi;
-	info->u.pirq.vector = vector;
 	info->u.pirq.domid = domid;
 	info->u.pirq.flags = flags;
 }
@@ -403,11 +399,23 @@ static void unmask_evtchn(int port)
 
 	if (unlikely((cpu != cpu_from_evtchn(port))))
 		do_hypercall = 1;
-	else
+	else {
+		/*
+		 * Need to clear the mask before checking pending to
+		 * avoid a race with an event becoming pending.
+		 *
+		 * EVTCHNOP_unmask will only trigger an upcall if the
+		 * mask bit was set, so if a hypercall is needed
+		 * remask the event.
+		 */
+		sync_clear_bit(port, BM(&s->evtchn_mask[0]));
 		evtchn_pending = sync_test_bit(port, BM(&s->evtchn_pending[0]));
 
-	if (unlikely(evtchn_pending && xen_hvm_domain()))
-		do_hypercall = 1;
+		if (unlikely(evtchn_pending && xen_hvm_domain())) {
+			sync_set_bit(port, BM(&s->evtchn_mask[0]));
+			do_hypercall = 1;
+		}
+	}
 
 	/* Slow path (hypercall) if this is a non-local port or if this is
 	 * an hvm domain and an event is pending (hvm domains don't have
@@ -418,8 +426,6 @@ static void unmask_evtchn(int port)
 	} else {
 		struct vcpu_info *vcpu_info = __this_cpu_read(xen_vcpu);
 
-		sync_clear_bit(port, BM(&s->evtchn_mask[0]));
-
 		/*
 		 * The following is basically the equivalent of
 		 * 'hw_resend_irq'. Just like a real IO-APIC we 'lose
@@ -509,6 +515,9 @@ static void xen_free_irq(unsigned irq)
 {
 	struct irq_info *info = irq_get_handler_data(irq);
 
+	if (WARN_ON(!info))
+		return;
+
 	list_del(&info->list);
 
 	irq_set_handler_data(irq, NULL);
@@ -704,7 +713,7 @@ int xen_bind_pirq_gsi_to_irq(unsigned gsi,
 		goto out;
 	}
 
-	xen_irq_info_pirq_init(irq, 0, pirq, gsi, irq_op.vector, DOMID_SELF,
+	xen_irq_info_pirq_init(irq, 0, pirq, gsi, DOMID_SELF,
 			       shareable ? PIRQ_SHAREABLE : 0);
 
 	pirq_query_unmask(irq);
@@ -752,8 +761,7 @@ int xen_allocate_pirq_msi(struct pci_dev *dev, struct msi_desc *msidesc)
 }
 
 int xen_bind_pirq_msi_to_irq(struct pci_dev *dev, struct msi_desc *msidesc,
-			     int pirq, int vector, const char *name,
-			     domid_t domid)
+			     int pirq, const char *name, domid_t domid)
 {
 	int irq, ret;
 
@@ -766,7 +774,7 @@ int xen_bind_pirq_msi_to_irq(struct pci_dev *dev, struct msi_desc *msidesc,
 	irq_set_chip_and_handler_name(irq, &xen_pirq_chip, handle_edge_irq,
 			name);
 
-	xen_irq_info_pirq_init(irq, 0, pirq, 0, vector, domid, 0);
+	xen_irq_info_pirq_init(irq, 0, pirq, 0, domid, 0);
 	ret = irq_set_msi_desc(irq, msidesc);
 	if (ret < 0)
 		goto error_irq;
@@ -998,6 +1006,9 @@ static void unbind_from_irq(unsigned int irq)
 	int evtchn = evtchn_from_irq(irq);
 	struct irq_info *info = irq_get_handler_data(irq);
 
+	if (WARN_ON(!info))
+		return;
+
 	mutex_lock(&irq_mapping_update_lock);
 
 	if (info->refcnt > 0) {
@@ -1125,6 +1136,10 @@ int bind_ipi_to_irqhandler(enum ipi_vector ipi,
 
 void unbind_from_irqhandler(unsigned int irq, void *dev_id)
 {
+	struct irq_info *info = irq_get_handler_data(irq);
+
+	if (WARN_ON(!info))
+		return;
 	free_irq(irq, dev_id);
 	unbind_from_irq(irq);
 }
@@ -1306,7 +1321,7 @@ static void __xen_evtchn_do_upcall(void)
 {
 	int start_word_idx, start_bit_idx;
 	int word_idx, bit_idx;
-	int i;
+	int i, irq;
 	int cpu = get_cpu();
 	struct shared_info *s = HYPERVISOR_shared_info;
 	struct vcpu_info *vcpu_info = __this_cpu_read(xen_vcpu);
@@ -1314,6 +1329,8 @@ static void __xen_evtchn_do_upcall(void)
 
 	do {
 		xen_ulong_t pending_words;
+		xen_ulong_t pending_bits;
+		struct irq_desc *desc;
 
 		vcpu_info->evtchn_upcall_pending = 0;
 
@@ -1325,6 +1342,17 @@ static void __xen_evtchn_do_upcall(void)
 		 * selector flag. xchg_xen_ulong must contain an
 		 * appropriate barrier.
 		 */
+		if ((irq = per_cpu(virq_to_irq, cpu)[VIRQ_TIMER]) != -1) {
+			int evtchn = evtchn_from_irq(irq);
+			word_idx = evtchn / BITS_PER_LONG;
+			pending_bits = evtchn % BITS_PER_LONG;
+			if (active_evtchns(cpu, s, word_idx) & (1ULL << pending_bits)) {
+				desc = irq_to_desc(irq);
+				if (desc)
+					generic_handle_irq_desc(irq, desc);
+			}
+		}
+
 		pending_words = xchg_xen_ulong(&vcpu_info->evtchn_pending_sel, 0);
 
 		start_word_idx = __this_cpu_read(current_word_idx);
@@ -1333,7 +1361,6 @@ static void __xen_evtchn_do_upcall(void)
 		word_idx = start_word_idx;
 
 		for (i = 0; pending_words != 0; i++) {
-			xen_ulong_t pending_bits;
 			xen_ulong_t words;
 
 			words = MASK_LSBS(pending_words, word_idx);
@@ -1362,8 +1389,7 @@ static void __xen_evtchn_do_upcall(void)
 
 			do {
 				xen_ulong_t bits;
-				int port, irq;
-				struct irq_desc *desc;
+				int port;
 
 				bits = MASK_LSBS(pending_bits, bit_idx);
 
@@ -1436,6 +1462,9 @@ void rebind_evtchn_irq(int evtchn, int irq)
 {
 	struct irq_info *info = info_for_irq(irq);
 
+	if (WARN_ON(!info))
+		return;
+
 	/* Make sure the irq is masked, since the new event channel
 	   will also be masked. */
 	disable_irq(irq);
@@ -1709,7 +1738,12 @@ void xen_poll_irq(int irq)
 int xen_test_irq_shared(int irq)
 {
 	struct irq_info *info = info_for_irq(irq);
-	struct physdev_irq_status_query irq_status = { .irq = info->u.pirq.pirq };
+	struct physdev_irq_status_query irq_status;
+
+	if (WARN_ON(!info))
+		return -ENOENT;
+
+	irq_status.irq = info->u.pirq.pirq;
 
 	if (HYPERVISOR_physdev_op(PHYSDEVOP_irq_status_query, &irq_status))
 		return 0;
diff --git a/drivers/xen/fallback.c b/drivers/xen/fallback.c
index 0ef7c4d40f8..b04fb64c5a9 100644
--- a/drivers/xen/fallback.c
+++ b/drivers/xen/fallback.c
@@ -44,7 +44,7 @@ int xen_event_channel_op_compat(int cmd, void *arg)
 }
 EXPORT_SYMBOL_GPL(xen_event_channel_op_compat);
 
-int HYPERVISOR_physdev_op_compat(int cmd, void *arg)
+int xen_physdev_op_compat(int cmd, void *arg)
 {
 	struct physdev_op op;
 	int rc;
@@ -78,3 +78,4 @@ int HYPERVISOR_physdev_op_compat(int cmd, void *arg)
 
 	return rc;
 }
+EXPORT_SYMBOL_GPL(xen_physdev_op_compat);
diff --git a/drivers/xen/xen-acpi-processor.c b/drivers/xen/xen-acpi-processor.c
index f3278a6603c..8abd7d57903 100644
--- a/drivers/xen/xen-acpi-processor.c
+++ b/drivers/xen/xen-acpi-processor.c
@@ -25,6 +25,7 @@
 #include <linux/init.h>
 #include <linux/module.h>
 #include <linux/types.h>
+#include <linux/syscore_ops.h>
 #include <acpi/acpi_bus.h>
 #include <acpi/acpi_drivers.h>
 #include <acpi/processor.h>
@@ -51,9 +52,9 @@ static DEFINE_MUTEX(acpi_ids_mutex);
 /* Which ACPI ID we have processed from 'struct acpi_processor'. */
 static unsigned long *acpi_ids_done;
 /* Which ACPI ID exist in the SSDT/DSDT processor definitions. */
-static unsigned long __initdata *acpi_id_present;
+static unsigned long *acpi_id_present;
 /* And if there is an _CST definition (or a PBLK) for the ACPI IDs */
-static unsigned long __initdata *acpi_id_cst_present;
+static unsigned long *acpi_id_cst_present;
 
 static int push_cxx_to_hypervisor(struct acpi_processor *_pr)
 {
@@ -329,7 +330,7 @@ static unsigned int __init get_max_acpi_id(void)
  * for_each_[present|online]_cpu macros which are banded to the virtual
  * CPU amount.
  */
-static acpi_status __init
+static acpi_status
 read_acpi_id(acpi_handle handle, u32 lvl, void *context, void **rv)
 {
 	u32 acpi_id;
@@ -384,12 +385,16 @@ read_acpi_id(acpi_handle handle, u32 lvl, void *context, void **rv)
 
 	return AE_OK;
 }
-static int __init check_acpi_ids(struct acpi_processor *pr_backup)
+static int check_acpi_ids(struct acpi_processor *pr_backup)
 {
 
 	if (!pr_backup)
 		return -ENODEV;
 
+	if (acpi_id_present && acpi_id_cst_present)
+		/* OK, done this once .. skip to uploading */
+		goto upload;
+
 	/* All online CPUs have been processed at this stage. Now verify
 	 * whether in fact "online CPUs" == physical CPUs.
 	 */
@@ -408,6 +413,7 @@ static int __init check_acpi_ids(struct acpi_processor *pr_backup)
 			    read_acpi_id, NULL, NULL, NULL);
 	acpi_get_devices("ACPI0007", read_acpi_id, NULL, NULL);
 
+upload:
 	if (!bitmap_equal(acpi_id_present, acpi_ids_done, nr_acpi_bits)) {
 		unsigned int i;
 		for_each_set_bit(i, acpi_id_present, nr_acpi_bits) {
@@ -417,10 +423,7 @@ static int __init check_acpi_ids(struct acpi_processor *pr_backup)
 			(void)upload_pm_data(pr_backup);
 		}
 	}
-	kfree(acpi_id_present);
-	acpi_id_present = NULL;
-	kfree(acpi_id_cst_present);
-	acpi_id_cst_present = NULL;
+
 	return 0;
 }
 static int __init check_prereq(void)
@@ -467,10 +470,47 @@ static void free_acpi_perf_data(void)
 	free_percpu(acpi_perf_data);
 }
 
-static int __init xen_acpi_processor_init(void)
+static int xen_upload_processor_pm_data(void)
 {
 	struct acpi_processor *pr_backup = NULL;
 	unsigned int i;
+	int rc = 0;
+
+	pr_info(DRV_NAME "Uploading Xen processor PM info\n");
+
+	for_each_possible_cpu(i) {
+		struct acpi_processor *_pr;
+		_pr = per_cpu(processors, i /* APIC ID */);
+		if (!_pr)
+			continue;
+
+		if (!pr_backup) {
+			pr_backup = kzalloc(sizeof(struct acpi_processor), GFP_KERNEL);
+			if (pr_backup)
+				memcpy(pr_backup, _pr, sizeof(struct acpi_processor));
+		}
+		(void)upload_pm_data(_pr);
+	}
+
+	rc = check_acpi_ids(pr_backup);
+	kfree(pr_backup);
+
+	return rc;
+}
+
+static void xen_acpi_processor_resume(void)
+{
+	bitmap_zero(acpi_ids_done, nr_acpi_bits);
+	xen_upload_processor_pm_data();
+}
+
+static struct syscore_ops xap_syscore_ops = {
+	.resume	= xen_acpi_processor_resume,
+};
+
+static int __init xen_acpi_processor_init(void)
+{
+	unsigned int i;
 	int rc = check_prereq();
 
 	if (rc)
@@ -505,33 +545,21 @@ static int __init xen_acpi_processor_init(void)
 
 		pr = per_cpu(processors, i);
 		perf = per_cpu_ptr(acpi_perf_data, i);
+		if (!pr)
+			continue;
+
 		pr->performance = perf;
 		rc = acpi_processor_get_performance_info(pr);
 		if (rc)
 			goto err_out;
 	}
 
-	for_each_possible_cpu(i) {
-		struct acpi_processor *_pr;
-		_pr = per_cpu(processors, i /* APIC ID */);
-		if (!_pr)
-			continue;
-
-		if (!pr_backup) {
-			pr_backup = kzalloc(sizeof(struct acpi_processor), GFP_KERNEL);
-			if (pr_backup)
-				memcpy(pr_backup, _pr, sizeof(struct acpi_processor));
-		}
-		(void)upload_pm_data(_pr);
-	}
-	rc = check_acpi_ids(pr_backup);
-
-	kfree(pr_backup);
-	pr_backup = NULL;
-
+	rc = xen_upload_processor_pm_data();
 	if (rc)
 		goto err_unregister;
 
+	register_syscore_ops(&xap_syscore_ops);
+
 	return 0;
 err_unregister:
 	for_each_possible_cpu(i) {
@@ -549,7 +577,10 @@ static void __exit xen_acpi_processor_exit(void)
 {
 	int i;
 
+	unregister_syscore_ops(&xap_syscore_ops);
 	kfree(acpi_ids_done);
+	kfree(acpi_id_present);
+	kfree(acpi_id_cst_present);
 	for_each_possible_cpu(i) {
 		struct acpi_processor_performance *perf;
 		perf = per_cpu_ptr(acpi_perf_data, i);
diff --git a/drivers/xen/xen-pciback/pci_stub.c b/drivers/xen/xen-pciback/pci_stub.c
index 9204126f156..a2278ba7fb2 100644
--- a/drivers/xen/xen-pciback/pci_stub.c
+++ b/drivers/xen/xen-pciback/pci_stub.c
@@ -17,6 +17,7 @@
 #include <xen/events.h>
 #include <asm/xen/pci.h>
 #include <asm/xen/hypervisor.h>
+#include <xen/interface/physdev.h>
 #include "pciback.h"
 #include "conf_space.h"
 #include "conf_space_quirks.h"
@@ -85,37 +86,52 @@ static struct pcistub_device *pcistub_device_alloc(struct pci_dev *dev)
 static void pcistub_device_release(struct kref *kref)
 {
 	struct pcistub_device *psdev;
+	struct pci_dev *dev;
 	struct xen_pcibk_dev_data *dev_data;
 
 	psdev = container_of(kref, struct pcistub_device, kref);
-	dev_data = pci_get_drvdata(psdev->dev);
+	dev = psdev->dev;
+	dev_data = pci_get_drvdata(dev);
 
-	dev_dbg(&psdev->dev->dev, "pcistub_device_release\n");
+	dev_dbg(&dev->dev, "pcistub_device_release\n");
 
-	xen_unregister_device_domain_owner(psdev->dev);
+	xen_unregister_device_domain_owner(dev);
 
 	/* Call the reset function which does not take lock as this
 	 * is called from "unbind" which takes a device_lock mutex.
 	 */
-	__pci_reset_function_locked(psdev->dev);
-	if (pci_load_and_free_saved_state(psdev->dev,
-					  &dev_data->pci_saved_state)) {
-		dev_dbg(&psdev->dev->dev, "Could not reload PCI state\n");
-	} else
-		pci_restore_state(psdev->dev);
+	__pci_reset_function_locked(dev);
+	if (pci_load_and_free_saved_state(dev, &dev_data->pci_saved_state))
+		dev_dbg(&dev->dev, "Could not reload PCI state\n");
+	else
+		pci_restore_state(dev);
+
+	if (pci_find_capability(dev, PCI_CAP_ID_MSIX)) {
+		struct physdev_pci_device ppdev = {
+			.seg = pci_domain_nr(dev->bus),
+			.bus = dev->bus->number,
+			.devfn = dev->devfn
+		};
+		int err = HYPERVISOR_physdev_op(PHYSDEVOP_release_msix,
+						&ppdev);
+
+		if (err)
+			dev_warn(&dev->dev, "MSI-X release failed (%d)\n",
+				 err);
+	}
 
 	/* Disable the device */
-	xen_pcibk_reset_device(psdev->dev);
+	xen_pcibk_reset_device(dev);
 
 	kfree(dev_data);
-	pci_set_drvdata(psdev->dev, NULL);
+	pci_set_drvdata(dev, NULL);
 
 	/* Clean-up the device */
-	xen_pcibk_config_free_dyn_fields(psdev->dev);
-	xen_pcibk_config_free_dev(psdev->dev);
+	xen_pcibk_config_free_dyn_fields(dev);
+	xen_pcibk_config_free_dev(dev);
 
-	psdev->dev->dev_flags &= ~PCI_DEV_FLAGS_ASSIGNED;
-	pci_dev_put(psdev->dev);
+	dev->dev_flags &= ~PCI_DEV_FLAGS_ASSIGNED;
+	pci_dev_put(dev);
 
 	kfree(psdev);
 }
@@ -355,6 +371,19 @@ static int pcistub_init_device(struct pci_dev *dev)
 	if (err)
 		goto config_release;
 
+	if (pci_find_capability(dev, PCI_CAP_ID_MSIX)) {
+		struct physdev_pci_device ppdev = {
+			.seg = pci_domain_nr(dev->bus),
+			.bus = dev->bus->number,
+			.devfn = dev->devfn
+		};
+
+		err = HYPERVISOR_physdev_op(PHYSDEVOP_prepare_msix, &ppdev);
+		if (err)
+			dev_err(&dev->dev, "MSI-X preparation failed (%d)\n",
+				err);
+	}
+
 	/* We need the device active to save the state. */
 	dev_dbg(&dev->dev, "save state of device\n");
 	pci_save_state(dev);
diff --git a/fs/Makefile b/fs/Makefile
index 9d53192236f..3b2c76759ec 100644
--- a/fs/Makefile
+++ b/fs/Makefile
@@ -10,7 +10,7 @@ obj-y :=	open.o read_write.o file_table.o super.o \
 		ioctl.o readdir.o select.o fifo.o dcache.o inode.o \
 		attr.o bad_inode.o file.o filesystems.o namespace.o \
 		seq_file.o xattr.o libfs.o fs-writeback.o \
-		pnode.o drop_caches.o splice.o sync.o utimes.o \
+		pnode.o splice.o sync.o utimes.o \
 		stack.o fs_struct.o statfs.o
 
 ifeq ($(CONFIG_BLOCK),y)
@@ -49,6 +49,7 @@ obj-$(CONFIG_FS_POSIX_ACL)	+= posix_acl.o xattr_acl.o
 obj-$(CONFIG_NFS_COMMON)	+= nfs_common/
 obj-$(CONFIG_GENERIC_ACL)	+= generic_acl.o
 obj-$(CONFIG_COREDUMP)		+= coredump.o
+obj-$(CONFIG_SYSCTL)		+= drop_caches.o
 
 obj-$(CONFIG_FHANDLE)		+= fhandle.o
 
diff --git a/fs/aio.c b/fs/aio.c
index 3f941f2a305..1dc8786f458 100644
--- a/fs/aio.c
+++ b/fs/aio.c
@@ -1029,9 +1029,9 @@ static int aio_read_evt(struct kioctx *ioctx, struct io_event *ent)
 	spin_unlock(&info->ring_lock);
 
 out:
-	kunmap_atomic(ring);
 	dprintk("leaving aio_read_evt: %d  h%lu t%lu\n", ret,
 		 (unsigned long)ring->head, (unsigned long)ring->tail);
+	kunmap_atomic(ring);
 	return ret;
 }
 
diff --git a/fs/befs/btree.c b/fs/befs/btree.c
index a66c9b1136e..74e397db0b8 100644
--- a/fs/befs/btree.c
+++ b/fs/befs/btree.c
@@ -436,8 +436,7 @@ befs_btree_read(struct super_block *sb, befs_data_stream * ds,
 		goto error;
 	}
 
-	if ((this_node = (befs_btree_node *)
-	     kmalloc(sizeof (befs_btree_node), GFP_NOFS)) == NULL) {
+	if ((this_node = kmalloc(sizeof (befs_btree_node), GFP_NOFS)) == NULL) {
 		befs_error(sb, "befs_btree_read() failed to allocate %u "
 			   "bytes of memory", sizeof (befs_btree_node));
 		goto error;
diff --git a/fs/binfmt_elf.c b/fs/binfmt_elf.c
index 3939829f6c5..86af964c242 100644
--- a/fs/binfmt_elf.c
+++ b/fs/binfmt_elf.c
@@ -1137,6 +1137,7 @@ static unsigned long vma_dump_size(struct vm_area_struct *vma,
 			goto whole;
 		if (!(vma->vm_flags & VM_SHARED) && FILTER(HUGETLB_PRIVATE))
 			goto whole;
+		return 0;
 	}
 
 	/* Do not dump I/O mapped devices or special mappings */
diff --git a/fs/bio.c b/fs/bio.c
index bb5768f59b3..b96fc6ce485 100644
--- a/fs/bio.c
+++ b/fs/bio.c
@@ -1428,8 +1428,6 @@ void bio_endio(struct bio *bio, int error)
 	else if (!test_bit(BIO_UPTODATE, &bio->bi_flags))
 		error = -EIO;
 
-	trace_block_bio_complete(bio, error);
-
 	if (bio->bi_end_io)
 		bio->bi_end_io(bio, error);
 }
diff --git a/fs/block_dev.c b/fs/block_dev.c
index aea605c98ba..aae187a7f94 100644
--- a/fs/block_dev.c
+++ b/fs/block_dev.c
@@ -551,6 +551,7 @@ struct block_device *bdgrab(struct block_device *bdev)
 	ihold(bdev->bd_inode);
 	return bdev;
 }
+EXPORT_SYMBOL(bdgrab);
 
 long nr_blockdev_pages(void)
 {
diff --git a/fs/btrfs/ctree.c b/fs/btrfs/ctree.c
index ecd25a1b4e5..ca9d8f1a3bb 100644
--- a/fs/btrfs/ctree.c
+++ b/fs/btrfs/ctree.c
@@ -651,6 +651,8 @@ tree_mod_log_insert_root(struct btrfs_fs_info *fs_info,
 	if (tree_mod_dont_log(fs_info, NULL))
 		return 0;
 
+	__tree_mod_log_free_eb(fs_info, old_root);
+
 	ret = tree_mod_alloc(fs_info, flags, &tm);
 	if (ret < 0)
 		goto out;
@@ -736,7 +738,7 @@ tree_mod_log_search(struct btrfs_fs_info *fs_info, u64 start, u64 min_seq)
 static noinline void
 tree_mod_log_eb_copy(struct btrfs_fs_info *fs_info, struct extent_buffer *dst,
 		     struct extent_buffer *src, unsigned long dst_offset,
-		     unsigned long src_offset, int nr_items)
+		     unsigned long src_offset, int nr_items, int log_removal)
 {
 	int ret;
 	int i;
@@ -750,10 +752,12 @@ tree_mod_log_eb_copy(struct btrfs_fs_info *fs_info, struct extent_buffer *dst,
 	}
 
 	for (i = 0; i < nr_items; i++) {
-		ret = tree_mod_log_insert_key_locked(fs_info, src,
-						     i + src_offset,
-						     MOD_LOG_KEY_REMOVE);
-		BUG_ON(ret < 0);
+		if (log_removal) {
+			ret = tree_mod_log_insert_key_locked(fs_info, src,
+							i + src_offset,
+							MOD_LOG_KEY_REMOVE);
+			BUG_ON(ret < 0);
+		}
 		ret = tree_mod_log_insert_key_locked(fs_info, dst,
 						     i + dst_offset,
 						     MOD_LOG_KEY_ADD);
@@ -927,7 +931,6 @@ static noinline int update_ref_for_cow(struct btrfs_trans_handle *trans,
 			ret = btrfs_dec_ref(trans, root, buf, 1, 1);
 			BUG_ON(ret); /* -ENOMEM */
 		}
-		tree_mod_log_free_eb(root->fs_info, buf);
 		clean_tree_block(trans, root, buf);
 		*last_ref = 1;
 	}
@@ -1046,6 +1049,7 @@ static noinline int __btrfs_cow_block(struct btrfs_trans_handle *trans,
 		btrfs_set_node_ptr_generation(parent, parent_slot,
 					      trans->transid);
 		btrfs_mark_buffer_dirty(parent);
+		tree_mod_log_free_eb(root->fs_info, buf);
 		btrfs_free_tree_block(trans, root, buf, parent_start,
 				      last_ref);
 	}
@@ -1750,7 +1754,6 @@ static noinline int balance_level(struct btrfs_trans_handle *trans,
 			goto enospc;
 		}
 
-		tree_mod_log_free_eb(root->fs_info, root->node);
 		tree_mod_log_set_root_pointer(root, child);
 		rcu_assign_pointer(root->node, child);
 
@@ -2995,7 +2998,7 @@ static int push_node_left(struct btrfs_trans_handle *trans,
 		push_items = min(src_nritems - 8, push_items);
 
 	tree_mod_log_eb_copy(root->fs_info, dst, src, dst_nritems, 0,
-			     push_items);
+			     push_items, 1);
 	copy_extent_buffer(dst, src,
 			   btrfs_node_key_ptr_offset(dst_nritems),
 			   btrfs_node_key_ptr_offset(0),
@@ -3066,7 +3069,7 @@ static int balance_node_right(struct btrfs_trans_handle *trans,
 				      sizeof(struct btrfs_key_ptr));
 
 	tree_mod_log_eb_copy(root->fs_info, dst, src, 0,
-			     src_nritems - push_items, push_items);
+			     src_nritems - push_items, push_items, 1);
 	copy_extent_buffer(dst, src,
 			   btrfs_node_key_ptr_offset(0),
 			   btrfs_node_key_ptr_offset(src_nritems - push_items),
@@ -3218,12 +3221,18 @@ static noinline int split_node(struct btrfs_trans_handle *trans,
 	int mid;
 	int ret;
 	u32 c_nritems;
+	int tree_mod_log_removal = 1;
 
 	c = path->nodes[level];
 	WARN_ON(btrfs_header_generation(c) != trans->transid);
 	if (c == root->node) {
 		/* trying to split the root, lets make a new one */
 		ret = insert_new_root(trans, root, path, level + 1);
+		/*
+		 * removal of root nodes has been logged by
+		 * tree_mod_log_set_root_pointer due to locking
+		 */
+		tree_mod_log_removal = 0;
 		if (ret)
 			return ret;
 	} else {
@@ -3261,7 +3270,8 @@ static noinline int split_node(struct btrfs_trans_handle *trans,
 			    (unsigned long)btrfs_header_chunk_tree_uuid(split),
 			    BTRFS_UUID_SIZE);
 
-	tree_mod_log_eb_copy(root->fs_info, split, c, 0, mid, c_nritems - mid);
+	tree_mod_log_eb_copy(root->fs_info, split, c, 0, mid, c_nritems - mid,
+			     tree_mod_log_removal);
 	copy_extent_buffer(split, c,
 			   btrfs_node_key_ptr_offset(0),
 			   btrfs_node_key_ptr_offset(mid),
diff --git a/fs/btrfs/disk-io.c b/fs/btrfs/disk-io.c
index 7d84651e850..6d19a0a554a 100644
--- a/fs/btrfs/disk-io.c
+++ b/fs/btrfs/disk-io.c
@@ -1291,6 +1291,7 @@ struct btrfs_root *btrfs_create_tree(struct btrfs_trans_handle *trans,
 				      0, objectid, NULL, 0, 0, 0);
 	if (IS_ERR(leaf)) {
 		ret = PTR_ERR(leaf);
+		leaf = NULL;
 		goto fail;
 	}
 
@@ -1334,11 +1335,16 @@ struct btrfs_root *btrfs_create_tree(struct btrfs_trans_handle *trans,
 
 	btrfs_tree_unlock(leaf);
 
+	return root;
+
 fail:
-	if (ret)
-		return ERR_PTR(ret);
+	if (leaf) {
+		btrfs_tree_unlock(leaf);
+		free_extent_buffer(leaf);
+	}
+	kfree(root);
 
-	return root;
+	return ERR_PTR(ret);
 }
 
 static struct btrfs_root *alloc_log_tree(struct btrfs_trans_handle *trans,
@@ -3253,7 +3259,7 @@ void btrfs_free_fs_root(struct btrfs_fs_info *fs_info, struct btrfs_root *root)
 	if (btrfs_root_refs(&root->root_item) == 0)
 		synchronize_srcu(&fs_info->subvol_srcu);
 
-	if (fs_info->fs_state & BTRFS_SUPER_FLAG_ERROR) {
+	if (test_bit(BTRFS_FS_STATE_ERROR, &fs_info->fs_state)) {
 		btrfs_free_log(NULL, root);
 		btrfs_free_log_root_tree(NULL, fs_info);
 	}
diff --git a/fs/btrfs/extent-tree.c b/fs/btrfs/extent-tree.c
index 9ac2eca681e..3d551231cab 100644
--- a/fs/btrfs/extent-tree.c
+++ b/fs/btrfs/extent-tree.c
@@ -257,7 +257,8 @@ static int exclude_super_stripes(struct btrfs_root *root,
 		cache->bytes_super += stripe_len;
 		ret = add_excluded_extent(root, cache->key.objectid,
 					  stripe_len);
-		BUG_ON(ret); /* -ENOMEM */
+		if (ret)
+			return ret;
 	}
 
 	for (i = 0; i < BTRFS_SUPER_MIRROR_MAX; i++) {
@@ -265,13 +266,17 @@ static int exclude_super_stripes(struct btrfs_root *root,
 		ret = btrfs_rmap_block(&root->fs_info->mapping_tree,
 				       cache->key.objectid, bytenr,
 				       0, &logical, &nr, &stripe_len);
-		BUG_ON(ret); /* -ENOMEM */
+		if (ret)
+			return ret;
 
 		while (nr--) {
 			cache->bytes_super += stripe_len;
 			ret = add_excluded_extent(root, logical[nr],
 						  stripe_len);
-			BUG_ON(ret); /* -ENOMEM */
+			if (ret) {
+				kfree(logical);
+				return ret;
+			}
 		}
 
 		kfree(logical);
@@ -4438,7 +4443,7 @@ static void update_global_block_rsv(struct btrfs_fs_info *fs_info)
 	spin_lock(&sinfo->lock);
 	spin_lock(&block_rsv->lock);
 
-	block_rsv->size = num_bytes;
+	block_rsv->size = min_t(u64, num_bytes, 512 * 1024 * 1024);
 
 	num_bytes = sinfo->bytes_used + sinfo->bytes_pinned +
 		    sinfo->bytes_reserved + sinfo->bytes_readonly +
@@ -4793,14 +4798,49 @@ out_fail:
 	 * If the inodes csum_bytes is the same as the original
 	 * csum_bytes then we know we haven't raced with any free()ers
 	 * so we can just reduce our inodes csum bytes and carry on.
-	 * Otherwise we have to do the normal free thing to account for
-	 * the case that the free side didn't free up its reserve
-	 * because of this outstanding reservation.
 	 */
-	if (BTRFS_I(inode)->csum_bytes == csum_bytes)
+	if (BTRFS_I(inode)->csum_bytes == csum_bytes) {
 		calc_csum_metadata_size(inode, num_bytes, 0);
-	else
-		to_free = calc_csum_metadata_size(inode, num_bytes, 0);
+	} else {
+		u64 orig_csum_bytes = BTRFS_I(inode)->csum_bytes;
+		u64 bytes;
+
+		/*
+		 * This is tricky, but first we need to figure out how much we
+		 * free'd from any free-ers that occured during this
+		 * reservation, so we reset ->csum_bytes to the csum_bytes
+		 * before we dropped our lock, and then call the free for the
+		 * number of bytes that were freed while we were trying our
+		 * reservation.
+		 */
+		bytes = csum_bytes - BTRFS_I(inode)->csum_bytes;
+		BTRFS_I(inode)->csum_bytes = csum_bytes;
+		to_free = calc_csum_metadata_size(inode, bytes, 0);
+
+
+		/*
+		 * Now we need to see how much we would have freed had we not
+		 * been making this reservation and our ->csum_bytes were not
+		 * artificially inflated.
+		 */
+		BTRFS_I(inode)->csum_bytes = csum_bytes - num_bytes;
+		bytes = csum_bytes - orig_csum_bytes;
+		bytes = calc_csum_metadata_size(inode, bytes, 0);
+
+		/*
+		 * Now reset ->csum_bytes to what it should be.  If bytes is
+		 * more than to_free then we would have free'd more space had we
+		 * not had an artificially high ->csum_bytes, so we need to free
+		 * the remainder.  If bytes is the same or less then we don't
+		 * need to do anything, the other free-ers did the correct
+		 * thing.
+		 */
+		BTRFS_I(inode)->csum_bytes = orig_csum_bytes - num_bytes;
+		if (bytes > to_free)
+			to_free = bytes - to_free;
+		else
+			to_free = 0;
+	}
 	spin_unlock(&BTRFS_I(inode)->lock);
 	if (dropped)
 		to_free += btrfs_calc_trans_metadata_size(root, dropped);
@@ -7947,7 +7987,17 @@ int btrfs_read_block_groups(struct btrfs_root *root)
 		 * info has super bytes accounted for, otherwise we'll think
 		 * we have more space than we actually do.
 		 */
-		exclude_super_stripes(root, cache);
+		ret = exclude_super_stripes(root, cache);
+		if (ret) {
+			/*
+			 * We may have excluded something, so call this just in
+			 * case.
+			 */
+			free_excluded_extents(root, cache);
+			kfree(cache->free_space_ctl);
+			kfree(cache);
+			goto error;
+		}
 
 		/*
 		 * check for two cases, either we are full, and therefore
@@ -8089,7 +8139,17 @@ int btrfs_make_block_group(struct btrfs_trans_handle *trans,
 
 	cache->last_byte_to_unpin = (u64)-1;
 	cache->cached = BTRFS_CACHE_FINISHED;
-	exclude_super_stripes(root, cache);
+	ret = exclude_super_stripes(root, cache);
+	if (ret) {
+		/*
+		 * We may have excluded something, so call this just in
+		 * case.
+		 */
+		free_excluded_extents(root, cache);
+		kfree(cache->free_space_ctl);
+		kfree(cache);
+		return ret;
+	}
 
 	add_new_free_space(cache, root->fs_info, chunk_offset,
 			   chunk_offset + size);
diff --git a/fs/btrfs/extent_io.c b/fs/btrfs/extent_io.c
index f173c5af646..cdee391fc7b 100644
--- a/fs/btrfs/extent_io.c
+++ b/fs/btrfs/extent_io.c
@@ -1257,6 +1257,39 @@ int unlock_extent(struct extent_io_tree *tree, u64 start, u64 end)
 				GFP_NOFS);
 }
 
+int extent_range_clear_dirty_for_io(struct inode *inode, u64 start, u64 end)
+{
+	unsigned long index = start >> PAGE_CACHE_SHIFT;
+	unsigned long end_index = end >> PAGE_CACHE_SHIFT;
+	struct page *page;
+
+	while (index <= end_index) {
+		page = find_get_page(inode->i_mapping, index);
+		BUG_ON(!page); /* Pages should be in the extent_io_tree */
+		clear_page_dirty_for_io(page);
+		page_cache_release(page);
+		index++;
+	}
+	return 0;
+}
+
+int extent_range_redirty_for_io(struct inode *inode, u64 start, u64 end)
+{
+	unsigned long index = start >> PAGE_CACHE_SHIFT;
+	unsigned long end_index = end >> PAGE_CACHE_SHIFT;
+	struct page *page;
+
+	while (index <= end_index) {
+		page = find_get_page(inode->i_mapping, index);
+		BUG_ON(!page); /* Pages should be in the extent_io_tree */
+		account_page_redirty(page);
+		__set_page_dirty_nobuffers(page);
+		page_cache_release(page);
+		index++;
+	}
+	return 0;
+}
+
 /*
  * helper function to set both pages and extents in the tree writeback
  */
diff --git a/fs/btrfs/extent_io.h b/fs/btrfs/extent_io.h
index 6068a198556..258c9215685 100644
--- a/fs/btrfs/extent_io.h
+++ b/fs/btrfs/extent_io.h
@@ -325,6 +325,8 @@ int map_private_extent_buffer(struct extent_buffer *eb, unsigned long offset,
 		      unsigned long *map_len);
 int extent_range_uptodate(struct extent_io_tree *tree,
 			  u64 start, u64 end);
+int extent_range_clear_dirty_for_io(struct inode *inode, u64 start, u64 end);
+int extent_range_redirty_for_io(struct inode *inode, u64 start, u64 end);
 int extent_clear_unlock_delalloc(struct inode *inode,
 				struct extent_io_tree *tree,
 				u64 start, u64 end, struct page *locked_page,
diff --git a/fs/btrfs/file-item.c b/fs/btrfs/file-item.c
index ec160202be3..c4628a201cb 100644
--- a/fs/btrfs/file-item.c
+++ b/fs/btrfs/file-item.c
@@ -118,9 +118,11 @@ struct btrfs_csum_item *btrfs_lookup_csum(struct btrfs_trans_handle *trans,
 		csums_in_item = btrfs_item_size_nr(leaf, path->slots[0]);
 		csums_in_item /= csum_size;
 
-		if (csum_offset >= csums_in_item) {
+		if (csum_offset == csums_in_item) {
 			ret = -EFBIG;
 			goto fail;
+		} else if (csum_offset > csums_in_item) {
+			goto fail;
 		}
 	}
 	item = btrfs_item_ptr(leaf, path->slots[0], struct btrfs_csum_item);
@@ -728,7 +730,6 @@ int btrfs_csum_file_blocks(struct btrfs_trans_handle *trans,
 		return -ENOMEM;
 
 	sector_sum = sums->sums;
-	trans->adding_csums = 1;
 again:
 	next_offset = (u64)-1;
 	found_next = 0;
@@ -899,7 +900,6 @@ next_sector:
 		goto again;
 	}
 out:
-	trans->adding_csums = 0;
 	btrfs_free_path(path);
 	return ret;
 
diff --git a/fs/btrfs/file.c b/fs/btrfs/file.c
index 5b4ea5f55b8..ade03e6f7bd 100644
--- a/fs/btrfs/file.c
+++ b/fs/btrfs/file.c
@@ -2142,6 +2142,7 @@ static long btrfs_fallocate(struct file *file, int mode,
 {
 	struct inode *inode = file_inode(file);
 	struct extent_state *cached_state = NULL;
+	struct btrfs_root *root = BTRFS_I(inode)->root;
 	u64 cur_offset;
 	u64 last_byte;
 	u64 alloc_start;
@@ -2169,6 +2170,11 @@ static long btrfs_fallocate(struct file *file, int mode,
 	ret = btrfs_check_data_free_space(inode, alloc_end - alloc_start);
 	if (ret)
 		return ret;
+	if (root->fs_info->quota_enabled) {
+		ret = btrfs_qgroup_reserve(root, alloc_end - alloc_start);
+		if (ret)
+			goto out_reserve_fail;
+	}
 
 	/*
 	 * wait for ordered IO before we have any locks.  We'll loop again
@@ -2272,6 +2278,9 @@ static long btrfs_fallocate(struct file *file, int mode,
 			     &cached_state, GFP_NOFS);
 out:
 	mutex_unlock(&inode->i_mutex);
+	if (root->fs_info->quota_enabled)
+		btrfs_qgroup_free(root, alloc_end - alloc_start);
+out_reserve_fail:
 	/* Let go of our reservation. */
 	btrfs_free_reserved_data_space(inode, alloc_end - alloc_start);
 	return ret;
diff --git a/fs/btrfs/inode.c b/fs/btrfs/inode.c
index ca1b767d51f..09c58a35b42 100644
--- a/fs/btrfs/inode.c
+++ b/fs/btrfs/inode.c
@@ -353,6 +353,7 @@ static noinline int compress_file_range(struct inode *inode,
 	int i;
 	int will_compress;
 	int compress_type = root->fs_info->compress_type;
+	int redirty = 0;
 
 	/* if this is a small write inside eof, kick off a defrag */
 	if ((end - start + 1) < 16 * 1024 &&
@@ -415,6 +416,17 @@ again:
 		if (BTRFS_I(inode)->force_compress)
 			compress_type = BTRFS_I(inode)->force_compress;
 
+		/*
+		 * we need to call clear_page_dirty_for_io on each
+		 * page in the range.  Otherwise applications with the file
+		 * mmap'd can wander in and change the page contents while
+		 * we are compressing them.
+		 *
+		 * If the compression fails for any reason, we set the pages
+		 * dirty again later on.
+		 */
+		extent_range_clear_dirty_for_io(inode, start, end);
+		redirty = 1;
 		ret = btrfs_compress_pages(compress_type,
 					   inode->i_mapping, start,
 					   total_compressed, pages,
@@ -554,6 +566,8 @@ cleanup_and_bail_uncompressed:
 			__set_page_dirty_nobuffers(locked_page);
 			/* unlocked later on in the async handlers */
 		}
+		if (redirty)
+			extent_range_redirty_for_io(inode, start, end);
 		add_async_extent(async_cow, start, end - start + 1,
 				 0, NULL, 0, BTRFS_COMPRESS_NONE);
 		*num_added += 1;
@@ -1743,8 +1757,10 @@ static noinline int add_pending_csums(struct btrfs_trans_handle *trans,
 	struct btrfs_ordered_sum *sum;
 
 	list_for_each_entry(sum, list, list) {
+		trans->adding_csums = 1;
 		btrfs_csum_file_blocks(trans,
 		       BTRFS_I(inode)->root->fs_info->csum_root, sum);
+		trans->adding_csums = 0;
 	}
 	return 0;
 }
@@ -3679,11 +3695,9 @@ static struct btrfs_trans_handle *__unlink_start_trans(struct inode *dir,
 	 * 1 for the dir item
 	 * 1 for the dir index
 	 * 1 for the inode ref
-	 * 1 for the inode ref in the tree log
-	 * 2 for the dir entries in the log
 	 * 1 for the inode
 	 */
-	trans = btrfs_start_transaction(root, 8);
+	trans = btrfs_start_transaction(root, 5);
 	if (!IS_ERR(trans) || PTR_ERR(trans) != -ENOSPC)
 		return trans;
 
@@ -8127,7 +8141,7 @@ static int btrfs_rename(struct inode *old_dir, struct dentry *old_dentry,
 	 * inodes.  So 5 * 2 is 10, plus 1 for the new link, so 11 total items
 	 * should cover the worst case number of items we'll modify.
 	 */
-	trans = btrfs_start_transaction(root, 20);
+	trans = btrfs_start_transaction(root, 11);
 	if (IS_ERR(trans)) {
                 ret = PTR_ERR(trans);
                 goto out_notrans;
diff --git a/fs/btrfs/ordered-data.c b/fs/btrfs/ordered-data.c
index dc08d77b717..005c45db699 100644
--- a/fs/btrfs/ordered-data.c
+++ b/fs/btrfs/ordered-data.c
@@ -557,6 +557,7 @@ void btrfs_wait_ordered_extents(struct btrfs_root *root, int delay_iput)
 	INIT_LIST_HEAD(&splice);
 	INIT_LIST_HEAD(&works);
 
+	mutex_lock(&root->fs_info->ordered_operations_mutex);
 	spin_lock(&root->fs_info->ordered_extent_lock);
 	list_splice_init(&root->fs_info->ordered_extents, &splice);
 	while (!list_empty(&splice)) {
@@ -600,6 +601,7 @@ void btrfs_wait_ordered_extents(struct btrfs_root *root, int delay_iput)
 
 		cond_resched();
 	}
+	mutex_unlock(&root->fs_info->ordered_operations_mutex);
 }
 
 /*
diff --git a/fs/btrfs/qgroup.c b/fs/btrfs/qgroup.c
index 5471e47d655..b44124dd237 100644
--- a/fs/btrfs/qgroup.c
+++ b/fs/btrfs/qgroup.c
@@ -1153,7 +1153,7 @@ int btrfs_qgroup_account_ref(struct btrfs_trans_handle *trans,
 	ret = btrfs_find_all_roots(trans, fs_info, node->bytenr,
 				   sgn > 0 ? node->seq - 1 : node->seq, &roots);
 	if (ret < 0)
-		goto out;
+		return ret;
 
 	spin_lock(&fs_info->qgroup_lock);
 	quota_root = fs_info->quota_root;
@@ -1275,7 +1275,6 @@ int btrfs_qgroup_account_ref(struct btrfs_trans_handle *trans,
 	ret = 0;
 unlock:
 	spin_unlock(&fs_info->qgroup_lock);
-out:
 	ulist_free(roots);
 	ulist_free(tmp);
 
diff --git a/fs/btrfs/scrub.c b/fs/btrfs/scrub.c
index 53c3501fa4c..85e072b956d 100644
--- a/fs/btrfs/scrub.c
+++ b/fs/btrfs/scrub.c
@@ -542,7 +542,6 @@ static void scrub_print_warning(const char *errstr, struct scrub_block *sblock)
 	eb = path->nodes[0];
 	ei = btrfs_item_ptr(eb, path->slots[0], struct btrfs_extent_item);
 	item_size = btrfs_item_size_nr(eb, path->slots[0]);
-	btrfs_release_path(path);
 
 	if (flags & BTRFS_EXTENT_FLAG_TREE_BLOCK) {
 		do {
@@ -558,7 +557,9 @@ static void scrub_print_warning(const char *errstr, struct scrub_block *sblock)
 				ret < 0 ? -1 : ref_level,
 				ret < 0 ? -1 : ref_root);
 		} while (ret != 1);
+		btrfs_release_path(path);
 	} else {
+		btrfs_release_path(path);
 		swarn.path = path;
 		swarn.dev = dev;
 		iterate_extent_inodes(fs_info, found_key.objectid,
diff --git a/fs/btrfs/send.c b/fs/btrfs/send.c
index f7a8b861058..c85e7c6b459 100644
--- a/fs/btrfs/send.c
+++ b/fs/btrfs/send.c
@@ -3945,12 +3945,10 @@ static int is_extent_unchanged(struct send_ctx *sctx,
 		    found_key.type != key.type) {
 			key.offset += right_len;
 			break;
-		} else {
-			if (found_key.offset != key.offset + right_len) {
-				/* Should really not happen */
-				ret = -EIO;
-				goto out;
-			}
+		}
+		if (found_key.offset != key.offset + right_len) {
+			ret = 0;
+			goto out;
 		}
 		key = found_key;
 	}
diff --git a/fs/btrfs/tree-log.c b/fs/btrfs/tree-log.c
index 451fad96ecd..ef96381569a 100644
--- a/fs/btrfs/tree-log.c
+++ b/fs/btrfs/tree-log.c
@@ -317,6 +317,7 @@ static noinline int overwrite_item(struct btrfs_trans_handle *trans,
 	unsigned long src_ptr;
 	unsigned long dst_ptr;
 	int overwrite_root = 0;
+	bool inode_item = key->type == BTRFS_INODE_ITEM_KEY;
 
 	if (root->root_key.objectid != BTRFS_TREE_LOG_OBJECTID)
 		overwrite_root = 1;
@@ -326,6 +327,9 @@ static noinline int overwrite_item(struct btrfs_trans_handle *trans,
 
 	/* look for the key in the destination tree */
 	ret = btrfs_search_slot(NULL, root, key, path, 0, 0);
+	if (ret < 0)
+		return ret;
+
 	if (ret == 0) {
 		char *src_copy;
 		char *dst_copy;
@@ -367,6 +371,30 @@ static noinline int overwrite_item(struct btrfs_trans_handle *trans,
 			return 0;
 		}
 
+		/*
+		 * We need to load the old nbytes into the inode so when we
+		 * replay the extents we've logged we get the right nbytes.
+		 */
+		if (inode_item) {
+			struct btrfs_inode_item *item;
+			u64 nbytes;
+
+			item = btrfs_item_ptr(path->nodes[0], path->slots[0],
+					      struct btrfs_inode_item);
+			nbytes = btrfs_inode_nbytes(path->nodes[0], item);
+			item = btrfs_item_ptr(eb, slot,
+					      struct btrfs_inode_item);
+			btrfs_set_inode_nbytes(eb, item, nbytes);
+		}
+	} else if (inode_item) {
+		struct btrfs_inode_item *item;
+
+		/*
+		 * New inode, set nbytes to 0 so that the nbytes comes out
+		 * properly when we replay the extents.
+		 */
+		item = btrfs_item_ptr(eb, slot, struct btrfs_inode_item);
+		btrfs_set_inode_nbytes(eb, item, 0);
 	}
 insert:
 	btrfs_release_path(path);
@@ -486,7 +514,7 @@ static noinline int replay_one_extent(struct btrfs_trans_handle *trans,
 	int found_type;
 	u64 extent_end;
 	u64 start = key->offset;
-	u64 saved_nbytes;
+	u64 nbytes = 0;
 	struct btrfs_file_extent_item *item;
 	struct inode *inode = NULL;
 	unsigned long size;
@@ -496,10 +524,19 @@ static noinline int replay_one_extent(struct btrfs_trans_handle *trans,
 	found_type = btrfs_file_extent_type(eb, item);
 
 	if (found_type == BTRFS_FILE_EXTENT_REG ||
-	    found_type == BTRFS_FILE_EXTENT_PREALLOC)
-		extent_end = start + btrfs_file_extent_num_bytes(eb, item);
-	else if (found_type == BTRFS_FILE_EXTENT_INLINE) {
+	    found_type == BTRFS_FILE_EXTENT_PREALLOC) {
+		nbytes = btrfs_file_extent_num_bytes(eb, item);
+		extent_end = start + nbytes;
+
+		/*
+		 * We don't add to the inodes nbytes if we are prealloc or a
+		 * hole.
+		 */
+		if (btrfs_file_extent_disk_bytenr(eb, item) == 0)
+			nbytes = 0;
+	} else if (found_type == BTRFS_FILE_EXTENT_INLINE) {
 		size = btrfs_file_extent_inline_len(eb, item);
+		nbytes = btrfs_file_extent_ram_bytes(eb, item);
 		extent_end = ALIGN(start + size, root->sectorsize);
 	} else {
 		ret = 0;
@@ -548,7 +585,6 @@ static noinline int replay_one_extent(struct btrfs_trans_handle *trans,
 	}
 	btrfs_release_path(path);
 
-	saved_nbytes = inode_get_bytes(inode);
 	/* drop any overlapping extents */
 	ret = btrfs_drop_extents(trans, root, inode, start, extent_end, 1);
 	BUG_ON(ret);
@@ -635,7 +671,7 @@ static noinline int replay_one_extent(struct btrfs_trans_handle *trans,
 		BUG_ON(ret);
 	}
 
-	inode_set_bytes(inode, saved_nbytes);
+	inode_add_bytes(inode, nbytes);
 	ret = btrfs_update_inode(trans, root, inode);
 out:
 	if (inode)
diff --git a/fs/btrfs/volumes.c b/fs/btrfs/volumes.c
index 5989a92236f..2854c824ab6 100644
--- a/fs/btrfs/volumes.c
+++ b/fs/btrfs/volumes.c
@@ -4935,7 +4935,18 @@ int btrfs_rmap_block(struct btrfs_mapping_tree *map_tree,
 	em = lookup_extent_mapping(em_tree, chunk_start, 1);
 	read_unlock(&em_tree->lock);
 
-	BUG_ON(!em || em->start != chunk_start);
+	if (!em) {
+		printk(KERN_ERR "btrfs: couldn't find em for chunk %Lu\n",
+		       chunk_start);
+		return -EIO;
+	}
+
+	if (em->start != chunk_start) {
+		printk(KERN_ERR "btrfs: bad chunk start, em=%Lu, wanted=%Lu\n",
+		       em->start, chunk_start);
+		free_extent_map(em);
+		return -EIO;
+	}
 	map = (struct map_lookup *)em->bdev;
 
 	length = em->len;
diff --git a/fs/buffer.c b/fs/buffer.c
index b4dcb34c963..10ef81e10b2 100644
--- a/fs/buffer.c
+++ b/fs/buffer.c
@@ -865,8 +865,6 @@ try_again:
 
 		/* Link the buffer to its page */
 		set_bh_page(bh, page, offset);
-
-		init_buffer(bh, NULL, NULL);
 	}
 	return head;
 /*
@@ -2949,7 +2947,7 @@ static void guard_bh_eod(int rw, struct bio *bio, struct buffer_head *bh)
 	}
 }
 
-int submit_bh(int rw, struct buffer_head * bh)
+int _submit_bh(int rw, struct buffer_head *bh, unsigned long bio_flags)
 {
 	struct bio *bio;
 	int ret = 0;
@@ -2984,6 +2982,7 @@ int submit_bh(int rw, struct buffer_head * bh)
 
 	bio->bi_end_io = end_bio_bh_io_sync;
 	bio->bi_private = bh;
+	bio->bi_flags |= bio_flags;
 
 	/* Take care of bh's that straddle the end of the device */
 	guard_bh_eod(rw, bio, bh);
@@ -2997,6 +2996,12 @@ int submit_bh(int rw, struct buffer_head * bh)
 	bio_put(bio);
 	return ret;
 }
+EXPORT_SYMBOL_GPL(_submit_bh);
+
+int submit_bh(int rw, struct buffer_head *bh)
+{
+	return _submit_bh(rw, bh, 0);
+}
 EXPORT_SYMBOL(submit_bh);
 
 /**
diff --git a/fs/cifs/connect.c b/fs/cifs/connect.c
index 991c63c6bdd..21b3a291c32 100644
--- a/fs/cifs/connect.c
+++ b/fs/cifs/connect.c
@@ -1575,14 +1575,24 @@ cifs_parse_mount_options(const char *mountdata, const char *devname,
 			}
 			break;
 		case Opt_blank_pass:
-			vol->password = NULL;
-			break;
-		case Opt_pass:
 			/* passwords have to be handled differently
 			 * to allow the character used for deliminator
 			 * to be passed within them
 			 */
 
+			/*
+			 * Check if this is a case where the  password
+			 * starts with a delimiter
+			 */
+			tmp_end = strchr(data, '=');
+			tmp_end++;
+			if (!(tmp_end < end && tmp_end[1] == delim)) {
+				/* No it is not. Set the password to NULL */
+				vol->password = NULL;
+				break;
+			}
+			/* Yes it is. Drop down to Opt_pass below.*/
+		case Opt_pass:
 			/* Obtain the value string */
 			value = strchr(data, '=');
 			value++;
diff --git a/fs/dcache.c b/fs/dcache.c
index fbfae008ba4..e8bc3420d63 100644
--- a/fs/dcache.c
+++ b/fs/dcache.c
@@ -2542,7 +2542,6 @@ static int prepend_path(const struct path *path,
 	bool slash = false;
 	int error = 0;
 
-	br_read_lock(&vfsmount_lock);
 	while (dentry != root->dentry || vfsmnt != root->mnt) {
 		struct dentry * parent;
 
@@ -2572,8 +2571,6 @@ static int prepend_path(const struct path *path,
 	if (!error && !slash)
 		error = prepend(buffer, buflen, "/", 1);
 
-out:
-	br_read_unlock(&vfsmount_lock);
 	return error;
 
 global_root:
@@ -2590,7 +2587,7 @@ global_root:
 		error = prepend(buffer, buflen, "/", 1);
 	if (!error)
 		error = is_mounted(vfsmnt) ? 1 : 2;
-	goto out;
+	return error;
 }
 
 /**
@@ -2617,9 +2614,11 @@ char *__d_path(const struct path *path,
 	int error;
 
 	prepend(&res, &buflen, "\0", 1);
+	br_read_lock(&vfsmount_lock);
 	write_seqlock(&rename_lock);
 	error = prepend_path(path, root, &res, &buflen);
 	write_sequnlock(&rename_lock);
+	br_read_unlock(&vfsmount_lock);
 
 	if (error < 0)
 		return ERR_PTR(error);
@@ -2636,9 +2635,11 @@ char *d_absolute_path(const struct path *path,
 	int error;
 
 	prepend(&res, &buflen, "\0", 1);
+	br_read_lock(&vfsmount_lock);
 	write_seqlock(&rename_lock);
 	error = prepend_path(path, &root, &res, &buflen);
 	write_sequnlock(&rename_lock);
+	br_read_unlock(&vfsmount_lock);
 
 	if (error > 1)
 		error = -EINVAL;
@@ -2702,11 +2703,13 @@ char *d_path(const struct path *path, char *buf, int buflen)
 		return path->dentry->d_op->d_dname(path->dentry, buf, buflen);
 
 	get_fs_root(current->fs, &root);
+	br_read_lock(&vfsmount_lock);
 	write_seqlock(&rename_lock);
 	error = path_with_deleted(path, &root, &res, &buflen);
+	write_sequnlock(&rename_lock);
+	br_read_unlock(&vfsmount_lock);
 	if (error < 0)
 		res = ERR_PTR(error);
-	write_sequnlock(&rename_lock);
 	path_put(&root);
 	return res;
 }
@@ -2830,6 +2833,7 @@ SYSCALL_DEFINE2(getcwd, char __user *, buf, unsigned long, size)
 	get_fs_root_and_pwd(current->fs, &root, &pwd);
 
 	error = -ENOENT;
+	br_read_lock(&vfsmount_lock);
 	write_seqlock(&rename_lock);
 	if (!d_unlinked(pwd.dentry)) {
 		unsigned long len;
@@ -2839,6 +2843,7 @@ SYSCALL_DEFINE2(getcwd, char __user *, buf, unsigned long, size)
 		prepend(&cwd, &buflen, "\0", 1);
 		error = prepend_path(&pwd, &root, &cwd, &buflen);
 		write_sequnlock(&rename_lock);
+		br_read_unlock(&vfsmount_lock);
 
 		if (error < 0)
 			goto out;
@@ -2859,6 +2864,7 @@ SYSCALL_DEFINE2(getcwd, char __user *, buf, unsigned long, size)
 		}
 	} else {
 		write_sequnlock(&rename_lock);
+		br_read_unlock(&vfsmount_lock);
 	}
 
 out:
diff --git a/fs/direct-io.c b/fs/direct-io.c
index f853263cf74..cfb816dc6d9 100644
--- a/fs/direct-io.c
+++ b/fs/direct-io.c
@@ -672,12 +672,6 @@ static inline int dio_send_cur_page(struct dio *dio, struct dio_submit *sdio,
 		if (sdio->final_block_in_bio != sdio->cur_page_block ||
 		    cur_offset != bio_next_offset)
 			dio_bio_submit(dio, sdio);
-		/*
-		 * Submit now if the underlying fs is about to perform a
-		 * metadata read
-		 */
-		else if (sdio->boundary)
-			dio_bio_submit(dio, sdio);
 	}
 
 	if (sdio->bio == NULL) {
@@ -737,16 +731,6 @@ submit_page_section(struct dio *dio, struct dio_submit *sdio, struct page *page,
 	    sdio->cur_page_block +
 	    (sdio->cur_page_len >> sdio->blkbits) == blocknr) {
 		sdio->cur_page_len += len;
-
-		/*
-		 * If sdio->boundary then we want to schedule the IO now to
-		 * avoid metadata seeks.
-		 */
-		if (sdio->boundary) {
-			ret = dio_send_cur_page(dio, sdio, map_bh);
-			page_cache_release(sdio->cur_page);
-			sdio->cur_page = NULL;
-		}
 		goto out;
 	}
 
@@ -758,7 +742,7 @@ submit_page_section(struct dio *dio, struct dio_submit *sdio, struct page *page,
 		page_cache_release(sdio->cur_page);
 		sdio->cur_page = NULL;
 		if (ret)
-			goto out;
+			return ret;
 	}
 
 	page_cache_get(page);		/* It is in dio */
@@ -768,6 +752,16 @@ submit_page_section(struct dio *dio, struct dio_submit *sdio, struct page *page,
 	sdio->cur_page_block = blocknr;
 	sdio->cur_page_fs_offset = sdio->block_in_file << sdio->blkbits;
 out:
+	/*
+	 * If sdio->boundary then we want to schedule the IO now to
+	 * avoid metadata seeks.
+	 */
+	if (sdio->boundary) {
+		ret = dio_send_cur_page(dio, sdio, map_bh);
+		dio_bio_submit(dio, sdio);
+		page_cache_release(sdio->cur_page);
+		sdio->cur_page = NULL;
+	}
 	return ret;
 }
 
@@ -969,7 +963,8 @@ do_holes:
 			this_chunk_bytes = this_chunk_blocks << blkbits;
 			BUG_ON(this_chunk_bytes == 0);
 
-			sdio->boundary = buffer_boundary(map_bh);
+			if (this_chunk_blocks == sdio->blocks_available)
+				sdio->boundary = buffer_boundary(map_bh);
 			ret = submit_page_section(dio, sdio, page,
 						  offset_in_page,
 						  this_chunk_bytes,
diff --git a/fs/dlm/plock.c b/fs/dlm/plock.c
index 01fd5c11a7f..f704458ea5f 100644
--- a/fs/dlm/plock.c
+++ b/fs/dlm/plock.c
@@ -247,6 +247,7 @@ int dlm_posix_unlock(dlm_lockspace_t *lockspace, u64 number, struct file *file,
 	struct dlm_ls *ls;
 	struct plock_op *op;
 	int rv;
+	unsigned char fl_flags = fl->fl_flags;
 
 	ls = dlm_find_lockspace_local(lockspace);
 	if (!ls)
@@ -258,9 +259,18 @@ int dlm_posix_unlock(dlm_lockspace_t *lockspace, u64 number, struct file *file,
 		goto out;
 	}
 
-	if (posix_lock_file_wait(file, fl) < 0)
-		log_error(ls, "dlm_posix_unlock: vfs unlock error %llx",
-			  (unsigned long long)number);
+	/* cause the vfs unlock to return ENOENT if lock is not found */
+	fl->fl_flags |= FL_EXISTS;
+
+	rv = posix_lock_file_wait(file, fl);
+	if (rv == -ENOENT) {
+		rv = 0;
+		goto out_free;
+	}
+	if (rv < 0) {
+		log_error(ls, "dlm_posix_unlock: vfs unlock error %d %llx",
+			  rv, (unsigned long long)number);
+	}
 
 	op->info.optype		= DLM_PLOCK_OP_UNLOCK;
 	op->info.pid		= fl->fl_pid;
@@ -296,9 +306,11 @@ int dlm_posix_unlock(dlm_lockspace_t *lockspace, u64 number, struct file *file,
 	if (rv == -ENOENT)
 		rv = 0;
 
+out_free:
 	kfree(op);
 out:
 	dlm_put_lockspace(ls);
+	fl->fl_flags = fl_flags;
 	return rv;
 }
 EXPORT_SYMBOL_GPL(dlm_posix_unlock);
diff --git a/fs/ecryptfs/miscdev.c b/fs/ecryptfs/miscdev.c
index 412e6eda25f..e4141f25749 100644
--- a/fs/ecryptfs/miscdev.c
+++ b/fs/ecryptfs/miscdev.c
@@ -80,13 +80,6 @@ ecryptfs_miscdev_open(struct inode *inode, struct file *file)
 	int rc;
 
 	mutex_lock(&ecryptfs_daemon_hash_mux);
-	rc = try_module_get(THIS_MODULE);
-	if (rc == 0) {
-		rc = -EIO;
-		printk(KERN_ERR "%s: Error attempting to increment module use "
-		       "count; rc = [%d]\n", __func__, rc);
-		goto out_unlock_daemon_list;
-	}
 	rc = ecryptfs_find_daemon_by_euid(&daemon);
 	if (!rc) {
 		rc = -EINVAL;
@@ -96,7 +89,7 @@ ecryptfs_miscdev_open(struct inode *inode, struct file *file)
 	if (rc) {
 		printk(KERN_ERR "%s: Error attempting to spawn daemon; "
 		       "rc = [%d]\n", __func__, rc);
-		goto out_module_put_unlock_daemon_list;
+		goto out_unlock_daemon_list;
 	}
 	mutex_lock(&daemon->mux);
 	if (daemon->flags & ECRYPTFS_DAEMON_MISCDEV_OPEN) {
@@ -108,9 +101,6 @@ ecryptfs_miscdev_open(struct inode *inode, struct file *file)
 	atomic_inc(&ecryptfs_num_miscdev_opens);
 out_unlock_daemon:
 	mutex_unlock(&daemon->mux);
-out_module_put_unlock_daemon_list:
-	if (rc)
-		module_put(THIS_MODULE);
 out_unlock_daemon_list:
 	mutex_unlock(&ecryptfs_daemon_hash_mux);
 	return rc;
@@ -147,7 +137,6 @@ ecryptfs_miscdev_release(struct inode *inode, struct file *file)
 		       "bug.\n", __func__, rc);
 		BUG();
 	}
-	module_put(THIS_MODULE);
 	return rc;
 }
 
@@ -471,6 +460,7 @@ out_free:
 
 
 static const struct file_operations ecryptfs_miscdev_fops = {
+	.owner   = THIS_MODULE,
 	.open    = ecryptfs_miscdev_open,
 	.poll    = ecryptfs_miscdev_poll,
 	.read    = ecryptfs_miscdev_read,
diff --git a/fs/exec.c b/fs/exec.c
index a96a4885bbb..87e731f020f 100644
--- a/fs/exec.c
+++ b/fs/exec.c
@@ -613,7 +613,7 @@ static int shift_arg_pages(struct vm_area_struct *vma, unsigned long shift)
 		 * when the old and new regions overlap clear from new_end.
 		 */
 		free_pgd_range(&tlb, new_end, old_end, new_end,
-			vma->vm_next ? vma->vm_next->vm_start : 0);
+			vma->vm_next ? vma->vm_next->vm_start : USER_PGTABLES_CEILING);
 	} else {
 		/*
 		 * otherwise, clean from old_start; this is done to not touch
@@ -622,7 +622,7 @@ static int shift_arg_pages(struct vm_area_struct *vma, unsigned long shift)
 		 * for the others its just a little faster.
 		 */
 		free_pgd_range(&tlb, old_start, old_end, new_end,
-			vma->vm_next ? vma->vm_next->vm_start : 0);
+			vma->vm_next ? vma->vm_next->vm_start : USER_PGTABLES_CEILING);
 	}
 	tlb_finish_mmu(&tlb, new_end, old_end);
 
diff --git a/fs/ext3/super.c b/fs/ext3/super.c
index fb5120a5505..3dc48cc8b6e 100644
--- a/fs/ext3/super.c
+++ b/fs/ext3/super.c
@@ -2067,7 +2067,6 @@ static int ext3_fill_super (struct super_block *sb, void *data, int silent)
 		test_opt(sb,DATA_FLAGS) == EXT3_MOUNT_JOURNAL_DATA ? "journal":
 		test_opt(sb,DATA_FLAGS) == EXT3_MOUNT_ORDERED_DATA ? "ordered":
 		"writeback");
-	sb->s_flags |= MS_SNAP_STABLE;
 
 	return 0;
 
diff --git a/fs/ext4/extents.c b/fs/ext4/extents.c
index 56efcaadf84..9c6d06dcef8 100644
--- a/fs/ext4/extents.c
+++ b/fs/ext4/extents.c
@@ -2999,20 +2999,23 @@ static int ext4_split_extent_at(handle_t *handle,
 			if (split_flag & EXT4_EXT_DATA_VALID1) {
 				err = ext4_ext_zeroout(inode, ex2);
 				zero_ex.ee_block = ex2->ee_block;
-				zero_ex.ee_len = ext4_ext_get_actual_len(ex2);
+				zero_ex.ee_len = cpu_to_le16(
+						ext4_ext_get_actual_len(ex2));
 				ext4_ext_store_pblock(&zero_ex,
 						      ext4_ext_pblock(ex2));
 			} else {
 				err = ext4_ext_zeroout(inode, ex);
 				zero_ex.ee_block = ex->ee_block;
-				zero_ex.ee_len = ext4_ext_get_actual_len(ex);
+				zero_ex.ee_len = cpu_to_le16(
+						ext4_ext_get_actual_len(ex));
 				ext4_ext_store_pblock(&zero_ex,
 						      ext4_ext_pblock(ex));
 			}
 		} else {
 			err = ext4_ext_zeroout(inode, &orig_ex);
 			zero_ex.ee_block = orig_ex.ee_block;
-			zero_ex.ee_len = ext4_ext_get_actual_len(&orig_ex);
+			zero_ex.ee_len = cpu_to_le16(
+						ext4_ext_get_actual_len(&orig_ex));
 			ext4_ext_store_pblock(&zero_ex,
 					      ext4_ext_pblock(&orig_ex));
 		}
@@ -3272,7 +3275,7 @@ static int ext4_ext_convert_to_initialized(handle_t *handle,
 		if (err)
 			goto out;
 		zero_ex.ee_block = ex->ee_block;
-		zero_ex.ee_len = ext4_ext_get_actual_len(ex);
+		zero_ex.ee_len = cpu_to_le16(ext4_ext_get_actual_len(ex));
 		ext4_ext_store_pblock(&zero_ex, ext4_ext_pblock(ex));
 
 		err = ext4_ext_get_access(handle, inode, path + depth);
diff --git a/fs/ext4/indirect.c b/fs/ext4/indirect.c
index b505a145a59..a04183127ef 100644
--- a/fs/ext4/indirect.c
+++ b/fs/ext4/indirect.c
@@ -1539,9 +1539,9 @@ static int free_hole_blocks(handle_t *handle, struct inode *inode,
 		blk = *i_data;
 		if (level > 0) {
 			ext4_lblk_t first2;
-			bh = sb_bread(inode->i_sb, blk);
+			bh = sb_bread(inode->i_sb, le32_to_cpu(blk));
 			if (!bh) {
-				EXT4_ERROR_INODE_BLOCK(inode, blk,
+				EXT4_ERROR_INODE_BLOCK(inode, le32_to_cpu(blk),
 						       "Read failure");
 				return -EIO;
 			}
diff --git a/fs/f2fs/f2fs.h b/fs/f2fs/f2fs.h
index cc2213afdcc..201c8d3b0f8 100644
--- a/fs/f2fs/f2fs.h
+++ b/fs/f2fs/f2fs.h
@@ -137,7 +137,7 @@ struct extent_info {
 	rwlock_t ext_lock;	/* rwlock for consistency */
 	unsigned int fofs;	/* start offset in a file */
 	u32 blk_addr;		/* start block address of the extent */
-	unsigned int len;	/* lenth of the extent */
+	unsigned int len;	/* length of the extent */
 };
 
 /*
diff --git a/fs/f2fs/gc.c b/fs/f2fs/gc.c
index 94b8a0c4845..2e3eb2d4fc3 100644
--- a/fs/f2fs/gc.c
+++ b/fs/f2fs/gc.c
@@ -222,7 +222,7 @@ static unsigned int get_gc_cost(struct f2fs_sb_info *sbi, unsigned int segno,
 }
 
 /*
- * This function is called from two pathes.
+ * This function is called from two paths.
  * One is garbage collection and the other is SSR segment selection.
  * When it is called during GC, it just gets a victim segment
  * and it does not remove it from dirty seglist.
diff --git a/fs/f2fs/super.c b/fs/f2fs/super.c
index fea6e582a2e..62e017743af 100644
--- a/fs/f2fs/super.c
+++ b/fs/f2fs/super.c
@@ -82,7 +82,7 @@ static struct inode *f2fs_alloc_inode(struct super_block *sb)
 
 	init_once((void *) fi);
 
-	/* Initilize f2fs-specific inode info */
+	/* Initialize f2fs-specific inode info */
 	fi->vfs_inode.i_version = 1;
 	atomic_set(&fi->dirty_dents, 0);
 	fi->i_current_depth = 1;
diff --git a/fs/fat/dir.c b/fs/fat/dir.c
index 165012ef363..7a6f02caf28 100644
--- a/fs/fat/dir.c
+++ b/fs/fat/dir.c
@@ -964,6 +964,29 @@ int fat_scan(struct inode *dir, const unsigned char *name,
 }
 EXPORT_SYMBOL_GPL(fat_scan);
 
+/*
+ * Scans a directory for a given logstart.
+ * Returns an error code or zero.
+ */
+int fat_scan_logstart(struct inode *dir, int i_logstart,
+		      struct fat_slot_info *sinfo)
+{
+	struct super_block *sb = dir->i_sb;
+
+	sinfo->slot_off = 0;
+	sinfo->bh = NULL;
+	while (fat_get_short_entry(dir, &sinfo->slot_off, &sinfo->bh,
+				   &sinfo->de) >= 0) {
+		if (fat_get_start(MSDOS_SB(sb), sinfo->de) == i_logstart) {
+			sinfo->slot_off -= sizeof(*sinfo->de);
+			sinfo->nr_slots = 1;
+			sinfo->i_pos = fat_make_i_pos(sb, sinfo->bh, sinfo->de);
+			return 0;
+		}
+	}
+	return -ENOENT;
+}
+
 static int __fat_remove_entries(struct inode *dir, loff_t pos, int nr_slots)
 {
 	struct super_block *sb = dir->i_sb;
diff --git a/fs/fat/fat.h b/fs/fat/fat.h
index e9cc3f0d58e..21664fcf361 100644
--- a/fs/fat/fat.h
+++ b/fs/fat/fat.h
@@ -23,6 +23,9 @@
 #define FAT_ERRORS_PANIC	2      /* panic on error */
 #define FAT_ERRORS_RO		3      /* remount r/o on error */
 
+#define FAT_NFS_STALE_RW	1      /* NFS RW support, can cause ESTALE */
+#define FAT_NFS_NOSTALE_RO	2      /* NFS RO support, no ESTALE issue */
+
 struct fat_mount_options {
 	kuid_t fs_uid;
 	kgid_t fs_gid;
@@ -34,6 +37,7 @@ struct fat_mount_options {
 	unsigned short shortname;  /* flags for shortname display/create rule */
 	unsigned char name_check;  /* r = relaxed, n = normal, s = strict */
 	unsigned char errors;	   /* On error: continue, panic, remount-ro */
+	unsigned char nfs;	  /* NFS support: nostale_ro, stale_rw */
 	unsigned short allow_utime;/* permission for setting the [am]time */
 	unsigned quiet:1,          /* set = fake successful chmods and chowns */
 		 showexec:1,       /* set = only set x bit for com/exe/bat */
@@ -48,8 +52,7 @@ struct fat_mount_options {
 		 usefree:1,	   /* Use free_clusters for FAT32 */
 		 tz_set:1,	   /* Filesystem timestamps' offset set */
 		 rodir:1,	   /* allow ATTR_RO for directory */
-		 discard:1,	   /* Issue discard requests on deletions */
-		 nfs:1;		   /* Do extra work needed for NFS export */
+		 discard:1;	   /* Issue discard requests on deletions */
 };
 
 #define FAT_HASH_BITS	8
@@ -72,6 +75,7 @@ struct msdos_sb_info {
 	unsigned long root_cluster;   /* first cluster of the root directory */
 	unsigned long fsinfo_sector;  /* sector number of FAT32 fsinfo */
 	struct mutex fat_lock;
+	struct mutex nfs_build_inode_lock;
 	struct mutex s_lock;
 	unsigned int prev_free;      /* previously allocated cluster number */
 	unsigned int free_clusters;  /* -1 if undefined */
@@ -215,6 +219,27 @@ static inline sector_t fat_clus_to_blknr(struct msdos_sb_info *sbi, int clus)
 		+ sbi->data_start;
 }
 
+static inline void fat_get_blknr_offset(struct msdos_sb_info *sbi,
+				loff_t i_pos, sector_t *blknr, int *offset)
+{
+	*blknr = i_pos >> sbi->dir_per_block_bits;
+	*offset = i_pos & (sbi->dir_per_block - 1);
+}
+
+static inline loff_t fat_i_pos_read(struct msdos_sb_info *sbi,
+					struct inode *inode)
+{
+	loff_t i_pos;
+#if BITS_PER_LONG == 32
+	spin_lock(&sbi->inode_hash_lock);
+#endif
+	i_pos = MSDOS_I(inode)->i_pos;
+#if BITS_PER_LONG == 32
+	spin_unlock(&sbi->inode_hash_lock);
+#endif
+	return i_pos;
+}
+
 static inline void fat16_towchar(wchar_t *dst, const __u8 *src, size_t len)
 {
 #ifdef __BIG_ENDIAN
@@ -271,6 +296,8 @@ extern int fat_dir_empty(struct inode *dir);
 extern int fat_subdirs(struct inode *dir);
 extern int fat_scan(struct inode *dir, const unsigned char *name,
 		    struct fat_slot_info *sinfo);
+extern int fat_scan_logstart(struct inode *dir, int i_logstart,
+			     struct fat_slot_info *sinfo);
 extern int fat_get_dotdot_entry(struct inode *dir, struct buffer_head **bh,
 				struct msdos_dir_entry **de);
 extern int fat_alloc_new_dir(struct inode *dir, struct timespec *ts);
@@ -348,6 +375,7 @@ extern struct inode *fat_build_inode(struct super_block *sb,
 extern int fat_sync_inode(struct inode *inode);
 extern int fat_fill_super(struct super_block *sb, void *data, int silent,
 			  int isvfat, void (*setup)(struct super_block *));
+extern int fat_fill_inode(struct inode *inode, struct msdos_dir_entry *de);
 
 extern int fat_flush_inodes(struct super_block *sb, struct inode *i1,
 			    struct inode *i2);
@@ -382,12 +410,8 @@ int fat_cache_init(void);
 void fat_cache_destroy(void);
 
 /* fat/nfs.c */
-struct fid;
-extern struct dentry *fat_fh_to_dentry(struct super_block *sb, struct fid *fid,
-				       int fh_len, int fh_type);
-extern struct dentry *fat_fh_to_parent(struct super_block *sb, struct fid *fid,
-				       int fh_len, int fh_type);
-extern struct dentry *fat_get_parent(struct dentry *child_dir);
+extern const struct export_operations fat_export_ops;
+extern const struct export_operations fat_export_ops_nostale;
 
 /* helper for printk */
 typedef unsigned long long	llu;
diff --git a/fs/fat/file.c b/fs/fat/file.c
index 3978f8ca182..b0b632e50dd 100644
--- a/fs/fat/file.c
+++ b/fs/fat/file.c
@@ -306,6 +306,11 @@ int fat_getattr(struct vfsmount *mnt, struct dentry *dentry, struct kstat *stat)
 	struct inode *inode = dentry->d_inode;
 	generic_fillattr(inode, stat);
 	stat->blksize = MSDOS_SB(inode->i_sb)->cluster_size;
+
+	if (MSDOS_SB(inode->i_sb)->options.nfs == FAT_NFS_NOSTALE_RO) {
+		/* Use i_pos for ino. This is used as fileid of nfs. */
+		stat->ino = fat_i_pos_read(MSDOS_SB(inode->i_sb), inode);
+	}
 	return 0;
 }
 EXPORT_SYMBOL_GPL(fat_getattr);
diff --git a/fs/fat/inode.c b/fs/fat/inode.c
index acf6e479b44..4ff901632b2 100644
--- a/fs/fat/inode.c
+++ b/fs/fat/inode.c
@@ -18,7 +18,6 @@
 #include <linux/pagemap.h>
 #include <linux/mpage.h>
 #include <linux/buffer_head.h>
-#include <linux/exportfs.h>
 #include <linux/mount.h>
 #include <linux/vfs.h>
 #include <linux/parser.h>
@@ -385,7 +384,7 @@ static int fat_calc_dir_size(struct inode *inode)
 }
 
 /* doesn't deal with root inode */
-static int fat_fill_inode(struct inode *inode, struct msdos_dir_entry *de)
+int fat_fill_inode(struct inode *inode, struct msdos_dir_entry *de)
 {
 	struct msdos_sb_info *sbi = MSDOS_SB(inode->i_sb);
 	int error;
@@ -444,12 +443,25 @@ static int fat_fill_inode(struct inode *inode, struct msdos_dir_entry *de)
 	return 0;
 }
 
+static inline void fat_lock_build_inode(struct msdos_sb_info *sbi)
+{
+	if (sbi->options.nfs == FAT_NFS_NOSTALE_RO)
+		mutex_lock(&sbi->nfs_build_inode_lock);
+}
+
+static inline void fat_unlock_build_inode(struct msdos_sb_info *sbi)
+{
+	if (sbi->options.nfs == FAT_NFS_NOSTALE_RO)
+		mutex_unlock(&sbi->nfs_build_inode_lock);
+}
+
 struct inode *fat_build_inode(struct super_block *sb,
 			struct msdos_dir_entry *de, loff_t i_pos)
 {
 	struct inode *inode;
 	int err;
 
+	fat_lock_build_inode(MSDOS_SB(sb));
 	inode = fat_iget(sb, i_pos);
 	if (inode)
 		goto out;
@@ -469,6 +481,7 @@ struct inode *fat_build_inode(struct super_block *sb,
 	fat_attach(inode, i_pos);
 	insert_inode_hash(inode);
 out:
+	fat_unlock_build_inode(MSDOS_SB(sb));
 	return inode;
 }
 
@@ -655,20 +668,6 @@ static int fat_statfs(struct dentry *dentry, struct kstatfs *buf)
 	return 0;
 }
 
-static inline loff_t fat_i_pos_read(struct msdos_sb_info *sbi,
-				    struct inode *inode)
-{
-	loff_t i_pos;
-#if BITS_PER_LONG == 32
-	spin_lock(&sbi->inode_hash_lock);
-#endif
-	i_pos = MSDOS_I(inode)->i_pos;
-#if BITS_PER_LONG == 32
-	spin_unlock(&sbi->inode_hash_lock);
-#endif
-	return i_pos;
-}
-
 static int __fat_write_inode(struct inode *inode, int wait)
 {
 	struct super_block *sb = inode->i_sb;
@@ -676,7 +675,8 @@ static int __fat_write_inode(struct inode *inode, int wait)
 	struct buffer_head *bh;
 	struct msdos_dir_entry *raw_entry;
 	loff_t i_pos;
-	int err;
+	sector_t blocknr;
+	int err, offset;
 
 	if (inode->i_ino == MSDOS_ROOT_INO)
 		return 0;
@@ -686,7 +686,8 @@ retry:
 	if (!i_pos)
 		return 0;
 
-	bh = sb_bread(sb, i_pos >> sbi->dir_per_block_bits);
+	fat_get_blknr_offset(sbi, i_pos, &blocknr, &offset);
+	bh = sb_bread(sb, blocknr);
 	if (!bh) {
 		fat_msg(sb, KERN_ERR, "unable to read inode block "
 		       "for updating (i_pos %lld)", i_pos);
@@ -699,8 +700,7 @@ retry:
 		goto retry;
 	}
 
-	raw_entry = &((struct msdos_dir_entry *) (bh->b_data))
-	    [i_pos & (sbi->dir_per_block - 1)];
+	raw_entry = &((struct msdos_dir_entry *) (bh->b_data))[offset];
 	if (S_ISDIR(inode->i_mode))
 		raw_entry->size = 0;
 	else
@@ -761,12 +761,6 @@ static const struct super_operations fat_sops = {
 	.show_options	= fat_show_options,
 };
 
-static const struct export_operations fat_export_ops = {
-	.fh_to_dentry	= fat_fh_to_dentry,
-	.fh_to_parent	= fat_fh_to_parent,
-	.get_parent	= fat_get_parent,
-};
-
 static int fat_show_options(struct seq_file *m, struct dentry *root)
 {
 	struct msdos_sb_info *sbi = MSDOS_SB(root->d_sb);
@@ -814,8 +808,6 @@ static int fat_show_options(struct seq_file *m, struct dentry *root)
 		seq_puts(m, ",usefree");
 	if (opts->quiet)
 		seq_puts(m, ",quiet");
-	if (opts->nfs)
-		seq_puts(m, ",nfs");
 	if (opts->showexec)
 		seq_puts(m, ",showexec");
 	if (opts->sys_immutable)
@@ -849,6 +841,10 @@ static int fat_show_options(struct seq_file *m, struct dentry *root)
 		seq_puts(m, ",errors=panic");
 	else
 		seq_puts(m, ",errors=remount-ro");
+	if (opts->nfs == FAT_NFS_NOSTALE_RO)
+		seq_puts(m, ",nfs=nostale_ro");
+	else if (opts->nfs)
+		seq_puts(m, ",nfs=stale_rw");
 	if (opts->discard)
 		seq_puts(m, ",discard");
 
@@ -865,7 +861,7 @@ enum {
 	Opt_uni_xl_no, Opt_uni_xl_yes, Opt_nonumtail_no, Opt_nonumtail_yes,
 	Opt_obsolete, Opt_flush, Opt_tz_utc, Opt_rodir, Opt_err_cont,
 	Opt_err_panic, Opt_err_ro, Opt_discard, Opt_nfs, Opt_time_offset,
-	Opt_err,
+	Opt_nfs_stale_rw, Opt_nfs_nostale_ro, Opt_err,
 };
 
 static const match_table_t fat_tokens = {
@@ -895,7 +891,9 @@ static const match_table_t fat_tokens = {
 	{Opt_err_panic, "errors=panic"},
 	{Opt_err_ro, "errors=remount-ro"},
 	{Opt_discard, "discard"},
-	{Opt_nfs, "nfs"},
+	{Opt_nfs_stale_rw, "nfs"},
+	{Opt_nfs_stale_rw, "nfs=stale_rw"},
+	{Opt_nfs_nostale_ro, "nfs=nostale_ro"},
 	{Opt_obsolete, "conv=binary"},
 	{Opt_obsolete, "conv=text"},
 	{Opt_obsolete, "conv=auto"},
@@ -1092,6 +1090,12 @@ static int parse_options(struct super_block *sb, char *options, int is_vfat,
 		case Opt_err_ro:
 			opts->errors = FAT_ERRORS_RO;
 			break;
+		case Opt_nfs_stale_rw:
+			opts->nfs = FAT_NFS_STALE_RW;
+			break;
+		case Opt_nfs_nostale_ro:
+			opts->nfs = FAT_NFS_NOSTALE_RO;
+			break;
 
 		/* msdos specific */
 		case Opt_dots:
@@ -1150,9 +1154,6 @@ static int parse_options(struct super_block *sb, char *options, int is_vfat,
 		case Opt_discard:
 			opts->discard = 1;
 			break;
-		case Opt_nfs:
-			opts->nfs = 1;
-			break;
 
 		/* obsolete mount options */
 		case Opt_obsolete:
@@ -1183,6 +1184,10 @@ out:
 		opts->allow_utime = ~opts->fs_dmask & (S_IWGRP | S_IWOTH);
 	if (opts->unicode_xlate)
 		opts->utf8 = 0;
+	if (opts->nfs == FAT_NFS_NOSTALE_RO) {
+		sb->s_flags |= MS_RDONLY;
+		sb->s_export_op = &fat_export_ops_nostale;
+	}
 
 	return 0;
 }
@@ -1193,7 +1198,7 @@ static int fat_read_root(struct inode *inode)
 	struct msdos_sb_info *sbi = MSDOS_SB(sb);
 	int error;
 
-	MSDOS_I(inode)->i_pos = 0;
+	MSDOS_I(inode)->i_pos = MSDOS_ROOT_INO;
 	inode->i_uid = sbi->options.fs_uid;
 	inode->i_gid = sbi->options.fs_gid;
 	inode->i_version++;
@@ -1256,6 +1261,7 @@ int fat_fill_super(struct super_block *sb, void *data, int silent, int isvfat,
 	sb->s_magic = MSDOS_SUPER_MAGIC;
 	sb->s_op = &fat_sops;
 	sb->s_export_op = &fat_export_ops;
+	mutex_init(&sbi->nfs_build_inode_lock);
 	ratelimit_state_init(&sbi->ratelimit, DEFAULT_RATELIMIT_INTERVAL,
 			     DEFAULT_RATELIMIT_BURST);
 
diff --git a/fs/fat/nfs.c b/fs/fat/nfs.c
index 499c10438ca..93e14933dcb 100644
--- a/fs/fat/nfs.c
+++ b/fs/fat/nfs.c
@@ -14,6 +14,18 @@
 #include <linux/exportfs.h>
 #include "fat.h"
 
+struct fat_fid {
+	u32 i_gen;
+	u32 i_pos_low;
+	u16 i_pos_hi;
+	u16 parent_i_pos_hi;
+	u32 parent_i_pos_low;
+	u32 parent_i_gen;
+};
+
+#define FAT_FID_SIZE_WITHOUT_PARENT 3
+#define FAT_FID_SIZE_WITH_PARENT (sizeof(struct fat_fid)/sizeof(u32))
+
 /**
  * Look up a directory inode given its starting cluster.
  */
@@ -38,63 +50,252 @@ static struct inode *fat_dget(struct super_block *sb, int i_logstart)
 	return inode;
 }
 
-static struct inode *fat_nfs_get_inode(struct super_block *sb,
-				       u64 ino, u32 generation)
+static struct inode *fat_ilookup(struct super_block *sb, u64 ino, loff_t i_pos)
 {
-	struct inode *inode;
+	if (MSDOS_SB(sb)->options.nfs == FAT_NFS_NOSTALE_RO)
+		return fat_iget(sb, i_pos);
 
-	if ((ino < MSDOS_ROOT_INO) || (ino == MSDOS_FSINFO_INO))
-		return NULL;
+	else {
+		if ((ino < MSDOS_ROOT_INO) || (ino == MSDOS_FSINFO_INO))
+			return NULL;
+		return ilookup(sb, ino);
+	}
+}
+
+static struct inode *__fat_nfs_get_inode(struct super_block *sb,
+				       u64 ino, u32 generation, loff_t i_pos)
+{
+	struct inode *inode = fat_ilookup(sb, ino, i_pos);
 
-	inode = ilookup(sb, ino);
 	if (inode && generation && (inode->i_generation != generation)) {
 		iput(inode);
 		inode = NULL;
 	}
+	if (inode == NULL && MSDOS_SB(sb)->options.nfs == FAT_NFS_NOSTALE_RO) {
+		struct buffer_head *bh = NULL;
+		struct msdos_dir_entry *de ;
+		sector_t blocknr;
+		int offset;
+		fat_get_blknr_offset(MSDOS_SB(sb), i_pos, &blocknr, &offset);
+		bh = sb_bread(sb, blocknr);
+		if (!bh) {
+			fat_msg(sb, KERN_ERR,
+				"unable to read block(%llu) for building NFS inode",
+				(llu)blocknr);
+			return inode;
+		}
+		de = (struct msdos_dir_entry *)bh->b_data;
+		/* If a file is deleted on server and client is not updated
+		 * yet, we must not build the inode upon a lookup call.
+		 */
+		if (IS_FREE(de[offset].name))
+			inode = NULL;
+		else
+			inode = fat_build_inode(sb, &de[offset], i_pos);
+		brelse(bh);
+	}
 
 	return inode;
 }
 
+static struct inode *fat_nfs_get_inode(struct super_block *sb,
+				       u64 ino, u32 generation)
+{
+
+	return __fat_nfs_get_inode(sb, ino, generation, 0);
+}
+
+static int
+fat_encode_fh_nostale(struct inode *inode, __u32 *fh, int *lenp,
+		      struct inode *parent)
+{
+	int len = *lenp;
+	struct msdos_sb_info *sbi = MSDOS_SB(inode->i_sb);
+	struct fat_fid *fid = (struct fat_fid *) fh;
+	loff_t i_pos;
+	int type = FILEID_FAT_WITHOUT_PARENT;
+
+	if (parent) {
+		if (len < FAT_FID_SIZE_WITH_PARENT) {
+			*lenp = FAT_FID_SIZE_WITH_PARENT;
+			return FILEID_INVALID;
+		}
+	} else {
+		if (len < FAT_FID_SIZE_WITHOUT_PARENT) {
+			*lenp = FAT_FID_SIZE_WITHOUT_PARENT;
+			return FILEID_INVALID;
+		}
+	}
+
+	i_pos = fat_i_pos_read(sbi, inode);
+	*lenp = FAT_FID_SIZE_WITHOUT_PARENT;
+	fid->i_gen = inode->i_generation;
+	fid->i_pos_low = i_pos & 0xFFFFFFFF;
+	fid->i_pos_hi = (i_pos >> 32) & 0xFFFF;
+	if (parent) {
+		i_pos = fat_i_pos_read(sbi, parent);
+		fid->parent_i_pos_hi = (i_pos >> 32) & 0xFFFF;
+		fid->parent_i_pos_low = i_pos & 0xFFFFFFFF;
+		fid->parent_i_gen = parent->i_generation;
+		type = FILEID_FAT_WITH_PARENT;
+		*lenp = FAT_FID_SIZE_WITH_PARENT;
+	}
+
+	return type;
+}
+
 /**
  * Map a NFS file handle to a corresponding dentry.
  * The dentry may or may not be connected to the filesystem root.
  */
-struct dentry *fat_fh_to_dentry(struct super_block *sb, struct fid *fid,
+static struct dentry *fat_fh_to_dentry(struct super_block *sb, struct fid *fid,
 				int fh_len, int fh_type)
 {
 	return generic_fh_to_dentry(sb, fid, fh_len, fh_type,
 				    fat_nfs_get_inode);
 }
 
+static struct dentry *fat_fh_to_dentry_nostale(struct super_block *sb,
+					       struct fid *fh, int fh_len,
+					       int fh_type)
+{
+	struct inode *inode = NULL;
+	struct fat_fid *fid = (struct fat_fid *)fh;
+	loff_t i_pos;
+
+	switch (fh_type) {
+	case FILEID_FAT_WITHOUT_PARENT:
+		if (fh_len < FAT_FID_SIZE_WITHOUT_PARENT)
+			return NULL;
+		break;
+	case FILEID_FAT_WITH_PARENT:
+		if (fh_len < FAT_FID_SIZE_WITH_PARENT)
+			return NULL;
+		break;
+	default:
+		return NULL;
+	}
+	i_pos = fid->i_pos_hi;
+	i_pos = (i_pos << 32) | (fid->i_pos_low);
+	inode = __fat_nfs_get_inode(sb, 0, fid->i_gen, i_pos);
+
+	return d_obtain_alias(inode);
+}
+
 /*
  * Find the parent for a file specified by NFS handle.
  * This requires that the handle contain the i_ino of the parent.
  */
-struct dentry *fat_fh_to_parent(struct super_block *sb, struct fid *fid,
+static struct dentry *fat_fh_to_parent(struct super_block *sb, struct fid *fid,
 				int fh_len, int fh_type)
 {
 	return generic_fh_to_parent(sb, fid, fh_len, fh_type,
 				    fat_nfs_get_inode);
 }
 
+static struct dentry *fat_fh_to_parent_nostale(struct super_block *sb,
+					       struct fid *fh, int fh_len,
+					       int fh_type)
+{
+	struct inode *inode = NULL;
+	struct fat_fid *fid = (struct fat_fid *)fh;
+	loff_t i_pos;
+
+	if (fh_len < FAT_FID_SIZE_WITH_PARENT)
+		return NULL;
+
+	switch (fh_type) {
+	case FILEID_FAT_WITH_PARENT:
+		i_pos = fid->parent_i_pos_hi;
+		i_pos = (i_pos << 32) | (fid->parent_i_pos_low);
+		inode = __fat_nfs_get_inode(sb, 0, fid->parent_i_gen, i_pos);
+		break;
+	}
+
+	return d_obtain_alias(inode);
+}
+
+/*
+ * Rebuild the parent for a directory that is not connected
+ *  to the filesystem root
+ */
+static
+struct inode *fat_rebuild_parent(struct super_block *sb, int parent_logstart)
+{
+	int search_clus, clus_to_match;
+	struct msdos_dir_entry *de;
+	struct inode *parent = NULL;
+	struct inode *dummy_grand_parent = NULL;
+	struct fat_slot_info sinfo;
+	struct msdos_sb_info *sbi = MSDOS_SB(sb);
+	sector_t blknr = fat_clus_to_blknr(sbi, parent_logstart);
+	struct buffer_head *parent_bh = sb_bread(sb, blknr);
+	if (!parent_bh) {
+		fat_msg(sb, KERN_ERR,
+			"unable to read cluster of parent directory");
+		return NULL;
+	}
+
+	de = (struct msdos_dir_entry *) parent_bh->b_data;
+	clus_to_match = fat_get_start(sbi, &de[0]);
+	search_clus = fat_get_start(sbi, &de[1]);
+
+	dummy_grand_parent = fat_dget(sb, search_clus);
+	if (!dummy_grand_parent) {
+		dummy_grand_parent = new_inode(sb);
+		if (!dummy_grand_parent) {
+			brelse(parent_bh);
+			return parent;
+		}
+
+		dummy_grand_parent->i_ino = iunique(sb, MSDOS_ROOT_INO);
+		fat_fill_inode(dummy_grand_parent, &de[1]);
+		MSDOS_I(dummy_grand_parent)->i_pos = -1;
+	}
+
+	if (!fat_scan_logstart(dummy_grand_parent, clus_to_match, &sinfo))
+		parent = fat_build_inode(sb, sinfo.de, sinfo.i_pos);
+
+	brelse(parent_bh);
+	iput(dummy_grand_parent);
+
+	return parent;
+}
+
 /*
  * Find the parent for a directory that is not currently connected to
  * the filesystem root.
  *
  * On entry, the caller holds child_dir->d_inode->i_mutex.
  */
-struct dentry *fat_get_parent(struct dentry *child_dir)
+static struct dentry *fat_get_parent(struct dentry *child_dir)
 {
 	struct super_block *sb = child_dir->d_sb;
 	struct buffer_head *bh = NULL;
 	struct msdos_dir_entry *de;
 	struct inode *parent_inode = NULL;
+	struct msdos_sb_info *sbi = MSDOS_SB(sb);
 
 	if (!fat_get_dotdot_entry(child_dir->d_inode, &bh, &de)) {
-		int parent_logstart = fat_get_start(MSDOS_SB(sb), de);
+		int parent_logstart = fat_get_start(sbi, de);
 		parent_inode = fat_dget(sb, parent_logstart);
+		if (!parent_inode && sbi->options.nfs == FAT_NFS_NOSTALE_RO)
+			parent_inode = fat_rebuild_parent(sb, parent_logstart);
 	}
 	brelse(bh);
 
 	return d_obtain_alias(parent_inode);
 }
+
+const struct export_operations fat_export_ops = {
+	.fh_to_dentry   = fat_fh_to_dentry,
+	.fh_to_parent   = fat_fh_to_parent,
+	.get_parent     = fat_get_parent,
+};
+
+const struct export_operations fat_export_ops_nostale = {
+	.encode_fh      = fat_encode_fh_nostale,
+	.fh_to_dentry   = fat_fh_to_dentry_nostale,
+	.fh_to_parent   = fat_fh_to_parent_nostale,
+	.get_parent     = fat_get_parent,
+};
diff --git a/fs/fscache/stats.c b/fs/fscache/stats.c
index 8179e8bc4a3..40d13c70ef5 100644
--- a/fs/fscache/stats.c
+++ b/fs/fscache/stats.c
@@ -287,5 +287,5 @@ const struct file_operations fscache_stats_fops = {
 	.open		= fscache_stats_open,
 	.read		= seq_read,
 	.llseek		= seq_lseek,
-	.release	= seq_release,
+	.release        = single_release,
 };
diff --git a/fs/gfs2/aops.c b/fs/gfs2/aops.c
index 24f414f0ce6..9883694f1e7 100644
--- a/fs/gfs2/aops.c
+++ b/fs/gfs2/aops.c
@@ -1055,7 +1055,7 @@ int gfs2_releasepage(struct page *page, gfp_t gfp_mask)
 		if (atomic_read(&bh->b_count))
 			goto cannot_release;
 		bd = bh->b_private;
-		if (bd && bd->bd_ail)
+		if (bd && bd->bd_tr)
 			goto cannot_release;
 		if (buffer_pinned(bh) || buffer_dirty(bh))
 			goto not_possible;
diff --git a/fs/gfs2/bmap.c b/fs/gfs2/bmap.c
index 5e83657f046..1dc9a13ce6b 100644
--- a/fs/gfs2/bmap.c
+++ b/fs/gfs2/bmap.c
@@ -787,7 +787,7 @@ static int do_strip(struct gfs2_inode *ip, struct buffer_head *dibh,
 		goto out_rlist;
 
 	if (gfs2_rs_active(ip->i_res)) /* needs to be done with the rgrp glock held */
-		gfs2_rs_deltree(ip, ip->i_res);
+		gfs2_rs_deltree(ip->i_res);
 
 	error = gfs2_trans_begin(sdp, rg_blocks + RES_DINODE +
 				 RES_INDIRECT + RES_STATFS + RES_QUOTA,
diff --git a/fs/gfs2/file.c b/fs/gfs2/file.c
index 019f45e4509..d79c2dadc53 100644
--- a/fs/gfs2/file.c
+++ b/fs/gfs2/file.c
@@ -923,8 +923,11 @@ static int gfs2_lock(struct file *file, int cmd, struct file_lock *fl)
 		cmd = F_SETLK;
 		fl->fl_type = F_UNLCK;
 	}
-	if (unlikely(test_bit(SDF_SHUTDOWN, &sdp->sd_flags)))
+	if (unlikely(test_bit(SDF_SHUTDOWN, &sdp->sd_flags))) {
+		if (fl->fl_type == F_UNLCK)
+			posix_lock_file_wait(file, fl);
 		return -EIO;
+	}
 	if (IS_GETLK(cmd))
 		return dlm_posix_get(ls->ls_dlm, ip->i_no_addr, file, fl);
 	else if (fl->fl_type == F_UNLCK)
diff --git a/fs/gfs2/glock.c b/fs/gfs2/glock.c
index cf351554673..9435384562a 100644
--- a/fs/gfs2/glock.c
+++ b/fs/gfs2/glock.c
@@ -912,7 +912,7 @@ int gfs2_glock_wait(struct gfs2_holder *gh)
  */
 
 static void handle_callback(struct gfs2_glock *gl, unsigned int state,
-			    unsigned long delay)
+			    unsigned long delay, bool remote)
 {
 	int bit = delay ? GLF_PENDING_DEMOTE : GLF_DEMOTE;
 
@@ -925,8 +925,8 @@ static void handle_callback(struct gfs2_glock *gl, unsigned int state,
 		gl->gl_demote_state = LM_ST_UNLOCKED;
 	}
 	if (gl->gl_ops->go_callback)
-		gl->gl_ops->go_callback(gl);
-	trace_gfs2_demote_rq(gl);
+		gl->gl_ops->go_callback(gl, remote);
+	trace_gfs2_demote_rq(gl, remote);
 }
 
 void gfs2_print_dbg(struct seq_file *seq, const char *fmt, ...)
@@ -1017,11 +1017,11 @@ do_cancel:
 	return;
 
 trap_recursive:
-	print_symbol(KERN_ERR "original: %s\n", gh2->gh_ip);
+	printk(KERN_ERR "original: %pSR\n", (void *)gh2->gh_ip);
 	printk(KERN_ERR "pid: %d\n", pid_nr(gh2->gh_owner_pid));
 	printk(KERN_ERR "lock type: %d req lock state : %d\n",
 	       gh2->gh_gl->gl_name.ln_type, gh2->gh_state);
-	print_symbol(KERN_ERR "new: %s\n", gh->gh_ip);
+	printk(KERN_ERR "new: %pSR\n", (void *)gh->gh_ip);
 	printk(KERN_ERR "pid: %d\n", pid_nr(gh->gh_owner_pid));
 	printk(KERN_ERR "lock type: %d req lock state : %d\n",
 	       gh->gh_gl->gl_name.ln_type, gh->gh_state);
@@ -1091,7 +1091,7 @@ void gfs2_glock_dq(struct gfs2_holder *gh)
 
 	spin_lock(&gl->gl_spin);
 	if (gh->gh_flags & GL_NOCACHE)
-		handle_callback(gl, LM_ST_UNLOCKED, 0);
+		handle_callback(gl, LM_ST_UNLOCKED, 0, false);
 
 	list_del_init(&gh->gh_list);
 	if (find_first_holder(gl) == NULL) {
@@ -1279,19 +1279,6 @@ void gfs2_glock_dq_m(unsigned int num_gh, struct gfs2_holder *ghs)
 		gfs2_glock_dq(&ghs[num_gh]);
 }
 
-/**
- * gfs2_glock_dq_uninit_m - release multiple glocks
- * @num_gh: the number of structures
- * @ghs: an array of struct gfs2_holder structures
- *
- */
-
-void gfs2_glock_dq_uninit_m(unsigned int num_gh, struct gfs2_holder *ghs)
-{
-	while (num_gh--)
-		gfs2_glock_dq_uninit(&ghs[num_gh]);
-}
-
 void gfs2_glock_cb(struct gfs2_glock *gl, unsigned int state)
 {
 	unsigned long delay = 0;
@@ -1309,7 +1296,7 @@ void gfs2_glock_cb(struct gfs2_glock *gl, unsigned int state)
 	}
 
 	spin_lock(&gl->gl_spin);
-	handle_callback(gl, state, delay);
+	handle_callback(gl, state, delay, true);
 	spin_unlock(&gl->gl_spin);
 	if (queue_delayed_work(glock_workqueue, &gl->gl_work, delay) == 0)
 		gfs2_glock_put(gl);
@@ -1422,7 +1409,7 @@ __acquires(&lru_lock)
 		spin_unlock(&lru_lock);
 		spin_lock(&gl->gl_spin);
 		if (demote_ok(gl))
-			handle_callback(gl, LM_ST_UNLOCKED, 0);
+			handle_callback(gl, LM_ST_UNLOCKED, 0, false);
 		WARN_ON(!test_and_clear_bit(GLF_LOCK, &gl->gl_flags));
 		smp_mb__after_clear_bit();
 		if (queue_delayed_work(glock_workqueue, &gl->gl_work, 0) == 0)
@@ -1547,7 +1534,7 @@ static void clear_glock(struct gfs2_glock *gl)
 
 	spin_lock(&gl->gl_spin);
 	if (gl->gl_state != LM_ST_UNLOCKED)
-		handle_callback(gl, LM_ST_UNLOCKED, 0);
+		handle_callback(gl, LM_ST_UNLOCKED, 0, false);
 	spin_unlock(&gl->gl_spin);
 	gfs2_glock_hold(gl);
 	if (queue_delayed_work(glock_workqueue, &gl->gl_work, 0) == 0)
@@ -1590,6 +1577,7 @@ static void dump_glock_func(struct gfs2_glock *gl)
 void gfs2_gl_hash_clear(struct gfs2_sbd *sdp)
 {
 	set_bit(SDF_SKIP_DLM_UNLOCK, &sdp->sd_flags);
+	flush_workqueue(glock_workqueue);
 	glock_hash_walk(clear_glock, sdp);
 	flush_workqueue(glock_workqueue);
 	wait_event(sdp->sd_glock_wait, atomic_read(&sdp->sd_glock_disposal) == 0);
diff --git a/fs/gfs2/glock.h b/fs/gfs2/glock.h
index fd580b7861d..69f66e3d22b 100644
--- a/fs/gfs2/glock.h
+++ b/fs/gfs2/glock.h
@@ -201,7 +201,6 @@ extern int gfs2_glock_nq_num(struct gfs2_sbd *sdp, u64 number,
 			     struct gfs2_holder *gh);
 extern int gfs2_glock_nq_m(unsigned int num_gh, struct gfs2_holder *ghs);
 extern void gfs2_glock_dq_m(unsigned int num_gh, struct gfs2_holder *ghs);
-extern void gfs2_glock_dq_uninit_m(unsigned int num_gh, struct gfs2_holder *ghs);
 extern int gfs2_dump_glock(struct seq_file *seq, const struct gfs2_glock *gl);
 #define GLOCK_BUG_ON(gl,x) do { if (unlikely(x)) { gfs2_dump_glock(NULL, gl); BUG(); } } while(0)
 extern __printf(2, 3)
diff --git a/fs/gfs2/glops.c b/fs/gfs2/glops.c
index 444b6503ebc..c66e99c9757 100644
--- a/fs/gfs2/glops.c
+++ b/fs/gfs2/glops.c
@@ -515,12 +515,12 @@ static int trans_go_demote_ok(const struct gfs2_glock *gl)
  *
  * gl_spin lock is held while calling this
  */
-static void iopen_go_callback(struct gfs2_glock *gl)
+static void iopen_go_callback(struct gfs2_glock *gl, bool remote)
 {
 	struct gfs2_inode *ip = (struct gfs2_inode *)gl->gl_object;
 	struct gfs2_sbd *sdp = gl->gl_sbd;
 
-	if (sdp->sd_vfs->s_flags & MS_RDONLY)
+	if (!remote || (sdp->sd_vfs->s_flags & MS_RDONLY))
 		return;
 
 	if (gl->gl_demote_state == LM_ST_UNLOCKED &&
diff --git a/fs/gfs2/incore.h b/fs/gfs2/incore.h
index 156e42ec84e..26aabd7caba 100644
--- a/fs/gfs2/incore.h
+++ b/fs/gfs2/incore.h
@@ -31,7 +31,6 @@ struct gfs2_holder;
 struct gfs2_glock;
 struct gfs2_quota_data;
 struct gfs2_trans;
-struct gfs2_ail;
 struct gfs2_jdesc;
 struct gfs2_sbd;
 struct lm_lockops;
@@ -53,7 +52,7 @@ struct gfs2_log_header_host {
 
 struct gfs2_log_operations {
 	void (*lo_before_commit) (struct gfs2_sbd *sdp);
-	void (*lo_after_commit) (struct gfs2_sbd *sdp, struct gfs2_ail *ai);
+	void (*lo_after_commit) (struct gfs2_sbd *sdp, struct gfs2_trans *tr);
 	void (*lo_before_scan) (struct gfs2_jdesc *jd,
 				struct gfs2_log_header_host *head, int pass);
 	int (*lo_scan_elements) (struct gfs2_jdesc *jd, unsigned int start,
@@ -139,7 +138,7 @@ struct gfs2_bufdata {
 	struct list_head bd_list;
 	const struct gfs2_log_operations *bd_ops;
 
-	struct gfs2_ail *bd_ail;
+	struct gfs2_trans *bd_tr;
 	struct list_head bd_ail_st_list;
 	struct list_head bd_ail_gl_list;
 };
@@ -211,7 +210,7 @@ struct gfs2_glock_operations {
 	int (*go_lock) (struct gfs2_holder *gh);
 	void (*go_unlock) (struct gfs2_holder *gh);
 	int (*go_dump)(struct seq_file *seq, const struct gfs2_glock *gl);
-	void (*go_callback) (struct gfs2_glock *gl);
+	void (*go_callback)(struct gfs2_glock *gl, bool remote);
 	const int go_type;
 	const unsigned long go_flags;
 #define GLOF_ASPACE 1
@@ -433,6 +432,7 @@ struct gfs2_trans {
 	struct gfs2_holder tr_t_gh;
 
 	int tr_touched;
+	int tr_attached;
 
 	unsigned int tr_num_buf_new;
 	unsigned int tr_num_databuf_new;
@@ -440,14 +440,12 @@ struct gfs2_trans {
 	unsigned int tr_num_databuf_rm;
 	unsigned int tr_num_revoke;
 	unsigned int tr_num_revoke_rm;
-};
 
-struct gfs2_ail {
-	struct list_head ai_list;
+	struct list_head tr_list;
 
-	unsigned int ai_first;
-	struct list_head ai_ail1_list;
-	struct list_head ai_ail2_list;
+	unsigned int tr_first;
+	struct list_head tr_ail1_list;
+	struct list_head tr_ail2_list;
 };
 
 struct gfs2_journal_extent {
@@ -588,6 +586,7 @@ struct lm_lockstruct {
 	struct dlm_lksb ls_control_lksb; /* control_lock */
 	char ls_control_lvb[GDLM_LVB_SIZE]; /* control_lock lvb */
 	struct completion ls_sync_wait; /* {control,mounted}_{lock,unlock} */
+	char *ls_lvb_bits;
 
 	spinlock_t ls_recover_spin; /* protects following fields */
 	unsigned long ls_recover_flags; /* DFL_ */
@@ -709,6 +708,7 @@ struct gfs2_sbd {
 
 	spinlock_t sd_log_lock;
 
+	struct gfs2_trans *sd_log_tr;
 	unsigned int sd_log_blks_reserved;
 	unsigned int sd_log_commited_buf;
 	unsigned int sd_log_commited_databuf;
diff --git a/fs/gfs2/inode.c b/fs/gfs2/inode.c
index cc00bd1d1f8..8833a4f264e 100644
--- a/fs/gfs2/inode.c
+++ b/fs/gfs2/inode.c
@@ -392,11 +392,15 @@ static int alloc_dinode(struct gfs2_inode *ip, u32 flags)
 	int error;
 	int dblocks = 1;
 
-	error = gfs2_inplace_reserve(ip, RES_DINODE, flags);
+	error = gfs2_quota_lock_check(ip);
 	if (error)
 		goto out;
 
-	error = gfs2_trans_begin(sdp, RES_RG_BIT + RES_STATFS, 0);
+	error = gfs2_inplace_reserve(ip, RES_DINODE, flags);
+	if (error)
+		goto out_quota;
+
+	error = gfs2_trans_begin(sdp, RES_RG_BIT + RES_STATFS + RES_QUOTA, 0);
 	if (error)
 		goto out_ipreserv;
 
@@ -409,6 +413,8 @@ static int alloc_dinode(struct gfs2_inode *ip, u32 flags)
 
 out_ipreserv:
 	gfs2_inplace_release(ip);
+out_quota:
+	gfs2_quota_unlock(ip);
 out:
 	return error;
 }
@@ -440,59 +446,27 @@ static void gfs2_init_dir(struct buffer_head *dibh,
  */
 
 static void init_dinode(struct gfs2_inode *dip, struct gfs2_inode *ip,
-			const char *symname, struct buffer_head **bhp)
+			const char *symname)
 {
-	struct gfs2_sbd *sdp = GFS2_SB(&dip->i_inode);
 	struct gfs2_dinode *di;
 	struct buffer_head *dibh;
-	struct timespec tv = CURRENT_TIME;
 
 	dibh = gfs2_meta_new(ip->i_gl, ip->i_no_addr);
 	gfs2_trans_add_meta(ip->i_gl, dibh);
-	gfs2_metatype_set(dibh, GFS2_METATYPE_DI, GFS2_FORMAT_DI);
-	gfs2_buffer_clear_tail(dibh, sizeof(struct gfs2_dinode));
 	di = (struct gfs2_dinode *)dibh->b_data;
+	gfs2_dinode_out(ip, di);
 
-	di->di_num.no_formal_ino = cpu_to_be64(ip->i_no_formal_ino);
-	di->di_num.no_addr = cpu_to_be64(ip->i_no_addr);
-	di->di_mode = cpu_to_be32(ip->i_inode.i_mode);
-	di->di_uid = cpu_to_be32(i_uid_read(&ip->i_inode));
-	di->di_gid = cpu_to_be32(i_gid_read(&ip->i_inode));
-	di->di_nlink = 0;
-	di->di_size = cpu_to_be64(ip->i_inode.i_size);
-	di->di_blocks = cpu_to_be64(1);
-	di->di_atime = di->di_mtime = di->di_ctime = cpu_to_be64(tv.tv_sec);
 	di->di_major = cpu_to_be32(MAJOR(ip->i_inode.i_rdev));
 	di->di_minor = cpu_to_be32(MINOR(ip->i_inode.i_rdev));
-	di->di_goal_meta = di->di_goal_data = cpu_to_be64(ip->i_no_addr);
-	di->di_generation = cpu_to_be64(ip->i_generation);
-	di->di_flags = 0;
 	di->__pad1 = 0;
-	di->di_payload_format = cpu_to_be32(S_ISDIR(ip->i_inode.i_mode) ? GFS2_FORMAT_DE : 0);
-	di->di_height = 0;
 	di->__pad2 = 0;
 	di->__pad3 = 0;
-	di->di_depth = 0;
-	di->di_entries = 0;
 	memset(&di->__pad4, 0, sizeof(di->__pad4));
-	di->di_eattr = 0;
-	di->di_atime_nsec = cpu_to_be32(tv.tv_nsec);
-	di->di_mtime_nsec = cpu_to_be32(tv.tv_nsec);
-	di->di_ctime_nsec = cpu_to_be32(tv.tv_nsec);
 	memset(&di->di_reserved, 0, sizeof(di->di_reserved));
+	gfs2_buffer_clear_tail(dibh, sizeof(struct gfs2_dinode));
 
 	switch(ip->i_inode.i_mode & S_IFMT) {
-	case S_IFREG:
-		if ((dip->i_diskflags & GFS2_DIF_INHERIT_JDATA) ||
-		    gfs2_tune_get(sdp, gt_new_files_jdata))
-			di->di_flags |= cpu_to_be32(GFS2_DIF_JDATA);
-		break;
 	case S_IFDIR:
-		di->di_flags |= cpu_to_be32(dip->i_diskflags &
-					    GFS2_DIF_INHERIT_JDATA);
-		di->di_flags |= cpu_to_be32(GFS2_DIF_JDATA);
-		di->di_size = cpu_to_be64(sdp->sd_sb.sb_bsize - sizeof(struct gfs2_dinode));
-		di->di_entries = cpu_to_be32(2);
 		gfs2_init_dir(dibh, dip);
 		break;
 	case S_IFLNK:
@@ -501,63 +475,17 @@ static void init_dinode(struct gfs2_inode *dip, struct gfs2_inode *ip,
 	}
 
 	set_buffer_uptodate(dibh);
-
-	*bhp = dibh;
-}
-
-static int make_dinode(struct gfs2_inode *dip, struct gfs2_inode *ip,
-		       const char *symname, struct buffer_head **bhp)
-{
-	struct inode *inode = &ip->i_inode;
-	struct gfs2_sbd *sdp = GFS2_SB(&dip->i_inode);
-	int error;
-
-	error = gfs2_rindex_update(sdp);
-	if (error)
-		return error;
-
-	error = gfs2_quota_lock(dip, inode->i_uid, inode->i_gid);
-	if (error)
-		return error;
-
-	error = gfs2_quota_check(dip, inode->i_uid, inode->i_gid);
-	if (error)
-		goto out_quota;
-
-	error = gfs2_trans_begin(sdp, RES_DINODE + RES_QUOTA, 0);
-	if (error)
-		goto out_quota;
-
-	init_dinode(dip, ip, symname, bhp);
-	gfs2_quota_change(dip, +1, inode->i_uid, inode->i_gid);
-	gfs2_trans_end(sdp);
-
-out_quota:
-	gfs2_quota_unlock(dip);
-	return error;
+	brelse(dibh);
 }
 
 static int link_dinode(struct gfs2_inode *dip, const struct qstr *name,
-		       struct gfs2_inode *ip)
+		       struct gfs2_inode *ip, int arq)
 {
 	struct gfs2_sbd *sdp = GFS2_SB(&dip->i_inode);
-	int alloc_required;
-	struct buffer_head *dibh;
 	int error;
 
-	error = gfs2_rindex_update(sdp);
-	if (error)
-		return error;
-
-	error = gfs2_quota_lock(dip, NO_UID_QUOTA_CHANGE, NO_GID_QUOTA_CHANGE);
-	if (error)
-		goto fail;
-
-	error = alloc_required = gfs2_diradd_alloc_required(&dip->i_inode, name);
-	if (alloc_required < 0)
-		goto fail_quota_locks;
-	if (alloc_required) {
-		error = gfs2_quota_check(dip, dip->i_inode.i_uid, dip->i_inode.i_gid);
+	if (arq) {
+		error = gfs2_quota_lock_check(dip);
 		if (error)
 			goto fail_quota_locks;
 
@@ -581,26 +509,12 @@ static int link_dinode(struct gfs2_inode *dip, const struct qstr *name,
 	if (error)
 		goto fail_end_trans;
 
-	error = gfs2_meta_inode_buffer(ip, &dibh);
-	if (error)
-		goto fail_end_trans;
-	set_nlink(&ip->i_inode, S_ISDIR(ip->i_inode.i_mode) ? 2 : 1);
-	gfs2_trans_add_meta(ip->i_gl, dibh);
-	gfs2_dinode_out(ip, dibh->b_data);
-	brelse(dibh);
-	return 0;
-
 fail_end_trans:
 	gfs2_trans_end(sdp);
-
 fail_ipreserv:
-	if (alloc_required)
-		gfs2_inplace_release(dip);
-
+	gfs2_inplace_release(dip);
 fail_quota_locks:
 	gfs2_quota_unlock(dip);
-
-fail:
 	return error;
 }
 
@@ -650,8 +564,8 @@ static int gfs2_create_inode(struct inode *dir, struct dentry *dentry,
 	struct gfs2_sbd *sdp = GFS2_SB(&dip->i_inode);
 	struct gfs2_glock *io_gl;
 	int error;
-	struct buffer_head *bh = NULL;
 	u32 aflags = 0;
+	int arq;
 
 	if (!name->len || name->len > GFS2_FNAMESIZE)
 		return -ENAMETOOLONG;
@@ -660,6 +574,10 @@ static int gfs2_create_inode(struct inode *dir, struct dentry *dentry,
 	if (error)
 		return error;
 
+	error = gfs2_rindex_update(sdp);
+	if (error)
+		return error;
+
 	error = gfs2_glock_nq_init(dip->i_gl, LM_ST_EXCLUSIVE, 0, ghs);
 	if (error)
 		goto fail;
@@ -674,22 +592,48 @@ static int gfs2_create_inode(struct inode *dir, struct dentry *dentry,
 	if (error)
 		goto fail_gunlock;
 
+	arq = error = gfs2_diradd_alloc_required(dir, name);
+	if (error < 0)
+		goto fail_gunlock;
+
 	inode = new_inode(sdp->sd_vfs);
-	if (!inode) {
-		gfs2_glock_dq_uninit(ghs);
-		return -ENOMEM;
-	}
+	error = -ENOMEM;
+	if (!inode)
+		goto fail_gunlock;
+
 	ip = GFS2_I(inode);
 	error = gfs2_rs_alloc(ip);
 	if (error)
 		goto fail_free_inode;
 
-	set_bit(GIF_INVALID, &ip->i_flags);
 	inode->i_mode = mode;
+	set_nlink(inode, S_ISDIR(mode) ? 2 : 1);
 	inode->i_rdev = dev;
 	inode->i_size = size;
+	inode->i_atime = inode->i_mtime = inode->i_ctime = CURRENT_TIME;
+	gfs2_set_inode_blocks(inode, 1);
 	munge_mode_uid_gid(dip, inode);
 	ip->i_goal = dip->i_goal;
+	ip->i_diskflags = 0;
+	ip->i_eattr = 0;
+	ip->i_height = 0;
+	ip->i_depth = 0;
+	ip->i_entries = 0;
+
+	switch(mode & S_IFMT) {
+	case S_IFREG:
+		if ((dip->i_diskflags & GFS2_DIF_INHERIT_JDATA) ||
+		    gfs2_tune_get(sdp, gt_new_files_jdata))
+			ip->i_diskflags |= GFS2_DIF_JDATA;
+		gfs2_set_aops(inode);
+		break;
+	case S_IFDIR:
+		ip->i_diskflags |= (dip->i_diskflags & GFS2_DIF_INHERIT_JDATA);
+		ip->i_diskflags |= GFS2_DIF_JDATA;
+		ip->i_entries = 2;
+		break;
+	}
+	gfs2_set_inode_flags(inode);
 
 	if ((GFS2_I(sdp->sd_root_dir->d_inode) == dip) ||
 	    (dip->i_diskflags & GFS2_DIF_TOPDIR))
@@ -708,10 +652,13 @@ static int gfs2_create_inode(struct inode *dir, struct dentry *dentry,
 	if (error)
 		goto fail_free_inode;
 
-	error = make_dinode(dip, ip, symname, &bh);
+	error = gfs2_trans_begin(sdp, RES_DINODE, 0);
 	if (error)
 		goto fail_gunlock2;
 
+	init_dinode(dip, ip, symname);
+	gfs2_trans_end(sdp);
+
 	error = gfs2_glock_get(sdp, ip->i_no_addr, &gfs2_iopen_glops, CREATE, &io_gl);
 	if (error)
 		goto fail_gunlock2;
@@ -725,10 +672,6 @@ static int gfs2_create_inode(struct inode *dir, struct dentry *dentry,
 	gfs2_set_iop(inode);
 	insert_inode_hash(inode);
 
-	error = gfs2_inode_refresh(ip);
-	if (error)
-		goto fail_gunlock3;
-
 	error = gfs2_acl_create(dip, inode);
 	if (error)
 		goto fail_gunlock3;
@@ -737,18 +680,13 @@ static int gfs2_create_inode(struct inode *dir, struct dentry *dentry,
 	if (error)
 		goto fail_gunlock3;
 
-	error = link_dinode(dip, name, ip);
+	error = link_dinode(dip, name, ip, arq);
 	if (error)
 		goto fail_gunlock3;
 
-	if (bh)
-		brelse(bh);
-
-	gfs2_trans_end(sdp);
-	gfs2_inplace_release(dip);
-	gfs2_quota_unlock(dip);
 	mark_inode_dirty(inode);
-	gfs2_glock_dq_uninit_m(2, ghs);
+	gfs2_glock_dq_uninit(ghs);
+	gfs2_glock_dq_uninit(ghs + 1);
 	d_instantiate(dentry, inode);
 	return 0;
 
@@ -769,12 +707,12 @@ fail_free_inode:
 fail_gunlock:
 	gfs2_glock_dq_uninit(ghs);
 	if (inode && !IS_ERR(inode)) {
+		clear_nlink(inode);
+		mark_inode_dirty(inode);
 		set_bit(GIF_ALLOC_FAILED, &GFS2_I(inode)->i_flags);
 		iput(inode);
 	}
 fail:
-	if (bh)
-		brelse(bh);
 	return error;
 }
 
@@ -1151,7 +1089,9 @@ static int gfs2_symlink(struct inode *dir, struct dentry *dentry,
 
 static int gfs2_mkdir(struct inode *dir, struct dentry *dentry, umode_t mode)
 {
-	return gfs2_create_inode(dir, dentry, S_IFDIR | mode, 0, NULL, 0, 0);
+	struct gfs2_sbd *sdp = GFS2_SB(dir);
+	unsigned dsize = sdp->sd_sb.sb_bsize - sizeof(struct gfs2_dinode);
+	return gfs2_create_inode(dir, dentry, S_IFDIR | mode, 0, NULL, dsize, 0);
 }
 
 /**
diff --git a/fs/gfs2/lock_dlm.c b/fs/gfs2/lock_dlm.c
index 9802de0f85e..c8423d6de6c 100644
--- a/fs/gfs2/lock_dlm.c
+++ b/fs/gfs2/lock_dlm.c
@@ -483,12 +483,8 @@ static void control_lvb_write(struct lm_lockstruct *ls, uint32_t lvb_gen,
 
 static int all_jid_bits_clear(char *lvb)
 {
-	int i;
-	for (i = JID_BITMAP_OFFSET; i < GDLM_LVB_SIZE; i++) {
-		if (lvb[i])
-			return 0;
-	}
-	return 1;
+	return !memchr_inv(lvb + JID_BITMAP_OFFSET, 0,
+			GDLM_LVB_SIZE - JID_BITMAP_OFFSET);
 }
 
 static void sync_wait_cb(void *arg)
@@ -580,7 +576,6 @@ static void gfs2_control_func(struct work_struct *work)
 {
 	struct gfs2_sbd *sdp = container_of(work, struct gfs2_sbd, sd_control_work.work);
 	struct lm_lockstruct *ls = &sdp->sd_lockstruct;
-	char lvb_bits[GDLM_LVB_SIZE];
 	uint32_t block_gen, start_gen, lvb_gen, flags;
 	int recover_set = 0;
 	int write_lvb = 0;
@@ -634,7 +629,7 @@ static void gfs2_control_func(struct work_struct *work)
 		return;
 	}
 
-	control_lvb_read(ls, &lvb_gen, lvb_bits);
+	control_lvb_read(ls, &lvb_gen, ls->ls_lvb_bits);
 
 	spin_lock(&ls->ls_recover_spin);
 	if (block_gen != ls->ls_recover_block ||
@@ -664,10 +659,10 @@ static void gfs2_control_func(struct work_struct *work)
 
 			ls->ls_recover_result[i] = 0;
 
-			if (!test_bit_le(i, lvb_bits + JID_BITMAP_OFFSET))
+			if (!test_bit_le(i, ls->ls_lvb_bits + JID_BITMAP_OFFSET))
 				continue;
 
-			__clear_bit_le(i, lvb_bits + JID_BITMAP_OFFSET);
+			__clear_bit_le(i, ls->ls_lvb_bits + JID_BITMAP_OFFSET);
 			write_lvb = 1;
 		}
 	}
@@ -691,7 +686,7 @@ static void gfs2_control_func(struct work_struct *work)
 				continue;
 			if (ls->ls_recover_submit[i] < start_gen) {
 				ls->ls_recover_submit[i] = 0;
-				__set_bit_le(i, lvb_bits + JID_BITMAP_OFFSET);
+				__set_bit_le(i, ls->ls_lvb_bits + JID_BITMAP_OFFSET);
 			}
 		}
 		/* even if there are no bits to set, we need to write the
@@ -705,7 +700,7 @@ static void gfs2_control_func(struct work_struct *work)
 	spin_unlock(&ls->ls_recover_spin);
 
 	if (write_lvb) {
-		control_lvb_write(ls, start_gen, lvb_bits);
+		control_lvb_write(ls, start_gen, ls->ls_lvb_bits);
 		flags = DLM_LKF_CONVERT | DLM_LKF_VALBLK;
 	} else {
 		flags = DLM_LKF_CONVERT;
@@ -725,7 +720,7 @@ static void gfs2_control_func(struct work_struct *work)
 	 */
 
 	for (i = 0; i < recover_size; i++) {
-		if (test_bit_le(i, lvb_bits + JID_BITMAP_OFFSET)) {
+		if (test_bit_le(i, ls->ls_lvb_bits + JID_BITMAP_OFFSET)) {
 			fs_info(sdp, "recover generation %u jid %d\n",
 				start_gen, i);
 			gfs2_recover_set(sdp, i);
@@ -758,7 +753,6 @@ static void gfs2_control_func(struct work_struct *work)
 static int control_mount(struct gfs2_sbd *sdp)
 {
 	struct lm_lockstruct *ls = &sdp->sd_lockstruct;
-	char lvb_bits[GDLM_LVB_SIZE];
 	uint32_t start_gen, block_gen, mount_gen, lvb_gen;
 	int mounted_mode;
 	int retries = 0;
@@ -857,7 +851,7 @@ locks_done:
 	 * lvb_gen will be non-zero.
 	 */
 
-	control_lvb_read(ls, &lvb_gen, lvb_bits);
+	control_lvb_read(ls, &lvb_gen, ls->ls_lvb_bits);
 
 	if (lvb_gen == 0xFFFFFFFF) {
 		/* special value to force mount attempts to fail */
@@ -887,7 +881,7 @@ locks_done:
 	 * and all lvb bits to be clear (no pending journal recoveries.)
 	 */
 
-	if (!all_jid_bits_clear(lvb_bits)) {
+	if (!all_jid_bits_clear(ls->ls_lvb_bits)) {
 		/* journals need recovery, wait until all are clear */
 		fs_info(sdp, "control_mount wait for journal recovery\n");
 		goto restart;
@@ -949,7 +943,6 @@ static int dlm_recovery_wait(void *word)
 static int control_first_done(struct gfs2_sbd *sdp)
 {
 	struct lm_lockstruct *ls = &sdp->sd_lockstruct;
-	char lvb_bits[GDLM_LVB_SIZE];
 	uint32_t start_gen, block_gen;
 	int error;
 
@@ -991,8 +984,8 @@ restart:
 	memset(ls->ls_recover_result, 0, ls->ls_recover_size*sizeof(uint32_t));
 	spin_unlock(&ls->ls_recover_spin);
 
-	memset(lvb_bits, 0, sizeof(lvb_bits));
-	control_lvb_write(ls, start_gen, lvb_bits);
+	memset(ls->ls_lvb_bits, 0, GDLM_LVB_SIZE);
+	control_lvb_write(ls, start_gen, ls->ls_lvb_bits);
 
 	error = mounted_lock(sdp, DLM_LOCK_PR, DLM_LKF_CONVERT);
 	if (error)
@@ -1022,6 +1015,12 @@ static int set_recover_size(struct gfs2_sbd *sdp, struct dlm_slot *slots,
 	uint32_t old_size, new_size;
 	int i, max_jid;
 
+	if (!ls->ls_lvb_bits) {
+		ls->ls_lvb_bits = kzalloc(GDLM_LVB_SIZE, GFP_NOFS);
+		if (!ls->ls_lvb_bits)
+			return -ENOMEM;
+	}
+
 	max_jid = 0;
 	for (i = 0; i < num_slots; i++) {
 		if (max_jid < slots[i].slot - 1)
@@ -1057,6 +1056,7 @@ static int set_recover_size(struct gfs2_sbd *sdp, struct dlm_slot *slots,
 
 static void free_recover_size(struct lm_lockstruct *ls)
 {
+	kfree(ls->ls_lvb_bits);
 	kfree(ls->ls_recover_submit);
 	kfree(ls->ls_recover_result);
 	ls->ls_recover_submit = NULL;
@@ -1205,6 +1205,7 @@ static int gdlm_mount(struct gfs2_sbd *sdp, const char *table)
 	ls->ls_recover_size = 0;
 	ls->ls_recover_submit = NULL;
 	ls->ls_recover_result = NULL;
+	ls->ls_lvb_bits = NULL;
 
 	error = set_recover_size(sdp, NULL, 0);
 	if (error)
diff --git a/fs/gfs2/log.c b/fs/gfs2/log.c
index 9a2ca8be764..b404f485303 100644
--- a/fs/gfs2/log.c
+++ b/fs/gfs2/log.c
@@ -73,7 +73,7 @@ unsigned int gfs2_struct2blk(struct gfs2_sbd *sdp, unsigned int nstruct,
 
 void gfs2_remove_from_ail(struct gfs2_bufdata *bd)
 {
-	bd->bd_ail = NULL;
+	bd->bd_tr = NULL;
 	list_del_init(&bd->bd_ail_st_list);
 	list_del_init(&bd->bd_ail_gl_list);
 	atomic_dec(&bd->bd_gl->gl_ail_count);
@@ -90,7 +90,7 @@ void gfs2_remove_from_ail(struct gfs2_bufdata *bd)
 
 static int gfs2_ail1_start_one(struct gfs2_sbd *sdp,
 			       struct writeback_control *wbc,
-			       struct gfs2_ail *ai)
+			       struct gfs2_trans *tr)
 __releases(&sdp->sd_ail_lock)
 __acquires(&sdp->sd_ail_lock)
 {
@@ -99,15 +99,15 @@ __acquires(&sdp->sd_ail_lock)
 	struct gfs2_bufdata *bd, *s;
 	struct buffer_head *bh;
 
-	list_for_each_entry_safe_reverse(bd, s, &ai->ai_ail1_list, bd_ail_st_list) {
+	list_for_each_entry_safe_reverse(bd, s, &tr->tr_ail1_list, bd_ail_st_list) {
 		bh = bd->bd_bh;
 
-		gfs2_assert(sdp, bd->bd_ail == ai);
+		gfs2_assert(sdp, bd->bd_tr == tr);
 
 		if (!buffer_busy(bh)) {
 			if (!buffer_uptodate(bh))
 				gfs2_io_error_bh(sdp, bh);
-			list_move(&bd->bd_ail_st_list, &ai->ai_ail2_list);
+			list_move(&bd->bd_ail_st_list, &tr->tr_ail2_list);
 			continue;
 		}
 
@@ -116,7 +116,7 @@ __acquires(&sdp->sd_ail_lock)
 		if (gl == bd->bd_gl)
 			continue;
 		gl = bd->bd_gl;
-		list_move(&bd->bd_ail_st_list, &ai->ai_ail1_list);
+		list_move(&bd->bd_ail_st_list, &tr->tr_ail1_list);
 		mapping = bh->b_page->mapping;
 		if (!mapping)
 			continue;
@@ -144,15 +144,15 @@ __acquires(&sdp->sd_ail_lock)
 void gfs2_ail1_flush(struct gfs2_sbd *sdp, struct writeback_control *wbc)
 {
 	struct list_head *head = &sdp->sd_ail1_list;
-	struct gfs2_ail *ai;
+	struct gfs2_trans *tr;
 
 	trace_gfs2_ail_flush(sdp, wbc, 1);
 	spin_lock(&sdp->sd_ail_lock);
 restart:
-	list_for_each_entry_reverse(ai, head, ai_list) {
+	list_for_each_entry_reverse(tr, head, tr_list) {
 		if (wbc->nr_to_write <= 0)
 			break;
-		if (gfs2_ail1_start_one(sdp, wbc, ai))
+		if (gfs2_ail1_start_one(sdp, wbc, tr))
 			goto restart;
 	}
 	spin_unlock(&sdp->sd_ail_lock);
@@ -183,20 +183,20 @@ static void gfs2_ail1_start(struct gfs2_sbd *sdp)
  *
  */
 
-static void gfs2_ail1_empty_one(struct gfs2_sbd *sdp, struct gfs2_ail *ai)
+static void gfs2_ail1_empty_one(struct gfs2_sbd *sdp, struct gfs2_trans *tr)
 {
 	struct gfs2_bufdata *bd, *s;
 	struct buffer_head *bh;
 
-	list_for_each_entry_safe_reverse(bd, s, &ai->ai_ail1_list,
+	list_for_each_entry_safe_reverse(bd, s, &tr->tr_ail1_list,
 					 bd_ail_st_list) {
 		bh = bd->bd_bh;
-		gfs2_assert(sdp, bd->bd_ail == ai);
+		gfs2_assert(sdp, bd->bd_tr == tr);
 		if (buffer_busy(bh))
 			continue;
 		if (!buffer_uptodate(bh))
 			gfs2_io_error_bh(sdp, bh);
-		list_move(&bd->bd_ail_st_list, &ai->ai_ail2_list);
+		list_move(&bd->bd_ail_st_list, &tr->tr_ail2_list);
 	}
 
 }
@@ -210,14 +210,14 @@ static void gfs2_ail1_empty_one(struct gfs2_sbd *sdp, struct gfs2_ail *ai)
 
 static int gfs2_ail1_empty(struct gfs2_sbd *sdp)
 {
-	struct gfs2_ail *ai, *s;
+	struct gfs2_trans *tr, *s;
 	int ret;
 
 	spin_lock(&sdp->sd_ail_lock);
-	list_for_each_entry_safe_reverse(ai, s, &sdp->sd_ail1_list, ai_list) {
-		gfs2_ail1_empty_one(sdp, ai);
-		if (list_empty(&ai->ai_ail1_list))
-			list_move(&ai->ai_list, &sdp->sd_ail2_list);
+	list_for_each_entry_safe_reverse(tr, s, &sdp->sd_ail1_list, tr_list) {
+		gfs2_ail1_empty_one(sdp, tr);
+		if (list_empty(&tr->tr_ail1_list))
+			list_move(&tr->tr_list, &sdp->sd_ail2_list);
 		else
 			break;
 	}
@@ -229,13 +229,13 @@ static int gfs2_ail1_empty(struct gfs2_sbd *sdp)
 
 static void gfs2_ail1_wait(struct gfs2_sbd *sdp)
 {
-	struct gfs2_ail *ai;
+	struct gfs2_trans *tr;
 	struct gfs2_bufdata *bd;
 	struct buffer_head *bh;
 
 	spin_lock(&sdp->sd_ail_lock);
-	list_for_each_entry_reverse(ai, &sdp->sd_ail1_list, ai_list) {
-		list_for_each_entry(bd, &ai->ai_ail1_list, bd_ail_st_list) {
+	list_for_each_entry_reverse(tr, &sdp->sd_ail1_list, tr_list) {
+		list_for_each_entry(bd, &tr->tr_ail1_list, bd_ail_st_list) {
 			bh = bd->bd_bh;
 			if (!buffer_locked(bh))
 				continue;
@@ -256,40 +256,40 @@ static void gfs2_ail1_wait(struct gfs2_sbd *sdp)
  *
  */
 
-static void gfs2_ail2_empty_one(struct gfs2_sbd *sdp, struct gfs2_ail *ai)
+static void gfs2_ail2_empty_one(struct gfs2_sbd *sdp, struct gfs2_trans *tr)
 {
-	struct list_head *head = &ai->ai_ail2_list;
+	struct list_head *head = &tr->tr_ail2_list;
 	struct gfs2_bufdata *bd;
 
 	while (!list_empty(head)) {
 		bd = list_entry(head->prev, struct gfs2_bufdata,
 				bd_ail_st_list);
-		gfs2_assert(sdp, bd->bd_ail == ai);
+		gfs2_assert(sdp, bd->bd_tr == tr);
 		gfs2_remove_from_ail(bd);
 	}
 }
 
 static void ail2_empty(struct gfs2_sbd *sdp, unsigned int new_tail)
 {
-	struct gfs2_ail *ai, *safe;
+	struct gfs2_trans *tr, *safe;
 	unsigned int old_tail = sdp->sd_log_tail;
 	int wrap = (new_tail < old_tail);
 	int a, b, rm;
 
 	spin_lock(&sdp->sd_ail_lock);
 
-	list_for_each_entry_safe(ai, safe, &sdp->sd_ail2_list, ai_list) {
-		a = (old_tail <= ai->ai_first);
-		b = (ai->ai_first < new_tail);
+	list_for_each_entry_safe(tr, safe, &sdp->sd_ail2_list, tr_list) {
+		a = (old_tail <= tr->tr_first);
+		b = (tr->tr_first < new_tail);
 		rm = (wrap) ? (a || b) : (a && b);
 		if (!rm)
 			continue;
 
-		gfs2_ail2_empty_one(sdp, ai);
-		list_del(&ai->ai_list);
-		gfs2_assert_warn(sdp, list_empty(&ai->ai_ail1_list));
-		gfs2_assert_warn(sdp, list_empty(&ai->ai_ail2_list));
-		kfree(ai);
+		gfs2_ail2_empty_one(sdp, tr);
+		list_del(&tr->tr_list);
+		gfs2_assert_warn(sdp, list_empty(&tr->tr_ail1_list));
+		gfs2_assert_warn(sdp, list_empty(&tr->tr_ail2_list));
+		kfree(tr);
 	}
 
 	spin_unlock(&sdp->sd_ail_lock);
@@ -435,7 +435,7 @@ static unsigned int calc_reserved(struct gfs2_sbd *sdp)
 
 static unsigned int current_tail(struct gfs2_sbd *sdp)
 {
-	struct gfs2_ail *ai;
+	struct gfs2_trans *tr;
 	unsigned int tail;
 
 	spin_lock(&sdp->sd_ail_lock);
@@ -443,8 +443,9 @@ static unsigned int current_tail(struct gfs2_sbd *sdp)
 	if (list_empty(&sdp->sd_ail1_list)) {
 		tail = sdp->sd_log_head;
 	} else {
-		ai = list_entry(sdp->sd_ail1_list.prev, struct gfs2_ail, ai_list);
-		tail = ai->ai_first;
+		tr = list_entry(sdp->sd_ail1_list.prev, struct gfs2_trans,
+				tr_list);
+		tail = tr->tr_first;
 	}
 
 	spin_unlock(&sdp->sd_ail_lock);
@@ -600,7 +601,7 @@ static void log_write_header(struct gfs2_sbd *sdp, u32 flags)
 
 void gfs2_log_flush(struct gfs2_sbd *sdp, struct gfs2_glock *gl)
 {
-	struct gfs2_ail *ai;
+	struct gfs2_trans *tr;
 
 	down_write(&sdp->sd_log_flush_lock);
 
@@ -611,9 +612,12 @@ void gfs2_log_flush(struct gfs2_sbd *sdp, struct gfs2_glock *gl)
 	}
 	trace_gfs2_log_flush(sdp, 1);
 
-	ai = kzalloc(sizeof(struct gfs2_ail), GFP_NOFS | __GFP_NOFAIL);
-	INIT_LIST_HEAD(&ai->ai_ail1_list);
-	INIT_LIST_HEAD(&ai->ai_ail2_list);
+	tr = sdp->sd_log_tr;
+	if (tr) {
+		sdp->sd_log_tr = NULL;
+		INIT_LIST_HEAD(&tr->tr_ail1_list);
+		INIT_LIST_HEAD(&tr->tr_ail2_list);
+	}
 
 	if (sdp->sd_log_num_buf != sdp->sd_log_commited_buf) {
 		printk(KERN_INFO "GFS2: log buf %u %u\n", sdp->sd_log_num_buf,
@@ -630,7 +634,8 @@ void gfs2_log_flush(struct gfs2_sbd *sdp, struct gfs2_glock *gl)
 
 	sdp->sd_log_flush_head = sdp->sd_log_head;
 	sdp->sd_log_flush_wrapped = 0;
-	ai->ai_first = sdp->sd_log_flush_head;
+	if (tr)
+		tr->tr_first = sdp->sd_log_flush_head;
 
 	gfs2_ordered_write(sdp);
 	lops_before_commit(sdp);
@@ -643,7 +648,7 @@ void gfs2_log_flush(struct gfs2_sbd *sdp, struct gfs2_glock *gl)
 		trace_gfs2_log_blocks(sdp, -1);
 		log_write_header(sdp, 0);
 	}
-	lops_after_commit(sdp, ai);
+	lops_after_commit(sdp, tr);
 
 	gfs2_log_lock(sdp);
 	sdp->sd_log_head = sdp->sd_log_flush_head;
@@ -653,16 +658,16 @@ void gfs2_log_flush(struct gfs2_sbd *sdp, struct gfs2_glock *gl)
 	sdp->sd_log_commited_revoke = 0;
 
 	spin_lock(&sdp->sd_ail_lock);
-	if (!list_empty(&ai->ai_ail1_list)) {
-		list_add(&ai->ai_list, &sdp->sd_ail1_list);
-		ai = NULL;
+	if (tr && !list_empty(&tr->tr_ail1_list)) {
+		list_add(&tr->tr_list, &sdp->sd_ail1_list);
+		tr = NULL;
 	}
 	spin_unlock(&sdp->sd_ail_lock);
 	gfs2_log_unlock(sdp);
 	trace_gfs2_log_flush(sdp, 0);
 	up_write(&sdp->sd_log_flush_lock);
 
-	kfree(ai);
+	kfree(tr);
 }
 
 static void log_refund(struct gfs2_sbd *sdp, struct gfs2_trans *tr)
@@ -687,6 +692,12 @@ static void log_refund(struct gfs2_sbd *sdp, struct gfs2_trans *tr)
 			     sdp->sd_jdesc->jd_blocks);
 	sdp->sd_log_blks_reserved = reserved;
 
+	if (sdp->sd_log_tr == NULL &&
+	    (tr->tr_num_buf_new || tr->tr_num_databuf_new)) {
+		gfs2_assert_withdraw(sdp, tr->tr_t_gh.gh_gl);
+		sdp->sd_log_tr = tr;
+		tr->tr_attached = 1;
+	}
 	gfs2_log_unlock(sdp);
 }
 
@@ -708,7 +719,6 @@ static void log_refund(struct gfs2_sbd *sdp, struct gfs2_trans *tr)
 void gfs2_log_commit(struct gfs2_sbd *sdp, struct gfs2_trans *tr)
 {
 	log_refund(sdp, tr);
-	up_read(&sdp->sd_log_flush_lock);
 
 	if (atomic_read(&sdp->sd_log_pinned) > atomic_read(&sdp->sd_log_thresh1) ||
 	    ((sdp->sd_jdesc->jd_blocks - atomic_read(&sdp->sd_log_blks_free)) >
diff --git a/fs/gfs2/lops.c b/fs/gfs2/lops.c
index a5055977a21..7318abf9d0f 100644
--- a/fs/gfs2/lops.c
+++ b/fs/gfs2/lops.c
@@ -53,8 +53,8 @@ void gfs2_pin(struct gfs2_sbd *sdp, struct buffer_head *bh)
 	 * to in-place disk block, remove it from the AIL.
 	 */
 	spin_lock(&sdp->sd_ail_lock);
-	if (bd->bd_ail)
-		list_move(&bd->bd_ail_st_list, &bd->bd_ail->ai_ail2_list);
+	if (bd->bd_tr)
+		list_move(&bd->bd_ail_st_list, &bd->bd_tr->tr_ail2_list);
 	spin_unlock(&sdp->sd_ail_lock);
 	get_bh(bh);
 	atomic_inc(&sdp->sd_log_pinned);
@@ -94,7 +94,7 @@ static void maybe_release_space(struct gfs2_bufdata *bd)
  */
 
 static void gfs2_unpin(struct gfs2_sbd *sdp, struct buffer_head *bh,
-		       struct gfs2_ail *ai)
+		       struct gfs2_trans *tr)
 {
 	struct gfs2_bufdata *bd = bh->b_private;
 
@@ -109,7 +109,7 @@ static void gfs2_unpin(struct gfs2_sbd *sdp, struct buffer_head *bh,
 		maybe_release_space(bd);
 
 	spin_lock(&sdp->sd_ail_lock);
-	if (bd->bd_ail) {
+	if (bd->bd_tr) {
 		list_del(&bd->bd_ail_st_list);
 		brelse(bh);
 	} else {
@@ -117,8 +117,8 @@ static void gfs2_unpin(struct gfs2_sbd *sdp, struct buffer_head *bh,
 		list_add(&bd->bd_ail_gl_list, &gl->gl_ail_list);
 		atomic_inc(&gl->gl_ail_count);
 	}
-	bd->bd_ail = ai;
-	list_add(&bd->bd_ail_st_list, &ai->ai_ail1_list);
+	bd->bd_tr = tr;
+	list_add(&bd->bd_ail_st_list, &tr->tr_ail1_list);
 	spin_unlock(&sdp->sd_ail_lock);
 
 	clear_bit(GLF_LFLUSH, &bd->bd_gl->gl_flags);
@@ -480,17 +480,22 @@ static void buf_lo_before_commit(struct gfs2_sbd *sdp)
 			   &sdp->sd_log_le_buf, 0);
 }
 
-static void buf_lo_after_commit(struct gfs2_sbd *sdp, struct gfs2_ail *ai)
+static void buf_lo_after_commit(struct gfs2_sbd *sdp, struct gfs2_trans *tr)
 {
 	struct list_head *head = &sdp->sd_log_le_buf;
 	struct gfs2_bufdata *bd;
 
+	if (tr == NULL) {
+		gfs2_assert(sdp, list_empty(head));
+		return;
+	}
+
 	while (!list_empty(head)) {
 		bd = list_entry(head->next, struct gfs2_bufdata, bd_list);
 		list_del_init(&bd->bd_list);
 		sdp->sd_log_num_buf--;
 
-		gfs2_unpin(sdp, bd->bd_bh, ai);
+		gfs2_unpin(sdp, bd->bd_bh, tr);
 	}
 	gfs2_assert_warn(sdp, !sdp->sd_log_num_buf);
 }
@@ -613,7 +618,7 @@ static void revoke_lo_before_commit(struct gfs2_sbd *sdp)
 	gfs2_log_write_page(sdp, page);
 }
 
-static void revoke_lo_after_commit(struct gfs2_sbd *sdp, struct gfs2_ail *ai)
+static void revoke_lo_after_commit(struct gfs2_sbd *sdp, struct gfs2_trans *tr)
 {
 	struct list_head *head = &sdp->sd_log_le_revoke;
 	struct gfs2_bufdata *bd;
@@ -791,16 +796,21 @@ static void databuf_lo_after_scan(struct gfs2_jdesc *jd, int error, int pass)
 		jd->jd_jid, sdp->sd_replayed_blocks, sdp->sd_found_blocks);
 }
 
-static void databuf_lo_after_commit(struct gfs2_sbd *sdp, struct gfs2_ail *ai)
+static void databuf_lo_after_commit(struct gfs2_sbd *sdp, struct gfs2_trans *tr)
 {
 	struct list_head *head = &sdp->sd_log_le_databuf;
 	struct gfs2_bufdata *bd;
 
+	if (tr == NULL) {
+		gfs2_assert(sdp, list_empty(head));
+		return;
+	}
+
 	while (!list_empty(head)) {
 		bd = list_entry(head->next, struct gfs2_bufdata, bd_list);
 		list_del_init(&bd->bd_list);
 		sdp->sd_log_num_databuf--;
-		gfs2_unpin(sdp, bd->bd_bh, ai);
+		gfs2_unpin(sdp, bd->bd_bh, tr);
 	}
 	gfs2_assert_warn(sdp, !sdp->sd_log_num_databuf);
 }
diff --git a/fs/gfs2/lops.h b/fs/gfs2/lops.h
index ba77b7da832..87e062e05c9 100644
--- a/fs/gfs2/lops.h
+++ b/fs/gfs2/lops.h
@@ -55,12 +55,13 @@ static inline void lops_before_commit(struct gfs2_sbd *sdp)
 			gfs2_log_ops[x]->lo_before_commit(sdp);
 }
 
-static inline void lops_after_commit(struct gfs2_sbd *sdp, struct gfs2_ail *ai)
+static inline void lops_after_commit(struct gfs2_sbd *sdp,
+				     struct gfs2_trans *tr)
 {
 	int x;
 	for (x = 0; gfs2_log_ops[x]; x++)
 		if (gfs2_log_ops[x]->lo_after_commit)
-			gfs2_log_ops[x]->lo_after_commit(sdp, ai);
+			gfs2_log_ops[x]->lo_after_commit(sdp, tr);
 }
 
 static inline void lops_before_scan(struct gfs2_jdesc *jd,
diff --git a/fs/gfs2/meta_io.c b/fs/gfs2/meta_io.c
index b059bbb5059..1a89afb6847 100644
--- a/fs/gfs2/meta_io.c
+++ b/fs/gfs2/meta_io.c
@@ -295,7 +295,7 @@ void gfs2_remove_from_journal(struct buffer_head *bh, struct gfs2_trans *tr, int
 	}
 	if (bd) {
 		spin_lock(&sdp->sd_ail_lock);
-		if (bd->bd_ail) {
+		if (bd->bd_tr) {
 			gfs2_remove_from_ail(bd);
 			bh->b_private = NULL;
 			bd->bd_bh = NULL;
diff --git a/fs/gfs2/rgrp.c b/fs/gfs2/rgrp.c
index d1f51fd73f8..0c5a575b513 100644
--- a/fs/gfs2/rgrp.c
+++ b/fs/gfs2/rgrp.c
@@ -576,7 +576,7 @@ int gfs2_rs_alloc(struct gfs2_inode *ip)
 	RB_CLEAR_NODE(&ip->i_res->rs_node);
 out:
 	up_write(&ip->i_rw_mutex);
-	return 0;
+	return error;
 }
 
 static void dump_rs(struct seq_file *seq, const struct gfs2_blkreserv *rs)
@@ -592,7 +592,7 @@ static void dump_rs(struct seq_file *seq, const struct gfs2_blkreserv *rs)
  * @rs: The reservation to remove
  *
  */
-static void __rs_deltree(struct gfs2_inode *ip, struct gfs2_blkreserv *rs)
+static void __rs_deltree(struct gfs2_blkreserv *rs)
 {
 	struct gfs2_rgrpd *rgd;
 
@@ -605,7 +605,7 @@ static void __rs_deltree(struct gfs2_inode *ip, struct gfs2_blkreserv *rs)
 	RB_CLEAR_NODE(&rs->rs_node);
 
 	if (rs->rs_free) {
-		/* return reserved blocks to the rgrp and the ip */
+		/* return reserved blocks to the rgrp */
 		BUG_ON(rs->rs_rbm.rgd->rd_reserved < rs->rs_free);
 		rs->rs_rbm.rgd->rd_reserved -= rs->rs_free;
 		rs->rs_free = 0;
@@ -619,14 +619,14 @@ static void __rs_deltree(struct gfs2_inode *ip, struct gfs2_blkreserv *rs)
  * @rs: The reservation to remove
  *
  */
-void gfs2_rs_deltree(struct gfs2_inode *ip, struct gfs2_blkreserv *rs)
+void gfs2_rs_deltree(struct gfs2_blkreserv *rs)
 {
 	struct gfs2_rgrpd *rgd;
 
 	rgd = rs->rs_rbm.rgd;
 	if (rgd) {
 		spin_lock(&rgd->rd_rsspin);
-		__rs_deltree(ip, rs);
+		__rs_deltree(rs);
 		spin_unlock(&rgd->rd_rsspin);
 	}
 }
@@ -640,7 +640,7 @@ void gfs2_rs_delete(struct gfs2_inode *ip)
 {
 	down_write(&ip->i_rw_mutex);
 	if (ip->i_res) {
-		gfs2_rs_deltree(ip, ip->i_res);
+		gfs2_rs_deltree(ip->i_res);
 		BUG_ON(ip->i_res->rs_free);
 		kmem_cache_free(gfs2_rsrv_cachep, ip->i_res);
 		ip->i_res = NULL;
@@ -664,7 +664,7 @@ static void return_all_reservations(struct gfs2_rgrpd *rgd)
 	spin_lock(&rgd->rd_rsspin);
 	while ((n = rb_first(&rgd->rd_rstree))) {
 		rs = rb_entry(n, struct gfs2_blkreserv, rs_node);
-		__rs_deltree(NULL, rs);
+		__rs_deltree(rs);
 	}
 	spin_unlock(&rgd->rd_rsspin);
 }
@@ -1181,12 +1181,9 @@ int gfs2_rgrp_send_discards(struct gfs2_sbd *sdp, u64 offset,
 			     const struct gfs2_bitmap *bi, unsigned minlen, u64 *ptrimmed)
 {
 	struct super_block *sb = sdp->sd_vfs;
-	struct block_device *bdev = sb->s_bdev;
-	const unsigned int sects_per_blk = sdp->sd_sb.sb_bsize /
-					   bdev_logical_block_size(sb->s_bdev);
 	u64 blk;
 	sector_t start = 0;
-	sector_t nr_sects = 0;
+	sector_t nr_blks = 0;
 	int rv;
 	unsigned int x;
 	u32 trimmed = 0;
@@ -1206,35 +1203,34 @@ int gfs2_rgrp_send_discards(struct gfs2_sbd *sdp, u64 offset,
 		if (diff == 0)
 			continue;
 		blk = offset + ((bi->bi_start + x) * GFS2_NBBY);
-		blk *= sects_per_blk; /* convert to sectors */
 		while(diff) {
 			if (diff & 1) {
-				if (nr_sects == 0)
+				if (nr_blks == 0)
 					goto start_new_extent;
-				if ((start + nr_sects) != blk) {
-					if (nr_sects >= minlen) {
-						rv = blkdev_issue_discard(bdev,
-							start, nr_sects,
+				if ((start + nr_blks) != blk) {
+					if (nr_blks >= minlen) {
+						rv = sb_issue_discard(sb,
+							start, nr_blks,
 							GFP_NOFS, 0);
 						if (rv)
 							goto fail;
-						trimmed += nr_sects;
+						trimmed += nr_blks;
 					}
-					nr_sects = 0;
+					nr_blks = 0;
 start_new_extent:
 					start = blk;
 				}
-				nr_sects += sects_per_blk;
+				nr_blks++;
 			}
 			diff >>= 2;
-			blk += sects_per_blk;
+			blk++;
 		}
 	}
-	if (nr_sects >= minlen) {
-		rv = blkdev_issue_discard(bdev, start, nr_sects, GFP_NOFS, 0);
+	if (nr_blks >= minlen) {
+		rv = sb_issue_discard(sb, start, nr_blks, GFP_NOFS, 0);
 		if (rv)
 			goto fail;
-		trimmed += nr_sects;
+		trimmed += nr_blks;
 	}
 	if (ptrimmed)
 		*ptrimmed = trimmed;
@@ -1878,7 +1874,7 @@ int gfs2_inplace_reserve(struct gfs2_inode *ip, u32 requested, u32 aflags)
 
 		/* Drop reservation, if we couldn't use reserved rgrp */
 		if (gfs2_rs_active(rs))
-			gfs2_rs_deltree(ip, rs);
+			gfs2_rs_deltree(rs);
 check_rgrp:
 		/* Check for unlinked inodes which can be reclaimed */
 		if (rs->rs_rbm.rgd->rd_flags & GFS2_RDF_CHECK)
@@ -2091,7 +2087,7 @@ static void gfs2_adjust_reservation(struct gfs2_inode *ip,
 			if (rs->rs_free && !ret)
 				goto out;
 		}
-		__rs_deltree(ip, rs);
+		__rs_deltree(rs);
 	}
 out:
 	spin_unlock(&rgd->rd_rsspin);
@@ -2184,13 +2180,7 @@ int gfs2_alloc_blocks(struct gfs2_inode *ip, u64 *bn, unsigned int *nblocks,
 	if (dinode)
 		gfs2_trans_add_unrevoke(sdp, block, 1);
 
-	/*
-	 * This needs reviewing to see why we cannot do the quota change
-	 * at this point in the dinode case.
-	 */
-	if (ndata)
-		gfs2_quota_change(ip, ndata, ip->i_inode.i_uid,
-				  ip->i_inode.i_gid);
+	gfs2_quota_change(ip, *nblocks, ip->i_inode.i_uid, ip->i_inode.i_gid);
 
 	rbm.rgd->rd_free_clone -= *nblocks;
 	trace_gfs2_block_alloc(ip, rbm.rgd, block, *nblocks,
diff --git a/fs/gfs2/rgrp.h b/fs/gfs2/rgrp.h
index 842185853f6..5b3f4a896e6 100644
--- a/fs/gfs2/rgrp.h
+++ b/fs/gfs2/rgrp.h
@@ -47,7 +47,7 @@ extern int gfs2_alloc_blocks(struct gfs2_inode *ip, u64 *bn, unsigned int *n,
 			     bool dinode, u64 *generation);
 
 extern int gfs2_rs_alloc(struct gfs2_inode *ip);
-extern void gfs2_rs_deltree(struct gfs2_inode *ip, struct gfs2_blkreserv *rs);
+extern void gfs2_rs_deltree(struct gfs2_blkreserv *rs);
 extern void gfs2_rs_delete(struct gfs2_inode *ip);
 extern void __gfs2_free_blocks(struct gfs2_inode *ip, u64 bstart, u32 blen, int meta);
 extern void gfs2_free_meta(struct gfs2_inode *ip, u64 bstart, u32 blen);
diff --git a/fs/gfs2/super.c b/fs/gfs2/super.c
index cab77b8ba84..917c8e1eb4a 100644
--- a/fs/gfs2/super.c
+++ b/fs/gfs2/super.c
@@ -1512,7 +1512,7 @@ out_truncate:
 out_unlock:
 	/* Error path for case 1 */
 	if (gfs2_rs_active(ip->i_res))
-		gfs2_rs_deltree(ip, ip->i_res);
+		gfs2_rs_deltree(ip->i_res);
 
 	if (test_bit(HIF_HOLDER, &ip->i_iopen_gh.gh_iflags))
 		gfs2_glock_dq(&ip->i_iopen_gh);
diff --git a/fs/gfs2/trace_gfs2.h b/fs/gfs2/trace_gfs2.h
index 2ee13e841e9..20c007d747a 100644
--- a/fs/gfs2/trace_gfs2.h
+++ b/fs/gfs2/trace_gfs2.h
@@ -159,9 +159,9 @@ TRACE_EVENT(gfs2_glock_put,
 /* Callback (local or remote) requesting lock demotion */
 TRACE_EVENT(gfs2_demote_rq,
 
-	TP_PROTO(const struct gfs2_glock *gl),
+	TP_PROTO(const struct gfs2_glock *gl, bool remote),
 
-	TP_ARGS(gl),
+	TP_ARGS(gl, remote),
 
 	TP_STRUCT__entry(
 		__field(        dev_t,  dev                     )
@@ -170,6 +170,7 @@ TRACE_EVENT(gfs2_demote_rq,
 		__field(	u8,	cur_state		)
 		__field(	u8,	dmt_state		)
 		__field(	unsigned long,	flags		)
+		__field(	bool,	remote			)
 	),
 
 	TP_fast_assign(
@@ -179,14 +180,16 @@ TRACE_EVENT(gfs2_demote_rq,
 		__entry->cur_state	= glock_trace_state(gl->gl_state);
 		__entry->dmt_state	= glock_trace_state(gl->gl_demote_state);
 		__entry->flags		= gl->gl_flags  | (gl->gl_object ? (1UL<<GLF_OBJECT) : 0);
+		__entry->remote		= remote;
 	),
 
-	TP_printk("%u,%u glock %d:%lld demote %s to %s flags:%s",
+	TP_printk("%u,%u glock %d:%lld demote %s to %s flags:%s %s",
 		  MAJOR(__entry->dev), MINOR(__entry->dev), __entry->gltype,
 		  (unsigned long long)__entry->glnum,
                   glock_trace_name(__entry->cur_state),
                   glock_trace_name(__entry->dmt_state),
-		  show_glock_flags(__entry->flags))
+		  show_glock_flags(__entry->flags),
+		  __entry->remote ? "remote" : "local")
 
 );
 
diff --git a/fs/gfs2/trans.c b/fs/gfs2/trans.c
index 88162fae27a..7374907742a 100644
--- a/fs/gfs2/trans.c
+++ b/fs/gfs2/trans.c
@@ -96,7 +96,8 @@ static void gfs2_log_release(struct gfs2_sbd *sdp, unsigned int blks)
 
 static void gfs2_print_trans(const struct gfs2_trans *tr)
 {
-	print_symbol(KERN_WARNING "GFS2: Transaction created at: %s\n", tr->tr_ip);
+	printk(KERN_WARNING "GFS2: Transaction created at: %pSR\n",
+	       (void *)tr->tr_ip);
 	printk(KERN_WARNING "GFS2: blocks=%u revokes=%u reserved=%u touched=%d\n",
 	       tr->tr_blocks, tr->tr_revokes, tr->tr_reserved, tr->tr_touched);
 	printk(KERN_WARNING "GFS2: Buf %u/%u Databuf %u/%u Revoke %u/%u\n",
@@ -135,8 +136,10 @@ void gfs2_trans_end(struct gfs2_sbd *sdp)
 	if (tr->tr_t_gh.gh_gl) {
 		gfs2_glock_dq(&tr->tr_t_gh);
 		gfs2_holder_uninit(&tr->tr_t_gh);
-		kfree(tr);
+		if (!tr->tr_attached)
+			kfree(tr);
 	}
+	up_read(&sdp->sd_log_flush_lock);
 
 	if (sdp->sd_vfs->s_flags & MS_SYNCHRONOUS)
 		gfs2_log_flush(sdp, NULL);
diff --git a/fs/hfsplus/extents.c b/fs/hfsplus/extents.c
index a94f0f779d5..fe0a76213d9 100644
--- a/fs/hfsplus/extents.c
+++ b/fs/hfsplus/extents.c
@@ -533,7 +533,7 @@ void hfsplus_file_truncate(struct inode *inode)
 		struct address_space *mapping = inode->i_mapping;
 		struct page *page;
 		void *fsdata;
-		u32 size = inode->i_size;
+		loff_t size = inode->i_size;
 
 		res = pagecache_write_begin(NULL, mapping, size, 0,
 						AOP_FLAG_UNINTERRUPTIBLE,
diff --git a/fs/hugetlbfs/inode.c b/fs/hugetlbfs/inode.c
index 84e3d856e91..523464e6284 100644
--- a/fs/hugetlbfs/inode.c
+++ b/fs/hugetlbfs/inode.c
@@ -110,7 +110,7 @@ static int hugetlbfs_file_mmap(struct file *file, struct vm_area_struct *vma)
 	 * way when do_mmap_pgoff unwinds (may be important on powerpc
 	 * and ia64).
 	 */
-	vma->vm_flags |= VM_HUGETLB | VM_DONTEXPAND | VM_DONTDUMP;
+	vma->vm_flags |= VM_HUGETLB | VM_DONTEXPAND;
 	vma->vm_ops = &hugetlb_vm_ops;
 
 	if (vma->vm_pgoff & (~huge_page_mask(h) >> PAGE_SHIFT))
diff --git a/fs/inode.c b/fs/inode.c
index f5f7c06c36f..a898b3d43cc 100644
--- a/fs/inode.c
+++ b/fs/inode.c
@@ -725,7 +725,7 @@ void prune_icache_sb(struct super_block *sb, int nr_to_scan)
 		 * inode to the back of the list so we don't spin on it.
 		 */
 		if (!spin_trylock(&inode->i_lock)) {
-			list_move_tail(&inode->i_lru, &sb->s_inode_lru);
+			list_move(&inode->i_lru, &sb->s_inode_lru);
 			continue;
 		}
 
diff --git a/fs/internal.h b/fs/internal.h
index 507141fceb9..4be78237d89 100644
--- a/fs/internal.h
+++ b/fs/internal.h
@@ -125,3 +125,8 @@ extern int invalidate_inodes(struct super_block *, bool);
  * dcache.c
  */
 extern struct dentry *__d_alloc(struct super_block *, const struct qstr *);
+
+/*
+ * read_write.c
+ */
+extern ssize_t __kernel_write(struct file *, const char *, size_t, loff_t *);
diff --git a/fs/jbd/commit.c b/fs/jbd/commit.c
index 86b39b167c2..11bb11f48b3 100644
--- a/fs/jbd/commit.c
+++ b/fs/jbd/commit.c
@@ -162,8 +162,17 @@ static void journal_do_submit_data(struct buffer_head **wbuf, int bufs,
 
 	for (i = 0; i < bufs; i++) {
 		wbuf[i]->b_end_io = end_buffer_write_sync;
-		/* We use-up our safety reference in submit_bh() */
-		submit_bh(write_op, wbuf[i]);
+		/*
+		 * Here we write back pagecache data that may be mmaped. Since
+		 * we cannot afford to clean the page and set PageWriteback
+		 * here due to lock ordering (page lock ranks above transaction
+		 * start), the data can change while IO is in flight. Tell the
+		 * block layer it should bounce the bio pages if stable data
+		 * during write is required.
+		 *
+		 * We use up our safety reference in submit_bh().
+		 */
+		_submit_bh(write_op, wbuf[i], 1 << BIO_SNAP_STABLE);
 	}
 }
 
@@ -667,7 +676,17 @@ start_journal_io:
 				clear_buffer_dirty(bh);
 				set_buffer_uptodate(bh);
 				bh->b_end_io = journal_end_buffer_io_sync;
-				submit_bh(write_op, bh);
+				/*
+				 * In data=journal mode, here we can end up
+				 * writing pagecache data that might be
+				 * mmapped. Since we can't afford to clean the
+				 * page and set PageWriteback (see the comment
+				 * near the other use of _submit_bh()), the
+				 * data can change while the write is in
+				 * flight.  Tell the block layer to bounce the
+				 * bio pages if stable pages are required.
+				 */
+				_submit_bh(write_op, bh, 1 << BIO_SNAP_STABLE);
 			}
 			cond_resched();
 
diff --git a/fs/jbd/journal.c b/fs/jbd/journal.c
index 81cc7eaff86..865c4308acb 100644
--- a/fs/jbd/journal.c
+++ b/fs/jbd/journal.c
@@ -310,8 +310,6 @@ int journal_write_metadata_buffer(transaction_t *transaction,
 
 	new_bh = alloc_buffer_head(GFP_NOFS|__GFP_NOFAIL);
 	/* keep subsequent assertions sane */
-	new_bh->b_state = 0;
-	init_buffer(new_bh, NULL, NULL);
 	atomic_set(&new_bh->b_count, 1);
 	new_jh = journal_add_journal_head(new_bh);	/* This sleeps */
 
diff --git a/fs/jbd2/journal.c b/fs/jbd2/journal.c
index ed10991ab00..8b220f1ab54 100644
--- a/fs/jbd2/journal.c
+++ b/fs/jbd2/journal.c
@@ -367,8 +367,6 @@ retry_alloc:
 	}
 
 	/* keep subsequent assertions sane */
-	new_bh->b_state = 0;
-	init_buffer(new_bh, NULL, NULL);
 	atomic_set(&new_bh->b_count, 1);
 	new_jh = jbd2_journal_add_journal_head(new_bh);	/* This sleeps */
 
diff --git a/fs/lockd/clntlock.c b/fs/lockd/clntlock.c
index 0796c45d0d4..01bfe766275 100644
--- a/fs/lockd/clntlock.c
+++ b/fs/lockd/clntlock.c
@@ -144,6 +144,9 @@ int nlmclnt_block(struct nlm_wait *block, struct nlm_rqst *req, long timeout)
 			timeout);
 	if (ret < 0)
 		return -ERESTARTSYS;
+	/* Reset the lock status after a server reboot so we resend */
+	if (block->b_status == nlm_lck_denied_grace_period)
+		block->b_status = nlm_lck_blocked;
 	req->a_res.status = block->b_status;
 	return 0;
 }
diff --git a/fs/lockd/clntproc.c b/fs/lockd/clntproc.c
index 7e529c3c45c..9760ecb9b60 100644
--- a/fs/lockd/clntproc.c
+++ b/fs/lockd/clntproc.c
@@ -550,9 +550,6 @@ again:
 		status = nlmclnt_block(block, req, NLMCLNT_POLL_TIMEOUT);
 		if (status < 0)
 			break;
-		/* Resend the blocking lock request after a server reboot */
-		if (resp->status ==  nlm_lck_denied_grace_period)
-			continue;
 		if (resp->status != nlm_lck_blocked)
 			break;
 	}
diff --git a/fs/namespace.c b/fs/namespace.c
index 50ca17d3cb4..341d3f56408 100644
--- a/fs/namespace.c
+++ b/fs/namespace.c
@@ -798,6 +798,10 @@ static struct mount *clone_mnt(struct mount *old, struct dentry *root,
 	}
 
 	mnt->mnt.mnt_flags = old->mnt.mnt_flags & ~MNT_WRITE_HOLD;
+	/* Don't allow unprivileged users to change mount flags */
+	if ((flag & CL_UNPRIVILEGED) && (mnt->mnt.mnt_flags & MNT_READONLY))
+		mnt->mnt.mnt_flags |= MNT_LOCK_READONLY;
+
 	atomic_inc(&sb->s_active);
 	mnt->mnt.mnt_sb = sb;
 	mnt->mnt.mnt_root = dget(root);
@@ -1686,7 +1690,7 @@ static int do_loopback(struct path *path, const char *old_name,
 
 	if (IS_ERR(mnt)) {
 		err = PTR_ERR(mnt);
-		goto out;
+		goto out2;
 	}
 
 	err = graft_tree(mnt, path);
@@ -1713,6 +1717,9 @@ static int change_mount_flags(struct vfsmount *mnt, int ms_flags)
 	if (readonly_request == __mnt_is_readonly(mnt))
 		return 0;
 
+	if (mnt->mnt_flags & MNT_LOCK_READONLY)
+		return -EPERM;
+
 	if (readonly_request)
 		error = mnt_make_readonly(real_mount(mnt));
 	else
@@ -2339,7 +2346,7 @@ static struct mnt_namespace *dup_mnt_ns(struct mnt_namespace *mnt_ns,
 	/* First pass: copy the tree topology */
 	copy_flags = CL_COPY_ALL | CL_EXPIRE;
 	if (user_ns != mnt_ns->user_ns)
-		copy_flags |= CL_SHARED_TO_SLAVE;
+		copy_flags |= CL_SHARED_TO_SLAVE | CL_UNPRIVILEGED;
 	new = copy_tree(old, old->mnt.mnt_root, copy_flags);
 	if (IS_ERR(new)) {
 		up_write(&namespace_sem);
@@ -2732,6 +2739,51 @@ bool our_mnt(struct vfsmount *mnt)
 	return check_mnt(real_mount(mnt));
 }
 
+bool current_chrooted(void)
+{
+	/* Does the current process have a non-standard root */
+	struct path ns_root;
+	struct path fs_root;
+	bool chrooted;
+
+	/* Find the namespace root */
+	ns_root.mnt = &current->nsproxy->mnt_ns->root->mnt;
+	ns_root.dentry = ns_root.mnt->mnt_root;
+	path_get(&ns_root);
+	while (d_mountpoint(ns_root.dentry) && follow_down_one(&ns_root))
+		;
+
+	get_fs_root(current->fs, &fs_root);
+
+	chrooted = !path_equal(&fs_root, &ns_root);
+
+	path_put(&fs_root);
+	path_put(&ns_root);
+
+	return chrooted;
+}
+
+void update_mnt_policy(struct user_namespace *userns)
+{
+	struct mnt_namespace *ns = current->nsproxy->mnt_ns;
+	struct mount *mnt;
+
+	down_read(&namespace_sem);
+	list_for_each_entry(mnt, &ns->list, mnt_list) {
+		switch (mnt->mnt.mnt_sb->s_magic) {
+		case SYSFS_MAGIC:
+			userns->may_mount_sysfs = true;
+			break;
+		case PROC_SUPER_MAGIC:
+			userns->may_mount_proc = true;
+			break;
+		}
+		if (userns->may_mount_sysfs && userns->may_mount_proc)
+			break;
+	}
+	up_read(&namespace_sem);
+}
+
 static void *mntns_get(struct task_struct *task)
 {
 	struct mnt_namespace *ns = NULL;
diff --git a/fs/nfs/blocklayout/blocklayoutdm.c b/fs/nfs/blocklayout/blocklayoutdm.c
index 737d839bc17..6fc7b5cae92 100644
--- a/fs/nfs/blocklayout/blocklayoutdm.c
+++ b/fs/nfs/blocklayout/blocklayoutdm.c
@@ -55,7 +55,8 @@ static void dev_remove(struct net *net, dev_t dev)
 
 	bl_pipe_msg.bl_wq = &nn->bl_wq;
 	memset(msg, 0, sizeof(*msg));
-	msg->data = kzalloc(1 + sizeof(bl_umount_request), GFP_NOFS);
+	msg->len = sizeof(bl_msg) + bl_msg.totallen;
+	msg->data = kzalloc(msg->len, GFP_NOFS);
 	if (!msg->data)
 		goto out;
 
@@ -66,7 +67,6 @@ static void dev_remove(struct net *net, dev_t dev)
 	memcpy(msg->data, &bl_msg, sizeof(bl_msg));
 	dataptr = (uint8_t *) msg->data;
 	memcpy(&dataptr[sizeof(bl_msg)], &bl_umount_request, sizeof(bl_umount_request));
-	msg->len = sizeof(bl_msg) + bl_msg.totallen;
 
 	add_wait_queue(&nn->bl_wq, &wq);
 	if (rpc_queue_upcall(nn->bl_device_pipe, msg) < 0) {
diff --git a/fs/nfs/callback.c b/fs/nfs/callback.c
index 5088b57b078..cff089a412c 100644
--- a/fs/nfs/callback.c
+++ b/fs/nfs/callback.c
@@ -125,6 +125,9 @@ nfs41_callback_svc(void *vrqstp)
 	set_freezable();
 
 	while (!kthread_should_stop()) {
+		if (try_to_freeze())
+			continue;
+
 		prepare_to_wait(&serv->sv_cb_waitq, &wq, TASK_INTERRUPTIBLE);
 		spin_lock_bh(&serv->sv_cb_lock);
 		if (!list_empty(&serv->sv_cb_list)) {
diff --git a/fs/nfs/callback_proc.c b/fs/nfs/callback_proc.c
index 2960512792c..a13d26ede25 100644
--- a/fs/nfs/callback_proc.c
+++ b/fs/nfs/callback_proc.c
@@ -500,7 +500,7 @@ __be32 nfs4_callback_recallany(struct cb_recallanyargs *args, void *dummy,
 		     &args->craa_type_mask))
 		pnfs_recall_all_layouts(cps->clp);
 	if (flags)
-		nfs_expire_all_delegation_types(cps->clp, flags);
+		nfs_expire_unused_delegation_types(cps->clp, flags);
 out:
 	dprintk("%s: exit with status = %d\n", __func__, ntohl(status));
 	return status;
diff --git a/fs/nfs/client.c b/fs/nfs/client.c
index 84d8eae203a..c513b0cc835 100644
--- a/fs/nfs/client.c
+++ b/fs/nfs/client.c
@@ -593,6 +593,8 @@ int nfs_create_rpc_client(struct nfs_client *clp,
 		args.flags |= RPC_CLNT_CREATE_DISCRTRY;
 	if (test_bit(NFS_CS_NORESVPORT, &clp->cl_flags))
 		args.flags |= RPC_CLNT_CREATE_NONPRIVPORT;
+	if (test_bit(NFS_CS_INFINITE_SLOTS, &clp->cl_flags))
+		args.flags |= RPC_CLNT_CREATE_INFINITE_SLOTS;
 
 	if (!IS_ERR(clp->cl_rpcclient))
 		return 0;
diff --git a/fs/nfs/delegation.c b/fs/nfs/delegation.c
index 6390a4b5fee..57db3244f4d 100644
--- a/fs/nfs/delegation.c
+++ b/fs/nfs/delegation.c
@@ -64,17 +64,15 @@ int nfs4_have_delegation(struct inode *inode, fmode_t flags)
 	return ret;
 }
 
-static int nfs_delegation_claim_locks(struct nfs_open_context *ctx, struct nfs4_state *state)
+static int nfs_delegation_claim_locks(struct nfs_open_context *ctx, struct nfs4_state *state, const nfs4_stateid *stateid)
 {
 	struct inode *inode = state->inode;
 	struct file_lock *fl;
 	int status = 0;
 
 	if (inode->i_flock == NULL)
-		return 0;
-
-	if (inode->i_flock == NULL)
 		goto out;
+
 	/* Protect inode->i_flock using the file locks lock */
 	lock_flocks();
 	for (fl = inode->i_flock; fl != NULL; fl = fl->fl_next) {
@@ -83,7 +81,7 @@ static int nfs_delegation_claim_locks(struct nfs_open_context *ctx, struct nfs4_
 		if (nfs_file_open_context(fl->fl_file) != ctx)
 			continue;
 		unlock_flocks();
-		status = nfs4_lock_delegation_recall(state, fl);
+		status = nfs4_lock_delegation_recall(fl, state, stateid);
 		if (status < 0)
 			goto out;
 		lock_flocks();
@@ -120,7 +118,7 @@ again:
 		seq = raw_seqcount_begin(&sp->so_reclaim_seqcount);
 		err = nfs4_open_delegation_recall(ctx, state, stateid);
 		if (!err)
-			err = nfs_delegation_claim_locks(ctx, state);
+			err = nfs_delegation_claim_locks(ctx, state, stateid);
 		if (!err && read_seqcount_retry(&sp->so_reclaim_seqcount, seq))
 			err = -EAGAIN;
 		mutex_unlock(&sp->so_delegreturn_mutex);
@@ -389,6 +387,24 @@ out:
 	return err;
 }
 
+static bool nfs_delegation_need_return(struct nfs_delegation *delegation)
+{
+	bool ret = false;
+
+	if (test_and_clear_bit(NFS_DELEGATION_RETURN, &delegation->flags))
+		ret = true;
+	if (test_and_clear_bit(NFS_DELEGATION_RETURN_IF_CLOSED, &delegation->flags) && !ret) {
+		struct inode *inode;
+
+		spin_lock(&delegation->lock);
+		inode = delegation->inode;
+		if (inode && list_empty(&NFS_I(inode)->open_files))
+			ret = true;
+		spin_unlock(&delegation->lock);
+	}
+	return ret;
+}
+
 /**
  * nfs_client_return_marked_delegations - return previously marked delegations
  * @clp: nfs_client to process
@@ -411,8 +427,7 @@ restart:
 	list_for_each_entry_rcu(server, &clp->cl_superblocks, client_link) {
 		list_for_each_entry_rcu(delegation, &server->delegations,
 								super_list) {
-			if (!test_and_clear_bit(NFS_DELEGATION_RETURN,
-							&delegation->flags))
+			if (!nfs_delegation_need_return(delegation))
 				continue;
 			inode = nfs_delegation_grab_inode(delegation);
 			if (inode == NULL)
@@ -471,6 +486,13 @@ int nfs4_inode_return_delegation(struct inode *inode)
 	return err;
 }
 
+static void nfs_mark_return_if_closed_delegation(struct nfs_server *server,
+		struct nfs_delegation *delegation)
+{
+	set_bit(NFS_DELEGATION_RETURN_IF_CLOSED, &delegation->flags);
+	set_bit(NFS4CLNT_DELEGRETURN, &server->nfs_client->cl_state);
+}
+
 static void nfs_mark_return_delegation(struct nfs_server *server,
 		struct nfs_delegation *delegation)
 {
@@ -478,6 +500,45 @@ static void nfs_mark_return_delegation(struct nfs_server *server,
 	set_bit(NFS4CLNT_DELEGRETURN, &server->nfs_client->cl_state);
 }
 
+static bool nfs_server_mark_return_all_delegations(struct nfs_server *server)
+{
+	struct nfs_delegation *delegation;
+	bool ret = false;
+
+	list_for_each_entry_rcu(delegation, &server->delegations, super_list) {
+		nfs_mark_return_delegation(server, delegation);
+		ret = true;
+	}
+	return ret;
+}
+
+static void nfs_client_mark_return_all_delegations(struct nfs_client *clp)
+{
+	struct nfs_server *server;
+
+	rcu_read_lock();
+	list_for_each_entry_rcu(server, &clp->cl_superblocks, client_link)
+		nfs_server_mark_return_all_delegations(server);
+	rcu_read_unlock();
+}
+
+static void nfs_delegation_run_state_manager(struct nfs_client *clp)
+{
+	if (test_bit(NFS4CLNT_DELEGRETURN, &clp->cl_state))
+		nfs4_schedule_state_manager(clp);
+}
+
+/**
+ * nfs_expire_all_delegations
+ * @clp: client to process
+ *
+ */
+void nfs_expire_all_delegations(struct nfs_client *clp)
+{
+	nfs_client_mark_return_all_delegations(clp);
+	nfs_delegation_run_state_manager(clp);
+}
+
 /**
  * nfs_super_return_all_delegations - return delegations for one superblock
  * @sb: sb to process
@@ -486,24 +547,22 @@ static void nfs_mark_return_delegation(struct nfs_server *server,
 void nfs_server_return_all_delegations(struct nfs_server *server)
 {
 	struct nfs_client *clp = server->nfs_client;
-	struct nfs_delegation *delegation;
+	bool need_wait;
 
 	if (clp == NULL)
 		return;
 
 	rcu_read_lock();
-	list_for_each_entry_rcu(delegation, &server->delegations, super_list) {
-		spin_lock(&delegation->lock);
-		set_bit(NFS_DELEGATION_RETURN, &delegation->flags);
-		spin_unlock(&delegation->lock);
-	}
+	need_wait = nfs_server_mark_return_all_delegations(server);
 	rcu_read_unlock();
 
-	if (nfs_client_return_marked_delegations(clp) != 0)
+	if (need_wait) {
 		nfs4_schedule_state_manager(clp);
+		nfs4_wait_clnt_recover(clp);
+	}
 }
 
-static void nfs_mark_return_all_delegation_types(struct nfs_server *server,
+static void nfs_mark_return_unused_delegation_types(struct nfs_server *server,
 						 fmode_t flags)
 {
 	struct nfs_delegation *delegation;
@@ -512,27 +571,21 @@ static void nfs_mark_return_all_delegation_types(struct nfs_server *server,
 		if ((delegation->type == (FMODE_READ|FMODE_WRITE)) && !(flags & FMODE_WRITE))
 			continue;
 		if (delegation->type & flags)
-			nfs_mark_return_delegation(server, delegation);
+			nfs_mark_return_if_closed_delegation(server, delegation);
 	}
 }
 
-static void nfs_client_mark_return_all_delegation_types(struct nfs_client *clp,
+static void nfs_client_mark_return_unused_delegation_types(struct nfs_client *clp,
 							fmode_t flags)
 {
 	struct nfs_server *server;
 
 	rcu_read_lock();
 	list_for_each_entry_rcu(server, &clp->cl_superblocks, client_link)
-		nfs_mark_return_all_delegation_types(server, flags);
+		nfs_mark_return_unused_delegation_types(server, flags);
 	rcu_read_unlock();
 }
 
-static void nfs_delegation_run_state_manager(struct nfs_client *clp)
-{
-	if (test_bit(NFS4CLNT_DELEGRETURN, &clp->cl_state))
-		nfs4_schedule_state_manager(clp);
-}
-
 void nfs_remove_bad_delegation(struct inode *inode)
 {
 	struct nfs_delegation *delegation;
@@ -546,27 +599,17 @@ void nfs_remove_bad_delegation(struct inode *inode)
 EXPORT_SYMBOL_GPL(nfs_remove_bad_delegation);
 
 /**
- * nfs_expire_all_delegation_types
+ * nfs_expire_unused_delegation_types
  * @clp: client to process
  * @flags: delegation types to expire
  *
  */
-void nfs_expire_all_delegation_types(struct nfs_client *clp, fmode_t flags)
+void nfs_expire_unused_delegation_types(struct nfs_client *clp, fmode_t flags)
 {
-	nfs_client_mark_return_all_delegation_types(clp, flags);
+	nfs_client_mark_return_unused_delegation_types(clp, flags);
 	nfs_delegation_run_state_manager(clp);
 }
 
-/**
- * nfs_expire_all_delegations
- * @clp: client to process
- *
- */
-void nfs_expire_all_delegations(struct nfs_client *clp)
-{
-	nfs_expire_all_delegation_types(clp, FMODE_READ|FMODE_WRITE);
-}
-
 static void nfs_mark_return_unreferenced_delegations(struct nfs_server *server)
 {
 	struct nfs_delegation *delegation;
@@ -574,7 +617,7 @@ static void nfs_mark_return_unreferenced_delegations(struct nfs_server *server)
 	list_for_each_entry_rcu(delegation, &server->delegations, super_list) {
 		if (test_and_clear_bit(NFS_DELEGATION_REFERENCED, &delegation->flags))
 			continue;
-		nfs_mark_return_delegation(server, delegation);
+		nfs_mark_return_if_closed_delegation(server, delegation);
 	}
 }
 
diff --git a/fs/nfs/delegation.h b/fs/nfs/delegation.h
index d54d4fca679..9a79c7a99d6 100644
--- a/fs/nfs/delegation.h
+++ b/fs/nfs/delegation.h
@@ -28,6 +28,7 @@ struct nfs_delegation {
 enum {
 	NFS_DELEGATION_NEED_RECLAIM = 0,
 	NFS_DELEGATION_RETURN,
+	NFS_DELEGATION_RETURN_IF_CLOSED,
 	NFS_DELEGATION_REFERENCED,
 	NFS_DELEGATION_RETURNING,
 };
@@ -41,7 +42,7 @@ void nfs_inode_return_delegation_noreclaim(struct inode *inode);
 struct inode *nfs_delegation_find_inode(struct nfs_client *clp, const struct nfs_fh *fhandle);
 void nfs_server_return_all_delegations(struct nfs_server *);
 void nfs_expire_all_delegations(struct nfs_client *clp);
-void nfs_expire_all_delegation_types(struct nfs_client *clp, fmode_t flags);
+void nfs_expire_unused_delegation_types(struct nfs_client *clp, fmode_t flags);
 void nfs_expire_unreferenced_delegations(struct nfs_client *clp);
 int nfs_client_return_marked_delegations(struct nfs_client *clp);
 int nfs_delegations_present(struct nfs_client *clp);
@@ -53,7 +54,7 @@ void nfs_delegation_reap_unclaimed(struct nfs_client *clp);
 /* NFSv4 delegation-related procedures */
 int nfs4_proc_delegreturn(struct inode *inode, struct rpc_cred *cred, const nfs4_stateid *stateid, int issync);
 int nfs4_open_delegation_recall(struct nfs_open_context *ctx, struct nfs4_state *state, const nfs4_stateid *stateid);
-int nfs4_lock_delegation_recall(struct nfs4_state *state, struct file_lock *fl);
+int nfs4_lock_delegation_recall(struct file_lock *fl, struct nfs4_state *state, const nfs4_stateid *stateid);
 bool nfs4_copy_delegation_stateid(nfs4_stateid *dst, struct inode *inode, fmode_t flags);
 
 void nfs_mark_delegation_referenced(struct nfs_delegation *delegation);
diff --git a/fs/nfs/dir.c b/fs/nfs/dir.c
index f23f455be42..e093e73178b 100644
--- a/fs/nfs/dir.c
+++ b/fs/nfs/dir.c
@@ -1486,6 +1486,8 @@ static int nfs4_lookup_revalidate(struct dentry *dentry, unsigned int flags)
 		goto no_open;
 	if (d_mountpoint(dentry))
 		goto no_open;
+	if (NFS_SB(dentry->d_sb)->caps & NFS_CAP_ATOMIC_OPEN_V1)
+		goto no_open;
 
 	inode = dentry->d_inode;
 	parent = dget_parent(dentry);
diff --git a/fs/nfs/file.c b/fs/nfs/file.c
index 29f4a48a0ee..a87a44f8411 100644
--- a/fs/nfs/file.c
+++ b/fs/nfs/file.c
@@ -744,6 +744,7 @@ static int
 do_unlk(struct file *filp, int cmd, struct file_lock *fl, int is_local)
 {
 	struct inode *inode = filp->f_mapping->host;
+	struct nfs_lock_context *l_ctx;
 	int status;
 
 	/*
@@ -752,6 +753,14 @@ do_unlk(struct file *filp, int cmd, struct file_lock *fl, int is_local)
 	 */
 	nfs_sync_mapping(filp->f_mapping);
 
+	l_ctx = nfs_get_lock_context(nfs_file_open_context(filp));
+	if (!IS_ERR(l_ctx)) {
+		status = nfs_iocounter_wait(&l_ctx->io_count);
+		nfs_put_lock_context(l_ctx);
+		if (status < 0)
+			return status;
+	}
+
 	/* NOTE: special case
 	 * 	If we're signalled while cleaning up locks on process exit, we
 	 * 	still need to complete the unlock.
diff --git a/fs/nfs/idmap.c b/fs/nfs/idmap.c
index dc0f98dfa71..c516da5873f 100644
--- a/fs/nfs/idmap.c
+++ b/fs/nfs/idmap.c
@@ -726,9 +726,9 @@ out1:
 	return ret;
 }
 
-static int nfs_idmap_instantiate(struct key *key, struct key *authkey, char *data)
+static int nfs_idmap_instantiate(struct key *key, struct key *authkey, char *data, size_t datalen)
 {
-	return key_instantiate_and_link(key, data, strlen(data) + 1,
+	return key_instantiate_and_link(key, data, datalen,
 					id_resolver_cache->thread_keyring,
 					authkey);
 }
@@ -738,6 +738,7 @@ static int nfs_idmap_read_and_verify_message(struct idmap_msg *im,
 		struct key *key, struct key *authkey)
 {
 	char id_str[NFS_UINT_MAXLEN];
+	size_t len;
 	int ret = -ENOKEY;
 
 	/* ret = -ENOKEY */
@@ -747,13 +748,15 @@ static int nfs_idmap_read_and_verify_message(struct idmap_msg *im,
 	case IDMAP_CONV_NAMETOID:
 		if (strcmp(upcall->im_name, im->im_name) != 0)
 			break;
-		sprintf(id_str, "%d", im->im_id);
-		ret = nfs_idmap_instantiate(key, authkey, id_str);
+		/* Note: here we store the NUL terminator too */
+		len = sprintf(id_str, "%d", im->im_id) + 1;
+		ret = nfs_idmap_instantiate(key, authkey, id_str, len);
 		break;
 	case IDMAP_CONV_IDTONAME:
 		if (upcall->im_id != im->im_id)
 			break;
-		ret = nfs_idmap_instantiate(key, authkey, im->im_name);
+		len = strlen(im->im_name);
+		ret = nfs_idmap_instantiate(key, authkey, im->im_name, len);
 		break;
 	default:
 		ret = -EINVAL;
diff --git a/fs/nfs/inode.c b/fs/nfs/inode.c
index 1f941674b08..c1c7a9d7872 100644
--- a/fs/nfs/inode.c
+++ b/fs/nfs/inode.c
@@ -561,20 +561,22 @@ static void nfs_init_lock_context(struct nfs_lock_context *l_ctx)
 	l_ctx->lockowner.l_owner = current->files;
 	l_ctx->lockowner.l_pid = current->tgid;
 	INIT_LIST_HEAD(&l_ctx->list);
+	nfs_iocounter_init(&l_ctx->io_count);
 }
 
 static struct nfs_lock_context *__nfs_find_lock_context(struct nfs_open_context *ctx)
 {
-	struct nfs_lock_context *pos;
+	struct nfs_lock_context *head = &ctx->lock_context;
+	struct nfs_lock_context *pos = head;
 
-	list_for_each_entry(pos, &ctx->lock_context.list, list) {
+	do {
 		if (pos->lockowner.l_owner != current->files)
 			continue;
 		if (pos->lockowner.l_pid != current->tgid)
 			continue;
 		atomic_inc(&pos->count);
 		return pos;
-	}
+	} while ((pos = list_entry(pos->list.next, typeof(*pos), list)) != head);
 	return NULL;
 }
 
diff --git a/fs/nfs/internal.h b/fs/nfs/internal.h
index 541c9ebdbc5..91e59a39fc0 100644
--- a/fs/nfs/internal.h
+++ b/fs/nfs/internal.h
@@ -229,6 +229,13 @@ extern void nfs_pgheader_init(struct nfs_pageio_descriptor *desc,
 			      struct nfs_pgio_header *hdr,
 			      void (*release)(struct nfs_pgio_header *hdr));
 void nfs_set_pgio_error(struct nfs_pgio_header *hdr, int error, loff_t pos);
+int nfs_iocounter_wait(struct nfs_io_counter *c);
+
+static inline void nfs_iocounter_init(struct nfs_io_counter *c)
+{
+	c->flags = 0;
+	atomic_set(&c->io_count, 0);
+}
 
 /* nfs2xdr.c */
 extern struct rpc_procinfo nfs_procedures[];
diff --git a/fs/nfs/nfs4_fs.h b/fs/nfs/nfs4_fs.h
index 944c9a5c103..553a83cc410 100644
--- a/fs/nfs/nfs4_fs.h
+++ b/fs/nfs/nfs4_fs.h
@@ -36,6 +36,7 @@ enum nfs4_client_state {
 
 struct nfs4_minor_version_ops {
 	u32	minor_version;
+	unsigned init_caps;
 
 	int	(*call_sync)(struct rpc_clnt *clnt,
 			struct nfs_server *server,
@@ -143,12 +144,14 @@ struct nfs4_lock_state {
 enum {
 	LK_STATE_IN_USE,
 	NFS_DELEGATED_STATE,		/* Current stateid is delegation */
+	NFS_OPEN_STATE,			/* OPEN stateid is set */
 	NFS_O_RDONLY_STATE,		/* OPEN stateid has read-only state */
 	NFS_O_WRONLY_STATE,		/* OPEN stateid has write-only state */
 	NFS_O_RDWR_STATE,		/* OPEN stateid has read/write state */
 	NFS_STATE_RECLAIM_REBOOT,	/* OPEN stateid server rebooted */
 	NFS_STATE_RECLAIM_NOGRACE,	/* OPEN stateid needs to recover state */
 	NFS_STATE_POSIX_LOCKS,		/* Posix locks are supported */
+	NFS_STATE_RECOVERY_FAILED,	/* OPEN stateid state recovery failed */
 };
 
 struct nfs4_state {
@@ -233,6 +236,10 @@ extern struct rpc_clnt *nfs4_proc_lookup_mountpoint(struct inode *, struct qstr
 extern int nfs4_proc_secinfo(struct inode *, const struct qstr *, struct nfs4_secinfo_flavors *);
 extern int nfs4_release_lockowner(struct nfs4_lock_state *);
 extern const struct xattr_handler *nfs4_xattr_handlers[];
+extern int nfs4_set_rw_stateid(nfs4_stateid *stateid,
+		const struct nfs_open_context *ctx,
+		const struct nfs_lock_context *l_ctx,
+		fmode_t fmode);
 
 #if defined(CONFIG_NFS_V4_1)
 static inline struct nfs4_session *nfs4_get_session(const struct nfs_server *server)
@@ -347,13 +354,13 @@ extern int nfs4_wait_clnt_recover(struct nfs_client *clp);
 extern int nfs4_client_recover_expired_lease(struct nfs_client *clp);
 extern void nfs4_schedule_state_manager(struct nfs_client *);
 extern void nfs4_schedule_path_down_recovery(struct nfs_client *clp);
-extern void nfs4_schedule_stateid_recovery(const struct nfs_server *, struct nfs4_state *);
+extern int nfs4_schedule_stateid_recovery(const struct nfs_server *, struct nfs4_state *);
 extern void nfs41_handle_sequence_flag_errors(struct nfs_client *clp, u32 flags);
 extern void nfs41_handle_server_scope(struct nfs_client *,
 				      struct nfs41_server_scope **);
 extern void nfs4_put_lock_state(struct nfs4_lock_state *lsp);
 extern int nfs4_set_lock_state(struct nfs4_state *state, struct file_lock *fl);
-extern void nfs4_select_rw_stateid(nfs4_stateid *, struct nfs4_state *,
+extern int nfs4_select_rw_stateid(nfs4_stateid *, struct nfs4_state *,
 		fmode_t, const struct nfs_lockowner *);
 
 extern struct nfs_seqid *nfs_alloc_seqid(struct nfs_seqid_counter *counter, gfp_t gfp_mask);
@@ -412,6 +419,11 @@ static inline bool nfs4_stateid_match(const nfs4_stateid *dst, const nfs4_statei
 	return memcmp(dst, src, sizeof(*dst)) == 0;
 }
 
+static inline bool nfs4_valid_open_stateid(const struct nfs4_state *state)
+{
+	return test_bit(NFS_STATE_RECOVERY_FAILED, &state->flags) == 0;
+}
+
 #else
 
 #define nfs4_close_state(a, b) do { } while (0)
diff --git a/fs/nfs/nfs4client.c b/fs/nfs/nfs4client.c
index ac4fc9a8fdb..947b0c908aa 100644
--- a/fs/nfs/nfs4client.c
+++ b/fs/nfs/nfs4client.c
@@ -198,8 +198,12 @@ struct nfs_client *nfs4_init_client(struct nfs_client *clp,
 	/* Check NFS protocol revision and initialize RPC op vector */
 	clp->rpc_ops = &nfs_v4_clientops;
 
+	if (clp->cl_minorversion != 0)
+		__set_bit(NFS_CS_INFINITE_SLOTS, &clp->cl_flags);
 	__set_bit(NFS_CS_DISCRTRY, &clp->cl_flags);
-	error = nfs_create_rpc_client(clp, timeparms, authflavour);
+	error = nfs_create_rpc_client(clp, timeparms, RPC_AUTH_GSS_KRB5I);
+	if (error == -EINVAL)
+		error = nfs_create_rpc_client(clp, timeparms, RPC_AUTH_NULL);
 	if (error < 0)
 		goto error;
 
@@ -300,7 +304,7 @@ int nfs40_walk_client_list(struct nfs_client *new,
 			   struct rpc_cred *cred)
 {
 	struct nfs_net *nn = net_generic(new->cl_net, nfs_net_id);
-	struct nfs_client *pos, *n, *prev = NULL;
+	struct nfs_client *pos, *prev = NULL;
 	struct nfs4_setclientid_res clid = {
 		.clientid	= new->cl_clientid,
 		.confirm	= new->cl_confirm,
@@ -308,10 +312,23 @@ int nfs40_walk_client_list(struct nfs_client *new,
 	int status = -NFS4ERR_STALE_CLIENTID;
 
 	spin_lock(&nn->nfs_client_lock);
-	list_for_each_entry_safe(pos, n, &nn->nfs_client_list, cl_share_link) {
+	list_for_each_entry(pos, &nn->nfs_client_list, cl_share_link) {
 		/* If "pos" isn't marked ready, we can't trust the
 		 * remaining fields in "pos" */
-		if (pos->cl_cons_state < NFS_CS_READY)
+		if (pos->cl_cons_state > NFS_CS_READY) {
+			atomic_inc(&pos->cl_count);
+			spin_unlock(&nn->nfs_client_lock);
+
+			if (prev)
+				nfs_put_client(prev);
+			prev = pos;
+
+			status = nfs_wait_client_init_complete(pos);
+			spin_lock(&nn->nfs_client_lock);
+			if (status < 0)
+				continue;
+		}
+		if (pos->cl_cons_state != NFS_CS_READY)
 			continue;
 
 		if (pos->rpc_ops != new->rpc_ops)
@@ -423,16 +440,16 @@ int nfs41_walk_client_list(struct nfs_client *new,
 			   struct rpc_cred *cred)
 {
 	struct nfs_net *nn = net_generic(new->cl_net, nfs_net_id);
-	struct nfs_client *pos, *n, *prev = NULL;
+	struct nfs_client *pos, *prev = NULL;
 	int status = -NFS4ERR_STALE_CLIENTID;
 
 	spin_lock(&nn->nfs_client_lock);
-	list_for_each_entry_safe(pos, n, &nn->nfs_client_list, cl_share_link) {
+	list_for_each_entry(pos, &nn->nfs_client_list, cl_share_link) {
 		/* If "pos" isn't marked ready, we can't trust the
 		 * remaining fields in "pos", especially the client
 		 * ID and serverowner fields.  Wait for CREATE_SESSION
 		 * to finish. */
-		if (pos->cl_cons_state < NFS_CS_READY) {
+		if (pos->cl_cons_state > NFS_CS_READY) {
 			atomic_inc(&pos->cl_count);
 			spin_unlock(&nn->nfs_client_lock);
 
@@ -440,18 +457,17 @@ int nfs41_walk_client_list(struct nfs_client *new,
 				nfs_put_client(prev);
 			prev = pos;
 
-			nfs4_schedule_lease_recovery(pos);
 			status = nfs_wait_client_init_complete(pos);
-			if (status < 0) {
-				nfs_put_client(pos);
-				spin_lock(&nn->nfs_client_lock);
-				continue;
+			if (status == 0) {
+				nfs4_schedule_lease_recovery(pos);
+				status = nfs4_wait_clnt_recover(pos);
 			}
-			status = pos->cl_cons_state;
 			spin_lock(&nn->nfs_client_lock);
 			if (status < 0)
 				continue;
 		}
+		if (pos->cl_cons_state != NFS_CS_READY)
+			continue;
 
 		if (pos->rpc_ops != new->rpc_ops)
 			continue;
@@ -469,17 +485,18 @@ int nfs41_walk_client_list(struct nfs_client *new,
 			continue;
 
 		atomic_inc(&pos->cl_count);
-		spin_unlock(&nn->nfs_client_lock);
+		*result = pos;
+		status = 0;
 		dprintk("NFS: <-- %s using nfs_client = %p ({%d})\n",
 			__func__, pos, atomic_read(&pos->cl_count));
-
-		*result = pos;
-		return 0;
+		break;
 	}
 
 	/* No matching nfs_client found. */
 	spin_unlock(&nn->nfs_client_lock);
 	dprintk("NFS: <-- %s status = %d\n", __func__, status);
+	if (prev)
+		nfs_put_client(prev);
 	return status;
 }
 #endif	/* CONFIG_NFS_V4_1 */
@@ -717,6 +734,19 @@ static int nfs4_server_common_setup(struct nfs_server *server,
 	if (error < 0)
 		goto out;
 
+	/* Set the basic capabilities */
+	server->caps |= server->nfs_client->cl_mvops->init_caps;
+	if (server->flags & NFS_MOUNT_NORDIRPLUS)
+			server->caps &= ~NFS_CAP_READDIRPLUS;
+	/*
+	 * Don't use NFS uid/gid mapping if we're using AUTH_SYS or lower
+	 * authentication.
+	 */
+	if (nfs4_disable_idmapping &&
+			server->client->cl_auth->au_flavor == RPC_AUTH_UNIX)
+		server->caps |= NFS_CAP_UIDGID_NOMAP;
+
+
 	/* Probe the root fh to retrieve its FSID and filehandle */
 	error = nfs4_get_rootfh(server, mntfh);
 	if (error < 0)
@@ -760,9 +790,6 @@ static int nfs4_init_server(struct nfs_server *server,
 
 	/* Initialise the client representation from the mount data */
 	server->flags = data->flags;
-	server->caps |= NFS_CAP_ATOMIC_OPEN|NFS_CAP_CHANGE_ATTR|NFS_CAP_POSIX_LOCK;
-	if (!(data->flags & NFS_MOUNT_NORDIRPLUS))
-			server->caps |= NFS_CAP_READDIRPLUS;
 	server->options = data->options;
 
 	/* Get a client record */
@@ -779,13 +806,6 @@ static int nfs4_init_server(struct nfs_server *server,
 	if (error < 0)
 		goto error;
 
-	/*
-	 * Don't use NFS uid/gid mapping if we're using AUTH_SYS or lower
-	 * authentication.
-	 */
-	if (nfs4_disable_idmapping && data->auth_flavors[0] == RPC_AUTH_UNIX)
-		server->caps |= NFS_CAP_UIDGID_NOMAP;
-
 	if (data->rsize)
 		server->rsize = nfs_block_size(data->rsize, NULL);
 	if (data->wsize)
@@ -863,7 +883,6 @@ struct nfs_server *nfs4_create_referral_server(struct nfs_clone_mount *data,
 
 	/* Initialise the client representation from the parent server */
 	nfs_server_copy_userdata(server, parent_server);
-	server->caps |= NFS_CAP_ATOMIC_OPEN|NFS_CAP_CHANGE_ATTR;
 
 	/* Get a client representation.
 	 * Note: NFSv4 always uses TCP, */
diff --git a/fs/nfs/nfs4filelayout.c b/fs/nfs/nfs4filelayout.c
index 49eeb044c10..22d10623f5e 100644
--- a/fs/nfs/nfs4filelayout.c
+++ b/fs/nfs/nfs4filelayout.c
@@ -129,7 +129,6 @@ static void filelayout_fenceme(struct inode *inode, struct pnfs_layout_hdr *lo)
 {
 	if (!test_and_clear_bit(NFS_LAYOUT_RETURN, &lo->plh_flags))
 		return;
-	clear_bit(NFS_INO_LAYOUTCOMMIT, &NFS_I(inode)->flags);
 	pnfs_return_layout(inode);
 }
 
@@ -159,11 +158,14 @@ static int filelayout_async_handle_error(struct rpc_task *task,
 	case -NFS4ERR_OPENMODE:
 		if (state == NULL)
 			break;
-		nfs4_schedule_stateid_recovery(mds_server, state);
+		if (nfs4_schedule_stateid_recovery(mds_server, state) < 0)
+			goto out_bad_stateid;
 		goto wait_on_recovery;
 	case -NFS4ERR_EXPIRED:
-		if (state != NULL)
-			nfs4_schedule_stateid_recovery(mds_server, state);
+		if (state != NULL) {
+			if (nfs4_schedule_stateid_recovery(mds_server, state) < 0)
+				goto out_bad_stateid;
+		}
 		nfs4_schedule_lease_recovery(mds_client);
 		goto wait_on_recovery;
 	/* DS session errors */
@@ -227,6 +229,9 @@ reset:
 out:
 	task->tk_status = 0;
 	return -EAGAIN;
+out_bad_stateid:
+	task->tk_status = -EIO;
+	return 0;
 wait_on_recovery:
 	rpc_sleep_on(&mds_client->cl_rpcwaitq, task, NULL);
 	if (test_bit(NFS4CLNT_MANAGER_RUNNING, &mds_client->cl_state) == 0)
@@ -300,6 +305,10 @@ static void filelayout_read_prepare(struct rpc_task *task, void *data)
 {
 	struct nfs_read_data *rdata = data;
 
+	if (unlikely(test_bit(NFS_CONTEXT_BAD, &rdata->args.context->flags))) {
+		rpc_exit(task, -EIO);
+		return;
+	}
 	if (filelayout_reset_to_mds(rdata->header->lseg)) {
 		dprintk("%s task %u reset io to MDS\n", __func__, task->tk_pid);
 		filelayout_reset_read(rdata);
@@ -308,10 +317,13 @@ static void filelayout_read_prepare(struct rpc_task *task, void *data)
 	}
 	rdata->read_done_cb = filelayout_read_done_cb;
 
-	nfs41_setup_sequence(rdata->ds_clp->cl_session,
+	if (nfs41_setup_sequence(rdata->ds_clp->cl_session,
 			&rdata->args.seq_args,
 			&rdata->res.seq_res,
-			task);
+			task))
+		return;
+	nfs4_set_rw_stateid(&rdata->args.stateid, rdata->args.context,
+			rdata->args.lock_context, FMODE_READ);
 }
 
 static void filelayout_read_call_done(struct rpc_task *task, void *data)
@@ -402,16 +414,23 @@ static void filelayout_write_prepare(struct rpc_task *task, void *data)
 {
 	struct nfs_write_data *wdata = data;
 
+	if (unlikely(test_bit(NFS_CONTEXT_BAD, &wdata->args.context->flags))) {
+		rpc_exit(task, -EIO);
+		return;
+	}
 	if (filelayout_reset_to_mds(wdata->header->lseg)) {
 		dprintk("%s task %u reset io to MDS\n", __func__, task->tk_pid);
 		filelayout_reset_write(wdata);
 		rpc_exit(task, 0);
 		return;
 	}
-	nfs41_setup_sequence(wdata->ds_clp->cl_session,
+	if (nfs41_setup_sequence(wdata->ds_clp->cl_session,
 			&wdata->args.seq_args,
 			&wdata->res.seq_res,
-			task);
+			task))
+		return;
+	nfs4_set_rw_stateid(&wdata->args.stateid, wdata->args.context,
+			wdata->args.lock_context, FMODE_WRITE);
 }
 
 static void filelayout_write_call_done(struct rpc_task *task, void *data)
diff --git a/fs/nfs/nfs4namespace.c b/fs/nfs/nfs4namespace.c
index 0dd766079e1..cdb0b41a481 100644
--- a/fs/nfs/nfs4namespace.c
+++ b/fs/nfs/nfs4namespace.c
@@ -134,33 +134,38 @@ static size_t nfs_parse_server_name(char *string, size_t len,
 	return ret;
 }
 
+/**
+ * nfs_find_best_sec - Find a security mechanism supported locally
+ * @flavors: List of security tuples returned by SECINFO procedure
+ *
+ * Return the pseudoflavor of the first security mechanism in
+ * "flavors" that is locally supported.  Return RPC_AUTH_UNIX if
+ * no matching flavor is found in the array.  The "flavors" array
+ * is searched in the order returned from the server, per RFC 3530
+ * recommendation.
+ */
 rpc_authflavor_t nfs_find_best_sec(struct nfs4_secinfo_flavors *flavors)
 {
-	struct gss_api_mech *mech;
-	struct xdr_netobj oid;
-	int i;
-	rpc_authflavor_t pseudoflavor = RPC_AUTH_UNIX;
+	rpc_authflavor_t pseudoflavor;
+	struct nfs4_secinfo4 *secinfo;
+	unsigned int i;
 
 	for (i = 0; i < flavors->num_flavors; i++) {
-		struct nfs4_secinfo_flavor *flavor;
-		flavor = &flavors->flavors[i];
-
-		if (flavor->flavor == RPC_AUTH_NULL || flavor->flavor == RPC_AUTH_UNIX) {
-			pseudoflavor = flavor->flavor;
-			break;
-		} else if (flavor->flavor == RPC_AUTH_GSS) {
-			oid.len  = flavor->gss.sec_oid4.len;
-			oid.data = flavor->gss.sec_oid4.data;
-			mech = gss_mech_get_by_OID(&oid);
-			if (!mech)
-				continue;
-			pseudoflavor = gss_svc_to_pseudoflavor(mech, flavor->gss.service);
-			gss_mech_put(mech);
+		secinfo = &flavors->flavors[i];
+
+		switch (secinfo->flavor) {
+		case RPC_AUTH_NULL:
+		case RPC_AUTH_UNIX:
+		case RPC_AUTH_GSS:
+			pseudoflavor = rpcauth_get_pseudoflavor(secinfo->flavor,
+							&secinfo->flavor_info);
+			if (pseudoflavor != RPC_AUTH_MAXFLAVOR)
+				return pseudoflavor;
 			break;
 		}
 	}
 
-	return pseudoflavor;
+	return RPC_AUTH_UNIX;
 }
 
 static rpc_authflavor_t nfs4_negotiate_security(struct inode *inode, struct qstr *name)
diff --git a/fs/nfs/nfs4proc.c b/fs/nfs/nfs4proc.c
index b2671cb0f90..9da4bd55eb3 100644
--- a/fs/nfs/nfs4proc.c
+++ b/fs/nfs/nfs4proc.c
@@ -107,6 +107,8 @@ static int nfs4_map_errors(int err)
 		return -EPROTONOSUPPORT;
 	case -NFS4ERR_ACCESS:
 		return -EACCES;
+	case -NFS4ERR_FILE_OPEN:
+		return -EBUSY;
 	default:
 		dprintk("%s could not handle NFSv4 error %d\n",
 				__func__, -err);
@@ -295,19 +297,30 @@ static int nfs4_handle_exception(struct nfs_server *server, int errorcode, struc
 			}
 			if (state == NULL)
 				break;
-			nfs4_schedule_stateid_recovery(server, state);
+			ret = nfs4_schedule_stateid_recovery(server, state);
+			if (ret < 0)
+				break;
 			goto wait_on_recovery;
 		case -NFS4ERR_DELEG_REVOKED:
 		case -NFS4ERR_ADMIN_REVOKED:
 		case -NFS4ERR_BAD_STATEID:
+			if (inode != NULL && nfs4_have_delegation(inode, FMODE_READ)) {
+				nfs_remove_bad_delegation(inode);
+				exception->retry = 1;
+				break;
+			}
 			if (state == NULL)
 				break;
-			nfs_remove_bad_delegation(state->inode);
-			nfs4_schedule_stateid_recovery(server, state);
+			ret = nfs4_schedule_stateid_recovery(server, state);
+			if (ret < 0)
+				break;
 			goto wait_on_recovery;
 		case -NFS4ERR_EXPIRED:
-			if (state != NULL)
-				nfs4_schedule_stateid_recovery(server, state);
+			if (state != NULL) {
+				ret = nfs4_schedule_stateid_recovery(server, state);
+				if (ret < 0)
+					break;
+			}
 		case -NFS4ERR_STALE_STATEID:
 		case -NFS4ERR_STALE_CLIENTID:
 			nfs4_schedule_lease_recovery(clp);
@@ -756,10 +769,40 @@ struct nfs4_opendata {
 	struct iattr attrs;
 	unsigned long timestamp;
 	unsigned int rpc_done : 1;
+	unsigned int is_recover : 1;
 	int rpc_status;
 	int cancelled;
 };
 
+static bool nfs4_clear_cap_atomic_open_v1(struct nfs_server *server,
+		int err, struct nfs4_exception *exception)
+{
+	if (err != -EINVAL)
+		return false;
+	if (!(server->caps & NFS_CAP_ATOMIC_OPEN_V1))
+		return false;
+	server->caps &= ~NFS_CAP_ATOMIC_OPEN_V1;
+	exception->retry = 1;
+	return true;
+}
+
+static enum open_claim_type4
+nfs4_map_atomic_open_claim(struct nfs_server *server,
+		enum open_claim_type4 claim)
+{
+	if (server->caps & NFS_CAP_ATOMIC_OPEN_V1)
+		return claim;
+	switch (claim) {
+	default:
+		return claim;
+	case NFS4_OPEN_CLAIM_FH:
+		return NFS4_OPEN_CLAIM_NULL;
+	case NFS4_OPEN_CLAIM_DELEG_CUR_FH:
+		return NFS4_OPEN_CLAIM_DELEGATE_CUR;
+	case NFS4_OPEN_CLAIM_DELEG_PREV_FH:
+		return NFS4_OPEN_CLAIM_DELEGATE_PREV;
+	}
+}
 
 static void nfs4_init_opendata_res(struct nfs4_opendata *p)
 {
@@ -775,6 +818,7 @@ static void nfs4_init_opendata_res(struct nfs4_opendata *p)
 static struct nfs4_opendata *nfs4_opendata_alloc(struct dentry *dentry,
 		struct nfs4_state_owner *sp, fmode_t fmode, int flags,
 		const struct iattr *attrs,
+		enum open_claim_type4 claim,
 		gfp_t gfp_mask)
 {
 	struct dentry *parent = dget_parent(dentry);
@@ -793,7 +837,6 @@ static struct nfs4_opendata *nfs4_opendata_alloc(struct dentry *dentry,
 	p->dir = parent;
 	p->owner = sp;
 	atomic_inc(&sp->so_count);
-	p->o_arg.fh = NFS_FH(dir);
 	p->o_arg.open_flags = flags;
 	p->o_arg.fmode = fmode & (FMODE_READ|FMODE_WRITE);
 	/* don't put an ACCESS op in OPEN compound if O_EXCL, because ACCESS
@@ -811,7 +854,19 @@ static struct nfs4_opendata *nfs4_opendata_alloc(struct dentry *dentry,
 	p->o_arg.server = server;
 	p->o_arg.bitmask = server->attr_bitmask;
 	p->o_arg.open_bitmap = &nfs4_fattr_bitmap[0];
-	p->o_arg.claim = NFS4_OPEN_CLAIM_NULL;
+	p->o_arg.claim = nfs4_map_atomic_open_claim(server, claim);
+	switch (p->o_arg.claim) {
+	case NFS4_OPEN_CLAIM_NULL:
+	case NFS4_OPEN_CLAIM_DELEGATE_CUR:
+	case NFS4_OPEN_CLAIM_DELEGATE_PREV:
+		p->o_arg.fh = NFS_FH(dir);
+		break;
+	case NFS4_OPEN_CLAIM_PREVIOUS:
+	case NFS4_OPEN_CLAIM_FH:
+	case NFS4_OPEN_CLAIM_DELEG_CUR_FH:
+	case NFS4_OPEN_CLAIM_DELEG_PREV_FH:
+		p->o_arg.fh = NFS_FH(dentry->d_inode);
+	}
 	if (attrs != NULL && attrs->ia_valid != 0) {
 		__be32 verf[2];
 
@@ -924,6 +979,7 @@ static void nfs_set_open_stateid_locked(struct nfs4_state *state, nfs4_stateid *
 	if (test_bit(NFS_DELEGATED_STATE, &state->flags) == 0)
 		nfs4_stateid_copy(&state->stateid, stateid);
 	nfs4_stateid_copy(&state->open_stateid, stateid);
+	set_bit(NFS_OPEN_STATE, &state->flags);
 	switch (fmode) {
 		case FMODE_READ:
 			set_bit(NFS_O_RDONLY_STATE, &state->flags);
@@ -1046,9 +1102,12 @@ static struct nfs4_state *nfs4_try_open_cached(struct nfs4_opendata *opendata)
 		/* Save the delegation */
 		nfs4_stateid_copy(&stateid, &delegation->stateid);
 		rcu_read_unlock();
-		ret = nfs_may_open(state->inode, state->owner->so_cred, open_mode);
-		if (ret != 0)
-			goto out;
+		nfs_release_seqid(opendata->o_arg.seqid);
+		if (!opendata->is_recover) {
+			ret = nfs_may_open(state->inode, state->owner->so_cred, open_mode);
+			if (ret != 0)
+				goto out;
+		}
 		ret = -EAGAIN;
 
 		/* Try to update the stateid using the delegation */
@@ -1193,11 +1252,13 @@ static struct nfs_open_context *nfs4_state_find_open_context(struct nfs4_state *
 	return ERR_PTR(-ENOENT);
 }
 
-static struct nfs4_opendata *nfs4_open_recoverdata_alloc(struct nfs_open_context *ctx, struct nfs4_state *state)
+static struct nfs4_opendata *nfs4_open_recoverdata_alloc(struct nfs_open_context *ctx,
+		struct nfs4_state *state, enum open_claim_type4 claim)
 {
 	struct nfs4_opendata *opendata;
 
-	opendata = nfs4_opendata_alloc(ctx->dentry, state->owner, 0, 0, NULL, GFP_NOFS);
+	opendata = nfs4_opendata_alloc(ctx->dentry, state->owner, 0, 0,
+			NULL, claim, GFP_NOFS);
 	if (opendata == NULL)
 		return ERR_PTR(-ENOMEM);
 	opendata->state = state;
@@ -1233,6 +1294,7 @@ static int nfs4_open_recover(struct nfs4_opendata *opendata, struct nfs4_state *
 
 	/* memory barrier prior to reading state->n_* */
 	clear_bit(NFS_DELEGATED_STATE, &state->flags);
+	clear_bit(NFS_OPEN_STATE, &state->flags);
 	smp_rmb();
 	if (state->n_rdwr != 0) {
 		clear_bit(NFS_O_RDWR_STATE, &state->flags);
@@ -1283,11 +1345,10 @@ static int _nfs4_do_open_reclaim(struct nfs_open_context *ctx, struct nfs4_state
 	fmode_t delegation_type = 0;
 	int status;
 
-	opendata = nfs4_open_recoverdata_alloc(ctx, state);
+	opendata = nfs4_open_recoverdata_alloc(ctx, state,
+			NFS4_OPEN_CLAIM_PREVIOUS);
 	if (IS_ERR(opendata))
 		return PTR_ERR(opendata);
-	opendata->o_arg.claim = NFS4_OPEN_CLAIM_PREVIOUS;
-	opendata->o_arg.fh = NFS_FH(state->inode);
 	rcu_read_lock();
 	delegation = rcu_dereference(NFS_I(state->inode)->delegation);
 	if (delegation != NULL && test_bit(NFS_DELEGATION_NEED_RECLAIM, &delegation->flags) != 0)
@@ -1306,6 +1367,8 @@ static int nfs4_do_open_reclaim(struct nfs_open_context *ctx, struct nfs4_state
 	int err;
 	do {
 		err = _nfs4_do_open_reclaim(ctx, state);
+		if (nfs4_clear_cap_atomic_open_v1(server, err, &exception))
+			continue;
 		if (err != -NFS4ERR_DELAY)
 			break;
 		nfs4_handle_exception(server, err, &exception);
@@ -1320,71 +1383,72 @@ static int nfs4_open_reclaim(struct nfs4_state_owner *sp, struct nfs4_state *sta
 
 	ctx = nfs4_state_find_open_context(state);
 	if (IS_ERR(ctx))
-		return PTR_ERR(ctx);
+		return -EAGAIN;
 	ret = nfs4_do_open_reclaim(ctx, state);
 	put_nfs_open_context(ctx);
 	return ret;
 }
 
-static int _nfs4_open_delegation_recall(struct nfs_open_context *ctx, struct nfs4_state *state, const nfs4_stateid *stateid)
+static int nfs4_handle_delegation_recall_error(struct nfs_server *server, struct nfs4_state *state, const nfs4_stateid *stateid, int err)
 {
-	struct nfs4_opendata *opendata;
-	int ret;
-
-	opendata = nfs4_open_recoverdata_alloc(ctx, state);
-	if (IS_ERR(opendata))
-		return PTR_ERR(opendata);
-	opendata->o_arg.claim = NFS4_OPEN_CLAIM_DELEGATE_CUR;
-	nfs4_stateid_copy(&opendata->o_arg.u.delegation, stateid);
-	ret = nfs4_open_recover(opendata, state);
-	nfs4_opendata_put(opendata);
-	return ret;
+	switch (err) {
+		default:
+			printk(KERN_ERR "NFS: %s: unhandled error "
+					"%d.\n", __func__, err);
+		case 0:
+		case -ENOENT:
+		case -ESTALE:
+			break;
+		case -NFS4ERR_BADSESSION:
+		case -NFS4ERR_BADSLOT:
+		case -NFS4ERR_BAD_HIGH_SLOT:
+		case -NFS4ERR_CONN_NOT_BOUND_TO_SESSION:
+		case -NFS4ERR_DEADSESSION:
+			set_bit(NFS_DELEGATED_STATE, &state->flags);
+			nfs4_schedule_session_recovery(server->nfs_client->cl_session, err);
+			return -EAGAIN;
+		case -NFS4ERR_STALE_CLIENTID:
+		case -NFS4ERR_STALE_STATEID:
+			set_bit(NFS_DELEGATED_STATE, &state->flags);
+		case -NFS4ERR_EXPIRED:
+			/* Don't recall a delegation if it was lost */
+			nfs4_schedule_lease_recovery(server->nfs_client);
+			return -EAGAIN;
+		case -NFS4ERR_DELEG_REVOKED:
+		case -NFS4ERR_ADMIN_REVOKED:
+		case -NFS4ERR_BAD_STATEID:
+		case -NFS4ERR_OPENMODE:
+			nfs_inode_find_state_and_recover(state->inode,
+					stateid);
+			nfs4_schedule_stateid_recovery(server, state);
+			return 0;
+		case -NFS4ERR_DELAY:
+		case -NFS4ERR_GRACE:
+			set_bit(NFS_DELEGATED_STATE, &state->flags);
+			ssleep(1);
+			return -EAGAIN;
+		case -ENOMEM:
+		case -NFS4ERR_DENIED:
+			/* kill_proc(fl->fl_pid, SIGLOST, 1); */
+			return 0;
+	}
+	return err;
 }
 
 int nfs4_open_delegation_recall(struct nfs_open_context *ctx, struct nfs4_state *state, const nfs4_stateid *stateid)
 {
-	struct nfs4_exception exception = { };
 	struct nfs_server *server = NFS_SERVER(state->inode);
+	struct nfs4_opendata *opendata;
 	int err;
-	do {
-		err = _nfs4_open_delegation_recall(ctx, state, stateid);
-		switch (err) {
-			case 0:
-			case -ENOENT:
-			case -ESTALE:
-				goto out;
-			case -NFS4ERR_BADSESSION:
-			case -NFS4ERR_BADSLOT:
-			case -NFS4ERR_BAD_HIGH_SLOT:
-			case -NFS4ERR_CONN_NOT_BOUND_TO_SESSION:
-			case -NFS4ERR_DEADSESSION:
-				set_bit(NFS_DELEGATED_STATE, &state->flags);
-				nfs4_schedule_session_recovery(server->nfs_client->cl_session, err);
-				err = -EAGAIN;
-				goto out;
-			case -NFS4ERR_STALE_CLIENTID:
-			case -NFS4ERR_STALE_STATEID:
-				set_bit(NFS_DELEGATED_STATE, &state->flags);
-			case -NFS4ERR_EXPIRED:
-				/* Don't recall a delegation if it was lost */
-				nfs4_schedule_lease_recovery(server->nfs_client);
-				err = -EAGAIN;
-				goto out;
-			case -NFS4ERR_DELEG_REVOKED:
-			case -NFS4ERR_ADMIN_REVOKED:
-			case -NFS4ERR_BAD_STATEID:
-				nfs_inode_find_state_and_recover(state->inode,
-						stateid);
-				nfs4_schedule_stateid_recovery(server, state);
-			case -ENOMEM:
-				err = 0;
-				goto out;
-		}
-		set_bit(NFS_DELEGATED_STATE, &state->flags);
-		err = nfs4_handle_exception(server, err, &exception);
-	} while (exception.retry);
-out:
-	return err;
+
+	opendata = nfs4_open_recoverdata_alloc(ctx, state,
+			NFS4_OPEN_CLAIM_DELEG_CUR_FH);
+	if (IS_ERR(opendata))
+		return PTR_ERR(opendata);
+	nfs4_stateid_copy(&opendata->o_arg.u.delegation, stateid);
+	err = nfs4_open_recover(opendata, state);
+	nfs4_opendata_put(opendata);
+	return nfs4_handle_delegation_recall_error(server, state, stateid, err);
 }
 
 static void nfs4_open_confirm_done(struct rpc_task *task, void *calldata)
@@ -1467,6 +1531,7 @@ static void nfs4_open_prepare(struct rpc_task *task, void *calldata)
 {
 	struct nfs4_opendata *data = calldata;
 	struct nfs4_state_owner *sp = data->owner;
+	struct nfs_client *clp = sp->so_server->nfs_client;
 
 	if (nfs_wait_on_sequence(data->o_arg.seqid, task) != 0)
 		goto out_wait;
@@ -1482,15 +1547,20 @@ static void nfs4_open_prepare(struct rpc_task *task, void *calldata)
 		rcu_read_lock();
 		delegation = rcu_dereference(NFS_I(data->state->inode)->delegation);
 		if (data->o_arg.claim != NFS4_OPEN_CLAIM_DELEGATE_CUR &&
+		    data->o_arg.claim != NFS4_OPEN_CLAIM_DELEG_CUR_FH &&
 		    can_open_delegated(delegation, data->o_arg.fmode))
 			goto unlock_no_action;
 		rcu_read_unlock();
 	}
 	/* Update client id. */
-	data->o_arg.clientid = sp->so_server->nfs_client->cl_clientid;
-	if (data->o_arg.claim == NFS4_OPEN_CLAIM_PREVIOUS) {
-		task->tk_msg.rpc_proc = &nfs4_procedures[NFSPROC4_CLNT_OPEN_NOATTR];
+	data->o_arg.clientid = clp->cl_clientid;
+	switch (data->o_arg.claim) {
+	case NFS4_OPEN_CLAIM_PREVIOUS:
+	case NFS4_OPEN_CLAIM_DELEG_CUR_FH:
+	case NFS4_OPEN_CLAIM_DELEG_PREV_FH:
 		data->o_arg.open_bitmap = &nfs4_open_noattr_bitmap[0];
+	case NFS4_OPEN_CLAIM_FH:
+		task->tk_msg.rpc_proc = &nfs4_procedures[NFSPROC4_CLNT_OPEN_NOATTR];
 		nfs_copy_fh(&data->o_res.fh, data->o_arg.fh);
 	}
 	data->timestamp = jiffies;
@@ -1499,6 +1569,16 @@ static void nfs4_open_prepare(struct rpc_task *task, void *calldata)
 				&data->o_res.seq_res,
 				task) != 0)
 		nfs_release_seqid(data->o_arg.seqid);
+
+	/* Set the create mode (note dependency on the session type) */
+	data->o_arg.createmode = NFS4_CREATE_UNCHECKED;
+	if (data->o_arg.open_flags & O_EXCL) {
+		data->o_arg.createmode = NFS4_CREATE_EXCLUSIVE;
+		if (nfs4_has_persistent_session(clp))
+			data->o_arg.createmode = NFS4_CREATE_GUARDED;
+		else if (clp->cl_mvops->minor_version > 0)
+			data->o_arg.createmode = NFS4_CREATE_EXCLUSIVE4_1;
+	}
 	return;
 unlock_no_action:
 	rcu_read_unlock();
@@ -1594,8 +1674,11 @@ static int nfs4_run_open_task(struct nfs4_opendata *data, int isrecover)
 	data->rpc_done = 0;
 	data->rpc_status = 0;
 	data->cancelled = 0;
-	if (isrecover)
+	data->is_recover = 0;
+	if (isrecover) {
 		nfs4_set_sequence_privileged(&o_arg->seq_args);
+		data->is_recover = 1;
+	}
 	task = rpc_run_task(&task_setup_data);
         if (IS_ERR(task))
                 return PTR_ERR(task);
@@ -1720,7 +1803,8 @@ static int _nfs4_open_expired(struct nfs_open_context *ctx, struct nfs4_state *s
 	struct nfs4_opendata *opendata;
 	int ret;
 
-	opendata = nfs4_open_recoverdata_alloc(ctx, state);
+	opendata = nfs4_open_recoverdata_alloc(ctx, state,
+			NFS4_OPEN_CLAIM_FH);
 	if (IS_ERR(opendata))
 		return PTR_ERR(opendata);
 	ret = nfs4_open_recover(opendata, state);
@@ -1738,6 +1822,8 @@ static int nfs4_do_open_expired(struct nfs_open_context *ctx, struct nfs4_state
 
 	do {
 		err = _nfs4_open_expired(ctx, state);
+		if (nfs4_clear_cap_atomic_open_v1(server, err, &exception))
+			continue;
 		switch (err) {
 		default:
 			goto out;
@@ -1758,7 +1844,7 @@ static int nfs4_open_expired(struct nfs4_state_owner *sp, struct nfs4_state *sta
 
 	ctx = nfs4_state_find_open_context(state);
 	if (IS_ERR(ctx))
-		return PTR_ERR(ctx);
+		return -EAGAIN;
 	ret = nfs4_do_open_expired(ctx, state);
 	put_nfs_open_context(ctx);
 	return ret;
@@ -1820,6 +1906,7 @@ static int nfs41_check_open_stateid(struct nfs4_state *state)
 		clear_bit(NFS_O_RDONLY_STATE, &state->flags);
 		clear_bit(NFS_O_WRONLY_STATE, &state->flags);
 		clear_bit(NFS_O_RDWR_STATE, &state->flags);
+		clear_bit(NFS_OPEN_STATE, &state->flags);
 	}
 	return status;
 }
@@ -1880,10 +1967,8 @@ static int _nfs4_open_and_get_state(struct nfs4_opendata *opendata,
 	if (ret != 0)
 		goto out;
 
-	if (read_seqcount_retry(&sp->so_reclaim_seqcount, seq)) {
+	if (read_seqcount_retry(&sp->so_reclaim_seqcount, seq))
 		nfs4_schedule_stateid_recovery(server, state);
-		nfs4_wait_clnt_recover(server->nfs_client);
-	}
 	*res = state;
 out:
 	return ret;
@@ -1905,6 +1990,7 @@ static int _nfs4_do_open(struct inode *dir,
 	struct nfs4_state     *state = NULL;
 	struct nfs_server       *server = NFS_SERVER(dir);
 	struct nfs4_opendata *opendata;
+	enum open_claim_type4 claim = NFS4_OPEN_CLAIM_NULL;
 	int status;
 
 	/* Protect against reboot recovery conflicts */
@@ -1920,7 +2006,10 @@ static int _nfs4_do_open(struct inode *dir,
 	if (dentry->d_inode != NULL)
 		nfs4_return_incompatible_delegation(dentry->d_inode, fmode);
 	status = -ENOMEM;
-	opendata = nfs4_opendata_alloc(dentry, sp, fmode, flags, sattr, GFP_KERNEL);
+	if (dentry->d_inode)
+		claim = NFS4_OPEN_CLAIM_FH;
+	opendata = nfs4_opendata_alloc(dentry, sp, fmode, flags, sattr,
+			claim, GFP_KERNEL);
 	if (opendata == NULL)
 		goto err_put_state_owner;
 
@@ -1937,7 +2026,8 @@ static int _nfs4_do_open(struct inode *dir,
 	if (status != 0)
 		goto err_opendata_put;
 
-	if (opendata->o_arg.open_flags & O_EXCL) {
+	if ((opendata->o_arg.open_flags & O_EXCL) &&
+	    (opendata->o_arg.createmode != NFS4_CREATE_GUARDED)) {
 		nfs4_exclusive_attrset(opendata, sattr);
 
 		nfs_fattr_init(opendata->o_res.f_attr);
@@ -1978,6 +2068,7 @@ static struct nfs4_state *nfs4_do_open(struct inode *dir,
 					struct rpc_cred *cred,
 					struct nfs4_threshold **ctx_th)
 {
+	struct nfs_server *server = NFS_SERVER(dir);
 	struct nfs4_exception exception = { };
 	struct nfs4_state *res;
 	int status;
@@ -2021,7 +2112,9 @@ static struct nfs4_state *nfs4_do_open(struct inode *dir,
 			exception.retry = 1;
 			continue;
 		}
-		res = ERR_PTR(nfs4_handle_exception(NFS_SERVER(dir),
+		if (nfs4_clear_cap_atomic_open_v1(server, status, &exception))
+			continue;
+		res = ERR_PTR(nfs4_handle_exception(server,
 					status, &exception));
 	} while (exception.retry);
 	return res;
@@ -2049,20 +2142,25 @@ static int _nfs4_do_setattr(struct inode *inode, struct rpc_cred *cred,
 		.rpc_cred	= cred,
         };
 	unsigned long timestamp = jiffies;
+	fmode_t fmode;
+	bool truncate;
 	int status;
 
 	nfs_fattr_init(fattr);
 
-	if (state != NULL) {
+	/* Servers should only apply open mode checks for file size changes */
+	truncate = (sattr->ia_valid & ATTR_SIZE) ? true : false;
+	fmode = truncate ? FMODE_WRITE : FMODE_READ;
+
+	if (nfs4_copy_delegation_stateid(&arg.stateid, inode, fmode)) {
+		/* Use that stateid */
+	} else if (truncate && state != NULL && nfs4_valid_open_stateid(state)) {
 		struct nfs_lockowner lockowner = {
 			.l_owner = current->files,
 			.l_pid = current->tgid,
 		};
 		nfs4_select_rw_stateid(&arg.stateid, state, FMODE_WRITE,
 				&lockowner);
-	} else if (nfs4_copy_delegation_stateid(&arg.stateid, inode,
-				FMODE_WRITE)) {
-		/* Use that stateid */
 	} else
 		nfs4_stateid_copy(&arg.stateid, &zero_stateid);
 
@@ -2086,6 +2184,13 @@ static int nfs4_do_setattr(struct inode *inode, struct rpc_cred *cred,
 		err = _nfs4_do_setattr(inode, cred, fattr, sattr, state);
 		switch (err) {
 		case -NFS4ERR_OPENMODE:
+			if (!(sattr->ia_valid & ATTR_SIZE)) {
+				pr_warn_once("NFSv4: server %s is incorrectly "
+						"applying open mode checks to "
+						"a SETATTR that is not "
+						"changing file size.\n",
+						server->nfs_client->cl_hostname);
+			}
 			if (state && !(state->state & FMODE_WRITE)) {
 				err = -EBADF;
 				if (sattr->ia_valid & ATTR_OPEN)
@@ -2129,11 +2234,19 @@ static void nfs4_close_clear_stateid_flags(struct nfs4_state *state,
 		fmode_t fmode)
 {
 	spin_lock(&state->owner->so_lock);
-	if (!(fmode & FMODE_READ))
+	clear_bit(NFS_O_RDWR_STATE, &state->flags);
+	switch (fmode & (FMODE_READ|FMODE_WRITE)) {
+	case FMODE_WRITE:
 		clear_bit(NFS_O_RDONLY_STATE, &state->flags);
-	if (!(fmode & FMODE_WRITE))
+		break;
+	case FMODE_READ:
 		clear_bit(NFS_O_WRONLY_STATE, &state->flags);
-	clear_bit(NFS_O_RDWR_STATE, &state->flags);
+		break;
+	case 0:
+		clear_bit(NFS_O_RDONLY_STATE, &state->flags);
+		clear_bit(NFS_O_WRONLY_STATE, &state->flags);
+		clear_bit(NFS_OPEN_STATE, &state->flags);
+	}
 	spin_unlock(&state->owner->so_lock);
 }
 
@@ -2201,6 +2314,8 @@ static void nfs4_close_prepare(struct rpc_task *task, void *data)
 			calldata->arg.fmode &= ~FMODE_WRITE;
 		}
 	}
+	if (!nfs4_valid_open_stateid(state))
+		call_close = 0;
 	spin_unlock(&state->owner->so_lock);
 
 	if (!call_close) {
@@ -2211,8 +2326,10 @@ static void nfs4_close_prepare(struct rpc_task *task, void *data)
 	if (calldata->arg.fmode == 0) {
 		task->tk_msg.rpc_proc = &nfs4_procedures[NFSPROC4_CLNT_CLOSE];
 		if (calldata->roc &&
-		    pnfs_roc_drain(inode, &calldata->roc_barrier, task))
+		    pnfs_roc_drain(inode, &calldata->roc_barrier, task)) {
+			nfs_release_seqid(calldata->arg.seqid);
 			goto out_wait;
+		    }
 	}
 
 	nfs_fattr_init(calldata->res.fattr);
@@ -2443,7 +2560,7 @@ static int nfs4_lookup_root_sec(struct nfs_server *server, struct nfs_fh *fhandl
 
 	auth = rpcauth_create(flavor, server->client);
 	if (IS_ERR(auth)) {
-		ret = -EIO;
+		ret = -EACCES;
 		goto out;
 	}
 	ret = nfs4_lookup_root(server, fhandle, info);
@@ -2451,27 +2568,36 @@ out:
 	return ret;
 }
 
+/*
+ * Retry pseudoroot lookup with various security flavors.  We do this when:
+ *
+ *   NFSv4.0: the PUTROOTFH operation returns NFS4ERR_WRONGSEC
+ *   NFSv4.1: the server does not support the SECINFO_NO_NAME operation
+ *
+ * Returns zero on success, or a negative NFS4ERR value, or a
+ * negative errno value.
+ */
 static int nfs4_find_root_sec(struct nfs_server *server, struct nfs_fh *fhandle,
 			      struct nfs_fsinfo *info)
 {
-	int i, len, status = 0;
-	rpc_authflavor_t flav_array[NFS_MAX_SECFLAVORS];
-
-	len = rpcauth_list_flavors(flav_array, ARRAY_SIZE(flav_array));
-	if (len < 0)
-		return len;
-
-	for (i = 0; i < len; i++) {
-		/* AUTH_UNIX is the default flavor if none was specified,
-		 * thus has already been tried. */
-		if (flav_array[i] == RPC_AUTH_UNIX)
-			continue;
+	/* Per 3530bis 15.33.5 */
+	static const rpc_authflavor_t flav_array[] = {
+		RPC_AUTH_GSS_KRB5P,
+		RPC_AUTH_GSS_KRB5I,
+		RPC_AUTH_GSS_KRB5,
+		RPC_AUTH_UNIX,			/* courtesy */
+		RPC_AUTH_NULL,
+	};
+	int status = -EPERM;
+	size_t i;
 
+	for (i = 0; i < ARRAY_SIZE(flav_array); i++) {
 		status = nfs4_lookup_root_sec(server, fhandle, info, flav_array[i]);
 		if (status == -NFS4ERR_WRONGSEC || status == -EACCES)
 			continue;
 		break;
 	}
+
 	/*
 	 * -EACCESS could mean that the user doesn't have correct permissions
 	 * to access the mount.  It could also mean that we tried to mount
@@ -2484,24 +2610,36 @@ static int nfs4_find_root_sec(struct nfs_server *server, struct nfs_fh *fhandle,
 	return status;
 }
 
-/*
- * get the file handle for the "/" directory on the server
+static int nfs4_do_find_root_sec(struct nfs_server *server,
+		struct nfs_fh *fhandle, struct nfs_fsinfo *info)
+{
+	int mv = server->nfs_client->cl_minorversion;
+	return nfs_v4_minor_ops[mv]->find_root_sec(server, fhandle, info);
+}
+
+/**
+ * nfs4_proc_get_rootfh - get file handle for server's pseudoroot
+ * @server: initialized nfs_server handle
+ * @fhandle: we fill in the pseudo-fs root file handle
+ * @info: we fill in an FSINFO struct
+ *
+ * Returns zero on success, or a negative errno.
  */
 int nfs4_proc_get_rootfh(struct nfs_server *server, struct nfs_fh *fhandle,
 			 struct nfs_fsinfo *info)
 {
-	int minor_version = server->nfs_client->cl_minorversion;
-	int status = nfs4_lookup_root(server, fhandle, info);
-	if ((status == -NFS4ERR_WRONGSEC) && !(server->flags & NFS_MOUNT_SECFLAVOUR))
-		/*
-		 * A status of -NFS4ERR_WRONGSEC will be mapped to -EPERM
-		 * by nfs4_map_errors() as this function exits.
-		 */
-		status = nfs_v4_minor_ops[minor_version]->find_root_sec(server, fhandle, info);
+	int status;
+
+	status = nfs4_lookup_root(server, fhandle, info);
+	if ((status == -NFS4ERR_WRONGSEC) &&
+	    !(server->flags & NFS_MOUNT_SECFLAVOUR))
+		status = nfs4_do_find_root_sec(server, fhandle, info);
+
 	if (status == 0)
 		status = nfs4_server_capabilities(server, fhandle);
 	if (status == 0)
 		status = nfs4_do_fsinfo(server, fhandle, info);
+
 	return nfs4_map_errors(status);
 }
 
@@ -2632,7 +2770,7 @@ nfs4_proc_setattr(struct dentry *dentry, struct nfs_fattr *fattr,
 	int status;
 
 	if (pnfs_ld_layoutret_on_setattr(inode))
-		pnfs_return_layout(inode);
+		pnfs_commit_and_return_layout(inode);
 
 	nfs_fattr_init(fattr);
 	
@@ -3380,12 +3518,21 @@ static int _nfs4_do_fsinfo(struct nfs_server *server, struct nfs_fh *fhandle,
 static int nfs4_do_fsinfo(struct nfs_server *server, struct nfs_fh *fhandle, struct nfs_fsinfo *fsinfo)
 {
 	struct nfs4_exception exception = { };
+	unsigned long now = jiffies;
 	int err;
 
 	do {
-		err = nfs4_handle_exception(server,
-				_nfs4_do_fsinfo(server, fhandle, fsinfo),
-				&exception);
+		err = _nfs4_do_fsinfo(server, fhandle, fsinfo);
+		if (err == 0) {
+			struct nfs_client *clp = server->nfs_client;
+
+			spin_lock(&clp->cl_lock);
+			clp->cl_lease_time = fsinfo->lease_time * HZ;
+			clp->cl_last_renewal = now;
+			spin_unlock(&clp->cl_lock);
+			break;
+		}
+		err = nfs4_handle_exception(server, err, &exception);
 	} while (exception.retry);
 	return err;
 }
@@ -3445,6 +3592,46 @@ static int nfs4_proc_pathconf(struct nfs_server *server, struct nfs_fh *fhandle,
 	return err;
 }
 
+int nfs4_set_rw_stateid(nfs4_stateid *stateid,
+		const struct nfs_open_context *ctx,
+		const struct nfs_lock_context *l_ctx,
+		fmode_t fmode)
+{
+	const struct nfs_lockowner *lockowner = NULL;
+
+	if (l_ctx != NULL)
+		lockowner = &l_ctx->lockowner;
+	return nfs4_select_rw_stateid(stateid, ctx->state, fmode, lockowner);
+}
+EXPORT_SYMBOL_GPL(nfs4_set_rw_stateid);
+
+static bool nfs4_stateid_is_current(nfs4_stateid *stateid,
+		const struct nfs_open_context *ctx,
+		const struct nfs_lock_context *l_ctx,
+		fmode_t fmode)
+{
+	nfs4_stateid current_stateid;
+
+	if (nfs4_set_rw_stateid(&current_stateid, ctx, l_ctx, fmode))
+		return false;
+	return nfs4_stateid_match(stateid, &current_stateid);
+}
+
+static bool nfs4_error_stateid_expired(int err)
+{
+	switch (err) {
+	case -NFS4ERR_DELEG_REVOKED:
+	case -NFS4ERR_ADMIN_REVOKED:
+	case -NFS4ERR_BAD_STATEID:
+	case -NFS4ERR_STALE_STATEID:
+	case -NFS4ERR_OLD_STATEID:
+	case -NFS4ERR_OPENMODE:
+	case -NFS4ERR_EXPIRED:
+		return true;
+	}
+	return false;
+}
+
 void __nfs4_read_done_cb(struct nfs_read_data *data)
 {
 	nfs_invalidate_atime(data->header->inode);
@@ -3465,6 +3652,20 @@ static int nfs4_read_done_cb(struct rpc_task *task, struct nfs_read_data *data)
 	return 0;
 }
 
+static bool nfs4_read_stateid_changed(struct rpc_task *task,
+		struct nfs_readargs *args)
+{
+
+	if (!nfs4_error_stateid_expired(task->tk_status) ||
+		nfs4_stateid_is_current(&args->stateid,
+				args->context,
+				args->lock_context,
+				FMODE_READ))
+		return false;
+	rpc_restart_call_prepare(task);
+	return true;
+}
+
 static int nfs4_read_done(struct rpc_task *task, struct nfs_read_data *data)
 {
 
@@ -3472,7 +3673,8 @@ static int nfs4_read_done(struct rpc_task *task, struct nfs_read_data *data)
 
 	if (!nfs4_sequence_done(task, &data->res.seq_res))
 		return -EAGAIN;
-
+	if (nfs4_read_stateid_changed(task, &data->args))
+		return -EAGAIN;
 	return data->read_done_cb ? data->read_done_cb(task, data) :
 				    nfs4_read_done_cb(task, data);
 }
@@ -3487,10 +3689,13 @@ static void nfs4_proc_read_setup(struct nfs_read_data *data, struct rpc_message
 
 static void nfs4_proc_read_rpc_prepare(struct rpc_task *task, struct nfs_read_data *data)
 {
-	nfs4_setup_sequence(NFS_SERVER(data->header->inode),
+	if (nfs4_setup_sequence(NFS_SERVER(data->header->inode),
 			&data->args.seq_args,
 			&data->res.seq_res,
-			task);
+			task))
+		return;
+	nfs4_set_rw_stateid(&data->args.stateid, data->args.context,
+			data->args.lock_context, FMODE_READ);
 }
 
 static int nfs4_write_done_cb(struct rpc_task *task, struct nfs_write_data *data)
@@ -3508,10 +3713,26 @@ static int nfs4_write_done_cb(struct rpc_task *task, struct nfs_write_data *data
 	return 0;
 }
 
+static bool nfs4_write_stateid_changed(struct rpc_task *task,
+		struct nfs_writeargs *args)
+{
+
+	if (!nfs4_error_stateid_expired(task->tk_status) ||
+		nfs4_stateid_is_current(&args->stateid,
+				args->context,
+				args->lock_context,
+				FMODE_WRITE))
+		return false;
+	rpc_restart_call_prepare(task);
+	return true;
+}
+
 static int nfs4_write_done(struct rpc_task *task, struct nfs_write_data *data)
 {
 	if (!nfs4_sequence_done(task, &data->res.seq_res))
 		return -EAGAIN;
+	if (nfs4_write_stateid_changed(task, &data->args))
+		return -EAGAIN;
 	return data->write_done_cb ? data->write_done_cb(task, data) :
 		nfs4_write_done_cb(task, data);
 }
@@ -3551,10 +3772,13 @@ static void nfs4_proc_write_setup(struct nfs_write_data *data, struct rpc_messag
 
 static void nfs4_proc_write_rpc_prepare(struct rpc_task *task, struct nfs_write_data *data)
 {
-	nfs4_setup_sequence(NFS_SERVER(data->header->inode),
+	if (nfs4_setup_sequence(NFS_SERVER(data->header->inode),
 			&data->args.seq_args,
 			&data->res.seq_res,
-			task);
+			task))
+		return;
+	nfs4_set_rw_stateid(&data->args.stateid, data->args.context,
+			data->args.lock_context, FMODE_WRITE);
 }
 
 static void nfs4_proc_commit_rpc_prepare(struct rpc_task *task, struct nfs_commit_data *data)
@@ -3656,7 +3880,7 @@ static int nfs4_proc_async_renew(struct nfs_client *clp, struct rpc_cred *cred,
 		return -ENOMEM;
 	data->client = clp;
 	data->timestamp = jiffies;
-	return rpc_call_async(clp->cl_rpcclient, &msg, RPC_TASK_SOFT,
+	return rpc_call_async(clp->cl_rpcclient, &msg, RPC_TASK_TIMEOUT,
 			&nfs4_renew_ops, data);
 }
 
@@ -3670,7 +3894,7 @@ static int nfs4_proc_renew(struct nfs_client *clp, struct rpc_cred *cred)
 	unsigned long now = jiffies;
 	int status;
 
-	status = rpc_call_sync(clp->cl_rpcclient, &msg, 0);
+	status = rpc_call_sync(clp->cl_rpcclient, &msg, RPC_TASK_TIMEOUT);
 	if (status < 0)
 		return status;
 	do_renew_lease(clp, now);
@@ -3980,11 +4204,14 @@ nfs4_async_handle_error(struct rpc_task *task, const struct nfs_server *server,
 		case -NFS4ERR_OPENMODE:
 			if (state == NULL)
 				break;
-			nfs4_schedule_stateid_recovery(server, state);
+			if (nfs4_schedule_stateid_recovery(server, state) < 0)
+				goto stateid_invalid;
 			goto wait_on_recovery;
 		case -NFS4ERR_EXPIRED:
-			if (state != NULL)
-				nfs4_schedule_stateid_recovery(server, state);
+			if (state != NULL) {
+				if (nfs4_schedule_stateid_recovery(server, state) < 0)
+					goto stateid_invalid;
+			}
 		case -NFS4ERR_STALE_STATEID:
 		case -NFS4ERR_STALE_CLIENTID:
 			nfs4_schedule_lease_recovery(clp);
@@ -4016,6 +4243,9 @@ nfs4_async_handle_error(struct rpc_task *task, const struct nfs_server *server,
 	}
 	task->tk_status = nfs4_map_errors(task->tk_status);
 	return 0;
+stateid_invalid:
+	task->tk_status = -EIO;
+	return 0;
 wait_on_recovery:
 	rpc_sleep_on(&clp->cl_rpcwaitq, task, NULL);
 	if (test_bit(NFS4CLNT_MANAGER_RUNNING, &clp->cl_state) == 0)
@@ -4143,27 +4373,17 @@ int nfs4_proc_setclientid_confirm(struct nfs_client *clp,
 		struct nfs4_setclientid_res *arg,
 		struct rpc_cred *cred)
 {
-	struct nfs_fsinfo fsinfo;
 	struct rpc_message msg = {
 		.rpc_proc = &nfs4_procedures[NFSPROC4_CLNT_SETCLIENTID_CONFIRM],
 		.rpc_argp = arg,
-		.rpc_resp = &fsinfo,
 		.rpc_cred = cred,
 	};
-	unsigned long now;
 	int status;
 
 	dprintk("NFS call  setclientid_confirm auth=%s, (client ID %llx)\n",
 		clp->cl_rpcclient->cl_auth->au_ops->au_name,
 		clp->cl_clientid);
-	now = jiffies;
 	status = rpc_call_sync(clp->cl_rpcclient, &msg, RPC_TASK_TIMEOUT);
-	if (status == 0) {
-		spin_lock(&clp->cl_lock);
-		clp->cl_lease_time = fsinfo.lease_time * HZ;
-		clp->cl_last_renewal = now;
-		spin_unlock(&clp->cl_lock);
-	}
 	dprintk("NFS reply setclientid_confirm: %d\n", status);
 	return status;
 }
@@ -4627,17 +4847,23 @@ static void nfs4_lock_prepare(struct rpc_task *task, void *calldata)
 		if (nfs_wait_on_sequence(data->arg.open_seqid, task) != 0) {
 			goto out_release_lock_seqid;
 		}
-		data->arg.open_stateid = &state->stateid;
+		data->arg.open_stateid = &state->open_stateid;
 		data->arg.new_lock_owner = 1;
 		data->res.open_seqid = data->arg.open_seqid;
 	} else
 		data->arg.new_lock_owner = 0;
+	if (!nfs4_valid_open_stateid(state)) {
+		data->rpc_status = -EBADF;
+		task->tk_action = NULL;
+		goto out_release_open_seqid;
+	}
 	data->timestamp = jiffies;
 	if (nfs4_setup_sequence(data->server,
 				&data->arg.seq_args,
 				&data->res.seq_res,
 				task) == 0)
 		return;
+out_release_open_seqid:
 	nfs_release_seqid(data->arg.open_seqid);
 out_release_lock_seqid:
 	nfs_release_seqid(data->arg.lock_seqid);
@@ -4983,58 +5209,16 @@ nfs4_proc_lock(struct file *filp, int cmd, struct file_lock *request)
 	return status;
 }
 
-int nfs4_lock_delegation_recall(struct nfs4_state *state, struct file_lock *fl)
+int nfs4_lock_delegation_recall(struct file_lock *fl, struct nfs4_state *state, const nfs4_stateid *stateid)
 {
 	struct nfs_server *server = NFS_SERVER(state->inode);
-	struct nfs4_exception exception = { };
 	int err;
 
 	err = nfs4_set_lock_state(state, fl);
 	if (err != 0)
-		goto out;
-	do {
-		err = _nfs4_do_setlk(state, F_SETLK, fl, NFS_LOCK_NEW);
-		switch (err) {
-			default:
-				printk(KERN_ERR "NFS: %s: unhandled error "
-					"%d.\n", __func__, err);
-			case 0:
-			case -ESTALE:
-				goto out;
-			case -NFS4ERR_STALE_CLIENTID:
-			case -NFS4ERR_STALE_STATEID:
-				set_bit(NFS_DELEGATED_STATE, &state->flags);
-			case -NFS4ERR_EXPIRED:
-				nfs4_schedule_lease_recovery(server->nfs_client);
-				err = -EAGAIN;
-				goto out;
-			case -NFS4ERR_BADSESSION:
-			case -NFS4ERR_BADSLOT:
-			case -NFS4ERR_BAD_HIGH_SLOT:
-			case -NFS4ERR_CONN_NOT_BOUND_TO_SESSION:
-			case -NFS4ERR_DEADSESSION:
-				set_bit(NFS_DELEGATED_STATE, &state->flags);
-				nfs4_schedule_session_recovery(server->nfs_client->cl_session, err);
-				err = -EAGAIN;
-				goto out;
-			case -NFS4ERR_DELEG_REVOKED:
-			case -NFS4ERR_ADMIN_REVOKED:
-			case -NFS4ERR_BAD_STATEID:
-			case -NFS4ERR_OPENMODE:
-				nfs4_schedule_stateid_recovery(server, state);
-				err = 0;
-				goto out;
-			case -ENOMEM:
-			case -NFS4ERR_DENIED:
-				/* kill_proc(fl->fl_pid, SIGLOST, 1); */
-				err = 0;
-				goto out;
-		}
-		set_bit(NFS_DELEGATED_STATE, &state->flags);
-		err = nfs4_handle_exception(server, err, &exception);
-	} while (exception.retry);
-out:
-	return err;
+		return err;
+	err = _nfs4_do_setlk(state, F_SETLK, fl, NFS_LOCK_NEW);
+	return nfs4_handle_delegation_recall_error(server, state, stateid, err);
 }
 
 struct nfs_release_lockowner_data {
@@ -5848,7 +6032,7 @@ static struct rpc_task *_nfs41_proc_sequence(struct nfs_client *clp,
 		.rpc_client = clp->cl_rpcclient,
 		.rpc_message = &msg,
 		.callback_ops = &nfs41_sequence_ops,
-		.flags = RPC_TASK_ASYNC | RPC_TASK_SOFT,
+		.flags = RPC_TASK_ASYNC | RPC_TASK_TIMEOUT,
 	};
 
 	if (!atomic_inc_not_zero(&clp->cl_count))
@@ -6416,22 +6600,8 @@ nfs4_layoutcommit_done(struct rpc_task *task, void *calldata)
 static void nfs4_layoutcommit_release(void *calldata)
 {
 	struct nfs4_layoutcommit_data *data = calldata;
-	struct pnfs_layout_segment *lseg, *tmp;
-	unsigned long *bitlock = &NFS_I(data->args.inode)->flags;
 
 	pnfs_cleanup_layoutcommit(data);
-	/* Matched by references in pnfs_set_layoutcommit */
-	list_for_each_entry_safe(lseg, tmp, &data->lseg_list, pls_lc_list) {
-		list_del_init(&lseg->pls_lc_list);
-		if (test_and_clear_bit(NFS_LSEG_LAYOUTCOMMIT,
-				       &lseg->pls_flags))
-			pnfs_put_lseg(lseg);
-	}
-
-	clear_bit_unlock(NFS_INO_LAYOUTCOMMITTING, bitlock);
-	smp_mb__after_clear_bit();
-	wake_up_bit(bitlock, NFS_INO_LAYOUTCOMMITTING);
-
 	put_rpccred(data->cred);
 	kfree(data);
 }
@@ -6739,6 +6909,10 @@ static const struct nfs4_state_maintenance_ops nfs41_state_renewal_ops = {
 
 static const struct nfs4_minor_version_ops nfs_v4_0_minor_ops = {
 	.minor_version = 0,
+	.init_caps = NFS_CAP_READDIRPLUS
+		| NFS_CAP_ATOMIC_OPEN
+		| NFS_CAP_CHANGE_ATTR
+		| NFS_CAP_POSIX_LOCK,
 	.call_sync = _nfs4_call_sync,
 	.match_stateid = nfs4_match_stateid,
 	.find_root_sec = nfs4_find_root_sec,
@@ -6750,6 +6924,12 @@ static const struct nfs4_minor_version_ops nfs_v4_0_minor_ops = {
 #if defined(CONFIG_NFS_V4_1)
 static const struct nfs4_minor_version_ops nfs_v4_1_minor_ops = {
 	.minor_version = 1,
+	.init_caps = NFS_CAP_READDIRPLUS
+		| NFS_CAP_ATOMIC_OPEN
+		| NFS_CAP_CHANGE_ATTR
+		| NFS_CAP_POSIX_LOCK
+		| NFS_CAP_STATEID_NFSV41
+		| NFS_CAP_ATOMIC_OPEN_V1,
 	.call_sync = nfs4_call_sync_sequence,
 	.match_stateid = nfs41_match_stateid,
 	.find_root_sec = nfs41_find_root_sec,
diff --git a/fs/nfs/nfs4state.c b/fs/nfs/nfs4state.c
index 6ace365c633..0b32f9483b7 100644
--- a/fs/nfs/nfs4state.c
+++ b/fs/nfs/nfs4state.c
@@ -154,18 +154,6 @@ struct rpc_cred *nfs4_get_machine_cred_locked(struct nfs_client *clp)
 	return cred;
 }
 
-static void nfs4_clear_machine_cred(struct nfs_client *clp)
-{
-	struct rpc_cred *cred;
-
-	spin_lock(&clp->cl_lock);
-	cred = clp->cl_machine_cred;
-	clp->cl_machine_cred = NULL;
-	spin_unlock(&clp->cl_lock);
-	if (cred != NULL)
-		put_rpccred(cred);
-}
-
 static struct rpc_cred *
 nfs4_get_renew_cred_server_locked(struct nfs_server *server)
 {
@@ -699,6 +687,8 @@ __nfs4_find_state_byowner(struct inode *inode, struct nfs4_state_owner *owner)
 	list_for_each_entry(state, &nfsi->open_states, inode_states) {
 		if (state->owner != owner)
 			continue;
+		if (!nfs4_valid_open_stateid(state))
+			continue;
 		if (atomic_inc_not_zero(&state->count))
 			return state;
 	}
@@ -987,13 +977,14 @@ int nfs4_set_lock_state(struct nfs4_state *state, struct file_lock *fl)
 	return 0;
 }
 
-static bool nfs4_copy_lock_stateid(nfs4_stateid *dst, struct nfs4_state *state,
+static int nfs4_copy_lock_stateid(nfs4_stateid *dst,
+		struct nfs4_state *state,
 		const struct nfs_lockowner *lockowner)
 {
 	struct nfs4_lock_state *lsp;
 	fl_owner_t fl_owner;
 	pid_t fl_pid;
-	bool ret = false;
+	int ret = -ENOENT;
 
 
 	if (lockowner == NULL)
@@ -1008,7 +999,10 @@ static bool nfs4_copy_lock_stateid(nfs4_stateid *dst, struct nfs4_state *state,
 	lsp = __nfs4_find_lock_state(state, fl_owner, fl_pid, NFS4_ANY_LOCK_TYPE);
 	if (lsp != NULL && test_bit(NFS_LOCK_INITIALIZED, &lsp->ls_flags) != 0) {
 		nfs4_stateid_copy(dst, &lsp->ls_stateid);
-		ret = true;
+		ret = 0;
+		smp_rmb();
+		if (!list_empty(&lsp->ls_seqid.list))
+			ret = -EWOULDBLOCK;
 	}
 	spin_unlock(&state->state_lock);
 	nfs4_put_lock_state(lsp);
@@ -1016,28 +1010,44 @@ out:
 	return ret;
 }
 
-static void nfs4_copy_open_stateid(nfs4_stateid *dst, struct nfs4_state *state)
+static int nfs4_copy_open_stateid(nfs4_stateid *dst, struct nfs4_state *state)
 {
+	const nfs4_stateid *src;
+	int ret;
 	int seq;
 
 	do {
+		src = &zero_stateid;
 		seq = read_seqbegin(&state->seqlock);
-		nfs4_stateid_copy(dst, &state->stateid);
+		if (test_bit(NFS_OPEN_STATE, &state->flags))
+			src = &state->open_stateid;
+		nfs4_stateid_copy(dst, src);
+		ret = 0;
+		smp_rmb();
+		if (!list_empty(&state->owner->so_seqid.list))
+			ret = -EWOULDBLOCK;
 	} while (read_seqretry(&state->seqlock, seq));
+	return ret;
 }
 
 /*
  * Byte-range lock aware utility to initialize the stateid of read/write
  * requests.
  */
-void nfs4_select_rw_stateid(nfs4_stateid *dst, struct nfs4_state *state,
+int nfs4_select_rw_stateid(nfs4_stateid *dst, struct nfs4_state *state,
 		fmode_t fmode, const struct nfs_lockowner *lockowner)
 {
+	int ret = 0;
 	if (nfs4_copy_delegation_stateid(dst, state->inode, fmode))
-		return;
-	if (nfs4_copy_lock_stateid(dst, state, lockowner))
-		return;
-	nfs4_copy_open_stateid(dst, state);
+		goto out;
+	ret = nfs4_copy_lock_stateid(dst, state, lockowner);
+	if (ret != -ENOENT)
+		goto out;
+	ret = nfs4_copy_open_stateid(dst, state);
+out:
+	if (nfs_server_capable(state->inode, NFS_CAP_STATEID_NFSV41))
+		dst->seqid = 0;
+	return ret;
 }
 
 struct nfs_seqid *nfs_alloc_seqid(struct nfs_seqid_counter *counter, gfp_t gfp_mask)
@@ -1286,14 +1296,17 @@ static int nfs4_state_mark_reclaim_nograce(struct nfs_client *clp, struct nfs4_s
 	return 1;
 }
 
-void nfs4_schedule_stateid_recovery(const struct nfs_server *server, struct nfs4_state *state)
+int nfs4_schedule_stateid_recovery(const struct nfs_server *server, struct nfs4_state *state)
 {
 	struct nfs_client *clp = server->nfs_client;
 
+	if (!nfs4_valid_open_stateid(state))
+		return -EBADF;
 	nfs4_state_mark_reclaim_nograce(clp, state);
 	dprintk("%s: scheduling stateid recovery for server %s\n", __func__,
 			clp->cl_hostname);
 	nfs4_schedule_state_manager(clp);
+	return 0;
 }
 EXPORT_SYMBOL_GPL(nfs4_schedule_stateid_recovery);
 
@@ -1323,6 +1336,27 @@ void nfs_inode_find_state_and_recover(struct inode *inode,
 		nfs4_schedule_state_manager(clp);
 }
 
+static void nfs4_state_mark_open_context_bad(struct nfs4_state *state)
+{
+	struct inode *inode = state->inode;
+	struct nfs_inode *nfsi = NFS_I(inode);
+	struct nfs_open_context *ctx;
+
+	spin_lock(&inode->i_lock);
+	list_for_each_entry(ctx, &nfsi->open_files, list) {
+		if (ctx->state != state)
+			continue;
+		set_bit(NFS_CONTEXT_BAD, &ctx->flags);
+	}
+	spin_unlock(&inode->i_lock);
+}
+
+static void nfs4_state_mark_recovery_failed(struct nfs4_state *state, int error)
+{
+	set_bit(NFS_STATE_RECOVERY_FAILED, &state->flags);
+	nfs4_state_mark_open_context_bad(state);
+}
+
 
 static int nfs4_reclaim_locks(struct nfs4_state *state, const struct nfs4_state_recovery_ops *ops)
 {
@@ -1398,6 +1432,8 @@ restart:
 	list_for_each_entry(state, &sp->so_states, open_states) {
 		if (!test_and_clear_bit(ops->state_flag_bit, &state->flags))
 			continue;
+		if (!nfs4_valid_open_stateid(state))
+			continue;
 		if (state->state == 0)
 			continue;
 		atomic_inc(&state->count);
@@ -1430,11 +1466,10 @@ restart:
 				 * Open state on this file cannot be recovered
 				 * All we can do is revert to using the zero stateid.
 				 */
-				memset(&state->stateid, 0,
-					sizeof(state->stateid));
-				/* Mark the file as being 'closed' */
-				state->state = 0;
+				nfs4_state_mark_recovery_failed(state, status);
 				break;
+			case -EAGAIN:
+				ssleep(1);
 			case -NFS4ERR_ADMIN_REVOKED:
 			case -NFS4ERR_STALE_STATEID:
 			case -NFS4ERR_BAD_STATEID:
@@ -1696,6 +1731,10 @@ static int nfs4_check_lease(struct nfs_client *clp)
 	}
 	status = ops->renew_lease(clp, cred);
 	put_rpccred(cred);
+	if (status == -ETIMEDOUT) {
+		set_bit(NFS4CLNT_CHECK_LEASE, &clp->cl_state);
+		return 0;
+	}
 out:
 	return nfs4_recovery_handle_error(clp, status);
 }
@@ -1725,10 +1764,6 @@ static int nfs4_handle_reclaim_lease_error(struct nfs_client *clp, int status)
 		clear_bit(NFS4CLNT_LEASE_CONFIRM, &clp->cl_state);
 		return -EPERM;
 	case -EACCES:
-		if (clp->cl_machine_cred == NULL)
-			return -EACCES;
-		/* Handle case where the user hasn't set up machine creds */
-		nfs4_clear_machine_cred(clp);
 	case -NFS4ERR_DELAY:
 	case -ETIMEDOUT:
 	case -EAGAIN:
@@ -1823,31 +1858,18 @@ int nfs4_discover_server_trunking(struct nfs_client *clp,
 {
 	const struct nfs4_state_recovery_ops *ops =
 				clp->cl_mvops->reboot_recovery_ops;
-	rpc_authflavor_t *flavors, flav, save;
 	struct rpc_clnt *clnt;
 	struct rpc_cred *cred;
-	int i, len, status;
+	int i, status;
 
 	dprintk("NFS: %s: testing '%s'\n", __func__, clp->cl_hostname);
 
-	len = NFS_MAX_SECFLAVORS;
-	flavors = kcalloc(len, sizeof(*flavors), GFP_KERNEL);
-	if (flavors == NULL) {
-		status = -ENOMEM;
-		goto out;
-	}
-	len = rpcauth_list_flavors(flavors, len);
-	if (len < 0) {
-		status = len;
-		goto out_free;
-	}
 	clnt = clp->cl_rpcclient;
-	save = clnt->cl_auth->au_flavor;
 	i = 0;
 
 	mutex_lock(&nfs_clid_init_mutex);
-	status  = -ENOENT;
 again:
+	status  = -ENOENT;
 	cred = ops->get_clid_cred(clp);
 	if (cred == NULL)
 		goto out_unlock;
@@ -1857,12 +1879,6 @@ again:
 	switch (status) {
 	case 0:
 		break;
-
-	case -EACCES:
-		if (clp->cl_machine_cred == NULL)
-			break;
-		/* Handle case where the user hasn't set up machine creds */
-		nfs4_clear_machine_cred(clp);
 	case -NFS4ERR_DELAY:
 	case -ETIMEDOUT:
 	case -EAGAIN:
@@ -1871,22 +1887,23 @@ again:
 		dprintk("NFS: %s after status %d, retrying\n",
 			__func__, status);
 		goto again;
-
+	case -EACCES:
+		if (i++)
+			break;
 	case -NFS4ERR_CLID_INUSE:
 	case -NFS4ERR_WRONGSEC:
-		status = -EPERM;
-		if (i >= len)
-			break;
-
-		flav = flavors[i++];
-		if (flav == save)
-			flav = flavors[i++];
-		clnt = rpc_clone_client_set_auth(clnt, flav);
+		clnt = rpc_clone_client_set_auth(clnt, RPC_AUTH_UNIX);
 		if (IS_ERR(clnt)) {
 			status = PTR_ERR(clnt);
 			break;
 		}
-		clp->cl_rpcclient = clnt;
+		/* Note: this is safe because we haven't yet marked the
+		 * client as ready, so we are the only user of
+		 * clp->cl_rpcclient
+		 */
+		clnt = xchg(&clp->cl_rpcclient, clnt);
+		rpc_shutdown_client(clnt);
+		clnt = clp->cl_rpcclient;
 		goto again;
 
 	case -NFS4ERR_MINOR_VERS_MISMATCH:
@@ -1897,13 +1914,15 @@ again:
 	case -NFS4ERR_NOT_SAME: /* FixMe: implement recovery
 				 * in nfs4_exchange_id */
 		status = -EKEYEXPIRED;
+		break;
+	default:
+		pr_warn("NFS: %s unhandled error %d. Exiting with error EIO\n",
+				__func__, status);
+		status = -EIO;
 	}
 
 out_unlock:
 	mutex_unlock(&nfs_clid_init_mutex);
-out_free:
-	kfree(flavors);
-out:
 	dprintk("NFS: %s: status = %d\n", __func__, status);
 	return status;
 }
diff --git a/fs/nfs/nfs4super.c b/fs/nfs/nfs4super.c
index 569b166cc05..a5e1a3026d4 100644
--- a/fs/nfs/nfs4super.c
+++ b/fs/nfs/nfs4super.c
@@ -252,6 +252,8 @@ struct dentry *nfs4_try_mount(int flags, const char *dev_name,
 
 	dfprintk(MOUNT, "--> nfs4_try_mount()\n");
 
+	if (data->auth_flavors[0] == RPC_AUTH_MAXFLAVOR)
+		data->auth_flavors[0] = RPC_AUTH_UNIX;
 	export_path = data->nfs_server.export_path;
 	data->nfs_server.export_path = "/";
 	root_mnt = nfs_do_root_mount(&nfs4_remote_fs_type, flags, mount_info,
diff --git a/fs/nfs/nfs4xdr.c b/fs/nfs/nfs4xdr.c
index e3edda554ac..3c79c5878c6 100644
--- a/fs/nfs/nfs4xdr.c
+++ b/fs/nfs/nfs4xdr.c
@@ -530,14 +530,10 @@ static int nfs4_stat_to_errno(int);
 				decode_setclientid_maxsz)
 #define NFS4_enc_setclientid_confirm_sz \
 				(compound_encode_hdr_maxsz + \
-				encode_setclientid_confirm_maxsz + \
-				encode_putrootfh_maxsz + \
-				encode_fsinfo_maxsz)
+				encode_setclientid_confirm_maxsz)
 #define NFS4_dec_setclientid_confirm_sz \
 				(compound_decode_hdr_maxsz + \
-				decode_setclientid_confirm_maxsz + \
-				decode_putrootfh_maxsz + \
-				decode_fsinfo_maxsz)
+				decode_setclientid_confirm_maxsz)
 #define NFS4_enc_lock_sz        (compound_encode_hdr_maxsz + \
 				encode_sequence_maxsz + \
 				encode_putfh_maxsz + \
@@ -1058,8 +1054,7 @@ static void encode_attrs(struct xdr_stream *xdr, const struct iattr *iap, const
 	if (iap->ia_valid & ATTR_ATIME_SET) {
 		bmval1 |= FATTR4_WORD1_TIME_ACCESS_SET;
 		*p++ = cpu_to_be32(NFS4_SET_TO_CLIENT_TIME);
-		*p++ = cpu_to_be32(0);
-		*p++ = cpu_to_be32(iap->ia_atime.tv_sec);
+		p = xdr_encode_hyper(p, (s64)iap->ia_atime.tv_sec);
 		*p++ = cpu_to_be32(iap->ia_atime.tv_nsec);
 	}
 	else if (iap->ia_valid & ATTR_ATIME) {
@@ -1069,8 +1064,7 @@ static void encode_attrs(struct xdr_stream *xdr, const struct iattr *iap, const
 	if (iap->ia_valid & ATTR_MTIME_SET) {
 		bmval1 |= FATTR4_WORD1_TIME_MODIFY_SET;
 		*p++ = cpu_to_be32(NFS4_SET_TO_CLIENT_TIME);
-		*p++ = cpu_to_be32(0);
-		*p++ = cpu_to_be32(iap->ia_mtime.tv_sec);
+		p = xdr_encode_hyper(p, (s64)iap->ia_mtime.tv_sec);
 		*p++ = cpu_to_be32(iap->ia_mtime.tv_nsec);
 	}
 	else if (iap->ia_valid & ATTR_MTIME) {
@@ -1366,33 +1360,28 @@ static inline void encode_openhdr(struct xdr_stream *xdr, const struct nfs_opena
 
 static inline void encode_createmode(struct xdr_stream *xdr, const struct nfs_openargs *arg)
 {
+	struct iattr dummy;
 	__be32 *p;
-	struct nfs_client *clp;
 
 	p = reserve_space(xdr, 4);
-	switch(arg->open_flags & O_EXCL) {
-	case 0:
+	switch(arg->createmode) {
+	case NFS4_CREATE_UNCHECKED:
 		*p = cpu_to_be32(NFS4_CREATE_UNCHECKED);
 		encode_attrs(xdr, arg->u.attrs, arg->server);
 		break;
-	default:
-		clp = arg->server->nfs_client;
-		if (clp->cl_mvops->minor_version > 0) {
-			if (nfs4_has_persistent_session(clp)) {
-				*p = cpu_to_be32(NFS4_CREATE_GUARDED);
-				encode_attrs(xdr, arg->u.attrs, arg->server);
-			} else {
-				struct iattr dummy;
-
-				*p = cpu_to_be32(NFS4_CREATE_EXCLUSIVE4_1);
-				encode_nfs4_verifier(xdr, &arg->u.verifier);
-				dummy.ia_valid = 0;
-				encode_attrs(xdr, &dummy, arg->server);
-			}
-		} else {
-			*p = cpu_to_be32(NFS4_CREATE_EXCLUSIVE);
-			encode_nfs4_verifier(xdr, &arg->u.verifier);
-		}
+	case NFS4_CREATE_GUARDED:
+		*p = cpu_to_be32(NFS4_CREATE_GUARDED);
+		encode_attrs(xdr, arg->u.attrs, arg->server);
+		break;
+	case NFS4_CREATE_EXCLUSIVE:
+		*p = cpu_to_be32(NFS4_CREATE_EXCLUSIVE);
+		encode_nfs4_verifier(xdr, &arg->u.verifier);
+		break;
+	case NFS4_CREATE_EXCLUSIVE4_1:
+		*p = cpu_to_be32(NFS4_CREATE_EXCLUSIVE4_1);
+		encode_nfs4_verifier(xdr, &arg->u.verifier);
+		dummy.ia_valid = 0;
+		encode_attrs(xdr, &dummy, arg->server);
 	}
 }
 
@@ -1459,6 +1448,23 @@ static inline void encode_claim_delegate_cur(struct xdr_stream *xdr, const struc
 	encode_string(xdr, name->len, name->name);
 }
 
+static inline void encode_claim_fh(struct xdr_stream *xdr)
+{
+	__be32 *p;
+
+	p = reserve_space(xdr, 4);
+	*p = cpu_to_be32(NFS4_OPEN_CLAIM_FH);
+}
+
+static inline void encode_claim_delegate_cur_fh(struct xdr_stream *xdr, const nfs4_stateid *stateid)
+{
+	__be32 *p;
+
+	p = reserve_space(xdr, 4);
+	*p = cpu_to_be32(NFS4_OPEN_CLAIM_DELEG_CUR_FH);
+	encode_nfs4_stateid(xdr, stateid);
+}
+
 static void encode_open(struct xdr_stream *xdr, const struct nfs_openargs *arg, struct compound_hdr *hdr)
 {
 	encode_op_hdr(xdr, OP_OPEN, decode_open_maxsz, hdr);
@@ -1474,6 +1480,12 @@ static void encode_open(struct xdr_stream *xdr, const struct nfs_openargs *arg,
 	case NFS4_OPEN_CLAIM_DELEGATE_CUR:
 		encode_claim_delegate_cur(xdr, arg->name, &arg->u.delegation);
 		break;
+	case NFS4_OPEN_CLAIM_FH:
+		encode_claim_fh(xdr);
+		break;
+	case NFS4_OPEN_CLAIM_DELEG_CUR_FH:
+		encode_claim_delegate_cur_fh(xdr, &arg->u.delegation);
+		break;
 	default:
 		BUG();
 	}
@@ -1506,35 +1518,12 @@ static void encode_putrootfh(struct xdr_stream *xdr, struct compound_hdr *hdr)
 	encode_op_hdr(xdr, OP_PUTROOTFH, decode_putrootfh_maxsz, hdr);
 }
 
-static void encode_open_stateid(struct xdr_stream *xdr,
-		const struct nfs_open_context *ctx,
-		const struct nfs_lock_context *l_ctx,
-		fmode_t fmode,
-		int zero_seqid)
-{
-	nfs4_stateid stateid;
-
-	if (ctx->state != NULL) {
-		const struct nfs_lockowner *lockowner = NULL;
-
-		if (l_ctx != NULL)
-			lockowner = &l_ctx->lockowner;
-		nfs4_select_rw_stateid(&stateid, ctx->state,
-				fmode, lockowner);
-		if (zero_seqid)
-			stateid.seqid = 0;
-		encode_nfs4_stateid(xdr, &stateid);
-	} else
-		encode_nfs4_stateid(xdr, &zero_stateid);
-}
-
 static void encode_read(struct xdr_stream *xdr, const struct nfs_readargs *args, struct compound_hdr *hdr)
 {
 	__be32 *p;
 
 	encode_op_hdr(xdr, OP_READ, decode_read_maxsz, hdr);
-	encode_open_stateid(xdr, args->context, args->lock_context,
-			FMODE_READ, hdr->minorversion);
+	encode_nfs4_stateid(xdr, &args->stateid);
 
 	p = reserve_space(xdr, 12);
 	p = xdr_encode_hyper(p, args->offset);
@@ -1670,8 +1659,7 @@ static void encode_write(struct xdr_stream *xdr, const struct nfs_writeargs *arg
 	__be32 *p;
 
 	encode_op_hdr(xdr, OP_WRITE, decode_write_maxsz, hdr);
-	encode_open_stateid(xdr, args->context, args->lock_context,
-			FMODE_WRITE, hdr->minorversion);
+	encode_nfs4_stateid(xdr, &args->stateid);
 
 	p = reserve_space(xdr, 16);
 	p = xdr_encode_hyper(p, args->offset);
@@ -2609,12 +2597,9 @@ static void nfs4_xdr_enc_setclientid_confirm(struct rpc_rqst *req,
 	struct compound_hdr hdr = {
 		.nops	= 0,
 	};
-	const u32 lease_bitmap[3] = { FATTR4_WORD0_LEASE_TIME };
 
 	encode_compound_hdr(xdr, req, &hdr);
 	encode_setclientid_confirm(xdr, arg, &hdr);
-	encode_putrootfh(xdr, &hdr);
-	encode_fsinfo(xdr, lease_bitmap, &hdr);
 	encode_nops(&hdr);
 }
 
@@ -3497,8 +3482,11 @@ static int decode_pathname(struct xdr_stream *xdr, struct nfs4_pathname *path)
 	if (n == 0)
 		goto root_path;
 	dprintk("pathname4: ");
-	path->ncomponents = 0;
-	while (path->ncomponents < n) {
+	if (n > NFS4_PATHNAME_MAXCOMPONENTS) {
+		dprintk("cannot parse %d components in path\n", n);
+		goto out_eio;
+	}
+	for (path->ncomponents = 0; path->ncomponents < n; path->ncomponents++) {
 		struct nfs4_string *component = &path->components[path->ncomponents];
 		status = decode_opaque_inline(xdr, &component->len, &component->data);
 		if (unlikely(status != 0))
@@ -3507,12 +3495,6 @@ static int decode_pathname(struct xdr_stream *xdr, struct nfs4_pathname *path)
 			pr_cont("%s%.*s ",
 				(path->ncomponents != n ? "/ " : ""),
 				component->len, component->data);
-		if (path->ncomponents < NFS4_PATHNAME_MAXCOMPONENTS)
-			path->ncomponents++;
-		else {
-			dprintk("cannot parse %d components in path\n", n);
-			goto out_eio;
-		}
 	}
 out:
 	return status;
@@ -3557,27 +3539,23 @@ static int decode_attr_fs_locations(struct xdr_stream *xdr, uint32_t *bitmap, st
 	n = be32_to_cpup(p);
 	if (n <= 0)
 		goto out_eio;
-	res->nlocations = 0;
-	while (res->nlocations < n) {
+	for (res->nlocations = 0; res->nlocations < n; res->nlocations++) {
 		u32 m;
-		struct nfs4_fs_location *loc = &res->locations[res->nlocations];
+		struct nfs4_fs_location *loc;
 
+		if (res->nlocations == NFS4_FS_LOCATIONS_MAXENTRIES)
+			break;
+		loc = &res->locations[res->nlocations];
 		p = xdr_inline_decode(xdr, 4);
 		if (unlikely(!p))
 			goto out_overflow;
 		m = be32_to_cpup(p);
 
-		loc->nservers = 0;
 		dprintk("%s: servers:\n", __func__);
-		while (loc->nservers < m) {
-			struct nfs4_string *server = &loc->servers[loc->nservers];
-			status = decode_opaque_inline(xdr, &server->len, &server->data);
-			if (unlikely(status != 0))
-				goto out_eio;
-			dprintk("%s ", server->data);
-			if (loc->nservers < NFS4_FS_LOCATION_MAXSERVERS)
-				loc->nservers++;
-			else {
+		for (loc->nservers = 0; loc->nservers < m; loc->nservers++) {
+			struct nfs4_string *server;
+
+			if (loc->nservers == NFS4_FS_LOCATION_MAXSERVERS) {
 				unsigned int i;
 				dprintk("%s: using first %u of %u servers "
 					"returned for location %u\n",
@@ -3591,13 +3569,17 @@ static int decode_attr_fs_locations(struct xdr_stream *xdr, uint32_t *bitmap, st
 					if (unlikely(status != 0))
 						goto out_eio;
 				}
+				break;
 			}
+			server = &loc->servers[loc->nservers];
+			status = decode_opaque_inline(xdr, &server->len, &server->data);
+			if (unlikely(status != 0))
+				goto out_eio;
+			dprintk("%s ", server->data);
 		}
 		status = decode_pathname(xdr, &loc->rootpath);
 		if (unlikely(status != 0))
 			goto out_eio;
-		if (res->nlocations < NFS4_FS_LOCATIONS_MAXENTRIES)
-			res->nlocations++;
 	}
 	if (res->nlocations != 0)
 		status = NFS_ATTR_FATTR_V4_LOCATIONS;
@@ -5209,27 +5191,30 @@ static int decode_delegreturn(struct xdr_stream *xdr)
 	return decode_op_hdr(xdr, OP_DELEGRETURN);
 }
 
-static int decode_secinfo_gss(struct xdr_stream *xdr, struct nfs4_secinfo_flavor *flavor)
+static int decode_secinfo_gss(struct xdr_stream *xdr,
+			      struct nfs4_secinfo4 *flavor)
 {
+	u32 oid_len;
 	__be32 *p;
 
 	p = xdr_inline_decode(xdr, 4);
 	if (unlikely(!p))
 		goto out_overflow;
-	flavor->gss.sec_oid4.len = be32_to_cpup(p);
-	if (flavor->gss.sec_oid4.len > GSS_OID_MAX_LEN)
+	oid_len = be32_to_cpup(p);
+	if (oid_len > GSS_OID_MAX_LEN)
 		goto out_err;
 
-	p = xdr_inline_decode(xdr, flavor->gss.sec_oid4.len);
+	p = xdr_inline_decode(xdr, oid_len);
 	if (unlikely(!p))
 		goto out_overflow;
-	memcpy(flavor->gss.sec_oid4.data, p, flavor->gss.sec_oid4.len);
+	memcpy(flavor->flavor_info.oid.data, p, oid_len);
+	flavor->flavor_info.oid.len = oid_len;
 
 	p = xdr_inline_decode(xdr, 8);
 	if (unlikely(!p))
 		goto out_overflow;
-	flavor->gss.qop4 = be32_to_cpup(p++);
-	flavor->gss.service = be32_to_cpup(p);
+	flavor->flavor_info.qop = be32_to_cpup(p++);
+	flavor->flavor_info.service = be32_to_cpup(p);
 
 	return 0;
 
@@ -5242,10 +5227,10 @@ out_err:
 
 static int decode_secinfo_common(struct xdr_stream *xdr, struct nfs4_secinfo_res *res)
 {
-	struct nfs4_secinfo_flavor *sec_flavor;
+	struct nfs4_secinfo4 *sec_flavor;
+	unsigned int i, num_flavors;
 	int status;
 	__be32 *p;
-	int i, num_flavors;
 
 	p = xdr_inline_decode(xdr, 4);
 	if (unlikely(!p))
@@ -6648,8 +6633,7 @@ static int nfs4_xdr_dec_setclientid(struct rpc_rqst *req,
  * Decode SETCLIENTID_CONFIRM response
  */
 static int nfs4_xdr_dec_setclientid_confirm(struct rpc_rqst *req,
-					    struct xdr_stream *xdr,
-					    struct nfs_fsinfo *fsinfo)
+					    struct xdr_stream *xdr)
 {
 	struct compound_hdr hdr;
 	int status;
@@ -6657,10 +6641,6 @@ static int nfs4_xdr_dec_setclientid_confirm(struct rpc_rqst *req,
 	status = decode_compound_hdr(xdr, &hdr);
 	if (!status)
 		status = decode_setclientid_confirm(xdr);
-	if (!status)
-		status = decode_putrootfh(xdr);
-	if (!status)
-		status = decode_fsinfo(xdr, fsinfo);
 	return status;
 }
 
diff --git a/fs/nfs/objlayout/objio_osd.c b/fs/nfs/objlayout/objio_osd.c
index 88f9611a945..5457745dd4f 100644
--- a/fs/nfs/objlayout/objio_osd.c
+++ b/fs/nfs/objlayout/objio_osd.c
@@ -234,7 +234,7 @@ static int __alloc_objio_seg(unsigned numdevs, gfp_t gfp_flags,
 
 	lseg = kzalloc(lseg_size, gfp_flags);
 	if (unlikely(!lseg)) {
-		dprintk("%s: Faild allocation numdevs=%d size=%zd\n", __func__,
+		dprintk("%s: Failed allocation numdevs=%d size=%zd\n", __func__,
 			numdevs, lseg_size);
 		return -ENOMEM;
 	}
diff --git a/fs/nfs/objlayout/objlayout.h b/fs/nfs/objlayout/objlayout.h
index 880ba086be9..87aa1dec612 100644
--- a/fs/nfs/objlayout/objlayout.h
+++ b/fs/nfs/objlayout/objlayout.h
@@ -114,7 +114,7 @@ extern int objio_alloc_lseg(struct pnfs_layout_segment **outp,
 	gfp_t gfp_flags);
 extern void objio_free_lseg(struct pnfs_layout_segment *lseg);
 
-/* objio_free_result will free these @oir structs recieved from
+/* objio_free_result will free these @oir structs received from
  * objlayout_{read,write}_done
  */
 extern void objio_free_result(struct objlayout_io_res *oir);
diff --git a/fs/nfs/pagelist.c b/fs/nfs/pagelist.c
index e56e846e9d2..29cfb7ade12 100644
--- a/fs/nfs/pagelist.c
+++ b/fs/nfs/pagelist.c
@@ -84,6 +84,55 @@ nfs_page_free(struct nfs_page *p)
 	kmem_cache_free(nfs_page_cachep, p);
 }
 
+static void
+nfs_iocounter_inc(struct nfs_io_counter *c)
+{
+	atomic_inc(&c->io_count);
+}
+
+static void
+nfs_iocounter_dec(struct nfs_io_counter *c)
+{
+	if (atomic_dec_and_test(&c->io_count)) {
+		clear_bit(NFS_IO_INPROGRESS, &c->flags);
+		smp_mb__after_clear_bit();
+		wake_up_bit(&c->flags, NFS_IO_INPROGRESS);
+	}
+}
+
+static int
+__nfs_iocounter_wait(struct nfs_io_counter *c)
+{
+	wait_queue_head_t *wq = bit_waitqueue(&c->flags, NFS_IO_INPROGRESS);
+	DEFINE_WAIT_BIT(q, &c->flags, NFS_IO_INPROGRESS);
+	int ret = 0;
+
+	do {
+		prepare_to_wait(wq, &q.wait, TASK_KILLABLE);
+		set_bit(NFS_IO_INPROGRESS, &c->flags);
+		if (atomic_read(&c->io_count) == 0)
+			break;
+		ret = nfs_wait_bit_killable(&c->flags);
+	} while (atomic_read(&c->io_count) != 0);
+	finish_wait(wq, &q.wait);
+	return ret;
+}
+
+/**
+ * nfs_iocounter_wait - wait for i/o to complete
+ * @c: nfs_io_counter to use
+ *
+ * returns -ERESTARTSYS if interrupted by a fatal signal.
+ * Otherwise returns 0 once the io_count hits 0.
+ */
+int
+nfs_iocounter_wait(struct nfs_io_counter *c)
+{
+	if (atomic_read(&c->io_count) == 0)
+		return 0;
+	return __nfs_iocounter_wait(c);
+}
+
 /**
  * nfs_create_request - Create an NFS read/write request.
  * @ctx: open context to use
@@ -104,6 +153,8 @@ nfs_create_request(struct nfs_open_context *ctx, struct inode *inode,
 	struct nfs_page		*req;
 	struct nfs_lock_context *l_ctx;
 
+	if (test_bit(NFS_CONTEXT_BAD, &ctx->flags))
+		return ERR_PTR(-EBADF);
 	/* try to allocate the request struct */
 	req = nfs_page_alloc();
 	if (req == NULL)
@@ -116,6 +167,7 @@ nfs_create_request(struct nfs_open_context *ctx, struct inode *inode,
 		return ERR_CAST(l_ctx);
 	}
 	req->wb_lock_context = l_ctx;
+	nfs_iocounter_inc(&l_ctx->io_count);
 
 	/* Initialize the request struct. Initially, we assume a
 	 * long write-back delay. This will be adjusted in
@@ -175,6 +227,7 @@ static void nfs_clear_request(struct nfs_page *req)
 		req->wb_page = NULL;
 	}
 	if (l_ctx != NULL) {
+		nfs_iocounter_dec(&l_ctx->io_count);
 		nfs_put_lock_context(l_ctx);
 		req->wb_lock_context = NULL;
 	}
diff --git a/fs/nfs/pnfs.c b/fs/nfs/pnfs.c
index 48ac5aad625..c5bd758e563 100644
--- a/fs/nfs/pnfs.c
+++ b/fs/nfs/pnfs.c
@@ -417,6 +417,16 @@ should_free_lseg(struct pnfs_layout_range *lseg_range,
 	       lo_seg_intersecting(lseg_range, recall_range);
 }
 
+static bool pnfs_lseg_dec_and_remove_zero(struct pnfs_layout_segment *lseg,
+		struct list_head *tmp_list)
+{
+	if (!atomic_dec_and_test(&lseg->pls_refcount))
+		return false;
+	pnfs_layout_remove_lseg(lseg->pls_layout, lseg);
+	list_add(&lseg->pls_list, tmp_list);
+	return true;
+}
+
 /* Returns 1 if lseg is removed from list, 0 otherwise */
 static int mark_lseg_invalid(struct pnfs_layout_segment *lseg,
 			     struct list_head *tmp_list)
@@ -430,11 +440,8 @@ static int mark_lseg_invalid(struct pnfs_layout_segment *lseg,
 		 */
 		dprintk("%s: lseg %p ref %d\n", __func__, lseg,
 			atomic_read(&lseg->pls_refcount));
-		if (atomic_dec_and_test(&lseg->pls_refcount)) {
-			pnfs_layout_remove_lseg(lseg->pls_layout, lseg);
-			list_add(&lseg->pls_list, tmp_list);
+		if (pnfs_lseg_dec_and_remove_zero(lseg, tmp_list))
 			rv = 1;
-		}
 	}
 	return rv;
 }
@@ -711,6 +718,8 @@ pnfs_choose_layoutget_stateid(nfs4_stateid *dst, struct pnfs_layout_hdr *lo,
 	spin_lock(&lo->plh_inode->i_lock);
 	if (pnfs_layoutgets_blocked(lo, 1)) {
 		status = -EAGAIN;
+	} else if (!nfs4_valid_open_stateid(open_state)) {
+		status = -EBADF;
 	} else if (list_empty(&lo->plh_segs)) {
 		int seq;
 
@@ -777,6 +786,21 @@ send_layoutget(struct pnfs_layout_hdr *lo,
 	return lseg;
 }
 
+static void pnfs_clear_layoutcommit(struct inode *inode,
+		struct list_head *head)
+{
+	struct nfs_inode *nfsi = NFS_I(inode);
+	struct pnfs_layout_segment *lseg, *tmp;
+
+	if (!test_and_clear_bit(NFS_INO_LAYOUTCOMMIT, &nfsi->flags))
+		return;
+	list_for_each_entry_safe(lseg, tmp, &nfsi->layout->plh_segs, pls_list) {
+		if (!test_and_clear_bit(NFS_LSEG_LAYOUTCOMMIT, &lseg->pls_flags))
+			continue;
+		pnfs_lseg_dec_and_remove_zero(lseg, head);
+	}
+}
+
 /*
  * Initiates a LAYOUTRETURN(FILE), and removes the pnfs_layout_hdr
  * when the layout segment list is empty.
@@ -808,6 +832,7 @@ _pnfs_return_layout(struct inode *ino)
 	/* Reference matched in nfs4_layoutreturn_release */
 	pnfs_get_layout_hdr(lo);
 	empty = list_empty(&lo->plh_segs);
+	pnfs_clear_layoutcommit(ino, &tmp_list);
 	pnfs_mark_matching_lsegs_invalid(lo, &tmp_list, NULL);
 	/* Don't send a LAYOUTRETURN if list was initially empty */
 	if (empty) {
@@ -820,8 +845,6 @@ _pnfs_return_layout(struct inode *ino)
 	spin_unlock(&ino->i_lock);
 	pnfs_free_lseg_list(&tmp_list);
 
-	WARN_ON(test_bit(NFS_INO_LAYOUTCOMMIT, &nfsi->flags));
-
 	lrp = kzalloc(sizeof(*lrp), GFP_KERNEL);
 	if (unlikely(lrp == NULL)) {
 		status = -ENOMEM;
@@ -845,6 +868,33 @@ out:
 }
 EXPORT_SYMBOL_GPL(_pnfs_return_layout);
 
+int
+pnfs_commit_and_return_layout(struct inode *inode)
+{
+	struct pnfs_layout_hdr *lo;
+	int ret;
+
+	spin_lock(&inode->i_lock);
+	lo = NFS_I(inode)->layout;
+	if (lo == NULL) {
+		spin_unlock(&inode->i_lock);
+		return 0;
+	}
+	pnfs_get_layout_hdr(lo);
+	/* Block new layoutgets and read/write to ds */
+	lo->plh_block_lgets++;
+	spin_unlock(&inode->i_lock);
+	filemap_fdatawait(inode->i_mapping);
+	ret = pnfs_layoutcommit_inode(inode, true);
+	if (ret == 0)
+		ret = _pnfs_return_layout(inode);
+	spin_lock(&inode->i_lock);
+	lo->plh_block_lgets--;
+	spin_unlock(&inode->i_lock);
+	pnfs_put_layout_hdr(lo);
+	return ret;
+}
+
 bool pnfs_roc(struct inode *ino)
 {
 	struct pnfs_layout_hdr *lo;
@@ -1458,7 +1508,6 @@ static void pnfs_ld_handle_write_error(struct nfs_write_data *data)
 	dprintk("pnfs write error = %d\n", hdr->pnfs_error);
 	if (NFS_SERVER(hdr->inode)->pnfs_curr_ld->flags &
 	    PNFS_LAYOUTRET_ON_ERROR) {
-		clear_bit(NFS_INO_LAYOUTCOMMIT, &NFS_I(hdr->inode)->flags);
 		pnfs_return_layout(hdr->inode);
 	}
 	if (!test_and_set_bit(NFS_IOHDR_REDO, &hdr->flags))
@@ -1613,7 +1662,6 @@ static void pnfs_ld_handle_read_error(struct nfs_read_data *data)
 	dprintk("pnfs read error = %d\n", hdr->pnfs_error);
 	if (NFS_SERVER(hdr->inode)->pnfs_curr_ld->flags &
 	    PNFS_LAYOUTRET_ON_ERROR) {
-		clear_bit(NFS_INO_LAYOUTCOMMIT, &NFS_I(hdr->inode)->flags);
 		pnfs_return_layout(hdr->inode);
 	}
 	if (!test_and_set_bit(NFS_IOHDR_REDO, &hdr->flags))
@@ -1746,11 +1794,27 @@ static void pnfs_list_write_lseg(struct inode *inode, struct list_head *listp)
 
 	list_for_each_entry(lseg, &NFS_I(inode)->layout->plh_segs, pls_list) {
 		if (lseg->pls_range.iomode == IOMODE_RW &&
-		    test_bit(NFS_LSEG_LAYOUTCOMMIT, &lseg->pls_flags))
+		    test_and_clear_bit(NFS_LSEG_LAYOUTCOMMIT, &lseg->pls_flags))
 			list_add(&lseg->pls_lc_list, listp);
 	}
 }
 
+static void pnfs_list_write_lseg_done(struct inode *inode, struct list_head *listp)
+{
+	struct pnfs_layout_segment *lseg, *tmp;
+	unsigned long *bitlock = &NFS_I(inode)->flags;
+
+	/* Matched by references in pnfs_set_layoutcommit */
+	list_for_each_entry_safe(lseg, tmp, listp, pls_lc_list) {
+		list_del_init(&lseg->pls_lc_list);
+		pnfs_put_lseg(lseg);
+	}
+
+	clear_bit_unlock(NFS_INO_LAYOUTCOMMITTING, bitlock);
+	smp_mb__after_clear_bit();
+	wake_up_bit(bitlock, NFS_INO_LAYOUTCOMMITTING);
+}
+
 void pnfs_set_lo_fail(struct pnfs_layout_segment *lseg)
 {
 	pnfs_layout_io_set_failed(lseg->pls_layout, lseg->pls_range.iomode);
@@ -1795,6 +1859,7 @@ void pnfs_cleanup_layoutcommit(struct nfs4_layoutcommit_data *data)
 
 	if (nfss->pnfs_curr_ld->cleanup_layoutcommit)
 		nfss->pnfs_curr_ld->cleanup_layoutcommit(data);
+	pnfs_list_write_lseg_done(data->args.inode, &data->lseg_list);
 }
 
 /*
diff --git a/fs/nfs/pnfs.h b/fs/nfs/pnfs.h
index 94ba8041774..f5f8a470a64 100644
--- a/fs/nfs/pnfs.h
+++ b/fs/nfs/pnfs.h
@@ -219,6 +219,7 @@ void pnfs_set_layoutcommit(struct nfs_write_data *wdata);
 void pnfs_cleanup_layoutcommit(struct nfs4_layoutcommit_data *data);
 int pnfs_layoutcommit_inode(struct inode *inode, bool sync);
 int _pnfs_return_layout(struct inode *);
+int pnfs_commit_and_return_layout(struct inode *);
 void pnfs_ld_write_done(struct nfs_write_data *);
 void pnfs_ld_read_done(struct nfs_read_data *);
 struct pnfs_layout_segment *pnfs_update_layout(struct inode *ino,
@@ -407,6 +408,11 @@ static inline int pnfs_return_layout(struct inode *ino)
 	return 0;
 }
 
+static inline int pnfs_commit_and_return_layout(struct inode *inode)
+{
+	return 0;
+}
+
 static inline bool
 pnfs_ld_layoutret_on_setattr(struct inode *inode)
 {
diff --git a/fs/nfs/read.c b/fs/nfs/read.c
index a5e5d9899d5..70a26c651f0 100644
--- a/fs/nfs/read.c
+++ b/fs/nfs/read.c
@@ -514,6 +514,8 @@ void nfs_read_prepare(struct rpc_task *task, void *calldata)
 {
 	struct nfs_read_data *data = calldata;
 	NFS_PROTO(data->header->inode)->read_rpc_prepare(task, data);
+	if (unlikely(test_bit(NFS_CONTEXT_BAD, &data->args.context->flags)))
+		rpc_exit(task, -EIO);
 }
 
 static const struct rpc_call_ops nfs_read_common_ops = {
diff --git a/fs/nfs/super.c b/fs/nfs/super.c
index 2f8a29db0f1..eb494f6a4c6 100644
--- a/fs/nfs/super.c
+++ b/fs/nfs/super.c
@@ -920,7 +920,7 @@ static struct nfs_parsed_mount_data *nfs_alloc_parsed_mount_data(void)
 		data->mount_server.port	= NFS_UNSPEC_PORT;
 		data->nfs_server.port	= NFS_UNSPEC_PORT;
 		data->nfs_server.protocol = XPRT_TRANSPORT_TCP;
-		data->auth_flavors[0]	= RPC_AUTH_UNIX;
+		data->auth_flavors[0]	= RPC_AUTH_MAXFLAVOR;
 		data->auth_flavor_len	= 1;
 		data->minorversion	= 0;
 		data->need_mount	= true;
@@ -1608,49 +1608,57 @@ out_security_failure:
 }
 
 /*
- * Match the requested auth flavors with the list returned by
- * the server.  Returns zero and sets the mount's authentication
- * flavor on success; returns -EACCES if server does not support
- * the requested flavor.
+ * Select a security flavor for this mount.  The selected flavor
+ * is planted in args->auth_flavors[0].
  */
-static int nfs_walk_authlist(struct nfs_parsed_mount_data *args,
-			     struct nfs_mount_request *request)
+static void nfs_select_flavor(struct nfs_parsed_mount_data *args,
+			      struct nfs_mount_request *request)
 {
-	unsigned int i, j, server_authlist_len = *(request->auth_flav_len);
+	unsigned int i, count = *(request->auth_flav_len);
+	rpc_authflavor_t flavor;
+
+	if (args->auth_flavors[0] != RPC_AUTH_MAXFLAVOR)
+		goto out;
+
+	/*
+	 * The NFSv2 MNT operation does not return a flavor list.
+	 */
+	if (args->mount_server.version != NFS_MNT3_VERSION)
+		goto out_default;
 
 	/*
 	 * Certain releases of Linux's mountd return an empty
-	 * flavor list.  To prevent behavioral regression with
-	 * these servers (ie. rejecting mounts that used to
-	 * succeed), revert to pre-2.6.32 behavior (no checking)
-	 * if the returned flavor list is empty.
+	 * flavor list in some cases.
 	 */
-	if (server_authlist_len == 0)
-		return 0;
+	if (count == 0)
+		goto out_default;
 
 	/*
-	 * We avoid sophisticated negotiating here, as there are
-	 * plenty of cases where we can get it wrong, providing
-	 * either too little or too much security.
-	 *
 	 * RFC 2623, section 2.7 suggests we SHOULD prefer the
 	 * flavor listed first.  However, some servers list
-	 * AUTH_NULL first.  Our caller plants AUTH_SYS, the
-	 * preferred default, in args->auth_flavors[0] if user
-	 * didn't specify sec= mount option.
+	 * AUTH_NULL first.  Avoid ever choosing AUTH_NULL.
 	 */
-	for (i = 0; i < args->auth_flavor_len; i++)
-		for (j = 0; j < server_authlist_len; j++)
-			if (args->auth_flavors[i] == request->auth_flavs[j]) {
-				dfprintk(MOUNT, "NFS: using auth flavor %d\n",
-					request->auth_flavs[j]);
-				args->auth_flavors[0] = request->auth_flavs[j];
-				return 0;
-			}
+	for (i = 0; i < count; i++) {
+		struct rpcsec_gss_info info;
+
+		flavor = request->auth_flavs[i];
+		switch (flavor) {
+		case RPC_AUTH_UNIX:
+			goto out_set;
+		case RPC_AUTH_NULL:
+			continue;
+		default:
+			if (rpcauth_get_gssinfo(flavor, &info) == 0)
+				goto out_set;
+		}
+	}
 
-	dfprintk(MOUNT, "NFS: server does not support requested auth flavor\n");
-	nfs_umount(request);
-	return -EACCES;
+out_default:
+	flavor = RPC_AUTH_UNIX;
+out_set:
+	args->auth_flavors[0] = flavor;
+out:
+	dfprintk(MOUNT, "NFS: using auth flavor %d\n", args->auth_flavors[0]);
 }
 
 /*
@@ -1713,12 +1721,8 @@ static int nfs_request_mount(struct nfs_parsed_mount_data *args,
 		return status;
 	}
 
-	/*
-	 * MNTv1 (NFSv2) does not support auth flavor negotiation.
-	 */
-	if (args->mount_server.version != NFS_MNT3_VERSION)
-		return 0;
-	return nfs_walk_authlist(args, &request);
+	nfs_select_flavor(args, &request);
+	return 0;
 }
 
 struct dentry *nfs_try_mount(int flags, const char *dev_name,
diff --git a/fs/nfs/write.c b/fs/nfs/write.c
index c483cc50b82..a2c7c28049d 100644
--- a/fs/nfs/write.c
+++ b/fs/nfs/write.c
@@ -1251,6 +1251,8 @@ void nfs_write_prepare(struct rpc_task *task, void *calldata)
 {
 	struct nfs_write_data *data = calldata;
 	NFS_PROTO(data->header->inode)->write_rpc_prepare(task, data);
+	if (unlikely(test_bit(NFS_CONTEXT_BAD, &data->args.context->flags)))
+		rpc_exit(task, -EIO);
 }
 
 void nfs_commit_prepare(struct rpc_task *task, void *calldata)
diff --git a/fs/nfsd/nfs4state.c b/fs/nfsd/nfs4state.c
index 2e27430b907..417c8487774 100644
--- a/fs/nfsd/nfs4state.c
+++ b/fs/nfsd/nfs4state.c
@@ -234,7 +234,6 @@ static struct nfs4_stid *nfs4_alloc_stid(struct nfs4_client *cl, struct
 kmem_cache *slab)
 {
 	struct idr *stateids = &cl->cl_stateids;
-	static int min_stateid = 0;
 	struct nfs4_stid *stid;
 	int new_id;
 
@@ -242,7 +241,7 @@ kmem_cache *slab)
 	if (!stid)
 		return NULL;
 
-	new_id = idr_alloc(stateids, stid, min_stateid, 0, GFP_KERNEL);
+	new_id = idr_alloc_cyclic(stateids, stid, 0, 0, GFP_KERNEL);
 	if (new_id < 0)
 		goto out_free;
 	stid->sc_client = cl;
@@ -261,10 +260,6 @@ kmem_cache *slab)
 	 * amount of time until an id is reused, by ensuring they always
 	 * "increase" (mod INT_MAX):
 	 */
-
-	min_stateid = new_id+1;
-	if (min_stateid == INT_MAX)
-		min_stateid = 0;
 	return stid;
 out_free:
 	kfree(stid);
diff --git a/fs/nfsd/nfs4xdr.c b/fs/nfsd/nfs4xdr.c
index 01168865dd3..2502951714b 100644
--- a/fs/nfsd/nfs4xdr.c
+++ b/fs/nfsd/nfs4xdr.c
@@ -264,7 +264,7 @@ nfsd4_decode_fattr(struct nfsd4_compoundargs *argp, u32 *bmval,
 		iattr->ia_valid |= ATTR_SIZE;
 	}
 	if (bmval[0] & FATTR4_WORD0_ACL) {
-		int nace;
+		u32 nace;
 		struct nfs4_ace *ace;
 
 		READ_BUF(4); len += 4;
@@ -3138,10 +3138,9 @@ nfsd4_encode_rename(struct nfsd4_compoundres *resp, __be32 nfserr, struct nfsd4_
 
 static __be32
 nfsd4_do_encode_secinfo(struct nfsd4_compoundres *resp,
-			 __be32 nfserr,struct svc_export *exp)
+			 __be32 nfserr, struct svc_export *exp)
 {
-	int i = 0;
-	u32 nflavs;
+	u32 i, nflavs;
 	struct exp_flavor_info *flavs;
 	struct exp_flavor_info def_flavs[2];
 	__be32 *p;
@@ -3172,30 +3171,29 @@ nfsd4_do_encode_secinfo(struct nfsd4_compoundres *resp,
 	WRITE32(nflavs);
 	ADJUST_ARGS();
 	for (i = 0; i < nflavs; i++) {
-		u32 flav = flavs[i].pseudoflavor;
-		struct gss_api_mech *gm = gss_mech_get_by_pseudoflavor(flav);
+		struct rpcsec_gss_info info;
 
-		if (gm) {
+		if (rpcauth_get_gssinfo(flavs[i].pseudoflavor, &info) == 0) {
 			RESERVE_SPACE(4);
 			WRITE32(RPC_AUTH_GSS);
 			ADJUST_ARGS();
-			RESERVE_SPACE(4 + gm->gm_oid.len);
-			WRITE32(gm->gm_oid.len);
-			WRITEMEM(gm->gm_oid.data, gm->gm_oid.len);
+			RESERVE_SPACE(4 + info.oid.len);
+			WRITE32(info.oid.len);
+			WRITEMEM(info.oid.data, info.oid.len);
 			ADJUST_ARGS();
 			RESERVE_SPACE(4);
-			WRITE32(0); /* qop */
+			WRITE32(info.qop);
 			ADJUST_ARGS();
 			RESERVE_SPACE(4);
-			WRITE32(gss_pseudoflavor_to_service(gm, flav));
+			WRITE32(info.service);
 			ADJUST_ARGS();
-			gss_mech_put(gm);
 		} else {
 			RESERVE_SPACE(4);
-			WRITE32(flav);
+			WRITE32(flavs[i].pseudoflavor);
 			ADJUST_ARGS();
 		}
 	}
+
 out:
 	if (exp)
 		exp_put(exp);
diff --git a/fs/nfsd/nfscache.c b/fs/nfsd/nfscache.c
index 62c1ee128ae..ca05f6dc354 100644
--- a/fs/nfsd/nfscache.c
+++ b/fs/nfsd/nfscache.c
@@ -102,7 +102,8 @@ nfsd_reply_cache_free_locked(struct svc_cacherep *rp)
 {
 	if (rp->c_type == RC_REPLBUFF)
 		kfree(rp->c_replvec.iov_base);
-	hlist_del(&rp->c_hash);
+	if (!hlist_unhashed(&rp->c_hash))
+		hlist_del(&rp->c_hash);
 	list_del(&rp->c_lru);
 	--num_drc_entries;
 	kmem_cache_free(drc_slab, rp);
@@ -118,6 +119,10 @@ nfsd_reply_cache_free(struct svc_cacherep *rp)
 
 int nfsd_reply_cache_init(void)
 {
+	INIT_LIST_HEAD(&lru_head);
+	max_drc_entries = nfsd_cache_size_limit();
+	num_drc_entries = 0;
+
 	register_shrinker(&nfsd_reply_cache_shrinker);
 	drc_slab = kmem_cache_create("nfsd_drc", sizeof(struct svc_cacherep),
 					0, 0, NULL);
@@ -128,10 +133,6 @@ int nfsd_reply_cache_init(void)
 	if (!cache_hash)
 		goto out_nomem;
 
-	INIT_LIST_HEAD(&lru_head);
-	max_drc_entries = nfsd_cache_size_limit();
-	num_drc_entries = 0;
-
 	return 0;
 out_nomem:
 	printk(KERN_ERR "nfsd: failed to allocate reply cache\n");
diff --git a/fs/nfsd/vfs.c b/fs/nfsd/vfs.c
index 2a7eb536de0..2b2e2396a86 100644
--- a/fs/nfsd/vfs.c
+++ b/fs/nfsd/vfs.c
@@ -1013,6 +1013,7 @@ nfsd_vfs_write(struct svc_rqst *rqstp, struct svc_fh *fhp, struct file *file,
 	int			host_err;
 	int			stable = *stablep;
 	int			use_wgather;
+	loff_t			pos = offset;
 
 	dentry = file->f_path.dentry;
 	inode = dentry->d_inode;
@@ -1025,7 +1026,7 @@ nfsd_vfs_write(struct svc_rqst *rqstp, struct svc_fh *fhp, struct file *file,
 
 	/* Write the data. */
 	oldfs = get_fs(); set_fs(KERNEL_DS);
-	host_err = vfs_writev(file, (struct iovec __user *)vec, vlen, &offset);
+	host_err = vfs_writev(file, (struct iovec __user *)vec, vlen, &pos);
 	set_fs(oldfs);
 	if (host_err < 0)
 		goto out_nfserr;
diff --git a/fs/notify/inotify/inotify_user.c b/fs/notify/inotify/inotify_user.c
index e0f7c1241a6..8562bd3af94 100644
--- a/fs/notify/inotify/inotify_user.c
+++ b/fs/notify/inotify/inotify_user.c
@@ -359,7 +359,6 @@ static int inotify_find_inode(const char __user *dirname, struct path *path, uns
 }
 
 static int inotify_add_to_idr(struct idr *idr, spinlock_t *idr_lock,
-			      int *last_wd,
 			      struct inotify_inode_mark *i_mark)
 {
 	int ret;
@@ -367,11 +366,10 @@ static int inotify_add_to_idr(struct idr *idr, spinlock_t *idr_lock,
 	idr_preload(GFP_KERNEL);
 	spin_lock(idr_lock);
 
-	ret = idr_alloc(idr, i_mark, *last_wd + 1, 0, GFP_NOWAIT);
+	ret = idr_alloc_cyclic(idr, i_mark, 1, 0, GFP_NOWAIT);
 	if (ret >= 0) {
 		/* we added the mark to the idr, take a reference */
 		i_mark->wd = ret;
-		*last_wd = i_mark->wd;
 		fsnotify_get_mark(&i_mark->fsn_mark);
 	}
 
@@ -638,8 +636,7 @@ static int inotify_new_watch(struct fsnotify_group *group,
 	if (atomic_read(&group->inotify_data.user->inotify_watches) >= inotify_max_user_watches)
 		goto out_err;
 
-	ret = inotify_add_to_idr(idr, idr_lock, &group->inotify_data.last_wd,
-				 tmp_i_mark);
+	ret = inotify_add_to_idr(idr, idr_lock, tmp_i_mark);
 	if (ret)
 		goto out_err;
 
@@ -697,7 +694,6 @@ static struct fsnotify_group *inotify_new_group(unsigned int max_events)
 
 	spin_lock_init(&group->inotify_data.idr_lock);
 	idr_init(&group->inotify_data.idr);
-	group->inotify_data.last_wd = 0;
 	group->inotify_data.user = get_current_user();
 
 	if (atomic_inc_return(&group->inotify_data.user->inotify_devs) >
diff --git a/fs/ocfs2/dlm/dlmrecovery.c b/fs/ocfs2/dlm/dlmrecovery.c
index eeac97bb3bf..b3fdd1a323d 100644
--- a/fs/ocfs2/dlm/dlmrecovery.c
+++ b/fs/ocfs2/dlm/dlmrecovery.c
@@ -1498,10 +1498,8 @@ leave:
 
 	dlm_put(dlm);
 	if (ret < 0) {
-		if (buf)
-			kfree(buf);
-		if (item)
-			kfree(item);
+		kfree(buf);
+		kfree(item);
 		mlog_errno(ret);
 	}
 
diff --git a/fs/ocfs2/ioctl.c b/fs/ocfs2/ioctl.c
index 752f0b26221..0c60ef2d805 100644
--- a/fs/ocfs2/ioctl.c
+++ b/fs/ocfs2/ioctl.c
@@ -101,13 +101,6 @@ static int ocfs2_set_inode_attr(struct inode *inode, unsigned flags,
 	if (!S_ISDIR(inode->i_mode))
 		flags &= ~OCFS2_DIRSYNC_FL;
 
-	handle = ocfs2_start_trans(osb, OCFS2_INODE_UPDATE_CREDITS);
-	if (IS_ERR(handle)) {
-		status = PTR_ERR(handle);
-		mlog_errno(status);
-		goto bail_unlock;
-	}
-
 	oldflags = ocfs2_inode->ip_attr;
 	flags = flags & mask;
 	flags |= oldflags & ~mask;
@@ -120,7 +113,14 @@ static int ocfs2_set_inode_attr(struct inode *inode, unsigned flags,
 	if ((oldflags & OCFS2_IMMUTABLE_FL) || ((flags ^ oldflags) &
 		(OCFS2_APPEND_FL | OCFS2_IMMUTABLE_FL))) {
 		if (!capable(CAP_LINUX_IMMUTABLE))
-			goto bail_commit;
+			goto bail_unlock;
+	}
+
+	handle = ocfs2_start_trans(osb, OCFS2_INODE_UPDATE_CREDITS);
+	if (IS_ERR(handle)) {
+		status = PTR_ERR(handle);
+		mlog_errno(status);
+		goto bail_unlock;
 	}
 
 	ocfs2_inode->ip_attr = flags;
@@ -130,8 +130,8 @@ static int ocfs2_set_inode_attr(struct inode *inode, unsigned flags,
 	if (status < 0)
 		mlog_errno(status);
 
-bail_commit:
 	ocfs2_commit_trans(osb, handle);
+
 bail_unlock:
 	ocfs2_inode_unlock(inode, 1);
 bail:
@@ -706,8 +706,10 @@ int ocfs2_info_handle_freefrag(struct inode *inode,
 
 	o2info_set_request_filled(&oiff->iff_req);
 
-	if (o2info_to_user(*oiff, req))
+	if (o2info_to_user(*oiff, req)) {
+		status = -EFAULT;
 		goto bail;
+	}
 
 	status = 0;
 bail:
diff --git a/fs/ocfs2/move_extents.c b/fs/ocfs2/move_extents.c
index 9f8dcadd9a5..f1fc172175b 100644
--- a/fs/ocfs2/move_extents.c
+++ b/fs/ocfs2/move_extents.c
@@ -471,7 +471,7 @@ static int ocfs2_validate_and_adjust_move_goal(struct inode *inode,
 	int ret, goal_bit = 0;
 
 	struct buffer_head *gd_bh = NULL;
-	struct ocfs2_group_desc *bg = NULL;
+	struct ocfs2_group_desc *bg;
 	struct ocfs2_super *osb = OCFS2_SB(inode->i_sb);
 	int c_to_b = 1 << (osb->s_clustersize_bits -
 					inode->i_sb->s_blocksize_bits);
@@ -482,13 +482,6 @@ static int ocfs2_validate_and_adjust_move_goal(struct inode *inode,
 	range->me_goal = ocfs2_block_to_cluster_start(inode->i_sb,
 						      range->me_goal);
 	/*
-	 * moving goal is not allowd to start with a group desc blok(#0 blk)
-	 * let's compromise to the latter cluster.
-	 */
-	if (range->me_goal == le64_to_cpu(bg->bg_blkno))
-		range->me_goal += c_to_b;
-
-	/*
 	 * validate goal sits within global_bitmap, and return the victim
 	 * group desc
 	 */
@@ -502,6 +495,13 @@ static int ocfs2_validate_and_adjust_move_goal(struct inode *inode,
 	bg = (struct ocfs2_group_desc *)gd_bh->b_data;
 
 	/*
+	 * moving goal is not allowd to start with a group desc blok(#0 blk)
+	 * let's compromise to the latter cluster.
+	 */
+	if (range->me_goal == le64_to_cpu(bg->bg_blkno))
+		range->me_goal += c_to_b;
+
+	/*
 	 * movement is not gonna cross two groups.
 	 */
 	if ((le16_to_cpu(bg->bg_bits) - goal_bit) * osb->s_clustersize <
@@ -1057,42 +1057,40 @@ int ocfs2_ioctl_move_extents(struct file *filp, void __user *argp)
 
 	struct inode *inode = file_inode(filp);
 	struct ocfs2_move_extents range;
-	struct ocfs2_move_extents_context *context = NULL;
+	struct ocfs2_move_extents_context *context;
+
+	if (!argp)
+		return -EINVAL;
 
 	status = mnt_want_write_file(filp);
 	if (status)
 		return status;
 
 	if ((!S_ISREG(inode->i_mode)) || !(filp->f_mode & FMODE_WRITE))
-		goto out;
+		goto out_drop;
 
 	if (inode->i_flags & (S_IMMUTABLE|S_APPEND)) {
 		status = -EPERM;
-		goto out;
+		goto out_drop;
 	}
 
 	context = kzalloc(sizeof(struct ocfs2_move_extents_context), GFP_NOFS);
 	if (!context) {
 		status = -ENOMEM;
 		mlog_errno(status);
-		goto out;
+		goto out_drop;
 	}
 
 	context->inode = inode;
 	context->file = filp;
 
-	if (argp) {
-		if (copy_from_user(&range, argp, sizeof(range))) {
-			status = -EFAULT;
-			goto out;
-		}
-	} else {
-		status = -EINVAL;
-		goto out;
+	if (copy_from_user(&range, argp, sizeof(range))) {
+		status = -EFAULT;
+		goto out_free;
 	}
 
 	if (range.me_start > i_size_read(inode))
-		goto out;
+		goto out_free;
 
 	if (range.me_start + range.me_len > i_size_read(inode))
 			range.me_len = i_size_read(inode) - range.me_start;
@@ -1124,25 +1122,24 @@ int ocfs2_ioctl_move_extents(struct file *filp, void __user *argp)
 
 		status = ocfs2_validate_and_adjust_move_goal(inode, &range);
 		if (status)
-			goto out;
+			goto out_copy;
 	}
 
 	status = ocfs2_move_extents(context);
 	if (status)
 		mlog_errno(status);
-out:
+out_copy:
 	/*
 	 * movement/defragmentation may end up being partially completed,
 	 * that's the reason why we need to return userspace the finished
 	 * length and new_offset even if failure happens somewhere.
 	 */
-	if (argp) {
-		if (copy_to_user(argp, &range, sizeof(range)))
-			status = -EFAULT;
-	}
+	if (copy_to_user(argp, &range, sizeof(range)))
+		status = -EFAULT;
 
+out_free:
 	kfree(context);
-
+out_drop:
 	mnt_drop_write_file(filp);
 
 	return status;
diff --git a/fs/pnode.c b/fs/pnode.c
index 3e000a51ac0..8b29d2164da 100644
--- a/fs/pnode.c
+++ b/fs/pnode.c
@@ -9,6 +9,7 @@
 #include <linux/mnt_namespace.h>
 #include <linux/mount.h>
 #include <linux/fs.h>
+#include <linux/nsproxy.h>
 #include "internal.h"
 #include "pnode.h"
 
@@ -220,6 +221,7 @@ static struct mount *get_source(struct mount *dest,
 int propagate_mnt(struct mount *dest_mnt, struct dentry *dest_dentry,
 		    struct mount *source_mnt, struct list_head *tree_list)
 {
+	struct user_namespace *user_ns = current->nsproxy->mnt_ns->user_ns;
 	struct mount *m, *child;
 	int ret = 0;
 	struct mount *prev_dest_mnt = dest_mnt;
@@ -237,6 +239,10 @@ int propagate_mnt(struct mount *dest_mnt, struct dentry *dest_dentry,
 
 		source =  get_source(m, prev_dest_mnt, prev_src_mnt, &type);
 
+		/* Notice when we are propagating across user namespaces */
+		if (m->mnt_ns->user_ns != user_ns)
+			type |= CL_UNPRIVILEGED;
+
 		child = copy_tree(source, source->mnt.mnt_root, type);
 		if (IS_ERR(child)) {
 			ret = PTR_ERR(child);
diff --git a/fs/pnode.h b/fs/pnode.h
index 19b853a3445..a0493d5ebfb 100644
--- a/fs/pnode.h
+++ b/fs/pnode.h
@@ -23,6 +23,7 @@
 #define CL_MAKE_SHARED 		0x08
 #define CL_PRIVATE 		0x10
 #define CL_SHARED_TO_SLAVE	0x20
+#define CL_UNPRIVILEGED		0x40
 
 static inline void set_mnt_shared(struct mount *mnt)
 {
diff --git a/fs/proc/Makefile b/fs/proc/Makefile
index 712f24db960..ab30716584f 100644
--- a/fs/proc/Makefile
+++ b/fs/proc/Makefile
@@ -5,7 +5,7 @@
 obj-y   += proc.o
 
 proc-y			:= nommu.o task_nommu.o
-proc-$(CONFIG_MMU)	:= mmu.o task_mmu.o
+proc-$(CONFIG_MMU)	:= task_mmu.o
 
 proc-y       += inode.o root.o base.o generic.o array.o \
 		fd.o
diff --git a/fs/proc/array.c b/fs/proc/array.c
index f7ed9ee46eb..cbd0f1b324b 100644
--- a/fs/proc/array.c
+++ b/fs/proc/array.c
@@ -143,6 +143,7 @@ static const char * const task_state_array[] = {
 	"x (dead)",		/*  64 */
 	"K (wakekill)",		/* 128 */
 	"W (waking)",		/* 256 */
+	"P (parked)",		/* 512 */
 };
 
 static inline const char *get_task_state(struct task_struct *tsk)
diff --git a/fs/proc/base.c b/fs/proc/base.c
index 69078c7cef1..a1930860414 100644
--- a/fs/proc/base.c
+++ b/fs/proc/base.c
@@ -86,6 +86,7 @@
 #include <linux/fs_struct.h>
 #include <linux/slab.h>
 #include <linux/flex_array.h>
+#include <linux/posix-timers.h>
 #ifdef CONFIG_HARDWALL
 #include <asm/hardwall.h>
 #endif
@@ -2013,6 +2014,102 @@ static const struct file_operations proc_map_files_operations = {
 	.llseek		= default_llseek,
 };
 
+struct timers_private {
+	struct pid *pid;
+	struct task_struct *task;
+	struct sighand_struct *sighand;
+	struct pid_namespace *ns;
+	unsigned long flags;
+};
+
+static void *timers_start(struct seq_file *m, loff_t *pos)
+{
+	struct timers_private *tp = m->private;
+
+	tp->task = get_pid_task(tp->pid, PIDTYPE_PID);
+	if (!tp->task)
+		return ERR_PTR(-ESRCH);
+
+	tp->sighand = lock_task_sighand(tp->task, &tp->flags);
+	if (!tp->sighand)
+		return ERR_PTR(-ESRCH);
+
+	return seq_list_start(&tp->task->signal->posix_timers, *pos);
+}
+
+static void *timers_next(struct seq_file *m, void *v, loff_t *pos)
+{
+	struct timers_private *tp = m->private;
+	return seq_list_next(v, &tp->task->signal->posix_timers, pos);
+}
+
+static void timers_stop(struct seq_file *m, void *v)
+{
+	struct timers_private *tp = m->private;
+
+	if (tp->sighand) {
+		unlock_task_sighand(tp->task, &tp->flags);
+		tp->sighand = NULL;
+	}
+
+	if (tp->task) {
+		put_task_struct(tp->task);
+		tp->task = NULL;
+	}
+}
+
+static int show_timer(struct seq_file *m, void *v)
+{
+	struct k_itimer *timer;
+	struct timers_private *tp = m->private;
+	int notify;
+	static char *nstr[] = {
+		[SIGEV_SIGNAL] = "signal",
+		[SIGEV_NONE] = "none",
+		[SIGEV_THREAD] = "thread",
+	};
+
+	timer = list_entry((struct list_head *)v, struct k_itimer, list);
+	notify = timer->it_sigev_notify;
+
+	seq_printf(m, "ID: %d\n", timer->it_id);
+	seq_printf(m, "signal: %d/%p\n", timer->sigq->info.si_signo,
+			timer->sigq->info.si_value.sival_ptr);
+	seq_printf(m, "notify: %s/%s.%d\n",
+		nstr[notify & ~SIGEV_THREAD_ID],
+		(notify & SIGEV_THREAD_ID) ? "tid" : "pid",
+		pid_nr_ns(timer->it_pid, tp->ns));
+
+	return 0;
+}
+
+static const struct seq_operations proc_timers_seq_ops = {
+	.start	= timers_start,
+	.next	= timers_next,
+	.stop	= timers_stop,
+	.show	= show_timer,
+};
+
+static int proc_timers_open(struct inode *inode, struct file *file)
+{
+	struct timers_private *tp;
+
+	tp = __seq_open_private(file, &proc_timers_seq_ops,
+			sizeof(struct timers_private));
+	if (!tp)
+		return -ENOMEM;
+
+	tp->pid = proc_pid(inode);
+	tp->ns = inode->i_sb->s_fs_info;
+	return 0;
+}
+
+static const struct file_operations proc_timers_operations = {
+	.open		= proc_timers_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= seq_release_private,
+};
 #endif /* CONFIG_CHECKPOINT_RESTORE */
 
 static struct dentry *proc_pident_instantiate(struct inode *dir,
@@ -2583,6 +2680,9 @@ static const struct pid_entry tgid_base_stuff[] = {
 	REG("gid_map",    S_IRUGO|S_IWUSR, proc_gid_map_operations),
 	REG("projid_map", S_IRUGO|S_IWUSR, proc_projid_map_operations),
 #endif
+#ifdef CONFIG_CHECKPOINT_RESTORE
+	REG("timers",	  S_IRUGO, proc_timers_operations),
+#endif
 };
 
 static int proc_tgid_base_readdir(struct file * filp,
diff --git a/fs/proc/generic.c b/fs/proc/generic.c
index 4b3b3ffb52f..21e1a8f1659 100644
--- a/fs/proc/generic.c
+++ b/fs/proc/generic.c
@@ -755,37 +755,8 @@ void pde_put(struct proc_dir_entry *pde)
 		free_proc_entry(pde);
 }
 
-/*
- * Remove a /proc entry and free it if it's not currently in use.
- */
-void remove_proc_entry(const char *name, struct proc_dir_entry *parent)
+static void entry_rundown(struct proc_dir_entry *de)
 {
-	struct proc_dir_entry **p;
-	struct proc_dir_entry *de = NULL;
-	const char *fn = name;
-	unsigned int len;
-
-	spin_lock(&proc_subdir_lock);
-	if (__xlate_proc_name(name, &parent, &fn) != 0) {
-		spin_unlock(&proc_subdir_lock);
-		return;
-	}
-	len = strlen(fn);
-
-	for (p = &parent->subdir; *p; p=&(*p)->next ) {
-		if (proc_match(len, fn, *p)) {
-			de = *p;
-			*p = de->next;
-			de->next = NULL;
-			break;
-		}
-	}
-	spin_unlock(&proc_subdir_lock);
-	if (!de) {
-		WARN(1, "name '%s'\n", name);
-		return;
-	}
-
 	spin_lock(&de->pde_unload_lock);
 	/*
 	 * Stop accepting new callers into module. If you're
@@ -817,6 +788,40 @@ void remove_proc_entry(const char *name, struct proc_dir_entry *parent)
 		spin_lock(&de->pde_unload_lock);
 	}
 	spin_unlock(&de->pde_unload_lock);
+}
+
+/*
+ * Remove a /proc entry and free it if it's not currently in use.
+ */
+void remove_proc_entry(const char *name, struct proc_dir_entry *parent)
+{
+	struct proc_dir_entry **p;
+	struct proc_dir_entry *de = NULL;
+	const char *fn = name;
+	unsigned int len;
+
+	spin_lock(&proc_subdir_lock);
+	if (__xlate_proc_name(name, &parent, &fn) != 0) {
+		spin_unlock(&proc_subdir_lock);
+		return;
+	}
+	len = strlen(fn);
+
+	for (p = &parent->subdir; *p; p=&(*p)->next ) {
+		if (proc_match(len, fn, *p)) {
+			de = *p;
+			*p = de->next;
+			de->next = NULL;
+			break;
+		}
+	}
+	spin_unlock(&proc_subdir_lock);
+	if (!de) {
+		WARN(1, "name '%s'\n", name);
+		return;
+	}
+
+	entry_rundown(de);
 
 	if (S_ISDIR(de->mode))
 		parent->nlink--;
@@ -827,3 +832,57 @@ void remove_proc_entry(const char *name, struct proc_dir_entry *parent)
 	pde_put(de);
 }
 EXPORT_SYMBOL(remove_proc_entry);
+
+int remove_proc_subtree(const char *name, struct proc_dir_entry *parent)
+{
+	struct proc_dir_entry **p;
+	struct proc_dir_entry *root = NULL, *de, *next;
+	const char *fn = name;
+	unsigned int len;
+
+	spin_lock(&proc_subdir_lock);
+	if (__xlate_proc_name(name, &parent, &fn) != 0) {
+		spin_unlock(&proc_subdir_lock);
+		return -ENOENT;
+	}
+	len = strlen(fn);
+
+	for (p = &parent->subdir; *p; p=&(*p)->next ) {
+		if (proc_match(len, fn, *p)) {
+			root = *p;
+			*p = root->next;
+			root->next = NULL;
+			break;
+		}
+	}
+	if (!root) {
+		spin_unlock(&proc_subdir_lock);
+		return -ENOENT;
+	}
+	de = root;
+	while (1) {
+		next = de->subdir;
+		if (next) {
+			de->subdir = next->next;
+			next->next = NULL;
+			de = next;
+			continue;
+		}
+		spin_unlock(&proc_subdir_lock);
+
+		entry_rundown(de);
+		next = de->parent;
+		if (S_ISDIR(de->mode))
+			next->nlink--;
+		de->nlink = 0;
+		if (de == root)
+			break;
+		pde_put(de);
+
+		spin_lock(&proc_subdir_lock);
+		de = next;
+	}
+	pde_put(root);
+	return 0;
+}
+EXPORT_SYMBOL(remove_proc_subtree);
diff --git a/fs/proc/internal.h b/fs/proc/internal.h
index 85ff3a4598b..75710357a51 100644
--- a/fs/proc/internal.h
+++ b/fs/proc/internal.h
@@ -30,24 +30,6 @@ extern int proc_net_init(void);
 static inline int proc_net_init(void) { return 0; }
 #endif
 
-struct vmalloc_info {
-	unsigned long	used;
-	unsigned long	largest_chunk;
-};
-
-#ifdef CONFIG_MMU
-#define VMALLOC_TOTAL (VMALLOC_END - VMALLOC_START)
-extern void get_vmalloc_info(struct vmalloc_info *vmi);
-#else
-
-#define VMALLOC_TOTAL 0UL
-#define get_vmalloc_info(vmi)			\
-do {						\
-	(vmi)->used = 0;			\
-	(vmi)->largest_chunk = 0;		\
-} while(0)
-#endif
-
 extern int proc_tid_stat(struct seq_file *m, struct pid_namespace *ns,
 				struct pid *pid, struct task_struct *task);
 extern int proc_tgid_stat(struct seq_file *m, struct pid_namespace *ns,
diff --git a/fs/proc/kcore.c b/fs/proc/kcore.c
index eda6f017f27..f6a13f489e3 100644
--- a/fs/proc/kcore.c
+++ b/fs/proc/kcore.c
@@ -15,6 +15,7 @@
 #include <linux/capability.h>
 #include <linux/elf.h>
 #include <linux/elfcore.h>
+#include <linux/notifier.h>
 #include <linux/vmalloc.h>
 #include <linux/highmem.h>
 #include <linux/printk.h>
@@ -564,7 +565,6 @@ static const struct file_operations proc_kcore_operations = {
 	.llseek		= default_llseek,
 };
 
-#ifdef CONFIG_MEMORY_HOTPLUG
 /* just remember that we have to update kcore */
 static int __meminit kcore_callback(struct notifier_block *self,
 				    unsigned long action, void *arg)
@@ -578,8 +578,11 @@ static int __meminit kcore_callback(struct notifier_block *self,
 	}
 	return NOTIFY_OK;
 }
-#endif
 
+static struct notifier_block kcore_callback_nb __meminitdata = {
+	.notifier_call = kcore_callback,
+	.priority = 0,
+};
 
 static struct kcore_list kcore_vmalloc;
 
@@ -631,7 +634,7 @@ static int __init proc_kcore_init(void)
 	add_modules_range();
 	/* Store direct-map area from physical memory map */
 	kcore_update_ram();
-	hotplug_memory_notifier(kcore_callback, 0);
+	register_hotmemory_notifier(&kcore_callback_nb);
 
 	return 0;
 }
diff --git a/fs/proc/meminfo.c b/fs/proc/meminfo.c
index 1efaaa19c4f..5aa847a603c 100644
--- a/fs/proc/meminfo.c
+++ b/fs/proc/meminfo.c
@@ -11,6 +11,7 @@
 #include <linux/swap.h>
 #include <linux/vmstat.h>
 #include <linux/atomic.h>
+#include <linux/vmalloc.h>
 #include <asm/page.h>
 #include <asm/pgtable.h>
 #include "internal.h"
diff --git a/fs/proc/mmu.c b/fs/proc/mmu.c
deleted file mode 100644
index 8ae221dfd01..00000000000
--- a/fs/proc/mmu.c
+++ /dev/null
@@ -1,60 +0,0 @@
-/* mmu.c: mmu memory info files
- *
- * Copyright (C) 2004 Red Hat, Inc. All Rights Reserved.
- * Written by David Howells (dhowells@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.
- */
-#include <linux/spinlock.h>
-#include <linux/vmalloc.h>
-#include <linux/highmem.h>
-#include <asm/pgtable.h>
-#include "internal.h"
-
-void get_vmalloc_info(struct vmalloc_info *vmi)
-{
-	struct vm_struct *vma;
-	unsigned long free_area_size;
-	unsigned long prev_end;
-
-	vmi->used = 0;
-
-	if (!vmlist) {
-		vmi->largest_chunk = VMALLOC_TOTAL;
-	}
-	else {
-		vmi->largest_chunk = 0;
-
-		prev_end = VMALLOC_START;
-
-		read_lock(&vmlist_lock);
-
-		for (vma = vmlist; vma; vma = vma->next) {
-			unsigned long addr = (unsigned long) vma->addr;
-
-			/*
-			 * Some archs keep another range for modules in vmlist
-			 */
-			if (addr < VMALLOC_START)
-				continue;
-			if (addr >= VMALLOC_END)
-				break;
-
-			vmi->used += vma->size;
-
-			free_area_size = addr - prev_end;
-			if (vmi->largest_chunk < free_area_size)
-				vmi->largest_chunk = free_area_size;
-
-			prev_end = vma->size + addr;
-		}
-
-		if (VMALLOC_END - prev_end > vmi->largest_chunk)
-			vmi->largest_chunk = VMALLOC_END - prev_end;
-
-		read_unlock(&vmlist_lock);
-	}
-}
diff --git a/fs/proc/root.c b/fs/proc/root.c
index c6e9fac26ba..9c7fab1d23f 100644
--- a/fs/proc/root.c
+++ b/fs/proc/root.c
@@ -16,6 +16,7 @@
 #include <linux/sched.h>
 #include <linux/module.h>
 #include <linux/bitops.h>
+#include <linux/user_namespace.h>
 #include <linux/mount.h>
 #include <linux/pid_namespace.h>
 #include <linux/parser.h>
@@ -108,6 +109,9 @@ static struct dentry *proc_mount(struct file_system_type *fs_type,
 	} else {
 		ns = task_active_pid_ns(current);
 		options = data;
+
+		if (!current_user_ns()->may_mount_proc)
+			return ERR_PTR(-EPERM);
 	}
 
 	sb = sget(fs_type, proc_test_super, proc_set_super, flags, ns);
diff --git a/fs/read_write.c b/fs/read_write.c
index a698eff457f..7a648911246 100644
--- a/fs/read_write.c
+++ b/fs/read_write.c
@@ -17,6 +17,7 @@
 #include <linux/splice.h>
 #include <linux/compat.h>
 #include "read_write.h"
+#include "internal.h"
 
 #include <asm/uaccess.h>
 #include <asm/unistd.h>
@@ -127,7 +128,7 @@ EXPORT_SYMBOL(generic_file_llseek_size);
  *
  * This is a generic implemenation of ->llseek useable for all normal local
  * filesystems.  It just updates the file offset to the value specified by
- * @offset and @whence under i_mutex.
+ * @offset and @whence.
  */
 loff_t generic_file_llseek(struct file *file, loff_t offset, int whence)
 {
@@ -417,6 +418,33 @@ ssize_t do_sync_write(struct file *filp, const char __user *buf, size_t len, lof
 
 EXPORT_SYMBOL(do_sync_write);
 
+ssize_t __kernel_write(struct file *file, const char *buf, size_t count, loff_t *pos)
+{
+	mm_segment_t old_fs;
+	const char __user *p;
+	ssize_t ret;
+
+	if (!file->f_op || (!file->f_op->write && !file->f_op->aio_write))
+		return -EINVAL;
+
+	old_fs = get_fs();
+	set_fs(get_ds());
+	p = (__force const char __user *)buf;
+	if (count > MAX_RW_COUNT)
+		count =  MAX_RW_COUNT;
+	if (file->f_op->write)
+		ret = file->f_op->write(file, p, count, pos);
+	else
+		ret = do_sync_write(file, p, count, pos);
+	set_fs(old_fs);
+	if (ret > 0) {
+		fsnotify_modify(file);
+		add_wchar(current, ret);
+	}
+	inc_syscw(current);
+	return ret;
+}
+
 ssize_t vfs_write(struct file *file, const char __user *buf, size_t count, loff_t *pos)
 {
 	ssize_t ret;
diff --git a/fs/reiserfs/xattr.c b/fs/reiserfs/xattr.c
index c196369fe40..4cce1d9552f 100644
--- a/fs/reiserfs/xattr.c
+++ b/fs/reiserfs/xattr.c
@@ -187,8 +187,8 @@ fill_with_dentries(void *buf, const char *name, int namelen, loff_t offset,
 	if (dbuf->count == ARRAY_SIZE(dbuf->dentries))
 		return -ENOSPC;
 
-	if (name[0] == '.' && (name[1] == '\0' ||
-			       (name[1] == '.' && name[2] == '\0')))
+	if (name[0] == '.' && (namelen < 2 ||
+			       (namelen == 2 && name[1] == '.')))
 		return 0;
 
 	dentry = lookup_one_len(name, dbuf->xadir, namelen);
diff --git a/fs/splice.c b/fs/splice.c
index 718bd005638..29e394e49dd 100644
--- a/fs/splice.c
+++ b/fs/splice.c
@@ -31,6 +31,7 @@
 #include <linux/security.h>
 #include <linux/gfp.h>
 #include <linux/socket.h>
+#include "internal.h"
 
 /*
  * Attempt to steal a page from a pipe buffer. This should perhaps go into
@@ -1048,9 +1049,10 @@ static int write_pipe_buf(struct pipe_inode_info *pipe, struct pipe_buffer *buf,
 {
 	int ret;
 	void *data;
+	loff_t tmp = sd->pos;
 
 	data = buf->ops->map(pipe, buf, 0);
-	ret = kernel_write(sd->u.file, data + buf->offset, sd->len, sd->pos);
+	ret = __kernel_write(sd->u.file, data + buf->offset, sd->len, &tmp);
 	buf->ops->unmap(pipe, buf, data);
 
 	return ret;
diff --git a/fs/sysfs/dir.c b/fs/sysfs/dir.c
index 2fbdff6be25..e8e0e71b29d 100644
--- a/fs/sysfs/dir.c
+++ b/fs/sysfs/dir.c
@@ -165,21 +165,8 @@ struct sysfs_dirent *sysfs_get_active(struct sysfs_dirent *sd)
 	if (unlikely(!sd))
 		return NULL;
 
-	while (1) {
-		int v, t;
-
-		v = atomic_read(&sd->s_active);
-		if (unlikely(v < 0))
-			return NULL;
-
-		t = atomic_cmpxchg(&sd->s_active, v, v + 1);
-		if (likely(t == v))
-			break;
-		if (t < 0)
-			return NULL;
-
-		cpu_relax();
-	}
+	if (!atomic_inc_unless_negative(&sd->s_active))
+		return NULL;
 
 	if (likely(!ignore_lockdep(sd)))
 		rwsem_acquire_read(&sd->dep_map, 0, 1, _RET_IP_);
@@ -281,6 +268,10 @@ void release_sysfs_dirent(struct sysfs_dirent * sd)
 	 */
 	parent_sd = sd->s_parent;
 
+	WARN(!(sd->s_flags & SYSFS_FLAG_REMOVED),
+		"sysfs: free using entry: %s/%s\n",
+		parent_sd ? parent_sd->s_name : "", sd->s_name);
+
 	if (sysfs_type(sd) == SYSFS_KOBJ_LINK)
 		sysfs_put(sd->s_symlink.target_sd);
 	if (sysfs_type(sd) & SYSFS_COPY_NAME)
@@ -399,7 +390,7 @@ struct sysfs_dirent *sysfs_new_dirent(const char *name, umode_t mode, int type)
 
 	sd->s_name = name;
 	sd->s_mode = mode;
-	sd->s_flags = type;
+	sd->s_flags = type | SYSFS_FLAG_REMOVED;
 
 	return sd;
 
@@ -479,6 +470,9 @@ int __sysfs_add_one(struct sysfs_addrm_cxt *acxt, struct sysfs_dirent *sd)
 		ps_iattrs->ia_ctime = ps_iattrs->ia_mtime = CURRENT_TIME;
 	}
 
+	/* Mark the entry added into directory tree */
+	sd->s_flags &= ~SYSFS_FLAG_REMOVED;
+
 	return 0;
 }
 
@@ -1012,6 +1006,7 @@ static int sysfs_readdir(struct file * filp, void * dirent, filldir_t filldir)
 	enum kobj_ns_type type;
 	const void *ns;
 	ino_t ino;
+	loff_t off;
 
 	type = sysfs_ns_type(parent_sd);
 	ns = sysfs_info(dentry->d_sb)->ns[type];
@@ -1020,6 +1015,8 @@ static int sysfs_readdir(struct file * filp, void * dirent, filldir_t filldir)
 		ino = parent_sd->s_ino;
 		if (filldir(dirent, ".", 1, filp->f_pos, ino, DT_DIR) == 0)
 			filp->f_pos++;
+		else
+			return 0;
 	}
 	if (filp->f_pos == 1) {
 		if (parent_sd->s_parent)
@@ -1028,8 +1025,11 @@ static int sysfs_readdir(struct file * filp, void * dirent, filldir_t filldir)
 			ino = parent_sd->s_ino;
 		if (filldir(dirent, "..", 2, filp->f_pos, ino, DT_DIR) == 0)
 			filp->f_pos++;
+		else
+			return 0;
 	}
 	mutex_lock(&sysfs_mutex);
+	off = filp->f_pos;
 	for (pos = sysfs_dir_pos(ns, parent_sd, filp->f_pos, pos);
 	     pos;
 	     pos = sysfs_dir_next_pos(ns, parent_sd, filp->f_pos, pos)) {
@@ -1041,27 +1041,43 @@ static int sysfs_readdir(struct file * filp, void * dirent, filldir_t filldir)
 		len = strlen(name);
 		ino = pos->s_ino;
 		type = dt_type(pos);
-		filp->f_pos = pos->s_hash;
+		off = filp->f_pos = pos->s_hash;
 		filp->private_data = sysfs_get(pos);
 
 		mutex_unlock(&sysfs_mutex);
-		ret = filldir(dirent, name, len, filp->f_pos, ino, type);
+		ret = filldir(dirent, name, len, off, ino, type);
 		mutex_lock(&sysfs_mutex);
 		if (ret < 0)
 			break;
 	}
 	mutex_unlock(&sysfs_mutex);
-	if ((filp->f_pos > 1) && !pos) { /* EOF */
-		filp->f_pos = INT_MAX;
+
+	/* don't reference last entry if its refcount is dropped */
+	if (!pos) {
 		filp->private_data = NULL;
+
+		/* EOF and not changed as 0 or 1 in read/write path */
+		if (off == filp->f_pos && off > 1)
+			filp->f_pos = INT_MAX;
 	}
 	return 0;
 }
 
+static loff_t sysfs_dir_llseek(struct file *file, loff_t offset, int whence)
+{
+	struct inode *inode = file_inode(file);
+	loff_t ret;
+
+	mutex_lock(&inode->i_mutex);
+	ret = generic_file_llseek(file, offset, whence);
+	mutex_unlock(&inode->i_mutex);
+
+	return ret;
+}
 
 const struct file_operations sysfs_dir_operations = {
 	.read		= generic_read_dir,
 	.readdir	= sysfs_readdir,
 	.release	= sysfs_dir_release,
-	.llseek		= generic_file_llseek,
+	.llseek		= sysfs_dir_llseek,
 };
diff --git a/fs/sysfs/mount.c b/fs/sysfs/mount.c
index 8d924b5ec73..afd83273e6c 100644
--- a/fs/sysfs/mount.c
+++ b/fs/sysfs/mount.c
@@ -19,6 +19,7 @@
 #include <linux/module.h>
 #include <linux/magic.h>
 #include <linux/slab.h>
+#include <linux/user_namespace.h>
 
 #include "sysfs.h"
 
@@ -111,6 +112,9 @@ static struct dentry *sysfs_mount(struct file_system_type *fs_type,
 	struct super_block *sb;
 	int error;
 
+	if (!(flags & MS_KERNMOUNT) && !current_user_ns()->may_mount_sysfs)
+		return ERR_PTR(-EPERM);
+
 	info = kzalloc(sizeof(*info), GFP_KERNEL);
 	if (!info)
 		return ERR_PTR(-ENOMEM);
diff --git a/fs/ubifs/super.c b/fs/ubifs/super.c
index ac838b84493..f21acf0ef01 100644
--- a/fs/ubifs/super.c
+++ b/fs/ubifs/super.c
@@ -1568,6 +1568,12 @@ static int ubifs_remount_rw(struct ubifs_info *c)
 	c->remounting_rw = 1;
 	c->ro_mount = 0;
 
+	if (c->space_fixup) {
+		err = ubifs_fixup_free_space(c);
+		if (err)
+			return err;
+	}
+
 	err = check_free_space(c);
 	if (err)
 		goto out;
@@ -1684,12 +1690,6 @@ static int ubifs_remount_rw(struct ubifs_info *c)
 		err = dbg_check_space_info(c);
 	}
 
-	if (c->space_fixup) {
-		err = ubifs_fixup_free_space(c);
-		if (err)
-			goto out;
-	}
-
 	mutex_unlock(&c->umount_mutex);
 	return err;
 
diff --git a/fs/ufs/util.c b/fs/ufs/util.c
index 95425b59ce0..b6c2f94e041 100644
--- a/fs/ufs/util.c
+++ b/fs/ufs/util.c
@@ -26,8 +26,7 @@ struct ufs_buffer_head * _ubh_bread_ (struct ufs_sb_private_info * uspi,
 	count = size >> uspi->s_fshift;
 	if (count > UFS_MAXFRAG)
 		return NULL;
-	ubh = (struct ufs_buffer_head *)
-		kmalloc (sizeof (struct ufs_buffer_head), GFP_NOFS);
+	ubh = kmalloc (sizeof (struct ufs_buffer_head), GFP_NOFS);
 	if (!ubh)
 		return NULL;
 	ubh->fragment = fragment;
diff --git a/include/asm-generic/hugetlb.h b/include/asm-generic/hugetlb.h
new file mode 100644
index 00000000000..d06079c774a
--- /dev/null
+++ b/include/asm-generic/hugetlb.h
@@ -0,0 +1,40 @@
+#ifndef _ASM_GENERIC_HUGETLB_H
+#define _ASM_GENERIC_HUGETLB_H
+
+static inline pte_t mk_huge_pte(struct page *page, pgprot_t pgprot)
+{
+	return mk_pte(page, pgprot);
+}
+
+static inline int huge_pte_write(pte_t pte)
+{
+	return pte_write(pte);
+}
+
+static inline int huge_pte_dirty(pte_t pte)
+{
+	return pte_dirty(pte);
+}
+
+static inline pte_t huge_pte_mkwrite(pte_t pte)
+{
+	return pte_mkwrite(pte);
+}
+
+static inline pte_t huge_pte_mkdirty(pte_t pte)
+{
+	return pte_mkdirty(pte);
+}
+
+static inline pte_t huge_pte_modify(pte_t pte, pgprot_t newprot)
+{
+	return pte_modify(pte, newprot);
+}
+
+static inline void huge_pte_clear(struct mm_struct *mm, unsigned long addr,
+				  pte_t *ptep)
+{
+	pte_clear(mm, addr, ptep);
+}
+
+#endif /* _ASM_GENERIC_HUGETLB_H */
diff --git a/include/asm-generic/pgtable.h b/include/asm-generic/pgtable.h
index bfd87685fc1..a59ff51b016 100644
--- a/include/asm-generic/pgtable.h
+++ b/include/asm-generic/pgtable.h
@@ -7,6 +7,16 @@
 #include <linux/mm_types.h>
 #include <linux/bug.h>
 
+/*
+ * On almost all architectures and configurations, 0 can be used as the
+ * upper ceiling to free_pgtables(): on many architectures it has the same
+ * effect as using TASK_SIZE.  However, there is one configuration which
+ * must impose a more careful limit, to avoid freeing kernel pgtables.
+ */
+#ifndef USER_PGTABLES_CEILING
+#define USER_PGTABLES_CEILING	0UL
+#endif
+
 #ifndef __HAVE_ARCH_PTEP_SET_ACCESS_FLAGS
 extern int ptep_set_access_flags(struct vm_area_struct *vma,
 				 unsigned long address, pte_t *ptep,
diff --git a/include/asm-generic/tlb.h b/include/asm-generic/tlb.h
index 25f01d0bc14..b1b1fa6ffff 100644
--- a/include/asm-generic/tlb.h
+++ b/include/asm-generic/tlb.h
@@ -99,7 +99,12 @@ struct mmu_gather {
 	unsigned int		need_flush : 1,	/* Did free PTEs */
 				fast_mode  : 1; /* No batching   */
 
-	unsigned int		fullmm;
+	/* we are in the middle of an operation to clear
+	 * a full mm and can make some optimizations */
+	unsigned int		fullmm : 1,
+	/* we have performed an operation which
+	 * requires a complete flush of the tlb */
+				need_flush_all : 1;
 
 	struct mmu_gather_batch *active;
 	struct mmu_gather_batch	local;
diff --git a/include/linux/acpi.h b/include/linux/acpi.h
index bcbdd7484e5..03053aca5b3 100644
--- a/include/linux/acpi.h
+++ b/include/linux/acpi.h
@@ -152,15 +152,6 @@ void acpi_penalize_isa_irq(int irq, int active);
 
 void acpi_pci_irq_disable (struct pci_dev *dev);
 
-struct acpi_pci_driver {
-	struct list_head node;
-	int (*add)(struct acpi_pci_root *root);
-	void (*remove)(struct acpi_pci_root *root);
-};
-
-int acpi_pci_register_driver(struct acpi_pci_driver *driver);
-void acpi_pci_unregister_driver(struct acpi_pci_driver *driver);
-
 extern int ec_read(u8 addr, u8 *val);
 extern int ec_write(u8 addr, u8 val);
 extern int ec_transaction(u8 command,
diff --git a/include/linux/amba/serial.h b/include/linux/amba/serial.h
index f612c783170..62d9303c283 100644
--- a/include/linux/amba/serial.h
+++ b/include/linux/amba/serial.h
@@ -203,6 +203,9 @@ struct amba_pl011_data {
 	bool (*dma_filter)(struct dma_chan *chan, void *filter_param);
 	void *dma_rx_param;
 	void *dma_tx_param;
+	bool dma_rx_poll_enable;
+	unsigned int dma_rx_poll_rate;
+	unsigned int dma_rx_poll_timeout;
         void (*init) (void);
 	void (*exit) (void);
 };
diff --git a/include/linux/async.h b/include/linux/async.h
index a2e3f18b2ad..6b0226bdaad 100644
--- a/include/linux/async.h
+++ b/include/linux/async.h
@@ -16,9 +16,8 @@
 #include <linux/list.h>
 
 typedef u64 async_cookie_t;
-typedef void (async_func_ptr) (void *data, async_cookie_t cookie);
+typedef void (*async_func_t) (void *data, async_cookie_t cookie);
 struct async_domain {
-	struct list_head node;
 	struct list_head pending;
 	unsigned registered:1;
 };
@@ -27,8 +26,7 @@ struct async_domain {
  * domain participates in global async_synchronize_full
  */
 #define ASYNC_DOMAIN(_name) \
-	struct async_domain _name = { .node = LIST_HEAD_INIT(_name.node), \
-				      .pending = LIST_HEAD_INIT(_name.pending), \
+	struct async_domain _name = { .pending = LIST_HEAD_INIT(_name.pending),	\
 				      .registered = 1 }
 
 /*
@@ -36,12 +34,11 @@ struct async_domain {
  * complete, this domain does not participate in async_synchronize_full
  */
 #define ASYNC_DOMAIN_EXCLUSIVE(_name) \
-	struct async_domain _name = { .node = LIST_HEAD_INIT(_name.node), \
-				      .pending = LIST_HEAD_INIT(_name.pending), \
+	struct async_domain _name = { .pending = LIST_HEAD_INIT(_name.pending), \
 				      .registered = 0 }
 
-extern async_cookie_t async_schedule(async_func_ptr *ptr, void *data);
-extern async_cookie_t async_schedule_domain(async_func_ptr *ptr, void *data,
+extern async_cookie_t async_schedule(async_func_t func, void *data);
+extern async_cookie_t async_schedule_domain(async_func_t func, void *data,
 					    struct async_domain *domain);
 void async_unregister_domain(struct async_domain *domain);
 extern void async_synchronize_full(void);
diff --git a/include/linux/ata.h b/include/linux/ata.h
index 8f7a3d68371..ee0bd952405 100644
--- a/include/linux/ata.h
+++ b/include/linux/ata.h
@@ -954,7 +954,7 @@ static inline int atapi_cdb_len(const u16 *dev_id)
 	}
 }
 
-static inline bool atapi_command_packet_set(const u16 *dev_id)
+static inline int atapi_command_packet_set(const u16 *dev_id)
 {
 	return (dev_id[ATA_ID_CONFIG] >> 8) & 0x1f;
 }
diff --git a/include/linux/blk_types.h b/include/linux/blk_types.h
index cdf11191e64..22990cf4439 100644
--- a/include/linux/blk_types.h
+++ b/include/linux/blk_types.h
@@ -111,12 +111,13 @@ struct bio {
 #define BIO_FS_INTEGRITY 9	/* fs owns integrity data, not block layer */
 #define BIO_QUIET	10	/* Make BIO Quiet */
 #define BIO_MAPPED_INTEGRITY 11/* integrity metadata has been remapped */
+#define BIO_SNAP_STABLE	12	/* bio data must be snapshotted during write */
 
 /*
  * Flags starting here get preserved by bio_reset() - this includes
  * BIO_POOL_IDX()
  */
-#define BIO_RESET_BITS	12
+#define BIO_RESET_BITS	13
 
 #define bio_flagged(bio, flag)	((bio)->bi_flags & (1 << (flag)))
 
diff --git a/include/linux/blktrace_api.h b/include/linux/blktrace_api.h
index 0ea61e07a91..7c2e030e72f 100644
--- a/include/linux/blktrace_api.h
+++ b/include/linux/blktrace_api.h
@@ -12,7 +12,6 @@
 
 struct blk_trace {
 	int trace_state;
-	bool rq_based;
 	struct rchan *rchan;
 	unsigned long __percpu *sequence;
 	unsigned char __percpu *msg_data;
diff --git a/include/linux/bootmem.h b/include/linux/bootmem.h
index cdc3bab0183..5f0b0e1f7c0 100644
--- a/include/linux/bootmem.h
+++ b/include/linux/bootmem.h
@@ -44,7 +44,6 @@ extern unsigned long init_bootmem_node(pg_data_t *pgdat,
 				       unsigned long endpfn);
 extern unsigned long init_bootmem(unsigned long addr, unsigned long memend);
 
-extern unsigned long free_low_memory_core_early(int nodeid);
 extern unsigned long free_all_bootmem_node(pg_data_t *pgdat);
 extern unsigned long free_all_bootmem(void);
 
diff --git a/include/linux/buffer_head.h b/include/linux/buffer_head.h
index 5afc4f94d11..4c16c4a88d4 100644
--- a/include/linux/buffer_head.h
+++ b/include/linux/buffer_head.h
@@ -181,6 +181,7 @@ void ll_rw_block(int, int, struct buffer_head * bh[]);
 int sync_dirty_buffer(struct buffer_head *bh);
 int __sync_dirty_buffer(struct buffer_head *bh, int rw);
 void write_dirty_buffer(struct buffer_head *bh, int rw);
+int _submit_bh(int rw, struct buffer_head *bh, unsigned long bio_flags);
 int submit_bh(int, struct buffer_head *);
 void write_boundary_block(struct block_device *bdev,
 			sector_t bblock, unsigned blocksize);
diff --git a/include/linux/capability.h b/include/linux/capability.h
index 98503b79236..d9a4f7f40f3 100644
--- a/include/linux/capability.h
+++ b/include/linux/capability.h
@@ -35,6 +35,7 @@ struct cpu_vfs_cap_data {
 #define _KERNEL_CAP_T_SIZE     (sizeof(kernel_cap_t))
 
 
+struct file;
 struct inode;
 struct dentry;
 struct user_namespace;
@@ -211,6 +212,7 @@ extern bool capable(int cap);
 extern bool ns_capable(struct user_namespace *ns, int cap);
 extern bool nsown_capable(int cap);
 extern bool inode_capable(const struct inode *inode, int cap);
+extern bool file_ns_capable(const struct file *file, struct user_namespace *ns, int cap);
 
 /* audit system wants to get cap info from files as well */
 extern int get_vfs_caps_from_disk(const struct dentry *dentry, struct cpu_vfs_cap_data *cpu_caps);
diff --git a/include/linux/cgroup.h b/include/linux/cgroup.h
index 900af5964f5..646ab9d15e4 100644
--- a/include/linux/cgroup.h
+++ b/include/linux/cgroup.h
@@ -19,6 +19,7 @@
 #include <linux/idr.h>
 #include <linux/workqueue.h>
 #include <linux/xattr.h>
+#include <linux/fs.h>
 
 #ifdef CONFIG_CGROUPS
 
@@ -30,10 +31,6 @@ struct css_id;
 
 extern int cgroup_init_early(void);
 extern int cgroup_init(void);
-extern void cgroup_lock(void);
-extern int cgroup_lock_is_held(void);
-extern bool cgroup_lock_live_group(struct cgroup *cgrp);
-extern void cgroup_unlock(void);
 extern void cgroup_fork(struct task_struct *p);
 extern void cgroup_post_fork(struct task_struct *p);
 extern void cgroup_exit(struct task_struct *p, int run_callbacks);
@@ -44,14 +41,25 @@ extern void cgroup_unload_subsys(struct cgroup_subsys *ss);
 
 extern const struct file_operations proc_cgroup_operations;
 
-/* Define the enumeration of all builtin cgroup subsystems */
+/*
+ * Define the enumeration of all cgroup subsystems.
+ *
+ * We define ids for builtin subsystems and then modular ones.
+ */
 #define SUBSYS(_x) _x ## _subsys_id,
-#define IS_SUBSYS_ENABLED(option) IS_ENABLED(option)
 enum cgroup_subsys_id {
+#define IS_SUBSYS_ENABLED(option) IS_BUILTIN(option)
 #include <linux/cgroup_subsys.h>
+#undef IS_SUBSYS_ENABLED
+	CGROUP_BUILTIN_SUBSYS_COUNT,
+
+	__CGROUP_SUBSYS_TEMP_PLACEHOLDER = CGROUP_BUILTIN_SUBSYS_COUNT - 1,
+
+#define IS_SUBSYS_ENABLED(option) IS_MODULE(option)
+#include <linux/cgroup_subsys.h>
+#undef IS_SUBSYS_ENABLED
 	CGROUP_SUBSYS_COUNT,
 };
-#undef IS_SUBSYS_ENABLED
 #undef SUBSYS
 
 /* Per-subsystem/per-cgroup state maintained by the system. */
@@ -148,6 +156,13 @@ enum {
 	 * specified at mount time and thus is implemented here.
 	 */
 	CGRP_CPUSET_CLONE_CHILDREN,
+	/* see the comment above CGRP_ROOT_SANE_BEHAVIOR for details */
+	CGRP_SANE_BEHAVIOR,
+};
+
+struct cgroup_name {
+	struct rcu_head rcu_head;
+	char name[];
 };
 
 struct cgroup {
@@ -172,11 +187,23 @@ struct cgroup {
 	struct cgroup *parent;		/* my parent */
 	struct dentry *dentry;		/* cgroup fs entry, RCU protected */
 
+	/*
+	 * This is a copy of dentry->d_name, and it's needed because
+	 * we can't use dentry->d_name in cgroup_path().
+	 *
+	 * You must acquire rcu_read_lock() to access cgrp->name, and
+	 * the only place that can change it is rename(), which is
+	 * protected by parent dir's i_mutex.
+	 *
+	 * Normally you should use cgroup_name() wrapper rather than
+	 * access it directly.
+	 */
+	struct cgroup_name __rcu *name;
+
 	/* Private pointers for each registered subsystem */
 	struct cgroup_subsys_state *subsys[CGROUP_SUBSYS_COUNT];
 
 	struct cgroupfs_root *root;
-	struct cgroup *top_cgroup;
 
 	/*
 	 * List of cg_cgroup_links pointing at css_sets with
@@ -213,6 +240,96 @@ struct cgroup {
 	struct simple_xattrs xattrs;
 };
 
+#define MAX_CGROUP_ROOT_NAMELEN 64
+
+/* cgroupfs_root->flags */
+enum {
+	/*
+	 * Unfortunately, cgroup core and various controllers are riddled
+	 * with idiosyncrasies and pointless options.  The following flag,
+	 * when set, will force sane behavior - some options are forced on,
+	 * others are disallowed, and some controllers will change their
+	 * hierarchical or other behaviors.
+	 *
+	 * The set of behaviors affected by this flag are still being
+	 * determined and developed and the mount option for this flag is
+	 * prefixed with __DEVEL__.  The prefix will be dropped once we
+	 * reach the point where all behaviors are compatible with the
+	 * planned unified hierarchy, which will automatically turn on this
+	 * flag.
+	 *
+	 * The followings are the behaviors currently affected this flag.
+	 *
+	 * - Mount options "noprefix" and "clone_children" are disallowed.
+	 *   Also, cgroupfs file cgroup.clone_children is not created.
+	 *
+	 * - When mounting an existing superblock, mount options should
+	 *   match.
+	 *
+	 * - Remount is disallowed.
+	 *
+	 * - memcg: use_hierarchy is on by default and the cgroup file for
+	 *   the flag is not created.
+	 *
+	 * The followings are planned changes.
+	 *
+	 * - release_agent will be disallowed once replacement notification
+	 *   mechanism is implemented.
+	 */
+	CGRP_ROOT_SANE_BEHAVIOR	= (1 << 0),
+
+	CGRP_ROOT_NOPREFIX	= (1 << 1), /* mounted subsystems have no named prefix */
+	CGRP_ROOT_XATTR		= (1 << 2), /* supports extended attributes */
+};
+
+/*
+ * A cgroupfs_root represents the root of a cgroup hierarchy, and may be
+ * associated with a superblock to form an active hierarchy.  This is
+ * internal to cgroup core.  Don't access directly from controllers.
+ */
+struct cgroupfs_root {
+	struct super_block *sb;
+
+	/*
+	 * The bitmask of subsystems intended to be attached to this
+	 * hierarchy
+	 */
+	unsigned long subsys_mask;
+
+	/* Unique id for this hierarchy. */
+	int hierarchy_id;
+
+	/* The bitmask of subsystems currently attached to this hierarchy */
+	unsigned long actual_subsys_mask;
+
+	/* A list running through the attached subsystems */
+	struct list_head subsys_list;
+
+	/* The root cgroup for this hierarchy */
+	struct cgroup top_cgroup;
+
+	/* Tracks how many cgroups are currently defined in hierarchy.*/
+	int number_of_cgroups;
+
+	/* A list running through the active hierarchies */
+	struct list_head root_list;
+
+	/* All cgroups on this root, cgroup_mutex protected */
+	struct list_head allcg_list;
+
+	/* Hierarchy-specific flags */
+	unsigned long flags;
+
+	/* IDs for cgroups in this hierarchy */
+	struct ida cgroup_ida;
+
+	/* The path to use for release notifications. */
+	char release_agent_path[PATH_MAX];
+
+	/* The name for this hierarchy - may be empty */
+	char name[MAX_CGROUP_ROOT_NAMELEN];
+};
+
 /*
  * A css_set is a structure holding pointers to a set of
  * cgroup_subsys_state objects. This saves space in the task struct
@@ -278,6 +395,7 @@ struct cgroup_map_cb {
 /* cftype->flags */
 #define CFTYPE_ONLY_ON_ROOT	(1U << 0)	/* only create on root cg */
 #define CFTYPE_NOT_ON_ROOT	(1U << 1)	/* don't create on root cg */
+#define CFTYPE_INSANE		(1U << 2)	/* don't create if sane_behavior */
 
 #define MAX_CFTYPE_NAME		64
 
@@ -304,9 +422,6 @@ struct cftype {
 	/* CFTYPE_* flags */
 	unsigned int flags;
 
-	/* file xattrs */
-	struct simple_xattrs xattrs;
-
 	int (*open)(struct inode *inode, struct file *file);
 	ssize_t (*read)(struct cgroup *cgrp, struct cftype *cft,
 			struct file *file,
@@ -404,18 +519,31 @@ struct cgroup_scanner {
 	void *data;
 };
 
+/*
+ * See the comment above CGRP_ROOT_SANE_BEHAVIOR for details.  This
+ * function can be called as long as @cgrp is accessible.
+ */
+static inline bool cgroup_sane_behavior(const struct cgroup *cgrp)
+{
+	return cgrp->root->flags & CGRP_ROOT_SANE_BEHAVIOR;
+}
+
+/* Caller should hold rcu_read_lock() */
+static inline const char *cgroup_name(const struct cgroup *cgrp)
+{
+	return rcu_dereference(cgrp->name)->name;
+}
+
 int cgroup_add_cftypes(struct cgroup_subsys *ss, struct cftype *cfts);
 int cgroup_rm_cftypes(struct cgroup_subsys *ss, struct cftype *cfts);
 
 int cgroup_is_removed(const struct cgroup *cgrp);
+bool cgroup_is_descendant(struct cgroup *cgrp, struct cgroup *ancestor);
 
 int cgroup_path(const struct cgroup *cgrp, char *buf, int buflen);
 
 int cgroup_task_count(const struct cgroup *cgrp);
 
-/* Return true if cgrp is a descendant of the task's cgroup */
-int cgroup_is_descendant(const struct cgroup *cgrp, struct task_struct *task);
-
 /*
  * Control Group taskset, used to pass around set of tasks to cgroup_subsys
  * methods.
@@ -458,7 +586,6 @@ struct cgroup_subsys {
 	void (*bind)(struct cgroup *root);
 
 	int subsys_id;
-	int active;
 	int disabled;
 	int early_init;
 	/*
@@ -523,10 +650,16 @@ static inline struct cgroup_subsys_state *cgroup_subsys_state(
  * rcu_dereference_check() conditions, such as locks used during the
  * cgroup_subsys::attach() methods.
  */
+#ifdef CONFIG_PROVE_RCU
+extern struct mutex cgroup_mutex;
 #define task_subsys_state_check(task, subsys_id, __c)			\
-	rcu_dereference_check(task->cgroups->subsys[subsys_id],		\
-			      lockdep_is_held(&task->alloc_lock) ||	\
-			      cgroup_lock_is_held() || (__c))
+	rcu_dereference_check((task)->cgroups->subsys[(subsys_id)],	\
+			      lockdep_is_held(&(task)->alloc_lock) ||	\
+			      lockdep_is_held(&cgroup_mutex) || (__c))
+#else
+#define task_subsys_state_check(task, subsys_id, __c)			\
+	rcu_dereference((task)->cgroups->subsys[(subsys_id)])
+#endif
 
 static inline struct cgroup_subsys_state *
 task_subsys_state(struct task_struct *task, int subsys_id)
@@ -661,8 +794,8 @@ struct task_struct *cgroup_iter_next(struct cgroup *cgrp,
 					struct cgroup_iter *it);
 void cgroup_iter_end(struct cgroup *cgrp, struct cgroup_iter *it);
 int cgroup_scan_tasks(struct cgroup_scanner *scan);
-int cgroup_attach_task(struct cgroup *, struct task_struct *);
 int cgroup_attach_task_all(struct task_struct *from, struct task_struct *);
+int cgroup_transfer_tasks(struct cgroup *to, struct cgroup *from);
 
 /*
  * CSS ID is ID for cgroup_subsys_state structs under subsys. This only works
@@ -687,13 +820,6 @@ void free_css_id(struct cgroup_subsys *ss, struct cgroup_subsys_state *css);
 
 struct cgroup_subsys_state *css_lookup(struct cgroup_subsys *ss, int id);
 
-/*
- * Get a cgroup whose id is greater than or equal to id under tree of root.
- * Returning a cgroup_subsys_state or NULL.
- */
-struct cgroup_subsys_state *css_get_next(struct cgroup_subsys *ss, int id,
-		struct cgroup_subsys_state *root, int *foundid);
-
 /* Returns true if root is ancestor of cg */
 bool css_is_ancestor(struct cgroup_subsys_state *cg,
 		     const struct cgroup_subsys_state *root);
diff --git a/include/linux/clk-private.h b/include/linux/clk-private.h
index 9c7f5807824..dd7adff76e8 100644
--- a/include/linux/clk-private.h
+++ b/include/linux/clk-private.h
@@ -152,7 +152,7 @@ struct clk {
 		},						\
 		.reg = _reg,					\
 		.shift = _shift,				\
-		.width = _width,				\
+		.mask = BIT(_width) - 1,			\
 		.flags = _mux_flags,				\
 		.lock = _lock,					\
 	};							\
diff --git a/include/linux/clk-provider.h b/include/linux/clk-provider.h
index 7f197d7addb..11860985fec 100644
--- a/include/linux/clk-provider.h
+++ b/include/linux/clk-provider.h
@@ -45,6 +45,14 @@ struct clk_hw;
  * 		undo any work done in the @prepare callback. Called with
  * 		prepare_lock held.
  *
+ * @is_prepared: Queries the hardware to determine if the clock is prepared.
+ *		This function is allowed to sleep. Optional, if this op is not
+ *		set then the prepare count will be used.
+ *
+ * @unprepare_unused: Unprepare the clock atomically.  Only called from
+ *		clk_disable_unused for prepare clocks with special needs.
+ *		Called with prepare mutex held. This function may sleep.
+ *
  * @enable:	Enable the clock atomically. This must not return until the
  * 		clock is generating a valid clock signal, usable by consumer
  * 		devices. Called with enable_lock held. This function must not
@@ -108,6 +116,8 @@ struct clk_hw;
 struct clk_ops {
 	int		(*prepare)(struct clk_hw *hw);
 	void		(*unprepare)(struct clk_hw *hw);
+	int		(*is_prepared)(struct clk_hw *hw);
+	void		(*unprepare_unused)(struct clk_hw *hw);
 	int		(*enable)(struct clk_hw *hw);
 	void		(*disable)(struct clk_hw *hw);
 	int		(*is_enabled)(struct clk_hw *hw);
@@ -239,9 +249,14 @@ struct clk_div_table {
  * CLK_DIVIDER_ONE_BASED - by default the divisor is the value read from the
  * 	register plus one.  If CLK_DIVIDER_ONE_BASED is set then the divider is
  * 	the raw value read from the register, with the value of zero considered
- * 	invalid
+ *	invalid, unless CLK_DIVIDER_ALLOW_ZERO is set.
  * CLK_DIVIDER_POWER_OF_TWO - clock divisor is 2 raised to the value read from
  * 	the hardware register
+ * CLK_DIVIDER_ALLOW_ZERO - Allow zero divisors.  For dividers which have
+ *	CLK_DIVIDER_ONE_BASED set, it is possible to end up with a zero divisor.
+ *	Some hardware implementations gracefully handle this case and allow a
+ *	zero divisor by not modifying their input clock
+ *	(divide by one / bypass).
  */
 struct clk_divider {
 	struct clk_hw	hw;
@@ -255,6 +270,7 @@ struct clk_divider {
 
 #define CLK_DIVIDER_ONE_BASED		BIT(0)
 #define CLK_DIVIDER_POWER_OF_TWO	BIT(1)
+#define CLK_DIVIDER_ALLOW_ZERO		BIT(2)
 
 extern const struct clk_ops clk_divider_ops;
 struct clk *clk_register_divider(struct device *dev, const char *name,
@@ -274,7 +290,7 @@ struct clk *clk_register_divider_table(struct device *dev, const char *name,
  * @reg:	register controlling multiplexer
  * @shift:	shift to multiplexer bit field
  * @width:	width of mutliplexer bit field
- * @num_clks:	number of parent clocks
+ * @flags:	hardware-specific flags
  * @lock:	register lock
  *
  * Clock with multiple selectable parents.  Implements .get_parent, .set_parent
@@ -287,8 +303,9 @@ struct clk *clk_register_divider_table(struct device *dev, const char *name,
 struct clk_mux {
 	struct clk_hw	hw;
 	void __iomem	*reg;
+	u32		*table;
+	u32		mask;
 	u8		shift;
-	u8		width;
 	u8		flags;
 	spinlock_t	*lock;
 };
@@ -297,11 +314,19 @@ struct clk_mux {
 #define CLK_MUX_INDEX_BIT		BIT(1)
 
 extern const struct clk_ops clk_mux_ops;
+
 struct clk *clk_register_mux(struct device *dev, const char *name,
 		const char **parent_names, u8 num_parents, unsigned long flags,
 		void __iomem *reg, u8 shift, u8 width,
 		u8 clk_mux_flags, spinlock_t *lock);
 
+struct clk *clk_register_mux_table(struct device *dev, const char *name,
+		const char **parent_names, u8 num_parents, unsigned long flags,
+		void __iomem *reg, u8 shift, u32 mask,
+		u8 clk_mux_flags, u32 *table, spinlock_t *lock);
+
+void of_fixed_factor_clk_setup(struct device_node *node);
+
 /**
  * struct clk_fixed_factor - fixed multiplier and divider clock
  *
@@ -325,6 +350,37 @@ struct clk *clk_register_fixed_factor(struct device *dev, const char *name,
 		const char *parent_name, unsigned long flags,
 		unsigned int mult, unsigned int div);
 
+/***
+ * struct clk_composite - aggregate clock of mux, divider and gate clocks
+ *
+ * @hw:		handle between common and hardware-specific interfaces
+ * @mux_hw:	handle between composite and hardware-specific mux clock
+ * @rate_hw:	handle between composite and hardware-specific rate clock
+ * @gate_hw:	handle between composite and hardware-specific gate clock
+ * @mux_ops:	clock ops for mux
+ * @rate_ops:	clock ops for rate
+ * @gate_ops:	clock ops for gate
+ */
+struct clk_composite {
+	struct clk_hw	hw;
+	struct clk_ops	ops;
+
+	struct clk_hw	*mux_hw;
+	struct clk_hw	*rate_hw;
+	struct clk_hw	*gate_hw;
+
+	const struct clk_ops	*mux_ops;
+	const struct clk_ops	*rate_ops;
+	const struct clk_ops	*gate_ops;
+};
+
+struct clk *clk_register_composite(struct device *dev, const char *name,
+		const char **parent_names, int num_parents,
+		struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
+		struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
+		struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
+		unsigned long flags);
+
 /**
  * clk_register - allocate a new clock, register it and return an opaque cookie
  * @dev: device that is registering this clock
@@ -351,6 +407,7 @@ unsigned int __clk_get_enable_count(struct clk *clk);
 unsigned int __clk_get_prepare_count(struct clk *clk);
 unsigned long __clk_get_rate(struct clk *clk);
 unsigned long __clk_get_flags(struct clk *clk);
+bool __clk_is_prepared(struct clk *clk);
 bool __clk_is_enabled(struct clk *clk);
 struct clk *__clk_lookup(const char *name);
 
diff --git a/include/linux/clk.h b/include/linux/clk.h
index b3ac22d0fc1..9a6d04524b1 100644
--- a/include/linux/clk.h
+++ b/include/linux/clk.h
@@ -28,16 +28,16 @@ struct clk;
  * PRE_RATE_CHANGE - called immediately before the clk rate is changed,
  *     to indicate that the rate change will proceed.  Drivers must
  *     immediately terminate any operations that will be affected by the
- *     rate change.  Callbacks may either return NOTIFY_DONE or
- *     NOTIFY_STOP.
+ *     rate change.  Callbacks may either return NOTIFY_DONE, NOTIFY_OK,
+ *     NOTIFY_STOP or NOTIFY_BAD.
  *
  * ABORT_RATE_CHANGE: called if the rate change failed for some reason
  *     after PRE_RATE_CHANGE.  In this case, all registered notifiers on
  *     the clk will be called with ABORT_RATE_CHANGE. Callbacks must
- *     always return NOTIFY_DONE.
+ *     always return NOTIFY_DONE or NOTIFY_OK.
  *
  * POST_RATE_CHANGE - called after the clk rate change has successfully
- *     completed.  Callbacks must always return NOTIFY_DONE.
+ *     completed.  Callbacks must always return NOTIFY_DONE or NOTIFY_OK.
  *
  */
 #define PRE_RATE_CHANGE			BIT(0)
diff --git a/include/linux/clk/sunxi.h b/include/linux/clk/sunxi.h
new file mode 100644
index 00000000000..e074fdd5a23
--- /dev/null
+++ b/include/linux/clk/sunxi.h
@@ -0,0 +1,22 @@
+/*
+ * Copyright 2012 Maxime Ripard
+ *
+ * Maxime Ripard <maxime.ripard@free-electrons.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.
+ */
+
+#ifndef __LINUX_CLK_SUNXI_H_
+#define __LINUX_CLK_SUNXI_H_
+
+void __init sunxi_init_clocks(void);
+
+#endif
diff --git a/include/linux/clockchips.h b/include/linux/clockchips.h
index 66346521cb6..464e229e7d8 100644
--- a/include/linux/clockchips.h
+++ b/include/linux/clockchips.h
@@ -55,6 +55,11 @@ enum clock_event_nofitiers {
 #define CLOCK_EVT_FEAT_C3STOP		0x000008
 #define CLOCK_EVT_FEAT_DUMMY		0x000010
 
+/*
+ * Core shall set the interrupt affinity dynamically in broadcast mode
+ */
+#define CLOCK_EVT_FEAT_DYNIRQ		0x000020
+
 /**
  * struct clock_event_device - clock event device descriptor
  * @event_handler:	Assigned by the framework to be called by the low
@@ -170,6 +175,12 @@ extern void tick_broadcast(const struct cpumask *mask);
 extern int tick_receive_broadcast(void);
 #endif
 
+#if defined(CONFIG_GENERIC_CLOCKEVENTS_BROADCAST) && defined(CONFIG_TICK_ONESHOT)
+extern int tick_check_broadcast_expired(void);
+#else
+static inline int tick_check_broadcast_expired(void) { return 0; }
+#endif
+
 #ifdef CONFIG_GENERIC_CLOCKEVENTS
 extern void clockevents_notify(unsigned long reason, void *arg);
 #else
@@ -182,6 +193,7 @@ static inline void clockevents_suspend(void) {}
 static inline void clockevents_resume(void) {}
 
 #define clockevents_notify(reason, arg) do { } while (0)
+static inline int tick_check_broadcast_expired(void) { return 0; }
 
 #endif
 
diff --git a/include/linux/clocksource.h b/include/linux/clocksource.h
index 27cfda427dd..aa7032c7238 100644
--- a/include/linux/clocksource.h
+++ b/include/linux/clocksource.h
@@ -206,6 +206,7 @@ struct clocksource {
 #define CLOCK_SOURCE_WATCHDOG			0x10
 #define CLOCK_SOURCE_VALID_FOR_HRES		0x20
 #define CLOCK_SOURCE_UNSTABLE			0x40
+#define CLOCK_SOURCE_SUSPEND_NONSTOP		0x80
 
 /* simplify initialization of mask field */
 #define CLOCKSOURCE_MASK(bits) (cycle_t)((bits) < 64 ? ((1ULL<<(bits))-1) : -1)
diff --git a/include/linux/compat.h b/include/linux/compat.h
index 76a87fb57ac..377cd8c3395 100644
--- a/include/linux/compat.h
+++ b/include/linux/compat.h
@@ -141,11 +141,11 @@ typedef struct {
 } compat_sigset_t;
 
 struct compat_sigaction {
-#ifndef __ARCH_HAS_ODD_SIGACTION
+#ifndef __ARCH_HAS_IRIX_SIGACTION
 	compat_uptr_t			sa_handler;
 	compat_ulong_t			sa_flags;
 #else
-	compat_ulong_t			sa_flags;
+	compat_uint_t			sa_flags;
 	compat_uptr_t			sa_handler;
 #endif
 #ifdef __ARCH_HAS_SA_RESTORER
diff --git a/include/linux/compiler-gcc4.h b/include/linux/compiler-gcc4.h
index 68b162d9225..842de225055 100644
--- a/include/linux/compiler-gcc4.h
+++ b/include/linux/compiler-gcc4.h
@@ -13,7 +13,7 @@
 #define __must_check 		__attribute__((warn_unused_result))
 #define __compiler_offsetof(a,b) __builtin_offsetof(a,b)
 
-#if GCC_VERSION >= 40100
+#if GCC_VERSION >= 40100 && GCC_VERSION < 40600
 # define __compiletime_object_size(obj) __builtin_object_size(obj, 0)
 #endif
 
diff --git a/include/linux/compiler.h b/include/linux/compiler.h
index 10b8f23fab0..92669cd182a 100644
--- a/include/linux/compiler.h
+++ b/include/linux/compiler.h
@@ -351,4 +351,10 @@ void ftrace_likely_update(struct ftrace_branch_data *f, int val, int expect);
  */
 #define ACCESS_ONCE(x) (*(volatile typeof(x) *)&(x))
 
+/* Ignore/forbid kprobes attach on very low level functions marked by this attribute: */
+#ifdef CONFIG_KPROBES
+# define __kprobes	__attribute__((__section__(".kprobes.text")))
+#else
+# define __kprobes
+#endif
 #endif /* __LINUX_COMPILER_H */
diff --git a/include/linux/console.h b/include/linux/console.h
index 29680a8cda9..73bab0f58af 100644
--- a/include/linux/console.h
+++ b/include/linux/console.h
@@ -141,6 +141,7 @@ struct console {
 	for (con = console_drivers; con != NULL; con = con->next)
 
 extern int console_set_on_cmdline;
+extern struct console *early_console;
 
 extern int add_preferred_console(char *name, int idx, char *options);
 extern int update_console_cmdline(char *name, int idx, char *name_new, int idx_new, char *options);
diff --git a/include/linux/context_tracking.h b/include/linux/context_tracking.h
index b28d161c109..365f4a61bf0 100644
--- a/include/linux/context_tracking.h
+++ b/include/linux/context_tracking.h
@@ -1,9 +1,9 @@
 #ifndef _LINUX_CONTEXT_TRACKING_H
 #define _LINUX_CONTEXT_TRACKING_H
 
-#ifdef CONFIG_CONTEXT_TRACKING
 #include <linux/sched.h>
 #include <linux/percpu.h>
+#include <asm/ptrace.h>
 
 struct context_tracking {
 	/*
@@ -13,12 +13,13 @@ struct context_tracking {
 	 * may be further optimized using static keys.
 	 */
 	bool active;
-	enum {
+	enum ctx_state {
 		IN_KERNEL = 0,
 		IN_USER,
 	} state;
 };
 
+#ifdef CONFIG_CONTEXT_TRACKING
 DECLARE_PER_CPU(struct context_tracking, context_tracking);
 
 static inline bool context_tracking_in_user(void)
@@ -33,12 +34,31 @@ static inline bool context_tracking_active(void)
 
 extern void user_enter(void);
 extern void user_exit(void);
+
+static inline enum ctx_state exception_enter(void)
+{
+	enum ctx_state prev_ctx;
+
+	prev_ctx = this_cpu_read(context_tracking.state);
+	user_exit();
+
+	return prev_ctx;
+}
+
+static inline void exception_exit(enum ctx_state prev_ctx)
+{
+	if (prev_ctx == IN_USER)
+		user_enter();
+}
+
 extern void context_tracking_task_switch(struct task_struct *prev,
 					 struct task_struct *next);
 #else
 static inline bool context_tracking_in_user(void) { return false; }
 static inline void user_enter(void) { }
 static inline void user_exit(void) { }
+static inline enum ctx_state exception_enter(void) { return 0; }
+static inline void exception_exit(enum ctx_state prev_ctx) { }
 static inline void context_tracking_task_switch(struct task_struct *prev,
 						struct task_struct *next) { }
 #endif /* !CONFIG_CONTEXT_TRACKING */
diff --git a/include/linux/cpu.h b/include/linux/cpu.h
index ce7a074f251..c6f6e0839b6 100644
--- a/include/linux/cpu.h
+++ b/include/linux/cpu.h
@@ -212,4 +212,20 @@ static inline int disable_nonboot_cpus(void) { return 0; }
 static inline void enable_nonboot_cpus(void) {}
 #endif /* !CONFIG_PM_SLEEP_SMP */
 
+enum cpuhp_state {
+	CPUHP_OFFLINE,
+	CPUHP_ONLINE,
+};
+
+void cpu_startup_entry(enum cpuhp_state state);
+void cpu_idle(void);
+
+void cpu_idle_poll_ctrl(bool enable);
+
+void arch_cpu_idle(void);
+void arch_cpu_idle_prepare(void);
+void arch_cpu_idle_enter(void);
+void arch_cpu_idle_exit(void);
+void arch_cpu_idle_dead(void);
+
 #endif /* _LINUX_CPU_H_ */
diff --git a/include/linux/cpumask.h b/include/linux/cpumask.h
index 032560295fc..d08e4d2a9b9 100644
--- a/include/linux/cpumask.h
+++ b/include/linux/cpumask.h
@@ -591,6 +591,21 @@ static inline int cpulist_scnprintf(char *buf, int len,
 }
 
 /**
+ * cpumask_parse - extract a cpumask from from a string
+ * @buf: the buffer to extract from
+ * @dstp: the cpumask to set.
+ *
+ * Returns -errno, or 0 for success.
+ */
+static inline int cpumask_parse(const char *buf, struct cpumask *dstp)
+{
+	char *nl = strchr(buf, '\n');
+	int len = nl ? nl - buf : strlen(buf);
+
+	return bitmap_parse(buf, len, cpumask_bits(dstp), nr_cpumask_bits);
+}
+
+/**
  * cpulist_parse - extract a cpumask from a user string of ranges
  * @buf: the buffer to extract from
  * @dstp: the cpumask to set.
diff --git a/include/linux/cpuset.h b/include/linux/cpuset.h
index 8c8a60d2940..ccd1de8ad82 100644
--- a/include/linux/cpuset.h
+++ b/include/linux/cpuset.h
@@ -11,7 +11,6 @@
 #include <linux/sched.h>
 #include <linux/cpumask.h>
 #include <linux/nodemask.h>
-#include <linux/cgroup.h>
 #include <linux/mm.h>
 
 #ifdef CONFIG_CPUSETS
diff --git a/include/linux/ctype.h b/include/linux/ctype.h
index 8acfe312f94..653589e3e30 100644
--- a/include/linux/ctype.h
+++ b/include/linux/ctype.h
@@ -61,4 +61,10 @@ static inline char _tolower(const char c)
 	return c | 0x20;
 }
 
+/* Fast check for octal digit */
+static inline int isodigit(const char c)
+{
+	return c >= '0' && c <= '7';
+}
+
 #endif
diff --git a/include/linux/debug_locks.h b/include/linux/debug_locks.h
index a975de1ff59..21ca773f77b 100644
--- a/include/linux/debug_locks.h
+++ b/include/linux/debug_locks.h
@@ -27,7 +27,7 @@ extern int debug_locks_off(void);
 									\
 	if (!oops_in_progress && unlikely(c)) {				\
 		if (debug_locks_off() && !debug_locks_silent)		\
-			WARN_ON(1);					\
+			WARN(1, "DEBUG_LOCKS_WARN_ON(%s)", #c);		\
 		__ret = 1;						\
 	}								\
 	__ret;								\
@@ -51,7 +51,7 @@ struct task_struct;
 extern void debug_show_all_locks(void);
 extern void debug_show_held_locks(struct task_struct *task);
 extern void debug_check_no_locks_freed(const void *from, unsigned long len);
-extern void debug_check_no_locks_held(void);
+extern void debug_check_no_locks_held(struct task_struct *task);
 #else
 static inline void debug_show_all_locks(void)
 {
@@ -67,7 +67,7 @@ debug_check_no_locks_freed(const void *from, unsigned long len)
 }
 
 static inline void
-debug_check_no_locks_held(void)
+debug_check_no_locks_held(struct task_struct *task)
 {
 }
 #endif
diff --git a/include/linux/devfreq.h b/include/linux/devfreq.h
index e83ef39b3be..fe8c4476f7e 100644
--- a/include/linux/devfreq.h
+++ b/include/linux/devfreq.h
@@ -213,7 +213,7 @@ struct devfreq_simple_ondemand_data {
 #endif
 
 #else /* !CONFIG_PM_DEVFREQ */
-static struct devfreq *devfreq_add_device(struct device *dev,
+static inline struct devfreq *devfreq_add_device(struct device *dev,
 					  struct devfreq_dev_profile *profile,
 					  const char *governor_name,
 					  void *data)
@@ -221,34 +221,34 @@ static struct devfreq *devfreq_add_device(struct device *dev,
 	return NULL;
 }
 
-static int devfreq_remove_device(struct devfreq *devfreq)
+static inline int devfreq_remove_device(struct devfreq *devfreq)
 {
 	return 0;
 }
 
-static int devfreq_suspend_device(struct devfreq *devfreq)
+static inline int devfreq_suspend_device(struct devfreq *devfreq)
 {
 	return 0;
 }
 
-static int devfreq_resume_device(struct devfreq *devfreq)
+static inline int devfreq_resume_device(struct devfreq *devfreq)
 {
 	return 0;
 }
 
-static struct opp *devfreq_recommended_opp(struct device *dev,
+static inline struct opp *devfreq_recommended_opp(struct device *dev,
 					   unsigned long *freq, u32 flags)
 {
-	return -EINVAL;
+	return ERR_PTR(-EINVAL);
 }
 
-static int devfreq_register_opp_notifier(struct device *dev,
+static inline int devfreq_register_opp_notifier(struct device *dev,
 					 struct devfreq *devfreq)
 {
 	return -EINVAL;
 }
 
-static int devfreq_unregister_opp_notifier(struct device *dev,
+static inline int devfreq_unregister_opp_notifier(struct device *dev,
 					   struct devfreq *devfreq)
 {
 	return -EINVAL;
diff --git a/include/linux/device.h b/include/linux/device.h
index 9d6464ea99c..711793b145f 100644
--- a/include/linux/device.h
+++ b/include/linux/device.h
@@ -25,6 +25,7 @@
 #include <linux/pm.h>
 #include <linux/atomic.h>
 #include <linux/ratelimit.h>
+#include <linux/uidgid.h>
 #include <asm/device.h>
 
 struct device;
@@ -111,17 +112,11 @@ struct bus_type {
 	struct iommu_ops *iommu_ops;
 
 	struct subsys_private *p;
+	struct lock_class_key lock_key;
 };
 
-/* This is a #define to keep the compiler from merging different
- * instances of the __key variable */
-#define bus_register(subsys)			\
-({						\
-	static struct lock_class_key __key;	\
-	__bus_register(subsys, &__key);	\
-})
-extern int __must_check __bus_register(struct bus_type *bus,
-				       struct lock_class_key *key);
+extern int __must_check bus_register(struct bus_type *bus);
+
 extern void bus_unregister(struct bus_type *bus);
 
 extern int __must_check bus_rescan_devices(struct bus_type *bus);
@@ -302,6 +297,8 @@ void subsys_interface_unregister(struct subsys_interface *sif);
 
 int subsys_system_register(struct bus_type *subsys,
 			   const struct attribute_group **groups);
+int subsys_virtual_register(struct bus_type *subsys,
+			    const struct attribute_group **groups);
 
 /**
  * struct class - device classes
@@ -471,7 +468,8 @@ struct device_type {
 	const char *name;
 	const struct attribute_group **groups;
 	int (*uevent)(struct device *dev, struct kobj_uevent_env *env);
-	char *(*devnode)(struct device *dev, umode_t *mode);
+	char *(*devnode)(struct device *dev, umode_t *mode,
+			 kuid_t *uid, kgid_t *gid);
 	void (*release)(struct device *dev);
 
 	const struct dev_pm_ops *pm;
@@ -849,7 +847,8 @@ extern int device_rename(struct device *dev, const char *new_name);
 extern int device_move(struct device *dev, struct device *new_parent,
 		       enum dpm_order dpm_order);
 extern const char *device_get_devnode(struct device *dev,
-				      umode_t *mode, const char **tmp);
+				      umode_t *mode, kuid_t *uid, kgid_t *gid,
+				      const char **tmp);
 extern void *dev_get_drvdata(const struct device *dev);
 extern int dev_set_drvdata(struct device *dev, void *data);
 
diff --git a/include/linux/efi.h b/include/linux/efi.h
index 9bf2f1fcae2..3d7df3d32c6 100644
--- a/include/linux/efi.h
+++ b/include/linux/efi.h
@@ -333,6 +333,7 @@ typedef efi_status_t efi_query_capsule_caps_t(efi_capsule_header_t **capsules,
 					      unsigned long count,
 					      u64 *max_size,
 					      int *reset_type);
+typedef efi_status_t efi_query_variable_store_t(u32 attributes, unsigned long size);
 
 /*
  *  EFI Configuration Table and GUID definitions
@@ -575,9 +576,15 @@ extern void efi_enter_virtual_mode (void);	/* switch EFI to virtual mode, if pos
 #ifdef CONFIG_X86
 extern void efi_late_init(void);
 extern void efi_free_boot_services(void);
+extern efi_status_t efi_query_variable_store(u32 attributes, unsigned long size);
 #else
 static inline void efi_late_init(void) {}
 static inline void efi_free_boot_services(void) {}
+
+static inline efi_status_t efi_query_variable_store(u32 attributes, unsigned long size)
+{
+	return EFI_SUCCESS;
+}
 #endif
 extern void __iomem *efi_lookup_mapped_addr(u64 phys_addr);
 extern u64 efi_get_iobase (void);
@@ -731,7 +738,7 @@ struct efivar_operations {
 	efi_get_variable_t *get_variable;
 	efi_get_next_variable_t *get_next_variable;
 	efi_set_variable_t *set_variable;
-	efi_query_variable_info_t *query_variable_info;
+	efi_query_variable_store_t *query_variable_store;
 };
 
 struct efivars {
diff --git a/include/linux/evm.h b/include/linux/evm.h
index 9fc13a76092..1fcb88ca88d 100644
--- a/include/linux/evm.h
+++ b/include/linux/evm.h
@@ -96,5 +96,5 @@ static inline int evm_inode_init_security(struct inode *inode,
 	return 0;
 }
 
-#endif /* CONFIG_EVM_H */
+#endif /* CONFIG_EVM */
 #endif /* LINUX_EVM_H */
diff --git a/include/linux/exportfs.h b/include/linux/exportfs.h
index 5b9b5b31718..41b223a59a6 100644
--- a/include/linux/exportfs.h
+++ b/include/linux/exportfs.h
@@ -85,6 +85,17 @@ enum fid_type {
 	FILEID_NILFS_WITH_PARENT = 0x62,
 
 	/*
+	 * 32 bit generation number, 40 bit i_pos.
+	 */
+	FILEID_FAT_WITHOUT_PARENT = 0x71,
+
+	/*
+	 * 32 bit generation number, 40 bit i_pos,
+	 * 32 bit parent generation number, 40 bit parent i_pos
+	 */
+	FILEID_FAT_WITH_PARENT = 0x72,
+
+	/*
 	 * Filesystems must not use 0xff file ID.
 	 */
 	FILEID_INVALID = 0xff,
diff --git a/include/linux/freezer.h b/include/linux/freezer.h
index 043a5cf8b5b..e70df40d84f 100644
--- a/include/linux/freezer.h
+++ b/include/linux/freezer.h
@@ -3,7 +3,6 @@
 #ifndef FREEZER_H_INCLUDED
 #define FREEZER_H_INCLUDED
 
-#include <linux/debug_locks.h>
 #include <linux/sched.h>
 #include <linux/wait.h>
 #include <linux/atomic.h>
@@ -49,8 +48,6 @@ extern void thaw_kernel_threads(void);
 
 static inline bool try_to_freeze(void)
 {
-	if (!(current->flags & PF_NOFREEZE))
-		debug_check_no_locks_held();
 	might_sleep();
 	if (likely(!freezing(current)))
 		return false;
diff --git a/include/linux/fs_struct.h b/include/linux/fs_struct.h
index 729eded4b24..2b93a9a5a1e 100644
--- a/include/linux/fs_struct.h
+++ b/include/linux/fs_struct.h
@@ -50,4 +50,6 @@ static inline void get_fs_root_and_pwd(struct fs_struct *fs, struct path *root,
 	spin_unlock(&fs->lock);
 }
 
+extern bool current_chrooted(void);
+
 #endif /* _LINUX_FS_STRUCT_H */
diff --git a/include/linux/fsnotify_backend.h b/include/linux/fsnotify_backend.h
index d5b0910d496..4b2ee8d12f5 100644
--- a/include/linux/fsnotify_backend.h
+++ b/include/linux/fsnotify_backend.h
@@ -157,7 +157,6 @@ struct fsnotify_group {
 		struct inotify_group_private_data {
 			spinlock_t	idr_lock;
 			struct idr      idr;
-			u32             last_wd;
 			struct user_struct      *user;
 		} inotify_data;
 #endif
diff --git a/include/linux/ftrace.h b/include/linux/ftrace.h
index e5ca8ef50e9..f83e17a40e8 100644
--- a/include/linux/ftrace.h
+++ b/include/linux/ftrace.h
@@ -89,6 +89,7 @@ typedef void (*ftrace_func_t)(unsigned long ip, unsigned long parent_ip,
  *            that the call back has its own recursion protection. If it does
  *            not set this, then the ftrace infrastructure will add recursion
  *            protection for the caller.
+ * STUB   - The ftrace_ops is just a place holder.
  */
 enum {
 	FTRACE_OPS_FL_ENABLED			= 1 << 0,
@@ -98,6 +99,7 @@ enum {
 	FTRACE_OPS_FL_SAVE_REGS			= 1 << 4,
 	FTRACE_OPS_FL_SAVE_REGS_IF_SUPPORTED	= 1 << 5,
 	FTRACE_OPS_FL_RECURSION_SAFE		= 1 << 6,
+	FTRACE_OPS_FL_STUB			= 1 << 7,
 };
 
 struct ftrace_ops {
@@ -259,8 +261,10 @@ struct ftrace_probe_ops {
 	void			(*func)(unsigned long ip,
 					unsigned long parent_ip,
 					void **data);
-	int			(*callback)(unsigned long ip, void **data);
-	void			(*free)(void **data);
+	int			(*init)(struct ftrace_probe_ops *ops,
+					unsigned long ip, void **data);
+	void			(*free)(struct ftrace_probe_ops *ops,
+					unsigned long ip, void **data);
 	int			(*print)(struct seq_file *m,
 					 unsigned long ip,
 					 struct ftrace_probe_ops *ops,
@@ -394,7 +398,6 @@ ssize_t ftrace_filter_write(struct file *file, const char __user *ubuf,
 			    size_t cnt, loff_t *ppos);
 ssize_t ftrace_notrace_write(struct file *file, const char __user *ubuf,
 			     size_t cnt, loff_t *ppos);
-loff_t ftrace_regex_lseek(struct file *file, loff_t offset, int whence);
 int ftrace_regex_release(struct inode *inode, struct file *file);
 
 void __init
@@ -567,6 +570,8 @@ static inline int
 ftrace_regex_release(struct inode *inode, struct file *file) { return -ENODEV; }
 #endif /* CONFIG_DYNAMIC_FTRACE */
 
+loff_t ftrace_filter_lseek(struct file *file, loff_t offset, int whence);
+
 /* totally disable ftrace - can not re-enable after this */
 void ftrace_kill(void);
 
diff --git a/include/linux/ftrace_event.h b/include/linux/ftrace_event.h
index 13a54d0bdfa..34e00fb49be 100644
--- a/include/linux/ftrace_event.h
+++ b/include/linux/ftrace_event.h
@@ -8,6 +8,7 @@
 #include <linux/perf_event.h>
 
 struct trace_array;
+struct trace_buffer;
 struct tracer;
 struct dentry;
 
@@ -38,6 +39,12 @@ const char *ftrace_print_symbols_seq_u64(struct trace_seq *p,
 const char *ftrace_print_hex_seq(struct trace_seq *p,
 				 const unsigned char *buf, int len);
 
+struct trace_iterator;
+struct trace_event;
+
+int ftrace_raw_output_prep(struct trace_iterator *iter,
+			   struct trace_event *event);
+
 /*
  * The trace entry - the most basic unit of tracing. This is what
  * is printed in the end as a single line in the trace output, such as:
@@ -61,6 +68,7 @@ struct trace_entry {
 struct trace_iterator {
 	struct trace_array	*tr;
 	struct tracer		*trace;
+	struct trace_buffer	*trace_buffer;
 	void			*private;
 	int			cpu_file;
 	struct mutex		mutex;
@@ -95,8 +103,6 @@ enum trace_iter_flags {
 };
 
 
-struct trace_event;
-
 typedef enum print_line_t (*trace_print_func)(struct trace_iterator *iter,
 				      int flags, struct trace_event *event);
 
@@ -128,6 +134,13 @@ enum print_line_t {
 void tracing_generic_entry_update(struct trace_entry *entry,
 				  unsigned long flags,
 				  int pc);
+struct ftrace_event_file;
+
+struct ring_buffer_event *
+trace_event_buffer_lock_reserve(struct ring_buffer **current_buffer,
+				struct ftrace_event_file *ftrace_file,
+				int type, unsigned long len,
+				unsigned long flags, int pc);
 struct ring_buffer_event *
 trace_current_buffer_lock_reserve(struct ring_buffer **current_buffer,
 				  int type, unsigned long len,
@@ -182,53 +195,49 @@ extern int ftrace_event_reg(struct ftrace_event_call *event,
 			    enum trace_reg type, void *data);
 
 enum {
-	TRACE_EVENT_FL_ENABLED_BIT,
 	TRACE_EVENT_FL_FILTERED_BIT,
-	TRACE_EVENT_FL_RECORDED_CMD_BIT,
 	TRACE_EVENT_FL_CAP_ANY_BIT,
 	TRACE_EVENT_FL_NO_SET_FILTER_BIT,
 	TRACE_EVENT_FL_IGNORE_ENABLE_BIT,
+	TRACE_EVENT_FL_WAS_ENABLED_BIT,
 };
 
+/*
+ * Event flags:
+ *  FILTERED	  - The event has a filter attached
+ *  CAP_ANY	  - Any user can enable for perf
+ *  NO_SET_FILTER - Set when filter has error and is to be ignored
+ *  IGNORE_ENABLE - For ftrace internal events, do not enable with debugfs file
+ *  WAS_ENABLED   - Set and stays set when an event was ever enabled
+ *                    (used for module unloading, if a module event is enabled,
+ *                     it is best to clear the buffers that used it).
+ */
 enum {
-	TRACE_EVENT_FL_ENABLED		= (1 << TRACE_EVENT_FL_ENABLED_BIT),
 	TRACE_EVENT_FL_FILTERED		= (1 << TRACE_EVENT_FL_FILTERED_BIT),
-	TRACE_EVENT_FL_RECORDED_CMD	= (1 << TRACE_EVENT_FL_RECORDED_CMD_BIT),
 	TRACE_EVENT_FL_CAP_ANY		= (1 << TRACE_EVENT_FL_CAP_ANY_BIT),
 	TRACE_EVENT_FL_NO_SET_FILTER	= (1 << TRACE_EVENT_FL_NO_SET_FILTER_BIT),
 	TRACE_EVENT_FL_IGNORE_ENABLE	= (1 << TRACE_EVENT_FL_IGNORE_ENABLE_BIT),
+	TRACE_EVENT_FL_WAS_ENABLED	= (1 << TRACE_EVENT_FL_WAS_ENABLED_BIT),
 };
 
 struct ftrace_event_call {
 	struct list_head	list;
 	struct ftrace_event_class *class;
 	char			*name;
-	struct dentry		*dir;
 	struct trace_event	event;
 	const char		*print_fmt;
 	struct event_filter	*filter;
+	struct list_head	*files;
 	void			*mod;
 	void			*data;
-
 	/*
-	 * 32 bit flags:
-	 *   bit 1:		enabled
-	 *   bit 2:		filter_active
-	 *   bit 3:		enabled cmd record
-	 *   bit 4:		allow trace by non root (cap any)
-	 *   bit 5:		failed to apply filter
-	 *   bit 6:		ftrace internal event (do not enable)
-	 *
-	 * Changes to flags must hold the event_mutex.
-	 *
-	 * Note: Reads of flags do not hold the event_mutex since
-	 * they occur in critical sections. But the way flags
-	 * is currently used, these changes do no affect the code
-	 * except that when a change is made, it may have a slight
-	 * delay in propagating the changes to other CPUs due to
-	 * caching and such.
+	 *   bit 0:		filter_active
+	 *   bit 1:		allow trace by non root (cap any)
+	 *   bit 2:		failed to apply filter
+	 *   bit 3:		ftrace internal event (do not enable)
+	 *   bit 4:		Event was enabled by module
 	 */
-	unsigned int		flags;
+	int			flags; /* static flags of different events */
 
 #ifdef CONFIG_PERF_EVENTS
 	int				perf_refcount;
@@ -236,6 +245,56 @@ struct ftrace_event_call {
 #endif
 };
 
+struct trace_array;
+struct ftrace_subsystem_dir;
+
+enum {
+	FTRACE_EVENT_FL_ENABLED_BIT,
+	FTRACE_EVENT_FL_RECORDED_CMD_BIT,
+	FTRACE_EVENT_FL_SOFT_MODE_BIT,
+	FTRACE_EVENT_FL_SOFT_DISABLED_BIT,
+};
+
+/*
+ * Ftrace event file flags:
+ *  ENABLED	  - The event is enabled
+ *  RECORDED_CMD  - The comms should be recorded at sched_switch
+ *  SOFT_MODE     - The event is enabled/disabled by SOFT_DISABLED
+ *  SOFT_DISABLED - When set, do not trace the event (even though its
+ *                   tracepoint may be enabled)
+ */
+enum {
+	FTRACE_EVENT_FL_ENABLED		= (1 << FTRACE_EVENT_FL_ENABLED_BIT),
+	FTRACE_EVENT_FL_RECORDED_CMD	= (1 << FTRACE_EVENT_FL_RECORDED_CMD_BIT),
+	FTRACE_EVENT_FL_SOFT_MODE	= (1 << FTRACE_EVENT_FL_SOFT_MODE_BIT),
+	FTRACE_EVENT_FL_SOFT_DISABLED	= (1 << FTRACE_EVENT_FL_SOFT_DISABLED_BIT),
+};
+
+struct ftrace_event_file {
+	struct list_head		list;
+	struct ftrace_event_call	*event_call;
+	struct dentry			*dir;
+	struct trace_array		*tr;
+	struct ftrace_subsystem_dir	*system;
+
+	/*
+	 * 32 bit flags:
+	 *   bit 0:		enabled
+	 *   bit 1:		enabled cmd record
+	 *   bit 2:		enable/disable with the soft disable bit
+	 *   bit 3:		soft disabled
+	 *
+	 * Note: The bits must be set atomically to prevent races
+	 * from other writers. Reads of flags do not need to be in
+	 * sync as they occur in critical sections. But the way flags
+	 * is currently used, these changes do not affect the code
+	 * except that when a change is made, it may have a slight
+	 * delay in propagating the changes to other CPUs due to
+	 * caching and such. Which is mostly OK ;-)
+	 */
+	unsigned long		flags;
+};
+
 #define __TRACE_EVENT_FLAGS(name, value)				\
 	static int __init trace_init_flags_##name(void)			\
 	{								\
@@ -274,7 +333,7 @@ extern int trace_define_field(struct ftrace_event_call *call, const char *type,
 extern int trace_add_event_call(struct ftrace_event_call *call);
 extern void trace_remove_event_call(struct ftrace_event_call *call);
 
-#define is_signed_type(type)	(((type)(-1)) < (type)0)
+#define is_signed_type(type)	(((type)(-1)) < (type)1)
 
 int trace_set_clr_event(const char *system, const char *event, int set);
 
diff --git a/include/linux/genalloc.h b/include/linux/genalloc.h
index dd7c569aaca..661d374aeb2 100644
--- a/include/linux/genalloc.h
+++ b/include/linux/genalloc.h
@@ -29,6 +29,10 @@
 
 #ifndef __GENALLOC_H__
 #define __GENALLOC_H__
+
+struct device;
+struct device_node;
+
 /**
  * Allocation callback function type definition
  * @map: Pointer to bitmap
@@ -105,4 +109,18 @@ extern unsigned long gen_pool_first_fit(unsigned long *map, unsigned long size,
 extern unsigned long gen_pool_best_fit(unsigned long *map, unsigned long size,
 		unsigned long start, unsigned int nr, void *data);
 
+extern struct gen_pool *devm_gen_pool_create(struct device *dev,
+		int min_alloc_order, int nid);
+extern struct gen_pool *dev_get_gen_pool(struct device *dev);
+
+#ifdef CONFIG_OF
+extern struct gen_pool *of_get_named_gen_pool(struct device_node *np,
+	const char *propname, int index);
+#else
+static inline struct gen_pool *of_get_named_gen_pool(struct device_node *np,
+	const char *propname, int index)
+{
+	return NULL;
+}
+#endif
 #endif /* __GENALLOC_H__ */
diff --git a/include/linux/hid-debug.h b/include/linux/hid-debug.h
index 53744fa1c8b..8663f216c56 100644
--- a/include/linux/hid-debug.h
+++ b/include/linux/hid-debug.h
@@ -22,11 +22,12 @@
  *
  */
 
-#define HID_DEBUG_BUFSIZE 512
-
 #ifdef CONFIG_DEBUG_FS
 
+#define HID_DEBUG_BUFSIZE 512
+
 void hid_dump_input(struct hid_device *, struct hid_usage *, __s32);
+void hid_dump_report(struct hid_device *, int , u8 *, int);
 void hid_dump_device(struct hid_device *, struct seq_file *);
 void hid_dump_field(struct hid_field *, int, struct seq_file *);
 char *hid_resolv_usage(unsigned, struct seq_file *);
@@ -50,6 +51,7 @@ struct hid_debug_list {
 #else
 
 #define hid_dump_input(a,b,c)		do { } while (0)
+#define hid_dump_report(a,b,c,d)	do { } while (0)
 #define hid_dump_device(a,b)		do { } while (0)
 #define hid_dump_field(a,b,c)		do { } while (0)
 #define hid_resolv_usage(a,b)		do { } while (0)
diff --git a/include/linux/hid.h b/include/linux/hid.h
index e14b465b114..af1b86d46f6 100644
--- a/include/linux/hid.h
+++ b/include/linux/hid.h
@@ -282,6 +282,7 @@ struct hid_item {
 #define HID_QUIRK_BADPAD			0x00000020
 #define HID_QUIRK_MULTI_INPUT			0x00000040
 #define HID_QUIRK_HIDINPUT_FORCE		0x00000080
+#define HID_QUIRK_NO_EMPTY_INPUT		0x00000100
 #define HID_QUIRK_SKIP_OUTPUT_REPORTS		0x00010000
 #define HID_QUIRK_FULLSPEED_INTERVAL		0x10000000
 #define HID_QUIRK_NO_INIT_REPORTS		0x20000000
@@ -456,7 +457,8 @@ struct hid_device {							/* device report descriptor */
 	unsigned country;						/* HID country */
 	struct hid_report_enum report_enum[HID_REPORT_TYPES];
 
-	struct semaphore driver_lock;					/* protects the current driver */
+	struct semaphore driver_lock;					/* protects the current driver, except during input */
+	struct semaphore driver_input_lock;				/* protects the current driver */
 	struct device dev;						/* device */
 	struct hid_driver *driver;
 	struct hid_ll_driver *ll_driver;
@@ -477,6 +479,7 @@ struct hid_device {							/* device report descriptor */
 	unsigned int status;						/* see STAT flags above */
 	unsigned claimed;						/* Claimed by hidinput, hiddev? */
 	unsigned quirks;						/* Various quirks the device can pull on us */
+	bool io_started;						/* Protected by driver_lock. If IO has started */
 
 	struct list_head inputs;					/* The list of inputs */
 	void *hiddev;							/* The hiddev structure */
@@ -512,6 +515,7 @@ struct hid_device {							/* device report descriptor */
 	struct dentry *debug_rdesc;
 	struct dentry *debug_events;
 	struct list_head debug_list;
+	struct mutex debug_list_lock;
 	wait_queue_head_t debug_wait;
 };
 
@@ -599,6 +603,10 @@ struct hid_usage_id {
  * @resume: invoked on resume if device was not reset (NULL means nop)
  * @reset_resume: invoked on resume if device was reset (NULL means nop)
  *
+ * probe should return -errno on error, or 0 on success. During probe,
+ * input will not be passed to raw_event unless hid_device_io_start is
+ * called.
+ *
  * raw_event and event should return 0 on no action performed, 1 when no
  * further processing should be done and negative on error
  *
@@ -662,6 +670,9 @@ struct hid_driver {
  * @hidinput_input_event: event input event (e.g. ff or leds)
  * @parse: this method is called only once to parse the device data,
  *	   shouldn't allocate anything to not leak memory
+ * @request: send report request to device (e.g. feature report)
+ * @wait: wait for buffered io to complete (send/recv reports)
+ * @idle: send idle request to device
  */
 struct hid_ll_driver {
 	int (*start)(struct hid_device *hdev);
@@ -676,6 +687,13 @@ struct hid_ll_driver {
 			unsigned int code, int value);
 
 	int (*parse)(struct hid_device *hdev);
+
+	void (*request)(struct hid_device *hdev,
+			struct hid_report *report, int reqtype);
+
+	int (*wait)(struct hid_device *hdev);
+	int (*idle)(struct hid_device *hdev, int report, int idle, int reqtype);
+
 };
 
 #define	PM_HINT_FULLON	1<<5
@@ -738,6 +756,44 @@ const struct hid_device_id *hid_match_id(struct hid_device *hdev,
 s32 hid_snto32(__u32 value, unsigned n);
 
 /**
+ * hid_device_io_start - enable HID input during probe, remove
+ *
+ * @hid - the device
+ *
+ * This should only be called during probe or remove and only be
+ * called by the thread calling probe or remove. It will allow
+ * incoming packets to be delivered to the driver.
+ */
+static inline void hid_device_io_start(struct hid_device *hid) {
+	if (hid->io_started) {
+		dev_warn(&hid->dev, "io already started");
+		return;
+	}
+	hid->io_started = true;
+	up(&hid->driver_input_lock);
+}
+
+/**
+ * hid_device_io_stop - disable HID input during probe, remove
+ *
+ * @hid - the device
+ *
+ * Should only be called after hid_device_io_start. It will prevent
+ * incoming packets from going to the driver for the duration of
+ * probe, remove. If called during probe, packets will still go to the
+ * driver after probe is complete. This function should only be called
+ * by the thread calling probe or remove.
+ */
+static inline void hid_device_io_stop(struct hid_device *hid) {
+	if (!hid->io_started) {
+		dev_warn(&hid->dev, "io already stopped");
+		return;
+	}
+	hid->io_started = false;
+	down(&hid->driver_input_lock);
+}
+
+/**
  * hid_map_usage - map usage input bits
  *
  * @hidinput: hidinput which we are interested in
@@ -883,6 +939,49 @@ static inline int hid_hw_power(struct hid_device *hdev, int level)
 	return hdev->ll_driver->power ? hdev->ll_driver->power(hdev, level) : 0;
 }
 
+
+/**
+ * hid_hw_request - send report request to device
+ *
+ * @hdev: hid device
+ * @report: report to send
+ * @reqtype: hid request type
+ */
+static inline void hid_hw_request(struct hid_device *hdev,
+				  struct hid_report *report, int reqtype)
+{
+	if (hdev->ll_driver->request)
+		hdev->ll_driver->request(hdev, report, reqtype);
+}
+
+/**
+ * hid_hw_idle - send idle request to device
+ *
+ * @hdev: hid device
+ * @report: report to control
+ * @idle: idle state
+ * @reqtype: hid request type
+ */
+static inline int hid_hw_idle(struct hid_device *hdev, int report, int idle,
+		int reqtype)
+{
+	if (hdev->ll_driver->idle)
+		return hdev->ll_driver->idle(hdev, report, idle, reqtype);
+
+	return 0;
+}
+
+/**
+ * hid_hw_wait - wait for buffered io to complete
+ *
+ * @hdev: hid device
+ */
+static inline void hid_hw_wait(struct hid_device *hdev)
+{
+	if (hdev->ll_driver->wait)
+		hdev->ll_driver->wait(hdev);
+}
+
 int hid_report_raw_event(struct hid_device *hid, int type, u8 *data, int size,
 		int interrupt);
 
diff --git a/include/linux/hrtimer.h b/include/linux/hrtimer.h
index cc07d2777bb..d19a5c2d227 100644
--- a/include/linux/hrtimer.h
+++ b/include/linux/hrtimer.h
@@ -157,6 +157,7 @@ enum  hrtimer_base_type {
 	HRTIMER_BASE_MONOTONIC,
 	HRTIMER_BASE_REALTIME,
 	HRTIMER_BASE_BOOTTIME,
+	HRTIMER_BASE_TAI,
 	HRTIMER_MAX_CLOCK_BASES,
 };
 
@@ -327,7 +328,9 @@ extern ktime_t ktime_get(void);
 extern ktime_t ktime_get_real(void);
 extern ktime_t ktime_get_boottime(void);
 extern ktime_t ktime_get_monotonic_offset(void);
-extern ktime_t ktime_get_update_offsets(ktime_t *offs_real, ktime_t *offs_boot);
+extern ktime_t ktime_get_clocktai(void);
+extern ktime_t ktime_get_update_offsets(ktime_t *offs_real, ktime_t *offs_boot,
+					 ktime_t *offs_tai);
 
 DECLARE_PER_CPU(struct tick_device, tick_cpu_device);
 
diff --git a/include/linux/huge_mm.h b/include/linux/huge_mm.h
index ee1c244a62a..528454c2caa 100644
--- a/include/linux/huge_mm.h
+++ b/include/linux/huge_mm.h
@@ -99,7 +99,11 @@ extern int copy_pte_range(struct mm_struct *dst_mm, struct mm_struct *src_mm,
 extern int handle_pte_fault(struct mm_struct *mm,
 			    struct vm_area_struct *vma, unsigned long address,
 			    pte_t *pte, pmd_t *pmd, unsigned int flags);
-extern int split_huge_page(struct page *page);
+extern int split_huge_page_to_list(struct page *page, struct list_head *list);
+static inline int split_huge_page(struct page *page)
+{
+	return split_huge_page_to_list(page, NULL);
+}
 extern void __split_huge_page_pmd(struct vm_area_struct *vma,
 		unsigned long address, pmd_t *pmd);
 #define split_huge_page_pmd(__vma, __address, __pmd)			\
@@ -186,6 +190,11 @@ extern int do_huge_pmd_numa_page(struct mm_struct *mm, struct vm_area_struct *vm
 #define transparent_hugepage_enabled(__vma) 0
 
 #define transparent_hugepage_flags 0UL
+static inline int
+split_huge_page_to_list(struct page *page, struct list_head *list)
+{
+	return 0;
+}
 static inline int split_huge_page(struct page *page)
 {
 	return 0;
diff --git a/include/linux/hugetlb.h b/include/linux/hugetlb.h
index 16e4e9a643f..3a62df310f2 100644
--- a/include/linux/hugetlb.h
+++ b/include/linux/hugetlb.h
@@ -58,6 +58,7 @@ void __unmap_hugepage_range(struct mmu_gather *tlb, struct vm_area_struct *vma,
 int hugetlb_prefault(struct address_space *, struct vm_area_struct *);
 void hugetlb_report_meminfo(struct seq_file *);
 int hugetlb_report_node_meminfo(int, char *);
+void hugetlb_show_meminfo(void);
 unsigned long hugetlb_total_pages(void);
 int hugetlb_fault(struct mm_struct *mm, struct vm_area_struct *vma,
 			unsigned long address, unsigned int flags);
@@ -114,6 +115,9 @@ static inline void hugetlb_report_meminfo(struct seq_file *m)
 {
 }
 #define hugetlb_report_node_meminfo(n, buf)	0
+static inline void hugetlb_show_meminfo(void)
+{
+}
 #define follow_huge_pmd(mm, addr, pmd, write)	NULL
 #define follow_huge_pud(mm, addr, pud, write)	NULL
 #define prepare_hugepage_range(file, addr, len)	(-EINVAL)
diff --git a/include/linux/hyperv.h b/include/linux/hyperv.h
index df77ba9a816..c2559847d7e 100644
--- a/include/linux/hyperv.h
+++ b/include/linux/hyperv.h
@@ -27,6 +27,63 @@
 
 #include <linux/types.h>
 
+
+/*
+ * Implementation of host controlled snapshot of the guest.
+ */
+
+#define VSS_OP_REGISTER 128
+
+enum hv_vss_op {
+	VSS_OP_CREATE = 0,
+	VSS_OP_DELETE,
+	VSS_OP_HOT_BACKUP,
+	VSS_OP_GET_DM_INFO,
+	VSS_OP_BU_COMPLETE,
+	/*
+	 * Following operations are only supported with IC version >= 5.0
+	 */
+	VSS_OP_FREEZE, /* Freeze the file systems in the VM */
+	VSS_OP_THAW, /* Unfreeze the file systems */
+	VSS_OP_AUTO_RECOVER,
+	VSS_OP_COUNT /* Number of operations, must be last */
+};
+
+
+/*
+ * Header for all VSS messages.
+ */
+struct hv_vss_hdr {
+	__u8 operation;
+	__u8 reserved[7];
+} __attribute__((packed));
+
+
+/*
+ * Flag values for the hv_vss_check_feature. Linux supports only
+ * one value.
+ */
+#define VSS_HBU_NO_AUTO_RECOVERY	0x00000005
+
+struct hv_vss_check_feature {
+	__u32 flags;
+} __attribute__((packed));
+
+struct hv_vss_check_dm_info {
+	__u32 flags;
+} __attribute__((packed));
+
+struct hv_vss_msg {
+	union {
+		struct hv_vss_hdr vss_hdr;
+		int error;
+	};
+	union {
+		struct hv_vss_check_feature vss_cf;
+		struct hv_vss_check_dm_info dm_info;
+	};
+} __attribute__((packed));
+
 /*
  * An implementation of HyperV key value pair (KVP) functionality for Linux.
  *
@@ -1253,6 +1310,25 @@ void vmbus_driver_unregister(struct hv_driver *hv_driver);
 		}
 
 /*
+ * VSS (Backup/Restore) GUID
+ */
+#define HV_VSS_GUID \
+	.guid = { \
+			0x29, 0x2e, 0xfa, 0x35, 0x23, 0xea, 0x36, 0x42, \
+			0x96, 0xae, 0x3a, 0x6e, 0xba, 0xcb, 0xa4,  0x40 \
+		}
+/*
+ * Synthetic Video GUID
+ * {DA0A7802-E377-4aac-8E77-0558EB1073F8}
+ */
+#define HV_SYNTHVID_GUID \
+	.guid = { \
+			0x02, 0x78, 0x0a, 0xda, 0x77, 0xe3, 0xac, 0x4a, \
+			0x8e, 0x77, 0x05, 0x58, 0xeb, 0x10, 0x73, 0xf8 \
+		}
+
+
+/*
  * Common header for Hyper-V ICs
  */
 
@@ -1356,6 +1432,10 @@ int hv_kvp_init(struct hv_util_service *);
 void hv_kvp_deinit(void);
 void hv_kvp_onchannelcallback(void *);
 
+int hv_vss_init(struct hv_util_service *);
+void hv_vss_deinit(void);
+void hv_vss_onchannelcallback(void *);
+
 /*
  * Negotiated version with the Host.
  */
diff --git a/include/linux/idr.h b/include/linux/idr.h
index 2640c7e99e5..a470ac3ef49 100644
--- a/include/linux/idr.h
+++ b/include/linux/idr.h
@@ -42,6 +42,7 @@ struct idr {
 	struct idr_layer	*id_free;
 	int			layers;	/* only valid w/o concurrent changes */
 	int			id_free_cnt;
+	int			cur;	/* current pos for cyclic allocation */
 	spinlock_t		lock;
 };
 
@@ -75,6 +76,7 @@ struct idr {
 void *idr_find_slowpath(struct idr *idp, int id);
 void idr_preload(gfp_t gfp_mask);
 int idr_alloc(struct idr *idp, void *ptr, int start, int end, gfp_t gfp_mask);
+int idr_alloc_cyclic(struct idr *idr, void *ptr, int start, int end, gfp_t gfp_mask);
 int idr_for_each(struct idr *idp,
 		 int (*fn)(int id, void *p, void *data), void *data);
 void *idr_get_next(struct idr *idp, int *nextid);
diff --git a/include/linux/iio/adc/ad_sigma_delta.h b/include/linux/iio/adc/ad_sigma_delta.h
index 2e4eab9868a..e7fdec4db9d 100644
--- a/include/linux/iio/adc/ad_sigma_delta.h
+++ b/include/linux/iio/adc/ad_sigma_delta.h
@@ -133,9 +133,9 @@ int ad_sd_validate_trigger(struct iio_dev *indio_dev, struct iio_trigger *trig);
 		.channel2 = (_channel2), \
 		.address = (_address), \
 		.extend_name = (_extend_name), \
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
-			IIO_CHAN_INFO_SCALE_SHARED_BIT | \
-			IIO_CHAN_INFO_OFFSET_SEPARATE_BIT, \
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+			BIT(IIO_CHAN_INFO_OFFSET), \
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \
 		.scan_index = (_si), \
 		.scan_type = { \
 			.sign = 'u', \
diff --git a/include/linux/iio/common/st_sensors.h b/include/linux/iio/common/st_sensors.h
index 8bd12be0b02..172c5b23cb8 100644
--- a/include/linux/iio/common/st_sensors.h
+++ b/include/linux/iio/common/st_sensors.h
@@ -15,6 +15,7 @@
 #include <linux/spi/spi.h>
 #include <linux/irqreturn.h>
 #include <linux/iio/trigger.h>
+#include <linux/bitops.h>
 
 #define ST_SENSORS_TX_MAX_LENGTH		2
 #define ST_SENSORS_RX_MAX_LENGTH		6
@@ -45,8 +46,8 @@
 { \
 	.type = device_type, \
 	.modified = 1, \
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
-			IIO_CHAN_INFO_SCALE_SEPARATE_BIT, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+			BIT(IIO_CHAN_INFO_SCALE), \
 	.scan_index = index, \
 	.channel2 = mod, \
 	.address = addr, \
diff --git a/include/linux/iio/gyro/itg3200.h b/include/linux/iio/gyro/itg3200.h
index c53f16914b7..2a820850f28 100644
--- a/include/linux/iio/gyro/itg3200.h
+++ b/include/linux/iio/gyro/itg3200.h
@@ -149,6 +149,6 @@ static inline void itg3200_buffer_unconfigure(struct iio_dev *indio_dev)
 {
 }
 
-#endif  /* CONFIG_IIO_RING_BUFFER */
+#endif  /* CONFIG_IIO_BUFFER */
 
 #endif /* ITG3200_H_ */
diff --git a/include/linux/iio/iio.h b/include/linux/iio/iio.h
index da8c776ba0b..8d171f42763 100644
--- a/include/linux/iio/iio.h
+++ b/include/linux/iio/iio.h
@@ -38,76 +38,6 @@ enum iio_chan_info_enum {
 	IIO_CHAN_INFO_HYSTERESIS,
 };
 
-#define IIO_CHAN_INFO_SHARED_BIT(type) BIT(type*2)
-#define IIO_CHAN_INFO_SEPARATE_BIT(type) BIT(type*2 + 1)
-#define IIO_CHAN_INFO_BITS(type) (IIO_CHAN_INFO_SHARED_BIT(type) | \
-				    IIO_CHAN_INFO_SEPARATE_BIT(type))
-
-#define IIO_CHAN_INFO_RAW_SEPARATE_BIT			\
-	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_RAW)
-#define IIO_CHAN_INFO_PROCESSED_SEPARATE_BIT			\
-	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_PROCESSED)
-#define IIO_CHAN_INFO_SCALE_SEPARATE_BIT		\
-	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_SCALE)
-#define IIO_CHAN_INFO_SCALE_SHARED_BIT			\
-	IIO_CHAN_INFO_SHARED_BIT(IIO_CHAN_INFO_SCALE)
-#define IIO_CHAN_INFO_OFFSET_SEPARATE_BIT			\
-	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_OFFSET)
-#define IIO_CHAN_INFO_OFFSET_SHARED_BIT			\
-	IIO_CHAN_INFO_SHARED_BIT(IIO_CHAN_INFO_OFFSET)
-#define IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT			\
-	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_CALIBSCALE)
-#define IIO_CHAN_INFO_CALIBSCALE_SHARED_BIT			\
-	IIO_CHAN_INFO_SHARED_BIT(IIO_CHAN_INFO_CALIBSCALE)
-#define IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT			\
-	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_CALIBBIAS)
-#define IIO_CHAN_INFO_CALIBBIAS_SHARED_BIT			\
-	IIO_CHAN_INFO_SHARED_BIT(IIO_CHAN_INFO_CALIBBIAS)
-#define IIO_CHAN_INFO_PEAK_SEPARATE_BIT			\
-	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_PEAK)
-#define IIO_CHAN_INFO_PEAK_SHARED_BIT			\
-	IIO_CHAN_INFO_SHARED_BIT(IIO_CHAN_INFO_PEAK)
-#define IIO_CHAN_INFO_PEAKSCALE_SEPARATE_BIT			\
-	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_PEAKSCALE)
-#define IIO_CHAN_INFO_PEAKSCALE_SHARED_BIT			\
-	IIO_CHAN_INFO_SHARED_BIT(IIO_CHAN_INFO_PEAKSCALE)
-#define IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW_SEPARATE_BIT	\
-	IIO_CHAN_INFO_SEPARATE_BIT(				\
-		IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW)
-#define IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW_SHARED_BIT	\
-	IIO_CHAN_INFO_SHARED_BIT(				\
-		IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW)
-#define IIO_CHAN_INFO_AVERAGE_RAW_SEPARATE_BIT			\
-	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_AVERAGE_RAW)
-#define IIO_CHAN_INFO_AVERAGE_RAW_SHARED_BIT			\
-	IIO_CHAN_INFO_SHARED_BIT(IIO_CHAN_INFO_AVERAGE_RAW)
-#define IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT \
-	IIO_CHAN_INFO_SHARED_BIT(			       \
-		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY)
-#define IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SEPARATE_BIT \
-	IIO_CHAN_INFO_SEPARATE_BIT(			       \
-		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY)
-#define IIO_CHAN_INFO_SAMP_FREQ_SEPARATE_BIT		\
-	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_SAMP_FREQ)
-#define IIO_CHAN_INFO_SAMP_FREQ_SHARED_BIT			\
-	IIO_CHAN_INFO_SHARED_BIT(IIO_CHAN_INFO_SAMP_FREQ)
-#define IIO_CHAN_INFO_FREQUENCY_SEPARATE_BIT			\
-	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_FREQUENCY)
-#define IIO_CHAN_INFO_FREQUENCY_SHARED_BIT			\
-	IIO_CHAN_INFO_SHARED_BIT(IIO_CHAN_INFO_FREQUENCY)
-#define IIO_CHAN_INFO_PHASE_SEPARATE_BIT			\
-	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_PHASE)
-#define IIO_CHAN_INFO_PHASE_SHARED_BIT			\
-	IIO_CHAN_INFO_SHARED_BIT(IIO_CHAN_INFO_PHASE)
-#define IIO_CHAN_INFO_HARDWAREGAIN_SEPARATE_BIT			\
-	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_HARDWAREGAIN)
-#define IIO_CHAN_INFO_HARDWAREGAIN_SHARED_BIT			\
-	IIO_CHAN_INFO_SHARED_BIT(IIO_CHAN_INFO_HARDWAREGAIN)
-#define IIO_CHAN_INFO_HYSTERESIS_SEPARATE_BIT			\
-	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_HYSTERESIS)
-#define IIO_CHAN_INFO_HYSTERESIS_SHARED_BIT			\
-	IIO_CHAN_INFO_SHARED_BIT(IIO_CHAN_INFO_HYSTERESIS)
-
 enum iio_endian {
 	IIO_CPU,
 	IIO_BE,
@@ -218,6 +148,10 @@ ssize_t iio_enum_write(struct iio_dev *indio_dev,
  *			endianness:	little or big endian
  * @info_mask:		What information is to be exported about this channel.
  *			This includes calibbias, scale etc.
+ * @info_mask_separate: What information is to be exported that is specific to
+ *			this channel.
+ * @info_mask_shared_by_type: What information is to be exported that is shared
+*			by all channels of the same type.
  * @event_mask:		What events can this channel produce.
  * @ext_info:		Array of extended info attributes for this channel.
  *			The array is NULL terminated, the last element should
@@ -253,6 +187,8 @@ struct iio_chan_spec {
 		enum iio_endian endianness;
 	} scan_type;
 	long			info_mask;
+	long			info_mask_separate;
+	long			info_mask_shared_by_type;
 	long			event_mask;
 	const struct iio_chan_spec_ext_info *ext_info;
 	const char		*extend_name;
@@ -275,7 +211,8 @@ struct iio_chan_spec {
 static inline bool iio_channel_has_info(const struct iio_chan_spec *chan,
 	enum iio_chan_info_enum type)
 {
-	return chan->info_mask & IIO_CHAN_INFO_BITS(type);
+	return (chan->info_mask_separate & type) |
+	       (chan->info_mask_shared_by_type & type);
 }
 
 #define IIO_ST(si, rb, sb, sh)						\
diff --git a/include/linux/iio/imu/adis.h b/include/linux/iio/imu/adis.h
index ff781dca2e9..b665dc7f017 100644
--- a/include/linux/iio/imu/adis.h
+++ b/include/linux/iio/imu/adis.h
@@ -162,8 +162,8 @@ int adis_single_conversion(struct iio_dev *indio_dev,
 	.indexed = 1, \
 	.channel = (chan), \
 	.extend_name = name, \
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
-		IIO_CHAN_INFO_SCALE_SEPARATE_BIT, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+		BIT(IIO_CHAN_INFO_SCALE), \
 	.address = (addr), \
 	.scan_index = (si), \
 	.scan_type = { \
@@ -184,9 +184,9 @@ int adis_single_conversion(struct iio_dev *indio_dev,
 	.type = IIO_TEMP, \
 	.indexed = 1, \
 	.channel = 0, \
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
-		IIO_CHAN_INFO_SCALE_SEPARATE_BIT | \
-		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+		BIT(IIO_CHAN_INFO_SCALE) | \
+		BIT(IIO_CHAN_INFO_OFFSET), \
 	.address = (addr), \
 	.scan_index = (si), \
 	.scan_type = { \
@@ -197,13 +197,13 @@ int adis_single_conversion(struct iio_dev *indio_dev,
 	}, \
 }
 
-#define ADIS_MOD_CHAN(_type, mod, addr, si, info, bits) { \
+#define ADIS_MOD_CHAN(_type, mod, addr, si, info_sep, bits) { \
 	.type = (_type), \
 	.modified = 1, \
 	.channel2 = IIO_MOD_ ## mod, \
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
-		 IIO_CHAN_INFO_SCALE_SHARED_BIT | \
-		 info, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+		 info_sep, \
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \
 	.address = (addr), \
 	.scan_index = (si), \
 	.scan_type = { \
@@ -214,17 +214,17 @@ int adis_single_conversion(struct iio_dev *indio_dev,
 	}, \
 }
 
-#define ADIS_ACCEL_CHAN(mod, addr, si, info, bits) \
-	ADIS_MOD_CHAN(IIO_ACCEL, mod, addr, si, info, bits)
+#define ADIS_ACCEL_CHAN(mod, addr, si, info_sep, bits) \
+	ADIS_MOD_CHAN(IIO_ACCEL, mod, addr, si, info_sep, bits)
 
-#define ADIS_GYRO_CHAN(mod, addr, si, info, bits) \
-	ADIS_MOD_CHAN(IIO_ANGL_VEL, mod, addr, si, info, bits)
+#define ADIS_GYRO_CHAN(mod, addr, si, info_sep, bits) \
+	ADIS_MOD_CHAN(IIO_ANGL_VEL, mod, addr, si, info_sep, bits)
 
-#define ADIS_INCLI_CHAN(mod, addr, si, info, bits) \
-	ADIS_MOD_CHAN(IIO_INCLI, mod, addr, si, info, bits)
+#define ADIS_INCLI_CHAN(mod, addr, si, info_sep, bits) \
+	ADIS_MOD_CHAN(IIO_INCLI, mod, addr, si, info_sep, bits)
 
-#define ADIS_ROT_CHAN(mod, addr, si, info, bits) \
-	ADIS_MOD_CHAN(IIO_ROT, mod, addr, si, info, bits)
+#define ADIS_ROT_CHAN(mod, addr, si, info_sep, bits) \
+	ADIS_MOD_CHAN(IIO_ROT, mod, addr, si, info_sep, bits)
 
 #ifdef CONFIG_IIO_ADIS_LIB_BUFFER
 
diff --git a/include/linux/iio/trigger.h b/include/linux/iio/trigger.h
index c66e0a96f6e..3869c525b05 100644
--- a/include/linux/iio/trigger.h
+++ b/include/linux/iio/trigger.h
@@ -44,7 +44,6 @@ struct iio_trigger_ops {
  * @id:			[INTERN] unique id number
  * @name:		[DRIVER] unique name
  * @dev:		[DRIVER] associated device (if relevant)
- * @private_data:	[DRIVER] device specific data
  * @list:		[INTERN] used in maintenance of global trigger list
  * @alloc_list:		[DRIVER] used for driver specific trigger list
  * @use_count:		use count for the trigger
@@ -60,7 +59,6 @@ struct iio_trigger {
 	const char			*name;
 	struct device			dev;
 
-	void				*private_data;
 	struct list_head		list;
 	struct list_head		alloc_list;
 	int use_count;
@@ -92,6 +90,30 @@ static inline void iio_trigger_get(struct iio_trigger *trig)
 }
 
 /**
+ * iio_device_set_drvdata() - Set trigger driver data
+ * @trig: IIO trigger structure
+ * @data: Driver specific data
+ *
+ * Allows to attach an arbitrary pointer to an IIO trigger, which can later be
+ * retrieved by iio_trigger_get_drvdata().
+ */
+static inline void iio_trigger_set_drvdata(struct iio_trigger *trig, void *data)
+{
+	dev_set_drvdata(&trig->dev, data);
+}
+
+/**
+ * iio_trigger_get_drvdata() - Get trigger driver data
+ * @trig: IIO trigger structure
+ *
+ * Returns the data previously set with iio_trigger_set_drvdata()
+ */
+static inline void *iio_trigger_get_drvdata(struct iio_trigger *trig)
+{
+	return dev_get_drvdata(&trig->dev);
+}
+
+/**
  * iio_trigger_register() - register a trigger with the IIO core
  * @trig_info:	trigger to be registered
  **/
diff --git a/include/linux/ima.h b/include/linux/ima.h
index 86c361e947b..1b7f268cddc 100644
--- a/include/linux/ima.h
+++ b/include/linux/ima.h
@@ -46,7 +46,7 @@ static inline int ima_module_check(struct file *file)
 	return 0;
 }
 
-#endif /* CONFIG_IMA_H */
+#endif /* CONFIG_IMA */
 
 #ifdef CONFIG_IMA_APPRAISE
 extern void ima_inode_post_setattr(struct dentry *dentry);
@@ -72,5 +72,5 @@ static inline int ima_inode_removexattr(struct dentry *dentry,
 {
 	return 0;
 }
-#endif /* CONFIG_IMA_APPRAISE_H */
+#endif /* CONFIG_IMA_APPRAISE */
 #endif /* _LINUX_IMA_H */
diff --git a/include/linux/integrity.h b/include/linux/integrity.h
index 66c5fe9550a..83222cebd47 100644
--- a/include/linux/integrity.h
+++ b/include/linux/integrity.h
@@ -36,5 +36,5 @@ static inline void integrity_inode_free(struct inode *inode)
 {
 	return;
 }
-#endif /* CONFIG_INTEGRITY_H */
+#endif /* CONFIG_INTEGRITY */
 #endif /* _LINUX_INTEGRITY_H */
diff --git a/include/linux/ioport.h b/include/linux/ioport.h
index 85ac9b9b72a..89b7c24a36e 100644
--- a/include/linux/ioport.h
+++ b/include/linux/ioport.h
@@ -192,6 +192,10 @@ extern struct resource * __request_region(struct resource *,
 extern int __check_region(struct resource *, resource_size_t, resource_size_t);
 extern void __release_region(struct resource *, resource_size_t,
 				resource_size_t);
+#ifdef CONFIG_MEMORY_HOTREMOVE
+extern int release_mem_region_adjustable(struct resource *, resource_size_t,
+				resource_size_t);
+#endif
 
 static inline int __deprecated check_region(resource_size_t s,
 						resource_size_t n)
diff --git a/include/linux/ipack.h b/include/linux/ipack.h
index fea12cbb2ae..1888e06ddf6 100644
--- a/include/linux/ipack.h
+++ b/include/linux/ipack.h
@@ -207,19 +207,41 @@ int ipack_driver_register(struct ipack_driver *edrv, struct module *owner,
 void ipack_driver_unregister(struct ipack_driver *edrv);
 
 /**
- *	ipack_device_register -- register an IPack device with the kernel
- *	@dev: the new device to register.
+ *	ipack_device_init -- initialize an IPack device
+ * @dev: the new device to initialize.
  *
- *	Register a new IPack device ("module" in IndustryPack jargon). The call
- *	is done by the carrier driver.  The carrier should populate the fields
- *	bus and slot as well as the region array of @dev prior to calling this
- *	function.  The rest of the fields will be allocated and populated
- *	during registration.
+ * Initialize a new IPack device ("module" in IndustryPack jargon). The call
+ * is done by the carrier driver.  The carrier should populate the fields
+ * bus and slot as well as the region array of @dev prior to calling this
+ * function.  The rest of the fields will be allocated and populated
+ * during initalization.
  *
- *	Return zero on success or error code on failure.
+ * Return zero on success or error code on failure.
+ *
+ * NOTE: _Never_ directly free @dev after calling this function, even
+ * if it returned an error! Always use ipack_put_device() to give up the
+ * reference initialized in this function instead.
+ */
+int ipack_device_init(struct ipack_device *dev);
+
+/**
+ *	ipack_device_add -- Add an IPack device
+ * @dev: the new device to add.
+ *
+ * Add a new IPack device. The call is done by the carrier driver
+ * after calling ipack_device_init().
+ *
+ * Return zero on success or error code on failure.
+ *
+ * NOTE: _Never_ directly free @dev after calling this function, even
+ * if it returned an error! Always use ipack_put_device() to give up the
+ * reference initialized in this function instead.
  */
-int ipack_device_register(struct ipack_device *dev);
-void ipack_device_unregister(struct ipack_device *dev);
+int ipack_device_add(struct ipack_device *dev);
+void ipack_device_del(struct ipack_device *dev);
+
+void ipack_get_device(struct ipack_device *dev);
+void ipack_put_device(struct ipack_device *dev);
 
 /**
  * DEFINE_IPACK_DEVICE_TABLE - macro used to describe a IndustryPack table
diff --git a/include/linux/jiffies.h b/include/linux/jiffies.h
index 82ed068b1eb..8fb8edf1241 100644
--- a/include/linux/jiffies.h
+++ b/include/linux/jiffies.h
@@ -75,7 +75,6 @@ extern int register_refined_jiffies(long clock_tick_rate);
  */
 extern u64 __jiffy_data jiffies_64;
 extern unsigned long volatile __jiffy_data jiffies;
-extern seqlock_t jiffies_lock;
 
 #if (BITS_PER_LONG < 64)
 u64 get_jiffies_64(void);
diff --git a/include/linux/kernel.h b/include/linux/kernel.h
index 79fdd80a42d..2dac79c3919 100644
--- a/include/linux/kernel.h
+++ b/include/linux/kernel.h
@@ -486,6 +486,8 @@ enum ftrace_dump_mode {
 void tracing_on(void);
 void tracing_off(void);
 int tracing_is_on(void);
+void tracing_snapshot(void);
+void tracing_snapshot_alloc(void);
 
 extern void tracing_start(void);
 extern void tracing_stop(void);
@@ -515,10 +517,32 @@ do {									\
  *
  * This is intended as a debugging tool for the developer only.
  * Please refrain from leaving trace_printks scattered around in
- * your code.
+ * your code. (Extra memory is used for special buffers that are
+ * allocated when trace_printk() is used)
+ *
+ * A little optization trick is done here. If there's only one
+ * argument, there's no need to scan the string for printf formats.
+ * The trace_puts() will suffice. But how can we take advantage of
+ * using trace_puts() when trace_printk() has only one argument?
+ * By stringifying the args and checking the size we can tell
+ * whether or not there are args. __stringify((__VA_ARGS__)) will
+ * turn into "()\0" with a size of 3 when there are no args, anything
+ * else will be bigger. All we need to do is define a string to this,
+ * and then take its size and compare to 3. If it's bigger, use
+ * do_trace_printk() otherwise, optimize it to trace_puts(). Then just
+ * let gcc optimize the rest.
  */
 
-#define trace_printk(fmt, args...)					\
+#define trace_printk(fmt, ...)				\
+do {							\
+	char _______STR[] = __stringify((__VA_ARGS__));	\
+	if (sizeof(_______STR) > 3)			\
+		do_trace_printk(fmt, ##__VA_ARGS__);	\
+	else						\
+		trace_puts(fmt);			\
+} while (0)
+
+#define do_trace_printk(fmt, args...)					\
 do {									\
 	static const char *trace_printk_fmt				\
 		__attribute__((section("__trace_printk_fmt"))) =	\
@@ -538,7 +562,45 @@ int __trace_bprintk(unsigned long ip, const char *fmt, ...);
 extern __printf(2, 3)
 int __trace_printk(unsigned long ip, const char *fmt, ...);
 
-extern void trace_dump_stack(void);
+/**
+ * trace_puts - write a string into the ftrace buffer
+ * @str: the string to record
+ *
+ * Note: __trace_bputs is an internal function for trace_puts and
+ *       the @ip is passed in via the trace_puts macro.
+ *
+ * This is similar to trace_printk() but is made for those really fast
+ * paths that a developer wants the least amount of "Heisenbug" affects,
+ * where the processing of the print format is still too much.
+ *
+ * This function allows a kernel developer to debug fast path sections
+ * that printk is not appropriate for. By scattering in various
+ * printk like tracing in the code, a developer can quickly see
+ * where problems are occurring.
+ *
+ * This is intended as a debugging tool for the developer only.
+ * Please refrain from leaving trace_puts scattered around in
+ * your code. (Extra memory is used for special buffers that are
+ * allocated when trace_puts() is used)
+ *
+ * Returns: 0 if nothing was written, positive # if string was.
+ *  (1 when __trace_bputs is used, strlen(str) when __trace_puts is used)
+ */
+
+extern int __trace_bputs(unsigned long ip, const char *str);
+extern int __trace_puts(unsigned long ip, const char *str, int size);
+#define trace_puts(str) ({						\
+	static const char *trace_printk_fmt				\
+		__attribute__((section("__trace_printk_fmt"))) =	\
+		__builtin_constant_p(str) ? str : NULL;			\
+									\
+	if (__builtin_constant_p(str))					\
+		__trace_bputs(_THIS_IP_, trace_printk_fmt);		\
+	else								\
+		__trace_puts(_THIS_IP_, str, strlen(str));		\
+})
+
+extern void trace_dump_stack(int skip);
 
 /*
  * The double __builtin_constant_p is because gcc will give us an error
@@ -573,6 +635,8 @@ static inline void trace_dump_stack(void) { }
 static inline void tracing_on(void) { }
 static inline void tracing_off(void) { }
 static inline int tracing_is_on(void) { return 0; }
+static inline void tracing_snapshot(void) { }
+static inline void tracing_snapshot_alloc(void) { }
 
 static inline __printf(1, 2)
 int trace_printk(const char *fmt, ...)
diff --git a/include/linux/kexec.h b/include/linux/kexec.h
index d2e6927bbaa..d78d28a733b 100644
--- a/include/linux/kexec.h
+++ b/include/linux/kexec.h
@@ -200,6 +200,8 @@ extern size_t vmcoreinfo_max_size;
 
 int __init parse_crashkernel(char *cmdline, unsigned long long system_ram,
 		unsigned long long *crash_size, unsigned long long *crash_base);
+int parse_crashkernel_high(char *cmdline, unsigned long long system_ram,
+		unsigned long long *crash_size, unsigned long long *crash_base);
 int parse_crashkernel_low(char *cmdline, unsigned long long system_ram,
 		unsigned long long *crash_size, unsigned long long *crash_base);
 int crash_shrink_memory(unsigned long new_size);
diff --git a/include/linux/kprobes.h b/include/linux/kprobes.h
index 4b6ef4d33cc..ca1d27a0d6a 100644
--- a/include/linux/kprobes.h
+++ b/include/linux/kprobes.h
@@ -29,6 +29,7 @@
  *		<jkenisto@us.ibm.com>  and Prasanna S Panchamukhi
  *		<prasanna@in.ibm.com> added function-return probes.
  */
+#include <linux/compiler.h>	/* for __kprobes */
 #include <linux/linkage.h>
 #include <linux/list.h>
 #include <linux/notifier.h>
@@ -49,16 +50,11 @@
 #define KPROBE_REENTER		0x00000004
 #define KPROBE_HIT_SSDONE	0x00000008
 
-/* Attach to insert probes on any functions which should be ignored*/
-#define __kprobes	__attribute__((__section__(".kprobes.text")))
-
 #else /* CONFIG_KPROBES */
 typedef int kprobe_opcode_t;
 struct arch_specific_insn {
 	int dummy;
 };
-#define __kprobes
-
 #endif /* CONFIG_KPROBES */
 
 struct kprobe;
diff --git a/include/linux/kvm_host.h b/include/linux/kvm_host.h
index cad77fe09d7..c1395825192 100644
--- a/include/linux/kvm_host.h
+++ b/include/linux/kvm_host.h
@@ -518,7 +518,7 @@ int kvm_write_guest(struct kvm *kvm, gpa_t gpa, const void *data,
 int kvm_write_guest_cached(struct kvm *kvm, struct gfn_to_hva_cache *ghc,
 			   void *data, unsigned long len);
 int kvm_gfn_to_hva_cache_init(struct kvm *kvm, struct gfn_to_hva_cache *ghc,
-			      gpa_t gpa);
+			      gpa_t gpa, unsigned long len);
 int kvm_clear_guest_page(struct kvm *kvm, gfn_t gfn, int offset, int len);
 int kvm_clear_guest(struct kvm *kvm, gpa_t gpa, unsigned long len);
 struct kvm_memory_slot *gfn_to_memslot(struct kvm *kvm, gfn_t gfn);
diff --git a/include/linux/kvm_types.h b/include/linux/kvm_types.h
index fa7cc7244cb..b0bcce0ddc9 100644
--- a/include/linux/kvm_types.h
+++ b/include/linux/kvm_types.h
@@ -71,6 +71,7 @@ struct gfn_to_hva_cache {
 	u64 generation;
 	gpa_t gpa;
 	unsigned long hva;
+	unsigned long len;
 	struct kvm_memory_slot *memslot;
 };
 
diff --git a/include/linux/libata.h b/include/linux/libata.h
index 91c9d109e5f..eae7a053dc5 100644
--- a/include/linux/libata.h
+++ b/include/linux/libata.h
@@ -398,6 +398,7 @@ enum {
 	ATA_HORKAGE_NOSETXFER	= (1 << 14),	/* skip SETXFER, SATA only */
 	ATA_HORKAGE_BROKEN_FPDMA_AA	= (1 << 15),	/* skip AA */
 	ATA_HORKAGE_DUMP_ID	= (1 << 16),	/* dump IDENTIFY data */
+	ATA_HORKAGE_MAX_SEC_LBA48 = (1 << 17),	/* Set max sects to 65535 */
 
 	 /* DMA mask for user DMA control: User visible values; DO NOT
 	    renumber */
diff --git a/include/linux/list_bl.h b/include/linux/list_bl.h
index 31f9d75adc5..2eb88556c5c 100644
--- a/include/linux/list_bl.h
+++ b/include/linux/list_bl.h
@@ -125,6 +125,11 @@ static inline void hlist_bl_unlock(struct hlist_bl_head *b)
 	__bit_spin_unlock(0, (unsigned long *)b);
 }
 
+static inline bool hlist_bl_is_locked(struct hlist_bl_head *b)
+{
+	return bit_spin_is_locked(0, (unsigned long *)b);
+}
+
 /**
  * hlist_bl_for_each_entry	- iterate over list of given type
  * @tpos:	the type * to use as a loop cursor.
diff --git a/include/linux/math64.h b/include/linux/math64.h
index b8ba8554472..931a619407b 100644
--- a/include/linux/math64.h
+++ b/include/linux/math64.h
@@ -30,6 +30,15 @@ static inline s64 div_s64_rem(s64 dividend, s32 divisor, s32 *remainder)
 }
 
 /**
+ * div64_u64_rem - unsigned 64bit divide with 64bit divisor
+ */
+static inline u64 div64_u64_rem(u64 dividend, u64 divisor, u64 *remainder)
+{
+	*remainder = dividend % divisor;
+	return dividend / divisor;
+}
+
+/**
  * div64_u64 - unsigned 64bit divide with 64bit divisor
  */
 static inline u64 div64_u64(u64 dividend, u64 divisor)
@@ -61,8 +70,16 @@ static inline u64 div_u64_rem(u64 dividend, u32 divisor, u32 *remainder)
 extern s64 div_s64_rem(s64 dividend, s32 divisor, s32 *remainder);
 #endif
 
+#ifndef div64_u64_rem
+extern u64 div64_u64_rem(u64 dividend, u64 divisor, u64 *remainder);
+#endif
+
 #ifndef div64_u64
-extern u64 div64_u64(u64 dividend, u64 divisor);
+static inline u64 div64_u64(u64 dividend, u64 divisor)
+{
+	u64 remainder;
+	return div64_u64_rem(dividend, divisor, &remainder);
+}
 #endif
 
 #ifndef div64_s64
diff --git a/include/linux/mei_cl_bus.h b/include/linux/mei_cl_bus.h
new file mode 100644
index 00000000000..d14af7b722e
--- /dev/null
+++ b/include/linux/mei_cl_bus.h
@@ -0,0 +1,44 @@
+#ifndef _LINUX_MEI_CL_BUS_H
+#define _LINUX_MEI_CL_BUS_H
+
+#include <linux/device.h>
+#include <linux/uuid.h>
+
+struct mei_cl_device;
+
+struct mei_cl_driver {
+	struct device_driver driver;
+	const char *name;
+
+	const struct mei_cl_device_id *id_table;
+
+	int (*probe)(struct mei_cl_device *dev,
+		     const struct mei_cl_device_id *id);
+	int (*remove)(struct mei_cl_device *dev);
+};
+
+int __mei_cl_driver_register(struct mei_cl_driver *driver,
+				struct module *owner);
+#define mei_cl_driver_register(driver)             \
+	__mei_cl_driver_register(driver, THIS_MODULE)
+
+void mei_cl_driver_unregister(struct mei_cl_driver *driver);
+
+int mei_cl_send(struct mei_cl_device *device, u8 *buf, size_t length);
+int mei_cl_recv(struct mei_cl_device *device, u8 *buf, size_t length);
+
+typedef void (*mei_cl_event_cb_t)(struct mei_cl_device *device,
+			       u32 events, void *context);
+int mei_cl_register_event_cb(struct mei_cl_device *device,
+			  mei_cl_event_cb_t read_cb, void *context);
+
+#define MEI_CL_EVENT_RX 0
+#define MEI_CL_EVENT_TX 1
+
+void *mei_cl_get_drvdata(const struct mei_cl_device *device);
+void mei_cl_set_drvdata(struct mei_cl_device *device, void *data);
+
+int mei_cl_enable_device(struct mei_cl_device *device);
+int mei_cl_disable_device(struct mei_cl_device *device);
+
+#endif /* _LINUX_MEI_CL_BUS_H */
diff --git a/include/linux/memory.h b/include/linux/memory.h
index 45e93b46887..73817af8b48 100644
--- a/include/linux/memory.h
+++ b/include/linux/memory.h
@@ -18,6 +18,7 @@
 #include <linux/node.h>
 #include <linux/compiler.h>
 #include <linux/mutex.h>
+#include <linux/notifier.h>
 
 #define MIN_MEMORY_BLOCK_SIZE     (1UL << SECTION_SIZE_BITS)
 
@@ -114,9 +115,10 @@ extern void unregister_memory_notifier(struct notifier_block *nb);
 extern int register_memory_isolate_notifier(struct notifier_block *nb);
 extern void unregister_memory_isolate_notifier(struct notifier_block *nb);
 extern int register_new_memory(int, struct mem_section *);
+#ifdef CONFIG_MEMORY_HOTREMOVE
 extern int unregister_memory_section(struct mem_section *);
+#endif
 extern int memory_dev_init(void);
-extern int remove_memory_block(unsigned long, struct mem_section *, int);
 extern int memory_notify(unsigned long val, void *v);
 extern int memory_isolate_notify(unsigned long val, void *v);
 extern struct memory_block *find_memory_block_hinted(struct mem_section *,
@@ -127,13 +129,18 @@ enum mem_add_context { BOOT, HOTPLUG };
 #endif /* CONFIG_MEMORY_HOTPLUG_SPARSE */
 
 #ifdef CONFIG_MEMORY_HOTPLUG
-#define hotplug_memory_notifier(fn, pri) {			\
+#define hotplug_memory_notifier(fn, pri) ({		\
 	static __meminitdata struct notifier_block fn##_mem_nb =\
-		{ .notifier_call = fn, .priority = pri };	\
+		{ .notifier_call = fn, .priority = pri };\
 	register_memory_notifier(&fn##_mem_nb);			\
-}
+})
+#define register_hotmemory_notifier(nb)		register_memory_notifier(nb)
+#define unregister_hotmemory_notifier(nb) 	unregister_memory_notifier(nb)
 #else
-#define hotplug_memory_notifier(fn, pri) do { } while (0)
+#define hotplug_memory_notifier(fn, pri)	(0)
+/* These aren't inline functions due to a GCC bug. */
+#define register_hotmemory_notifier(nb)    ({ (void)(nb); 0; })
+#define unregister_hotmemory_notifier(nb)  ({ (void)(nb); })
 #endif
 
 /*
diff --git a/include/linux/memory_hotplug.h b/include/linux/memory_hotplug.h
index b6a3be7d47b..3e622c61092 100644
--- a/include/linux/memory_hotplug.h
+++ b/include/linux/memory_hotplug.h
@@ -97,13 +97,13 @@ extern void __online_page_free(struct page *page);
 #ifdef CONFIG_MEMORY_HOTREMOVE
 extern bool is_pageblock_removable_nolock(struct page *page);
 extern int arch_remove_memory(u64 start, u64 size);
+extern int __remove_pages(struct zone *zone, unsigned long start_pfn,
+	unsigned long nr_pages);
 #endif /* CONFIG_MEMORY_HOTREMOVE */
 
 /* reasonably generic interface to expand the physical pages in a zone  */
 extern int __add_pages(int nid, struct zone *zone, unsigned long start_pfn,
 	unsigned long nr_pages);
-extern int __remove_pages(struct zone *zone, unsigned long start_pfn,
-	unsigned long nr_pages);
 
 #ifdef CONFIG_NUMA
 extern int memory_add_physaddr_to_nid(u64 start);
diff --git a/include/linux/mfd/abx500/ab8500.h b/include/linux/mfd/abx500/ab8500.h
index b5780fd40fe..fb1bf7d6a41 100644
--- a/include/linux/mfd/abx500/ab8500.h
+++ b/include/linux/mfd/abx500/ab8500.h
@@ -365,8 +365,7 @@ struct ab8500 {
 	int it_latchhier_num;
 };
 
-struct regulator_reg_init;
-struct regulator_init_data;
+struct ab8500_regulator_platform_data;
 struct ab8500_gpio_platform_data;
 struct ab8500_codec_platform_data;
 struct ab8500_sysctrl_platform_data;
@@ -376,19 +375,13 @@ struct ab8500_sysctrl_platform_data;
  * @irq_base: start of AB8500 IRQs, AB8500_NR_IRQS will be used
  * @pm_power_off: Should machine pm power off hook be registered or not
  * @init: board-specific initialization after detection of ab8500
- * @num_regulator_reg_init: number of regulator init registers
- * @regulator_reg_init: regulator init registers
- * @num_regulator: number of regulators
  * @regulator: machine-specific constraints for regulators
  */
 struct ab8500_platform_data {
 	int irq_base;
 	bool pm_power_off;
 	void (*init) (struct ab8500 *);
-	int num_regulator_reg_init;
-	struct ab8500_regulator_reg_init *regulator_reg_init;
-	int num_regulator;
-	struct regulator_init_data *regulator;
+	struct ab8500_regulator_platform_data *regulator;
 	struct abx500_gpio_platform_data *gpio;
 	struct ab8500_codec_platform_data *codec;
 	struct ab8500_sysctrl_platform_data *sysctrl;
diff --git a/include/linux/mfd/arizona/core.h b/include/linux/mfd/arizona/core.h
index a710255528d..cc281368dc5 100644
--- a/include/linux/mfd/arizona/core.h
+++ b/include/linux/mfd/arizona/core.h
@@ -100,6 +100,9 @@ struct arizona {
 	struct regmap_irq_chip_data *aod_irq_chip;
 	struct regmap_irq_chip_data *irq_chip;
 
+	bool hpdet_magic;
+	unsigned int hp_ena;
+
 	struct mutex clk_lock;
 	int clk32k_ref;
 
diff --git a/include/linux/mfd/arizona/pdata.h b/include/linux/mfd/arizona/pdata.h
index 455c51d22d6..a0f940987a3 100644
--- a/include/linux/mfd/arizona/pdata.h
+++ b/include/linux/mfd/arizona/pdata.h
@@ -86,6 +86,11 @@ struct arizona_micd_config {
 	bool gpio;
 };
 
+struct arizona_micd_range {
+	int max;  /** Ohms */
+	int key;  /** Key to report to input layer */
+};
+
 struct arizona_pdata {
 	int reset;      /** GPIO controlling /RESET, if any */
 	int ldoena;     /** GPIO controlling LODENA, if any */
@@ -117,12 +122,21 @@ struct arizona_pdata {
 	/** GPIO5 is used for jack detection */
 	bool jd_gpio5;
 
+	/** Internal pull on GPIO5 is disabled when used for jack detection */
+	bool jd_gpio5_nopull;
+
 	/** Use the headphone detect circuit to identify the accessory */
 	bool hpdet_acc_id;
 
+	/** Check for line output with HPDET method */
+	bool hpdet_acc_id_line;
+
 	/** GPIO used for mic isolation with HPDET */
 	int hpdet_id_gpio;
 
+	/** Extra debounce timeout used during initial mic detection (ms) */
+	int micd_detect_debounce;
+
 	/** GPIO for mic detection polarity */
 	int micd_pol_gpio;
 
@@ -135,9 +149,16 @@ struct arizona_pdata {
 	/** Mic detect debounce level */
 	int micd_dbtime;
 
+	/** Mic detect timeout (ms) */
+	int micd_timeout;
+
 	/** Force MICBIAS on for mic detect */
 	bool micd_force_micbias;
 
+	/** Mic detect level parameters */
+	const struct arizona_micd_range *micd_ranges;
+	int num_micd_ranges;
+
 	/** Headset polarity configurations */
 	struct arizona_micd_config *micd_configs;
 	int num_micd_configs;
diff --git a/include/linux/mfd/arizona/registers.h b/include/linux/mfd/arizona/registers.h
index 34035513606..f43aa7c8d04 100644
--- a/include/linux/mfd/arizona/registers.h
+++ b/include/linux/mfd/arizona/registers.h
@@ -124,6 +124,10 @@
 #define ARIZONA_MIC_DETECT_1                     0x2A3
 #define ARIZONA_MIC_DETECT_2                     0x2A4
 #define ARIZONA_MIC_DETECT_3                     0x2A5
+#define ARIZONA_MIC_DETECT_LEVEL_1		 0x2A6
+#define ARIZONA_MIC_DETECT_LEVEL_2		 0x2A7
+#define ARIZONA_MIC_DETECT_LEVEL_3		 0x2A8
+#define ARIZONA_MIC_DETECT_LEVEL_4		 0x2A9
 #define ARIZONA_MIC_NOISE_MIX_CONTROL_1          0x2C3
 #define ARIZONA_ISOLATION_CONTROL                0x2CB
 #define ARIZONA_JACK_DETECT_ANALOGUE             0x2D3
diff --git a/include/linux/mfd/max77693-private.h b/include/linux/mfd/max77693-private.h
index 5b18ecde69b..1aa4f13cdfa 100644
--- a/include/linux/mfd/max77693-private.h
+++ b/include/linux/mfd/max77693-private.h
@@ -106,6 +106,29 @@ enum max77693_muic_reg {
 	MAX77693_MUIC_REG_END,
 };
 
+/* MAX77693 INTMASK1~2 Register */
+#define INTMASK1_ADC1K_SHIFT		3
+#define INTMASK1_ADCERR_SHIFT		2
+#define INTMASK1_ADCLOW_SHIFT		1
+#define INTMASK1_ADC_SHIFT		0
+#define INTMASK1_ADC1K_MASK		(1 << INTMASK1_ADC1K_SHIFT)
+#define INTMASK1_ADCERR_MASK		(1 << INTMASK1_ADCERR_SHIFT)
+#define INTMASK1_ADCLOW_MASK		(1 << INTMASK1_ADCLOW_SHIFT)
+#define INTMASK1_ADC_MASK		(1 << INTMASK1_ADC_SHIFT)
+
+#define INTMASK2_VIDRM_SHIFT		5
+#define INTMASK2_VBVOLT_SHIFT		4
+#define INTMASK2_DXOVP_SHIFT		3
+#define INTMASK2_DCDTMR_SHIFT		2
+#define INTMASK2_CHGDETRUN_SHIFT	1
+#define INTMASK2_CHGTYP_SHIFT		0
+#define INTMASK2_VIDRM_MASK		(1 << INTMASK2_VIDRM_SHIFT)
+#define INTMASK2_VBVOLT_MASK		(1 << INTMASK2_VBVOLT_SHIFT)
+#define INTMASK2_DXOVP_MASK		(1 << INTMASK2_DXOVP_SHIFT)
+#define INTMASK2_DCDTMR_MASK		(1 << INTMASK2_DCDTMR_SHIFT)
+#define INTMASK2_CHGDETRUN_MASK		(1 << INTMASK2_CHGDETRUN_SHIFT)
+#define INTMASK2_CHGTYP_MASK		(1 << INTMASK2_CHGTYP_SHIFT)
+
 /* MAX77693 MUIC - STATUS1~3 Register */
 #define STATUS1_ADC_SHIFT		(0)
 #define STATUS1_ADCLOW_SHIFT		(5)
diff --git a/include/linux/mfd/palmas.h b/include/linux/mfd/palmas.h
index 3bbda22721e..ecddc5173c7 100644
--- a/include/linux/mfd/palmas.h
+++ b/include/linux/mfd/palmas.h
@@ -109,19 +109,6 @@ struct palmas_reg_init {
 	 */
 	int mode_sleep;
 
-	/* tstep is the timestep loaded to the TSTEP register
-	 *
-	 * For SMPS
-	 *
-	 * 0: Jump (no slope control)
-	 * 1: 10mV/us
-	 * 2: 5mV/us
-	 * 3: 2.5mV/us
-	 *
-	 * For LDO unused
-	 */
-	int tstep;
-
 	/* voltage_sel is the bitfield loaded onto the SMPSX_VOLTAGE
 	 * register. Set this is the default voltage set in OTP needs
 	 * to be overridden.
@@ -154,6 +141,12 @@ enum palmas_regulators {
 	PALMAS_REG_LDO9,
 	PALMAS_REG_LDOLN,
 	PALMAS_REG_LDOUSB,
+	/* External regulators */
+	PALMAS_REG_REGEN1,
+	PALMAS_REG_REGEN2,
+	PALMAS_REG_REGEN3,
+	PALMAS_REG_SYSEN1,
+	PALMAS_REG_SYSEN2,
 	/* Total number of regulators */
 	PALMAS_NUM_REGS,
 };
@@ -171,6 +164,9 @@ struct palmas_pmic_platform_data {
 
 	/* use LDO6 for vibrator control */
 	int ldo6_vibrator;
+
+	/* Enable tracking mode of LDO8 */
+	bool enable_ldo8_tracking;
 };
 
 struct palmas_usb_platform_data {
@@ -331,6 +327,8 @@ struct palmas_pmic {
 	int smps457;
 
 	int range[PALMAS_REG_SMPS10];
+	unsigned int ramp_delay[PALMAS_REG_SMPS10];
+	unsigned int current_reg_mode[PALMAS_REG_SMPS10];
 };
 
 struct palmas_resource {
diff --git a/include/linux/mfd/tps65217.h b/include/linux/mfd/tps65217.h
index 290762f9393..29eab2bd3df 100644
--- a/include/linux/mfd/tps65217.h
+++ b/include/linux/mfd/tps65217.h
@@ -228,6 +228,7 @@ enum tps65217_bl_fdim {
 struct tps65217_bl_pdata {
 	enum tps65217_bl_isel isel;
 	enum tps65217_bl_fdim fdim;
+	int dft_brightness;
 };
 
 /**
diff --git a/include/linux/mm.h b/include/linux/mm.h
index 7acc9dc73c9..c05d7cfbb6b 100644
--- a/include/linux/mm.h
+++ b/include/linux/mm.h
@@ -44,6 +44,9 @@ extern int sysctl_legacy_va_layout;
 #include <asm/pgtable.h>
 #include <asm/processor.h>
 
+extern unsigned long sysctl_user_reserve_kbytes;
+extern unsigned long sysctl_admin_reserve_kbytes;
+
 #define nth_page(page,n) pfn_to_page(page_to_pfn((page)) + (n))
 
 /* to align the pointer to the (next) page boundary */
@@ -87,7 +90,6 @@ extern unsigned int kobjsize(const void *objp);
 #define VM_PFNMAP	0x00000400	/* Page-ranges managed without "struct page", just pure PFN */
 #define VM_DENYWRITE	0x00000800	/* ETXTBSY on write attempts.. */
 
-#define VM_POPULATE     0x00001000
 #define VM_LOCKED	0x00002000
 #define VM_IO           0x00004000	/* Memory mapped I/O or similar */
 
@@ -900,7 +902,8 @@ extern void pagefault_out_of_memory(void);
  * Flags passed to show_mem() and show_free_areas() to suppress output in
  * various contexts.
  */
-#define SHOW_MEM_FILTER_NODES	(0x0001u)	/* filter disallowed nodes */
+#define SHOW_MEM_FILTER_NODES		(0x0001u)	/* disallowed nodes */
+#define SHOW_MEM_FILTER_PAGE_COUNT	(0x0002u)	/* page type count */
 
 extern void show_free_areas(unsigned int flags);
 extern bool skip_free_areas_node(unsigned int flags, int nid);
@@ -1295,6 +1298,61 @@ extern void free_area_init_node(int nid, unsigned long * zones_size,
 		unsigned long zone_start_pfn, unsigned long *zholes_size);
 extern void free_initmem(void);
 
+/*
+ * Free reserved pages within range [PAGE_ALIGN(start), end & PAGE_MASK)
+ * into the buddy system. The freed pages will be poisoned with pattern
+ * "poison" if it's non-zero.
+ * Return pages freed into the buddy system.
+ */
+extern unsigned long free_reserved_area(unsigned long start, unsigned long end,
+					int poison, char *s);
+#ifdef	CONFIG_HIGHMEM
+/*
+ * Free a highmem page into the buddy system, adjusting totalhigh_pages
+ * and totalram_pages.
+ */
+extern void free_highmem_page(struct page *page);
+#endif
+
+static inline void adjust_managed_page_count(struct page *page, long count)
+{
+	totalram_pages += count;
+}
+
+/* Free the reserved page into the buddy system, so it gets managed. */
+static inline void __free_reserved_page(struct page *page)
+{
+	ClearPageReserved(page);
+	init_page_count(page);
+	__free_page(page);
+}
+
+static inline void free_reserved_page(struct page *page)
+{
+	__free_reserved_page(page);
+	adjust_managed_page_count(page, 1);
+}
+
+static inline void mark_page_reserved(struct page *page)
+{
+	SetPageReserved(page);
+	adjust_managed_page_count(page, -1);
+}
+
+/*
+ * Default method to free all the __init memory into the buddy system.
+ * The freed pages will be poisoned with pattern "poison" if it is
+ * non-zero. Return pages freed into the buddy system.
+ */
+static inline unsigned long free_initmem_default(int poison)
+{
+	extern char __init_begin[], __init_end[];
+
+	return free_reserved_area(PAGE_ALIGN((unsigned long)&__init_begin) ,
+				  ((unsigned long)&__init_end) & PAGE_MASK,
+				  poison, "unused kernel");
+}
+
 #ifdef CONFIG_HAVE_MEMBLOCK_NODE_MAP
 /*
  * With CONFIG_HAVE_MEMBLOCK_NODE_MAP set, an architecture may initialise its
@@ -1612,6 +1670,8 @@ int vm_insert_pfn(struct vm_area_struct *vma, unsigned long addr,
 			unsigned long pfn);
 int vm_insert_mixed(struct vm_area_struct *vma, unsigned long addr,
 			unsigned long pfn);
+int vm_iomap_memory(struct vm_area_struct *vma, phys_addr_t start, unsigned long len);
+
 
 struct page *follow_page_mask(struct vm_area_struct *vma,
 			      unsigned long address, unsigned int foll_flags,
@@ -1674,8 +1734,12 @@ int in_gate_area_no_mm(unsigned long addr);
 #define in_gate_area(mm, addr) ({(void)mm; in_gate_area_no_mm(addr);})
 #endif	/* __HAVE_ARCH_GATE_AREA */
 
+#ifdef CONFIG_SYSCTL
+extern int sysctl_drop_caches;
 int drop_caches_sysctl_handler(struct ctl_table *, int,
 					void __user *, size_t *, loff_t *);
+#endif
+
 unsigned long shrink_slab(struct shrink_control *shrink,
 			  unsigned long nr_pages_scanned,
 			  unsigned long lru_pages);
@@ -1703,12 +1767,12 @@ pte_t *vmemmap_pte_populate(pmd_t *pmd, unsigned long addr, int node);
 void *vmemmap_alloc_block(unsigned long size, int node);
 void *vmemmap_alloc_block_buf(unsigned long size, int node);
 void vmemmap_verify(pte_t *, int, unsigned long, unsigned long);
-int vmemmap_populate_basepages(struct page *start_page,
-						unsigned long pages, int node);
-int vmemmap_populate(struct page *start_page, unsigned long pages, int node);
+int vmemmap_populate_basepages(unsigned long start, unsigned long end,
+			       int node);
+int vmemmap_populate(unsigned long start, unsigned long end, int node);
 void vmemmap_populate_print_last(void);
 #ifdef CONFIG_MEMORY_HOTPLUG
-void vmemmap_free(struct page *memmap, unsigned long nr_pages);
+void vmemmap_free(unsigned long start, unsigned long end);
 #endif
 void register_page_bootmem_memmap(unsigned long section_nr, struct page *map,
 				  unsigned long size);
@@ -1755,5 +1819,11 @@ static inline unsigned int debug_guardpage_minorder(void) { return 0; }
 static inline bool page_is_guard(struct page *page) { return false; }
 #endif /* CONFIG_DEBUG_PAGEALLOC */
 
+#if MAX_NUMNODES > 1
+void __init setup_nr_node_ids(void);
+#else
+static inline void setup_nr_node_ids(void) {}
+#endif
+
 #endif /* __KERNEL__ */
 #endif /* _LINUX_MM_H */
diff --git a/include/linux/mman.h b/include/linux/mman.h
index 61c7a87e5d2..9aa863da287 100644
--- a/include/linux/mman.h
+++ b/include/linux/mman.h
@@ -79,8 +79,6 @@ calc_vm_flag_bits(unsigned long flags)
 {
 	return _calc_vm_trans(flags, MAP_GROWSDOWN,  VM_GROWSDOWN ) |
 	       _calc_vm_trans(flags, MAP_DENYWRITE,  VM_DENYWRITE ) |
-	       ((flags & MAP_LOCKED) ? (VM_LOCKED | VM_POPULATE) : 0) |
-	       (((flags & (MAP_POPULATE | MAP_NONBLOCK)) == MAP_POPULATE) ?
-							VM_POPULATE : 0);
+	       _calc_vm_trans(flags, MAP_LOCKED,     VM_LOCKED    );
 }
 #endif /* _LINUX_MMAN_H */
diff --git a/include/linux/mmzone.h b/include/linux/mmzone.h
index c74092eebf5..5c76737d836 100644
--- a/include/linux/mmzone.h
+++ b/include/linux/mmzone.h
@@ -450,7 +450,7 @@ struct zone {
 	 *
 	 * present_pages is physical pages existing within the zone, which
 	 * is calculated as:
-	 *	present_pages = spanned_pages - absent_pages(pags in holes);
+	 *	present_pages = spanned_pages - absent_pages(pages in holes);
 	 *
 	 * managed_pages is present pages managed by the buddy system, which
 	 * is calculated as (reserved_pages includes pages allocated by the
diff --git a/include/linux/mod_devicetable.h b/include/linux/mod_devicetable.h
index 779cf7c4a3d..b508016fb76 100644
--- a/include/linux/mod_devicetable.h
+++ b/include/linux/mod_devicetable.h
@@ -9,6 +9,7 @@
 
 #ifdef __KERNEL__
 #include <linux/types.h>
+#include <linux/uuid.h>
 typedef unsigned long kernel_ulong_t;
 #endif
 
@@ -568,4 +569,12 @@ struct ipack_device_id {
 	__u32 device;			/* Device ID or IPACK_ANY_ID */
 };
 
+#define MEI_CL_MODULE_PREFIX "mei:"
+#define MEI_CL_NAME_SIZE 32
+
+struct mei_cl_device_id {
+	char name[MEI_CL_NAME_SIZE];
+	kernel_ulong_t driver_info;
+};
+
 #endif /* LINUX_MOD_DEVICETABLE_H */
diff --git a/include/linux/mount.h b/include/linux/mount.h
index d7029f4a191..73005f9957e 100644
--- a/include/linux/mount.h
+++ b/include/linux/mount.h
@@ -47,6 +47,8 @@ struct mnt_namespace;
 
 #define MNT_INTERNAL	0x4000
 
+#define MNT_LOCK_READONLY	0x400000
+
 struct vfsmount {
 	struct dentry *mnt_root;	/* root of the mounted tree */
 	struct super_block *mnt_sb;	/* pointer to superblock */
diff --git a/include/linux/msi.h b/include/linux/msi.h
index ce93a341337..20c2d6dd5d2 100644
--- a/include/linux/msi.h
+++ b/include/linux/msi.h
@@ -13,14 +13,14 @@ struct msi_msg {
 /* Helper functions */
 struct irq_data;
 struct msi_desc;
-extern void mask_msi_irq(struct irq_data *data);
-extern void unmask_msi_irq(struct irq_data *data);
-extern void __read_msi_msg(struct msi_desc *entry, struct msi_msg *msg);
-extern void __get_cached_msi_msg(struct msi_desc *entry, struct msi_msg *msg);
-extern void __write_msi_msg(struct msi_desc *entry, struct msi_msg *msg);
-extern void read_msi_msg(unsigned int irq, struct msi_msg *msg);
-extern void get_cached_msi_msg(unsigned int irq, struct msi_msg *msg);
-extern void write_msi_msg(unsigned int irq, struct msi_msg *msg);
+void mask_msi_irq(struct irq_data *data);
+void unmask_msi_irq(struct irq_data *data);
+void __read_msi_msg(struct msi_desc *entry, struct msi_msg *msg);
+void __get_cached_msi_msg(struct msi_desc *entry, struct msi_msg *msg);
+void __write_msi_msg(struct msi_desc *entry, struct msi_msg *msg);
+void read_msi_msg(unsigned int irq, struct msi_msg *msg);
+void get_cached_msi_msg(unsigned int irq, struct msi_msg *msg);
+void write_msi_msg(unsigned int irq, struct msi_msg *msg);
 
 struct msi_desc {
 	struct {
@@ -54,9 +54,8 @@ struct msi_desc {
  */
 int arch_setup_msi_irq(struct pci_dev *dev, struct msi_desc *desc);
 void arch_teardown_msi_irq(unsigned int irq);
-extern int arch_setup_msi_irqs(struct pci_dev *dev, int nvec, int type);
-extern void arch_teardown_msi_irqs(struct pci_dev *dev);
-extern int arch_msi_check_device(struct pci_dev* dev, int nvec, int type);
-
+int arch_setup_msi_irqs(struct pci_dev *dev, int nvec, int type);
+void arch_teardown_msi_irqs(struct pci_dev *dev);
+int arch_msi_check_device(struct pci_dev* dev, int nvec, int type);
 
 #endif /* LINUX_MSI_H */
diff --git a/include/linux/mutex.h b/include/linux/mutex.h
index 9121595a8eb..433da8a1a42 100644
--- a/include/linux/mutex.h
+++ b/include/linux/mutex.h
@@ -53,6 +53,9 @@ struct mutex {
 #if defined(CONFIG_DEBUG_MUTEXES) || defined(CONFIG_SMP)
 	struct task_struct	*owner;
 #endif
+#ifdef CONFIG_MUTEX_SPIN_ON_OWNER
+	void			*spin_mlock;	/* Spinner MCS lock */
+#endif
 #ifdef CONFIG_DEBUG_MUTEXES
 	const char 		*name;
 	void			*magic;
diff --git a/include/linux/mxsfb.h b/include/linux/mxsfb.h
index f14943d5531..f80af867434 100644
--- a/include/linux/mxsfb.h
+++ b/include/linux/mxsfb.h
@@ -24,8 +24,8 @@
 #define STMLCDIF_18BIT 2 /** pixel data bus to the display is of 18 bit width */
 #define STMLCDIF_24BIT 3 /** pixel data bus to the display is of 24 bit width */
 
-#define FB_SYNC_DATA_ENABLE_HIGH_ACT	(1 << 6)
-#define FB_SYNC_DOTCLK_FAILING_ACT	(1 << 7) /* failing/negtive edge sampling */
+#define MXSFB_SYNC_DATA_ENABLE_HIGH_ACT	(1 << 6)
+#define MXSFB_SYNC_DOTCLK_FAILING_ACT	(1 << 7) /* failing/negtive edge sampling */
 
 struct mxsfb_platform_data {
 	struct fb_videomode *mode_list;
@@ -44,6 +44,9 @@ struct mxsfb_platform_data {
 				 * allocated. If specified,fb_size must also be specified.
 				 * fb_phys must be unused by Linux.
 				 */
+	u32 sync;		/* sync mask, contains MXSFB specifics not
+				 * carried in fb_info->var.sync
+				 */
 };
 
 #endif /* __LINUX_MXSFB_H */
diff --git a/include/linux/net.h b/include/linux/net.h
index aa1673160a4..99c9f0c103c 100644
--- a/include/linux/net.h
+++ b/include/linux/net.h
@@ -240,8 +240,8 @@ do {								\
 #define net_dbg_ratelimited(fmt, ...)				\
 	net_ratelimited_function(pr_debug, fmt, ##__VA_ARGS__)
 
-#define net_random()		random32()
-#define net_srandom(seed)	srandom32((__force u32)seed)
+#define net_random()		prandom_u32()
+#define net_srandom(seed)	prandom_seed((__force u32)(seed))
 
 extern int   	     kernel_sendmsg(struct socket *sock, struct msghdr *msg,
 				    struct kvec *vec, size_t num, size_t len);
diff --git a/include/linux/netdevice.h b/include/linux/netdevice.h
index b3d00fa4b31..6151e903eef 100644
--- a/include/linux/netdevice.h
+++ b/include/linux/netdevice.h
@@ -210,9 +210,9 @@ struct netdev_hw_addr {
 #define NETDEV_HW_ADDR_T_SLAVE		3
 #define NETDEV_HW_ADDR_T_UNICAST	4
 #define NETDEV_HW_ADDR_T_MULTICAST	5
-	bool			synced;
 	bool			global_use;
 	int			refcount;
+	int			synced;
 	struct rcu_head		rcu_head;
 };
 
@@ -895,7 +895,7 @@ struct netdev_fcoe_hbainfo {
  *
  * int (*ndo_bridge_setlink)(struct net_device *dev, struct nlmsghdr *nlh)
  * int (*ndo_bridge_getlink)(struct sk_buff *skb, u32 pid, u32 seq,
- *			     struct net_device *dev)
+ *			     struct net_device *dev, u32 filter_mask)
  *
  * int (*ndo_change_carrier)(struct net_device *dev, bool new_carrier);
  *	Called to change device carrier. Soft-devices (like dummy, team, etc)
diff --git a/include/linux/netfilter/ipset/ip_set_ahash.h b/include/linux/netfilter/ipset/ip_set_ahash.h
index 01d25e6fc79..0214c4c146f 100644
--- a/include/linux/netfilter/ipset/ip_set_ahash.h
+++ b/include/linux/netfilter/ipset/ip_set_ahash.h
@@ -291,6 +291,7 @@ ip_set_hash_destroy(struct ip_set *set)
 #define type_pf_data_tlist	TOKEN(TYPE, PF, _data_tlist)
 #define type_pf_data_next	TOKEN(TYPE, PF, _data_next)
 #define type_pf_data_flags	TOKEN(TYPE, PF, _data_flags)
+#define type_pf_data_reset_flags TOKEN(TYPE, PF, _data_reset_flags)
 #ifdef IP_SET_HASH_WITH_NETS
 #define type_pf_data_match	TOKEN(TYPE, PF, _data_match)
 #else
@@ -385,9 +386,9 @@ type_pf_resize(struct ip_set *set, bool retried)
 	struct ip_set_hash *h = set->data;
 	struct htable *t, *orig = h->table;
 	u8 htable_bits = orig->htable_bits;
-	const struct type_pf_elem *data;
+	struct type_pf_elem *data;
 	struct hbucket *n, *m;
-	u32 i, j;
+	u32 i, j, flags = 0;
 	int ret;
 
 retry:
@@ -412,9 +413,16 @@ retry:
 		n = hbucket(orig, i);
 		for (j = 0; j < n->pos; j++) {
 			data = ahash_data(n, j);
+#ifdef IP_SET_HASH_WITH_NETS
+			flags = 0;
+			type_pf_data_reset_flags(data, &flags);
+#endif
 			m = hbucket(t, HKEY(data, h->initval, htable_bits));
-			ret = type_pf_elem_add(m, data, AHASH_MAX(h), 0);
+			ret = type_pf_elem_add(m, data, AHASH_MAX(h), flags);
 			if (ret < 0) {
+#ifdef IP_SET_HASH_WITH_NETS
+				type_pf_data_flags(data, flags);
+#endif
 				read_unlock_bh(&set->lock);
 				ahash_destroy(t);
 				if (ret == -EAGAIN)
@@ -836,9 +844,9 @@ type_pf_tresize(struct ip_set *set, bool retried)
 	struct ip_set_hash *h = set->data;
 	struct htable *t, *orig = h->table;
 	u8 htable_bits = orig->htable_bits;
-	const struct type_pf_elem *data;
+	struct type_pf_elem *data;
 	struct hbucket *n, *m;
-	u32 i, j;
+	u32 i, j, flags = 0;
 	int ret;
 
 	/* Try to cleanup once */
@@ -873,10 +881,17 @@ retry:
 		n = hbucket(orig, i);
 		for (j = 0; j < n->pos; j++) {
 			data = ahash_tdata(n, j);
+#ifdef IP_SET_HASH_WITH_NETS
+			flags = 0;
+			type_pf_data_reset_flags(data, &flags);
+#endif
 			m = hbucket(t, HKEY(data, h->initval, htable_bits));
-			ret = type_pf_elem_tadd(m, data, AHASH_MAX(h), 0,
-						ip_set_timeout_get(type_pf_data_timeout(data)));
+			ret = type_pf_elem_tadd(m, data, AHASH_MAX(h), flags,
+				ip_set_timeout_get(type_pf_data_timeout(data)));
 			if (ret < 0) {
+#ifdef IP_SET_HASH_WITH_NETS
+				type_pf_data_flags(data, flags);
+#endif
 				read_unlock_bh(&set->lock);
 				ahash_destroy(t);
 				if (ret == -EAGAIN)
@@ -1187,6 +1202,7 @@ type_pf_gc_init(struct ip_set *set)
 #undef type_pf_data_tlist
 #undef type_pf_data_next
 #undef type_pf_data_flags
+#undef type_pf_data_reset_flags
 #undef type_pf_data_match
 
 #undef type_pf_elem
diff --git a/include/linux/nfs_fs.h b/include/linux/nfs_fs.h
index 1cc25682b20..fc01d5cb4cf 100644
--- a/include/linux/nfs_fs.h
+++ b/include/linux/nfs_fs.h
@@ -59,11 +59,18 @@ struct nfs_lockowner {
 	pid_t l_pid;
 };
 
+#define NFS_IO_INPROGRESS 0
+struct nfs_io_counter {
+	unsigned long flags;
+	atomic_t io_count;
+};
+
 struct nfs_lock_context {
 	atomic_t count;
 	struct list_head list;
 	struct nfs_open_context *open_context;
 	struct nfs_lockowner lockowner;
+	struct nfs_io_counter io_count;
 };
 
 struct nfs4_state;
@@ -77,6 +84,7 @@ struct nfs_open_context {
 	unsigned long flags;
 #define NFS_CONTEXT_ERROR_WRITE		(0)
 #define NFS_CONTEXT_RESEND_WRITES	(1)
+#define NFS_CONTEXT_BAD			(2)
 	int error;
 
 	struct list_head list;
diff --git a/include/linux/nfs_fs_sb.h b/include/linux/nfs_fs_sb.h
index 6c6ed153a9b..3b7fa2abecc 100644
--- a/include/linux/nfs_fs_sb.h
+++ b/include/linux/nfs_fs_sb.h
@@ -40,6 +40,7 @@ struct nfs_client {
 #define NFS_CS_NORESVPORT	0		/* - use ephemeral src port */
 #define NFS_CS_DISCRTRY		1		/* - disconnect on RPC retry */
 #define NFS_CS_MIGRATION	2		/* - transparent state migr */
+#define NFS_CS_INFINITE_SLOTS	3		/* - don't limit TCP slots */
 	struct sockaddr_storage	cl_addr;	/* server identifier */
 	size_t			cl_addrlen;
 	char *			cl_hostname;	/* hostname of server */
@@ -197,5 +198,7 @@ struct nfs_server {
 #define NFS_CAP_MTIME		(1U << 13)
 #define NFS_CAP_POSIX_LOCK	(1U << 14)
 #define NFS_CAP_UIDGID_NOMAP	(1U << 15)
+#define NFS_CAP_STATEID_NFSV41	(1U << 16)
+#define NFS_CAP_ATOMIC_OPEN_V1	(1U << 17)
 
 #endif
diff --git a/include/linux/nfs_xdr.h b/include/linux/nfs_xdr.h
index 4b993d358da..766c5bc9d44 100644
--- a/include/linux/nfs_xdr.h
+++ b/include/linux/nfs_xdr.h
@@ -14,9 +14,6 @@
 #define NFS_DEF_FILE_IO_SIZE	(4096U)
 #define NFS_MIN_FILE_IO_SIZE	(1024U)
 
-/* Forward declaration for NFS v3 */
-struct nfs4_secinfo_flavors;
-
 struct nfs4_string {
 	unsigned int len;
 	char *data;
@@ -349,6 +346,7 @@ struct nfs_openargs {
 	const u32 *		bitmask;
 	const u32 *		open_bitmap;
 	__u32			claim;
+	enum createmode4	createmode;
 };
 
 struct nfs_openres {
@@ -486,6 +484,7 @@ struct nfs_readargs {
 	struct nfs_fh *		fh;
 	struct nfs_open_context *context;
 	struct nfs_lock_context *lock_context;
+	nfs4_stateid		stateid;
 	__u64			offset;
 	__u32			count;
 	unsigned int		pgbase;
@@ -507,6 +506,7 @@ struct nfs_writeargs {
 	struct nfs_fh *		fh;
 	struct nfs_open_context *context;
 	struct nfs_lock_context *lock_context;
+	nfs4_stateid		stateid;
 	__u64			offset;
 	__u32			count;
 	enum nfs3_stable_how	stable;
@@ -1050,25 +1050,14 @@ struct nfs4_fs_locations_res {
 	struct nfs4_fs_locations       *fs_locations;
 };
 
-struct nfs4_secinfo_oid {
-	unsigned int len;
-	char data[GSS_OID_MAX_LEN];
-};
-
-struct nfs4_secinfo_gss {
-	struct nfs4_secinfo_oid sec_oid4;
-	unsigned int qop4;
-	unsigned int service;
-};
-
-struct nfs4_secinfo_flavor {
-	unsigned int 		flavor;
-	struct nfs4_secinfo_gss	gss;
+struct nfs4_secinfo4 {
+	u32			flavor;
+	struct rpcsec_gss_info	flavor_info;
 };
 
 struct nfs4_secinfo_flavors {
-	unsigned int num_flavors;
-	struct nfs4_secinfo_flavor flavors[0];
+	unsigned int		num_flavors;
+	struct nfs4_secinfo4	flavors[0];
 };
 
 struct nfs4_secinfo_arg {
diff --git a/include/linux/notifier.h b/include/linux/notifier.h
index d65746efc95..d14a4c36246 100644
--- a/include/linux/notifier.h
+++ b/include/linux/notifier.h
@@ -47,8 +47,11 @@
  * runtime initialization.
  */
 
+typedef	int (*notifier_fn_t)(struct notifier_block *nb,
+			unsigned long action, void *data);
+
 struct notifier_block {
-	int (*notifier_call)(struct notifier_block *, unsigned long, void *);
+	notifier_fn_t notifier_call;
 	struct notifier_block __rcu *next;
 	int priority;
 };
diff --git a/include/linux/pagemap.h b/include/linux/pagemap.h
index 0e38e13eb24..e3dea75a078 100644
--- a/include/linux/pagemap.h
+++ b/include/linux/pagemap.h
@@ -149,7 +149,7 @@ static inline int page_cache_get_speculative(struct page *page)
 {
 	VM_BUG_ON(in_interrupt());
 
-#if !defined(CONFIG_SMP) && defined(CONFIG_TREE_RCU)
+#ifdef CONFIG_TINY_RCU
 # ifdef CONFIG_PREEMPT_COUNT
 	VM_BUG_ON(!in_atomic());
 # endif
diff --git a/include/linux/pci-acpi.h b/include/linux/pci-acpi.h
index 9a22b5efb38..81b31613eb2 100644
--- a/include/linux/pci-acpi.h
+++ b/include/linux/pci-acpi.h
@@ -41,8 +41,37 @@ static inline acpi_handle acpi_pci_get_bridge_handle(struct pci_bus *pbus)
 
 	return DEVICE_ACPI_HANDLE(dev);
 }
+
+void acpi_pci_add_bus(struct pci_bus *bus);
+void acpi_pci_remove_bus(struct pci_bus *bus);
+
+#ifdef	CONFIG_ACPI_PCI_SLOT
+void acpi_pci_slot_init(void);
+void acpi_pci_slot_enumerate(struct pci_bus *bus, acpi_handle handle);
+void acpi_pci_slot_remove(struct pci_bus *bus);
+#else
+static inline void acpi_pci_slot_init(void) { }
+static inline void acpi_pci_slot_enumerate(struct pci_bus *bus,
+					   acpi_handle handle) { }
+static inline void acpi_pci_slot_remove(struct pci_bus *bus) { }
 #endif
 
+#ifdef	CONFIG_HOTPLUG_PCI_ACPI
+void acpiphp_init(void);
+void acpiphp_enumerate_slots(struct pci_bus *bus, acpi_handle handle);
+void acpiphp_remove_slots(struct pci_bus *bus);
+#else
+static inline void acpiphp_init(void) { }
+static inline void acpiphp_enumerate_slots(struct pci_bus *bus,
+					   acpi_handle handle) { }
+static inline void acpiphp_remove_slots(struct pci_bus *bus) { }
+#endif
+
+#else	/* CONFIG_ACPI */
+static inline void acpi_pci_add_bus(struct pci_bus *bus) { }
+static inline void acpi_pci_remove_bus(struct pci_bus *bus) { }
+#endif	/* CONFIG_ACPI */
+
 #ifdef CONFIG_ACPI_APEI
 extern bool aer_acpi_firmware_first(void);
 #else
diff --git a/include/linux/pci-aspm.h b/include/linux/pci-aspm.h
index c8320144fe7..8af4610c2e4 100644
--- a/include/linux/pci-aspm.h
+++ b/include/linux/pci-aspm.h
@@ -23,14 +23,14 @@
 #define PCIE_LINK_STATE_CLKPM	4
 
 #ifdef CONFIG_PCIEASPM
-extern void pcie_aspm_init_link_state(struct pci_dev *pdev);
-extern void pcie_aspm_exit_link_state(struct pci_dev *pdev);
-extern void pcie_aspm_pm_state_change(struct pci_dev *pdev);
-extern void pcie_aspm_powersave_config_link(struct pci_dev *pdev);
-extern void pci_disable_link_state(struct pci_dev *pdev, int state);
-extern void pci_disable_link_state_locked(struct pci_dev *pdev, int state);
-extern void pcie_clear_aspm(struct pci_bus *bus);
-extern void pcie_no_aspm(void);
+void pcie_aspm_init_link_state(struct pci_dev *pdev);
+void pcie_aspm_exit_link_state(struct pci_dev *pdev);
+void pcie_aspm_pm_state_change(struct pci_dev *pdev);
+void pcie_aspm_powersave_config_link(struct pci_dev *pdev);
+void pci_disable_link_state(struct pci_dev *pdev, int state);
+void pci_disable_link_state_locked(struct pci_dev *pdev, int state);
+void pcie_clear_aspm(struct pci_bus *bus);
+void pcie_no_aspm(void);
 #else
 static inline void pcie_aspm_init_link_state(struct pci_dev *pdev)
 {
@@ -56,8 +56,8 @@ static inline void pcie_no_aspm(void)
 #endif
 
 #ifdef CONFIG_PCIEASPM_DEBUG /* this depends on CONFIG_PCIEASPM */
-extern void pcie_aspm_create_sysfs_dev_files(struct pci_dev *pdev);
-extern void pcie_aspm_remove_sysfs_dev_files(struct pci_dev *pdev);
+void pcie_aspm_create_sysfs_dev_files(struct pci_dev *pdev);
+void pcie_aspm_remove_sysfs_dev_files(struct pci_dev *pdev);
 #else
 static inline void pcie_aspm_create_sysfs_dev_files(struct pci_dev *pdev)
 {
diff --git a/include/linux/pci-ats.h b/include/linux/pci-ats.h
index 7ef68724f0f..68bcefd7fca 100644
--- a/include/linux/pci-ats.h
+++ b/include/linux/pci-ats.h
@@ -14,9 +14,9 @@ struct pci_ats {
 
 #ifdef CONFIG_PCI_ATS
 
-extern int pci_enable_ats(struct pci_dev *dev, int ps);
-extern void pci_disable_ats(struct pci_dev *dev);
-extern int pci_ats_queue_depth(struct pci_dev *dev);
+int pci_enable_ats(struct pci_dev *dev, int ps);
+void pci_disable_ats(struct pci_dev *dev);
+int pci_ats_queue_depth(struct pci_dev *dev);
 
 /**
  * pci_ats_enabled - query the ATS status
@@ -54,12 +54,12 @@ static inline int pci_ats_enabled(struct pci_dev *dev)
 
 #ifdef CONFIG_PCI_PRI
 
-extern int  pci_enable_pri(struct pci_dev *pdev, u32 reqs);
-extern void pci_disable_pri(struct pci_dev *pdev);
-extern bool pci_pri_enabled(struct pci_dev *pdev);
-extern int  pci_reset_pri(struct pci_dev *pdev);
-extern bool pci_pri_stopped(struct pci_dev *pdev);
-extern int  pci_pri_status(struct pci_dev *pdev);
+int pci_enable_pri(struct pci_dev *pdev, u32 reqs);
+void pci_disable_pri(struct pci_dev *pdev);
+bool pci_pri_enabled(struct pci_dev *pdev);
+int pci_reset_pri(struct pci_dev *pdev);
+bool pci_pri_stopped(struct pci_dev *pdev);
+int pci_pri_status(struct pci_dev *pdev);
 
 #else /* CONFIG_PCI_PRI */
 
@@ -95,10 +95,10 @@ static inline int pci_pri_status(struct pci_dev *pdev)
 
 #ifdef CONFIG_PCI_PASID
 
-extern int pci_enable_pasid(struct pci_dev *pdev, int features);
-extern void pci_disable_pasid(struct pci_dev *pdev);
-extern int pci_pasid_features(struct pci_dev *pdev);
-extern int pci_max_pasids(struct pci_dev *pdev);
+int pci_enable_pasid(struct pci_dev *pdev, int features);
+void pci_disable_pasid(struct pci_dev *pdev);
+int pci_pasid_features(struct pci_dev *pdev);
+int pci_max_pasids(struct pci_dev *pdev);
 
 #else  /* CONFIG_PCI_PASID */
 
diff --git a/include/linux/pci.h b/include/linux/pci.h
index 2461033a798..e73dfa308b8 100644
--- a/include/linux/pci.h
+++ b/include/linux/pci.h
@@ -35,6 +35,21 @@
 /* Include the ID list */
 #include <linux/pci_ids.h>
 
+/*
+ * The PCI interface treats multi-function devices as independent
+ * devices.  The slot/function address of each device is encoded
+ * in a single byte as follows:
+ *
+ *	7:3 = slot
+ *	2:0 = function
+ * PCI_DEVFN(), PCI_SLOT(), and PCI_FUNC() are defined uapi/linux/pci.h
+ * In the interest of not exposing interfaces to user-space unnecessarily,
+ * the following kernel only defines are being added here.
+ */
+#define PCI_DEVID(bus, devfn)  ((((u16)bus) << 8) | devfn)
+/* return bus from PCI devid = ((u16)bus_number) << 8) | devfn */
+#define PCI_BUS_NUM(x) (((x) >> 8) & 0xff)
+
 /* pci_slot represents a physical slot */
 struct pci_slot {
 	struct pci_bus *bus;		/* The bus this slot is on */
@@ -232,6 +247,8 @@ struct pci_dev {
 	u8		revision;	/* PCI revision, low byte of class word */
 	u8		hdr_type;	/* PCI header type (`multi' flag masked out) */
 	u8		pcie_cap;	/* PCI-E capability offset */
+	u8		msi_cap;	/* MSI capability offset */
+	u8		msix_cap;	/* MSI-X capability offset */
 	u8		pcie_mpss:3;	/* PCI-E Max Payload Size Supported */
 	u8		rom_base_reg;	/* which config register controls the ROM */
 	u8		pin;  		/* which interrupt pin this device uses */
@@ -249,8 +266,7 @@ struct pci_dev {
 	pci_power_t     current_state;  /* Current operating state. In ACPI-speak,
 					   this is D0-D3, D0 being fully functional,
 					   and D3 being off. */
-	int		pm_cap;		/* PM capability offset in the
-					   configuration space */
+	u8		pm_cap;		/* PM capability offset */
 	unsigned int	pme_support:5;	/* Bitmask of states from which PME#
 					   can be generated */
 	unsigned int	pme_interrupt:1;
@@ -348,7 +364,7 @@ static inline struct pci_dev *pci_physfn(struct pci_dev *dev)
 	return dev;
 }
 
-extern struct pci_dev *alloc_pci_dev(void);
+struct pci_dev *alloc_pci_dev(void);
 
 #define	to_pci_dev(n) container_of(n, struct pci_dev, dev)
 #define for_each_pci_dev(d) while ((d = pci_get_device(PCI_ANY_ID, PCI_ANY_ID, d)) != NULL)
@@ -504,10 +520,10 @@ struct pci_ops {
  * ACPI needs to be able to access PCI config space before we've done a
  * PCI bus scan and created pci_bus structures.
  */
-extern int raw_pci_read(unsigned int domain, unsigned int bus,
-			unsigned int devfn, int reg, int len, u32 *val);
-extern int raw_pci_write(unsigned int domain, unsigned int bus,
-			unsigned int devfn, int reg, int len, u32 val);
+int raw_pci_read(unsigned int domain, unsigned int bus, unsigned int devfn,
+		 int reg, int len, u32 *val);
+int raw_pci_write(unsigned int domain, unsigned int bus, unsigned int devfn,
+		  int reg, int len, u32 val);
 
 struct pci_bus_region {
 	resource_size_t start;
@@ -658,7 +674,7 @@ struct pci_driver {
 /* these external functions are only available when PCI support is enabled */
 #ifdef CONFIG_PCI
 
-extern void pcie_bus_configure_settings(struct pci_bus *bus, u8 smpss);
+void pcie_bus_configure_settings(struct pci_bus *bus, u8 smpss);
 
 enum pcie_bus_config_types {
 	PCIE_BUS_TUNE_OFF,
@@ -675,9 +691,11 @@ extern struct bus_type pci_bus_type;
  * code, or pci core code. */
 extern struct list_head pci_root_buses;	/* list of all known PCI buses */
 /* Some device drivers need know if pci is initiated */
-extern int no_pci_devices(void);
+int no_pci_devices(void);
 
 void pcibios_resource_survey_bus(struct pci_bus *bus);
+void pcibios_add_bus(struct pci_bus *bus);
+void pcibios_remove_bus(struct pci_bus *bus);
 void pcibios_fixup_bus(struct pci_bus *);
 int __must_check pcibios_enable_device(struct pci_dev *, int mask);
 /* Architecture specific versions may override this (weak) */
@@ -699,7 +717,7 @@ void pcibios_resource_to_bus(struct pci_dev *dev, struct pci_bus_region *region,
 void pcibios_bus_to_resource(struct pci_dev *dev, struct resource *res,
 			     struct pci_bus_region *region);
 void pcibios_scan_specific_bus(int busn);
-extern struct pci_bus *pci_find_bus(int domain, int busnr);
+struct pci_bus *pci_find_bus(int domain, int busnr);
 void pci_bus_add_devices(const struct pci_bus *bus);
 struct pci_bus *pci_scan_bus_parented(struct device *parent, int bus,
 				      struct pci_ops *ops, void *sysdata);
@@ -732,14 +750,14 @@ struct resource *pci_find_parent_resource(const struct pci_dev *dev,
 u8 pci_swizzle_interrupt_pin(const struct pci_dev *dev, u8 pin);
 int pci_get_interrupt_pin(struct pci_dev *dev, struct pci_dev **bridge);
 u8 pci_common_swizzle(struct pci_dev *dev, u8 *pinp);
-extern struct pci_dev *pci_dev_get(struct pci_dev *dev);
-extern void pci_dev_put(struct pci_dev *dev);
-extern void pci_remove_bus(struct pci_bus *b);
-extern void pci_stop_and_remove_bus_device(struct pci_dev *dev);
+struct pci_dev *pci_dev_get(struct pci_dev *dev);
+void pci_dev_put(struct pci_dev *dev);
+void pci_remove_bus(struct pci_bus *b);
+void pci_stop_and_remove_bus_device(struct pci_dev *dev);
 void pci_stop_root_bus(struct pci_bus *bus);
 void pci_remove_root_bus(struct pci_bus *bus);
 void pci_setup_cardbus(struct pci_bus *bus);
-extern void pci_sort_breadthfirst(void);
+void pci_sort_breadthfirst(void);
 #define dev_is_pci(d) ((d)->bus == &pci_bus_type)
 #define dev_is_pf(d) ((dev_is_pci(d) ? to_pci_dev(d)->is_physfn : false))
 #define dev_num_vf(d) ((dev_is_pci(d) ? pci_num_vf(to_pci_dev(d)) : 0))
@@ -916,6 +934,7 @@ void pci_disable_rom(struct pci_dev *pdev);
 void __iomem __must_check *pci_map_rom(struct pci_dev *pdev, size_t *size);
 void pci_unmap_rom(struct pci_dev *pdev, void __iomem *rom);
 size_t pci_get_rom_size(struct pci_dev *pdev, void __iomem *rom, size_t size);
+void __iomem __must_check *pci_platform_rom(struct pci_dev *pdev, size_t *size);
 
 /* Power management related routines */
 int pci_save_state(struct pci_dev *dev);
@@ -1141,18 +1160,17 @@ static inline int pci_msi_enabled(void)
 	return 0;
 }
 #else
-extern int pci_enable_msi_block(struct pci_dev *dev, unsigned int nvec);
-extern int pci_enable_msi_block_auto(struct pci_dev *dev, unsigned int *maxvec);
-extern void pci_msi_shutdown(struct pci_dev *dev);
-extern void pci_disable_msi(struct pci_dev *dev);
-extern int pci_msix_table_size(struct pci_dev *dev);
-extern int pci_enable_msix(struct pci_dev *dev,
-	struct msix_entry *entries, int nvec);
-extern void pci_msix_shutdown(struct pci_dev *dev);
-extern void pci_disable_msix(struct pci_dev *dev);
-extern void msi_remove_pci_irq_vectors(struct pci_dev *dev);
-extern void pci_restore_msi_state(struct pci_dev *dev);
-extern int pci_msi_enabled(void);
+int pci_enable_msi_block(struct pci_dev *dev, unsigned int nvec);
+int pci_enable_msi_block_auto(struct pci_dev *dev, unsigned int *maxvec);
+void pci_msi_shutdown(struct pci_dev *dev);
+void pci_disable_msi(struct pci_dev *dev);
+int pci_msix_table_size(struct pci_dev *dev);
+int pci_enable_msix(struct pci_dev *dev, struct msix_entry *entries, int nvec);
+void pci_msix_shutdown(struct pci_dev *dev);
+void pci_disable_msix(struct pci_dev *dev);
+void msi_remove_pci_irq_vectors(struct pci_dev *dev);
+void pci_restore_msi_state(struct pci_dev *dev);
+int pci_msi_enabled(void);
 #endif
 
 #ifdef CONFIG_PCIEPORTBUS
@@ -1167,8 +1185,8 @@ extern bool pcie_ports_auto;
 static inline int pcie_aspm_enabled(void) { return 0; }
 static inline bool pcie_aspm_support_enabled(void) { return false; }
 #else
-extern int pcie_aspm_enabled(void);
-extern bool pcie_aspm_support_enabled(void);
+int pcie_aspm_enabled(void);
+bool pcie_aspm_support_enabled(void);
 #endif
 
 #ifdef CONFIG_PCIEAER
@@ -1186,8 +1204,8 @@ static inline void pcie_set_ecrc_checking(struct pci_dev *dev)
 }
 static inline void pcie_ecrc_get_policy(char *str) {};
 #else
-extern void pcie_set_ecrc_checking(struct pci_dev *dev);
-extern void pcie_ecrc_get_policy(char *str);
+void pcie_set_ecrc_checking(struct pci_dev *dev);
+void pcie_ecrc_get_policy(char *str);
 #endif
 
 #define pci_enable_msi(pdev)	pci_enable_msi_block(pdev, 1)
@@ -1198,9 +1216,9 @@ int  ht_create_irq(struct pci_dev *dev, int idx);
 void ht_destroy_irq(unsigned int irq);
 #endif /* CONFIG_HT_IRQ */
 
-extern void pci_cfg_access_lock(struct pci_dev *dev);
-extern bool pci_cfg_access_trylock(struct pci_dev *dev);
-extern void pci_cfg_access_unlock(struct pci_dev *dev);
+void pci_cfg_access_lock(struct pci_dev *dev);
+bool pci_cfg_access_trylock(struct pci_dev *dev);
+void pci_cfg_access_unlock(struct pci_dev *dev);
 
 /*
  * PCI domain support.  Sometimes called PCI segment (eg by ACPI),
@@ -1225,7 +1243,7 @@ static inline int pci_proc_domain(struct pci_bus *bus)
 /* some architectures require additional setup to direct VGA traffic */
 typedef int (*arch_set_vga_state_t)(struct pci_dev *pdev, bool decode,
 		      unsigned int command_bits, u32 flags);
-extern void pci_register_set_vga_state(arch_set_vga_state_t func);
+void pci_register_set_vga_state(arch_set_vga_state_t func);
 
 #else /* CONFIG_PCI is not enabled */
 
@@ -1627,8 +1645,8 @@ int pcibios_set_pcie_reset_state(struct pci_dev *dev,
 int pcibios_add_device(struct pci_dev *dev);
 
 #ifdef CONFIG_PCI_MMCONFIG
-extern void __init pci_mmcfg_early_init(void);
-extern void __init pci_mmcfg_late_init(void);
+void __init pci_mmcfg_early_init(void);
+void __init pci_mmcfg_late_init(void);
 #else
 static inline void pci_mmcfg_early_init(void) { }
 static inline void pci_mmcfg_late_init(void) { }
@@ -1639,12 +1657,12 @@ int pci_ext_cfg_avail(void);
 void __iomem *pci_ioremap_bar(struct pci_dev *pdev, int bar);
 
 #ifdef CONFIG_PCI_IOV
-extern int pci_enable_sriov(struct pci_dev *dev, int nr_virtfn);
-extern void pci_disable_sriov(struct pci_dev *dev);
-extern irqreturn_t pci_sriov_migration(struct pci_dev *dev);
-extern int pci_num_vf(struct pci_dev *dev);
-extern int pci_sriov_set_totalvfs(struct pci_dev *dev, u16 numvfs);
-extern int pci_sriov_get_totalvfs(struct pci_dev *dev);
+int pci_enable_sriov(struct pci_dev *dev, int nr_virtfn);
+void pci_disable_sriov(struct pci_dev *dev);
+irqreturn_t pci_sriov_migration(struct pci_dev *dev);
+int pci_num_vf(struct pci_dev *dev);
+int pci_sriov_set_totalvfs(struct pci_dev *dev, u16 numvfs);
+int pci_sriov_get_totalvfs(struct pci_dev *dev);
 #else
 static inline int pci_enable_sriov(struct pci_dev *dev, int nr_virtfn)
 {
@@ -1672,8 +1690,8 @@ static inline int pci_sriov_get_totalvfs(struct pci_dev *dev)
 #endif
 
 #if defined(CONFIG_HOTPLUG_PCI) || defined(CONFIG_HOTPLUG_PCI_MODULE)
-extern void pci_hp_create_module_link(struct pci_slot *pci_slot);
-extern void pci_hp_remove_module_link(struct pci_slot *pci_slot);
+void pci_hp_create_module_link(struct pci_slot *pci_slot);
+void pci_hp_remove_module_link(struct pci_slot *pci_slot);
 #endif
 
 /**
@@ -1817,13 +1835,13 @@ int pci_vpd_find_info_keyword(const u8 *buf, unsigned int off,
 /* PCI <-> OF binding helpers */
 #ifdef CONFIG_OF
 struct device_node;
-extern void pci_set_of_node(struct pci_dev *dev);
-extern void pci_release_of_node(struct pci_dev *dev);
-extern void pci_set_bus_of_node(struct pci_bus *bus);
-extern void pci_release_bus_of_node(struct pci_bus *bus);
+void pci_set_of_node(struct pci_dev *dev);
+void pci_release_of_node(struct pci_dev *dev);
+void pci_set_bus_of_node(struct pci_bus *bus);
+void pci_release_bus_of_node(struct pci_bus *bus);
 
 /* Arch may override this (weak) */
-extern struct device_node * __weak pcibios_get_phb_of_node(struct pci_bus *bus);
+struct device_node *pcibios_get_phb_of_node(struct pci_bus *bus);
 
 static inline struct device_node *
 pci_device_to_OF_node(const struct pci_dev *pdev)
diff --git a/include/linux/pci_hotplug.h b/include/linux/pci_hotplug.h
index 45fc162cbdc..8db71dcd633 100644
--- a/include/linux/pci_hotplug.h
+++ b/include/linux/pci_hotplug.h
@@ -125,12 +125,12 @@ static inline const char *hotplug_slot_name(const struct hotplug_slot *slot)
 	return pci_slot_name(slot->pci_slot);
 }
 
-extern int __pci_hp_register(struct hotplug_slot *slot, struct pci_bus *pbus,
-			     int nr, const char *name,
-			     struct module *owner, const char *mod_name);
-extern int pci_hp_deregister(struct hotplug_slot *slot);
-extern int __must_check pci_hp_change_slot_info	(struct hotplug_slot *slot,
-						 struct hotplug_slot_info *info);
+int __pci_hp_register(struct hotplug_slot *slot, struct pci_bus *pbus, int nr,
+		      const char *name, struct module *owner,
+		      const char *mod_name);
+int pci_hp_deregister(struct hotplug_slot *slot);
+int __must_check pci_hp_change_slot_info(struct hotplug_slot *slot,
+					 struct hotplug_slot_info *info);
 
 /* use a define to avoid include chaining to get THIS_MODULE & friends */
 #define pci_hp_register(slot, pbus, devnr, name) \
diff --git a/include/linux/pci_ids.h b/include/linux/pci_ids.h
index f11c1c2609d..2b85c521f73 100644
--- a/include/linux/pci_ids.h
+++ b/include/linux/pci_ids.h
@@ -524,6 +524,8 @@
 #define PCI_DEVICE_ID_AMD_15H_NB_F3	0x1603
 #define PCI_DEVICE_ID_AMD_15H_NB_F4	0x1604
 #define PCI_DEVICE_ID_AMD_15H_NB_F5	0x1605
+#define PCI_DEVICE_ID_AMD_16H_NB_F3	0x1533
+#define PCI_DEVICE_ID_AMD_16H_NB_F4	0x1534
 #define PCI_DEVICE_ID_AMD_CNB17H_F3	0x1703
 #define PCI_DEVICE_ID_AMD_LANCE		0x2000
 #define PCI_DEVICE_ID_AMD_LANCE_HOME	0x2001
@@ -1604,6 +1606,7 @@
 #define PCI_SUBDEVICE_ID_KEYSPAN_SX2	0x5334
 
 #define PCI_VENDOR_ID_MARVELL		0x11ab
+#define PCI_VENDOR_ID_MARVELL_EXT	0x1b4b
 #define PCI_DEVICE_ID_MARVELL_GT64111	0x4146
 #define PCI_DEVICE_ID_MARVELL_GT64260	0x6430
 #define PCI_DEVICE_ID_MARVELL_MV64360	0x6460
diff --git a/include/linux/pcieport_if.h b/include/linux/pcieport_if.h
index e6f91b1406d..9572669eea9 100644
--- a/include/linux/pcieport_if.h
+++ b/include/linux/pcieport_if.h
@@ -62,7 +62,7 @@ struct pcie_port_service_driver {
 #define to_service_driver(d) \
 	container_of(d, struct pcie_port_service_driver, driver)
 
-extern int pcie_port_service_register(struct pcie_port_service_driver *new);
-extern void pcie_port_service_unregister(struct pcie_port_service_driver *new);
+int pcie_port_service_register(struct pcie_port_service_driver *new);
+void pcie_port_service_unregister(struct pcie_port_service_driver *new);
 
 #endif /* _PCIEPORT_IF_H_ */
diff --git a/include/linux/perf_event.h b/include/linux/perf_event.h
index 1d795df6f4c..e0373d26c24 100644
--- a/include/linux/perf_event.h
+++ b/include/linux/perf_event.h
@@ -21,7 +21,6 @@
  */
 
 #ifdef CONFIG_PERF_EVENTS
-# include <linux/cgroup.h>
 # include <asm/perf_event.h>
 # include <asm/local64.h>
 #endif
@@ -128,6 +127,7 @@ struct hw_perf_event {
 			int		event_base_rdpmc;
 			int		idx;
 			int		last_cpu;
+			int		flags;
 
 			struct hw_perf_event_extra extra_reg;
 			struct hw_perf_event_extra branch_reg;
@@ -299,22 +299,7 @@ struct swevent_hlist {
 #define PERF_ATTACH_GROUP	0x02
 #define PERF_ATTACH_TASK	0x04
 
-#ifdef CONFIG_CGROUP_PERF
-/*
- * perf_cgroup_info keeps track of time_enabled for a cgroup.
- * This is a per-cpu dynamically allocated data structure.
- */
-struct perf_cgroup_info {
-	u64				time;
-	u64				timestamp;
-};
-
-struct perf_cgroup {
-	struct				cgroup_subsys_state css;
-	struct				perf_cgroup_info *info;	/* timing info, one per cpu */
-};
-#endif
-
+struct perf_cgroup;
 struct ring_buffer;
 
 /**
@@ -583,11 +568,13 @@ struct perf_sample_data {
 		u32	reserved;
 	}				cpu_entry;
 	u64				period;
+	union  perf_mem_data_src	data_src;
 	struct perf_callchain_entry	*callchain;
 	struct perf_raw_record		*raw;
 	struct perf_branch_stack	*br_stack;
 	struct perf_regs_user		regs_user;
 	u64				stack_user_size;
+	u64				weight;
 };
 
 static inline void perf_sample_data_init(struct perf_sample_data *data,
@@ -601,6 +588,8 @@ static inline void perf_sample_data_init(struct perf_sample_data *data,
 	data->regs_user.abi = PERF_SAMPLE_REGS_ABI_NONE;
 	data->regs_user.regs = NULL;
 	data->stack_user_size = 0;
+	data->weight = 0;
+	data->data_src.val = 0;
 }
 
 extern void perf_output_sample(struct perf_output_handle *handle,
@@ -831,6 +820,7 @@ do {									\
 struct perf_pmu_events_attr {
 	struct device_attribute attr;
 	u64 id;
+	const char *event_str;
 };
 
 #define PMU_EVENT_ATTR(_name, _var, _id, _show)				\
diff --git a/include/linux/pinctrl/pinconf.h b/include/linux/pinctrl/pinconf.h
index e7a720104a4..1ad4f31ef6b 100644
--- a/include/linux/pinctrl/pinconf.h
+++ b/include/linux/pinctrl/pinconf.h
@@ -14,6 +14,8 @@
 
 #ifdef CONFIG_PINCONF
 
+#include <linux/pinctrl/machine.h>
+
 struct pinctrl_dev;
 struct seq_file;
 
@@ -28,6 +30,7 @@ struct seq_file;
  * @pin_config_set: configure an individual pin
  * @pin_config_group_get: get configurations for an entire pin group
  * @pin_config_group_set: configure all pins in a group
+ * @pin_config_group_dbg_set: optional debugfs to modify a pin configuration
  * @pin_config_dbg_show: optional debugfs display hook that will provide
  *	per-device info for a certain pin in debugfs
  * @pin_config_group_dbg_show: optional debugfs display hook that will provide
@@ -51,6 +54,9 @@ struct pinconf_ops {
 	int (*pin_config_group_set) (struct pinctrl_dev *pctldev,
 				     unsigned selector,
 				     unsigned long config);
+	int (*pin_config_dbg_parse_modify) (struct pinctrl_dev *pctldev,
+					   const char *arg,
+					   unsigned long *config);
 	void (*pin_config_dbg_show) (struct pinctrl_dev *pctldev,
 				     struct seq_file *s,
 				     unsigned offset);
diff --git a/include/linux/pinctrl/pinctrl.h b/include/linux/pinctrl/pinctrl.h
index 778804df293..2c2a9e8d857 100644
--- a/include/linux/pinctrl/pinctrl.h
+++ b/include/linux/pinctrl/pinctrl.h
@@ -118,9 +118,9 @@ struct pinctrl_desc {
 	const char *name;
 	struct pinctrl_pin_desc const *pins;
 	unsigned int npins;
-	struct pinctrl_ops *pctlops;
-	struct pinmux_ops *pmxops;
-	struct pinconf_ops *confops;
+	const struct pinctrl_ops *pctlops;
+	const struct pinmux_ops *pmxops;
+	const struct pinconf_ops *confops;
 	struct module *owner;
 };
 
diff --git a/include/linux/platform_data/coda.h b/include/linux/platform_data/coda.h
new file mode 100644
index 00000000000..6ad4410d9e2
--- /dev/null
+++ b/include/linux/platform_data/coda.h
@@ -0,0 +1,18 @@
+/*
+ * Copyright (C) 2013 Philipp Zabel, Pengutronix
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#ifndef PLATFORM_CODA_H
+#define PLATFORM_CODA_H
+
+struct device;
+
+struct coda_platform_data {
+	struct device *iram_dev;
+};
+
+#endif
diff --git a/include/linux/platform_data/dwc3-omap.h b/include/linux/platform_data/dwc3-omap.h
index ada401244e0..1d36ca874cc 100644
--- a/include/linux/platform_data/dwc3-omap.h
+++ b/include/linux/platform_data/dwc3-omap.h
@@ -41,7 +41,3 @@ enum dwc3_omap_utmi_mode {
 	DWC3_OMAP_UTMI_MODE_HW,
 	DWC3_OMAP_UTMI_MODE_SW,
 };
-
-struct dwc3_omap_data {
-	enum dwc3_omap_utmi_mode	utmi_mode;
-};
diff --git a/include/linux/platform_data/emif_plat.h b/include/linux/platform_data/emif_plat.h
index 03378ca8406..5c19a2a647c 100644
--- a/include/linux/platform_data/emif_plat.h
+++ b/include/linux/platform_data/emif_plat.h
@@ -40,6 +40,7 @@
 /* Custom config requests */
 #define EMIF_CUSTOM_CONFIG_LPMODE			0x00000001
 #define EMIF_CUSTOM_CONFIG_TEMP_ALERT_POLL_INTERVAL	0x00000002
+#define EMIF_CUSTOM_CONFIG_EXTENDED_TEMP_PART		0x00000004
 
 #ifndef __ASSEMBLY__
 /**
diff --git a/include/linux/platform_data/lp855x.h b/include/linux/platform_data/lp855x.h
index 20ee8b221db..ea3200527dd 100644
--- a/include/linux/platform_data/lp855x.h
+++ b/include/linux/platform_data/lp855x.h
@@ -69,11 +69,6 @@ enum lp855x_chip_id {
 	LP8557,
 };
 
-enum lp855x_brightness_ctrl_mode {
-	PWM_BASED = 1,
-	REGISTER_BASED,
-};
-
 enum lp8550_brighntess_source {
 	LP8550_PWM_ONLY,
 	LP8550_I2C_ONLY = 2,
@@ -116,24 +111,18 @@ struct lp855x_rom_data {
 /**
  * struct lp855x_platform_data
  * @name : Backlight driver name. If it is not defined, default name is set.
- * @mode : brightness control by pwm or lp855x register
  * @device_control : value of DEVICE CONTROL register
  * @initial_brightness : initial value of backlight brightness
  * @period_ns : platform specific pwm period value. unit is nano.
 		Only valid when mode is PWM_BASED.
- * @load_new_rom_data :
-	0 : use default configuration data
-	1 : update values of eeprom or eprom registers on loading driver
  * @size_program : total size of lp855x_rom_data
  * @rom_data : list of new eeprom/eprom registers
  */
 struct lp855x_platform_data {
-	char *name;
-	enum lp855x_brightness_ctrl_mode mode;
+	const char *name;
 	u8 device_control;
-	int initial_brightness;
+	u8 initial_brightness;
 	unsigned int period_ns;
-	u8 load_new_rom_data;
 	int size_program;
 	struct lp855x_rom_data *rom_data;
 };
diff --git a/include/linux/platform_data/mv_usb.h b/include/linux/platform_data/mv_usb.h
index 944b01dd103..98b7925f1a2 100644
--- a/include/linux/platform_data/mv_usb.h
+++ b/include/linux/platform_data/mv_usb.h
@@ -34,8 +34,6 @@ struct mv_usb_addon_irq {
 };
 
 struct mv_usb_platform_data {
-	unsigned int		clknum;
-	char			**clkname;
 	struct mv_usb_addon_irq	*id;	/* Only valid for OTG. ID pin change*/
 	struct mv_usb_addon_irq	*vbus;	/* valid for OTG/UDC. VBUS change*/
 
diff --git a/include/linux/platform_data/ntc_thermistor.h b/include/linux/platform_data/ntc_thermistor.h
index 88734e871e3..c7285b57546 100644
--- a/include/linux/platform_data/ntc_thermistor.h
+++ b/include/linux/platform_data/ntc_thermistor.h
@@ -21,6 +21,8 @@
 #ifndef _LINUX_NTC_H
 #define _LINUX_NTC_H
 
+struct iio_channel;
+
 enum ntc_thermistor_type {
 	TYPE_NCPXXWB473,
 	TYPE_NCPXXWL333,
@@ -39,13 +41,17 @@ struct ntc_thermistor_platform_data {
 	 * described at Documentation/hwmon/ntc_thermistor
 	 *
 	 * pullup/down_ohm: 0 for infinite / not-connected
+	 *
+	 * chan: iio_channel pointer to communicate with the ADC which the
+	 * thermistor is using for conversion of the analog values.
 	 */
-	int (*read_uV)(void);
-	unsigned int pullup_uV;
+	int (*read_uv)(struct ntc_thermistor_platform_data *);
+	unsigned int pullup_uv;
 
 	unsigned int pullup_ohm;
 	unsigned int pulldown_ohm;
 	enum { NTC_CONNECTED_POSITIVE, NTC_CONNECTED_GROUND } connect;
+	struct iio_channel *chan;
 
 	int (*read_ohm)(void);
 };
diff --git a/include/linux/platform_data/serial-sccnxp.h b/include/linux/platform_data/serial-sccnxp.h
index 215574d1e81..bdc510d0324 100644
--- a/include/linux/platform_data/serial-sccnxp.h
+++ b/include/linux/platform_data/serial-sccnxp.h
@@ -86,10 +86,6 @@ struct sccnxp_pdata {
 	const u32		mctrl_cfg[SCCNXP_MAX_UARTS];
 	/* Timer value for polling mode (usecs) */
 	const unsigned int	poll_time_us;
-	/* Called during startup */
-	void (*init)(void);
-	/* Called before finish */
-	void (*exit)(void);
 };
 
 #endif
diff --git a/include/linux/platform_data/si5351.h b/include/linux/platform_data/si5351.h
new file mode 100644
index 00000000000..92dabcaf649
--- /dev/null
+++ b/include/linux/platform_data/si5351.h
@@ -0,0 +1,114 @@
+/*
+ * Si5351A/B/C programmable clock generator platform_data.
+ */
+
+#ifndef __LINUX_PLATFORM_DATA_SI5351_H__
+#define __LINUX_PLATFORM_DATA_SI5351_H__
+
+struct clk;
+
+/**
+ * enum si5351_variant - SiLabs Si5351 chip variant
+ * @SI5351_VARIANT_A: Si5351A (8 output clocks, XTAL input)
+ * @SI5351_VARIANT_A3: Si5351A MSOP10 (3 output clocks, XTAL input)
+ * @SI5351_VARIANT_B: Si5351B (8 output clocks, XTAL/VXCO input)
+ * @SI5351_VARIANT_C: Si5351C (8 output clocks, XTAL/CLKIN input)
+ */
+enum si5351_variant {
+	SI5351_VARIANT_A = 1,
+	SI5351_VARIANT_A3 = 2,
+	SI5351_VARIANT_B = 3,
+	SI5351_VARIANT_C = 4,
+};
+
+/**
+ * enum si5351_pll_src - Si5351 pll clock source
+ * @SI5351_PLL_SRC_DEFAULT: default, do not change eeprom config
+ * @SI5351_PLL_SRC_XTAL: pll source clock is XTAL input
+ * @SI5351_PLL_SRC_CLKIN: pll source clock is CLKIN input (Si5351C only)
+ */
+enum si5351_pll_src {
+	SI5351_PLL_SRC_DEFAULT = 0,
+	SI5351_PLL_SRC_XTAL = 1,
+	SI5351_PLL_SRC_CLKIN = 2,
+};
+
+/**
+ * enum si5351_multisynth_src - Si5351 multisynth clock source
+ * @SI5351_MULTISYNTH_SRC_DEFAULT: default, do not change eeprom config
+ * @SI5351_MULTISYNTH_SRC_VCO0: multisynth source clock is VCO0
+ * @SI5351_MULTISYNTH_SRC_VCO1: multisynth source clock is VCO1/VXCO
+ */
+enum si5351_multisynth_src {
+	SI5351_MULTISYNTH_SRC_DEFAULT = 0,
+	SI5351_MULTISYNTH_SRC_VCO0 = 1,
+	SI5351_MULTISYNTH_SRC_VCO1 = 2,
+};
+
+/**
+ * enum si5351_clkout_src - Si5351 clock output clock source
+ * @SI5351_CLKOUT_SRC_DEFAULT: default, do not change eeprom config
+ * @SI5351_CLKOUT_SRC_MSYNTH_N: clkout N source clock is multisynth N
+ * @SI5351_CLKOUT_SRC_MSYNTH_0_4: clkout N source clock is multisynth 0 (N<4)
+ *                                or 4 (N>=4)
+ * @SI5351_CLKOUT_SRC_XTAL: clkout N source clock is XTAL
+ * @SI5351_CLKOUT_SRC_CLKIN: clkout N source clock is CLKIN (Si5351C only)
+ */
+enum si5351_clkout_src {
+	SI5351_CLKOUT_SRC_DEFAULT = 0,
+	SI5351_CLKOUT_SRC_MSYNTH_N = 1,
+	SI5351_CLKOUT_SRC_MSYNTH_0_4 = 2,
+	SI5351_CLKOUT_SRC_XTAL = 3,
+	SI5351_CLKOUT_SRC_CLKIN = 4,
+};
+
+/**
+ * enum si5351_drive_strength - Si5351 clock output drive strength
+ * @SI5351_DRIVE_DEFAULT: default, do not change eeprom config
+ * @SI5351_DRIVE_2MA: 2mA clock output drive strength
+ * @SI5351_DRIVE_4MA: 4mA clock output drive strength
+ * @SI5351_DRIVE_6MA: 6mA clock output drive strength
+ * @SI5351_DRIVE_8MA: 8mA clock output drive strength
+ */
+enum si5351_drive_strength {
+	SI5351_DRIVE_DEFAULT = 0,
+	SI5351_DRIVE_2MA = 2,
+	SI5351_DRIVE_4MA = 4,
+	SI5351_DRIVE_6MA = 6,
+	SI5351_DRIVE_8MA = 8,
+};
+
+/**
+ * struct si5351_clkout_config - Si5351 clock output configuration
+ * @clkout: clkout number
+ * @multisynth_src: multisynth source clock
+ * @clkout_src: clkout source clock
+ * @pll_master: if true, clkout can also change pll rate
+ * @drive: output drive strength
+ * @rate: initial clkout rate, or default if 0
+ */
+struct si5351_clkout_config {
+	enum si5351_multisynth_src multisynth_src;
+	enum si5351_clkout_src clkout_src;
+	enum si5351_drive_strength drive;
+	bool pll_master;
+	unsigned long rate;
+};
+
+/**
+ * struct si5351_platform_data - Platform data for the Si5351 clock driver
+ * @variant: Si5351 chip variant
+ * @clk_xtal: xtal input clock
+ * @clk_clkin: clkin input clock
+ * @pll_src: array of pll source clock setting
+ * @clkout: array of clkout configuration
+ */
+struct si5351_platform_data {
+	enum si5351_variant variant;
+	struct clk *clk_xtal;
+	struct clk *clk_clkin;
+	enum si5351_pll_src pll_src[2];
+	struct si5351_clkout_config clkout[8];
+};
+
+#endif
diff --git a/include/linux/platform_data/spi-s3c64xx.h b/include/linux/platform_data/spi-s3c64xx.h
index ceba18d23a5..8447f634c7f 100644
--- a/include/linux/platform_data/spi-s3c64xx.h
+++ b/include/linux/platform_data/spi-s3c64xx.h
@@ -11,6 +11,8 @@
 #ifndef __S3C64XX_PLAT_SPI_H
 #define __S3C64XX_PLAT_SPI_H
 
+#include <linux/dmaengine.h>
+
 struct platform_device;
 
 /**
@@ -38,6 +40,7 @@ struct s3c64xx_spi_info {
 	int src_clk_nr;
 	int num_cs;
 	int (*cfg_gpio)(void);
+	dma_filter_fn filter;
 };
 
 /**
diff --git a/include/linux/platform_data/video-vt8500lcdfb.h b/include/linux/platform_data/video-vt8500lcdfb.h
deleted file mode 100644
index 7f399c370fe..00000000000
--- a/include/linux/platform_data/video-vt8500lcdfb.h
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- *  VT8500/WM8505 Frame Buffer platform data definitions
- *
- *  Copyright (C) 2010 Ed Spiridonov <edo.rus@gmail.com>
- *
- * 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.
- */
-
-#ifndef _VT8500FB_H
-#define _VT8500FB_H
-
-#include <linux/fb.h>
-
-struct vt8500fb_platform_data {
-	struct fb_videomode	mode;
-	u32			xres_virtual;
-	u32			yres_virtual;
-	u32			bpp;
-	unsigned long		video_mem_phys;
-	void			*video_mem_virt;
-	unsigned long		video_mem_len;
-};
-
-#endif /* _VT8500FB_H */
diff --git a/include/linux/platform_data/video_s3c.h b/include/linux/platform_data/video_s3c.h
new file mode 100644
index 00000000000..48883995f47
--- /dev/null
+++ b/include/linux/platform_data/video_s3c.h
@@ -0,0 +1,54 @@
+#ifndef __PLATFORM_DATA_VIDEO_S3C
+#define __PLATFORM_DATA_VIDEO_S3C
+
+/* S3C_FB_MAX_WIN
+ * Set to the maximum number of windows that any of the supported hardware
+ * can use. Since the platform data uses this for an array size, having it
+ * set to the maximum of any version of the hardware can do is safe.
+ */
+#define S3C_FB_MAX_WIN	(5)
+
+/**
+ * struct s3c_fb_pd_win - per window setup data
+ * @xres     : The window X size.
+ * @yres     : The window Y size.
+ * @virtual_x: The virtual X size.
+ * @virtual_y: The virtual Y size.
+ */
+struct s3c_fb_pd_win {
+	unsigned short		default_bpp;
+	unsigned short		max_bpp;
+	unsigned short		xres;
+	unsigned short		yres;
+	unsigned short		virtual_x;
+	unsigned short		virtual_y;
+};
+
+/**
+ * struct s3c_fb_platdata -  S3C driver platform specific information
+ * @setup_gpio: Setup the external GPIO pins to the right state to transfer
+ *		the data from the display system to the connected display
+ *		device.
+ * @vidcon0: The base vidcon0 values to control the panel data format.
+ * @vidcon1: The base vidcon1 values to control the panel data output.
+ * @vtiming: Video timing when connected to a RGB type panel.
+ * @win: The setup data for each hardware window, or NULL for unused.
+ * @display_mode: The LCD output display mode.
+ *
+ * The platform data supplies the video driver with all the information
+ * it requires to work with the display(s) attached to the machine. It
+ * controls the initial mode, the number of display windows (0 is always
+ * the base framebuffer) that are initialised etc.
+ *
+ */
+struct s3c_fb_platdata {
+	void	(*setup_gpio)(void);
+
+	struct s3c_fb_pd_win	*win[S3C_FB_MAX_WIN];
+	struct fb_videomode     *vtiming;
+
+	u32			 vidcon0;
+	u32			 vidcon1;
+};
+
+#endif
diff --git a/include/linux/platform_device.h b/include/linux/platform_device.h
index c082c71f722..9abf1db6aea 100644
--- a/include/linux/platform_device.h
+++ b/include/linux/platform_device.h
@@ -20,12 +20,12 @@
 struct mfd_cell;
 
 struct platform_device {
-	const char	* name;
+	const char	*name;
 	int		id;
 	bool		id_auto;
 	struct device	dev;
 	u32		num_resources;
-	struct resource	* resource;
+	struct resource	*resource;
 
 	const struct platform_device_id	*id_entry;
 
@@ -47,9 +47,12 @@ extern struct bus_type platform_bus_type;
 extern struct device platform_bus;
 
 extern void arch_setup_pdev_archdata(struct platform_device *);
-extern struct resource *platform_get_resource(struct platform_device *, unsigned int, unsigned int);
+extern struct resource *platform_get_resource(struct platform_device *,
+					      unsigned int, unsigned int);
 extern int platform_get_irq(struct platform_device *, unsigned int);
-extern struct resource *platform_get_resource_byname(struct platform_device *, unsigned int, const char *);
+extern struct resource *platform_get_resource_byname(struct platform_device *,
+						     unsigned int,
+						     const char *);
 extern int platform_get_irq_byname(struct platform_device *, const char *);
 extern int platform_add_devices(struct platform_device **, int);
 
@@ -161,7 +164,8 @@ extern struct platform_device *platform_device_alloc(const char *name, int id);
 extern int platform_device_add_resources(struct platform_device *pdev,
 					 const struct resource *res,
 					 unsigned int num);
-extern int platform_device_add_data(struct platform_device *pdev, const void *data, size_t size);
+extern int platform_device_add_data(struct platform_device *pdev,
+				    const void *data, size_t size);
 extern int platform_device_add(struct platform_device *pdev);
 extern void platform_device_del(struct platform_device *pdev);
 extern void platform_device_put(struct platform_device *pdev);
@@ -190,7 +194,8 @@ static inline void *platform_get_drvdata(const struct platform_device *pdev)
 	return dev_get_drvdata(&pdev->dev);
 }
 
-static inline void platform_set_drvdata(struct platform_device *pdev, void *data)
+static inline void platform_set_drvdata(struct platform_device *pdev,
+					void *data)
 {
 	dev_set_drvdata(&pdev->dev, data);
 }
@@ -222,10 +227,10 @@ static void __exit __platform_driver##_exit(void) \
 } \
 module_exit(__platform_driver##_exit);
 
-extern struct platform_device *platform_create_bundle(struct platform_driver *driver,
-					int (*probe)(struct platform_device *),
-					struct resource *res, unsigned int n_res,
-					const void *data, size_t size);
+extern struct platform_device *platform_create_bundle(
+	struct platform_driver *driver, int (*probe)(struct platform_device *),
+	struct resource *res, unsigned int n_res,
+	const void *data, size_t size);
 
 /* early platform driver interface */
 struct early_platform_driver {
diff --git a/include/linux/posix-timers.h b/include/linux/posix-timers.h
index 042058fdb0a..60bac697a91 100644
--- a/include/linux/posix-timers.h
+++ b/include/linux/posix-timers.h
@@ -55,6 +55,7 @@ struct cpu_timer_list {
 /* POSIX.1b interval timer structure. */
 struct k_itimer {
 	struct list_head list;		/* free/ allocate list */
+	struct hlist_node t_hash;
 	spinlock_t it_lock;
 	clockid_t it_clock;		/* which timer type */
 	timer_t it_id;			/* timer id */
diff --git a/include/linux/preempt.h b/include/linux/preempt.h
index 5a710b9c578..87a03c746f1 100644
--- a/include/linux/preempt.h
+++ b/include/linux/preempt.h
@@ -93,14 +93,20 @@ do { \
 
 #else /* !CONFIG_PREEMPT_COUNT */
 
-#define preempt_disable()		do { } while (0)
-#define sched_preempt_enable_no_resched()	do { } while (0)
-#define preempt_enable_no_resched()	do { } while (0)
-#define preempt_enable()		do { } while (0)
-
-#define preempt_disable_notrace()		do { } while (0)
-#define preempt_enable_no_resched_notrace()	do { } while (0)
-#define preempt_enable_notrace()		do { } while (0)
+/*
+ * Even if we don't have any preemption, we need preempt disable/enable
+ * to be barriers, so that we don't have things like get_user/put_user
+ * that can cause faults and scheduling migrate into our preempt-protected
+ * region.
+ */
+#define preempt_disable()		barrier()
+#define sched_preempt_enable_no_resched()	barrier()
+#define preempt_enable_no_resched()	barrier()
+#define preempt_enable()		barrier()
+
+#define preempt_disable_notrace()		barrier()
+#define preempt_enable_no_resched_notrace()	barrier()
+#define preempt_enable_notrace()		barrier()
 
 #endif /* CONFIG_PREEMPT_COUNT */
 
diff --git a/include/linux/printk.h b/include/linux/printk.h
index 822171fcb1c..4890fe62c01 100644
--- a/include/linux/printk.h
+++ b/include/linux/printk.h
@@ -1,6 +1,7 @@
 #ifndef __KERNEL_PRINTK__
 #define __KERNEL_PRINTK__
 
+#include <stdarg.h>
 #include <linux/init.h>
 #include <linux/kern_levels.h>
 
@@ -95,8 +96,14 @@ int no_printk(const char *fmt, ...)
 	return 0;
 }
 
+#ifdef CONFIG_EARLY_PRINTK
 extern asmlinkage __printf(1, 2)
 void early_printk(const char *fmt, ...);
+void early_vprintk(const char *fmt, va_list ap);
+#else
+static inline __printf(1, 2) __cold
+void early_printk(const char *s, ...) { }
+#endif
 
 #ifdef CONFIG_PRINTK
 asmlinkage __printf(5, 0)
diff --git a/include/linux/proc_fs.h b/include/linux/proc_fs.h
index 8307f2f94d8..94dfb2aa553 100644
--- a/include/linux/proc_fs.h
+++ b/include/linux/proc_fs.h
@@ -117,6 +117,7 @@ struct proc_dir_entry *proc_create_data(const char *name, umode_t mode,
 				const struct file_operations *proc_fops,
 				void *data);
 extern void remove_proc_entry(const char *name, struct proc_dir_entry *parent);
+extern int remove_proc_subtree(const char *name, struct proc_dir_entry *parent);
 
 struct pid_namespace;
 
@@ -202,6 +203,7 @@ static inline struct proc_dir_entry *proc_create_data(const char *name,
 	return NULL;
 }
 #define remove_proc_entry(name, parent) do {} while (0)
+#define remove_proc_subtree(name, parent) do {} while (0)
 
 static inline struct proc_dir_entry *proc_symlink(const char *name,
 		struct proc_dir_entry *parent,const char *dest) {return NULL;}
diff --git a/include/linux/ramfs.h b/include/linux/ramfs.h
index 5bf5500db83..69e37c2d1ea 100644
--- a/include/linux/ramfs.h
+++ b/include/linux/ramfs.h
@@ -6,7 +6,13 @@ struct inode *ramfs_get_inode(struct super_block *sb, const struct inode *dir,
 extern struct dentry *ramfs_mount(struct file_system_type *fs_type,
 	 int flags, const char *dev_name, void *data);
 
-#ifndef CONFIG_MMU
+#ifdef CONFIG_MMU
+static inline int
+ramfs_nommu_expand_for_mapping(struct inode *inode, size_t newsize)
+{
+	return 0;
+}
+#else
 extern int ramfs_nommu_expand_for_mapping(struct inode *inode, size_t newsize);
 extern unsigned long ramfs_nommu_get_unmapped_area(struct file *file,
 						   unsigned long addr,
diff --git a/include/linux/rculist_bl.h b/include/linux/rculist_bl.h
index cf1244fbf3b..4f216c59e7d 100644
--- a/include/linux/rculist_bl.h
+++ b/include/linux/rculist_bl.h
@@ -20,7 +20,7 @@ static inline void hlist_bl_set_first_rcu(struct hlist_bl_head *h,
 static inline struct hlist_bl_node *hlist_bl_first_rcu(struct hlist_bl_head *h)
 {
 	return (struct hlist_bl_node *)
-		((unsigned long)rcu_dereference(h->first) & ~LIST_BL_LOCKMASK);
+		((unsigned long)rcu_dereference_check(h->first, hlist_bl_is_locked(h)) & ~LIST_BL_LOCKMASK);
 }
 
 /**
diff --git a/include/linux/rcupdate.h b/include/linux/rcupdate.h
index b758ce17b30..9ed2c9a4de4 100644
--- a/include/linux/rcupdate.h
+++ b/include/linux/rcupdate.h
@@ -80,6 +80,7 @@ extern void do_trace_rcu_torture_read(char *rcutorturename,
 #define UINT_CMP_LT(a, b)	(UINT_MAX / 2 < (a) - (b))
 #define ULONG_CMP_GE(a, b)	(ULONG_MAX / 2 >= (a) - (b))
 #define ULONG_CMP_LT(a, b)	(ULONG_MAX / 2 < (a) - (b))
+#define ulong2long(a)		(*(long *)(&(a)))
 
 /* Exported common interfaces */
 
diff --git a/include/linux/regmap.h b/include/linux/regmap.h
index bf77dfdabef..02d84e24b7c 100644
--- a/include/linux/regmap.h
+++ b/include/linux/regmap.h
@@ -389,6 +389,7 @@ int regmap_update_bits_check(struct regmap *map, unsigned int reg,
 			     bool *change);
 int regmap_get_val_bytes(struct regmap *map);
 int regmap_async_complete(struct regmap *map);
+bool regmap_can_raw_write(struct regmap *map);
 
 int regcache_sync(struct regmap *map);
 int regcache_sync_region(struct regmap *map, unsigned int min,
diff --git a/include/linux/regulator/ab8500.h b/include/linux/regulator/ab8500.h
index 7bd73bbdfd1..7c5ff0c5577 100644
--- a/include/linux/regulator/ab8500.h
+++ b/include/linux/regulator/ab8500.h
@@ -5,11 +5,14 @@
  *
  * Authors: Sundar Iyer <sundar.iyer@stericsson.com> for ST-Ericsson
  *          Bengt Jonsson <bengt.g.jonsson@stericsson.com> for ST-Ericsson
+ *          Daniel Willerud <daniel.willerud@stericsson.com> for ST-Ericsson
  */
 
 #ifndef __LINUX_MFD_AB8500_REGULATOR_H
 #define __LINUX_MFD_AB8500_REGULATOR_H
 
+#include <linux/platform_device.h>
+
 /* AB8500 regulators */
 enum ab8500_regulator_id {
 	AB8500_LDO_AUX1,
@@ -17,7 +20,6 @@ enum ab8500_regulator_id {
 	AB8500_LDO_AUX3,
 	AB8500_LDO_INTCORE,
 	AB8500_LDO_TVOUT,
-	AB8500_LDO_USB,
 	AB8500_LDO_AUDIO,
 	AB8500_LDO_ANAMIC1,
 	AB8500_LDO_ANAMIC2,
@@ -26,7 +28,28 @@ enum ab8500_regulator_id {
 	AB8500_NUM_REGULATORS,
 };
 
-/* AB9450 regulators */
+/* AB8505 regulators */
+enum ab8505_regulator_id {
+	AB8505_LDO_AUX1,
+	AB8505_LDO_AUX2,
+	AB8505_LDO_AUX3,
+	AB8505_LDO_AUX4,
+	AB8505_LDO_AUX5,
+	AB8505_LDO_AUX6,
+	AB8505_LDO_INTCORE,
+	AB8505_LDO_ADC,
+	AB8505_LDO_USB,
+	AB8505_LDO_AUDIO,
+	AB8505_LDO_ANAMIC1,
+	AB8505_LDO_ANAMIC2,
+	AB8505_LDO_AUX8,
+	AB8505_LDO_ANA,
+	AB8505_SYSCLKREQ_2,
+	AB8505_SYSCLKREQ_4,
+	AB8505_NUM_REGULATORS,
+};
+
+/* AB9540 regulators */
 enum ab9540_regulator_id {
 	AB9540_LDO_AUX1,
 	AB9540_LDO_AUX2,
@@ -45,16 +68,39 @@ enum ab9540_regulator_id {
 	AB9540_NUM_REGULATORS,
 };
 
-/* AB8500 and AB9540 register initialization */
+/* AB8540 regulators */
+enum ab8540_regulator_id {
+	AB8540_LDO_AUX1,
+	AB8540_LDO_AUX2,
+	AB8540_LDO_AUX3,
+	AB8540_LDO_AUX4,
+	AB8540_LDO_AUX5,
+	AB8540_LDO_AUX6,
+	AB8540_LDO_INTCORE,
+	AB8540_LDO_TVOUT,
+	AB8540_LDO_AUDIO,
+	AB8540_LDO_ANAMIC1,
+	AB8540_LDO_ANAMIC2,
+	AB8540_LDO_DMIC,
+	AB8540_LDO_ANA,
+	AB8540_LDO_SDIO,
+	AB8540_SYSCLKREQ_2,
+	AB8540_SYSCLKREQ_4,
+	AB8540_NUM_REGULATORS,
+};
+
+/* AB8500, AB8505, and AB9540 register initialization */
 struct ab8500_regulator_reg_init {
 	int id;
+	u8 mask;
 	u8 value;
 };
 
-#define INIT_REGULATOR_REGISTER(_id, _value)	\
-	{					\
-		.id = _id,			\
-		.value = _value,		\
+#define INIT_REGULATOR_REGISTER(_id, _mask, _value)	\
+	{						\
+		.id = _id,				\
+		.mask = _mask,				\
+		.value = _value,			\
 	}
 
 /* AB8500 registers */
@@ -86,10 +132,58 @@ enum ab8500_regulator_reg {
 	AB8500_REGUCTRL2SPARE,
 	AB8500_REGUCTRLDISCH,
 	AB8500_REGUCTRLDISCH2,
-	AB8500_VSMPS1SEL1,
 	AB8500_NUM_REGULATOR_REGISTERS,
 };
 
+/* AB8505 registers */
+enum ab8505_regulator_reg {
+	AB8505_REGUREQUESTCTRL1,
+	AB8505_REGUREQUESTCTRL2,
+	AB8505_REGUREQUESTCTRL3,
+	AB8505_REGUREQUESTCTRL4,
+	AB8505_REGUSYSCLKREQ1HPVALID1,
+	AB8505_REGUSYSCLKREQ1HPVALID2,
+	AB8505_REGUHWHPREQ1VALID1,
+	AB8505_REGUHWHPREQ1VALID2,
+	AB8505_REGUHWHPREQ2VALID1,
+	AB8505_REGUHWHPREQ2VALID2,
+	AB8505_REGUSWHPREQVALID1,
+	AB8505_REGUSWHPREQVALID2,
+	AB8505_REGUSYSCLKREQVALID1,
+	AB8505_REGUSYSCLKREQVALID2,
+	AB8505_REGUVAUX4REQVALID,
+	AB8505_REGUMISC1,
+	AB8505_VAUDIOSUPPLY,
+	AB8505_REGUCTRL1VAMIC,
+	AB8505_VSMPSAREGU,
+	AB8505_VSMPSBREGU,
+	AB8505_VSAFEREGU, /* NOTE! PRCMU register */
+	AB8505_VPLLVANAREGU,
+	AB8505_EXTSUPPLYREGU,
+	AB8505_VAUX12REGU,
+	AB8505_VRF1VAUX3REGU,
+	AB8505_VSMPSASEL1,
+	AB8505_VSMPSASEL2,
+	AB8505_VSMPSASEL3,
+	AB8505_VSMPSBSEL1,
+	AB8505_VSMPSBSEL2,
+	AB8505_VSMPSBSEL3,
+	AB8505_VSAFESEL1, /* NOTE! PRCMU register */
+	AB8505_VSAFESEL2, /* NOTE! PRCMU register */
+	AB8505_VSAFESEL3, /* NOTE! PRCMU register */
+	AB8505_VAUX1SEL,
+	AB8505_VAUX2SEL,
+	AB8505_VRF1VAUX3SEL,
+	AB8505_VAUX4REQCTRL,
+	AB8505_VAUX4REGU,
+	AB8505_VAUX4SEL,
+	AB8505_REGUCTRLDISCH,
+	AB8505_REGUCTRLDISCH2,
+	AB8505_REGUCTRLDISCH3,
+	AB8505_CTRLVAUX5,
+	AB8505_CTRLVAUX6,
+	AB8505_NUM_REGULATOR_REGISTERS,
+};
 
 /* AB9540 registers */
 enum ab9540_regulator_reg {
@@ -139,4 +233,111 @@ enum ab9540_regulator_reg {
 	AB9540_NUM_REGULATOR_REGISTERS,
 };
 
+/* AB8540 registers */
+enum ab8540_regulator_reg {
+	AB8540_REGUREQUESTCTRL1,
+	AB8540_REGUREQUESTCTRL2,
+	AB8540_REGUREQUESTCTRL3,
+	AB8540_REGUREQUESTCTRL4,
+	AB8540_REGUSYSCLKREQ1HPVALID1,
+	AB8540_REGUSYSCLKREQ1HPVALID2,
+	AB8540_REGUHWHPREQ1VALID1,
+	AB8540_REGUHWHPREQ1VALID2,
+	AB8540_REGUHWHPREQ2VALID1,
+	AB8540_REGUHWHPREQ2VALID2,
+	AB8540_REGUSWHPREQVALID1,
+	AB8540_REGUSWHPREQVALID2,
+	AB8540_REGUSYSCLKREQVALID1,
+	AB8540_REGUSYSCLKREQVALID2,
+	AB8540_REGUVAUX4REQVALID,
+	AB8540_REGUVAUX5REQVALID,
+	AB8540_REGUVAUX6REQVALID,
+	AB8540_REGUVCLKBREQVALID,
+	AB8540_REGUVRF1REQVALID,
+	AB8540_REGUMISC1,
+	AB8540_VAUDIOSUPPLY,
+	AB8540_REGUCTRL1VAMIC,
+	AB8540_VHSIC,
+	AB8540_VSDIO,
+	AB8540_VSMPS1REGU,
+	AB8540_VSMPS2REGU,
+	AB8540_VSMPS3REGU,
+	AB8540_VPLLVANAREGU,
+	AB8540_EXTSUPPLYREGU,
+	AB8540_VAUX12REGU,
+	AB8540_VRF1VAUX3REGU,
+	AB8540_VSMPS1SEL1,
+	AB8540_VSMPS1SEL2,
+	AB8540_VSMPS1SEL3,
+	AB8540_VSMPS2SEL1,
+	AB8540_VSMPS2SEL2,
+	AB8540_VSMPS2SEL3,
+	AB8540_VSMPS3SEL1,
+	AB8540_VSMPS3SEL2,
+	AB8540_VAUX1SEL,
+	AB8540_VAUX2SEL,
+	AB8540_VRF1VAUX3SEL,
+	AB8540_REGUCTRL2SPARE,
+	AB8540_VAUX4REQCTRL,
+	AB8540_VAUX4REGU,
+	AB8540_VAUX4SEL,
+	AB8540_VAUX5REQCTRL,
+	AB8540_VAUX5REGU,
+	AB8540_VAUX5SEL,
+	AB8540_VAUX6REQCTRL,
+	AB8540_VAUX6REGU,
+	AB8540_VAUX6SEL,
+	AB8540_VCLKBREQCTRL,
+	AB8540_VCLKBREGU,
+	AB8540_VCLKBSEL,
+	AB8540_VRF1REQCTRL,
+	AB8540_REGUCTRLDISCH,
+	AB8540_REGUCTRLDISCH2,
+	AB8540_REGUCTRLDISCH3,
+	AB8540_REGUCTRLDISCH4,
+	AB8540_VSIMSYSCLKCTRL,
+	AB8540_VANAVPLLSEL,
+	AB8540_NUM_REGULATOR_REGISTERS,
+};
+
+/* AB8500 external regulators */
+struct ab8500_ext_regulator_cfg {
+	bool hwreq; /* requires hw mode or high power mode */
+};
+
+enum ab8500_ext_regulator_id {
+	AB8500_EXT_SUPPLY1,
+	AB8500_EXT_SUPPLY2,
+	AB8500_EXT_SUPPLY3,
+	AB8500_NUM_EXT_REGULATORS,
+};
+
+/* AB8500 regulator platform data */
+struct ab8500_regulator_platform_data {
+	int num_reg_init;
+	struct ab8500_regulator_reg_init *reg_init;
+	int num_regulator;
+	struct regulator_init_data *regulator;
+	int num_ext_regulator;
+	struct regulator_init_data *ext_regulator;
+};
+
+#ifdef CONFIG_REGULATOR_AB8500_DEBUG
+int ab8500_regulator_debug_init(struct platform_device *pdev);
+int ab8500_regulator_debug_exit(struct platform_device *pdev);
+#else
+static inline int ab8500_regulator_debug_init(struct platform_device *pdev)
+{
+	return 0;
+}
+static inline int ab8500_regulator_debug_exit(struct platform_device *pdev)
+{
+	return 0;
+}
+#endif
+
+/* AB8500 external regulator functions. */
+int ab8500_ext_regulator_init(struct platform_device *pdev);
+void ab8500_ext_regulator_exit(struct platform_device *pdev);
+
 #endif
diff --git a/include/linux/regulator/consumer.h b/include/linux/regulator/consumer.h
index 7bc732ce6e5..145022a8308 100644
--- a/include/linux/regulator/consumer.h
+++ b/include/linux/regulator/consumer.h
@@ -141,18 +141,18 @@ void regulator_put(struct regulator *regulator);
 void devm_regulator_put(struct regulator *regulator);
 
 /* regulator output control and status */
-int regulator_enable(struct regulator *regulator);
+int __must_check regulator_enable(struct regulator *regulator);
 int regulator_disable(struct regulator *regulator);
 int regulator_force_disable(struct regulator *regulator);
 int regulator_is_enabled(struct regulator *regulator);
 int regulator_disable_deferred(struct regulator *regulator, int ms);
 
-int regulator_bulk_get(struct device *dev, int num_consumers,
-		       struct regulator_bulk_data *consumers);
-int devm_regulator_bulk_get(struct device *dev, int num_consumers,
-			    struct regulator_bulk_data *consumers);
-int regulator_bulk_enable(int num_consumers,
-			  struct regulator_bulk_data *consumers);
+int __must_check regulator_bulk_get(struct device *dev, int num_consumers,
+				    struct regulator_bulk_data *consumers);
+int __must_check devm_regulator_bulk_get(struct device *dev, int num_consumers,
+					 struct regulator_bulk_data *consumers);
+int __must_check regulator_bulk_enable(int num_consumers,
+				       struct regulator_bulk_data *consumers);
 int regulator_bulk_disable(int num_consumers,
 			   struct regulator_bulk_data *consumers);
 int regulator_bulk_force_disable(int num_consumers,
diff --git a/include/linux/regulator/driver.h b/include/linux/regulator/driver.h
index 7df93f52db0..6700cc94bdd 100644
--- a/include/linux/regulator/driver.h
+++ b/include/linux/regulator/driver.h
@@ -22,6 +22,7 @@
 struct regmap;
 struct regulator_dev;
 struct regulator_init_data;
+struct regulator_enable_gpio;
 
 enum regulator_status {
 	REGULATOR_STATUS_OFF,
@@ -199,6 +200,8 @@ enum regulator_type {
  *                output when using regulator_set_voltage_sel_regmap
  * @enable_reg: Register for control when using regmap enable/disable ops
  * @enable_mask: Mask for control when using regmap enable/disable ops
+ * @enable_is_inverted: A flag to indicate set enable_mask bits to disable
+ *                      when using regulator_enable_regmap and friends APIs.
  * @bypass_reg: Register for control when using regmap set_bypass
  * @bypass_mask: Mask for control when using regmap set_bypass
  *
@@ -228,6 +231,7 @@ struct regulator_desc {
 	unsigned int apply_bit;
 	unsigned int enable_reg;
 	unsigned int enable_mask;
+	bool enable_is_inverted;
 	unsigned int bypass_reg;
 	unsigned int bypass_mask;
 
@@ -302,8 +306,7 @@ struct regulator_dev {
 
 	struct dentry *debugfs;
 
-	int ena_gpio;
-	unsigned int ena_gpio_invert:1;
+	struct regulator_enable_gpio *ena_pin;
 	unsigned int ena_gpio_state:1;
 };
 
@@ -329,6 +332,8 @@ int regulator_map_voltage_linear(struct regulator_dev *rdev,
 				  int min_uV, int max_uV);
 int regulator_map_voltage_iterate(struct regulator_dev *rdev,
 				  int min_uV, int max_uV);
+int regulator_map_voltage_ascend(struct regulator_dev *rdev,
+				  int min_uV, int max_uV);
 int regulator_get_voltage_sel_regmap(struct regulator_dev *rdev);
 int regulator_set_voltage_sel_regmap(struct regulator_dev *rdev, unsigned sel);
 int regulator_is_enabled_regmap(struct regulator_dev *rdev);
diff --git a/include/linux/regulator/max8952.h b/include/linux/regulator/max8952.h
index 45e42855ad0..4dbb63a1d4a 100644
--- a/include/linux/regulator/max8952.h
+++ b/include/linux/regulator/max8952.h
@@ -122,13 +122,13 @@ struct max8952_platform_data {
 	int gpio_vid1;
 	int gpio_en;
 
-	u8 default_mode;
-	u8 dvs_mode[MAX8952_NUM_DVS_MODE]; /* MAX8952_DVS_MODEx_XXXXmV */
+	u32 default_mode;
+	u32 dvs_mode[MAX8952_NUM_DVS_MODE]; /* MAX8952_DVS_MODEx_XXXXmV */
 
-	u8 sync_freq;
-	u8 ramp_speed;
+	u32 sync_freq;
+	u32 ramp_speed;
 
-	struct regulator_init_data reg_data;
+	struct regulator_init_data *reg_data;
 };
 
 
diff --git a/include/linux/res_counter.h b/include/linux/res_counter.h
index c23099413ad..96a509b6be0 100644
--- a/include/linux/res_counter.h
+++ b/include/linux/res_counter.h
@@ -13,7 +13,7 @@
  * info about what this counter is.
  */
 
-#include <linux/cgroup.h>
+#include <linux/spinlock.h>
 #include <linux/errno.h>
 
 /*
diff --git a/include/linux/ring_buffer.h b/include/linux/ring_buffer.h
index 1342e69542f..d69cf637a15 100644
--- a/include/linux/ring_buffer.h
+++ b/include/linux/ring_buffer.h
@@ -4,6 +4,7 @@
 #include <linux/kmemcheck.h>
 #include <linux/mm.h>
 #include <linux/seq_file.h>
+#include <linux/poll.h>
 
 struct ring_buffer;
 struct ring_buffer_iter;
@@ -96,6 +97,11 @@ __ring_buffer_alloc(unsigned long size, unsigned flags, struct lock_class_key *k
 	__ring_buffer_alloc((size), (flags), &__key);	\
 })
 
+void ring_buffer_wait(struct ring_buffer *buffer, int cpu);
+int ring_buffer_poll_wait(struct ring_buffer *buffer, int cpu,
+			  struct file *filp, poll_table *poll_table);
+
+
 #define RING_BUFFER_ALL_CPUS -1
 
 void ring_buffer_free(struct ring_buffer *buffer);
diff --git a/include/linux/rtc.h b/include/linux/rtc.h
index 580b24c8b8c..c2c28975293 100644
--- a/include/linux/rtc.h
+++ b/include/linux/rtc.h
@@ -133,7 +133,13 @@ extern struct rtc_device *rtc_device_register(const char *name,
 					struct device *dev,
 					const struct rtc_class_ops *ops,
 					struct module *owner);
+extern struct rtc_device *devm_rtc_device_register(struct device *dev,
+					const char *name,
+					const struct rtc_class_ops *ops,
+					struct module *owner);
 extern void rtc_device_unregister(struct rtc_device *rtc);
+extern void devm_rtc_device_unregister(struct device *dev,
+					struct rtc_device *rtc);
 
 extern int rtc_read_time(struct rtc_device *rtc, struct rtc_time *tm);
 extern int rtc_set_time(struct rtc_device *rtc, struct rtc_time *tm);
diff --git a/include/linux/sched.h b/include/linux/sched.h
index d35d2b6ddbf..54ddcb82cdd 100644
--- a/include/linux/sched.h
+++ b/include/linux/sched.h
@@ -127,18 +127,6 @@ extern void proc_sched_show_task(struct task_struct *p, struct seq_file *m);
 extern void proc_sched_set_task(struct task_struct *p);
 extern void
 print_cfs_rq(struct seq_file *m, int cpu, struct cfs_rq *cfs_rq);
-#else
-static inline void
-proc_sched_show_task(struct task_struct *p, struct seq_file *m)
-{
-}
-static inline void proc_sched_set_task(struct task_struct *p)
-{
-}
-static inline void
-print_cfs_rq(struct seq_file *m, int cpu, struct cfs_rq *cfs_rq)
-{
-}
 #endif
 
 /*
@@ -163,9 +151,10 @@ print_cfs_rq(struct seq_file *m, int cpu, struct cfs_rq *cfs_rq)
 #define TASK_DEAD		64
 #define TASK_WAKEKILL		128
 #define TASK_WAKING		256
-#define TASK_STATE_MAX		512
+#define TASK_PARKED		512
+#define TASK_STATE_MAX		1024
 
-#define TASK_STATE_TO_CHAR_STR "RSDTtZXxKW"
+#define TASK_STATE_TO_CHAR_STR "RSDTtZXxKWP"
 
 extern char ___assert_task_state[1 - 2*!!(
 		sizeof(TASK_STATE_TO_CHAR_STR)-1 != ilog2(TASK_STATE_MAX)+1)];
@@ -320,7 +309,6 @@ extern signed long schedule_timeout_killable(signed long timeout);
 extern signed long schedule_timeout_uninterruptible(signed long timeout);
 asmlinkage void schedule(void);
 extern void schedule_preempt_disabled(void);
-extern int mutex_spin_on_owner(struct mutex *lock, struct task_struct *owner);
 
 struct nsproxy;
 struct user_namespace;
@@ -526,7 +514,8 @@ struct signal_struct {
 	unsigned int		has_child_subreaper:1;
 
 	/* POSIX.1b Interval Timers */
-	struct list_head posix_timers;
+	int			posix_timer_id;
+	struct list_head	posix_timers;
 
 	/* ITIMER_REAL timer for the process */
 	struct hrtimer real_timer;
@@ -570,7 +559,7 @@ struct signal_struct {
 	cputime_t utime, stime, cutime, cstime;
 	cputime_t gtime;
 	cputime_t cgtime;
-#ifndef CONFIG_VIRT_CPU_ACCOUNTING
+#ifndef CONFIG_VIRT_CPU_ACCOUNTING_NATIVE
 	struct cputime prev_cputime;
 #endif
 	unsigned long nvcsw, nivcsw, cnvcsw, cnivcsw;
@@ -768,31 +757,6 @@ enum cpu_idle_type {
 };
 
 /*
- * Increase resolution of nice-level calculations for 64-bit architectures.
- * The extra resolution improves shares distribution and load balancing of
- * low-weight task groups (eg. nice +19 on an autogroup), deeper taskgroup
- * hierarchies, especially on larger systems. This is not a user-visible change
- * and does not change the user-interface for setting shares/weights.
- *
- * We increase resolution only if we have enough bits to allow this increased
- * resolution (i.e. BITS_PER_LONG > 32). The costs for increasing resolution
- * when BITS_PER_LONG <= 32 are pretty high and the returns do not justify the
- * increased costs.
- */
-#if 0 /* BITS_PER_LONG > 32 -- currently broken: it increases power usage under light load  */
-# define SCHED_LOAD_RESOLUTION	10
-# define scale_load(w)		((w) << SCHED_LOAD_RESOLUTION)
-# define scale_load_down(w)	((w) >> SCHED_LOAD_RESOLUTION)
-#else
-# define SCHED_LOAD_RESOLUTION	0
-# define scale_load(w)		(w)
-# define scale_load_down(w)	(w)
-#endif
-
-#define SCHED_LOAD_SHIFT	(10 + SCHED_LOAD_RESOLUTION)
-#define SCHED_LOAD_SCALE	(1L << SCHED_LOAD_SHIFT)
-
-/*
  * Increase resolution of cpu_power calculations
  */
 #define SCHED_POWER_SHIFT	10
@@ -817,62 +781,6 @@ enum cpu_idle_type {
 
 extern int __weak arch_sd_sibiling_asym_packing(void);
 
-struct sched_group_power {
-	atomic_t ref;
-	/*
-	 * CPU power of this group, SCHED_LOAD_SCALE being max power for a
-	 * single CPU.
-	 */
-	unsigned int power, power_orig;
-	unsigned long next_update;
-	/*
-	 * Number of busy cpus in this group.
-	 */
-	atomic_t nr_busy_cpus;
-
-	unsigned long cpumask[0]; /* iteration mask */
-};
-
-struct sched_group {
-	struct sched_group *next;	/* Must be a circular list */
-	atomic_t ref;
-
-	unsigned int group_weight;
-	struct sched_group_power *sgp;
-
-	/*
-	 * The CPUs this group covers.
-	 *
-	 * NOTE: this field is variable length. (Allocated dynamically
-	 * by attaching extra space to the end of the structure,
-	 * depending on how many CPUs the kernel has booted up with)
-	 */
-	unsigned long cpumask[0];
-};
-
-static inline struct cpumask *sched_group_cpus(struct sched_group *sg)
-{
-	return to_cpumask(sg->cpumask);
-}
-
-/*
- * cpumask masking which cpus in the group are allowed to iterate up the domain
- * tree.
- */
-static inline struct cpumask *sched_group_mask(struct sched_group *sg)
-{
-	return to_cpumask(sg->sgp->cpumask);
-}
-
-/**
- * group_first_cpu - Returns the first cpu in the cpumask of a sched_group.
- * @group: The group whose first cpu is to be returned.
- */
-static inline unsigned int group_first_cpu(struct sched_group *group)
-{
-	return cpumask_first(sched_group_cpus(group));
-}
-
 struct sched_domain_attr {
 	int relax_domain_level;
 };
@@ -883,6 +791,8 @@ struct sched_domain_attr {
 
 extern int sched_domain_level_max;
 
+struct sched_group;
+
 struct sched_domain {
 	/* These fields must be setup */
 	struct sched_domain *parent;	/* top domain must be null terminated */
@@ -899,6 +809,8 @@ struct sched_domain {
 	unsigned int wake_idx;
 	unsigned int forkexec_idx;
 	unsigned int smt_gain;
+
+	int nohz_idle;			/* NOHZ IDLE status */
 	int flags;			/* See SD_* */
 	int level;
 
@@ -971,18 +883,6 @@ extern void partition_sched_domains(int ndoms_new, cpumask_var_t doms_new[],
 cpumask_var_t *alloc_sched_domains(unsigned int ndoms);
 void free_sched_domains(cpumask_var_t doms[], unsigned int ndoms);
 
-/* Test a flag in parent sched domain */
-static inline int test_sd_parent(struct sched_domain *sd, int flag)
-{
-	if (sd->parent && (sd->parent->flags & flag))
-		return 1;
-
-	return 0;
-}
-
-unsigned long default_scale_freq_power(struct sched_domain *sd, int cpu);
-unsigned long default_scale_smt_power(struct sched_domain *sd, int cpu);
-
 bool cpus_share_cache(int this_cpu, int that_cpu);
 
 #else /* CONFIG_SMP */
@@ -1017,72 +917,6 @@ struct mempolicy;
 struct pipe_inode_info;
 struct uts_namespace;
 
-struct rq;
-struct sched_domain;
-
-/*
- * wake flags
- */
-#define WF_SYNC		0x01		/* waker goes to sleep after wakup */
-#define WF_FORK		0x02		/* child wakeup after fork */
-#define WF_MIGRATED	0x04		/* internal use, task got migrated */
-
-#define ENQUEUE_WAKEUP		1
-#define ENQUEUE_HEAD		2
-#ifdef CONFIG_SMP
-#define ENQUEUE_WAKING		4	/* sched_class::task_waking was called */
-#else
-#define ENQUEUE_WAKING		0
-#endif
-
-#define DEQUEUE_SLEEP		1
-
-struct sched_class {
-	const struct sched_class *next;
-
-	void (*enqueue_task) (struct rq *rq, struct task_struct *p, int flags);
-	void (*dequeue_task) (struct rq *rq, struct task_struct *p, int flags);
-	void (*yield_task) (struct rq *rq);
-	bool (*yield_to_task) (struct rq *rq, struct task_struct *p, bool preempt);
-
-	void (*check_preempt_curr) (struct rq *rq, struct task_struct *p, int flags);
-
-	struct task_struct * (*pick_next_task) (struct rq *rq);
-	void (*put_prev_task) (struct rq *rq, struct task_struct *p);
-
-#ifdef CONFIG_SMP
-	int  (*select_task_rq)(struct task_struct *p, int sd_flag, int flags);
-	void (*migrate_task_rq)(struct task_struct *p, int next_cpu);
-
-	void (*pre_schedule) (struct rq *this_rq, struct task_struct *task);
-	void (*post_schedule) (struct rq *this_rq);
-	void (*task_waking) (struct task_struct *task);
-	void (*task_woken) (struct rq *this_rq, struct task_struct *task);
-
-	void (*set_cpus_allowed)(struct task_struct *p,
-				 const struct cpumask *newmask);
-
-	void (*rq_online)(struct rq *rq);
-	void (*rq_offline)(struct rq *rq);
-#endif
-
-	void (*set_curr_task) (struct rq *rq);
-	void (*task_tick) (struct rq *rq, struct task_struct *p, int queued);
-	void (*task_fork) (struct task_struct *p);
-
-	void (*switched_from) (struct rq *this_rq, struct task_struct *task);
-	void (*switched_to) (struct rq *this_rq, struct task_struct *task);
-	void (*prio_changed) (struct rq *this_rq, struct task_struct *task,
-			     int oldprio);
-
-	unsigned int (*get_rr_interval) (struct rq *rq,
-					 struct task_struct *task);
-
-#ifdef CONFIG_FAIR_GROUP_SCHED
-	void (*task_move_group) (struct task_struct *p, int on_rq);
-#endif
-};
-
 struct load_weight {
 	unsigned long weight, inv_weight;
 };
@@ -1274,8 +1108,10 @@ struct task_struct {
 	int exit_code, exit_signal;
 	int pdeath_signal;  /*  The signal sent when the parent dies  */
 	unsigned int jobctl;	/* JOBCTL_*, siglock protected */
-	/* ??? */
+
+	/* Used for emulating ABI behavior of previous Linux versions */
 	unsigned int personality;
+
 	unsigned did_exec:1;
 	unsigned in_execve:1;	/* Tell the LSMs that the process is doing an
 				 * execve */
@@ -1327,7 +1163,7 @@ struct task_struct {
 
 	cputime_t utime, stime, utimescaled, stimescaled;
 	cputime_t gtime;
-#ifndef CONFIG_VIRT_CPU_ACCOUNTING
+#ifndef CONFIG_VIRT_CPU_ACCOUNTING_NATIVE
 	struct cputime prev_cputime;
 #endif
 #ifdef CONFIG_VIRT_CPU_ACCOUNTING_GEN
@@ -1793,7 +1629,7 @@ extern void thread_group_cputime_adjusted(struct task_struct *p, cputime_t *ut,
 #define PF_SWAPWRITE	0x00800000	/* Allowed to write to swap */
 #define PF_SPREAD_PAGE	0x01000000	/* Spread page cache over cpuset */
 #define PF_SPREAD_SLAB	0x02000000	/* Spread some slab caches over cpuset */
-#define PF_THREAD_BOUND	0x04000000	/* Thread bound to specific cpu */
+#define PF_NO_SETAFFINITY 0x04000000	/* Userland is not allowed to meddle with cpus_allowed */
 #define PF_MCE_EARLY    0x08000000      /* Early kill for mce process policy */
 #define PF_MEMPOLICY	0x10000000	/* Non-default NUMA mempolicy */
 #define PF_MUTEX_TESTER	0x20000000	/* Thread belongs to the rt mutex tester */
@@ -2622,6 +2458,47 @@ static inline int spin_needbreak(spinlock_t *lock)
 }
 
 /*
+ * Idle thread specific functions to determine the need_resched
+ * polling state. We have two versions, one based on TS_POLLING in
+ * thread_info.status and one based on TIF_POLLING_NRFLAG in
+ * thread_info.flags
+ */
+#ifdef TS_POLLING
+static inline int tsk_is_polling(struct task_struct *p)
+{
+	return task_thread_info(p)->status & TS_POLLING;
+}
+static inline void current_set_polling(void)
+{
+	current_thread_info()->status |= TS_POLLING;
+}
+
+static inline void current_clr_polling(void)
+{
+	current_thread_info()->status &= ~TS_POLLING;
+	smp_mb__after_clear_bit();
+}
+#elif defined(TIF_POLLING_NRFLAG)
+static inline int tsk_is_polling(struct task_struct *p)
+{
+	return test_tsk_thread_flag(p, TIF_POLLING_NRFLAG);
+}
+static inline void current_set_polling(void)
+{
+	set_thread_flag(TIF_POLLING_NRFLAG);
+}
+
+static inline void current_clr_polling(void)
+{
+	clear_thread_flag(TIF_POLLING_NRFLAG);
+}
+#else
+static inline int tsk_is_polling(struct task_struct *p) { return 0; }
+static inline void current_set_polling(void) { }
+static inline void current_clr_polling(void) { }
+#endif
+
+/*
  * Thread group CPU time accounting.
  */
 void thread_group_cputime(struct task_struct *tsk, struct task_cputime *times);
@@ -2681,28 +2558,7 @@ extern long sched_setaffinity(pid_t pid, const struct cpumask *new_mask);
 extern long sched_getaffinity(pid_t pid, struct cpumask *mask);
 
 #ifdef CONFIG_CGROUP_SCHED
-
 extern struct task_group root_task_group;
-
-extern struct task_group *sched_create_group(struct task_group *parent);
-extern void sched_online_group(struct task_group *tg,
-			       struct task_group *parent);
-extern void sched_destroy_group(struct task_group *tg);
-extern void sched_offline_group(struct task_group *tg);
-extern void sched_move_task(struct task_struct *tsk);
-#ifdef CONFIG_FAIR_GROUP_SCHED
-extern int sched_group_set_shares(struct task_group *tg, unsigned long shares);
-extern unsigned long sched_group_shares(struct task_group *tg);
-#endif
-#ifdef CONFIG_RT_GROUP_SCHED
-extern int sched_group_set_rt_runtime(struct task_group *tg,
-				      long rt_runtime_us);
-extern long sched_group_rt_runtime(struct task_group *tg);
-extern int sched_group_set_rt_period(struct task_group *tg,
-				      long rt_period_us);
-extern long sched_group_rt_period(struct task_group *tg);
-extern int sched_rt_can_attach(struct task_group *tg, struct task_struct *tsk);
-#endif
 #endif /* CONFIG_CGROUP_SCHED */
 
 extern int task_can_switch_user(struct user_struct *up,
diff --git a/include/linux/security.h b/include/linux/security.h
index eee7478cda7..032c366ef1c 100644
--- a/include/linux/security.h
+++ b/include/linux/security.h
@@ -1012,6 +1012,10 @@ static inline void security_free_mnt_opts(struct security_mnt_opts *opts)
  *	This hook can be used by the module to update any security state
  *	associated with the TUN device's security structure.
  *	@security pointer to the TUN devices's security structure.
+ * @skb_owned_by:
+ *	This hook sets the packet's owning sock.
+ *	@skb is the packet.
+ *	@sk the sock which owns the packet.
  *
  * Security hooks for XFRM operations.
  *
@@ -1638,6 +1642,7 @@ struct security_operations {
 	int (*tun_dev_attach_queue) (void *security);
 	int (*tun_dev_attach) (struct sock *sk, void *security);
 	int (*tun_dev_open) (void *security);
+	void (*skb_owned_by) (struct sk_buff *skb, struct sock *sk);
 #endif	/* CONFIG_SECURITY_NETWORK */
 
 #ifdef CONFIG_SECURITY_NETWORK_XFRM
@@ -2588,6 +2593,8 @@ int security_tun_dev_attach_queue(void *security);
 int security_tun_dev_attach(struct sock *sk, void *security);
 int security_tun_dev_open(void *security);
 
+void security_skb_owned_by(struct sk_buff *skb, struct sock *sk);
+
 #else	/* CONFIG_SECURITY_NETWORK */
 static inline int security_unix_stream_connect(struct sock *sock,
 					       struct sock *other,
@@ -2779,6 +2786,11 @@ static inline int security_tun_dev_open(void *security)
 {
 	return 0;
 }
+
+static inline void security_skb_owned_by(struct sk_buff *skb, struct sock *sk)
+{
+}
+
 #endif	/* CONFIG_SECURITY_NETWORK */
 
 #ifdef CONFIG_SECURITY_NETWORK_XFRM
diff --git a/include/linux/serial_s3c.h b/include/linux/serial_s3c.h
new file mode 100644
index 00000000000..907d9d1d56c
--- /dev/null
+++ b/include/linux/serial_s3c.h
@@ -0,0 +1,260 @@
+/*
+ *  Internal header file for Samsung S3C2410 serial ports (UART0-2)
+ *
+ *  Copyright (C) 2002 Shane Nay (shane@minirl.com)
+ *
+ *  Additional defines, Copyright 2003 Simtec Electronics (linux@simtec.co.uk)
+ *
+ *  Adapted from:
+ *
+ *  Internal header file for MX1ADS serial ports (UART1 & 2)
+ *
+ *  Copyright (C) 2002 Shane Nay (shane@minirl.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
+*/
+
+#ifndef __ASM_ARM_REGS_SERIAL_H
+#define __ASM_ARM_REGS_SERIAL_H
+
+#define S3C2410_URXH	  (0x24)
+#define S3C2410_UTXH	  (0x20)
+#define S3C2410_ULCON	  (0x00)
+#define S3C2410_UCON	  (0x04)
+#define S3C2410_UFCON	  (0x08)
+#define S3C2410_UMCON	  (0x0C)
+#define S3C2410_UBRDIV	  (0x28)
+#define S3C2410_UTRSTAT	  (0x10)
+#define S3C2410_UERSTAT	  (0x14)
+#define S3C2410_UFSTAT	  (0x18)
+#define S3C2410_UMSTAT	  (0x1C)
+
+#define S3C2410_LCON_CFGMASK	  ((0xF<<3)|(0x3))
+
+#define S3C2410_LCON_CS5	  (0x0)
+#define S3C2410_LCON_CS6	  (0x1)
+#define S3C2410_LCON_CS7	  (0x2)
+#define S3C2410_LCON_CS8	  (0x3)
+#define S3C2410_LCON_CSMASK	  (0x3)
+
+#define S3C2410_LCON_PNONE	  (0x0)
+#define S3C2410_LCON_PEVEN	  (0x5 << 3)
+#define S3C2410_LCON_PODD	  (0x4 << 3)
+#define S3C2410_LCON_PMASK	  (0x7 << 3)
+
+#define S3C2410_LCON_STOPB	  (1<<2)
+#define S3C2410_LCON_IRM          (1<<6)
+
+#define S3C2440_UCON_CLKMASK	  (3<<10)
+#define S3C2440_UCON_CLKSHIFT	  (10)
+#define S3C2440_UCON_PCLK	  (0<<10)
+#define S3C2440_UCON_UCLK	  (1<<10)
+#define S3C2440_UCON_PCLK2	  (2<<10)
+#define S3C2440_UCON_FCLK	  (3<<10)
+#define S3C2443_UCON_EPLL	  (3<<10)
+
+#define S3C6400_UCON_CLKMASK	(3<<10)
+#define S3C6400_UCON_CLKSHIFT	(10)
+#define S3C6400_UCON_PCLK	(0<<10)
+#define S3C6400_UCON_PCLK2	(2<<10)
+#define S3C6400_UCON_UCLK0	(1<<10)
+#define S3C6400_UCON_UCLK1	(3<<10)
+
+#define S3C2440_UCON2_FCLK_EN	  (1<<15)
+#define S3C2440_UCON0_DIVMASK	  (15 << 12)
+#define S3C2440_UCON1_DIVMASK	  (15 << 12)
+#define S3C2440_UCON2_DIVMASK	  (7 << 12)
+#define S3C2440_UCON_DIVSHIFT	  (12)
+
+#define S3C2412_UCON_CLKMASK	(3<<10)
+#define S3C2412_UCON_CLKSHIFT	(10)
+#define S3C2412_UCON_UCLK	(1<<10)
+#define S3C2412_UCON_USYSCLK	(3<<10)
+#define S3C2412_UCON_PCLK	(0<<10)
+#define S3C2412_UCON_PCLK2	(2<<10)
+
+#define S3C2410_UCON_CLKMASK	(1 << 10)
+#define S3C2410_UCON_CLKSHIFT	(10)
+#define S3C2410_UCON_UCLK	  (1<<10)
+#define S3C2410_UCON_SBREAK	  (1<<4)
+
+#define S3C2410_UCON_TXILEVEL	  (1<<9)
+#define S3C2410_UCON_RXILEVEL	  (1<<8)
+#define S3C2410_UCON_TXIRQMODE	  (1<<2)
+#define S3C2410_UCON_RXIRQMODE	  (1<<0)
+#define S3C2410_UCON_RXFIFO_TOI	  (1<<7)
+#define S3C2443_UCON_RXERR_IRQEN  (1<<6)
+#define S3C2443_UCON_LOOPBACK	  (1<<5)
+
+#define S3C2410_UCON_DEFAULT	  (S3C2410_UCON_TXILEVEL  | \
+				   S3C2410_UCON_RXILEVEL  | \
+				   S3C2410_UCON_TXIRQMODE | \
+				   S3C2410_UCON_RXIRQMODE | \
+				   S3C2410_UCON_RXFIFO_TOI)
+
+#define S3C2410_UFCON_FIFOMODE	  (1<<0)
+#define S3C2410_UFCON_TXTRIG0	  (0<<6)
+#define S3C2410_UFCON_RXTRIG8	  (1<<4)
+#define S3C2410_UFCON_RXTRIG12	  (2<<4)
+
+/* S3C2440 FIFO trigger levels */
+#define S3C2440_UFCON_RXTRIG1	  (0<<4)
+#define S3C2440_UFCON_RXTRIG8	  (1<<4)
+#define S3C2440_UFCON_RXTRIG16	  (2<<4)
+#define S3C2440_UFCON_RXTRIG32	  (3<<4)
+
+#define S3C2440_UFCON_TXTRIG0	  (0<<6)
+#define S3C2440_UFCON_TXTRIG16	  (1<<6)
+#define S3C2440_UFCON_TXTRIG32	  (2<<6)
+#define S3C2440_UFCON_TXTRIG48	  (3<<6)
+
+#define S3C2410_UFCON_RESETBOTH	  (3<<1)
+#define S3C2410_UFCON_RESETTX	  (1<<2)
+#define S3C2410_UFCON_RESETRX	  (1<<1)
+
+#define S3C2410_UFCON_DEFAULT	  (S3C2410_UFCON_FIFOMODE | \
+				   S3C2410_UFCON_TXTRIG0  | \
+				   S3C2410_UFCON_RXTRIG8 )
+
+#define	S3C2410_UMCOM_AFC	  (1<<4)
+#define	S3C2410_UMCOM_RTS_LOW	  (1<<0)
+
+#define S3C2412_UMCON_AFC_63	(0<<5)		/* same as s3c2443 */
+#define S3C2412_UMCON_AFC_56	(1<<5)
+#define S3C2412_UMCON_AFC_48	(2<<5)
+#define S3C2412_UMCON_AFC_40	(3<<5)
+#define S3C2412_UMCON_AFC_32	(4<<5)
+#define S3C2412_UMCON_AFC_24	(5<<5)
+#define S3C2412_UMCON_AFC_16	(6<<5)
+#define S3C2412_UMCON_AFC_8	(7<<5)
+
+#define S3C2410_UFSTAT_TXFULL	  (1<<9)
+#define S3C2410_UFSTAT_RXFULL	  (1<<8)
+#define S3C2410_UFSTAT_TXMASK	  (15<<4)
+#define S3C2410_UFSTAT_TXSHIFT	  (4)
+#define S3C2410_UFSTAT_RXMASK	  (15<<0)
+#define S3C2410_UFSTAT_RXSHIFT	  (0)
+
+/* UFSTAT S3C2443 same as S3C2440 */
+#define S3C2440_UFSTAT_TXFULL	  (1<<14)
+#define S3C2440_UFSTAT_RXFULL	  (1<<6)
+#define S3C2440_UFSTAT_TXSHIFT	  (8)
+#define S3C2440_UFSTAT_RXSHIFT	  (0)
+#define S3C2440_UFSTAT_TXMASK	  (63<<8)
+#define S3C2440_UFSTAT_RXMASK	  (63)
+
+#define S3C2410_UTRSTAT_TXE	  (1<<2)
+#define S3C2410_UTRSTAT_TXFE	  (1<<1)
+#define S3C2410_UTRSTAT_RXDR	  (1<<0)
+
+#define S3C2410_UERSTAT_OVERRUN	  (1<<0)
+#define S3C2410_UERSTAT_FRAME	  (1<<2)
+#define S3C2410_UERSTAT_BREAK	  (1<<3)
+#define S3C2443_UERSTAT_PARITY	  (1<<1)
+
+#define S3C2410_UERSTAT_ANY	  (S3C2410_UERSTAT_OVERRUN | \
+				   S3C2410_UERSTAT_FRAME | \
+				   S3C2410_UERSTAT_BREAK)
+
+#define S3C2410_UMSTAT_CTS	  (1<<0)
+#define S3C2410_UMSTAT_DeltaCTS	  (1<<2)
+
+#define S3C2443_DIVSLOT		  (0x2C)
+
+/* S3C64XX interrupt registers. */
+#define S3C64XX_UINTP		0x30
+#define S3C64XX_UINTSP		0x34
+#define S3C64XX_UINTM		0x38
+
+#define S3C64XX_UINTM_RXD	(0)
+#define S3C64XX_UINTM_TXD	(2)
+#define S3C64XX_UINTM_RXD_MSK	(1 << S3C64XX_UINTM_RXD)
+#define S3C64XX_UINTM_TXD_MSK	(1 << S3C64XX_UINTM_TXD)
+
+/* Following are specific to S5PV210 */
+#define S5PV210_UCON_CLKMASK	(1<<10)
+#define S5PV210_UCON_CLKSHIFT	(10)
+#define S5PV210_UCON_PCLK	(0<<10)
+#define S5PV210_UCON_UCLK	(1<<10)
+
+#define S5PV210_UFCON_TXTRIG0	(0<<8)
+#define S5PV210_UFCON_TXTRIG4	(1<<8)
+#define S5PV210_UFCON_TXTRIG8	(2<<8)
+#define S5PV210_UFCON_TXTRIG16	(3<<8)
+#define S5PV210_UFCON_TXTRIG32	(4<<8)
+#define S5PV210_UFCON_TXTRIG64	(5<<8)
+#define S5PV210_UFCON_TXTRIG128 (6<<8)
+#define S5PV210_UFCON_TXTRIG256 (7<<8)
+
+#define S5PV210_UFCON_RXTRIG1	(0<<4)
+#define S5PV210_UFCON_RXTRIG4	(1<<4)
+#define S5PV210_UFCON_RXTRIG8	(2<<4)
+#define S5PV210_UFCON_RXTRIG16	(3<<4)
+#define S5PV210_UFCON_RXTRIG32	(4<<4)
+#define S5PV210_UFCON_RXTRIG64	(5<<4)
+#define S5PV210_UFCON_RXTRIG128	(6<<4)
+#define S5PV210_UFCON_RXTRIG256	(7<<4)
+
+#define S5PV210_UFSTAT_TXFULL	(1<<24)
+#define S5PV210_UFSTAT_RXFULL	(1<<8)
+#define S5PV210_UFSTAT_TXMASK	(255<<16)
+#define S5PV210_UFSTAT_TXSHIFT	(16)
+#define S5PV210_UFSTAT_RXMASK	(255<<0)
+#define S5PV210_UFSTAT_RXSHIFT	(0)
+
+#define S3C2410_UCON_CLKSEL0	(1 << 0)
+#define S3C2410_UCON_CLKSEL1	(1 << 1)
+#define S3C2410_UCON_CLKSEL2	(1 << 2)
+#define S3C2410_UCON_CLKSEL3	(1 << 3)
+
+/* Default values for s5pv210 UCON and UFCON uart registers */
+#define S5PV210_UCON_DEFAULT	(S3C2410_UCON_TXILEVEL |	\
+				 S3C2410_UCON_RXILEVEL |	\
+				 S3C2410_UCON_TXIRQMODE |	\
+				 S3C2410_UCON_RXIRQMODE |	\
+				 S3C2410_UCON_RXFIFO_TOI |	\
+				 S3C2443_UCON_RXERR_IRQEN)
+
+#define S5PV210_UFCON_DEFAULT	(S3C2410_UFCON_FIFOMODE |	\
+				 S5PV210_UFCON_TXTRIG4 |	\
+				 S5PV210_UFCON_RXTRIG4)
+
+#ifndef __ASSEMBLY__
+
+/* configuration structure for per-machine configurations for the
+ * serial port
+ *
+ * the pointer is setup by the machine specific initialisation from the
+ * arch/arm/mach-s3c2410/ directory.
+*/
+
+struct s3c2410_uartcfg {
+	unsigned char	   hwport;	 /* hardware port number */
+	unsigned char	   unused;
+	unsigned short	   flags;
+	upf_t		   uart_flags;	 /* default uart flags */
+	unsigned int	   clk_sel;
+
+	unsigned int	   has_fracval;
+
+	unsigned long	   ucon;	 /* value of ucon for port */
+	unsigned long	   ulcon;	 /* value of ulcon for port */
+	unsigned long	   ufcon;	 /* value of ufcon for port */
+};
+
+#endif /* __ASSEMBLY__ */
+
+#endif /* __ASM_ARM_REGS_SERIAL_H */
+
diff --git a/include/linux/signal.h b/include/linux/signal.h
index a2dcb94ea49..9475c5cb28b 100644
--- a/include/linux/signal.h
+++ b/include/linux/signal.h
@@ -250,11 +250,11 @@ extern int show_unhandled_signals;
 extern int sigsuspend(sigset_t *);
 
 struct sigaction {
-#ifndef __ARCH_HAS_ODD_SIGACTION
+#ifndef __ARCH_HAS_IRIX_SIGACTION
 	__sighandler_t	sa_handler;
 	unsigned long	sa_flags;
 #else
-	unsigned long	sa_flags;
+	unsigned int	sa_flags;
 	__sighandler_t	sa_handler;
 #endif
 #ifdef __ARCH_HAS_SA_RESTORER
diff --git a/include/linux/skbuff.h b/include/linux/skbuff.h
index 441f5bfdab8..b8292d8cc9f 100644
--- a/include/linux/skbuff.h
+++ b/include/linux/skbuff.h
@@ -2643,6 +2643,13 @@ static inline void nf_reset(struct sk_buff *skb)
 #endif
 }
 
+static inline void nf_reset_trace(struct sk_buff *skb)
+{
+#if IS_ENABLED(CONFIG_NETFILTER_XT_TARGET_TRACE)
+	skb->nf_trace = 0;
+#endif
+}
+
 /* Note: This doesn't put any conntrack and bridge info in dst. */
 static inline void __nf_copy(struct sk_buff *dst, const struct sk_buff *src)
 {
diff --git a/include/linux/spi/spi-tegra.h b/include/linux/spi/spi-tegra.h
deleted file mode 100644
index 786932c62ed..00000000000
--- a/include/linux/spi/spi-tegra.h
+++ /dev/null
@@ -1,40 +0,0 @@
-/*
- * spi-tegra.h: SPI interface for Nvidia Tegra20 SLINK controller.
- *
- * Copyright (C) 2011 NVIDIA Corporation
- *
- * 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 _LINUX_SPI_TEGRA_H
-#define _LINUX_SPI_TEGRA_H
-
-struct tegra_spi_platform_data {
-	int dma_req_sel;
-	unsigned int spi_max_frequency;
-};
-
-/*
- * Controller data from device to pass some info like
- * hw based chip select can be used or not and if yes
- * then CS hold and setup time.
- */
-struct tegra_spi_device_controller_data {
-	bool is_hw_based_cs;
-	int cs_setup_clk_count;
-	int cs_hold_clk_count;
-};
-
-#endif /* _LINUX_SPI_TEGRA_H */
diff --git a/include/linux/spi/spi.h b/include/linux/spi/spi.h
index 38c2b925923..733eb5ee31c 100644
--- a/include/linux/spi/spi.h
+++ b/include/linux/spi/spi.h
@@ -228,6 +228,11 @@ static inline void spi_unregister_driver(struct spi_driver *sdrv)
  *	every chipselect is connected to a slave.
  * @dma_alignment: SPI controller constraint on DMA buffers alignment.
  * @mode_bits: flags understood by this controller driver
+ * @bits_per_word_mask: A mask indicating which values of bits_per_word are
+ *	supported by the driver. Bit n indicates that a bits_per_word n+1 is
+ *	suported. If set, the SPI core will reject any transfer with an
+ *	unsupported bits_per_word. If not set, this value is simply ignored,
+ *	and it's up to the individual driver to perform any validation.
  * @flags: other constraints relevant to this driver
  * @bus_lock_spinlock: spinlock for SPI bus locking
  * @bus_lock_mutex: mutex for SPI bus locking
@@ -301,6 +306,9 @@ struct spi_master {
 	/* spi_device.mode flags understood by this controller driver */
 	u16			mode_bits;
 
+	/* bitmask of supported bits_per_word for transfers */
+	u32			bits_per_word_mask;
+
 	/* other constraints relevant to this driver */
 	u16			flags;
 #define SPI_MASTER_HALF_DUPLEX	BIT(0)		/* can't do full duplex */
diff --git a/include/linux/spinlock_up.h b/include/linux/spinlock_up.h
index a26e2fb604e..e2369c167db 100644
--- a/include/linux/spinlock_up.h
+++ b/include/linux/spinlock_up.h
@@ -16,7 +16,10 @@
  * In the debug case, 1 means unlocked, 0 means locked. (the values
  * are inverted, to catch initialization bugs)
  *
- * No atomicity anywhere, we are on UP.
+ * No atomicity anywhere, we are on UP. However, we still need
+ * the compiler barriers, because we do not want the compiler to
+ * move potentially faulting instructions (notably user accesses)
+ * into the locked sequence, resulting in non-atomic execution.
  */
 
 #ifdef CONFIG_DEBUG_SPINLOCK
@@ -25,6 +28,7 @@
 static inline void arch_spin_lock(arch_spinlock_t *lock)
 {
 	lock->slock = 0;
+	barrier();
 }
 
 static inline void
@@ -32,6 +36,7 @@ arch_spin_lock_flags(arch_spinlock_t *lock, unsigned long flags)
 {
 	local_irq_save(flags);
 	lock->slock = 0;
+	barrier();
 }
 
 static inline int arch_spin_trylock(arch_spinlock_t *lock)
@@ -39,32 +44,34 @@ static inline int arch_spin_trylock(arch_spinlock_t *lock)
 	char oldval = lock->slock;
 
 	lock->slock = 0;
+	barrier();
 
 	return oldval > 0;
 }
 
 static inline void arch_spin_unlock(arch_spinlock_t *lock)
 {
+	barrier();
 	lock->slock = 1;
 }
 
 /*
  * Read-write spinlocks. No debug version.
  */
-#define arch_read_lock(lock)		do { (void)(lock); } while (0)
-#define arch_write_lock(lock)		do { (void)(lock); } while (0)
-#define arch_read_trylock(lock)	({ (void)(lock); 1; })
-#define arch_write_trylock(lock)	({ (void)(lock); 1; })
-#define arch_read_unlock(lock)		do { (void)(lock); } while (0)
-#define arch_write_unlock(lock)	do { (void)(lock); } while (0)
+#define arch_read_lock(lock)		do { barrier(); (void)(lock); } while (0)
+#define arch_write_lock(lock)		do { barrier(); (void)(lock); } while (0)
+#define arch_read_trylock(lock)	({ barrier(); (void)(lock); 1; })
+#define arch_write_trylock(lock)	({ barrier(); (void)(lock); 1; })
+#define arch_read_unlock(lock)		do { barrier(); (void)(lock); } while (0)
+#define arch_write_unlock(lock)	do { barrier(); (void)(lock); } while (0)
 
 #else /* DEBUG_SPINLOCK */
 #define arch_spin_is_locked(lock)	((void)(lock), 0)
 /* for sched.c and kernel_lock.c: */
-# define arch_spin_lock(lock)		do { (void)(lock); } while (0)
-# define arch_spin_lock_flags(lock, flags)	do { (void)(lock); } while (0)
-# define arch_spin_unlock(lock)	do { (void)(lock); } while (0)
-# define arch_spin_trylock(lock)	({ (void)(lock); 1; })
+# define arch_spin_lock(lock)		do { barrier(); (void)(lock); } while (0)
+# define arch_spin_lock_flags(lock, flags)	do { barrier(); (void)(lock); } while (0)
+# define arch_spin_unlock(lock)	do { barrier(); (void)(lock); } while (0)
+# define arch_spin_trylock(lock)	({ barrier(); (void)(lock); 1; })
 #endif /* DEBUG_SPINLOCK */
 
 #define arch_spin_is_contended(lock)	(((void)(lock), 0))
diff --git a/include/linux/ssb/ssb_driver_chipcommon.h b/include/linux/ssb/ssb_driver_chipcommon.h
index 9e492be5244..6fcfe99bd99 100644
--- a/include/linux/ssb/ssb_driver_chipcommon.h
+++ b/include/linux/ssb/ssb_driver_chipcommon.h
@@ -219,6 +219,7 @@
 #define SSB_CHIPCO_PMU_CTL			0x0600 /* PMU control */
 #define  SSB_CHIPCO_PMU_CTL_ILP_DIV		0xFFFF0000 /* ILP div mask */
 #define  SSB_CHIPCO_PMU_CTL_ILP_DIV_SHIFT	16
+#define  SSB_CHIPCO_PMU_CTL_PLL_UPD		0x00000400
 #define  SSB_CHIPCO_PMU_CTL_NOILPONW		0x00000200 /* No ILP on wait */
 #define  SSB_CHIPCO_PMU_CTL_HTREQEN		0x00000100 /* HT req enable */
 #define  SSB_CHIPCO_PMU_CTL_ALPREQEN		0x00000080 /* ALP req enable */
@@ -667,5 +668,6 @@ enum ssb_pmu_ldo_volt_id {
 void ssb_pmu_set_ldo_voltage(struct ssb_chipcommon *cc,
 			     enum ssb_pmu_ldo_volt_id id, u32 voltage);
 void ssb_pmu_set_ldo_paref(struct ssb_chipcommon *cc, bool on);
+void ssb_pmu_spuravoid_pllupdate(struct ssb_chipcommon *cc, int spuravoid);
 
 #endif /* LINUX_SSB_CHIPCO_H_ */
diff --git a/include/linux/ssbi.h b/include/linux/ssbi.h
new file mode 100644
index 00000000000..44ef5da2147
--- /dev/null
+++ b/include/linux/ssbi.h
@@ -0,0 +1,38 @@
+/* Copyright (C) 2010 Google, Inc.
+ * Copyright (c) 2011, Code Aurora Forum. All rights reserved.
+ * Author: Dima Zavin <dima@android.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 and
+ * only 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.
+ */
+
+#ifndef _LINUX_SSBI_H
+#define _LINUX_SSBI_H
+
+#include <linux/types.h>
+
+struct ssbi_slave_info {
+	const char	*name;
+	void		*platform_data;
+};
+
+enum ssbi_controller_type {
+	MSM_SBI_CTRL_SSBI = 0,
+	MSM_SBI_CTRL_SSBI2,
+	MSM_SBI_CTRL_PMIC_ARBITER,
+};
+
+struct ssbi_platform_data {
+	struct ssbi_slave_info	slave;
+	enum ssbi_controller_type controller_type;
+};
+
+int ssbi_write(struct device *dev, u16 addr, u8 *buf, int len);
+int ssbi_read(struct device *dev, u16 addr, u8 *buf, int len);
+#endif
diff --git a/include/linux/sunrpc/auth.h b/include/linux/sunrpc/auth.h
index 58fda1c3c78..0dd00f4f681 100644
--- a/include/linux/sunrpc/auth.h
+++ b/include/linux/sunrpc/auth.h
@@ -22,6 +22,8 @@
 /* size of the nodename buffer */
 #define UNX_MAXNODENAME	32
 
+struct rpcsec_gss_info;
+
 /* Work around the lack of a VFS credential */
 struct auth_cred {
 	kuid_t	uid;
@@ -103,6 +105,9 @@ struct rpc_authops {
 	int			(*pipes_create)(struct rpc_auth *);
 	void			(*pipes_destroy)(struct rpc_auth *);
 	int			(*list_pseudoflavors)(rpc_authflavor_t *, int);
+	rpc_authflavor_t	(*info2flavor)(struct rpcsec_gss_info *);
+	int			(*flavor2info)(rpc_authflavor_t,
+						struct rpcsec_gss_info *);
 };
 
 struct rpc_credops {
@@ -137,6 +142,10 @@ int			rpcauth_register(const struct rpc_authops *);
 int			rpcauth_unregister(const struct rpc_authops *);
 struct rpc_auth *	rpcauth_create(rpc_authflavor_t, struct rpc_clnt *);
 void			rpcauth_release(struct rpc_auth *);
+rpc_authflavor_t	rpcauth_get_pseudoflavor(rpc_authflavor_t,
+				struct rpcsec_gss_info *);
+int			rpcauth_get_gssinfo(rpc_authflavor_t,
+				struct rpcsec_gss_info *);
 int			rpcauth_list_flavors(rpc_authflavor_t *, int);
 struct rpc_cred *	rpcauth_lookup_credcache(struct rpc_auth *, struct auth_cred *, int);
 void			rpcauth_init_cred(struct rpc_cred *, const struct auth_cred *, struct rpc_auth *, const struct rpc_credops *);
diff --git a/include/linux/sunrpc/clnt.h b/include/linux/sunrpc/clnt.h
index 2cf4ffaa3cd..e7d492ce7c1 100644
--- a/include/linux/sunrpc/clnt.h
+++ b/include/linux/sunrpc/clnt.h
@@ -124,6 +124,7 @@ struct rpc_create_args {
 #define RPC_CLNT_CREATE_NOPING		(1UL << 4)
 #define RPC_CLNT_CREATE_DISCRTRY	(1UL << 5)
 #define RPC_CLNT_CREATE_QUIET		(1UL << 6)
+#define RPC_CLNT_CREATE_INFINITE_SLOTS	(1UL << 7)
 
 struct rpc_clnt *rpc_create(struct rpc_create_args *args);
 struct rpc_clnt	*rpc_bind_new_program(struct rpc_clnt *,
diff --git a/include/linux/sunrpc/gss_api.h b/include/linux/sunrpc/gss_api.h
index a19e2547ae6..f32b7a47e13 100644
--- a/include/linux/sunrpc/gss_api.h
+++ b/include/linux/sunrpc/gss_api.h
@@ -25,10 +25,21 @@ struct gss_ctx {
 
 #define GSS_C_NO_BUFFER		((struct xdr_netobj) 0)
 #define GSS_C_NO_CONTEXT	((struct gss_ctx *) 0)
-#define GSS_C_NULL_OID		((struct xdr_netobj) 0)
+#define GSS_C_QOP_DEFAULT	(0)
 
 /*XXX  arbitrary length - is this set somewhere? */
 #define GSS_OID_MAX_LEN 32
+struct rpcsec_gss_oid {
+	unsigned int	len;
+	u8		data[GSS_OID_MAX_LEN];
+};
+
+/* From RFC 3530 */
+struct rpcsec_gss_info {
+	struct rpcsec_gss_oid	oid;
+	u32			qop;
+	u32			service;
+};
 
 /* gss-api prototypes; note that these are somewhat simplified versions of
  * the prototypes specified in RFC 2744. */
@@ -58,12 +69,14 @@ u32 gss_unwrap(
 u32 gss_delete_sec_context(
 		struct gss_ctx		**ctx_id);
 
-u32 gss_svc_to_pseudoflavor(struct gss_api_mech *, u32 service);
+rpc_authflavor_t gss_svc_to_pseudoflavor(struct gss_api_mech *, u32 qop,
+					u32 service);
 u32 gss_pseudoflavor_to_service(struct gss_api_mech *, u32 pseudoflavor);
 char *gss_service_to_auth_domain_name(struct gss_api_mech *, u32 service);
 
 struct pf_desc {
 	u32	pseudoflavor;
+	u32	qop;
 	u32	service;
 	char	*name;
 	char	*auth_domain_name;
@@ -76,7 +89,7 @@ struct pf_desc {
 struct gss_api_mech {
 	struct list_head	gm_list;
 	struct module		*gm_owner;
-	struct xdr_netobj	gm_oid;
+	struct rpcsec_gss_oid	gm_oid;
 	char			*gm_name;
 	const struct gss_api_ops *gm_ops;
 	/* pseudoflavors supported by this mechanism: */
@@ -117,9 +130,11 @@ struct gss_api_ops {
 int gss_mech_register(struct gss_api_mech *);
 void gss_mech_unregister(struct gss_api_mech *);
 
-/* returns a mechanism descriptor given an OID, and increments the mechanism's
- * reference count. */
-struct gss_api_mech * gss_mech_get_by_OID(struct xdr_netobj *);
+/* Given a GSS security tuple, look up a pseudoflavor */
+rpc_authflavor_t gss_mech_info2flavor(struct rpcsec_gss_info *);
+
+/* Given a pseudoflavor, look up a GSS security tuple */
+int gss_mech_flavor2info(rpc_authflavor_t, struct rpcsec_gss_info *);
 
 /* Returns a reference to a mechanism, given a name like "krb5" etc. */
 struct gss_api_mech *gss_mech_get_by_name(const char *);
@@ -130,9 +145,6 @@ struct gss_api_mech *gss_mech_get_by_pseudoflavor(u32);
 /* Fill in an array with a list of supported pseudoflavors */
 int gss_mech_list_pseudoflavors(rpc_authflavor_t *, int);
 
-/* Just increments the mechanism's reference count and returns its input: */
-struct gss_api_mech * gss_mech_get(struct gss_api_mech *);
-
 /* For every successful gss_mech_get or gss_mech_get_by_* call there must be a
  * corresponding call to gss_mech_put. */
 void gss_mech_put(struct gss_api_mech *);
diff --git a/include/linux/sunrpc/xprt.h b/include/linux/sunrpc/xprt.h
index 30834be0301..ff5392421cb 100644
--- a/include/linux/sunrpc/xprt.h
+++ b/include/linux/sunrpc/xprt.h
@@ -255,6 +255,8 @@ static inline int bc_prealloc(struct rpc_rqst *req)
 }
 #endif /* CONFIG_SUNRPC_BACKCHANNEL */
 
+#define XPRT_CREATE_INFINITE_SLOTS	(1U)
+
 struct xprt_create {
 	int			ident;		/* XPRT_TRANSPORT identifier */
 	struct net *		net;
@@ -263,6 +265,7 @@ struct xprt_create {
 	size_t			addrlen;
 	const char		*servername;
 	struct svc_xprt		*bc_xprt;	/* NFSv4.1 backchannel */
+	unsigned int		flags;
 };
 
 struct xprt_class {
@@ -279,6 +282,7 @@ struct xprt_class {
 struct rpc_xprt		*xprt_create_transport(struct xprt_create *args);
 void			xprt_connect(struct rpc_task *task);
 void			xprt_reserve(struct rpc_task *task);
+void			xprt_retry_reserve(struct rpc_task *task);
 int			xprt_reserve_xprt(struct rpc_xprt *xprt, struct rpc_task *task);
 int			xprt_reserve_xprt_cong(struct rpc_xprt *xprt, struct rpc_task *task);
 void			xprt_alloc_slot(struct rpc_xprt *xprt, struct rpc_task *task);
@@ -334,6 +338,7 @@ int			xs_swapper(struct rpc_xprt *xprt, int enable);
 #define XPRT_CLOSING		(6)
 #define XPRT_CONNECTION_ABORT	(7)
 #define XPRT_CONNECTION_CLOSE	(8)
+#define XPRT_CONGESTED		(9)
 
 static inline void xprt_set_connected(struct rpc_xprt *xprt)
 {
diff --git a/include/linux/swap.h b/include/linux/swap.h
index 2818a123f3e..1701ce4be74 100644
--- a/include/linux/swap.h
+++ b/include/linux/swap.h
@@ -236,7 +236,7 @@ extern unsigned long nr_free_pagecache_pages(void);
 extern void __lru_cache_add(struct page *, enum lru_list lru);
 extern void lru_cache_add_lru(struct page *, enum lru_list lru);
 extern void lru_add_page_tail(struct page *page, struct page *page_tail,
-			      struct lruvec *lruvec);
+			 struct lruvec *lruvec, struct list_head *head);
 extern void activate_page(struct page *);
 extern void mark_page_accessed(struct page *);
 extern void lru_add_drain(void);
@@ -330,6 +330,9 @@ static inline void mem_cgroup_uncharge_swap(swp_entry_t ent)
 /* linux/mm/page_io.c */
 extern int swap_readpage(struct page *);
 extern int swap_writepage(struct page *page, struct writeback_control *wbc);
+extern void end_swap_bio_write(struct bio *bio, int err);
+extern int __swap_writepage(struct page *page, struct writeback_control *wbc,
+	void (*end_write_func)(struct bio *, int));
 extern int swap_set_page_dirty(struct page *page);
 extern void end_swap_bio_read(struct bio *bio, int err);
 
@@ -343,8 +346,9 @@ extern struct address_space swapper_spaces[];
 #define swap_address_space(entry) (&swapper_spaces[swp_type(entry)])
 extern unsigned long total_swapcache_pages(void);
 extern void show_swap_cache_info(void);
-extern int add_to_swap(struct page *);
+extern int add_to_swap(struct page *, struct list_head *list);
 extern int add_to_swap_cache(struct page *, swp_entry_t, gfp_t);
+extern int __add_to_swap_cache(struct page *page, swp_entry_t entry);
 extern void __delete_from_swap_cache(struct page *);
 extern void delete_from_swap_cache(struct page *);
 extern void free_page_and_swap_cache(struct page *);
@@ -461,7 +465,7 @@ static inline struct page *lookup_swap_cache(swp_entry_t swp)
 	return NULL;
 }
 
-static inline int add_to_swap(struct page *page)
+static inline int add_to_swap(struct page *page, struct list_head *list)
 {
 	return 0;
 }
diff --git a/include/linux/swiotlb.h b/include/linux/swiotlb.h
index 2de42f9401d..a5ffd32642f 100644
--- a/include/linux/swiotlb.h
+++ b/include/linux/swiotlb.h
@@ -25,6 +25,7 @@ extern int swiotlb_force;
 extern void swiotlb_init(int verbose);
 int swiotlb_init_with_tbl(char *tlb, unsigned long nslabs, int verbose);
 extern unsigned long swiotlb_nr_tbl(void);
+unsigned long swiotlb_size_or_default(void);
 extern int swiotlb_late_init_with_tbl(char *tlb, unsigned long nslabs);
 
 /*
diff --git a/include/linux/thermal.h b/include/linux/thermal.h
index f0bd7f90a90..e3c0ae9bb1f 100644
--- a/include/linux/thermal.h
+++ b/include/linux/thermal.h
@@ -44,7 +44,7 @@
 /* Adding event notification support elements */
 #define THERMAL_GENL_FAMILY_NAME                "thermal_event"
 #define THERMAL_GENL_VERSION                    0x01
-#define THERMAL_GENL_MCAST_GROUP_NAME           "thermal_mc_group"
+#define THERMAL_GENL_MCAST_GROUP_NAME           "thermal_mc_grp"
 
 /* Default Thermal Governor */
 #if defined(CONFIG_THERMAL_DEFAULT_GOV_STEP_WISE)
diff --git a/include/linux/time.h b/include/linux/time.h
index d4835dfdf25..22d81b3c955 100644
--- a/include/linux/time.h
+++ b/include/linux/time.h
@@ -181,6 +181,9 @@ extern struct timespec timespec_trunc(struct timespec t, unsigned gran);
 extern int timekeeping_valid_for_hres(void);
 extern u64 timekeeping_max_deferment(void);
 extern int timekeeping_inject_offset(struct timespec *ts);
+extern s32 timekeeping_get_tai_offset(void);
+extern void timekeeping_set_tai_offset(s32 tai_offset);
+extern void timekeeping_clocktai(struct timespec *ts);
 
 struct tms;
 extern void do_sys_times(struct tms *);
diff --git a/include/linux/timekeeper_internal.h b/include/linux/timekeeper_internal.h
index e1d558e237e..c1825eb436e 100644
--- a/include/linux/timekeeper_internal.h
+++ b/include/linux/timekeeper_internal.h
@@ -20,6 +20,8 @@ struct timekeeper {
 	u32			shift;
 	/* Number of clock cycles in one NTP interval. */
 	cycle_t			cycle_interval;
+	/* Last cycle value (also stored in clock->cycle_last) */
+	cycle_t			cycle_last;
 	/* Number of clock shifted nano seconds in one NTP interval. */
 	u64			xtime_interval;
 	/* shifted nano seconds left over when rounding cycle_interval */
@@ -62,8 +64,11 @@ struct timekeeper {
 	ktime_t			offs_boot;
 	/* The raw monotonic time for the CLOCK_MONOTONIC_RAW posix clock. */
 	struct timespec		raw_time;
-	/* Seqlock for all timekeeper values */
-	seqlock_t		lock;
+	/* The current UTC to TAI offset in seconds */
+	s32			tai_offset;
+	/* Offset clock monotonic -> clock tai */
+	ktime_t			offs_tai;
+
 };
 
 static inline struct timespec tk_xtime(struct timekeeper *tk)
diff --git a/include/linux/timex.h b/include/linux/timex.h
index 5ec87c60b97..b3726e61368 100644
--- a/include/linux/timex.h
+++ b/include/linux/timex.h
@@ -125,9 +125,6 @@
 extern unsigned long tick_usec;		/* USER_HZ period (usec) */
 extern unsigned long tick_nsec;		/* SHIFTED_HZ period (nsec) */
 
-extern void ntp_init(void);
-extern void ntp_clear(void);
-
 /* Required to safely shift negative values */
 #define shift_right(x, s) ({	\
 	__typeof__(x) __x = (x);	\
@@ -140,10 +137,6 @@ extern void ntp_clear(void);
 #define NTP_INTERVAL_FREQ  (HZ)
 #define NTP_INTERVAL_LENGTH (NSEC_PER_SEC/NTP_INTERVAL_FREQ)
 
-/* Returns how long ticks are at present, in ns / 2^NTP_SCALE_SHIFT. */
-extern u64 ntp_tick_length(void);
-
-extern int second_overflow(unsigned long secs);
 extern int do_adjtimex(struct timex *);
 extern void hardpps(const struct timespec *, const struct timespec *);
 
diff --git a/include/linux/trace_clock.h b/include/linux/trace_clock.h
index d563f37e1a1..1d7ca273927 100644
--- a/include/linux/trace_clock.h
+++ b/include/linux/trace_clock.h
@@ -16,6 +16,7 @@
 
 extern u64 notrace trace_clock_local(void);
 extern u64 notrace trace_clock(void);
+extern u64 notrace trace_clock_jiffies(void);
 extern u64 notrace trace_clock_global(void);
 extern u64 notrace trace_clock_counter(void);
 
diff --git a/include/linux/tty.h b/include/linux/tty.h
index c75d886b030..367a9dfc4ea 100644
--- a/include/linux/tty.h
+++ b/include/linux/tty.h
@@ -255,9 +255,9 @@ struct tty_struct {
 	int count;
 	struct winsize winsize;		/* termios mutex */
 	unsigned char stopped:1, hw_stopped:1, flow_stopped:1, packet:1;
-	unsigned char warned:1;
 	unsigned char ctrl_status;	/* ctrl_lock */
 	unsigned int receive_room;	/* Bytes free for queue */
+	int flow_change;
 
 	struct tty_struct *link;
 	struct fasync_struct *fasync;
@@ -315,9 +315,25 @@ struct tty_file_private {
 #define TTY_NO_WRITE_SPLIT 	17	/* Preserve write boundaries to driver */
 #define TTY_HUPPED 		18	/* Post driver->hangup() */
 #define TTY_HUPPING 		21	/* ->hangup() in progress */
+#define TTY_LDISC_HALTED	22	/* Line discipline is halted */
 
 #define TTY_WRITE_FLUSH(tty) tty_write_flush((tty))
 
+/* Values for tty->flow_change */
+#define TTY_THROTTLE_SAFE 1
+#define TTY_UNTHROTTLE_SAFE 2
+
+static inline void __tty_set_flow_change(struct tty_struct *tty, int val)
+{
+	tty->flow_change = val;
+}
+
+static inline void tty_set_flow_change(struct tty_struct *tty, int val)
+{
+	tty->flow_change = val;
+	smp_mb();
+}
+
 #ifdef CONFIG_TTY
 extern void console_init(void);
 extern void tty_kref_put(struct tty_struct *tty);
@@ -400,6 +416,8 @@ extern int tty_write_room(struct tty_struct *tty);
 extern void tty_driver_flush_buffer(struct tty_struct *tty);
 extern void tty_throttle(struct tty_struct *tty);
 extern void tty_unthrottle(struct tty_struct *tty);
+extern int tty_throttle_safe(struct tty_struct *tty);
+extern int tty_unthrottle_safe(struct tty_struct *tty);
 extern int tty_do_resize(struct tty_struct *tty, struct winsize *ws);
 extern void tty_driver_remove_tty(struct tty_driver *driver,
 				  struct tty_struct *tty);
@@ -419,13 +437,28 @@ extern void tty_flush_to_ldisc(struct tty_struct *tty);
 extern void tty_buffer_free_all(struct tty_port *port);
 extern void tty_buffer_flush(struct tty_struct *tty);
 extern void tty_buffer_init(struct tty_port *port);
-extern speed_t tty_get_baud_rate(struct tty_struct *tty);
 extern speed_t tty_termios_baud_rate(struct ktermios *termios);
 extern speed_t tty_termios_input_baud_rate(struct ktermios *termios);
 extern void tty_termios_encode_baud_rate(struct ktermios *termios,
 						speed_t ibaud, speed_t obaud);
 extern void tty_encode_baud_rate(struct tty_struct *tty,
 						speed_t ibaud, speed_t obaud);
+
+/**
+ *	tty_get_baud_rate	-	get tty bit rates
+ *	@tty: tty to query
+ *
+ *	Returns the baud rate as an integer for this terminal. The
+ *	termios lock must be held by the caller and the terminal bit
+ *	flags may be updated.
+ *
+ *	Locking: none
+ */
+static inline speed_t tty_get_baud_rate(struct tty_struct *tty)
+{
+	return tty_termios_baud_rate(&tty->termios);
+}
+
 extern void tty_termios_copy_hw(struct ktermios *new, struct ktermios *old);
 extern int tty_termios_hw_change(struct ktermios *a, struct ktermios *b);
 extern int tty_set_termios(struct tty_struct *tty, struct ktermios *kt);
@@ -502,6 +535,8 @@ extern int tty_port_carrier_raised(struct tty_port *port);
 extern void tty_port_raise_dtr_rts(struct tty_port *port);
 extern void tty_port_lower_dtr_rts(struct tty_port *port);
 extern void tty_port_hangup(struct tty_port *port);
+extern void tty_port_tty_hangup(struct tty_port *port, bool check_clocal);
+extern void tty_port_tty_wakeup(struct tty_port *port);
 extern int tty_port_block_til_ready(struct tty_port *port,
 				struct tty_struct *tty, struct file *filp);
 extern int tty_port_close_start(struct tty_port *port,
@@ -526,8 +561,6 @@ extern void tty_ldisc_release(struct tty_struct *tty, struct tty_struct *o_tty);
 extern void tty_ldisc_init(struct tty_struct *tty);
 extern void tty_ldisc_deinit(struct tty_struct *tty);
 extern void tty_ldisc_begin(void);
-/* This last one is just for the tty layer internals and shouldn't be used elsewhere */
-extern void tty_ldisc_enable(struct tty_struct *tty);
 
 
 /* n_tty.c */
diff --git a/include/linux/tty_ldisc.h b/include/linux/tty_ldisc.h
index 455a0d7bf22..58390c73df8 100644
--- a/include/linux/tty_ldisc.h
+++ b/include/linux/tty_ldisc.h
@@ -9,89 +9,89 @@
  *
  * int	(*open)(struct tty_struct *);
  *
- * 	This function is called when the line discipline is associated
- * 	with the tty.  The line discipline can use this as an
- * 	opportunity to initialize any state needed by the ldisc routines.
- * 
+ *	This function is called when the line discipline is associated
+ *	with the tty.  The line discipline can use this as an
+ *	opportunity to initialize any state needed by the ldisc routines.
+ *
  * void	(*close)(struct tty_struct *);
  *
  *	This function is called when the line discipline is being
- * 	shutdown, either because the tty is being closed or because
- * 	the tty is being changed to use a new line discipline
- * 
+ *	shutdown, either because the tty is being closed or because
+ *	the tty is being changed to use a new line discipline
+ *
  * void	(*flush_buffer)(struct tty_struct *tty);
  *
- * 	This function instructs the line discipline to clear its
- * 	buffers of any input characters it may have queued to be
- * 	delivered to the user mode process.
- * 
+ *	This function instructs the line discipline to clear its
+ *	buffers of any input characters it may have queued to be
+ *	delivered to the user mode process.
+ *
  * ssize_t (*chars_in_buffer)(struct tty_struct *tty);
  *
- * 	This function returns the number of input characters the line
+ *	This function returns the number of input characters the line
  *	discipline may have queued up to be delivered to the user mode
  *	process.
- * 
+ *
  * ssize_t (*read)(struct tty_struct * tty, struct file * file,
  *		   unsigned char * buf, size_t nr);
  *
- * 	This function is called when the user requests to read from
- * 	the tty.  The line discipline will return whatever characters
- * 	it has buffered up for the user.  If this function is not
- * 	defined, the user will receive an EIO error.
- * 
+ *	This function is called when the user requests to read from
+ *	the tty.  The line discipline will return whatever characters
+ *	it has buffered up for the user.  If this function is not
+ *	defined, the user will receive an EIO error.
+ *
  * ssize_t (*write)(struct tty_struct * tty, struct file * file,
- * 		    const unsigned char * buf, size_t nr);
- *
- * 	This function is called when the user requests to write to the
- * 	tty.  The line discipline will deliver the characters to the
- * 	low-level tty device for transmission, optionally performing
- * 	some processing on the characters first.  If this function is
- * 	not defined, the user will receive an EIO error.
- * 
+ *		    const unsigned char * buf, size_t nr);
+ *
+ *	This function is called when the user requests to write to the
+ *	tty.  The line discipline will deliver the characters to the
+ *	low-level tty device for transmission, optionally performing
+ *	some processing on the characters first.  If this function is
+ *	not defined, the user will receive an EIO error.
+ *
  * int	(*ioctl)(struct tty_struct * tty, struct file * file,
- * 		 unsigned int cmd, unsigned long arg);
+ *		 unsigned int cmd, unsigned long arg);
  *
  *	This function is called when the user requests an ioctl which
- * 	is not handled by the tty layer or the low-level tty driver.
- * 	It is intended for ioctls which affect line discpline
- * 	operation.  Note that the search order for ioctls is (1) tty
- * 	layer, (2) tty low-level driver, (3) line discpline.  So a
- * 	low-level driver can "grab" an ioctl request before the line
- * 	discpline has a chance to see it.
- * 
+ *	is not handled by the tty layer or the low-level tty driver.
+ *	It is intended for ioctls which affect line discpline
+ *	operation.  Note that the search order for ioctls is (1) tty
+ *	layer, (2) tty low-level driver, (3) line discpline.  So a
+ *	low-level driver can "grab" an ioctl request before the line
+ *	discpline has a chance to see it.
+ *
  * long	(*compat_ioctl)(struct tty_struct * tty, struct file * file,
- * 		        unsigned int cmd, unsigned long arg);
+ *		        unsigned int cmd, unsigned long arg);
  *
- *      Process ioctl calls from 32-bit process on 64-bit system
+ *	Process ioctl calls from 32-bit process on 64-bit system
  *
  * void	(*set_termios)(struct tty_struct *tty, struct ktermios * old);
  *
- * 	This function notifies the line discpline that a change has
- * 	been made to the termios structure.
- * 
+ *	This function notifies the line discpline that a change has
+ *	been made to the termios structure.
+ *
  * int	(*poll)(struct tty_struct * tty, struct file * file,
- * 		  poll_table *wait);
+ *		  poll_table *wait);
  *
- * 	This function is called when a user attempts to select/poll on a
- * 	tty device.  It is solely the responsibility of the line
- * 	discipline to handle poll requests.
+ *	This function is called when a user attempts to select/poll on a
+ *	tty device.  It is solely the responsibility of the line
+ *	discipline to handle poll requests.
  *
  * void	(*receive_buf)(struct tty_struct *, const unsigned char *cp,
- * 		       char *fp, int count);
- *
- * 	This function is called by the low-level tty driver to send
- * 	characters received by the hardware to the line discpline for
- * 	processing.  <cp> is a pointer to the buffer of input
- * 	character received by the device.  <fp> is a pointer to a
- * 	pointer of flag bytes which indicate whether a character was
- * 	received with a parity error, etc.
- * 
+ *		       char *fp, int count);
+ *
+ *	This function is called by the low-level tty driver to send
+ *	characters received by the hardware to the line discpline for
+ *	processing.  <cp> is a pointer to the buffer of input
+ *	character received by the device.  <fp> is a pointer to a
+ *	pointer of flag bytes which indicate whether a character was
+ *	received with a parity error, etc.
+ *
  * void	(*write_wakeup)(struct tty_struct *);
  *
- * 	This function is called by the low-level tty driver to signal
- * 	that line discpline should try to send more characters to the
- * 	low-level driver for transmission.  If the line discpline does
- * 	not have any more data to send, it can just return.
+ *	This function is called by the low-level tty driver to signal
+ *	that line discpline should try to send more characters to the
+ *	low-level driver for transmission.  If the line discpline does
+ *	not have any more data to send, it can just return.
  *
  * int (*hangup)(struct tty_struct *)
  *
@@ -115,7 +115,7 @@ struct tty_ldisc_ops {
 	char	*name;
 	int	num;
 	int	flags;
-	
+
 	/*
 	 * The following routines are called from above.
 	 */
@@ -123,19 +123,19 @@ struct tty_ldisc_ops {
 	void	(*close)(struct tty_struct *);
 	void	(*flush_buffer)(struct tty_struct *tty);
 	ssize_t	(*chars_in_buffer)(struct tty_struct *tty);
-	ssize_t	(*read)(struct tty_struct * tty, struct file * file,
-			unsigned char __user * buf, size_t nr);
-	ssize_t	(*write)(struct tty_struct * tty, struct file * file,
-			 const unsigned char * buf, size_t nr);	
-	int	(*ioctl)(struct tty_struct * tty, struct file * file,
+	ssize_t	(*read)(struct tty_struct *tty, struct file *file,
+			unsigned char __user *buf, size_t nr);
+	ssize_t	(*write)(struct tty_struct *tty, struct file *file,
+			 const unsigned char *buf, size_t nr);
+	int	(*ioctl)(struct tty_struct *tty, struct file *file,
 			 unsigned int cmd, unsigned long arg);
-	long	(*compat_ioctl)(struct tty_struct * tty, struct file * file,
+	long	(*compat_ioctl)(struct tty_struct *tty, struct file *file,
 				unsigned int cmd, unsigned long arg);
-	void	(*set_termios)(struct tty_struct *tty, struct ktermios * old);
+	void	(*set_termios)(struct tty_struct *tty, struct ktermios *old);
 	unsigned int (*poll)(struct tty_struct *, struct file *,
 			     struct poll_table_struct *);
 	int	(*hangup)(struct tty_struct *tty);
-	
+
 	/*
 	 * The following routines are called from below.
 	 */
@@ -145,7 +145,7 @@ struct tty_ldisc_ops {
 	void	(*dcd_change)(struct tty_struct *, unsigned int);
 
 	struct  module *owner;
-	
+
 	int refcount;
 };
 
diff --git a/include/linux/ucs2_string.h b/include/linux/ucs2_string.h
new file mode 100644
index 00000000000..cbb20afdbc0
--- /dev/null
+++ b/include/linux/ucs2_string.h
@@ -0,0 +1,14 @@
+#ifndef _LINUX_UCS2_STRING_H_
+#define _LINUX_UCS2_STRING_H_
+
+#include <linux/types.h>	/* for size_t */
+#include <linux/stddef.h>	/* for NULL */
+
+typedef u16 ucs2_char_t;
+
+unsigned long ucs2_strnlen(const ucs2_char_t *s, size_t maxlength);
+unsigned long ucs2_strlen(const ucs2_char_t *s);
+unsigned long ucs2_strsize(const ucs2_char_t *data, unsigned long maxlength);
+int ucs2_strncmp(const ucs2_char_t *a, const ucs2_char_t *b, size_t len);
+
+#endif /* _LINUX_UCS2_STRING_H_ */
diff --git a/include/linux/udp.h b/include/linux/udp.h
index 9d81de123c9..42278bbf7a8 100644
--- a/include/linux/udp.h
+++ b/include/linux/udp.h
@@ -68,6 +68,7 @@ struct udp_sock {
 	 * For encapsulation sockets.
 	 */
 	int (*encap_rcv)(struct sock *sk, struct sk_buff *skb);
+	void (*encap_destroy)(struct sock *sk);
 };
 
 static inline struct udp_sock *udp_sk(const struct sock *sk)
diff --git a/include/linux/uprobes.h b/include/linux/uprobes.h
index 02b83db8e2c..06f28beed7c 100644
--- a/include/linux/uprobes.h
+++ b/include/linux/uprobes.h
@@ -38,6 +38,8 @@ struct inode;
 #define UPROBE_HANDLER_REMOVE		1
 #define UPROBE_HANDLER_MASK		1
 
+#define MAX_URETPROBE_DEPTH		64
+
 enum uprobe_filter_ctx {
 	UPROBE_FILTER_REGISTER,
 	UPROBE_FILTER_UNREGISTER,
@@ -46,6 +48,9 @@ enum uprobe_filter_ctx {
 
 struct uprobe_consumer {
 	int (*handler)(struct uprobe_consumer *self, struct pt_regs *regs);
+	int (*ret_handler)(struct uprobe_consumer *self,
+				unsigned long func,
+				struct pt_regs *regs);
 	bool (*filter)(struct uprobe_consumer *self,
 				enum uprobe_filter_ctx ctx,
 				struct mm_struct *mm);
@@ -68,6 +73,8 @@ struct uprobe_task {
 	enum uprobe_task_state		state;
 	struct arch_uprobe_task		autask;
 
+	struct return_instance		*return_instances;
+	unsigned int			depth;
 	struct uprobe			*active_uprobe;
 
 	unsigned long			xol_vaddr;
@@ -100,6 +107,7 @@ struct uprobes_state {
 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);
 extern bool __weak is_swbp_insn(uprobe_opcode_t *insn);
+extern bool __weak is_trap_insn(uprobe_opcode_t *insn);
 extern int uprobe_register(struct inode *inode, loff_t offset, struct uprobe_consumer *uc);
 extern int uprobe_apply(struct inode *inode, loff_t offset, struct uprobe_consumer *uc, bool);
 extern void uprobe_unregister(struct inode *inode, loff_t offset, struct uprobe_consumer *uc);
diff --git a/include/linux/usb.h b/include/linux/usb.h
index 4d22d0f6167..a0bee5a28d1 100644
--- a/include/linux/usb.h
+++ b/include/linux/usb.h
@@ -469,14 +469,12 @@ struct usb3_lpm_parameters {
  * @lpm_capable: device supports LPM
  * @usb2_hw_lpm_capable: device can perform USB2 hardware LPM
  * @usb2_hw_lpm_enabled: USB2 hardware LPM enabled
+ * @usb3_lpm_enabled: USB3 hardware LPM enabled
  * @string_langid: language ID for strings
  * @product: iProduct string, if present (static)
  * @manufacturer: iManufacturer string, if present (static)
  * @serial: iSerialNumber string, if present (static)
  * @filelist: usbfs files that are open to this device
- * @usb_classdev: USB class device that was created for usbfs device
- *	access from userspace
- * @usbfs_dentry: usbfs dentry entry for the device
  * @maxchild: number of ports if hub
  * @quirks: quirks of the whole device
  * @urbnum: number of URBs submitted for the whole device
@@ -619,7 +617,7 @@ static inline bool usb_acpi_power_manageable(struct usb_device *hdev, int index)
 #endif
 
 /* USB autosuspend and autoresume */
-#ifdef CONFIG_USB_SUSPEND
+#ifdef CONFIG_PM_RUNTIME
 extern void usb_enable_autosuspend(struct usb_device *udev);
 extern void usb_disable_autosuspend(struct usb_device *udev);
 
@@ -978,7 +976,12 @@ struct usbdrv_wrap {
  *	the "usbfs" filesystem.  This lets devices provide ways to
  *	expose information to user space regardless of where they
  *	do (or don't) show up otherwise in the filesystem.
- * @suspend: Called when the device is going to be suspended by the system.
+ * @suspend: Called when the device is going to be suspended by the
+ *	system either from system sleep or runtime suspend context. The
+ *	return value will be ignored in system sleep context, so do NOT
+ *	try to continue using the device if suspend fails in this case.
+ *	Instead, let the resume or reset-resume routine recover from
+ *	the failure.
  * @resume: Called when the device is being resumed by the system.
  * @reset_resume: Called when the suspended device has been reset instead
  *	of being resumed.
diff --git a/include/linux/usb/cdc-wdm.h b/include/linux/usb/cdc-wdm.h
index 719c332620f..0b3f4295c02 100644
--- a/include/linux/usb/cdc-wdm.h
+++ b/include/linux/usb/cdc-wdm.h
@@ -11,6 +11,8 @@
 #ifndef __LINUX_USB_CDC_WDM_H
 #define __LINUX_USB_CDC_WDM_H
 
+#include <uapi/linux/usb/cdc-wdm.h>
+
 extern struct usb_driver *usb_cdc_wdm_register(struct usb_interface *intf,
 					struct usb_endpoint_descriptor *ep,
 					int bufsize,
diff --git a/include/linux/usb/ch9.h b/include/linux/usb/ch9.h
index 9c210f2283d..27603bcbb9b 100644
--- a/include/linux/usb/ch9.h
+++ b/include/linux/usb/ch9.h
@@ -43,4 +43,13 @@
  */
 extern const char *usb_speed_string(enum usb_device_speed speed);
 
+
+/**
+ * usb_state_string - Returns human readable name for the state.
+ * @state: The state to return a human-readable name for. If it's not
+ *	any of the states devices in usb_device_state_string enum,
+ *	the string UNKNOWN will be returned.
+ */
+extern const char *usb_state_string(enum usb_device_state state);
+
 #endif /* __LINUX_USB_CH9_H */
diff --git a/include/linux/usb/composite.h b/include/linux/usb/composite.h
index 8860594d636..5e61589fc16 100644
--- a/include/linux/usb/composite.h
+++ b/include/linux/usb/composite.h
@@ -39,6 +39,7 @@
 #include <linux/usb/ch9.h>
 #include <linux/usb/gadget.h>
 #include <linux/log2.h>
+#include <linux/configfs.h>
 
 /*
  * USB function drivers should return USB_GADGET_DELAYED_STATUS if they
@@ -464,6 +465,8 @@ struct usb_function_driver {
 };
 
 struct usb_function_instance {
+	struct config_group group;
+	struct list_head cfs_list;
 	struct usb_function_driver *fd;
 	void (*free_func_inst)(struct usb_function_instance *inst);
 };
diff --git a/include/linux/usb/dwc3-omap.h b/include/linux/usb/dwc3-omap.h
index 51eae14477f..5615f4d8272 100644
--- a/include/linux/usb/dwc3-omap.h
+++ b/include/linux/usb/dwc3-omap.h
@@ -19,11 +19,11 @@ enum omap_dwc3_vbus_id_status {
 };
 
 #if (defined(CONFIG_USB_DWC3) || defined(CONFIG_USB_DWC3_MODULE))
-extern void dwc3_omap_mailbox(enum omap_dwc3_vbus_id_status status);
+extern int dwc3_omap_mailbox(enum omap_dwc3_vbus_id_status status);
 #else
-static inline void dwc3_omap_mailbox(enum omap_dwc3_vbus_id_status status)
+static inline int dwc3_omap_mailbox(enum omap_dwc3_vbus_id_status status)
 {
-	return;
+	return -ENODEV;
 }
 #endif
 
diff --git a/include/linux/usb/gadget.h b/include/linux/usb/gadget.h
index 2e297e80d59..c454a88abf2 100644
--- a/include/linux/usb/gadget.h
+++ b/include/linux/usb/gadget.h
@@ -482,6 +482,7 @@ struct usb_gadget_ops {
  * @speed: Speed of current connection to USB host.
  * @max_speed: Maximal speed the UDC can handle.  UDC must support this
  *      and all slower speeds.
+ * @state: the state we are now (attached, suspended, configured, etc)
  * @sg_supported: true if we can handle scatter-gather
  * @is_otg: True if the USB device port uses a Mini-AB jack, so that the
  *	gadget driver must provide a USB OTG descriptor.
@@ -525,6 +526,7 @@ struct usb_gadget {
 	struct list_head		ep_list;	/* of usb_ep */
 	enum usb_device_speed		speed;
 	enum usb_device_speed		max_speed;
+	enum usb_device_state		state;
 	unsigned			sg_supported:1;
 	unsigned			is_otg:1;
 	unsigned			is_a_peripheral:1;
@@ -872,6 +874,8 @@ int usb_gadget_probe_driver(struct usb_gadget_driver *driver);
  */
 int usb_gadget_unregister_driver(struct usb_gadget_driver *driver);
 
+extern int usb_add_gadget_udc_release(struct device *parent,
+		struct usb_gadget *gadget, void (*release)(struct device *dev));
 extern int usb_add_gadget_udc(struct device *parent, struct usb_gadget *gadget);
 extern void usb_del_gadget_udc(struct usb_gadget *gadget);
 extern int udc_attach_driver(const char *name,
@@ -959,6 +963,13 @@ extern void usb_gadget_unmap_request(struct usb_gadget *gadget,
 
 /*-------------------------------------------------------------------------*/
 
+/* utility to set gadget state properly */
+
+extern void usb_gadget_set_state(struct usb_gadget *gadget,
+		enum usb_device_state state);
+
+/*-------------------------------------------------------------------------*/
+
 /* utility wrapping a simple endpoint selection policy */
 
 extern struct usb_ep *usb_ep_autoconfig(struct usb_gadget *,
diff --git a/include/linux/usb/gadget_configfs.h b/include/linux/usb/gadget_configfs.h
new file mode 100644
index 00000000000..d74c0ae989d
--- /dev/null
+++ b/include/linux/usb/gadget_configfs.h
@@ -0,0 +1,110 @@
+#ifndef __GADGET_CONFIGFS__
+#define __GADGET_CONFIGFS__
+
+#include <linux/configfs.h>
+
+int check_user_usb_string(const char *name,
+		struct usb_gadget_strings *stringtab_dev);
+
+#define GS_STRINGS_W(__struct, __name)	\
+	static ssize_t __struct##_##__name##_store(struct __struct *gs, \
+		const char *page, size_t len)		\
+{							\
+	int ret;					\
+							\
+	ret = usb_string_copy(page, &gs->__name);	\
+	if (ret)					\
+		return ret;				\
+	return len;					\
+}
+
+#define GS_STRINGS_R(__struct, __name)	\
+	static ssize_t __struct##_##__name##_show(struct __struct *gs, \
+			char *page)	\
+{	\
+	return sprintf(page, "%s\n", gs->__name ?: "");	\
+}
+
+#define GS_STRING_ITEM_ATTR(struct_name, name)	\
+	static struct struct_name##_attribute struct_name##_##name = \
+		__CONFIGFS_ATTR(name,  S_IRUGO | S_IWUSR,		\
+				struct_name##_##name##_show,		\
+				struct_name##_##name##_store)
+
+#define GS_STRINGS_RW(struct_name, _name)	\
+	GS_STRINGS_R(struct_name, _name)	\
+	GS_STRINGS_W(struct_name, _name)	\
+	GS_STRING_ITEM_ATTR(struct_name, _name)
+
+#define USB_CONFIG_STRING_RW_OPS(struct_in)				\
+	CONFIGFS_ATTR_OPS(struct_in);					\
+									\
+static struct configfs_item_operations struct_in##_langid_item_ops = {	\
+	.release                = struct_in##_attr_release,		\
+	.show_attribute         = struct_in##_attr_show,		\
+	.store_attribute        = struct_in##_attr_store,		\
+};									\
+									\
+static struct config_item_type struct_in##_langid_type = {		\
+	.ct_item_ops	= &struct_in##_langid_item_ops,			\
+	.ct_attrs	= struct_in##_langid_attrs,			\
+	.ct_owner	= THIS_MODULE,					\
+}
+
+#define USB_CONFIG_STRINGS_LANG(struct_in, struct_member)	\
+	static struct config_group *struct_in##_strings_make(		\
+			struct config_group *group,			\
+			const char *name)				\
+	{								\
+	struct struct_member *gi;					\
+	struct struct_in *gs;						\
+	struct struct_in *new;						\
+	int langs = 0;							\
+	int ret;							\
+									\
+	new = kzalloc(sizeof(*new), GFP_KERNEL);			\
+	if (!new)							\
+		return ERR_PTR(-ENOMEM);				\
+									\
+	ret = check_user_usb_string(name, &new->stringtab_dev);		\
+	if (ret)							\
+		goto err;						\
+	config_group_init_type_name(&new->group, name,			\
+			&struct_in##_langid_type);			\
+									\
+	gi = container_of(group, struct struct_member, strings_group);	\
+	ret = -EEXIST;							\
+	list_for_each_entry(gs, &gi->string_list, list) {		\
+		if (gs->stringtab_dev.language == new->stringtab_dev.language) \
+			goto err;					\
+		langs++;						\
+	}								\
+	ret = -EOVERFLOW;						\
+	if (langs >= MAX_USB_STRING_LANGS)				\
+		goto err;						\
+									\
+	list_add_tail(&new->list, &gi->string_list);			\
+	return &new->group;						\
+err:									\
+	kfree(new);							\
+	return ERR_PTR(ret);						\
+}									\
+									\
+static void struct_in##_strings_drop(					\
+		struct config_group *group,				\
+		struct config_item *item)				\
+{									\
+	config_item_put(item);						\
+}									\
+									\
+static struct configfs_group_operations struct_in##_strings_ops = {	\
+	.make_group     = &struct_in##_strings_make,			\
+	.drop_item      = &struct_in##_strings_drop,			\
+};									\
+									\
+static struct config_item_type struct_in##_strings_type = {		\
+	.ct_group_ops   = &struct_in##_strings_ops,			\
+	.ct_owner       = THIS_MODULE,					\
+}
+
+#endif
diff --git a/include/linux/usb/hcd.h b/include/linux/usb/hcd.h
index 0a78df5f6cf..f5f5c7dfda9 100644
--- a/include/linux/usb/hcd.h
+++ b/include/linux/usb/hcd.h
@@ -84,7 +84,7 @@ struct usb_hcd {
 
 	struct timer_list	rh_timer;	/* drives root-hub polling */
 	struct urb		*status_urb;	/* the current status urb */
-#ifdef CONFIG_USB_SUSPEND
+#ifdef CONFIG_PM_RUNTIME
 	struct work_struct	wakeup_work;	/* for remote wakeup */
 #endif
 
@@ -357,6 +357,7 @@ struct hc_driver {
 		 */
 	int	(*disable_usb3_lpm_timeout)(struct usb_hcd *,
 			struct usb_device *, enum usb3_link_state state);
+	int	(*find_raw_port_number)(struct usb_hcd *, int);
 };
 
 extern int usb_hcd_link_urb_to_ep(struct usb_hcd *hcd, struct urb *urb);
@@ -396,6 +397,7 @@ extern int usb_hcd_is_primary_hcd(struct usb_hcd *hcd);
 extern int usb_add_hcd(struct usb_hcd *hcd,
 		unsigned int irqnum, unsigned long irqflags);
 extern void usb_remove_hcd(struct usb_hcd *hcd);
+extern int usb_hcd_find_raw_port_number(struct usb_hcd *hcd, int port1);
 
 struct platform_device;
 extern void usb_hcd_platform_shutdown(struct platform_device *dev);
@@ -591,14 +593,14 @@ extern int hcd_bus_suspend(struct usb_device *rhdev, pm_message_t msg);
 extern int hcd_bus_resume(struct usb_device *rhdev, pm_message_t msg);
 #endif /* CONFIG_PM */
 
-#ifdef CONFIG_USB_SUSPEND
+#ifdef CONFIG_PM_RUNTIME
 extern void usb_hcd_resume_root_hub(struct usb_hcd *hcd);
 #else
 static inline void usb_hcd_resume_root_hub(struct usb_hcd *hcd)
 {
 	return;
 }
-#endif /* CONFIG_USB_SUSPEND */
+#endif /* CONFIG_PM_RUNTIME */
 
 /*-------------------------------------------------------------------------*/
 
diff --git a/include/linux/usb/musb-ux500.h b/include/linux/usb/musb-ux500.h
new file mode 100644
index 00000000000..1e2c7130f6e
--- /dev/null
+++ b/include/linux/usb/musb-ux500.h
@@ -0,0 +1,31 @@
+/*
+ * Copyright (C) 2013 ST-Ericsson AB
+ *
+ * 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.
+ */
+
+#ifndef __MUSB_UX500_H__
+#define __MUSB_UX500_H__
+
+enum ux500_musb_vbus_id_status {
+	UX500_MUSB_NONE = 0,
+	UX500_MUSB_VBUS,
+	UX500_MUSB_ID,
+	UX500_MUSB_CHARGER,
+	UX500_MUSB_ENUMERATED,
+	UX500_MUSB_RIDA,
+	UX500_MUSB_RIDB,
+	UX500_MUSB_RIDC,
+	UX500_MUSB_PREPARE,
+	UX500_MUSB_CLEAN,
+};
+
+#endif	/* __MUSB_UX500_H__ */
diff --git a/include/linux/usb/nop-usb-xceiv.h b/include/linux/usb/nop-usb-xceiv.h
index 28884c71741..148d35171aa 100644
--- a/include/linux/usb/nop-usb-xceiv.h
+++ b/include/linux/usb/nop-usb-xceiv.h
@@ -5,6 +5,11 @@
 
 struct nop_usb_xceiv_platform_data {
 	enum usb_phy_type type;
+	unsigned long clk_rate;
+
+	/* if set fails with -EPROBE_DEFER if can't get regulator */
+	unsigned int needs_vcc:1;
+	unsigned int needs_reset:1;
 };
 
 #if defined(CONFIG_NOP_USB_XCEIV) || (defined(CONFIG_NOP_USB_XCEIV_MODULE) && defined(MODULE))
diff --git a/include/linux/usb/otg.h b/include/linux/usb/otg.h
index e8a5fe87c6b..291e01ba32e 100644
--- a/include/linux/usb/otg.h
+++ b/include/linux/usb/otg.h
@@ -36,14 +36,7 @@ struct usb_otg {
 
 };
 
-#ifdef CONFIG_USB_OTG_UTILS
-extern const char *otg_state_string(enum usb_otg_state state);
-#else
-static inline const char *otg_state_string(enum usb_otg_state state)
-{
-	return NULL;
-}
-#endif
+extern const char *usb_otg_state_string(enum usb_otg_state state);
 
 /* Context: can sleep */
 static inline int
diff --git a/include/linux/usb/phy.h b/include/linux/usb/phy.h
index 15847cbdb51..6b5978f5763 100644
--- a/include/linux/usb/phy.h
+++ b/include/linux/usb/phy.h
@@ -91,6 +91,9 @@ struct usb_phy {
 	int	(*init)(struct usb_phy *x);
 	void	(*shutdown)(struct usb_phy *x);
 
+	/* enable/disable VBUS */
+	int	(*set_vbus)(struct usb_phy *x, int on);
+
 	/* effective for B devices, ignored for A-peripheral */
 	int	(*set_power)(struct usb_phy *x,
 				unsigned mA);
@@ -160,8 +163,26 @@ usb_phy_shutdown(struct usb_phy *x)
 		x->shutdown(x);
 }
 
+static inline int
+usb_phy_vbus_on(struct usb_phy *x)
+{
+	if (!x->set_vbus)
+		return 0;
+
+	return x->set_vbus(x, true);
+}
+
+static inline int
+usb_phy_vbus_off(struct usb_phy *x)
+{
+	if (!x->set_vbus)
+		return 0;
+
+	return x->set_vbus(x, false);
+}
+
 /* for usb host and peripheral controller drivers */
-#ifdef CONFIG_USB_OTG_UTILS
+#if IS_ENABLED(CONFIG_USB_PHY)
 extern struct usb_phy *usb_get_phy(enum usb_phy_type type);
 extern struct usb_phy *devm_usb_get_phy(struct device *dev,
 	enum usb_phy_type type);
@@ -176,29 +197,29 @@ extern int usb_bind_phy(const char *dev_name, u8 index,
 #else
 static inline struct usb_phy *usb_get_phy(enum usb_phy_type type)
 {
-	return NULL;
+	return ERR_PTR(-ENXIO);
 }
 
 static inline struct usb_phy *devm_usb_get_phy(struct device *dev,
 	enum usb_phy_type type)
 {
-	return NULL;
+	return ERR_PTR(-ENXIO);
 }
 
 static inline struct usb_phy *usb_get_phy_dev(struct device *dev, u8 index)
 {
-	return NULL;
+	return ERR_PTR(-ENXIO);
 }
 
 static inline struct usb_phy *devm_usb_get_phy_dev(struct device *dev, u8 index)
 {
-	return NULL;
+	return ERR_PTR(-ENXIO);
 }
 
 static inline struct usb_phy *devm_usb_get_phy_by_phandle(struct device *dev,
 	const char *phandle, u8 index)
 {
-	return NULL;
+	return ERR_PTR(-ENXIO);
 }
 
 static inline void usb_put_phy(struct usb_phy *x)
diff --git a/include/linux/usb/renesas_usbhs.h b/include/linux/usb/renesas_usbhs.h
index c5d36c65c33..e452ba6ec6b 100644
--- a/include/linux/usb/renesas_usbhs.h
+++ b/include/linux/usb/renesas_usbhs.h
@@ -62,14 +62,14 @@ struct renesas_usbhs_platform_callback {
 	 * Hardware exit function for platform.
 	 * it is called when driver was removed
 	 */
-	void (*hardware_exit)(struct platform_device *pdev);
+	int (*hardware_exit)(struct platform_device *pdev);
 
 	/*
 	 * option:
 	 *
 	 * for board specific clock control
 	 */
-	void (*power_ctrl)(struct platform_device *pdev,
+	int (*power_ctrl)(struct platform_device *pdev,
 			   void __iomem *base, int enable);
 
 	/*
@@ -77,7 +77,7 @@ struct renesas_usbhs_platform_callback {
 	 *
 	 * Phy reset for platform
 	 */
-	void (*phy_reset)(struct platform_device *pdev);
+	int (*phy_reset)(struct platform_device *pdev);
 
 	/*
 	 * get USB ID function
diff --git a/include/linux/usb/serial.h b/include/linux/usb/serial.h
index 1819b59aab2..b9b0f7b4e43 100644
--- a/include/linux/usb/serial.h
+++ b/include/linux/usb/serial.h
@@ -15,6 +15,7 @@
 
 #include <linux/kref.h>
 #include <linux/mutex.h>
+#include <linux/serial.h>
 #include <linux/sysrq.h>
 #include <linux/kfifo.h>
 
@@ -61,12 +62,12 @@
  * @bulk_out_buffers: pointers to the bulk out buffers for this port
  * @write_urbs: pointers to the bulk out urbs for this port
  * @write_urbs_free: status bitmap the for bulk out urbs
+ * @icount: interrupt counters
  * @tx_bytes: number of bytes currently in host stack queues
  * @bulk_out_endpointAddress: endpoint address for the bulk out pipe for this
  *	port.
  * @flags: usb serial port flags
  * @write_wait: a wait_queue_head_t used by the port.
- * @delta_msr_wait: modem-status-change wait queue
  * @work: work queue entry for the line discipline waking up.
  * @throttled: nonzero if the read urb is inactive to throttle the device
  * @throttle_req: nonzero if the tty wants to throttle us
@@ -109,11 +110,11 @@ struct usb_serial_port {
 	unsigned long		write_urbs_free;
 	__u8			bulk_out_endpointAddress;
 
+	struct async_icount	icount;
 	int			tx_bytes;
 
 	unsigned long		flags;
 	wait_queue_head_t	write_wait;
-	wait_queue_head_t	delta_msr_wait;
 	struct work_struct	work;
 	char			throttled;
 	char			throttle_req;
@@ -272,6 +273,7 @@ struct usb_serial_driver {
 	int  (*tiocmget)(struct tty_struct *tty);
 	int  (*tiocmset)(struct tty_struct *tty,
 			 unsigned int set, unsigned int clear);
+	int  (*tiocmiwait)(struct tty_struct *tty, unsigned long arg);
 	int  (*get_icount)(struct tty_struct *tty,
 			struct serial_icounter_struct *icount);
 	/* Called by the tty layer for port level work. There may or may not
@@ -329,8 +331,10 @@ extern void usb_serial_generic_read_bulk_callback(struct urb *urb);
 extern void usb_serial_generic_write_bulk_callback(struct urb *urb);
 extern void usb_serial_generic_throttle(struct tty_struct *tty);
 extern void usb_serial_generic_unthrottle(struct tty_struct *tty);
-extern void usb_serial_generic_disconnect(struct usb_serial *serial);
-extern void usb_serial_generic_release(struct usb_serial *serial);
+extern int usb_serial_generic_tiocmiwait(struct tty_struct *tty,
+							unsigned long arg);
+extern int usb_serial_generic_get_icount(struct tty_struct *tty,
+					struct serial_icounter_struct *icount);
 extern int usb_serial_generic_register(void);
 extern void usb_serial_generic_deregister(void);
 extern int usb_serial_generic_submit_read_urbs(struct usb_serial_port *port,
diff --git a/include/linux/usb/tegra_usb_phy.h b/include/linux/usb/tegra_usb_phy.h
index 9ebebe90692..1b7519a8c0b 100644
--- a/include/linux/usb/tegra_usb_phy.h
+++ b/include/linux/usb/tegra_usb_phy.h
@@ -61,10 +61,14 @@ struct tegra_usb_phy {
 	struct device *dev;
 	bool is_legacy_phy;
 	bool is_ulpi_phy;
+	void (*set_pts)(struct usb_phy *x, u8 pts_val);
+	void (*set_phcd)(struct usb_phy *x, bool enable);
 };
 
 struct tegra_usb_phy *tegra_usb_phy_open(struct device *dev, int instance,
-	void __iomem *regs, void *config, enum tegra_usb_phy_mode phy_mode);
+	void __iomem *regs, void *config, enum tegra_usb_phy_mode phy_mode,
+	void (*set_pts)(struct usb_phy *x, u8 pts_val),
+	void (*set_phcd)(struct usb_phy *x, bool enable));
 
 void tegra_usb_phy_preresume(struct usb_phy *phy);
 
@@ -75,8 +79,4 @@ void tegra_ehci_phy_restore_start(struct usb_phy *phy,
 
 void tegra_ehci_phy_restore_end(struct usb_phy *phy);
 
-void tegra_ehci_set_pts(struct usb_phy *x, u8 pts_val);
-
-void tegra_ehci_set_phcd(struct usb_phy *x, bool enable);
-
 #endif /* __TEGRA_USB_PHY_H */
diff --git a/include/linux/user_namespace.h b/include/linux/user_namespace.h
index 4ce00932493..b6b215f13b4 100644
--- a/include/linux/user_namespace.h
+++ b/include/linux/user_namespace.h
@@ -26,6 +26,8 @@ struct user_namespace {
 	kuid_t			owner;
 	kgid_t			group;
 	unsigned int		proc_inum;
+	bool			may_mount_sysfs;
+	bool			may_mount_proc;
 };
 
 extern struct user_namespace init_user_ns;
@@ -82,4 +84,6 @@ static inline void put_user_ns(struct user_namespace *ns)
 
 #endif
 
+void update_mnt_policy(struct user_namespace *userns);
+
 #endif /* _LINUX_USER_H */
diff --git a/include/linux/vmalloc.h b/include/linux/vmalloc.h
index 6071e911c7f..7d5773a99f2 100644
--- a/include/linux/vmalloc.h
+++ b/include/linux/vmalloc.h
@@ -3,7 +3,9 @@
 
 #include <linux/spinlock.h>
 #include <linux/init.h>
+#include <linux/list.h>
 #include <asm/page.h>		/* pgprot_t */
+#include <linux/rbtree.h>
 
 struct vm_area_struct;		/* vma defining user mapping in mm_types.h */
 
@@ -35,6 +37,17 @@ struct vm_struct {
 	const void		*caller;
 };
 
+struct vmap_area {
+	unsigned long va_start;
+	unsigned long va_end;
+	unsigned long flags;
+	struct rb_node rb_node;         /* address sorted rbtree */
+	struct list_head list;          /* address sorted list */
+	struct list_head purge_list;    /* "lazy purge" list */
+	struct vm_struct *vm;
+	struct rcu_head rcu_head;
+};
+
 /*
  *	Highlevel APIs for driver use
  */
@@ -130,8 +143,7 @@ extern long vwrite(char *buf, char *addr, unsigned long count);
 /*
  *	Internals.  Dont't use..
  */
-extern rwlock_t vmlist_lock;
-extern struct vm_struct *vmlist;
+extern struct list_head vmap_area_list;
 extern __init void vm_area_add_early(struct vm_struct *vm);
 extern __init void vm_area_register_early(struct vm_struct *vm, size_t align);
 
@@ -158,4 +170,22 @@ pcpu_free_vm_areas(struct vm_struct **vms, int nr_vms)
 # endif
 #endif
 
+struct vmalloc_info {
+	unsigned long   used;
+	unsigned long   largest_chunk;
+};
+
+#ifdef CONFIG_MMU
+#define VMALLOC_TOTAL (VMALLOC_END - VMALLOC_START)
+extern void get_vmalloc_info(struct vmalloc_info *vmi);
+#else
+
+#define VMALLOC_TOTAL 0UL
+#define get_vmalloc_info(vmi)			\
+do {						\
+	(vmi)->used = 0;			\
+	(vmi)->largest_chunk = 0;		\
+} while (0)
+#endif
+
 #endif /* _LINUX_VMALLOC_H */
diff --git a/include/linux/vmpressure.h b/include/linux/vmpressure.h
new file mode 100644
index 00000000000..76be077340e
--- /dev/null
+++ b/include/linux/vmpressure.h
@@ -0,0 +1,47 @@
+#ifndef __LINUX_VMPRESSURE_H
+#define __LINUX_VMPRESSURE_H
+
+#include <linux/mutex.h>
+#include <linux/list.h>
+#include <linux/workqueue.h>
+#include <linux/gfp.h>
+#include <linux/types.h>
+#include <linux/cgroup.h>
+
+struct vmpressure {
+	unsigned long scanned;
+	unsigned long reclaimed;
+	/* The lock is used to keep the scanned/reclaimed above in sync. */
+	struct mutex sr_lock;
+
+	/* The list of vmpressure_event structs. */
+	struct list_head events;
+	/* Have to grab the lock on events traversal or modifications. */
+	struct mutex events_lock;
+
+	struct work_struct work;
+};
+
+struct mem_cgroup;
+
+#ifdef CONFIG_MEMCG
+extern void vmpressure(gfp_t gfp, struct mem_cgroup *memcg,
+		       unsigned long scanned, unsigned long reclaimed);
+extern void vmpressure_prio(gfp_t gfp, struct mem_cgroup *memcg, int prio);
+
+extern void vmpressure_init(struct vmpressure *vmpr);
+extern struct vmpressure *memcg_to_vmpressure(struct mem_cgroup *memcg);
+extern struct cgroup_subsys_state *vmpressure_to_css(struct vmpressure *vmpr);
+extern struct vmpressure *css_to_vmpressure(struct cgroup_subsys_state *css);
+extern int vmpressure_register_event(struct cgroup *cg, struct cftype *cft,
+				     struct eventfd_ctx *eventfd,
+				     const char *args);
+extern void vmpressure_unregister_event(struct cgroup *cg, struct cftype *cft,
+					struct eventfd_ctx *eventfd);
+#else
+static inline void vmpressure(gfp_t gfp, struct mem_cgroup *memcg,
+			      unsigned long scanned, unsigned long reclaimed) {}
+static inline void vmpressure_prio(gfp_t gfp, struct mem_cgroup *memcg,
+				   int prio) {}
+#endif /* CONFIG_MEMCG */
+#endif /* __LINUX_VMPRESSURE_H */
diff --git a/include/linux/vmstat.h b/include/linux/vmstat.h
index 5fd71a7d0df..c586679b6fe 100644
--- a/include/linux/vmstat.h
+++ b/include/linux/vmstat.h
@@ -48,13 +48,8 @@ static inline void count_vm_events(enum vm_event_item item, long delta)
 }
 
 extern void all_vm_events(unsigned long *);
-#ifdef CONFIG_HOTPLUG
+
 extern void vm_events_fold_cpu(int cpu);
-#else
-static inline void vm_events_fold_cpu(int cpu)
-{
-}
-#endif
 
 #else
 
diff --git a/include/linux/workqueue.h b/include/linux/workqueue.h
index 8afab27cdbc..71797563937 100644
--- a/include/linux/workqueue.h
+++ b/include/linux/workqueue.h
@@ -11,6 +11,7 @@
 #include <linux/lockdep.h>
 #include <linux/threads.h>
 #include <linux/atomic.h>
+#include <linux/cpumask.h>
 
 struct workqueue_struct;
 
@@ -68,7 +69,7 @@ enum {
 				  WORK_STRUCT_COLOR_BITS,
 
 	/* data contains off-queue information when !WORK_STRUCT_PWQ */
-	WORK_OFFQ_FLAG_BASE	= WORK_STRUCT_FLAG_BITS,
+	WORK_OFFQ_FLAG_BASE	= WORK_STRUCT_COLOR_SHIFT,
 
 	WORK_OFFQ_CANCELING	= (1 << WORK_OFFQ_FLAG_BASE),
 
@@ -115,6 +116,20 @@ struct delayed_work {
 	int cpu;
 };
 
+/*
+ * A struct for workqueue attributes.  This can be used to change
+ * attributes of an unbound workqueue.
+ *
+ * Unlike other fields, ->no_numa isn't a property of a worker_pool.  It
+ * only modifies how apply_workqueue_attrs() select pools and thus doesn't
+ * participate in pool hash calculations or equality comparisons.
+ */
+struct workqueue_attrs {
+	int			nice;		/* nice level */
+	cpumask_var_t		cpumask;	/* allowed CPUs */
+	bool			no_numa;	/* disable NUMA affinity */
+};
+
 static inline struct delayed_work *to_delayed_work(struct work_struct *work)
 {
 	return container_of(work, struct delayed_work, work);
@@ -283,9 +298,10 @@ enum {
 	WQ_MEM_RECLAIM		= 1 << 3, /* may be used for memory reclaim */
 	WQ_HIGHPRI		= 1 << 4, /* high priority */
 	WQ_CPU_INTENSIVE	= 1 << 5, /* cpu instensive workqueue */
+	WQ_SYSFS		= 1 << 6, /* visible in sysfs, see wq_sysfs_register() */
 
-	WQ_DRAINING		= 1 << 6, /* internal: workqueue is draining */
-	WQ_RESCUER		= 1 << 7, /* internal: workqueue has rescuer */
+	__WQ_DRAINING		= 1 << 16, /* internal: workqueue is draining */
+	__WQ_ORDERED		= 1 << 17, /* internal: workqueue is ordered */
 
 	WQ_MAX_ACTIVE		= 512,	  /* I like 512, better ideas? */
 	WQ_MAX_UNBOUND_PER_CPU	= 4,	  /* 4 * #cpus for unbound wq */
@@ -388,7 +404,7 @@ __alloc_workqueue_key(const char *fmt, unsigned int flags, int max_active,
  * Pointer to the allocated workqueue on success, %NULL on failure.
  */
 #define alloc_ordered_workqueue(fmt, flags, args...)			\
-	alloc_workqueue(fmt, WQ_UNBOUND | (flags), 1, ##args)
+	alloc_workqueue(fmt, WQ_UNBOUND | __WQ_ORDERED | (flags), 1, ##args)
 
 #define create_workqueue(name)						\
 	alloc_workqueue((name), WQ_MEM_RECLAIM, 1)
@@ -399,30 +415,23 @@ __alloc_workqueue_key(const char *fmt, unsigned int flags, int max_active,
 
 extern void destroy_workqueue(struct workqueue_struct *wq);
 
+struct workqueue_attrs *alloc_workqueue_attrs(gfp_t gfp_mask);
+void free_workqueue_attrs(struct workqueue_attrs *attrs);
+int apply_workqueue_attrs(struct workqueue_struct *wq,
+			  const struct workqueue_attrs *attrs);
+
 extern bool queue_work_on(int cpu, struct workqueue_struct *wq,
 			struct work_struct *work);
-extern bool queue_work(struct workqueue_struct *wq, struct work_struct *work);
 extern bool queue_delayed_work_on(int cpu, struct workqueue_struct *wq,
 			struct delayed_work *work, unsigned long delay);
-extern bool queue_delayed_work(struct workqueue_struct *wq,
-			struct delayed_work *work, unsigned long delay);
 extern bool mod_delayed_work_on(int cpu, struct workqueue_struct *wq,
 			struct delayed_work *dwork, unsigned long delay);
-extern bool mod_delayed_work(struct workqueue_struct *wq,
-			struct delayed_work *dwork, unsigned long delay);
 
 extern void flush_workqueue(struct workqueue_struct *wq);
 extern void drain_workqueue(struct workqueue_struct *wq);
 extern void flush_scheduled_work(void);
 
-extern bool schedule_work_on(int cpu, struct work_struct *work);
-extern bool schedule_work(struct work_struct *work);
-extern bool schedule_delayed_work_on(int cpu, struct delayed_work *work,
-				     unsigned long delay);
-extern bool schedule_delayed_work(struct delayed_work *work,
-				  unsigned long delay);
 extern int schedule_on_each_cpu(work_func_t func);
-extern int keventd_up(void);
 
 int execute_in_process_context(work_func_t fn, struct execute_work *);
 
@@ -435,9 +444,121 @@ extern bool cancel_delayed_work_sync(struct delayed_work *dwork);
 
 extern void workqueue_set_max_active(struct workqueue_struct *wq,
 				     int max_active);
-extern bool workqueue_congested(unsigned int cpu, struct workqueue_struct *wq);
+extern bool current_is_workqueue_rescuer(void);
+extern bool workqueue_congested(int cpu, struct workqueue_struct *wq);
 extern unsigned int work_busy(struct work_struct *work);
 
+/**
+ * queue_work - queue work on a workqueue
+ * @wq: workqueue to use
+ * @work: work to queue
+ *
+ * Returns %false if @work was already on a queue, %true otherwise.
+ *
+ * We queue the work to the CPU on which it was submitted, but if the CPU dies
+ * it can be processed by another CPU.
+ */
+static inline bool queue_work(struct workqueue_struct *wq,
+			      struct work_struct *work)
+{
+	return queue_work_on(WORK_CPU_UNBOUND, wq, work);
+}
+
+/**
+ * queue_delayed_work - queue work on a workqueue after delay
+ * @wq: workqueue to use
+ * @dwork: delayable work to queue
+ * @delay: number of jiffies to wait before queueing
+ *
+ * Equivalent to queue_delayed_work_on() but tries to use the local CPU.
+ */
+static inline bool queue_delayed_work(struct workqueue_struct *wq,
+				      struct delayed_work *dwork,
+				      unsigned long delay)
+{
+	return queue_delayed_work_on(WORK_CPU_UNBOUND, wq, dwork, delay);
+}
+
+/**
+ * mod_delayed_work - modify delay of or queue a delayed work
+ * @wq: workqueue to use
+ * @dwork: work to queue
+ * @delay: number of jiffies to wait before queueing
+ *
+ * mod_delayed_work_on() on local CPU.
+ */
+static inline bool mod_delayed_work(struct workqueue_struct *wq,
+				    struct delayed_work *dwork,
+				    unsigned long delay)
+{
+	return mod_delayed_work_on(WORK_CPU_UNBOUND, wq, dwork, delay);
+}
+
+/**
+ * schedule_work_on - put work task on a specific cpu
+ * @cpu: cpu to put the work task on
+ * @work: job to be done
+ *
+ * This puts a job on a specific cpu
+ */
+static inline bool schedule_work_on(int cpu, struct work_struct *work)
+{
+	return queue_work_on(cpu, system_wq, work);
+}
+
+/**
+ * schedule_work - put work task in global workqueue
+ * @work: job to be done
+ *
+ * Returns %false if @work was already on the kernel-global workqueue and
+ * %true otherwise.
+ *
+ * This puts a job in the kernel-global workqueue if it was not already
+ * queued and leaves it in the same position on the kernel-global
+ * workqueue otherwise.
+ */
+static inline bool schedule_work(struct work_struct *work)
+{
+	return queue_work(system_wq, work);
+}
+
+/**
+ * schedule_delayed_work_on - queue work in global workqueue on CPU after delay
+ * @cpu: cpu to use
+ * @dwork: job to be done
+ * @delay: number of jiffies to wait
+ *
+ * After waiting for a given time this puts a job in the kernel-global
+ * workqueue on the specified CPU.
+ */
+static inline bool schedule_delayed_work_on(int cpu, struct delayed_work *dwork,
+					    unsigned long delay)
+{
+	return queue_delayed_work_on(cpu, system_wq, dwork, delay);
+}
+
+/**
+ * schedule_delayed_work - put work task in global workqueue after delay
+ * @dwork: job to be done
+ * @delay: number of jiffies to wait or 0 for immediate execution
+ *
+ * After waiting for a given time this puts a job in the kernel-global
+ * workqueue.
+ */
+static inline bool schedule_delayed_work(struct delayed_work *dwork,
+					 unsigned long delay)
+{
+	return queue_delayed_work(system_wq, dwork, delay);
+}
+
+/**
+ * keventd_up - is workqueue initialized yet?
+ */
+static inline bool keventd_up(void)
+{
+	return system_wq != NULL;
+}
+
 /*
  * Like above, but uses del_timer() instead of del_timer_sync(). This means,
  * if it returns 0 the timer function may be running and the queueing is in
@@ -466,12 +587,12 @@ static inline bool __deprecated flush_delayed_work_sync(struct delayed_work *dwo
 }
 
 #ifndef CONFIG_SMP
-static inline long work_on_cpu(unsigned int cpu, long (*fn)(void *), void *arg)
+static inline long work_on_cpu(int cpu, long (*fn)(void *), void *arg)
 {
 	return fn(arg);
 }
 #else
-long work_on_cpu(unsigned int cpu, long (*fn)(void *), void *arg);
+long work_on_cpu(int cpu, long (*fn)(void *), void *arg);
 #endif /* CONFIG_SMP */
 
 #ifdef CONFIG_FREEZER
@@ -480,4 +601,11 @@ extern bool freeze_workqueues_busy(void);
 extern void thaw_workqueues(void);
 #endif /* CONFIG_FREEZER */
 
+#ifdef CONFIG_SYSFS
+int workqueue_sysfs_register(struct workqueue_struct *wq);
+#else	/* CONFIG_SYSFS */
+static inline int workqueue_sysfs_register(struct workqueue_struct *wq)
+{ return 0; }
+#endif	/* CONFIG_SYSFS */
+
 #endif
diff --git a/include/media/davinci/dm355_ccdc.h b/include/media/davinci/dm355_ccdc.h
index adf2fe4bf0b..c669a9fb75e 100644
--- a/include/media/davinci/dm355_ccdc.h
+++ b/include/media/davinci/dm355_ccdc.h
@@ -38,7 +38,7 @@ enum ccdc_sample_line {
 	CCDC_SAMPLE_16LINES
 };
 
-/* enum for Alaw gama width */
+/* enum for Alaw gamma width */
 enum ccdc_gamma_width {
 	CCDC_GAMMA_BITS_13_4,
 	CCDC_GAMMA_BITS_12_3,
@@ -97,8 +97,8 @@ enum ccdc_mfilt2 {
 struct ccdc_a_law {
 	/* Enable/disable A-Law */
 	unsigned char enable;
-	/* Gama Width Input */
-	enum ccdc_gamma_width gama_wd;
+	/* Gamma Width Input */
+	enum ccdc_gamma_width gamma_wd;
 };
 
 /* structure for Black Clamping */
diff --git a/include/media/davinci/dm644x_ccdc.h b/include/media/davinci/dm644x_ccdc.h
index 3e178eb52fb..852e96c4bb4 100644
--- a/include/media/davinci/dm644x_ccdc.h
+++ b/include/media/davinci/dm644x_ccdc.h
@@ -38,17 +38,23 @@ enum ccdc_sample_line {
 	CCDC_SAMPLE_16LINES
 };
 
-/* enum for Alaw gama width */
-enum ccdc_gama_width {
-	CCDC_GAMMA_BITS_15_6,
+/* enum for Alaw gamma width */
+enum ccdc_gamma_width {
+	CCDC_GAMMA_BITS_15_6,	/* use bits 15-6 for gamma */
 	CCDC_GAMMA_BITS_14_5,
 	CCDC_GAMMA_BITS_13_4,
 	CCDC_GAMMA_BITS_12_3,
 	CCDC_GAMMA_BITS_11_2,
 	CCDC_GAMMA_BITS_10_1,
-	CCDC_GAMMA_BITS_09_0
+	CCDC_GAMMA_BITS_09_0	/* use bits 9-0 for gamma */
 };
 
+/* returns the highest bit used for the gamma */
+static inline u8 ccdc_gamma_width_max_bit(enum ccdc_gamma_width width)
+{
+	return 15 - width;
+}
+
 enum ccdc_data_size {
 	CCDC_DATA_16BITS,
 	CCDC_DATA_15BITS,
@@ -60,12 +66,18 @@ enum ccdc_data_size {
 	CCDC_DATA_8BITS
 };
 
+/* returns the highest bit used for this data size */
+static inline u8 ccdc_data_size_max_bit(enum ccdc_data_size sz)
+{
+	return sz == CCDC_DATA_8BITS ? 7 : 15 - sz;
+}
+
 /* structure for ALaw */
 struct ccdc_a_law {
 	/* Enable/disable A-Law */
 	unsigned char enable;
-	/* Gama Width Input */
-	enum ccdc_gama_width gama_wd;
+	/* Gamma Width Input */
+	enum ccdc_gamma_width gamma_wd;
 };
 
 /* structure for Black Clamping */
diff --git a/include/media/davinci/vpbe.h b/include/media/davinci/vpbe.h
index a7ca4884c46..57585c7004a 100644
--- a/include/media/davinci/vpbe.h
+++ b/include/media/davinci/vpbe.h
@@ -132,7 +132,7 @@ struct vpbe_device_ops {
 			       struct v4l2_enum_dv_timings *timings_info);
 
 	/* Set std at the output */
-	int (*s_std)(struct vpbe_device *vpbe_dev, v4l2_std_id *std_id);
+	int (*s_std)(struct vpbe_device *vpbe_dev, v4l2_std_id std_id);
 
 	/* Get the current std at the output */
 	int (*g_std)(struct vpbe_device *vpbe_dev, v4l2_std_id *std_id);
diff --git a/include/media/davinci/vpbe_types.h b/include/media/davinci/vpbe_types.h
index 9b85396514b..05dbe0ba514 100644
--- a/include/media/davinci/vpbe_types.h
+++ b/include/media/davinci/vpbe_types.h
@@ -26,8 +26,7 @@ enum vpbe_version {
 /* vpbe_timing_type - Timing types used in vpbe device */
 enum vpbe_enc_timings_type {
 	VPBE_ENC_STD = 0x1,
-	VPBE_ENC_DV_PRESET = 0x2,
-	VPBE_ENC_CUSTOM_TIMINGS = 0x4,
+	VPBE_ENC_DV_TIMINGS = 0x4,
 	/* Used when set timings through FB device interface */
 	VPBE_ENC_TIMINGS_INVALID = 0x8,
 };
diff --git a/include/media/media-devnode.h b/include/media/media-devnode.h
index f6caafc874c..3446af279fc 100644
--- a/include/media/media-devnode.h
+++ b/include/media/media-devnode.h
@@ -46,6 +46,7 @@ struct media_file_operations {
 	ssize_t (*write) (struct file *, const char __user *, size_t, loff_t *);
 	unsigned int (*poll) (struct file *, struct poll_table_struct *);
 	long (*ioctl) (struct file *, unsigned int, unsigned long);
+	long (*compat_ioctl) (struct file *, unsigned int, unsigned long);
 	int (*open) (struct file *);
 	int (*release) (struct file *);
 };
diff --git a/include/media/mt9p031.h b/include/media/mt9p031.h
index 0c97b19af29..b1e63f2b72b 100644
--- a/include/media/mt9p031.h
+++ b/include/media/mt9p031.h
@@ -5,13 +5,11 @@ struct v4l2_subdev;
 
 /*
  * 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;
 	int ext_freq;
 	int target_freq;
diff --git a/include/media/rc-core.h b/include/media/rc-core.h
index f03445f3c76..06a75deff55 100644
--- a/include/media/rc-core.h
+++ b/include/media/rc-core.h
@@ -51,6 +51,7 @@ enum rc_driver_type {
  * @driver_type: specifies if protocol decoding is done in hardware or software
  * @idle: used to keep track of RX state
  * @allowed_protos: bitmask with the supported RC_BIT_* protocols
+ * @enabled_protocols: bitmask with the enabled RC_BIT_* protocols
  * @scanmask: some hardware decoders are not capable of providing the full
  *	scancode to the application. As this is a hardware limit, we can't do
  *	anything with it. Yet, as the same keycode table can be used with other
@@ -99,6 +100,7 @@ struct rc_dev {
 	enum rc_driver_type		driver_type;
 	bool				idle;
 	u64				allowed_protos;
+	u64				enabled_protocols;
 	u32				scanmask;
 	void				*priv;
 	spinlock_t			keylock;
diff --git a/include/media/rc-map.h b/include/media/rc-map.h
index f74ee6f8971..5d5d3a30f04 100644
--- a/include/media/rc-map.h
+++ b/include/media/rc-map.h
@@ -172,6 +172,7 @@ void rc_map_init(void);
 #define RC_MAP_RC5_TV                    "rc-rc5-tv"
 #define RC_MAP_RC6_MCE                   "rc-rc6-mce"
 #define RC_MAP_REAL_AUDIO_220_32_KEYS    "rc-real-audio-220-32-keys"
+#define RC_MAP_REDDO                     "rc-reddo"
 #define RC_MAP_SNAPSTREAM_FIREFLY        "rc-snapstream-firefly"
 #define RC_MAP_STREAMZAP                 "rc-streamzap"
 #define RC_MAP_TBS_NEC                   "rc-tbs-nec"
diff --git a/include/media/s5p_fimc.h b/include/media/s5p_fimc.h
index 28f3590aa03..f50969025ef 100644
--- a/include/media/s5p_fimc.h
+++ b/include/media/s5p_fimc.h
@@ -13,6 +13,20 @@
 #define S5P_FIMC_H_
 
 #include <media/media-entity.h>
+#include <media/v4l2-mediabus.h>
+
+/*
+ * Enumeration of data inputs to the camera subsystem.
+ */
+enum fimc_input {
+	FIMC_INPUT_PARALLEL_0	= 1,
+	FIMC_INPUT_PARALLEL_1,
+	FIMC_INPUT_MIPI_CSI2_0	= 3,
+	FIMC_INPUT_MIPI_CSI2_1,
+	FIMC_INPUT_WRITEBACK_A	= 5,
+	FIMC_INPUT_WRITEBACK_B,
+	FIMC_INPUT_WRITEBACK_ISP = 5,
+};
 
 /*
  * Enumeration of the FIMC data bus types.
@@ -32,6 +46,20 @@ enum fimc_bus_type {
 	FIMC_BUS_TYPE_ISP_WRITEBACK = FIMC_BUS_TYPE_LCD_WRITEBACK_B,
 };
 
+#define fimc_input_is_parallel(x) ((x) == 1 || (x) == 2)
+#define fimc_input_is_mipi_csi(x) ((x) == 3 || (x) == 4)
+
+/*
+ * The subdevices' group IDs.
+ */
+#define GRP_ID_SENSOR		(1 << 8)
+#define GRP_ID_FIMC_IS_SENSOR	(1 << 9)
+#define GRP_ID_WRITEBACK	(1 << 10)
+#define GRP_ID_CSIS		(1 << 11)
+#define GRP_ID_FIMC		(1 << 12)
+#define GRP_ID_FLITE		(1 << 13)
+#define GRP_ID_FIMC_IS		(1 << 14)
+
 struct i2c_board_info;
 
 /**
@@ -77,10 +105,46 @@ struct s5p_platform_fimc {
  */
 #define S5P_FIMC_TX_END_NOTIFY _IO('e', 0)
 
+#define FIMC_MAX_PLANES	3
+
+/**
+ * struct fimc_fmt - color format data structure
+ * @mbus_code: media bus pixel code, -1 if not applicable
+ * @name: format description
+ * @fourcc: fourcc code for this format, 0 if not applicable
+ * @color: the driver's private color format id
+ * @memplanes: number of physically non-contiguous data planes
+ * @colplanes: number of physically contiguous data planes
+ * @depth: per plane driver's private 'number of bits per pixel'
+ * @mdataplanes: bitmask indicating meta data plane(s), (1 << plane_no)
+ * @flags: flags indicating which operation mode format applies to
+ */
+struct fimc_fmt {
+	enum v4l2_mbus_pixelcode mbus_code;
+	char	*name;
+	u32	fourcc;
+	u32	color;
+	u16	memplanes;
+	u16	colplanes;
+	u8	depth[FIMC_MAX_PLANES];
+	u16	mdataplanes;
+	u16	flags;
+#define FMT_FLAGS_CAM		(1 << 0)
+#define FMT_FLAGS_M2M_IN	(1 << 1)
+#define FMT_FLAGS_M2M_OUT	(1 << 2)
+#define FMT_FLAGS_M2M		(1 << 1 | 1 << 2)
+#define FMT_HAS_ALPHA		(1 << 3)
+#define FMT_FLAGS_COMPRESSED	(1 << 4)
+#define FMT_FLAGS_WRITEBACK	(1 << 5)
+#define FMT_FLAGS_RAW_BAYER	(1 << 6)
+#define FMT_FLAGS_YUV		(1 << 7)
+};
+
 enum fimc_subdev_index {
 	IDX_SENSOR,
 	IDX_CSIS,
 	IDX_FLITE,
+	IDX_IS_ISP,
 	IDX_FIMC,
 	IDX_MAX,
 };
diff --git a/include/media/saa7115.h b/include/media/saa7115.h
index bab21271959..407918625c8 100644
--- a/include/media/saa7115.h
+++ b/include/media/saa7115.h
@@ -21,6 +21,8 @@
 #ifndef _SAA7115_H_
 #define _SAA7115_H_
 
+/* s_routing inputs, outputs, and config */
+
 /* SAA7111/3/4/5 HW inputs */
 #define SAA7115_COMPOSITE0 0
 #define SAA7115_COMPOSITE1 1
@@ -33,24 +35,34 @@
 #define SAA7115_SVIDEO2    8
 #define SAA7115_SVIDEO3    9
 
-/* SAA7115 v4l2_crystal_freq frequency values */
-#define SAA7115_FREQ_32_11_MHZ  32110000   /* 32.11 MHz crystal, SAA7114/5 only */
-#define SAA7115_FREQ_24_576_MHZ 24576000   /* 24.576 MHz crystal */
-
-/* SAA7115 v4l2_crystal_freq audio clock control flags */
-#define SAA7115_FREQ_FL_UCGC   (1 << 0)	   /* SA 3A[7], UCGC, SAA7115 only */
-#define SAA7115_FREQ_FL_CGCDIV (1 << 1)	   /* SA 3A[6], CGCDIV, SAA7115 only */
-#define SAA7115_FREQ_FL_APLL   (1 << 2)	   /* SA 3A[3], APLL, SAA7114/5 only */
-
+/* outputs */
 #define SAA7115_IPORT_ON    	1
 #define SAA7115_IPORT_OFF   	0
 
-/* SAA7111 specific output flags */
+/* SAA7111 specific outputs. */
 #define SAA7111_VBI_BYPASS 	2
 #define SAA7111_FMT_YUV422      0x00
 #define SAA7111_FMT_RGB 	0x40
 #define SAA7111_FMT_CCIR 	0x80
 #define SAA7111_FMT_YUV411 	0xc0
 
+/* config flags */
+/* Register 0x85 should set bit 0 to 0 (it's 1 by default). This bit
+ * controls the IDQ signal polarity which is set to 'inverted' if the bit
+ * it 1 and to 'default' if it is 0. */
+#define SAA7115_IDQ_IS_DEFAULT  (1 << 0)
+
+/* s_crystal_freq values and flags */
+
+/* SAA7115 v4l2_crystal_freq frequency values */
+#define SAA7115_FREQ_32_11_MHZ  32110000   /* 32.11 MHz crystal, SAA7114/5 only */
+#define SAA7115_FREQ_24_576_MHZ 24576000   /* 24.576 MHz crystal */
+
+/* SAA7115 v4l2_crystal_freq audio clock control flags */
+#define SAA7115_FREQ_FL_UCGC         (1 << 0) /* SA 3A[7], UCGC, SAA7115 only */
+#define SAA7115_FREQ_FL_CGCDIV       (1 << 1) /* SA 3A[6], CGCDIV, SAA7115 only */
+#define SAA7115_FREQ_FL_APLL         (1 << 2) /* SA 3A[3], APLL, SAA7114/5 only */
+#define SAA7115_FREQ_FL_DOUBLE_ASCLK (1 << 3) /* SA 39, LRDIV, SAA7114/5 only */
+
 #endif
 
diff --git a/include/media/si476x.h b/include/media/si476x.h
new file mode 100644
index 00000000000..e02e241e2d2
--- /dev/null
+++ b/include/media/si476x.h
@@ -0,0 +1,37 @@
+/*
+ * include/media/si476x.h -- Common definitions for si476x driver
+ *
+ * Copyright (C) 2012 Innovative Converged Devices(ICD)
+ * Copyright (C) 2013 Andrey Smirnov
+ *
+ * Author: Andrey Smirnov <andrew.smirnov@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; version 2 of the License.
+ *
+ * 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.
+ *
+ */
+
+#ifndef SI476X_H
+#define SI476X_H
+
+#include <linux/types.h>
+#include <linux/videodev2.h>
+
+#include <linux/mfd/si476x-reports.h>
+
+enum si476x_ctrl_id {
+	V4L2_CID_SI476X_RSSI_THRESHOLD	= (V4L2_CID_USER_SI476X_BASE + 1),
+	V4L2_CID_SI476X_SNR_THRESHOLD	= (V4L2_CID_USER_SI476X_BASE + 2),
+	V4L2_CID_SI476X_MAX_TUNE_ERROR	= (V4L2_CID_USER_SI476X_BASE + 3),
+	V4L2_CID_SI476X_HARMONICS_COUNT	= (V4L2_CID_USER_SI476X_BASE + 4),
+	V4L2_CID_SI476X_DIVERSITY_MODE	= (V4L2_CID_USER_SI476X_BASE + 5),
+	V4L2_CID_SI476X_INTERCHIP_LINK	= (V4L2_CID_USER_SI476X_BASE + 6),
+};
+
+#endif /* SI476X_H*/
diff --git a/include/media/soc_camera.h b/include/media/soc_camera.h
index 2cc70cf318b..ff77d08c30f 100644
--- a/include/media/soc_camera.h
+++ b/include/media/soc_camera.h
@@ -292,12 +292,17 @@ struct soc_camera_sense {
 #define SOCAM_DATAWIDTH_8	SOCAM_DATAWIDTH(8)
 #define SOCAM_DATAWIDTH_9	SOCAM_DATAWIDTH(9)
 #define SOCAM_DATAWIDTH_10	SOCAM_DATAWIDTH(10)
+#define SOCAM_DATAWIDTH_12	SOCAM_DATAWIDTH(12)
 #define SOCAM_DATAWIDTH_15	SOCAM_DATAWIDTH(15)
 #define SOCAM_DATAWIDTH_16	SOCAM_DATAWIDTH(16)
+#define SOCAM_DATAWIDTH_18	SOCAM_DATAWIDTH(18)
+#define SOCAM_DATAWIDTH_24	SOCAM_DATAWIDTH(24)
 
 #define SOCAM_DATAWIDTH_MASK (SOCAM_DATAWIDTH_4 | SOCAM_DATAWIDTH_8 | \
 			      SOCAM_DATAWIDTH_9 | SOCAM_DATAWIDTH_10 | \
-			      SOCAM_DATAWIDTH_15 | SOCAM_DATAWIDTH_16)
+			      SOCAM_DATAWIDTH_12 | SOCAM_DATAWIDTH_15 | \
+			      SOCAM_DATAWIDTH_16 | SOCAM_DATAWIDTH_18 | \
+			      SOCAM_DATAWIDTH_24)
 
 static inline void soc_camera_limit_side(int *start, int *length,
 		unsigned int start_min,
diff --git a/include/media/soc_mediabus.h b/include/media/soc_mediabus.h
index 0dc6f4625b9..d33f6d05969 100644
--- a/include/media/soc_mediabus.h
+++ b/include/media/soc_mediabus.h
@@ -26,6 +26,8 @@
  * @SOC_MBUS_PACKING_VARIABLE:	compressed formats with variable packing
  * @SOC_MBUS_PACKING_1_5X8:	used for packed YUV 4:2:0 formats, where 4
  *				pixels occupy 6 bytes in RAM
+ * @SOC_MBUS_PACKING_EXTEND32:	sample width (e.g., 24 bits) has to be extended
+ *				to 32 bits
  */
 enum soc_mbus_packing {
 	SOC_MBUS_PACKING_NONE,
@@ -34,6 +36,7 @@ enum soc_mbus_packing {
 	SOC_MBUS_PACKING_EXTEND16,
 	SOC_MBUS_PACKING_VARIABLE,
 	SOC_MBUS_PACKING_1_5X8,
+	SOC_MBUS_PACKING_EXTEND32,
 };
 
 /**
diff --git a/include/media/ths7303.h b/include/media/ths7303.h
new file mode 100644
index 00000000000..980ec51d574
--- /dev/null
+++ b/include/media/ths7303.h
@@ -0,0 +1,42 @@
+/*
+ * Copyright (C) 2013 Texas Instruments Inc
+ *
+ * Copyright 2013 Cisco Systems, Inc. and/or its affiliates.
+ *
+ * Contributors:
+ *     Hans Verkuil <hans.verkuil@cisco.com>
+ *     Lad, Prabhakar <prabhakar.lad@ti.com>
+ *     Martin Bugge <marbugge@cisco.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 version 2.
+ *
+ * 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
+ */
+
+#ifndef THS7353_H
+#define THS7353_H
+
+/**
+ * struct ths7303_platform_data - Platform dependent data
+ * @ch_1: Bias value for channel one.
+ * @ch_2: Bias value for channel two.
+ * @ch_3: Bias value for channel three.
+ * @init_enable: initalize on init.
+ */
+struct ths7303_platform_data {
+	u8 ch_1;
+	u8 ch_2;
+	u8 ch_3;
+	u8 init_enable;
+};
+
+#endif
diff --git a/include/media/tuner.h b/include/media/tuner.h
index 926aff9bdf6..b46ebb48fe7 100644
--- a/include/media/tuner.h
+++ b/include/media/tuner.h
@@ -138,6 +138,10 @@
 #define TUNER_XC4000			87	/* Xceive Silicon Tuner */
 #define TUNER_XC5000C			88	/* Xceive Silicon Tuner */
 
+#define TUNER_SONY_BTF_PG472Z		89	/* PAL+SECAM */
+#define TUNER_SONY_BTF_PK467Z		90	/* NTSC_JP */
+#define TUNER_SONY_BTF_PB463Z		91	/* NTSC */
+
 /* tv card specific */
 #define TDA9887_PRESENT 		(1<<0)
 #define TDA9887_PORT1_INACTIVE 		(1<<1)
@@ -188,7 +192,7 @@ struct tuner_setup {
 	unsigned short	addr; 	/* I2C address */
 	unsigned int	type;   /* Tuner type */
 	unsigned int	mode_mask;  /* Allowed tuner modes */
-	unsigned int	config; /* configuraion for more complex tuners */
+	void		*config;    /* configuraion for more complex tuners */
 	int (*tuner_callback) (void *dev, int component, int cmd, int arg);
 };
 
diff --git a/include/media/uda1342.h b/include/media/uda1342.h
new file mode 100644
index 00000000000..cd156403a36
--- /dev/null
+++ b/include/media/uda1342.h
@@ -0,0 +1,29 @@
+/*
+ * uda1342.h - definition for uda1342 inputs
+ *
+ * Copyright 2013 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * 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.
+ *
+ */
+
+#ifndef _UDA1342_H_
+#define _UDA1342_H_
+
+/* The UDA1342 has 2 inputs */
+
+#define UDA1342_IN1 1
+#define UDA1342_IN2 2
+
+#endif
diff --git a/include/media/v4l2-chip-ident.h b/include/media/v4l2-chip-ident.h
index 4ee125bae71..c259b36bf1e 100644
--- a/include/media/v4l2-chip-ident.h
+++ b/include/media/v4l2-chip-ident.h
@@ -96,12 +96,20 @@ enum {
 	/* module au0828 */
 	V4L2_IDENT_AU0828 = 828,
 
+	/* module bttv: ident 848 + 849 */
+	V4L2_IDENT_BT848 = 848,
+	V4L2_IDENT_BT849 = 849,
+
 	/* module bt856: just ident 856 */
 	V4L2_IDENT_BT856 = 856,
 
 	/* module bt866: just ident 866 */
 	V4L2_IDENT_BT866 = 866,
 
+	/* module bttv: ident 878 + 879 */
+	V4L2_IDENT_BT878 = 878,
+	V4L2_IDENT_BT879 = 879,
+
 	/* module ks0127: reserved range 1120-1129 */
 	V4L2_IDENT_KS0122S = 1122,
 	V4L2_IDENT_KS0127  = 1127,
@@ -180,6 +188,9 @@ enum {
 	/* module adv7343: just ident 7343 */
 	V4L2_IDENT_ADV7343 = 7343,
 
+	/* module ths7353: just ident 7353 */
+	V4L2_IDENT_THS7353 = 7353,
+
 	/* module adv7393: just ident 7393 */
 	V4L2_IDENT_ADV7393 = 7393,
 
diff --git a/include/media/v4l2-common.h b/include/media/v4l2-common.h
index ec7c9c00b25..1d93c48cb37 100644
--- a/include/media/v4l2-common.h
+++ b/include/media/v4l2-common.h
@@ -201,7 +201,6 @@ void v4l_bound_align_image(unsigned int *w, unsigned int wmin,
 			   unsigned int *h, unsigned int hmin,
 			   unsigned int hmax, unsigned int halign,
 			   unsigned int salign);
-int v4l_fill_dv_preset_info(u32 preset, struct v4l2_dv_enum_preset *info);
 
 struct v4l2_discrete_probe {
 	const struct v4l2_frmsize_discrete	*sizes;
diff --git a/include/media/v4l2-ctrls.h b/include/media/v4l2-ctrls.h
index f00d42bc01a..7343a27fe81 100644
--- a/include/media/v4l2-ctrls.h
+++ b/include/media/v4l2-ctrls.h
@@ -259,7 +259,7 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
 		    s32 *min, s32 *max, s32 *step, s32 *def, u32 *flags);
 
 
-/** v4l2_ctrl_handler_init() - Initialize the control handler.
+/** v4l2_ctrl_handler_init_class() - Initialize the control handler.
   * @hdl:	The control handler.
   * @nr_of_controls_hint: A hint of how many controls this handler is
   *		expected to refer to. This is the total number, so including
@@ -268,12 +268,35 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
   *		are allocated) or the control lookup becomes slower (not enough
   *		buckets are allocated, so there are more slow list lookups).
   *		It will always work, though.
+  * @key:	Used by the lock validator if CONFIG_LOCKDEP is set.
+  * @name:	Used by the lock validator if CONFIG_LOCKDEP is set.
   *
   * Returns an error if the buckets could not be allocated. This error will
   * also be stored in @hdl->error.
+  *
+  * Never use this call directly, always use the v4l2_ctrl_handler_init
+  * macro that hides the @key and @name arguments.
   */
-int v4l2_ctrl_handler_init(struct v4l2_ctrl_handler *hdl,
-			   unsigned nr_of_controls_hint);
+int v4l2_ctrl_handler_init_class(struct v4l2_ctrl_handler *hdl,
+				 unsigned nr_of_controls_hint,
+				 struct lock_class_key *key, const char *name);
+
+#ifdef CONFIG_LOCKDEP
+#define v4l2_ctrl_handler_init(hdl, nr_of_controls_hint)		\
+(									\
+	({								\
+		static struct lock_class_key _key;			\
+		v4l2_ctrl_handler_init_class(hdl, nr_of_controls_hint,	\
+					&_key,				\
+					KBUILD_BASENAME ":"		\
+					__stringify(__LINE__) ":"	\
+					"(" #hdl ")->_lock");		\
+	})								\
+)
+#else
+#define v4l2_ctrl_handler_init(hdl, nr_of_controls_hint)		\
+	v4l2_ctrl_handler_init_class(hdl, nr_of_controls_hint, NULL, NULL)
+#endif
 
 /** v4l2_ctrl_handler_free() - Free all controls owned by the handler and free
   * the control list.
diff --git a/include/media/v4l2-device.h b/include/media/v4l2-device.h
index d61febfb166..c9b1593923f 100644
--- a/include/media/v4l2-device.h
+++ b/include/media/v4l2-device.h
@@ -190,4 +190,17 @@ v4l2_device_register_subdev_nodes(struct v4l2_device *v4l2_dev);
 			##args);					\
 })
 
+#define v4l2_device_has_op(v4l2_dev, o, f)				\
+({									\
+	struct v4l2_subdev *__sd;					\
+	bool __result = false;						\
+	list_for_each_entry(__sd, &(v4l2_dev)->subdevs, list) {		\
+		if (v4l2_subdev_has_op(__sd, o, f)) {			\
+			__result = true;				\
+			break;						\
+		}							\
+	}								\
+	__result;							\
+})
+
 #endif
diff --git a/include/media/v4l2-ioctl.h b/include/media/v4l2-ioctl.h
index 4118ad1324c..931652f0e2a 100644
--- a/include/media/v4l2-ioctl.h
+++ b/include/media/v4l2-ioctl.h
@@ -132,7 +132,7 @@ struct v4l2_ioctl_ops {
 			ENUMSTD is handled by videodev.c
 		 */
 	int (*vidioc_g_std) (struct file *file, void *fh, v4l2_std_id *norm);
-	int (*vidioc_s_std) (struct file *file, void *fh, v4l2_std_id *norm);
+	int (*vidioc_s_std) (struct file *file, void *fh, v4l2_std_id norm);
 	int (*vidioc_querystd) (struct file *file, void *fh, v4l2_std_id *a);
 
 		/* Input handling */
@@ -219,11 +219,11 @@ struct v4l2_ioctl_ops {
 	int (*vidioc_g_tuner)          (struct file *file, void *fh,
 					struct v4l2_tuner *a);
 	int (*vidioc_s_tuner)          (struct file *file, void *fh,
-					struct v4l2_tuner *a);
+					const struct v4l2_tuner *a);
 	int (*vidioc_g_frequency)      (struct file *file, void *fh,
 					struct v4l2_frequency *a);
 	int (*vidioc_s_frequency)      (struct file *file, void *fh,
-					struct v4l2_frequency *a);
+					const struct v4l2_frequency *a);
 	int (*vidioc_enum_freq_bands) (struct file *file, void *fh,
 				    struct v4l2_frequency_band *band);
 
@@ -242,7 +242,10 @@ struct v4l2_ioctl_ops {
 	int (*vidioc_g_register)       (struct file *file, void *fh,
 					struct v4l2_dbg_register *reg);
 	int (*vidioc_s_register)       (struct file *file, void *fh,
-					struct v4l2_dbg_register *reg);
+					const struct v4l2_dbg_register *reg);
+
+	int (*vidioc_g_chip_info)      (struct file *file, void *fh,
+					struct v4l2_dbg_chip_info *chip);
 #endif
 	int (*vidioc_g_chip_ident)     (struct file *file, void *fh,
 					struct v4l2_dbg_chip_ident *chip);
@@ -254,15 +257,6 @@ struct v4l2_ioctl_ops {
 					   struct v4l2_frmivalenum *fival);
 
 	/* DV Timings IOCTLs */
-	int (*vidioc_enum_dv_presets) (struct file *file, void *fh,
-				       struct v4l2_dv_enum_preset *preset);
-
-	int (*vidioc_s_dv_preset) (struct file *file, void *fh,
-				   struct v4l2_dv_preset *preset);
-	int (*vidioc_g_dv_preset) (struct file *file, void *fh,
-				   struct v4l2_dv_preset *preset);
-	int (*vidioc_query_dv_preset) (struct file *file, void *fh,
-					struct v4l2_dv_preset *qpreset);
 	int (*vidioc_s_dv_timings) (struct file *file, void *fh,
 				    struct v4l2_dv_timings *timings);
 	int (*vidioc_g_dv_timings) (struct file *file, void *fh,
@@ -281,7 +275,7 @@ struct v4l2_ioctl_ops {
 
 	/* For other private ioctls */
 	long (*vidioc_default)	       (struct file *file, void *fh,
-					bool valid_prio, int cmd, void *arg);
+					bool valid_prio, unsigned int cmd, void *arg);
 };
 
 
diff --git a/include/media/v4l2-of.h b/include/media/v4l2-of.h
new file mode 100644
index 00000000000..3a8a84124b4
--- /dev/null
+++ b/include/media/v4l2-of.h
@@ -0,0 +1,111 @@
+/*
+ * V4L2 OF binding parsing library
+ *
+ * Copyright (C) 2012 - 2013 Samsung Electronics Co., Ltd.
+ * Author: Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * Copyright (C) 2012 Renesas Electronics Corp.
+ * Author: Guennadi Liakhovetski <g.liakhovetski@gmx.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ */
+#ifndef _V4L2_OF_H
+#define _V4L2_OF_H
+
+#include <linux/list.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+
+#include <media/v4l2-mediabus.h>
+
+struct device_node;
+
+/**
+ * struct v4l2_of_bus_mipi_csi2 - MIPI CSI-2 bus data structure
+ * @flags: media bus (V4L2_MBUS_*) flags
+ * @data_lanes: an array of physical data lane indexes
+ * @clock_lane: physical lane index of the clock lane
+ * @num_data_lanes: number of data lanes
+ */
+struct v4l2_of_bus_mipi_csi2 {
+	unsigned int flags;
+	unsigned char data_lanes[4];
+	unsigned char clock_lane;
+	unsigned short num_data_lanes;
+};
+
+/**
+ * struct v4l2_of_bus_parallel - parallel data bus data structure
+ * @flags: media bus (V4L2_MBUS_*) flags
+ * @bus_width: bus width in bits
+ * @data_shift: data shift in bits
+ */
+struct v4l2_of_bus_parallel {
+	unsigned int flags;
+	unsigned char bus_width;
+	unsigned char data_shift;
+};
+
+/**
+ * struct v4l2_of_endpoint - the endpoint data structure
+ * @port: identifier (value of reg property) of a port this endpoint belongs to
+ * @id: identifier (value of reg property) of this endpoint
+ * @local_node: pointer to device_node of this endpoint
+ * @remote: phandle to remote endpoint node
+ * @bus_type: bus type
+ * @bus: bus configuration data structure
+ * @head: list head for this structure
+ */
+struct v4l2_of_endpoint {
+	unsigned int port;
+	unsigned int id;
+	const struct device_node *local_node;
+	const __be32 *remote;
+	enum v4l2_mbus_type bus_type;
+	union {
+		struct v4l2_of_bus_parallel parallel;
+		struct v4l2_of_bus_mipi_csi2 mipi_csi2;
+	} bus;
+	struct list_head head;
+};
+
+#ifdef CONFIG_OF
+void v4l2_of_parse_endpoint(const struct device_node *node,
+				struct v4l2_of_endpoint *link);
+struct device_node *v4l2_of_get_next_endpoint(const struct device_node *parent,
+					struct device_node *previous);
+struct device_node *v4l2_of_get_remote_port_parent(
+					const struct device_node *node);
+struct device_node *v4l2_of_get_remote_port(const struct device_node *node);
+#else /* CONFIG_OF */
+
+static inline int v4l2_of_parse_endpoint(const struct device_node *node,
+					struct v4l2_of_endpoint *link)
+{
+	return -ENOSYS;
+}
+
+static inline struct device_node *v4l2_of_get_next_endpoint(
+					const struct device_node *parent,
+					struct device_node *previous)
+{
+	return NULL;
+}
+
+static inline struct device_node *v4l2_of_get_remote_port_parent(
+					const struct device_node *node)
+{
+	return NULL;
+}
+
+static inline struct device_node *v4l2_of_get_remote_port(
+					const struct device_node *node)
+{
+	return NULL;
+}
+
+#endif /* CONFIG_OF */
+
+#endif /* _V4L2_OF_H */
diff --git a/include/media/v4l2-subdev.h b/include/media/v4l2-subdev.h
index b137a5e1151..5298d678d0f 100644
--- a/include/media/v4l2-subdev.h
+++ b/include/media/v4l2-subdev.h
@@ -165,7 +165,7 @@ struct v4l2_subdev_core_ops {
 	long (*ioctl)(struct v4l2_subdev *sd, unsigned int cmd, void *arg);
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 	int (*g_register)(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg);
-	int (*s_register)(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg);
+	int (*s_register)(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg);
 #endif
 	int (*s_power)(struct v4l2_subdev *sd, int on);
 	int (*interrupt_service_routine)(struct v4l2_subdev *sd,
@@ -191,10 +191,10 @@ struct v4l2_subdev_core_ops {
  */
 struct v4l2_subdev_tuner_ops {
 	int (*s_radio)(struct v4l2_subdev *sd);
-	int (*s_frequency)(struct v4l2_subdev *sd, struct v4l2_frequency *freq);
+	int (*s_frequency)(struct v4l2_subdev *sd, const struct v4l2_frequency *freq);
 	int (*g_frequency)(struct v4l2_subdev *sd, struct v4l2_frequency *freq);
 	int (*g_tuner)(struct v4l2_subdev *sd, struct v4l2_tuner *vt);
-	int (*s_tuner)(struct v4l2_subdev *sd, struct v4l2_tuner *vt);
+	int (*s_tuner)(struct v4l2_subdev *sd, const struct v4l2_tuner *vt);
 	int (*g_modulator)(struct v4l2_subdev *sd, struct v4l2_modulator *vm);
 	int (*s_modulator)(struct v4l2_subdev *sd, const struct v4l2_modulator *vm);
 	int (*s_type_addr)(struct v4l2_subdev *sd, struct tuner_setup *type);
@@ -279,14 +279,6 @@ struct v4l2_mbus_frame_desc {
    s_routing: see s_routing in audio_ops, except this version is for video
 	devices.
 
-   s_dv_preset: set dv (Digital Video) preset in the sub device. Similar to
-	s_std()
-
-   g_dv_preset: get current dv (Digital Video) preset in the sub device.
-
-   query_dv_preset: query dv preset in the sub device. This is similar to
-	querystd()
-
    s_dv_timings(): Set custom dv timings in the sub device. This is used
 	when sub device is capable of setting detailed timing information
 	in the hardware to generate/detect the video signal.
@@ -331,14 +323,6 @@ struct v4l2_subdev_video_ops {
 				struct v4l2_subdev_frame_interval *interval);
 	int (*enum_framesizes)(struct v4l2_subdev *sd, struct v4l2_frmsizeenum *fsize);
 	int (*enum_frameintervals)(struct v4l2_subdev *sd, struct v4l2_frmivalenum *fival);
-	int (*enum_dv_presets) (struct v4l2_subdev *sd,
-			struct v4l2_dv_enum_preset *preset);
-	int (*s_dv_preset)(struct v4l2_subdev *sd,
-			struct v4l2_dv_preset *preset);
-	int (*g_dv_preset)(struct v4l2_subdev *sd,
-			struct v4l2_dv_preset *preset);
-	int (*query_dv_preset)(struct v4l2_subdev *sd,
-			struct v4l2_dv_preset *preset);
 	int (*s_dv_timings)(struct v4l2_subdev *sd,
 			struct v4l2_dv_timings *timings);
 	int (*g_dv_timings)(struct v4l2_subdev *sd,
@@ -687,4 +671,7 @@ void v4l2_subdev_init(struct v4l2_subdev *sd,
 	((!(sd) || !(sd)->v4l2_dev || !(sd)->v4l2_dev->notify) ? -ENODEV : \
 	 (sd)->v4l2_dev->notify((sd), (notification), (arg)))
 
+#define v4l2_subdev_has_op(sd, o, f) \
+	((sd)->ops->o && (sd)->ops->o->f)
+
 #endif
diff --git a/include/media/videobuf-dma-contig.h b/include/media/videobuf-dma-contig.h
index f473aeb86d3..f0ed82543d9 100644
--- a/include/media/videobuf-dma-contig.h
+++ b/include/media/videobuf-dma-contig.h
@@ -26,16 +26,6 @@ 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/media/videobuf2-core.h b/include/media/videobuf2-core.h
index 9cfd4ee9e56..d88a098d1af 100644
--- a/include/media/videobuf2-core.h
+++ b/include/media/videobuf2-core.h
@@ -27,7 +27,9 @@ struct vb2_fileio_data;
  *		return NULL on failure or a pointer to allocator private,
  *		per-buffer data on success; the returned private structure
  *		will then be passed as buf_priv argument to other ops in this
- *		structure
+ *		structure. Additional gfp_flags to use when allocating the
+ *		are also passed to this operation. These flags are from the
+ *		gfp_flags field of vb2_queue.
  * @put:	inform the allocator that the buffer will no longer be used;
  *		usually will result in the allocator freeing the buffer (if
  *		no other users of this buffer are present); the buf_priv
@@ -79,7 +81,7 @@ struct vb2_fileio_data;
  *				  unmap_dmabuf.
  */
 struct vb2_mem_ops {
-	void		*(*alloc)(void *alloc_ctx, unsigned long size);
+	void		*(*alloc)(void *alloc_ctx, unsigned long size, gfp_t gfp_flags);
 	void		(*put)(void *buf_priv);
 	struct dma_buf *(*get_dmabuf)(void *buf_priv);
 
@@ -302,6 +304,9 @@ struct v4l2_fh;
  * @buf_struct_size: size of the driver-specific buffer structure;
  *		"0" indicates the driver doesn't want to use a custom buffer
  *		structure type, so sizeof(struct vb2_buffer) will is used
+ * @gfp_flags:	additional gfp flags used when allocating the buffers.
+ *		Typically this is 0, but it may be e.g. GFP_DMA or __GFP_DMA32
+ *		to force the buffer allocation to a specific memory zone.
  *
  * @memory:	current memory type used
  * @bufs:	videobuf buffer structures
@@ -326,6 +331,8 @@ struct vb2_queue {
 	const struct vb2_mem_ops	*mem_ops;
 	void				*drv_priv;
 	unsigned int			buf_struct_size;
+	u32				timestamp_type;
+	gfp_t				gfp_flags;
 
 /* private: internal use only */
 	enum v4l2_memory		memory;
diff --git a/include/net/addrconf.h b/include/net/addrconf.h
index 40be2a0d8ae..84a6440f1f1 100644
--- a/include/net/addrconf.h
+++ b/include/net/addrconf.h
@@ -199,6 +199,7 @@ extern bool ipv6_chk_acast_addr(struct net *net, struct net_device *dev,
 /* Device notifier */
 extern int register_inet6addr_notifier(struct notifier_block *nb);
 extern int unregister_inet6addr_notifier(struct notifier_block *nb);
+extern int inet6addr_notifier_call_chain(unsigned long val, void *v);
 
 extern void inet6_netconf_notify_devconf(struct net *net, int type, int ifindex,
 					 struct ipv6_devconf *devconf);
diff --git a/include/net/flow_keys.h b/include/net/flow_keys.h
index 80461c1ae9e..bb8271d487b 100644
--- a/include/net/flow_keys.h
+++ b/include/net/flow_keys.h
@@ -9,6 +9,7 @@ struct flow_keys {
 		__be32 ports;
 		__be16 port16[2];
 	};
+	u16 thoff;
 	u8 ip_proto;
 };
 
diff --git a/include/net/ip_vs.h b/include/net/ip_vs.h
index 68c69d54d39..fce8e6b66d5 100644
--- a/include/net/ip_vs.h
+++ b/include/net/ip_vs.h
@@ -976,6 +976,7 @@ struct netns_ipvs {
 	int			sysctl_sync_retries;
 	int			sysctl_nat_icmp_send;
 	int			sysctl_pmtu_disc;
+	int			sysctl_backup_only;
 
 	/* ip_vs_lblc */
 	int			sysctl_lblc_expiration;
@@ -1067,6 +1068,12 @@ static inline int sysctl_pmtu_disc(struct netns_ipvs *ipvs)
 	return ipvs->sysctl_pmtu_disc;
 }
 
+static inline int sysctl_backup_only(struct netns_ipvs *ipvs)
+{
+	return ipvs->sync_state & IP_VS_STATE_BACKUP &&
+	       ipvs->sysctl_backup_only;
+}
+
 #else
 
 static inline int sysctl_sync_threshold(struct netns_ipvs *ipvs)
@@ -1114,6 +1121,11 @@ static inline int sysctl_pmtu_disc(struct netns_ipvs *ipvs)
 	return 1;
 }
 
+static inline int sysctl_backup_only(struct netns_ipvs *ipvs)
+{
+	return 0;
+}
+
 #endif
 
 /*
diff --git a/include/net/ipip.h b/include/net/ipip.h
index fd19625ff99..982141c1520 100644
--- a/include/net/ipip.h
+++ b/include/net/ipip.h
@@ -77,15 +77,11 @@ static inline void tunnel_ip_select_ident(struct sk_buff *skb,
 {
 	struct iphdr *iph = ip_hdr(skb);
 
-	if (iph->frag_off & htons(IP_DF))
-		iph->id	= 0;
-	else {
-		/* Use inner packet iph-id if possible. */
-		if (skb->protocol == htons(ETH_P_IP) && old_iph->id)
-			iph->id	= old_iph->id;
-		else
-			__ip_select_ident(iph, dst,
-					  (skb_shinfo(skb)->gso_segs ?: 1) - 1);
-	}
+	/* Use inner packet iph-id if possible. */
+	if (skb->protocol == htons(ETH_P_IP) && old_iph->id)
+		iph->id	= old_iph->id;
+	else
+		__ip_select_ident(iph, dst,
+				  (skb_shinfo(skb)->gso_segs ?: 1) - 1);
 }
 #endif
diff --git a/include/net/irda/irlmp.h b/include/net/irda/irlmp.h
index f74109144d3..f132924cc9d 100644
--- a/include/net/irda/irlmp.h
+++ b/include/net/irda/irlmp.h
@@ -256,7 +256,8 @@ static inline __u32 irlmp_get_daddr(const struct lsap_cb *self)
 	return (self && self->lap) ? self->lap->daddr : 0;
 }
 
-extern const char *irlmp_reasons[];
+const char *irlmp_reason_str(LM_REASON reason);
+
 extern int sysctl_discovery_timeout;
 extern int sysctl_discovery_slots;
 extern int sysctl_discovery;
diff --git a/include/net/iucv/af_iucv.h b/include/net/iucv/af_iucv.h
index cc7c1973238..714cc9a54a4 100644
--- a/include/net/iucv/af_iucv.h
+++ b/include/net/iucv/af_iucv.h
@@ -130,6 +130,14 @@ struct iucv_sock {
 					       enum iucv_tx_notify n);
 };
 
+struct iucv_skb_cb {
+	u32	class;		/* target class of message */
+	u32	tag;		/* tag associated with message */
+	u32	offset;		/* offset for skb receival */
+};
+
+#define IUCV_SKB_CB(__skb)	((struct iucv_skb_cb *)&((__skb)->cb[0]))
+
 /* iucv socket options (SOL_IUCV) */
 #define SO_IPRMDATA_MSG	0x0080		/* send/recv IPRM_DATA msgs */
 #define SO_MSGLIMIT	0x1000		/* get/set IUCV MSGLIMIT */
diff --git a/include/net/scm.h b/include/net/scm.h
index 975cca01048..b1170810568 100644
--- a/include/net/scm.h
+++ b/include/net/scm.h
@@ -56,8 +56,8 @@ static __inline__ void scm_set_cred(struct scm_cookie *scm,
 	scm->pid  = get_pid(pid);
 	scm->cred = cred ? get_cred(cred) : NULL;
 	scm->creds.pid = pid_vnr(pid);
-	scm->creds.uid = cred ? cred->euid : INVALID_UID;
-	scm->creds.gid = cred ? cred->egid : INVALID_GID;
+	scm->creds.uid = cred ? cred->uid : INVALID_UID;
+	scm->creds.gid = cred ? cred->gid : INVALID_GID;
 }
 
 static __inline__ void scm_destroy_cred(struct scm_cookie *scm)
diff --git a/include/net/sctp/sctp.h b/include/net/sctp/sctp.h
index df85a0c0f2d..cd89510eab2 100644
--- a/include/net/sctp/sctp.h
+++ b/include/net/sctp/sctp.h
@@ -576,7 +576,7 @@ for (pos = chunk->subh.fwdtsn_hdr->skip;\
 #define WORD_ROUND(s) (((s)+3)&~3)
 
 /* Make a new instance of type.  */
-#define t_new(type, flags)	(type *)kzalloc(sizeof(type), flags)
+#define t_new(type, flags)	kzalloc(sizeof(type), flags)
 
 /* Compare two timevals.  */
 #define tv_lt(s, t) \
diff --git a/include/pcmcia/ds.h b/include/pcmcia/ds.h
index 3bbbd78e143..2d56e428506 100644
--- a/include/pcmcia/ds.h
+++ b/include/pcmcia/ds.h
@@ -65,6 +65,18 @@ struct pcmcia_driver {
 int pcmcia_register_driver(struct pcmcia_driver *driver);
 void pcmcia_unregister_driver(struct pcmcia_driver *driver);
 
+/**
+ * module_pcmcia_driver() - Helper macro for registering a pcmcia driver
+ * @__pcmcia_driver: pcmcia_driver struct
+ *
+ * Helper macro for pcmcia 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_pcmcia_driver(__pcmcia_driver) \
+	module_driver(__pcmcia_driver, pcmcia_register_driver, \
+			pcmcia_unregister_driver)
+
 /* for struct resource * array embedded in struct pcmcia_device */
 enum {
 	PCMCIA_IOPORT_0,
diff --git a/include/scsi/iscsi_if.h b/include/scsi/iscsi_if.h
index 917741bb8e1..fe7f06c86f6 100644
--- a/include/scsi/iscsi_if.h
+++ b/include/scsi/iscsi_if.h
@@ -63,6 +63,12 @@ enum iscsi_uevent_e {
 	ISCSI_UEVENT_PING		= UEVENT_BASE + 22,
 	ISCSI_UEVENT_GET_CHAP		= UEVENT_BASE + 23,
 	ISCSI_UEVENT_DELETE_CHAP	= UEVENT_BASE + 24,
+	ISCSI_UEVENT_SET_FLASHNODE_PARAMS	= UEVENT_BASE + 25,
+	ISCSI_UEVENT_NEW_FLASHNODE	= UEVENT_BASE + 26,
+	ISCSI_UEVENT_DEL_FLASHNODE	= UEVENT_BASE + 27,
+	ISCSI_UEVENT_LOGIN_FLASHNODE	= UEVENT_BASE + 28,
+	ISCSI_UEVENT_LOGOUT_FLASHNODE	= UEVENT_BASE + 29,
+	ISCSI_UEVENT_LOGOUT_FLASHNODE_SID	= UEVENT_BASE + 30,
 
 	/* up events */
 	ISCSI_KEVENT_RECV_PDU		= KEVENT_BASE + 1,
@@ -210,6 +216,31 @@ struct iscsi_uevent {
 		       uint32_t        host_no;
 		       uint16_t        chap_tbl_idx;
 		} delete_chap;
+		struct msg_set_flashnode_param {
+			uint32_t	host_no;
+			uint32_t	flashnode_idx;
+			uint32_t	count;
+		} set_flashnode;
+		struct msg_new_flashnode {
+			uint32_t	host_no;
+			uint32_t	len;
+		} new_flashnode;
+		struct msg_del_flashnode {
+			uint32_t	host_no;
+			uint32_t	flashnode_idx;
+		} del_flashnode;
+		struct msg_login_flashnode {
+			uint32_t	host_no;
+			uint32_t	flashnode_idx;
+		} login_flashnode;
+		struct msg_logout_flashnode {
+			uint32_t	host_no;
+			uint32_t	flashnode_idx;
+		} logout_flashnode;
+		struct msg_logout_flashnode_sid {
+			uint32_t	host_no;
+			uint32_t	sid;
+		} logout_flashnode_sid;
 	} u;
 	union {
 		/* messages k -> u */
@@ -267,6 +298,9 @@ struct iscsi_uevent {
 						   with each ping request */
 			uint32_t        data_size;
 		} ping_comp;
+		struct msg_new_flashnode_ret {
+			uint32_t	flashnode_idx;
+		} new_flashnode_ret;
 	} r;
 } __attribute__ ((aligned (sizeof(uint64_t))));
 
@@ -274,6 +308,7 @@ enum iscsi_param_type {
 	ISCSI_PARAM,		/* iscsi_param (session, conn, target, LU) */
 	ISCSI_HOST_PARAM,	/* iscsi_host_param */
 	ISCSI_NET_PARAM,	/* iscsi_net_param */
+	ISCSI_FLASHNODE_PARAM,	/* iscsi_flashnode_param */
 };
 
 struct iscsi_iface_param_info {
@@ -469,6 +504,88 @@ enum iscsi_host_param {
 	ISCSI_HOST_PARAM_MAX,
 };
 
+/* portal type */
+#define PORTAL_TYPE_IPV4	"ipv4"
+#define PORTAL_TYPE_IPV6	"ipv6"
+
+/* iSCSI Flash Target params */
+enum iscsi_flashnode_param {
+	ISCSI_FLASHNODE_IS_FW_ASSIGNED_IPV6,
+	ISCSI_FLASHNODE_PORTAL_TYPE,
+	ISCSI_FLASHNODE_AUTO_SND_TGT_DISABLE,
+	ISCSI_FLASHNODE_DISCOVERY_SESS,
+	ISCSI_FLASHNODE_ENTRY_EN,
+	ISCSI_FLASHNODE_HDR_DGST_EN,
+	ISCSI_FLASHNODE_DATA_DGST_EN,
+	ISCSI_FLASHNODE_IMM_DATA_EN,
+	ISCSI_FLASHNODE_INITIAL_R2T_EN,
+	ISCSI_FLASHNODE_DATASEQ_INORDER,
+	ISCSI_FLASHNODE_PDU_INORDER,
+	ISCSI_FLASHNODE_CHAP_AUTH_EN,
+	ISCSI_FLASHNODE_SNACK_REQ_EN,
+	ISCSI_FLASHNODE_DISCOVERY_LOGOUT_EN,
+	ISCSI_FLASHNODE_BIDI_CHAP_EN,
+	/* make authentication for discovery sessions optional */
+	ISCSI_FLASHNODE_DISCOVERY_AUTH_OPTIONAL,
+	ISCSI_FLASHNODE_ERL,
+	ISCSI_FLASHNODE_TCP_TIMESTAMP_STAT,
+	ISCSI_FLASHNODE_TCP_NAGLE_DISABLE,
+	ISCSI_FLASHNODE_TCP_WSF_DISABLE,
+	ISCSI_FLASHNODE_TCP_TIMER_SCALE,
+	ISCSI_FLASHNODE_TCP_TIMESTAMP_EN,
+	ISCSI_FLASHNODE_IP_FRAG_DISABLE,
+	ISCSI_FLASHNODE_MAX_RECV_DLENGTH,
+	ISCSI_FLASHNODE_MAX_XMIT_DLENGTH,
+	ISCSI_FLASHNODE_FIRST_BURST,
+	ISCSI_FLASHNODE_DEF_TIME2WAIT,
+	ISCSI_FLASHNODE_DEF_TIME2RETAIN,
+	ISCSI_FLASHNODE_MAX_R2T,
+	ISCSI_FLASHNODE_KEEPALIVE_TMO,
+	ISCSI_FLASHNODE_ISID,
+	ISCSI_FLASHNODE_TSID,
+	ISCSI_FLASHNODE_PORT,
+	ISCSI_FLASHNODE_MAX_BURST,
+	ISCSI_FLASHNODE_DEF_TASKMGMT_TMO,
+	ISCSI_FLASHNODE_IPADDR,
+	ISCSI_FLASHNODE_ALIAS,
+	ISCSI_FLASHNODE_REDIRECT_IPADDR,
+	ISCSI_FLASHNODE_MAX_SEGMENT_SIZE,
+	ISCSI_FLASHNODE_LOCAL_PORT,
+	ISCSI_FLASHNODE_IPV4_TOS,
+	ISCSI_FLASHNODE_IPV6_TC,
+	ISCSI_FLASHNODE_IPV6_FLOW_LABEL,
+	ISCSI_FLASHNODE_NAME,
+	ISCSI_FLASHNODE_TPGT,
+	ISCSI_FLASHNODE_LINK_LOCAL_IPV6,
+	ISCSI_FLASHNODE_DISCOVERY_PARENT_IDX,
+	ISCSI_FLASHNODE_DISCOVERY_PARENT_TYPE,
+	ISCSI_FLASHNODE_TCP_XMIT_WSF,
+	ISCSI_FLASHNODE_TCP_RECV_WSF,
+	ISCSI_FLASHNODE_CHAP_IN_IDX,
+	ISCSI_FLASHNODE_CHAP_OUT_IDX,
+	ISCSI_FLASHNODE_USERNAME,
+	ISCSI_FLASHNODE_USERNAME_IN,
+	ISCSI_FLASHNODE_PASSWORD,
+	ISCSI_FLASHNODE_PASSWORD_IN,
+	ISCSI_FLASHNODE_STATSN,
+	ISCSI_FLASHNODE_EXP_STATSN,
+	ISCSI_FLASHNODE_IS_BOOT_TGT,
+
+	ISCSI_FLASHNODE_MAX,
+};
+
+struct iscsi_flashnode_param_info {
+	uint32_t len;		/* Actual length of the param */
+	uint16_t param;		/* iscsi param value */
+	uint8_t value[0];	/* length sized value follows */
+} __packed;
+
+enum iscsi_discovery_parent_type {
+	ISCSI_DISC_PARENT_UNKNOWN	= 0x1,
+	ISCSI_DISC_PARENT_SENDTGT	= 0x2,
+	ISCSI_DISC_PARENT_ISNS		= 0x3,
+};
+
 /* iSCSI port Speed */
 enum iscsi_port_speed {
 	ISCSI_PORT_SPEED_UNKNOWN	= 0x1,
diff --git a/include/scsi/libfc.h b/include/scsi/libfc.h
index 399162b50a8..e1379b4e8fa 100644
--- a/include/scsi/libfc.h
+++ b/include/scsi/libfc.h
@@ -1074,7 +1074,8 @@ void fc_rport_terminate_io(struct fc_rport *);
 /*
  * DISCOVERY LAYER
  *****************************/
-int fc_disc_init(struct fc_lport *);
+void fc_disc_init(struct fc_lport *);
+void fc_disc_config(struct fc_lport *, void *);
 
 static inline struct fc_lport *fc_disc_lport(struct fc_disc *disc)
 {
diff --git a/include/scsi/libiscsi.h b/include/scsi/libiscsi.h
index 6e33386a389..09c041e6c35 100644
--- a/include/scsi/libiscsi.h
+++ b/include/scsi/libiscsi.h
@@ -427,6 +427,7 @@ extern void iscsi_complete_scsi_task(struct iscsi_task *task,
  */
 extern void iscsi_pool_free(struct iscsi_pool *);
 extern int iscsi_pool_init(struct iscsi_pool *, int, void ***, int);
+extern int iscsi_switch_str_param(char **, char *);
 
 /*
  * inline functions to deal with padding.
diff --git a/include/scsi/scsi_transport_iscsi.h b/include/scsi/scsi_transport_iscsi.h
index 53f0b361d66..4a58cca2ecc 100644
--- a/include/scsi/scsi_transport_iscsi.h
+++ b/include/scsi/scsi_transport_iscsi.h
@@ -39,6 +39,8 @@ struct iscsi_task;
 struct sockaddr;
 struct iscsi_iface;
 struct bsg_job;
+struct iscsi_bus_flash_session;
+struct iscsi_bus_flash_conn;
 
 /**
  * struct iscsi_transport - iSCSI Transport template
@@ -150,6 +152,19 @@ struct iscsi_transport {
 	int (*get_chap) (struct Scsi_Host *shost, uint16_t chap_tbl_idx,
 			 uint32_t *num_entries, char *buf);
 	int (*delete_chap) (struct Scsi_Host *shost, uint16_t chap_tbl_idx);
+	int (*get_flashnode_param) (struct iscsi_bus_flash_session *fnode_sess,
+				    int param, char *buf);
+	int (*set_flashnode_param) (struct iscsi_bus_flash_session *fnode_sess,
+				    struct iscsi_bus_flash_conn *fnode_conn,
+				    void *data, int len);
+	int (*new_flashnode) (struct Scsi_Host *shost, const char *buf,
+			      int len);
+	int (*del_flashnode) (struct iscsi_bus_flash_session *fnode_sess);
+	int (*login_flashnode) (struct iscsi_bus_flash_session *fnode_sess,
+				struct iscsi_bus_flash_conn *fnode_conn);
+	int (*logout_flashnode) (struct iscsi_bus_flash_session *fnode_sess,
+				 struct iscsi_bus_flash_conn *fnode_conn);
+	int (*logout_flashnode_sid) (struct iscsi_cls_session *cls_sess);
 };
 
 /*
@@ -286,6 +301,112 @@ struct iscsi_iface {
 #define iscsi_iface_to_shost(_iface) \
 	dev_to_shost(_iface->dev.parent)
 
+
+struct iscsi_bus_flash_conn {
+	struct list_head conn_list;	/* item in connlist */
+	void *dd_data;			/* LLD private data */
+	struct iscsi_transport *transport;
+	struct device dev;		/* sysfs transport/container device */
+	/* iscsi connection parameters */
+	uint32_t		exp_statsn;
+	uint32_t		statsn;
+	unsigned		max_recv_dlength; /* initiator_max_recv_dsl*/
+	unsigned		max_xmit_dlength; /* target_max_recv_dsl */
+	unsigned		max_segment_size;
+	unsigned		tcp_xmit_wsf;
+	unsigned		tcp_recv_wsf;
+	int			hdrdgst_en;
+	int			datadgst_en;
+	int			port;
+	char			*ipaddress;
+	char			*link_local_ipv6_addr;
+	char			*redirect_ipaddr;
+	uint16_t		keepalive_timeout;
+	uint16_t		local_port;
+	uint8_t			snack_req_en;
+	/* tcp timestamp negotiation status */
+	uint8_t			tcp_timestamp_stat;
+	uint8_t			tcp_nagle_disable;
+	/* tcp window scale factor */
+	uint8_t			tcp_wsf_disable;
+	uint8_t			tcp_timer_scale;
+	uint8_t			tcp_timestamp_en;
+	uint8_t			ipv4_tos;
+	uint8_t			ipv6_traffic_class;
+	uint8_t			ipv6_flow_label;
+	uint8_t			fragment_disable;
+	/* Link local IPv6 address is assigned by firmware or driver */
+	uint8_t			is_fw_assigned_ipv6;
+};
+
+#define iscsi_dev_to_flash_conn(_dev) \
+	container_of(_dev, struct iscsi_bus_flash_conn, dev)
+
+#define iscsi_flash_conn_to_flash_session(_conn) \
+	iscsi_dev_to_flash_session(_conn->dev.parent)
+
+#define ISID_SIZE 6
+
+struct iscsi_bus_flash_session {
+	struct list_head sess_list;		/* item in session_list */
+	struct iscsi_transport *transport;
+	unsigned int target_id;
+	int flash_state;	/* persistent or non-persistent */
+	void *dd_data;				/* LLD private data */
+	struct device dev;	/* sysfs transport/container device */
+	/* iscsi session parameters */
+	unsigned		first_burst;
+	unsigned		max_burst;
+	unsigned short		max_r2t;
+	int			default_taskmgmt_timeout;
+	int			initial_r2t_en;
+	int			imm_data_en;
+	int			time2wait;
+	int			time2retain;
+	int			pdu_inorder_en;
+	int			dataseq_inorder_en;
+	int			erl;
+	int			tpgt;
+	char			*username;
+	char			*username_in;
+	char			*password;
+	char			*password_in;
+	char			*targetname;
+	char			*targetalias;
+	char			*portal_type;
+	uint16_t		tsid;
+	uint16_t		chap_in_idx;
+	uint16_t		chap_out_idx;
+	/* index of iSCSI discovery session if the entry is
+	 * discovered by iSCSI discovery session
+	 */
+	uint16_t		discovery_parent_idx;
+	/* indicates if discovery was done through iSNS discovery service
+	 * or through sendTarget */
+	uint16_t		discovery_parent_type;
+	/* Firmware auto sendtarget discovery disable */
+	uint8_t			auto_snd_tgt_disable;
+	uint8_t			discovery_sess;
+	/* indicates if this flashnode entry is enabled or disabled */
+	uint8_t			entry_state;
+	uint8_t			chap_auth_en;
+	/* enables firmware to auto logout the discovery session on discovery
+	 * completion
+	 */
+	uint8_t			discovery_logout_en;
+	uint8_t			bidi_chap_en;
+	/* makes authentication for discovery session optional */
+	uint8_t			discovery_auth_optional;
+	uint8_t			isid[ISID_SIZE];
+	uint8_t			is_boot_target;
+};
+
+#define iscsi_dev_to_flash_session(_dev) \
+	container_of(_dev, struct iscsi_bus_flash_session, dev)
+
+#define iscsi_flash_session_to_shost(_session) \
+	dev_to_shost(_session->dev.parent)
+
 /*
  * session and connection functions that can be used by HW iSCSI LLDs
  */
@@ -330,4 +451,34 @@ extern char *iscsi_get_port_speed_name(struct Scsi_Host *shost);
 extern char *iscsi_get_port_state_name(struct Scsi_Host *shost);
 extern int iscsi_is_session_dev(const struct device *dev);
 
+extern char *iscsi_get_discovery_parent_name(int parent_type);
+extern struct device *
+iscsi_find_flashnode(struct Scsi_Host *shost, void *data,
+		     int (*fn)(struct device *dev, void *data));
+
+extern struct iscsi_bus_flash_session *
+iscsi_create_flashnode_sess(struct Scsi_Host *shost, int index,
+			    struct iscsi_transport *transport, int dd_size);
+
+extern struct iscsi_bus_flash_conn *
+iscsi_create_flashnode_conn(struct Scsi_Host *shost,
+			    struct iscsi_bus_flash_session *fnode_sess,
+			    struct iscsi_transport *transport, int dd_size);
+
+extern void
+iscsi_destroy_flashnode_sess(struct iscsi_bus_flash_session *fnode_sess);
+
+extern void iscsi_destroy_all_flashnode(struct Scsi_Host *shost);
+extern int iscsi_flashnode_bus_match(struct device *dev,
+				     struct device_driver *drv);
+extern int iscsi_is_flashnode_conn_dev(struct device *dev, void *data);
+
+extern struct device *
+iscsi_find_flashnode_sess(struct Scsi_Host *shost, void *data,
+			  int (*fn)(struct device *dev, void *data));
+
+extern struct device *
+iscsi_find_flashnode_conn(struct iscsi_bus_flash_session *fnode_sess,
+			  void *data,
+			  int (*fn)(struct device *dev, void *data));
 #endif
diff --git a/include/sound/max98090.h b/include/sound/max98090.h
index 95efb13f847..95efb13f847 100755..100644
--- a/include/sound/max98090.h
+++ b/include/sound/max98090.h
diff --git a/include/sound/pcm.h b/include/sound/pcm.h
index 45c1981c9ca..5ec42dbd230 100644
--- a/include/sound/pcm.h
+++ b/include/sound/pcm.h
@@ -867,7 +867,7 @@ int snd_pcm_format_physical_width(snd_pcm_format_t format);		/* in bits */
 ssize_t snd_pcm_format_size(snd_pcm_format_t format, size_t samples);
 const unsigned char *snd_pcm_format_silence_64(snd_pcm_format_t format);
 int snd_pcm_format_set_silence(snd_pcm_format_t format, void *buf, unsigned int frames);
-snd_pcm_format_t snd_pcm_build_linear_format(int width, int unsignd, int big_endian);
+snd_pcm_format_t snd_pcm_build_linear_format(int width, int unsigned, int big_endian);
 
 void snd_pcm_set_ops(struct snd_pcm * pcm, int direction, struct snd_pcm_ops *ops);
 void snd_pcm_set_sync(struct snd_pcm_substream *substream);
diff --git a/include/sound/soc-dapm.h b/include/sound/soc-dapm.h
index e1ef63d4a5c..44a30b10868 100644
--- a/include/sound/soc-dapm.h
+++ b/include/sound/soc-dapm.h
@@ -488,6 +488,7 @@ struct snd_soc_dapm_path {
 	/* status */
 	u32 connect:1;	/* source and sink widgets are connected */
 	u32 walked:1;	/* path has been walked */
+	u32 walking:1;  /* path is in the process of being walked */
 	u32 weak:1;	/* path ignored for power management */
 
 	int (*connected)(struct snd_soc_dapm_widget *source,
diff --git a/include/target/iscsi/iscsi_transport.h b/include/target/iscsi/iscsi_transport.h
new file mode 100644
index 00000000000..23a87d0cd72
--- /dev/null
+++ b/include/target/iscsi/iscsi_transport.h
@@ -0,0 +1,83 @@
+#include <linux/module.h>
+#include <linux/list.h>
+#include "../../../drivers/target/iscsi/iscsi_target_core.h"
+
+struct iscsit_transport {
+#define ISCSIT_TRANSPORT_NAME	16
+	char name[ISCSIT_TRANSPORT_NAME];
+	int transport_type;
+	struct module *owner;
+	struct list_head t_node;
+	int (*iscsit_setup_np)(struct iscsi_np *, struct __kernel_sockaddr_storage *);
+	int (*iscsit_accept_np)(struct iscsi_np *, struct iscsi_conn *);
+	void (*iscsit_free_np)(struct iscsi_np *);
+	void (*iscsit_free_conn)(struct iscsi_conn *);
+	struct iscsi_cmd *(*iscsit_alloc_cmd)(struct iscsi_conn *, gfp_t);
+	int (*iscsit_get_login_rx)(struct iscsi_conn *, struct iscsi_login *);
+	int (*iscsit_put_login_tx)(struct iscsi_conn *, struct iscsi_login *, u32);
+	int (*iscsit_immediate_queue)(struct iscsi_conn *, struct iscsi_cmd *, int);
+	int (*iscsit_response_queue)(struct iscsi_conn *, struct iscsi_cmd *, int);
+	int (*iscsit_get_dataout)(struct iscsi_conn *, struct iscsi_cmd *, bool);
+	int (*iscsit_queue_data_in)(struct iscsi_conn *, struct iscsi_cmd *);
+	int (*iscsit_queue_status)(struct iscsi_conn *, struct iscsi_cmd *);
+};
+
+/*
+ * From iscsi_target_transport.c
+ */
+
+extern int iscsit_register_transport(struct iscsit_transport *);
+extern void iscsit_unregister_transport(struct iscsit_transport *);
+extern struct iscsit_transport *iscsit_get_transport(int);
+extern void iscsit_put_transport(struct iscsit_transport *);
+
+/*
+ * From iscsi_target.c
+ */
+extern int iscsit_add_reject_from_cmd(u8, int, int, unsigned char *,
+				struct iscsi_cmd *);
+extern int iscsit_setup_scsi_cmd(struct iscsi_conn *, struct iscsi_cmd *,
+				unsigned char *);
+extern void iscsit_set_unsoliticed_dataout(struct iscsi_cmd *);
+extern int iscsit_process_scsi_cmd(struct iscsi_conn *, struct iscsi_cmd *,
+				struct iscsi_scsi_req *);
+extern int iscsit_check_dataout_hdr(struct iscsi_conn *, unsigned char *,
+				struct iscsi_cmd **);
+extern int iscsit_check_dataout_payload(struct iscsi_cmd *, struct iscsi_data *,
+				bool);
+extern int iscsit_handle_nop_out(struct iscsi_conn *, struct iscsi_cmd *,
+				unsigned char *);
+extern int iscsit_handle_logout_cmd(struct iscsi_conn *, struct iscsi_cmd *,
+				unsigned char *);
+extern int iscsit_handle_task_mgt_cmd(struct iscsi_conn *, struct iscsi_cmd *,
+				unsigned char *);
+extern void iscsit_build_rsp_pdu(struct iscsi_cmd *, struct iscsi_conn *,
+				bool, struct iscsi_scsi_rsp *);
+extern void iscsit_build_nopin_rsp(struct iscsi_cmd *, struct iscsi_conn *,
+				struct iscsi_nopin *, bool);
+extern void iscsit_build_task_mgt_rsp(struct iscsi_cmd *, struct iscsi_conn *,
+				struct iscsi_tm_rsp *);
+extern void iscsit_build_reject(struct iscsi_cmd *, struct iscsi_conn *,
+				struct iscsi_reject *);
+extern int iscsit_build_logout_rsp(struct iscsi_cmd *, struct iscsi_conn *,
+				struct iscsi_logout_rsp *);
+extern int iscsit_logout_post_handler(struct iscsi_cmd *, struct iscsi_conn *);
+/*
+ * From iscsi_target_device.c
+ */
+extern void iscsit_increment_maxcmdsn(struct iscsi_cmd *, struct iscsi_session *);
+/*
+ * From iscsi_target_erl1.c
+ */
+extern void iscsit_stop_dataout_timer(struct iscsi_cmd *);
+
+/*
+ * From iscsi_target_tmr.c
+ */
+extern int iscsit_tmr_post_handler(struct iscsi_cmd *, struct iscsi_conn *);
+
+/*
+ * From iscsi_target_util.c
+ */
+extern struct iscsi_cmd *iscsit_allocate_cmd(struct iscsi_conn *, gfp_t);
+extern int iscsit_sequence_cmd(struct iscsi_conn *, struct iscsi_cmd *, __be32);
diff --git a/include/target/target_core_backend.h b/include/target/target_core_backend.h
index b128c20770b..ffa2696d64d 100644
--- a/include/target/target_core_backend.h
+++ b/include/target/target_core_backend.h
@@ -60,6 +60,10 @@ sense_reason_t	sbc_parse_cdb(struct se_cmd *cmd, struct sbc_ops *ops);
 u32	sbc_get_device_rev(struct se_device *dev);
 u32	sbc_get_device_type(struct se_device *dev);
 sector_t	sbc_get_write_same_sectors(struct se_cmd *cmd);
+sense_reason_t sbc_execute_unmap(struct se_cmd *cmd,
+	sense_reason_t (*do_unmap_fn)(struct se_cmd *cmd, void *priv,
+				      sector_t lba, sector_t nolb),
+	void *priv);
 
 void	transport_set_vpd_proto_id(struct t10_vpd *, unsigned char *);
 int	transport_set_vpd_assoc(struct t10_vpd *, unsigned char *);
diff --git a/include/target/target_core_fabric.h b/include/target/target_core_fabric.h
index aaa1ee6ab39..ba3471b73c0 100644
--- a/include/target/target_core_fabric.h
+++ b/include/target/target_core_fabric.h
@@ -120,7 +120,7 @@ bool	transport_wait_for_tasks(struct se_cmd *);
 int	transport_check_aborted_status(struct se_cmd *, int);
 int	transport_send_check_condition_and_sense(struct se_cmd *,
 		sense_reason_t, int);
-
+int	target_get_sess_cmd(struct se_session *, struct se_cmd *, bool);
 int	target_put_sess_cmd(struct se_session *, struct se_cmd *);
 void	target_sess_cmd_list_set_waiting(struct se_session *);
 void	target_wait_for_sess_cmds(struct se_session *, int);
diff --git a/include/trace/events/block.h b/include/trace/events/block.h
index 9961726523d..9c1467357b0 100644
--- a/include/trace/events/block.h
+++ b/include/trace/events/block.h
@@ -257,6 +257,7 @@ TRACE_EVENT(block_bio_bounce,
 
 /**
  * block_bio_complete - completed all work on the block operation
+ * @q: queue holding the block operation
  * @bio: block operation completed
  * @error: io error value
  *
@@ -265,9 +266,9 @@ TRACE_EVENT(block_bio_bounce,
  */
 TRACE_EVENT(block_bio_complete,
 
-	TP_PROTO(struct bio *bio, int error),
+	TP_PROTO(struct request_queue *q, struct bio *bio, int error),
 
-	TP_ARGS(bio, error),
+	TP_ARGS(q, bio, error),
 
 	TP_STRUCT__entry(
 		__field( dev_t,		dev		)
@@ -278,8 +279,7 @@ TRACE_EVENT(block_bio_complete,
 	),
 
 	TP_fast_assign(
-		__entry->dev		= bio->bi_bdev ?
-					  bio->bi_bdev->bd_dev : 0;
+		__entry->dev		= bio->bi_bdev->bd_dev;
 		__entry->sector		= bio->bi_sector;
 		__entry->nr_sector	= bio->bi_size >> 9;
 		__entry->error		= error;
diff --git a/include/trace/events/filemap.h b/include/trace/events/filemap.h
new file mode 100644
index 00000000000..0421f49a20f
--- /dev/null
+++ b/include/trace/events/filemap.h
@@ -0,0 +1,58 @@
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM filemap
+
+#if !defined(_TRACE_FILEMAP_H) || defined(TRACE_HEADER_MULTI_READ)
+#define _TRACE_FILEMAP_H
+
+#include <linux/types.h>
+#include <linux/tracepoint.h>
+#include <linux/mm.h>
+#include <linux/memcontrol.h>
+#include <linux/device.h>
+#include <linux/kdev_t.h>
+
+DECLARE_EVENT_CLASS(mm_filemap_op_page_cache,
+
+	TP_PROTO(struct page *page),
+
+	TP_ARGS(page),
+
+	TP_STRUCT__entry(
+		__field(struct page *, page)
+		__field(unsigned long, i_ino)
+		__field(unsigned long, index)
+		__field(dev_t, s_dev)
+	),
+
+	TP_fast_assign(
+		__entry->page = page;
+		__entry->i_ino = page->mapping->host->i_ino;
+		__entry->index = page->index;
+		if (page->mapping->host->i_sb)
+			__entry->s_dev = page->mapping->host->i_sb->s_dev;
+		else
+			__entry->s_dev = page->mapping->host->i_rdev;
+	),
+
+	TP_printk("dev %d:%d ino %lx page=%p pfn=%lu ofs=%lu",
+		MAJOR(__entry->s_dev), MINOR(__entry->s_dev),
+		__entry->i_ino,
+		__entry->page,
+		page_to_pfn(__entry->page),
+		__entry->index << PAGE_SHIFT)
+);
+
+DEFINE_EVENT(mm_filemap_op_page_cache, mm_filemap_delete_from_page_cache,
+	TP_PROTO(struct page *page),
+	TP_ARGS(page)
+	);
+
+DEFINE_EVENT(mm_filemap_op_page_cache, mm_filemap_add_to_page_cache,
+	TP_PROTO(struct page *page),
+	TP_ARGS(page)
+	);
+
+#endif /* _TRACE_FILEMAP_H */
+
+/* This part must be outside protection */
+#include <trace/define_trace.h>
diff --git a/include/trace/events/printk.h b/include/trace/events/printk.h
index 94ec79cc011..c008bc99f9f 100644
--- a/include/trace/events/printk.h
+++ b/include/trace/events/printk.h
@@ -6,31 +6,18 @@
 
 #include <linux/tracepoint.h>
 
-TRACE_EVENT_CONDITION(console,
-	TP_PROTO(const char *log_buf, unsigned start, unsigned end,
-		 unsigned log_buf_len),
+TRACE_EVENT(console,
+	TP_PROTO(const char *text, size_t len),
 
-	TP_ARGS(log_buf, start, end, log_buf_len),
-
-	TP_CONDITION(start != end),
+	TP_ARGS(text, len),
 
 	TP_STRUCT__entry(
-		__dynamic_array(char, msg, end - start + 1)
+		__dynamic_array(char, msg, len + 1)
 	),
 
 	TP_fast_assign(
-		if ((start & (log_buf_len - 1)) > (end & (log_buf_len - 1))) {
-			memcpy(__get_dynamic_array(msg),
-			       log_buf + (start & (log_buf_len - 1)),
-			       log_buf_len - (start & (log_buf_len - 1)));
-			memcpy((char *)__get_dynamic_array(msg) +
-			       log_buf_len - (start & (log_buf_len - 1)),
-			       log_buf, end & (log_buf_len - 1));
-		} else
-			memcpy(__get_dynamic_array(msg),
-			       log_buf + (start & (log_buf_len - 1)),
-			       end - start);
-		((char *)__get_dynamic_array(msg))[end - start] = 0;
+		memcpy(__get_dynamic_array(msg), text, len);
+		((char *)__get_dynamic_array(msg))[len] = 0;
 	),
 
 	TP_printk("%s", __get_str(msg))
diff --git a/include/trace/events/rcu.h b/include/trace/events/rcu.h
index 1918e832da4..59ebcc89f14 100644
--- a/include/trace/events/rcu.h
+++ b/include/trace/events/rcu.h
@@ -72,6 +72,58 @@ TRACE_EVENT(rcu_grace_period,
 );
 
 /*
+ * Tracepoint for future grace-period events, including those for no-callbacks
+ * CPUs.  The caller should pull the data from the rcu_node structure,
+ * other than rcuname, which comes from the rcu_state structure, and event,
+ * which is one of the following:
+ *
+ * "Startleaf": Request a nocb grace period based on leaf-node data.
+ * "Startedleaf": Leaf-node start proved sufficient.
+ * "Startedleafroot": Leaf-node start proved sufficient after checking root.
+ * "Startedroot": Requested a nocb grace period based on root-node data.
+ * "StartWait": Start waiting for the requested grace period.
+ * "ResumeWait": Resume waiting after signal.
+ * "EndWait": Complete wait.
+ * "Cleanup": Clean up rcu_node structure after previous GP.
+ * "CleanupMore": Clean up, and another no-CB GP is needed.
+ */
+TRACE_EVENT(rcu_future_grace_period,
+
+	TP_PROTO(char *rcuname, unsigned long gpnum, unsigned long completed,
+		 unsigned long c, u8 level, int grplo, int grphi,
+		 char *gpevent),
+
+	TP_ARGS(rcuname, gpnum, completed, c, level, grplo, grphi, gpevent),
+
+	TP_STRUCT__entry(
+		__field(char *, rcuname)
+		__field(unsigned long, gpnum)
+		__field(unsigned long, completed)
+		__field(unsigned long, c)
+		__field(u8, level)
+		__field(int, grplo)
+		__field(int, grphi)
+		__field(char *, gpevent)
+	),
+
+	TP_fast_assign(
+		__entry->rcuname = rcuname;
+		__entry->gpnum = gpnum;
+		__entry->completed = completed;
+		__entry->c = c;
+		__entry->level = level;
+		__entry->grplo = grplo;
+		__entry->grphi = grphi;
+		__entry->gpevent = gpevent;
+	),
+
+	TP_printk("%s %lu %lu %lu %u %d %d %s",
+		  __entry->rcuname, __entry->gpnum, __entry->completed,
+		  __entry->c, __entry->level, __entry->grplo, __entry->grphi,
+		  __entry->gpevent)
+);
+
+/*
  * Tracepoint for grace-period-initialization events.  These are
  * distinguished by the type of RCU, the new grace-period number, the
  * rcu_node structure level, the starting and ending CPU covered by the
@@ -601,6 +653,9 @@ TRACE_EVENT(rcu_barrier,
 #define trace_rcu_grace_period(rcuname, gpnum, gpevent) do { } while (0)
 #define trace_rcu_grace_period_init(rcuname, gpnum, level, grplo, grphi, \
 				    qsmask) do { } while (0)
+#define trace_rcu_future_grace_period(rcuname, gpnum, completed, c, \
+				      level, grplo, grphi, event) \
+				      do { } while (0)
 #define trace_rcu_preempt_task(rcuname, pid, gpnum) do { } while (0)
 #define trace_rcu_unlock_preempted_task(rcuname, gpnum, pid) do { } while (0)
 #define trace_rcu_quiescent_state_report(rcuname, gpnum, mask, qsmask, level, \
diff --git a/include/trace/events/regmap.h b/include/trace/events/regmap.h
index 41a7dbd570e..a43a2f67bd8 100644
--- a/include/trace/events/regmap.h
+++ b/include/trace/events/regmap.h
@@ -175,6 +175,54 @@ DEFINE_EVENT(regmap_bool, regmap_cache_bypass,
 
 );
 
+DECLARE_EVENT_CLASS(regmap_async,
+
+	TP_PROTO(struct device *dev),
+
+	TP_ARGS(dev),
+
+	TP_STRUCT__entry(
+		__string(	name,		dev_name(dev)	)
+	),
+
+	TP_fast_assign(
+		__assign_str(name, dev_name(dev));
+	),
+
+	TP_printk("%s", __get_str(name))
+);
+
+DEFINE_EVENT(regmap_block, regmap_async_write_start,
+
+	TP_PROTO(struct device *dev, unsigned int reg, int count),
+
+	TP_ARGS(dev, reg, count)
+);
+
+DEFINE_EVENT(regmap_async, regmap_async_io_complete,
+
+	TP_PROTO(struct device *dev),
+
+	TP_ARGS(dev)
+
+);
+
+DEFINE_EVENT(regmap_async, regmap_async_complete_start,
+
+	TP_PROTO(struct device *dev),
+
+	TP_ARGS(dev)
+
+);
+
+DEFINE_EVENT(regmap_async, regmap_async_complete_done,
+
+	TP_PROTO(struct device *dev),
+
+	TP_ARGS(dev)
+
+);
+
 #endif /* _TRACE_REGMAP_H */
 
 /* This part must be outside protection */
diff --git a/include/trace/events/sched.h b/include/trace/events/sched.h
index 5a8671e8a67..e5586caff67 100644
--- a/include/trace/events/sched.h
+++ b/include/trace/events/sched.h
@@ -147,7 +147,7 @@ TRACE_EVENT(sched_switch,
 		  __print_flags(__entry->prev_state & (TASK_STATE_MAX-1), "|",
 				{ 1, "S"} , { 2, "D" }, { 4, "T" }, { 8, "t" },
 				{ 16, "Z" }, { 32, "X" }, { 64, "x" },
-				{ 128, "W" }) : "R",
+				{ 128, "K" }, { 256, "W" }, { 512, "P" }) : "R",
 		__entry->prev_state & TASK_STATE_MAX ? "+" : "",
 		__entry->next_comm, __entry->next_pid, __entry->next_prio)
 );
diff --git a/include/trace/events/timer.h b/include/trace/events/timer.h
index 425bcfe56c6..8d219470624 100644
--- a/include/trace/events/timer.h
+++ b/include/trace/events/timer.h
@@ -123,7 +123,7 @@ DEFINE_EVENT(timer_class, timer_cancel,
 
 /**
  * hrtimer_init - called when the hrtimer is initialized
- * @timer:	pointer to struct hrtimer
+ * @hrtimer:	pointer to struct hrtimer
  * @clockid:	the hrtimers clock
  * @mode:	the hrtimers mode
  */
@@ -155,7 +155,7 @@ TRACE_EVENT(hrtimer_init,
 
 /**
  * hrtimer_start - called when the hrtimer is started
- * @timer: pointer to struct hrtimer
+ * @hrtimer: pointer to struct hrtimer
  */
 TRACE_EVENT(hrtimer_start,
 
@@ -186,8 +186,8 @@ TRACE_EVENT(hrtimer_start,
 );
 
 /**
- * htimmer_expire_entry - called immediately before the hrtimer callback
- * @timer:	pointer to struct hrtimer
+ * hrtimer_expire_entry - called immediately before the hrtimer callback
+ * @hrtimer:	pointer to struct hrtimer
  * @now:	pointer to variable which contains current time of the
  *		timers base.
  *
@@ -234,7 +234,7 @@ DECLARE_EVENT_CLASS(hrtimer_class,
 
 /**
  * hrtimer_expire_exit - called immediately after the hrtimer callback returns
- * @timer:	pointer to struct hrtimer
+ * @hrtimer:	pointer to struct hrtimer
  *
  * When used in combination with the hrtimer_expire_entry tracepoint we can
  * determine the runtime of the callback function.
diff --git a/include/trace/ftrace.h b/include/trace/ftrace.h
index 40dc5e8fe34..19edd7facaa 100644
--- a/include/trace/ftrace.h
+++ b/include/trace/ftrace.h
@@ -227,29 +227,18 @@ static notrace enum print_line_t					\
 ftrace_raw_output_##call(struct trace_iterator *iter, int flags,	\
 			 struct trace_event *trace_event)		\
 {									\
-	struct ftrace_event_call *event;				\
 	struct trace_seq *s = &iter->seq;				\
+	struct trace_seq __maybe_unused *p = &iter->tmp_seq;		\
 	struct ftrace_raw_##call *field;				\
-	struct trace_entry *entry;					\
-	struct trace_seq *p = &iter->tmp_seq;				\
 	int ret;							\
 									\
-	event = container_of(trace_event, struct ftrace_event_call,	\
-			     event);					\
-									\
-	entry = iter->ent;						\
-									\
-	if (entry->type != event->event.type) {				\
-		WARN_ON_ONCE(1);					\
-		return TRACE_TYPE_UNHANDLED;				\
-	}								\
-									\
-	field = (typeof(field))entry;					\
+	field = (typeof(field))iter->ent;				\
 									\
-	trace_seq_init(p);						\
-	ret = trace_seq_printf(s, "%s: ", event->name);			\
+	ret = ftrace_raw_output_prep(iter, trace_event);		\
 	if (ret)							\
-		ret = trace_seq_printf(s, print);			\
+		return ret;						\
+									\
+	ret = trace_seq_printf(s, print);				\
 	if (!ret)							\
 		return TRACE_TYPE_PARTIAL_LINE;				\
 									\
@@ -335,7 +324,7 @@ static struct trace_event_functions ftrace_event_type_funcs_##call = {	\
 
 #undef DECLARE_EVENT_CLASS
 #define DECLARE_EVENT_CLASS(call, proto, args, tstruct, func, print)	\
-static int notrace							\
+static int notrace __init						\
 ftrace_define_fields_##call(struct ftrace_event_call *event_call)	\
 {									\
 	struct ftrace_raw_##call field;					\
@@ -414,7 +403,8 @@ static inline notrace int ftrace_get_offsets_##call(			\
  *
  * static void ftrace_raw_event_<call>(void *__data, proto)
  * {
- *	struct ftrace_event_call *event_call = __data;
+ *	struct ftrace_event_file *ftrace_file = __data;
+ *	struct ftrace_event_call *event_call = ftrace_file->event_call;
  *	struct ftrace_data_offsets_<call> __maybe_unused __data_offsets;
  *	struct ring_buffer_event *event;
  *	struct ftrace_raw_<call> *entry; <-- defined in stage 1
@@ -423,12 +413,16 @@ static inline notrace int ftrace_get_offsets_##call(			\
  *	int __data_size;
  *	int pc;
  *
+ *	if (test_bit(FTRACE_EVENT_FL_SOFT_DISABLED_BIT,
+ *		     &ftrace_file->flags))
+ *		return;
+ *
  *	local_save_flags(irq_flags);
  *	pc = preempt_count();
  *
  *	__data_size = ftrace_get_offsets_<call>(&__data_offsets, args);
  *
- *	event = trace_current_buffer_lock_reserve(&buffer,
+ *	event = trace_event_buffer_lock_reserve(&buffer, ftrace_file,
  *				  event_<call>->event.type,
  *				  sizeof(*entry) + __data_size,
  *				  irq_flags, pc);
@@ -440,7 +434,7 @@ static inline notrace int ftrace_get_offsets_##call(			\
  *			   __array macros.
  *
  *	if (!filter_current_check_discard(buffer, event_call, entry, event))
- *		trace_current_buffer_unlock_commit(buffer,
+ *		trace_nowake_buffer_unlock_commit(buffer,
  *						   event, irq_flags, pc);
  * }
  *
@@ -518,7 +512,8 @@ static inline notrace int ftrace_get_offsets_##call(			\
 static notrace void							\
 ftrace_raw_event_##call(void *__data, proto)				\
 {									\
-	struct ftrace_event_call *event_call = __data;			\
+	struct ftrace_event_file *ftrace_file = __data;			\
+	struct ftrace_event_call *event_call = ftrace_file->event_call;	\
 	struct ftrace_data_offsets_##call __maybe_unused __data_offsets;\
 	struct ring_buffer_event *event;				\
 	struct ftrace_raw_##call *entry;				\
@@ -527,12 +522,16 @@ ftrace_raw_event_##call(void *__data, proto)				\
 	int __data_size;						\
 	int pc;								\
 									\
+	if (test_bit(FTRACE_EVENT_FL_SOFT_DISABLED_BIT,			\
+		     &ftrace_file->flags))				\
+		return;							\
+									\
 	local_save_flags(irq_flags);					\
 	pc = preempt_count();						\
 									\
 	__data_size = ftrace_get_offsets_##call(&__data_offsets, args); \
 									\
-	event = trace_current_buffer_lock_reserve(&buffer,		\
+	event = trace_event_buffer_lock_reserve(&buffer, ftrace_file,	\
 				 event_call->event.type,		\
 				 sizeof(*entry) + __data_size,		\
 				 irq_flags, pc);			\
@@ -581,7 +580,7 @@ static inline void ftrace_test_probe_##call(void)			\
 #define DECLARE_EVENT_CLASS(call, proto, args, tstruct, assign, print)	\
 _TRACE_PERF_PROTO(call, PARAMS(proto));					\
 static const char print_fmt_##call[] = print;				\
-static struct ftrace_event_class __used event_class_##call = {		\
+static struct ftrace_event_class __used __refdata event_class_##call = { \
 	.system			= __stringify(TRACE_SYSTEM),		\
 	.define_fields		= ftrace_define_fields_##call,		\
 	.fields			= LIST_HEAD_INIT(event_class_##call.fields),\
@@ -705,5 +704,3 @@ static inline void perf_test_probe_##call(void)				\
 #include TRACE_INCLUDE(TRACE_INCLUDE_FILE)
 #endif /* CONFIG_PERF_EVENTS */
 
-#undef _TRACE_PROFILE_INIT
-
diff --git a/include/uapi/linux/connector.h b/include/uapi/linux/connector.h
index 8761a0349c7..4cb283505e4 100644
--- a/include/uapi/linux/connector.h
+++ b/include/uapi/linux/connector.h
@@ -44,8 +44,11 @@
 #define CN_VAL_DRBD			0x1
 #define CN_KVP_IDX			0x9	/* HyperV KVP */
 #define CN_KVP_VAL			0x1	/* queries from the kernel */
+#define CN_VSS_IDX			0xA     /* HyperV VSS */
+#define CN_VSS_VAL			0x1     /* queries from the kernel */
 
-#define CN_NETLINK_USERS		10	/* Highest index + 1 */
+
+#define CN_NETLINK_USERS		11	/* Highest index + 1 */
 
 /*
  * Maximum connector's message size.
diff --git a/include/uapi/linux/dvb/dmx.h b/include/uapi/linux/dvb/dmx.h
index b2a9ad8cafd..b4fb650d9d4 100644
--- a/include/uapi/linux/dvb/dmx.h
+++ b/include/uapi/linux/dvb/dmx.h
@@ -51,7 +51,7 @@ typedef enum
 } dmx_input_t;
 
 
-typedef enum
+typedef enum dmx_ts_pes
 {
 	DMX_PES_AUDIO0,
 	DMX_PES_VIDEO0,
diff --git a/include/uapi/linux/elf.h b/include/uapi/linux/elf.h
index 8072d352b98..ef6103bf1f9 100644
--- a/include/uapi/linux/elf.h
+++ b/include/uapi/linux/elf.h
@@ -397,6 +397,7 @@ typedef struct elf64_shdr {
 #define NT_ARM_HW_WATCH	0x403		/* ARM hardware watchpoint registers */
 #define NT_METAG_CBUF	0x500		/* Metag catch buffer registers */
 #define NT_METAG_RPIPE	0x501		/* Metag read pipeline state */
+#define NT_METAG_TLS	0x502		/* Metag TLS pointer */
 
 
 /* Note header in a PT_NOTE section */
diff --git a/include/uapi/linux/fs.h b/include/uapi/linux/fs.h
index c7fc1e6517c..a4ed56cf0ea 100644
--- a/include/uapi/linux/fs.h
+++ b/include/uapi/linux/fs.h
@@ -88,7 +88,6 @@ struct inodes_stat_t {
 #define MS_STRICTATIME	(1<<24) /* Always perform atime updates */
 
 /* These sb flags are internal to the kernel */
-#define MS_SNAP_STABLE	(1<<27) /* Snapshot pages during writeback, if needed */
 #define MS_NOSEC	(1<<28)
 #define MS_BORN		(1<<29)
 #define MS_ACTIVE	(1<<30)
diff --git a/include/uapi/linux/fuse.h b/include/uapi/linux/fuse.h
index 4c43b444879..706d035fa74 100644
--- a/include/uapi/linux/fuse.h
+++ b/include/uapi/linux/fuse.h
@@ -95,15 +95,10 @@
 #ifndef _LINUX_FUSE_H
 #define _LINUX_FUSE_H
 
-#ifdef __linux__
+#ifdef __KERNEL__
 #include <linux/types.h>
 #else
 #include <stdint.h>
-#define __u64 uint64_t
-#define __s64 int64_t
-#define __u32 uint32_t
-#define __s32 int32_t
-#define __u16 uint16_t
 #endif
 
 /*
@@ -139,42 +134,42 @@
    userspace works under 64bit kernels */
 
 struct fuse_attr {
-	__u64	ino;
-	__u64	size;
-	__u64	blocks;
-	__u64	atime;
-	__u64	mtime;
-	__u64	ctime;
-	__u32	atimensec;
-	__u32	mtimensec;
-	__u32	ctimensec;
-	__u32	mode;
-	__u32	nlink;
-	__u32	uid;
-	__u32	gid;
-	__u32	rdev;
-	__u32	blksize;
-	__u32	padding;
+	uint64_t	ino;
+	uint64_t	size;
+	uint64_t	blocks;
+	uint64_t	atime;
+	uint64_t	mtime;
+	uint64_t	ctime;
+	uint32_t	atimensec;
+	uint32_t	mtimensec;
+	uint32_t	ctimensec;
+	uint32_t	mode;
+	uint32_t	nlink;
+	uint32_t	uid;
+	uint32_t	gid;
+	uint32_t	rdev;
+	uint32_t	blksize;
+	uint32_t	padding;
 };
 
 struct fuse_kstatfs {
-	__u64	blocks;
-	__u64	bfree;
-	__u64	bavail;
-	__u64	files;
-	__u64	ffree;
-	__u32	bsize;
-	__u32	namelen;
-	__u32	frsize;
-	__u32	padding;
-	__u32	spare[6];
+	uint64_t	blocks;
+	uint64_t	bfree;
+	uint64_t	bavail;
+	uint64_t	files;
+	uint64_t	ffree;
+	uint32_t	bsize;
+	uint32_t	namelen;
+	uint32_t	frsize;
+	uint32_t	padding;
+	uint32_t	spare[6];
 };
 
 struct fuse_file_lock {
-	__u64	start;
-	__u64	end;
-	__u32	type;
-	__u32	pid; /* tgid */
+	uint64_t	start;
+	uint64_t	end;
+	uint32_t	type;
+	uint32_t	pid; /* tgid */
 };
 
 /**
@@ -364,143 +359,143 @@ enum fuse_notify_code {
 #define FUSE_COMPAT_ENTRY_OUT_SIZE 120
 
 struct fuse_entry_out {
-	__u64	nodeid;		/* Inode ID */
-	__u64	generation;	/* Inode generation: nodeid:gen must
-				   be unique for the fs's lifetime */
-	__u64	entry_valid;	/* Cache timeout for the name */
-	__u64	attr_valid;	/* Cache timeout for the attributes */
-	__u32	entry_valid_nsec;
-	__u32	attr_valid_nsec;
+	uint64_t	nodeid;		/* Inode ID */
+	uint64_t	generation;	/* Inode generation: nodeid:gen must
+					   be unique for the fs's lifetime */
+	uint64_t	entry_valid;	/* Cache timeout for the name */
+	uint64_t	attr_valid;	/* Cache timeout for the attributes */
+	uint32_t	entry_valid_nsec;
+	uint32_t	attr_valid_nsec;
 	struct fuse_attr attr;
 };
 
 struct fuse_forget_in {
-	__u64	nlookup;
+	uint64_t	nlookup;
 };
 
 struct fuse_forget_one {
-	__u64	nodeid;
-	__u64	nlookup;
+	uint64_t	nodeid;
+	uint64_t	nlookup;
 };
 
 struct fuse_batch_forget_in {
-	__u32	count;
-	__u32	dummy;
+	uint32_t	count;
+	uint32_t	dummy;
 };
 
 struct fuse_getattr_in {
-	__u32	getattr_flags;
-	__u32	dummy;
-	__u64	fh;
+	uint32_t	getattr_flags;
+	uint32_t	dummy;
+	uint64_t	fh;
 };
 
 #define FUSE_COMPAT_ATTR_OUT_SIZE 96
 
 struct fuse_attr_out {
-	__u64	attr_valid;	/* Cache timeout for the attributes */
-	__u32	attr_valid_nsec;
-	__u32	dummy;
+	uint64_t	attr_valid;	/* Cache timeout for the attributes */
+	uint32_t	attr_valid_nsec;
+	uint32_t	dummy;
 	struct fuse_attr attr;
 };
 
 #define FUSE_COMPAT_MKNOD_IN_SIZE 8
 
 struct fuse_mknod_in {
-	__u32	mode;
-	__u32	rdev;
-	__u32	umask;
-	__u32	padding;
+	uint32_t	mode;
+	uint32_t	rdev;
+	uint32_t	umask;
+	uint32_t	padding;
 };
 
 struct fuse_mkdir_in {
-	__u32	mode;
-	__u32	umask;
+	uint32_t	mode;
+	uint32_t	umask;
 };
 
 struct fuse_rename_in {
-	__u64	newdir;
+	uint64_t	newdir;
 };
 
 struct fuse_link_in {
-	__u64	oldnodeid;
+	uint64_t	oldnodeid;
 };
 
 struct fuse_setattr_in {
-	__u32	valid;
-	__u32	padding;
-	__u64	fh;
-	__u64	size;
-	__u64	lock_owner;
-	__u64	atime;
-	__u64	mtime;
-	__u64	unused2;
-	__u32	atimensec;
-	__u32	mtimensec;
-	__u32	unused3;
-	__u32	mode;
-	__u32	unused4;
-	__u32	uid;
-	__u32	gid;
-	__u32	unused5;
+	uint32_t	valid;
+	uint32_t	padding;
+	uint64_t	fh;
+	uint64_t	size;
+	uint64_t	lock_owner;
+	uint64_t	atime;
+	uint64_t	mtime;
+	uint64_t	unused2;
+	uint32_t	atimensec;
+	uint32_t	mtimensec;
+	uint32_t	unused3;
+	uint32_t	mode;
+	uint32_t	unused4;
+	uint32_t	uid;
+	uint32_t	gid;
+	uint32_t	unused5;
 };
 
 struct fuse_open_in {
-	__u32	flags;
-	__u32	unused;
+	uint32_t	flags;
+	uint32_t	unused;
 };
 
 struct fuse_create_in {
-	__u32	flags;
-	__u32	mode;
-	__u32	umask;
-	__u32	padding;
+	uint32_t	flags;
+	uint32_t	mode;
+	uint32_t	umask;
+	uint32_t	padding;
 };
 
 struct fuse_open_out {
-	__u64	fh;
-	__u32	open_flags;
-	__u32	padding;
+	uint64_t	fh;
+	uint32_t	open_flags;
+	uint32_t	padding;
 };
 
 struct fuse_release_in {
-	__u64	fh;
-	__u32	flags;
-	__u32	release_flags;
-	__u64	lock_owner;
+	uint64_t	fh;
+	uint32_t	flags;
+	uint32_t	release_flags;
+	uint64_t	lock_owner;
 };
 
 struct fuse_flush_in {
-	__u64	fh;
-	__u32	unused;
-	__u32	padding;
-	__u64	lock_owner;
+	uint64_t	fh;
+	uint32_t	unused;
+	uint32_t	padding;
+	uint64_t	lock_owner;
 };
 
 struct fuse_read_in {
-	__u64	fh;
-	__u64	offset;
-	__u32	size;
-	__u32	read_flags;
-	__u64	lock_owner;
-	__u32	flags;
-	__u32	padding;
+	uint64_t	fh;
+	uint64_t	offset;
+	uint32_t	size;
+	uint32_t	read_flags;
+	uint64_t	lock_owner;
+	uint32_t	flags;
+	uint32_t	padding;
 };
 
 #define FUSE_COMPAT_WRITE_IN_SIZE 24
 
 struct fuse_write_in {
-	__u64	fh;
-	__u64	offset;
-	__u32	size;
-	__u32	write_flags;
-	__u64	lock_owner;
-	__u32	flags;
-	__u32	padding;
+	uint64_t	fh;
+	uint64_t	offset;
+	uint32_t	size;
+	uint32_t	write_flags;
+	uint64_t	lock_owner;
+	uint32_t	flags;
+	uint32_t	padding;
 };
 
 struct fuse_write_out {
-	__u32	size;
-	__u32	padding;
+	uint32_t	size;
+	uint32_t	padding;
 };
 
 #define FUSE_COMPAT_STATFS_SIZE 48
@@ -510,32 +505,32 @@ struct fuse_statfs_out {
 };
 
 struct fuse_fsync_in {
-	__u64	fh;
-	__u32	fsync_flags;
-	__u32	padding;
+	uint64_t	fh;
+	uint32_t	fsync_flags;
+	uint32_t	padding;
 };
 
 struct fuse_setxattr_in {
-	__u32	size;
-	__u32	flags;
+	uint32_t	size;
+	uint32_t	flags;
 };
 
 struct fuse_getxattr_in {
-	__u32	size;
-	__u32	padding;
+	uint32_t	size;
+	uint32_t	padding;
 };
 
 struct fuse_getxattr_out {
-	__u32	size;
-	__u32	padding;
+	uint32_t	size;
+	uint32_t	padding;
 };
 
 struct fuse_lk_in {
-	__u64	fh;
-	__u64	owner;
+	uint64_t	fh;
+	uint64_t	owner;
 	struct fuse_file_lock lk;
-	__u32	lk_flags;
-	__u32	padding;
+	uint32_t	lk_flags;
+	uint32_t	padding;
 };
 
 struct fuse_lk_out {
@@ -543,134 +538,135 @@ struct fuse_lk_out {
 };
 
 struct fuse_access_in {
-	__u32	mask;
-	__u32	padding;
+	uint32_t	mask;
+	uint32_t	padding;
 };
 
 struct fuse_init_in {
-	__u32	major;
-	__u32	minor;
-	__u32	max_readahead;
-	__u32	flags;
+	uint32_t	major;
+	uint32_t	minor;
+	uint32_t	max_readahead;
+	uint32_t	flags;
 };
 
 struct fuse_init_out {
-	__u32	major;
-	__u32	minor;
-	__u32	max_readahead;
-	__u32	flags;
-	__u16   max_background;
-	__u16   congestion_threshold;
-	__u32	max_write;
+	uint32_t	major;
+	uint32_t	minor;
+	uint32_t	max_readahead;
+	uint32_t	flags;
+	uint16_t	max_background;
+	uint16_t	congestion_threshold;
+	uint32_t	max_write;
 };
 
 #define CUSE_INIT_INFO_MAX 4096
 
 struct cuse_init_in {
-	__u32	major;
-	__u32	minor;
-	__u32	unused;
-	__u32	flags;
+	uint32_t	major;
+	uint32_t	minor;
+	uint32_t	unused;
+	uint32_t	flags;
 };
 
 struct cuse_init_out {
-	__u32	major;
-	__u32	minor;
-	__u32	unused;
-	__u32	flags;
-	__u32	max_read;
-	__u32	max_write;
-	__u32	dev_major;		/* chardev major */
-	__u32	dev_minor;		/* chardev minor */
-	__u32	spare[10];
+	uint32_t	major;
+	uint32_t	minor;
+	uint32_t	unused;
+	uint32_t	flags;
+	uint32_t	max_read;
+	uint32_t	max_write;
+	uint32_t	dev_major;		/* chardev major */
+	uint32_t	dev_minor;		/* chardev minor */
+	uint32_t	spare[10];
 };
 
 struct fuse_interrupt_in {
-	__u64	unique;
+	uint64_t	unique;
 };
 
 struct fuse_bmap_in {
-	__u64	block;
-	__u32	blocksize;
-	__u32	padding;
+	uint64_t	block;
+	uint32_t	blocksize;
+	uint32_t	padding;
 };
 
 struct fuse_bmap_out {
-	__u64	block;
+	uint64_t	block;
 };
 
 struct fuse_ioctl_in {
-	__u64	fh;
-	__u32	flags;
-	__u32	cmd;
-	__u64	arg;
-	__u32	in_size;
-	__u32	out_size;
+	uint64_t	fh;
+	uint32_t	flags;
+	uint32_t	cmd;
+	uint64_t	arg;
+	uint32_t	in_size;
+	uint32_t	out_size;
 };
 
 struct fuse_ioctl_iovec {
-	__u64	base;
-	__u64	len;
+	uint64_t	base;
+	uint64_t	len;
 };
 
 struct fuse_ioctl_out {
-	__s32	result;
-	__u32	flags;
-	__u32	in_iovs;
-	__u32	out_iovs;
+	int32_t		result;
+	uint32_t	flags;
+	uint32_t	in_iovs;
+	uint32_t	out_iovs;
 };
 
 struct fuse_poll_in {
-	__u64	fh;
-	__u64	kh;
-	__u32	flags;
-	__u32   events;
+	uint64_t	fh;
+	uint64_t	kh;
+	uint32_t	flags;
+	uint32_t	events;
 };
 
 struct fuse_poll_out {
-	__u32	revents;
-	__u32	padding;
+	uint32_t	revents;
+	uint32_t	padding;
 };
 
 struct fuse_notify_poll_wakeup_out {
-	__u64	kh;
+	uint64_t	kh;
 };
 
 struct fuse_fallocate_in {
-	__u64	fh;
-	__u64	offset;
-	__u64	length;
-	__u32	mode;
-	__u32	padding;
+	uint64_t	fh;
+	uint64_t	offset;
+	uint64_t	length;
+	uint32_t	mode;
+	uint32_t	padding;
 };
 
 struct fuse_in_header {
-	__u32	len;
-	__u32	opcode;
-	__u64	unique;
-	__u64	nodeid;
-	__u32	uid;
-	__u32	gid;
-	__u32	pid;
-	__u32	padding;
+	uint32_t	len;
+	uint32_t	opcode;
+	uint64_t	unique;
+	uint64_t	nodeid;
+	uint32_t	uid;
+	uint32_t	gid;
+	uint32_t	pid;
+	uint32_t	padding;
 };
 
 struct fuse_out_header {
-	__u32	len;
-	__s32	error;
-	__u64	unique;
+	uint32_t	len;
+	int32_t		error;
+	uint64_t	unique;
 };
 
 struct fuse_dirent {
-	__u64	ino;
-	__u64	off;
-	__u32	namelen;
-	__u32	type;
+	uint64_t	ino;
+	uint64_t	off;
+	uint32_t	namelen;
+	uint32_t	type;
 	char name[];
 };
 
 #define FUSE_NAME_OFFSET offsetof(struct fuse_dirent, name)
-#define FUSE_DIRENT_ALIGN(x) (((x) + sizeof(__u64) - 1) & ~(sizeof(__u64) - 1))
+#define FUSE_DIRENT_ALIGN(x) \
+	(((x) + sizeof(uint64_t) - 1) & ~(sizeof(uint64_t) - 1))
 #define FUSE_DIRENT_SIZE(d) \
 	FUSE_DIRENT_ALIGN(FUSE_NAME_OFFSET + (d)->namelen)
 
@@ -685,47 +681,47 @@ struct fuse_direntplus {
 	FUSE_DIRENT_ALIGN(FUSE_NAME_OFFSET_DIRENTPLUS + (d)->dirent.namelen)
 
 struct fuse_notify_inval_inode_out {
-	__u64	ino;
-	__s64	off;
-	__s64	len;
+	uint64_t	ino;
+	int64_t		off;
+	int64_t		len;
 };
 
 struct fuse_notify_inval_entry_out {
-	__u64	parent;
-	__u32	namelen;
-	__u32	padding;
+	uint64_t	parent;
+	uint32_t	namelen;
+	uint32_t	padding;
 };
 
 struct fuse_notify_delete_out {
-	__u64	parent;
-	__u64	child;
-	__u32	namelen;
-	__u32	padding;
+	uint64_t	parent;
+	uint64_t	child;
+	uint32_t	namelen;
+	uint32_t	padding;
 };
 
 struct fuse_notify_store_out {
-	__u64	nodeid;
-	__u64	offset;
-	__u32	size;
-	__u32	padding;
+	uint64_t	nodeid;
+	uint64_t	offset;
+	uint32_t	size;
+	uint32_t	padding;
 };
 
 struct fuse_notify_retrieve_out {
-	__u64	notify_unique;
-	__u64	nodeid;
-	__u64	offset;
-	__u32	size;
-	__u32	padding;
+	uint64_t	notify_unique;
+	uint64_t	nodeid;
+	uint64_t	offset;
+	uint32_t	size;
+	uint32_t	padding;
 };
 
 /* Matches the size of fuse_write_in */
 struct fuse_notify_retrieve_in {
-	__u64	dummy1;
-	__u64	offset;
-	__u32	size;
-	__u32	dummy2;
-	__u64	dummy3;
-	__u64	dummy4;
+	uint64_t	dummy1;
+	uint64_t	offset;
+	uint32_t	size;
+	uint32_t	dummy2;
+	uint64_t	dummy3;
+	uint64_t	dummy4;
 };
 
 #endif /* _LINUX_FUSE_H */
diff --git a/include/uapi/linux/media.h b/include/uapi/linux/media.h
index 0ef883327de..ed49574ad75 100644
--- a/include/uapi/linux/media.h
+++ b/include/uapi/linux/media.h
@@ -56,6 +56,8 @@ struct media_device_info {
 #define MEDIA_ENT_T_V4L2_SUBDEV_SENSOR	(MEDIA_ENT_T_V4L2_SUBDEV + 1)
 #define MEDIA_ENT_T_V4L2_SUBDEV_FLASH	(MEDIA_ENT_T_V4L2_SUBDEV + 2)
 #define MEDIA_ENT_T_V4L2_SUBDEV_LENS	(MEDIA_ENT_T_V4L2_SUBDEV + 3)
+/* A converter of analogue video to its digital representation. */
+#define MEDIA_ENT_T_V4L2_SUBDEV_DECODER	(MEDIA_ENT_T_V4L2_SUBDEV + 4)
 
 #define MEDIA_ENT_FL_DEFAULT		(1 << 0)
 
diff --git a/include/uapi/linux/packet_diag.h b/include/uapi/linux/packet_diag.h
index 93f5fa94a43..afafd703ad9 100644
--- a/include/uapi/linux/packet_diag.h
+++ b/include/uapi/linux/packet_diag.h
@@ -33,9 +33,11 @@ enum {
 	PACKET_DIAG_TX_RING,
 	PACKET_DIAG_FANOUT,
 
-	PACKET_DIAG_MAX,
+	__PACKET_DIAG_MAX,
 };
 
+#define PACKET_DIAG_MAX (__PACKET_DIAG_MAX - 1)
+
 struct packet_diag_info {
 	__u32	pdi_index;
 	__u32	pdi_version;
diff --git a/include/uapi/linux/pci_regs.h b/include/uapi/linux/pci_regs.h
index ebfadc56d1b..864e324da80 100644
--- a/include/uapi/linux/pci_regs.h
+++ b/include/uapi/linux/pci_regs.h
@@ -292,12 +292,12 @@
 
 /* Message Signalled Interrupts registers */
 
-#define PCI_MSI_FLAGS		2	/* Various flags */
-#define  PCI_MSI_FLAGS_64BIT	0x80	/* 64-bit addresses allowed */
-#define  PCI_MSI_FLAGS_QSIZE	0x70	/* Message queue size configured */
-#define  PCI_MSI_FLAGS_QMASK	0x0e	/* Maximum queue size available */
-#define  PCI_MSI_FLAGS_ENABLE	0x01	/* MSI feature enabled */
-#define  PCI_MSI_FLAGS_MASKBIT	0x100	/* 64-bit mask bits allowed */
+#define PCI_MSI_FLAGS		2	/* Message Control */
+#define  PCI_MSI_FLAGS_ENABLE	0x0001	/* MSI feature enabled */
+#define  PCI_MSI_FLAGS_QMASK	0x000e	/* Maximum queue size available */
+#define  PCI_MSI_FLAGS_QSIZE	0x0070	/* Message queue size configured */
+#define  PCI_MSI_FLAGS_64BIT	0x0080	/* 64-bit addresses allowed */
+#define  PCI_MSI_FLAGS_MASKBIT	0x0100	/* Per-vector masking capable */
 #define PCI_MSI_RFU		3	/* Rest of capability flags */
 #define PCI_MSI_ADDRESS_LO	4	/* Lower 32 bits */
 #define PCI_MSI_ADDRESS_HI	8	/* Upper 32 bits (if PCI_MSI_FLAGS_64BIT set) */
@@ -309,13 +309,17 @@
 #define PCI_MSI_PENDING_64	20	/* Pending intrs for 64-bit devices */
 
 /* MSI-X registers */
-#define PCI_MSIX_FLAGS		2
-#define  PCI_MSIX_FLAGS_QSIZE	0x7FF
-#define  PCI_MSIX_FLAGS_ENABLE	(1 << 15)
-#define  PCI_MSIX_FLAGS_MASKALL	(1 << 14)
-#define PCI_MSIX_TABLE		4
-#define PCI_MSIX_PBA		8
-#define  PCI_MSIX_FLAGS_BIRMASK	(7 << 0)
+#define PCI_MSIX_FLAGS		2	/* Message Control */
+#define  PCI_MSIX_FLAGS_QSIZE	0x07FF	/* Table size */
+#define  PCI_MSIX_FLAGS_MASKALL	0x4000	/* Mask all vectors for this function */
+#define  PCI_MSIX_FLAGS_ENABLE	0x8000	/* MSI-X enable */
+#define PCI_MSIX_TABLE		4	/* Table offset */
+#define  PCI_MSIX_TABLE_BIR	0x00000007 /* BAR index */
+#define  PCI_MSIX_TABLE_OFFSET	0xfffffff8 /* Offset into specified BAR */
+#define PCI_MSIX_PBA		8	/* Pending Bit Array offset */
+#define  PCI_MSIX_PBA_BIR	0x00000007 /* BAR index */
+#define  PCI_MSIX_PBA_OFFSET	0xfffffff8 /* Offset into specified BAR */
+#define  PCI_MSIX_FLAGS_BIRMASK	(7 << 0)   /* deprecated */
 #define PCI_CAP_MSIX_SIZEOF	12	/* size of MSIX registers */
 
 /* MSI-X entry's format */
diff --git a/include/uapi/linux/perf_event.h b/include/uapi/linux/perf_event.h
index 9fa9c622a7f..fb104e51496 100644
--- a/include/uapi/linux/perf_event.h
+++ b/include/uapi/linux/perf_event.h
@@ -132,8 +132,10 @@ enum perf_event_sample_format {
 	PERF_SAMPLE_BRANCH_STACK		= 1U << 11,
 	PERF_SAMPLE_REGS_USER			= 1U << 12,
 	PERF_SAMPLE_STACK_USER			= 1U << 13,
+	PERF_SAMPLE_WEIGHT			= 1U << 14,
+	PERF_SAMPLE_DATA_SRC			= 1U << 15,
 
-	PERF_SAMPLE_MAX = 1U << 14,		/* non-ABI */
+	PERF_SAMPLE_MAX = 1U << 16,		/* non-ABI */
 };
 
 /*
@@ -443,6 +445,7 @@ struct perf_event_mmap_page {
 #define PERF_RECORD_MISC_GUEST_KERNEL		(4 << 0)
 #define PERF_RECORD_MISC_GUEST_USER		(5 << 0)
 
+#define PERF_RECORD_MISC_MMAP_DATA		(1 << 13)
 /*
  * Indicates that the content of PERF_SAMPLE_IP points to
  * the actual instruction that triggered the event. See also
@@ -588,6 +591,9 @@ enum perf_event_type {
 	 * 	{ u64			size;
 	 * 	  char			data[size];
 	 * 	  u64			dyn_size; } && PERF_SAMPLE_STACK_USER
+	 *
+	 *	{ u64			weight;   } && PERF_SAMPLE_WEIGHT
+	 *	{ u64			data_src;     } && PERF_SAMPLE_DATA_SRC
 	 * };
 	 */
 	PERF_RECORD_SAMPLE			= 9,
@@ -613,4 +619,67 @@ enum perf_callchain_context {
 #define PERF_FLAG_FD_OUTPUT		(1U << 1)
 #define PERF_FLAG_PID_CGROUP		(1U << 2) /* pid=cgroup id, per-cpu mode only */
 
+union perf_mem_data_src {
+	__u64 val;
+	struct {
+		__u64   mem_op:5,	/* type of opcode */
+			mem_lvl:14,	/* memory hierarchy level */
+			mem_snoop:5,	/* snoop mode */
+			mem_lock:2,	/* lock instr */
+			mem_dtlb:7,	/* tlb access */
+			mem_rsvd:31;
+	};
+};
+
+/* type of opcode (load/store/prefetch,code) */
+#define PERF_MEM_OP_NA		0x01 /* not available */
+#define PERF_MEM_OP_LOAD	0x02 /* load instruction */
+#define PERF_MEM_OP_STORE	0x04 /* store instruction */
+#define PERF_MEM_OP_PFETCH	0x08 /* prefetch */
+#define PERF_MEM_OP_EXEC	0x10 /* code (execution) */
+#define PERF_MEM_OP_SHIFT	0
+
+/* memory hierarchy (memory level, hit or miss) */
+#define PERF_MEM_LVL_NA		0x01  /* not available */
+#define PERF_MEM_LVL_HIT	0x02  /* hit level */
+#define PERF_MEM_LVL_MISS	0x04  /* miss level  */
+#define PERF_MEM_LVL_L1		0x08  /* L1 */
+#define PERF_MEM_LVL_LFB	0x10  /* Line Fill Buffer */
+#define PERF_MEM_LVL_L2		0x20  /* L2 */
+#define PERF_MEM_LVL_L3		0x40  /* L3 */
+#define PERF_MEM_LVL_LOC_RAM	0x80  /* Local DRAM */
+#define PERF_MEM_LVL_REM_RAM1	0x100 /* Remote DRAM (1 hop) */
+#define PERF_MEM_LVL_REM_RAM2	0x200 /* Remote DRAM (2 hops) */
+#define PERF_MEM_LVL_REM_CCE1	0x400 /* Remote Cache (1 hop) */
+#define PERF_MEM_LVL_REM_CCE2	0x800 /* Remote Cache (2 hops) */
+#define PERF_MEM_LVL_IO		0x1000 /* I/O memory */
+#define PERF_MEM_LVL_UNC	0x2000 /* Uncached memory */
+#define PERF_MEM_LVL_SHIFT	5
+
+/* snoop mode */
+#define PERF_MEM_SNOOP_NA	0x01 /* not available */
+#define PERF_MEM_SNOOP_NONE	0x02 /* no snoop */
+#define PERF_MEM_SNOOP_HIT	0x04 /* snoop hit */
+#define PERF_MEM_SNOOP_MISS	0x08 /* snoop miss */
+#define PERF_MEM_SNOOP_HITM	0x10 /* snoop hit modified */
+#define PERF_MEM_SNOOP_SHIFT	19
+
+/* locked instruction */
+#define PERF_MEM_LOCK_NA	0x01 /* not available */
+#define PERF_MEM_LOCK_LOCKED	0x02 /* locked transaction */
+#define PERF_MEM_LOCK_SHIFT	24
+
+/* TLB access */
+#define PERF_MEM_TLB_NA		0x01 /* not available */
+#define PERF_MEM_TLB_HIT	0x02 /* hit level */
+#define PERF_MEM_TLB_MISS	0x04 /* miss level */
+#define PERF_MEM_TLB_L1		0x08 /* L1 */
+#define PERF_MEM_TLB_L2		0x10 /* L2 */
+#define PERF_MEM_TLB_WK		0x20 /* Hardware Walker*/
+#define PERF_MEM_TLB_OS		0x40 /* OS fault handler */
+#define PERF_MEM_TLB_SHIFT	26
+
+#define PERF_MEM_S(a, s) \
+	(((u64)PERF_MEM_##a##_##s) << PERF_MEM_##a##_SHIFT)
+
 #endif /* _UAPI_LINUX_PERF_EVENT_H */
diff --git a/include/uapi/linux/time.h b/include/uapi/linux/time.h
index 0d3c0edc3ed..e75e1b6ff27 100644
--- a/include/uapi/linux/time.h
+++ b/include/uapi/linux/time.h
@@ -54,11 +54,9 @@ struct itimerval {
 #define CLOCK_BOOTTIME			7
 #define CLOCK_REALTIME_ALARM		8
 #define CLOCK_BOOTTIME_ALARM		9
+#define CLOCK_SGI_CYCLE			10	/* Hardware specific */
+#define CLOCK_TAI			11
 
-/*
- * The IDs of various hardware clocks:
- */
-#define CLOCK_SGI_CYCLE			10
 #define MAX_CLOCKS			16
 #define CLOCKS_MASK			(CLOCK_REALTIME | CLOCK_MONOTONIC)
 #define CLOCKS_MONO			CLOCK_MONOTONIC
diff --git a/include/uapi/linux/unix_diag.h b/include/uapi/linux/unix_diag.h
index b8a24941db2..b9e2a6a7446 100644
--- a/include/uapi/linux/unix_diag.h
+++ b/include/uapi/linux/unix_diag.h
@@ -39,9 +39,11 @@ enum {
 	UNIX_DIAG_MEMINFO,
 	UNIX_DIAG_SHUTDOWN,
 
-	UNIX_DIAG_MAX,
+	__UNIX_DIAG_MAX,
 };
 
+#define UNIX_DIAG_MAX (__UNIX_DIAG_MAX - 1)
+
 struct unix_diag_vfs {
 	__u32	udiag_vfs_ino;
 	__u32	udiag_vfs_dev;
diff --git a/include/uapi/linux/usb/cdc-wdm.h b/include/uapi/linux/usb/cdc-wdm.h
new file mode 100644
index 00000000000..f03134feebd
--- /dev/null
+++ b/include/uapi/linux/usb/cdc-wdm.h
@@ -0,0 +1,21 @@
+/*
+ * USB CDC Device Management userspace API definitions
+ *
+ * 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 _UAPI__LINUX_USB_CDC_WDM_H
+#define _UAPI__LINUX_USB_CDC_WDM_H
+
+/*
+ * This IOCTL is used to retrieve the wMaxCommand for the device,
+ * defining the message limit for both reading and writing.
+ *
+ * For CDC WDM functions this will be the wMaxCommand field of the
+ * Device Management Functional Descriptor.
+ */
+#define IOCTL_WDM_MAX_COMMAND _IOR('H', 0xA0, __u16)
+
+#endif /* _UAPI__LINUX_USB_CDC_WDM_H */
diff --git a/include/uapi/linux/usb/ch9.h b/include/uapi/linux/usb/ch9.h
index f738e25377f..aa33fd1b2d4 100644
--- a/include/uapi/linux/usb/ch9.h
+++ b/include/uapi/linux/usb/ch9.h
@@ -138,7 +138,7 @@
 
 /*
  * New Feature Selectors as added by USB 3.0
- * See USB 3.0 spec Table 9-6
+ * See USB 3.0 spec Table 9-7
  */
 #define USB_DEVICE_U1_ENABLE	48	/* dev may initiate U1 transition */
 #define USB_DEVICE_U2_ENABLE	49	/* dev may initiate U2 transition */
@@ -147,7 +147,7 @@
 
 #define USB_INTR_FUNC_SUSPEND_OPT_MASK	0xFF00
 /*
- * Suspend Options, Table 9-7 USB 3.0 spec
+ * Suspend Options, Table 9-8 USB 3.0 spec
  */
 #define USB_INTRF_FUNC_SUSPEND_LP	(1 << (8 + 0))
 #define USB_INTRF_FUNC_SUSPEND_RW	(1 << (8 + 1))
diff --git a/include/uapi/linux/v4l2-controls.h b/include/uapi/linux/v4l2-controls.h
index dcd63745e83..69bd5bb0d5a 100644
--- a/include/uapi/linux/v4l2-controls.h
+++ b/include/uapi/linux/v4l2-controls.h
@@ -59,6 +59,7 @@
 #define V4L2_CTRL_CLASS_IMAGE_SOURCE	0x009e0000	/* Image source controls */
 #define V4L2_CTRL_CLASS_IMAGE_PROC	0x009f0000	/* Image processing controls */
 #define V4L2_CTRL_CLASS_DV		0x00a00000	/* Digital Video controls */
+#define V4L2_CTRL_CLASS_FM_RX		0x00a10000	/* Digital Video controls */
 
 /* User-class control IDs */
 
@@ -146,6 +147,19 @@ enum v4l2_colorfx {
  * of controls. We reserve 16 controls for this driver. */
 #define V4L2_CID_USER_MEYE_BASE			(V4L2_CID_USER_BASE + 0x1000)
 
+/* The base for the bttv driver controls.
+ * We reserve 32 controls for this driver. */
+#define V4L2_CID_USER_BTTV_BASE			(V4L2_CID_USER_BASE + 0x1010)
+
+
+/* The base for the s2255 driver controls.
+ * We reserve 16 controls for this driver. */
+#define V4L2_CID_USER_S2255_BASE		(V4L2_CID_USER_BASE + 0x1030)
+
+/* The base for the si476x driver controls. See include/media/si476x.h for the list
+ * of controls. Total of 16 controls is reserved for this driver */
+#define V4L2_CID_USER_SI476X_BASE		(V4L2_CID_USER_BASE + 0x1040)
+
 /* MPEG-class control IDs */
 
 #define V4L2_CID_MPEG_BASE 			(V4L2_CTRL_CLASS_MPEG | 0x900)
@@ -351,6 +365,7 @@ enum v4l2_mpeg_video_multi_slice_mode {
 #define V4L2_CID_MPEG_VIDEO_DEC_PTS			(V4L2_CID_MPEG_BASE+223)
 #define V4L2_CID_MPEG_VIDEO_DEC_FRAME			(V4L2_CID_MPEG_BASE+224)
 #define V4L2_CID_MPEG_VIDEO_VBV_DELAY			(V4L2_CID_MPEG_BASE+225)
+#define V4L2_CID_MPEG_VIDEO_REPEAT_SEQ_HEADER		(V4L2_CID_MPEG_BASE+226)
 
 #define V4L2_CID_MPEG_VIDEO_H263_I_FRAME_QP		(V4L2_CID_MPEG_BASE+300)
 #define V4L2_CID_MPEG_VIDEO_H263_P_FRAME_QP		(V4L2_CID_MPEG_BASE+301)
@@ -643,6 +658,7 @@ enum v4l2_exposure_metering {
 	V4L2_EXPOSURE_METERING_AVERAGE		= 0,
 	V4L2_EXPOSURE_METERING_CENTER_WEIGHTED	= 1,
 	V4L2_EXPOSURE_METERING_SPOT		= 2,
+	V4L2_EXPOSURE_METERING_MATRIX		= 3,
 };
 
 #define V4L2_CID_SCENE_MODE			(V4L2_CID_CAMERA_CLASS_BASE+26)
@@ -825,4 +841,16 @@ enum v4l2_dv_rgb_range {
 #define	V4L2_CID_DV_RX_POWER_PRESENT		(V4L2_CID_DV_CLASS_BASE + 100)
 #define V4L2_CID_DV_RX_RGB_RANGE		(V4L2_CID_DV_CLASS_BASE + 101)
 
+#define V4L2_CID_FM_RX_CLASS_BASE		(V4L2_CTRL_CLASS_FM_RX | 0x900)
+#define V4L2_CID_FM_RX_CLASS			(V4L2_CTRL_CLASS_FM_RX | 1)
+
+#define V4L2_CID_TUNE_DEEMPHASIS		(V4L2_CID_FM_RX_CLASS_BASE + 1)
+enum v4l2_deemphasis {
+	V4L2_DEEMPHASIS_DISABLED	= V4L2_PREEMPHASIS_DISABLED,
+	V4L2_DEEMPHASIS_50_uS		= V4L2_PREEMPHASIS_50_uS,
+	V4L2_DEEMPHASIS_75_uS		= V4L2_PREEMPHASIS_75_uS,
+};
+
+#define V4L2_CID_RDS_RECEPTION			(V4L2_CID_FM_RX_CLASS_BASE + 2)
+
 #endif
diff --git a/include/uapi/linux/v4l2-dv-timings.h b/include/uapi/linux/v4l2-dv-timings.h
index 9ef8172e5ed..4e0c58d25ff 100644
--- a/include/uapi/linux/v4l2-dv-timings.h
+++ b/include/uapi/linux/v4l2-dv-timings.h
@@ -42,6 +42,15 @@
 		V4L2_DV_BT_STD_DMT | V4L2_DV_BT_STD_CEA861, 0) \
 }
 
+/* Note: these are the nominal timings, for HDMI links this format is typically
+ * double-clocked to meet the minimum pixelclock requirements.  */
+#define V4L2_DV_BT_CEA_720X480I59_94 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(720, 480, 1, 0, \
+		13500000, 19, 62, 57, 4, 3, 15, 4, 3, 16, \
+		V4L2_DV_BT_STD_CEA861, V4L2_DV_FL_HALF_LINE) \
+}
+
 #define V4L2_DV_BT_CEA_720X480P59_94 { \
 	.type = V4L2_DV_BT_656_1120, \
 	V4L2_INIT_BT_TIMINGS(720, 480, 0, 0, \
@@ -49,6 +58,15 @@
 		V4L2_DV_BT_STD_CEA861, 0) \
 }
 
+/* Note: these are the nominal timings, for HDMI links this format is typically
+ * double-clocked to meet the minimum pixelclock requirements.  */
+#define V4L2_DV_BT_CEA_720X576I50 { \
+	.type = V4L2_DV_BT_656_1120, \
+	V4L2_INIT_BT_TIMINGS(720, 576, 1, 0, \
+		13500000, 12, 63, 69, 2, 3, 19, 2, 3, 20, \
+		V4L2_DV_BT_STD_CEA861, V4L2_DV_FL_HALF_LINE) \
+}
+
 #define V4L2_DV_BT_CEA_720X576P50 { \
 	.type = V4L2_DV_BT_656_1120, \
 	V4L2_INIT_BT_TIMINGS(720, 576, 0, 0, \
diff --git a/include/uapi/linux/v4l2-mediabus.h b/include/uapi/linux/v4l2-mediabus.h
index b9b7bea0453..6ee63d09b32 100644
--- a/include/uapi/linux/v4l2-mediabus.h
+++ b/include/uapi/linux/v4l2-mediabus.h
@@ -37,7 +37,7 @@
 enum v4l2_mbus_pixelcode {
 	V4L2_MBUS_FMT_FIXED = 0x0001,
 
-	/* RGB - next is 0x1009 */
+	/* RGB - next is 0x100d */
 	V4L2_MBUS_FMT_RGB444_2X8_PADHI_BE = 0x1001,
 	V4L2_MBUS_FMT_RGB444_2X8_PADHI_LE = 0x1002,
 	V4L2_MBUS_FMT_RGB555_2X8_PADHI_BE = 0x1003,
@@ -46,6 +46,10 @@ enum v4l2_mbus_pixelcode {
 	V4L2_MBUS_FMT_BGR565_2X8_LE = 0x1006,
 	V4L2_MBUS_FMT_RGB565_2X8_BE = 0x1007,
 	V4L2_MBUS_FMT_RGB565_2X8_LE = 0x1008,
+	V4L2_MBUS_FMT_RGB666_1X18 = 0x1009,
+	V4L2_MBUS_FMT_RGB888_1X24 = 0x100a,
+	V4L2_MBUS_FMT_RGB888_2X12_BE = 0x100b,
+	V4L2_MBUS_FMT_RGB888_2X12_LE = 0x100c,
 
 	/* YUV (including grey) - next is 0x2017 */
 	V4L2_MBUS_FMT_Y8_1X8 = 0x2001,
diff --git a/include/uapi/linux/videodev2.h b/include/uapi/linux/videodev2.h
index 234d1d87091..f40b41c7e10 100644
--- a/include/uapi/linux/videodev2.h
+++ b/include/uapi/linux/videodev2.h
@@ -72,27 +72,6 @@
 #define VIDEO_MAX_FRAME               32
 #define VIDEO_MAX_PLANES               8
 
-#ifndef __KERNEL__
-
-/* These defines are V4L1 specific and should not be used with the V4L2 API!
-   They will be removed from this header in the future. */
-
-#define VID_TYPE_CAPTURE	1	/* Can capture */
-#define VID_TYPE_TUNER		2	/* Can tune */
-#define VID_TYPE_TELETEXT	4	/* Does teletext */
-#define VID_TYPE_OVERLAY	8	/* Overlay onto frame buffer */
-#define VID_TYPE_CHROMAKEY	16	/* Overlay by chromakey */
-#define VID_TYPE_CLIPPING	32	/* Can clip */
-#define VID_TYPE_FRAMERAM	64	/* Uses the frame buffer memory */
-#define VID_TYPE_SCALES		128	/* Scalable */
-#define VID_TYPE_MONOCHROME	256	/* Monochrome only */
-#define VID_TYPE_SUBCAPTURE	512	/* Can capture subareas of the image */
-#define VID_TYPE_MPEG_DECODER	1024	/* Can decode MPEG streams */
-#define VID_TYPE_MPEG_ENCODER	2048	/* Can encode MPEG streams */
-#define VID_TYPE_MJPEG_DECODER	4096	/* Can decode MJPEG streams */
-#define VID_TYPE_MJPEG_ENCODER	8192	/* Can encode MJPEG streams */
-#endif
-
 /*
  *	M I S C E L L A N E O U S
  */
@@ -705,6 +684,7 @@ struct v4l2_buffer {
 #define V4L2_BUF_FLAG_TIMESTAMP_MASK		0xe000
 #define V4L2_BUF_FLAG_TIMESTAMP_UNKNOWN		0x0000
 #define V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC	0x2000
+#define V4L2_BUF_FLAG_TIMESTAMP_COPY		0x4000
 
 /**
  * struct v4l2_exportbuffer - export of video buffer as DMABUF file descriptor
@@ -980,52 +960,6 @@ 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
- */
-struct v4l2_dv_preset {
-	__u32	preset;
-	__u32	reserved[4];
-};
-
-/*
- *	D V	P R E S E T S	E N U M E R A T I O N
- */
-struct v4l2_dv_enum_preset {
-	__u32	index;
-	__u32	preset;
-	__u8	name[32]; /* Name of the preset timing */
-	__u32	width;
-	__u32	height;
-	__u32	reserved[4];
-};
-
-/*
- * 	D V	P R E S E T	V A L U E S
- */
-#define		V4L2_DV_INVALID		0
-#define		V4L2_DV_480P59_94	1 /* BT.1362 */
-#define		V4L2_DV_576P50		2 /* BT.1362 */
-#define		V4L2_DV_720P24		3 /* SMPTE 296M */
-#define		V4L2_DV_720P25		4 /* SMPTE 296M */
-#define		V4L2_DV_720P30		5 /* SMPTE 296M */
-#define		V4L2_DV_720P50		6 /* SMPTE 296M */
-#define		V4L2_DV_720P59_94	7 /* SMPTE 274M */
-#define		V4L2_DV_720P60		8 /* SMPTE 274M/296M */
-#define		V4L2_DV_1080I29_97	9 /* BT.1120/ SMPTE 274M */
-#define		V4L2_DV_1080I30		10 /* BT.1120/ SMPTE 274M */
-#define		V4L2_DV_1080I25		11 /* BT.1120 */
-#define		V4L2_DV_1080I50		12 /* SMPTE 296M */
-#define		V4L2_DV_1080I60		13 /* SMPTE 296M */
-#define		V4L2_DV_1080P24		14 /* SMPTE 296M */
-#define		V4L2_DV_1080P25		15 /* SMPTE 296M */
-#define		V4L2_DV_1080P30		16 /* SMPTE 296M */
-#define		V4L2_DV_1080P50		17 /* BT.1120 */
-#define		V4L2_DV_1080P60		18 /* BT.1120 */
-
 /*
  *	D V 	B T	T I M I N G S
  */
@@ -1119,7 +1053,7 @@ struct v4l2_bt_timings {
    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)
+#define V4L2_DV_FL_HALF_LINE			(1 << 3)
 
 
 /** struct v4l2_dv_timings - DV timings
@@ -1239,7 +1173,6 @@ struct v4l2_input {
 #define V4L2_IN_ST_VTR         0x04000000  /* VTR time constant */
 
 /* capabilities flags */
-#define V4L2_IN_CAP_PRESETS		0x00000001 /* Supports S_DV_PRESET */
 #define V4L2_IN_CAP_DV_TIMINGS		0x00000002 /* Supports S_DV_TIMINGS */
 #define V4L2_IN_CAP_CUSTOM_TIMINGS	V4L2_IN_CAP_DV_TIMINGS /* For compatibility */
 #define V4L2_IN_CAP_STD			0x00000004 /* Supports S_STD */
@@ -1263,7 +1196,6 @@ struct v4l2_output {
 #define V4L2_OUTPUT_TYPE_ANALOGVGAOVERLAY	3
 
 /* capabilities flags */
-#define V4L2_OUT_CAP_PRESETS		0x00000001 /* Supports S_DV_PRESET */
 #define V4L2_OUT_CAP_DV_TIMINGS		0x00000002 /* Supports S_DV_TIMINGS */
 #define V4L2_OUT_CAP_CUSTOM_TIMINGS	V4L2_OUT_CAP_DV_TIMINGS /* For compatibility */
 #define V4L2_OUT_CAP_STD		0x00000004 /* Supports S_STD */
@@ -1854,10 +1786,12 @@ struct v4l2_event_subscription {
 
 /* VIDIOC_DBG_G_REGISTER and VIDIOC_DBG_S_REGISTER */
 
-#define V4L2_CHIP_MATCH_HOST       0  /* Match against chip ID on host (0 for the host) */
-#define V4L2_CHIP_MATCH_I2C_DRIVER 1  /* Match against I2C driver name */
-#define V4L2_CHIP_MATCH_I2C_ADDR   2  /* Match against I2C 7-bit address */
-#define V4L2_CHIP_MATCH_AC97       3  /* Match against anciliary AC97 chip */
+#define V4L2_CHIP_MATCH_BRIDGE      0  /* Match against chip ID on the bridge (0 for the bridge) */
+#define V4L2_CHIP_MATCH_HOST V4L2_CHIP_MATCH_BRIDGE
+#define V4L2_CHIP_MATCH_I2C_DRIVER  1  /* Match against I2C driver name */
+#define V4L2_CHIP_MATCH_I2C_ADDR    2  /* Match against I2C 7-bit address */
+#define V4L2_CHIP_MATCH_AC97        3  /* Match against anciliary AC97 chip */
+#define V4L2_CHIP_MATCH_SUBDEV      4  /* Match against subdev index */
 
 struct v4l2_dbg_match {
 	__u32 type; /* Match type */
@@ -1881,6 +1815,17 @@ struct v4l2_dbg_chip_ident {
 	__u32 revision;    /* chip revision, chip specific */
 } __attribute__ ((packed));
 
+#define V4L2_CHIP_FL_READABLE (1 << 0)
+#define V4L2_CHIP_FL_WRITABLE (1 << 1)
+
+/* VIDIOC_DBG_G_CHIP_INFO */
+struct v4l2_dbg_chip_info {
+	struct v4l2_dbg_match match;
+	char name[32];
+	__u32 flags;
+	__u32 reserved[32];
+} __attribute__ ((packed));
+
 /**
  * struct v4l2_create_buffers - VIDIOC_CREATE_BUFS argument
  * @index:	on return, index of the first created buffer
@@ -1958,15 +1903,12 @@ struct v4l2_create_buffers {
 #define VIDIOC_G_EXT_CTRLS	_IOWR('V', 71, struct v4l2_ext_controls)
 #define VIDIOC_S_EXT_CTRLS	_IOWR('V', 72, struct v4l2_ext_controls)
 #define VIDIOC_TRY_EXT_CTRLS	_IOWR('V', 73, struct v4l2_ext_controls)
-#if 1
 #define VIDIOC_ENUM_FRAMESIZES	_IOWR('V', 74, struct v4l2_frmsizeenum)
 #define VIDIOC_ENUM_FRAMEINTERVALS _IOWR('V', 75, struct v4l2_frmivalenum)
 #define VIDIOC_G_ENC_INDEX       _IOR('V', 76, struct v4l2_enc_idx)
 #define VIDIOC_ENCODER_CMD      _IOWR('V', 77, struct v4l2_encoder_cmd)
 #define VIDIOC_TRY_ENCODER_CMD  _IOWR('V', 78, struct v4l2_encoder_cmd)
-#endif
 
-#if 1
 /* Experimental, meant for debugging, testing and internal use.
    Only implemented if CONFIG_VIDEO_ADV_DEBUG is defined.
    You must be root to use these ioctls. Never use these in applications! */
@@ -1974,18 +1916,13 @@ struct v4l2_create_buffers {
 #define	VIDIOC_DBG_G_REGISTER 	_IOWR('V', 80, struct v4l2_dbg_register)
 
 /* Experimental, meant for debugging, testing and internal use.
-   Never use this ioctl in applications! */
+   Never use this ioctl in applications!
+   Note: this ioctl is deprecated in favor of VIDIOC_DBG_G_CHIP_INFO and
+   will go away in the future. */
 #define VIDIOC_DBG_G_CHIP_IDENT _IOWR('V', 81, struct v4l2_dbg_chip_ident)
-#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)
-#define	VIDIOC_QUERY_DV_PRESET	_IOR('V',  86, struct v4l2_dv_preset)
 #define	VIDIOC_S_DV_TIMINGS	_IOWR('V', 87, struct v4l2_dv_timings)
 #define	VIDIOC_G_DV_TIMINGS	_IOWR('V', 88, struct v4l2_dv_timings)
 #define	VIDIOC_DQEVENT		 _IOR('V', 89, struct v4l2_event)
@@ -2016,6 +1953,10 @@ struct v4l2_create_buffers {
    versions. */
 #define VIDIOC_ENUM_FREQ_BANDS	_IOWR('V', 101, struct v4l2_frequency_band)
 
+/* Experimental, meant for debugging, testing and internal use.
+   Never use these in applications! */
+#define VIDIOC_DBG_G_CHIP_INFO  _IOWR('V', 102, struct v4l2_dbg_chip_info)
+
 /* Reminder: when adding new ioctls please add support for them to
    drivers/media/video/v4l2-compat-ioctl32.c as well! */
 
diff --git a/include/video/auo_k190xfb.h b/include/video/auo_k190xfb.h
index 609efe8c686..ac329ee1d75 100644
--- a/include/video/auo_k190xfb.h
+++ b/include/video/auo_k190xfb.h
@@ -22,6 +22,8 @@
  */
 #define AUOK190X_RESOLUTION_800_600		0
 #define AUOK190X_RESOLUTION_1024_768		1
+#define AUOK190X_RESOLUTION_600_800		4
+#define AUOK190X_RESOLUTION_768_1024		5
 
 /*
  * struct used by auok190x. board specific stuff comes from *board
@@ -98,7 +100,6 @@ struct auok190x_board {
 	int gpio_nbusy;
 
 	int resolution;
-	int rotation;
 	int quirks;
 	int fps;
 };
diff --git a/include/video/display_timing.h b/include/video/display_timing.h
index 71e9a383a98..5d0259b08e0 100644
--- a/include/video/display_timing.h
+++ b/include/video/display_timing.h
@@ -12,19 +12,22 @@
 #include <linux/bitops.h>
 #include <linux/types.h>
 
-/* VESA display monitor timing parameters */
-#define VESA_DMT_HSYNC_LOW		BIT(0)
-#define VESA_DMT_HSYNC_HIGH		BIT(1)
-#define VESA_DMT_VSYNC_LOW		BIT(2)
-#define VESA_DMT_VSYNC_HIGH		BIT(3)
-
-/* display specific flags */
-#define DISPLAY_FLAGS_DE_LOW		BIT(0)	/* data enable flag */
-#define DISPLAY_FLAGS_DE_HIGH		BIT(1)
-#define DISPLAY_FLAGS_PIXDATA_POSEDGE	BIT(2)	/* drive data on pos. edge */
-#define DISPLAY_FLAGS_PIXDATA_NEGEDGE	BIT(3)	/* drive data on neg. edge */
-#define DISPLAY_FLAGS_INTERLACED	BIT(4)
-#define DISPLAY_FLAGS_DOUBLESCAN	BIT(5)
+enum display_flags {
+	DISPLAY_FLAGS_HSYNC_LOW		= BIT(0),
+	DISPLAY_FLAGS_HSYNC_HIGH	= BIT(1),
+	DISPLAY_FLAGS_VSYNC_LOW		= BIT(2),
+	DISPLAY_FLAGS_VSYNC_HIGH	= BIT(3),
+
+	/* data enable flag */
+	DISPLAY_FLAGS_DE_LOW		= BIT(4),
+	DISPLAY_FLAGS_DE_HIGH		= BIT(5),
+	/* drive data on pos. edge */
+	DISPLAY_FLAGS_PIXDATA_POSEDGE	= BIT(6),
+	/* drive data on neg. edge */
+	DISPLAY_FLAGS_PIXDATA_NEGEDGE	= BIT(7),
+	DISPLAY_FLAGS_INTERLACED	= BIT(8),
+	DISPLAY_FLAGS_DOUBLESCAN	= BIT(9),
+};
 
 /*
  * A single signal can be specified via a range of minimal and maximal values
@@ -36,12 +39,6 @@ struct timing_entry {
 	u32 max;
 };
 
-enum timing_entry_index {
-	TE_MIN = 0,
-	TE_TYP = 1,
-	TE_MAX = 2,
-};
-
 /*
  * Single "mode" entry. This describes one set of signal timings a display can
  * have in one setting. This struct can later be converted to struct videomode
@@ -72,8 +69,7 @@ struct display_timing {
 	struct timing_entry vback_porch;	/* ver. back porch */
 	struct timing_entry vsync_len;		/* ver. sync len */
 
-	unsigned int dmt_flags;			/* VESA DMT flags */
-	unsigned int data_flags;		/* video data flags */
+	enum display_flags flags;		/* display flags */
 };
 
 /*
@@ -89,25 +85,6 @@ struct display_timings {
 	struct display_timing **timings;
 };
 
-/* get value specified by index from struct timing_entry */
-static inline u32 display_timing_get_value(const struct timing_entry *te,
-					   enum timing_entry_index index)
-{
-	switch (index) {
-	case TE_MIN:
-		return te->min;
-		break;
-	case TE_TYP:
-		return te->typ;
-		break;
-	case TE_MAX:
-		return te->max;
-		break;
-	default:
-		return te->typ;
-	}
-}
-
 /* get one entry from struct display_timings */
 static inline struct display_timing *display_timings_get(const struct
 							 display_timings *disp,
diff --git a/include/video/platform_lcd.h b/include/video/platform_lcd.h
index ad3bdfe743b..23864b28414 100644
--- a/include/video/platform_lcd.h
+++ b/include/video/platform_lcd.h
@@ -15,6 +15,7 @@ struct plat_lcd_data;
 struct fb_info;
 
 struct plat_lcd_data {
+	int	(*probe)(struct plat_lcd_data *);
 	void	(*set_power)(struct plat_lcd_data *, unsigned int power);
 	int	(*match_fb)(struct plat_lcd_data *, struct fb_info *);
 };
diff --git a/include/video/videomode.h b/include/video/videomode.h
index a42156234dd..3f1049d870d 100644
--- a/include/video/videomode.h
+++ b/include/video/videomode.h
@@ -29,20 +29,30 @@ struct videomode {
 	u32 vback_porch;
 	u32 vsync_len;
 
-	unsigned int dmt_flags;	/* VESA DMT flags */
-	unsigned int data_flags; /* video data flags */
+	enum display_flags flags; /* display flags */
 };
 
 /**
  * videomode_from_timing - convert display timing to videomode
+ * @dt: display_timing structure
+ * @vm: return value
+ *
+ * DESCRIPTION:
+ * This function converts a struct display_timing to a struct videomode.
+ */
+void videomode_from_timing(const struct display_timing *dt,
+			  struct videomode *vm);
+
+/**
+ * videomode_from_timings - convert one display timings entry to videomode
  * @disp: structure with all possible timing entries
  * @vm: return value
  * @index: index into the list of display timings in devicetree
  *
  * DESCRIPTION:
- * This function converts a struct display_timing to a struct videomode.
+ * This function converts one struct display_timing entry to a struct videomode.
  */
-int videomode_from_timing(const struct display_timings *disp,
+int videomode_from_timings(const struct display_timings *disp,
 			  struct videomode *vm, unsigned int index);
 
 #endif
diff --git a/include/xen/events.h b/include/xen/events.h
index c6bfe01acf6..b2b27c6a0f7 100644
--- a/include/xen/events.h
+++ b/include/xen/events.h
@@ -90,8 +90,7 @@ int xen_bind_pirq_gsi_to_irq(unsigned gsi,
 int xen_allocate_pirq_msi(struct pci_dev *dev, struct msi_desc *msidesc);
 /* Bind an PSI pirq to an irq. */
 int xen_bind_pirq_msi_to_irq(struct pci_dev *dev, struct msi_desc *msidesc,
-			     int pirq, int vector, const char *name,
-			     domid_t domid);
+			     int pirq, const char *name, domid_t domid);
 #endif
 
 /* De-allocates the above mentioned physical interrupt. */
diff --git a/include/xen/interface/io/blkif.h b/include/xen/interface/io/blkif.h
index 01c3d62436e..ffd4652de91 100644
--- a/include/xen/interface/io/blkif.h
+++ b/include/xen/interface/io/blkif.h
@@ -138,11 +138,21 @@ struct blkif_request_discard {
 	uint8_t        _pad3;
 } __attribute__((__packed__));
 
+struct blkif_request_other {
+	uint8_t      _pad1;
+	blkif_vdev_t _pad2;        /* only for read/write requests         */
+#ifdef CONFIG_X86_64
+	uint32_t     _pad3;        /* offsetof(blkif_req..,u.other.id)==8*/
+#endif
+	uint64_t     id;           /* private guest value, echoed in resp  */
+} __attribute__((__packed__));
+
 struct blkif_request {
 	uint8_t        operation;    /* BLKIF_OP_???                         */
 	union {
 		struct blkif_request_rw rw;
 		struct blkif_request_discard discard;
+		struct blkif_request_other other;
 	} u;
 } __attribute__((__packed__));
 
diff --git a/include/xen/interface/physdev.h b/include/xen/interface/physdev.h
index 1844d31f455..7000bb1f6e9 100644
--- a/include/xen/interface/physdev.h
+++ b/include/xen/interface/physdev.h
@@ -251,6 +251,12 @@ struct physdev_pci_device_add {
 
 #define PHYSDEVOP_pci_device_remove     26
 #define PHYSDEVOP_restore_msi_ext       27
+/*
+ * Dom0 should use these two to announce MMIO resources assigned to
+ * MSI-X capable devices won't (prepare) or may (release) change.
+ */
+#define PHYSDEVOP_prepare_msix          30
+#define PHYSDEVOP_release_msix          31
 struct physdev_pci_device {
     /* IN */
     uint16_t seg;
diff --git a/init/Kconfig b/init/Kconfig
index 5341d7232c3..4367e137900 100644
--- a/init/Kconfig
+++ b/init/Kconfig
@@ -505,6 +505,7 @@ config RCU_USER_QS
 config CONTEXT_TRACKING_FORCE
 	bool "Force context tracking"
 	depends on CONTEXT_TRACKING
+	default CONTEXT_TRACKING
 	help
 	  Probe on user/kernel boundaries by default in order to
 	  test the features that rely on it such as userspace RCU extended
@@ -578,13 +579,16 @@ config RCU_FAST_NO_HZ
 	depends on NO_HZ && SMP
 	default n
 	help
-	  This option causes RCU to attempt to accelerate grace periods in
-	  order to allow CPUs to enter dynticks-idle state more quickly.
-	  On the other hand, this option increases the overhead of the
-	  dynticks-idle checking, thus degrading scheduling latency.
+	  This option permits CPUs to enter dynticks-idle state even if
+	  they have RCU callbacks queued, and prevents RCU from waking
+	  these CPUs up more than roughly once every four jiffies (by
+	  default, you can adjust this using the rcutree.rcu_idle_gp_delay
+	  parameter), thus improving energy efficiency.  On the other
+	  hand, this option increases the duration of RCU grace periods,
+	  for example, slowing down synchronize_rcu().
 
-	  Say Y if energy efficiency is critically important, and you don't
-	  	care about real-time response.
+	  Say Y if energy efficiency is critically important, and you
+	  	don't care about increased grace-period durations.
 
 	  Say N if you are unsure.
 
@@ -651,7 +655,7 @@ config RCU_BOOST_DELAY
 	  Accept the default if unsure.
 
 config RCU_NOCB_CPU
-	bool "Offload RCU callback processing from boot-selected CPUs"
+	bool "Offload RCU callback processing from boot-selected CPUs (EXPERIMENTAL"
 	depends on TREE_RCU || TREE_PREEMPT_RCU
 	default n
 	help
@@ -662,16 +666,56 @@ config RCU_NOCB_CPU
 
 	  This option offloads callback invocation from the set of
 	  CPUs specified at boot time by the rcu_nocbs parameter.
-	  For each such CPU, a kthread ("rcuoN") will be created to
-	  invoke callbacks, where the "N" is the CPU being offloaded.
-	  Nothing prevents this kthread from running on the specified
-	  CPUs, but (1) the kthreads may be preempted between each
-	  callback, and (2) affinity or cgroups can be used to force
-	  the kthreads to run on whatever set of CPUs is desired.
-
-	  Say Y here if you want reduced OS jitter on selected CPUs.
+	  For each such CPU, a kthread ("rcuox/N") will be created to
+	  invoke callbacks, where the "N" is the CPU being offloaded,
+	  and where the "x" is "b" for RCU-bh, "p" for RCU-preempt, and
+	  "s" for RCU-sched.  Nothing prevents this kthread from running
+	  on the specified CPUs, but (1) the kthreads may be preempted
+	  between each callback, and (2) affinity or cgroups can be used
+	  to force the kthreads to run on whatever set of CPUs is desired.
+
+	  Say Y here if you want to help to debug reduced OS jitter.
 	  Say N here if you are unsure.
 
+choice
+	prompt "Build-forced no-CBs CPUs"
+	default RCU_NOCB_CPU_NONE
+	help
+	  This option allows no-CBs CPUs to be specified at build time.
+	  Additional no-CBs CPUs may be specified by the rcu_nocbs=
+	  boot parameter.
+
+config RCU_NOCB_CPU_NONE
+	bool "No build_forced no-CBs CPUs"
+	depends on RCU_NOCB_CPU
+	help
+	  This option does not force any of the CPUs to be no-CBs CPUs.
+	  Only CPUs designated by the rcu_nocbs= boot parameter will be
+	  no-CBs CPUs.
+
+config RCU_NOCB_CPU_ZERO
+	bool "CPU 0 is a build_forced no-CBs CPU"
+	depends on RCU_NOCB_CPU
+	help
+	  This option forces CPU 0 to be a no-CBs CPU.  Additional CPUs
+	  may be designated as no-CBs CPUs using the rcu_nocbs= boot
+	  parameter will be no-CBs CPUs.
+
+	  Select this if CPU 0 needs to be a no-CBs CPU for real-time
+	  or energy-efficiency reasons.
+
+config RCU_NOCB_CPU_ALL
+	bool "All CPUs are build_forced no-CBs CPUs"
+	depends on RCU_NOCB_CPU
+	help
+	  This option forces all CPUs to be no-CBs CPUs.  The rcu_nocbs=
+	  boot parameter will be ignored.
+
+	  Select this if all CPUs need to be no-CBs CPUs for real-time
+	  or energy-efficiency reasons.
+
+endchoice
+
 endmenu # "RCU Subsystem"
 
 config IKCONFIG
diff --git a/init/main.c b/init/main.c
index 63534a141b4..bea1287aecd 100644
--- a/init/main.c
+++ b/init/main.c
@@ -9,6 +9,8 @@
  *  Simplified starting of init:  Michael A. Griffith <grif@acm.org> 
  */
 
+#define DEBUG		/* Enable initcall_debug */
+
 #include <linux/types.h>
 #include <linux/module.h>
 #include <linux/proc_fs.h>
@@ -174,8 +176,8 @@ static int __init obsolete_checksetup(char *line)
 				if (line[n] == '\0' || line[n] == '=')
 					had_early_param = 1;
 			} else if (!p->setup_func) {
-				printk(KERN_WARNING "Parameter %s is obsolete,"
-				       " ignored\n", p->str);
+				pr_warn("Parameter %s is obsolete, ignored\n",
+					p->str);
 				return 1;
 			} else if (p->setup_func(line + n))
 				return 1;
@@ -384,7 +386,7 @@ static noinline void __init_refok rest_init(void)
 	init_idle_bootup_task(current);
 	schedule_preempt_disabled();
 	/* Call into cpu_idle with preempt disabled */
-	cpu_idle();
+	cpu_startup_entry(CPUHP_ONLINE);
 }
 
 /* Check for early params. */
@@ -398,8 +400,7 @@ static int __init do_early_param(char *param, char *val, const char *unused)
 		     strcmp(p->str, "earlycon") == 0)
 		) {
 			if (p->setup_func(val) != 0)
-				printk(KERN_WARNING
-				       "Malformed early option '%s'\n", param);
+				pr_warn("Malformed early option '%s'\n", param);
 		}
 	}
 	/* We accept everything at this stage. */
@@ -494,10 +495,9 @@ asmlinkage void __init start_kernel(void)
  * Interrupts are still disabled. Do necessary setups, then
  * enable them
  */
-	tick_init();
 	boot_cpu_init();
 	page_address_init();
-	printk(KERN_NOTICE "%s", linux_banner);
+	pr_notice("%s", linux_banner);
 	setup_arch(&command_line);
 	mm_init_owner(&init_mm, &init_task);
 	mm_init_cpumask(&init_mm);
@@ -509,7 +509,7 @@ asmlinkage void __init start_kernel(void)
 	build_all_zonelists(NULL, NULL);
 	page_alloc_init();
 
-	printk(KERN_NOTICE "Kernel command line: %s\n", boot_command_line);
+	pr_notice("Kernel command line: %s\n", boot_command_line);
 	parse_early_param();
 	parse_args("Booting kernel", static_command_line, __start___param,
 		   __stop___param - __start___param,
@@ -539,11 +539,8 @@ asmlinkage void __init start_kernel(void)
 	 * fragile until we cpu_idle() for the first time.
 	 */
 	preempt_disable();
-	if (!irqs_disabled()) {
-		printk(KERN_WARNING "start_kernel(): bug: interrupts were "
-				"enabled *very* early, fixing it\n");
+	if (WARN(!irqs_disabled(), "Interrupts were enabled *very* early, fixing it\n"))
 		local_irq_disable();
-	}
 	idr_init_cache();
 	perf_event_init();
 	rcu_init();
@@ -551,6 +548,7 @@ asmlinkage void __init start_kernel(void)
 	/* init some links before init_ISA_irqs() */
 	early_irq_init();
 	init_IRQ();
+	tick_init();
 	init_timers();
 	hrtimers_init();
 	softirq_init();
@@ -558,9 +556,7 @@ asmlinkage void __init start_kernel(void)
 	time_init();
 	profile_init();
 	call_function_init();
-	if (!irqs_disabled())
-		printk(KERN_CRIT "start_kernel(): bug: interrupts were "
-				 "enabled early\n");
+	WARN(!irqs_disabled(), "Interrupts were enabled early\n");
 	early_boot_irqs_disabled = false;
 	local_irq_enable();
 
@@ -587,8 +583,7 @@ asmlinkage void __init start_kernel(void)
 #ifdef CONFIG_BLK_DEV_INITRD
 	if (initrd_start && !initrd_below_start_ok &&
 	    page_to_pfn(virt_to_page((void *)initrd_start)) < min_low_pfn) {
-		printk(KERN_CRIT "initrd overwritten (0x%08lx < 0x%08lx) - "
-		    "disabling it.\n",
+		pr_crit("initrd overwritten (0x%08lx < 0x%08lx) - disabling it.\n",
 		    page_to_pfn(virt_to_page((void *)initrd_start)),
 		    min_low_pfn);
 		initrd_start = 0;
@@ -667,14 +662,14 @@ static int __init_or_module do_one_initcall_debug(initcall_t fn)
 	unsigned long long duration;
 	int ret;
 
-	printk(KERN_DEBUG "calling  %pF @ %i\n", fn, task_pid_nr(current));
+	pr_debug("calling  %pF @ %i\n", fn, task_pid_nr(current));
 	calltime = ktime_get();
 	ret = fn();
 	rettime = ktime_get();
 	delta = ktime_sub(rettime, calltime);
 	duration = (unsigned long long) ktime_to_ns(delta) >> 10;
-	printk(KERN_DEBUG "initcall %pF returned %d after %lld usecs\n", fn,
-		ret, duration);
+	pr_debug("initcall %pF returned %d after %lld usecs\n",
+		 fn, ret, duration);
 
 	return ret;
 }
@@ -702,9 +697,7 @@ int __init_or_module do_one_initcall(initcall_t fn)
 		strlcat(msgbuf, "disabled interrupts ", sizeof(msgbuf));
 		local_irq_enable();
 	}
-	if (msgbuf[0]) {
-		printk("initcall %pF returned with %s\n", fn, msgbuf);
-	}
+	WARN(msgbuf[0], "initcall %pF returned with %s\n", fn, msgbuf);
 
 	return ret;
 }
@@ -832,8 +825,7 @@ static int __ref kernel_init(void *unused)
 	if (ramdisk_execute_command) {
 		if (!run_init_process(ramdisk_execute_command))
 			return 0;
-		printk(KERN_WARNING "Failed to execute %s\n",
-				ramdisk_execute_command);
+		pr_err("Failed to execute %s\n", ramdisk_execute_command);
 	}
 
 	/*
@@ -845,8 +837,8 @@ static int __ref kernel_init(void *unused)
 	if (execute_command) {
 		if (!run_init_process(execute_command))
 			return 0;
-		printk(KERN_WARNING "Failed to execute %s.  Attempting "
-					"defaults...\n", execute_command);
+		pr_err("Failed to execute %s.  Attempting defaults...\n",
+			execute_command);
 	}
 	if (!run_init_process("/sbin/init") ||
 	    !run_init_process("/etc/init") ||
@@ -891,7 +883,7 @@ static noinline void __init kernel_init_freeable(void)
 
 	/* Open the /dev/console on the rootfs, this should never fail */
 	if (sys_open((const char __user *) "/dev/console", O_RDWR, 0) < 0)
-		printk(KERN_WARNING "Warning: unable to open an initial console.\n");
+		pr_err("Warning: unable to open an initial console.\n");
 
 	(void) sys_dup(0);
 	(void) sys_dup(0);
diff --git a/ipc/mqueue.c b/ipc/mqueue.c
index 3953fda2e8b..e4e47f64744 100644
--- a/ipc/mqueue.c
+++ b/ipc/mqueue.c
@@ -330,8 +330,16 @@ static struct dentry *mqueue_mount(struct file_system_type *fs_type,
 			 int flags, const char *dev_name,
 			 void *data)
 {
-	if (!(flags & MS_KERNMOUNT))
-		data = current->nsproxy->ipc_ns;
+	if (!(flags & MS_KERNMOUNT)) {
+		struct ipc_namespace *ns = current->nsproxy->ipc_ns;
+		/* Don't allow mounting unless the caller has CAP_SYS_ADMIN
+		 * over the ipc namespace.
+		 */
+		if (!ns_capable(ns->user_ns, CAP_SYS_ADMIN))
+			return ERR_PTR(-EPERM);
+
+		data = ns;
+	}
 	return mount_ns(fs_type, flags, data, mqueue_fill_super);
 }
 
diff --git a/ipc/msg.c b/ipc/msg.c
index 31cd1bf6af2..fede1d06ef3 100644
--- a/ipc/msg.c
+++ b/ipc/msg.c
@@ -872,6 +872,7 @@ long do_msgrcv(int msqid, void __user *buf, size_t bufsz, long msgtyp,
 							goto out_unlock;
 						break;
 					}
+					msg = ERR_PTR(-EAGAIN);
 				} else
 					break;
 				msg_counter++;
diff --git a/ipc/util.c b/ipc/util.c
index 464a8abd779..03eadd8fb0f 100644
--- a/ipc/util.c
+++ b/ipc/util.c
@@ -23,6 +23,7 @@
 #include <linux/msg.h>
 #include <linux/vmalloc.h>
 #include <linux/slab.h>
+#include <linux/notifier.h>
 #include <linux/capability.h>
 #include <linux/highuid.h>
 #include <linux/security.h>
@@ -47,19 +48,16 @@ struct ipc_proc_iface {
 	int (*show)(struct seq_file *, void *);
 };
 
-#ifdef CONFIG_MEMORY_HOTPLUG
-
 static void ipc_memory_notifier(struct work_struct *work)
 {
 	ipcns_notify(IPCNS_MEMCHANGED);
 }
 
-static DECLARE_WORK(ipc_memory_wq, ipc_memory_notifier);
-
-
 static int ipc_memory_callback(struct notifier_block *self,
 				unsigned long action, void *arg)
 {
+	static DECLARE_WORK(ipc_memory_wq, ipc_memory_notifier);
+
 	switch (action) {
 	case MEM_ONLINE:    /* memory successfully brought online */
 	case MEM_OFFLINE:   /* or offline: it's time to recompute msgmni */
@@ -85,7 +83,10 @@ static int ipc_memory_callback(struct notifier_block *self,
 	return NOTIFY_OK;
 }
 
-#endif /* CONFIG_MEMORY_HOTPLUG */
+static struct notifier_block ipc_memory_nb = {
+	.notifier_call = ipc_memory_callback,
+	.priority = IPC_CALLBACK_PRI,
+};
 
 /**
  *	ipc_init	-	initialise IPC subsystem
@@ -102,7 +103,7 @@ static int __init ipc_init(void)
 	sem_init();
 	msg_init();
 	shm_init();
-	hotplug_memory_notifier(ipc_memory_callback, IPC_CALLBACK_PRI);
+	register_hotmemory_notifier(&ipc_memory_nb);
 	register_ipcns_notifier(&init_ipc_ns);
 	return 0;
 }
diff --git a/kernel/.gitignore b/kernel/.gitignore
index ab4f1090f43..b3097bde4e9 100644
--- a/kernel/.gitignore
+++ b/kernel/.gitignore
@@ -4,3 +4,4 @@
 config_data.h
 config_data.gz
 timeconst.h
+hz.bc
diff --git a/kernel/Makefile b/kernel/Makefile
index bbde5f1a448..d1574d47cf2 100644
--- a/kernel/Makefile
+++ b/kernel/Makefile
@@ -24,6 +24,7 @@ endif
 
 obj-y += sched/
 obj-y += power/
+obj-y += cpu/
 
 obj-$(CONFIG_CHECKPOINT_RESTORE) += kcmp.o
 obj-$(CONFIG_FREEZER) += freezer.o
diff --git a/kernel/async.c b/kernel/async.c
index 8ddee2c3e5b..61f023ce022 100644
--- a/kernel/async.c
+++ b/kernel/async.c
@@ -73,7 +73,7 @@ struct async_entry {
 	struct list_head	global_list;
 	struct work_struct	work;
 	async_cookie_t		cookie;
-	async_func_ptr		*func;
+	async_func_t		func;
 	void			*data;
 	struct async_domain	*domain;
 };
@@ -84,24 +84,20 @@ static atomic_t entry_count;
 
 static async_cookie_t lowest_in_progress(struct async_domain *domain)
 {
-	struct async_entry *first = NULL;
+	struct list_head *pending;
 	async_cookie_t ret = ASYNC_COOKIE_MAX;
 	unsigned long flags;
 
 	spin_lock_irqsave(&async_lock, flags);
 
-	if (domain) {
-		if (!list_empty(&domain->pending))
-			first = list_first_entry(&domain->pending,
-					struct async_entry, domain_list);
-	} else {
-		if (!list_empty(&async_global_pending))
-			first = list_first_entry(&async_global_pending,
-					struct async_entry, global_list);
-	}
+	if (domain)
+		pending = &domain->pending;
+	else
+		pending = &async_global_pending;
 
-	if (first)
-		ret = first->cookie;
+	if (!list_empty(pending))
+		ret = list_first_entry(pending, struct async_entry,
+				       domain_list)->cookie;
 
 	spin_unlock_irqrestore(&async_lock, flags);
 	return ret;
@@ -149,7 +145,7 @@ static void async_run_entry_fn(struct work_struct *work)
 	wake_up(&async_done);
 }
 
-static async_cookie_t __async_schedule(async_func_ptr *ptr, void *data, struct async_domain *domain)
+static async_cookie_t __async_schedule(async_func_t func, void *data, struct async_domain *domain)
 {
 	struct async_entry *entry;
 	unsigned long flags;
@@ -169,13 +165,13 @@ static async_cookie_t __async_schedule(async_func_ptr *ptr, void *data, struct a
 		spin_unlock_irqrestore(&async_lock, flags);
 
 		/* low on memory.. run synchronously */
-		ptr(data, newcookie);
+		func(data, newcookie);
 		return newcookie;
 	}
 	INIT_LIST_HEAD(&entry->domain_list);
 	INIT_LIST_HEAD(&entry->global_list);
 	INIT_WORK(&entry->work, async_run_entry_fn);
-	entry->func = ptr;
+	entry->func = func;
 	entry->data = data;
 	entry->domain = domain;
 
@@ -202,21 +198,21 @@ static async_cookie_t __async_schedule(async_func_ptr *ptr, void *data, struct a
 
 /**
  * async_schedule - schedule a function for asynchronous execution
- * @ptr: function to execute asynchronously
+ * @func: function to execute asynchronously
  * @data: data pointer to pass to the function
  *
  * Returns an async_cookie_t that may be used for checkpointing later.
  * Note: This function may be called from atomic or non-atomic contexts.
  */
-async_cookie_t async_schedule(async_func_ptr *ptr, void *data)
+async_cookie_t async_schedule(async_func_t func, void *data)
 {
-	return __async_schedule(ptr, data, &async_dfl_domain);
+	return __async_schedule(func, data, &async_dfl_domain);
 }
 EXPORT_SYMBOL_GPL(async_schedule);
 
 /**
  * async_schedule_domain - schedule a function for asynchronous execution within a certain domain
- * @ptr: function to execute asynchronously
+ * @func: function to execute asynchronously
  * @data: data pointer to pass to the function
  * @domain: the domain
  *
@@ -226,10 +222,10 @@ EXPORT_SYMBOL_GPL(async_schedule);
  * synchronization domain is specified via @domain.  Note: This function
  * may be called from atomic or non-atomic contexts.
  */
-async_cookie_t async_schedule_domain(async_func_ptr *ptr, void *data,
+async_cookie_t async_schedule_domain(async_func_t func, void *data,
 				     struct async_domain *domain)
 {
-	return __async_schedule(ptr, data, domain);
+	return __async_schedule(func, data, domain);
 }
 EXPORT_SYMBOL_GPL(async_schedule_domain);
 
diff --git a/kernel/audit.c b/kernel/audit.c
index d596e5355f1..9816a1b96cf 100644
--- a/kernel/audit.c
+++ b/kernel/audit.c
@@ -660,14 +660,14 @@ static int audit_receive_msg(struct sk_buff *skb, struct nlmsghdr *nlh)
 
 	/* As soon as there's any sign of userspace auditd,
 	 * start kauditd to talk to it */
-	if (!kauditd_task)
+	if (!kauditd_task) {
 		kauditd_task = kthread_run(kauditd_thread, NULL, "kauditd");
-	if (IS_ERR(kauditd_task)) {
-		err = PTR_ERR(kauditd_task);
-		kauditd_task = NULL;
-		return err;
+		if (IS_ERR(kauditd_task)) {
+			err = PTR_ERR(kauditd_task);
+			kauditd_task = NULL;
+			return err;
+		}
 	}
-
 	loginuid = audit_get_loginuid(current);
 	sessionid = audit_get_sessionid(current);
 	security_task_getsecid(current, &sid);
diff --git a/kernel/audit.h b/kernel/audit.h
index d51cba868e1..11468d99dad 100644
--- a/kernel/audit.h
+++ b/kernel/audit.h
@@ -59,10 +59,7 @@ struct audit_entry {
 	struct audit_krule	rule;
 };
 
-#ifdef CONFIG_AUDIT
-extern int audit_enabled;
 extern int audit_ever_enabled;
-#endif
 
 extern int audit_pid;
 
diff --git a/kernel/audit_tree.c b/kernel/audit_tree.c
index 642a89c4f3d..a291aa23fb3 100644
--- a/kernel/audit_tree.c
+++ b/kernel/audit_tree.c
@@ -617,9 +617,9 @@ void audit_trim_trees(void)
 		}
 		spin_unlock(&hash_lock);
 		trim_marked(tree);
-		put_tree(tree);
 		drop_collected_mounts(root_mnt);
 skip_it:
+		put_tree(tree);
 		mutex_lock(&audit_filter_mutex);
 	}
 	list_del(&cursor);
diff --git a/kernel/auditfilter.c b/kernel/auditfilter.c
index f9fc54bbe06..267436826c3 100644
--- a/kernel/auditfilter.c
+++ b/kernel/auditfilter.c
@@ -594,6 +594,10 @@ exit_nofree:
 	return entry;
 
 exit_free:
+	if (entry->rule.watch)
+		audit_put_watch(entry->rule.watch); /* matches initial get */
+	if (entry->rule.tree)
+		audit_put_tree(entry->rule.tree); /* that's the temporary one */
 	audit_free_rule(entry);
 	return ERR_PTR(err);
 }
diff --git a/kernel/auditsc.c b/kernel/auditsc.c
index a371f857a0a..c68229411a7 100644
--- a/kernel/auditsc.c
+++ b/kernel/auditsc.c
@@ -1034,21 +1034,15 @@ static inline void audit_free_aux(struct audit_context *context)
 	}
 }
 
-static inline void audit_zero_context(struct audit_context *context,
-				      enum audit_state state)
-{
-	memset(context, 0, sizeof(*context));
-	context->state      = state;
-	context->prio = state == AUDIT_RECORD_CONTEXT ? ~0ULL : 0;
-}
-
 static inline struct audit_context *audit_alloc_context(enum audit_state state)
 {
 	struct audit_context *context;
 
-	if (!(context = kmalloc(sizeof(*context), GFP_KERNEL)))
+	context = kzalloc(sizeof(*context), GFP_KERNEL);
+	if (!context)
 		return NULL;
-	audit_zero_context(context, state);
+	context->state = state;
+	context->prio = state == AUDIT_RECORD_CONTEXT ? ~0ULL : 0;
 	INIT_LIST_HEAD(&context->killed_trees);
 	INIT_LIST_HEAD(&context->names_list);
 	return context;
diff --git a/kernel/capability.c b/kernel/capability.c
index 493d9725948..f6c2ce5701e 100644
--- a/kernel/capability.c
+++ b/kernel/capability.c
@@ -393,6 +393,30 @@ bool ns_capable(struct user_namespace *ns, int cap)
 EXPORT_SYMBOL(ns_capable);
 
 /**
+ * file_ns_capable - Determine if the file's opener had a capability in effect
+ * @file:  The file we want to check
+ * @ns:  The usernamespace we want the capability in
+ * @cap: The capability to be tested for
+ *
+ * Return true if task that opened the file had a capability in effect
+ * when the file was opened.
+ *
+ * This does not set PF_SUPERPRIV because the caller may not
+ * actually be privileged.
+ */
+bool file_ns_capable(const struct file *file, struct user_namespace *ns, int cap)
+{
+	if (WARN_ON_ONCE(!cap_valid(cap)))
+		return false;
+
+	if (security_capable(file->f_cred, ns, cap) == 0)
+		return true;
+
+	return false;
+}
+EXPORT_SYMBOL(file_ns_capable);
+
+/**
  * capable - Determine if the current task has a superior capability in effect
  * @cap: The capability to be tested for
  *
diff --git a/kernel/cgroup.c b/kernel/cgroup.c
index a32f9432666..d3abce2d645 100644
--- a/kernel/cgroup.c
+++ b/kernel/cgroup.c
@@ -30,7 +30,6 @@
 #include <linux/cred.h>
 #include <linux/ctype.h>
 #include <linux/errno.h>
-#include <linux/fs.h>
 #include <linux/init_task.h>
 #include <linux/kernel.h>
 #include <linux/list.h>
@@ -59,7 +58,7 @@
 #include <linux/vmalloc.h> /* TODO: replace with more sophisticated array */
 #include <linux/eventfd.h>
 #include <linux/poll.h>
-#include <linux/flex_array.h> /* used in cgroup_attach_proc */
+#include <linux/flex_array.h> /* used in cgroup_attach_task */
 #include <linux/kthread.h>
 
 #include <linux/atomic.h>
@@ -83,7 +82,13 @@
  * B happens only through cgroup_show_options() and using cgroup_root_mutex
  * breaks it.
  */
+#ifdef CONFIG_PROVE_RCU
+DEFINE_MUTEX(cgroup_mutex);
+EXPORT_SYMBOL_GPL(cgroup_mutex);	/* only for task_subsys_state_check() */
+#else
 static DEFINE_MUTEX(cgroup_mutex);
+#endif
+
 static DEFINE_MUTEX(cgroup_root_mutex);
 
 /*
@@ -98,56 +103,6 @@ static struct cgroup_subsys *subsys[CGROUP_SUBSYS_COUNT] = {
 #include <linux/cgroup_subsys.h>
 };
 
-#define MAX_CGROUP_ROOT_NAMELEN 64
-
-/*
- * A cgroupfs_root represents the root of a cgroup hierarchy,
- * and may be associated with a superblock to form an active
- * hierarchy
- */
-struct cgroupfs_root {
-	struct super_block *sb;
-
-	/*
-	 * The bitmask of subsystems intended to be attached to this
-	 * hierarchy
-	 */
-	unsigned long subsys_mask;
-
-	/* Unique id for this hierarchy. */
-	int hierarchy_id;
-
-	/* The bitmask of subsystems currently attached to this hierarchy */
-	unsigned long actual_subsys_mask;
-
-	/* A list running through the attached subsystems */
-	struct list_head subsys_list;
-
-	/* The root cgroup for this hierarchy */
-	struct cgroup top_cgroup;
-
-	/* Tracks how many cgroups are currently defined in hierarchy.*/
-	int number_of_cgroups;
-
-	/* A list running through the active hierarchies */
-	struct list_head root_list;
-
-	/* All cgroups on this root, cgroup_mutex protected */
-	struct list_head allcg_list;
-
-	/* Hierarchy-specific flags */
-	unsigned long flags;
-
-	/* IDs for cgroups in this hierarchy */
-	struct ida cgroup_ida;
-
-	/* The path to use for release notifications. */
-	char release_agent_path[PATH_MAX];
-
-	/* The name for this hierarchy - may be empty */
-	char name[MAX_CGROUP_ROOT_NAMELEN];
-};
-
 /*
  * The "rootnode" hierarchy is the "dummy hierarchy", reserved for the
  * subsystems that are otherwise unattached - it never has more than a
@@ -162,6 +117,9 @@ struct cfent {
 	struct list_head		node;
 	struct dentry			*dentry;
 	struct cftype			*type;
+
+	/* file xattrs */
+	struct simple_xattrs		xattrs;
 };
 
 /*
@@ -238,6 +196,8 @@ static DEFINE_SPINLOCK(hierarchy_id_lock);
 /* dummytop is a shorthand for the dummy hierarchy's top cgroup */
 #define dummytop (&rootnode.top_cgroup)
 
+static struct cgroup_name root_cgroup_name = { .name = "/" };
+
 /* This flag indicates whether tasks in the fork and exit paths should
  * check for fork/exit handlers to call. This avoids us having to do
  * extra work in the fork/exit path if none of the subsystems need to
@@ -249,20 +209,6 @@ static int cgroup_destroy_locked(struct cgroup *cgrp);
 static int cgroup_addrm_files(struct cgroup *cgrp, struct cgroup_subsys *subsys,
 			      struct cftype cfts[], bool is_add);
 
-#ifdef CONFIG_PROVE_LOCKING
-int cgroup_lock_is_held(void)
-{
-	return lockdep_is_held(&cgroup_mutex);
-}
-#else /* #ifdef CONFIG_PROVE_LOCKING */
-int cgroup_lock_is_held(void)
-{
-	return mutex_is_locked(&cgroup_mutex);
-}
-#endif /* #else #ifdef CONFIG_PROVE_LOCKING */
-
-EXPORT_SYMBOL_GPL(cgroup_lock_is_held);
-
 static int css_unbias_refcnt(int refcnt)
 {
 	return refcnt >= 0 ? refcnt : refcnt - CSS_DEACT_BIAS;
@@ -282,11 +228,25 @@ inline int cgroup_is_removed(const struct cgroup *cgrp)
 	return test_bit(CGRP_REMOVED, &cgrp->flags);
 }
 
-/* bits in struct cgroupfs_root flags field */
-enum {
-	ROOT_NOPREFIX,	/* mounted subsystems have no named prefix */
-	ROOT_XATTR,	/* supports extended attributes */
-};
+/**
+ * cgroup_is_descendant - test ancestry
+ * @cgrp: the cgroup to be tested
+ * @ancestor: possible ancestor of @cgrp
+ *
+ * Test whether @cgrp is a descendant of @ancestor.  It also returns %true
+ * if @cgrp == @ancestor.  This function is safe to call as long as @cgrp
+ * and @ancestor are accessible.
+ */
+bool cgroup_is_descendant(struct cgroup *cgrp, struct cgroup *ancestor)
+{
+	while (cgrp) {
+		if (cgrp == ancestor)
+			return true;
+		cgrp = cgrp->parent;
+	}
+	return false;
+}
+EXPORT_SYMBOL_GPL(cgroup_is_descendant);
 
 static int cgroup_is_releasable(const struct cgroup *cgrp)
 {
@@ -327,6 +287,23 @@ static inline struct cftype *__d_cft(struct dentry *dentry)
 	return __d_cfe(dentry)->type;
 }
 
+/**
+ * cgroup_lock_live_group - take cgroup_mutex and check that cgrp is alive.
+ * @cgrp: the cgroup to be checked for liveness
+ *
+ * On success, returns true; the mutex should be later unlocked.  On
+ * failure returns false with no lock held.
+ */
+static bool cgroup_lock_live_group(struct cgroup *cgrp)
+{
+	mutex_lock(&cgroup_mutex);
+	if (cgroup_is_removed(cgrp)) {
+		mutex_unlock(&cgroup_mutex);
+		return false;
+	}
+	return true;
+}
+
 /* the list of cgroups eligible for automatic release. Protected by
  * release_list_lock */
 static LIST_HEAD(release_list);
@@ -800,27 +777,6 @@ static struct cgroup *task_cgroup_from_root(struct task_struct *task,
  * update of a tasks cgroup pointer by cgroup_attach_task()
  */
 
-/**
- * cgroup_lock - lock out any changes to cgroup structures
- *
- */
-void cgroup_lock(void)
-{
-	mutex_lock(&cgroup_mutex);
-}
-EXPORT_SYMBOL_GPL(cgroup_lock);
-
-/**
- * cgroup_unlock - release lock on cgroup changes
- *
- * Undo the lock taken in a previous cgroup_lock() call.
- */
-void cgroup_unlock(void)
-{
-	mutex_unlock(&cgroup_mutex);
-}
-EXPORT_SYMBOL_GPL(cgroup_unlock);
-
 /*
  * A couple of forward declarations required, due to cyclic reference loop:
  * cgroup_mkdir -> cgroup_create -> cgroup_populate_dir ->
@@ -859,6 +815,17 @@ static struct inode *cgroup_new_inode(umode_t mode, struct super_block *sb)
 	return inode;
 }
 
+static struct cgroup_name *cgroup_alloc_name(struct dentry *dentry)
+{
+	struct cgroup_name *name;
+
+	name = kmalloc(sizeof(*name) + dentry->d_name.len + 1, GFP_KERNEL);
+	if (!name)
+		return NULL;
+	strcpy(name->name, dentry->d_name.name);
+	return name;
+}
+
 static void cgroup_free_fn(struct work_struct *work)
 {
 	struct cgroup *cgrp = container_of(work, struct cgroup, free_work);
@@ -875,8 +842,18 @@ static void cgroup_free_fn(struct work_struct *work)
 	mutex_unlock(&cgroup_mutex);
 
 	/*
+	 * We get a ref to the parent's dentry, and put the ref when
+	 * this cgroup is being freed, so it's guaranteed that the
+	 * parent won't be destroyed before its children.
+	 */
+	dput(cgrp->parent->dentry);
+
+	ida_simple_remove(&cgrp->root->cgroup_ida, cgrp->id);
+
+	/*
 	 * Drop the active superblock reference that we took when we
-	 * created the cgroup
+	 * created the cgroup. This will free cgrp->root, if we are
+	 * holding the last reference to @sb.
 	 */
 	deactivate_super(cgrp->root->sb);
 
@@ -888,7 +865,7 @@ static void cgroup_free_fn(struct work_struct *work)
 
 	simple_xattrs_free(&cgrp->xattrs);
 
-	ida_simple_remove(&cgrp->root->cgroup_ida, cgrp->id);
+	kfree(rcu_dereference_raw(cgrp->name));
 	kfree(cgrp);
 }
 
@@ -910,13 +887,12 @@ static void cgroup_diput(struct dentry *dentry, struct inode *inode)
 	} else {
 		struct cfent *cfe = __d_cfe(dentry);
 		struct cgroup *cgrp = dentry->d_parent->d_fsdata;
-		struct cftype *cft = cfe->type;
 
 		WARN_ONCE(!list_empty(&cfe->node) &&
 			  cgrp != &cgrp->root->top_cgroup,
 			  "cfe still linked for %s\n", cfe->type->name);
+		simple_xattrs_free(&cfe->xattrs);
 		kfree(cfe);
-		simple_xattrs_free(&cft->xattrs);
 	}
 	iput(inode);
 }
@@ -1108,9 +1084,11 @@ static int cgroup_show_options(struct seq_file *seq, struct dentry *dentry)
 	mutex_lock(&cgroup_root_mutex);
 	for_each_subsys(root, ss)
 		seq_printf(seq, ",%s", ss->name);
-	if (test_bit(ROOT_NOPREFIX, &root->flags))
+	if (root->flags & CGRP_ROOT_SANE_BEHAVIOR)
+		seq_puts(seq, ",sane_behavior");
+	if (root->flags & CGRP_ROOT_NOPREFIX)
 		seq_puts(seq, ",noprefix");
-	if (test_bit(ROOT_XATTR, &root->flags))
+	if (root->flags & CGRP_ROOT_XATTR)
 		seq_puts(seq, ",xattr");
 	if (strlen(root->release_agent_path))
 		seq_printf(seq, ",release_agent=%s", root->release_agent_path);
@@ -1172,8 +1150,12 @@ static int parse_cgroupfs_options(char *data, struct cgroup_sb_opts *opts)
 			all_ss = true;
 			continue;
 		}
+		if (!strcmp(token, "__DEVEL__sane_behavior")) {
+			opts->flags |= CGRP_ROOT_SANE_BEHAVIOR;
+			continue;
+		}
 		if (!strcmp(token, "noprefix")) {
-			set_bit(ROOT_NOPREFIX, &opts->flags);
+			opts->flags |= CGRP_ROOT_NOPREFIX;
 			continue;
 		}
 		if (!strcmp(token, "clone_children")) {
@@ -1181,7 +1163,7 @@ static int parse_cgroupfs_options(char *data, struct cgroup_sb_opts *opts)
 			continue;
 		}
 		if (!strcmp(token, "xattr")) {
-			set_bit(ROOT_XATTR, &opts->flags);
+			opts->flags |= CGRP_ROOT_XATTR;
 			continue;
 		}
 		if (!strncmp(token, "release_agent=", 14)) {
@@ -1259,13 +1241,26 @@ static int parse_cgroupfs_options(char *data, struct cgroup_sb_opts *opts)
 
 	/* Consistency checks */
 
+	if (opts->flags & CGRP_ROOT_SANE_BEHAVIOR) {
+		pr_warning("cgroup: sane_behavior: this is still under development and its behaviors will change, proceed at your own risk\n");
+
+		if (opts->flags & CGRP_ROOT_NOPREFIX) {
+			pr_err("cgroup: sane_behavior: noprefix is not allowed\n");
+			return -EINVAL;
+		}
+
+		if (opts->cpuset_clone_children) {
+			pr_err("cgroup: sane_behavior: clone_children is not allowed\n");
+			return -EINVAL;
+		}
+	}
+
 	/*
 	 * Option noprefix was introduced just for backward compatibility
 	 * with the old cpuset, so we allow noprefix only if mounting just
 	 * the cpuset subsystem.
 	 */
-	if (test_bit(ROOT_NOPREFIX, &opts->flags) &&
-	    (opts->subsys_mask & mask))
+	if ((opts->flags & CGRP_ROOT_NOPREFIX) && (opts->subsys_mask & mask))
 		return -EINVAL;
 
 
@@ -1336,6 +1331,11 @@ static int cgroup_remount(struct super_block *sb, int *flags, char *data)
 	struct cgroup_sb_opts opts;
 	unsigned long added_mask, removed_mask;
 
+	if (root->flags & CGRP_ROOT_SANE_BEHAVIOR) {
+		pr_err("cgroup: sane_behavior: remount is not allowed\n");
+		return -EINVAL;
+	}
+
 	mutex_lock(&cgrp->dentry->d_inode->i_mutex);
 	mutex_lock(&cgroup_mutex);
 	mutex_lock(&cgroup_root_mutex);
@@ -1421,7 +1421,7 @@ static void init_cgroup_root(struct cgroupfs_root *root)
 	INIT_LIST_HEAD(&root->allcg_list);
 	root->number_of_cgroups = 1;
 	cgrp->root = root;
-	cgrp->top_cgroup = cgrp;
+	cgrp->name = &root_cgroup_name;
 	init_cgroup_housekeeping(cgrp);
 	list_add_tail(&cgrp->allcg_node, &root->allcg_list);
 }
@@ -1685,6 +1685,14 @@ static struct dentry *cgroup_mount(struct file_system_type *fs_type,
 		 * any) is not needed
 		 */
 		cgroup_drop_root(opts.new_root);
+
+		if (((root->flags | opts.flags) & CGRP_ROOT_SANE_BEHAVIOR) &&
+		    root->flags != opts.flags) {
+			pr_err("cgroup: sane_behavior: new mount options should match the existing superblock\n");
+			ret = -EINVAL;
+			goto drop_new_super;
+		}
+
 		/* no subsys rebinding, so refcounts don't change */
 		drop_parsed_module_refcounts(opts.subsys_mask);
 	}
@@ -1769,49 +1777,48 @@ static struct kobject *cgroup_kobj;
  * @buf: the buffer to write the path into
  * @buflen: the length of the buffer
  *
- * Called with cgroup_mutex held or else with an RCU-protected cgroup
- * reference.  Writes path of cgroup into buf.  Returns 0 on success,
- * -errno on error.
+ * Writes path of cgroup into buf.  Returns 0 on success, -errno on error.
+ *
+ * We can't generate cgroup path using dentry->d_name, as accessing
+ * dentry->name must be protected by irq-unsafe dentry->d_lock or parent
+ * inode's i_mutex, while on the other hand cgroup_path() can be called
+ * with some irq-safe spinlocks held.
  */
 int cgroup_path(const struct cgroup *cgrp, char *buf, int buflen)
 {
-	struct dentry *dentry = cgrp->dentry;
+	int ret = -ENAMETOOLONG;
 	char *start;
 
-	rcu_lockdep_assert(rcu_read_lock_held() || cgroup_lock_is_held(),
-			   "cgroup_path() called without proper locking");
-
-	if (cgrp == dummytop) {
-		/*
-		 * Inactive subsystems have no dentry for their root
-		 * cgroup
-		 */
-		strcpy(buf, "/");
+	if (!cgrp->parent) {
+		if (strlcpy(buf, "/", buflen) >= buflen)
+			return -ENAMETOOLONG;
 		return 0;
 	}
 
 	start = buf + buflen - 1;
-
 	*start = '\0';
-	for (;;) {
-		int len = dentry->d_name.len;
 
+	rcu_read_lock();
+	do {
+		const char *name = cgroup_name(cgrp);
+		int len;
+
+		len = strlen(name);
 		if ((start -= len) < buf)
-			return -ENAMETOOLONG;
-		memcpy(start, dentry->d_name.name, len);
-		cgrp = cgrp->parent;
-		if (!cgrp)
-			break;
+			goto out;
+		memcpy(start, name, len);
 
-		dentry = cgrp->dentry;
-		if (!cgrp->parent)
-			continue;
 		if (--start < buf)
-			return -ENAMETOOLONG;
+			goto out;
 		*start = '/';
-	}
+
+		cgrp = cgrp->parent;
+	} while (cgrp->parent);
+	ret = 0;
 	memmove(buf, start, buf + buflen - start);
-	return 0;
+out:
+	rcu_read_unlock();
+	return ret;
 }
 EXPORT_SYMBOL_GPL(cgroup_path);
 
@@ -1900,7 +1907,7 @@ EXPORT_SYMBOL_GPL(cgroup_taskset_size);
  *
  * Must be called with cgroup_mutex and threadgroup locked.
  */
-static void cgroup_task_migrate(struct cgroup *cgrp, struct cgroup *oldcgrp,
+static void cgroup_task_migrate(struct cgroup *oldcgrp,
 				struct task_struct *tsk, struct css_set *newcg)
 {
 	struct css_set *oldcg;
@@ -1933,121 +1940,22 @@ static void cgroup_task_migrate(struct cgroup *cgrp, struct cgroup *oldcgrp,
 }
 
 /**
- * cgroup_attach_task - attach task 'tsk' to cgroup 'cgrp'
- * @cgrp: the cgroup the task is attaching to
- * @tsk: the task to be attached
- *
- * Call with cgroup_mutex and threadgroup locked. May take task_lock of
- * @tsk during call.
- */
-int cgroup_attach_task(struct cgroup *cgrp, struct task_struct *tsk)
-{
-	int retval = 0;
-	struct cgroup_subsys *ss, *failed_ss = NULL;
-	struct cgroup *oldcgrp;
-	struct cgroupfs_root *root = cgrp->root;
-	struct cgroup_taskset tset = { };
-	struct css_set *newcg;
-
-	/* @tsk either already exited or can't exit until the end */
-	if (tsk->flags & PF_EXITING)
-		return -ESRCH;
-
-	/* Nothing to do if the task is already in that cgroup */
-	oldcgrp = task_cgroup_from_root(tsk, root);
-	if (cgrp == oldcgrp)
-		return 0;
-
-	tset.single.task = tsk;
-	tset.single.cgrp = oldcgrp;
-
-	for_each_subsys(root, ss) {
-		if (ss->can_attach) {
-			retval = ss->can_attach(cgrp, &tset);
-			if (retval) {
-				/*
-				 * Remember on which subsystem the can_attach()
-				 * failed, so that we only call cancel_attach()
-				 * against the subsystems whose can_attach()
-				 * succeeded. (See below)
-				 */
-				failed_ss = ss;
-				goto out;
-			}
-		}
-	}
-
-	newcg = find_css_set(tsk->cgroups, cgrp);
-	if (!newcg) {
-		retval = -ENOMEM;
-		goto out;
-	}
-
-	cgroup_task_migrate(cgrp, oldcgrp, tsk, newcg);
-
-	for_each_subsys(root, ss) {
-		if (ss->attach)
-			ss->attach(cgrp, &tset);
-	}
-
-out:
-	if (retval) {
-		for_each_subsys(root, ss) {
-			if (ss == failed_ss)
-				/*
-				 * This subsystem was the one that failed the
-				 * can_attach() check earlier, so we don't need
-				 * to call cancel_attach() against it or any
-				 * remaining subsystems.
-				 */
-				break;
-			if (ss->cancel_attach)
-				ss->cancel_attach(cgrp, &tset);
-		}
-	}
-	return retval;
-}
-
-/**
- * cgroup_attach_task_all - attach task 'tsk' to all cgroups of task 'from'
- * @from: attach to all cgroups of a given task
- * @tsk: the task to be attached
- */
-int cgroup_attach_task_all(struct task_struct *from, struct task_struct *tsk)
-{
-	struct cgroupfs_root *root;
-	int retval = 0;
-
-	cgroup_lock();
-	for_each_active_root(root) {
-		struct cgroup *from_cg = task_cgroup_from_root(from, root);
-
-		retval = cgroup_attach_task(from_cg, tsk);
-		if (retval)
-			break;
-	}
-	cgroup_unlock();
-
-	return retval;
-}
-EXPORT_SYMBOL_GPL(cgroup_attach_task_all);
-
-/**
- * cgroup_attach_proc - attach all threads in a threadgroup to a cgroup
+ * cgroup_attach_task - attach a task or a whole threadgroup to a cgroup
  * @cgrp: the cgroup to attach to
- * @leader: the threadgroup leader task_struct of the group to be attached
+ * @tsk: the task or the leader of the threadgroup to be attached
+ * @threadgroup: attach the whole threadgroup?
  *
  * Call holding cgroup_mutex and the group_rwsem of the leader. Will take
- * task_lock of each thread in leader's threadgroup individually in turn.
+ * task_lock of @tsk or each thread in the threadgroup individually in turn.
  */
-static int cgroup_attach_proc(struct cgroup *cgrp, struct task_struct *leader)
+static int cgroup_attach_task(struct cgroup *cgrp, struct task_struct *tsk,
+			      bool threadgroup)
 {
 	int retval, i, group_size;
 	struct cgroup_subsys *ss, *failed_ss = NULL;
-	/* guaranteed to be initialized later, but the compiler needs this */
 	struct cgroupfs_root *root = cgrp->root;
 	/* threadgroup list cursor and array */
-	struct task_struct *tsk;
+	struct task_struct *leader = tsk;
 	struct task_and_cgroup *tc;
 	struct flex_array *group;
 	struct cgroup_taskset tset = { };
@@ -2059,17 +1967,19 @@ static int cgroup_attach_proc(struct cgroup *cgrp, struct task_struct *leader)
 	 * group - group_rwsem prevents new threads from appearing, and if
 	 * threads exit, this will just be an over-estimate.
 	 */
-	group_size = get_nr_threads(leader);
+	if (threadgroup)
+		group_size = get_nr_threads(tsk);
+	else
+		group_size = 1;
 	/* flex_array supports very large thread-groups better than kmalloc. */
 	group = flex_array_alloc(sizeof(*tc), group_size, GFP_KERNEL);
 	if (!group)
 		return -ENOMEM;
 	/* pre-allocate to guarantee space while iterating in rcu read-side. */
-	retval = flex_array_prealloc(group, 0, group_size - 1, GFP_KERNEL);
+	retval = flex_array_prealloc(group, 0, group_size, GFP_KERNEL);
 	if (retval)
 		goto out_free_group_list;
 
-	tsk = leader;
 	i = 0;
 	/*
 	 * Prevent freeing of tasks while we take a snapshot. Tasks that are
@@ -2098,6 +2008,9 @@ static int cgroup_attach_proc(struct cgroup *cgrp, struct task_struct *leader)
 		retval = flex_array_put(group, i, &ent, GFP_ATOMIC);
 		BUG_ON(retval != 0);
 		i++;
+
+		if (!threadgroup)
+			break;
 	} while_each_thread(leader, tsk);
 	rcu_read_unlock();
 	/* remember the number of threads in the array for later. */
@@ -2143,7 +2056,7 @@ static int cgroup_attach_proc(struct cgroup *cgrp, struct task_struct *leader)
 	 */
 	for (i = 0; i < group_size; i++) {
 		tc = flex_array_get(group, i);
-		cgroup_task_migrate(cgrp, tc->cgrp, tc->task, tc->cg);
+		cgroup_task_migrate(tc->cgrp, tc->task, tc->cg);
 	}
 	/* nothing is sensitive to fork() after this point. */
 
@@ -2224,11 +2137,11 @@ retry_find_task:
 		tsk = tsk->group_leader;
 
 	/*
-	 * Workqueue threads may acquire PF_THREAD_BOUND and become
+	 * Workqueue threads may acquire PF_NO_SETAFFINITY and become
 	 * trapped in a cpuset, or RT worker may be born in a cgroup
 	 * with no rt_runtime allocated.  Just say no.
 	 */
-	if (tsk == kthreadd_task || (tsk->flags & PF_THREAD_BOUND)) {
+	if (tsk == kthreadd_task || (tsk->flags & PF_NO_SETAFFINITY)) {
 		ret = -EINVAL;
 		rcu_read_unlock();
 		goto out_unlock_cgroup;
@@ -2251,17 +2164,42 @@ retry_find_task:
 			put_task_struct(tsk);
 			goto retry_find_task;
 		}
-		ret = cgroup_attach_proc(cgrp, tsk);
-	} else
-		ret = cgroup_attach_task(cgrp, tsk);
+	}
+
+	ret = cgroup_attach_task(cgrp, tsk, threadgroup);
+
 	threadgroup_unlock(tsk);
 
 	put_task_struct(tsk);
 out_unlock_cgroup:
-	cgroup_unlock();
+	mutex_unlock(&cgroup_mutex);
 	return ret;
 }
 
+/**
+ * cgroup_attach_task_all - attach task 'tsk' to all cgroups of task 'from'
+ * @from: attach to all cgroups of a given task
+ * @tsk: the task to be attached
+ */
+int cgroup_attach_task_all(struct task_struct *from, struct task_struct *tsk)
+{
+	struct cgroupfs_root *root;
+	int retval = 0;
+
+	mutex_lock(&cgroup_mutex);
+	for_each_active_root(root) {
+		struct cgroup *from_cg = task_cgroup_from_root(from, root);
+
+		retval = cgroup_attach_task(from_cg, tsk, false);
+		if (retval)
+			break;
+	}
+	mutex_unlock(&cgroup_mutex);
+
+	return retval;
+}
+EXPORT_SYMBOL_GPL(cgroup_attach_task_all);
+
 static int cgroup_tasks_write(struct cgroup *cgrp, struct cftype *cft, u64 pid)
 {
 	return attach_task_by_pid(cgrp, pid, false);
@@ -2272,24 +2210,6 @@ static int cgroup_procs_write(struct cgroup *cgrp, struct cftype *cft, u64 tgid)
 	return attach_task_by_pid(cgrp, tgid, true);
 }
 
-/**
- * cgroup_lock_live_group - take cgroup_mutex and check that cgrp is alive.
- * @cgrp: the cgroup to be checked for liveness
- *
- * On success, returns true; the lock should be later released with
- * cgroup_unlock(). On failure returns false with no lock held.
- */
-bool cgroup_lock_live_group(struct cgroup *cgrp)
-{
-	mutex_lock(&cgroup_mutex);
-	if (cgroup_is_removed(cgrp)) {
-		mutex_unlock(&cgroup_mutex);
-		return false;
-	}
-	return true;
-}
-EXPORT_SYMBOL_GPL(cgroup_lock_live_group);
-
 static int cgroup_release_agent_write(struct cgroup *cgrp, struct cftype *cft,
 				      const char *buffer)
 {
@@ -2301,7 +2221,7 @@ static int cgroup_release_agent_write(struct cgroup *cgrp, struct cftype *cft,
 	mutex_lock(&cgroup_root_mutex);
 	strcpy(cgrp->root->release_agent_path, buffer);
 	mutex_unlock(&cgroup_root_mutex);
-	cgroup_unlock();
+	mutex_unlock(&cgroup_mutex);
 	return 0;
 }
 
@@ -2312,7 +2232,14 @@ static int cgroup_release_agent_show(struct cgroup *cgrp, struct cftype *cft,
 		return -ENODEV;
 	seq_puts(seq, cgrp->root->release_agent_path);
 	seq_putc(seq, '\n');
-	cgroup_unlock();
+	mutex_unlock(&cgroup_mutex);
+	return 0;
+}
+
+static int cgroup_sane_behavior_show(struct cgroup *cgrp, struct cftype *cft,
+				     struct seq_file *seq)
+{
+	seq_printf(seq, "%d\n", cgroup_sane_behavior(cgrp));
 	return 0;
 }
 
@@ -2537,13 +2464,40 @@ static int cgroup_file_release(struct inode *inode, struct file *file)
 static int cgroup_rename(struct inode *old_dir, struct dentry *old_dentry,
 			    struct inode *new_dir, struct dentry *new_dentry)
 {
+	int ret;
+	struct cgroup_name *name, *old_name;
+	struct cgroup *cgrp;
+
+	/*
+	 * It's convinient to use parent dir's i_mutex to protected
+	 * cgrp->name.
+	 */
+	lockdep_assert_held(&old_dir->i_mutex);
+
 	if (!S_ISDIR(old_dentry->d_inode->i_mode))
 		return -ENOTDIR;
 	if (new_dentry->d_inode)
 		return -EEXIST;
 	if (old_dir != new_dir)
 		return -EIO;
-	return simple_rename(old_dir, old_dentry, new_dir, new_dentry);
+
+	cgrp = __d_cgrp(old_dentry);
+
+	name = cgroup_alloc_name(new_dentry);
+	if (!name)
+		return -ENOMEM;
+
+	ret = simple_rename(old_dir, old_dentry, new_dir, new_dentry);
+	if (ret) {
+		kfree(name);
+		return ret;
+	}
+
+	old_name = cgrp->name;
+	rcu_assign_pointer(cgrp->name, name);
+
+	kfree_rcu(old_name, rcu_head);
+	return 0;
 }
 
 static struct simple_xattrs *__d_xattrs(struct dentry *dentry)
@@ -2551,13 +2505,13 @@ static struct simple_xattrs *__d_xattrs(struct dentry *dentry)
 	if (S_ISDIR(dentry->d_inode->i_mode))
 		return &__d_cgrp(dentry)->xattrs;
 	else
-		return &__d_cft(dentry)->xattrs;
+		return &__d_cfe(dentry)->xattrs;
 }
 
 static inline int xattr_enabled(struct dentry *dentry)
 {
 	struct cgroupfs_root *root = dentry->d_sb->s_fs_info;
-	return test_bit(ROOT_XATTR, &root->flags);
+	return root->flags & CGRP_ROOT_XATTR;
 }
 
 static bool is_valid_xattr(const char *name)
@@ -2727,9 +2681,7 @@ static int cgroup_add_file(struct cgroup *cgrp, struct cgroup_subsys *subsys,
 	umode_t mode;
 	char name[MAX_CGROUP_TYPE_NAMELEN + MAX_CFTYPE_NAME + 2] = { 0 };
 
-	simple_xattrs_init(&cft->xattrs);
-
-	if (subsys && !test_bit(ROOT_NOPREFIX, &cgrp->root->flags)) {
+	if (subsys && !(cgrp->root->flags & CGRP_ROOT_NOPREFIX)) {
 		strcpy(name, subsys->name);
 		strcat(name, ".");
 	}
@@ -2753,6 +2705,7 @@ static int cgroup_add_file(struct cgroup *cgrp, struct cgroup_subsys *subsys,
 		cfe->type = (void *)cft;
 		cfe->dentry = dentry;
 		dentry->d_fsdata = cfe;
+		simple_xattrs_init(&cfe->xattrs);
 		list_add_tail(&cfe->node, &parent->files);
 		cfe = NULL;
 	}
@@ -2770,6 +2723,8 @@ static int cgroup_addrm_files(struct cgroup *cgrp, struct cgroup_subsys *subsys,
 
 	for (cft = cfts; cft->name[0] != '\0'; cft++) {
 		/* does cft->flags tell us to skip this file on @cgrp? */
+		if ((cft->flags & CFTYPE_INSANE) && cgroup_sane_behavior(cgrp))
+			continue;
 		if ((cft->flags & CFTYPE_NOT_ON_ROOT) && !cgrp->parent)
 			continue;
 		if ((cft->flags & CFTYPE_ONLY_ON_ROOT) && cgrp->parent)
@@ -3300,6 +3255,34 @@ int cgroup_scan_tasks(struct cgroup_scanner *scan)
 	return 0;
 }
 
+static void cgroup_transfer_one_task(struct task_struct *task,
+				     struct cgroup_scanner *scan)
+{
+	struct cgroup *new_cgroup = scan->data;
+
+	mutex_lock(&cgroup_mutex);
+	cgroup_attach_task(new_cgroup, task, false);
+	mutex_unlock(&cgroup_mutex);
+}
+
+/**
+ * cgroup_trasnsfer_tasks - move tasks from one cgroup to another
+ * @to: cgroup to which the tasks will be moved
+ * @from: cgroup in which the tasks currently reside
+ */
+int cgroup_transfer_tasks(struct cgroup *to, struct cgroup *from)
+{
+	struct cgroup_scanner scan;
+
+	scan.cg = from;
+	scan.test_task = NULL; /* select all tasks in cgroup */
+	scan.process_task = cgroup_transfer_one_task;
+	scan.heap = NULL;
+	scan.data = to;
+
+	return cgroup_scan_tasks(&scan);
+}
+
 /*
  * Stuff for reading the 'tasks'/'procs' files.
  *
@@ -3362,35 +3345,14 @@ static void pidlist_free(void *p)
 	else
 		kfree(p);
 }
-static void *pidlist_resize(void *p, int newcount)
-{
-	void *newlist;
-	/* note: if new alloc fails, old p will still be valid either way */
-	if (is_vmalloc_addr(p)) {
-		newlist = vmalloc(newcount * sizeof(pid_t));
-		if (!newlist)
-			return NULL;
-		memcpy(newlist, p, newcount * sizeof(pid_t));
-		vfree(p);
-	} else {
-		newlist = krealloc(p, newcount * sizeof(pid_t), GFP_KERNEL);
-	}
-	return newlist;
-}
 
 /*
  * pidlist_uniq - given a kmalloc()ed list, strip out all duplicate entries
- * If the new stripped list is sufficiently smaller and there's enough memory
- * to allocate a new buffer, will let go of the unneeded memory. Returns the
- * number of unique elements.
+ * Returns the number of unique elements.
  */
-/* is the size difference enough that we should re-allocate the array? */
-#define PIDLIST_REALLOC_DIFFERENCE(old, new) ((old) - PAGE_SIZE >= (new))
-static int pidlist_uniq(pid_t **p, int length)
+static int pidlist_uniq(pid_t *list, int length)
 {
 	int src, dest = 1;
-	pid_t *list = *p;
-	pid_t *newlist;
 
 	/*
 	 * we presume the 0th element is unique, so i starts at 1. trivial
@@ -3411,16 +3373,6 @@ static int pidlist_uniq(pid_t **p, int length)
 		dest++;
 	}
 after:
-	/*
-	 * if the length difference is large enough, we want to allocate a
-	 * smaller buffer to save memory. if this fails due to out of memory,
-	 * we'll just stay with what we've got.
-	 */
-	if (PIDLIST_REALLOC_DIFFERENCE(length, dest)) {
-		newlist = pidlist_resize(list, dest);
-		if (newlist)
-			*p = newlist;
-	}
 	return dest;
 }
 
@@ -3516,7 +3468,7 @@ static int pidlist_array_load(struct cgroup *cgrp, enum cgroup_filetype type,
 	/* now sort & (if procs) strip out duplicates */
 	sort(array, length, sizeof(pid_t), cmppid, NULL);
 	if (type == CGROUP_FILE_PROCS)
-		length = pidlist_uniq(&array, length);
+		length = pidlist_uniq(array, length);
 	l = cgroup_pidlist_find(cgrp, type);
 	if (!l) {
 		pidlist_free(array);
@@ -3930,11 +3882,7 @@ static int cgroup_write_event_control(struct cgroup *cgrp, struct cftype *cft,
 	if (ret)
 		goto fail;
 
-	if (efile->f_op->poll(efile, &event->pt) & POLLHUP) {
-		event->cft->unregister_event(cgrp, event->cft, event->eventfd);
-		ret = 0;
-		goto fail;
-	}
+	efile->f_op->poll(efile, &event->pt);
 
 	/*
 	 * Events should be removed after rmdir of cgroup directory, but before
@@ -4016,10 +3964,16 @@ static struct cftype files[] = {
 	},
 	{
 		.name = "cgroup.clone_children",
+		.flags = CFTYPE_INSANE,
 		.read_u64 = cgroup_clone_children_read,
 		.write_u64 = cgroup_clone_children_write,
 	},
 	{
+		.name = "cgroup.sane_behavior",
+		.flags = CFTYPE_ONLY_ON_ROOT,
+		.read_seq_string = cgroup_sane_behavior_show,
+	},
+	{
 		.name = "release_agent",
 		.flags = CFTYPE_ONLY_ON_ROOT,
 		.read_seq_string = cgroup_release_agent_show,
@@ -4131,17 +4085,8 @@ static void offline_css(struct cgroup_subsys *ss, struct cgroup *cgrp)
 	if (!(css->flags & CSS_ONLINE))
 		return;
 
-	/*
-	 * css_offline() should be called with cgroup_mutex unlocked.  See
-	 * 3fa59dfbc3 ("cgroup: fix potential deadlock in pre_destroy") for
-	 * details.  This temporary unlocking should go away once
-	 * cgroup_mutex is unexported from controllers.
-	 */
-	if (ss->css_offline) {
-		mutex_unlock(&cgroup_mutex);
+	if (ss->css_offline)
 		ss->css_offline(cgrp);
-		mutex_lock(&cgroup_mutex);
-	}
 
 	cgrp->subsys[ss->subsys_id]->flags &= ~CSS_ONLINE;
 }
@@ -4158,6 +4103,7 @@ static long cgroup_create(struct cgroup *parent, struct dentry *dentry,
 			     umode_t mode)
 {
 	struct cgroup *cgrp;
+	struct cgroup_name *name;
 	struct cgroupfs_root *root = parent->root;
 	int err = 0;
 	struct cgroup_subsys *ss;
@@ -4168,9 +4114,14 @@ static long cgroup_create(struct cgroup *parent, struct dentry *dentry,
 	if (!cgrp)
 		return -ENOMEM;
 
+	name = cgroup_alloc_name(dentry);
+	if (!name)
+		goto err_free_cgrp;
+	rcu_assign_pointer(cgrp->name, name);
+
 	cgrp->id = ida_simple_get(&root->cgroup_ida, 1, 0, GFP_KERNEL);
 	if (cgrp->id < 0)
-		goto err_free_cgrp;
+		goto err_free_name;
 
 	/*
 	 * Only live parents can have children.  Note that the liveliness
@@ -4198,7 +4149,6 @@ static long cgroup_create(struct cgroup *parent, struct dentry *dentry,
 
 	cgrp->parent = parent;
 	cgrp->root = parent->root;
-	cgrp->top_cgroup = parent->top_cgroup;
 
 	if (notify_on_release(parent))
 		set_bit(CGRP_NOTIFY_ON_RELEASE, &cgrp->flags);
@@ -4241,6 +4191,9 @@ static long cgroup_create(struct cgroup *parent, struct dentry *dentry,
 	for_each_subsys(root, ss)
 		dget(dentry);
 
+	/* hold a ref to the parent's dentry */
+	dget(parent->dentry);
+
 	/* creation succeeded, notify subsystems */
 	for_each_subsys(root, ss) {
 		err = online_css(ss, cgrp);
@@ -4276,6 +4229,8 @@ err_free_all:
 	deactivate_super(sb);
 err_free_id:
 	ida_simple_remove(&root->cgroup_ida, cgrp->id);
+err_free_name:
+	kfree(rcu_dereference_raw(cgrp->name));
 err_free_cgrp:
 	kfree(cgrp);
 	return err;
@@ -4295,56 +4250,13 @@ static int cgroup_mkdir(struct inode *dir, struct dentry *dentry, umode_t mode)
 	return cgroup_create(c_parent, dentry, mode | S_IFDIR);
 }
 
-/*
- * Check the reference count on each subsystem. Since we already
- * established that there are no tasks in the cgroup, if the css refcount
- * is also 1, then there should be no outstanding references, so the
- * subsystem is safe to destroy. We scan across all subsystems rather than
- * using the per-hierarchy linked list of mounted subsystems since we can
- * be called via check_for_release() with no synchronization other than
- * RCU, and the subsystem linked list isn't RCU-safe.
- */
-static int cgroup_has_css_refs(struct cgroup *cgrp)
-{
-	int i;
-
-	/*
-	 * We won't need to lock the subsys array, because the subsystems
-	 * we're concerned about aren't going anywhere since our cgroup root
-	 * has a reference on them.
-	 */
-	for (i = 0; i < CGROUP_SUBSYS_COUNT; i++) {
-		struct cgroup_subsys *ss = subsys[i];
-		struct cgroup_subsys_state *css;
-
-		/* Skip subsystems not present or not in this hierarchy */
-		if (ss == NULL || ss->root != cgrp->root)
-			continue;
-
-		css = cgrp->subsys[ss->subsys_id];
-		/*
-		 * When called from check_for_release() it's possible
-		 * that by this point the cgroup has been removed
-		 * and the css deleted. But a false-positive doesn't
-		 * matter, since it can only happen if the cgroup
-		 * has been deleted and hence no longer needs the
-		 * release agent to be called anyway.
-		 */
-		if (css && css_refcnt(css) > 1)
-			return 1;
-	}
-	return 0;
-}
-
 static int cgroup_destroy_locked(struct cgroup *cgrp)
 	__releases(&cgroup_mutex) __acquires(&cgroup_mutex)
 {
 	struct dentry *d = cgrp->dentry;
 	struct cgroup *parent = cgrp->parent;
-	DEFINE_WAIT(wait);
 	struct cgroup_event *event, *tmp;
 	struct cgroup_subsys *ss;
-	LIST_HEAD(tmp_list);
 
 	lockdep_assert_held(&d->d_inode->i_mutex);
 	lockdep_assert_held(&cgroup_mutex);
@@ -4468,7 +4380,6 @@ static void __init cgroup_init_subsys(struct cgroup_subsys *ss)
 	 * need to invoke fork callbacks here. */
 	BUG_ON(!list_empty(&init_task.tasks));
 
-	ss->active = 1;
 	BUG_ON(online_css(ss, dummytop));
 
 	mutex_unlock(&cgroup_mutex);
@@ -4573,7 +4484,6 @@ int __init_or_module cgroup_load_subsys(struct cgroup_subsys *ss)
 	}
 	write_unlock(&css_set_lock);
 
-	ss->active = 1;
 	ret = online_css(ss, dummytop);
 	if (ret)
 		goto err_unload;
@@ -4614,7 +4524,6 @@ void cgroup_unload_subsys(struct cgroup_subsys *ss)
 	mutex_lock(&cgroup_mutex);
 
 	offline_css(ss, dummytop);
-	ss->active = 0;
 
 	if (ss->use_id)
 		idr_destroy(&ss->idr);
@@ -4935,17 +4844,17 @@ void cgroup_post_fork(struct task_struct *child)
 	 * and addition to css_set.
 	 */
 	if (need_forkexit_callback) {
-		for (i = 0; i < CGROUP_SUBSYS_COUNT; i++) {
+		/*
+		 * fork/exit callbacks are supported only for builtin
+		 * subsystems, and the builtin section of the subsys
+		 * array is immutable, so we don't need to lock the
+		 * subsys array here. On the other hand, modular section
+		 * of the array can be freed at module unload, so we
+		 * can't touch that.
+		 */
+		for (i = 0; i < CGROUP_BUILTIN_SUBSYS_COUNT; i++) {
 			struct cgroup_subsys *ss = subsys[i];
 
-			/*
-			 * fork/exit callbacks are supported only for
-			 * builtin subsystems and we don't need further
-			 * synchronization as they never go away.
-			 */
-			if (!ss || ss->module)
-				continue;
-
 			if (ss->fork)
 				ss->fork(child);
 		}
@@ -5010,13 +4919,13 @@ void cgroup_exit(struct task_struct *tsk, int run_callbacks)
 	tsk->cgroups = &init_css_set;
 
 	if (run_callbacks && need_forkexit_callback) {
-		for (i = 0; i < CGROUP_SUBSYS_COUNT; i++) {
+		/*
+		 * fork/exit callbacks are supported only for builtin
+		 * subsystems, see cgroup_post_fork() for details.
+		 */
+		for (i = 0; i < CGROUP_BUILTIN_SUBSYS_COUNT; i++) {
 			struct cgroup_subsys *ss = subsys[i];
 
-			/* modular subsystems can't use callbacks */
-			if (!ss || ss->module)
-				continue;
-
 			if (ss->exit) {
 				struct cgroup *old_cgrp =
 					rcu_dereference_raw(cg->subsys[i])->cgroup;
@@ -5030,44 +4939,19 @@ void cgroup_exit(struct task_struct *tsk, int run_callbacks)
 	put_css_set_taskexit(cg);
 }
 
-/**
- * cgroup_is_descendant - see if @cgrp is a descendant of @task's cgrp
- * @cgrp: the cgroup in question
- * @task: the task in question
- *
- * See if @cgrp is a descendant of @task's cgroup in the appropriate
- * hierarchy.
- *
- * If we are sending in dummytop, then presumably we are creating
- * the top cgroup in the subsystem.
- *
- * Called only by the ns (nsproxy) cgroup.
- */
-int cgroup_is_descendant(const struct cgroup *cgrp, struct task_struct *task)
-{
-	int ret;
-	struct cgroup *target;
-
-	if (cgrp == dummytop)
-		return 1;
-
-	target = task_cgroup_from_root(task, cgrp->root);
-	while (cgrp != target && cgrp!= cgrp->top_cgroup)
-		cgrp = cgrp->parent;
-	ret = (cgrp == target);
-	return ret;
-}
-
 static void check_for_release(struct cgroup *cgrp)
 {
 	/* All of these checks rely on RCU to keep the cgroup
 	 * structure alive */
-	if (cgroup_is_releasable(cgrp) && !atomic_read(&cgrp->count)
-	    && list_empty(&cgrp->children) && !cgroup_has_css_refs(cgrp)) {
-		/* Control Group is currently removeable. If it's not
+	if (cgroup_is_releasable(cgrp) &&
+	    !atomic_read(&cgrp->count) && list_empty(&cgrp->children)) {
+		/*
+		 * Control Group is currently removeable. If it's not
 		 * already queued for a userspace notification, queue
-		 * it now */
+		 * it now
+		 */
 		int need_schedule_work = 0;
+
 		raw_spin_lock(&release_list_lock);
 		if (!cgroup_is_removed(cgrp) &&
 		    list_empty(&cgrp->release_list)) {
@@ -5100,24 +4984,11 @@ EXPORT_SYMBOL_GPL(__css_tryget);
 /* Caller must verify that the css is not for root cgroup */
 void __css_put(struct cgroup_subsys_state *css)
 {
-	struct cgroup *cgrp = css->cgroup;
 	int v;
 
-	rcu_read_lock();
 	v = css_unbias_refcnt(atomic_dec_return(&css->refcnt));
-
-	switch (v) {
-	case 1:
-		if (notify_on_release(cgrp)) {
-			set_bit(CGRP_RELEASABLE, &cgrp->flags);
-			check_for_release(cgrp);
-		}
-		break;
-	case 0:
+	if (v == 0)
 		schedule_work(&css->dput_work);
-		break;
-	}
-	rcu_read_unlock();
 }
 EXPORT_SYMBOL_GPL(__css_put);
 
@@ -5416,55 +5287,6 @@ struct cgroup_subsys_state *css_lookup(struct cgroup_subsys *ss, int id)
 }
 EXPORT_SYMBOL_GPL(css_lookup);
 
-/**
- * css_get_next - lookup next cgroup under specified hierarchy.
- * @ss: pointer to subsystem
- * @id: current position of iteration.
- * @root: pointer to css. search tree under this.
- * @foundid: position of found object.
- *
- * Search next css under the specified hierarchy of rootid. Calling under
- * rcu_read_lock() is necessary. Returns NULL if it reaches the end.
- */
-struct cgroup_subsys_state *
-css_get_next(struct cgroup_subsys *ss, int id,
-	     struct cgroup_subsys_state *root, int *foundid)
-{
-	struct cgroup_subsys_state *ret = NULL;
-	struct css_id *tmp;
-	int tmpid;
-	int rootid = css_id(root);
-	int depth = css_depth(root);
-
-	if (!rootid)
-		return NULL;
-
-	BUG_ON(!ss->use_id);
-	WARN_ON_ONCE(!rcu_read_lock_held());
-
-	/* fill start point for scan */
-	tmpid = id;
-	while (1) {
-		/*
-		 * scan next entry from bitmap(tree), tmpid is updated after
-		 * idr_get_next().
-		 */
-		tmp = idr_get_next(&ss->idr, &tmpid);
-		if (!tmp)
-			break;
-		if (tmp->depth >= depth && tmp->stack[depth] == rootid) {
-			ret = rcu_dereference(tmp->css);
-			if (ret) {
-				*foundid = tmpid;
-				break;
-			}
-		}
-		/* continue to scan from next id */
-		tmpid = tmpid + 1;
-	}
-	return ret;
-}
-
 /*
  * get corresponding css from file open on cgroupfs directory
  */
diff --git a/kernel/cpu/Makefile b/kernel/cpu/Makefile
new file mode 100644
index 00000000000..59ab052ef7a
--- /dev/null
+++ b/kernel/cpu/Makefile
@@ -0,0 +1 @@
+obj-y	= idle.o
diff --git a/kernel/cpu/idle.c b/kernel/cpu/idle.c
new file mode 100644
index 00000000000..8b86c0c68ed
--- /dev/null
+++ b/kernel/cpu/idle.c
@@ -0,0 +1,116 @@
+/*
+ * Generic entry point for the idle threads
+ */
+#include <linux/sched.h>
+#include <linux/cpu.h>
+#include <linux/tick.h>
+#include <linux/mm.h>
+
+#include <asm/tlb.h>
+
+#include <trace/events/power.h>
+
+static int __read_mostly cpu_idle_force_poll;
+
+void cpu_idle_poll_ctrl(bool enable)
+{
+	if (enable) {
+		cpu_idle_force_poll++;
+	} else {
+		cpu_idle_force_poll--;
+		WARN_ON_ONCE(cpu_idle_force_poll < 0);
+	}
+}
+
+#ifdef CONFIG_GENERIC_IDLE_POLL_SETUP
+static int __init cpu_idle_poll_setup(char *__unused)
+{
+	cpu_idle_force_poll = 1;
+	return 1;
+}
+__setup("nohlt", cpu_idle_poll_setup);
+
+static int __init cpu_idle_nopoll_setup(char *__unused)
+{
+	cpu_idle_force_poll = 0;
+	return 1;
+}
+__setup("hlt", cpu_idle_nopoll_setup);
+#endif
+
+static inline int cpu_idle_poll(void)
+{
+	trace_cpu_idle_rcuidle(0, smp_processor_id());
+	local_irq_enable();
+	while (!need_resched())
+		cpu_relax();
+	trace_cpu_idle_rcuidle(PWR_EVENT_EXIT, smp_processor_id());
+	return 1;
+}
+
+/* Weak implementations for optional arch specific functions */
+void __weak arch_cpu_idle_prepare(void) { }
+void __weak arch_cpu_idle_enter(void) { }
+void __weak arch_cpu_idle_exit(void) { }
+void __weak arch_cpu_idle_dead(void) { }
+void __weak arch_cpu_idle(void)
+{
+	cpu_idle_force_poll = 1;
+}
+
+/*
+ * Generic idle loop implementation
+ */
+static void cpu_idle_loop(void)
+{
+	while (1) {
+		tick_nohz_idle_enter();
+
+		while (!need_resched()) {
+			check_pgt_cache();
+			rmb();
+
+			if (cpu_is_offline(smp_processor_id()))
+				arch_cpu_idle_dead();
+
+			local_irq_disable();
+			arch_cpu_idle_enter();
+
+			/*
+			 * In poll mode we reenable interrupts and spin.
+			 *
+			 * Also if we detected in the wakeup from idle
+			 * path that the tick broadcast device expired
+			 * for us, we don't want to go deep idle as we
+			 * know that the IPI is going to arrive right
+			 * away
+			 */
+			if (cpu_idle_force_poll || tick_check_broadcast_expired()) {
+				cpu_idle_poll();
+			} else {
+				current_clr_polling();
+				if (!need_resched()) {
+					stop_critical_timings();
+					rcu_idle_enter();
+					arch_cpu_idle();
+					WARN_ON_ONCE(irqs_disabled());
+					rcu_idle_exit();
+					start_critical_timings();
+				} else {
+					local_irq_enable();
+				}
+				current_set_polling();
+			}
+			arch_cpu_idle_exit();
+		}
+		tick_nohz_idle_exit();
+		schedule_preempt_disabled();
+	}
+}
+
+void cpu_startup_entry(enum cpuhp_state state)
+{
+	current_set_polling();
+	arch_cpu_idle_prepare();
+	cpu_idle_loop();
+}
diff --git a/kernel/cpuset.c b/kernel/cpuset.c
index 4f9dfe43ecb..12331120767 100644
--- a/kernel/cpuset.c
+++ b/kernel/cpuset.c
@@ -265,17 +265,6 @@ static DEFINE_MUTEX(cpuset_mutex);
 static DEFINE_MUTEX(callback_mutex);
 
 /*
- * cpuset_buffer_lock protects both the cpuset_name and cpuset_nodelist
- * buffers.  They are statically allocated to prevent using excess stack
- * when calling cpuset_print_task_mems_allowed().
- */
-#define CPUSET_NAME_LEN		(128)
-#define	CPUSET_NODELIST_LEN	(256)
-static char cpuset_name[CPUSET_NAME_LEN];
-static char cpuset_nodelist[CPUSET_NODELIST_LEN];
-static DEFINE_SPINLOCK(cpuset_buffer_lock);
-
-/*
  * CPU / memory hotplug is handled asynchronously.
  */
 static struct workqueue_struct *cpuset_propagate_hotplug_wq;
@@ -780,25 +769,26 @@ static void rebuild_sched_domains_locked(void)
 	lockdep_assert_held(&cpuset_mutex);
 	get_online_cpus();
 
+	/*
+	 * We have raced with CPU hotplug. Don't do anything to avoid
+	 * passing doms with offlined cpu to partition_sched_domains().
+	 * Anyways, hotplug work item will rebuild sched domains.
+	 */
+	if (!cpumask_equal(top_cpuset.cpus_allowed, cpu_active_mask))
+		goto out;
+
 	/* Generate domain masks and attrs */
 	ndoms = generate_sched_domains(&doms, &attr);
 
 	/* Have scheduler rebuild the domains */
 	partition_sched_domains(ndoms, doms, attr);
-
+out:
 	put_online_cpus();
 }
 #else /* !CONFIG_SMP */
 static void rebuild_sched_domains_locked(void)
 {
 }
-
-static int generate_sched_domains(cpumask_var_t **domains,
-			struct sched_domain_attr **attributes)
-{
-	*domains = NULL;
-	return 1;
-}
 #endif /* CONFIG_SMP */
 
 void rebuild_sched_domains(void)
@@ -1388,16 +1378,16 @@ static int cpuset_can_attach(struct cgroup *cgrp, struct cgroup_taskset *tset)
 
 	cgroup_taskset_for_each(task, cgrp, tset) {
 		/*
-		 * Kthreads bound to specific cpus cannot be moved to a new
-		 * cpuset; we cannot change their cpu affinity and
-		 * isolating such threads by their set of allowed nodes is
-		 * unnecessary.  Thus, cpusets are not applicable for such
-		 * threads.  This prevents checking for success of
-		 * set_cpus_allowed_ptr() on all attached tasks before
-		 * cpus_allowed may be changed.
+		 * Kthreads which disallow setaffinity shouldn't be moved
+		 * to a new cpuset; we don't want to change their cpu
+		 * affinity and isolating such threads by their set of
+		 * allowed nodes is unnecessary.  Thus, cpusets are not
+		 * applicable for such threads.  This prevents checking for
+		 * success of set_cpus_allowed_ptr() on all attached tasks
+		 * before cpus_allowed may be changed.
 		 */
 		ret = -EINVAL;
-		if (task->flags & PF_THREAD_BOUND)
+		if (task->flags & PF_NO_SETAFFINITY)
 			goto out_unlock;
 		ret = security_task_setscheduler(task);
 		if (ret)
@@ -2005,50 +1995,6 @@ int __init cpuset_init(void)
 	return 0;
 }
 
-/**
- * cpuset_do_move_task - move a given task to another cpuset
- * @tsk: pointer to task_struct the task to move
- * @scan: struct cgroup_scanner contained in its struct cpuset_hotplug_scanner
- *
- * Called by cgroup_scan_tasks() for each task in a cgroup.
- * Return nonzero to stop the walk through the tasks.
- */
-static void cpuset_do_move_task(struct task_struct *tsk,
-				struct cgroup_scanner *scan)
-{
-	struct cgroup *new_cgroup = scan->data;
-
-	cgroup_lock();
-	cgroup_attach_task(new_cgroup, tsk);
-	cgroup_unlock();
-}
-
-/**
- * move_member_tasks_to_cpuset - move tasks from one cpuset to another
- * @from: cpuset in which the tasks currently reside
- * @to: cpuset to which the tasks will be moved
- *
- * Called with cpuset_mutex held
- * callback_mutex must not be held, as cpuset_attach() will take it.
- *
- * The cgroup_scan_tasks() function will scan all the tasks in a cgroup,
- * calling callback functions for each.
- */
-static void move_member_tasks_to_cpuset(struct cpuset *from, struct cpuset *to)
-{
-	struct cgroup_scanner scan;
-
-	scan.cg = from->css.cgroup;
-	scan.test_task = NULL; /* select all tasks in cgroup */
-	scan.process_task = cpuset_do_move_task;
-	scan.heap = NULL;
-	scan.data = to->css.cgroup;
-
-	if (cgroup_scan_tasks(&scan))
-		printk(KERN_ERR "move_member_tasks_to_cpuset: "
-				"cgroup_scan_tasks failed\n");
-}
-
 /*
  * If CPU and/or memory hotplug handlers, below, unplug any CPUs
  * or memory nodes, we need to walk over the cpuset hierarchy,
@@ -2069,7 +2015,12 @@ static void remove_tasks_in_empty_cpuset(struct cpuset *cs)
 			nodes_empty(parent->mems_allowed))
 		parent = parent_cs(parent);
 
-	move_member_tasks_to_cpuset(cs, parent);
+	if (cgroup_transfer_tasks(parent->css.cgroup, cs->css.cgroup)) {
+		rcu_read_lock();
+		printk(KERN_ERR "cpuset: failed to transfer tasks out of empty cpuset %s\n",
+		       cgroup_name(cs->css.cgroup));
+		rcu_read_unlock();
+	}
 }
 
 /**
@@ -2222,17 +2173,8 @@ static void cpuset_hotplug_workfn(struct work_struct *work)
 	flush_workqueue(cpuset_propagate_hotplug_wq);
 
 	/* rebuild sched domains if cpus_allowed has changed */
-	if (cpus_updated) {
-		struct sched_domain_attr *attr;
-		cpumask_var_t *doms;
-		int ndoms;
-
-		mutex_lock(&cpuset_mutex);
-		ndoms = generate_sched_domains(&doms, &attr);
-		mutex_unlock(&cpuset_mutex);
-
-		partition_sched_domains(ndoms, doms, attr);
-	}
+	if (cpus_updated)
+		rebuild_sched_domains();
 }
 
 void cpuset_update_active_cpus(bool cpu_online)
@@ -2251,7 +2193,6 @@ void cpuset_update_active_cpus(bool cpu_online)
 	schedule_work(&cpuset_hotplug_work);
 }
 
-#ifdef CONFIG_MEMORY_HOTPLUG
 /*
  * Keep top_cpuset.mems_allowed tracking node_states[N_MEMORY].
  * Call this routine anytime after node_states[N_MEMORY] changes.
@@ -2263,20 +2204,23 @@ static int cpuset_track_online_nodes(struct notifier_block *self,
 	schedule_work(&cpuset_hotplug_work);
 	return NOTIFY_OK;
 }
-#endif
+
+static struct notifier_block cpuset_track_online_nodes_nb = {
+	.notifier_call = cpuset_track_online_nodes,
+	.priority = 10,		/* ??! */
+};
 
 /**
  * cpuset_init_smp - initialize cpus_allowed
  *
  * Description: Finish top cpuset after cpu, node maps are initialized
- **/
-
+ */
 void __init cpuset_init_smp(void)
 {
 	cpumask_copy(top_cpuset.cpus_allowed, cpu_active_mask);
 	top_cpuset.mems_allowed = node_states[N_MEMORY];
 
-	hotplug_memory_notifier(cpuset_track_online_nodes, 10);
+	register_hotmemory_notifier(&cpuset_track_online_nodes_nb);
 
 	cpuset_propagate_hotplug_wq =
 		alloc_ordered_workqueue("cpuset_hotplug", 0);
@@ -2592,6 +2536,8 @@ int cpuset_mems_allowed_intersects(const struct task_struct *tsk1,
 	return nodes_intersects(tsk1->mems_allowed, tsk2->mems_allowed);
 }
 
+#define CPUSET_NODELIST_LEN	(256)
+
 /**
  * cpuset_print_task_mems_allowed - prints task's cpuset and mems_allowed
  * @task: pointer to task_struct of some task.
@@ -2602,25 +2548,22 @@ int cpuset_mems_allowed_intersects(const struct task_struct *tsk1,
  */
 void cpuset_print_task_mems_allowed(struct task_struct *tsk)
 {
-	struct dentry *dentry;
+	 /* Statically allocated to prevent using excess stack. */
+	static char cpuset_nodelist[CPUSET_NODELIST_LEN];
+	static DEFINE_SPINLOCK(cpuset_buffer_lock);
 
-	dentry = task_cs(tsk)->css.cgroup->dentry;
-	spin_lock(&cpuset_buffer_lock);
+	struct cgroup *cgrp = task_cs(tsk)->css.cgroup;
 
-	if (!dentry) {
-		strcpy(cpuset_name, "/");
-	} else {
-		spin_lock(&dentry->d_lock);
-		strlcpy(cpuset_name, (const char *)dentry->d_name.name,
-			CPUSET_NAME_LEN);
-		spin_unlock(&dentry->d_lock);
-	}
+	rcu_read_lock();
+	spin_lock(&cpuset_buffer_lock);
 
 	nodelist_scnprintf(cpuset_nodelist, CPUSET_NODELIST_LEN,
 			   tsk->mems_allowed);
 	printk(KERN_INFO "%s cpuset=%s mems_allowed=%s\n",
-	       tsk->comm, cpuset_name, cpuset_nodelist);
+	       tsk->comm, cgroup_name(cgrp), cpuset_nodelist);
+
 	spin_unlock(&cpuset_buffer_lock);
+	rcu_read_unlock();
 }
 
 /*
diff --git a/kernel/events/core.c b/kernel/events/core.c
index 59412d037ee..3820e3cefba 100644
--- a/kernel/events/core.c
+++ b/kernel/events/core.c
@@ -37,6 +37,7 @@
 #include <linux/ftrace_event.h>
 #include <linux/hw_breakpoint.h>
 #include <linux/mm_types.h>
+#include <linux/cgroup.h>
 
 #include "internal.h"
 
@@ -234,6 +235,20 @@ static void perf_ctx_unlock(struct perf_cpu_context *cpuctx,
 #ifdef CONFIG_CGROUP_PERF
 
 /*
+ * perf_cgroup_info keeps track of time_enabled for a cgroup.
+ * This is a per-cpu dynamically allocated data structure.
+ */
+struct perf_cgroup_info {
+	u64				time;
+	u64				timestamp;
+};
+
+struct perf_cgroup {
+	struct cgroup_subsys_state	css;
+	struct perf_cgroup_info	__percpu *info;
+};
+
+/*
  * Must ensure cgroup is pinned (css_get) before calling
  * this function. In other words, we cannot call this function
  * if there is no cgroup event for the current CPU context.
@@ -251,7 +266,22 @@ perf_cgroup_match(struct perf_event *event)
 	struct perf_event_context *ctx = event->ctx;
 	struct perf_cpu_context *cpuctx = __get_cpu_context(ctx);
 
-	return !event->cgrp || event->cgrp == cpuctx->cgrp;
+	/* @event doesn't care about cgroup */
+	if (!event->cgrp)
+		return true;
+
+	/* wants specific cgroup scope but @cpuctx isn't associated with any */
+	if (!cpuctx->cgrp)
+		return false;
+
+	/*
+	 * Cgroup scoping is recursive.  An event enabled for a cgroup is
+	 * also enabled for all its descendant cgroups.  If @cpuctx's
+	 * cgroup is a descendant of @event's (the test covers identity
+	 * case), it's a match.
+	 */
+	return cgroup_is_descendant(cpuctx->cgrp->css.cgroup,
+				    event->cgrp->css.cgroup);
 }
 
 static inline bool perf_tryget_cgroup(struct perf_event *event)
@@ -961,9 +991,15 @@ static void perf_event__header_size(struct perf_event *event)
 	if (sample_type & PERF_SAMPLE_PERIOD)
 		size += sizeof(data->period);
 
+	if (sample_type & PERF_SAMPLE_WEIGHT)
+		size += sizeof(data->weight);
+
 	if (sample_type & PERF_SAMPLE_READ)
 		size += event->read_size;
 
+	if (sample_type & PERF_SAMPLE_DATA_SRC)
+		size += sizeof(data->data_src.val);
+
 	event->header_size = size;
 }
 
@@ -4178,6 +4214,12 @@ void perf_output_sample(struct perf_output_handle *handle,
 		perf_output_sample_ustack(handle,
 					  data->stack_user_size,
 					  data->regs_user.regs);
+
+	if (sample_type & PERF_SAMPLE_WEIGHT)
+		perf_output_put(handle, data->weight);
+
+	if (sample_type & PERF_SAMPLE_DATA_SRC)
+		perf_output_put(handle, data->data_src.val);
 }
 
 void perf_prepare_sample(struct perf_event_header *header,
@@ -4596,6 +4638,7 @@ void perf_event_comm(struct task_struct *task)
 	struct perf_event_context *ctx;
 	int ctxn;
 
+	rcu_read_lock();
 	for_each_task_context_nr(ctxn) {
 		ctx = task->perf_event_ctxp[ctxn];
 		if (!ctx)
@@ -4603,6 +4646,7 @@ void perf_event_comm(struct task_struct *task)
 
 		perf_event_enable_on_exec(ctx);
 	}
+	rcu_read_unlock();
 
 	if (!atomic_read(&nr_comm_events))
 		return;
@@ -4737,7 +4781,8 @@ static void perf_event_mmap_event(struct perf_mmap_event *mmap_event)
 	} else {
 		if (arch_vma_name(mmap_event->vma)) {
 			name = strncpy(tmp, arch_vma_name(mmap_event->vma),
-				       sizeof(tmp));
+				       sizeof(tmp) - 1);
+			tmp[sizeof(tmp) - 1] = '\0';
 			goto got_name;
 		}
 
@@ -4764,6 +4809,9 @@ got_name:
 	mmap_event->file_name = name;
 	mmap_event->file_size = size;
 
+	if (!(vma->vm_flags & VM_EXEC))
+		mmap_event->event_id.header.misc |= PERF_RECORD_MISC_MMAP_DATA;
+
 	mmap_event->event_id.header.size = sizeof(mmap_event->event_id) + size;
 
 	rcu_read_lock();
@@ -5330,7 +5378,7 @@ static void sw_perf_event_destroy(struct perf_event *event)
 
 static int perf_swevent_init(struct perf_event *event)
 {
-	int event_id = event->attr.config;
+	u64 event_id = event->attr.config;
 
 	if (event->attr.type != PERF_TYPE_SOFTWARE)
 		return -ENOENT;
@@ -5986,6 +6034,7 @@ skip_type:
 	if (pmu->pmu_cpu_context)
 		goto got_cpu_context;
 
+	ret = -ENOMEM;
 	pmu->pmu_cpu_context = alloc_percpu(struct perf_cpu_context);
 	if (!pmu->pmu_cpu_context)
 		goto free_dev;
@@ -7513,12 +7562,5 @@ struct cgroup_subsys perf_subsys = {
 	.css_free	= perf_cgroup_css_free,
 	.exit		= perf_cgroup_exit,
 	.attach		= perf_cgroup_attach,
-
-	/*
-	 * perf_event cgroup doesn't handle nesting correctly.
-	 * ctx->nr_cgroups adjustments should be propagated through the
-	 * cgroup hierarchy.  Fix it and remove the following.
-	 */
-	.broken_hierarchy = true,
 };
 #endif /* CONFIG_CGROUP_PERF */
diff --git a/kernel/events/internal.h b/kernel/events/internal.h
index d56a64c99a8..eb675c4d59d 100644
--- a/kernel/events/internal.h
+++ b/kernel/events/internal.h
@@ -16,7 +16,7 @@ struct ring_buffer {
 	int				page_order;	/* allocation order  */
 #endif
 	int				nr_pages;	/* nr of data pages  */
-	int				writable;	/* are we writable   */
+	int				overwrite;	/* can overwrite itself */
 
 	atomic_t			poll;		/* POLL_ for wakeups */
 
diff --git a/kernel/events/ring_buffer.c b/kernel/events/ring_buffer.c
index 23cb34ff397..97fddb09762 100644
--- a/kernel/events/ring_buffer.c
+++ b/kernel/events/ring_buffer.c
@@ -18,12 +18,24 @@
 static bool perf_output_space(struct ring_buffer *rb, unsigned long tail,
 			      unsigned long offset, unsigned long head)
 {
-	unsigned long mask;
+	unsigned long sz = perf_data_size(rb);
+	unsigned long mask = sz - 1;
 
-	if (!rb->writable)
+	/*
+	 * check if user-writable
+	 * overwrite : over-write its own tail
+	 * !overwrite: buffer possibly drops events.
+	 */
+	if (rb->overwrite)
 		return true;
 
-	mask = perf_data_size(rb) - 1;
+	/*
+	 * verify that payload is not bigger than buffer
+	 * otherwise masking logic may fail to detect
+	 * the "not enough space" condition
+	 */
+	if ((head - offset) > sz)
+		return false;
 
 	offset = (offset - tail) & mask;
 	head   = (head   - tail) & mask;
@@ -212,7 +224,9 @@ ring_buffer_init(struct ring_buffer *rb, long watermark, int flags)
 		rb->watermark = max_size / 2;
 
 	if (flags & RING_BUFFER_WRITABLE)
-		rb->writable = 1;
+		rb->overwrite = 0;
+	else
+		rb->overwrite = 1;
 
 	atomic_set(&rb->refcount, 1);
 
diff --git a/kernel/events/uprobes.c b/kernel/events/uprobes.c
index a567c8c7ef3..f3569747d62 100644
--- a/kernel/events/uprobes.c
+++ b/kernel/events/uprobes.c
@@ -75,6 +75,15 @@ struct uprobe {
 	struct arch_uprobe	arch;
 };
 
+struct return_instance {
+	struct uprobe		*uprobe;
+	unsigned long		func;
+	unsigned long		orig_ret_vaddr; /* original return address */
+	bool			chained;	/* true, if instance is nested */
+
+	struct return_instance	*next;		/* keep as stack */
+};
+
 /*
  * valid_vma: Verify if the specified vma is an executable vma
  * Relax restrictions while unregistering: vm_flags might have
@@ -173,10 +182,31 @@ bool __weak is_swbp_insn(uprobe_opcode_t *insn)
 	return *insn == UPROBE_SWBP_INSN;
 }
 
-static void copy_opcode(struct page *page, unsigned long vaddr, uprobe_opcode_t *opcode)
+/**
+ * is_trap_insn - check if instruction is breakpoint instruction.
+ * @insn: instruction to be checked.
+ * Default implementation of is_trap_insn
+ * Returns true if @insn is a breakpoint instruction.
+ *
+ * This function is needed for the case where an architecture has multiple
+ * trap instructions (like powerpc).
+ */
+bool __weak is_trap_insn(uprobe_opcode_t *insn)
+{
+	return is_swbp_insn(insn);
+}
+
+static void copy_from_page(struct page *page, unsigned long vaddr, void *dst, int len)
 {
 	void *kaddr = kmap_atomic(page);
-	memcpy(opcode, kaddr + (vaddr & ~PAGE_MASK), UPROBE_SWBP_INSN_SIZE);
+	memcpy(dst, kaddr + (vaddr & ~PAGE_MASK), len);
+	kunmap_atomic(kaddr);
+}
+
+static void copy_to_page(struct page *page, unsigned long vaddr, const void *src, int len)
+{
+	void *kaddr = kmap_atomic(page);
+	memcpy(kaddr + (vaddr & ~PAGE_MASK), src, len);
 	kunmap_atomic(kaddr);
 }
 
@@ -185,7 +215,16 @@ static int verify_opcode(struct page *page, unsigned long vaddr, uprobe_opcode_t
 	uprobe_opcode_t old_opcode;
 	bool is_swbp;
 
-	copy_opcode(page, vaddr, &old_opcode);
+	/*
+	 * Note: We only check if the old_opcode is UPROBE_SWBP_INSN here.
+	 * We do not check if it is any other 'trap variant' which could
+	 * be conditional trap instruction such as the one powerpc supports.
+	 *
+	 * The logic is that we do not care if the underlying instruction
+	 * is a trap variant; uprobes always wins over any other (gdb)
+	 * breakpoint.
+	 */
+	copy_from_page(page, vaddr, &old_opcode, UPROBE_SWBP_INSN_SIZE);
 	is_swbp = is_swbp_insn(&old_opcode);
 
 	if (is_swbp_insn(new_opcode)) {
@@ -204,7 +243,7 @@ static int verify_opcode(struct page *page, unsigned long vaddr, uprobe_opcode_t
  * 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 is_swbp_at_addr and
+ * supported by that architecture then we need to modify is_trap_at_addr and
  * write_opcode accordingly. This would never be a problem for archs that
  * have fixed length instructions.
  */
@@ -225,7 +264,6 @@ static int write_opcode(struct mm_struct *mm, unsigned long vaddr,
 			uprobe_opcode_t opcode)
 {
 	struct page *old_page, *new_page;
-	void *vaddr_old, *vaddr_new;
 	struct vm_area_struct *vma;
 	int ret;
 
@@ -246,15 +284,8 @@ retry:
 
 	__SetPageUptodate(new_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);
-	memcpy(vaddr_new + (vaddr & ~PAGE_MASK), &opcode, UPROBE_SWBP_INSN_SIZE);
-
-	kunmap_atomic(vaddr_new);
-	kunmap_atomic(vaddr_old);
+	copy_highpage(new_page, old_page);
+	copy_to_page(new_page, vaddr, &opcode, UPROBE_SWBP_INSN_SIZE);
 
 	ret = anon_vma_prepare(vma);
 	if (ret)
@@ -477,30 +508,18 @@ __copy_insn(struct address_space *mapping, struct file *filp, char *insn,
 			unsigned long nbytes, loff_t offset)
 {
 	struct page *page;
-	void *vaddr;
-	unsigned long off;
-	pgoff_t idx;
-
-	if (!filp)
-		return -EINVAL;
 
 	if (!mapping->a_ops->readpage)
 		return -EIO;
-
-	idx = offset >> PAGE_CACHE_SHIFT;
-	off = 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);
+	page = read_mapping_page(mapping, offset >> PAGE_CACHE_SHIFT, filp);
 	if (IS_ERR(page))
 		return PTR_ERR(page);
 
-	vaddr = kmap_atomic(page);
-	memcpy(insn, vaddr + off, nbytes);
-	kunmap_atomic(vaddr);
+	copy_from_page(page, offset, insn, nbytes);
 	page_cache_release(page);
 
 	return 0;
@@ -550,7 +569,7 @@ static int prepare_uprobe(struct uprobe *uprobe, struct file *file,
 		goto out;
 
 	ret = -ENOTSUPP;
-	if (is_swbp_insn((uprobe_opcode_t *)uprobe->arch.insn))
+	if (is_trap_insn((uprobe_opcode_t *)uprobe->arch.insn))
 		goto out;
 
 	ret = arch_uprobe_analyze_insn(&uprobe->arch, mm, vaddr);
@@ -758,7 +777,7 @@ register_for_each_vma(struct uprobe *uprobe, struct uprobe_consumer *new)
 		down_write(&mm->mmap_sem);
 		vma = find_vma(mm, info->vaddr);
 		if (!vma || !valid_vma(vma, is_register) ||
-		    vma->vm_file->f_mapping->host != uprobe->inode)
+		    file_inode(vma->vm_file) != uprobe->inode)
 			goto unlock;
 
 		if (vma->vm_start > info->vaddr ||
@@ -828,6 +847,10 @@ int uprobe_register(struct inode *inode, loff_t offset, struct uprobe_consumer *
 	struct uprobe *uprobe;
 	int ret;
 
+	/* Uprobe must have at least one set consumer */
+	if (!uc->handler && !uc->ret_handler)
+		return -EINVAL;
+
 	/* Racy, just to catch the obvious mistakes */
 	if (offset > i_size_read(inode))
 		return -EINVAL;
@@ -917,7 +940,7 @@ static int unapply_uprobe(struct uprobe *uprobe, struct mm_struct *mm)
 		loff_t offset;
 
 		if (!valid_vma(vma, false) ||
-		    vma->vm_file->f_mapping->host != uprobe->inode)
+		    file_inode(vma->vm_file) != uprobe->inode)
 			continue;
 
 		offset = (loff_t)vma->vm_pgoff << PAGE_SHIFT;
@@ -1010,7 +1033,7 @@ int uprobe_mmap(struct vm_area_struct *vma)
 	if (no_uprobe_events() || !valid_vma(vma, true))
 		return 0;
 
-	inode = vma->vm_file->f_mapping->host;
+	inode = file_inode(vma->vm_file);
 	if (!inode)
 		return 0;
 
@@ -1041,7 +1064,7 @@ vma_has_uprobes(struct vm_area_struct *vma, unsigned long start, unsigned long e
 	struct inode *inode;
 	struct rb_node *n;
 
-	inode = vma->vm_file->f_mapping->host;
+	inode = file_inode(vma->vm_file);
 
 	min = vaddr_to_offset(vma, start);
 	max = min + (end - start) - 1;
@@ -1114,6 +1137,7 @@ static struct xol_area *get_xol_area(void)
 {
 	struct mm_struct *mm = current->mm;
 	struct xol_area *area;
+	uprobe_opcode_t insn = UPROBE_SWBP_INSN;
 
 	area = mm->uprobes_state.xol_area;
 	if (area)
@@ -1131,7 +1155,12 @@ static struct xol_area *get_xol_area(void)
 	if (!area->page)
 		goto free_bitmap;
 
+	/* allocate first slot of task's xol_area for the return probes */
+	set_bit(0, area->bitmap);
+	copy_to_page(area->page, 0, &insn, UPROBE_SWBP_INSN_SIZE);
+	atomic_set(&area->slot_count, 1);
 	init_waitqueue_head(&area->wq);
+
 	if (!xol_add_vma(area))
 		return area;
 
@@ -1216,9 +1245,7 @@ static unsigned long xol_take_insn_slot(struct xol_area *area)
 static unsigned long xol_get_insn_slot(struct uprobe *uprobe)
 {
 	struct xol_area *area;
-	unsigned long offset;
 	unsigned long xol_vaddr;
-	void *vaddr;
 
 	area = get_xol_area();
 	if (!area)
@@ -1229,10 +1256,7 @@ static unsigned long xol_get_insn_slot(struct uprobe *uprobe)
 		return 0;
 
 	/* Initialize the slot */
-	offset = xol_vaddr & ~PAGE_MASK;
-	vaddr = kmap_atomic(area->page);
-	memcpy(vaddr + offset, uprobe->arch.insn, MAX_UINSN_BYTES);
-	kunmap_atomic(vaddr);
+	copy_to_page(area->page, xol_vaddr, uprobe->arch.insn, MAX_UINSN_BYTES);
 	/*
 	 * We probably need flush_icache_user_range() but it needs vma.
 	 * This should work on supported architectures too.
@@ -1298,6 +1322,7 @@ unsigned long __weak uprobe_get_swbp_addr(struct pt_regs *regs)
 void uprobe_free_utask(struct task_struct *t)
 {
 	struct uprobe_task *utask = t->utask;
+	struct return_instance *ri, *tmp;
 
 	if (!utask)
 		return;
@@ -1305,6 +1330,15 @@ void uprobe_free_utask(struct task_struct *t)
 	if (utask->active_uprobe)
 		put_uprobe(utask->active_uprobe);
 
+	ri = utask->return_instances;
+	while (ri) {
+		tmp = ri;
+		ri = ri->next;
+
+		put_uprobe(tmp->uprobe);
+		kfree(tmp);
+	}
+
 	xol_free_insn_slot(t);
 	kfree(utask);
 	t->utask = NULL;
@@ -1333,6 +1367,93 @@ static struct uprobe_task *get_utask(void)
 	return current->utask;
 }
 
+/*
+ * Current area->vaddr notion assume the trampoline address is always
+ * equal area->vaddr.
+ *
+ * Returns -1 in case the xol_area is not allocated.
+ */
+static unsigned long get_trampoline_vaddr(void)
+{
+	struct xol_area *area;
+	unsigned long trampoline_vaddr = -1;
+
+	area = current->mm->uprobes_state.xol_area;
+	smp_read_barrier_depends();
+	if (area)
+		trampoline_vaddr = area->vaddr;
+
+	return trampoline_vaddr;
+}
+
+static void prepare_uretprobe(struct uprobe *uprobe, struct pt_regs *regs)
+{
+	struct return_instance *ri;
+	struct uprobe_task *utask;
+	unsigned long orig_ret_vaddr, trampoline_vaddr;
+	bool chained = false;
+
+	if (!get_xol_area())
+		return;
+
+	utask = get_utask();
+	if (!utask)
+		return;
+
+	if (utask->depth >= MAX_URETPROBE_DEPTH) {
+		printk_ratelimited(KERN_INFO "uprobe: omit uretprobe due to"
+				" nestedness limit pid/tgid=%d/%d\n",
+				current->pid, current->tgid);
+		return;
+	}
+
+	ri = kzalloc(sizeof(struct return_instance), GFP_KERNEL);
+	if (!ri)
+		goto fail;
+
+	trampoline_vaddr = get_trampoline_vaddr();
+	orig_ret_vaddr = arch_uretprobe_hijack_return_addr(trampoline_vaddr, regs);
+	if (orig_ret_vaddr == -1)
+		goto fail;
+
+	/*
+	 * We don't want to keep trampoline address in stack, rather keep the
+	 * original return address of first caller thru all the consequent
+	 * instances. This also makes breakpoint unwrapping easier.
+	 */
+	if (orig_ret_vaddr == trampoline_vaddr) {
+		if (!utask->return_instances) {
+			/*
+			 * This situation is not possible. Likely we have an
+			 * attack from user-space.
+			 */
+			pr_warn("uprobe: unable to set uretprobe pid/tgid=%d/%d\n",
+						current->pid, current->tgid);
+			goto fail;
+		}
+
+		chained = true;
+		orig_ret_vaddr = utask->return_instances->orig_ret_vaddr;
+	}
+
+	atomic_inc(&uprobe->ref);
+	ri->uprobe = uprobe;
+	ri->func = instruction_pointer(regs);
+	ri->orig_ret_vaddr = orig_ret_vaddr;
+	ri->chained = chained;
+
+	utask->depth++;
+
+	/* add instance to the stack */
+	ri->next = utask->return_instances;
+	utask->return_instances = ri;
+
+	return;
+
+ fail:
+	kfree(ri);
+}
+
 /* Prepare to single-step probed instruction out of line. */
 static int
 pre_ssout(struct uprobe *uprobe, struct pt_regs *regs, unsigned long bp_vaddr)
@@ -1431,7 +1552,7 @@ static void mmf_recalc_uprobes(struct mm_struct *mm)
 	clear_bit(MMF_HAS_UPROBES, &mm->flags);
 }
 
-static int is_swbp_at_addr(struct mm_struct *mm, unsigned long vaddr)
+static int is_trap_at_addr(struct mm_struct *mm, unsigned long vaddr)
 {
 	struct page *page;
 	uprobe_opcode_t opcode;
@@ -1449,10 +1570,11 @@ static int is_swbp_at_addr(struct mm_struct *mm, unsigned long vaddr)
 	if (result < 0)
 		return result;
 
-	copy_opcode(page, vaddr, &opcode);
+	copy_from_page(page, vaddr, &opcode, UPROBE_SWBP_INSN_SIZE);
 	put_page(page);
  out:
-	return is_swbp_insn(&opcode);
+	/* This needs to return true for any variant of the trap insn */
+	return is_trap_insn(&opcode);
 }
 
 static struct uprobe *find_active_uprobe(unsigned long bp_vaddr, int *is_swbp)
@@ -1465,14 +1587,14 @@ static struct uprobe *find_active_uprobe(unsigned long bp_vaddr, int *is_swbp)
 	vma = find_vma(mm, bp_vaddr);
 	if (vma && vma->vm_start <= bp_vaddr) {
 		if (valid_vma(vma, false)) {
-			struct inode *inode = vma->vm_file->f_mapping->host;
+			struct inode *inode = file_inode(vma->vm_file);
 			loff_t offset = vaddr_to_offset(vma, bp_vaddr);
 
 			uprobe = find_uprobe(inode, offset);
 		}
 
 		if (!uprobe)
-			*is_swbp = is_swbp_at_addr(mm, bp_vaddr);
+			*is_swbp = is_trap_at_addr(mm, bp_vaddr);
 	} else {
 		*is_swbp = -EFAULT;
 	}
@@ -1488,16 +1610,27 @@ static void handler_chain(struct uprobe *uprobe, struct pt_regs *regs)
 {
 	struct uprobe_consumer *uc;
 	int remove = UPROBE_HANDLER_REMOVE;
+	bool need_prep = false; /* prepare return uprobe, when needed */
 
 	down_read(&uprobe->register_rwsem);
 	for (uc = uprobe->consumers; uc; uc = uc->next) {
-		int rc = uc->handler(uc, regs);
+		int rc = 0;
+
+		if (uc->handler) {
+			rc = uc->handler(uc, regs);
+			WARN(rc & ~UPROBE_HANDLER_MASK,
+				"bad rc=0x%x from %pf()\n", rc, uc->handler);
+		}
+
+		if (uc->ret_handler)
+			need_prep = true;
 
-		WARN(rc & ~UPROBE_HANDLER_MASK,
-			"bad rc=0x%x from %pf()\n", rc, uc->handler);
 		remove &= rc;
 	}
 
+	if (need_prep && !remove)
+		prepare_uretprobe(uprobe, regs); /* put bp at return */
+
 	if (remove && uprobe->consumers) {
 		WARN_ON(!uprobe_is_active(uprobe));
 		unapply_uprobe(uprobe, current->mm);
@@ -1505,6 +1638,64 @@ static void handler_chain(struct uprobe *uprobe, struct pt_regs *regs)
 	up_read(&uprobe->register_rwsem);
 }
 
+static void
+handle_uretprobe_chain(struct return_instance *ri, struct pt_regs *regs)
+{
+	struct uprobe *uprobe = ri->uprobe;
+	struct uprobe_consumer *uc;
+
+	down_read(&uprobe->register_rwsem);
+	for (uc = uprobe->consumers; uc; uc = uc->next) {
+		if (uc->ret_handler)
+			uc->ret_handler(uc, ri->func, regs);
+	}
+	up_read(&uprobe->register_rwsem);
+}
+
+static bool handle_trampoline(struct pt_regs *regs)
+{
+	struct uprobe_task *utask;
+	struct return_instance *ri, *tmp;
+	bool chained;
+
+	utask = current->utask;
+	if (!utask)
+		return false;
+
+	ri = utask->return_instances;
+	if (!ri)
+		return false;
+
+	/*
+	 * TODO: we should throw out return_instance's invalidated by
+	 * longjmp(), currently we assume that the probed function always
+	 * returns.
+	 */
+	instruction_pointer_set(regs, ri->orig_ret_vaddr);
+
+	for (;;) {
+		handle_uretprobe_chain(ri, regs);
+
+		chained = ri->chained;
+		put_uprobe(ri->uprobe);
+
+		tmp = ri;
+		ri = ri->next;
+		kfree(tmp);
+
+		if (!chained)
+			break;
+
+		utask->depth--;
+
+		BUG_ON(!ri);
+	}
+
+	utask->return_instances = ri;
+
+	return true;
+}
+
 /*
  * Run handler and ask thread to singlestep.
  * Ensure all non-fatal signals cannot interrupt thread while it singlesteps.
@@ -1516,8 +1707,15 @@ static void handle_swbp(struct pt_regs *regs)
 	int uninitialized_var(is_swbp);
 
 	bp_vaddr = uprobe_get_swbp_addr(regs);
-	uprobe = find_active_uprobe(bp_vaddr, &is_swbp);
+	if (bp_vaddr == get_trampoline_vaddr()) {
+		if (handle_trampoline(regs))
+			return;
+
+		pr_warn("uprobe: unable to handle uretprobe pid/tgid=%d/%d\n",
+						current->pid, current->tgid);
+	}
 
+	uprobe = find_active_uprobe(bp_vaddr, &is_swbp);
 	if (!uprobe) {
 		if (is_swbp > 0) {
 			/* No matching uprobe; signal SIGTRAP. */
@@ -1616,7 +1814,11 @@ void uprobe_notify_resume(struct pt_regs *regs)
  */
 int uprobe_pre_sstep_notifier(struct pt_regs *regs)
 {
-	if (!current->mm || !test_bit(MMF_HAS_UPROBES, &current->mm->flags))
+	if (!current->mm)
+		return 0;
+
+	if (!test_bit(MMF_HAS_UPROBES, &current->mm->flags) &&
+	    (!current->utask || !current->utask->return_instances))
 		return 0;
 
 	set_thread_flag(TIF_UPROBE);
diff --git a/kernel/exit.c b/kernel/exit.c
index 51e485ca993..60bc027c61c 100644
--- a/kernel/exit.c
+++ b/kernel/exit.c
@@ -835,7 +835,7 @@ void do_exit(long code)
 	/*
 	 * Make sure we are holding no locks:
 	 */
-	debug_check_no_locks_held();
+	debug_check_no_locks_held(tsk);
 	/*
 	 * We can do this unlocked here. The futex code uses this flag
 	 * just to verify whether the pi state cleanup has been done
diff --git a/kernel/extable.c b/kernel/extable.c
index fe35a634bf7..67460b93b1a 100644
--- a/kernel/extable.c
+++ b/kernel/extable.c
@@ -41,10 +41,10 @@ u32 __initdata main_extable_sort_needed = 1;
 /* Sort the kernel's built-in exception table */
 void __init sort_main_extable(void)
 {
-	if (main_extable_sort_needed)
+	if (main_extable_sort_needed) {
+		pr_notice("Sorting __ex_table...\n");
 		sort_extable(__start___ex_table, __stop___ex_table);
-	else
-		pr_notice("__ex_table already sorted, skipping sort\n");
+	}
 }
 
 /* Given an address, look for it in the exception tables. */
diff --git a/kernel/fork.c b/kernel/fork.c
index 1766d324d5e..339f60dfd62 100644
--- a/kernel/fork.c
+++ b/kernel/fork.c
@@ -1233,7 +1233,7 @@ static struct task_struct *copy_process(unsigned long clone_flags,
 
 	p->utime = p->stime = p->gtime = 0;
 	p->utimescaled = p->stimescaled = 0;
-#ifndef CONFIG_VIRT_CPU_ACCOUNTING
+#ifndef CONFIG_VIRT_CPU_ACCOUNTING_NATIVE
 	p->prev_cputime.utime = p->prev_cputime.stime = 0;
 #endif
 #ifdef CONFIG_VIRT_CPU_ACCOUNTING_GEN
diff --git a/kernel/hrtimer.c b/kernel/hrtimer.c
index cc47812d3fe..609d8ff38b7 100644
--- a/kernel/hrtimer.c
+++ b/kernel/hrtimer.c
@@ -63,6 +63,7 @@
 DEFINE_PER_CPU(struct hrtimer_cpu_base, hrtimer_bases) =
 {
 
+	.lock = __RAW_SPIN_LOCK_UNLOCKED(hrtimer_bases.lock),
 	.clock_base =
 	{
 		{
@@ -83,6 +84,12 @@ DEFINE_PER_CPU(struct hrtimer_cpu_base, hrtimer_bases) =
 			.get_time = &ktime_get_boottime,
 			.resolution = KTIME_LOW_RES,
 		},
+		{
+			.index = HRTIMER_BASE_TAI,
+			.clockid = CLOCK_TAI,
+			.get_time = &ktime_get_clocktai,
+			.resolution = KTIME_LOW_RES,
+		},
 	}
 };
 
@@ -90,6 +97,7 @@ static const int hrtimer_clock_to_base_table[MAX_CLOCKS] = {
 	[CLOCK_REALTIME]	= HRTIMER_BASE_REALTIME,
 	[CLOCK_MONOTONIC]	= HRTIMER_BASE_MONOTONIC,
 	[CLOCK_BOOTTIME]	= HRTIMER_BASE_BOOTTIME,
+	[CLOCK_TAI]		= HRTIMER_BASE_TAI,
 };
 
 static inline int hrtimer_clockid_to_base(clockid_t clock_id)
@@ -106,8 +114,10 @@ static void hrtimer_get_softirq_time(struct hrtimer_cpu_base *base)
 {
 	ktime_t xtim, mono, boot;
 	struct timespec xts, tom, slp;
+	s32 tai_offset;
 
 	get_xtime_and_monotonic_and_sleep_offset(&xts, &tom, &slp);
+	tai_offset = timekeeping_get_tai_offset();
 
 	xtim = timespec_to_ktime(xts);
 	mono = ktime_add(xtim, timespec_to_ktime(tom));
@@ -115,6 +125,8 @@ static void hrtimer_get_softirq_time(struct hrtimer_cpu_base *base)
 	base->clock_base[HRTIMER_BASE_REALTIME].softirq_time = xtim;
 	base->clock_base[HRTIMER_BASE_MONOTONIC].softirq_time = mono;
 	base->clock_base[HRTIMER_BASE_BOOTTIME].softirq_time = boot;
+	base->clock_base[HRTIMER_BASE_TAI].softirq_time =
+				ktime_add(xtim,	ktime_set(tai_offset, 0));
 }
 
 /*
@@ -275,6 +287,10 @@ ktime_t ktime_add_ns(const ktime_t kt, u64 nsec)
 	} else {
 		unsigned long rem = do_div(nsec, NSEC_PER_SEC);
 
+		/* Make sure nsec fits into long */
+		if (unlikely(nsec > KTIME_SEC_MAX))
+			return (ktime_t){ .tv64 = KTIME_MAX };
+
 		tmp = ktime_set((long)nsec, rem);
 	}
 
@@ -651,8 +667,9 @@ static inline ktime_t hrtimer_update_base(struct hrtimer_cpu_base *base)
 {
 	ktime_t *offs_real = &base->clock_base[HRTIMER_BASE_REALTIME].offset;
 	ktime_t *offs_boot = &base->clock_base[HRTIMER_BASE_BOOTTIME].offset;
+	ktime_t *offs_tai = &base->clock_base[HRTIMER_BASE_TAI].offset;
 
-	return ktime_get_update_offsets(offs_real, offs_boot);
+	return ktime_get_update_offsets(offs_real, offs_boot, offs_tai);
 }
 
 /*
@@ -1010,7 +1027,8 @@ int __hrtimer_start_range_ns(struct hrtimer *timer, ktime_t tim,
  * @timer:	the timer to be added
  * @tim:	expiry time
  * @delta_ns:	"slack" range for the timer
- * @mode:	expiry mode: absolute (HRTIMER_ABS) or relative (HRTIMER_REL)
+ * @mode:	expiry mode: absolute (HRTIMER_MODE_ABS) or
+ *		relative (HRTIMER_MODE_REL)
  *
  * Returns:
  *  0 on success
@@ -1027,7 +1045,8 @@ EXPORT_SYMBOL_GPL(hrtimer_start_range_ns);
  * hrtimer_start - (re)start an hrtimer on the current CPU
  * @timer:	the timer to be added
  * @tim:	expiry time
- * @mode:	expiry mode: absolute (HRTIMER_ABS) or relative (HRTIMER_REL)
+ * @mode:	expiry mode: absolute (HRTIMER_MODE_ABS) or
+ *		relative (HRTIMER_MODE_REL)
  *
  * Returns:
  *  0 on success
@@ -1309,6 +1328,8 @@ retry:
 
 				expires = ktime_sub(hrtimer_get_expires(timer),
 						    base->offset);
+				if (expires.tv64 < 0)
+					expires.tv64 = KTIME_MAX;
 				if (expires.tv64 < expires_next.tv64)
 					expires_next = expires;
 				break;
@@ -1642,8 +1663,6 @@ static void __cpuinit init_hrtimers_cpu(int cpu)
 	struct hrtimer_cpu_base *cpu_base = &per_cpu(hrtimer_bases, cpu);
 	int i;
 
-	raw_spin_lock_init(&cpu_base->lock);
-
 	for (i = 0; i < HRTIMER_MAX_CLOCK_BASES; i++) {
 		cpu_base->clock_base[i].cpu_base = cpu_base;
 		timerqueue_init_head(&cpu_base->clock_base[i].active);
diff --git a/kernel/kexec.c b/kernel/kexec.c
index bddd3d7a74b..b574920cbd4 100644
--- a/kernel/kexec.c
+++ b/kernel/kexec.c
@@ -55,7 +55,7 @@ struct resource crashk_res = {
 	.flags = IORESOURCE_BUSY | IORESOURCE_MEM
 };
 struct resource crashk_low_res = {
-	.name  = "Crash kernel low",
+	.name  = "Crash kernel",
 	.start = 0,
 	.end   = 0,
 	.flags = IORESOURCE_BUSY | IORESOURCE_MEM
@@ -1118,12 +1118,8 @@ void __weak crash_free_reserved_phys_range(unsigned long begin,
 {
 	unsigned long addr;
 
-	for (addr = begin; addr < end; addr += PAGE_SIZE) {
-		ClearPageReserved(pfn_to_page(addr >> PAGE_SHIFT));
-		init_page_count(pfn_to_page(addr >> PAGE_SHIFT));
-		free_page((unsigned long)__va(addr));
-		totalram_pages++;
-	}
+	for (addr = begin; addr < end; addr += PAGE_SIZE)
+		free_reserved_page(pfn_to_page(addr >> PAGE_SHIFT));
 }
 
 int crash_shrink_memory(unsigned long new_size)
@@ -1368,35 +1364,114 @@ static int __init parse_crashkernel_simple(char 		*cmdline,
 	return 0;
 }
 
+#define SUFFIX_HIGH 0
+#define SUFFIX_LOW  1
+#define SUFFIX_NULL 2
+static __initdata char *suffix_tbl[] = {
+	[SUFFIX_HIGH] = ",high",
+	[SUFFIX_LOW]  = ",low",
+	[SUFFIX_NULL] = NULL,
+};
+
 /*
- * That function is the entry point for command line parsing and should be
- * called from the arch-specific code.
+ * That function parses "suffix"  crashkernel command lines like
+ *
+ *	crashkernel=size,[high|low]
+ *
+ * It returns 0 on success and -EINVAL on failure.
  */
+static int __init parse_crashkernel_suffix(char *cmdline,
+					   unsigned long long	*crash_size,
+					   unsigned long long	*crash_base,
+					   const char *suffix)
+{
+	char *cur = cmdline;
+
+	*crash_size = memparse(cmdline, &cur);
+	if (cmdline == cur) {
+		pr_warn("crashkernel: memory value expected\n");
+		return -EINVAL;
+	}
+
+	/* check with suffix */
+	if (strncmp(cur, suffix, strlen(suffix))) {
+		pr_warn("crashkernel: unrecognized char\n");
+		return -EINVAL;
+	}
+	cur += strlen(suffix);
+	if (*cur != ' ' && *cur != '\0') {
+		pr_warn("crashkernel: unrecognized char\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static __init char *get_last_crashkernel(char *cmdline,
+			     const char *name,
+			     const char *suffix)
+{
+	char *p = cmdline, *ck_cmdline = NULL;
+
+	/* find crashkernel and use the last one if there are more */
+	p = strstr(p, name);
+	while (p) {
+		char *end_p = strchr(p, ' ');
+		char *q;
+
+		if (!end_p)
+			end_p = p + strlen(p);
+
+		if (!suffix) {
+			int i;
+
+			/* skip the one with any known suffix */
+			for (i = 0; suffix_tbl[i]; i++) {
+				q = end_p - strlen(suffix_tbl[i]);
+				if (!strncmp(q, suffix_tbl[i],
+					     strlen(suffix_tbl[i])))
+					goto next;
+			}
+			ck_cmdline = p;
+		} else {
+			q = end_p - strlen(suffix);
+			if (!strncmp(q, suffix, strlen(suffix)))
+				ck_cmdline = p;
+		}
+next:
+		p = strstr(p+1, name);
+	}
+
+	if (!ck_cmdline)
+		return NULL;
+
+	return ck_cmdline;
+}
+
 static int __init __parse_crashkernel(char *cmdline,
 			     unsigned long long system_ram,
 			     unsigned long long *crash_size,
 			     unsigned long long *crash_base,
-				const char *name)
+			     const char *name,
+			     const char *suffix)
 {
-	char 	*p = cmdline, *ck_cmdline = NULL;
 	char	*first_colon, *first_space;
+	char	*ck_cmdline;
 
 	BUG_ON(!crash_size || !crash_base);
 	*crash_size = 0;
 	*crash_base = 0;
 
-	/* find crashkernel and use the last one if there are more */
-	p = strstr(p, name);
-	while (p) {
-		ck_cmdline = p;
-		p = strstr(p+1, name);
-	}
+	ck_cmdline = get_last_crashkernel(cmdline, name, suffix);
 
 	if (!ck_cmdline)
 		return -EINVAL;
 
 	ck_cmdline += strlen(name);
 
+	if (suffix)
+		return parse_crashkernel_suffix(ck_cmdline, crash_size,
+				crash_base, suffix);
 	/*
 	 * if the commandline contains a ':', then that's the extended
 	 * syntax -- if not, it must be the classic syntax
@@ -1413,13 +1488,26 @@ static int __init __parse_crashkernel(char *cmdline,
 	return 0;
 }
 
+/*
+ * That function is the entry point for command line parsing and should be
+ * called from the arch-specific code.
+ */
 int __init parse_crashkernel(char *cmdline,
 			     unsigned long long system_ram,
 			     unsigned long long *crash_size,
 			     unsigned long long *crash_base)
 {
 	return __parse_crashkernel(cmdline, system_ram, crash_size, crash_base,
-					"crashkernel=");
+					"crashkernel=", NULL);
+}
+
+int __init parse_crashkernel_high(char *cmdline,
+			     unsigned long long system_ram,
+			     unsigned long long *crash_size,
+			     unsigned long long *crash_base)
+{
+	return __parse_crashkernel(cmdline, system_ram, crash_size, crash_base,
+				"crashkernel=", suffix_tbl[SUFFIX_HIGH]);
 }
 
 int __init parse_crashkernel_low(char *cmdline,
@@ -1428,7 +1516,7 @@ int __init parse_crashkernel_low(char *cmdline,
 			     unsigned long long *crash_base)
 {
 	return __parse_crashkernel(cmdline, system_ram, crash_size, crash_base,
-					"crashkernel_low=");
+				"crashkernel=", suffix_tbl[SUFFIX_LOW]);
 }
 
 static void update_vmcoreinfo_note(void)
@@ -1489,7 +1577,7 @@ static int __init crash_save_vmcoreinfo_init(void)
 	VMCOREINFO_SYMBOL(swapper_pg_dir);
 #endif
 	VMCOREINFO_SYMBOL(_stext);
-	VMCOREINFO_SYMBOL(vmlist);
+	VMCOREINFO_SYMBOL(vmap_area_list);
 
 #ifndef CONFIG_NEED_MULTIPLE_NODES
 	VMCOREINFO_SYMBOL(mem_map);
@@ -1527,7 +1615,8 @@ static int __init crash_save_vmcoreinfo_init(void)
 	VMCOREINFO_OFFSET(free_area, free_list);
 	VMCOREINFO_OFFSET(list_head, next);
 	VMCOREINFO_OFFSET(list_head, prev);
-	VMCOREINFO_OFFSET(vm_struct, addr);
+	VMCOREINFO_OFFSET(vmap_area, va_start);
+	VMCOREINFO_OFFSET(vmap_area, list);
 	VMCOREINFO_LENGTH(zone.free_area, MAX_ORDER);
 	log_buf_kexec_setup();
 	VMCOREINFO_LENGTH(free_area.free_list, MIGRATE_TYPES);
diff --git a/kernel/kprobes.c b/kernel/kprobes.c
index e35be53f661..3fed7f0cbcd 100644
--- a/kernel/kprobes.c
+++ b/kernel/kprobes.c
@@ -794,16 +794,16 @@ out:
 }
 
 #ifdef CONFIG_SYSCTL
-/* This should be called with kprobe_mutex locked */
 static void __kprobes optimize_all_kprobes(void)
 {
 	struct hlist_head *head;
 	struct kprobe *p;
 	unsigned int i;
 
+	mutex_lock(&kprobe_mutex);
 	/* If optimization is already allowed, just return */
 	if (kprobes_allow_optimization)
-		return;
+		goto out;
 
 	kprobes_allow_optimization = true;
 	for (i = 0; i < KPROBE_TABLE_SIZE; i++) {
@@ -813,18 +813,22 @@ static void __kprobes optimize_all_kprobes(void)
 				optimize_kprobe(p);
 	}
 	printk(KERN_INFO "Kprobes globally optimized\n");
+out:
+	mutex_unlock(&kprobe_mutex);
 }
 
-/* This should be called with kprobe_mutex locked */
 static void __kprobes unoptimize_all_kprobes(void)
 {
 	struct hlist_head *head;
 	struct kprobe *p;
 	unsigned int i;
 
+	mutex_lock(&kprobe_mutex);
 	/* If optimization is already prohibited, just return */
-	if (!kprobes_allow_optimization)
+	if (!kprobes_allow_optimization) {
+		mutex_unlock(&kprobe_mutex);
 		return;
+	}
 
 	kprobes_allow_optimization = false;
 	for (i = 0; i < KPROBE_TABLE_SIZE; i++) {
@@ -834,11 +838,14 @@ static void __kprobes unoptimize_all_kprobes(void)
 				unoptimize_kprobe(p, false);
 		}
 	}
+	mutex_unlock(&kprobe_mutex);
+
 	/* Wait for unoptimizing completion */
 	wait_for_kprobe_optimizer();
 	printk(KERN_INFO "Kprobes globally unoptimized\n");
 }
 
+static DEFINE_MUTEX(kprobe_sysctl_mutex);
 int sysctl_kprobes_optimization;
 int proc_kprobes_optimization_handler(struct ctl_table *table, int write,
 				      void __user *buffer, size_t *length,
@@ -846,7 +853,7 @@ int proc_kprobes_optimization_handler(struct ctl_table *table, int write,
 {
 	int ret;
 
-	mutex_lock(&kprobe_mutex);
+	mutex_lock(&kprobe_sysctl_mutex);
 	sysctl_kprobes_optimization = kprobes_allow_optimization ? 1 : 0;
 	ret = proc_dointvec_minmax(table, write, buffer, length, ppos);
 
@@ -854,7 +861,7 @@ int proc_kprobes_optimization_handler(struct ctl_table *table, int write,
 		optimize_all_kprobes();
 	else
 		unoptimize_all_kprobes();
-	mutex_unlock(&kprobe_mutex);
+	mutex_unlock(&kprobe_sysctl_mutex);
 
 	return ret;
 }
diff --git a/kernel/kthread.c b/kernel/kthread.c
index 691dc2ef9ba..16d8ddd268b 100644
--- a/kernel/kthread.c
+++ b/kernel/kthread.c
@@ -52,8 +52,21 @@ enum KTHREAD_BITS {
 	KTHREAD_IS_PARKED,
 };
 
-#define to_kthread(tsk)	\
-	container_of((tsk)->vfork_done, struct kthread, exited)
+#define __to_kthread(vfork)	\
+	container_of(vfork, struct kthread, exited)
+
+static inline struct kthread *to_kthread(struct task_struct *k)
+{
+	return __to_kthread(k->vfork_done);
+}
+
+static struct kthread *to_live_kthread(struct task_struct *k)
+{
+	struct completion *vfork = ACCESS_ONCE(k->vfork_done);
+	if (likely(vfork))
+		return __to_kthread(vfork);
+	return NULL;
+}
 
 /**
  * kthread_should_stop - should this kthread return now?
@@ -124,12 +137,12 @@ void *kthread_data(struct task_struct *task)
 
 static void __kthread_parkme(struct kthread *self)
 {
-	__set_current_state(TASK_INTERRUPTIBLE);
+	__set_current_state(TASK_PARKED);
 	while (test_bit(KTHREAD_SHOULD_PARK, &self->flags)) {
 		if (!test_and_set_bit(KTHREAD_IS_PARKED, &self->flags))
 			complete(&self->parked);
 		schedule();
-		__set_current_state(TASK_INTERRUPTIBLE);
+		__set_current_state(TASK_PARKED);
 	}
 	clear_bit(KTHREAD_IS_PARKED, &self->flags);
 	__set_current_state(TASK_RUNNING);
@@ -256,11 +269,16 @@ struct task_struct *kthread_create_on_node(int (*threadfn)(void *data),
 }
 EXPORT_SYMBOL(kthread_create_on_node);
 
-static void __kthread_bind(struct task_struct *p, unsigned int cpu)
+static void __kthread_bind(struct task_struct *p, unsigned int cpu, long state)
 {
+	/* Must have done schedule() in kthread() before we set_task_cpu */
+	if (!wait_task_inactive(p, state)) {
+		WARN_ON(1);
+		return;
+	}
 	/* It's safe because the task is inactive. */
 	do_set_cpus_allowed(p, cpumask_of(cpu));
-	p->flags |= PF_THREAD_BOUND;
+	p->flags |= PF_NO_SETAFFINITY;
 }
 
 /**
@@ -274,12 +292,7 @@ static void __kthread_bind(struct task_struct *p, unsigned int cpu)
  */
 void kthread_bind(struct task_struct *p, unsigned int cpu)
 {
-	/* Must have done schedule() in kthread() before we set_task_cpu */
-	if (!wait_task_inactive(p, TASK_UNINTERRUPTIBLE)) {
-		WARN_ON(1);
-		return;
-	}
-	__kthread_bind(p, cpu);
+	__kthread_bind(p, cpu, TASK_UNINTERRUPTIBLE);
 }
 EXPORT_SYMBOL(kthread_bind);
 
@@ -311,17 +324,20 @@ struct task_struct *kthread_create_on_cpu(int (*threadfn)(void *data),
 	return p;
 }
 
-static struct kthread *task_get_live_kthread(struct task_struct *k)
+static void __kthread_unpark(struct task_struct *k, struct kthread *kthread)
 {
-	struct kthread *kthread;
-
-	get_task_struct(k);
-	kthread = to_kthread(k);
-	/* It might have exited */
-	barrier();
-	if (k->vfork_done != NULL)
-		return kthread;
-	return NULL;
+	clear_bit(KTHREAD_SHOULD_PARK, &kthread->flags);
+	/*
+	 * We clear the IS_PARKED bit here as we don't wait
+	 * until the task has left the park code. So if we'd
+	 * park before that happens we'd see the IS_PARKED bit
+	 * which might be about to be cleared.
+	 */
+	if (test_and_clear_bit(KTHREAD_IS_PARKED, &kthread->flags)) {
+		if (test_bit(KTHREAD_IS_PER_CPU, &kthread->flags))
+			__kthread_bind(k, kthread->cpu, TASK_PARKED);
+		wake_up_state(k, TASK_PARKED);
+	}
 }
 
 /**
@@ -334,23 +350,10 @@ static struct kthread *task_get_live_kthread(struct task_struct *k)
  */
 void kthread_unpark(struct task_struct *k)
 {
-	struct kthread *kthread = task_get_live_kthread(k);
+	struct kthread *kthread = to_live_kthread(k);
 
-	if (kthread) {
-		clear_bit(KTHREAD_SHOULD_PARK, &kthread->flags);
-		/*
-		 * We clear the IS_PARKED bit here as we don't wait
-		 * until the task has left the park code. So if we'd
-		 * park before that happens we'd see the IS_PARKED bit
-		 * which might be about to be cleared.
-		 */
-		if (test_and_clear_bit(KTHREAD_IS_PARKED, &kthread->flags)) {
-			if (test_bit(KTHREAD_IS_PER_CPU, &kthread->flags))
-				__kthread_bind(k, kthread->cpu);
-			wake_up_process(k);
-		}
-	}
-	put_task_struct(k);
+	if (kthread)
+		__kthread_unpark(k, kthread);
 }
 
 /**
@@ -367,7 +370,7 @@ void kthread_unpark(struct task_struct *k)
  */
 int kthread_park(struct task_struct *k)
 {
-	struct kthread *kthread = task_get_live_kthread(k);
+	struct kthread *kthread = to_live_kthread(k);
 	int ret = -ENOSYS;
 
 	if (kthread) {
@@ -380,7 +383,6 @@ int kthread_park(struct task_struct *k)
 		}
 		ret = 0;
 	}
-	put_task_struct(k);
 	return ret;
 }
 
@@ -401,21 +403,23 @@ int kthread_park(struct task_struct *k)
  */
 int kthread_stop(struct task_struct *k)
 {
-	struct kthread *kthread = task_get_live_kthread(k);
+	struct kthread *kthread;
 	int ret;
 
 	trace_sched_kthread_stop(k);
+
+	get_task_struct(k);
+	kthread = to_live_kthread(k);
 	if (kthread) {
 		set_bit(KTHREAD_SHOULD_STOP, &kthread->flags);
-		clear_bit(KTHREAD_SHOULD_PARK, &kthread->flags);
+		__kthread_unpark(k, kthread);
 		wake_up_process(k);
 		wait_for_completion(&kthread->exited);
 	}
 	ret = k->exit_code;
-
 	put_task_struct(k);
-	trace_sched_kthread_stop_ret(ret);
 
+	trace_sched_kthread_stop_ret(ret);
 	return ret;
 }
 EXPORT_SYMBOL(kthread_stop);
diff --git a/kernel/lockdep.c b/kernel/lockdep.c
index 259db207b5d..6a3bccba7e7 100644
--- a/kernel/lockdep.c
+++ b/kernel/lockdep.c
@@ -380,6 +380,13 @@ static int verbose(struct lock_class *class)
 unsigned long nr_stack_trace_entries;
 static unsigned long stack_trace[MAX_STACK_TRACE_ENTRIES];
 
+static void print_lockdep_off(const char *bug_msg)
+{
+	printk(KERN_DEBUG "%s\n", bug_msg);
+	printk(KERN_DEBUG "turning off the locking correctness validator.\n");
+	printk(KERN_DEBUG "Please attach the output of /proc/lock_stat to the bug report\n");
+}
+
 static int save_trace(struct stack_trace *trace)
 {
 	trace->nr_entries = 0;
@@ -409,8 +416,7 @@ static int save_trace(struct stack_trace *trace)
 		if (!debug_locks_off_graph_unlock())
 			return 0;
 
-		printk("BUG: MAX_STACK_TRACE_ENTRIES too low!\n");
-		printk("turning off the locking correctness validator.\n");
+		print_lockdep_off("BUG: MAX_STACK_TRACE_ENTRIES too low!");
 		dump_stack();
 
 		return 0;
@@ -763,8 +769,7 @@ register_lock_class(struct lockdep_map *lock, unsigned int subclass, int force)
 		}
 		raw_local_irq_restore(flags);
 
-		printk("BUG: MAX_LOCKDEP_KEYS too low!\n");
-		printk("turning off the locking correctness validator.\n");
+		print_lockdep_off("BUG: MAX_LOCKDEP_KEYS too low!");
 		dump_stack();
 		return NULL;
 	}
@@ -834,8 +839,7 @@ static struct lock_list *alloc_list_entry(void)
 		if (!debug_locks_off_graph_unlock())
 			return NULL;
 
-		printk("BUG: MAX_LOCKDEP_ENTRIES too low!\n");
-		printk("turning off the locking correctness validator.\n");
+		print_lockdep_off("BUG: MAX_LOCKDEP_ENTRIES too low!");
 		dump_stack();
 		return NULL;
 	}
@@ -2000,7 +2004,7 @@ static inline int lookup_chain_cache(struct task_struct *curr,
 	struct lock_class *class = hlock_class(hlock);
 	struct list_head *hash_head = chainhashentry(chain_key);
 	struct lock_chain *chain;
-	struct held_lock *hlock_curr, *hlock_next;
+	struct held_lock *hlock_curr;
 	int i, j;
 
 	/*
@@ -2048,8 +2052,7 @@ cache_hit:
 		if (!debug_locks_off_graph_unlock())
 			return 0;
 
-		printk("BUG: MAX_LOCKDEP_CHAINS too low!\n");
-		printk("turning off the locking correctness validator.\n");
+		print_lockdep_off("BUG: MAX_LOCKDEP_CHAINS too low!");
 		dump_stack();
 		return 0;
 	}
@@ -2057,12 +2060,10 @@ cache_hit:
 	chain->chain_key = chain_key;
 	chain->irq_context = hlock->irq_context;
 	/* Find the first held_lock of current chain */
-	hlock_next = hlock;
 	for (i = curr->lockdep_depth - 1; i >= 0; i--) {
 		hlock_curr = curr->held_locks + i;
-		if (hlock_curr->irq_context != hlock_next->irq_context)
+		if (hlock_curr->irq_context != hlock->irq_context)
 			break;
-		hlock_next = hlock;
 	}
 	i++;
 	chain->depth = curr->lockdep_depth + 1 - i;
@@ -3190,9 +3191,9 @@ static int __lock_acquire(struct lockdep_map *lock, unsigned int subclass,
 #endif
 	if (unlikely(curr->lockdep_depth >= MAX_LOCK_DEPTH)) {
 		debug_locks_off();
-		printk("BUG: MAX_LOCK_DEPTH too low, depth: %i  max: %lu!\n",
+		print_lockdep_off("BUG: MAX_LOCK_DEPTH too low!");
+		printk(KERN_DEBUG "depth: %i  max: %lu!\n",
 		       curr->lockdep_depth, MAX_LOCK_DEPTH);
-		printk("turning off the locking correctness validator.\n");
 
 		lockdep_print_held_locks(current);
 		debug_show_all_locks();
@@ -4088,7 +4089,7 @@ void debug_check_no_locks_freed(const void *mem_from, unsigned long mem_len)
 }
 EXPORT_SYMBOL_GPL(debug_check_no_locks_freed);
 
-static void print_held_locks_bug(void)
+static void print_held_locks_bug(struct task_struct *curr)
 {
 	if (!debug_locks_off())
 		return;
@@ -4097,21 +4098,22 @@ static void print_held_locks_bug(void)
 
 	printk("\n");
 	printk("=====================================\n");
-	printk("[ BUG: %s/%d still has locks held! ]\n",
-	       current->comm, task_pid_nr(current));
+	printk("[ BUG: lock held at task exit time! ]\n");
 	print_kernel_ident();
 	printk("-------------------------------------\n");
-	lockdep_print_held_locks(current);
+	printk("%s/%d is exiting with locks still held!\n",
+		curr->comm, task_pid_nr(curr));
+	lockdep_print_held_locks(curr);
+
 	printk("\nstack backtrace:\n");
 	dump_stack();
 }
 
-void debug_check_no_locks_held(void)
+void debug_check_no_locks_held(struct task_struct *task)
 {
-	if (unlikely(current->lockdep_depth > 0))
-		print_held_locks_bug();
+	if (unlikely(task->lockdep_depth > 0))
+		print_held_locks_bug(task);
 }
-EXPORT_SYMBOL_GPL(debug_check_no_locks_held);
 
 void debug_show_all_locks(void)
 {
diff --git a/kernel/mutex.c b/kernel/mutex.c
index 52f23011b6e..ad53a664f11 100644
--- a/kernel/mutex.c
+++ b/kernel/mutex.c
@@ -37,6 +37,12 @@
 # include <asm/mutex.h>
 #endif
 
+/*
+ * A negative mutex count indicates that waiters are sleeping waiting for the
+ * mutex.
+ */
+#define	MUTEX_SHOW_NO_WAITER(mutex)	(atomic_read(&(mutex)->count) >= 0)
+
 void
 __mutex_init(struct mutex *lock, const char *name, struct lock_class_key *key)
 {
@@ -44,6 +50,9 @@ __mutex_init(struct mutex *lock, const char *name, struct lock_class_key *key)
 	spin_lock_init(&lock->wait_lock);
 	INIT_LIST_HEAD(&lock->wait_list);
 	mutex_clear_owner(lock);
+#ifdef CONFIG_MUTEX_SPIN_ON_OWNER
+	lock->spin_mlock = NULL;
+#endif
 
 	debug_mutex_init(lock, name, key);
 }
@@ -95,6 +104,124 @@ void __sched mutex_lock(struct mutex *lock)
 EXPORT_SYMBOL(mutex_lock);
 #endif
 
+#ifdef CONFIG_MUTEX_SPIN_ON_OWNER
+/*
+ * In order to avoid a stampede of mutex spinners from acquiring the mutex
+ * more or less simultaneously, the spinners need to acquire a MCS lock
+ * first before spinning on the owner field.
+ *
+ * We don't inline mspin_lock() so that perf can correctly account for the
+ * time spent in this lock function.
+ */
+struct mspin_node {
+	struct mspin_node *next ;
+	int		  locked;	/* 1 if lock acquired */
+};
+#define	MLOCK(mutex)	((struct mspin_node **)&((mutex)->spin_mlock))
+
+static noinline
+void mspin_lock(struct mspin_node **lock, struct mspin_node *node)
+{
+	struct mspin_node *prev;
+
+	/* Init node */
+	node->locked = 0;
+	node->next   = NULL;
+
+	prev = xchg(lock, node);
+	if (likely(prev == NULL)) {
+		/* Lock acquired */
+		node->locked = 1;
+		return;
+	}
+	ACCESS_ONCE(prev->next) = node;
+	smp_wmb();
+	/* Wait until the lock holder passes the lock down */
+	while (!ACCESS_ONCE(node->locked))
+		arch_mutex_cpu_relax();
+}
+
+static void mspin_unlock(struct mspin_node **lock, struct mspin_node *node)
+{
+	struct mspin_node *next = ACCESS_ONCE(node->next);
+
+	if (likely(!next)) {
+		/*
+		 * Release the lock by setting it to NULL
+		 */
+		if (cmpxchg(lock, node, NULL) == node)
+			return;
+		/* Wait until the next pointer is set */
+		while (!(next = ACCESS_ONCE(node->next)))
+			arch_mutex_cpu_relax();
+	}
+	ACCESS_ONCE(next->locked) = 1;
+	smp_wmb();
+}
+
+/*
+ * Mutex spinning code migrated from kernel/sched/core.c
+ */
+
+static inline bool owner_running(struct mutex *lock, struct task_struct *owner)
+{
+	if (lock->owner != owner)
+		return false;
+
+	/*
+	 * Ensure we emit the owner->on_cpu, dereference _after_ checking
+	 * lock->owner still matches owner, if that fails, owner might
+	 * point to free()d memory, if it still matches, the rcu_read_lock()
+	 * ensures the memory stays valid.
+	 */
+	barrier();
+
+	return owner->on_cpu;
+}
+
+/*
+ * Look out! "owner" is an entirely speculative pointer
+ * access and not reliable.
+ */
+static noinline
+int mutex_spin_on_owner(struct mutex *lock, struct task_struct *owner)
+{
+	rcu_read_lock();
+	while (owner_running(lock, owner)) {
+		if (need_resched())
+			break;
+
+		arch_mutex_cpu_relax();
+	}
+	rcu_read_unlock();
+
+	/*
+	 * We break out the loop above on need_resched() and when the
+	 * owner changed, which is a sign for heavy contention. Return
+	 * success only when lock->owner is NULL.
+	 */
+	return lock->owner == NULL;
+}
+
+/*
+ * Initial check for entering the mutex spinning loop
+ */
+static inline int mutex_can_spin_on_owner(struct mutex *lock)
+{
+	int retval = 1;
+
+	rcu_read_lock();
+	if (lock->owner)
+		retval = lock->owner->on_cpu;
+	rcu_read_unlock();
+	/*
+	 * if lock->owner is not set, the mutex owner may have just acquired
+	 * it and not set the owner yet or the mutex has been released.
+	 */
+	return retval;
+}
+#endif
+
 static __used noinline void __sched __mutex_unlock_slowpath(atomic_t *lock_count);
 
 /**
@@ -158,25 +285,39 @@ __mutex_lock_common(struct mutex *lock, long state, unsigned int subclass,
 	 *
 	 * We can't do this for DEBUG_MUTEXES because that relies on wait_lock
 	 * to serialize everything.
+	 *
+	 * The mutex spinners are queued up using MCS lock so that only one
+	 * spinner can compete for the mutex. However, if mutex spinning isn't
+	 * going to happen, there is no point in going through the lock/unlock
+	 * overhead.
 	 */
+	if (!mutex_can_spin_on_owner(lock))
+		goto slowpath;
 
 	for (;;) {
 		struct task_struct *owner;
+		struct mspin_node  node;
 
 		/*
 		 * If there's an owner, wait for it to either
 		 * release the lock or go to sleep.
 		 */
+		mspin_lock(MLOCK(lock), &node);
 		owner = ACCESS_ONCE(lock->owner);
-		if (owner && !mutex_spin_on_owner(lock, owner))
+		if (owner && !mutex_spin_on_owner(lock, owner)) {
+			mspin_unlock(MLOCK(lock), &node);
 			break;
+		}
 
-		if (atomic_cmpxchg(&lock->count, 1, 0) == 1) {
+		if ((atomic_read(&lock->count) == 1) &&
+		    (atomic_cmpxchg(&lock->count, 1, 0) == 1)) {
 			lock_acquired(&lock->dep_map, ip);
 			mutex_set_owner(lock);
+			mspin_unlock(MLOCK(lock), &node);
 			preempt_enable();
 			return 0;
 		}
+		mspin_unlock(MLOCK(lock), &node);
 
 		/*
 		 * When there's no owner, we might have preempted between the
@@ -195,6 +336,7 @@ __mutex_lock_common(struct mutex *lock, long state, unsigned int subclass,
 		 */
 		arch_mutex_cpu_relax();
 	}
+slowpath:
 #endif
 	spin_lock_mutex(&lock->wait_lock, flags);
 
@@ -205,7 +347,7 @@ __mutex_lock_common(struct mutex *lock, long state, unsigned int subclass,
 	list_add_tail(&waiter.list, &lock->wait_list);
 	waiter.task = task;
 
-	if (atomic_xchg(&lock->count, -1) == 1)
+	if (MUTEX_SHOW_NO_WAITER(lock) && (atomic_xchg(&lock->count, -1) == 1))
 		goto done;
 
 	lock_contended(&lock->dep_map, ip);
@@ -220,7 +362,8 @@ __mutex_lock_common(struct mutex *lock, long state, unsigned int subclass,
 		 * that when we release the lock, we properly wake up the
 		 * other waiters:
 		 */
-		if (atomic_xchg(&lock->count, -1) == 1)
+		if (MUTEX_SHOW_NO_WAITER(lock) &&
+		   (atomic_xchg(&lock->count, -1) == 1))
 			break;
 
 		/*
diff --git a/kernel/pid_namespace.c b/kernel/pid_namespace.c
index c1c3dc1c602..bea15bdf82b 100644
--- a/kernel/pid_namespace.c
+++ b/kernel/pid_namespace.c
@@ -181,6 +181,7 @@ void zap_pid_ns_processes(struct pid_namespace *pid_ns)
 	int nr;
 	int rc;
 	struct task_struct *task, *me = current;
+	int init_pids = thread_group_leader(me) ? 1 : 2;
 
 	/* Don't allow any more processes into the pid namespace */
 	disable_pid_allocation(pid_ns);
@@ -230,7 +231,7 @@ void zap_pid_ns_processes(struct pid_namespace *pid_ns)
 	 */
 	for (;;) {
 		set_current_state(TASK_UNINTERRUPTIBLE);
-		if (pid_ns->nr_hashed == 1)
+		if (pid_ns->nr_hashed == init_pids)
 			break;
 		schedule();
 	}
diff --git a/kernel/posix-timers.c b/kernel/posix-timers.c
index 6edbb2c55c2..424c2d4265c 100644
--- a/kernel/posix-timers.c
+++ b/kernel/posix-timers.c
@@ -40,38 +40,31 @@
 #include <linux/list.h>
 #include <linux/init.h>
 #include <linux/compiler.h>
-#include <linux/idr.h>
+#include <linux/hash.h>
 #include <linux/posix-clock.h>
 #include <linux/posix-timers.h>
 #include <linux/syscalls.h>
 #include <linux/wait.h>
 #include <linux/workqueue.h>
 #include <linux/export.h>
+#include <linux/hashtable.h>
 
 /*
- * Management arrays for POSIX timers.	 Timers are kept in slab memory
- * Timer ids are allocated by an external routine that keeps track of the
- * id and the timer.  The external interface is:
- *
- * void *idr_find(struct idr *idp, int id);           to find timer_id <id>
- * int idr_get_new(struct idr *idp, void *ptr);       to get a new id and
- *                                                    related it to <ptr>
- * void idr_remove(struct idr *idp, int id);          to release <id>
- * void idr_init(struct idr *idp);                    to initialize <idp>
- *                                                    which we supply.
- * The idr_get_new *may* call slab for more memory so it must not be
- * called under a spin lock.  Likewise idr_remore may release memory
- * (but it may be ok to do this under a lock...).
- * idr_find is just a memory look up and is quite fast.  A -1 return
- * indicates that the requested id does not exist.
+ * Management arrays for POSIX timers. Timers are now kept in static hash table
+ * with 512 entries.
+ * Timer ids are allocated by local routine, which selects proper hash head by
+ * key, constructed from current->signal address and per signal struct counter.
+ * This keeps timer ids unique per process, but now they can intersect between
+ * processes.
  */
 
 /*
  * Lets keep our timers in a slab cache :-)
  */
 static struct kmem_cache *posix_timers_cache;
-static struct idr posix_timers_id;
-static DEFINE_SPINLOCK(idr_lock);
+
+static DEFINE_HASHTABLE(posix_timers_hashtable, 9);
+static DEFINE_SPINLOCK(hash_lock);
 
 /*
  * we assume that the new SIGEV_THREAD_ID shares no bits with the other
@@ -152,6 +145,56 @@ static struct k_itimer *__lock_timer(timer_t timer_id, unsigned long *flags);
 	__timr;								   \
 })
 
+static int hash(struct signal_struct *sig, unsigned int nr)
+{
+	return hash_32(hash32_ptr(sig) ^ nr, HASH_BITS(posix_timers_hashtable));
+}
+
+static struct k_itimer *__posix_timers_find(struct hlist_head *head,
+					    struct signal_struct *sig,
+					    timer_t id)
+{
+	struct k_itimer *timer;
+
+	hlist_for_each_entry_rcu(timer, head, t_hash) {
+		if ((timer->it_signal == sig) && (timer->it_id == id))
+			return timer;
+	}
+	return NULL;
+}
+
+static struct k_itimer *posix_timer_by_id(timer_t id)
+{
+	struct signal_struct *sig = current->signal;
+	struct hlist_head *head = &posix_timers_hashtable[hash(sig, id)];
+
+	return __posix_timers_find(head, sig, id);
+}
+
+static int posix_timer_add(struct k_itimer *timer)
+{
+	struct signal_struct *sig = current->signal;
+	int first_free_id = sig->posix_timer_id;
+	struct hlist_head *head;
+	int ret = -ENOENT;
+
+	do {
+		spin_lock(&hash_lock);
+		head = &posix_timers_hashtable[hash(sig, sig->posix_timer_id)];
+		if (!__posix_timers_find(head, sig, sig->posix_timer_id)) {
+			hlist_add_head_rcu(&timer->t_hash, head);
+			ret = sig->posix_timer_id;
+		}
+		if (++sig->posix_timer_id < 0)
+			sig->posix_timer_id = 0;
+		if ((sig->posix_timer_id == first_free_id) && (ret == -ENOENT))
+			/* Loop over all possible ids completed */
+			ret = -EAGAIN;
+		spin_unlock(&hash_lock);
+	} while (ret == -ENOENT);
+	return ret;
+}
+
 static inline void unlock_timer(struct k_itimer *timr, unsigned long flags)
 {
 	spin_unlock_irqrestore(&timr->it_lock, flags);
@@ -221,6 +264,11 @@ static int posix_get_boottime(const clockid_t which_clock, struct timespec *tp)
 	return 0;
 }
 
+static int posix_get_tai(clockid_t which_clock, struct timespec *tp)
+{
+	timekeeping_clocktai(tp);
+	return 0;
+}
 
 /*
  * Initialize everything, well, just everything in Posix clocks/timers ;)
@@ -261,6 +309,16 @@ static __init int init_posix_timers(void)
 		.clock_getres	= posix_get_coarse_res,
 		.clock_get	= posix_get_monotonic_coarse,
 	};
+	struct k_clock clock_tai = {
+		.clock_getres	= hrtimer_get_res,
+		.clock_get	= posix_get_tai,
+		.nsleep		= common_nsleep,
+		.nsleep_restart	= hrtimer_nanosleep_restart,
+		.timer_create	= common_timer_create,
+		.timer_set	= common_timer_set,
+		.timer_get	= common_timer_get,
+		.timer_del	= common_timer_del,
+	};
 	struct k_clock clock_boottime = {
 		.clock_getres	= hrtimer_get_res,
 		.clock_get	= posix_get_boottime,
@@ -278,11 +336,11 @@ static __init int init_posix_timers(void)
 	posix_timers_register_clock(CLOCK_REALTIME_COARSE, &clock_realtime_coarse);
 	posix_timers_register_clock(CLOCK_MONOTONIC_COARSE, &clock_monotonic_coarse);
 	posix_timers_register_clock(CLOCK_BOOTTIME, &clock_boottime);
+	posix_timers_register_clock(CLOCK_TAI, &clock_tai);
 
 	posix_timers_cache = kmem_cache_create("posix_timers_cache",
 					sizeof (struct k_itimer), 0, SLAB_PANIC,
 					NULL);
-	idr_init(&posix_timers_id);
 	return 0;
 }
 
@@ -504,9 +562,9 @@ static void release_posix_timer(struct k_itimer *tmr, int it_id_set)
 {
 	if (it_id_set) {
 		unsigned long flags;
-		spin_lock_irqsave(&idr_lock, flags);
-		idr_remove(&posix_timers_id, tmr->it_id);
-		spin_unlock_irqrestore(&idr_lock, flags);
+		spin_lock_irqsave(&hash_lock, flags);
+		hlist_del_rcu(&tmr->t_hash);
+		spin_unlock_irqrestore(&hash_lock, flags);
 	}
 	put_pid(tmr->it_pid);
 	sigqueue_free(tmr->sigq);
@@ -552,22 +610,11 @@ SYSCALL_DEFINE3(timer_create, const clockid_t, which_clock,
 		return -EAGAIN;
 
 	spin_lock_init(&new_timer->it_lock);
-
-	idr_preload(GFP_KERNEL);
-	spin_lock_irq(&idr_lock);
-	error = idr_alloc(&posix_timers_id, new_timer, 0, 0, GFP_NOWAIT);
-	spin_unlock_irq(&idr_lock);
-	idr_preload_end();
-	if (error < 0) {
-		/*
-		 * Weird looking, but we return EAGAIN if the IDR is
-		 * full (proper POSIX return value for this)
-		 */
-		if (error == -ENOSPC)
-			error = -EAGAIN;
+	new_timer_id = posix_timer_add(new_timer);
+	if (new_timer_id < 0) {
+		error = new_timer_id;
 		goto out;
 	}
-	new_timer_id = error;
 
 	it_id_set = IT_ID_SET;
 	new_timer->it_id = (timer_t) new_timer_id;
@@ -645,7 +692,7 @@ static struct k_itimer *__lock_timer(timer_t timer_id, unsigned long *flags)
 		return NULL;
 
 	rcu_read_lock();
-	timr = idr_find(&posix_timers_id, (int)timer_id);
+	timr = posix_timer_by_id(timer_id);
 	if (timr) {
 		spin_lock_irqsave(&timr->it_lock, *flags);
 		if (timr->it_signal == current->signal) {
diff --git a/kernel/printk.c b/kernel/printk.c
index abbdd9e2ac8..376914e2869 100644
--- a/kernel/printk.c
+++ b/kernel/printk.c
@@ -49,13 +49,6 @@
 #define CREATE_TRACE_POINTS
 #include <trace/events/printk.h>
 
-/*
- * Architectures can override it:
- */
-void asmlinkage __attribute__((weak)) early_printk(const char *fmt, ...)
-{
-}
-
 /* printk's without a loglevel use this.. */
 #define DEFAULT_MESSAGE_LOGLEVEL CONFIG_DEFAULT_MESSAGE_LOGLEVEL
 
@@ -608,7 +601,8 @@ static unsigned int devkmsg_poll(struct file *file, poll_table *wait)
 		/* return error when data has vanished underneath us */
 		if (user->seq < log_first_seq)
 			ret = POLLIN|POLLRDNORM|POLLERR|POLLPRI;
-		ret = POLLIN|POLLRDNORM;
+		else
+			ret = POLLIN|POLLRDNORM;
 	}
 	raw_spin_unlock_irq(&logbuf_lock);
 
@@ -1265,7 +1259,7 @@ static void call_console_drivers(int level, const char *text, size_t len)
 {
 	struct console *con;
 
-	trace_console(text, 0, len, len);
+	trace_console(text, len);
 
 	if (level >= console_loglevel && !ignore_loglevel)
 		return;
@@ -1723,6 +1717,29 @@ static size_t cont_print_text(char *text, size_t size) { return 0; }
 
 #endif /* CONFIG_PRINTK */
 
+#ifdef CONFIG_EARLY_PRINTK
+struct console *early_console;
+
+void early_vprintk(const char *fmt, va_list ap)
+{
+	if (early_console) {
+		char buf[512];
+		int n = vscnprintf(buf, sizeof(buf), fmt, ap);
+
+		early_console->write(early_console, buf, n);
+	}
+}
+
+asmlinkage void early_printk(const char *fmt, ...)
+{
+	va_list ap;
+
+	va_start(ap, fmt);
+	early_vprintk(fmt, ap);
+	va_end(ap);
+}
+#endif
+
 static int __add_preferred_console(char *name, int idx, char *options,
 				   char *brl_options)
 {
diff --git a/kernel/rcutree.c b/kernel/rcutree.c
index 5b8ad827fd8..d8534308fd0 100644
--- a/kernel/rcutree.c
+++ b/kernel/rcutree.c
@@ -64,7 +64,7 @@
 static struct lock_class_key rcu_node_class[RCU_NUM_LVLS];
 static struct lock_class_key rcu_fqs_class[RCU_NUM_LVLS];
 
-#define RCU_STATE_INITIALIZER(sname, cr) { \
+#define RCU_STATE_INITIALIZER(sname, sabbr, cr) { \
 	.level = { &sname##_state.node[0] }, \
 	.call = cr, \
 	.fqs_state = RCU_GP_IDLE, \
@@ -76,13 +76,14 @@ static struct lock_class_key rcu_fqs_class[RCU_NUM_LVLS];
 	.barrier_mutex = __MUTEX_INITIALIZER(sname##_state.barrier_mutex), \
 	.onoff_mutex = __MUTEX_INITIALIZER(sname##_state.onoff_mutex), \
 	.name = #sname, \
+	.abbr = sabbr, \
 }
 
 struct rcu_state rcu_sched_state =
-	RCU_STATE_INITIALIZER(rcu_sched, call_rcu_sched);
+	RCU_STATE_INITIALIZER(rcu_sched, 's', call_rcu_sched);
 DEFINE_PER_CPU(struct rcu_data, rcu_sched_data);
 
-struct rcu_state rcu_bh_state = RCU_STATE_INITIALIZER(rcu_bh, call_rcu_bh);
+struct rcu_state rcu_bh_state = RCU_STATE_INITIALIZER(rcu_bh, 'b', call_rcu_bh);
 DEFINE_PER_CPU(struct rcu_data, rcu_bh_data);
 
 static struct rcu_state *rcu_state;
@@ -223,6 +224,8 @@ static ulong jiffies_till_next_fqs = RCU_JIFFIES_TILL_FORCE_QS;
 module_param(jiffies_till_first_fqs, ulong, 0644);
 module_param(jiffies_till_next_fqs, ulong, 0644);
 
+static void rcu_start_gp_advanced(struct rcu_state *rsp, struct rcu_node *rnp,
+				  struct rcu_data *rdp);
 static void force_qs_rnp(struct rcu_state *rsp, int (*f)(struct rcu_data *));
 static void force_quiescent_state(struct rcu_state *rsp);
 static int rcu_pending(int cpu);
@@ -310,6 +313,8 @@ cpu_needs_another_gp(struct rcu_state *rsp, struct rcu_data *rdp)
 
 	if (rcu_gp_in_progress(rsp))
 		return 0;  /* No, a grace period is already in progress. */
+	if (rcu_nocb_needs_gp(rsp))
+		return 1;  /* Yes, a no-CBs CPU needs one. */
 	if (!rdp->nxttail[RCU_NEXT_TAIL])
 		return 0;  /* No, this is a no-CBs (or offline) CPU. */
 	if (*rdp->nxttail[RCU_NEXT_READY_TAIL])
@@ -1035,10 +1040,11 @@ static void init_callback_list(struct rcu_data *rdp)
 {
 	int i;
 
+	if (init_nocb_callback_list(rdp))
+		return;
 	rdp->nxtlist = NULL;
 	for (i = 0; i < RCU_NEXT_SIZE; i++)
 		rdp->nxttail[i] = &rdp->nxtlist;
-	init_nocb_callback_list(rdp);
 }
 
 /*
@@ -1071,6 +1077,120 @@ static unsigned long rcu_cbs_completed(struct rcu_state *rsp,
 }
 
 /*
+ * Trace-event helper function for rcu_start_future_gp() and
+ * rcu_nocb_wait_gp().
+ */
+static void trace_rcu_future_gp(struct rcu_node *rnp, struct rcu_data *rdp,
+				unsigned long c, char *s)
+{
+	trace_rcu_future_grace_period(rdp->rsp->name, rnp->gpnum,
+				      rnp->completed, c, rnp->level,
+				      rnp->grplo, rnp->grphi, s);
+}
+
+/*
+ * Start some future grace period, as needed to handle newly arrived
+ * callbacks.  The required future grace periods are recorded in each
+ * rcu_node structure's ->need_future_gp field.
+ *
+ * The caller must hold the specified rcu_node structure's ->lock.
+ */
+static unsigned long __maybe_unused
+rcu_start_future_gp(struct rcu_node *rnp, struct rcu_data *rdp)
+{
+	unsigned long c;
+	int i;
+	struct rcu_node *rnp_root = rcu_get_root(rdp->rsp);
+
+	/*
+	 * Pick up grace-period number for new callbacks.  If this
+	 * grace period is already marked as needed, return to the caller.
+	 */
+	c = rcu_cbs_completed(rdp->rsp, rnp);
+	trace_rcu_future_gp(rnp, rdp, c, "Startleaf");
+	if (rnp->need_future_gp[c & 0x1]) {
+		trace_rcu_future_gp(rnp, rdp, c, "Prestartleaf");
+		return c;
+	}
+
+	/*
+	 * If either this rcu_node structure or the root rcu_node structure
+	 * believe that a grace period is in progress, then we must wait
+	 * for the one following, which is in "c".  Because our request
+	 * will be noticed at the end of the current grace period, we don't
+	 * need to explicitly start one.
+	 */
+	if (rnp->gpnum != rnp->completed ||
+	    ACCESS_ONCE(rnp->gpnum) != ACCESS_ONCE(rnp->completed)) {
+		rnp->need_future_gp[c & 0x1]++;
+		trace_rcu_future_gp(rnp, rdp, c, "Startedleaf");
+		return c;
+	}
+
+	/*
+	 * There might be no grace period in progress.  If we don't already
+	 * hold it, acquire the root rcu_node structure's lock in order to
+	 * start one (if needed).
+	 */
+	if (rnp != rnp_root)
+		raw_spin_lock(&rnp_root->lock);
+
+	/*
+	 * Get a new grace-period number.  If there really is no grace
+	 * period in progress, it will be smaller than the one we obtained
+	 * earlier.  Adjust callbacks as needed.  Note that even no-CBs
+	 * CPUs have a ->nxtcompleted[] array, so no no-CBs checks needed.
+	 */
+	c = rcu_cbs_completed(rdp->rsp, rnp_root);
+	for (i = RCU_DONE_TAIL; i < RCU_NEXT_TAIL; i++)
+		if (ULONG_CMP_LT(c, rdp->nxtcompleted[i]))
+			rdp->nxtcompleted[i] = c;
+
+	/*
+	 * If the needed for the required grace period is already
+	 * recorded, trace and leave.
+	 */
+	if (rnp_root->need_future_gp[c & 0x1]) {
+		trace_rcu_future_gp(rnp, rdp, c, "Prestartedroot");
+		goto unlock_out;
+	}
+
+	/* Record the need for the future grace period. */
+	rnp_root->need_future_gp[c & 0x1]++;
+
+	/* If a grace period is not already in progress, start one. */
+	if (rnp_root->gpnum != rnp_root->completed) {
+		trace_rcu_future_gp(rnp, rdp, c, "Startedleafroot");
+	} else {
+		trace_rcu_future_gp(rnp, rdp, c, "Startedroot");
+		rcu_start_gp_advanced(rdp->rsp, rnp_root, rdp);
+	}
+unlock_out:
+	if (rnp != rnp_root)
+		raw_spin_unlock(&rnp_root->lock);
+	return c;
+}
+
+/*
+ * Clean up any old requests for the just-ended grace period.  Also return
+ * whether any additional grace periods have been requested.  Also invoke
+ * rcu_nocb_gp_cleanup() in order to wake up any no-callbacks kthreads
+ * waiting for this grace period to complete.
+ */
+static int rcu_future_gp_cleanup(struct rcu_state *rsp, struct rcu_node *rnp)
+{
+	int c = rnp->completed;
+	int needmore;
+	struct rcu_data *rdp = this_cpu_ptr(rsp->rda);
+
+	rcu_nocb_gp_cleanup(rsp, rnp);
+	rnp->need_future_gp[c & 0x1] = 0;
+	needmore = rnp->need_future_gp[(c + 1) & 0x1];
+	trace_rcu_future_gp(rnp, rdp, c, needmore ? "CleanupMore" : "Cleanup");
+	return needmore;
+}
+
+/*
  * If there is room, assign a ->completed number to any callbacks on
  * this CPU that have not already been assigned.  Also accelerate any
  * callbacks that were previously assigned a ->completed number that has
@@ -1129,6 +1249,8 @@ static void rcu_accelerate_cbs(struct rcu_state *rsp, struct rcu_node *rnp,
 		rdp->nxttail[i] = rdp->nxttail[RCU_NEXT_TAIL];
 		rdp->nxtcompleted[i] = c;
 	}
+	/* Record any needed additional grace periods. */
+	rcu_start_future_gp(rnp, rdp);
 
 	/* Trace depending on how much we were able to accelerate. */
 	if (!*rdp->nxttail[RCU_WAIT_TAIL])
@@ -1308,9 +1430,9 @@ static int rcu_gp_init(struct rcu_state *rsp)
 		rdp = this_cpu_ptr(rsp->rda);
 		rcu_preempt_check_blocked_tasks(rnp);
 		rnp->qsmask = rnp->qsmaskinit;
-		rnp->gpnum = rsp->gpnum;
+		ACCESS_ONCE(rnp->gpnum) = rsp->gpnum;
 		WARN_ON_ONCE(rnp->completed != rsp->completed);
-		rnp->completed = rsp->completed;
+		ACCESS_ONCE(rnp->completed) = rsp->completed;
 		if (rnp == rdp->mynode)
 			rcu_start_gp_per_cpu(rsp, rnp, rdp);
 		rcu_preempt_boost_start_gp(rnp);
@@ -1319,7 +1441,8 @@ static int rcu_gp_init(struct rcu_state *rsp)
 					    rnp->grphi, rnp->qsmask);
 		raw_spin_unlock_irq(&rnp->lock);
 #ifdef CONFIG_PROVE_RCU_DELAY
-		if ((random32() % (rcu_num_nodes * 8)) == 0)
+		if ((prandom_u32() % (rcu_num_nodes * 8)) == 0 &&
+		    system_state == SYSTEM_RUNNING)
 			schedule_timeout_uninterruptible(2);
 #endif /* #ifdef CONFIG_PROVE_RCU_DELAY */
 		cond_resched();
@@ -1361,6 +1484,7 @@ int rcu_gp_fqs(struct rcu_state *rsp, int fqs_state_in)
 static void rcu_gp_cleanup(struct rcu_state *rsp)
 {
 	unsigned long gp_duration;
+	int nocb = 0;
 	struct rcu_data *rdp;
 	struct rcu_node *rnp = rcu_get_root(rsp);
 
@@ -1390,17 +1514,23 @@ static void rcu_gp_cleanup(struct rcu_state *rsp)
 	 */
 	rcu_for_each_node_breadth_first(rsp, rnp) {
 		raw_spin_lock_irq(&rnp->lock);
-		rnp->completed = rsp->gpnum;
+		ACCESS_ONCE(rnp->completed) = rsp->gpnum;
+		rdp = this_cpu_ptr(rsp->rda);
+		if (rnp == rdp->mynode)
+			__rcu_process_gp_end(rsp, rnp, rdp);
+		nocb += rcu_future_gp_cleanup(rsp, rnp);
 		raw_spin_unlock_irq(&rnp->lock);
 		cond_resched();
 	}
 	rnp = rcu_get_root(rsp);
 	raw_spin_lock_irq(&rnp->lock);
+	rcu_nocb_gp_set(rnp, nocb);
 
 	rsp->completed = rsp->gpnum; /* Declare grace period done. */
 	trace_rcu_grace_period(rsp->name, rsp->completed, "end");
 	rsp->fqs_state = RCU_GP_IDLE;
 	rdp = this_cpu_ptr(rsp->rda);
+	rcu_advance_cbs(rsp, rnp, rdp);  /* Reduce false positives below. */
 	if (cpu_needs_another_gp(rsp, rdp))
 		rsp->gp_flags = 1;
 	raw_spin_unlock_irq(&rnp->lock);
@@ -1476,57 +1606,62 @@ static int __noreturn rcu_gp_kthread(void *arg)
 /*
  * Start a new RCU grace period if warranted, re-initializing the hierarchy
  * in preparation for detecting the next grace period.  The caller must hold
- * the root node's ->lock, which is released before return.  Hard irqs must
- * be disabled.
+ * the root node's ->lock and hard irqs must be disabled.
  *
  * Note that it is legal for a dying CPU (which is marked as offline) to
  * invoke this function.  This can happen when the dying CPU reports its
  * quiescent state.
  */
 static void
-rcu_start_gp(struct rcu_state *rsp, unsigned long flags)
-	__releases(rcu_get_root(rsp)->lock)
+rcu_start_gp_advanced(struct rcu_state *rsp, struct rcu_node *rnp,
+		      struct rcu_data *rdp)
 {
-	struct rcu_data *rdp = this_cpu_ptr(rsp->rda);
-	struct rcu_node *rnp = rcu_get_root(rsp);
-
-	if (!rsp->gp_kthread ||
-	    !cpu_needs_another_gp(rsp, rdp)) {
+	if (!rsp->gp_kthread || !cpu_needs_another_gp(rsp, rdp)) {
 		/*
 		 * Either we have not yet spawned the grace-period
 		 * task, this CPU does not need another grace period,
 		 * or a grace period is already in progress.
 		 * Either way, don't start a new grace period.
 		 */
-		raw_spin_unlock_irqrestore(&rnp->lock, flags);
 		return;
 	}
-
-	/*
-	 * Because there is no grace period in progress right now,
-	 * any callbacks we have up to this point will be satisfied
-	 * by the next grace period.  So this is a good place to
-	 * assign a grace period number to recently posted callbacks.
-	 */
-	rcu_accelerate_cbs(rsp, rnp, rdp);
-
 	rsp->gp_flags = RCU_GP_FLAG_INIT;
-	raw_spin_unlock(&rnp->lock); /* Interrupts remain disabled. */
-
-	/* Ensure that CPU is aware of completion of last grace period. */
-	rcu_process_gp_end(rsp, rdp);
-	local_irq_restore(flags);
 
 	/* Wake up rcu_gp_kthread() to start the grace period. */
 	wake_up(&rsp->gp_wq);
 }
 
 /*
+ * Similar to rcu_start_gp_advanced(), but also advance the calling CPU's
+ * callbacks.  Note that rcu_start_gp_advanced() cannot do this because it
+ * is invoked indirectly from rcu_advance_cbs(), which would result in
+ * endless recursion -- or would do so if it wasn't for the self-deadlock
+ * that is encountered beforehand.
+ */
+static void
+rcu_start_gp(struct rcu_state *rsp)
+{
+	struct rcu_data *rdp = this_cpu_ptr(rsp->rda);
+	struct rcu_node *rnp = rcu_get_root(rsp);
+
+	/*
+	 * If there is no grace period in progress right now, any
+	 * callbacks we have up to this point will be satisfied by the
+	 * next grace period.  Also, advancing the callbacks reduces the
+	 * probability of false positives from cpu_needs_another_gp()
+	 * resulting in pointless grace periods.  So, advance callbacks
+	 * then start the grace period!
+	 */
+	rcu_advance_cbs(rsp, rnp, rdp);
+	rcu_start_gp_advanced(rsp, rnp, rdp);
+}
+
+/*
  * Report a full set of quiescent states to the specified rcu_state
  * data structure.  This involves cleaning up after the prior grace
  * period and letting rcu_start_gp() start up the next grace period
- * if one is needed.  Note that the caller must hold rnp->lock, as
- * required by rcu_start_gp(), which will release it.
+ * if one is needed.  Note that the caller must hold rnp->lock, which
+ * is released before return.
  */
 static void rcu_report_qs_rsp(struct rcu_state *rsp, unsigned long flags)
 	__releases(rcu_get_root(rsp)->lock)
@@ -2124,7 +2259,8 @@ __rcu_process_callbacks(struct rcu_state *rsp)
 	local_irq_save(flags);
 	if (cpu_needs_another_gp(rsp, rdp)) {
 		raw_spin_lock(&rcu_get_root(rsp)->lock); /* irqs disabled. */
-		rcu_start_gp(rsp, flags);  /* releases above lock */
+		rcu_start_gp(rsp);
+		raw_spin_unlock_irqrestore(&rcu_get_root(rsp)->lock, flags);
 	} else {
 		local_irq_restore(flags);
 	}
@@ -2169,7 +2305,8 @@ static void invoke_rcu_callbacks(struct rcu_state *rsp, struct rcu_data *rdp)
 
 static void invoke_rcu_core(void)
 {
-	raise_softirq(RCU_SOFTIRQ);
+	if (cpu_online(smp_processor_id()))
+		raise_softirq(RCU_SOFTIRQ);
 }
 
 /*
@@ -2204,11 +2341,11 @@ static void __call_rcu_core(struct rcu_state *rsp, struct rcu_data *rdp,
 
 		/* Start a new grace period if one not already started. */
 		if (!rcu_gp_in_progress(rsp)) {
-			unsigned long nestflag;
 			struct rcu_node *rnp_root = rcu_get_root(rsp);
 
-			raw_spin_lock_irqsave(&rnp_root->lock, nestflag);
-			rcu_start_gp(rsp, nestflag);  /* rlses rnp_root->lock */
+			raw_spin_lock(&rnp_root->lock);
+			rcu_start_gp(rsp);
+			raw_spin_unlock(&rnp_root->lock);
 		} else {
 			/* Give the grace period a kick. */
 			rdp->blimit = LONG_MAX;
@@ -2628,19 +2765,27 @@ static int rcu_pending(int cpu)
 }
 
 /*
- * Check to see if any future RCU-related work will need to be done
- * by the current CPU, even if none need be done immediately, returning
- * 1 if so.
+ * Return true if the specified CPU has any callback.  If all_lazy is
+ * non-NULL, store an indication of whether all callbacks are lazy.
+ * (If there are no callbacks, all of them are deemed to be lazy.)
  */
-static int rcu_cpu_has_callbacks(int cpu)
+static int rcu_cpu_has_callbacks(int cpu, bool *all_lazy)
 {
+	bool al = true;
+	bool hc = false;
+	struct rcu_data *rdp;
 	struct rcu_state *rsp;
 
-	/* RCU callbacks either ready or pending? */
-	for_each_rcu_flavor(rsp)
-		if (per_cpu_ptr(rsp->rda, cpu)->nxtlist)
-			return 1;
-	return 0;
+	for_each_rcu_flavor(rsp) {
+		rdp = per_cpu_ptr(rsp->rda, cpu);
+		if (rdp->qlen != rdp->qlen_lazy)
+			al = false;
+		if (rdp->nxtlist)
+			hc = true;
+	}
+	if (all_lazy)
+		*all_lazy = al;
+	return hc;
 }
 
 /*
@@ -2859,7 +3004,6 @@ rcu_init_percpu_data(int cpu, struct rcu_state *rsp, int preemptible)
 	rdp->dynticks->dynticks_nesting = DYNTICK_TASK_EXIT_IDLE;
 	atomic_set(&rdp->dynticks->dynticks,
 		   (atomic_read(&rdp->dynticks->dynticks) & ~0x1) + 1);
-	rcu_prepare_for_idle_init(cpu);
 	raw_spin_unlock(&rnp->lock);		/* irqs remain disabled. */
 
 	/* Add CPU to rcu_node bitmasks. */
@@ -2909,7 +3053,6 @@ static int __cpuinit rcu_cpu_notify(struct notifier_block *self,
 	struct rcu_data *rdp = per_cpu_ptr(rcu_state->rda, cpu);
 	struct rcu_node *rnp = rdp->mynode;
 	struct rcu_state *rsp;
-	int ret = NOTIFY_OK;
 
 	trace_rcu_utilization("Start CPU hotplug");
 	switch (action) {
@@ -2923,21 +3066,12 @@ static int __cpuinit rcu_cpu_notify(struct notifier_block *self,
 		rcu_boost_kthread_setaffinity(rnp, -1);
 		break;
 	case CPU_DOWN_PREPARE:
-		if (nocb_cpu_expendable(cpu))
-			rcu_boost_kthread_setaffinity(rnp, cpu);
-		else
-			ret = NOTIFY_BAD;
+		rcu_boost_kthread_setaffinity(rnp, cpu);
 		break;
 	case CPU_DYING:
 	case CPU_DYING_FROZEN:
-		/*
-		 * The whole machine is "stopped" except this CPU, so we can
-		 * touch any data without introducing corruption. We send the
-		 * dying CPU's callbacks to an arbitrarily chosen online CPU.
-		 */
 		for_each_rcu_flavor(rsp)
 			rcu_cleanup_dying_cpu(rsp);
-		rcu_cleanup_after_idle(cpu);
 		break;
 	case CPU_DEAD:
 	case CPU_DEAD_FROZEN:
@@ -2950,7 +3084,7 @@ static int __cpuinit rcu_cpu_notify(struct notifier_block *self,
 		break;
 	}
 	trace_rcu_utilization("End CPU hotplug");
-	return ret;
+	return NOTIFY_OK;
 }
 
 /*
@@ -3085,6 +3219,7 @@ static void __init rcu_init_one(struct rcu_state *rsp,
 			}
 			rnp->level = i;
 			INIT_LIST_HEAD(&rnp->blkd_tasks);
+			rcu_init_one_nocb(rnp);
 		}
 	}
 
@@ -3170,8 +3305,7 @@ void __init rcu_init(void)
 	rcu_init_one(&rcu_sched_state, &rcu_sched_data);
 	rcu_init_one(&rcu_bh_state, &rcu_bh_data);
 	__rcu_init_preempt();
-	rcu_init_nocb();
-	 open_softirq(RCU_SOFTIRQ, rcu_process_callbacks);
+	open_softirq(RCU_SOFTIRQ, rcu_process_callbacks);
 
 	/*
 	 * We don't need protection against CPU-hotplug here because
diff --git a/kernel/rcutree.h b/kernel/rcutree.h
index c896b5045d9..14ee40795d6 100644
--- a/kernel/rcutree.h
+++ b/kernel/rcutree.h
@@ -88,18 +88,13 @@ struct rcu_dynticks {
 	int dynticks_nmi_nesting;   /* Track NMI nesting level. */
 	atomic_t dynticks;	    /* Even value for idle, else odd. */
 #ifdef CONFIG_RCU_FAST_NO_HZ
-	int dyntick_drain;	    /* Prepare-for-idle state variable. */
-	unsigned long dyntick_holdoff;
-				    /* No retries for the jiffy of failure. */
-	struct timer_list idle_gp_timer;
-				    /* Wake up CPU sleeping with callbacks. */
-	unsigned long idle_gp_timer_expires;
-				    /* When to wake up CPU (for repost). */
-	bool idle_first_pass;	    /* First pass of attempt to go idle? */
+	bool all_lazy;		    /* Are all CPU's CBs lazy? */
 	unsigned long nonlazy_posted;
 				    /* # times non-lazy CBs posted to CPU. */
 	unsigned long nonlazy_posted_snap;
 				    /* idle-period nonlazy_posted snapshot. */
+	unsigned long last_accelerate;
+				    /* Last jiffy CBs were accelerated. */
 	int tick_nohz_enabled_snap; /* Previously seen value from sysfs. */
 #endif /* #ifdef CONFIG_RCU_FAST_NO_HZ */
 };
@@ -134,9 +129,6 @@ struct rcu_node {
 				/*  elements that need to drain to allow the */
 				/*  current expedited grace period to */
 				/*  complete (only for TREE_PREEMPT_RCU). */
-	atomic_t wakemask;	/* CPUs whose kthread needs to be awakened. */
-				/*  Since this has meaning only for leaf */
-				/*  rcu_node structures, 32 bits suffices. */
 	unsigned long qsmaskinit;
 				/* Per-GP initial value for qsmask & expmask. */
 	unsigned long grpmask;	/* Mask to apply to parent qsmask. */
@@ -196,6 +188,12 @@ struct rcu_node {
 				/* Refused to boost: not sure why, though. */
 				/*  This can happen due to race conditions. */
 #endif /* #ifdef CONFIG_RCU_BOOST */
+#ifdef CONFIG_RCU_NOCB_CPU
+	wait_queue_head_t nocb_gp_wq[2];
+				/* Place for rcu_nocb_kthread() to wait GP. */
+#endif /* #ifdef CONFIG_RCU_NOCB_CPU */
+	int need_future_gp[2];
+				/* Counts of upcoming no-CB GP requests. */
 	raw_spinlock_t fqslock ____cacheline_internodealigned_in_smp;
 } ____cacheline_internodealigned_in_smp;
 
@@ -328,6 +326,11 @@ struct rcu_data {
 	struct task_struct *nocb_kthread;
 #endif /* #ifdef CONFIG_RCU_NOCB_CPU */
 
+	/* 8) RCU CPU stall data. */
+#ifdef CONFIG_RCU_CPU_STALL_INFO
+	unsigned int softirq_snap;	/* Snapshot of softirq activity. */
+#endif /* #ifdef CONFIG_RCU_CPU_STALL_INFO */
+
 	int cpu;
 	struct rcu_state *rsp;
 };
@@ -375,12 +378,6 @@ struct rcu_state {
 	struct rcu_data __percpu *rda;		/* pointer of percu rcu_data. */
 	void (*call)(struct rcu_head *head,	/* call_rcu() flavor. */
 		     void (*func)(struct rcu_head *head));
-#ifdef CONFIG_RCU_NOCB_CPU
-	void (*call_remote)(struct rcu_head *head,
-		     void (*func)(struct rcu_head *head));
-						/* call_rcu() flavor, but for */
-						/*  placing on remote CPU. */
-#endif /* #ifdef CONFIG_RCU_NOCB_CPU */
 
 	/* The following fields are guarded by the root rcu_node's lock. */
 
@@ -443,6 +440,7 @@ struct rcu_state {
 	unsigned long gp_max;			/* Maximum GP duration in */
 						/*  jiffies. */
 	char *name;				/* Name of structure. */
+	char abbr;				/* Abbreviated name. */
 	struct list_head flavors;		/* List of RCU flavors. */
 };
 
@@ -520,7 +518,6 @@ static int __cpuinit rcu_spawn_one_boost_kthread(struct rcu_state *rsp,
 						 struct rcu_node *rnp);
 #endif /* #ifdef CONFIG_RCU_BOOST */
 static void __cpuinit rcu_prepare_kthreads(int cpu);
-static void rcu_prepare_for_idle_init(int cpu);
 static void rcu_cleanup_after_idle(int cpu);
 static void rcu_prepare_for_idle(int cpu);
 static void rcu_idle_count_callbacks_posted(void);
@@ -529,16 +526,18 @@ static void print_cpu_stall_info(struct rcu_state *rsp, int cpu);
 static void print_cpu_stall_info_end(void);
 static void zero_cpu_stall_ticks(struct rcu_data *rdp);
 static void increment_cpu_stall_ticks(void);
+static int rcu_nocb_needs_gp(struct rcu_state *rsp);
+static void rcu_nocb_gp_set(struct rcu_node *rnp, int nrq);
+static void rcu_nocb_gp_cleanup(struct rcu_state *rsp, struct rcu_node *rnp);
+static void rcu_init_one_nocb(struct rcu_node *rnp);
 static bool is_nocb_cpu(int cpu);
 static bool __call_rcu_nocb(struct rcu_data *rdp, struct rcu_head *rhp,
 			    bool lazy);
 static bool rcu_nocb_adopt_orphan_cbs(struct rcu_state *rsp,
 				      struct rcu_data *rdp);
-static bool nocb_cpu_expendable(int cpu);
 static void rcu_boot_init_nocb_percpu_data(struct rcu_data *rdp);
 static void rcu_spawn_nocb_kthreads(struct rcu_state *rsp);
-static void init_nocb_callback_list(struct rcu_data *rdp);
-static void __init rcu_init_nocb(void);
+static bool init_nocb_callback_list(struct rcu_data *rdp);
 
 #endif /* #ifndef RCU_TREE_NONCORE */
 
diff --git a/kernel/rcutree_plugin.h b/kernel/rcutree_plugin.h
index c1cc7e17ff9..d084ae3f281 100644
--- a/kernel/rcutree_plugin.h
+++ b/kernel/rcutree_plugin.h
@@ -85,11 +85,21 @@ static void __init rcu_bootup_announce_oddness(void)
 	if (nr_cpu_ids != NR_CPUS)
 		printk(KERN_INFO "\tRCU restricting CPUs from NR_CPUS=%d to nr_cpu_ids=%d.\n", NR_CPUS, nr_cpu_ids);
 #ifdef CONFIG_RCU_NOCB_CPU
+#ifndef CONFIG_RCU_NOCB_CPU_NONE
+	if (!have_rcu_nocb_mask) {
+		alloc_bootmem_cpumask_var(&rcu_nocb_mask);
+		have_rcu_nocb_mask = true;
+	}
+#ifdef CONFIG_RCU_NOCB_CPU_ZERO
+	pr_info("\tExperimental no-CBs CPU 0\n");
+	cpumask_set_cpu(0, rcu_nocb_mask);
+#endif /* #ifdef CONFIG_RCU_NOCB_CPU_ZERO */
+#ifdef CONFIG_RCU_NOCB_CPU_ALL
+	pr_info("\tExperimental no-CBs for all CPUs\n");
+	cpumask_setall(rcu_nocb_mask);
+#endif /* #ifdef CONFIG_RCU_NOCB_CPU_ALL */
+#endif /* #ifndef CONFIG_RCU_NOCB_CPU_NONE */
 	if (have_rcu_nocb_mask) {
-		if (cpumask_test_cpu(0, rcu_nocb_mask)) {
-			cpumask_clear_cpu(0, rcu_nocb_mask);
-			pr_info("\tCPU 0: illegal no-CBs CPU (cleared).\n");
-		}
 		cpulist_scnprintf(nocb_buf, sizeof(nocb_buf), rcu_nocb_mask);
 		pr_info("\tExperimental no-CBs CPUs: %s.\n", nocb_buf);
 		if (rcu_nocb_poll)
@@ -101,7 +111,7 @@ static void __init rcu_bootup_announce_oddness(void)
 #ifdef CONFIG_TREE_PREEMPT_RCU
 
 struct rcu_state rcu_preempt_state =
-	RCU_STATE_INITIALIZER(rcu_preempt, call_rcu);
+	RCU_STATE_INITIALIZER(rcu_preempt, 'p', call_rcu);
 DEFINE_PER_CPU(struct rcu_data, rcu_preempt_data);
 static struct rcu_state *rcu_state = &rcu_preempt_state;
 
@@ -1533,14 +1543,7 @@ static void __cpuinit rcu_prepare_kthreads(int cpu)
 int rcu_needs_cpu(int cpu, unsigned long *delta_jiffies)
 {
 	*delta_jiffies = ULONG_MAX;
-	return rcu_cpu_has_callbacks(cpu);
-}
-
-/*
- * Because we do not have RCU_FAST_NO_HZ, don't bother initializing for it.
- */
-static void rcu_prepare_for_idle_init(int cpu)
-{
+	return rcu_cpu_has_callbacks(cpu, NULL);
 }
 
 /*
@@ -1577,16 +1580,6 @@ static void rcu_idle_count_callbacks_posted(void)
  *
  * The following three proprocessor symbols control this state machine:
  *
- * RCU_IDLE_FLUSHES gives the maximum number of times that we will attempt
- *	to satisfy RCU.  Beyond this point, it is better to incur a periodic
- *	scheduling-clock interrupt than to loop through the state machine
- *	at full power.
- * RCU_IDLE_OPT_FLUSHES gives the number of RCU_IDLE_FLUSHES that are
- *	optional if RCU does not need anything immediately from this
- *	CPU, even if this CPU still has RCU callbacks queued.  The first
- *	times through the state machine are mandatory: we need to give
- *	the state machine a chance to communicate a quiescent state
- *	to the RCU core.
  * RCU_IDLE_GP_DELAY gives the number of jiffies that a CPU is permitted
  *	to sleep in dyntick-idle mode with RCU callbacks pending.  This
  *	is sized to be roughly one RCU grace period.  Those energy-efficiency
@@ -1602,186 +1595,108 @@ static void rcu_idle_count_callbacks_posted(void)
  * adjustment, they can be converted into kernel config parameters, though
  * making the state machine smarter might be a better option.
  */
-#define RCU_IDLE_FLUSHES 5		/* Number of dyntick-idle tries. */
-#define RCU_IDLE_OPT_FLUSHES 3		/* Optional dyntick-idle tries. */
 #define RCU_IDLE_GP_DELAY 4		/* Roughly one grace period. */
 #define RCU_IDLE_LAZY_GP_DELAY (6 * HZ)	/* Roughly six seconds. */
 
-extern int tick_nohz_enabled;
-
-/*
- * Does the specified flavor of RCU have non-lazy callbacks pending on
- * the specified CPU?  Both RCU flavor and CPU are specified by the
- * rcu_data structure.
- */
-static bool __rcu_cpu_has_nonlazy_callbacks(struct rcu_data *rdp)
-{
-	return rdp->qlen != rdp->qlen_lazy;
-}
+static int rcu_idle_gp_delay = RCU_IDLE_GP_DELAY;
+module_param(rcu_idle_gp_delay, int, 0644);
+static int rcu_idle_lazy_gp_delay = RCU_IDLE_LAZY_GP_DELAY;
+module_param(rcu_idle_lazy_gp_delay, int, 0644);
 
-#ifdef CONFIG_TREE_PREEMPT_RCU
+extern int tick_nohz_enabled;
 
 /*
- * Are there non-lazy RCU-preempt callbacks?  (There cannot be if there
- * is no RCU-preempt in the kernel.)
+ * Try to advance callbacks for all flavors of RCU on the current CPU.
+ * Afterwards, if there are any callbacks ready for immediate invocation,
+ * return true.
  */
-static bool rcu_preempt_cpu_has_nonlazy_callbacks(int cpu)
+static bool rcu_try_advance_all_cbs(void)
 {
-	struct rcu_data *rdp = &per_cpu(rcu_preempt_data, cpu);
-
-	return __rcu_cpu_has_nonlazy_callbacks(rdp);
-}
-
-#else /* #ifdef CONFIG_TREE_PREEMPT_RCU */
+	bool cbs_ready = false;
+	struct rcu_data *rdp;
+	struct rcu_node *rnp;
+	struct rcu_state *rsp;
 
-static bool rcu_preempt_cpu_has_nonlazy_callbacks(int cpu)
-{
-	return 0;
-}
+	for_each_rcu_flavor(rsp) {
+		rdp = this_cpu_ptr(rsp->rda);
+		rnp = rdp->mynode;
 
-#endif /* else #ifdef CONFIG_TREE_PREEMPT_RCU */
+		/*
+		 * Don't bother checking unless a grace period has
+		 * completed since we last checked and there are
+		 * callbacks not yet ready to invoke.
+		 */
+		if (rdp->completed != rnp->completed &&
+		    rdp->nxttail[RCU_DONE_TAIL] != rdp->nxttail[RCU_NEXT_TAIL])
+			rcu_process_gp_end(rsp, rdp);
 
-/*
- * Does any flavor of RCU have non-lazy callbacks on the specified CPU?
- */
-static bool rcu_cpu_has_nonlazy_callbacks(int cpu)
-{
-	return __rcu_cpu_has_nonlazy_callbacks(&per_cpu(rcu_sched_data, cpu)) ||
-	       __rcu_cpu_has_nonlazy_callbacks(&per_cpu(rcu_bh_data, cpu)) ||
-	       rcu_preempt_cpu_has_nonlazy_callbacks(cpu);
+		if (cpu_has_callbacks_ready_to_invoke(rdp))
+			cbs_ready = true;
+	}
+	return cbs_ready;
 }
 
 /*
- * Allow the CPU to enter dyntick-idle mode if either: (1) There are no
- * callbacks on this CPU, (2) this CPU has not yet attempted to enter
- * dyntick-idle mode, or (3) this CPU is in the process of attempting to
- * enter dyntick-idle mode.  Otherwise, if we have recently tried and failed
- * to enter dyntick-idle mode, we refuse to try to enter it.  After all,
- * it is better to incur scheduling-clock interrupts than to spin
- * continuously for the same time duration!
+ * Allow the CPU to enter dyntick-idle mode unless it has callbacks ready
+ * to invoke.  If the CPU has callbacks, try to advance them.  Tell the
+ * caller to set the timeout based on whether or not there are non-lazy
+ * callbacks.
  *
- * The delta_jiffies argument is used to store the time when RCU is
- * going to need the CPU again if it still has callbacks.  The reason
- * for this is that rcu_prepare_for_idle() might need to post a timer,
- * but if so, it will do so after tick_nohz_stop_sched_tick() has set
- * the wakeup time for this CPU.  This means that RCU's timer can be
- * delayed until the wakeup time, which defeats the purpose of posting
- * a timer.
+ * The caller must have disabled interrupts.
  */
-int rcu_needs_cpu(int cpu, unsigned long *delta_jiffies)
+int rcu_needs_cpu(int cpu, unsigned long *dj)
 {
 	struct rcu_dynticks *rdtp = &per_cpu(rcu_dynticks, cpu);
 
-	/* Flag a new idle sojourn to the idle-entry state machine. */
-	rdtp->idle_first_pass = 1;
+	/* Snapshot to detect later posting of non-lazy callback. */
+	rdtp->nonlazy_posted_snap = rdtp->nonlazy_posted;
+
 	/* If no callbacks, RCU doesn't need the CPU. */
-	if (!rcu_cpu_has_callbacks(cpu)) {
-		*delta_jiffies = ULONG_MAX;
+	if (!rcu_cpu_has_callbacks(cpu, &rdtp->all_lazy)) {
+		*dj = ULONG_MAX;
 		return 0;
 	}
-	if (rdtp->dyntick_holdoff == jiffies) {
-		/* RCU recently tried and failed, so don't try again. */
-		*delta_jiffies = 1;
+
+	/* Attempt to advance callbacks. */
+	if (rcu_try_advance_all_cbs()) {
+		/* Some ready to invoke, so initiate later invocation. */
+		invoke_rcu_core();
 		return 1;
 	}
-	/* Set up for the possibility that RCU will post a timer. */
-	if (rcu_cpu_has_nonlazy_callbacks(cpu)) {
-		*delta_jiffies = round_up(RCU_IDLE_GP_DELAY + jiffies,
-					  RCU_IDLE_GP_DELAY) - jiffies;
+	rdtp->last_accelerate = jiffies;
+
+	/* Request timer delay depending on laziness, and round. */
+	if (rdtp->all_lazy) {
+		*dj = round_up(rcu_idle_gp_delay + jiffies,
+			       rcu_idle_gp_delay) - jiffies;
 	} else {
-		*delta_jiffies = jiffies + RCU_IDLE_LAZY_GP_DELAY;
-		*delta_jiffies = round_jiffies(*delta_jiffies) - jiffies;
+		*dj = round_jiffies(rcu_idle_lazy_gp_delay + jiffies) - jiffies;
 	}
 	return 0;
 }
 
 /*
- * Handler for smp_call_function_single().  The only point of this
- * handler is to wake the CPU up, so the handler does only tracing.
- */
-void rcu_idle_demigrate(void *unused)
-{
-	trace_rcu_prep_idle("Demigrate");
-}
-
-/*
- * Timer handler used to force CPU to start pushing its remaining RCU
- * callbacks in the case where it entered dyntick-idle mode with callbacks
- * pending.  The hander doesn't really need to do anything because the
- * real work is done upon re-entry to idle, or by the next scheduling-clock
- * interrupt should idle not be re-entered.
- *
- * One special case: the timer gets migrated without awakening the CPU
- * on which the timer was scheduled on.  In this case, we must wake up
- * that CPU.  We do so with smp_call_function_single().
- */
-static void rcu_idle_gp_timer_func(unsigned long cpu_in)
-{
-	int cpu = (int)cpu_in;
-
-	trace_rcu_prep_idle("Timer");
-	if (cpu != smp_processor_id())
-		smp_call_function_single(cpu, rcu_idle_demigrate, NULL, 0);
-	else
-		WARN_ON_ONCE(1); /* Getting here can hang the system... */
-}
-
-/*
- * Initialize the timer used to pull CPUs out of dyntick-idle mode.
- */
-static void rcu_prepare_for_idle_init(int cpu)
-{
-	struct rcu_dynticks *rdtp = &per_cpu(rcu_dynticks, cpu);
-
-	rdtp->dyntick_holdoff = jiffies - 1;
-	setup_timer(&rdtp->idle_gp_timer, rcu_idle_gp_timer_func, cpu);
-	rdtp->idle_gp_timer_expires = jiffies - 1;
-	rdtp->idle_first_pass = 1;
-}
-
-/*
- * Clean up for exit from idle.  Because we are exiting from idle, there
- * is no longer any point to ->idle_gp_timer, so cancel it.  This will
- * do nothing if this timer is not active, so just cancel it unconditionally.
- */
-static void rcu_cleanup_after_idle(int cpu)
-{
-	struct rcu_dynticks *rdtp = &per_cpu(rcu_dynticks, cpu);
-
-	del_timer(&rdtp->idle_gp_timer);
-	trace_rcu_prep_idle("Cleanup after idle");
-	rdtp->tick_nohz_enabled_snap = ACCESS_ONCE(tick_nohz_enabled);
-}
-
-/*
- * Check to see if any RCU-related work can be done by the current CPU,
- * and if so, schedule a softirq to get it done.  This function is part
- * of the RCU implementation; it is -not- an exported member of the RCU API.
- *
- * The idea is for the current CPU to clear out all work required by the
- * RCU core for the current grace period, so that this CPU can be permitted
- * to enter dyntick-idle mode.  In some cases, it will need to be awakened
- * at the end of the grace period by whatever CPU ends the grace period.
- * This allows CPUs to go dyntick-idle more quickly, and to reduce the
- * number of wakeups by a modest integer factor.
- *
- * Because it is not legal to invoke rcu_process_callbacks() with irqs
- * disabled, we do one pass of force_quiescent_state(), then do a
- * invoke_rcu_core() to cause rcu_process_callbacks() to be invoked
- * later.  The ->dyntick_drain field controls the sequencing.
+ * Prepare a CPU for idle from an RCU perspective.  The first major task
+ * is to sense whether nohz mode has been enabled or disabled via sysfs.
+ * The second major task is to check to see if a non-lazy callback has
+ * arrived at a CPU that previously had only lazy callbacks.  The third
+ * major task is to accelerate (that is, assign grace-period numbers to)
+ * any recently arrived callbacks.
  *
  * The caller must have disabled interrupts.
  */
 static void rcu_prepare_for_idle(int cpu)
 {
-	struct timer_list *tp;
+	struct rcu_data *rdp;
 	struct rcu_dynticks *rdtp = &per_cpu(rcu_dynticks, cpu);
+	struct rcu_node *rnp;
+	struct rcu_state *rsp;
 	int tne;
 
 	/* Handle nohz enablement switches conservatively. */
 	tne = ACCESS_ONCE(tick_nohz_enabled);
 	if (tne != rdtp->tick_nohz_enabled_snap) {
-		if (rcu_cpu_has_callbacks(cpu))
+		if (rcu_cpu_has_callbacks(cpu, NULL))
 			invoke_rcu_core(); /* force nohz to see update. */
 		rdtp->tick_nohz_enabled_snap = tne;
 		return;
@@ -1789,125 +1704,56 @@ static void rcu_prepare_for_idle(int cpu)
 	if (!tne)
 		return;
 
-	/* Adaptive-tick mode, where usermode execution is idle to RCU. */
-	if (!is_idle_task(current)) {
-		rdtp->dyntick_holdoff = jiffies - 1;
-		if (rcu_cpu_has_nonlazy_callbacks(cpu)) {
-			trace_rcu_prep_idle("User dyntick with callbacks");
-			rdtp->idle_gp_timer_expires =
-				round_up(jiffies + RCU_IDLE_GP_DELAY,
-					 RCU_IDLE_GP_DELAY);
-		} else if (rcu_cpu_has_callbacks(cpu)) {
-			rdtp->idle_gp_timer_expires =
-				round_jiffies(jiffies + RCU_IDLE_LAZY_GP_DELAY);
-			trace_rcu_prep_idle("User dyntick with lazy callbacks");
-		} else {
-			return;
-		}
-		tp = &rdtp->idle_gp_timer;
-		mod_timer_pinned(tp, rdtp->idle_gp_timer_expires);
+	/* If this is a no-CBs CPU, no callbacks, just return. */
+	if (is_nocb_cpu(cpu))
 		return;
-	}
 
 	/*
-	 * If this is an idle re-entry, for example, due to use of
-	 * RCU_NONIDLE() or the new idle-loop tracing API within the idle
-	 * loop, then don't take any state-machine actions, unless the
-	 * momentary exit from idle queued additional non-lazy callbacks.
-	 * Instead, repost the ->idle_gp_timer if this CPU has callbacks
-	 * pending.
+	 * If a non-lazy callback arrived at a CPU having only lazy
+	 * callbacks, invoke RCU core for the side-effect of recalculating
+	 * idle duration on re-entry to idle.
 	 */
-	if (!rdtp->idle_first_pass &&
-	    (rdtp->nonlazy_posted == rdtp->nonlazy_posted_snap)) {
-		if (rcu_cpu_has_callbacks(cpu)) {
-			tp = &rdtp->idle_gp_timer;
-			mod_timer_pinned(tp, rdtp->idle_gp_timer_expires);
-		}
+	if (rdtp->all_lazy &&
+	    rdtp->nonlazy_posted != rdtp->nonlazy_posted_snap) {
+		invoke_rcu_core();
 		return;
 	}
-	rdtp->idle_first_pass = 0;
-	rdtp->nonlazy_posted_snap = rdtp->nonlazy_posted - 1;
 
 	/*
-	 * If there are no callbacks on this CPU, enter dyntick-idle mode.
-	 * Also reset state to avoid prejudicing later attempts.
+	 * If we have not yet accelerated this jiffy, accelerate all
+	 * callbacks on this CPU.
 	 */
-	if (!rcu_cpu_has_callbacks(cpu)) {
-		rdtp->dyntick_holdoff = jiffies - 1;
-		rdtp->dyntick_drain = 0;
-		trace_rcu_prep_idle("No callbacks");
+	if (rdtp->last_accelerate == jiffies)
 		return;
+	rdtp->last_accelerate = jiffies;
+	for_each_rcu_flavor(rsp) {
+		rdp = per_cpu_ptr(rsp->rda, cpu);
+		if (!*rdp->nxttail[RCU_DONE_TAIL])
+			continue;
+		rnp = rdp->mynode;
+		raw_spin_lock(&rnp->lock); /* irqs already disabled. */
+		rcu_accelerate_cbs(rsp, rnp, rdp);
+		raw_spin_unlock(&rnp->lock); /* irqs remain disabled. */
 	}
+}
 
-	/*
-	 * If in holdoff mode, just return.  We will presumably have
-	 * refrained from disabling the scheduling-clock tick.
-	 */
-	if (rdtp->dyntick_holdoff == jiffies) {
-		trace_rcu_prep_idle("In holdoff");
-		return;
-	}
+/*
+ * Clean up for exit from idle.  Attempt to advance callbacks based on
+ * any grace periods that elapsed while the CPU was idle, and if any
+ * callbacks are now ready to invoke, initiate invocation.
+ */
+static void rcu_cleanup_after_idle(int cpu)
+{
+	struct rcu_data *rdp;
+	struct rcu_state *rsp;
 
-	/* Check and update the ->dyntick_drain sequencing. */
-	if (rdtp->dyntick_drain <= 0) {
-		/* First time through, initialize the counter. */
-		rdtp->dyntick_drain = RCU_IDLE_FLUSHES;
-	} else if (rdtp->dyntick_drain <= RCU_IDLE_OPT_FLUSHES &&
-		   !rcu_pending(cpu) &&
-		   !local_softirq_pending()) {
-		/* Can we go dyntick-idle despite still having callbacks? */
-		rdtp->dyntick_drain = 0;
-		rdtp->dyntick_holdoff = jiffies;
-		if (rcu_cpu_has_nonlazy_callbacks(cpu)) {
-			trace_rcu_prep_idle("Dyntick with callbacks");
-			rdtp->idle_gp_timer_expires =
-				round_up(jiffies + RCU_IDLE_GP_DELAY,
-					 RCU_IDLE_GP_DELAY);
-		} else {
-			rdtp->idle_gp_timer_expires =
-				round_jiffies(jiffies + RCU_IDLE_LAZY_GP_DELAY);
-			trace_rcu_prep_idle("Dyntick with lazy callbacks");
-		}
-		tp = &rdtp->idle_gp_timer;
-		mod_timer_pinned(tp, rdtp->idle_gp_timer_expires);
-		rdtp->nonlazy_posted_snap = rdtp->nonlazy_posted;
-		return; /* Nothing more to do immediately. */
-	} else if (--(rdtp->dyntick_drain) <= 0) {
-		/* We have hit the limit, so time to give up. */
-		rdtp->dyntick_holdoff = jiffies;
-		trace_rcu_prep_idle("Begin holdoff");
-		invoke_rcu_core();  /* Force the CPU out of dyntick-idle. */
+	if (is_nocb_cpu(cpu))
 		return;
-	}
-
-	/*
-	 * Do one step of pushing the remaining RCU callbacks through
-	 * the RCU core state machine.
-	 */
-#ifdef CONFIG_TREE_PREEMPT_RCU
-	if (per_cpu(rcu_preempt_data, cpu).nxtlist) {
-		rcu_preempt_qs(cpu);
-		force_quiescent_state(&rcu_preempt_state);
-	}
-#endif /* #ifdef CONFIG_TREE_PREEMPT_RCU */
-	if (per_cpu(rcu_sched_data, cpu).nxtlist) {
-		rcu_sched_qs(cpu);
-		force_quiescent_state(&rcu_sched_state);
-	}
-	if (per_cpu(rcu_bh_data, cpu).nxtlist) {
-		rcu_bh_qs(cpu);
-		force_quiescent_state(&rcu_bh_state);
-	}
-
-	/*
-	 * If RCU callbacks are still pending, RCU still needs this CPU.
-	 * So try forcing the callbacks through the grace period.
-	 */
-	if (rcu_cpu_has_callbacks(cpu)) {
-		trace_rcu_prep_idle("More callbacks");
-		invoke_rcu_core();
-	} else {
-		trace_rcu_prep_idle("Callbacks drained");
+	rcu_try_advance_all_cbs();
+	for_each_rcu_flavor(rsp) {
+		rdp = per_cpu_ptr(rsp->rda, cpu);
+		if (cpu_has_callbacks_ready_to_invoke(rdp))
+			invoke_rcu_core();
 	}
 }
 
@@ -2015,16 +1861,13 @@ early_initcall(rcu_register_oom_notifier);
 static void print_cpu_stall_fast_no_hz(char *cp, int cpu)
 {
 	struct rcu_dynticks *rdtp = &per_cpu(rcu_dynticks, cpu);
-	struct timer_list *tltp = &rdtp->idle_gp_timer;
-	char c;
+	unsigned long nlpd = rdtp->nonlazy_posted - rdtp->nonlazy_posted_snap;
 
-	c = rdtp->dyntick_holdoff == jiffies ? 'H' : '.';
-	if (timer_pending(tltp))
-		sprintf(cp, "drain=%d %c timer=%lu",
-			rdtp->dyntick_drain, c, tltp->expires - jiffies);
-	else
-		sprintf(cp, "drain=%d %c timer not pending",
-			rdtp->dyntick_drain, c);
+	sprintf(cp, "last_accelerate: %04lx/%04lx, nonlazy_posted: %ld, %c%c",
+		rdtp->last_accelerate & 0xffff, jiffies & 0xffff,
+		ulong2long(nlpd),
+		rdtp->all_lazy ? 'L' : '.',
+		rdtp->tick_nohz_enabled_snap ? '.' : 'D');
 }
 
 #else /* #ifdef CONFIG_RCU_FAST_NO_HZ */
@@ -2070,10 +1913,11 @@ static void print_cpu_stall_info(struct rcu_state *rsp, int cpu)
 		ticks_value = rsp->gpnum - rdp->gpnum;
 	}
 	print_cpu_stall_fast_no_hz(fast_no_hz, cpu);
-	printk(KERN_ERR "\t%d: (%lu %s) idle=%03x/%llx/%d %s\n",
+	printk(KERN_ERR "\t%d: (%lu %s) idle=%03x/%llx/%d softirq=%u/%u %s\n",
 	       cpu, ticks_value, ticks_title,
 	       atomic_read(&rdtp->dynticks) & 0xfff,
 	       rdtp->dynticks_nesting, rdtp->dynticks_nmi_nesting,
+	       rdp->softirq_snap, kstat_softirqs_cpu(RCU_SOFTIRQ, cpu),
 	       fast_no_hz);
 }
 
@@ -2087,6 +1931,7 @@ static void print_cpu_stall_info_end(void)
 static void zero_cpu_stall_ticks(struct rcu_data *rdp)
 {
 	rdp->ticks_this_gp = 0;
+	rdp->softirq_snap = kstat_softirqs_cpu(RCU_SOFTIRQ, smp_processor_id());
 }
 
 /* Increment ->ticks_this_gp for all flavors of RCU. */
@@ -2165,6 +2010,47 @@ static int __init parse_rcu_nocb_poll(char *arg)
 }
 early_param("rcu_nocb_poll", parse_rcu_nocb_poll);
 
+/*
+ * Do any no-CBs CPUs need another grace period?
+ *
+ * Interrupts must be disabled.  If the caller does not hold the root
+ * rnp_node structure's ->lock, the results are advisory only.
+ */
+static int rcu_nocb_needs_gp(struct rcu_state *rsp)
+{
+	struct rcu_node *rnp = rcu_get_root(rsp);
+
+	return rnp->need_future_gp[(ACCESS_ONCE(rnp->completed) + 1) & 0x1];
+}
+
+/*
+ * Wake up any no-CBs CPUs' kthreads that were waiting on the just-ended
+ * grace period.
+ */
+static void rcu_nocb_gp_cleanup(struct rcu_state *rsp, struct rcu_node *rnp)
+{
+	wake_up_all(&rnp->nocb_gp_wq[rnp->completed & 0x1]);
+}
+
+/*
+ * Set the root rcu_node structure's ->need_future_gp field
+ * based on the sum of those of all rcu_node structures.  This does
+ * double-count the root rcu_node structure's requests, but this
+ * is necessary to handle the possibility of a rcu_nocb_kthread()
+ * having awakened during the time that the rcu_node structures
+ * were being updated for the end of the previous grace period.
+ */
+static void rcu_nocb_gp_set(struct rcu_node *rnp, int nrq)
+{
+	rnp->need_future_gp[(rnp->completed + 1) & 0x1] += nrq;
+}
+
+static void rcu_init_one_nocb(struct rcu_node *rnp)
+{
+	init_waitqueue_head(&rnp->nocb_gp_wq[0]);
+	init_waitqueue_head(&rnp->nocb_gp_wq[1]);
+}
+
 /* Is the specified CPU a no-CPUs CPU? */
 static bool is_nocb_cpu(int cpu)
 {
@@ -2227,6 +2113,13 @@ static bool __call_rcu_nocb(struct rcu_data *rdp, struct rcu_head *rhp,
 	if (!is_nocb_cpu(rdp->cpu))
 		return 0;
 	__call_rcu_nocb_enqueue(rdp, rhp, &rhp->next, 1, lazy);
+	if (__is_kfree_rcu_offset((unsigned long)rhp->func))
+		trace_rcu_kfree_callback(rdp->rsp->name, rhp,
+					 (unsigned long)rhp->func,
+					 rdp->qlen_lazy, rdp->qlen);
+	else
+		trace_rcu_callback(rdp->rsp->name, rhp,
+				   rdp->qlen_lazy, rdp->qlen);
 	return 1;
 }
 
@@ -2265,95 +2158,36 @@ static bool __maybe_unused rcu_nocb_adopt_orphan_cbs(struct rcu_state *rsp,
 }
 
 /*
- * There must be at least one non-no-CBs CPU in operation at any given
- * time, because no-CBs CPUs are not capable of initiating grace periods
- * independently.  This function therefore complains if the specified
- * CPU is the last non-no-CBs CPU, allowing the CPU-hotplug system to
- * avoid offlining the last such CPU.  (Recursion is a wonderful thing,
- * but you have to have a base case!)
+ * If necessary, kick off a new grace period, and either way wait
+ * for a subsequent grace period to complete.
  */
-static bool nocb_cpu_expendable(int cpu)
+static void rcu_nocb_wait_gp(struct rcu_data *rdp)
 {
-	cpumask_var_t non_nocb_cpus;
-	int ret;
+	unsigned long c;
+	bool d;
+	unsigned long flags;
+	struct rcu_node *rnp = rdp->mynode;
+
+	raw_spin_lock_irqsave(&rnp->lock, flags);
+	c = rcu_start_future_gp(rnp, rdp);
+	raw_spin_unlock_irqrestore(&rnp->lock, flags);
 
 	/*
-	 * If there are no no-CB CPUs or if this CPU is not a no-CB CPU,
-	 * then offlining this CPU is harmless.  Let it happen.
+	 * Wait for the grace period.  Do so interruptibly to avoid messing
+	 * up the load average.
 	 */
-	if (!have_rcu_nocb_mask || is_nocb_cpu(cpu))
-		return 1;
-
-	/* If no memory, play it safe and keep the CPU around. */
-	if (!alloc_cpumask_var(&non_nocb_cpus, GFP_NOIO))
-		return 0;
-	cpumask_andnot(non_nocb_cpus, cpu_online_mask, rcu_nocb_mask);
-	cpumask_clear_cpu(cpu, non_nocb_cpus);
-	ret = !cpumask_empty(non_nocb_cpus);
-	free_cpumask_var(non_nocb_cpus);
-	return ret;
-}
-
-/*
- * Helper structure for remote registry of RCU callbacks.
- * This is needed for when a no-CBs CPU needs to start a grace period.
- * If it just invokes call_rcu(), the resulting callback will be queued,
- * which can result in deadlock.
- */
-struct rcu_head_remote {
-	struct rcu_head *rhp;
-	call_rcu_func_t *crf;
-	void (*func)(struct rcu_head *rhp);
-};
-
-/*
- * Register a callback as specified by the rcu_head_remote struct.
- * This function is intended to be invoked via smp_call_function_single().
- */
-static void call_rcu_local(void *arg)
-{
-	struct rcu_head_remote *rhrp =
-		container_of(arg, struct rcu_head_remote, rhp);
-
-	rhrp->crf(rhrp->rhp, rhrp->func);
-}
-
-/*
- * Set up an rcu_head_remote structure and the invoke call_rcu_local()
- * on CPU 0 (which is guaranteed to be a non-no-CBs CPU) via
- * smp_call_function_single().
- */
-static void invoke_crf_remote(struct rcu_head *rhp,
-			      void (*func)(struct rcu_head *rhp),
-			      call_rcu_func_t crf)
-{
-	struct rcu_head_remote rhr;
-
-	rhr.rhp = rhp;
-	rhr.crf = crf;
-	rhr.func = func;
-	smp_call_function_single(0, call_rcu_local, &rhr, 1);
-}
-
-/*
- * Helper functions to be passed to wait_rcu_gp(), each of which
- * invokes invoke_crf_remote() to register a callback appropriately.
- */
-static void __maybe_unused
-call_rcu_preempt_remote(struct rcu_head *rhp,
-			void (*func)(struct rcu_head *rhp))
-{
-	invoke_crf_remote(rhp, func, call_rcu);
-}
-static void call_rcu_bh_remote(struct rcu_head *rhp,
-			       void (*func)(struct rcu_head *rhp))
-{
-	invoke_crf_remote(rhp, func, call_rcu_bh);
-}
-static void call_rcu_sched_remote(struct rcu_head *rhp,
-				  void (*func)(struct rcu_head *rhp))
-{
-	invoke_crf_remote(rhp, func, call_rcu_sched);
+	trace_rcu_future_gp(rnp, rdp, c, "StartWait");
+	for (;;) {
+		wait_event_interruptible(
+			rnp->nocb_gp_wq[c & 0x1],
+			(d = ULONG_CMP_GE(ACCESS_ONCE(rnp->completed), c)));
+		if (likely(d))
+			break;
+		flush_signals(current);
+		trace_rcu_future_gp(rnp, rdp, c, "ResumeWait");
+	}
+	trace_rcu_future_gp(rnp, rdp, c, "EndWait");
+	smp_mb(); /* Ensure that CB invocation happens after GP end. */
 }
 
 /*
@@ -2390,7 +2224,7 @@ static int rcu_nocb_kthread(void *arg)
 		cl = atomic_long_xchg(&rdp->nocb_q_count_lazy, 0);
 		ACCESS_ONCE(rdp->nocb_p_count) += c;
 		ACCESS_ONCE(rdp->nocb_p_count_lazy) += cl;
-		wait_rcu_gp(rdp->rsp->call_remote);
+		rcu_nocb_wait_gp(rdp);
 
 		/* Each pass through the following loop invokes a callback. */
 		trace_rcu_batch_start(rdp->rsp->name, cl, c, -1);
@@ -2436,32 +2270,41 @@ static void __init rcu_spawn_nocb_kthreads(struct rcu_state *rsp)
 		return;
 	for_each_cpu(cpu, rcu_nocb_mask) {
 		rdp = per_cpu_ptr(rsp->rda, cpu);
-		t = kthread_run(rcu_nocb_kthread, rdp, "rcuo%d", cpu);
+		t = kthread_run(rcu_nocb_kthread, rdp,
+				"rcuo%c/%d", rsp->abbr, cpu);
 		BUG_ON(IS_ERR(t));
 		ACCESS_ONCE(rdp->nocb_kthread) = t;
 	}
 }
 
 /* Prevent __call_rcu() from enqueuing callbacks on no-CBs CPUs */
-static void init_nocb_callback_list(struct rcu_data *rdp)
+static bool init_nocb_callback_list(struct rcu_data *rdp)
 {
 	if (rcu_nocb_mask == NULL ||
 	    !cpumask_test_cpu(rdp->cpu, rcu_nocb_mask))
-		return;
+		return false;
 	rdp->nxttail[RCU_NEXT_TAIL] = NULL;
+	return true;
+}
+
+#else /* #ifdef CONFIG_RCU_NOCB_CPU */
+
+static int rcu_nocb_needs_gp(struct rcu_state *rsp)
+{
+	return 0;
 }
 
-/* Initialize the ->call_remote fields in the rcu_state structures. */
-static void __init rcu_init_nocb(void)
+static void rcu_nocb_gp_cleanup(struct rcu_state *rsp, struct rcu_node *rnp)
 {
-#ifdef CONFIG_PREEMPT_RCU
-	rcu_preempt_state.call_remote = call_rcu_preempt_remote;
-#endif /* #ifdef CONFIG_PREEMPT_RCU */
-	rcu_bh_state.call_remote = call_rcu_bh_remote;
-	rcu_sched_state.call_remote = call_rcu_sched_remote;
 }
 
-#else /* #ifdef CONFIG_RCU_NOCB_CPU */
+static void rcu_nocb_gp_set(struct rcu_node *rnp, int nrq)
+{
+}
+
+static void rcu_init_one_nocb(struct rcu_node *rnp)
+{
+}
 
 static bool is_nocb_cpu(int cpu)
 {
@@ -2480,11 +2323,6 @@ static bool __maybe_unused rcu_nocb_adopt_orphan_cbs(struct rcu_state *rsp,
 	return 0;
 }
 
-static bool nocb_cpu_expendable(int cpu)
-{
-	return 1;
-}
-
 static void __init rcu_boot_init_nocb_percpu_data(struct rcu_data *rdp)
 {
 }
@@ -2493,12 +2331,9 @@ static void __init rcu_spawn_nocb_kthreads(struct rcu_state *rsp)
 {
 }
 
-static void init_nocb_callback_list(struct rcu_data *rdp)
-{
-}
-
-static void __init rcu_init_nocb(void)
+static bool init_nocb_callback_list(struct rcu_data *rdp)
 {
+	return false;
 }
 
 #endif /* #else #ifdef CONFIG_RCU_NOCB_CPU */
diff --git a/kernel/rcutree_trace.c b/kernel/rcutree_trace.c
index 0d095dcaa67..49099e81c87 100644
--- a/kernel/rcutree_trace.c
+++ b/kernel/rcutree_trace.c
@@ -46,8 +46,6 @@
 #define RCU_TREE_NONCORE
 #include "rcutree.h"
 
-#define ulong2long(a) (*(long *)(&(a)))
-
 static int r_open(struct inode *inode, struct file *file,
 					const struct seq_operations *op)
 {
diff --git a/kernel/resource.c b/kernel/resource.c
index 73f35d4b30b..d7386986e10 100644
--- a/kernel/resource.c
+++ b/kernel/resource.c
@@ -21,6 +21,7 @@
 #include <linux/seq_file.h>
 #include <linux/device.h>
 #include <linux/pfn.h>
+#include <linux/mm.h>
 #include <asm/io.h>
 
 
@@ -50,6 +51,14 @@ struct resource_constraint {
 
 static DEFINE_RWLOCK(resource_lock);
 
+/*
+ * For memory hotplug, there is no way to free resource entries allocated
+ * by boot mem after the system is up. So for reusing the resource entry
+ * we need to remember the resource.
+ */
+static struct resource *bootmem_resource_free;
+static DEFINE_SPINLOCK(bootmem_resource_lock);
+
 static void *r_next(struct seq_file *m, void *v, loff_t *pos)
 {
 	struct resource *p = v;
@@ -151,6 +160,40 @@ __initcall(ioresources_init);
 
 #endif /* CONFIG_PROC_FS */
 
+static void free_resource(struct resource *res)
+{
+	if (!res)
+		return;
+
+	if (!PageSlab(virt_to_head_page(res))) {
+		spin_lock(&bootmem_resource_lock);
+		res->sibling = bootmem_resource_free;
+		bootmem_resource_free = res;
+		spin_unlock(&bootmem_resource_lock);
+	} else {
+		kfree(res);
+	}
+}
+
+static struct resource *alloc_resource(gfp_t flags)
+{
+	struct resource *res = NULL;
+
+	spin_lock(&bootmem_resource_lock);
+	if (bootmem_resource_free) {
+		res = bootmem_resource_free;
+		bootmem_resource_free = res->sibling;
+	}
+	spin_unlock(&bootmem_resource_lock);
+
+	if (res)
+		memset(res, 0, sizeof(struct resource));
+	else
+		res = kzalloc(sizeof(struct resource), flags);
+
+	return res;
+}
+
 /* Return the conflict entry if you can't request it */
 static struct resource * __request_resource(struct resource *root, struct resource *new)
 {
@@ -706,24 +749,13 @@ void insert_resource_expand_to_fit(struct resource *root, struct resource *new)
 	write_unlock(&resource_lock);
 }
 
-/**
- * adjust_resource - modify a resource's start and size
- * @res: resource to modify
- * @start: new start value
- * @size: new size
- *
- * Given an existing resource, change its start and size to match the
- * arguments.  Returns 0 on success, -EBUSY if it can't fit.
- * Existing children of the resource are assumed to be immutable.
- */
-int adjust_resource(struct resource *res, resource_size_t start, resource_size_t size)
+static int __adjust_resource(struct resource *res, resource_size_t start,
+				resource_size_t size)
 {
 	struct resource *tmp, *parent = res->parent;
 	resource_size_t end = start + size - 1;
 	int result = -EBUSY;
 
-	write_lock(&resource_lock);
-
 	if (!parent)
 		goto skip;
 
@@ -751,6 +783,26 @@ skip:
 	result = 0;
 
  out:
+	return result;
+}
+
+/**
+ * adjust_resource - modify a resource's start and size
+ * @res: resource to modify
+ * @start: new start value
+ * @size: new size
+ *
+ * Given an existing resource, change its start and size to match the
+ * arguments.  Returns 0 on success, -EBUSY if it can't fit.
+ * Existing children of the resource are assumed to be immutable.
+ */
+int adjust_resource(struct resource *res, resource_size_t start,
+			resource_size_t size)
+{
+	int result;
+
+	write_lock(&resource_lock);
+	result = __adjust_resource(res, start, size);
 	write_unlock(&resource_lock);
 	return result;
 }
@@ -762,7 +814,7 @@ static void __init __reserve_region_with_split(struct resource *root,
 {
 	struct resource *parent = root;
 	struct resource *conflict;
-	struct resource *res = kzalloc(sizeof(*res), GFP_ATOMIC);
+	struct resource *res = alloc_resource(GFP_ATOMIC);
 	struct resource *next_res = NULL;
 
 	if (!res)
@@ -787,7 +839,7 @@ static void __init __reserve_region_with_split(struct resource *root,
 		/* conflict covered whole area */
 		if (conflict->start <= res->start &&
 				conflict->end >= res->end) {
-			kfree(res);
+			free_resource(res);
 			WARN_ON(next_res);
 			break;
 		}
@@ -797,10 +849,9 @@ static void __init __reserve_region_with_split(struct resource *root,
 			end = res->end;
 			res->end = conflict->start - 1;
 			if (conflict->end < end) {
-				next_res = kzalloc(sizeof(*next_res),
-						GFP_ATOMIC);
+				next_res = alloc_resource(GFP_ATOMIC);
 				if (!next_res) {
-					kfree(res);
+					free_resource(res);
 					break;
 				}
 				next_res->name = name;
@@ -890,7 +941,7 @@ struct resource * __request_region(struct resource *parent,
 				   const char *name, int flags)
 {
 	DECLARE_WAITQUEUE(wait, current);
-	struct resource *res = kzalloc(sizeof(*res), GFP_KERNEL);
+	struct resource *res = alloc_resource(GFP_KERNEL);
 
 	if (!res)
 		return NULL;
@@ -924,7 +975,7 @@ struct resource * __request_region(struct resource *parent,
 			continue;
 		}
 		/* Uhhuh, that didn't work out.. */
-		kfree(res);
+		free_resource(res);
 		res = NULL;
 		break;
 	}
@@ -958,7 +1009,7 @@ int __check_region(struct resource *parent, resource_size_t start,
 		return -EBUSY;
 
 	release_resource(res);
-	kfree(res);
+	free_resource(res);
 	return 0;
 }
 EXPORT_SYMBOL(__check_region);
@@ -998,7 +1049,7 @@ void __release_region(struct resource *parent, resource_size_t start,
 			write_unlock(&resource_lock);
 			if (res->flags & IORESOURCE_MUXED)
 				wake_up(&muxed_resource_wait);
-			kfree(res);
+			free_resource(res);
 			return;
 		}
 		p = &res->sibling;
@@ -1012,6 +1063,109 @@ void __release_region(struct resource *parent, resource_size_t start,
 }
 EXPORT_SYMBOL(__release_region);
 
+#ifdef CONFIG_MEMORY_HOTREMOVE
+/**
+ * release_mem_region_adjustable - release a previously reserved memory region
+ * @parent: parent resource descriptor
+ * @start: resource start address
+ * @size: resource region size
+ *
+ * This interface is intended for memory hot-delete.  The requested region
+ * is released from a currently busy memory resource.  The requested region
+ * must either match exactly or fit into a single busy resource entry.  In
+ * the latter case, the remaining resource is adjusted accordingly.
+ * Existing children of the busy memory resource must be immutable in the
+ * request.
+ *
+ * Note:
+ * - Additional release conditions, such as overlapping region, can be
+ *   supported after they are confirmed as valid cases.
+ * - When a busy memory resource gets split into two entries, the code
+ *   assumes that all children remain in the lower address entry for
+ *   simplicity.  Enhance this logic when necessary.
+ */
+int release_mem_region_adjustable(struct resource *parent,
+			resource_size_t start, resource_size_t size)
+{
+	struct resource **p;
+	struct resource *res;
+	struct resource *new_res;
+	resource_size_t end;
+	int ret = -EINVAL;
+
+	end = start + size - 1;
+	if ((start < parent->start) || (end > parent->end))
+		return ret;
+
+	/* The alloc_resource() result gets checked later */
+	new_res = alloc_resource(GFP_KERNEL);
+
+	p = &parent->child;
+	write_lock(&resource_lock);
+
+	while ((res = *p)) {
+		if (res->start >= end)
+			break;
+
+		/* look for the next resource if it does not fit into */
+		if (res->start > start || res->end < end) {
+			p = &res->sibling;
+			continue;
+		}
+
+		if (!(res->flags & IORESOURCE_MEM))
+			break;
+
+		if (!(res->flags & IORESOURCE_BUSY)) {
+			p = &res->child;
+			continue;
+		}
+
+		/* found the target resource; let's adjust accordingly */
+		if (res->start == start && res->end == end) {
+			/* free the whole entry */
+			*p = res->sibling;
+			free_resource(res);
+			ret = 0;
+		} else if (res->start == start && res->end != end) {
+			/* adjust the start */
+			ret = __adjust_resource(res, end + 1,
+						res->end - end);
+		} else if (res->start != start && res->end == end) {
+			/* adjust the end */
+			ret = __adjust_resource(res, res->start,
+						start - res->start);
+		} else {
+			/* split into two entries */
+			if (!new_res) {
+				ret = -ENOMEM;
+				break;
+			}
+			new_res->name = res->name;
+			new_res->start = end + 1;
+			new_res->end = res->end;
+			new_res->flags = res->flags;
+			new_res->parent = res->parent;
+			new_res->sibling = res->sibling;
+			new_res->child = NULL;
+
+			ret = __adjust_resource(res, res->start,
+						start - res->start);
+			if (ret)
+				break;
+			res->sibling = new_res;
+			new_res = NULL;
+		}
+
+		break;
+	}
+
+	write_unlock(&resource_lock);
+	free_resource(new_res);
+	return ret;
+}
+#endif	/* CONFIG_MEMORY_HOTREMOVE */
+
 /*
  * Managed region resource
  */
diff --git a/kernel/rtmutex-tester.c b/kernel/rtmutex-tester.c
index 7890b10084a..1d96dd0d93c 100644
--- a/kernel/rtmutex-tester.c
+++ b/kernel/rtmutex-tester.c
@@ -14,6 +14,7 @@
 #include <linux/spinlock.h>
 #include <linux/timer.h>
 #include <linux/freezer.h>
+#include <linux/stat.h>
 
 #include "rtmutex.h"
 
@@ -366,8 +367,8 @@ static ssize_t sysfs_test_status(struct device *dev, struct device_attribute *at
 	return curr - buf;
 }
 
-static DEVICE_ATTR(status, 0600, sysfs_test_status, NULL);
-static DEVICE_ATTR(command, 0600, NULL, sysfs_test_command);
+static DEVICE_ATTR(status, S_IRUSR, sysfs_test_status, NULL);
+static DEVICE_ATTR(command, S_IWUSR, NULL, sysfs_test_command);
 
 static struct bus_type rttest_subsys = {
 	.name = "rttest",
diff --git a/kernel/sched/Makefile b/kernel/sched/Makefile
index f06d249e103..deaf90e4a1d 100644
--- a/kernel/sched/Makefile
+++ b/kernel/sched/Makefile
@@ -16,3 +16,4 @@ obj-$(CONFIG_SMP) += cpupri.o
 obj-$(CONFIG_SCHED_AUTOGROUP) += auto_group.o
 obj-$(CONFIG_SCHEDSTATS) += stats.o
 obj-$(CONFIG_SCHED_DEBUG) += debug.o
+obj-$(CONFIG_CGROUP_CPUACCT) += cpuacct.o
diff --git a/kernel/sched/clock.c b/kernel/sched/clock.c
index c685e31492d..c3ae1446461 100644
--- a/kernel/sched/clock.c
+++ b/kernel/sched/clock.c
@@ -176,10 +176,36 @@ static u64 sched_clock_remote(struct sched_clock_data *scd)
 	u64 this_clock, remote_clock;
 	u64 *ptr, old_val, val;
 
+#if BITS_PER_LONG != 64
+again:
+	/*
+	 * Careful here: The local and the remote clock values need to
+	 * be read out atomic as we need to compare the values and
+	 * then update either the local or the remote side. So the
+	 * cmpxchg64 below only protects one readout.
+	 *
+	 * We must reread via sched_clock_local() in the retry case on
+	 * 32bit as an NMI could use sched_clock_local() via the
+	 * tracer and hit between the readout of
+	 * the low32bit and the high 32bit portion.
+	 */
+	this_clock = sched_clock_local(my_scd);
+	/*
+	 * We must enforce atomic readout on 32bit, otherwise the
+	 * update on the remote cpu can hit inbetween the readout of
+	 * the low32bit and the high 32bit portion.
+	 */
+	remote_clock = cmpxchg64(&scd->clock, 0, 0);
+#else
+	/*
+	 * On 64bit the read of [my]scd->clock is atomic versus the
+	 * update, so we can avoid the above 32bit dance.
+	 */
 	sched_clock_local(my_scd);
 again:
 	this_clock = my_scd->clock;
 	remote_clock = scd->clock;
+#endif
 
 	/*
 	 * Use the opportunity that we have both locks
diff --git a/kernel/sched/core.c b/kernel/sched/core.c
index 7f12624a393..c70a8814a76 100644
--- a/kernel/sched/core.c
+++ b/kernel/sched/core.c
@@ -512,11 +512,6 @@ static inline void init_hrtick(void)
  * the target CPU.
  */
 #ifdef CONFIG_SMP
-
-#ifndef tsk_is_polling
-#define tsk_is_polling(t) 0
-#endif
-
 void resched_task(struct task_struct *p)
 {
 	int cpu;
@@ -1288,8 +1283,8 @@ static void ttwu_activate(struct rq *rq, struct task_struct *p, int en_flags)
 static void
 ttwu_do_wakeup(struct rq *rq, struct task_struct *p, int wake_flags)
 {
-	trace_sched_wakeup(p, true);
 	check_preempt_curr(rq, p, wake_flags);
+	trace_sched_wakeup(p, true);
 
 	p->state = TASK_RUNNING;
 #ifdef CONFIG_SMP
@@ -1498,8 +1493,10 @@ static void try_to_wake_up_local(struct task_struct *p)
 {
 	struct rq *rq = task_rq(p);
 
-	BUG_ON(rq != this_rq());
-	BUG_ON(p == current);
+	if (WARN_ON_ONCE(rq != this_rq()) ||
+	    WARN_ON_ONCE(p == current))
+		return;
+
 	lockdep_assert_held(&rq->lock);
 
 	if (!raw_spin_trylock(&p->pi_lock)) {
@@ -2997,51 +2994,6 @@ void __sched schedule_preempt_disabled(void)
 	preempt_disable();
 }
 
-#ifdef CONFIG_MUTEX_SPIN_ON_OWNER
-
-static inline bool owner_running(struct mutex *lock, struct task_struct *owner)
-{
-	if (lock->owner != owner)
-		return false;
-
-	/*
-	 * Ensure we emit the owner->on_cpu, dereference _after_ checking
-	 * lock->owner still matches owner, if that fails, owner might
-	 * point to free()d memory, if it still matches, the rcu_read_lock()
-	 * ensures the memory stays valid.
-	 */
-	barrier();
-
-	return owner->on_cpu;
-}
-
-/*
- * Look out! "owner" is an entirely speculative pointer
- * access and not reliable.
- */
-int mutex_spin_on_owner(struct mutex *lock, struct task_struct *owner)
-{
-	if (!sched_feat(OWNER_SPIN))
-		return 0;
-
-	rcu_read_lock();
-	while (owner_running(lock, owner)) {
-		if (need_resched())
-			break;
-
-		arch_mutex_cpu_relax();
-	}
-	rcu_read_unlock();
-
-	/*
-	 * We break out the loop above on need_resched() and when the
-	 * owner changed, which is a sign for heavy contention. Return
-	 * success only when lock->owner is NULL.
-	 */
-	return lock->owner == NULL;
-}
-#endif
-
 #ifdef CONFIG_PREEMPT
 /*
  * this is the entry point to schedule() from in-kernel preemption
@@ -3082,11 +3034,13 @@ EXPORT_SYMBOL(preempt_schedule);
 asmlinkage void __sched preempt_schedule_irq(void)
 {
 	struct thread_info *ti = current_thread_info();
+	enum ctx_state prev_state;
 
 	/* Catch callers which need to be fixed */
 	BUG_ON(ti->preempt_count || !irqs_disabled());
 
-	user_exit();
+	prev_state = exception_enter();
+
 	do {
 		add_preempt_count(PREEMPT_ACTIVE);
 		local_irq_enable();
@@ -3100,6 +3054,8 @@ asmlinkage void __sched preempt_schedule_irq(void)
 		 */
 		barrier();
 	} while (need_resched());
+
+	exception_exit(prev_state);
 }
 
 #endif /* CONFIG_PREEMPT */
@@ -4126,6 +4082,10 @@ long sched_setaffinity(pid_t pid, const struct cpumask *in_mask)
 	get_task_struct(p);
 	rcu_read_unlock();
 
+	if (p->flags & PF_NO_SETAFFINITY) {
+		retval = -EINVAL;
+		goto out_put_task;
+	}
 	if (!alloc_cpumask_var(&cpus_allowed, GFP_KERNEL)) {
 		retval = -ENOMEM;
 		goto out_put_task;
@@ -4773,11 +4733,6 @@ int set_cpus_allowed_ptr(struct task_struct *p, const struct cpumask *new_mask)
 		goto out;
 	}
 
-	if (unlikely((p->flags & PF_THREAD_BOUND) && p != current)) {
-		ret = -EINVAL;
-		goto out;
-	}
-
 	do_set_cpus_allowed(p, new_mask);
 
 	/* Can the task run on the task's current CPU? If so, we're done */
@@ -4999,7 +4954,7 @@ static void sd_free_ctl_entry(struct ctl_table **tablep)
 }
 
 static int min_load_idx = 0;
-static int max_load_idx = CPU_LOAD_IDX_MAX;
+static int max_load_idx = CPU_LOAD_IDX_MAX-1;
 
 static void
 set_table_entry(struct ctl_table *entry,
@@ -6248,7 +6203,7 @@ static void sched_init_numa(void)
 	 * 'level' contains the number of unique distances, excluding the
 	 * identity distance node_distance(i,i).
 	 *
-	 * The sched_domains_nume_distance[] array includes the actual distance
+	 * The sched_domains_numa_distance[] array includes the actual distance
 	 * numbers.
 	 */
 
@@ -6861,11 +6816,15 @@ int in_sched_functions(unsigned long addr)
 }
 
 #ifdef CONFIG_CGROUP_SCHED
+/*
+ * Default task group.
+ * Every task in system belongs to this group at bootup.
+ */
 struct task_group root_task_group;
 LIST_HEAD(task_groups);
 #endif
 
-DECLARE_PER_CPU(cpumask_var_t, load_balance_tmpmask);
+DECLARE_PER_CPU(cpumask_var_t, load_balance_mask);
 
 void __init sched_init(void)
 {
@@ -6902,7 +6861,7 @@ void __init sched_init(void)
 #endif /* CONFIG_RT_GROUP_SCHED */
 #ifdef CONFIG_CPUMASK_OFFSTACK
 		for_each_possible_cpu(i) {
-			per_cpu(load_balance_tmpmask, i) = (void *)ptr;
+			per_cpu(load_balance_mask, i) = (void *)ptr;
 			ptr += cpumask_size();
 		}
 #endif /* CONFIG_CPUMASK_OFFSTACK */
@@ -6928,12 +6887,6 @@ void __init sched_init(void)
 
 #endif /* CONFIG_CGROUP_SCHED */
 
-#ifdef CONFIG_CGROUP_CPUACCT
-	root_cpuacct.cpustat = &kernel_cpustat;
-	root_cpuacct.cpuusage = alloc_percpu(u64);
-	/* Too early, not expected to fail */
-	BUG_ON(!root_cpuacct.cpuusage);
-#endif
 	for_each_possible_cpu(i) {
 		struct rq *rq;
 
@@ -7455,7 +7408,7 @@ unlock:
 	return err;
 }
 
-int sched_group_set_rt_runtime(struct task_group *tg, long rt_runtime_us)
+static int sched_group_set_rt_runtime(struct task_group *tg, long rt_runtime_us)
 {
 	u64 rt_runtime, rt_period;
 
@@ -7467,7 +7420,7 @@ int sched_group_set_rt_runtime(struct task_group *tg, long rt_runtime_us)
 	return tg_set_rt_bandwidth(tg, rt_period, rt_runtime);
 }
 
-long sched_group_rt_runtime(struct task_group *tg)
+static long sched_group_rt_runtime(struct task_group *tg)
 {
 	u64 rt_runtime_us;
 
@@ -7479,7 +7432,7 @@ long sched_group_rt_runtime(struct task_group *tg)
 	return rt_runtime_us;
 }
 
-int sched_group_set_rt_period(struct task_group *tg, long rt_period_us)
+static int sched_group_set_rt_period(struct task_group *tg, long rt_period_us)
 {
 	u64 rt_runtime, rt_period;
 
@@ -7492,7 +7445,7 @@ int sched_group_set_rt_period(struct task_group *tg, long rt_period_us)
 	return tg_set_rt_bandwidth(tg, rt_period, rt_runtime);
 }
 
-long sched_group_rt_period(struct task_group *tg)
+static long sched_group_rt_period(struct task_group *tg)
 {
 	u64 rt_period_us;
 
@@ -7527,7 +7480,7 @@ static int sched_rt_global_constraints(void)
 	return ret;
 }
 
-int sched_rt_can_attach(struct task_group *tg, struct task_struct *tsk)
+static int sched_rt_can_attach(struct task_group *tg, struct task_struct *tsk)
 {
 	/* Don't accept realtime tasks when there is no way for them to run */
 	if (rt_task(tsk) && tg->rt_bandwidth.rt_runtime == 0)
@@ -8035,226 +7988,6 @@ struct cgroup_subsys cpu_cgroup_subsys = {
 
 #endif	/* CONFIG_CGROUP_SCHED */
 
-#ifdef CONFIG_CGROUP_CPUACCT
-
-/*
- * CPU accounting code for task groups.
- *
- * Based on the work by Paul Menage (menage@google.com) and Balbir Singh
- * (balbir@in.ibm.com).
- */
-
-struct cpuacct root_cpuacct;
-
-/* create a new cpu accounting group */
-static struct cgroup_subsys_state *cpuacct_css_alloc(struct cgroup *cgrp)
-{
-	struct cpuacct *ca;
-
-	if (!cgrp->parent)
-		return &root_cpuacct.css;
-
-	ca = kzalloc(sizeof(*ca), GFP_KERNEL);
-	if (!ca)
-		goto out;
-
-	ca->cpuusage = alloc_percpu(u64);
-	if (!ca->cpuusage)
-		goto out_free_ca;
-
-	ca->cpustat = alloc_percpu(struct kernel_cpustat);
-	if (!ca->cpustat)
-		goto out_free_cpuusage;
-
-	return &ca->css;
-
-out_free_cpuusage:
-	free_percpu(ca->cpuusage);
-out_free_ca:
-	kfree(ca);
-out:
-	return ERR_PTR(-ENOMEM);
-}
-
-/* destroy an existing cpu accounting group */
-static void cpuacct_css_free(struct cgroup *cgrp)
-{
-	struct cpuacct *ca = cgroup_ca(cgrp);
-
-	free_percpu(ca->cpustat);
-	free_percpu(ca->cpuusage);
-	kfree(ca);
-}
-
-static u64 cpuacct_cpuusage_read(struct cpuacct *ca, int cpu)
-{
-	u64 *cpuusage = per_cpu_ptr(ca->cpuusage, cpu);
-	u64 data;
-
-#ifndef CONFIG_64BIT
-	/*
-	 * Take rq->lock to make 64-bit read safe on 32-bit platforms.
-	 */
-	raw_spin_lock_irq(&cpu_rq(cpu)->lock);
-	data = *cpuusage;
-	raw_spin_unlock_irq(&cpu_rq(cpu)->lock);
-#else
-	data = *cpuusage;
-#endif
-
-	return data;
-}
-
-static void cpuacct_cpuusage_write(struct cpuacct *ca, int cpu, u64 val)
-{
-	u64 *cpuusage = per_cpu_ptr(ca->cpuusage, cpu);
-
-#ifndef CONFIG_64BIT
-	/*
-	 * Take rq->lock to make 64-bit write safe on 32-bit platforms.
-	 */
-	raw_spin_lock_irq(&cpu_rq(cpu)->lock);
-	*cpuusage = val;
-	raw_spin_unlock_irq(&cpu_rq(cpu)->lock);
-#else
-	*cpuusage = val;
-#endif
-}
-
-/* return total cpu usage (in nanoseconds) of a group */
-static u64 cpuusage_read(struct cgroup *cgrp, struct cftype *cft)
-{
-	struct cpuacct *ca = cgroup_ca(cgrp);
-	u64 totalcpuusage = 0;
-	int i;
-
-	for_each_present_cpu(i)
-		totalcpuusage += cpuacct_cpuusage_read(ca, i);
-
-	return totalcpuusage;
-}
-
-static int cpuusage_write(struct cgroup *cgrp, struct cftype *cftype,
-								u64 reset)
-{
-	struct cpuacct *ca = cgroup_ca(cgrp);
-	int err = 0;
-	int i;
-
-	if (reset) {
-		err = -EINVAL;
-		goto out;
-	}
-
-	for_each_present_cpu(i)
-		cpuacct_cpuusage_write(ca, i, 0);
-
-out:
-	return err;
-}
-
-static int cpuacct_percpu_seq_read(struct cgroup *cgroup, struct cftype *cft,
-				   struct seq_file *m)
-{
-	struct cpuacct *ca = cgroup_ca(cgroup);
-	u64 percpu;
-	int i;
-
-	for_each_present_cpu(i) {
-		percpu = cpuacct_cpuusage_read(ca, i);
-		seq_printf(m, "%llu ", (unsigned long long) percpu);
-	}
-	seq_printf(m, "\n");
-	return 0;
-}
-
-static const char *cpuacct_stat_desc[] = {
-	[CPUACCT_STAT_USER] = "user",
-	[CPUACCT_STAT_SYSTEM] = "system",
-};
-
-static int cpuacct_stats_show(struct cgroup *cgrp, struct cftype *cft,
-			      struct cgroup_map_cb *cb)
-{
-	struct cpuacct *ca = cgroup_ca(cgrp);
-	int cpu;
-	s64 val = 0;
-
-	for_each_online_cpu(cpu) {
-		struct kernel_cpustat *kcpustat = per_cpu_ptr(ca->cpustat, cpu);
-		val += kcpustat->cpustat[CPUTIME_USER];
-		val += kcpustat->cpustat[CPUTIME_NICE];
-	}
-	val = cputime64_to_clock_t(val);
-	cb->fill(cb, cpuacct_stat_desc[CPUACCT_STAT_USER], val);
-
-	val = 0;
-	for_each_online_cpu(cpu) {
-		struct kernel_cpustat *kcpustat = per_cpu_ptr(ca->cpustat, cpu);
-		val += kcpustat->cpustat[CPUTIME_SYSTEM];
-		val += kcpustat->cpustat[CPUTIME_IRQ];
-		val += kcpustat->cpustat[CPUTIME_SOFTIRQ];
-	}
-
-	val = cputime64_to_clock_t(val);
-	cb->fill(cb, cpuacct_stat_desc[CPUACCT_STAT_SYSTEM], val);
-
-	return 0;
-}
-
-static struct cftype files[] = {
-	{
-		.name = "usage",
-		.read_u64 = cpuusage_read,
-		.write_u64 = cpuusage_write,
-	},
-	{
-		.name = "usage_percpu",
-		.read_seq_string = cpuacct_percpu_seq_read,
-	},
-	{
-		.name = "stat",
-		.read_map = cpuacct_stats_show,
-	},
-	{ }	/* terminate */
-};
-
-/*
- * charge this task's execution time to its accounting group.
- *
- * called with rq->lock held.
- */
-void cpuacct_charge(struct task_struct *tsk, u64 cputime)
-{
-	struct cpuacct *ca;
-	int cpu;
-
-	if (unlikely(!cpuacct_subsys.active))
-		return;
-
-	cpu = task_cpu(tsk);
-
-	rcu_read_lock();
-
-	ca = task_ca(tsk);
-
-	for (; ca; ca = parent_ca(ca)) {
-		u64 *cpuusage = per_cpu_ptr(ca->cpuusage, cpu);
-		*cpuusage += cputime;
-	}
-
-	rcu_read_unlock();
-}
-
-struct cgroup_subsys cpuacct_subsys = {
-	.name = "cpuacct",
-	.css_alloc = cpuacct_css_alloc,
-	.css_free = cpuacct_css_free,
-	.subsys_id = cpuacct_subsys_id,
-	.base_cftypes = files,
-};
-#endif	/* CONFIG_CGROUP_CPUACCT */
-
 void dump_cpu_task(int cpu)
 {
 	pr_info("Task dump for CPU %d:\n", cpu);
diff --git a/kernel/sched/cpuacct.c b/kernel/sched/cpuacct.c
new file mode 100644
index 00000000000..dbb7e2cd95e
--- /dev/null
+++ b/kernel/sched/cpuacct.c
@@ -0,0 +1,296 @@
+#include <linux/cgroup.h>
+#include <linux/slab.h>
+#include <linux/percpu.h>
+#include <linux/spinlock.h>
+#include <linux/cpumask.h>
+#include <linux/seq_file.h>
+#include <linux/rcupdate.h>
+#include <linux/kernel_stat.h>
+#include <linux/err.h>
+
+#include "sched.h"
+
+/*
+ * CPU accounting code for task groups.
+ *
+ * Based on the work by Paul Menage (menage@google.com) and Balbir Singh
+ * (balbir@in.ibm.com).
+ */
+
+/* Time spent by the tasks of the cpu accounting group executing in ... */
+enum cpuacct_stat_index {
+	CPUACCT_STAT_USER,	/* ... user mode */
+	CPUACCT_STAT_SYSTEM,	/* ... kernel mode */
+
+	CPUACCT_STAT_NSTATS,
+};
+
+/* track cpu usage of a group of tasks and its child groups */
+struct cpuacct {
+	struct cgroup_subsys_state css;
+	/* cpuusage holds pointer to a u64-type object on every cpu */
+	u64 __percpu *cpuusage;
+	struct kernel_cpustat __percpu *cpustat;
+};
+
+/* return cpu accounting group corresponding to this container */
+static inline struct cpuacct *cgroup_ca(struct cgroup *cgrp)
+{
+	return container_of(cgroup_subsys_state(cgrp, cpuacct_subsys_id),
+			    struct cpuacct, css);
+}
+
+/* return cpu accounting group to which this task belongs */
+static inline struct cpuacct *task_ca(struct task_struct *tsk)
+{
+	return container_of(task_subsys_state(tsk, cpuacct_subsys_id),
+			    struct cpuacct, css);
+}
+
+static inline struct cpuacct *__parent_ca(struct cpuacct *ca)
+{
+	return cgroup_ca(ca->css.cgroup->parent);
+}
+
+static inline struct cpuacct *parent_ca(struct cpuacct *ca)
+{
+	if (!ca->css.cgroup->parent)
+		return NULL;
+	return cgroup_ca(ca->css.cgroup->parent);
+}
+
+static DEFINE_PER_CPU(u64, root_cpuacct_cpuusage);
+static struct cpuacct root_cpuacct = {
+	.cpustat	= &kernel_cpustat,
+	.cpuusage	= &root_cpuacct_cpuusage,
+};
+
+/* create a new cpu accounting group */
+static struct cgroup_subsys_state *cpuacct_css_alloc(struct cgroup *cgrp)
+{
+	struct cpuacct *ca;
+
+	if (!cgrp->parent)
+		return &root_cpuacct.css;
+
+	ca = kzalloc(sizeof(*ca), GFP_KERNEL);
+	if (!ca)
+		goto out;
+
+	ca->cpuusage = alloc_percpu(u64);
+	if (!ca->cpuusage)
+		goto out_free_ca;
+
+	ca->cpustat = alloc_percpu(struct kernel_cpustat);
+	if (!ca->cpustat)
+		goto out_free_cpuusage;
+
+	return &ca->css;
+
+out_free_cpuusage:
+	free_percpu(ca->cpuusage);
+out_free_ca:
+	kfree(ca);
+out:
+	return ERR_PTR(-ENOMEM);
+}
+
+/* destroy an existing cpu accounting group */
+static void cpuacct_css_free(struct cgroup *cgrp)
+{
+	struct cpuacct *ca = cgroup_ca(cgrp);
+
+	free_percpu(ca->cpustat);
+	free_percpu(ca->cpuusage);
+	kfree(ca);
+}
+
+static u64 cpuacct_cpuusage_read(struct cpuacct *ca, int cpu)
+{
+	u64 *cpuusage = per_cpu_ptr(ca->cpuusage, cpu);
+	u64 data;
+
+#ifndef CONFIG_64BIT
+	/*
+	 * Take rq->lock to make 64-bit read safe on 32-bit platforms.
+	 */
+	raw_spin_lock_irq(&cpu_rq(cpu)->lock);
+	data = *cpuusage;
+	raw_spin_unlock_irq(&cpu_rq(cpu)->lock);
+#else
+	data = *cpuusage;
+#endif
+
+	return data;
+}
+
+static void cpuacct_cpuusage_write(struct cpuacct *ca, int cpu, u64 val)
+{
+	u64 *cpuusage = per_cpu_ptr(ca->cpuusage, cpu);
+
+#ifndef CONFIG_64BIT
+	/*
+	 * Take rq->lock to make 64-bit write safe on 32-bit platforms.
+	 */
+	raw_spin_lock_irq(&cpu_rq(cpu)->lock);
+	*cpuusage = val;
+	raw_spin_unlock_irq(&cpu_rq(cpu)->lock);
+#else
+	*cpuusage = val;
+#endif
+}
+
+/* return total cpu usage (in nanoseconds) of a group */
+static u64 cpuusage_read(struct cgroup *cgrp, struct cftype *cft)
+{
+	struct cpuacct *ca = cgroup_ca(cgrp);
+	u64 totalcpuusage = 0;
+	int i;
+
+	for_each_present_cpu(i)
+		totalcpuusage += cpuacct_cpuusage_read(ca, i);
+
+	return totalcpuusage;
+}
+
+static int cpuusage_write(struct cgroup *cgrp, struct cftype *cftype,
+								u64 reset)
+{
+	struct cpuacct *ca = cgroup_ca(cgrp);
+	int err = 0;
+	int i;
+
+	if (reset) {
+		err = -EINVAL;
+		goto out;
+	}
+
+	for_each_present_cpu(i)
+		cpuacct_cpuusage_write(ca, i, 0);
+
+out:
+	return err;
+}
+
+static int cpuacct_percpu_seq_read(struct cgroup *cgroup, struct cftype *cft,
+				   struct seq_file *m)
+{
+	struct cpuacct *ca = cgroup_ca(cgroup);
+	u64 percpu;
+	int i;
+
+	for_each_present_cpu(i) {
+		percpu = cpuacct_cpuusage_read(ca, i);
+		seq_printf(m, "%llu ", (unsigned long long) percpu);
+	}
+	seq_printf(m, "\n");
+	return 0;
+}
+
+static const char * const cpuacct_stat_desc[] = {
+	[CPUACCT_STAT_USER] = "user",
+	[CPUACCT_STAT_SYSTEM] = "system",
+};
+
+static int cpuacct_stats_show(struct cgroup *cgrp, struct cftype *cft,
+			      struct cgroup_map_cb *cb)
+{
+	struct cpuacct *ca = cgroup_ca(cgrp);
+	int cpu;
+	s64 val = 0;
+
+	for_each_online_cpu(cpu) {
+		struct kernel_cpustat *kcpustat = per_cpu_ptr(ca->cpustat, cpu);
+		val += kcpustat->cpustat[CPUTIME_USER];
+		val += kcpustat->cpustat[CPUTIME_NICE];
+	}
+	val = cputime64_to_clock_t(val);
+	cb->fill(cb, cpuacct_stat_desc[CPUACCT_STAT_USER], val);
+
+	val = 0;
+	for_each_online_cpu(cpu) {
+		struct kernel_cpustat *kcpustat = per_cpu_ptr(ca->cpustat, cpu);
+		val += kcpustat->cpustat[CPUTIME_SYSTEM];
+		val += kcpustat->cpustat[CPUTIME_IRQ];
+		val += kcpustat->cpustat[CPUTIME_SOFTIRQ];
+	}
+
+	val = cputime64_to_clock_t(val);
+	cb->fill(cb, cpuacct_stat_desc[CPUACCT_STAT_SYSTEM], val);
+
+	return 0;
+}
+
+static struct cftype files[] = {
+	{
+		.name = "usage",
+		.read_u64 = cpuusage_read,
+		.write_u64 = cpuusage_write,
+	},
+	{
+		.name = "usage_percpu",
+		.read_seq_string = cpuacct_percpu_seq_read,
+	},
+	{
+		.name = "stat",
+		.read_map = cpuacct_stats_show,
+	},
+	{ }	/* terminate */
+};
+
+/*
+ * charge this task's execution time to its accounting group.
+ *
+ * called with rq->lock held.
+ */
+void cpuacct_charge(struct task_struct *tsk, u64 cputime)
+{
+	struct cpuacct *ca;
+	int cpu;
+
+	cpu = task_cpu(tsk);
+
+	rcu_read_lock();
+
+	ca = task_ca(tsk);
+
+	while (true) {
+		u64 *cpuusage = per_cpu_ptr(ca->cpuusage, cpu);
+		*cpuusage += cputime;
+
+		ca = parent_ca(ca);
+		if (!ca)
+			break;
+	}
+
+	rcu_read_unlock();
+}
+
+/*
+ * Add user/system time to cpuacct.
+ *
+ * Note: it's the caller that updates the account of the root cgroup.
+ */
+void cpuacct_account_field(struct task_struct *p, int index, u64 val)
+{
+	struct kernel_cpustat *kcpustat;
+	struct cpuacct *ca;
+
+	rcu_read_lock();
+	ca = task_ca(p);
+	while (ca != &root_cpuacct) {
+		kcpustat = this_cpu_ptr(ca->cpustat);
+		kcpustat->cpustat[index] += val;
+		ca = __parent_ca(ca);
+	}
+	rcu_read_unlock();
+}
+
+struct cgroup_subsys cpuacct_subsys = {
+	.name		= "cpuacct",
+	.css_alloc	= cpuacct_css_alloc,
+	.css_free	= cpuacct_css_free,
+	.subsys_id	= cpuacct_subsys_id,
+	.base_cftypes	= files,
+	.early_init	= 1,
+};
diff --git a/kernel/sched/cpuacct.h b/kernel/sched/cpuacct.h
new file mode 100644
index 00000000000..ed605624a5e
--- /dev/null
+++ b/kernel/sched/cpuacct.h
@@ -0,0 +1,17 @@
+#ifdef CONFIG_CGROUP_CPUACCT
+
+extern void cpuacct_charge(struct task_struct *tsk, u64 cputime);
+extern void cpuacct_account_field(struct task_struct *p, int index, u64 val);
+
+#else
+
+static inline void cpuacct_charge(struct task_struct *tsk, u64 cputime)
+{
+}
+
+static inline void
+cpuacct_account_field(struct task_struct *p, int index, u64 val)
+{
+}
+
+#endif
diff --git a/kernel/sched/cputime.c b/kernel/sched/cputime.c
index ed12cbb135f..ea32f02bf2c 100644
--- a/kernel/sched/cputime.c
+++ b/kernel/sched/cputime.c
@@ -115,10 +115,6 @@ static int irqtime_account_si_update(void)
 static inline void task_group_account_field(struct task_struct *p, int index,
 					    u64 tmp)
 {
-#ifdef CONFIG_CGROUP_CPUACCT
-	struct kernel_cpustat *kcpustat;
-	struct cpuacct *ca;
-#endif
 	/*
 	 * Since all updates are sure to touch the root cgroup, we
 	 * get ourselves ahead and touch it first. If the root cgroup
@@ -127,19 +123,7 @@ static inline void task_group_account_field(struct task_struct *p, int index,
 	 */
 	__get_cpu_var(kernel_cpustat).cpustat[index] += tmp;
 
-#ifdef CONFIG_CGROUP_CPUACCT
-	if (unlikely(!cpuacct_subsys.active))
-		return;
-
-	rcu_read_lock();
-	ca = task_ca(p);
-	while (ca && (ca != &root_cpuacct)) {
-		kcpustat = this_cpu_ptr(ca->cpustat);
-		kcpustat->cpustat[index] += tmp;
-		ca = parent_ca(ca);
-	}
-	rcu_read_unlock();
-#endif
+	cpuacct_account_field(p, index, tmp);
 }
 
 /*
@@ -310,7 +294,7 @@ void thread_group_cputime(struct task_struct *tsk, struct task_cputime *times)
 
 	t = tsk;
 	do {
-		task_cputime(tsk, &utime, &stime);
+		task_cputime(t, &utime, &stime);
 		times->utime += utime;
 		times->stime += stime;
 		times->sum_exec_runtime += task_sched_runtime(t);
@@ -388,82 +372,10 @@ static inline void irqtime_account_process_tick(struct task_struct *p, int user_
 						struct rq *rq) {}
 #endif /* CONFIG_IRQ_TIME_ACCOUNTING */
 
-#ifndef CONFIG_VIRT_CPU_ACCOUNTING_NATIVE
-/*
- * Account a single tick of cpu time.
- * @p: the process that the cpu time gets accounted to
- * @user_tick: indicates if the tick is a user or a system tick
- */
-void account_process_tick(struct task_struct *p, int user_tick)
-{
-	cputime_t one_jiffy_scaled = cputime_to_scaled(cputime_one_jiffy);
-	struct rq *rq = this_rq();
-
-	if (vtime_accounting_enabled())
-		return;
-
-	if (sched_clock_irqtime) {
-		irqtime_account_process_tick(p, user_tick, rq);
-		return;
-	}
-
-	if (steal_account_process_tick())
-		return;
-
-	if (user_tick)
-		account_user_time(p, cputime_one_jiffy, one_jiffy_scaled);
-	else if ((p != rq->idle) || (irq_count() != HARDIRQ_OFFSET))
-		account_system_time(p, HARDIRQ_OFFSET, cputime_one_jiffy,
-				    one_jiffy_scaled);
-	else
-		account_idle_time(cputime_one_jiffy);
-}
-
-/*
- * Account multiple ticks of steal time.
- * @p: the process from which the cpu time has been stolen
- * @ticks: number of stolen ticks
- */
-void account_steal_ticks(unsigned long ticks)
-{
-	account_steal_time(jiffies_to_cputime(ticks));
-}
-
-/*
- * Account multiple ticks of idle time.
- * @ticks: number of stolen ticks
- */
-void account_idle_ticks(unsigned long ticks)
-{
-
-	if (sched_clock_irqtime) {
-		irqtime_account_idle_ticks(ticks);
-		return;
-	}
-
-	account_idle_time(jiffies_to_cputime(ticks));
-}
-#endif /* !CONFIG_VIRT_CPU_ACCOUNTING_NATIVE */
-
 /*
  * Use precise platform statistics if available:
  */
 #ifdef CONFIG_VIRT_CPU_ACCOUNTING
-void task_cputime_adjusted(struct task_struct *p, cputime_t *ut, cputime_t *st)
-{
-	*ut = p->utime;
-	*st = p->stime;
-}
-
-void thread_group_cputime_adjusted(struct task_struct *p, cputime_t *ut, cputime_t *st)
-{
-	struct task_cputime cputime;
-
-	thread_group_cputime(p, &cputime);
-
-	*ut = cputime.utime;
-	*st = cputime.stime;
-}
 
 #ifndef __ARCH_HAS_VTIME_TASK_SWITCH
 void vtime_task_switch(struct task_struct *prev)
@@ -518,21 +430,111 @@ void vtime_account_irq_enter(struct task_struct *tsk)
 }
 EXPORT_SYMBOL_GPL(vtime_account_irq_enter);
 #endif /* __ARCH_HAS_VTIME_ACCOUNT */
+#endif /* CONFIG_VIRT_CPU_ACCOUNTING */
+
+
+#ifdef CONFIG_VIRT_CPU_ACCOUNTING_NATIVE
+void task_cputime_adjusted(struct task_struct *p, cputime_t *ut, cputime_t *st)
+{
+	*ut = p->utime;
+	*st = p->stime;
+}
 
-#else /* !CONFIG_VIRT_CPU_ACCOUNTING */
+void thread_group_cputime_adjusted(struct task_struct *p, cputime_t *ut, cputime_t *st)
+{
+	struct task_cputime cputime;
 
-static cputime_t scale_stime(cputime_t stime, cputime_t rtime, cputime_t total)
+	thread_group_cputime(p, &cputime);
+
+	*ut = cputime.utime;
+	*st = cputime.stime;
+}
+#else /* !CONFIG_VIRT_CPU_ACCOUNTING_NATIVE */
+/*
+ * Account a single tick of cpu time.
+ * @p: the process that the cpu time gets accounted to
+ * @user_tick: indicates if the tick is a user or a system tick
+ */
+void account_process_tick(struct task_struct *p, int user_tick)
 {
-	u64 temp = (__force u64) rtime;
+	cputime_t one_jiffy_scaled = cputime_to_scaled(cputime_one_jiffy);
+	struct rq *rq = this_rq();
 
-	temp *= (__force u64) stime;
+	if (vtime_accounting_enabled())
+		return;
+
+	if (sched_clock_irqtime) {
+		irqtime_account_process_tick(p, user_tick, rq);
+		return;
+	}
+
+	if (steal_account_process_tick())
+		return;
 
-	if (sizeof(cputime_t) == 4)
-		temp = div_u64(temp, (__force u32) total);
+	if (user_tick)
+		account_user_time(p, cputime_one_jiffy, one_jiffy_scaled);
+	else if ((p != rq->idle) || (irq_count() != HARDIRQ_OFFSET))
+		account_system_time(p, HARDIRQ_OFFSET, cputime_one_jiffy,
+				    one_jiffy_scaled);
 	else
-		temp = div64_u64(temp, (__force u64) total);
+		account_idle_time(cputime_one_jiffy);
+}
 
-	return (__force cputime_t) temp;
+/*
+ * Account multiple ticks of steal time.
+ * @p: the process from which the cpu time has been stolen
+ * @ticks: number of stolen ticks
+ */
+void account_steal_ticks(unsigned long ticks)
+{
+	account_steal_time(jiffies_to_cputime(ticks));
+}
+
+/*
+ * Account multiple ticks of idle time.
+ * @ticks: number of stolen ticks
+ */
+void account_idle_ticks(unsigned long ticks)
+{
+
+	if (sched_clock_irqtime) {
+		irqtime_account_idle_ticks(ticks);
+		return;
+	}
+
+	account_idle_time(jiffies_to_cputime(ticks));
+}
+
+/*
+ * Perform (stime * rtime) / total with reduced chances
+ * of multiplication overflows by using smaller factors
+ * like quotient and remainders of divisions between
+ * rtime and total.
+ */
+static cputime_t scale_stime(u64 stime, u64 rtime, u64 total)
+{
+	u64 rem, res, scaled;
+
+	if (rtime >= total) {
+		/*
+		 * Scale up to rtime / total then add
+		 * the remainder scaled to stime / total.
+		 */
+		res = div64_u64_rem(rtime, total, &rem);
+		scaled = stime * res;
+		scaled += div64_u64(stime * rem, total);
+	} else {
+		/*
+		 * Same in reverse: scale down to total / rtime
+		 * then substract that result scaled to
+		 * to the remaining part.
+		 */
+		res = div64_u64_rem(total, rtime, &rem);
+		scaled = div64_u64(stime, res);
+		scaled -= div64_u64(scaled * rem, total);
+	}
+
+	return (__force cputime_t) scaled;
 }
 
 /*
@@ -545,6 +547,12 @@ static void cputime_adjust(struct task_cputime *curr,
 {
 	cputime_t rtime, stime, total;
 
+	if (vtime_accounting_enabled()) {
+		*ut = curr->utime;
+		*st = curr->stime;
+		return;
+	}
+
 	stime = curr->stime;
 	total = stime + curr->utime;
 
@@ -560,10 +568,14 @@ static void cputime_adjust(struct task_cputime *curr,
 	 */
 	rtime = nsecs_to_cputime(curr->sum_exec_runtime);
 
-	if (total)
-		stime = scale_stime(stime, rtime, total);
-	else
+	if (!rtime) {
+		stime = 0;
+	} else if (!total) {
 		stime = rtime;
+	} else {
+		stime = scale_stime((__force u64)stime,
+				    (__force u64)rtime, (__force u64)total);
+	}
 
 	/*
 	 * If the tick based count grows faster than the scheduler one,
@@ -597,7 +609,7 @@ void thread_group_cputime_adjusted(struct task_struct *p, cputime_t *ut, cputime
 	thread_group_cputime(p, &cputime);
 	cputime_adjust(&cputime, &p->signal->prev_cputime, ut, st);
 }
-#endif /* !CONFIG_VIRT_CPU_ACCOUNTING */
+#endif /* !CONFIG_VIRT_CPU_ACCOUNTING_NATIVE */
 
 #ifdef CONFIG_VIRT_CPU_ACCOUNTING_GEN
 static unsigned long long vtime_delta(struct task_struct *tsk)
diff --git a/kernel/sched/fair.c b/kernel/sched/fair.c
index 7a33e5986fc..8bf7081b1ec 100644
--- a/kernel/sched/fair.c
+++ b/kernel/sched/fair.c
@@ -431,13 +431,13 @@ void account_cfs_rq_runtime(struct cfs_rq *cfs_rq, unsigned long delta_exec);
  * Scheduling class tree data structure manipulation methods:
  */
 
-static inline u64 max_vruntime(u64 min_vruntime, u64 vruntime)
+static inline u64 max_vruntime(u64 max_vruntime, u64 vruntime)
 {
-	s64 delta = (s64)(vruntime - min_vruntime);
+	s64 delta = (s64)(vruntime - max_vruntime);
 	if (delta > 0)
-		min_vruntime = vruntime;
+		max_vruntime = vruntime;
 
-	return min_vruntime;
+	return max_vruntime;
 }
 
 static inline u64 min_vruntime(u64 min_vruntime, u64 vruntime)
@@ -473,6 +473,7 @@ static void update_min_vruntime(struct cfs_rq *cfs_rq)
 			vruntime = min_vruntime(vruntime, se->vruntime);
 	}
 
+	/* ensure we never gain time by being placed backwards. */
 	cfs_rq->min_vruntime = max_vruntime(cfs_rq->min_vruntime, vruntime);
 #ifndef CONFIG_64BIT
 	smp_wmb();
@@ -652,7 +653,7 @@ static u64 sched_slice(struct cfs_rq *cfs_rq, struct sched_entity *se)
 }
 
 /*
- * We calculate the vruntime slice of a to be inserted task
+ * We calculate the vruntime slice of a to-be-inserted task.
  *
  * vs = s/w
  */
@@ -1562,6 +1563,27 @@ static inline void dequeue_entity_load_avg(struct cfs_rq *cfs_rq,
 		se->avg.decay_count = atomic64_read(&cfs_rq->decay_counter);
 	} /* migrations, e.g. sleep=0 leave decay_count == 0 */
 }
+
+/*
+ * Update the rq's load with the elapsed running time before entering
+ * idle. if the last scheduled task is not a CFS task, idle_enter will
+ * be the only way to update the runnable statistic.
+ */
+void idle_enter_fair(struct rq *this_rq)
+{
+	update_rq_runnable_avg(this_rq, 1);
+}
+
+/*
+ * Update the rq's load with the elapsed idle time before a task is
+ * scheduled. if the newly scheduled task is not a CFS task, idle_exit will
+ * be the only way to update the runnable statistic.
+ */
+void idle_exit_fair(struct rq *this_rq)
+{
+	update_rq_runnable_avg(this_rq, 0);
+}
+
 #else
 static inline void update_entity_load_avg(struct sched_entity *se,
 					  int update_cfs_rq) {}
@@ -3874,12 +3896,16 @@ int can_migrate_task(struct task_struct *p, struct lb_env *env)
 	int tsk_cache_hot = 0;
 	/*
 	 * We do not migrate tasks that are:
-	 * 1) running (obviously), or
+	 * 1) throttled_lb_pair, or
 	 * 2) cannot be migrated to this CPU due to cpus_allowed, or
-	 * 3) are cache-hot on their current CPU.
+	 * 3) running (obviously), or
+	 * 4) are cache-hot on their current CPU.
 	 */
+	if (throttled_lb_pair(task_group(p), env->src_cpu, env->dst_cpu))
+		return 0;
+
 	if (!cpumask_test_cpu(env->dst_cpu, tsk_cpus_allowed(p))) {
-		int new_dst_cpu;
+		int cpu;
 
 		schedstat_inc(p, se.statistics.nr_failed_migrations_affine);
 
@@ -3894,12 +3920,15 @@ int can_migrate_task(struct task_struct *p, struct lb_env *env)
 		if (!env->dst_grpmask || (env->flags & LBF_SOME_PINNED))
 			return 0;
 
-		new_dst_cpu = cpumask_first_and(env->dst_grpmask,
-						tsk_cpus_allowed(p));
-		if (new_dst_cpu < nr_cpu_ids) {
-			env->flags |= LBF_SOME_PINNED;
-			env->new_dst_cpu = new_dst_cpu;
+		/* Prevent to re-select dst_cpu via env's cpus */
+		for_each_cpu_and(cpu, env->dst_grpmask, env->cpus) {
+			if (cpumask_test_cpu(cpu, tsk_cpus_allowed(p))) {
+				env->flags |= LBF_SOME_PINNED;
+				env->new_dst_cpu = cpu;
+				break;
+			}
 		}
+
 		return 0;
 	}
 
@@ -3920,20 +3949,17 @@ int can_migrate_task(struct task_struct *p, struct lb_env *env)
 	tsk_cache_hot = task_hot(p, env->src_rq->clock_task, env->sd);
 	if (!tsk_cache_hot ||
 		env->sd->nr_balance_failed > env->sd->cache_nice_tries) {
-#ifdef CONFIG_SCHEDSTATS
+
 		if (tsk_cache_hot) {
 			schedstat_inc(env->sd, lb_hot_gained[env->idle]);
 			schedstat_inc(p, se.statistics.nr_forced_migrations);
 		}
-#endif
+
 		return 1;
 	}
 
-	if (tsk_cache_hot) {
-		schedstat_inc(p, se.statistics.nr_failed_migrations_hot);
-		return 0;
-	}
-	return 1;
+	schedstat_inc(p, se.statistics.nr_failed_migrations_hot);
+	return 0;
 }
 
 /*
@@ -3948,9 +3974,6 @@ static int move_one_task(struct lb_env *env)
 	struct task_struct *p, *n;
 
 	list_for_each_entry_safe(p, n, &env->src_rq->cfs_tasks, se.group_node) {
-		if (throttled_lb_pair(task_group(p), env->src_rq->cpu, env->dst_cpu))
-			continue;
-
 		if (!can_migrate_task(p, env))
 			continue;
 
@@ -4002,7 +4025,7 @@ static int move_tasks(struct lb_env *env)
 			break;
 		}
 
-		if (throttled_lb_pair(task_group(p), env->src_cpu, env->dst_cpu))
+		if (!can_migrate_task(p, env))
 			goto next;
 
 		load = task_h_load(p);
@@ -4013,9 +4036,6 @@ static int move_tasks(struct lb_env *env)
 		if ((load / 2) > env->imbalance)
 			goto next;
 
-		if (!can_migrate_task(p, env))
-			goto next;
-
 		move_task(p, env);
 		pulled++;
 		env->imbalance -= load;
@@ -4245,7 +4265,7 @@ static inline int get_sd_load_idx(struct sched_domain *sd,
 	return load_idx;
 }
 
-unsigned long default_scale_freq_power(struct sched_domain *sd, int cpu)
+static unsigned long default_scale_freq_power(struct sched_domain *sd, int cpu)
 {
 	return SCHED_POWER_SCALE;
 }
@@ -4255,7 +4275,7 @@ unsigned long __weak arch_scale_freq_power(struct sched_domain *sd, int cpu)
 	return default_scale_freq_power(sd, cpu);
 }
 
-unsigned long default_scale_smt_power(struct sched_domain *sd, int cpu)
+static unsigned long default_scale_smt_power(struct sched_domain *sd, int cpu)
 {
 	unsigned long weight = sd->span_weight;
 	unsigned long smt_gain = sd->smt_gain;
@@ -4270,7 +4290,7 @@ unsigned long __weak arch_scale_smt_power(struct sched_domain *sd, int cpu)
 	return default_scale_smt_power(sd, cpu);
 }
 
-unsigned long scale_rt_power(int cpu)
+static unsigned long scale_rt_power(int cpu)
 {
 	struct rq *rq = cpu_rq(cpu);
 	u64 total, available, age_stamp, avg;
@@ -4960,7 +4980,7 @@ static struct rq *find_busiest_queue(struct lb_env *env,
 #define MAX_PINNED_INTERVAL	512
 
 /* Working cpumask for load_balance and load_balance_newidle. */
-DEFINE_PER_CPU(cpumask_var_t, load_balance_tmpmask);
+DEFINE_PER_CPU(cpumask_var_t, load_balance_mask);
 
 static int need_active_balance(struct lb_env *env)
 {
@@ -4991,11 +5011,10 @@ static int load_balance(int this_cpu, struct rq *this_rq,
 			int *balance)
 {
 	int ld_moved, cur_ld_moved, active_balance = 0;
-	int lb_iterations, max_lb_iterations;
 	struct sched_group *group;
 	struct rq *busiest;
 	unsigned long flags;
-	struct cpumask *cpus = __get_cpu_var(load_balance_tmpmask);
+	struct cpumask *cpus = __get_cpu_var(load_balance_mask);
 
 	struct lb_env env = {
 		.sd		= sd,
@@ -5007,8 +5026,14 @@ static int load_balance(int this_cpu, struct rq *this_rq,
 		.cpus		= cpus,
 	};
 
+	/*
+	 * For NEWLY_IDLE load_balancing, we don't need to consider
+	 * other cpus in our group
+	 */
+	if (idle == CPU_NEWLY_IDLE)
+		env.dst_grpmask = NULL;
+
 	cpumask_copy(cpus, cpu_active_mask);
-	max_lb_iterations = cpumask_weight(env.dst_grpmask);
 
 	schedstat_inc(sd, lb_count[idle]);
 
@@ -5034,7 +5059,6 @@ redo:
 	schedstat_add(sd, lb_imbalance[idle], env.imbalance);
 
 	ld_moved = 0;
-	lb_iterations = 1;
 	if (busiest->nr_running > 1) {
 		/*
 		 * Attempt to move tasks. If find_busiest_group has found
@@ -5061,17 +5085,17 @@ more_balance:
 		double_rq_unlock(env.dst_rq, busiest);
 		local_irq_restore(flags);
 
-		if (env.flags & LBF_NEED_BREAK) {
-			env.flags &= ~LBF_NEED_BREAK;
-			goto more_balance;
-		}
-
 		/*
 		 * some other cpu did the load balance for us.
 		 */
 		if (cur_ld_moved && env.dst_cpu != smp_processor_id())
 			resched_cpu(env.dst_cpu);
 
+		if (env.flags & LBF_NEED_BREAK) {
+			env.flags &= ~LBF_NEED_BREAK;
+			goto more_balance;
+		}
+
 		/*
 		 * Revisit (affine) tasks on src_cpu that couldn't be moved to
 		 * us and move them to an alternate dst_cpu in our sched_group
@@ -5091,14 +5115,17 @@ more_balance:
 		 * moreover subsequent load balance cycles should correct the
 		 * excess load moved.
 		 */
-		if ((env.flags & LBF_SOME_PINNED) && env.imbalance > 0 &&
-				lb_iterations++ < max_lb_iterations) {
+		if ((env.flags & LBF_SOME_PINNED) && env.imbalance > 0) {
 
 			env.dst_rq	 = cpu_rq(env.new_dst_cpu);
 			env.dst_cpu	 = env.new_dst_cpu;
 			env.flags	&= ~LBF_SOME_PINNED;
 			env.loop	 = 0;
 			env.loop_break	 = sched_nr_migrate_break;
+
+			/* Prevent to re-select dst_cpu via env's cpus */
+			cpumask_clear_cpu(env.dst_cpu, env.cpus);
+
 			/*
 			 * Go back to "more_balance" rather than "redo" since we
 			 * need to continue with same src_cpu.
@@ -5219,8 +5246,6 @@ void idle_balance(int this_cpu, struct rq *this_rq)
 	if (this_rq->avg_idle < sysctl_sched_migration_cost)
 		return;
 
-	update_rq_runnable_avg(this_rq, 1);
-
 	/*
 	 * Drop the rq->lock, but keep IRQ/preempt disabled.
 	 */
@@ -5395,13 +5420,16 @@ static inline void set_cpu_sd_state_busy(void)
 	struct sched_domain *sd;
 	int cpu = smp_processor_id();
 
-	if (!test_bit(NOHZ_IDLE, nohz_flags(cpu)))
-		return;
-	clear_bit(NOHZ_IDLE, nohz_flags(cpu));
-
 	rcu_read_lock();
-	for_each_domain(cpu, sd)
+	sd = rcu_dereference_check_sched_domain(cpu_rq(cpu)->sd);
+
+	if (!sd || !sd->nohz_idle)
+		goto unlock;
+	sd->nohz_idle = 0;
+
+	for (; sd; sd = sd->parent)
 		atomic_inc(&sd->groups->sgp->nr_busy_cpus);
+unlock:
 	rcu_read_unlock();
 }
 
@@ -5410,13 +5438,16 @@ void set_cpu_sd_state_idle(void)
 	struct sched_domain *sd;
 	int cpu = smp_processor_id();
 
-	if (test_bit(NOHZ_IDLE, nohz_flags(cpu)))
-		return;
-	set_bit(NOHZ_IDLE, nohz_flags(cpu));
-
 	rcu_read_lock();
-	for_each_domain(cpu, sd)
+	sd = rcu_dereference_check_sched_domain(cpu_rq(cpu)->sd);
+
+	if (!sd || sd->nohz_idle)
+		goto unlock;
+	sd->nohz_idle = 1;
+
+	for (; sd; sd = sd->parent)
 		atomic_dec(&sd->groups->sgp->nr_busy_cpus);
+unlock:
 	rcu_read_unlock();
 }
 
@@ -5468,7 +5499,7 @@ void update_max_interval(void)
  * It checks each scheduling domain to see if it is due to be balanced,
  * and initiates a balancing operation if so.
  *
- * Balancing parameters are set up in arch_init_sched_domains.
+ * Balancing parameters are set up in init_sched_domains.
  */
 static void rebalance_domains(int cpu, enum cpu_idle_type idle)
 {
@@ -5506,10 +5537,11 @@ static void rebalance_domains(int cpu, enum cpu_idle_type idle)
 		if (time_after_eq(jiffies, sd->last_balance + interval)) {
 			if (load_balance(cpu, rq, sd, idle, &balance)) {
 				/*
-				 * We've pulled tasks over so either we're no
-				 * longer idle.
+				 * The LBF_SOME_PINNED logic could have changed
+				 * env->dst_cpu, so we can't know our idle
+				 * state even if we migrated tasks. Update it.
 				 */
-				idle = CPU_NOT_IDLE;
+				idle = idle_cpu(cpu) ? CPU_IDLE : CPU_NOT_IDLE;
 			}
 			sd->last_balance = jiffies;
 		}
diff --git a/kernel/sched/features.h b/kernel/sched/features.h
index 1ad1d2b5395..99399f8e479 100644
--- a/kernel/sched/features.h
+++ b/kernel/sched/features.h
@@ -46,13 +46,6 @@ SCHED_FEAT(DOUBLE_TICK, false)
 SCHED_FEAT(LB_BIAS, true)
 
 /*
- * Spin-wait on mutex acquisition when the mutex owner is running on
- * another cpu -- assumes that when the owner is running, it will soon
- * release the lock. Decreases scheduling overhead.
- */
-SCHED_FEAT(OWNER_SPIN, true)
-
-/*
  * Decrement CPU power based on time not spent running tasks
  */
 SCHED_FEAT(NONTASK_POWER, true)
diff --git a/kernel/sched/idle_task.c b/kernel/sched/idle_task.c
index b6baf370cae..b8ce7732834 100644
--- a/kernel/sched/idle_task.c
+++ b/kernel/sched/idle_task.c
@@ -13,6 +13,16 @@ select_task_rq_idle(struct task_struct *p, int sd_flag, int flags)
 {
 	return task_cpu(p); /* IDLE tasks as never migrated */
 }
+
+static void pre_schedule_idle(struct rq *rq, struct task_struct *prev)
+{
+	idle_exit_fair(rq);
+}
+
+static void post_schedule_idle(struct rq *rq)
+{
+	idle_enter_fair(rq);
+}
 #endif /* CONFIG_SMP */
 /*
  * Idle tasks are unconditionally rescheduled:
@@ -25,6 +35,10 @@ static void check_preempt_curr_idle(struct rq *rq, struct task_struct *p, int fl
 static struct task_struct *pick_next_task_idle(struct rq *rq)
 {
 	schedstat_inc(rq, sched_goidle);
+#ifdef CONFIG_SMP
+	/* Trigger the post schedule to do an idle_enter for CFS */
+	rq->post_schedule = 1;
+#endif
 	return rq->idle;
 }
 
@@ -86,6 +100,8 @@ const struct sched_class idle_sched_class = {
 
 #ifdef CONFIG_SMP
 	.select_task_rq		= select_task_rq_idle,
+	.pre_schedule		= pre_schedule_idle,
+	.post_schedule		= post_schedule_idle,
 #endif
 
 	.set_curr_task          = set_curr_task_idle,
diff --git a/kernel/sched/sched.h b/kernel/sched/sched.h
index cc03cfdf469..4c225c4c711 100644
--- a/kernel/sched/sched.h
+++ b/kernel/sched/sched.h
@@ -7,6 +7,7 @@
 #include <linux/stop_machine.h>
 
 #include "cpupri.h"
+#include "cpuacct.h"
 
 extern __read_mostly int scheduler_running;
 
@@ -33,6 +34,31 @@ extern __read_mostly int scheduler_running;
  */
 #define NS_TO_JIFFIES(TIME)	((unsigned long)(TIME) / (NSEC_PER_SEC / HZ))
 
+/*
+ * Increase resolution of nice-level calculations for 64-bit architectures.
+ * The extra resolution improves shares distribution and load balancing of
+ * low-weight task groups (eg. nice +19 on an autogroup), deeper taskgroup
+ * hierarchies, especially on larger systems. This is not a user-visible change
+ * and does not change the user-interface for setting shares/weights.
+ *
+ * We increase resolution only if we have enough bits to allow this increased
+ * resolution (i.e. BITS_PER_LONG > 32). The costs for increasing resolution
+ * when BITS_PER_LONG <= 32 are pretty high and the returns do not justify the
+ * increased costs.
+ */
+#if 0 /* BITS_PER_LONG > 32 -- currently broken: it increases power usage under light load  */
+# define SCHED_LOAD_RESOLUTION	10
+# define scale_load(w)		((w) << SCHED_LOAD_RESOLUTION)
+# define scale_load_down(w)	((w) >> SCHED_LOAD_RESOLUTION)
+#else
+# define SCHED_LOAD_RESOLUTION	0
+# define scale_load(w)		(w)
+# define scale_load_down(w)	(w)
+#endif
+
+#define SCHED_LOAD_SHIFT	(10 + SCHED_LOAD_RESOLUTION)
+#define SCHED_LOAD_SCALE	(1L << SCHED_LOAD_SHIFT)
+
 #define NICE_0_LOAD		SCHED_LOAD_SCALE
 #define NICE_0_SHIFT		SCHED_LOAD_SHIFT
 
@@ -154,11 +180,6 @@ struct task_group {
 #define MAX_SHARES	(1UL << 18)
 #endif
 
-/* Default task group.
- *	Every task in system belong to this group at bootup.
- */
-extern struct task_group root_task_group;
-
 typedef int (*tg_visitor)(struct task_group *, void *);
 
 extern int walk_tg_tree_from(struct task_group *from,
@@ -196,6 +217,18 @@ extern void init_tg_rt_entry(struct task_group *tg, struct rt_rq *rt_rq,
 		struct sched_rt_entity *rt_se, int cpu,
 		struct sched_rt_entity *parent);
 
+extern struct task_group *sched_create_group(struct task_group *parent);
+extern void sched_online_group(struct task_group *tg,
+			       struct task_group *parent);
+extern void sched_destroy_group(struct task_group *tg);
+extern void sched_offline_group(struct task_group *tg);
+
+extern void sched_move_task(struct task_struct *tsk);
+
+#ifdef CONFIG_FAIR_GROUP_SCHED
+extern int sched_group_set_shares(struct task_group *tg, unsigned long shares);
+#endif
+
 #else /* CONFIG_CGROUP_SCHED */
 
 struct cfs_bandwidth { };
@@ -547,6 +580,62 @@ static inline struct sched_domain *highest_flag_domain(int cpu, int flag)
 DECLARE_PER_CPU(struct sched_domain *, sd_llc);
 DECLARE_PER_CPU(int, sd_llc_id);
 
+struct sched_group_power {
+	atomic_t ref;
+	/*
+	 * CPU power of this group, SCHED_LOAD_SCALE being max power for a
+	 * single CPU.
+	 */
+	unsigned int power, power_orig;
+	unsigned long next_update;
+	/*
+	 * Number of busy cpus in this group.
+	 */
+	atomic_t nr_busy_cpus;
+
+	unsigned long cpumask[0]; /* iteration mask */
+};
+
+struct sched_group {
+	struct sched_group *next;	/* Must be a circular list */
+	atomic_t ref;
+
+	unsigned int group_weight;
+	struct sched_group_power *sgp;
+
+	/*
+	 * The CPUs this group covers.
+	 *
+	 * NOTE: this field is variable length. (Allocated dynamically
+	 * by attaching extra space to the end of the structure,
+	 * depending on how many CPUs the kernel has booted up with)
+	 */
+	unsigned long cpumask[0];
+};
+
+static inline struct cpumask *sched_group_cpus(struct sched_group *sg)
+{
+	return to_cpumask(sg->cpumask);
+}
+
+/*
+ * cpumask masking which cpus in the group are allowed to iterate up the domain
+ * tree.
+ */
+static inline struct cpumask *sched_group_mask(struct sched_group *sg)
+{
+	return to_cpumask(sg->sgp->cpumask);
+}
+
+/**
+ * group_first_cpu - Returns the first cpu in the cpumask of a sched_group.
+ * @group: The group whose first cpu is to be returned.
+ */
+static inline unsigned int group_first_cpu(struct sched_group *group)
+{
+	return cpumask_first(sched_group_cpus(group));
+}
+
 extern int group_balance_cpu(struct sched_group *sg);
 
 #endif /* CONFIG_SMP */
@@ -784,6 +873,12 @@ static inline void finish_lock_switch(struct rq *rq, struct task_struct *prev)
 }
 #endif /* __ARCH_WANT_UNLOCKED_CTXSW */
 
+/*
+ * wake flags
+ */
+#define WF_SYNC		0x01		/* waker goes to sleep after wakeup */
+#define WF_FORK		0x02		/* child wakeup after fork */
+#define WF_MIGRATED	0x4		/* internal use, task got migrated */
 
 static inline void update_load_add(struct load_weight *lw, unsigned long inc)
 {
@@ -856,14 +951,61 @@ static const u32 prio_to_wmult[40] = {
  /*  15 */ 119304647, 148102320, 186737708, 238609294, 286331153,
 };
 
-/* Time spent by the tasks of the cpu accounting group executing in ... */
-enum cpuacct_stat_index {
-	CPUACCT_STAT_USER,	/* ... user mode */
-	CPUACCT_STAT_SYSTEM,	/* ... kernel mode */
+#define ENQUEUE_WAKEUP		1
+#define ENQUEUE_HEAD		2
+#ifdef CONFIG_SMP
+#define ENQUEUE_WAKING		4	/* sched_class::task_waking was called */
+#else
+#define ENQUEUE_WAKING		0
+#endif
 
-	CPUACCT_STAT_NSTATS,
-};
+#define DEQUEUE_SLEEP		1
+
+struct sched_class {
+	const struct sched_class *next;
+
+	void (*enqueue_task) (struct rq *rq, struct task_struct *p, int flags);
+	void (*dequeue_task) (struct rq *rq, struct task_struct *p, int flags);
+	void (*yield_task) (struct rq *rq);
+	bool (*yield_to_task) (struct rq *rq, struct task_struct *p, bool preempt);
+
+	void (*check_preempt_curr) (struct rq *rq, struct task_struct *p, int flags);
+
+	struct task_struct * (*pick_next_task) (struct rq *rq);
+	void (*put_prev_task) (struct rq *rq, struct task_struct *p);
+
+#ifdef CONFIG_SMP
+	int  (*select_task_rq)(struct task_struct *p, int sd_flag, int flags);
+	void (*migrate_task_rq)(struct task_struct *p, int next_cpu);
+
+	void (*pre_schedule) (struct rq *this_rq, struct task_struct *task);
+	void (*post_schedule) (struct rq *this_rq);
+	void (*task_waking) (struct task_struct *task);
+	void (*task_woken) (struct rq *this_rq, struct task_struct *task);
+
+	void (*set_cpus_allowed)(struct task_struct *p,
+				 const struct cpumask *newmask);
 
+	void (*rq_online)(struct rq *rq);
+	void (*rq_offline)(struct rq *rq);
+#endif
+
+	void (*set_curr_task) (struct rq *rq);
+	void (*task_tick) (struct rq *rq, struct task_struct *p, int queued);
+	void (*task_fork) (struct task_struct *p);
+
+	void (*switched_from) (struct rq *this_rq, struct task_struct *task);
+	void (*switched_to) (struct rq *this_rq, struct task_struct *task);
+	void (*prio_changed) (struct rq *this_rq, struct task_struct *task,
+			     int oldprio);
+
+	unsigned int (*get_rr_interval) (struct rq *rq,
+					 struct task_struct *task);
+
+#ifdef CONFIG_FAIR_GROUP_SCHED
+	void (*task_move_group) (struct task_struct *p, int on_rq);
+#endif
+};
 
 #define sched_class_highest (&stop_sched_class)
 #define for_each_class(class) \
@@ -877,9 +1019,23 @@ extern const struct sched_class idle_sched_class;
 
 #ifdef CONFIG_SMP
 
+extern void update_group_power(struct sched_domain *sd, int cpu);
+
 extern void trigger_load_balance(struct rq *rq, int cpu);
 extern void idle_balance(int this_cpu, struct rq *this_rq);
 
+/*
+ * Only depends on SMP, FAIR_GROUP_SCHED may be removed when runnable_avg
+ * becomes useful in lb
+ */
+#if defined(CONFIG_FAIR_GROUP_SCHED)
+extern void idle_enter_fair(struct rq *this_rq);
+extern void idle_exit_fair(struct rq *this_rq);
+#else
+static inline void idle_enter_fair(struct rq *this_rq) {}
+static inline void idle_exit_fair(struct rq *this_rq) {}
+#endif
+
 #else	/* CONFIG_SMP */
 
 static inline void idle_balance(int cpu, struct rq *rq)
@@ -891,7 +1047,6 @@ static inline void idle_balance(int cpu, struct rq *rq)
 extern void sysrq_sched_debug_show(void);
 extern void sched_init_granularity(void);
 extern void update_max_interval(void);
-extern void update_group_power(struct sched_domain *sd, int cpu);
 extern int update_runtime(struct notifier_block *nfb, unsigned long action, void *hcpu);
 extern void init_sched_rt_class(void);
 extern void init_sched_fair_class(void);
@@ -904,45 +1059,6 @@ extern void init_rt_bandwidth(struct rt_bandwidth *rt_b, u64 period, u64 runtime
 
 extern void update_idle_cpu_load(struct rq *this_rq);
 
-#ifdef CONFIG_CGROUP_CPUACCT
-#include <linux/cgroup.h>
-/* track cpu usage of a group of tasks and its child groups */
-struct cpuacct {
-	struct cgroup_subsys_state css;
-	/* cpuusage holds pointer to a u64-type object on every cpu */
-	u64 __percpu *cpuusage;
-	struct kernel_cpustat __percpu *cpustat;
-};
-
-extern struct cgroup_subsys cpuacct_subsys;
-extern struct cpuacct root_cpuacct;
-
-/* return cpu accounting group corresponding to this container */
-static inline struct cpuacct *cgroup_ca(struct cgroup *cgrp)
-{
-	return container_of(cgroup_subsys_state(cgrp, cpuacct_subsys_id),
-			    struct cpuacct, css);
-}
-
-/* return cpu accounting group to which this task belongs */
-static inline struct cpuacct *task_ca(struct task_struct *tsk)
-{
-	return container_of(task_subsys_state(tsk, cpuacct_subsys_id),
-			    struct cpuacct, css);
-}
-
-static inline struct cpuacct *parent_ca(struct cpuacct *ca)
-{
-	if (!ca || !ca->css.cgroup->parent)
-		return NULL;
-	return cgroup_ca(ca->css.cgroup->parent);
-}
-
-extern void cpuacct_charge(struct task_struct *tsk, u64 cputime);
-#else
-static inline void cpuacct_charge(struct task_struct *tsk, u64 cputime) {}
-#endif
-
 #ifdef CONFIG_PARAVIRT
 static inline u64 steal_ticks(u64 steal)
 {
@@ -1187,7 +1303,6 @@ extern void account_cfs_bandwidth_used(int enabled, int was_enabled);
 enum rq_nohz_flag_bits {
 	NOHZ_TICK_STOPPED,
 	NOHZ_BALANCE_KICK,
-	NOHZ_IDLE,
 };
 
 #define nohz_flags(cpu)	(&cpu_rq(cpu)->nohz_flags)
diff --git a/kernel/signal.c b/kernel/signal.c
index dd72567767d..598dc06be42 100644
--- a/kernel/signal.c
+++ b/kernel/signal.c
@@ -2948,7 +2948,7 @@ do_send_specific(pid_t tgid, pid_t pid, int sig, struct siginfo *info)
 
 static int do_tkill(pid_t tgid, pid_t pid, int sig)
 {
-	struct siginfo info;
+	struct siginfo info = {};
 
 	info.si_signo = sig;
 	info.si_errno = 0;
diff --git a/kernel/smpboot.c b/kernel/smpboot.c
index 8eaed9aa9cf..02fc5c93367 100644
--- a/kernel/smpboot.c
+++ b/kernel/smpboot.c
@@ -185,8 +185,18 @@ __smpboot_create_thread(struct smp_hotplug_thread *ht, unsigned int cpu)
 	}
 	get_task_struct(tsk);
 	*per_cpu_ptr(ht->store, cpu) = tsk;
-	if (ht->create)
-		ht->create(cpu);
+	if (ht->create) {
+		/*
+		 * Make sure that the task has actually scheduled out
+		 * into park position, before calling the create
+		 * callback. At least the migration thread callback
+		 * requires that the task is off the runqueue.
+		 */
+		if (!wait_task_inactive(tsk, TASK_PARKED))
+			WARN_ON(1);
+		else
+			ht->create(cpu);
+	}
 	return 0;
 }
 
diff --git a/kernel/sys.c b/kernel/sys.c
index 39c9c4a2949..0da73cf73e6 100644
--- a/kernel/sys.c
+++ b/kernel/sys.c
@@ -324,7 +324,6 @@ void kernel_restart_prepare(char *cmd)
 	system_state = SYSTEM_RESTART;
 	usermodehelper_disable();
 	device_shutdown();
-	syscore_shutdown();
 }
 
 /**
@@ -370,6 +369,7 @@ void kernel_restart(char *cmd)
 {
 	kernel_restart_prepare(cmd);
 	disable_nonboot_cpus();
+	syscore_shutdown();
 	if (!cmd)
 		printk(KERN_EMERG "Restarting system.\n");
 	else
@@ -395,6 +395,7 @@ static void kernel_shutdown_prepare(enum system_states state)
 void kernel_halt(void)
 {
 	kernel_shutdown_prepare(SYSTEM_HALT);
+	disable_nonboot_cpus();
 	syscore_shutdown();
 	printk(KERN_EMERG "System halted.\n");
 	kmsg_dump(KMSG_DUMP_HALT);
diff --git a/kernel/sysctl.c b/kernel/sysctl.c
index afc1dc60f3f..9edcf456e0f 100644
--- a/kernel/sysctl.c
+++ b/kernel/sysctl.c
@@ -106,7 +106,6 @@ extern unsigned int core_pipe_limit;
 #endif
 extern int pid_max;
 extern int pid_max_min, pid_max_max;
-extern int sysctl_drop_caches;
 extern int percpu_pagelist_fraction;
 extern int compat_log;
 extern int latencytop_enabled;
@@ -1430,6 +1429,20 @@ static struct ctl_table vm_table[] = {
 		.extra2		= &one,
 	},
 #endif
+	{
+		.procname	= "user_reserve_kbytes",
+		.data		= &sysctl_user_reserve_kbytes,
+		.maxlen		= sizeof(sysctl_user_reserve_kbytes),
+		.mode		= 0644,
+		.proc_handler	= proc_doulongvec_minmax,
+	},
+	{
+		.procname	= "admin_reserve_kbytes",
+		.data		= &sysctl_admin_reserve_kbytes,
+		.maxlen		= sizeof(sysctl_admin_reserve_kbytes),
+		.mode		= 0644,
+		.proc_handler	= proc_doulongvec_minmax,
+	},
 	{ }
 };
 
diff --git a/kernel/test_kprobes.c b/kernel/test_kprobes.c
index f8b11a28317..12d6ebbfdd8 100644
--- a/kernel/test_kprobes.c
+++ b/kernel/test_kprobes.c
@@ -365,7 +365,7 @@ int init_test_probes(void)
 	target2 = kprobe_target2;
 
 	do {
-		rand1 = random32();
+		rand1 = prandom_u32();
 	} while (rand1 <= div_factor);
 
 	printk(KERN_INFO "Kprobe smoke test started\n");
diff --git a/kernel/time.c b/kernel/time.c
index f8342a41efa..d3617dbd3dc 100644
--- a/kernel/time.c
+++ b/kernel/time.c
@@ -138,13 +138,14 @@ int persistent_clock_is_local;
  */
 static inline void warp_clock(void)
 {
-	struct timespec adjust;
+	if (sys_tz.tz_minuteswest != 0) {
+		struct timespec adjust;
 
-	adjust = current_kernel_time();
-	if (sys_tz.tz_minuteswest != 0)
 		persistent_clock_is_local = 1;
-	adjust.tv_sec += sys_tz.tz_minuteswest * 60;
-	do_settimeofday(&adjust);
+		adjust.tv_sec = sys_tz.tz_minuteswest * 60;
+		adjust.tv_nsec = 0;
+		timekeeping_inject_offset(&adjust);
+	}
 }
 
 /*
diff --git a/kernel/time/ntp.c b/kernel/time/ntp.c
index 072bb066bb7..12ff13a838c 100644
--- a/kernel/time/ntp.c
+++ b/kernel/time/ntp.c
@@ -18,13 +18,14 @@
 #include <linux/rtc.h>
 
 #include "tick-internal.h"
+#include "ntp_internal.h"
 
 /*
  * NTP timekeeping variables:
+ *
+ * Note: All of the NTP state is protected by the timekeeping locks.
  */
 
-DEFINE_RAW_SPINLOCK(ntp_lock);
-
 
 /* USER_HZ period (usecs): */
 unsigned long			tick_usec = TICK_USEC;
@@ -53,9 +54,6 @@ static int			time_state = TIME_OK;
 /* clock status bits:							*/
 static int			time_status = STA_UNSYNC;
 
-/* TAI offset (secs):							*/
-static long			time_tai;
-
 /* time adjustment (nsecs):						*/
 static s64			time_offset;
 
@@ -134,8 +132,6 @@ static inline void pps_reset_freq_interval(void)
 
 /**
  * pps_clear - Clears the PPS state variables
- *
- * Must be called while holding a write on the ntp_lock
  */
 static inline void pps_clear(void)
 {
@@ -150,8 +146,6 @@ static inline void pps_clear(void)
 /* Decrease pps_valid to indicate that another second has passed since
  * the last PPS signal. When it reaches 0, indicate that PPS signal is
  * missing.
- *
- * Must be called while holding a write on the ntp_lock
  */
 static inline void pps_dec_valid(void)
 {
@@ -346,10 +340,6 @@ static void ntp_update_offset(long offset)
  */
 void ntp_clear(void)
 {
-	unsigned long flags;
-
-	raw_spin_lock_irqsave(&ntp_lock, flags);
-
 	time_adjust	= 0;		/* stop active adjtime() */
 	time_status	|= STA_UNSYNC;
 	time_maxerror	= NTP_PHASE_LIMIT;
@@ -362,20 +352,12 @@ void ntp_clear(void)
 
 	/* Clear PPS state variables */
 	pps_clear();
-	raw_spin_unlock_irqrestore(&ntp_lock, flags);
-
 }
 
 
 u64 ntp_tick_length(void)
 {
-	unsigned long flags;
-	s64 ret;
-
-	raw_spin_lock_irqsave(&ntp_lock, flags);
-	ret = tick_length;
-	raw_spin_unlock_irqrestore(&ntp_lock, flags);
-	return ret;
+	return tick_length;
 }
 
 
@@ -393,9 +375,6 @@ int second_overflow(unsigned long secs)
 {
 	s64 delta;
 	int leap = 0;
-	unsigned long flags;
-
-	raw_spin_lock_irqsave(&ntp_lock, flags);
 
 	/*
 	 * Leap second processing. If in leap-insert state at the end of the
@@ -415,7 +394,6 @@ int second_overflow(unsigned long secs)
 		else if (secs % 86400 == 0) {
 			leap = -1;
 			time_state = TIME_OOP;
-			time_tai++;
 			printk(KERN_NOTICE
 				"Clock: inserting leap second 23:59:60 UTC\n");
 		}
@@ -425,7 +403,6 @@ int second_overflow(unsigned long secs)
 			time_state = TIME_OK;
 		else if ((secs + 1) % 86400 == 0) {
 			leap = 1;
-			time_tai--;
 			time_state = TIME_WAIT;
 			printk(KERN_NOTICE
 				"Clock: deleting leap second 23:59:59 UTC\n");
@@ -479,8 +456,6 @@ int second_overflow(unsigned long secs)
 	time_adjust = 0;
 
 out:
-	raw_spin_unlock_irqrestore(&ntp_lock, flags);
-
 	return leap;
 }
 
@@ -575,11 +550,10 @@ static inline void process_adj_status(struct timex *txc, struct timespec *ts)
 	time_status |= txc->status & ~STA_RONLY;
 }
 
-/*
- * 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)
+
+static inline void process_adjtimex_modes(struct timex *txc,
+						struct timespec *ts,
+						s32 *time_tai)
 {
 	if (txc->modes & ADJ_STATUS)
 		process_adj_status(txc, ts);
@@ -613,7 +587,7 @@ static inline void process_adjtimex_modes(struct timex *txc, struct timespec *ts
 	}
 
 	if (txc->modes & ADJ_TAI && txc->constant > 0)
-		time_tai = txc->constant;
+		*time_tai = txc->constant;
 
 	if (txc->modes & ADJ_OFFSET)
 		ntp_update_offset(txc->offset);
@@ -625,16 +599,13 @@ static inline void process_adjtimex_modes(struct timex *txc, struct timespec *ts
 		ntp_update_frequency();
 }
 
-/*
- * adjtimex mainly allows reading (and writing, if superuser) of
- * kernel time-keeping variables. used by xntpd.
+
+
+/**
+ * ntp_validate_timex - Ensures the timex is ok for use in do_adjtimex
  */
-int do_adjtimex(struct timex *txc)
+int ntp_validate_timex(struct timex *txc)
 {
-	struct timespec ts;
-	int result;
-
-	/* Validate the data before disabling interrupts */
 	if (txc->modes & ADJ_ADJTIME) {
 		/* singleshot must not be used with any other mode bits */
 		if (!(txc->modes & ADJ_OFFSET_SINGLESHOT))
@@ -646,7 +617,6 @@ int do_adjtimex(struct timex *txc)
 		/* In order to modify anything, you gotta be super-user! */
 		 if (txc->modes && !capable(CAP_SYS_TIME))
 			return -EPERM;
-
 		/*
 		 * if the quartz is off by more than 10% then
 		 * something is VERY wrong!
@@ -657,22 +627,20 @@ int do_adjtimex(struct timex *txc)
 			return -EINVAL;
 	}
 
-	if (txc->modes & ADJ_SETOFFSET) {
-		struct timespec delta;
-		delta.tv_sec  = txc->time.tv_sec;
-		delta.tv_nsec = txc->time.tv_usec;
-		if (!capable(CAP_SYS_TIME))
-			return -EPERM;
-		if (!(txc->modes & ADJ_NANO))
-			delta.tv_nsec *= 1000;
-		result = timekeeping_inject_offset(&delta);
-		if (result)
-			return result;
-	}
+	if ((txc->modes & ADJ_SETOFFSET) && (!capable(CAP_SYS_TIME)))
+		return -EPERM;
 
-	getnstimeofday(&ts);
+	return 0;
+}
 
-	raw_spin_lock_irq(&ntp_lock);
+
+/*
+ * adjtimex mainly allows reading (and writing, if superuser) of
+ * kernel time-keeping variables. used by xntpd.
+ */
+int __do_adjtimex(struct timex *txc, struct timespec *ts, s32 *time_tai)
+{
+	int result;
 
 	if (txc->modes & ADJ_ADJTIME) {
 		long save_adjust = time_adjust;
@@ -687,7 +655,7 @@ int do_adjtimex(struct timex *txc)
 
 		/* If there are input parameters, then process them: */
 		if (txc->modes)
-			process_adjtimex_modes(txc, &ts);
+			process_adjtimex_modes(txc, ts, time_tai);
 
 		txc->offset = shift_right(time_offset * NTP_INTERVAL_FREQ,
 				  NTP_SCALE_SHIFT);
@@ -709,15 +677,13 @@ int do_adjtimex(struct timex *txc)
 	txc->precision	   = 1;
 	txc->tolerance	   = MAXFREQ_SCALED / PPM_SCALE;
 	txc->tick	   = tick_usec;
-	txc->tai	   = time_tai;
+	txc->tai	   = *time_tai;
 
 	/* fill PPS status fields */
 	pps_fill_timex(txc);
 
-	raw_spin_unlock_irq(&ntp_lock);
-
-	txc->time.tv_sec = ts.tv_sec;
-	txc->time.tv_usec = ts.tv_nsec;
+	txc->time.tv_sec = ts->tv_sec;
+	txc->time.tv_usec = ts->tv_nsec;
 	if (!(time_status & STA_NANO))
 		txc->time.tv_usec /= NSEC_PER_USEC;
 
@@ -894,7 +860,7 @@ static void hardpps_update_phase(long error)
 }
 
 /*
- * hardpps() - discipline CPU clock oscillator to external PPS signal
+ * __hardpps() - discipline CPU clock oscillator to external PPS signal
  *
  * This routine is called at each PPS signal arrival in order to
  * discipline the CPU clock oscillator to the PPS signal. It takes two
@@ -905,15 +871,13 @@ static void hardpps_update_phase(long error)
  * This code is based on David Mills's reference nanokernel
  * implementation. It was mostly rewritten but keeps the same idea.
  */
-void hardpps(const struct timespec *phase_ts, const struct timespec *raw_ts)
+void __hardpps(const struct timespec *phase_ts, const struct timespec *raw_ts)
 {
 	struct pps_normtime pts_norm, freq_norm;
 	unsigned long flags;
 
 	pts_norm = pps_normalize_ts(*phase_ts);
 
-	raw_spin_lock_irqsave(&ntp_lock, flags);
-
 	/* clear the error bits, they will be set again if needed */
 	time_status &= ~(STA_PPSJITTER | STA_PPSWANDER | STA_PPSERROR);
 
@@ -925,7 +889,6 @@ void hardpps(const struct timespec *phase_ts, const struct timespec *raw_ts)
 	 * just start the frequency interval */
 	if (unlikely(pps_fbase.tv_sec == 0)) {
 		pps_fbase = *raw_ts;
-		raw_spin_unlock_irqrestore(&ntp_lock, flags);
 		return;
 	}
 
@@ -940,7 +903,6 @@ void hardpps(const struct timespec *phase_ts, const struct timespec *raw_ts)
 		time_status |= STA_PPSJITTER;
 		/* restart the frequency calibration interval */
 		pps_fbase = *raw_ts;
-		raw_spin_unlock_irqrestore(&ntp_lock, flags);
 		pr_err("hardpps: PPSJITTER: bad pulse\n");
 		return;
 	}
@@ -957,10 +919,7 @@ void hardpps(const struct timespec *phase_ts, const struct timespec *raw_ts)
 
 	hardpps_update_phase(pts_norm.nsec);
 
-	raw_spin_unlock_irqrestore(&ntp_lock, flags);
 }
-EXPORT_SYMBOL(hardpps);
-
 #endif	/* CONFIG_NTP_PPS */
 
 static int __init ntp_tick_adj_setup(char *str)
diff --git a/kernel/time/ntp_internal.h b/kernel/time/ntp_internal.h
new file mode 100644
index 00000000000..1950cb4ca2a
--- /dev/null
+++ b/kernel/time/ntp_internal.h
@@ -0,0 +1,12 @@
+#ifndef _LINUX_NTP_INTERNAL_H
+#define _LINUX_NTP_INTERNAL_H
+
+extern void ntp_init(void);
+extern void ntp_clear(void);
+/* Returns how long ticks are at present, in ns / 2^NTP_SCALE_SHIFT. */
+extern u64 ntp_tick_length(void);
+extern int second_overflow(unsigned long secs);
+extern int ntp_validate_timex(struct timex *);
+extern int __do_adjtimex(struct timex *, struct timespec *, s32 *);
+extern void __hardpps(const struct timespec *, const struct timespec *);
+#endif /* _LINUX_NTP_INTERNAL_H */
diff --git a/kernel/time/tick-broadcast.c b/kernel/time/tick-broadcast.c
index 2fb8cb88df8..61d00a8cdf2 100644
--- a/kernel/time/tick-broadcast.c
+++ b/kernel/time/tick-broadcast.c
@@ -28,9 +28,8 @@
  */
 
 static struct tick_device tick_broadcast_device;
-/* FIXME: Use cpumask_var_t. */
-static DECLARE_BITMAP(tick_broadcast_mask, NR_CPUS);
-static DECLARE_BITMAP(tmpmask, NR_CPUS);
+static cpumask_var_t tick_broadcast_mask;
+static cpumask_var_t tmpmask;
 static DEFINE_RAW_SPINLOCK(tick_broadcast_lock);
 static int tick_broadcast_force;
 
@@ -50,7 +49,7 @@ struct tick_device *tick_get_broadcast_device(void)
 
 struct cpumask *tick_get_broadcast_mask(void)
 {
-	return to_cpumask(tick_broadcast_mask);
+	return tick_broadcast_mask;
 }
 
 /*
@@ -67,15 +66,30 @@ static void tick_broadcast_start_periodic(struct clock_event_device *bc)
  */
 int tick_check_broadcast_device(struct clock_event_device *dev)
 {
-	if ((tick_broadcast_device.evtdev &&
+	struct clock_event_device *cur = tick_broadcast_device.evtdev;
+
+	if ((dev->features & CLOCK_EVT_FEAT_DUMMY) ||
+	    (tick_broadcast_device.evtdev &&
 	     tick_broadcast_device.evtdev->rating >= dev->rating) ||
 	     (dev->features & CLOCK_EVT_FEAT_C3STOP))
 		return 0;
 
 	clockevents_exchange_device(tick_broadcast_device.evtdev, dev);
+	if (cur)
+		cur->event_handler = clockevents_handle_noop;
 	tick_broadcast_device.evtdev = dev;
-	if (!cpumask_empty(tick_get_broadcast_mask()))
+	if (!cpumask_empty(tick_broadcast_mask))
 		tick_broadcast_start_periodic(dev);
+	/*
+	 * Inform all cpus about this. We might be in a situation
+	 * where we did not switch to oneshot mode because the per cpu
+	 * devices are affected by CLOCK_EVT_FEAT_C3STOP and the lack
+	 * of a oneshot capable broadcast device. Without that
+	 * notification the systems stays stuck in periodic mode
+	 * forever.
+	 */
+	if (dev->features & CLOCK_EVT_FEAT_ONESHOT)
+		tick_clock_notify();
 	return 1;
 }
 
@@ -123,7 +137,7 @@ int tick_device_uses_broadcast(struct clock_event_device *dev, int cpu)
 	if (!tick_device_is_functional(dev)) {
 		dev->event_handler = tick_handle_periodic;
 		tick_device_setup_broadcast_func(dev);
-		cpumask_set_cpu(cpu, tick_get_broadcast_mask());
+		cpumask_set_cpu(cpu, tick_broadcast_mask);
 		tick_broadcast_start_periodic(tick_broadcast_device.evtdev);
 		ret = 1;
 	} else {
@@ -134,7 +148,7 @@ int tick_device_uses_broadcast(struct clock_event_device *dev, int cpu)
 		 */
 		if (!(dev->features & CLOCK_EVT_FEAT_C3STOP)) {
 			int cpu = smp_processor_id();
-			cpumask_clear_cpu(cpu, tick_get_broadcast_mask());
+			cpumask_clear_cpu(cpu, tick_broadcast_mask);
 			tick_broadcast_clear_oneshot(cpu);
 		} else {
 			tick_device_setup_broadcast_func(dev);
@@ -198,9 +212,8 @@ static void tick_do_periodic_broadcast(void)
 {
 	raw_spin_lock(&tick_broadcast_lock);
 
-	cpumask_and(to_cpumask(tmpmask),
-		    cpu_online_mask, tick_get_broadcast_mask());
-	tick_do_broadcast(to_cpumask(tmpmask));
+	cpumask_and(tmpmask, cpu_online_mask, tick_broadcast_mask);
+	tick_do_broadcast(tmpmask);
 
 	raw_spin_unlock(&tick_broadcast_lock);
 }
@@ -263,13 +276,12 @@ static void tick_do_broadcast_on_off(unsigned long *reason)
 	if (!tick_device_is_functional(dev))
 		goto out;
 
-	bc_stopped = cpumask_empty(tick_get_broadcast_mask());
+	bc_stopped = cpumask_empty(tick_broadcast_mask);
 
 	switch (*reason) {
 	case CLOCK_EVT_NOTIFY_BROADCAST_ON:
 	case CLOCK_EVT_NOTIFY_BROADCAST_FORCE:
-		if (!cpumask_test_cpu(cpu, tick_get_broadcast_mask())) {
-			cpumask_set_cpu(cpu, tick_get_broadcast_mask());
+		if (!cpumask_test_and_set_cpu(cpu, tick_broadcast_mask)) {
 			if (tick_broadcast_device.mode ==
 			    TICKDEV_MODE_PERIODIC)
 				clockevents_shutdown(dev);
@@ -279,8 +291,7 @@ static void tick_do_broadcast_on_off(unsigned long *reason)
 		break;
 	case CLOCK_EVT_NOTIFY_BROADCAST_OFF:
 		if (!tick_broadcast_force &&
-		    cpumask_test_cpu(cpu, tick_get_broadcast_mask())) {
-			cpumask_clear_cpu(cpu, tick_get_broadcast_mask());
+		    cpumask_test_and_clear_cpu(cpu, tick_broadcast_mask)) {
 			if (tick_broadcast_device.mode ==
 			    TICKDEV_MODE_PERIODIC)
 				tick_setup_periodic(dev, 0);
@@ -288,7 +299,7 @@ static void tick_do_broadcast_on_off(unsigned long *reason)
 		break;
 	}
 
-	if (cpumask_empty(tick_get_broadcast_mask())) {
+	if (cpumask_empty(tick_broadcast_mask)) {
 		if (!bc_stopped)
 			clockevents_shutdown(bc);
 	} else if (bc_stopped) {
@@ -337,10 +348,10 @@ void tick_shutdown_broadcast(unsigned int *cpup)
 	raw_spin_lock_irqsave(&tick_broadcast_lock, flags);
 
 	bc = tick_broadcast_device.evtdev;
-	cpumask_clear_cpu(cpu, tick_get_broadcast_mask());
+	cpumask_clear_cpu(cpu, tick_broadcast_mask);
 
 	if (tick_broadcast_device.mode == TICKDEV_MODE_PERIODIC) {
-		if (bc && cpumask_empty(tick_get_broadcast_mask()))
+		if (bc && cpumask_empty(tick_broadcast_mask))
 			clockevents_shutdown(bc);
 	}
 
@@ -376,13 +387,13 @@ int tick_resume_broadcast(void)
 
 		switch (tick_broadcast_device.mode) {
 		case TICKDEV_MODE_PERIODIC:
-			if (!cpumask_empty(tick_get_broadcast_mask()))
+			if (!cpumask_empty(tick_broadcast_mask))
 				tick_broadcast_start_periodic(bc);
 			broadcast = cpumask_test_cpu(smp_processor_id(),
-						     tick_get_broadcast_mask());
+						     tick_broadcast_mask);
 			break;
 		case TICKDEV_MODE_ONESHOT:
-			if (!cpumask_empty(tick_get_broadcast_mask()))
+			if (!cpumask_empty(tick_broadcast_mask))
 				broadcast = tick_resume_broadcast_oneshot(bc);
 			break;
 		}
@@ -395,25 +406,58 @@ int tick_resume_broadcast(void)
 
 #ifdef CONFIG_TICK_ONESHOT
 
-/* FIXME: use cpumask_var_t. */
-static DECLARE_BITMAP(tick_broadcast_oneshot_mask, NR_CPUS);
+static cpumask_var_t tick_broadcast_oneshot_mask;
+static cpumask_var_t tick_broadcast_pending_mask;
+static cpumask_var_t tick_broadcast_force_mask;
 
 /*
  * Exposed for debugging: see timer_list.c
  */
 struct cpumask *tick_get_broadcast_oneshot_mask(void)
 {
-	return to_cpumask(tick_broadcast_oneshot_mask);
+	return tick_broadcast_oneshot_mask;
 }
 
-static int tick_broadcast_set_event(ktime_t expires, int force)
+/*
+ * Called before going idle with interrupts disabled. Checks whether a
+ * broadcast event from the other core is about to happen. We detected
+ * that in tick_broadcast_oneshot_control(). The callsite can use this
+ * to avoid a deep idle transition as we are about to get the
+ * broadcast IPI right away.
+ */
+int tick_check_broadcast_expired(void)
 {
-	struct clock_event_device *bc = tick_broadcast_device.evtdev;
+	return cpumask_test_cpu(smp_processor_id(), tick_broadcast_force_mask);
+}
+
+/*
+ * Set broadcast interrupt affinity
+ */
+static void tick_broadcast_set_affinity(struct clock_event_device *bc,
+					const struct cpumask *cpumask)
+{
+	if (!(bc->features & CLOCK_EVT_FEAT_DYNIRQ))
+		return;
+
+	if (cpumask_equal(bc->cpumask, cpumask))
+		return;
+
+	bc->cpumask = cpumask;
+	irq_set_affinity(bc->irq, bc->cpumask);
+}
+
+static int tick_broadcast_set_event(struct clock_event_device *bc, int cpu,
+				    ktime_t expires, int force)
+{
+	int ret;
 
 	if (bc->mode != CLOCK_EVT_MODE_ONESHOT)
 		clockevents_set_mode(bc, CLOCK_EVT_MODE_ONESHOT);
 
-	return clockevents_program_event(bc, expires, force);
+	ret = clockevents_program_event(bc, expires, force);
+	if (!ret)
+		tick_broadcast_set_affinity(bc, cpumask_of(cpu));
+	return ret;
 }
 
 int tick_resume_broadcast_oneshot(struct clock_event_device *bc)
@@ -428,7 +472,7 @@ int tick_resume_broadcast_oneshot(struct clock_event_device *bc)
  */
 void tick_check_oneshot_broadcast(int cpu)
 {
-	if (cpumask_test_cpu(cpu, to_cpumask(tick_broadcast_oneshot_mask))) {
+	if (cpumask_test_cpu(cpu, tick_broadcast_oneshot_mask)) {
 		struct tick_device *td = &per_cpu(tick_cpu_device, cpu);
 
 		clockevents_set_mode(td->evtdev, CLOCK_EVT_MODE_ONESHOT);
@@ -442,27 +486,39 @@ static void tick_handle_oneshot_broadcast(struct clock_event_device *dev)
 {
 	struct tick_device *td;
 	ktime_t now, next_event;
-	int cpu;
+	int cpu, next_cpu = 0;
 
 	raw_spin_lock(&tick_broadcast_lock);
 again:
 	dev->next_event.tv64 = KTIME_MAX;
 	next_event.tv64 = KTIME_MAX;
-	cpumask_clear(to_cpumask(tmpmask));
+	cpumask_clear(tmpmask);
 	now = ktime_get();
 	/* Find all expired events */
-	for_each_cpu(cpu, tick_get_broadcast_oneshot_mask()) {
+	for_each_cpu(cpu, tick_broadcast_oneshot_mask) {
 		td = &per_cpu(tick_cpu_device, cpu);
-		if (td->evtdev->next_event.tv64 <= now.tv64)
-			cpumask_set_cpu(cpu, to_cpumask(tmpmask));
-		else if (td->evtdev->next_event.tv64 < next_event.tv64)
+		if (td->evtdev->next_event.tv64 <= now.tv64) {
+			cpumask_set_cpu(cpu, tmpmask);
+			/*
+			 * Mark the remote cpu in the pending mask, so
+			 * it can avoid reprogramming the cpu local
+			 * timer in tick_broadcast_oneshot_control().
+			 */
+			cpumask_set_cpu(cpu, tick_broadcast_pending_mask);
+		} else if (td->evtdev->next_event.tv64 < next_event.tv64) {
 			next_event.tv64 = td->evtdev->next_event.tv64;
+			next_cpu = cpu;
+		}
 	}
 
+	/* Take care of enforced broadcast requests */
+	cpumask_or(tmpmask, tmpmask, tick_broadcast_force_mask);
+	cpumask_clear(tick_broadcast_force_mask);
+
 	/*
 	 * Wakeup the cpus which have an expired event.
 	 */
-	tick_do_broadcast(to_cpumask(tmpmask));
+	tick_do_broadcast(tmpmask);
 
 	/*
 	 * Two reasons for reprogram:
@@ -479,7 +535,7 @@ again:
 		 * Rearm the broadcast device. If event expired,
 		 * repeat the above
 		 */
-		if (tick_broadcast_set_event(next_event, 0))
+		if (tick_broadcast_set_event(dev, next_cpu, next_event, 0))
 			goto again;
 	}
 	raw_spin_unlock(&tick_broadcast_lock);
@@ -494,6 +550,7 @@ void tick_broadcast_oneshot_control(unsigned long reason)
 	struct clock_event_device *bc, *dev;
 	struct tick_device *td;
 	unsigned long flags;
+	ktime_t now;
 	int cpu;
 
 	/*
@@ -518,21 +575,84 @@ void tick_broadcast_oneshot_control(unsigned long reason)
 
 	raw_spin_lock_irqsave(&tick_broadcast_lock, flags);
 	if (reason == CLOCK_EVT_NOTIFY_BROADCAST_ENTER) {
-		if (!cpumask_test_cpu(cpu, tick_get_broadcast_oneshot_mask())) {
-			cpumask_set_cpu(cpu, tick_get_broadcast_oneshot_mask());
+		WARN_ON_ONCE(cpumask_test_cpu(cpu, tick_broadcast_pending_mask));
+		if (!cpumask_test_and_set_cpu(cpu, tick_broadcast_oneshot_mask)) {
 			clockevents_set_mode(dev, CLOCK_EVT_MODE_SHUTDOWN);
-			if (dev->next_event.tv64 < bc->next_event.tv64)
-				tick_broadcast_set_event(dev->next_event, 1);
+			/*
+			 * We only reprogram the broadcast timer if we
+			 * did not mark ourself in the force mask and
+			 * if the cpu local event is earlier than the
+			 * broadcast event. If the current CPU is in
+			 * the force mask, then we are going to be
+			 * woken by the IPI right away.
+			 */
+			if (!cpumask_test_cpu(cpu, tick_broadcast_force_mask) &&
+			    dev->next_event.tv64 < bc->next_event.tv64)
+				tick_broadcast_set_event(bc, cpu, dev->next_event, 1);
 		}
 	} else {
-		if (cpumask_test_cpu(cpu, tick_get_broadcast_oneshot_mask())) {
-			cpumask_clear_cpu(cpu,
-					  tick_get_broadcast_oneshot_mask());
+		if (cpumask_test_and_clear_cpu(cpu, tick_broadcast_oneshot_mask)) {
 			clockevents_set_mode(dev, CLOCK_EVT_MODE_ONESHOT);
-			if (dev->next_event.tv64 != KTIME_MAX)
-				tick_program_event(dev->next_event, 1);
+			if (dev->next_event.tv64 == KTIME_MAX)
+				goto out;
+			/*
+			 * The cpu which was handling the broadcast
+			 * timer marked this cpu in the broadcast
+			 * pending mask and fired the broadcast
+			 * IPI. So we are going to handle the expired
+			 * event anyway via the broadcast IPI
+			 * handler. No need to reprogram the timer
+			 * with an already expired event.
+			 */
+			if (cpumask_test_and_clear_cpu(cpu,
+				       tick_broadcast_pending_mask))
+				goto out;
+
+			/*
+			 * If the pending bit is not set, then we are
+			 * either the CPU handling the broadcast
+			 * interrupt or we got woken by something else.
+			 *
+			 * We are not longer in the broadcast mask, so
+			 * if the cpu local expiry time is already
+			 * reached, we would reprogram the cpu local
+			 * timer with an already expired event.
+			 *
+			 * This can lead to a ping-pong when we return
+			 * to idle and therefor rearm the broadcast
+			 * timer before the cpu local timer was able
+			 * to fire. This happens because the forced
+			 * reprogramming makes sure that the event
+			 * will happen in the future and depending on
+			 * the min_delta setting this might be far
+			 * enough out that the ping-pong starts.
+			 *
+			 * If the cpu local next_event has expired
+			 * then we know that the broadcast timer
+			 * next_event has expired as well and
+			 * broadcast is about to be handled. So we
+			 * avoid reprogramming and enforce that the
+			 * broadcast handler, which did not run yet,
+			 * will invoke the cpu local handler.
+			 *
+			 * We cannot call the handler directly from
+			 * here, because we might be in a NOHZ phase
+			 * and we did not go through the irq_enter()
+			 * nohz fixups.
+			 */
+			now = ktime_get();
+			if (dev->next_event.tv64 <= now.tv64) {
+				cpumask_set_cpu(cpu, tick_broadcast_force_mask);
+				goto out;
+			}
+			/*
+			 * We got woken by something else. Reprogram
+			 * the cpu local timer device.
+			 */
+			tick_program_event(dev->next_event, 1);
 		}
 	}
+out:
 	raw_spin_unlock_irqrestore(&tick_broadcast_lock, flags);
 }
 
@@ -543,7 +663,7 @@ void tick_broadcast_oneshot_control(unsigned long reason)
  */
 static void tick_broadcast_clear_oneshot(int cpu)
 {
-	cpumask_clear_cpu(cpu, tick_get_broadcast_oneshot_mask());
+	cpumask_clear_cpu(cpu, tick_broadcast_oneshot_mask);
 }
 
 static void tick_broadcast_init_next_event(struct cpumask *mask,
@@ -581,17 +701,16 @@ void tick_broadcast_setup_oneshot(struct clock_event_device *bc)
 		 * oneshot_mask bits for those and program the
 		 * broadcast device to fire.
 		 */
-		cpumask_copy(to_cpumask(tmpmask), tick_get_broadcast_mask());
-		cpumask_clear_cpu(cpu, to_cpumask(tmpmask));
-		cpumask_or(tick_get_broadcast_oneshot_mask(),
-			   tick_get_broadcast_oneshot_mask(),
-			   to_cpumask(tmpmask));
+		cpumask_copy(tmpmask, tick_broadcast_mask);
+		cpumask_clear_cpu(cpu, tmpmask);
+		cpumask_or(tick_broadcast_oneshot_mask,
+			   tick_broadcast_oneshot_mask, tmpmask);
 
-		if (was_periodic && !cpumask_empty(to_cpumask(tmpmask))) {
+		if (was_periodic && !cpumask_empty(tmpmask)) {
 			clockevents_set_mode(bc, CLOCK_EVT_MODE_ONESHOT);
-			tick_broadcast_init_next_event(to_cpumask(tmpmask),
+			tick_broadcast_init_next_event(tmpmask,
 						       tick_next_period);
-			tick_broadcast_set_event(tick_next_period, 1);
+			tick_broadcast_set_event(bc, cpu, tick_next_period, 1);
 		} else
 			bc->next_event.tv64 = KTIME_MAX;
 	} else {
@@ -639,7 +758,7 @@ void tick_shutdown_broadcast_oneshot(unsigned int *cpup)
 	 * Clear the broadcast mask flag for the dead cpu, but do not
 	 * stop the broadcast device!
 	 */
-	cpumask_clear_cpu(cpu, tick_get_broadcast_oneshot_mask());
+	cpumask_clear_cpu(cpu, tick_broadcast_oneshot_mask);
 
 	raw_spin_unlock_irqrestore(&tick_broadcast_lock, flags);
 }
@@ -663,3 +782,14 @@ bool tick_broadcast_oneshot_available(void)
 }
 
 #endif
+
+void __init tick_broadcast_init(void)
+{
+	alloc_cpumask_var(&tick_broadcast_mask, GFP_NOWAIT);
+	alloc_cpumask_var(&tmpmask, GFP_NOWAIT);
+#ifdef CONFIG_TICK_ONESHOT
+	alloc_cpumask_var(&tick_broadcast_oneshot_mask, GFP_NOWAIT);
+	alloc_cpumask_var(&tick_broadcast_pending_mask, GFP_NOWAIT);
+	alloc_cpumask_var(&tick_broadcast_force_mask, GFP_NOWAIT);
+#endif
+}
diff --git a/kernel/time/tick-common.c b/kernel/time/tick-common.c
index b1600a6973f..6176a3e4570 100644
--- a/kernel/time/tick-common.c
+++ b/kernel/time/tick-common.c
@@ -323,6 +323,7 @@ static void tick_shutdown(unsigned int *cpup)
 		 */
 		dev->mode = CLOCK_EVT_MODE_UNUSED;
 		clockevents_exchange_device(dev, NULL);
+		dev->event_handler = clockevents_handle_noop;
 		td->evtdev = NULL;
 	}
 	raw_spin_unlock_irqrestore(&tick_device_lock, flags);
@@ -416,4 +417,5 @@ static struct notifier_block tick_notifier = {
 void __init tick_init(void)
 {
 	clockevents_register_notifier(&tick_notifier);
+	tick_broadcast_init();
 }
diff --git a/kernel/time/tick-internal.h b/kernel/time/tick-internal.h
index cf3e59ed6dc..f0299eae460 100644
--- a/kernel/time/tick-internal.h
+++ b/kernel/time/tick-internal.h
@@ -4,6 +4,8 @@
 #include <linux/hrtimer.h>
 #include <linux/tick.h>
 
+extern seqlock_t jiffies_lock;
+
 #ifdef CONFIG_GENERIC_CLOCKEVENTS_BUILD
 
 #define TICK_DO_TIMER_NONE	-1
@@ -94,7 +96,7 @@ extern void tick_broadcast_on_off(unsigned long reason, int *oncpu);
 extern void tick_shutdown_broadcast(unsigned int *cpup);
 extern void tick_suspend_broadcast(void);
 extern int tick_resume_broadcast(void);
-
+extern void tick_broadcast_init(void);
 extern void
 tick_set_periodic_handler(struct clock_event_device *dev, int broadcast);
 
@@ -119,6 +121,7 @@ static inline void tick_broadcast_on_off(unsigned long reason, int *oncpu) { }
 static inline void tick_shutdown_broadcast(unsigned int *cpup) { }
 static inline void tick_suspend_broadcast(void) { }
 static inline int tick_resume_broadcast(void) { return 0; }
+static inline void tick_broadcast_init(void) { }
 
 /*
  * Set the periodic handler in non broadcast mode
diff --git a/kernel/time/tick-sched.c b/kernel/time/tick-sched.c
index a19a39952c1..225f8bf1909 100644
--- a/kernel/time/tick-sched.c
+++ b/kernel/time/tick-sched.c
@@ -482,8 +482,8 @@ static bool can_stop_idle_tick(int cpu, struct tick_sched *ts)
 
 		if (ratelimit < 10 &&
 		    (local_softirq_pending() & SOFTIRQ_STOP_IDLE_MASK)) {
-			printk(KERN_ERR "NOHZ: local_softirq_pending %02x\n",
-			       (unsigned int) local_softirq_pending());
+			pr_warn("NOHZ: local_softirq_pending %02x\n",
+				(unsigned int) local_softirq_pending());
 			ratelimit++;
 		}
 		return false;
diff --git a/kernel/time/timekeeping.c b/kernel/time/timekeeping.c
index 9a0bc98fbe1..98cd470bbe4 100644
--- a/kernel/time/timekeeping.c
+++ b/kernel/time/timekeeping.c
@@ -23,8 +23,13 @@
 #include <linux/stop_machine.h>
 #include <linux/pvclock_gtod.h>
 
+#include "tick-internal.h"
+#include "ntp_internal.h"
 
 static struct timekeeper timekeeper;
+static DEFINE_RAW_SPINLOCK(timekeeper_lock);
+static seqcount_t timekeeper_seq;
+static struct timekeeper shadow_timekeeper;
 
 /* flag for if timekeeping is suspended */
 int __read_mostly timekeeping_suspended;
@@ -67,6 +72,7 @@ static void tk_set_wall_to_mono(struct timekeeper *tk, struct timespec wtm)
 	tk->wall_to_monotonic = wtm;
 	set_normalized_timespec(&tmp, -wtm.tv_sec, -wtm.tv_nsec);
 	tk->offs_real = timespec_to_ktime(tmp);
+	tk->offs_tai = ktime_sub(tk->offs_real, ktime_set(tk->tai_offset, 0));
 }
 
 static void tk_set_sleep_time(struct timekeeper *tk, struct timespec t)
@@ -96,7 +102,7 @@ static void tk_setup_internals(struct timekeeper *tk, struct clocksource *clock)
 
 	old_clock = tk->clock;
 	tk->clock = clock;
-	clock->cycle_last = clock->read(clock);
+	tk->cycle_last = clock->cycle_last = clock->read(clock);
 
 	/* Do the ns -> cycle conversion first, using original mult */
 	tmp = NTP_INTERVAL_LENGTH;
@@ -201,8 +207,6 @@ static void update_pvclock_gtod(struct timekeeper *tk)
 
 /**
  * pvclock_gtod_register_notifier - register a pvclock timedata update listener
- *
- * Must hold write on timekeeper.lock
  */
 int pvclock_gtod_register_notifier(struct notifier_block *nb)
 {
@@ -210,11 +214,10 @@ int pvclock_gtod_register_notifier(struct notifier_block *nb)
 	unsigned long flags;
 	int ret;
 
-	write_seqlock_irqsave(&tk->lock, flags);
+	raw_spin_lock_irqsave(&timekeeper_lock, flags);
 	ret = raw_notifier_chain_register(&pvclock_gtod_chain, nb);
-	/* update timekeeping data */
 	update_pvclock_gtod(tk);
-	write_sequnlock_irqrestore(&tk->lock, flags);
+	raw_spin_unlock_irqrestore(&timekeeper_lock, flags);
 
 	return ret;
 }
@@ -223,25 +226,22 @@ EXPORT_SYMBOL_GPL(pvclock_gtod_register_notifier);
 /**
  * pvclock_gtod_unregister_notifier - unregister a pvclock
  * timedata update listener
- *
- * Must hold write on timekeeper.lock
  */
 int pvclock_gtod_unregister_notifier(struct notifier_block *nb)
 {
-	struct timekeeper *tk = &timekeeper;
 	unsigned long flags;
 	int ret;
 
-	write_seqlock_irqsave(&tk->lock, flags);
+	raw_spin_lock_irqsave(&timekeeper_lock, flags);
 	ret = raw_notifier_chain_unregister(&pvclock_gtod_chain, nb);
-	write_sequnlock_irqrestore(&tk->lock, flags);
+	raw_spin_unlock_irqrestore(&timekeeper_lock, flags);
 
 	return ret;
 }
 EXPORT_SYMBOL_GPL(pvclock_gtod_unregister_notifier);
 
-/* must hold write on timekeeper.lock */
-static void timekeeping_update(struct timekeeper *tk, bool clearntp)
+/* must hold timekeeper_lock */
+static void timekeeping_update(struct timekeeper *tk, bool clearntp, bool mirror)
 {
 	if (clearntp) {
 		tk->ntp_error = 0;
@@ -249,6 +249,9 @@ static void timekeeping_update(struct timekeeper *tk, bool clearntp)
 	}
 	update_vsyscall(tk);
 	update_pvclock_gtod(tk);
+
+	if (mirror)
+		memcpy(&shadow_timekeeper, &timekeeper, sizeof(timekeeper));
 }
 
 /**
@@ -267,7 +270,7 @@ static void timekeeping_forward_now(struct timekeeper *tk)
 	clock = tk->clock;
 	cycle_now = clock->read(clock);
 	cycle_delta = (cycle_now - clock->cycle_last) & clock->mask;
-	clock->cycle_last = cycle_now;
+	tk->cycle_last = clock->cycle_last = cycle_now;
 
 	tk->xtime_nsec += cycle_delta * tk->mult;
 
@@ -294,12 +297,12 @@ int __getnstimeofday(struct timespec *ts)
 	s64 nsecs = 0;
 
 	do {
-		seq = read_seqbegin(&tk->lock);
+		seq = read_seqcount_begin(&timekeeper_seq);
 
 		ts->tv_sec = tk->xtime_sec;
 		nsecs = timekeeping_get_ns(tk);
 
-	} while (read_seqretry(&tk->lock, seq));
+	} while (read_seqcount_retry(&timekeeper_seq, seq));
 
 	ts->tv_nsec = 0;
 	timespec_add_ns(ts, nsecs);
@@ -335,11 +338,11 @@ ktime_t ktime_get(void)
 	WARN_ON(timekeeping_suspended);
 
 	do {
-		seq = read_seqbegin(&tk->lock);
+		seq = read_seqcount_begin(&timekeeper_seq);
 		secs = tk->xtime_sec + tk->wall_to_monotonic.tv_sec;
 		nsecs = timekeeping_get_ns(tk) + tk->wall_to_monotonic.tv_nsec;
 
-	} while (read_seqretry(&tk->lock, seq));
+	} while (read_seqcount_retry(&timekeeper_seq, seq));
 	/*
 	 * Use ktime_set/ktime_add_ns to create a proper ktime on
 	 * 32-bit architectures without CONFIG_KTIME_SCALAR.
@@ -366,12 +369,12 @@ void ktime_get_ts(struct timespec *ts)
 	WARN_ON(timekeeping_suspended);
 
 	do {
-		seq = read_seqbegin(&tk->lock);
+		seq = read_seqcount_begin(&timekeeper_seq);
 		ts->tv_sec = tk->xtime_sec;
 		nsec = timekeeping_get_ns(tk);
 		tomono = tk->wall_to_monotonic;
 
-	} while (read_seqretry(&tk->lock, seq));
+	} while (read_seqcount_retry(&timekeeper_seq, seq));
 
 	ts->tv_sec += tomono.tv_sec;
 	ts->tv_nsec = 0;
@@ -379,6 +382,50 @@ void ktime_get_ts(struct timespec *ts)
 }
 EXPORT_SYMBOL_GPL(ktime_get_ts);
 
+
+/**
+ * timekeeping_clocktai - Returns the TAI time of day in a timespec
+ * @ts:		pointer to the timespec to be set
+ *
+ * Returns the time of day in a timespec.
+ */
+void timekeeping_clocktai(struct timespec *ts)
+{
+	struct timekeeper *tk = &timekeeper;
+	unsigned long seq;
+	u64 nsecs;
+
+	WARN_ON(timekeeping_suspended);
+
+	do {
+		seq = read_seqcount_begin(&timekeeper_seq);
+
+		ts->tv_sec = tk->xtime_sec + tk->tai_offset;
+		nsecs = timekeeping_get_ns(tk);
+
+	} while (read_seqcount_retry(&timekeeper_seq, seq));
+
+	ts->tv_nsec = 0;
+	timespec_add_ns(ts, nsecs);
+
+}
+EXPORT_SYMBOL(timekeeping_clocktai);
+
+
+/**
+ * ktime_get_clocktai - Returns the TAI time of day in a ktime
+ *
+ * Returns the time of day in a ktime.
+ */
+ktime_t ktime_get_clocktai(void)
+{
+	struct timespec ts;
+
+	timekeeping_clocktai(&ts);
+	return timespec_to_ktime(ts);
+}
+EXPORT_SYMBOL(ktime_get_clocktai);
+
 #ifdef CONFIG_NTP_PPS
 
 /**
@@ -399,7 +446,7 @@ void getnstime_raw_and_real(struct timespec *ts_raw, struct timespec *ts_real)
 	WARN_ON_ONCE(timekeeping_suspended);
 
 	do {
-		seq = read_seqbegin(&tk->lock);
+		seq = read_seqcount_begin(&timekeeper_seq);
 
 		*ts_raw = tk->raw_time;
 		ts_real->tv_sec = tk->xtime_sec;
@@ -408,7 +455,7 @@ void getnstime_raw_and_real(struct timespec *ts_raw, struct timespec *ts_real)
 		nsecs_raw = timekeeping_get_ns_raw(tk);
 		nsecs_real = timekeeping_get_ns(tk);
 
-	} while (read_seqretry(&tk->lock, seq));
+	} while (read_seqcount_retry(&timekeeper_seq, seq));
 
 	timespec_add_ns(ts_raw, nsecs_raw);
 	timespec_add_ns(ts_real, nsecs_real);
@@ -448,7 +495,8 @@ int do_settimeofday(const struct timespec *tv)
 	if (!timespec_valid_strict(tv))
 		return -EINVAL;
 
-	write_seqlock_irqsave(&tk->lock, flags);
+	raw_spin_lock_irqsave(&timekeeper_lock, flags);
+	write_seqcount_begin(&timekeeper_seq);
 
 	timekeeping_forward_now(tk);
 
@@ -460,9 +508,10 @@ int do_settimeofday(const struct timespec *tv)
 
 	tk_set_xtime(tk, tv);
 
-	timekeeping_update(tk, true);
+	timekeeping_update(tk, true, true);
 
-	write_sequnlock_irqrestore(&tk->lock, flags);
+	write_seqcount_end(&timekeeper_seq);
+	raw_spin_unlock_irqrestore(&timekeeper_lock, flags);
 
 	/* signal hrtimers about time change */
 	clock_was_set();
@@ -487,7 +536,8 @@ int timekeeping_inject_offset(struct timespec *ts)
 	if ((unsigned long)ts->tv_nsec >= NSEC_PER_SEC)
 		return -EINVAL;
 
-	write_seqlock_irqsave(&tk->lock, flags);
+	raw_spin_lock_irqsave(&timekeeper_lock, flags);
+	write_seqcount_begin(&timekeeper_seq);
 
 	timekeeping_forward_now(tk);
 
@@ -502,9 +552,10 @@ int timekeeping_inject_offset(struct timespec *ts)
 	tk_set_wall_to_mono(tk, timespec_sub(tk->wall_to_monotonic, *ts));
 
 error: /* even if we error out, we forwarded the time, so call update */
-	timekeeping_update(tk, true);
+	timekeeping_update(tk, true, true);
 
-	write_sequnlock_irqrestore(&tk->lock, flags);
+	write_seqcount_end(&timekeeper_seq);
+	raw_spin_unlock_irqrestore(&timekeeper_lock, flags);
 
 	/* signal hrtimers about time change */
 	clock_was_set();
@@ -513,6 +564,52 @@ error: /* even if we error out, we forwarded the time, so call update */
 }
 EXPORT_SYMBOL(timekeeping_inject_offset);
 
+
+/**
+ * timekeeping_get_tai_offset - Returns current TAI offset from UTC
+ *
+ */
+s32 timekeeping_get_tai_offset(void)
+{
+	struct timekeeper *tk = &timekeeper;
+	unsigned int seq;
+	s32 ret;
+
+	do {
+		seq = read_seqcount_begin(&timekeeper_seq);
+		ret = tk->tai_offset;
+	} while (read_seqcount_retry(&timekeeper_seq, seq));
+
+	return ret;
+}
+
+/**
+ * __timekeeping_set_tai_offset - Lock free worker function
+ *
+ */
+static void __timekeeping_set_tai_offset(struct timekeeper *tk, s32 tai_offset)
+{
+	tk->tai_offset = tai_offset;
+	tk->offs_tai = ktime_sub(tk->offs_real, ktime_set(tai_offset, 0));
+}
+
+/**
+ * timekeeping_set_tai_offset - Sets the current TAI offset from UTC
+ *
+ */
+void timekeeping_set_tai_offset(s32 tai_offset)
+{
+	struct timekeeper *tk = &timekeeper;
+	unsigned long flags;
+
+	raw_spin_lock_irqsave(&timekeeper_lock, flags);
+	write_seqcount_begin(&timekeeper_seq);
+	__timekeeping_set_tai_offset(tk, tai_offset);
+	write_seqcount_end(&timekeeper_seq);
+	raw_spin_unlock_irqrestore(&timekeeper_lock, flags);
+	clock_was_set();
+}
+
 /**
  * change_clocksource - Swaps clocksources if a new one is available
  *
@@ -526,7 +623,8 @@ static int change_clocksource(void *data)
 
 	new = (struct clocksource *) data;
 
-	write_seqlock_irqsave(&tk->lock, flags);
+	raw_spin_lock_irqsave(&timekeeper_lock, flags);
+	write_seqcount_begin(&timekeeper_seq);
 
 	timekeeping_forward_now(tk);
 	if (!new->enable || new->enable(new) == 0) {
@@ -535,9 +633,10 @@ static int change_clocksource(void *data)
 		if (old->disable)
 			old->disable(old);
 	}
-	timekeeping_update(tk, true);
+	timekeeping_update(tk, true, true);
 
-	write_sequnlock_irqrestore(&tk->lock, flags);
+	write_seqcount_end(&timekeeper_seq);
+	raw_spin_unlock_irqrestore(&timekeeper_lock, flags);
 
 	return 0;
 }
@@ -587,11 +686,11 @@ void getrawmonotonic(struct timespec *ts)
 	s64 nsecs;
 
 	do {
-		seq = read_seqbegin(&tk->lock);
+		seq = read_seqcount_begin(&timekeeper_seq);
 		nsecs = timekeeping_get_ns_raw(tk);
 		*ts = tk->raw_time;
 
-	} while (read_seqretry(&tk->lock, seq));
+	} while (read_seqcount_retry(&timekeeper_seq, seq));
 
 	timespec_add_ns(ts, nsecs);
 }
@@ -607,11 +706,11 @@ int timekeeping_valid_for_hres(void)
 	int ret;
 
 	do {
-		seq = read_seqbegin(&tk->lock);
+		seq = read_seqcount_begin(&timekeeper_seq);
 
 		ret = tk->clock->flags & CLOCK_SOURCE_VALID_FOR_HRES;
 
-	} while (read_seqretry(&tk->lock, seq));
+	} while (read_seqcount_retry(&timekeeper_seq, seq));
 
 	return ret;
 }
@@ -626,11 +725,11 @@ u64 timekeeping_max_deferment(void)
 	u64 ret;
 
 	do {
-		seq = read_seqbegin(&tk->lock);
+		seq = read_seqcount_begin(&timekeeper_seq);
 
 		ret = tk->clock->max_idle_ns;
 
-	} while (read_seqretry(&tk->lock, seq));
+	} while (read_seqcount_retry(&timekeeper_seq, seq));
 
 	return ret;
 }
@@ -693,11 +792,10 @@ void __init timekeeping_init(void)
 		boot.tv_nsec = 0;
 	}
 
-	seqlock_init(&tk->lock);
-
+	raw_spin_lock_irqsave(&timekeeper_lock, flags);
+	write_seqcount_begin(&timekeeper_seq);
 	ntp_init();
 
-	write_seqlock_irqsave(&tk->lock, flags);
 	clock = clocksource_default_clock();
 	if (clock->enable)
 		clock->enable(clock);
@@ -716,7 +814,10 @@ void __init timekeeping_init(void)
 	tmp.tv_nsec = 0;
 	tk_set_sleep_time(tk, tmp);
 
-	write_sequnlock_irqrestore(&tk->lock, flags);
+	memcpy(&shadow_timekeeper, &timekeeper, sizeof(timekeeper));
+
+	write_seqcount_end(&timekeeper_seq);
+	raw_spin_unlock_irqrestore(&timekeeper_lock, flags);
 }
 
 /* time in seconds when suspend began */
@@ -764,15 +865,17 @@ void timekeeping_inject_sleeptime(struct timespec *delta)
 	if (has_persistent_clock())
 		return;
 
-	write_seqlock_irqsave(&tk->lock, flags);
+	raw_spin_lock_irqsave(&timekeeper_lock, flags);
+	write_seqcount_begin(&timekeeper_seq);
 
 	timekeeping_forward_now(tk);
 
 	__timekeeping_inject_sleeptime(tk, delta);
 
-	timekeeping_update(tk, true);
+	timekeeping_update(tk, true, true);
 
-	write_sequnlock_irqrestore(&tk->lock, flags);
+	write_seqcount_end(&timekeeper_seq);
+	raw_spin_unlock_irqrestore(&timekeeper_lock, flags);
 
 	/* signal hrtimers about time change */
 	clock_was_set();
@@ -788,26 +891,72 @@ void timekeeping_inject_sleeptime(struct timespec *delta)
 static void timekeeping_resume(void)
 {
 	struct timekeeper *tk = &timekeeper;
+	struct clocksource *clock = tk->clock;
 	unsigned long flags;
-	struct timespec ts;
+	struct timespec ts_new, ts_delta;
+	cycle_t cycle_now, cycle_delta;
+	bool suspendtime_found = false;
 
-	read_persistent_clock(&ts);
+	read_persistent_clock(&ts_new);
 
 	clockevents_resume();
 	clocksource_resume();
 
-	write_seqlock_irqsave(&tk->lock, flags);
+	raw_spin_lock_irqsave(&timekeeper_lock, flags);
+	write_seqcount_begin(&timekeeper_seq);
+
+	/*
+	 * After system resumes, we need to calculate the suspended time and
+	 * compensate it for the OS time. There are 3 sources that could be
+	 * used: Nonstop clocksource during suspend, persistent clock and rtc
+	 * device.
+	 *
+	 * One specific platform may have 1 or 2 or all of them, and the
+	 * preference will be:
+	 *	suspend-nonstop clocksource -> persistent clock -> rtc
+	 * The less preferred source will only be tried if there is no better
+	 * usable source. The rtc part is handled separately in rtc core code.
+	 */
+	cycle_now = clock->read(clock);
+	if ((clock->flags & CLOCK_SOURCE_SUSPEND_NONSTOP) &&
+		cycle_now > clock->cycle_last) {
+		u64 num, max = ULLONG_MAX;
+		u32 mult = clock->mult;
+		u32 shift = clock->shift;
+		s64 nsec = 0;
+
+		cycle_delta = (cycle_now - clock->cycle_last) & clock->mask;
 
-	if (timespec_compare(&ts, &timekeeping_suspend_time) > 0) {
-		ts = timespec_sub(ts, timekeeping_suspend_time);
-		__timekeeping_inject_sleeptime(tk, &ts);
+		/*
+		 * "cycle_delta * mutl" may cause 64 bits overflow, if the
+		 * suspended time is too long. In that case we need do the
+		 * 64 bits math carefully
+		 */
+		do_div(max, mult);
+		if (cycle_delta > max) {
+			num = div64_u64(cycle_delta, max);
+			nsec = (((u64) max * mult) >> shift) * num;
+			cycle_delta -= num * max;
+		}
+		nsec += ((u64) cycle_delta * mult) >> shift;
+
+		ts_delta = ns_to_timespec(nsec);
+		suspendtime_found = true;
+	} else if (timespec_compare(&ts_new, &timekeeping_suspend_time) > 0) {
+		ts_delta = timespec_sub(ts_new, timekeeping_suspend_time);
+		suspendtime_found = true;
 	}
-	/* re-base the last cycle value */
-	tk->clock->cycle_last = tk->clock->read(tk->clock);
+
+	if (suspendtime_found)
+		__timekeeping_inject_sleeptime(tk, &ts_delta);
+
+	/* Re-base the last cycle value */
+	tk->cycle_last = clock->cycle_last = cycle_now;
 	tk->ntp_error = 0;
 	timekeeping_suspended = 0;
-	timekeeping_update(tk, false);
-	write_sequnlock_irqrestore(&tk->lock, flags);
+	timekeeping_update(tk, false, true);
+	write_seqcount_end(&timekeeper_seq);
+	raw_spin_unlock_irqrestore(&timekeeper_lock, flags);
 
 	touch_softlockup_watchdog();
 
@@ -826,7 +975,8 @@ static int timekeeping_suspend(void)
 
 	read_persistent_clock(&timekeeping_suspend_time);
 
-	write_seqlock_irqsave(&tk->lock, flags);
+	raw_spin_lock_irqsave(&timekeeper_lock, flags);
+	write_seqcount_begin(&timekeeper_seq);
 	timekeeping_forward_now(tk);
 	timekeeping_suspended = 1;
 
@@ -849,7 +999,8 @@ static int timekeeping_suspend(void)
 		timekeeping_suspend_time =
 			timespec_add(timekeeping_suspend_time, delta_delta);
 	}
-	write_sequnlock_irqrestore(&tk->lock, flags);
+	write_seqcount_end(&timekeeper_seq);
+	raw_spin_unlock_irqrestore(&timekeeper_lock, flags);
 
 	clockevents_notify(CLOCK_EVT_NOTIFY_SUSPEND, NULL);
 	clocksource_suspend();
@@ -1099,6 +1250,8 @@ static inline void accumulate_nsecs_to_secs(struct timekeeper *tk)
 			tk_set_wall_to_mono(tk,
 				timespec_sub(tk->wall_to_monotonic, ts));
 
+			__timekeeping_set_tai_offset(tk, tk->tai_offset - leap);
+
 			clock_was_set_delayed();
 		}
 	}
@@ -1116,15 +1269,16 @@ static inline void accumulate_nsecs_to_secs(struct timekeeper *tk)
 static cycle_t logarithmic_accumulation(struct timekeeper *tk, cycle_t offset,
 						u32 shift)
 {
+	cycle_t interval = tk->cycle_interval << shift;
 	u64 raw_nsecs;
 
 	/* If the offset is smaller then a shifted interval, do nothing */
-	if (offset < tk->cycle_interval<<shift)
+	if (offset < interval)
 		return offset;
 
 	/* Accumulate one shifted interval */
-	offset -= tk->cycle_interval << shift;
-	tk->clock->cycle_last += tk->cycle_interval << shift;
+	offset -= interval;
+	tk->cycle_last += interval;
 
 	tk->xtime_nsec += tk->xtime_interval << shift;
 	accumulate_nsecs_to_secs(tk);
@@ -1181,27 +1335,28 @@ static inline void old_vsyscall_fixup(struct timekeeper *tk)
 static void update_wall_time(void)
 {
 	struct clocksource *clock;
-	struct timekeeper *tk = &timekeeper;
+	struct timekeeper *real_tk = &timekeeper;
+	struct timekeeper *tk = &shadow_timekeeper;
 	cycle_t offset;
 	int shift = 0, maxshift;
 	unsigned long flags;
 
-	write_seqlock_irqsave(&tk->lock, flags);
+	raw_spin_lock_irqsave(&timekeeper_lock, flags);
 
 	/* Make sure we're fully resumed: */
 	if (unlikely(timekeeping_suspended))
 		goto out;
 
-	clock = tk->clock;
+	clock = real_tk->clock;
 
 #ifdef CONFIG_ARCH_USES_GETTIMEOFFSET
-	offset = tk->cycle_interval;
+	offset = real_tk->cycle_interval;
 #else
 	offset = (clock->read(clock) - clock->cycle_last) & clock->mask;
 #endif
 
 	/* Check if there's really nothing to do */
-	if (offset < tk->cycle_interval)
+	if (offset < real_tk->cycle_interval)
 		goto out;
 
 	/*
@@ -1238,11 +1393,24 @@ static void update_wall_time(void)
 	 */
 	accumulate_nsecs_to_secs(tk);
 
-	timekeeping_update(tk, false);
-
+	write_seqcount_begin(&timekeeper_seq);
+	/* Update clock->cycle_last with the new value */
+	clock->cycle_last = tk->cycle_last;
+	/*
+	 * Update the real timekeeper.
+	 *
+	 * We could avoid this memcpy by switching pointers, but that
+	 * requires changes to all other timekeeper usage sites as
+	 * well, i.e. move the timekeeper pointer getter into the
+	 * spinlocked/seqcount protected sections. And we trade this
+	 * memcpy under the timekeeper_seq against one before we start
+	 * updating.
+	 */
+	memcpy(real_tk, tk, sizeof(*tk));
+	timekeeping_update(real_tk, false, false);
+	write_seqcount_end(&timekeeper_seq);
 out:
-	write_sequnlock_irqrestore(&tk->lock, flags);
-
+	raw_spin_unlock_irqrestore(&timekeeper_lock, flags);
 }
 
 /**
@@ -1289,13 +1457,13 @@ void get_monotonic_boottime(struct timespec *ts)
 	WARN_ON(timekeeping_suspended);
 
 	do {
-		seq = read_seqbegin(&tk->lock);
+		seq = read_seqcount_begin(&timekeeper_seq);
 		ts->tv_sec = tk->xtime_sec;
 		nsec = timekeeping_get_ns(tk);
 		tomono = tk->wall_to_monotonic;
 		sleep = tk->total_sleep_time;
 
-	} while (read_seqretry(&tk->lock, seq));
+	} while (read_seqcount_retry(&timekeeper_seq, seq));
 
 	ts->tv_sec += tomono.tv_sec + sleep.tv_sec;
 	ts->tv_nsec = 0;
@@ -1354,10 +1522,10 @@ struct timespec current_kernel_time(void)
 	unsigned long seq;
 
 	do {
-		seq = read_seqbegin(&tk->lock);
+		seq = read_seqcount_begin(&timekeeper_seq);
 
 		now = tk_xtime(tk);
-	} while (read_seqretry(&tk->lock, seq));
+	} while (read_seqcount_retry(&timekeeper_seq, seq));
 
 	return now;
 }
@@ -1370,11 +1538,11 @@ struct timespec get_monotonic_coarse(void)
 	unsigned long seq;
 
 	do {
-		seq = read_seqbegin(&tk->lock);
+		seq = read_seqcount_begin(&timekeeper_seq);
 
 		now = tk_xtime(tk);
 		mono = tk->wall_to_monotonic;
-	} while (read_seqretry(&tk->lock, seq));
+	} while (read_seqcount_retry(&timekeeper_seq, seq));
 
 	set_normalized_timespec(&now, now.tv_sec + mono.tv_sec,
 				now.tv_nsec + mono.tv_nsec);
@@ -1405,11 +1573,11 @@ void get_xtime_and_monotonic_and_sleep_offset(struct timespec *xtim,
 	unsigned long seq;
 
 	do {
-		seq = read_seqbegin(&tk->lock);
+		seq = read_seqcount_begin(&timekeeper_seq);
 		*xtim = tk_xtime(tk);
 		*wtom = tk->wall_to_monotonic;
 		*sleep = tk->total_sleep_time;
-	} while (read_seqretry(&tk->lock, seq));
+	} while (read_seqcount_retry(&timekeeper_seq, seq));
 }
 
 #ifdef CONFIG_HIGH_RES_TIMERS
@@ -1421,7 +1589,8 @@ void get_xtime_and_monotonic_and_sleep_offset(struct timespec *xtim,
  * Returns current monotonic time and updates the offsets
  * Called from hrtimer_interupt() or retrigger_next_event()
  */
-ktime_t ktime_get_update_offsets(ktime_t *offs_real, ktime_t *offs_boot)
+ktime_t ktime_get_update_offsets(ktime_t *offs_real, ktime_t *offs_boot,
+							ktime_t *offs_tai)
 {
 	struct timekeeper *tk = &timekeeper;
 	ktime_t now;
@@ -1429,14 +1598,15 @@ ktime_t ktime_get_update_offsets(ktime_t *offs_real, ktime_t *offs_boot)
 	u64 secs, nsecs;
 
 	do {
-		seq = read_seqbegin(&tk->lock);
+		seq = read_seqcount_begin(&timekeeper_seq);
 
 		secs = tk->xtime_sec;
 		nsecs = timekeeping_get_ns(tk);
 
 		*offs_real = tk->offs_real;
 		*offs_boot = tk->offs_boot;
-	} while (read_seqretry(&tk->lock, seq));
+		*offs_tai = tk->offs_tai;
+	} while (read_seqcount_retry(&timekeeper_seq, seq));
 
 	now = ktime_add_ns(ktime_set(secs, 0), nsecs);
 	now = ktime_sub(now, *offs_real);
@@ -1454,15 +1624,79 @@ ktime_t ktime_get_monotonic_offset(void)
 	struct timespec wtom;
 
 	do {
-		seq = read_seqbegin(&tk->lock);
+		seq = read_seqcount_begin(&timekeeper_seq);
 		wtom = tk->wall_to_monotonic;
-	} while (read_seqretry(&tk->lock, seq));
+	} while (read_seqcount_retry(&timekeeper_seq, seq));
 
 	return timespec_to_ktime(wtom);
 }
 EXPORT_SYMBOL_GPL(ktime_get_monotonic_offset);
 
 /**
+ * do_adjtimex() - Accessor function to NTP __do_adjtimex function
+ */
+int do_adjtimex(struct timex *txc)
+{
+	struct timekeeper *tk = &timekeeper;
+	unsigned long flags;
+	struct timespec ts;
+	s32 orig_tai, tai;
+	int ret;
+
+	/* Validate the data before disabling interrupts */
+	ret = ntp_validate_timex(txc);
+	if (ret)
+		return ret;
+
+	if (txc->modes & ADJ_SETOFFSET) {
+		struct timespec delta;
+		delta.tv_sec  = txc->time.tv_sec;
+		delta.tv_nsec = txc->time.tv_usec;
+		if (!(txc->modes & ADJ_NANO))
+			delta.tv_nsec *= 1000;
+		ret = timekeeping_inject_offset(&delta);
+		if (ret)
+			return ret;
+	}
+
+	getnstimeofday(&ts);
+
+	raw_spin_lock_irqsave(&timekeeper_lock, flags);
+	write_seqcount_begin(&timekeeper_seq);
+
+	orig_tai = tai = tk->tai_offset;
+	ret = __do_adjtimex(txc, &ts, &tai);
+
+	if (tai != orig_tai) {
+		__timekeeping_set_tai_offset(tk, tai);
+		clock_was_set_delayed();
+	}
+	write_seqcount_end(&timekeeper_seq);
+	raw_spin_unlock_irqrestore(&timekeeper_lock, flags);
+
+	return ret;
+}
+
+#ifdef CONFIG_NTP_PPS
+/**
+ * hardpps() - Accessor function to NTP __hardpps function
+ */
+void hardpps(const struct timespec *phase_ts, const struct timespec *raw_ts)
+{
+	unsigned long flags;
+
+	raw_spin_lock_irqsave(&timekeeper_lock, flags);
+	write_seqcount_begin(&timekeeper_seq);
+
+	__hardpps(phase_ts, raw_ts);
+
+	write_seqcount_end(&timekeeper_seq);
+	raw_spin_unlock_irqrestore(&timekeeper_lock, flags);
+}
+EXPORT_SYMBOL(hardpps);
+#endif
+
+/**
  * xtime_update() - advances the timekeeping infrastructure
  * @ticks:	number of ticks, that have elapsed since the last call.
  *
diff --git a/kernel/time/timer_list.c b/kernel/time/timer_list.c
index af5a7e9f164..3bdf2832301 100644
--- a/kernel/time/timer_list.c
+++ b/kernel/time/timer_list.c
@@ -20,6 +20,13 @@
 
 #include <asm/uaccess.h>
 
+
+struct timer_list_iter {
+	int cpu;
+	bool second_pass;
+	u64 now;
+};
+
 typedef void (*print_fn_t)(struct seq_file *m, unsigned int *classes);
 
 DECLARE_PER_CPU(struct hrtimer_cpu_base, hrtimer_bases);
@@ -133,7 +140,6 @@ static void print_cpu(struct seq_file *m, int cpu, u64 now)
 	struct hrtimer_cpu_base *cpu_base = &per_cpu(hrtimer_bases, cpu);
 	int i;
 
-	SEQ_printf(m, "\n");
 	SEQ_printf(m, "cpu: %d\n", cpu);
 	for (i = 0; i < HRTIMER_MAX_CLOCK_BASES; i++) {
 		SEQ_printf(m, " clock %d:\n", i);
@@ -187,6 +193,7 @@ static void print_cpu(struct seq_file *m, int cpu, u64 now)
 
 #undef P
 #undef P_ns
+	SEQ_printf(m, "\n");
 }
 
 #ifdef CONFIG_GENERIC_CLOCKEVENTS
@@ -195,7 +202,6 @@ print_tickdevice(struct seq_file *m, struct tick_device *td, int cpu)
 {
 	struct clock_event_device *dev = td->evtdev;
 
-	SEQ_printf(m, "\n");
 	SEQ_printf(m, "Tick Device: mode:     %d\n", td->mode);
 	if (cpu < 0)
 		SEQ_printf(m, "Broadcast device\n");
@@ -230,12 +236,11 @@ print_tickdevice(struct seq_file *m, struct tick_device *td, int cpu)
 	print_name_offset(m, dev->event_handler);
 	SEQ_printf(m, "\n");
 	SEQ_printf(m, " retries:        %lu\n", dev->retries);
+	SEQ_printf(m, "\n");
 }
 
-static void timer_list_show_tickdevices(struct seq_file *m)
+static void timer_list_show_tickdevices_header(struct seq_file *m)
 {
-	int cpu;
-
 #ifdef CONFIG_GENERIC_CLOCKEVENTS_BROADCAST
 	print_tickdevice(m, tick_get_broadcast_device(), -1);
 	SEQ_printf(m, "tick_broadcast_mask: %08lx\n",
@@ -246,47 +251,104 @@ static void timer_list_show_tickdevices(struct seq_file *m)
 #endif
 	SEQ_printf(m, "\n");
 #endif
-	for_each_online_cpu(cpu)
-		print_tickdevice(m, tick_get_device(cpu), cpu);
-	SEQ_printf(m, "\n");
 }
-#else
-static void timer_list_show_tickdevices(struct seq_file *m) { }
 #endif
 
+static inline void timer_list_header(struct seq_file *m, u64 now)
+{
+	SEQ_printf(m, "Timer List Version: v0.7\n");
+	SEQ_printf(m, "HRTIMER_MAX_CLOCK_BASES: %d\n", HRTIMER_MAX_CLOCK_BASES);
+	SEQ_printf(m, "now at %Ld nsecs\n", (unsigned long long)now);
+	SEQ_printf(m, "\n");
+}
+
 static int timer_list_show(struct seq_file *m, void *v)
 {
+	struct timer_list_iter *iter = v;
+	u64 now = ktime_to_ns(ktime_get());
+
+	if (iter->cpu == -1 && !iter->second_pass)
+		timer_list_header(m, now);
+	else if (!iter->second_pass)
+		print_cpu(m, iter->cpu, iter->now);
+#ifdef CONFIG_GENERIC_CLOCKEVENTS
+	else if (iter->cpu == -1 && iter->second_pass)
+		timer_list_show_tickdevices_header(m);
+	else
+		print_tickdevice(m, tick_get_device(iter->cpu), iter->cpu);
+#endif
+	return 0;
+}
+
+void sysrq_timer_list_show(void)
+{
 	u64 now = ktime_to_ns(ktime_get());
 	int cpu;
 
-	SEQ_printf(m, "Timer List Version: v0.7\n");
-	SEQ_printf(m, "HRTIMER_MAX_CLOCK_BASES: %d\n", HRTIMER_MAX_CLOCK_BASES);
-	SEQ_printf(m, "now at %Ld nsecs\n", (unsigned long long)now);
+	timer_list_header(NULL, now);
 
 	for_each_online_cpu(cpu)
-		print_cpu(m, cpu, now);
+		print_cpu(NULL, cpu, now);
 
-	SEQ_printf(m, "\n");
-	timer_list_show_tickdevices(m);
+#ifdef CONFIG_GENERIC_CLOCKEVENTS
+	timer_list_show_tickdevices_header(NULL);
+	for_each_online_cpu(cpu)
+		print_tickdevice(NULL, tick_get_device(cpu), cpu);
+#endif
+	return;
+}
 
-	return 0;
+static void *timer_list_start(struct seq_file *file, loff_t *offset)
+{
+	struct timer_list_iter *iter = file->private;
+
+	if (!*offset) {
+		iter->cpu = -1;
+		iter->now = ktime_to_ns(ktime_get());
+	} else if (iter->cpu >= nr_cpu_ids) {
+#ifdef CONFIG_GENERIC_CLOCKEVENTS
+		if (!iter->second_pass) {
+			iter->cpu = -1;
+			iter->second_pass = true;
+		} else
+			return NULL;
+#else
+		return NULL;
+#endif
+	}
+	return iter;
 }
 
-void sysrq_timer_list_show(void)
+static void *timer_list_next(struct seq_file *file, void *v, loff_t *offset)
+{
+	struct timer_list_iter *iter = file->private;
+	iter->cpu = cpumask_next(iter->cpu, cpu_online_mask);
+	++*offset;
+	return timer_list_start(file, offset);
+}
+
+static void timer_list_stop(struct seq_file *seq, void *v)
 {
-	timer_list_show(NULL, NULL);
 }
 
+static const struct seq_operations timer_list_sops = {
+	.start = timer_list_start,
+	.next = timer_list_next,
+	.stop = timer_list_stop,
+	.show = timer_list_show,
+};
+
 static int timer_list_open(struct inode *inode, struct file *filp)
 {
-	return single_open(filp, timer_list_show, NULL);
+	return seq_open_private(filp, &timer_list_sops,
+			sizeof(struct timer_list_iter));
 }
 
 static const struct file_operations timer_list_fops = {
 	.open		= timer_list_open,
 	.read		= seq_read,
 	.llseek		= seq_lseek,
-	.release	= single_release,
+	.release	= seq_release_private,
 };
 
 static int __init init_timer_list_procfs(void)
diff --git a/kernel/trace/Kconfig b/kernel/trace/Kconfig
index fc382d6e276..5e9efd4b83a 100644
--- a/kernel/trace/Kconfig
+++ b/kernel/trace/Kconfig
@@ -176,6 +176,8 @@ config IRQSOFF_TRACER
 	select GENERIC_TRACER
 	select TRACER_MAX_TRACE
 	select RING_BUFFER_ALLOW_SWAP
+	select TRACER_SNAPSHOT
+	select TRACER_SNAPSHOT_PER_CPU_SWAP
 	help
 	  This option measures the time spent in irqs-off critical
 	  sections, with microsecond accuracy.
@@ -198,6 +200,8 @@ config PREEMPT_TRACER
 	select GENERIC_TRACER
 	select TRACER_MAX_TRACE
 	select RING_BUFFER_ALLOW_SWAP
+	select TRACER_SNAPSHOT
+	select TRACER_SNAPSHOT_PER_CPU_SWAP
 	help
 	  This option measures the time spent in preemption-off critical
 	  sections, with microsecond accuracy.
@@ -217,6 +221,7 @@ config SCHED_TRACER
 	select GENERIC_TRACER
 	select CONTEXT_SWITCH_TRACER
 	select TRACER_MAX_TRACE
+	select TRACER_SNAPSHOT
 	help
 	  This tracer tracks the latency of the highest priority task
 	  to be scheduled in, starting from the point it has woken up.
@@ -248,6 +253,27 @@ config TRACER_SNAPSHOT
 	      echo 1 > /sys/kernel/debug/tracing/snapshot
 	      cat snapshot
 
+config TRACER_SNAPSHOT_PER_CPU_SWAP
+        bool "Allow snapshot to swap per CPU"
+	depends on TRACER_SNAPSHOT
+	select RING_BUFFER_ALLOW_SWAP
+	help
+	  Allow doing a snapshot of a single CPU buffer instead of a
+	  full swap (all buffers). If this is set, then the following is
+	  allowed:
+
+	      echo 1 > /sys/kernel/debug/tracing/per_cpu/cpu2/snapshot
+
+	  After which, only the tracing buffer for CPU 2 was swapped with
+	  the main tracing buffer, and the other CPU buffers remain the same.
+
+	  When this is enabled, this adds a little more overhead to the
+	  trace recording, as it needs to add some checks to synchronize
+	  recording with swaps. But this does not affect the performance
+	  of the overall system. This is enabled by default when the preempt
+	  or irq latency tracers are enabled, as those need to swap as well
+	  and already adds the overhead (plus a lot more).
+
 config TRACE_BRANCH_PROFILING
 	bool
 	select GENERIC_TRACER
@@ -524,6 +550,29 @@ config RING_BUFFER_BENCHMARK
 
 	  If unsure, say N.
 
+config RING_BUFFER_STARTUP_TEST
+       bool "Ring buffer startup self test"
+       depends on RING_BUFFER
+       help
+         Run a simple self test on the ring buffer on boot up. Late in the
+	 kernel boot sequence, the test will start that kicks off
+	 a thread per cpu. Each thread will write various size events
+	 into the ring buffer. Another thread is created to send IPIs
+	 to each of the threads, where the IPI handler will also write
+	 to the ring buffer, to test/stress the nesting ability.
+	 If any anomalies are discovered, a warning will be displayed
+	 and all ring buffers will be disabled.
+
+	 The test runs for 10 seconds. This will slow your boot time
+	 by at least 10 more seconds.
+
+	 At the end of the test, statics and more checks are done.
+	 It will output the stats of each per cpu buffer. What
+	 was written, the sizes, what was read, what was lost, and
+	 other similar details.
+
+	 If unsure, say N
+
 endif # FTRACE
 
 endif # TRACING_SUPPORT
diff --git a/kernel/trace/blktrace.c b/kernel/trace/blktrace.c
index 9e5b8c272ee..ed58a3216a6 100644
--- a/kernel/trace/blktrace.c
+++ b/kernel/trace/blktrace.c
@@ -72,7 +72,7 @@ static void trace_note(struct blk_trace *bt, pid_t pid, int action,
 	bool blk_tracer = blk_tracer_enabled;
 
 	if (blk_tracer) {
-		buffer = blk_tr->buffer;
+		buffer = blk_tr->trace_buffer.buffer;
 		pc = preempt_count();
 		event = trace_buffer_lock_reserve(buffer, TRACE_BLK,
 						  sizeof(*t) + len,
@@ -218,7 +218,7 @@ static void __blk_add_trace(struct blk_trace *bt, sector_t sector, int bytes,
 	if (blk_tracer) {
 		tracing_record_cmdline(current);
 
-		buffer = blk_tr->buffer;
+		buffer = blk_tr->trace_buffer.buffer;
 		pc = preempt_count();
 		event = trace_buffer_lock_reserve(buffer, TRACE_BLK,
 						  sizeof(*t) + pdu_len,
@@ -739,12 +739,6 @@ static void blk_add_trace_rq_complete(void *ignore,
 				      struct request_queue *q,
 				      struct request *rq)
 {
-	struct blk_trace *bt = q->blk_trace;
-
-	/* if control ever passes through here, it's a request based driver */
-	if (unlikely(bt && !bt->rq_based))
-		bt->rq_based = true;
-
 	blk_add_trace_rq(q, rq, BLK_TA_COMPLETE);
 }
 
@@ -780,24 +774,10 @@ static void blk_add_trace_bio_bounce(void *ignore,
 	blk_add_trace_bio(q, bio, BLK_TA_BOUNCE, 0);
 }
 
-static void blk_add_trace_bio_complete(void *ignore, struct bio *bio, int error)
+static void blk_add_trace_bio_complete(void *ignore,
+				       struct request_queue *q, struct bio *bio,
+				       int error)
 {
-	struct request_queue *q;
-	struct blk_trace *bt;
-
-	if (!bio->bi_bdev)
-		return;
-
-	q = bdev_get_queue(bio->bi_bdev);
-	bt = q->blk_trace;
-
-	/*
-	 * Request based drivers will generate both rq and bio completions.
-	 * Ignore bio ones.
-	 */
-	if (likely(!bt) || bt->rq_based)
-		return;
-
 	blk_add_trace_bio(q, bio, BLK_TA_COMPLETE, error);
 }
 
diff --git a/kernel/trace/ftrace.c b/kernel/trace/ftrace.c
index 6893d5a2bf0..8a5c017bb50 100644
--- a/kernel/trace/ftrace.c
+++ b/kernel/trace/ftrace.c
@@ -66,7 +66,7 @@
 
 static struct ftrace_ops ftrace_list_end __read_mostly = {
 	.func		= ftrace_stub,
-	.flags		= FTRACE_OPS_FL_RECURSION_SAFE,
+	.flags		= FTRACE_OPS_FL_RECURSION_SAFE | FTRACE_OPS_FL_STUB,
 };
 
 /* ftrace_enabled is a method to turn ftrace on or off */
@@ -486,7 +486,6 @@ struct ftrace_profile_stat {
 #define PROFILES_PER_PAGE					\
 	(PROFILE_RECORDS_SIZE / sizeof(struct ftrace_profile))
 
-static int ftrace_profile_bits __read_mostly;
 static int ftrace_profile_enabled __read_mostly;
 
 /* ftrace_profile_lock - synchronize the enable and disable of the profiler */
@@ -494,7 +493,8 @@ static DEFINE_MUTEX(ftrace_profile_lock);
 
 static DEFINE_PER_CPU(struct ftrace_profile_stat, ftrace_profile_stats);
 
-#define FTRACE_PROFILE_HASH_SIZE 1024 /* must be power of 2 */
+#define FTRACE_PROFILE_HASH_BITS 10
+#define FTRACE_PROFILE_HASH_SIZE (1 << FTRACE_PROFILE_HASH_BITS)
 
 static void *
 function_stat_next(void *v, int idx)
@@ -676,7 +676,7 @@ int ftrace_profile_pages_init(struct ftrace_profile_stat *stat)
 
 	pages = DIV_ROUND_UP(functions, PROFILES_PER_PAGE);
 
-	for (i = 0; i < pages; i++) {
+	for (i = 1; i < pages; i++) {
 		pg->next = (void *)get_zeroed_page(GFP_KERNEL);
 		if (!pg->next)
 			goto out_free;
@@ -694,7 +694,6 @@ int ftrace_profile_pages_init(struct ftrace_profile_stat *stat)
 		free_page(tmp);
 	}
 
-	free_page((unsigned long)stat->pages);
 	stat->pages = NULL;
 	stat->start = NULL;
 
@@ -725,13 +724,6 @@ static int ftrace_profile_init_cpu(int cpu)
 	if (!stat->hash)
 		return -ENOMEM;
 
-	if (!ftrace_profile_bits) {
-		size--;
-
-		for (; size; size >>= 1)
-			ftrace_profile_bits++;
-	}
-
 	/* Preallocate the function profiling pages */
 	if (ftrace_profile_pages_init(stat) < 0) {
 		kfree(stat->hash);
@@ -764,7 +756,7 @@ ftrace_find_profiled_func(struct ftrace_profile_stat *stat, unsigned long ip)
 	struct hlist_head *hhd;
 	unsigned long key;
 
-	key = hash_long(ip, ftrace_profile_bits);
+	key = hash_long(ip, FTRACE_PROFILE_HASH_BITS);
 	hhd = &stat->hash[key];
 
 	if (hlist_empty(hhd))
@@ -783,7 +775,7 @@ static void ftrace_add_profile(struct ftrace_profile_stat *stat,
 {
 	unsigned long key;
 
-	key = hash_long(rec->ip, ftrace_profile_bits);
+	key = hash_long(rec->ip, FTRACE_PROFILE_HASH_BITS);
 	hlist_add_head_rcu(&rec->node, &stat->hash[key]);
 }
 
@@ -1053,6 +1045,19 @@ static __init void ftrace_profile_debugfs(struct dentry *d_tracer)
 
 static struct pid * const ftrace_swapper_pid = &init_struct_pid;
 
+loff_t
+ftrace_filter_lseek(struct file *file, loff_t offset, int whence)
+{
+	loff_t ret;
+
+	if (file->f_mode & FMODE_READ)
+		ret = seq_lseek(file, offset, whence);
+	else
+		file->f_pos = ret = 1;
+
+	return ret;
+}
+
 #ifdef CONFIG_DYNAMIC_FTRACE
 
 #ifndef CONFIG_FTRACE_MCOUNT_RECORD
@@ -1067,7 +1072,7 @@ struct ftrace_func_probe {
 	unsigned long		flags;
 	unsigned long		ip;
 	void			*data;
-	struct rcu_head		rcu;
+	struct list_head	free_list;
 };
 
 struct ftrace_func_entry {
@@ -1317,7 +1322,6 @@ ftrace_hash_move(struct ftrace_ops *ops, int enable,
 	struct hlist_head *hhd;
 	struct ftrace_hash *old_hash;
 	struct ftrace_hash *new_hash;
-	unsigned long key;
 	int size = src->count;
 	int bits = 0;
 	int ret;
@@ -1360,10 +1364,6 @@ ftrace_hash_move(struct ftrace_ops *ops, int enable,
 	for (i = 0; i < size; i++) {
 		hhd = &src->buckets[i];
 		hlist_for_each_entry_safe(entry, tn, hhd, hlist) {
-			if (bits > 0)
-				key = hash_long(entry->ip, bits);
-			else
-				key = 0;
 			remove_hash_entry(src, entry);
 			__add_hash_entry(new_hash, entry);
 		}
@@ -2613,7 +2613,7 @@ static void ftrace_filter_reset(struct ftrace_hash *hash)
  * routine, you can use ftrace_filter_write() for the write
  * routine if @flag has FTRACE_ITER_FILTER set, or
  * ftrace_notrace_write() if @flag has FTRACE_ITER_NOTRACE set.
- * ftrace_regex_lseek() should be used as the lseek routine, and
+ * ftrace_filter_lseek() should be used as the lseek routine, and
  * release must call ftrace_regex_release().
  */
 int
@@ -2697,19 +2697,6 @@ ftrace_notrace_open(struct inode *inode, struct file *file)
 				 inode, file);
 }
 
-loff_t
-ftrace_regex_lseek(struct file *file, loff_t offset, int whence)
-{
-	loff_t ret;
-
-	if (file->f_mode & FMODE_READ)
-		ret = seq_lseek(file, offset, whence);
-	else
-		file->f_pos = ret = 1;
-
-	return ret;
-}
-
 static int ftrace_match(char *str, char *regex, int len, int type)
 {
 	int matched = 0;
@@ -2974,28 +2961,27 @@ static void __disable_ftrace_function_probe(void)
 }
 
 
-static void ftrace_free_entry_rcu(struct rcu_head *rhp)
+static void ftrace_free_entry(struct ftrace_func_probe *entry)
 {
-	struct ftrace_func_probe *entry =
-		container_of(rhp, struct ftrace_func_probe, rcu);
-
 	if (entry->ops->free)
-		entry->ops->free(&entry->data);
+		entry->ops->free(entry->ops, entry->ip, &entry->data);
 	kfree(entry);
 }
 
-
 int
 register_ftrace_function_probe(char *glob, struct ftrace_probe_ops *ops,
 			      void *data)
 {
 	struct ftrace_func_probe *entry;
+	struct ftrace_hash **orig_hash = &trace_probe_ops.filter_hash;
+	struct ftrace_hash *hash;
 	struct ftrace_page *pg;
 	struct dyn_ftrace *rec;
 	int type, len, not;
 	unsigned long key;
 	int count = 0;
 	char *search;
+	int ret;
 
 	type = filter_parse_regex(glob, strlen(glob), &search, &not);
 	len = strlen(search);
@@ -3006,8 +2992,16 @@ register_ftrace_function_probe(char *glob, struct ftrace_probe_ops *ops,
 
 	mutex_lock(&ftrace_lock);
 
-	if (unlikely(ftrace_disabled))
+	hash = alloc_and_copy_ftrace_hash(FTRACE_HASH_DEFAULT_BITS, *orig_hash);
+	if (!hash) {
+		count = -ENOMEM;
+		goto out_unlock;
+	}
+
+	if (unlikely(ftrace_disabled)) {
+		count = -ENODEV;
 		goto out_unlock;
+	}
 
 	do_for_each_ftrace_rec(pg, rec) {
 
@@ -3031,14 +3025,21 @@ register_ftrace_function_probe(char *glob, struct ftrace_probe_ops *ops,
 		 * for each function we find. We call the callback
 		 * to give the caller an opportunity to do so.
 		 */
-		if (ops->callback) {
-			if (ops->callback(rec->ip, &entry->data) < 0) {
+		if (ops->init) {
+			if (ops->init(ops, rec->ip, &entry->data) < 0) {
 				/* caller does not like this func */
 				kfree(entry);
 				continue;
 			}
 		}
 
+		ret = enter_record(hash, rec, 0);
+		if (ret < 0) {
+			kfree(entry);
+			count = ret;
+			goto out_unlock;
+		}
+
 		entry->ops = ops;
 		entry->ip = rec->ip;
 
@@ -3046,10 +3047,16 @@ register_ftrace_function_probe(char *glob, struct ftrace_probe_ops *ops,
 		hlist_add_head_rcu(&entry->node, &ftrace_func_hash[key]);
 
 	} while_for_each_ftrace_rec();
+
+	ret = ftrace_hash_move(&trace_probe_ops, 1, orig_hash, hash);
+	if (ret < 0)
+		count = ret;
+
 	__enable_ftrace_function_probe();
 
  out_unlock:
 	mutex_unlock(&ftrace_lock);
+	free_ftrace_hash(hash);
 
 	return count;
 }
@@ -3063,7 +3070,12 @@ static void
 __unregister_ftrace_function_probe(char *glob, struct ftrace_probe_ops *ops,
 				  void *data, int flags)
 {
+	struct ftrace_func_entry *rec_entry;
 	struct ftrace_func_probe *entry;
+	struct ftrace_func_probe *p;
+	struct ftrace_hash **orig_hash = &trace_probe_ops.filter_hash;
+	struct list_head free_list;
+	struct ftrace_hash *hash;
 	struct hlist_node *tmp;
 	char str[KSYM_SYMBOL_LEN];
 	int type = MATCH_FULL;
@@ -3084,6 +3096,14 @@ __unregister_ftrace_function_probe(char *glob, struct ftrace_probe_ops *ops,
 	}
 
 	mutex_lock(&ftrace_lock);
+
+	hash = alloc_and_copy_ftrace_hash(FTRACE_HASH_DEFAULT_BITS, *orig_hash);
+	if (!hash)
+		/* Hmm, should report this somehow */
+		goto out_unlock;
+
+	INIT_LIST_HEAD(&free_list);
+
 	for (i = 0; i < FTRACE_FUNC_HASHSIZE; i++) {
 		struct hlist_head *hhd = &ftrace_func_hash[i];
 
@@ -3104,12 +3124,30 @@ __unregister_ftrace_function_probe(char *glob, struct ftrace_probe_ops *ops,
 					continue;
 			}
 
+			rec_entry = ftrace_lookup_ip(hash, entry->ip);
+			/* It is possible more than one entry had this ip */
+			if (rec_entry)
+				free_hash_entry(hash, rec_entry);
+
 			hlist_del_rcu(&entry->node);
-			call_rcu_sched(&entry->rcu, ftrace_free_entry_rcu);
+			list_add(&entry->free_list, &free_list);
 		}
 	}
 	__disable_ftrace_function_probe();
+	/*
+	 * Remove after the disable is called. Otherwise, if the last
+	 * probe is removed, a null hash means *all enabled*.
+	 */
+	ftrace_hash_move(&trace_probe_ops, 1, orig_hash, hash);
+	synchronize_sched();
+	list_for_each_entry_safe(entry, p, &free_list, free_list) {
+		list_del(&entry->free_list);
+		ftrace_free_entry(entry);
+	}
+		
+ out_unlock:
 	mutex_unlock(&ftrace_lock);
+	free_ftrace_hash(hash);
 }
 
 void
@@ -3441,14 +3479,14 @@ static char ftrace_filter_buf[FTRACE_FILTER_SIZE] __initdata;
 
 static int __init set_ftrace_notrace(char *str)
 {
-	strncpy(ftrace_notrace_buf, str, FTRACE_FILTER_SIZE);
+	strlcpy(ftrace_notrace_buf, str, FTRACE_FILTER_SIZE);
 	return 1;
 }
 __setup("ftrace_notrace=", set_ftrace_notrace);
 
 static int __init set_ftrace_filter(char *str)
 {
-	strncpy(ftrace_filter_buf, str, FTRACE_FILTER_SIZE);
+	strlcpy(ftrace_filter_buf, str, FTRACE_FILTER_SIZE);
 	return 1;
 }
 __setup("ftrace_filter=", set_ftrace_filter);
@@ -3571,7 +3609,7 @@ static const struct file_operations ftrace_filter_fops = {
 	.open = ftrace_filter_open,
 	.read = seq_read,
 	.write = ftrace_filter_write,
-	.llseek = ftrace_regex_lseek,
+	.llseek = ftrace_filter_lseek,
 	.release = ftrace_regex_release,
 };
 
@@ -3579,7 +3617,7 @@ static const struct file_operations ftrace_notrace_fops = {
 	.open = ftrace_notrace_open,
 	.read = seq_read,
 	.write = ftrace_notrace_write,
-	.llseek = ftrace_regex_lseek,
+	.llseek = ftrace_filter_lseek,
 	.release = ftrace_regex_release,
 };
 
@@ -3737,7 +3775,8 @@ out:
 	if (fail)
 		return -EINVAL;
 
-	ftrace_graph_filter_enabled = 1;
+	ftrace_graph_filter_enabled = !!(*idx);
+
 	return 0;
 }
 
@@ -3784,8 +3823,8 @@ static const struct file_operations ftrace_graph_fops = {
 	.open		= ftrace_graph_open,
 	.read		= seq_read,
 	.write		= ftrace_graph_write,
+	.llseek		= ftrace_filter_lseek,
 	.release	= ftrace_graph_release,
-	.llseek		= seq_lseek,
 };
 #endif /* CONFIG_FUNCTION_GRAPH_TRACER */
 
@@ -4131,7 +4170,8 @@ ftrace_ops_control_func(unsigned long ip, unsigned long parent_ip,
 	preempt_disable_notrace();
 	trace_recursion_set(TRACE_CONTROL_BIT);
 	do_for_each_ftrace_op(op, ftrace_control_list) {
-		if (!ftrace_function_local_disabled(op) &&
+		if (!(op->flags & FTRACE_OPS_FL_STUB) &&
+		    !ftrace_function_local_disabled(op) &&
 		    ftrace_ops_test(op, ip))
 			op->func(ip, parent_ip, op, regs);
 	} while_for_each_ftrace_op(op);
@@ -4439,7 +4479,7 @@ static const struct file_operations ftrace_pid_fops = {
 	.open		= ftrace_pid_open,
 	.write		= ftrace_pid_write,
 	.read		= seq_read,
-	.llseek		= seq_lseek,
+	.llseek		= ftrace_filter_lseek,
 	.release	= ftrace_pid_release,
 };
 
@@ -4555,12 +4595,8 @@ ftrace_enable_sysctl(struct ctl_table *table, int write,
 		ftrace_startup_sysctl();
 
 		/* we are starting ftrace again */
-		if (ftrace_ops_list != &ftrace_list_end) {
-			if (ftrace_ops_list->next == &ftrace_list_end)
-				ftrace_trace_function = ftrace_ops_list->func;
-			else
-				ftrace_trace_function = ftrace_ops_list_func;
-		}
+		if (ftrace_ops_list != &ftrace_list_end)
+			update_ftrace_function();
 
 	} else {
 		/* stopping ftrace calls (just send to ftrace_stub) */
diff --git a/kernel/trace/ring_buffer.c b/kernel/trace/ring_buffer.c
index 6989df2ba19..b59aea2c48c 100644
--- a/kernel/trace/ring_buffer.c
+++ b/kernel/trace/ring_buffer.c
@@ -8,13 +8,16 @@
 #include <linux/trace_clock.h>
 #include <linux/trace_seq.h>
 #include <linux/spinlock.h>
+#include <linux/irq_work.h>
 #include <linux/debugfs.h>
 #include <linux/uaccess.h>
 #include <linux/hardirq.h>
+#include <linux/kthread.h>	/* for self test */
 #include <linux/kmemcheck.h>
 #include <linux/module.h>
 #include <linux/percpu.h>
 #include <linux/mutex.h>
+#include <linux/delay.h>
 #include <linux/slab.h>
 #include <linux/init.h>
 #include <linux/hash.h>
@@ -444,6 +447,12 @@ int ring_buffer_print_page_header(struct trace_seq *s)
 	return ret;
 }
 
+struct rb_irq_work {
+	struct irq_work			work;
+	wait_queue_head_t		waiters;
+	bool				waiters_pending;
+};
+
 /*
  * head_page == tail_page && head == tail then buffer is empty.
  */
@@ -478,6 +487,8 @@ struct ring_buffer_per_cpu {
 	struct list_head		new_pages; /* new pages to add */
 	struct work_struct		update_pages_work;
 	struct completion		update_done;
+
+	struct rb_irq_work		irq_work;
 };
 
 struct ring_buffer {
@@ -497,6 +508,8 @@ struct ring_buffer {
 	struct notifier_block		cpu_notify;
 #endif
 	u64				(*clock)(void);
+
+	struct rb_irq_work		irq_work;
 };
 
 struct ring_buffer_iter {
@@ -508,6 +521,118 @@ struct ring_buffer_iter {
 	u64				read_stamp;
 };
 
+/*
+ * rb_wake_up_waiters - wake up tasks waiting for ring buffer input
+ *
+ * Schedules a delayed work to wake up any task that is blocked on the
+ * ring buffer waiters queue.
+ */
+static void rb_wake_up_waiters(struct irq_work *work)
+{
+	struct rb_irq_work *rbwork = container_of(work, struct rb_irq_work, work);
+
+	wake_up_all(&rbwork->waiters);
+}
+
+/**
+ * ring_buffer_wait - wait for input to the ring buffer
+ * @buffer: buffer to wait on
+ * @cpu: the cpu buffer to wait on
+ *
+ * If @cpu == RING_BUFFER_ALL_CPUS then the task will wake up as soon
+ * as data is added to any of the @buffer's cpu buffers. Otherwise
+ * it will wait for data to be added to a specific cpu buffer.
+ */
+void ring_buffer_wait(struct ring_buffer *buffer, int cpu)
+{
+	struct ring_buffer_per_cpu *cpu_buffer;
+	DEFINE_WAIT(wait);
+	struct rb_irq_work *work;
+
+	/*
+	 * Depending on what the caller is waiting for, either any
+	 * data in any cpu buffer, or a specific buffer, put the
+	 * caller on the appropriate wait queue.
+	 */
+	if (cpu == RING_BUFFER_ALL_CPUS)
+		work = &buffer->irq_work;
+	else {
+		cpu_buffer = buffer->buffers[cpu];
+		work = &cpu_buffer->irq_work;
+	}
+
+
+	prepare_to_wait(&work->waiters, &wait, TASK_INTERRUPTIBLE);
+
+	/*
+	 * The events can happen in critical sections where
+	 * checking a work queue can cause deadlocks.
+	 * After adding a task to the queue, this flag is set
+	 * only to notify events to try to wake up the queue
+	 * using irq_work.
+	 *
+	 * We don't clear it even if the buffer is no longer
+	 * empty. The flag only causes the next event to run
+	 * irq_work to do the work queue wake up. The worse
+	 * that can happen if we race with !trace_empty() is that
+	 * an event will cause an irq_work to try to wake up
+	 * an empty queue.
+	 *
+	 * There's no reason to protect this flag either, as
+	 * the work queue and irq_work logic will do the necessary
+	 * synchronization for the wake ups. The only thing
+	 * that is necessary is that the wake up happens after
+	 * a task has been queued. It's OK for spurious wake ups.
+	 */
+	work->waiters_pending = true;
+
+	if ((cpu == RING_BUFFER_ALL_CPUS && ring_buffer_empty(buffer)) ||
+	    (cpu != RING_BUFFER_ALL_CPUS && ring_buffer_empty_cpu(buffer, cpu)))
+		schedule();
+
+	finish_wait(&work->waiters, &wait);
+}
+
+/**
+ * ring_buffer_poll_wait - poll on buffer input
+ * @buffer: buffer to wait on
+ * @cpu: the cpu buffer to wait on
+ * @filp: the file descriptor
+ * @poll_table: The poll descriptor
+ *
+ * If @cpu == RING_BUFFER_ALL_CPUS then the task will wake up as soon
+ * as data is added to any of the @buffer's cpu buffers. Otherwise
+ * it will wait for data to be added to a specific cpu buffer.
+ *
+ * Returns POLLIN | POLLRDNORM if data exists in the buffers,
+ * zero otherwise.
+ */
+int ring_buffer_poll_wait(struct ring_buffer *buffer, int cpu,
+			  struct file *filp, poll_table *poll_table)
+{
+	struct ring_buffer_per_cpu *cpu_buffer;
+	struct rb_irq_work *work;
+
+	if ((cpu == RING_BUFFER_ALL_CPUS && !ring_buffer_empty(buffer)) ||
+	    (cpu != RING_BUFFER_ALL_CPUS && !ring_buffer_empty_cpu(buffer, cpu)))
+		return POLLIN | POLLRDNORM;
+
+	if (cpu == RING_BUFFER_ALL_CPUS)
+		work = &buffer->irq_work;
+	else {
+		cpu_buffer = buffer->buffers[cpu];
+		work = &cpu_buffer->irq_work;
+	}
+
+	work->waiters_pending = true;
+	poll_wait(filp, &work->waiters, poll_table);
+
+	if ((cpu == RING_BUFFER_ALL_CPUS && !ring_buffer_empty(buffer)) ||
+	    (cpu != RING_BUFFER_ALL_CPUS && !ring_buffer_empty_cpu(buffer, cpu)))
+		return POLLIN | POLLRDNORM;
+	return 0;
+}
+
 /* buffer may be either ring_buffer or ring_buffer_per_cpu */
 #define RB_WARN_ON(b, cond)						\
 	({								\
@@ -1063,6 +1188,8 @@ rb_allocate_cpu_buffer(struct ring_buffer *buffer, int nr_pages, int cpu)
 	cpu_buffer->lock = (arch_spinlock_t)__ARCH_SPIN_LOCK_UNLOCKED;
 	INIT_WORK(&cpu_buffer->update_pages_work, update_pages_handler);
 	init_completion(&cpu_buffer->update_done);
+	init_irq_work(&cpu_buffer->irq_work.work, rb_wake_up_waiters);
+	init_waitqueue_head(&cpu_buffer->irq_work.waiters);
 
 	bpage = kzalloc_node(ALIGN(sizeof(*bpage), cache_line_size()),
 			    GFP_KERNEL, cpu_to_node(cpu));
@@ -1158,6 +1285,9 @@ struct ring_buffer *__ring_buffer_alloc(unsigned long size, unsigned flags,
 	buffer->clock = trace_clock_local;
 	buffer->reader_lock_key = key;
 
+	init_irq_work(&buffer->irq_work.work, rb_wake_up_waiters);
+	init_waitqueue_head(&buffer->irq_work.waiters);
+
 	/* need at least two pages */
 	if (nr_pages < 2)
 		nr_pages = 2;
@@ -1553,11 +1683,22 @@ int ring_buffer_resize(struct ring_buffer *buffer, unsigned long size,
 			if (!cpu_buffer->nr_pages_to_update)
 				continue;
 
-			if (cpu_online(cpu))
+			/* The update must run on the CPU that is being updated. */
+			preempt_disable();
+			if (cpu == smp_processor_id() || !cpu_online(cpu)) {
+				rb_update_pages(cpu_buffer);
+				cpu_buffer->nr_pages_to_update = 0;
+			} else {
+				/*
+				 * Can not disable preemption for schedule_work_on()
+				 * on PREEMPT_RT.
+				 */
+				preempt_enable();
 				schedule_work_on(cpu,
 						&cpu_buffer->update_pages_work);
-			else
-				rb_update_pages(cpu_buffer);
+				preempt_disable();
+			}
+			preempt_enable();
 		}
 
 		/* wait for all the updates to complete */
@@ -1595,12 +1736,22 @@ int ring_buffer_resize(struct ring_buffer *buffer, unsigned long size,
 
 		get_online_cpus();
 
-		if (cpu_online(cpu_id)) {
+		preempt_disable();
+		/* The update must run on the CPU that is being updated. */
+		if (cpu_id == smp_processor_id() || !cpu_online(cpu_id))
+			rb_update_pages(cpu_buffer);
+		else {
+			/*
+			 * Can not disable preemption for schedule_work_on()
+			 * on PREEMPT_RT.
+			 */
+			preempt_enable();
 			schedule_work_on(cpu_id,
 					 &cpu_buffer->update_pages_work);
 			wait_for_completion(&cpu_buffer->update_done);
-		} else
-			rb_update_pages(cpu_buffer);
+			preempt_disable();
+		}
+		preempt_enable();
 
 		cpu_buffer->nr_pages_to_update = 0;
 		put_online_cpus();
@@ -2612,6 +2763,22 @@ static void rb_commit(struct ring_buffer_per_cpu *cpu_buffer,
 	rb_end_commit(cpu_buffer);
 }
 
+static __always_inline void
+rb_wakeups(struct ring_buffer *buffer, struct ring_buffer_per_cpu *cpu_buffer)
+{
+	if (buffer->irq_work.waiters_pending) {
+		buffer->irq_work.waiters_pending = false;
+		/* irq_work_queue() supplies it's own memory barriers */
+		irq_work_queue(&buffer->irq_work.work);
+	}
+
+	if (cpu_buffer->irq_work.waiters_pending) {
+		cpu_buffer->irq_work.waiters_pending = false;
+		/* irq_work_queue() supplies it's own memory barriers */
+		irq_work_queue(&cpu_buffer->irq_work.work);
+	}
+}
+
 /**
  * ring_buffer_unlock_commit - commit a reserved
  * @buffer: The buffer to commit to
@@ -2631,6 +2798,8 @@ int ring_buffer_unlock_commit(struct ring_buffer *buffer,
 
 	rb_commit(cpu_buffer, event);
 
+	rb_wakeups(buffer, cpu_buffer);
+
 	trace_recursive_unlock();
 
 	preempt_enable_notrace();
@@ -2803,6 +2972,8 @@ int ring_buffer_write(struct ring_buffer *buffer,
 
 	rb_commit(cpu_buffer, event);
 
+	rb_wakeups(buffer, cpu_buffer);
+
 	ret = 0;
  out:
 	preempt_enable_notrace();
@@ -4467,3 +4638,320 @@ static int rb_cpu_notify(struct notifier_block *self,
 	return NOTIFY_OK;
 }
 #endif
+
+#ifdef CONFIG_RING_BUFFER_STARTUP_TEST
+/*
+ * This is a basic integrity check of the ring buffer.
+ * Late in the boot cycle this test will run when configured in.
+ * It will kick off a thread per CPU that will go into a loop
+ * writing to the per cpu ring buffer various sizes of data.
+ * Some of the data will be large items, some small.
+ *
+ * Another thread is created that goes into a spin, sending out
+ * IPIs to the other CPUs to also write into the ring buffer.
+ * this is to test the nesting ability of the buffer.
+ *
+ * Basic stats are recorded and reported. If something in the
+ * ring buffer should happen that's not expected, a big warning
+ * is displayed and all ring buffers are disabled.
+ */
+static struct task_struct *rb_threads[NR_CPUS] __initdata;
+
+struct rb_test_data {
+	struct ring_buffer	*buffer;
+	unsigned long		events;
+	unsigned long		bytes_written;
+	unsigned long		bytes_alloc;
+	unsigned long		bytes_dropped;
+	unsigned long		events_nested;
+	unsigned long		bytes_written_nested;
+	unsigned long		bytes_alloc_nested;
+	unsigned long		bytes_dropped_nested;
+	int			min_size_nested;
+	int			max_size_nested;
+	int			max_size;
+	int			min_size;
+	int			cpu;
+	int			cnt;
+};
+
+static struct rb_test_data rb_data[NR_CPUS] __initdata;
+
+/* 1 meg per cpu */
+#define RB_TEST_BUFFER_SIZE	1048576
+
+static char rb_string[] __initdata =
+	"abcdefghijklmnopqrstuvwxyz1234567890!@#$%^&*()?+\\"
+	"?+|:';\",.<>/?abcdefghijklmnopqrstuvwxyz1234567890"
+	"!@#$%^&*()?+\\?+|:';\",.<>/?abcdefghijklmnopqrstuv";
+
+static bool rb_test_started __initdata;
+
+struct rb_item {
+	int size;
+	char str[];
+};
+
+static __init int rb_write_something(struct rb_test_data *data, bool nested)
+{
+	struct ring_buffer_event *event;
+	struct rb_item *item;
+	bool started;
+	int event_len;
+	int size;
+	int len;
+	int cnt;
+
+	/* Have nested writes different that what is written */
+	cnt = data->cnt + (nested ? 27 : 0);
+
+	/* Multiply cnt by ~e, to make some unique increment */
+	size = (data->cnt * 68 / 25) % (sizeof(rb_string) - 1);
+
+	len = size + sizeof(struct rb_item);
+
+	started = rb_test_started;
+	/* read rb_test_started before checking buffer enabled */
+	smp_rmb();
+
+	event = ring_buffer_lock_reserve(data->buffer, len);
+	if (!event) {
+		/* Ignore dropped events before test starts. */
+		if (started) {
+			if (nested)
+				data->bytes_dropped += len;
+			else
+				data->bytes_dropped_nested += len;
+		}
+		return len;
+	}
+
+	event_len = ring_buffer_event_length(event);
+
+	if (RB_WARN_ON(data->buffer, event_len < len))
+		goto out;
+
+	item = ring_buffer_event_data(event);
+	item->size = size;
+	memcpy(item->str, rb_string, size);
+
+	if (nested) {
+		data->bytes_alloc_nested += event_len;
+		data->bytes_written_nested += len;
+		data->events_nested++;
+		if (!data->min_size_nested || len < data->min_size_nested)
+			data->min_size_nested = len;
+		if (len > data->max_size_nested)
+			data->max_size_nested = len;
+	} else {
+		data->bytes_alloc += event_len;
+		data->bytes_written += len;
+		data->events++;
+		if (!data->min_size || len < data->min_size)
+			data->max_size = len;
+		if (len > data->max_size)
+			data->max_size = len;
+	}
+
+ out:
+	ring_buffer_unlock_commit(data->buffer, event);
+
+	return 0;
+}
+
+static __init int rb_test(void *arg)
+{
+	struct rb_test_data *data = arg;
+
+	while (!kthread_should_stop()) {
+		rb_write_something(data, false);
+		data->cnt++;
+
+		set_current_state(TASK_INTERRUPTIBLE);
+		/* Now sleep between a min of 100-300us and a max of 1ms */
+		usleep_range(((data->cnt % 3) + 1) * 100, 1000);
+	}
+
+	return 0;
+}
+
+static __init void rb_ipi(void *ignore)
+{
+	struct rb_test_data *data;
+	int cpu = smp_processor_id();
+
+	data = &rb_data[cpu];
+	rb_write_something(data, true);
+}
+
+static __init int rb_hammer_test(void *arg)
+{
+	while (!kthread_should_stop()) {
+
+		/* Send an IPI to all cpus to write data! */
+		smp_call_function(rb_ipi, NULL, 1);
+		/* No sleep, but for non preempt, let others run */
+		schedule();
+	}
+
+	return 0;
+}
+
+static __init int test_ringbuffer(void)
+{
+	struct task_struct *rb_hammer;
+	struct ring_buffer *buffer;
+	int cpu;
+	int ret = 0;
+
+	pr_info("Running ring buffer tests...\n");
+
+	buffer = ring_buffer_alloc(RB_TEST_BUFFER_SIZE, RB_FL_OVERWRITE);
+	if (WARN_ON(!buffer))
+		return 0;
+
+	/* Disable buffer so that threads can't write to it yet */
+	ring_buffer_record_off(buffer);
+
+	for_each_online_cpu(cpu) {
+		rb_data[cpu].buffer = buffer;
+		rb_data[cpu].cpu = cpu;
+		rb_data[cpu].cnt = cpu;
+		rb_threads[cpu] = kthread_create(rb_test, &rb_data[cpu],
+						 "rbtester/%d", cpu);
+		if (WARN_ON(!rb_threads[cpu])) {
+			pr_cont("FAILED\n");
+			ret = -1;
+			goto out_free;
+		}
+
+		kthread_bind(rb_threads[cpu], cpu);
+ 		wake_up_process(rb_threads[cpu]);
+	}
+
+	/* Now create the rb hammer! */
+	rb_hammer = kthread_run(rb_hammer_test, NULL, "rbhammer");
+	if (WARN_ON(!rb_hammer)) {
+		pr_cont("FAILED\n");
+		ret = -1;
+		goto out_free;
+	}
+
+	ring_buffer_record_on(buffer);
+	/*
+	 * Show buffer is enabled before setting rb_test_started.
+	 * Yes there's a small race window where events could be
+	 * dropped and the thread wont catch it. But when a ring
+	 * buffer gets enabled, there will always be some kind of
+	 * delay before other CPUs see it. Thus, we don't care about
+	 * those dropped events. We care about events dropped after
+	 * the threads see that the buffer is active.
+	 */
+	smp_wmb();
+	rb_test_started = true;
+
+	set_current_state(TASK_INTERRUPTIBLE);
+	/* Just run for 10 seconds */;
+	schedule_timeout(10 * HZ);
+
+	kthread_stop(rb_hammer);
+
+ out_free:
+	for_each_online_cpu(cpu) {
+		if (!rb_threads[cpu])
+			break;
+		kthread_stop(rb_threads[cpu]);
+	}
+	if (ret) {
+		ring_buffer_free(buffer);
+		return ret;
+	}
+
+	/* Report! */
+	pr_info("finished\n");
+	for_each_online_cpu(cpu) {
+		struct ring_buffer_event *event;
+		struct rb_test_data *data = &rb_data[cpu];
+		struct rb_item *item;
+		unsigned long total_events;
+		unsigned long total_dropped;
+		unsigned long total_written;
+		unsigned long total_alloc;
+		unsigned long total_read = 0;
+		unsigned long total_size = 0;
+		unsigned long total_len = 0;
+		unsigned long total_lost = 0;
+		unsigned long lost;
+		int big_event_size;
+		int small_event_size;
+
+		ret = -1;
+
+		total_events = data->events + data->events_nested;
+		total_written = data->bytes_written + data->bytes_written_nested;
+		total_alloc = data->bytes_alloc + data->bytes_alloc_nested;
+		total_dropped = data->bytes_dropped + data->bytes_dropped_nested;
+
+		big_event_size = data->max_size + data->max_size_nested;
+		small_event_size = data->min_size + data->min_size_nested;
+
+		pr_info("CPU %d:\n", cpu);
+		pr_info("              events:    %ld\n", total_events);
+		pr_info("       dropped bytes:    %ld\n", total_dropped);
+		pr_info("       alloced bytes:    %ld\n", total_alloc);
+		pr_info("       written bytes:    %ld\n", total_written);
+		pr_info("       biggest event:    %d\n", big_event_size);
+		pr_info("      smallest event:    %d\n", small_event_size);
+
+		if (RB_WARN_ON(buffer, total_dropped))
+			break;
+
+		ret = 0;
+
+		while ((event = ring_buffer_consume(buffer, cpu, NULL, &lost))) {
+			total_lost += lost;
+			item = ring_buffer_event_data(event);
+			total_len += ring_buffer_event_length(event);
+			total_size += item->size + sizeof(struct rb_item);
+			if (memcmp(&item->str[0], rb_string, item->size) != 0) {
+				pr_info("FAILED!\n");
+				pr_info("buffer had: %.*s\n", item->size, item->str);
+				pr_info("expected:   %.*s\n", item->size, rb_string);
+				RB_WARN_ON(buffer, 1);
+				ret = -1;
+				break;
+			}
+			total_read++;
+		}
+		if (ret)
+			break;
+
+		ret = -1;
+
+		pr_info("         read events:   %ld\n", total_read);
+		pr_info("         lost events:   %ld\n", total_lost);
+		pr_info("        total events:   %ld\n", total_lost + total_read);
+		pr_info("  recorded len bytes:   %ld\n", total_len);
+		pr_info(" recorded size bytes:   %ld\n", total_size);
+		if (total_lost)
+			pr_info(" With dropped events, record len and size may not match\n"
+				" alloced and written from above\n");
+		if (!total_lost) {
+			if (RB_WARN_ON(buffer, total_len != total_alloc ||
+				       total_size != total_written))
+				break;
+		}
+		if (RB_WARN_ON(buffer, total_lost + total_read != total_events))
+			break;
+
+		ret = 0;
+	}
+	if (!ret)
+		pr_info("Ring buffer PASSED!\n");
+
+	ring_buffer_free(buffer);
+	return 0;
+}
+
+late_initcall(test_ringbuffer);
+#endif /* CONFIG_RING_BUFFER_STARTUP_TEST */
diff --git a/kernel/trace/trace.c b/kernel/trace/trace.c
index 4f1dade5698..ae6fa2d1cdf 100644
--- a/kernel/trace/trace.c
+++ b/kernel/trace/trace.c
@@ -1,7 +1,7 @@
 /*
  * ring buffer based function tracer
  *
- * Copyright (C) 2007-2008 Steven Rostedt <srostedt@redhat.com>
+ * Copyright (C) 2007-2012 Steven Rostedt <srostedt@redhat.com>
  * Copyright (C) 2008 Ingo Molnar <mingo@redhat.com>
  *
  * Originally taken from the RT patch by:
@@ -19,7 +19,6 @@
 #include <linux/seq_file.h>
 #include <linux/notifier.h>
 #include <linux/irqflags.h>
-#include <linux/irq_work.h>
 #include <linux/debugfs.h>
 #include <linux/pagemap.h>
 #include <linux/hardirq.h>
@@ -48,7 +47,7 @@
  * On boot up, the ring buffer is set to the minimum size, so that
  * we do not waste memory on systems that are not using tracing.
  */
-int ring_buffer_expanded;
+bool ring_buffer_expanded;
 
 /*
  * We need to change this state when a selftest is running.
@@ -87,14 +86,6 @@ static int dummy_set_flag(u32 old_flags, u32 bit, int set)
 static DEFINE_PER_CPU(bool, trace_cmdline_save);
 
 /*
- * When a reader is waiting for data, then this variable is
- * set to true.
- */
-static bool trace_wakeup_needed;
-
-static struct irq_work trace_work_wakeup;
-
-/*
  * Kill all tracing for good (never come back).
  * It is initialized to 1 but will turn to zero if the initialization
  * of the tracer is successful. But that is the only place that sets
@@ -130,12 +121,14 @@ static int tracing_set_tracer(const char *buf);
 static char bootup_tracer_buf[MAX_TRACER_SIZE] __initdata;
 static char *default_bootup_tracer;
 
+static bool allocate_snapshot;
+
 static int __init set_cmdline_ftrace(char *str)
 {
-	strncpy(bootup_tracer_buf, str, MAX_TRACER_SIZE);
+	strlcpy(bootup_tracer_buf, str, MAX_TRACER_SIZE);
 	default_bootup_tracer = bootup_tracer_buf;
 	/* We are using ftrace early, expand it */
-	ring_buffer_expanded = 1;
+	ring_buffer_expanded = true;
 	return 1;
 }
 __setup("ftrace=", set_cmdline_ftrace);
@@ -156,13 +149,22 @@ static int __init set_ftrace_dump_on_oops(char *str)
 }
 __setup("ftrace_dump_on_oops", set_ftrace_dump_on_oops);
 
+static int __init boot_alloc_snapshot(char *str)
+{
+	allocate_snapshot = true;
+	/* We also need the main ring buffer expanded */
+	ring_buffer_expanded = true;
+	return 1;
+}
+__setup("alloc_snapshot", boot_alloc_snapshot);
+
 
 static char trace_boot_options_buf[MAX_TRACER_SIZE] __initdata;
 static char *trace_boot_options __initdata;
 
 static int __init set_trace_boot_options(char *str)
 {
-	strncpy(trace_boot_options_buf, str, MAX_TRACER_SIZE);
+	strlcpy(trace_boot_options_buf, str, MAX_TRACER_SIZE);
 	trace_boot_options = trace_boot_options_buf;
 	return 0;
 }
@@ -189,7 +191,7 @@ unsigned long long ns2usecs(cycle_t nsec)
  */
 static struct trace_array	global_trace;
 
-static DEFINE_PER_CPU(struct trace_array_cpu, global_trace_cpu);
+LIST_HEAD(ftrace_trace_arrays);
 
 int filter_current_check_discard(struct ring_buffer *buffer,
 				 struct ftrace_event_call *call, void *rec,
@@ -204,29 +206,15 @@ cycle_t ftrace_now(int cpu)
 	u64 ts;
 
 	/* Early boot up does not have a buffer yet */
-	if (!global_trace.buffer)
+	if (!global_trace.trace_buffer.buffer)
 		return trace_clock_local();
 
-	ts = ring_buffer_time_stamp(global_trace.buffer, cpu);
-	ring_buffer_normalize_time_stamp(global_trace.buffer, cpu, &ts);
+	ts = ring_buffer_time_stamp(global_trace.trace_buffer.buffer, cpu);
+	ring_buffer_normalize_time_stamp(global_trace.trace_buffer.buffer, cpu, &ts);
 
 	return ts;
 }
 
-/*
- * The max_tr is used to snapshot the global_trace when a maximum
- * latency is reached. Some tracers will use this to store a maximum
- * trace while it continues examining live traces.
- *
- * The buffers for the max_tr are set up the same as the global_trace.
- * When a snapshot is taken, the link list of the max_tr is swapped
- * with the link list of the global_trace and the buffers are reset for
- * the global_trace so the tracing can continue.
- */
-static struct trace_array	max_tr;
-
-static DEFINE_PER_CPU(struct trace_array_cpu, max_tr_data);
-
 int tracing_is_enabled(void)
 {
 	return tracing_is_on();
@@ -249,9 +237,6 @@ static unsigned long		trace_buf_size = TRACE_BUF_SIZE_DEFAULT;
 /* trace_types holds a link list of available tracers. */
 static struct tracer		*trace_types __read_mostly;
 
-/* current_trace points to the tracer that is currently active */
-static struct tracer		*current_trace __read_mostly = &nop_trace;
-
 /*
  * trace_types_lock is used to protect the trace_types list.
  */
@@ -285,13 +270,13 @@ static DEFINE_PER_CPU(struct mutex, cpu_access_lock);
 
 static inline void trace_access_lock(int cpu)
 {
-	if (cpu == TRACE_PIPE_ALL_CPU) {
+	if (cpu == RING_BUFFER_ALL_CPUS) {
 		/* gain it for accessing the whole ring buffer. */
 		down_write(&all_cpu_access_lock);
 	} else {
 		/* gain it for accessing a cpu ring buffer. */
 
-		/* Firstly block other trace_access_lock(TRACE_PIPE_ALL_CPU). */
+		/* Firstly block other trace_access_lock(RING_BUFFER_ALL_CPUS). */
 		down_read(&all_cpu_access_lock);
 
 		/* Secondly block other access to this @cpu ring buffer. */
@@ -301,7 +286,7 @@ static inline void trace_access_lock(int cpu)
 
 static inline void trace_access_unlock(int cpu)
 {
-	if (cpu == TRACE_PIPE_ALL_CPU) {
+	if (cpu == RING_BUFFER_ALL_CPUS) {
 		up_write(&all_cpu_access_lock);
 	} else {
 		mutex_unlock(&per_cpu(cpu_access_lock, cpu));
@@ -339,30 +324,11 @@ static inline void trace_access_lock_init(void)
 
 #endif
 
-/* trace_wait is a waitqueue for tasks blocked on trace_poll */
-static DECLARE_WAIT_QUEUE_HEAD(trace_wait);
-
 /* trace_flags holds trace_options default values */
 unsigned long trace_flags = TRACE_ITER_PRINT_PARENT | TRACE_ITER_PRINTK |
 	TRACE_ITER_ANNOTATE | TRACE_ITER_CONTEXT_INFO | TRACE_ITER_SLEEP_TIME |
 	TRACE_ITER_GRAPH_TIME | TRACE_ITER_RECORD_CMD | TRACE_ITER_OVERWRITE |
-	TRACE_ITER_IRQ_INFO | TRACE_ITER_MARKERS;
-
-static int trace_stop_count;
-static DEFINE_RAW_SPINLOCK(tracing_start_lock);
-
-/**
- * trace_wake_up - wake up tasks waiting for trace input
- *
- * Schedules a delayed work to wake up any task that is blocked on the
- * trace_wait queue. These is used with trace_poll for tasks polling the
- * trace.
- */
-static void trace_wake_up(struct irq_work *work)
-{
-	wake_up_all(&trace_wait);
-
-}
+	TRACE_ITER_IRQ_INFO | TRACE_ITER_MARKERS | TRACE_ITER_FUNCTION;
 
 /**
  * tracing_on - enable tracing buffers
@@ -372,8 +338,8 @@ static void trace_wake_up(struct irq_work *work)
  */
 void tracing_on(void)
 {
-	if (global_trace.buffer)
-		ring_buffer_record_on(global_trace.buffer);
+	if (global_trace.trace_buffer.buffer)
+		ring_buffer_record_on(global_trace.trace_buffer.buffer);
 	/*
 	 * This flag is only looked at when buffers haven't been
 	 * allocated yet. We don't really care about the race
@@ -385,6 +351,196 @@ void tracing_on(void)
 EXPORT_SYMBOL_GPL(tracing_on);
 
 /**
+ * __trace_puts - write a constant string into the trace buffer.
+ * @ip:	   The address of the caller
+ * @str:   The constant string to write
+ * @size:  The size of the string.
+ */
+int __trace_puts(unsigned long ip, const char *str, int size)
+{
+	struct ring_buffer_event *event;
+	struct ring_buffer *buffer;
+	struct print_entry *entry;
+	unsigned long irq_flags;
+	int alloc;
+
+	alloc = sizeof(*entry) + size + 2; /* possible \n added */
+
+	local_save_flags(irq_flags);
+	buffer = global_trace.trace_buffer.buffer;
+	event = trace_buffer_lock_reserve(buffer, TRACE_PRINT, alloc, 
+					  irq_flags, preempt_count());
+	if (!event)
+		return 0;
+
+	entry = ring_buffer_event_data(event);
+	entry->ip = ip;
+
+	memcpy(&entry->buf, str, size);
+
+	/* Add a newline if necessary */
+	if (entry->buf[size - 1] != '\n') {
+		entry->buf[size] = '\n';
+		entry->buf[size + 1] = '\0';
+	} else
+		entry->buf[size] = '\0';
+
+	__buffer_unlock_commit(buffer, event);
+
+	return size;
+}
+EXPORT_SYMBOL_GPL(__trace_puts);
+
+/**
+ * __trace_bputs - write the pointer to a constant string into trace buffer
+ * @ip:	   The address of the caller
+ * @str:   The constant string to write to the buffer to
+ */
+int __trace_bputs(unsigned long ip, const char *str)
+{
+	struct ring_buffer_event *event;
+	struct ring_buffer *buffer;
+	struct bputs_entry *entry;
+	unsigned long irq_flags;
+	int size = sizeof(struct bputs_entry);
+
+	local_save_flags(irq_flags);
+	buffer = global_trace.trace_buffer.buffer;
+	event = trace_buffer_lock_reserve(buffer, TRACE_BPUTS, size,
+					  irq_flags, preempt_count());
+	if (!event)
+		return 0;
+
+	entry = ring_buffer_event_data(event);
+	entry->ip			= ip;
+	entry->str			= str;
+
+	__buffer_unlock_commit(buffer, event);
+
+	return 1;
+}
+EXPORT_SYMBOL_GPL(__trace_bputs);
+
+#ifdef CONFIG_TRACER_SNAPSHOT
+/**
+ * trace_snapshot - take a snapshot of the current buffer.
+ *
+ * This causes a swap between the snapshot buffer and the current live
+ * tracing buffer. You can use this to take snapshots of the live
+ * trace when some condition is triggered, but continue to trace.
+ *
+ * Note, make sure to allocate the snapshot with either
+ * a tracing_snapshot_alloc(), or by doing it manually
+ * with: echo 1 > /sys/kernel/debug/tracing/snapshot
+ *
+ * If the snapshot buffer is not allocated, it will stop tracing.
+ * Basically making a permanent snapshot.
+ */
+void tracing_snapshot(void)
+{
+	struct trace_array *tr = &global_trace;
+	struct tracer *tracer = tr->current_trace;
+	unsigned long flags;
+
+	if (in_nmi()) {
+		internal_trace_puts("*** SNAPSHOT CALLED FROM NMI CONTEXT ***\n");
+		internal_trace_puts("*** snapshot is being ignored        ***\n");
+		return;
+	}
+
+	if (!tr->allocated_snapshot) {
+		internal_trace_puts("*** SNAPSHOT NOT ALLOCATED ***\n");
+		internal_trace_puts("*** stopping trace here!   ***\n");
+		tracing_off();
+		return;
+	}
+
+	/* Note, snapshot can not be used when the tracer uses it */
+	if (tracer->use_max_tr) {
+		internal_trace_puts("*** LATENCY TRACER ACTIVE ***\n");
+		internal_trace_puts("*** Can not use snapshot (sorry) ***\n");
+		return;
+	}
+
+	local_irq_save(flags);
+	update_max_tr(tr, current, smp_processor_id());
+	local_irq_restore(flags);
+}
+EXPORT_SYMBOL_GPL(tracing_snapshot);
+
+static int resize_buffer_duplicate_size(struct trace_buffer *trace_buf,
+					struct trace_buffer *size_buf, int cpu_id);
+static void set_buffer_entries(struct trace_buffer *buf, unsigned long val);
+
+static int alloc_snapshot(struct trace_array *tr)
+{
+	int ret;
+
+	if (!tr->allocated_snapshot) {
+
+		/* allocate spare buffer */
+		ret = resize_buffer_duplicate_size(&tr->max_buffer,
+				   &tr->trace_buffer, RING_BUFFER_ALL_CPUS);
+		if (ret < 0)
+			return ret;
+
+		tr->allocated_snapshot = true;
+	}
+
+	return 0;
+}
+
+void free_snapshot(struct trace_array *tr)
+{
+	/*
+	 * We don't free the ring buffer. instead, resize it because
+	 * The max_tr ring buffer has some state (e.g. ring->clock) and
+	 * we want preserve it.
+	 */
+	ring_buffer_resize(tr->max_buffer.buffer, 1, RING_BUFFER_ALL_CPUS);
+	set_buffer_entries(&tr->max_buffer, 1);
+	tracing_reset_online_cpus(&tr->max_buffer);
+	tr->allocated_snapshot = false;
+}
+
+/**
+ * trace_snapshot_alloc - allocate and take a snapshot of the current buffer.
+ *
+ * This is similar to trace_snapshot(), but it will allocate the
+ * snapshot buffer if it isn't already allocated. Use this only
+ * where it is safe to sleep, as the allocation may sleep.
+ *
+ * This causes a swap between the snapshot buffer and the current live
+ * tracing buffer. You can use this to take snapshots of the live
+ * trace when some condition is triggered, but continue to trace.
+ */
+void tracing_snapshot_alloc(void)
+{
+	struct trace_array *tr = &global_trace;
+	int ret;
+
+	ret = alloc_snapshot(tr);
+	if (WARN_ON(ret < 0))
+		return;
+
+	tracing_snapshot();
+}
+EXPORT_SYMBOL_GPL(tracing_snapshot_alloc);
+#else
+void tracing_snapshot(void)
+{
+	WARN_ONCE(1, "Snapshot feature not enabled, but internal snapshot used");
+}
+EXPORT_SYMBOL_GPL(tracing_snapshot);
+void tracing_snapshot_alloc(void)
+{
+	/* Give warning */
+	tracing_snapshot();
+}
+EXPORT_SYMBOL_GPL(tracing_snapshot_alloc);
+#endif /* CONFIG_TRACER_SNAPSHOT */
+
+/**
  * tracing_off - turn off tracing buffers
  *
  * This function stops the tracing buffers from recording data.
@@ -394,8 +550,8 @@ EXPORT_SYMBOL_GPL(tracing_on);
  */
 void tracing_off(void)
 {
-	if (global_trace.buffer)
-		ring_buffer_record_off(global_trace.buffer);
+	if (global_trace.trace_buffer.buffer)
+		ring_buffer_record_off(global_trace.trace_buffer.buffer);
 	/*
 	 * This flag is only looked at when buffers haven't been
 	 * allocated yet. We don't really care about the race
@@ -411,8 +567,8 @@ EXPORT_SYMBOL_GPL(tracing_off);
  */
 int tracing_is_on(void)
 {
-	if (global_trace.buffer)
-		return ring_buffer_record_is_on(global_trace.buffer);
+	if (global_trace.trace_buffer.buffer)
+		return ring_buffer_record_is_on(global_trace.trace_buffer.buffer);
 	return !global_trace.buffer_disabled;
 }
 EXPORT_SYMBOL_GPL(tracing_is_on);
@@ -479,6 +635,7 @@ static const char *trace_options[] = {
 	"disable_on_free",
 	"irq-info",
 	"markers",
+	"function-trace",
 	NULL
 };
 
@@ -490,6 +647,8 @@ static struct {
 	{ trace_clock_local,	"local",	1 },
 	{ trace_clock_global,	"global",	1 },
 	{ trace_clock_counter,	"counter",	0 },
+	{ trace_clock_jiffies,	"uptime",	1 },
+	{ trace_clock,		"perf",		1 },
 	ARCH_TRACE_CLOCKS
 };
 
@@ -670,13 +829,14 @@ unsigned long __read_mostly	tracing_max_latency;
 static void
 __update_max_tr(struct trace_array *tr, struct task_struct *tsk, int cpu)
 {
-	struct trace_array_cpu *data = tr->data[cpu];
-	struct trace_array_cpu *max_data;
+	struct trace_buffer *trace_buf = &tr->trace_buffer;
+	struct trace_buffer *max_buf = &tr->max_buffer;
+	struct trace_array_cpu *data = per_cpu_ptr(trace_buf->data, cpu);
+	struct trace_array_cpu *max_data = per_cpu_ptr(max_buf->data, cpu);
 
-	max_tr.cpu = cpu;
-	max_tr.time_start = data->preempt_timestamp;
+	max_buf->cpu = cpu;
+	max_buf->time_start = data->preempt_timestamp;
 
-	max_data = max_tr.data[cpu];
 	max_data->saved_latency = tracing_max_latency;
 	max_data->critical_start = data->critical_start;
 	max_data->critical_end = data->critical_end;
@@ -706,22 +866,22 @@ update_max_tr(struct trace_array *tr, struct task_struct *tsk, int cpu)
 {
 	struct ring_buffer *buf;
 
-	if (trace_stop_count)
+	if (tr->stop_count)
 		return;
 
 	WARN_ON_ONCE(!irqs_disabled());
 
-	if (!current_trace->allocated_snapshot) {
+	if (!tr->allocated_snapshot) {
 		/* Only the nop tracer should hit this when disabling */
-		WARN_ON_ONCE(current_trace != &nop_trace);
+		WARN_ON_ONCE(tr->current_trace != &nop_trace);
 		return;
 	}
 
 	arch_spin_lock(&ftrace_max_lock);
 
-	buf = tr->buffer;
-	tr->buffer = max_tr.buffer;
-	max_tr.buffer = buf;
+	buf = tr->trace_buffer.buffer;
+	tr->trace_buffer.buffer = tr->max_buffer.buffer;
+	tr->max_buffer.buffer = buf;
 
 	__update_max_tr(tr, tsk, cpu);
 	arch_spin_unlock(&ftrace_max_lock);
@@ -740,16 +900,19 @@ update_max_tr_single(struct trace_array *tr, struct task_struct *tsk, int cpu)
 {
 	int ret;
 
-	if (trace_stop_count)
+	if (tr->stop_count)
 		return;
 
 	WARN_ON_ONCE(!irqs_disabled());
-	if (WARN_ON_ONCE(!current_trace->allocated_snapshot))
+	if (!tr->allocated_snapshot) {
+		/* Only the nop tracer should hit this when disabling */
+		WARN_ON_ONCE(tr->current_trace != &nop_trace);
 		return;
+	}
 
 	arch_spin_lock(&ftrace_max_lock);
 
-	ret = ring_buffer_swap_cpu(max_tr.buffer, tr->buffer, cpu);
+	ret = ring_buffer_swap_cpu(tr->max_buffer.buffer, tr->trace_buffer.buffer, cpu);
 
 	if (ret == -EBUSY) {
 		/*
@@ -758,7 +921,7 @@ update_max_tr_single(struct trace_array *tr, struct task_struct *tsk, int cpu)
 		 * the max trace buffer (no one writes directly to it)
 		 * and flag that it failed.
 		 */
-		trace_array_printk(&max_tr, _THIS_IP_,
+		trace_array_printk_buf(tr->max_buffer.buffer, _THIS_IP_,
 			"Failed to swap buffers due to commit in progress\n");
 	}
 
@@ -771,37 +934,78 @@ update_max_tr_single(struct trace_array *tr, struct task_struct *tsk, int cpu)
 
 static void default_wait_pipe(struct trace_iterator *iter)
 {
-	DEFINE_WAIT(wait);
+	/* Iterators are static, they should be filled or empty */
+	if (trace_buffer_iter(iter, iter->cpu_file))
+		return;
+
+	ring_buffer_wait(iter->trace_buffer->buffer, iter->cpu_file);
+}
+
+#ifdef CONFIG_FTRACE_STARTUP_TEST
+static int run_tracer_selftest(struct tracer *type)
+{
+	struct trace_array *tr = &global_trace;
+	struct tracer *saved_tracer = tr->current_trace;
+	int ret;
 
-	prepare_to_wait(&trace_wait, &wait, TASK_INTERRUPTIBLE);
+	if (!type->selftest || tracing_selftest_disabled)
+		return 0;
 
 	/*
-	 * The events can happen in critical sections where
-	 * checking a work queue can cause deadlocks.
-	 * After adding a task to the queue, this flag is set
-	 * only to notify events to try to wake up the queue
-	 * using irq_work.
-	 *
-	 * We don't clear it even if the buffer is no longer
-	 * empty. The flag only causes the next event to run
-	 * irq_work to do the work queue wake up. The worse
-	 * that can happen if we race with !trace_empty() is that
-	 * an event will cause an irq_work to try to wake up
-	 * an empty queue.
-	 *
-	 * There's no reason to protect this flag either, as
-	 * the work queue and irq_work logic will do the necessary
-	 * synchronization for the wake ups. The only thing
-	 * that is necessary is that the wake up happens after
-	 * a task has been queued. It's OK for spurious wake ups.
+	 * Run a selftest on this tracer.
+	 * Here we reset the trace buffer, and set the current
+	 * tracer to be this tracer. The tracer can then run some
+	 * internal tracing to verify that everything is in order.
+	 * If we fail, we do not register this tracer.
 	 */
-	trace_wakeup_needed = true;
+	tracing_reset_online_cpus(&tr->trace_buffer);
 
-	if (trace_empty(iter))
-		schedule();
+	tr->current_trace = type;
+
+#ifdef CONFIG_TRACER_MAX_TRACE
+	if (type->use_max_tr) {
+		/* If we expanded the buffers, make sure the max is expanded too */
+		if (ring_buffer_expanded)
+			ring_buffer_resize(tr->max_buffer.buffer, trace_buf_size,
+					   RING_BUFFER_ALL_CPUS);
+		tr->allocated_snapshot = true;
+	}
+#endif
 
-	finish_wait(&trace_wait, &wait);
+	/* the test is responsible for initializing and enabling */
+	pr_info("Testing tracer %s: ", type->name);
+	ret = type->selftest(type, tr);
+	/* the test is responsible for resetting too */
+	tr->current_trace = saved_tracer;
+	if (ret) {
+		printk(KERN_CONT "FAILED!\n");
+		/* Add the warning after printing 'FAILED' */
+		WARN_ON(1);
+		return -1;
+	}
+	/* Only reset on passing, to avoid touching corrupted buffers */
+	tracing_reset_online_cpus(&tr->trace_buffer);
+
+#ifdef CONFIG_TRACER_MAX_TRACE
+	if (type->use_max_tr) {
+		tr->allocated_snapshot = false;
+
+		/* Shrink the max buffer again */
+		if (ring_buffer_expanded)
+			ring_buffer_resize(tr->max_buffer.buffer, 1,
+					   RING_BUFFER_ALL_CPUS);
+	}
+#endif
+
+	printk(KERN_CONT "PASSED\n");
+	return 0;
 }
+#else
+static inline int run_tracer_selftest(struct tracer *type)
+{
+	return 0;
+}
+#endif /* CONFIG_FTRACE_STARTUP_TEST */
 
 /**
  * register_tracer - register a tracer with the ftrace system.
@@ -848,57 +1052,9 @@ int register_tracer(struct tracer *type)
 	if (!type->wait_pipe)
 		type->wait_pipe = default_wait_pipe;
 
-
-#ifdef CONFIG_FTRACE_STARTUP_TEST
-	if (type->selftest && !tracing_selftest_disabled) {
-		struct tracer *saved_tracer = current_trace;
-		struct trace_array *tr = &global_trace;
-
-		/*
-		 * Run a selftest on this tracer.
-		 * Here we reset the trace buffer, and set the current
-		 * tracer to be this tracer. The tracer can then run some
-		 * internal tracing to verify that everything is in order.
-		 * If we fail, we do not register this tracer.
-		 */
-		tracing_reset_online_cpus(tr);
-
-		current_trace = type;
-
-		if (type->use_max_tr) {
-			/* If we expanded the buffers, make sure the max is expanded too */
-			if (ring_buffer_expanded)
-				ring_buffer_resize(max_tr.buffer, trace_buf_size,
-						   RING_BUFFER_ALL_CPUS);
-			type->allocated_snapshot = true;
-		}
-
-		/* the test is responsible for initializing and enabling */
-		pr_info("Testing tracer %s: ", type->name);
-		ret = type->selftest(type, tr);
-		/* the test is responsible for resetting too */
-		current_trace = saved_tracer;
-		if (ret) {
-			printk(KERN_CONT "FAILED!\n");
-			/* Add the warning after printing 'FAILED' */
-			WARN_ON(1);
-			goto out;
-		}
-		/* Only reset on passing, to avoid touching corrupted buffers */
-		tracing_reset_online_cpus(tr);
-
-		if (type->use_max_tr) {
-			type->allocated_snapshot = false;
-
-			/* Shrink the max buffer again */
-			if (ring_buffer_expanded)
-				ring_buffer_resize(max_tr.buffer, 1,
-						   RING_BUFFER_ALL_CPUS);
-		}
-
-		printk(KERN_CONT "PASSED\n");
-	}
-#endif
+	ret = run_tracer_selftest(type);
+	if (ret < 0)
+		goto out;
 
 	type->next = trace_types;
 	trace_types = type;
@@ -918,7 +1074,7 @@ int register_tracer(struct tracer *type)
 	tracing_set_tracer(type->name);
 	default_bootup_tracer = NULL;
 	/* disable other selftests, since this will break it. */
-	tracing_selftest_disabled = 1;
+	tracing_selftest_disabled = true;
 #ifdef CONFIG_FTRACE_STARTUP_TEST
 	printk(KERN_INFO "Disabling FTRACE selftests due to running tracer '%s'\n",
 	       type->name);
@@ -928,9 +1084,9 @@ int register_tracer(struct tracer *type)
 	return ret;
 }
 
-void tracing_reset(struct trace_array *tr, int cpu)
+void tracing_reset(struct trace_buffer *buf, int cpu)
 {
-	struct ring_buffer *buffer = tr->buffer;
+	struct ring_buffer *buffer = buf->buffer;
 
 	if (!buffer)
 		return;
@@ -944,9 +1100,9 @@ void tracing_reset(struct trace_array *tr, int cpu)
 	ring_buffer_record_enable(buffer);
 }
 
-void tracing_reset_online_cpus(struct trace_array *tr)
+void tracing_reset_online_cpus(struct trace_buffer *buf)
 {
-	struct ring_buffer *buffer = tr->buffer;
+	struct ring_buffer *buffer = buf->buffer;
 	int cpu;
 
 	if (!buffer)
@@ -957,7 +1113,7 @@ void tracing_reset_online_cpus(struct trace_array *tr)
 	/* Make sure all commits have finished */
 	synchronize_sched();
 
-	tr->time_start = ftrace_now(tr->cpu);
+	buf->time_start = ftrace_now(buf->cpu);
 
 	for_each_online_cpu(cpu)
 		ring_buffer_reset_cpu(buffer, cpu);
@@ -967,12 +1123,21 @@ void tracing_reset_online_cpus(struct trace_array *tr)
 
 void tracing_reset_current(int cpu)
 {
-	tracing_reset(&global_trace, cpu);
+	tracing_reset(&global_trace.trace_buffer, cpu);
 }
 
-void tracing_reset_current_online_cpus(void)
+void tracing_reset_all_online_cpus(void)
 {
-	tracing_reset_online_cpus(&global_trace);
+	struct trace_array *tr;
+
+	mutex_lock(&trace_types_lock);
+	list_for_each_entry(tr, &ftrace_trace_arrays, list) {
+		tracing_reset_online_cpus(&tr->trace_buffer);
+#ifdef CONFIG_TRACER_MAX_TRACE
+		tracing_reset_online_cpus(&tr->max_buffer);
+#endif
+	}
+	mutex_unlock(&trace_types_lock);
 }
 
 #define SAVED_CMDLINES 128
@@ -995,7 +1160,7 @@ static void trace_init_cmdlines(void)
 
 int is_tracing_stopped(void)
 {
-	return trace_stop_count;
+	return global_trace.stop_count;
 }
 
 /**
@@ -1027,12 +1192,12 @@ void tracing_start(void)
 	if (tracing_disabled)
 		return;
 
-	raw_spin_lock_irqsave(&tracing_start_lock, flags);
-	if (--trace_stop_count) {
-		if (trace_stop_count < 0) {
+	raw_spin_lock_irqsave(&global_trace.start_lock, flags);
+	if (--global_trace.stop_count) {
+		if (global_trace.stop_count < 0) {
 			/* Someone screwed up their debugging */
 			WARN_ON_ONCE(1);
-			trace_stop_count = 0;
+			global_trace.stop_count = 0;
 		}
 		goto out;
 	}
@@ -1040,19 +1205,52 @@ void tracing_start(void)
 	/* Prevent the buffers from switching */
 	arch_spin_lock(&ftrace_max_lock);
 
-	buffer = global_trace.buffer;
+	buffer = global_trace.trace_buffer.buffer;
 	if (buffer)
 		ring_buffer_record_enable(buffer);
 
-	buffer = max_tr.buffer;
+#ifdef CONFIG_TRACER_MAX_TRACE
+	buffer = global_trace.max_buffer.buffer;
 	if (buffer)
 		ring_buffer_record_enable(buffer);
+#endif
 
 	arch_spin_unlock(&ftrace_max_lock);
 
 	ftrace_start();
  out:
-	raw_spin_unlock_irqrestore(&tracing_start_lock, flags);
+	raw_spin_unlock_irqrestore(&global_trace.start_lock, flags);
+}
+
+static void tracing_start_tr(struct trace_array *tr)
+{
+	struct ring_buffer *buffer;
+	unsigned long flags;
+
+	if (tracing_disabled)
+		return;
+
+	/* If global, we need to also start the max tracer */
+	if (tr->flags & TRACE_ARRAY_FL_GLOBAL)
+		return tracing_start();
+
+	raw_spin_lock_irqsave(&tr->start_lock, flags);
+
+	if (--tr->stop_count) {
+		if (tr->stop_count < 0) {
+			/* Someone screwed up their debugging */
+			WARN_ON_ONCE(1);
+			tr->stop_count = 0;
+		}
+		goto out;
+	}
+
+	buffer = tr->trace_buffer.buffer;
+	if (buffer)
+		ring_buffer_record_enable(buffer);
+
+ out:
+	raw_spin_unlock_irqrestore(&tr->start_lock, flags);
 }
 
 /**
@@ -1067,25 +1265,48 @@ void tracing_stop(void)
 	unsigned long flags;
 
 	ftrace_stop();
-	raw_spin_lock_irqsave(&tracing_start_lock, flags);
-	if (trace_stop_count++)
+	raw_spin_lock_irqsave(&global_trace.start_lock, flags);
+	if (global_trace.stop_count++)
 		goto out;
 
 	/* Prevent the buffers from switching */
 	arch_spin_lock(&ftrace_max_lock);
 
-	buffer = global_trace.buffer;
+	buffer = global_trace.trace_buffer.buffer;
 	if (buffer)
 		ring_buffer_record_disable(buffer);
 
-	buffer = max_tr.buffer;
+#ifdef CONFIG_TRACER_MAX_TRACE
+	buffer = global_trace.max_buffer.buffer;
 	if (buffer)
 		ring_buffer_record_disable(buffer);
+#endif
 
 	arch_spin_unlock(&ftrace_max_lock);
 
  out:
-	raw_spin_unlock_irqrestore(&tracing_start_lock, flags);
+	raw_spin_unlock_irqrestore(&global_trace.start_lock, flags);
+}
+
+static void tracing_stop_tr(struct trace_array *tr)
+{
+	struct ring_buffer *buffer;
+	unsigned long flags;
+
+	/* If global, we need to also stop the max tracer */
+	if (tr->flags & TRACE_ARRAY_FL_GLOBAL)
+		return tracing_stop();
+
+	raw_spin_lock_irqsave(&tr->start_lock, flags);
+	if (tr->stop_count++)
+		goto out;
+
+	buffer = tr->trace_buffer.buffer;
+	if (buffer)
+		ring_buffer_record_disable(buffer);
+
+ out:
+	raw_spin_unlock_irqrestore(&tr->start_lock, flags);
 }
 
 void trace_stop_cmdline_recording(void);
@@ -1218,11 +1439,6 @@ void
 __buffer_unlock_commit(struct ring_buffer *buffer, struct ring_buffer_event *event)
 {
 	__this_cpu_write(trace_cmdline_save, true);
-	if (trace_wakeup_needed) {
-		trace_wakeup_needed = false;
-		/* irq_work_queue() supplies it's own memory barriers */
-		irq_work_queue(&trace_work_wakeup);
-	}
 	ring_buffer_unlock_commit(buffer, event);
 }
 
@@ -1246,11 +1462,23 @@ void trace_buffer_unlock_commit(struct ring_buffer *buffer,
 EXPORT_SYMBOL_GPL(trace_buffer_unlock_commit);
 
 struct ring_buffer_event *
+trace_event_buffer_lock_reserve(struct ring_buffer **current_rb,
+			  struct ftrace_event_file *ftrace_file,
+			  int type, unsigned long len,
+			  unsigned long flags, int pc)
+{
+	*current_rb = ftrace_file->tr->trace_buffer.buffer;
+	return trace_buffer_lock_reserve(*current_rb,
+					 type, len, flags, pc);
+}
+EXPORT_SYMBOL_GPL(trace_event_buffer_lock_reserve);
+
+struct ring_buffer_event *
 trace_current_buffer_lock_reserve(struct ring_buffer **current_rb,
 				  int type, unsigned long len,
 				  unsigned long flags, int pc)
 {
-	*current_rb = global_trace.buffer;
+	*current_rb = global_trace.trace_buffer.buffer;
 	return trace_buffer_lock_reserve(*current_rb,
 					 type, len, flags, pc);
 }
@@ -1289,7 +1517,7 @@ trace_function(struct trace_array *tr,
 	       int pc)
 {
 	struct ftrace_event_call *call = &event_function;
-	struct ring_buffer *buffer = tr->buffer;
+	struct ring_buffer *buffer = tr->trace_buffer.buffer;
 	struct ring_buffer_event *event;
 	struct ftrace_entry *entry;
 
@@ -1430,13 +1658,14 @@ void ftrace_trace_stack(struct ring_buffer *buffer, unsigned long flags,
 void __trace_stack(struct trace_array *tr, unsigned long flags, int skip,
 		   int pc)
 {
-	__ftrace_trace_stack(tr->buffer, flags, skip, pc, NULL);
+	__ftrace_trace_stack(tr->trace_buffer.buffer, flags, skip, pc, NULL);
 }
 
 /**
  * trace_dump_stack - record a stack back trace in the trace buffer
+ * @skip: Number of functions to skip (helper handlers)
  */
-void trace_dump_stack(void)
+void trace_dump_stack(int skip)
 {
 	unsigned long flags;
 
@@ -1445,8 +1674,13 @@ void trace_dump_stack(void)
 
 	local_save_flags(flags);
 
-	/* skipping 3 traces, seems to get us at the caller of this function */
-	__ftrace_trace_stack(global_trace.buffer, flags, 3, preempt_count(), NULL);
+	/*
+	 * Skip 3 more, seems to get us at the caller of
+	 * this function.
+	 */
+	skip += 3;
+	__ftrace_trace_stack(global_trace.trace_buffer.buffer,
+			     flags, skip, preempt_count(), NULL);
 }
 
 static DEFINE_PER_CPU(int, user_stack_count);
@@ -1616,7 +1850,7 @@ void trace_printk_init_buffers(void)
 	 * directly here. If the global_trace.buffer is already
 	 * allocated here, then this was called by module code.
 	 */
-	if (global_trace.buffer)
+	if (global_trace.trace_buffer.buffer)
 		tracing_start_cmdline_record();
 }
 
@@ -1676,7 +1910,7 @@ int trace_vbprintk(unsigned long ip, const char *fmt, va_list args)
 
 	local_save_flags(flags);
 	size = sizeof(*entry) + sizeof(u32) * len;
-	buffer = tr->buffer;
+	buffer = tr->trace_buffer.buffer;
 	event = trace_buffer_lock_reserve(buffer, TRACE_BPRINT, size,
 					  flags, pc);
 	if (!event)
@@ -1699,27 +1933,12 @@ out:
 }
 EXPORT_SYMBOL_GPL(trace_vbprintk);
 
-int trace_array_printk(struct trace_array *tr,
-		       unsigned long ip, const char *fmt, ...)
-{
-	int ret;
-	va_list ap;
-
-	if (!(trace_flags & TRACE_ITER_PRINTK))
-		return 0;
-
-	va_start(ap, fmt);
-	ret = trace_array_vprintk(tr, ip, fmt, ap);
-	va_end(ap);
-	return ret;
-}
-
-int trace_array_vprintk(struct trace_array *tr,
-			unsigned long ip, const char *fmt, va_list args)
+static int
+__trace_array_vprintk(struct ring_buffer *buffer,
+		      unsigned long ip, const char *fmt, va_list args)
 {
 	struct ftrace_event_call *call = &event_print;
 	struct ring_buffer_event *event;
-	struct ring_buffer *buffer;
 	int len = 0, size, pc;
 	struct print_entry *entry;
 	unsigned long flags;
@@ -1747,7 +1966,6 @@ int trace_array_vprintk(struct trace_array *tr,
 
 	local_save_flags(flags);
 	size = sizeof(*entry) + len + 1;
-	buffer = tr->buffer;
 	event = trace_buffer_lock_reserve(buffer, TRACE_PRINT, size,
 					  flags, pc);
 	if (!event)
@@ -1768,6 +1986,42 @@ int trace_array_vprintk(struct trace_array *tr,
 	return len;
 }
 
+int trace_array_vprintk(struct trace_array *tr,
+			unsigned long ip, const char *fmt, va_list args)
+{
+	return __trace_array_vprintk(tr->trace_buffer.buffer, ip, fmt, args);
+}
+
+int trace_array_printk(struct trace_array *tr,
+		       unsigned long ip, const char *fmt, ...)
+{
+	int ret;
+	va_list ap;
+
+	if (!(trace_flags & TRACE_ITER_PRINTK))
+		return 0;
+
+	va_start(ap, fmt);
+	ret = trace_array_vprintk(tr, ip, fmt, ap);
+	va_end(ap);
+	return ret;
+}
+
+int trace_array_printk_buf(struct ring_buffer *buffer,
+			   unsigned long ip, const char *fmt, ...)
+{
+	int ret;
+	va_list ap;
+
+	if (!(trace_flags & TRACE_ITER_PRINTK))
+		return 0;
+
+	va_start(ap, fmt);
+	ret = __trace_array_vprintk(buffer, ip, fmt, ap);
+	va_end(ap);
+	return ret;
+}
+
 int trace_vprintk(unsigned long ip, const char *fmt, va_list args)
 {
 	return trace_array_vprintk(&global_trace, ip, fmt, args);
@@ -1793,7 +2047,7 @@ peek_next_entry(struct trace_iterator *iter, int cpu, u64 *ts,
 	if (buf_iter)
 		event = ring_buffer_iter_peek(buf_iter, ts);
 	else
-		event = ring_buffer_peek(iter->tr->buffer, cpu, ts,
+		event = ring_buffer_peek(iter->trace_buffer->buffer, cpu, ts,
 					 lost_events);
 
 	if (event) {
@@ -1808,7 +2062,7 @@ static struct trace_entry *
 __find_next_entry(struct trace_iterator *iter, int *ent_cpu,
 		  unsigned long *missing_events, u64 *ent_ts)
 {
-	struct ring_buffer *buffer = iter->tr->buffer;
+	struct ring_buffer *buffer = iter->trace_buffer->buffer;
 	struct trace_entry *ent, *next = NULL;
 	unsigned long lost_events = 0, next_lost = 0;
 	int cpu_file = iter->cpu_file;
@@ -1821,7 +2075,7 @@ __find_next_entry(struct trace_iterator *iter, int *ent_cpu,
 	 * If we are in a per_cpu trace file, don't bother by iterating over
 	 * all cpu and peek directly.
 	 */
-	if (cpu_file > TRACE_PIPE_ALL_CPU) {
+	if (cpu_file > RING_BUFFER_ALL_CPUS) {
 		if (ring_buffer_empty_cpu(buffer, cpu_file))
 			return NULL;
 		ent = peek_next_entry(iter, cpu_file, ent_ts, missing_events);
@@ -1885,7 +2139,7 @@ void *trace_find_next_entry_inc(struct trace_iterator *iter)
 
 static void trace_consume(struct trace_iterator *iter)
 {
-	ring_buffer_consume(iter->tr->buffer, iter->cpu, &iter->ts,
+	ring_buffer_consume(iter->trace_buffer->buffer, iter->cpu, &iter->ts,
 			    &iter->lost_events);
 }
 
@@ -1918,13 +2172,12 @@ static void *s_next(struct seq_file *m, void *v, loff_t *pos)
 
 void tracing_iter_reset(struct trace_iterator *iter, int cpu)
 {
-	struct trace_array *tr = iter->tr;
 	struct ring_buffer_event *event;
 	struct ring_buffer_iter *buf_iter;
 	unsigned long entries = 0;
 	u64 ts;
 
-	tr->data[cpu]->skipped_entries = 0;
+	per_cpu_ptr(iter->trace_buffer->data, cpu)->skipped_entries = 0;
 
 	buf_iter = trace_buffer_iter(iter, cpu);
 	if (!buf_iter)
@@ -1938,13 +2191,13 @@ void tracing_iter_reset(struct trace_iterator *iter, int cpu)
 	 * by the timestamp being before the start of the buffer.
 	 */
 	while ((event = ring_buffer_iter_peek(buf_iter, &ts))) {
-		if (ts >= iter->tr->time_start)
+		if (ts >= iter->trace_buffer->time_start)
 			break;
 		entries++;
 		ring_buffer_read(buf_iter, NULL);
 	}
 
-	tr->data[cpu]->skipped_entries = entries;
+	per_cpu_ptr(iter->trace_buffer->data, cpu)->skipped_entries = entries;
 }
 
 /*
@@ -1954,6 +2207,7 @@ void tracing_iter_reset(struct trace_iterator *iter, int cpu)
 static void *s_start(struct seq_file *m, loff_t *pos)
 {
 	struct trace_iterator *iter = m->private;
+	struct trace_array *tr = iter->tr;
 	int cpu_file = iter->cpu_file;
 	void *p = NULL;
 	loff_t l = 0;
@@ -1966,12 +2220,14 @@ static void *s_start(struct seq_file *m, loff_t *pos)
 	 * will point to the same string as current_trace->name.
 	 */
 	mutex_lock(&trace_types_lock);
-	if (unlikely(current_trace && iter->trace->name != current_trace->name))
-		*iter->trace = *current_trace;
+	if (unlikely(tr->current_trace && iter->trace->name != tr->current_trace->name))
+		*iter->trace = *tr->current_trace;
 	mutex_unlock(&trace_types_lock);
 
+#ifdef CONFIG_TRACER_MAX_TRACE
 	if (iter->snapshot && iter->trace->use_max_tr)
 		return ERR_PTR(-EBUSY);
+#endif
 
 	if (!iter->snapshot)
 		atomic_inc(&trace_record_cmdline_disabled);
@@ -1981,7 +2237,7 @@ static void *s_start(struct seq_file *m, loff_t *pos)
 		iter->cpu = 0;
 		iter->idx = -1;
 
-		if (cpu_file == TRACE_PIPE_ALL_CPU) {
+		if (cpu_file == RING_BUFFER_ALL_CPUS) {
 			for_each_tracing_cpu(cpu)
 				tracing_iter_reset(iter, cpu);
 		} else
@@ -2013,17 +2269,21 @@ static void s_stop(struct seq_file *m, void *p)
 {
 	struct trace_iterator *iter = m->private;
 
+#ifdef CONFIG_TRACER_MAX_TRACE
 	if (iter->snapshot && iter->trace->use_max_tr)
 		return;
+#endif
 
 	if (!iter->snapshot)
 		atomic_dec(&trace_record_cmdline_disabled);
+
 	trace_access_unlock(iter->cpu_file);
 	trace_event_read_unlock();
 }
 
 static void
-get_total_entries(struct trace_array *tr, unsigned long *total, unsigned long *entries)
+get_total_entries(struct trace_buffer *buf,
+		  unsigned long *total, unsigned long *entries)
 {
 	unsigned long count;
 	int cpu;
@@ -2032,19 +2292,19 @@ get_total_entries(struct trace_array *tr, unsigned long *total, unsigned long *e
 	*entries = 0;
 
 	for_each_tracing_cpu(cpu) {
-		count = ring_buffer_entries_cpu(tr->buffer, cpu);
+		count = ring_buffer_entries_cpu(buf->buffer, cpu);
 		/*
 		 * If this buffer has skipped entries, then we hold all
 		 * entries for the trace and we need to ignore the
 		 * ones before the time stamp.
 		 */
-		if (tr->data[cpu]->skipped_entries) {
-			count -= tr->data[cpu]->skipped_entries;
+		if (per_cpu_ptr(buf->data, cpu)->skipped_entries) {
+			count -= per_cpu_ptr(buf->data, cpu)->skipped_entries;
 			/* total is the same as the entries */
 			*total += count;
 		} else
 			*total += count +
-				ring_buffer_overrun_cpu(tr->buffer, cpu);
+				ring_buffer_overrun_cpu(buf->buffer, cpu);
 		*entries += count;
 	}
 }
@@ -2061,27 +2321,27 @@ static void print_lat_help_header(struct seq_file *m)
 	seq_puts(m, "#     \\   /      |||||  \\    |   /           \n");
 }
 
-static void print_event_info(struct trace_array *tr, struct seq_file *m)
+static void print_event_info(struct trace_buffer *buf, struct seq_file *m)
 {
 	unsigned long total;
 	unsigned long entries;
 
-	get_total_entries(tr, &total, &entries);
+	get_total_entries(buf, &total, &entries);
 	seq_printf(m, "# entries-in-buffer/entries-written: %lu/%lu   #P:%d\n",
 		   entries, total, num_online_cpus());
 	seq_puts(m, "#\n");
 }
 
-static void print_func_help_header(struct trace_array *tr, struct seq_file *m)
+static void print_func_help_header(struct trace_buffer *buf, struct seq_file *m)
 {
-	print_event_info(tr, m);
+	print_event_info(buf, m);
 	seq_puts(m, "#           TASK-PID   CPU#      TIMESTAMP  FUNCTION\n");
 	seq_puts(m, "#              | |       |          |         |\n");
 }
 
-static void print_func_help_header_irq(struct trace_array *tr, struct seq_file *m)
+static void print_func_help_header_irq(struct trace_buffer *buf, struct seq_file *m)
 {
-	print_event_info(tr, m);
+	print_event_info(buf, m);
 	seq_puts(m, "#                              _-----=> irqs-off\n");
 	seq_puts(m, "#                             / _----=> need-resched\n");
 	seq_puts(m, "#                            | / _---=> hardirq/softirq\n");
@@ -2095,16 +2355,16 @@ void
 print_trace_header(struct seq_file *m, struct trace_iterator *iter)
 {
 	unsigned long sym_flags = (trace_flags & TRACE_ITER_SYM_MASK);
-	struct trace_array *tr = iter->tr;
-	struct trace_array_cpu *data = tr->data[tr->cpu];
-	struct tracer *type = current_trace;
+	struct trace_buffer *buf = iter->trace_buffer;
+	struct trace_array_cpu *data = per_cpu_ptr(buf->data, buf->cpu);
+	struct tracer *type = iter->trace;
 	unsigned long entries;
 	unsigned long total;
 	const char *name = "preemption";
 
 	name = type->name;
 
-	get_total_entries(tr, &total, &entries);
+	get_total_entries(buf, &total, &entries);
 
 	seq_printf(m, "# %s latency trace v1.1.5 on %s\n",
 		   name, UTS_RELEASE);
@@ -2115,7 +2375,7 @@ print_trace_header(struct seq_file *m, struct trace_iterator *iter)
 		   nsecs_to_usecs(data->saved_latency),
 		   entries,
 		   total,
-		   tr->cpu,
+		   buf->cpu,
 #if defined(CONFIG_PREEMPT_NONE)
 		   "server",
 #elif defined(CONFIG_PREEMPT_VOLUNTARY)
@@ -2166,7 +2426,7 @@ static void test_cpu_buff_start(struct trace_iterator *iter)
 	if (cpumask_test_cpu(iter->cpu, iter->started))
 		return;
 
-	if (iter->tr->data[iter->cpu]->skipped_entries)
+	if (per_cpu_ptr(iter->trace_buffer->data, iter->cpu)->skipped_entries)
 		return;
 
 	cpumask_set_cpu(iter->cpu, iter->started);
@@ -2289,14 +2549,14 @@ int trace_empty(struct trace_iterator *iter)
 	int cpu;
 
 	/* If we are looking at one CPU buffer, only check that one */
-	if (iter->cpu_file != TRACE_PIPE_ALL_CPU) {
+	if (iter->cpu_file != RING_BUFFER_ALL_CPUS) {
 		cpu = iter->cpu_file;
 		buf_iter = trace_buffer_iter(iter, cpu);
 		if (buf_iter) {
 			if (!ring_buffer_iter_empty(buf_iter))
 				return 0;
 		} else {
-			if (!ring_buffer_empty_cpu(iter->tr->buffer, cpu))
+			if (!ring_buffer_empty_cpu(iter->trace_buffer->buffer, cpu))
 				return 0;
 		}
 		return 1;
@@ -2308,7 +2568,7 @@ int trace_empty(struct trace_iterator *iter)
 			if (!ring_buffer_iter_empty(buf_iter))
 				return 0;
 		} else {
-			if (!ring_buffer_empty_cpu(iter->tr->buffer, cpu))
+			if (!ring_buffer_empty_cpu(iter->trace_buffer->buffer, cpu))
 				return 0;
 		}
 	}
@@ -2332,6 +2592,11 @@ enum print_line_t print_trace_line(struct trace_iterator *iter)
 			return ret;
 	}
 
+	if (iter->ent->type == TRACE_BPUTS &&
+			trace_flags & TRACE_ITER_PRINTK &&
+			trace_flags & TRACE_ITER_PRINTK_MSGONLY)
+		return trace_print_bputs_msg_only(iter);
+
 	if (iter->ent->type == TRACE_BPRINT &&
 			trace_flags & TRACE_ITER_PRINTK &&
 			trace_flags & TRACE_ITER_PRINTK_MSGONLY)
@@ -2386,9 +2651,9 @@ void trace_default_header(struct seq_file *m)
 	} else {
 		if (!(trace_flags & TRACE_ITER_VERBOSE)) {
 			if (trace_flags & TRACE_ITER_IRQ_INFO)
-				print_func_help_header_irq(iter->tr, m);
+				print_func_help_header_irq(iter->trace_buffer, m);
 			else
-				print_func_help_header(iter->tr, m);
+				print_func_help_header(iter->trace_buffer, m);
 		}
 	}
 }
@@ -2402,14 +2667,8 @@ static void test_ftrace_alive(struct seq_file *m)
 }
 
 #ifdef CONFIG_TRACER_MAX_TRACE
-static void print_snapshot_help(struct seq_file *m, struct trace_iterator *iter)
+static void show_snapshot_main_help(struct seq_file *m)
 {
-	if (iter->trace->allocated_snapshot)
-		seq_printf(m, "#\n# * Snapshot is allocated *\n#\n");
-	else
-		seq_printf(m, "#\n# * Snapshot is freed *\n#\n");
-
-	seq_printf(m, "# Snapshot commands:\n");
 	seq_printf(m, "# echo 0 > snapshot : Clears and frees snapshot buffer\n");
 	seq_printf(m, "# echo 1 > snapshot : Allocates snapshot buffer, if not already allocated.\n");
 	seq_printf(m, "#                      Takes a snapshot of the main buffer.\n");
@@ -2417,6 +2676,35 @@ static void print_snapshot_help(struct seq_file *m, struct trace_iterator *iter)
 	seq_printf(m, "#                      (Doesn't have to be '2' works with any number that\n");
 	seq_printf(m, "#                       is not a '0' or '1')\n");
 }
+
+static void show_snapshot_percpu_help(struct seq_file *m)
+{
+	seq_printf(m, "# echo 0 > snapshot : Invalid for per_cpu snapshot file.\n");
+#ifdef CONFIG_RING_BUFFER_ALLOW_SWAP
+	seq_printf(m, "# echo 1 > snapshot : Allocates snapshot buffer, if not already allocated.\n");
+	seq_printf(m, "#                      Takes a snapshot of the main buffer for this cpu.\n");
+#else
+	seq_printf(m, "# echo 1 > snapshot : Not supported with this kernel.\n");
+	seq_printf(m, "#                     Must use main snapshot file to allocate.\n");
+#endif
+	seq_printf(m, "# echo 2 > snapshot : Clears this cpu's snapshot buffer (but does not allocate)\n");
+	seq_printf(m, "#                      (Doesn't have to be '2' works with any number that\n");
+	seq_printf(m, "#                       is not a '0' or '1')\n");
+}
+
+static void print_snapshot_help(struct seq_file *m, struct trace_iterator *iter)
+{
+	if (iter->tr->allocated_snapshot)
+		seq_printf(m, "#\n# * Snapshot is allocated *\n#\n");
+	else
+		seq_printf(m, "#\n# * Snapshot is freed *\n#\n");
+
+	seq_printf(m, "# Snapshot commands:\n");
+	if (iter->cpu_file == RING_BUFFER_ALL_CPUS)
+		show_snapshot_main_help(m);
+	else
+		show_snapshot_percpu_help(m);
+}
 #else
 /* Should never be called */
 static inline void print_snapshot_help(struct seq_file *m, struct trace_iterator *iter) { }
@@ -2476,7 +2764,8 @@ static const struct seq_operations tracer_seq_ops = {
 static struct trace_iterator *
 __tracing_open(struct inode *inode, struct file *file, bool snapshot)
 {
-	long cpu_file = (long) inode->i_private;
+	struct trace_cpu *tc = inode->i_private;
+	struct trace_array *tr = tc->tr;
 	struct trace_iterator *iter;
 	int cpu;
 
@@ -2501,26 +2790,31 @@ __tracing_open(struct inode *inode, struct file *file, bool snapshot)
 	if (!iter->trace)
 		goto fail;
 
-	*iter->trace = *current_trace;
+	*iter->trace = *tr->current_trace;
 
 	if (!zalloc_cpumask_var(&iter->started, GFP_KERNEL))
 		goto fail;
 
-	if (current_trace->print_max || snapshot)
-		iter->tr = &max_tr;
+	iter->tr = tr;
+
+#ifdef CONFIG_TRACER_MAX_TRACE
+	/* Currently only the top directory has a snapshot */
+	if (tr->current_trace->print_max || snapshot)
+		iter->trace_buffer = &tr->max_buffer;
 	else
-		iter->tr = &global_trace;
+#endif
+		iter->trace_buffer = &tr->trace_buffer;
 	iter->snapshot = snapshot;
 	iter->pos = -1;
 	mutex_init(&iter->mutex);
-	iter->cpu_file = cpu_file;
+	iter->cpu_file = tc->cpu;
 
 	/* Notify the tracer early; before we stop tracing. */
 	if (iter->trace && iter->trace->open)
 		iter->trace->open(iter);
 
 	/* Annotate start of buffers if we had overruns */
-	if (ring_buffer_overruns(iter->tr->buffer))
+	if (ring_buffer_overruns(iter->trace_buffer->buffer))
 		iter->iter_flags |= TRACE_FILE_ANNOTATE;
 
 	/* Output in nanoseconds only if we are using a clock in nanoseconds. */
@@ -2529,12 +2823,12 @@ __tracing_open(struct inode *inode, struct file *file, bool snapshot)
 
 	/* stop the trace while dumping if we are not opening "snapshot" */
 	if (!iter->snapshot)
-		tracing_stop();
+		tracing_stop_tr(tr);
 
-	if (iter->cpu_file == TRACE_PIPE_ALL_CPU) {
+	if (iter->cpu_file == RING_BUFFER_ALL_CPUS) {
 		for_each_tracing_cpu(cpu) {
 			iter->buffer_iter[cpu] =
-				ring_buffer_read_prepare(iter->tr->buffer, cpu);
+				ring_buffer_read_prepare(iter->trace_buffer->buffer, cpu);
 		}
 		ring_buffer_read_prepare_sync();
 		for_each_tracing_cpu(cpu) {
@@ -2544,12 +2838,14 @@ __tracing_open(struct inode *inode, struct file *file, bool snapshot)
 	} else {
 		cpu = iter->cpu_file;
 		iter->buffer_iter[cpu] =
-			ring_buffer_read_prepare(iter->tr->buffer, cpu);
+			ring_buffer_read_prepare(iter->trace_buffer->buffer, cpu);
 		ring_buffer_read_prepare_sync();
 		ring_buffer_read_start(iter->buffer_iter[cpu]);
 		tracing_iter_reset(iter, cpu);
 	}
 
+	tr->ref++;
+
 	mutex_unlock(&trace_types_lock);
 
 	return iter;
@@ -2576,14 +2872,20 @@ static int tracing_release(struct inode *inode, struct file *file)
 {
 	struct seq_file *m = file->private_data;
 	struct trace_iterator *iter;
+	struct trace_array *tr;
 	int cpu;
 
 	if (!(file->f_mode & FMODE_READ))
 		return 0;
 
 	iter = m->private;
+	tr = iter->tr;
 
 	mutex_lock(&trace_types_lock);
+
+	WARN_ON(!tr->ref);
+	tr->ref--;
+
 	for_each_tracing_cpu(cpu) {
 		if (iter->buffer_iter[cpu])
 			ring_buffer_read_finish(iter->buffer_iter[cpu]);
@@ -2594,7 +2896,7 @@ static int tracing_release(struct inode *inode, struct file *file)
 
 	if (!iter->snapshot)
 		/* reenable tracing if it was previously enabled */
-		tracing_start();
+		tracing_start_tr(tr);
 	mutex_unlock(&trace_types_lock);
 
 	mutex_destroy(&iter->mutex);
@@ -2613,12 +2915,13 @@ static int tracing_open(struct inode *inode, struct file *file)
 	/* If this file was open for write, then erase contents */
 	if ((file->f_mode & FMODE_WRITE) &&
 	    (file->f_flags & O_TRUNC)) {
-		long cpu = (long) inode->i_private;
+		struct trace_cpu *tc = inode->i_private;
+		struct trace_array *tr = tc->tr;
 
-		if (cpu == TRACE_PIPE_ALL_CPU)
-			tracing_reset_online_cpus(&global_trace);
+		if (tc->cpu == RING_BUFFER_ALL_CPUS)
+			tracing_reset_online_cpus(&tr->trace_buffer);
 		else
-			tracing_reset(&global_trace, cpu);
+			tracing_reset(&tr->trace_buffer, tc->cpu);
 	}
 
 	if (file->f_mode & FMODE_READ) {
@@ -2765,8 +3068,9 @@ static ssize_t
 tracing_cpumask_write(struct file *filp, const char __user *ubuf,
 		      size_t count, loff_t *ppos)
 {
-	int err, cpu;
+	struct trace_array *tr = filp->private_data;
 	cpumask_var_t tracing_cpumask_new;
+	int err, cpu;
 
 	if (!alloc_cpumask_var(&tracing_cpumask_new, GFP_KERNEL))
 		return -ENOMEM;
@@ -2786,13 +3090,13 @@ tracing_cpumask_write(struct file *filp, const char __user *ubuf,
 		 */
 		if (cpumask_test_cpu(cpu, tracing_cpumask) &&
 				!cpumask_test_cpu(cpu, tracing_cpumask_new)) {
-			atomic_inc(&global_trace.data[cpu]->disabled);
-			ring_buffer_record_disable_cpu(global_trace.buffer, cpu);
+			atomic_inc(&per_cpu_ptr(tr->trace_buffer.data, cpu)->disabled);
+			ring_buffer_record_disable_cpu(tr->trace_buffer.buffer, cpu);
 		}
 		if (!cpumask_test_cpu(cpu, tracing_cpumask) &&
 				cpumask_test_cpu(cpu, tracing_cpumask_new)) {
-			atomic_dec(&global_trace.data[cpu]->disabled);
-			ring_buffer_record_enable_cpu(global_trace.buffer, cpu);
+			atomic_dec(&per_cpu_ptr(tr->trace_buffer.data, cpu)->disabled);
+			ring_buffer_record_enable_cpu(tr->trace_buffer.buffer, cpu);
 		}
 	}
 	arch_spin_unlock(&ftrace_max_lock);
@@ -2821,12 +3125,13 @@ static const struct file_operations tracing_cpumask_fops = {
 static int tracing_trace_options_show(struct seq_file *m, void *v)
 {
 	struct tracer_opt *trace_opts;
+	struct trace_array *tr = m->private;
 	u32 tracer_flags;
 	int i;
 
 	mutex_lock(&trace_types_lock);
-	tracer_flags = current_trace->flags->val;
-	trace_opts = current_trace->flags->opts;
+	tracer_flags = tr->current_trace->flags->val;
+	trace_opts = tr->current_trace->flags->opts;
 
 	for (i = 0; trace_options[i]; i++) {
 		if (trace_flags & (1 << i))
@@ -2890,15 +3195,15 @@ int trace_keep_overwrite(struct tracer *tracer, u32 mask, int set)
 	return 0;
 }
 
-int set_tracer_flag(unsigned int mask, int enabled)
+int set_tracer_flag(struct trace_array *tr, unsigned int mask, int enabled)
 {
 	/* do nothing if flag is already set */
 	if (!!(trace_flags & mask) == !!enabled)
 		return 0;
 
 	/* Give the tracer a chance to approve the change */
-	if (current_trace->flag_changed)
-		if (current_trace->flag_changed(current_trace, mask, !!enabled))
+	if (tr->current_trace->flag_changed)
+		if (tr->current_trace->flag_changed(tr->current_trace, mask, !!enabled))
 			return -EINVAL;
 
 	if (enabled)
@@ -2910,9 +3215,9 @@ int set_tracer_flag(unsigned int mask, int enabled)
 		trace_event_enable_cmd_record(enabled);
 
 	if (mask == TRACE_ITER_OVERWRITE) {
-		ring_buffer_change_overwrite(global_trace.buffer, enabled);
+		ring_buffer_change_overwrite(tr->trace_buffer.buffer, enabled);
 #ifdef CONFIG_TRACER_MAX_TRACE
-		ring_buffer_change_overwrite(max_tr.buffer, enabled);
+		ring_buffer_change_overwrite(tr->max_buffer.buffer, enabled);
 #endif
 	}
 
@@ -2922,7 +3227,7 @@ int set_tracer_flag(unsigned int mask, int enabled)
 	return 0;
 }
 
-static int trace_set_options(char *option)
+static int trace_set_options(struct trace_array *tr, char *option)
 {
 	char *cmp;
 	int neg = 0;
@@ -2940,14 +3245,14 @@ static int trace_set_options(char *option)
 
 	for (i = 0; trace_options[i]; i++) {
 		if (strcmp(cmp, trace_options[i]) == 0) {
-			ret = set_tracer_flag(1 << i, !neg);
+			ret = set_tracer_flag(tr, 1 << i, !neg);
 			break;
 		}
 	}
 
 	/* If no option could be set, test the specific tracer options */
 	if (!trace_options[i])
-		ret = set_tracer_option(current_trace, cmp, neg);
+		ret = set_tracer_option(tr->current_trace, cmp, neg);
 
 	mutex_unlock(&trace_types_lock);
 
@@ -2958,6 +3263,8 @@ static ssize_t
 tracing_trace_options_write(struct file *filp, const char __user *ubuf,
 			size_t cnt, loff_t *ppos)
 {
+	struct seq_file *m = filp->private_data;
+	struct trace_array *tr = m->private;
 	char buf[64];
 	int ret;
 
@@ -2969,7 +3276,7 @@ tracing_trace_options_write(struct file *filp, const char __user *ubuf,
 
 	buf[cnt] = 0;
 
-	ret = trace_set_options(buf);
+	ret = trace_set_options(tr, buf);
 	if (ret < 0)
 		return ret;
 
@@ -2982,7 +3289,8 @@ static int tracing_trace_options_open(struct inode *inode, struct file *file)
 {
 	if (tracing_disabled)
 		return -ENODEV;
-	return single_open(file, tracing_trace_options_show, NULL);
+
+	return single_open(file, tracing_trace_options_show, inode->i_private);
 }
 
 static const struct file_operations tracing_iter_fops = {
@@ -2995,20 +3303,84 @@ static const struct file_operations tracing_iter_fops = {
 
 static const char readme_msg[] =
 	"tracing mini-HOWTO:\n\n"
-	"# mount -t debugfs nodev /sys/kernel/debug\n\n"
-	"# cat /sys/kernel/debug/tracing/available_tracers\n"
-	"wakeup wakeup_rt preemptirqsoff preemptoff irqsoff function nop\n\n"
-	"# cat /sys/kernel/debug/tracing/current_tracer\n"
-	"nop\n"
-	"# echo wakeup > /sys/kernel/debug/tracing/current_tracer\n"
-	"# cat /sys/kernel/debug/tracing/current_tracer\n"
-	"wakeup\n"
-	"# cat /sys/kernel/debug/tracing/trace_options\n"
-	"noprint-parent nosym-offset nosym-addr noverbose\n"
-	"# echo print-parent > /sys/kernel/debug/tracing/trace_options\n"
-	"# echo 1 > /sys/kernel/debug/tracing/tracing_on\n"
-	"# cat /sys/kernel/debug/tracing/trace > /tmp/trace.txt\n"
-	"# echo 0 > /sys/kernel/debug/tracing/tracing_on\n"
+	"# echo 0 > tracing_on : quick way to disable tracing\n"
+	"# echo 1 > tracing_on : quick way to re-enable tracing\n\n"
+	" Important files:\n"
+	"  trace\t\t\t- The static contents of the buffer\n"
+	"\t\t\t  To clear the buffer write into this file: echo > trace\n"
+	"  trace_pipe\t\t- A consuming read to see the contents of the buffer\n"
+	"  current_tracer\t- function and latency tracers\n"
+	"  available_tracers\t- list of configured tracers for current_tracer\n"
+	"  buffer_size_kb\t- view and modify size of per cpu buffer\n"
+	"  buffer_total_size_kb  - view total size of all cpu buffers\n\n"
+	"  trace_clock\t\t-change the clock used to order events\n"
+	"       local:   Per cpu clock but may not be synced across CPUs\n"
+	"      global:   Synced across CPUs but slows tracing down.\n"
+	"     counter:   Not a clock, but just an increment\n"
+	"      uptime:   Jiffy counter from time of boot\n"
+	"        perf:   Same clock that perf events use\n"
+#ifdef CONFIG_X86_64
+	"     x86-tsc:   TSC cycle counter\n"
+#endif
+	"\n  trace_marker\t\t- Writes into this file writes into the kernel buffer\n"
+	"  tracing_cpumask\t- Limit which CPUs to trace\n"
+	"  instances\t\t- Make sub-buffers with: mkdir instances/foo\n"
+	"\t\t\t  Remove sub-buffer with rmdir\n"
+	"  trace_options\t\t- Set format or modify how tracing happens\n"
+	"\t\t\t  Disable an option by adding a suffix 'no' to the option name\n"
+#ifdef CONFIG_DYNAMIC_FTRACE
+	"\n  available_filter_functions - list of functions that can be filtered on\n"
+	"  set_ftrace_filter\t- echo function name in here to only trace these functions\n"
+	"            accepts: func_full_name, *func_end, func_begin*, *func_middle*\n"
+	"            modules: Can select a group via module\n"
+	"             Format: :mod:<module-name>\n"
+	"             example: echo :mod:ext3 > set_ftrace_filter\n"
+	"            triggers: a command to perform when function is hit\n"
+	"              Format: <function>:<trigger>[:count]\n"
+	"             trigger: traceon, traceoff\n"
+	"                      enable_event:<system>:<event>\n"
+	"                      disable_event:<system>:<event>\n"
+#ifdef CONFIG_STACKTRACE
+	"                      stacktrace\n"
+#endif
+#ifdef CONFIG_TRACER_SNAPSHOT
+	"                      snapshot\n"
+#endif
+	"             example: echo do_fault:traceoff > set_ftrace_filter\n"
+	"                      echo do_trap:traceoff:3 > set_ftrace_filter\n"
+	"             The first one will disable tracing every time do_fault is hit\n"
+	"             The second will disable tracing at most 3 times when do_trap is hit\n"
+	"               The first time do trap is hit and it disables tracing, the counter\n"
+	"               will decrement to 2. If tracing is already disabled, the counter\n"
+	"               will not decrement. It only decrements when the trigger did work\n"
+	"             To remove trigger without count:\n"
+	"               echo '!<function>:<trigger> > set_ftrace_filter\n"
+	"             To remove trigger with a count:\n"
+	"               echo '!<function>:<trigger>:0 > set_ftrace_filter\n"
+	"  set_ftrace_notrace\t- echo function name in here to never trace.\n"
+	"            accepts: func_full_name, *func_end, func_begin*, *func_middle*\n"
+	"            modules: Can select a group via module command :mod:\n"
+	"            Does not accept triggers\n"
+#endif /* CONFIG_DYNAMIC_FTRACE */
+#ifdef CONFIG_FUNCTION_TRACER
+	"  set_ftrace_pid\t- Write pid(s) to only function trace those pids (function)\n"
+#endif
+#ifdef CONFIG_FUNCTION_GRAPH_TRACER
+	"  set_graph_function\t- Trace the nested calls of a function (function_graph)\n"
+	"  max_graph_depth\t- Trace a limited depth of nested calls (0 is unlimited)\n"
+#endif
+#ifdef CONFIG_TRACER_SNAPSHOT
+	"\n  snapshot\t\t- Like 'trace' but shows the content of the static snapshot buffer\n"
+	"\t\t\t  Read the contents for more information\n"
+#endif
+#ifdef CONFIG_STACKTRACE
+	"  stack_trace\t\t- Shows the max stack trace when active\n"
+	"  stack_max_size\t- Shows current max stack size that was traced\n"
+	"\t\t\t  Write into this file to reset the max size (trigger a new trace)\n"
+#ifdef CONFIG_DYNAMIC_FTRACE
+	"  stack_trace_filter\t- Like set_ftrace_filter but limits what stack_trace traces\n"
+#endif
+#endif /* CONFIG_STACKTRACE */
 ;
 
 static ssize_t
@@ -3080,11 +3452,12 @@ static ssize_t
 tracing_set_trace_read(struct file *filp, char __user *ubuf,
 		       size_t cnt, loff_t *ppos)
 {
+	struct trace_array *tr = filp->private_data;
 	char buf[MAX_TRACER_SIZE+2];
 	int r;
 
 	mutex_lock(&trace_types_lock);
-	r = sprintf(buf, "%s\n", current_trace->name);
+	r = sprintf(buf, "%s\n", tr->current_trace->name);
 	mutex_unlock(&trace_types_lock);
 
 	return simple_read_from_buffer(ubuf, cnt, ppos, buf, r);
@@ -3092,43 +3465,48 @@ tracing_set_trace_read(struct file *filp, char __user *ubuf,
 
 int tracer_init(struct tracer *t, struct trace_array *tr)
 {
-	tracing_reset_online_cpus(tr);
+	tracing_reset_online_cpus(&tr->trace_buffer);
 	return t->init(tr);
 }
 
-static void set_buffer_entries(struct trace_array *tr, unsigned long val)
+static void set_buffer_entries(struct trace_buffer *buf, unsigned long val)
 {
 	int cpu;
+
 	for_each_tracing_cpu(cpu)
-		tr->data[cpu]->entries = val;
+		per_cpu_ptr(buf->data, cpu)->entries = val;
 }
 
+#ifdef CONFIG_TRACER_MAX_TRACE
 /* resize @tr's buffer to the size of @size_tr's entries */
-static int resize_buffer_duplicate_size(struct trace_array *tr,
-					struct trace_array *size_tr, int cpu_id)
+static int resize_buffer_duplicate_size(struct trace_buffer *trace_buf,
+					struct trace_buffer *size_buf, int cpu_id)
 {
 	int cpu, ret = 0;
 
 	if (cpu_id == RING_BUFFER_ALL_CPUS) {
 		for_each_tracing_cpu(cpu) {
-			ret = ring_buffer_resize(tr->buffer,
-					size_tr->data[cpu]->entries, cpu);
+			ret = ring_buffer_resize(trace_buf->buffer,
+				 per_cpu_ptr(size_buf->data, cpu)->entries, cpu);
 			if (ret < 0)
 				break;
-			tr->data[cpu]->entries = size_tr->data[cpu]->entries;
+			per_cpu_ptr(trace_buf->data, cpu)->entries =
+				per_cpu_ptr(size_buf->data, cpu)->entries;
 		}
 	} else {
-		ret = ring_buffer_resize(tr->buffer,
-					size_tr->data[cpu_id]->entries, cpu_id);
+		ret = ring_buffer_resize(trace_buf->buffer,
+				 per_cpu_ptr(size_buf->data, cpu_id)->entries, cpu_id);
 		if (ret == 0)
-			tr->data[cpu_id]->entries =
-				size_tr->data[cpu_id]->entries;
+			per_cpu_ptr(trace_buf->data, cpu_id)->entries =
+				per_cpu_ptr(size_buf->data, cpu_id)->entries;
 	}
 
 	return ret;
 }
+#endif /* CONFIG_TRACER_MAX_TRACE */
 
-static int __tracing_resize_ring_buffer(unsigned long size, int cpu)
+static int __tracing_resize_ring_buffer(struct trace_array *tr,
+					unsigned long size, int cpu)
 {
 	int ret;
 
@@ -3137,23 +3515,25 @@ static int __tracing_resize_ring_buffer(unsigned long size, int cpu)
 	 * we use the size that was given, and we can forget about
 	 * expanding it later.
 	 */
-	ring_buffer_expanded = 1;
+	ring_buffer_expanded = true;
 
 	/* May be called before buffers are initialized */
-	if (!global_trace.buffer)
+	if (!tr->trace_buffer.buffer)
 		return 0;
 
-	ret = ring_buffer_resize(global_trace.buffer, size, cpu);
+	ret = ring_buffer_resize(tr->trace_buffer.buffer, size, cpu);
 	if (ret < 0)
 		return ret;
 
-	if (!current_trace->use_max_tr)
+#ifdef CONFIG_TRACER_MAX_TRACE
+	if (!(tr->flags & TRACE_ARRAY_FL_GLOBAL) ||
+	    !tr->current_trace->use_max_tr)
 		goto out;
 
-	ret = ring_buffer_resize(max_tr.buffer, size, cpu);
+	ret = ring_buffer_resize(tr->max_buffer.buffer, size, cpu);
 	if (ret < 0) {
-		int r = resize_buffer_duplicate_size(&global_trace,
-						     &global_trace, cpu);
+		int r = resize_buffer_duplicate_size(&tr->trace_buffer,
+						     &tr->trace_buffer, cpu);
 		if (r < 0) {
 			/*
 			 * AARGH! We are left with different
@@ -3176,20 +3556,23 @@ static int __tracing_resize_ring_buffer(unsigned long size, int cpu)
 	}
 
 	if (cpu == RING_BUFFER_ALL_CPUS)
-		set_buffer_entries(&max_tr, size);
+		set_buffer_entries(&tr->max_buffer, size);
 	else
-		max_tr.data[cpu]->entries = size;
+		per_cpu_ptr(tr->max_buffer.data, cpu)->entries = size;
 
  out:
+#endif /* CONFIG_TRACER_MAX_TRACE */
+
 	if (cpu == RING_BUFFER_ALL_CPUS)
-		set_buffer_entries(&global_trace, size);
+		set_buffer_entries(&tr->trace_buffer, size);
 	else
-		global_trace.data[cpu]->entries = size;
+		per_cpu_ptr(tr->trace_buffer.data, cpu)->entries = size;
 
 	return ret;
 }
 
-static ssize_t tracing_resize_ring_buffer(unsigned long size, int cpu_id)
+static ssize_t tracing_resize_ring_buffer(struct trace_array *tr,
+					  unsigned long size, int cpu_id)
 {
 	int ret = size;
 
@@ -3203,7 +3586,7 @@ static ssize_t tracing_resize_ring_buffer(unsigned long size, int cpu_id)
 		}
 	}
 
-	ret = __tracing_resize_ring_buffer(size, cpu_id);
+	ret = __tracing_resize_ring_buffer(tr, size, cpu_id);
 	if (ret < 0)
 		ret = -ENOMEM;
 
@@ -3230,7 +3613,7 @@ int tracing_update_buffers(void)
 
 	mutex_lock(&trace_types_lock);
 	if (!ring_buffer_expanded)
-		ret = __tracing_resize_ring_buffer(trace_buf_size,
+		ret = __tracing_resize_ring_buffer(&global_trace, trace_buf_size,
 						RING_BUFFER_ALL_CPUS);
 	mutex_unlock(&trace_types_lock);
 
@@ -3240,7 +3623,7 @@ int tracing_update_buffers(void)
 struct trace_option_dentry;
 
 static struct trace_option_dentry *
-create_trace_option_files(struct tracer *tracer);
+create_trace_option_files(struct trace_array *tr, struct tracer *tracer);
 
 static void
 destroy_trace_option_files(struct trace_option_dentry *topts);
@@ -3250,13 +3633,15 @@ static int tracing_set_tracer(const char *buf)
 	static struct trace_option_dentry *topts;
 	struct trace_array *tr = &global_trace;
 	struct tracer *t;
+#ifdef CONFIG_TRACER_MAX_TRACE
 	bool had_max_tr;
+#endif
 	int ret = 0;
 
 	mutex_lock(&trace_types_lock);
 
 	if (!ring_buffer_expanded) {
-		ret = __tracing_resize_ring_buffer(trace_buf_size,
+		ret = __tracing_resize_ring_buffer(tr, trace_buf_size,
 						RING_BUFFER_ALL_CPUS);
 		if (ret < 0)
 			goto out;
@@ -3271,18 +3656,21 @@ static int tracing_set_tracer(const char *buf)
 		ret = -EINVAL;
 		goto out;
 	}
-	if (t == current_trace)
+	if (t == tr->current_trace)
 		goto out;
 
 	trace_branch_disable();
 
-	current_trace->enabled = false;
+	tr->current_trace->enabled = false;
+
+	if (tr->current_trace->reset)
+		tr->current_trace->reset(tr);
 
-	if (current_trace->reset)
-		current_trace->reset(tr);
+	/* Current trace needs to be nop_trace before synchronize_sched */
+	tr->current_trace = &nop_trace;
 
-	had_max_tr = current_trace->allocated_snapshot;
-	current_trace = &nop_trace;
+#ifdef CONFIG_TRACER_MAX_TRACE
+	had_max_tr = tr->allocated_snapshot;
 
 	if (had_max_tr && !t->use_max_tr) {
 		/*
@@ -3293,27 +3681,20 @@ static int tracing_set_tracer(const char *buf)
 		 * so a synchronized_sched() is sufficient.
 		 */
 		synchronize_sched();
-		/*
-		 * We don't free the ring buffer. instead, resize it because
-		 * The max_tr ring buffer has some state (e.g. ring->clock) and
-		 * we want preserve it.
-		 */
-		ring_buffer_resize(max_tr.buffer, 1, RING_BUFFER_ALL_CPUS);
-		set_buffer_entries(&max_tr, 1);
-		tracing_reset_online_cpus(&max_tr);
-		current_trace->allocated_snapshot = false;
+		free_snapshot(tr);
 	}
+#endif
 	destroy_trace_option_files(topts);
 
-	topts = create_trace_option_files(t);
+	topts = create_trace_option_files(tr, t);
+
+#ifdef CONFIG_TRACER_MAX_TRACE
 	if (t->use_max_tr && !had_max_tr) {
-		/* we need to make per cpu buffer sizes equivalent */
-		ret = resize_buffer_duplicate_size(&max_tr, &global_trace,
-						   RING_BUFFER_ALL_CPUS);
+		ret = alloc_snapshot(tr);
 		if (ret < 0)
 			goto out;
-		t->allocated_snapshot = true;
 	}
+#endif
 
 	if (t->init) {
 		ret = tracer_init(t, tr);
@@ -3321,8 +3702,8 @@ static int tracing_set_tracer(const char *buf)
 			goto out;
 	}
 
-	current_trace = t;
-	current_trace->enabled = true;
+	tr->current_trace = t;
+	tr->current_trace->enabled = true;
 	trace_branch_enable(tr);
  out:
 	mutex_unlock(&trace_types_lock);
@@ -3396,7 +3777,8 @@ tracing_max_lat_write(struct file *filp, const char __user *ubuf,
 
 static int tracing_open_pipe(struct inode *inode, struct file *filp)
 {
-	long cpu_file = (long) inode->i_private;
+	struct trace_cpu *tc = inode->i_private;
+	struct trace_array *tr = tc->tr;
 	struct trace_iterator *iter;
 	int ret = 0;
 
@@ -3421,7 +3803,7 @@ static int tracing_open_pipe(struct inode *inode, struct file *filp)
 		ret = -ENOMEM;
 		goto fail;
 	}
-	*iter->trace = *current_trace;
+	*iter->trace = *tr->current_trace;
 
 	if (!alloc_cpumask_var(&iter->started, GFP_KERNEL)) {
 		ret = -ENOMEM;
@@ -3438,8 +3820,9 @@ static int tracing_open_pipe(struct inode *inode, struct file *filp)
 	if (trace_clocks[trace_clock_id].in_ns)
 		iter->iter_flags |= TRACE_FILE_TIME_IN_NS;
 
-	iter->cpu_file = cpu_file;
-	iter->tr = &global_trace;
+	iter->cpu_file = tc->cpu;
+	iter->tr = tc->tr;
+	iter->trace_buffer = &tc->tr->trace_buffer;
 	mutex_init(&iter->mutex);
 	filp->private_data = iter;
 
@@ -3478,24 +3861,28 @@ static int tracing_release_pipe(struct inode *inode, struct file *file)
 }
 
 static unsigned int
-tracing_poll_pipe(struct file *filp, poll_table *poll_table)
+trace_poll(struct trace_iterator *iter, struct file *filp, poll_table *poll_table)
 {
-	struct trace_iterator *iter = filp->private_data;
+	/* Iterators are static, they should be filled or empty */
+	if (trace_buffer_iter(iter, iter->cpu_file))
+		return POLLIN | POLLRDNORM;
 
-	if (trace_flags & TRACE_ITER_BLOCK) {
+	if (trace_flags & TRACE_ITER_BLOCK)
 		/*
 		 * Always select as readable when in blocking mode
 		 */
 		return POLLIN | POLLRDNORM;
-	} else {
-		if (!trace_empty(iter))
-			return POLLIN | POLLRDNORM;
-		poll_wait(filp, &trace_wait, poll_table);
-		if (!trace_empty(iter))
-			return POLLIN | POLLRDNORM;
+	else
+		return ring_buffer_poll_wait(iter->trace_buffer->buffer, iter->cpu_file,
+					     filp, poll_table);
+}
 
-		return 0;
-	}
+static unsigned int
+tracing_poll_pipe(struct file *filp, poll_table *poll_table)
+{
+	struct trace_iterator *iter = filp->private_data;
+
+	return trace_poll(iter, filp, poll_table);
 }
 
 /*
@@ -3561,6 +3948,7 @@ tracing_read_pipe(struct file *filp, char __user *ubuf,
 		  size_t cnt, loff_t *ppos)
 {
 	struct trace_iterator *iter = filp->private_data;
+	struct trace_array *tr = iter->tr;
 	ssize_t sret;
 
 	/* return any leftover data */
@@ -3572,8 +3960,8 @@ tracing_read_pipe(struct file *filp, char __user *ubuf,
 
 	/* copy the tracer to avoid using a global lock all around */
 	mutex_lock(&trace_types_lock);
-	if (unlikely(iter->trace->name != current_trace->name))
-		*iter->trace = *current_trace;
+	if (unlikely(iter->trace->name != tr->current_trace->name))
+		*iter->trace = *tr->current_trace;
 	mutex_unlock(&trace_types_lock);
 
 	/*
@@ -3729,6 +4117,7 @@ static ssize_t tracing_splice_read_pipe(struct file *filp,
 		.ops		= &tracing_pipe_buf_ops,
 		.spd_release	= tracing_spd_release_pipe,
 	};
+	struct trace_array *tr = iter->tr;
 	ssize_t ret;
 	size_t rem;
 	unsigned int i;
@@ -3738,8 +4127,8 @@ static ssize_t tracing_splice_read_pipe(struct file *filp,
 
 	/* copy the tracer to avoid using a global lock all around */
 	mutex_lock(&trace_types_lock);
-	if (unlikely(iter->trace->name != current_trace->name))
-		*iter->trace = *current_trace;
+	if (unlikely(iter->trace->name != tr->current_trace->name))
+		*iter->trace = *tr->current_trace;
 	mutex_unlock(&trace_types_lock);
 
 	mutex_lock(&iter->mutex);
@@ -3801,43 +4190,19 @@ out_err:
 	goto out;
 }
 
-struct ftrace_entries_info {
-	struct trace_array	*tr;
-	int			cpu;
-};
-
-static int tracing_entries_open(struct inode *inode, struct file *filp)
-{
-	struct ftrace_entries_info *info;
-
-	if (tracing_disabled)
-		return -ENODEV;
-
-	info = kzalloc(sizeof(*info), GFP_KERNEL);
-	if (!info)
-		return -ENOMEM;
-
-	info->tr = &global_trace;
-	info->cpu = (unsigned long)inode->i_private;
-
-	filp->private_data = info;
-
-	return 0;
-}
-
 static ssize_t
 tracing_entries_read(struct file *filp, char __user *ubuf,
 		     size_t cnt, loff_t *ppos)
 {
-	struct ftrace_entries_info *info = filp->private_data;
-	struct trace_array *tr = info->tr;
+	struct trace_cpu *tc = filp->private_data;
+	struct trace_array *tr = tc->tr;
 	char buf[64];
 	int r = 0;
 	ssize_t ret;
 
 	mutex_lock(&trace_types_lock);
 
-	if (info->cpu == RING_BUFFER_ALL_CPUS) {
+	if (tc->cpu == RING_BUFFER_ALL_CPUS) {
 		int cpu, buf_size_same;
 		unsigned long size;
 
@@ -3847,8 +4212,8 @@ tracing_entries_read(struct file *filp, char __user *ubuf,
 		for_each_tracing_cpu(cpu) {
 			/* fill in the size from first enabled cpu */
 			if (size == 0)
-				size = tr->data[cpu]->entries;
-			if (size != tr->data[cpu]->entries) {
+				size = per_cpu_ptr(tr->trace_buffer.data, cpu)->entries;
+			if (size != per_cpu_ptr(tr->trace_buffer.data, cpu)->entries) {
 				buf_size_same = 0;
 				break;
 			}
@@ -3864,7 +4229,7 @@ tracing_entries_read(struct file *filp, char __user *ubuf,
 		} else
 			r = sprintf(buf, "X\n");
 	} else
-		r = sprintf(buf, "%lu\n", tr->data[info->cpu]->entries >> 10);
+		r = sprintf(buf, "%lu\n", per_cpu_ptr(tr->trace_buffer.data, tc->cpu)->entries >> 10);
 
 	mutex_unlock(&trace_types_lock);
 
@@ -3876,7 +4241,7 @@ static ssize_t
 tracing_entries_write(struct file *filp, const char __user *ubuf,
 		      size_t cnt, loff_t *ppos)
 {
-	struct ftrace_entries_info *info = filp->private_data;
+	struct trace_cpu *tc = filp->private_data;
 	unsigned long val;
 	int ret;
 
@@ -3891,7 +4256,7 @@ tracing_entries_write(struct file *filp, const char __user *ubuf,
 	/* value is in KB */
 	val <<= 10;
 
-	ret = tracing_resize_ring_buffer(val, info->cpu);
+	ret = tracing_resize_ring_buffer(tc->tr, val, tc->cpu);
 	if (ret < 0)
 		return ret;
 
@@ -3900,16 +4265,6 @@ tracing_entries_write(struct file *filp, const char __user *ubuf,
 	return cnt;
 }
 
-static int
-tracing_entries_release(struct inode *inode, struct file *filp)
-{
-	struct ftrace_entries_info *info = filp->private_data;
-
-	kfree(info);
-
-	return 0;
-}
-
 static ssize_t
 tracing_total_entries_read(struct file *filp, char __user *ubuf,
 				size_t cnt, loff_t *ppos)
@@ -3921,7 +4276,7 @@ tracing_total_entries_read(struct file *filp, char __user *ubuf,
 
 	mutex_lock(&trace_types_lock);
 	for_each_tracing_cpu(cpu) {
-		size += tr->data[cpu]->entries >> 10;
+		size += per_cpu_ptr(tr->trace_buffer.data, cpu)->entries >> 10;
 		if (!ring_buffer_expanded)
 			expanded_size += trace_buf_size >> 10;
 	}
@@ -3951,11 +4306,13 @@ tracing_free_buffer_write(struct file *filp, const char __user *ubuf,
 static int
 tracing_free_buffer_release(struct inode *inode, struct file *filp)
 {
+	struct trace_array *tr = inode->i_private;
+
 	/* disable tracing ? */
 	if (trace_flags & TRACE_ITER_STOP_ON_FREE)
 		tracing_off();
 	/* resize the ring buffer to 0 */
-	tracing_resize_ring_buffer(0, RING_BUFFER_ALL_CPUS);
+	tracing_resize_ring_buffer(tr, 0, RING_BUFFER_ALL_CPUS);
 
 	return 0;
 }
@@ -4024,7 +4381,7 @@ tracing_mark_write(struct file *filp, const char __user *ubuf,
 
 	local_save_flags(irq_flags);
 	size = sizeof(*entry) + cnt + 2; /* possible \n added */
-	buffer = global_trace.buffer;
+	buffer = global_trace.trace_buffer.buffer;
 	event = trace_buffer_lock_reserve(buffer, TRACE_PRINT, size,
 					  irq_flags, preempt_count());
 	if (!event) {
@@ -4066,13 +4423,14 @@ tracing_mark_write(struct file *filp, const char __user *ubuf,
 
 static int tracing_clock_show(struct seq_file *m, void *v)
 {
+	struct trace_array *tr = m->private;
 	int i;
 
 	for (i = 0; i < ARRAY_SIZE(trace_clocks); i++)
 		seq_printf(m,
 			"%s%s%s%s", i ? " " : "",
-			i == trace_clock_id ? "[" : "", trace_clocks[i].name,
-			i == trace_clock_id ? "]" : "");
+			i == tr->clock_id ? "[" : "", trace_clocks[i].name,
+			i == tr->clock_id ? "]" : "");
 	seq_putc(m, '\n');
 
 	return 0;
@@ -4081,6 +4439,8 @@ static int tracing_clock_show(struct seq_file *m, void *v)
 static ssize_t tracing_clock_write(struct file *filp, const char __user *ubuf,
 				   size_t cnt, loff_t *fpos)
 {
+	struct seq_file *m = filp->private_data;
+	struct trace_array *tr = m->private;
 	char buf[64];
 	const char *clockstr;
 	int i;
@@ -4102,20 +4462,23 @@ static ssize_t tracing_clock_write(struct file *filp, const char __user *ubuf,
 	if (i == ARRAY_SIZE(trace_clocks))
 		return -EINVAL;
 
-	trace_clock_id = i;
-
 	mutex_lock(&trace_types_lock);
 
-	ring_buffer_set_clock(global_trace.buffer, trace_clocks[i].func);
-	if (max_tr.buffer)
-		ring_buffer_set_clock(max_tr.buffer, trace_clocks[i].func);
+	tr->clock_id = i;
+
+	ring_buffer_set_clock(tr->trace_buffer.buffer, trace_clocks[i].func);
 
 	/*
 	 * New clock may not be consistent with the previous clock.
 	 * Reset the buffer so that it doesn't have incomparable timestamps.
 	 */
-	tracing_reset_online_cpus(&global_trace);
-	tracing_reset_online_cpus(&max_tr);
+	tracing_reset_online_cpus(&global_trace.trace_buffer);
+
+#ifdef CONFIG_TRACER_MAX_TRACE
+	if (tr->flags & TRACE_ARRAY_FL_GLOBAL && tr->max_buffer.buffer)
+		ring_buffer_set_clock(tr->max_buffer.buffer, trace_clocks[i].func);
+	tracing_reset_online_cpus(&global_trace.max_buffer);
+#endif
 
 	mutex_unlock(&trace_types_lock);
 
@@ -4128,20 +4491,45 @@ static int tracing_clock_open(struct inode *inode, struct file *file)
 {
 	if (tracing_disabled)
 		return -ENODEV;
-	return single_open(file, tracing_clock_show, NULL);
+
+	return single_open(file, tracing_clock_show, inode->i_private);
 }
 
+struct ftrace_buffer_info {
+	struct trace_iterator	iter;
+	void			*spare;
+	unsigned int		read;
+};
+
 #ifdef CONFIG_TRACER_SNAPSHOT
 static int tracing_snapshot_open(struct inode *inode, struct file *file)
 {
+	struct trace_cpu *tc = inode->i_private;
 	struct trace_iterator *iter;
+	struct seq_file *m;
 	int ret = 0;
 
 	if (file->f_mode & FMODE_READ) {
 		iter = __tracing_open(inode, file, true);
 		if (IS_ERR(iter))
 			ret = PTR_ERR(iter);
+	} else {
+		/* Writes still need the seq_file to hold the private data */
+		m = kzalloc(sizeof(*m), GFP_KERNEL);
+		if (!m)
+			return -ENOMEM;
+		iter = kzalloc(sizeof(*iter), GFP_KERNEL);
+		if (!iter) {
+			kfree(m);
+			return -ENOMEM;
+		}
+		iter->tr = tc->tr;
+		iter->trace_buffer = &tc->tr->max_buffer;
+		iter->cpu_file = tc->cpu;
+		m->private = iter;
+		file->private_data = m;
 	}
+
 	return ret;
 }
 
@@ -4149,6 +4537,9 @@ static ssize_t
 tracing_snapshot_write(struct file *filp, const char __user *ubuf, size_t cnt,
 		       loff_t *ppos)
 {
+	struct seq_file *m = filp->private_data;
+	struct trace_iterator *iter = m->private;
+	struct trace_array *tr = iter->tr;
 	unsigned long val;
 	int ret;
 
@@ -4162,40 +4553,48 @@ tracing_snapshot_write(struct file *filp, const char __user *ubuf, size_t cnt,
 
 	mutex_lock(&trace_types_lock);
 
-	if (current_trace->use_max_tr) {
+	if (tr->current_trace->use_max_tr) {
 		ret = -EBUSY;
 		goto out;
 	}
 
 	switch (val) {
 	case 0:
-		if (current_trace->allocated_snapshot) {
-			/* free spare buffer */
-			ring_buffer_resize(max_tr.buffer, 1,
-					   RING_BUFFER_ALL_CPUS);
-			set_buffer_entries(&max_tr, 1);
-			tracing_reset_online_cpus(&max_tr);
-			current_trace->allocated_snapshot = false;
+		if (iter->cpu_file != RING_BUFFER_ALL_CPUS) {
+			ret = -EINVAL;
+			break;
 		}
+		if (tr->allocated_snapshot)
+			free_snapshot(tr);
 		break;
 	case 1:
-		if (!current_trace->allocated_snapshot) {
-			/* allocate spare buffer */
-			ret = resize_buffer_duplicate_size(&max_tr,
-					&global_trace, RING_BUFFER_ALL_CPUS);
+/* Only allow per-cpu swap if the ring buffer supports it */
+#ifndef CONFIG_RING_BUFFER_ALLOW_SWAP
+		if (iter->cpu_file != RING_BUFFER_ALL_CPUS) {
+			ret = -EINVAL;
+			break;
+		}
+#endif
+		if (!tr->allocated_snapshot) {
+			ret = alloc_snapshot(tr);
 			if (ret < 0)
 				break;
-			current_trace->allocated_snapshot = true;
 		}
-
 		local_irq_disable();
 		/* Now, we're going to swap */
-		update_max_tr(&global_trace, current, smp_processor_id());
+		if (iter->cpu_file == RING_BUFFER_ALL_CPUS)
+			update_max_tr(tr, current, smp_processor_id());
+		else
+			update_max_tr_single(tr, current, iter->cpu_file);
 		local_irq_enable();
 		break;
 	default:
-		if (current_trace->allocated_snapshot)
-			tracing_reset_online_cpus(&max_tr);
+		if (tr->allocated_snapshot) {
+			if (iter->cpu_file == RING_BUFFER_ALL_CPUS)
+				tracing_reset_online_cpus(&tr->max_buffer);
+			else
+				tracing_reset(&tr->max_buffer, iter->cpu_file);
+		}
 		break;
 	}
 
@@ -4207,6 +4606,51 @@ out:
 	mutex_unlock(&trace_types_lock);
 	return ret;
 }
+
+static int tracing_snapshot_release(struct inode *inode, struct file *file)
+{
+	struct seq_file *m = file->private_data;
+
+	if (file->f_mode & FMODE_READ)
+		return tracing_release(inode, file);
+
+	/* If write only, the seq_file is just a stub */
+	if (m)
+		kfree(m->private);
+	kfree(m);
+
+	return 0;
+}
+
+static int tracing_buffers_open(struct inode *inode, struct file *filp);
+static ssize_t tracing_buffers_read(struct file *filp, char __user *ubuf,
+				    size_t count, loff_t *ppos);
+static int tracing_buffers_release(struct inode *inode, struct file *file);
+static ssize_t tracing_buffers_splice_read(struct file *file, loff_t *ppos,
+		   struct pipe_inode_info *pipe, size_t len, unsigned int flags);
+
+static int snapshot_raw_open(struct inode *inode, struct file *filp)
+{
+	struct ftrace_buffer_info *info;
+	int ret;
+
+	ret = tracing_buffers_open(inode, filp);
+	if (ret < 0)
+		return ret;
+
+	info = filp->private_data;
+
+	if (info->iter.trace->use_max_tr) {
+		tracing_buffers_release(inode, filp);
+		return -EBUSY;
+	}
+
+	info->iter.snapshot = true;
+	info->iter.trace_buffer = &info->iter.tr->max_buffer;
+
+	return ret;
+}
+
 #endif /* CONFIG_TRACER_SNAPSHOT */
 
 
@@ -4234,10 +4678,9 @@ static const struct file_operations tracing_pipe_fops = {
 };
 
 static const struct file_operations tracing_entries_fops = {
-	.open		= tracing_entries_open,
+	.open		= tracing_open_generic,
 	.read		= tracing_entries_read,
 	.write		= tracing_entries_write,
-	.release	= tracing_entries_release,
 	.llseek		= generic_file_llseek,
 };
 
@@ -4272,20 +4715,23 @@ static const struct file_operations snapshot_fops = {
 	.read		= seq_read,
 	.write		= tracing_snapshot_write,
 	.llseek		= tracing_seek,
-	.release	= tracing_release,
+	.release	= tracing_snapshot_release,
 };
-#endif /* CONFIG_TRACER_SNAPSHOT */
 
-struct ftrace_buffer_info {
-	struct trace_array	*tr;
-	void			*spare;
-	int			cpu;
-	unsigned int		read;
+static const struct file_operations snapshot_raw_fops = {
+	.open		= snapshot_raw_open,
+	.read		= tracing_buffers_read,
+	.release	= tracing_buffers_release,
+	.splice_read	= tracing_buffers_splice_read,
+	.llseek		= no_llseek,
 };
 
+#endif /* CONFIG_TRACER_SNAPSHOT */
+
 static int tracing_buffers_open(struct inode *inode, struct file *filp)
 {
-	int cpu = (int)(long)inode->i_private;
+	struct trace_cpu *tc = inode->i_private;
+	struct trace_array *tr = tc->tr;
 	struct ftrace_buffer_info *info;
 
 	if (tracing_disabled)
@@ -4295,72 +4741,131 @@ static int tracing_buffers_open(struct inode *inode, struct file *filp)
 	if (!info)
 		return -ENOMEM;
 
-	info->tr	= &global_trace;
-	info->cpu	= cpu;
-	info->spare	= NULL;
+	mutex_lock(&trace_types_lock);
+
+	tr->ref++;
+
+	info->iter.tr		= tr;
+	info->iter.cpu_file	= tc->cpu;
+	info->iter.trace	= tr->current_trace;
+	info->iter.trace_buffer = &tr->trace_buffer;
+	info->spare		= NULL;
 	/* Force reading ring buffer for first read */
-	info->read	= (unsigned int)-1;
+	info->read		= (unsigned int)-1;
 
 	filp->private_data = info;
 
+	mutex_unlock(&trace_types_lock);
+
 	return nonseekable_open(inode, filp);
 }
 
+static unsigned int
+tracing_buffers_poll(struct file *filp, poll_table *poll_table)
+{
+	struct ftrace_buffer_info *info = filp->private_data;
+	struct trace_iterator *iter = &info->iter;
+
+	return trace_poll(iter, filp, poll_table);
+}
+
 static ssize_t
 tracing_buffers_read(struct file *filp, char __user *ubuf,
 		     size_t count, loff_t *ppos)
 {
 	struct ftrace_buffer_info *info = filp->private_data;
+	struct trace_iterator *iter = &info->iter;
 	ssize_t ret;
-	size_t size;
+	ssize_t size;
 
 	if (!count)
 		return 0;
 
+	mutex_lock(&trace_types_lock);
+
+#ifdef CONFIG_TRACER_MAX_TRACE
+	if (iter->snapshot && iter->tr->current_trace->use_max_tr) {
+		size = -EBUSY;
+		goto out_unlock;
+	}
+#endif
+
 	if (!info->spare)
-		info->spare = ring_buffer_alloc_read_page(info->tr->buffer, info->cpu);
+		info->spare = ring_buffer_alloc_read_page(iter->trace_buffer->buffer,
+							  iter->cpu_file);
+	size = -ENOMEM;
 	if (!info->spare)
-		return -ENOMEM;
+		goto out_unlock;
 
 	/* Do we have previous read data to read? */
 	if (info->read < PAGE_SIZE)
 		goto read;
 
-	trace_access_lock(info->cpu);
-	ret = ring_buffer_read_page(info->tr->buffer,
+ again:
+	trace_access_lock(iter->cpu_file);
+	ret = ring_buffer_read_page(iter->trace_buffer->buffer,
 				    &info->spare,
 				    count,
-				    info->cpu, 0);
-	trace_access_unlock(info->cpu);
-	if (ret < 0)
-		return 0;
+				    iter->cpu_file, 0);
+	trace_access_unlock(iter->cpu_file);
 
-	info->read = 0;
+	if (ret < 0) {
+		if (trace_empty(iter)) {
+			if ((filp->f_flags & O_NONBLOCK)) {
+				size = -EAGAIN;
+				goto out_unlock;
+			}
+			mutex_unlock(&trace_types_lock);
+			iter->trace->wait_pipe(iter);
+			mutex_lock(&trace_types_lock);
+			if (signal_pending(current)) {
+				size = -EINTR;
+				goto out_unlock;
+			}
+			goto again;
+		}
+		size = 0;
+		goto out_unlock;
+	}
 
-read:
+	info->read = 0;
+ read:
 	size = PAGE_SIZE - info->read;
 	if (size > count)
 		size = count;
 
 	ret = copy_to_user(ubuf, info->spare + info->read, size);
-	if (ret == size)
-		return -EFAULT;
+	if (ret == size) {
+		size = -EFAULT;
+		goto out_unlock;
+	}
 	size -= ret;
 
 	*ppos += size;
 	info->read += size;
 
+ out_unlock:
+	mutex_unlock(&trace_types_lock);
+
 	return size;
 }
 
 static int tracing_buffers_release(struct inode *inode, struct file *file)
 {
 	struct ftrace_buffer_info *info = file->private_data;
+	struct trace_iterator *iter = &info->iter;
+
+	mutex_lock(&trace_types_lock);
+
+	WARN_ON(!iter->tr->ref);
+	iter->tr->ref--;
 
 	if (info->spare)
-		ring_buffer_free_read_page(info->tr->buffer, info->spare);
+		ring_buffer_free_read_page(iter->trace_buffer->buffer, info->spare);
 	kfree(info);
 
+	mutex_unlock(&trace_types_lock);
+
 	return 0;
 }
 
@@ -4425,6 +4930,7 @@ tracing_buffers_splice_read(struct file *file, loff_t *ppos,
 			    unsigned int flags)
 {
 	struct ftrace_buffer_info *info = file->private_data;
+	struct trace_iterator *iter = &info->iter;
 	struct partial_page partial_def[PIPE_DEF_BUFFERS];
 	struct page *pages_def[PIPE_DEF_BUFFERS];
 	struct splice_pipe_desc spd = {
@@ -4437,10 +4943,21 @@ tracing_buffers_splice_read(struct file *file, loff_t *ppos,
 	};
 	struct buffer_ref *ref;
 	int entries, size, i;
-	size_t ret;
+	ssize_t ret;
 
-	if (splice_grow_spd(pipe, &spd))
-		return -ENOMEM;
+	mutex_lock(&trace_types_lock);
+
+#ifdef CONFIG_TRACER_MAX_TRACE
+	if (iter->snapshot && iter->tr->current_trace->use_max_tr) {
+		ret = -EBUSY;
+		goto out;
+	}
+#endif
+
+	if (splice_grow_spd(pipe, &spd)) {
+		ret = -ENOMEM;
+		goto out;
+	}
 
 	if (*ppos & (PAGE_SIZE - 1)) {
 		ret = -EINVAL;
@@ -4455,8 +4972,9 @@ tracing_buffers_splice_read(struct file *file, loff_t *ppos,
 		len &= PAGE_MASK;
 	}
 
-	trace_access_lock(info->cpu);
-	entries = ring_buffer_entries_cpu(info->tr->buffer, info->cpu);
+ again:
+	trace_access_lock(iter->cpu_file);
+	entries = ring_buffer_entries_cpu(iter->trace_buffer->buffer, iter->cpu_file);
 
 	for (i = 0; i < pipe->buffers && len && entries; i++, len -= PAGE_SIZE) {
 		struct page *page;
@@ -4467,15 +4985,15 @@ tracing_buffers_splice_read(struct file *file, loff_t *ppos,
 			break;
 
 		ref->ref = 1;
-		ref->buffer = info->tr->buffer;
-		ref->page = ring_buffer_alloc_read_page(ref->buffer, info->cpu);
+		ref->buffer = iter->trace_buffer->buffer;
+		ref->page = ring_buffer_alloc_read_page(ref->buffer, iter->cpu_file);
 		if (!ref->page) {
 			kfree(ref);
 			break;
 		}
 
 		r = ring_buffer_read_page(ref->buffer, &ref->page,
-					  len, info->cpu, 1);
+					  len, iter->cpu_file, 1);
 		if (r < 0) {
 			ring_buffer_free_read_page(ref->buffer, ref->page);
 			kfree(ref);
@@ -4499,31 +5017,40 @@ tracing_buffers_splice_read(struct file *file, loff_t *ppos,
 		spd.nr_pages++;
 		*ppos += PAGE_SIZE;
 
-		entries = ring_buffer_entries_cpu(info->tr->buffer, info->cpu);
+		entries = ring_buffer_entries_cpu(iter->trace_buffer->buffer, iter->cpu_file);
 	}
 
-	trace_access_unlock(info->cpu);
+	trace_access_unlock(iter->cpu_file);
 	spd.nr_pages = i;
 
 	/* did we read anything? */
 	if (!spd.nr_pages) {
-		if (flags & SPLICE_F_NONBLOCK)
+		if ((file->f_flags & O_NONBLOCK) || (flags & SPLICE_F_NONBLOCK)) {
 			ret = -EAGAIN;
-		else
-			ret = 0;
-		/* TODO: block */
-		goto out;
+			goto out;
+		}
+		mutex_unlock(&trace_types_lock);
+		iter->trace->wait_pipe(iter);
+		mutex_lock(&trace_types_lock);
+		if (signal_pending(current)) {
+			ret = -EINTR;
+			goto out;
+		}
+		goto again;
 	}
 
 	ret = splice_to_pipe(pipe, &spd);
 	splice_shrink_spd(&spd);
 out:
+	mutex_unlock(&trace_types_lock);
+
 	return ret;
 }
 
 static const struct file_operations tracing_buffers_fops = {
 	.open		= tracing_buffers_open,
 	.read		= tracing_buffers_read,
+	.poll		= tracing_buffers_poll,
 	.release	= tracing_buffers_release,
 	.splice_read	= tracing_buffers_splice_read,
 	.llseek		= no_llseek,
@@ -4533,12 +5060,14 @@ static ssize_t
 tracing_stats_read(struct file *filp, char __user *ubuf,
 		   size_t count, loff_t *ppos)
 {
-	unsigned long cpu = (unsigned long)filp->private_data;
-	struct trace_array *tr = &global_trace;
+	struct trace_cpu *tc = filp->private_data;
+	struct trace_array *tr = tc->tr;
+	struct trace_buffer *trace_buf = &tr->trace_buffer;
 	struct trace_seq *s;
 	unsigned long cnt;
 	unsigned long long t;
 	unsigned long usec_rem;
+	int cpu = tc->cpu;
 
 	s = kmalloc(sizeof(*s), GFP_KERNEL);
 	if (!s)
@@ -4546,41 +5075,41 @@ tracing_stats_read(struct file *filp, char __user *ubuf,
 
 	trace_seq_init(s);
 
-	cnt = ring_buffer_entries_cpu(tr->buffer, cpu);
+	cnt = ring_buffer_entries_cpu(trace_buf->buffer, cpu);
 	trace_seq_printf(s, "entries: %ld\n", cnt);
 
-	cnt = ring_buffer_overrun_cpu(tr->buffer, cpu);
+	cnt = ring_buffer_overrun_cpu(trace_buf->buffer, cpu);
 	trace_seq_printf(s, "overrun: %ld\n", cnt);
 
-	cnt = ring_buffer_commit_overrun_cpu(tr->buffer, cpu);
+	cnt = ring_buffer_commit_overrun_cpu(trace_buf->buffer, cpu);
 	trace_seq_printf(s, "commit overrun: %ld\n", cnt);
 
-	cnt = ring_buffer_bytes_cpu(tr->buffer, cpu);
+	cnt = ring_buffer_bytes_cpu(trace_buf->buffer, cpu);
 	trace_seq_printf(s, "bytes: %ld\n", cnt);
 
 	if (trace_clocks[trace_clock_id].in_ns) {
 		/* local or global for trace_clock */
-		t = ns2usecs(ring_buffer_oldest_event_ts(tr->buffer, cpu));
+		t = ns2usecs(ring_buffer_oldest_event_ts(trace_buf->buffer, cpu));
 		usec_rem = do_div(t, USEC_PER_SEC);
 		trace_seq_printf(s, "oldest event ts: %5llu.%06lu\n",
 								t, usec_rem);
 
-		t = ns2usecs(ring_buffer_time_stamp(tr->buffer, cpu));
+		t = ns2usecs(ring_buffer_time_stamp(trace_buf->buffer, cpu));
 		usec_rem = do_div(t, USEC_PER_SEC);
 		trace_seq_printf(s, "now ts: %5llu.%06lu\n", t, usec_rem);
 	} else {
 		/* counter or tsc mode for trace_clock */
 		trace_seq_printf(s, "oldest event ts: %llu\n",
-				ring_buffer_oldest_event_ts(tr->buffer, cpu));
+				ring_buffer_oldest_event_ts(trace_buf->buffer, cpu));
 
 		trace_seq_printf(s, "now ts: %llu\n",
-				ring_buffer_time_stamp(tr->buffer, cpu));
+				ring_buffer_time_stamp(trace_buf->buffer, cpu));
 	}
 
-	cnt = ring_buffer_dropped_events_cpu(tr->buffer, cpu);
+	cnt = ring_buffer_dropped_events_cpu(trace_buf->buffer, cpu);
 	trace_seq_printf(s, "dropped events: %ld\n", cnt);
 
-	cnt = ring_buffer_read_events_cpu(tr->buffer, cpu);
+	cnt = ring_buffer_read_events_cpu(trace_buf->buffer, cpu);
 	trace_seq_printf(s, "read events: %ld\n", cnt);
 
 	count = simple_read_from_buffer(ubuf, count, ppos, s->buffer, s->len);
@@ -4632,60 +5161,161 @@ static const struct file_operations tracing_dyn_info_fops = {
 	.read		= tracing_read_dyn_info,
 	.llseek		= generic_file_llseek,
 };
-#endif
+#endif /* CONFIG_DYNAMIC_FTRACE */
 
-static struct dentry *d_tracer;
+#if defined(CONFIG_TRACER_SNAPSHOT) && defined(CONFIG_DYNAMIC_FTRACE)
+static void
+ftrace_snapshot(unsigned long ip, unsigned long parent_ip, void **data)
+{
+	tracing_snapshot();
+}
 
-struct dentry *tracing_init_dentry(void)
+static void
+ftrace_count_snapshot(unsigned long ip, unsigned long parent_ip, void **data)
+{
+	unsigned long *count = (long *)data;
+
+	if (!*count)
+		return;
+
+	if (*count != -1)
+		(*count)--;
+
+	tracing_snapshot();
+}
+
+static int
+ftrace_snapshot_print(struct seq_file *m, unsigned long ip,
+		      struct ftrace_probe_ops *ops, void *data)
+{
+	long count = (long)data;
+
+	seq_printf(m, "%ps:", (void *)ip);
+
+	seq_printf(m, "snapshot");
+
+	if (count == -1)
+		seq_printf(m, ":unlimited\n");
+	else
+		seq_printf(m, ":count=%ld\n", count);
+
+	return 0;
+}
+
+static struct ftrace_probe_ops snapshot_probe_ops = {
+	.func			= ftrace_snapshot,
+	.print			= ftrace_snapshot_print,
+};
+
+static struct ftrace_probe_ops snapshot_count_probe_ops = {
+	.func			= ftrace_count_snapshot,
+	.print			= ftrace_snapshot_print,
+};
+
+static int
+ftrace_trace_snapshot_callback(struct ftrace_hash *hash,
+			       char *glob, char *cmd, char *param, int enable)
 {
-	static int once;
+	struct ftrace_probe_ops *ops;
+	void *count = (void *)-1;
+	char *number;
+	int ret;
 
-	if (d_tracer)
-		return d_tracer;
+	/* hash funcs only work with set_ftrace_filter */
+	if (!enable)
+		return -EINVAL;
+
+	ops = param ? &snapshot_count_probe_ops :  &snapshot_probe_ops;
+
+	if (glob[0] == '!') {
+		unregister_ftrace_function_probe_func(glob+1, ops);
+		return 0;
+	}
+
+	if (!param)
+		goto out_reg;
+
+	number = strsep(&param, ":");
+
+	if (!strlen(number))
+		goto out_reg;
+
+	/*
+	 * We use the callback data field (which is a pointer)
+	 * as our counter.
+	 */
+	ret = kstrtoul(number, 0, (unsigned long *)&count);
+	if (ret)
+		return ret;
+
+ out_reg:
+	ret = register_ftrace_function_probe(glob, ops, count);
+
+	if (ret >= 0)
+		alloc_snapshot(&global_trace);
+
+	return ret < 0 ? ret : 0;
+}
+
+static struct ftrace_func_command ftrace_snapshot_cmd = {
+	.name			= "snapshot",
+	.func			= ftrace_trace_snapshot_callback,
+};
+
+static int register_snapshot_cmd(void)
+{
+	return register_ftrace_command(&ftrace_snapshot_cmd);
+}
+#else
+static inline int register_snapshot_cmd(void) { return 0; }
+#endif /* defined(CONFIG_TRACER_SNAPSHOT) && defined(CONFIG_DYNAMIC_FTRACE) */
+
+struct dentry *tracing_init_dentry_tr(struct trace_array *tr)
+{
+	if (tr->dir)
+		return tr->dir;
 
 	if (!debugfs_initialized())
 		return NULL;
 
-	d_tracer = debugfs_create_dir("tracing", NULL);
+	if (tr->flags & TRACE_ARRAY_FL_GLOBAL)
+		tr->dir = debugfs_create_dir("tracing", NULL);
 
-	if (!d_tracer && !once) {
-		once = 1;
-		pr_warning("Could not create debugfs directory 'tracing'\n");
-		return NULL;
-	}
+	if (!tr->dir)
+		pr_warn_once("Could not create debugfs directory 'tracing'\n");
 
-	return d_tracer;
+	return tr->dir;
 }
 
-static struct dentry *d_percpu;
+struct dentry *tracing_init_dentry(void)
+{
+	return tracing_init_dentry_tr(&global_trace);
+}
 
-static struct dentry *tracing_dentry_percpu(void)
+static struct dentry *tracing_dentry_percpu(struct trace_array *tr, int cpu)
 {
-	static int once;
 	struct dentry *d_tracer;
 
-	if (d_percpu)
-		return d_percpu;
-
-	d_tracer = tracing_init_dentry();
+	if (tr->percpu_dir)
+		return tr->percpu_dir;
 
+	d_tracer = tracing_init_dentry_tr(tr);
 	if (!d_tracer)
 		return NULL;
 
-	d_percpu = debugfs_create_dir("per_cpu", d_tracer);
+	tr->percpu_dir = debugfs_create_dir("per_cpu", d_tracer);
 
-	if (!d_percpu && !once) {
-		once = 1;
-		pr_warning("Could not create debugfs directory 'per_cpu'\n");
-		return NULL;
-	}
+	WARN_ONCE(!tr->percpu_dir,
+		  "Could not create debugfs directory 'per_cpu/%d'\n", cpu);
 
-	return d_percpu;
+	return tr->percpu_dir;
 }
 
-static void tracing_init_debugfs_percpu(long cpu)
+static void
+tracing_init_debugfs_percpu(struct trace_array *tr, long cpu)
 {
-	struct dentry *d_percpu = tracing_dentry_percpu();
+	struct trace_array_cpu *data = per_cpu_ptr(tr->trace_buffer.data, cpu);
+	struct dentry *d_percpu = tracing_dentry_percpu(tr, cpu);
 	struct dentry *d_cpu;
 	char cpu_dir[30]; /* 30 characters should be more than enough */
 
@@ -4701,20 +5331,28 @@ static void tracing_init_debugfs_percpu(long cpu)
 
 	/* per cpu trace_pipe */
 	trace_create_file("trace_pipe", 0444, d_cpu,
-			(void *) cpu, &tracing_pipe_fops);
+			(void *)&data->trace_cpu, &tracing_pipe_fops);
 
 	/* per cpu trace */
 	trace_create_file("trace", 0644, d_cpu,
-			(void *) cpu, &tracing_fops);
+			(void *)&data->trace_cpu, &tracing_fops);
 
 	trace_create_file("trace_pipe_raw", 0444, d_cpu,
-			(void *) cpu, &tracing_buffers_fops);
+			(void *)&data->trace_cpu, &tracing_buffers_fops);
 
 	trace_create_file("stats", 0444, d_cpu,
-			(void *) cpu, &tracing_stats_fops);
+			(void *)&data->trace_cpu, &tracing_stats_fops);
 
 	trace_create_file("buffer_size_kb", 0444, d_cpu,
-			(void *) cpu, &tracing_entries_fops);
+			(void *)&data->trace_cpu, &tracing_entries_fops);
+
+#ifdef CONFIG_TRACER_SNAPSHOT
+	trace_create_file("snapshot", 0644, d_cpu,
+			  (void *)&data->trace_cpu, &snapshot_fops);
+
+	trace_create_file("snapshot_raw", 0444, d_cpu,
+			(void *)&data->trace_cpu, &snapshot_raw_fops);
+#endif
 }
 
 #ifdef CONFIG_FTRACE_SELFTEST
@@ -4725,6 +5363,7 @@ static void tracing_init_debugfs_percpu(long cpu)
 struct trace_option_dentry {
 	struct tracer_opt		*opt;
 	struct tracer_flags		*flags;
+	struct trace_array		*tr;
 	struct dentry			*entry;
 };
 
@@ -4760,7 +5399,7 @@ trace_options_write(struct file *filp, const char __user *ubuf, size_t cnt,
 
 	if (!!(topt->flags->val & topt->opt->bit) != val) {
 		mutex_lock(&trace_types_lock);
-		ret = __set_tracer_option(current_trace, topt->flags,
+		ret = __set_tracer_option(topt->tr->current_trace, topt->flags,
 					  topt->opt, !val);
 		mutex_unlock(&trace_types_lock);
 		if (ret)
@@ -4799,6 +5438,7 @@ static ssize_t
 trace_options_core_write(struct file *filp, const char __user *ubuf, size_t cnt,
 			 loff_t *ppos)
 {
+	struct trace_array *tr = &global_trace;
 	long index = (long)filp->private_data;
 	unsigned long val;
 	int ret;
@@ -4811,7 +5451,7 @@ trace_options_core_write(struct file *filp, const char __user *ubuf, size_t cnt,
 		return -EINVAL;
 
 	mutex_lock(&trace_types_lock);
-	ret = set_tracer_flag(1 << index, val);
+	ret = set_tracer_flag(tr, 1 << index, val);
 	mutex_unlock(&trace_types_lock);
 
 	if (ret < 0)
@@ -4845,40 +5485,41 @@ struct dentry *trace_create_file(const char *name,
 }
 
 
-static struct dentry *trace_options_init_dentry(void)
+static struct dentry *trace_options_init_dentry(struct trace_array *tr)
 {
 	struct dentry *d_tracer;
-	static struct dentry *t_options;
 
-	if (t_options)
-		return t_options;
+	if (tr->options)
+		return tr->options;
 
-	d_tracer = tracing_init_dentry();
+	d_tracer = tracing_init_dentry_tr(tr);
 	if (!d_tracer)
 		return NULL;
 
-	t_options = debugfs_create_dir("options", d_tracer);
-	if (!t_options) {
+	tr->options = debugfs_create_dir("options", d_tracer);
+	if (!tr->options) {
 		pr_warning("Could not create debugfs directory 'options'\n");
 		return NULL;
 	}
 
-	return t_options;
+	return tr->options;
 }
 
 static void
-create_trace_option_file(struct trace_option_dentry *topt,
+create_trace_option_file(struct trace_array *tr,
+			 struct trace_option_dentry *topt,
 			 struct tracer_flags *flags,
 			 struct tracer_opt *opt)
 {
 	struct dentry *t_options;
 
-	t_options = trace_options_init_dentry();
+	t_options = trace_options_init_dentry(tr);
 	if (!t_options)
 		return;
 
 	topt->flags = flags;
 	topt->opt = opt;
+	topt->tr = tr;
 
 	topt->entry = trace_create_file(opt->name, 0644, t_options, topt,
 				    &trace_options_fops);
@@ -4886,7 +5527,7 @@ create_trace_option_file(struct trace_option_dentry *topt,
 }
 
 static struct trace_option_dentry *
-create_trace_option_files(struct tracer *tracer)
+create_trace_option_files(struct trace_array *tr, struct tracer *tracer)
 {
 	struct trace_option_dentry *topts;
 	struct tracer_flags *flags;
@@ -4911,7 +5552,7 @@ create_trace_option_files(struct tracer *tracer)
 		return NULL;
 
 	for (cnt = 0; opts[cnt].name; cnt++)
-		create_trace_option_file(&topts[cnt], flags,
+		create_trace_option_file(tr, &topts[cnt], flags,
 					 &opts[cnt]);
 
 	return topts;
@@ -4934,11 +5575,12 @@ destroy_trace_option_files(struct trace_option_dentry *topts)
 }
 
 static struct dentry *
-create_trace_option_core_file(const char *option, long index)
+create_trace_option_core_file(struct trace_array *tr,
+			      const char *option, long index)
 {
 	struct dentry *t_options;
 
-	t_options = trace_options_init_dentry();
+	t_options = trace_options_init_dentry(tr);
 	if (!t_options)
 		return NULL;
 
@@ -4946,17 +5588,17 @@ create_trace_option_core_file(const char *option, long index)
 				    &trace_options_core_fops);
 }
 
-static __init void create_trace_options_dir(void)
+static __init void create_trace_options_dir(struct trace_array *tr)
 {
 	struct dentry *t_options;
 	int i;
 
-	t_options = trace_options_init_dentry();
+	t_options = trace_options_init_dentry(tr);
 	if (!t_options)
 		return;
 
 	for (i = 0; trace_options[i]; i++)
-		create_trace_option_core_file(trace_options[i], i);
+		create_trace_option_core_file(tr, trace_options[i], i);
 }
 
 static ssize_t
@@ -4964,7 +5606,7 @@ rb_simple_read(struct file *filp, char __user *ubuf,
 	       size_t cnt, loff_t *ppos)
 {
 	struct trace_array *tr = filp->private_data;
-	struct ring_buffer *buffer = tr->buffer;
+	struct ring_buffer *buffer = tr->trace_buffer.buffer;
 	char buf[64];
 	int r;
 
@@ -4983,7 +5625,7 @@ rb_simple_write(struct file *filp, const char __user *ubuf,
 		size_t cnt, loff_t *ppos)
 {
 	struct trace_array *tr = filp->private_data;
-	struct ring_buffer *buffer = tr->buffer;
+	struct ring_buffer *buffer = tr->trace_buffer.buffer;
 	unsigned long val;
 	int ret;
 
@@ -4995,12 +5637,12 @@ rb_simple_write(struct file *filp, const char __user *ubuf,
 		mutex_lock(&trace_types_lock);
 		if (val) {
 			ring_buffer_record_on(buffer);
-			if (current_trace->start)
-				current_trace->start(tr);
+			if (tr->current_trace->start)
+				tr->current_trace->start(tr);
 		} else {
 			ring_buffer_record_off(buffer);
-			if (current_trace->stop)
-				current_trace->stop(tr);
+			if (tr->current_trace->stop)
+				tr->current_trace->stop(tr);
 		}
 		mutex_unlock(&trace_types_lock);
 	}
@@ -5017,23 +5659,310 @@ static const struct file_operations rb_simple_fops = {
 	.llseek		= default_llseek,
 };
 
+struct dentry *trace_instance_dir;
+
+static void
+init_tracer_debugfs(struct trace_array *tr, struct dentry *d_tracer);
+
+static void init_trace_buffers(struct trace_array *tr, struct trace_buffer *buf)
+{
+	int cpu;
+
+	for_each_tracing_cpu(cpu) {
+		memset(per_cpu_ptr(buf->data, cpu), 0, sizeof(struct trace_array_cpu));
+		per_cpu_ptr(buf->data, cpu)->trace_cpu.cpu = cpu;
+		per_cpu_ptr(buf->data, cpu)->trace_cpu.tr = tr;
+	}
+}
+
+static int
+allocate_trace_buffer(struct trace_array *tr, struct trace_buffer *buf, int size)
+{
+	enum ring_buffer_flags rb_flags;
+
+	rb_flags = trace_flags & TRACE_ITER_OVERWRITE ? RB_FL_OVERWRITE : 0;
+
+	buf->buffer = ring_buffer_alloc(size, rb_flags);
+	if (!buf->buffer)
+		return -ENOMEM;
+
+	buf->data = alloc_percpu(struct trace_array_cpu);
+	if (!buf->data) {
+		ring_buffer_free(buf->buffer);
+		return -ENOMEM;
+	}
+
+	init_trace_buffers(tr, buf);
+
+	/* Allocate the first page for all buffers */
+	set_buffer_entries(&tr->trace_buffer,
+			   ring_buffer_size(tr->trace_buffer.buffer, 0));
+
+	return 0;
+}
+
+static int allocate_trace_buffers(struct trace_array *tr, int size)
+{
+	int ret;
+
+	ret = allocate_trace_buffer(tr, &tr->trace_buffer, size);
+	if (ret)
+		return ret;
+
+#ifdef CONFIG_TRACER_MAX_TRACE
+	ret = allocate_trace_buffer(tr, &tr->max_buffer,
+				    allocate_snapshot ? size : 1);
+	if (WARN_ON(ret)) {
+		ring_buffer_free(tr->trace_buffer.buffer);
+		free_percpu(tr->trace_buffer.data);
+		return -ENOMEM;
+	}
+	tr->allocated_snapshot = allocate_snapshot;
+
+	/*
+	 * Only the top level trace array gets its snapshot allocated
+	 * from the kernel command line.
+	 */
+	allocate_snapshot = false;
+#endif
+	return 0;
+}
+
+static int new_instance_create(const char *name)
+{
+	struct trace_array *tr;
+	int ret;
+
+	mutex_lock(&trace_types_lock);
+
+	ret = -EEXIST;
+	list_for_each_entry(tr, &ftrace_trace_arrays, list) {
+		if (tr->name && strcmp(tr->name, name) == 0)
+			goto out_unlock;
+	}
+
+	ret = -ENOMEM;
+	tr = kzalloc(sizeof(*tr), GFP_KERNEL);
+	if (!tr)
+		goto out_unlock;
+
+	tr->name = kstrdup(name, GFP_KERNEL);
+	if (!tr->name)
+		goto out_free_tr;
+
+	raw_spin_lock_init(&tr->start_lock);
+
+	tr->current_trace = &nop_trace;
+
+	INIT_LIST_HEAD(&tr->systems);
+	INIT_LIST_HEAD(&tr->events);
+
+	if (allocate_trace_buffers(tr, trace_buf_size) < 0)
+		goto out_free_tr;
+
+	/* Holder for file callbacks */
+	tr->trace_cpu.cpu = RING_BUFFER_ALL_CPUS;
+	tr->trace_cpu.tr = tr;
+
+	tr->dir = debugfs_create_dir(name, trace_instance_dir);
+	if (!tr->dir)
+		goto out_free_tr;
+
+	ret = event_trace_add_tracer(tr->dir, tr);
+	if (ret)
+		goto out_free_tr;
+
+	init_tracer_debugfs(tr, tr->dir);
+
+	list_add(&tr->list, &ftrace_trace_arrays);
+
+	mutex_unlock(&trace_types_lock);
+
+	return 0;
+
+ out_free_tr:
+	if (tr->trace_buffer.buffer)
+		ring_buffer_free(tr->trace_buffer.buffer);
+	kfree(tr->name);
+	kfree(tr);
+
+ out_unlock:
+	mutex_unlock(&trace_types_lock);
+
+	return ret;
+
+}
+
+static int instance_delete(const char *name)
+{
+	struct trace_array *tr;
+	int found = 0;
+	int ret;
+
+	mutex_lock(&trace_types_lock);
+
+	ret = -ENODEV;
+	list_for_each_entry(tr, &ftrace_trace_arrays, list) {
+		if (tr->name && strcmp(tr->name, name) == 0) {
+			found = 1;
+			break;
+		}
+	}
+	if (!found)
+		goto out_unlock;
+
+	ret = -EBUSY;
+	if (tr->ref)
+		goto out_unlock;
+
+	list_del(&tr->list);
+
+	event_trace_del_tracer(tr);
+	debugfs_remove_recursive(tr->dir);
+	free_percpu(tr->trace_buffer.data);
+	ring_buffer_free(tr->trace_buffer.buffer);
+
+	kfree(tr->name);
+	kfree(tr);
+
+	ret = 0;
+
+ out_unlock:
+	mutex_unlock(&trace_types_lock);
+
+	return ret;
+}
+
+static int instance_mkdir (struct inode *inode, struct dentry *dentry, umode_t mode)
+{
+	struct dentry *parent;
+	int ret;
+
+	/* Paranoid: Make sure the parent is the "instances" directory */
+	parent = hlist_entry(inode->i_dentry.first, struct dentry, d_alias);
+	if (WARN_ON_ONCE(parent != trace_instance_dir))
+		return -ENOENT;
+
+	/*
+	 * The inode mutex is locked, but debugfs_create_dir() will also
+	 * take the mutex. As the instances directory can not be destroyed
+	 * or changed in any other way, it is safe to unlock it, and
+	 * let the dentry try. If two users try to make the same dir at
+	 * the same time, then the new_instance_create() will determine the
+	 * winner.
+	 */
+	mutex_unlock(&inode->i_mutex);
+
+	ret = new_instance_create(dentry->d_iname);
+
+	mutex_lock(&inode->i_mutex);
+
+	return ret;
+}
+
+static int instance_rmdir(struct inode *inode, struct dentry *dentry)
+{
+	struct dentry *parent;
+	int ret;
+
+	/* Paranoid: Make sure the parent is the "instances" directory */
+	parent = hlist_entry(inode->i_dentry.first, struct dentry, d_alias);
+	if (WARN_ON_ONCE(parent != trace_instance_dir))
+		return -ENOENT;
+
+	/* The caller did a dget() on dentry */
+	mutex_unlock(&dentry->d_inode->i_mutex);
+
+	/*
+	 * The inode mutex is locked, but debugfs_create_dir() will also
+	 * take the mutex. As the instances directory can not be destroyed
+	 * or changed in any other way, it is safe to unlock it, and
+	 * let the dentry try. If two users try to make the same dir at
+	 * the same time, then the instance_delete() will determine the
+	 * winner.
+	 */
+	mutex_unlock(&inode->i_mutex);
+
+	ret = instance_delete(dentry->d_iname);
+
+	mutex_lock_nested(&inode->i_mutex, I_MUTEX_PARENT);
+	mutex_lock(&dentry->d_inode->i_mutex);
+
+	return ret;
+}
+
+static const struct inode_operations instance_dir_inode_operations = {
+	.lookup		= simple_lookup,
+	.mkdir		= instance_mkdir,
+	.rmdir		= instance_rmdir,
+};
+
+static __init void create_trace_instances(struct dentry *d_tracer)
+{
+	trace_instance_dir = debugfs_create_dir("instances", d_tracer);
+	if (WARN_ON(!trace_instance_dir))
+		return;
+
+	/* Hijack the dir inode operations, to allow mkdir */
+	trace_instance_dir->d_inode->i_op = &instance_dir_inode_operations;
+}
+
+static void
+init_tracer_debugfs(struct trace_array *tr, struct dentry *d_tracer)
+{
+	int cpu;
+
+	trace_create_file("trace_options", 0644, d_tracer,
+			  tr, &tracing_iter_fops);
+
+	trace_create_file("trace", 0644, d_tracer,
+			(void *)&tr->trace_cpu, &tracing_fops);
+
+	trace_create_file("trace_pipe", 0444, d_tracer,
+			(void *)&tr->trace_cpu, &tracing_pipe_fops);
+
+	trace_create_file("buffer_size_kb", 0644, d_tracer,
+			(void *)&tr->trace_cpu, &tracing_entries_fops);
+
+	trace_create_file("buffer_total_size_kb", 0444, d_tracer,
+			  tr, &tracing_total_entries_fops);
+
+	trace_create_file("free_buffer", 0644, d_tracer,
+			  tr, &tracing_free_buffer_fops);
+
+	trace_create_file("trace_marker", 0220, d_tracer,
+			  tr, &tracing_mark_fops);
+
+	trace_create_file("trace_clock", 0644, d_tracer, tr,
+			  &trace_clock_fops);
+
+	trace_create_file("tracing_on", 0644, d_tracer,
+			    tr, &rb_simple_fops);
+
+#ifdef CONFIG_TRACER_SNAPSHOT
+	trace_create_file("snapshot", 0644, d_tracer,
+			  (void *)&tr->trace_cpu, &snapshot_fops);
+#endif
+
+	for_each_tracing_cpu(cpu)
+		tracing_init_debugfs_percpu(tr, cpu);
+
+}
+
 static __init int tracer_init_debugfs(void)
 {
 	struct dentry *d_tracer;
-	int cpu;
 
 	trace_access_lock_init();
 
 	d_tracer = tracing_init_dentry();
+	if (!d_tracer)
+		return 0;
 
-	trace_create_file("trace_options", 0644, d_tracer,
-			NULL, &tracing_iter_fops);
+	init_tracer_debugfs(&global_trace, d_tracer);
 
 	trace_create_file("tracing_cpumask", 0644, d_tracer,
-			NULL, &tracing_cpumask_fops);
-
-	trace_create_file("trace", 0644, d_tracer,
-			(void *) TRACE_PIPE_ALL_CPU, &tracing_fops);
+			&global_trace, &tracing_cpumask_fops);
 
 	trace_create_file("available_tracers", 0444, d_tracer,
 			&global_trace, &show_traces_fops);
@@ -5052,44 +5981,17 @@ static __init int tracer_init_debugfs(void)
 	trace_create_file("README", 0444, d_tracer,
 			NULL, &tracing_readme_fops);
 
-	trace_create_file("trace_pipe", 0444, d_tracer,
-			(void *) TRACE_PIPE_ALL_CPU, &tracing_pipe_fops);
-
-	trace_create_file("buffer_size_kb", 0644, d_tracer,
-			(void *) RING_BUFFER_ALL_CPUS, &tracing_entries_fops);
-
-	trace_create_file("buffer_total_size_kb", 0444, d_tracer,
-			&global_trace, &tracing_total_entries_fops);
-
-	trace_create_file("free_buffer", 0644, d_tracer,
-			&global_trace, &tracing_free_buffer_fops);
-
-	trace_create_file("trace_marker", 0220, d_tracer,
-			NULL, &tracing_mark_fops);
-
 	trace_create_file("saved_cmdlines", 0444, d_tracer,
 			NULL, &tracing_saved_cmdlines_fops);
 
-	trace_create_file("trace_clock", 0644, d_tracer, NULL,
-			  &trace_clock_fops);
-
-	trace_create_file("tracing_on", 0644, d_tracer,
-			    &global_trace, &rb_simple_fops);
-
 #ifdef CONFIG_DYNAMIC_FTRACE
 	trace_create_file("dyn_ftrace_total_info", 0444, d_tracer,
 			&ftrace_update_tot_cnt, &tracing_dyn_info_fops);
 #endif
 
-#ifdef CONFIG_TRACER_SNAPSHOT
-	trace_create_file("snapshot", 0644, d_tracer,
-			  (void *) TRACE_PIPE_ALL_CPU, &snapshot_fops);
-#endif
+	create_trace_instances(d_tracer);
 
-	create_trace_options_dir();
-
-	for_each_tracing_cpu(cpu)
-		tracing_init_debugfs_percpu(cpu);
+	create_trace_options_dir(&global_trace);
 
 	return 0;
 }
@@ -5145,8 +6047,8 @@ void
 trace_printk_seq(struct trace_seq *s)
 {
 	/* Probably should print a warning here. */
-	if (s->len >= 1000)
-		s->len = 1000;
+	if (s->len >= TRACE_MAX_PRINT)
+		s->len = TRACE_MAX_PRINT;
 
 	/* should be zero ended, but we are paranoid. */
 	s->buffer[s->len] = 0;
@@ -5159,46 +6061,43 @@ trace_printk_seq(struct trace_seq *s)
 void trace_init_global_iter(struct trace_iterator *iter)
 {
 	iter->tr = &global_trace;
-	iter->trace = current_trace;
-	iter->cpu_file = TRACE_PIPE_ALL_CPU;
+	iter->trace = iter->tr->current_trace;
+	iter->cpu_file = RING_BUFFER_ALL_CPUS;
+	iter->trace_buffer = &global_trace.trace_buffer;
 }
 
-static void
-__ftrace_dump(bool disable_tracing, enum ftrace_dump_mode oops_dump_mode)
+void ftrace_dump(enum ftrace_dump_mode oops_dump_mode)
 {
-	static arch_spinlock_t ftrace_dump_lock =
-		(arch_spinlock_t)__ARCH_SPIN_LOCK_UNLOCKED;
 	/* use static because iter can be a bit big for the stack */
 	static struct trace_iterator iter;
+	static atomic_t dump_running;
 	unsigned int old_userobj;
-	static int dump_ran;
 	unsigned long flags;
 	int cnt = 0, cpu;
 
-	/* only one dump */
-	local_irq_save(flags);
-	arch_spin_lock(&ftrace_dump_lock);
-	if (dump_ran)
-		goto out;
-
-	dump_ran = 1;
+	/* Only allow one dump user at a time. */
+	if (atomic_inc_return(&dump_running) != 1) {
+		atomic_dec(&dump_running);
+		return;
+	}
 
+	/*
+	 * Always turn off tracing when we dump.
+	 * We don't need to show trace output of what happens
+	 * between multiple crashes.
+	 *
+	 * If the user does a sysrq-z, then they can re-enable
+	 * tracing with echo 1 > tracing_on.
+	 */
 	tracing_off();
 
-	/* Did function tracer already get disabled? */
-	if (ftrace_is_dead()) {
-		printk("# WARNING: FUNCTION TRACING IS CORRUPTED\n");
-		printk("#          MAY BE MISSING FUNCTION EVENTS\n");
-	}
-
-	if (disable_tracing)
-		ftrace_kill();
+	local_irq_save(flags);
 
 	/* Simulate the iterator */
 	trace_init_global_iter(&iter);
 
 	for_each_tracing_cpu(cpu) {
-		atomic_inc(&iter.tr->data[cpu]->disabled);
+		atomic_inc(&per_cpu_ptr(iter.tr->trace_buffer.data, cpu)->disabled);
 	}
 
 	old_userobj = trace_flags & TRACE_ITER_SYM_USEROBJ;
@@ -5208,7 +6107,7 @@ __ftrace_dump(bool disable_tracing, enum ftrace_dump_mode oops_dump_mode)
 
 	switch (oops_dump_mode) {
 	case DUMP_ALL:
-		iter.cpu_file = TRACE_PIPE_ALL_CPU;
+		iter.cpu_file = RING_BUFFER_ALL_CPUS;
 		break;
 	case DUMP_ORIG:
 		iter.cpu_file = raw_smp_processor_id();
@@ -5217,11 +6116,17 @@ __ftrace_dump(bool disable_tracing, enum ftrace_dump_mode oops_dump_mode)
 		goto out_enable;
 	default:
 		printk(KERN_TRACE "Bad dumping mode, switching to all CPUs dump\n");
-		iter.cpu_file = TRACE_PIPE_ALL_CPU;
+		iter.cpu_file = RING_BUFFER_ALL_CPUS;
 	}
 
 	printk(KERN_TRACE "Dumping ftrace buffer:\n");
 
+	/* Did function tracer already get disabled? */
+	if (ftrace_is_dead()) {
+		printk("# WARNING: FUNCTION TRACING IS CORRUPTED\n");
+		printk("#          MAY BE MISSING FUNCTION EVENTS\n");
+	}
+
 	/*
 	 * We need to stop all tracing on all CPUS to read the
 	 * the next buffer. This is a bit expensive, but is
@@ -5261,33 +6166,19 @@ __ftrace_dump(bool disable_tracing, enum ftrace_dump_mode oops_dump_mode)
 		printk(KERN_TRACE "---------------------------------\n");
 
  out_enable:
-	/* Re-enable tracing if requested */
-	if (!disable_tracing) {
-		trace_flags |= old_userobj;
+	trace_flags |= old_userobj;
 
-		for_each_tracing_cpu(cpu) {
-			atomic_dec(&iter.tr->data[cpu]->disabled);
-		}
-		tracing_on();
+	for_each_tracing_cpu(cpu) {
+		atomic_dec(&per_cpu_ptr(iter.trace_buffer->data, cpu)->disabled);
 	}
-
- out:
-	arch_spin_unlock(&ftrace_dump_lock);
+ 	atomic_dec(&dump_running);
 	local_irq_restore(flags);
 }
-
-/* By default: disable tracing after the dump */
-void ftrace_dump(enum ftrace_dump_mode oops_dump_mode)
-{
-	__ftrace_dump(true, oops_dump_mode);
-}
 EXPORT_SYMBOL_GPL(ftrace_dump);
 
 __init static int tracer_alloc_buffers(void)
 {
 	int ring_buf_size;
-	enum ring_buffer_flags rb_flags;
-	int i;
 	int ret = -ENOMEM;
 
 
@@ -5308,49 +6199,27 @@ __init static int tracer_alloc_buffers(void)
 	else
 		ring_buf_size = 1;
 
-	rb_flags = trace_flags & TRACE_ITER_OVERWRITE ? RB_FL_OVERWRITE : 0;
-
 	cpumask_copy(tracing_buffer_mask, cpu_possible_mask);
 	cpumask_copy(tracing_cpumask, cpu_all_mask);
 
+	raw_spin_lock_init(&global_trace.start_lock);
+
 	/* TODO: make the number of buffers hot pluggable with CPUS */
-	global_trace.buffer = ring_buffer_alloc(ring_buf_size, rb_flags);
-	if (!global_trace.buffer) {
+	if (allocate_trace_buffers(&global_trace, ring_buf_size) < 0) {
 		printk(KERN_ERR "tracer: failed to allocate ring buffer!\n");
 		WARN_ON(1);
 		goto out_free_cpumask;
 	}
+
 	if (global_trace.buffer_disabled)
 		tracing_off();
 
-
-#ifdef CONFIG_TRACER_MAX_TRACE
-	max_tr.buffer = ring_buffer_alloc(1, rb_flags);
-	if (!max_tr.buffer) {
-		printk(KERN_ERR "tracer: failed to allocate max ring buffer!\n");
-		WARN_ON(1);
-		ring_buffer_free(global_trace.buffer);
-		goto out_free_cpumask;
-	}
-#endif
-
-	/* Allocate the first page for all buffers */
-	for_each_tracing_cpu(i) {
-		global_trace.data[i] = &per_cpu(global_trace_cpu, i);
-		max_tr.data[i] = &per_cpu(max_tr_data, i);
-	}
-
-	set_buffer_entries(&global_trace,
-			   ring_buffer_size(global_trace.buffer, 0));
-#ifdef CONFIG_TRACER_MAX_TRACE
-	set_buffer_entries(&max_tr, 1);
-#endif
-
 	trace_init_cmdlines();
-	init_irq_work(&trace_work_wakeup, trace_wake_up);
 
 	register_tracer(&nop_trace);
 
+	global_trace.current_trace = &nop_trace;
+
 	/* All seems OK, enable tracing */
 	tracing_disabled = 0;
 
@@ -5359,16 +6228,32 @@ __init static int tracer_alloc_buffers(void)
 
 	register_die_notifier(&trace_die_notifier);
 
+	global_trace.flags = TRACE_ARRAY_FL_GLOBAL;
+
+	/* Holder for file callbacks */
+	global_trace.trace_cpu.cpu = RING_BUFFER_ALL_CPUS;
+	global_trace.trace_cpu.tr = &global_trace;
+
+	INIT_LIST_HEAD(&global_trace.systems);
+	INIT_LIST_HEAD(&global_trace.events);
+	list_add(&global_trace.list, &ftrace_trace_arrays);
+
 	while (trace_boot_options) {
 		char *option;
 
 		option = strsep(&trace_boot_options, ",");
-		trace_set_options(option);
+		trace_set_options(&global_trace, option);
 	}
 
+	register_snapshot_cmd();
+
 	return 0;
 
 out_free_cpumask:
+	free_percpu(global_trace.trace_buffer.data);
+#ifdef CONFIG_TRACER_MAX_TRACE
+	free_percpu(global_trace.max_buffer.data);
+#endif
 	free_cpumask_var(tracing_cpumask);
 out_free_buffer_mask:
 	free_cpumask_var(tracing_buffer_mask);
diff --git a/kernel/trace/trace.h b/kernel/trace/trace.h
index 2081971367e..711ca7d3e7f 100644
--- a/kernel/trace/trace.h
+++ b/kernel/trace/trace.h
@@ -13,6 +13,11 @@
 #include <linux/trace_seq.h>
 #include <linux/ftrace_event.h>
 
+#ifdef CONFIG_FTRACE_SYSCALLS
+#include <asm/unistd.h>		/* For NR_SYSCALLS	     */
+#include <asm/syscall.h>	/* some archs define it here */
+#endif
+
 enum trace_type {
 	__TRACE_FIRST_TYPE = 0,
 
@@ -29,6 +34,7 @@ enum trace_type {
 	TRACE_GRAPH_ENT,
 	TRACE_USER_STACK,
 	TRACE_BLK,
+	TRACE_BPUTS,
 
 	__TRACE_LAST_TYPE,
 };
@@ -103,11 +109,6 @@ 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:
@@ -127,12 +128,21 @@ enum trace_flag_type {
 
 #define TRACE_BUF_SIZE		1024
 
+struct trace_array;
+
+struct trace_cpu {
+	struct trace_array	*tr;
+	struct dentry		*dir;
+	int			cpu;
+};
+
 /*
  * The CPU trace array - it consists of thousands of trace entries
  * plus some other descriptor data: (for example which task started
  * the trace, etc.)
  */
 struct trace_array_cpu {
+	struct trace_cpu	trace_cpu;
 	atomic_t		disabled;
 	void			*buffer_page;	/* ring buffer spare */
 
@@ -151,20 +161,83 @@ struct trace_array_cpu {
 	char			comm[TASK_COMM_LEN];
 };
 
+struct tracer;
+
+struct trace_buffer {
+	struct trace_array		*tr;
+	struct ring_buffer		*buffer;
+	struct trace_array_cpu __percpu	*data;
+	cycle_t				time_start;
+	int				cpu;
+};
+
 /*
  * The trace array - an array of per-CPU trace arrays. This is the
  * highest level data structure that individual tracers deal with.
  * They have on/off state as well:
  */
 struct trace_array {
-	struct ring_buffer	*buffer;
-	int			cpu;
+	struct list_head	list;
+	char			*name;
+	struct trace_buffer	trace_buffer;
+#ifdef CONFIG_TRACER_MAX_TRACE
+	/*
+	 * The max_buffer is used to snapshot the trace when a maximum
+	 * latency is reached, or when the user initiates a snapshot.
+	 * Some tracers will use this to store a maximum trace while
+	 * it continues examining live traces.
+	 *
+	 * The buffers for the max_buffer are set up the same as the trace_buffer
+	 * When a snapshot is taken, the buffer of the max_buffer is swapped
+	 * with the buffer of the trace_buffer and the buffers are reset for
+	 * the trace_buffer so the tracing can continue.
+	 */
+	struct trace_buffer	max_buffer;
+	bool			allocated_snapshot;
+#endif
 	int			buffer_disabled;
-	cycle_t			time_start;
+	struct trace_cpu	trace_cpu;	/* place holder */
+#ifdef CONFIG_FTRACE_SYSCALLS
+	int			sys_refcount_enter;
+	int			sys_refcount_exit;
+	DECLARE_BITMAP(enabled_enter_syscalls, NR_syscalls);
+	DECLARE_BITMAP(enabled_exit_syscalls, NR_syscalls);
+#endif
+	int			stop_count;
+	int			clock_id;
+	struct tracer		*current_trace;
+	unsigned int		flags;
+	raw_spinlock_t		start_lock;
+	struct dentry		*dir;
+	struct dentry		*options;
+	struct dentry		*percpu_dir;
+	struct dentry		*event_dir;
+	struct list_head	systems;
+	struct list_head	events;
 	struct task_struct	*waiter;
-	struct trace_array_cpu	*data[NR_CPUS];
+	int			ref;
+};
+
+enum {
+	TRACE_ARRAY_FL_GLOBAL	= (1 << 0)
 };
 
+extern struct list_head ftrace_trace_arrays;
+
+/*
+ * The global tracer (top) should be the first trace array added,
+ * but we check the flag anyway.
+ */
+static inline struct trace_array *top_trace_array(void)
+{
+	struct trace_array *tr;
+
+	tr = list_entry(ftrace_trace_arrays.prev,
+			typeof(*tr), list);
+	WARN_ON(!(tr->flags & TRACE_ARRAY_FL_GLOBAL));
+	return tr;
+}
+
 #define FTRACE_CMP_TYPE(var, type) \
 	__builtin_types_compatible_p(typeof(var), type *)
 
@@ -200,6 +273,7 @@ extern void __ftrace_bad_type(void);
 		IF_ASSIGN(var, ent, struct userstack_entry, TRACE_USER_STACK);\
 		IF_ASSIGN(var, ent, struct print_entry, TRACE_PRINT);	\
 		IF_ASSIGN(var, ent, struct bprint_entry, TRACE_BPRINT);	\
+		IF_ASSIGN(var, ent, struct bputs_entry, TRACE_BPUTS);	\
 		IF_ASSIGN(var, ent, struct trace_mmiotrace_rw,		\
 			  TRACE_MMIO_RW);				\
 		IF_ASSIGN(var, ent, struct trace_mmiotrace_map,		\
@@ -289,9 +363,10 @@ struct tracer {
 	struct tracer		*next;
 	struct tracer_flags	*flags;
 	bool			print_max;
-	bool			use_max_tr;
-	bool			allocated_snapshot;
 	bool			enabled;
+#ifdef CONFIG_TRACER_MAX_TRACE
+	bool			use_max_tr;
+#endif
 };
 
 
@@ -427,8 +502,6 @@ static __always_inline void trace_clear_recursion(int bit)
 	current->trace_recursion = val;
 }
 
-#define TRACE_PIPE_ALL_CPU	-1
-
 static inline struct ring_buffer_iter *
 trace_buffer_iter(struct trace_iterator *iter, int cpu)
 {
@@ -439,10 +512,10 @@ trace_buffer_iter(struct trace_iterator *iter, int cpu)
 
 int tracer_init(struct tracer *t, struct trace_array *tr);
 int tracing_is_enabled(void);
-void tracing_reset(struct trace_array *tr, int cpu);
-void tracing_reset_online_cpus(struct trace_array *tr);
+void tracing_reset(struct trace_buffer *buf, int cpu);
+void tracing_reset_online_cpus(struct trace_buffer *buf);
 void tracing_reset_current(int cpu);
-void tracing_reset_current_online_cpus(void);
+void tracing_reset_all_online_cpus(void);
 int tracing_open_generic(struct inode *inode, struct file *filp);
 struct dentry *trace_create_file(const char *name,
 				 umode_t mode,
@@ -450,6 +523,7 @@ struct dentry *trace_create_file(const char *name,
 				 void *data,
 				 const struct file_operations *fops);
 
+struct dentry *tracing_init_dentry_tr(struct trace_array *tr);
 struct dentry *tracing_init_dentry(void);
 
 struct ring_buffer_event;
@@ -583,7 +657,7 @@ extern int DYN_FTRACE_TEST_NAME(void);
 #define DYN_FTRACE_TEST_NAME2 trace_selftest_dynamic_test_func2
 extern int DYN_FTRACE_TEST_NAME2(void);
 
-extern int ring_buffer_expanded;
+extern bool ring_buffer_expanded;
 extern bool tracing_selftest_disabled;
 DECLARE_PER_CPU(int, ftrace_cpu_disabled);
 
@@ -619,6 +693,8 @@ trace_array_vprintk(struct trace_array *tr,
 		    unsigned long ip, const char *fmt, va_list args);
 int trace_array_printk(struct trace_array *tr,
 		       unsigned long ip, const char *fmt, ...);
+int trace_array_printk_buf(struct ring_buffer *buffer,
+			   unsigned long ip, const char *fmt, ...);
 void trace_printk_seq(struct trace_seq *s);
 enum print_line_t print_trace_line(struct trace_iterator *iter);
 
@@ -786,6 +862,7 @@ enum trace_iterator_flags {
 	TRACE_ITER_STOP_ON_FREE		= 0x400000,
 	TRACE_ITER_IRQ_INFO		= 0x800000,
 	TRACE_ITER_MARKERS		= 0x1000000,
+	TRACE_ITER_FUNCTION		= 0x2000000,
 };
 
 /*
@@ -832,8 +909,8 @@ enum {
 
 struct ftrace_event_field {
 	struct list_head	link;
-	char			*name;
-	char			*type;
+	const char		*name;
+	const char		*type;
 	int			filter_type;
 	int			offset;
 	int			size;
@@ -851,12 +928,19 @@ struct event_filter {
 struct event_subsystem {
 	struct list_head	list;
 	const char		*name;
-	struct dentry		*entry;
 	struct event_filter	*filter;
-	int			nr_events;
 	int			ref_count;
 };
 
+struct ftrace_subsystem_dir {
+	struct list_head		list;
+	struct event_subsystem		*subsystem;
+	struct trace_array		*tr;
+	struct dentry			*entry;
+	int				ref_count;
+	int				nr_events;
+};
+
 #define FILTER_PRED_INVALID	((unsigned short)-1)
 #define FILTER_PRED_IS_RIGHT	(1 << 15)
 #define FILTER_PRED_FOLD	(1 << 15)
@@ -906,22 +990,20 @@ struct filter_pred {
 	unsigned short		right;
 };
 
-extern struct list_head ftrace_common_fields;
-
 extern enum regex_type
 filter_parse_regex(char *buff, int len, char **search, int *not);
 extern void print_event_filter(struct ftrace_event_call *call,
 			       struct trace_seq *s);
 extern int apply_event_filter(struct ftrace_event_call *call,
 			      char *filter_string);
-extern int apply_subsystem_event_filter(struct event_subsystem *system,
+extern int apply_subsystem_event_filter(struct ftrace_subsystem_dir *dir,
 					char *filter_string);
 extern void print_subsystem_event_filter(struct event_subsystem *system,
 					 struct trace_seq *s);
 extern int filter_assign_type(const char *type);
 
-struct list_head *
-trace_get_fields(struct ftrace_event_call *event_call);
+struct ftrace_event_field *
+trace_find_event_field(struct ftrace_event_call *call, char *name);
 
 static inline int
 filter_check_discard(struct ftrace_event_call *call, void *rec,
@@ -938,6 +1020,8 @@ filter_check_discard(struct ftrace_event_call *call, void *rec,
 }
 
 extern void trace_event_enable_cmd_record(bool enable);
+extern int event_trace_add_tracer(struct dentry *parent, struct trace_array *tr);
+extern int event_trace_del_tracer(struct trace_array *tr);
 
 extern struct mutex event_mutex;
 extern struct list_head ftrace_events;
@@ -948,7 +1032,18 @@ extern const char *__stop___trace_bprintk_fmt[];
 void trace_printk_init_buffers(void);
 void trace_printk_start_comm(void);
 int trace_keep_overwrite(struct tracer *tracer, u32 mask, int set);
-int set_tracer_flag(unsigned int mask, int enabled);
+int set_tracer_flag(struct trace_array *tr, unsigned int mask, int enabled);
+
+/*
+ * Normal trace_printk() and friends allocates special buffers
+ * to do the manipulation, as well as saves the print formats
+ * into sections to display. But the trace infrastructure wants
+ * to use these without the added overhead at the price of being
+ * a bit slower (used mainly for warnings, where we don't care
+ * about performance). The internal_trace_puts() is for such
+ * a purpose.
+ */
+#define internal_trace_puts(str) __trace_puts(_THIS_IP_, str, strlen(str))
 
 #undef FTRACE_ENTRY
 #define FTRACE_ENTRY(call, struct_name, id, tstruct, print, filter)	\
diff --git a/kernel/trace/trace_branch.c b/kernel/trace/trace_branch.c
index 95e96842ed2..d594da0dc03 100644
--- a/kernel/trace/trace_branch.c
+++ b/kernel/trace/trace_branch.c
@@ -32,6 +32,7 @@ probe_likely_condition(struct ftrace_branch_data *f, int val, int expect)
 {
 	struct ftrace_event_call *call = &event_branch;
 	struct trace_array *tr = branch_tracer;
+	struct trace_array_cpu *data;
 	struct ring_buffer_event *event;
 	struct trace_branch *entry;
 	struct ring_buffer *buffer;
@@ -51,11 +52,12 @@ probe_likely_condition(struct ftrace_branch_data *f, int val, int expect)
 
 	local_irq_save(flags);
 	cpu = raw_smp_processor_id();
-	if (atomic_inc_return(&tr->data[cpu]->disabled) != 1)
+	data = per_cpu_ptr(tr->trace_buffer.data, cpu);
+	if (atomic_inc_return(&data->disabled) != 1)
 		goto out;
 
 	pc = preempt_count();
-	buffer = tr->buffer;
+	buffer = tr->trace_buffer.buffer;
 	event = trace_buffer_lock_reserve(buffer, TRACE_BRANCH,
 					  sizeof(*entry), flags, pc);
 	if (!event)
@@ -80,7 +82,7 @@ probe_likely_condition(struct ftrace_branch_data *f, int val, int expect)
 		__buffer_unlock_commit(buffer, event);
 
  out:
-	atomic_dec(&tr->data[cpu]->disabled);
+	atomic_dec(&data->disabled);
 	local_irq_restore(flags);
 }
 
diff --git a/kernel/trace/trace_clock.c b/kernel/trace/trace_clock.c
index aa8f5f48dae..26dc348332b 100644
--- a/kernel/trace/trace_clock.c
+++ b/kernel/trace/trace_clock.c
@@ -57,6 +57,16 @@ u64 notrace trace_clock(void)
 	return local_clock();
 }
 
+/*
+ * trace_jiffy_clock(): Simply use jiffies as a clock counter.
+ */
+u64 notrace trace_clock_jiffies(void)
+{
+	u64 jiffy = jiffies - INITIAL_JIFFIES;
+
+	/* Return nsecs */
+	return (u64)jiffies_to_usecs(jiffy) * 1000ULL;
+}
 
 /*
  * trace_clock_global(): special globally coherent trace clock
diff --git a/kernel/trace/trace_entries.h b/kernel/trace/trace_entries.h
index 4108e1250ca..e2d027ac66a 100644
--- a/kernel/trace/trace_entries.h
+++ b/kernel/trace/trace_entries.h
@@ -223,8 +223,8 @@ FTRACE_ENTRY(bprint, bprint_entry,
 		__dynamic_array(	u32,	buf	)
 	),
 
-	F_printk("%08lx fmt:%p",
-		 __entry->ip, __entry->fmt),
+	F_printk("%pf: %s",
+		 (void *)__entry->ip, __entry->fmt),
 
 	FILTER_OTHER
 );
@@ -238,8 +238,23 @@ FTRACE_ENTRY(print, print_entry,
 		__dynamic_array(	char,	buf	)
 	),
 
-	F_printk("%08lx %s",
-		 __entry->ip, __entry->buf),
+	F_printk("%pf: %s",
+		 (void *)__entry->ip, __entry->buf),
+
+	FILTER_OTHER
+);
+
+FTRACE_ENTRY(bputs, bputs_entry,
+
+	TRACE_BPUTS,
+
+	F_STRUCT(
+		__field(	unsigned long,	ip	)
+		__field(	const char *,	str	)
+	),
+
+	F_printk("%pf: %s",
+		 (void *)__entry->ip, __entry->str),
 
 	FILTER_OTHER
 );
diff --git a/kernel/trace/trace_events.c b/kernel/trace/trace_events.c
index 57e9b284250..53582e982e5 100644
--- a/kernel/trace/trace_events.c
+++ b/kernel/trace/trace_events.c
@@ -34,9 +34,27 @@ char event_storage[EVENT_STORAGE_SIZE];
 EXPORT_SYMBOL_GPL(event_storage);
 
 LIST_HEAD(ftrace_events);
-LIST_HEAD(ftrace_common_fields);
+static LIST_HEAD(ftrace_common_fields);
 
-struct list_head *
+#define GFP_TRACE (GFP_KERNEL | __GFP_ZERO)
+
+static struct kmem_cache *field_cachep;
+static struct kmem_cache *file_cachep;
+
+/* Double loops, do not use break, only goto's work */
+#define do_for_each_event_file(tr, file)			\
+	list_for_each_entry(tr, &ftrace_trace_arrays, list) {	\
+		list_for_each_entry(file, &tr->events, list)
+
+#define do_for_each_event_file_safe(tr, file)			\
+	list_for_each_entry(tr, &ftrace_trace_arrays, list) {	\
+		struct ftrace_event_file *___n;				\
+		list_for_each_entry_safe(file, ___n, &tr->events, list)
+
+#define while_for_each_event_file()		\
+	}
+
+static struct list_head *
 trace_get_fields(struct ftrace_event_call *event_call)
 {
 	if (!event_call->class->get_fields)
@@ -44,23 +62,45 @@ trace_get_fields(struct ftrace_event_call *event_call)
 	return event_call->class->get_fields(event_call);
 }
 
+static struct ftrace_event_field *
+__find_event_field(struct list_head *head, char *name)
+{
+	struct ftrace_event_field *field;
+
+	list_for_each_entry(field, head, link) {
+		if (!strcmp(field->name, name))
+			return field;
+	}
+
+	return NULL;
+}
+
+struct ftrace_event_field *
+trace_find_event_field(struct ftrace_event_call *call, char *name)
+{
+	struct ftrace_event_field *field;
+	struct list_head *head;
+
+	field = __find_event_field(&ftrace_common_fields, name);
+	if (field)
+		return field;
+
+	head = trace_get_fields(call);
+	return __find_event_field(head, name);
+}
+
 static int __trace_define_field(struct list_head *head, const char *type,
 				const char *name, int offset, int size,
 				int is_signed, int filter_type)
 {
 	struct ftrace_event_field *field;
 
-	field = kzalloc(sizeof(*field), GFP_KERNEL);
+	field = kmem_cache_alloc(field_cachep, GFP_TRACE);
 	if (!field)
 		goto err;
 
-	field->name = kstrdup(name, GFP_KERNEL);
-	if (!field->name)
-		goto err;
-
-	field->type = kstrdup(type, GFP_KERNEL);
-	if (!field->type)
-		goto err;
+	field->name = name;
+	field->type = type;
 
 	if (filter_type == FILTER_OTHER)
 		field->filter_type = filter_assign_type(type);
@@ -76,9 +116,7 @@ static int __trace_define_field(struct list_head *head, const char *type,
 	return 0;
 
 err:
-	if (field)
-		kfree(field->name);
-	kfree(field);
+	kmem_cache_free(field_cachep, field);
 
 	return -ENOMEM;
 }
@@ -120,7 +158,7 @@ static int trace_define_common_fields(void)
 	return ret;
 }
 
-void trace_destroy_fields(struct ftrace_event_call *call)
+static void trace_destroy_fields(struct ftrace_event_call *call)
 {
 	struct ftrace_event_field *field, *next;
 	struct list_head *head;
@@ -128,9 +166,7 @@ void trace_destroy_fields(struct ftrace_event_call *call)
 	head = trace_get_fields(call);
 	list_for_each_entry_safe(field, next, head, link) {
 		list_del(&field->link);
-		kfree(field->type);
-		kfree(field->name);
-		kfree(field);
+		kmem_cache_free(field_cachep, field);
 	}
 }
 
@@ -149,15 +185,17 @@ EXPORT_SYMBOL_GPL(trace_event_raw_init);
 int ftrace_event_reg(struct ftrace_event_call *call,
 		     enum trace_reg type, void *data)
 {
+	struct ftrace_event_file *file = data;
+
 	switch (type) {
 	case TRACE_REG_REGISTER:
 		return tracepoint_probe_register(call->name,
 						 call->class->probe,
-						 call);
+						 file);
 	case TRACE_REG_UNREGISTER:
 		tracepoint_probe_unregister(call->name,
 					    call->class->probe,
-					    call);
+					    file);
 		return 0;
 
 #ifdef CONFIG_PERF_EVENTS
@@ -183,54 +221,100 @@ EXPORT_SYMBOL_GPL(ftrace_event_reg);
 
 void trace_event_enable_cmd_record(bool enable)
 {
-	struct ftrace_event_call *call;
+	struct ftrace_event_file *file;
+	struct trace_array *tr;
 
 	mutex_lock(&event_mutex);
-	list_for_each_entry(call, &ftrace_events, list) {
-		if (!(call->flags & TRACE_EVENT_FL_ENABLED))
+	do_for_each_event_file(tr, file) {
+
+		if (!(file->flags & FTRACE_EVENT_FL_ENABLED))
 			continue;
 
 		if (enable) {
 			tracing_start_cmdline_record();
-			call->flags |= TRACE_EVENT_FL_RECORDED_CMD;
+			set_bit(FTRACE_EVENT_FL_RECORDED_CMD_BIT, &file->flags);
 		} else {
 			tracing_stop_cmdline_record();
-			call->flags &= ~TRACE_EVENT_FL_RECORDED_CMD;
+			clear_bit(FTRACE_EVENT_FL_RECORDED_CMD_BIT, &file->flags);
 		}
-	}
+	} while_for_each_event_file();
 	mutex_unlock(&event_mutex);
 }
 
-static int ftrace_event_enable_disable(struct ftrace_event_call *call,
-					int enable)
+static int __ftrace_event_enable_disable(struct ftrace_event_file *file,
+					 int enable, int soft_disable)
 {
+	struct ftrace_event_call *call = file->event_call;
 	int ret = 0;
+	int disable;
 
 	switch (enable) {
 	case 0:
-		if (call->flags & TRACE_EVENT_FL_ENABLED) {
-			call->flags &= ~TRACE_EVENT_FL_ENABLED;
-			if (call->flags & TRACE_EVENT_FL_RECORDED_CMD) {
+		/*
+		 * When soft_disable is set and enable is cleared, we want
+		 * to clear the SOFT_DISABLED flag but leave the event in the
+		 * state that it was. That is, if the event was enabled and
+		 * SOFT_DISABLED isn't set, then do nothing. But if SOFT_DISABLED
+		 * is set we do not want the event to be enabled before we
+		 * clear the bit.
+		 *
+		 * When soft_disable is not set but the SOFT_MODE flag is,
+		 * we do nothing. Do not disable the tracepoint, otherwise
+		 * "soft enable"s (clearing the SOFT_DISABLED bit) wont work.
+		 */
+		if (soft_disable) {
+			disable = file->flags & FTRACE_EVENT_FL_SOFT_DISABLED;
+			clear_bit(FTRACE_EVENT_FL_SOFT_MODE_BIT, &file->flags);
+		} else
+			disable = !(file->flags & FTRACE_EVENT_FL_SOFT_MODE);
+
+		if (disable && (file->flags & FTRACE_EVENT_FL_ENABLED)) {
+			clear_bit(FTRACE_EVENT_FL_ENABLED_BIT, &file->flags);
+			if (file->flags & FTRACE_EVENT_FL_RECORDED_CMD) {
 				tracing_stop_cmdline_record();
-				call->flags &= ~TRACE_EVENT_FL_RECORDED_CMD;
+				clear_bit(FTRACE_EVENT_FL_RECORDED_CMD_BIT, &file->flags);
 			}
-			call->class->reg(call, TRACE_REG_UNREGISTER, NULL);
+			call->class->reg(call, TRACE_REG_UNREGISTER, file);
 		}
+		/* If in SOFT_MODE, just set the SOFT_DISABLE_BIT */
+		if (file->flags & FTRACE_EVENT_FL_SOFT_MODE)
+			set_bit(FTRACE_EVENT_FL_SOFT_DISABLED_BIT, &file->flags);
 		break;
 	case 1:
-		if (!(call->flags & TRACE_EVENT_FL_ENABLED)) {
+		/*
+		 * When soft_disable is set and enable is set, we want to
+		 * register the tracepoint for the event, but leave the event
+		 * as is. That means, if the event was already enabled, we do
+		 * nothing (but set SOFT_MODE). If the event is disabled, we
+		 * set SOFT_DISABLED before enabling the event tracepoint, so
+		 * it still seems to be disabled.
+		 */
+		if (!soft_disable)
+			clear_bit(FTRACE_EVENT_FL_SOFT_DISABLED_BIT, &file->flags);
+		else
+			set_bit(FTRACE_EVENT_FL_SOFT_MODE_BIT, &file->flags);
+
+		if (!(file->flags & FTRACE_EVENT_FL_ENABLED)) {
+
+			/* Keep the event disabled, when going to SOFT_MODE. */
+			if (soft_disable)
+				set_bit(FTRACE_EVENT_FL_SOFT_DISABLED_BIT, &file->flags);
+
 			if (trace_flags & TRACE_ITER_RECORD_CMD) {
 				tracing_start_cmdline_record();
-				call->flags |= TRACE_EVENT_FL_RECORDED_CMD;
+				set_bit(FTRACE_EVENT_FL_RECORDED_CMD_BIT, &file->flags);
 			}
-			ret = call->class->reg(call, TRACE_REG_REGISTER, NULL);
+			ret = call->class->reg(call, TRACE_REG_REGISTER, file);
 			if (ret) {
 				tracing_stop_cmdline_record();
 				pr_info("event trace: Could not enable event "
 					"%s\n", call->name);
 				break;
 			}
-			call->flags |= TRACE_EVENT_FL_ENABLED;
+			set_bit(FTRACE_EVENT_FL_ENABLED_BIT, &file->flags);
+
+			/* WAS_ENABLED gets set but never cleared. */
+			call->flags |= TRACE_EVENT_FL_WAS_ENABLED;
 		}
 		break;
 	}
@@ -238,13 +322,19 @@ static int ftrace_event_enable_disable(struct ftrace_event_call *call,
 	return ret;
 }
 
-static void ftrace_clear_events(void)
+static int ftrace_event_enable_disable(struct ftrace_event_file *file,
+				       int enable)
 {
-	struct ftrace_event_call *call;
+	return __ftrace_event_enable_disable(file, enable, 0);
+}
+
+static void ftrace_clear_events(struct trace_array *tr)
+{
+	struct ftrace_event_file *file;
 
 	mutex_lock(&event_mutex);
-	list_for_each_entry(call, &ftrace_events, list) {
-		ftrace_event_enable_disable(call, 0);
+	list_for_each_entry(file, &tr->events, list) {
+		ftrace_event_enable_disable(file, 0);
 	}
 	mutex_unlock(&event_mutex);
 }
@@ -257,11 +347,12 @@ static void __put_system(struct event_subsystem *system)
 	if (--system->ref_count)
 		return;
 
+	list_del(&system->list);
+
 	if (filter) {
 		kfree(filter->filter_string);
 		kfree(filter);
 	}
-	kfree(system->name);
 	kfree(system);
 }
 
@@ -271,24 +362,45 @@ static void __get_system(struct event_subsystem *system)
 	system->ref_count++;
 }
 
-static void put_system(struct event_subsystem *system)
+static void __get_system_dir(struct ftrace_subsystem_dir *dir)
+{
+	WARN_ON_ONCE(dir->ref_count == 0);
+	dir->ref_count++;
+	__get_system(dir->subsystem);
+}
+
+static void __put_system_dir(struct ftrace_subsystem_dir *dir)
+{
+	WARN_ON_ONCE(dir->ref_count == 0);
+	/* If the subsystem is about to be freed, the dir must be too */
+	WARN_ON_ONCE(dir->subsystem->ref_count == 1 && dir->ref_count != 1);
+
+	__put_system(dir->subsystem);
+	if (!--dir->ref_count)
+		kfree(dir);
+}
+
+static void put_system(struct ftrace_subsystem_dir *dir)
 {
 	mutex_lock(&event_mutex);
-	__put_system(system);
+	__put_system_dir(dir);
 	mutex_unlock(&event_mutex);
 }
 
 /*
  * __ftrace_set_clr_event(NULL, NULL, NULL, set) will set/unset all events.
  */
-static int __ftrace_set_clr_event(const char *match, const char *sub,
-				  const char *event, int set)
+static int __ftrace_set_clr_event(struct trace_array *tr, const char *match,
+				  const char *sub, const char *event, int set)
 {
+	struct ftrace_event_file *file;
 	struct ftrace_event_call *call;
 	int ret = -EINVAL;
 
 	mutex_lock(&event_mutex);
-	list_for_each_entry(call, &ftrace_events, list) {
+	list_for_each_entry(file, &tr->events, list) {
+
+		call = file->event_call;
 
 		if (!call->name || !call->class || !call->class->reg)
 			continue;
@@ -307,7 +419,7 @@ static int __ftrace_set_clr_event(const char *match, const char *sub,
 		if (event && strcmp(event, call->name) != 0)
 			continue;
 
-		ftrace_event_enable_disable(call, set);
+		ftrace_event_enable_disable(file, set);
 
 		ret = 0;
 	}
@@ -316,7 +428,7 @@ static int __ftrace_set_clr_event(const char *match, const char *sub,
 	return ret;
 }
 
-static int ftrace_set_clr_event(char *buf, int set)
+static int ftrace_set_clr_event(struct trace_array *tr, char *buf, int set)
 {
 	char *event = NULL, *sub = NULL, *match;
 
@@ -344,7 +456,7 @@ static int ftrace_set_clr_event(char *buf, int set)
 			event = NULL;
 	}
 
-	return __ftrace_set_clr_event(match, sub, event, set);
+	return __ftrace_set_clr_event(tr, match, sub, event, set);
 }
 
 /**
@@ -361,7 +473,9 @@ static int ftrace_set_clr_event(char *buf, int set)
  */
 int trace_set_clr_event(const char *system, const char *event, int set)
 {
-	return __ftrace_set_clr_event(NULL, system, event, set);
+	struct trace_array *tr = top_trace_array();
+
+	return __ftrace_set_clr_event(tr, NULL, system, event, set);
 }
 EXPORT_SYMBOL_GPL(trace_set_clr_event);
 
@@ -373,6 +487,8 @@ ftrace_event_write(struct file *file, const char __user *ubuf,
 		   size_t cnt, loff_t *ppos)
 {
 	struct trace_parser parser;
+	struct seq_file *m = file->private_data;
+	struct trace_array *tr = m->private;
 	ssize_t read, ret;
 
 	if (!cnt)
@@ -395,7 +511,7 @@ ftrace_event_write(struct file *file, const char __user *ubuf,
 
 		parser.buffer[parser.idx] = 0;
 
-		ret = ftrace_set_clr_event(parser.buffer + !set, set);
+		ret = ftrace_set_clr_event(tr, parser.buffer + !set, set);
 		if (ret)
 			goto out_put;
 	}
@@ -411,17 +527,20 @@ ftrace_event_write(struct file *file, const char __user *ubuf,
 static void *
 t_next(struct seq_file *m, void *v, loff_t *pos)
 {
-	struct ftrace_event_call *call = v;
+	struct ftrace_event_file *file = v;
+	struct ftrace_event_call *call;
+	struct trace_array *tr = m->private;
 
 	(*pos)++;
 
-	list_for_each_entry_continue(call, &ftrace_events, list) {
+	list_for_each_entry_continue(file, &tr->events, list) {
+		call = file->event_call;
 		/*
 		 * The ftrace subsystem is for showing formats only.
 		 * They can not be enabled or disabled via the event files.
 		 */
 		if (call->class && call->class->reg)
-			return call;
+			return file;
 	}
 
 	return NULL;
@@ -429,30 +548,32 @@ t_next(struct seq_file *m, void *v, loff_t *pos)
 
 static void *t_start(struct seq_file *m, loff_t *pos)
 {
-	struct ftrace_event_call *call;
+	struct ftrace_event_file *file;
+	struct trace_array *tr = m->private;
 	loff_t l;
 
 	mutex_lock(&event_mutex);
 
-	call = list_entry(&ftrace_events, struct ftrace_event_call, list);
+	file = list_entry(&tr->events, struct ftrace_event_file, list);
 	for (l = 0; l <= *pos; ) {
-		call = t_next(m, call, &l);
-		if (!call)
+		file = t_next(m, file, &l);
+		if (!file)
 			break;
 	}
-	return call;
+	return file;
 }
 
 static void *
 s_next(struct seq_file *m, void *v, loff_t *pos)
 {
-	struct ftrace_event_call *call = v;
+	struct ftrace_event_file *file = v;
+	struct trace_array *tr = m->private;
 
 	(*pos)++;
 
-	list_for_each_entry_continue(call, &ftrace_events, list) {
-		if (call->flags & TRACE_EVENT_FL_ENABLED)
-			return call;
+	list_for_each_entry_continue(file, &tr->events, list) {
+		if (file->flags & FTRACE_EVENT_FL_ENABLED)
+			return file;
 	}
 
 	return NULL;
@@ -460,23 +581,25 @@ s_next(struct seq_file *m, void *v, loff_t *pos)
 
 static void *s_start(struct seq_file *m, loff_t *pos)
 {
-	struct ftrace_event_call *call;
+	struct ftrace_event_file *file;
+	struct trace_array *tr = m->private;
 	loff_t l;
 
 	mutex_lock(&event_mutex);
 
-	call = list_entry(&ftrace_events, struct ftrace_event_call, list);
+	file = list_entry(&tr->events, struct ftrace_event_file, list);
 	for (l = 0; l <= *pos; ) {
-		call = s_next(m, call, &l);
-		if (!call)
+		file = s_next(m, file, &l);
+		if (!file)
 			break;
 	}
-	return call;
+	return file;
 }
 
 static int t_show(struct seq_file *m, void *v)
 {
-	struct ftrace_event_call *call = v;
+	struct ftrace_event_file *file = v;
+	struct ftrace_event_call *call = file->event_call;
 
 	if (strcmp(call->class->system, TRACE_SYSTEM) != 0)
 		seq_printf(m, "%s:", call->class->system);
@@ -494,25 +617,31 @@ static ssize_t
 event_enable_read(struct file *filp, char __user *ubuf, size_t cnt,
 		  loff_t *ppos)
 {
-	struct ftrace_event_call *call = filp->private_data;
+	struct ftrace_event_file *file = filp->private_data;
 	char *buf;
 
-	if (call->flags & TRACE_EVENT_FL_ENABLED)
-		buf = "1\n";
-	else
+	if (file->flags & FTRACE_EVENT_FL_ENABLED) {
+		if (file->flags & FTRACE_EVENT_FL_SOFT_DISABLED)
+			buf = "0*\n";
+		else
+			buf = "1\n";
+	} else
 		buf = "0\n";
 
-	return simple_read_from_buffer(ubuf, cnt, ppos, buf, 2);
+	return simple_read_from_buffer(ubuf, cnt, ppos, buf, strlen(buf));
 }
 
 static ssize_t
 event_enable_write(struct file *filp, const char __user *ubuf, size_t cnt,
 		   loff_t *ppos)
 {
-	struct ftrace_event_call *call = filp->private_data;
+	struct ftrace_event_file *file = filp->private_data;
 	unsigned long val;
 	int ret;
 
+	if (!file)
+		return -EINVAL;
+
 	ret = kstrtoul_from_user(ubuf, cnt, 10, &val);
 	if (ret)
 		return ret;
@@ -525,7 +654,7 @@ event_enable_write(struct file *filp, const char __user *ubuf, size_t cnt,
 	case 0:
 	case 1:
 		mutex_lock(&event_mutex);
-		ret = ftrace_event_enable_disable(call, val);
+		ret = ftrace_event_enable_disable(file, val);
 		mutex_unlock(&event_mutex);
 		break;
 
@@ -543,14 +672,18 @@ system_enable_read(struct file *filp, char __user *ubuf, size_t cnt,
 		   loff_t *ppos)
 {
 	const char set_to_char[4] = { '?', '0', '1', 'X' };
-	struct event_subsystem *system = filp->private_data;
+	struct ftrace_subsystem_dir *dir = filp->private_data;
+	struct event_subsystem *system = dir->subsystem;
 	struct ftrace_event_call *call;
+	struct ftrace_event_file *file;
+	struct trace_array *tr = dir->tr;
 	char buf[2];
 	int set = 0;
 	int ret;
 
 	mutex_lock(&event_mutex);
-	list_for_each_entry(call, &ftrace_events, list) {
+	list_for_each_entry(file, &tr->events, list) {
+		call = file->event_call;
 		if (!call->name || !call->class || !call->class->reg)
 			continue;
 
@@ -562,7 +695,7 @@ system_enable_read(struct file *filp, char __user *ubuf, size_t cnt,
 		 * or if all events or cleared, or if we have
 		 * a mixture.
 		 */
-		set |= (1 << !!(call->flags & TRACE_EVENT_FL_ENABLED));
+		set |= (1 << !!(file->flags & FTRACE_EVENT_FL_ENABLED));
 
 		/*
 		 * If we have a mixture, no need to look further.
@@ -584,7 +717,8 @@ static ssize_t
 system_enable_write(struct file *filp, const char __user *ubuf, size_t cnt,
 		    loff_t *ppos)
 {
-	struct event_subsystem *system = filp->private_data;
+	struct ftrace_subsystem_dir *dir = filp->private_data;
+	struct event_subsystem *system = dir->subsystem;
 	const char *name = NULL;
 	unsigned long val;
 	ssize_t ret;
@@ -607,7 +741,7 @@ system_enable_write(struct file *filp, const char __user *ubuf, size_t cnt,
 	if (system)
 		name = system->name;
 
-	ret = __ftrace_set_clr_event(NULL, name, NULL, val);
+	ret = __ftrace_set_clr_event(dir->tr, NULL, name, NULL, val);
 	if (ret)
 		goto out;
 
@@ -845,43 +979,75 @@ static LIST_HEAD(event_subsystems);
 static int subsystem_open(struct inode *inode, struct file *filp)
 {
 	struct event_subsystem *system = NULL;
+	struct ftrace_subsystem_dir *dir = NULL; /* Initialize for gcc */
+	struct trace_array *tr;
 	int ret;
 
-	if (!inode->i_private)
-		goto skip_search;
-
 	/* Make sure the system still exists */
 	mutex_lock(&event_mutex);
-	list_for_each_entry(system, &event_subsystems, list) {
-		if (system == inode->i_private) {
-			/* Don't open systems with no events */
-			if (!system->nr_events) {
-				system = NULL;
-				break;
+	list_for_each_entry(tr, &ftrace_trace_arrays, list) {
+		list_for_each_entry(dir, &tr->systems, list) {
+			if (dir == inode->i_private) {
+				/* Don't open systems with no events */
+				if (dir->nr_events) {
+					__get_system_dir(dir);
+					system = dir->subsystem;
+				}
+				goto exit_loop;
 			}
-			__get_system(system);
-			break;
 		}
 	}
+ exit_loop:
 	mutex_unlock(&event_mutex);
 
-	if (system != inode->i_private)
+	if (!system)
 		return -ENODEV;
 
- skip_search:
+	/* Some versions of gcc think dir can be uninitialized here */
+	WARN_ON(!dir);
+
 	ret = tracing_open_generic(inode, filp);
-	if (ret < 0 && system)
-		put_system(system);
+	if (ret < 0)
+		put_system(dir);
+
+	return ret;
+}
+
+static int system_tr_open(struct inode *inode, struct file *filp)
+{
+	struct ftrace_subsystem_dir *dir;
+	struct trace_array *tr = inode->i_private;
+	int ret;
+
+	/* Make a temporary dir that has no system but points to tr */
+	dir = kzalloc(sizeof(*dir), GFP_KERNEL);
+	if (!dir)
+		return -ENOMEM;
+
+	dir->tr = tr;
+
+	ret = tracing_open_generic(inode, filp);
+	if (ret < 0)
+		kfree(dir);
+
+	filp->private_data = dir;
 
 	return ret;
 }
 
 static int subsystem_release(struct inode *inode, struct file *file)
 {
-	struct event_subsystem *system = inode->i_private;
+	struct ftrace_subsystem_dir *dir = file->private_data;
 
-	if (system)
-		put_system(system);
+	/*
+	 * If dir->subsystem is NULL, then this is a temporary
+	 * descriptor that was made for a trace_array to enable
+	 * all subsystems.
+	 */
+	if (dir->subsystem)
+		put_system(dir);
+	else
+		kfree(dir);
 
 	return 0;
 }
@@ -890,7 +1056,8 @@ static ssize_t
 subsystem_filter_read(struct file *filp, char __user *ubuf, size_t cnt,
 		      loff_t *ppos)
 {
-	struct event_subsystem *system = filp->private_data;
+	struct ftrace_subsystem_dir *dir = filp->private_data;
+	struct event_subsystem *system = dir->subsystem;
 	struct trace_seq *s;
 	int r;
 
@@ -915,7 +1082,7 @@ static ssize_t
 subsystem_filter_write(struct file *filp, const char __user *ubuf, size_t cnt,
 		       loff_t *ppos)
 {
-	struct event_subsystem *system = filp->private_data;
+	struct ftrace_subsystem_dir *dir = filp->private_data;
 	char *buf;
 	int err;
 
@@ -932,7 +1099,7 @@ subsystem_filter_write(struct file *filp, const char __user *ubuf, size_t cnt,
 	}
 	buf[cnt] = '\0';
 
-	err = apply_subsystem_event_filter(system, buf);
+	err = apply_subsystem_event_filter(dir, buf);
 	free_page((unsigned long) buf);
 	if (err < 0)
 		return err;
@@ -1041,30 +1208,35 @@ static const struct file_operations ftrace_system_enable_fops = {
 	.release = subsystem_release,
 };
 
+static const struct file_operations ftrace_tr_enable_fops = {
+	.open = system_tr_open,
+	.read = system_enable_read,
+	.write = system_enable_write,
+	.llseek = default_llseek,
+	.release = subsystem_release,
+};
+
 static const struct file_operations ftrace_show_header_fops = {
 	.open = tracing_open_generic,
 	.read = show_header,
 	.llseek = default_llseek,
 };
 
-static struct dentry *event_trace_events_dir(void)
+static int
+ftrace_event_open(struct inode *inode, struct file *file,
+		  const struct seq_operations *seq_ops)
 {
-	static struct dentry *d_tracer;
-	static struct dentry *d_events;
-
-	if (d_events)
-		return d_events;
-
-	d_tracer = tracing_init_dentry();
-	if (!d_tracer)
-		return NULL;
+	struct seq_file *m;
+	int ret;
 
-	d_events = debugfs_create_dir("events", d_tracer);
-	if (!d_events)
-		pr_warning("Could not create debugfs "
-			   "'events' directory\n");
+	ret = seq_open(file, seq_ops);
+	if (ret < 0)
+		return ret;
+	m = file->private_data;
+	/* copy tr over to seq ops */
+	m->private = inode->i_private;
 
-	return d_events;
+	return ret;
 }
 
 static int
@@ -1072,117 +1244,165 @@ ftrace_event_avail_open(struct inode *inode, struct file *file)
 {
 	const struct seq_operations *seq_ops = &show_event_seq_ops;
 
-	return seq_open(file, seq_ops);
+	return ftrace_event_open(inode, file, seq_ops);
 }
 
 static int
 ftrace_event_set_open(struct inode *inode, struct file *file)
 {
 	const struct seq_operations *seq_ops = &show_set_event_seq_ops;
+	struct trace_array *tr = inode->i_private;
 
 	if ((file->f_mode & FMODE_WRITE) &&
 	    (file->f_flags & O_TRUNC))
-		ftrace_clear_events();
+		ftrace_clear_events(tr);
 
-	return seq_open(file, seq_ops);
+	return ftrace_event_open(inode, file, seq_ops);
+}
+
+static struct event_subsystem *
+create_new_subsystem(const char *name)
+{
+	struct event_subsystem *system;
+
+	/* need to create new entry */
+	system = kmalloc(sizeof(*system), GFP_KERNEL);
+	if (!system)
+		return NULL;
+
+	system->ref_count = 1;
+	system->name = name;
+
+	system->filter = NULL;
+
+	system->filter = kzalloc(sizeof(struct event_filter), GFP_KERNEL);
+	if (!system->filter)
+		goto out_free;
+
+	list_add(&system->list, &event_subsystems);
+
+	return system;
+
+ out_free:
+	kfree(system);
+	return NULL;
 }
 
 static struct dentry *
-event_subsystem_dir(const char *name, struct dentry *d_events)
+event_subsystem_dir(struct trace_array *tr, const char *name,
+		    struct ftrace_event_file *file, struct dentry *parent)
 {
+	struct ftrace_subsystem_dir *dir;
 	struct event_subsystem *system;
 	struct dentry *entry;
 
 	/* First see if we did not already create this dir */
-	list_for_each_entry(system, &event_subsystems, list) {
+	list_for_each_entry(dir, &tr->systems, list) {
+		system = dir->subsystem;
 		if (strcmp(system->name, name) == 0) {
-			system->nr_events++;
-			return system->entry;
+			dir->nr_events++;
+			file->system = dir;
+			return dir->entry;
 		}
 	}
 
-	/* need to create new entry */
-	system = kmalloc(sizeof(*system), GFP_KERNEL);
-	if (!system) {
-		pr_warning("No memory to create event subsystem %s\n",
-			   name);
-		return d_events;
+	/* Now see if the system itself exists. */
+	list_for_each_entry(system, &event_subsystems, list) {
+		if (strcmp(system->name, name) == 0)
+			break;
 	}
+	/* Reset system variable when not found */
+	if (&system->list == &event_subsystems)
+		system = NULL;
 
-	system->entry = debugfs_create_dir(name, d_events);
-	if (!system->entry) {
-		pr_warning("Could not create event subsystem %s\n",
-			   name);
-		kfree(system);
-		return d_events;
-	}
+	dir = kmalloc(sizeof(*dir), GFP_KERNEL);
+	if (!dir)
+		goto out_fail;
 
-	system->nr_events = 1;
-	system->ref_count = 1;
-	system->name = kstrdup(name, GFP_KERNEL);
-	if (!system->name) {
-		debugfs_remove(system->entry);
-		kfree(system);
-		return d_events;
+	if (!system) {
+		system = create_new_subsystem(name);
+		if (!system)
+			goto out_free;
+	} else
+		__get_system(system);
+
+	dir->entry = debugfs_create_dir(name, parent);
+	if (!dir->entry) {
+		pr_warning("Failed to create system directory %s\n", name);
+		__put_system(system);
+		goto out_free;
 	}
 
-	list_add(&system->list, &event_subsystems);
-
-	system->filter = NULL;
-
-	system->filter = kzalloc(sizeof(struct event_filter), GFP_KERNEL);
-	if (!system->filter) {
-		pr_warning("Could not allocate filter for subsystem "
-			   "'%s'\n", name);
-		return system->entry;
-	}
+	dir->tr = tr;
+	dir->ref_count = 1;
+	dir->nr_events = 1;
+	dir->subsystem = system;
+	file->system = dir;
 
-	entry = debugfs_create_file("filter", 0644, system->entry, system,
+	entry = debugfs_create_file("filter", 0644, dir->entry, dir,
 				    &ftrace_subsystem_filter_fops);
 	if (!entry) {
 		kfree(system->filter);
 		system->filter = NULL;
-		pr_warning("Could not create debugfs "
-			   "'%s/filter' entry\n", name);
+		pr_warning("Could not create debugfs '%s/filter' entry\n", name);
 	}
 
-	trace_create_file("enable", 0644, system->entry, system,
+	trace_create_file("enable", 0644, dir->entry, dir,
 			  &ftrace_system_enable_fops);
 
-	return system->entry;
+	list_add(&dir->list, &tr->systems);
+
+	return dir->entry;
+
+ out_free:
+	kfree(dir);
+ out_fail:
+	/* Only print this message if failed on memory allocation */
+	if (!dir || !system)
+		pr_warning("No memory to create event subsystem %s\n",
+			   name);
+	return NULL;
 }
 
 static int
-event_create_dir(struct ftrace_event_call *call, struct dentry *d_events,
+event_create_dir(struct dentry *parent,
+		 struct ftrace_event_file *file,
 		 const struct file_operations *id,
 		 const struct file_operations *enable,
 		 const struct file_operations *filter,
 		 const struct file_operations *format)
 {
+	struct ftrace_event_call *call = file->event_call;
+	struct trace_array *tr = file->tr;
 	struct list_head *head;
+	struct dentry *d_events;
 	int ret;
 
 	/*
 	 * If the trace point header did not define TRACE_SYSTEM
 	 * then the system would be called "TRACE_SYSTEM".
 	 */
-	if (strcmp(call->class->system, TRACE_SYSTEM) != 0)
-		d_events = event_subsystem_dir(call->class->system, d_events);
-
-	call->dir = debugfs_create_dir(call->name, d_events);
-	if (!call->dir) {
-		pr_warning("Could not create debugfs "
-			   "'%s' directory\n", call->name);
+	if (strcmp(call->class->system, TRACE_SYSTEM) != 0) {
+		d_events = event_subsystem_dir(tr, call->class->system, file, parent);
+		if (!d_events)
+			return -ENOMEM;
+	} else
+		d_events = parent;
+
+	file->dir = debugfs_create_dir(call->name, d_events);
+	if (!file->dir) {
+		pr_warning("Could not create debugfs '%s' directory\n",
+			   call->name);
 		return -1;
 	}
 
 	if (call->class->reg && !(call->flags & TRACE_EVENT_FL_IGNORE_ENABLE))
-		trace_create_file("enable", 0644, call->dir, call,
+		trace_create_file("enable", 0644, file->dir, file,
 				  enable);
 
 #ifdef CONFIG_PERF_EVENTS
 	if (call->event.type && call->class->reg)
-		trace_create_file("id", 0444, call->dir, call,
+		trace_create_file("id", 0444, file->dir, call,
 		 		  id);
 #endif
 
@@ -1196,23 +1416,76 @@ event_create_dir(struct ftrace_event_call *call, struct dentry *d_events,
 		if (ret < 0) {
 			pr_warning("Could not initialize trace point"
 				   " events/%s\n", call->name);
-			return ret;
+			return -1;
 		}
 	}
-	trace_create_file("filter", 0644, call->dir, call,
+	trace_create_file("filter", 0644, file->dir, call,
 			  filter);
 
-	trace_create_file("format", 0444, call->dir, call,
+	trace_create_file("format", 0444, file->dir, call,
 			  format);
 
 	return 0;
 }
 
+static void remove_subsystem(struct ftrace_subsystem_dir *dir)
+{
+	if (!dir)
+		return;
+
+	if (!--dir->nr_events) {
+		debugfs_remove_recursive(dir->entry);
+		list_del(&dir->list);
+		__put_system_dir(dir);
+	}
+}
+
+static void remove_event_from_tracers(struct ftrace_event_call *call)
+{
+	struct ftrace_event_file *file;
+	struct trace_array *tr;
+
+	do_for_each_event_file_safe(tr, file) {
+
+		if (file->event_call != call)
+			continue;
+
+		list_del(&file->list);
+		debugfs_remove_recursive(file->dir);
+		remove_subsystem(file->system);
+		kmem_cache_free(file_cachep, file);
+
+		/*
+		 * The do_for_each_event_file_safe() is
+		 * a double loop. After finding the call for this
+		 * trace_array, we use break to jump to the next
+		 * trace_array.
+		 */
+		break;
+	} while_for_each_event_file();
+}
+
 static void event_remove(struct ftrace_event_call *call)
 {
-	ftrace_event_enable_disable(call, 0);
+	struct trace_array *tr;
+	struct ftrace_event_file *file;
+
+	do_for_each_event_file(tr, file) {
+		if (file->event_call != call)
+			continue;
+		ftrace_event_enable_disable(file, 0);
+		/*
+		 * The do_for_each_event_file() is
+		 * a double loop. After finding the call for this
+		 * trace_array, we use break to jump to the next
+		 * trace_array.
+		 */
+		break;
+	} while_for_each_event_file();
+
 	if (call->event.funcs)
 		__unregister_ftrace_event(&call->event);
+	remove_event_from_tracers(call);
 	list_del(&call->list);
 }
 
@@ -1234,82 +1507,99 @@ static int event_init(struct ftrace_event_call *call)
 }
 
 static int
-__trace_add_event_call(struct ftrace_event_call *call, struct module *mod,
-		       const struct file_operations *id,
-		       const struct file_operations *enable,
-		       const struct file_operations *filter,
-		       const struct file_operations *format)
+__register_event(struct ftrace_event_call *call, struct module *mod)
 {
-	struct dentry *d_events;
 	int ret;
 
 	ret = event_init(call);
 	if (ret < 0)
 		return ret;
 
-	d_events = event_trace_events_dir();
-	if (!d_events)
-		return -ENOENT;
-
-	ret = event_create_dir(call, d_events, id, enable, filter, format);
-	if (!ret)
-		list_add(&call->list, &ftrace_events);
+	list_add(&call->list, &ftrace_events);
 	call->mod = mod;
 
-	return ret;
+	return 0;
+}
+
+/* Add an event to a trace directory */
+static int
+__trace_add_new_event(struct ftrace_event_call *call,
+		      struct trace_array *tr,
+		      const struct file_operations *id,
+		      const struct file_operations *enable,
+		      const struct file_operations *filter,
+		      const struct file_operations *format)
+{
+	struct ftrace_event_file *file;
+
+	file = kmem_cache_alloc(file_cachep, GFP_TRACE);
+	if (!file)
+		return -ENOMEM;
+
+	file->event_call = call;
+	file->tr = tr;
+	list_add(&file->list, &tr->events);
+
+	return event_create_dir(tr->event_dir, file, id, enable, filter, format);
 }
 
+/*
+ * Just create a decriptor for early init. A descriptor is required
+ * for enabling events at boot. We want to enable events before
+ * the filesystem is initialized.
+ */
+static __init int
+__trace_early_add_new_event(struct ftrace_event_call *call,
+			    struct trace_array *tr)
+{
+	struct ftrace_event_file *file;
+
+	file = kmem_cache_alloc(file_cachep, GFP_TRACE);
+	if (!file)
+		return -ENOMEM;
+
+	file->event_call = call;
+	file->tr = tr;
+	list_add(&file->list, &tr->events);
+
+	return 0;
+}
+
+struct ftrace_module_file_ops;
+static void __add_event_to_tracers(struct ftrace_event_call *call,
+				   struct ftrace_module_file_ops *file_ops);
+
 /* Add an additional event_call dynamically */
 int trace_add_event_call(struct ftrace_event_call *call)
 {
 	int ret;
 	mutex_lock(&event_mutex);
-	ret = __trace_add_event_call(call, NULL, &ftrace_event_id_fops,
-				     &ftrace_enable_fops,
-				     &ftrace_event_filter_fops,
-				     &ftrace_event_format_fops);
-	mutex_unlock(&event_mutex);
-	return ret;
-}
 
-static void remove_subsystem_dir(const char *name)
-{
-	struct event_subsystem *system;
-
-	if (strcmp(name, TRACE_SYSTEM) == 0)
-		return;
+	ret = __register_event(call, NULL);
+	if (ret >= 0)
+		__add_event_to_tracers(call, NULL);
 
-	list_for_each_entry(system, &event_subsystems, list) {
-		if (strcmp(system->name, name) == 0) {
-			if (!--system->nr_events) {
-				debugfs_remove_recursive(system->entry);
-				list_del(&system->list);
-				__put_system(system);
-			}
-			break;
-		}
-	}
+	mutex_unlock(&event_mutex);
+	return ret;
 }
 
 /*
- * Must be called under locking both of event_mutex and trace_event_mutex.
+ * Must be called under locking both of event_mutex and trace_event_sem.
  */
 static void __trace_remove_event_call(struct ftrace_event_call *call)
 {
 	event_remove(call);
 	trace_destroy_fields(call);
 	destroy_preds(call);
-	debugfs_remove_recursive(call->dir);
-	remove_subsystem_dir(call->class->system);
 }
 
 /* Remove an event_call */
 void trace_remove_event_call(struct ftrace_event_call *call)
 {
 	mutex_lock(&event_mutex);
-	down_write(&trace_event_mutex);
+	down_write(&trace_event_sem);
 	__trace_remove_event_call(call);
-	up_write(&trace_event_mutex);
+	up_write(&trace_event_sem);
 	mutex_unlock(&event_mutex);
 }
 
@@ -1336,6 +1626,26 @@ struct ftrace_module_file_ops {
 };
 
 static struct ftrace_module_file_ops *
+find_ftrace_file_ops(struct ftrace_module_file_ops *file_ops, struct module *mod)
+{
+	/*
+	 * As event_calls are added in groups by module,
+	 * when we find one file_ops, we don't need to search for
+	 * each call in that module, as the rest should be the
+	 * same. Only search for a new one if the last one did
+	 * not match.
+	 */
+	if (file_ops && mod == file_ops->mod)
+		return file_ops;
+
+	list_for_each_entry(file_ops, &ftrace_module_file_list, list) {
+		if (file_ops->mod == mod)
+			return file_ops;
+	}
+	return NULL;
+}
+
+static struct ftrace_module_file_ops *
 trace_create_file_ops(struct module *mod)
 {
 	struct ftrace_module_file_ops *file_ops;
@@ -1386,9 +1696,8 @@ static void trace_module_add_events(struct module *mod)
 		return;
 
 	for_each_event(call, start, end) {
-		__trace_add_event_call(*call, mod,
-				       &file_ops->id, &file_ops->enable,
-				       &file_ops->filter, &file_ops->format);
+		__register_event(*call, mod);
+		__add_event_to_tracers(*call, file_ops);
 	}
 }
 
@@ -1396,12 +1705,13 @@ static void trace_module_remove_events(struct module *mod)
 {
 	struct ftrace_module_file_ops *file_ops;
 	struct ftrace_event_call *call, *p;
-	bool found = false;
+	bool clear_trace = false;
 
-	down_write(&trace_event_mutex);
+	down_write(&trace_event_sem);
 	list_for_each_entry_safe(call, p, &ftrace_events, list) {
 		if (call->mod == mod) {
-			found = true;
+			if (call->flags & TRACE_EVENT_FL_WAS_ENABLED)
+				clear_trace = true;
 			__trace_remove_event_call(call);
 		}
 	}
@@ -1415,14 +1725,18 @@ static void trace_module_remove_events(struct module *mod)
 		list_del(&file_ops->list);
 		kfree(file_ops);
 	}
+	up_write(&trace_event_sem);
 
 	/*
 	 * It is safest to reset the ring buffer if the module being unloaded
-	 * registered any events.
+	 * registered any events that were used. The only worry is if
+	 * a new module gets loaded, and takes on the same id as the events
+	 * of this module. When printing out the buffer, traced events left
+	 * over from this module may be passed to the new module events and
+	 * unexpected results may occur.
 	 */
-	if (found)
-		tracing_reset_current_online_cpus();
-	up_write(&trace_event_mutex);
+	if (clear_trace)
+		tracing_reset_all_online_cpus();
 }
 
 static int trace_module_notify(struct notifier_block *self,
@@ -1443,14 +1757,433 @@ static int trace_module_notify(struct notifier_block *self,
 
 	return 0;
 }
+
+static int
+__trace_add_new_mod_event(struct ftrace_event_call *call,
+			  struct trace_array *tr,
+			  struct ftrace_module_file_ops *file_ops)
+{
+	return __trace_add_new_event(call, tr,
+				     &file_ops->id, &file_ops->enable,
+				     &file_ops->filter, &file_ops->format);
+}
+
 #else
-static int trace_module_notify(struct notifier_block *self,
-			       unsigned long val, void *data)
+static inline struct ftrace_module_file_ops *
+find_ftrace_file_ops(struct ftrace_module_file_ops *file_ops, struct module *mod)
+{
+	return NULL;
+}
+static inline int trace_module_notify(struct notifier_block *self,
+				      unsigned long val, void *data)
 {
 	return 0;
 }
+static inline int
+__trace_add_new_mod_event(struct ftrace_event_call *call,
+			  struct trace_array *tr,
+			  struct ftrace_module_file_ops *file_ops)
+{
+	return -ENODEV;
+}
 #endif /* CONFIG_MODULES */
 
+/* Create a new event directory structure for a trace directory. */
+static void
+__trace_add_event_dirs(struct trace_array *tr)
+{
+	struct ftrace_module_file_ops *file_ops = NULL;
+	struct ftrace_event_call *call;
+	int ret;
+
+	list_for_each_entry(call, &ftrace_events, list) {
+		if (call->mod) {
+			/*
+			 * Directories for events by modules need to
+			 * keep module ref counts when opened (as we don't
+			 * want the module to disappear when reading one
+			 * of these files). The file_ops keep account of
+			 * the module ref count.
+			 */
+			file_ops = find_ftrace_file_ops(file_ops, call->mod);
+			if (!file_ops)
+				continue; /* Warn? */
+			ret = __trace_add_new_mod_event(call, tr, file_ops);
+			if (ret < 0)
+				pr_warning("Could not create directory for event %s\n",
+					   call->name);
+			continue;
+		}
+		ret = __trace_add_new_event(call, tr,
+					    &ftrace_event_id_fops,
+					    &ftrace_enable_fops,
+					    &ftrace_event_filter_fops,
+					    &ftrace_event_format_fops);
+		if (ret < 0)
+			pr_warning("Could not create directory for event %s\n",
+				   call->name);
+	}
+}
+
+#ifdef CONFIG_DYNAMIC_FTRACE
+
+/* Avoid typos */
+#define ENABLE_EVENT_STR	"enable_event"
+#define DISABLE_EVENT_STR	"disable_event"
+
+struct event_probe_data {
+	struct ftrace_event_file	*file;
+	unsigned long			count;
+	int				ref;
+	bool				enable;
+};
+
+static struct ftrace_event_file *
+find_event_file(struct trace_array *tr, const char *system,  const char *event)
+{
+	struct ftrace_event_file *file;
+	struct ftrace_event_call *call;
+
+	list_for_each_entry(file, &tr->events, list) {
+
+		call = file->event_call;
+
+		if (!call->name || !call->class || !call->class->reg)
+			continue;
+
+		if (call->flags & TRACE_EVENT_FL_IGNORE_ENABLE)
+			continue;
+
+		if (strcmp(event, call->name) == 0 &&
+		    strcmp(system, call->class->system) == 0)
+			return file;
+	}
+	return NULL;
+}
+
+static void
+event_enable_probe(unsigned long ip, unsigned long parent_ip, void **_data)
+{
+	struct event_probe_data **pdata = (struct event_probe_data **)_data;
+	struct event_probe_data *data = *pdata;
+
+	if (!data)
+		return;
+
+	if (data->enable)
+		clear_bit(FTRACE_EVENT_FL_SOFT_DISABLED_BIT, &data->file->flags);
+	else
+		set_bit(FTRACE_EVENT_FL_SOFT_DISABLED_BIT, &data->file->flags);
+}
+
+static void
+event_enable_count_probe(unsigned long ip, unsigned long parent_ip, void **_data)
+{
+	struct event_probe_data **pdata = (struct event_probe_data **)_data;
+	struct event_probe_data *data = *pdata;
+
+	if (!data)
+		return;
+
+	if (!data->count)
+		return;
+
+	/* Skip if the event is in a state we want to switch to */
+	if (data->enable == !(data->file->flags & FTRACE_EVENT_FL_SOFT_DISABLED))
+		return;
+
+	if (data->count != -1)
+		(data->count)--;
+
+	event_enable_probe(ip, parent_ip, _data);
+}
+
+static int
+event_enable_print(struct seq_file *m, unsigned long ip,
+		      struct ftrace_probe_ops *ops, void *_data)
+{
+	struct event_probe_data *data = _data;
+
+	seq_printf(m, "%ps:", (void *)ip);
+
+	seq_printf(m, "%s:%s:%s",
+		   data->enable ? ENABLE_EVENT_STR : DISABLE_EVENT_STR,
+		   data->file->event_call->class->system,
+		   data->file->event_call->name);
+
+	if (data->count == -1)
+		seq_printf(m, ":unlimited\n");
+	else
+		seq_printf(m, ":count=%ld\n", data->count);
+
+	return 0;
+}
+
+static int
+event_enable_init(struct ftrace_probe_ops *ops, unsigned long ip,
+		  void **_data)
+{
+	struct event_probe_data **pdata = (struct event_probe_data **)_data;
+	struct event_probe_data *data = *pdata;
+
+	data->ref++;
+	return 0;
+}
+
+static void
+event_enable_free(struct ftrace_probe_ops *ops, unsigned long ip,
+		  void **_data)
+{
+	struct event_probe_data **pdata = (struct event_probe_data **)_data;
+	struct event_probe_data *data = *pdata;
+
+	if (WARN_ON_ONCE(data->ref <= 0))
+		return;
+
+	data->ref--;
+	if (!data->ref) {
+		/* Remove the SOFT_MODE flag */
+		__ftrace_event_enable_disable(data->file, 0, 1);
+		module_put(data->file->event_call->mod);
+		kfree(data);
+	}
+	*pdata = NULL;
+}
+
+static struct ftrace_probe_ops event_enable_probe_ops = {
+	.func			= event_enable_probe,
+	.print			= event_enable_print,
+	.init			= event_enable_init,
+	.free			= event_enable_free,
+};
+
+static struct ftrace_probe_ops event_enable_count_probe_ops = {
+	.func			= event_enable_count_probe,
+	.print			= event_enable_print,
+	.init			= event_enable_init,
+	.free			= event_enable_free,
+};
+
+static struct ftrace_probe_ops event_disable_probe_ops = {
+	.func			= event_enable_probe,
+	.print			= event_enable_print,
+	.init			= event_enable_init,
+	.free			= event_enable_free,
+};
+
+static struct ftrace_probe_ops event_disable_count_probe_ops = {
+	.func			= event_enable_count_probe,
+	.print			= event_enable_print,
+	.init			= event_enable_init,
+	.free			= event_enable_free,
+};
+
+static int
+event_enable_func(struct ftrace_hash *hash,
+		  char *glob, char *cmd, char *param, int enabled)
+{
+	struct trace_array *tr = top_trace_array();
+	struct ftrace_event_file *file;
+	struct ftrace_probe_ops *ops;
+	struct event_probe_data *data;
+	const char *system;
+	const char *event;
+	char *number;
+	bool enable;
+	int ret;
+
+	/* hash funcs only work with set_ftrace_filter */
+	if (!enabled)
+		return -EINVAL;
+
+	if (!param)
+		return -EINVAL;
+
+	system = strsep(&param, ":");
+	if (!param)
+		return -EINVAL;
+
+	event = strsep(&param, ":");
+
+	mutex_lock(&event_mutex);
+
+	ret = -EINVAL;
+	file = find_event_file(tr, system, event);
+	if (!file)
+		goto out;
+
+	enable = strcmp(cmd, ENABLE_EVENT_STR) == 0;
+
+	if (enable)
+		ops = param ? &event_enable_count_probe_ops : &event_enable_probe_ops;
+	else
+		ops = param ? &event_disable_count_probe_ops : &event_disable_probe_ops;
+
+	if (glob[0] == '!') {
+		unregister_ftrace_function_probe_func(glob+1, ops);
+		ret = 0;
+		goto out;
+	}
+
+	ret = -ENOMEM;
+	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (!data)
+		goto out;
+
+	data->enable = enable;
+	data->count = -1;
+	data->file = file;
+
+	if (!param)
+		goto out_reg;
+
+	number = strsep(&param, ":");
+
+	ret = -EINVAL;
+	if (!strlen(number))
+		goto out_free;
+
+	/*
+	 * We use the callback data field (which is a pointer)
+	 * as our counter.
+	 */
+	ret = kstrtoul(number, 0, &data->count);
+	if (ret)
+		goto out_free;
+
+ out_reg:
+	/* Don't let event modules unload while probe registered */
+	ret = try_module_get(file->event_call->mod);
+	if (!ret)
+		goto out_free;
+
+	ret = __ftrace_event_enable_disable(file, 1, 1);
+	if (ret < 0)
+		goto out_put;
+	ret = register_ftrace_function_probe(glob, ops, data);
+	if (!ret)
+		goto out_disable;
+ out:
+	mutex_unlock(&event_mutex);
+	return ret;
+
+ out_disable:
+	__ftrace_event_enable_disable(file, 0, 1);
+ out_put:
+	module_put(file->event_call->mod);
+ out_free:
+	kfree(data);
+	goto out;
+}
+
+static struct ftrace_func_command event_enable_cmd = {
+	.name			= ENABLE_EVENT_STR,
+	.func			= event_enable_func,
+};
+
+static struct ftrace_func_command event_disable_cmd = {
+	.name			= DISABLE_EVENT_STR,
+	.func			= event_enable_func,
+};
+
+static __init int register_event_cmds(void)
+{
+	int ret;
+
+	ret = register_ftrace_command(&event_enable_cmd);
+	if (WARN_ON(ret < 0))
+		return ret;
+	ret = register_ftrace_command(&event_disable_cmd);
+	if (WARN_ON(ret < 0))
+		unregister_ftrace_command(&event_enable_cmd);
+	return ret;
+}
+#else
+static inline int register_event_cmds(void) { return 0; }
+#endif /* CONFIG_DYNAMIC_FTRACE */
+
+/*
+ * The top level array has already had its ftrace_event_file
+ * descriptors created in order to allow for early events to
+ * be recorded. This function is called after the debugfs has been
+ * initialized, and we now have to create the files associated
+ * to the events.
+ */
+static __init void
+__trace_early_add_event_dirs(struct trace_array *tr)
+{
+	struct ftrace_event_file *file;
+	int ret;
+
+
+	list_for_each_entry(file, &tr->events, list) {
+		ret = event_create_dir(tr->event_dir, file,
+				       &ftrace_event_id_fops,
+				       &ftrace_enable_fops,
+				       &ftrace_event_filter_fops,
+				       &ftrace_event_format_fops);
+		if (ret < 0)
+			pr_warning("Could not create directory for event %s\n",
+				   file->event_call->name);
+	}
+}
+
+/*
+ * For early boot up, the top trace array requires to have
+ * a list of events that can be enabled. This must be done before
+ * the filesystem is set up in order to allow events to be traced
+ * early.
+ */
+static __init void
+__trace_early_add_events(struct trace_array *tr)
+{
+	struct ftrace_event_call *call;
+	int ret;
+
+	list_for_each_entry(call, &ftrace_events, list) {
+		/* Early boot up should not have any modules loaded */
+		if (WARN_ON_ONCE(call->mod))
+			continue;
+
+		ret = __trace_early_add_new_event(call, tr);
+		if (ret < 0)
+			pr_warning("Could not create early event %s\n",
+				   call->name);
+	}
+}
+
+/* Remove the event directory structure for a trace directory. */
+static void
+__trace_remove_event_dirs(struct trace_array *tr)
+{
+	struct ftrace_event_file *file, *next;
+
+	list_for_each_entry_safe(file, next, &tr->events, list) {
+		list_del(&file->list);
+		debugfs_remove_recursive(file->dir);
+		remove_subsystem(file->system);
+		kmem_cache_free(file_cachep, file);
+	}
+}
+
+static void
+__add_event_to_tracers(struct ftrace_event_call *call,
+		       struct ftrace_module_file_ops *file_ops)
+{
+	struct trace_array *tr;
+
+	list_for_each_entry(tr, &ftrace_trace_arrays, list) {
+		if (file_ops)
+			__trace_add_new_mod_event(call, tr, file_ops);
+		else
+			__trace_add_new_event(call, tr,
+					      &ftrace_event_id_fops,
+					      &ftrace_enable_fops,
+					      &ftrace_event_filter_fops,
+					      &ftrace_event_format_fops);
+	}
+}
+
 static struct notifier_block trace_module_nb = {
 	.notifier_call = trace_module_notify,
 	.priority = 0,
@@ -1464,15 +2197,135 @@ static char bootup_event_buf[COMMAND_LINE_SIZE] __initdata;
 static __init int setup_trace_event(char *str)
 {
 	strlcpy(bootup_event_buf, str, COMMAND_LINE_SIZE);
-	ring_buffer_expanded = 1;
-	tracing_selftest_disabled = 1;
+	ring_buffer_expanded = true;
+	tracing_selftest_disabled = true;
 
 	return 1;
 }
 __setup("trace_event=", setup_trace_event);
 
+/* Expects to have event_mutex held when called */
+static int
+create_event_toplevel_files(struct dentry *parent, struct trace_array *tr)
+{
+	struct dentry *d_events;
+	struct dentry *entry;
+
+	entry = debugfs_create_file("set_event", 0644, parent,
+				    tr, &ftrace_set_event_fops);
+	if (!entry) {
+		pr_warning("Could not create debugfs 'set_event' entry\n");
+		return -ENOMEM;
+	}
+
+	d_events = debugfs_create_dir("events", parent);
+	if (!d_events) {
+		pr_warning("Could not create debugfs 'events' directory\n");
+		return -ENOMEM;
+	}
+
+	/* ring buffer internal formats */
+	trace_create_file("header_page", 0444, d_events,
+			  ring_buffer_print_page_header,
+			  &ftrace_show_header_fops);
+
+	trace_create_file("header_event", 0444, d_events,
+			  ring_buffer_print_entry_header,
+			  &ftrace_show_header_fops);
+
+	trace_create_file("enable", 0644, d_events,
+			  tr, &ftrace_tr_enable_fops);
+
+	tr->event_dir = d_events;
+
+	return 0;
+}
+
+/**
+ * event_trace_add_tracer - add a instance of a trace_array to events
+ * @parent: The parent dentry to place the files/directories for events in
+ * @tr: The trace array associated with these events
+ *
+ * When a new instance is created, it needs to set up its events
+ * directory, as well as other files associated with events. It also
+ * creates the event hierachry in the @parent/events directory.
+ *
+ * Returns 0 on success.
+ */
+int event_trace_add_tracer(struct dentry *parent, struct trace_array *tr)
+{
+	int ret;
+
+	mutex_lock(&event_mutex);
+
+	ret = create_event_toplevel_files(parent, tr);
+	if (ret)
+		goto out_unlock;
+
+	down_write(&trace_event_sem);
+	__trace_add_event_dirs(tr);
+	up_write(&trace_event_sem);
+
+ out_unlock:
+	mutex_unlock(&event_mutex);
+
+	return ret;
+}
+
+/*
+ * The top trace array already had its file descriptors created.
+ * Now the files themselves need to be created.
+ */
+static __init int
+early_event_add_tracer(struct dentry *parent, struct trace_array *tr)
+{
+	int ret;
+
+	mutex_lock(&event_mutex);
+
+	ret = create_event_toplevel_files(parent, tr);
+	if (ret)
+		goto out_unlock;
+
+	down_write(&trace_event_sem);
+	__trace_early_add_event_dirs(tr);
+	up_write(&trace_event_sem);
+
+ out_unlock:
+	mutex_unlock(&event_mutex);
+
+	return ret;
+}
+
+int event_trace_del_tracer(struct trace_array *tr)
+{
+	/* Disable any running events */
+	__ftrace_set_clr_event(tr, NULL, NULL, NULL, 0);
+
+	mutex_lock(&event_mutex);
+
+	down_write(&trace_event_sem);
+	__trace_remove_event_dirs(tr);
+	debugfs_remove_recursive(tr->event_dir);
+	up_write(&trace_event_sem);
+
+	tr->event_dir = NULL;
+
+	mutex_unlock(&event_mutex);
+
+	return 0;
+}
+
+static __init int event_trace_memsetup(void)
+{
+	field_cachep = KMEM_CACHE(ftrace_event_field, SLAB_PANIC);
+	file_cachep = KMEM_CACHE(ftrace_event_file, SLAB_PANIC);
+	return 0;
+}
+
 static __init int event_trace_enable(void)
 {
+	struct trace_array *tr = top_trace_array();
 	struct ftrace_event_call **iter, *call;
 	char *buf = bootup_event_buf;
 	char *token;
@@ -1486,6 +2339,14 @@ static __init int event_trace_enable(void)
 			list_add(&call->list, &ftrace_events);
 	}
 
+	/*
+	 * We need the top trace array to have a working set of trace
+	 * points at early init, before the debug files and directories
+	 * are created. Create the file entries now, and attach them
+	 * to the actual file dentries later.
+	 */
+	__trace_early_add_events(tr);
+
 	while (true) {
 		token = strsep(&buf, ",");
 
@@ -1494,73 +2355,43 @@ static __init int event_trace_enable(void)
 		if (!*token)
 			continue;
 
-		ret = ftrace_set_clr_event(token, 1);
+		ret = ftrace_set_clr_event(tr, token, 1);
 		if (ret)
 			pr_warn("Failed to enable trace event: %s\n", token);
 	}
 
 	trace_printk_start_comm();
 
+	register_event_cmds();
+
 	return 0;
 }
 
 static __init int event_trace_init(void)
 {
-	struct ftrace_event_call *call;
+	struct trace_array *tr;
 	struct dentry *d_tracer;
 	struct dentry *entry;
-	struct dentry *d_events;
 	int ret;
 
+	tr = top_trace_array();
+
 	d_tracer = tracing_init_dentry();
 	if (!d_tracer)
 		return 0;
 
 	entry = debugfs_create_file("available_events", 0444, d_tracer,
-				    NULL, &ftrace_avail_fops);
+				    tr, &ftrace_avail_fops);
 	if (!entry)
 		pr_warning("Could not create debugfs "
 			   "'available_events' entry\n");
 
-	entry = debugfs_create_file("set_event", 0644, d_tracer,
-				    NULL, &ftrace_set_event_fops);
-	if (!entry)
-		pr_warning("Could not create debugfs "
-			   "'set_event' entry\n");
-
-	d_events = event_trace_events_dir();
-	if (!d_events)
-		return 0;
-
-	/* ring buffer internal formats */
-	trace_create_file("header_page", 0444, d_events,
-			  ring_buffer_print_page_header,
-			  &ftrace_show_header_fops);
-
-	trace_create_file("header_event", 0444, d_events,
-			  ring_buffer_print_entry_header,
-			  &ftrace_show_header_fops);
-
-	trace_create_file("enable", 0644, d_events,
-			  NULL, &ftrace_system_enable_fops);
-
 	if (trace_define_common_fields())
 		pr_warning("tracing: Failed to allocate common fields");
 
-	/*
-	 * Early initialization already enabled ftrace event.
-	 * Now it's only necessary to create the event directory.
-	 */
-	list_for_each_entry(call, &ftrace_events, list) {
-
-		ret = event_create_dir(call, d_events,
-				       &ftrace_event_id_fops,
-				       &ftrace_enable_fops,
-				       &ftrace_event_filter_fops,
-				       &ftrace_event_format_fops);
-		if (ret < 0)
-			event_remove(call);
-	}
+	ret = early_event_add_tracer(d_tracer, tr);
+	if (ret)
+		return ret;
 
 	ret = register_module_notifier(&trace_module_nb);
 	if (ret)
@@ -1568,6 +2399,7 @@ static __init int event_trace_init(void)
 
 	return 0;
 }
+early_initcall(event_trace_memsetup);
 core_initcall(event_trace_enable);
 fs_initcall(event_trace_init);
 
@@ -1627,13 +2459,20 @@ static __init void event_test_stuff(void)
  */
 static __init void event_trace_self_tests(void)
 {
+	struct ftrace_subsystem_dir *dir;
+	struct ftrace_event_file *file;
 	struct ftrace_event_call *call;
 	struct event_subsystem *system;
+	struct trace_array *tr;
 	int ret;
 
+	tr = top_trace_array();
+
 	pr_info("Running tests on trace events:\n");
 
-	list_for_each_entry(call, &ftrace_events, list) {
+	list_for_each_entry(file, &tr->events, list) {
+
+		call = file->event_call;
 
 		/* Only test those that have a probe */
 		if (!call->class || !call->class->probe)
@@ -1657,15 +2496,15 @@ static __init void event_trace_self_tests(void)
 		 * If an event is already enabled, someone is using
 		 * it and the self test should not be on.
 		 */
-		if (call->flags & TRACE_EVENT_FL_ENABLED) {
+		if (file->flags & FTRACE_EVENT_FL_ENABLED) {
 			pr_warning("Enabled event during self test!\n");
 			WARN_ON_ONCE(1);
 			continue;
 		}
 
-		ftrace_event_enable_disable(call, 1);
+		ftrace_event_enable_disable(file, 1);
 		event_test_stuff();
-		ftrace_event_enable_disable(call, 0);
+		ftrace_event_enable_disable(file, 0);
 
 		pr_cont("OK\n");
 	}
@@ -1674,7 +2513,9 @@ static __init void event_trace_self_tests(void)
 
 	pr_info("Running tests on trace event systems:\n");
 
-	list_for_each_entry(system, &event_subsystems, list) {
+	list_for_each_entry(dir, &tr->systems, list) {
+
+		system = dir->subsystem;
 
 		/* the ftrace system is special, skip it */
 		if (strcmp(system->name, "ftrace") == 0)
@@ -1682,7 +2523,7 @@ static __init void event_trace_self_tests(void)
 
 		pr_info("Testing event system %s: ", system->name);
 
-		ret = __ftrace_set_clr_event(NULL, system->name, NULL, 1);
+		ret = __ftrace_set_clr_event(tr, NULL, system->name, NULL, 1);
 		if (WARN_ON_ONCE(ret)) {
 			pr_warning("error enabling system %s\n",
 				   system->name);
@@ -1691,7 +2532,7 @@ static __init void event_trace_self_tests(void)
 
 		event_test_stuff();
 
-		ret = __ftrace_set_clr_event(NULL, system->name, NULL, 0);
+		ret = __ftrace_set_clr_event(tr, NULL, system->name, NULL, 0);
 		if (WARN_ON_ONCE(ret)) {
 			pr_warning("error disabling system %s\n",
 				   system->name);
@@ -1706,7 +2547,7 @@ static __init void event_trace_self_tests(void)
 	pr_info("Running tests on all trace events:\n");
 	pr_info("Testing all events: ");
 
-	ret = __ftrace_set_clr_event(NULL, NULL, NULL, 1);
+	ret = __ftrace_set_clr_event(tr, NULL, NULL, NULL, 1);
 	if (WARN_ON_ONCE(ret)) {
 		pr_warning("error enabling all events\n");
 		return;
@@ -1715,7 +2556,7 @@ static __init void event_trace_self_tests(void)
 	event_test_stuff();
 
 	/* reset sysname */
-	ret = __ftrace_set_clr_event(NULL, NULL, NULL, 0);
+	ret = __ftrace_set_clr_event(tr, NULL, NULL, NULL, 0);
 	if (WARN_ON_ONCE(ret)) {
 		pr_warning("error disabling all events\n");
 		return;
diff --git a/kernel/trace/trace_events_filter.c b/kernel/trace/trace_events_filter.c
index e5b0ca8b8d4..a6361178de5 100644
--- a/kernel/trace/trace_events_filter.c
+++ b/kernel/trace/trace_events_filter.c
@@ -658,33 +658,6 @@ void print_subsystem_event_filter(struct event_subsystem *system,
 	mutex_unlock(&event_mutex);
 }
 
-static struct ftrace_event_field *
-__find_event_field(struct list_head *head, char *name)
-{
-	struct ftrace_event_field *field;
-
-	list_for_each_entry(field, head, link) {
-		if (!strcmp(field->name, name))
-			return field;
-	}
-
-	return NULL;
-}
-
-static struct ftrace_event_field *
-find_event_field(struct ftrace_event_call *call, char *name)
-{
-	struct ftrace_event_field *field;
-	struct list_head *head;
-
-	field = __find_event_field(&ftrace_common_fields, name);
-	if (field)
-		return field;
-
-	head = trace_get_fields(call);
-	return __find_event_field(head, name);
-}
-
 static int __alloc_pred_stack(struct pred_stack *stack, int n_preds)
 {
 	stack->preds = kcalloc(n_preds + 1, sizeof(*stack->preds), GFP_KERNEL);
@@ -1337,7 +1310,7 @@ static struct filter_pred *create_pred(struct filter_parse_state *ps,
 		return NULL;
 	}
 
-	field = find_event_field(call, operand1);
+	field = trace_find_event_field(call, operand1);
 	if (!field) {
 		parse_error(ps, FILT_ERR_FIELD_NOT_FOUND, 0);
 		return NULL;
@@ -1907,16 +1880,17 @@ out_unlock:
 	return err;
 }
 
-int apply_subsystem_event_filter(struct event_subsystem *system,
+int apply_subsystem_event_filter(struct ftrace_subsystem_dir *dir,
 				 char *filter_string)
 {
+	struct event_subsystem *system = dir->subsystem;
 	struct event_filter *filter;
 	int err = 0;
 
 	mutex_lock(&event_mutex);
 
 	/* Make sure the system still has events */
-	if (!system->nr_events) {
+	if (!dir->nr_events) {
 		err = -ENODEV;
 		goto out_unlock;
 	}
diff --git a/kernel/trace/trace_export.c b/kernel/trace/trace_export.c
index e039906b037..d21a7467008 100644
--- a/kernel/trace/trace_export.c
+++ b/kernel/trace/trace_export.c
@@ -129,7 +129,7 @@ static void __always_unused ____ftrace_check_##name(void)		\
 
 #undef FTRACE_ENTRY
 #define FTRACE_ENTRY(name, struct_name, id, tstruct, print, filter)	\
-int									\
+static int __init							\
 ftrace_define_fields_##name(struct ftrace_event_call *event_call)	\
 {									\
 	struct struct_name field;					\
@@ -168,7 +168,7 @@ ftrace_define_fields_##name(struct ftrace_event_call *event_call)	\
 #define FTRACE_ENTRY_REG(call, struct_name, etype, tstruct, print, filter,\
 			 regfn)						\
 									\
-struct ftrace_event_class event_class_ftrace_##call = {			\
+struct ftrace_event_class __refdata event_class_ftrace_##call = {	\
 	.system			= __stringify(TRACE_SYSTEM),		\
 	.define_fields		= ftrace_define_fields_##call,		\
 	.fields			= LIST_HEAD_INIT(event_class_ftrace_##call.fields),\
diff --git a/kernel/trace/trace_functions.c b/kernel/trace/trace_functions.c
index 60115252332..c4d6d719198 100644
--- a/kernel/trace/trace_functions.c
+++ b/kernel/trace/trace_functions.c
@@ -28,7 +28,7 @@ static void tracing_stop_function_trace(void);
 static int function_trace_init(struct trace_array *tr)
 {
 	func_trace = tr;
-	tr->cpu = get_cpu();
+	tr->trace_buffer.cpu = get_cpu();
 	put_cpu();
 
 	tracing_start_cmdline_record();
@@ -44,7 +44,7 @@ static void function_trace_reset(struct trace_array *tr)
 
 static void function_trace_start(struct trace_array *tr)
 {
-	tracing_reset_online_cpus(tr);
+	tracing_reset_online_cpus(&tr->trace_buffer);
 }
 
 /* Our option */
@@ -76,7 +76,7 @@ function_trace_call(unsigned long ip, unsigned long parent_ip,
 		goto out;
 
 	cpu = smp_processor_id();
-	data = tr->data[cpu];
+	data = per_cpu_ptr(tr->trace_buffer.data, cpu);
 	if (!atomic_read(&data->disabled)) {
 		local_save_flags(flags);
 		trace_function(tr, ip, parent_ip, flags, pc);
@@ -107,7 +107,7 @@ function_stack_trace_call(unsigned long ip, unsigned long parent_ip,
 	 */
 	local_irq_save(flags);
 	cpu = raw_smp_processor_id();
-	data = tr->data[cpu];
+	data = per_cpu_ptr(tr->trace_buffer.data, cpu);
 	disabled = atomic_inc_return(&data->disabled);
 
 	if (likely(disabled == 1)) {
@@ -214,66 +214,89 @@ static struct tracer function_trace __read_mostly =
 };
 
 #ifdef CONFIG_DYNAMIC_FTRACE
-static void
-ftrace_traceon(unsigned long ip, unsigned long parent_ip, void **data)
+static int update_count(void **data)
 {
-	long *count = (long *)data;
-
-	if (tracing_is_on())
-		return;
+	unsigned long *count = (long *)data;
 
 	if (!*count)
-		return;
+		return 0;
 
 	if (*count != -1)
 		(*count)--;
 
-	tracing_on();
+	return 1;
 }
 
 static void
-ftrace_traceoff(unsigned long ip, unsigned long parent_ip, void **data)
+ftrace_traceon_count(unsigned long ip, unsigned long parent_ip, void **data)
 {
-	long *count = (long *)data;
+	if (tracing_is_on())
+		return;
+
+	if (update_count(data))
+		tracing_on();
+}
 
+static void
+ftrace_traceoff_count(unsigned long ip, unsigned long parent_ip, void **data)
+{
 	if (!tracing_is_on())
 		return;
 
-	if (!*count)
+	if (update_count(data))
+		tracing_off();
+}
+
+static void
+ftrace_traceon(unsigned long ip, unsigned long parent_ip, void **data)
+{
+	if (tracing_is_on())
 		return;
 
-	if (*count != -1)
-		(*count)--;
+	tracing_on();
+}
+
+static void
+ftrace_traceoff(unsigned long ip, unsigned long parent_ip, void **data)
+{
+	if (!tracing_is_on())
+		return;
 
 	tracing_off();
 }
 
-static int
-ftrace_trace_onoff_print(struct seq_file *m, unsigned long ip,
-			 struct ftrace_probe_ops *ops, void *data);
+/*
+ * Skip 4:
+ *   ftrace_stacktrace()
+ *   function_trace_probe_call()
+ *   ftrace_ops_list_func()
+ *   ftrace_call()
+ */
+#define STACK_SKIP 4
 
-static struct ftrace_probe_ops traceon_probe_ops = {
-	.func			= ftrace_traceon,
-	.print			= ftrace_trace_onoff_print,
-};
+static void
+ftrace_stacktrace(unsigned long ip, unsigned long parent_ip, void **data)
+{
+	trace_dump_stack(STACK_SKIP);
+}
 
-static struct ftrace_probe_ops traceoff_probe_ops = {
-	.func			= ftrace_traceoff,
-	.print			= ftrace_trace_onoff_print,
-};
+static void
+ftrace_stacktrace_count(unsigned long ip, unsigned long parent_ip, void **data)
+{
+	if (!tracing_is_on())
+		return;
+
+	if (update_count(data))
+		trace_dump_stack(STACK_SKIP);
+}
 
 static int
-ftrace_trace_onoff_print(struct seq_file *m, unsigned long ip,
-			 struct ftrace_probe_ops *ops, void *data)
+ftrace_probe_print(const char *name, struct seq_file *m,
+		   unsigned long ip, void *data)
 {
 	long count = (long)data;
 
-	seq_printf(m, "%ps:", (void *)ip);
-
-	if (ops == &traceon_probe_ops)
-		seq_printf(m, "traceon");
-	else
-		seq_printf(m, "traceoff");
+	seq_printf(m, "%ps:%s", (void *)ip, name);
 
 	if (count == -1)
 		seq_printf(m, ":unlimited\n");
@@ -284,26 +307,61 @@ ftrace_trace_onoff_print(struct seq_file *m, unsigned long ip,
 }
 
 static int
-ftrace_trace_onoff_unreg(char *glob, char *cmd, char *param)
+ftrace_traceon_print(struct seq_file *m, unsigned long ip,
+			 struct ftrace_probe_ops *ops, void *data)
 {
-	struct ftrace_probe_ops *ops;
-
-	/* we register both traceon and traceoff to this callback */
-	if (strcmp(cmd, "traceon") == 0)
-		ops = &traceon_probe_ops;
-	else
-		ops = &traceoff_probe_ops;
+	return ftrace_probe_print("traceon", m, ip, data);
+}
 
-	unregister_ftrace_function_probe_func(glob, ops);
+static int
+ftrace_traceoff_print(struct seq_file *m, unsigned long ip,
+			 struct ftrace_probe_ops *ops, void *data)
+{
+	return ftrace_probe_print("traceoff", m, ip, data);
+}
 
-	return 0;
+static int
+ftrace_stacktrace_print(struct seq_file *m, unsigned long ip,
+			struct ftrace_probe_ops *ops, void *data)
+{
+	return ftrace_probe_print("stacktrace", m, ip, data);
 }
 
+static struct ftrace_probe_ops traceon_count_probe_ops = {
+	.func			= ftrace_traceon_count,
+	.print			= ftrace_traceon_print,
+};
+
+static struct ftrace_probe_ops traceoff_count_probe_ops = {
+	.func			= ftrace_traceoff_count,
+	.print			= ftrace_traceoff_print,
+};
+
+static struct ftrace_probe_ops stacktrace_count_probe_ops = {
+	.func			= ftrace_stacktrace_count,
+	.print			= ftrace_stacktrace_print,
+};
+
+static struct ftrace_probe_ops traceon_probe_ops = {
+	.func			= ftrace_traceon,
+	.print			= ftrace_traceon_print,
+};
+
+static struct ftrace_probe_ops traceoff_probe_ops = {
+	.func			= ftrace_traceoff,
+	.print			= ftrace_traceoff_print,
+};
+
+static struct ftrace_probe_ops stacktrace_probe_ops = {
+	.func			= ftrace_stacktrace,
+	.print			= ftrace_stacktrace_print,
+};
+
 static int
-ftrace_trace_onoff_callback(struct ftrace_hash *hash,
-			    char *glob, char *cmd, char *param, int enable)
+ftrace_trace_probe_callback(struct ftrace_probe_ops *ops,
+			    struct ftrace_hash *hash, char *glob,
+			    char *cmd, char *param, int enable)
 {
-	struct ftrace_probe_ops *ops;
 	void *count = (void *)-1;
 	char *number;
 	int ret;
@@ -312,14 +370,10 @@ ftrace_trace_onoff_callback(struct ftrace_hash *hash,
 	if (!enable)
 		return -EINVAL;
 
-	if (glob[0] == '!')
-		return ftrace_trace_onoff_unreg(glob+1, cmd, param);
-
-	/* we register both traceon and traceoff to this callback */
-	if (strcmp(cmd, "traceon") == 0)
-		ops = &traceon_probe_ops;
-	else
-		ops = &traceoff_probe_ops;
+	if (glob[0] == '!') {
+		unregister_ftrace_function_probe_func(glob+1, ops);
+		return 0;
+	}
 
 	if (!param)
 		goto out_reg;
@@ -343,6 +397,34 @@ ftrace_trace_onoff_callback(struct ftrace_hash *hash,
 	return ret < 0 ? ret : 0;
 }
 
+static int
+ftrace_trace_onoff_callback(struct ftrace_hash *hash,
+			    char *glob, char *cmd, char *param, int enable)
+{
+	struct ftrace_probe_ops *ops;
+
+	/* we register both traceon and traceoff to this callback */
+	if (strcmp(cmd, "traceon") == 0)
+		ops = param ? &traceon_count_probe_ops : &traceon_probe_ops;
+	else
+		ops = param ? &traceoff_count_probe_ops : &traceoff_probe_ops;
+
+	return ftrace_trace_probe_callback(ops, hash, glob, cmd,
+					   param, enable);
+}
+
+static int
+ftrace_stacktrace_callback(struct ftrace_hash *hash,
+			   char *glob, char *cmd, char *param, int enable)
+{
+	struct ftrace_probe_ops *ops;
+
+	ops = param ? &stacktrace_count_probe_ops : &stacktrace_probe_ops;
+
+	return ftrace_trace_probe_callback(ops, hash, glob, cmd,
+					   param, enable);
+}
+
 static struct ftrace_func_command ftrace_traceon_cmd = {
 	.name			= "traceon",
 	.func			= ftrace_trace_onoff_callback,
@@ -353,6 +435,11 @@ static struct ftrace_func_command ftrace_traceoff_cmd = {
 	.func			= ftrace_trace_onoff_callback,
 };
 
+static struct ftrace_func_command ftrace_stacktrace_cmd = {
+	.name			= "stacktrace",
+	.func			= ftrace_stacktrace_callback,
+};
+
 static int __init init_func_cmd_traceon(void)
 {
 	int ret;
@@ -364,6 +451,12 @@ static int __init init_func_cmd_traceon(void)
 	ret = register_ftrace_command(&ftrace_traceon_cmd);
 	if (ret)
 		unregister_ftrace_command(&ftrace_traceoff_cmd);
+
+	ret = register_ftrace_command(&ftrace_stacktrace_cmd);
+	if (ret) {
+		unregister_ftrace_command(&ftrace_traceoff_cmd);
+		unregister_ftrace_command(&ftrace_traceon_cmd);
+	}
 	return ret;
 }
 #else
diff --git a/kernel/trace/trace_functions_graph.c b/kernel/trace/trace_functions_graph.c
index 39ada66389c..8388bc99f2e 100644
--- a/kernel/trace/trace_functions_graph.c
+++ b/kernel/trace/trace_functions_graph.c
@@ -218,7 +218,7 @@ int __trace_graph_entry(struct trace_array *tr,
 {
 	struct ftrace_event_call *call = &event_funcgraph_entry;
 	struct ring_buffer_event *event;
-	struct ring_buffer *buffer = tr->buffer;
+	struct ring_buffer *buffer = tr->trace_buffer.buffer;
 	struct ftrace_graph_ent_entry *entry;
 
 	if (unlikely(__this_cpu_read(ftrace_cpu_disabled)))
@@ -265,7 +265,7 @@ int trace_graph_entry(struct ftrace_graph_ent *trace)
 
 	local_irq_save(flags);
 	cpu = raw_smp_processor_id();
-	data = tr->data[cpu];
+	data = per_cpu_ptr(tr->trace_buffer.data, cpu);
 	disabled = atomic_inc_return(&data->disabled);
 	if (likely(disabled == 1)) {
 		pc = preempt_count();
@@ -323,7 +323,7 @@ void __trace_graph_return(struct trace_array *tr,
 {
 	struct ftrace_event_call *call = &event_funcgraph_exit;
 	struct ring_buffer_event *event;
-	struct ring_buffer *buffer = tr->buffer;
+	struct ring_buffer *buffer = tr->trace_buffer.buffer;
 	struct ftrace_graph_ret_entry *entry;
 
 	if (unlikely(__this_cpu_read(ftrace_cpu_disabled)))
@@ -350,7 +350,7 @@ void trace_graph_return(struct ftrace_graph_ret *trace)
 
 	local_irq_save(flags);
 	cpu = raw_smp_processor_id();
-	data = tr->data[cpu];
+	data = per_cpu_ptr(tr->trace_buffer.data, cpu);
 	disabled = atomic_inc_return(&data->disabled);
 	if (likely(disabled == 1)) {
 		pc = preempt_count();
@@ -560,9 +560,9 @@ get_return_for_leaf(struct trace_iterator *iter,
 			 * We need to consume the current entry to see
 			 * the next one.
 			 */
-			ring_buffer_consume(iter->tr->buffer, iter->cpu,
+			ring_buffer_consume(iter->trace_buffer->buffer, iter->cpu,
 					    NULL, NULL);
-			event = ring_buffer_peek(iter->tr->buffer, iter->cpu,
+			event = ring_buffer_peek(iter->trace_buffer->buffer, iter->cpu,
 						 NULL, NULL);
 		}
 
diff --git a/kernel/trace/trace_irqsoff.c b/kernel/trace/trace_irqsoff.c
index 443b25b43b4..b19d065a28c 100644
--- a/kernel/trace/trace_irqsoff.c
+++ b/kernel/trace/trace_irqsoff.c
@@ -33,6 +33,7 @@ enum {
 static int trace_type __read_mostly;
 
 static int save_flags;
+static bool function_enabled;
 
 static void stop_irqsoff_tracer(struct trace_array *tr, int graph);
 static int start_irqsoff_tracer(struct trace_array *tr, int graph);
@@ -121,7 +122,7 @@ static int func_prolog_dec(struct trace_array *tr,
 	if (!irqs_disabled_flags(*flags))
 		return 0;
 
-	*data = tr->data[cpu];
+	*data = per_cpu_ptr(tr->trace_buffer.data, cpu);
 	disabled = atomic_inc_return(&(*data)->disabled);
 
 	if (likely(disabled == 1))
@@ -175,7 +176,7 @@ static int irqsoff_set_flag(u32 old_flags, u32 bit, int set)
 		per_cpu(tracing_cpu, cpu) = 0;
 
 	tracing_max_latency = 0;
-	tracing_reset_online_cpus(irqsoff_trace);
+	tracing_reset_online_cpus(&irqsoff_trace->trace_buffer);
 
 	return start_irqsoff_tracer(irqsoff_trace, set);
 }
@@ -380,7 +381,7 @@ start_critical_timing(unsigned long ip, unsigned long parent_ip)
 	if (per_cpu(tracing_cpu, cpu))
 		return;
 
-	data = tr->data[cpu];
+	data = per_cpu_ptr(tr->trace_buffer.data, cpu);
 
 	if (unlikely(!data) || atomic_read(&data->disabled))
 		return;
@@ -418,7 +419,7 @@ stop_critical_timing(unsigned long ip, unsigned long parent_ip)
 	if (!tracer_enabled)
 		return;
 
-	data = tr->data[cpu];
+	data = per_cpu_ptr(tr->trace_buffer.data, cpu);
 
 	if (unlikely(!data) ||
 	    !data->critical_start || atomic_read(&data->disabled))
@@ -528,15 +529,60 @@ void trace_preempt_off(unsigned long a0, unsigned long a1)
 }
 #endif /* CONFIG_PREEMPT_TRACER */
 
-static int start_irqsoff_tracer(struct trace_array *tr, int graph)
+static int register_irqsoff_function(int graph, int set)
 {
-	int ret = 0;
+	int ret;
 
-	if (!graph)
-		ret = register_ftrace_function(&trace_ops);
-	else
+	/* 'set' is set if TRACE_ITER_FUNCTION is about to be set */
+	if (function_enabled || (!set && !(trace_flags & TRACE_ITER_FUNCTION)))
+		return 0;
+
+	if (graph)
 		ret = register_ftrace_graph(&irqsoff_graph_return,
 					    &irqsoff_graph_entry);
+	else
+		ret = register_ftrace_function(&trace_ops);
+
+	if (!ret)
+		function_enabled = true;
+
+	return ret;
+}
+
+static void unregister_irqsoff_function(int graph)
+{
+	if (!function_enabled)
+		return;
+
+	if (graph)
+		unregister_ftrace_graph();
+	else
+		unregister_ftrace_function(&trace_ops);
+
+	function_enabled = false;
+}
+
+static void irqsoff_function_set(int set)
+{
+	if (set)
+		register_irqsoff_function(is_graph(), 1);
+	else
+		unregister_irqsoff_function(is_graph());
+}
+
+static int irqsoff_flag_changed(struct tracer *tracer, u32 mask, int set)
+{
+	if (mask & TRACE_ITER_FUNCTION)
+		irqsoff_function_set(set);
+
+	return trace_keep_overwrite(tracer, mask, set);
+}
+
+static int start_irqsoff_tracer(struct trace_array *tr, int graph)
+{
+	int ret;
+
+	ret = register_irqsoff_function(graph, 0);
 
 	if (!ret && tracing_is_enabled())
 		tracer_enabled = 1;
@@ -550,10 +596,7 @@ static void stop_irqsoff_tracer(struct trace_array *tr, int graph)
 {
 	tracer_enabled = 0;
 
-	if (!graph)
-		unregister_ftrace_function(&trace_ops);
-	else
-		unregister_ftrace_graph();
+	unregister_irqsoff_function(graph);
 }
 
 static void __irqsoff_tracer_init(struct trace_array *tr)
@@ -561,14 +604,14 @@ static void __irqsoff_tracer_init(struct trace_array *tr)
 	save_flags = trace_flags;
 
 	/* non overwrite screws up the latency tracers */
-	set_tracer_flag(TRACE_ITER_OVERWRITE, 1);
-	set_tracer_flag(TRACE_ITER_LATENCY_FMT, 1);
+	set_tracer_flag(tr, TRACE_ITER_OVERWRITE, 1);
+	set_tracer_flag(tr, TRACE_ITER_LATENCY_FMT, 1);
 
 	tracing_max_latency = 0;
 	irqsoff_trace = tr;
 	/* make sure that the tracer is visible */
 	smp_wmb();
-	tracing_reset_online_cpus(tr);
+	tracing_reset_online_cpus(&tr->trace_buffer);
 
 	if (start_irqsoff_tracer(tr, is_graph()))
 		printk(KERN_ERR "failed to start irqsoff tracer\n");
@@ -581,8 +624,8 @@ static void irqsoff_tracer_reset(struct trace_array *tr)
 
 	stop_irqsoff_tracer(tr, is_graph());
 
-	set_tracer_flag(TRACE_ITER_LATENCY_FMT, lat_flag);
-	set_tracer_flag(TRACE_ITER_OVERWRITE, overwrite_flag);
+	set_tracer_flag(tr, TRACE_ITER_LATENCY_FMT, lat_flag);
+	set_tracer_flag(tr, TRACE_ITER_OVERWRITE, overwrite_flag);
 }
 
 static void irqsoff_tracer_start(struct trace_array *tr)
@@ -615,7 +658,7 @@ static struct tracer irqsoff_tracer __read_mostly =
 	.print_line     = irqsoff_print_line,
 	.flags		= &tracer_flags,
 	.set_flag	= irqsoff_set_flag,
-	.flag_changed	= trace_keep_overwrite,
+	.flag_changed	= irqsoff_flag_changed,
 #ifdef CONFIG_FTRACE_SELFTEST
 	.selftest    = trace_selftest_startup_irqsoff,
 #endif
@@ -649,7 +692,7 @@ static struct tracer preemptoff_tracer __read_mostly =
 	.print_line     = irqsoff_print_line,
 	.flags		= &tracer_flags,
 	.set_flag	= irqsoff_set_flag,
-	.flag_changed	= trace_keep_overwrite,
+	.flag_changed	= irqsoff_flag_changed,
 #ifdef CONFIG_FTRACE_SELFTEST
 	.selftest    = trace_selftest_startup_preemptoff,
 #endif
@@ -685,7 +728,7 @@ static struct tracer preemptirqsoff_tracer __read_mostly =
 	.print_line     = irqsoff_print_line,
 	.flags		= &tracer_flags,
 	.set_flag	= irqsoff_set_flag,
-	.flag_changed	= trace_keep_overwrite,
+	.flag_changed	= irqsoff_flag_changed,
 #ifdef CONFIG_FTRACE_SELFTEST
 	.selftest    = trace_selftest_startup_preemptirqsoff,
 #endif
diff --git a/kernel/trace/trace_kdb.c b/kernel/trace/trace_kdb.c
index 3c5c5dfea0b..bd90e1b0608 100644
--- a/kernel/trace/trace_kdb.c
+++ b/kernel/trace/trace_kdb.c
@@ -26,7 +26,7 @@ static void ftrace_dump_buf(int skip_lines, long cpu_file)
 	trace_init_global_iter(&iter);
 
 	for_each_tracing_cpu(cpu) {
-		atomic_inc(&iter.tr->data[cpu]->disabled);
+		atomic_inc(&per_cpu_ptr(iter.trace_buffer->data, cpu)->disabled);
 	}
 
 	old_userobj = trace_flags;
@@ -43,17 +43,17 @@ static void ftrace_dump_buf(int skip_lines, long cpu_file)
 	iter.iter_flags |= TRACE_FILE_LAT_FMT;
 	iter.pos = -1;
 
-	if (cpu_file == TRACE_PIPE_ALL_CPU) {
+	if (cpu_file == RING_BUFFER_ALL_CPUS) {
 		for_each_tracing_cpu(cpu) {
 			iter.buffer_iter[cpu] =
-			ring_buffer_read_prepare(iter.tr->buffer, cpu);
+			ring_buffer_read_prepare(iter.trace_buffer->buffer, cpu);
 			ring_buffer_read_start(iter.buffer_iter[cpu]);
 			tracing_iter_reset(&iter, cpu);
 		}
 	} else {
 		iter.cpu_file = cpu_file;
 		iter.buffer_iter[cpu_file] =
-			ring_buffer_read_prepare(iter.tr->buffer, cpu_file);
+			ring_buffer_read_prepare(iter.trace_buffer->buffer, cpu_file);
 		ring_buffer_read_start(iter.buffer_iter[cpu_file]);
 		tracing_iter_reset(&iter, cpu_file);
 	}
@@ -83,7 +83,7 @@ out:
 	trace_flags = old_userobj;
 
 	for_each_tracing_cpu(cpu) {
-		atomic_dec(&iter.tr->data[cpu]->disabled);
+		atomic_dec(&per_cpu_ptr(iter.trace_buffer->data, cpu)->disabled);
 	}
 
 	for_each_tracing_cpu(cpu)
@@ -115,7 +115,7 @@ static int kdb_ftdump(int argc, const char **argv)
 		    !cpu_online(cpu_file))
 			return KDB_BADINT;
 	} else {
-		cpu_file = TRACE_PIPE_ALL_CPU;
+		cpu_file = RING_BUFFER_ALL_CPUS;
 	}
 
 	kdb_trap_printk++;
diff --git a/kernel/trace/trace_mmiotrace.c b/kernel/trace/trace_mmiotrace.c
index fd3c8aae55e..a5e8f4878bf 100644
--- a/kernel/trace/trace_mmiotrace.c
+++ b/kernel/trace/trace_mmiotrace.c
@@ -31,7 +31,7 @@ static void mmio_reset_data(struct trace_array *tr)
 	overrun_detected = false;
 	prev_overruns = 0;
 
-	tracing_reset_online_cpus(tr);
+	tracing_reset_online_cpus(&tr->trace_buffer);
 }
 
 static int mmio_trace_init(struct trace_array *tr)
@@ -128,7 +128,7 @@ static void mmio_close(struct trace_iterator *iter)
 static unsigned long count_overruns(struct trace_iterator *iter)
 {
 	unsigned long cnt = atomic_xchg(&dropped_count, 0);
-	unsigned long over = ring_buffer_overruns(iter->tr->buffer);
+	unsigned long over = ring_buffer_overruns(iter->trace_buffer->buffer);
 
 	if (over > prev_overruns)
 		cnt += over - prev_overruns;
@@ -309,7 +309,7 @@ static void __trace_mmiotrace_rw(struct trace_array *tr,
 				struct mmiotrace_rw *rw)
 {
 	struct ftrace_event_call *call = &event_mmiotrace_rw;
-	struct ring_buffer *buffer = tr->buffer;
+	struct ring_buffer *buffer = tr->trace_buffer.buffer;
 	struct ring_buffer_event *event;
 	struct trace_mmiotrace_rw *entry;
 	int pc = preempt_count();
@@ -330,7 +330,7 @@ static void __trace_mmiotrace_rw(struct trace_array *tr,
 void mmio_trace_rw(struct mmiotrace_rw *rw)
 {
 	struct trace_array *tr = mmio_trace_array;
-	struct trace_array_cpu *data = tr->data[smp_processor_id()];
+	struct trace_array_cpu *data = per_cpu_ptr(tr->trace_buffer.data, smp_processor_id());
 	__trace_mmiotrace_rw(tr, data, rw);
 }
 
@@ -339,7 +339,7 @@ static void __trace_mmiotrace_map(struct trace_array *tr,
 				struct mmiotrace_map *map)
 {
 	struct ftrace_event_call *call = &event_mmiotrace_map;
-	struct ring_buffer *buffer = tr->buffer;
+	struct ring_buffer *buffer = tr->trace_buffer.buffer;
 	struct ring_buffer_event *event;
 	struct trace_mmiotrace_map *entry;
 	int pc = preempt_count();
@@ -363,7 +363,7 @@ void mmio_trace_mapping(struct mmiotrace_map *map)
 	struct trace_array_cpu *data;
 
 	preempt_disable();
-	data = tr->data[smp_processor_id()];
+	data = per_cpu_ptr(tr->trace_buffer.data, smp_processor_id());
 	__trace_mmiotrace_map(tr, data, map);
 	preempt_enable();
 }
diff --git a/kernel/trace/trace_output.c b/kernel/trace/trace_output.c
index 697e88d1390..bb922d9ee51 100644
--- a/kernel/trace/trace_output.c
+++ b/kernel/trace/trace_output.c
@@ -14,7 +14,7 @@
 /* must be a power of 2 */
 #define EVENT_HASHSIZE	128
 
-DECLARE_RWSEM(trace_event_mutex);
+DECLARE_RWSEM(trace_event_sem);
 
 static struct hlist_head event_hash[EVENT_HASHSIZE] __read_mostly;
 
@@ -37,6 +37,22 @@ int trace_print_seq(struct seq_file *m, struct trace_seq *s)
 	return ret;
 }
 
+enum print_line_t trace_print_bputs_msg_only(struct trace_iterator *iter)
+{
+	struct trace_seq *s = &iter->seq;
+	struct trace_entry *entry = iter->ent;
+	struct bputs_entry *field;
+	int ret;
+
+	trace_assign_type(field, entry);
+
+	ret = trace_seq_puts(s, field->str);
+	if (!ret)
+		return TRACE_TYPE_PARTIAL_LINE;
+
+	return TRACE_TYPE_HANDLED;
+}
+
 enum print_line_t trace_print_bprintk_msg_only(struct trace_iterator *iter)
 {
 	struct trace_seq *s = &iter->seq;
@@ -397,6 +413,32 @@ ftrace_print_hex_seq(struct trace_seq *p, const unsigned char *buf, int buf_len)
 }
 EXPORT_SYMBOL(ftrace_print_hex_seq);
 
+int ftrace_raw_output_prep(struct trace_iterator *iter,
+			   struct trace_event *trace_event)
+{
+	struct ftrace_event_call *event;
+	struct trace_seq *s = &iter->seq;
+	struct trace_seq *p = &iter->tmp_seq;
+	struct trace_entry *entry;
+	int ret;
+
+	event = container_of(trace_event, struct ftrace_event_call, event);
+	entry = iter->ent;
+
+	if (entry->type != event->event.type) {
+		WARN_ON_ONCE(1);
+		return TRACE_TYPE_UNHANDLED;
+	}
+
+	trace_seq_init(p);
+	ret = trace_seq_printf(s, "%s: ", event->name);
+	if (!ret)
+		return TRACE_TYPE_PARTIAL_LINE;
+
+	return 0;
+}
+EXPORT_SYMBOL(ftrace_raw_output_prep);
+
 #ifdef CONFIG_KRETPROBES
 static inline const char *kretprobed(const char *name)
 {
@@ -617,7 +659,7 @@ lat_print_timestamp(struct trace_iterator *iter, u64 next_ts)
 {
 	unsigned long verbose = trace_flags & TRACE_ITER_VERBOSE;
 	unsigned long in_ns = iter->iter_flags & TRACE_FILE_TIME_IN_NS;
-	unsigned long long abs_ts = iter->ts - iter->tr->time_start;
+	unsigned long long abs_ts = iter->ts - iter->trace_buffer->time_start;
 	unsigned long long rel_ts = next_ts - iter->ts;
 	struct trace_seq *s = &iter->seq;
 
@@ -783,12 +825,12 @@ static int trace_search_list(struct list_head **list)
 
 void trace_event_read_lock(void)
 {
-	down_read(&trace_event_mutex);
+	down_read(&trace_event_sem);
 }
 
 void trace_event_read_unlock(void)
 {
-	up_read(&trace_event_mutex);
+	up_read(&trace_event_sem);
 }
 
 /**
@@ -811,7 +853,7 @@ int register_ftrace_event(struct trace_event *event)
 	unsigned key;
 	int ret = 0;
 
-	down_write(&trace_event_mutex);
+	down_write(&trace_event_sem);
 
 	if (WARN_ON(!event))
 		goto out;
@@ -866,14 +908,14 @@ int register_ftrace_event(struct trace_event *event)
 
 	ret = event->type;
  out:
-	up_write(&trace_event_mutex);
+	up_write(&trace_event_sem);
 
 	return ret;
 }
 EXPORT_SYMBOL_GPL(register_ftrace_event);
 
 /*
- * Used by module code with the trace_event_mutex held for write.
+ * Used by module code with the trace_event_sem held for write.
  */
 int __unregister_ftrace_event(struct trace_event *event)
 {
@@ -888,9 +930,9 @@ int __unregister_ftrace_event(struct trace_event *event)
  */
 int unregister_ftrace_event(struct trace_event *event)
 {
-	down_write(&trace_event_mutex);
+	down_write(&trace_event_sem);
 	__unregister_ftrace_event(event);
-	up_write(&trace_event_mutex);
+	up_write(&trace_event_sem);
 
 	return 0;
 }
@@ -1217,6 +1259,64 @@ static struct trace_event trace_user_stack_event = {
 	.funcs		= &trace_user_stack_funcs,
 };
 
+/* TRACE_BPUTS */
+static enum print_line_t
+trace_bputs_print(struct trace_iterator *iter, int flags,
+		   struct trace_event *event)
+{
+	struct trace_entry *entry = iter->ent;
+	struct trace_seq *s = &iter->seq;
+	struct bputs_entry *field;
+
+	trace_assign_type(field, entry);
+
+	if (!seq_print_ip_sym(s, field->ip, flags))
+		goto partial;
+
+	if (!trace_seq_puts(s, ": "))
+		goto partial;
+
+	if (!trace_seq_puts(s, field->str))
+		goto partial;
+
+	return TRACE_TYPE_HANDLED;
+
+ partial:
+	return TRACE_TYPE_PARTIAL_LINE;
+}
+
+
+static enum print_line_t
+trace_bputs_raw(struct trace_iterator *iter, int flags,
+		struct trace_event *event)
+{
+	struct bputs_entry *field;
+	struct trace_seq *s = &iter->seq;
+
+	trace_assign_type(field, iter->ent);
+
+	if (!trace_seq_printf(s, ": %lx : ", field->ip))
+		goto partial;
+
+	if (!trace_seq_puts(s, field->str))
+		goto partial;
+
+	return TRACE_TYPE_HANDLED;
+
+ partial:
+	return TRACE_TYPE_PARTIAL_LINE;
+}
+
+static struct trace_event_functions trace_bputs_funcs = {
+	.trace		= trace_bputs_print,
+	.raw		= trace_bputs_raw,
+};
+
+static struct trace_event trace_bputs_event = {
+	.type		= TRACE_BPUTS,
+	.funcs		= &trace_bputs_funcs,
+};
+
 /* TRACE_BPRINT */
 static enum print_line_t
 trace_bprint_print(struct trace_iterator *iter, int flags,
@@ -1329,6 +1429,7 @@ static struct trace_event *events[] __initdata = {
 	&trace_wake_event,
 	&trace_stack_event,
 	&trace_user_stack_event,
+	&trace_bputs_event,
 	&trace_bprint_event,
 	&trace_print_event,
 	NULL
diff --git a/kernel/trace/trace_output.h b/kernel/trace/trace_output.h
index c038eba0492..127a9d8c835 100644
--- a/kernel/trace/trace_output.h
+++ b/kernel/trace/trace_output.h
@@ -5,6 +5,8 @@
 #include "trace.h"
 
 extern enum print_line_t
+trace_print_bputs_msg_only(struct trace_iterator *iter);
+extern enum print_line_t
 trace_print_bprintk_msg_only(struct trace_iterator *iter);
 extern enum print_line_t
 trace_print_printk_msg_only(struct trace_iterator *iter);
@@ -31,7 +33,7 @@ trace_print_lat_fmt(struct trace_seq *s, struct trace_entry *entry);
 
 /* used by module unregistering */
 extern int __unregister_ftrace_event(struct trace_event *event);
-extern struct rw_semaphore trace_event_mutex;
+extern struct rw_semaphore trace_event_sem;
 
 #define MAX_MEMHEX_BYTES	8
 #define HEX_CHARS		(MAX_MEMHEX_BYTES*2 + 1)
diff --git a/kernel/trace/trace_sched_switch.c b/kernel/trace/trace_sched_switch.c
index 3374c792ccd..4e98e3b257a 100644
--- a/kernel/trace/trace_sched_switch.c
+++ b/kernel/trace/trace_sched_switch.c
@@ -28,7 +28,7 @@ tracing_sched_switch_trace(struct trace_array *tr,
 			   unsigned long flags, int pc)
 {
 	struct ftrace_event_call *call = &event_context_switch;
-	struct ring_buffer *buffer = tr->buffer;
+	struct ring_buffer *buffer = tr->trace_buffer.buffer;
 	struct ring_buffer_event *event;
 	struct ctx_switch_entry *entry;
 
@@ -69,7 +69,7 @@ probe_sched_switch(void *ignore, struct task_struct *prev, struct task_struct *n
 	pc = preempt_count();
 	local_irq_save(flags);
 	cpu = raw_smp_processor_id();
-	data = ctx_trace->data[cpu];
+	data = per_cpu_ptr(ctx_trace->trace_buffer.data, cpu);
 
 	if (likely(!atomic_read(&data->disabled)))
 		tracing_sched_switch_trace(ctx_trace, prev, next, flags, pc);
@@ -86,7 +86,7 @@ tracing_sched_wakeup_trace(struct trace_array *tr,
 	struct ftrace_event_call *call = &event_wakeup;
 	struct ring_buffer_event *event;
 	struct ctx_switch_entry *entry;
-	struct ring_buffer *buffer = tr->buffer;
+	struct ring_buffer *buffer = tr->trace_buffer.buffer;
 
 	event = trace_buffer_lock_reserve(buffer, TRACE_WAKE,
 					  sizeof(*entry), flags, pc);
@@ -123,7 +123,7 @@ probe_sched_wakeup(void *ignore, struct task_struct *wakee, int success)
 	pc = preempt_count();
 	local_irq_save(flags);
 	cpu = raw_smp_processor_id();
-	data = ctx_trace->data[cpu];
+	data = per_cpu_ptr(ctx_trace->trace_buffer.data, cpu);
 
 	if (likely(!atomic_read(&data->disabled)))
 		tracing_sched_wakeup_trace(ctx_trace, wakee, current,
diff --git a/kernel/trace/trace_sched_wakeup.c b/kernel/trace/trace_sched_wakeup.c
index fde652c9a51..fee77e15d81 100644
--- a/kernel/trace/trace_sched_wakeup.c
+++ b/kernel/trace/trace_sched_wakeup.c
@@ -37,6 +37,7 @@ static int wakeup_graph_entry(struct ftrace_graph_ent *trace);
 static void wakeup_graph_return(struct ftrace_graph_ret *trace);
 
 static int save_flags;
+static bool function_enabled;
 
 #define TRACE_DISPLAY_GRAPH     1
 
@@ -89,7 +90,7 @@ func_prolog_preempt_disable(struct trace_array *tr,
 	if (cpu != wakeup_current_cpu)
 		goto out_enable;
 
-	*data = tr->data[cpu];
+	*data = per_cpu_ptr(tr->trace_buffer.data, cpu);
 	disabled = atomic_inc_return(&(*data)->disabled);
 	if (unlikely(disabled != 1))
 		goto out;
@@ -134,15 +135,60 @@ static struct ftrace_ops trace_ops __read_mostly =
 };
 #endif /* CONFIG_FUNCTION_TRACER */
 
-static int start_func_tracer(int graph)
+static int register_wakeup_function(int graph, int set)
 {
 	int ret;
 
-	if (!graph)
-		ret = register_ftrace_function(&trace_ops);
-	else
+	/* 'set' is set if TRACE_ITER_FUNCTION is about to be set */
+	if (function_enabled || (!set && !(trace_flags & TRACE_ITER_FUNCTION)))
+		return 0;
+
+	if (graph)
 		ret = register_ftrace_graph(&wakeup_graph_return,
 					    &wakeup_graph_entry);
+	else
+		ret = register_ftrace_function(&trace_ops);
+
+	if (!ret)
+		function_enabled = true;
+
+	return ret;
+}
+
+static void unregister_wakeup_function(int graph)
+{
+	if (!function_enabled)
+		return;
+
+	if (graph)
+		unregister_ftrace_graph();
+	else
+		unregister_ftrace_function(&trace_ops);
+
+	function_enabled = false;
+}
+
+static void wakeup_function_set(int set)
+{
+	if (set)
+		register_wakeup_function(is_graph(), 1);
+	else
+		unregister_wakeup_function(is_graph());
+}
+
+static int wakeup_flag_changed(struct tracer *tracer, u32 mask, int set)
+{
+	if (mask & TRACE_ITER_FUNCTION)
+		wakeup_function_set(set);
+
+	return trace_keep_overwrite(tracer, mask, set);
+}
+
+static int start_func_tracer(int graph)
+{
+	int ret;
+
+	ret = register_wakeup_function(graph, 0);
 
 	if (!ret && tracing_is_enabled())
 		tracer_enabled = 1;
@@ -156,10 +202,7 @@ static void stop_func_tracer(int graph)
 {
 	tracer_enabled = 0;
 
-	if (!graph)
-		unregister_ftrace_function(&trace_ops);
-	else
-		unregister_ftrace_graph();
+	unregister_wakeup_function(graph);
 }
 
 #ifdef CONFIG_FUNCTION_GRAPH_TRACER
@@ -353,7 +396,7 @@ probe_wakeup_sched_switch(void *ignore,
 
 	/* disable local data, not wakeup_cpu data */
 	cpu = raw_smp_processor_id();
-	disabled = atomic_inc_return(&wakeup_trace->data[cpu]->disabled);
+	disabled = atomic_inc_return(&per_cpu_ptr(wakeup_trace->trace_buffer.data, cpu)->disabled);
 	if (likely(disabled != 1))
 		goto out;
 
@@ -365,7 +408,7 @@ probe_wakeup_sched_switch(void *ignore,
 		goto out_unlock;
 
 	/* The task we are waiting for is waking up */
-	data = wakeup_trace->data[wakeup_cpu];
+	data = per_cpu_ptr(wakeup_trace->trace_buffer.data, wakeup_cpu);
 
 	__trace_function(wakeup_trace, CALLER_ADDR0, CALLER_ADDR1, flags, pc);
 	tracing_sched_switch_trace(wakeup_trace, prev, next, flags, pc);
@@ -387,7 +430,7 @@ out_unlock:
 	arch_spin_unlock(&wakeup_lock);
 	local_irq_restore(flags);
 out:
-	atomic_dec(&wakeup_trace->data[cpu]->disabled);
+	atomic_dec(&per_cpu_ptr(wakeup_trace->trace_buffer.data, cpu)->disabled);
 }
 
 static void __wakeup_reset(struct trace_array *tr)
@@ -405,7 +448,7 @@ static void wakeup_reset(struct trace_array *tr)
 {
 	unsigned long flags;
 
-	tracing_reset_online_cpus(tr);
+	tracing_reset_online_cpus(&tr->trace_buffer);
 
 	local_irq_save(flags);
 	arch_spin_lock(&wakeup_lock);
@@ -435,7 +478,7 @@ probe_wakeup(void *ignore, struct task_struct *p, int success)
 		return;
 
 	pc = preempt_count();
-	disabled = atomic_inc_return(&wakeup_trace->data[cpu]->disabled);
+	disabled = atomic_inc_return(&per_cpu_ptr(wakeup_trace->trace_buffer.data, cpu)->disabled);
 	if (unlikely(disabled != 1))
 		goto out;
 
@@ -458,7 +501,7 @@ probe_wakeup(void *ignore, struct task_struct *p, int success)
 
 	local_save_flags(flags);
 
-	data = wakeup_trace->data[wakeup_cpu];
+	data = per_cpu_ptr(wakeup_trace->trace_buffer.data, wakeup_cpu);
 	data->preempt_timestamp = ftrace_now(cpu);
 	tracing_sched_wakeup_trace(wakeup_trace, p, current, flags, pc);
 
@@ -472,7 +515,7 @@ probe_wakeup(void *ignore, struct task_struct *p, int success)
 out_locked:
 	arch_spin_unlock(&wakeup_lock);
 out:
-	atomic_dec(&wakeup_trace->data[cpu]->disabled);
+	atomic_dec(&per_cpu_ptr(wakeup_trace->trace_buffer.data, cpu)->disabled);
 }
 
 static void start_wakeup_tracer(struct trace_array *tr)
@@ -543,8 +586,8 @@ static int __wakeup_tracer_init(struct trace_array *tr)
 	save_flags = trace_flags;
 
 	/* non overwrite screws up the latency tracers */
-	set_tracer_flag(TRACE_ITER_OVERWRITE, 1);
-	set_tracer_flag(TRACE_ITER_LATENCY_FMT, 1);
+	set_tracer_flag(tr, TRACE_ITER_OVERWRITE, 1);
+	set_tracer_flag(tr, TRACE_ITER_LATENCY_FMT, 1);
 
 	tracing_max_latency = 0;
 	wakeup_trace = tr;
@@ -573,8 +616,8 @@ static void wakeup_tracer_reset(struct trace_array *tr)
 	/* make sure we put back any tasks we are tracing */
 	wakeup_reset(tr);
 
-	set_tracer_flag(TRACE_ITER_LATENCY_FMT, lat_flag);
-	set_tracer_flag(TRACE_ITER_OVERWRITE, overwrite_flag);
+	set_tracer_flag(tr, TRACE_ITER_LATENCY_FMT, lat_flag);
+	set_tracer_flag(tr, TRACE_ITER_OVERWRITE, overwrite_flag);
 }
 
 static void wakeup_tracer_start(struct trace_array *tr)
@@ -600,7 +643,7 @@ static struct tracer wakeup_tracer __read_mostly =
 	.print_line	= wakeup_print_line,
 	.flags		= &tracer_flags,
 	.set_flag	= wakeup_set_flag,
-	.flag_changed	= trace_keep_overwrite,
+	.flag_changed	= wakeup_flag_changed,
 #ifdef CONFIG_FTRACE_SELFTEST
 	.selftest    = trace_selftest_startup_wakeup,
 #endif
@@ -622,7 +665,7 @@ static struct tracer wakeup_rt_tracer __read_mostly =
 	.print_line	= wakeup_print_line,
 	.flags		= &tracer_flags,
 	.set_flag	= wakeup_set_flag,
-	.flag_changed	= trace_keep_overwrite,
+	.flag_changed	= wakeup_flag_changed,
 #ifdef CONFIG_FTRACE_SELFTEST
 	.selftest    = trace_selftest_startup_wakeup,
 #endif
diff --git a/kernel/trace/trace_selftest.c b/kernel/trace/trace_selftest.c
index 51c819c12c2..55e2cf66967 100644
--- a/kernel/trace/trace_selftest.c
+++ b/kernel/trace/trace_selftest.c
@@ -21,13 +21,13 @@ static inline int trace_valid_entry(struct trace_entry *entry)
 	return 0;
 }
 
-static int trace_test_buffer_cpu(struct trace_array *tr, int cpu)
+static int trace_test_buffer_cpu(struct trace_buffer *buf, int cpu)
 {
 	struct ring_buffer_event *event;
 	struct trace_entry *entry;
 	unsigned int loops = 0;
 
-	while ((event = ring_buffer_consume(tr->buffer, cpu, NULL, NULL))) {
+	while ((event = ring_buffer_consume(buf->buffer, cpu, NULL, NULL))) {
 		entry = ring_buffer_event_data(event);
 
 		/*
@@ -58,7 +58,7 @@ static int trace_test_buffer_cpu(struct trace_array *tr, int cpu)
  * Test the trace buffer to see if all the elements
  * are still sane.
  */
-static int trace_test_buffer(struct trace_array *tr, unsigned long *count)
+static int trace_test_buffer(struct trace_buffer *buf, unsigned long *count)
 {
 	unsigned long flags, cnt = 0;
 	int cpu, ret = 0;
@@ -67,7 +67,7 @@ static int trace_test_buffer(struct trace_array *tr, unsigned long *count)
 	local_irq_save(flags);
 	arch_spin_lock(&ftrace_max_lock);
 
-	cnt = ring_buffer_entries(tr->buffer);
+	cnt = ring_buffer_entries(buf->buffer);
 
 	/*
 	 * The trace_test_buffer_cpu runs a while loop to consume all data.
@@ -78,7 +78,7 @@ static int trace_test_buffer(struct trace_array *tr, unsigned long *count)
 	 */
 	tracing_off();
 	for_each_possible_cpu(cpu) {
-		ret = trace_test_buffer_cpu(tr, cpu);
+		ret = trace_test_buffer_cpu(buf, cpu);
 		if (ret)
 			break;
 	}
@@ -355,7 +355,7 @@ int trace_selftest_startup_dynamic_tracing(struct tracer *trace,
 	msleep(100);
 
 	/* we should have nothing in the buffer */
-	ret = trace_test_buffer(tr, &count);
+	ret = trace_test_buffer(&tr->trace_buffer, &count);
 	if (ret)
 		goto out;
 
@@ -376,7 +376,7 @@ int trace_selftest_startup_dynamic_tracing(struct tracer *trace,
 	ftrace_enabled = 0;
 
 	/* check the trace buffer */
-	ret = trace_test_buffer(tr, &count);
+	ret = trace_test_buffer(&tr->trace_buffer, &count);
 	tracing_start();
 
 	/* we should only have one item */
@@ -666,7 +666,7 @@ trace_selftest_startup_function(struct tracer *trace, struct trace_array *tr)
 	ftrace_enabled = 0;
 
 	/* check the trace buffer */
-	ret = trace_test_buffer(tr, &count);
+	ret = trace_test_buffer(&tr->trace_buffer, &count);
 	trace->reset(tr);
 	tracing_start();
 
@@ -703,8 +703,6 @@ trace_selftest_startup_function(struct tracer *trace, struct trace_array *tr)
 /* Maximum number of functions to trace before diagnosing a hang */
 #define GRAPH_MAX_FUNC_TEST	100000000
 
-static void
-__ftrace_dump(bool disable_tracing, enum ftrace_dump_mode oops_dump_mode);
 static unsigned int graph_hang_thresh;
 
 /* Wrap the real function entry probe to avoid possible hanging */
@@ -714,8 +712,11 @@ static int trace_graph_entry_watchdog(struct ftrace_graph_ent *trace)
 	if (unlikely(++graph_hang_thresh > GRAPH_MAX_FUNC_TEST)) {
 		ftrace_graph_stop();
 		printk(KERN_WARNING "BUG: Function graph tracer hang!\n");
-		if (ftrace_dump_on_oops)
-			__ftrace_dump(false, DUMP_ALL);
+		if (ftrace_dump_on_oops) {
+			ftrace_dump(DUMP_ALL);
+			/* ftrace_dump() disables tracing */
+			tracing_on();
+		}
 		return 0;
 	}
 
@@ -737,7 +738,7 @@ trace_selftest_startup_function_graph(struct tracer *trace,
 	 * Simulate the init() callback but we attach a watchdog callback
 	 * to detect and recover from possible hangs
 	 */
-	tracing_reset_online_cpus(tr);
+	tracing_reset_online_cpus(&tr->trace_buffer);
 	set_graph_array(tr);
 	ret = register_ftrace_graph(&trace_graph_return,
 				    &trace_graph_entry_watchdog);
@@ -760,7 +761,7 @@ trace_selftest_startup_function_graph(struct tracer *trace,
 	tracing_stop();
 
 	/* check the trace buffer */
-	ret = trace_test_buffer(tr, &count);
+	ret = trace_test_buffer(&tr->trace_buffer, &count);
 
 	trace->reset(tr);
 	tracing_start();
@@ -815,9 +816,9 @@ trace_selftest_startup_irqsoff(struct tracer *trace, struct trace_array *tr)
 	/* stop the tracing. */
 	tracing_stop();
 	/* check both trace buffers */
-	ret = trace_test_buffer(tr, NULL);
+	ret = trace_test_buffer(&tr->trace_buffer, NULL);
 	if (!ret)
-		ret = trace_test_buffer(&max_tr, &count);
+		ret = trace_test_buffer(&tr->max_buffer, &count);
 	trace->reset(tr);
 	tracing_start();
 
@@ -877,9 +878,9 @@ trace_selftest_startup_preemptoff(struct tracer *trace, struct trace_array *tr)
 	/* stop the tracing. */
 	tracing_stop();
 	/* check both trace buffers */
-	ret = trace_test_buffer(tr, NULL);
+	ret = trace_test_buffer(&tr->trace_buffer, NULL);
 	if (!ret)
-		ret = trace_test_buffer(&max_tr, &count);
+		ret = trace_test_buffer(&tr->max_buffer, &count);
 	trace->reset(tr);
 	tracing_start();
 
@@ -943,11 +944,11 @@ trace_selftest_startup_preemptirqsoff(struct tracer *trace, struct trace_array *
 	/* stop the tracing. */
 	tracing_stop();
 	/* check both trace buffers */
-	ret = trace_test_buffer(tr, NULL);
+	ret = trace_test_buffer(&tr->trace_buffer, NULL);
 	if (ret)
 		goto out;
 
-	ret = trace_test_buffer(&max_tr, &count);
+	ret = trace_test_buffer(&tr->max_buffer, &count);
 	if (ret)
 		goto out;
 
@@ -973,11 +974,11 @@ trace_selftest_startup_preemptirqsoff(struct tracer *trace, struct trace_array *
 	/* stop the tracing. */
 	tracing_stop();
 	/* check both trace buffers */
-	ret = trace_test_buffer(tr, NULL);
+	ret = trace_test_buffer(&tr->trace_buffer, NULL);
 	if (ret)
 		goto out;
 
-	ret = trace_test_buffer(&max_tr, &count);
+	ret = trace_test_buffer(&tr->max_buffer, &count);
 
 	if (!ret && !count) {
 		printk(KERN_CONT ".. no entries found ..");
@@ -1084,10 +1085,10 @@ trace_selftest_startup_wakeup(struct tracer *trace, struct trace_array *tr)
 	/* stop the tracing. */
 	tracing_stop();
 	/* check both trace buffers */
-	ret = trace_test_buffer(tr, NULL);
+	ret = trace_test_buffer(&tr->trace_buffer, NULL);
 	printk("ret = %d\n", ret);
 	if (!ret)
-		ret = trace_test_buffer(&max_tr, &count);
+		ret = trace_test_buffer(&tr->max_buffer, &count);
 
 
 	trace->reset(tr);
@@ -1126,7 +1127,7 @@ trace_selftest_startup_sched_switch(struct tracer *trace, struct trace_array *tr
 	/* stop the tracing. */
 	tracing_stop();
 	/* check the trace buffer */
-	ret = trace_test_buffer(tr, &count);
+	ret = trace_test_buffer(&tr->trace_buffer, &count);
 	trace->reset(tr);
 	tracing_start();
 
diff --git a/kernel/trace/trace_stack.c b/kernel/trace/trace_stack.c
index 42ca822fc70..b20428c5efe 100644
--- a/kernel/trace/trace_stack.c
+++ b/kernel/trace/trace_stack.c
@@ -20,13 +20,24 @@
 
 #define STACK_TRACE_ENTRIES 500
 
+#ifdef CC_USING_FENTRY
+# define fentry		1
+#else
+# define fentry		0
+#endif
+
 static unsigned long stack_dump_trace[STACK_TRACE_ENTRIES+1] =
 	 { [0 ... (STACK_TRACE_ENTRIES)] = ULONG_MAX };
 static unsigned stack_dump_index[STACK_TRACE_ENTRIES];
 
+/*
+ * Reserve one entry for the passed in ip. This will allow
+ * us to remove most or all of the stack size overhead
+ * added by the stack tracer itself.
+ */
 static struct stack_trace max_stack_trace = {
-	.max_entries		= STACK_TRACE_ENTRIES,
-	.entries		= stack_dump_trace,
+	.max_entries		= STACK_TRACE_ENTRIES - 1,
+	.entries		= &stack_dump_trace[1],
 };
 
 static unsigned long max_stack_size;
@@ -39,25 +50,34 @@ static DEFINE_MUTEX(stack_sysctl_mutex);
 int stack_tracer_enabled;
 static int last_stack_tracer_enabled;
 
-static inline void check_stack(void)
+static inline void
+check_stack(unsigned long ip, unsigned long *stack)
 {
 	unsigned long this_size, flags;
 	unsigned long *p, *top, *start;
+	static int tracer_frame;
+	int frame_size = ACCESS_ONCE(tracer_frame);
 	int i;
 
-	this_size = ((unsigned long)&this_size) & (THREAD_SIZE-1);
+	this_size = ((unsigned long)stack) & (THREAD_SIZE-1);
 	this_size = THREAD_SIZE - this_size;
+	/* Remove the frame of the tracer */
+	this_size -= frame_size;
 
 	if (this_size <= max_stack_size)
 		return;
 
 	/* we do not handle interrupt stacks yet */
-	if (!object_is_on_stack(&this_size))
+	if (!object_is_on_stack(stack))
 		return;
 
 	local_irq_save(flags);
 	arch_spin_lock(&max_stack_lock);
 
+	/* In case another CPU set the tracer_frame on us */
+	if (unlikely(!frame_size))
+		this_size -= tracer_frame;
+
 	/* a race could have already updated it */
 	if (this_size <= max_stack_size)
 		goto out;
@@ -70,10 +90,18 @@ static inline void check_stack(void)
 	save_stack_trace(&max_stack_trace);
 
 	/*
+	 * Add the passed in ip from the function tracer.
+	 * Searching for this on the stack will skip over
+	 * most of the overhead from the stack tracer itself.
+	 */
+	stack_dump_trace[0] = ip;
+	max_stack_trace.nr_entries++;
+
+	/*
 	 * Now find where in the stack these are.
 	 */
 	i = 0;
-	start = &this_size;
+	start = stack;
 	top = (unsigned long *)
 		(((unsigned long)start & ~(THREAD_SIZE-1)) + THREAD_SIZE);
 
@@ -97,6 +125,18 @@ static inline void check_stack(void)
 				found = 1;
 				/* Start the search from here */
 				start = p + 1;
+				/*
+				 * We do not want to show the overhead
+				 * of the stack tracer stack in the
+				 * max stack. If we haven't figured
+				 * out what that is, then figure it out
+				 * now.
+				 */
+				if (unlikely(!tracer_frame) && i == 1) {
+					tracer_frame = (p - stack) *
+						sizeof(unsigned long);
+					max_stack_size -= tracer_frame;
+				}
 			}
 		}
 
@@ -113,6 +153,7 @@ static void
 stack_trace_call(unsigned long ip, unsigned long parent_ip,
 		 struct ftrace_ops *op, struct pt_regs *pt_regs)
 {
+	unsigned long stack;
 	int cpu;
 
 	preempt_disable_notrace();
@@ -122,7 +163,26 @@ stack_trace_call(unsigned long ip, unsigned long parent_ip,
 	if (per_cpu(trace_active, cpu)++ != 0)
 		goto out;
 
-	check_stack();
+	/*
+	 * When fentry is used, the traced function does not get
+	 * its stack frame set up, and we lose the parent.
+	 * The ip is pretty useless because the function tracer
+	 * was called before that function set up its stack frame.
+	 * In this case, we use the parent ip.
+	 *
+	 * By adding the return address of either the parent ip
+	 * or the current ip we can disregard most of the stack usage
+	 * caused by the stack tracer itself.
+	 *
+	 * The function tracer always reports the address of where the
+	 * mcount call was, but the stack will hold the return address.
+	 */
+	if (fentry)
+		ip = parent_ip;
+	else
+		ip += MCOUNT_INSN_SIZE;
+
+	check_stack(ip, &stack);
 
  out:
 	per_cpu(trace_active, cpu)--;
@@ -322,7 +382,7 @@ static const struct file_operations stack_trace_filter_fops = {
 	.open = stack_trace_filter_open,
 	.read = seq_read,
 	.write = ftrace_filter_write,
-	.llseek = ftrace_regex_lseek,
+	.llseek = ftrace_filter_lseek,
 	.release = ftrace_regex_release,
 };
 
@@ -371,6 +431,8 @@ static __init int stack_trace_init(void)
 	struct dentry *d_tracer;
 
 	d_tracer = tracing_init_dentry();
+	if (!d_tracer)
+		return 0;
 
 	trace_create_file("stack_max_size", 0644, d_tracer,
 			&max_stack_size, &stack_max_size_fops);
diff --git a/kernel/trace/trace_stat.c b/kernel/trace/trace_stat.c
index 96cffb269e7..847f88a6194 100644
--- a/kernel/trace/trace_stat.c
+++ b/kernel/trace/trace_stat.c
@@ -307,6 +307,8 @@ static int tracing_stat_init(void)
 	struct dentry *d_tracing;
 
 	d_tracing = tracing_init_dentry();
+	if (!d_tracing)
+		return 0;
 
 	stat_dir = debugfs_create_dir("trace_stat", d_tracing);
 	if (!stat_dir)
diff --git a/kernel/trace/trace_syscalls.c b/kernel/trace/trace_syscalls.c
index 7a809e32105..8f2ac73c7a5 100644
--- a/kernel/trace/trace_syscalls.c
+++ b/kernel/trace/trace_syscalls.c
@@ -12,10 +12,6 @@
 #include "trace.h"
 
 static DEFINE_MUTEX(syscall_trace_lock);
-static int sys_refcount_enter;
-static int sys_refcount_exit;
-static DECLARE_BITMAP(enabled_enter_syscalls, NR_syscalls);
-static DECLARE_BITMAP(enabled_exit_syscalls, NR_syscalls);
 
 static int syscall_enter_register(struct ftrace_event_call *event,
 				 enum trace_reg type, void *data);
@@ -41,7 +37,7 @@ static inline bool arch_syscall_match_sym_name(const char *sym, const char *name
 	/*
 	 * Only compare after the "sys" prefix. Archs that use
 	 * syscall wrappers may have syscalls symbols aliases prefixed
-	 * with "SyS" instead of "sys", leading to an unwanted
+	 * with ".SyS" or ".sys" instead of "sys", leading to an unwanted
 	 * mismatch.
 	 */
 	return !strcmp(sym + 3, name + 3);
@@ -265,7 +261,7 @@ static void free_syscall_print_fmt(struct ftrace_event_call *call)
 		kfree(call->print_fmt);
 }
 
-static int syscall_enter_define_fields(struct ftrace_event_call *call)
+static int __init syscall_enter_define_fields(struct ftrace_event_call *call)
 {
 	struct syscall_trace_enter trace;
 	struct syscall_metadata *meta = call->data;
@@ -288,7 +284,7 @@ static int syscall_enter_define_fields(struct ftrace_event_call *call)
 	return ret;
 }
 
-static int syscall_exit_define_fields(struct ftrace_event_call *call)
+static int __init syscall_exit_define_fields(struct ftrace_event_call *call)
 {
 	struct syscall_trace_exit trace;
 	int ret;
@@ -303,8 +299,9 @@ static int syscall_exit_define_fields(struct ftrace_event_call *call)
 	return ret;
 }
 
-static void ftrace_syscall_enter(void *ignore, struct pt_regs *regs, long id)
+static void ftrace_syscall_enter(void *data, struct pt_regs *regs, long id)
 {
+	struct trace_array *tr = data;
 	struct syscall_trace_enter *entry;
 	struct syscall_metadata *sys_data;
 	struct ring_buffer_event *event;
@@ -315,7 +312,7 @@ static void ftrace_syscall_enter(void *ignore, struct pt_regs *regs, long id)
 	syscall_nr = trace_get_syscall_nr(current, regs);
 	if (syscall_nr < 0)
 		return;
-	if (!test_bit(syscall_nr, enabled_enter_syscalls))
+	if (!test_bit(syscall_nr, tr->enabled_enter_syscalls))
 		return;
 
 	sys_data = syscall_nr_to_meta(syscall_nr);
@@ -324,7 +321,8 @@ static void ftrace_syscall_enter(void *ignore, struct pt_regs *regs, long id)
 
 	size = sizeof(*entry) + sizeof(unsigned long) * sys_data->nb_args;
 
-	event = trace_current_buffer_lock_reserve(&buffer,
+	buffer = tr->trace_buffer.buffer;
+	event = trace_buffer_lock_reserve(buffer,
 			sys_data->enter_event->event.type, size, 0, 0);
 	if (!event)
 		return;
@@ -338,8 +336,9 @@ static void ftrace_syscall_enter(void *ignore, struct pt_regs *regs, long id)
 		trace_current_buffer_unlock_commit(buffer, event, 0, 0);
 }
 
-static void ftrace_syscall_exit(void *ignore, struct pt_regs *regs, long ret)
+static void ftrace_syscall_exit(void *data, struct pt_regs *regs, long ret)
 {
+	struct trace_array *tr = data;
 	struct syscall_trace_exit *entry;
 	struct syscall_metadata *sys_data;
 	struct ring_buffer_event *event;
@@ -349,14 +348,15 @@ static void ftrace_syscall_exit(void *ignore, struct pt_regs *regs, long ret)
 	syscall_nr = trace_get_syscall_nr(current, regs);
 	if (syscall_nr < 0)
 		return;
-	if (!test_bit(syscall_nr, enabled_exit_syscalls))
+	if (!test_bit(syscall_nr, tr->enabled_exit_syscalls))
 		return;
 
 	sys_data = syscall_nr_to_meta(syscall_nr);
 	if (!sys_data)
 		return;
 
-	event = trace_current_buffer_lock_reserve(&buffer,
+	buffer = tr->trace_buffer.buffer;
+	event = trace_buffer_lock_reserve(buffer,
 			sys_data->exit_event->event.type, sizeof(*entry), 0, 0);
 	if (!event)
 		return;
@@ -370,8 +370,10 @@ static void ftrace_syscall_exit(void *ignore, struct pt_regs *regs, long ret)
 		trace_current_buffer_unlock_commit(buffer, event, 0, 0);
 }
 
-static int reg_event_syscall_enter(struct ftrace_event_call *call)
+static int reg_event_syscall_enter(struct ftrace_event_file *file,
+				   struct ftrace_event_call *call)
 {
+	struct trace_array *tr = file->tr;
 	int ret = 0;
 	int num;
 
@@ -379,33 +381,37 @@ static int reg_event_syscall_enter(struct ftrace_event_call *call)
 	if (WARN_ON_ONCE(num < 0 || num >= NR_syscalls))
 		return -ENOSYS;
 	mutex_lock(&syscall_trace_lock);
-	if (!sys_refcount_enter)
-		ret = register_trace_sys_enter(ftrace_syscall_enter, NULL);
+	if (!tr->sys_refcount_enter)
+		ret = register_trace_sys_enter(ftrace_syscall_enter, tr);
 	if (!ret) {
-		set_bit(num, enabled_enter_syscalls);
-		sys_refcount_enter++;
+		set_bit(num, tr->enabled_enter_syscalls);
+		tr->sys_refcount_enter++;
 	}
 	mutex_unlock(&syscall_trace_lock);
 	return ret;
 }
 
-static void unreg_event_syscall_enter(struct ftrace_event_call *call)
+static void unreg_event_syscall_enter(struct ftrace_event_file *file,
+				      struct ftrace_event_call *call)
 {
+	struct trace_array *tr = file->tr;
 	int num;
 
 	num = ((struct syscall_metadata *)call->data)->syscall_nr;
 	if (WARN_ON_ONCE(num < 0 || num >= NR_syscalls))
 		return;
 	mutex_lock(&syscall_trace_lock);
-	sys_refcount_enter--;
-	clear_bit(num, enabled_enter_syscalls);
-	if (!sys_refcount_enter)
-		unregister_trace_sys_enter(ftrace_syscall_enter, NULL);
+	tr->sys_refcount_enter--;
+	clear_bit(num, tr->enabled_enter_syscalls);
+	if (!tr->sys_refcount_enter)
+		unregister_trace_sys_enter(ftrace_syscall_enter, tr);
 	mutex_unlock(&syscall_trace_lock);
 }
 
-static int reg_event_syscall_exit(struct ftrace_event_call *call)
+static int reg_event_syscall_exit(struct ftrace_event_file *file,
+				  struct ftrace_event_call *call)
 {
+	struct trace_array *tr = file->tr;
 	int ret = 0;
 	int num;
 
@@ -413,28 +419,30 @@ static int reg_event_syscall_exit(struct ftrace_event_call *call)
 	if (WARN_ON_ONCE(num < 0 || num >= NR_syscalls))
 		return -ENOSYS;
 	mutex_lock(&syscall_trace_lock);
-	if (!sys_refcount_exit)
-		ret = register_trace_sys_exit(ftrace_syscall_exit, NULL);
+	if (!tr->sys_refcount_exit)
+		ret = register_trace_sys_exit(ftrace_syscall_exit, tr);
 	if (!ret) {
-		set_bit(num, enabled_exit_syscalls);
-		sys_refcount_exit++;
+		set_bit(num, tr->enabled_exit_syscalls);
+		tr->sys_refcount_exit++;
 	}
 	mutex_unlock(&syscall_trace_lock);
 	return ret;
 }
 
-static void unreg_event_syscall_exit(struct ftrace_event_call *call)
+static void unreg_event_syscall_exit(struct ftrace_event_file *file,
+				     struct ftrace_event_call *call)
 {
+	struct trace_array *tr = file->tr;
 	int num;
 
 	num = ((struct syscall_metadata *)call->data)->syscall_nr;
 	if (WARN_ON_ONCE(num < 0 || num >= NR_syscalls))
 		return;
 	mutex_lock(&syscall_trace_lock);
-	sys_refcount_exit--;
-	clear_bit(num, enabled_exit_syscalls);
-	if (!sys_refcount_exit)
-		unregister_trace_sys_exit(ftrace_syscall_exit, NULL);
+	tr->sys_refcount_exit--;
+	clear_bit(num, tr->enabled_exit_syscalls);
+	if (!tr->sys_refcount_exit)
+		unregister_trace_sys_exit(ftrace_syscall_exit, tr);
 	mutex_unlock(&syscall_trace_lock);
 }
 
@@ -471,7 +479,7 @@ struct trace_event_functions exit_syscall_print_funcs = {
 	.trace		= print_syscall_exit,
 };
 
-struct ftrace_event_class event_class_syscall_enter = {
+struct ftrace_event_class __refdata event_class_syscall_enter = {
 	.system		= "syscalls",
 	.reg		= syscall_enter_register,
 	.define_fields	= syscall_enter_define_fields,
@@ -479,7 +487,7 @@ struct ftrace_event_class event_class_syscall_enter = {
 	.raw_init	= init_syscall_trace,
 };
 
-struct ftrace_event_class event_class_syscall_exit = {
+struct ftrace_event_class __refdata event_class_syscall_exit = {
 	.system		= "syscalls",
 	.reg		= syscall_exit_register,
 	.define_fields	= syscall_exit_define_fields,
@@ -685,11 +693,13 @@ static void perf_sysexit_disable(struct ftrace_event_call *call)
 static int syscall_enter_register(struct ftrace_event_call *event,
 				 enum trace_reg type, void *data)
 {
+	struct ftrace_event_file *file = data;
+
 	switch (type) {
 	case TRACE_REG_REGISTER:
-		return reg_event_syscall_enter(event);
+		return reg_event_syscall_enter(file, event);
 	case TRACE_REG_UNREGISTER:
-		unreg_event_syscall_enter(event);
+		unreg_event_syscall_enter(file, event);
 		return 0;
 
 #ifdef CONFIG_PERF_EVENTS
@@ -711,11 +721,13 @@ static int syscall_enter_register(struct ftrace_event_call *event,
 static int syscall_exit_register(struct ftrace_event_call *event,
 				 enum trace_reg type, void *data)
 {
+	struct ftrace_event_file *file = data;
+
 	switch (type) {
 	case TRACE_REG_REGISTER:
-		return reg_event_syscall_exit(event);
+		return reg_event_syscall_exit(file, event);
 	case TRACE_REG_UNREGISTER:
-		unreg_event_syscall_exit(event);
+		unreg_event_syscall_exit(file, event);
 		return 0;
 
 #ifdef CONFIG_PERF_EVENTS
diff --git a/kernel/trace/trace_uprobe.c b/kernel/trace/trace_uprobe.c
index 8dad2a92dee..32494fb0ee6 100644
--- a/kernel/trace/trace_uprobe.c
+++ b/kernel/trace/trace_uprobe.c
@@ -28,6 +28,18 @@
 
 #define UPROBE_EVENT_SYSTEM	"uprobes"
 
+struct uprobe_trace_entry_head {
+	struct trace_entry	ent;
+	unsigned long		vaddr[];
+};
+
+#define SIZEOF_TRACE_ENTRY(is_return)			\
+	(sizeof(struct uprobe_trace_entry_head) +	\
+	 sizeof(unsigned long) * (is_return ? 2 : 1))
+
+#define DATAOF_TRACE_ENTRY(entry, is_return)		\
+	((void*)(entry) + SIZEOF_TRACE_ENTRY(is_return))
+
 struct trace_uprobe_filter {
 	rwlock_t		rwlock;
 	int			nr_systemwide;
@@ -64,6 +76,8 @@ static DEFINE_MUTEX(uprobe_lock);
 static LIST_HEAD(uprobe_list);
 
 static int uprobe_dispatcher(struct uprobe_consumer *con, struct pt_regs *regs);
+static int uretprobe_dispatcher(struct uprobe_consumer *con,
+				unsigned long func, struct pt_regs *regs);
 
 static inline void init_trace_uprobe_filter(struct trace_uprobe_filter *filter)
 {
@@ -77,11 +91,16 @@ static inline bool uprobe_filter_is_empty(struct trace_uprobe_filter *filter)
 	return !filter->nr_systemwide && list_empty(&filter->perf_events);
 }
 
+static inline bool is_ret_probe(struct trace_uprobe *tu)
+{
+	return tu->consumer.ret_handler != NULL;
+}
+
 /*
  * Allocate new trace_uprobe and initialize it (including uprobes).
  */
 static struct trace_uprobe *
-alloc_trace_uprobe(const char *group, const char *event, int nargs)
+alloc_trace_uprobe(const char *group, const char *event, int nargs, bool is_ret)
 {
 	struct trace_uprobe *tu;
 
@@ -106,6 +125,8 @@ alloc_trace_uprobe(const char *group, const char *event, int nargs)
 
 	INIT_LIST_HEAD(&tu->list);
 	tu->consumer.handler = uprobe_dispatcher;
+	if (is_ret)
+		tu->consumer.ret_handler = uretprobe_dispatcher;
 	init_trace_uprobe_filter(&tu->filter);
 	return tu;
 
@@ -180,7 +201,7 @@ end:
 
 /*
  * Argument syntax:
- *  - Add uprobe: p[:[GRP/]EVENT] PATH:SYMBOL[+offs] [FETCHARGS]
+ *  - Add uprobe: p|r[:[GRP/]EVENT] PATH:SYMBOL [FETCHARGS]
  *
  *  - Remove uprobe: -:[GRP/]EVENT
  */
@@ -192,20 +213,23 @@ static int create_trace_uprobe(int argc, char **argv)
 	char buf[MAX_EVENT_NAME_LEN];
 	struct path path;
 	unsigned long offset;
-	bool is_delete;
+	bool is_delete, is_return;
 	int i, ret;
 
 	inode = NULL;
 	ret = 0;
 	is_delete = false;
+	is_return = false;
 	event = NULL;
 	group = NULL;
 
 	/* argc must be >= 1 */
 	if (argv[0][0] == '-')
 		is_delete = true;
+	else if (argv[0][0] == 'r')
+		is_return = true;
 	else if (argv[0][0] != 'p') {
-		pr_info("Probe definition must be started with 'p' or '-'.\n");
+		pr_info("Probe definition must be started with 'p', 'r' or '-'.\n");
 		return -EINVAL;
 	}
 
@@ -303,7 +327,7 @@ static int create_trace_uprobe(int argc, char **argv)
 		kfree(tail);
 	}
 
-	tu = alloc_trace_uprobe(group, event, argc);
+	tu = alloc_trace_uprobe(group, event, argc, is_return);
 	if (IS_ERR(tu)) {
 		pr_info("Failed to allocate trace_uprobe.(%d)\n", (int)PTR_ERR(tu));
 		ret = PTR_ERR(tu);
@@ -414,9 +438,10 @@ static void probes_seq_stop(struct seq_file *m, void *v)
 static int probes_seq_show(struct seq_file *m, void *v)
 {
 	struct trace_uprobe *tu = v;
+	char c = is_ret_probe(tu) ? 'r' : 'p';
 	int i;
 
-	seq_printf(m, "p:%s/%s", tu->call.class->system, tu->call.name);
+	seq_printf(m, "%c:%s/%s", c, 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++)
@@ -485,65 +510,81 @@ static const struct file_operations uprobe_profile_ops = {
 	.release	= seq_release,
 };
 
-/* uprobe handler */
-static int uprobe_trace_func(struct trace_uprobe *tu, struct pt_regs *regs)
+static void uprobe_trace_print(struct trace_uprobe *tu,
+				unsigned long func, 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;
+	void *data;
+	int size, i;
 	struct ftrace_event_call *call = &tu->call;
 
-	local_save_flags(irq_flags);
-	pc = preempt_count();
-
-	size = sizeof(*entry) + tu->size;
-
+	size = SIZEOF_TRACE_ENTRY(is_ret_probe(tu));
 	event = trace_current_buffer_lock_reserve(&buffer, call->event.type,
-						  size, irq_flags, pc);
+						  size + tu->size, 0, 0);
 	if (!event)
-		return 0;
+		return;
 
 	entry = ring_buffer_event_data(event);
-	entry->ip = instruction_pointer(task_pt_regs(current));
-	data = (u8 *)&entry[1];
+	if (is_ret_probe(tu)) {
+		entry->vaddr[0] = func;
+		entry->vaddr[1] = instruction_pointer(regs);
+		data = DATAOF_TRACE_ENTRY(entry, true);
+	} else {
+		entry->vaddr[0] = instruction_pointer(regs);
+		data = DATAOF_TRACE_ENTRY(entry, false);
+	}
+
 	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);
+		trace_buffer_unlock_commit(buffer, event, 0, 0);
+}
 
+/* uprobe handler */
+static int uprobe_trace_func(struct trace_uprobe *tu, struct pt_regs *regs)
+{
+	if (!is_ret_probe(tu))
+		uprobe_trace_print(tu, 0, regs);
 	return 0;
 }
 
+static void uretprobe_trace_func(struct trace_uprobe *tu, unsigned long func,
+				struct pt_regs *regs)
+{
+	uprobe_trace_print(tu, func, regs);
+}
+
 /* 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 uprobe_trace_entry_head *entry;
 	struct trace_seq *s = &iter->seq;
 	struct trace_uprobe *tu;
 	u8 *data;
 	int i;
 
-	field = (struct uprobe_trace_entry_head *)iter->ent;
+	entry = (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;
+	if (is_ret_probe(tu)) {
+		if (!trace_seq_printf(s, "%s: (0x%lx <- 0x%lx)", tu->call.name,
+					entry->vaddr[1], entry->vaddr[0]))
+			goto partial;
+		data = DATAOF_TRACE_ENTRY(entry, true);
+	} else {
+		if (!trace_seq_printf(s, "%s: (0x%lx)", tu->call.name,
+					entry->vaddr[0]))
+			goto partial;
+		data = DATAOF_TRACE_ENTRY(entry, false);
+	}
 
-	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))
+					     data + tu->args[i].offset, entry))
 			goto partial;
 	}
 
@@ -595,16 +636,23 @@ static void probe_event_disable(struct trace_uprobe *tu, int flag)
 
 static int uprobe_event_define_fields(struct ftrace_event_call *event_call)
 {
-	int ret, i;
+	int ret, i, size;
 	struct uprobe_trace_entry_head field;
-	struct trace_uprobe *tu = (struct trace_uprobe *)event_call->data;
+	struct trace_uprobe *tu = event_call->data;
 
-	DEFINE_FIELD(unsigned long, ip, FIELD_STRING_IP, 0);
+	if (is_ret_probe(tu)) {
+		DEFINE_FIELD(unsigned long, vaddr[0], FIELD_STRING_FUNC, 0);
+		DEFINE_FIELD(unsigned long, vaddr[1], FIELD_STRING_RETIP, 0);
+		size = SIZEOF_TRACE_ENTRY(true);
+	} else {
+		DEFINE_FIELD(unsigned long, vaddr[0], FIELD_STRING_IP, 0);
+		size = SIZEOF_TRACE_ENTRY(false);
+	}
 	/* 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,
+					 size + tu->args[i].offset,
 					 tu->args[i].type->size,
 					 tu->args[i].type->is_signed,
 					 FILTER_OTHER);
@@ -622,8 +670,13 @@ static int __set_print_fmt(struct trace_uprobe *tu, char *buf, int len)
 	int i;
 	int pos = 0;
 
-	fmt = "(%lx)";
-	arg = "REC->" FIELD_STRING_IP;
+	if (is_ret_probe(tu)) {
+		fmt = "(%lx <- %lx)";
+		arg = "REC->" FIELD_STRING_FUNC ", REC->" FIELD_STRING_RETIP;
+	} else {
+		fmt = "(%lx)";
+		arg = "REC->" FIELD_STRING_IP;
+	}
 
 	/* When len=0, we just calculate the needed length */
 
@@ -752,49 +805,68 @@ static bool uprobe_perf_filter(struct uprobe_consumer *uc,
 	return ret;
 }
 
-/* uprobe profile handler */
-static int uprobe_perf_func(struct trace_uprobe *tu, struct pt_regs *regs)
+static void uprobe_perf_print(struct trace_uprobe *tu,
+				unsigned long func, 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;
+	void *data;
+	int size, rctx, i;
 
-	if (!uprobe_perf_filter(&tu->consumer, 0, current->mm))
-		return UPROBE_HANDLER_REMOVE;
-
-	__size = sizeof(*entry) + tu->size;
-	size = ALIGN(__size + sizeof(u32), sizeof(u64));
-	size -= sizeof(u32);
+	size = SIZEOF_TRACE_ENTRY(is_ret_probe(tu));
+	size = ALIGN(size + tu->size + sizeof(u32), sizeof(u64)) - sizeof(u32);
 	if (WARN_ONCE(size > PERF_MAX_TRACE_SIZE, "profile buffer not large enough"))
-		return 0;
+		return;
 
 	preempt_disable();
+	head = this_cpu_ptr(call->perf_events);
+	if (hlist_empty(head))
+		goto out;
 
 	entry = perf_trace_buf_prepare(size, call->event.type, regs, &rctx);
 	if (!entry)
 		goto out;
 
-	entry->ip = instruction_pointer(task_pt_regs(current));
-	data = (u8 *)&entry[1];
+	if (is_ret_probe(tu)) {
+		entry->vaddr[0] = func;
+		entry->vaddr[1] = instruction_pointer(regs);
+		data = DATAOF_TRACE_ENTRY(entry, true);
+	} else {
+		entry->vaddr[0] = instruction_pointer(regs);
+		data = DATAOF_TRACE_ENTRY(entry, false);
+	}
+
 	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, NULL);
-
+	perf_trace_buf_submit(entry, size, rctx, 0, 1, regs, head, NULL);
  out:
 	preempt_enable();
+}
+
+/* uprobe profile handler */
+static int uprobe_perf_func(struct trace_uprobe *tu, struct pt_regs *regs)
+{
+	if (!uprobe_perf_filter(&tu->consumer, 0, current->mm))
+		return UPROBE_HANDLER_REMOVE;
+
+	if (!is_ret_probe(tu))
+		uprobe_perf_print(tu, 0, regs);
 	return 0;
 }
+
+static void uretprobe_perf_func(struct trace_uprobe *tu, unsigned long func,
+				struct pt_regs *regs)
+{
+	uprobe_perf_print(tu, func, regs);
+}
 #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;
+	struct trace_uprobe *tu = event->data;
 
 	switch (type) {
 	case TRACE_REG_REGISTER:
@@ -843,6 +915,23 @@ static int uprobe_dispatcher(struct uprobe_consumer *con, struct pt_regs *regs)
 	return ret;
 }
 
+static int uretprobe_dispatcher(struct uprobe_consumer *con,
+				unsigned long func, struct pt_regs *regs)
+{
+	struct trace_uprobe *tu;
+
+	tu = container_of(con, struct trace_uprobe, consumer);
+
+	if (tu->flags & TP_FLAG_TRACE)
+		uretprobe_trace_func(tu, func, regs);
+
+#ifdef CONFIG_PERF_EVENTS
+	if (tu->flags & TP_FLAG_PROFILE)
+		uretprobe_perf_func(tu, func, regs);
+#endif
+	return 0;
+}
+
 static struct trace_event_functions uprobe_funcs = {
 	.trace		= print_uprobe_event
 };
diff --git a/kernel/tracepoint.c b/kernel/tracepoint.c
index 0c05a459204..29f26540e9c 100644
--- a/kernel/tracepoint.c
+++ b/kernel/tracepoint.c
@@ -112,7 +112,8 @@ tracepoint_entry_add_probe(struct tracepoint_entry *entry,
 	int nr_probes = 0;
 	struct tracepoint_func *old, *new;
 
-	WARN_ON(!probe);
+	if (WARN_ON(!probe))
+		return ERR_PTR(-EINVAL);
 
 	debug_print_probes(entry);
 	old = entry->funcs;
@@ -152,13 +153,18 @@ tracepoint_entry_remove_probe(struct tracepoint_entry *entry,
 
 	debug_print_probes(entry);
 	/* (N -> M), (N > 1, M >= 0) probes */
-	for (nr_probes = 0; old[nr_probes].func; nr_probes++) {
-		if (!probe ||
-		    (old[nr_probes].func == probe &&
-		     old[nr_probes].data == data))
-			nr_del++;
+	if (probe) {
+		for (nr_probes = 0; old[nr_probes].func; nr_probes++) {
+			if (old[nr_probes].func == probe &&
+			     old[nr_probes].data == data)
+				nr_del++;
+		}
 	}
 
+	/*
+	 * If probe is NULL, then nr_probes = nr_del = 0, and then the
+	 * entire entry will be removed.
+	 */
 	if (nr_probes - nr_del == 0) {
 		/* N -> 0, (N > 1) */
 		entry->funcs = NULL;
@@ -173,8 +179,7 @@ tracepoint_entry_remove_probe(struct tracepoint_entry *entry,
 		if (new == NULL)
 			return ERR_PTR(-ENOMEM);
 		for (i = 0; old[i].func; i++)
-			if (probe &&
-			    (old[i].func != probe || old[i].data != data))
+			if (old[i].func != probe || old[i].data != data)
 				new[j++] = old[i];
 		new[nr_probes - nr_del].func = NULL;
 		entry->refcount = nr_probes - nr_del;
diff --git a/kernel/user.c b/kernel/user.c
index e81978e8c03..8e635a18ab5 100644
--- a/kernel/user.c
+++ b/kernel/user.c
@@ -51,6 +51,8 @@ struct user_namespace init_user_ns = {
 	.owner = GLOBAL_ROOT_UID,
 	.group = GLOBAL_ROOT_GID,
 	.proc_inum = PROC_USER_INIT_INO,
+	.may_mount_sysfs = true,
+	.may_mount_proc = true,
 };
 EXPORT_SYMBOL_GPL(init_user_ns);
 
diff --git a/kernel/user_namespace.c b/kernel/user_namespace.c
index b14f4d34204..e134d8f365d 100644
--- a/kernel/user_namespace.c
+++ b/kernel/user_namespace.c
@@ -25,7 +25,8 @@
 
 static struct kmem_cache *user_ns_cachep __read_mostly;
 
-static bool new_idmap_permitted(struct user_namespace *ns, int cap_setid,
+static bool new_idmap_permitted(const struct file *file,
+				struct user_namespace *ns, int cap_setid,
 				struct uid_gid_map *map);
 
 static void set_cred_user_ns(struct cred *cred, struct user_namespace *user_ns)
@@ -61,6 +62,15 @@ int create_user_ns(struct cred *new)
 	kgid_t group = new->egid;
 	int ret;
 
+	/*
+	 * Verify that we can not violate the policy of which files
+	 * may be accessed that is specified by the root directory,
+	 * by verifing that the root directory is at the root of the
+	 * mount namespace which allows all files to be accessed.
+	 */
+	if (current_chrooted())
+		return -EPERM;
+
 	/* The creator needs a mapping in the parent user namespace
 	 * or else we won't be able to reasonably tell userspace who
 	 * created a user_namespace.
@@ -87,6 +97,8 @@ int create_user_ns(struct cred *new)
 
 	set_cred_user_ns(new, ns);
 
+	update_mnt_policy(ns);
+
 	return 0;
 }
 
@@ -601,10 +613,10 @@ static ssize_t map_write(struct file *file, const char __user *buf,
 	if (map->nr_extents != 0)
 		goto out;
 
-	/* Require the appropriate privilege CAP_SETUID or CAP_SETGID
-	 * over the user namespace in order to set the id mapping.
+	/*
+	 * Adjusting namespace settings requires capabilities on the target.
 	 */
-	if (cap_valid(cap_setid) && !ns_capable(ns, cap_setid))
+	if (cap_valid(cap_setid) && !file_ns_capable(file, ns, CAP_SYS_ADMIN))
 		goto out;
 
 	/* Get a buffer */
@@ -689,7 +701,7 @@ static ssize_t map_write(struct file *file, const char __user *buf,
 
 	ret = -EPERM;
 	/* Validate the user is allowed to use user id's mapped to. */
-	if (!new_idmap_permitted(ns, cap_setid, &new_map))
+	if (!new_idmap_permitted(file, ns, cap_setid, &new_map))
 		goto out;
 
 	/* Map the lower ids from the parent user namespace to the
@@ -776,7 +788,8 @@ ssize_t proc_projid_map_write(struct file *file, const char __user *buf, size_t
 			 &ns->projid_map, &ns->parent->projid_map);
 }
 
-static bool new_idmap_permitted(struct user_namespace *ns, int cap_setid,
+static bool new_idmap_permitted(const struct file *file, 
+				struct user_namespace *ns, int cap_setid,
 				struct uid_gid_map *new_map)
 {
 	/* Allow mapping to your own filesystem ids */
@@ -784,12 +797,12 @@ static bool new_idmap_permitted(struct user_namespace *ns, int cap_setid,
 		u32 id = new_map->extent[0].lower_first;
 		if (cap_setid == CAP_SETUID) {
 			kuid_t uid = make_kuid(ns->parent, id);
-			if (uid_eq(uid, current_fsuid()))
+			if (uid_eq(uid, file->f_cred->fsuid))
 				return true;
 		}
 		else if (cap_setid == CAP_SETGID) {
 			kgid_t gid = make_kgid(ns->parent, id);
-			if (gid_eq(gid, current_fsgid()))
+			if (gid_eq(gid, file->f_cred->fsgid))
 				return true;
 		}
 	}
@@ -800,8 +813,10 @@ static bool new_idmap_permitted(struct user_namespace *ns, int cap_setid,
 
 	/* Allow the specified ids if we have the appropriate capability
 	 * (CAP_SETUID or CAP_SETGID) over the parent user namespace.
+	 * And the opener of the id file also had the approprpiate capability.
 	 */
-	if (ns_capable(ns->parent, cap_setid))
+	if (ns_capable(ns->parent, cap_setid) &&
+	    file_ns_capable(file, ns->parent, cap_setid))
 		return true;
 
 	return false;
diff --git a/kernel/watchdog.c b/kernel/watchdog.c
index 4a944676358..05039e348f0 100644
--- a/kernel/watchdog.c
+++ b/kernel/watchdog.c
@@ -517,6 +517,11 @@ int proc_dowatchdog(struct ctl_table *table, int write,
 		return ret;
 
 	set_sample_period();
+	/*
+	 * Watchdog threads shouldn't be enabled if they are
+	 * disabled. The 'watchdog_disabled' variable check in
+	 * watchdog_*_all_cpus() function takes care of this.
+	 */
 	if (watchdog_enabled && watchdog_thresh)
 		watchdog_enable_all_cpus();
 	else
diff --git a/kernel/workqueue.c b/kernel/workqueue.c
index b48cd597145..154aa12af48 100644
--- a/kernel/workqueue.c
+++ b/kernel/workqueue.c
@@ -41,7 +41,11 @@
 #include <linux/debug_locks.h>
 #include <linux/lockdep.h>
 #include <linux/idr.h>
+#include <linux/jhash.h>
 #include <linux/hashtable.h>
+#include <linux/rculist.h>
+#include <linux/nodemask.h>
+#include <linux/moduleparam.h>
 
 #include "workqueue_internal.h"
 
@@ -58,12 +62,11 @@ enum {
 	 * %WORKER_UNBOUND set and concurrency management disabled, and may
 	 * be executing on any CPU.  The pool behaves as an unbound one.
 	 *
-	 * Note that DISASSOCIATED can be flipped only while holding
-	 * assoc_mutex to avoid changing binding state while
+	 * Note that DISASSOCIATED should be flipped only while holding
+	 * manager_mutex to avoid changing binding state while
 	 * create_worker() is in progress.
 	 */
 	POOL_MANAGE_WORKERS	= 1 << 0,	/* need to manage workers */
-	POOL_MANAGING_WORKERS   = 1 << 1,       /* managing workers */
 	POOL_DISASSOCIATED	= 1 << 2,	/* cpu can't serve workers */
 	POOL_FREEZING		= 1 << 3,	/* freeze in progress */
 
@@ -74,12 +77,14 @@ enum {
 	WORKER_PREP		= 1 << 3,	/* preparing to run works */
 	WORKER_CPU_INTENSIVE	= 1 << 6,	/* cpu intensive */
 	WORKER_UNBOUND		= 1 << 7,	/* worker is unbound */
+	WORKER_REBOUND		= 1 << 8,	/* worker was rebound */
 
-	WORKER_NOT_RUNNING	= WORKER_PREP | WORKER_UNBOUND |
-				  WORKER_CPU_INTENSIVE,
+	WORKER_NOT_RUNNING	= WORKER_PREP | WORKER_CPU_INTENSIVE |
+				  WORKER_UNBOUND | WORKER_REBOUND,
 
 	NR_STD_WORKER_POOLS	= 2,		/* # standard pools per cpu */
 
+	UNBOUND_POOL_HASH_ORDER	= 6,		/* hashed by pool->attrs */
 	BUSY_WORKER_HASH_ORDER	= 6,		/* 64 pointers */
 
 	MAX_IDLE_WORKERS_RATIO	= 4,		/* 1/4 of busy can be idle */
@@ -97,6 +102,8 @@ enum {
 	 */
 	RESCUER_NICE_LEVEL	= -20,
 	HIGHPRI_NICE_LEVEL	= -20,
+
+	WQ_NAME_LEN		= 24,
 };
 
 /*
@@ -115,16 +122,26 @@ enum {
  *    cpu or grabbing pool->lock is enough for read access.  If
  *    POOL_DISASSOCIATED is set, it's identical to L.
  *
- * F: wq->flush_mutex protected.
+ * MG: pool->manager_mutex and pool->lock protected.  Writes require both
+ *     locks.  Reads can happen under either lock.
+ *
+ * PL: wq_pool_mutex protected.
+ *
+ * PR: wq_pool_mutex protected for writes.  Sched-RCU protected for reads.
+ *
+ * WQ: wq->mutex protected.
  *
- * W: workqueue_lock protected.
+ * WR: wq->mutex protected for writes.  Sched-RCU protected for reads.
+ *
+ * MD: wq_mayday_lock protected.
  */
 
 /* struct worker is defined in workqueue_internal.h */
 
 struct worker_pool {
 	spinlock_t		lock;		/* the pool lock */
-	unsigned int		cpu;		/* I: the associated cpu */
+	int			cpu;		/* I: the associated cpu */
+	int			node;		/* I: the associated node ID */
 	int			id;		/* I: pool ID */
 	unsigned int		flags;		/* X: flags */
 
@@ -138,12 +155,18 @@ struct worker_pool {
 	struct timer_list	idle_timer;	/* L: worker idle timeout */
 	struct timer_list	mayday_timer;	/* L: SOS timer for workers */
 
-	/* workers are chained either in busy_hash or idle_list */
+	/* a workers is either on busy_hash or idle_list, or the manager */
 	DECLARE_HASHTABLE(busy_hash, BUSY_WORKER_HASH_ORDER);
 						/* L: hash of busy workers */
 
-	struct mutex		assoc_mutex;	/* protect POOL_DISASSOCIATED */
-	struct ida		worker_ida;	/* L: for worker IDs */
+	/* see manage_workers() for details on the two manager mutexes */
+	struct mutex		manager_arb;	/* manager arbitration */
+	struct mutex		manager_mutex;	/* manager exclusion */
+	struct idr		worker_idr;	/* MG: worker IDs and iteration */
+
+	struct workqueue_attrs	*attrs;		/* I: worker attributes */
+	struct hlist_node	hash_node;	/* PL: unbound_pool_hash node */
+	int			refcnt;		/* PL: refcnt for unbound pools */
 
 	/*
 	 * The current concurrency level.  As it's likely to be accessed
@@ -151,6 +174,12 @@ struct worker_pool {
 	 * cacheline.
 	 */
 	atomic_t		nr_running ____cacheline_aligned_in_smp;
+
+	/*
+	 * Destruction of pool is sched-RCU protected to allow dereferences
+	 * from get_work_pool().
+	 */
+	struct rcu_head		rcu;
 } ____cacheline_aligned_in_smp;
 
 /*
@@ -164,75 +193,107 @@ struct pool_workqueue {
 	struct workqueue_struct *wq;		/* I: the owning workqueue */
 	int			work_color;	/* L: current color */
 	int			flush_color;	/* L: flushing color */
+	int			refcnt;		/* L: reference count */
 	int			nr_in_flight[WORK_NR_COLORS];
 						/* L: nr of in_flight works */
 	int			nr_active;	/* L: nr of active works */
 	int			max_active;	/* L: max active works */
 	struct list_head	delayed_works;	/* L: delayed works */
-};
+	struct list_head	pwqs_node;	/* WR: node on wq->pwqs */
+	struct list_head	mayday_node;	/* MD: node on wq->maydays */
+
+	/*
+	 * Release of unbound pwq is punted to system_wq.  See put_pwq()
+	 * and pwq_unbound_release_workfn() for details.  pool_workqueue
+	 * itself is also sched-RCU protected so that the first pwq can be
+	 * determined without grabbing wq->mutex.
+	 */
+	struct work_struct	unbound_release_work;
+	struct rcu_head		rcu;
+} __aligned(1 << WORK_STRUCT_FLAG_BITS);
 
 /*
  * Structure used to wait for workqueue flush.
  */
 struct wq_flusher {
-	struct list_head	list;		/* F: list of flushers */
-	int			flush_color;	/* F: flush color waiting for */
+	struct list_head	list;		/* WQ: list of flushers */
+	int			flush_color;	/* WQ: flush color waiting for */
 	struct completion	done;		/* flush completion */
 };
 
-/*
- * All cpumasks are assumed to be always set on UP and thus can't be
- * used to determine whether there's something to be done.
- */
-#ifdef CONFIG_SMP
-typedef cpumask_var_t mayday_mask_t;
-#define mayday_test_and_set_cpu(cpu, mask)	\
-	cpumask_test_and_set_cpu((cpu), (mask))
-#define mayday_clear_cpu(cpu, mask)		cpumask_clear_cpu((cpu), (mask))
-#define for_each_mayday_cpu(cpu, mask)		for_each_cpu((cpu), (mask))
-#define alloc_mayday_mask(maskp, gfp)		zalloc_cpumask_var((maskp), (gfp))
-#define free_mayday_mask(mask)			free_cpumask_var((mask))
-#else
-typedef unsigned long mayday_mask_t;
-#define mayday_test_and_set_cpu(cpu, mask)	test_and_set_bit(0, &(mask))
-#define mayday_clear_cpu(cpu, mask)		clear_bit(0, &(mask))
-#define for_each_mayday_cpu(cpu, mask)		if ((cpu) = 0, (mask))
-#define alloc_mayday_mask(maskp, gfp)		true
-#define free_mayday_mask(mask)			do { } while (0)
-#endif
+struct wq_device;
 
 /*
- * The externally visible workqueue abstraction is an array of
- * per-CPU workqueues:
+ * The externally visible workqueue.  It relays the issued work items to
+ * the appropriate worker_pool through its pool_workqueues.
  */
 struct workqueue_struct {
-	unsigned int		flags;		/* W: WQ_* flags */
-	union {
-		struct pool_workqueue __percpu		*pcpu;
-		struct pool_workqueue			*single;
-		unsigned long				v;
-	} pool_wq;				/* I: pwq's */
-	struct list_head	list;		/* W: list of all workqueues */
-
-	struct mutex		flush_mutex;	/* protects wq flushing */
-	int			work_color;	/* F: current work color */
-	int			flush_color;	/* F: current flush color */
+	struct list_head	pwqs;		/* WR: all pwqs of this wq */
+	struct list_head	list;		/* PL: list of all workqueues */
+
+	struct mutex		mutex;		/* protects this wq */
+	int			work_color;	/* WQ: current work color */
+	int			flush_color;	/* WQ: current flush color */
 	atomic_t		nr_pwqs_to_flush; /* flush in progress */
-	struct wq_flusher	*first_flusher;	/* F: first flusher */
-	struct list_head	flusher_queue;	/* F: flush waiters */
-	struct list_head	flusher_overflow; /* F: flush overflow list */
+	struct wq_flusher	*first_flusher;	/* WQ: first flusher */
+	struct list_head	flusher_queue;	/* WQ: flush waiters */
+	struct list_head	flusher_overflow; /* WQ: flush overflow list */
 
-	mayday_mask_t		mayday_mask;	/* cpus requesting rescue */
+	struct list_head	maydays;	/* MD: pwqs requesting rescue */
 	struct worker		*rescuer;	/* I: rescue worker */
 
-	int			nr_drainers;	/* W: drain in progress */
-	int			saved_max_active; /* W: saved pwq max_active */
+	int			nr_drainers;	/* WQ: drain in progress */
+	int			saved_max_active; /* WQ: saved pwq max_active */
+
+	struct workqueue_attrs	*unbound_attrs;	/* WQ: only for unbound wqs */
+	struct pool_workqueue	*dfl_pwq;	/* WQ: only for unbound wqs */
+
+#ifdef CONFIG_SYSFS
+	struct wq_device	*wq_dev;	/* I: for sysfs interface */
+#endif
 #ifdef CONFIG_LOCKDEP
 	struct lockdep_map	lockdep_map;
 #endif
-	char			name[];		/* I: workqueue name */
+	char			name[WQ_NAME_LEN]; /* I: workqueue name */
+
+	/* hot fields used during command issue, aligned to cacheline */
+	unsigned int		flags ____cacheline_aligned; /* WQ: WQ_* flags */
+	struct pool_workqueue __percpu *cpu_pwqs; /* I: per-cpu pwqs */
+	struct pool_workqueue __rcu *numa_pwq_tbl[]; /* FR: unbound pwqs indexed by node */
 };
 
+static struct kmem_cache *pwq_cache;
+
+static int wq_numa_tbl_len;		/* highest possible NUMA node id + 1 */
+static cpumask_var_t *wq_numa_possible_cpumask;
+					/* possible CPUs of each node */
+
+static bool wq_disable_numa;
+module_param_named(disable_numa, wq_disable_numa, bool, 0444);
+
+static bool wq_numa_enabled;		/* unbound NUMA affinity enabled */
+
+/* buf for wq_update_unbound_numa_attrs(), protected by CPU hotplug exclusion */
+static struct workqueue_attrs *wq_update_unbound_numa_attrs_buf;
+
+static DEFINE_MUTEX(wq_pool_mutex);	/* protects pools and workqueues list */
+static DEFINE_SPINLOCK(wq_mayday_lock);	/* protects wq->maydays list */
+
+static LIST_HEAD(workqueues);		/* PL: list of all workqueues */
+static bool workqueue_freezing;		/* PL: have wqs started freezing? */
+
+/* the per-cpu worker pools */
+static DEFINE_PER_CPU_SHARED_ALIGNED(struct worker_pool [NR_STD_WORKER_POOLS],
+				     cpu_worker_pools);
+
+static DEFINE_IDR(worker_pool_idr);	/* PR: idr of all pools */
+
+/* PL: hash of all unbound pools keyed by pool->attrs */
+static DEFINE_HASHTABLE(unbound_pool_hash, UNBOUND_POOL_HASH_ORDER);
+
+/* I: attributes used when instantiating standard unbound pools on demand */
+static struct workqueue_attrs *unbound_std_wq_attrs[NR_STD_WORKER_POOLS];
+
 struct workqueue_struct *system_wq __read_mostly;
 EXPORT_SYMBOL_GPL(system_wq);
 struct workqueue_struct *system_highpri_wq __read_mostly;
@@ -244,64 +305,87 @@ EXPORT_SYMBOL_GPL(system_unbound_wq);
 struct workqueue_struct *system_freezable_wq __read_mostly;
 EXPORT_SYMBOL_GPL(system_freezable_wq);
 
+static int worker_thread(void *__worker);
+static void copy_workqueue_attrs(struct workqueue_attrs *to,
+				 const struct workqueue_attrs *from);
+
 #define CREATE_TRACE_POINTS
 #include <trace/events/workqueue.h>
 
-#define for_each_std_worker_pool(pool, cpu)				\
-	for ((pool) = &std_worker_pools(cpu)[0];			\
-	     (pool) < &std_worker_pools(cpu)[NR_STD_WORKER_POOLS]; (pool)++)
+#define assert_rcu_or_pool_mutex()					\
+	rcu_lockdep_assert(rcu_read_lock_sched_held() ||		\
+			   lockdep_is_held(&wq_pool_mutex),		\
+			   "sched RCU or wq_pool_mutex should be held")
 
-#define for_each_busy_worker(worker, i, pool)				\
-	hash_for_each(pool->busy_hash, i, worker, hentry)
+#define assert_rcu_or_wq_mutex(wq)					\
+	rcu_lockdep_assert(rcu_read_lock_sched_held() ||		\
+			   lockdep_is_held(&wq->mutex),			\
+			   "sched RCU or wq->mutex should be held")
 
-static inline int __next_wq_cpu(int cpu, const struct cpumask *mask,
-				unsigned int sw)
-{
-	if (cpu < nr_cpu_ids) {
-		if (sw & 1) {
-			cpu = cpumask_next(cpu, mask);
-			if (cpu < nr_cpu_ids)
-				return cpu;
-		}
-		if (sw & 2)
-			return WORK_CPU_UNBOUND;
-	}
-	return WORK_CPU_END;
-}
+#ifdef CONFIG_LOCKDEP
+#define assert_manager_or_pool_lock(pool)				\
+	WARN_ONCE(debug_locks &&					\
+		  !lockdep_is_held(&(pool)->manager_mutex) &&		\
+		  !lockdep_is_held(&(pool)->lock),			\
+		  "pool->manager_mutex or ->lock should be held")
+#else
+#define assert_manager_or_pool_lock(pool)	do { } while (0)
+#endif
 
-static inline int __next_pwq_cpu(int cpu, const struct cpumask *mask,
-				 struct workqueue_struct *wq)
-{
-	return __next_wq_cpu(cpu, mask, !(wq->flags & WQ_UNBOUND) ? 1 : 2);
-}
+#define for_each_cpu_worker_pool(pool, cpu)				\
+	for ((pool) = &per_cpu(cpu_worker_pools, cpu)[0];		\
+	     (pool) < &per_cpu(cpu_worker_pools, cpu)[NR_STD_WORKER_POOLS]; \
+	     (pool)++)
 
-/*
- * CPU iterators
+/**
+ * for_each_pool - iterate through all worker_pools in the system
+ * @pool: iteration cursor
+ * @pi: integer used for iteration
  *
- * An extra cpu number is defined using an invalid cpu number
- * (WORK_CPU_UNBOUND) to host workqueues which are not bound to any
- * specific CPU.  The following iterators are similar to for_each_*_cpu()
- * iterators but also considers the unbound CPU.
+ * This must be called either with wq_pool_mutex held or sched RCU read
+ * locked.  If the pool needs to be used beyond the locking in effect, the
+ * caller is responsible for guaranteeing that the pool stays online.
  *
- * for_each_wq_cpu()		: possible CPUs + WORK_CPU_UNBOUND
- * for_each_online_wq_cpu()	: online CPUs + WORK_CPU_UNBOUND
- * for_each_pwq_cpu()		: possible CPUs for bound workqueues,
- *				  WORK_CPU_UNBOUND for unbound workqueues
+ * The if/else clause exists only for the lockdep assertion and can be
+ * ignored.
  */
-#define for_each_wq_cpu(cpu)						\
-	for ((cpu) = __next_wq_cpu(-1, cpu_possible_mask, 3);		\
-	     (cpu) < WORK_CPU_END;					\
-	     (cpu) = __next_wq_cpu((cpu), cpu_possible_mask, 3))
+#define for_each_pool(pool, pi)						\
+	idr_for_each_entry(&worker_pool_idr, pool, pi)			\
+		if (({ assert_rcu_or_pool_mutex(); false; })) { }	\
+		else
 
-#define for_each_online_wq_cpu(cpu)					\
-	for ((cpu) = __next_wq_cpu(-1, cpu_online_mask, 3);		\
-	     (cpu) < WORK_CPU_END;					\
-	     (cpu) = __next_wq_cpu((cpu), cpu_online_mask, 3))
+/**
+ * for_each_pool_worker - iterate through all workers of a worker_pool
+ * @worker: iteration cursor
+ * @wi: integer used for iteration
+ * @pool: worker_pool to iterate workers of
+ *
+ * This must be called with either @pool->manager_mutex or ->lock held.
+ *
+ * The if/else clause exists only for the lockdep assertion and can be
+ * ignored.
+ */
+#define for_each_pool_worker(worker, wi, pool)				\
+	idr_for_each_entry(&(pool)->worker_idr, (worker), (wi))		\
+		if (({ assert_manager_or_pool_lock((pool)); false; })) { } \
+		else
 
-#define for_each_pwq_cpu(cpu, wq)					\
-	for ((cpu) = __next_pwq_cpu(-1, cpu_possible_mask, (wq));	\
-	     (cpu) < WORK_CPU_END;					\
-	     (cpu) = __next_pwq_cpu((cpu), cpu_possible_mask, (wq)))
+/**
+ * for_each_pwq - iterate through all pool_workqueues of the specified workqueue
+ * @pwq: iteration cursor
+ * @wq: the target workqueue
+ *
+ * This must be called either with wq->mutex held or sched RCU read locked.
+ * If the pwq needs to be used beyond the locking in effect, the caller is
+ * responsible for guaranteeing that the pwq stays online.
+ *
+ * The if/else clause exists only for the lockdep assertion and can be
+ * ignored.
+ */
+#define for_each_pwq(pwq, wq)						\
+	list_for_each_entry_rcu((pwq), &(wq)->pwqs, pwqs_node)		\
+		if (({ assert_rcu_or_wq_mutex(wq); false; })) { }	\
+		else
 
 #ifdef CONFIG_DEBUG_OBJECTS_WORK
 
@@ -419,77 +503,35 @@ static inline void debug_work_activate(struct work_struct *work) { }
 static inline void debug_work_deactivate(struct work_struct *work) { }
 #endif
 
-/* Serializes the accesses to the list of workqueues. */
-static DEFINE_SPINLOCK(workqueue_lock);
-static LIST_HEAD(workqueues);
-static bool workqueue_freezing;		/* W: have wqs started freezing? */
-
-/*
- * The CPU and unbound standard worker pools.  The unbound ones have
- * POOL_DISASSOCIATED set, and their workers have WORKER_UNBOUND set.
- */
-static DEFINE_PER_CPU_SHARED_ALIGNED(struct worker_pool [NR_STD_WORKER_POOLS],
-				     cpu_std_worker_pools);
-static struct worker_pool unbound_std_worker_pools[NR_STD_WORKER_POOLS];
-
-/* idr of all pools */
-static DEFINE_MUTEX(worker_pool_idr_mutex);
-static DEFINE_IDR(worker_pool_idr);
-
-static int worker_thread(void *__worker);
-
-static struct worker_pool *std_worker_pools(int cpu)
-{
-	if (cpu != WORK_CPU_UNBOUND)
-		return per_cpu(cpu_std_worker_pools, cpu);
-	else
-		return unbound_std_worker_pools;
-}
-
-static int std_worker_pool_pri(struct worker_pool *pool)
-{
-	return pool - std_worker_pools(pool->cpu);
-}
-
 /* allocate ID and assign it to @pool */
 static int worker_pool_assign_id(struct worker_pool *pool)
 {
 	int ret;
 
-	mutex_lock(&worker_pool_idr_mutex);
+	lockdep_assert_held(&wq_pool_mutex);
+
 	ret = idr_alloc(&worker_pool_idr, pool, 0, 0, GFP_KERNEL);
-	if (ret >= 0)
+	if (ret >= 0) {
 		pool->id = ret;
-	mutex_unlock(&worker_pool_idr_mutex);
-
-	return ret < 0 ? ret : 0;
+		return 0;
+	}
+	return ret;
 }
 
-/*
- * Lookup worker_pool by id.  The idr currently is built during boot and
- * never modified.  Don't worry about locking for now.
+/**
+ * unbound_pwq_by_node - return the unbound pool_workqueue for the given node
+ * @wq: the target workqueue
+ * @node: the node ID
+ *
+ * This must be called either with pwq_lock held or sched RCU read locked.
+ * If the pwq needs to be used beyond the locking in effect, the caller is
+ * responsible for guaranteeing that the pwq stays online.
  */
-static struct worker_pool *worker_pool_by_id(int pool_id)
+static struct pool_workqueue *unbound_pwq_by_node(struct workqueue_struct *wq,
+						  int node)
 {
-	return idr_find(&worker_pool_idr, pool_id);
-}
-
-static struct worker_pool *get_std_worker_pool(int cpu, bool highpri)
-{
-	struct worker_pool *pools = std_worker_pools(cpu);
-
-	return &pools[highpri];
-}
-
-static struct pool_workqueue *get_pwq(unsigned int cpu,
-				      struct workqueue_struct *wq)
-{
-	if (!(wq->flags & WQ_UNBOUND)) {
-		if (likely(cpu < nr_cpu_ids))
-			return per_cpu_ptr(wq->pool_wq.pcpu, cpu);
-	} else if (likely(cpu == WORK_CPU_UNBOUND))
-		return wq->pool_wq.single;
-	return NULL;
+	assert_rcu_or_wq_mutex(wq);
+	return rcu_dereference_raw(wq->numa_pwq_tbl[node]);
 }
 
 static unsigned int work_color_to_flags(int color)
@@ -531,7 +573,7 @@ static int work_next_color(int color)
 static inline void set_work_data(struct work_struct *work, unsigned long data,
 				 unsigned long flags)
 {
-	BUG_ON(!work_pending(work));
+	WARN_ON_ONCE(!work_pending(work));
 	atomic_long_set(&work->data, data | flags | work_static(work));
 }
 
@@ -583,13 +625,23 @@ static struct pool_workqueue *get_work_pwq(struct work_struct *work)
  * @work: the work item of interest
  *
  * Return the worker_pool @work was last associated with.  %NULL if none.
+ *
+ * Pools are created and destroyed under wq_pool_mutex, and allows read
+ * access under sched-RCU read lock.  As such, this function should be
+ * called under wq_pool_mutex or with preemption disabled.
+ *
+ * All fields of the returned pool are accessible as long as the above
+ * mentioned locking is in effect.  If the returned pool needs to be used
+ * beyond the critical section, the caller is responsible for ensuring the
+ * returned pool is and stays online.
  */
 static struct worker_pool *get_work_pool(struct work_struct *work)
 {
 	unsigned long data = atomic_long_read(&work->data);
-	struct worker_pool *pool;
 	int pool_id;
 
+	assert_rcu_or_pool_mutex();
+
 	if (data & WORK_STRUCT_PWQ)
 		return ((struct pool_workqueue *)
 			(data & WORK_STRUCT_WQ_DATA_MASK))->pool;
@@ -598,9 +650,7 @@ static struct worker_pool *get_work_pool(struct work_struct *work)
 	if (pool_id == WORK_OFFQ_POOL_NONE)
 		return NULL;
 
-	pool = worker_pool_by_id(pool_id);
-	WARN_ON_ONCE(!pool);
-	return pool;
+	return idr_find(&worker_pool_idr, pool_id);
 }
 
 /**
@@ -689,7 +739,7 @@ static bool need_to_manage_workers(struct worker_pool *pool)
 /* Do we have too many workers and should some go away? */
 static bool too_many_workers(struct worker_pool *pool)
 {
-	bool managing = pool->flags & POOL_MANAGING_WORKERS;
+	bool managing = mutex_is_locked(&pool->manager_arb);
 	int nr_idle = pool->nr_idle + managing; /* manager is considered idle */
 	int nr_busy = pool->nr_workers - nr_idle;
 
@@ -744,7 +794,7 @@ static void wake_up_worker(struct worker_pool *pool)
  * CONTEXT:
  * spin_lock_irq(rq->lock)
  */
-void wq_worker_waking_up(struct task_struct *task, unsigned int cpu)
+void wq_worker_waking_up(struct task_struct *task, int cpu)
 {
 	struct worker *worker = kthread_data(task);
 
@@ -769,8 +819,7 @@ void wq_worker_waking_up(struct task_struct *task, unsigned int cpu)
  * RETURNS:
  * Worker task on @cpu to wake up, %NULL if none.
  */
-struct task_struct *wq_worker_sleeping(struct task_struct *task,
-				       unsigned int cpu)
+struct task_struct *wq_worker_sleeping(struct task_struct *task, int cpu)
 {
 	struct worker *worker = kthread_data(task), *to_wakeup = NULL;
 	struct worker_pool *pool;
@@ -786,7 +835,8 @@ struct task_struct *wq_worker_sleeping(struct task_struct *task,
 	pool = worker->pool;
 
 	/* this can only happen on the local cpu */
-	BUG_ON(cpu != raw_smp_processor_id());
+	if (WARN_ON_ONCE(cpu != raw_smp_processor_id()))
+		return NULL;
 
 	/*
 	 * The counterpart of the following dec_and_test, implied mb,
@@ -891,13 +941,12 @@ static inline void worker_clr_flags(struct worker *worker, unsigned int flags)
  * recycled work item as currently executing and make it wait until the
  * current execution finishes, introducing an unwanted dependency.
  *
- * This function checks the work item address, work function and workqueue
- * to avoid false positives.  Note that this isn't complete as one may
- * construct a work function which can introduce dependency onto itself
- * through a recycled work item.  Well, if somebody wants to shoot oneself
- * in the foot that badly, there's only so much we can do, and if such
- * deadlock actually occurs, it should be easy to locate the culprit work
- * function.
+ * This function checks the work item address and work function to avoid
+ * false positives.  Note that this isn't complete as one may construct a
+ * work function which can introduce dependency onto itself through a
+ * recycled work item.  Well, if somebody wants to shoot oneself in the
+ * foot that badly, there's only so much we can do, and if such deadlock
+ * actually occurs, it should be easy to locate the culprit work function.
  *
  * CONTEXT:
  * spin_lock_irq(pool->lock).
@@ -961,6 +1010,64 @@ static void move_linked_works(struct work_struct *work, struct list_head *head,
 		*nextp = n;
 }
 
+/**
+ * get_pwq - get an extra reference on the specified pool_workqueue
+ * @pwq: pool_workqueue to get
+ *
+ * Obtain an extra reference on @pwq.  The caller should guarantee that
+ * @pwq has positive refcnt and be holding the matching pool->lock.
+ */
+static void get_pwq(struct pool_workqueue *pwq)
+{
+	lockdep_assert_held(&pwq->pool->lock);
+	WARN_ON_ONCE(pwq->refcnt <= 0);
+	pwq->refcnt++;
+}
+
+/**
+ * put_pwq - put a pool_workqueue reference
+ * @pwq: pool_workqueue to put
+ *
+ * Drop a reference of @pwq.  If its refcnt reaches zero, schedule its
+ * destruction.  The caller should be holding the matching pool->lock.
+ */
+static void put_pwq(struct pool_workqueue *pwq)
+{
+	lockdep_assert_held(&pwq->pool->lock);
+	if (likely(--pwq->refcnt))
+		return;
+	if (WARN_ON_ONCE(!(pwq->wq->flags & WQ_UNBOUND)))
+		return;
+	/*
+	 * @pwq can't be released under pool->lock, bounce to
+	 * pwq_unbound_release_workfn().  This never recurses on the same
+	 * pool->lock as this path is taken only for unbound workqueues and
+	 * the release work item is scheduled on a per-cpu workqueue.  To
+	 * avoid lockdep warning, unbound pool->locks are given lockdep
+	 * subclass of 1 in get_unbound_pool().
+	 */
+	schedule_work(&pwq->unbound_release_work);
+}
+
+/**
+ * put_pwq_unlocked - put_pwq() with surrounding pool lock/unlock
+ * @pwq: pool_workqueue to put (can be %NULL)
+ *
+ * put_pwq() with locking.  This function also allows %NULL @pwq.
+ */
+static void put_pwq_unlocked(struct pool_workqueue *pwq)
+{
+	if (pwq) {
+		/*
+		 * As both pwqs and pools are sched-RCU protected, the
+		 * following lock operations are safe.
+		 */
+		spin_lock_irq(&pwq->pool->lock);
+		put_pwq(pwq);
+		spin_unlock_irq(&pwq->pool->lock);
+	}
+}
+
 static void pwq_activate_delayed_work(struct work_struct *work)
 {
 	struct pool_workqueue *pwq = get_work_pwq(work);
@@ -992,9 +1099,9 @@ static void pwq_activate_first_delayed(struct pool_workqueue *pwq)
  */
 static void pwq_dec_nr_in_flight(struct pool_workqueue *pwq, int color)
 {
-	/* ignore uncolored works */
+	/* uncolored work items don't participate in flushing or nr_active */
 	if (color == WORK_NO_COLOR)
-		return;
+		goto out_put;
 
 	pwq->nr_in_flight[color]--;
 
@@ -1007,11 +1114,11 @@ static void pwq_dec_nr_in_flight(struct pool_workqueue *pwq, int color)
 
 	/* is flush in progress and are we at the flushing tip? */
 	if (likely(pwq->flush_color != color))
-		return;
+		goto out_put;
 
 	/* are there still in-flight works? */
 	if (pwq->nr_in_flight[color])
-		return;
+		goto out_put;
 
 	/* this pwq is done, clear flush_color */
 	pwq->flush_color = -1;
@@ -1022,6 +1129,8 @@ static void pwq_dec_nr_in_flight(struct pool_workqueue *pwq, int color)
 	 */
 	if (atomic_dec_and_test(&pwq->wq->nr_pwqs_to_flush))
 		complete(&pwq->wq->first_flusher->done);
+out_put:
+	put_pwq(pwq);
 }
 
 /**
@@ -1144,11 +1253,12 @@ static void insert_work(struct pool_workqueue *pwq, struct work_struct *work,
 	/* we own @work, set data and link */
 	set_work_pwq(work, pwq, extra_flags);
 	list_add_tail(&work->entry, head);
+	get_pwq(pwq);
 
 	/*
-	 * Ensure either worker_sched_deactivated() sees the above
-	 * list_add_tail() or we see zero nr_running to avoid workers
-	 * lying around lazily while there are works to be processed.
+	 * Ensure either wq_worker_sleeping() sees the above
+	 * list_add_tail() or we see zero nr_running to avoid workers lying
+	 * around lazily while there are works to be processed.
 	 */
 	smp_mb();
 
@@ -1172,10 +1282,11 @@ static bool is_chained_work(struct workqueue_struct *wq)
 	return worker && worker->current_pwq->wq == wq;
 }
 
-static void __queue_work(unsigned int cpu, struct workqueue_struct *wq,
+static void __queue_work(int cpu, struct workqueue_struct *wq,
 			 struct work_struct *work)
 {
 	struct pool_workqueue *pwq;
+	struct worker_pool *last_pool;
 	struct list_head *worklist;
 	unsigned int work_flags;
 	unsigned int req_cpu = cpu;
@@ -1191,48 +1302,62 @@ static void __queue_work(unsigned int cpu, struct workqueue_struct *wq,
 	debug_work_activate(work);
 
 	/* if dying, only works from the same workqueue are allowed */
-	if (unlikely(wq->flags & WQ_DRAINING) &&
+	if (unlikely(wq->flags & __WQ_DRAINING) &&
 	    WARN_ON_ONCE(!is_chained_work(wq)))
 		return;
+retry:
+	if (req_cpu == WORK_CPU_UNBOUND)
+		cpu = raw_smp_processor_id();
 
-	/* determine the pwq to use */
-	if (!(wq->flags & WQ_UNBOUND)) {
-		struct worker_pool *last_pool;
-
-		if (cpu == WORK_CPU_UNBOUND)
-			cpu = raw_smp_processor_id();
-
-		/*
-		 * It's multi cpu.  If @work was previously on a different
-		 * cpu, it might still be running there, in which case the
-		 * work needs to be queued on that cpu to guarantee
-		 * non-reentrancy.
-		 */
-		pwq = get_pwq(cpu, wq);
-		last_pool = get_work_pool(work);
+	/* pwq which will be used unless @work is executing elsewhere */
+	if (!(wq->flags & WQ_UNBOUND))
+		pwq = per_cpu_ptr(wq->cpu_pwqs, cpu);
+	else
+		pwq = unbound_pwq_by_node(wq, cpu_to_node(cpu));
 
-		if (last_pool && last_pool != pwq->pool) {
-			struct worker *worker;
+	/*
+	 * If @work was previously on a different pool, it might still be
+	 * running there, in which case the work needs to be queued on that
+	 * pool to guarantee non-reentrancy.
+	 */
+	last_pool = get_work_pool(work);
+	if (last_pool && last_pool != pwq->pool) {
+		struct worker *worker;
 
-			spin_lock(&last_pool->lock);
+		spin_lock(&last_pool->lock);
 
-			worker = find_worker_executing_work(last_pool, work);
+		worker = find_worker_executing_work(last_pool, work);
 
-			if (worker && worker->current_pwq->wq == wq) {
-				pwq = get_pwq(last_pool->cpu, wq);
-			} else {
-				/* meh... not running there, queue here */
-				spin_unlock(&last_pool->lock);
-				spin_lock(&pwq->pool->lock);
-			}
+		if (worker && worker->current_pwq->wq == wq) {
+			pwq = worker->current_pwq;
 		} else {
+			/* meh... not running there, queue here */
+			spin_unlock(&last_pool->lock);
 			spin_lock(&pwq->pool->lock);
 		}
 	} else {
-		pwq = get_pwq(WORK_CPU_UNBOUND, wq);
 		spin_lock(&pwq->pool->lock);
 	}
 
+	/*
+	 * pwq is determined and locked.  For unbound pools, we could have
+	 * raced with pwq release and it could already be dead.  If its
+	 * refcnt is zero, repeat pwq selection.  Note that pwqs never die
+	 * without another pwq replacing it in the numa_pwq_tbl or while
+	 * work items are executing on it, so the retrying is guaranteed to
+	 * make forward-progress.
+	 */
+	if (unlikely(!pwq->refcnt)) {
+		if (wq->flags & WQ_UNBOUND) {
+			spin_unlock(&pwq->pool->lock);
+			cpu_relax();
+			goto retry;
+		}
+		/* oops */
+		WARN_ONCE(true, "workqueue: per-cpu pwq for %s on cpu%d has 0 refcnt",
+			  wq->name, cpu);
+	}
+
 	/* pwq determined, queue */
 	trace_workqueue_queue_work(req_cpu, pwq, work);
 
@@ -1287,22 +1412,6 @@ bool queue_work_on(int cpu, struct workqueue_struct *wq,
 }
 EXPORT_SYMBOL_GPL(queue_work_on);
 
-/**
- * queue_work - queue work on a workqueue
- * @wq: workqueue to use
- * @work: work to queue
- *
- * Returns %false if @work was already on a queue, %true otherwise.
- *
- * We queue the work to the CPU on which it was submitted, but if the CPU dies
- * it can be processed by another CPU.
- */
-bool queue_work(struct workqueue_struct *wq, struct work_struct *work)
-{
-	return queue_work_on(WORK_CPU_UNBOUND, wq, work);
-}
-EXPORT_SYMBOL_GPL(queue_work);
-
 void delayed_work_timer_fn(unsigned long __data)
 {
 	struct delayed_work *dwork = (struct delayed_work *)__data;
@@ -1378,21 +1487,6 @@ bool queue_delayed_work_on(int cpu, struct workqueue_struct *wq,
 EXPORT_SYMBOL_GPL(queue_delayed_work_on);
 
 /**
- * queue_delayed_work - queue work on a workqueue after delay
- * @wq: workqueue to use
- * @dwork: delayable work to queue
- * @delay: number of jiffies to wait before queueing
- *
- * Equivalent to queue_delayed_work_on() but tries to use the local CPU.
- */
-bool queue_delayed_work(struct workqueue_struct *wq,
-			struct delayed_work *dwork, unsigned long delay)
-{
-	return queue_delayed_work_on(WORK_CPU_UNBOUND, wq, dwork, delay);
-}
-EXPORT_SYMBOL_GPL(queue_delayed_work);
-
-/**
  * mod_delayed_work_on - modify delay of or queue a delayed work on specific CPU
  * @cpu: CPU number to execute work on
  * @wq: workqueue to use
@@ -1431,21 +1525,6 @@ bool mod_delayed_work_on(int cpu, struct workqueue_struct *wq,
 EXPORT_SYMBOL_GPL(mod_delayed_work_on);
 
 /**
- * mod_delayed_work - modify delay of or queue a delayed work
- * @wq: workqueue to use
- * @dwork: work to queue
- * @delay: number of jiffies to wait before queueing
- *
- * mod_delayed_work_on() on local CPU.
- */
-bool mod_delayed_work(struct workqueue_struct *wq, struct delayed_work *dwork,
-		      unsigned long delay)
-{
-	return mod_delayed_work_on(WORK_CPU_UNBOUND, wq, dwork, delay);
-}
-EXPORT_SYMBOL_GPL(mod_delayed_work);
-
-/**
  * worker_enter_idle - enter idle state
  * @worker: worker which is entering idle state
  *
@@ -1459,9 +1538,10 @@ static void worker_enter_idle(struct worker *worker)
 {
 	struct worker_pool *pool = worker->pool;
 
-	BUG_ON(worker->flags & WORKER_IDLE);
-	BUG_ON(!list_empty(&worker->entry) &&
-	       (worker->hentry.next || worker->hentry.pprev));
+	if (WARN_ON_ONCE(worker->flags & WORKER_IDLE) ||
+	    WARN_ON_ONCE(!list_empty(&worker->entry) &&
+			 (worker->hentry.next || worker->hentry.pprev)))
+		return;
 
 	/* can't use worker_set_flags(), also called from start_worker() */
 	worker->flags |= WORKER_IDLE;
@@ -1498,22 +1578,25 @@ static void worker_leave_idle(struct worker *worker)
 {
 	struct worker_pool *pool = worker->pool;
 
-	BUG_ON(!(worker->flags & WORKER_IDLE));
+	if (WARN_ON_ONCE(!(worker->flags & WORKER_IDLE)))
+		return;
 	worker_clr_flags(worker, WORKER_IDLE);
 	pool->nr_idle--;
 	list_del_init(&worker->entry);
 }
 
 /**
- * worker_maybe_bind_and_lock - bind worker to its cpu if possible and lock pool
- * @worker: self
+ * worker_maybe_bind_and_lock - try to bind %current to worker_pool and lock it
+ * @pool: target worker_pool
+ *
+ * Bind %current to the cpu of @pool if it is associated and lock @pool.
  *
  * Works which are scheduled while the cpu is online must at least be
  * scheduled to a worker which is bound to the cpu so that if they are
  * flushed from cpu callbacks while cpu is going down, they are
  * guaranteed to execute on the cpu.
  *
- * This function is to be used by rogue workers and rescuers to bind
+ * This function is to be used by unbound workers and rescuers to bind
  * themselves to the target cpu and may race with cpu going down or
  * coming online.  kthread_bind() can't be used because it may put the
  * worker to already dead cpu and set_cpus_allowed_ptr() can't be used
@@ -1534,12 +1617,9 @@ static void worker_leave_idle(struct worker *worker)
  * %true if the associated pool is online (@worker is successfully
  * bound), %false if offline.
  */
-static bool worker_maybe_bind_and_lock(struct worker *worker)
+static bool worker_maybe_bind_and_lock(struct worker_pool *pool)
 __acquires(&pool->lock)
 {
-	struct worker_pool *pool = worker->pool;
-	struct task_struct *task = worker->task;
-
 	while (true) {
 		/*
 		 * The following call may fail, succeed or succeed
@@ -1548,14 +1628,13 @@ __acquires(&pool->lock)
 		 * against POOL_DISASSOCIATED.
 		 */
 		if (!(pool->flags & POOL_DISASSOCIATED))
-			set_cpus_allowed_ptr(task, get_cpu_mask(pool->cpu));
+			set_cpus_allowed_ptr(current, pool->attrs->cpumask);
 
 		spin_lock_irq(&pool->lock);
 		if (pool->flags & POOL_DISASSOCIATED)
 			return false;
-		if (task_cpu(task) == pool->cpu &&
-		    cpumask_equal(&current->cpus_allowed,
-				  get_cpu_mask(pool->cpu)))
+		if (task_cpu(current) == pool->cpu &&
+		    cpumask_equal(&current->cpus_allowed, pool->attrs->cpumask))
 			return true;
 		spin_unlock_irq(&pool->lock);
 
@@ -1570,108 +1649,6 @@ __acquires(&pool->lock)
 	}
 }
 
-/*
- * Rebind an idle @worker to its CPU.  worker_thread() will test
- * list_empty(@worker->entry) before leaving idle and call this function.
- */
-static void idle_worker_rebind(struct worker *worker)
-{
-	/* CPU may go down again inbetween, clear UNBOUND only on success */
-	if (worker_maybe_bind_and_lock(worker))
-		worker_clr_flags(worker, WORKER_UNBOUND);
-
-	/* rebind complete, become available again */
-	list_add(&worker->entry, &worker->pool->idle_list);
-	spin_unlock_irq(&worker->pool->lock);
-}
-
-/*
- * Function for @worker->rebind.work used to rebind unbound busy workers to
- * the associated cpu which is coming back online.  This is scheduled by
- * cpu up but can race with other cpu hotplug operations and may be
- * executed twice without intervening cpu down.
- */
-static void busy_worker_rebind_fn(struct work_struct *work)
-{
-	struct worker *worker = container_of(work, struct worker, rebind_work);
-
-	if (worker_maybe_bind_and_lock(worker))
-		worker_clr_flags(worker, WORKER_UNBOUND);
-
-	spin_unlock_irq(&worker->pool->lock);
-}
-
-/**
- * rebind_workers - rebind all workers of a pool to the associated CPU
- * @pool: pool of interest
- *
- * @pool->cpu is coming online.  Rebind all workers to the CPU.  Rebinding
- * is different for idle and busy ones.
- *
- * Idle ones will be removed from the idle_list and woken up.  They will
- * add themselves back after completing rebind.  This ensures that the
- * idle_list doesn't contain any unbound workers when re-bound busy workers
- * try to perform local wake-ups for concurrency management.
- *
- * Busy workers can rebind after they finish their current work items.
- * Queueing the rebind work item at the head of the scheduled list is
- * enough.  Note that nr_running will be properly bumped as busy workers
- * rebind.
- *
- * On return, all non-manager workers are scheduled for rebind - see
- * manage_workers() for the manager special case.  Any idle worker
- * including the manager will not appear on @idle_list until rebind is
- * complete, making local wake-ups safe.
- */
-static void rebind_workers(struct worker_pool *pool)
-{
-	struct worker *worker, *n;
-	int i;
-
-	lockdep_assert_held(&pool->assoc_mutex);
-	lockdep_assert_held(&pool->lock);
-
-	/* dequeue and kick idle ones */
-	list_for_each_entry_safe(worker, n, &pool->idle_list, entry) {
-		/*
-		 * idle workers should be off @pool->idle_list until rebind
-		 * is complete to avoid receiving premature local wake-ups.
-		 */
-		list_del_init(&worker->entry);
-
-		/*
-		 * worker_thread() will see the above dequeuing and call
-		 * idle_worker_rebind().
-		 */
-		wake_up_process(worker->task);
-	}
-
-	/* rebind busy workers */
-	for_each_busy_worker(worker, i, pool) {
-		struct work_struct *rebind_work = &worker->rebind_work;
-		struct workqueue_struct *wq;
-
-		if (test_and_set_bit(WORK_STRUCT_PENDING_BIT,
-				     work_data_bits(rebind_work)))
-			continue;
-
-		debug_work_activate(rebind_work);
-
-		/*
-		 * wq doesn't really matter but let's keep @worker->pool
-		 * and @pwq->pool consistent for sanity.
-		 */
-		if (std_worker_pool_pri(worker->pool))
-			wq = system_highpri_wq;
-		else
-			wq = system_wq;
-
-		insert_work(get_pwq(pool->cpu, wq), rebind_work,
-			    worker->scheduled.next,
-			    work_color_to_flags(WORK_NO_COLOR));
-	}
-}
-
 static struct worker *alloc_worker(void)
 {
 	struct worker *worker;
@@ -1680,7 +1657,6 @@ static struct worker *alloc_worker(void)
 	if (worker) {
 		INIT_LIST_HEAD(&worker->entry);
 		INIT_LIST_HEAD(&worker->scheduled);
-		INIT_WORK(&worker->rebind_work, busy_worker_rebind_fn);
 		/* on creation a worker is in !idle && prep state */
 		worker->flags = WORKER_PREP;
 	}
@@ -1703,18 +1679,25 @@ static struct worker *alloc_worker(void)
  */
 static struct worker *create_worker(struct worker_pool *pool)
 {
-	const char *pri = std_worker_pool_pri(pool) ? "H" : "";
 	struct worker *worker = NULL;
 	int id = -1;
+	char id_buf[16];
+
+	lockdep_assert_held(&pool->manager_mutex);
 
+	/*
+	 * ID is needed to determine kthread name.  Allocate ID first
+	 * without installing the pointer.
+	 */
+	idr_preload(GFP_KERNEL);
 	spin_lock_irq(&pool->lock);
-	while (ida_get_new(&pool->worker_ida, &id)) {
-		spin_unlock_irq(&pool->lock);
-		if (!ida_pre_get(&pool->worker_ida, GFP_KERNEL))
-			goto fail;
-		spin_lock_irq(&pool->lock);
-	}
+
+	id = idr_alloc(&pool->worker_idr, NULL, 0, 0, GFP_NOWAIT);
+
 	spin_unlock_irq(&pool->lock);
+	idr_preload_end();
+	if (id < 0)
+		goto fail;
 
 	worker = alloc_worker();
 	if (!worker)
@@ -1723,40 +1706,46 @@ static struct worker *create_worker(struct worker_pool *pool)
 	worker->pool = pool;
 	worker->id = id;
 
-	if (pool->cpu != WORK_CPU_UNBOUND)
-		worker->task = kthread_create_on_node(worker_thread,
-					worker, cpu_to_node(pool->cpu),
-					"kworker/%u:%d%s", pool->cpu, id, pri);
+	if (pool->cpu >= 0)
+		snprintf(id_buf, sizeof(id_buf), "%d:%d%s", pool->cpu, id,
+			 pool->attrs->nice < 0  ? "H" : "");
 	else
-		worker->task = kthread_create(worker_thread, worker,
-					      "kworker/u:%d%s", id, pri);
+		snprintf(id_buf, sizeof(id_buf), "u%d:%d", pool->id, id);
+
+	worker->task = kthread_create_on_node(worker_thread, worker, pool->node,
+					      "kworker/%s", id_buf);
 	if (IS_ERR(worker->task))
 		goto fail;
 
-	if (std_worker_pool_pri(pool))
-		set_user_nice(worker->task, HIGHPRI_NICE_LEVEL);
+	/*
+	 * set_cpus_allowed_ptr() will fail if the cpumask doesn't have any
+	 * online CPUs.  It'll be re-applied when any of the CPUs come up.
+	 */
+	set_user_nice(worker->task, pool->attrs->nice);
+	set_cpus_allowed_ptr(worker->task, pool->attrs->cpumask);
+
+	/* prevent userland from meddling with cpumask of workqueue workers */
+	worker->task->flags |= PF_NO_SETAFFINITY;
 
 	/*
-	 * Determine CPU binding of the new worker depending on
-	 * %POOL_DISASSOCIATED.  The caller is responsible for ensuring the
-	 * flag remains stable across this function.  See the comments
-	 * above the flag definition for details.
-	 *
-	 * As an unbound worker may later become a regular one if CPU comes
-	 * online, make sure every worker has %PF_THREAD_BOUND set.
+	 * The caller is responsible for ensuring %POOL_DISASSOCIATED
+	 * remains stable across this function.  See the comments above the
+	 * flag definition for details.
 	 */
-	if (!(pool->flags & POOL_DISASSOCIATED)) {
-		kthread_bind(worker->task, pool->cpu);
-	} else {
-		worker->task->flags |= PF_THREAD_BOUND;
+	if (pool->flags & POOL_DISASSOCIATED)
 		worker->flags |= WORKER_UNBOUND;
-	}
+
+	/* successful, commit the pointer to idr */
+	spin_lock_irq(&pool->lock);
+	idr_replace(&pool->worker_idr, worker, worker->id);
+	spin_unlock_irq(&pool->lock);
 
 	return worker;
+
 fail:
 	if (id >= 0) {
 		spin_lock_irq(&pool->lock);
-		ida_remove(&pool->worker_ida, id);
+		idr_remove(&pool->worker_idr, id);
 		spin_unlock_irq(&pool->lock);
 	}
 	kfree(worker);
@@ -1781,6 +1770,30 @@ static void start_worker(struct worker *worker)
 }
 
 /**
+ * create_and_start_worker - create and start a worker for a pool
+ * @pool: the target pool
+ *
+ * Grab the managership of @pool and create and start a new worker for it.
+ */
+static int create_and_start_worker(struct worker_pool *pool)
+{
+	struct worker *worker;
+
+	mutex_lock(&pool->manager_mutex);
+
+	worker = create_worker(pool);
+	if (worker) {
+		spin_lock_irq(&pool->lock);
+		start_worker(worker);
+		spin_unlock_irq(&pool->lock);
+	}
+
+	mutex_unlock(&pool->manager_mutex);
+
+	return worker ? 0 : -ENOMEM;
+}
+
+/**
  * destroy_worker - destroy a workqueue worker
  * @worker: worker to be destroyed
  *
@@ -1792,11 +1805,14 @@ static void start_worker(struct worker *worker)
 static void destroy_worker(struct worker *worker)
 {
 	struct worker_pool *pool = worker->pool;
-	int id = worker->id;
+
+	lockdep_assert_held(&pool->manager_mutex);
+	lockdep_assert_held(&pool->lock);
 
 	/* sanity check frenzy */
-	BUG_ON(worker->current_work);
-	BUG_ON(!list_empty(&worker->scheduled));
+	if (WARN_ON(worker->current_work) ||
+	    WARN_ON(!list_empty(&worker->scheduled)))
+		return;
 
 	if (worker->flags & WORKER_STARTED)
 		pool->nr_workers--;
@@ -1806,13 +1822,14 @@ static void destroy_worker(struct worker *worker)
 	list_del_init(&worker->entry);
 	worker->flags |= WORKER_DIE;
 
+	idr_remove(&pool->worker_idr, worker->id);
+
 	spin_unlock_irq(&pool->lock);
 
 	kthread_stop(worker->task);
 	kfree(worker);
 
 	spin_lock_irq(&pool->lock);
-	ida_remove(&pool->worker_ida, id);
 }
 
 static void idle_worker_timeout(unsigned long __pool)
@@ -1841,23 +1858,21 @@ static void idle_worker_timeout(unsigned long __pool)
 	spin_unlock_irq(&pool->lock);
 }
 
-static bool send_mayday(struct work_struct *work)
+static void send_mayday(struct work_struct *work)
 {
 	struct pool_workqueue *pwq = get_work_pwq(work);
 	struct workqueue_struct *wq = pwq->wq;
-	unsigned int cpu;
 
-	if (!(wq->flags & WQ_RESCUER))
-		return false;
+	lockdep_assert_held(&wq_mayday_lock);
+
+	if (!wq->rescuer)
+		return;
 
 	/* mayday mayday mayday */
-	cpu = pwq->pool->cpu;
-	/* WORK_CPU_UNBOUND can't be set in cpumask, use cpu 0 instead */
-	if (cpu == WORK_CPU_UNBOUND)
-		cpu = 0;
-	if (!mayday_test_and_set_cpu(cpu, wq->mayday_mask))
+	if (list_empty(&pwq->mayday_node)) {
+		list_add_tail(&pwq->mayday_node, &wq->maydays);
 		wake_up_process(wq->rescuer->task);
-	return true;
+	}
 }
 
 static void pool_mayday_timeout(unsigned long __pool)
@@ -1865,7 +1880,8 @@ static void pool_mayday_timeout(unsigned long __pool)
 	struct worker_pool *pool = (void *)__pool;
 	struct work_struct *work;
 
-	spin_lock_irq(&pool->lock);
+	spin_lock_irq(&wq_mayday_lock);		/* for wq->maydays */
+	spin_lock(&pool->lock);
 
 	if (need_to_create_worker(pool)) {
 		/*
@@ -1878,7 +1894,8 @@ static void pool_mayday_timeout(unsigned long __pool)
 			send_mayday(work);
 	}
 
-	spin_unlock_irq(&pool->lock);
+	spin_unlock(&pool->lock);
+	spin_unlock_irq(&wq_mayday_lock);
 
 	mod_timer(&pool->mayday_timer, jiffies + MAYDAY_INTERVAL);
 }
@@ -1893,8 +1910,8 @@ static void pool_mayday_timeout(unsigned long __pool)
  * sent to all rescuers with works scheduled on @pool to resolve
  * possible allocation deadlock.
  *
- * On return, need_to_create_worker() is guaranteed to be false and
- * may_start_working() true.
+ * On return, need_to_create_worker() is guaranteed to be %false and
+ * may_start_working() %true.
  *
  * LOCKING:
  * spin_lock_irq(pool->lock) which may be released and regrabbed
@@ -1902,7 +1919,7 @@ static void pool_mayday_timeout(unsigned long __pool)
  * manager.
  *
  * RETURNS:
- * false if no action was taken and pool->lock stayed locked, true
+ * %false if no action was taken and pool->lock stayed locked, %true
  * otherwise.
  */
 static bool maybe_create_worker(struct worker_pool *pool)
@@ -1925,7 +1942,8 @@ restart:
 			del_timer_sync(&pool->mayday_timer);
 			spin_lock_irq(&pool->lock);
 			start_worker(worker);
-			BUG_ON(need_to_create_worker(pool));
+			if (WARN_ON_ONCE(need_to_create_worker(pool)))
+				goto restart;
 			return true;
 		}
 
@@ -1958,7 +1976,7 @@ restart:
  * multiple times.  Called only from manager.
  *
  * RETURNS:
- * false if no action was taken and pool->lock stayed locked, true
+ * %false if no action was taken and pool->lock stayed locked, %true
  * otherwise.
  */
 static bool maybe_destroy_workers(struct worker_pool *pool)
@@ -2009,42 +2027,37 @@ static bool manage_workers(struct worker *worker)
 	struct worker_pool *pool = worker->pool;
 	bool ret = false;
 
-	if (pool->flags & POOL_MANAGING_WORKERS)
+	/*
+	 * Managership is governed by two mutexes - manager_arb and
+	 * manager_mutex.  manager_arb handles arbitration of manager role.
+	 * Anyone who successfully grabs manager_arb wins the arbitration
+	 * and becomes the manager.  mutex_trylock() on pool->manager_arb
+	 * failure while holding pool->lock reliably indicates that someone
+	 * else is managing the pool and the worker which failed trylock
+	 * can proceed to executing work items.  This means that anyone
+	 * grabbing manager_arb is responsible for actually performing
+	 * manager duties.  If manager_arb is grabbed and released without
+	 * actual management, the pool may stall indefinitely.
+	 *
+	 * manager_mutex is used for exclusion of actual management
+	 * operations.  The holder of manager_mutex can be sure that none
+	 * of management operations, including creation and destruction of
+	 * workers, won't take place until the mutex is released.  Because
+	 * manager_mutex doesn't interfere with manager role arbitration,
+	 * it is guaranteed that the pool's management, while may be
+	 * delayed, won't be disturbed by someone else grabbing
+	 * manager_mutex.
+	 */
+	if (!mutex_trylock(&pool->manager_arb))
 		return ret;
 
-	pool->flags |= POOL_MANAGING_WORKERS;
-
 	/*
-	 * To simplify both worker management and CPU hotplug, hold off
-	 * management while hotplug is in progress.  CPU hotplug path can't
-	 * grab %POOL_MANAGING_WORKERS to achieve this because that can
-	 * lead to idle worker depletion (all become busy thinking someone
-	 * else is managing) which in turn can result in deadlock under
-	 * extreme circumstances.  Use @pool->assoc_mutex to synchronize
-	 * manager against CPU hotplug.
-	 *
-	 * assoc_mutex would always be free unless CPU hotplug is in
-	 * progress.  trylock first without dropping @pool->lock.
+	 * With manager arbitration won, manager_mutex would be free in
+	 * most cases.  trylock first without dropping @pool->lock.
 	 */
-	if (unlikely(!mutex_trylock(&pool->assoc_mutex))) {
+	if (unlikely(!mutex_trylock(&pool->manager_mutex))) {
 		spin_unlock_irq(&pool->lock);
-		mutex_lock(&pool->assoc_mutex);
-		/*
-		 * CPU hotplug could have happened while we were waiting
-		 * for assoc_mutex.  Hotplug itself can't handle us
-		 * because manager isn't either on idle or busy list, and
-		 * @pool's state and ours could have deviated.
-		 *
-		 * As hotplug is now excluded via assoc_mutex, we can
-		 * simply try to bind.  It will succeed or fail depending
-		 * on @pool's current state.  Try it and adjust
-		 * %WORKER_UNBOUND accordingly.
-		 */
-		if (worker_maybe_bind_and_lock(worker))
-			worker->flags &= ~WORKER_UNBOUND;
-		else
-			worker->flags |= WORKER_UNBOUND;
-
+		mutex_lock(&pool->manager_mutex);
 		ret = true;
 	}
 
@@ -2057,8 +2070,8 @@ static bool manage_workers(struct worker *worker)
 	ret |= maybe_destroy_workers(pool);
 	ret |= maybe_create_worker(pool);
 
-	pool->flags &= ~POOL_MANAGING_WORKERS;
-	mutex_unlock(&pool->assoc_mutex);
+	mutex_unlock(&pool->manager_mutex);
+	mutex_unlock(&pool->manager_arb);
 	return ret;
 }
 
@@ -2212,11 +2225,11 @@ static void process_scheduled_works(struct worker *worker)
  * worker_thread - the worker thread function
  * @__worker: self
  *
- * The worker thread function.  There are NR_CPU_WORKER_POOLS dynamic pools
- * of these per each cpu.  These workers process all works regardless of
- * their specific target workqueue.  The only exception is works which
- * belong to workqueues with a rescuer which will be explained in
- * rescuer_thread().
+ * The worker thread function.  All workers belong to a worker_pool -
+ * either a per-cpu one or dynamic unbound one.  These workers process all
+ * work items regardless of their specific target workqueue.  The only
+ * exception is work items which belong to workqueues with a rescuer which
+ * will be explained in rescuer_thread().
  */
 static int worker_thread(void *__worker)
 {
@@ -2228,19 +2241,12 @@ static int worker_thread(void *__worker)
 woke_up:
 	spin_lock_irq(&pool->lock);
 
-	/* we are off idle list if destruction or rebind is requested */
-	if (unlikely(list_empty(&worker->entry))) {
+	/* am I supposed to die? */
+	if (unlikely(worker->flags & WORKER_DIE)) {
 		spin_unlock_irq(&pool->lock);
-
-		/* if DIE is set, destruction is requested */
-		if (worker->flags & WORKER_DIE) {
-			worker->task->flags &= ~PF_WQ_WORKER;
-			return 0;
-		}
-
-		/* otherwise, rebind */
-		idle_worker_rebind(worker);
-		goto woke_up;
+		WARN_ON_ONCE(!list_empty(&worker->entry));
+		worker->task->flags &= ~PF_WQ_WORKER;
+		return 0;
 	}
 
 	worker_leave_idle(worker);
@@ -2258,14 +2264,16 @@ recheck:
 	 * preparing to process a work or actually processing it.
 	 * Make sure nobody diddled with it while I was sleeping.
 	 */
-	BUG_ON(!list_empty(&worker->scheduled));
+	WARN_ON_ONCE(!list_empty(&worker->scheduled));
 
 	/*
-	 * When control reaches this point, we're guaranteed to have
-	 * at least one idle worker or that someone else has already
-	 * assumed the manager role.
+	 * Finish PREP stage.  We're guaranteed to have at least one idle
+	 * worker or that someone else has already assumed the manager
+	 * role.  This is where @worker starts participating in concurrency
+	 * management if applicable and concurrency management is restored
+	 * after being rebound.  See rebind_workers() for details.
 	 */
-	worker_clr_flags(worker, WORKER_PREP);
+	worker_clr_flags(worker, WORKER_PREP | WORKER_REBOUND);
 
 	do {
 		struct work_struct *work =
@@ -2307,7 +2315,7 @@ sleep:
  * @__rescuer: self
  *
  * Workqueue rescuer thread function.  There's one rescuer for each
- * workqueue which has WQ_RESCUER set.
+ * workqueue which has WQ_MEM_RECLAIM set.
  *
  * Regular work processing on a pool may block trying to create a new
  * worker which uses GFP_KERNEL allocation which has slight chance of
@@ -2326,8 +2334,6 @@ static int rescuer_thread(void *__rescuer)
 	struct worker *rescuer = __rescuer;
 	struct workqueue_struct *wq = rescuer->rescue_wq;
 	struct list_head *scheduled = &rescuer->scheduled;
-	bool is_unbound = wq->flags & WQ_UNBOUND;
-	unsigned int cpu;
 
 	set_user_nice(current, RESCUER_NICE_LEVEL);
 
@@ -2345,28 +2351,29 @@ repeat:
 		return 0;
 	}
 
-	/*
-	 * See whether any cpu is asking for help.  Unbounded
-	 * workqueues use cpu 0 in mayday_mask for CPU_UNBOUND.
-	 */
-	for_each_mayday_cpu(cpu, wq->mayday_mask) {
-		unsigned int tcpu = is_unbound ? WORK_CPU_UNBOUND : cpu;
-		struct pool_workqueue *pwq = get_pwq(tcpu, wq);
+	/* see whether any pwq is asking for help */
+	spin_lock_irq(&wq_mayday_lock);
+
+	while (!list_empty(&wq->maydays)) {
+		struct pool_workqueue *pwq = list_first_entry(&wq->maydays,
+					struct pool_workqueue, mayday_node);
 		struct worker_pool *pool = pwq->pool;
 		struct work_struct *work, *n;
 
 		__set_current_state(TASK_RUNNING);
-		mayday_clear_cpu(cpu, wq->mayday_mask);
+		list_del_init(&pwq->mayday_node);
+
+		spin_unlock_irq(&wq_mayday_lock);
 
 		/* migrate to the target cpu if possible */
+		worker_maybe_bind_and_lock(pool);
 		rescuer->pool = pool;
-		worker_maybe_bind_and_lock(rescuer);
 
 		/*
 		 * Slurp in all works issued via this workqueue and
 		 * process'em.
 		 */
-		BUG_ON(!list_empty(&rescuer->scheduled));
+		WARN_ON_ONCE(!list_empty(&rescuer->scheduled));
 		list_for_each_entry_safe(work, n, &pool->worklist, entry)
 			if (get_work_pwq(work) == pwq)
 				move_linked_works(work, scheduled, &n);
@@ -2381,9 +2388,13 @@ repeat:
 		if (keep_working(pool))
 			wake_up_worker(pool);
 
-		spin_unlock_irq(&pool->lock);
+		rescuer->pool = NULL;
+		spin_unlock(&pool->lock);
+		spin_lock(&wq_mayday_lock);
 	}
 
+	spin_unlock_irq(&wq_mayday_lock);
+
 	/* rescuers should never participate in concurrency management */
 	WARN_ON_ONCE(!(rescuer->flags & WORKER_NOT_RUNNING));
 	schedule();
@@ -2487,7 +2498,7 @@ static void insert_wq_barrier(struct pool_workqueue *pwq,
  * advanced to @work_color.
  *
  * CONTEXT:
- * mutex_lock(wq->flush_mutex).
+ * mutex_lock(wq->mutex).
  *
  * RETURNS:
  * %true if @flush_color >= 0 and there's something to flush.  %false
@@ -2497,21 +2508,20 @@ static bool flush_workqueue_prep_pwqs(struct workqueue_struct *wq,
 				      int flush_color, int work_color)
 {
 	bool wait = false;
-	unsigned int cpu;
+	struct pool_workqueue *pwq;
 
 	if (flush_color >= 0) {
-		BUG_ON(atomic_read(&wq->nr_pwqs_to_flush));
+		WARN_ON_ONCE(atomic_read(&wq->nr_pwqs_to_flush));
 		atomic_set(&wq->nr_pwqs_to_flush, 1);
 	}
 
-	for_each_pwq_cpu(cpu, wq) {
-		struct pool_workqueue *pwq = get_pwq(cpu, wq);
+	for_each_pwq(pwq, wq) {
 		struct worker_pool *pool = pwq->pool;
 
 		spin_lock_irq(&pool->lock);
 
 		if (flush_color >= 0) {
-			BUG_ON(pwq->flush_color != -1);
+			WARN_ON_ONCE(pwq->flush_color != -1);
 
 			if (pwq->nr_in_flight[flush_color]) {
 				pwq->flush_color = flush_color;
@@ -2521,7 +2531,7 @@ static bool flush_workqueue_prep_pwqs(struct workqueue_struct *wq,
 		}
 
 		if (work_color >= 0) {
-			BUG_ON(work_color != work_next_color(pwq->work_color));
+			WARN_ON_ONCE(work_color != work_next_color(pwq->work_color));
 			pwq->work_color = work_color;
 		}
 
@@ -2538,11 +2548,8 @@ static bool flush_workqueue_prep_pwqs(struct workqueue_struct *wq,
  * flush_workqueue - ensure that any scheduled work has run to completion.
  * @wq: workqueue to flush
  *
- * Forces execution of the workqueue and blocks until its completion.
- * This is typically used in driver shutdown handlers.
- *
- * We sleep until all works which were queued on entry have been handled,
- * but we are not livelocked by new incoming ones.
+ * This function sleeps until all work items which were queued on entry
+ * have finished execution, but it is not livelocked by new incoming ones.
  */
 void flush_workqueue(struct workqueue_struct *wq)
 {
@@ -2556,7 +2563,7 @@ void flush_workqueue(struct workqueue_struct *wq)
 	lock_map_acquire(&wq->lockdep_map);
 	lock_map_release(&wq->lockdep_map);
 
-	mutex_lock(&wq->flush_mutex);
+	mutex_lock(&wq->mutex);
 
 	/*
 	 * Start-to-wait phase
@@ -2569,13 +2576,13 @@ void flush_workqueue(struct workqueue_struct *wq)
 		 * becomes our flush_color and work_color is advanced
 		 * by one.
 		 */
-		BUG_ON(!list_empty(&wq->flusher_overflow));
+		WARN_ON_ONCE(!list_empty(&wq->flusher_overflow));
 		this_flusher.flush_color = wq->work_color;
 		wq->work_color = next_color;
 
 		if (!wq->first_flusher) {
 			/* no flush in progress, become the first flusher */
-			BUG_ON(wq->flush_color != this_flusher.flush_color);
+			WARN_ON_ONCE(wq->flush_color != this_flusher.flush_color);
 
 			wq->first_flusher = &this_flusher;
 
@@ -2588,7 +2595,7 @@ void flush_workqueue(struct workqueue_struct *wq)
 			}
 		} else {
 			/* wait in queue */
-			BUG_ON(wq->flush_color == this_flusher.flush_color);
+			WARN_ON_ONCE(wq->flush_color == this_flusher.flush_color);
 			list_add_tail(&this_flusher.list, &wq->flusher_queue);
 			flush_workqueue_prep_pwqs(wq, -1, wq->work_color);
 		}
@@ -2601,7 +2608,7 @@ void flush_workqueue(struct workqueue_struct *wq)
 		list_add_tail(&this_flusher.list, &wq->flusher_overflow);
 	}
 
-	mutex_unlock(&wq->flush_mutex);
+	mutex_unlock(&wq->mutex);
 
 	wait_for_completion(&this_flusher.done);
 
@@ -2614,7 +2621,7 @@ void flush_workqueue(struct workqueue_struct *wq)
 	if (wq->first_flusher != &this_flusher)
 		return;
 
-	mutex_lock(&wq->flush_mutex);
+	mutex_lock(&wq->mutex);
 
 	/* we might have raced, check again with mutex held */
 	if (wq->first_flusher != &this_flusher)
@@ -2622,8 +2629,8 @@ void flush_workqueue(struct workqueue_struct *wq)
 
 	wq->first_flusher = NULL;
 
-	BUG_ON(!list_empty(&this_flusher.list));
-	BUG_ON(wq->flush_color != this_flusher.flush_color);
+	WARN_ON_ONCE(!list_empty(&this_flusher.list));
+	WARN_ON_ONCE(wq->flush_color != this_flusher.flush_color);
 
 	while (true) {
 		struct wq_flusher *next, *tmp;
@@ -2636,8 +2643,8 @@ void flush_workqueue(struct workqueue_struct *wq)
 			complete(&next->done);
 		}
 
-		BUG_ON(!list_empty(&wq->flusher_overflow) &&
-		       wq->flush_color != work_next_color(wq->work_color));
+		WARN_ON_ONCE(!list_empty(&wq->flusher_overflow) &&
+			     wq->flush_color != work_next_color(wq->work_color));
 
 		/* this flush_color is finished, advance by one */
 		wq->flush_color = work_next_color(wq->flush_color);
@@ -2661,7 +2668,7 @@ void flush_workqueue(struct workqueue_struct *wq)
 		}
 
 		if (list_empty(&wq->flusher_queue)) {
-			BUG_ON(wq->flush_color != wq->work_color);
+			WARN_ON_ONCE(wq->flush_color != wq->work_color);
 			break;
 		}
 
@@ -2669,8 +2676,8 @@ void flush_workqueue(struct workqueue_struct *wq)
 		 * Need to flush more colors.  Make the next flusher
 		 * the new first flusher and arm pwqs.
 		 */
-		BUG_ON(wq->flush_color == wq->work_color);
-		BUG_ON(wq->flush_color != next->flush_color);
+		WARN_ON_ONCE(wq->flush_color == wq->work_color);
+		WARN_ON_ONCE(wq->flush_color != next->flush_color);
 
 		list_del_init(&next->list);
 		wq->first_flusher = next;
@@ -2686,7 +2693,7 @@ void flush_workqueue(struct workqueue_struct *wq)
 	}
 
 out_unlock:
-	mutex_unlock(&wq->flush_mutex);
+	mutex_unlock(&wq->mutex);
 }
 EXPORT_SYMBOL_GPL(flush_workqueue);
 
@@ -2704,22 +2711,23 @@ EXPORT_SYMBOL_GPL(flush_workqueue);
 void drain_workqueue(struct workqueue_struct *wq)
 {
 	unsigned int flush_cnt = 0;
-	unsigned int cpu;
+	struct pool_workqueue *pwq;
 
 	/*
 	 * __queue_work() needs to test whether there are drainers, is much
 	 * hotter than drain_workqueue() and already looks at @wq->flags.
-	 * Use WQ_DRAINING so that queue doesn't have to check nr_drainers.
+	 * Use __WQ_DRAINING so that queue doesn't have to check nr_drainers.
 	 */
-	spin_lock(&workqueue_lock);
+	mutex_lock(&wq->mutex);
 	if (!wq->nr_drainers++)
-		wq->flags |= WQ_DRAINING;
-	spin_unlock(&workqueue_lock);
+		wq->flags |= __WQ_DRAINING;
+	mutex_unlock(&wq->mutex);
 reflush:
 	flush_workqueue(wq);
 
-	for_each_pwq_cpu(cpu, wq) {
-		struct pool_workqueue *pwq = get_pwq(cpu, wq);
+	mutex_lock(&wq->mutex);
+
+	for_each_pwq(pwq, wq) {
 		bool drained;
 
 		spin_lock_irq(&pwq->pool->lock);
@@ -2731,15 +2739,16 @@ reflush:
 
 		if (++flush_cnt == 10 ||
 		    (flush_cnt % 100 == 0 && flush_cnt <= 1000))
-			pr_warn("workqueue %s: flush on destruction isn't complete after %u tries\n",
+			pr_warn("workqueue %s: drain_workqueue() isn't complete after %u tries\n",
 				wq->name, flush_cnt);
+
+		mutex_unlock(&wq->mutex);
 		goto reflush;
 	}
 
-	spin_lock(&workqueue_lock);
 	if (!--wq->nr_drainers)
-		wq->flags &= ~WQ_DRAINING;
-	spin_unlock(&workqueue_lock);
+		wq->flags &= ~__WQ_DRAINING;
+	mutex_unlock(&wq->mutex);
 }
 EXPORT_SYMBOL_GPL(drain_workqueue);
 
@@ -2750,11 +2759,15 @@ static bool start_flush_work(struct work_struct *work, struct wq_barrier *barr)
 	struct pool_workqueue *pwq;
 
 	might_sleep();
+
+	local_irq_disable();
 	pool = get_work_pool(work);
-	if (!pool)
+	if (!pool) {
+		local_irq_enable();
 		return false;
+	}
 
-	spin_lock_irq(&pool->lock);
+	spin_lock(&pool->lock);
 	/* see the comment in try_to_grab_pending() with the same code */
 	pwq = get_work_pwq(work);
 	if (pwq) {
@@ -2776,7 +2789,7 @@ static bool start_flush_work(struct work_struct *work, struct wq_barrier *barr)
 	 * flusher is not running on the same workqueue by verifying write
 	 * access.
 	 */
-	if (pwq->wq->saved_max_active == 1 || pwq->wq->flags & WQ_RESCUER)
+	if (pwq->wq->saved_max_active == 1 || pwq->wq->rescuer)
 		lock_map_acquire(&pwq->wq->lockdep_map);
 	else
 		lock_map_acquire_read(&pwq->wq->lockdep_map);
@@ -2933,66 +2946,6 @@ bool cancel_delayed_work_sync(struct delayed_work *dwork)
 EXPORT_SYMBOL(cancel_delayed_work_sync);
 
 /**
- * schedule_work_on - put work task on a specific cpu
- * @cpu: cpu to put the work task on
- * @work: job to be done
- *
- * This puts a job on a specific cpu
- */
-bool schedule_work_on(int cpu, struct work_struct *work)
-{
-	return queue_work_on(cpu, system_wq, work);
-}
-EXPORT_SYMBOL(schedule_work_on);
-
-/**
- * schedule_work - put work task in global workqueue
- * @work: job to be done
- *
- * Returns %false if @work was already on the kernel-global workqueue and
- * %true otherwise.
- *
- * This puts a job in the kernel-global workqueue if it was not already
- * queued and leaves it in the same position on the kernel-global
- * workqueue otherwise.
- */
-bool schedule_work(struct work_struct *work)
-{
-	return queue_work(system_wq, work);
-}
-EXPORT_SYMBOL(schedule_work);
-
-/**
- * schedule_delayed_work_on - queue work in global workqueue on CPU after delay
- * @cpu: cpu to use
- * @dwork: job to be done
- * @delay: number of jiffies to wait
- *
- * After waiting for a given time this puts a job in the kernel-global
- * workqueue on the specified CPU.
- */
-bool schedule_delayed_work_on(int cpu, struct delayed_work *dwork,
-			      unsigned long delay)
-{
-	return queue_delayed_work_on(cpu, system_wq, dwork, delay);
-}
-EXPORT_SYMBOL(schedule_delayed_work_on);
-
-/**
- * schedule_delayed_work - put work task in global workqueue after delay
- * @dwork: job to be done
- * @delay: number of jiffies to wait or 0 for immediate execution
- *
- * After waiting for a given time this puts a job in the kernel-global
- * workqueue.
- */
-bool schedule_delayed_work(struct delayed_work *dwork, unsigned long delay)
-{
-	return queue_delayed_work(system_wq, dwork, delay);
-}
-EXPORT_SYMBOL(schedule_delayed_work);
-
-/**
  * schedule_on_each_cpu - execute a function synchronously on each online CPU
  * @func: the function to call
  *
@@ -3085,51 +3038,1025 @@ int execute_in_process_context(work_func_t fn, struct execute_work *ew)
 }
 EXPORT_SYMBOL_GPL(execute_in_process_context);
 
-int keventd_up(void)
+#ifdef CONFIG_SYSFS
+/*
+ * Workqueues with WQ_SYSFS flag set is visible to userland via
+ * /sys/bus/workqueue/devices/WQ_NAME.  All visible workqueues have the
+ * following attributes.
+ *
+ *  per_cpu	RO bool	: whether the workqueue is per-cpu or unbound
+ *  max_active	RW int	: maximum number of in-flight work items
+ *
+ * Unbound workqueues have the following extra attributes.
+ *
+ *  id		RO int	: the associated pool ID
+ *  nice	RW int	: nice value of the workers
+ *  cpumask	RW mask	: bitmask of allowed CPUs for the workers
+ */
+struct wq_device {
+	struct workqueue_struct		*wq;
+	struct device			dev;
+};
+
+static struct workqueue_struct *dev_to_wq(struct device *dev)
+{
+	struct wq_device *wq_dev = container_of(dev, struct wq_device, dev);
+
+	return wq_dev->wq;
+}
+
+static ssize_t wq_per_cpu_show(struct device *dev,
+			       struct device_attribute *attr, char *buf)
+{
+	struct workqueue_struct *wq = dev_to_wq(dev);
+
+	return scnprintf(buf, PAGE_SIZE, "%d\n", (bool)!(wq->flags & WQ_UNBOUND));
+}
+
+static ssize_t wq_max_active_show(struct device *dev,
+				  struct device_attribute *attr, char *buf)
+{
+	struct workqueue_struct *wq = dev_to_wq(dev);
+
+	return scnprintf(buf, PAGE_SIZE, "%d\n", wq->saved_max_active);
+}
+
+static ssize_t wq_max_active_store(struct device *dev,
+				   struct device_attribute *attr,
+				   const char *buf, size_t count)
+{
+	struct workqueue_struct *wq = dev_to_wq(dev);
+	int val;
+
+	if (sscanf(buf, "%d", &val) != 1 || val <= 0)
+		return -EINVAL;
+
+	workqueue_set_max_active(wq, val);
+	return count;
+}
+
+static struct device_attribute wq_sysfs_attrs[] = {
+	__ATTR(per_cpu, 0444, wq_per_cpu_show, NULL),
+	__ATTR(max_active, 0644, wq_max_active_show, wq_max_active_store),
+	__ATTR_NULL,
+};
+
+static ssize_t wq_pool_ids_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct workqueue_struct *wq = dev_to_wq(dev);
+	const char *delim = "";
+	int node, written = 0;
+
+	rcu_read_lock_sched();
+	for_each_node(node) {
+		written += scnprintf(buf + written, PAGE_SIZE - written,
+				     "%s%d:%d", delim, node,
+				     unbound_pwq_by_node(wq, node)->pool->id);
+		delim = " ";
+	}
+	written += scnprintf(buf + written, PAGE_SIZE - written, "\n");
+	rcu_read_unlock_sched();
+
+	return written;
+}
+
+static ssize_t wq_nice_show(struct device *dev, struct device_attribute *attr,
+			    char *buf)
+{
+	struct workqueue_struct *wq = dev_to_wq(dev);
+	int written;
+
+	mutex_lock(&wq->mutex);
+	written = scnprintf(buf, PAGE_SIZE, "%d\n", wq->unbound_attrs->nice);
+	mutex_unlock(&wq->mutex);
+
+	return written;
+}
+
+/* prepare workqueue_attrs for sysfs store operations */
+static struct workqueue_attrs *wq_sysfs_prep_attrs(struct workqueue_struct *wq)
+{
+	struct workqueue_attrs *attrs;
+
+	attrs = alloc_workqueue_attrs(GFP_KERNEL);
+	if (!attrs)
+		return NULL;
+
+	mutex_lock(&wq->mutex);
+	copy_workqueue_attrs(attrs, wq->unbound_attrs);
+	mutex_unlock(&wq->mutex);
+	return attrs;
+}
+
+static ssize_t wq_nice_store(struct device *dev, struct device_attribute *attr,
+			     const char *buf, size_t count)
+{
+	struct workqueue_struct *wq = dev_to_wq(dev);
+	struct workqueue_attrs *attrs;
+	int ret;
+
+	attrs = wq_sysfs_prep_attrs(wq);
+	if (!attrs)
+		return -ENOMEM;
+
+	if (sscanf(buf, "%d", &attrs->nice) == 1 &&
+	    attrs->nice >= -20 && attrs->nice <= 19)
+		ret = apply_workqueue_attrs(wq, attrs);
+	else
+		ret = -EINVAL;
+
+	free_workqueue_attrs(attrs);
+	return ret ?: count;
+}
+
+static ssize_t wq_cpumask_show(struct device *dev,
+			       struct device_attribute *attr, char *buf)
 {
-	return system_wq != NULL;
+	struct workqueue_struct *wq = dev_to_wq(dev);
+	int written;
+
+	mutex_lock(&wq->mutex);
+	written = cpumask_scnprintf(buf, PAGE_SIZE, wq->unbound_attrs->cpumask);
+	mutex_unlock(&wq->mutex);
+
+	written += scnprintf(buf + written, PAGE_SIZE - written, "\n");
+	return written;
 }
 
-static int alloc_pwqs(struct workqueue_struct *wq)
+static ssize_t wq_cpumask_store(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf, size_t count)
 {
+	struct workqueue_struct *wq = dev_to_wq(dev);
+	struct workqueue_attrs *attrs;
+	int ret;
+
+	attrs = wq_sysfs_prep_attrs(wq);
+	if (!attrs)
+		return -ENOMEM;
+
+	ret = cpumask_parse(buf, attrs->cpumask);
+	if (!ret)
+		ret = apply_workqueue_attrs(wq, attrs);
+
+	free_workqueue_attrs(attrs);
+	return ret ?: count;
+}
+
+static ssize_t wq_numa_show(struct device *dev, struct device_attribute *attr,
+			    char *buf)
+{
+	struct workqueue_struct *wq = dev_to_wq(dev);
+	int written;
+
+	mutex_lock(&wq->mutex);
+	written = scnprintf(buf, PAGE_SIZE, "%d\n",
+			    !wq->unbound_attrs->no_numa);
+	mutex_unlock(&wq->mutex);
+
+	return written;
+}
+
+static ssize_t wq_numa_store(struct device *dev, struct device_attribute *attr,
+			     const char *buf, size_t count)
+{
+	struct workqueue_struct *wq = dev_to_wq(dev);
+	struct workqueue_attrs *attrs;
+	int v, ret;
+
+	attrs = wq_sysfs_prep_attrs(wq);
+	if (!attrs)
+		return -ENOMEM;
+
+	ret = -EINVAL;
+	if (sscanf(buf, "%d", &v) == 1) {
+		attrs->no_numa = !v;
+		ret = apply_workqueue_attrs(wq, attrs);
+	}
+
+	free_workqueue_attrs(attrs);
+	return ret ?: count;
+}
+
+static struct device_attribute wq_sysfs_unbound_attrs[] = {
+	__ATTR(pool_ids, 0444, wq_pool_ids_show, NULL),
+	__ATTR(nice, 0644, wq_nice_show, wq_nice_store),
+	__ATTR(cpumask, 0644, wq_cpumask_show, wq_cpumask_store),
+	__ATTR(numa, 0644, wq_numa_show, wq_numa_store),
+	__ATTR_NULL,
+};
+
+static struct bus_type wq_subsys = {
+	.name				= "workqueue",
+	.dev_attrs			= wq_sysfs_attrs,
+};
+
+static int __init wq_sysfs_init(void)
+{
+	return subsys_virtual_register(&wq_subsys, NULL);
+}
+core_initcall(wq_sysfs_init);
+
+static void wq_device_release(struct device *dev)
+{
+	struct wq_device *wq_dev = container_of(dev, struct wq_device, dev);
+
+	kfree(wq_dev);
+}
+
+/**
+ * workqueue_sysfs_register - make a workqueue visible in sysfs
+ * @wq: the workqueue to register
+ *
+ * Expose @wq in sysfs under /sys/bus/workqueue/devices.
+ * alloc_workqueue*() automatically calls this function if WQ_SYSFS is set
+ * which is the preferred method.
+ *
+ * Workqueue user should use this function directly iff it wants to apply
+ * workqueue_attrs before making the workqueue visible in sysfs; otherwise,
+ * apply_workqueue_attrs() may race against userland updating the
+ * attributes.
+ *
+ * Returns 0 on success, -errno on failure.
+ */
+int workqueue_sysfs_register(struct workqueue_struct *wq)
+{
+	struct wq_device *wq_dev;
+	int ret;
+
 	/*
-	 * pwqs are forced aligned according to WORK_STRUCT_FLAG_BITS.
-	 * Make sure that the alignment isn't lower than that of
-	 * unsigned long long.
+	 * Adjusting max_active or creating new pwqs by applyting
+	 * attributes breaks ordering guarantee.  Disallow exposing ordered
+	 * workqueues.
 	 */
-	const size_t size = sizeof(struct pool_workqueue);
-	const size_t align = max_t(size_t, 1 << WORK_STRUCT_FLAG_BITS,
-				   __alignof__(unsigned long long));
+	if (WARN_ON(wq->flags & __WQ_ORDERED))
+		return -EINVAL;
 
-	if (!(wq->flags & WQ_UNBOUND))
-		wq->pool_wq.pcpu = __alloc_percpu(size, align);
-	else {
-		void *ptr;
+	wq->wq_dev = wq_dev = kzalloc(sizeof(*wq_dev), GFP_KERNEL);
+	if (!wq_dev)
+		return -ENOMEM;
+
+	wq_dev->wq = wq;
+	wq_dev->dev.bus = &wq_subsys;
+	wq_dev->dev.init_name = wq->name;
+	wq_dev->dev.release = wq_device_release;
+
+	/*
+	 * unbound_attrs are created separately.  Suppress uevent until
+	 * everything is ready.
+	 */
+	dev_set_uevent_suppress(&wq_dev->dev, true);
+
+	ret = device_register(&wq_dev->dev);
+	if (ret) {
+		kfree(wq_dev);
+		wq->wq_dev = NULL;
+		return ret;
+	}
+
+	if (wq->flags & WQ_UNBOUND) {
+		struct device_attribute *attr;
+
+		for (attr = wq_sysfs_unbound_attrs; attr->attr.name; attr++) {
+			ret = device_create_file(&wq_dev->dev, attr);
+			if (ret) {
+				device_unregister(&wq_dev->dev);
+				wq->wq_dev = NULL;
+				return ret;
+			}
+		}
+	}
+
+	kobject_uevent(&wq_dev->dev.kobj, KOBJ_ADD);
+	return 0;
+}
+
+/**
+ * workqueue_sysfs_unregister - undo workqueue_sysfs_register()
+ * @wq: the workqueue to unregister
+ *
+ * If @wq is registered to sysfs by workqueue_sysfs_register(), unregister.
+ */
+static void workqueue_sysfs_unregister(struct workqueue_struct *wq)
+{
+	struct wq_device *wq_dev = wq->wq_dev;
+
+	if (!wq->wq_dev)
+		return;
+
+	wq->wq_dev = NULL;
+	device_unregister(&wq_dev->dev);
+}
+#else	/* CONFIG_SYSFS */
+static void workqueue_sysfs_unregister(struct workqueue_struct *wq)	{ }
+#endif	/* CONFIG_SYSFS */
+
+/**
+ * free_workqueue_attrs - free a workqueue_attrs
+ * @attrs: workqueue_attrs to free
+ *
+ * Undo alloc_workqueue_attrs().
+ */
+void free_workqueue_attrs(struct workqueue_attrs *attrs)
+{
+	if (attrs) {
+		free_cpumask_var(attrs->cpumask);
+		kfree(attrs);
+	}
+}
+
+/**
+ * alloc_workqueue_attrs - allocate a workqueue_attrs
+ * @gfp_mask: allocation mask to use
+ *
+ * Allocate a new workqueue_attrs, initialize with default settings and
+ * return it.  Returns NULL on failure.
+ */
+struct workqueue_attrs *alloc_workqueue_attrs(gfp_t gfp_mask)
+{
+	struct workqueue_attrs *attrs;
+
+	attrs = kzalloc(sizeof(*attrs), gfp_mask);
+	if (!attrs)
+		goto fail;
+	if (!alloc_cpumask_var(&attrs->cpumask, gfp_mask))
+		goto fail;
+
+	cpumask_copy(attrs->cpumask, cpu_possible_mask);
+	return attrs;
+fail:
+	free_workqueue_attrs(attrs);
+	return NULL;
+}
+
+static void copy_workqueue_attrs(struct workqueue_attrs *to,
+				 const struct workqueue_attrs *from)
+{
+	to->nice = from->nice;
+	cpumask_copy(to->cpumask, from->cpumask);
+}
+
+/* hash value of the content of @attr */
+static u32 wqattrs_hash(const struct workqueue_attrs *attrs)
+{
+	u32 hash = 0;
+
+	hash = jhash_1word(attrs->nice, hash);
+	hash = jhash(cpumask_bits(attrs->cpumask),
+		     BITS_TO_LONGS(nr_cpumask_bits) * sizeof(long), hash);
+	return hash;
+}
+
+/* content equality test */
+static bool wqattrs_equal(const struct workqueue_attrs *a,
+			  const struct workqueue_attrs *b)
+{
+	if (a->nice != b->nice)
+		return false;
+	if (!cpumask_equal(a->cpumask, b->cpumask))
+		return false;
+	return true;
+}
+
+/**
+ * init_worker_pool - initialize a newly zalloc'd worker_pool
+ * @pool: worker_pool to initialize
+ *
+ * Initiailize a newly zalloc'd @pool.  It also allocates @pool->attrs.
+ * Returns 0 on success, -errno on failure.  Even on failure, all fields
+ * inside @pool proper are initialized and put_unbound_pool() can be called
+ * on @pool safely to release it.
+ */
+static int init_worker_pool(struct worker_pool *pool)
+{
+	spin_lock_init(&pool->lock);
+	pool->id = -1;
+	pool->cpu = -1;
+	pool->node = NUMA_NO_NODE;
+	pool->flags |= POOL_DISASSOCIATED;
+	INIT_LIST_HEAD(&pool->worklist);
+	INIT_LIST_HEAD(&pool->idle_list);
+	hash_init(pool->busy_hash);
+
+	init_timer_deferrable(&pool->idle_timer);
+	pool->idle_timer.function = idle_worker_timeout;
+	pool->idle_timer.data = (unsigned long)pool;
+
+	setup_timer(&pool->mayday_timer, pool_mayday_timeout,
+		    (unsigned long)pool);
+
+	mutex_init(&pool->manager_arb);
+	mutex_init(&pool->manager_mutex);
+	idr_init(&pool->worker_idr);
+
+	INIT_HLIST_NODE(&pool->hash_node);
+	pool->refcnt = 1;
+
+	/* shouldn't fail above this point */
+	pool->attrs = alloc_workqueue_attrs(GFP_KERNEL);
+	if (!pool->attrs)
+		return -ENOMEM;
+	return 0;
+}
+
+static void rcu_free_pool(struct rcu_head *rcu)
+{
+	struct worker_pool *pool = container_of(rcu, struct worker_pool, rcu);
+
+	idr_destroy(&pool->worker_idr);
+	free_workqueue_attrs(pool->attrs);
+	kfree(pool);
+}
+
+/**
+ * put_unbound_pool - put a worker_pool
+ * @pool: worker_pool to put
+ *
+ * Put @pool.  If its refcnt reaches zero, it gets destroyed in sched-RCU
+ * safe manner.  get_unbound_pool() calls this function on its failure path
+ * and this function should be able to release pools which went through,
+ * successfully or not, init_worker_pool().
+ *
+ * Should be called with wq_pool_mutex held.
+ */
+static void put_unbound_pool(struct worker_pool *pool)
+{
+	struct worker *worker;
+
+	lockdep_assert_held(&wq_pool_mutex);
+
+	if (--pool->refcnt)
+		return;
+
+	/* sanity checks */
+	if (WARN_ON(!(pool->flags & POOL_DISASSOCIATED)) ||
+	    WARN_ON(!list_empty(&pool->worklist)))
+		return;
+
+	/* release id and unhash */
+	if (pool->id >= 0)
+		idr_remove(&worker_pool_idr, pool->id);
+	hash_del(&pool->hash_node);
+
+	/*
+	 * Become the manager and destroy all workers.  Grabbing
+	 * manager_arb prevents @pool's workers from blocking on
+	 * manager_mutex.
+	 */
+	mutex_lock(&pool->manager_arb);
+	mutex_lock(&pool->manager_mutex);
+	spin_lock_irq(&pool->lock);
+
+	while ((worker = first_worker(pool)))
+		destroy_worker(worker);
+	WARN_ON(pool->nr_workers || pool->nr_idle);
+
+	spin_unlock_irq(&pool->lock);
+	mutex_unlock(&pool->manager_mutex);
+	mutex_unlock(&pool->manager_arb);
+
+	/* shut down the timers */
+	del_timer_sync(&pool->idle_timer);
+	del_timer_sync(&pool->mayday_timer);
+
+	/* sched-RCU protected to allow dereferences from get_work_pool() */
+	call_rcu_sched(&pool->rcu, rcu_free_pool);
+}
+
+/**
+ * get_unbound_pool - get a worker_pool with the specified attributes
+ * @attrs: the attributes of the worker_pool to get
+ *
+ * Obtain a worker_pool which has the same attributes as @attrs, bump the
+ * reference count and return it.  If there already is a matching
+ * worker_pool, it will be used; otherwise, this function attempts to
+ * create a new one.  On failure, returns NULL.
+ *
+ * Should be called with wq_pool_mutex held.
+ */
+static struct worker_pool *get_unbound_pool(const struct workqueue_attrs *attrs)
+{
+	u32 hash = wqattrs_hash(attrs);
+	struct worker_pool *pool;
+	int node;
+
+	lockdep_assert_held(&wq_pool_mutex);
+
+	/* do we already have a matching pool? */
+	hash_for_each_possible(unbound_pool_hash, pool, hash_node, hash) {
+		if (wqattrs_equal(pool->attrs, attrs)) {
+			pool->refcnt++;
+			goto out_unlock;
+		}
+	}
+
+	/* nope, create a new one */
+	pool = kzalloc(sizeof(*pool), GFP_KERNEL);
+	if (!pool || init_worker_pool(pool) < 0)
+		goto fail;
+
+	if (workqueue_freezing)
+		pool->flags |= POOL_FREEZING;
+
+	lockdep_set_subclass(&pool->lock, 1);	/* see put_pwq() */
+	copy_workqueue_attrs(pool->attrs, attrs);
+
+	/* if cpumask is contained inside a NUMA node, we belong to that node */
+	if (wq_numa_enabled) {
+		for_each_node(node) {
+			if (cpumask_subset(pool->attrs->cpumask,
+					   wq_numa_possible_cpumask[node])) {
+				pool->node = node;
+				break;
+			}
+		}
+	}
+
+	if (worker_pool_assign_id(pool) < 0)
+		goto fail;
+
+	/* create and start the initial worker */
+	if (create_and_start_worker(pool) < 0)
+		goto fail;
+
+	/* install */
+	hash_add(unbound_pool_hash, &pool->hash_node, hash);
+out_unlock:
+	return pool;
+fail:
+	if (pool)
+		put_unbound_pool(pool);
+	return NULL;
+}
+
+static void rcu_free_pwq(struct rcu_head *rcu)
+{
+	kmem_cache_free(pwq_cache,
+			container_of(rcu, struct pool_workqueue, rcu));
+}
+
+/*
+ * Scheduled on system_wq by put_pwq() when an unbound pwq hits zero refcnt
+ * and needs to be destroyed.
+ */
+static void pwq_unbound_release_workfn(struct work_struct *work)
+{
+	struct pool_workqueue *pwq = container_of(work, struct pool_workqueue,
+						  unbound_release_work);
+	struct workqueue_struct *wq = pwq->wq;
+	struct worker_pool *pool = pwq->pool;
+	bool is_last;
+
+	if (WARN_ON_ONCE(!(wq->flags & WQ_UNBOUND)))
+		return;
+
+	/*
+	 * Unlink @pwq.  Synchronization against wq->mutex isn't strictly
+	 * necessary on release but do it anyway.  It's easier to verify
+	 * and consistent with the linking path.
+	 */
+	mutex_lock(&wq->mutex);
+	list_del_rcu(&pwq->pwqs_node);
+	is_last = list_empty(&wq->pwqs);
+	mutex_unlock(&wq->mutex);
+
+	mutex_lock(&wq_pool_mutex);
+	put_unbound_pool(pool);
+	mutex_unlock(&wq_pool_mutex);
+
+	call_rcu_sched(&pwq->rcu, rcu_free_pwq);
+
+	/*
+	 * If we're the last pwq going away, @wq is already dead and no one
+	 * is gonna access it anymore.  Free it.
+	 */
+	if (is_last) {
+		free_workqueue_attrs(wq->unbound_attrs);
+		kfree(wq);
+	}
+}
+
+/**
+ * pwq_adjust_max_active - update a pwq's max_active to the current setting
+ * @pwq: target pool_workqueue
+ *
+ * If @pwq isn't freezing, set @pwq->max_active to the associated
+ * workqueue's saved_max_active and activate delayed work items
+ * accordingly.  If @pwq is freezing, clear @pwq->max_active to zero.
+ */
+static void pwq_adjust_max_active(struct pool_workqueue *pwq)
+{
+	struct workqueue_struct *wq = pwq->wq;
+	bool freezable = wq->flags & WQ_FREEZABLE;
+
+	/* for @wq->saved_max_active */
+	lockdep_assert_held(&wq->mutex);
+
+	/* fast exit for non-freezable wqs */
+	if (!freezable && pwq->max_active == wq->saved_max_active)
+		return;
+
+	spin_lock_irq(&pwq->pool->lock);
+
+	if (!freezable || !(pwq->pool->flags & POOL_FREEZING)) {
+		pwq->max_active = wq->saved_max_active;
+
+		while (!list_empty(&pwq->delayed_works) &&
+		       pwq->nr_active < pwq->max_active)
+			pwq_activate_first_delayed(pwq);
 
 		/*
-		 * Allocate enough room to align pwq and put an extra
-		 * pointer at the end pointing back to the originally
-		 * allocated pointer which will be used for free.
+		 * Need to kick a worker after thawed or an unbound wq's
+		 * max_active is bumped.  It's a slow path.  Do it always.
 		 */
-		ptr = kzalloc(size + align + sizeof(void *), GFP_KERNEL);
-		if (ptr) {
-			wq->pool_wq.single = PTR_ALIGN(ptr, align);
-			*(void **)(wq->pool_wq.single + 1) = ptr;
+		wake_up_worker(pwq->pool);
+	} else {
+		pwq->max_active = 0;
+	}
+
+	spin_unlock_irq(&pwq->pool->lock);
+}
+
+/* initialize newly alloced @pwq which is associated with @wq and @pool */
+static void init_pwq(struct pool_workqueue *pwq, struct workqueue_struct *wq,
+		     struct worker_pool *pool)
+{
+	BUG_ON((unsigned long)pwq & WORK_STRUCT_FLAG_MASK);
+
+	memset(pwq, 0, sizeof(*pwq));
+
+	pwq->pool = pool;
+	pwq->wq = wq;
+	pwq->flush_color = -1;
+	pwq->refcnt = 1;
+	INIT_LIST_HEAD(&pwq->delayed_works);
+	INIT_LIST_HEAD(&pwq->pwqs_node);
+	INIT_LIST_HEAD(&pwq->mayday_node);
+	INIT_WORK(&pwq->unbound_release_work, pwq_unbound_release_workfn);
+}
+
+/* sync @pwq with the current state of its associated wq and link it */
+static void link_pwq(struct pool_workqueue *pwq)
+{
+	struct workqueue_struct *wq = pwq->wq;
+
+	lockdep_assert_held(&wq->mutex);
+
+	/* may be called multiple times, ignore if already linked */
+	if (!list_empty(&pwq->pwqs_node))
+		return;
+
+	/*
+	 * Set the matching work_color.  This is synchronized with
+	 * wq->mutex to avoid confusing flush_workqueue().
+	 */
+	pwq->work_color = wq->work_color;
+
+	/* sync max_active to the current setting */
+	pwq_adjust_max_active(pwq);
+
+	/* link in @pwq */
+	list_add_rcu(&pwq->pwqs_node, &wq->pwqs);
+}
+
+/* obtain a pool matching @attr and create a pwq associating the pool and @wq */
+static struct pool_workqueue *alloc_unbound_pwq(struct workqueue_struct *wq,
+					const struct workqueue_attrs *attrs)
+{
+	struct worker_pool *pool;
+	struct pool_workqueue *pwq;
+
+	lockdep_assert_held(&wq_pool_mutex);
+
+	pool = get_unbound_pool(attrs);
+	if (!pool)
+		return NULL;
+
+	pwq = kmem_cache_alloc_node(pwq_cache, GFP_KERNEL, pool->node);
+	if (!pwq) {
+		put_unbound_pool(pool);
+		return NULL;
+	}
+
+	init_pwq(pwq, wq, pool);
+	return pwq;
+}
+
+/* undo alloc_unbound_pwq(), used only in the error path */
+static void free_unbound_pwq(struct pool_workqueue *pwq)
+{
+	lockdep_assert_held(&wq_pool_mutex);
+
+	if (pwq) {
+		put_unbound_pool(pwq->pool);
+		kmem_cache_free(pwq_cache, pwq);
+	}
+}
+
+/**
+ * wq_calc_node_mask - calculate a wq_attrs' cpumask for the specified node
+ * @attrs: the wq_attrs of interest
+ * @node: the target NUMA node
+ * @cpu_going_down: if >= 0, the CPU to consider as offline
+ * @cpumask: outarg, the resulting cpumask
+ *
+ * Calculate the cpumask a workqueue with @attrs should use on @node.  If
+ * @cpu_going_down is >= 0, that cpu is considered offline during
+ * calculation.  The result is stored in @cpumask.  This function returns
+ * %true if the resulting @cpumask is different from @attrs->cpumask,
+ * %false if equal.
+ *
+ * If NUMA affinity is not enabled, @attrs->cpumask is always used.  If
+ * enabled and @node has online CPUs requested by @attrs, the returned
+ * cpumask is the intersection of the possible CPUs of @node and
+ * @attrs->cpumask.
+ *
+ * The caller is responsible for ensuring that the cpumask of @node stays
+ * stable.
+ */
+static bool wq_calc_node_cpumask(const struct workqueue_attrs *attrs, int node,
+				 int cpu_going_down, cpumask_t *cpumask)
+{
+	if (!wq_numa_enabled || attrs->no_numa)
+		goto use_dfl;
+
+	/* does @node have any online CPUs @attrs wants? */
+	cpumask_and(cpumask, cpumask_of_node(node), attrs->cpumask);
+	if (cpu_going_down >= 0)
+		cpumask_clear_cpu(cpu_going_down, cpumask);
+
+	if (cpumask_empty(cpumask))
+		goto use_dfl;
+
+	/* yeap, return possible CPUs in @node that @attrs wants */
+	cpumask_and(cpumask, attrs->cpumask, wq_numa_possible_cpumask[node]);
+	return !cpumask_equal(cpumask, attrs->cpumask);
+
+use_dfl:
+	cpumask_copy(cpumask, attrs->cpumask);
+	return false;
+}
+
+/* install @pwq into @wq's numa_pwq_tbl[] for @node and return the old pwq */
+static struct pool_workqueue *numa_pwq_tbl_install(struct workqueue_struct *wq,
+						   int node,
+						   struct pool_workqueue *pwq)
+{
+	struct pool_workqueue *old_pwq;
+
+	lockdep_assert_held(&wq->mutex);
+
+	/* link_pwq() can handle duplicate calls */
+	link_pwq(pwq);
+
+	old_pwq = rcu_access_pointer(wq->numa_pwq_tbl[node]);
+	rcu_assign_pointer(wq->numa_pwq_tbl[node], pwq);
+	return old_pwq;
+}
+
+/**
+ * apply_workqueue_attrs - apply new workqueue_attrs to an unbound workqueue
+ * @wq: the target workqueue
+ * @attrs: the workqueue_attrs to apply, allocated with alloc_workqueue_attrs()
+ *
+ * Apply @attrs to an unbound workqueue @wq.  Unless disabled, on NUMA
+ * machines, this function maps a separate pwq to each NUMA node with
+ * possibles CPUs in @attrs->cpumask so that work items are affine to the
+ * NUMA node it was issued on.  Older pwqs are released as in-flight work
+ * items finish.  Note that a work item which repeatedly requeues itself
+ * back-to-back will stay on its current pwq.
+ *
+ * Performs GFP_KERNEL allocations.  Returns 0 on success and -errno on
+ * failure.
+ */
+int apply_workqueue_attrs(struct workqueue_struct *wq,
+			  const struct workqueue_attrs *attrs)
+{
+	struct workqueue_attrs *new_attrs, *tmp_attrs;
+	struct pool_workqueue **pwq_tbl, *dfl_pwq;
+	int node, ret;
+
+	/* only unbound workqueues can change attributes */
+	if (WARN_ON(!(wq->flags & WQ_UNBOUND)))
+		return -EINVAL;
+
+	/* creating multiple pwqs breaks ordering guarantee */
+	if (WARN_ON((wq->flags & __WQ_ORDERED) && !list_empty(&wq->pwqs)))
+		return -EINVAL;
+
+	pwq_tbl = kzalloc(wq_numa_tbl_len * sizeof(pwq_tbl[0]), GFP_KERNEL);
+	new_attrs = alloc_workqueue_attrs(GFP_KERNEL);
+	tmp_attrs = alloc_workqueue_attrs(GFP_KERNEL);
+	if (!pwq_tbl || !new_attrs || !tmp_attrs)
+		goto enomem;
+
+	/* make a copy of @attrs and sanitize it */
+	copy_workqueue_attrs(new_attrs, attrs);
+	cpumask_and(new_attrs->cpumask, new_attrs->cpumask, cpu_possible_mask);
+
+	/*
+	 * We may create multiple pwqs with differing cpumasks.  Make a
+	 * copy of @new_attrs which will be modified and used to obtain
+	 * pools.
+	 */
+	copy_workqueue_attrs(tmp_attrs, new_attrs);
+
+	/*
+	 * CPUs should stay stable across pwq creations and installations.
+	 * Pin CPUs, determine the target cpumask for each node and create
+	 * pwqs accordingly.
+	 */
+	get_online_cpus();
+
+	mutex_lock(&wq_pool_mutex);
+
+	/*
+	 * If something goes wrong during CPU up/down, we'll fall back to
+	 * the default pwq covering whole @attrs->cpumask.  Always create
+	 * it even if we don't use it immediately.
+	 */
+	dfl_pwq = alloc_unbound_pwq(wq, new_attrs);
+	if (!dfl_pwq)
+		goto enomem_pwq;
+
+	for_each_node(node) {
+		if (wq_calc_node_cpumask(attrs, node, -1, tmp_attrs->cpumask)) {
+			pwq_tbl[node] = alloc_unbound_pwq(wq, tmp_attrs);
+			if (!pwq_tbl[node])
+				goto enomem_pwq;
+		} else {
+			dfl_pwq->refcnt++;
+			pwq_tbl[node] = dfl_pwq;
 		}
 	}
 
-	/* just in case, make sure it's actually aligned */
-	BUG_ON(!IS_ALIGNED(wq->pool_wq.v, align));
-	return wq->pool_wq.v ? 0 : -ENOMEM;
+	mutex_unlock(&wq_pool_mutex);
+
+	/* all pwqs have been created successfully, let's install'em */
+	mutex_lock(&wq->mutex);
+
+	copy_workqueue_attrs(wq->unbound_attrs, new_attrs);
+
+	/* save the previous pwq and install the new one */
+	for_each_node(node)
+		pwq_tbl[node] = numa_pwq_tbl_install(wq, node, pwq_tbl[node]);
+
+	/* @dfl_pwq might not have been used, ensure it's linked */
+	link_pwq(dfl_pwq);
+	swap(wq->dfl_pwq, dfl_pwq);
+
+	mutex_unlock(&wq->mutex);
+
+	/* put the old pwqs */
+	for_each_node(node)
+		put_pwq_unlocked(pwq_tbl[node]);
+	put_pwq_unlocked(dfl_pwq);
+
+	put_online_cpus();
+	ret = 0;
+	/* fall through */
+out_free:
+	free_workqueue_attrs(tmp_attrs);
+	free_workqueue_attrs(new_attrs);
+	kfree(pwq_tbl);
+	return ret;
+
+enomem_pwq:
+	free_unbound_pwq(dfl_pwq);
+	for_each_node(node)
+		if (pwq_tbl && pwq_tbl[node] != dfl_pwq)
+			free_unbound_pwq(pwq_tbl[node]);
+	mutex_unlock(&wq_pool_mutex);
+	put_online_cpus();
+enomem:
+	ret = -ENOMEM;
+	goto out_free;
 }
 
-static void free_pwqs(struct workqueue_struct *wq)
+/**
+ * wq_update_unbound_numa - update NUMA affinity of a wq for CPU hot[un]plug
+ * @wq: the target workqueue
+ * @cpu: the CPU coming up or going down
+ * @online: whether @cpu is coming up or going down
+ *
+ * This function is to be called from %CPU_DOWN_PREPARE, %CPU_ONLINE and
+ * %CPU_DOWN_FAILED.  @cpu is being hot[un]plugged, update NUMA affinity of
+ * @wq accordingly.
+ *
+ * If NUMA affinity can't be adjusted due to memory allocation failure, it
+ * falls back to @wq->dfl_pwq which may not be optimal but is always
+ * correct.
+ *
+ * Note that when the last allowed CPU of a NUMA node goes offline for a
+ * workqueue with a cpumask spanning multiple nodes, the workers which were
+ * already executing the work items for the workqueue will lose their CPU
+ * affinity and may execute on any CPU.  This is similar to how per-cpu
+ * workqueues behave on CPU_DOWN.  If a workqueue user wants strict
+ * affinity, it's the user's responsibility to flush the work item from
+ * CPU_DOWN_PREPARE.
+ */
+static void wq_update_unbound_numa(struct workqueue_struct *wq, int cpu,
+				   bool online)
 {
-	if (!(wq->flags & WQ_UNBOUND))
-		free_percpu(wq->pool_wq.pcpu);
-	else if (wq->pool_wq.single) {
-		/* the pointer to free is stored right after the pwq */
-		kfree(*(void **)(wq->pool_wq.single + 1));
+	int node = cpu_to_node(cpu);
+	int cpu_off = online ? -1 : cpu;
+	struct pool_workqueue *old_pwq = NULL, *pwq;
+	struct workqueue_attrs *target_attrs;
+	cpumask_t *cpumask;
+
+	lockdep_assert_held(&wq_pool_mutex);
+
+	if (!wq_numa_enabled || !(wq->flags & WQ_UNBOUND))
+		return;
+
+	/*
+	 * We don't wanna alloc/free wq_attrs for each wq for each CPU.
+	 * Let's use a preallocated one.  The following buf is protected by
+	 * CPU hotplug exclusion.
+	 */
+	target_attrs = wq_update_unbound_numa_attrs_buf;
+	cpumask = target_attrs->cpumask;
+
+	mutex_lock(&wq->mutex);
+	if (wq->unbound_attrs->no_numa)
+		goto out_unlock;
+
+	copy_workqueue_attrs(target_attrs, wq->unbound_attrs);
+	pwq = unbound_pwq_by_node(wq, node);
+
+	/*
+	 * Let's determine what needs to be done.  If the target cpumask is
+	 * different from wq's, we need to compare it to @pwq's and create
+	 * a new one if they don't match.  If the target cpumask equals
+	 * wq's, the default pwq should be used.  If @pwq is already the
+	 * default one, nothing to do; otherwise, install the default one.
+	 */
+	if (wq_calc_node_cpumask(wq->unbound_attrs, node, cpu_off, cpumask)) {
+		if (cpumask_equal(cpumask, pwq->pool->attrs->cpumask))
+			goto out_unlock;
+	} else {
+		if (pwq == wq->dfl_pwq)
+			goto out_unlock;
+		else
+			goto use_dfl_pwq;
+	}
+
+	mutex_unlock(&wq->mutex);
+
+	/* create a new pwq */
+	pwq = alloc_unbound_pwq(wq, target_attrs);
+	if (!pwq) {
+		pr_warning("workqueue: allocation failed while updating NUMA affinity of \"%s\"\n",
+			   wq->name);
+		goto out_unlock;
+	}
+
+	/*
+	 * Install the new pwq.  As this function is called only from CPU
+	 * hotplug callbacks and applying a new attrs is wrapped with
+	 * get/put_online_cpus(), @wq->unbound_attrs couldn't have changed
+	 * inbetween.
+	 */
+	mutex_lock(&wq->mutex);
+	old_pwq = numa_pwq_tbl_install(wq, node, pwq);
+	goto out_unlock;
+
+use_dfl_pwq:
+	spin_lock_irq(&wq->dfl_pwq->pool->lock);
+	get_pwq(wq->dfl_pwq);
+	spin_unlock_irq(&wq->dfl_pwq->pool->lock);
+	old_pwq = numa_pwq_tbl_install(wq, node, wq->dfl_pwq);
+out_unlock:
+	mutex_unlock(&wq->mutex);
+	put_pwq_unlocked(old_pwq);
+}
+
+static int alloc_and_link_pwqs(struct workqueue_struct *wq)
+{
+	bool highpri = wq->flags & WQ_HIGHPRI;
+	int cpu;
+
+	if (!(wq->flags & WQ_UNBOUND)) {
+		wq->cpu_pwqs = alloc_percpu(struct pool_workqueue);
+		if (!wq->cpu_pwqs)
+			return -ENOMEM;
+
+		for_each_possible_cpu(cpu) {
+			struct pool_workqueue *pwq =
+				per_cpu_ptr(wq->cpu_pwqs, cpu);
+			struct worker_pool *cpu_pools =
+				per_cpu(cpu_worker_pools, cpu);
+
+			init_pwq(pwq, wq, &cpu_pools[highpri]);
+
+			mutex_lock(&wq->mutex);
+			link_pwq(pwq);
+			mutex_unlock(&wq->mutex);
+		}
+		return 0;
+	} else {
+		return apply_workqueue_attrs(wq, unbound_std_wq_attrs[highpri]);
 	}
 }
 
@@ -3151,30 +4078,28 @@ struct workqueue_struct *__alloc_workqueue_key(const char *fmt,
 					       struct lock_class_key *key,
 					       const char *lock_name, ...)
 {
-	va_list args, args1;
+	size_t tbl_size = 0;
+	va_list args;
 	struct workqueue_struct *wq;
-	unsigned int cpu;
-	size_t namelen;
+	struct pool_workqueue *pwq;
 
-	/* determine namelen, allocate wq and format name */
-	va_start(args, lock_name);
-	va_copy(args1, args);
-	namelen = vsnprintf(NULL, 0, fmt, args) + 1;
+	/* allocate wq and format name */
+	if (flags & WQ_UNBOUND)
+		tbl_size = wq_numa_tbl_len * sizeof(wq->numa_pwq_tbl[0]);
 
-	wq = kzalloc(sizeof(*wq) + namelen, GFP_KERNEL);
+	wq = kzalloc(sizeof(*wq) + tbl_size, GFP_KERNEL);
 	if (!wq)
-		goto err;
+		return NULL;
 
-	vsnprintf(wq->name, namelen, fmt, args1);
-	va_end(args);
-	va_end(args1);
+	if (flags & WQ_UNBOUND) {
+		wq->unbound_attrs = alloc_workqueue_attrs(GFP_KERNEL);
+		if (!wq->unbound_attrs)
+			goto err_free_wq;
+	}
 
-	/*
-	 * Workqueues which may be used during memory reclaim should
-	 * have a rescuer to guarantee forward progress.
-	 */
-	if (flags & WQ_MEM_RECLAIM)
-		flags |= WQ_RESCUER;
+	va_start(args, lock_name);
+	vsnprintf(wq->name, sizeof(wq->name), fmt, args);
+	va_end(args);
 
 	max_active = max_active ?: WQ_DFL_ACTIVE;
 	max_active = wq_clamp_max_active(max_active, flags, wq->name);
@@ -3182,71 +4107,70 @@ struct workqueue_struct *__alloc_workqueue_key(const char *fmt,
 	/* init wq */
 	wq->flags = flags;
 	wq->saved_max_active = max_active;
-	mutex_init(&wq->flush_mutex);
+	mutex_init(&wq->mutex);
 	atomic_set(&wq->nr_pwqs_to_flush, 0);
+	INIT_LIST_HEAD(&wq->pwqs);
 	INIT_LIST_HEAD(&wq->flusher_queue);
 	INIT_LIST_HEAD(&wq->flusher_overflow);
+	INIT_LIST_HEAD(&wq->maydays);
 
 	lockdep_init_map(&wq->lockdep_map, lock_name, key, 0);
 	INIT_LIST_HEAD(&wq->list);
 
-	if (alloc_pwqs(wq) < 0)
-		goto err;
-
-	for_each_pwq_cpu(cpu, wq) {
-		struct pool_workqueue *pwq = get_pwq(cpu, wq);
+	if (alloc_and_link_pwqs(wq) < 0)
+		goto err_free_wq;
 
-		BUG_ON((unsigned long)pwq & WORK_STRUCT_FLAG_MASK);
-		pwq->pool = get_std_worker_pool(cpu, flags & WQ_HIGHPRI);
-		pwq->wq = wq;
-		pwq->flush_color = -1;
-		pwq->max_active = max_active;
-		INIT_LIST_HEAD(&pwq->delayed_works);
-	}
-
-	if (flags & WQ_RESCUER) {
+	/*
+	 * Workqueues which may be used during memory reclaim should
+	 * have a rescuer to guarantee forward progress.
+	 */
+	if (flags & WQ_MEM_RECLAIM) {
 		struct worker *rescuer;
 
-		if (!alloc_mayday_mask(&wq->mayday_mask, GFP_KERNEL))
-			goto err;
-
-		wq->rescuer = rescuer = alloc_worker();
+		rescuer = alloc_worker();
 		if (!rescuer)
-			goto err;
+			goto err_destroy;
 
 		rescuer->rescue_wq = wq;
 		rescuer->task = kthread_create(rescuer_thread, rescuer, "%s",
 					       wq->name);
-		if (IS_ERR(rescuer->task))
-			goto err;
+		if (IS_ERR(rescuer->task)) {
+			kfree(rescuer);
+			goto err_destroy;
+		}
 
-		rescuer->task->flags |= PF_THREAD_BOUND;
+		wq->rescuer = rescuer;
+		rescuer->task->flags |= PF_NO_SETAFFINITY;
 		wake_up_process(rescuer->task);
 	}
 
+	if ((wq->flags & WQ_SYSFS) && workqueue_sysfs_register(wq))
+		goto err_destroy;
+
 	/*
-	 * workqueue_lock protects global freeze state and workqueues
-	 * list.  Grab it, set max_active accordingly and add the new
-	 * workqueue to workqueues list.
+	 * wq_pool_mutex protects global freeze state and workqueues list.
+	 * Grab it, adjust max_active and add the new @wq to workqueues
+	 * list.
 	 */
-	spin_lock(&workqueue_lock);
+	mutex_lock(&wq_pool_mutex);
 
-	if (workqueue_freezing && wq->flags & WQ_FREEZABLE)
-		for_each_pwq_cpu(cpu, wq)
-			get_pwq(cpu, wq)->max_active = 0;
+	mutex_lock(&wq->mutex);
+	for_each_pwq(pwq, wq)
+		pwq_adjust_max_active(pwq);
+	mutex_unlock(&wq->mutex);
 
 	list_add(&wq->list, &workqueues);
 
-	spin_unlock(&workqueue_lock);
+	mutex_unlock(&wq_pool_mutex);
 
 	return wq;
-err:
-	if (wq) {
-		free_pwqs(wq);
-		free_mayday_mask(wq->mayday_mask);
-		kfree(wq->rescuer);
-		kfree(wq);
-	}
+
+err_free_wq:
+	free_workqueue_attrs(wq->unbound_attrs);
+	kfree(wq);
+	return NULL;
+err_destroy:
+	destroy_workqueue(wq);
 	return NULL;
 }
 EXPORT_SYMBOL_GPL(__alloc_workqueue_key);
@@ -3259,60 +4183,78 @@ EXPORT_SYMBOL_GPL(__alloc_workqueue_key);
  */
 void destroy_workqueue(struct workqueue_struct *wq)
 {
-	unsigned int cpu;
+	struct pool_workqueue *pwq;
+	int node;
 
 	/* drain it before proceeding with destruction */
 	drain_workqueue(wq);
 
+	/* sanity checks */
+	mutex_lock(&wq->mutex);
+	for_each_pwq(pwq, wq) {
+		int i;
+
+		for (i = 0; i < WORK_NR_COLORS; i++) {
+			if (WARN_ON(pwq->nr_in_flight[i])) {
+				mutex_unlock(&wq->mutex);
+				return;
+			}
+		}
+
+		if (WARN_ON((pwq != wq->dfl_pwq) && (pwq->refcnt > 1)) ||
+		    WARN_ON(pwq->nr_active) ||
+		    WARN_ON(!list_empty(&pwq->delayed_works))) {
+			mutex_unlock(&wq->mutex);
+			return;
+		}
+	}
+	mutex_unlock(&wq->mutex);
+
 	/*
 	 * wq list is used to freeze wq, remove from list after
 	 * flushing is complete in case freeze races us.
 	 */
-	spin_lock(&workqueue_lock);
-	list_del(&wq->list);
-	spin_unlock(&workqueue_lock);
+	mutex_lock(&wq_pool_mutex);
+	list_del_init(&wq->list);
+	mutex_unlock(&wq_pool_mutex);
 
-	/* sanity check */
-	for_each_pwq_cpu(cpu, wq) {
-		struct pool_workqueue *pwq = get_pwq(cpu, wq);
-		int i;
+	workqueue_sysfs_unregister(wq);
 
-		for (i = 0; i < WORK_NR_COLORS; i++)
-			BUG_ON(pwq->nr_in_flight[i]);
-		BUG_ON(pwq->nr_active);
-		BUG_ON(!list_empty(&pwq->delayed_works));
-	}
-
-	if (wq->flags & WQ_RESCUER) {
+	if (wq->rescuer) {
 		kthread_stop(wq->rescuer->task);
-		free_mayday_mask(wq->mayday_mask);
 		kfree(wq->rescuer);
+		wq->rescuer = NULL;
 	}
 
-	free_pwqs(wq);
-	kfree(wq);
-}
-EXPORT_SYMBOL_GPL(destroy_workqueue);
-
-/**
- * pwq_set_max_active - adjust max_active of a pwq
- * @pwq: target pool_workqueue
- * @max_active: new max_active value.
- *
- * Set @pwq->max_active to @max_active and activate delayed works if
- * increased.
- *
- * CONTEXT:
- * spin_lock_irq(pool->lock).
- */
-static void pwq_set_max_active(struct pool_workqueue *pwq, int max_active)
-{
-	pwq->max_active = max_active;
+	if (!(wq->flags & WQ_UNBOUND)) {
+		/*
+		 * The base ref is never dropped on per-cpu pwqs.  Directly
+		 * free the pwqs and wq.
+		 */
+		free_percpu(wq->cpu_pwqs);
+		kfree(wq);
+	} else {
+		/*
+		 * We're the sole accessor of @wq at this point.  Directly
+		 * access numa_pwq_tbl[] and dfl_pwq to put the base refs.
+		 * @wq will be freed when the last pwq is released.
+		 */
+		for_each_node(node) {
+			pwq = rcu_access_pointer(wq->numa_pwq_tbl[node]);
+			RCU_INIT_POINTER(wq->numa_pwq_tbl[node], NULL);
+			put_pwq_unlocked(pwq);
+		}
 
-	while (!list_empty(&pwq->delayed_works) &&
-	       pwq->nr_active < pwq->max_active)
-		pwq_activate_first_delayed(pwq);
+		/*
+		 * Put dfl_pwq.  @wq may be freed any time after dfl_pwq is
+		 * put.  Don't access it afterwards.
+		 */
+		pwq = wq->dfl_pwq;
+		wq->dfl_pwq = NULL;
+		put_pwq_unlocked(pwq);
+	}
 }
+EXPORT_SYMBOL_GPL(destroy_workqueue);
 
 /**
  * workqueue_set_max_active - adjust max_active of a workqueue
@@ -3326,30 +4268,37 @@ static void pwq_set_max_active(struct pool_workqueue *pwq, int max_active)
  */
 void workqueue_set_max_active(struct workqueue_struct *wq, int max_active)
 {
-	unsigned int cpu;
+	struct pool_workqueue *pwq;
+
+	/* disallow meddling with max_active for ordered workqueues */
+	if (WARN_ON(wq->flags & __WQ_ORDERED))
+		return;
 
 	max_active = wq_clamp_max_active(max_active, wq->flags, wq->name);
 
-	spin_lock(&workqueue_lock);
+	mutex_lock(&wq->mutex);
 
 	wq->saved_max_active = max_active;
 
-	for_each_pwq_cpu(cpu, wq) {
-		struct pool_workqueue *pwq = get_pwq(cpu, wq);
-		struct worker_pool *pool = pwq->pool;
-
-		spin_lock_irq(&pool->lock);
+	for_each_pwq(pwq, wq)
+		pwq_adjust_max_active(pwq);
 
-		if (!(wq->flags & WQ_FREEZABLE) ||
-		    !(pool->flags & POOL_FREEZING))
-			pwq_set_max_active(pwq, max_active);
+	mutex_unlock(&wq->mutex);
+}
+EXPORT_SYMBOL_GPL(workqueue_set_max_active);
 
-		spin_unlock_irq(&pool->lock);
-	}
+/**
+ * current_is_workqueue_rescuer - is %current workqueue rescuer?
+ *
+ * Determine whether %current is a workqueue rescuer.  Can be used from
+ * work functions to determine whether it's being run off the rescuer task.
+ */
+bool current_is_workqueue_rescuer(void)
+{
+	struct worker *worker = current_wq_worker();
 
-	spin_unlock(&workqueue_lock);
+	return worker && worker->rescue_wq;
 }
-EXPORT_SYMBOL_GPL(workqueue_set_max_active);
 
 /**
  * workqueue_congested - test whether a workqueue is congested
@@ -3363,11 +4312,22 @@ EXPORT_SYMBOL_GPL(workqueue_set_max_active);
  * RETURNS:
  * %true if congested, %false otherwise.
  */
-bool workqueue_congested(unsigned int cpu, struct workqueue_struct *wq)
+bool workqueue_congested(int cpu, struct workqueue_struct *wq)
 {
-	struct pool_workqueue *pwq = get_pwq(cpu, wq);
+	struct pool_workqueue *pwq;
+	bool ret;
 
-	return !list_empty(&pwq->delayed_works);
+	rcu_read_lock_sched();
+
+	if (!(wq->flags & WQ_UNBOUND))
+		pwq = per_cpu_ptr(wq->cpu_pwqs, cpu);
+	else
+		pwq = unbound_pwq_by_node(wq, cpu_to_node(cpu));
+
+	ret = !list_empty(&pwq->delayed_works);
+	rcu_read_unlock_sched();
+
+	return ret;
 }
 EXPORT_SYMBOL_GPL(workqueue_congested);
 
@@ -3384,19 +4344,22 @@ EXPORT_SYMBOL_GPL(workqueue_congested);
  */
 unsigned int work_busy(struct work_struct *work)
 {
-	struct worker_pool *pool = get_work_pool(work);
+	struct worker_pool *pool;
 	unsigned long flags;
 	unsigned int ret = 0;
 
 	if (work_pending(work))
 		ret |= WORK_BUSY_PENDING;
 
+	local_irq_save(flags);
+	pool = get_work_pool(work);
 	if (pool) {
-		spin_lock_irqsave(&pool->lock, flags);
+		spin_lock(&pool->lock);
 		if (find_worker_executing_work(pool, work))
 			ret |= WORK_BUSY_RUNNING;
-		spin_unlock_irqrestore(&pool->lock, flags);
+		spin_unlock(&pool->lock);
 	}
+	local_irq_restore(flags);
 
 	return ret;
 }
@@ -3422,31 +4385,28 @@ static void wq_unbind_fn(struct work_struct *work)
 	int cpu = smp_processor_id();
 	struct worker_pool *pool;
 	struct worker *worker;
-	int i;
+	int wi;
 
-	for_each_std_worker_pool(pool, cpu) {
-		BUG_ON(cpu != smp_processor_id());
+	for_each_cpu_worker_pool(pool, cpu) {
+		WARN_ON_ONCE(cpu != smp_processor_id());
 
-		mutex_lock(&pool->assoc_mutex);
+		mutex_lock(&pool->manager_mutex);
 		spin_lock_irq(&pool->lock);
 
 		/*
-		 * We've claimed all manager positions.  Make all workers
+		 * We've blocked all manager operations.  Make all workers
 		 * unbound and set DISASSOCIATED.  Before this, all workers
 		 * except for the ones which are still executing works from
 		 * before the last CPU down must be on the cpu.  After
 		 * this, they may become diasporas.
 		 */
-		list_for_each_entry(worker, &pool->idle_list, entry)
-			worker->flags |= WORKER_UNBOUND;
-
-		for_each_busy_worker(worker, i, pool)
+		for_each_pool_worker(worker, wi, pool)
 			worker->flags |= WORKER_UNBOUND;
 
 		pool->flags |= POOL_DISASSOCIATED;
 
 		spin_unlock_irq(&pool->lock);
-		mutex_unlock(&pool->assoc_mutex);
+		mutex_unlock(&pool->manager_mutex);
 
 		/*
 		 * Call schedule() so that we cross rq->lock and thus can
@@ -3477,6 +4437,103 @@ static void wq_unbind_fn(struct work_struct *work)
 	}
 }
 
+/**
+ * rebind_workers - rebind all workers of a pool to the associated CPU
+ * @pool: pool of interest
+ *
+ * @pool->cpu is coming online.  Rebind all workers to the CPU.
+ */
+static void rebind_workers(struct worker_pool *pool)
+{
+	struct worker *worker;
+	int wi;
+
+	lockdep_assert_held(&pool->manager_mutex);
+
+	/*
+	 * Restore CPU affinity of all workers.  As all idle workers should
+	 * be on the run-queue of the associated CPU before any local
+	 * wake-ups for concurrency management happen, restore CPU affinty
+	 * of all workers first and then clear UNBOUND.  As we're called
+	 * from CPU_ONLINE, the following shouldn't fail.
+	 */
+	for_each_pool_worker(worker, wi, pool)
+		WARN_ON_ONCE(set_cpus_allowed_ptr(worker->task,
+						  pool->attrs->cpumask) < 0);
+
+	spin_lock_irq(&pool->lock);
+
+	for_each_pool_worker(worker, wi, pool) {
+		unsigned int worker_flags = worker->flags;
+
+		/*
+		 * A bound idle worker should actually be on the runqueue
+		 * of the associated CPU for local wake-ups targeting it to
+		 * work.  Kick all idle workers so that they migrate to the
+		 * associated CPU.  Doing this in the same loop as
+		 * replacing UNBOUND with REBOUND is safe as no worker will
+		 * be bound before @pool->lock is released.
+		 */
+		if (worker_flags & WORKER_IDLE)
+			wake_up_process(worker->task);
+
+		/*
+		 * We want to clear UNBOUND but can't directly call
+		 * worker_clr_flags() or adjust nr_running.  Atomically
+		 * replace UNBOUND with another NOT_RUNNING flag REBOUND.
+		 * @worker will clear REBOUND using worker_clr_flags() when
+		 * it initiates the next execution cycle thus restoring
+		 * concurrency management.  Note that when or whether
+		 * @worker clears REBOUND doesn't affect correctness.
+		 *
+		 * ACCESS_ONCE() is necessary because @worker->flags may be
+		 * tested without holding any lock in
+		 * wq_worker_waking_up().  Without it, NOT_RUNNING test may
+		 * fail incorrectly leading to premature concurrency
+		 * management operations.
+		 */
+		WARN_ON_ONCE(!(worker_flags & WORKER_UNBOUND));
+		worker_flags |= WORKER_REBOUND;
+		worker_flags &= ~WORKER_UNBOUND;
+		ACCESS_ONCE(worker->flags) = worker_flags;
+	}
+
+	spin_unlock_irq(&pool->lock);
+}
+
+/**
+ * restore_unbound_workers_cpumask - restore cpumask of unbound workers
+ * @pool: unbound pool of interest
+ * @cpu: the CPU which is coming up
+ *
+ * An unbound pool may end up with a cpumask which doesn't have any online
+ * CPUs.  When a worker of such pool get scheduled, the scheduler resets
+ * its cpus_allowed.  If @cpu is in @pool's cpumask which didn't have any
+ * online CPU before, cpus_allowed of all its workers should be restored.
+ */
+static void restore_unbound_workers_cpumask(struct worker_pool *pool, int cpu)
+{
+	static cpumask_t cpumask;
+	struct worker *worker;
+	int wi;
+
+	lockdep_assert_held(&pool->manager_mutex);
+
+	/* is @cpu allowed for @pool? */
+	if (!cpumask_test_cpu(cpu, pool->attrs->cpumask))
+		return;
+
+	/* is @cpu the only online CPU? */
+	cpumask_and(&cpumask, pool->attrs->cpumask, cpu_online_mask);
+	if (cpumask_weight(&cpumask) != 1)
+		return;
+
+	/* as we're called from CPU_ONLINE, the following shouldn't fail */
+	for_each_pool_worker(worker, wi, pool)
+		WARN_ON_ONCE(set_cpus_allowed_ptr(worker->task,
+						  pool->attrs->cpumask) < 0);
+}
+
 /*
  * Workqueues should be brought up before normal priority CPU notifiers.
  * This will be registered high priority CPU notifier.
@@ -3485,39 +4542,46 @@ static int __cpuinit workqueue_cpu_up_callback(struct notifier_block *nfb,
 					       unsigned long action,
 					       void *hcpu)
 {
-	unsigned int cpu = (unsigned long)hcpu;
+	int cpu = (unsigned long)hcpu;
 	struct worker_pool *pool;
+	struct workqueue_struct *wq;
+	int pi;
 
 	switch (action & ~CPU_TASKS_FROZEN) {
 	case CPU_UP_PREPARE:
-		for_each_std_worker_pool(pool, cpu) {
-			struct worker *worker;
-
+		for_each_cpu_worker_pool(pool, cpu) {
 			if (pool->nr_workers)
 				continue;
-
-			worker = create_worker(pool);
-			if (!worker)
+			if (create_and_start_worker(pool) < 0)
 				return NOTIFY_BAD;
-
-			spin_lock_irq(&pool->lock);
-			start_worker(worker);
-			spin_unlock_irq(&pool->lock);
 		}
 		break;
 
 	case CPU_DOWN_FAILED:
 	case CPU_ONLINE:
-		for_each_std_worker_pool(pool, cpu) {
-			mutex_lock(&pool->assoc_mutex);
-			spin_lock_irq(&pool->lock);
+		mutex_lock(&wq_pool_mutex);
 
-			pool->flags &= ~POOL_DISASSOCIATED;
-			rebind_workers(pool);
+		for_each_pool(pool, pi) {
+			mutex_lock(&pool->manager_mutex);
+
+			if (pool->cpu == cpu) {
+				spin_lock_irq(&pool->lock);
+				pool->flags &= ~POOL_DISASSOCIATED;
+				spin_unlock_irq(&pool->lock);
+
+				rebind_workers(pool);
+			} else if (pool->cpu < 0) {
+				restore_unbound_workers_cpumask(pool, cpu);
+			}
 
-			spin_unlock_irq(&pool->lock);
-			mutex_unlock(&pool->assoc_mutex);
+			mutex_unlock(&pool->manager_mutex);
 		}
+
+		/* update NUMA affinity of unbound workqueues */
+		list_for_each_entry(wq, &workqueues, list)
+			wq_update_unbound_numa(wq, cpu, true);
+
+		mutex_unlock(&wq_pool_mutex);
 		break;
 	}
 	return NOTIFY_OK;
@@ -3531,14 +4595,23 @@ static int __cpuinit workqueue_cpu_down_callback(struct notifier_block *nfb,
 						 unsigned long action,
 						 void *hcpu)
 {
-	unsigned int cpu = (unsigned long)hcpu;
+	int cpu = (unsigned long)hcpu;
 	struct work_struct unbind_work;
+	struct workqueue_struct *wq;
 
 	switch (action & ~CPU_TASKS_FROZEN) {
 	case CPU_DOWN_PREPARE:
-		/* unbinding should happen on the local CPU */
+		/* unbinding per-cpu workers should happen on the local CPU */
 		INIT_WORK_ONSTACK(&unbind_work, wq_unbind_fn);
 		queue_work_on(cpu, system_highpri_wq, &unbind_work);
+
+		/* update NUMA affinity of unbound workqueues */
+		mutex_lock(&wq_pool_mutex);
+		list_for_each_entry(wq, &workqueues, list)
+			wq_update_unbound_numa(wq, cpu, false);
+		mutex_unlock(&wq_pool_mutex);
+
+		/* wait for per-cpu unbinding to finish */
 		flush_work(&unbind_work);
 		break;
 	}
@@ -3571,7 +4644,7 @@ static void work_for_cpu_fn(struct work_struct *work)
  * It is up to the caller to ensure that the cpu doesn't go offline.
  * The caller must not hold any locks which would prevent @fn from completing.
  */
-long work_on_cpu(unsigned int cpu, long (*fn)(void *), void *arg)
+long work_on_cpu(int cpu, long (*fn)(void *), void *arg)
 {
 	struct work_for_cpu wfc = { .fn = fn, .arg = arg };
 
@@ -3589,44 +4662,40 @@ EXPORT_SYMBOL_GPL(work_on_cpu);
  * freeze_workqueues_begin - begin freezing workqueues
  *
  * Start freezing workqueues.  After this function returns, all freezable
- * workqueues will queue new works to their frozen_works list instead of
+ * workqueues will queue new works to their delayed_works list instead of
  * pool->worklist.
  *
  * CONTEXT:
- * Grabs and releases workqueue_lock and pool->lock's.
+ * Grabs and releases wq_pool_mutex, wq->mutex and pool->lock's.
  */
 void freeze_workqueues_begin(void)
 {
-	unsigned int cpu;
+	struct worker_pool *pool;
+	struct workqueue_struct *wq;
+	struct pool_workqueue *pwq;
+	int pi;
 
-	spin_lock(&workqueue_lock);
+	mutex_lock(&wq_pool_mutex);
 
-	BUG_ON(workqueue_freezing);
+	WARN_ON_ONCE(workqueue_freezing);
 	workqueue_freezing = true;
 
-	for_each_wq_cpu(cpu) {
-		struct worker_pool *pool;
-		struct workqueue_struct *wq;
-
-		for_each_std_worker_pool(pool, cpu) {
-			spin_lock_irq(&pool->lock);
-
-			WARN_ON_ONCE(pool->flags & POOL_FREEZING);
-			pool->flags |= POOL_FREEZING;
-
-			list_for_each_entry(wq, &workqueues, list) {
-				struct pool_workqueue *pwq = get_pwq(cpu, wq);
-
-				if (pwq && pwq->pool == pool &&
-				    (wq->flags & WQ_FREEZABLE))
-					pwq->max_active = 0;
-			}
+	/* set FREEZING */
+	for_each_pool(pool, pi) {
+		spin_lock_irq(&pool->lock);
+		WARN_ON_ONCE(pool->flags & POOL_FREEZING);
+		pool->flags |= POOL_FREEZING;
+		spin_unlock_irq(&pool->lock);
+	}
 
-			spin_unlock_irq(&pool->lock);
-		}
+	list_for_each_entry(wq, &workqueues, list) {
+		mutex_lock(&wq->mutex);
+		for_each_pwq(pwq, wq)
+			pwq_adjust_max_active(pwq);
+		mutex_unlock(&wq->mutex);
 	}
 
-	spin_unlock(&workqueue_lock);
+	mutex_unlock(&wq_pool_mutex);
 }
 
 /**
@@ -3636,7 +4705,7 @@ void freeze_workqueues_begin(void)
  * between freeze_workqueues_begin() and thaw_workqueues().
  *
  * CONTEXT:
- * Grabs and releases workqueue_lock.
+ * Grabs and releases wq_pool_mutex.
  *
  * RETURNS:
  * %true if some freezable workqueues are still busy.  %false if freezing
@@ -3644,34 +4713,34 @@ void freeze_workqueues_begin(void)
  */
 bool freeze_workqueues_busy(void)
 {
-	unsigned int cpu;
 	bool busy = false;
+	struct workqueue_struct *wq;
+	struct pool_workqueue *pwq;
 
-	spin_lock(&workqueue_lock);
+	mutex_lock(&wq_pool_mutex);
 
-	BUG_ON(!workqueue_freezing);
+	WARN_ON_ONCE(!workqueue_freezing);
 
-	for_each_wq_cpu(cpu) {
-		struct workqueue_struct *wq;
+	list_for_each_entry(wq, &workqueues, list) {
+		if (!(wq->flags & WQ_FREEZABLE))
+			continue;
 		/*
 		 * nr_active is monotonically decreasing.  It's safe
 		 * to peek without lock.
 		 */
-		list_for_each_entry(wq, &workqueues, list) {
-			struct pool_workqueue *pwq = get_pwq(cpu, wq);
-
-			if (!pwq || !(wq->flags & WQ_FREEZABLE))
-				continue;
-
-			BUG_ON(pwq->nr_active < 0);
+		rcu_read_lock_sched();
+		for_each_pwq(pwq, wq) {
+			WARN_ON_ONCE(pwq->nr_active < 0);
 			if (pwq->nr_active) {
 				busy = true;
+				rcu_read_unlock_sched();
 				goto out_unlock;
 			}
 		}
+		rcu_read_unlock_sched();
 	}
 out_unlock:
-	spin_unlock(&workqueue_lock);
+	mutex_unlock(&wq_pool_mutex);
 	return busy;
 }
 
@@ -3682,104 +4751,141 @@ out_unlock:
  * frozen works are transferred to their respective pool worklists.
  *
  * CONTEXT:
- * Grabs and releases workqueue_lock and pool->lock's.
+ * Grabs and releases wq_pool_mutex, wq->mutex and pool->lock's.
  */
 void thaw_workqueues(void)
 {
-	unsigned int cpu;
+	struct workqueue_struct *wq;
+	struct pool_workqueue *pwq;
+	struct worker_pool *pool;
+	int pi;
 
-	spin_lock(&workqueue_lock);
+	mutex_lock(&wq_pool_mutex);
 
 	if (!workqueue_freezing)
 		goto out_unlock;
 
-	for_each_wq_cpu(cpu) {
-		struct worker_pool *pool;
-		struct workqueue_struct *wq;
+	/* clear FREEZING */
+	for_each_pool(pool, pi) {
+		spin_lock_irq(&pool->lock);
+		WARN_ON_ONCE(!(pool->flags & POOL_FREEZING));
+		pool->flags &= ~POOL_FREEZING;
+		spin_unlock_irq(&pool->lock);
+	}
 
-		for_each_std_worker_pool(pool, cpu) {
-			spin_lock_irq(&pool->lock);
+	/* restore max_active and repopulate worklist */
+	list_for_each_entry(wq, &workqueues, list) {
+		mutex_lock(&wq->mutex);
+		for_each_pwq(pwq, wq)
+			pwq_adjust_max_active(pwq);
+		mutex_unlock(&wq->mutex);
+	}
 
-			WARN_ON_ONCE(!(pool->flags & POOL_FREEZING));
-			pool->flags &= ~POOL_FREEZING;
+	workqueue_freezing = false;
+out_unlock:
+	mutex_unlock(&wq_pool_mutex);
+}
+#endif /* CONFIG_FREEZER */
 
-			list_for_each_entry(wq, &workqueues, list) {
-				struct pool_workqueue *pwq = get_pwq(cpu, wq);
+static void __init wq_numa_init(void)
+{
+	cpumask_var_t *tbl;
+	int node, cpu;
 
-				if (!pwq || pwq->pool != pool ||
-				    !(wq->flags & WQ_FREEZABLE))
-					continue;
+	/* determine NUMA pwq table len - highest node id + 1 */
+	for_each_node(node)
+		wq_numa_tbl_len = max(wq_numa_tbl_len, node + 1);
 
-				/* restore max_active and repopulate worklist */
-				pwq_set_max_active(pwq, wq->saved_max_active);
-			}
+	if (num_possible_nodes() <= 1)
+		return;
 
-			wake_up_worker(pool);
+	if (wq_disable_numa) {
+		pr_info("workqueue: NUMA affinity support disabled\n");
+		return;
+	}
+
+	wq_update_unbound_numa_attrs_buf = alloc_workqueue_attrs(GFP_KERNEL);
+	BUG_ON(!wq_update_unbound_numa_attrs_buf);
 
-			spin_unlock_irq(&pool->lock);
+	/*
+	 * We want masks of possible CPUs of each node which isn't readily
+	 * available.  Build one from cpu_to_node() which should have been
+	 * fully initialized by now.
+	 */
+	tbl = kzalloc(wq_numa_tbl_len * sizeof(tbl[0]), GFP_KERNEL);
+	BUG_ON(!tbl);
+
+	for_each_node(node)
+		BUG_ON(!alloc_cpumask_var_node(&tbl[node], GFP_KERNEL, node));
+
+	for_each_possible_cpu(cpu) {
+		node = cpu_to_node(cpu);
+		if (WARN_ON(node == NUMA_NO_NODE)) {
+			pr_warn("workqueue: NUMA node mapping not available for cpu%d, disabling NUMA support\n", cpu);
+			/* happens iff arch is bonkers, let's just proceed */
+			return;
 		}
+		cpumask_set_cpu(cpu, tbl[node]);
 	}
 
-	workqueue_freezing = false;
-out_unlock:
-	spin_unlock(&workqueue_lock);
+	wq_numa_possible_cpumask = tbl;
+	wq_numa_enabled = true;
 }
-#endif /* CONFIG_FREEZER */
 
 static int __init init_workqueues(void)
 {
-	unsigned int cpu;
+	int std_nice[NR_STD_WORKER_POOLS] = { 0, HIGHPRI_NICE_LEVEL };
+	int i, cpu;
 
 	/* make sure we have enough bits for OFFQ pool ID */
 	BUILD_BUG_ON((1LU << (BITS_PER_LONG - WORK_OFFQ_POOL_SHIFT)) <
 		     WORK_CPU_END * NR_STD_WORKER_POOLS);
 
+	WARN_ON(__alignof__(struct pool_workqueue) < __alignof__(long long));
+
+	pwq_cache = KMEM_CACHE(pool_workqueue, SLAB_PANIC);
+
 	cpu_notifier(workqueue_cpu_up_callback, CPU_PRI_WORKQUEUE_UP);
 	hotcpu_notifier(workqueue_cpu_down_callback, CPU_PRI_WORKQUEUE_DOWN);
 
+	wq_numa_init();
+
 	/* initialize CPU pools */
-	for_each_wq_cpu(cpu) {
+	for_each_possible_cpu(cpu) {
 		struct worker_pool *pool;
 
-		for_each_std_worker_pool(pool, cpu) {
-			spin_lock_init(&pool->lock);
+		i = 0;
+		for_each_cpu_worker_pool(pool, cpu) {
+			BUG_ON(init_worker_pool(pool));
 			pool->cpu = cpu;
-			pool->flags |= POOL_DISASSOCIATED;
-			INIT_LIST_HEAD(&pool->worklist);
-			INIT_LIST_HEAD(&pool->idle_list);
-			hash_init(pool->busy_hash);
-
-			init_timer_deferrable(&pool->idle_timer);
-			pool->idle_timer.function = idle_worker_timeout;
-			pool->idle_timer.data = (unsigned long)pool;
-
-			setup_timer(&pool->mayday_timer, pool_mayday_timeout,
-				    (unsigned long)pool);
-
-			mutex_init(&pool->assoc_mutex);
-			ida_init(&pool->worker_ida);
+			cpumask_copy(pool->attrs->cpumask, cpumask_of(cpu));
+			pool->attrs->nice = std_nice[i++];
+			pool->node = cpu_to_node(cpu);
 
 			/* alloc pool ID */
+			mutex_lock(&wq_pool_mutex);
 			BUG_ON(worker_pool_assign_id(pool));
+			mutex_unlock(&wq_pool_mutex);
 		}
 	}
 
 	/* create the initial worker */
-	for_each_online_wq_cpu(cpu) {
+	for_each_online_cpu(cpu) {
 		struct worker_pool *pool;
 
-		for_each_std_worker_pool(pool, cpu) {
-			struct worker *worker;
+		for_each_cpu_worker_pool(pool, cpu) {
+			pool->flags &= ~POOL_DISASSOCIATED;
+			BUG_ON(create_and_start_worker(pool) < 0);
+		}
+	}
 
-			if (cpu != WORK_CPU_UNBOUND)
-				pool->flags &= ~POOL_DISASSOCIATED;
+	/* create default unbound wq attrs */
+	for (i = 0; i < NR_STD_WORKER_POOLS; i++) {
+		struct workqueue_attrs *attrs;
 
-			worker = create_worker(pool);
-			BUG_ON(!worker);
-			spin_lock_irq(&pool->lock);
-			start_worker(worker);
-			spin_unlock_irq(&pool->lock);
-		}
+		BUG_ON(!(attrs = alloc_workqueue_attrs(GFP_KERNEL)));
+		attrs->nice = std_nice[i];
+		unbound_std_wq_attrs[i] = attrs;
 	}
 
 	system_wq = alloc_workqueue("events", 0, 0);
diff --git a/kernel/workqueue_internal.h b/kernel/workqueue_internal.h
index 07650264ec1..84ab6e1dc6f 100644
--- a/kernel/workqueue_internal.h
+++ b/kernel/workqueue_internal.h
@@ -32,14 +32,12 @@ struct worker {
 	struct list_head	scheduled;	/* L: scheduled works */
 	struct task_struct	*task;		/* I: worker task */
 	struct worker_pool	*pool;		/* I: the associated pool */
+						/* L: for rescuers */
 	/* 64 bytes boundary on 64bit, 32 on 32bit */
 	unsigned long		last_active;	/* L: last active timestamp */
 	unsigned int		flags;		/* X: flags */
 	int			id;		/* I: worker id */
 
-	/* for rebinding worker to CPU */
-	struct work_struct	rebind_work;	/* L: for busy worker */
-
 	/* used only by rescuers to point to the target workqueue */
 	struct workqueue_struct	*rescue_wq;	/* I: the workqueue to rescue */
 };
@@ -58,8 +56,7 @@ static inline struct worker *current_wq_worker(void)
  * Scheduler hooks for concurrency managed workqueue.  Only to be used from
  * sched.c and workqueue.c.
  */
-void wq_worker_waking_up(struct task_struct *task, unsigned int cpu);
-struct task_struct *wq_worker_sleeping(struct task_struct *task,
-				       unsigned int cpu);
+void wq_worker_waking_up(struct task_struct *task, int cpu);
+struct task_struct *wq_worker_sleeping(struct task_struct *task, int cpu);
 
 #endif /* _KERNEL_WORKQUEUE_INTERNAL_H */
diff --git a/lib/Kconfig b/lib/Kconfig
index 3958dc4389f..fe01d418b09 100644
--- a/lib/Kconfig
+++ b/lib/Kconfig
@@ -404,4 +404,7 @@ config OID_REGISTRY
 	help
 	  Enable fast lookup object identifier registry.
 
+config UCS2_STRING
+        tristate
+
 endmenu
diff --git a/lib/Makefile b/lib/Makefile
index d7946ff75b2..6e2cc561f76 100644
--- a/lib/Makefile
+++ b/lib/Makefile
@@ -174,3 +174,5 @@ quiet_cmd_build_OID_registry = GEN     $@
       cmd_build_OID_registry = perl $(srctree)/$(src)/build_OID_registry $< $@
 
 clean-files	+= oid_registry_data.c
+
+obj-$(CONFIG_UCS2_STRING) += ucs2_string.o
diff --git a/lib/argv_split.c b/lib/argv_split.c
index 1e9a6cbc368..e927ed0e18a 100644
--- a/lib/argv_split.c
+++ b/lib/argv_split.c
@@ -8,23 +8,17 @@
 #include <linux/slab.h>
 #include <linux/export.h>
 
-static const char *skip_arg(const char *cp)
-{
-	while (*cp && !isspace(*cp))
-		cp++;
-
-	return cp;
-}
-
 static int count_argc(const char *str)
 {
 	int count = 0;
+	bool was_space;
 
-	while (*str) {
-		str = skip_spaces(str);
-		if (*str) {
+	for (was_space = true; *str; str++) {
+		if (isspace(*str)) {
+			was_space = true;
+		} else if (was_space) {
+			was_space = false;
 			count++;
-			str = skip_arg(str);
 		}
 	}
 
@@ -39,10 +33,8 @@ static int count_argc(const char *str)
  */
 void argv_free(char **argv)
 {
-	char **p;
-	for (p = argv; *p; p++)
-		kfree(*p);
-
+	argv--;
+	kfree(argv[0]);
 	kfree(argv);
 }
 EXPORT_SYMBOL(argv_free);
@@ -59,43 +51,44 @@ EXPORT_SYMBOL(argv_free);
  * considered to be a single argument separator.  The returned array
  * is always NULL-terminated.  Returns NULL on memory allocation
  * failure.
+ *
+ * The source string at `str' may be undergoing concurrent alteration via
+ * userspace sysctl activity (at least).  The argv_split() implementation
+ * attempts to handle this gracefully by taking a local copy to work on.
  */
 char **argv_split(gfp_t gfp, const char *str, int *argcp)
 {
-	int argc = count_argc(str);
-	char **argv = kzalloc(sizeof(*argv) * (argc+1), gfp);
-	char **argvp;
-
-	if (argv == NULL)
-		goto out;
-
-	if (argcp)
-		*argcp = argc;
-
-	argvp = argv;
-
-	while (*str) {
-		str = skip_spaces(str);
-
-		if (*str) {
-			const char *p = str;
-			char *t;
-
-			str = skip_arg(str);
+	char *argv_str;
+	bool was_space;
+	char **argv, **argv_ret;
+	int argc;
+
+	argv_str = kstrndup(str, KMALLOC_MAX_SIZE - 1, gfp);
+	if (!argv_str)
+		return NULL;
+
+	argc = count_argc(argv_str);
+	argv = kmalloc(sizeof(*argv) * (argc + 2), gfp);
+	if (!argv) {
+		kfree(argv_str);
+		return NULL;
+	}
 
-			t = kstrndup(p, str-p, gfp);
-			if (t == NULL)
-				goto fail;
-			*argvp++ = t;
+	*argv = argv_str;
+	argv_ret = ++argv;
+	for (was_space = true; *argv_str; argv_str++) {
+		if (isspace(*argv_str)) {
+			was_space = true;
+			*argv_str = 0;
+		} else if (was_space) {
+			was_space = false;
+			*argv++ = argv_str;
 		}
 	}
-	*argvp = NULL;
-
-  out:
-	return argv;
+	*argv = NULL;
 
-  fail:
-	argv_free(argv);
-	return NULL;
+	if (argcp)
+		*argcp = argc;
+	return argv_ret;
 }
 EXPORT_SYMBOL(argv_split);
diff --git a/lib/div64.c b/lib/div64.c
index a163b6caef7..3af5728d95f 100644
--- a/lib/div64.c
+++ b/lib/div64.c
@@ -79,9 +79,10 @@ EXPORT_SYMBOL(div_s64_rem);
 #endif
 
 /**
- * div64_u64 - unsigned 64bit divide with 64bit divisor
+ * div64_u64_rem - unsigned 64bit divide with 64bit divisor and 64bit remainder
  * @dividend:	64bit dividend
  * @divisor:	64bit divisor
+ * @remainder:  64bit remainder
  *
  * This implementation is a modified version of the algorithm proposed
  * by the book 'Hacker's Delight'.  The original source and full proof
@@ -89,27 +90,33 @@ EXPORT_SYMBOL(div_s64_rem);
  *
  * 'http://www.hackersdelight.org/HDcode/newCode/divDouble.c.txt'
  */
-#ifndef div64_u64
-u64 div64_u64(u64 dividend, u64 divisor)
+#ifndef div64_u64_rem
+u64 div64_u64_rem(u64 dividend, u64 divisor, u64 *remainder)
 {
 	u32 high = divisor >> 32;
 	u64 quot;
 
 	if (high == 0) {
-		quot = div_u64(dividend, divisor);
+		u32 rem32;
+		quot = div_u64_rem(dividend, divisor, &rem32);
+		*remainder = rem32;
 	} else {
 		int n = 1 + fls(high);
 		quot = div_u64(dividend >> n, divisor >> n);
 
 		if (quot != 0)
 			quot--;
-		if ((dividend - quot * divisor) >= divisor)
+
+		*remainder = dividend - quot * divisor;
+		if (*remainder >= divisor) {
 			quot++;
+			*remainder -= divisor;
+		}
 	}
 
 	return quot;
 }
-EXPORT_SYMBOL(div64_u64);
+EXPORT_SYMBOL(div64_u64_rem);
 #endif
 
 /**
diff --git a/lib/dynamic_debug.c b/lib/dynamic_debug.c
index 5276b99ca65..46032453abd 100644
--- a/lib/dynamic_debug.c
+++ b/lib/dynamic_debug.c
@@ -281,7 +281,6 @@ static inline int parse_lineno(const char *str, unsigned int *val)
  * allow the user to express a query which matches a format
  * containing embedded spaces.
  */
-#define isodigit(c)		((c) >= '0' && (c) <= '7')
 static char *unescape(char *str)
 {
 	char *in = str;
diff --git a/lib/fault-inject.c b/lib/fault-inject.c
index f7210ad6cff..c5c7a762b85 100644
--- a/lib/fault-inject.c
+++ b/lib/fault-inject.c
@@ -122,7 +122,7 @@ bool should_fail(struct fault_attr *attr, ssize_t size)
 			return false;
 	}
 
-	if (attr->probability <= random32() % 100)
+	if (attr->probability <= prandom_u32() % 100)
 		return false;
 
 	if (!fail_stacktrace(attr))
diff --git a/lib/genalloc.c b/lib/genalloc.c
index 54920433705..b35cfa9bc3d 100644
--- a/lib/genalloc.c
+++ b/lib/genalloc.c
@@ -34,6 +34,8 @@
 #include <linux/rculist.h>
 #include <linux/interrupt.h>
 #include <linux/genalloc.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
 
 static int set_bits_ll(unsigned long *addr, unsigned long mask_to_set)
 {
@@ -480,3 +482,82 @@ unsigned long gen_pool_best_fit(unsigned long *map, unsigned long size,
 	return start_bit;
 }
 EXPORT_SYMBOL(gen_pool_best_fit);
+
+static void devm_gen_pool_release(struct device *dev, void *res)
+{
+	gen_pool_destroy(*(struct gen_pool **)res);
+}
+
+/**
+ * devm_gen_pool_create - managed gen_pool_create
+ * @dev: device that provides the gen_pool
+ * @min_alloc_order: log base 2 of number of bytes each bitmap bit represents
+ * @nid: node id of the node the pool structure should be allocated on, or -1
+ *
+ * Create a new special memory pool that can be used to manage special purpose
+ * memory not managed by the regular kmalloc/kfree interface. The pool will be
+ * automatically destroyed by the device management code.
+ */
+struct gen_pool *devm_gen_pool_create(struct device *dev, int min_alloc_order,
+		int nid)
+{
+	struct gen_pool **ptr, *pool;
+
+	ptr = devres_alloc(devm_gen_pool_release, sizeof(*ptr), GFP_KERNEL);
+
+	pool = gen_pool_create(min_alloc_order, nid);
+	if (pool) {
+		*ptr = pool;
+		devres_add(dev, ptr);
+	} else {
+		devres_free(ptr);
+	}
+
+	return pool;
+}
+
+/**
+ * dev_get_gen_pool - Obtain the gen_pool (if any) for a device
+ * @dev: device to retrieve the gen_pool from
+ * @name: Optional name for the gen_pool, usually NULL
+ *
+ * Returns the gen_pool for the device if one is present, or NULL.
+ */
+struct gen_pool *dev_get_gen_pool(struct device *dev)
+{
+	struct gen_pool **p = devres_find(dev, devm_gen_pool_release, NULL,
+					NULL);
+
+	if (!p)
+		return NULL;
+	return *p;
+}
+EXPORT_SYMBOL_GPL(dev_get_gen_pool);
+
+#ifdef CONFIG_OF
+/**
+ * of_get_named_gen_pool - find a pool by phandle property
+ * @np: device node
+ * @propname: property name containing phandle(s)
+ * @index: index into the phandle array
+ *
+ * Returns the pool that contains the chunk starting at the physical
+ * address of the device tree node pointed at by the phandle property,
+ * or NULL if not found.
+ */
+struct gen_pool *of_get_named_gen_pool(struct device_node *np,
+	const char *propname, int index)
+{
+	struct platform_device *pdev;
+	struct device_node *np_pool;
+
+	np_pool = of_parse_phandle(np, propname, index);
+	if (!np_pool)
+		return NULL;
+	pdev = of_find_device_by_node(np_pool);
+	if (!pdev)
+		return NULL;
+	return dev_get_gen_pool(&pdev->dev);
+}
+EXPORT_SYMBOL_GPL(of_get_named_gen_pool);
+#endif /* CONFIG_OF */
diff --git a/lib/idr.c b/lib/idr.c
index 322e2816f2f..cca4b9302a7 100644
--- a/lib/idr.c
+++ b/lib/idr.c
@@ -495,6 +495,33 @@ int idr_alloc(struct idr *idr, void *ptr, int start, int end, gfp_t gfp_mask)
 }
 EXPORT_SYMBOL_GPL(idr_alloc);
 
+/**
+ * idr_alloc_cyclic - allocate new idr entry in a cyclical fashion
+ * @idr: the (initialized) idr
+ * @ptr: pointer to be associated with the new id
+ * @start: the minimum id (inclusive)
+ * @end: the maximum id (exclusive, <= 0 for max)
+ * @gfp_mask: memory allocation flags
+ *
+ * Essentially the same as idr_alloc, but prefers to allocate progressively
+ * higher ids if it can. If the "cur" counter wraps, then it will start again
+ * at the "start" end of the range and allocate one that has already been used.
+ */
+int idr_alloc_cyclic(struct idr *idr, void *ptr, int start, int end,
+			gfp_t gfp_mask)
+{
+	int id;
+
+	id = idr_alloc(idr, ptr, max(start, idr->cur), end, gfp_mask);
+	if (id == -ENOSPC)
+		id = idr_alloc(idr, ptr, start, end, gfp_mask);
+
+	if (likely(id >= 0))
+		idr->cur = id + 1;
+	return id;
+}
+EXPORT_SYMBOL(idr_alloc_cyclic);
+
 static void idr_remove_warning(int id)
 {
 	printk(KERN_WARNING
diff --git a/lib/int_sqrt.c b/lib/int_sqrt.c
index fc2eeb7cb2e..1ef4cc34497 100644
--- a/lib/int_sqrt.c
+++ b/lib/int_sqrt.c
@@ -1,3 +1,9 @@
+/*
+ * Copyright (C) 2013 Davidlohr Bueso <davidlohr.bueso@hp.com>
+ *
+ *  Based on the shift-and-subtract algorithm for computing integer
+ *  square root from Guy L. Steele.
+ */
 
 #include <linux/kernel.h>
 #include <linux/export.h>
@@ -10,23 +16,23 @@
  */
 unsigned long int_sqrt(unsigned long x)
 {
-	unsigned long op, res, one;
+	unsigned long b, m, y = 0;
 
-	op = x;
-	res = 0;
+	if (x <= 1)
+		return x;
 
-	one = 1UL << (BITS_PER_LONG - 2);
-	while (one > op)
-		one >>= 2;
+	m = 1UL << (BITS_PER_LONG - 2);
+	while (m != 0) {
+		b = y + m;
+		y >>= 1;
 
-	while (one != 0) {
-		if (op >= res + one) {
-			op = op - (res + one);
-			res = res +  2 * one;
+		if (x >= b) {
+			x -= b;
+			y += m;
 		}
-		res /= 2;
-		one /= 4;
+		m >>= 2;
 	}
-	return res;
+
+	return y;
 }
 EXPORT_SYMBOL(int_sqrt);
diff --git a/lib/kobject.c b/lib/kobject.c
index e07ee1fcd6f..a65486613d7 100644
--- a/lib/kobject.c
+++ b/lib/kobject.c
@@ -529,6 +529,13 @@ struct kobject *kobject_get(struct kobject *kobj)
 	return kobj;
 }
 
+static struct kobject *kobject_get_unless_zero(struct kobject *kobj)
+{
+	if (!kref_get_unless_zero(&kobj->kref))
+		kobj = NULL;
+	return kobj;
+}
+
 /*
  * kobject_cleanup - free kobject resources.
  * @kobj: object to cleanup
@@ -751,7 +758,7 @@ struct kobject *kset_find_obj(struct kset *kset, const char *name)
 
 	list_for_each_entry(k, &kset->list, entry) {
 		if (kobject_name(k) && !strcmp(kobject_name(k), name)) {
-			ret = kobject_get(k);
+			ret = kobject_get_unless_zero(k);
 			break;
 		}
 	}
diff --git a/lib/list_sort.c b/lib/list_sort.c
index d7325c6b103..1183fa70a44 100644
--- a/lib/list_sort.c
+++ b/lib/list_sort.c
@@ -229,7 +229,7 @@ static int __init list_sort_test(void)
 			goto exit;
 		}
 		 /* force some equivalencies */
-		el->value = random32() % (TEST_LIST_LEN/3);
+		el->value = prandom_u32() % (TEST_LIST_LEN / 3);
 		el->serial = i;
 		el->poison1 = TEST_POISON1;
 		el->poison2 = TEST_POISON2;
diff --git a/lib/show_mem.c b/lib/show_mem.c
index 4407f8c9b1f..b7c72311ad0 100644
--- a/lib/show_mem.c
+++ b/lib/show_mem.c
@@ -18,6 +18,9 @@ void show_mem(unsigned int filter)
 	printk("Mem-Info:\n");
 	show_free_areas(filter);
 
+	if (filter & SHOW_MEM_FILTER_PAGE_COUNT)
+		return;
+
 	for_each_online_pgdat(pgdat) {
 		unsigned long i, flags;
 
diff --git a/lib/swiotlb.c b/lib/swiotlb.c
index bfe02b8fc55..d23762e6652 100644
--- a/lib/swiotlb.c
+++ b/lib/swiotlb.c
@@ -105,9 +105,9 @@ setup_io_tlb_npages(char *str)
 	if (!strcmp(str, "force"))
 		swiotlb_force = 1;
 
-	return 1;
+	return 0;
 }
-__setup("swiotlb=", setup_io_tlb_npages);
+early_param("swiotlb", setup_io_tlb_npages);
 /* make io_tlb_overflow tunable too? */
 
 unsigned long swiotlb_nr_tbl(void)
@@ -115,6 +115,18 @@ unsigned long swiotlb_nr_tbl(void)
 	return io_tlb_nslabs;
 }
 EXPORT_SYMBOL_GPL(swiotlb_nr_tbl);
+
+/* default to 64MB */
+#define IO_TLB_DEFAULT_SIZE (64UL<<20)
+unsigned long swiotlb_size_or_default(void)
+{
+	unsigned long size;
+
+	size = io_tlb_nslabs << IO_TLB_SHIFT;
+
+	return size ? size : (IO_TLB_DEFAULT_SIZE);
+}
+
 /* Note that this doesn't work with highmem page */
 static dma_addr_t swiotlb_virt_to_bus(struct device *hwdev,
 				      volatile void *address)
@@ -188,8 +200,7 @@ int __init swiotlb_init_with_tbl(char *tlb, unsigned long nslabs, int verbose)
 void  __init
 swiotlb_init(int verbose)
 {
-	/* default to 64MB */
-	size_t default_size = 64UL<<20;
+	size_t default_size = IO_TLB_DEFAULT_SIZE;
 	unsigned char *vstart;
 	unsigned long bytes;
 
diff --git a/lib/ucs2_string.c b/lib/ucs2_string.c
new file mode 100644
index 00000000000..6f500ef2301
--- /dev/null
+++ b/lib/ucs2_string.c
@@ -0,0 +1,51 @@
+#include <linux/ucs2_string.h>
+#include <linux/module.h>
+
+/* Return the number of unicode characters in data */
+unsigned long
+ucs2_strnlen(const ucs2_char_t *s, size_t maxlength)
+{
+        unsigned long length = 0;
+
+        while (*s++ != 0 && length < maxlength)
+                length++;
+        return length;
+}
+EXPORT_SYMBOL(ucs2_strnlen);
+
+unsigned long
+ucs2_strlen(const ucs2_char_t *s)
+{
+        return ucs2_strnlen(s, ~0UL);
+}
+EXPORT_SYMBOL(ucs2_strlen);
+
+/*
+ * Return the number of bytes is the length of this string
+ * Note: this is NOT the same as the number of unicode characters
+ */
+unsigned long
+ucs2_strsize(const ucs2_char_t *data, unsigned long maxlength)
+{
+        return ucs2_strnlen(data, maxlength/sizeof(ucs2_char_t)) * sizeof(ucs2_char_t);
+}
+EXPORT_SYMBOL(ucs2_strsize);
+
+int
+ucs2_strncmp(const ucs2_char_t *a, const ucs2_char_t *b, size_t len)
+{
+        while (1) {
+                if (len == 0)
+                        return 0;
+                if (*a < *b)
+                        return -1;
+                if (*a > *b)
+                        return 1;
+                if (*a == 0) /* implies *b == 0 */
+                        return 0;
+                a++;
+                b++;
+                len--;
+        }
+}
+EXPORT_SYMBOL(ucs2_strncmp);
diff --git a/lib/uuid.c b/lib/uuid.c
index 52a6fe6387d..398821e4dce 100644
--- a/lib/uuid.c
+++ b/lib/uuid.c
@@ -25,13 +25,7 @@
 
 static void __uuid_gen_common(__u8 b[16])
 {
-	int i;
-	u32 r;
-
-	for (i = 0; i < 4; i++) {
-		r = random32();
-		memcpy(b + i * 4, &r, 4);
-	}
+	prandom_bytes(b, 16);
 	/* reversion 0b10 */
 	b[8] = (b[8] & 0x3F) | 0x80;
 }
diff --git a/lib/vsprintf.c b/lib/vsprintf.c
index 0d62fd700f6..e149c641638 100644
--- a/lib/vsprintf.c
+++ b/lib/vsprintf.c
@@ -534,14 +534,21 @@ char *string(char *buf, char *end, const char *s, struct printf_spec spec)
 
 static noinline_for_stack
 char *symbol_string(char *buf, char *end, void *ptr,
-		    struct printf_spec spec, char ext)
+		    struct printf_spec spec, const char *fmt)
 {
-	unsigned long value = (unsigned long) ptr;
+	unsigned long value;
 #ifdef CONFIG_KALLSYMS
 	char sym[KSYM_SYMBOL_LEN];
-	if (ext == 'B')
+#endif
+
+	if (fmt[1] == 'R')
+		ptr = __builtin_extract_return_addr(ptr);
+	value = (unsigned long)ptr;
+
+#ifdef CONFIG_KALLSYMS
+	if (*fmt == 'B')
 		sprint_backtrace(sym, value);
-	else if (ext != 'f' && ext != 's')
+	else if (*fmt != 'f' && *fmt != 's')
 		sprint_symbol(sym, value);
 	else
 		sprint_symbol_no_offset(sym, value);
@@ -987,6 +994,7 @@ int kptr_restrict __read_mostly;
  * - 'f' For simple symbolic function names without offset
  * - 'S' For symbolic direct pointers with offset
  * - 's' For symbolic direct pointers without offset
+ * - '[FfSs]R' as above with __builtin_extract_return_addr() translation
  * - 'B' For backtraced symbolic direct pointers with offset
  * - 'R' For decoded struct resource, e.g., [mem 0x0-0x1f 64bit pref]
  * - 'r' For raw struct resource, e.g., [mem 0x0-0x1f flags 0x201]
@@ -1060,7 +1068,7 @@ char *pointer(const char *fmt, char *buf, char *end, void *ptr,
 	case 'S':
 	case 's':
 	case 'B':
-		return symbol_string(buf, end, ptr, spec, *fmt);
+		return symbol_string(buf, end, ptr, spec, fmt);
 	case 'R':
 	case 'r':
 		return resource_string(buf, end, ptr, spec, fmt);
diff --git a/mm/Kconfig b/mm/Kconfig
index 3bea74f1ccf..e742d06285b 100644
--- a/mm/Kconfig
+++ b/mm/Kconfig
@@ -263,8 +263,14 @@ config ZONE_DMA_FLAG
 	default "1"
 
 config BOUNCE
-	def_bool y
+	bool "Enable bounce buffers"
+	default y
 	depends on BLOCK && MMU && (ZONE_DMA || HIGHMEM)
+	help
+	  Enable bounce buffers for devices that cannot access
+	  the full range of memory available to the CPU. Enabled
+	  by default when ZONE_DMA or HIGHMEM is selected, but you
+	  may say n to override this.
 
 # On the 'tile' arch, USB OHCI needs the bounce pool since tilegx will often
 # have more than 4GB of memory, but we don't currently use the IOTLB to present
diff --git a/mm/Makefile b/mm/Makefile
index 3a4628751f8..72c5acb9345 100644
--- a/mm/Makefile
+++ b/mm/Makefile
@@ -50,7 +50,7 @@ obj-$(CONFIG_FS_XIP) += filemap_xip.o
 obj-$(CONFIG_MIGRATION) += migrate.o
 obj-$(CONFIG_QUICKLIST) += quicklist.o
 obj-$(CONFIG_TRANSPARENT_HUGEPAGE) += huge_memory.o
-obj-$(CONFIG_MEMCG) += memcontrol.o page_cgroup.o
+obj-$(CONFIG_MEMCG) += memcontrol.o page_cgroup.o vmpressure.o
 obj-$(CONFIG_CGROUP_HUGETLB) += hugetlb_cgroup.o
 obj-$(CONFIG_MEMORY_FAILURE) += memory-failure.o
 obj-$(CONFIG_HWPOISON_INJECT) += hwpoison-inject.o
diff --git a/mm/bounce.c b/mm/bounce.c
index 5f890176860..a5c2ec3589c 100644
--- a/mm/bounce.c
+++ b/mm/bounce.c
@@ -181,32 +181,13 @@ static void bounce_end_io_read_isa(struct bio *bio, int err)
 #ifdef CONFIG_NEED_BOUNCE_POOL
 static int must_snapshot_stable_pages(struct request_queue *q, struct bio *bio)
 {
-	struct page *page;
-	struct backing_dev_info *bdi;
-	struct address_space *mapping;
-	struct bio_vec *from;
-	int i;
-
 	if (bio_data_dir(bio) != WRITE)
 		return 0;
 
 	if (!bdi_cap_stable_pages_required(&q->backing_dev_info))
 		return 0;
 
-	/*
-	 * Based on the first page that has a valid mapping, decide whether or
-	 * not we have to employ bounce buffering to guarantee stable pages.
-	 */
-	bio_for_each_segment(from, bio, i) {
-		page = from->bv_page;
-		mapping = page_mapping(page);
-		if (!mapping)
-			continue;
-		bdi = mapping->backing_dev_info;
-		return mapping->host->i_sb->s_flags & MS_SNAP_STABLE;
-	}
-
-	return 0;
+	return test_bit(BIO_SNAP_STABLE, &bio->bi_flags);
 }
 #else
 static int must_snapshot_stable_pages(struct request_queue *q, struct bio *bio)
diff --git a/mm/filemap.c b/mm/filemap.c
index e1979fdca80..e989fb1eaa7 100644
--- a/mm/filemap.c
+++ b/mm/filemap.c
@@ -35,6 +35,9 @@
 #include <linux/cleancache.h>
 #include "internal.h"
 
+#define CREATE_TRACE_POINTS
+#include <trace/events/filemap.h>
+
 /*
  * FIXME: remove all knowledge of the buffer layer from the core VM
  */
@@ -113,6 +116,7 @@ void __delete_from_page_cache(struct page *page)
 {
 	struct address_space *mapping = page->mapping;
 
+	trace_mm_filemap_delete_from_page_cache(page);
 	/*
 	 * if we're uptodate, flush out into the cleancache, otherwise
 	 * invalidate any existing cleancache entries.  We can't leave
@@ -184,6 +188,17 @@ static int sleep_on_page_killable(void *word)
 	return fatal_signal_pending(current) ? -EINTR : 0;
 }
 
+static int filemap_check_errors(struct address_space *mapping)
+{
+	int ret = 0;
+	/* Check for outstanding write errors */
+	if (test_and_clear_bit(AS_ENOSPC, &mapping->flags))
+		ret = -ENOSPC;
+	if (test_and_clear_bit(AS_EIO, &mapping->flags))
+		ret = -EIO;
+	return ret;
+}
+
 /**
  * __filemap_fdatawrite_range - start writeback on mapping dirty pages in range
  * @mapping:	address space structure to write
@@ -265,10 +280,10 @@ int filemap_fdatawait_range(struct address_space *mapping, loff_t start_byte,
 	pgoff_t end = end_byte >> PAGE_CACHE_SHIFT;
 	struct pagevec pvec;
 	int nr_pages;
-	int ret = 0;
+	int ret2, ret = 0;
 
 	if (end_byte < start_byte)
-		return 0;
+		goto out;
 
 	pagevec_init(&pvec, 0);
 	while ((index <= end) &&
@@ -291,12 +306,10 @@ int filemap_fdatawait_range(struct address_space *mapping, loff_t start_byte,
 		pagevec_release(&pvec);
 		cond_resched();
 	}
-
-	/* Check for outstanding write errors */
-	if (test_and_clear_bit(AS_ENOSPC, &mapping->flags))
-		ret = -ENOSPC;
-	if (test_and_clear_bit(AS_EIO, &mapping->flags))
-		ret = -EIO;
+out:
+	ret2 = filemap_check_errors(mapping);
+	if (!ret)
+		ret = ret2;
 
 	return ret;
 }
@@ -337,6 +350,8 @@ int filemap_write_and_wait(struct address_space *mapping)
 			if (!err)
 				err = err2;
 		}
+	} else {
+		err = filemap_check_errors(mapping);
 	}
 	return err;
 }
@@ -368,6 +383,8 @@ int filemap_write_and_wait_range(struct address_space *mapping,
 			if (!err)
 				err = err2;
 		}
+	} else {
+		err = filemap_check_errors(mapping);
 	}
 	return err;
 }
@@ -464,6 +481,7 @@ int add_to_page_cache_locked(struct page *page, struct address_space *mapping,
 			mapping->nrpages++;
 			__inc_zone_page_state(page, NR_FILE_PAGES);
 			spin_unlock_irq(&mapping->tree_lock);
+			trace_mm_filemap_add_to_page_cache(page);
 		} else {
 			page->mapping = NULL;
 			/* Leave page->index set: truncation relies upon it */
diff --git a/mm/fremap.c b/mm/fremap.c
index 4723ac8d2fc..87da3590c61 100644
--- a/mm/fremap.c
+++ b/mm/fremap.c
@@ -204,10 +204,8 @@ get_write_lock:
 			unsigned long addr;
 			struct file *file = get_file(vma->vm_file);
 
-			vm_flags = vma->vm_flags;
-			if (!(flags & MAP_NONBLOCK))
-				vm_flags |= VM_POPULATE;
-			addr = mmap_region(file, start, size, vm_flags, pgoff);
+			addr = mmap_region(file, start, size,
+					vma->vm_flags, pgoff);
 			fput(file);
 			if (IS_ERR_VALUE(addr)) {
 				err = addr;
@@ -226,12 +224,6 @@ get_write_lock:
 		mutex_unlock(&mapping->i_mmap_mutex);
 	}
 
-	if (!(flags & MAP_NONBLOCK) && !(vma->vm_flags & VM_POPULATE)) {
-		if (!has_write_lock)
-			goto get_write_lock;
-		vma->vm_flags |= VM_POPULATE;
-	}
-
 	if (vma->vm_flags & VM_LOCKED) {
 		/*
 		 * drop PG_Mlocked flag for over-mapped range
diff --git a/mm/huge_memory.c b/mm/huge_memory.c
index e2f7f5aaaaf..03a89a2f464 100644
--- a/mm/huge_memory.c
+++ b/mm/huge_memory.c
@@ -163,35 +163,34 @@ static int start_khugepaged(void)
 }
 
 static atomic_t huge_zero_refcount;
-static unsigned long huge_zero_pfn __read_mostly;
+static struct page *huge_zero_page __read_mostly;
 
-static inline bool is_huge_zero_pfn(unsigned long pfn)
+static inline bool is_huge_zero_page(struct page *page)
 {
-	unsigned long zero_pfn = ACCESS_ONCE(huge_zero_pfn);
-	return zero_pfn && pfn == zero_pfn;
+	return ACCESS_ONCE(huge_zero_page) == page;
 }
 
 static inline bool is_huge_zero_pmd(pmd_t pmd)
 {
-	return is_huge_zero_pfn(pmd_pfn(pmd));
+	return is_huge_zero_page(pmd_page(pmd));
 }
 
-static unsigned long get_huge_zero_page(void)
+static struct page *get_huge_zero_page(void)
 {
 	struct page *zero_page;
 retry:
 	if (likely(atomic_inc_not_zero(&huge_zero_refcount)))
-		return ACCESS_ONCE(huge_zero_pfn);
+		return ACCESS_ONCE(huge_zero_page);
 
 	zero_page = alloc_pages((GFP_TRANSHUGE | __GFP_ZERO) & ~__GFP_MOVABLE,
 			HPAGE_PMD_ORDER);
 	if (!zero_page) {
 		count_vm_event(THP_ZERO_PAGE_ALLOC_FAILED);
-		return 0;
+		return NULL;
 	}
 	count_vm_event(THP_ZERO_PAGE_ALLOC);
 	preempt_disable();
-	if (cmpxchg(&huge_zero_pfn, 0, page_to_pfn(zero_page))) {
+	if (cmpxchg(&huge_zero_page, NULL, zero_page)) {
 		preempt_enable();
 		__free_page(zero_page);
 		goto retry;
@@ -200,7 +199,7 @@ retry:
 	/* We take additional reference here. It will be put back by shrinker */
 	atomic_set(&huge_zero_refcount, 2);
 	preempt_enable();
-	return ACCESS_ONCE(huge_zero_pfn);
+	return ACCESS_ONCE(huge_zero_page);
 }
 
 static void put_huge_zero_page(void)
@@ -220,9 +219,9 @@ static int shrink_huge_zero_page(struct shrinker *shrink,
 		return atomic_read(&huge_zero_refcount) == 1 ? HPAGE_PMD_NR : 0;
 
 	if (atomic_cmpxchg(&huge_zero_refcount, 1, 0) == 1) {
-		unsigned long zero_pfn = xchg(&huge_zero_pfn, 0);
-		BUG_ON(zero_pfn == 0);
-		__free_page(__pfn_to_page(zero_pfn));
+		struct page *zero_page = xchg(&huge_zero_page, NULL);
+		BUG_ON(zero_page == NULL);
+		__free_page(zero_page);
 	}
 
 	return 0;
@@ -713,6 +712,11 @@ static int __do_huge_pmd_anonymous_page(struct mm_struct *mm,
 		return VM_FAULT_OOM;
 
 	clear_huge_page(page, haddr, HPAGE_PMD_NR);
+	/*
+	 * The memory barrier inside __SetPageUptodate makes sure that
+	 * clear_huge_page writes become visible before the set_pmd_at()
+	 * write.
+	 */
 	__SetPageUptodate(page);
 
 	spin_lock(&mm->page_table_lock);
@@ -724,12 +728,6 @@ static int __do_huge_pmd_anonymous_page(struct mm_struct *mm,
 	} else {
 		pmd_t entry;
 		entry = mk_huge_pmd(page, vma);
-		/*
-		 * The spinlocking to take the lru_lock inside
-		 * page_add_new_anon_rmap() acts as a full memory
-		 * barrier to be sure clear_huge_page writes become
-		 * visible after the set_pmd_at() write.
-		 */
 		page_add_new_anon_rmap(page, vma, haddr);
 		set_pmd_at(mm, haddr, pmd, entry);
 		pgtable_trans_huge_deposit(mm, pgtable);
@@ -765,12 +763,12 @@ static inline struct page *alloc_hugepage(int defrag)
 
 static bool set_huge_zero_page(pgtable_t pgtable, struct mm_struct *mm,
 		struct vm_area_struct *vma, unsigned long haddr, pmd_t *pmd,
-		unsigned long zero_pfn)
+		struct page *zero_page)
 {
 	pmd_t entry;
 	if (!pmd_none(*pmd))
 		return false;
-	entry = pfn_pmd(zero_pfn, vma->vm_page_prot);
+	entry = mk_pmd(zero_page, vma->vm_page_prot);
 	entry = pmd_wrprotect(entry);
 	entry = pmd_mkhuge(entry);
 	set_pmd_at(mm, haddr, pmd, entry);
@@ -795,20 +793,20 @@ int do_huge_pmd_anonymous_page(struct mm_struct *mm, struct vm_area_struct *vma,
 		if (!(flags & FAULT_FLAG_WRITE) &&
 				transparent_hugepage_use_zero_page()) {
 			pgtable_t pgtable;
-			unsigned long zero_pfn;
+			struct page *zero_page;
 			bool set;
 			pgtable = pte_alloc_one(mm, haddr);
 			if (unlikely(!pgtable))
 				return VM_FAULT_OOM;
-			zero_pfn = get_huge_zero_page();
-			if (unlikely(!zero_pfn)) {
+			zero_page = get_huge_zero_page();
+			if (unlikely(!zero_page)) {
 				pte_free(mm, pgtable);
 				count_vm_event(THP_FAULT_FALLBACK);
 				goto out;
 			}
 			spin_lock(&mm->page_table_lock);
 			set = set_huge_zero_page(pgtable, mm, vma, haddr, pmd,
-					zero_pfn);
+					zero_page);
 			spin_unlock(&mm->page_table_lock);
 			if (!set) {
 				pte_free(mm, pgtable);
@@ -887,16 +885,16 @@ int copy_huge_pmd(struct mm_struct *dst_mm, struct mm_struct *src_mm,
 	 * a page table.
 	 */
 	if (is_huge_zero_pmd(pmd)) {
-		unsigned long zero_pfn;
+		struct page *zero_page;
 		bool set;
 		/*
 		 * get_huge_zero_page() will never allocate a new page here,
 		 * since we already have a zero page to copy. It just takes a
 		 * reference.
 		 */
-		zero_pfn = get_huge_zero_page();
+		zero_page = get_huge_zero_page();
 		set = set_huge_zero_page(pgtable, dst_mm, vma, addr, dst_pmd,
-				zero_pfn);
+				zero_page);
 		BUG_ON(!set); /* unexpected !pmd_none(dst_pmd) */
 		ret = 0;
 		goto out_unlock;
@@ -1560,7 +1558,8 @@ static int __split_huge_page_splitting(struct page *page,
 	return ret;
 }
 
-static void __split_huge_page_refcount(struct page *page)
+static void __split_huge_page_refcount(struct page *page,
+				       struct list_head *list)
 {
 	int i;
 	struct zone *zone = page_zone(page);
@@ -1646,7 +1645,7 @@ static void __split_huge_page_refcount(struct page *page)
 		BUG_ON(!PageDirty(page_tail));
 		BUG_ON(!PageSwapBacked(page_tail));
 
-		lru_add_page_tail(page, page_tail, lruvec);
+		lru_add_page_tail(page, page_tail, lruvec, list);
 	}
 	atomic_sub(tail_count, &page->_count);
 	BUG_ON(atomic_read(&page->_count) <= 0);
@@ -1753,7 +1752,8 @@ static int __split_huge_page_map(struct page *page,
 
 /* must be called with anon_vma->root->rwsem held */
 static void __split_huge_page(struct page *page,
-			      struct anon_vma *anon_vma)
+			      struct anon_vma *anon_vma,
+			      struct list_head *list)
 {
 	int mapcount, mapcount2;
 	pgoff_t pgoff = page->index << (PAGE_CACHE_SHIFT - PAGE_SHIFT);
@@ -1784,7 +1784,7 @@ static void __split_huge_page(struct page *page,
 		       mapcount, page_mapcount(page));
 	BUG_ON(mapcount != page_mapcount(page));
 
-	__split_huge_page_refcount(page);
+	__split_huge_page_refcount(page, list);
 
 	mapcount2 = 0;
 	anon_vma_interval_tree_foreach(avc, &anon_vma->rb_root, pgoff, pgoff) {
@@ -1799,12 +1799,19 @@ static void __split_huge_page(struct page *page,
 	BUG_ON(mapcount != mapcount2);
 }
 
-int split_huge_page(struct page *page)
+/*
+ * Split a hugepage into normal pages. This doesn't change the position of head
+ * page. If @list is null, tail pages will be added to LRU list, otherwise, to
+ * @list. Both head page and tail pages will inherit mapping, flags, and so on
+ * from the hugepage.
+ * Return 0 if the hugepage is split successfully otherwise return 1.
+ */
+int split_huge_page_to_list(struct page *page, struct list_head *list)
 {
 	struct anon_vma *anon_vma;
 	int ret = 1;
 
-	BUG_ON(is_huge_zero_pfn(page_to_pfn(page)));
+	BUG_ON(is_huge_zero_page(page));
 	BUG_ON(!PageAnon(page));
 
 	/*
@@ -1824,7 +1831,7 @@ int split_huge_page(struct page *page)
 		goto out_unlock;
 
 	BUG_ON(!PageSwapBacked(page));
-	__split_huge_page(page, anon_vma);
+	__split_huge_page(page, anon_vma, list);
 	count_vm_event(THP_SPLIT);
 
 	BUG_ON(PageCompound(page));
diff --git a/mm/hugetlb.c b/mm/hugetlb.c
index ca9a7c6d7e9..f8feeeca668 100644
--- a/mm/hugetlb.c
+++ b/mm/hugetlb.c
@@ -1761,7 +1761,7 @@ static struct hstate *kobj_to_node_hstate(struct kobject *kobj, int *nidp)
  * Unregister hstate attributes from a single node device.
  * No-op if no hstate attributes attached.
  */
-void hugetlb_unregister_node(struct node *node)
+static void hugetlb_unregister_node(struct node *node)
 {
 	struct hstate *h;
 	struct node_hstate *nhs = &node_hstates[node->dev.id];
@@ -1805,7 +1805,7 @@ static void hugetlb_unregister_all_nodes(void)
  * Register hstate attributes for a single node device.
  * No-op if attributes already registered.
  */
-void hugetlb_register_node(struct node *node)
+static void hugetlb_register_node(struct node *node)
 {
 	struct hstate *h;
 	struct node_hstate *nhs = &node_hstates[node->dev.id];
@@ -2121,6 +2121,21 @@ int hugetlb_report_node_meminfo(int nid, char *buf)
 		nid, h->surplus_huge_pages_node[nid]);
 }
 
+void hugetlb_show_meminfo(void)
+{
+	struct hstate *h;
+	int nid;
+
+	for_each_node_state(nid, N_MEMORY)
+		for_each_hstate(h)
+			pr_info("Node %d hugepages_total=%u hugepages_free=%u hugepages_surp=%u hugepages_size=%lukB\n",
+				nid,
+				h->nr_huge_pages_node[nid],
+				h->free_huge_pages_node[nid],
+				h->surplus_huge_pages_node[nid],
+				1UL << (huge_page_order(h) + PAGE_SHIFT - 10));
+}
+
 /* Return the number pages of memory we physically have, in PAGE_SIZE units. */
 unsigned long hugetlb_total_pages(void)
 {
@@ -2247,10 +2262,11 @@ static pte_t make_huge_pte(struct vm_area_struct *vma, struct page *page,
 	pte_t entry;
 
 	if (writable) {
-		entry =
-		    pte_mkwrite(pte_mkdirty(mk_pte(page, vma->vm_page_prot)));
+		entry = huge_pte_mkwrite(huge_pte_mkdirty(mk_huge_pte(page,
+					 vma->vm_page_prot)));
 	} else {
-		entry = huge_pte_wrprotect(mk_pte(page, vma->vm_page_prot));
+		entry = huge_pte_wrprotect(mk_huge_pte(page,
+					   vma->vm_page_prot));
 	}
 	entry = pte_mkyoung(entry);
 	entry = pte_mkhuge(entry);
@@ -2264,7 +2280,7 @@ static void set_huge_ptep_writable(struct vm_area_struct *vma,
 {
 	pte_t entry;
 
-	entry = pte_mkwrite(pte_mkdirty(huge_ptep_get(ptep)));
+	entry = huge_pte_mkwrite(huge_pte_mkdirty(huge_ptep_get(ptep)));
 	if (huge_ptep_set_access_flags(vma, address, ptep, entry, 1))
 		update_mmu_cache(vma, address, ptep);
 }
@@ -2379,7 +2395,7 @@ again:
 		 * HWPoisoned hugepage is already unmapped and dropped reference
 		 */
 		if (unlikely(is_hugetlb_entry_hwpoisoned(pte))) {
-			pte_clear(mm, address, ptep);
+			huge_pte_clear(mm, address, ptep);
 			continue;
 		}
 
@@ -2403,7 +2419,7 @@ again:
 
 		pte = huge_ptep_get_and_clear(mm, address, ptep);
 		tlb_remove_tlb_entry(tlb, ptep, address);
-		if (pte_dirty(pte))
+		if (huge_pte_dirty(pte))
 			set_page_dirty(page);
 
 		page_remove_rmap(page);
@@ -2856,7 +2872,7 @@ int hugetlb_fault(struct mm_struct *mm, struct vm_area_struct *vma,
 	 * page now as it is used to determine if a reservation has been
 	 * consumed.
 	 */
-	if ((flags & FAULT_FLAG_WRITE) && !pte_write(entry)) {
+	if ((flags & FAULT_FLAG_WRITE) && !huge_pte_write(entry)) {
 		if (vma_needs_reservation(h, vma, address) < 0) {
 			ret = VM_FAULT_OOM;
 			goto out_mutex;
@@ -2886,12 +2902,12 @@ int hugetlb_fault(struct mm_struct *mm, struct vm_area_struct *vma,
 
 
 	if (flags & FAULT_FLAG_WRITE) {
-		if (!pte_write(entry)) {
+		if (!huge_pte_write(entry)) {
 			ret = hugetlb_cow(mm, vma, address, ptep, entry,
 							pagecache_page);
 			goto out_page_table_lock;
 		}
-		entry = pte_mkdirty(entry);
+		entry = huge_pte_mkdirty(entry);
 	}
 	entry = pte_mkyoung(entry);
 	if (huge_ptep_set_access_flags(vma, address, ptep, entry,
@@ -2961,8 +2977,19 @@ long follow_hugetlb_page(struct mm_struct *mm, struct vm_area_struct *vma,
 			break;
 		}
 
-		if (absent ||
-		    ((flags & FOLL_WRITE) && !pte_write(huge_ptep_get(pte)))) {
+		/*
+		 * We need call hugetlb_fault for both hugepages under migration
+		 * (in which case hugetlb_fault waits for the migration,) and
+		 * hwpoisoned hugepages (in which case we need to prevent the
+		 * caller from accessing to them.) In order to do this, we use
+		 * here is_swap_pte instead of is_hugetlb_entry_migration and
+		 * is_hugetlb_entry_hwpoisoned. This is because it simply covers
+		 * both cases, and because we can't follow correct pages
+		 * directly from any kind of swap entries.
+		 */
+		if (absent || is_swap_pte(huge_ptep_get(pte)) ||
+		    ((flags & FOLL_WRITE) &&
+		      !huge_pte_write(huge_ptep_get(pte)))) {
 			int ret;
 
 			spin_unlock(&mm->page_table_lock);
@@ -3032,7 +3059,7 @@ unsigned long hugetlb_change_protection(struct vm_area_struct *vma,
 		}
 		if (!huge_pte_none(huge_ptep_get(ptep))) {
 			pte = huge_ptep_get_and_clear(mm, address, ptep);
-			pte = pte_mkhuge(pte_modify(pte, newprot));
+			pte = pte_mkhuge(huge_pte_modify(pte, newprot));
 			pte = arch_make_huge_pte(pte, vma, NULL, 0);
 			set_huge_pte_at(mm, address, ptep, pte);
 			pages++;
diff --git a/mm/madvise.c b/mm/madvise.c
index c58c94b56c3..7055883e6e2 100644
--- a/mm/madvise.c
+++ b/mm/madvise.c
@@ -473,27 +473,27 @@ SYSCALL_DEFINE3(madvise, unsigned long, start, size_t, len_in, int, behavior)
 	if (!madvise_behavior_valid(behavior))
 		return error;
 
-	write = madvise_need_mmap_write(behavior);
-	if (write)
-		down_write(&current->mm->mmap_sem);
-	else
-		down_read(&current->mm->mmap_sem);
-
 	if (start & ~PAGE_MASK)
-		goto out;
+		return error;
 	len = (len_in + ~PAGE_MASK) & PAGE_MASK;
 
 	/* Check to see whether len was rounded up from small -ve to zero */
 	if (len_in && !len)
-		goto out;
+		return error;
 
 	end = start + len;
 	if (end < start)
-		goto out;
+		return error;
 
 	error = 0;
 	if (end == start)
-		goto out;
+		return error;
+
+	write = madvise_need_mmap_write(behavior);
+	if (write)
+		down_write(&current->mm->mmap_sem);
+	else
+		down_read(&current->mm->mmap_sem);
 
 	/*
 	 * If the interval [start,end) covers some unmapped address
@@ -509,14 +509,14 @@ SYSCALL_DEFINE3(madvise, unsigned long, start, size_t, len_in, int, behavior)
 		/* Still start < end. */
 		error = -ENOMEM;
 		if (!vma)
-			goto out_plug;
+			goto out;
 
 		/* Here start < (end|vma->vm_end). */
 		if (start < vma->vm_start) {
 			unmapped_error = -ENOMEM;
 			start = vma->vm_start;
 			if (start >= end)
-				goto out_plug;
+				goto out;
 		}
 
 		/* Here vma->vm_start <= start < (end|vma->vm_end) */
@@ -527,21 +527,20 @@ SYSCALL_DEFINE3(madvise, unsigned long, start, size_t, len_in, int, behavior)
 		/* Here vma->vm_start <= start < tmp <= (end|vma->vm_end). */
 		error = madvise_vma(vma, &prev, start, tmp, behavior);
 		if (error)
-			goto out_plug;
+			goto out;
 		start = tmp;
 		if (prev && start < prev->vm_end)
 			start = prev->vm_end;
 		error = unmapped_error;
 		if (start >= end)
-			goto out_plug;
+			goto out;
 		if (prev)
 			vma = prev->vm_next;
 		else	/* madvise_remove dropped mmap_sem */
 			vma = find_vma(current->mm, start);
 	}
-out_plug:
-	blk_finish_plug(&plug);
 out:
+	blk_finish_plug(&plug);
 	if (write)
 		up_write(&current->mm->mmap_sem);
 	else
diff --git a/mm/memblock.c b/mm/memblock.c
index b8d9147e5c0..c5fad932fa5 100644
--- a/mm/memblock.c
+++ b/mm/memblock.c
@@ -322,10 +322,11 @@ static void __init_memblock memblock_merge_regions(struct memblock_type *type)
 
 /**
  * memblock_insert_region - insert new memblock region
- * @type: memblock type to insert into
- * @idx: index for the insertion point
- * @base: base address of the new region
- * @size: size of the new region
+ * @type:	memblock type to insert into
+ * @idx:	index for the insertion point
+ * @base:	base address of the new region
+ * @size:	size of the new region
+ * @nid:	node id of the new region
  *
  * Insert new memblock region [@base,@base+@size) into @type at @idx.
  * @type must already have extra room to accomodate the new region.
@@ -771,6 +772,9 @@ static phys_addr_t __init memblock_alloc_base_nid(phys_addr_t size,
 {
 	phys_addr_t found;
 
+	if (WARN_ON(!align))
+		align = __alignof__(long long);
+
 	/* align @size to avoid excessive fragmentation on reserved array */
 	size = round_up(size, align);
 
diff --git a/mm/memcontrol.c b/mm/memcontrol.c
index 2b552224f5c..0f1d92163f3 100644
--- a/mm/memcontrol.c
+++ b/mm/memcontrol.c
@@ -49,6 +49,7 @@
 #include <linux/fs.h>
 #include <linux/seq_file.h>
 #include <linux/vmalloc.h>
+#include <linux/vmpressure.h>
 #include <linux/mm_inline.h>
 #include <linux/page_cgroup.h>
 #include <linux/cpu.h>
@@ -152,8 +153,13 @@ struct mem_cgroup_stat_cpu {
 };
 
 struct mem_cgroup_reclaim_iter {
-	/* css_id of the last scanned hierarchy member */
-	int position;
+	/*
+	 * last scanned hierarchy member. Valid only if last_dead_count
+	 * matches memcg->dead_count of the hierarchy root group.
+	 */
+	struct mem_cgroup *last_visited;
+	unsigned long last_dead_count;
+
 	/* scan generation, increased every round-trip */
 	unsigned int generation;
 };
@@ -256,6 +262,9 @@ struct mem_cgroup {
 	 */
 	struct res_counter res;
 
+	/* vmpressure notifications */
+	struct vmpressure vmpressure;
+
 	union {
 		/*
 		 * the counter to account for mem+swap usage.
@@ -335,6 +344,7 @@ struct mem_cgroup {
 	struct mem_cgroup_stat_cpu nocpu_base;
 	spinlock_t pcp_counter_lock;
 
+	atomic_t	dead_count;
 #if defined(CONFIG_MEMCG_KMEM) && defined(CONFIG_INET)
 	struct tcp_memcontrol tcp_mem;
 #endif
@@ -353,6 +363,7 @@ struct mem_cgroup {
 	atomic_t	numainfo_events;
 	atomic_t	numainfo_updating;
 #endif
+
 	/*
 	 * Per cgroup active and inactive list, similar to the
 	 * per zone LRU lists.
@@ -504,6 +515,24 @@ struct mem_cgroup *mem_cgroup_from_css(struct cgroup_subsys_state *s)
 	return container_of(s, struct mem_cgroup, css);
 }
 
+/* Some nice accessors for the vmpressure. */
+struct vmpressure *memcg_to_vmpressure(struct mem_cgroup *memcg)
+{
+	if (!memcg)
+		memcg = root_mem_cgroup;
+	return &memcg->vmpressure;
+}
+
+struct cgroup_subsys_state *vmpressure_to_css(struct vmpressure *vmpr)
+{
+	return &container_of(vmpr, struct mem_cgroup, vmpressure)->css;
+}
+
+struct vmpressure *css_to_vmpressure(struct cgroup_subsys_state *css)
+{
+	return &mem_cgroup_from_css(css)->vmpressure;
+}
+
 static inline bool mem_cgroup_is_root(struct mem_cgroup *memcg)
 {
 	return (memcg == root_mem_cgroup);
@@ -1067,6 +1096,51 @@ struct mem_cgroup *try_get_mem_cgroup_from_mm(struct mm_struct *mm)
 	return memcg;
 }
 
+/*
+ * Returns a next (in a pre-order walk) alive memcg (with elevated css
+ * ref. count) or NULL if the whole root's subtree has been visited.
+ *
+ * helper function to be used by mem_cgroup_iter
+ */
+static struct mem_cgroup *__mem_cgroup_iter_next(struct mem_cgroup *root,
+		struct mem_cgroup *last_visited)
+{
+	struct cgroup *prev_cgroup, *next_cgroup;
+
+	/*
+	 * Root is not visited by cgroup iterators so it needs an
+	 * explicit visit.
+	 */
+	if (!last_visited)
+		return root;
+
+	prev_cgroup = (last_visited == root) ? NULL
+		: last_visited->css.cgroup;
+skip_node:
+	next_cgroup = cgroup_next_descendant_pre(
+			prev_cgroup, root->css.cgroup);
+
+	/*
+	 * Even if we found a group we have to make sure it is
+	 * alive. css && !memcg means that the groups should be
+	 * skipped and we should continue the tree walk.
+	 * last_visited css is safe to use because it is
+	 * protected by css_get and the tree walk is rcu safe.
+	 */
+	if (next_cgroup) {
+		struct mem_cgroup *mem = mem_cgroup_from_cont(
+				next_cgroup);
+		if (css_tryget(&mem->css))
+			return mem;
+		else {
+			prev_cgroup = next_cgroup;
+			goto skip_node;
+		}
+	}
+
+	return NULL;
+}
+
 /**
  * mem_cgroup_iter - iterate over memory cgroup hierarchy
  * @root: hierarchy root
@@ -1089,7 +1163,8 @@ struct mem_cgroup *mem_cgroup_iter(struct mem_cgroup *root,
 				   struct mem_cgroup_reclaim_cookie *reclaim)
 {
 	struct mem_cgroup *memcg = NULL;
-	int id = 0;
+	struct mem_cgroup *last_visited = NULL;
+	unsigned long uninitialized_var(dead_count);
 
 	if (mem_cgroup_disabled())
 		return NULL;
@@ -1098,20 +1173,17 @@ struct mem_cgroup *mem_cgroup_iter(struct mem_cgroup *root,
 		root = root_mem_cgroup;
 
 	if (prev && !reclaim)
-		id = css_id(&prev->css);
-
-	if (prev && prev != root)
-		css_put(&prev->css);
+		last_visited = prev;
 
 	if (!root->use_hierarchy && root != root_mem_cgroup) {
 		if (prev)
-			return NULL;
+			goto out_css_put;
 		return root;
 	}
 
+	rcu_read_lock();
 	while (!memcg) {
 		struct mem_cgroup_reclaim_iter *uninitialized_var(iter);
-		struct cgroup_subsys_state *css;
 
 		if (reclaim) {
 			int nid = zone_to_nid(reclaim->zone);
@@ -1120,31 +1192,60 @@ struct mem_cgroup *mem_cgroup_iter(struct mem_cgroup *root,
 
 			mz = mem_cgroup_zoneinfo(root, nid, zid);
 			iter = &mz->reclaim_iter[reclaim->priority];
-			if (prev && reclaim->generation != iter->generation)
-				return NULL;
-			id = iter->position;
+			last_visited = iter->last_visited;
+			if (prev && reclaim->generation != iter->generation) {
+				iter->last_visited = NULL;
+				goto out_unlock;
+			}
+
+			/*
+			 * If the dead_count mismatches, a destruction
+			 * has happened or is happening concurrently.
+			 * If the dead_count matches, a destruction
+			 * might still happen concurrently, but since
+			 * we checked under RCU, that destruction
+			 * won't free the object until we release the
+			 * RCU reader lock.  Thus, the dead_count
+			 * check verifies the pointer is still valid,
+			 * css_tryget() verifies the cgroup pointed to
+			 * is alive.
+			 */
+			dead_count = atomic_read(&root->dead_count);
+			smp_rmb();
+			last_visited = iter->last_visited;
+			if (last_visited) {
+				if ((dead_count != iter->last_dead_count) ||
+					!css_tryget(&last_visited->css)) {
+					last_visited = NULL;
+				}
+			}
 		}
 
-		rcu_read_lock();
-		css = css_get_next(&mem_cgroup_subsys, id + 1, &root->css, &id);
-		if (css) {
-			if (css == &root->css || css_tryget(css))
-				memcg = mem_cgroup_from_css(css);
-		} else
-			id = 0;
-		rcu_read_unlock();
+		memcg = __mem_cgroup_iter_next(root, last_visited);
 
 		if (reclaim) {
-			iter->position = id;
-			if (!css)
+			if (last_visited)
+				css_put(&last_visited->css);
+
+			iter->last_visited = memcg;
+			smp_wmb();
+			iter->last_dead_count = dead_count;
+
+			if (!memcg)
 				iter->generation++;
 			else if (!prev && memcg)
 				reclaim->generation = iter->generation;
 		}
 
-		if (prev && !css)
-			return NULL;
+		if (prev && !memcg)
+			goto out_unlock;
 	}
+out_unlock:
+	rcu_read_unlock();
+out_css_put:
+	if (prev && prev != root)
+		css_put(&prev->css);
+
 	return memcg;
 }
 
@@ -1686,11 +1787,11 @@ static void mem_cgroup_out_of_memory(struct mem_cgroup *memcg, gfp_t gfp_mask,
 	struct task_struct *chosen = NULL;
 
 	/*
-	 * If current has a pending SIGKILL, then automatically select it.  The
-	 * goal is to allow it to allocate so that it may quickly exit and free
-	 * its memory.
+	 * If current has a pending SIGKILL or is exiting, then automatically
+	 * select it.  The goal is to allow it to allocate so that it may
+	 * quickly exit and free its memory.
 	 */
-	if (fatal_signal_pending(current)) {
+	if (fatal_signal_pending(current) || current->flags & PF_EXITING) {
 		set_thread_flag(TIF_MEMDIE);
 		return;
 	}
@@ -3114,12 +3215,12 @@ void memcg_release_cache(struct kmem_cache *s)
 
 	root = s->memcg_params->root_cache;
 	root->memcg_params->memcg_caches[id] = NULL;
-	mem_cgroup_put(memcg);
 
 	mutex_lock(&memcg->slab_caches_mutex);
 	list_del(&s->memcg_params->list);
 	mutex_unlock(&memcg->slab_caches_mutex);
 
+	mem_cgroup_put(memcg);
 out:
 	kfree(s->memcg_params);
 }
@@ -3220,52 +3321,53 @@ void mem_cgroup_destroy_cache(struct kmem_cache *cachep)
 	schedule_work(&cachep->memcg_params->destroy);
 }
 
-static char *memcg_cache_name(struct mem_cgroup *memcg, struct kmem_cache *s)
-{
-	char *name;
-	struct dentry *dentry;
-
-	rcu_read_lock();
-	dentry = rcu_dereference(memcg->css.cgroup->dentry);
-	rcu_read_unlock();
-
-	BUG_ON(dentry == NULL);
-
-	name = kasprintf(GFP_KERNEL, "%s(%d:%s)", s->name,
-			 memcg_cache_id(memcg), dentry->d_name.name);
-
-	return name;
-}
+/*
+ * This lock protects updaters, not readers. We want readers to be as fast as
+ * they can, and they will either see NULL or a valid cache value. Our model
+ * allow them to see NULL, in which case the root memcg will be selected.
+ *
+ * We need this lock because multiple allocations to the same cache from a non
+ * will span more than one worker. Only one of them can create the cache.
+ */
+static DEFINE_MUTEX(memcg_cache_mutex);
 
+/*
+ * Called with memcg_cache_mutex held
+ */
 static struct kmem_cache *kmem_cache_dup(struct mem_cgroup *memcg,
 					 struct kmem_cache *s)
 {
-	char *name;
 	struct kmem_cache *new;
+	static char *tmp_name = NULL;
 
-	name = memcg_cache_name(memcg, s);
-	if (!name)
-		return NULL;
+	lockdep_assert_held(&memcg_cache_mutex);
+
+	/*
+	 * kmem_cache_create_memcg duplicates the given name and
+	 * cgroup_name for this name requires RCU context.
+	 * This static temporary buffer is used to prevent from
+	 * pointless shortliving allocation.
+	 */
+	if (!tmp_name) {
+		tmp_name = kmalloc(PATH_MAX, GFP_KERNEL);
+		if (!tmp_name)
+			return NULL;
+	}
+
+	rcu_read_lock();
+	snprintf(tmp_name, PATH_MAX, "%s(%d:%s)", s->name,
+			 memcg_cache_id(memcg), cgroup_name(memcg->css.cgroup));
+	rcu_read_unlock();
 
-	new = kmem_cache_create_memcg(memcg, name, s->object_size, s->align,
+	new = kmem_cache_create_memcg(memcg, tmp_name, s->object_size, s->align,
 				      (s->flags & ~SLAB_PANIC), s->ctor, s);
 
 	if (new)
 		new->allocflags |= __GFP_KMEMCG;
 
-	kfree(name);
 	return new;
 }
 
-/*
- * This lock protects updaters, not readers. We want readers to be as fast as
- * they can, and they will either see NULL or a valid cache value. Our model
- * allow them to see NULL, in which case the root memcg will be selected.
- *
- * We need this lock because multiple allocations to the same cache from a non
- * will span more than one worker. Only one of them can create the cache.
- */
-static DEFINE_MUTEX(memcg_cache_mutex);
 static struct kmem_cache *memcg_create_kmem_cache(struct mem_cgroup *memcg,
 						  struct kmem_cache *cachep)
 {
@@ -3382,7 +3484,6 @@ static void memcg_create_cache_work_func(struct work_struct *w)
 
 /*
  * Enqueue the creation of a per-memcg kmem_cache.
- * Called with rcu_read_lock.
  */
 static void __memcg_create_cache_enqueue(struct mem_cgroup *memcg,
 					 struct kmem_cache *cachep)
@@ -3390,12 +3491,8 @@ static void __memcg_create_cache_enqueue(struct mem_cgroup *memcg,
 	struct create_work *cw;
 
 	cw = kmalloc(sizeof(struct create_work), GFP_NOWAIT);
-	if (cw == NULL)
-		return;
-
-	/* The corresponding put will be done in the workqueue. */
-	if (!css_tryget(&memcg->css)) {
-		kfree(cw);
+	if (cw == NULL) {
+		css_put(&memcg->css);
 		return;
 	}
 
@@ -3451,10 +3548,9 @@ struct kmem_cache *__memcg_kmem_get_cache(struct kmem_cache *cachep,
 
 	rcu_read_lock();
 	memcg = mem_cgroup_from_task(rcu_dereference(current->mm->owner));
-	rcu_read_unlock();
 
 	if (!memcg_can_account_kmem(memcg))
-		return cachep;
+		goto out;
 
 	idx = memcg_cache_id(memcg);
 
@@ -3463,29 +3559,38 @@ struct kmem_cache *__memcg_kmem_get_cache(struct kmem_cache *cachep,
 	 * code updating memcg_caches will issue a write barrier to match this.
 	 */
 	read_barrier_depends();
-	if (unlikely(cachep->memcg_params->memcg_caches[idx] == NULL)) {
-		/*
-		 * If we are in a safe context (can wait, and not in interrupt
-		 * context), we could be be predictable and return right away.
-		 * This would guarantee that the allocation being performed
-		 * already belongs in the new cache.
-		 *
-		 * However, there are some clashes that can arrive from locking.
-		 * For instance, because we acquire the slab_mutex while doing
-		 * kmem_cache_dup, this means no further allocation could happen
-		 * with the slab_mutex held.
-		 *
-		 * Also, because cache creation issue get_online_cpus(), this
-		 * creates a lock chain: memcg_slab_mutex -> cpu_hotplug_mutex,
-		 * that ends up reversed during cpu hotplug. (cpuset allocates
-		 * a bunch of GFP_KERNEL memory during cpuup). Due to all that,
-		 * better to defer everything.
-		 */
-		memcg_create_cache_enqueue(memcg, cachep);
-		return cachep;
+	if (likely(cachep->memcg_params->memcg_caches[idx])) {
+		cachep = cachep->memcg_params->memcg_caches[idx];
+		goto out;
 	}
 
-	return cachep->memcg_params->memcg_caches[idx];
+	/* The corresponding put will be done in the workqueue. */
+	if (!css_tryget(&memcg->css))
+		goto out;
+	rcu_read_unlock();
+
+	/*
+	 * If we are in a safe context (can wait, and not in interrupt
+	 * context), we could be be predictable and return right away.
+	 * This would guarantee that the allocation being performed
+	 * already belongs in the new cache.
+	 *
+	 * However, there are some clashes that can arrive from locking.
+	 * For instance, because we acquire the slab_mutex while doing
+	 * kmem_cache_dup, this means no further allocation could happen
+	 * with the slab_mutex held.
+	 *
+	 * Also, because cache creation issue get_online_cpus(), this
+	 * creates a lock chain: memcg_slab_mutex -> cpu_hotplug_mutex,
+	 * that ends up reversed during cpu hotplug. (cpuset allocates
+	 * a bunch of GFP_KERNEL memory during cpuup). Due to all that,
+	 * better to defer everything.
+	 */
+	memcg_create_cache_enqueue(memcg, cachep);
+	return cachep;
+out:
+	rcu_read_unlock();
+	return cachep;
 }
 EXPORT_SYMBOL(__memcg_kmem_get_cache);
 
@@ -4947,9 +5052,6 @@ static ssize_t mem_cgroup_read(struct cgroup *cont, struct cftype *cft,
 	type = MEMFILE_TYPE(cft->private);
 	name = MEMFILE_ATTR(cft->private);
 
-	if (!do_swap_account && type == _MEMSWAP)
-		return -EOPNOTSUPP;
-
 	switch (type) {
 	case _MEM:
 		if (name == RES_USAGE)
@@ -5084,9 +5186,6 @@ static int mem_cgroup_write(struct cgroup *cont, struct cftype *cft,
 	type = MEMFILE_TYPE(cft->private);
 	name = MEMFILE_ATTR(cft->private);
 
-	if (!do_swap_account && type == _MEMSWAP)
-		return -EOPNOTSUPP;
-
 	switch (name) {
 	case RES_LIMIT:
 		if (mem_cgroup_is_root(memcg)) { /* Can't set limit on root */
@@ -5163,9 +5262,6 @@ static int mem_cgroup_reset(struct cgroup *cont, unsigned int event)
 	type = MEMFILE_TYPE(event);
 	name = MEMFILE_ATTR(event);
 
-	if (!do_swap_account && type == _MEMSWAP)
-		return -EOPNOTSUPP;
-
 	switch (name) {
 	case RES_MAX_USAGE:
 		if (type == _MEM)
@@ -5744,7 +5840,7 @@ static int memcg_init_kmem(struct mem_cgroup *memcg, struct cgroup_subsys *ss)
 		return ret;
 
 	return mem_cgroup_sockets_init(memcg, ss);
-};
+}
 
 static void kmem_cgroup_destroy(struct mem_cgroup *memcg)
 {
@@ -5817,6 +5913,7 @@ static struct cftype mem_cgroup_files[] = {
 	},
 	{
 		.name = "use_hierarchy",
+		.flags = CFTYPE_INSANE,
 		.write_u64 = mem_cgroup_hierarchy_write,
 		.read_u64 = mem_cgroup_hierarchy_read,
 	},
@@ -5838,6 +5935,11 @@ static struct cftype mem_cgroup_files[] = {
 		.unregister_event = mem_cgroup_oom_unregister_event,
 		.private = MEMFILE_PRIVATE(_OOM_TYPE, OOM_CONTROL),
 	},
+	{
+		.name = "pressure_level",
+		.register_event = vmpressure_register_event,
+		.unregister_event = vmpressure_unregister_event,
+	},
 #ifdef CONFIG_NUMA
 	{
 		.name = "numa_stat",
@@ -6119,6 +6221,7 @@ mem_cgroup_css_alloc(struct cgroup *cont)
 	memcg->move_charge_at_immigrate = 0;
 	mutex_init(&memcg->thresholds_lock);
 	spin_lock_init(&memcg->move_lock);
+	vmpressure_init(&memcg->vmpressure);
 
 	return &memcg->css;
 
@@ -6184,10 +6287,29 @@ mem_cgroup_css_online(struct cgroup *cont)
 	return error;
 }
 
+/*
+ * Announce all parents that a group from their hierarchy is gone.
+ */
+static void mem_cgroup_invalidate_reclaim_iterators(struct mem_cgroup *memcg)
+{
+	struct mem_cgroup *parent = memcg;
+
+	while ((parent = parent_mem_cgroup(parent)))
+		atomic_inc(&parent->dead_count);
+
+	/*
+	 * if the root memcg is not hierarchical we have to check it
+	 * explicitely.
+	 */
+	if (!root_mem_cgroup->use_hierarchy)
+		atomic_inc(&root_mem_cgroup->dead_count);
+}
+
 static void mem_cgroup_css_offline(struct cgroup *cont)
 {
 	struct mem_cgroup *memcg = mem_cgroup_from_cont(cont);
 
+	mem_cgroup_invalidate_reclaim_iterators(memcg);
 	mem_cgroup_reparent_charges(memcg);
 	mem_cgroup_destroy_all_caches(memcg);
 }
@@ -6787,6 +6909,21 @@ static void mem_cgroup_move_task(struct cgroup *cont,
 }
 #endif
 
+/*
+ * Cgroup retains root cgroups across [un]mount cycles making it necessary
+ * to verify sane_behavior flag on each mount attempt.
+ */
+static void mem_cgroup_bind(struct cgroup *root)
+{
+	/*
+	 * use_hierarchy is forced with sane_behavior.  cgroup core
+	 * guarantees that @root doesn't have any children, so turning it
+	 * on for the root memcg is enough.
+	 */
+	if (cgroup_sane_behavior(root))
+		mem_cgroup_from_cont(root)->use_hierarchy = true;
+}
+
 struct cgroup_subsys mem_cgroup_subsys = {
 	.name = "memory",
 	.subsys_id = mem_cgroup_subsys_id,
@@ -6797,6 +6934,7 @@ struct cgroup_subsys mem_cgroup_subsys = {
 	.can_attach = mem_cgroup_can_attach,
 	.cancel_attach = mem_cgroup_cancel_attach,
 	.attach = mem_cgroup_move_task,
+	.bind = mem_cgroup_bind,
 	.base_cftypes = mem_cgroup_files,
 	.early_init = 0,
 	.use_id = 1,
diff --git a/mm/memory-failure.c b/mm/memory-failure.c
index df0694c6ade..ceb0c7f1932 100644
--- a/mm/memory-failure.c
+++ b/mm/memory-failure.c
@@ -785,10 +785,10 @@ static struct page_state {
 	{ sc|dirty,	sc,		"clean swapcache",	me_swapcache_clean },
 
 	{ mlock|dirty,	mlock|dirty,	"dirty mlocked LRU",	me_pagecache_dirty },
-	{ mlock,	mlock,		"clean mlocked LRU",	me_pagecache_clean },
+	{ mlock|dirty,	mlock,		"clean mlocked LRU",	me_pagecache_clean },
 
 	{ unevict|dirty, unevict|dirty,	"dirty unevictable LRU", me_pagecache_dirty },
-	{ unevict,	unevict,	"clean unevictable LRU", me_pagecache_clean },
+	{ unevict|dirty, unevict,	"clean unevictable LRU", me_pagecache_clean },
 
 	{ lru|dirty,	lru|dirty,	"dirty LRU",	me_pagecache_dirty },
 	{ lru|dirty,	lru,		"clean LRU",	me_pagecache_clean },
diff --git a/mm/memory.c b/mm/memory.c
index 494526ae024..6dc1882fbd7 100644
--- a/mm/memory.c
+++ b/mm/memory.c
@@ -216,6 +216,7 @@ void tlb_gather_mmu(struct mmu_gather *tlb, struct mm_struct *mm, bool fullmm)
 	tlb->mm = mm;
 
 	tlb->fullmm     = fullmm;
+	tlb->need_flush_all = 0;
 	tlb->start	= -1UL;
 	tlb->end	= 0;
 	tlb->need_flush = 0;
@@ -714,11 +715,11 @@ static void print_bad_pte(struct vm_area_struct *vma, unsigned long addr,
 	 * Choose text because data symbols depend on CONFIG_KALLSYMS_ALL=y
 	 */
 	if (vma->vm_ops)
-		print_symbol(KERN_ALERT "vma->vm_ops->fault: %s\n",
-				(unsigned long)vma->vm_ops->fault);
+		printk(KERN_ALERT "vma->vm_ops->fault: %pSR\n",
+		       vma->vm_ops->fault);
 	if (vma->vm_file && vma->vm_file->f_op)
-		print_symbol(KERN_ALERT "vma->vm_file->f_op->mmap: %s\n",
-				(unsigned long)vma->vm_file->f_op->mmap);
+		printk(KERN_ALERT "vma->vm_file->f_op->mmap: %pSR\n",
+		       vma->vm_file->f_op->mmap);
 	dump_stack();
 	add_taint(TAINT_BAD_PAGE, LOCKDEP_NOW_UNRELIABLE);
 }
@@ -2392,6 +2393,53 @@ int remap_pfn_range(struct vm_area_struct *vma, unsigned long addr,
 }
 EXPORT_SYMBOL(remap_pfn_range);
 
+/**
+ * vm_iomap_memory - remap memory to userspace
+ * @vma: user vma to map to
+ * @start: start of area
+ * @len: size of area
+ *
+ * This is a simplified io_remap_pfn_range() for common driver use. The
+ * driver just needs to give us the physical memory range to be mapped,
+ * we'll figure out the rest from the vma information.
+ *
+ * NOTE! Some drivers might want to tweak vma->vm_page_prot first to get
+ * whatever write-combining details or similar.
+ */
+int vm_iomap_memory(struct vm_area_struct *vma, phys_addr_t start, unsigned long len)
+{
+	unsigned long vm_len, pfn, pages;
+
+	/* Check that the physical memory area passed in looks valid */
+	if (start + len < start)
+		return -EINVAL;
+	/*
+	 * You *really* shouldn't map things that aren't page-aligned,
+	 * but we've historically allowed it because IO memory might
+	 * just have smaller alignment.
+	 */
+	len += start & ~PAGE_MASK;
+	pfn = start >> PAGE_SHIFT;
+	pages = (len + ~PAGE_MASK) >> PAGE_SHIFT;
+	if (pfn + pages < pfn)
+		return -EINVAL;
+
+	/* We start the mapping 'vm_pgoff' pages into the area */
+	if (vma->vm_pgoff > pages)
+		return -EINVAL;
+	pfn += vma->vm_pgoff;
+	pages -= vma->vm_pgoff;
+
+	/* Can we fit all of the mapping? */
+	vm_len = vma->vm_end - vma->vm_start;
+	if (vm_len >> PAGE_SHIFT > pages)
+		return -EINVAL;
+
+	/* Ok, let it rip */
+	return io_remap_pfn_range(vma, vma->vm_start, pfn, vm_len, vma->vm_page_prot);
+}
+EXPORT_SYMBOL(vm_iomap_memory);
+
 static int apply_to_pte_range(struct mm_struct *mm, pmd_t *pmd,
 				     unsigned long addr, unsigned long end,
 				     pte_fn_t fn, void *data)
@@ -3196,6 +3244,11 @@ static int do_anonymous_page(struct mm_struct *mm, struct vm_area_struct *vma,
 	page = alloc_zeroed_user_highpage_movable(vma, address);
 	if (!page)
 		goto oom;
+	/*
+	 * The memory barrier inside __SetPageUptodate makes sure that
+	 * preceeding stores to the page contents become visible before
+	 * the set_pte_at() write.
+	 */
 	__SetPageUptodate(page);
 
 	if (mem_cgroup_newpage_charge(page, mm, GFP_KERNEL))
diff --git a/mm/memory_hotplug.c b/mm/memory_hotplug.c
index ee376576081..a221fac1f47 100644
--- a/mm/memory_hotplug.c
+++ b/mm/memory_hotplug.c
@@ -436,6 +436,40 @@ static int __meminit __add_section(int nid, struct zone *zone,
 	return register_new_memory(nid, __pfn_to_section(phys_start_pfn));
 }
 
+/*
+ * Reasonably generic function for adding memory.  It is
+ * expected that archs that support memory hotplug will
+ * call this function after deciding the zone to which to
+ * add the new pages.
+ */
+int __ref __add_pages(int nid, struct zone *zone, unsigned long phys_start_pfn,
+			unsigned long nr_pages)
+{
+	unsigned long i;
+	int err = 0;
+	int start_sec, end_sec;
+	/* during initialize mem_map, align hot-added range to section */
+	start_sec = pfn_to_section_nr(phys_start_pfn);
+	end_sec = pfn_to_section_nr(phys_start_pfn + nr_pages - 1);
+
+	for (i = start_sec; i <= end_sec; i++) {
+		err = __add_section(nid, zone, i << PFN_SECTION_SHIFT);
+
+		/*
+		 * EEXIST is finally dealt with by ioresource collision
+		 * check. see add_memory() => register_memory_resource()
+		 * Warning will be printed if there is collision.
+		 */
+		if (err && (err != -EEXIST))
+			break;
+		err = 0;
+	}
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(__add_pages);
+
+#ifdef CONFIG_MEMORY_HOTREMOVE
 /* find the smallest valid pfn in the range [start_pfn, end_pfn) */
 static int find_smallest_section_pfn(int nid, struct zone *zone,
 				     unsigned long start_pfn,
@@ -658,39 +692,6 @@ static int __remove_section(struct zone *zone, struct mem_section *ms)
 	return 0;
 }
 
-/*
- * Reasonably generic function for adding memory.  It is
- * expected that archs that support memory hotplug will
- * call this function after deciding the zone to which to
- * add the new pages.
- */
-int __ref __add_pages(int nid, struct zone *zone, unsigned long phys_start_pfn,
-			unsigned long nr_pages)
-{
-	unsigned long i;
-	int err = 0;
-	int start_sec, end_sec;
-	/* during initialize mem_map, align hot-added range to section */
-	start_sec = pfn_to_section_nr(phys_start_pfn);
-	end_sec = pfn_to_section_nr(phys_start_pfn + nr_pages - 1);
-
-	for (i = start_sec; i <= end_sec; i++) {
-		err = __add_section(nid, zone, i << PFN_SECTION_SHIFT);
-
-		/*
-		 * EEXIST is finally dealt with by ioresource collision
-		 * check. see add_memory() => register_memory_resource()
-		 * Warning will be printed if there is collision.
-		 */
-		if (err && (err != -EEXIST))
-			break;
-		err = 0;
-	}
-
-	return err;
-}
-EXPORT_SYMBOL_GPL(__add_pages);
-
 /**
  * __remove_pages() - remove sections of pages from a zone
  * @zone: zone from which pages need to be removed
@@ -705,8 +706,10 @@ EXPORT_SYMBOL_GPL(__add_pages);
 int __remove_pages(struct zone *zone, unsigned long phys_start_pfn,
 		 unsigned long nr_pages)
 {
-	unsigned long i, ret = 0;
+	unsigned long i;
 	int sections_to_remove;
+	resource_size_t start, size;
+	int ret = 0;
 
 	/*
 	 * We can only remove entire sections
@@ -714,7 +717,12 @@ int __remove_pages(struct zone *zone, unsigned long phys_start_pfn,
 	BUG_ON(phys_start_pfn & ~PAGE_SECTION_MASK);
 	BUG_ON(nr_pages % PAGES_PER_SECTION);
 
-	release_mem_region(phys_start_pfn << PAGE_SHIFT, nr_pages * PAGE_SIZE);
+	start = phys_start_pfn << PAGE_SHIFT;
+	size = nr_pages * PAGE_SIZE;
+	ret = release_mem_region_adjustable(&iomem_resource, start, size);
+	if (ret)
+		pr_warn("Unable to release resource <%016llx-%016llx> (%d)\n",
+				start, start + size - 1, ret);
 
 	sections_to_remove = nr_pages / PAGES_PER_SECTION;
 	for (i = 0; i < sections_to_remove; i++) {
@@ -726,6 +734,7 @@ int __remove_pages(struct zone *zone, unsigned long phys_start_pfn,
 	return ret;
 }
 EXPORT_SYMBOL_GPL(__remove_pages);
+#endif /* CONFIG_MEMORY_HOTREMOVE */
 
 int set_online_page_callback(online_page_callback_t callback)
 {
@@ -1613,7 +1622,7 @@ int offline_pages(unsigned long start_pfn, unsigned long nr_pages)
 /**
  * walk_memory_range - walks through all mem sections in [start_pfn, end_pfn)
  * @start_pfn: start pfn of the memory range
- * @end_pfn: end pft of the memory range
+ * @end_pfn: end pfn of the memory range
  * @arg: argument passed to func
  * @func: callback for each memory section walked
  *
@@ -1681,11 +1690,15 @@ static int is_memblock_offlined_cb(struct memory_block *mem, void *arg)
 {
 	int ret = !is_memblock_offlined(mem);
 
-	if (unlikely(ret))
+	if (unlikely(ret)) {
+		phys_addr_t beginpa, endpa;
+
+		beginpa = PFN_PHYS(section_nr_to_pfn(mem->start_section_nr));
+		endpa = PFN_PHYS(section_nr_to_pfn(mem->end_section_nr + 1))-1;
 		pr_warn("removing memory fails, because memory "
-			"[%#010llx-%#010llx] is onlined\n",
-			PFN_PHYS(section_nr_to_pfn(mem->start_section_nr)),
-			PFN_PHYS(section_nr_to_pfn(mem->end_section_nr + 1))-1);
+			"[%pa-%pa] is onlined\n",
+			&beginpa, &endpa);
+	}
 
 	return ret;
 }
diff --git a/mm/migrate.c b/mm/migrate.c
index 3bbaf5d230b..27ed22579fd 100644
--- a/mm/migrate.c
+++ b/mm/migrate.c
@@ -736,7 +736,7 @@ static int __unmap_and_move(struct page *page, struct page *newpage,
 
 	if (PageWriteback(page)) {
 		/*
-		 * Only in the case of a full syncronous migration is it
+		 * Only in the case of a full synchronous migration is it
 		 * necessary to wait for PageWriteback. In the async case,
 		 * the retry loop is too short and in the sync-light case,
 		 * the overhead of stalling is too much
@@ -973,19 +973,23 @@ out:
 }
 
 /*
- * migrate_pages
+ * migrate_pages - migrate the pages specified in a list, to the free pages
+ *		   supplied as the target for the page migration
  *
- * The function takes one list of pages to migrate and a function
- * that determines from the page to be migrated and the private data
- * the target of the move and allocates the page.
+ * @from:		The list of pages to be migrated.
+ * @get_new_page:	The function used to allocate free pages to be used
+ *			as the target of the page migration.
+ * @private:		Private data to be passed on to get_new_page()
+ * @mode:		The migration mode that specifies the constraints for
+ *			page migration, if any.
+ * @reason:		The reason for page migration.
  *
- * The function returns after 10 attempts or if no pages
- * are movable anymore because to has become empty
- * or no retryable pages exist anymore.
- * Caller should call putback_lru_pages to return pages to the LRU
+ * The function returns after 10 attempts or if no pages are movable any more
+ * because the list has become empty or no retryable pages exist any more.
+ * The caller should call putback_lru_pages() to return pages to the LRU
  * or free list only if ret != 0.
  *
- * Return: Number of pages not migrated or error code.
+ * Returns the number of pages that were not migrated, or an error code.
  */
 int migrate_pages(struct list_head *from, new_page_t get_new_page,
 		unsigned long private, enum migrate_mode mode, int reason)
diff --git a/mm/mlock.c b/mm/mlock.c
index 1c5e33fce63..79b7cf7d1bc 100644
--- a/mm/mlock.c
+++ b/mm/mlock.c
@@ -358,7 +358,7 @@ static int do_mlock(unsigned long start, size_t len, int on)
 
 		newflags = vma->vm_flags & ~VM_LOCKED;
 		if (on)
-			newflags |= VM_LOCKED | VM_POPULATE;
+			newflags |= VM_LOCKED;
 
 		tmp = vma->vm_end;
 		if (tmp > end)
@@ -418,8 +418,7 @@ int __mm_populate(unsigned long start, unsigned long len, int ignore_errors)
 		 * range with the first VMA. Also, skip undesirable VMA types.
 		 */
 		nend = min(end, vma->vm_end);
-		if ((vma->vm_flags & (VM_IO | VM_PFNMAP | VM_POPULATE)) !=
-		    VM_POPULATE)
+		if (vma->vm_flags & (VM_IO | VM_PFNMAP))
 			continue;
 		if (nstart < vma->vm_start)
 			nstart = vma->vm_start;
@@ -492,9 +491,9 @@ static int do_mlockall(int flags)
 	struct vm_area_struct * vma, * prev = NULL;
 
 	if (flags & MCL_FUTURE)
-		current->mm->def_flags |= VM_LOCKED | VM_POPULATE;
+		current->mm->def_flags |= VM_LOCKED;
 	else
-		current->mm->def_flags &= ~(VM_LOCKED | VM_POPULATE);
+		current->mm->def_flags &= ~VM_LOCKED;
 	if (flags == MCL_FUTURE)
 		goto out;
 
@@ -503,7 +502,7 @@ static int do_mlockall(int flags)
 
 		newflags = vma->vm_flags & ~VM_LOCKED;
 		if (flags & MCL_CURRENT)
-			newflags |= VM_LOCKED | VM_POPULATE;
+			newflags |= VM_LOCKED;
 
 		/* Ignore errors */
 		mlock_fixup(vma, &prev, vma->vm_start, vma->vm_end, newflags);
diff --git a/mm/mmap.c b/mm/mmap.c
index 2664a47cec9..da3e9c04bf3 100644
--- a/mm/mmap.c
+++ b/mm/mmap.c
@@ -6,6 +6,7 @@
  * Address space accounting code	<alan@lxorguk.ukuu.org.uk>
  */
 
+#include <linux/kernel.h>
 #include <linux/slab.h>
 #include <linux/backing-dev.h>
 #include <linux/mm.h>
@@ -33,6 +34,8 @@
 #include <linux/uprobes.h>
 #include <linux/rbtree_augmented.h>
 #include <linux/sched/sysctl.h>
+#include <linux/notifier.h>
+#include <linux/memory.h>
 
 #include <asm/uaccess.h>
 #include <asm/cacheflush.h>
@@ -84,6 +87,8 @@ EXPORT_SYMBOL(vm_get_page_prot);
 int sysctl_overcommit_memory __read_mostly = OVERCOMMIT_GUESS;  /* heuristic overcommit */
 int sysctl_overcommit_ratio __read_mostly = 50;	/* default is 50% */
 int sysctl_max_map_count __read_mostly = DEFAULT_MAX_MAP_COUNT;
+unsigned long sysctl_user_reserve_kbytes __read_mostly = 1UL << 17; /* 128MB */
+unsigned long sysctl_admin_reserve_kbytes __read_mostly = 1UL << 13; /* 8MB */
 /*
  * Make sure vm_committed_as in one cacheline and not cacheline shared with
  * other variables. It can be updated by several CPUs frequently.
@@ -122,7 +127,7 @@ EXPORT_SYMBOL_GPL(vm_memory_committed);
  */
 int __vm_enough_memory(struct mm_struct *mm, long pages, int cap_sys_admin)
 {
-	unsigned long free, allowed;
+	unsigned long free, allowed, reserve;
 
 	vm_acct_memory(pages);
 
@@ -163,10 +168,10 @@ int __vm_enough_memory(struct mm_struct *mm, long pages, int cap_sys_admin)
 			free -= totalreserve_pages;
 
 		/*
-		 * Leave the last 3% for root
+		 * Reserve some for root
 		 */
 		if (!cap_sys_admin)
-			free -= free / 32;
+			free -= sysctl_admin_reserve_kbytes >> (PAGE_SHIFT - 10);
 
 		if (free > pages)
 			return 0;
@@ -177,16 +182,19 @@ int __vm_enough_memory(struct mm_struct *mm, long pages, int cap_sys_admin)
 	allowed = (totalram_pages - hugetlb_total_pages())
 	       	* sysctl_overcommit_ratio / 100;
 	/*
-	 * Leave the last 3% for root
+	 * Reserve some for root
 	 */
 	if (!cap_sys_admin)
-		allowed -= allowed / 32;
+		allowed -= sysctl_admin_reserve_kbytes >> (PAGE_SHIFT - 10);
 	allowed += total_swap_pages;
 
-	/* Don't let a single process grow too big:
-	   leave 3% of the size of this process for other processes */
-	if (mm)
-		allowed -= mm->total_vm / 32;
+	/*
+	 * Don't let a single process grow so big a user can't recover
+	 */
+	if (mm) {
+		reserve = sysctl_user_reserve_kbytes >> (PAGE_SHIFT - 10);
+		allowed -= min(mm->total_vm / 32, reserve);
+	}
 
 	if (percpu_counter_read_positive(&vm_committed_as) < allowed)
 		return 0;
@@ -543,6 +551,34 @@ static int find_vma_links(struct mm_struct *mm, unsigned long addr,
 	return 0;
 }
 
+static unsigned long count_vma_pages_range(struct mm_struct *mm,
+		unsigned long addr, unsigned long end)
+{
+	unsigned long nr_pages = 0;
+	struct vm_area_struct *vma;
+
+	/* Find first overlaping mapping */
+	vma = find_vma_intersection(mm, addr, end);
+	if (!vma)
+		return 0;
+
+	nr_pages = (min(end, vma->vm_end) -
+		max(addr, vma->vm_start)) >> PAGE_SHIFT;
+
+	/* Iterate over the rest of the overlaps */
+	for (vma = vma->vm_next; vma; vma = vma->vm_next) {
+		unsigned long overlap_len;
+
+		if (vma->vm_start > end)
+			break;
+
+		overlap_len = min(end, vma->vm_end) - vma->vm_start;
+		nr_pages += overlap_len >> PAGE_SHIFT;
+	}
+
+	return nr_pages;
+}
+
 void __vma_link_rb(struct mm_struct *mm, struct vm_area_struct *vma,
 		struct rb_node **rb_link, struct rb_node *rb_parent)
 {
@@ -829,7 +865,7 @@ again:			remove_next = 1 + (end > next->vm_end);
 		if (next->anon_vma)
 			anon_vma_merge(vma, next);
 		mm->map_count--;
-		mpol_put(vma_policy(next));
+		vma_set_policy(vma, vma_policy(next));
 		kmem_cache_free(vm_area_cachep, next);
 		/*
 		 * In mprotect's case 6 (see comments on vma_merge),
@@ -1306,7 +1342,9 @@ unsigned long do_mmap_pgoff(struct file *file, unsigned long addr,
 	}
 
 	addr = mmap_region(file, addr, len, vm_flags, pgoff);
-	if (!IS_ERR_VALUE(addr) && (vm_flags & VM_POPULATE))
+	if (!IS_ERR_VALUE(addr) &&
+	    ((vm_flags & VM_LOCKED) ||
+	     (flags & (MAP_POPULATE | MAP_NONBLOCK)) == MAP_POPULATE))
 		*populate = len;
 	return addr;
 }
@@ -1433,6 +1471,23 @@ unsigned long mmap_region(struct file *file, unsigned long addr,
 	unsigned long charged = 0;
 	struct inode *inode =  file ? file_inode(file) : NULL;
 
+	/* Check against address space limit. */
+	if (!may_expand_vm(mm, len >> PAGE_SHIFT)) {
+		unsigned long nr_pages;
+
+		/*
+		 * MAP_FIXED may remove pages of mappings that intersects with
+		 * requested mapping. Account for the pages it would unmap.
+		 */
+		if (!(vm_flags & MAP_FIXED))
+			return -ENOMEM;
+
+		nr_pages = count_vma_pages_range(mm, addr, addr + len);
+
+		if (!may_expand_vm(mm, (len >> PAGE_SHIFT) - nr_pages))
+			return -ENOMEM;
+	}
+
 	/* Clear old maps */
 	error = -ENOMEM;
 munmap_back:
@@ -1442,10 +1497,6 @@ munmap_back:
 		goto munmap_back;
 	}
 
-	/* Check against address space limit. */
-	if (!may_expand_vm(mm, len >> PAGE_SHIFT))
-		return -ENOMEM;
-
 	/*
 	 * Private writable mapping: check memory availability
 	 */
@@ -1933,12 +1984,9 @@ struct vm_area_struct *find_vma(struct mm_struct *mm, unsigned long addr)
 {
 	struct vm_area_struct *vma = NULL;
 
-	if (WARN_ON_ONCE(!mm))		/* Remove this in linux-3.6 */
-		return NULL;
-
 	/* Check the cache first. */
 	/* (Cache hit rate is typically around 35%.) */
-	vma = mm->mmap_cache;
+	vma = ACCESS_ONCE(mm->mmap_cache);
 	if (!(vma && vma->vm_end > addr && vma->vm_start <= addr)) {
 		struct rb_node *rb_node;
 
@@ -2303,7 +2351,7 @@ static void unmap_region(struct mm_struct *mm,
 	update_hiwater_rss(mm);
 	unmap_vmas(&tlb, vma, start, end);
 	free_pgtables(&tlb, vma, prev ? prev->vm_end : FIRST_USER_ADDRESS,
-				 next ? next->vm_start : 0);
+				 next ? next->vm_start : USER_PGTABLES_CEILING);
 	tlb_finish_mmu(&tlb, start, end);
 }
 
@@ -2683,7 +2731,7 @@ void exit_mmap(struct mm_struct *mm)
 	/* Use -1 here to ensure all VMAs in the mm are unmapped */
 	unmap_vmas(&tlb, vma, 0, -1);
 
-	free_pgtables(&tlb, vma, FIRST_USER_ADDRESS, 0);
+	free_pgtables(&tlb, vma, FIRST_USER_ADDRESS, USER_PGTABLES_CEILING);
 	tlb_finish_mmu(&tlb, 0, -1);
 
 	/*
@@ -3095,3 +3143,115 @@ void __init mmap_init(void)
 	ret = percpu_counter_init(&vm_committed_as, 0);
 	VM_BUG_ON(ret);
 }
+
+/*
+ * Initialise sysctl_user_reserve_kbytes.
+ *
+ * This is intended to prevent a user from starting a single memory hogging
+ * process, such that they cannot recover (kill the hog) in OVERCOMMIT_NEVER
+ * mode.
+ *
+ * The default value is min(3% of free memory, 128MB)
+ * 128MB is enough to recover with sshd/login, bash, and top/kill.
+ */
+static int init_user_reserve(void)
+{
+	unsigned long free_kbytes;
+
+	free_kbytes = global_page_state(NR_FREE_PAGES) << (PAGE_SHIFT - 10);
+
+	sysctl_user_reserve_kbytes = min(free_kbytes / 32, 1UL << 17);
+	return 0;
+}
+module_init(init_user_reserve)
+
+/*
+ * Initialise sysctl_admin_reserve_kbytes.
+ *
+ * The purpose of sysctl_admin_reserve_kbytes is to allow the sys admin
+ * to log in and kill a memory hogging process.
+ *
+ * Systems with more than 256MB will reserve 8MB, enough to recover
+ * with sshd, bash, and top in OVERCOMMIT_GUESS. Smaller systems will
+ * only reserve 3% of free pages by default.
+ */
+static int init_admin_reserve(void)
+{
+	unsigned long free_kbytes;
+
+	free_kbytes = global_page_state(NR_FREE_PAGES) << (PAGE_SHIFT - 10);
+
+	sysctl_admin_reserve_kbytes = min(free_kbytes / 32, 1UL << 13);
+	return 0;
+}
+module_init(init_admin_reserve)
+
+/*
+ * Reinititalise user and admin reserves if memory is added or removed.
+ *
+ * The default user reserve max is 128MB, and the default max for the
+ * admin reserve is 8MB. These are usually, but not always, enough to
+ * enable recovery from a memory hogging process using login/sshd, a shell,
+ * and tools like top. It may make sense to increase or even disable the
+ * reserve depending on the existence of swap or variations in the recovery
+ * tools. So, the admin may have changed them.
+ *
+ * If memory is added and the reserves have been eliminated or increased above
+ * the default max, then we'll trust the admin.
+ *
+ * If memory is removed and there isn't enough free memory, then we
+ * need to reset the reserves.
+ *
+ * Otherwise keep the reserve set by the admin.
+ */
+static int reserve_mem_notifier(struct notifier_block *nb,
+			     unsigned long action, void *data)
+{
+	unsigned long tmp, free_kbytes;
+
+	switch (action) {
+	case MEM_ONLINE:
+		/* Default max is 128MB. Leave alone if modified by operator. */
+		tmp = sysctl_user_reserve_kbytes;
+		if (0 < tmp && tmp < (1UL << 17))
+			init_user_reserve();
+
+		/* Default max is 8MB.  Leave alone if modified by operator. */
+		tmp = sysctl_admin_reserve_kbytes;
+		if (0 < tmp && tmp < (1UL << 13))
+			init_admin_reserve();
+
+		break;
+	case MEM_OFFLINE:
+		free_kbytes = global_page_state(NR_FREE_PAGES) << (PAGE_SHIFT - 10);
+
+		if (sysctl_user_reserve_kbytes > free_kbytes) {
+			init_user_reserve();
+			pr_info("vm.user_reserve_kbytes reset to %lu\n",
+				sysctl_user_reserve_kbytes);
+		}
+
+		if (sysctl_admin_reserve_kbytes > free_kbytes) {
+			init_admin_reserve();
+			pr_info("vm.admin_reserve_kbytes reset to %lu\n",
+				sysctl_admin_reserve_kbytes);
+		}
+		break;
+	default:
+		break;
+	}
+	return NOTIFY_OK;
+}
+
+static struct notifier_block reserve_mem_nb = {
+	.notifier_call = reserve_mem_notifier,
+};
+
+static int __meminit init_reserve_notifier(void)
+{
+	if (register_hotmemory_notifier(&reserve_mem_nb))
+		printk("Failed registering memory add/remove notifier for admin reserve");
+
+	return 0;
+}
+module_init(init_reserve_notifier)
diff --git a/mm/nobootmem.c b/mm/nobootmem.c
index 5e07d36e381..bdd3fa2fc73 100644
--- a/mm/nobootmem.c
+++ b/mm/nobootmem.c
@@ -45,9 +45,9 @@ static void * __init __alloc_memory_core_early(int nid, u64 size, u64 align,
 	if (!addr)
 		return NULL;
 
+	memblock_reserve(addr, size);
 	ptr = phys_to_virt(addr);
 	memset(ptr, 0, size);
-	memblock_reserve(addr, size);
 	/*
 	 * The min_count is set to 0 so that bootmem allocated blocks
 	 * are never reported as leaks.
@@ -120,7 +120,7 @@ static unsigned long __init __free_memory_core(phys_addr_t start,
 	return end_pfn - start_pfn;
 }
 
-unsigned long __init free_low_memory_core_early(int nodeid)
+static unsigned long __init free_low_memory_core_early(void)
 {
 	unsigned long count = 0;
 	phys_addr_t start, end, size;
@@ -170,7 +170,7 @@ unsigned long __init free_all_bootmem(void)
 	 *  because in some case like Node0 doesn't have RAM installed
 	 *  low ram will be on Node1
 	 */
-	return free_low_memory_core_early(MAX_NUMNODES);
+	return free_low_memory_core_early();
 }
 
 /**
diff --git a/mm/nommu.c b/mm/nommu.c
index e1932808753..fbe3e2f317e 100644
--- a/mm/nommu.c
+++ b/mm/nommu.c
@@ -63,6 +63,8 @@ int sysctl_overcommit_memory = OVERCOMMIT_GUESS; /* heuristic overcommit */
 int sysctl_overcommit_ratio = 50; /* default is 50% */
 int sysctl_max_map_count = DEFAULT_MAX_MAP_COUNT;
 int sysctl_nr_trim_pages = CONFIG_NOMMU_INITIAL_TRIM_EXCESS;
+unsigned long sysctl_user_reserve_kbytes __read_mostly = 1UL << 17; /* 128MB */
+unsigned long sysctl_admin_reserve_kbytes __read_mostly = 1UL << 13; /* 8MB */
 int heap_stack_gap = 0;
 
 atomic_long_t mmap_pages_allocated;
@@ -228,8 +230,7 @@ int follow_pfn(struct vm_area_struct *vma, unsigned long address,
 }
 EXPORT_SYMBOL(follow_pfn);
 
-DEFINE_RWLOCK(vmlist_lock);
-struct vm_struct *vmlist;
+LIST_HEAD(vmap_area_list);
 
 void vfree(const void *addr)
 {
@@ -821,7 +822,7 @@ struct vm_area_struct *find_vma(struct mm_struct *mm, unsigned long addr)
 	struct vm_area_struct *vma;
 
 	/* check the cache first */
-	vma = mm->mmap_cache;
+	vma = ACCESS_ONCE(mm->mmap_cache);
 	if (vma && vma->vm_start <= addr && vma->vm_end > addr)
 		return vma;
 
@@ -1838,6 +1839,16 @@ int remap_pfn_range(struct vm_area_struct *vma, unsigned long addr,
 }
 EXPORT_SYMBOL(remap_pfn_range);
 
+int vm_iomap_memory(struct vm_area_struct *vma, phys_addr_t start, unsigned long len)
+{
+	unsigned long pfn = start >> PAGE_SHIFT;
+	unsigned long vm_len = vma->vm_end - vma->vm_start;
+
+	pfn += vma->vm_pgoff;
+	return io_remap_pfn_range(vma, vma->vm_start, pfn, vm_len, vma->vm_page_prot);
+}
+EXPORT_SYMBOL(vm_iomap_memory);
+
 int remap_vmalloc_range(struct vm_area_struct *vma, void *addr,
 			unsigned long pgoff)
 {
@@ -1888,7 +1899,7 @@ EXPORT_SYMBOL(unmap_mapping_range);
  */
 int __vm_enough_memory(struct mm_struct *mm, long pages, int cap_sys_admin)
 {
-	unsigned long free, allowed;
+	unsigned long free, allowed, reserve;
 
 	vm_acct_memory(pages);
 
@@ -1929,10 +1940,10 @@ int __vm_enough_memory(struct mm_struct *mm, long pages, int cap_sys_admin)
 			free -= totalreserve_pages;
 
 		/*
-		 * Leave the last 3% for root
+		 * Reserve some for root
 		 */
 		if (!cap_sys_admin)
-			free -= free / 32;
+			free -= sysctl_admin_reserve_kbytes >> (PAGE_SHIFT - 10);
 
 		if (free > pages)
 			return 0;
@@ -1942,16 +1953,19 @@ int __vm_enough_memory(struct mm_struct *mm, long pages, int cap_sys_admin)
 
 	allowed = totalram_pages * sysctl_overcommit_ratio / 100;
 	/*
-	 * Leave the last 3% for root
+	 * Reserve some 3% for root
 	 */
 	if (!cap_sys_admin)
-		allowed -= allowed / 32;
+		allowed -= sysctl_admin_reserve_kbytes >> (PAGE_SHIFT - 10);
 	allowed += total_swap_pages;
 
-	/* Don't let a single process grow too big:
-	   leave 3% of the size of this process for other processes */
-	if (mm)
-		allowed -= mm->total_vm / 32;
+	/*
+	 * Don't let a single process grow so big a user can't recover
+	 */
+	if (mm) {
+		reserve = sysctl_user_reserve_kbytes >> (PAGE_SHIFT - 10);
+		allowed -= min(mm->total_vm / 32, reserve);
+	}
 
 	if (percpu_counter_read_positive(&vm_committed_as) < allowed)
 		return 0;
@@ -2113,3 +2127,45 @@ int nommu_shrink_inode_mappings(struct inode *inode, size_t size,
 	up_write(&nommu_region_sem);
 	return 0;
 }
+
+/*
+ * Initialise sysctl_user_reserve_kbytes.
+ *
+ * This is intended to prevent a user from starting a single memory hogging
+ * process, such that they cannot recover (kill the hog) in OVERCOMMIT_NEVER
+ * mode.
+ *
+ * The default value is min(3% of free memory, 128MB)
+ * 128MB is enough to recover with sshd/login, bash, and top/kill.
+ */
+static int __meminit init_user_reserve(void)
+{
+	unsigned long free_kbytes;
+
+	free_kbytes = global_page_state(NR_FREE_PAGES) << (PAGE_SHIFT - 10);
+
+	sysctl_user_reserve_kbytes = min(free_kbytes / 32, 1UL << 17);
+	return 0;
+}
+module_init(init_user_reserve)
+
+/*
+ * Initialise sysctl_admin_reserve_kbytes.
+ *
+ * The purpose of sysctl_admin_reserve_kbytes is to allow the sys admin
+ * to log in and kill a memory hogging process.
+ *
+ * Systems with more than 256MB will reserve 8MB, enough to recover
+ * with sshd, bash, and top in OVERCOMMIT_GUESS. Smaller systems will
+ * only reserve 3% of free pages by default.
+ */
+static int __meminit init_admin_reserve(void)
+{
+	unsigned long free_kbytes;
+
+	free_kbytes = global_page_state(NR_FREE_PAGES) << (PAGE_SHIFT - 10);
+
+	sysctl_admin_reserve_kbytes = min(free_kbytes / 32, 1UL << 13);
+	return 0;
+}
+module_init(init_admin_reserve)
diff --git a/mm/page-writeback.c b/mm/page-writeback.c
index efe68148f62..4514ad7415c 100644
--- a/mm/page-writeback.c
+++ b/mm/page-writeback.c
@@ -2311,10 +2311,6 @@ void wait_for_stable_page(struct page *page)
 
 	if (!bdi_cap_stable_pages_required(bdi))
 		return;
-#ifdef CONFIG_NEED_BOUNCE_POOL
-	if (mapping->host->i_sb->s_flags & MS_SNAP_STABLE)
-		return;
-#endif /* CONFIG_NEED_BOUNCE_POOL */
 
 	wait_on_page_writeback(page);
 }
diff --git a/mm/page_alloc.c b/mm/page_alloc.c
index 8fcced7823f..98cbdf6e553 100644
--- a/mm/page_alloc.c
+++ b/mm/page_alloc.c
@@ -58,6 +58,7 @@
 #include <linux/prefetch.h>
 #include <linux/migrate.h>
 #include <linux/page-debug-flags.h>
+#include <linux/hugetlb.h>
 #include <linux/sched/rt.h>
 
 #include <asm/tlbflush.h>
@@ -1397,6 +1398,7 @@ void split_page(struct page *page, unsigned int order)
 	for (i = 1; i < (1 << order); i++)
 		set_page_refcounted(page + i);
 }
+EXPORT_SYMBOL_GPL(split_page);
 
 static int __isolate_free_page(struct page *page, unsigned int order)
 {
@@ -1940,9 +1942,24 @@ zonelist_scan:
 				continue;
 			default:
 				/* did we reclaim enough */
-				if (!zone_watermark_ok(zone, order, mark,
+				if (zone_watermark_ok(zone, order, mark,
 						classzone_idx, alloc_flags))
+					goto try_this_zone;
+
+				/*
+				 * Failed to reclaim enough to meet watermark.
+				 * Only mark the zone full if checking the min
+				 * watermark or if we failed to reclaim just
+				 * 1<<order pages or else the page allocator
+				 * fastpath will prematurely mark zones full
+				 * when the watermark is between the low and
+				 * min watermarks.
+				 */
+				if (((alloc_flags & ALLOC_WMARK_MASK) == ALLOC_WMARK_MIN) ||
+				    ret == ZONE_RECLAIM_SOME)
 					goto this_zone_full;
+
+				continue;
 			}
 		}
 
@@ -2002,6 +2019,13 @@ void warn_alloc_failed(gfp_t gfp_mask, int order, const char *fmt, ...)
 		return;
 
 	/*
+	 * Walking all memory to count page types is very expensive and should
+	 * be inhibited in non-blockable contexts.
+	 */
+	if (!(gfp_mask & __GFP_WAIT))
+		filter |= SHOW_MEM_FILTER_PAGE_COUNT;
+
+	/*
 	 * This documents exceptions given to allocations in certain
 	 * contexts that are allowed to allocate outside current's set
 	 * of allowed nodes.
@@ -3105,6 +3129,8 @@ void show_free_areas(unsigned int filter)
 		printk("= %lukB\n", K(total));
 	}
 
+	hugetlb_show_meminfo();
+
 	printk("%ld total pagecache pages\n", global_page_state(NR_FILE_PAGES));
 
 	show_swap_cache_info();
@@ -4161,10 +4187,23 @@ int __meminit __early_pfn_to_nid(unsigned long pfn)
 {
 	unsigned long start_pfn, end_pfn;
 	int i, nid;
+	/*
+	 * NOTE: The following SMP-unsafe globals are only used early in boot
+	 * when the kernel is running single-threaded.
+	 */
+	static unsigned long __meminitdata last_start_pfn, last_end_pfn;
+	static int __meminitdata last_nid;
+
+	if (last_start_pfn <= pfn && pfn < last_end_pfn)
+		return last_nid;
 
 	for_each_mem_pfn_range(i, MAX_NUMNODES, &start_pfn, &end_pfn, &nid)
-		if (start_pfn <= pfn && pfn < end_pfn)
+		if (start_pfn <= pfn && pfn < end_pfn) {
+			last_start_pfn = start_pfn;
+			last_end_pfn = end_pfn;
+			last_nid = nid;
 			return nid;
+		}
 	/* This is a memory hole */
 	return -1;
 }
@@ -4710,7 +4749,7 @@ void __paginginit free_area_init_node(int nid, unsigned long *zones_size,
 /*
  * Figure out the number of possible node ids.
  */
-static void __init setup_nr_node_ids(void)
+void __init setup_nr_node_ids(void)
 {
 	unsigned int node;
 	unsigned int highest = 0;
@@ -4719,10 +4758,6 @@ static void __init setup_nr_node_ids(void)
 		highest = node;
 	nr_node_ids = highest + 1;
 }
-#else
-static inline void setup_nr_node_ids(void)
-{
-}
 #endif
 
 /**
@@ -5113,6 +5148,35 @@ early_param("movablecore", cmdline_parse_movablecore);
 
 #endif /* CONFIG_HAVE_MEMBLOCK_NODE_MAP */
 
+unsigned long free_reserved_area(unsigned long start, unsigned long end,
+				 int poison, char *s)
+{
+	unsigned long pages, pos;
+
+	pos = start = PAGE_ALIGN(start);
+	end &= PAGE_MASK;
+	for (pages = 0; pos < end; pos += PAGE_SIZE, pages++) {
+		if (poison)
+			memset((void *)pos, poison, PAGE_SIZE);
+		free_reserved_page(virt_to_page(pos));
+	}
+
+	if (pages && s)
+		pr_info("Freeing %s memory: %ldK (%lx - %lx)\n",
+			s, pages << (PAGE_SHIFT - 10), start, end);
+
+	return pages;
+}
+
+#ifdef	CONFIG_HIGHMEM
+void free_highmem_page(struct page *page)
+{
+	__free_reserved_page(page);
+	totalram_pages++;
+	totalhigh_pages++;
+}
+#endif
+
 /**
  * set_dma_reserve - set the specified number of pages reserved in the first zone
  * @new_dma_reserve: The number of pages to mark reserved
diff --git a/mm/page_io.c b/mm/page_io.c
index 78eee32ee48..bb5d7527468 100644
--- a/mm/page_io.c
+++ b/mm/page_io.c
@@ -42,7 +42,7 @@ static struct bio *get_swap_bio(gfp_t gfp_flags,
 	return bio;
 }
 
-static void end_swap_bio_write(struct bio *bio, int err)
+void end_swap_bio_write(struct bio *bio, int err)
 {
 	const int uptodate = test_bit(BIO_UPTODATE, &bio->bi_flags);
 	struct page *page = bio->bi_io_vec[0].bv_page;
@@ -185,9 +185,7 @@ bad_bmap:
  */
 int swap_writepage(struct page *page, struct writeback_control *wbc)
 {
-	struct bio *bio;
-	int ret = 0, rw = WRITE;
-	struct swap_info_struct *sis = page_swap_info(page);
+	int ret = 0;
 
 	if (try_to_free_swap(page)) {
 		unlock_page(page);
@@ -199,6 +197,17 @@ int swap_writepage(struct page *page, struct writeback_control *wbc)
 		end_page_writeback(page);
 		goto out;
 	}
+	ret = __swap_writepage(page, wbc, end_swap_bio_write);
+out:
+	return ret;
+}
+
+int __swap_writepage(struct page *page, struct writeback_control *wbc,
+	void (*end_write_func)(struct bio *, int))
+{
+	struct bio *bio;
+	int ret = 0, rw = WRITE;
+	struct swap_info_struct *sis = page_swap_info(page);
 
 	if (sis->flags & SWP_FILE) {
 		struct kiocb kiocb;
@@ -214,6 +223,7 @@ int swap_writepage(struct page *page, struct writeback_control *wbc)
 		kiocb.ki_left = PAGE_SIZE;
 		kiocb.ki_nbytes = PAGE_SIZE;
 
+		set_page_writeback(page);
 		unlock_page(page);
 		ret = mapping->a_ops->direct_IO(KERNEL_WRITE,
 						&kiocb, &iov,
@@ -222,11 +232,27 @@ int swap_writepage(struct page *page, struct writeback_control *wbc)
 		if (ret == PAGE_SIZE) {
 			count_vm_event(PSWPOUT);
 			ret = 0;
+		} else {
+			/*
+			 * In the case of swap-over-nfs, this can be a
+			 * temporary failure if the system has limited
+			 * memory for allocating transmit buffers.
+			 * Mark the page dirty and avoid
+			 * rotate_reclaimable_page but rate-limit the
+			 * messages but do not flag PageError like
+			 * the normal direct-to-bio case as it could
+			 * be temporary.
+			 */
+			set_page_dirty(page);
+			ClearPageReclaim(page);
+			pr_err_ratelimited("Write error on dio swapfile (%Lu)\n",
+				page_file_offset(page));
 		}
+		end_page_writeback(page);
 		return ret;
 	}
 
-	bio = get_swap_bio(GFP_NOIO, page, end_swap_bio_write);
+	bio = get_swap_bio(GFP_NOIO, page, end_write_func);
 	if (bio == NULL) {
 		set_page_dirty(page);
 		unlock_page(page);
diff --git a/mm/rmap.c b/mm/rmap.c
index 807c96bf0dc..6280da86b5d 100644
--- a/mm/rmap.c
+++ b/mm/rmap.c
@@ -1513,6 +1513,9 @@ static int try_to_unmap_file(struct page *page, enum ttu_flags flags)
 	unsigned long max_nl_size = 0;
 	unsigned int mapcount;
 
+	if (PageHuge(page))
+		pgoff = page->index << compound_order(page);
+
 	mutex_lock(&mapping->i_mmap_mutex);
 	vma_interval_tree_foreach(vma, &mapping->i_mmap, pgoff, pgoff) {
 		unsigned long address = vma_address(page, vma);
diff --git a/mm/shmem.c b/mm/shmem.c
index 1c44af71fcf..39b2a0b86fe 100644
--- a/mm/shmem.c
+++ b/mm/shmem.c
@@ -25,6 +25,7 @@
 #include <linux/init.h>
 #include <linux/vfs.h>
 #include <linux/mount.h>
+#include <linux/ramfs.h>
 #include <linux/pagemap.h>
 #include <linux/file.h>
 #include <linux/mm.h>
@@ -2830,8 +2831,6 @@ out4:
  * effectively equivalent, but much lighter weight.
  */
 
-#include <linux/ramfs.h>
-
 static struct file_system_type shmem_fs_type = {
 	.name		= "tmpfs",
 	.mount		= ramfs_mount,
@@ -2931,11 +2930,9 @@ struct file *shmem_file_setup(const char *name, loff_t size, unsigned long flags
 	d_instantiate(path.dentry, inode);
 	inode->i_size = size;
 	clear_nlink(inode);	/* It is unlinked */
-#ifndef CONFIG_MMU
 	res = ERR_PTR(ramfs_nommu_expand_for_mapping(inode, size));
 	if (IS_ERR(res))
 		goto put_dentry;
-#endif
 
 	res = alloc_file(&path, FMODE_WRITE | FMODE_READ,
 		  &shmem_file_operations);
diff --git a/mm/slab.c b/mm/slab.c
index 856e4a192d2..96079244c86 100644
--- a/mm/slab.c
+++ b/mm/slab.c
@@ -2040,11 +2040,9 @@ static void print_objinfo(struct kmem_cache *cachep, void *objp, int lines)
 	}
 
 	if (cachep->flags & SLAB_STORE_USER) {
-		printk(KERN_ERR "Last user: [<%p>]",
-			*dbg_userword(cachep, objp));
-		print_symbol("(%s)",
-				(unsigned long)*dbg_userword(cachep, objp));
-		printk("\n");
+		printk(KERN_ERR "Last user: [<%p>](%pSR)\n",
+		       *dbg_userword(cachep, objp),
+		       *dbg_userword(cachep, objp));
 	}
 	realobj = (char *)objp + obj_offset(cachep);
 	size = cachep->object_size;
diff --git a/mm/slub.c b/mm/slub.c
index 4aec53705e4..a0206df88ab 100644
--- a/mm/slub.c
+++ b/mm/slub.c
@@ -18,6 +18,7 @@
 #include <linux/slab.h>
 #include "slab.h"
 #include <linux/proc_fs.h>
+#include <linux/notifier.h>
 #include <linux/seq_file.h>
 #include <linux/kmemcheck.h>
 #include <linux/cpu.h>
@@ -3483,7 +3484,6 @@ int kmem_cache_shrink(struct kmem_cache *s)
 }
 EXPORT_SYMBOL(kmem_cache_shrink);
 
-#if defined(CONFIG_MEMORY_HOTPLUG)
 static int slab_mem_going_offline_callback(void *arg)
 {
 	struct kmem_cache *s;
@@ -3598,7 +3598,10 @@ static int slab_memory_callback(struct notifier_block *self,
 	return ret;
 }
 
-#endif /* CONFIG_MEMORY_HOTPLUG */
+static struct notifier_block slab_memory_callback_nb = {
+	.notifier_call = slab_memory_callback,
+	.priority = SLAB_CALLBACK_PRI,
+};
 
 /********************************************************************
  *			Basic setup of slabs
@@ -3651,7 +3654,7 @@ void __init kmem_cache_init(void)
 	create_boot_cache(kmem_cache_node, "kmem_cache_node",
 		sizeof(struct kmem_cache_node), SLAB_HWCACHE_ALIGN);
 
-	hotplug_memory_notifier(slab_memory_callback, SLAB_CALLBACK_PRI);
+	register_hotmemory_notifier(&slab_memory_callback_nb);
 
 	/* Able to allocate the per node structures */
 	slab_state = PARTIAL;
diff --git a/mm/sparse-vmemmap.c b/mm/sparse-vmemmap.c
index 1b7e22ab9b0..27eeab3be75 100644
--- a/mm/sparse-vmemmap.c
+++ b/mm/sparse-vmemmap.c
@@ -53,10 +53,12 @@ void * __meminit vmemmap_alloc_block(unsigned long size, int node)
 		struct page *page;
 
 		if (node_state(node, N_HIGH_MEMORY))
-			page = alloc_pages_node(node,
-				GFP_KERNEL | __GFP_ZERO, get_order(size));
+			page = alloc_pages_node(
+				node, GFP_KERNEL | __GFP_ZERO | __GFP_REPEAT,
+				get_order(size));
 		else
-			page = alloc_pages(GFP_KERNEL | __GFP_ZERO,
+			page = alloc_pages(
+				GFP_KERNEL | __GFP_ZERO | __GFP_REPEAT,
 				get_order(size));
 		if (page)
 			return page_address(page);
@@ -145,11 +147,10 @@ pgd_t * __meminit vmemmap_pgd_populate(unsigned long addr, int node)
 	return pgd;
 }
 
-int __meminit vmemmap_populate_basepages(struct page *start_page,
-						unsigned long size, int node)
+int __meminit vmemmap_populate_basepages(unsigned long start,
+					 unsigned long end, int node)
 {
-	unsigned long addr = (unsigned long)start_page;
-	unsigned long end = (unsigned long)(start_page + size);
+	unsigned long addr = start;
 	pgd_t *pgd;
 	pud_t *pud;
 	pmd_t *pmd;
@@ -176,9 +177,15 @@ int __meminit vmemmap_populate_basepages(struct page *start_page,
 
 struct page * __meminit sparse_mem_map_populate(unsigned long pnum, int nid)
 {
-	struct page *map = pfn_to_page(pnum * PAGES_PER_SECTION);
-	int error = vmemmap_populate(map, PAGES_PER_SECTION, nid);
-	if (error)
+	unsigned long start;
+	unsigned long end;
+	struct page *map;
+
+	map = pfn_to_page(pnum * PAGES_PER_SECTION);
+	start = (unsigned long)map;
+	end = (unsigned long)(map + PAGES_PER_SECTION);
+
+	if (vmemmap_populate(start, end, nid))
 		return NULL;
 
 	return map;
diff --git a/mm/sparse.c b/mm/sparse.c
index 7ca6dc84794..1c91f0d3f6a 100644
--- a/mm/sparse.c
+++ b/mm/sparse.c
@@ -615,12 +615,20 @@ static inline struct page *kmalloc_section_memmap(unsigned long pnum, int nid,
 }
 static void __kfree_section_memmap(struct page *memmap, unsigned long nr_pages)
 {
-	vmemmap_free(memmap, nr_pages);
+	unsigned long start = (unsigned long)memmap;
+	unsigned long end = (unsigned long)(memmap + nr_pages);
+
+	vmemmap_free(start, end);
 }
+#ifdef CONFIG_MEMORY_HOTREMOVE
 static void free_map_bootmem(struct page *memmap, unsigned long nr_pages)
 {
-	vmemmap_free(memmap, nr_pages);
+	unsigned long start = (unsigned long)memmap;
+	unsigned long end = (unsigned long)(memmap + nr_pages);
+
+	vmemmap_free(start, end);
 }
+#endif /* CONFIG_MEMORY_HOTREMOVE */
 #else
 static struct page *__kmalloc_section_memmap(unsigned long nr_pages)
 {
@@ -658,6 +666,7 @@ static void __kfree_section_memmap(struct page *memmap, unsigned long nr_pages)
 			   get_order(sizeof(struct page) * nr_pages));
 }
 
+#ifdef CONFIG_MEMORY_HOTREMOVE
 static void free_map_bootmem(struct page *memmap, unsigned long nr_pages)
 {
 	unsigned long maps_section_nr, removing_section_nr, i;
@@ -684,40 +693,9 @@ static void free_map_bootmem(struct page *memmap, unsigned long nr_pages)
 			put_page_bootmem(page);
 	}
 }
+#endif /* CONFIG_MEMORY_HOTREMOVE */
 #endif /* CONFIG_SPARSEMEM_VMEMMAP */
 
-static void free_section_usemap(struct page *memmap, unsigned long *usemap)
-{
-	struct page *usemap_page;
-	unsigned long nr_pages;
-
-	if (!usemap)
-		return;
-
-	usemap_page = virt_to_page(usemap);
-	/*
-	 * Check to see if allocation came from hot-plug-add
-	 */
-	if (PageSlab(usemap_page) || PageCompound(usemap_page)) {
-		kfree(usemap);
-		if (memmap)
-			__kfree_section_memmap(memmap, PAGES_PER_SECTION);
-		return;
-	}
-
-	/*
-	 * The usemap came from bootmem. This is packed with other usemaps
-	 * on the section which has pgdat at boot time. Just keep it as is now.
-	 */
-
-	if (memmap) {
-		nr_pages = PAGE_ALIGN(PAGES_PER_SECTION * sizeof(struct page))
-			>> PAGE_SHIFT;
-
-		free_map_bootmem(memmap, nr_pages);
-	}
-}
-
 /*
  * returns the number of sections whose mem_maps were properly
  * set.  If this is <=0, then that means that the passed-in
@@ -794,6 +772,39 @@ static inline void clear_hwpoisoned_pages(struct page *memmap, int nr_pages)
 }
 #endif
 
+#ifdef CONFIG_MEMORY_HOTREMOVE
+static void free_section_usemap(struct page *memmap, unsigned long *usemap)
+{
+	struct page *usemap_page;
+	unsigned long nr_pages;
+
+	if (!usemap)
+		return;
+
+	usemap_page = virt_to_page(usemap);
+	/*
+	 * Check to see if allocation came from hot-plug-add
+	 */
+	if (PageSlab(usemap_page) || PageCompound(usemap_page)) {
+		kfree(usemap);
+		if (memmap)
+			__kfree_section_memmap(memmap, PAGES_PER_SECTION);
+		return;
+	}
+
+	/*
+	 * The usemap came from bootmem. This is packed with other usemaps
+	 * on the section which has pgdat at boot time. Just keep it as is now.
+	 */
+
+	if (memmap) {
+		nr_pages = PAGE_ALIGN(PAGES_PER_SECTION * sizeof(struct page))
+			>> PAGE_SHIFT;
+
+		free_map_bootmem(memmap, nr_pages);
+	}
+}
+
 void sparse_remove_one_section(struct zone *zone, struct mem_section *ms)
 {
 	struct page *memmap = NULL;
@@ -813,4 +824,5 @@ void sparse_remove_one_section(struct zone *zone, struct mem_section *ms)
 	clear_hwpoisoned_pages(memmap, PAGES_PER_SECTION);
 	free_section_usemap(memmap, usemap);
 }
-#endif
+#endif /* CONFIG_MEMORY_HOTREMOVE */
+#endif /* CONFIG_MEMORY_HOTPLUG */
diff --git a/mm/swap.c b/mm/swap.c
index 8a529a01e8f..acd40bfffa8 100644
--- a/mm/swap.c
+++ b/mm/swap.c
@@ -737,7 +737,7 @@ EXPORT_SYMBOL(__pagevec_release);
 #ifdef CONFIG_TRANSPARENT_HUGEPAGE
 /* used by __split_huge_page_refcount() */
 void lru_add_page_tail(struct page *page, struct page *page_tail,
-		       struct lruvec *lruvec)
+		       struct lruvec *lruvec, struct list_head *list)
 {
 	int uninitialized_var(active);
 	enum lru_list lru;
@@ -749,7 +749,8 @@ void lru_add_page_tail(struct page *page, struct page *page_tail,
 	VM_BUG_ON(NR_CPUS != 1 &&
 		  !spin_is_locked(&lruvec_zone(lruvec)->lru_lock));
 
-	SetPageLRU(page_tail);
+	if (!list)
+		SetPageLRU(page_tail);
 
 	if (page_evictable(page_tail)) {
 		if (PageActive(page)) {
@@ -767,7 +768,11 @@ void lru_add_page_tail(struct page *page, struct page *page_tail,
 
 	if (likely(PageLRU(page)))
 		list_add_tail(&page_tail->lru, &page->lru);
-	else {
+	else if (list) {
+		/* page reclaim is reclaiming a huge page */
+		get_page(page_tail);
+		list_add_tail(&page_tail->lru, list);
+	} else {
 		struct list_head *list_head;
 		/*
 		 * Head page has not yet been counted, as an hpage,
diff --git a/mm/swap_state.c b/mm/swap_state.c
index 7efcf152592..b3d40dcf362 100644
--- a/mm/swap_state.c
+++ b/mm/swap_state.c
@@ -78,7 +78,7 @@ void show_swap_cache_info(void)
  * __add_to_swap_cache resembles add_to_page_cache_locked on swapper_space,
  * but sets SwapCache flag and private instead of mapping and index.
  */
-static int __add_to_swap_cache(struct page *page, swp_entry_t entry)
+int __add_to_swap_cache(struct page *page, swp_entry_t entry)
 {
 	int error;
 	struct address_space *address_space;
@@ -160,7 +160,7 @@ void __delete_from_swap_cache(struct page *page)
  * Allocate swap space for the page and add the page to the
  * swap cache.  Caller needs to hold the page lock. 
  */
-int add_to_swap(struct page *page)
+int add_to_swap(struct page *page, struct list_head *list)
 {
 	swp_entry_t entry;
 	int err;
@@ -173,7 +173,7 @@ int add_to_swap(struct page *page)
 		return 0;
 
 	if (unlikely(PageTransHuge(page)))
-		if (unlikely(split_huge_page(page))) {
+		if (unlikely(split_huge_page_to_list(page, list))) {
 			swapcache_free(entry, NULL);
 			return 0;
 		}
diff --git a/mm/swapfile.c b/mm/swapfile.c
index a1f7772a01f..d417efddfe7 100644
--- a/mm/swapfile.c
+++ b/mm/swapfile.c
@@ -2120,7 +2120,7 @@ SYSCALL_DEFINE2(swapon, const char __user *, specialfile, int, swap_flags)
 	if (p->bdev) {
 		if (blk_queue_nonrot(bdev_get_queue(p->bdev))) {
 			p->flags |= SWP_SOLIDSTATE;
-			p->cluster_next = 1 + (random32() % p->highest_bit);
+			p->cluster_next = 1 + (prandom_u32() % p->highest_bit);
 		}
 		if ((swap_flags & SWAP_FLAG_DISCARD) && discard_swap(p) == 0)
 			p->flags |= SWP_DISCARDABLE;
diff --git a/mm/vmalloc.c b/mm/vmalloc.c
index 0f751f2068c..72043d6c88c 100644
--- a/mm/vmalloc.c
+++ b/mm/vmalloc.c
@@ -249,19 +249,9 @@ EXPORT_SYMBOL(vmalloc_to_pfn);
 #define VM_LAZY_FREEING	0x02
 #define VM_VM_AREA	0x04
 
-struct vmap_area {
-	unsigned long va_start;
-	unsigned long va_end;
-	unsigned long flags;
-	struct rb_node rb_node;		/* address sorted rbtree */
-	struct list_head list;		/* address sorted list */
-	struct list_head purge_list;	/* "lazy purge" list */
-	struct vm_struct *vm;
-	struct rcu_head rcu_head;
-};
-
 static DEFINE_SPINLOCK(vmap_area_lock);
-static LIST_HEAD(vmap_area_list);
+/* Export for kexec only */
+LIST_HEAD(vmap_area_list);
 static struct rb_root vmap_area_root = RB_ROOT;
 
 /* The vmap cache globals are protected by vmap_area_lock */
@@ -313,7 +303,7 @@ static void __insert_vmap_area(struct vmap_area *va)
 	rb_link_node(&va->rb_node, parent, p);
 	rb_insert_color(&va->rb_node, &vmap_area_root);
 
-	/* address-sort this list so it is usable like the vmlist */
+	/* address-sort this list */
 	tmp = rb_prev(&va->rb_node);
 	if (tmp) {
 		struct vmap_area *prev;
@@ -1125,6 +1115,7 @@ void *vm_map_ram(struct page **pages, unsigned int count, int node, pgprot_t pro
 }
 EXPORT_SYMBOL(vm_map_ram);
 
+static struct vm_struct *vmlist __initdata;
 /**
  * vm_area_add_early - add vmap area early during boot
  * @vm: vm_struct to add
@@ -1283,41 +1274,35 @@ int map_vm_area(struct vm_struct *area, pgprot_t prot, struct page ***pages)
 }
 EXPORT_SYMBOL_GPL(map_vm_area);
 
-/*** Old vmalloc interfaces ***/
-DEFINE_RWLOCK(vmlist_lock);
-struct vm_struct *vmlist;
-
 static void setup_vmalloc_vm(struct vm_struct *vm, struct vmap_area *va,
 			      unsigned long flags, const void *caller)
 {
+	spin_lock(&vmap_area_lock);
 	vm->flags = flags;
 	vm->addr = (void *)va->va_start;
 	vm->size = va->va_end - va->va_start;
 	vm->caller = caller;
 	va->vm = vm;
 	va->flags |= VM_VM_AREA;
+	spin_unlock(&vmap_area_lock);
 }
 
-static void insert_vmalloc_vmlist(struct vm_struct *vm)
+static void clear_vm_unlist(struct vm_struct *vm)
 {
-	struct vm_struct *tmp, **p;
-
+	/*
+	 * Before removing VM_UNLIST,
+	 * we should make sure that vm has proper values.
+	 * Pair with smp_rmb() in show_numa_info().
+	 */
+	smp_wmb();
 	vm->flags &= ~VM_UNLIST;
-	write_lock(&vmlist_lock);
-	for (p = &vmlist; (tmp = *p) != NULL; p = &tmp->next) {
-		if (tmp->addr >= vm->addr)
-			break;
-	}
-	vm->next = *p;
-	*p = vm;
-	write_unlock(&vmlist_lock);
 }
 
 static void insert_vmalloc_vm(struct vm_struct *vm, struct vmap_area *va,
 			      unsigned long flags, const void *caller)
 {
 	setup_vmalloc_vm(vm, va, flags, caller);
-	insert_vmalloc_vmlist(vm);
+	clear_vm_unlist(vm);
 }
 
 static struct vm_struct *__get_vm_area_node(unsigned long size,
@@ -1360,10 +1345,9 @@ static struct vm_struct *__get_vm_area_node(unsigned long size,
 
 	/*
 	 * When this function is called from __vmalloc_node_range,
-	 * we do not add vm_struct to vmlist here to avoid
-	 * accessing uninitialized members of vm_struct such as
-	 * pages and nr_pages fields. They will be set later.
-	 * To distinguish it from others, we use a VM_UNLIST flag.
+	 * we add VM_UNLIST flag to avoid accessing uninitialized
+	 * members of vm_struct such as pages and nr_pages fields.
+	 * They will be set later.
 	 */
 	if (flags & VM_UNLIST)
 		setup_vmalloc_vm(area, va, flags, caller);
@@ -1447,19 +1431,10 @@ struct vm_struct *remove_vm_area(const void *addr)
 	if (va && va->flags & VM_VM_AREA) {
 		struct vm_struct *vm = va->vm;
 
-		if (!(vm->flags & VM_UNLIST)) {
-			struct vm_struct *tmp, **p;
-			/*
-			 * remove from list and disallow access to
-			 * this vm_struct before unmap. (address range
-			 * confliction is maintained by vmap.)
-			 */
-			write_lock(&vmlist_lock);
-			for (p = &vmlist; (tmp = *p) != vm; p = &tmp->next)
-				;
-			*p = tmp->next;
-			write_unlock(&vmlist_lock);
-		}
+		spin_lock(&vmap_area_lock);
+		va->vm = NULL;
+		va->flags &= ~VM_VM_AREA;
+		spin_unlock(&vmap_area_lock);
 
 		vmap_debug_free_range(va->va_start, va->va_end);
 		free_unmap_vmap_area(va);
@@ -1680,10 +1655,11 @@ void *__vmalloc_node_range(unsigned long size, unsigned long align,
 		return NULL;
 
 	/*
-	 * In this function, newly allocated vm_struct is not added
-	 * to vmlist at __get_vm_area_node(). so, it is added here.
+	 * In this function, newly allocated vm_struct has VM_UNLIST flag.
+	 * It means that vm_struct is not fully initialized.
+	 * Now, it is fully initialized, so remove this flag here.
 	 */
-	insert_vmalloc_vmlist(area);
+	clear_vm_unlist(area);
 
 	/*
 	 * A ref_count = 3 is needed because the vm_struct and vmap_area
@@ -2005,7 +1981,8 @@ static int aligned_vwrite(char *buf, char *addr, unsigned long count)
 
 long vread(char *buf, char *addr, unsigned long count)
 {
-	struct vm_struct *tmp;
+	struct vmap_area *va;
+	struct vm_struct *vm;
 	char *vaddr, *buf_start = buf;
 	unsigned long buflen = count;
 	unsigned long n;
@@ -2014,10 +1991,17 @@ long vread(char *buf, char *addr, unsigned long count)
 	if ((unsigned long) addr + count < count)
 		count = -(unsigned long) addr;
 
-	read_lock(&vmlist_lock);
-	for (tmp = vmlist; count && tmp; tmp = tmp->next) {
-		vaddr = (char *) tmp->addr;
-		if (addr >= vaddr + tmp->size - PAGE_SIZE)
+	spin_lock(&vmap_area_lock);
+	list_for_each_entry(va, &vmap_area_list, list) {
+		if (!count)
+			break;
+
+		if (!(va->flags & VM_VM_AREA))
+			continue;
+
+		vm = va->vm;
+		vaddr = (char *) vm->addr;
+		if (addr >= vaddr + vm->size - PAGE_SIZE)
 			continue;
 		while (addr < vaddr) {
 			if (count == 0)
@@ -2027,10 +2011,10 @@ long vread(char *buf, char *addr, unsigned long count)
 			addr++;
 			count--;
 		}
-		n = vaddr + tmp->size - PAGE_SIZE - addr;
+		n = vaddr + vm->size - PAGE_SIZE - addr;
 		if (n > count)
 			n = count;
-		if (!(tmp->flags & VM_IOREMAP))
+		if (!(vm->flags & VM_IOREMAP))
 			aligned_vread(buf, addr, n);
 		else /* IOREMAP area is treated as memory hole */
 			memset(buf, 0, n);
@@ -2039,7 +2023,7 @@ long vread(char *buf, char *addr, unsigned long count)
 		count -= n;
 	}
 finished:
-	read_unlock(&vmlist_lock);
+	spin_unlock(&vmap_area_lock);
 
 	if (buf == buf_start)
 		return 0;
@@ -2078,7 +2062,8 @@ finished:
 
 long vwrite(char *buf, char *addr, unsigned long count)
 {
-	struct vm_struct *tmp;
+	struct vmap_area *va;
+	struct vm_struct *vm;
 	char *vaddr;
 	unsigned long n, buflen;
 	int copied = 0;
@@ -2088,10 +2073,17 @@ long vwrite(char *buf, char *addr, unsigned long count)
 		count = -(unsigned long) addr;
 	buflen = count;
 
-	read_lock(&vmlist_lock);
-	for (tmp = vmlist; count && tmp; tmp = tmp->next) {
-		vaddr = (char *) tmp->addr;
-		if (addr >= vaddr + tmp->size - PAGE_SIZE)
+	spin_lock(&vmap_area_lock);
+	list_for_each_entry(va, &vmap_area_list, list) {
+		if (!count)
+			break;
+
+		if (!(va->flags & VM_VM_AREA))
+			continue;
+
+		vm = va->vm;
+		vaddr = (char *) vm->addr;
+		if (addr >= vaddr + vm->size - PAGE_SIZE)
 			continue;
 		while (addr < vaddr) {
 			if (count == 0)
@@ -2100,10 +2092,10 @@ long vwrite(char *buf, char *addr, unsigned long count)
 			addr++;
 			count--;
 		}
-		n = vaddr + tmp->size - PAGE_SIZE - addr;
+		n = vaddr + vm->size - PAGE_SIZE - addr;
 		if (n > count)
 			n = count;
-		if (!(tmp->flags & VM_IOREMAP)) {
+		if (!(vm->flags & VM_IOREMAP)) {
 			aligned_vwrite(buf, addr, n);
 			copied++;
 		}
@@ -2112,7 +2104,7 @@ long vwrite(char *buf, char *addr, unsigned long count)
 		count -= n;
 	}
 finished:
-	read_unlock(&vmlist_lock);
+	spin_unlock(&vmap_area_lock);
 	if (!copied)
 		return 0;
 	return buflen;
@@ -2519,19 +2511,19 @@ void pcpu_free_vm_areas(struct vm_struct **vms, int nr_vms)
 
 #ifdef CONFIG_PROC_FS
 static void *s_start(struct seq_file *m, loff_t *pos)
-	__acquires(&vmlist_lock)
+	__acquires(&vmap_area_lock)
 {
 	loff_t n = *pos;
-	struct vm_struct *v;
+	struct vmap_area *va;
 
-	read_lock(&vmlist_lock);
-	v = vmlist;
-	while (n > 0 && v) {
+	spin_lock(&vmap_area_lock);
+	va = list_entry((&vmap_area_list)->next, typeof(*va), list);
+	while (n > 0 && &va->list != &vmap_area_list) {
 		n--;
-		v = v->next;
+		va = list_entry(va->list.next, typeof(*va), list);
 	}
-	if (!n)
-		return v;
+	if (!n && &va->list != &vmap_area_list)
+		return va;
 
 	return NULL;
 
@@ -2539,16 +2531,20 @@ static void *s_start(struct seq_file *m, loff_t *pos)
 
 static void *s_next(struct seq_file *m, void *p, loff_t *pos)
 {
-	struct vm_struct *v = p;
+	struct vmap_area *va = p, *next;
 
 	++*pos;
-	return v->next;
+	next = list_entry(va->list.next, typeof(*va), list);
+	if (&next->list != &vmap_area_list)
+		return next;
+
+	return NULL;
 }
 
 static void s_stop(struct seq_file *m, void *p)
-	__releases(&vmlist_lock)
+	__releases(&vmap_area_lock)
 {
-	read_unlock(&vmlist_lock);
+	spin_unlock(&vmap_area_lock);
 }
 
 static void show_numa_info(struct seq_file *m, struct vm_struct *v)
@@ -2559,6 +2555,11 @@ static void show_numa_info(struct seq_file *m, struct vm_struct *v)
 		if (!counters)
 			return;
 
+		/* Pair with smp_wmb() in clear_vm_unlist() */
+		smp_rmb();
+		if (v->flags & VM_UNLIST)
+			return;
+
 		memset(counters, 0, nr_node_ids * sizeof(unsigned int));
 
 		for (nr = 0; nr < v->nr_pages; nr++)
@@ -2572,7 +2573,20 @@ static void show_numa_info(struct seq_file *m, struct vm_struct *v)
 
 static int s_show(struct seq_file *m, void *p)
 {
-	struct vm_struct *v = p;
+	struct vmap_area *va = p;
+	struct vm_struct *v;
+
+	if (va->flags & (VM_LAZY_FREE | VM_LAZY_FREEING))
+		return 0;
+
+	if (!(va->flags & VM_VM_AREA)) {
+		seq_printf(m, "0x%pK-0x%pK %7ld vm_map_ram\n",
+			(void *)va->va_start, (void *)va->va_end,
+					va->va_end - va->va_start);
+		return 0;
+	}
+
+	v = va->vm;
 
 	seq_printf(m, "0x%pK-0x%pK %7ld",
 		v->addr, v->addr + v->size, v->size);
@@ -2645,5 +2659,53 @@ static int __init proc_vmalloc_init(void)
 	return 0;
 }
 module_init(proc_vmalloc_init);
+
+void get_vmalloc_info(struct vmalloc_info *vmi)
+{
+	struct vmap_area *va;
+	unsigned long free_area_size;
+	unsigned long prev_end;
+
+	vmi->used = 0;
+	vmi->largest_chunk = 0;
+
+	prev_end = VMALLOC_START;
+
+	spin_lock(&vmap_area_lock);
+
+	if (list_empty(&vmap_area_list)) {
+		vmi->largest_chunk = VMALLOC_TOTAL;
+		goto out;
+	}
+
+	list_for_each_entry(va, &vmap_area_list, list) {
+		unsigned long addr = va->va_start;
+
+		/*
+		 * Some archs keep another range for modules in vmalloc space
+		 */
+		if (addr < VMALLOC_START)
+			continue;
+		if (addr >= VMALLOC_END)
+			break;
+
+		if (va->flags & (VM_LAZY_FREE | VM_LAZY_FREEING))
+			continue;
+
+		vmi->used += (va->va_end - va->va_start);
+
+		free_area_size = addr - prev_end;
+		if (vmi->largest_chunk < free_area_size)
+			vmi->largest_chunk = free_area_size;
+
+		prev_end = va->va_end;
+	}
+
+	if (VMALLOC_END - prev_end > vmi->largest_chunk)
+		vmi->largest_chunk = VMALLOC_END - prev_end;
+
+out:
+	spin_unlock(&vmap_area_lock);
+}
 #endif
 
diff --git a/mm/vmpressure.c b/mm/vmpressure.c
new file mode 100644
index 00000000000..736a6011c2c
--- /dev/null
+++ b/mm/vmpressure.c
@@ -0,0 +1,374 @@
+/*
+ * Linux VM pressure
+ *
+ * Copyright 2012 Linaro Ltd.
+ *		  Anton Vorontsov <anton.vorontsov@linaro.org>
+ *
+ * Based on ideas from Andrew Morton, David Rientjes, KOSAKI Motohiro,
+ * Leonid Moiseichuk, Mel Gorman, Minchan Kim and Pekka Enberg.
+ *
+ * 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/cgroup.h>
+#include <linux/fs.h>
+#include <linux/log2.h>
+#include <linux/sched.h>
+#include <linux/mm.h>
+#include <linux/vmstat.h>
+#include <linux/eventfd.h>
+#include <linux/swap.h>
+#include <linux/printk.h>
+#include <linux/vmpressure.h>
+
+/*
+ * The window size (vmpressure_win) is the number of scanned pages before
+ * we try to analyze scanned/reclaimed ratio. So the window is used as a
+ * rate-limit tunable for the "low" level notification, and also for
+ * averaging the ratio for medium/critical levels. Using small window
+ * sizes can cause lot of false positives, but too big window size will
+ * delay the notifications.
+ *
+ * As the vmscan reclaimer logic works with chunks which are multiple of
+ * SWAP_CLUSTER_MAX, it makes sense to use it for the window size as well.
+ *
+ * TODO: Make the window size depend on machine size, as we do for vmstat
+ * thresholds. Currently we set it to 512 pages (2MB for 4KB pages).
+ */
+static const unsigned long vmpressure_win = SWAP_CLUSTER_MAX * 16;
+
+/*
+ * These thresholds are used when we account memory pressure through
+ * scanned/reclaimed ratio. The current values were chosen empirically. In
+ * essence, they are percents: the higher the value, the more number
+ * unsuccessful reclaims there were.
+ */
+static const unsigned int vmpressure_level_med = 60;
+static const unsigned int vmpressure_level_critical = 95;
+
+/*
+ * When there are too little pages left to scan, vmpressure() may miss the
+ * critical pressure as number of pages will be less than "window size".
+ * However, in that case the vmscan priority will raise fast as the
+ * reclaimer will try to scan LRUs more deeply.
+ *
+ * The vmscan logic considers these special priorities:
+ *
+ * prio == DEF_PRIORITY (12): reclaimer starts with that value
+ * prio <= DEF_PRIORITY - 2 : kswapd becomes somewhat overwhelmed
+ * prio == 0                : close to OOM, kernel scans every page in an lru
+ *
+ * Any value in this range is acceptable for this tunable (i.e. from 12 to
+ * 0). Current value for the vmpressure_level_critical_prio is chosen
+ * empirically, but the number, in essence, means that we consider
+ * critical level when scanning depth is ~10% of the lru size (vmscan
+ * scans 'lru_size >> prio' pages, so it is actually 12.5%, or one
+ * eights).
+ */
+static const unsigned int vmpressure_level_critical_prio = ilog2(100 / 10);
+
+static struct vmpressure *work_to_vmpressure(struct work_struct *work)
+{
+	return container_of(work, struct vmpressure, work);
+}
+
+static struct vmpressure *cg_to_vmpressure(struct cgroup *cg)
+{
+	return css_to_vmpressure(cgroup_subsys_state(cg, mem_cgroup_subsys_id));
+}
+
+static struct vmpressure *vmpressure_parent(struct vmpressure *vmpr)
+{
+	struct cgroup *cg = vmpressure_to_css(vmpr)->cgroup;
+	struct mem_cgroup *memcg = mem_cgroup_from_cont(cg);
+
+	memcg = parent_mem_cgroup(memcg);
+	if (!memcg)
+		return NULL;
+	return memcg_to_vmpressure(memcg);
+}
+
+enum vmpressure_levels {
+	VMPRESSURE_LOW = 0,
+	VMPRESSURE_MEDIUM,
+	VMPRESSURE_CRITICAL,
+	VMPRESSURE_NUM_LEVELS,
+};
+
+static const char * const vmpressure_str_levels[] = {
+	[VMPRESSURE_LOW] = "low",
+	[VMPRESSURE_MEDIUM] = "medium",
+	[VMPRESSURE_CRITICAL] = "critical",
+};
+
+static enum vmpressure_levels vmpressure_level(unsigned long pressure)
+{
+	if (pressure >= vmpressure_level_critical)
+		return VMPRESSURE_CRITICAL;
+	else if (pressure >= vmpressure_level_med)
+		return VMPRESSURE_MEDIUM;
+	return VMPRESSURE_LOW;
+}
+
+static enum vmpressure_levels vmpressure_calc_level(unsigned long scanned,
+						    unsigned long reclaimed)
+{
+	unsigned long scale = scanned + reclaimed;
+	unsigned long pressure;
+
+	/*
+	 * We calculate the ratio (in percents) of how many pages were
+	 * scanned vs. reclaimed in a given time frame (window). Note that
+	 * time is in VM reclaimer's "ticks", i.e. number of pages
+	 * scanned. This makes it possible to set desired reaction time
+	 * and serves as a ratelimit.
+	 */
+	pressure = scale - (reclaimed * scale / scanned);
+	pressure = pressure * 100 / scale;
+
+	pr_debug("%s: %3lu  (s: %lu  r: %lu)\n", __func__, pressure,
+		 scanned, reclaimed);
+
+	return vmpressure_level(pressure);
+}
+
+struct vmpressure_event {
+	struct eventfd_ctx *efd;
+	enum vmpressure_levels level;
+	struct list_head node;
+};
+
+static bool vmpressure_event(struct vmpressure *vmpr,
+			     unsigned long scanned, unsigned long reclaimed)
+{
+	struct vmpressure_event *ev;
+	enum vmpressure_levels level;
+	bool signalled = false;
+
+	level = vmpressure_calc_level(scanned, reclaimed);
+
+	mutex_lock(&vmpr->events_lock);
+
+	list_for_each_entry(ev, &vmpr->events, node) {
+		if (level >= ev->level) {
+			eventfd_signal(ev->efd, 1);
+			signalled = true;
+		}
+	}
+
+	mutex_unlock(&vmpr->events_lock);
+
+	return signalled;
+}
+
+static void vmpressure_work_fn(struct work_struct *work)
+{
+	struct vmpressure *vmpr = work_to_vmpressure(work);
+	unsigned long scanned;
+	unsigned long reclaimed;
+
+	/*
+	 * Several contexts might be calling vmpressure(), so it is
+	 * possible that the work was rescheduled again before the old
+	 * work context cleared the counters. In that case we will run
+	 * just after the old work returns, but then scanned might be zero
+	 * here. No need for any locks here since we don't care if
+	 * vmpr->reclaimed is in sync.
+	 */
+	if (!vmpr->scanned)
+		return;
+
+	mutex_lock(&vmpr->sr_lock);
+	scanned = vmpr->scanned;
+	reclaimed = vmpr->reclaimed;
+	vmpr->scanned = 0;
+	vmpr->reclaimed = 0;
+	mutex_unlock(&vmpr->sr_lock);
+
+	do {
+		if (vmpressure_event(vmpr, scanned, reclaimed))
+			break;
+		/*
+		 * If not handled, propagate the event upward into the
+		 * hierarchy.
+		 */
+	} while ((vmpr = vmpressure_parent(vmpr)));
+}
+
+/**
+ * vmpressure() - Account memory pressure through scanned/reclaimed ratio
+ * @gfp:	reclaimer's gfp mask
+ * @memcg:	cgroup memory controller handle
+ * @scanned:	number of pages scanned
+ * @reclaimed:	number of pages reclaimed
+ *
+ * This function should be called from the vmscan reclaim path to account
+ * "instantaneous" memory pressure (scanned/reclaimed ratio). The raw
+ * pressure index is then further refined and averaged over time.
+ *
+ * This function does not return any value.
+ */
+void vmpressure(gfp_t gfp, struct mem_cgroup *memcg,
+		unsigned long scanned, unsigned long reclaimed)
+{
+	struct vmpressure *vmpr = memcg_to_vmpressure(memcg);
+
+	/*
+	 * Here we only want to account pressure that userland is able to
+	 * help us with. For example, suppose that DMA zone is under
+	 * pressure; if we notify userland about that kind of pressure,
+	 * then it will be mostly a waste as it will trigger unnecessary
+	 * freeing of memory by userland (since userland is more likely to
+	 * have HIGHMEM/MOVABLE pages instead of the DMA fallback). That
+	 * is why we include only movable, highmem and FS/IO pages.
+	 * Indirect reclaim (kswapd) sets sc->gfp_mask to GFP_KERNEL, so
+	 * we account it too.
+	 */
+	if (!(gfp & (__GFP_HIGHMEM | __GFP_MOVABLE | __GFP_IO | __GFP_FS)))
+		return;
+
+	/*
+	 * If we got here with no pages scanned, then that is an indicator
+	 * that reclaimer was unable to find any shrinkable LRUs at the
+	 * current scanning depth. But it does not mean that we should
+	 * report the critical pressure, yet. If the scanning priority
+	 * (scanning depth) goes too high (deep), we will be notified
+	 * through vmpressure_prio(). But so far, keep calm.
+	 */
+	if (!scanned)
+		return;
+
+	mutex_lock(&vmpr->sr_lock);
+	vmpr->scanned += scanned;
+	vmpr->reclaimed += reclaimed;
+	scanned = vmpr->scanned;
+	mutex_unlock(&vmpr->sr_lock);
+
+	if (scanned < vmpressure_win || work_pending(&vmpr->work))
+		return;
+	schedule_work(&vmpr->work);
+}
+
+/**
+ * vmpressure_prio() - Account memory pressure through reclaimer priority level
+ * @gfp:	reclaimer's gfp mask
+ * @memcg:	cgroup memory controller handle
+ * @prio:	reclaimer's priority
+ *
+ * This function should be called from the reclaim path every time when
+ * the vmscan's reclaiming priority (scanning depth) changes.
+ *
+ * This function does not return any value.
+ */
+void vmpressure_prio(gfp_t gfp, struct mem_cgroup *memcg, int prio)
+{
+	/*
+	 * We only use prio for accounting critical level. For more info
+	 * see comment for vmpressure_level_critical_prio variable above.
+	 */
+	if (prio > vmpressure_level_critical_prio)
+		return;
+
+	/*
+	 * OK, the prio is below the threshold, updating vmpressure
+	 * information before shrinker dives into long shrinking of long
+	 * range vmscan. Passing scanned = vmpressure_win, reclaimed = 0
+	 * to the vmpressure() basically means that we signal 'critical'
+	 * level.
+	 */
+	vmpressure(gfp, memcg, vmpressure_win, 0);
+}
+
+/**
+ * vmpressure_register_event() - Bind vmpressure notifications to an eventfd
+ * @cg:		cgroup that is interested in vmpressure notifications
+ * @cft:	cgroup control files handle
+ * @eventfd:	eventfd context to link notifications with
+ * @args:	event arguments (used to set up a pressure level threshold)
+ *
+ * This function associates eventfd context with the vmpressure
+ * infrastructure, so that the notifications will be delivered to the
+ * @eventfd. The @args parameter is a string that denotes pressure level
+ * threshold (one of vmpressure_str_levels, i.e. "low", "medium", or
+ * "critical").
+ *
+ * This function should not be used directly, just pass it to (struct
+ * cftype).register_event, and then cgroup core will handle everything by
+ * itself.
+ */
+int vmpressure_register_event(struct cgroup *cg, struct cftype *cft,
+			      struct eventfd_ctx *eventfd, const char *args)
+{
+	struct vmpressure *vmpr = cg_to_vmpressure(cg);
+	struct vmpressure_event *ev;
+	int level;
+
+	for (level = 0; level < VMPRESSURE_NUM_LEVELS; level++) {
+		if (!strcmp(vmpressure_str_levels[level], args))
+			break;
+	}
+
+	if (level >= VMPRESSURE_NUM_LEVELS)
+		return -EINVAL;
+
+	ev = kzalloc(sizeof(*ev), GFP_KERNEL);
+	if (!ev)
+		return -ENOMEM;
+
+	ev->efd = eventfd;
+	ev->level = level;
+
+	mutex_lock(&vmpr->events_lock);
+	list_add(&ev->node, &vmpr->events);
+	mutex_unlock(&vmpr->events_lock);
+
+	return 0;
+}
+
+/**
+ * vmpressure_unregister_event() - Unbind eventfd from vmpressure
+ * @cg:		cgroup handle
+ * @cft:	cgroup control files handle
+ * @eventfd:	eventfd context that was used to link vmpressure with the @cg
+ *
+ * This function does internal manipulations to detach the @eventfd from
+ * the vmpressure notifications, and then frees internal resources
+ * associated with the @eventfd (but the @eventfd itself is not freed).
+ *
+ * This function should not be used directly, just pass it to (struct
+ * cftype).unregister_event, and then cgroup core will handle everything
+ * by itself.
+ */
+void vmpressure_unregister_event(struct cgroup *cg, struct cftype *cft,
+				 struct eventfd_ctx *eventfd)
+{
+	struct vmpressure *vmpr = cg_to_vmpressure(cg);
+	struct vmpressure_event *ev;
+
+	mutex_lock(&vmpr->events_lock);
+	list_for_each_entry(ev, &vmpr->events, node) {
+		if (ev->efd != eventfd)
+			continue;
+		list_del(&ev->node);
+		kfree(ev);
+		break;
+	}
+	mutex_unlock(&vmpr->events_lock);
+}
+
+/**
+ * vmpressure_init() - Initialize vmpressure control structure
+ * @vmpr:	Structure to be initialized
+ *
+ * This function should be called on every allocated vmpressure structure
+ * before any usage.
+ */
+void vmpressure_init(struct vmpressure *vmpr)
+{
+	mutex_init(&vmpr->sr_lock);
+	mutex_init(&vmpr->events_lock);
+	INIT_LIST_HEAD(&vmpr->events);
+	INIT_WORK(&vmpr->work, vmpressure_work_fn);
+}
diff --git a/mm/vmscan.c b/mm/vmscan.c
index 88c5fed8b9a..fa6a85378ee 100644
--- a/mm/vmscan.c
+++ b/mm/vmscan.c
@@ -19,6 +19,7 @@
 #include <linux/pagemap.h>
 #include <linux/init.h>
 #include <linux/highmem.h>
+#include <linux/vmpressure.h>
 #include <linux/vmstat.h>
 #include <linux/file.h>
 #include <linux/writeback.h>
@@ -780,7 +781,7 @@ static unsigned long shrink_page_list(struct list_head *page_list,
 		if (PageAnon(page) && !PageSwapCache(page)) {
 			if (!(sc->gfp_mask & __GFP_IO))
 				goto keep_locked;
-			if (!add_to_swap(page))
+			if (!add_to_swap(page, page_list))
 				goto activate_locked;
 			may_enter_fs = 1;
 		}
@@ -1982,6 +1983,11 @@ static void shrink_zone(struct zone *zone, struct scan_control *sc)
 			}
 			memcg = mem_cgroup_iter(root, memcg, &reclaim);
 		} while (memcg);
+
+		vmpressure(sc->gfp_mask, sc->target_mem_cgroup,
+			   sc->nr_scanned - nr_scanned,
+			   sc->nr_reclaimed - nr_reclaimed);
+
 	} while (should_continue_reclaim(zone, sc->nr_reclaimed - nr_reclaimed,
 					 sc->nr_scanned - nr_scanned, sc));
 }
@@ -2167,6 +2173,8 @@ static unsigned long do_try_to_free_pages(struct zonelist *zonelist,
 		count_vm_event(ALLOCSTALL);
 
 	do {
+		vmpressure_prio(sc->gfp_mask, sc->target_mem_cgroup,
+				sc->priority);
 		sc->nr_scanned = 0;
 		aborted_reclaim = shrink_zones(zonelist, sc);
 
@@ -2619,7 +2627,6 @@ static unsigned long balance_pgdat(pg_data_t *pgdat, int order,
 	bool pgdat_is_balanced = false;
 	int i;
 	int end_zone = 0;	/* Inclusive.  0 = ZONE_DMA */
-	unsigned long total_scanned;
 	struct reclaim_state *reclaim_state = current->reclaim_state;
 	unsigned long nr_soft_reclaimed;
 	unsigned long nr_soft_scanned;
@@ -2639,7 +2646,6 @@ static unsigned long balance_pgdat(pg_data_t *pgdat, int order,
 		.gfp_mask = sc.gfp_mask,
 	};
 loop_again:
-	total_scanned = 0;
 	sc.priority = DEF_PRIORITY;
 	sc.nr_reclaimed = 0;
 	sc.may_writepage = !laptop_mode;
@@ -2730,7 +2736,6 @@ loop_again:
 							order, sc.gfp_mask,
 							&nr_soft_scanned);
 			sc.nr_reclaimed += nr_soft_reclaimed;
-			total_scanned += nr_soft_scanned;
 
 			/*
 			 * We put equal pressure on every zone, unless
@@ -2765,7 +2770,6 @@ loop_again:
 				reclaim_state->reclaimed_slab = 0;
 				nr_slab = shrink_slab(&shrink, sc.nr_scanned, lru_pages);
 				sc.nr_reclaimed += reclaim_state->reclaimed_slab;
-				total_scanned += sc.nr_scanned;
 
 				if (nr_slab == 0 && !zone_reclaimable(zone))
 					zone->all_unreclaimable = 1;
@@ -3188,9 +3192,9 @@ int kswapd_run(int nid)
 	if (IS_ERR(pgdat->kswapd)) {
 		/* failure at boot is fatal */
 		BUG_ON(system_state == SYSTEM_BOOTING);
-		pgdat->kswapd = NULL;
 		pr_err("Failed to start kswapd on node %d\n", nid);
 		ret = PTR_ERR(pgdat->kswapd);
+		pgdat->kswapd = NULL;
 	}
 	return ret;
 }
diff --git a/mm/vmstat.c b/mm/vmstat.c
index e1d8ed172c4..f42745e6578 100644
--- a/mm/vmstat.c
+++ b/mm/vmstat.c
@@ -52,7 +52,6 @@ void all_vm_events(unsigned long *ret)
 }
 EXPORT_SYMBOL_GPL(all_vm_events);
 
-#ifdef CONFIG_HOTPLUG
 /*
  * Fold the foreign cpu events into our own.
  *
@@ -69,7 +68,6 @@ void vm_events_fold_cpu(int cpu)
 		fold_state->event[i] = 0;
 	}
 }
-#endif /* CONFIG_HOTPLUG */
 
 #endif /* CONFIG_VM_EVENT_COUNTERS */
 
@@ -495,6 +493,10 @@ void refresh_cpu_vm_stats(int cpu)
 			atomic_long_add(global_diff[i], &vm_stat[i]);
 }
 
+/*
+ * this is only called if !populated_zone(zone), which implies no other users of
+ * pset->vm_stat_diff[] exsist.
+ */
 void drain_zonestat(struct zone *zone, struct per_cpu_pageset *pset)
 {
 	int i;
diff --git a/net/802/mrp.c b/net/802/mrp.c
index a4cc3229952..e085bcc754f 100644
--- a/net/802/mrp.c
+++ b/net/802/mrp.c
@@ -870,8 +870,12 @@ void mrp_uninit_applicant(struct net_device *dev, struct mrp_application *appl)
 	 * all pending messages before the applicant is gone.
 	 */
 	del_timer_sync(&app->join_timer);
+
+	spin_lock(&app->lock);
 	mrp_mad_event(app, MRP_EVENT_TX);
 	mrp_pdu_queue(app);
+	spin_unlock(&app->lock);
+
 	mrp_queue_xmit(app);
 
 	dev_mc_del(dev, appl->group_address);
diff --git a/net/8021q/vlan.c b/net/8021q/vlan.c
index a18714469bf..85addcd9372 100644
--- a/net/8021q/vlan.c
+++ b/net/8021q/vlan.c
@@ -86,13 +86,6 @@ void unregister_vlan_dev(struct net_device *dev, struct list_head *head)
 
 	grp = &vlan_info->grp;
 
-	/* Take it out of our own structures, but be sure to interlock with
-	 * HW accelerating devices or SW vlan input packet processing if
-	 * VLAN is not 0 (leave it there for 802.1p).
-	 */
-	if (vlan_id)
-		vlan_vid_del(real_dev, vlan_id);
-
 	grp->nr_vlan_devs--;
 
 	if (vlan->flags & VLAN_FLAG_MVRP)
@@ -114,6 +107,13 @@ void unregister_vlan_dev(struct net_device *dev, struct list_head *head)
 		vlan_gvrp_uninit_applicant(real_dev);
 	}
 
+	/* Take it out of our own structures, but be sure to interlock with
+	 * HW accelerating devices or SW vlan input packet processing if
+	 * VLAN is not 0 (leave it there for 802.1p).
+	 */
+	if (vlan_id)
+		vlan_vid_del(real_dev, vlan_id);
+
 	/* Get rid of the vlan's reference to real_dev */
 	dev_put(real_dev);
 }
diff --git a/net/atm/common.c b/net/atm/common.c
index 7b491006eaf..737bef59ce8 100644
--- a/net/atm/common.c
+++ b/net/atm/common.c
@@ -531,6 +531,8 @@ int vcc_recvmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg,
 	struct sk_buff *skb;
 	int copied, error = -EINVAL;
 
+	msg->msg_namelen = 0;
+
 	if (sock->state != SS_CONNECTED)
 		return -ENOTCONN;
 
diff --git a/net/ax25/af_ax25.c b/net/ax25/af_ax25.c
index 7b11f8bc507..e277e38f736 100644
--- a/net/ax25/af_ax25.c
+++ b/net/ax25/af_ax25.c
@@ -1642,6 +1642,7 @@ static int ax25_recvmsg(struct kiocb *iocb, struct socket *sock,
 		ax25_address src;
 		const unsigned char *mac = skb_mac_header(skb);
 
+		memset(sax, 0, sizeof(struct full_sockaddr_ax25));
 		ax25_addr_parse(mac + 1, skb->data - mac - 1, &src, NULL,
 				&digi, NULL, NULL);
 		sax->sax25_family = AF_AX25;
diff --git a/net/batman-adv/main.c b/net/batman-adv/main.c
index 0488d70c8c3..fa563e497c4 100644
--- a/net/batman-adv/main.c
+++ b/net/batman-adv/main.c
@@ -169,7 +169,7 @@ void batadv_mesh_free(struct net_device *soft_iface)
 	atomic_set(&bat_priv->mesh_state, BATADV_MESH_INACTIVE);
 }
 
-int batadv_is_my_mac(const uint8_t *addr)
+int batadv_is_my_mac(struct batadv_priv *bat_priv, const uint8_t *addr)
 {
 	const struct batadv_hard_iface *hard_iface;
 
@@ -178,6 +178,9 @@ int batadv_is_my_mac(const uint8_t *addr)
 		if (hard_iface->if_status != BATADV_IF_ACTIVE)
 			continue;
 
+		if (hard_iface->soft_iface != bat_priv->soft_iface)
+			continue;
+
 		if (batadv_compare_eth(hard_iface->net_dev->dev_addr, addr)) {
 			rcu_read_unlock();
 			return 1;
diff --git a/net/batman-adv/main.h b/net/batman-adv/main.h
index ced08b936a9..d40910dfc8e 100644
--- a/net/batman-adv/main.h
+++ b/net/batman-adv/main.h
@@ -162,7 +162,7 @@ extern struct workqueue_struct *batadv_event_workqueue;
 
 int batadv_mesh_init(struct net_device *soft_iface);
 void batadv_mesh_free(struct net_device *soft_iface);
-int batadv_is_my_mac(const uint8_t *addr);
+int batadv_is_my_mac(struct batadv_priv *bat_priv, const uint8_t *addr);
 struct batadv_hard_iface *
 batadv_seq_print_text_primary_if_get(struct seq_file *seq);
 int batadv_batman_skb_recv(struct sk_buff *skb, struct net_device *dev,
diff --git a/net/batman-adv/routing.c b/net/batman-adv/routing.c
index 5ee21cebbbb..319f2906c71 100644
--- a/net/batman-adv/routing.c
+++ b/net/batman-adv/routing.c
@@ -402,7 +402,7 @@ int batadv_recv_icmp_packet(struct sk_buff *skb,
 		goto out;
 
 	/* not for me */
-	if (!batadv_is_my_mac(ethhdr->h_dest))
+	if (!batadv_is_my_mac(bat_priv, ethhdr->h_dest))
 		goto out;
 
 	icmp_packet = (struct batadv_icmp_packet_rr *)skb->data;
@@ -416,7 +416,7 @@ int batadv_recv_icmp_packet(struct sk_buff *skb,
 	}
 
 	/* packet for me */
-	if (batadv_is_my_mac(icmp_packet->dst))
+	if (batadv_is_my_mac(bat_priv, icmp_packet->dst))
 		return batadv_recv_my_icmp_packet(bat_priv, skb, hdr_size);
 
 	/* TTL exceeded */
@@ -548,7 +548,8 @@ batadv_find_ifalter_router(struct batadv_orig_node *primary_orig,
 	return router;
 }
 
-static int batadv_check_unicast_packet(struct sk_buff *skb, int hdr_size)
+static int batadv_check_unicast_packet(struct batadv_priv *bat_priv,
+				       struct sk_buff *skb, int hdr_size)
 {
 	struct ethhdr *ethhdr;
 
@@ -567,7 +568,7 @@ static int batadv_check_unicast_packet(struct sk_buff *skb, int hdr_size)
 		return -1;
 
 	/* not for me */
-	if (!batadv_is_my_mac(ethhdr->h_dest))
+	if (!batadv_is_my_mac(bat_priv, ethhdr->h_dest))
 		return -1;
 
 	return 0;
@@ -582,7 +583,7 @@ int batadv_recv_tt_query(struct sk_buff *skb, struct batadv_hard_iface *recv_if)
 	char tt_flag;
 	size_t packet_size;
 
-	if (batadv_check_unicast_packet(skb, hdr_size) < 0)
+	if (batadv_check_unicast_packet(bat_priv, skb, hdr_size) < 0)
 		return NET_RX_DROP;
 
 	/* I could need to modify it */
@@ -614,7 +615,7 @@ int batadv_recv_tt_query(struct sk_buff *skb, struct batadv_hard_iface *recv_if)
 	case BATADV_TT_RESPONSE:
 		batadv_inc_counter(bat_priv, BATADV_CNT_TT_RESPONSE_RX);
 
-		if (batadv_is_my_mac(tt_query->dst)) {
+		if (batadv_is_my_mac(bat_priv, tt_query->dst)) {
 			/* packet needs to be linearized to access the TT
 			 * changes
 			 */
@@ -657,14 +658,15 @@ int batadv_recv_roam_adv(struct sk_buff *skb, struct batadv_hard_iface *recv_if)
 	struct batadv_roam_adv_packet *roam_adv_packet;
 	struct batadv_orig_node *orig_node;
 
-	if (batadv_check_unicast_packet(skb, sizeof(*roam_adv_packet)) < 0)
+	if (batadv_check_unicast_packet(bat_priv, skb,
+					sizeof(*roam_adv_packet)) < 0)
 		goto out;
 
 	batadv_inc_counter(bat_priv, BATADV_CNT_TT_ROAM_ADV_RX);
 
 	roam_adv_packet = (struct batadv_roam_adv_packet *)skb->data;
 
-	if (!batadv_is_my_mac(roam_adv_packet->dst))
+	if (!batadv_is_my_mac(bat_priv, roam_adv_packet->dst))
 		return batadv_route_unicast_packet(skb, recv_if);
 
 	/* check if it is a backbone gateway. we don't accept
@@ -967,7 +969,7 @@ static int batadv_check_unicast_ttvn(struct batadv_priv *bat_priv,
 	 * last time) the packet had an updated information or not
 	 */
 	curr_ttvn = (uint8_t)atomic_read(&bat_priv->tt.vn);
-	if (!batadv_is_my_mac(unicast_packet->dest)) {
+	if (!batadv_is_my_mac(bat_priv, unicast_packet->dest)) {
 		orig_node = batadv_orig_hash_find(bat_priv,
 						  unicast_packet->dest);
 		/* if it is not possible to find the orig_node representing the
@@ -1044,14 +1046,14 @@ int batadv_recv_unicast_packet(struct sk_buff *skb,
 	if (is4addr)
 		hdr_size = sizeof(*unicast_4addr_packet);
 
-	if (batadv_check_unicast_packet(skb, hdr_size) < 0)
+	if (batadv_check_unicast_packet(bat_priv, skb, hdr_size) < 0)
 		return NET_RX_DROP;
 
 	if (!batadv_check_unicast_ttvn(bat_priv, skb))
 		return NET_RX_DROP;
 
 	/* packet for me */
-	if (batadv_is_my_mac(unicast_packet->dest)) {
+	if (batadv_is_my_mac(bat_priv, unicast_packet->dest)) {
 		if (is4addr) {
 			batadv_dat_inc_counter(bat_priv,
 					       unicast_4addr_packet->subtype);
@@ -1088,7 +1090,7 @@ int batadv_recv_ucast_frag_packet(struct sk_buff *skb,
 	struct sk_buff *new_skb = NULL;
 	int ret;
 
-	if (batadv_check_unicast_packet(skb, hdr_size) < 0)
+	if (batadv_check_unicast_packet(bat_priv, skb, hdr_size) < 0)
 		return NET_RX_DROP;
 
 	if (!batadv_check_unicast_ttvn(bat_priv, skb))
@@ -1097,7 +1099,7 @@ int batadv_recv_ucast_frag_packet(struct sk_buff *skb,
 	unicast_packet = (struct batadv_unicast_frag_packet *)skb->data;
 
 	/* packet for me */
-	if (batadv_is_my_mac(unicast_packet->dest)) {
+	if (batadv_is_my_mac(bat_priv, unicast_packet->dest)) {
 		ret = batadv_frag_reassemble_skb(skb, bat_priv, &new_skb);
 
 		if (ret == NET_RX_DROP)
@@ -1151,13 +1153,13 @@ int batadv_recv_bcast_packet(struct sk_buff *skb,
 		goto out;
 
 	/* ignore broadcasts sent by myself */
-	if (batadv_is_my_mac(ethhdr->h_source))
+	if (batadv_is_my_mac(bat_priv, ethhdr->h_source))
 		goto out;
 
 	bcast_packet = (struct batadv_bcast_packet *)skb->data;
 
 	/* ignore broadcasts originated by myself */
-	if (batadv_is_my_mac(bcast_packet->orig))
+	if (batadv_is_my_mac(bat_priv, bcast_packet->orig))
 		goto out;
 
 	if (bcast_packet->header.ttl < 2)
@@ -1243,14 +1245,14 @@ int batadv_recv_vis_packet(struct sk_buff *skb,
 	ethhdr = (struct ethhdr *)skb_mac_header(skb);
 
 	/* not for me */
-	if (!batadv_is_my_mac(ethhdr->h_dest))
+	if (!batadv_is_my_mac(bat_priv, ethhdr->h_dest))
 		return NET_RX_DROP;
 
 	/* ignore own packets */
-	if (batadv_is_my_mac(vis_packet->vis_orig))
+	if (batadv_is_my_mac(bat_priv, vis_packet->vis_orig))
 		return NET_RX_DROP;
 
-	if (batadv_is_my_mac(vis_packet->sender_orig))
+	if (batadv_is_my_mac(bat_priv, vis_packet->sender_orig))
 		return NET_RX_DROP;
 
 	switch (vis_packet->vis_type) {
diff --git a/net/batman-adv/translation-table.c b/net/batman-adv/translation-table.c
index 98a66a021a6..7abee19567e 100644
--- a/net/batman-adv/translation-table.c
+++ b/net/batman-adv/translation-table.c
@@ -1953,7 +1953,7 @@ out:
 bool batadv_send_tt_response(struct batadv_priv *bat_priv,
 			     struct batadv_tt_query_packet *tt_request)
 {
-	if (batadv_is_my_mac(tt_request->dst)) {
+	if (batadv_is_my_mac(bat_priv, tt_request->dst)) {
 		/* don't answer backbone gws! */
 		if (batadv_bla_is_backbone_gw_orig(bat_priv, tt_request->src))
 			return true;
diff --git a/net/batman-adv/vis.c b/net/batman-adv/vis.c
index c053244b97b..6a1e646be96 100644
--- a/net/batman-adv/vis.c
+++ b/net/batman-adv/vis.c
@@ -477,7 +477,7 @@ void batadv_receive_client_update_packet(struct batadv_priv *bat_priv,
 
 	/* Are we the target for this VIS packet? */
 	if (vis_server == BATADV_VIS_TYPE_SERVER_SYNC	&&
-	    batadv_is_my_mac(vis_packet->target_orig))
+	    batadv_is_my_mac(bat_priv, vis_packet->target_orig))
 		are_target = 1;
 
 	spin_lock_bh(&bat_priv->vis.hash_lock);
@@ -496,7 +496,7 @@ void batadv_receive_client_update_packet(struct batadv_priv *bat_priv,
 		batadv_send_list_add(bat_priv, info);
 
 		/* ... we're not the recipient (and thus need to forward). */
-	} else if (!batadv_is_my_mac(packet->target_orig)) {
+	} else if (!batadv_is_my_mac(bat_priv, packet->target_orig)) {
 		batadv_send_list_add(bat_priv, info);
 	}
 
diff --git a/net/bluetooth/af_bluetooth.c b/net/bluetooth/af_bluetooth.c
index d3ee69b35a7..0d1b08cc76e 100644
--- a/net/bluetooth/af_bluetooth.c
+++ b/net/bluetooth/af_bluetooth.c
@@ -230,6 +230,8 @@ int bt_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
 	if (flags & (MSG_OOB))
 		return -EOPNOTSUPP;
 
+	msg->msg_namelen = 0;
+
 	skb = skb_recv_datagram(sk, flags, noblock, &err);
 	if (!skb) {
 		if (sk->sk_shutdown & RCV_SHUTDOWN)
@@ -237,8 +239,6 @@ int bt_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
 		return err;
 	}
 
-	msg->msg_namelen = 0;
-
 	copied = skb->len;
 	if (len < copied) {
 		msg->msg_flags |= MSG_TRUNC;
diff --git a/net/bluetooth/rfcomm/sock.c b/net/bluetooth/rfcomm/sock.c
index c23bae86263..7c9224bcce1 100644
--- a/net/bluetooth/rfcomm/sock.c
+++ b/net/bluetooth/rfcomm/sock.c
@@ -608,6 +608,7 @@ static int rfcomm_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
 
 	if (test_and_clear_bit(RFCOMM_DEFER_SETUP, &d->flags)) {
 		rfcomm_dlc_accept(d);
+		msg->msg_namelen = 0;
 		return 0;
 	}
 
diff --git a/net/bluetooth/sco.c b/net/bluetooth/sco.c
index 79d87d8d4f5..fb6192c9812 100644
--- a/net/bluetooth/sco.c
+++ b/net/bluetooth/sco.c
@@ -359,6 +359,7 @@ static void __sco_sock_close(struct sock *sk)
 			sco_chan_del(sk, ECONNRESET);
 		break;
 
+	case BT_CONNECT2:
 	case BT_CONNECT:
 	case BT_DISCONN:
 		sco_chan_del(sk, ECONNRESET);
@@ -664,6 +665,7 @@ static int sco_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
 	    test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags)) {
 		hci_conn_accept(pi->conn->hcon, 0);
 		sk->sk_state = BT_CONFIG;
+		msg->msg_namelen = 0;
 
 		release_sock(sk);
 		return 0;
diff --git a/net/bridge/br_fdb.c b/net/bridge/br_fdb.c
index b0812c91c0f..bab338e6270 100644
--- a/net/bridge/br_fdb.c
+++ b/net/bridge/br_fdb.c
@@ -423,7 +423,7 @@ static int fdb_insert(struct net_bridge *br, struct net_bridge_port *source,
 			return 0;
 		br_warn(br, "adding interface %s with same address "
 		       "as a received packet\n",
-		       source->dev->name);
+		       source ? source->dev->name : br->dev->name);
 		fdb_delete(br, fdb);
 	}
 
diff --git a/net/bridge/br_if.c b/net/bridge/br_if.c
index ef1b91431c6..459dab22b3f 100644
--- a/net/bridge/br_if.c
+++ b/net/bridge/br_if.c
@@ -67,7 +67,8 @@ void br_port_carrier_check(struct net_bridge_port *p)
 	struct net_device *dev = p->dev;
 	struct net_bridge *br = p->br;
 
-	if (netif_running(dev) && netif_oper_up(dev))
+	if (!(p->flags & BR_ADMIN_COST) &&
+	    netif_running(dev) && netif_oper_up(dev))
 		p->path_cost = port_cost(dev);
 
 	if (!netif_running(br->dev))
diff --git a/net/bridge/br_private.h b/net/bridge/br_private.h
index 3cbf5beb3d4..d2c043a857b 100644
--- a/net/bridge/br_private.h
+++ b/net/bridge/br_private.h
@@ -156,6 +156,7 @@ struct net_bridge_port
 #define BR_BPDU_GUARD           0x00000002
 #define BR_ROOT_BLOCK		0x00000004
 #define BR_MULTICAST_FAST_LEAVE	0x00000008
+#define BR_ADMIN_COST		0x00000010
 
 #ifdef CONFIG_BRIDGE_IGMP_SNOOPING
 	u32				multicast_startup_queries_sent;
diff --git a/net/bridge/br_stp_if.c b/net/bridge/br_stp_if.c
index 0bdb4ebd362..d45e760141b 100644
--- a/net/bridge/br_stp_if.c
+++ b/net/bridge/br_stp_if.c
@@ -288,6 +288,7 @@ int br_stp_set_path_cost(struct net_bridge_port *p, unsigned long path_cost)
 	    path_cost > BR_MAX_PATH_COST)
 		return -ERANGE;
 
+	p->flags |= BR_ADMIN_COST;
 	p->path_cost = path_cost;
 	br_configuration_update(p->br);
 	br_port_state_selection(p->br);
diff --git a/net/caif/caif_socket.c b/net/caif/caif_socket.c
index 095259f8390..ff2ff3ce696 100644
--- a/net/caif/caif_socket.c
+++ b/net/caif/caif_socket.c
@@ -286,6 +286,8 @@ static int caif_seqpkt_recvmsg(struct kiocb *iocb, struct socket *sock,
 	if (m->msg_flags&MSG_OOB)
 		goto read_error;
 
+	m->msg_namelen = 0;
+
 	skb = skb_recv_datagram(sk, flags, 0 , &ret);
 	if (!skb)
 		goto read_error;
diff --git a/net/can/gw.c b/net/can/gw.c
index 2d117dc5ebe..117814a7e73 100644
--- a/net/can/gw.c
+++ b/net/can/gw.c
@@ -466,7 +466,7 @@ static int cgw_notifier(struct notifier_block *nb,
 			if (gwj->src.dev == dev || gwj->dst.dev == dev) {
 				hlist_del(&gwj->list);
 				cgw_unregister_filter(gwj);
-				kfree(gwj);
+				kmem_cache_free(cgw_cache, gwj);
 			}
 		}
 	}
@@ -864,7 +864,7 @@ static void cgw_remove_all_jobs(void)
 	hlist_for_each_entry_safe(gwj, nx, &cgw_list, list) {
 		hlist_del(&gwj->list);
 		cgw_unregister_filter(gwj);
-		kfree(gwj);
+		kmem_cache_free(cgw_cache, gwj);
 	}
 }
 
@@ -920,7 +920,7 @@ static int cgw_remove_job(struct sk_buff *skb,  struct nlmsghdr *nlh, void *arg)
 
 		hlist_del(&gwj->list);
 		cgw_unregister_filter(gwj);
-		kfree(gwj);
+		kmem_cache_free(cgw_cache, gwj);
 		err = 0;
 		break;
 	}
diff --git a/net/core/dev.c b/net/core/dev.c
index d540ced1f6c..b24ab0e98eb 100644
--- a/net/core/dev.c
+++ b/net/core/dev.c
@@ -1545,7 +1545,6 @@ void net_enable_timestamp(void)
 		return;
 	}
 #endif
-	WARN_ON(in_interrupt());
 	static_key_slow_inc(&netstamp_needed);
 }
 EXPORT_SYMBOL(net_enable_timestamp);
@@ -1625,7 +1624,6 @@ int dev_forward_skb(struct net_device *dev, struct sk_buff *skb)
 	}
 
 	skb_orphan(skb);
-	nf_reset(skb);
 
 	if (unlikely(!is_skb_forwardable(dev, skb))) {
 		atomic_long_inc(&dev->rx_dropped);
@@ -1641,6 +1639,7 @@ int dev_forward_skb(struct net_device *dev, struct sk_buff *skb)
 	skb->mark = 0;
 	secpath_reset(skb);
 	nf_reset(skb);
+	nf_reset_trace(skb);
 	return netif_rx(skb);
 }
 EXPORT_SYMBOL_GPL(dev_forward_skb);
@@ -2149,6 +2148,9 @@ static void skb_warn_bad_offload(const struct sk_buff *skb)
 	struct net_device *dev = skb->dev;
 	const char *driver = "";
 
+	if (!net_ratelimit())
+		return;
+
 	if (dev && dev->dev.parent)
 		driver = dev_driver_string(dev->dev.parent);
 
@@ -3315,6 +3317,7 @@ int netdev_rx_handler_register(struct net_device *dev,
 	if (dev->rx_handler)
 		return -EBUSY;
 
+	/* Note: rx_handler_data must be set before rx_handler */
 	rcu_assign_pointer(dev->rx_handler_data, rx_handler_data);
 	rcu_assign_pointer(dev->rx_handler, rx_handler);
 
@@ -3335,6 +3338,11 @@ void netdev_rx_handler_unregister(struct net_device *dev)
 
 	ASSERT_RTNL();
 	RCU_INIT_POINTER(dev->rx_handler, NULL);
+	/* a reader seeing a non NULL rx_handler in a rcu_read_lock()
+	 * section has a guarantee to see a non NULL rx_handler_data
+	 * as well.
+	 */
+	synchronize_net();
 	RCU_INIT_POINTER(dev->rx_handler_data, NULL);
 }
 EXPORT_SYMBOL_GPL(netdev_rx_handler_unregister);
diff --git a/net/core/dev_addr_lists.c b/net/core/dev_addr_lists.c
index bd2eb9d3e36..abdc9e6ef33 100644
--- a/net/core/dev_addr_lists.c
+++ b/net/core/dev_addr_lists.c
@@ -37,7 +37,7 @@ static int __hw_addr_create_ex(struct netdev_hw_addr_list *list,
 	ha->type = addr_type;
 	ha->refcount = 1;
 	ha->global_use = global;
-	ha->synced = false;
+	ha->synced = 0;
 	list_add_tail_rcu(&ha->list, &list->list);
 	list->count++;
 
@@ -165,7 +165,7 @@ int __hw_addr_sync(struct netdev_hw_addr_list *to_list,
 					    addr_len, ha->type);
 			if (err)
 				break;
-			ha->synced = true;
+			ha->synced++;
 			ha->refcount++;
 		} else if (ha->refcount == 1) {
 			__hw_addr_del(to_list, ha->addr, addr_len, ha->type);
@@ -186,7 +186,7 @@ void __hw_addr_unsync(struct netdev_hw_addr_list *to_list,
 		if (ha->synced) {
 			__hw_addr_del(to_list, ha->addr,
 				      addr_len, ha->type);
-			ha->synced = false;
+			ha->synced--;
 			__hw_addr_del(from_list, ha->addr,
 				      addr_len, ha->type);
 		}
diff --git a/net/core/flow.c b/net/core/flow.c
index c56ea6f7f6c..2bfd081c59f 100644
--- a/net/core/flow.c
+++ b/net/core/flow.c
@@ -328,7 +328,7 @@ static void flow_cache_flush_per_cpu(void *data)
 	struct flow_flush_info *info = data;
 	struct tasklet_struct *tasklet;
 
-	tasklet = this_cpu_ptr(&info->cache->percpu->flush_tasklet);
+	tasklet = &this_cpu_ptr(info->cache->percpu)->flush_tasklet;
 	tasklet->data = (unsigned long)info;
 	tasklet_schedule(tasklet);
 }
diff --git a/net/core/flow_dissector.c b/net/core/flow_dissector.c
index 9d4c7201400..e187bf06d67 100644
--- a/net/core/flow_dissector.c
+++ b/net/core/flow_dissector.c
@@ -140,6 +140,8 @@ ipv6:
 			flow->ports = *ports;
 	}
 
+	flow->thoff = (u16) nhoff;
+
 	return true;
 }
 EXPORT_SYMBOL(skb_flow_dissect);
diff --git a/net/core/pktgen.c b/net/core/pktgen.c
index 6048fc1da1c..5c217427a66 100644
--- a/net/core/pktgen.c
+++ b/net/core/pktgen.c
@@ -2198,7 +2198,7 @@ static inline int f_pick(struct pktgen_dev *pkt_dev)
 				pkt_dev->curfl = 0; /*reset */
 		}
 	} else {
-		flow = random32() % pkt_dev->cflows;
+		flow = prandom_u32() % pkt_dev->cflows;
 		pkt_dev->curfl = flow;
 
 		if (pkt_dev->flows[flow].count > pkt_dev->lflow) {
@@ -2246,7 +2246,7 @@ static void set_cur_queue_map(struct pktgen_dev *pkt_dev)
 	else if (pkt_dev->queue_map_min <= pkt_dev->queue_map_max) {
 		__u16 t;
 		if (pkt_dev->flags & F_QUEUE_MAP_RND) {
-			t = random32() %
+			t = prandom_u32() %
 				(pkt_dev->queue_map_max -
 				 pkt_dev->queue_map_min + 1)
 				+ pkt_dev->queue_map_min;
@@ -2278,7 +2278,7 @@ static void mod_cur_headers(struct pktgen_dev *pkt_dev)
 		__u32 tmp;
 
 		if (pkt_dev->flags & F_MACSRC_RND)
-			mc = random32() % pkt_dev->src_mac_count;
+			mc = prandom_u32() % pkt_dev->src_mac_count;
 		else {
 			mc = pkt_dev->cur_src_mac_offset++;
 			if (pkt_dev->cur_src_mac_offset >=
@@ -2304,7 +2304,7 @@ static void mod_cur_headers(struct pktgen_dev *pkt_dev)
 		__u32 tmp;
 
 		if (pkt_dev->flags & F_MACDST_RND)
-			mc = random32() % pkt_dev->dst_mac_count;
+			mc = prandom_u32() % pkt_dev->dst_mac_count;
 
 		else {
 			mc = pkt_dev->cur_dst_mac_offset++;
@@ -2331,21 +2331,21 @@ static void mod_cur_headers(struct pktgen_dev *pkt_dev)
 		for (i = 0; i < pkt_dev->nr_labels; i++)
 			if (pkt_dev->labels[i] & MPLS_STACK_BOTTOM)
 				pkt_dev->labels[i] = MPLS_STACK_BOTTOM |
-					     ((__force __be32)random32() &
+					     ((__force __be32)prandom_u32() &
 						      htonl(0x000fffff));
 	}
 
 	if ((pkt_dev->flags & F_VID_RND) && (pkt_dev->vlan_id != 0xffff)) {
-		pkt_dev->vlan_id = random32() & (4096-1);
+		pkt_dev->vlan_id = prandom_u32() & (4096 - 1);
 	}
 
 	if ((pkt_dev->flags & F_SVID_RND) && (pkt_dev->svlan_id != 0xffff)) {
-		pkt_dev->svlan_id = random32() & (4096 - 1);
+		pkt_dev->svlan_id = prandom_u32() & (4096 - 1);
 	}
 
 	if (pkt_dev->udp_src_min < pkt_dev->udp_src_max) {
 		if (pkt_dev->flags & F_UDPSRC_RND)
-			pkt_dev->cur_udp_src = random32() %
+			pkt_dev->cur_udp_src = prandom_u32() %
 				(pkt_dev->udp_src_max - pkt_dev->udp_src_min)
 				+ pkt_dev->udp_src_min;
 
@@ -2358,7 +2358,7 @@ static void mod_cur_headers(struct pktgen_dev *pkt_dev)
 
 	if (pkt_dev->udp_dst_min < pkt_dev->udp_dst_max) {
 		if (pkt_dev->flags & F_UDPDST_RND) {
-			pkt_dev->cur_udp_dst = random32() %
+			pkt_dev->cur_udp_dst = prandom_u32() %
 				(pkt_dev->udp_dst_max - pkt_dev->udp_dst_min)
 				+ pkt_dev->udp_dst_min;
 		} else {
@@ -2375,7 +2375,7 @@ static void mod_cur_headers(struct pktgen_dev *pkt_dev)
 		if (imn < imx) {
 			__u32 t;
 			if (pkt_dev->flags & F_IPSRC_RND)
-				t = random32() % (imx - imn) + imn;
+				t = prandom_u32() % (imx - imn) + imn;
 			else {
 				t = ntohl(pkt_dev->cur_saddr);
 				t++;
@@ -2396,17 +2396,15 @@ static void mod_cur_headers(struct pktgen_dev *pkt_dev)
 				__be32 s;
 				if (pkt_dev->flags & F_IPDST_RND) {
 
-					t = random32() % (imx - imn) + imn;
-					s = htonl(t);
-
-					while (ipv4_is_loopback(s) ||
-					       ipv4_is_multicast(s) ||
-					       ipv4_is_lbcast(s) ||
-					       ipv4_is_zeronet(s) ||
-					       ipv4_is_local_multicast(s)) {
-						t = random32() % (imx - imn) + imn;
+					do {
+						t = prandom_u32() %
+							(imx - imn) + imn;
 						s = htonl(t);
-					}
+					} while (ipv4_is_loopback(s) ||
+						ipv4_is_multicast(s) ||
+						ipv4_is_lbcast(s) ||
+						ipv4_is_zeronet(s) ||
+						ipv4_is_local_multicast(s));
 					pkt_dev->cur_daddr = s;
 				} else {
 					t = ntohl(pkt_dev->cur_daddr);
@@ -2437,7 +2435,7 @@ static void mod_cur_headers(struct pktgen_dev *pkt_dev)
 
 			for (i = 0; i < 4; i++) {
 				pkt_dev->cur_in6_daddr.s6_addr32[i] =
-				    (((__force __be32)random32() |
+				    (((__force __be32)prandom_u32() |
 				      pkt_dev->min_in6_daddr.s6_addr32[i]) &
 				     pkt_dev->max_in6_daddr.s6_addr32[i]);
 			}
@@ -2447,7 +2445,7 @@ static void mod_cur_headers(struct pktgen_dev *pkt_dev)
 	if (pkt_dev->min_pkt_size < pkt_dev->max_pkt_size) {
 		__u32 t;
 		if (pkt_dev->flags & F_TXSIZE_RND) {
-			t = random32() %
+			t = prandom_u32() %
 				(pkt_dev->max_pkt_size - pkt_dev->min_pkt_size)
 				+ pkt_dev->min_pkt_size;
 		} else {
diff --git a/net/core/rtnetlink.c b/net/core/rtnetlink.c
index 5fb8d7e4729..23854b51a25 100644
--- a/net/core/rtnetlink.c
+++ b/net/core/rtnetlink.c
@@ -496,8 +496,10 @@ static int rtnl_link_fill(struct sk_buff *skb, const struct net_device *dev)
 	}
 	if (ops->fill_info) {
 		data = nla_nest_start(skb, IFLA_INFO_DATA);
-		if (data == NULL)
+		if (data == NULL) {
+			err = -EMSGSIZE;
 			goto err_cancel_link;
+		}
 		err = ops->fill_info(skb, dev);
 		if (err < 0)
 			goto err_cancel_data;
@@ -1070,7 +1072,7 @@ static int rtnl_dump_ifinfo(struct sk_buff *skb, struct netlink_callback *cb)
 	rcu_read_lock();
 	cb->seq = net->dev_base_seq;
 
-	if (nlmsg_parse(cb->nlh, sizeof(struct rtgenmsg), tb, IFLA_MAX,
+	if (nlmsg_parse(cb->nlh, sizeof(struct ifinfomsg), tb, IFLA_MAX,
 			ifla_policy) >= 0) {
 
 		if (tb[IFLA_EXT_MASK])
@@ -1920,7 +1922,7 @@ static u16 rtnl_calcit(struct sk_buff *skb, struct nlmsghdr *nlh)
 	u32 ext_filter_mask = 0;
 	u16 min_ifinfo_dump_size = 0;
 
-	if (nlmsg_parse(nlh, sizeof(struct rtgenmsg), tb, IFLA_MAX,
+	if (nlmsg_parse(nlh, sizeof(struct ifinfomsg), tb, IFLA_MAX,
 			ifla_policy) >= 0) {
 		if (tb[IFLA_EXT_MASK])
 			ext_filter_mask = nla_get_u32(tb[IFLA_EXT_MASK]);
diff --git a/net/core/scm.c b/net/core/scm.c
index 905dcc6ad1e..2dc6cdaaae8 100644
--- a/net/core/scm.c
+++ b/net/core/scm.c
@@ -24,6 +24,7 @@
 #include <linux/interrupt.h>
 #include <linux/netdevice.h>
 #include <linux/security.h>
+#include <linux/pid_namespace.h>
 #include <linux/pid.h>
 #include <linux/nsproxy.h>
 #include <linux/slab.h>
@@ -52,7 +53,8 @@ static __inline__ int scm_check_creds(struct ucred *creds)
 	if (!uid_valid(uid) || !gid_valid(gid))
 		return -EINVAL;
 
-	if ((creds->pid == task_tgid_vnr(current) || nsown_capable(CAP_SYS_ADMIN)) &&
+	if ((creds->pid == task_tgid_vnr(current) ||
+	     ns_capable(current->nsproxy->pid_ns->user_ns, CAP_SYS_ADMIN)) &&
 	    ((uid_eq(uid, cred->uid)   || uid_eq(uid, cred->euid) ||
 	      uid_eq(uid, cred->suid)) || nsown_capable(CAP_SETUID)) &&
 	    ((gid_eq(gid, cred->gid)   || gid_eq(gid, cred->egid) ||
diff --git a/net/ipv4/af_inet.c b/net/ipv4/af_inet.c
index 68f6a94f766..c929d9c1c4b 100644
--- a/net/ipv4/af_inet.c
+++ b/net/ipv4/af_inet.c
@@ -1333,8 +1333,7 @@ static struct sk_buff *inet_gso_segment(struct sk_buff *skb,
 				iph->frag_off |= htons(IP_MF);
 			offset += (skb->len - skb->mac_len - iph->ihl * 4);
 		} else  {
-			if (!(iph->frag_off & htons(IP_DF)))
-				iph->id = htons(id++);
+			iph->id = htons(id++);
 		}
 		iph->tot_len = htons(skb->len - skb->mac_len);
 		iph->check = 0;
diff --git a/net/ipv4/devinet.c b/net/ipv4/devinet.c
index f678507bc82..c6287cd978c 100644
--- a/net/ipv4/devinet.c
+++ b/net/ipv4/devinet.c
@@ -587,13 +587,16 @@ static void check_lifetime(struct work_struct *work)
 {
 	unsigned long now, next, next_sec, next_sched;
 	struct in_ifaddr *ifa;
+	struct hlist_node *n;
 	int i;
 
 	now = jiffies;
 	next = round_jiffies_up(now + ADDR_CHECK_FREQUENCY);
 
-	rcu_read_lock();
 	for (i = 0; i < IN4_ADDR_HSIZE; i++) {
+		bool change_needed = false;
+
+		rcu_read_lock();
 		hlist_for_each_entry_rcu(ifa, &inet_addr_lst[i], hash) {
 			unsigned long age;
 
@@ -606,16 +609,7 @@ static void check_lifetime(struct work_struct *work)
 
 			if (ifa->ifa_valid_lft != INFINITY_LIFE_TIME &&
 			    age >= ifa->ifa_valid_lft) {
-				struct in_ifaddr **ifap ;
-
-				rtnl_lock();
-				for (ifap = &ifa->ifa_dev->ifa_list;
-				     *ifap != NULL; ifap = &ifa->ifa_next) {
-					if (*ifap == ifa)
-						inet_del_ifa(ifa->ifa_dev,
-							     ifap, 1);
-				}
-				rtnl_unlock();
+				change_needed = true;
 			} else if (ifa->ifa_preferred_lft ==
 				   INFINITY_LIFE_TIME) {
 				continue;
@@ -625,10 +619,8 @@ static void check_lifetime(struct work_struct *work)
 					next = ifa->ifa_tstamp +
 					       ifa->ifa_valid_lft * HZ;
 
-				if (!(ifa->ifa_flags & IFA_F_DEPRECATED)) {
-					ifa->ifa_flags |= IFA_F_DEPRECATED;
-					rtmsg_ifa(RTM_NEWADDR, ifa, NULL, 0);
-				}
+				if (!(ifa->ifa_flags & IFA_F_DEPRECATED))
+					change_needed = true;
 			} else if (time_before(ifa->ifa_tstamp +
 					       ifa->ifa_preferred_lft * HZ,
 					       next)) {
@@ -636,8 +628,42 @@ static void check_lifetime(struct work_struct *work)
 				       ifa->ifa_preferred_lft * HZ;
 			}
 		}
+		rcu_read_unlock();
+		if (!change_needed)
+			continue;
+		rtnl_lock();
+		hlist_for_each_entry_safe(ifa, n, &inet_addr_lst[i], hash) {
+			unsigned long age;
+
+			if (ifa->ifa_flags & IFA_F_PERMANENT)
+				continue;
+
+			/* We try to batch several events at once. */
+			age = (now - ifa->ifa_tstamp +
+			       ADDRCONF_TIMER_FUZZ_MINUS) / HZ;
+
+			if (ifa->ifa_valid_lft != INFINITY_LIFE_TIME &&
+			    age >= ifa->ifa_valid_lft) {
+				struct in_ifaddr **ifap;
+
+				for (ifap = &ifa->ifa_dev->ifa_list;
+				     *ifap != NULL; ifap = &(*ifap)->ifa_next) {
+					if (*ifap == ifa) {
+						inet_del_ifa(ifa->ifa_dev,
+							     ifap, 1);
+						break;
+					}
+				}
+			} else if (ifa->ifa_preferred_lft !=
+				   INFINITY_LIFE_TIME &&
+				   age >= ifa->ifa_preferred_lft &&
+				   !(ifa->ifa_flags & IFA_F_DEPRECATED)) {
+				ifa->ifa_flags |= IFA_F_DEPRECATED;
+				rtmsg_ifa(RTM_NEWADDR, ifa, NULL, 0);
+			}
+		}
+		rtnl_unlock();
 	}
-	rcu_read_unlock();
 
 	next_sec = round_jiffies_up(next);
 	next_sched = next;
@@ -802,8 +828,12 @@ static int inet_rtm_newaddr(struct sk_buff *skb, struct nlmsghdr *nlh, void *arg
 		if (nlh->nlmsg_flags & NLM_F_EXCL ||
 		    !(nlh->nlmsg_flags & NLM_F_REPLACE))
 			return -EEXIST;
-
-		set_ifa_lifetime(ifa_existing, valid_lft, prefered_lft);
+		ifa = ifa_existing;
+		set_ifa_lifetime(ifa, valid_lft, prefered_lft);
+		cancel_delayed_work(&check_lifetime_work);
+		schedule_delayed_work(&check_lifetime_work, 0);
+		rtmsg_ifa(RTM_NEWADDR, ifa, nlh, NETLINK_CB(skb).portid);
+		blocking_notifier_call_chain(&inetaddr_chain, NETDEV_UP, ifa);
 	}
 	return 0;
 }
diff --git a/net/ipv4/esp4.c b/net/ipv4/esp4.c
index 3b4f0cd2e63..4cfe34d4cc9 100644
--- a/net/ipv4/esp4.c
+++ b/net/ipv4/esp4.c
@@ -139,8 +139,6 @@ static int esp_output(struct xfrm_state *x, struct sk_buff *skb)
 
 	/* skb is pure payload to encrypt */
 
-	err = -ENOMEM;
-
 	esp = x->data;
 	aead = esp->aead;
 	alen = crypto_aead_authsize(aead);
@@ -176,8 +174,10 @@ static int esp_output(struct xfrm_state *x, struct sk_buff *skb)
 	}
 
 	tmp = esp_alloc_tmp(aead, nfrags + sglists, seqhilen);
-	if (!tmp)
+	if (!tmp) {
+		err = -ENOMEM;
 		goto error;
+	}
 
 	seqhi = esp_tmp_seqhi(tmp);
 	iv = esp_tmp_iv(aead, tmp, seqhilen);
diff --git a/net/ipv4/ip_fragment.c b/net/ipv4/ip_fragment.c
index a6445b843ef..52c273ea05c 100644
--- a/net/ipv4/ip_fragment.c
+++ b/net/ipv4/ip_fragment.c
@@ -248,8 +248,7 @@ static void ip_expire(unsigned long arg)
 		if (!head->dev)
 			goto out_rcu_unlock;
 
-		/* skb dst is stale, drop it, and perform route lookup again */
-		skb_dst_drop(head);
+		/* skb has no dst, perform route lookup again */
 		iph = ip_hdr(head);
 		err = ip_route_input_noref(head, iph->daddr, iph->saddr,
 					   iph->tos, head->dev);
@@ -523,9 +522,16 @@ found:
 		qp->q.max_size = skb->len + ihl;
 
 	if (qp->q.last_in == (INET_FRAG_FIRST_IN | INET_FRAG_LAST_IN) &&
-	    qp->q.meat == qp->q.len)
-		return ip_frag_reasm(qp, prev, dev);
+	    qp->q.meat == qp->q.len) {
+		unsigned long orefdst = skb->_skb_refdst;
 
+		skb->_skb_refdst = 0UL;
+		err = ip_frag_reasm(qp, prev, dev);
+		skb->_skb_refdst = orefdst;
+		return err;
+	}
+
+	skb_dst_drop(skb);
 	inet_frag_lru_move(&qp->q);
 	return -EINPROGRESS;
 
diff --git a/net/ipv4/ipconfig.c b/net/ipv4/ipconfig.c
index 98cbc687701..bf6c5cf31ae 100644
--- a/net/ipv4/ipconfig.c
+++ b/net/ipv4/ipconfig.c
@@ -1522,7 +1522,8 @@ static int __init ip_auto_config(void)
 		}
 	for (i++; i < CONF_NAMESERVERS_MAX; i++)
 		if (ic_nameservers[i] != NONE)
-			pr_cont(", nameserver%u=%pI4\n", i, &ic_nameservers[i]);
+			pr_cont(", nameserver%u=%pI4", i, &ic_nameservers[i]);
+	pr_cont("\n");
 #endif /* !SILENT */
 
 	return 0;
diff --git a/net/ipv4/netfilter/Kconfig b/net/ipv4/netfilter/Kconfig
index ce2d43e1f09..0d755c50994 100644
--- a/net/ipv4/netfilter/Kconfig
+++ b/net/ipv4/netfilter/Kconfig
@@ -36,19 +36,6 @@ config NF_CONNTRACK_PROC_COMPAT
 
 	  If unsure, say Y.
 
-config IP_NF_QUEUE
-	tristate "IP Userspace queueing via NETLINK (OBSOLETE)"
-	depends on NETFILTER_ADVANCED
-	help
-	  Netfilter has the ability to queue packets to user space: the
-	  netlink device can be used to access them using this driver.
-
-	  This option enables the old IPv4-only "ip_queue" implementation
-	  which has been obsoleted by the new "nfnetlink_queue" code (see
-	  CONFIG_NETFILTER_NETLINK_QUEUE).
-
-	  To compile it as a module, choose M here.  If unsure, say N.
-
 config IP_NF_IPTABLES
 	tristate "IP tables support (required for filtering/masq/NAT)"
 	default m if NETFILTER_ADVANCED=n
diff --git a/net/ipv4/netfilter/ipt_rpfilter.c b/net/ipv4/netfilter/ipt_rpfilter.c
index c30130062cd..c49dcd0284a 100644
--- a/net/ipv4/netfilter/ipt_rpfilter.c
+++ b/net/ipv4/netfilter/ipt_rpfilter.c
@@ -66,6 +66,12 @@ static bool rpfilter_lookup_reverse(struct flowi4 *fl4,
 	return dev_match;
 }
 
+static bool rpfilter_is_local(const struct sk_buff *skb)
+{
+	const struct rtable *rt = skb_rtable(skb);
+	return rt && (rt->rt_flags & RTCF_LOCAL);
+}
+
 static bool rpfilter_mt(const struct sk_buff *skb, struct xt_action_param *par)
 {
 	const struct xt_rpfilter_info *info;
@@ -76,7 +82,7 @@ static bool rpfilter_mt(const struct sk_buff *skb, struct xt_action_param *par)
 	info = par->matchinfo;
 	invert = info->flags & XT_RPFILTER_INVERT;
 
-	if (par->in->flags & IFF_LOOPBACK)
+	if (rpfilter_is_local(skb))
 		return true ^ invert;
 
 	iph = ip_hdr(skb);
diff --git a/net/ipv4/syncookies.c b/net/ipv4/syncookies.c
index ef54377fb11..397e0f69435 100644
--- a/net/ipv4/syncookies.c
+++ b/net/ipv4/syncookies.c
@@ -349,8 +349,8 @@ struct sock *cookie_v4_check(struct sock *sk, struct sk_buff *skb,
 	 * hasn't changed since we received the original syn, but I see
 	 * no easy way to do this.
 	 */
-	flowi4_init_output(&fl4, 0, sk->sk_mark, RT_CONN_FLAGS(sk),
-			   RT_SCOPE_UNIVERSE, IPPROTO_TCP,
+	flowi4_init_output(&fl4, sk->sk_bound_dev_if, sk->sk_mark,
+			   RT_CONN_FLAGS(sk), RT_SCOPE_UNIVERSE, IPPROTO_TCP,
 			   inet_sk_flowi_flags(sk),
 			   (opt && opt->srr) ? opt->faddr : ireq->rmt_addr,
 			   ireq->loc_addr, th->source, th->dest);
diff --git a/net/ipv4/tcp_input.c b/net/ipv4/tcp_input.c
index 0d9bdacce99..13b9c08fc15 100644
--- a/net/ipv4/tcp_input.c
+++ b/net/ipv4/tcp_input.c
@@ -113,6 +113,7 @@ int sysctl_tcp_early_retrans __read_mostly = 2;
 #define FLAG_DSACKING_ACK	0x800 /* SACK blocks contained D-SACK info */
 #define FLAG_NONHEAD_RETRANS_ACKED	0x1000 /* Non-head rexmitted data was ACKed */
 #define FLAG_SACK_RENEGING	0x2000 /* snd_una advanced to a sacked seq */
+#define FLAG_UPDATE_TS_RECENT	0x4000 /* tcp_replace_ts_recent() */
 
 #define FLAG_ACKED		(FLAG_DATA_ACKED|FLAG_SYN_ACKED)
 #define FLAG_NOT_DUP		(FLAG_DATA|FLAG_WIN_UPDATE|FLAG_ACKED)
@@ -2059,11 +2060,8 @@ void tcp_enter_loss(struct sock *sk, int how)
 	if (tcp_is_reno(tp))
 		tcp_reset_reno_sack(tp);
 
-	if (!how) {
-		/* Push undo marker, if it was plain RTO and nothing
-		 * was retransmitted. */
-		tp->undo_marker = tp->snd_una;
-	} else {
+	tp->undo_marker = tp->snd_una;
+	if (how) {
 		tp->sacked_out = 0;
 		tp->fackets_out = 0;
 	}
@@ -3567,6 +3565,27 @@ static void tcp_send_challenge_ack(struct sock *sk)
 	}
 }
 
+static void tcp_store_ts_recent(struct tcp_sock *tp)
+{
+	tp->rx_opt.ts_recent = tp->rx_opt.rcv_tsval;
+	tp->rx_opt.ts_recent_stamp = get_seconds();
+}
+
+static void tcp_replace_ts_recent(struct tcp_sock *tp, u32 seq)
+{
+	if (tp->rx_opt.saw_tstamp && !after(seq, tp->rcv_wup)) {
+		/* PAWS bug workaround wrt. ACK frames, the PAWS discard
+		 * extra check below makes sure this can only happen
+		 * for pure ACK frames.  -DaveM
+		 *
+		 * Not only, also it occurs for expired timestamps.
+		 */
+
+		if (tcp_paws_check(&tp->rx_opt, 0))
+			tcp_store_ts_recent(tp);
+	}
+}
+
 /* This routine deals with incoming acks, but not outgoing ones. */
 static int tcp_ack(struct sock *sk, const struct sk_buff *skb, int flag)
 {
@@ -3610,6 +3629,12 @@ static int tcp_ack(struct sock *sk, const struct sk_buff *skb, int flag)
 	prior_fackets = tp->fackets_out;
 	prior_in_flight = tcp_packets_in_flight(tp);
 
+	/* ts_recent update must be made after we are sure that the packet
+	 * is in window.
+	 */
+	if (flag & FLAG_UPDATE_TS_RECENT)
+		tcp_replace_ts_recent(tp, TCP_SKB_CB(skb)->seq);
+
 	if (!(flag & FLAG_SLOWPATH) && after(ack, prior_snd_una)) {
 		/* Window is constant, pure forward advance.
 		 * No more checks are required.
@@ -3930,27 +3955,6 @@ const u8 *tcp_parse_md5sig_option(const struct tcphdr *th)
 EXPORT_SYMBOL(tcp_parse_md5sig_option);
 #endif
 
-static inline void tcp_store_ts_recent(struct tcp_sock *tp)
-{
-	tp->rx_opt.ts_recent = tp->rx_opt.rcv_tsval;
-	tp->rx_opt.ts_recent_stamp = get_seconds();
-}
-
-static inline void tcp_replace_ts_recent(struct tcp_sock *tp, u32 seq)
-{
-	if (tp->rx_opt.saw_tstamp && !after(seq, tp->rcv_wup)) {
-		/* PAWS bug workaround wrt. ACK frames, the PAWS discard
-		 * extra check below makes sure this can only happen
-		 * for pure ACK frames.  -DaveM
-		 *
-		 * Not only, also it occurs for expired timestamps.
-		 */
-
-		if (tcp_paws_check(&tp->rx_opt, 0))
-			tcp_store_ts_recent(tp);
-	}
-}
-
 /* Sorry, PAWS as specified is broken wrt. pure-ACKs -DaveM
  *
  * It is not fatal. If this ACK does _not_ change critical state (seqs, window)
@@ -5546,14 +5550,9 @@ slow_path:
 		return 0;
 
 step5:
-	if (tcp_ack(sk, skb, FLAG_SLOWPATH) < 0)
+	if (tcp_ack(sk, skb, FLAG_SLOWPATH | FLAG_UPDATE_TS_RECENT) < 0)
 		goto discard;
 
-	/* ts_recent update must be made after we are sure that the packet
-	 * is in window.
-	 */
-	tcp_replace_ts_recent(tp, TCP_SKB_CB(skb)->seq);
-
 	tcp_rcv_rtt_measure_ts(sk, skb);
 
 	/* Process urgent data. */
@@ -5989,7 +5988,8 @@ int tcp_rcv_state_process(struct sock *sk, struct sk_buff *skb,
 
 	/* step 5: check the ACK field */
 	if (true) {
-		int acceptable = tcp_ack(sk, skb, FLAG_SLOWPATH) > 0;
+		int acceptable = tcp_ack(sk, skb, FLAG_SLOWPATH |
+						  FLAG_UPDATE_TS_RECENT) > 0;
 
 		switch (sk->sk_state) {
 		case TCP_SYN_RECV:
@@ -6140,11 +6140,6 @@ int tcp_rcv_state_process(struct sock *sk, struct sk_buff *skb,
 		}
 	}
 
-	/* ts_recent update must be made after we are sure that the packet
-	 * is in window.
-	 */
-	tcp_replace_ts_recent(tp, TCP_SKB_CB(skb)->seq);
-
 	/* step 6: check the URG bit */
 	tcp_urg(sk, skb, th);
 
diff --git a/net/ipv4/tcp_output.c b/net/ipv4/tcp_output.c
index 817fbb396bc..509912a5ff9 100644
--- a/net/ipv4/tcp_output.c
+++ b/net/ipv4/tcp_output.c
@@ -1809,8 +1809,11 @@ static bool tcp_tso_should_defer(struct sock *sk, struct sk_buff *skb)
 			goto send_now;
 	}
 
-	/* Ok, it looks like it is advisable to defer.  */
-	tp->tso_deferred = 1 | (jiffies << 1);
+	/* Ok, it looks like it is advisable to defer.
+	 * Do not rearm the timer if already set to not break TCP ACK clocking.
+	 */
+	if (!tp->tso_deferred)
+		tp->tso_deferred = 1 | (jiffies << 1);
 
 	return true;
 
@@ -2385,8 +2388,12 @@ int __tcp_retransmit_skb(struct sock *sk, struct sk_buff *skb)
 	 */
 	TCP_SKB_CB(skb)->when = tcp_time_stamp;
 
-	/* make sure skb->data is aligned on arches that require it */
-	if (unlikely(NET_IP_ALIGN && ((unsigned long)skb->data & 3))) {
+	/* make sure skb->data is aligned on arches that require it
+	 * and check if ack-trimming & collapsing extended the headroom
+	 * beyond what csum_start can cover.
+	 */
+	if (unlikely((NET_IP_ALIGN && ((unsigned long)skb->data & 3)) ||
+		     skb_headroom(skb) >= 0xFFFF)) {
 		struct sk_buff *nskb = __pskb_copy(skb, MAX_TCP_HEADER,
 						   GFP_ATOMIC);
 		return nskb ? tcp_transmit_skb(sk, nskb, 0, GFP_ATOMIC) :
@@ -2706,6 +2713,7 @@ struct sk_buff *tcp_make_synack(struct sock *sk, struct dst_entry *dst,
 	skb_reserve(skb, MAX_TCP_HEADER);
 
 	skb_dst_set(skb, dst);
+	security_skb_owned_by(skb, sk);
 
 	mss = dst_metric_advmss(dst);
 	if (tp->rx_opt.user_mss && tp->rx_opt.user_mss < mss)
diff --git a/net/ipv4/udp.c b/net/ipv4/udp.c
index 265c42cf963..0a073a26372 100644
--- a/net/ipv4/udp.c
+++ b/net/ipv4/udp.c
@@ -1762,9 +1762,16 @@ int udp_rcv(struct sk_buff *skb)
 
 void udp_destroy_sock(struct sock *sk)
 {
+	struct udp_sock *up = udp_sk(sk);
 	bool slow = lock_sock_fast(sk);
 	udp_flush_pending_frames(sk);
 	unlock_sock_fast(sk, slow);
+	if (static_key_false(&udp_encap_needed) && up->encap_type) {
+		void (*encap_destroy)(struct sock *sk);
+		encap_destroy = ACCESS_ONCE(up->encap_destroy);
+		if (encap_destroy)
+			encap_destroy(sk);
+	}
 }
 
 /*
diff --git a/net/ipv6/addrconf.c b/net/ipv6/addrconf.c
index f2c7e615f90..dae802c0af7 100644
--- a/net/ipv6/addrconf.c
+++ b/net/ipv6/addrconf.c
@@ -168,8 +168,6 @@ static void inet6_prefix_notify(int event, struct inet6_dev *idev,
 static bool ipv6_chk_same_addr(struct net *net, const struct in6_addr *addr,
 			       struct net_device *dev);
 
-static ATOMIC_NOTIFIER_HEAD(inet6addr_chain);
-
 static struct ipv6_devconf ipv6_devconf __read_mostly = {
 	.forwarding		= 0,
 	.hop_limit		= IPV6_DEFAULT_HOPLIMIT,
@@ -837,7 +835,7 @@ out2:
 	rcu_read_unlock_bh();
 
 	if (likely(err == 0))
-		atomic_notifier_call_chain(&inet6addr_chain, NETDEV_UP, ifa);
+		inet6addr_notifier_call_chain(NETDEV_UP, ifa);
 	else {
 		kfree(ifa);
 		ifa = ERR_PTR(err);
@@ -927,7 +925,7 @@ static void ipv6_del_addr(struct inet6_ifaddr *ifp)
 
 	ipv6_ifa_notify(RTM_DELADDR, ifp);
 
-	atomic_notifier_call_chain(&inet6addr_chain, NETDEV_DOWN, ifp);
+	inet6addr_notifier_call_chain(NETDEV_DOWN, ifp);
 
 	/*
 	 * Purge or update corresponding prefix
@@ -2529,6 +2527,9 @@ static void sit_add_v4_addrs(struct inet6_dev *idev)
 static void init_loopback(struct net_device *dev)
 {
 	struct inet6_dev  *idev;
+	struct net_device *sp_dev;
+	struct inet6_ifaddr *sp_ifa;
+	struct rt6_info *sp_rt;
 
 	/* ::1 */
 
@@ -2540,6 +2541,30 @@ static void init_loopback(struct net_device *dev)
 	}
 
 	add_addr(idev, &in6addr_loopback, 128, IFA_HOST);
+
+	/* Add routes to other interface's IPv6 addresses */
+	for_each_netdev(dev_net(dev), sp_dev) {
+		if (!strcmp(sp_dev->name, dev->name))
+			continue;
+
+		idev = __in6_dev_get(sp_dev);
+		if (!idev)
+			continue;
+
+		read_lock_bh(&idev->lock);
+		list_for_each_entry(sp_ifa, &idev->addr_list, if_list) {
+
+			if (sp_ifa->flags & (IFA_F_DADFAILED | IFA_F_TENTATIVE))
+				continue;
+
+			sp_rt = addrconf_dst_alloc(idev, &sp_ifa->addr, 0);
+
+			/* Failure cases are ignored */
+			if (!IS_ERR(sp_rt))
+				ip6_ins_rt(sp_rt);
+		}
+		read_unlock_bh(&idev->lock);
+	}
 }
 
 static void addrconf_add_linklocal(struct inet6_dev *idev, const struct in6_addr *addr)
@@ -2961,7 +2986,7 @@ static int addrconf_ifdown(struct net_device *dev, int how)
 
 		if (state != INET6_IFADDR_STATE_DEAD) {
 			__ipv6_ifa_notify(RTM_DELADDR, ifa);
-			atomic_notifier_call_chain(&inet6addr_chain, NETDEV_DOWN, ifa);
+			inet6addr_notifier_call_chain(NETDEV_DOWN, ifa);
 		}
 		in6_ifa_put(ifa);
 
@@ -4784,26 +4809,20 @@ static void addrconf_sysctl_unregister(struct inet6_dev *idev)
 
 static int __net_init addrconf_init_net(struct net *net)
 {
-	int err;
+	int err = -ENOMEM;
 	struct ipv6_devconf *all, *dflt;
 
-	err = -ENOMEM;
-	all = &ipv6_devconf;
-	dflt = &ipv6_devconf_dflt;
+	all = kmemdup(&ipv6_devconf, sizeof(ipv6_devconf), GFP_KERNEL);
+	if (all == NULL)
+		goto err_alloc_all;
 
-	if (!net_eq(net, &init_net)) {
-		all = kmemdup(all, sizeof(ipv6_devconf), GFP_KERNEL);
-		if (all == NULL)
-			goto err_alloc_all;
+	dflt = kmemdup(&ipv6_devconf_dflt, sizeof(ipv6_devconf_dflt), GFP_KERNEL);
+	if (dflt == NULL)
+		goto err_alloc_dflt;
 
-		dflt = kmemdup(dflt, sizeof(ipv6_devconf_dflt), GFP_KERNEL);
-		if (dflt == NULL)
-			goto err_alloc_dflt;
-	} else {
-		/* these will be inherited by all namespaces */
-		dflt->autoconf = ipv6_defaults.autoconf;
-		dflt->disable_ipv6 = ipv6_defaults.disable_ipv6;
-	}
+	/* these will be inherited by all namespaces */
+	dflt->autoconf = ipv6_defaults.autoconf;
+	dflt->disable_ipv6 = ipv6_defaults.disable_ipv6;
 
 	net->ipv6.devconf_all = all;
 	net->ipv6.devconf_dflt = dflt;
@@ -4848,22 +4867,6 @@ static struct pernet_operations addrconf_ops = {
 	.exit = addrconf_exit_net,
 };
 
-/*
- *      Device notifier
- */
-
-int register_inet6addr_notifier(struct notifier_block *nb)
-{
-	return atomic_notifier_chain_register(&inet6addr_chain, nb);
-}
-EXPORT_SYMBOL(register_inet6addr_notifier);
-
-int unregister_inet6addr_notifier(struct notifier_block *nb)
-{
-	return atomic_notifier_chain_unregister(&inet6addr_chain, nb);
-}
-EXPORT_SYMBOL(unregister_inet6addr_notifier);
-
 static struct rtnl_af_ops inet6_ops = {
 	.family		  = AF_INET6,
 	.fill_link_af	  = inet6_fill_link_af,
diff --git a/net/ipv6/addrconf_core.c b/net/ipv6/addrconf_core.c
index d051e5f4bf3..72104562c86 100644
--- a/net/ipv6/addrconf_core.c
+++ b/net/ipv6/addrconf_core.c
@@ -78,3 +78,22 @@ int __ipv6_addr_type(const struct in6_addr *addr)
 }
 EXPORT_SYMBOL(__ipv6_addr_type);
 
+static ATOMIC_NOTIFIER_HEAD(inet6addr_chain);
+
+int register_inet6addr_notifier(struct notifier_block *nb)
+{
+	return atomic_notifier_chain_register(&inet6addr_chain, nb);
+}
+EXPORT_SYMBOL(register_inet6addr_notifier);
+
+int unregister_inet6addr_notifier(struct notifier_block *nb)
+{
+	return atomic_notifier_chain_unregister(&inet6addr_chain, nb);
+}
+EXPORT_SYMBOL(unregister_inet6addr_notifier);
+
+int inet6addr_notifier_call_chain(unsigned long val, void *v)
+{
+	return atomic_notifier_call_chain(&inet6addr_chain, val, v);
+}
+EXPORT_SYMBOL(inet6addr_notifier_call_chain);
diff --git a/net/ipv6/ip6_input.c b/net/ipv6/ip6_input.c
index e33fe0ab256..2bab2aa5974 100644
--- a/net/ipv6/ip6_input.c
+++ b/net/ipv6/ip6_input.c
@@ -118,6 +118,18 @@ int ipv6_rcv(struct sk_buff *skb, struct net_device *dev, struct packet_type *pt
 	    ipv6_addr_loopback(&hdr->daddr))
 		goto err;
 
+	/* RFC4291 Errata ID: 3480
+	 * Interface-Local scope spans only a single interface on a
+	 * node and is useful only for loopback transmission of
+	 * multicast.  Packets with interface-local scope received
+	 * from another node must be discarded.
+	 */
+	if (!(skb->pkt_type == PACKET_LOOPBACK ||
+	      dev->flags & IFF_LOOPBACK) &&
+	    ipv6_addr_is_multicast(&hdr->daddr) &&
+	    IPV6_ADDR_MC_SCOPE(&hdr->daddr) == 1)
+		goto err;
+
 	/* RFC4291 2.7
 	 * Nodes must not originate a packet to a multicast address whose scope
 	 * field contains the reserved value 0; if such a packet is received, it
diff --git a/net/ipv6/netfilter/ip6t_NPT.c b/net/ipv6/netfilter/ip6t_NPT.c
index 83acc1405a1..cb631143721 100644
--- a/net/ipv6/netfilter/ip6t_NPT.c
+++ b/net/ipv6/netfilter/ip6t_NPT.c
@@ -57,7 +57,7 @@ static bool ip6t_npt_map_pfx(const struct ip6t_npt_tginfo *npt,
 		if (pfx_len - i >= 32)
 			mask = 0;
 		else
-			mask = htonl(~((1 << (pfx_len - i)) - 1));
+			mask = htonl((1 << (i - pfx_len + 32)) - 1);
 
 		idx = i / 32;
 		addr->s6_addr32[idx] &= mask;
@@ -114,6 +114,7 @@ ip6t_dnpt_tg(struct sk_buff *skb, const struct xt_action_param *par)
 static struct xt_target ip6t_npt_target_reg[] __read_mostly = {
 	{
 		.name		= "SNPT",
+		.table		= "mangle",
 		.target		= ip6t_snpt_tg,
 		.targetsize	= sizeof(struct ip6t_npt_tginfo),
 		.checkentry	= ip6t_npt_checkentry,
@@ -124,6 +125,7 @@ static struct xt_target ip6t_npt_target_reg[] __read_mostly = {
 	},
 	{
 		.name		= "DNPT",
+		.table		= "mangle",
 		.target		= ip6t_dnpt_tg,
 		.targetsize	= sizeof(struct ip6t_npt_tginfo),
 		.checkentry	= ip6t_npt_checkentry,
diff --git a/net/ipv6/netfilter/ip6t_rpfilter.c b/net/ipv6/netfilter/ip6t_rpfilter.c
index 5060d54199a..e0983f3648a 100644
--- a/net/ipv6/netfilter/ip6t_rpfilter.c
+++ b/net/ipv6/netfilter/ip6t_rpfilter.c
@@ -71,6 +71,12 @@ static bool rpfilter_lookup_reverse6(const struct sk_buff *skb,
 	return ret;
 }
 
+static bool rpfilter_is_local(const struct sk_buff *skb)
+{
+	const struct rt6_info *rt = (const void *) skb_dst(skb);
+	return rt && (rt->rt6i_flags & RTF_LOCAL);
+}
+
 static bool rpfilter_mt(const struct sk_buff *skb, struct xt_action_param *par)
 {
 	const struct xt_rpfilter_info *info = par->matchinfo;
@@ -78,7 +84,7 @@ static bool rpfilter_mt(const struct sk_buff *skb, struct xt_action_param *par)
 	struct ipv6hdr *iph;
 	bool invert = info->flags & XT_RPFILTER_INVERT;
 
-	if (par->in->flags & IFF_LOOPBACK)
+	if (rpfilter_is_local(skb))
 		return true ^ invert;
 
 	iph = ipv6_hdr(skb);
diff --git a/net/ipv6/reassembly.c b/net/ipv6/reassembly.c
index 196ab9347ad..0ba10e53a62 100644
--- a/net/ipv6/reassembly.c
+++ b/net/ipv6/reassembly.c
@@ -330,9 +330,17 @@ found:
 	}
 
 	if (fq->q.last_in == (INET_FRAG_FIRST_IN | INET_FRAG_LAST_IN) &&
-	    fq->q.meat == fq->q.len)
-		return ip6_frag_reasm(fq, prev, dev);
+	    fq->q.meat == fq->q.len) {
+		int res;
+		unsigned long orefdst = skb->_skb_refdst;
+
+		skb->_skb_refdst = 0UL;
+		res = ip6_frag_reasm(fq, prev, dev);
+		skb->_skb_refdst = orefdst;
+		return res;
+	}
 
+	skb_dst_drop(skb);
 	inet_frag_lru_move(&fq->q);
 	return -1;
 
diff --git a/net/ipv6/tcp_ipv6.c b/net/ipv6/tcp_ipv6.c
index f6d629fd6ae..46a5be85be8 100644
--- a/net/ipv6/tcp_ipv6.c
+++ b/net/ipv6/tcp_ipv6.c
@@ -386,6 +386,7 @@ static void tcp_v6_err(struct sk_buff *skb, struct inet6_skb_parm *opt,
 
 		if (dst)
 			dst->ops->redirect(dst, sk, skb);
+		goto out;
 	}
 
 	if (type == ICMPV6_PKT_TOOBIG) {
diff --git a/net/ipv6/udp.c b/net/ipv6/udp.c
index 599e1ba6d1c..d8e5e852fc7 100644
--- a/net/ipv6/udp.c
+++ b/net/ipv6/udp.c
@@ -1285,10 +1285,18 @@ do_confirm:
 
 void udpv6_destroy_sock(struct sock *sk)
 {
+	struct udp_sock *up = udp_sk(sk);
 	lock_sock(sk);
 	udp_v6_flush_pending_frames(sk);
 	release_sock(sk);
 
+	if (static_key_false(&udpv6_encap_needed) && up->encap_type) {
+		void (*encap_destroy)(struct sock *sk);
+		encap_destroy = ACCESS_ONCE(up->encap_destroy);
+		if (encap_destroy)
+			encap_destroy(sk);
+	}
+
 	inet6_destroy_sock(sk);
 }
 
diff --git a/net/irda/af_irda.c b/net/irda/af_irda.c
index d07e3a62644..e493b3397ae 100644
--- a/net/irda/af_irda.c
+++ b/net/irda/af_irda.c
@@ -1386,6 +1386,8 @@ static int irda_recvmsg_dgram(struct kiocb *iocb, struct socket *sock,
 
 	IRDA_DEBUG(4, "%s()\n", __func__);
 
+	msg->msg_namelen = 0;
+
 	skb = skb_recv_datagram(sk, flags & ~MSG_DONTWAIT,
 				flags & MSG_DONTWAIT, &err);
 	if (!skb)
@@ -2583,8 +2585,10 @@ bed:
 				    NULL, NULL, NULL);
 
 		/* Check if the we got some results */
-		if (!self->cachedaddr)
-			return -EAGAIN;		/* Didn't find any devices */
+		if (!self->cachedaddr) {
+			err = -EAGAIN;		/* Didn't find any devices */
+			goto out;
+		}
 		daddr = self->cachedaddr;
 		/* Cleanup */
 		self->cachedaddr = 0;
diff --git a/net/irda/ircomm/ircomm_tty.c b/net/irda/ircomm/ircomm_tty.c
index 362ba47968e..41ac7938268 100644
--- a/net/irda/ircomm/ircomm_tty.c
+++ b/net/irda/ircomm/ircomm_tty.c
@@ -328,7 +328,7 @@ static int ircomm_tty_block_til_ready(struct ircomm_tty_cb *self,
 	spin_unlock_irqrestore(&port->lock, flags);
 
 	while (1) {
-		if (tty->termios.c_cflag & CBAUD)
+		if (C_BAUD(tty) && test_bit(ASYNCB_INITIALIZED, &port->flags))
 			tty_port_raise_dtr_rts(port);
 
 		set_current_state(TASK_INTERRUPTIBLE);
diff --git a/net/irda/ircomm/ircomm_tty_attach.c b/net/irda/ircomm/ircomm_tty_attach.c
index edab393e0c8..a2a508f5f26 100644
--- a/net/irda/ircomm/ircomm_tty_attach.c
+++ b/net/irda/ircomm/ircomm_tty_attach.c
@@ -997,12 +997,8 @@ static int ircomm_tty_state_ready(struct ircomm_tty_cb *self,
 			self->settings.dce = IRCOMM_DELTA_CD;
 			ircomm_tty_check_modem_status(self);
 		} else {
-			struct tty_struct *tty = tty_port_tty_get(&self->port);
 			IRDA_DEBUG(0, "%s(), hanging up!\n", __func__ );
-			if (tty) {
-				tty_hangup(tty);
-				tty_kref_put(tty);
-			}
+			tty_port_tty_hangup(&self->port, false);
 		}
 		break;
 	default:
diff --git a/net/irda/iriap.c b/net/irda/iriap.c
index 29340a9a6fb..e1b37f5a269 100644
--- a/net/irda/iriap.c
+++ b/net/irda/iriap.c
@@ -303,7 +303,8 @@ static void iriap_disconnect_indication(void *instance, void *sap,
 {
 	struct iriap_cb *self;
 
-	IRDA_DEBUG(4, "%s(), reason=%s\n", __func__, irlmp_reasons[reason]);
+	IRDA_DEBUG(4, "%s(), reason=%s [%d]\n", __func__,
+		   irlmp_reason_str(reason), reason);
 
 	self = instance;
 
diff --git a/net/irda/irlmp.c b/net/irda/irlmp.c
index 6115a44c0a2..1064621da6f 100644
--- a/net/irda/irlmp.c
+++ b/net/irda/irlmp.c
@@ -66,8 +66,15 @@ const char *irlmp_reasons[] = {
 	"LM_LAP_RESET",
 	"LM_INIT_DISCONNECT",
 	"ERROR, NOT USED",
+	"UNKNOWN",
 };
 
+const char *irlmp_reason_str(LM_REASON reason)
+{
+	reason = min_t(size_t, reason, ARRAY_SIZE(irlmp_reasons) - 1);
+	return irlmp_reasons[reason];
+}
+
 /*
  * Function irlmp_init (void)
  *
@@ -747,7 +754,8 @@ void irlmp_disconnect_indication(struct lsap_cb *self, LM_REASON reason,
 {
 	struct lsap_cb *lsap;
 
-	IRDA_DEBUG(1, "%s(), reason=%s\n", __func__, irlmp_reasons[reason]);
+	IRDA_DEBUG(1, "%s(), reason=%s [%d]\n", __func__,
+		   irlmp_reason_str(reason), reason);
 	IRDA_ASSERT(self != NULL, return;);
 	IRDA_ASSERT(self->magic == LMP_LSAP_MAGIC, return;);
 
diff --git a/net/iucv/af_iucv.c b/net/iucv/af_iucv.c
index a7d11ffe428..206ce6db2c3 100644
--- a/net/iucv/af_iucv.c
+++ b/net/iucv/af_iucv.c
@@ -49,12 +49,6 @@ static const u8 iprm_shutdown[8] =
 
 #define TRGCLS_SIZE	(sizeof(((struct iucv_message *)0)->class))
 
-/* macros to set/get socket control buffer at correct offset */
-#define CB_TAG(skb)	((skb)->cb)		/* iucv message tag */
-#define CB_TAG_LEN	(sizeof(((struct iucv_message *) 0)->tag))
-#define CB_TRGCLS(skb)	((skb)->cb + CB_TAG_LEN) /* iucv msg target class */
-#define CB_TRGCLS_LEN	(TRGCLS_SIZE)
-
 #define __iucv_sock_wait(sk, condition, timeo, ret)			\
 do {									\
 	DEFINE_WAIT(__wait);						\
@@ -1141,7 +1135,7 @@ static int iucv_sock_sendmsg(struct kiocb *iocb, struct socket *sock,
 
 	/* increment and save iucv message tag for msg_completion cbk */
 	txmsg.tag = iucv->send_tag++;
-	memcpy(CB_TAG(skb), &txmsg.tag, CB_TAG_LEN);
+	IUCV_SKB_CB(skb)->tag = txmsg.tag;
 
 	if (iucv->transport == AF_IUCV_TRANS_HIPER) {
 		atomic_inc(&iucv->msg_sent);
@@ -1224,7 +1218,7 @@ static int iucv_fragment_skb(struct sock *sk, struct sk_buff *skb, int len)
 			return -ENOMEM;
 
 		/* copy target class to control buffer of new skb */
-		memcpy(CB_TRGCLS(nskb), CB_TRGCLS(skb), CB_TRGCLS_LEN);
+		IUCV_SKB_CB(nskb)->class = IUCV_SKB_CB(skb)->class;
 
 		/* copy data fragment */
 		memcpy(nskb->data, skb->data + copied, size);
@@ -1256,7 +1250,7 @@ static void iucv_process_message(struct sock *sk, struct sk_buff *skb,
 
 	/* store msg target class in the second 4 bytes of skb ctrl buffer */
 	/* Note: the first 4 bytes are reserved for msg tag */
-	memcpy(CB_TRGCLS(skb), &msg->class, CB_TRGCLS_LEN);
+	IUCV_SKB_CB(skb)->class = msg->class;
 
 	/* check for special IPRM messages (e.g. iucv_sock_shutdown) */
 	if ((msg->flags & IUCV_IPRMDATA) && len > 7) {
@@ -1292,6 +1286,7 @@ static void iucv_process_message(struct sock *sk, struct sk_buff *skb,
 		}
 	}
 
+	IUCV_SKB_CB(skb)->offset = 0;
 	if (sock_queue_rcv_skb(sk, skb))
 		skb_queue_head(&iucv_sk(sk)->backlog_skb_q, skb);
 }
@@ -1327,6 +1322,9 @@ static int iucv_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
 	unsigned int copied, rlen;
 	struct sk_buff *skb, *rskb, *cskb;
 	int err = 0;
+	u32 offset;
+
+	msg->msg_namelen = 0;
 
 	if ((sk->sk_state == IUCV_DISCONN) &&
 	    skb_queue_empty(&iucv->backlog_skb_q) &&
@@ -1346,13 +1344,14 @@ static int iucv_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
 		return err;
 	}
 
-	rlen   = skb->len;		/* real length of skb */
+	offset = IUCV_SKB_CB(skb)->offset;
+	rlen   = skb->len - offset;		/* real length of skb */
 	copied = min_t(unsigned int, rlen, len);
 	if (!rlen)
 		sk->sk_shutdown = sk->sk_shutdown | RCV_SHUTDOWN;
 
 	cskb = skb;
-	if (skb_copy_datagram_iovec(cskb, 0, msg->msg_iov, copied)) {
+	if (skb_copy_datagram_iovec(cskb, offset, msg->msg_iov, copied)) {
 		if (!(flags & MSG_PEEK))
 			skb_queue_head(&sk->sk_receive_queue, skb);
 		return -EFAULT;
@@ -1370,7 +1369,8 @@ static int iucv_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
 	 * get the trgcls from the control buffer of the skb due to
 	 * fragmentation of original iucv message. */
 	err = put_cmsg(msg, SOL_IUCV, SCM_IUCV_TRGCLS,
-			CB_TRGCLS_LEN, CB_TRGCLS(skb));
+		       sizeof(IUCV_SKB_CB(skb)->class),
+		       (void *)&IUCV_SKB_CB(skb)->class);
 	if (err) {
 		if (!(flags & MSG_PEEK))
 			skb_queue_head(&sk->sk_receive_queue, skb);
@@ -1382,9 +1382,8 @@ static int iucv_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
 
 		/* SOCK_STREAM: re-queue skb if it contains unreceived data */
 		if (sk->sk_type == SOCK_STREAM) {
-			skb_pull(skb, copied);
-			if (skb->len) {
-				skb_queue_head(&sk->sk_receive_queue, skb);
+			if (copied < rlen) {
+				IUCV_SKB_CB(skb)->offset = offset + copied;
 				goto done;
 			}
 		}
@@ -1403,6 +1402,7 @@ static int iucv_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
 		spin_lock_bh(&iucv->message_q.lock);
 		rskb = skb_dequeue(&iucv->backlog_skb_q);
 		while (rskb) {
+			IUCV_SKB_CB(rskb)->offset = 0;
 			if (sock_queue_rcv_skb(sk, rskb)) {
 				skb_queue_head(&iucv->backlog_skb_q,
 						rskb);
@@ -1830,7 +1830,7 @@ static void iucv_callback_txdone(struct iucv_path *path,
 		spin_lock_irqsave(&list->lock, flags);
 
 		while (list_skb != (struct sk_buff *)list) {
-			if (!memcmp(&msg->tag, CB_TAG(list_skb), CB_TAG_LEN)) {
+			if (msg->tag != IUCV_SKB_CB(list_skb)->tag) {
 				this = list_skb;
 				break;
 			}
@@ -2091,6 +2091,7 @@ static int afiucv_hs_callback_rx(struct sock *sk, struct sk_buff *skb)
 	skb_pull(skb, sizeof(struct af_iucv_trans_hdr));
 	skb_reset_transport_header(skb);
 	skb_reset_network_header(skb);
+	IUCV_SKB_CB(skb)->offset = 0;
 	spin_lock(&iucv->message_q.lock);
 	if (skb_queue_empty(&iucv->backlog_skb_q)) {
 		if (sock_queue_rcv_skb(sk, skb)) {
@@ -2195,8 +2196,7 @@ static int afiucv_hs_rcv(struct sk_buff *skb, struct net_device *dev,
 		/* fall through and receive zero length data */
 	case 0:
 		/* plain data frame */
-		memcpy(CB_TRGCLS(skb), &trans_hdr->iucv_hdr.class,
-		       CB_TRGCLS_LEN);
+		IUCV_SKB_CB(skb)->class = trans_hdr->iucv_hdr.class;
 		err = afiucv_hs_callback_rx(sk, skb);
 		break;
 	default:
diff --git a/net/key/af_key.c b/net/key/af_key.c
index 8555f331ea6..5b1e5af2571 100644
--- a/net/key/af_key.c
+++ b/net/key/af_key.c
@@ -2693,6 +2693,7 @@ static int key_notify_policy_flush(const struct km_event *c)
 	hdr->sadb_msg_pid = c->portid;
 	hdr->sadb_msg_version = PF_KEY_V2;
 	hdr->sadb_msg_errno = (uint8_t) 0;
+	hdr->sadb_msg_satype = SADB_SATYPE_UNSPEC;
 	hdr->sadb_msg_len = (sizeof(struct sadb_msg) / sizeof(uint64_t));
 	pfkey_broadcast(skb_out, GFP_ATOMIC, BROADCAST_ALL, NULL, c->net);
 	return 0;
diff --git a/net/l2tp/l2tp_core.c b/net/l2tp/l2tp_core.c
index d36875f3427..8aecf5df665 100644
--- a/net/l2tp/l2tp_core.c
+++ b/net/l2tp/l2tp_core.c
@@ -114,7 +114,6 @@ struct l2tp_net {
 
 static void l2tp_session_set_header_len(struct l2tp_session *session, int version);
 static void l2tp_tunnel_free(struct l2tp_tunnel *tunnel);
-static void l2tp_tunnel_closeall(struct l2tp_tunnel *tunnel);
 
 static inline struct l2tp_net *l2tp_pernet(struct net *net)
 {
@@ -192,6 +191,7 @@ struct sock *l2tp_tunnel_sock_lookup(struct l2tp_tunnel *tunnel)
 	} else {
 		/* Socket is owned by kernelspace */
 		sk = tunnel->sock;
+		sock_hold(sk);
 	}
 
 out:
@@ -210,6 +210,7 @@ void l2tp_tunnel_sock_put(struct sock *sk)
 		}
 		sock_put(sk);
 	}
+	sock_put(sk);
 }
 EXPORT_SYMBOL_GPL(l2tp_tunnel_sock_put);
 
@@ -373,10 +374,8 @@ static void l2tp_recv_queue_skb(struct l2tp_session *session, struct sk_buff *sk
 	struct sk_buff *skbp;
 	struct sk_buff *tmp;
 	u32 ns = L2TP_SKB_CB(skb)->ns;
-	struct l2tp_stats *sstats;
 
 	spin_lock_bh(&session->reorder_q.lock);
-	sstats = &session->stats;
 	skb_queue_walk_safe(&session->reorder_q, skbp, tmp) {
 		if (L2TP_SKB_CB(skbp)->ns > ns) {
 			__skb_queue_before(&session->reorder_q, skbp, skb);
@@ -384,9 +383,7 @@ static void l2tp_recv_queue_skb(struct l2tp_session *session, struct sk_buff *sk
 				 "%s: pkt %hu, inserted before %hu, reorder_q len=%d\n",
 				 session->name, ns, L2TP_SKB_CB(skbp)->ns,
 				 skb_queue_len(&session->reorder_q));
-			u64_stats_update_begin(&sstats->syncp);
-			sstats->rx_oos_packets++;
-			u64_stats_update_end(&sstats->syncp);
+			atomic_long_inc(&session->stats.rx_oos_packets);
 			goto out;
 		}
 	}
@@ -403,23 +400,16 @@ static void l2tp_recv_dequeue_skb(struct l2tp_session *session, struct sk_buff *
 {
 	struct l2tp_tunnel *tunnel = session->tunnel;
 	int length = L2TP_SKB_CB(skb)->length;
-	struct l2tp_stats *tstats, *sstats;
 
 	/* We're about to requeue the skb, so return resources
 	 * to its current owner (a socket receive buffer).
 	 */
 	skb_orphan(skb);
 
-	tstats = &tunnel->stats;
-	u64_stats_update_begin(&tstats->syncp);
-	sstats = &session->stats;
-	u64_stats_update_begin(&sstats->syncp);
-	tstats->rx_packets++;
-	tstats->rx_bytes += length;
-	sstats->rx_packets++;
-	sstats->rx_bytes += length;
-	u64_stats_update_end(&tstats->syncp);
-	u64_stats_update_end(&sstats->syncp);
+	atomic_long_inc(&tunnel->stats.rx_packets);
+	atomic_long_add(length, &tunnel->stats.rx_bytes);
+	atomic_long_inc(&session->stats.rx_packets);
+	atomic_long_add(length, &session->stats.rx_bytes);
 
 	if (L2TP_SKB_CB(skb)->has_seq) {
 		/* Bump our Nr */
@@ -450,7 +440,6 @@ static void l2tp_recv_dequeue(struct l2tp_session *session)
 {
 	struct sk_buff *skb;
 	struct sk_buff *tmp;
-	struct l2tp_stats *sstats;
 
 	/* If the pkt at the head of the queue has the nr that we
 	 * expect to send up next, dequeue it and any other
@@ -458,13 +447,10 @@ static void l2tp_recv_dequeue(struct l2tp_session *session)
 	 */
 start:
 	spin_lock_bh(&session->reorder_q.lock);
-	sstats = &session->stats;
 	skb_queue_walk_safe(&session->reorder_q, skb, tmp) {
 		if (time_after(jiffies, L2TP_SKB_CB(skb)->expires)) {
-			u64_stats_update_begin(&sstats->syncp);
-			sstats->rx_seq_discards++;
-			sstats->rx_errors++;
-			u64_stats_update_end(&sstats->syncp);
+			atomic_long_inc(&session->stats.rx_seq_discards);
+			atomic_long_inc(&session->stats.rx_errors);
 			l2tp_dbg(session, L2TP_MSG_SEQ,
 				 "%s: oos pkt %u len %d discarded (too old), waiting for %u, reorder_q_len=%d\n",
 				 session->name, L2TP_SKB_CB(skb)->ns,
@@ -623,7 +609,6 @@ void l2tp_recv_common(struct l2tp_session *session, struct sk_buff *skb,
 	struct l2tp_tunnel *tunnel = session->tunnel;
 	int offset;
 	u32 ns, nr;
-	struct l2tp_stats *sstats = &session->stats;
 
 	/* The ref count is increased since we now hold a pointer to
 	 * the session. Take care to decrement the refcnt when exiting
@@ -640,9 +625,7 @@ void l2tp_recv_common(struct l2tp_session *session, struct sk_buff *skb,
 				  "%s: cookie mismatch (%u/%u). Discarding.\n",
 				  tunnel->name, tunnel->tunnel_id,
 				  session->session_id);
-			u64_stats_update_begin(&sstats->syncp);
-			sstats->rx_cookie_discards++;
-			u64_stats_update_end(&sstats->syncp);
+			atomic_long_inc(&session->stats.rx_cookie_discards);
 			goto discard;
 		}
 		ptr += session->peer_cookie_len;
@@ -711,9 +694,7 @@ void l2tp_recv_common(struct l2tp_session *session, struct sk_buff *skb,
 			l2tp_warn(session, L2TP_MSG_SEQ,
 				  "%s: recv data has no seq numbers when required. Discarding.\n",
 				  session->name);
-			u64_stats_update_begin(&sstats->syncp);
-			sstats->rx_seq_discards++;
-			u64_stats_update_end(&sstats->syncp);
+			atomic_long_inc(&session->stats.rx_seq_discards);
 			goto discard;
 		}
 
@@ -732,9 +713,7 @@ void l2tp_recv_common(struct l2tp_session *session, struct sk_buff *skb,
 			l2tp_warn(session, L2TP_MSG_SEQ,
 				  "%s: recv data has no seq numbers when required. Discarding.\n",
 				  session->name);
-			u64_stats_update_begin(&sstats->syncp);
-			sstats->rx_seq_discards++;
-			u64_stats_update_end(&sstats->syncp);
+			atomic_long_inc(&session->stats.rx_seq_discards);
 			goto discard;
 		}
 	}
@@ -788,9 +767,7 @@ void l2tp_recv_common(struct l2tp_session *session, struct sk_buff *skb,
 			 * packets
 			 */
 			if (L2TP_SKB_CB(skb)->ns != session->nr) {
-				u64_stats_update_begin(&sstats->syncp);
-				sstats->rx_seq_discards++;
-				u64_stats_update_end(&sstats->syncp);
+				atomic_long_inc(&session->stats.rx_seq_discards);
 				l2tp_dbg(session, L2TP_MSG_SEQ,
 					 "%s: oos pkt %u len %d discarded, waiting for %u, reorder_q_len=%d\n",
 					 session->name, L2TP_SKB_CB(skb)->ns,
@@ -816,9 +793,7 @@ void l2tp_recv_common(struct l2tp_session *session, struct sk_buff *skb,
 	return;
 
 discard:
-	u64_stats_update_begin(&sstats->syncp);
-	sstats->rx_errors++;
-	u64_stats_update_end(&sstats->syncp);
+	atomic_long_inc(&session->stats.rx_errors);
 	kfree_skb(skb);
 
 	if (session->deref)
@@ -828,6 +803,23 @@ discard:
 }
 EXPORT_SYMBOL(l2tp_recv_common);
 
+/* Drop skbs from the session's reorder_q
+ */
+int l2tp_session_queue_purge(struct l2tp_session *session)
+{
+	struct sk_buff *skb = NULL;
+	BUG_ON(!session);
+	BUG_ON(session->magic != L2TP_SESSION_MAGIC);
+	while ((skb = skb_dequeue(&session->reorder_q))) {
+		atomic_long_inc(&session->stats.rx_errors);
+		kfree_skb(skb);
+		if (session->deref)
+			(*session->deref)(session);
+	}
+	return 0;
+}
+EXPORT_SYMBOL_GPL(l2tp_session_queue_purge);
+
 /* Internal UDP receive frame. Do the real work of receiving an L2TP data frame
  * here. The skb is not on a list when we get here.
  * Returns 0 if the packet was a data packet and was successfully passed on.
@@ -843,7 +835,6 @@ static int l2tp_udp_recv_core(struct l2tp_tunnel *tunnel, struct sk_buff *skb,
 	u32 tunnel_id, session_id;
 	u16 version;
 	int length;
-	struct l2tp_stats *tstats;
 
 	if (tunnel->sock && l2tp_verify_udp_checksum(tunnel->sock, skb))
 		goto discard_bad_csum;
@@ -932,10 +923,7 @@ static int l2tp_udp_recv_core(struct l2tp_tunnel *tunnel, struct sk_buff *skb,
 discard_bad_csum:
 	LIMIT_NETDEBUG("%s: UDP: bad checksum\n", tunnel->name);
 	UDP_INC_STATS_USER(tunnel->l2tp_net, UDP_MIB_INERRORS, 0);
-	tstats = &tunnel->stats;
-	u64_stats_update_begin(&tstats->syncp);
-	tstats->rx_errors++;
-	u64_stats_update_end(&tstats->syncp);
+	atomic_long_inc(&tunnel->stats.rx_errors);
 	kfree_skb(skb);
 
 	return 0;
@@ -1062,7 +1050,6 @@ static int l2tp_xmit_core(struct l2tp_session *session, struct sk_buff *skb,
 	struct l2tp_tunnel *tunnel = session->tunnel;
 	unsigned int len = skb->len;
 	int error;
-	struct l2tp_stats *tstats, *sstats;
 
 	/* Debug */
 	if (session->send_seq)
@@ -1091,21 +1078,15 @@ static int l2tp_xmit_core(struct l2tp_session *session, struct sk_buff *skb,
 		error = ip_queue_xmit(skb, fl);
 
 	/* Update stats */
-	tstats = &tunnel->stats;
-	u64_stats_update_begin(&tstats->syncp);
-	sstats = &session->stats;
-	u64_stats_update_begin(&sstats->syncp);
 	if (error >= 0) {
-		tstats->tx_packets++;
-		tstats->tx_bytes += len;
-		sstats->tx_packets++;
-		sstats->tx_bytes += len;
+		atomic_long_inc(&tunnel->stats.tx_packets);
+		atomic_long_add(len, &tunnel->stats.tx_bytes);
+		atomic_long_inc(&session->stats.tx_packets);
+		atomic_long_add(len, &session->stats.tx_bytes);
 	} else {
-		tstats->tx_errors++;
-		sstats->tx_errors++;
+		atomic_long_inc(&tunnel->stats.tx_errors);
+		atomic_long_inc(&session->stats.tx_errors);
 	}
-	u64_stats_update_end(&tstats->syncp);
-	u64_stats_update_end(&sstats->syncp);
 
 	return 0;
 }
@@ -1282,6 +1263,7 @@ static void l2tp_tunnel_destruct(struct sock *sk)
 		/* No longer an encapsulation socket. See net/ipv4/udp.c */
 		(udp_sk(sk))->encap_type = 0;
 		(udp_sk(sk))->encap_rcv = NULL;
+		(udp_sk(sk))->encap_destroy = NULL;
 		break;
 	case L2TP_ENCAPTYPE_IP:
 		break;
@@ -1311,7 +1293,7 @@ end:
 
 /* When the tunnel is closed, all the attached sessions need to go too.
  */
-static void l2tp_tunnel_closeall(struct l2tp_tunnel *tunnel)
+void l2tp_tunnel_closeall(struct l2tp_tunnel *tunnel)
 {
 	int hash;
 	struct hlist_node *walk;
@@ -1334,25 +1316,13 @@ again:
 
 			hlist_del_init(&session->hlist);
 
-			/* Since we should hold the sock lock while
-			 * doing any unbinding, we need to release the
-			 * lock we're holding before taking that lock.
-			 * Hold a reference to the sock so it doesn't
-			 * disappear as we're jumping between locks.
-			 */
 			if (session->ref != NULL)
 				(*session->ref)(session);
 
 			write_unlock_bh(&tunnel->hlist_lock);
 
-			if (tunnel->version != L2TP_HDR_VER_2) {
-				struct l2tp_net *pn = l2tp_pernet(tunnel->l2tp_net);
-
-				spin_lock_bh(&pn->l2tp_session_hlist_lock);
-				hlist_del_init_rcu(&session->global_hlist);
-				spin_unlock_bh(&pn->l2tp_session_hlist_lock);
-				synchronize_rcu();
-			}
+			__l2tp_session_unhash(session);
+			l2tp_session_queue_purge(session);
 
 			if (session->session_close != NULL)
 				(*session->session_close)(session);
@@ -1360,6 +1330,8 @@ again:
 			if (session->deref != NULL)
 				(*session->deref)(session);
 
+			l2tp_session_dec_refcount(session);
+
 			write_lock_bh(&tunnel->hlist_lock);
 
 			/* Now restart from the beginning of this hash
@@ -1372,6 +1344,17 @@ again:
 	}
 	write_unlock_bh(&tunnel->hlist_lock);
 }
+EXPORT_SYMBOL_GPL(l2tp_tunnel_closeall);
+
+/* Tunnel socket destroy hook for UDP encapsulation */
+static void l2tp_udp_encap_destroy(struct sock *sk)
+{
+	struct l2tp_tunnel *tunnel = l2tp_sock_to_tunnel(sk);
+	if (tunnel) {
+		l2tp_tunnel_closeall(tunnel);
+		sock_put(sk);
+	}
+}
 
 /* Really kill the tunnel.
  * Come here only when all sessions have been cleared from the tunnel.
@@ -1397,19 +1380,21 @@ static void l2tp_tunnel_del_work(struct work_struct *work)
 		return;
 
 	sock = sk->sk_socket;
-	BUG_ON(!sock);
 
-	/* If the tunnel socket was created directly by the kernel, use the
-	 * sk_* API to release the socket now.  Otherwise go through the
-	 * inet_* layer to shut the socket down, and let userspace close it.
+	/* If the tunnel socket was created by userspace, then go through the
+	 * inet layer to shut the socket down, and let userspace close it.
+	 * Otherwise, if we created the socket directly within the kernel, use
+	 * the sk API to release it here.
 	 * In either case the tunnel resources are freed in the socket
 	 * destructor when the tunnel socket goes away.
 	 */
-	if (sock->file == NULL) {
-		kernel_sock_shutdown(sock, SHUT_RDWR);
-		sk_release_kernel(sk);
+	if (tunnel->fd >= 0) {
+		if (sock)
+			inet_shutdown(sock, 2);
 	} else {
-		inet_shutdown(sock, 2);
+		if (sock)
+			kernel_sock_shutdown(sock, SHUT_RDWR);
+		sk_release_kernel(sk);
 	}
 
 	l2tp_tunnel_sock_put(sk);
@@ -1668,6 +1653,7 @@ int l2tp_tunnel_create(struct net *net, int fd, int version, u32 tunnel_id, u32
 		/* Mark socket as an encapsulation socket. See net/ipv4/udp.c */
 		udp_sk(sk)->encap_type = UDP_ENCAP_L2TPINUDP;
 		udp_sk(sk)->encap_rcv = l2tp_udp_encap_recv;
+		udp_sk(sk)->encap_destroy = l2tp_udp_encap_destroy;
 #if IS_ENABLED(CONFIG_IPV6)
 		if (sk->sk_family == PF_INET6)
 			udpv6_encap_enable();
@@ -1723,6 +1709,7 @@ EXPORT_SYMBOL_GPL(l2tp_tunnel_create);
  */
 int l2tp_tunnel_delete(struct l2tp_tunnel *tunnel)
 {
+	l2tp_tunnel_closeall(tunnel);
 	return (false == queue_work(l2tp_wq, &tunnel->del_work));
 }
 EXPORT_SYMBOL_GPL(l2tp_tunnel_delete);
@@ -1731,62 +1718,71 @@ EXPORT_SYMBOL_GPL(l2tp_tunnel_delete);
  */
 void l2tp_session_free(struct l2tp_session *session)
 {
-	struct l2tp_tunnel *tunnel;
+	struct l2tp_tunnel *tunnel = session->tunnel;
 
 	BUG_ON(atomic_read(&session->ref_count) != 0);
 
-	tunnel = session->tunnel;
-	if (tunnel != NULL) {
+	if (tunnel) {
 		BUG_ON(tunnel->magic != L2TP_TUNNEL_MAGIC);
+		if (session->session_id != 0)
+			atomic_dec(&l2tp_session_count);
+		sock_put(tunnel->sock);
+		session->tunnel = NULL;
+		l2tp_tunnel_dec_refcount(tunnel);
+	}
+
+	kfree(session);
 
-		/* Delete the session from the hash */
+	return;
+}
+EXPORT_SYMBOL_GPL(l2tp_session_free);
+
+/* Remove an l2tp session from l2tp_core's hash lists.
+ * Provides a tidyup interface for pseudowire code which can't just route all
+ * shutdown via. l2tp_session_delete and a pseudowire-specific session_close
+ * callback.
+ */
+void __l2tp_session_unhash(struct l2tp_session *session)
+{
+	struct l2tp_tunnel *tunnel = session->tunnel;
+
+	/* Remove the session from core hashes */
+	if (tunnel) {
+		/* Remove from the per-tunnel hash */
 		write_lock_bh(&tunnel->hlist_lock);
 		hlist_del_init(&session->hlist);
 		write_unlock_bh(&tunnel->hlist_lock);
 
-		/* Unlink from the global hash if not L2TPv2 */
+		/* For L2TPv3 we have a per-net hash: remove from there, too */
 		if (tunnel->version != L2TP_HDR_VER_2) {
 			struct l2tp_net *pn = l2tp_pernet(tunnel->l2tp_net);
-
 			spin_lock_bh(&pn->l2tp_session_hlist_lock);
 			hlist_del_init_rcu(&session->global_hlist);
 			spin_unlock_bh(&pn->l2tp_session_hlist_lock);
 			synchronize_rcu();
 		}
-
-		if (session->session_id != 0)
-			atomic_dec(&l2tp_session_count);
-
-		sock_put(tunnel->sock);
-
-		/* This will delete the tunnel context if this
-		 * is the last session on the tunnel.
-		 */
-		session->tunnel = NULL;
-		l2tp_tunnel_dec_refcount(tunnel);
 	}
-
-	kfree(session);
-
-	return;
 }
-EXPORT_SYMBOL_GPL(l2tp_session_free);
+EXPORT_SYMBOL_GPL(__l2tp_session_unhash);
 
 /* This function is used by the netlink SESSION_DELETE command and by
    pseudowire modules.
  */
 int l2tp_session_delete(struct l2tp_session *session)
 {
+	if (session->ref)
+		(*session->ref)(session);
+	__l2tp_session_unhash(session);
+	l2tp_session_queue_purge(session);
 	if (session->session_close != NULL)
 		(*session->session_close)(session);
-
+	if (session->deref)
+		(*session->ref)(session);
 	l2tp_session_dec_refcount(session);
-
 	return 0;
 }
 EXPORT_SYMBOL_GPL(l2tp_session_delete);
 
-
 /* We come here whenever a session's send_seq, cookie_len or
  * l2specific_len parameters are set.
  */
diff --git a/net/l2tp/l2tp_core.h b/net/l2tp/l2tp_core.h
index 8eb8f1d47f3..485a490fd99 100644
--- a/net/l2tp/l2tp_core.h
+++ b/net/l2tp/l2tp_core.h
@@ -36,16 +36,15 @@ enum {
 struct sk_buff;
 
 struct l2tp_stats {
-	u64			tx_packets;
-	u64			tx_bytes;
-	u64			tx_errors;
-	u64			rx_packets;
-	u64			rx_bytes;
-	u64			rx_seq_discards;
-	u64			rx_oos_packets;
-	u64			rx_errors;
-	u64			rx_cookie_discards;
-	struct u64_stats_sync	syncp;
+	atomic_long_t		tx_packets;
+	atomic_long_t		tx_bytes;
+	atomic_long_t		tx_errors;
+	atomic_long_t		rx_packets;
+	atomic_long_t		rx_bytes;
+	atomic_long_t		rx_seq_discards;
+	atomic_long_t		rx_oos_packets;
+	atomic_long_t		rx_errors;
+	atomic_long_t		rx_cookie_discards;
 };
 
 struct l2tp_tunnel;
@@ -240,11 +239,14 @@ extern struct l2tp_tunnel *l2tp_tunnel_find(struct net *net, u32 tunnel_id);
 extern struct l2tp_tunnel *l2tp_tunnel_find_nth(struct net *net, int nth);
 
 extern int l2tp_tunnel_create(struct net *net, int fd, int version, u32 tunnel_id, u32 peer_tunnel_id, struct l2tp_tunnel_cfg *cfg, struct l2tp_tunnel **tunnelp);
+extern void l2tp_tunnel_closeall(struct l2tp_tunnel *tunnel);
 extern int l2tp_tunnel_delete(struct l2tp_tunnel *tunnel);
 extern struct l2tp_session *l2tp_session_create(int priv_size, struct l2tp_tunnel *tunnel, u32 session_id, u32 peer_session_id, struct l2tp_session_cfg *cfg);
+extern void __l2tp_session_unhash(struct l2tp_session *session);
 extern int l2tp_session_delete(struct l2tp_session *session);
 extern void l2tp_session_free(struct l2tp_session *session);
 extern void l2tp_recv_common(struct l2tp_session *session, struct sk_buff *skb, unsigned char *ptr, unsigned char *optr, u16 hdrflags, int length, int (*payload_hook)(struct sk_buff *skb));
+extern int l2tp_session_queue_purge(struct l2tp_session *session);
 extern int l2tp_udp_encap_recv(struct sock *sk, struct sk_buff *skb);
 
 extern int l2tp_xmit_skb(struct l2tp_session *session, struct sk_buff *skb, int hdr_len);
diff --git a/net/l2tp/l2tp_debugfs.c b/net/l2tp/l2tp_debugfs.c
index c3813bc8455..072d7202e18 100644
--- a/net/l2tp/l2tp_debugfs.c
+++ b/net/l2tp/l2tp_debugfs.c
@@ -146,14 +146,14 @@ static void l2tp_dfs_seq_tunnel_show(struct seq_file *m, void *v)
 		   tunnel->sock ? atomic_read(&tunnel->sock->sk_refcnt) : 0,
 		   atomic_read(&tunnel->ref_count));
 
-	seq_printf(m, " %08x rx %llu/%llu/%llu rx %llu/%llu/%llu\n",
+	seq_printf(m, " %08x rx %ld/%ld/%ld rx %ld/%ld/%ld\n",
 		   tunnel->debug,
-		   (unsigned long long)tunnel->stats.tx_packets,
-		   (unsigned long long)tunnel->stats.tx_bytes,
-		   (unsigned long long)tunnel->stats.tx_errors,
-		   (unsigned long long)tunnel->stats.rx_packets,
-		   (unsigned long long)tunnel->stats.rx_bytes,
-		   (unsigned long long)tunnel->stats.rx_errors);
+		   atomic_long_read(&tunnel->stats.tx_packets),
+		   atomic_long_read(&tunnel->stats.tx_bytes),
+		   atomic_long_read(&tunnel->stats.tx_errors),
+		   atomic_long_read(&tunnel->stats.rx_packets),
+		   atomic_long_read(&tunnel->stats.rx_bytes),
+		   atomic_long_read(&tunnel->stats.rx_errors));
 
 	if (tunnel->show != NULL)
 		tunnel->show(m, tunnel);
@@ -203,14 +203,14 @@ static void l2tp_dfs_seq_session_show(struct seq_file *m, void *v)
 		seq_printf(m, "\n");
 	}
 
-	seq_printf(m, "   %hu/%hu tx %llu/%llu/%llu rx %llu/%llu/%llu\n",
+	seq_printf(m, "   %hu/%hu tx %ld/%ld/%ld rx %ld/%ld/%ld\n",
 		   session->nr, session->ns,
-		   (unsigned long long)session->stats.tx_packets,
-		   (unsigned long long)session->stats.tx_bytes,
-		   (unsigned long long)session->stats.tx_errors,
-		   (unsigned long long)session->stats.rx_packets,
-		   (unsigned long long)session->stats.rx_bytes,
-		   (unsigned long long)session->stats.rx_errors);
+		   atomic_long_read(&session->stats.tx_packets),
+		   atomic_long_read(&session->stats.tx_bytes),
+		   atomic_long_read(&session->stats.tx_errors),
+		   atomic_long_read(&session->stats.rx_packets),
+		   atomic_long_read(&session->stats.rx_bytes),
+		   atomic_long_read(&session->stats.rx_errors));
 
 	if (session->show != NULL)
 		session->show(m, session);
diff --git a/net/l2tp/l2tp_ip.c b/net/l2tp/l2tp_ip.c
index 7f41b705126..571db8dd229 100644
--- a/net/l2tp/l2tp_ip.c
+++ b/net/l2tp/l2tp_ip.c
@@ -228,10 +228,16 @@ static void l2tp_ip_close(struct sock *sk, long timeout)
 static void l2tp_ip_destroy_sock(struct sock *sk)
 {
 	struct sk_buff *skb;
+	struct l2tp_tunnel *tunnel = l2tp_sock_to_tunnel(sk);
 
 	while ((skb = __skb_dequeue_tail(&sk->sk_write_queue)) != NULL)
 		kfree_skb(skb);
 
+	if (tunnel) {
+		l2tp_tunnel_closeall(tunnel);
+		sock_put(sk);
+	}
+
 	sk_refcnt_debug_dec(sk);
 }
 
diff --git a/net/l2tp/l2tp_ip6.c b/net/l2tp/l2tp_ip6.c
index 41f2f8126eb..b8a6039314e 100644
--- a/net/l2tp/l2tp_ip6.c
+++ b/net/l2tp/l2tp_ip6.c
@@ -241,10 +241,17 @@ static void l2tp_ip6_close(struct sock *sk, long timeout)
 
 static void l2tp_ip6_destroy_sock(struct sock *sk)
 {
+	struct l2tp_tunnel *tunnel = l2tp_sock_to_tunnel(sk);
+
 	lock_sock(sk);
 	ip6_flush_pending_frames(sk);
 	release_sock(sk);
 
+	if (tunnel) {
+		l2tp_tunnel_closeall(tunnel);
+		sock_put(sk);
+	}
+
 	inet6_destroy_sock(sk);
 }
 
@@ -683,6 +690,7 @@ static int l2tp_ip6_recvmsg(struct kiocb *iocb, struct sock *sk,
 		lsa->l2tp_addr = ipv6_hdr(skb)->saddr;
 		lsa->l2tp_flowinfo = 0;
 		lsa->l2tp_scope_id = 0;
+		lsa->l2tp_conn_id = 0;
 		if (ipv6_addr_type(&lsa->l2tp_addr) & IPV6_ADDR_LINKLOCAL)
 			lsa->l2tp_scope_id = IP6CB(skb)->iif;
 	}
diff --git a/net/l2tp/l2tp_netlink.c b/net/l2tp/l2tp_netlink.c
index c1bab22db85..0825ff26e11 100644
--- a/net/l2tp/l2tp_netlink.c
+++ b/net/l2tp/l2tp_netlink.c
@@ -246,8 +246,6 @@ static int l2tp_nl_tunnel_send(struct sk_buff *skb, u32 portid, u32 seq, int fla
 #if IS_ENABLED(CONFIG_IPV6)
 	struct ipv6_pinfo *np = NULL;
 #endif
-	struct l2tp_stats stats;
-	unsigned int start;
 
 	hdr = genlmsg_put(skb, portid, seq, &l2tp_nl_family, flags,
 			  L2TP_CMD_TUNNEL_GET);
@@ -265,28 +263,22 @@ static int l2tp_nl_tunnel_send(struct sk_buff *skb, u32 portid, u32 seq, int fla
 	if (nest == NULL)
 		goto nla_put_failure;
 
-	do {
-		start = u64_stats_fetch_begin(&tunnel->stats.syncp);
-		stats.tx_packets = tunnel->stats.tx_packets;
-		stats.tx_bytes = tunnel->stats.tx_bytes;
-		stats.tx_errors = tunnel->stats.tx_errors;
-		stats.rx_packets = tunnel->stats.rx_packets;
-		stats.rx_bytes = tunnel->stats.rx_bytes;
-		stats.rx_errors = tunnel->stats.rx_errors;
-		stats.rx_seq_discards = tunnel->stats.rx_seq_discards;
-		stats.rx_oos_packets = tunnel->stats.rx_oos_packets;
-	} while (u64_stats_fetch_retry(&tunnel->stats.syncp, start));
-
-	if (nla_put_u64(skb, L2TP_ATTR_TX_PACKETS, stats.tx_packets) ||
-	    nla_put_u64(skb, L2TP_ATTR_TX_BYTES, stats.tx_bytes) ||
-	    nla_put_u64(skb, L2TP_ATTR_TX_ERRORS, stats.tx_errors) ||
-	    nla_put_u64(skb, L2TP_ATTR_RX_PACKETS, stats.rx_packets) ||
-	    nla_put_u64(skb, L2TP_ATTR_RX_BYTES, stats.rx_bytes) ||
+	if (nla_put_u64(skb, L2TP_ATTR_TX_PACKETS,
+		    atomic_long_read(&tunnel->stats.tx_packets)) ||
+	    nla_put_u64(skb, L2TP_ATTR_TX_BYTES,
+		    atomic_long_read(&tunnel->stats.tx_bytes)) ||
+	    nla_put_u64(skb, L2TP_ATTR_TX_ERRORS,
+		    atomic_long_read(&tunnel->stats.tx_errors)) ||
+	    nla_put_u64(skb, L2TP_ATTR_RX_PACKETS,
+		    atomic_long_read(&tunnel->stats.rx_packets)) ||
+	    nla_put_u64(skb, L2TP_ATTR_RX_BYTES,
+		    atomic_long_read(&tunnel->stats.rx_bytes)) ||
 	    nla_put_u64(skb, L2TP_ATTR_RX_SEQ_DISCARDS,
-			stats.rx_seq_discards) ||
+		    atomic_long_read(&tunnel->stats.rx_seq_discards)) ||
 	    nla_put_u64(skb, L2TP_ATTR_RX_OOS_PACKETS,
-			stats.rx_oos_packets) ||
-	    nla_put_u64(skb, L2TP_ATTR_RX_ERRORS, stats.rx_errors))
+		    atomic_long_read(&tunnel->stats.rx_oos_packets)) ||
+	    nla_put_u64(skb, L2TP_ATTR_RX_ERRORS,
+		    atomic_long_read(&tunnel->stats.rx_errors)))
 		goto nla_put_failure;
 	nla_nest_end(skb, nest);
 
@@ -612,8 +604,6 @@ static int l2tp_nl_session_send(struct sk_buff *skb, u32 portid, u32 seq, int fl
 	struct nlattr *nest;
 	struct l2tp_tunnel *tunnel = session->tunnel;
 	struct sock *sk = NULL;
-	struct l2tp_stats stats;
-	unsigned int start;
 
 	sk = tunnel->sock;
 
@@ -656,28 +646,22 @@ static int l2tp_nl_session_send(struct sk_buff *skb, u32 portid, u32 seq, int fl
 	if (nest == NULL)
 		goto nla_put_failure;
 
-	do {
-		start = u64_stats_fetch_begin(&session->stats.syncp);
-		stats.tx_packets = session->stats.tx_packets;
-		stats.tx_bytes = session->stats.tx_bytes;
-		stats.tx_errors = session->stats.tx_errors;
-		stats.rx_packets = session->stats.rx_packets;
-		stats.rx_bytes = session->stats.rx_bytes;
-		stats.rx_errors = session->stats.rx_errors;
-		stats.rx_seq_discards = session->stats.rx_seq_discards;
-		stats.rx_oos_packets = session->stats.rx_oos_packets;
-	} while (u64_stats_fetch_retry(&session->stats.syncp, start));
-
-	if (nla_put_u64(skb, L2TP_ATTR_TX_PACKETS, stats.tx_packets) ||
-	    nla_put_u64(skb, L2TP_ATTR_TX_BYTES, stats.tx_bytes) ||
-	    nla_put_u64(skb, L2TP_ATTR_TX_ERRORS, stats.tx_errors) ||
-	    nla_put_u64(skb, L2TP_ATTR_RX_PACKETS, stats.rx_packets) ||
-	    nla_put_u64(skb, L2TP_ATTR_RX_BYTES, stats.rx_bytes) ||
+	if (nla_put_u64(skb, L2TP_ATTR_TX_PACKETS,
+		atomic_long_read(&session->stats.tx_packets)) ||
+	    nla_put_u64(skb, L2TP_ATTR_TX_BYTES,
+		atomic_long_read(&session->stats.tx_bytes)) ||
+	    nla_put_u64(skb, L2TP_ATTR_TX_ERRORS,
+		atomic_long_read(&session->stats.tx_errors)) ||
+	    nla_put_u64(skb, L2TP_ATTR_RX_PACKETS,
+		atomic_long_read(&session->stats.rx_packets)) ||
+	    nla_put_u64(skb, L2TP_ATTR_RX_BYTES,
+		atomic_long_read(&session->stats.rx_bytes)) ||
 	    nla_put_u64(skb, L2TP_ATTR_RX_SEQ_DISCARDS,
-			stats.rx_seq_discards) ||
+		atomic_long_read(&session->stats.rx_seq_discards)) ||
 	    nla_put_u64(skb, L2TP_ATTR_RX_OOS_PACKETS,
-			stats.rx_oos_packets) ||
-	    nla_put_u64(skb, L2TP_ATTR_RX_ERRORS, stats.rx_errors))
+		atomic_long_read(&session->stats.rx_oos_packets)) ||
+	    nla_put_u64(skb, L2TP_ATTR_RX_ERRORS,
+		atomic_long_read(&session->stats.rx_errors)))
 		goto nla_put_failure;
 	nla_nest_end(skb, nest);
 
diff --git a/net/l2tp/l2tp_ppp.c b/net/l2tp/l2tp_ppp.c
index 6a53371dba1..637a341c1e2 100644
--- a/net/l2tp/l2tp_ppp.c
+++ b/net/l2tp/l2tp_ppp.c
@@ -97,6 +97,7 @@
 #include <net/ip.h>
 #include <net/udp.h>
 #include <net/xfrm.h>
+#include <net/inet_common.h>
 
 #include <asm/byteorder.h>
 #include <linux/atomic.h>
@@ -259,7 +260,7 @@ static void pppol2tp_recv(struct l2tp_session *session, struct sk_buff *skb, int
 			  session->name);
 
 		/* Not bound. Nothing we can do, so discard. */
-		session->stats.rx_errors++;
+		atomic_long_inc(&session->stats.rx_errors);
 		kfree_skb(skb);
 	}
 
@@ -447,34 +448,16 @@ static void pppol2tp_session_close(struct l2tp_session *session)
 {
 	struct pppol2tp_session *ps = l2tp_session_priv(session);
 	struct sock *sk = ps->sock;
-	struct sk_buff *skb;
+	struct socket *sock = sk->sk_socket;
 
 	BUG_ON(session->magic != L2TP_SESSION_MAGIC);
 
-	if (session->session_id == 0)
-		goto out;
-
-	if (sk != NULL) {
-		lock_sock(sk);
-
-		if (sk->sk_state & (PPPOX_CONNECTED | PPPOX_BOUND)) {
-			pppox_unbind_sock(sk);
-			sk->sk_state = PPPOX_DEAD;
-			sk->sk_state_change(sk);
-		}
-
-		/* Purge any queued data */
-		skb_queue_purge(&sk->sk_receive_queue);
-		skb_queue_purge(&sk->sk_write_queue);
-		while ((skb = skb_dequeue(&session->reorder_q))) {
-			kfree_skb(skb);
-			sock_put(sk);
-		}
 
-		release_sock(sk);
+	if (sock) {
+		inet_shutdown(sock, 2);
+		/* Don't let the session go away before our socket does */
+		l2tp_session_inc_refcount(session);
 	}
-
-out:
 	return;
 }
 
@@ -483,19 +466,12 @@ out:
  */
 static void pppol2tp_session_destruct(struct sock *sk)
 {
-	struct l2tp_session *session;
-
-	if (sk->sk_user_data != NULL) {
-		session = sk->sk_user_data;
-		if (session == NULL)
-			goto out;
-
+	struct l2tp_session *session = sk->sk_user_data;
+	if (session) {
 		sk->sk_user_data = NULL;
 		BUG_ON(session->magic != L2TP_SESSION_MAGIC);
 		l2tp_session_dec_refcount(session);
 	}
-
-out:
 	return;
 }
 
@@ -525,16 +501,13 @@ static int pppol2tp_release(struct socket *sock)
 	session = pppol2tp_sock_to_session(sk);
 
 	/* Purge any queued data */
-	skb_queue_purge(&sk->sk_receive_queue);
-	skb_queue_purge(&sk->sk_write_queue);
 	if (session != NULL) {
-		struct sk_buff *skb;
-		while ((skb = skb_dequeue(&session->reorder_q))) {
-			kfree_skb(skb);
-			sock_put(sk);
-		}
+		__l2tp_session_unhash(session);
+		l2tp_session_queue_purge(session);
 		sock_put(sk);
 	}
+	skb_queue_purge(&sk->sk_receive_queue);
+	skb_queue_purge(&sk->sk_write_queue);
 
 	release_sock(sk);
 
@@ -880,18 +853,6 @@ out:
 	return error;
 }
 
-/* Called when deleting sessions via the netlink interface.
- */
-static int pppol2tp_session_delete(struct l2tp_session *session)
-{
-	struct pppol2tp_session *ps = l2tp_session_priv(session);
-
-	if (ps->sock == NULL)
-		l2tp_session_dec_refcount(session);
-
-	return 0;
-}
-
 #endif /* CONFIG_L2TP_V3 */
 
 /* getname() support.
@@ -1025,14 +986,14 @@ end:
 static void pppol2tp_copy_stats(struct pppol2tp_ioc_stats *dest,
 				struct l2tp_stats *stats)
 {
-	dest->tx_packets = stats->tx_packets;
-	dest->tx_bytes = stats->tx_bytes;
-	dest->tx_errors = stats->tx_errors;
-	dest->rx_packets = stats->rx_packets;
-	dest->rx_bytes = stats->rx_bytes;
-	dest->rx_seq_discards = stats->rx_seq_discards;
-	dest->rx_oos_packets = stats->rx_oos_packets;
-	dest->rx_errors = stats->rx_errors;
+	dest->tx_packets = atomic_long_read(&stats->tx_packets);
+	dest->tx_bytes = atomic_long_read(&stats->tx_bytes);
+	dest->tx_errors = atomic_long_read(&stats->tx_errors);
+	dest->rx_packets = atomic_long_read(&stats->rx_packets);
+	dest->rx_bytes = atomic_long_read(&stats->rx_bytes);
+	dest->rx_seq_discards = atomic_long_read(&stats->rx_seq_discards);
+	dest->rx_oos_packets = atomic_long_read(&stats->rx_oos_packets);
+	dest->rx_errors = atomic_long_read(&stats->rx_errors);
 }
 
 /* Session ioctl helper.
@@ -1666,14 +1627,14 @@ static void pppol2tp_seq_tunnel_show(struct seq_file *m, void *v)
 		   tunnel->name,
 		   (tunnel == tunnel->sock->sk_user_data) ? 'Y' : 'N',
 		   atomic_read(&tunnel->ref_count) - 1);
-	seq_printf(m, " %08x %llu/%llu/%llu %llu/%llu/%llu\n",
+	seq_printf(m, " %08x %ld/%ld/%ld %ld/%ld/%ld\n",
 		   tunnel->debug,
-		   (unsigned long long)tunnel->stats.tx_packets,
-		   (unsigned long long)tunnel->stats.tx_bytes,
-		   (unsigned long long)tunnel->stats.tx_errors,
-		   (unsigned long long)tunnel->stats.rx_packets,
-		   (unsigned long long)tunnel->stats.rx_bytes,
-		   (unsigned long long)tunnel->stats.rx_errors);
+		   atomic_long_read(&tunnel->stats.tx_packets),
+		   atomic_long_read(&tunnel->stats.tx_bytes),
+		   atomic_long_read(&tunnel->stats.tx_errors),
+		   atomic_long_read(&tunnel->stats.rx_packets),
+		   atomic_long_read(&tunnel->stats.rx_bytes),
+		   atomic_long_read(&tunnel->stats.rx_errors));
 }
 
 static void pppol2tp_seq_session_show(struct seq_file *m, void *v)
@@ -1708,14 +1669,14 @@ static void pppol2tp_seq_session_show(struct seq_file *m, void *v)
 		   session->lns_mode ? "LNS" : "LAC",
 		   session->debug,
 		   jiffies_to_msecs(session->reorder_timeout));
-	seq_printf(m, "   %hu/%hu %llu/%llu/%llu %llu/%llu/%llu\n",
+	seq_printf(m, "   %hu/%hu %ld/%ld/%ld %ld/%ld/%ld\n",
 		   session->nr, session->ns,
-		   (unsigned long long)session->stats.tx_packets,
-		   (unsigned long long)session->stats.tx_bytes,
-		   (unsigned long long)session->stats.tx_errors,
-		   (unsigned long long)session->stats.rx_packets,
-		   (unsigned long long)session->stats.rx_bytes,
-		   (unsigned long long)session->stats.rx_errors);
+		   atomic_long_read(&session->stats.tx_packets),
+		   atomic_long_read(&session->stats.tx_bytes),
+		   atomic_long_read(&session->stats.tx_errors),
+		   atomic_long_read(&session->stats.rx_packets),
+		   atomic_long_read(&session->stats.rx_bytes),
+		   atomic_long_read(&session->stats.rx_errors));
 
 	if (po)
 		seq_printf(m, "   interface %s\n", ppp_dev_name(&po->chan));
@@ -1839,7 +1800,7 @@ static const struct pppox_proto pppol2tp_proto = {
 
 static const struct l2tp_nl_cmd_ops pppol2tp_nl_cmd_ops = {
 	.session_create	= pppol2tp_session_create,
-	.session_delete	= pppol2tp_session_delete,
+	.session_delete	= l2tp_session_delete,
 };
 
 #endif /* CONFIG_L2TP_V3 */
diff --git a/net/llc/af_llc.c b/net/llc/af_llc.c
index 88709882c46..48aaa89253e 100644
--- a/net/llc/af_llc.c
+++ b/net/llc/af_llc.c
@@ -720,6 +720,8 @@ static int llc_ui_recvmsg(struct kiocb *iocb, struct socket *sock,
 	int target;	/* Read at least this many bytes */
 	long timeo;
 
+	msg->msg_namelen = 0;
+
 	lock_sock(sk);
 	copied = -ENOTCONN;
 	if (unlikely(sk->sk_type == SOCK_STREAM && sk->sk_state == TCP_LISTEN))
diff --git a/net/mac80211/cfg.c b/net/mac80211/cfg.c
index fb306814576..a6893602f87 100644
--- a/net/mac80211/cfg.c
+++ b/net/mac80211/cfg.c
@@ -2582,7 +2582,7 @@ static int ieee80211_cancel_roc(struct ieee80211_local *local,
 			list_del(&dep->list);
 			mutex_unlock(&local->mtx);
 
-			ieee80211_roc_notify_destroy(dep);
+			ieee80211_roc_notify_destroy(dep, true);
 			return 0;
 		}
 
@@ -2622,7 +2622,7 @@ static int ieee80211_cancel_roc(struct ieee80211_local *local,
 			ieee80211_start_next_roc(local);
 		mutex_unlock(&local->mtx);
 
-		ieee80211_roc_notify_destroy(found);
+		ieee80211_roc_notify_destroy(found, true);
 	} else {
 		/* work may be pending so use it all the time */
 		found->abort = true;
@@ -2632,6 +2632,8 @@ static int ieee80211_cancel_roc(struct ieee80211_local *local,
 
 		/* work will clean up etc */
 		flush_delayed_work(&found->work);
+		WARN_ON(!found->to_be_freed);
+		kfree(found);
 	}
 
 	return 0;
diff --git a/net/mac80211/chan.c b/net/mac80211/chan.c
index 78c0d90dd64..931be419ab5 100644
--- a/net/mac80211/chan.c
+++ b/net/mac80211/chan.c
@@ -63,6 +63,7 @@ ieee80211_new_chanctx(struct ieee80211_local *local,
 		      enum ieee80211_chanctx_mode mode)
 {
 	struct ieee80211_chanctx *ctx;
+	u32 changed;
 	int err;
 
 	lockdep_assert_held(&local->chanctx_mtx);
@@ -76,6 +77,13 @@ ieee80211_new_chanctx(struct ieee80211_local *local,
 	ctx->conf.rx_chains_dynamic = 1;
 	ctx->mode = mode;
 
+	/* acquire mutex to prevent idle from changing */
+	mutex_lock(&local->mtx);
+	/* turn idle off *before* setting channel -- some drivers need that */
+	changed = ieee80211_idle_off(local);
+	if (changed)
+		ieee80211_hw_config(local, changed);
+
 	if (!local->use_chanctx) {
 		local->_oper_channel_type =
 			cfg80211_get_chandef_type(chandef);
@@ -85,14 +93,17 @@ ieee80211_new_chanctx(struct ieee80211_local *local,
 		err = drv_add_chanctx(local, ctx);
 		if (err) {
 			kfree(ctx);
-			return ERR_PTR(err);
+			ctx = ERR_PTR(err);
+
+			ieee80211_recalc_idle(local);
+			goto out;
 		}
 	}
 
+	/* and keep the mutex held until the new chanctx is on the list */
 	list_add_rcu(&ctx->list, &local->chanctx_list);
 
-	mutex_lock(&local->mtx);
-	ieee80211_recalc_idle(local);
+ out:
 	mutex_unlock(&local->mtx);
 
 	return ctx;
diff --git a/net/mac80211/ieee80211_i.h b/net/mac80211/ieee80211_i.h
index 388580a1bad..5672533a083 100644
--- a/net/mac80211/ieee80211_i.h
+++ b/net/mac80211/ieee80211_i.h
@@ -309,6 +309,7 @@ struct ieee80211_roc_work {
 	struct ieee80211_channel *chan;
 
 	bool started, abort, hw_begun, notified;
+	bool to_be_freed;
 
 	unsigned long hw_start_time;
 
@@ -1347,7 +1348,7 @@ void ieee80211_offchannel_return(struct ieee80211_local *local);
 void ieee80211_roc_setup(struct ieee80211_local *local);
 void ieee80211_start_next_roc(struct ieee80211_local *local);
 void ieee80211_roc_purge(struct ieee80211_sub_if_data *sdata);
-void ieee80211_roc_notify_destroy(struct ieee80211_roc_work *roc);
+void ieee80211_roc_notify_destroy(struct ieee80211_roc_work *roc, bool free);
 void ieee80211_sw_roc_work(struct work_struct *work);
 void ieee80211_handle_roc_started(struct ieee80211_roc_work *roc);
 
@@ -1361,6 +1362,7 @@ int ieee80211_if_change_type(struct ieee80211_sub_if_data *sdata,
 			     enum nl80211_iftype type);
 void ieee80211_if_remove(struct ieee80211_sub_if_data *sdata);
 void ieee80211_remove_interfaces(struct ieee80211_local *local);
+u32 ieee80211_idle_off(struct ieee80211_local *local);
 void ieee80211_recalc_idle(struct ieee80211_local *local);
 void ieee80211_adjust_monitor_flags(struct ieee80211_sub_if_data *sdata,
 				    const int offset);
diff --git a/net/mac80211/iface.c b/net/mac80211/iface.c
index baaa8608e52..9ed49ad0380 100644
--- a/net/mac80211/iface.c
+++ b/net/mac80211/iface.c
@@ -78,7 +78,7 @@ void ieee80211_recalc_txpower(struct ieee80211_sub_if_data *sdata)
 		ieee80211_bss_info_change_notify(sdata, BSS_CHANGED_TXPOWER);
 }
 
-static u32 ieee80211_idle_off(struct ieee80211_local *local)
+static u32 __ieee80211_idle_off(struct ieee80211_local *local)
 {
 	if (!(local->hw.conf.flags & IEEE80211_CONF_IDLE))
 		return 0;
@@ -87,7 +87,7 @@ static u32 ieee80211_idle_off(struct ieee80211_local *local)
 	return IEEE80211_CONF_CHANGE_IDLE;
 }
 
-static u32 ieee80211_idle_on(struct ieee80211_local *local)
+static u32 __ieee80211_idle_on(struct ieee80211_local *local)
 {
 	if (local->hw.conf.flags & IEEE80211_CONF_IDLE)
 		return 0;
@@ -98,16 +98,18 @@ static u32 ieee80211_idle_on(struct ieee80211_local *local)
 	return IEEE80211_CONF_CHANGE_IDLE;
 }
 
-void ieee80211_recalc_idle(struct ieee80211_local *local)
+static u32 __ieee80211_recalc_idle(struct ieee80211_local *local,
+				   bool force_active)
 {
 	bool working = false, scanning, active;
 	unsigned int led_trig_start = 0, led_trig_stop = 0;
 	struct ieee80211_roc_work *roc;
-	u32 change;
 
 	lockdep_assert_held(&local->mtx);
 
-	active = !list_empty(&local->chanctx_list) || local->monitors;
+	active = force_active ||
+		 !list_empty(&local->chanctx_list) ||
+		 local->monitors;
 
 	if (!local->ops->remain_on_channel) {
 		list_for_each_entry(roc, &local->roc_list, list) {
@@ -132,9 +134,18 @@ void ieee80211_recalc_idle(struct ieee80211_local *local)
 	ieee80211_mod_tpt_led_trig(local, led_trig_start, led_trig_stop);
 
 	if (working || scanning || active)
-		change = ieee80211_idle_off(local);
-	else
-		change = ieee80211_idle_on(local);
+		return __ieee80211_idle_off(local);
+	return __ieee80211_idle_on(local);
+}
+
+u32 ieee80211_idle_off(struct ieee80211_local *local)
+{
+	return __ieee80211_recalc_idle(local, true);
+}
+
+void ieee80211_recalc_idle(struct ieee80211_local *local)
+{
+	u32 change = __ieee80211_recalc_idle(local, false);
 	if (change)
 		ieee80211_hw_config(local, change);
 }
@@ -349,21 +360,19 @@ static void ieee80211_set_default_queues(struct ieee80211_sub_if_data *sdata)
 static int ieee80211_add_virtual_monitor(struct ieee80211_local *local)
 {
 	struct ieee80211_sub_if_data *sdata;
-	int ret = 0;
+	int ret;
 
 	if (!(local->hw.flags & IEEE80211_HW_WANT_MONITOR_VIF))
 		return 0;
 
-	mutex_lock(&local->iflist_mtx);
+	ASSERT_RTNL();
 
 	if (local->monitor_sdata)
-		goto out_unlock;
+		return 0;
 
 	sdata = kzalloc(sizeof(*sdata) + local->hw.vif_data_size, GFP_KERNEL);
-	if (!sdata) {
-		ret = -ENOMEM;
-		goto out_unlock;
-	}
+	if (!sdata)
+		return -ENOMEM;
 
 	/* set up data */
 	sdata->local = local;
@@ -377,13 +386,13 @@ static int ieee80211_add_virtual_monitor(struct ieee80211_local *local)
 	if (WARN_ON(ret)) {
 		/* ok .. stupid driver, it asked for this! */
 		kfree(sdata);
-		goto out_unlock;
+		return ret;
 	}
 
 	ret = ieee80211_check_queues(sdata);
 	if (ret) {
 		kfree(sdata);
-		goto out_unlock;
+		return ret;
 	}
 
 	ret = ieee80211_vif_use_channel(sdata, &local->monitor_chandef,
@@ -391,13 +400,14 @@ static int ieee80211_add_virtual_monitor(struct ieee80211_local *local)
 	if (ret) {
 		drv_remove_interface(local, sdata);
 		kfree(sdata);
-		goto out_unlock;
+		return ret;
 	}
 
+	mutex_lock(&local->iflist_mtx);
 	rcu_assign_pointer(local->monitor_sdata, sdata);
- out_unlock:
 	mutex_unlock(&local->iflist_mtx);
-	return ret;
+
+	return 0;
 }
 
 static void ieee80211_del_virtual_monitor(struct ieee80211_local *local)
@@ -407,14 +417,20 @@ static void ieee80211_del_virtual_monitor(struct ieee80211_local *local)
 	if (!(local->hw.flags & IEEE80211_HW_WANT_MONITOR_VIF))
 		return;
 
+	ASSERT_RTNL();
+
 	mutex_lock(&local->iflist_mtx);
 
 	sdata = rcu_dereference_protected(local->monitor_sdata,
 					  lockdep_is_held(&local->iflist_mtx));
-	if (!sdata)
-		goto out_unlock;
+	if (!sdata) {
+		mutex_unlock(&local->iflist_mtx);
+		return;
+	}
 
 	rcu_assign_pointer(local->monitor_sdata, NULL);
+	mutex_unlock(&local->iflist_mtx);
+
 	synchronize_net();
 
 	ieee80211_vif_release_channel(sdata);
@@ -422,8 +438,6 @@ static void ieee80211_del_virtual_monitor(struct ieee80211_local *local)
 	drv_remove_interface(local, sdata);
 
 	kfree(sdata);
- out_unlock:
-	mutex_unlock(&local->iflist_mtx);
 }
 
 /*
diff --git a/net/mac80211/mesh.c b/net/mac80211/mesh.c
index 29ce2aa87e7..4749b385869 100644
--- a/net/mac80211/mesh.c
+++ b/net/mac80211/mesh.c
@@ -1060,7 +1060,8 @@ void ieee80211_mesh_notify_scan_completed(struct ieee80211_local *local)
 
 	rcu_read_lock();
 	list_for_each_entry_rcu(sdata, &local->interfaces, list)
-		if (ieee80211_vif_is_mesh(&sdata->vif))
+		if (ieee80211_vif_is_mesh(&sdata->vif) &&
+		    ieee80211_sdata_running(sdata))
 			ieee80211_queue_work(&local->hw, &sdata->work);
 	rcu_read_unlock();
 }
diff --git a/net/mac80211/mlme.c b/net/mac80211/mlme.c
index 141577412d8..346ad4cfb01 100644
--- a/net/mac80211/mlme.c
+++ b/net/mac80211/mlme.c
@@ -3608,8 +3608,10 @@ void ieee80211_mlme_notify_scan_completed(struct ieee80211_local *local)
 
 	/* Restart STA timers */
 	rcu_read_lock();
-	list_for_each_entry_rcu(sdata, &local->interfaces, list)
-		ieee80211_restart_sta_timer(sdata);
+	list_for_each_entry_rcu(sdata, &local->interfaces, list) {
+		if (ieee80211_sdata_running(sdata))
+			ieee80211_restart_sta_timer(sdata);
+	}
 	rcu_read_unlock();
 }
 
@@ -3962,8 +3964,16 @@ int ieee80211_mgd_auth(struct ieee80211_sub_if_data *sdata,
 	/* prep auth_data so we don't go into idle on disassoc */
 	ifmgd->auth_data = auth_data;
 
-	if (ifmgd->associated)
-		ieee80211_set_disassoc(sdata, 0, 0, false, NULL);
+	if (ifmgd->associated) {
+		u8 frame_buf[IEEE80211_DEAUTH_FRAME_LEN];
+
+		ieee80211_set_disassoc(sdata, IEEE80211_STYPE_DEAUTH,
+				       WLAN_REASON_UNSPECIFIED,
+				       false, frame_buf);
+
+		__cfg80211_send_deauth(sdata->dev, frame_buf,
+				       sizeof(frame_buf));
+	}
 
 	sdata_info(sdata, "authenticate with %pM\n", req->bss->bssid);
 
@@ -4023,8 +4033,16 @@ int ieee80211_mgd_assoc(struct ieee80211_sub_if_data *sdata,
 
 	mutex_lock(&ifmgd->mtx);
 
-	if (ifmgd->associated)
-		ieee80211_set_disassoc(sdata, 0, 0, false, NULL);
+	if (ifmgd->associated) {
+		u8 frame_buf[IEEE80211_DEAUTH_FRAME_LEN];
+
+		ieee80211_set_disassoc(sdata, IEEE80211_STYPE_DEAUTH,
+				       WLAN_REASON_UNSPECIFIED,
+				       false, frame_buf);
+
+		__cfg80211_send_deauth(sdata->dev, frame_buf,
+				       sizeof(frame_buf));
+	}
 
 	if (ifmgd->auth_data && !ifmgd->auth_data->done) {
 		err = -EBUSY;
diff --git a/net/mac80211/offchannel.c b/net/mac80211/offchannel.c
index cc79b4a2e82..430bd254e49 100644
--- a/net/mac80211/offchannel.c
+++ b/net/mac80211/offchannel.c
@@ -297,10 +297,13 @@ void ieee80211_start_next_roc(struct ieee80211_local *local)
 	}
 }
 
-void ieee80211_roc_notify_destroy(struct ieee80211_roc_work *roc)
+void ieee80211_roc_notify_destroy(struct ieee80211_roc_work *roc, bool free)
 {
 	struct ieee80211_roc_work *dep, *tmp;
 
+	if (WARN_ON(roc->to_be_freed))
+		return;
+
 	/* was never transmitted */
 	if (roc->frame) {
 		cfg80211_mgmt_tx_status(&roc->sdata->wdev,
@@ -316,9 +319,12 @@ void ieee80211_roc_notify_destroy(struct ieee80211_roc_work *roc)
 						   GFP_KERNEL);
 
 	list_for_each_entry_safe(dep, tmp, &roc->dependents, list)
-		ieee80211_roc_notify_destroy(dep);
+		ieee80211_roc_notify_destroy(dep, true);
 
-	kfree(roc);
+	if (free)
+		kfree(roc);
+	else
+		roc->to_be_freed = true;
 }
 
 void ieee80211_sw_roc_work(struct work_struct *work)
@@ -331,6 +337,9 @@ void ieee80211_sw_roc_work(struct work_struct *work)
 
 	mutex_lock(&local->mtx);
 
+	if (roc->to_be_freed)
+		goto out_unlock;
+
 	if (roc->abort)
 		goto finish;
 
@@ -370,7 +379,7 @@ void ieee80211_sw_roc_work(struct work_struct *work)
  finish:
 		list_del(&roc->list);
 		started = roc->started;
-		ieee80211_roc_notify_destroy(roc);
+		ieee80211_roc_notify_destroy(roc, !roc->abort);
 
 		if (started) {
 			drv_flush(local, false);
@@ -410,7 +419,7 @@ static void ieee80211_hw_roc_done(struct work_struct *work)
 
 	list_del(&roc->list);
 
-	ieee80211_roc_notify_destroy(roc);
+	ieee80211_roc_notify_destroy(roc, true);
 
 	/* if there's another roc, start it now */
 	ieee80211_start_next_roc(local);
@@ -460,12 +469,14 @@ void ieee80211_roc_purge(struct ieee80211_sub_if_data *sdata)
 	list_for_each_entry_safe(roc, tmp, &tmp_list, list) {
 		if (local->ops->remain_on_channel) {
 			list_del(&roc->list);
-			ieee80211_roc_notify_destroy(roc);
+			ieee80211_roc_notify_destroy(roc, true);
 		} else {
 			ieee80211_queue_delayed_work(&local->hw, &roc->work, 0);
 
 			/* work will clean up etc */
 			flush_delayed_work(&roc->work);
+			WARN_ON(!roc->to_be_freed);
+			kfree(roc);
 		}
 	}
 
diff --git a/net/mac80211/rx.c b/net/mac80211/rx.c
index bb73ed2d20b..c6844ad080b 100644
--- a/net/mac80211/rx.c
+++ b/net/mac80211/rx.c
@@ -2675,7 +2675,19 @@ ieee80211_rx_h_action_return(struct ieee80211_rx_data *rx)
 
 		memset(nskb->cb, 0, sizeof(nskb->cb));
 
-		ieee80211_tx_skb(rx->sdata, nskb);
+		if (rx->sdata->vif.type == NL80211_IFTYPE_P2P_DEVICE) {
+			struct ieee80211_tx_info *info = IEEE80211_SKB_CB(nskb);
+
+			info->flags = IEEE80211_TX_CTL_TX_OFFCHAN |
+				      IEEE80211_TX_INTFL_OFFCHAN_TX_OK |
+				      IEEE80211_TX_CTL_NO_CCK_RATE;
+			if (local->hw.flags & IEEE80211_HW_QUEUE_CONTROL)
+				info->hw_queue =
+					local->hw.offchannel_tx_hw_queue;
+		}
+
+		__ieee80211_tx_skb_tid_band(rx->sdata, nskb, 7,
+					    status->band);
 	}
 	dev_kfree_skb(rx->skb);
 	return RX_QUEUED;
diff --git a/net/mac80211/sta_info.c b/net/mac80211/sta_info.c
index a79ce820cb5..238a0cca320 100644
--- a/net/mac80211/sta_info.c
+++ b/net/mac80211/sta_info.c
@@ -766,6 +766,7 @@ int __must_check __sta_info_destroy(struct sta_info *sta)
 	struct ieee80211_local *local;
 	struct ieee80211_sub_if_data *sdata;
 	int ret, i;
+	bool have_key = false;
 
 	might_sleep();
 
@@ -793,12 +794,19 @@ int __must_check __sta_info_destroy(struct sta_info *sta)
 	list_del_rcu(&sta->list);
 
 	mutex_lock(&local->key_mtx);
-	for (i = 0; i < NUM_DEFAULT_KEYS; i++)
+	for (i = 0; i < NUM_DEFAULT_KEYS; i++) {
 		__ieee80211_key_free(key_mtx_dereference(local, sta->gtk[i]));
-	if (sta->ptk)
+		have_key = true;
+	}
+	if (sta->ptk) {
 		__ieee80211_key_free(key_mtx_dereference(local, sta->ptk));
+		have_key = true;
+	}
 	mutex_unlock(&local->key_mtx);
 
+	if (!have_key)
+		synchronize_net();
+
 	sta->dead = true;
 
 	local->num_sta--;
diff --git a/net/netfilter/ipset/ip_set_bitmap_ipmac.c b/net/netfilter/ipset/ip_set_bitmap_ipmac.c
index 0f92dc24cb8..d7df6ac2c6f 100644
--- a/net/netfilter/ipset/ip_set_bitmap_ipmac.c
+++ b/net/netfilter/ipset/ip_set_bitmap_ipmac.c
@@ -339,7 +339,11 @@ bitmap_ipmac_tlist(const struct ip_set *set,
 nla_put_failure:
 	nla_nest_cancel(skb, nested);
 	ipset_nest_end(skb, atd);
-	return -EMSGSIZE;
+	if (unlikely(id == first)) {
+		cb->args[2] = 0;
+		return -EMSGSIZE;
+	}
+	return 0;
 }
 
 static int
diff --git a/net/netfilter/ipset/ip_set_hash_ipportnet.c b/net/netfilter/ipset/ip_set_hash_ipportnet.c
index f2627226a08..10a30b4fc7d 100644
--- a/net/netfilter/ipset/ip_set_hash_ipportnet.c
+++ b/net/netfilter/ipset/ip_set_hash_ipportnet.c
@@ -104,6 +104,15 @@ hash_ipportnet4_data_flags(struct hash_ipportnet4_elem *dst, u32 flags)
 	dst->nomatch = !!(flags & IPSET_FLAG_NOMATCH);
 }
 
+static inline void
+hash_ipportnet4_data_reset_flags(struct hash_ipportnet4_elem *dst, u32 *flags)
+{
+	if (dst->nomatch) {
+		*flags = IPSET_FLAG_NOMATCH;
+		dst->nomatch = 0;
+	}
+}
+
 static inline int
 hash_ipportnet4_data_match(const struct hash_ipportnet4_elem *elem)
 {
@@ -414,6 +423,15 @@ hash_ipportnet6_data_flags(struct hash_ipportnet6_elem *dst, u32 flags)
 	dst->nomatch = !!(flags & IPSET_FLAG_NOMATCH);
 }
 
+static inline void
+hash_ipportnet6_data_reset_flags(struct hash_ipportnet6_elem *dst, u32 *flags)
+{
+	if (dst->nomatch) {
+		*flags = IPSET_FLAG_NOMATCH;
+		dst->nomatch = 0;
+	}
+}
+
 static inline int
 hash_ipportnet6_data_match(const struct hash_ipportnet6_elem *elem)
 {
diff --git a/net/netfilter/ipset/ip_set_hash_net.c b/net/netfilter/ipset/ip_set_hash_net.c
index 4b677cf6bf7..d6a59154d71 100644
--- a/net/netfilter/ipset/ip_set_hash_net.c
+++ b/net/netfilter/ipset/ip_set_hash_net.c
@@ -87,7 +87,16 @@ hash_net4_data_copy(struct hash_net4_elem *dst,
 static inline void
 hash_net4_data_flags(struct hash_net4_elem *dst, u32 flags)
 {
-	dst->nomatch = flags & IPSET_FLAG_NOMATCH;
+	dst->nomatch = !!(flags & IPSET_FLAG_NOMATCH);
+}
+
+static inline void
+hash_net4_data_reset_flags(struct hash_net4_elem *dst, u32 *flags)
+{
+	if (dst->nomatch) {
+		*flags = IPSET_FLAG_NOMATCH;
+		dst->nomatch = 0;
+	}
 }
 
 static inline int
@@ -308,7 +317,16 @@ hash_net6_data_copy(struct hash_net6_elem *dst,
 static inline void
 hash_net6_data_flags(struct hash_net6_elem *dst, u32 flags)
 {
-	dst->nomatch = flags & IPSET_FLAG_NOMATCH;
+	dst->nomatch = !!(flags & IPSET_FLAG_NOMATCH);
+}
+
+static inline void
+hash_net6_data_reset_flags(struct hash_net6_elem *dst, u32 *flags)
+{
+	if (dst->nomatch) {
+		*flags = IPSET_FLAG_NOMATCH;
+		dst->nomatch = 0;
+	}
 }
 
 static inline int
diff --git a/net/netfilter/ipset/ip_set_hash_netiface.c b/net/netfilter/ipset/ip_set_hash_netiface.c
index 6ba985f1c96..f2b0a3c3013 100644
--- a/net/netfilter/ipset/ip_set_hash_netiface.c
+++ b/net/netfilter/ipset/ip_set_hash_netiface.c
@@ -198,7 +198,16 @@ hash_netiface4_data_copy(struct hash_netiface4_elem *dst,
 static inline void
 hash_netiface4_data_flags(struct hash_netiface4_elem *dst, u32 flags)
 {
-	dst->nomatch = flags & IPSET_FLAG_NOMATCH;
+	dst->nomatch = !!(flags & IPSET_FLAG_NOMATCH);
+}
+
+static inline void
+hash_netiface4_data_reset_flags(struct hash_netiface4_elem *dst, u32 *flags)
+{
+	if (dst->nomatch) {
+		*flags = IPSET_FLAG_NOMATCH;
+		dst->nomatch = 0;
+	}
 }
 
 static inline int
@@ -494,7 +503,7 @@ hash_netiface6_data_copy(struct hash_netiface6_elem *dst,
 static inline void
 hash_netiface6_data_flags(struct hash_netiface6_elem *dst, u32 flags)
 {
-	dst->nomatch = flags & IPSET_FLAG_NOMATCH;
+	dst->nomatch = !!(flags & IPSET_FLAG_NOMATCH);
 }
 
 static inline int
@@ -504,6 +513,15 @@ hash_netiface6_data_match(const struct hash_netiface6_elem *elem)
 }
 
 static inline void
+hash_netiface6_data_reset_flags(struct hash_netiface6_elem *dst, u32 *flags)
+{
+	if (dst->nomatch) {
+		*flags = IPSET_FLAG_NOMATCH;
+		dst->nomatch = 0;
+	}
+}
+
+static inline void
 hash_netiface6_data_zero_out(struct hash_netiface6_elem *elem)
 {
 	elem->elem = 0;
diff --git a/net/netfilter/ipset/ip_set_hash_netport.c b/net/netfilter/ipset/ip_set_hash_netport.c
index af20c0c5ced..349deb672a2 100644
--- a/net/netfilter/ipset/ip_set_hash_netport.c
+++ b/net/netfilter/ipset/ip_set_hash_netport.c
@@ -104,6 +104,15 @@ hash_netport4_data_flags(struct hash_netport4_elem *dst, u32 flags)
 	dst->nomatch = !!(flags & IPSET_FLAG_NOMATCH);
 }
 
+static inline void
+hash_netport4_data_reset_flags(struct hash_netport4_elem *dst, u32 *flags)
+{
+	if (dst->nomatch) {
+		*flags = IPSET_FLAG_NOMATCH;
+		dst->nomatch = 0;
+	}
+}
+
 static inline int
 hash_netport4_data_match(const struct hash_netport4_elem *elem)
 {
@@ -375,6 +384,15 @@ hash_netport6_data_flags(struct hash_netport6_elem *dst, u32 flags)
 	dst->nomatch = !!(flags & IPSET_FLAG_NOMATCH);
 }
 
+static inline void
+hash_netport6_data_reset_flags(struct hash_netport6_elem *dst, u32 *flags)
+{
+	if (dst->nomatch) {
+		*flags = IPSET_FLAG_NOMATCH;
+		dst->nomatch = 0;
+	}
+}
+
 static inline int
 hash_netport6_data_match(const struct hash_netport6_elem *elem)
 {
diff --git a/net/netfilter/ipset/ip_set_list_set.c b/net/netfilter/ipset/ip_set_list_set.c
index 8371c2bac2e..09c744aa898 100644
--- a/net/netfilter/ipset/ip_set_list_set.c
+++ b/net/netfilter/ipset/ip_set_list_set.c
@@ -174,9 +174,13 @@ list_set_add(struct list_set *map, u32 i, ip_set_id_t id,
 {
 	const struct set_elem *e = list_set_elem(map, i);
 
-	if (i == map->size - 1 && e->id != IPSET_INVALID_ID)
-		/* Last element replaced: e.g. add new,before,last */
-		ip_set_put_byindex(e->id);
+	if (e->id != IPSET_INVALID_ID) {
+		const struct set_elem *x = list_set_elem(map, map->size - 1);
+
+		/* Last element replaced or pushed off */
+		if (x->id != IPSET_INVALID_ID)
+			ip_set_put_byindex(x->id);
+	}
 	if (with_timeout(map->timeout))
 		list_elem_tadd(map, i, id, ip_set_timeout_set(timeout));
 	else
diff --git a/net/netfilter/ipvs/ip_vs_core.c b/net/netfilter/ipvs/ip_vs_core.c
index 47edf5a40a5..61f49d24171 100644
--- a/net/netfilter/ipvs/ip_vs_core.c
+++ b/net/netfilter/ipvs/ip_vs_core.c
@@ -1394,10 +1394,8 @@ ip_vs_in_icmp(struct sk_buff *skb, int *related, unsigned int hooknum)
 			skb_reset_network_header(skb);
 			IP_VS_DBG(12, "ICMP for IPIP %pI4->%pI4: mtu=%u\n",
 				&ip_hdr(skb)->saddr, &ip_hdr(skb)->daddr, mtu);
-			rcu_read_lock();
 			ipv4_update_pmtu(skb, dev_net(skb->dev),
 					 mtu, 0, 0, 0, 0);
-			rcu_read_unlock();
 			/* Client uses PMTUD? */
 			if (!(cih->frag_off & htons(IP_DF)))
 				goto ignore_ipip;
@@ -1577,7 +1575,8 @@ ip_vs_in(unsigned int hooknum, struct sk_buff *skb, int af)
 	}
 	/* ipvs enabled in this netns ? */
 	net = skb_net(skb);
-	if (!net_ipvs(net)->enable)
+	ipvs = net_ipvs(net);
+	if (unlikely(sysctl_backup_only(ipvs) || !ipvs->enable))
 		return NF_ACCEPT;
 
 	ip_vs_fill_iph_skb(af, skb, &iph);
@@ -1654,7 +1653,6 @@ ip_vs_in(unsigned int hooknum, struct sk_buff *skb, int af)
 	}
 
 	IP_VS_DBG_PKT(11, af, pp, skb, 0, "Incoming packet");
-	ipvs = net_ipvs(net);
 	/* Check the server status */
 	if (cp->dest && !(cp->dest->flags & IP_VS_DEST_F_AVAILABLE)) {
 		/* the destination server is not available */
@@ -1815,13 +1813,15 @@ ip_vs_forward_icmp(unsigned int hooknum, struct sk_buff *skb,
 {
 	int r;
 	struct net *net;
+	struct netns_ipvs *ipvs;
 
 	if (ip_hdr(skb)->protocol != IPPROTO_ICMP)
 		return NF_ACCEPT;
 
 	/* ipvs enabled in this netns ? */
 	net = skb_net(skb);
-	if (!net_ipvs(net)->enable)
+	ipvs = net_ipvs(net);
+	if (unlikely(sysctl_backup_only(ipvs) || !ipvs->enable))
 		return NF_ACCEPT;
 
 	return ip_vs_in_icmp(skb, &r, hooknum);
@@ -1835,6 +1835,7 @@ ip_vs_forward_icmp_v6(unsigned int hooknum, struct sk_buff *skb,
 {
 	int r;
 	struct net *net;
+	struct netns_ipvs *ipvs;
 	struct ip_vs_iphdr iphdr;
 
 	ip_vs_fill_iph_skb(AF_INET6, skb, &iphdr);
@@ -1843,7 +1844,8 @@ ip_vs_forward_icmp_v6(unsigned int hooknum, struct sk_buff *skb,
 
 	/* ipvs enabled in this netns ? */
 	net = skb_net(skb);
-	if (!net_ipvs(net)->enable)
+	ipvs = net_ipvs(net);
+	if (unlikely(sysctl_backup_only(ipvs) || !ipvs->enable))
 		return NF_ACCEPT;
 
 	return ip_vs_in_icmp_v6(skb, &r, hooknum, &iphdr);
diff --git a/net/netfilter/ipvs/ip_vs_ctl.c b/net/netfilter/ipvs/ip_vs_ctl.c
index c68198bf912..9e2d1cccd1e 100644
--- a/net/netfilter/ipvs/ip_vs_ctl.c
+++ b/net/netfilter/ipvs/ip_vs_ctl.c
@@ -1808,6 +1808,12 @@ static struct ctl_table vs_vars[] = {
 		.mode		= 0644,
 		.proc_handler	= proc_dointvec,
 	},
+	{
+		.procname	= "backup_only",
+		.maxlen		= sizeof(int),
+		.mode		= 0644,
+		.proc_handler	= proc_dointvec,
+	},
 #ifdef CONFIG_IP_VS_DEBUG
 	{
 		.procname	= "debug_level",
@@ -3741,6 +3747,7 @@ static int __net_init ip_vs_control_net_init_sysctl(struct net *net)
 	tbl[idx++].data = &ipvs->sysctl_nat_icmp_send;
 	ipvs->sysctl_pmtu_disc = 1;
 	tbl[idx++].data = &ipvs->sysctl_pmtu_disc;
+	tbl[idx++].data = &ipvs->sysctl_backup_only;
 
 
 	ipvs->sysctl_hdr = register_net_sysctl(net, "net/ipv4/vs", tbl);
diff --git a/net/netfilter/ipvs/ip_vs_proto_sctp.c b/net/netfilter/ipvs/ip_vs_proto_sctp.c
index ae8ec6f2768..cd1d7298f7b 100644
--- a/net/netfilter/ipvs/ip_vs_proto_sctp.c
+++ b/net/netfilter/ipvs/ip_vs_proto_sctp.c
@@ -906,7 +906,7 @@ set_sctp_state(struct ip_vs_proto_data *pd, struct ip_vs_conn *cp,
 	sctp_chunkhdr_t _sctpch, *sch;
 	unsigned char chunk_type;
 	int event, next_state;
-	int ihl;
+	int ihl, cofs;
 
 #ifdef CONFIG_IP_VS_IPV6
 	ihl = cp->af == AF_INET ? ip_hdrlen(skb) : sizeof(struct ipv6hdr);
@@ -914,8 +914,8 @@ set_sctp_state(struct ip_vs_proto_data *pd, struct ip_vs_conn *cp,
 	ihl = ip_hdrlen(skb);
 #endif
 
-	sch = skb_header_pointer(skb, ihl + sizeof(sctp_sctphdr_t),
-				sizeof(_sctpch), &_sctpch);
+	cofs = ihl + sizeof(sctp_sctphdr_t);
+	sch = skb_header_pointer(skb, cofs, sizeof(_sctpch), &_sctpch);
 	if (sch == NULL)
 		return;
 
@@ -933,10 +933,12 @@ set_sctp_state(struct ip_vs_proto_data *pd, struct ip_vs_conn *cp,
 	 */
 	if ((sch->type == SCTP_CID_COOKIE_ECHO) ||
 	    (sch->type == SCTP_CID_COOKIE_ACK)) {
-		sch = skb_header_pointer(skb, (ihl + sizeof(sctp_sctphdr_t) +
-				sch->length), sizeof(_sctpch), &_sctpch);
-		if (sch) {
-			if (sch->type == SCTP_CID_ABORT)
+		int clen = ntohs(sch->length);
+
+		if (clen >= sizeof(sctp_chunkhdr_t)) {
+			sch = skb_header_pointer(skb, cofs + ALIGN(clen, 4),
+						 sizeof(_sctpch), &_sctpch);
+			if (sch && sch->type == SCTP_CID_ABORT)
 				chunk_type = sch->type;
 		}
 	}
diff --git a/net/netfilter/nf_conntrack_core.c b/net/netfilter/nf_conntrack_core.c
index c8e001a9c45..f84965af4a4 100644
--- a/net/netfilter/nf_conntrack_core.c
+++ b/net/netfilter/nf_conntrack_core.c
@@ -264,7 +264,7 @@ static void death_by_event(unsigned long ul_conntrack)
 	if (nf_conntrack_event(IPCT_DESTROY, ct) < 0) {
 		/* bad luck, let's retry again */
 		ecache->timeout.expires = jiffies +
-			(random32() % net->ct.sysctl_events_retry_timeout);
+			(prandom_u32() % net->ct.sysctl_events_retry_timeout);
 		add_timer(&ecache->timeout);
 		return;
 	}
@@ -283,7 +283,7 @@ void nf_ct_dying_timeout(struct nf_conn *ct)
 	/* set a new timer to retry event delivery */
 	setup_timer(&ecache->timeout, death_by_event, (unsigned long)ct);
 	ecache->timeout.expires = jiffies +
-		(random32() % net->ct.sysctl_events_retry_timeout);
+		(prandom_u32() % net->ct.sysctl_events_retry_timeout);
 	add_timer(&ecache->timeout);
 }
 EXPORT_SYMBOL_GPL(nf_ct_dying_timeout);
diff --git a/net/netfilter/nf_conntrack_proto_dccp.c b/net/netfilter/nf_conntrack_proto_dccp.c
index 432f9578000..ba65b2041eb 100644
--- a/net/netfilter/nf_conntrack_proto_dccp.c
+++ b/net/netfilter/nf_conntrack_proto_dccp.c
@@ -969,6 +969,10 @@ static int __init nf_conntrack_proto_dccp_init(void)
 {
 	int ret;
 
+	ret = register_pernet_subsys(&dccp_net_ops);
+	if (ret < 0)
+		goto out_pernet;
+
 	ret = nf_ct_l4proto_register(&dccp_proto4);
 	if (ret < 0)
 		goto out_dccp4;
@@ -977,16 +981,12 @@ static int __init nf_conntrack_proto_dccp_init(void)
 	if (ret < 0)
 		goto out_dccp6;
 
-	ret = register_pernet_subsys(&dccp_net_ops);
-	if (ret < 0)
-		goto out_pernet;
-
 	return 0;
-out_pernet:
-	nf_ct_l4proto_unregister(&dccp_proto6);
 out_dccp6:
 	nf_ct_l4proto_unregister(&dccp_proto4);
 out_dccp4:
+	unregister_pernet_subsys(&dccp_net_ops);
+out_pernet:
 	return ret;
 }
 
diff --git a/net/netfilter/nf_conntrack_proto_gre.c b/net/netfilter/nf_conntrack_proto_gre.c
index bd7d01d9c7e..155ce9f8a0d 100644
--- a/net/netfilter/nf_conntrack_proto_gre.c
+++ b/net/netfilter/nf_conntrack_proto_gre.c
@@ -420,18 +420,18 @@ static int __init nf_ct_proto_gre_init(void)
 {
 	int ret;
 
-	ret = nf_ct_l4proto_register(&nf_conntrack_l4proto_gre4);
-	if (ret < 0)
-		goto out_gre4;
-
 	ret = register_pernet_subsys(&proto_gre_net_ops);
 	if (ret < 0)
 		goto out_pernet;
 
+	ret = nf_ct_l4proto_register(&nf_conntrack_l4proto_gre4);
+	if (ret < 0)
+		goto out_gre4;
+
 	return 0;
-out_pernet:
-	nf_ct_l4proto_unregister(&nf_conntrack_l4proto_gre4);
 out_gre4:
+	unregister_pernet_subsys(&proto_gre_net_ops);
+out_pernet:
 	return ret;
 }
 
diff --git a/net/netfilter/nf_conntrack_proto_sctp.c b/net/netfilter/nf_conntrack_proto_sctp.c
index 480f616d593..ec83536def9 100644
--- a/net/netfilter/nf_conntrack_proto_sctp.c
+++ b/net/netfilter/nf_conntrack_proto_sctp.c
@@ -888,6 +888,10 @@ static int __init nf_conntrack_proto_sctp_init(void)
 {
 	int ret;
 
+	ret = register_pernet_subsys(&sctp_net_ops);
+	if (ret < 0)
+		goto out_pernet;
+
 	ret = nf_ct_l4proto_register(&nf_conntrack_l4proto_sctp4);
 	if (ret < 0)
 		goto out_sctp4;
@@ -896,16 +900,12 @@ static int __init nf_conntrack_proto_sctp_init(void)
 	if (ret < 0)
 		goto out_sctp6;
 
-	ret = register_pernet_subsys(&sctp_net_ops);
-	if (ret < 0)
-		goto out_pernet;
-
 	return 0;
-out_pernet:
-	nf_ct_l4proto_unregister(&nf_conntrack_l4proto_sctp6);
 out_sctp6:
 	nf_ct_l4proto_unregister(&nf_conntrack_l4proto_sctp4);
 out_sctp4:
+	unregister_pernet_subsys(&sctp_net_ops);
+out_pernet:
 	return ret;
 }
 
diff --git a/net/netfilter/nf_conntrack_proto_udplite.c b/net/netfilter/nf_conntrack_proto_udplite.c
index 157489581c3..ca969f6273f 100644
--- a/net/netfilter/nf_conntrack_proto_udplite.c
+++ b/net/netfilter/nf_conntrack_proto_udplite.c
@@ -371,6 +371,10 @@ static int __init nf_conntrack_proto_udplite_init(void)
 {
 	int ret;
 
+	ret = register_pernet_subsys(&udplite_net_ops);
+	if (ret < 0)
+		goto out_pernet;
+
 	ret = nf_ct_l4proto_register(&nf_conntrack_l4proto_udplite4);
 	if (ret < 0)
 		goto out_udplite4;
@@ -379,16 +383,12 @@ static int __init nf_conntrack_proto_udplite_init(void)
 	if (ret < 0)
 		goto out_udplite6;
 
-	ret = register_pernet_subsys(&udplite_net_ops);
-	if (ret < 0)
-		goto out_pernet;
-
 	return 0;
-out_pernet:
-	nf_ct_l4proto_unregister(&nf_conntrack_l4proto_udplite6);
 out_udplite6:
 	nf_ct_l4proto_unregister(&nf_conntrack_l4proto_udplite4);
 out_udplite4:
+	unregister_pernet_subsys(&udplite_net_ops);
+out_pernet:
 	return ret;
 }
 
diff --git a/net/netfilter/nf_conntrack_sip.c b/net/netfilter/nf_conntrack_sip.c
index 0e7d423324c..e0c4373b474 100644
--- a/net/netfilter/nf_conntrack_sip.c
+++ b/net/netfilter/nf_conntrack_sip.c
@@ -1593,10 +1593,8 @@ static int sip_help_tcp(struct sk_buff *skb, unsigned int protoff,
 		end += strlen("\r\n\r\n") + clen;
 
 		msglen = origlen = end - dptr;
-		if (msglen > datalen) {
-			nf_ct_helper_log(skb, ct, "incomplete/bad SIP message");
-			return NF_DROP;
-		}
+		if (msglen > datalen)
+			return NF_ACCEPT;
 
 		ret = process_sip_msg(skb, ct, protoff, dataoff,
 				      &dptr, &msglen);
diff --git a/net/netfilter/nf_conntrack_standalone.c b/net/netfilter/nf_conntrack_standalone.c
index 6bcce401fd1..fedee394366 100644
--- a/net/netfilter/nf_conntrack_standalone.c
+++ b/net/netfilter/nf_conntrack_standalone.c
@@ -568,6 +568,7 @@ static int __init nf_conntrack_standalone_init(void)
 		register_net_sysctl(&init_net, "net", nf_ct_netfilter_table);
 	if (!nf_ct_netfilter_header) {
 		pr_err("nf_conntrack: can't register to sysctl.\n");
+		ret = -ENOMEM;
 		goto out_sysctl;
 	}
 #endif
diff --git a/net/netfilter/nf_nat_core.c b/net/netfilter/nf_nat_core.c
index 8d5769c6d16..ad24be070e5 100644
--- a/net/netfilter/nf_nat_core.c
+++ b/net/netfilter/nf_nat_core.c
@@ -467,33 +467,22 @@ EXPORT_SYMBOL_GPL(nf_nat_packet);
 struct nf_nat_proto_clean {
 	u8	l3proto;
 	u8	l4proto;
-	bool	hash;
 };
 
-/* Clear NAT section of all conntracks, in case we're loaded again. */
-static int nf_nat_proto_clean(struct nf_conn *i, void *data)
+/* kill conntracks with affected NAT section */
+static int nf_nat_proto_remove(struct nf_conn *i, void *data)
 {
 	const struct nf_nat_proto_clean *clean = data;
 	struct nf_conn_nat *nat = nfct_nat(i);
 
 	if (!nat)
 		return 0;
-	if (!(i->status & IPS_SRC_NAT_DONE))
-		return 0;
+
 	if ((clean->l3proto && nf_ct_l3num(i) != clean->l3proto) ||
 	    (clean->l4proto && nf_ct_protonum(i) != clean->l4proto))
 		return 0;
 
-	if (clean->hash) {
-		spin_lock_bh(&nf_nat_lock);
-		hlist_del_rcu(&nat->bysource);
-		spin_unlock_bh(&nf_nat_lock);
-	} else {
-		memset(nat, 0, sizeof(*nat));
-		i->status &= ~(IPS_NAT_MASK | IPS_NAT_DONE_MASK |
-			       IPS_SEQ_ADJUST);
-	}
-	return 0;
+	return i->status & IPS_NAT_MASK ? 1 : 0;
 }
 
 static void nf_nat_l4proto_clean(u8 l3proto, u8 l4proto)
@@ -505,16 +494,8 @@ static void nf_nat_l4proto_clean(u8 l3proto, u8 l4proto)
 	struct net *net;
 
 	rtnl_lock();
-	/* Step 1 - remove from bysource hash */
-	clean.hash = true;
 	for_each_net(net)
-		nf_ct_iterate_cleanup(net, nf_nat_proto_clean, &clean);
-	synchronize_rcu();
-
-	/* Step 2 - clean NAT section */
-	clean.hash = false;
-	for_each_net(net)
-		nf_ct_iterate_cleanup(net, nf_nat_proto_clean, &clean);
+		nf_ct_iterate_cleanup(net, nf_nat_proto_remove, &clean);
 	rtnl_unlock();
 }
 
@@ -526,16 +507,9 @@ static void nf_nat_l3proto_clean(u8 l3proto)
 	struct net *net;
 
 	rtnl_lock();
-	/* Step 1 - remove from bysource hash */
-	clean.hash = true;
-	for_each_net(net)
-		nf_ct_iterate_cleanup(net, nf_nat_proto_clean, &clean);
-	synchronize_rcu();
 
-	/* Step 2 - clean NAT section */
-	clean.hash = false;
 	for_each_net(net)
-		nf_ct_iterate_cleanup(net, nf_nat_proto_clean, &clean);
+		nf_ct_iterate_cleanup(net, nf_nat_proto_remove, &clean);
 	rtnl_unlock();
 }
 
@@ -773,7 +747,7 @@ static void __net_exit nf_nat_net_exit(struct net *net)
 {
 	struct nf_nat_proto_clean clean = {};
 
-	nf_ct_iterate_cleanup(net, &nf_nat_proto_clean, &clean);
+	nf_ct_iterate_cleanup(net, &nf_nat_proto_remove, &clean);
 	synchronize_rcu();
 	nf_ct_free_hashtable(net->ct.nat_bysource, net->ct.nat_htable_size);
 }
diff --git a/net/netfilter/nfnetlink_acct.c b/net/netfilter/nfnetlink_acct.c
index 589d686f0b4..dc3fd5d4446 100644
--- a/net/netfilter/nfnetlink_acct.c
+++ b/net/netfilter/nfnetlink_acct.c
@@ -49,6 +49,8 @@ nfnl_acct_new(struct sock *nfnl, struct sk_buff *skb,
 		return -EINVAL;
 
 	acct_name = nla_data(tb[NFACCT_NAME]);
+	if (strlen(acct_name) == 0)
+		return -EINVAL;
 
 	list_for_each_entry(nfacct, &nfnl_acct_list, head) {
 		if (strncmp(nfacct->name, acct_name, NFACCT_NAME_MAX) != 0)
diff --git a/net/netfilter/nfnetlink_queue_core.c b/net/netfilter/nfnetlink_queue_core.c
index 858fd52c104..42680b2baa1 100644
--- a/net/netfilter/nfnetlink_queue_core.c
+++ b/net/netfilter/nfnetlink_queue_core.c
@@ -112,7 +112,7 @@ instance_create(u_int16_t queue_num, int portid)
 	inst->queue_num = queue_num;
 	inst->peer_portid = portid;
 	inst->queue_maxlen = NFQNL_QMAX_DEFAULT;
-	inst->copy_range = 0xfffff;
+	inst->copy_range = 0xffff;
 	inst->copy_mode = NFQNL_COPY_NONE;
 	spin_lock_init(&inst->lock);
 	INIT_LIST_HEAD(&inst->queue_list);
@@ -1062,8 +1062,10 @@ static int __init nfnetlink_queue_init(void)
 
 #ifdef CONFIG_PROC_FS
 	if (!proc_create("nfnetlink_queue", 0440,
-			 proc_net_netfilter, &nfqnl_file_ops))
+			 proc_net_netfilter, &nfqnl_file_ops)) {
+		status = -ENOMEM;
 		goto cleanup_subsys;
+	}
 #endif
 
 	register_netdevice_notifier(&nfqnl_dev_notifier);
diff --git a/net/netlink/genetlink.c b/net/netlink/genetlink.c
index f2aabb6f410..5a55be3f17a 100644
--- a/net/netlink/genetlink.c
+++ b/net/netlink/genetlink.c
@@ -142,6 +142,7 @@ int genl_register_mc_group(struct genl_family *family,
 	int err = 0;
 
 	BUG_ON(grp->name[0] == '\0');
+	BUG_ON(memchr(grp->name, '\0', GENL_NAMSIZ) == NULL);
 
 	genl_lock();
 
diff --git a/net/netrom/af_netrom.c b/net/netrom/af_netrom.c
index d1fa1d9ffd2..103bd704b5f 100644
--- a/net/netrom/af_netrom.c
+++ b/net/netrom/af_netrom.c
@@ -1173,6 +1173,7 @@ static int nr_recvmsg(struct kiocb *iocb, struct socket *sock,
 	}
 
 	if (sax != NULL) {
+		memset(sax, 0, sizeof(*sax));
 		sax->sax25_family = AF_NETROM;
 		skb_copy_from_linear_data_offset(skb, 7, sax->sax25_call.ax25_call,
 			      AX25_ADDR_LEN);
diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
index b530afadd76..ee25f25f0cd 100644
--- a/net/nfc/llcp/llcp.c
+++ b/net/nfc/llcp/llcp.c
@@ -107,8 +107,6 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen,
 				accept_sk->sk_state_change(sk);
 
 				bh_unlock_sock(accept_sk);
-
-				sock_orphan(accept_sk);
 			}
 
 			if (listen == true) {
@@ -134,8 +132,6 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen,
 
 		bh_unlock_sock(sk);
 
-		sock_orphan(sk);
-
 		sk_del_node_init(sk);
 	}
 
@@ -164,8 +160,6 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen,
 
 		bh_unlock_sock(sk);
 
-		sock_orphan(sk);
-
 		sk_del_node_init(sk);
 	}
 
@@ -827,7 +821,6 @@ static void nfc_llcp_recv_ui(struct nfc_llcp_local *local,
 		skb_get(skb);
 	} else {
 		pr_err("Receive queue is full\n");
-		kfree_skb(skb);
 	}
 
 	nfc_llcp_sock_put(llcp_sock);
@@ -1028,7 +1021,6 @@ static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
 			skb_get(skb);
 		} else {
 			pr_err("Receive queue is full\n");
-			kfree_skb(skb);
 		}
 	}
 
diff --git a/net/nfc/llcp/sock.c b/net/nfc/llcp/sock.c
index 5c7cdf3f2a8..6c94447ec41 100644
--- a/net/nfc/llcp/sock.c
+++ b/net/nfc/llcp/sock.c
@@ -270,7 +270,9 @@ struct sock *nfc_llcp_accept_dequeue(struct sock *parent,
 		}
 
 		if (sk->sk_state == LLCP_CONNECTED || !newsock) {
-			nfc_llcp_accept_unlink(sk);
+			list_del_init(&lsk->accept_queue);
+			sock_put(sk);
+
 			if (newsock)
 				sock_graft(sk, newsock);
 
@@ -464,8 +466,6 @@ static int llcp_sock_release(struct socket *sock)
 			nfc_llcp_accept_unlink(accept_sk);
 
 			release_sock(accept_sk);
-
-			sock_orphan(accept_sk);
 		}
 	}
 
@@ -646,6 +646,8 @@ static int llcp_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
 
 	pr_debug("%p %zu\n", sk, len);
 
+	msg->msg_namelen = 0;
+
 	lock_sock(sk);
 
 	if (sk->sk_state == LLCP_CLOSED &&
@@ -691,6 +693,7 @@ static int llcp_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
 
 		pr_debug("Datagram socket %d %d\n", ui_cb->dsap, ui_cb->ssap);
 
+		memset(sockaddr, 0, sizeof(*sockaddr));
 		sockaddr->sa_family = AF_NFC;
 		sockaddr->nfc_protocol = NFC_PROTO_NFC_DEP;
 		sockaddr->dsap = ui_cb->dsap;
diff --git a/net/openvswitch/datapath.c b/net/openvswitch/datapath.c
index a4b724708a1..6980c3e6f06 100644
--- a/net/openvswitch/datapath.c
+++ b/net/openvswitch/datapath.c
@@ -1593,10 +1593,8 @@ struct sk_buff *ovs_vport_cmd_build_info(struct vport *vport, u32 portid,
 		return ERR_PTR(-ENOMEM);
 
 	retval = ovs_vport_cmd_fill_info(vport, skb, portid, seq, 0, cmd);
-	if (retval < 0) {
-		kfree_skb(skb);
-		return ERR_PTR(retval);
-	}
+	BUG_ON(retval < 0);
+
 	return skb;
 }
 
@@ -1726,24 +1724,32 @@ static int ovs_vport_cmd_set(struct sk_buff *skb, struct genl_info *info)
 	    nla_get_u32(a[OVS_VPORT_ATTR_TYPE]) != vport->ops->type)
 		err = -EINVAL;
 
+	reply = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+	if (!reply) {
+		err = -ENOMEM;
+		goto exit_unlock;
+	}
+
 	if (!err && a[OVS_VPORT_ATTR_OPTIONS])
 		err = ovs_vport_set_options(vport, a[OVS_VPORT_ATTR_OPTIONS]);
 	if (err)
-		goto exit_unlock;
+		goto exit_free;
+
 	if (a[OVS_VPORT_ATTR_UPCALL_PID])
 		vport->upcall_portid = nla_get_u32(a[OVS_VPORT_ATTR_UPCALL_PID]);
 
-	reply = ovs_vport_cmd_build_info(vport, info->snd_portid, info->snd_seq,
-					 OVS_VPORT_CMD_NEW);
-	if (IS_ERR(reply)) {
-		netlink_set_err(sock_net(skb->sk)->genl_sock, 0,
-				ovs_dp_vport_multicast_group.id, PTR_ERR(reply));
-		goto exit_unlock;
-	}
+	err = ovs_vport_cmd_fill_info(vport, reply, info->snd_portid,
+				      info->snd_seq, 0, OVS_VPORT_CMD_NEW);
+	BUG_ON(err < 0);
 
 	genl_notify(reply, genl_info_net(info), info->snd_portid,
 		    ovs_dp_vport_multicast_group.id, info->nlhdr, GFP_KERNEL);
 
+	rtnl_unlock();
+	return 0;
+
+exit_free:
+	kfree_skb(reply);
 exit_unlock:
 	rtnl_unlock();
 	return err;
diff --git a/net/openvswitch/flow.c b/net/openvswitch/flow.c
index fe0e4215c73..67a2b783fe7 100644
--- a/net/openvswitch/flow.c
+++ b/net/openvswitch/flow.c
@@ -795,9 +795,9 @@ void ovs_flow_tbl_insert(struct flow_table *table, struct sw_flow *flow)
 
 void ovs_flow_tbl_remove(struct flow_table *table, struct sw_flow *flow)
 {
+	BUG_ON(table->count == 0);
 	hlist_del_rcu(&flow->hash_node[table->node_ver]);
 	table->count--;
-	BUG_ON(table->count < 0);
 }
 
 /* The size of the argument for each %OVS_KEY_ATTR_* Netlink attribute.  */
diff --git a/net/rose/af_rose.c b/net/rose/af_rose.c
index cf68e6e4054..9c834745159 100644
--- a/net/rose/af_rose.c
+++ b/net/rose/af_rose.c
@@ -1253,6 +1253,7 @@ static int rose_recvmsg(struct kiocb *iocb, struct socket *sock,
 	skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied);
 
 	if (srose != NULL) {
+		memset(srose, 0, msg->msg_namelen);
 		srose->srose_family = AF_ROSE;
 		srose->srose_addr   = rose->dest_addr;
 		srose->srose_call   = rose->dest_call;
diff --git a/net/sched/cls_fw.c b/net/sched/cls_fw.c
index 1135d8227f9..9b97172db84 100644
--- a/net/sched/cls_fw.c
+++ b/net/sched/cls_fw.c
@@ -204,7 +204,6 @@ fw_change_attrs(struct net *net, struct tcf_proto *tp, struct fw_filter *f,
 	if (err < 0)
 		return err;
 
-	err = -EINVAL;
 	if (tb[TCA_FW_CLASSID]) {
 		f->res.classid = nla_get_u32(tb[TCA_FW_CLASSID]);
 		tcf_bind_filter(tp, &f->res, base);
@@ -218,6 +217,7 @@ fw_change_attrs(struct net *net, struct tcf_proto *tp, struct fw_filter *f,
 	}
 #endif /* CONFIG_NET_CLS_IND */
 
+	err = -EINVAL;
 	if (tb[TCA_FW_MASK]) {
 		mask = nla_get_u32(tb[TCA_FW_MASK]);
 		if (mask != head->mask)
diff --git a/net/sched/sch_cbq.c b/net/sched/sch_cbq.c
index 13aa47aa2ff..1bc210ffcba 100644
--- a/net/sched/sch_cbq.c
+++ b/net/sched/sch_cbq.c
@@ -962,8 +962,11 @@ cbq_dequeue(struct Qdisc *sch)
 		cbq_update(q);
 		if ((incr -= incr2) < 0)
 			incr = 0;
+		q->now += incr;
+	} else {
+		if (now > q->now)
+			q->now = now;
 	}
-	q->now += incr;
 	q->now_rt = now;
 
 	for (;;) {
diff --git a/net/sched/sch_choke.c b/net/sched/sch_choke.c
index cc37dd52ecf..ef53ab8d0aa 100644
--- a/net/sched/sch_choke.c
+++ b/net/sched/sch_choke.c
@@ -80,7 +80,7 @@ struct choke_sched_data {
 /* deliver a random number between 0 and N - 1 */
 static u32 random_N(unsigned int N)
 {
-	return reciprocal_divide(random32(), N);
+	return reciprocal_divide(prandom_u32(), N);
 }
 
 /* number of elements in queue including holes */
diff --git a/net/sched/sch_fq_codel.c b/net/sched/sch_fq_codel.c
index 4e606fcb253..55786283a3d 100644
--- a/net/sched/sch_fq_codel.c
+++ b/net/sched/sch_fq_codel.c
@@ -195,7 +195,7 @@ static int fq_codel_enqueue(struct sk_buff *skb, struct Qdisc *sch)
 		flow->deficit = q->quantum;
 		flow->dropped = 0;
 	}
-	if (++sch->q.qlen < sch->limit)
+	if (++sch->q.qlen <= sch->limit)
 		return NET_XMIT_SUCCESS;
 
 	q->drop_overlimit++;
diff --git a/net/sched/sch_generic.c b/net/sched/sch_generic.c
index ffad48109a2..eac7e0ee23c 100644
--- a/net/sched/sch_generic.c
+++ b/net/sched/sch_generic.c
@@ -904,7 +904,7 @@ void psched_ratecfg_precompute(struct psched_ratecfg *r, u32 rate)
 	u64 mult;
 	int shift;
 
-	r->rate_bps = rate << 3;
+	r->rate_bps = (u64)rate << 3;
 	r->shift = 0;
 	r->mult = 1;
 	/*
diff --git a/net/sctp/associola.c b/net/sctp/associola.c
index d2709e2b7be..fa261a3594c 100644
--- a/net/sctp/associola.c
+++ b/net/sctp/associola.c
@@ -66,13 +66,6 @@ static void sctp_assoc_bh_rcv(struct work_struct *work);
 static void sctp_assoc_free_asconf_acks(struct sctp_association *asoc);
 static void sctp_assoc_free_asconf_queue(struct sctp_association *asoc);
 
-/* Keep track of the new idr low so that we don't re-use association id
- * numbers too fast.  It is protected by they idr spin lock is in the
- * range of 1 - INT_MAX.
- */
-static u32 idr_low = 1;
-
-
 /* 1st Level Abstractions. */
 
 /* Initialize a new association from provided memory. */
@@ -1601,13 +1594,8 @@ int sctp_assoc_set_id(struct sctp_association *asoc, gfp_t gfp)
 	if (preload)
 		idr_preload(gfp);
 	spin_lock_bh(&sctp_assocs_id_lock);
-	/* 0 is not a valid id, idr_low is always >= 1 */
-	ret = idr_alloc(&sctp_assocs_id, asoc, idr_low, 0, GFP_NOWAIT);
-	if (ret >= 0) {
-		idr_low = ret + 1;
-		if (idr_low == INT_MAX)
-			idr_low = 1;
-	}
+	/* 0 is not a valid assoc_id, must be >= 1 */
+	ret = idr_alloc_cyclic(&sctp_assocs_id, asoc, 1, 0, GFP_NOWAIT);
 	spin_unlock_bh(&sctp_assocs_id_lock);
 	if (preload)
 		idr_preload_end();
diff --git a/net/sctp/protocol.c b/net/sctp/protocol.c
index 1c2e46cb919..eaee00c6113 100644
--- a/net/sctp/protocol.c
+++ b/net/sctp/protocol.c
@@ -1403,7 +1403,7 @@ SCTP_STATIC __init int sctp_init(void)
 
 	/* Allocate and initialize the endpoint hash table.  */
 	sctp_ep_hashsize = 64;
-	sctp_ep_hashtable = (struct sctp_hashbucket *)
+	sctp_ep_hashtable =
 		kmalloc(64 * sizeof(struct sctp_hashbucket), GFP_KERNEL);
 	if (!sctp_ep_hashtable) {
 		pr_err("Failed endpoint_hash alloc\n");
diff --git a/net/sunrpc/Kconfig b/net/sunrpc/Kconfig
index 516fe2caac2..241b54f3020 100644
--- a/net/sunrpc/Kconfig
+++ b/net/sunrpc/Kconfig
@@ -3,6 +3,7 @@ config SUNRPC
 
 config SUNRPC_GSS
 	tristate
+	select OID_REGISTRY
 
 config SUNRPC_BACKCHANNEL
 	bool
@@ -24,7 +25,6 @@ config SUNRPC_XPRT_RDMA
 config SUNRPC_SWAP
 	bool
 	depends on SUNRPC
-	select NETVM
 
 config RPCSEC_GSS_KRB5
 	tristate "Secure RPC: Kerberos V mechanism"
diff --git a/net/sunrpc/auth.c b/net/sunrpc/auth.c
index f5294047df7..ed2fdd210c0 100644
--- a/net/sunrpc/auth.c
+++ b/net/sunrpc/auth.c
@@ -82,7 +82,7 @@ MODULE_PARM_DESC(auth_hashtable_size, "RPC credential cache hashtable size");
 
 static u32
 pseudoflavor_to_flavor(u32 flavor) {
-	if (flavor >= RPC_AUTH_MAXFLAVOR)
+	if (flavor > RPC_AUTH_MAXFLAVOR)
 		return RPC_AUTH_GSS;
 	return flavor;
 }
@@ -124,6 +124,79 @@ rpcauth_unregister(const struct rpc_authops *ops)
 EXPORT_SYMBOL_GPL(rpcauth_unregister);
 
 /**
+ * rpcauth_get_pseudoflavor - check if security flavor is supported
+ * @flavor: a security flavor
+ * @info: a GSS mech OID, quality of protection, and service value
+ *
+ * Verifies that an appropriate kernel module is available or already loaded.
+ * Returns an equivalent pseudoflavor, or RPC_AUTH_MAXFLAVOR if "flavor" is
+ * not supported locally.
+ */
+rpc_authflavor_t
+rpcauth_get_pseudoflavor(rpc_authflavor_t flavor, struct rpcsec_gss_info *info)
+{
+	const struct rpc_authops *ops;
+	rpc_authflavor_t pseudoflavor;
+
+	ops = auth_flavors[flavor];
+	if (ops == NULL)
+		request_module("rpc-auth-%u", flavor);
+	spin_lock(&rpc_authflavor_lock);
+	ops = auth_flavors[flavor];
+	if (ops == NULL || !try_module_get(ops->owner)) {
+		spin_unlock(&rpc_authflavor_lock);
+		return RPC_AUTH_MAXFLAVOR;
+	}
+	spin_unlock(&rpc_authflavor_lock);
+
+	pseudoflavor = flavor;
+	if (ops->info2flavor != NULL)
+		pseudoflavor = ops->info2flavor(info);
+
+	module_put(ops->owner);
+	return pseudoflavor;
+}
+EXPORT_SYMBOL_GPL(rpcauth_get_pseudoflavor);
+
+/**
+ * rpcauth_get_gssinfo - find GSS tuple matching a GSS pseudoflavor
+ * @pseudoflavor: GSS pseudoflavor to match
+ * @info: rpcsec_gss_info structure to fill in
+ *
+ * Returns zero and fills in "info" if pseudoflavor matches a
+ * supported mechanism.
+ */
+int
+rpcauth_get_gssinfo(rpc_authflavor_t pseudoflavor, struct rpcsec_gss_info *info)
+{
+	rpc_authflavor_t flavor = pseudoflavor_to_flavor(pseudoflavor);
+	const struct rpc_authops *ops;
+	int result;
+
+	if (flavor >= RPC_AUTH_MAXFLAVOR)
+		return -EINVAL;
+
+	ops = auth_flavors[flavor];
+	if (ops == NULL)
+		request_module("rpc-auth-%u", flavor);
+	spin_lock(&rpc_authflavor_lock);
+	ops = auth_flavors[flavor];
+	if (ops == NULL || !try_module_get(ops->owner)) {
+		spin_unlock(&rpc_authflavor_lock);
+		return -ENOENT;
+	}
+	spin_unlock(&rpc_authflavor_lock);
+
+	result = -ENOENT;
+	if (ops->flavor2info != NULL)
+		result = ops->flavor2info(pseudoflavor, info);
+
+	module_put(ops->owner);
+	return result;
+}
+EXPORT_SYMBOL_GPL(rpcauth_get_gssinfo);
+
+/**
  * rpcauth_list_flavors - discover registered flavors and pseudoflavors
  * @array: array to fill in
  * @size: size of "array"
diff --git a/net/sunrpc/auth_gss/auth_gss.c b/net/sunrpc/auth_gss/auth_gss.c
index 5257d2982ba..51415b07174 100644
--- a/net/sunrpc/auth_gss/auth_gss.c
+++ b/net/sunrpc/auth_gss/auth_gss.c
@@ -1641,6 +1641,8 @@ static const struct rpc_authops authgss_ops = {
 	.pipes_create	= gss_pipes_dentries_create,
 	.pipes_destroy	= gss_pipes_dentries_destroy,
 	.list_pseudoflavors = gss_mech_list_pseudoflavors,
+	.info2flavor	= gss_mech_info2flavor,
+	.flavor2info	= gss_mech_flavor2info,
 };
 
 static const struct rpc_credops gss_credops = {
@@ -1733,6 +1735,7 @@ static void __exit exit_rpcsec_gss(void)
 	rcu_barrier(); /* Wait for completion of call_rcu()'s */
 }
 
+MODULE_ALIAS("rpc-auth-6");
 MODULE_LICENSE("GPL");
 module_param_named(expired_cred_retry_delay,
 		   gss_expired_cred_retry_delay,
diff --git a/net/sunrpc/auth_gss/gss_krb5_mech.c b/net/sunrpc/auth_gss/gss_krb5_mech.c
index d3611f11a8d..33255ff889c 100644
--- a/net/sunrpc/auth_gss/gss_krb5_mech.c
+++ b/net/sunrpc/auth_gss/gss_krb5_mech.c
@@ -729,16 +729,19 @@ static const struct gss_api_ops gss_kerberos_ops = {
 static struct pf_desc gss_kerberos_pfs[] = {
 	[0] = {
 		.pseudoflavor = RPC_AUTH_GSS_KRB5,
+		.qop = GSS_C_QOP_DEFAULT,
 		.service = RPC_GSS_SVC_NONE,
 		.name = "krb5",
 	},
 	[1] = {
 		.pseudoflavor = RPC_AUTH_GSS_KRB5I,
+		.qop = GSS_C_QOP_DEFAULT,
 		.service = RPC_GSS_SVC_INTEGRITY,
 		.name = "krb5i",
 	},
 	[2] = {
 		.pseudoflavor = RPC_AUTH_GSS_KRB5P,
+		.qop = GSS_C_QOP_DEFAULT,
 		.service = RPC_GSS_SVC_PRIVACY,
 		.name = "krb5p",
 	},
@@ -750,11 +753,12 @@ MODULE_ALIAS("rpc-auth-gss-krb5p");
 MODULE_ALIAS("rpc-auth-gss-390003");
 MODULE_ALIAS("rpc-auth-gss-390004");
 MODULE_ALIAS("rpc-auth-gss-390005");
+MODULE_ALIAS("rpc-auth-gss-1.2.840.113554.1.2.2");
 
 static struct gss_api_mech gss_kerberos_mech = {
 	.gm_name	= "krb5",
 	.gm_owner	= THIS_MODULE,
-	.gm_oid		= {9, (void *)"\x2a\x86\x48\x86\xf7\x12\x01\x02\x02"},
+	.gm_oid		= { 9, "\x2a\x86\x48\x86\xf7\x12\x01\x02\x02" },
 	.gm_ops		= &gss_kerberos_ops,
 	.gm_pf_num	= ARRAY_SIZE(gss_kerberos_pfs),
 	.gm_pfs		= gss_kerberos_pfs,
diff --git a/net/sunrpc/auth_gss/gss_krb5_wrap.c b/net/sunrpc/auth_gss/gss_krb5_wrap.c
index 88edec929d7..1da52d1406f 100644
--- a/net/sunrpc/auth_gss/gss_krb5_wrap.c
+++ b/net/sunrpc/auth_gss/gss_krb5_wrap.c
@@ -130,8 +130,8 @@ gss_krb5_make_confounder(char *p, u32 conflen)
 
 	/* initialize to random value */
 	if (i == 0) {
-		i = random32();
-		i = (i << 32) | random32();
+		i = prandom_u32();
+		i = (i << 32) | prandom_u32();
 	}
 
 	switch (conflen) {
diff --git a/net/sunrpc/auth_gss/gss_mech_switch.c b/net/sunrpc/auth_gss/gss_mech_switch.c
index f0f4eee63a3..79881d6e68a 100644
--- a/net/sunrpc/auth_gss/gss_mech_switch.c
+++ b/net/sunrpc/auth_gss/gss_mech_switch.c
@@ -36,6 +36,7 @@
 #include <linux/types.h>
 #include <linux/slab.h>
 #include <linux/module.h>
+#include <linux/oid_registry.h>
 #include <linux/sunrpc/msg_prot.h>
 #include <linux/sunrpc/gss_asn1.h>
 #include <linux/sunrpc/auth_gss.h>
@@ -102,8 +103,13 @@ out:
 	return status;
 }
 
-int
-gss_mech_register(struct gss_api_mech *gm)
+/**
+ * gss_mech_register - register a GSS mechanism
+ * @gm: GSS mechanism handle
+ *
+ * Returns zero if successful, or a negative errno.
+ */
+int gss_mech_register(struct gss_api_mech *gm)
 {
 	int status;
 
@@ -116,11 +122,14 @@ gss_mech_register(struct gss_api_mech *gm)
 	dprintk("RPC:       registered gss mechanism %s\n", gm->gm_name);
 	return 0;
 }
-
 EXPORT_SYMBOL_GPL(gss_mech_register);
 
-void
-gss_mech_unregister(struct gss_api_mech *gm)
+/**
+ * gss_mech_unregister - release a GSS mechanism
+ * @gm: GSS mechanism handle
+ *
+ */
+void gss_mech_unregister(struct gss_api_mech *gm)
 {
 	spin_lock(&registered_mechs_lock);
 	list_del(&gm->gm_list);
@@ -128,18 +137,14 @@ gss_mech_unregister(struct gss_api_mech *gm)
 	dprintk("RPC:       unregistered gss mechanism %s\n", gm->gm_name);
 	gss_mech_free(gm);
 }
-
 EXPORT_SYMBOL_GPL(gss_mech_unregister);
 
-struct gss_api_mech *
-gss_mech_get(struct gss_api_mech *gm)
+static struct gss_api_mech *gss_mech_get(struct gss_api_mech *gm)
 {
 	__module_get(gm->gm_owner);
 	return gm;
 }
 
-EXPORT_SYMBOL_GPL(gss_mech_get);
-
 static struct gss_api_mech *
 _gss_mech_get_by_name(const char *name)
 {
@@ -169,12 +174,16 @@ struct gss_api_mech * gss_mech_get_by_name(const char *name)
 	}
 	return gm;
 }
-EXPORT_SYMBOL_GPL(gss_mech_get_by_name);
 
-struct gss_api_mech *
-gss_mech_get_by_OID(struct xdr_netobj *obj)
+static struct gss_api_mech *gss_mech_get_by_OID(struct rpcsec_gss_oid *obj)
 {
 	struct gss_api_mech	*pos, *gm = NULL;
+	char buf[32];
+
+	if (sprint_oid(obj->data, obj->len, buf, sizeof(buf)) < 0)
+		return NULL;
+	dprintk("RPC:       %s(%s)\n", __func__, buf);
+	request_module("rpc-auth-gss-%s", buf);
 
 	spin_lock(&registered_mechs_lock);
 	list_for_each_entry(pos, &registered_mechs, gm_list) {
@@ -188,11 +197,8 @@ gss_mech_get_by_OID(struct xdr_netobj *obj)
 	}
 	spin_unlock(&registered_mechs_lock);
 	return gm;
-
 }
 
-EXPORT_SYMBOL_GPL(gss_mech_get_by_OID);
-
 static inline int
 mech_supports_pseudoflavor(struct gss_api_mech *gm, u32 pseudoflavor)
 {
@@ -237,8 +243,6 @@ gss_mech_get_by_pseudoflavor(u32 pseudoflavor)
 	return gm;
 }
 
-EXPORT_SYMBOL_GPL(gss_mech_get_by_pseudoflavor);
-
 /**
  * gss_mech_list_pseudoflavors - Discover registered GSS pseudoflavors
  * @array: array to fill in
@@ -268,19 +272,82 @@ int gss_mech_list_pseudoflavors(rpc_authflavor_t *array_ptr, int size)
 	return i;
 }
 
-u32
-gss_svc_to_pseudoflavor(struct gss_api_mech *gm, u32 service)
+/**
+ * gss_svc_to_pseudoflavor - map a GSS service number to a pseudoflavor
+ * @gm: GSS mechanism handle
+ * @qop: GSS quality-of-protection value
+ * @service: GSS service value
+ *
+ * Returns a matching security flavor, or RPC_AUTH_MAXFLAVOR if none is found.
+ */
+rpc_authflavor_t gss_svc_to_pseudoflavor(struct gss_api_mech *gm, u32 qop,
+					 u32 service)
 {
 	int i;
 
 	for (i = 0; i < gm->gm_pf_num; i++) {
-		if (gm->gm_pfs[i].service == service) {
+		if (gm->gm_pfs[i].qop == qop &&
+		    gm->gm_pfs[i].service == service) {
 			return gm->gm_pfs[i].pseudoflavor;
 		}
 	}
-	return RPC_AUTH_MAXFLAVOR; /* illegal value */
+	return RPC_AUTH_MAXFLAVOR;
+}
+
+/**
+ * gss_mech_info2flavor - look up a pseudoflavor given a GSS tuple
+ * @info: a GSS mech OID, quality of protection, and service value
+ *
+ * Returns a matching pseudoflavor, or RPC_AUTH_MAXFLAVOR if the tuple is
+ * not supported.
+ */
+rpc_authflavor_t gss_mech_info2flavor(struct rpcsec_gss_info *info)
+{
+	rpc_authflavor_t pseudoflavor;
+	struct gss_api_mech *gm;
+
+	gm = gss_mech_get_by_OID(&info->oid);
+	if (gm == NULL)
+		return RPC_AUTH_MAXFLAVOR;
+
+	pseudoflavor = gss_svc_to_pseudoflavor(gm, info->qop, info->service);
+
+	gss_mech_put(gm);
+	return pseudoflavor;
+}
+
+/**
+ * gss_mech_flavor2info - look up a GSS tuple for a given pseudoflavor
+ * @pseudoflavor: GSS pseudoflavor to match
+ * @info: rpcsec_gss_info structure to fill in
+ *
+ * Returns zero and fills in "info" if pseudoflavor matches a
+ * supported mechanism.  Otherwise a negative errno is returned.
+ */
+int gss_mech_flavor2info(rpc_authflavor_t pseudoflavor,
+			 struct rpcsec_gss_info *info)
+{
+	struct gss_api_mech *gm;
+	int i;
+
+	gm = gss_mech_get_by_pseudoflavor(pseudoflavor);
+	if (gm == NULL)
+		return -ENOENT;
+
+	for (i = 0; i < gm->gm_pf_num; i++) {
+		if (gm->gm_pfs[i].pseudoflavor == pseudoflavor) {
+			memcpy(info->oid.data, gm->gm_oid.data, gm->gm_oid.len);
+			info->oid.len = gm->gm_oid.len;
+			info->qop = gm->gm_pfs[i].qop;
+			info->service = gm->gm_pfs[i].service;
+			gss_mech_put(gm);
+			return 0;
+		}
+	}
+
+	gss_mech_put(gm);
+	return -ENOENT;
 }
-EXPORT_SYMBOL_GPL(gss_svc_to_pseudoflavor);
 
 u32
 gss_pseudoflavor_to_service(struct gss_api_mech *gm, u32 pseudoflavor)
@@ -294,8 +361,6 @@ gss_pseudoflavor_to_service(struct gss_api_mech *gm, u32 pseudoflavor)
 	return 0;
 }
 
-EXPORT_SYMBOL_GPL(gss_pseudoflavor_to_service);
-
 char *
 gss_service_to_auth_domain_name(struct gss_api_mech *gm, u32 service)
 {
@@ -308,8 +373,6 @@ gss_service_to_auth_domain_name(struct gss_api_mech *gm, u32 service)
 	return NULL;
 }
 
-EXPORT_SYMBOL_GPL(gss_service_to_auth_domain_name);
-
 void
 gss_mech_put(struct gss_api_mech * gm)
 {
@@ -317,8 +380,6 @@ gss_mech_put(struct gss_api_mech * gm)
 		module_put(gm->gm_owner);
 }
 
-EXPORT_SYMBOL_GPL(gss_mech_put);
-
 /* The mech could probably be determined from the token instead, but it's just
  * as easy for now to pass it in. */
 int
diff --git a/net/sunrpc/auth_gss/svcauth_gss.c b/net/sunrpc/auth_gss/svcauth_gss.c
index 5ead6055089..c3ba570222d 100644
--- a/net/sunrpc/auth_gss/svcauth_gss.c
+++ b/net/sunrpc/auth_gss/svcauth_gss.c
@@ -1220,7 +1220,9 @@ svcauth_gss_accept(struct svc_rqst *rqstp, __be32 *authp)
 		svcdata->rsci = rsci;
 		cache_get(&rsci->h);
 		rqstp->rq_cred.cr_flavor = gss_svc_to_pseudoflavor(
-					rsci->mechctx->mech_type, gc->gc_svc);
+					rsci->mechctx->mech_type,
+					GSS_C_QOP_DEFAULT,
+					gc->gc_svc);
 		ret = SVC_OK;
 		goto out;
 	}
diff --git a/net/sunrpc/cache.c b/net/sunrpc/cache.c
index 25d58e76601..ce2d180d05a 100644
--- a/net/sunrpc/cache.c
+++ b/net/sunrpc/cache.c
@@ -1208,7 +1208,6 @@ EXPORT_SYMBOL_GPL(sunrpc_cache_pipe_upcall);
  * key and content are both parsed by cache
  */
 
-#define isodigit(c) (isdigit(c) && c <= '7')
 int qword_get(char **bpp, char *dest, int bufsize)
 {
 	/* return bytes copied, or -1 on error */
diff --git a/net/sunrpc/clnt.c b/net/sunrpc/clnt.c
index dcc446e7fbf..d259fa96692 100644
--- a/net/sunrpc/clnt.c
+++ b/net/sunrpc/clnt.c
@@ -304,10 +304,8 @@ static struct rpc_clnt * rpc_new_client(const struct rpc_create_args *args, stru
 	err = rpciod_up();
 	if (err)
 		goto out_no_rpciod;
-	err = -EINVAL;
-	if (!xprt)
-		goto out_no_xprt;
 
+	err = -EINVAL;
 	if (args->version >= program->nrvers)
 		goto out_err;
 	version = program->version[args->version];
@@ -382,10 +380,9 @@ out_no_principal:
 out_no_stats:
 	kfree(clnt);
 out_err:
-	xprt_put(xprt);
-out_no_xprt:
 	rpciod_down();
 out_no_rpciod:
+	xprt_put(xprt);
 	return ERR_PTR(err);
 }
 
@@ -414,6 +411,8 @@ struct rpc_clnt *rpc_create(struct rpc_create_args *args)
 	};
 	char servername[48];
 
+	if (args->flags & RPC_CLNT_CREATE_INFINITE_SLOTS)
+		xprtargs.flags |= XPRT_CREATE_INFINITE_SLOTS;
 	/*
 	 * If the caller chooses not to specify a hostname, whip
 	 * up a string representation of the passed-in address.
@@ -512,7 +511,7 @@ static struct rpc_clnt *__rpc_clone_client(struct rpc_create_args *args,
 	new = rpc_new_client(args, xprt);
 	if (IS_ERR(new)) {
 		err = PTR_ERR(new);
-		goto out_put;
+		goto out_err;
 	}
 
 	atomic_inc(&clnt->cl_count);
@@ -525,8 +524,6 @@ static struct rpc_clnt *__rpc_clone_client(struct rpc_create_args *args,
 	new->cl_chatty = clnt->cl_chatty;
 	return new;
 
-out_put:
-	xprt_put(xprt);
 out_err:
 	dprintk("RPC:       %s: returned error %d\n", __func__, err);
 	return ERR_PTR(err);
@@ -1306,6 +1303,8 @@ call_reserve(struct rpc_task *task)
 	xprt_reserve(task);
 }
 
+static void call_retry_reserve(struct rpc_task *task);
+
 /*
  * 1b.	Grok the result of xprt_reserve()
  */
@@ -1347,7 +1346,7 @@ call_reserveresult(struct rpc_task *task)
 	case -ENOMEM:
 		rpc_delay(task, HZ >> 2);
 	case -EAGAIN:	/* woken up; retry */
-		task->tk_action = call_reserve;
+		task->tk_action = call_retry_reserve;
 		return;
 	case -EIO:	/* probably a shutdown */
 		break;
@@ -1360,6 +1359,19 @@ call_reserveresult(struct rpc_task *task)
 }
 
 /*
+ * 1c.	Retry reserving an RPC call slot
+ */
+static void
+call_retry_reserve(struct rpc_task *task)
+{
+	dprint_status(task);
+
+	task->tk_status  = 0;
+	task->tk_action  = call_reserveresult;
+	xprt_retry_reserve(task);
+}
+
+/*
  * 2.	Bind and/or refresh the credentials
  */
 static void
@@ -1644,22 +1656,26 @@ call_connect_status(struct rpc_task *task)
 
 	dprint_status(task);
 
-	task->tk_status = 0;
-	if (status >= 0 || status == -EAGAIN) {
-		clnt->cl_stats->netreconn++;
-		task->tk_action = call_transmit;
-		return;
-	}
-
 	trace_rpc_connect_status(task, status);
 	switch (status) {
 		/* if soft mounted, test if we've timed out */
 	case -ETIMEDOUT:
 		task->tk_action = call_timeout;
-		break;
-	default:
-		rpc_exit(task, -EIO);
+		return;
+	case -ECONNREFUSED:
+	case -ECONNRESET:
+	case -ENETUNREACH:
+		if (RPC_IS_SOFTCONN(task))
+			break;
+		/* retry with existing socket, after a delay */
+	case 0:
+	case -EAGAIN:
+		task->tk_status = 0;
+		clnt->cl_stats->netreconn++;
+		task->tk_action = call_transmit;
+		return;
 	}
+	rpc_exit(task, status);
 }
 
 /*
diff --git a/net/sunrpc/sched.c b/net/sunrpc/sched.c
index fb20f25ddec..f8529fc8e54 100644
--- a/net/sunrpc/sched.c
+++ b/net/sunrpc/sched.c
@@ -180,6 +180,8 @@ static void __rpc_add_wait_queue(struct rpc_wait_queue *queue,
 		list_add_tail(&task->u.tk_wait.list, &queue->tasks[0]);
 	task->tk_waitqueue = queue;
 	queue->qlen++;
+	/* barrier matches the read in rpc_wake_up_task_queue_locked() */
+	smp_wmb();
 	rpc_set_queued(task);
 
 	dprintk("RPC: %5u added to queue %p \"%s\"\n",
@@ -430,8 +432,11 @@ static void __rpc_do_wake_up_task(struct rpc_wait_queue *queue, struct rpc_task
  */
 static void rpc_wake_up_task_queue_locked(struct rpc_wait_queue *queue, struct rpc_task *task)
 {
-	if (RPC_IS_QUEUED(task) && task->tk_waitqueue == queue)
-		__rpc_do_wake_up_task(queue, task);
+	if (RPC_IS_QUEUED(task)) {
+		smp_rmb();
+		if (task->tk_waitqueue == queue)
+			__rpc_do_wake_up_task(queue, task);
+	}
 }
 
 /*
diff --git a/net/sunrpc/xprt.c b/net/sunrpc/xprt.c
index b7478d5e7ff..745fca3cfd3 100644
--- a/net/sunrpc/xprt.c
+++ b/net/sunrpc/xprt.c
@@ -948,6 +948,34 @@ void xprt_transmit(struct rpc_task *task)
 	spin_unlock_bh(&xprt->transport_lock);
 }
 
+static void xprt_add_backlog(struct rpc_xprt *xprt, struct rpc_task *task)
+{
+	set_bit(XPRT_CONGESTED, &xprt->state);
+	rpc_sleep_on(&xprt->backlog, task, NULL);
+}
+
+static void xprt_wake_up_backlog(struct rpc_xprt *xprt)
+{
+	if (rpc_wake_up_next(&xprt->backlog) == NULL)
+		clear_bit(XPRT_CONGESTED, &xprt->state);
+}
+
+static bool xprt_throttle_congested(struct rpc_xprt *xprt, struct rpc_task *task)
+{
+	bool ret = false;
+
+	if (!test_bit(XPRT_CONGESTED, &xprt->state))
+		goto out;
+	spin_lock(&xprt->reserve_lock);
+	if (test_bit(XPRT_CONGESTED, &xprt->state)) {
+		rpc_sleep_on(&xprt->backlog, task, NULL);
+		ret = true;
+	}
+	spin_unlock(&xprt->reserve_lock);
+out:
+	return ret;
+}
+
 static struct rpc_rqst *xprt_dynamic_alloc_slot(struct rpc_xprt *xprt, gfp_t gfp_flags)
 {
 	struct rpc_rqst *req = ERR_PTR(-EAGAIN);
@@ -992,7 +1020,7 @@ void xprt_alloc_slot(struct rpc_xprt *xprt, struct rpc_task *task)
 		task->tk_status = -ENOMEM;
 		break;
 	case -EAGAIN:
-		rpc_sleep_on(&xprt->backlog, task, NULL);
+		xprt_add_backlog(xprt, task);
 		dprintk("RPC:       waiting for request slot\n");
 	default:
 		task->tk_status = -EAGAIN;
@@ -1028,7 +1056,7 @@ static void xprt_free_slot(struct rpc_xprt *xprt, struct rpc_rqst *req)
 		memset(req, 0, sizeof(*req));	/* mark unused */
 		list_add(&req->rq_list, &xprt->free);
 	}
-	rpc_wake_up_next(&xprt->backlog);
+	xprt_wake_up_backlog(xprt);
 	spin_unlock(&xprt->reserve_lock);
 }
 
@@ -1092,7 +1120,8 @@ EXPORT_SYMBOL_GPL(xprt_free);
  * xprt_reserve - allocate an RPC request slot
  * @task: RPC task requesting a slot allocation
  *
- * If no more slots are available, place the task on the transport's
+ * If the transport is marked as being congested, or if no more
+ * slots are available, place the task on the transport's
  * backlog queue.
  */
 void xprt_reserve(struct rpc_task *task)
@@ -1107,6 +1136,32 @@ void xprt_reserve(struct rpc_task *task)
 	task->tk_status = -EAGAIN;
 	rcu_read_lock();
 	xprt = rcu_dereference(task->tk_client->cl_xprt);
+	if (!xprt_throttle_congested(xprt, task))
+		xprt->ops->alloc_slot(xprt, task);
+	rcu_read_unlock();
+}
+
+/**
+ * xprt_retry_reserve - allocate an RPC request slot
+ * @task: RPC task requesting a slot allocation
+ *
+ * If no more slots are available, place the task on the transport's
+ * backlog queue.
+ * Note that the only difference with xprt_reserve is that we now
+ * ignore the value of the XPRT_CONGESTED flag.
+ */
+void xprt_retry_reserve(struct rpc_task *task)
+{
+	struct rpc_xprt	*xprt;
+
+	task->tk_status = 0;
+	if (task->tk_rqstp != NULL)
+		return;
+
+	task->tk_timeout = 0;
+	task->tk_status = -EAGAIN;
+	rcu_read_lock();
+	xprt = rcu_dereference(task->tk_client->cl_xprt);
 	xprt->ops->alloc_slot(xprt, task);
 	rcu_read_unlock();
 }
diff --git a/net/sunrpc/xprtsock.c b/net/sunrpc/xprtsock.c
index 3d02130828d..9c2825827de 100644
--- a/net/sunrpc/xprtsock.c
+++ b/net/sunrpc/xprtsock.c
@@ -2207,10 +2207,6 @@ static void xs_tcp_setup_socket(struct work_struct *work)
 		 */
 		xs_tcp_force_close(xprt);
 		break;
-	case -ECONNREFUSED:
-	case -ECONNRESET:
-	case -ENETUNREACH:
-		/* retry with existing socket, after a delay */
 	case 0:
 	case -EINPROGRESS:
 	case -EALREADY:
@@ -2221,6 +2217,10 @@ static void xs_tcp_setup_socket(struct work_struct *work)
 		/* Happens, for instance, if the user specified a link
 		 * local IPv6 address without a scope-id.
 		 */
+	case -ECONNREFUSED:
+	case -ECONNRESET:
+	case -ENETUNREACH:
+		/* retry with existing socket, after a delay */
 		goto out;
 	}
 out_eagain:
@@ -2767,9 +2767,13 @@ static struct rpc_xprt *xs_setup_tcp(struct xprt_create *args)
 	struct rpc_xprt *xprt;
 	struct sock_xprt *transport;
 	struct rpc_xprt *ret;
+	unsigned int max_slot_table_size = xprt_max_tcp_slot_table_entries;
+
+	if (args->flags & XPRT_CREATE_INFINITE_SLOTS)
+		max_slot_table_size = RPC_MAX_SLOT_TABLE_LIMIT;
 
 	xprt = xs_setup_xprt(args, xprt_tcp_slot_table_entries,
-			xprt_max_tcp_slot_table_entries);
+			max_slot_table_size);
 	if (IS_ERR(xprt))
 		return xprt;
 	transport = container_of(xprt, struct sock_xprt, xprt);
diff --git a/net/tipc/socket.c b/net/tipc/socket.c
index a9622b6cd91..515ce38e4f4 100644
--- a/net/tipc/socket.c
+++ b/net/tipc/socket.c
@@ -790,6 +790,7 @@ static void set_orig_addr(struct msghdr *m, struct tipc_msg *msg)
 	if (addr) {
 		addr->family = AF_TIPC;
 		addr->addrtype = TIPC_ADDR_ID;
+		memset(&addr->addr, 0, sizeof(addr->addr));
 		addr->addr.id.ref = msg_origport(msg);
 		addr->addr.id.node = msg_orignode(msg);
 		addr->addr.name.domain = 0;	/* could leave uninitialized */
@@ -904,6 +905,9 @@ static int recv_msg(struct kiocb *iocb, struct socket *sock,
 		goto exit;
 	}
 
+	/* will be updated in set_orig_addr() if needed */
+	m->msg_namelen = 0;
+
 	timeout = sock_rcvtimeo(sk, flags & MSG_DONTWAIT);
 restart:
 
@@ -1013,6 +1017,9 @@ static int recv_stream(struct kiocb *iocb, struct socket *sock,
 		goto exit;
 	}
 
+	/* will be updated in set_orig_addr() if needed */
+	m->msg_namelen = 0;
+
 	target = sock_rcvlowat(sk, flags & MSG_WAITALL, buf_len);
 	timeout = sock_rcvtimeo(sk, flags & MSG_DONTWAIT);
 
diff --git a/net/unix/af_unix.c b/net/unix/af_unix.c
index 51be64f163e..2db702d82e7 100644
--- a/net/unix/af_unix.c
+++ b/net/unix/af_unix.c
@@ -382,7 +382,7 @@ static void unix_sock_destructor(struct sock *sk)
 #endif
 }
 
-static int unix_release_sock(struct sock *sk, int embrion)
+static void unix_release_sock(struct sock *sk, int embrion)
 {
 	struct unix_sock *u = unix_sk(sk);
 	struct path path;
@@ -451,8 +451,6 @@ static int unix_release_sock(struct sock *sk, int embrion)
 
 	if (unix_tot_inflight)
 		unix_gc();		/* Garbage collect fds */
-
-	return 0;
 }
 
 static void init_peercred(struct sock *sk)
@@ -699,9 +697,10 @@ static int unix_release(struct socket *sock)
 	if (!sk)
 		return 0;
 
+	unix_release_sock(sk, 0);
 	sock->sk = NULL;
 
-	return unix_release_sock(sk, 0);
+	return 0;
 }
 
 static int unix_autobind(struct socket *sock)
@@ -1994,7 +1993,7 @@ again:
 			if ((UNIXCB(skb).pid  != siocb->scm->pid) ||
 			    (UNIXCB(skb).cred != siocb->scm->cred))
 				break;
-		} else {
+		} else if (test_bit(SOCK_PASSCRED, &sock->flags)) {
 			/* Copy credentials */
 			scm_set_cred(siocb->scm, UNIXCB(skb).pid, UNIXCB(skb).cred);
 			check_creds = 1;
diff --git a/net/vmw_vsock/af_vsock.c b/net/vmw_vsock/af_vsock.c
index ca511c4f388..7f93e2a42d7 100644
--- a/net/vmw_vsock/af_vsock.c
+++ b/net/vmw_vsock/af_vsock.c
@@ -207,7 +207,7 @@ static struct sock *__vsock_find_bound_socket(struct sockaddr_vm *addr)
 	struct vsock_sock *vsk;
 
 	list_for_each_entry(vsk, vsock_bound_sockets(addr), bound_table)
-		if (vsock_addr_equals_addr_any(addr, &vsk->local_addr))
+		if (addr->svm_port == vsk->local_addr.svm_port)
 			return sk_vsock(vsk);
 
 	return NULL;
@@ -220,8 +220,8 @@ static struct sock *__vsock_find_connected_socket(struct sockaddr_vm *src,
 
 	list_for_each_entry(vsk, vsock_connected_sockets(src, dst),
 			    connected_table) {
-		if (vsock_addr_equals_addr(src, &vsk->remote_addr)
-		    && vsock_addr_equals_addr(dst, &vsk->local_addr)) {
+		if (vsock_addr_equals_addr(src, &vsk->remote_addr) &&
+		    dst->svm_port == vsk->local_addr.svm_port) {
 			return sk_vsock(vsk);
 		}
 	}
@@ -1670,6 +1670,8 @@ vsock_stream_recvmsg(struct kiocb *kiocb,
 	vsk = vsock_sk(sk);
 	err = 0;
 
+	msg->msg_namelen = 0;
+
 	lock_sock(sk);
 
 	if (sk->sk_state != SS_CONNECTED) {
diff --git a/net/vmw_vsock/vmci_transport.c b/net/vmw_vsock/vmci_transport.c
index a70ace83a15..5e04d3d9628 100644
--- a/net/vmw_vsock/vmci_transport.c
+++ b/net/vmw_vsock/vmci_transport.c
@@ -464,19 +464,16 @@ static struct sock *vmci_transport_get_pending(
 	struct vsock_sock *vlistener;
 	struct vsock_sock *vpending;
 	struct sock *pending;
+	struct sockaddr_vm src;
+
+	vsock_addr_init(&src, pkt->dg.src.context, pkt->src_port);
 
 	vlistener = vsock_sk(listener);
 
 	list_for_each_entry(vpending, &vlistener->pending_links,
 			    pending_links) {
-		struct sockaddr_vm src;
-		struct sockaddr_vm dst;
-
-		vsock_addr_init(&src, pkt->dg.src.context, pkt->src_port);
-		vsock_addr_init(&dst, pkt->dg.dst.context, pkt->dst_port);
-
 		if (vsock_addr_equals_addr(&src, &vpending->remote_addr) &&
-		    vsock_addr_equals_addr(&dst, &vpending->local_addr)) {
+		    pkt->dst_port == vpending->local_addr.svm_port) {
 			pending = sk_vsock(vpending);
 			sock_hold(pending);
 			goto found;
@@ -739,10 +736,15 @@ static int vmci_transport_recv_stream_cb(void *data, struct vmci_datagram *dg)
 	 */
 	bh_lock_sock(sk);
 
-	if (!sock_owned_by_user(sk) && sk->sk_state == SS_CONNECTED)
-		vmci_trans(vsk)->notify_ops->handle_notify_pkt(
-				sk, pkt, true, &dst, &src,
-				&bh_process_pkt);
+	if (!sock_owned_by_user(sk)) {
+		/* The local context ID may be out of date, update it. */
+		vsk->local_addr.svm_cid = dst.svm_cid;
+
+		if (sk->sk_state == SS_CONNECTED)
+			vmci_trans(vsk)->notify_ops->handle_notify_pkt(
+					sk, pkt, true, &dst, &src,
+					&bh_process_pkt);
+	}
 
 	bh_unlock_sock(sk);
 
@@ -902,6 +904,9 @@ static void vmci_transport_recv_pkt_work(struct work_struct *work)
 
 	lock_sock(sk);
 
+	/* The local context ID may be out of date. */
+	vsock_sk(sk)->local_addr.svm_cid = pkt->dg.dst.context;
+
 	switch (sk->sk_state) {
 	case SS_LISTEN:
 		vmci_transport_recv_listen(sk, pkt);
@@ -958,6 +963,10 @@ static int vmci_transport_recv_listen(struct sock *sk,
 	pending = vmci_transport_get_pending(sk, pkt);
 	if (pending) {
 		lock_sock(pending);
+
+		/* The local context ID may be out of date. */
+		vsock_sk(pending)->local_addr.svm_cid = pkt->dg.dst.context;
+
 		switch (pending->sk_state) {
 		case SS_CONNECTING:
 			err = vmci_transport_recv_connecting_server(sk,
@@ -1727,6 +1736,8 @@ static int vmci_transport_dgram_dequeue(struct kiocb *kiocb,
 	if (flags & MSG_OOB || flags & MSG_ERRQUEUE)
 		return -EOPNOTSUPP;
 
+	msg->msg_namelen = 0;
+
 	/* Retrieve the head sk_buff from the socket's receive queue. */
 	err = 0;
 	skb = skb_recv_datagram(&vsk->sk, flags, noblock, &err);
@@ -1759,7 +1770,6 @@ static int vmci_transport_dgram_dequeue(struct kiocb *kiocb,
 	if (err)
 		goto out;
 
-	msg->msg_namelen = 0;
 	if (msg->msg_name) {
 		struct sockaddr_vm *vm_addr;
 
diff --git a/net/vmw_vsock/vsock_addr.c b/net/vmw_vsock/vsock_addr.c
index b7df1aea7c5..ec2611b4ea0 100644
--- a/net/vmw_vsock/vsock_addr.c
+++ b/net/vmw_vsock/vsock_addr.c
@@ -64,16 +64,6 @@ bool vsock_addr_equals_addr(const struct sockaddr_vm *addr,
 }
 EXPORT_SYMBOL_GPL(vsock_addr_equals_addr);
 
-bool vsock_addr_equals_addr_any(const struct sockaddr_vm *addr,
-				const struct sockaddr_vm *other)
-{
-	return (addr->svm_cid == VMADDR_CID_ANY ||
-		other->svm_cid == VMADDR_CID_ANY ||
-		addr->svm_cid == other->svm_cid) &&
-	       addr->svm_port == other->svm_port;
-}
-EXPORT_SYMBOL_GPL(vsock_addr_equals_addr_any);
-
 int vsock_addr_cast(const struct sockaddr *addr,
 		    size_t len, struct sockaddr_vm **out_addr)
 {
diff --git a/net/vmw_vsock/vsock_addr.h b/net/vmw_vsock/vsock_addr.h
index cdfbcefdf84..9ccd5316eac 100644
--- a/net/vmw_vsock/vsock_addr.h
+++ b/net/vmw_vsock/vsock_addr.h
@@ -24,8 +24,6 @@ bool vsock_addr_bound(const struct sockaddr_vm *addr);
 void vsock_addr_unbind(struct sockaddr_vm *addr);
 bool vsock_addr_equals_addr(const struct sockaddr_vm *addr,
 			    const struct sockaddr_vm *other);
-bool vsock_addr_equals_addr_any(const struct sockaddr_vm *addr,
-				const struct sockaddr_vm *other);
 int vsock_addr_cast(const struct sockaddr *addr, size_t len,
 		    struct sockaddr_vm **out_addr);
 
diff --git a/net/wireless/core.c b/net/wireless/core.c
index ea4155fe973..6ddf74f0ae1 100644
--- a/net/wireless/core.c
+++ b/net/wireless/core.c
@@ -212,6 +212,39 @@ static void cfg80211_rfkill_poll(struct rfkill *rfkill, void *data)
 	rdev_rfkill_poll(rdev);
 }
 
+void cfg80211_stop_p2p_device(struct cfg80211_registered_device *rdev,
+			      struct wireless_dev *wdev)
+{
+	lockdep_assert_held(&rdev->devlist_mtx);
+	lockdep_assert_held(&rdev->sched_scan_mtx);
+
+	if (WARN_ON(wdev->iftype != NL80211_IFTYPE_P2P_DEVICE))
+		return;
+
+	if (!wdev->p2p_started)
+		return;
+
+	rdev_stop_p2p_device(rdev, wdev);
+	wdev->p2p_started = false;
+
+	rdev->opencount--;
+
+	if (rdev->scan_req && rdev->scan_req->wdev == wdev) {
+		bool busy = work_busy(&rdev->scan_done_wk);
+
+		/*
+		 * If the work isn't pending or running (in which case it would
+		 * be waiting for the lock we hold) the driver didn't properly
+		 * cancel the scan when the interface was removed. In this case
+		 * warn and leak the scan request object to not crash later.
+		 */
+		WARN_ON(!busy);
+
+		rdev->scan_req->aborted = true;
+		___cfg80211_scan_done(rdev, !busy);
+	}
+}
+
 static int cfg80211_rfkill_set_block(void *data, bool blocked)
 {
 	struct cfg80211_registered_device *rdev = data;
@@ -221,7 +254,8 @@ static int cfg80211_rfkill_set_block(void *data, bool blocked)
 		return 0;
 
 	rtnl_lock();
-	mutex_lock(&rdev->devlist_mtx);
+
+	/* read-only iteration need not hold the devlist_mtx */
 
 	list_for_each_entry(wdev, &rdev->wdev_list, list) {
 		if (wdev->netdev) {
@@ -231,18 +265,18 @@ static int cfg80211_rfkill_set_block(void *data, bool blocked)
 		/* otherwise, check iftype */
 		switch (wdev->iftype) {
 		case NL80211_IFTYPE_P2P_DEVICE:
-			if (!wdev->p2p_started)
-				break;
-			rdev_stop_p2p_device(rdev, wdev);
-			wdev->p2p_started = false;
-			rdev->opencount--;
+			/* but this requires it */
+			mutex_lock(&rdev->devlist_mtx);
+			mutex_lock(&rdev->sched_scan_mtx);
+			cfg80211_stop_p2p_device(rdev, wdev);
+			mutex_unlock(&rdev->sched_scan_mtx);
+			mutex_unlock(&rdev->devlist_mtx);
 			break;
 		default:
 			break;
 		}
 	}
 
-	mutex_unlock(&rdev->devlist_mtx);
 	rtnl_unlock();
 
 	return 0;
@@ -745,17 +779,13 @@ static void wdev_cleanup_work(struct work_struct *work)
 	wdev = container_of(work, struct wireless_dev, cleanup_work);
 	rdev = wiphy_to_dev(wdev->wiphy);
 
-	cfg80211_lock_rdev(rdev);
+	mutex_lock(&rdev->sched_scan_mtx);
 
 	if (WARN_ON(rdev->scan_req && rdev->scan_req->wdev == wdev)) {
 		rdev->scan_req->aborted = true;
 		___cfg80211_scan_done(rdev, true);
 	}
 
-	cfg80211_unlock_rdev(rdev);
-
-	mutex_lock(&rdev->sched_scan_mtx);
-
 	if (WARN_ON(rdev->sched_scan_req &&
 		    rdev->sched_scan_req->dev == wdev->netdev)) {
 		__cfg80211_stop_sched_scan(rdev, false);
@@ -781,21 +811,19 @@ void cfg80211_unregister_wdev(struct wireless_dev *wdev)
 		return;
 
 	mutex_lock(&rdev->devlist_mtx);
+	mutex_lock(&rdev->sched_scan_mtx);
 	list_del_rcu(&wdev->list);
 	rdev->devlist_generation++;
 
 	switch (wdev->iftype) {
 	case NL80211_IFTYPE_P2P_DEVICE:
-		if (!wdev->p2p_started)
-			break;
-		rdev_stop_p2p_device(rdev, wdev);
-		wdev->p2p_started = false;
-		rdev->opencount--;
+		cfg80211_stop_p2p_device(rdev, wdev);
 		break;
 	default:
 		WARN_ON_ONCE(1);
 		break;
 	}
+	mutex_unlock(&rdev->sched_scan_mtx);
 	mutex_unlock(&rdev->devlist_mtx);
 }
 EXPORT_SYMBOL(cfg80211_unregister_wdev);
@@ -936,6 +964,7 @@ static int cfg80211_netdev_notifier_call(struct notifier_block *nb,
 		cfg80211_update_iface_num(rdev, wdev->iftype, 1);
 		cfg80211_lock_rdev(rdev);
 		mutex_lock(&rdev->devlist_mtx);
+		mutex_lock(&rdev->sched_scan_mtx);
 		wdev_lock(wdev);
 		switch (wdev->iftype) {
 #ifdef CONFIG_CFG80211_WEXT
@@ -967,6 +996,7 @@ static int cfg80211_netdev_notifier_call(struct notifier_block *nb,
 			break;
 		}
 		wdev_unlock(wdev);
+		mutex_unlock(&rdev->sched_scan_mtx);
 		rdev->opencount++;
 		mutex_unlock(&rdev->devlist_mtx);
 		cfg80211_unlock_rdev(rdev);
diff --git a/net/wireless/core.h b/net/wireless/core.h
index 3aec0e429d8..5845c2b37aa 100644
--- a/net/wireless/core.h
+++ b/net/wireless/core.h
@@ -503,6 +503,9 @@ int cfg80211_validate_beacon_int(struct cfg80211_registered_device *rdev,
 void cfg80211_update_iface_num(struct cfg80211_registered_device *rdev,
 			       enum nl80211_iftype iftype, int num);
 
+void cfg80211_stop_p2p_device(struct cfg80211_registered_device *rdev,
+			      struct wireless_dev *wdev);
+
 #define CFG80211_MAX_NUM_DIFFERENT_CHANNELS 10
 
 #ifdef CONFIG_CFG80211_DEVELOPER_WARNINGS
diff --git a/net/wireless/nl80211.c b/net/wireless/nl80211.c
index d44ab216c0e..58e13a8c95f 100644
--- a/net/wireless/nl80211.c
+++ b/net/wireless/nl80211.c
@@ -4702,14 +4702,19 @@ static int nl80211_trigger_scan(struct sk_buff *skb, struct genl_info *info)
 	if (!rdev->ops->scan)
 		return -EOPNOTSUPP;
 
-	if (rdev->scan_req)
-		return -EBUSY;
+	mutex_lock(&rdev->sched_scan_mtx);
+	if (rdev->scan_req) {
+		err = -EBUSY;
+		goto unlock;
+	}
 
 	if (info->attrs[NL80211_ATTR_SCAN_FREQUENCIES]) {
 		n_channels = validate_scan_freqs(
 				info->attrs[NL80211_ATTR_SCAN_FREQUENCIES]);
-		if (!n_channels)
-			return -EINVAL;
+		if (!n_channels) {
+			err = -EINVAL;
+			goto unlock;
+		}
 	} else {
 		enum ieee80211_band band;
 		n_channels = 0;
@@ -4723,23 +4728,29 @@ static int nl80211_trigger_scan(struct sk_buff *skb, struct genl_info *info)
 		nla_for_each_nested(attr, info->attrs[NL80211_ATTR_SCAN_SSIDS], tmp)
 			n_ssids++;
 
-	if (n_ssids > wiphy->max_scan_ssids)
-		return -EINVAL;
+	if (n_ssids > wiphy->max_scan_ssids) {
+		err = -EINVAL;
+		goto unlock;
+	}
 
 	if (info->attrs[NL80211_ATTR_IE])
 		ie_len = nla_len(info->attrs[NL80211_ATTR_IE]);
 	else
 		ie_len = 0;
 
-	if (ie_len > wiphy->max_scan_ie_len)
-		return -EINVAL;
+	if (ie_len > wiphy->max_scan_ie_len) {
+		err = -EINVAL;
+		goto unlock;
+	}
 
 	request = kzalloc(sizeof(*request)
 			+ sizeof(*request->ssids) * n_ssids
 			+ sizeof(*request->channels) * n_channels
 			+ ie_len, GFP_KERNEL);
-	if (!request)
-		return -ENOMEM;
+	if (!request) {
+		err = -ENOMEM;
+		goto unlock;
+	}
 
 	if (n_ssids)
 		request->ssids = (void *)&request->channels[n_channels];
@@ -4876,6 +4887,8 @@ static int nl80211_trigger_scan(struct sk_buff *skb, struct genl_info *info)
 		kfree(request);
 	}
 
+ unlock:
+	mutex_unlock(&rdev->sched_scan_mtx);
 	return err;
 }
 
@@ -7749,20 +7762,9 @@ static int nl80211_stop_p2p_device(struct sk_buff *skb, struct genl_info *info)
 	if (!rdev->ops->stop_p2p_device)
 		return -EOPNOTSUPP;
 
-	if (!wdev->p2p_started)
-		return 0;
-
-	rdev_stop_p2p_device(rdev, wdev);
-	wdev->p2p_started = false;
-
-	mutex_lock(&rdev->devlist_mtx);
-	rdev->opencount--;
-	mutex_unlock(&rdev->devlist_mtx);
-
-	if (WARN_ON(rdev->scan_req && rdev->scan_req->wdev == wdev)) {
-		rdev->scan_req->aborted = true;
-		___cfg80211_scan_done(rdev, true);
-	}
+	mutex_lock(&rdev->sched_scan_mtx);
+	cfg80211_stop_p2p_device(rdev, wdev);
+	mutex_unlock(&rdev->sched_scan_mtx);
 
 	return 0;
 }
@@ -8486,7 +8488,7 @@ static int nl80211_add_scan_req(struct sk_buff *msg,
 	struct nlattr *nest;
 	int i;
 
-	ASSERT_RDEV_LOCK(rdev);
+	lockdep_assert_held(&rdev->sched_scan_mtx);
 
 	if (WARN_ON(!req))
 		return 0;
diff --git a/net/wireless/scan.c b/net/wireless/scan.c
index 674aadca007..fd99ea495b7 100644
--- a/net/wireless/scan.c
+++ b/net/wireless/scan.c
@@ -169,7 +169,7 @@ void ___cfg80211_scan_done(struct cfg80211_registered_device *rdev, bool leak)
 	union iwreq_data wrqu;
 #endif
 
-	ASSERT_RDEV_LOCK(rdev);
+	lockdep_assert_held(&rdev->sched_scan_mtx);
 
 	request = rdev->scan_req;
 
@@ -230,9 +230,9 @@ void __cfg80211_scan_done(struct work_struct *wk)
 	rdev = container_of(wk, struct cfg80211_registered_device,
 			    scan_done_wk);
 
-	cfg80211_lock_rdev(rdev);
+	mutex_lock(&rdev->sched_scan_mtx);
 	___cfg80211_scan_done(rdev, false);
-	cfg80211_unlock_rdev(rdev);
+	mutex_unlock(&rdev->sched_scan_mtx);
 }
 
 void cfg80211_scan_done(struct cfg80211_scan_request *request, bool aborted)
@@ -698,11 +698,6 @@ cfg80211_bss_update(struct cfg80211_registered_device *dev,
 	found = rb_find_bss(dev, tmp, BSS_CMP_REGULAR);
 
 	if (found) {
-		found->pub.beacon_interval = tmp->pub.beacon_interval;
-		found->pub.signal = tmp->pub.signal;
-		found->pub.capability = tmp->pub.capability;
-		found->ts = tmp->ts;
-
 		/* Update IEs */
 		if (rcu_access_pointer(tmp->pub.proberesp_ies)) {
 			const struct cfg80211_bss_ies *old;
@@ -723,6 +718,8 @@ cfg80211_bss_update(struct cfg80211_registered_device *dev,
 
 			if (found->pub.hidden_beacon_bss &&
 			    !list_empty(&found->hidden_list)) {
+				const struct cfg80211_bss_ies *f;
+
 				/*
 				 * The found BSS struct is one of the probe
 				 * response members of a group, but we're
@@ -732,6 +729,10 @@ cfg80211_bss_update(struct cfg80211_registered_device *dev,
 				 * SSID to showing it, which is confusing so
 				 * drop this information.
 				 */
+
+				f = rcu_access_pointer(tmp->pub.beacon_ies);
+				kfree_rcu((struct cfg80211_bss_ies *)f,
+					  rcu_head);
 				goto drop;
 			}
 
@@ -761,6 +762,11 @@ cfg80211_bss_update(struct cfg80211_registered_device *dev,
 				kfree_rcu((struct cfg80211_bss_ies *)old,
 					  rcu_head);
 		}
+
+		found->pub.beacon_interval = tmp->pub.beacon_interval;
+		found->pub.signal = tmp->pub.signal;
+		found->pub.capability = tmp->pub.capability;
+		found->ts = tmp->ts;
 	} else {
 		struct cfg80211_internal_bss *new;
 		struct cfg80211_internal_bss *hidden;
@@ -1056,6 +1062,7 @@ int cfg80211_wext_siwscan(struct net_device *dev,
 	if (IS_ERR(rdev))
 		return PTR_ERR(rdev);
 
+	mutex_lock(&rdev->sched_scan_mtx);
 	if (rdev->scan_req) {
 		err = -EBUSY;
 		goto out;
@@ -1162,6 +1169,7 @@ int cfg80211_wext_siwscan(struct net_device *dev,
 		dev_hold(dev);
 	}
  out:
+	mutex_unlock(&rdev->sched_scan_mtx);
 	kfree(creq);
 	cfg80211_unlock_rdev(rdev);
 	return err;
diff --git a/net/wireless/sme.c b/net/wireless/sme.c
index f432bd3755b..482c70e7012 100644
--- a/net/wireless/sme.c
+++ b/net/wireless/sme.c
@@ -85,6 +85,7 @@ static int cfg80211_conn_scan(struct wireless_dev *wdev)
 	ASSERT_RTNL();
 	ASSERT_RDEV_LOCK(rdev);
 	ASSERT_WDEV_LOCK(wdev);
+	lockdep_assert_held(&rdev->sched_scan_mtx);
 
 	if (rdev->scan_req)
 		return -EBUSY;
@@ -223,6 +224,7 @@ void cfg80211_conn_work(struct work_struct *work)
 	rtnl_lock();
 	cfg80211_lock_rdev(rdev);
 	mutex_lock(&rdev->devlist_mtx);
+	mutex_lock(&rdev->sched_scan_mtx);
 
 	list_for_each_entry(wdev, &rdev->wdev_list, list) {
 		wdev_lock(wdev);
@@ -247,6 +249,7 @@ void cfg80211_conn_work(struct work_struct *work)
 		wdev_unlock(wdev);
 	}
 
+	mutex_unlock(&rdev->sched_scan_mtx);
 	mutex_unlock(&rdev->devlist_mtx);
 	cfg80211_unlock_rdev(rdev);
 	rtnl_unlock();
@@ -320,11 +323,9 @@ void cfg80211_sme_scan_done(struct net_device *dev)
 {
 	struct wireless_dev *wdev = dev->ieee80211_ptr;
 
-	mutex_lock(&wiphy_to_dev(wdev->wiphy)->devlist_mtx);
 	wdev_lock(wdev);
 	__cfg80211_sme_scan_done(dev);
 	wdev_unlock(wdev);
-	mutex_unlock(&wiphy_to_dev(wdev->wiphy)->devlist_mtx);
 }
 
 void cfg80211_sme_rx_auth(struct net_device *dev,
@@ -924,9 +925,12 @@ int cfg80211_connect(struct cfg80211_registered_device *rdev,
 	int err;
 
 	mutex_lock(&rdev->devlist_mtx);
+	/* might request scan - scan_mtx -> wdev_mtx dependency */
+	mutex_lock(&rdev->sched_scan_mtx);
 	wdev_lock(dev->ieee80211_ptr);
 	err = __cfg80211_connect(rdev, dev, connect, connkeys, NULL);
 	wdev_unlock(dev->ieee80211_ptr);
+	mutex_unlock(&rdev->sched_scan_mtx);
 	mutex_unlock(&rdev->devlist_mtx);
 
 	return err;
diff --git a/net/wireless/trace.h b/net/wireless/trace.h
index b7a531380e1..7586de77a2f 100644
--- a/net/wireless/trace.h
+++ b/net/wireless/trace.h
@@ -27,7 +27,8 @@
 #define WIPHY_PR_ARG	__entry->wiphy_name
 
 #define WDEV_ENTRY	__field(u32, id)
-#define WDEV_ASSIGN	(__entry->id) = (wdev ? wdev->identifier : 0)
+#define WDEV_ASSIGN	(__entry->id) = (!IS_ERR_OR_NULL(wdev)	\
+					 ? wdev->identifier : 0)
 #define WDEV_PR_FMT	"wdev(%u)"
 #define WDEV_PR_ARG	(__entry->id)
 
@@ -1778,7 +1779,7 @@ TRACE_EVENT(rdev_set_mac_acl,
 	),
 	TP_fast_assign(
 		WIPHY_ASSIGN;
-		WIPHY_ASSIGN;
+		NETDEV_ASSIGN;
 		__entry->acl_policy = params->acl_policy;
 	),
 	TP_printk(WIPHY_PR_FMT ", " NETDEV_PR_FMT ", acl policy: %d",
diff --git a/net/wireless/wext-sme.c b/net/wireless/wext-sme.c
index fb9622f6d99..e79cb5c0655 100644
--- a/net/wireless/wext-sme.c
+++ b/net/wireless/wext-sme.c
@@ -89,6 +89,7 @@ int cfg80211_mgd_wext_siwfreq(struct net_device *dev,
 
 	cfg80211_lock_rdev(rdev);
 	mutex_lock(&rdev->devlist_mtx);
+	mutex_lock(&rdev->sched_scan_mtx);
 	wdev_lock(wdev);
 
 	if (wdev->sme_state != CFG80211_SME_IDLE) {
@@ -135,6 +136,7 @@ int cfg80211_mgd_wext_siwfreq(struct net_device *dev,
 	err = cfg80211_mgd_wext_connect(rdev, wdev);
  out:
 	wdev_unlock(wdev);
+	mutex_unlock(&rdev->sched_scan_mtx);
 	mutex_unlock(&rdev->devlist_mtx);
 	cfg80211_unlock_rdev(rdev);
 	return err;
@@ -190,6 +192,7 @@ int cfg80211_mgd_wext_siwessid(struct net_device *dev,
 
 	cfg80211_lock_rdev(rdev);
 	mutex_lock(&rdev->devlist_mtx);
+	mutex_lock(&rdev->sched_scan_mtx);
 	wdev_lock(wdev);
 
 	err = 0;
@@ -223,6 +226,7 @@ int cfg80211_mgd_wext_siwessid(struct net_device *dev,
 	err = cfg80211_mgd_wext_connect(rdev, wdev);
  out:
 	wdev_unlock(wdev);
+	mutex_unlock(&rdev->sched_scan_mtx);
 	mutex_unlock(&rdev->devlist_mtx);
 	cfg80211_unlock_rdev(rdev);
 	return err;
@@ -285,6 +289,7 @@ int cfg80211_mgd_wext_siwap(struct net_device *dev,
 
 	cfg80211_lock_rdev(rdev);
 	mutex_lock(&rdev->devlist_mtx);
+	mutex_lock(&rdev->sched_scan_mtx);
 	wdev_lock(wdev);
 
 	if (wdev->sme_state != CFG80211_SME_IDLE) {
@@ -313,6 +318,7 @@ int cfg80211_mgd_wext_siwap(struct net_device *dev,
 	err = cfg80211_mgd_wext_connect(rdev, wdev);
  out:
 	wdev_unlock(wdev);
+	mutex_unlock(&rdev->sched_scan_mtx);
 	mutex_unlock(&rdev->devlist_mtx);
 	cfg80211_unlock_rdev(rdev);
 	return err;
diff --git a/net/xfrm/xfrm_replay.c b/net/xfrm/xfrm_replay.c
index 35754cc8a9e..8dafe6d3c6e 100644
--- a/net/xfrm/xfrm_replay.c
+++ b/net/xfrm/xfrm_replay.c
@@ -334,6 +334,70 @@ static void xfrm_replay_notify_bmp(struct xfrm_state *x, int event)
 		x->xflags &= ~XFRM_TIME_DEFER;
 }
 
+static void xfrm_replay_notify_esn(struct xfrm_state *x, int event)
+{
+	u32 seq_diff, oseq_diff;
+	struct km_event c;
+	struct xfrm_replay_state_esn *replay_esn = x->replay_esn;
+	struct xfrm_replay_state_esn *preplay_esn = x->preplay_esn;
+
+	/* we send notify messages in case
+	 *  1. we updated on of the sequence numbers, and the seqno difference
+	 *     is at least x->replay_maxdiff, in this case we also update the
+	 *     timeout of our timer function
+	 *  2. if x->replay_maxage has elapsed since last update,
+	 *     and there were changes
+	 *
+	 *  The state structure must be locked!
+	 */
+
+	switch (event) {
+	case XFRM_REPLAY_UPDATE:
+		if (!x->replay_maxdiff)
+			break;
+
+		if (replay_esn->seq_hi == preplay_esn->seq_hi)
+			seq_diff = replay_esn->seq - preplay_esn->seq;
+		else
+			seq_diff = ~preplay_esn->seq + replay_esn->seq + 1;
+
+		if (replay_esn->oseq_hi == preplay_esn->oseq_hi)
+			oseq_diff = replay_esn->oseq - preplay_esn->oseq;
+		else
+			oseq_diff = ~preplay_esn->oseq + replay_esn->oseq + 1;
+
+		if (seq_diff < x->replay_maxdiff &&
+		    oseq_diff < x->replay_maxdiff) {
+
+			if (x->xflags & XFRM_TIME_DEFER)
+				event = XFRM_REPLAY_TIMEOUT;
+			else
+				return;
+		}
+
+		break;
+
+	case XFRM_REPLAY_TIMEOUT:
+		if (memcmp(x->replay_esn, x->preplay_esn,
+			   xfrm_replay_state_esn_len(replay_esn)) == 0) {
+			x->xflags |= XFRM_TIME_DEFER;
+			return;
+		}
+
+		break;
+	}
+
+	memcpy(x->preplay_esn, x->replay_esn,
+	       xfrm_replay_state_esn_len(replay_esn));
+	c.event = XFRM_MSG_NEWAE;
+	c.data.aevent = event;
+	km_state_notify(x, &c);
+
+	if (x->replay_maxage &&
+	    !mod_timer(&x->rtimer, jiffies + x->replay_maxage))
+		x->xflags &= ~XFRM_TIME_DEFER;
+}
+
 static int xfrm_replay_overflow_esn(struct xfrm_state *x, struct sk_buff *skb)
 {
 	int err = 0;
@@ -510,7 +574,7 @@ static struct xfrm_replay xfrm_replay_esn = {
 	.advance	= xfrm_replay_advance_esn,
 	.check		= xfrm_replay_check_esn,
 	.recheck	= xfrm_replay_recheck_esn,
-	.notify		= xfrm_replay_notify_bmp,
+	.notify		= xfrm_replay_notify_esn,
 	.overflow	= xfrm_replay_overflow_esn,
 };
 
diff --git a/samples/hidraw/hid-example.c b/samples/hidraw/hid-example.c
index 816e2dcda7c..512a7e50bca 100644
--- a/samples/hidraw/hid-example.c
+++ b/samples/hidraw/hid-example.c
@@ -17,10 +17,9 @@
 /*
  * Ugly hack to work around failing compilation on systems that don't
  * yet populate new version of hidraw.h to userspace.
- *
- * If you need this, please have your distro update the kernel headers.
  */
 #ifndef HIDIOCSFEATURE
+#warning Please have your distro update the userspace kernel headers
 #define HIDIOCSFEATURE(len)    _IOC(_IOC_WRITE|_IOC_READ, 'H', 0x06, len)
 #define HIDIOCGFEATURE(len)    _IOC(_IOC_WRITE|_IOC_READ, 'H', 0x07, len)
 #endif
diff --git a/scripts/checkpatch.pl b/scripts/checkpatch.pl
index b28cc384a5b..b954de58304 100755
--- a/scripts/checkpatch.pl
+++ b/scripts/checkpatch.pl
@@ -281,6 +281,7 @@ our $signature_tags = qr{(?xi:
 	Tested-by:|
 	Reviewed-by:|
 	Reported-by:|
+	Suggested-by:|
 	To:|
 	Cc:
 )};
@@ -628,6 +629,13 @@ sub sanitise_line {
 	return $res;
 }
 
+sub get_quoted_string {
+	my ($line, $rawline) = @_;
+
+	return "" if ($line !~ m/(\"[X]+\")/g);
+	return substr($rawline, $-[0], $+[0] - $-[0]);
+}
+
 sub ctx_statement_block {
 	my ($linenr, $remain, $off) = @_;
 	my $line = $linenr - 1;
@@ -1576,7 +1584,8 @@ sub process {
 # Check for incorrect file permissions
 		if ($line =~ /^new (file )?mode.*[7531]\d{0,2}$/) {
 			my $permhere = $here . "FILE: $realfile\n";
-			if ($realfile =~ /(Makefile|Kconfig|\.c|\.h|\.S|\.tmpl)$/) {
+			if ($realfile !~ m@scripts/@ &&
+			    $realfile !~ /\.(py|pl|awk|sh)$/) {
 				ERROR("EXECUTE_PERMISSIONS",
 				      "do not set execute permissions for source files\n" . $permhere);
 			}
@@ -2514,8 +2523,8 @@ sub process {
 
 # check for whitespace before a non-naked semicolon
 		if ($line =~ /^\+.*\S\s+;/) {
-			CHK("SPACING",
-			    "space prohibited before semicolon\n" . $herecurr);
+			WARN("SPACING",
+			     "space prohibited before semicolon\n" . $herecurr);
 		}
 
 # Check operator spacing.
@@ -3016,6 +3025,7 @@ sub process {
 			    $dstat !~ /^'X'$/ &&					# character constants
 			    $dstat !~ /$exceptions/ &&
 			    $dstat !~ /^\.$Ident\s*=/ &&				# .foo =
+			    $dstat !~ /^(?:\#\s*$Ident|\#\s*$Constant)\s*$/ &&		# stringification #foo
 			    $dstat !~ /^do\s*$Constant\s*while\s*$Constant;?$/ &&	# do {...} while (...); // do {...} while (...)
 			    $dstat !~ /^for\s*$Constant$/ &&				# for (...)
 			    $dstat !~ /^for\s*$Constant\s+(?:$Ident|-?$Constant)$/ &&	# for (...) bar()
@@ -3220,7 +3230,7 @@ sub process {
 		}
 
 # check for unnecessary blank lines around braces
-		if (($line =~ /^..*}\s*$/ && $prevline =~ /^.\s*$/)) {
+		if (($line =~ /^.\s*}\s*$/ && $prevline =~ /^.\s*$/)) {
 			CHK("BRACES",
 			    "Blank lines aren't necessary before a close brace '}'\n" . $hereprev);
 		}
@@ -3372,6 +3382,15 @@ sub process {
 			     "struct spinlock should be spinlock_t\n" . $herecurr);
 		}
 
+# check for seq_printf uses that could be seq_puts
+		if ($line =~ /\bseq_printf\s*\(/) {
+			my $fmt = get_quoted_string($line, $rawline);
+			if ($fmt !~ /[^\\]\%/) {
+				WARN("PREFER_SEQ_PUTS",
+				     "Prefer seq_puts to seq_printf\n" . $herecurr);
+			}
+		}
+
 # Check for misused memsets
 		if ($^V && $^V ge 5.10.0 &&
 		    defined $stat &&
@@ -3476,6 +3495,13 @@ sub process {
 			     "unnecessary cast may hide bugs, see http://c-faq.com/malloc/mallocnocast.html\n" . $herecurr);
 		}
 
+# check for krealloc arg reuse
+		if ($^V && $^V ge 5.10.0 &&
+		    $line =~ /\b($Lval)\s*\=\s*(?:$balanced_parens)?\s*krealloc\s*\(\s*\1\s*,/) {
+			WARN("KREALLOC_ARG_REUSE",
+			     "Reusing the krealloc arg is almost always a bug\n" . $herecurr);
+		}
+
 # check for alloc argument mismatch
 		if ($line =~ /\b(kcalloc|kmalloc_array)\s*\(\s*sizeof\b/) {
 			WARN("ALLOC_ARRAY_ARGS",
diff --git a/scripts/decodecode b/scripts/decodecode
index 4f8248d5a11..d8824f37acc 100755
--- a/scripts/decodecode
+++ b/scripts/decodecode
@@ -89,10 +89,16 @@ echo $code >> $T.s
 disas $T
 cat $T.dis >> $T.aa
 
+# (lines of whole $T.oo) - (lines of $T.aa, i.e. "Code starting") + 3,
+# i.e. the title + the "===..=" line (sed is counting from 1, 0 address is
+# special)
+faultlinenum=$(( $(wc -l $T.oo  | cut -d" " -f1) - \
+		 $(wc -l $T.aa  | cut -d" " -f1) + 3))
+
 faultline=`cat $T.dis | head -1 | cut -d":" -f2-`
 faultline=`echo "$faultline" | sed -e 's/\[/\\\[/g; s/\]/\\\]/g'`
 
-cat $T.oo | sed -e "s/\($faultline\)/\*\1     <-- trapping instruction/g"
+cat $T.oo | sed -e "${faultlinenum}s/^\(.*:\)\(.*\)/\1\*\2\t\t<-- trapping instruction/"
 echo
 cat $T.aa
 cleanup
diff --git a/scripts/get_maintainer.pl b/scripts/get_maintainer.pl
index ce4cc837b74..5e4fb144a04 100755
--- a/scripts/get_maintainer.pl
+++ b/scripts/get_maintainer.pl
@@ -611,7 +611,7 @@ sub get_maintainers {
 				    $hash{$tvi} = $value_pd;
 				}
 			    }
-			} elsif ($type eq 'K') {
+			} elsif ($type eq 'N') {
 			    if ($file =~ m/$value/x) {
 				$hash{$tvi} = 0;
 			    }
diff --git a/scripts/kconfig/streamline_config.pl b/scripts/kconfig/streamline_config.pl
index 33689396953..4606cdfb859 100644
--- a/scripts/kconfig/streamline_config.pl
+++ b/scripts/kconfig/streamline_config.pl
@@ -156,7 +156,6 @@ sub read_kconfig {
 
     my $state = "NONE";
     my $config;
-    my @kconfigs;
 
     my $cont = 0;
     my $line;
@@ -190,7 +189,13 @@ sub read_kconfig {
 
 	# collect any Kconfig sources
 	if (/^source\s*"(.*)"/) {
-	    $kconfigs[$#kconfigs+1] = $1;
+	    my $kconfig = $1;
+	    # prevent reading twice.
+	    if (!defined($read_kconfigs{$kconfig})) {
+		$read_kconfigs{$kconfig} = 1;
+		read_kconfig($kconfig);
+	    }
+	    next;
 	}
 
 	# configs found
@@ -250,14 +255,6 @@ sub read_kconfig {
 	}
     }
     close($kinfile);
-
-    # read in any configs that were found.
-    foreach my $kconfig (@kconfigs) {
-	if (!defined($read_kconfigs{$kconfig})) {
-	    $read_kconfigs{$kconfig} = 1;
-	    read_kconfig($kconfig);
-	}
-    }
 }
 
 if ($kconfig) {
@@ -396,6 +393,15 @@ foreach my $module (keys(%modules)) {
 	foreach my $conf (@arr) {
 	    $configs{$conf} = $module;
 	    dprint "$conf added by direct ($module)\n";
+	    if ($debugprint) {
+		my $c=$conf;
+		$c =~ s/^CONFIG_//;
+		if (defined($depends{$c})) {
+		    dprint " deps = $depends{$c}\n";
+		} else {
+		    dprint " no deps\n";
+		}
+	    }
 	}
     } else {
 	# Most likely, someone has a custom (binary?) module loaded.
diff --git a/scripts/mod/devicetable-offsets.c b/scripts/mod/devicetable-offsets.c
index b45260bfeaa..e66d4d258e1 100644
--- a/scripts/mod/devicetable-offsets.c
+++ b/scripts/mod/devicetable-offsets.c
@@ -174,5 +174,8 @@ int main(void)
 	DEVID_FIELD(x86_cpu_id, model);
 	DEVID_FIELD(x86_cpu_id, vendor);
 
+	DEVID(mei_cl_device_id);
+	DEVID_FIELD(mei_cl_device_id, name);
+
 	return 0;
 }
diff --git a/scripts/mod/file2alias.c b/scripts/mod/file2alias.c
index 771ac17f635..45f9a3377dc 100644
--- a/scripts/mod/file2alias.c
+++ b/scripts/mod/file2alias.c
@@ -1133,6 +1133,18 @@ static int do_x86cpu_entry(const char *filename, void *symval,
 }
 ADD_TO_DEVTABLE("x86cpu", x86_cpu_id, do_x86cpu_entry);
 
+/* Looks like: mei:S */
+static int do_mei_entry(const char *filename, void *symval,
+			char *alias)
+{
+	DEF_FIELD_ADDR(symval, mei_cl_device_id, name);
+
+	sprintf(alias, MEI_CL_MODULE_PREFIX "%s", *name);
+
+	return 1;
+}
+ADD_TO_DEVTABLE("mei", mei_cl_device_id, do_mei_entry);
+
 /* Does namelen bytes of name exactly match the symbol? */
 static bool sym_is(const char *name, unsigned namelen, const char *symbol)
 {
diff --git a/security/capability.c b/security/capability.c
index 57977508896..6783c3e6c88 100644
--- a/security/capability.c
+++ b/security/capability.c
@@ -737,6 +737,11 @@ static int cap_tun_dev_open(void *security)
 {
 	return 0;
 }
+
+static void cap_skb_owned_by(struct sk_buff *skb, struct sock *sk)
+{
+}
+
 #endif	/* CONFIG_SECURITY_NETWORK */
 
 #ifdef CONFIG_SECURITY_NETWORK_XFRM
@@ -1071,6 +1076,7 @@ void __init security_fixup_ops(struct security_operations *ops)
 	set_to_cap_if_null(ops, tun_dev_open);
 	set_to_cap_if_null(ops, tun_dev_attach_queue);
 	set_to_cap_if_null(ops, tun_dev_attach);
+	set_to_cap_if_null(ops, skb_owned_by);
 #endif	/* CONFIG_SECURITY_NETWORK */
 #ifdef CONFIG_SECURITY_NETWORK_XFRM
 	set_to_cap_if_null(ops, xfrm_policy_alloc_security);
diff --git a/security/device_cgroup.c b/security/device_cgroup.c
index 1c69e38e3a2..dd0dc574d78 100644
--- a/security/device_cgroup.c
+++ b/security/device_cgroup.c
@@ -25,6 +25,12 @@
 
 static DEFINE_MUTEX(devcgroup_mutex);
 
+enum devcg_behavior {
+	DEVCG_DEFAULT_NONE,
+	DEVCG_DEFAULT_ALLOW,
+	DEVCG_DEFAULT_DENY,
+};
+
 /*
  * exception list locking rules:
  * hold devcgroup_mutex for update/read.
@@ -42,10 +48,9 @@ struct dev_exception_item {
 struct dev_cgroup {
 	struct cgroup_subsys_state css;
 	struct list_head exceptions;
-	enum {
-		DEVCG_DEFAULT_ALLOW,
-		DEVCG_DEFAULT_DENY,
-	} behavior;
+	enum devcg_behavior behavior;
+	/* temporary list for pending propagation operations */
+	struct list_head propagate_pending;
 };
 
 static inline struct dev_cgroup *css_to_devcgroup(struct cgroup_subsys_state *s)
@@ -182,35 +187,62 @@ static void dev_exception_clean(struct dev_cgroup *dev_cgroup)
 	__dev_exception_clean(dev_cgroup);
 }
 
+static inline bool is_devcg_online(const struct dev_cgroup *devcg)
+{
+	return (devcg->behavior != DEVCG_DEFAULT_NONE);
+}
+
+/**
+ * devcgroup_online - initializes devcgroup's behavior and exceptions based on
+ * 		      parent's
+ * @cgroup: cgroup getting online
+ * returns 0 in case of success, error code otherwise
+ */
+static int devcgroup_online(struct cgroup *cgroup)
+{
+	struct dev_cgroup *dev_cgroup, *parent_dev_cgroup = NULL;
+	int ret = 0;
+
+	mutex_lock(&devcgroup_mutex);
+	dev_cgroup = cgroup_to_devcgroup(cgroup);
+	if (cgroup->parent)
+		parent_dev_cgroup = cgroup_to_devcgroup(cgroup->parent);
+
+	if (parent_dev_cgroup == NULL)
+		dev_cgroup->behavior = DEVCG_DEFAULT_ALLOW;
+	else {
+		ret = dev_exceptions_copy(&dev_cgroup->exceptions,
+					  &parent_dev_cgroup->exceptions);
+		if (!ret)
+			dev_cgroup->behavior = parent_dev_cgroup->behavior;
+	}
+	mutex_unlock(&devcgroup_mutex);
+
+	return ret;
+}
+
+static void devcgroup_offline(struct cgroup *cgroup)
+{
+	struct dev_cgroup *dev_cgroup = cgroup_to_devcgroup(cgroup);
+
+	mutex_lock(&devcgroup_mutex);
+	dev_cgroup->behavior = DEVCG_DEFAULT_NONE;
+	mutex_unlock(&devcgroup_mutex);
+}
+
 /*
  * called from kernel/cgroup.c with cgroup_lock() held.
  */
 static struct cgroup_subsys_state *devcgroup_css_alloc(struct cgroup *cgroup)
 {
-	struct dev_cgroup *dev_cgroup, *parent_dev_cgroup;
-	struct cgroup *parent_cgroup;
-	int ret;
+	struct dev_cgroup *dev_cgroup;
 
 	dev_cgroup = kzalloc(sizeof(*dev_cgroup), GFP_KERNEL);
 	if (!dev_cgroup)
 		return ERR_PTR(-ENOMEM);
 	INIT_LIST_HEAD(&dev_cgroup->exceptions);
-	parent_cgroup = cgroup->parent;
-
-	if (parent_cgroup == NULL)
-		dev_cgroup->behavior = DEVCG_DEFAULT_ALLOW;
-	else {
-		parent_dev_cgroup = cgroup_to_devcgroup(parent_cgroup);
-		mutex_lock(&devcgroup_mutex);
-		ret = dev_exceptions_copy(&dev_cgroup->exceptions,
-					  &parent_dev_cgroup->exceptions);
-		dev_cgroup->behavior = parent_dev_cgroup->behavior;
-		mutex_unlock(&devcgroup_mutex);
-		if (ret) {
-			kfree(dev_cgroup);
-			return ERR_PTR(ret);
-		}
-	}
+	INIT_LIST_HEAD(&dev_cgroup->propagate_pending);
+	dev_cgroup->behavior = DEVCG_DEFAULT_NONE;
 
 	return &dev_cgroup->css;
 }
@@ -304,9 +336,11 @@ static int devcgroup_seq_read(struct cgroup *cgroup, struct cftype *cft,
  *		verify if a certain access is allowed.
  * @dev_cgroup: dev cgroup to be tested against
  * @refex: new exception
+ * @behavior: behavior of the exception
  */
-static int may_access(struct dev_cgroup *dev_cgroup,
-		      struct dev_exception_item *refex)
+static bool may_access(struct dev_cgroup *dev_cgroup,
+		       struct dev_exception_item *refex,
+		       enum devcg_behavior behavior)
 {
 	struct dev_exception_item *ex;
 	bool match = false;
@@ -330,18 +364,29 @@ static int may_access(struct dev_cgroup *dev_cgroup,
 		break;
 	}
 
-	/*
-	 * In two cases we'll consider this new exception valid:
-	 * - the dev cgroup has its default policy to allow + exception list:
-	 *   the new exception should *not* match any of the exceptions
-	 *   (behavior == DEVCG_DEFAULT_ALLOW, !match)
-	 * - the dev cgroup has its default policy to deny + exception list:
-	 *   the new exception *should* match the exceptions
-	 *   (behavior == DEVCG_DEFAULT_DENY, match)
-	 */
-	if ((dev_cgroup->behavior == DEVCG_DEFAULT_DENY) == match)
-		return 1;
-	return 0;
+	if (dev_cgroup->behavior == DEVCG_DEFAULT_ALLOW) {
+		if (behavior == DEVCG_DEFAULT_ALLOW) {
+			/* the exception will deny access to certain devices */
+			return true;
+		} else {
+			/* the exception will allow access to certain devices */
+			if (match)
+				/*
+				 * a new exception allowing access shouldn't
+				 * match an parent's exception
+				 */
+				return false;
+			return true;
+		}
+	} else {
+		/* only behavior == DEVCG_DEFAULT_DENY allowed here */
+		if (match)
+			/* parent has an exception that matches the proposed */
+			return true;
+		else
+			return false;
+	}
+	return false;
 }
 
 /*
@@ -358,7 +403,7 @@ static int parent_has_perm(struct dev_cgroup *childcg,
 	if (!pcg)
 		return 1;
 	parent = cgroup_to_devcgroup(pcg);
-	return may_access(parent, ex);
+	return may_access(parent, ex, childcg->behavior);
 }
 
 /**
@@ -374,6 +419,111 @@ static inline int may_allow_all(struct dev_cgroup *parent)
 	return parent->behavior == DEVCG_DEFAULT_ALLOW;
 }
 
+/**
+ * revalidate_active_exceptions - walks through the active exception list and
+ * 				  revalidates the exceptions based on parent's
+ * 				  behavior and exceptions. The exceptions that
+ * 				  are no longer valid will be removed.
+ * 				  Called with devcgroup_mutex held.
+ * @devcg: cgroup which exceptions will be checked
+ *
+ * This is one of the three key functions for hierarchy implementation.
+ * This function is responsible for re-evaluating all the cgroup's active
+ * exceptions due to a parent's exception change.
+ * Refer to Documentation/cgroups/devices.txt for more details.
+ */
+static void revalidate_active_exceptions(struct dev_cgroup *devcg)
+{
+	struct dev_exception_item *ex;
+	struct list_head *this, *tmp;
+
+	list_for_each_safe(this, tmp, &devcg->exceptions) {
+		ex = container_of(this, struct dev_exception_item, list);
+		if (!parent_has_perm(devcg, ex))
+			dev_exception_rm(devcg, ex);
+	}
+}
+
+/**
+ * get_online_devcg - walks the cgroup tree and fills a list with the online
+ * 		      groups
+ * @root: cgroup used as starting point
+ * @online: list that will be filled with online groups
+ *
+ * Must be called with devcgroup_mutex held. Grabs RCU lock.
+ * Because devcgroup_mutex is held, no devcg will become online or offline
+ * during the tree walk (see devcgroup_online, devcgroup_offline)
+ * A separated list is needed because propagate_behavior() and
+ * propagate_exception() need to allocate memory and can block.
+ */
+static void get_online_devcg(struct cgroup *root, struct list_head *online)
+{
+	struct cgroup *pos;
+	struct dev_cgroup *devcg;
+
+	lockdep_assert_held(&devcgroup_mutex);
+
+	rcu_read_lock();
+	cgroup_for_each_descendant_pre(pos, root) {
+		devcg = cgroup_to_devcgroup(pos);
+		if (is_devcg_online(devcg))
+			list_add_tail(&devcg->propagate_pending, online);
+	}
+	rcu_read_unlock();
+}
+
+/**
+ * propagate_exception - propagates a new exception to the children
+ * @devcg_root: device cgroup that added a new exception
+ * @ex: new exception to be propagated
+ *
+ * returns: 0 in case of success, != 0 in case of error
+ */
+static int propagate_exception(struct dev_cgroup *devcg_root,
+			       struct dev_exception_item *ex)
+{
+	struct cgroup *root = devcg_root->css.cgroup;
+	struct dev_cgroup *devcg, *parent, *tmp;
+	int rc = 0;
+	LIST_HEAD(pending);
+
+	get_online_devcg(root, &pending);
+
+	list_for_each_entry_safe(devcg, tmp, &pending, propagate_pending) {
+		parent = cgroup_to_devcgroup(devcg->css.cgroup->parent);
+
+		/*
+		 * in case both root's behavior and devcg is allow, a new
+		 * restriction means adding to the exception list
+		 */
+		if (devcg_root->behavior == DEVCG_DEFAULT_ALLOW &&
+		    devcg->behavior == DEVCG_DEFAULT_ALLOW) {
+			rc = dev_exception_add(devcg, ex);
+			if (rc)
+				break;
+		} else {
+			/*
+			 * in the other possible cases:
+			 * root's behavior: allow, devcg's: deny
+			 * root's behavior: deny, devcg's: deny
+			 * the exception will be removed
+			 */
+			dev_exception_rm(devcg, ex);
+		}
+		revalidate_active_exceptions(devcg);
+
+		list_del_init(&devcg->propagate_pending);
+	}
+	return rc;
+}
+
+static inline bool has_children(struct dev_cgroup *devcgroup)
+{
+	struct cgroup *cgrp = devcgroup->css.cgroup;
+
+	return !list_empty(&cgrp->children);
+}
+
 /*
  * Modify the exception list using allow/deny rules.
  * CAP_SYS_ADMIN is needed for this.  It's at least separate from CAP_MKNOD
@@ -392,7 +542,7 @@ static int devcgroup_update_access(struct dev_cgroup *devcgroup,
 {
 	const char *b;
 	char temp[12];		/* 11 + 1 characters needed for a u32 */
-	int count, rc;
+	int count, rc = 0;
 	struct dev_exception_item ex;
 	struct cgroup *p = devcgroup->css.cgroup;
 	struct dev_cgroup *parent = NULL;
@@ -410,6 +560,9 @@ static int devcgroup_update_access(struct dev_cgroup *devcgroup,
 	case 'a':
 		switch (filetype) {
 		case DEVCG_ALLOW:
+			if (has_children(devcgroup))
+				return -EINVAL;
+
 			if (!may_allow_all(parent))
 				return -EPERM;
 			dev_exception_clean(devcgroup);
@@ -423,6 +576,9 @@ static int devcgroup_update_access(struct dev_cgroup *devcgroup,
 				return rc;
 			break;
 		case DEVCG_DENY:
+			if (has_children(devcgroup))
+				return -EINVAL;
+
 			dev_exception_clean(devcgroup);
 			devcgroup->behavior = DEVCG_DEFAULT_DENY;
 			break;
@@ -517,22 +673,28 @@ static int devcgroup_update_access(struct dev_cgroup *devcgroup,
 			dev_exception_rm(devcgroup, &ex);
 			return 0;
 		}
-		return dev_exception_add(devcgroup, &ex);
+		rc = dev_exception_add(devcgroup, &ex);
+		break;
 	case DEVCG_DENY:
 		/*
 		 * If the default policy is to deny by default, try to remove
 		 * an matching exception instead. And be silent about it: we
 		 * don't want to break compatibility
 		 */
-		if (devcgroup->behavior == DEVCG_DEFAULT_DENY) {
+		if (devcgroup->behavior == DEVCG_DEFAULT_DENY)
 			dev_exception_rm(devcgroup, &ex);
-			return 0;
-		}
-		return dev_exception_add(devcgroup, &ex);
+		else
+			rc = dev_exception_add(devcgroup, &ex);
+
+		if (rc)
+			break;
+		/* we only propagate new restrictions */
+		rc = propagate_exception(devcgroup, &ex);
+		break;
 	default:
-		return -EINVAL;
+		rc = -EINVAL;
 	}
-	return 0;
+	return rc;
 }
 
 static int devcgroup_access_write(struct cgroup *cgrp, struct cftype *cft,
@@ -571,17 +733,10 @@ struct cgroup_subsys devices_subsys = {
 	.can_attach = devcgroup_can_attach,
 	.css_alloc = devcgroup_css_alloc,
 	.css_free = devcgroup_css_free,
+	.css_online = devcgroup_online,
+	.css_offline = devcgroup_offline,
 	.subsys_id = devices_subsys_id,
 	.base_cftypes = dev_cgroup_files,
-
-	/*
-	 * While devices cgroup has the rudimentary hierarchy support which
-	 * checks the parent's restriction, it doesn't properly propagates
-	 * config changes in ancestors to their descendents.  A child
-	 * should only be allowed to add more restrictions to the parent's
-	 * configuration.  Fix it and remove the following.
-	 */
-	.broken_hierarchy = true,
 };
 
 /**
@@ -609,7 +764,7 @@ static int __devcgroup_check_permission(short type, u32 major, u32 minor,
 
 	rcu_read_lock();
 	dev_cgroup = task_devcgroup(current);
-	rc = may_access(dev_cgroup, &ex);
+	rc = may_access(dev_cgroup, &ex, dev_cgroup->behavior);
 	rcu_read_unlock();
 
 	if (!rc)
diff --git a/security/security.c b/security/security.c
index 7b88c6aeaed..03f248b84e9 100644
--- a/security/security.c
+++ b/security/security.c
@@ -1290,6 +1290,11 @@ int security_tun_dev_open(void *security)
 }
 EXPORT_SYMBOL(security_tun_dev_open);
 
+void security_skb_owned_by(struct sk_buff *skb, struct sock *sk)
+{
+	security_ops->skb_owned_by(skb, sk);
+}
+
 #endif	/* CONFIG_SECURITY_NETWORK */
 
 #ifdef CONFIG_SECURITY_NETWORK_XFRM
diff --git a/security/selinux/hooks.c b/security/selinux/hooks.c
index 2fa28c88900..7171a957b93 100644
--- a/security/selinux/hooks.c
+++ b/security/selinux/hooks.c
@@ -51,6 +51,7 @@
 #include <linux/tty.h>
 #include <net/icmp.h>
 #include <net/ip.h>		/* for local_port_range[] */
+#include <net/sock.h>
 #include <net/tcp.h>		/* struct or_callable used in sock_rcv_skb */
 #include <net/net_namespace.h>
 #include <net/netlabel.h>
@@ -4363,6 +4364,11 @@ static void selinux_inet_conn_established(struct sock *sk, struct sk_buff *skb)
 	selinux_skb_peerlbl_sid(skb, family, &sksec->peer_sid);
 }
 
+static void selinux_skb_owned_by(struct sk_buff *skb, struct sock *sk)
+{
+	skb_set_owner_w(skb, sk);
+}
+
 static int selinux_secmark_relabel_packet(u32 sid)
 {
 	const struct task_security_struct *__tsec;
@@ -5664,6 +5670,7 @@ static struct security_operations selinux_ops = {
 	.tun_dev_attach_queue =		selinux_tun_dev_attach_queue,
 	.tun_dev_attach =		selinux_tun_dev_attach,
 	.tun_dev_open =			selinux_tun_dev_open,
+	.skb_owned_by =			selinux_skb_owned_by,
 
 #ifdef CONFIG_SECURITY_NETWORK_XFRM
 	.xfrm_policy_alloc_security =	selinux_xfrm_policy_alloc,
diff --git a/security/yama/yama_lsm.c b/security/yama/yama_lsm.c
index 23414b93771..13c88fbcf03 100644
--- a/security/yama/yama_lsm.c
+++ b/security/yama/yama_lsm.c
@@ -347,10 +347,8 @@ int yama_ptrace_traceme(struct task_struct *parent)
 	/* Only disallow PTRACE_TRACEME on more aggressive settings. */
 	switch (ptrace_scope) {
 	case YAMA_SCOPE_CAPABILITY:
-		rcu_read_lock();
-		if (!ns_capable(__task_cred(parent)->user_ns, CAP_SYS_PTRACE))
+		if (!has_ns_capability(parent, current_user_ns(), CAP_SYS_PTRACE))
 			rc = -EPERM;
-		rcu_read_unlock();
 		break;
 	case YAMA_SCOPE_NO_ATTACH:
 		rc = -EPERM;
diff --git a/sound/core/pcm_native.c b/sound/core/pcm_native.c
index 71ae86ca64a..eb560fa3232 100644
--- a/sound/core/pcm_native.c
+++ b/sound/core/pcm_native.c
@@ -3222,18 +3222,10 @@ EXPORT_SYMBOL_GPL(snd_pcm_lib_default_mmap);
 int snd_pcm_lib_mmap_iomem(struct snd_pcm_substream *substream,
 			   struct vm_area_struct *area)
 {
-	long size;
-	unsigned long offset;
+	struct snd_pcm_runtime *runtime = substream->runtime;;
 
 	area->vm_page_prot = pgprot_noncached(area->vm_page_prot);
-	area->vm_flags |= VM_IO;
-	size = area->vm_end - area->vm_start;
-	offset = area->vm_pgoff << PAGE_SHIFT;
-	if (io_remap_pfn_range(area, area->vm_start,
-				(substream->runtime->dma_addr + offset) >> PAGE_SHIFT,
-				size, area->vm_page_prot))
-		return -EAGAIN;
-	return 0;
+	return vm_iomap_memory(area, runtime->dma_addr, runtime->dma_bytes);
 }
 
 EXPORT_SYMBOL(snd_pcm_lib_mmap_iomem);
diff --git a/sound/i2c/other/tea575x-tuner.c b/sound/i2c/other/tea575x-tuner.c
index 3c6c1e3226f..8a36a1d9803 100644
--- a/sound/i2c/other/tea575x-tuner.c
+++ b/sound/i2c/other/tea575x-tuner.c
@@ -306,7 +306,7 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-					struct v4l2_tuner *v)
+					const struct v4l2_tuner *v)
 {
 	struct snd_tea575x *tea = video_drvdata(file);
 	u32 orig_val = tea->val;
@@ -336,7 +336,7 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-					struct v4l2_frequency *f)
+					const struct v4l2_frequency *f)
 {
 	struct snd_tea575x *tea = video_drvdata(file);
 
@@ -350,7 +350,7 @@ static int vidioc_s_frequency(struct file *file, void *priv,
 	else
 		tea->band = BAND_FM;
 
-	tea->freq = clamp(f->frequency, bands[tea->band].rangelow,
+	tea->freq = clamp_t(u32, f->frequency, bands[tea->band].rangelow,
 					bands[tea->band].rangehigh);
 	snd_tea575x_set_freq(tea);
 	return 0;
diff --git a/sound/pci/hda/hda_codec.c b/sound/pci/hda/hda_codec.c
index ecdf30eb587..4aba7646dd9 100644
--- a/sound/pci/hda/hda_codec.c
+++ b/sound/pci/hda/hda_codec.c
@@ -173,7 +173,7 @@ const char *snd_hda_get_jack_type(u32 cfg)
 		"Line Out", "Speaker", "HP Out", "CD",
 		"SPDIF Out", "Digital Out", "Modem Line", "Modem Hand",
 		"Line In", "Aux", "Mic", "Telephony",
-		"SPDIF In", "Digitial In", "Reserved", "Other"
+		"SPDIF In", "Digital In", "Reserved", "Other"
 	};
 
 	return jack_types[(cfg & AC_DEFCFG_DEVICE)
diff --git a/sound/pci/hda/hda_eld.c b/sound/pci/hda/hda_eld.c
index 7dd846380a5..d0d7ac1e99d 100644
--- a/sound/pci/hda/hda_eld.c
+++ b/sound/pci/hda/hda_eld.c
@@ -320,7 +320,7 @@ int snd_hdmi_get_eld(struct hda_codec *codec, hda_nid_t nid,
 		     unsigned char *buf, int *eld_size)
 {
 	int i;
-	int ret;
+	int ret = 0;
 	int size;
 
 	/*
diff --git a/sound/pci/hda/hda_generic.c b/sound/pci/hda/hda_generic.c
index 43c2ea53956..2dbe767be16 100644
--- a/sound/pci/hda/hda_generic.c
+++ b/sound/pci/hda/hda_generic.c
@@ -740,7 +740,7 @@ EXPORT_SYMBOL_HDA(snd_hda_activate_path);
 static void path_power_down_sync(struct hda_codec *codec, struct nid_path *path)
 {
 	struct hda_gen_spec *spec = codec->spec;
-	bool changed;
+	bool changed = false;
 	int i;
 
 	if (!spec->power_down_unused || path->active)
diff --git a/sound/pci/hda/hda_intel.c b/sound/pci/hda/hda_intel.c
index 418bfc0eb0a..bcd40ee488e 100644
--- a/sound/pci/hda/hda_intel.c
+++ b/sound/pci/hda/hda_intel.c
@@ -134,8 +134,8 @@ MODULE_PARM_DESC(power_save, "Automatic power-saving timeout "
  * this may give more power-saving, but will take longer time to
  * wake up.
  */
-static int power_save_controller = -1;
-module_param(power_save_controller, bint, 0644);
+static bool power_save_controller = 1;
+module_param(power_save_controller, bool, 0644);
 MODULE_PARM_DESC(power_save_controller, "Reset controller in power save mode.");
 #endif /* CONFIG_PM */
 
@@ -2931,8 +2931,6 @@ static int azx_runtime_idle(struct device *dev)
 	struct snd_card *card = dev_get_drvdata(dev);
 	struct azx *chip = card->private_data;
 
-	if (power_save_controller > 0)
-		return 0;
 	if (!power_save_controller ||
 	    !(chip->driver_caps & AZX_DCAPS_PM_RUNTIME))
 		return -EBUSY;
diff --git a/sound/pci/hda/patch_hdmi.c b/sound/pci/hda/patch_hdmi.c
index 78e1827d0a9..de8ac5c07fd 100644
--- a/sound/pci/hda/patch_hdmi.c
+++ b/sound/pci/hda/patch_hdmi.c
@@ -1196,7 +1196,7 @@ static void hdmi_present_sense(struct hdmi_spec_per_pin *per_pin, int repoll)
 
 	_snd_printd(SND_PR_VERBOSE,
 		"HDMI status: Codec=%d Pin=%d Presence_Detect=%d ELD_Valid=%d\n",
-		codec->addr, pin_nid, eld->monitor_present, eld->eld_valid);
+		codec->addr, pin_nid, pin_eld->monitor_present, eld->eld_valid);
 
 	if (eld->eld_valid) {
 		if (snd_hdmi_get_eld(codec, pin_nid, eld->eld_buffer,
diff --git a/sound/pci/hda/patch_realtek.c b/sound/pci/hda/patch_realtek.c
index 563c24df4d6..f15c36bde54 100644
--- a/sound/pci/hda/patch_realtek.c
+++ b/sound/pci/hda/patch_realtek.c
@@ -3440,7 +3440,8 @@ static int alc662_parse_auto_config(struct hda_codec *codec)
 	const hda_nid_t *ssids;
 
 	if (codec->vendor_id == 0x10ec0272 || codec->vendor_id == 0x10ec0663 ||
-	    codec->vendor_id == 0x10ec0665 || codec->vendor_id == 0x10ec0670)
+	    codec->vendor_id == 0x10ec0665 || codec->vendor_id == 0x10ec0670 ||
+	    codec->vendor_id == 0x10ec0671)
 		ssids = alc663_ssids;
 	else
 		ssids = alc662_ssids;
@@ -3894,6 +3895,7 @@ static const struct hda_codec_preset snd_hda_preset_realtek[] = {
 	{ .id = 0x10ec0665, .name = "ALC665", .patch = patch_alc662 },
 	{ .id = 0x10ec0668, .name = "ALC668", .patch = patch_alc662 },
 	{ .id = 0x10ec0670, .name = "ALC670", .patch = patch_alc662 },
+	{ .id = 0x10ec0671, .name = "ALC671", .patch = patch_alc662 },
 	{ .id = 0x10ec0680, .name = "ALC680", .patch = patch_alc680 },
 	{ .id = 0x10ec0880, .name = "ALC880", .patch = patch_alc880 },
 	{ .id = 0x10ec0882, .name = "ALC882", .patch = patch_alc882 },
diff --git a/sound/pci/mixart/mixart.c b/sound/pci/mixart/mixart.c
index 01f7f37a841..934dec98e2c 100644
--- a/sound/pci/mixart/mixart.c
+++ b/sound/pci/mixart/mixart.c
@@ -1175,7 +1175,7 @@ static void snd_mixart_proc_read(struct snd_info_entry *entry,
 
 			snd_iprintf(buffer, "\tstreaming          : %d\n", streaming);
 			snd_iprintf(buffer, "\tmailbox            : %d\n", mailbox);
-			snd_iprintf(buffer, "\tinterrups handling : %d\n\n", interr);
+			snd_iprintf(buffer, "\tinterrupts handling : %d\n\n", interr);
 		}
 	} /* endif elf loaded */
 }
diff --git a/sound/pcmcia/pdaudiocf/pdaudiocf.c b/sound/pcmcia/pdaudiocf/pdaudiocf.c
index f9b5229b272..8f489de5c4c 100644
--- a/sound/pcmcia/pdaudiocf/pdaudiocf.c
+++ b/sound/pcmcia/pdaudiocf/pdaudiocf.c
@@ -295,18 +295,5 @@ static struct pcmcia_driver pdacf_cs_driver = {
 	.suspend	= pdacf_suspend,
 	.resume		= pdacf_resume,
 #endif
-
 };
-
-static int __init init_pdacf(void)
-{
-	return pcmcia_register_driver(&pdacf_cs_driver);
-}
-
-static void __exit exit_pdacf(void)
-{
-	pcmcia_unregister_driver(&pdacf_cs_driver);
-}
-
-module_init(init_pdacf);
-module_exit(exit_pdacf);
+module_pcmcia_driver(pdacf_cs_driver);
diff --git a/sound/pcmcia/vx/vxpocket.c b/sound/pcmcia/vx/vxpocket.c
index 8f9350475c7..d4db7ecaa6b 100644
--- a/sound/pcmcia/vx/vxpocket.c
+++ b/sound/pcmcia/vx/vxpocket.c
@@ -367,16 +367,4 @@ static struct pcmcia_driver vxp_cs_driver = {
 	.resume		= vxp_resume,
 #endif
 };
-
-static int __init init_vxpocket(void)
-{
-	return pcmcia_register_driver(&vxp_cs_driver);
-}
-
-static void __exit exit_vxpocket(void)
-{
-	pcmcia_unregister_driver(&vxp_cs_driver);
-}
-
-module_init(init_vxpocket);
-module_exit(exit_vxpocket);
+module_pcmcia_driver(vxp_cs_driver);
diff --git a/sound/soc/codecs/arizona.c b/sound/soc/codecs/arizona.c
index ac948a671ea..e7d34711412 100644
--- a/sound/soc/codecs/arizona.c
+++ b/sound/soc/codecs/arizona.c
@@ -364,6 +364,39 @@ int arizona_out_ev(struct snd_soc_dapm_widget *w,
 }
 EXPORT_SYMBOL_GPL(arizona_out_ev);
 
+int arizona_hp_ev(struct snd_soc_dapm_widget *w,
+		   struct snd_kcontrol *kcontrol,
+		   int event)
+{
+	struct arizona_priv *priv = snd_soc_codec_get_drvdata(w->codec);
+	unsigned int mask = 1 << w->shift;
+	unsigned int val;
+
+	switch (event) {
+	case SND_SOC_DAPM_POST_PMU:
+		val = mask;
+		break;
+	case SND_SOC_DAPM_PRE_PMD:
+		val = 0;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* Store the desired state for the HP outputs */
+	priv->arizona->hp_ena &= ~mask;
+	priv->arizona->hp_ena |= val;
+
+	/* Force off if HPDET magic is active */
+	if (priv->arizona->hpdet_magic)
+		val = 0;
+
+	snd_soc_update_bits(w->codec, ARIZONA_OUTPUT_ENABLES_1, mask, val);
+
+	return arizona_out_ev(w, kcontrol, event);
+}
+EXPORT_SYMBOL_GPL(arizona_hp_ev);
+
 static unsigned int arizona_sysclk_48k_rates[] = {
 	6144000,
 	12288000,
diff --git a/sound/soc/codecs/arizona.h b/sound/soc/codecs/arizona.h
index 116372c91f5..13dd2916b72 100644
--- a/sound/soc/codecs/arizona.h
+++ b/sound/soc/codecs/arizona.h
@@ -184,6 +184,9 @@ extern int arizona_in_ev(struct snd_soc_dapm_widget *w,
 extern int arizona_out_ev(struct snd_soc_dapm_widget *w,
 			  struct snd_kcontrol *kcontrol,
 			  int event);
+extern int arizona_hp_ev(struct snd_soc_dapm_widget *w,
+			 struct snd_kcontrol *kcontrol,
+			 int event);
 
 extern int arizona_set_sysclk(struct snd_soc_codec *codec, int clk_id,
 			      int source, unsigned int freq, int dir);
diff --git a/sound/soc/codecs/max98090.c b/sound/soc/codecs/max98090.c
index fc176044994..fc176044994 100755..100644
--- a/sound/soc/codecs/max98090.c
+++ b/sound/soc/codecs/max98090.c
diff --git a/sound/soc/codecs/max98090.h b/sound/soc/codecs/max98090.h
index 7e103f24905..7e103f24905 100755..100644
--- a/sound/soc/codecs/max98090.h
+++ b/sound/soc/codecs/max98090.h
diff --git a/sound/soc/codecs/si476x.c b/sound/soc/codecs/si476x.c
index f2d61a18783..566ea3256e2 100644
--- a/sound/soc/codecs/si476x.c
+++ b/sound/soc/codecs/si476x.c
@@ -159,6 +159,7 @@ static int si476x_codec_hw_params(struct snd_pcm_substream *substream,
 	switch (params_format(params)) {
 	case SNDRV_PCM_FORMAT_S8:
 		width = SI476X_PCM_FORMAT_S8;
+		break;
 	case SNDRV_PCM_FORMAT_S16_LE:
 		width = SI476X_PCM_FORMAT_S16_LE;
 		break;
diff --git a/sound/soc/codecs/wm5102.c b/sound/soc/codecs/wm5102.c
index b82bbf58414..15bc31f1abb 100644
--- a/sound/soc/codecs/wm5102.c
+++ b/sound/soc/codecs/wm5102.c
@@ -584,7 +584,7 @@ static int wm5102_sysclk_ev(struct snd_soc_dapm_widget *w,
 			    struct snd_kcontrol *kcontrol, int event)
 {
 	struct snd_soc_codec *codec = w->codec;
-	struct arizona *arizona = dev_get_drvdata(codec->dev);
+	struct arizona *arizona = dev_get_drvdata(codec->dev->parent);
 	struct regmap *regmap = codec->control_data;
 	const struct reg_default *patch = NULL;
 	int i, patch_size;
@@ -1131,11 +1131,11 @@ ARIZONA_DSP_WIDGETS(DSP1, "DSP1"),
 SND_SOC_DAPM_VALUE_MUX("AEC Loopback", ARIZONA_DAC_AEC_CONTROL_1,
 		       ARIZONA_AEC_LOOPBACK_ENA, 0, &wm5102_aec_loopback_mux),
 
-SND_SOC_DAPM_PGA_E("OUT1L", ARIZONA_OUTPUT_ENABLES_1,
-		   ARIZONA_OUT1L_ENA_SHIFT, 0, NULL, 0, arizona_out_ev,
+SND_SOC_DAPM_PGA_E("OUT1L", SND_SOC_NOPM,
+		   ARIZONA_OUT1L_ENA_SHIFT, 0, NULL, 0, arizona_hp_ev,
 		   SND_SOC_DAPM_PRE_PMD | SND_SOC_DAPM_POST_PMU),
-SND_SOC_DAPM_PGA_E("OUT1R", ARIZONA_OUTPUT_ENABLES_1,
-		   ARIZONA_OUT1R_ENA_SHIFT, 0, NULL, 0, arizona_out_ev,
+SND_SOC_DAPM_PGA_E("OUT1R", SND_SOC_NOPM,
+		   ARIZONA_OUT1R_ENA_SHIFT, 0, NULL, 0, arizona_hp_ev,
 		   SND_SOC_DAPM_PRE_PMD | SND_SOC_DAPM_POST_PMU),
 SND_SOC_DAPM_PGA_E("OUT2L", ARIZONA_OUTPUT_ENABLES_1,
 		   ARIZONA_OUT2L_ENA_SHIFT, 0, NULL, 0, arizona_out_ev,
diff --git a/sound/soc/codecs/wm5110.c b/sound/soc/codecs/wm5110.c
index cdeb301da1f..7841b42a819 100644
--- a/sound/soc/codecs/wm5110.c
+++ b/sound/soc/codecs/wm5110.c
@@ -551,11 +551,11 @@ SND_SOC_DAPM_AIF_IN("AIF3RX1", NULL, 0,
 SND_SOC_DAPM_AIF_IN("AIF3RX2", NULL, 0,
 		    ARIZONA_AIF3_RX_ENABLES, ARIZONA_AIF3RX2_ENA_SHIFT, 0),
 
-SND_SOC_DAPM_PGA_E("OUT1L", ARIZONA_OUTPUT_ENABLES_1,
-		   ARIZONA_OUT1L_ENA_SHIFT, 0, NULL, 0, arizona_out_ev,
+SND_SOC_DAPM_PGA_E("OUT1L", SND_SOC_NOPM,
+		   ARIZONA_OUT1L_ENA_SHIFT, 0, NULL, 0, arizona_hp_ev,
 		   SND_SOC_DAPM_PRE_PMD | SND_SOC_DAPM_POST_PMU),
-SND_SOC_DAPM_PGA_E("OUT1R", ARIZONA_OUTPUT_ENABLES_1,
-		   ARIZONA_OUT1R_ENA_SHIFT, 0, NULL, 0, arizona_out_ev,
+SND_SOC_DAPM_PGA_E("OUT1R", SND_SOC_NOPM,
+		   ARIZONA_OUT1R_ENA_SHIFT, 0, NULL, 0, arizona_hp_ev,
 		   SND_SOC_DAPM_PRE_PMD | SND_SOC_DAPM_POST_PMU),
 SND_SOC_DAPM_PGA_E("OUT2L", ARIZONA_OUTPUT_ENABLES_1,
 		   ARIZONA_OUT2L_ENA_SHIFT, 0, NULL, 0, arizona_out_ev,
diff --git a/sound/soc/codecs/wm8903.c b/sound/soc/codecs/wm8903.c
index 134e41c870b..f8a31ad0b20 100644
--- a/sound/soc/codecs/wm8903.c
+++ b/sound/soc/codecs/wm8903.c
@@ -1083,6 +1083,8 @@ static const struct snd_soc_dapm_route wm8903_intercon[] = {
 	{ "ROP", NULL, "Right Speaker PGA" },
 	{ "RON", NULL, "Right Speaker PGA" },
 
+	{ "Charge Pump", NULL, "CLK_DSP" },
+
 	{ "Left Headphone Output PGA", NULL, "Charge Pump" },
 	{ "Right Headphone Output PGA", NULL, "Charge Pump" },
 	{ "Left Line Output PGA", NULL, "Charge Pump" },
diff --git a/sound/soc/codecs/wm8974.c b/sound/soc/codecs/wm8974.c
index b47c252ef90..a2d01d10a5d 100644
--- a/sound/soc/codecs/wm8974.c
+++ b/sound/soc/codecs/wm8974.c
@@ -125,7 +125,7 @@ SOC_ENUM("Equaliser EQ4 Bandwidth", wm8974_enum[9]),
 SOC_ENUM("EQ4 Cut Off", wm8974_enum[10]),
 SOC_SINGLE_TLV("EQ4 Volume", WM8974_EQ4,  0, 24, 1, eq_tlv),
 
-SOC_ENUM("Equaliser EQ5 Bandwith", wm8974_enum[11]),
+SOC_ENUM("Equaliser EQ5 Bandwidth", wm8974_enum[11]),
 SOC_ENUM("EQ5 Cut Off", wm8974_enum[12]),
 SOC_SINGLE_TLV("EQ5 Volume", WM8974_EQ5,  0, 24, 1, eq_tlv),
 
diff --git a/sound/soc/codecs/wm_adsp.c b/sound/soc/codecs/wm_adsp.c
index f3f7e75f862..9af1bddc4c6 100644
--- a/sound/soc/codecs/wm_adsp.c
+++ b/sound/soc/codecs/wm_adsp.c
@@ -828,7 +828,8 @@ static int wm_adsp_load_coeff(struct wm_adsp *dsp)
 						&buf_list);
 			if (!buf) {
 				adsp_err(dsp, "Out of memory\n");
-				return -ENOMEM;
+				ret = -ENOMEM;
+				goto out_fw;
 			}
 
 			adsp_dbg(dsp, "%s.%d: Writing %d bytes at %x\n",
@@ -865,7 +866,7 @@ out_fw:
 	wm_adsp_buf_free(&buf_list);
 out:
 	kfree(file);
-	return 0;
+	return ret;
 }
 
 int wm_adsp1_init(struct wm_adsp *adsp)
diff --git a/sound/soc/fsl/imx-ssi.c b/sound/soc/fsl/imx-ssi.c
index 55464a5b070..810c7eeb7b0 100644
--- a/sound/soc/fsl/imx-ssi.c
+++ b/sound/soc/fsl/imx-ssi.c
@@ -496,6 +496,8 @@ static void imx_ssi_ac97_reset(struct snd_ac97 *ac97)
 
 	if (imx_ssi->ac97_reset)
 		imx_ssi->ac97_reset(ac97);
+	/* First read sometimes fails, do a dummy read */
+	imx_ssi_ac97_read(ac97, 0);
 }
 
 static void imx_ssi_ac97_warm_reset(struct snd_ac97 *ac97)
@@ -504,6 +506,9 @@ static void imx_ssi_ac97_warm_reset(struct snd_ac97 *ac97)
 
 	if (imx_ssi->ac97_warm_reset)
 		imx_ssi->ac97_warm_reset(ac97);
+
+	/* First read sometimes fails, do a dummy read */
+	imx_ssi_ac97_read(ac97, 0);
 }
 
 struct snd_ac97_bus_ops soc_ac97_ops = {
diff --git a/sound/soc/fsl/pcm030-audio-fabric.c b/sound/soc/fsl/pcm030-audio-fabric.c
index 8e52c1485df..eb4373840bb 100644
--- a/sound/soc/fsl/pcm030-audio-fabric.c
+++ b/sound/soc/fsl/pcm030-audio-fabric.c
@@ -51,7 +51,7 @@ static struct snd_soc_card pcm030_card = {
 	.num_links = ARRAY_SIZE(pcm030_fabric_dai),
 };
 
-static int __init pcm030_fabric_probe(struct platform_device *op)
+static int pcm030_fabric_probe(struct platform_device *op)
 {
 	struct device_node *np = op->dev.of_node;
 	struct device_node *platform_np;
diff --git a/sound/soc/samsung/i2s.c b/sound/soc/samsung/i2s.c
index d7231e336a7..6bbeb0bf1a7 100644
--- a/sound/soc/samsung/i2s.c
+++ b/sound/soc/samsung/i2s.c
@@ -972,6 +972,7 @@ static const struct snd_soc_dai_ops samsung_i2s_dai_ops = {
 static struct i2s_dai *i2s_alloc_dai(struct platform_device *pdev, bool sec)
 {
 	struct i2s_dai *i2s;
+	int ret;
 
 	i2s = devm_kzalloc(&pdev->dev, sizeof(struct i2s_dai), GFP_KERNEL);
 	if (i2s == NULL)
@@ -996,15 +997,17 @@ static struct i2s_dai *i2s_alloc_dai(struct platform_device *pdev, bool sec)
 		i2s->i2s_dai_drv.capture.channels_max = 2;
 		i2s->i2s_dai_drv.capture.rates = SAMSUNG_I2S_RATES;
 		i2s->i2s_dai_drv.capture.formats = SAMSUNG_I2S_FMTS;
+		dev_set_drvdata(&i2s->pdev->dev, i2s);
 	} else {	/* Create a new platform_device for Secondary */
-		i2s->pdev = platform_device_register_resndata(NULL,
-				"samsung-i2s-sec", -1, NULL, 0, NULL, 0);
+		i2s->pdev = platform_device_alloc("samsung-i2s-sec", -1);
 		if (IS_ERR(i2s->pdev))
 			return NULL;
-	}
 
-	/* Pre-assign snd_soc_dai_set_drvdata */
-	dev_set_drvdata(&i2s->pdev->dev, i2s);
+		platform_set_drvdata(i2s->pdev, i2s);
+		ret = platform_device_add(i2s->pdev);
+		if (ret < 0)
+			return NULL;
+	}
 
 	return i2s;
 }
@@ -1107,6 +1110,10 @@ static int samsung_i2s_probe(struct platform_device *pdev)
 
 	if (samsung_dai_type == TYPE_SEC) {
 		sec_dai = dev_get_drvdata(&pdev->dev);
+		if (!sec_dai) {
+			dev_err(&pdev->dev, "Unable to get drvdata\n");
+			return -EFAULT;
+		}
 		snd_soc_register_dai(&sec_dai->pdev->dev,
 			&sec_dai->i2s_dai_drv);
 		asoc_dma_platform_register(&pdev->dev);
diff --git a/sound/soc/sh/dma-sh7760.c b/sound/soc/sh/dma-sh7760.c
index 19eff8fc4fd..1a8b03e4b41 100644
--- a/sound/soc/sh/dma-sh7760.c
+++ b/sound/soc/sh/dma-sh7760.c
@@ -342,8 +342,8 @@ static int camelot_pcm_new(struct snd_soc_pcm_runtime *rtd)
 	return 0;
 }
 
-static struct snd_soc_platform sh7760_soc_platform = {
-	.pcm_ops 	= &camelot_pcm_ops,
+static struct snd_soc_platform_driver sh7760_soc_platform = {
+	.ops		= &camelot_pcm_ops,
 	.pcm_new	= camelot_pcm_new,
 	.pcm_free	= camelot_pcm_free,
 };
diff --git a/sound/soc/soc-compress.c b/sound/soc/soc-compress.c
index b5b3db71e25..ed0bfb0ddb9 100644
--- a/sound/soc/soc-compress.c
+++ b/sound/soc/soc-compress.c
@@ -211,19 +211,27 @@ static int soc_compr_set_params(struct snd_compr_stream *cstream,
 	if (platform->driver->compr_ops && platform->driver->compr_ops->set_params) {
 		ret = platform->driver->compr_ops->set_params(cstream, params);
 		if (ret < 0)
-			goto out;
+			goto err;
 	}
 
 	if (rtd->dai_link->compr_ops && rtd->dai_link->compr_ops->set_params) {
 		ret = rtd->dai_link->compr_ops->set_params(cstream);
 		if (ret < 0)
-			goto out;
+			goto err;
 	}
 
 	snd_soc_dapm_stream_event(rtd, SNDRV_PCM_STREAM_PLAYBACK,
 				SND_SOC_DAPM_STREAM_START);
 
-out:
+	/* cancel any delayed stream shutdown that is pending */
+	rtd->pop_wait = 0;
+	mutex_unlock(&rtd->pcm_mutex);
+
+	cancel_delayed_work_sync(&rtd->delayed_work);
+
+	return ret;
+
+err:
 	mutex_unlock(&rtd->pcm_mutex);
 	return ret;
 }
diff --git a/sound/soc/soc-core.c b/sound/soc/soc-core.c
index b7e84a7cd9e..ff4b45a5d79 100644
--- a/sound/soc/soc-core.c
+++ b/sound/soc/soc-core.c
@@ -2963,7 +2963,7 @@ int snd_soc_put_volsw_range(struct snd_kcontrol *kcontrol,
 	val = val << shift;
 
 	ret = snd_soc_update_bits_locked(codec, reg, val_mask, val);
-	if (ret != 0)
+	if (ret < 0)
 		return ret;
 
 	if (snd_soc_volsw_is_stereo(mc)) {
@@ -3140,7 +3140,7 @@ int snd_soc_bytes_put(struct snd_kcontrol *kcontrol,
 	if (params->mask) {
 		ret = regmap_read(codec->control_data, params->base, &val);
 		if (ret != 0)
-			return ret;
+			goto out;
 
 		val &= params->mask;
 
@@ -3158,13 +3158,15 @@ int snd_soc_bytes_put(struct snd_kcontrol *kcontrol,
 			((u32 *)data)[0] |= cpu_to_be32(val);
 			break;
 		default:
-			return -EINVAL;
+			ret = -EINVAL;
+			goto out;
 		}
 	}
 
 	ret = regmap_raw_write(codec->control_data, params->base,
 			       data, len);
 
+out:
 	kfree(data);
 
 	return ret;
@@ -4197,7 +4199,6 @@ int snd_soc_of_parse_audio_routing(struct snd_soc_card *card,
 			dev_err(card->dev,
 				"ASoC: Property '%s' index %d could not be read: %d\n",
 				propname, 2 * i, ret);
-			kfree(routes);
 			return -EINVAL;
 		}
 		ret = of_property_read_string_index(np, propname,
@@ -4206,7 +4207,6 @@ int snd_soc_of_parse_audio_routing(struct snd_soc_card *card,
 			dev_err(card->dev,
 				"ASoC: Property '%s' index %d could not be read: %d\n",
 				propname, (2 * i) + 1, ret);
-			kfree(routes);
 			return -EINVAL;
 		}
 	}
diff --git a/sound/soc/soc-dapm.c b/sound/soc/soc-dapm.c
index 1d6a9b3ceb2..d6d9ba2e691 100644
--- a/sound/soc/soc-dapm.c
+++ b/sound/soc/soc-dapm.c
@@ -831,6 +831,9 @@ static int is_connected_output_ep(struct snd_soc_dapm_widget *widget,
 		if (path->weak)
 			continue;
 
+		if (path->walking)
+			return 1;
+
 		if (path->walked)
 			continue;
 
@@ -838,6 +841,7 @@ static int is_connected_output_ep(struct snd_soc_dapm_widget *widget,
 
 		if (path->sink && path->connect) {
 			path->walked = 1;
+			path->walking = 1;
 
 			/* do we need to add this widget to the list ? */
 			if (list) {
@@ -847,11 +851,14 @@ static int is_connected_output_ep(struct snd_soc_dapm_widget *widget,
 					dev_err(widget->dapm->dev,
 						"ASoC: could not add widget %s\n",
 						widget->name);
+					path->walking = 0;
 					return con;
 				}
 			}
 
 			con += is_connected_output_ep(path->sink, list);
+
+			path->walking = 0;
 		}
 	}
 
@@ -931,6 +938,9 @@ static int is_connected_input_ep(struct snd_soc_dapm_widget *widget,
 		if (path->weak)
 			continue;
 
+		if (path->walking)
+			return 1;
+
 		if (path->walked)
 			continue;
 
@@ -938,6 +948,7 @@ static int is_connected_input_ep(struct snd_soc_dapm_widget *widget,
 
 		if (path->source && path->connect) {
 			path->walked = 1;
+			path->walking = 1;
 
 			/* do we need to add this widget to the list ? */
 			if (list) {
@@ -947,11 +958,14 @@ static int is_connected_input_ep(struct snd_soc_dapm_widget *widget,
 					dev_err(widget->dapm->dev,
 						"ASoC: could not add widget %s\n",
 						widget->name);
+					path->walking = 0;
 					return con;
 				}
 			}
 
 			con += is_connected_input_ep(path->source, list);
+
+			path->walking = 0;
 		}
 	}
 
diff --git a/sound/soc/spear/spear_pcm.c b/sound/soc/spear/spear_pcm.c
index 9b76cc5a114..5e7aebe1e66 100644
--- a/sound/soc/spear/spear_pcm.c
+++ b/sound/soc/spear/spear_pcm.c
@@ -149,9 +149,9 @@ static void spear_pcm_free(struct snd_pcm *pcm)
 
 static u64 spear_pcm_dmamask = DMA_BIT_MASK(32);
 
-static int spear_pcm_new(struct snd_card *card,
-		struct snd_soc_dai *dai, struct snd_pcm *pcm)
+static int spear_pcm_new(struct snd_soc_pcm_runtime *rtd)
 {
+	struct snd_card *card = rtd->card->snd_card;
 	int ret;
 
 	if (!card->dev->dma_mask)
@@ -159,16 +159,16 @@ static int spear_pcm_new(struct snd_card *card,
 	if (!card->dev->coherent_dma_mask)
 		card->dev->coherent_dma_mask = DMA_BIT_MASK(32);
 
-	if (dai->driver->playback.channels_min) {
-		ret = spear_pcm_preallocate_dma_buffer(pcm,
+	if (rtd->cpu_dai->driver->playback.channels_min) {
+		ret = spear_pcm_preallocate_dma_buffer(rtd->pcm,
 				SNDRV_PCM_STREAM_PLAYBACK,
 				spear_pcm_hardware.buffer_bytes_max);
 		if (ret)
 			return ret;
 	}
 
-	if (dai->driver->capture.channels_min) {
-		ret = spear_pcm_preallocate_dma_buffer(pcm,
+	if (rtd->cpu_dai->driver->capture.channels_min) {
+		ret = spear_pcm_preallocate_dma_buffer(rtd->pcm,
 				SNDRV_PCM_STREAM_CAPTURE,
 				spear_pcm_hardware.buffer_bytes_max);
 		if (ret)
diff --git a/sound/soc/tegra/tegra_pcm.c b/sound/soc/tegra/tegra_pcm.c
index c925ab0adeb..5e2c55c5b25 100644
--- a/sound/soc/tegra/tegra_pcm.c
+++ b/sound/soc/tegra/tegra_pcm.c
@@ -43,8 +43,6 @@
 static const struct snd_pcm_hardware tegra_pcm_hardware = {
 	.info			= SNDRV_PCM_INFO_MMAP |
 				  SNDRV_PCM_INFO_MMAP_VALID |
-				  SNDRV_PCM_INFO_PAUSE |
-				  SNDRV_PCM_INFO_RESUME |
 				  SNDRV_PCM_INFO_INTERLEAVED,
 	.formats		= SNDRV_PCM_FMTBIT_S16_LE,
 	.channels_min		= 2,
@@ -127,26 +125,6 @@ static int tegra_pcm_hw_free(struct snd_pcm_substream *substream)
 	return 0;
 }
 
-static int tegra_pcm_trigger(struct snd_pcm_substream *substream, int cmd)
-{
-	switch (cmd) {
-	case SNDRV_PCM_TRIGGER_START:
-	case SNDRV_PCM_TRIGGER_RESUME:
-	case SNDRV_PCM_TRIGGER_PAUSE_RELEASE:
-		return snd_dmaengine_pcm_trigger(substream,
-					SNDRV_PCM_TRIGGER_START);
-
-	case SNDRV_PCM_TRIGGER_STOP:
-	case SNDRV_PCM_TRIGGER_SUSPEND:
-	case SNDRV_PCM_TRIGGER_PAUSE_PUSH:
-		return snd_dmaengine_pcm_trigger(substream,
-					SNDRV_PCM_TRIGGER_STOP);
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
 static int tegra_pcm_mmap(struct snd_pcm_substream *substream,
 				struct vm_area_struct *vma)
 {
@@ -164,7 +142,7 @@ static struct snd_pcm_ops tegra_pcm_ops = {
 	.ioctl		= snd_pcm_lib_ioctl,
 	.hw_params	= tegra_pcm_hw_params,
 	.hw_free	= tegra_pcm_hw_free,
-	.trigger	= tegra_pcm_trigger,
+	.trigger	= snd_dmaengine_pcm_trigger,
 	.pointer	= snd_dmaengine_pcm_pointer,
 	.mmap		= tegra_pcm_mmap,
 };
diff --git a/sound/soc/ux500/Kconfig b/sound/soc/ux500/Kconfig
index 069330d82be..6b799c0e88d 100644
--- a/sound/soc/ux500/Kconfig
+++ b/sound/soc/ux500/Kconfig
@@ -20,8 +20,8 @@ config SND_SOC_UX500_PLAT_DMA
 	help
 		Say Y if you want to enable the Ux500 platform-driver.
 
-+config SND_SOC_UX500_MACH_MOP500
-+	tristate "Machine - MOP500 (Ux500 + AB8500)"
+config SND_SOC_UX500_MACH_MOP500
+	tristate "Machine - MOP500 (Ux500 + AB8500)"
 	depends on AB8500_CORE && AB8500_GPADC && SND_SOC_UX500
 	select SND_SOC_AB8500_CODEC
 	select SND_SOC_UX500_PLAT_MSP_I2S
diff --git a/sound/usb/clock.c b/sound/usb/clock.c
index 5e634a2eb28..9e2703a2515 100644
--- a/sound/usb/clock.c
+++ b/sound/usb/clock.c
@@ -253,7 +253,7 @@ static int set_sample_rate_v2(struct snd_usb_audio *chip, int iface,
 {
 	struct usb_device *dev = chip->dev;
 	unsigned char data[4];
-	int err, crate;
+	int err, cur_rate, prev_rate;
 	int clock = snd_usb_clock_find_source(chip, fmt->clock);
 
 	if (clock < 0)
@@ -266,6 +266,19 @@ static int set_sample_rate_v2(struct snd_usb_audio *chip, int iface,
 		return -ENXIO;
 	}
 
+	err = snd_usb_ctl_msg(dev, usb_rcvctrlpipe(dev, 0), UAC2_CS_CUR,
+			      USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_IN,
+			      UAC2_CS_CONTROL_SAM_FREQ << 8,
+			      snd_usb_ctrl_intf(chip) | (clock << 8),
+			      data, sizeof(data));
+	if (err < 0) {
+		snd_printk(KERN_WARNING "%d:%d:%d: cannot get freq (v2)\n",
+			   dev->devnum, iface, fmt->altsetting);
+		prev_rate = 0;
+	} else {
+		prev_rate = data[0] | (data[1] << 8) | (data[2] << 16) | (data[3] << 24);
+	}
+
 	data[0] = rate;
 	data[1] = rate >> 8;
 	data[2] = rate >> 16;
@@ -280,19 +293,31 @@ static int set_sample_rate_v2(struct snd_usb_audio *chip, int iface,
 		return err;
 	}
 
-	if ((err = snd_usb_ctl_msg(dev, usb_rcvctrlpipe(dev, 0), UAC2_CS_CUR,
-				   USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_IN,
-				   UAC2_CS_CONTROL_SAM_FREQ << 8,
-				   snd_usb_ctrl_intf(chip) | (clock << 8),
-				   data, sizeof(data))) < 0) {
+	err = snd_usb_ctl_msg(dev, usb_rcvctrlpipe(dev, 0), UAC2_CS_CUR,
+			      USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_IN,
+			      UAC2_CS_CONTROL_SAM_FREQ << 8,
+			      snd_usb_ctrl_intf(chip) | (clock << 8),
+			      data, sizeof(data));
+	if (err < 0) {
 		snd_printk(KERN_WARNING "%d:%d:%d: cannot get freq (v2)\n",
 			   dev->devnum, iface, fmt->altsetting);
-		return err;
+		cur_rate = 0;
+	} else {
+		cur_rate = data[0] | (data[1] << 8) | (data[2] << 16) | (data[3] << 24);
 	}
 
-	crate = data[0] | (data[1] << 8) | (data[2] << 16) | (data[3] << 24);
-	if (crate != rate)
-		snd_printd(KERN_WARNING "current rate %d is different from the runtime rate %d\n", crate, rate);
+	if (cur_rate != rate) {
+		snd_printd(KERN_WARNING
+			   "current rate %d is different from the runtime rate %d\n",
+			   cur_rate, rate);
+	}
+
+	/* Some devices doesn't respond to sample rate changes while the
+	 * interface is active. */
+	if (rate != prev_rate) {
+		usb_set_interface(dev, iface, 0);
+		usb_set_interface(dev, iface, fmt->altsetting);
+	}
 
 	return 0;
 }
diff --git a/sound/usb/mixer_quirks.c b/sound/usb/mixer_quirks.c
index 497d2741d11..ebe91440a06 100644
--- a/sound/usb/mixer_quirks.c
+++ b/sound/usb/mixer_quirks.c
@@ -509,7 +509,7 @@ static int snd_nativeinstruments_control_get(struct snd_kcontrol *kcontrol,
 	else
 		ret = usb_control_msg(dev, usb_rcvctrlpipe(dev, 0), bRequest,
 				  USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
-				  0, cpu_to_le16(wIndex),
+				  0, wIndex,
 				  &tmp, sizeof(tmp), 1000);
 	up_read(&mixer->chip->shutdown_rwsem);
 
@@ -540,7 +540,7 @@ static int snd_nativeinstruments_control_put(struct snd_kcontrol *kcontrol,
 	else
 		ret = usb_control_msg(dev, usb_sndctrlpipe(dev, 0), bRequest,
 				  USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_OUT,
-				  cpu_to_le16(wValue), cpu_to_le16(wIndex),
+				  wValue, wIndex,
 				  NULL, 0, 1000);
 	up_read(&mixer->chip->shutdown_rwsem);
 
diff --git a/sound/usb/quirks.c b/sound/usb/quirks.c
index 5325a3869bb..9c5ab22358b 100644
--- a/sound/usb/quirks.c
+++ b/sound/usb/quirks.c
@@ -486,7 +486,7 @@ static int snd_usb_nativeinstruments_boot_quirk(struct usb_device *dev)
 {
 	int ret = usb_control_msg(dev, usb_sndctrlpipe(dev, 0),
 				  0xaf, USB_TYPE_VENDOR | USB_RECIP_DEVICE,
-				  cpu_to_le16(1), 0, NULL, 0, 1000);
+				  1, 0, NULL, 0, 1000);
 
 	if (ret < 0)
 		return ret;
diff --git a/tools/Makefile b/tools/Makefile
index fa36565b209..6aaeb6cd867 100644
--- a/tools/Makefile
+++ b/tools/Makefile
@@ -34,7 +34,13 @@ help:
 cpupower: FORCE
 	$(call descend,power/$@)
 
-cgroup firewire lguest perf usb virtio vm: FORCE
+cgroup firewire guest usb virtio vm: FORCE
+	$(call descend,$@)
+
+liblk: FORCE
+	$(call descend,lib/lk)
+
+perf: liblk FORCE
 	$(call descend,$@)
 
 selftests: FORCE
@@ -62,7 +68,13 @@ install: cgroup_install cpupower_install firewire_install lguest_install \
 cpupower_clean:
 	$(call descend,power/cpupower,clean)
 
-cgroup_clean firewire_clean lguest_clean perf_clean usb_clean virtio_clean vm_clean:
+cgroup_clean firewire_clean lguest_clean usb_clean virtio_clean vm_clean:
+	$(call descend,$(@:_clean=),clean)
+
+liblk_clean:
+	$(call descend,lib/lk,clean)
+
+perf_clean: liblk_clean
 	$(call descend,$(@:_clean=),clean)
 
 selftests_clean:
diff --git a/tools/hv/hv_kvp_daemon.c b/tools/hv/hv_kvp_daemon.c
index c800ea4c8bf..5a1f6489d18 100644
--- a/tools/hv/hv_kvp_daemon.c
+++ b/tools/hv/hv_kvp_daemon.c
@@ -102,6 +102,10 @@ static struct utsname uts_buf;
 #define MAX_FILE_NAME 100
 #define ENTRIES_PER_BLOCK 50
 
+#ifndef SOL_NETLINK
+#define SOL_NETLINK 270
+#endif
+
 struct kvp_record {
 	char key[HV_KVP_EXCHANGE_MAX_KEY_SIZE];
 	char value[HV_KVP_EXCHANGE_MAX_VALUE_SIZE];
@@ -1407,7 +1411,7 @@ netlink_send(int fd, struct cn_msg *msg)
 
 int main(void)
 {
-	int fd, len, sock_opt;
+	int fd, len, nl_group;
 	int error;
 	struct cn_msg *message;
 	struct pollfd pfd;
@@ -1443,7 +1447,7 @@ int main(void)
 	addr.nl_family = AF_NETLINK;
 	addr.nl_pad = 0;
 	addr.nl_pid = 0;
-	addr.nl_groups = CN_KVP_IDX;
+	addr.nl_groups = 0;
 
 
 	error = bind(fd, (struct sockaddr *)&addr, sizeof(addr));
@@ -1452,8 +1456,8 @@ int main(void)
 		close(fd);
 		exit(EXIT_FAILURE);
 	}
-	sock_opt = addr.nl_groups;
-	setsockopt(fd, 270, 1, &sock_opt, sizeof(sock_opt));
+	nl_group = CN_KVP_IDX;
+	setsockopt(fd, SOL_NETLINK, NETLINK_ADD_MEMBERSHIP, &nl_group, sizeof(nl_group));
 	/*
 	 * Register ourselves with the kernel.
 	 */
@@ -1499,6 +1503,10 @@ int main(void)
 		}
 
 		incoming_msg = (struct nlmsghdr *)kvp_recv_buffer;
+
+		if (incoming_msg->nlmsg_type != NLMSG_DONE)
+			continue;
+
 		incoming_cn_msg = (struct cn_msg *)NLMSG_DATA(incoming_msg);
 		hv_msg = (struct hv_kvp_msg *)incoming_cn_msg->data;
 
diff --git a/tools/hv/hv_vss_daemon.c b/tools/hv/hv_vss_daemon.c
new file mode 100644
index 00000000000..fea03a3edaf
--- /dev/null
+++ b/tools/hv/hv_vss_daemon.c
@@ -0,0 +1,249 @@
+/*
+ * An implementation of the host initiated guest snapshot for Hyper-V.
+ *
+ *
+ * Copyright (C) 2013, Microsoft, Inc.
+ * Author : K. Y. Srinivasan <kys@microsoft.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, GOOD TITLE or
+ * NON INFRINGEMENT.  See the GNU General Public License for more
+ * details.
+ *
+ */
+
+
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/poll.h>
+#include <sys/ioctl.h>
+#include <linux/types.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <mntent.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <ctype.h>
+#include <errno.h>
+#include <arpa/inet.h>
+#include <linux/fs.h>
+#include <linux/connector.h>
+#include <linux/hyperv.h>
+#include <linux/netlink.h>
+#include <syslog.h>
+
+static char vss_recv_buffer[4096];
+static char vss_send_buffer[4096];
+static struct sockaddr_nl addr;
+
+#ifndef SOL_NETLINK
+#define SOL_NETLINK 270
+#endif
+
+
+static int vss_do_freeze(char *dir, unsigned int cmd, char *fs_op)
+{
+	int ret, fd = open(dir, O_RDONLY);
+
+	if (fd < 0)
+		return 1;
+	ret = ioctl(fd, cmd, 0);
+	syslog(LOG_INFO, "VSS: %s of %s: %s\n", fs_op, dir, strerror(errno));
+	close(fd);
+	return !!ret;
+}
+
+static int vss_operate(int operation)
+{
+	char *fs_op;
+	char match[] = "/dev/";
+	FILE *mounts;
+	struct mntent *ent;
+	unsigned int cmd;
+	int error = 0, root_seen = 0;
+
+	switch (operation) {
+	case VSS_OP_FREEZE:
+		cmd = FIFREEZE;
+		fs_op = "freeze";
+		break;
+	case VSS_OP_THAW:
+		cmd = FITHAW;
+		fs_op = "thaw";
+		break;
+	default:
+		return -1;
+	}
+
+	mounts = setmntent("/proc/mounts", "r");
+	if (mounts == NULL)
+		return -1;
+
+	while ((ent = getmntent(mounts))) {
+		if (strncmp(ent->mnt_fsname, match, strlen(match)))
+			continue;
+		if (strcmp(ent->mnt_type, "iso9660") == 0)
+			continue;
+		if (strcmp(ent->mnt_dir, "/") == 0) {
+			root_seen = 1;
+			continue;
+		}
+		error |= vss_do_freeze(ent->mnt_dir, cmd, fs_op);
+	}
+	endmntent(mounts);
+
+	if (root_seen) {
+		error |= vss_do_freeze("/", cmd, fs_op);
+	}
+
+	return error;
+}
+
+static int netlink_send(int fd, struct cn_msg *msg)
+{
+	struct nlmsghdr *nlh;
+	unsigned int size;
+	struct msghdr message;
+	char buffer[64];
+	struct iovec iov[2];
+
+	size = NLMSG_SPACE(sizeof(struct cn_msg) + msg->len);
+
+	nlh = (struct nlmsghdr *)buffer;
+	nlh->nlmsg_seq = 0;
+	nlh->nlmsg_pid = getpid();
+	nlh->nlmsg_type = NLMSG_DONE;
+	nlh->nlmsg_len = NLMSG_LENGTH(size - sizeof(*nlh));
+	nlh->nlmsg_flags = 0;
+
+	iov[0].iov_base = nlh;
+	iov[0].iov_len = sizeof(*nlh);
+
+	iov[1].iov_base = msg;
+	iov[1].iov_len = size;
+
+	memset(&message, 0, sizeof(message));
+	message.msg_name = &addr;
+	message.msg_namelen = sizeof(addr);
+	message.msg_iov = iov;
+	message.msg_iovlen = 2;
+
+	return sendmsg(fd, &message, 0);
+}
+
+int main(void)
+{
+	int fd, len, nl_group;
+	int error;
+	struct cn_msg *message;
+	struct pollfd pfd;
+	struct nlmsghdr *incoming_msg;
+	struct cn_msg	*incoming_cn_msg;
+	int	op;
+	struct hv_vss_msg *vss_msg;
+
+	if (daemon(1, 0))
+		return 1;
+
+	openlog("Hyper-V VSS", 0, LOG_USER);
+	syslog(LOG_INFO, "VSS starting; pid is:%d", getpid());
+
+	fd = socket(AF_NETLINK, SOCK_DGRAM, NETLINK_CONNECTOR);
+	if (fd < 0) {
+		syslog(LOG_ERR, "netlink socket creation failed; error:%d", fd);
+		exit(EXIT_FAILURE);
+	}
+	addr.nl_family = AF_NETLINK;
+	addr.nl_pad = 0;
+	addr.nl_pid = 0;
+	addr.nl_groups = 0;
+
+
+	error = bind(fd, (struct sockaddr *)&addr, sizeof(addr));
+	if (error < 0) {
+		syslog(LOG_ERR, "bind failed; error:%d", error);
+		close(fd);
+		exit(EXIT_FAILURE);
+	}
+	nl_group = CN_VSS_IDX;
+	setsockopt(fd, SOL_NETLINK, NETLINK_ADD_MEMBERSHIP, &nl_group, sizeof(nl_group));
+	/*
+	 * Register ourselves with the kernel.
+	 */
+	message = (struct cn_msg *)vss_send_buffer;
+	message->id.idx = CN_VSS_IDX;
+	message->id.val = CN_VSS_VAL;
+	message->ack = 0;
+	vss_msg = (struct hv_vss_msg *)message->data;
+	vss_msg->vss_hdr.operation = VSS_OP_REGISTER;
+
+	message->len = sizeof(struct hv_vss_msg);
+
+	len = netlink_send(fd, message);
+	if (len < 0) {
+		syslog(LOG_ERR, "netlink_send failed; error:%d", len);
+		close(fd);
+		exit(EXIT_FAILURE);
+	}
+
+	pfd.fd = fd;
+
+	while (1) {
+		struct sockaddr *addr_p = (struct sockaddr *) &addr;
+		socklen_t addr_l = sizeof(addr);
+		pfd.events = POLLIN;
+		pfd.revents = 0;
+		poll(&pfd, 1, -1);
+
+		len = recvfrom(fd, vss_recv_buffer, sizeof(vss_recv_buffer), 0,
+				addr_p, &addr_l);
+
+		if (len < 0) {
+			syslog(LOG_ERR, "recvfrom failed; pid:%u error:%d %s",
+					addr.nl_pid, errno, strerror(errno));
+			close(fd);
+			return -1;
+		}
+
+		if (addr.nl_pid) {
+			syslog(LOG_WARNING,
+				"Received packet from untrusted pid:%u",
+				addr.nl_pid);
+			continue;
+		}
+
+		incoming_msg = (struct nlmsghdr *)vss_recv_buffer;
+
+		if (incoming_msg->nlmsg_type != NLMSG_DONE)
+			continue;
+
+		incoming_cn_msg = (struct cn_msg *)NLMSG_DATA(incoming_msg);
+		vss_msg = (struct hv_vss_msg *)incoming_cn_msg->data;
+		op = vss_msg->vss_hdr.operation;
+		error =  HV_S_OK;
+
+		switch (op) {
+		case VSS_OP_FREEZE:
+		case VSS_OP_THAW:
+			error = vss_operate(op);
+			if (error)
+				error = HV_E_FAIL;
+			break;
+		default:
+			syslog(LOG_ERR, "Illegal op:%d\n", op);
+		}
+		vss_msg->error = error;
+		len = netlink_send(fd, incoming_cn_msg);
+		if (len < 0) {
+			syslog(LOG_ERR, "net_link send failed; error:%d", len);
+			exit(EXIT_FAILURE);
+		}
+	}
+
+}
diff --git a/tools/lib/lk/Makefile b/tools/lib/lk/Makefile
new file mode 100644
index 00000000000..926cbf3efc7
--- /dev/null
+++ b/tools/lib/lk/Makefile
@@ -0,0 +1,35 @@
+include ../../scripts/Makefile.include
+
+# guard against environment variables
+LIB_H=
+LIB_OBJS=
+
+LIB_H += debugfs.h
+
+LIB_OBJS += $(OUTPUT)debugfs.o
+
+LIBFILE = liblk.a
+
+CFLAGS = -ggdb3 -Wall -Wextra -std=gnu99 -Werror -O6 -D_FORTIFY_SOURCE=2 $(EXTRA_WARNINGS) $(EXTRA_CFLAGS) -fPIC
+EXTLIBS = -lpthread -lrt -lelf -lm
+ALL_CFLAGS = $(CFLAGS) $(BASIC_CFLAGS) -D_LARGEFILE64_SOURCE -D_FILE_OFFSET_BITS=64
+ALL_LDFLAGS = $(LDFLAGS)
+
+RM = rm -f
+
+$(LIBFILE): $(LIB_OBJS)
+	$(QUIET_AR)$(RM) $@ && $(AR) rcs $(OUTPUT)$@ $(LIB_OBJS)
+
+$(LIB_OBJS): $(LIB_H)
+
+$(OUTPUT)%.o: %.c
+	$(QUIET_CC)$(CC) -o $@ -c $(ALL_CFLAGS) $<
+$(OUTPUT)%.s: %.c
+	$(QUIET_CC)$(CC) -S $(ALL_CFLAGS) $<
+$(OUTPUT)%.o: %.S
+	$(QUIET_CC)$(CC) -o $@ -c $(ALL_CFLAGS) $<
+
+clean:
+	$(RM) $(LIB_OBJS) $(LIBFILE)
+
+.PHONY: clean
diff --git a/tools/perf/util/debugfs.c b/tools/lib/lk/debugfs.c
index dd8b19319c0..099e7cd022e 100644
--- a/tools/perf/util/debugfs.c
+++ b/tools/lib/lk/debugfs.c
@@ -1,36 +1,39 @@
-#include "util.h"
-#include "debugfs.h"
-#include "cache.h"
-
-#include <linux/kernel.h>
+#include <errno.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <sys/vfs.h>
 #include <sys/mount.h>
+#include <linux/magic.h>
+#include <linux/kernel.h>
+
+#include "debugfs.h"
 
-static int debugfs_premounted;
 char debugfs_mountpoint[PATH_MAX + 1] = "/sys/kernel/debug";
-char tracing_events_path[PATH_MAX + 1] = "/sys/kernel/debug/tracing/events";
 
-static const char *debugfs_known_mountpoints[] = {
+static const char * const debugfs_known_mountpoints[] = {
 	"/sys/kernel/debug/",
 	"/debug/",
 	0,
 };
 
-static int debugfs_found;
+static bool debugfs_found;
 
 /* find the path to the mounted debugfs */
 const char *debugfs_find_mountpoint(void)
 {
-	const char **ptr;
+	const char * const *ptr;
 	char type[100];
 	FILE *fp;
 
 	if (debugfs_found)
-		return (const char *) debugfs_mountpoint;
+		return (const char *)debugfs_mountpoint;
 
 	ptr = debugfs_known_mountpoints;
 	while (*ptr) {
 		if (debugfs_valid_mountpoint(*ptr) == 0) {
-			debugfs_found = 1;
+			debugfs_found = true;
 			strcpy(debugfs_mountpoint, *ptr);
 			return debugfs_mountpoint;
 		}
@@ -52,7 +55,7 @@ const char *debugfs_find_mountpoint(void)
 	if (strcmp(type, "debugfs") != 0)
 		return NULL;
 
-	debugfs_found = 1;
+	debugfs_found = true;
 
 	return debugfs_mountpoint;
 }
@@ -71,21 +74,12 @@ int debugfs_valid_mountpoint(const char *debugfs)
 	return 0;
 }
 
-static void debugfs_set_tracing_events_path(const char *mountpoint)
-{
-	snprintf(tracing_events_path, sizeof(tracing_events_path), "%s/%s",
-		 mountpoint, "tracing/events");
-}
-
 /* mount the debugfs somewhere if it's not mounted */
-
 char *debugfs_mount(const char *mountpoint)
 {
 	/* see if it's already mounted */
-	if (debugfs_find_mountpoint()) {
-		debugfs_premounted = 1;
+	if (debugfs_find_mountpoint())
 		goto out;
-	}
 
 	/* if not mounted and no argument */
 	if (mountpoint == NULL) {
@@ -100,15 +94,8 @@ char *debugfs_mount(const char *mountpoint)
 		return NULL;
 
 	/* save the mountpoint */
-	debugfs_found = 1;
+	debugfs_found = true;
 	strncpy(debugfs_mountpoint, mountpoint, sizeof(debugfs_mountpoint));
 out:
-	debugfs_set_tracing_events_path(debugfs_mountpoint);
 	return debugfs_mountpoint;
 }
-
-void debugfs_set_path(const char *mountpoint)
-{
-	snprintf(debugfs_mountpoint, sizeof(debugfs_mountpoint), "%s", mountpoint);
-	debugfs_set_tracing_events_path(mountpoint);
-}
diff --git a/tools/lib/lk/debugfs.h b/tools/lib/lk/debugfs.h
new file mode 100644
index 00000000000..935c59bdb44
--- /dev/null
+++ b/tools/lib/lk/debugfs.h
@@ -0,0 +1,29 @@
+#ifndef __LK_DEBUGFS_H__
+#define __LK_DEBUGFS_H__
+
+#define _STR(x) #x
+#define STR(x) _STR(x)
+
+/*
+ * On most systems <limits.h> would have given us this, but  not on some systems
+ * (e.g. GNU/Hurd).
+ */
+#ifndef PATH_MAX
+#define PATH_MAX 4096
+#endif
+
+#ifndef DEBUGFS_MAGIC
+#define DEBUGFS_MAGIC          0x64626720
+#endif
+
+#ifndef PERF_DEBUGFS_ENVIRONMENT
+#define PERF_DEBUGFS_ENVIRONMENT "PERF_DEBUGFS_DIR"
+#endif
+
+const char *debugfs_find_mountpoint(void);
+int debugfs_valid_mountpoint(const char *debugfs);
+char *debugfs_mount(const char *mountpoint);
+
+extern char debugfs_mountpoint[];
+
+#endif /* __LK_DEBUGFS_H__ */
diff --git a/tools/perf/Documentation/perf-annotate.txt b/tools/perf/Documentation/perf-annotate.txt
index 5ad07ef417f..e9cd39a92dc 100644
--- a/tools/perf/Documentation/perf-annotate.txt
+++ b/tools/perf/Documentation/perf-annotate.txt
@@ -93,6 +93,9 @@ OPTIONS
 --skip-missing::
 	Skip symbols that cannot be annotated.
 
+--group::
+	Show event group information together
+
 SEE ALSO
 --------
 linkperf:perf-record[1], linkperf:perf-report[1]
diff --git a/tools/perf/Documentation/perf-mem.txt b/tools/perf/Documentation/perf-mem.txt
new file mode 100644
index 00000000000..888d51137fb
--- /dev/null
+++ b/tools/perf/Documentation/perf-mem.txt
@@ -0,0 +1,48 @@
+perf-mem(1)
+===========
+
+NAME
+----
+perf-mem - Profile memory accesses
+
+SYNOPSIS
+--------
+[verse]
+'perf mem' [<options>] (record [<command>] | report)
+
+DESCRIPTION
+-----------
+"perf mem -t <TYPE> record" runs a command and gathers memory operation data
+from it, into perf.data. Perf record options are accepted and are passed through.
+
+"perf mem -t <TYPE> report" displays the result. It invokes perf report with the
+right set of options to display a memory access profile.
+
+OPTIONS
+-------
+<command>...::
+	Any command you can specify in a shell.
+
+-t::
+--type=::
+	Select the memory operation type: load or store (default: load)
+
+-D::
+--dump-raw-samples=::
+	Dump the raw decoded samples on the screen in a format that is easy to parse with
+	one sample per line.
+
+-x::
+--field-separator::
+	Specify the field separator used when dump raw samples (-D option). By default,
+	The separator is the space character.
+
+-C::
+--cpu-list::
+	Restrict dump of raw samples to those provided via this option. Note that the same
+	option can be passed in record mode. It will be interpreted the same way as perf
+	record.
+
+SEE ALSO
+--------
+linkperf:perf-record[1], linkperf:perf-report[1]
diff --git a/tools/perf/Documentation/perf-record.txt b/tools/perf/Documentation/perf-record.txt
index 938e8904f64..d4da111ef53 100644
--- a/tools/perf/Documentation/perf-record.txt
+++ b/tools/perf/Documentation/perf-record.txt
@@ -182,6 +182,12 @@ is enabled for all the sampling events. The sampled branch type is the same for
 The various filters must be specified as a comma separated list: --branch-filter any_ret,u,k
 Note that this feature may not be available on all processors.
 
+-W::
+--weight::
+Enable weightened sampling. An additional weight is recorded per sample and can be
+displayed with the weight and local_weight sort keys.  This currently works for TSX
+abort events and some memory events in precise mode on modern Intel CPUs.
+
 SEE ALSO
 --------
 linkperf:perf-stat[1], linkperf:perf-list[1]
diff --git a/tools/perf/Documentation/perf-report.txt b/tools/perf/Documentation/perf-report.txt
index 02284a0067f..7d5f4f38aa5 100644
--- a/tools/perf/Documentation/perf-report.txt
+++ b/tools/perf/Documentation/perf-report.txt
@@ -59,7 +59,7 @@ OPTIONS
 --sort=::
 	Sort histogram entries by given key(s) - multiple keys can be specified
 	in CSV format.  Following sort keys are available:
-	pid, comm, dso, symbol, parent, cpu, srcline.
+	pid, comm, dso, symbol, parent, cpu, srcline, weight, local_weight.
 
 	Each key has following meaning:
 
@@ -206,6 +206,10 @@ OPTIONS
 --group::
 	Show event group information together.
 
+--demangle::
+	Demangle symbol names to human readable form. It's enabled by default,
+	disable with --no-demangle.
+
 SEE ALSO
 --------
 linkperf:perf-stat[1], linkperf:perf-annotate[1]
diff --git a/tools/perf/Documentation/perf-stat.txt b/tools/perf/Documentation/perf-stat.txt
index faf4f4feebc..2fe87fb558f 100644
--- a/tools/perf/Documentation/perf-stat.txt
+++ b/tools/perf/Documentation/perf-stat.txt
@@ -52,7 +52,7 @@ OPTIONS
 
 -r::
 --repeat=<n>::
-	repeat command and print average + stddev (max: 100)
+	repeat command and print average + stddev (max: 100). 0 means forever.
 
 -B::
 --big-num::
@@ -119,13 +119,19 @@ perf stat --repeat 10 --null --sync --pre 'make -s O=defconfig-build/clean' -- m
 	Print count deltas every N milliseconds (minimum: 100ms)
 	example: perf stat -I 1000 -e cycles -a sleep 5
 
---aggr-socket::
+--per-socket::
 Aggregate counts per processor socket for system-wide mode measurements.  This
 is a useful mode to detect imbalance between sockets.  To enable this mode,
-use --aggr-socket in addition to -a. (system-wide).  The output includes the
+use --per-socket in addition to -a. (system-wide).  The output includes the
 socket number and the number of online processors on that socket. This is
 useful to gauge the amount of aggregation.
 
+--per-core::
+Aggregate counts per physical processor for system-wide mode measurements.  This
+is a useful mode to detect imbalance between physical cores.  To enable this mode,
+use --per-core in addition to -a. (system-wide).  The output includes the
+core number and the number of online logical processors on that physical processor.
+
 EXAMPLES
 --------
 
diff --git a/tools/perf/Documentation/perf-top.txt b/tools/perf/Documentation/perf-top.txt
index a414bc95fd5..9f1a2fe5475 100644
--- a/tools/perf/Documentation/perf-top.txt
+++ b/tools/perf/Documentation/perf-top.txt
@@ -112,7 +112,7 @@ Default is to monitor all CPUS.
 
 -s::
 --sort::
-	Sort by key(s): pid, comm, dso, symbol, parent, srcline.
+	Sort by key(s): pid, comm, dso, symbol, parent, srcline, weight, local_weight.
 
 -n::
 --show-nr-samples::
diff --git a/tools/perf/MANIFEST b/tools/perf/MANIFEST
index 39d41068484..025de796067 100644
--- a/tools/perf/MANIFEST
+++ b/tools/perf/MANIFEST
@@ -1,6 +1,7 @@
 tools/perf
 tools/scripts
 tools/lib/traceevent
+tools/lib/lk
 include/linux/const.h
 include/linux/perf_event.h
 include/linux/rbtree.h
diff --git a/tools/perf/Makefile b/tools/perf/Makefile
index bb74c79cd16..b0f164b133d 100644
--- a/tools/perf/Makefile
+++ b/tools/perf/Makefile
@@ -35,7 +35,9 @@ include config/utilities.mak
 #
 # Define WERROR=0 to disable treating any warnings as errors.
 #
-# Define NO_NEWT if you do not want TUI support.
+# Define NO_NEWT if you do not want TUI support. (deprecated)
+#
+# Define NO_SLANG if you do not want TUI support.
 #
 # Define NO_GTK2 if you do not want GTK+ GUI support.
 #
@@ -104,6 +106,10 @@ ifdef PARSER_DEBUG
 	PARSER_DEBUG_CFLAGS := -DPARSER_DEBUG
 endif
 
+ifdef NO_NEWT
+	NO_SLANG=1
+endif
+
 CFLAGS = -fno-omit-frame-pointer -ggdb3 -funwind-tables -Wall -Wextra -std=gnu99 $(CFLAGS_WERROR) $(CFLAGS_OPTIMIZE) $(EXTRA_WARNINGS) $(EXTRA_CFLAGS) $(PARSER_DEBUG_CFLAGS)
 EXTLIBS = -lpthread -lrt -lelf -lm
 ALL_CFLAGS = $(CFLAGS) -D_LARGEFILE64_SOURCE -D_FILE_OFFSET_BITS=64 -D_GNU_SOURCE
@@ -215,6 +221,7 @@ BASIC_CFLAGS = \
 	-Iutil \
 	-I. \
 	-I$(TRACE_EVENT_DIR) \
+	-I../lib/ \
 	-D_LARGEFILE64_SOURCE -D_FILE_OFFSET_BITS=64 -D_GNU_SOURCE
 
 BASIC_LDFLAGS =
@@ -240,19 +247,28 @@ SCRIPT_SH += perf-archive.sh
 grep-libs = $(filter -l%,$(1))
 strip-libs = $(filter-out -l%,$(1))
 
+LK_DIR = ../lib/lk/
 TRACE_EVENT_DIR = ../lib/traceevent/
 
+LK_PATH=$(LK_DIR)
+
 ifneq ($(OUTPUT),)
 	TE_PATH=$(OUTPUT)
+ifneq ($(subdir),)
+	LK_PATH=$(OUTPUT)$(LK_DIR)
+else
+	LK_PATH=$(OUTPUT)
+endif
 else
 	TE_PATH=$(TRACE_EVENT_DIR)
 endif
 
 LIBTRACEEVENT = $(TE_PATH)libtraceevent.a
-TE_LIB := -L$(TE_PATH) -ltraceevent
-
 export LIBTRACEEVENT
 
+LIBLK = $(LK_PATH)liblk.a
+export LIBLK
+
 # python extension build directories
 PYTHON_EXTBUILD     := $(OUTPUT)python_ext_build/
 PYTHON_EXTBUILD_LIB := $(PYTHON_EXTBUILD)lib/
@@ -262,7 +278,7 @@ export PYTHON_EXTBUILD_LIB PYTHON_EXTBUILD_TMP
 python-clean := rm -rf $(PYTHON_EXTBUILD) $(OUTPUT)python/perf.so
 
 PYTHON_EXT_SRCS := $(shell grep -v ^\# util/python-ext-sources)
-PYTHON_EXT_DEPS := util/python-ext-sources util/setup.py
+PYTHON_EXT_DEPS := util/python-ext-sources util/setup.py $(LIBTRACEEVENT)
 
 $(OUTPUT)python/perf.so: $(PYTHON_EXT_SRCS) $(PYTHON_EXT_DEPS)
 	$(QUIET_GEN)CFLAGS='$(BASIC_CFLAGS)' $(PYTHON_WORD) util/setup.py \
@@ -355,7 +371,6 @@ LIB_H += util/cache.h
 LIB_H += util/callchain.h
 LIB_H += util/build-id.h
 LIB_H += util/debug.h
-LIB_H += util/debugfs.h
 LIB_H += util/sysfs.h
 LIB_H += util/pmu.h
 LIB_H += util/event.h
@@ -416,7 +431,6 @@ LIB_OBJS += $(OUTPUT)util/annotate.o
 LIB_OBJS += $(OUTPUT)util/build-id.o
 LIB_OBJS += $(OUTPUT)util/config.o
 LIB_OBJS += $(OUTPUT)util/ctype.o
-LIB_OBJS += $(OUTPUT)util/debugfs.o
 LIB_OBJS += $(OUTPUT)util/sysfs.o
 LIB_OBJS += $(OUTPUT)util/pmu.o
 LIB_OBJS += $(OUTPUT)util/environment.o
@@ -503,6 +517,10 @@ LIB_OBJS += $(OUTPUT)tests/evsel-tp-sched.o
 LIB_OBJS += $(OUTPUT)tests/pmu.o
 LIB_OBJS += $(OUTPUT)tests/hists_link.o
 LIB_OBJS += $(OUTPUT)tests/python-use.o
+LIB_OBJS += $(OUTPUT)tests/bp_signal.o
+LIB_OBJS += $(OUTPUT)tests/bp_signal_overflow.o
+LIB_OBJS += $(OUTPUT)tests/task-exit.o
+LIB_OBJS += $(OUTPUT)tests/sw-clock.o
 
 BUILTIN_OBJS += $(OUTPUT)builtin-annotate.o
 BUILTIN_OBJS += $(OUTPUT)builtin-bench.o
@@ -535,8 +553,9 @@ BUILTIN_OBJS += $(OUTPUT)builtin-lock.o
 BUILTIN_OBJS += $(OUTPUT)builtin-kvm.o
 BUILTIN_OBJS += $(OUTPUT)builtin-inject.o
 BUILTIN_OBJS += $(OUTPUT)tests/builtin-test.o
+BUILTIN_OBJS += $(OUTPUT)builtin-mem.o
 
-PERFLIBS = $(LIB_FILE) $(LIBTRACEEVENT)
+PERFLIBS = $(LIB_FILE) $(LIBLK) $(LIBTRACEEVENT)
 
 #
 # Platform specific tweaks
@@ -667,15 +686,15 @@ ifndef NO_LIBAUDIT
 	endif
 endif
 
-ifndef NO_NEWT
-	FLAGS_NEWT=$(ALL_CFLAGS) $(ALL_LDFLAGS) $(EXTLIBS) -lnewt
-	ifneq ($(call try-cc,$(SOURCE_NEWT),$(FLAGS_NEWT),libnewt),y)
-		msg := $(warning newt not found, disables TUI support. Please install newt-devel or libnewt-dev);
+ifndef NO_SLANG
+	FLAGS_SLANG=$(ALL_CFLAGS) $(ALL_LDFLAGS) $(EXTLIBS) -I/usr/include/slang -lslang
+	ifneq ($(call try-cc,$(SOURCE_SLANG),$(FLAGS_SLANG),libslang),y)
+		msg := $(warning slang not found, disables TUI support. Please install slang-devel or libslang-dev);
 	else
 		# Fedora has /usr/include/slang/slang.h, but ubuntu /usr/include/slang.h
 		BASIC_CFLAGS += -I/usr/include/slang
-		BASIC_CFLAGS += -DNEWT_SUPPORT
-		EXTLIBS += -lnewt -lslang
+		BASIC_CFLAGS += -DSLANG_SUPPORT
+		EXTLIBS += -lslang
 		LIB_OBJS += $(OUTPUT)ui/browser.o
 		LIB_OBJS += $(OUTPUT)ui/browsers/annotate.o
 		LIB_OBJS += $(OUTPUT)ui/browsers/hists.o
@@ -1051,6 +1070,18 @@ $(LIBTRACEEVENT):
 $(LIBTRACEEVENT)-clean:
 	$(QUIET_SUBDIR0)$(TRACE_EVENT_DIR) $(QUIET_SUBDIR1) O=$(OUTPUT) clean
 
+# if subdir is set, we've been called from above so target has been built
+# already
+$(LIBLK):
+ifeq ($(subdir),)
+	$(QUIET_SUBDIR0)$(LK_DIR) $(QUIET_SUBDIR1) O=$(OUTPUT) liblk.a
+endif
+
+$(LIBLK)-clean:
+ifeq ($(subdir),)
+	$(QUIET_SUBDIR0)$(LK_DIR) $(QUIET_SUBDIR1) O=$(OUTPUT) clean
+endif
+
 help:
 	@echo 'Perf make targets:'
 	@echo '  doc		- make *all* documentation (see below)'
@@ -1171,7 +1202,7 @@ $(INSTALL_DOC_TARGETS):
 
 ### Cleaning rules
 
-clean: $(LIBTRACEEVENT)-clean
+clean: $(LIBTRACEEVENT)-clean $(LIBLK)-clean
 	$(RM) $(LIB_OBJS) $(BUILTIN_OBJS) $(LIB_FILE) $(OUTPUT)perf-archive $(OUTPUT)perf.o $(LANG_BINDINGS)
 	$(RM) $(ALL_PROGRAMS) perf
 	$(RM) *.spec *.pyc *.pyo */*.pyc */*.pyo $(OUTPUT)common-cmds.h TAGS tags cscope*
@@ -1181,6 +1212,6 @@ clean: $(LIBTRACEEVENT)-clean
 	$(RM) $(OUTPUT)util/*-flex*
 	$(python-clean)
 
-.PHONY: all install clean strip $(LIBTRACEEVENT)
+.PHONY: all install clean strip $(LIBTRACEEVENT) $(LIBLK)
 .PHONY: shell_compatibility_test please_set_SHELL_PATH_to_a_more_modern_shell
 .PHONY: .FORCE-PERF-VERSION-FILE TAGS tags cscope .FORCE-PERF-CFLAGS
diff --git a/tools/perf/arch/arm/util/dwarf-regs.c b/tools/perf/arch/arm/util/dwarf-regs.c
index e8d5c551c69..33ec5b339da 100644
--- a/tools/perf/arch/arm/util/dwarf-regs.c
+++ b/tools/perf/arch/arm/util/dwarf-regs.c
@@ -8,10 +8,7 @@
  * published by the Free Software Foundation.
  */
 
-#include <stdlib.h>
-#ifndef __UCLIBC__
-#include <libio.h>
-#endif
+#include <stddef.h>
 #include <dwarf-regs.h>
 
 struct pt_regs_dwarfnum {
diff --git a/tools/perf/arch/powerpc/util/dwarf-regs.c b/tools/perf/arch/powerpc/util/dwarf-regs.c
index 7cdd61d0e27..733151cdf46 100644
--- a/tools/perf/arch/powerpc/util/dwarf-regs.c
+++ b/tools/perf/arch/powerpc/util/dwarf-regs.c
@@ -9,10 +9,7 @@
  * 2 of the License, or (at your option) any later version.
  */
 
-#include <stdlib.h>
-#ifndef __UCLIBC__
-#include <libio.h>
-#endif
+#include <stddef.h>
 #include <dwarf-regs.h>
 
 
diff --git a/tools/perf/arch/s390/util/dwarf-regs.c b/tools/perf/arch/s390/util/dwarf-regs.c
index e19653e025f..0469df02ee6 100644
--- a/tools/perf/arch/s390/util/dwarf-regs.c
+++ b/tools/perf/arch/s390/util/dwarf-regs.c
@@ -6,7 +6,7 @@
  *
  */
 
-#include <libio.h>
+#include <stddef.h>
 #include <dwarf-regs.h>
 
 #define NUM_GPRS 16
diff --git a/tools/perf/arch/sh/util/dwarf-regs.c b/tools/perf/arch/sh/util/dwarf-regs.c
index a11edb007a6..0d0897f57a1 100644
--- a/tools/perf/arch/sh/util/dwarf-regs.c
+++ b/tools/perf/arch/sh/util/dwarf-regs.c
@@ -19,7 +19,7 @@
  *
  */
 
-#include <libio.h>
+#include <stddef.h>
 #include <dwarf-regs.h>
 
 /*
diff --git a/tools/perf/arch/sparc/util/dwarf-regs.c b/tools/perf/arch/sparc/util/dwarf-regs.c
index 0ab88483720..92eda412fed 100644
--- a/tools/perf/arch/sparc/util/dwarf-regs.c
+++ b/tools/perf/arch/sparc/util/dwarf-regs.c
@@ -9,7 +9,7 @@
  * 2 of the License, or (at your option) any later version.
  */
 
-#include <libio.h>
+#include <stddef.h>
 #include <dwarf-regs.h>
 
 #define SPARC_MAX_REGS	96
diff --git a/tools/perf/arch/x86/util/dwarf-regs.c b/tools/perf/arch/x86/util/dwarf-regs.c
index a794d308192..be22dd46323 100644
--- a/tools/perf/arch/x86/util/dwarf-regs.c
+++ b/tools/perf/arch/x86/util/dwarf-regs.c
@@ -20,7 +20,7 @@
  *
  */
 
-#include <libio.h>
+#include <stddef.h>
 #include <dwarf-regs.h>
 
 /*
diff --git a/tools/perf/builtin-annotate.c b/tools/perf/builtin-annotate.c
index 2e6961ea318..db491e9a812 100644
--- a/tools/perf/builtin-annotate.c
+++ b/tools/perf/builtin-annotate.c
@@ -63,7 +63,7 @@ static int perf_evsel__add_sample(struct perf_evsel *evsel,
 		return 0;
 	}
 
-	he = __hists__add_entry(&evsel->hists, al, NULL, 1);
+	he = __hists__add_entry(&evsel->hists, al, NULL, 1, 1);
 	if (he == NULL)
 		return -ENOMEM;
 
@@ -109,14 +109,16 @@ static int process_sample_event(struct perf_tool *tool,
 	return 0;
 }
 
-static int hist_entry__tty_annotate(struct hist_entry *he, int evidx,
+static int hist_entry__tty_annotate(struct hist_entry *he,
+				    struct perf_evsel *evsel,
 				    struct perf_annotate *ann)
 {
-	return symbol__tty_annotate(he->ms.sym, he->ms.map, evidx,
+	return symbol__tty_annotate(he->ms.sym, he->ms.map, evsel,
 				    ann->print_line, ann->full_paths, 0, 0);
 }
 
-static void hists__find_annotations(struct hists *self, int evidx,
+static void hists__find_annotations(struct hists *self,
+				    struct perf_evsel *evsel,
 				    struct perf_annotate *ann)
 {
 	struct rb_node *nd = rb_first(&self->entries), *next;
@@ -142,14 +144,14 @@ find_next:
 		if (use_browser == 2) {
 			int ret;
 
-			ret = hist_entry__gtk_annotate(he, evidx, NULL);
+			ret = hist_entry__gtk_annotate(he, evsel, NULL);
 			if (!ret || !ann->skip_missing)
 				return;
 
 			/* skip missing symbols */
 			nd = rb_next(nd);
 		} else if (use_browser == 1) {
-			key = hist_entry__tui_annotate(he, evidx, NULL);
+			key = hist_entry__tui_annotate(he, evsel, NULL);
 			switch (key) {
 			case -1:
 				if (!ann->skip_missing)
@@ -168,7 +170,7 @@ find_next:
 			if (next != NULL)
 				nd = next;
 		} else {
-			hist_entry__tty_annotate(he, evidx, ann);
+			hist_entry__tty_annotate(he, evsel, ann);
 			nd = rb_next(nd);
 			/*
 			 * Since we have a hist_entry per IP for the same
@@ -230,7 +232,12 @@ static int __cmd_annotate(struct perf_annotate *ann)
 			total_nr_samples += nr_samples;
 			hists__collapse_resort(hists);
 			hists__output_resort(hists);
-			hists__find_annotations(hists, pos->idx, ann);
+
+			if (symbol_conf.event_group &&
+			    !perf_evsel__is_group_leader(pos))
+				continue;
+
+			hists__find_annotations(hists, pos, ann);
 		}
 	}
 
@@ -312,6 +319,8 @@ int cmd_annotate(int argc, const char **argv, const char *prefix __maybe_unused)
 		   "Specify disassembler style (e.g. -M intel for intel syntax)"),
 	OPT_STRING(0, "objdump", &objdump_path, "path",
 		   "objdump binary to use for disassembly and annotations"),
+	OPT_BOOLEAN(0, "group", &symbol_conf.event_group,
+		    "Show event group information together"),
 	OPT_END()
 	};
 
diff --git a/tools/perf/builtin-diff.c b/tools/perf/builtin-diff.c
index d207a97a2db..2d0462d89a9 100644
--- a/tools/perf/builtin-diff.c
+++ b/tools/perf/builtin-diff.c
@@ -231,9 +231,10 @@ int perf_diff__formula(struct hist_entry *he, struct hist_entry *pair,
 }
 
 static int hists__add_entry(struct hists *self,
-			    struct addr_location *al, u64 period)
+			    struct addr_location *al, u64 period,
+			    u64 weight)
 {
-	if (__hists__add_entry(self, al, NULL, period) != NULL)
+	if (__hists__add_entry(self, al, NULL, period, weight) != NULL)
 		return 0;
 	return -ENOMEM;
 }
@@ -255,7 +256,7 @@ static int diff__process_sample_event(struct perf_tool *tool __maybe_unused,
 	if (al.filtered)
 		return 0;
 
-	if (hists__add_entry(&evsel->hists, &al, sample->period)) {
+	if (hists__add_entry(&evsel->hists, &al, sample->period, sample->weight)) {
 		pr_warning("problem incrementing symbol period, skipping event\n");
 		return -1;
 	}
diff --git a/tools/perf/builtin-kvm.c b/tools/perf/builtin-kvm.c
index 37a769d7f9f..533501e2b07 100644
--- a/tools/perf/builtin-kvm.c
+++ b/tools/perf/builtin-kvm.c
@@ -12,7 +12,7 @@
 #include "util/parse-options.h"
 #include "util/trace-event.h"
 #include "util/debug.h"
-#include "util/debugfs.h"
+#include <lk/debugfs.h>
 #include "util/tool.h"
 #include "util/stat.h"
 
diff --git a/tools/perf/builtin-mem.c b/tools/perf/builtin-mem.c
new file mode 100644
index 00000000000..a8ff6d264e5
--- /dev/null
+++ b/tools/perf/builtin-mem.c
@@ -0,0 +1,242 @@
+#include "builtin.h"
+#include "perf.h"
+
+#include "util/parse-options.h"
+#include "util/trace-event.h"
+#include "util/tool.h"
+#include "util/session.h"
+
+#define MEM_OPERATION_LOAD	"load"
+#define MEM_OPERATION_STORE	"store"
+
+static const char	*mem_operation		= MEM_OPERATION_LOAD;
+
+struct perf_mem {
+	struct perf_tool	tool;
+	char const		*input_name;
+	symbol_filter_t		annotate_init;
+	bool			hide_unresolved;
+	bool			dump_raw;
+	const char		*cpu_list;
+	DECLARE_BITMAP(cpu_bitmap, MAX_NR_CPUS);
+};
+
+static const char * const mem_usage[] = {
+	"perf mem [<options>] {record <command> |report}",
+	NULL
+};
+
+static int __cmd_record(int argc, const char **argv)
+{
+	int rec_argc, i = 0, j;
+	const char **rec_argv;
+	char event[64];
+	int ret;
+
+	rec_argc = argc + 4;
+	rec_argv = calloc(rec_argc + 1, sizeof(char *));
+	if (!rec_argv)
+		return -1;
+
+	rec_argv[i++] = strdup("record");
+	if (!strcmp(mem_operation, MEM_OPERATION_LOAD))
+		rec_argv[i++] = strdup("-W");
+	rec_argv[i++] = strdup("-d");
+	rec_argv[i++] = strdup("-e");
+
+	if (strcmp(mem_operation, MEM_OPERATION_LOAD))
+		sprintf(event, "cpu/mem-stores/pp");
+	else
+		sprintf(event, "cpu/mem-loads/pp");
+
+	rec_argv[i++] = strdup(event);
+	for (j = 1; j < argc; j++, i++)
+		rec_argv[i] = argv[j];
+
+	ret = cmd_record(i, rec_argv, NULL);
+	free(rec_argv);
+	return ret;
+}
+
+static int
+dump_raw_samples(struct perf_tool *tool,
+		 union perf_event *event,
+		 struct perf_sample *sample,
+		 struct perf_evsel *evsel __maybe_unused,
+		 struct machine *machine)
+{
+	struct perf_mem *mem = container_of(tool, struct perf_mem, tool);
+	struct addr_location al;
+	const char *fmt;
+
+	if (perf_event__preprocess_sample(event, machine, &al, sample,
+				mem->annotate_init) < 0) {
+		fprintf(stderr, "problem processing %d event, skipping it.\n",
+				event->header.type);
+		return -1;
+	}
+
+	if (al.filtered || (mem->hide_unresolved && al.sym == NULL))
+		return 0;
+
+	if (al.map != NULL)
+		al.map->dso->hit = 1;
+
+	if (symbol_conf.field_sep) {
+		fmt = "%d%s%d%s0x%"PRIx64"%s0x%"PRIx64"%s%"PRIu64
+		      "%s0x%"PRIx64"%s%s:%s\n";
+	} else {
+		fmt = "%5d%s%5d%s0x%016"PRIx64"%s0x016%"PRIx64
+		      "%s%5"PRIu64"%s0x%06"PRIx64"%s%s:%s\n";
+		symbol_conf.field_sep = " ";
+	}
+
+	printf(fmt,
+		sample->pid,
+		symbol_conf.field_sep,
+		sample->tid,
+		symbol_conf.field_sep,
+		event->ip.ip,
+		symbol_conf.field_sep,
+		sample->addr,
+		symbol_conf.field_sep,
+		sample->weight,
+		symbol_conf.field_sep,
+		sample->data_src,
+		symbol_conf.field_sep,
+		al.map ? (al.map->dso ? al.map->dso->long_name : "???") : "???",
+		al.sym ? al.sym->name : "???");
+
+	return 0;
+}
+
+static int process_sample_event(struct perf_tool *tool,
+				union perf_event *event,
+				struct perf_sample *sample,
+				struct perf_evsel *evsel,
+				struct machine *machine)
+{
+	return dump_raw_samples(tool, event, sample, evsel, machine);
+}
+
+static int report_raw_events(struct perf_mem *mem)
+{
+	int err = -EINVAL;
+	int ret;
+	struct perf_session *session = perf_session__new(input_name, O_RDONLY,
+							 0, false, &mem->tool);
+
+	if (session == NULL)
+		return -ENOMEM;
+
+	if (mem->cpu_list) {
+		ret = perf_session__cpu_bitmap(session, mem->cpu_list,
+					       mem->cpu_bitmap);
+		if (ret)
+			goto out_delete;
+	}
+
+	if (symbol__init() < 0)
+		return -1;
+
+	printf("# PID, TID, IP, ADDR, LOCAL WEIGHT, DSRC, SYMBOL\n");
+
+	err = perf_session__process_events(session, &mem->tool);
+	if (err)
+		return err;
+
+	return 0;
+
+out_delete:
+	perf_session__delete(session);
+	return err;
+}
+
+static int report_events(int argc, const char **argv, struct perf_mem *mem)
+{
+	const char **rep_argv;
+	int ret, i = 0, j, rep_argc;
+
+	if (mem->dump_raw)
+		return report_raw_events(mem);
+
+	rep_argc = argc + 3;
+	rep_argv = calloc(rep_argc + 1, sizeof(char *));
+	if (!rep_argv)
+		return -1;
+
+	rep_argv[i++] = strdup("report");
+	rep_argv[i++] = strdup("--mem-mode");
+	rep_argv[i++] = strdup("-n"); /* display number of samples */
+
+	/*
+	 * there is no weight (cost) associated with stores, so don't print
+	 * the column
+	 */
+	if (strcmp(mem_operation, MEM_OPERATION_LOAD))
+		rep_argv[i++] = strdup("--sort=mem,sym,dso,symbol_daddr,"
+				       "dso_daddr,tlb,locked");
+
+	for (j = 1; j < argc; j++, i++)
+		rep_argv[i] = argv[j];
+
+	ret = cmd_report(i, rep_argv, NULL);
+	free(rep_argv);
+	return ret;
+}
+
+int cmd_mem(int argc, const char **argv, const char *prefix __maybe_unused)
+{
+	struct stat st;
+	struct perf_mem mem = {
+		.tool = {
+			.sample		= process_sample_event,
+			.mmap		= perf_event__process_mmap,
+			.comm		= perf_event__process_comm,
+			.lost		= perf_event__process_lost,
+			.fork		= perf_event__process_fork,
+			.build_id	= perf_event__process_build_id,
+			.ordered_samples = true,
+		},
+		.input_name		 = "perf.data",
+	};
+	const struct option mem_options[] = {
+	OPT_STRING('t', "type", &mem_operation,
+		   "type", "memory operations(load/store)"),
+	OPT_BOOLEAN('D', "dump-raw-samples", &mem.dump_raw,
+		    "dump raw samples in ASCII"),
+	OPT_BOOLEAN('U', "hide-unresolved", &mem.hide_unresolved,
+		    "Only display entries resolved to a symbol"),
+	OPT_STRING('i', "input", &input_name, "file",
+		   "input file name"),
+	OPT_STRING('C', "cpu", &mem.cpu_list, "cpu",
+		   "list of cpus to profile"),
+	OPT_STRING('x', "field-separator", &symbol_conf.field_sep,
+		   "separator",
+		   "separator for columns, no spaces will be added"
+		   " between columns '.' is reserved."),
+	OPT_END()
+	};
+
+	argc = parse_options(argc, argv, mem_options, mem_usage,
+			     PARSE_OPT_STOP_AT_NON_OPTION);
+
+	if (!argc || !(strncmp(argv[0], "rec", 3) || mem_operation))
+		usage_with_options(mem_usage, mem_options);
+
+	if (!mem.input_name || !strlen(mem.input_name)) {
+		if (!fstat(STDIN_FILENO, &st) && S_ISFIFO(st.st_mode))
+			mem.input_name = "-";
+		else
+			mem.input_name = "perf.data";
+	}
+
+	if (!strncmp(argv[0], "rec", 3))
+		return __cmd_record(argc, argv);
+	else if (!strncmp(argv[0], "rep", 3))
+		return report_events(argc, argv, &mem);
+	else
+		usage_with_options(mem_usage, mem_options);
+
+	return 0;
+}
diff --git a/tools/perf/builtin-probe.c b/tools/perf/builtin-probe.c
index de38a034b12..e8a66f9a671 100644
--- a/tools/perf/builtin-probe.c
+++ b/tools/perf/builtin-probe.c
@@ -37,7 +37,7 @@
 #include "util/strfilter.h"
 #include "util/symbol.h"
 #include "util/debug.h"
-#include "util/debugfs.h"
+#include <lk/debugfs.h>
 #include "util/parse-options.h"
 #include "util/probe-finder.h"
 #include "util/probe-event.h"
diff --git a/tools/perf/builtin-record.c b/tools/perf/builtin-record.c
index f1a939ebc19..cdf58ecc04b 100644
--- a/tools/perf/builtin-record.c
+++ b/tools/perf/builtin-record.c
@@ -5,8 +5,6 @@
  * (or a CPU, or a PID) into the perf.data output file - for
  * later analysis via perf report.
  */
-#define _FILE_OFFSET_BITS 64
-
 #include "builtin.h"
 
 #include "perf.h"
@@ -474,7 +472,9 @@ static int __cmd_record(struct perf_record *rec, int argc, const char **argv)
 	}
 
 	if (forks) {
-		err = perf_evlist__prepare_workload(evsel_list, opts, argv);
+		err = perf_evlist__prepare_workload(evsel_list, &opts->target,
+						    argv, opts->pipe_output,
+						    true);
 		if (err < 0) {
 			pr_err("Couldn't run the workload!\n");
 			goto out_delete_session;
@@ -953,6 +953,8 @@ const struct option record_options[] = {
 	OPT_CALLBACK('j', "branch-filter", &record.opts.branch_stack,
 		     "branch filter mask", "branch stack filter modes",
 		     parse_branch_stack),
+	OPT_BOOLEAN('W', "weight", &record.opts.sample_weight,
+		    "sample by weight (on special events only)"),
 	OPT_END()
 };
 
@@ -964,7 +966,7 @@ int cmd_record(int argc, const char **argv, const char *prefix __maybe_unused)
 	struct perf_record *rec = &record;
 	char errbuf[BUFSIZ];
 
-	evsel_list = perf_evlist__new(NULL, NULL);
+	evsel_list = perf_evlist__new();
 	if (evsel_list == NULL)
 		return -ENOMEM;
 
@@ -1026,7 +1028,7 @@ int cmd_record(int argc, const char **argv, const char *prefix __maybe_unused)
 		ui__error("%s", errbuf);
 
 		err = -saved_errno;
-		goto out_free_fd;
+		goto out_symbol_exit;
 	}
 
 	err = -ENOMEM;
@@ -1057,6 +1059,9 @@ int cmd_record(int argc, const char **argv, const char *prefix __maybe_unused)
 	}
 
 	err = __cmd_record(&record, argc, argv);
+
+	perf_evlist__munmap(evsel_list);
+	perf_evlist__close(evsel_list);
 out_free_fd:
 	perf_evlist__delete_maps(evsel_list);
 out_symbol_exit:
diff --git a/tools/perf/builtin-report.c b/tools/perf/builtin-report.c
index 96b5a7fee4b..bd0ca81eeac 100644
--- a/tools/perf/builtin-report.c
+++ b/tools/perf/builtin-report.c
@@ -13,7 +13,6 @@
 #include "util/annotate.h"
 #include "util/color.h"
 #include <linux/list.h>
-#include "util/cache.h"
 #include <linux/rbtree.h>
 #include "util/symbol.h"
 #include "util/callchain.h"
@@ -47,6 +46,7 @@ struct perf_report {
 	bool			show_full_info;
 	bool			show_threads;
 	bool			inverted_callchain;
+	bool			mem_mode;
 	struct perf_read_values	show_threads_values;
 	const char		*pretty_printing_style;
 	symbol_filter_t		annotate_init;
@@ -65,6 +65,99 @@ static int perf_report_config(const char *var, const char *value, void *cb)
 	return perf_default_config(var, value, cb);
 }
 
+static int perf_report__add_mem_hist_entry(struct perf_tool *tool,
+					   struct addr_location *al,
+					   struct perf_sample *sample,
+					   struct perf_evsel *evsel,
+					   struct machine *machine,
+					   union perf_event *event)
+{
+	struct perf_report *rep = container_of(tool, struct perf_report, tool);
+	struct symbol *parent = NULL;
+	u8 cpumode = event->header.misc & PERF_RECORD_MISC_CPUMODE_MASK;
+	int err = 0;
+	struct hist_entry *he;
+	struct mem_info *mi, *mx;
+	uint64_t cost;
+
+	if ((sort__has_parent || symbol_conf.use_callchain) &&
+	    sample->callchain) {
+		err = machine__resolve_callchain(machine, evsel, al->thread,
+						 sample, &parent);
+		if (err)
+			return err;
+	}
+
+	mi = machine__resolve_mem(machine, al->thread, sample, cpumode);
+	if (!mi)
+		return -ENOMEM;
+
+	if (rep->hide_unresolved && !al->sym)
+		return 0;
+
+	cost = sample->weight;
+	if (!cost)
+		cost = 1;
+
+	/*
+	 * must pass period=weight in order to get the correct
+	 * sorting from hists__collapse_resort() which is solely
+	 * based on periods. We want sorting be done on nr_events * weight
+	 * and this is indirectly achieved by passing period=weight here
+	 * and the he_stat__add_period() function.
+	 */
+	he = __hists__add_mem_entry(&evsel->hists, al, parent, mi, cost, cost);
+	if (!he)
+		return -ENOMEM;
+
+	/*
+	 * In the TUI browser, we are doing integrated annotation,
+	 * so we don't allocate the extra space needed because the stdio
+	 * code will not use it.
+	 */
+	if (sort__has_sym && he->ms.sym && use_browser > 0) {
+		struct annotation *notes = symbol__annotation(he->ms.sym);
+
+		assert(evsel != NULL);
+
+		if (notes->src == NULL && symbol__alloc_hist(he->ms.sym) < 0)
+			goto out;
+
+		err = hist_entry__inc_addr_samples(he, evsel->idx, al->addr);
+		if (err)
+			goto out;
+	}
+
+	if (sort__has_sym && he->mem_info->daddr.sym && use_browser > 0) {
+		struct annotation *notes;
+
+		mx = he->mem_info;
+
+		notes = symbol__annotation(mx->daddr.sym);
+		if (notes->src == NULL && symbol__alloc_hist(mx->daddr.sym) < 0)
+			goto out;
+
+		err = symbol__inc_addr_samples(mx->daddr.sym,
+					       mx->daddr.map,
+					       evsel->idx,
+					       mx->daddr.al_addr);
+		if (err)
+			goto out;
+	}
+
+	evsel->hists.stats.total_period += cost;
+	hists__inc_nr_events(&evsel->hists, PERF_RECORD_SAMPLE);
+	err = 0;
+
+	if (symbol_conf.use_callchain) {
+		err = callchain_append(he->callchain,
+				       &callchain_cursor,
+				       sample->period);
+	}
+out:
+	return err;
+}
+
 static int perf_report__add_branch_hist_entry(struct perf_tool *tool,
 					struct addr_location *al,
 					struct perf_sample *sample,
@@ -99,7 +192,7 @@ static int perf_report__add_branch_hist_entry(struct perf_tool *tool,
 		 * and not events sampled. Thus we use a pseudo period of 1.
 		 */
 		he = __hists__add_branch_entry(&evsel->hists, al, parent,
-				&bi[i], 1);
+				&bi[i], 1, 1);
 		if (he) {
 			struct annotation *notes;
 			err = -ENOMEM;
@@ -157,7 +250,8 @@ static int perf_evsel__add_hist_entry(struct perf_evsel *evsel,
 			return err;
 	}
 
-	he = __hists__add_entry(&evsel->hists, al, parent, sample->period);
+	he = __hists__add_entry(&evsel->hists, al, parent, sample->period,
+					sample->weight);
 	if (he == NULL)
 		return -ENOMEM;
 
@@ -169,7 +263,7 @@ static int perf_evsel__add_hist_entry(struct perf_evsel *evsel,
 			return err;
 	}
 	/*
-	 * Only in the newt browser we are doing integrated annotation,
+	 * Only in the TUI browser we are doing integrated annotation,
 	 * so we don't allocated the extra space needed because the stdio
 	 * code will not use it.
 	 */
@@ -220,6 +314,12 @@ static int process_sample_event(struct perf_tool *tool,
 			pr_debug("problem adding lbr entry, skipping event\n");
 			return -1;
 		}
+	} else if (rep->mem_mode == 1) {
+		if (perf_report__add_mem_hist_entry(tool, &al, sample,
+						    evsel, machine, event)) {
+			pr_debug("problem adding mem entry, skipping event\n");
+			return -1;
+		}
 	} else {
 		if (al.map != NULL)
 			al.map->dso->hit = 1;
@@ -303,7 +403,8 @@ static void sig_handler(int sig __maybe_unused)
 	session_done = 1;
 }
 
-static size_t hists__fprintf_nr_sample_events(struct hists *self,
+static size_t hists__fprintf_nr_sample_events(struct perf_report *rep,
+					      struct hists *self,
 					      const char *evname, FILE *fp)
 {
 	size_t ret;
@@ -314,7 +415,7 @@ static size_t hists__fprintf_nr_sample_events(struct hists *self,
 	char buf[512];
 	size_t size = sizeof(buf);
 
-	if (symbol_conf.event_group && evsel->nr_members > 1) {
+	if (perf_evsel__is_group_event(evsel)) {
 		struct perf_evsel *pos;
 
 		perf_evsel__group_desc(evsel, buf, size);
@@ -331,7 +432,11 @@ static size_t hists__fprintf_nr_sample_events(struct hists *self,
 	if (evname != NULL)
 		ret += fprintf(fp, " of event '%s'", evname);
 
-	ret += fprintf(fp, "\n# Event count (approx.): %" PRIu64, nr_events);
+	if (rep->mem_mode) {
+		ret += fprintf(fp, "\n# Total weight : %" PRIu64, nr_events);
+		ret += fprintf(fp, "\n# Sort order   : %s", sort_order);
+	} else
+		ret += fprintf(fp, "\n# Event count (approx.): %" PRIu64, nr_events);
 	return ret + fprintf(fp, "\n#\n");
 }
 
@@ -349,7 +454,7 @@ static int perf_evlist__tty_browse_hists(struct perf_evlist *evlist,
 		    !perf_evsel__is_group_leader(pos))
 			continue;
 
-		hists__fprintf_nr_sample_events(hists, evname, stdout);
+		hists__fprintf_nr_sample_events(rep, hists, evname, stdout);
 		hists__fprintf(hists, true, 0, 0, stdout);
 		fprintf(stdout, "\n\n");
 	}
@@ -645,7 +750,9 @@ int cmd_report(int argc, const char **argv, const char *prefix __maybe_unused)
 		    "Use the stdio interface"),
 	OPT_STRING('s', "sort", &sort_order, "key[,key2...]",
 		   "sort by key(s): pid, comm, dso, symbol, parent, cpu, srcline,"
-		   " dso_to, dso_from, symbol_to, symbol_from, mispredict"),
+		   " dso_to, dso_from, symbol_to, symbol_from, mispredict,"
+		   " weight, local_weight, mem, symbol_daddr, dso_daddr, tlb, "
+		   "snoop, locked"),
 	OPT_BOOLEAN(0, "showcpuutilization", &symbol_conf.show_cpu_utilization,
 		    "Show sample percentage for different cpu modes"),
 	OPT_STRING('p', "parent", &parent_pattern, "regex",
@@ -693,6 +800,9 @@ int cmd_report(int argc, const char **argv, const char *prefix __maybe_unused)
 		    "use branch records for histogram filling", parse_branch_mode),
 	OPT_STRING(0, "objdump", &objdump_path, "path",
 		   "objdump binary to use for disassembly and annotations"),
+	OPT_BOOLEAN(0, "demangle", &symbol_conf.demangle,
+		    "Disable symbol demangling"),
+	OPT_BOOLEAN(0, "mem-mode", &report.mem_mode, "mem access profile"),
 	OPT_END()
 	};
 
@@ -750,12 +860,24 @@ repeat:
 				     "dso_to,symbol_to";
 
 	}
+	if (report.mem_mode) {
+		if (sort__branch_mode == 1) {
+			fprintf(stderr, "branch and mem mode incompatible\n");
+			goto error;
+		}
+		/*
+		 * if no sort_order is provided, then specify
+		 * branch-mode specific order
+		 */
+		if (sort_order == default_sort_order)
+			sort_order = "local_weight,mem,sym,dso,symbol_daddr,dso_daddr,snoop,tlb,locked";
+	}
 
 	if (setup_sorting() < 0)
 		usage_with_options(report_usage, options);
 
 	/*
-	 * Only in the newt browser we are doing integrated annotation,
+	 * Only in the TUI browser we are doing integrated annotation,
 	 * so don't allocate extra space that won't be used in the stdio
 	 * implementation.
 	 */
@@ -815,6 +937,14 @@ repeat:
 		sort_entry__setup_elide(&sort_sym_from, symbol_conf.sym_from_list, "sym_from", stdout);
 		sort_entry__setup_elide(&sort_sym_to, symbol_conf.sym_to_list, "sym_to", stdout);
 	} else {
+		if (report.mem_mode) {
+			sort_entry__setup_elide(&sort_dso, symbol_conf.dso_list, "symbol_daddr", stdout);
+			sort_entry__setup_elide(&sort_dso, symbol_conf.dso_list, "dso_daddr", stdout);
+			sort_entry__setup_elide(&sort_dso, symbol_conf.dso_list, "mem", stdout);
+			sort_entry__setup_elide(&sort_dso, symbol_conf.dso_list, "local_weight", stdout);
+			sort_entry__setup_elide(&sort_dso, symbol_conf.dso_list, "tlb", stdout);
+			sort_entry__setup_elide(&sort_dso, symbol_conf.dso_list, "snoop", stdout);
+		}
 		sort_entry__setup_elide(&sort_dso, symbol_conf.dso_list, "dso", stdout);
 		sort_entry__setup_elide(&sort_sym, symbol_conf.sym_list, "symbol", stdout);
 	}
diff --git a/tools/perf/builtin-sched.c b/tools/perf/builtin-sched.c
index 138229439a9..2da2a6ca22b 100644
--- a/tools/perf/builtin-sched.c
+++ b/tools/perf/builtin-sched.c
@@ -1671,7 +1671,6 @@ int cmd_sched(int argc, const char **argv, const char *prefix __maybe_unused)
 			.sample		 = perf_sched__process_tracepoint_sample,
 			.comm		 = perf_event__process_comm,
 			.lost		 = perf_event__process_lost,
-			.exit		 = perf_event__process_exit,
 			.fork		 = perf_event__process_fork,
 			.ordered_samples = true,
 		},
diff --git a/tools/perf/builtin-stat.c b/tools/perf/builtin-stat.c
index 99848761f57..7e910bab109 100644
--- a/tools/perf/builtin-stat.c
+++ b/tools/perf/builtin-stat.c
@@ -68,7 +68,7 @@
 static void print_stat(int argc, const char **argv);
 static void print_counter_aggr(struct perf_evsel *counter, char *prefix);
 static void print_counter(struct perf_evsel *counter, char *prefix);
-static void print_aggr_socket(char *prefix);
+static void print_aggr(char *prefix);
 
 static struct perf_evlist	*evsel_list;
 
@@ -76,11 +76,17 @@ static struct perf_target	target = {
 	.uid	= UINT_MAX,
 };
 
+enum aggr_mode {
+	AGGR_NONE,
+	AGGR_GLOBAL,
+	AGGR_SOCKET,
+	AGGR_CORE,
+};
+
 static int			run_count			=  1;
 static bool			no_inherit			= false;
 static bool			scale				=  true;
-static bool			no_aggr				= false;
-static bool			aggr_socket			= false;
+static enum aggr_mode		aggr_mode			= AGGR_GLOBAL;
 static pid_t			child_pid			= -1;
 static bool			null_run			=  false;
 static int			detailed_run			=  0;
@@ -94,8 +100,10 @@ static const char		*pre_cmd			= NULL;
 static const char		*post_cmd			= NULL;
 static bool			sync_run			= false;
 static unsigned int		interval			= 0;
+static bool			forever				= false;
 static struct timespec		ref_time;
-static struct cpu_map		*sock_map;
+static struct cpu_map		*aggr_map;
+static int			(*aggr_get_id)(struct cpu_map *m, int cpu);
 
 static volatile int done = 0;
 
@@ -125,6 +133,11 @@ static inline int perf_evsel__nr_cpus(struct perf_evsel *evsel)
 	return perf_evsel__cpus(evsel)->nr;
 }
 
+static void perf_evsel__reset_stat_priv(struct perf_evsel *evsel)
+{
+	memset(evsel->priv, 0, sizeof(struct perf_stat));
+}
+
 static int perf_evsel__alloc_stat_priv(struct perf_evsel *evsel)
 {
 	evsel->priv = zalloc(sizeof(struct perf_stat));
@@ -160,6 +173,35 @@ static void perf_evsel__free_prev_raw_counts(struct perf_evsel *evsel)
 	evsel->prev_raw_counts = NULL;
 }
 
+static void perf_evlist__free_stats(struct perf_evlist *evlist)
+{
+	struct perf_evsel *evsel;
+
+	list_for_each_entry(evsel, &evlist->entries, node) {
+		perf_evsel__free_stat_priv(evsel);
+		perf_evsel__free_counts(evsel);
+		perf_evsel__free_prev_raw_counts(evsel);
+	}
+}
+
+static int perf_evlist__alloc_stats(struct perf_evlist *evlist, bool alloc_raw)
+{
+	struct perf_evsel *evsel;
+
+	list_for_each_entry(evsel, &evlist->entries, node) {
+		if (perf_evsel__alloc_stat_priv(evsel) < 0 ||
+		    perf_evsel__alloc_counts(evsel, perf_evsel__nr_cpus(evsel)) < 0 ||
+		    (alloc_raw && perf_evsel__alloc_prev_raw_counts(evsel) < 0))
+			goto out_free;
+	}
+
+	return 0;
+
+out_free:
+	perf_evlist__free_stats(evlist);
+	return -1;
+}
+
 static struct stats runtime_nsecs_stats[MAX_NR_CPUS];
 static struct stats runtime_cycles_stats[MAX_NR_CPUS];
 static struct stats runtime_stalled_cycles_front_stats[MAX_NR_CPUS];
@@ -173,6 +215,29 @@ static struct stats runtime_itlb_cache_stats[MAX_NR_CPUS];
 static struct stats runtime_dtlb_cache_stats[MAX_NR_CPUS];
 static struct stats walltime_nsecs_stats;
 
+static void perf_stat__reset_stats(struct perf_evlist *evlist)
+{
+	struct perf_evsel *evsel;
+
+	list_for_each_entry(evsel, &evlist->entries, node) {
+		perf_evsel__reset_stat_priv(evsel);
+		perf_evsel__reset_counts(evsel, perf_evsel__nr_cpus(evsel));
+	}
+
+	memset(runtime_nsecs_stats, 0, sizeof(runtime_nsecs_stats));
+	memset(runtime_cycles_stats, 0, sizeof(runtime_cycles_stats));
+	memset(runtime_stalled_cycles_front_stats, 0, sizeof(runtime_stalled_cycles_front_stats));
+	memset(runtime_stalled_cycles_back_stats, 0, sizeof(runtime_stalled_cycles_back_stats));
+	memset(runtime_branches_stats, 0, sizeof(runtime_branches_stats));
+	memset(runtime_cacherefs_stats, 0, sizeof(runtime_cacherefs_stats));
+	memset(runtime_l1_dcache_stats, 0, sizeof(runtime_l1_dcache_stats));
+	memset(runtime_l1_icache_stats, 0, sizeof(runtime_l1_icache_stats));
+	memset(runtime_ll_cache_stats, 0, sizeof(runtime_ll_cache_stats));
+	memset(runtime_itlb_cache_stats, 0, sizeof(runtime_itlb_cache_stats));
+	memset(runtime_dtlb_cache_stats, 0, sizeof(runtime_dtlb_cache_stats));
+	memset(&walltime_nsecs_stats, 0, sizeof(walltime_nsecs_stats));
+}
+
 static int create_perf_stat_counter(struct perf_evsel *evsel)
 {
 	struct perf_event_attr *attr = &evsel->attr;
@@ -249,7 +314,7 @@ static int read_counter_aggr(struct perf_evsel *counter)
 	int i;
 
 	if (__perf_evsel__read(counter, perf_evsel__nr_cpus(counter),
-			       evsel_list->threads->nr, scale) < 0)
+			       thread_map__nr(evsel_list->threads), scale) < 0)
 		return -1;
 
 	for (i = 0; i < 3; i++)
@@ -297,56 +362,68 @@ static void print_interval(void)
 	struct timespec ts, rs;
 	char prefix[64];
 
-	if (no_aggr) {
+	if (aggr_mode == AGGR_GLOBAL) {
 		list_for_each_entry(counter, &evsel_list->entries, node) {
 			ps = counter->priv;
 			memset(ps->res_stats, 0, sizeof(ps->res_stats));
-			read_counter(counter);
+			read_counter_aggr(counter);
 		}
-	} else {
+	} else	{
 		list_for_each_entry(counter, &evsel_list->entries, node) {
 			ps = counter->priv;
 			memset(ps->res_stats, 0, sizeof(ps->res_stats));
-			read_counter_aggr(counter);
+			read_counter(counter);
 		}
 	}
+
 	clock_gettime(CLOCK_MONOTONIC, &ts);
 	diff_timespec(&rs, &ts, &ref_time);
 	sprintf(prefix, "%6lu.%09lu%s", rs.tv_sec, rs.tv_nsec, csv_sep);
 
 	if (num_print_interval == 0 && !csv_output) {
-		if (aggr_socket)
+		switch (aggr_mode) {
+		case AGGR_SOCKET:
 			fprintf(output, "#           time socket cpus             counts events\n");
-		else if (no_aggr)
+			break;
+		case AGGR_CORE:
+			fprintf(output, "#           time core         cpus             counts events\n");
+			break;
+		case AGGR_NONE:
 			fprintf(output, "#           time CPU                 counts events\n");
-		else
+			break;
+		case AGGR_GLOBAL:
+		default:
 			fprintf(output, "#           time             counts events\n");
+		}
 	}
 
 	if (++num_print_interval == 25)
 		num_print_interval = 0;
 
-	if (aggr_socket)
-		print_aggr_socket(prefix);
-	else if (no_aggr) {
+	switch (aggr_mode) {
+	case AGGR_CORE:
+	case AGGR_SOCKET:
+		print_aggr(prefix);
+		break;
+	case AGGR_NONE:
 		list_for_each_entry(counter, &evsel_list->entries, node)
 			print_counter(counter, prefix);
-	} else {
+		break;
+	case AGGR_GLOBAL:
+	default:
 		list_for_each_entry(counter, &evsel_list->entries, node)
 			print_counter_aggr(counter, prefix);
 	}
 }
 
-static int __run_perf_stat(int argc __maybe_unused, const char **argv)
+static int __run_perf_stat(int argc, const char **argv)
 {
 	char msg[512];
 	unsigned long long t0, t1;
 	struct perf_evsel *counter;
 	struct timespec ts;
 	int status = 0;
-	int child_ready_pipe[2], go_pipe[2];
 	const bool forks = (argc > 0);
-	char buf;
 
 	if (interval) {
 		ts.tv_sec  = interval / 1000;
@@ -356,61 +433,12 @@ static int __run_perf_stat(int argc __maybe_unused, const char **argv)
 		ts.tv_nsec = 0;
 	}
 
-	if (aggr_socket
-	    && cpu_map__build_socket_map(evsel_list->cpus, &sock_map)) {
-		perror("cannot build socket map");
-		return -1;
-	}
-
-	if (forks && (pipe(child_ready_pipe) < 0 || pipe(go_pipe) < 0)) {
-		perror("failed to create pipes");
-		return -1;
-	}
-
 	if (forks) {
-		if ((child_pid = fork()) < 0)
-			perror("failed to fork");
-
-		if (!child_pid) {
-			close(child_ready_pipe[0]);
-			close(go_pipe[1]);
-			fcntl(go_pipe[0], F_SETFD, FD_CLOEXEC);
-
-			/*
-			 * Do a dummy execvp to get the PLT entry resolved,
-			 * so we avoid the resolver overhead on the real
-			 * execvp call.
-			 */
-			execvp("", (char **)argv);
-
-			/*
-			 * Tell the parent we're ready to go
-			 */
-			close(child_ready_pipe[1]);
-
-			/*
-			 * Wait until the parent tells us to go.
-			 */
-			if (read(go_pipe[0], &buf, 1) == -1)
-				perror("unable to read pipe");
-
-			execvp(argv[0], (char **)argv);
-
-			perror(argv[0]);
-			exit(-1);
+		if (perf_evlist__prepare_workload(evsel_list, &target, argv,
+						  false, false) < 0) {
+			perror("failed to prepare workload");
+			return -1;
 		}
-
-		if (perf_target__none(&target))
-			evsel_list->threads->map[0] = child_pid;
-
-		/*
-		 * Wait for the child to be ready to exec.
-		 */
-		close(child_ready_pipe[1]);
-		close(go_pipe[0]);
-		if (read(child_ready_pipe[0], &buf, 1) == -1)
-			perror("unable to read pipe");
-		close(child_ready_pipe[0]);
 	}
 
 	if (group)
@@ -457,7 +485,8 @@ static int __run_perf_stat(int argc __maybe_unused, const char **argv)
 	clock_gettime(CLOCK_MONOTONIC, &ref_time);
 
 	if (forks) {
-		close(go_pipe[1]);
+		perf_evlist__start_workload(evsel_list);
+
 		if (interval) {
 			while (!waitpid(child_pid, &status, WNOHANG)) {
 				nanosleep(&ts, NULL);
@@ -479,16 +508,16 @@ static int __run_perf_stat(int argc __maybe_unused, const char **argv)
 
 	update_stats(&walltime_nsecs_stats, t1 - t0);
 
-	if (no_aggr) {
+	if (aggr_mode == AGGR_GLOBAL) {
 		list_for_each_entry(counter, &evsel_list->entries, node) {
-			read_counter(counter);
-			perf_evsel__close_fd(counter, perf_evsel__nr_cpus(counter), 1);
+			read_counter_aggr(counter);
+			perf_evsel__close_fd(counter, perf_evsel__nr_cpus(counter),
+					     thread_map__nr(evsel_list->threads));
 		}
 	} else {
 		list_for_each_entry(counter, &evsel_list->entries, node) {
-			read_counter_aggr(counter);
-			perf_evsel__close_fd(counter, perf_evsel__nr_cpus(counter),
-					     evsel_list->threads->nr);
+			read_counter(counter);
+			perf_evsel__close_fd(counter, perf_evsel__nr_cpus(counter), 1);
 		}
 	}
 
@@ -542,26 +571,47 @@ static void print_noise(struct perf_evsel *evsel, double avg)
 	print_noise_pct(stddev_stats(&ps->res_stats[0]), avg);
 }
 
-static void nsec_printout(int cpu, int nr, struct perf_evsel *evsel, double avg)
+static void aggr_printout(struct perf_evsel *evsel, int id, int nr)
 {
-	double msecs = avg / 1e6;
-	char cpustr[16] = { '\0', };
-	const char *fmt = csv_output ? "%s%.6f%s%s" : "%s%18.6f%s%-25s";
-
-	if (aggr_socket)
-		sprintf(cpustr, "S%*d%s%*d%s",
+	switch (aggr_mode) {
+	case AGGR_CORE:
+		fprintf(output, "S%d-C%*d%s%*d%s",
+			cpu_map__id_to_socket(id),
+			csv_output ? 0 : -8,
+			cpu_map__id_to_cpu(id),
+			csv_sep,
+			csv_output ? 0 : 4,
+			nr,
+			csv_sep);
+		break;
+	case AGGR_SOCKET:
+		fprintf(output, "S%*d%s%*d%s",
 			csv_output ? 0 : -5,
-			cpu,
+			id,
 			csv_sep,
 			csv_output ? 0 : 4,
 			nr,
 			csv_sep);
-	else if (no_aggr)
-		sprintf(cpustr, "CPU%*d%s",
+			break;
+	case AGGR_NONE:
+		fprintf(output, "CPU%*d%s",
 			csv_output ? 0 : -4,
-			perf_evsel__cpus(evsel)->map[cpu], csv_sep);
+			perf_evsel__cpus(evsel)->map[id], csv_sep);
+		break;
+	case AGGR_GLOBAL:
+	default:
+		break;
+	}
+}
+
+static void nsec_printout(int cpu, int nr, struct perf_evsel *evsel, double avg)
+{
+	double msecs = avg / 1e6;
+	const char *fmt = csv_output ? "%.6f%s%s" : "%18.6f%s%-25s";
+
+	aggr_printout(evsel, cpu, nr);
 
-	fprintf(output, fmt, cpustr, msecs, csv_sep, perf_evsel__name(evsel));
+	fprintf(output, fmt, msecs, csv_sep, perf_evsel__name(evsel));
 
 	if (evsel->cgrp)
 		fprintf(output, "%s%s", csv_sep, evsel->cgrp->name);
@@ -758,32 +808,21 @@ static void print_ll_cache_misses(int cpu,
 static void abs_printout(int cpu, int nr, struct perf_evsel *evsel, double avg)
 {
 	double total, ratio = 0.0;
-	char cpustr[16] = { '\0', };
 	const char *fmt;
 
 	if (csv_output)
-		fmt = "%s%.0f%s%s";
+		fmt = "%.0f%s%s";
 	else if (big_num)
-		fmt = "%s%'18.0f%s%-25s";
+		fmt = "%'18.0f%s%-25s";
 	else
-		fmt = "%s%18.0f%s%-25s";
+		fmt = "%18.0f%s%-25s";
 
-	if (aggr_socket)
-		sprintf(cpustr, "S%*d%s%*d%s",
-			csv_output ? 0 : -5,
-			cpu,
-			csv_sep,
-			csv_output ? 0 : 4,
-			nr,
-			csv_sep);
-	else if (no_aggr)
-		sprintf(cpustr, "CPU%*d%s",
-			csv_output ? 0 : -4,
-			perf_evsel__cpus(evsel)->map[cpu], csv_sep);
-	else
+	aggr_printout(evsel, cpu, nr);
+
+	if (aggr_mode == AGGR_GLOBAL)
 		cpu = 0;
 
-	fprintf(output, fmt, cpustr, avg, csv_sep, perf_evsel__name(evsel));
+	fprintf(output, fmt, avg, csv_sep, perf_evsel__name(evsel));
 
 	if (evsel->cgrp)
 		fprintf(output, "%s%s", csv_sep, evsel->cgrp->name);
@@ -882,23 +921,23 @@ static void abs_printout(int cpu, int nr, struct perf_evsel *evsel, double avg)
 	}
 }
 
-static void print_aggr_socket(char *prefix)
+static void print_aggr(char *prefix)
 {
 	struct perf_evsel *counter;
+	int cpu, s, s2, id, nr;
 	u64 ena, run, val;
-	int cpu, s, s2, sock, nr;
 
-	if (!sock_map)
+	if (!(aggr_map || aggr_get_id))
 		return;
 
-	for (s = 0; s < sock_map->nr; s++) {
-		sock = cpu_map__socket(sock_map, s);
+	for (s = 0; s < aggr_map->nr; s++) {
+		id = aggr_map->map[s];
 		list_for_each_entry(counter, &evsel_list->entries, node) {
 			val = ena = run = 0;
 			nr = 0;
 			for (cpu = 0; cpu < perf_evsel__nr_cpus(counter); cpu++) {
-				s2 = cpu_map__get_socket(evsel_list->cpus, cpu);
-				if (s2 != sock)
+				s2 = aggr_get_id(evsel_list->cpus, cpu);
+				if (s2 != id)
 					continue;
 				val += counter->counts->cpu[cpu].val;
 				ena += counter->counts->cpu[cpu].ena;
@@ -909,18 +948,15 @@ static void print_aggr_socket(char *prefix)
 				fprintf(output, "%s", prefix);
 
 			if (run == 0 || ena == 0) {
-				fprintf(output, "S%*d%s%*d%s%*s%s%*s",
-					csv_output ? 0 : -5,
-					s,
-					csv_sep,
-					csv_output ? 0 : 4,
-					nr,
-					csv_sep,
+				aggr_printout(counter, cpu, nr);
+
+				fprintf(output, "%*s%s%*s",
 					csv_output ? 0 : 18,
 					counter->supported ? CNTR_NOT_COUNTED : CNTR_NOT_SUPPORTED,
 					csv_sep,
 					csv_output ? 0 : -24,
 					perf_evsel__name(counter));
+
 				if (counter->cgrp)
 					fprintf(output, "%s%s",
 						csv_sep, counter->cgrp->name);
@@ -930,9 +966,9 @@ static void print_aggr_socket(char *prefix)
 			}
 
 			if (nsec_counter(counter))
-				nsec_printout(sock, nr, counter, val);
+				nsec_printout(id, nr, counter, val);
 			else
-				abs_printout(sock, nr, counter, val);
+				abs_printout(id, nr, counter, val);
 
 			if (!csv_output) {
 				print_noise(counter, 1.0);
@@ -1073,14 +1109,21 @@ static void print_stat(int argc, const char **argv)
 		fprintf(output, ":\n\n");
 	}
 
-	if (aggr_socket)
-		print_aggr_socket(NULL);
-	else if (no_aggr) {
-		list_for_each_entry(counter, &evsel_list->entries, node)
-			print_counter(counter, NULL);
-	} else {
+	switch (aggr_mode) {
+	case AGGR_CORE:
+	case AGGR_SOCKET:
+		print_aggr(NULL);
+		break;
+	case AGGR_GLOBAL:
 		list_for_each_entry(counter, &evsel_list->entries, node)
 			print_counter_aggr(counter, NULL);
+		break;
+	case AGGR_NONE:
+		list_for_each_entry(counter, &evsel_list->entries, node)
+			print_counter(counter, NULL);
+		break;
+	default:
+		break;
 	}
 
 	if (!csv_output) {
@@ -1126,6 +1169,32 @@ static int stat__set_big_num(const struct option *opt __maybe_unused,
 	return 0;
 }
 
+static int perf_stat_init_aggr_mode(void)
+{
+	switch (aggr_mode) {
+	case AGGR_SOCKET:
+		if (cpu_map__build_socket_map(evsel_list->cpus, &aggr_map)) {
+			perror("cannot build socket map");
+			return -1;
+		}
+		aggr_get_id = cpu_map__get_socket;
+		break;
+	case AGGR_CORE:
+		if (cpu_map__build_core_map(evsel_list->cpus, &aggr_map)) {
+			perror("cannot build core map");
+			return -1;
+		}
+		aggr_get_id = cpu_map__get_core;
+		break;
+	case AGGR_NONE:
+	case AGGR_GLOBAL:
+	default:
+		break;
+	}
+	return 0;
+}
+
+
 /*
  * Add default attributes, if there were no attributes specified or
  * if -d/--detailed, -d -d or -d -d -d is used:
@@ -1296,7 +1365,7 @@ int cmd_stat(int argc, const char **argv, const char *prefix __maybe_unused)
 	OPT_INCR('v', "verbose", &verbose,
 		    "be more verbose (show counter open errors, etc)"),
 	OPT_INTEGER('r', "repeat", &run_count,
-		    "repeat command and print average + stddev (max: 100)"),
+		    "repeat command and print average + stddev (max: 100, forever: 0)"),
 	OPT_BOOLEAN('n', "null", &null_run,
 		    "null run - dont start any counters"),
 	OPT_INCR('d', "detailed", &detailed_run,
@@ -1308,7 +1377,8 @@ int cmd_stat(int argc, const char **argv, const char *prefix __maybe_unused)
 			   stat__set_big_num),
 	OPT_STRING('C', "cpu", &target.cpu_list, "cpu",
 		    "list of cpus to monitor in system-wide"),
-	OPT_BOOLEAN('A', "no-aggr", &no_aggr, "disable CPU count aggregation"),
+	OPT_SET_UINT('A', "no-aggr", &aggr_mode,
+		    "disable CPU count aggregation", AGGR_NONE),
 	OPT_STRING('x', "field-separator", &csv_sep, "separator",
 		   "print counts with custom separator"),
 	OPT_CALLBACK('G', "cgroup", &evsel_list, "name",
@@ -1323,20 +1393,22 @@ int cmd_stat(int argc, const char **argv, const char *prefix __maybe_unused)
 			"command to run after to the measured command"),
 	OPT_UINTEGER('I', "interval-print", &interval,
 		    "print counts at regular interval in ms (>= 100)"),
-	OPT_BOOLEAN(0, "aggr-socket", &aggr_socket, "aggregate counts per processor socket"),
+	OPT_SET_UINT(0, "per-socket", &aggr_mode,
+		     "aggregate counts per processor socket", AGGR_SOCKET),
+	OPT_SET_UINT(0, "per-core", &aggr_mode,
+		     "aggregate counts per physical processor core", AGGR_CORE),
 	OPT_END()
 	};
 	const char * const stat_usage[] = {
 		"perf stat [<options>] [<command>]",
 		NULL
 	};
-	struct perf_evsel *pos;
 	int status = -ENOMEM, run_idx;
 	const char *mode;
 
 	setlocale(LC_ALL, "");
 
-	evsel_list = perf_evlist__new(NULL, NULL);
+	evsel_list = perf_evlist__new();
 	if (evsel_list == NULL)
 		return -ENOMEM;
 
@@ -1399,23 +1471,21 @@ int cmd_stat(int argc, const char **argv, const char *prefix __maybe_unused)
 
 	if (!argc && !perf_target__has_task(&target))
 		usage_with_options(stat_usage, options);
-	if (run_count <= 0)
+	if (run_count < 0) {
 		usage_with_options(stat_usage, options);
+	} else if (run_count == 0) {
+		forever = true;
+		run_count = 1;
+	}
 
 	/* no_aggr, cgroup are for system-wide only */
-	if ((no_aggr || nr_cgroups) && !perf_target__has_cpu(&target)) {
+	if ((aggr_mode != AGGR_GLOBAL || nr_cgroups)
+	     && !perf_target__has_cpu(&target)) {
 		fprintf(stderr, "both cgroup and no-aggregation "
 			"modes only available in system-wide mode\n");
 
 		usage_with_options(stat_usage, options);
-	}
-
-	if (aggr_socket) {
-		if (!perf_target__has_cpu(&target)) {
-			fprintf(stderr, "--aggr-socket only available in system-wide mode (-a)\n");
-			usage_with_options(stat_usage, options);
-		}
-		no_aggr = true;
+		return -1;
 	}
 
 	if (add_default_attributes())
@@ -1438,17 +1508,11 @@ int cmd_stat(int argc, const char **argv, const char *prefix __maybe_unused)
 		return -1;
 	}
 
-	list_for_each_entry(pos, &evsel_list->entries, node) {
-		if (perf_evsel__alloc_stat_priv(pos) < 0 ||
-		    perf_evsel__alloc_counts(pos, perf_evsel__nr_cpus(pos)) < 0)
-			goto out_free_fd;
-	}
-	if (interval) {
-		list_for_each_entry(pos, &evsel_list->entries, node) {
-			if (perf_evsel__alloc_prev_raw_counts(pos) < 0)
-				goto out_free_fd;
-		}
-	}
+	if (perf_evlist__alloc_stats(evsel_list, interval))
+		goto out_free_maps;
+
+	if (perf_stat_init_aggr_mode())
+		goto out;
 
 	/*
 	 * We dont want to block the signals - that would cause
@@ -1457,28 +1521,30 @@ int cmd_stat(int argc, const char **argv, const char *prefix __maybe_unused)
 	 * task, but being ignored by perf stat itself:
 	 */
 	atexit(sig_atexit);
-	signal(SIGINT,  skip_signal);
+	if (!forever)
+		signal(SIGINT,  skip_signal);
 	signal(SIGCHLD, skip_signal);
 	signal(SIGALRM, skip_signal);
 	signal(SIGABRT, skip_signal);
 
 	status = 0;
-	for (run_idx = 0; run_idx < run_count; run_idx++) {
+	for (run_idx = 0; forever || run_idx < run_count; run_idx++) {
 		if (run_count != 1 && verbose)
 			fprintf(output, "[ perf stat: executing run #%d ... ]\n",
 				run_idx + 1);
 
 		status = run_perf_stat(argc, argv);
+		if (forever && status != -1) {
+			print_stat(argc, argv);
+			perf_stat__reset_stats(evsel_list);
+		}
 	}
 
-	if (status != -1 && !interval)
+	if (!forever && status != -1 && !interval)
 		print_stat(argc, argv);
-out_free_fd:
-	list_for_each_entry(pos, &evsel_list->entries, node) {
-		perf_evsel__free_stat_priv(pos);
-		perf_evsel__free_counts(pos);
-		perf_evsel__free_prev_raw_counts(pos);
-	}
+
+	perf_evlist__free_stats(evsel_list);
+out_free_maps:
 	perf_evlist__delete_maps(evsel_list);
 out:
 	perf_evlist__delete(evsel_list);
diff --git a/tools/perf/builtin-top.c b/tools/perf/builtin-top.c
index 72f6eb7b417..67bdb9f14ad 100644
--- a/tools/perf/builtin-top.c
+++ b/tools/perf/builtin-top.c
@@ -231,7 +231,7 @@ static void perf_top__show_details(struct perf_top *top)
 	printf("Showing %s for %s\n", perf_evsel__name(top->sym_evsel), symbol->name);
 	printf("  Events  Pcnt (>=%d%%)\n", top->sym_pcnt_filter);
 
-	more = symbol__annotate_printf(symbol, he->ms.map, top->sym_evsel->idx,
+	more = symbol__annotate_printf(symbol, he->ms.map, top->sym_evsel,
 				       0, top->sym_pcnt_filter, top->print_entries, 4);
 	if (top->zero)
 		symbol__annotate_zero_histogram(symbol, top->sym_evsel->idx);
@@ -251,7 +251,8 @@ static struct hist_entry *perf_evsel__add_hist_entry(struct perf_evsel *evsel,
 {
 	struct hist_entry *he;
 
-	he = __hists__add_entry(&evsel->hists, al, NULL, sample->period);
+	he = __hists__add_entry(&evsel->hists, al, NULL, sample->period,
+				sample->weight);
 	if (he == NULL)
 		return NULL;
 
@@ -1088,7 +1089,7 @@ int cmd_top(int argc, const char **argv, const char *prefix __maybe_unused)
 	OPT_INCR('v', "verbose", &verbose,
 		    "be more verbose (show counter open errors, etc)"),
 	OPT_STRING('s', "sort", &sort_order, "key[,key2...]",
-		   "sort by key(s): pid, comm, dso, symbol, parent"),
+		   "sort by key(s): pid, comm, dso, symbol, parent, weight, local_weight"),
 	OPT_BOOLEAN('n', "show-nr-samples", &symbol_conf.show_nr_samples,
 		    "Show a column with the number of samples"),
 	OPT_CALLBACK_DEFAULT('G', "call-graph", &top.record_opts,
@@ -1116,7 +1117,7 @@ int cmd_top(int argc, const char **argv, const char *prefix __maybe_unused)
 		NULL
 	};
 
-	top.evlist = perf_evlist__new(NULL, NULL);
+	top.evlist = perf_evlist__new();
 	if (top.evlist == NULL)
 		return -ENOMEM;
 
diff --git a/tools/perf/builtin-trace.c b/tools/perf/builtin-trace.c
index d222d7fc7e9..ab3ed4af146 100644
--- a/tools/perf/builtin-trace.c
+++ b/tools/perf/builtin-trace.c
@@ -419,7 +419,7 @@ out_dump:
 
 static int trace__run(struct trace *trace, int argc, const char **argv)
 {
-	struct perf_evlist *evlist = perf_evlist__new(NULL, NULL);
+	struct perf_evlist *evlist = perf_evlist__new();
 	struct perf_evsel *evsel;
 	int err = -1, i;
 	unsigned long before;
@@ -452,7 +452,7 @@ static int trace__run(struct trace *trace, int argc, const char **argv)
 	err = trace__symbols_init(trace, evlist);
 	if (err < 0) {
 		printf("Problems initializing symbol libraries!\n");
-		goto out_delete_evlist;
+		goto out_delete_maps;
 	}
 
 	perf_evlist__config(evlist, &trace->opts);
@@ -461,23 +461,24 @@ static int trace__run(struct trace *trace, int argc, const char **argv)
 	signal(SIGINT, sig_handler);
 
 	if (forks) {
-		err = perf_evlist__prepare_workload(evlist, &trace->opts, argv);
+		err = perf_evlist__prepare_workload(evlist, &trace->opts.target,
+						    argv, false, false);
 		if (err < 0) {
 			printf("Couldn't run the workload!\n");
-			goto out_delete_evlist;
+			goto out_delete_maps;
 		}
 	}
 
 	err = perf_evlist__open(evlist);
 	if (err < 0) {
 		printf("Couldn't create the events: %s\n", strerror(errno));
-		goto out_delete_evlist;
+		goto out_delete_maps;
 	}
 
 	err = perf_evlist__mmap(evlist, UINT_MAX, false);
 	if (err < 0) {
 		printf("Couldn't mmap the events: %s\n", strerror(errno));
-		goto out_delete_evlist;
+		goto out_close_evlist;
 	}
 
 	perf_evlist__enable(evlist);
@@ -526,13 +527,6 @@ again:
 				continue;
 			}
 
-			if (sample.raw_data == NULL) {
-				printf("%s sample with no payload for tid: %d, cpu %d, raw_size=%d, skipping...\n",
-				       perf_evsel__name(evsel), sample.tid,
-				       sample.cpu, sample.raw_size);
-				continue;
-			}
-
 			handler = evsel->handler.func;
 			handler(trace, evsel, &sample);
 		}
@@ -540,7 +534,7 @@ again:
 
 	if (trace->nr_events == before) {
 		if (done)
-			goto out_delete_evlist;
+			goto out_unmap_evlist;
 
 		poll(evlist->pollfd, evlist->nr_fds, -1);
 	}
@@ -550,6 +544,12 @@ again:
 
 	goto again;
 
+out_unmap_evlist:
+	perf_evlist__munmap(evlist);
+out_close_evlist:
+	perf_evlist__close(evlist);
+out_delete_maps:
+	perf_evlist__delete_maps(evlist);
 out_delete_evlist:
 	perf_evlist__delete(evlist);
 out:
diff --git a/tools/perf/builtin.h b/tools/perf/builtin.h
index 08143bd854c..b210d62907e 100644
--- a/tools/perf/builtin.h
+++ b/tools/perf/builtin.h
@@ -36,6 +36,7 @@ extern int cmd_kvm(int argc, const char **argv, const char *prefix);
 extern int cmd_test(int argc, const char **argv, const char *prefix);
 extern int cmd_trace(int argc, const char **argv, const char *prefix);
 extern int cmd_inject(int argc, const char **argv, const char *prefix);
+extern int cmd_mem(int argc, const char **argv, const char *prefix);
 
 extern int find_scripts(char **scripts_array, char **scripts_path_array);
 #endif
diff --git a/tools/perf/command-list.txt b/tools/perf/command-list.txt
index 3e86bbd8c2d..0906fc401c5 100644
--- a/tools/perf/command-list.txt
+++ b/tools/perf/command-list.txt
@@ -10,17 +10,18 @@ perf-buildid-list		mainporcelain common
 perf-diff			mainporcelain common
 perf-evlist			mainporcelain common
 perf-inject			mainporcelain common
+perf-kmem			mainporcelain common
+perf-kvm			mainporcelain common
 perf-list			mainporcelain common
-perf-sched			mainporcelain common
+perf-lock			mainporcelain common
+perf-mem			mainporcelain common
+perf-probe			mainporcelain full
 perf-record			mainporcelain common
 perf-report			mainporcelain common
+perf-sched			mainporcelain common
+perf-script			mainporcelain common
 perf-stat			mainporcelain common
+perf-test			mainporcelain common
 perf-timechart			mainporcelain common
 perf-top			mainporcelain common
 perf-trace			mainporcelain common
-perf-script			mainporcelain common
-perf-probe			mainporcelain full
-perf-kmem			mainporcelain common
-perf-lock			mainporcelain common
-perf-kvm			mainporcelain common
-perf-test			mainporcelain common
diff --git a/tools/perf/config/feature-tests.mak b/tools/perf/config/feature-tests.mak
index b4eabb44e38..708fb8e9822 100644
--- a/tools/perf/config/feature-tests.mak
+++ b/tools/perf/config/feature-tests.mak
@@ -61,15 +61,13 @@ int main(void)
 }
 endef
 
-ifndef NO_NEWT
-define SOURCE_NEWT
-#include <newt.h>
+ifndef NO_SLANG
+define SOURCE_SLANG
+#include <slang.h>
 
 int main(void)
 {
-	newtInit();
-	newtCls();
-	return newtFinished();
+	return SLsmg_init_smg();
 }
 endef
 endif
@@ -235,4 +233,4 @@ int main(void)
 	numa_available();
 	return 0;
 }
-endef
\ No newline at end of file
+endef
diff --git a/tools/perf/perf.c b/tools/perf/perf.c
index 095b88207cd..85e1aed9520 100644
--- a/tools/perf/perf.c
+++ b/tools/perf/perf.c
@@ -13,7 +13,7 @@
 #include "util/quote.h"
 #include "util/run-command.h"
 #include "util/parse-events.h"
-#include "util/debugfs.h"
+#include <lk/debugfs.h>
 #include <pthread.h>
 
 const char perf_usage_string[] =
@@ -60,6 +60,7 @@ static struct cmd_struct commands[] = {
 	{ "trace",	cmd_trace,	0 },
 #endif
 	{ "inject",	cmd_inject,	0 },
+	{ "mem",	cmd_mem,	0 },
 };
 
 struct pager_config {
@@ -193,13 +194,13 @@ static int handle_options(const char ***argv, int *argc, int *envchanged)
 				fprintf(stderr, "No directory given for --debugfs-dir.\n");
 				usage(perf_usage_string);
 			}
-			debugfs_set_path((*argv)[1]);
+			perf_debugfs_set_path((*argv)[1]);
 			if (envchanged)
 				*envchanged = 1;
 			(*argv)++;
 			(*argc)--;
 		} else if (!prefixcmp(cmd, CMD_DEBUGFS_DIR)) {
-			debugfs_set_path(cmd + strlen(CMD_DEBUGFS_DIR));
+			perf_debugfs_set_path(cmd + strlen(CMD_DEBUGFS_DIR));
 			fprintf(stderr, "dir: %s\n", debugfs_mountpoint);
 			if (envchanged)
 				*envchanged = 1;
@@ -461,7 +462,7 @@ int main(int argc, const char **argv)
 	if (!cmd)
 		cmd = "perf-help";
 	/* get debugfs mount point from /proc/mounts */
-	debugfs_mount(NULL);
+	perf_debugfs_mount(NULL);
 	/*
 	 * "perf-xxxx" is the same as "perf xxxx", but we obviously:
 	 *
@@ -517,9 +518,8 @@ int main(int argc, const char **argv)
 
 	while (1) {
 		static int done_help;
-		static int was_alias;
+		int was_alias = run_argv(&argc, &argv);
 
-		was_alias = run_argv(&argc, &argv);
 		if (errno != ENOENT)
 			break;
 
diff --git a/tools/perf/perf.h b/tools/perf/perf.h
index 74659ecf93e..32bd102c32b 100644
--- a/tools/perf/perf.h
+++ b/tools/perf/perf.h
@@ -218,6 +218,7 @@ struct perf_record_opts {
 	bool	     pipe_output;
 	bool	     raw_samples;
 	bool	     sample_address;
+	bool	     sample_weight;
 	bool	     sample_time;
 	bool	     period;
 	unsigned int freq;
diff --git a/tools/perf/tests/attr.c b/tools/perf/tests/attr.c
index bdcceb886f7..00218f503b2 100644
--- a/tools/perf/tests/attr.c
+++ b/tools/perf/tests/attr.c
@@ -147,10 +147,15 @@ void test_attr__open(struct perf_event_attr *attr, pid_t pid, int cpu,
 
 static int run_dir(const char *d, const char *perf)
 {
+	char v[] = "-vvvvv";
+	int vcnt = min(verbose, (int) sizeof(v) - 1);
 	char cmd[3*PATH_MAX];
 
-	snprintf(cmd, 3*PATH_MAX, PYTHON " %s/attr.py -d %s/attr/ -p %s %s",
-		 d, d, perf, verbose ? "-v" : "");
+	if (verbose)
+		vcnt++;
+
+	snprintf(cmd, 3*PATH_MAX, PYTHON " %s/attr.py -d %s/attr/ -p %s %.*s",
+		 d, d, perf, vcnt, v);
 
 	return system(cmd);
 }
@@ -173,6 +178,6 @@ int test__attr(void)
 	    !lstat(path_perf, &st))
 		return run_dir(path_dir, path_perf);
 
-	fprintf(stderr, " (ommitted)");
+	fprintf(stderr, " (omitted)");
 	return 0;
 }
diff --git a/tools/perf/tests/attr.py b/tools/perf/tests/attr.py
index 2f629ca485b..c9b4b6269b5 100644
--- a/tools/perf/tests/attr.py
+++ b/tools/perf/tests/attr.py
@@ -24,6 +24,7 @@ class Unsup(Exception):
 
 class Event(dict):
     terms = [
+        'cpu',
         'flags',
         'type',
         'size',
@@ -121,7 +122,7 @@ class Test(object):
         parser = ConfigParser.SafeConfigParser()
         parser.read(path)
 
-        log.debug("running '%s'" % path)
+        log.warning("running '%s'" % path)
 
         self.path     = path
         self.test_dir = options.test_dir
@@ -172,7 +173,7 @@ class Test(object):
               self.perf, self.command, tempdir, self.args)
         ret = os.WEXITSTATUS(os.system(cmd))
 
-        log.warning("  running '%s' ret %d " % (cmd, ret))
+        log.info("  '%s' ret %d " % (cmd, ret))
 
         if ret != int(self.ret):
             raise Unsup(self)
diff --git a/tools/perf/tests/attr/base-record b/tools/perf/tests/attr/base-record
index 5bc3880f7be..b4fc835de60 100644
--- a/tools/perf/tests/attr/base-record
+++ b/tools/perf/tests/attr/base-record
@@ -2,6 +2,7 @@
 fd=1
 group_fd=-1
 flags=0
+cpu=*
 type=0|1
 size=96
 config=0
diff --git a/tools/perf/tests/attr/base-stat b/tools/perf/tests/attr/base-stat
index 4bd79a82784..748ee949a20 100644
--- a/tools/perf/tests/attr/base-stat
+++ b/tools/perf/tests/attr/base-stat
@@ -2,6 +2,7 @@
 fd=1
 group_fd=-1
 flags=0
+cpu=*
 type=0
 size=96
 config=0
diff --git a/tools/perf/tests/attr/test-record-C0 b/tools/perf/tests/attr/test-record-C0
new file mode 100644
index 00000000000..d6a7e43f61b
--- /dev/null
+++ b/tools/perf/tests/attr/test-record-C0
@@ -0,0 +1,13 @@
+[config]
+command = record
+args    = -C 0 kill >/dev/null 2>&1
+
+[event:base-record]
+cpu=0
+
+# no enable on exec for CPU attached
+enable_on_exec=0
+
+# PERF_SAMPLE_IP | PERF_SAMPLE_TID PERF_SAMPLE_TIME | # PERF_SAMPLE_PERIOD
+# + PERF_SAMPLE_CPU added by -C 0
+sample_type=391
diff --git a/tools/perf/tests/attr/test-stat-C0 b/tools/perf/tests/attr/test-stat-C0
new file mode 100644
index 00000000000..aa835950751
--- /dev/null
+++ b/tools/perf/tests/attr/test-stat-C0
@@ -0,0 +1,9 @@
+[config]
+command = stat
+args    = -e cycles -C 0 kill >/dev/null 2>&1
+ret     = 1
+
+[event:base-stat]
+# events are enabled by default when attached to cpu
+disabled=0
+enable_on_exec=0
diff --git a/tools/perf/tests/bp_signal.c b/tools/perf/tests/bp_signal.c
new file mode 100644
index 00000000000..68daa289e94
--- /dev/null
+++ b/tools/perf/tests/bp_signal.c
@@ -0,0 +1,186 @@
+/*
+ * Inspired by breakpoint overflow test done by
+ * Vince Weaver <vincent.weaver@maine.edu> for perf_event_tests
+ * (git://github.com/deater/perf_event_tests)
+ */
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <time.h>
+#include <fcntl.h>
+#include <signal.h>
+#include <sys/mman.h>
+#include <linux/compiler.h>
+#include <linux/hw_breakpoint.h>
+
+#include "tests.h"
+#include "debug.h"
+#include "perf.h"
+
+static int fd1;
+static int fd2;
+static int overflows;
+
+__attribute__ ((noinline))
+static int test_function(void)
+{
+	return time(NULL);
+}
+
+static void sig_handler(int signum __maybe_unused,
+			siginfo_t *oh __maybe_unused,
+			void *uc __maybe_unused)
+{
+	overflows++;
+
+	if (overflows > 10) {
+		/*
+		 * This should be executed only once during
+		 * this test, if we are here for the 10th
+		 * time, consider this the recursive issue.
+		 *
+		 * We can get out of here by disable events,
+		 * so no new SIGIO is delivered.
+		 */
+		ioctl(fd1, PERF_EVENT_IOC_DISABLE, 0);
+		ioctl(fd2, PERF_EVENT_IOC_DISABLE, 0);
+	}
+}
+
+static int bp_event(void *fn, int setup_signal)
+{
+	struct perf_event_attr pe;
+	int fd;
+
+	memset(&pe, 0, sizeof(struct perf_event_attr));
+	pe.type = PERF_TYPE_BREAKPOINT;
+	pe.size = sizeof(struct perf_event_attr);
+
+	pe.config = 0;
+	pe.bp_type = HW_BREAKPOINT_X;
+	pe.bp_addr = (unsigned long) fn;
+	pe.bp_len = sizeof(long);
+
+	pe.sample_period = 1;
+	pe.sample_type = PERF_SAMPLE_IP;
+	pe.wakeup_events = 1;
+
+	pe.disabled = 1;
+	pe.exclude_kernel = 1;
+	pe.exclude_hv = 1;
+
+	fd = sys_perf_event_open(&pe, 0, -1, -1, 0);
+	if (fd < 0) {
+		pr_debug("failed opening event %llx\n", pe.config);
+		return TEST_FAIL;
+	}
+
+	if (setup_signal) {
+		fcntl(fd, F_SETFL, O_RDWR|O_NONBLOCK|O_ASYNC);
+		fcntl(fd, F_SETSIG, SIGIO);
+		fcntl(fd, F_SETOWN, getpid());
+	}
+
+	ioctl(fd, PERF_EVENT_IOC_RESET, 0);
+
+	return fd;
+}
+
+static long long bp_count(int fd)
+{
+	long long count;
+	int ret;
+
+	ret = read(fd, &count, sizeof(long long));
+	if (ret != sizeof(long long)) {
+		pr_debug("failed to read: %d\n", ret);
+		return TEST_FAIL;
+	}
+
+	return count;
+}
+
+int test__bp_signal(void)
+{
+	struct sigaction sa;
+	long long count1, count2;
+
+	/* setup SIGIO signal handler */
+	memset(&sa, 0, sizeof(struct sigaction));
+	sa.sa_sigaction = (void *) sig_handler;
+	sa.sa_flags = SA_SIGINFO;
+
+	if (sigaction(SIGIO, &sa, NULL) < 0) {
+		pr_debug("failed setting up signal handler\n");
+		return TEST_FAIL;
+	}
+
+	/*
+	 * We create following events:
+	 *
+	 * fd1 - breakpoint event on test_function with SIGIO
+	 *       signal configured. We should get signal
+	 *       notification each time the breakpoint is hit
+	 *
+	 * fd2 - breakpoint event on sig_handler without SIGIO
+	 *       configured.
+	 *
+	 * Following processing should happen:
+	 *   - execute test_function
+	 *   - fd1 event breakpoint hit -> count1 == 1
+	 *   - SIGIO is delivered       -> overflows == 1
+	 *   - fd2 event breakpoint hit -> count2 == 1
+	 *
+	 * The test case check following error conditions:
+	 * - we get stuck in signal handler because of debug
+	 *   exception being triggered receursively due to
+	 *   the wrong RF EFLAG management
+	 *
+	 * - we never trigger the sig_handler breakpoint due
+	 *   to the rong RF EFLAG management
+	 *
+	 */
+
+	fd1 = bp_event(test_function, 1);
+	fd2 = bp_event(sig_handler, 0);
+
+	ioctl(fd1, PERF_EVENT_IOC_ENABLE, 0);
+	ioctl(fd2, PERF_EVENT_IOC_ENABLE, 0);
+
+	/*
+	 * Kick off the test by trigering 'fd1'
+	 * breakpoint.
+	 */
+	test_function();
+
+	ioctl(fd1, PERF_EVENT_IOC_DISABLE, 0);
+	ioctl(fd2, PERF_EVENT_IOC_DISABLE, 0);
+
+	count1 = bp_count(fd1);
+	count2 = bp_count(fd2);
+
+	close(fd1);
+	close(fd2);
+
+	pr_debug("count1 %lld, count2 %lld, overflow %d\n",
+		 count1, count2, overflows);
+
+	if (count1 != 1) {
+		if (count1 == 11)
+			pr_debug("failed: RF EFLAG recursion issue detected\n");
+		else
+			pr_debug("failed: wrong count for bp1%lld\n", count1);
+	}
+
+	if (overflows != 1)
+		pr_debug("failed: wrong overflow hit\n");
+
+	if (count2 != 1)
+		pr_debug("failed: wrong count for bp2\n");
+
+	return count1 == 1 && overflows == 1 && count2 == 1 ?
+		TEST_OK : TEST_FAIL;
+}
diff --git a/tools/perf/tests/bp_signal_overflow.c b/tools/perf/tests/bp_signal_overflow.c
new file mode 100644
index 00000000000..fe7ed28815f
--- /dev/null
+++ b/tools/perf/tests/bp_signal_overflow.c
@@ -0,0 +1,126 @@
+/*
+ * Originally done by Vince Weaver <vincent.weaver@maine.edu> for
+ * perf_event_tests (git://github.com/deater/perf_event_tests)
+ */
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <time.h>
+#include <fcntl.h>
+#include <signal.h>
+#include <sys/mman.h>
+#include <linux/compiler.h>
+#include <linux/hw_breakpoint.h>
+
+#include "tests.h"
+#include "debug.h"
+#include "perf.h"
+
+static int overflows;
+
+__attribute__ ((noinline))
+static int test_function(void)
+{
+	return time(NULL);
+}
+
+static void sig_handler(int signum __maybe_unused,
+			siginfo_t *oh __maybe_unused,
+			void *uc __maybe_unused)
+{
+	overflows++;
+}
+
+static long long bp_count(int fd)
+{
+	long long count;
+	int ret;
+
+	ret = read(fd, &count, sizeof(long long));
+	if (ret != sizeof(long long)) {
+		pr_debug("failed to read: %d\n", ret);
+		return TEST_FAIL;
+	}
+
+	return count;
+}
+
+#define EXECUTIONS 10000
+#define THRESHOLD  100
+
+int test__bp_signal_overflow(void)
+{
+	struct perf_event_attr pe;
+	struct sigaction sa;
+	long long count;
+	int fd, i, fails = 0;
+
+	/* setup SIGIO signal handler */
+	memset(&sa, 0, sizeof(struct sigaction));
+	sa.sa_sigaction = (void *) sig_handler;
+	sa.sa_flags = SA_SIGINFO;
+
+	if (sigaction(SIGIO, &sa, NULL) < 0) {
+		pr_debug("failed setting up signal handler\n");
+		return TEST_FAIL;
+	}
+
+	memset(&pe, 0, sizeof(struct perf_event_attr));
+	pe.type = PERF_TYPE_BREAKPOINT;
+	pe.size = sizeof(struct perf_event_attr);
+
+	pe.config = 0;
+	pe.bp_type = HW_BREAKPOINT_X;
+	pe.bp_addr = (unsigned long) test_function;
+	pe.bp_len = sizeof(long);
+
+	pe.sample_period = THRESHOLD;
+	pe.sample_type = PERF_SAMPLE_IP;
+	pe.wakeup_events = 1;
+
+	pe.disabled = 1;
+	pe.exclude_kernel = 1;
+	pe.exclude_hv = 1;
+
+	fd = sys_perf_event_open(&pe, 0, -1, -1, 0);
+	if (fd < 0) {
+		pr_debug("failed opening event %llx\n", pe.config);
+		return TEST_FAIL;
+	}
+
+	fcntl(fd, F_SETFL, O_RDWR|O_NONBLOCK|O_ASYNC);
+	fcntl(fd, F_SETSIG, SIGIO);
+	fcntl(fd, F_SETOWN, getpid());
+
+	ioctl(fd, PERF_EVENT_IOC_RESET, 0);
+	ioctl(fd, PERF_EVENT_IOC_ENABLE, 0);
+
+	for (i = 0; i < EXECUTIONS; i++)
+		test_function();
+
+	ioctl(fd, PERF_EVENT_IOC_DISABLE, 0);
+
+	count = bp_count(fd);
+
+	close(fd);
+
+	pr_debug("count %lld, overflow %d\n",
+		 count, overflows);
+
+	if (count != EXECUTIONS) {
+		pr_debug("\tWrong number of executions %lld != %d\n",
+		count, EXECUTIONS);
+		fails++;
+	}
+
+	if (overflows != EXECUTIONS / THRESHOLD) {
+		pr_debug("\tWrong number of overflows %d != %d\n",
+		overflows, EXECUTIONS / THRESHOLD);
+		fails++;
+	}
+
+	return fails ? TEST_FAIL : TEST_OK;
+}
diff --git a/tools/perf/tests/builtin-test.c b/tools/perf/tests/builtin-test.c
index acb98e0e39f..0918ada4cc4 100644
--- a/tools/perf/tests/builtin-test.c
+++ b/tools/perf/tests/builtin-test.c
@@ -78,6 +78,22 @@ static struct test {
 		.func = test__python_use,
 	},
 	{
+		.desc = "Test breakpoint overflow signal handler",
+		.func = test__bp_signal,
+	},
+	{
+		.desc = "Test breakpoint overflow sampling",
+		.func = test__bp_signal_overflow,
+	},
+	{
+		.desc = "Test number of exit event of a simple workload",
+		.func = test__task_exit,
+	},
+	{
+		.desc = "Test software clock events have valid period values",
+		.func = test__sw_clock_freq,
+	},
+	{
 		.func = NULL,
 	},
 };
diff --git a/tools/perf/tests/evsel-roundtrip-name.c b/tools/perf/tests/evsel-roundtrip-name.c
index 0fd99a9adb9..0197bda9c46 100644
--- a/tools/perf/tests/evsel-roundtrip-name.c
+++ b/tools/perf/tests/evsel-roundtrip-name.c
@@ -8,7 +8,7 @@ static int perf_evsel__roundtrip_cache_name_test(void)
 	char name[128];
 	int type, op, err = 0, ret = 0, i, idx;
 	struct perf_evsel *evsel;
-        struct perf_evlist *evlist = perf_evlist__new(NULL, NULL);
+	struct perf_evlist *evlist = perf_evlist__new();
 
         if (evlist == NULL)
                 return -ENOMEM;
@@ -64,7 +64,7 @@ static int __perf_evsel__name_array_test(const char *names[], int nr_names)
 {
 	int i, err;
 	struct perf_evsel *evsel;
-        struct perf_evlist *evlist = perf_evlist__new(NULL, NULL);
+	struct perf_evlist *evlist = perf_evlist__new();
 
         if (evlist == NULL)
                 return -ENOMEM;
diff --git a/tools/perf/tests/hists_link.c b/tools/perf/tests/hists_link.c
index 1be64a6c5da..89085a9615e 100644
--- a/tools/perf/tests/hists_link.c
+++ b/tools/perf/tests/hists_link.c
@@ -223,7 +223,7 @@ static int add_hist_entries(struct perf_evlist *evlist, struct machine *machine)
 							  &sample, 0) < 0)
 				goto out;
 
-			he = __hists__add_entry(&evsel->hists, &al, NULL, 1);
+			he = __hists__add_entry(&evsel->hists, &al, NULL, 1, 1);
 			if (he == NULL)
 				goto out;
 
@@ -247,7 +247,7 @@ static int add_hist_entries(struct perf_evlist *evlist, struct machine *machine)
 							  &sample, 0) < 0)
 				goto out;
 
-			he = __hists__add_entry(&evsel->hists, &al, NULL, 1);
+			he = __hists__add_entry(&evsel->hists, &al, NULL, 1, 1);
 			if (he == NULL)
 				goto out;
 
@@ -436,7 +436,7 @@ int test__hists_link(void)
 	struct machines machines;
 	struct machine *machine = NULL;
 	struct perf_evsel *evsel, *first;
-        struct perf_evlist *evlist = perf_evlist__new(NULL, NULL);
+	struct perf_evlist *evlist = perf_evlist__new();
 
 	if (evlist == NULL)
                 return -ENOMEM;
diff --git a/tools/perf/tests/mmap-basic.c b/tools/perf/tests/mmap-basic.c
index cdd50755af5..5b1b5aba722 100644
--- a/tools/perf/tests/mmap-basic.c
+++ b/tools/perf/tests/mmap-basic.c
@@ -53,12 +53,14 @@ int test__basic_mmap(void)
 		goto out_free_cpus;
 	}
 
-	evlist = perf_evlist__new(cpus, threads);
+	evlist = perf_evlist__new();
 	if (evlist == NULL) {
 		pr_debug("perf_evlist__new\n");
 		goto out_free_cpus;
 	}
 
+	perf_evlist__set_maps(evlist, cpus, threads);
+
 	for (i = 0; i < nsyscalls; ++i) {
 		char name[64];
 
diff --git a/tools/perf/tests/open-syscall-tp-fields.c b/tools/perf/tests/open-syscall-tp-fields.c
index 1c52fdc1164..fc5b9fca8b4 100644
--- a/tools/perf/tests/open-syscall-tp-fields.c
+++ b/tools/perf/tests/open-syscall-tp-fields.c
@@ -18,7 +18,7 @@ int test__syscall_open_tp_fields(void)
 	};
 	const char *filename = "/etc/passwd";
 	int flags = O_RDONLY | O_DIRECTORY;
-	struct perf_evlist *evlist = perf_evlist__new(NULL, NULL);
+	struct perf_evlist *evlist = perf_evlist__new();
 	struct perf_evsel *evsel;
 	int err = -1, i, nr_events = 0, nr_polls = 0;
 
@@ -48,13 +48,13 @@ int test__syscall_open_tp_fields(void)
 	err = perf_evlist__open(evlist);
 	if (err < 0) {
 		pr_debug("perf_evlist__open: %s\n", strerror(errno));
-		goto out_delete_evlist;
+		goto out_delete_maps;
 	}
 
 	err = perf_evlist__mmap(evlist, UINT_MAX, false);
 	if (err < 0) {
 		pr_debug("perf_evlist__mmap: %s\n", strerror(errno));
-		goto out_delete_evlist;
+		goto out_close_evlist;
 	}
 
 	perf_evlist__enable(evlist);
@@ -110,6 +110,10 @@ out_ok:
 	err = 0;
 out_munmap:
 	perf_evlist__munmap(evlist);
+out_close_evlist:
+	perf_evlist__close(evlist);
+out_delete_maps:
+	perf_evlist__delete_maps(evlist);
 out_delete_evlist:
 	perf_evlist__delete(evlist);
 out:
diff --git a/tools/perf/tests/parse-events.c b/tools/perf/tests/parse-events.c
index c5636f36fe3..0275bab4ea9 100644
--- a/tools/perf/tests/parse-events.c
+++ b/tools/perf/tests/parse-events.c
@@ -3,7 +3,7 @@
 #include "evsel.h"
 #include "evlist.h"
 #include "sysfs.h"
-#include "debugfs.h"
+#include <lk/debugfs.h>
 #include "tests.h"
 #include <linux/hw_breakpoint.h>
 
@@ -1218,7 +1218,7 @@ static int test_event(struct evlist_test *e)
 	struct perf_evlist *evlist;
 	int ret;
 
-	evlist = perf_evlist__new(NULL, NULL);
+	evlist = perf_evlist__new();
 	if (evlist == NULL)
 		return -ENOMEM;
 
@@ -1321,7 +1321,7 @@ static int test_pmu_events(void)
 
 	ret = stat(path, &st);
 	if (ret) {
-		pr_debug("ommiting PMU cpu events tests\n");
+		pr_debug("omitting PMU cpu events tests\n");
 		return 0;
 	}
 
diff --git a/tools/perf/tests/perf-record.c b/tools/perf/tests/perf-record.c
index 1e8e5128d0d..72d8881873b 100644
--- a/tools/perf/tests/perf-record.c
+++ b/tools/perf/tests/perf-record.c
@@ -45,7 +45,7 @@ int test__PERF_RECORD(void)
 	};
 	cpu_set_t cpu_mask;
 	size_t cpu_mask_size = sizeof(cpu_mask);
-	struct perf_evlist *evlist = perf_evlist__new(NULL, NULL);
+	struct perf_evlist *evlist = perf_evlist__new();
 	struct perf_evsel *evsel;
 	struct perf_sample sample;
 	const char *cmd = "sleep";
@@ -93,7 +93,8 @@ int test__PERF_RECORD(void)
 	 * so that we have time to open the evlist (calling sys_perf_event_open
 	 * on all the fds) and then mmap them.
 	 */
-	err = perf_evlist__prepare_workload(evlist, &opts, argv);
+	err = perf_evlist__prepare_workload(evlist, &opts.target, argv,
+					    false, false);
 	if (err < 0) {
 		pr_debug("Couldn't run the workload!\n");
 		goto out_delete_maps;
@@ -142,7 +143,7 @@ int test__PERF_RECORD(void)
 	err = perf_evlist__mmap(evlist, opts.mmap_pages, false);
 	if (err < 0) {
 		pr_debug("perf_evlist__mmap: %s\n", strerror(errno));
-		goto out_delete_maps;
+		goto out_close_evlist;
 	}
 
 	/*
@@ -305,6 +306,8 @@ found_exit:
 	}
 out_err:
 	perf_evlist__munmap(evlist);
+out_close_evlist:
+	perf_evlist__close(evlist);
 out_delete_maps:
 	perf_evlist__delete_maps(evlist);
 out_delete_evlist:
diff --git a/tools/perf/tests/sw-clock.c b/tools/perf/tests/sw-clock.c
new file mode 100644
index 00000000000..2e41e2d32cc
--- /dev/null
+++ b/tools/perf/tests/sw-clock.c
@@ -0,0 +1,119 @@
+#include <unistd.h>
+#include <stdlib.h>
+#include <signal.h>
+#include <sys/mman.h>
+
+#include "tests.h"
+#include "util/evsel.h"
+#include "util/evlist.h"
+#include "util/cpumap.h"
+#include "util/thread_map.h"
+
+#define NR_LOOPS  1000000
+
+/*
+ * This test will open software clock events (cpu-clock, task-clock)
+ * then check their frequency -> period conversion has no artifact of
+ * setting period to 1 forcefully.
+ */
+static int __test__sw_clock_freq(enum perf_sw_ids clock_id)
+{
+	int i, err = -1;
+	volatile int tmp = 0;
+	u64 total_periods = 0;
+	int nr_samples = 0;
+	union perf_event *event;
+	struct perf_evsel *evsel;
+	struct perf_evlist *evlist;
+	struct perf_event_attr attr = {
+		.type = PERF_TYPE_SOFTWARE,
+		.config = clock_id,
+		.sample_type = PERF_SAMPLE_PERIOD,
+		.exclude_kernel = 1,
+		.disabled = 1,
+		.freq = 1,
+	};
+
+	attr.sample_freq = 10000;
+
+	evlist = perf_evlist__new();
+	if (evlist == NULL) {
+		pr_debug("perf_evlist__new\n");
+		return -1;
+	}
+
+	evsel = perf_evsel__new(&attr, 0);
+	if (evsel == NULL) {
+		pr_debug("perf_evsel__new\n");
+		goto out_free_evlist;
+	}
+	perf_evlist__add(evlist, evsel);
+
+	evlist->cpus = cpu_map__dummy_new();
+	evlist->threads = thread_map__new_by_tid(getpid());
+	if (!evlist->cpus || !evlist->threads) {
+		err = -ENOMEM;
+		pr_debug("Not enough memory to create thread/cpu maps\n");
+		goto out_delete_maps;
+	}
+
+	perf_evlist__open(evlist);
+
+	err = perf_evlist__mmap(evlist, 128, true);
+	if (err < 0) {
+		pr_debug("failed to mmap event: %d (%s)\n", errno,
+			 strerror(errno));
+		goto out_close_evlist;
+	}
+
+	perf_evlist__enable(evlist);
+
+	/* collect samples */
+	for (i = 0; i < NR_LOOPS; i++)
+		tmp++;
+
+	perf_evlist__disable(evlist);
+
+	while ((event = perf_evlist__mmap_read(evlist, 0)) != NULL) {
+		struct perf_sample sample;
+
+		if (event->header.type != PERF_RECORD_SAMPLE)
+			continue;
+
+		err = perf_evlist__parse_sample(evlist, event, &sample);
+		if (err < 0) {
+			pr_debug("Error during parse sample\n");
+			goto out_unmap_evlist;
+		}
+
+		total_periods += sample.period;
+		nr_samples++;
+	}
+
+	if ((u64) nr_samples == total_periods) {
+		pr_debug("All (%d) samples have period value of 1!\n",
+			 nr_samples);
+		err = -1;
+	}
+
+out_unmap_evlist:
+	perf_evlist__munmap(evlist);
+out_close_evlist:
+	perf_evlist__close(evlist);
+out_delete_maps:
+	perf_evlist__delete_maps(evlist);
+out_free_evlist:
+	perf_evlist__delete(evlist);
+	return err;
+}
+
+int test__sw_clock_freq(void)
+{
+	int ret;
+
+	ret = __test__sw_clock_freq(PERF_COUNT_SW_CPU_CLOCK);
+	if (!ret)
+		ret = __test__sw_clock_freq(PERF_COUNT_SW_TASK_CLOCK);
+
+	return ret;
+}
diff --git a/tools/perf/tests/task-exit.c b/tools/perf/tests/task-exit.c
new file mode 100644
index 00000000000..28fe5894b06
--- /dev/null
+++ b/tools/perf/tests/task-exit.c
@@ -0,0 +1,123 @@
+#include "evlist.h"
+#include "evsel.h"
+#include "thread_map.h"
+#include "cpumap.h"
+#include "tests.h"
+
+#include <signal.h>
+
+static int exited;
+static int nr_exit;
+
+static void sig_handler(int sig)
+{
+	exited = 1;
+
+	if (sig == SIGUSR1)
+		nr_exit = -1;
+}
+
+/*
+ * This test will start a workload that does nothing then it checks
+ * if the number of exit event reported by the kernel is 1 or not
+ * in order to check the kernel returns correct number of event.
+ */
+int test__task_exit(void)
+{
+	int err = -1;
+	union perf_event *event;
+	struct perf_evsel *evsel;
+	struct perf_evlist *evlist;
+	struct perf_target target = {
+		.uid		= UINT_MAX,
+		.uses_mmap	= true,
+	};
+	const char *argv[] = { "true", NULL };
+
+	signal(SIGCHLD, sig_handler);
+	signal(SIGUSR1, sig_handler);
+
+	evlist = perf_evlist__new();
+	if (evlist == NULL) {
+		pr_debug("perf_evlist__new\n");
+		return -1;
+	}
+	/*
+	 * We need at least one evsel in the evlist, use the default
+	 * one: "cycles".
+	 */
+	err = perf_evlist__add_default(evlist);
+	if (err < 0) {
+		pr_debug("Not enough memory to create evsel\n");
+		goto out_free_evlist;
+	}
+
+	/*
+	 * Create maps of threads and cpus to monitor. In this case
+	 * we start with all threads and cpus (-1, -1) but then in
+	 * perf_evlist__prepare_workload we'll fill in the only thread
+	 * we're monitoring, the one forked there.
+	 */
+	evlist->cpus = cpu_map__dummy_new();
+	evlist->threads = thread_map__new_by_tid(-1);
+	if (!evlist->cpus || !evlist->threads) {
+		err = -ENOMEM;
+		pr_debug("Not enough memory to create thread/cpu maps\n");
+		goto out_delete_maps;
+	}
+
+	err = perf_evlist__prepare_workload(evlist, &target, argv, false, true);
+	if (err < 0) {
+		pr_debug("Couldn't run the workload!\n");
+		goto out_delete_maps;
+	}
+
+	evsel = perf_evlist__first(evlist);
+	evsel->attr.task = 1;
+	evsel->attr.sample_freq = 0;
+	evsel->attr.inherit = 0;
+	evsel->attr.watermark = 0;
+	evsel->attr.wakeup_events = 1;
+	evsel->attr.exclude_kernel = 1;
+
+	err = perf_evlist__open(evlist);
+	if (err < 0) {
+		pr_debug("Couldn't open the evlist: %s\n", strerror(-err));
+		goto out_delete_maps;
+	}
+
+	if (perf_evlist__mmap(evlist, 128, true) < 0) {
+		pr_debug("failed to mmap events: %d (%s)\n", errno,
+			 strerror(errno));
+		goto out_close_evlist;
+	}
+
+	perf_evlist__start_workload(evlist);
+
+retry:
+	while ((event = perf_evlist__mmap_read(evlist, 0)) != NULL) {
+		if (event->header.type != PERF_RECORD_EXIT)
+			continue;
+
+		nr_exit++;
+	}
+
+	if (!exited || !nr_exit) {
+		poll(evlist->pollfd, evlist->nr_fds, -1);
+		goto retry;
+	}
+
+	if (nr_exit != 1) {
+		pr_debug("received %d EXIT records\n", nr_exit);
+		err = -1;
+	}
+
+	perf_evlist__munmap(evlist);
+out_close_evlist:
+	perf_evlist__close(evlist);
+out_delete_maps:
+	perf_evlist__delete_maps(evlist);
+out_free_evlist:
+	perf_evlist__delete(evlist);
+	return err;
+}
diff --git a/tools/perf/tests/tests.h b/tools/perf/tests/tests.h
index 5de0be1ff4b..dd7feae2d37 100644
--- a/tools/perf/tests/tests.h
+++ b/tools/perf/tests/tests.h
@@ -23,5 +23,9 @@ int test__dso_data(void);
 int test__parse_events(void);
 int test__hists_link(void);
 int test__python_use(void);
+int test__bp_signal(void);
+int test__bp_signal_overflow(void);
+int test__task_exit(void);
+int test__sw_clock_freq(void);
 
 #endif /* TESTS_H */
diff --git a/tools/perf/ui/browser.c b/tools/perf/ui/browser.c
index 809ea4632a3..bbc782e364b 100644
--- a/tools/perf/ui/browser.c
+++ b/tools/perf/ui/browser.c
@@ -2,7 +2,6 @@
 #include "../cache.h"
 #include "../../perf.h"
 #include "libslang.h"
-#include <newt.h>
 #include "ui.h"
 #include "util.h"
 #include <linux/compiler.h>
@@ -234,7 +233,7 @@ void ui_browser__reset_index(struct ui_browser *browser)
 void __ui_browser__show_title(struct ui_browser *browser, const char *title)
 {
 	SLsmg_gotorc(0, 0);
-	ui_browser__set_color(browser, NEWT_COLORSET_ROOT);
+	ui_browser__set_color(browser, HE_COLORSET_ROOT);
 	slsmg_write_nstring(title, browser->width + 1);
 }
 
@@ -514,6 +513,12 @@ static struct ui_browser_colorset {
 		.bg	  = "default",
 	},
 	{
+		.colorset = HE_COLORSET_ROOT,
+		.name	  = "root",
+		.fg	  = "white",
+		.bg	  = "blue",
+	},
+	{
 		.name = NULL,
 	}
 };
diff --git a/tools/perf/ui/browser.h b/tools/perf/ui/browser.h
index af70314605e..404ff66a3e3 100644
--- a/tools/perf/ui/browser.h
+++ b/tools/perf/ui/browser.h
@@ -11,6 +11,7 @@
 #define HE_COLORSET_SELECTED	53
 #define HE_COLORSET_CODE	54
 #define HE_COLORSET_ADDR	55
+#define HE_COLORSET_ROOT	56
 
 struct ui_browser {
 	u64	      index, top_idx;
diff --git a/tools/perf/ui/browsers/annotate.c b/tools/perf/ui/browsers/annotate.c
index 7dca1555c61..cc64d3f7fc3 100644
--- a/tools/perf/ui/browsers/annotate.c
+++ b/tools/perf/ui/browsers/annotate.c
@@ -8,15 +8,19 @@
 #include "../../util/hist.h"
 #include "../../util/sort.h"
 #include "../../util/symbol.h"
+#include "../../util/evsel.h"
 #include <pthread.h>
-#include <newt.h>
 
 struct browser_disasm_line {
 	struct rb_node	rb_node;
-	double		percent;
 	u32		idx;
 	int		idx_asm;
 	int		jump_sources;
+	/*
+	 * actual length of this array is saved on the nr_events field
+	 * of the struct annotate_browser
+	 */
+	double		percent[1];
 };
 
 static struct annotate_browser_opt {
@@ -33,8 +37,9 @@ struct annotate_browser {
 	struct ui_browser b;
 	struct rb_root	  entries;
 	struct rb_node	  *curr_hot;
-	struct disasm_line	  *selection;
+	struct disasm_line  *selection;
 	struct disasm_line  **offsets;
+	int		    nr_events;
 	u64		    start;
 	int		    nr_asm_entries;
 	int		    nr_entries;
@@ -94,14 +99,24 @@ static void annotate_browser__write(struct ui_browser *browser, void *entry, int
 			     (!current_entry || (browser->use_navkeypressed &&
 					         !browser->navkeypressed)));
 	int width = browser->width, printed;
+	int i, pcnt_width = 7 * ab->nr_events;
+	double percent_max = 0.0;
 	char bf[256];
 
-	if (dl->offset != -1 && bdl->percent != 0.0) {
-		ui_browser__set_percent_color(browser, bdl->percent, current_entry);
-		slsmg_printf("%6.2f ", bdl->percent);
+	for (i = 0; i < ab->nr_events; i++) {
+		if (bdl->percent[i] > percent_max)
+			percent_max = bdl->percent[i];
+	}
+
+	if (dl->offset != -1 && percent_max != 0.0) {
+		for (i = 0; i < ab->nr_events; i++) {
+			ui_browser__set_percent_color(browser, bdl->percent[i],
+						      current_entry);
+			slsmg_printf("%6.2f ", bdl->percent[i]);
+		}
 	} else {
 		ui_browser__set_percent_color(browser, 0, current_entry);
-		slsmg_write_nstring(" ", 7);
+		slsmg_write_nstring(" ", pcnt_width);
 	}
 
 	SLsmg_write_char(' ');
@@ -111,12 +126,12 @@ static void annotate_browser__write(struct ui_browser *browser, void *entry, int
 		width += 1;
 
 	if (!*dl->line)
-		slsmg_write_nstring(" ", width - 7);
+		slsmg_write_nstring(" ", width - pcnt_width);
 	else if (dl->offset == -1) {
 		printed = scnprintf(bf, sizeof(bf), "%*s  ",
 				    ab->addr_width, " ");
 		slsmg_write_nstring(bf, printed);
-		slsmg_write_nstring(dl->line, width - printed - 6);
+		slsmg_write_nstring(dl->line, width - printed - pcnt_width + 1);
 	} else {
 		u64 addr = dl->offset;
 		int color = -1;
@@ -175,7 +190,7 @@ static void annotate_browser__write(struct ui_browser *browser, void *entry, int
 		}
 
 		disasm_line__scnprintf(dl, bf, sizeof(bf), !annotate_browser__opts.use_offset);
-		slsmg_write_nstring(bf, width - 10 - printed);
+		slsmg_write_nstring(bf, width - pcnt_width - 3 - printed);
 	}
 
 	if (current_entry)
@@ -200,6 +215,7 @@ static void annotate_browser__draw_current_jump(struct ui_browser *browser)
 	unsigned int from, to;
 	struct map_symbol *ms = ab->b.priv;
 	struct symbol *sym = ms->sym;
+	u8 pcnt_width = 7;
 
 	/* PLT symbols contain external offsets */
 	if (strstr(sym->name, "@plt"))
@@ -223,57 +239,44 @@ static void annotate_browser__draw_current_jump(struct ui_browser *browser)
 		to = (u64)btarget->idx;
 	}
 
+	pcnt_width *= ab->nr_events;
+
 	ui_browser__set_color(browser, HE_COLORSET_CODE);
-	__ui_browser__line_arrow(browser, 9 + ab->addr_width, from, to);
+	__ui_browser__line_arrow(browser, pcnt_width + 2 + ab->addr_width,
+				 from, to);
 }
 
 static unsigned int annotate_browser__refresh(struct ui_browser *browser)
 {
+	struct annotate_browser *ab = container_of(browser, struct annotate_browser, b);
 	int ret = ui_browser__list_head_refresh(browser);
+	int pcnt_width;
+
+	pcnt_width = 7 * ab->nr_events;
 
 	if (annotate_browser__opts.jump_arrows)
 		annotate_browser__draw_current_jump(browser);
 
 	ui_browser__set_color(browser, HE_COLORSET_NORMAL);
-	__ui_browser__vline(browser, 7, 0, browser->height - 1);
+	__ui_browser__vline(browser, pcnt_width, 0, browser->height - 1);
 	return ret;
 }
 
-static double disasm_line__calc_percent(struct disasm_line *dl, struct symbol *sym, int evidx)
+static int disasm__cmp(struct browser_disasm_line *a,
+		       struct browser_disasm_line *b, int nr_pcnt)
 {
-	double percent = 0.0;
-
-	if (dl->offset != -1) {
-		int len = sym->end - sym->start;
-		unsigned int hits = 0;
-		struct annotation *notes = symbol__annotation(sym);
-		struct source_line *src_line = notes->src->lines;
-		struct sym_hist *h = annotation__histogram(notes, evidx);
-		s64 offset = dl->offset;
-		struct disasm_line *next;
+	int i;
 
-		next = disasm__get_next_ip_line(&notes->src->source, dl);
-		while (offset < (s64)len &&
-		       (next == NULL || offset < next->offset)) {
-			if (src_line) {
-				percent += src_line[offset].percent;
-			} else
-				hits += h->addr[offset];
-
-			++offset;
-		}
-		/*
- 		 * If the percentage wasn't already calculated in
- 		 * symbol__get_source_line, do it now:
- 		 */
-		if (src_line == NULL && h->sum)
-			percent = 100.0 * hits / h->sum;
+	for (i = 0; i < nr_pcnt; i++) {
+		if (a->percent[i] == b->percent[i])
+			continue;
+		return a->percent[i] < b->percent[i];
 	}
-
-	return percent;
+	return 0;
 }
 
-static void disasm_rb_tree__insert(struct rb_root *root, struct browser_disasm_line *bdl)
+static void disasm_rb_tree__insert(struct rb_root *root, struct browser_disasm_line *bdl,
+				   int nr_events)
 {
 	struct rb_node **p = &root->rb_node;
 	struct rb_node *parent = NULL;
@@ -282,7 +285,8 @@ static void disasm_rb_tree__insert(struct rb_root *root, struct browser_disasm_l
 	while (*p != NULL) {
 		parent = *p;
 		l = rb_entry(parent, struct browser_disasm_line, rb_node);
-		if (bdl->percent < l->percent)
+
+		if (disasm__cmp(bdl, l, nr_events))
 			p = &(*p)->rb_left;
 		else
 			p = &(*p)->rb_right;
@@ -331,12 +335,13 @@ static void annotate_browser__set_rb_top(struct annotate_browser *browser,
 }
 
 static void annotate_browser__calc_percent(struct annotate_browser *browser,
-					   int evidx)
+					   struct perf_evsel *evsel)
 {
 	struct map_symbol *ms = browser->b.priv;
 	struct symbol *sym = ms->sym;
 	struct annotation *notes = symbol__annotation(sym);
-	struct disasm_line *pos;
+	struct disasm_line *pos, *next;
+	s64 len = symbol__size(sym);
 
 	browser->entries = RB_ROOT;
 
@@ -344,12 +349,34 @@ static void annotate_browser__calc_percent(struct annotate_browser *browser,
 
 	list_for_each_entry(pos, &notes->src->source, node) {
 		struct browser_disasm_line *bpos = disasm_line__browser(pos);
-		bpos->percent = disasm_line__calc_percent(pos, sym, evidx);
-		if (bpos->percent < 0.01) {
+		const char *path = NULL;
+		double max_percent = 0.0;
+		int i;
+
+		if (pos->offset == -1) {
+			RB_CLEAR_NODE(&bpos->rb_node);
+			continue;
+		}
+
+		next = disasm__get_next_ip_line(&notes->src->source, pos);
+
+		for (i = 0; i < browser->nr_events; i++) {
+			bpos->percent[i] = disasm__calc_percent(notes,
+						evsel->idx + i,
+						pos->offset,
+						next ? next->offset : len,
+					        &path);
+
+			if (max_percent < bpos->percent[i])
+				max_percent = bpos->percent[i];
+		}
+
+		if (max_percent < 0.01) {
 			RB_CLEAR_NODE(&bpos->rb_node);
 			continue;
 		}
-		disasm_rb_tree__insert(&browser->entries, bpos);
+		disasm_rb_tree__insert(&browser->entries, bpos,
+				       browser->nr_events);
 	}
 	pthread_mutex_unlock(&notes->lock);
 
@@ -401,7 +428,8 @@ static void annotate_browser__init_asm_mode(struct annotate_browser *browser)
 	browser->b.nr_entries = browser->nr_asm_entries;
 }
 
-static bool annotate_browser__callq(struct annotate_browser *browser, int evidx,
+static bool annotate_browser__callq(struct annotate_browser *browser,
+				    struct perf_evsel *evsel,
 				    struct hist_browser_timer *hbt)
 {
 	struct map_symbol *ms = browser->b.priv;
@@ -432,7 +460,7 @@ static bool annotate_browser__callq(struct annotate_browser *browser, int evidx,
 	}
 
 	pthread_mutex_unlock(&notes->lock);
-	symbol__tui_annotate(target, ms->map, evidx, hbt);
+	symbol__tui_annotate(target, ms->map, evsel, hbt);
 	ui_browser__show_title(&browser->b, sym->name);
 	return true;
 }
@@ -615,7 +643,8 @@ static void annotate_browser__update_addr_width(struct annotate_browser *browser
 		browser->addr_width += browser->jumps_width + 1;
 }
 
-static int annotate_browser__run(struct annotate_browser *browser, int evidx,
+static int annotate_browser__run(struct annotate_browser *browser,
+				 struct perf_evsel *evsel,
 				 struct hist_browser_timer *hbt)
 {
 	struct rb_node *nd = NULL;
@@ -628,7 +657,7 @@ static int annotate_browser__run(struct annotate_browser *browser, int evidx,
 	if (ui_browser__show(&browser->b, sym->name, help) < 0)
 		return -1;
 
-	annotate_browser__calc_percent(browser, evidx);
+	annotate_browser__calc_percent(browser, evsel);
 
 	if (browser->curr_hot) {
 		annotate_browser__set_rb_top(browser, browser->curr_hot);
@@ -641,7 +670,7 @@ static int annotate_browser__run(struct annotate_browser *browser, int evidx,
 		key = ui_browser__run(&browser->b, delay_secs);
 
 		if (delay_secs != 0) {
-			annotate_browser__calc_percent(browser, evidx);
+			annotate_browser__calc_percent(browser, evsel);
 			/*
 			 * Current line focus got out of the list of most active
 			 * lines, NULL it so that if TAB|UNTAB is pressed, we
@@ -657,7 +686,7 @@ static int annotate_browser__run(struct annotate_browser *browser, int evidx,
 				hbt->timer(hbt->arg);
 
 			if (delay_secs != 0)
-				symbol__annotate_decay_histogram(sym, evidx);
+				symbol__annotate_decay_histogram(sym, evsel->idx);
 			continue;
 		case K_TAB:
 			if (nd != NULL) {
@@ -754,7 +783,7 @@ show_help:
 					goto show_sup_ins;
 				goto out;
 			} else if (!(annotate_browser__jump(browser) ||
-				     annotate_browser__callq(browser, evidx, hbt))) {
+				     annotate_browser__callq(browser, evsel, hbt))) {
 show_sup_ins:
 				ui_helpline__puts("Actions are only available for 'callq', 'retq' & jump instructions.");
 			}
@@ -776,10 +805,10 @@ out:
 	return key;
 }
 
-int hist_entry__tui_annotate(struct hist_entry *he, int evidx,
+int hist_entry__tui_annotate(struct hist_entry *he, struct perf_evsel *evsel,
 			     struct hist_browser_timer *hbt)
 {
-	return symbol__tui_annotate(he->ms.sym, he->ms.map, evidx, hbt);
+	return symbol__tui_annotate(he->ms.sym, he->ms.map, evsel, hbt);
 }
 
 static void annotate_browser__mark_jump_targets(struct annotate_browser *browser,
@@ -826,7 +855,8 @@ static inline int width_jumps(int n)
 	return 1;
 }
 
-int symbol__tui_annotate(struct symbol *sym, struct map *map, int evidx,
+int symbol__tui_annotate(struct symbol *sym, struct map *map,
+			 struct perf_evsel *evsel,
 			 struct hist_browser_timer *hbt)
 {
 	struct disasm_line *pos, *n;
@@ -847,6 +877,8 @@ int symbol__tui_annotate(struct symbol *sym, struct map *map, int evidx,
 		},
 	};
 	int ret = -1;
+	int nr_pcnt = 1;
+	size_t sizeof_bdl = sizeof(struct browser_disasm_line);
 
 	if (sym == NULL)
 		return -1;
@@ -862,7 +894,12 @@ int symbol__tui_annotate(struct symbol *sym, struct map *map, int evidx,
 		return -1;
 	}
 
-	if (symbol__annotate(sym, map, sizeof(struct browser_disasm_line)) < 0) {
+	if (perf_evsel__is_group_event(evsel)) {
+		nr_pcnt = evsel->nr_members;
+		sizeof_bdl += sizeof(double) * (nr_pcnt - 1);
+	}
+
+	if (symbol__annotate(sym, map, sizeof_bdl) < 0) {
 		ui__error("%s", ui_helpline__last_msg);
 		goto out_free_offsets;
 	}
@@ -900,6 +937,7 @@ int symbol__tui_annotate(struct symbol *sym, struct map *map, int evidx,
 	browser.addr_width = browser.target_width = browser.min_addr_width = hex_width(size);
 	browser.max_addr_width = hex_width(sym->end);
 	browser.jumps_width = width_jumps(browser.max_jump_sources);
+	browser.nr_events = nr_pcnt;
 	browser.b.nr_entries = browser.nr_entries;
 	browser.b.entries = &notes->src->source,
 	browser.b.width += 18; /* Percentage */
@@ -909,7 +947,7 @@ int symbol__tui_annotate(struct symbol *sym, struct map *map, int evidx,
 
 	annotate_browser__update_addr_width(&browser);
 
-	ret = annotate_browser__run(&browser, evidx, hbt);
+	ret = annotate_browser__run(&browser, evsel, hbt);
 	list_for_each_entry_safe(pos, n, &notes->src->source, node) {
 		list_del(&pos->node);
 		disasm_line__free(pos);
diff --git a/tools/perf/ui/browsers/hists.c b/tools/perf/ui/browsers/hists.c
index aa22704047d..d88a2d0acb6 100644
--- a/tools/perf/ui/browsers/hists.c
+++ b/tools/perf/ui/browsers/hists.c
@@ -2,7 +2,6 @@
 #include "../libslang.h"
 #include <stdlib.h>
 #include <string.h>
-#include <newt.h>
 #include <linux/rbtree.h>
 
 #include "../../util/evsel.h"
@@ -1193,7 +1192,7 @@ static int hists__browser_title(struct hists *hists, char *bf, size_t size,
 	char buf[512];
 	size_t buflen = sizeof(buf);
 
-	if (symbol_conf.event_group && evsel->nr_members > 1) {
+	if (perf_evsel__is_group_event(evsel)) {
 		struct perf_evsel *pos;
 
 		perf_evsel__group_desc(evsel, buf, buflen);
@@ -1599,7 +1598,7 @@ do_annotate:
 			 * Don't let this be freed, say, by hists__decay_entry.
 			 */
 			he->used = true;
-			err = hist_entry__tui_annotate(he, evsel->idx, hbt);
+			err = hist_entry__tui_annotate(he, evsel, hbt);
 			he->used = false;
 			/*
 			 * offer option to annotate the other branch source or target
@@ -1709,7 +1708,7 @@ static void perf_evsel_menu__write(struct ui_browser *browser,
 	ui_browser__set_color(browser, current_entry ? HE_COLORSET_SELECTED :
 						       HE_COLORSET_NORMAL);
 
-	if (symbol_conf.event_group && evsel->nr_members > 1) {
+	if (perf_evsel__is_group_event(evsel)) {
 		struct perf_evsel *pos;
 
 		ev_name = perf_evsel__group_name(evsel);
diff --git a/tools/perf/ui/browsers/map.c b/tools/perf/ui/browsers/map.c
index 98851d55a53..95c7cfb8f2c 100644
--- a/tools/perf/ui/browsers/map.c
+++ b/tools/perf/ui/browsers/map.c
@@ -1,6 +1,5 @@
 #include "../libslang.h"
 #include <elf.h>
-#include <newt.h>
 #include <inttypes.h>
 #include <sys/ttydefaults.h>
 #include <string.h>
@@ -10,41 +9,9 @@
 #include "../../util/symbol.h"
 #include "../browser.h"
 #include "../helpline.h"
+#include "../keysyms.h"
 #include "map.h"
 
-static int ui_entry__read(const char *title, char *bf, size_t size, int width)
-{
-	struct newtExitStruct es;
-	newtComponent form, entry;
-	const char *result;
-	int err = -1;
-
-	newtCenteredWindow(width, 1, title);
-	form = newtForm(NULL, NULL, 0);
-	if (form == NULL)
-		return -1;
-
-	entry = newtEntry(0, 0, "0x", width, &result, NEWT_FLAG_SCROLL);
-	if (entry == NULL)
-		goto out_free_form;
-
-	newtFormAddComponent(form, entry);
-	newtFormAddHotKey(form, NEWT_KEY_ENTER);
-	newtFormAddHotKey(form, NEWT_KEY_ESCAPE);
-	newtFormAddHotKey(form, NEWT_KEY_LEFT);
-	newtFormAddHotKey(form, CTRL('c'));
-	newtFormRun(form, &es);
-
-	if (result != NULL) {
-		strncpy(bf, result, size);
-		err = 0;
-	}
-out_free_form:
-	newtPopWindow();
-	newtFormDestroy(form);
-	return err;
-}
-
 struct map_browser {
 	struct ui_browser b;
 	struct map	  *map;
@@ -78,10 +45,11 @@ static int map_browser__search(struct map_browser *self)
 {
 	char target[512];
 	struct symbol *sym;
-	int err = ui_entry__read("Search by name/addr", target, sizeof(target), 40);
-
-	if (err)
-		return err;
+	int err = ui_browser__input_window("Search by name/addr",
+					   "Prefix with 0x to search by address",
+					   target, "ENTER: OK, ESC: Cancel", 0);
+	if (err != K_ENTER)
+		return -1;
 
 	if (target[0] == '0' && tolower(target[1]) == 'x') {
 		u64 addr = strtoull(target, NULL, 16);
@@ -112,12 +80,20 @@ static int map_browser__run(struct map_browser *self)
 	while (1) {
 		key = ui_browser__run(&self->b, 0);
 
-		if (verbose && key == '/')
-			map_browser__search(self);
-		else
+		switch (key) {
+		case '/':
+			if (verbose)
+				map_browser__search(self);
+		default:
 			break;
+                case K_LEFT:
+                case K_ESC:
+                case 'q':
+                case CTRL('c'):
+                        goto out;
+		}
 	}
-
+out:
 	ui_browser__hide(&self->b);
 	return key;
 }
diff --git a/tools/perf/ui/browsers/scripts.c b/tools/perf/ui/browsers/scripts.c
index cbbd44b0d93..12f009e61e9 100644
--- a/tools/perf/ui/browsers/scripts.c
+++ b/tools/perf/ui/browsers/scripts.c
@@ -1,5 +1,4 @@
 #include <elf.h>
-#include <newt.h>
 #include <inttypes.h>
 #include <sys/ttydefaults.h>
 #include <string.h>
diff --git a/tools/perf/ui/gtk/annotate.c b/tools/perf/ui/gtk/annotate.c
index 7d8dc581a54..f538794615d 100644
--- a/tools/perf/ui/gtk/annotate.c
+++ b/tools/perf/ui/gtk/annotate.c
@@ -1,6 +1,7 @@
 #include "gtk.h"
 #include "util/debug.h"
 #include "util/annotate.h"
+#include "util/evsel.h"
 #include "ui/helpline.h"
 
 
@@ -32,7 +33,7 @@ static int perf_gtk__get_percent(char *buf, size_t size, struct symbol *sym,
 		return 0;
 
 	symhist = annotation__histogram(symbol__annotation(sym), evidx);
-	if (!symhist->addr[dl->offset])
+	if (!symbol_conf.event_group && !symhist->addr[dl->offset])
 		return 0;
 
 	percent = 100.0 * symhist->addr[dl->offset] / symhist->sum;
@@ -85,7 +86,7 @@ static int perf_gtk__get_line(char *buf, size_t size, struct disasm_line *dl)
 }
 
 static int perf_gtk__annotate_symbol(GtkWidget *window, struct symbol *sym,
-				struct map *map, int evidx,
+				struct map *map, struct perf_evsel *evsel,
 				struct hist_browser_timer *hbt __maybe_unused)
 {
 	struct disasm_line *pos, *n;
@@ -118,10 +119,24 @@ static int perf_gtk__annotate_symbol(GtkWidget *window, struct symbol *sym,
 
 	list_for_each_entry(pos, &notes->src->source, node) {
 		GtkTreeIter iter;
+		int ret = 0;
 
 		gtk_list_store_append(store, &iter);
 
-		if (perf_gtk__get_percent(s, sizeof(s), sym, pos, evidx))
+		if (perf_evsel__is_group_event(evsel)) {
+			for (i = 0; i < evsel->nr_members; i++) {
+				ret += perf_gtk__get_percent(s + ret,
+							     sizeof(s) - ret,
+							     sym, pos,
+							     evsel->idx + i);
+				ret += scnprintf(s + ret, sizeof(s) - ret, " ");
+			}
+		} else {
+			ret = perf_gtk__get_percent(s, sizeof(s), sym, pos,
+						    evsel->idx);
+		}
+
+		if (ret)
 			gtk_list_store_set(store, &iter, ANN_COL__PERCENT, s, -1);
 		if (perf_gtk__get_offset(s, sizeof(s), sym, map, pos))
 			gtk_list_store_set(store, &iter, ANN_COL__OFFSET, s, -1);
@@ -139,7 +154,8 @@ static int perf_gtk__annotate_symbol(GtkWidget *window, struct symbol *sym,
 	return 0;
 }
 
-int symbol__gtk_annotate(struct symbol *sym, struct map *map, int evidx,
+int symbol__gtk_annotate(struct symbol *sym, struct map *map,
+			 struct perf_evsel *evsel,
 			 struct hist_browser_timer *hbt)
 {
 	GtkWidget *window;
@@ -206,7 +222,7 @@ int symbol__gtk_annotate(struct symbol *sym, struct map *map, int evidx,
 	gtk_notebook_append_page(GTK_NOTEBOOK(notebook), scrolled_window,
 				 tab_label);
 
-	perf_gtk__annotate_symbol(scrolled_window, sym, map, evidx, hbt);
+	perf_gtk__annotate_symbol(scrolled_window, sym, map, evsel, hbt);
 	return 0;
 }
 
diff --git a/tools/perf/ui/gtk/hists.c b/tools/perf/ui/gtk/hists.c
index 1e764a8ad25..6f259b3d14e 100644
--- a/tools/perf/ui/gtk/hists.c
+++ b/tools/perf/ui/gtk/hists.c
@@ -32,21 +32,18 @@ static int __hpp__color_fmt(struct perf_hpp *hpp, struct hist_entry *he,
 	int ret;
 	double percent = 0.0;
 	struct hists *hists = he->hists;
+	struct perf_evsel *evsel = hists_to_evsel(hists);
 
 	if (hists->stats.total_period)
 		percent = 100.0 * get_field(he) / hists->stats.total_period;
 
 	ret = __percent_color_snprintf(hpp->buf, hpp->size, percent);
 
-	if (symbol_conf.event_group) {
+	if (perf_evsel__is_group_event(evsel)) {
 		int prev_idx, idx_delta;
-		struct perf_evsel *evsel = hists_to_evsel(hists);
 		struct hist_entry *pair;
 		int nr_members = evsel->nr_members;
 
-		if (nr_members <= 1)
-			return ret;
-
 		prev_idx = perf_evsel__group_idx(evsel);
 
 		list_for_each_entry(pair, &he->pairs.head, pairs.node) {
diff --git a/tools/perf/ui/hist.c b/tools/perf/ui/hist.c
index d671e63aa35..4bf91b09d62 100644
--- a/tools/perf/ui/hist.c
+++ b/tools/perf/ui/hist.c
@@ -16,6 +16,7 @@ static int __hpp__fmt(struct perf_hpp *hpp, struct hist_entry *he,
 {
 	int ret;
 	struct hists *hists = he->hists;
+	struct perf_evsel *evsel = hists_to_evsel(hists);
 
 	if (fmt_percent) {
 		double percent = 0.0;
@@ -28,15 +29,11 @@ static int __hpp__fmt(struct perf_hpp *hpp, struct hist_entry *he,
 	} else
 		ret = print_fn(hpp->buf, hpp->size, fmt, get_field(he));
 
-	if (symbol_conf.event_group) {
+	if (perf_evsel__is_group_event(evsel)) {
 		int prev_idx, idx_delta;
-		struct perf_evsel *evsel = hists_to_evsel(hists);
 		struct hist_entry *pair;
 		int nr_members = evsel->nr_members;
 
-		if (nr_members <= 1)
-			return ret;
-
 		prev_idx = perf_evsel__group_idx(evsel);
 
 		list_for_each_entry(pair, &he->pairs.head, pairs.node) {
diff --git a/tools/perf/ui/tui/setup.c b/tools/perf/ui/tui/setup.c
index 81efa192e86..b9401482d11 100644
--- a/tools/perf/ui/tui/setup.c
+++ b/tools/perf/ui/tui/setup.c
@@ -1,4 +1,3 @@
-#include <newt.h>
 #include <signal.h>
 #include <stdbool.h>
 
@@ -88,13 +87,6 @@ int ui__getch(int delay_secs)
 	return SLkp_getkey();
 }
 
-static void newt_suspend(void *d __maybe_unused)
-{
-	newtSuspend();
-	raise(SIGTSTP);
-	newtResume();
-}
-
 static void ui__signal(int sig)
 {
 	ui__exit(false);
@@ -106,7 +98,17 @@ int ui__init(void)
 {
 	int err;
 
-	newtInit();
+	SLutf8_enable(-1);
+	SLtt_get_terminfo();
+	SLtt_get_screen_size();
+
+	err = SLsmg_init_smg();
+	if (err < 0)
+		goto out;
+	err = SLang_init_tty(0, 0, 0);
+	if (err < 0)
+		goto out;
+
 	err = SLkp_init();
 	if (err < 0) {
 		pr_err("TUI initialization failed.\n");
@@ -115,7 +117,6 @@ int ui__init(void)
 
 	SLkp_define_keysym((char *)"^(kB)", SL_KEY_UNTAB);
 
-	newtSetSuspendCallback(newt_suspend, NULL);
 	ui_helpline__init();
 	ui_browser__init();
 	ui_progress__init();
diff --git a/tools/perf/ui/ui.h b/tools/perf/ui/ui.h
index d86359c9990..70cb0d4eb8a 100644
--- a/tools/perf/ui/ui.h
+++ b/tools/perf/ui/ui.h
@@ -12,7 +12,7 @@ extern int use_browser;
 void setup_browser(bool fallback_to_pager);
 void exit_browser(bool wait_for_ok);
 
-#ifdef NEWT_SUPPORT
+#ifdef SLANG_SUPPORT
 int ui__init(void);
 void ui__exit(bool wait_for_ok);
 #else
diff --git a/tools/perf/util/annotate.c b/tools/perf/util/annotate.c
index d33fe937e6f..d102716c43a 100644
--- a/tools/perf/util/annotate.c
+++ b/tools/perf/util/annotate.c
@@ -14,6 +14,7 @@
 #include "symbol.h"
 #include "debug.h"
 #include "annotate.h"
+#include "evsel.h"
 #include <pthread.h>
 #include <linux/bitops.h>
 
@@ -602,8 +603,42 @@ struct disasm_line *disasm__get_next_ip_line(struct list_head *head, struct disa
 	return NULL;
 }
 
+double disasm__calc_percent(struct annotation *notes, int evidx, s64 offset,
+			    s64 end, const char **path)
+{
+	struct source_line *src_line = notes->src->lines;
+	double percent = 0.0;
+
+	if (src_line) {
+		size_t sizeof_src_line = sizeof(*src_line) +
+				sizeof(src_line->p) * (src_line->nr_pcnt - 1);
+
+		while (offset < end) {
+			src_line = (void *)notes->src->lines +
+					(sizeof_src_line * offset);
+
+			if (*path == NULL)
+				*path = src_line->path;
+
+			percent += src_line->p[evidx].percent;
+			offset++;
+		}
+	} else {
+		struct sym_hist *h = annotation__histogram(notes, evidx);
+		unsigned int hits = 0;
+
+		while (offset < end)
+			hits += h->addr[offset++];
+
+		if (h->sum)
+			percent = 100.0 * hits / h->sum;
+	}
+
+	return percent;
+}
+
 static int disasm_line__print(struct disasm_line *dl, struct symbol *sym, u64 start,
-		      int evidx, u64 len, int min_pcnt, int printed,
+		      struct perf_evsel *evsel, u64 len, int min_pcnt, int printed,
 		      int max_lines, struct disasm_line *queue)
 {
 	static const char *prev_line;
@@ -611,34 +646,37 @@ static int disasm_line__print(struct disasm_line *dl, struct symbol *sym, u64 st
 
 	if (dl->offset != -1) {
 		const char *path = NULL;
-		unsigned int hits = 0;
-		double percent = 0.0;
+		double percent, max_percent = 0.0;
+		double *ppercents = &percent;
+		int i, nr_percent = 1;
 		const char *color;
 		struct annotation *notes = symbol__annotation(sym);
-		struct source_line *src_line = notes->src->lines;
-		struct sym_hist *h = annotation__histogram(notes, evidx);
 		s64 offset = dl->offset;
 		const u64 addr = start + offset;
 		struct disasm_line *next;
 
 		next = disasm__get_next_ip_line(&notes->src->source, dl);
 
-		while (offset < (s64)len &&
-		       (next == NULL || offset < next->offset)) {
-			if (src_line) {
-				if (path == NULL)
-					path = src_line[offset].path;
-				percent += src_line[offset].percent;
-			} else
-				hits += h->addr[offset];
-
-			++offset;
+		if (perf_evsel__is_group_event(evsel)) {
+			nr_percent = evsel->nr_members;
+			ppercents = calloc(nr_percent, sizeof(double));
+			if (ppercents == NULL)
+				return -1;
 		}
 
-		if (src_line == NULL && h->sum)
-			percent = 100.0 * hits / h->sum;
+		for (i = 0; i < nr_percent; i++) {
+			percent = disasm__calc_percent(notes,
+					notes->src->lines ? i : evsel->idx + i,
+					offset,
+					next ? next->offset : (s64) len,
+					&path);
+
+			ppercents[i] = percent;
+			if (percent > max_percent)
+				max_percent = percent;
+		}
 
-		if (percent < min_pcnt)
+		if (max_percent < min_pcnt)
 			return -1;
 
 		if (max_lines && printed >= max_lines)
@@ -648,12 +686,12 @@ static int disasm_line__print(struct disasm_line *dl, struct symbol *sym, u64 st
 			list_for_each_entry_from(queue, &notes->src->source, node) {
 				if (queue == dl)
 					break;
-				disasm_line__print(queue, sym, start, evidx, len,
+				disasm_line__print(queue, sym, start, evsel, len,
 						    0, 0, 1, NULL);
 			}
 		}
 
-		color = get_percent_color(percent);
+		color = get_percent_color(max_percent);
 
 		/*
 		 * Also color the filename and line if needed, with
@@ -669,25 +707,59 @@ static int disasm_line__print(struct disasm_line *dl, struct symbol *sym, u64 st
 			}
 		}
 
-		color_fprintf(stdout, color, " %7.2f", percent);
+		for (i = 0; i < nr_percent; i++) {
+			percent = ppercents[i];
+			color = get_percent_color(percent);
+			color_fprintf(stdout, color, " %7.2f", percent);
+		}
+
 		printf(" :	");
 		color_fprintf(stdout, PERF_COLOR_MAGENTA, "  %" PRIx64 ":", addr);
 		color_fprintf(stdout, PERF_COLOR_BLUE, "%s\n", dl->line);
+
+		if (ppercents != &percent)
+			free(ppercents);
+
 	} else if (max_lines && printed >= max_lines)
 		return 1;
 	else {
+		int width = 8;
+
 		if (queue)
 			return -1;
 
+		if (perf_evsel__is_group_event(evsel))
+			width *= evsel->nr_members;
+
 		if (!*dl->line)
-			printf("         :\n");
+			printf(" %*s:\n", width, " ");
 		else
-			printf("         :	%s\n", dl->line);
+			printf(" %*s:	%s\n", width, " ", dl->line);
 	}
 
 	return 0;
 }
 
+/*
+ * symbol__parse_objdump_line() parses objdump output (with -d --no-show-raw)
+ * which looks like following
+ *
+ *  0000000000415500 <_init>:
+ *    415500:       sub    $0x8,%rsp
+ *    415504:       mov    0x2f5ad5(%rip),%rax        # 70afe0 <_DYNAMIC+0x2f8>
+ *    41550b:       test   %rax,%rax
+ *    41550e:       je     415515 <_init+0x15>
+ *    415510:       callq  416e70 <__gmon_start__@plt>
+ *    415515:       add    $0x8,%rsp
+ *    415519:       retq
+ *
+ * it will be parsed and saved into struct disasm_line as
+ *  <offset>       <name>  <ops.raw>
+ *
+ * The offset will be a relative offset from the start of the symbol and -1
+ * means that it's not a disassembly line so should be treated differently.
+ * The ops.raw part will be parsed further according to type of the instruction.
+ */
 static int symbol__parse_objdump_line(struct symbol *sym, struct map *map,
 				      FILE *file, size_t privsize)
 {
@@ -858,7 +930,7 @@ static void insert_source_line(struct rb_root *root, struct source_line *src_lin
 	struct source_line *iter;
 	struct rb_node **p = &root->rb_node;
 	struct rb_node *parent = NULL;
-	int ret;
+	int i, ret;
 
 	while (*p != NULL) {
 		parent = *p;
@@ -866,7 +938,8 @@ static void insert_source_line(struct rb_root *root, struct source_line *src_lin
 
 		ret = strcmp(iter->path, src_line->path);
 		if (ret == 0) {
-			iter->percent_sum += src_line->percent;
+			for (i = 0; i < src_line->nr_pcnt; i++)
+				iter->p[i].percent_sum += src_line->p[i].percent;
 			return;
 		}
 
@@ -876,12 +949,26 @@ static void insert_source_line(struct rb_root *root, struct source_line *src_lin
 			p = &(*p)->rb_right;
 	}
 
-	src_line->percent_sum = src_line->percent;
+	for (i = 0; i < src_line->nr_pcnt; i++)
+		src_line->p[i].percent_sum = src_line->p[i].percent;
 
 	rb_link_node(&src_line->node, parent, p);
 	rb_insert_color(&src_line->node, root);
 }
 
+static int cmp_source_line(struct source_line *a, struct source_line *b)
+{
+	int i;
+
+	for (i = 0; i < a->nr_pcnt; i++) {
+		if (a->p[i].percent_sum == b->p[i].percent_sum)
+			continue;
+		return a->p[i].percent_sum > b->p[i].percent_sum;
+	}
+
+	return 0;
+}
+
 static void __resort_source_line(struct rb_root *root, struct source_line *src_line)
 {
 	struct source_line *iter;
@@ -892,7 +979,7 @@ static void __resort_source_line(struct rb_root *root, struct source_line *src_l
 		parent = *p;
 		iter = rb_entry(parent, struct source_line, node);
 
-		if (src_line->percent_sum > iter->percent_sum)
+		if (cmp_source_line(src_line, iter))
 			p = &(*p)->rb_left;
 		else
 			p = &(*p)->rb_right;
@@ -924,32 +1011,52 @@ static void symbol__free_source_line(struct symbol *sym, int len)
 {
 	struct annotation *notes = symbol__annotation(sym);
 	struct source_line *src_line = notes->src->lines;
+	size_t sizeof_src_line;
 	int i;
 
-	for (i = 0; i < len; i++)
-		free(src_line[i].path);
+	sizeof_src_line = sizeof(*src_line) +
+			  (sizeof(src_line->p) * (src_line->nr_pcnt - 1));
+
+	for (i = 0; i < len; i++) {
+		free(src_line->path);
+		src_line = (void *)src_line + sizeof_src_line;
+	}
 
-	free(src_line);
+	free(notes->src->lines);
 	notes->src->lines = NULL;
 }
 
 /* Get the filename:line for the colored entries */
 static int symbol__get_source_line(struct symbol *sym, struct map *map,
-				   int evidx, struct rb_root *root, int len,
+				   struct perf_evsel *evsel,
+				   struct rb_root *root, int len,
 				   const char *filename)
 {
 	u64 start;
-	int i;
+	int i, k;
+	int evidx = evsel->idx;
 	char cmd[PATH_MAX * 2];
 	struct source_line *src_line;
 	struct annotation *notes = symbol__annotation(sym);
 	struct sym_hist *h = annotation__histogram(notes, evidx);
 	struct rb_root tmp_root = RB_ROOT;
+	int nr_pcnt = 1;
+	u64 h_sum = h->sum;
+	size_t sizeof_src_line = sizeof(struct source_line);
+
+	if (perf_evsel__is_group_event(evsel)) {
+		for (i = 1; i < evsel->nr_members; i++) {
+			h = annotation__histogram(notes, evidx + i);
+			h_sum += h->sum;
+		}
+		nr_pcnt = evsel->nr_members;
+		sizeof_src_line += (nr_pcnt - 1) * sizeof(src_line->p);
+	}
 
-	if (!h->sum)
+	if (!h_sum)
 		return 0;
 
-	src_line = notes->src->lines = calloc(len, sizeof(struct source_line));
+	src_line = notes->src->lines = calloc(len, sizeof_src_line);
 	if (!notes->src->lines)
 		return -1;
 
@@ -960,29 +1067,41 @@ static int symbol__get_source_line(struct symbol *sym, struct map *map,
 		size_t line_len;
 		u64 offset;
 		FILE *fp;
+		double percent_max = 0.0;
 
-		src_line[i].percent = 100.0 * h->addr[i] / h->sum;
-		if (src_line[i].percent <= 0.5)
-			continue;
+		src_line->nr_pcnt = nr_pcnt;
+
+		for (k = 0; k < nr_pcnt; k++) {
+			h = annotation__histogram(notes, evidx + k);
+			src_line->p[k].percent = 100.0 * h->addr[i] / h->sum;
+
+			if (src_line->p[k].percent > percent_max)
+				percent_max = src_line->p[k].percent;
+		}
+
+		if (percent_max <= 0.5)
+			goto next;
 
 		offset = start + i;
 		sprintf(cmd, "addr2line -e %s %016" PRIx64, filename, offset);
 		fp = popen(cmd, "r");
 		if (!fp)
-			continue;
+			goto next;
 
 		if (getline(&path, &line_len, fp) < 0 || !line_len)
-			goto next;
+			goto next_close;
 
-		src_line[i].path = malloc(sizeof(char) * line_len + 1);
-		if (!src_line[i].path)
-			goto next;
+		src_line->path = malloc(sizeof(char) * line_len + 1);
+		if (!src_line->path)
+			goto next_close;
 
-		strcpy(src_line[i].path, path);
-		insert_source_line(&tmp_root, &src_line[i]);
+		strcpy(src_line->path, path);
+		insert_source_line(&tmp_root, src_line);
 
-	next:
+	next_close:
 		pclose(fp);
+	next:
+		src_line = (void *)src_line + sizeof_src_line;
 	}
 
 	resort_source_line(root, &tmp_root);
@@ -1004,24 +1123,33 @@ static void print_summary(struct rb_root *root, const char *filename)
 
 	node = rb_first(root);
 	while (node) {
-		double percent;
+		double percent, percent_max = 0.0;
 		const char *color;
 		char *path;
+		int i;
 
 		src_line = rb_entry(node, struct source_line, node);
-		percent = src_line->percent_sum;
-		color = get_percent_color(percent);
+		for (i = 0; i < src_line->nr_pcnt; i++) {
+			percent = src_line->p[i].percent_sum;
+			color = get_percent_color(percent);
+			color_fprintf(stdout, color, " %7.2f", percent);
+
+			if (percent > percent_max)
+				percent_max = percent;
+		}
+
 		path = src_line->path;
+		color = get_percent_color(percent_max);
+		color_fprintf(stdout, color, " %s", path);
 
-		color_fprintf(stdout, color, " %7.2f %s", percent, path);
 		node = rb_next(node);
 	}
 }
 
-static void symbol__annotate_hits(struct symbol *sym, int evidx)
+static void symbol__annotate_hits(struct symbol *sym, struct perf_evsel *evsel)
 {
 	struct annotation *notes = symbol__annotation(sym);
-	struct sym_hist *h = annotation__histogram(notes, evidx);
+	struct sym_hist *h = annotation__histogram(notes, evsel->idx);
 	u64 len = symbol__size(sym), offset;
 
 	for (offset = 0; offset < len; ++offset)
@@ -1031,9 +1159,9 @@ static void symbol__annotate_hits(struct symbol *sym, int evidx)
 	printf("%*s: %" PRIu64 "\n", BITS_PER_LONG / 2, "h->sum", h->sum);
 }
 
-int symbol__annotate_printf(struct symbol *sym, struct map *map, int evidx,
-			    bool full_paths, int min_pcnt, int max_lines,
-			    int context)
+int symbol__annotate_printf(struct symbol *sym, struct map *map,
+			    struct perf_evsel *evsel, bool full_paths,
+			    int min_pcnt, int max_lines, int context)
 {
 	struct dso *dso = map->dso;
 	char *filename;
@@ -1044,6 +1172,8 @@ int symbol__annotate_printf(struct symbol *sym, struct map *map, int evidx,
 	int printed = 2, queue_len = 0;
 	int more = 0;
 	u64 len;
+	int width = 8;
+	int namelen;
 
 	filename = strdup(dso->long_name);
 	if (!filename)
@@ -1055,12 +1185,18 @@ int symbol__annotate_printf(struct symbol *sym, struct map *map, int evidx,
 		d_filename = basename(filename);
 
 	len = symbol__size(sym);
+	namelen = strlen(d_filename);
+
+	if (perf_evsel__is_group_event(evsel))
+		width *= evsel->nr_members;
 
-	printf(" Percent |	Source code & Disassembly of %s\n", d_filename);
-	printf("------------------------------------------------\n");
+	printf(" %-*.*s|	Source code & Disassembly of %s\n",
+	       width, width, "Percent", d_filename);
+	printf("-%-*.*s-------------------------------------\n",
+	       width+namelen, width+namelen, graph_dotted_line);
 
 	if (verbose)
-		symbol__annotate_hits(sym, evidx);
+		symbol__annotate_hits(sym, evsel);
 
 	list_for_each_entry(pos, &notes->src->source, node) {
 		if (context && queue == NULL) {
@@ -1068,7 +1204,7 @@ int symbol__annotate_printf(struct symbol *sym, struct map *map, int evidx,
 			queue_len = 0;
 		}
 
-		switch (disasm_line__print(pos, sym, start, evidx, len,
+		switch (disasm_line__print(pos, sym, start, evsel, len,
 					    min_pcnt, printed, max_lines,
 					    queue)) {
 		case 0:
@@ -1163,9 +1299,9 @@ size_t disasm__fprintf(struct list_head *head, FILE *fp)
 	return printed;
 }
 
-int symbol__tty_annotate(struct symbol *sym, struct map *map, int evidx,
-			 bool print_lines, bool full_paths, int min_pcnt,
-			 int max_lines)
+int symbol__tty_annotate(struct symbol *sym, struct map *map,
+			 struct perf_evsel *evsel, bool print_lines,
+			 bool full_paths, int min_pcnt, int max_lines)
 {
 	struct dso *dso = map->dso;
 	const char *filename = dso->long_name;
@@ -1178,12 +1314,12 @@ int symbol__tty_annotate(struct symbol *sym, struct map *map, int evidx,
 	len = symbol__size(sym);
 
 	if (print_lines) {
-		symbol__get_source_line(sym, map, evidx, &source_line,
+		symbol__get_source_line(sym, map, evsel, &source_line,
 					len, filename);
 		print_summary(&source_line, filename);
 	}
 
-	symbol__annotate_printf(sym, map, evidx, full_paths,
+	symbol__annotate_printf(sym, map, evsel, full_paths,
 				min_pcnt, max_lines, 0);
 	if (print_lines)
 		symbol__free_source_line(sym, len);
diff --git a/tools/perf/util/annotate.h b/tools/perf/util/annotate.h
index c422440fe61..af755156d27 100644
--- a/tools/perf/util/annotate.h
+++ b/tools/perf/util/annotate.h
@@ -50,6 +50,8 @@ bool ins__is_jump(const struct ins *ins);
 bool ins__is_call(const struct ins *ins);
 int ins__scnprintf(struct ins *ins, char *bf, size_t size, struct ins_operands *ops);
 
+struct annotation;
+
 struct disasm_line {
 	struct list_head    node;
 	s64		    offset;
@@ -68,17 +70,24 @@ void disasm_line__free(struct disasm_line *dl);
 struct disasm_line *disasm__get_next_ip_line(struct list_head *head, struct disasm_line *pos);
 int disasm_line__scnprintf(struct disasm_line *dl, char *bf, size_t size, bool raw);
 size_t disasm__fprintf(struct list_head *head, FILE *fp);
+double disasm__calc_percent(struct annotation *notes, int evidx, s64 offset,
+			    s64 end, const char **path);
 
 struct sym_hist {
 	u64		sum;
 	u64		addr[0];
 };
 
-struct source_line {
-	struct rb_node	node;
+struct source_line_percent {
 	double		percent;
 	double		percent_sum;
+};
+
+struct source_line {
+	struct rb_node	node;
 	char		*path;
+	int		nr_pcnt;
+	struct source_line_percent p[1];
 };
 
 /** struct annotated_source - symbols with hits have this attached as in sannotation
@@ -130,47 +139,49 @@ void symbol__annotate_zero_histograms(struct symbol *sym);
 
 int symbol__annotate(struct symbol *sym, struct map *map, size_t privsize);
 int symbol__annotate_init(struct map *map __maybe_unused, struct symbol *sym);
-int symbol__annotate_printf(struct symbol *sym, struct map *map, int evidx,
-			    bool full_paths, int min_pcnt, int max_lines,
-			    int context);
+int symbol__annotate_printf(struct symbol *sym, struct map *map,
+			    struct perf_evsel *evsel, bool full_paths,
+			    int min_pcnt, int max_lines, int context);
 void symbol__annotate_zero_histogram(struct symbol *sym, int evidx);
 void symbol__annotate_decay_histogram(struct symbol *sym, int evidx);
 void disasm__purge(struct list_head *head);
 
-int symbol__tty_annotate(struct symbol *sym, struct map *map, int evidx,
-			 bool print_lines, bool full_paths, int min_pcnt,
-			 int max_lines);
+int symbol__tty_annotate(struct symbol *sym, struct map *map,
+			 struct perf_evsel *evsel, bool print_lines,
+			 bool full_paths, int min_pcnt, int max_lines);
 
-#ifdef NEWT_SUPPORT
-int symbol__tui_annotate(struct symbol *sym, struct map *map, int evidx,
+#ifdef SLANG_SUPPORT
+int symbol__tui_annotate(struct symbol *sym, struct map *map,
+			 struct perf_evsel *evsel,
 			 struct hist_browser_timer *hbt);
 #else
 static inline int symbol__tui_annotate(struct symbol *sym __maybe_unused,
-				       struct map *map __maybe_unused,
-				       int evidx __maybe_unused,
-				       struct hist_browser_timer *hbt
-				       __maybe_unused)
+				struct map *map __maybe_unused,
+				struct perf_evsel *evsel  __maybe_unused,
+				struct hist_browser_timer *hbt
+				__maybe_unused)
 {
 	return 0;
 }
 #endif
 
 #ifdef GTK2_SUPPORT
-int symbol__gtk_annotate(struct symbol *sym, struct map *map, int evidx,
+int symbol__gtk_annotate(struct symbol *sym, struct map *map,
+			 struct perf_evsel *evsel,
 			 struct hist_browser_timer *hbt);
 
-static inline int hist_entry__gtk_annotate(struct hist_entry *he, int evidx,
+static inline int hist_entry__gtk_annotate(struct hist_entry *he,
+					   struct perf_evsel *evsel,
 					   struct hist_browser_timer *hbt)
 {
-	return symbol__gtk_annotate(he->ms.sym, he->ms.map, evidx, hbt);
+	return symbol__gtk_annotate(he->ms.sym, he->ms.map, evsel, hbt);
 }
 
 void perf_gtk__show_annotations(void);
 #else
 static inline int hist_entry__gtk_annotate(struct hist_entry *he __maybe_unused,
-					   int evidx __maybe_unused,
-					   struct hist_browser_timer *hbt
-					   __maybe_unused)
+				struct perf_evsel *evsel __maybe_unused,
+				struct hist_browser_timer *hbt __maybe_unused)
 {
 	return 0;
 }
diff --git a/tools/perf/util/cpumap.c b/tools/perf/util/cpumap.c
index f817046e22b..beb8cf9f997 100644
--- a/tools/perf/util/cpumap.c
+++ b/tools/perf/util/cpumap.c
@@ -4,6 +4,7 @@
 #include "cpumap.h"
 #include <assert.h>
 #include <stdio.h>
+#include <stdlib.h>
 
 static struct cpu_map *cpu_map__default_new(void)
 {
@@ -219,7 +220,7 @@ int cpu_map__get_socket(struct cpu_map *map, int idx)
 	if (!mnt)
 		return -1;
 
-	sprintf(path,
+	snprintf(path, PATH_MAX,
 		"%s/devices/system/cpu/cpu%d/topology/physical_package_id",
 		mnt, cpu);
 
@@ -231,27 +232,88 @@ int cpu_map__get_socket(struct cpu_map *map, int idx)
 	return ret == 1 ? cpu : -1;
 }
 
-int cpu_map__build_socket_map(struct cpu_map *cpus, struct cpu_map **sockp)
+static int cmp_ids(const void *a, const void *b)
 {
-	struct cpu_map *sock;
+	return *(int *)a - *(int *)b;
+}
+
+static int cpu_map__build_map(struct cpu_map *cpus, struct cpu_map **res,
+			      int (*f)(struct cpu_map *map, int cpu))
+{
+	struct cpu_map *c;
 	int nr = cpus->nr;
 	int cpu, s1, s2;
 
-	sock = calloc(1, sizeof(*sock) + nr * sizeof(int));
-	if (!sock)
+	/* allocate as much as possible */
+	c = calloc(1, sizeof(*c) + nr * sizeof(int));
+	if (!c)
 		return -1;
 
 	for (cpu = 0; cpu < nr; cpu++) {
-		s1 = cpu_map__get_socket(cpus, cpu);
-		for (s2 = 0; s2 < sock->nr; s2++) {
-			if (s1 == sock->map[s2])
+		s1 = f(cpus, cpu);
+		for (s2 = 0; s2 < c->nr; s2++) {
+			if (s1 == c->map[s2])
 				break;
 		}
-		if (s2 == sock->nr) {
-			sock->map[sock->nr] = s1;
-			sock->nr++;
+		if (s2 == c->nr) {
+			c->map[c->nr] = s1;
+			c->nr++;
 		}
 	}
-	*sockp = sock;
+	/* ensure we process id in increasing order */
+	qsort(c->map, c->nr, sizeof(int), cmp_ids);
+
+	*res = c;
 	return 0;
 }
+
+int cpu_map__get_core(struct cpu_map *map, int idx)
+{
+	FILE *fp;
+	const char *mnt;
+	char path[PATH_MAX];
+	int cpu, ret, s;
+
+	if (idx > map->nr)
+		return -1;
+
+	cpu = map->map[idx];
+
+	mnt = sysfs_find_mountpoint();
+	if (!mnt)
+		return -1;
+
+	snprintf(path, PATH_MAX,
+		"%s/devices/system/cpu/cpu%d/topology/core_id",
+		mnt, cpu);
+
+	fp = fopen(path, "r");
+	if (!fp)
+		return -1;
+	ret = fscanf(fp, "%d", &cpu);
+	fclose(fp);
+	if (ret != 1)
+		return -1;
+
+	s = cpu_map__get_socket(map, idx);
+	if (s == -1)
+		return -1;
+
+	/*
+	 * encode socket in upper 16 bits
+	 * core_id is relative to socket, and
+	 * we need a global id. So we combine
+	 * socket+ core id
+	 */
+	return (s << 16) | (cpu & 0xffff);
+}
+
+int cpu_map__build_socket_map(struct cpu_map *cpus, struct cpu_map **sockp)
+{
+	return cpu_map__build_map(cpus, sockp, cpu_map__get_socket);
+}
+
+int cpu_map__build_core_map(struct cpu_map *cpus, struct cpu_map **corep)
+{
+	return cpu_map__build_map(cpus, corep, cpu_map__get_core);
+}
diff --git a/tools/perf/util/cpumap.h b/tools/perf/util/cpumap.h
index 161b00756a1..9bed02e5fb3 100644
--- a/tools/perf/util/cpumap.h
+++ b/tools/perf/util/cpumap.h
@@ -15,7 +15,9 @@ void cpu_map__delete(struct cpu_map *map);
 struct cpu_map *cpu_map__read(FILE *file);
 size_t cpu_map__fprintf(struct cpu_map *map, FILE *fp);
 int cpu_map__get_socket(struct cpu_map *map, int idx);
+int cpu_map__get_core(struct cpu_map *map, int idx);
 int cpu_map__build_socket_map(struct cpu_map *cpus, struct cpu_map **sockp);
+int cpu_map__build_core_map(struct cpu_map *cpus, struct cpu_map **corep);
 
 static inline int cpu_map__socket(struct cpu_map *sock, int s)
 {
@@ -24,6 +26,16 @@ static inline int cpu_map__socket(struct cpu_map *sock, int s)
 	return sock->map[s];
 }
 
+static inline int cpu_map__id_to_socket(int id)
+{
+	return id >> 16;
+}
+
+static inline int cpu_map__id_to_cpu(int id)
+{
+	return id & 0xffff;
+}
+
 static inline int cpu_map__nr(const struct cpu_map *map)
 {
 	return map ? map->nr : 1;
diff --git a/tools/perf/util/debugfs.h b/tools/perf/util/debugfs.h
deleted file mode 100644
index 68f3e87ec57..00000000000
--- a/tools/perf/util/debugfs.h
+++ /dev/null
@@ -1,12 +0,0 @@
-#ifndef __DEBUGFS_H__
-#define __DEBUGFS_H__
-
-const char *debugfs_find_mountpoint(void);
-int debugfs_valid_mountpoint(const char *debugfs);
-char *debugfs_mount(const char *mountpoint);
-void debugfs_set_path(const char *mountpoint);
-
-extern char debugfs_mountpoint[];
-extern char tracing_events_path[];
-
-#endif /* __DEBUGFS_H__ */
diff --git a/tools/perf/util/event.h b/tools/perf/util/event.h
index 0d573ff4771..181389535c0 100644
--- a/tools/perf/util/event.h
+++ b/tools/perf/util/event.h
@@ -88,8 +88,10 @@ struct perf_sample {
 	u64 id;
 	u64 stream_id;
 	u64 period;
+	u64 weight;
 	u32 cpu;
 	u32 raw_size;
+	u64 data_src;
 	void *raw_data;
 	struct ip_callchain *callchain;
 	struct branch_stack *branch_stack;
@@ -97,6 +99,13 @@ struct perf_sample {
 	struct stack_dump user_stack;
 };
 
+#define PERF_MEM_DATA_SRC_NONE \
+	(PERF_MEM_S(OP, NA) |\
+	 PERF_MEM_S(LVL, NA) |\
+	 PERF_MEM_S(SNOOP, NA) |\
+	 PERF_MEM_S(LOCK, NA) |\
+	 PERF_MEM_S(TLB, NA))
+
 struct build_id_event {
 	struct perf_event_header header;
 	pid_t			 pid;
diff --git a/tools/perf/util/evlist.c b/tools/perf/util/evlist.c
index c8be0fbc514..f7c727801aa 100644
--- a/tools/perf/util/evlist.c
+++ b/tools/perf/util/evlist.c
@@ -7,7 +7,7 @@
  * Released under the GPL v2. (and only v2, not any later version)
  */
 #include "util.h"
-#include "debugfs.h"
+#include <lk/debugfs.h>
 #include <poll.h>
 #include "cpumap.h"
 #include "thread_map.h"
@@ -38,13 +38,12 @@ void perf_evlist__init(struct perf_evlist *evlist, struct cpu_map *cpus,
 	evlist->workload.pid = -1;
 }
 
-struct perf_evlist *perf_evlist__new(struct cpu_map *cpus,
-				     struct thread_map *threads)
+struct perf_evlist *perf_evlist__new(void)
 {
 	struct perf_evlist *evlist = zalloc(sizeof(*evlist));
 
 	if (evlist != NULL)
-		perf_evlist__init(evlist, cpus, threads);
+		perf_evlist__init(evlist, NULL, NULL);
 
 	return evlist;
 }
@@ -228,12 +227,14 @@ void perf_evlist__disable(struct perf_evlist *evlist)
 {
 	int cpu, thread;
 	struct perf_evsel *pos;
+	int nr_cpus = cpu_map__nr(evlist->cpus);
+	int nr_threads = thread_map__nr(evlist->threads);
 
-	for (cpu = 0; cpu < evlist->cpus->nr; cpu++) {
+	for (cpu = 0; cpu < nr_cpus; cpu++) {
 		list_for_each_entry(pos, &evlist->entries, node) {
 			if (!perf_evsel__is_group_leader(pos))
 				continue;
-			for (thread = 0; thread < evlist->threads->nr; thread++)
+			for (thread = 0; thread < nr_threads; thread++)
 				ioctl(FD(pos, cpu, thread),
 				      PERF_EVENT_IOC_DISABLE, 0);
 		}
@@ -244,12 +245,14 @@ void perf_evlist__enable(struct perf_evlist *evlist)
 {
 	int cpu, thread;
 	struct perf_evsel *pos;
+	int nr_cpus = cpu_map__nr(evlist->cpus);
+	int nr_threads = thread_map__nr(evlist->threads);
 
-	for (cpu = 0; cpu < cpu_map__nr(evlist->cpus); cpu++) {
+	for (cpu = 0; cpu < nr_cpus; cpu++) {
 		list_for_each_entry(pos, &evlist->entries, node) {
 			if (!perf_evsel__is_group_leader(pos))
 				continue;
-			for (thread = 0; thread < evlist->threads->nr; thread++)
+			for (thread = 0; thread < nr_threads; thread++)
 				ioctl(FD(pos, cpu, thread),
 				      PERF_EVENT_IOC_ENABLE, 0);
 		}
@@ -258,7 +261,9 @@ void perf_evlist__enable(struct perf_evlist *evlist)
 
 static int perf_evlist__alloc_pollfd(struct perf_evlist *evlist)
 {
-	int nfds = cpu_map__nr(evlist->cpus) * evlist->threads->nr * evlist->nr_entries;
+	int nr_cpus = cpu_map__nr(evlist->cpus);
+	int nr_threads = thread_map__nr(evlist->threads);
+	int nfds = nr_cpus * nr_threads * evlist->nr_entries;
 	evlist->pollfd = malloc(sizeof(struct pollfd) * nfds);
 	return evlist->pollfd != NULL ? 0 : -ENOMEM;
 }
@@ -417,7 +422,7 @@ static int perf_evlist__alloc_mmap(struct perf_evlist *evlist)
 {
 	evlist->nr_mmaps = cpu_map__nr(evlist->cpus);
 	if (cpu_map__all(evlist->cpus))
-		evlist->nr_mmaps = evlist->threads->nr;
+		evlist->nr_mmaps = thread_map__nr(evlist->threads);
 	evlist->mmap = zalloc(evlist->nr_mmaps * sizeof(struct perf_mmap));
 	return evlist->mmap != NULL ? 0 : -ENOMEM;
 }
@@ -442,11 +447,13 @@ static int perf_evlist__mmap_per_cpu(struct perf_evlist *evlist, int prot, int m
 {
 	struct perf_evsel *evsel;
 	int cpu, thread;
+	int nr_cpus = cpu_map__nr(evlist->cpus);
+	int nr_threads = thread_map__nr(evlist->threads);
 
-	for (cpu = 0; cpu < evlist->cpus->nr; cpu++) {
+	for (cpu = 0; cpu < nr_cpus; cpu++) {
 		int output = -1;
 
-		for (thread = 0; thread < evlist->threads->nr; thread++) {
+		for (thread = 0; thread < nr_threads; thread++) {
 			list_for_each_entry(evsel, &evlist->entries, node) {
 				int fd = FD(evsel, cpu, thread);
 
@@ -470,7 +477,7 @@ static int perf_evlist__mmap_per_cpu(struct perf_evlist *evlist, int prot, int m
 	return 0;
 
 out_unmap:
-	for (cpu = 0; cpu < evlist->cpus->nr; cpu++) {
+	for (cpu = 0; cpu < nr_cpus; cpu++) {
 		if (evlist->mmap[cpu].base != NULL) {
 			munmap(evlist->mmap[cpu].base, evlist->mmap_len);
 			evlist->mmap[cpu].base = NULL;
@@ -483,8 +490,9 @@ static int perf_evlist__mmap_per_thread(struct perf_evlist *evlist, int prot, in
 {
 	struct perf_evsel *evsel;
 	int thread;
+	int nr_threads = thread_map__nr(evlist->threads);
 
-	for (thread = 0; thread < evlist->threads->nr; thread++) {
+	for (thread = 0; thread < nr_threads; thread++) {
 		int output = -1;
 
 		list_for_each_entry(evsel, &evlist->entries, node) {
@@ -509,7 +517,7 @@ static int perf_evlist__mmap_per_thread(struct perf_evlist *evlist, int prot, in
 	return 0;
 
 out_unmap:
-	for (thread = 0; thread < evlist->threads->nr; thread++) {
+	for (thread = 0; thread < nr_threads; thread++) {
 		if (evlist->mmap[thread].base != NULL) {
 			munmap(evlist->mmap[thread].base, evlist->mmap_len);
 			evlist->mmap[thread].base = NULL;
@@ -610,7 +618,7 @@ int perf_evlist__apply_filters(struct perf_evlist *evlist)
 	struct perf_evsel *evsel;
 	int err = 0;
 	const int ncpus = cpu_map__nr(evlist->cpus),
-		  nthreads = evlist->threads->nr;
+		  nthreads = thread_map__nr(evlist->threads);
 
 	list_for_each_entry(evsel, &evlist->entries, node) {
 		if (evsel->filter == NULL)
@@ -629,7 +637,7 @@ int perf_evlist__set_filter(struct perf_evlist *evlist, const char *filter)
 	struct perf_evsel *evsel;
 	int err = 0;
 	const int ncpus = cpu_map__nr(evlist->cpus),
-		  nthreads = evlist->threads->nr;
+		  nthreads = thread_map__nr(evlist->threads);
 
 	list_for_each_entry(evsel, &evlist->entries, node) {
 		err = perf_evsel__set_filter(evsel, ncpus, nthreads, filter);
@@ -712,10 +720,20 @@ void perf_evlist__set_selected(struct perf_evlist *evlist,
 	evlist->selected = evsel;
 }
 
+void perf_evlist__close(struct perf_evlist *evlist)
+{
+	struct perf_evsel *evsel;
+	int ncpus = cpu_map__nr(evlist->cpus);
+	int nthreads = thread_map__nr(evlist->threads);
+
+	list_for_each_entry_reverse(evsel, &evlist->entries, node)
+		perf_evsel__close(evsel, ncpus, nthreads);
+}
+
 int perf_evlist__open(struct perf_evlist *evlist)
 {
 	struct perf_evsel *evsel;
-	int err, ncpus, nthreads;
+	int err;
 
 	list_for_each_entry(evsel, &evlist->entries, node) {
 		err = perf_evsel__open(evsel, evlist->cpus, evlist->threads);
@@ -725,19 +743,15 @@ int perf_evlist__open(struct perf_evlist *evlist)
 
 	return 0;
 out_err:
-	ncpus = evlist->cpus ? evlist->cpus->nr : 1;
-	nthreads = evlist->threads ? evlist->threads->nr : 1;
-
-	list_for_each_entry_reverse(evsel, &evlist->entries, node)
-		perf_evsel__close(evsel, ncpus, nthreads);
-
+	perf_evlist__close(evlist);
 	errno = -err;
 	return err;
 }
 
 int perf_evlist__prepare_workload(struct perf_evlist *evlist,
-				  struct perf_record_opts *opts,
-				  const char *argv[])
+				  struct perf_target *target,
+				  const char *argv[], bool pipe_output,
+				  bool want_signal)
 {
 	int child_ready_pipe[2], go_pipe[2];
 	char bf;
@@ -759,7 +773,7 @@ int perf_evlist__prepare_workload(struct perf_evlist *evlist,
 	}
 
 	if (!evlist->workload.pid) {
-		if (opts->pipe_output)
+		if (pipe_output)
 			dup2(2, 1);
 
 		close(child_ready_pipe[0]);
@@ -787,11 +801,12 @@ int perf_evlist__prepare_workload(struct perf_evlist *evlist,
 		execvp(argv[0], (char **)argv);
 
 		perror(argv[0]);
-		kill(getppid(), SIGUSR1);
+		if (want_signal)
+			kill(getppid(), SIGUSR1);
 		exit(-1);
 	}
 
-	if (perf_target__none(&opts->target))
+	if (perf_target__none(target))
 		evlist->threads->map[0] = evlist->workload.pid;
 
 	close(child_ready_pipe[1]);
diff --git a/tools/perf/util/evlist.h b/tools/perf/util/evlist.h
index 2dd07bd60b4..0583d36252b 100644
--- a/tools/perf/util/evlist.h
+++ b/tools/perf/util/evlist.h
@@ -49,8 +49,7 @@ struct perf_evsel_str_handler {
 	void	   *handler;
 };
 
-struct perf_evlist *perf_evlist__new(struct cpu_map *cpus,
-				     struct thread_map *threads);
+struct perf_evlist *perf_evlist__new(void);
 void perf_evlist__init(struct perf_evlist *evlist, struct cpu_map *cpus,
 		       struct thread_map *threads);
 void perf_evlist__exit(struct perf_evlist *evlist);
@@ -82,13 +81,15 @@ struct perf_evsel *perf_evlist__id2evsel(struct perf_evlist *evlist, u64 id);
 union perf_event *perf_evlist__mmap_read(struct perf_evlist *self, int idx);
 
 int perf_evlist__open(struct perf_evlist *evlist);
+void perf_evlist__close(struct perf_evlist *evlist);
 
 void perf_evlist__config(struct perf_evlist *evlist,
 			 struct perf_record_opts *opts);
 
 int perf_evlist__prepare_workload(struct perf_evlist *evlist,
-				  struct perf_record_opts *opts,
-				  const char *argv[]);
+				  struct perf_target *target,
+				  const char *argv[], bool pipe_output,
+				  bool want_signal);
 int perf_evlist__start_workload(struct perf_evlist *evlist);
 
 int perf_evlist__mmap(struct perf_evlist *evlist, unsigned int pages,
diff --git a/tools/perf/util/evsel.c b/tools/perf/util/evsel.c
index 9c82f98f26d..07b1a3ad3e2 100644
--- a/tools/perf/util/evsel.c
+++ b/tools/perf/util/evsel.c
@@ -10,7 +10,7 @@
 #include <byteswap.h>
 #include <linux/bitops.h>
 #include "asm/bug.h"
-#include "debugfs.h"
+#include <lk/debugfs.h>
 #include "event-parse.h"
 #include "evsel.h"
 #include "evlist.h"
@@ -554,6 +554,9 @@ void perf_evsel__config(struct perf_evsel *evsel,
 		perf_evsel__set_sample_bit(evsel, CPU);
 	}
 
+	if (opts->sample_address)
+		attr->sample_type	|= PERF_SAMPLE_DATA_SRC;
+
 	if (opts->no_delay) {
 		attr->watermark = 0;
 		attr->wakeup_events = 1;
@@ -563,6 +566,9 @@ void perf_evsel__config(struct perf_evsel *evsel,
 		attr->branch_sample_type = opts->branch_stack;
 	}
 
+	if (opts->sample_weight)
+		attr->sample_type	|= PERF_SAMPLE_WEIGHT;
+
 	attr->mmap = track;
 	attr->comm = track;
 
@@ -633,6 +639,12 @@ int perf_evsel__alloc_id(struct perf_evsel *evsel, int ncpus, int nthreads)
 	return 0;
 }
 
+void perf_evsel__reset_counts(struct perf_evsel *evsel, int ncpus)
+{
+	memset(evsel->counts, 0, (sizeof(*evsel->counts) +
+				 (ncpus * sizeof(struct perf_counts_values))));
+}
+
 int perf_evsel__alloc_counts(struct perf_evsel *evsel, int ncpus)
 {
 	evsel->counts = zalloc((sizeof(*evsel->counts) +
@@ -673,9 +685,8 @@ void perf_evsel__free_counts(struct perf_evsel *evsel)
 void perf_evsel__exit(struct perf_evsel *evsel)
 {
 	assert(list_empty(&evsel->node));
-	xyarray__delete(evsel->fd);
-	xyarray__delete(evsel->sample_id);
-	free(evsel->id);
+	perf_evsel__free_fd(evsel);
+	perf_evsel__free_id(evsel);
 }
 
 void perf_evsel__delete(struct perf_evsel *evsel)
@@ -1012,6 +1023,7 @@ int perf_evsel__parse_sample(struct perf_evsel *evsel, union perf_event *event,
 	data->cpu = data->pid = data->tid = -1;
 	data->stream_id = data->id = data->time = -1ULL;
 	data->period = 1;
+	data->weight = 0;
 
 	if (event->header.type != PERF_RECORD_SAMPLE) {
 		if (!evsel->attr.sample_id_all)
@@ -1162,6 +1174,18 @@ int perf_evsel__parse_sample(struct perf_evsel *evsel, union perf_event *event,
 		}
 	}
 
+	data->weight = 0;
+	if (type & PERF_SAMPLE_WEIGHT) {
+		data->weight = *array;
+		array++;
+	}
+
+	data->data_src = PERF_MEM_DATA_SRC_NONE;
+	if (type & PERF_SAMPLE_DATA_SRC) {
+		data->data_src = *array;
+		array++;
+	}
+
 	return 0;
 }
 
diff --git a/tools/perf/util/evsel.h b/tools/perf/util/evsel.h
index 52021c3087d..3f156ccc1ac 100644
--- a/tools/perf/util/evsel.h
+++ b/tools/perf/util/evsel.h
@@ -9,6 +9,7 @@
 #include "xyarray.h"
 #include "cgroup.h"
 #include "hist.h"
+#include "symbol.h"
  
 struct perf_counts_values {
 	union {
@@ -120,6 +121,7 @@ int perf_evsel__group_desc(struct perf_evsel *evsel, char *buf, size_t size);
 int perf_evsel__alloc_fd(struct perf_evsel *evsel, int ncpus, int nthreads);
 int perf_evsel__alloc_id(struct perf_evsel *evsel, int ncpus, int nthreads);
 int perf_evsel__alloc_counts(struct perf_evsel *evsel, int ncpus);
+void perf_evsel__reset_counts(struct perf_evsel *evsel, int ncpus);
 void perf_evsel__free_fd(struct perf_evsel *evsel);
 void perf_evsel__free_id(struct perf_evsel *evsel);
 void perf_evsel__free_counts(struct perf_evsel *evsel);
@@ -246,11 +248,34 @@ static inline struct perf_evsel *perf_evsel__next(struct perf_evsel *evsel)
 	return list_entry(evsel->node.next, struct perf_evsel, node);
 }
 
+/**
+ * perf_evsel__is_group_leader - Return whether given evsel is a leader event
+ *
+ * @evsel - evsel selector to be tested
+ *
+ * Return %true if @evsel is a group leader or a stand-alone event
+ */
 static inline bool perf_evsel__is_group_leader(const struct perf_evsel *evsel)
 {
 	return evsel->leader == evsel;
 }
 
+/**
+ * perf_evsel__is_group_event - Return whether given evsel is a group event
+ *
+ * @evsel - evsel selector to be tested
+ *
+ * Return %true iff event group view is enabled and @evsel is a actual group
+ * leader which has other members in the group
+ */
+static inline bool perf_evsel__is_group_event(struct perf_evsel *evsel)
+{
+	if (!symbol_conf.event_group)
+		return false;
+
+	return perf_evsel__is_group_leader(evsel) && evsel->nr_members > 1;
+}
+
 struct perf_attr_details {
 	bool freq;
 	bool verbose;
diff --git a/tools/perf/util/header.c b/tools/perf/util/header.c
index f4bfd79ef6a..326068a593a 100644
--- a/tools/perf/util/header.c
+++ b/tools/perf/util/header.c
@@ -1,5 +1,3 @@
-#define _FILE_OFFSET_BITS 64
-
 #include "util.h"
 #include <sys/types.h>
 #include <byteswap.h>
@@ -1672,8 +1670,8 @@ static int process_tracing_data(struct perf_file_section *section __maybe_unused
 				struct perf_header *ph __maybe_unused,
 				int fd, void *data)
 {
-	trace_report(fd, data, false);
-	return 0;
+	ssize_t ret = trace_report(fd, data, false);
+	return ret < 0 ? -1 : 0;
 }
 
 static int process_build_id(struct perf_file_section *section,
@@ -2752,6 +2750,11 @@ static int perf_evsel__prepare_tracepoint_event(struct perf_evsel *evsel,
 	if (evsel->tp_format)
 		return 0;
 
+	if (pevent == NULL) {
+		pr_debug("broken or missing trace data\n");
+		return -1;
+	}
+
 	event = pevent_find_event(pevent, evsel->attr.config);
 	if (event == NULL)
 		return -1;
@@ -2789,7 +2792,7 @@ int perf_session__read_header(struct perf_session *session, int fd)
 	u64			f_id;
 	int nr_attrs, nr_ids, i, j;
 
-	session->evlist = perf_evlist__new(NULL, NULL);
+	session->evlist = perf_evlist__new();
 	if (session->evlist == NULL)
 		return -ENOMEM;
 
@@ -2940,7 +2943,7 @@ int perf_event__process_attr(union perf_event *event,
 	struct perf_evlist *evlist = *pevlist;
 
 	if (evlist == NULL) {
-		*pevlist = evlist = perf_evlist__new(NULL, NULL);
+		*pevlist = evlist = perf_evlist__new();
 		if (evlist == NULL)
 			return -ENOMEM;
 	}
diff --git a/tools/perf/util/hist.c b/tools/perf/util/hist.c
index f855941bebe..6b32721f829 100644
--- a/tools/perf/util/hist.c
+++ b/tools/perf/util/hist.c
@@ -67,12 +67,16 @@ static void hists__set_unres_dso_col_len(struct hists *hists, int dso)
 void hists__calc_col_len(struct hists *hists, struct hist_entry *h)
 {
 	const unsigned int unresolved_col_width = BITS_PER_LONG / 4;
+	int symlen;
 	u16 len;
 
 	if (h->ms.sym)
 		hists__new_col_len(hists, HISTC_SYMBOL, h->ms.sym->namelen + 4);
-	else
+	else {
+		symlen = unresolved_col_width + 4 + 2;
+		hists__new_col_len(hists, HISTC_SYMBOL, symlen);
 		hists__set_unres_dso_col_len(hists, HISTC_DSO);
+	}
 
 	len = thread__comm_len(h->thread);
 	if (hists__new_col_len(hists, HISTC_COMM, len))
@@ -87,7 +91,6 @@ void hists__calc_col_len(struct hists *hists, struct hist_entry *h)
 		hists__new_col_len(hists, HISTC_PARENT, h->parent->namelen);
 
 	if (h->branch_info) {
-		int symlen;
 		/*
 		 * +4 accounts for '[x] ' priv level info
 		 * +2 account of 0x prefix on raw addresses
@@ -116,6 +119,42 @@ void hists__calc_col_len(struct hists *hists, struct hist_entry *h)
 			hists__set_unres_dso_col_len(hists, HISTC_DSO_TO);
 		}
 	}
+
+	if (h->mem_info) {
+		/*
+		 * +4 accounts for '[x] ' priv level info
+		 * +2 account of 0x prefix on raw addresses
+		 */
+		if (h->mem_info->daddr.sym) {
+			symlen = (int)h->mem_info->daddr.sym->namelen + 4
+			       + unresolved_col_width + 2;
+			hists__new_col_len(hists, HISTC_MEM_DADDR_SYMBOL,
+					   symlen);
+		} else {
+			symlen = unresolved_col_width + 4 + 2;
+			hists__new_col_len(hists, HISTC_MEM_DADDR_SYMBOL,
+					   symlen);
+		}
+		if (h->mem_info->daddr.map) {
+			symlen = dso__name_len(h->mem_info->daddr.map->dso);
+			hists__new_col_len(hists, HISTC_MEM_DADDR_DSO,
+					   symlen);
+		} else {
+			symlen = unresolved_col_width + 4 + 2;
+			hists__set_unres_dso_col_len(hists, HISTC_MEM_DADDR_DSO);
+		}
+	} else {
+		symlen = unresolved_col_width + 4 + 2;
+		hists__new_col_len(hists, HISTC_MEM_DADDR_SYMBOL, symlen);
+		hists__set_unres_dso_col_len(hists, HISTC_MEM_DADDR_DSO);
+	}
+
+	hists__new_col_len(hists, HISTC_MEM_LOCKED, 6);
+	hists__new_col_len(hists, HISTC_MEM_TLB, 22);
+	hists__new_col_len(hists, HISTC_MEM_SNOOP, 12);
+	hists__new_col_len(hists, HISTC_MEM_LVL, 21 + 3);
+	hists__new_col_len(hists, HISTC_LOCAL_WEIGHT, 12);
+	hists__new_col_len(hists, HISTC_GLOBAL_WEIGHT, 12);
 }
 
 void hists__output_recalc_col_len(struct hists *hists, int max_rows)
@@ -155,9 +194,12 @@ static void hist_entry__add_cpumode_period(struct hist_entry *he,
 	}
 }
 
-static void he_stat__add_period(struct he_stat *he_stat, u64 period)
+static void he_stat__add_period(struct he_stat *he_stat, u64 period,
+				u64 weight)
 {
+
 	he_stat->period		+= period;
+	he_stat->weight		+= weight;
 	he_stat->nr_events	+= 1;
 }
 
@@ -169,12 +211,14 @@ static void he_stat__add_stat(struct he_stat *dest, struct he_stat *src)
 	dest->period_guest_sys	+= src->period_guest_sys;
 	dest->period_guest_us	+= src->period_guest_us;
 	dest->nr_events		+= src->nr_events;
+	dest->weight		+= src->weight;
 }
 
 static void hist_entry__decay(struct hist_entry *he)
 {
 	he->stat.period = (he->stat.period * 7) / 8;
 	he->stat.nr_events = (he->stat.nr_events * 7) / 8;
+	/* XXX need decay for weight too? */
 }
 
 static bool hists__decay_entry(struct hists *hists, struct hist_entry *he)
@@ -239,7 +283,7 @@ void hists__decay_entries_threaded(struct hists *hists,
 static struct hist_entry *hist_entry__new(struct hist_entry *template)
 {
 	size_t callchain_size = symbol_conf.use_callchain ? sizeof(struct callchain_root) : 0;
-	struct hist_entry *he = malloc(sizeof(*he) + callchain_size);
+	struct hist_entry *he = zalloc(sizeof(*he) + callchain_size);
 
 	if (he != NULL) {
 		*he = *template;
@@ -254,6 +298,13 @@ static struct hist_entry *hist_entry__new(struct hist_entry *template)
 				he->branch_info->to.map->referenced = true;
 		}
 
+		if (he->mem_info) {
+			if (he->mem_info->iaddr.map)
+				he->mem_info->iaddr.map->referenced = true;
+			if (he->mem_info->daddr.map)
+				he->mem_info->daddr.map->referenced = true;
+		}
+
 		if (symbol_conf.use_callchain)
 			callchain_init(he->callchain);
 
@@ -282,7 +333,8 @@ static u8 symbol__parent_filter(const struct symbol *parent)
 static struct hist_entry *add_hist_entry(struct hists *hists,
 				      struct hist_entry *entry,
 				      struct addr_location *al,
-				      u64 period)
+				      u64 period,
+				      u64 weight)
 {
 	struct rb_node **p;
 	struct rb_node *parent = NULL;
@@ -306,7 +358,7 @@ static struct hist_entry *add_hist_entry(struct hists *hists,
 		cmp = hist_entry__cmp(he, entry);
 
 		if (!cmp) {
-			he_stat__add_period(&he->stat, period);
+			he_stat__add_period(&he->stat, period, weight);
 
 			/* If the map of an existing hist_entry has
 			 * become out-of-date due to an exec() or
@@ -341,11 +393,42 @@ out_unlock:
 	return he;
 }
 
+struct hist_entry *__hists__add_mem_entry(struct hists *self,
+					  struct addr_location *al,
+					  struct symbol *sym_parent,
+					  struct mem_info *mi,
+					  u64 period,
+					  u64 weight)
+{
+	struct hist_entry entry = {
+		.thread	= al->thread,
+		.ms = {
+			.map	= al->map,
+			.sym	= al->sym,
+		},
+		.stat = {
+			.period	= period,
+			.weight = weight,
+			.nr_events = 1,
+		},
+		.cpu	= al->cpu,
+		.ip	= al->addr,
+		.level	= al->level,
+		.parent = sym_parent,
+		.filtered = symbol__parent_filter(sym_parent),
+		.hists = self,
+		.mem_info = mi,
+		.branch_info = NULL,
+	};
+	return add_hist_entry(self, &entry, al, period, weight);
+}
+
 struct hist_entry *__hists__add_branch_entry(struct hists *self,
 					     struct addr_location *al,
 					     struct symbol *sym_parent,
 					     struct branch_info *bi,
-					     u64 period)
+					     u64 period,
+					     u64 weight)
 {
 	struct hist_entry entry = {
 		.thread	= al->thread,
@@ -359,19 +442,22 @@ struct hist_entry *__hists__add_branch_entry(struct hists *self,
 		.stat = {
 			.period	= period,
 			.nr_events = 1,
+			.weight = weight,
 		},
 		.parent = sym_parent,
 		.filtered = symbol__parent_filter(sym_parent),
 		.branch_info = bi,
 		.hists	= self,
+		.mem_info = NULL,
 	};
 
-	return add_hist_entry(self, &entry, al, period);
+	return add_hist_entry(self, &entry, al, period, weight);
 }
 
 struct hist_entry *__hists__add_entry(struct hists *self,
 				      struct addr_location *al,
-				      struct symbol *sym_parent, u64 period)
+				      struct symbol *sym_parent, u64 period,
+				      u64 weight)
 {
 	struct hist_entry entry = {
 		.thread	= al->thread,
@@ -385,13 +471,16 @@ struct hist_entry *__hists__add_entry(struct hists *self,
 		.stat = {
 			.period	= period,
 			.nr_events = 1,
+			.weight = weight,
 		},
 		.parent = sym_parent,
 		.filtered = symbol__parent_filter(sym_parent),
 		.hists	= self,
+		.branch_info = NULL,
+		.mem_info = NULL,
 	};
 
-	return add_hist_entry(self, &entry, al, period);
+	return add_hist_entry(self, &entry, al, period, weight);
 }
 
 int64_t
@@ -431,6 +520,7 @@ hist_entry__collapse(struct hist_entry *left, struct hist_entry *right)
 void hist_entry__free(struct hist_entry *he)
 {
 	free(he->branch_info);
+	free(he->mem_info);
 	free(he);
 }
 
diff --git a/tools/perf/util/hist.h b/tools/perf/util/hist.h
index 226a4ae2f93..14c2fe20aa6 100644
--- a/tools/perf/util/hist.h
+++ b/tools/perf/util/hist.h
@@ -49,6 +49,14 @@ enum hist_column {
 	HISTC_DSO_FROM,
 	HISTC_DSO_TO,
 	HISTC_SRCLINE,
+	HISTC_LOCAL_WEIGHT,
+	HISTC_GLOBAL_WEIGHT,
+	HISTC_MEM_DADDR_SYMBOL,
+	HISTC_MEM_DADDR_DSO,
+	HISTC_MEM_LOCKED,
+	HISTC_MEM_TLB,
+	HISTC_MEM_LVL,
+	HISTC_MEM_SNOOP,
 	HISTC_NR_COLS, /* Last entry */
 };
 
@@ -73,7 +81,8 @@ struct hists {
 
 struct hist_entry *__hists__add_entry(struct hists *self,
 				      struct addr_location *al,
-				      struct symbol *parent, u64 period);
+				      struct symbol *parent, u64 period,
+				      u64 weight);
 int64_t hist_entry__cmp(struct hist_entry *left, struct hist_entry *right);
 int64_t hist_entry__collapse(struct hist_entry *left, struct hist_entry *right);
 int hist_entry__sort_snprintf(struct hist_entry *self, char *bf, size_t size,
@@ -84,7 +93,15 @@ struct hist_entry *__hists__add_branch_entry(struct hists *self,
 					     struct addr_location *al,
 					     struct symbol *sym_parent,
 					     struct branch_info *bi,
-					     u64 period);
+					     u64 period,
+					     u64 weight);
+
+struct hist_entry *__hists__add_mem_entry(struct hists *self,
+					  struct addr_location *al,
+					  struct symbol *sym_parent,
+					  struct mem_info *mi,
+					  u64 period,
+					  u64 weight);
 
 void hists__output_resort(struct hists *self);
 void hists__output_resort_threaded(struct hists *hists);
@@ -175,9 +192,9 @@ struct hist_browser_timer {
 	int refresh;
 };
 
-#ifdef NEWT_SUPPORT
+#ifdef SLANG_SUPPORT
 #include "../ui/keysyms.h"
-int hist_entry__tui_annotate(struct hist_entry *he, int evidx,
+int hist_entry__tui_annotate(struct hist_entry *he, struct perf_evsel *evsel,
 			     struct hist_browser_timer *hbt);
 
 int perf_evlist__tui_browse_hists(struct perf_evlist *evlist, const char *help,
@@ -196,7 +213,8 @@ int perf_evlist__tui_browse_hists(struct perf_evlist *evlist __maybe_unused,
 
 static inline int hist_entry__tui_annotate(struct hist_entry *self
 					   __maybe_unused,
-					   int evidx __maybe_unused,
+					   struct perf_evsel *evsel
+					   __maybe_unused,
 					   struct hist_browser_timer *hbt
 					   __maybe_unused)
 {
diff --git a/tools/perf/util/machine.c b/tools/perf/util/machine.c
index efdb38e65a9..b2ecad6ec46 100644
--- a/tools/perf/util/machine.c
+++ b/tools/perf/util/machine.c
@@ -955,6 +955,7 @@ int machine__process_mmap_event(struct machine *machine, union perf_event *event
 	u8 cpumode = event->header.misc & PERF_RECORD_MISC_CPUMODE_MASK;
 	struct thread *thread;
 	struct map *map;
+	enum map_type type;
 	int ret = 0;
 
 	if (dump_trace)
@@ -971,10 +972,17 @@ int machine__process_mmap_event(struct machine *machine, union perf_event *event
 	thread = machine__findnew_thread(machine, event->mmap.pid);
 	if (thread == NULL)
 		goto out_problem;
+
+	if (event->header.misc & PERF_RECORD_MISC_MMAP_DATA)
+		type = MAP__VARIABLE;
+	else
+		type = MAP__FUNCTION;
+
 	map = map__new(&machine->user_dsos, event->mmap.start,
 			event->mmap.len, event->mmap.pgoff,
 			event->mmap.pid, event->mmap.filename,
-			MAP__FUNCTION);
+			type);
+
 	if (map == NULL)
 		goto out_problem;
 
@@ -1003,6 +1011,17 @@ int machine__process_fork_event(struct machine *machine, union perf_event *event
 	return 0;
 }
 
+static void machine__remove_thread(struct machine *machine, struct thread *th)
+{
+	machine->last_match = NULL;
+	rb_erase(&th->rb_node, &machine->threads);
+	/*
+	 * We may have references to this thread, for instance in some hist_entry
+	 * instances, so just move them to a separate list.
+	 */
+	list_add_tail(&th->node, &machine->dead_threads);
+}
+
 int machine__process_exit_event(struct machine *machine, union perf_event *event)
 {
 	struct thread *thread = machine__find_thread(machine, event->fork.tid);
@@ -1039,17 +1058,6 @@ int machine__process_event(struct machine *machine, union perf_event *event)
 	return ret;
 }
 
-void machine__remove_thread(struct machine *machine, struct thread *th)
-{
-	machine->last_match = NULL;
-	rb_erase(&th->rb_node, &machine->threads);
-	/*
-	 * We may have references to this thread, for instance in some hist_entry
-	 * instances, so just move them to a separate list.
-	 */
-	list_add_tail(&th->node, &machine->dead_threads);
-}
-
 static bool symbol__match_parent_regex(struct symbol *sym)
 {
 	if (sym->name && !regexec(&parent_regex, sym->name, 0, NULL, 0))
@@ -1097,6 +1105,38 @@ found:
 	ams->map = al.map;
 }
 
+static void ip__resolve_data(struct machine *machine, struct thread *thread,
+			     u8 m, struct addr_map_symbol *ams, u64 addr)
+{
+	struct addr_location al;
+
+	memset(&al, 0, sizeof(al));
+
+	thread__find_addr_location(thread, machine, m, MAP__VARIABLE, addr, &al,
+				   NULL);
+	ams->addr = addr;
+	ams->al_addr = al.addr;
+	ams->sym = al.sym;
+	ams->map = al.map;
+}
+
+struct mem_info *machine__resolve_mem(struct machine *machine,
+				      struct thread *thr,
+				      struct perf_sample *sample,
+				      u8 cpumode)
+{
+	struct mem_info *mi = zalloc(sizeof(*mi));
+
+	if (!mi)
+		return NULL;
+
+	ip__resolve_ams(machine, thr, &mi->iaddr, sample->ip);
+	ip__resolve_data(machine, thr, cpumode, &mi->daddr, sample->addr);
+	mi->data_src.val = sample->data_src;
+
+	return mi;
+}
+
 struct branch_info *machine__resolve_bstack(struct machine *machine,
 					    struct thread *thr,
 					    struct branch_stack *bs)
diff --git a/tools/perf/util/machine.h b/tools/perf/util/machine.h
index 5ac5892f232..77940680f1f 100644
--- a/tools/perf/util/machine.h
+++ b/tools/perf/util/machine.h
@@ -76,6 +76,9 @@ void machine__delete(struct machine *machine);
 struct branch_info *machine__resolve_bstack(struct machine *machine,
 					    struct thread *thread,
 					    struct branch_stack *bs);
+struct mem_info *machine__resolve_mem(struct machine *machine,
+				      struct thread *thread,
+				      struct perf_sample *sample, u8 cpumode);
 int machine__resolve_callchain(struct machine *machine,
 			       struct perf_evsel *evsel,
 			       struct thread *thread,
@@ -97,7 +100,6 @@ static inline bool machine__is_host(struct machine *machine)
 }
 
 struct thread *machine__findnew_thread(struct machine *machine, pid_t pid);
-void machine__remove_thread(struct machine *machine, struct thread *th);
 
 size_t machine__fprintf(struct machine *machine, FILE *fp);
 
diff --git a/tools/perf/util/parse-events.c b/tools/perf/util/parse-events.c
index c84f48cf967..6c8bb0fb189 100644
--- a/tools/perf/util/parse-events.c
+++ b/tools/perf/util/parse-events.c
@@ -10,7 +10,7 @@
 #include "symbol.h"
 #include "cache.h"
 #include "header.h"
-#include "debugfs.h"
+#include <lk/debugfs.h>
 #include "parse-events-bison.h"
 #define YY_EXTRA_TYPE int
 #include "parse-events-flex.h"
diff --git a/tools/perf/util/probe-event.c b/tools/perf/util/probe-event.c
index 49a256e6e0a..aa04bf9c9ad 100644
--- a/tools/perf/util/probe-event.c
+++ b/tools/perf/util/probe-event.c
@@ -40,7 +40,7 @@
 #include "color.h"
 #include "symbol.h"
 #include "thread.h"
-#include "debugfs.h"
+#include <lk/debugfs.h>
 #include "trace-event.h"	/* For __maybe_unused */
 #include "probe-event.h"
 #include "probe-finder.h"
diff --git a/tools/perf/util/python-ext-sources b/tools/perf/util/python-ext-sources
index 64536a993f4..f75ae1b9900 100644
--- a/tools/perf/util/python-ext-sources
+++ b/tools/perf/util/python-ext-sources
@@ -15,7 +15,6 @@ util/thread_map.c
 util/util.c
 util/xyarray.c
 util/cgroup.c
-util/debugfs.c
 util/rblist.c
 util/strlist.c
 util/sysfs.c
diff --git a/tools/perf/util/session.c b/tools/perf/util/session.c
index bd85280bb6e..cf1fe01b7e8 100644
--- a/tools/perf/util/session.c
+++ b/tools/perf/util/session.c
@@ -1,5 +1,3 @@
-#define _FILE_OFFSET_BITS 64
-
 #include <linux/kernel.h>
 
 #include <byteswap.h>
@@ -800,6 +798,12 @@ static void dump_sample(struct perf_evsel *evsel, union perf_event *event,
 
 	if (sample_type & PERF_SAMPLE_STACK_USER)
 		stack_user__printf(&sample->user_stack);
+
+	if (sample_type & PERF_SAMPLE_WEIGHT)
+		printf("... weight: %" PRIu64 "\n", sample->weight);
+
+	if (sample_type & PERF_SAMPLE_DATA_SRC)
+		printf(" . data_src: 0x%"PRIx64"\n", sample->data_src);
 }
 
 static struct machine *
@@ -1365,18 +1369,6 @@ size_t perf_session__fprintf(struct perf_session *session, FILE *fp)
 	return machine__fprintf(&session->machines.host, fp);
 }
 
-void perf_session__remove_thread(struct perf_session *session,
-				 struct thread *th)
-{
-	/*
-	 * FIXME: This one makes no sense, we need to remove the thread from
-	 * the machine it belongs to, perf_session can have many machines, so
-	 * doing it always on ->machines.host is wrong.  Fix when auditing all
-	 * the 'perf kvm' code.
-	 */
-	machine__remove_thread(&session->machines.host, th);
-}
-
 struct perf_evsel *perf_session__find_first_evtype(struct perf_session *session,
 					      unsigned int type)
 {
diff --git a/tools/perf/util/session.h b/tools/perf/util/session.h
index b5c0847edfa..6b51d47acdb 100644
--- a/tools/perf/util/session.h
+++ b/tools/perf/util/session.h
@@ -72,7 +72,6 @@ void perf_event__attr_swap(struct perf_event_attr *attr);
 int perf_session__create_kernel_maps(struct perf_session *self);
 
 void perf_session__set_id_hdr_size(struct perf_session *session);
-void perf_session__remove_thread(struct perf_session *self, struct thread *th);
 
 static inline
 struct machine *perf_session__find_machine(struct perf_session *self, pid_t pid)
diff --git a/tools/perf/util/setup.py b/tools/perf/util/setup.py
index 73d51026978..6b0ed322907 100644
--- a/tools/perf/util/setup.py
+++ b/tools/perf/util/setup.py
@@ -24,6 +24,7 @@ cflags += getenv('CFLAGS', '').split()
 build_lib = getenv('PYTHON_EXTBUILD_LIB')
 build_tmp = getenv('PYTHON_EXTBUILD_TMP')
 libtraceevent = getenv('LIBTRACEEVENT')
+liblk = getenv('LIBLK')
 
 ext_sources = [f.strip() for f in file('util/python-ext-sources')
 				if len(f.strip()) > 0 and f[0] != '#']
@@ -32,7 +33,7 @@ perf = Extension('perf',
 		  sources = ext_sources,
 		  include_dirs = ['util/include'],
 		  extra_compile_args = cflags,
-		  extra_objects = [libtraceevent],
+		  extra_objects = [libtraceevent, liblk],
                  )
 
 setup(name='perf',
diff --git a/tools/perf/util/sort.c b/tools/perf/util/sort.c
index d41926cb9e3..5f52d492590 100644
--- a/tools/perf/util/sort.c
+++ b/tools/perf/util/sort.c
@@ -198,11 +198,19 @@ static int _hist_entry__sym_snprintf(struct map *map, struct symbol *sym,
 	}
 
 	ret += repsep_snprintf(bf + ret, size - ret, "[%c] ", level);
-	if (sym)
-		ret += repsep_snprintf(bf + ret, size - ret, "%-*s",
-				       width - ret,
-				       sym->name);
-	else {
+	if (sym && map) {
+		if (map->type == MAP__VARIABLE) {
+			ret += repsep_snprintf(bf + ret, size - ret, "%s", sym->name);
+			ret += repsep_snprintf(bf + ret, size - ret, "+0x%llx",
+					ip - map->unmap_ip(map, sym->start));
+			ret += repsep_snprintf(bf + ret, size - ret, "%-*s",
+				       width - ret, "");
+		} else {
+			ret += repsep_snprintf(bf + ret, size - ret, "%-*s",
+					       width - ret,
+					       sym->name);
+		}
+	} else {
 		size_t len = BITS_PER_LONG / 4;
 		ret += repsep_snprintf(bf + ret, size - ret, "%-#.*llx",
 				       len, ip);
@@ -457,6 +465,304 @@ static int hist_entry__mispredict_snprintf(struct hist_entry *self, char *bf,
 	return repsep_snprintf(bf, size, "%-*s", width, out);
 }
 
+/* --sort daddr_sym */
+static int64_t
+sort__daddr_cmp(struct hist_entry *left, struct hist_entry *right)
+{
+	uint64_t l = 0, r = 0;
+
+	if (left->mem_info)
+		l = left->mem_info->daddr.addr;
+	if (right->mem_info)
+		r = right->mem_info->daddr.addr;
+
+	return (int64_t)(r - l);
+}
+
+static int hist_entry__daddr_snprintf(struct hist_entry *self, char *bf,
+				    size_t size, unsigned int width)
+{
+	uint64_t addr = 0;
+	struct map *map = NULL;
+	struct symbol *sym = NULL;
+
+	if (self->mem_info) {
+		addr = self->mem_info->daddr.addr;
+		map = self->mem_info->daddr.map;
+		sym = self->mem_info->daddr.sym;
+	}
+	return _hist_entry__sym_snprintf(map, sym, addr, self->level, bf, size,
+					 width);
+}
+
+static int64_t
+sort__dso_daddr_cmp(struct hist_entry *left, struct hist_entry *right)
+{
+	struct map *map_l = NULL;
+	struct map *map_r = NULL;
+
+	if (left->mem_info)
+		map_l = left->mem_info->daddr.map;
+	if (right->mem_info)
+		map_r = right->mem_info->daddr.map;
+
+	return _sort__dso_cmp(map_l, map_r);
+}
+
+static int hist_entry__dso_daddr_snprintf(struct hist_entry *self, char *bf,
+				    size_t size, unsigned int width)
+{
+	struct map *map = NULL;
+
+	if (self->mem_info)
+		map = self->mem_info->daddr.map;
+
+	return _hist_entry__dso_snprintf(map, bf, size, width);
+}
+
+static int64_t
+sort__locked_cmp(struct hist_entry *left, struct hist_entry *right)
+{
+	union perf_mem_data_src data_src_l;
+	union perf_mem_data_src data_src_r;
+
+	if (left->mem_info)
+		data_src_l = left->mem_info->data_src;
+	else
+		data_src_l.mem_lock = PERF_MEM_LOCK_NA;
+
+	if (right->mem_info)
+		data_src_r = right->mem_info->data_src;
+	else
+		data_src_r.mem_lock = PERF_MEM_LOCK_NA;
+
+	return (int64_t)(data_src_r.mem_lock - data_src_l.mem_lock);
+}
+
+static int hist_entry__locked_snprintf(struct hist_entry *self, char *bf,
+				    size_t size, unsigned int width)
+{
+	const char *out;
+	u64 mask = PERF_MEM_LOCK_NA;
+
+	if (self->mem_info)
+		mask = self->mem_info->data_src.mem_lock;
+
+	if (mask & PERF_MEM_LOCK_NA)
+		out = "N/A";
+	else if (mask & PERF_MEM_LOCK_LOCKED)
+		out = "Yes";
+	else
+		out = "No";
+
+	return repsep_snprintf(bf, size, "%-*s", width, out);
+}
+
+static int64_t
+sort__tlb_cmp(struct hist_entry *left, struct hist_entry *right)
+{
+	union perf_mem_data_src data_src_l;
+	union perf_mem_data_src data_src_r;
+
+	if (left->mem_info)
+		data_src_l = left->mem_info->data_src;
+	else
+		data_src_l.mem_dtlb = PERF_MEM_TLB_NA;
+
+	if (right->mem_info)
+		data_src_r = right->mem_info->data_src;
+	else
+		data_src_r.mem_dtlb = PERF_MEM_TLB_NA;
+
+	return (int64_t)(data_src_r.mem_dtlb - data_src_l.mem_dtlb);
+}
+
+static const char * const tlb_access[] = {
+	"N/A",
+	"HIT",
+	"MISS",
+	"L1",
+	"L2",
+	"Walker",
+	"Fault",
+};
+#define NUM_TLB_ACCESS (sizeof(tlb_access)/sizeof(const char *))
+
+static int hist_entry__tlb_snprintf(struct hist_entry *self, char *bf,
+				    size_t size, unsigned int width)
+{
+	char out[64];
+	size_t sz = sizeof(out) - 1; /* -1 for null termination */
+	size_t l = 0, i;
+	u64 m = PERF_MEM_TLB_NA;
+	u64 hit, miss;
+
+	out[0] = '\0';
+
+	if (self->mem_info)
+		m = self->mem_info->data_src.mem_dtlb;
+
+	hit = m & PERF_MEM_TLB_HIT;
+	miss = m & PERF_MEM_TLB_MISS;
+
+	/* already taken care of */
+	m &= ~(PERF_MEM_TLB_HIT|PERF_MEM_TLB_MISS);
+
+	for (i = 0; m && i < NUM_TLB_ACCESS; i++, m >>= 1) {
+		if (!(m & 0x1))
+			continue;
+		if (l) {
+			strcat(out, " or ");
+			l += 4;
+		}
+		strncat(out, tlb_access[i], sz - l);
+		l += strlen(tlb_access[i]);
+	}
+	if (*out == '\0')
+		strcpy(out, "N/A");
+	if (hit)
+		strncat(out, " hit", sz - l);
+	if (miss)
+		strncat(out, " miss", sz - l);
+
+	return repsep_snprintf(bf, size, "%-*s", width, out);
+}
+
+static int64_t
+sort__lvl_cmp(struct hist_entry *left, struct hist_entry *right)
+{
+	union perf_mem_data_src data_src_l;
+	union perf_mem_data_src data_src_r;
+
+	if (left->mem_info)
+		data_src_l = left->mem_info->data_src;
+	else
+		data_src_l.mem_lvl = PERF_MEM_LVL_NA;
+
+	if (right->mem_info)
+		data_src_r = right->mem_info->data_src;
+	else
+		data_src_r.mem_lvl = PERF_MEM_LVL_NA;
+
+	return (int64_t)(data_src_r.mem_lvl - data_src_l.mem_lvl);
+}
+
+static const char * const mem_lvl[] = {
+	"N/A",
+	"HIT",
+	"MISS",
+	"L1",
+	"LFB",
+	"L2",
+	"L3",
+	"Local RAM",
+	"Remote RAM (1 hop)",
+	"Remote RAM (2 hops)",
+	"Remote Cache (1 hop)",
+	"Remote Cache (2 hops)",
+	"I/O",
+	"Uncached",
+};
+#define NUM_MEM_LVL (sizeof(mem_lvl)/sizeof(const char *))
+
+static int hist_entry__lvl_snprintf(struct hist_entry *self, char *bf,
+				    size_t size, unsigned int width)
+{
+	char out[64];
+	size_t sz = sizeof(out) - 1; /* -1 for null termination */
+	size_t i, l = 0;
+	u64 m =  PERF_MEM_LVL_NA;
+	u64 hit, miss;
+
+	if (self->mem_info)
+		m  = self->mem_info->data_src.mem_lvl;
+
+	out[0] = '\0';
+
+	hit = m & PERF_MEM_LVL_HIT;
+	miss = m & PERF_MEM_LVL_MISS;
+
+	/* already taken care of */
+	m &= ~(PERF_MEM_LVL_HIT|PERF_MEM_LVL_MISS);
+
+	for (i = 0; m && i < NUM_MEM_LVL; i++, m >>= 1) {
+		if (!(m & 0x1))
+			continue;
+		if (l) {
+			strcat(out, " or ");
+			l += 4;
+		}
+		strncat(out, mem_lvl[i], sz - l);
+		l += strlen(mem_lvl[i]);
+	}
+	if (*out == '\0')
+		strcpy(out, "N/A");
+	if (hit)
+		strncat(out, " hit", sz - l);
+	if (miss)
+		strncat(out, " miss", sz - l);
+
+	return repsep_snprintf(bf, size, "%-*s", width, out);
+}
+
+static int64_t
+sort__snoop_cmp(struct hist_entry *left, struct hist_entry *right)
+{
+	union perf_mem_data_src data_src_l;
+	union perf_mem_data_src data_src_r;
+
+	if (left->mem_info)
+		data_src_l = left->mem_info->data_src;
+	else
+		data_src_l.mem_snoop = PERF_MEM_SNOOP_NA;
+
+	if (right->mem_info)
+		data_src_r = right->mem_info->data_src;
+	else
+		data_src_r.mem_snoop = PERF_MEM_SNOOP_NA;
+
+	return (int64_t)(data_src_r.mem_snoop - data_src_l.mem_snoop);
+}
+
+static const char * const snoop_access[] = {
+	"N/A",
+	"None",
+	"Miss",
+	"Hit",
+	"HitM",
+};
+#define NUM_SNOOP_ACCESS (sizeof(snoop_access)/sizeof(const char *))
+
+static int hist_entry__snoop_snprintf(struct hist_entry *self, char *bf,
+				    size_t size, unsigned int width)
+{
+	char out[64];
+	size_t sz = sizeof(out) - 1; /* -1 for null termination */
+	size_t i, l = 0;
+	u64 m = PERF_MEM_SNOOP_NA;
+
+	out[0] = '\0';
+
+	if (self->mem_info)
+		m = self->mem_info->data_src.mem_snoop;
+
+	for (i = 0; m && i < NUM_SNOOP_ACCESS; i++, m >>= 1) {
+		if (!(m & 0x1))
+			continue;
+		if (l) {
+			strcat(out, " or ");
+			l += 4;
+		}
+		strncat(out, snoop_access[i], sz - l);
+		l += strlen(snoop_access[i]);
+	}
+
+	if (*out == '\0')
+		strcpy(out, "N/A");
+
+	return repsep_snprintf(bf, size, "%-*s", width, out);
+}
+
 struct sort_entry sort_mispredict = {
 	.se_header	= "Branch Mispredicted",
 	.se_cmp		= sort__mispredict_cmp,
@@ -464,6 +770,91 @@ struct sort_entry sort_mispredict = {
 	.se_width_idx	= HISTC_MISPREDICT,
 };
 
+static u64 he_weight(struct hist_entry *he)
+{
+	return he->stat.nr_events ? he->stat.weight / he->stat.nr_events : 0;
+}
+
+static int64_t
+sort__local_weight_cmp(struct hist_entry *left, struct hist_entry *right)
+{
+	return he_weight(left) - he_weight(right);
+}
+
+static int hist_entry__local_weight_snprintf(struct hist_entry *self, char *bf,
+				    size_t size, unsigned int width)
+{
+	return repsep_snprintf(bf, size, "%-*llu", width, he_weight(self));
+}
+
+struct sort_entry sort_local_weight = {
+	.se_header	= "Local Weight",
+	.se_cmp		= sort__local_weight_cmp,
+	.se_snprintf	= hist_entry__local_weight_snprintf,
+	.se_width_idx	= HISTC_LOCAL_WEIGHT,
+};
+
+static int64_t
+sort__global_weight_cmp(struct hist_entry *left, struct hist_entry *right)
+{
+	return left->stat.weight - right->stat.weight;
+}
+
+static int hist_entry__global_weight_snprintf(struct hist_entry *self, char *bf,
+					      size_t size, unsigned int width)
+{
+	return repsep_snprintf(bf, size, "%-*llu", width, self->stat.weight);
+}
+
+struct sort_entry sort_global_weight = {
+	.se_header	= "Weight",
+	.se_cmp		= sort__global_weight_cmp,
+	.se_snprintf	= hist_entry__global_weight_snprintf,
+	.se_width_idx	= HISTC_GLOBAL_WEIGHT,
+};
+
+struct sort_entry sort_mem_daddr_sym = {
+	.se_header	= "Data Symbol",
+	.se_cmp		= sort__daddr_cmp,
+	.se_snprintf	= hist_entry__daddr_snprintf,
+	.se_width_idx	= HISTC_MEM_DADDR_SYMBOL,
+};
+
+struct sort_entry sort_mem_daddr_dso = {
+	.se_header	= "Data Object",
+	.se_cmp		= sort__dso_daddr_cmp,
+	.se_snprintf	= hist_entry__dso_daddr_snprintf,
+	.se_width_idx	= HISTC_MEM_DADDR_SYMBOL,
+};
+
+struct sort_entry sort_mem_locked = {
+	.se_header	= "Locked",
+	.se_cmp		= sort__locked_cmp,
+	.se_snprintf	= hist_entry__locked_snprintf,
+	.se_width_idx	= HISTC_MEM_LOCKED,
+};
+
+struct sort_entry sort_mem_tlb = {
+	.se_header	= "TLB access",
+	.se_cmp		= sort__tlb_cmp,
+	.se_snprintf	= hist_entry__tlb_snprintf,
+	.se_width_idx	= HISTC_MEM_TLB,
+};
+
+struct sort_entry sort_mem_lvl = {
+	.se_header	= "Memory access",
+	.se_cmp		= sort__lvl_cmp,
+	.se_snprintf	= hist_entry__lvl_snprintf,
+	.se_width_idx	= HISTC_MEM_LVL,
+};
+
+struct sort_entry sort_mem_snoop = {
+	.se_header	= "Snoop",
+	.se_cmp		= sort__snoop_cmp,
+	.se_snprintf	= hist_entry__snoop_snprintf,
+	.se_width_idx	= HISTC_MEM_SNOOP,
+};
+
 struct sort_dimension {
 	const char		*name;
 	struct sort_entry	*entry;
@@ -480,6 +871,14 @@ static struct sort_dimension common_sort_dimensions[] = {
 	DIM(SORT_PARENT, "parent", sort_parent),
 	DIM(SORT_CPU, "cpu", sort_cpu),
 	DIM(SORT_SRCLINE, "srcline", sort_srcline),
+	DIM(SORT_LOCAL_WEIGHT, "local_weight", sort_local_weight),
+	DIM(SORT_GLOBAL_WEIGHT, "weight", sort_global_weight),
+	DIM(SORT_MEM_DADDR_SYMBOL, "symbol_daddr", sort_mem_daddr_sym),
+	DIM(SORT_MEM_DADDR_DSO, "dso_daddr", sort_mem_daddr_dso),
+	DIM(SORT_MEM_LOCKED, "locked", sort_mem_locked),
+	DIM(SORT_MEM_TLB, "tlb", sort_mem_tlb),
+	DIM(SORT_MEM_LVL, "mem", sort_mem_lvl),
+	DIM(SORT_MEM_SNOOP, "snoop", sort_mem_snoop),
 };
 
 #undef DIM
@@ -516,7 +915,10 @@ int sort_dimension__add(const char *tok)
 				return -EINVAL;
 			}
 			sort__has_parent = 1;
-		} else if (sd->entry == &sort_sym) {
+		} else if (sd->entry == &sort_sym ||
+			   sd->entry == &sort_sym_from ||
+			   sd->entry == &sort_sym_to ||
+			   sd->entry == &sort_mem_daddr_sym) {
 			sort__has_sym = 1;
 		}
 
diff --git a/tools/perf/util/sort.h b/tools/perf/util/sort.h
index b13e56f6ccb..f24bdf64238 100644
--- a/tools/perf/util/sort.h
+++ b/tools/perf/util/sort.h
@@ -49,6 +49,7 @@ struct he_stat {
 	u64			period_us;
 	u64			period_guest_sys;
 	u64			period_guest_us;
+	u64			weight;
 	u32			nr_events;
 };
 
@@ -100,7 +101,8 @@ struct hist_entry {
 	struct rb_root		sorted_chain;
 	struct branch_info	*branch_info;
 	struct hists		*hists;
-	struct callchain_root	callchain[0];
+	struct mem_info		*mem_info;
+	struct callchain_root	callchain[0]; /* must be last member */
 };
 
 static inline bool hist_entry__has_pairs(struct hist_entry *he)
@@ -130,6 +132,14 @@ enum sort_type {
 	SORT_PARENT,
 	SORT_CPU,
 	SORT_SRCLINE,
+	SORT_LOCAL_WEIGHT,
+	SORT_GLOBAL_WEIGHT,
+	SORT_MEM_DADDR_SYMBOL,
+	SORT_MEM_DADDR_DSO,
+	SORT_MEM_LOCKED,
+	SORT_MEM_TLB,
+	SORT_MEM_LVL,
+	SORT_MEM_SNOOP,
 
 	/* branch stack specific sort keys */
 	__SORT_BRANCH_STACK,
diff --git a/tools/perf/util/symbol-elf.c b/tools/perf/util/symbol-elf.c
index 54efcb5659a..4b12bf85032 100644
--- a/tools/perf/util/symbol-elf.c
+++ b/tools/perf/util/symbol-elf.c
@@ -806,9 +806,12 @@ int dso__load_sym(struct dso *dso, struct map *map,
 		 * DWARF DW_compile_unit has this, but we don't always have access
 		 * to it...
 		 */
-		demangled = bfd_demangle(NULL, elf_name, DMGL_PARAMS | DMGL_ANSI);
-		if (demangled != NULL)
-			elf_name = demangled;
+		if (symbol_conf.demangle) {
+			demangled = bfd_demangle(NULL, elf_name,
+						 DMGL_PARAMS | DMGL_ANSI);
+			if (demangled != NULL)
+				elf_name = demangled;
+		}
 new_symbol:
 		f = symbol__new(sym.st_value, sym.st_size,
 				GELF_ST_BIND(sym.st_info), elf_name);
diff --git a/tools/perf/util/symbol.c b/tools/perf/util/symbol.c
index e6432d85b43..8cf3b5426a9 100644
--- a/tools/perf/util/symbol.c
+++ b/tools/perf/util/symbol.c
@@ -36,6 +36,7 @@ struct symbol_conf symbol_conf = {
 	.use_modules	  = true,
 	.try_vmlinux_path = true,
 	.annotate_src	  = true,
+	.demangle	  = true,
 	.symfs            = "",
 };
 
diff --git a/tools/perf/util/symbol.h b/tools/perf/util/symbol.h
index b62ca37c4b7..5f720dc076d 100644
--- a/tools/perf/util/symbol.h
+++ b/tools/perf/util/symbol.h
@@ -97,7 +97,8 @@ struct symbol_conf {
 			kptr_restrict,
 			annotate_asm_raw,
 			annotate_src,
-			event_group;
+			event_group,
+			demangle;
 	const char	*vmlinux_name,
 			*kallsyms_name,
 			*source_prefix,
@@ -155,6 +156,12 @@ struct branch_info {
 	struct branch_flags flags;
 };
 
+struct mem_info {
+	struct addr_map_symbol iaddr;
+	struct addr_map_symbol daddr;
+	union perf_mem_data_src data_src;
+};
+
 struct addr_location {
 	struct thread *thread;
 	struct map    *map;
diff --git a/tools/perf/util/thread_map.h b/tools/perf/util/thread_map.h
index f718df8a3c5..0cd8b310808 100644
--- a/tools/perf/util/thread_map.h
+++ b/tools/perf/util/thread_map.h
@@ -21,4 +21,9 @@ void thread_map__delete(struct thread_map *threads);
 
 size_t thread_map__fprintf(struct thread_map *threads, FILE *fp);
 
+static inline int thread_map__nr(struct thread_map *threads)
+{
+	return threads ? threads->nr : 1;
+}
+
 #endif	/* __PERF_THREAD_MAP_H */
diff --git a/tools/perf/util/trace-event-info.c b/tools/perf/util/trace-event-info.c
index a8d81c35ef6..3917eb9a847 100644
--- a/tools/perf/util/trace-event-info.c
+++ b/tools/perf/util/trace-event-info.c
@@ -38,52 +38,20 @@
 
 #include "../perf.h"
 #include "trace-event.h"
-#include "debugfs.h"
+#include <lk/debugfs.h>
 #include "evsel.h"
 
 #define VERSION "0.5"
 
-#define TRACE_CTRL	"tracing_on"
-#define TRACE		"trace"
-#define AVAILABLE	"available_tracers"
-#define CURRENT		"current_tracer"
-#define ITER_CTRL	"trace_options"
-#define MAX_LATENCY	"tracing_max_latency"
-
-unsigned int page_size;
-
-static const char *output_file = "trace.info";
 static int output_fd;
 
-struct event_list {
-	struct event_list *next;
-	const char *event;
-};
-
-struct events {
-	struct events *sibling;
-	struct events *children;
-	struct events *next;
-	char *name;
-};
-
-
-static void *malloc_or_die(unsigned int size)
-{
-	void *data;
-
-	data = malloc(size);
-	if (!data)
-		die("malloc");
-	return data;
-}
 
 static const char *find_debugfs(void)
 {
-	const char *path = debugfs_mount(NULL);
+	const char *path = perf_debugfs_mount(NULL);
 
 	if (!path)
-		die("Your kernel not support debugfs filesystem");
+		pr_debug("Your kernel does not support the debugfs filesystem");
 
 	return path;
 }
@@ -102,8 +70,12 @@ static const char *find_tracing_dir(void)
 		return tracing;
 
 	debugfs = find_debugfs();
+	if (!debugfs)
+		return NULL;
 
-	tracing = malloc_or_die(strlen(debugfs) + 9);
+	tracing = malloc(strlen(debugfs) + 9);
+	if (!tracing)
+		return NULL;
 
 	sprintf(tracing, "%s/tracing", debugfs);
 
@@ -120,7 +92,9 @@ static char *get_tracing_file(const char *name)
 	if (!tracing)
 		return NULL;
 
-	file = malloc_or_die(strlen(tracing) + strlen(name) + 2);
+	file = malloc(strlen(tracing) + strlen(name) + 2);
+	if (!file)
+		return NULL;
 
 	sprintf(file, "%s/%s", tracing, name);
 	return file;
@@ -131,24 +105,6 @@ static void put_tracing_file(char *file)
 	free(file);
 }
 
-static ssize_t calc_data_size;
-
-static ssize_t write_or_die(const void *buf, size_t len)
-{
-	int ret;
-
-	if (calc_data_size) {
-		calc_data_size += len;
-		return len;
-	}
-
-	ret = write(output_fd, buf, len);
-	if (ret < 0)
-		die("writing to '%s'", output_file);
-
-	return ret;
-}
-
 int bigendian(void)
 {
 	unsigned char str[] = { 0x1, 0x2, 0x3, 0x4, 0x0, 0x0, 0x0, 0x0};
@@ -159,59 +115,106 @@ int bigendian(void)
 }
 
 /* unfortunately, you can not stat debugfs or proc files for size */
-static void record_file(const char *file, size_t hdr_sz)
+static int record_file(const char *file, ssize_t hdr_sz)
 {
 	unsigned long long size = 0;
 	char buf[BUFSIZ], *sizep;
 	off_t hdr_pos = lseek(output_fd, 0, SEEK_CUR);
 	int r, fd;
+	int err = -EIO;
 
 	fd = open(file, O_RDONLY);
-	if (fd < 0)
-		die("Can't read '%s'", file);
+	if (fd < 0) {
+		pr_debug("Can't read '%s'", file);
+		return -errno;
+	}
 
 	/* put in zeros for file size, then fill true size later */
-	if (hdr_sz)
-		write_or_die(&size, hdr_sz);
+	if (hdr_sz) {
+		if (write(output_fd, &size, hdr_sz) != hdr_sz)
+			goto out;
+	}
 
 	do {
 		r = read(fd, buf, BUFSIZ);
 		if (r > 0) {
 			size += r;
-			write_or_die(buf, r);
+			if (write(output_fd, buf, r) != r)
+				goto out;
 		}
 	} while (r > 0);
-	close(fd);
 
 	/* ugh, handle big-endian hdr_size == 4 */
 	sizep = (char*)&size;
 	if (bigendian())
 		sizep += sizeof(u64) - hdr_sz;
 
-	if (hdr_sz && pwrite(output_fd, sizep, hdr_sz, hdr_pos) < 0)
-		die("writing to %s", output_file);
+	if (hdr_sz && pwrite(output_fd, sizep, hdr_sz, hdr_pos) < 0) {
+		pr_debug("writing file size failed\n");
+		goto out;
+	}
+
+	err = 0;
+out:
+	close(fd);
+	return err;
 }
 
-static void read_header_files(void)
+static int read_header_files(void)
 {
 	char *path;
 	struct stat st;
+	int err = -EIO;
 
 	path = get_tracing_file("events/header_page");
-	if (stat(path, &st) < 0)
-		die("can't read '%s'", path);
+	if (!path) {
+		pr_debug("can't get tracing/events/header_page");
+		return -ENOMEM;
+	}
+
+	if (stat(path, &st) < 0) {
+		pr_debug("can't read '%s'", path);
+		goto out;
+	}
+
+	if (write(output_fd, "header_page", 12) != 12) {
+		pr_debug("can't write header_page\n");
+		goto out;
+	}
+
+	if (record_file(path, 8) < 0) {
+		pr_debug("can't record header_page file\n");
+		goto out;
+	}
 
-	write_or_die("header_page", 12);
-	record_file(path, 8);
 	put_tracing_file(path);
 
 	path = get_tracing_file("events/header_event");
-	if (stat(path, &st) < 0)
-		die("can't read '%s'", path);
+	if (!path) {
+		pr_debug("can't get tracing/events/header_event");
+		err = -ENOMEM;
+		goto out;
+	}
+
+	if (stat(path, &st) < 0) {
+		pr_debug("can't read '%s'", path);
+		goto out;
+	}
 
-	write_or_die("header_event", 13);
-	record_file(path, 8);
+	if (write(output_fd, "header_event", 13) != 13) {
+		pr_debug("can't write header_event\n");
+		goto out;
+	}
+
+	if (record_file(path, 8) < 0) {
+		pr_debug("can't record header_event file\n");
+		goto out;
+	}
+
+	err = 0;
+out:
 	put_tracing_file(path);
+	return err;
 }
 
 static bool name_in_tp_list(char *sys, struct tracepoint_path *tps)
@@ -225,7 +228,7 @@ static bool name_in_tp_list(char *sys, struct tracepoint_path *tps)
 	return false;
 }
 
-static void copy_event_system(const char *sys, struct tracepoint_path *tps)
+static int copy_event_system(const char *sys, struct tracepoint_path *tps)
 {
 	struct dirent *dent;
 	struct stat st;
@@ -233,10 +236,13 @@ static void copy_event_system(const char *sys, struct tracepoint_path *tps)
 	DIR *dir;
 	int count = 0;
 	int ret;
+	int err;
 
 	dir = opendir(sys);
-	if (!dir)
-		die("can't read directory '%s'", sys);
+	if (!dir) {
+		pr_debug("can't read directory '%s'", sys);
+		return -errno;
+	}
 
 	while ((dent = readdir(dir))) {
 		if (dent->d_type != DT_DIR ||
@@ -244,7 +250,11 @@ static void copy_event_system(const char *sys, struct tracepoint_path *tps)
 		    strcmp(dent->d_name, "..") == 0 ||
 		    !name_in_tp_list(dent->d_name, tps))
 			continue;
-		format = malloc_or_die(strlen(sys) + strlen(dent->d_name) + 10);
+		format = malloc(strlen(sys) + strlen(dent->d_name) + 10);
+		if (!format) {
+			err = -ENOMEM;
+			goto out;
+		}
 		sprintf(format, "%s/%s/format", sys, dent->d_name);
 		ret = stat(format, &st);
 		free(format);
@@ -253,7 +263,11 @@ static void copy_event_system(const char *sys, struct tracepoint_path *tps)
 		count++;
 	}
 
-	write_or_die(&count, 4);
+	if (write(output_fd, &count, 4) != 4) {
+		err = -EIO;
+		pr_debug("can't write count\n");
+		goto out;
+	}
 
 	rewinddir(dir);
 	while ((dent = readdir(dir))) {
@@ -262,27 +276,45 @@ static void copy_event_system(const char *sys, struct tracepoint_path *tps)
 		    strcmp(dent->d_name, "..") == 0 ||
 		    !name_in_tp_list(dent->d_name, tps))
 			continue;
-		format = malloc_or_die(strlen(sys) + strlen(dent->d_name) + 10);
+		format = malloc(strlen(sys) + strlen(dent->d_name) + 10);
+		if (!format) {
+			err = -ENOMEM;
+			goto out;
+		}
 		sprintf(format, "%s/%s/format", sys, dent->d_name);
 		ret = stat(format, &st);
 
-		if (ret >= 0)
-			record_file(format, 8);
-
+		if (ret >= 0) {
+			err = record_file(format, 8);
+			if (err) {
+				free(format);
+				goto out;
+			}
+		}
 		free(format);
 	}
+	err = 0;
+out:
 	closedir(dir);
+	return err;
 }
 
-static void read_ftrace_files(struct tracepoint_path *tps)
+static int read_ftrace_files(struct tracepoint_path *tps)
 {
 	char *path;
+	int ret;
 
 	path = get_tracing_file("events/ftrace");
+	if (!path) {
+		pr_debug("can't get tracing/events/ftrace");
+		return -ENOMEM;
+	}
 
-	copy_event_system(path, tps);
+	ret = copy_event_system(path, tps);
 
 	put_tracing_file(path);
+
+	return ret;
 }
 
 static bool system_in_tp_list(char *sys, struct tracepoint_path *tps)
@@ -296,7 +328,7 @@ static bool system_in_tp_list(char *sys, struct tracepoint_path *tps)
 	return false;
 }
 
-static void read_event_files(struct tracepoint_path *tps)
+static int read_event_files(struct tracepoint_path *tps)
 {
 	struct dirent *dent;
 	struct stat st;
@@ -305,12 +337,20 @@ static void read_event_files(struct tracepoint_path *tps)
 	DIR *dir;
 	int count = 0;
 	int ret;
+	int err;
 
 	path = get_tracing_file("events");
+	if (!path) {
+		pr_debug("can't get tracing/events");
+		return -ENOMEM;
+	}
 
 	dir = opendir(path);
-	if (!dir)
-		die("can't read directory '%s'", path);
+	if (!dir) {
+		err = -errno;
+		pr_debug("can't read directory '%s'", path);
+		goto out;
+	}
 
 	while ((dent = readdir(dir))) {
 		if (dent->d_type != DT_DIR ||
@@ -322,7 +362,11 @@ static void read_event_files(struct tracepoint_path *tps)
 		count++;
 	}
 
-	write_or_die(&count, 4);
+	if (write(output_fd, &count, 4) != 4) {
+		err = -EIO;
+		pr_debug("can't write count\n");
+		goto out;
+	}
 
 	rewinddir(dir);
 	while ((dent = readdir(dir))) {
@@ -332,56 +376,90 @@ static void read_event_files(struct tracepoint_path *tps)
 		    strcmp(dent->d_name, "ftrace") == 0 ||
 		    !system_in_tp_list(dent->d_name, tps))
 			continue;
-		sys = malloc_or_die(strlen(path) + strlen(dent->d_name) + 2);
+		sys = malloc(strlen(path) + strlen(dent->d_name) + 2);
+		if (!sys) {
+			err = -ENOMEM;
+			goto out;
+		}
 		sprintf(sys, "%s/%s", path, dent->d_name);
 		ret = stat(sys, &st);
 		if (ret >= 0) {
-			write_or_die(dent->d_name, strlen(dent->d_name) + 1);
-			copy_event_system(sys, tps);
+			ssize_t size = strlen(dent->d_name) + 1;
+
+			if (write(output_fd, dent->d_name, size) != size ||
+			    copy_event_system(sys, tps) < 0) {
+				err = -EIO;
+				free(sys);
+				goto out;
+			}
 		}
 		free(sys);
 	}
-
+	err = 0;
+out:
 	closedir(dir);
 	put_tracing_file(path);
+
+	return err;
 }
 
-static void read_proc_kallsyms(void)
+static int read_proc_kallsyms(void)
 {
 	unsigned int size;
 	const char *path = "/proc/kallsyms";
 	struct stat st;
-	int ret;
+	int ret, err = 0;
 
 	ret = stat(path, &st);
 	if (ret < 0) {
 		/* not found */
 		size = 0;
-		write_or_die(&size, 4);
-		return;
+		if (write(output_fd, &size, 4) != 4)
+			err = -EIO;
+		return err;
 	}
-	record_file(path, 4);
+	return record_file(path, 4);
 }
 
-static void read_ftrace_printk(void)
+static int read_ftrace_printk(void)
 {
 	unsigned int size;
 	char *path;
 	struct stat st;
-	int ret;
+	int ret, err = 0;
 
 	path = get_tracing_file("printk_formats");
+	if (!path) {
+		pr_debug("can't get tracing/printk_formats");
+		return -ENOMEM;
+	}
+
 	ret = stat(path, &st);
 	if (ret < 0) {
 		/* not found */
 		size = 0;
-		write_or_die(&size, 4);
+		if (write(output_fd, &size, 4) != 4)
+			err = -EIO;
 		goto out;
 	}
-	record_file(path, 4);
+	err = record_file(path, 4);
 
 out:
 	put_tracing_file(path);
+	return err;
+}
+
+static void
+put_tracepoints_path(struct tracepoint_path *tps)
+{
+	while (tps) {
+		struct tracepoint_path *t = tps;
+
+		tps = tps->next;
+		free(t->name);
+		free(t->system);
+		free(t);
+	}
 }
 
 static struct tracepoint_path *
@@ -396,27 +474,17 @@ get_tracepoints_path(struct list_head *pattrs)
 			continue;
 		++nr_tracepoints;
 		ppath->next = tracepoint_id_to_path(pos->attr.config);
-		if (!ppath->next)
-			die("%s\n", "No memory to alloc tracepoints list");
+		if (!ppath->next) {
+			pr_debug("No memory to alloc tracepoints list\n");
+			put_tracepoints_path(&path);
+			return NULL;
+		}
 		ppath = ppath->next;
 	}
 
 	return nr_tracepoints > 0 ? path.next : NULL;
 }
 
-static void
-put_tracepoints_path(struct tracepoint_path *tps)
-{
-	while (tps) {
-		struct tracepoint_path *t = tps;
-
-		tps = tps->next;
-		free(t->name);
-		free(t->system);
-		free(t);
-	}
-}
-
 bool have_tracepoints(struct list_head *pattrs)
 {
 	struct perf_evsel *pos;
@@ -428,9 +496,10 @@ bool have_tracepoints(struct list_head *pattrs)
 	return false;
 }
 
-static void tracing_data_header(void)
+static int tracing_data_header(void)
 {
 	char buf[20];
+	ssize_t size;
 
 	/* just guessing this is someone's birthday.. ;) */
 	buf[0] = 23;
@@ -438,9 +507,12 @@ static void tracing_data_header(void)
 	buf[2] = 68;
 	memcpy(buf + 3, "tracing", 7);
 
-	write_or_die(buf, 10);
+	if (write(output_fd, buf, 10) != 10)
+		return -1;
 
-	write_or_die(VERSION, strlen(VERSION) + 1);
+	size = strlen(VERSION) + 1;
+	if (write(output_fd, VERSION, size) != size)
+		return -1;
 
 	/* save endian */
 	if (bigendian())
@@ -450,15 +522,19 @@ static void tracing_data_header(void)
 
 	read_trace_init(buf[0], buf[0]);
 
-	write_or_die(buf, 1);
+	if (write(output_fd, buf, 1) != 1)
+		return -1;
 
 	/* save size of long */
 	buf[0] = sizeof(long);
-	write_or_die(buf, 1);
+	if (write(output_fd, buf, 1) != 1)
+		return -1;
 
 	/* save page_size */
-	page_size = sysconf(_SC_PAGESIZE);
-	write_or_die(&page_size, 4);
+	if (write(output_fd, &page_size, 4) != 4)
+		return -1;
+
+	return 0;
 }
 
 struct tracing_data *tracing_data_get(struct list_head *pattrs,
@@ -466,6 +542,7 @@ struct tracing_data *tracing_data_get(struct list_head *pattrs,
 {
 	struct tracepoint_path *tps;
 	struct tracing_data *tdata;
+	int err;
 
 	output_fd = fd;
 
@@ -473,7 +550,10 @@ struct tracing_data *tracing_data_get(struct list_head *pattrs,
 	if (!tps)
 		return NULL;
 
-	tdata = malloc_or_die(sizeof(*tdata));
+	tdata = malloc(sizeof(*tdata));
+	if (!tdata)
+		return NULL;
+
 	tdata->temp = temp;
 	tdata->size = 0;
 
@@ -482,12 +562,16 @@ struct tracing_data *tracing_data_get(struct list_head *pattrs,
 
 		snprintf(tdata->temp_file, sizeof(tdata->temp_file),
 			 "/tmp/perf-XXXXXX");
-		if (!mkstemp(tdata->temp_file))
-			die("Can't make temp file");
+		if (!mkstemp(tdata->temp_file)) {
+			pr_debug("Can't make temp file");
+			return NULL;
+		}
 
 		temp_fd = open(tdata->temp_file, O_RDWR);
-		if (temp_fd < 0)
-			die("Can't read '%s'", tdata->temp_file);
+		if (temp_fd < 0) {
+			pr_debug("Can't read '%s'", tdata->temp_file);
+			return NULL;
+		}
 
 		/*
 		 * Set the temp file the default output, so all the
@@ -496,13 +580,24 @@ struct tracing_data *tracing_data_get(struct list_head *pattrs,
 		output_fd = temp_fd;
 	}
 
-	tracing_data_header();
-	read_header_files();
-	read_ftrace_files(tps);
-	read_event_files(tps);
-	read_proc_kallsyms();
-	read_ftrace_printk();
+	err = tracing_data_header();
+	if (err)
+		goto out;
+	err = read_header_files();
+	if (err)
+		goto out;
+	err = read_ftrace_files(tps);
+	if (err)
+		goto out;
+	err = read_event_files(tps);
+	if (err)
+		goto out;
+	err = read_proc_kallsyms();
+	if (err)
+		goto out;
+	err = read_ftrace_printk();
 
+out:
 	/*
 	 * All tracing data are stored by now, we can restore
 	 * the default output file in case we used temp file.
@@ -513,22 +608,31 @@ struct tracing_data *tracing_data_get(struct list_head *pattrs,
 		output_fd = fd;
 	}
 
+	if (err) {
+		free(tdata);
+		tdata = NULL;
+	}
+
 	put_tracepoints_path(tps);
 	return tdata;
 }
 
-void tracing_data_put(struct tracing_data *tdata)
+int tracing_data_put(struct tracing_data *tdata)
 {
+	int err = 0;
+
 	if (tdata->temp) {
-		record_file(tdata->temp_file, 0);
+		err = record_file(tdata->temp_file, 0);
 		unlink(tdata->temp_file);
 	}
 
 	free(tdata);
+	return err;
 }
 
 int read_tracing_data(int fd, struct list_head *pattrs)
 {
+	int err;
 	struct tracing_data *tdata;
 
 	/*
@@ -539,6 +643,6 @@ int read_tracing_data(int fd, struct list_head *pattrs)
 	if (!tdata)
 		return -ENOMEM;
 
-	tracing_data_put(tdata);
-	return 0;
+	err = tracing_data_put(tdata);
+	return err;
 }
diff --git a/tools/perf/util/trace-event-parse.c b/tools/perf/util/trace-event-parse.c
index 3aabcd687cd..4454835a9eb 100644
--- a/tools/perf/util/trace-event-parse.c
+++ b/tools/perf/util/trace-event-parse.c
@@ -183,43 +183,6 @@ void event_format__print(struct event_format *event,
 	trace_seq_do_printf(&s);
 }
 
-void print_trace_event(struct pevent *pevent, int cpu, void *data, int size)
-{
-	int type = trace_parse_common_type(pevent, data);
-	struct event_format *event = pevent_find_event(pevent, type);
-
-	if (!event) {
-		warning("ug! no event found for type %d", type);
-		return;
-	}
-
-	event_format__print(event, cpu, data, size);
-}
-
-void print_event(struct pevent *pevent, int cpu, void *data, int size,
-		 unsigned long long nsecs, char *comm)
-{
-	struct pevent_record record;
-	struct trace_seq s;
-	int pid;
-
-	pevent->latency_format = latency_format;
-
-	record.ts = nsecs;
-	record.cpu = cpu;
-	record.size = size;
-	record.data = data;
-	pid = pevent_data_pid(pevent, &record);
-
-	if (!pevent_pid_is_registered(pevent, pid))
-		pevent_register_comm(pevent, comm, pid);
-
-	trace_seq_init(&s);
-	pevent_print_event(pevent, &s, &record);
-	trace_seq_do_printf(&s);
-	printf("\n");
-}
-
 void parse_proc_kallsyms(struct pevent *pevent,
 			 char *file, unsigned int size __maybe_unused)
 {
diff --git a/tools/perf/util/trace-event-read.c b/tools/perf/util/trace-event-read.c
index 3741572696a..af215c0d237 100644
--- a/tools/perf/util/trace-event-read.c
+++ b/tools/perf/util/trace-event-read.c
@@ -18,8 +18,6 @@
  *
  * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  */
-#define _FILE_OFFSET_BITS 64
-
 #include <dirent.h>
 #include <stdio.h>
 #include <stdlib.h>
@@ -41,26 +39,14 @@
 
 static int input_fd;
 
-static int read_page;
-
 int file_bigendian;
 int host_bigendian;
 static int long_size;
 
-static ssize_t calc_data_size;
+static ssize_t trace_data_size;
 static bool repipe;
 
-static void *malloc_or_die(int size)
-{
-	void *ret;
-
-	ret = malloc(size);
-	if (!ret)
-		die("malloc");
-	return ret;
-}
-
-static int do_read(int fd, void *buf, int size)
+static int __do_read(int fd, void *buf, int size)
 {
 	int rsize = size;
 
@@ -73,8 +59,10 @@ static int do_read(int fd, void *buf, int size)
 		if (repipe) {
 			int retw = write(STDOUT_FILENO, buf, ret);
 
-			if (retw <= 0 || retw != ret)
-				die("repiping input file");
+			if (retw <= 0 || retw != ret) {
+				pr_debug("repiping input file");
+				return -1;
+			}
 		}
 
 		size -= ret;
@@ -84,17 +72,18 @@ static int do_read(int fd, void *buf, int size)
 	return rsize;
 }
 
-static int read_or_die(void *data, int size)
+static int do_read(void *data, int size)
 {
 	int r;
 
-	r = do_read(input_fd, data, size);
-	if (r <= 0)
-		die("reading input file (size expected=%d received=%d)",
-		    size, r);
+	r = __do_read(input_fd, data, size);
+	if (r <= 0) {
+		pr_debug("reading input file (size expected=%d received=%d)",
+			 size, r);
+		return -1;
+	}
 
-	if (calc_data_size)
-		calc_data_size += r;
+	trace_data_size += r;
 
 	return r;
 }
@@ -107,7 +96,7 @@ static void skip(int size)
 
 	while (size) {
 		r = size > BUFSIZ ? BUFSIZ : size;
-		read_or_die(buf, r);
+		do_read(buf, r);
 		size -= r;
 	};
 }
@@ -116,7 +105,8 @@ static unsigned int read4(struct pevent *pevent)
 {
 	unsigned int data;
 
-	read_or_die(&data, 4);
+	if (do_read(&data, 4) < 0)
+		return 0;
 	return __data2host4(pevent, data);
 }
 
@@ -124,7 +114,8 @@ static unsigned long long read8(struct pevent *pevent)
 {
 	unsigned long long data;
 
-	read_or_die(&data, 8);
+	if (do_read(&data, 8) < 0)
+		return 0;
 	return __data2host8(pevent, data);
 }
 
@@ -138,17 +129,23 @@ static char *read_string(void)
 
 	for (;;) {
 		r = read(input_fd, &c, 1);
-		if (r < 0)
-			die("reading input file");
+		if (r < 0) {
+			pr_debug("reading input file");
+			goto out;
+		}
 
-		if (!r)
-			die("no data");
+		if (!r) {
+			pr_debug("no data");
+			goto out;
+		}
 
 		if (repipe) {
 			int retw = write(STDOUT_FILENO, &c, 1);
 
-			if (retw <= 0 || retw != r)
-				die("repiping input file string");
+			if (retw <= 0 || retw != r) {
+				pr_debug("repiping input file string");
+				goto out;
+			}
 		}
 
 		buf[size++] = c;
@@ -157,60 +154,79 @@ static char *read_string(void)
 			break;
 	}
 
-	if (calc_data_size)
-		calc_data_size += size;
-
-	str = malloc_or_die(size);
-	memcpy(str, buf, size);
+	trace_data_size += size;
 
+	str = malloc(size);
+	if (str)
+		memcpy(str, buf, size);
+out:
 	return str;
 }
 
-static void read_proc_kallsyms(struct pevent *pevent)
+static int read_proc_kallsyms(struct pevent *pevent)
 {
 	unsigned int size;
 	char *buf;
 
 	size = read4(pevent);
 	if (!size)
-		return;
+		return 0;
+
+	buf = malloc(size + 1);
+	if (buf == NULL)
+		return -1;
 
-	buf = malloc_or_die(size + 1);
-	read_or_die(buf, size);
+	if (do_read(buf, size) < 0) {
+		free(buf);
+		return -1;
+	}
 	buf[size] = '\0';
 
 	parse_proc_kallsyms(pevent, buf, size);
 
 	free(buf);
+	return 0;
 }
 
-static void read_ftrace_printk(struct pevent *pevent)
+static int read_ftrace_printk(struct pevent *pevent)
 {
 	unsigned int size;
 	char *buf;
 
+	/* it can have 0 size */
 	size = read4(pevent);
 	if (!size)
-		return;
+		return 0;
+
+	buf = malloc(size);
+	if (buf == NULL)
+		return -1;
 
-	buf = malloc_or_die(size);
-	read_or_die(buf, size);
+	if (do_read(buf, size) < 0) {
+		free(buf);
+		return -1;
+	}
 
 	parse_ftrace_printk(pevent, buf, size);
 
 	free(buf);
+	return 0;
 }
 
-static void read_header_files(struct pevent *pevent)
+static int read_header_files(struct pevent *pevent)
 {
 	unsigned long long size;
 	char *header_event;
 	char buf[BUFSIZ];
+	int ret = 0;
 
-	read_or_die(buf, 12);
+	if (do_read(buf, 12) < 0)
+		return -1;
 
-	if (memcmp(buf, "header_page", 12) != 0)
-		die("did not read header page");
+	if (memcmp(buf, "header_page", 12) != 0) {
+		pr_debug("did not read header page");
+		return -1;
+	}
 
 	size = read8(pevent);
 	skip(size);
@@ -221,269 +237,107 @@ static void read_header_files(struct pevent *pevent)
 	 */
 	long_size = header_page_size_size;
 
-	read_or_die(buf, 13);
-	if (memcmp(buf, "header_event", 13) != 0)
-		die("did not read header event");
+	if (do_read(buf, 13) < 0)
+		return -1;
+
+	if (memcmp(buf, "header_event", 13) != 0) {
+		pr_debug("did not read header event");
+		return -1;
+	}
 
 	size = read8(pevent);
-	header_event = malloc_or_die(size);
-	read_or_die(header_event, size);
+	header_event = malloc(size);
+	if (header_event == NULL)
+		return -1;
+
+	if (do_read(header_event, size) < 0)
+		ret = -1;
+
 	free(header_event);
+	return ret;
 }
 
-static void read_ftrace_file(struct pevent *pevent, unsigned long long size)
+static int read_ftrace_file(struct pevent *pevent, unsigned long long size)
 {
 	char *buf;
 
-	buf = malloc_or_die(size);
-	read_or_die(buf, size);
+	buf = malloc(size);
+	if (buf == NULL)
+		return -1;
+
+	if (do_read(buf, size) < 0) {
+		free(buf);
+		return -1;
+	}
+
 	parse_ftrace_file(pevent, buf, size);
 	free(buf);
+	return 0;
 }
 
-static void read_event_file(struct pevent *pevent, char *sys,
+static int read_event_file(struct pevent *pevent, char *sys,
 			    unsigned long long size)
 {
 	char *buf;
 
-	buf = malloc_or_die(size);
-	read_or_die(buf, size);
+	buf = malloc(size);
+	if (buf == NULL)
+		return -1;
+
+	if (do_read(buf, size) < 0) {
+		free(buf);
+		return -1;
+	}
+
 	parse_event_file(pevent, buf, size, sys);
 	free(buf);
+	return 0;
 }
 
-static void read_ftrace_files(struct pevent *pevent)
+static int read_ftrace_files(struct pevent *pevent)
 {
 	unsigned long long size;
 	int count;
 	int i;
+	int ret;
 
 	count = read4(pevent);
 
 	for (i = 0; i < count; i++) {
 		size = read8(pevent);
-		read_ftrace_file(pevent, size);
+		ret = read_ftrace_file(pevent, size);
+		if (ret)
+			return ret;
 	}
+	return 0;
 }
 
-static void read_event_files(struct pevent *pevent)
+static int read_event_files(struct pevent *pevent)
 {
 	unsigned long long size;
 	char *sys;
 	int systems;
 	int count;
 	int i,x;
+	int ret;
 
 	systems = read4(pevent);
 
 	for (i = 0; i < systems; i++) {
 		sys = read_string();
+		if (sys == NULL)
+			return -1;
 
 		count = read4(pevent);
+
 		for (x=0; x < count; x++) {
 			size = read8(pevent);
-			read_event_file(pevent, sys, size);
+			ret = read_event_file(pevent, sys, size);
+			if (ret)
+				return ret;
 		}
 	}
-}
-
-struct cpu_data {
-	unsigned long long	offset;
-	unsigned long long	size;
-	unsigned long long	timestamp;
-	struct pevent_record	*next;
-	char			*page;
-	int			cpu;
-	int			index;
-	int			page_size;
-};
-
-static struct cpu_data *cpu_data;
-
-static void update_cpu_data_index(int cpu)
-{
-	cpu_data[cpu].offset += page_size;
-	cpu_data[cpu].size -= page_size;
-	cpu_data[cpu].index = 0;
-}
-
-static void get_next_page(int cpu)
-{
-	off_t save_seek;
-	off_t ret;
-
-	if (!cpu_data[cpu].page)
-		return;
-
-	if (read_page) {
-		if (cpu_data[cpu].size <= page_size) {
-			free(cpu_data[cpu].page);
-			cpu_data[cpu].page = NULL;
-			return;
-		}
-
-		update_cpu_data_index(cpu);
-
-		/* other parts of the code may expect the pointer to not move */
-		save_seek = lseek(input_fd, 0, SEEK_CUR);
-
-		ret = lseek(input_fd, cpu_data[cpu].offset, SEEK_SET);
-		if (ret == (off_t)-1)
-			die("failed to lseek");
-		ret = read(input_fd, cpu_data[cpu].page, page_size);
-		if (ret < 0)
-			die("failed to read page");
-
-		/* reset the file pointer back */
-		lseek(input_fd, save_seek, SEEK_SET);
-
-		return;
-	}
-
-	munmap(cpu_data[cpu].page, page_size);
-	cpu_data[cpu].page = NULL;
-
-	if (cpu_data[cpu].size <= page_size)
-		return;
-
-	update_cpu_data_index(cpu);
-
-	cpu_data[cpu].page = mmap(NULL, page_size, PROT_READ, MAP_PRIVATE,
-				  input_fd, cpu_data[cpu].offset);
-	if (cpu_data[cpu].page == MAP_FAILED)
-		die("failed to mmap cpu %d at offset 0x%llx",
-		    cpu, cpu_data[cpu].offset);
-}
-
-static unsigned int type_len4host(unsigned int type_len_ts)
-{
-	if (file_bigendian)
-		return (type_len_ts >> 27) & ((1 << 5) - 1);
-	else
-		return type_len_ts & ((1 << 5) - 1);
-}
-
-static unsigned int ts4host(unsigned int type_len_ts)
-{
-	if (file_bigendian)
-		return type_len_ts & ((1 << 27) - 1);
-	else
-		return type_len_ts >> 5;
-}
-
-static int calc_index(void *ptr, int cpu)
-{
-	return (unsigned long)ptr - (unsigned long)cpu_data[cpu].page;
-}
-
-struct pevent_record *trace_peek_data(struct pevent *pevent, int cpu)
-{
-	struct pevent_record *data;
-	void *page = cpu_data[cpu].page;
-	int idx = cpu_data[cpu].index;
-	void *ptr = page + idx;
-	unsigned long long extend;
-	unsigned int type_len_ts;
-	unsigned int type_len;
-	unsigned int delta;
-	unsigned int length = 0;
-
-	if (cpu_data[cpu].next)
-		return cpu_data[cpu].next;
-
-	if (!page)
-		return NULL;
-
-	if (!idx) {
-		/* FIXME: handle header page */
-		if (header_page_ts_size != 8)
-			die("expected a long long type for timestamp");
-		cpu_data[cpu].timestamp = data2host8(pevent, ptr);
-		ptr += 8;
-		switch (header_page_size_size) {
-		case 4:
-			cpu_data[cpu].page_size = data2host4(pevent, ptr);
-			ptr += 4;
-			break;
-		case 8:
-			cpu_data[cpu].page_size = data2host8(pevent, ptr);
-			ptr += 8;
-			break;
-		default:
-			die("bad long size");
-		}
-		ptr = cpu_data[cpu].page + header_page_data_offset;
-	}
-
-read_again:
-	idx = calc_index(ptr, cpu);
-
-	if (idx >= cpu_data[cpu].page_size) {
-		get_next_page(cpu);
-		return trace_peek_data(pevent, cpu);
-	}
-
-	type_len_ts = data2host4(pevent, ptr);
-	ptr += 4;
-
-	type_len = type_len4host(type_len_ts);
-	delta = ts4host(type_len_ts);
-
-	switch (type_len) {
-	case RINGBUF_TYPE_PADDING:
-		if (!delta)
-			die("error, hit unexpected end of page");
-		length = data2host4(pevent, ptr);
-		ptr += 4;
-		length *= 4;
-		ptr += length;
-		goto read_again;
-
-	case RINGBUF_TYPE_TIME_EXTEND:
-		extend = data2host4(pevent, ptr);
-		ptr += 4;
-		extend <<= TS_SHIFT;
-		extend += delta;
-		cpu_data[cpu].timestamp += extend;
-		goto read_again;
-
-	case RINGBUF_TYPE_TIME_STAMP:
-		ptr += 12;
-		break;
-	case 0:
-		length = data2host4(pevent, ptr);
-		ptr += 4;
-		die("here! length=%d", length);
-		break;
-	default:
-		length = type_len * 4;
-		break;
-	}
-
-	cpu_data[cpu].timestamp += delta;
-
-	data = malloc_or_die(sizeof(*data));
-	memset(data, 0, sizeof(*data));
-
-	data->ts = cpu_data[cpu].timestamp;
-	data->size = length;
-	data->data = ptr;
-	ptr += length;
-
-	cpu_data[cpu].index = calc_index(ptr, cpu);
-	cpu_data[cpu].next = data;
-
-	return data;
-}
-
-struct pevent_record *trace_read_data(struct pevent *pevent, int cpu)
-{
-	struct pevent_record *data;
-
-	data = trace_peek_data(pevent, cpu);
-	cpu_data[cpu].next = NULL;
-
-	return data;
+	return 0;
 }
 
 ssize_t trace_report(int fd, struct pevent **ppevent, bool __repipe)
@@ -494,58 +348,85 @@ ssize_t trace_report(int fd, struct pevent **ppevent, bool __repipe)
 	int show_version = 0;
 	int show_funcs = 0;
 	int show_printk = 0;
-	ssize_t size;
+	ssize_t size = -1;
+	struct pevent *pevent;
+	int err;
 
-	calc_data_size = 1;
-	repipe = __repipe;
+	*ppevent = NULL;
 
+	repipe = __repipe;
 	input_fd = fd;
 
-	read_or_die(buf, 3);
-	if (memcmp(buf, test, 3) != 0)
-		die("no trace data in the file");
+	if (do_read(buf, 3) < 0)
+		return -1;
+	if (memcmp(buf, test, 3) != 0) {
+		pr_debug("no trace data in the file");
+		return -1;
+	}
 
-	read_or_die(buf, 7);
-	if (memcmp(buf, "tracing", 7) != 0)
-		die("not a trace file (missing 'tracing' tag)");
+	if (do_read(buf, 7) < 0)
+		return -1;
+	if (memcmp(buf, "tracing", 7) != 0) {
+		pr_debug("not a trace file (missing 'tracing' tag)");
+		return -1;
+	}
 
 	version = read_string();
+	if (version == NULL)
+		return -1;
 	if (show_version)
 		printf("version = %s\n", version);
 	free(version);
 
-	read_or_die(buf, 1);
+	if (do_read(buf, 1) < 0)
+		return -1;
 	file_bigendian = buf[0];
 	host_bigendian = bigendian();
 
-	*ppevent = read_trace_init(file_bigendian, host_bigendian);
-	if (*ppevent == NULL)
-		die("read_trace_init failed");
+	pevent = read_trace_init(file_bigendian, host_bigendian);
+	if (pevent == NULL) {
+		pr_debug("read_trace_init failed");
+		goto out;
+	}
 
-	read_or_die(buf, 1);
+	if (do_read(buf, 1) < 0)
+		goto out;
 	long_size = buf[0];
 
-	page_size = read4(*ppevent);
-
-	read_header_files(*ppevent);
-
-	read_ftrace_files(*ppevent);
-	read_event_files(*ppevent);
-	read_proc_kallsyms(*ppevent);
-	read_ftrace_printk(*ppevent);
-
-	size = calc_data_size - 1;
-	calc_data_size = 0;
+	page_size = read4(pevent);
+	if (!page_size)
+		goto out;
+
+	err = read_header_files(pevent);
+	if (err)
+		goto out;
+	err = read_ftrace_files(pevent);
+	if (err)
+		goto out;
+	err = read_event_files(pevent);
+	if (err)
+		goto out;
+	err = read_proc_kallsyms(pevent);
+	if (err)
+		goto out;
+	err = read_ftrace_printk(pevent);
+	if (err)
+		goto out;
+
+	size = trace_data_size;
 	repipe = false;
 
 	if (show_funcs) {
-		pevent_print_funcs(*ppevent);
-		return size;
-	}
-	if (show_printk) {
-		pevent_print_printk(*ppevent);
-		return size;
+		pevent_print_funcs(pevent);
+	} else if (show_printk) {
+		pevent_print_printk(pevent);
 	}
 
+	*ppevent = pevent;
+	pevent = NULL;
+
+out:
+	if (pevent)
+		pevent_free(pevent);
 	return size;
 }
diff --git a/tools/perf/util/trace-event.h b/tools/perf/util/trace-event.h
index a55fd37ffea..1978c398ad8 100644
--- a/tools/perf/util/trace-event.h
+++ b/tools/perf/util/trace-event.h
@@ -30,13 +30,9 @@ enum {
 int bigendian(void);
 
 struct pevent *read_trace_init(int file_bigendian, int host_bigendian);
-void print_trace_event(struct pevent *pevent, int cpu, void *data, int size);
 void event_format__print(struct event_format *event,
 			 int cpu, void *data, int size);
 
-void print_event(struct pevent *pevent, int cpu, void *data, int size,
-		 unsigned long long nsecs, char *comm);
-
 int parse_ftrace_file(struct pevent *pevent, char *buf, unsigned long size);
 int parse_event_file(struct pevent *pevent,
 		     char *buf, unsigned long size, char *sys);
@@ -72,7 +68,7 @@ struct tracing_data {
 
 struct tracing_data *tracing_data_get(struct list_head *pattrs,
 				      int fd, bool temp);
-void tracing_data_put(struct tracing_data *tdata);
+int tracing_data_put(struct tracing_data *tdata);
 
 
 struct addr_location;
diff --git a/tools/perf/util/util.c b/tools/perf/util/util.c
index 805d1f52c5b..59d868add27 100644
--- a/tools/perf/util/util.c
+++ b/tools/perf/util/util.c
@@ -17,6 +17,8 @@ bool test_attr__enabled;
 bool perf_host  = true;
 bool perf_guest = false;
 
+char tracing_events_path[PATH_MAX + 1] = "/sys/kernel/debug/tracing/events";
+
 void event_attr_init(struct perf_event_attr *attr)
 {
 	if (!perf_host)
@@ -242,3 +244,28 @@ void get_term_dimensions(struct winsize *ws)
 	ws->ws_row = 25;
 	ws->ws_col = 80;
 }
+
+static void set_tracing_events_path(const char *mountpoint)
+{
+	snprintf(tracing_events_path, sizeof(tracing_events_path), "%s/%s",
+		 mountpoint, "tracing/events");
+}
+
+const char *perf_debugfs_mount(const char *mountpoint)
+{
+	const char *mnt;
+
+	mnt = debugfs_mount(mountpoint);
+	if (!mnt)
+		return NULL;
+
+	set_tracing_events_path(mnt);
+
+	return mnt;
+}
+
+void perf_debugfs_set_path(const char *mntpt)
+{
+	snprintf(debugfs_mountpoint, strlen(debugfs_mountpoint), "%s", mntpt);
+	set_tracing_events_path(mntpt);
+}
diff --git a/tools/perf/util/util.h b/tools/perf/util/util.h
index 09b4c26b71a..a45710b70a5 100644
--- a/tools/perf/util/util.h
+++ b/tools/perf/util/util.h
@@ -1,8 +1,6 @@
 #ifndef GIT_COMPAT_UTIL_H
 #define GIT_COMPAT_UTIL_H
 
-#define _FILE_OFFSET_BITS 64
-
 #ifndef FLEX_ARRAY
 /*
  * See if our compiler is known to support flexible array members.
@@ -73,10 +71,14 @@
 #include <linux/magic.h>
 #include "types.h"
 #include <sys/ttydefaults.h>
+#include <lk/debugfs.h>
 
 extern const char *graph_line;
 extern const char *graph_dotted_line;
 extern char buildid_dir[];
+extern char tracing_events_path[];
+extern void perf_debugfs_set_path(const char *mountpoint);
+const char *perf_debugfs_mount(const char *mountpoint);
 
 /* On most systems <limits.h> would have given us this, but
  * not on some systems (e.g. GNU/Hurd).
@@ -274,5 +276,4 @@ extern unsigned int page_size;
 
 struct winsize;
 void get_term_dimensions(struct winsize *ws);
-
-#endif
+#endif /* GIT_COMPAT_UTIL_H */
diff --git a/tools/power/cpupower/debug/i386/intel_gsic.c b/tools/power/cpupower/debug/i386/intel_gsic.c
index 53f5293c9c9..d032c826d42 100644
--- a/tools/power/cpupower/debug/i386/intel_gsic.c
+++ b/tools/power/cpupower/debug/i386/intel_gsic.c
@@ -66,7 +66,7 @@ int main (void)
 		printf("ecx = 0x%.8x\n", r.ecx);
 		printf("edx = 0x%.8x\n", r.edx);
 		printf("Note also that some BIOS do not support the initial "
-		       "GSIC call, but the newer\nspeeedstep-smi driver may "
+		       "GSIC call, but the newer\nspeedstep-smi driver may "
 		       "work.\nFor this, you need to pass some arguments to "
 		       "the speedstep-smi driver:\n");
 		printf("\tsmi_cmd=0x?? smi_port=0x?? smi_sig=1\n");
diff --git a/tools/power/x86/turbostat/turbostat.c b/tools/power/x86/turbostat/turbostat.c
index 6f3214ed444..321e066a075 100644
--- a/tools/power/x86/turbostat/turbostat.c
+++ b/tools/power/x86/turbostat/turbostat.c
@@ -1421,6 +1421,7 @@ int has_nehalem_turbo_ratio_limit(unsigned int family, unsigned int model)
 	case 0x3C:	/* HSW */
 	case 0x3F:	/* HSW */
 	case 0x45:	/* HSW */
+	case 0x46:	/* HSW */
 		return 1;
 	case 0x2E:	/* Nehalem-EX Xeon - Beckton */
 	case 0x2F:	/* Westmere-EX Xeon - Eagleton */
@@ -1515,6 +1516,7 @@ void rapl_probe(unsigned int family, unsigned int model)
 	case 0x3C:	/* HSW */
 	case 0x3F:	/* HSW */
 	case 0x45:	/* HSW */
+	case 0x46:	/* HSW */
 		do_rapl = RAPL_PKG | RAPL_CORES | RAPL_GFX;
 		break;
 	case 0x2D:
@@ -1754,6 +1756,7 @@ int is_snb(unsigned int family, unsigned int model)
 	case 0x3C:	/* HSW */
 	case 0x3F:	/* HSW */
 	case 0x45:	/* HSW */
+	case 0x46:	/* HSW */
 		return 1;
 	}
 	return 0;
@@ -2276,7 +2279,7 @@ int main(int argc, char **argv)
 	cmdline(argc, argv);
 
 	if (verbose)
-		fprintf(stderr, "turbostat v3.2 February 11, 2013"
+		fprintf(stderr, "turbostat v3.3 March 15, 2013"
 			" - Len Brown <lenb@kernel.org>\n");
 
 	turbostat_init();
diff --git a/tools/scripts/Makefile.include b/tools/scripts/Makefile.include
index 2964b96aa55..f03e681f889 100644
--- a/tools/scripts/Makefile.include
+++ b/tools/scripts/Makefile.include
@@ -1,3 +1,4 @@
+ifneq ($(O),)
 ifeq ($(origin O), command line)
 	dummy := $(if $(shell test -d $(O) || echo $(O)),$(error O=$(O) does not exist),)
 	ABSOLUTE_O := $(shell cd $(O) ; pwd)
@@ -7,9 +8,10 @@ ifeq ($(objtree),)
 	objtree := $(O)
 endif
 endif
+endif
 
-ifneq ($(OUTPUT),)
 # check that the output directory actually exists
+ifneq ($(OUTPUT),)
 OUTDIR := $(shell cd $(OUTPUT) && /bin/pwd)
 $(if $(OUTDIR),, $(error output directory "$(OUTPUT)" does not exist))
 endif
@@ -70,7 +72,7 @@ ifndef V
 	QUIET_BISON    = @echo '   ' BISON $@;
 
 	descend = \
-		@echo '   ' DESCEND $(1); \
+		+@echo '   ' DESCEND $(1); \
 		mkdir -p $(OUTPUT)$(1) && \
 		$(MAKE) $(COMMAND_O) subdir=$(if $(subdir),$(subdir)/$(1),$(1)) $(PRINT_DIR) -C $(1) $(2)
 endif
diff --git a/tools/testing/ktest/ktest.pl b/tools/testing/ktest/ktest.pl
index 4e67d52eb3a..0d7fd8b5154 100755
--- a/tools/testing/ktest/ktest.pl
+++ b/tools/testing/ktest/ktest.pl
@@ -73,6 +73,7 @@ my $ktest_config;
 my $version;
 my $have_version = 0;
 my $machine;
+my $last_machine;
 my $ssh_user;
 my $tmpdir;
 my $builddir;
@@ -108,6 +109,7 @@ my $scp_to_target;
 my $scp_to_target_install;
 my $power_off;
 my $grub_menu;
+my $last_grub_menu;
 my $grub_file;
 my $grub_number;
 my $grub_reboot;
@@ -1538,7 +1540,9 @@ sub run_scp_mod {
 
 sub get_grub2_index {
 
-    return if (defined($grub_number));
+    return if (defined($grub_number) && defined($last_grub_menu) &&
+	       $last_grub_menu eq $grub_menu && defined($last_machine) &&
+	       $last_machine eq $machine);
 
     doprint "Find grub2 menu ... ";
     $grub_number = -1;
@@ -1565,6 +1569,8 @@ sub get_grub2_index {
     die "Could not find '$grub_menu' in $grub_file on $machine"
 	if (!$found);
     doprint "$grub_number\n";
+    $last_grub_menu = $grub_menu;
+    $last_machine = $machine;
 }
 
 sub get_grub_index {
@@ -1577,7 +1583,9 @@ sub get_grub_index {
     if ($reboot_type ne "grub") {
 	return;
     }
-    return if (defined($grub_number));
+    return if (defined($grub_number) && defined($last_grub_menu) &&
+	       $last_grub_menu eq $grub_menu && defined($last_machine) &&
+	       $last_machine eq $machine);
 
     doprint "Find grub menu ... ";
     $grub_number = -1;
@@ -1604,6 +1612,8 @@ sub get_grub_index {
     die "Could not find '$grub_menu' in /boot/grub/menu on $machine"
 	if (!$found);
     doprint "$grub_number\n";
+    $last_grub_menu = $grub_menu;
+    $last_machine = $machine;
 }
 
 sub wait_for_input
diff --git a/tools/vm/Makefile b/tools/vm/Makefile
index 8e30e5c40f8..24e9ddd93fa 100644
--- a/tools/vm/Makefile
+++ b/tools/vm/Makefile
@@ -1,11 +1,22 @@
 # Makefile for vm tools
+#
+TARGETS=page-types slabinfo
+
+LK_DIR = ../lib/lk
+LIBLK = $(LK_DIR)/liblk.a
 
 CC = $(CROSS_COMPILE)gcc
-CFLAGS = -Wall -Wextra
+CFLAGS = -Wall -Wextra -I../lib/
+LDFLAGS = $(LIBLK)
+
+$(TARGETS): liblk
+
+liblk:
+	make -C $(LK_DIR)
 
-all: page-types slabinfo
 %: %.c
-	$(CC) $(CFLAGS) -o $@ $^
+	$(CC) $(CFLAGS) -o $@ $< $(LDFLAGS)
 
 clean:
 	$(RM) page-types slabinfo
+	make -C ../lib/lk clean
diff --git a/tools/vm/page-types.c b/tools/vm/page-types.c
index b76edf2f833..71c9c2511ee 100644
--- a/tools/vm/page-types.c
+++ b/tools/vm/page-types.c
@@ -36,7 +36,7 @@
 #include <sys/statfs.h>
 #include "../../include/uapi/linux/magic.h"
 #include "../../include/uapi/linux/kernel-page-flags.h"
-
+#include <lk/debugfs.h>
 
 #ifndef MAX_PATH
 # define MAX_PATH 256
@@ -178,7 +178,7 @@ static int		kpageflags_fd;
 static int		opt_hwpoison;
 static int		opt_unpoison;
 
-static char		hwpoison_debug_fs[MAX_PATH+1];
+static char		*hwpoison_debug_fs;
 static int		hwpoison_inject_fd;
 static int		hwpoison_forget_fd;
 
@@ -458,81 +458,6 @@ static uint64_t kpageflags_flags(uint64_t flags)
 	return flags;
 }
 
-/* verify that a mountpoint is actually a debugfs instance */
-static int debugfs_valid_mountpoint(const char *debugfs)
-{
-	struct statfs st_fs;
-
-	if (statfs(debugfs, &st_fs) < 0)
-		return -ENOENT;
-	else if (st_fs.f_type != (long) DEBUGFS_MAGIC)
-		return -ENOENT;
-
-	return 0;
-}
-
-/* find the path to the mounted debugfs */
-static const char *debugfs_find_mountpoint(void)
-{
-	const char *const *ptr;
-	char type[100];
-	FILE *fp;
-
-	ptr = debugfs_known_mountpoints;
-	while (*ptr) {
-		if (debugfs_valid_mountpoint(*ptr) == 0) {
-			strcpy(hwpoison_debug_fs, *ptr);
-			return hwpoison_debug_fs;
-		}
-		ptr++;
-	}
-
-	/* give up and parse /proc/mounts */
-	fp = fopen("/proc/mounts", "r");
-	if (fp == NULL)
-		perror("Can't open /proc/mounts for read");
-
-	while (fscanf(fp, "%*s %"
-		      STR(MAX_PATH)
-		      "s %99s %*s %*d %*d\n",
-		      hwpoison_debug_fs, type) == 2) {
-		if (strcmp(type, "debugfs") == 0)
-			break;
-	}
-	fclose(fp);
-
-	if (strcmp(type, "debugfs") != 0)
-		return NULL;
-
-	return hwpoison_debug_fs;
-}
-
-/* mount the debugfs somewhere if it's not mounted */
-
-static void debugfs_mount(void)
-{
-	const char *const *ptr;
-
-	/* see if it's already mounted */
-	if (debugfs_find_mountpoint())
-		return;
-
-	ptr = debugfs_known_mountpoints;
-	while (*ptr) {
-		if (mount(NULL, *ptr, "debugfs", 0, NULL) == 0) {
-			/* save the mountpoint */
-			strcpy(hwpoison_debug_fs, *ptr);
-			break;
-		}
-		ptr++;
-	}
-
-	if (*ptr == NULL) {
-		perror("mount debugfs");
-		exit(EXIT_FAILURE);
-	}
-}
-
 /*
  * page actions
  */
@@ -541,7 +466,11 @@ static void prepare_hwpoison_fd(void)
 {
 	char buf[MAX_PATH + 1];
 
-	debugfs_mount();
+	hwpoison_debug_fs = debugfs_mount(NULL);
+	if (!hwpoison_debug_fs) {
+		perror("mount debugfs");
+		exit(EXIT_FAILURE);
+	}
 
 	if (opt_hwpoison && !hwpoison_inject_fd) {
 		snprintf(buf, MAX_PATH, "%s/hwpoison/corrupt-pfn",
diff --git a/virt/kvm/kvm_main.c b/virt/kvm/kvm_main.c
index adc68feb5c5..f18013f09e6 100644
--- a/virt/kvm/kvm_main.c
+++ b/virt/kvm/kvm_main.c
@@ -1541,21 +1541,38 @@ int kvm_write_guest(struct kvm *kvm, gpa_t gpa, const void *data,
 }
 
 int kvm_gfn_to_hva_cache_init(struct kvm *kvm, struct gfn_to_hva_cache *ghc,
-			      gpa_t gpa)
+			      gpa_t gpa, unsigned long len)
 {
 	struct kvm_memslots *slots = kvm_memslots(kvm);
 	int offset = offset_in_page(gpa);
-	gfn_t gfn = gpa >> PAGE_SHIFT;
+	gfn_t start_gfn = gpa >> PAGE_SHIFT;
+	gfn_t end_gfn = (gpa + len - 1) >> PAGE_SHIFT;
+	gfn_t nr_pages_needed = end_gfn - start_gfn + 1;
+	gfn_t nr_pages_avail;
 
 	ghc->gpa = gpa;
 	ghc->generation = slots->generation;
-	ghc->memslot = gfn_to_memslot(kvm, gfn);
-	ghc->hva = gfn_to_hva_many(ghc->memslot, gfn, NULL);
-	if (!kvm_is_error_hva(ghc->hva))
+	ghc->len = len;
+	ghc->memslot = gfn_to_memslot(kvm, start_gfn);
+	ghc->hva = gfn_to_hva_many(ghc->memslot, start_gfn, &nr_pages_avail);
+	if (!kvm_is_error_hva(ghc->hva) && nr_pages_avail >= nr_pages_needed) {
 		ghc->hva += offset;
-	else
-		return -EFAULT;
-
+	} else {
+		/*
+		 * If the requested region crosses two memslots, we still
+		 * verify that the entire region is valid here.
+		 */
+		while (start_gfn <= end_gfn) {
+			ghc->memslot = gfn_to_memslot(kvm, start_gfn);
+			ghc->hva = gfn_to_hva_many(ghc->memslot, start_gfn,
+						   &nr_pages_avail);
+			if (kvm_is_error_hva(ghc->hva))
+				return -EFAULT;
+			start_gfn += nr_pages_avail;
+		}
+		/* Use the slow path for cross page reads and writes. */
+		ghc->memslot = NULL;
+	}
 	return 0;
 }
 EXPORT_SYMBOL_GPL(kvm_gfn_to_hva_cache_init);
@@ -1566,8 +1583,13 @@ int kvm_write_guest_cached(struct kvm *kvm, struct gfn_to_hva_cache *ghc,
 	struct kvm_memslots *slots = kvm_memslots(kvm);
 	int r;
 
+	BUG_ON(len > ghc->len);
+
 	if (slots->generation != ghc->generation)
-		kvm_gfn_to_hva_cache_init(kvm, ghc, ghc->gpa);
+		kvm_gfn_to_hva_cache_init(kvm, ghc, ghc->gpa, ghc->len);
+
+	if (unlikely(!ghc->memslot))
+		return kvm_write_guest(kvm, ghc->gpa, data, len);
 
 	if (kvm_is_error_hva(ghc->hva))
 		return -EFAULT;
@@ -1587,8 +1609,13 @@ int kvm_read_guest_cached(struct kvm *kvm, struct gfn_to_hva_cache *ghc,
 	struct kvm_memslots *slots = kvm_memslots(kvm);
 	int r;
 
+	BUG_ON(len > ghc->len);
+
 	if (slots->generation != ghc->generation)
-		kvm_gfn_to_hva_cache_init(kvm, ghc, ghc->gpa);
+		kvm_gfn_to_hva_cache_init(kvm, ghc, ghc->gpa, ghc->len);
+
+	if (unlikely(!ghc->memslot))
+		return kvm_read_guest(kvm, ghc->gpa, data, len);
 
 	if (kvm_is_error_hva(ghc->hva))
 		return -EFAULT;